1832 files changed, 639687 insertions, 0 deletions

diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
new file mode 100644
index 000000000..5cfabd537
--- /dev/null
+++ b/drivers/staging/Kconfig
@@ -0,0 +1,83 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig STAGING
+	bool "Staging drivers"
+	help
+	  This option allows you to select a number of drivers that are
+	  not of the "normal" Linux kernel quality level.  These drivers
+	  are placed here in order to get a wider audience to make use of
+	  them.  Please note that these drivers are under heavy
+	  development, may or may not work, and may contain userspace
+	  interfaces that most likely will be changed in the near
+	  future.
+
+	  Using any of these drivers will taint your kernel which might
+	  affect support options from both the community, and various
+	  commercial support organizations.
+
+	  If you wish to work on these drivers, to help improve them, or
+	  to report problems you have with them, please see the
+	  drivers/staging/<driver_name>/TODO file to see what needs to be
+	  worked on, and who to contact.
+
+	  If in doubt, say N here.
+
+
+if STAGING
+
+source "drivers/staging/wlan-ng/Kconfig"
+
+source "drivers/staging/olpc_dcon/Kconfig"
+
+source "drivers/staging/rtl8192u/Kconfig"
+
+source "drivers/staging/rtl8192e/Kconfig"
+
+source "drivers/staging/rtl8723bs/Kconfig"
+
+source "drivers/staging/rtl8712/Kconfig"
+
+source "drivers/staging/r8188eu/Kconfig"
+
+source "drivers/staging/rts5208/Kconfig"
+
+source "drivers/staging/octeon/Kconfig"
+
+source "drivers/staging/vt6655/Kconfig"
+
+source "drivers/staging/vt6656/Kconfig"
+
+source "drivers/staging/iio/Kconfig"
+
+source "drivers/staging/sm750fb/Kconfig"
+
+source "drivers/staging/emxx_udc/Kconfig"
+
+source "drivers/staging/nvec/Kconfig"
+
+source "drivers/staging/media/Kconfig"
+
+source "drivers/staging/board/Kconfig"
+
+source "drivers/staging/gdm724x/Kconfig"
+
+source "drivers/staging/fbtft/Kconfig"
+
+source "drivers/staging/most/Kconfig"
+
+source "drivers/staging/ks7010/Kconfig"
+
+source "drivers/staging/greybus/Kconfig"
+
+source "drivers/staging/vc04_services/Kconfig"
+
+source "drivers/staging/pi433/Kconfig"
+
+source "drivers/staging/axis-fifo/Kconfig"
+
+source "drivers/staging/fieldbus/Kconfig"
+
+source "drivers/staging/qlge/Kconfig"
+
+source "drivers/staging/vme_user/Kconfig"
+
+endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
new file mode 100644
index 000000000..f8c3aa9c2
--- /dev/null
+++ b/drivers/staging/Makefile
@@ -0,0 +1,31 @@
+# SPDX-License-Identifier: GPL-2.0
+# Makefile for staging directory
+
+obj-y				+= media/
+obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
+obj-$(CONFIG_FB_OLPC_DCON)	+= olpc_dcon/
+obj-$(CONFIG_RTL8192U)		+= rtl8192u/
+obj-$(CONFIG_RTL8192E)		+= rtl8192e/
+obj-$(CONFIG_RTL8723BS)		+= rtl8723bs/
+obj-$(CONFIG_R8712U)		+= rtl8712/
+obj-$(CONFIG_R8188EU)		+= r8188eu/
+obj-$(CONFIG_RTS5208)		+= rts5208/
+obj-$(CONFIG_OCTEON_ETHERNET)	+= octeon/
+obj-$(CONFIG_VT6655)		+= vt6655/
+obj-$(CONFIG_VT6656)		+= vt6656/
+obj-$(CONFIG_VME_BUS)		+= vme_user/
+obj-$(CONFIG_IIO)		+= iio/
+obj-$(CONFIG_FB_SM750)		+= sm750fb/
+obj-$(CONFIG_USB_EMXX)		+= emxx_udc/
+obj-$(CONFIG_MFD_NVEC)		+= nvec/
+obj-$(CONFIG_STAGING_BOARD)	+= board/
+obj-$(CONFIG_LTE_GDM724X)	+= gdm724x/
+obj-$(CONFIG_FB_TFT)		+= fbtft/
+obj-$(CONFIG_MOST)		+= most/
+obj-$(CONFIG_KS7010)		+= ks7010/
+obj-$(CONFIG_GREYBUS)		+= greybus/
+obj-$(CONFIG_BCM2835_VCHIQ)	+= vc04_services/
+obj-$(CONFIG_PI433)		+= pi433/
+obj-$(CONFIG_XIL_AXIS_FIFO)	+= axis-fifo/
+obj-$(CONFIG_FIELDBUS_DEV)     += fieldbus/
+obj-$(CONFIG_QLGE)		+= qlge/
diff --git a/drivers/staging/axis-fifo/Kconfig b/drivers/staging/axis-fifo/Kconfig
new file mode 100644
index 000000000..f180a8e9f
--- /dev/null
+++ b/drivers/staging/axis-fifo/Kconfig
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# "Xilinx AXI-Stream FIFO IP core driver"
+#
+config XIL_AXIS_FIFO
+	tristate "Xilinx AXI-Stream FIFO IP core driver"
+	depends on OF && HAS_IOMEM
+	help
+	  This adds support for the Xilinx AXI-Stream FIFO IP core driver.
+	  The AXI Streaming FIFO allows memory mapped access to a AXI Streaming
+	  interface. The Xilinx AXI-Stream FIFO IP core can be used to interface
+	  to the AXI Ethernet without the need to use DMA.
diff --git a/drivers/staging/axis-fifo/Makefile b/drivers/staging/axis-fifo/Makefile
new file mode 100644
index 000000000..c626005c9
--- /dev/null
+++ b/drivers/staging/axis-fifo/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_XIL_AXIS_FIFO) += axis-fifo.o
diff --git a/drivers/staging/axis-fifo/README b/drivers/staging/axis-fifo/README
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/drivers/staging/axis-fifo/README
diff --git a/drivers/staging/axis-fifo/axis-fifo.c b/drivers/staging/axis-fifo/axis-fifo.c
new file mode 100644
index 000000000..0a85ea667
--- /dev/null
+++ b/drivers/staging/axis-fifo/axis-fifo.c
@@ -0,0 +1,973 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Xilinx AXIS FIFO: interface to the Xilinx AXI-Stream FIFO IP core
+ *
+ * Copyright (C) 2018 Jacob Feder
+ *
+ * Authors:  Jacob Feder <jacobsfeder@gmail.com>
+ *
+ * See Xilinx PG080 document for IP details
+ */
+
+/* ----------------------------
+ *           includes
+ * ----------------------------
+ */
+
+#include <linux/kernel.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/moduleparam.h>
+#include <linux/interrupt.h>
+#include <linux/param.h>
+#include <linux/fs.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/jiffies.h>
+#include <linux/miscdevice.h>
+
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+
+/* ----------------------------
+ *       driver parameters
+ * ----------------------------
+ */
+
+#define DRIVER_NAME "axis_fifo"
+
+#define READ_BUF_SIZE 128U /* read buffer length in words */
+#define WRITE_BUF_SIZE 128U /* write buffer length in words */
+
+/* ----------------------------
+ *     IP register offsets
+ * ----------------------------
+ */
+
+#define XLLF_ISR_OFFSET  0x00000000  /* Interrupt Status */
+#define XLLF_IER_OFFSET  0x00000004  /* Interrupt Enable */
+
+#define XLLF_TDFR_OFFSET 0x00000008  /* Transmit Reset */
+#define XLLF_TDFV_OFFSET 0x0000000c  /* Transmit Vacancy */
+#define XLLF_TDFD_OFFSET 0x00000010  /* Transmit Data */
+#define XLLF_TLR_OFFSET  0x00000014  /* Transmit Length */
+
+#define XLLF_RDFR_OFFSET 0x00000018  /* Receive Reset */
+#define XLLF_RDFO_OFFSET 0x0000001c  /* Receive Occupancy */
+#define XLLF_RDFD_OFFSET 0x00000020  /* Receive Data */
+#define XLLF_RLR_OFFSET  0x00000024  /* Receive Length */
+#define XLLF_SRR_OFFSET  0x00000028  /* Local Link Reset */
+#define XLLF_TDR_OFFSET  0x0000002C  /* Transmit Destination */
+#define XLLF_RDR_OFFSET  0x00000030  /* Receive Destination */
+
+/* ----------------------------
+ *     reset register masks
+ * ----------------------------
+ */
+
+#define XLLF_RDFR_RESET_MASK        0x000000a5 /* receive reset value */
+#define XLLF_TDFR_RESET_MASK        0x000000a5 /* Transmit reset value */
+#define XLLF_SRR_RESET_MASK         0x000000a5 /* Local Link reset value */
+
+/* ----------------------------
+ *       interrupt masks
+ * ----------------------------
+ */
+
+#define XLLF_INT_RPURE_MASK       0x80000000 /* Receive under-read */
+#define XLLF_INT_RPORE_MASK       0x40000000 /* Receive over-read */
+#define XLLF_INT_RPUE_MASK        0x20000000 /* Receive underrun (empty) */
+#define XLLF_INT_TPOE_MASK        0x10000000 /* Transmit overrun */
+#define XLLF_INT_TC_MASK          0x08000000 /* Transmit complete */
+#define XLLF_INT_RC_MASK          0x04000000 /* Receive complete */
+#define XLLF_INT_TSE_MASK         0x02000000 /* Transmit length mismatch */
+#define XLLF_INT_TRC_MASK         0x01000000 /* Transmit reset complete */
+#define XLLF_INT_RRC_MASK         0x00800000 /* Receive reset complete */
+#define XLLF_INT_TFPF_MASK        0x00400000 /* Tx FIFO Programmable Full */
+#define XLLF_INT_TFPE_MASK        0x00200000 /* Tx FIFO Programmable Empty */
+#define XLLF_INT_RFPF_MASK        0x00100000 /* Rx FIFO Programmable Full */
+#define XLLF_INT_RFPE_MASK        0x00080000 /* Rx FIFO Programmable Empty */
+#define XLLF_INT_ALL_MASK         0xfff80000 /* All the ints */
+#define XLLF_INT_ERROR_MASK       0xf2000000 /* Error status ints */
+#define XLLF_INT_RXERROR_MASK     0xe0000000 /* Receive Error status ints */
+#define XLLF_INT_TXERROR_MASK     0x12000000 /* Transmit Error status ints */
+
+/* ----------------------------
+ *           globals
+ * ----------------------------
+ */
+static long read_timeout = 1000; /* ms to wait before read() times out */
+static long write_timeout = 1000; /* ms to wait before write() times out */
+
+/* ----------------------------
+ * module command-line arguments
+ * ----------------------------
+ */
+
+module_param(read_timeout, long, 0444);
+MODULE_PARM_DESC(read_timeout, "ms to wait before blocking read() timing out; set to -1 for no timeout");
+module_param(write_timeout, long, 0444);
+MODULE_PARM_DESC(write_timeout, "ms to wait before blocking write() timing out; set to -1 for no timeout");
+
+/* ----------------------------
+ *            types
+ * ----------------------------
+ */
+
+struct axis_fifo {
+	int irq; /* interrupt */
+	void __iomem *base_addr; /* kernel space memory */
+
+	unsigned int rx_fifo_depth; /* max words in the receive fifo */
+	unsigned int tx_fifo_depth; /* max words in the transmit fifo */
+	int has_rx_fifo; /* whether the IP has the rx fifo enabled */
+	int has_tx_fifo; /* whether the IP has the tx fifo enabled */
+
+	wait_queue_head_t read_queue; /* wait queue for asynchronos read */
+	struct mutex read_lock; /* lock for reading */
+	wait_queue_head_t write_queue; /* wait queue for asynchronos write */
+	struct mutex write_lock; /* lock for writing */
+	unsigned int write_flags; /* write file flags */
+	unsigned int read_flags; /* read file flags */
+
+	struct device *dt_device; /* device created from the device tree */
+	struct miscdevice miscdev;
+};
+
+/* ----------------------------
+ *         sysfs entries
+ * ----------------------------
+ */
+
+static ssize_t sysfs_write(struct device *dev, const char *buf,
+			   size_t count, unsigned int addr_offset)
+{
+	struct axis_fifo *fifo = dev_get_drvdata(dev);
+	unsigned long tmp;
+	int rc;
+
+	rc = kstrtoul(buf, 0, &tmp);
+	if (rc < 0)
+		return rc;
+
+	iowrite32(tmp, fifo->base_addr + addr_offset);
+
+	return count;
+}
+
+static ssize_t sysfs_read(struct device *dev, char *buf,
+			  unsigned int addr_offset)
+{
+	struct axis_fifo *fifo = dev_get_drvdata(dev);
+	unsigned int read_val;
+	unsigned int len;
+	char tmp[32];
+
+	read_val = ioread32(fifo->base_addr + addr_offset);
+	len =  snprintf(tmp, sizeof(tmp), "0x%x\n", read_val);
+	memcpy(buf, tmp, len);
+
+	return len;
+}
+
+static ssize_t isr_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_ISR_OFFSET);
+}
+
+static ssize_t isr_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_ISR_OFFSET);
+}
+
+static DEVICE_ATTR_RW(isr);
+
+static ssize_t ier_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_IER_OFFSET);
+}
+
+static ssize_t ier_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_IER_OFFSET);
+}
+
+static DEVICE_ATTR_RW(ier);
+
+static ssize_t tdfr_store(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_TDFR_OFFSET);
+}
+
+static DEVICE_ATTR_WO(tdfr);
+
+static ssize_t tdfv_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_TDFV_OFFSET);
+}
+
+static DEVICE_ATTR_RO(tdfv);
+
+static ssize_t tdfd_store(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_TDFD_OFFSET);
+}
+
+static DEVICE_ATTR_WO(tdfd);
+
+static ssize_t tlr_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_TLR_OFFSET);
+}
+
+static DEVICE_ATTR_WO(tlr);
+
+static ssize_t rdfr_store(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_RDFR_OFFSET);
+}
+
+static DEVICE_ATTR_WO(rdfr);
+
+static ssize_t rdfo_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_RDFO_OFFSET);
+}
+
+static DEVICE_ATTR_RO(rdfo);
+
+static ssize_t rdfd_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_RDFD_OFFSET);
+}
+
+static DEVICE_ATTR_RO(rdfd);
+
+static ssize_t rlr_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_RLR_OFFSET);
+}
+
+static DEVICE_ATTR_RO(rlr);
+
+static ssize_t srr_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_SRR_OFFSET);
+}
+
+static DEVICE_ATTR_WO(srr);
+
+static ssize_t tdr_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	return sysfs_write(dev, buf, count, XLLF_TDR_OFFSET);
+}
+
+static DEVICE_ATTR_WO(tdr);
+
+static ssize_t rdr_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sysfs_read(dev, buf, XLLF_RDR_OFFSET);
+}
+
+static DEVICE_ATTR_RO(rdr);
+
+static struct attribute *axis_fifo_attrs[] = {
+	&dev_attr_isr.attr,
+	&dev_attr_ier.attr,
+	&dev_attr_tdfr.attr,
+	&dev_attr_tdfv.attr,
+	&dev_attr_tdfd.attr,
+	&dev_attr_tlr.attr,
+	&dev_attr_rdfr.attr,
+	&dev_attr_rdfo.attr,
+	&dev_attr_rdfd.attr,
+	&dev_attr_rlr.attr,
+	&dev_attr_srr.attr,
+	&dev_attr_tdr.attr,
+	&dev_attr_rdr.attr,
+	NULL,
+};
+
+static const struct attribute_group axis_fifo_attrs_group = {
+	.name = "ip_registers",
+	.attrs = axis_fifo_attrs,
+};
+
+static const struct attribute_group *axis_fifo_attrs_groups[] = {
+	&axis_fifo_attrs_group,
+	NULL,
+};
+
+/* ----------------------------
+ *        implementation
+ * ----------------------------
+ */
+
+static void reset_ip_core(struct axis_fifo *fifo)
+{
+	iowrite32(XLLF_SRR_RESET_MASK, fifo->base_addr + XLLF_SRR_OFFSET);
+	iowrite32(XLLF_TDFR_RESET_MASK, fifo->base_addr + XLLF_TDFR_OFFSET);
+	iowrite32(XLLF_RDFR_RESET_MASK, fifo->base_addr + XLLF_RDFR_OFFSET);
+	iowrite32(XLLF_INT_TC_MASK | XLLF_INT_RC_MASK | XLLF_INT_RPURE_MASK |
+		  XLLF_INT_RPORE_MASK | XLLF_INT_RPUE_MASK |
+		  XLLF_INT_TPOE_MASK | XLLF_INT_TSE_MASK,
+		  fifo->base_addr + XLLF_IER_OFFSET);
+	iowrite32(XLLF_INT_ALL_MASK, fifo->base_addr + XLLF_ISR_OFFSET);
+}
+
+/**
+ * axis_fifo_read() - Read a packet from AXIS-FIFO character device.
+ * @f: Open file.
+ * @buf: User space buffer to read to.
+ * @len: User space buffer length.
+ * @off: Buffer offset.
+ *
+ * As defined by the device's documentation, we need to check the device's
+ * occupancy before reading the length register and then the data. All these
+ * operations must be executed atomically, in order and one after the other
+ * without missing any.
+ *
+ * Returns the number of bytes read from the device or negative error code
+ *	on failure.
+ */
+static ssize_t axis_fifo_read(struct file *f, char __user *buf,
+			      size_t len, loff_t *off)
+{
+	struct axis_fifo *fifo = (struct axis_fifo *)f->private_data;
+	size_t bytes_available;
+	unsigned int words_available;
+	unsigned int copied;
+	unsigned int copy;
+	unsigned int i;
+	int ret;
+	u32 tmp_buf[READ_BUF_SIZE];
+
+	if (fifo->read_flags & O_NONBLOCK) {
+		/*
+		 * Device opened in non-blocking mode. Try to lock it and then
+		 * check if any packet is available.
+		 */
+		if (!mutex_trylock(&fifo->read_lock))
+			return -EAGAIN;
+
+		if (!ioread32(fifo->base_addr + XLLF_RDFO_OFFSET)) {
+			ret = -EAGAIN;
+			goto end_unlock;
+		}
+	} else {
+		/* opened in blocking mode
+		 * wait for a packet available interrupt (or timeout)
+		 * if nothing is currently available
+		 */
+		mutex_lock(&fifo->read_lock);
+		ret = wait_event_interruptible_timeout(fifo->read_queue,
+			ioread32(fifo->base_addr + XLLF_RDFO_OFFSET),
+			read_timeout);
+
+		if (ret <= 0) {
+			if (ret == 0) {
+				ret = -EAGAIN;
+			} else if (ret != -ERESTARTSYS) {
+				dev_err(fifo->dt_device, "wait_event_interruptible_timeout() error in read (ret=%i)\n",
+					ret);
+			}
+
+			goto end_unlock;
+		}
+	}
+
+	bytes_available = ioread32(fifo->base_addr + XLLF_RLR_OFFSET);
+	if (!bytes_available) {
+		dev_err(fifo->dt_device, "received a packet of length 0 - fifo core will be reset\n");
+		reset_ip_core(fifo);
+		ret = -EIO;
+		goto end_unlock;
+	}
+
+	if (bytes_available > len) {
+		dev_err(fifo->dt_device, "user read buffer too small (available bytes=%zu user buffer bytes=%zu) - fifo core will be reset\n",
+			bytes_available, len);
+		reset_ip_core(fifo);
+		ret = -EINVAL;
+		goto end_unlock;
+	}
+
+	if (bytes_available % sizeof(u32)) {
+		/* this probably can't happen unless IP
+		 * registers were previously mishandled
+		 */
+		dev_err(fifo->dt_device, "received a packet that isn't word-aligned - fifo core will be reset\n");
+		reset_ip_core(fifo);
+		ret = -EIO;
+		goto end_unlock;
+	}
+
+	words_available = bytes_available / sizeof(u32);
+
+	/* read data into an intermediate buffer, copying the contents
+	 * to userspace when the buffer is full
+	 */
+	copied = 0;
+	while (words_available > 0) {
+		copy = min(words_available, READ_BUF_SIZE);
+
+		for (i = 0; i < copy; i++) {
+			tmp_buf[i] = ioread32(fifo->base_addr +
+					      XLLF_RDFD_OFFSET);
+		}
+
+		if (copy_to_user(buf + copied * sizeof(u32), tmp_buf,
+				 copy * sizeof(u32))) {
+			reset_ip_core(fifo);
+			ret = -EFAULT;
+			goto end_unlock;
+		}
+
+		copied += copy;
+		words_available -= copy;
+	}
+
+	ret = bytes_available;
+
+end_unlock:
+	mutex_unlock(&fifo->read_lock);
+
+	return ret;
+}
+
+/**
+ * axis_fifo_write() - Write buffer to AXIS-FIFO character device.
+ * @f: Open file.
+ * @buf: User space buffer to write to the device.
+ * @len: User space buffer length.
+ * @off: Buffer offset.
+ *
+ * As defined by the device's documentation, we need to write to the device's
+ * data buffer then to the device's packet length register atomically. Also,
+ * we need to lock before checking if the device has available space to avoid
+ * any concurrency issue.
+ *
+ * Returns the number of bytes written to the device or negative error code
+ *	on failure.
+ */
+static ssize_t axis_fifo_write(struct file *f, const char __user *buf,
+			       size_t len, loff_t *off)
+{
+	struct axis_fifo *fifo = (struct axis_fifo *)f->private_data;
+	unsigned int words_to_write;
+	unsigned int copied;
+	unsigned int copy;
+	unsigned int i;
+	int ret;
+	u32 tmp_buf[WRITE_BUF_SIZE];
+
+	if (len % sizeof(u32)) {
+		dev_err(fifo->dt_device,
+			"tried to send a packet that isn't word-aligned\n");
+		return -EINVAL;
+	}
+
+	words_to_write = len / sizeof(u32);
+
+	if (!words_to_write) {
+		dev_err(fifo->dt_device,
+			"tried to send a packet of length 0\n");
+		return -EINVAL;
+	}
+
+	if (words_to_write > fifo->tx_fifo_depth) {
+		dev_err(fifo->dt_device, "tried to write more words [%u] than slots in the fifo buffer [%u]\n",
+			words_to_write, fifo->tx_fifo_depth);
+		return -EINVAL;
+	}
+
+	if (fifo->write_flags & O_NONBLOCK) {
+		/*
+		 * Device opened in non-blocking mode. Try to lock it and then
+		 * check if there is any room to write the given buffer.
+		 */
+		if (!mutex_trylock(&fifo->write_lock))
+			return -EAGAIN;
+
+		if (words_to_write > ioread32(fifo->base_addr +
+					      XLLF_TDFV_OFFSET)) {
+			ret = -EAGAIN;
+			goto end_unlock;
+		}
+	} else {
+		/* opened in blocking mode */
+
+		/* wait for an interrupt (or timeout) if there isn't
+		 * currently enough room in the fifo
+		 */
+		mutex_lock(&fifo->write_lock);
+		ret = wait_event_interruptible_timeout(fifo->write_queue,
+			ioread32(fifo->base_addr + XLLF_TDFV_OFFSET)
+				 >= words_to_write,
+			write_timeout);
+
+		if (ret <= 0) {
+			if (ret == 0) {
+				ret = -EAGAIN;
+			} else if (ret != -ERESTARTSYS) {
+				dev_err(fifo->dt_device, "wait_event_interruptible_timeout() error in write (ret=%i)\n",
+					ret);
+			}
+
+			goto end_unlock;
+		}
+	}
+
+	/* write data from an intermediate buffer into the fifo IP, refilling
+	 * the buffer with userspace data as needed
+	 */
+	copied = 0;
+	while (words_to_write > 0) {
+		copy = min(words_to_write, WRITE_BUF_SIZE);
+
+		if (copy_from_user(tmp_buf, buf + copied * sizeof(u32),
+				   copy * sizeof(u32))) {
+			reset_ip_core(fifo);
+			ret = -EFAULT;
+			goto end_unlock;
+		}
+
+		for (i = 0; i < copy; i++)
+			iowrite32(tmp_buf[i], fifo->base_addr +
+				  XLLF_TDFD_OFFSET);
+
+		copied += copy;
+		words_to_write -= copy;
+	}
+
+	ret = copied * sizeof(u32);
+
+	/* write packet size to fifo */
+	iowrite32(ret, fifo->base_addr + XLLF_TLR_OFFSET);
+
+end_unlock:
+	mutex_unlock(&fifo->write_lock);
+
+	return ret;
+}
+
+static irqreturn_t axis_fifo_irq(int irq, void *dw)
+{
+	struct axis_fifo *fifo = (struct axis_fifo *)dw;
+	unsigned int pending_interrupts;
+
+	do {
+		pending_interrupts = ioread32(fifo->base_addr +
+					      XLLF_IER_OFFSET) &
+					      ioread32(fifo->base_addr
+					      + XLLF_ISR_OFFSET);
+		if (pending_interrupts & XLLF_INT_RC_MASK) {
+			/* packet received */
+
+			/* wake the reader process if it is waiting */
+			wake_up(&fifo->read_queue);
+
+			/* clear interrupt */
+			iowrite32(XLLF_INT_RC_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TC_MASK) {
+			/* packet sent */
+
+			/* wake the writer process if it is waiting */
+			wake_up(&fifo->write_queue);
+
+			iowrite32(XLLF_INT_TC_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TFPF_MASK) {
+			/* transmit fifo programmable full */
+
+			iowrite32(XLLF_INT_TFPF_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TFPE_MASK) {
+			/* transmit fifo programmable empty */
+
+			iowrite32(XLLF_INT_TFPE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RFPF_MASK) {
+			/* receive fifo programmable full */
+
+			iowrite32(XLLF_INT_RFPF_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RFPE_MASK) {
+			/* receive fifo programmable empty */
+
+			iowrite32(XLLF_INT_RFPE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TRC_MASK) {
+			/* transmit reset complete interrupt */
+
+			iowrite32(XLLF_INT_TRC_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RRC_MASK) {
+			/* receive reset complete interrupt */
+
+			iowrite32(XLLF_INT_RRC_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RPURE_MASK) {
+			/* receive fifo under-read error interrupt */
+			dev_err(fifo->dt_device,
+				"receive under-read interrupt\n");
+
+			iowrite32(XLLF_INT_RPURE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RPORE_MASK) {
+			/* receive over-read error interrupt */
+			dev_err(fifo->dt_device,
+				"receive over-read interrupt\n");
+
+			iowrite32(XLLF_INT_RPORE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_RPUE_MASK) {
+			/* receive underrun error interrupt */
+			dev_err(fifo->dt_device,
+				"receive underrun error interrupt\n");
+
+			iowrite32(XLLF_INT_RPUE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TPOE_MASK) {
+			/* transmit overrun error interrupt */
+			dev_err(fifo->dt_device,
+				"transmit overrun error interrupt\n");
+
+			iowrite32(XLLF_INT_TPOE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts & XLLF_INT_TSE_MASK) {
+			/* transmit length mismatch error interrupt */
+			dev_err(fifo->dt_device,
+				"transmit length mismatch error interrupt\n");
+
+			iowrite32(XLLF_INT_TSE_MASK & XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		} else if (pending_interrupts) {
+			/* unknown interrupt type */
+			dev_err(fifo->dt_device,
+				"unknown interrupt(s) 0x%x\n",
+				pending_interrupts);
+
+			iowrite32(XLLF_INT_ALL_MASK,
+				  fifo->base_addr + XLLF_ISR_OFFSET);
+		}
+	} while (pending_interrupts);
+
+	return IRQ_HANDLED;
+}
+
+static int axis_fifo_open(struct inode *inod, struct file *f)
+{
+	struct axis_fifo *fifo = container_of(f->private_data,
+					      struct axis_fifo, miscdev);
+	f->private_data = fifo;
+
+	if (((f->f_flags & O_ACCMODE) == O_WRONLY) ||
+	    ((f->f_flags & O_ACCMODE) == O_RDWR)) {
+		if (fifo->has_tx_fifo) {
+			fifo->write_flags = f->f_flags;
+		} else {
+			dev_err(fifo->dt_device, "tried to open device for write but the transmit fifo is disabled\n");
+			return -EPERM;
+		}
+	}
+
+	if (((f->f_flags & O_ACCMODE) == O_RDONLY) ||
+	    ((f->f_flags & O_ACCMODE) == O_RDWR)) {
+		if (fifo->has_rx_fifo) {
+			fifo->read_flags = f->f_flags;
+		} else {
+			dev_err(fifo->dt_device, "tried to open device for read but the receive fifo is disabled\n");
+			return -EPERM;
+		}
+	}
+
+	return 0;
+}
+
+static int axis_fifo_close(struct inode *inod, struct file *f)
+{
+	f->private_data = NULL;
+
+	return 0;
+}
+
+static const struct file_operations fops = {
+	.owner = THIS_MODULE,
+	.open = axis_fifo_open,
+	.release = axis_fifo_close,
+	.read = axis_fifo_read,
+	.write = axis_fifo_write
+};
+
+/* read named property from the device tree */
+static int get_dts_property(struct axis_fifo *fifo,
+			    char *name, unsigned int *var)
+{
+	int rc;
+
+	rc = of_property_read_u32(fifo->dt_device->of_node, name, var);
+	if (rc) {
+		dev_err(fifo->dt_device, "couldn't read IP dts property '%s'",
+			name);
+		return rc;
+	}
+	dev_dbg(fifo->dt_device, "dts property '%s' = %u\n",
+		name, *var);
+
+	return 0;
+}
+
+static int axis_fifo_parse_dt(struct axis_fifo *fifo)
+{
+	int ret;
+	unsigned int value;
+
+	ret = get_dts_property(fifo, "xlnx,axi-str-rxd-tdata-width", &value);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,axi-str-rxd-tdata-width property\n");
+		goto end;
+	} else if (value != 32) {
+		dev_err(fifo->dt_device, "xlnx,axi-str-rxd-tdata-width only supports 32 bits\n");
+		ret = -EIO;
+		goto end;
+	}
+
+	ret = get_dts_property(fifo, "xlnx,axi-str-txd-tdata-width", &value);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,axi-str-txd-tdata-width property\n");
+		goto end;
+	} else if (value != 32) {
+		dev_err(fifo->dt_device, "xlnx,axi-str-txd-tdata-width only supports 32 bits\n");
+		ret = -EIO;
+		goto end;
+	}
+
+	ret = get_dts_property(fifo, "xlnx,rx-fifo-depth",
+			       &fifo->rx_fifo_depth);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,rx-fifo-depth property\n");
+		ret = -EIO;
+		goto end;
+	}
+
+	ret = get_dts_property(fifo, "xlnx,tx-fifo-depth",
+			       &fifo->tx_fifo_depth);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,tx-fifo-depth property\n");
+		ret = -EIO;
+		goto end;
+	}
+
+	/* IP sets TDFV to fifo depth - 4 so we will do the same */
+	fifo->tx_fifo_depth -= 4;
+
+	ret = get_dts_property(fifo, "xlnx,use-rx-data", &fifo->has_rx_fifo);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,use-rx-data property\n");
+		ret = -EIO;
+		goto end;
+	}
+
+	ret = get_dts_property(fifo, "xlnx,use-tx-data", &fifo->has_tx_fifo);
+	if (ret) {
+		dev_err(fifo->dt_device, "missing xlnx,use-tx-data property\n");
+		ret = -EIO;
+		goto end;
+	}
+
+end:
+	return ret;
+}
+
+static int axis_fifo_probe(struct platform_device *pdev)
+{
+	struct resource *r_mem; /* IO mem resources */
+	struct device *dev = &pdev->dev; /* OS device (from device tree) */
+	struct axis_fifo *fifo = NULL;
+	char *device_name;
+	int rc = 0; /* error return value */
+
+	/* ----------------------------
+	 *     init wrapper device
+	 * ----------------------------
+	 */
+
+	device_name = devm_kzalloc(dev, 32, GFP_KERNEL);
+	if (!device_name)
+		return -ENOMEM;
+
+	/* allocate device wrapper memory */
+	fifo = devm_kzalloc(dev, sizeof(*fifo), GFP_KERNEL);
+	if (!fifo)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, fifo);
+	fifo->dt_device = dev;
+
+	init_waitqueue_head(&fifo->read_queue);
+	init_waitqueue_head(&fifo->write_queue);
+
+	mutex_init(&fifo->read_lock);
+	mutex_init(&fifo->write_lock);
+
+	/* ----------------------------
+	 *   init device memory space
+	 * ----------------------------
+	 */
+
+	/* get iospace for the device */
+	r_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r_mem) {
+		dev_err(fifo->dt_device, "invalid address\n");
+		rc = -ENODEV;
+		goto err_initial;
+	}
+
+	/* request physical memory */
+	fifo->base_addr = devm_ioremap_resource(fifo->dt_device, r_mem);
+	if (IS_ERR(fifo->base_addr)) {
+		rc = PTR_ERR(fifo->base_addr);
+		goto err_initial;
+	}
+
+	dev_dbg(fifo->dt_device, "remapped memory to 0x%p\n", fifo->base_addr);
+
+	/* create unique device name */
+	snprintf(device_name, 32, "%s_%pa", DRIVER_NAME, &r_mem->start);
+	dev_dbg(fifo->dt_device, "device name [%s]\n", device_name);
+
+	/* ----------------------------
+	 *          init IP
+	 * ----------------------------
+	 */
+
+	rc = axis_fifo_parse_dt(fifo);
+	if (rc)
+		goto err_initial;
+
+	reset_ip_core(fifo);
+
+	/* ----------------------------
+	 *    init device interrupts
+	 * ----------------------------
+	 */
+
+	/* get IRQ resource */
+	rc = platform_get_irq(pdev, 0);
+	if (rc < 0)
+		goto err_initial;
+
+	/* request IRQ */
+	fifo->irq = rc;
+	rc = devm_request_irq(fifo->dt_device, fifo->irq, &axis_fifo_irq, 0,
+			      DRIVER_NAME, fifo);
+	if (rc) {
+		dev_err(fifo->dt_device, "couldn't allocate interrupt %i\n",
+			fifo->irq);
+		goto err_initial;
+	}
+
+	/* ----------------------------
+	 *      init char device
+	 * ----------------------------
+	 */
+
+	/* create character device */
+	fifo->miscdev.fops = &fops;
+	fifo->miscdev.minor = MISC_DYNAMIC_MINOR;
+	fifo->miscdev.name = device_name;
+	fifo->miscdev.groups = axis_fifo_attrs_groups;
+	fifo->miscdev.parent = dev;
+	rc = misc_register(&fifo->miscdev);
+	if (rc < 0)
+		goto err_initial;
+
+	dev_info(fifo->dt_device, "axis-fifo created at %pa mapped to 0x%pa, irq=%i\n",
+		 &r_mem->start, &fifo->base_addr, fifo->irq);
+
+	return 0;
+
+err_initial:
+	dev_set_drvdata(dev, NULL);
+	return rc;
+}
+
+static int axis_fifo_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct axis_fifo *fifo = dev_get_drvdata(dev);
+
+	misc_deregister(&fifo->miscdev);
+	dev_set_drvdata(dev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id axis_fifo_of_match[] = {
+	{ .compatible = "xlnx,axi-fifo-mm-s-4.1", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, axis_fifo_of_match);
+
+static struct platform_driver axis_fifo_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.of_match_table	= axis_fifo_of_match,
+	},
+	.probe		= axis_fifo_probe,
+	.remove		= axis_fifo_remove,
+};
+
+static int __init axis_fifo_init(void)
+{
+	if (read_timeout >= 0)
+		read_timeout = msecs_to_jiffies(read_timeout);
+	else
+		read_timeout = MAX_SCHEDULE_TIMEOUT;
+
+	if (write_timeout >= 0)
+		write_timeout = msecs_to_jiffies(write_timeout);
+	else
+		write_timeout = MAX_SCHEDULE_TIMEOUT;
+
+	pr_info("axis-fifo driver loaded with parameters read_timeout = %li, write_timeout = %li\n",
+		read_timeout, write_timeout);
+	return platform_driver_register(&axis_fifo_driver);
+}
+
+module_init(axis_fifo_init);
+
+static void __exit axis_fifo_exit(void)
+{
+	platform_driver_unregister(&axis_fifo_driver);
+}
+
+module_exit(axis_fifo_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jacob Feder <jacobsfeder@gmail.com>");
+MODULE_DESCRIPTION("Xilinx AXI-Stream FIFO v4.1 IP core driver");
diff --git a/drivers/staging/axis-fifo/axis-fifo.txt b/drivers/staging/axis-fifo/axis-fifo.txt
new file mode 100644
index 000000000..5828e1b8e
--- /dev/null
+++ b/drivers/staging/axis-fifo/axis-fifo.txt
@@ -0,0 +1,93 @@
+Xilinx AXI-Stream FIFO v4.1 IP core
+
+This IP core has read and write AXI-Stream FIFOs, the contents of which can
+be accessed from the AXI4 memory-mapped interface. This is useful for
+transferring data from a processor into the FPGA fabric. The driver creates
+a character device that can be read/written to with standard
+open/read/write/close.
+
+See Xilinx PG080 document for IP details.
+
+Currently supports only store-forward mode with a 32-bit
+AXI4-Lite interface. DOES NOT support:
+	- cut-through mode
+	- AXI4 (non-lite)
+
+Required properties:
+- compatible: Should be "xlnx,axi-fifo-mm-s-4.1"
+- interrupt-names: Should be "interrupt"
+- interrupt-parent: Should be <&intc>
+- interrupts: Should contain interrupts lines.
+- reg: Should contain registers location and length.
+- xlnx,axi-str-rxd-protocol: Should be "XIL_AXI_STREAM_ETH_DATA"
+- xlnx,axi-str-rxd-tdata-width: Should be <0x20>
+- xlnx,axi-str-txc-protocol: Should be "XIL_AXI_STREAM_ETH_CTRL"
+- xlnx,axi-str-txc-tdata-width: Should be <0x20>
+- xlnx,axi-str-txd-protocol: Should be "XIL_AXI_STREAM_ETH_DATA"
+- xlnx,axi-str-txd-tdata-width: Should be <0x20>
+- xlnx,axis-tdest-width: AXI-Stream TDEST width (ignored by the driver)
+- xlnx,axis-tid-width: AXI-Stream TID width (ignored by the driver)
+- xlnx,axis-tuser-width: AXI-Stream TUSER width (ignored by the driver)
+- xlnx,data-interface-type: Should be <0x0> (ignored by the driver)
+- xlnx,has-axis-tdest: Should be <0x0> (this feature isn't supported)
+- xlnx,has-axis-tid: Should be <0x0> (this feature isn't supported)
+- xlnx,has-axis-tkeep: Should be <0x0> (this feature isn't supported)
+- xlnx,has-axis-tstrb: Should be <0x0> (this feature isn't supported)
+- xlnx,has-axis-tuser: Should be <0x0> (this feature isn't supported)
+- xlnx,rx-fifo-depth: Depth of RX FIFO in words
+- xlnx,rx-fifo-pe-threshold: RX programmable empty interrupt threshold
+	(ignored by the driver)
+- xlnx,rx-fifo-pf-threshold: RX programmable full interrupt threshold
+	(ignored by the driver)
+- xlnx,s-axi-id-width: Should be <0x4> (ignored by the driver)
+- xlnx,s-axi4-data-width: Should be <0x20> (ignored by the driver)
+- xlnx,select-xpm: Should be <0x0> (ignored by the driver)
+- xlnx,tx-fifo-depth: Depth of TX FIFO in words
+- xlnx,tx-fifo-pe-threshold: TX programmable empty interrupt threshold
+	(ignored by the driver)
+- xlnx,tx-fifo-pf-threshold: TX programmable full interrupt threshold
+	(ignored by the driver)
+- xlnx,use-rx-cut-through: Should be <0x0> (this feature isn't supported)
+- xlnx,use-rx-data: <0x1> if RX FIFO is enabled, <0x0> otherwise
+- xlnx,use-tx-ctrl: Should be <0x0> (this feature isn't supported)
+- xlnx,use-tx-cut-through: Should be <0x0> (this feature isn't supported)
+- xlnx,use-tx-data: <0x1> if TX FIFO is enabled, <0x0> otherwise
+
+Example:
+
+axi_fifo_mm_s_0: axi_fifo_mm_s@43c00000 {
+	compatible = "xlnx,axi-fifo-mm-s-4.1";
+	interrupt-names = "interrupt";
+	interrupt-parent = <&intc>;
+	interrupts = <0 29 4>;
+	reg = <0x43c00000 0x10000>;
+	xlnx,axi-str-rxd-protocol = "XIL_AXI_STREAM_ETH_DATA";
+	xlnx,axi-str-rxd-tdata-width = <0x20>;
+	xlnx,axi-str-txc-protocol = "XIL_AXI_STREAM_ETH_CTRL";
+	xlnx,axi-str-txc-tdata-width = <0x20>;
+	xlnx,axi-str-txd-protocol = "XIL_AXI_STREAM_ETH_DATA";
+	xlnx,axi-str-txd-tdata-width = <0x20>;
+	xlnx,axis-tdest-width = <0x4>;
+	xlnx,axis-tid-width = <0x4>;
+	xlnx,axis-tuser-width = <0x4>;
+	xlnx,data-interface-type = <0x0>;
+	xlnx,has-axis-tdest = <0x0>;
+	xlnx,has-axis-tid = <0x0>;
+	xlnx,has-axis-tkeep = <0x0>;
+	xlnx,has-axis-tstrb = <0x0>;
+	xlnx,has-axis-tuser = <0x0>;
+	xlnx,rx-fifo-depth = <0x200>;
+	xlnx,rx-fifo-pe-threshold = <0x2>;
+	xlnx,rx-fifo-pf-threshold = <0x1fb>;
+	xlnx,s-axi-id-width = <0x4>;
+	xlnx,s-axi4-data-width = <0x20>;
+	xlnx,select-xpm = <0x0>;
+	xlnx,tx-fifo-depth = <0x8000>;
+	xlnx,tx-fifo-pe-threshold = <0x200>;
+	xlnx,tx-fifo-pf-threshold = <0x7ffb>;
+	xlnx,use-rx-cut-through = <0x0>;
+	xlnx,use-rx-data = <0x0>;
+	xlnx,use-tx-ctrl = <0x0>;
+	xlnx,use-tx-cut-through = <0x0>;
+	xlnx,use-tx-data = <0x1>;
+};
diff --git a/drivers/staging/board/Kconfig b/drivers/staging/board/Kconfig
new file mode 100644
index 000000000..b49216768
--- /dev/null
+++ b/drivers/staging/board/Kconfig
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+config STAGING_BOARD
+	bool "Staging Board Support"
+	depends on OF_ADDRESS && OF_IRQ && HAVE_CLK
+	help
+	  Staging board base is to support continuous upstream
+	  in-tree development and integration of platform devices.
+
+	  Helps developers integrate devices as platform devices for
+	  device drivers that only provide platform device bindings.
+	  This in turn allows for incremental development of both
+	  hardware feature support and DT binding work in parallel.
diff --git a/drivers/staging/board/Makefile b/drivers/staging/board/Makefile
new file mode 100644
index 000000000..ed7839752
--- /dev/null
+++ b/drivers/staging/board/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-y	:= board.o
+obj-$(CONFIG_ARCH_EMEV2)	+= kzm9d.o
+obj-$(CONFIG_ARCH_R8A7740)	+= armadillo800eva.o
diff --git a/drivers/staging/board/TODO b/drivers/staging/board/TODO
new file mode 100644
index 000000000..8db70e10a
--- /dev/null
+++ b/drivers/staging/board/TODO
@@ -0,0 +1,2 @@
+* replace platform device code with DT nodes once the driver supports DT
+* remove staging board code when no more platform devices are needed
diff --git a/drivers/staging/board/armadillo800eva.c b/drivers/staging/board/armadillo800eva.c
new file mode 100644
index 000000000..0225234dd
--- /dev/null
+++ b/drivers/staging/board/armadillo800eva.c
@@ -0,0 +1,88 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Staging board support for Armadillo 800 eva.
+ * Enable not-yet-DT-capable devices here.
+ *
+ * Based on board-armadillo800eva.c
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Copyright (C) 2012 Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include <video/sh_mobile_lcdc.h>
+
+#include "board.h"
+
+static struct fb_videomode lcdc0_mode = {
+	.name		= "AMPIER/AM-800480",
+	.xres		= 800,
+	.yres		= 480,
+	.left_margin	= 88,
+	.right_margin	= 40,
+	.hsync_len	= 128,
+	.upper_margin	= 20,
+	.lower_margin	= 5,
+	.vsync_len	= 5,
+	.sync		= 0,
+};
+
+static struct sh_mobile_lcdc_info lcdc0_info = {
+	.clock_source	= LCDC_CLK_BUS,
+	.ch[0] = {
+		.chan		= LCDC_CHAN_MAINLCD,
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.interface_type	= RGB24,
+		.clock_divider	= 5,
+		.flags		= 0,
+		.lcd_modes	= &lcdc0_mode,
+		.num_modes	= 1,
+		.panel_cfg = {
+			.width	= 111,
+			.height = 68,
+		},
+	},
+};
+
+static struct resource lcdc0_resources[] = {
+	DEFINE_RES_MEM_NAMED(0xfe940000, 0x4000, "LCD0"),
+	DEFINE_RES_IRQ(177 + 32),
+};
+
+static struct platform_device lcdc0_device = {
+	.name		= "sh_mobile_lcdc_fb",
+	.num_resources	= ARRAY_SIZE(lcdc0_resources),
+	.resource	= lcdc0_resources,
+	.id		= 0,
+	.dev	= {
+		.platform_data	= &lcdc0_info,
+		.coherent_dma_mask = DMA_BIT_MASK(32),
+	},
+};
+
+static const struct board_staging_clk lcdc0_clocks[] __initconst = {
+	{ "lcdc0", NULL, "sh_mobile_lcdc_fb.0" },
+};
+
+static const struct board_staging_dev armadillo800eva_devices[] __initconst = {
+	{
+		.pdev	 = &lcdc0_device,
+		.clocks	 = lcdc0_clocks,
+		.nclocks = ARRAY_SIZE(lcdc0_clocks),
+		.domain	 = "/system-controller@e6180000/pm-domains/c5/a4lc@1"
+	},
+};
+
+static void __init armadillo800eva_init(void)
+{
+	board_staging_gic_setup_xlate("arm,pl390", 32);
+	board_staging_register_devices(armadillo800eva_devices,
+				       ARRAY_SIZE(armadillo800eva_devices));
+}
+
+board_staging("renesas,armadillo800eva", armadillo800eva_init);
diff --git a/drivers/staging/board/board.c b/drivers/staging/board/board.c
new file mode 100644
index 000000000..f980af037
--- /dev/null
+++ b/drivers/staging/board/board.c
@@ -0,0 +1,204 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2014 Magnus Damm
+ * Copyright (C) 2015 Glider bvba
+ */
+
+#define pr_fmt(fmt)	"board_staging: "  fmt
+
+#include <linux/clkdev.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_domain.h>
+
+#include "board.h"
+
+static struct device_node *irqc_node __initdata;
+static unsigned int irqc_base __initdata;
+
+static bool find_by_address(u64 base_address)
+{
+	struct device_node *dn = of_find_all_nodes(NULL);
+	struct resource res;
+
+	while (dn) {
+		if (!of_address_to_resource(dn, 0, &res)) {
+			if (res.start == base_address) {
+				of_node_put(dn);
+				return true;
+			}
+		}
+		dn = of_find_all_nodes(dn);
+	}
+
+	return false;
+}
+
+bool __init board_staging_dt_node_available(const struct resource *resource,
+					    unsigned int num_resources)
+{
+	unsigned int i;
+
+	for (i = 0; i < num_resources; i++) {
+		const struct resource *r = resource + i;
+
+		if (resource_type(r) == IORESOURCE_MEM)
+			if (find_by_address(r->start))
+				return true; /* DT node available */
+	}
+
+	return false; /* Nothing found */
+}
+
+int __init board_staging_gic_setup_xlate(const char *gic_match,
+					 unsigned int base)
+{
+	WARN_ON(irqc_node);
+
+	irqc_node = of_find_compatible_node(NULL, NULL, gic_match);
+
+	WARN_ON(!irqc_node);
+	if (!irqc_node)
+		return -ENOENT;
+
+	irqc_base = base;
+	return 0;
+}
+
+static void __init gic_fixup_resource(struct resource *res)
+{
+	struct of_phandle_args irq_data;
+	unsigned int hwirq = res->start;
+	unsigned int virq;
+
+	if (resource_type(res) != IORESOURCE_IRQ || !irqc_node)
+		return;
+
+	irq_data.np = irqc_node;
+	irq_data.args_count = 3;
+	irq_data.args[0] = 0;
+	irq_data.args[1] = hwirq - irqc_base;
+	switch (res->flags &
+		(IORESOURCE_IRQ_LOWEDGE | IORESOURCE_IRQ_HIGHEDGE |
+		 IORESOURCE_IRQ_LOWLEVEL | IORESOURCE_IRQ_HIGHLEVEL)) {
+	case IORESOURCE_IRQ_LOWEDGE:
+		irq_data.args[2] = IRQ_TYPE_EDGE_FALLING;
+		break;
+	case IORESOURCE_IRQ_HIGHEDGE:
+		irq_data.args[2] = IRQ_TYPE_EDGE_RISING;
+		break;
+	case IORESOURCE_IRQ_LOWLEVEL:
+		irq_data.args[2] = IRQ_TYPE_LEVEL_LOW;
+		break;
+	case IORESOURCE_IRQ_HIGHLEVEL:
+	default:
+		irq_data.args[2] = IRQ_TYPE_LEVEL_HIGH;
+		break;
+	}
+
+	virq = irq_create_of_mapping(&irq_data);
+	if (WARN_ON(!virq))
+		return;
+
+	pr_debug("hwirq %u -> virq %u\n", hwirq, virq);
+	res->start = virq;
+}
+
+void __init board_staging_gic_fixup_resources(struct resource *res,
+					      unsigned int nres)
+{
+	unsigned int i;
+
+	for (i = 0; i < nres; i++)
+		gic_fixup_resource(&res[i]);
+}
+
+int __init board_staging_register_clock(const struct board_staging_clk *bsc)
+{
+	int error;
+
+	pr_debug("Aliasing clock %s for con_id %s dev_id %s\n", bsc->clk,
+		 bsc->con_id, bsc->dev_id);
+	error = clk_add_alias(bsc->con_id, bsc->dev_id, bsc->clk, NULL);
+	if (error)
+		pr_err("Failed to alias clock %s (%d)\n", bsc->clk, error);
+
+	return error;
+}
+
+#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
+static int board_staging_add_dev_domain(struct platform_device *pdev,
+					const char *domain)
+{
+	struct device *dev = &pdev->dev;
+	struct of_phandle_args pd_args;
+	struct device_node *np;
+
+	np = of_find_node_by_path(domain);
+	if (!np) {
+		pr_err("Cannot find domain node %s\n", domain);
+		return -ENOENT;
+	}
+
+	pd_args.np = np;
+	pd_args.args_count = 0;
+
+	/* Initialization similar to device_pm_init_common() */
+	spin_lock_init(&dev->power.lock);
+	dev->power.early_init = true;
+
+	return of_genpd_add_device(&pd_args, dev);
+}
+#else
+static inline int board_staging_add_dev_domain(struct platform_device *pdev,
+					       const char *domain)
+{
+	return 0;
+}
+#endif
+
+int __init board_staging_register_device(const struct board_staging_dev *dev)
+{
+	struct platform_device *pdev = dev->pdev;
+	unsigned int i;
+	int error;
+
+	pr_debug("Trying to register device %s\n", pdev->name);
+	if (board_staging_dt_node_available(pdev->resource,
+					    pdev->num_resources)) {
+		pr_warn("Skipping %s, already in DT\n", pdev->name);
+		return -EEXIST;
+	}
+
+	board_staging_gic_fixup_resources(pdev->resource, pdev->num_resources);
+
+	for (i = 0; i < dev->nclocks; i++)
+		board_staging_register_clock(&dev->clocks[i]);
+
+	if (dev->domain)
+		board_staging_add_dev_domain(pdev, dev->domain);
+
+	error = platform_device_register(pdev);
+	if (error) {
+		pr_err("Failed to register device %s (%d)\n", pdev->name,
+		       error);
+		return error;
+	}
+
+	return error;
+}
+
+void __init board_staging_register_devices(const struct board_staging_dev *devs,
+					   unsigned int ndevs)
+{
+	unsigned int i;
+
+	for (i = 0; i < ndevs; i++)
+		board_staging_register_device(&devs[i]);
+}
diff --git a/drivers/staging/board/board.h b/drivers/staging/board/board.h
new file mode 100644
index 000000000..5609daf4d
--- /dev/null
+++ b/drivers/staging/board/board.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __BOARD_H__
+#define __BOARD_H__
+
+#include <linux/init.h>
+#include <linux/of.h>
+
+struct board_staging_clk {
+	const char *clk;
+	const char *con_id;
+	const char *dev_id;
+};
+
+struct board_staging_dev {
+	/* Platform Device */
+	struct platform_device *pdev;
+	/* Clocks (optional) */
+	const struct board_staging_clk *clocks;
+	unsigned int nclocks;
+	/* Generic PM Domain (optional) */
+	const char *domain;
+};
+
+struct resource;
+
+bool board_staging_dt_node_available(const struct resource *resource,
+				     unsigned int num_resources);
+int board_staging_gic_setup_xlate(const char *gic_match, unsigned int base);
+void board_staging_gic_fixup_resources(struct resource *res, unsigned int nres);
+int board_staging_register_clock(const struct board_staging_clk *bsc);
+int board_staging_register_device(const struct board_staging_dev *dev);
+void board_staging_register_devices(const struct board_staging_dev *devs,
+				    unsigned int ndevs);
+
+#define board_staging(str, fn)			\
+static int __init runtime_board_check(void)	\
+{						\
+	if (of_machine_is_compatible(str))	\
+		fn();				\
+						\
+	return 0;				\
+}						\
+						\
+device_initcall(runtime_board_check)
+
+#endif /* __BOARD_H__ */
diff --git a/drivers/staging/board/kzm9d.c b/drivers/staging/board/kzm9d.c
new file mode 100644
index 000000000..d449a8374
--- /dev/null
+++ b/drivers/staging/board/kzm9d.c
@@ -0,0 +1,26 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Staging board support for KZM9D. Enable not-yet-DT-capable devices here. */
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include "board.h"
+
+static struct resource usbs1_res[] __initdata = {
+	DEFINE_RES_MEM(0xe2800000, 0x2000),
+	DEFINE_RES_IRQ(159),
+};
+
+static void __init kzm9d_init(void)
+{
+	board_staging_gic_setup_xlate("arm,pl390", 32);
+
+	if (!board_staging_dt_node_available(usbs1_res,
+					     ARRAY_SIZE(usbs1_res))) {
+		board_staging_gic_fixup_resources(usbs1_res,
+						  ARRAY_SIZE(usbs1_res));
+		platform_device_register_simple("emxx_udc", -1, usbs1_res,
+						ARRAY_SIZE(usbs1_res));
+	}
+}
+
+board_staging("renesas,kzm9d", kzm9d_init);
diff --git a/drivers/staging/emxx_udc/Kconfig b/drivers/staging/emxx_udc/Kconfig
new file mode 100644
index 000000000..e7a95b3b6
--- /dev/null
+++ b/drivers/staging/emxx_udc/Kconfig
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+config USB_EMXX
+	tristate "EMXX USB Function Device Controller"
+	depends on USB_GADGET && (ARCH_RENESAS || COMPILE_TEST)
+	help
+	   The Emma Mobile series of SoCs from Renesas Electronics and
+	   former NEC Electronics include USB Function hardware.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "emxx_udc" and force all
+	   gadget drivers to also be dynamically linked.
diff --git a/drivers/staging/emxx_udc/Makefile b/drivers/staging/emxx_udc/Makefile
new file mode 100644
index 000000000..569c5e9a9
--- /dev/null
+++ b/drivers/staging/emxx_udc/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_USB_EMXX)	:= emxx_udc.o
diff --git a/drivers/staging/emxx_udc/TODO b/drivers/staging/emxx_udc/TODO
new file mode 100644
index 000000000..471529a47
--- /dev/null
+++ b/drivers/staging/emxx_udc/TODO
@@ -0,0 +1,6 @@
+* add clock framework support (platform device with CCF needs special care)
+* break out board-specific VBUS GPIO to work with multiplatform
+* convert VBUS GPIO to use GPIO descriptors from <linux/gpio/consumer.h>
+  and stop using the old GPIO API
+* DT bindings
+* move driver into drivers/usb/gadget/
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
new file mode 100644
index 000000000..edd20a03f
--- /dev/null
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -0,0 +1,3228 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *  drivers/usb/gadget/emxx_udc.c
+ *     EMXX FCD (Function Controller Driver) for USB.
+ *
+ *  Copyright (C) 2010 Renesas Electronics Corporation
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/clk.h>
+#include <linux/ctype.h>
+#include <linux/string.h>
+#include <linux/dma-mapping.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+#include <linux/irq.h>
+#include <linux/gpio/consumer.h>
+
+#include "emxx_udc.h"
+
+#define	DRIVER_DESC	"EMXX UDC driver"
+#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
+
+static struct gpio_desc *vbus_gpio;
+static int vbus_irq;
+
+static const char	driver_name[] = "emxx_udc";
+
+/*===========================================================================*/
+/* Prototype */
+static void _nbu2ss_ep_dma_abort(struct nbu2ss_udc *, struct nbu2ss_ep *);
+static void _nbu2ss_ep0_enable(struct nbu2ss_udc *);
+/*static void _nbu2ss_ep0_disable(struct nbu2ss_udc *);*/
+static void _nbu2ss_ep_done(struct nbu2ss_ep *, struct nbu2ss_req *, int);
+static void _nbu2ss_set_test_mode(struct nbu2ss_udc *, u32 mode);
+static void _nbu2ss_endpoint_toggle_reset(struct nbu2ss_udc *udc, u8 ep_adrs);
+
+static int _nbu2ss_pullup(struct nbu2ss_udc *, int);
+static void _nbu2ss_fifo_flush(struct nbu2ss_udc *, struct nbu2ss_ep *);
+
+/*===========================================================================*/
+/* Macro */
+#define	_nbu2ss_zero_len_pkt(udc, epnum)	\
+	_nbu2ss_ep_in_end(udc, epnum, 0, 0)
+
+/*===========================================================================*/
+/* Global */
+static struct nbu2ss_udc udc_controller;
+
+/*-------------------------------------------------------------------------*/
+/* Read */
+static inline u32 _nbu2ss_readl(void __iomem *address)
+{
+	return __raw_readl(address);
+}
+
+/*-------------------------------------------------------------------------*/
+/* Write */
+static inline void _nbu2ss_writel(void __iomem *address, u32 udata)
+{
+	__raw_writel(udata, address);
+}
+
+/*-------------------------------------------------------------------------*/
+/* Set Bit */
+static inline void _nbu2ss_bitset(void __iomem *address, u32 udata)
+{
+	u32	reg_dt = __raw_readl(address) | (udata);
+
+	__raw_writel(reg_dt, address);
+}
+
+/*-------------------------------------------------------------------------*/
+/* Clear Bit */
+static inline void _nbu2ss_bitclr(void __iomem *address, u32 udata)
+{
+	u32	reg_dt = __raw_readl(address) & ~(udata);
+
+	__raw_writel(reg_dt, address);
+}
+
+#ifdef UDC_DEBUG_DUMP
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_dump_register(struct nbu2ss_udc *udc)
+{
+	int		i;
+	u32 reg_data;
+
+	pr_info("=== %s()\n", __func__);
+
+	if (!udc) {
+		pr_err("%s udc == NULL\n", __func__);
+		return;
+	}
+
+	spin_unlock(&udc->lock);
+
+	dev_dbg(&udc->dev, "\n-USB REG-\n");
+	for (i = 0x0 ; i < USB_BASE_SIZE ; i += 16) {
+		reg_data = _nbu2ss_readl(IO_ADDRESS(USB_BASE_ADDRESS + i));
+		dev_dbg(&udc->dev, "USB%04x =%08x", i, (int)reg_data);
+
+		reg_data = _nbu2ss_readl(IO_ADDRESS(USB_BASE_ADDRESS + i + 4));
+		dev_dbg(&udc->dev, " %08x", (int)reg_data);
+
+		reg_data = _nbu2ss_readl(IO_ADDRESS(USB_BASE_ADDRESS + i + 8));
+		dev_dbg(&udc->dev, " %08x", (int)reg_data);
+
+		reg_data = _nbu2ss_readl(IO_ADDRESS(USB_BASE_ADDRESS + i + 12));
+		dev_dbg(&udc->dev, " %08x\n", (int)reg_data);
+	}
+
+	spin_lock(&udc->lock);
+}
+#endif /* UDC_DEBUG_DUMP */
+
+/*-------------------------------------------------------------------------*/
+/* Endpoint 0 Callback (Complete) */
+static void _nbu2ss_ep0_complete(struct usb_ep *_ep, struct usb_request *_req)
+{
+	u8		recipient;
+	u16		selector;
+	u16		wIndex;
+	u32		test_mode;
+	struct usb_ctrlrequest	*p_ctrl;
+	struct nbu2ss_udc *udc;
+
+	if (!_ep || !_req)
+		return;
+
+	udc = (struct nbu2ss_udc *)_req->context;
+	p_ctrl = &udc->ctrl;
+	if ((p_ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+		if (p_ctrl->bRequest == USB_REQ_SET_FEATURE) {
+			/*-------------------------------------------------*/
+			/* SET_FEATURE */
+			recipient = (u8)(p_ctrl->bRequestType & USB_RECIP_MASK);
+			selector  = le16_to_cpu(p_ctrl->wValue);
+			if ((recipient == USB_RECIP_DEVICE) &&
+			    (selector == USB_DEVICE_TEST_MODE)) {
+				wIndex = le16_to_cpu(p_ctrl->wIndex);
+				test_mode = (u32)(wIndex >> 8);
+				_nbu2ss_set_test_mode(udc, test_mode);
+			}
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+/* Initialization usb_request */
+static void _nbu2ss_create_ep0_packet(struct nbu2ss_udc *udc,
+				      void *p_buf, unsigned int length)
+{
+	udc->ep0_req.req.buf		= p_buf;
+	udc->ep0_req.req.length		= length;
+	udc->ep0_req.req.dma		= 0;
+	udc->ep0_req.req.zero		= true;
+	udc->ep0_req.req.complete	= _nbu2ss_ep0_complete;
+	udc->ep0_req.req.status		= -EINPROGRESS;
+	udc->ep0_req.req.context	= udc;
+	udc->ep0_req.req.actual		= 0;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Acquisition of the first address of RAM(FIFO) */
+static u32 _nbu2ss_get_begin_ram_address(struct nbu2ss_udc *udc)
+{
+	u32		num, buf_type;
+	u32		data, last_ram_adr, use_ram_size;
+
+	struct ep_regs __iomem *p_ep_regs;
+
+	last_ram_adr = (D_RAM_SIZE_CTRL / sizeof(u32)) * 2;
+	use_ram_size = 0;
+
+	for (num = 0; num < NUM_ENDPOINTS - 1; num++) {
+		p_ep_regs = &udc->p_regs->EP_REGS[num];
+		data = _nbu2ss_readl(&p_ep_regs->EP_PCKT_ADRS);
+		buf_type = _nbu2ss_readl(&p_ep_regs->EP_CONTROL) & EPN_BUF_TYPE;
+		if (buf_type == 0) {
+			/* Single Buffer */
+			use_ram_size += (data & EPN_MPKT) / sizeof(u32);
+		} else {
+			/* Double Buffer */
+			use_ram_size += ((data & EPN_MPKT) / sizeof(u32)) * 2;
+		}
+
+		if ((data >> 16) > last_ram_adr)
+			last_ram_adr = data >> 16;
+	}
+
+	return last_ram_adr + use_ram_size;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Construction of Endpoint */
+static int _nbu2ss_ep_init(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	u32		num;
+	u32		data;
+	u32		begin_adrs;
+
+	if (ep->epnum == 0)
+		return	-EINVAL;
+
+	num = ep->epnum - 1;
+
+	/*-------------------------------------------------------------*/
+	/* RAM Transfer Address */
+	begin_adrs = _nbu2ss_get_begin_ram_address(udc);
+	data = (begin_adrs << 16) | ep->ep.maxpacket;
+	_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_PCKT_ADRS, data);
+
+	/*-------------------------------------------------------------*/
+	/* Interrupt Enable */
+	data = 1 << (ep->epnum + 8);
+	_nbu2ss_bitset(&udc->p_regs->USB_INT_ENA, data);
+
+	/*-------------------------------------------------------------*/
+	/* Endpoint Type(Mode) */
+	/*   Bulk, Interrupt, ISO */
+	switch (ep->ep_type) {
+	case USB_ENDPOINT_XFER_BULK:
+		data = EPN_BULK;
+		break;
+
+	case USB_ENDPOINT_XFER_INT:
+		data = EPN_BUF_SINGLE | EPN_INTERRUPT;
+		break;
+
+	case USB_ENDPOINT_XFER_ISOC:
+		data = EPN_ISO;
+		break;
+
+	default:
+		data = 0;
+		break;
+	}
+
+	_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+	_nbu2ss_endpoint_toggle_reset(udc, (ep->epnum | ep->direct));
+
+	if (ep->direct == USB_DIR_OUT) {
+		/*---------------------------------------------------------*/
+		/* OUT */
+		data = EPN_EN | EPN_BCLR | EPN_DIR0;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_ONAK | EPN_OSTL_EN | EPN_OSTL;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_OUT_EN | EPN_OUT_END_EN;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
+	} else {
+		/*---------------------------------------------------------*/
+		/* IN */
+		data = EPN_EN | EPN_BCLR | EPN_AUTO;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_ISTL;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_IN_EN | EPN_IN_END_EN;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Release of Endpoint */
+static int _nbu2ss_epn_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	u32		num;
+	u32		data;
+
+	if ((ep->epnum == 0) || (udc->vbus_active == 0))
+		return	-EINVAL;
+
+	num = ep->epnum - 1;
+
+	/*-------------------------------------------------------------*/
+	/* RAM Transfer Address */
+	_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_PCKT_ADRS, 0);
+
+	/*-------------------------------------------------------------*/
+	/* Interrupt Disable */
+	data = 1 << (ep->epnum + 8);
+	_nbu2ss_bitclr(&udc->p_regs->USB_INT_ENA, data);
+
+	if (ep->direct == USB_DIR_OUT) {
+		/*---------------------------------------------------------*/
+		/* OUT */
+		data = EPN_ONAK | EPN_BCLR;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_EN | EPN_DIR0;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_OUT_EN | EPN_OUT_END_EN;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
+	} else {
+		/*---------------------------------------------------------*/
+		/* IN */
+		data = EPN_BCLR;
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_EN | EPN_AUTO;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+
+		data = EPN_IN_EN | EPN_IN_END_EN;
+		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+/* DMA setting (without Endpoint 0) */
+static void _nbu2ss_ep_dma_init(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	u32		num;
+	u32		data;
+
+	data = _nbu2ss_readl(&udc->p_regs->USBSSCONF);
+	if (((ep->epnum == 0) || (data & (1 << ep->epnum)) == 0))
+		return;		/* Not Support DMA */
+
+	num = ep->epnum - 1;
+
+	if (ep->direct == USB_DIR_OUT) {
+		/*---------------------------------------------------------*/
+		/* OUT */
+		data = ep->ep.maxpacket;
+		_nbu2ss_writel(&udc->p_regs->EP_DCR[num].EP_DCR2, data);
+
+		/*---------------------------------------------------------*/
+		/* Transfer Direct */
+		data = DCR1_EPN_DIR0;
+		_nbu2ss_bitset(&udc->p_regs->EP_DCR[num].EP_DCR1, data);
+
+		/*---------------------------------------------------------*/
+		/* DMA Mode etc. */
+		data = EPN_STOP_MODE | EPN_STOP_SET  | EPN_DMAMODE0;
+		_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_DMA_CTRL, data);
+	} else {
+		/*---------------------------------------------------------*/
+		/* IN */
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, EPN_AUTO);
+
+		/*---------------------------------------------------------*/
+		/* DMA Mode etc. */
+		data = EPN_BURST_SET | EPN_DMAMODE0;
+		_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_DMA_CTRL, data);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+/* DMA setting release */
+static void _nbu2ss_ep_dma_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	u32		num;
+	u32		data;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (udc->vbus_active == 0)
+		return;		/* VBUS OFF */
+
+	data = _nbu2ss_readl(&preg->USBSSCONF);
+	if ((ep->epnum == 0) || ((data & (1 << ep->epnum)) == 0))
+		return;		/* Not Support DMA */
+
+	num = ep->epnum - 1;
+
+	_nbu2ss_ep_dma_abort(udc, ep);
+
+	if (ep->direct == USB_DIR_OUT) {
+		/*---------------------------------------------------------*/
+		/* OUT */
+		_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, 0);
+		_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPN_DIR0);
+		_nbu2ss_writel(&preg->EP_REGS[num].EP_DMA_CTRL, 0);
+	} else {
+		/*---------------------------------------------------------*/
+		/* IN */
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
+		_nbu2ss_writel(&preg->EP_REGS[num].EP_DMA_CTRL, 0);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+/* Abort DMA */
+static void _nbu2ss_ep_dma_abort(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	_nbu2ss_bitclr(&preg->EP_DCR[ep->epnum - 1].EP_DCR1, DCR1_EPN_REQEN);
+	mdelay(DMA_DISABLE_TIME);	/* DCR1_EPN_REQEN Clear */
+	_nbu2ss_bitclr(&preg->EP_REGS[ep->epnum - 1].EP_DMA_CTRL, EPN_DMA_EN);
+}
+
+/*-------------------------------------------------------------------------*/
+/* Start IN Transfer */
+static void _nbu2ss_ep_in_end(struct nbu2ss_udc *udc,
+			      u32 epnum, u32 data32, u32 length)
+{
+	u32		data;
+	u32		num;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (length >= sizeof(u32))
+		return;
+
+	if (epnum == 0) {
+		_nbu2ss_bitclr(&preg->EP0_CONTROL, EP0_AUTO);
+
+		/* Writing of 1-4 bytes */
+		if (length)
+			_nbu2ss_writel(&preg->EP0_WRITE, data32);
+
+		data = ((length << 5) & EP0_DW) | EP0_DEND;
+		_nbu2ss_writel(&preg->EP0_CONTROL, data);
+
+		_nbu2ss_bitset(&preg->EP0_CONTROL, EP0_AUTO);
+	} else {
+		num = epnum - 1;
+
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
+
+		/* Writing of 1-4 bytes */
+		if (length)
+			_nbu2ss_writel(&preg->EP_REGS[num].EP_WRITE, data32);
+
+		data = (((length) << 5) & EPN_DW) | EPN_DEND;
+		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, data);
+
+		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
+	}
+}
+
+#ifdef USE_DMA
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_dma_map_single(struct nbu2ss_udc *udc,
+				   struct nbu2ss_ep *ep,
+				   struct nbu2ss_req *req, u8 direct)
+{
+	if (req->req.dma == DMA_ADDR_INVALID) {
+		if (req->unaligned) {
+			req->req.dma = ep->phys_buf;
+		} else {
+			req->req.dma = dma_map_single(udc->gadget.dev.parent,
+						      req->req.buf,
+						      req->req.length,
+						      (direct == USB_DIR_IN)
+						      ? DMA_TO_DEVICE
+						      : DMA_FROM_DEVICE);
+		}
+		req->mapped = 1;
+	} else {
+		if (!req->unaligned)
+			dma_sync_single_for_device(udc->gadget.dev.parent,
+						   req->req.dma,
+						   req->req.length,
+						   (direct == USB_DIR_IN)
+						   ? DMA_TO_DEVICE
+						   : DMA_FROM_DEVICE);
+
+		req->mapped = 0;
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_dma_unmap_single(struct nbu2ss_udc *udc,
+				     struct nbu2ss_ep *ep,
+				     struct nbu2ss_req *req, u8 direct)
+{
+	u8		data[4];
+	u8		*p;
+	u32		count = 0;
+
+	if (direct == USB_DIR_OUT) {
+		count = req->req.actual % 4;
+		if (count) {
+			p = req->req.buf;
+			p += (req->req.actual - count);
+			memcpy(data, p, count);
+		}
+	}
+
+	if (req->mapped) {
+		if (req->unaligned) {
+			if (direct == USB_DIR_OUT)
+				memcpy(req->req.buf, ep->virt_buf,
+				       req->req.actual & 0xfffffffc);
+		} else {
+			dma_unmap_single(udc->gadget.dev.parent,
+					 req->req.dma, req->req.length,
+				(direct == USB_DIR_IN)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+		}
+		req->req.dma = DMA_ADDR_INVALID;
+		req->mapped = 0;
+	} else {
+		if (!req->unaligned)
+			dma_sync_single_for_cpu(udc->gadget.dev.parent,
+						req->req.dma, req->req.length,
+				(direct == USB_DIR_IN)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+	}
+
+	if (count) {
+		p = req->req.buf;
+		p += (req->req.actual - count);
+		memcpy(p, data, count);
+	}
+}
+#endif
+
+/*-------------------------------------------------------------------------*/
+/* Endpoint 0 OUT Transfer (PIO) */
+static int ep0_out_pio(struct nbu2ss_udc *udc, u8 *buf, u32 length)
+{
+	u32		i;
+	u32 numreads = length / sizeof(u32);
+	union usb_reg_access *buf32 = (union usb_reg_access *)buf;
+
+	if (!numreads)
+		return 0;
+
+	/* PIO Read */
+	for (i = 0; i < numreads; i++) {
+		buf32->dw = _nbu2ss_readl(&udc->p_regs->EP0_READ);
+		buf32++;
+	}
+
+	return  numreads * sizeof(u32);
+}
+
+/*-------------------------------------------------------------------------*/
+/* Endpoint 0 OUT Transfer (PIO, OverBytes) */
+static int ep0_out_overbytes(struct nbu2ss_udc *udc, u8 *p_buf, u32 length)
+{
+	u32		i;
+	u32		i_read_size = 0;
+	union usb_reg_access  temp_32;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
+
+	if ((length > 0) && (length < sizeof(u32))) {
+		temp_32.dw = _nbu2ss_readl(&udc->p_regs->EP0_READ);
+		for (i = 0 ; i < length ; i++)
+			p_buf_32->byte.DATA[i] = temp_32.byte.DATA[i];
+		i_read_size += length;
+	}
+
+	return i_read_size;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Endpoint 0 IN Transfer (PIO) */
+static int EP0_in_PIO(struct nbu2ss_udc *udc, u8 *p_buf, u32 length)
+{
+	u32		i;
+	u32		i_max_length   = EP0_PACKETSIZE;
+	u32		i_word_length  = 0;
+	u32		i_write_length = 0;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
+
+	/*------------------------------------------------------------*/
+	/* Transfer Length */
+	if (i_max_length < length)
+		i_word_length = i_max_length / sizeof(u32);
+	else
+		i_word_length = length / sizeof(u32);
+
+	/*------------------------------------------------------------*/
+	/* PIO */
+	for (i = 0; i < i_word_length; i++) {
+		_nbu2ss_writel(&udc->p_regs->EP0_WRITE, p_buf_32->dw);
+		p_buf_32++;
+		i_write_length += sizeof(u32);
+	}
+
+	return i_write_length;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Endpoint 0 IN Transfer (PIO, OverBytes) */
+static int ep0_in_overbytes(struct nbu2ss_udc *udc,
+			    u8 *p_buf,
+			    u32 i_remain_size)
+{
+	u32		i;
+	union usb_reg_access  temp_32;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
+
+	if ((i_remain_size > 0) && (i_remain_size < sizeof(u32))) {
+		for (i = 0 ; i < i_remain_size ; i++)
+			temp_32.byte.DATA[i] = p_buf_32->byte.DATA[i];
+		_nbu2ss_ep_in_end(udc, 0, temp_32.dw, i_remain_size);
+
+		return i_remain_size;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Transfer NULL Packet (Epndoint 0) */
+static int EP0_send_NULL(struct nbu2ss_udc *udc, bool pid_flag)
+{
+	u32		data;
+
+	data = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
+	data &= ~(u32)EP0_INAK;
+
+	if (pid_flag)
+		data |= (EP0_INAK_EN | EP0_PIDCLR | EP0_DEND);
+	else
+		data |= (EP0_INAK_EN | EP0_DEND);
+
+	_nbu2ss_writel(&udc->p_regs->EP0_CONTROL, data);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Receive NULL Packet (Endpoint 0) */
+static int EP0_receive_NULL(struct nbu2ss_udc *udc, bool pid_flag)
+{
+	u32		data;
+
+	data = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
+	data &= ~(u32)EP0_ONAK;
+
+	if (pid_flag)
+		data |= EP0_PIDCLR;
+
+	_nbu2ss_writel(&udc->p_regs->EP0_CONTROL, data);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_ep0_in_transfer(struct nbu2ss_udc *udc,
+				   struct nbu2ss_req *req)
+{
+	u8		*p_buffer;			/* IN Data Buffer */
+	u32		data;
+	u32		i_remain_size = 0;
+	int		result = 0;
+
+	/*-------------------------------------------------------------*/
+	/* End confirmation */
+	if (req->req.actual == req->req.length) {
+		if ((req->req.actual % EP0_PACKETSIZE) == 0) {
+			if (req->zero) {
+				req->zero = false;
+				EP0_send_NULL(udc, false);
+				return 1;
+			}
+		}
+
+		return 0;		/* Transfer End */
+	}
+
+	/*-------------------------------------------------------------*/
+	/* NAK release */
+	data = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
+	data |= EP0_INAK_EN;
+	data &= ~(u32)EP0_INAK;
+	_nbu2ss_writel(&udc->p_regs->EP0_CONTROL, data);
+
+	i_remain_size = req->req.length - req->req.actual;
+	p_buffer = (u8 *)req->req.buf;
+	p_buffer += req->req.actual;
+
+	/*-------------------------------------------------------------*/
+	/* Data transfer */
+	result = EP0_in_PIO(udc, p_buffer, i_remain_size);
+
+	req->div_len = result;
+	i_remain_size -= result;
+
+	if (i_remain_size == 0) {
+		EP0_send_NULL(udc, false);
+		return result;
+	}
+
+	if ((i_remain_size < sizeof(u32)) && (result != EP0_PACKETSIZE)) {
+		p_buffer += result;
+		result += ep0_in_overbytes(udc, p_buffer, i_remain_size);
+		req->div_len = result;
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_ep0_out_transfer(struct nbu2ss_udc *udc,
+				    struct nbu2ss_req *req)
+{
+	u8		*p_buffer;
+	u32		i_remain_size;
+	u32		i_recv_length;
+	int		result = 0;
+	int		f_rcv_zero;
+
+	/*-------------------------------------------------------------*/
+	/* Receive data confirmation */
+	i_recv_length = _nbu2ss_readl(&udc->p_regs->EP0_LENGTH) & EP0_LDATA;
+	if (i_recv_length != 0) {
+		f_rcv_zero = 0;
+
+		i_remain_size = req->req.length - req->req.actual;
+		p_buffer = (u8 *)req->req.buf;
+		p_buffer += req->req.actual;
+
+		result = ep0_out_pio(udc, p_buffer
+					, min(i_remain_size, i_recv_length));
+		if (result < 0)
+			return result;
+
+		req->req.actual += result;
+		i_recv_length -= result;
+
+		if ((i_recv_length > 0) && (i_recv_length < sizeof(u32))) {
+			p_buffer += result;
+			i_remain_size -= result;
+
+			result = ep0_out_overbytes(udc, p_buffer
+					, min(i_remain_size, i_recv_length));
+			req->req.actual += result;
+		}
+	} else {
+		f_rcv_zero = 1;
+	}
+
+	/*-------------------------------------------------------------*/
+	/* End confirmation */
+	if (req->req.actual == req->req.length) {
+		if ((req->req.actual % EP0_PACKETSIZE) == 0) {
+			if (req->zero) {
+				req->zero = false;
+				EP0_receive_NULL(udc, false);
+				return 1;
+			}
+		}
+
+		return 0;		/* Transfer End */
+	}
+
+	if ((req->req.actual % EP0_PACKETSIZE) != 0)
+		return 0;		/* Short Packet Transfer End */
+
+	if (req->req.actual > req->req.length) {
+		dev_err(udc->dev, " *** Overrun Error\n");
+		return -EOVERFLOW;
+	}
+
+	if (f_rcv_zero != 0) {
+		i_remain_size = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
+		if (i_remain_size & EP0_ONAK) {
+			/*---------------------------------------------------*/
+			/* NACK release */
+			_nbu2ss_bitclr(&udc->p_regs->EP0_CONTROL, EP0_ONAK);
+		}
+		result = 1;
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_out_dma(struct nbu2ss_udc *udc, struct nbu2ss_req *req,
+			   u32 num, u32 length)
+{
+	dma_addr_t	p_buffer;
+	u32		mpkt;
+	u32		lmpkt;
+	u32		dmacnt;
+	u32		burst = 1;
+	u32		data;
+	int		result;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (req->dma_flag)
+		return 1;		/* DMA is forwarded */
+
+	req->dma_flag = true;
+	p_buffer = req->req.dma;
+	p_buffer += req->req.actual;
+
+	/* DMA Address */
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)p_buffer);
+
+	/* Number of transfer packets */
+	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPN_MPKT;
+	dmacnt = length / mpkt;
+	lmpkt = (length % mpkt) & ~(u32)0x03;
+
+	if (dmacnt > DMA_MAX_COUNT) {
+		dmacnt = DMA_MAX_COUNT;
+		lmpkt = 0;
+	} else if (lmpkt != 0) {
+		if (dmacnt == 0)
+			burst = 0;	/* Burst OFF */
+		dmacnt++;
+	}
+
+	data = mpkt | (lmpkt << 16);
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, data);
+
+	data = ((dmacnt & 0xff) << 16) | DCR1_EPN_DIR0 | DCR1_EPN_REQEN;
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR1, data);
+
+	if (burst == 0) {
+		_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT, 0);
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_BURST_SET);
+	} else {
+		_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT
+				, (dmacnt << 16));
+		_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_BURST_SET);
+	}
+	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_DMA_EN);
+
+	result = length & ~(u32)0x03;
+	req->div_len = result;
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_out_pio(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep,
+			       struct nbu2ss_req *req, u32 length)
+{
+	u8		*p_buffer;
+	u32		i;
+	u32		data;
+	u32		i_word_length;
+	union usb_reg_access	temp_32;
+	union usb_reg_access	*p_buf_32;
+	int		result = 0;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (req->dma_flag)
+		return 1;		/* DMA is forwarded */
+
+	if (length == 0)
+		return 0;
+
+	p_buffer = (u8 *)req->req.buf;
+	p_buf_32 = (union usb_reg_access *)(p_buffer + req->req.actual);
+
+	i_word_length = length / sizeof(u32);
+	if (i_word_length > 0) {
+		/*---------------------------------------------------------*/
+		/* Copy of every four bytes */
+		for (i = 0; i < i_word_length; i++) {
+			p_buf_32->dw =
+			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_READ);
+			p_buf_32++;
+		}
+		result = i_word_length * sizeof(u32);
+	}
+
+	data = length - result;
+	if (data > 0) {
+		/*---------------------------------------------------------*/
+		/* Copy of fraction byte */
+		temp_32.dw =
+			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_READ);
+		for (i = 0 ; i < data ; i++)
+			p_buf_32->byte.DATA[i] = temp_32.byte.DATA[i];
+		result += data;
+	}
+
+	req->req.actual += result;
+
+	if ((req->req.actual == req->req.length) ||
+	    ((req->req.actual % ep->ep.maxpacket) != 0)) {
+		result = 0;
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_out_data(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep,
+				struct nbu2ss_req *req, u32 data_size)
+{
+	u32		num;
+	u32		i_buf_size;
+	int		nret = 1;
+
+	if (ep->epnum == 0)
+		return -EINVAL;
+
+	num = ep->epnum - 1;
+
+	i_buf_size = min((req->req.length - req->req.actual), data_size);
+
+	if ((ep->ep_type != USB_ENDPOINT_XFER_INT) && (req->req.dma != 0) &&
+	    (i_buf_size  >= sizeof(u32))) {
+		nret = _nbu2ss_out_dma(udc, req, num, i_buf_size);
+	} else {
+		i_buf_size = min_t(u32, i_buf_size, ep->ep.maxpacket);
+		nret = _nbu2ss_epn_out_pio(udc, ep, req, i_buf_size);
+	}
+
+	return nret;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_out_transfer(struct nbu2ss_udc *udc,
+				    struct nbu2ss_ep *ep,
+				    struct nbu2ss_req *req)
+{
+	u32		num;
+	u32		i_recv_length;
+	int		result = 1;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (ep->epnum == 0)
+		return -EINVAL;
+
+	num = ep->epnum - 1;
+
+	/*-------------------------------------------------------------*/
+	/* Receive Length */
+	i_recv_length =
+		_nbu2ss_readl(&preg->EP_REGS[num].EP_LEN_DCNT) & EPN_LDATA;
+
+	if (i_recv_length != 0) {
+		result = _nbu2ss_epn_out_data(udc, ep, req, i_recv_length);
+		if (i_recv_length < ep->ep.maxpacket) {
+			if (i_recv_length == result) {
+				req->req.actual += result;
+				result = 0;
+			}
+		}
+	} else {
+		if ((req->req.actual == req->req.length) ||
+		    ((req->req.actual % ep->ep.maxpacket) != 0)) {
+			result = 0;
+		}
+	}
+
+	if (result == 0) {
+		if ((req->req.actual % ep->ep.maxpacket) == 0) {
+			if (req->zero) {
+				req->zero = false;
+				return 1;
+			}
+		}
+	}
+
+	if (req->req.actual > req->req.length) {
+		dev_err(udc->dev, " Overrun Error\n");
+		dev_err(udc->dev, " actual = %d, length = %d\n",
+			req->req.actual, req->req.length);
+		result = -EOVERFLOW;
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_in_dma(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep,
+			  struct nbu2ss_req *req, u32 num, u32 length)
+{
+	dma_addr_t	p_buffer;
+	u32		mpkt;		/* MaxPacketSize */
+	u32		lmpkt;		/* Last Packet Data Size */
+	u32		dmacnt;		/* IN Data Size */
+	u32		i_write_length;
+	u32		data;
+	int		result = -EINVAL;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (req->dma_flag)
+		return 1;		/* DMA is forwarded */
+
+#ifdef USE_DMA
+	if (req->req.actual == 0)
+		_nbu2ss_dma_map_single(udc, ep, req, USB_DIR_IN);
+#endif
+	req->dma_flag = true;
+
+	/* MAX Packet Size */
+	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPN_MPKT;
+
+	if ((DMA_MAX_COUNT * mpkt) < length)
+		i_write_length = DMA_MAX_COUNT * mpkt;
+	else
+		i_write_length = length;
+
+	/*------------------------------------------------------------*/
+	/* Number of transmission packets */
+	if (mpkt < i_write_length) {
+		dmacnt = i_write_length / mpkt;
+		lmpkt  = (i_write_length % mpkt) & ~(u32)0x3;
+		if (lmpkt != 0)
+			dmacnt++;
+		else
+			lmpkt = mpkt & ~(u32)0x3;
+
+	} else {
+		dmacnt = 1;
+		lmpkt  = i_write_length & ~(u32)0x3;
+	}
+
+	/* Packet setting */
+	data = mpkt | (lmpkt << 16);
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, data);
+
+	/* Address setting */
+	p_buffer = req->req.dma;
+	p_buffer += req->req.actual;
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)p_buffer);
+
+	/* Packet and DMA setting */
+	data = ((dmacnt & 0xff) << 16) | DCR1_EPN_REQEN;
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR1, data);
+
+	/* Packet setting of EPC */
+	data = dmacnt << 16;
+	_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT, data);
+
+	/*DMA setting of EPC */
+	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_DMA_EN);
+
+	result = i_write_length & ~(u32)0x3;
+	req->div_len = result;
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_in_pio(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep,
+			      struct nbu2ss_req *req, u32 length)
+{
+	u8		*p_buffer;
+	u32		i;
+	u32		data;
+	u32		i_word_length;
+	union usb_reg_access	temp_32;
+	union usb_reg_access	*p_buf_32 = NULL;
+	int		result = 0;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (req->dma_flag)
+		return 1;		/* DMA is forwarded */
+
+	if (length > 0) {
+		p_buffer = (u8 *)req->req.buf;
+		p_buf_32 = (union usb_reg_access *)(p_buffer + req->req.actual);
+
+		i_word_length = length / sizeof(u32);
+		if (i_word_length > 0) {
+			for (i = 0; i < i_word_length; i++) {
+				_nbu2ss_writel(&preg->EP_REGS[ep->epnum - 1].EP_WRITE,
+					       p_buf_32->dw);
+
+				p_buf_32++;
+			}
+			result = i_word_length * sizeof(u32);
+		}
+	}
+
+	if (result != ep->ep.maxpacket) {
+		data = length - result;
+		temp_32.dw = 0;
+		for (i = 0 ; i < data ; i++)
+			temp_32.byte.DATA[i] = p_buf_32->byte.DATA[i];
+
+		_nbu2ss_ep_in_end(udc, ep->epnum, temp_32.dw, data);
+		result += data;
+	}
+
+	req->div_len = result;
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_in_data(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep,
+			       struct nbu2ss_req *req, u32 data_size)
+{
+	u32		num;
+	int		nret = 1;
+
+	if (ep->epnum == 0)
+		return -EINVAL;
+
+	num = ep->epnum - 1;
+
+	if ((ep->ep_type != USB_ENDPOINT_XFER_INT) && (req->req.dma != 0) &&
+	    (data_size >= sizeof(u32))) {
+		nret = _nbu2ss_in_dma(udc, ep, req, num, data_size);
+	} else {
+		data_size = min_t(u32, data_size, ep->ep.maxpacket);
+		nret = _nbu2ss_epn_in_pio(udc, ep, req, data_size);
+	}
+
+	return nret;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_epn_in_transfer(struct nbu2ss_udc *udc,
+				   struct nbu2ss_ep *ep, struct nbu2ss_req *req)
+{
+	u32		num;
+	u32		i_buf_size;
+	int		result = 0;
+	u32		status;
+
+	if (ep->epnum == 0)
+		return -EINVAL;
+
+	num = ep->epnum - 1;
+
+	status = _nbu2ss_readl(&udc->p_regs->EP_REGS[num].EP_STATUS);
+
+	/*-------------------------------------------------------------*/
+	/* State confirmation of FIFO */
+	if (req->req.actual == 0) {
+		if ((status & EPN_IN_EMPTY) == 0)
+			return 1;	/* Not Empty */
+
+	} else {
+		if ((status & EPN_IN_FULL) != 0)
+			return 1;	/* Not Empty */
+	}
+
+	/*-------------------------------------------------------------*/
+	/* Start transfer */
+	i_buf_size = req->req.length - req->req.actual;
+	if (i_buf_size > 0)
+		result = _nbu2ss_epn_in_data(udc, ep, req, i_buf_size);
+	else if (req->req.length == 0)
+		_nbu2ss_zero_len_pkt(udc, ep->epnum);
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_start_transfer(struct nbu2ss_udc *udc,
+				  struct nbu2ss_ep *ep,
+				  struct nbu2ss_req *req,
+				  bool	bflag)
+{
+	int		nret = -EINVAL;
+
+	req->dma_flag = false;
+	req->div_len = 0;
+
+	if (req->req.length == 0) {
+		req->zero = false;
+	} else {
+		if ((req->req.length % ep->ep.maxpacket) == 0)
+			req->zero = req->req.zero;
+		else
+			req->zero = false;
+	}
+
+	if (ep->epnum == 0) {
+		/* EP0 */
+		switch (udc->ep0state) {
+		case EP0_IN_DATA_PHASE:
+			nret = _nbu2ss_ep0_in_transfer(udc, req);
+			break;
+
+		case EP0_OUT_DATA_PHASE:
+			nret = _nbu2ss_ep0_out_transfer(udc, req);
+			break;
+
+		case EP0_IN_STATUS_PHASE:
+			nret = EP0_send_NULL(udc, true);
+			break;
+
+		default:
+			break;
+		}
+
+	} else {
+		/* EPN */
+		if (ep->direct == USB_DIR_OUT) {
+			/* OUT */
+			if (!bflag)
+				nret = _nbu2ss_epn_out_transfer(udc, ep, req);
+		} else {
+			/* IN */
+			nret = _nbu2ss_epn_in_transfer(udc, ep, req);
+		}
+	}
+
+	return nret;
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_restert_transfer(struct nbu2ss_ep *ep)
+{
+	u32		length;
+	bool	bflag = false;
+	struct nbu2ss_req *req;
+
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
+	if (!req)
+		return;
+
+	if (ep->epnum > 0) {
+		length = _nbu2ss_readl(&ep->udc->p_regs->EP_REGS[ep->epnum - 1].EP_LEN_DCNT);
+
+		length &= EPN_LDATA;
+		if (length < ep->ep.maxpacket)
+			bflag = true;
+	}
+
+	_nbu2ss_start_transfer(ep->udc, ep, req, bflag);
+}
+
+/*-------------------------------------------------------------------------*/
+/*	Endpoint Toggle Reset */
+static void _nbu2ss_endpoint_toggle_reset(struct nbu2ss_udc *udc, u8 ep_adrs)
+{
+	u8		num;
+	u32		data;
+
+	if ((ep_adrs == 0) || (ep_adrs == 0x80))
+		return;
+
+	num = (ep_adrs & 0x7F) - 1;
+
+	if (ep_adrs & USB_DIR_IN)
+		data = EPN_IPIDCLR;
+	else
+		data = EPN_BCLR | EPN_OPIDCLR;
+
+	_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
+}
+
+/*-------------------------------------------------------------------------*/
+/*	Endpoint STALL set */
+static void _nbu2ss_set_endpoint_stall(struct nbu2ss_udc *udc,
+				       u8 ep_adrs, bool bstall)
+{
+	u8		num, epnum;
+	u32		data;
+	struct nbu2ss_ep *ep;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if ((ep_adrs == 0) || (ep_adrs == 0x80)) {
+		if (bstall) {
+			/* Set STALL */
+			_nbu2ss_bitset(&preg->EP0_CONTROL, EP0_STL);
+		} else {
+			/* Clear STALL */
+			_nbu2ss_bitclr(&preg->EP0_CONTROL, EP0_STL);
+		}
+	} else {
+		epnum = ep_adrs & USB_ENDPOINT_NUMBER_MASK;
+		num = epnum - 1;
+		ep = &udc->ep[epnum];
+
+		if (bstall) {
+			/* Set STALL */
+			ep->halted = true;
+
+			if (ep_adrs & USB_DIR_IN)
+				data = EPN_BCLR | EPN_ISTL;
+			else
+				data = EPN_OSTL_EN | EPN_OSTL;
+
+			_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, data);
+		} else {
+			if (ep_adrs & USB_DIR_IN) {
+				_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL
+						, EPN_ISTL);
+			} else {
+				data =
+				_nbu2ss_readl(&preg->EP_REGS[num].EP_CONTROL);
+
+				data &= ~EPN_OSTL;
+				data |= EPN_OSTL_EN;
+
+				_nbu2ss_writel(&preg->EP_REGS[num].EP_CONTROL
+						, data);
+			}
+
+			/* Clear STALL */
+			ep->stalled = false;
+			if (ep->halted) {
+				ep->halted = false;
+				_nbu2ss_restert_transfer(ep);
+			}
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_set_test_mode(struct nbu2ss_udc *udc, u32 mode)
+{
+	u32		data;
+
+	if (mode > MAX_TEST_MODE_NUM)
+		return;
+
+	dev_info(udc->dev, "SET FEATURE : test mode = %d\n", mode);
+
+	data = _nbu2ss_readl(&udc->p_regs->USB_CONTROL);
+	data &= ~TEST_FORCE_ENABLE;
+	data |= mode << TEST_MODE_SHIFT;
+
+	_nbu2ss_writel(&udc->p_regs->USB_CONTROL, data);
+	_nbu2ss_bitset(&udc->p_regs->TEST_CONTROL, CS_TESTMODEEN);
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_set_feature_device(struct nbu2ss_udc *udc,
+				      u16 selector, u16 wIndex)
+{
+	int	result = -EOPNOTSUPP;
+
+	switch (selector) {
+	case USB_DEVICE_REMOTE_WAKEUP:
+		if (wIndex == 0x0000) {
+			udc->remote_wakeup = U2F_ENABLE;
+			result = 0;
+		}
+		break;
+
+	case USB_DEVICE_TEST_MODE:
+		wIndex >>= 8;
+		if (wIndex <= MAX_TEST_MODE_NUM)
+			result = 0;
+		break;
+
+	default:
+		break;
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_get_ep_stall(struct nbu2ss_udc *udc, u8 ep_adrs)
+{
+	u8		epnum;
+	u32		data = 0, bit_data;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	epnum = ep_adrs & ~USB_ENDPOINT_DIR_MASK;
+	if (epnum == 0) {
+		data = _nbu2ss_readl(&preg->EP0_CONTROL);
+		bit_data = EP0_STL;
+
+	} else {
+		data = _nbu2ss_readl(&preg->EP_REGS[epnum - 1].EP_CONTROL);
+		if ((data & EPN_EN) == 0)
+			return -1;
+
+		if (ep_adrs & USB_ENDPOINT_DIR_MASK)
+			bit_data = EPN_ISTL;
+		else
+			bit_data = EPN_OSTL;
+	}
+
+	if ((data & bit_data) == 0)
+		return 0;
+	return 1;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int _nbu2ss_req_feature(struct nbu2ss_udc *udc, bool bset)
+{
+	u8	recipient = (u8)(udc->ctrl.bRequestType & USB_RECIP_MASK);
+	u8	direction = (u8)(udc->ctrl.bRequestType & USB_DIR_IN);
+	u16	selector  = le16_to_cpu(udc->ctrl.wValue);
+	u16	wIndex    = le16_to_cpu(udc->ctrl.wIndex);
+	u8	ep_adrs;
+	int	result = -EOPNOTSUPP;
+
+	if ((udc->ctrl.wLength != 0x0000) ||
+	    (direction != USB_DIR_OUT)) {
+		return -EINVAL;
+	}
+
+	switch (recipient) {
+	case USB_RECIP_DEVICE:
+		if (bset)
+			result =
+			_nbu2ss_set_feature_device(udc, selector, wIndex);
+		break;
+
+	case USB_RECIP_ENDPOINT:
+		if (0x0000 == (wIndex & 0xFF70)) {
+			if (selector == USB_ENDPOINT_HALT) {
+				ep_adrs = wIndex & 0xFF;
+				if (!bset) {
+					_nbu2ss_endpoint_toggle_reset(udc,
+								      ep_adrs);
+				}
+
+				_nbu2ss_set_endpoint_stall(udc, ep_adrs, bset);
+
+				result = 0;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	if (result >= 0)
+		_nbu2ss_create_ep0_packet(udc, udc->ep0_buf, 0);
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline enum usb_device_speed _nbu2ss_get_speed(struct nbu2ss_udc *udc)
+{
+	u32		data;
+	enum usb_device_speed speed = USB_SPEED_FULL;
+
+	data = _nbu2ss_readl(&udc->p_regs->USB_STATUS);
+	if (data & HIGH_SPEED)
+		speed = USB_SPEED_HIGH;
+
+	return speed;
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_epn_set_stall(struct nbu2ss_udc *udc,
+				  struct nbu2ss_ep *ep)
+{
+	u8	ep_adrs;
+	u32	regdata;
+	int	limit_cnt = 0;
+
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (ep->direct == USB_DIR_IN) {
+		for (limit_cnt = 0
+			; limit_cnt < IN_DATA_EMPTY_COUNT
+			; limit_cnt++) {
+			regdata = _nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_STATUS);
+
+			if ((regdata & EPN_IN_DATA) == 0)
+				break;
+
+			mdelay(1);
+		}
+	}
+
+	ep_adrs = ep->epnum | ep->direct;
+	_nbu2ss_set_endpoint_stall(udc, ep_adrs, 1);
+}
+
+/*-------------------------------------------------------------------------*/
+static int std_req_get_status(struct nbu2ss_udc *udc)
+{
+	u32	length;
+	u16	status_data = 0;
+	u8	recipient = (u8)(udc->ctrl.bRequestType & USB_RECIP_MASK);
+	u8	direction = (u8)(udc->ctrl.bRequestType & USB_DIR_IN);
+	u8	ep_adrs;
+	int	result = -EINVAL;
+
+	if ((udc->ctrl.wValue != 0x0000) || (direction != USB_DIR_IN))
+		return result;
+
+	length =
+		min_t(u16, le16_to_cpu(udc->ctrl.wLength), sizeof(status_data));
+	switch (recipient) {
+	case USB_RECIP_DEVICE:
+		if (udc->ctrl.wIndex == 0x0000) {
+			if (udc->gadget.is_selfpowered)
+				status_data |= BIT(USB_DEVICE_SELF_POWERED);
+
+			if (udc->remote_wakeup)
+				status_data |= BIT(USB_DEVICE_REMOTE_WAKEUP);
+
+			result = 0;
+		}
+		break;
+
+	case USB_RECIP_ENDPOINT:
+		if (0x0000 == (le16_to_cpu(udc->ctrl.wIndex) & 0xFF70)) {
+			ep_adrs = (u8)(le16_to_cpu(udc->ctrl.wIndex) & 0xFF);
+			result = _nbu2ss_get_ep_stall(udc, ep_adrs);
+
+			if (result > 0)
+				status_data |= BIT(USB_ENDPOINT_HALT);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	if (result >= 0) {
+		memcpy(udc->ep0_buf, &status_data, length);
+		_nbu2ss_create_ep0_packet(udc, udc->ep0_buf, length);
+		_nbu2ss_ep0_in_transfer(udc, &udc->ep0_req);
+
+	} else {
+		dev_err(udc->dev, " Error GET_STATUS\n");
+	}
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int std_req_clear_feature(struct nbu2ss_udc *udc)
+{
+	return _nbu2ss_req_feature(udc, false);
+}
+
+/*-------------------------------------------------------------------------*/
+static int std_req_set_feature(struct nbu2ss_udc *udc)
+{
+	return _nbu2ss_req_feature(udc, true);
+}
+
+/*-------------------------------------------------------------------------*/
+static int std_req_set_address(struct nbu2ss_udc *udc)
+{
+	int		result = 0;
+	u32		wValue = le16_to_cpu(udc->ctrl.wValue);
+
+	if ((udc->ctrl.bRequestType != 0x00)	||
+	    (udc->ctrl.wIndex != 0x0000)	||
+		(udc->ctrl.wLength != 0x0000)) {
+		return -EINVAL;
+	}
+
+	if (wValue != (wValue & 0x007F))
+		return -EINVAL;
+
+	wValue <<= USB_ADRS_SHIFT;
+
+	_nbu2ss_writel(&udc->p_regs->USB_ADDRESS, wValue);
+	_nbu2ss_create_ep0_packet(udc, udc->ep0_buf, 0);
+
+	return result;
+}
+
+/*-------------------------------------------------------------------------*/
+static int std_req_set_configuration(struct nbu2ss_udc *udc)
+{
+	u32 config_value = (u32)(le16_to_cpu(udc->ctrl.wValue) & 0x00ff);
+
+	if ((udc->ctrl.wIndex != 0x0000)	||
+	    (udc->ctrl.wLength != 0x0000)	||
+		(udc->ctrl.bRequestType != 0x00)) {
+		return -EINVAL;
+	}
+
+	udc->curr_config = config_value;
+
+	if (config_value > 0) {
+		_nbu2ss_bitset(&udc->p_regs->USB_CONTROL, CONF);
+		udc->devstate = USB_STATE_CONFIGURED;
+
+	} else {
+		_nbu2ss_bitclr(&udc->p_regs->USB_CONTROL, CONF);
+		udc->devstate = USB_STATE_ADDRESS;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_read_request_data(struct nbu2ss_udc *udc, u32 *pdata)
+{
+	*pdata = _nbu2ss_readl(&udc->p_regs->SETUP_DATA0);
+	pdata++;
+	*pdata = _nbu2ss_readl(&udc->p_regs->SETUP_DATA1);
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int _nbu2ss_decode_request(struct nbu2ss_udc *udc)
+{
+	bool			bcall_back = true;
+	int			nret = -EINVAL;
+	struct usb_ctrlrequest	*p_ctrl;
+
+	p_ctrl = &udc->ctrl;
+	_nbu2ss_read_request_data(udc, (u32 *)p_ctrl);
+
+	/* ep0 state control */
+	if (p_ctrl->wLength == 0) {
+		udc->ep0state = EP0_IN_STATUS_PHASE;
+
+	} else {
+		if (p_ctrl->bRequestType & USB_DIR_IN)
+			udc->ep0state = EP0_IN_DATA_PHASE;
+		else
+			udc->ep0state = EP0_OUT_DATA_PHASE;
+	}
+
+	if ((p_ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+		switch (p_ctrl->bRequest) {
+		case USB_REQ_GET_STATUS:
+			nret = std_req_get_status(udc);
+			bcall_back = false;
+			break;
+
+		case USB_REQ_CLEAR_FEATURE:
+			nret = std_req_clear_feature(udc);
+			bcall_back = false;
+			break;
+
+		case USB_REQ_SET_FEATURE:
+			nret = std_req_set_feature(udc);
+			bcall_back = false;
+			break;
+
+		case USB_REQ_SET_ADDRESS:
+			nret = std_req_set_address(udc);
+			bcall_back = false;
+			break;
+
+		case USB_REQ_SET_CONFIGURATION:
+			nret = std_req_set_configuration(udc);
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (!bcall_back) {
+		if (udc->ep0state == EP0_IN_STATUS_PHASE) {
+			if (nret >= 0) {
+				/*--------------------------------------*/
+				/* Status Stage */
+				nret = EP0_send_NULL(udc, true);
+			}
+		}
+
+	} else {
+		spin_unlock(&udc->lock);
+		nret = udc->driver->setup(&udc->gadget, &udc->ctrl);
+		spin_lock(&udc->lock);
+	}
+
+	if (nret < 0)
+		udc->ep0state = EP0_IDLE;
+
+	return nret;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int _nbu2ss_ep0_in_data_stage(struct nbu2ss_udc *udc)
+{
+	int			nret;
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep = &udc->ep[0];
+
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
+	if (!req)
+		req = &udc->ep0_req;
+
+	req->req.actual += req->div_len;
+	req->div_len = 0;
+
+	nret = _nbu2ss_ep0_in_transfer(udc, req);
+	if (nret == 0) {
+		udc->ep0state = EP0_OUT_STATUS_PAHSE;
+		EP0_receive_NULL(udc, true);
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int _nbu2ss_ep0_out_data_stage(struct nbu2ss_udc *udc)
+{
+	int			nret;
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep = &udc->ep[0];
+
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
+	if (!req)
+		req = &udc->ep0_req;
+
+	nret = _nbu2ss_ep0_out_transfer(udc, req);
+	if (nret == 0) {
+		udc->ep0state = EP0_IN_STATUS_PHASE;
+		EP0_send_NULL(udc, true);
+
+	} else if (nret < 0) {
+		_nbu2ss_bitset(&udc->p_regs->EP0_CONTROL, EP0_BCLR);
+		req->req.status = nret;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int _nbu2ss_ep0_status_stage(struct nbu2ss_udc *udc)
+{
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep = &udc->ep[0];
+
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
+	if (!req) {
+		req = &udc->ep0_req;
+		if (req->req.complete)
+			req->req.complete(&ep->ep, &req->req);
+
+	} else {
+		if (req->req.complete)
+			_nbu2ss_ep_done(ep, req, 0);
+	}
+
+	udc->ep0state = EP0_IDLE;
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_ep0_int(struct nbu2ss_udc *udc)
+{
+	int		i;
+	u32		status;
+	u32		intr;
+	int		nret = -1;
+
+	status = _nbu2ss_readl(&udc->p_regs->EP0_STATUS);
+	intr = status & EP0_STATUS_RW_BIT;
+	_nbu2ss_writel(&udc->p_regs->EP0_STATUS, ~intr);
+
+	status &= (SETUP_INT | EP0_IN_INT | EP0_OUT_INT
+			| STG_END_INT | EP0_OUT_NULL_INT);
+
+	if (status == 0) {
+		dev_info(udc->dev, "%s Not Decode Interrupt\n", __func__);
+		dev_info(udc->dev, "EP0_STATUS = 0x%08x\n", intr);
+		return;
+	}
+
+	if (udc->gadget.speed == USB_SPEED_UNKNOWN)
+		udc->gadget.speed = _nbu2ss_get_speed(udc);
+
+	for (i = 0; i < EP0_END_XFER; i++) {
+		switch (udc->ep0state) {
+		case EP0_IDLE:
+			if (status & SETUP_INT) {
+				status = 0;
+				nret = _nbu2ss_decode_request(udc);
+			}
+			break;
+
+		case EP0_IN_DATA_PHASE:
+			if (status & EP0_IN_INT) {
+				status &= ~EP0_IN_INT;
+				nret = _nbu2ss_ep0_in_data_stage(udc);
+			}
+			break;
+
+		case EP0_OUT_DATA_PHASE:
+			if (status & EP0_OUT_INT) {
+				status &= ~EP0_OUT_INT;
+				nret = _nbu2ss_ep0_out_data_stage(udc);
+			}
+			break;
+
+		case EP0_IN_STATUS_PHASE:
+			if ((status & STG_END_INT) || (status & SETUP_INT)) {
+				status &= ~(STG_END_INT | EP0_IN_INT);
+				nret = _nbu2ss_ep0_status_stage(udc);
+			}
+			break;
+
+		case EP0_OUT_STATUS_PAHSE:
+			if ((status & STG_END_INT) || (status & SETUP_INT) ||
+			    (status & EP0_OUT_NULL_INT)) {
+				status &= ~(STG_END_INT
+						| EP0_OUT_INT
+						| EP0_OUT_NULL_INT);
+
+				nret = _nbu2ss_ep0_status_stage(udc);
+			}
+
+			break;
+
+		default:
+			status = 0;
+			break;
+		}
+
+		if (status == 0)
+			break;
+	}
+
+	if (nret < 0) {
+		/* Send Stall */
+		_nbu2ss_set_endpoint_stall(udc, 0, true);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_ep_done(struct nbu2ss_ep *ep,
+			    struct nbu2ss_req *req,
+			    int status)
+{
+	struct nbu2ss_udc *udc = ep->udc;
+
+	list_del_init(&req->queue);
+
+	if (status == -ECONNRESET)
+		_nbu2ss_fifo_flush(udc, ep);
+
+	if (likely(req->req.status == -EINPROGRESS))
+		req->req.status = status;
+
+	if (ep->stalled) {
+		_nbu2ss_epn_set_stall(udc, ep);
+	} else {
+		if (!list_empty(&ep->queue))
+			_nbu2ss_restert_transfer(ep);
+	}
+
+#ifdef USE_DMA
+	if ((ep->direct == USB_DIR_OUT) && (ep->epnum > 0) &&
+	    (req->req.dma != 0))
+		_nbu2ss_dma_unmap_single(udc, ep, req, USB_DIR_OUT);
+#endif
+
+	spin_unlock(&udc->lock);
+	req->req.complete(&ep->ep, &req->req);
+	spin_lock(&udc->lock);
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_epn_in_int(struct nbu2ss_udc *udc,
+				      struct nbu2ss_ep *ep,
+				      struct nbu2ss_req *req)
+{
+	int	result = 0;
+	u32	status;
+
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (req->dma_flag)
+		return;		/* DMA is forwarded */
+
+	req->req.actual += req->div_len;
+	req->div_len = 0;
+
+	if (req->req.actual != req->req.length) {
+		/*---------------------------------------------------------*/
+		/* remainder of data */
+		result = _nbu2ss_epn_in_transfer(udc, ep, req);
+
+	} else {
+		if (req->zero && ((req->req.actual % ep->ep.maxpacket) == 0)) {
+			status =
+			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_STATUS);
+
+			if ((status & EPN_IN_FULL) == 0) {
+				/*-----------------------------------------*/
+				/* 0 Length Packet */
+				req->zero = false;
+				_nbu2ss_zero_len_pkt(udc, ep->epnum);
+			}
+			return;
+		}
+	}
+
+	if (result <= 0) {
+		/*---------------------------------------------------------*/
+		/* Complete */
+		_nbu2ss_ep_done(ep, req, result);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_epn_out_int(struct nbu2ss_udc *udc,
+				       struct nbu2ss_ep *ep,
+				       struct nbu2ss_req *req)
+{
+	int	result;
+
+	result = _nbu2ss_epn_out_transfer(udc, ep, req);
+	if (result <= 0)
+		_nbu2ss_ep_done(ep, req, result);
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_epn_in_dma_int(struct nbu2ss_udc *udc,
+					  struct nbu2ss_ep *ep,
+					  struct nbu2ss_req *req)
+{
+	u32		mpkt;
+	u32		size;
+	struct usb_request *preq;
+
+	preq = &req->req;
+
+	if (!req->dma_flag)
+		return;
+
+	preq->actual += req->div_len;
+	req->div_len = 0;
+	req->dma_flag = false;
+
+#ifdef USE_DMA
+	_nbu2ss_dma_unmap_single(udc, ep, req, USB_DIR_IN);
+#endif
+
+	if (preq->actual != preq->length) {
+		_nbu2ss_epn_in_transfer(udc, ep, req);
+	} else {
+		mpkt = ep->ep.maxpacket;
+		size = preq->actual % mpkt;
+		if (size > 0) {
+			if (((preq->actual & 0x03) == 0) && (size < mpkt))
+				_nbu2ss_ep_in_end(udc, ep->epnum, 0, 0);
+		} else {
+			_nbu2ss_epn_in_int(udc, ep, req);
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_epn_out_dma_int(struct nbu2ss_udc *udc,
+					   struct nbu2ss_ep *ep,
+					   struct nbu2ss_req *req)
+{
+	int		i;
+	u32		num;
+	u32		dmacnt, ep_dmacnt;
+	u32		mpkt;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	num = ep->epnum - 1;
+
+	if (req->req.actual == req->req.length) {
+		if ((req->req.length % ep->ep.maxpacket) && !req->zero) {
+			req->div_len = 0;
+			req->dma_flag = false;
+			_nbu2ss_ep_done(ep, req, 0);
+			return;
+		}
+	}
+
+	ep_dmacnt = _nbu2ss_readl(&preg->EP_REGS[num].EP_LEN_DCNT)
+		 & EPN_DMACNT;
+	ep_dmacnt >>= 16;
+
+	for (i = 0; i < EPC_PLL_LOCK_COUNT; i++) {
+		dmacnt = _nbu2ss_readl(&preg->EP_DCR[num].EP_DCR1)
+			 & DCR1_EPN_DMACNT;
+		dmacnt >>= 16;
+		if (ep_dmacnt == dmacnt)
+			break;
+	}
+
+	_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPN_REQEN);
+
+	if (dmacnt != 0) {
+		mpkt = ep->ep.maxpacket;
+		if ((req->div_len % mpkt) == 0)
+			req->div_len -= mpkt * dmacnt;
+	}
+
+	if ((req->req.actual % ep->ep.maxpacket) > 0) {
+		if (req->req.actual == req->div_len) {
+			req->div_len = 0;
+			req->dma_flag = false;
+			_nbu2ss_ep_done(ep, req, 0);
+			return;
+		}
+	}
+
+	req->req.actual += req->div_len;
+	req->div_len = 0;
+	req->dma_flag = false;
+
+	_nbu2ss_epn_out_int(udc, ep, req);
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_epn_int(struct nbu2ss_udc *udc, u32 epnum)
+{
+	u32	num;
+	u32	status;
+
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep = &udc->ep[epnum];
+
+	num = epnum - 1;
+
+	/* Interrupt Status */
+	status = _nbu2ss_readl(&udc->p_regs->EP_REGS[num].EP_STATUS);
+
+	/* Interrupt Clear */
+	_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_STATUS, ~status);
+
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
+	if (!req) {
+		/* pr_warn("=== %s(%d) req == NULL\n", __func__, epnum); */
+		return;
+	}
+
+	if (status & EPN_OUT_END_INT) {
+		status &= ~EPN_OUT_INT;
+		_nbu2ss_epn_out_dma_int(udc, ep, req);
+	}
+
+	if (status & EPN_OUT_INT)
+		_nbu2ss_epn_out_int(udc, ep, req);
+
+	if (status & EPN_IN_END_INT) {
+		status &= ~EPN_IN_INT;
+		_nbu2ss_epn_in_dma_int(udc, ep, req);
+	}
+
+	if (status & EPN_IN_INT)
+		_nbu2ss_epn_in_int(udc, ep, req);
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_ep_int(struct nbu2ss_udc *udc, u32 epnum)
+{
+	if (epnum == 0)
+		_nbu2ss_ep0_int(udc);
+	else
+		_nbu2ss_epn_int(udc, epnum);
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_ep0_enable(struct nbu2ss_udc *udc)
+{
+	_nbu2ss_bitset(&udc->p_regs->EP0_CONTROL, (EP0_AUTO | EP0_BCLR));
+	_nbu2ss_writel(&udc->p_regs->EP0_INT_ENA, EP0_INT_EN_BIT);
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_nuke(struct nbu2ss_udc *udc,
+			struct nbu2ss_ep *ep,
+			int status)
+{
+	struct nbu2ss_req *req, *n;
+
+	/* Endpoint Disable */
+	_nbu2ss_epn_exit(udc, ep);
+
+	/* DMA Disable */
+	_nbu2ss_ep_dma_exit(udc, ep);
+
+	if (list_empty(&ep->queue))
+		return 0;
+
+	/* called with irqs blocked */
+	list_for_each_entry_safe(req, n, &ep->queue, queue) {
+		_nbu2ss_ep_done(ep, req, status);
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_quiesce(struct nbu2ss_udc *udc)
+{
+	struct nbu2ss_ep	*ep;
+
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+
+	_nbu2ss_nuke(udc, &udc->ep[0], -ESHUTDOWN);
+
+	/* Endpoint n */
+	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
+		_nbu2ss_nuke(udc, ep, -ESHUTDOWN);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_pullup(struct nbu2ss_udc *udc, int is_on)
+{
+	u32	reg_dt;
+
+	if (udc->vbus_active == 0)
+		return -ESHUTDOWN;
+
+	if (is_on) {
+		/* D+ Pullup */
+		if (udc->driver) {
+			reg_dt = (_nbu2ss_readl(&udc->p_regs->USB_CONTROL)
+				| PUE2) & ~(u32)CONNECTB;
+
+			_nbu2ss_writel(&udc->p_regs->USB_CONTROL, reg_dt);
+		}
+
+	} else {
+		/* D+ Pulldown */
+		reg_dt = (_nbu2ss_readl(&udc->p_regs->USB_CONTROL) | CONNECTB)
+			& ~(u32)PUE2;
+
+		_nbu2ss_writel(&udc->p_regs->USB_CONTROL, reg_dt);
+		udc->gadget.speed = USB_SPEED_UNKNOWN;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_fifo_flush(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
+{
+	struct fc_regs __iomem *p = udc->p_regs;
+
+	if (udc->vbus_active == 0)
+		return;
+
+	if (ep->epnum == 0) {
+		/* EP0 */
+		_nbu2ss_bitset(&p->EP0_CONTROL, EP0_BCLR);
+
+	} else {
+		/* EPN */
+		_nbu2ss_ep_dma_abort(udc, ep);
+		_nbu2ss_bitset(&p->EP_REGS[ep->epnum - 1].EP_CONTROL, EPN_BCLR);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static int _nbu2ss_enable_controller(struct nbu2ss_udc *udc)
+{
+	int	waitcnt = 0;
+
+	if (udc->udc_enabled)
+		return 0;
+
+	/* Reset */
+	_nbu2ss_bitset(&udc->p_regs->EPCTR, (DIRPD | EPC_RST));
+	udelay(EPC_RST_DISABLE_TIME);	/* 1us wait */
+
+	_nbu2ss_bitclr(&udc->p_regs->EPCTR, DIRPD);
+	mdelay(EPC_DIRPD_DISABLE_TIME);	/* 1ms wait */
+
+	_nbu2ss_bitclr(&udc->p_regs->EPCTR, EPC_RST);
+
+	_nbu2ss_writel(&udc->p_regs->AHBSCTR, WAIT_MODE);
+
+	_nbu2ss_writel(&udc->p_regs->AHBMCTR,
+		       HBUSREQ_MODE | HTRANS_MODE | WBURST_TYPE);
+
+	while (!(_nbu2ss_readl(&udc->p_regs->EPCTR) & PLL_LOCK)) {
+		waitcnt++;
+		udelay(1);	/* 1us wait */
+		if (waitcnt == EPC_PLL_LOCK_COUNT) {
+			dev_err(udc->dev, "*** Reset Cancel failed\n");
+			return -EINVAL;
+		}
+	}
+
+	_nbu2ss_bitset(&udc->p_regs->UTMI_CHARACTER_1, USB_SQUSET);
+
+	_nbu2ss_bitset(&udc->p_regs->USB_CONTROL, (INT_SEL | SOF_RCV));
+
+	/* EP0 */
+	_nbu2ss_ep0_enable(udc);
+
+	/* USB Interrupt Enable */
+	_nbu2ss_bitset(&udc->p_regs->USB_INT_ENA, USB_INT_EN_BIT);
+
+	udc->udc_enabled = true;
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_reset_controller(struct nbu2ss_udc *udc)
+{
+	_nbu2ss_bitset(&udc->p_regs->EPCTR, EPC_RST);
+	_nbu2ss_bitclr(&udc->p_regs->EPCTR, EPC_RST);
+}
+
+/*-------------------------------------------------------------------------*/
+static void _nbu2ss_disable_controller(struct nbu2ss_udc *udc)
+{
+	if (udc->udc_enabled) {
+		udc->udc_enabled = false;
+		_nbu2ss_reset_controller(udc);
+		_nbu2ss_bitset(&udc->p_regs->EPCTR, (DIRPD | EPC_RST));
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_check_vbus(struct nbu2ss_udc *udc)
+{
+	int	nret;
+	u32	reg_dt;
+
+	/* chattering */
+	mdelay(VBUS_CHATTERING_MDELAY);		/* wait (ms) */
+
+	/* VBUS ON Check*/
+	reg_dt = gpiod_get_value(vbus_gpio);
+	if (reg_dt == 0) {
+		udc->linux_suspended = 0;
+
+		_nbu2ss_reset_controller(udc);
+		dev_info(udc->dev, " ----- VBUS OFF\n");
+
+		if (udc->vbus_active == 1) {
+			/* VBUS OFF */
+			udc->vbus_active = 0;
+			if (udc->usb_suspended) {
+				udc->usb_suspended = 0;
+				/* _nbu2ss_reset_controller(udc); */
+			}
+			udc->devstate = USB_STATE_NOTATTACHED;
+
+			_nbu2ss_quiesce(udc);
+			if (udc->driver) {
+				spin_unlock(&udc->lock);
+				udc->driver->disconnect(&udc->gadget);
+				spin_lock(&udc->lock);
+			}
+
+			_nbu2ss_disable_controller(udc);
+		}
+	} else {
+		mdelay(5);		/* wait (5ms) */
+		reg_dt = gpiod_get_value(vbus_gpio);
+		if (reg_dt == 0)
+			return;
+
+		dev_info(udc->dev, " ----- VBUS ON\n");
+
+		if (udc->linux_suspended)
+			return;
+
+		if (udc->vbus_active == 0) {
+			/* VBUS ON */
+			udc->vbus_active = 1;
+			udc->devstate = USB_STATE_POWERED;
+
+			nret = _nbu2ss_enable_controller(udc);
+			if (nret < 0) {
+				_nbu2ss_disable_controller(udc);
+				udc->vbus_active = 0;
+				return;
+			}
+
+			_nbu2ss_pullup(udc, 1);
+
+#ifdef UDC_DEBUG_DUMP
+			_nbu2ss_dump_register(udc);
+#endif /* UDC_DEBUG_DUMP */
+
+		} else {
+			if (udc->devstate == USB_STATE_POWERED)
+				_nbu2ss_pullup(udc, 1);
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_int_bus_reset(struct nbu2ss_udc *udc)
+{
+	udc->devstate		= USB_STATE_DEFAULT;
+	udc->remote_wakeup	= 0;
+
+	_nbu2ss_quiesce(udc);
+
+	udc->ep0state = EP0_IDLE;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_int_usb_resume(struct nbu2ss_udc *udc)
+{
+	if (udc->usb_suspended == 1) {
+		udc->usb_suspended = 0;
+		if (udc->driver && udc->driver->resume) {
+			spin_unlock(&udc->lock);
+			udc->driver->resume(&udc->gadget);
+			spin_lock(&udc->lock);
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static inline void _nbu2ss_int_usb_suspend(struct nbu2ss_udc *udc)
+{
+	u32	reg_dt;
+
+	if (udc->usb_suspended == 0) {
+		reg_dt = gpiod_get_value(vbus_gpio);
+
+		if (reg_dt == 0)
+			return;
+
+		udc->usb_suspended = 1;
+		if (udc->driver && udc->driver->suspend) {
+			spin_unlock(&udc->lock);
+			udc->driver->suspend(&udc->gadget);
+			spin_lock(&udc->lock);
+		}
+
+		_nbu2ss_bitset(&udc->p_regs->USB_CONTROL, SUSPEND);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+/* VBUS (GPIO153) Interrupt */
+static irqreturn_t _nbu2ss_vbus_irq(int irq, void *_udc)
+{
+	struct nbu2ss_udc	*udc = (struct nbu2ss_udc *)_udc;
+
+	spin_lock(&udc->lock);
+	_nbu2ss_check_vbus(udc);
+	spin_unlock(&udc->lock);
+
+	return IRQ_HANDLED;
+}
+
+/*-------------------------------------------------------------------------*/
+/* Interrupt (udc) */
+static irqreturn_t _nbu2ss_udc_irq(int irq, void *_udc)
+{
+	u8	suspend_flag = 0;
+	u32	status;
+	u32	epnum, int_bit;
+
+	struct nbu2ss_udc	*udc = (struct nbu2ss_udc *)_udc;
+	struct fc_regs __iomem *preg = udc->p_regs;
+
+	if (gpiod_get_value(vbus_gpio) == 0) {
+		_nbu2ss_writel(&preg->USB_INT_STA, ~USB_INT_STA_RW);
+		_nbu2ss_writel(&preg->USB_INT_ENA, 0);
+		return IRQ_HANDLED;
+	}
+
+	spin_lock(&udc->lock);
+
+	for (;;) {
+		if (gpiod_get_value(vbus_gpio) == 0) {
+			_nbu2ss_writel(&preg->USB_INT_STA, ~USB_INT_STA_RW);
+			_nbu2ss_writel(&preg->USB_INT_ENA, 0);
+			status = 0;
+		} else {
+			status = _nbu2ss_readl(&preg->USB_INT_STA);
+		}
+
+		if (status == 0)
+			break;
+
+		_nbu2ss_writel(&preg->USB_INT_STA, ~(status & USB_INT_STA_RW));
+
+		if (status & USB_RST_INT) {
+			/* USB Reset */
+			_nbu2ss_int_bus_reset(udc);
+		}
+
+		if (status & RSUM_INT) {
+			/* Resume */
+			_nbu2ss_int_usb_resume(udc);
+		}
+
+		if (status & SPND_INT) {
+			/* Suspend */
+			suspend_flag = 1;
+		}
+
+		if (status & EPN_INT) {
+			/* EP INT */
+			int_bit = status >> 8;
+
+			for (epnum = 0; epnum < NUM_ENDPOINTS; epnum++) {
+				if (0x01 & int_bit)
+					_nbu2ss_ep_int(udc, epnum);
+
+				int_bit >>= 1;
+
+				if (int_bit == 0)
+					break;
+			}
+		}
+	}
+
+	if (suspend_flag)
+		_nbu2ss_int_usb_suspend(udc);
+
+	spin_unlock(&udc->lock);
+
+	return IRQ_HANDLED;
+}
+
+/*-------------------------------------------------------------------------*/
+/* usb_ep_ops */
+static int nbu2ss_ep_enable(struct usb_ep *_ep,
+			    const struct usb_endpoint_descriptor *desc)
+{
+	u8		ep_type;
+	unsigned long	flags;
+
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+
+	if (!_ep || !desc) {
+		pr_err(" *** %s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+	if (!ep->udc) {
+		pr_err(" *** %s, ep == NULL !!\n", __func__);
+		return -EINVAL;
+	}
+
+	ep_type = usb_endpoint_type(desc);
+	if ((ep_type == USB_ENDPOINT_XFER_CONTROL) ||
+	    (ep_type == USB_ENDPOINT_XFER_ISOC)) {
+		pr_err(" *** %s, bat bmAttributes\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = ep->udc;
+	if (udc->vbus_active == 0)
+		return -ESHUTDOWN;
+
+	if ((!udc->driver) || (udc->gadget.speed == USB_SPEED_UNKNOWN)) {
+		dev_err(ep->udc->dev, " *** %s, udc !!\n", __func__);
+		return -ESHUTDOWN;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	ep->desc = desc;
+	ep->epnum = usb_endpoint_num(desc);
+	ep->direct = desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK;
+	ep->ep_type = ep_type;
+	ep->wedged = 0;
+	ep->halted = false;
+	ep->stalled = false;
+
+	ep->ep.maxpacket = le16_to_cpu(desc->wMaxPacketSize);
+
+	/* DMA setting */
+	_nbu2ss_ep_dma_init(udc, ep);
+
+	/* Endpoint setting */
+	_nbu2ss_ep_init(udc, ep);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_ep_disable(struct usb_ep *_ep)
+{
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+
+	if (!_ep) {
+		pr_err(" *** %s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+	if (!ep->udc) {
+		pr_err("udc: *** %s, ep == NULL !!\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = ep->udc;
+	if (udc->vbus_active == 0)
+		return -ESHUTDOWN;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	_nbu2ss_nuke(udc, ep, -EINPROGRESS);		/* dequeue request */
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static struct usb_request *nbu2ss_ep_alloc_request(struct usb_ep *ep,
+						   gfp_t gfp_flags)
+{
+	struct nbu2ss_req *req;
+
+	req = kzalloc(sizeof(*req), gfp_flags);
+	if (!req)
+		return NULL;
+
+#ifdef USE_DMA
+	req->req.dma = DMA_ADDR_INVALID;
+#endif
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+/*-------------------------------------------------------------------------*/
+static void nbu2ss_ep_free_request(struct usb_ep *_ep,
+				   struct usb_request *_req)
+{
+	struct nbu2ss_req *req;
+
+	if (_req) {
+		req = container_of(_req, struct nbu2ss_req, req);
+
+		kfree(req);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_ep_queue(struct usb_ep *_ep,
+			   struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+	bool			bflag;
+	int			result = -EINVAL;
+
+	/* catch various bogus parameters */
+	if (!_ep || !_req) {
+		if (!_ep)
+			pr_err("udc: %s --- _ep == NULL\n", __func__);
+
+		if (!_req)
+			pr_err("udc: %s --- _req == NULL\n", __func__);
+
+		return -EINVAL;
+	}
+
+	req = container_of(_req, struct nbu2ss_req, req);
+	if (unlikely(!_req->complete ||
+		     !_req->buf ||
+		     !list_empty(&req->queue))) {
+		if (!_req->complete)
+			pr_err("udc: %s --- !_req->complete\n", __func__);
+
+		if (!_req->buf)
+			pr_err("udc:%s --- !_req->buf\n", __func__);
+
+		if (!list_empty(&req->queue))
+			pr_err("%s --- !list_empty(&req->queue)\n", __func__);
+
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+	udc = ep->udc;
+
+	if (udc->vbus_active == 0) {
+		dev_info(udc->dev, "Can't ep_queue (VBUS OFF)\n");
+		return -ESHUTDOWN;
+	}
+
+	if (unlikely(!udc->driver)) {
+		dev_err(udc->dev, "%s, bogus device state %p\n", __func__,
+			udc->driver);
+		return -ESHUTDOWN;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+#ifdef USE_DMA
+	if ((uintptr_t)req->req.buf & 0x3)
+		req->unaligned = true;
+	else
+		req->unaligned = false;
+
+	if (req->unaligned) {
+		if (!ep->virt_buf) {
+			ep->virt_buf = dma_alloc_coherent(udc->dev, PAGE_SIZE,
+							  &ep->phys_buf,
+							  GFP_ATOMIC | GFP_DMA);
+			if (!ep->virt_buf) {
+				spin_unlock_irqrestore(&udc->lock, flags);
+				return -ENOMEM;
+			}
+		}
+		if (ep->epnum > 0)  {
+			if (ep->direct == USB_DIR_IN)
+				memcpy(ep->virt_buf, req->req.buf,
+				       req->req.length);
+		}
+	}
+
+	if ((ep->epnum > 0) && (ep->direct == USB_DIR_OUT) &&
+	    (req->req.dma != 0))
+		_nbu2ss_dma_map_single(udc, ep, req, USB_DIR_OUT);
+#endif
+
+	_req->status = -EINPROGRESS;
+	_req->actual = 0;
+
+	bflag = list_empty(&ep->queue);
+	list_add_tail(&req->queue, &ep->queue);
+
+	if (bflag && !ep->stalled) {
+		result = _nbu2ss_start_transfer(udc, ep, req, false);
+		if (result < 0) {
+			dev_err(udc->dev, " *** %s, result = %d\n", __func__,
+				result);
+			list_del(&req->queue);
+		} else if ((ep->epnum > 0) && (ep->direct == USB_DIR_OUT)) {
+#ifdef USE_DMA
+			if (req->req.length < 4 &&
+			    req->req.length == req->req.actual)
+#else
+			if (req->req.length == req->req.actual)
+#endif
+				_nbu2ss_ep_done(ep, req, result);
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct nbu2ss_req	*req;
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+	unsigned long flags;
+
+	/* catch various bogus parameters */
+	if (!_ep || !_req) {
+		/* pr_err("%s, bad param(1)\n", __func__); */
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+
+	udc = ep->udc;
+	if (!udc)
+		return -EINVAL;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->req == _req) {
+			_nbu2ss_ep_done(ep, req, -ECONNRESET);
+			spin_unlock_irqrestore(&udc->lock, flags);
+			return 0;
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	pr_debug("%s no queue(EINVAL)\n", __func__);
+
+	return -EINVAL;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	u8		ep_adrs;
+	unsigned long	flags;
+
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+
+	if (!_ep) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+
+	udc = ep->udc;
+	if (!udc) {
+		dev_err(ep->udc->dev, " *** %s, bad udc\n", __func__);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	ep_adrs = ep->epnum | ep->direct;
+	if (value == 0) {
+		_nbu2ss_set_endpoint_stall(udc, ep_adrs, value);
+		ep->stalled = false;
+	} else {
+		if (list_empty(&ep->queue))
+			_nbu2ss_epn_set_stall(udc, ep);
+		else
+			ep->stalled = true;
+	}
+
+	if (value == 0)
+		ep->wedged = 0;
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static int nbu2ss_ep_set_wedge(struct usb_ep *_ep)
+{
+	return nbu2ss_ep_set_halt(_ep, 1);
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_ep_fifo_status(struct usb_ep *_ep)
+{
+	u32		data;
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+	struct fc_regs	__iomem *preg;
+
+	if (!_ep) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+
+	udc = ep->udc;
+	if (!udc) {
+		dev_err(ep->udc->dev, "%s, bad udc\n", __func__);
+		return -EINVAL;
+	}
+
+	preg = udc->p_regs;
+
+	data = gpiod_get_value(vbus_gpio);
+	if (data == 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (ep->epnum == 0) {
+		data = _nbu2ss_readl(&preg->EP0_LENGTH) & EP0_LDATA;
+
+	} else {
+		data = _nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_LEN_DCNT)
+			& EPN_LDATA;
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static void  nbu2ss_ep_fifo_flush(struct usb_ep *_ep)
+{
+	u32			data;
+	struct nbu2ss_ep	*ep;
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+
+	if (!_ep) {
+		pr_err("udc: %s, bad param\n", __func__);
+		return;
+	}
+
+	ep = container_of(_ep, struct nbu2ss_ep, ep);
+
+	udc = ep->udc;
+	if (!udc) {
+		dev_err(ep->udc->dev, "%s, bad udc\n", __func__);
+		return;
+	}
+
+	data = gpiod_get_value(vbus_gpio);
+	if (data == 0)
+		return;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	_nbu2ss_fifo_flush(udc, ep);
+	spin_unlock_irqrestore(&udc->lock, flags);
+}
+
+/*-------------------------------------------------------------------------*/
+static const struct usb_ep_ops nbu2ss_ep_ops = {
+	.enable		= nbu2ss_ep_enable,
+	.disable	= nbu2ss_ep_disable,
+
+	.alloc_request	= nbu2ss_ep_alloc_request,
+	.free_request	= nbu2ss_ep_free_request,
+
+	.queue		= nbu2ss_ep_queue,
+	.dequeue	= nbu2ss_ep_dequeue,
+
+	.set_halt	= nbu2ss_ep_set_halt,
+	.set_wedge	= nbu2ss_ep_set_wedge,
+
+	.fifo_status	= nbu2ss_ep_fifo_status,
+	.fifo_flush	= nbu2ss_ep_fifo_flush,
+};
+
+/*-------------------------------------------------------------------------*/
+/* usb_gadget_ops */
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_get_frame(struct usb_gadget *pgadget)
+{
+	u32			data;
+	struct nbu2ss_udc	*udc;
+
+	if (!pgadget) {
+		pr_err("udc: %s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
+	data = gpiod_get_value(vbus_gpio);
+	if (data == 0)
+		return -EINVAL;
+
+	return _nbu2ss_readl(&udc->p_regs->USB_ADDRESS) & FRAME;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_wakeup(struct usb_gadget *pgadget)
+{
+	int	i;
+	u32	data;
+
+	struct nbu2ss_udc	*udc;
+
+	if (!pgadget) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
+
+	data = gpiod_get_value(vbus_gpio);
+	if (data == 0) {
+		dev_warn(&pgadget->dev, "VBUS LEVEL = %d\n", data);
+		return -EINVAL;
+	}
+
+	_nbu2ss_bitset(&udc->p_regs->EPCTR, PLL_RESUME);
+
+	for (i = 0; i < EPC_PLL_LOCK_COUNT; i++) {
+		data = _nbu2ss_readl(&udc->p_regs->EPCTR);
+
+		if (data & PLL_LOCK)
+			break;
+	}
+
+	_nbu2ss_bitclr(&udc->p_regs->EPCTR, PLL_RESUME);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_set_selfpowered(struct usb_gadget *pgadget,
+				      int is_selfpowered)
+{
+	struct nbu2ss_udc       *udc;
+	unsigned long		flags;
+
+	if (!pgadget) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
+
+	spin_lock_irqsave(&udc->lock, flags);
+	pgadget->is_selfpowered = (is_selfpowered != 0);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_vbus_session(struct usb_gadget *pgadget, int is_active)
+{
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_vbus_draw(struct usb_gadget *pgadget, unsigned int mA)
+{
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+
+	if (!pgadget) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->mA = mA;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_pullup(struct usb_gadget *pgadget, int is_on)
+{
+	struct nbu2ss_udc	*udc;
+	unsigned long		flags;
+
+	if (!pgadget) {
+		pr_err("%s, bad param\n", __func__);
+		return -EINVAL;
+	}
+
+	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
+
+	if (!udc->driver) {
+		pr_warn("%s, Not Regist Driver\n", __func__);
+		return -EINVAL;
+	}
+
+	if (udc->vbus_active == 0)
+		return -ESHUTDOWN;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	_nbu2ss_pullup(udc, is_on);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_gad_ioctl(struct usb_gadget *pgadget,
+			    unsigned int code, unsigned long param)
+{
+	return 0;
+}
+
+static const struct usb_gadget_ops nbu2ss_gadget_ops = {
+	.get_frame		= nbu2ss_gad_get_frame,
+	.wakeup			= nbu2ss_gad_wakeup,
+	.set_selfpowered	= nbu2ss_gad_set_selfpowered,
+	.vbus_session		= nbu2ss_gad_vbus_session,
+	.vbus_draw		= nbu2ss_gad_vbus_draw,
+	.pullup			= nbu2ss_gad_pullup,
+	.ioctl			= nbu2ss_gad_ioctl,
+};
+
+static const struct {
+	const char *name;
+	const struct usb_ep_caps caps;
+} ep_info[NUM_ENDPOINTS] = {
+#define EP_INFO(_name, _caps) \
+	{ \
+		.name = _name, \
+		.caps = _caps, \
+	}
+
+	EP_INFO("ep0",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_CONTROL, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep1-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep2-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep3in-int",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_INT, USB_EP_CAPS_DIR_IN)),
+	EP_INFO("ep4-iso",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep5-iso",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep6-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep7-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("ep8in-int",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_INT, USB_EP_CAPS_DIR_IN)),
+	EP_INFO("ep9-iso",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("epa-iso",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("epb-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("epc-bulk",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK, USB_EP_CAPS_DIR_ALL)),
+	EP_INFO("epdin-int",
+		USB_EP_CAPS(USB_EP_CAPS_TYPE_INT, USB_EP_CAPS_DIR_IN)),
+
+#undef EP_INFO
+};
+
+/*-------------------------------------------------------------------------*/
+static void nbu2ss_drv_ep_init(struct nbu2ss_udc *udc)
+{
+	int	i;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	udc->gadget.ep0 = &udc->ep[0].ep;
+
+	for (i = 0; i < NUM_ENDPOINTS; i++) {
+		struct nbu2ss_ep *ep = &udc->ep[i];
+
+		ep->udc = udc;
+		ep->desc = NULL;
+
+		ep->ep.driver_data = NULL;
+		ep->ep.name = ep_info[i].name;
+		ep->ep.caps = ep_info[i].caps;
+		ep->ep.ops = &nbu2ss_ep_ops;
+
+		usb_ep_set_maxpacket_limit(&ep->ep,
+					   i == 0 ? EP0_PACKETSIZE
+					   : EP_PACKETSIZE);
+
+		list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+		INIT_LIST_HEAD(&ep->queue);
+	}
+
+	list_del_init(&udc->ep[0].ep.ep_list);
+}
+
+/*-------------------------------------------------------------------------*/
+/* platform_driver */
+static int nbu2ss_drv_contest_init(struct platform_device *pdev,
+				   struct nbu2ss_udc *udc)
+{
+	spin_lock_init(&udc->lock);
+	udc->dev = &pdev->dev;
+
+	udc->gadget.is_selfpowered = 1;
+	udc->devstate = USB_STATE_NOTATTACHED;
+	udc->pdev = pdev;
+	udc->mA = 0;
+
+	udc->pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	/* init Endpoint */
+	nbu2ss_drv_ep_init(udc);
+
+	/* init Gadget */
+	udc->gadget.ops = &nbu2ss_gadget_ops;
+	udc->gadget.ep0 = &udc->ep[0].ep;
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->gadget.name = driver_name;
+	/* udc->gadget.is_dualspeed = 1; */
+
+	device_initialize(&udc->gadget.dev);
+
+	dev_set_name(&udc->gadget.dev, "gadget");
+	udc->gadget.dev.parent = &pdev->dev;
+	udc->gadget.dev.dma_mask = pdev->dev.dma_mask;
+
+	return 0;
+}
+
+/*
+ *	probe - binds to the platform device
+ */
+static int nbu2ss_drv_probe(struct platform_device *pdev)
+{
+	int status;
+	struct nbu2ss_udc *udc;
+	int irq;
+	void __iomem *mmio_base;
+
+	udc = &udc_controller;
+	memset(udc, 0, sizeof(struct nbu2ss_udc));
+
+	platform_set_drvdata(pdev, udc);
+
+	/* require I/O memory and IRQ to be provided as resources */
+	mmio_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(mmio_base))
+		return PTR_ERR(mmio_base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+	status = devm_request_irq(&pdev->dev, irq, _nbu2ss_udc_irq,
+				  0, driver_name, udc);
+
+	/* IO Memory */
+	udc->p_regs = (struct fc_regs __iomem *)mmio_base;
+
+	/* USB Function Controller Interrupt */
+	if (status != 0) {
+		dev_err(udc->dev, "request_irq(USB_UDC_IRQ_1) failed\n");
+		return status;
+	}
+
+	/* Driver Initialization */
+	status = nbu2ss_drv_contest_init(pdev, udc);
+	if (status < 0) {
+		/* Error */
+		return status;
+	}
+
+	/* VBUS Interrupt */
+	vbus_irq = gpiod_to_irq(vbus_gpio);
+	irq_set_irq_type(vbus_irq, IRQ_TYPE_EDGE_BOTH);
+	status = request_irq(vbus_irq,
+			     _nbu2ss_vbus_irq, IRQF_SHARED, driver_name, udc);
+
+	if (status != 0) {
+		dev_err(udc->dev, "request_irq(vbus_irq) failed\n");
+		return status;
+	}
+
+	return status;
+}
+
+/*-------------------------------------------------------------------------*/
+static void nbu2ss_drv_shutdown(struct platform_device *pdev)
+{
+	struct nbu2ss_udc	*udc;
+
+	udc = platform_get_drvdata(pdev);
+	if (!udc)
+		return;
+
+	_nbu2ss_disable_controller(udc);
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_drv_remove(struct platform_device *pdev)
+{
+	struct nbu2ss_udc	*udc;
+	struct nbu2ss_ep	*ep;
+	int	i;
+
+	udc = &udc_controller;
+
+	for (i = 0; i < NUM_ENDPOINTS; i++) {
+		ep = &udc->ep[i];
+		if (ep->virt_buf)
+			dma_free_coherent(udc->dev, PAGE_SIZE, (void *)ep->virt_buf,
+					  ep->phys_buf);
+	}
+
+	/* Interrupt Handler - Release */
+	free_irq(vbus_irq, udc);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_drv_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct nbu2ss_udc	*udc;
+
+	udc = platform_get_drvdata(pdev);
+	if (!udc)
+		return 0;
+
+	if (udc->vbus_active) {
+		udc->vbus_active = 0;
+		udc->devstate = USB_STATE_NOTATTACHED;
+		udc->linux_suspended = 1;
+
+		if (udc->usb_suspended) {
+			udc->usb_suspended = 0;
+			_nbu2ss_reset_controller(udc);
+		}
+
+		_nbu2ss_quiesce(udc);
+	}
+	_nbu2ss_disable_controller(udc);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+static int nbu2ss_drv_resume(struct platform_device *pdev)
+{
+	u32	data;
+	struct nbu2ss_udc	*udc;
+
+	udc = platform_get_drvdata(pdev);
+	if (!udc)
+		return 0;
+
+	data = gpiod_get_value(vbus_gpio);
+	if (data) {
+		udc->vbus_active = 1;
+		udc->devstate = USB_STATE_POWERED;
+		_nbu2ss_enable_controller(udc);
+		_nbu2ss_pullup(udc, 1);
+	}
+
+	udc->linux_suspended = 0;
+
+	return 0;
+}
+
+static struct platform_driver udc_driver = {
+	.probe		= nbu2ss_drv_probe,
+	.shutdown	= nbu2ss_drv_shutdown,
+	.remove		= nbu2ss_drv_remove,
+	.suspend	= nbu2ss_drv_suspend,
+	.resume		= nbu2ss_drv_resume,
+	.driver		= {
+		.name	= driver_name,
+	},
+};
+
+module_platform_driver(udc_driver);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Renesas Electronics Corporation");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
new file mode 100644
index 000000000..c9e37a1b8
--- /dev/null
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -0,0 +1,554 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *  EMXX FCD (Function Controller Driver) for USB.
+ *
+ *  Copyright (C) 2010 Renesas Electronics Corporation
+ */
+
+#ifndef _LINUX_EMXX_H
+#define _LINUX_EMXX_H
+
+/*---------------------------------------------------------------------------*/
+
+/*----------------- Default define */
+#define	USE_DMA	1
+#define USE_SUSPEND_WAIT	1
+
+/*------------ Board dependence(Resource) */
+#define	VBUS_VALUE		GPIO_VBUS
+
+/* below hacked up for staging integration */
+#define GPIO_VBUS 0 /* GPIO_P153 on KZM9D */
+#define INT_VBUS 0 /* IRQ for GPIO_P153 */
+
+/*------------ Board dependence(Wait) */
+
+/* CHATTERING wait time ms */
+#define VBUS_CHATTERING_MDELAY		1
+/* DMA Abort wait time ms */
+#define DMA_DISABLE_TIME		10
+
+/*------------ Controller dependence */
+#define NUM_ENDPOINTS		14		/* Endpoint */
+#define REG_EP_NUM		15		/* Endpoint Register */
+#define DMA_MAX_COUNT		256		/* DMA Block */
+
+#define EPC_RST_DISABLE_TIME		1	/* 1 usec */
+#define EPC_DIRPD_DISABLE_TIME		1	/* 1 msec */
+#define EPC_PLL_LOCK_COUNT		1000	/* 1000 */
+#define IN_DATA_EMPTY_COUNT		1000	/* 1000 */
+
+#define CHATGER_TIME			700	/* 700msec */
+#define USB_SUSPEND_TIME		2000	/* 2 sec */
+
+/* U2F FLAG */
+#define U2F_ENABLE		1
+#define U2F_DISABLE		0
+
+#define TEST_FORCE_ENABLE		(BIT(18) | BIT(16))
+
+#define INT_SEL				BIT(10)
+#define CONSTFS				BIT(9)
+#define SOF_RCV				BIT(8)
+#define RSUM_IN				BIT(7)
+#define SUSPEND				BIT(6)
+#define CONF				BIT(5)
+#define DEFAULT				BIT(4)
+#define CONNECTB			BIT(3)
+#define PUE2				BIT(2)
+
+#define MAX_TEST_MODE_NUM		0x05
+#define TEST_MODE_SHIFT			16
+
+/*------- (0x0004) USB Status Register */
+#define SPEED_MODE			BIT(6)
+#define HIGH_SPEED			BIT(6)
+
+#define CONF				BIT(5)
+#define DEFAULT				BIT(4)
+#define USB_RST				BIT(3)
+#define SPND_OUT			BIT(2)
+#define RSUM_OUT			BIT(1)
+
+/*------- (0x0008) USB Address Register */
+#define USB_ADDR			0x007F0000
+#define SOF_STATUS			BIT(15)
+#define UFRAME				(BIT(14) | BIT(13) | BIT(12))
+#define FRAME				0x000007FF
+
+#define USB_ADRS_SHIFT			16
+
+/*------- (0x000C) UTMI Characteristic 1 Register */
+#define SQUSET				(BIT(7) | BIT(6) | BIT(5) | BIT(4))
+
+#define USB_SQUSET			(BIT(6) | BIT(5) | BIT(4))
+
+/*------- (0x0010) TEST Control Register */
+#define FORCEHS				BIT(2)
+#define CS_TESTMODEEN			BIT(1)
+#define LOOPBACK			BIT(0)
+
+/*------- (0x0018) Setup Data 0 Register */
+/*------- (0x001C) Setup Data 1 Register */
+
+/*------- (0x0020) USB Interrupt Status Register */
+#define EPN_INT				0x00FFFF00
+#define EP15_INT			BIT(23)
+#define EP14_INT			BIT(22)
+#define EP13_INT			BIT(21)
+#define EP12_INT			BIT(20)
+#define EP11_INT			BIT(19)
+#define EP10_INT			BIT(18)
+#define EP9_INT				BIT(17)
+#define EP8_INT				BIT(16)
+#define EP7_INT				BIT(15)
+#define EP6_INT				BIT(14)
+#define EP5_INT				BIT(13)
+#define EP4_INT				BIT(12)
+#define EP3_INT				BIT(11)
+#define EP2_INT				BIT(10)
+#define EP1_INT				BIT(9)
+#define EP0_INT				BIT(8)
+#define SPEED_MODE_INT			BIT(6)
+#define SOF_ERROR_INT			BIT(5)
+#define SOF_INT				BIT(4)
+#define USB_RST_INT			BIT(3)
+#define SPND_INT			BIT(2)
+#define RSUM_INT			BIT(1)
+
+#define USB_INT_STA_RW			0x7E
+
+/*------- (0x0024) USB Interrupt Enable Register */
+#define EP15_0_EN			0x00FFFF00
+#define EP15_EN				BIT(23)
+#define EP14_EN				BIT(22)
+#define EP13_EN				BIT(21)
+#define EP12_EN				BIT(20)
+#define EP11_EN				BIT(19)
+#define EP10_EN				BIT(18)
+#define EP9_EN				BIT(17)
+#define EP8_EN				BIT(16)
+#define EP7_EN				BIT(15)
+#define EP6_EN				BIT(14)
+#define EP5_EN				BIT(13)
+#define EP4_EN				BIT(12)
+#define EP3_EN				BIT(11)
+#define EP2_EN				BIT(10)
+#define EP1_EN				BIT(9)
+#define EP0_EN				BIT(8)
+#define SPEED_MODE_EN			BIT(6)
+#define SOF_ERROR_EN			BIT(5)
+#define SOF_EN				BIT(4)
+#define USB_RST_EN			BIT(3)
+#define SPND_EN				BIT(2)
+#define RSUM_EN				BIT(1)
+
+#define USB_INT_EN_BIT	\
+	(EP0_EN | SPEED_MODE_EN | USB_RST_EN | SPND_EN | RSUM_EN)
+
+/*------- (0x0028) EP0 Control Register */
+#define EP0_STGSEL			BIT(18)
+#define EP0_OVERSEL			BIT(17)
+#define EP0_AUTO			BIT(16)
+#define EP0_PIDCLR			BIT(9)
+#define EP0_BCLR			BIT(8)
+#define EP0_DEND			BIT(7)
+#define EP0_DW				(BIT(6) | BIT(5))
+#define EP0_DW4				0
+#define EP0_DW3				(BIT(6) | BIT(5))
+#define EP0_DW2				BIT(6)
+#define EP0_DW1				BIT(5)
+
+#define EP0_INAK_EN			BIT(4)
+#define EP0_PERR_NAK_CLR		BIT(3)
+#define EP0_STL				BIT(2)
+#define EP0_INAK			BIT(1)
+#define EP0_ONAK			BIT(0)
+
+/*------- (0x002C) EP0 Status Register */
+#define EP0_PID				BIT(18)
+#define EP0_PERR_NAK			BIT(17)
+#define EP0_PERR_NAK_INT		BIT(16)
+#define EP0_OUT_NAK_INT			BIT(15)
+#define EP0_OUT_NULL			BIT(14)
+#define EP0_OUT_FULL			BIT(13)
+#define EP0_OUT_EMPTY			BIT(12)
+#define EP0_IN_NAK_INT			BIT(11)
+#define EP0_IN_DATA			BIT(10)
+#define EP0_IN_FULL			BIT(9)
+#define EP0_IN_EMPTY			BIT(8)
+#define EP0_OUT_NULL_INT		BIT(7)
+#define EP0_OUT_OR_INT			BIT(6)
+#define EP0_OUT_INT			BIT(5)
+#define EP0_IN_INT			BIT(4)
+#define EP0_STALL_INT			BIT(3)
+#define STG_END_INT			BIT(2)
+#define STG_START_INT			BIT(1)
+#define SETUP_INT			BIT(0)
+
+#define EP0_STATUS_RW_BIT	(BIT(16) | BIT(15) | BIT(11) | 0xFF)
+
+/*------- (0x0030) EP0 Interrupt Enable Register */
+#define EP0_PERR_NAK_EN			BIT(16)
+#define EP0_OUT_NAK_EN			BIT(15)
+
+#define EP0_IN_NAK_EN			BIT(11)
+
+#define EP0_OUT_NULL_EN			BIT(7)
+#define EP0_OUT_OR_EN			BIT(6)
+#define EP0_OUT_EN			BIT(5)
+#define EP0_IN_EN			BIT(4)
+#define EP0_STALL_EN			BIT(3)
+#define STG_END_EN			BIT(2)
+#define STG_START_EN			BIT(1)
+#define SETUP_EN			BIT(0)
+
+#define EP0_INT_EN_BIT	\
+	(EP0_OUT_OR_EN | EP0_OUT_EN | EP0_IN_EN | STG_END_EN | SETUP_EN)
+
+/*------- (0x0034) EP0 Length Register */
+#define EP0_LDATA			0x0000007F
+
+/*------- (0x0038) EP0 Read Register */
+/*------- (0x003C) EP0 Write Register */
+
+/*------- (0x0040:) EPN Control Register */
+#define EPN_EN				BIT(31)
+#define EPN_BUF_TYPE			BIT(30)
+#define EPN_BUF_SINGLE			BIT(30)
+
+#define EPN_DIR0			BIT(26)
+#define EPN_MODE			(BIT(25) | BIT(24))
+#define EPN_BULK			0
+#define EPN_INTERRUPT			BIT(24)
+#define EPN_ISO				BIT(25)
+
+#define EPN_OVERSEL			BIT(17)
+#define EPN_AUTO			BIT(16)
+
+#define EPN_IPIDCLR			BIT(11)
+#define EPN_OPIDCLR			BIT(10)
+#define EPN_BCLR			BIT(9)
+#define EPN_CBCLR			BIT(8)
+#define EPN_DEND			BIT(7)
+#define EPN_DW				(BIT(6) | BIT(5))
+#define EPN_DW4				0
+#define EPN_DW3				(BIT(6) | BIT(5))
+#define EPN_DW2				BIT(6)
+#define EPN_DW1				BIT(5)
+
+#define EPN_OSTL_EN			BIT(4)
+#define EPN_ISTL			BIT(3)
+#define EPN_OSTL			BIT(2)
+
+#define EPN_ONAK			BIT(0)
+
+/*------- (0x0044:) EPN Status Register	*/
+#define EPN_ISO_PIDERR			BIT(29)		/* R */
+#define EPN_OPID			BIT(28)		/* R */
+#define EPN_OUT_NOTKN			BIT(27)		/* R */
+#define EPN_ISO_OR			BIT(26)		/* R */
+
+#define EPN_ISO_CRC			BIT(24)		/* R */
+#define EPN_OUT_END_INT			BIT(23)		/* RW */
+#define EPN_OUT_OR_INT			BIT(22)		/* RW */
+#define EPN_OUT_NAK_ERR_INT		BIT(21)		/* RW */
+#define EPN_OUT_STALL_INT		BIT(20)		/* RW */
+#define EPN_OUT_INT			BIT(19)		/* RW */
+#define EPN_OUT_NULL_INT		BIT(18)		/* RW */
+#define EPN_OUT_FULL			BIT(17)		/* R */
+#define EPN_OUT_EMPTY			BIT(16)		/* R */
+
+#define EPN_IPID			BIT(10)		/* R */
+#define EPN_IN_NOTKN			BIT(9)		/* R */
+#define EPN_ISO_UR			BIT(8)		/* R */
+#define EPN_IN_END_INT			BIT(7)		/* RW */
+
+#define EPN_IN_NAK_ERR_INT		BIT(5)		/* RW */
+#define EPN_IN_STALL_INT		BIT(4)		/* RW */
+#define EPN_IN_INT			BIT(3)		/* RW */
+#define EPN_IN_DATA			BIT(2)		/* R */
+#define EPN_IN_FULL			BIT(1)		/* R */
+#define EPN_IN_EMPTY			BIT(0)		/* R */
+
+#define EPN_INT_EN	\
+	(EPN_OUT_END_INT | EPN_OUT_INT | EPN_IN_END_INT | EPN_IN_INT)
+
+/*------- (0x0048:) EPN Interrupt Enable Register */
+#define EPN_OUT_END_EN			BIT(23)		/* RW */
+#define EPN_OUT_OR_EN			BIT(22)		/* RW */
+#define EPN_OUT_NAK_ERR_EN		BIT(21)		/* RW */
+#define EPN_OUT_STALL_EN		BIT(20)		/* RW */
+#define EPN_OUT_EN			BIT(19)		/* RW */
+#define EPN_OUT_NULL_EN			BIT(18)		/* RW */
+
+#define EPN_IN_END_EN			BIT(7)		/* RW */
+
+#define EPN_IN_NAK_ERR_EN		BIT(5)		/* RW */
+#define EPN_IN_STALL_EN			BIT(4)		/* RW */
+#define EPN_IN_EN			BIT(3)		/* RW */
+
+/*------- (0x004C:) EPN Interrupt Enable Register */
+#define EPN_STOP_MODE			BIT(11)
+#define EPN_DEND_SET			BIT(10)
+#define EPN_BURST_SET			BIT(9)
+#define EPN_STOP_SET			BIT(8)
+
+#define EPN_DMA_EN			BIT(4)
+
+#define EPN_DMAMODE0			BIT(0)
+
+/*------- (0x0050:) EPN MaxPacket & BaseAddress Register */
+#define EPN_BASEAD			0x1FFF0000
+#define EPN_MPKT			0x000007FF
+
+/*------- (0x0054:) EPN Length & DMA Count Register */
+#define EPN_DMACNT			0x01FF0000
+#define EPN_LDATA			0x000007FF
+
+/*------- (0x0058:) EPN Read Register */
+/*------- (0x005C:) EPN Write Register */
+
+/*------- (0x1000) AHBSCTR Register */
+#define WAIT_MODE			BIT(0)
+
+/*------- (0x1004) AHBMCTR Register */
+#define ARBITER_CTR			BIT(31)		/* RW */
+#define MCYCLE_RST			BIT(12)		/* RW */
+
+#define ENDIAN_CTR			(BIT(9) | BIT(8))	/* RW */
+#define ENDIAN_BYTE_SWAP		BIT(9)
+#define ENDIAN_HALF_WORD_SWAP		ENDIAN_CTR
+
+#define HBUSREQ_MODE			BIT(5)		/* RW */
+#define HTRANS_MODE			BIT(4)		/* RW */
+
+#define WBURST_TYPE			BIT(2)		/* RW */
+#define BURST_TYPE			(BIT(1) | BIT(0))	/* RW */
+#define BURST_MAX_16			0
+#define BURST_MAX_8			BIT(0)
+#define BURST_MAX_4			BIT(1)
+#define BURST_SINGLE			BURST_TYPE
+
+/*------- (0x1008) AHBBINT Register */
+#define DMA_ENDINT			0xFFFE0000	/* RW */
+
+#define AHB_VBUS_INT			BIT(13)		/* RW */
+
+#define MBUS_ERRINT			BIT(6)		/* RW */
+
+#define SBUS_ERRINT0			BIT(4)		/* RW */
+#define ERR_MASTER			0x0000000F	/* R */
+
+/*------- (0x100C) AHBBINTEN Register */
+#define DMA_ENDINTEN			0xFFFE0000	/* RW */
+
+#define VBUS_INTEN			BIT(13)		/* RW */
+
+#define MBUS_ERRINTEN			BIT(6)		/* RW */
+
+#define SBUS_ERRINT0EN			BIT(4)		/* RW */
+
+/*------- (0x1010) EPCTR Register */
+#define DIRPD				BIT(12)		/* RW */
+
+#define VBUS_LEVEL			BIT(8)		/* R */
+
+#define PLL_RESUME			BIT(5)		/* RW */
+#define PLL_LOCK			BIT(4)		/* R */
+
+#define EPC_RST				BIT(0)		/* RW */
+
+/*------- (0x1014) USBF_EPTEST Register */
+#define LINESTATE			(BIT(9) | BIT(8))	/* R */
+#define DM_LEVEL			BIT(9)		/* R */
+#define DP_LEVEL			BIT(8)		/* R */
+
+#define PHY_TST				BIT(1)		/* RW */
+#define PHY_TSTCLK			BIT(0)		/* RW */
+
+/*------- (0x1020) USBSSVER Register */
+#define AHBB_VER			0x00FF0000	/* R */
+#define EPC_VER				0x0000FF00	/* R */
+#define SS_VER				0x000000FF	/* R */
+
+/*------- (0x1024) USBSSCONF Register */
+#define EP_AVAILABLE			0xFFFF0000	/* R */
+#define DMA_AVAILABLE			0x0000FFFF	/* R */
+
+/*------- (0x1110:) EPNDCR1 Register */
+#define DCR1_EPN_DMACNT			0x00FF0000	/* RW */
+
+#define DCR1_EPN_DIR0			BIT(1)		/* RW */
+#define DCR1_EPN_REQEN			BIT(0)		/* RW */
+
+/*------- (0x1114:) EPNDCR2 Register */
+#define DCR2_EPN_LMPKT			0x07FF0000	/* RW */
+
+#define DCR2_EPN_MPKT			0x000007FF	/* RW */
+
+/*------- (0x1118:) EPNTADR Register */
+#define EPN_TADR			0xFFFFFFFF	/* RW */
+
+/*===========================================================================*/
+/* Struct */
+/*------- ep_regs */
+struct ep_regs {
+	u32 EP_CONTROL;			/* EP Control */
+	u32 EP_STATUS;			/* EP Status */
+	u32 EP_INT_ENA;			/* EP Interrupt Enable */
+	u32 EP_DMA_CTRL;		/* EP DMA Control */
+	u32 EP_PCKT_ADRS;		/* EP Maxpacket & BaseAddress */
+	u32 EP_LEN_DCNT;		/* EP Length & DMA count */
+	u32 EP_READ;			/* EP Read */
+	u32 EP_WRITE;			/* EP Write */
+};
+
+/*------- ep_dcr */
+struct ep_dcr {
+	u32 EP_DCR1;			/* EP_DCR1 */
+	u32 EP_DCR2;			/* EP_DCR2 */
+	u32 EP_TADR;			/* EP_TADR */
+	u32 Reserved;			/* Reserved */
+};
+
+/*------- Function Registers */
+struct fc_regs {
+	u32 USB_CONTROL;		/* (0x0000) USB Control */
+	u32 USB_STATUS;			/* (0x0004) USB Status */
+	u32 USB_ADDRESS;		/* (0x0008) USB Address */
+	u32 UTMI_CHARACTER_1;		/* (0x000C) UTMI Setting */
+	u32 TEST_CONTROL;		/* (0x0010) TEST Control */
+	u32 reserved_14;		/* (0x0014) Reserved */
+	u32 SETUP_DATA0;		/* (0x0018) Setup Data0 */
+	u32 SETUP_DATA1;		/* (0x001C) Setup Data1 */
+	u32 USB_INT_STA;		/* (0x0020) USB Interrupt Status */
+	u32 USB_INT_ENA;		/* (0x0024) USB Interrupt Enable */
+	u32 EP0_CONTROL;		/* (0x0028) EP0 Control */
+	u32 EP0_STATUS;			/* (0x002C) EP0 Status */
+	u32 EP0_INT_ENA;		/* (0x0030) EP0 Interrupt Enable */
+	u32 EP0_LENGTH;			/* (0x0034) EP0 Length */
+	u32 EP0_READ;			/* (0x0038) EP0 Read */
+	u32 EP0_WRITE;			/* (0x003C) EP0 Write */
+
+	struct ep_regs EP_REGS[REG_EP_NUM];	/* Endpoint Register */
+
+	u8 reserved_220[0x1000 - 0x220];	/* (0x0220:0x0FFF) Reserved */
+
+	u32 AHBSCTR;			/* (0x1000) AHBSCTR */
+	u32 AHBMCTR;			/* (0x1004) AHBMCTR */
+	u32 AHBBINT;			/* (0x1008) AHBBINT */
+	u32 AHBBINTEN;			/* (0x100C) AHBBINTEN */
+	u32 EPCTR;			/* (0x1010) EPCTR */
+	u32 USBF_EPTEST;		/* (0x1014) USBF_EPTEST */
+
+	u8 reserved_1018[0x20 - 0x18];	/* (0x1018:0x101F) Reserved */
+
+	u32 USBSSVER;			/* (0x1020) USBSSVER */
+	u32 USBSSCONF;			/* (0x1024) USBSSCONF */
+
+	u8 reserved_1028[0x110 - 0x28];	/* (0x1028:0x110F) Reserved */
+
+	struct ep_dcr EP_DCR[REG_EP_NUM];	/* */
+
+	u8 reserved_1200[0x1000 - 0x200];	/* Reserved */
+} __aligned(32);
+
+#define EP0_PACKETSIZE			64
+#define EP_PACKETSIZE			1024
+
+/* EPN RAM SIZE */
+#define D_RAM_SIZE_CTRL			64
+
+/* EPN Bulk Endpoint Max Packet Size */
+#define D_FS_RAM_SIZE_BULK		64
+#define D_HS_RAM_SIZE_BULK		512
+
+struct nbu2ss_udc;
+
+enum ep0_state {
+	EP0_IDLE,
+	EP0_IN_DATA_PHASE,
+	EP0_OUT_DATA_PHASE,
+	EP0_IN_STATUS_PHASE,
+	EP0_OUT_STATUS_PAHSE,
+	EP0_END_XFER,
+	EP0_SUSPEND,
+	EP0_STALL,
+};
+
+struct nbu2ss_req {
+	struct usb_request		req;
+	struct list_head		queue;
+
+	u32			div_len;
+	bool		dma_flag;
+	bool		zero;
+
+	bool		unaligned;
+
+	unsigned			mapped:1;
+};
+
+struct nbu2ss_ep {
+	struct usb_ep			ep;
+	struct list_head		queue;
+
+	struct nbu2ss_udc		*udc;
+
+	const struct usb_endpoint_descriptor *desc;
+
+	u8		epnum;
+	u8		direct;
+	u8		ep_type;
+
+	unsigned		wedged:1;
+	unsigned		halted:1;
+	unsigned		stalled:1;
+
+	u8		*virt_buf;
+	dma_addr_t	phys_buf;
+};
+
+struct nbu2ss_udc {
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *driver;
+	struct platform_device *pdev;
+	struct device *dev;
+	spinlock_t lock; /* Protects nbu2ss_udc structure fields */
+	struct completion		*pdone;
+
+	enum ep0_state			ep0state;
+	enum usb_device_state	devstate;
+	struct usb_ctrlrequest	ctrl;
+	struct nbu2ss_req		ep0_req;
+	u8		ep0_buf[EP0_PACKETSIZE];
+
+	struct nbu2ss_ep	ep[NUM_ENDPOINTS];
+
+	unsigned		softconnect:1;
+	unsigned		vbus_active:1;
+	unsigned		linux_suspended:1;
+	unsigned		linux_resume:1;
+	unsigned		usb_suspended:1;
+	unsigned		remote_wakeup:1;
+	unsigned		udc_enabled:1;
+
+	unsigned int		mA;
+
+	u32		curr_config;	/* Current Configuration Number */
+
+	struct fc_regs __iomem *p_regs;
+};
+
+/* USB register access structure */
+union usb_reg_access {
+	struct {
+		unsigned char	DATA[4];
+	} byte;
+	unsigned int		dw;
+};
+
+/*-------------------------------------------------------------------------*/
+
+#endif  /* _LINUX_EMXX_H */
diff --git a/drivers/staging/fbtft/Kconfig b/drivers/staging/fbtft/Kconfig
new file mode 100644
index 000000000..4d29e8c10
--- /dev/null
+++ b/drivers/staging/fbtft/Kconfig
@@ -0,0 +1,202 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig FB_TFT
+	tristate "Support for small TFT LCD display modules"
+	depends on FB && SPI
+	depends on GPIOLIB || COMPILE_TEST
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	select FB_DEFERRED_IO
+	select FB_BACKLIGHT
+
+config FB_TFT_AGM1264K_FL
+	tristate "FB driver for the AGM1264K-FL LCD display"
+	depends on FB_TFT
+	help
+	  Framebuffer support for the AGM1264K-FL LCD display (two Samsung KS0108 compatible chips)
+
+config FB_TFT_BD663474
+	tristate "FB driver for the BD663474 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for BD663474
+
+config FB_TFT_HX8340BN
+	tristate "FB driver for the HX8340BN LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for HX8340BN
+
+config FB_TFT_HX8347D
+	tristate "FB driver for the HX8347D LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for HX8347D
+
+config FB_TFT_HX8353D
+	tristate "FB driver for the HX8353D LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for HX8353D
+
+config FB_TFT_HX8357D
+	tristate "FB driver for the HX8357D LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for HX8357D
+
+config FB_TFT_ILI9163
+	tristate "FB driver for the ILI9163 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9163
+
+config FB_TFT_ILI9320
+	tristate "FB driver for the ILI9320 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9320
+
+config FB_TFT_ILI9325
+	tristate "FB driver for the ILI9325 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9325
+
+config FB_TFT_ILI9340
+	tristate "FB driver for the ILI9340 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9340
+
+config FB_TFT_ILI9341
+	tristate "FB driver for the ILI9341 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9341
+
+config FB_TFT_ILI9481
+	tristate "FB driver for the ILI9481 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9481
+
+config FB_TFT_ILI9486
+	tristate "FB driver for the ILI9486 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9486
+
+config FB_TFT_PCD8544
+	tristate "FB driver for the PCD8544 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for PCD8544
+
+config FB_TFT_RA8875
+	tristate "FB driver for the RA8875 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for RA8875
+
+config FB_TFT_S6D02A1
+	tristate "FB driver for the S6D02A1 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for S6D02A1
+
+config FB_TFT_S6D1121
+	tristate "FB driver for the S6D1211 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for S6D1121
+
+config FB_TFT_SEPS525
+	tristate "FB driver for the SEPS525 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for SEPS525
+	  Say Y if you have such a display that utilizes this controller.
+
+config FB_TFT_SH1106
+	tristate "FB driver for the SH1106 OLED Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SH1106
+
+config FB_TFT_SSD1289
+	tristate "FB driver for the SSD1289 LCD Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SSD1289
+
+config FB_TFT_SSD1305
+	tristate "FB driver for the SSD1305 OLED Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SSD1305
+
+config FB_TFT_SSD1306
+	tristate "FB driver for the SSD1306 OLED Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SSD1306
+
+config FB_TFT_SSD1331
+	tristate "FB driver for the SSD1331 LCD Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SSD1331
+
+config FB_TFT_SSD1351
+	tristate "FB driver for the SSD1351 LCD Controller"
+	depends on FB_TFT
+	help
+	  Framebuffer support for SSD1351
+
+config FB_TFT_ST7735R
+	tristate "FB driver for the ST7735R LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ST7735R
+
+config FB_TFT_ST7789V
+	tristate "FB driver for the ST7789V LCD Controller"
+	depends on FB_TFT
+	help
+	  This enables generic framebuffer support for the Sitronix ST7789V
+	  display controller. The controller is intended for small color
+	  displays with a resolution of up to 320x240 pixels.
+
+	  Say Y if you have such a display that utilizes this controller.
+
+config FB_TFT_TINYLCD
+	tristate "FB driver for tinylcd.com display"
+	depends on FB_TFT
+	help
+	  Custom Framebuffer support for tinylcd.com display
+
+config FB_TFT_TLS8204
+	tristate "FB driver for the TLS8204 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for TLS8204
+
+config FB_TFT_UC1611
+	tristate "FB driver for the UC1611 LCD controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for UC1611
+
+config FB_TFT_UC1701
+	tristate "FB driver for the UC1701 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for UC1701
+
+config FB_TFT_UPD161704
+	tristate "FB driver for the uPD161704 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for uPD161704
diff --git a/drivers/staging/fbtft/Makefile b/drivers/staging/fbtft/Makefile
new file mode 100644
index 000000000..e9cdf0f0a
--- /dev/null
+++ b/drivers/staging/fbtft/Makefile
@@ -0,0 +1,38 @@
+# SPDX-License-Identifier: GPL-2.0
+# Core module
+obj-$(CONFIG_FB_TFT)             += fbtft.o
+fbtft-y                          += fbtft-core.o fbtft-sysfs.o fbtft-bus.o fbtft-io.o
+
+# drivers
+obj-$(CONFIG_FB_TFT_AGM1264K_FL) += fb_agm1264k-fl.o
+obj-$(CONFIG_FB_TFT_BD663474)    += fb_bd663474.o
+obj-$(CONFIG_FB_TFT_HX8340BN)    += fb_hx8340bn.o
+obj-$(CONFIG_FB_TFT_HX8347D)     += fb_hx8347d.o
+obj-$(CONFIG_FB_TFT_HX8353D)     += fb_hx8353d.o
+obj-$(CONFIG_FB_TFT_HX8357D)     += fb_hx8357d.o
+obj-$(CONFIG_FB_TFT_ILI9163)     += fb_ili9163.o
+obj-$(CONFIG_FB_TFT_ILI9320)     += fb_ili9320.o
+obj-$(CONFIG_FB_TFT_ILI9325)     += fb_ili9325.o
+obj-$(CONFIG_FB_TFT_ILI9340)     += fb_ili9340.o
+obj-$(CONFIG_FB_TFT_ILI9341)     += fb_ili9341.o
+obj-$(CONFIG_FB_TFT_ILI9481)     += fb_ili9481.o
+obj-$(CONFIG_FB_TFT_ILI9486)     += fb_ili9486.o
+obj-$(CONFIG_FB_TFT_PCD8544)     += fb_pcd8544.o
+obj-$(CONFIG_FB_TFT_RA8875)      += fb_ra8875.o
+obj-$(CONFIG_FB_TFT_S6D02A1)     += fb_s6d02a1.o
+obj-$(CONFIG_FB_TFT_S6D1121)     += fb_s6d1121.o
+obj-$(CONFIG_FB_TFT_SEPS525)     += fb_seps525.o
+obj-$(CONFIG_FB_TFT_SH1106)      += fb_sh1106.o
+obj-$(CONFIG_FB_TFT_SSD1289)     += fb_ssd1289.o
+obj-$(CONFIG_FB_TFT_SSD1305)     += fb_ssd1305.o
+obj-$(CONFIG_FB_TFT_SSD1306)     += fb_ssd1306.o
+obj-$(CONFIG_FB_TFT_SSD1305)     += fb_ssd1325.o
+obj-$(CONFIG_FB_TFT_SSD1331)     += fb_ssd1331.o
+obj-$(CONFIG_FB_TFT_SSD1351)     += fb_ssd1351.o
+obj-$(CONFIG_FB_TFT_ST7735R)     += fb_st7735r.o
+obj-$(CONFIG_FB_TFT_ST7789V)     += fb_st7789v.o
+obj-$(CONFIG_FB_TFT_TINYLCD)     += fb_tinylcd.o
+obj-$(CONFIG_FB_TFT_TLS8204)     += fb_tls8204.o
+obj-$(CONFIG_FB_TFT_UC1611)      += fb_uc1611.o
+obj-$(CONFIG_FB_TFT_UC1701)      += fb_uc1701.o
+obj-$(CONFIG_FB_TFT_UPD161704)   += fb_upd161704.o
diff --git a/drivers/staging/fbtft/README b/drivers/staging/fbtft/README
new file mode 100644
index 000000000..ba4c74c92
--- /dev/null
+++ b/drivers/staging/fbtft/README
@@ -0,0 +1,32 @@
+  FBTFT
+=========
+
+Linux Framebuffer drivers for small TFT LCD display modules.
+The module 'fbtft' makes writing drivers for some of these displays very easy.
+
+Development is done on a Raspberry Pi running the Raspbian "wheezy" distribution.
+
+INSTALLATION
+  Download kernel sources
+
+  From Linux 3.15
+    cd drivers/video/fbdev/fbtft
+    git clone https://github.com/notro/fbtft.git
+
+    Add to drivers/video/fbdev/Kconfig:   source "drivers/video/fbdev/fbtft/Kconfig"
+    Add to drivers/video/fbdev/Makefile:  obj-y += fbtft/
+
+  Before Linux 3.15
+    cd drivers/video
+    git clone https://github.com/notro/fbtft.git
+
+    Add to drivers/video/Kconfig:   source "drivers/video/fbtft/Kconfig"
+    Add to drivers/video/Makefile:  obj-y += fbtft/
+
+  Enable driver(s) in menuconfig and build the kernel
+
+
+See wiki for more information: https://github.com/notro/fbtft/wiki
+
+
+Source: https://github.com/notro/fbtft/
diff --git a/drivers/staging/fbtft/TODO b/drivers/staging/fbtft/TODO
new file mode 100644
index 000000000..e72a08bf2
--- /dev/null
+++ b/drivers/staging/fbtft/TODO
@@ -0,0 +1,3 @@
+* convert all these over to drm_simple_display_pipe and submit for inclusion
+  into the DRM subsystem under drivers/gpu/drm - fbdev doesn't take any new
+  drivers anymore.
diff --git a/drivers/staging/fbtft/fb_agm1264k-fl.c b/drivers/staging/fbtft/fb_agm1264k-fl.c
new file mode 100644
index 000000000..207d57854
--- /dev/null
+++ b/drivers/staging/fbtft/fb_agm1264k-fl.c
@@ -0,0 +1,441 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for Two KS0108 LCD controllers in AGM1264K-FL display
+ *
+ * Copyright (C) 2014 ololoshka2871
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include "fbtft.h"
+
+/* Uncomment text line to use negative image on display */
+/*#define NEGATIVE*/
+
+#define WHITE		0xff
+#define BLACK		0
+
+#define DRVNAME		"fb_agm1264k-fl"
+#define WIDTH		64
+#define HEIGHT		64
+#define TOTALWIDTH	(WIDTH * 2)	 /* because 2 x ks0108 in one display */
+#define FPS			20
+
+#define EPIN		gpio.wr
+#define RS			gpio.dc
+#define RW			gpio.aux[2]
+#define CS0			gpio.aux[0]
+#define CS1			gpio.aux[1]
+
+/* diffusing error (Floyd-Steinberg) */
+#define DIFFUSING_MATRIX_WIDTH	2
+#define DIFFUSING_MATRIX_HEIGHT	2
+
+static const signed char
+diffusing_matrix[DIFFUSING_MATRIX_WIDTH][DIFFUSING_MATRIX_HEIGHT] = {
+	{-1, 3},
+	{3, 2},
+};
+
+static const unsigned char gamma_correction_table[] = {
+0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1,
+1, 1, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 6,
+6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 11, 11, 11, 12, 12, 13,
+13, 13, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18, 19, 19, 20, 20, 21,
+22, 22, 23, 23, 24, 25, 25, 26, 26, 27, 28, 28, 29, 30, 30, 31, 32,
+33, 33, 34, 35, 35, 36, 37, 38, 39, 39, 40, 41, 42, 43, 43, 44, 45,
+46, 47, 48, 49, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61,
+62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 73, 74, 75, 76, 77, 78, 79, 81,
+82, 83, 84, 85, 87, 88, 89, 90, 91, 93, 94, 95, 97, 98, 99, 100, 102,
+103, 105, 106, 107, 109, 110, 111, 113, 114, 116, 117, 119, 120, 121,
+123, 124, 126, 127, 129, 130, 132, 133, 135, 137, 138, 140, 141, 143,
+145, 146, 148, 149, 151, 153, 154, 156, 158, 159, 161, 163, 165, 166,
+168, 170, 172, 173, 175, 177, 179, 181, 182, 184, 186, 188, 190, 192,
+194, 196, 197, 199, 201, 203, 205, 207, 209, 211, 213, 215, 217, 219,
+221, 223, 225, 227, 229, 231, 234, 236, 238, 240, 242, 244, 246, 248,
+251, 253, 255
+};
+
+static int init_display(struct fbtft_par *par)
+{
+	u8 i;
+
+	par->fbtftops.reset(par);
+
+	for (i = 0; i < 2; ++i) {
+		write_reg(par, i, 0x3f); /* display on */
+		write_reg(par, i, 0x40); /* set x to 0 */
+		write_reg(par, i, 0xb0); /* set page to 0 */
+		write_reg(par, i, 0xc0); /* set start line to 0 */
+	}
+
+	return 0;
+}
+
+/* Check if all necessary GPIOS defined */
+static int verify_gpios(struct fbtft_par *par)
+{
+	int i;
+
+	dev_dbg(par->info->device,
+		"%s()\n", __func__);
+
+	if (!par->EPIN) {
+		dev_err(par->info->device,
+			"Missing info about 'wr' (aka E) gpio. Aborting.\n");
+		return -EINVAL;
+	}
+	for (i = 0; i < 8; ++i) {
+		if (!par->gpio.db[i]) {
+			dev_err(par->info->device,
+				"Missing info about 'db[%i]' gpio. Aborting.\n",
+				i);
+			return -EINVAL;
+		}
+	}
+	if (!par->CS0) {
+		dev_err(par->info->device,
+			"Missing info about 'cs0' gpio. Aborting.\n");
+		return -EINVAL;
+	}
+	if (!par->CS1) {
+		dev_err(par->info->device,
+			"Missing info about 'cs1' gpio. Aborting.\n");
+		return -EINVAL;
+	}
+	if (!par->RW) {
+		dev_err(par->info->device,
+			"Missing info about 'rw' gpio. Aborting.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static unsigned long
+request_gpios_match(struct fbtft_par *par, const struct fbtft_gpio *gpio)
+{
+	dev_dbg(par->info->device,
+		"%s('%s')\n", __func__, gpio->name);
+
+	if (strcasecmp(gpio->name, "wr") == 0) {
+		/* left ks0108 E pin */
+		par->EPIN = gpio->gpio;
+		return GPIOD_OUT_LOW;
+	} else if (strcasecmp(gpio->name, "cs0") == 0) {
+		/* left ks0108 controller pin */
+		par->CS0 = gpio->gpio;
+		return GPIOD_OUT_HIGH;
+	} else if (strcasecmp(gpio->name, "cs1") == 0) {
+		/* right ks0108 controller pin */
+		par->CS1 = gpio->gpio;
+		return GPIOD_OUT_HIGH;
+	}
+
+	/* if write (rw = 0) e(1->0) perform write */
+	/* if read (rw = 1) e(0->1) set data on D0-7*/
+	else if (strcasecmp(gpio->name, "rw") == 0) {
+		par->RW = gpio->gpio;
+		return GPIOD_OUT_LOW;
+	}
+
+	return FBTFT_GPIO_NO_MATCH;
+}
+
+/* This function oses to enter commands
+ * first byte - destination controller 0 or 1
+ * following - commands
+ */
+static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
+{
+	va_list args;
+	int i, ret;
+	u8 *buf = par->buf;
+
+	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {
+		va_start(args, len);
+		for (i = 0; i < len; i++)
+			buf[i] = (u8)va_arg(args, unsigned int);
+
+		va_end(args);
+		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par, par->info->device,
+				  u8, buf, len, "%s: ", __func__);
+}
+
+	va_start(args, len);
+
+	*buf = (u8)va_arg(args, unsigned int);
+
+	if (*buf > 1) {
+		va_end(args);
+		dev_err(par->info->device,
+			"Incorrect chip select request (%d)\n", *buf);
+		return;
+	}
+
+	/* select chip */
+	if (*buf) {
+		/* cs1 */
+		gpiod_set_value(par->CS0, 0);
+		gpiod_set_value(par->CS1, 1);
+	} else {
+		/* cs0 */
+		gpiod_set_value(par->CS0, 1);
+		gpiod_set_value(par->CS1, 0);
+	}
+
+	gpiod_set_value(par->RS, 0); /* RS->0 (command mode) */
+	len--;
+
+	if (len) {
+		i = len;
+		while (i--)
+			*buf++ = (u8)va_arg(args, unsigned int);
+		ret = par->fbtftops.write(par, par->buf, len * (sizeof(u8)));
+		if (ret < 0) {
+			va_end(args);
+			dev_err(par->info->device,
+				"write() failed and returned %d\n", ret);
+			return;
+		}
+	}
+
+	va_end(args);
+}
+
+static struct
+{
+	int xs, ys_page, xe, ye_page;
+} addr_win;
+
+/* save display writing zone */
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	addr_win.xs = xs;
+	addr_win.ys_page = ys / 8;
+	addr_win.xe = xe;
+	addr_win.ye_page = ye / 8;
+}
+
+static void
+construct_line_bitmap(struct fbtft_par *par, u8 *dest, signed short *src,
+		      int xs, int xe, int y)
+{
+	int x, i;
+
+	for (x = xs; x < xe; ++x) {
+		u8 res = 0;
+
+		for (i = 0; i < 8; i++)
+			if (src[(y * 8 + i) * par->info->var.xres + x])
+				res |= 1 << i;
+#ifdef NEGATIVE
+		*dest++ = res;
+#else
+		*dest++ = ~res;
+#endif
+	}
+}
+
+static void iterate_diffusion_matrix(u32 xres, u32 yres, int x,
+				     int y, signed short *convert_buf,
+				     signed short pixel, signed short error)
+{
+	u16 i, j;
+
+	/* diffusion matrix row */
+	for (i = 0; i < DIFFUSING_MATRIX_WIDTH; ++i)
+		/* diffusion matrix column */
+		for (j = 0; j < DIFFUSING_MATRIX_HEIGHT; ++j) {
+			signed short *write_pos;
+			signed char coeff;
+
+			/* skip pixels out of zone */
+			if (x + i < 0 || x + i >= xres || y + j >= yres)
+				continue;
+			write_pos = &convert_buf[(y + j) * xres + x + i];
+			coeff = diffusing_matrix[i][j];
+			if (-1 == coeff) {
+				/* pixel itself */
+				*write_pos = pixel;
+			} else {
+				signed short p = *write_pos + error * coeff;
+
+				if (p > WHITE)
+					p = WHITE;
+				if (p < BLACK)
+					p = BLACK;
+				*write_pos = p;
+			}
+		}
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u8 *buf = par->txbuf.buf;
+	int x, y;
+	int ret = 0;
+
+	/* buffer to convert RGB565 -> grayscale16 -> Dithered image 1bpp */
+	signed short *convert_buf = kmalloc_array(par->info->var.xres *
+		par->info->var.yres, sizeof(signed short), GFP_NOIO);
+
+	if (!convert_buf)
+		return -ENOMEM;
+
+	/* converting to grayscale16 */
+	for (x = 0; x < par->info->var.xres; ++x)
+		for (y = 0; y < par->info->var.yres; ++y) {
+			u16 pixel = vmem16[y *  par->info->var.xres + x];
+			u16 b = pixel & 0x1f;
+			u16 g = (pixel & (0x3f << 5)) >> 5;
+			u16 r = (pixel & (0x1f << (5 + 6))) >> (5 + 6);
+
+			pixel = (299 * r + 587 * g + 114 * b) / 200;
+			if (pixel > 255)
+				pixel = 255;
+
+			/* gamma-correction by table */
+			convert_buf[y *  par->info->var.xres + x] =
+				(signed short)gamma_correction_table[pixel];
+		}
+
+	/* Image Dithering */
+	for (x = 0; x < par->info->var.xres; ++x)
+		for (y = 0; y < par->info->var.yres; ++y) {
+			signed short pixel =
+				convert_buf[y *  par->info->var.xres + x];
+			signed short error_b = pixel - BLACK;
+			signed short error_w = pixel - WHITE;
+			signed short error;
+
+			/* what color close? */
+			if (abs(error_b) >= abs(error_w)) {
+				/* white */
+				error = error_w;
+				pixel = 0xff;
+			} else {
+				/* black */
+				error = error_b;
+				pixel = 0;
+			}
+
+			error /= 8;
+
+			iterate_diffusion_matrix(par->info->var.xres,
+						 par->info->var.yres,
+						 x, y, convert_buf,
+						 pixel, error);
+		}
+
+	/* 1 string = 2 pages */
+	for (y = addr_win.ys_page; y <= addr_win.ye_page; ++y) {
+		/* left half of display */
+		if (addr_win.xs < par->info->var.xres / 2) {
+			construct_line_bitmap(par, buf, convert_buf,
+					      addr_win.xs,
+					      par->info->var.xres / 2, y);
+
+			len = par->info->var.xres / 2 - addr_win.xs;
+
+			/* select left side (sc0)
+			 * set addr
+			 */
+			write_reg(par, 0x00, BIT(6) | (u8)addr_win.xs);
+			write_reg(par, 0x00, (0x17 << 3) | (u8)y);
+
+			/* write bitmap */
+			gpiod_set_value(par->RS, 1); /* RS->1 (data mode) */
+			ret = par->fbtftops.write(par, buf, len);
+			if (ret < 0)
+				dev_err(par->info->device,
+					"write failed and returned: %d\n",
+					ret);
+		}
+		/* right half of display */
+		if (addr_win.xe >= par->info->var.xres / 2) {
+			construct_line_bitmap(par, buf,
+					      convert_buf,
+					      par->info->var.xres / 2,
+					      addr_win.xe + 1, y);
+
+			len = addr_win.xe + 1 - par->info->var.xres / 2;
+
+			/* select right side (sc1)
+			 * set addr
+			 */
+			write_reg(par, 0x01, BIT(6));
+			write_reg(par, 0x01, (0x17 << 3) | (u8)y);
+
+			/* write bitmap */
+			gpiod_set_value(par->RS, 1); /* RS->1 (data mode) */
+			par->fbtftops.write(par, buf, len);
+			if (ret < 0)
+				dev_err(par->info->device,
+					"write failed and returned: %d\n",
+					ret);
+		}
+	}
+	kfree(convert_buf);
+
+	gpiod_set_value(par->CS0, 0);
+	gpiod_set_value(par->CS1, 0);
+
+	return ret;
+}
+
+static int write(struct fbtft_par *par, void *buf, size_t len)
+{
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	gpiod_set_value(par->RW, 0); /* set write mode */
+
+	while (len--) {
+		u8 i, data;
+
+		data = *(u8 *)buf++;
+
+		/* set data bus */
+		for (i = 0; i < 8; ++i)
+			gpiod_set_value(par->gpio.db[i], data & (1 << i));
+		/* set E */
+		gpiod_set_value(par->EPIN, 0);
+		udelay(5);
+		/* unset E - write */
+		gpiod_set_value(par->EPIN, 1);
+		udelay(1);
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = TOTALWIDTH,
+	.height = HEIGHT,
+	.fps = FPS,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.verify_gpios = verify_gpios,
+		.request_gpios_match = request_gpios_match,
+		.write = write,
+		.write_register = write_reg8_bus8,
+		.write_vmem = write_vmem,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "displaytronic,fb_agm1264k-fl", &display);
+
+MODULE_ALIAS("platform:" DRVNAME);
+
+MODULE_DESCRIPTION("Two KS0108 LCD controllers in AGM1264K-FL display");
+MODULE_AUTHOR("ololoshka2871");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_bd663474.c b/drivers/staging/fbtft/fb_bd663474.c
new file mode 100644
index 000000000..1629c2c44
--- /dev/null
+++ b/drivers/staging/fbtft/fb_bd663474.c
@@ -0,0 +1,171 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the uPD161704 LCD Controller
+ *
+ * Copyright (C) 2014 Seong-Woo Kim
+ *
+ * Based on fb_ili9325.c by Noralf Tronnes
+ * Based on ili9325.c by Jeroen Domburg
+ * Init code from UTFT library by Henning Karlsen
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_bd663474"
+#define WIDTH		240
+#define HEIGHT		320
+#define BPP		16
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Initialization sequence from Lib_UTFT */
+
+	/* oscillator start */
+	write_reg(par, 0x000, 0x0001);	/*oscillator 0: stop, 1: operation */
+	mdelay(10);
+
+	/* Power settings */
+	write_reg(par, 0x100, 0x0000); /* power supply setup */
+	write_reg(par, 0x101, 0x0000);
+	write_reg(par, 0x102, 0x3110);
+	write_reg(par, 0x103, 0xe200);
+	write_reg(par, 0x110, 0x009d);
+	write_reg(par, 0x111, 0x0022);
+	write_reg(par, 0x100, 0x0120);
+	mdelay(20);
+
+	write_reg(par, 0x100, 0x3120);
+	mdelay(80);
+	/* Display control */
+	write_reg(par, 0x001, 0x0100);
+	write_reg(par, 0x002, 0x0000);
+	write_reg(par, 0x003, 0x1230);
+	write_reg(par, 0x006, 0x0000);
+	write_reg(par, 0x007, 0x0101);
+	write_reg(par, 0x008, 0x0808);
+	write_reg(par, 0x009, 0x0000);
+	write_reg(par, 0x00b, 0x0000);
+	write_reg(par, 0x00c, 0x0000);
+	write_reg(par, 0x00d, 0x0018);
+	/* LTPS control settings */
+	write_reg(par, 0x012, 0x0000);
+	write_reg(par, 0x013, 0x0000);
+	write_reg(par, 0x018, 0x0000);
+	write_reg(par, 0x019, 0x0000);
+
+	write_reg(par, 0x203, 0x0000);
+	write_reg(par, 0x204, 0x0000);
+
+	write_reg(par, 0x210, 0x0000);
+	write_reg(par, 0x211, 0x00ef);
+	write_reg(par, 0x212, 0x0000);
+	write_reg(par, 0x213, 0x013f);
+	write_reg(par, 0x214, 0x0000);
+	write_reg(par, 0x215, 0x0000);
+	write_reg(par, 0x216, 0x0000);
+	write_reg(par, 0x217, 0x0000);
+
+	/* Gray scale settings */
+	write_reg(par, 0x300, 0x5343);
+	write_reg(par, 0x301, 0x1021);
+	write_reg(par, 0x302, 0x0003);
+	write_reg(par, 0x303, 0x0011);
+	write_reg(par, 0x304, 0x050a);
+	write_reg(par, 0x305, 0x4342);
+	write_reg(par, 0x306, 0x1100);
+	write_reg(par, 0x307, 0x0003);
+	write_reg(par, 0x308, 0x1201);
+	write_reg(par, 0x309, 0x050a);
+
+	/* RAM access settings */
+	write_reg(par, 0x400, 0x4027);
+	write_reg(par, 0x401, 0x0000);
+	write_reg(par, 0x402, 0x0000);  /* First screen drive position (1) */
+	write_reg(par, 0x403, 0x013f);  /* First screen drive position (2) */
+	write_reg(par, 0x404, 0x0000);
+
+	write_reg(par, 0x200, 0x0000);
+	write_reg(par, 0x201, 0x0000);
+	write_reg(par, 0x100, 0x7120);
+	write_reg(par, 0x007, 0x0103);
+	mdelay(10);
+	write_reg(par, 0x007, 0x0113);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R200h = Horizontal GRAM Start Address */
+	/* R201h = Vertical GRAM Start Address */
+	case 0:
+		write_reg(par, 0x0200, xs);
+		write_reg(par, 0x0201, ys);
+		break;
+	case 180:
+		write_reg(par, 0x0200, WIDTH - 1 - xs);
+		write_reg(par, 0x0201, HEIGHT - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x0200, WIDTH - 1 - ys);
+		write_reg(par, 0x0201, xs);
+		break;
+	case 90:
+		write_reg(par, 0x0200, ys);
+		write_reg(par, 0x0201, HEIGHT - 1 - xs);
+		break;
+	}
+	write_reg(par, 0x202); /* Write Data to GRAM */
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	/* AM: GRAM update direction */
+	case 0:
+		write_reg(par, 0x003, 0x1230);
+		break;
+	case 180:
+		write_reg(par, 0x003, 0x1200);
+		break;
+	case 270:
+		write_reg(par, 0x003, 0x1228);
+		break;
+	case 90:
+		write_reg(par, 0x003, 0x1218);
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "hitachi,bd663474", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:bd663474");
+MODULE_ALIAS("platform:bd663474");
+
+MODULE_DESCRIPTION("FB driver for the uPD161704 LCD Controller");
+MODULE_AUTHOR("Seong-Woo Kim");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8340bn.c b/drivers/staging/fbtft/fb_hx8340bn.c
new file mode 100644
index 000000000..2fd7b87ea
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8340bn.c
@@ -0,0 +1,225 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the HX8340BN LCD Controller
+ *
+ * This display uses 9-bit SPI: Data/Command bit + 8 data bits
+ * For platforms that doesn't support 9-bit, the driver is capable
+ * of emulating this using 8-bit transfer.
+ * This is done by transferring eight 9-bit words in 9 bytes.
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_hx8340bn"
+#define WIDTH		176
+#define HEIGHT		220
+#define TXBUFLEN	(4 * PAGE_SIZE)
+#define DEFAULT_GAMMA	"1 3 0E 5 0 2 09 0 6 1 7 1 0 2 2\n" \
+			"3 3 17 8 4 7 05 7 6 0 3 1 6 0 0 "
+
+static bool emulate;
+module_param(emulate, bool, 0000);
+MODULE_PARM_DESC(emulate, "Force emulation in 9-bit mode");
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* BTL221722-276L startup sequence, from datasheet */
+
+	/*
+	 * SETEXTCOM: Set extended command set (C1h)
+	 * This command is used to set extended command set access enable.
+	 * Enable: After command (C1h), must write: ffh,83h,40h
+	 */
+	write_reg(par, 0xC1, 0xFF, 0x83, 0x40);
+
+	/*
+	 * Sleep out
+	 * This command turns off sleep mode.
+	 * In this mode the DC/DC converter is enabled, Internal oscillator
+	 * is started, and panel scanning is started.
+	 */
+	write_reg(par, 0x11);
+	mdelay(150);
+
+	/* Undoc'd register? */
+	write_reg(par, 0xCA, 0x70, 0x00, 0xD9);
+
+	/*
+	 * SETOSC: Set Internal Oscillator (B0h)
+	 * This command is used to set internal oscillator related settings
+	 *	OSC_EN: Enable internal oscillator
+	 *	Internal oscillator frequency: 125% x 2.52MHz
+	 */
+	write_reg(par, 0xB0, 0x01, 0x11);
+
+	/* Drive ability setting */
+	write_reg(par, 0xC9, 0x90, 0x49, 0x10, 0x28, 0x28, 0x10, 0x00, 0x06);
+	mdelay(20);
+
+	/*
+	 * SETPWCTR5: Set Power Control 5(B5h)
+	 * This command is used to set VCOM Low and VCOM High Voltage
+	 * VCOMH 0110101 :  3.925
+	 * VCOML 0100000 : -1.700
+	 * 45h=69  VCOMH: "VMH" + 5d   VCOML: "VMH" + 5d
+	 */
+	write_reg(par, 0xB5, 0x35, 0x20, 0x45);
+
+	/*
+	 * SETPWCTR4: Set Power Control 4(B4h)
+	 *	VRH[4:0]:	Specify the VREG1 voltage adjusting.
+	 *			VREG1 voltage is for gamma voltage setting.
+	 *	BT[2:0]:	Switch the output factor of step-up circuit 2
+	 *			for VGH and VGL voltage generation.
+	 */
+	write_reg(par, 0xB4, 0x33, 0x25, 0x4C);
+	mdelay(10);
+
+	/*
+	 * Interface Pixel Format (3Ah)
+	 * This command is used to define the format of RGB picture data,
+	 * which is to be transfer via the system and RGB interface.
+	 * RGB interface: 16 Bit/Pixel
+	 */
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, MIPI_DCS_PIXEL_FMT_16BIT);
+
+	/*
+	 * Display on (29h)
+	 * This command is used to recover from DISPLAY OFF mode.
+	 * Output from the Frame Memory is enabled.
+	 */
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+	mdelay(10);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS, 0x00, xs, 0x00, xe);
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS, 0x00, ys, 0x00, ye);
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	/* MADCTL - Memory data access control */
+	/* RGB/BGR can be set with H/W pin SRGB and MADCTL BGR bit */
+#define MY BIT(7)
+#define MX BIT(6)
+#define MV BIT(5)
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, par->bgr << 3);
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MV | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MY | (par->bgr << 3));
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MY | MV | (par->bgr << 3));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma Curve selection, GC (only GC0 can be customized):
+ *   0 = 2.2, 1 = 1.8, 2 = 2.5, 3 = 1.0
+ * Gamma string format:
+ *   OP0 OP1 CP0 CP1 CP2 CP3 CP4 MP0 MP1 MP2 MP3 MP4 MP5 CGM0 CGM1
+ *   ON0 ON1 CN0 CN1 CN2 CN3 CN4 MN0 MN1 MN2 MN3 MN4 MN5 XXXX  GC
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x0f, 0x0f, 0x1f, 0x0f, 0x0f, 0x0f, 0x1f, 0x07, 0x07, 0x07,
+		0x07, 0x07, 0x07, 0x03, 0x03, 0x0f, 0x0f, 0x1f, 0x0f, 0x0f,
+		0x0f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, 0x00,
+	};
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < par->gamma.num_curves; i++)
+		for (j = 0; j < par->gamma.num_values; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	/* Gamma Set (26h) */
+	write_reg(par, MIPI_DCS_SET_GAMMA_CURVE, 1 << CURVE(1, 14));
+
+	if (CURVE(1, 14))
+		return 0; /* only GC0 can be customized */
+
+	write_reg(par, 0xC2,
+		  (CURVE(0, 8) << 4) | CURVE(0, 7),
+		  (CURVE(0, 10) << 4) | CURVE(0, 9),
+		  (CURVE(0, 12) << 4) | CURVE(0, 11),
+		  CURVE(0, 2),
+		  (CURVE(0, 4) << 4) | CURVE(0, 3),
+		  CURVE(0, 5),
+		  CURVE(0, 6),
+		  (CURVE(0, 1) << 4) | CURVE(0, 0),
+		  (CURVE(0, 14) << 2) | CURVE(0, 13));
+
+	write_reg(par, 0xC3,
+		  (CURVE(1, 8) << 4) | CURVE(1, 7),
+		  (CURVE(1, 10) << 4) | CURVE(1, 9),
+		  (CURVE(1, 12) << 4) | CURVE(1, 11),
+		  CURVE(1, 2),
+		  (CURVE(1, 4) << 4) | CURVE(1, 3),
+		  CURVE(1, 5),
+		  CURVE(1, 6),
+		  (CURVE(1, 1) << 4) | CURVE(1, 0));
+
+	mdelay(10);
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = TXBUFLEN,
+	.gamma_num = 2,
+	.gamma_len = 15,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "himax,hx8340bn", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:hx8340bn");
+MODULE_ALIAS("platform:hx8340bn");
+
+MODULE_DESCRIPTION("FB driver for the HX8340BN LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8347d.c b/drivers/staging/fbtft/fb_hx8347d.c
new file mode 100644
index 000000000..a9b72a8b4
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8347d.c
@@ -0,0 +1,183 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the HX8347D LCD Controller
+ *
+ * Copyright (C) 2013 Christian Vogelgsang
+ *
+ * Based on driver code found here: https://github.com/watterott/r61505u-Adapter
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_hx8347d"
+#define WIDTH		320
+#define HEIGHT		240
+#define DEFAULT_GAMMA	"0 0 0 0 0 0 0 0 0 0 0 0 0 0\n" \
+			"0 0 0 0 0 0 0 0 0 0 0 0 0 0"
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* driving ability */
+	write_reg(par, 0xEA, 0x00);
+	write_reg(par, 0xEB, 0x20);
+	write_reg(par, 0xEC, 0x0C);
+	write_reg(par, 0xED, 0xC4);
+	write_reg(par, 0xE8, 0x40);
+	write_reg(par, 0xE9, 0x38);
+	write_reg(par, 0xF1, 0x01);
+	write_reg(par, 0xF2, 0x10);
+	write_reg(par, 0x27, 0xA3);
+
+	/* power voltage */
+	write_reg(par, 0x1B, 0x1B);
+	write_reg(par, 0x1A, 0x01);
+	write_reg(par, 0x24, 0x2F);
+	write_reg(par, 0x25, 0x57);
+
+	/* VCOM offset */
+	write_reg(par, 0x23, 0x8D); /* for flicker adjust */
+
+	/* power on */
+	write_reg(par, 0x18, 0x36);
+	write_reg(par, 0x19, 0x01); /* start osc */
+	write_reg(par, 0x01, 0x00); /* wakeup */
+	write_reg(par, 0x1F, 0x88);
+	mdelay(5);
+	write_reg(par, 0x1F, 0x80);
+	mdelay(5);
+	write_reg(par, 0x1F, 0x90);
+	mdelay(5);
+	write_reg(par, 0x1F, 0xD0);
+	mdelay(5);
+
+	/* color selection */
+	write_reg(par, 0x17, 0x05); /* 65k */
+
+	/*panel characteristic */
+	write_reg(par, 0x36, 0x00);
+
+	/*display on */
+	write_reg(par, 0x28, 0x38);
+	mdelay(40);
+	write_reg(par, 0x28, 0x3C);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, 0x02, (xs >> 8) & 0xFF);
+	write_reg(par, 0x03, xs & 0xFF);
+	write_reg(par, 0x04, (xe >> 8) & 0xFF);
+	write_reg(par, 0x05, xe & 0xFF);
+	write_reg(par, 0x06, (ys >> 8) & 0xFF);
+	write_reg(par, 0x07, ys & 0xFF);
+	write_reg(par, 0x08, (ye >> 8) & 0xFF);
+	write_reg(par, 0x09, ye & 0xFF);
+	write_reg(par, 0x22);
+}
+
+#define MEM_Y   BIT(7) /* MY row address order */
+#define MEM_X   BIT(6) /* MX column address order */
+#define MEM_V   BIT(5) /* MV row / column exchange */
+#define MEM_L   BIT(4) /* ML vertical refresh order */
+#define MEM_BGR (3) /* RGB-BGR Order */
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, 0x16, MEM_V | MEM_X | (par->bgr << MEM_BGR));
+		break;
+	case 270:
+		write_reg(par, 0x16, par->bgr << MEM_BGR);
+		break;
+	case 180:
+		write_reg(par, 0x16, MEM_V | MEM_Y | (par->bgr << MEM_BGR));
+		break;
+	case 90:
+		write_reg(par, 0x16, MEM_X | MEM_Y | (par->bgr << MEM_BGR));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ *   VRP0 VRP1 VRP2 VRP3 VRP4 VRP5 PRP0 PRP1 PKP0 PKP1 PKP2 PKP3 PKP4 CGM
+ *   VRN0 VRN1 VRN2 VRN3 VRN4 VRN5 PRN0 PRN1 PKN0 PKN1 PKN2 PKN3 PKN4 CGM
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x7f, 0x7f, 0x1f, 0x1f,
+		0x1f, 0x1f, 0x1f, 0x0f,
+	};
+	int i, j;
+	int acc = 0;
+
+	/* apply mask */
+	for (i = 0; i < par->gamma.num_curves; i++)
+		for (j = 0; j < par->gamma.num_values; j++) {
+			acc += CURVE(i, j);
+			CURVE(i, j) &= mask[j];
+		}
+
+	if (acc == 0) /* skip if all values are zero */
+		return 0;
+
+	for (i = 0; i < par->gamma.num_curves; i++) {
+		write_reg(par, 0x40 + (i * 0x10), CURVE(i, 0));
+		write_reg(par, 0x41 + (i * 0x10), CURVE(i, 1));
+		write_reg(par, 0x42 + (i * 0x10), CURVE(i, 2));
+		write_reg(par, 0x43 + (i * 0x10), CURVE(i, 3));
+		write_reg(par, 0x44 + (i * 0x10), CURVE(i, 4));
+		write_reg(par, 0x45 + (i * 0x10), CURVE(i, 5));
+		write_reg(par, 0x46 + (i * 0x10), CURVE(i, 6));
+		write_reg(par, 0x47 + (i * 0x10), CURVE(i, 7));
+		write_reg(par, 0x48 + (i * 0x10), CURVE(i, 8));
+		write_reg(par, 0x49 + (i * 0x10), CURVE(i, 9));
+		write_reg(par, 0x4A + (i * 0x10), CURVE(i, 10));
+		write_reg(par, 0x4B + (i * 0x10), CURVE(i, 11));
+		write_reg(par, 0x4C + (i * 0x10), CURVE(i, 12));
+	}
+	write_reg(par, 0x5D, (CURVE(1, 0) << 4) | CURVE(0, 0));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 2,
+	.gamma_len = 14,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "himax,hx8347d", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:hx8347d");
+MODULE_ALIAS("platform:hx8347d");
+
+MODULE_DESCRIPTION("FB driver for the HX8347D LCD Controller");
+MODULE_AUTHOR("Christian Vogelgsang");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8353d.c b/drivers/staging/fbtft/fb_hx8353d.c
new file mode 100644
index 000000000..3e73b69b6
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8353d.c
@@ -0,0 +1,148 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the HX8353D LCD Controller
+ *
+ * Copyright (c) 2014 Petr Olivka
+ * Copyright (c) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME "fb_hx8353d"
+#define DEFAULT_GAMMA "50 77 40 08 BF 00 03 0F 00 01 73 00 72 03 B0 0F 08 00 0F"
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+	mdelay(150);
+
+	/* SETEXTC */
+	write_reg(par, 0xB9, 0xFF, 0x83, 0x53);
+
+	/* RADJ */
+	write_reg(par, 0xB0, 0x3C, 0x01);
+
+	/* VCOM */
+	write_reg(par, 0xB6, 0x94, 0x6C, 0x50);
+
+	/* PWR */
+	write_reg(par, 0xB1, 0x00, 0x01, 0x1B, 0x03, 0x01, 0x08, 0x77, 0x89);
+
+	/* COLMOD */
+	write_reg(par, 0x3A, 0x05);
+
+	/* MEM ACCESS */
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0xC0);
+
+	/* SLPOUT - Sleep out & booster on */
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+	mdelay(150);
+
+	/* DISPON - Display On */
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+
+	/* RGBSET */
+	write_reg(par, MIPI_DCS_WRITE_LUT,
+		  0,  2,  4,  6,  8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30,
+		32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62,
+		 0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15,
+		16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
+		32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47,
+		48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
+		 0,  2,  4,  6,  8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30,
+		32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62);
+
+	return 0;
+};
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* column address */
+	write_reg(par, 0x2a, xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
+
+	/* Row address */
+	write_reg(par, 0x2b, ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
+
+	/* memory write */
+	write_reg(par, 0x2c);
+}
+
+#define my BIT(7)
+#define mx BIT(6)
+#define mv BIT(5)
+static int set_var(struct fbtft_par *par)
+{
+	/*
+	 * madctl - memory data access control
+	 *   rgb/bgr:
+	 *   1. mode selection pin srgb
+	 *	rgb h/w pin for color filter setting: 0=rgb, 1=bgr
+	 *   2. madctl rgb bit
+	 *	rgb-bgr order color filter panel: 0=rgb, 1=bgr
+	 */
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  mx | my | (par->bgr << 3));
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  my | mv | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  par->bgr << 3);
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  mx | mv | (par->bgr << 3));
+		break;
+	}
+
+	return 0;
+}
+
+/* gamma string format: */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	write_reg(par, 0xE0,
+		  curves[0], curves[1], curves[2], curves[3],
+		  curves[4], curves[5], curves[6], curves[7],
+		  curves[8], curves[9], curves[10], curves[11],
+		  curves[12], curves[13], curves[14], curves[15],
+		  curves[16], curves[17], curves[18]);
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = 128,
+	.height = 160,
+	.gamma_num = 1,
+	.gamma_len = 19,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "himax,hx8353d", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:hx8353d");
+MODULE_ALIAS("platform:hx8353d");
+
+MODULE_DESCRIPTION("FB driver for the HX8353D LCD Controller");
+MODULE_AUTHOR("Petr Olivka");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8357d.c b/drivers/staging/fbtft/fb_hx8357d.c
new file mode 100644
index 000000000..94a357e8f
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8357d.c
@@ -0,0 +1,201 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the HX8357D LCD Controller
+ * Copyright (C) 2015 Adafruit Industries
+ *
+ * Based on the HX8347D FB driver
+ * Copyright (C) 2013 Christian Vogelgsang
+ *
+ * Based on driver code found here: https://github.com/watterott/r61505u-Adapter
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+#include "fb_hx8357d.h"
+
+#define DRVNAME		"fb_hx8357d"
+#define WIDTH		320
+#define HEIGHT		480
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Reset things like Gamma */
+	write_reg(par, MIPI_DCS_SOFT_RESET);
+	usleep_range(5000, 7000);
+
+	/* setextc */
+	write_reg(par, HX8357D_SETC, 0xFF, 0x83, 0x57);
+	msleep(150);
+
+	/* setRGB which also enables SDO */
+	write_reg(par, HX8357_SETRGB, 0x00, 0x00, 0x06, 0x06);
+
+	/* -1.52V */
+	write_reg(par, HX8357D_SETCOM, 0x25);
+
+	/* Normal mode 70Hz, Idle mode 55 Hz */
+	write_reg(par, HX8357_SETOSC, 0x68);
+
+	/* Set Panel - BGR, Gate direction swapped */
+	write_reg(par, HX8357_SETPANEL, 0x05);
+
+	write_reg(par, HX8357_SETPWR1,
+		  0x00,  /* Not deep standby */
+		  0x15,  /* BT */
+		  0x1C,  /* VSPR */
+		  0x1C,  /* VSNR */
+		  0x83,  /* AP */
+		  0xAA);  /* FS */
+
+	write_reg(par, HX8357D_SETSTBA,
+		  0x50,  /* OPON normal */
+		  0x50,  /* OPON idle */
+		  0x01,  /* STBA */
+		  0x3C,  /* STBA */
+		  0x1E,  /* STBA */
+		  0x08);  /* GEN */
+
+	write_reg(par, HX8357D_SETCYC,
+		  0x02,  /* NW 0x02 */
+		  0x40,  /* RTN */
+		  0x00,  /* DIV */
+		  0x2A,  /* DUM */
+		  0x2A,  /* DUM */
+		  0x0D,  /* GDON */
+		  0x78);  /* GDOFF */
+
+	write_reg(par, HX8357D_SETGAMMA,
+		  0x02,
+		  0x0A,
+		  0x11,
+		  0x1d,
+		  0x23,
+		  0x35,
+		  0x41,
+		  0x4b,
+		  0x4b,
+		  0x42,
+		  0x3A,
+		  0x27,
+		  0x1B,
+		  0x08,
+		  0x09,
+		  0x03,
+		  0x02,
+		  0x0A,
+		  0x11,
+		  0x1d,
+		  0x23,
+		  0x35,
+		  0x41,
+		  0x4b,
+		  0x4b,
+		  0x42,
+		  0x3A,
+		  0x27,
+		  0x1B,
+		  0x08,
+		  0x09,
+		  0x03,
+		  0x00,
+		  0x01);
+
+	/* 16 bit */
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, 0x55);
+
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0xC0);
+
+	/* TE off */
+	write_reg(par, MIPI_DCS_SET_TEAR_ON, 0x00);
+
+	/* tear line */
+	write_reg(par, MIPI_DCS_SET_TEAR_SCANLINE, 0x00, 0x02);
+
+	/* Exit Sleep */
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+	msleep(150);
+
+	/* display on */
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+	usleep_range(5000, 7000);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xff,  /* XSTART */
+		  xe >> 8, xe & 0xff); /* XEND */
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xff,  /* YSTART */
+		  ye >> 8, ye & 0xff); /* YEND */
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define HX8357D_MADCTL_MY  0x80
+#define HX8357D_MADCTL_MX  0x40
+#define HX8357D_MADCTL_MV  0x20
+#define HX8357D_MADCTL_ML  0x10
+#define HX8357D_MADCTL_RGB 0x00
+#define HX8357D_MADCTL_BGR 0x08
+#define HX8357D_MADCTL_MH  0x04
+static int set_var(struct fbtft_par *par)
+{
+	u8 val;
+
+	switch (par->info->var.rotate) {
+	case 270:
+		val = HX8357D_MADCTL_MV | HX8357D_MADCTL_MX;
+		break;
+	case 180:
+		val = 0;
+		break;
+	case 90:
+		val = HX8357D_MADCTL_MV | HX8357D_MADCTL_MY;
+		break;
+	default:
+		val = HX8357D_MADCTL_MX | HX8357D_MADCTL_MY;
+		break;
+	}
+
+	val |= (par->bgr ? HX8357D_MADCTL_RGB : HX8357D_MADCTL_BGR);
+
+	/* Memory Access Control */
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, val);
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 2,
+	.gamma_len = 14,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "himax,hx8357d", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:hx8357d");
+MODULE_ALIAS("platform:hx8357d");
+
+MODULE_DESCRIPTION("FB driver for the HX8357D LCD Controller");
+MODULE_AUTHOR("Sean Cross <xobs@kosagi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8357d.h b/drivers/staging/fbtft/fb_hx8357d.h
new file mode 100644
index 000000000..6180b093f
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8357d.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: MIT */
+/*
+ * This is our library for the Adafruit  ILI9341 Breakout and Shield
+ * ----> http://www.adafruit.com/products/1651
+ *
+ * Check out the links above for our tutorials and wiring diagrams
+ * These displays use SPI to communicate, 4 or 5 pins are required to
+ * interface (RST is optional)
+ * Adafruit invests time and resources providing this open source code,
+ * please support Adafruit and open-source hardware by purchasing
+ * products from Adafruit!
+ *
+ * Written by Limor Fried/Ladyada for Adafruit Industries.
+ * MIT license, all text above must be included in any redistribution
+ */
+
+#ifndef __HX8357_H__
+#define __HX8357_H__
+
+#define HX8357D 0xD
+#define HX8357B 0xB
+
+#define HX8357_TFTWIDTH  320
+#define HX8357_TFTHEIGHT 480
+
+#define HX8357_SETOSC 0xB0
+#define HX8357_SETPWR1 0xB1
+#define HX8357B_SETDISPLAY 0xB2
+#define HX8357_SETRGB 0xB3
+#define HX8357D_SETCOM  0xB6
+
+#define HX8357B_SETDISPMODE  0xB4
+#define HX8357D_SETCYC  0xB4
+#define HX8357B_SETOTP 0xB7
+#define HX8357D_SETC 0xB9
+
+#define HX8357B_SET_PANEL_DRIVING 0xC0
+#define HX8357D_SETSTBA 0xC0
+#define HX8357B_SETDGC  0xC1
+#define HX8357B_SETID  0xC3
+#define HX8357B_SETDDB  0xC4
+#define HX8357B_SETDISPLAYFRAME 0xC5
+#define HX8357B_GAMMASET 0xC8
+#define HX8357B_SETCABC  0xC9
+#define HX8357_SETPANEL  0xCC
+
+#define HX8357B_SETPOWER 0xD0
+#define HX8357B_SETVCOM 0xD1
+#define HX8357B_SETPWRNORMAL 0xD2
+
+#define HX8357B_RDID1   0xDA
+#define HX8357B_RDID2   0xDB
+#define HX8357B_RDID3   0xDC
+#define HX8357B_RDID4   0xDD
+
+#define HX8357D_SETGAMMA 0xE0
+
+#define HX8357B_SETGAMMA 0xC8
+#define HX8357B_SETPANELRELATED  0xE9
+
+/* Color definitions */
+#define	HX8357_BLACK   0x0000
+#define	HX8357_BLUE    0x001F
+#define	HX8357_RED     0xF800
+#define	HX8357_GREEN   0x07E0
+#define HX8357_CYAN    0x07FF
+#define HX8357_MAGENTA 0xF81F
+#define HX8357_YELLOW  0xFFE0
+#define HX8357_WHITE   0xFFFF
+
+#endif /* __HX8357_H__ */
diff --git a/drivers/staging/fbtft/fb_ili9163.c b/drivers/staging/fbtft/fb_ili9163.c
new file mode 100644
index 000000000..6582a2c90
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9163.c
@@ -0,0 +1,260 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9163 LCD Controller
+ *
+ * Copyright (C) 2015 Kozhevnikov Anatoly
+ *
+ * Based on ili9325.c by Noralf Tronnes and
+ * .S.U.M.O.T.O.Y. by Max MC Costa (https://github.com/sumotoy/TFT_ILI9163C).
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9163"
+#define WIDTH		128
+#define HEIGHT		128
+#define BPP		16
+#define FPS		30
+
+#ifdef GAMMA_ADJ
+#define GAMMA_LEN	15
+#define GAMMA_NUM	1
+#define DEFAULT_GAMMA	"36 29 12 22 1C 15 42 B7 2F 13 12 0A 11 0B 06\n"
+#endif
+
+/* ILI9163C commands */
+#define CMD_FRMCTR1	0xB1 /* Frame Rate Control */
+			     /*	(In normal mode/Full colors) */
+#define CMD_FRMCTR2	0xB2 /* Frame Rate Control (In Idle mode/8-colors) */
+#define CMD_FRMCTR3	0xB3 /* Frame Rate Control */
+			     /*	(In Partial mode/full colors) */
+#define CMD_DINVCTR	0xB4 /* Display Inversion Control */
+#define CMD_RGBBLK	0xB5 /* RGB Interface Blanking Porch setting */
+#define CMD_DFUNCTR	0xB6 /* Display Function set 5 */
+#define CMD_SDRVDIR	0xB7 /* Source Driver Direction Control */
+#define CMD_GDRVDIR	0xB8 /* Gate Driver Direction Control  */
+
+#define CMD_PWCTR1	0xC0 /* Power_Control1 */
+#define CMD_PWCTR2	0xC1 /* Power_Control2 */
+#define CMD_PWCTR3	0xC2 /* Power_Control3 */
+#define CMD_PWCTR4	0xC3 /* Power_Control4 */
+#define CMD_PWCTR5	0xC4 /* Power_Control5 */
+#define CMD_VCOMCTR1	0xC5 /* VCOM_Control 1 */
+#define CMD_VCOMCTR2	0xC6 /* VCOM_Control 2 */
+#define CMD_VCOMOFFS	0xC7 /* VCOM Offset Control */
+#define CMD_PGAMMAC	0xE0 /* Positive Gamma Correction Setting */
+#define CMD_NGAMMAC	0xE1 /* Negative Gamma Correction Setting */
+#define CMD_GAMRSEL	0xF2 /* GAM_R_SEL */
+
+/*
+ * This display:
+ * http://www.ebay.com/itm/Replace-Nokia-5110-LCD-1-44-Red-Serial-128X128-SPI-
+ * Color-TFT-LCD-Display-Module-/271422122271
+ * This particular display has a design error! The controller has 3 pins to
+ * configure to constrain the memory and resolution to a fixed dimension (in
+ * that case 128x128) but they leaved those pins configured for 128x160 so
+ * there was several pixel memory addressing problems.
+ * I solved by setup several parameters that dinamically fix the resolution as
+ * needit so below the parameters for this display. If you have a strain or a
+ * correct display (can happen with chinese) you can copy those parameters and
+ * create setup for different displays.
+ */
+
+#ifdef RED
+#define __OFFSET		32 /*see note 2 - this is the red version */
+#else
+#define __OFFSET		0  /*see note 2 - this is the black version */
+#endif
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, MIPI_DCS_SOFT_RESET); /* software reset */
+	mdelay(500);
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE); /* exit sleep */
+	mdelay(5);
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, MIPI_DCS_PIXEL_FMT_16BIT);
+	/* default gamma curve 3 */
+	write_reg(par, MIPI_DCS_SET_GAMMA_CURVE, 0x02);
+#ifdef GAMMA_ADJ
+	write_reg(par, CMD_GAMRSEL, 0x01); /* Enable Gamma adj */
+#endif
+	write_reg(par, MIPI_DCS_ENTER_NORMAL_MODE);
+	write_reg(par, CMD_DFUNCTR, 0xff, 0x06);
+	/* Frame Rate Control (In normal mode/Full colors) */
+	write_reg(par, CMD_FRMCTR1, 0x08, 0x02);
+	write_reg(par, CMD_DINVCTR, 0x07); /* display inversion  */
+	/* Set VRH1[4:0] & VC[2:0] for VCI1 & GVDD */
+	write_reg(par, CMD_PWCTR1, 0x0A, 0x02);
+	/* Set BT[2:0] for AVDD & VCL & VGH & VGL  */
+	write_reg(par, CMD_PWCTR2, 0x02);
+	/* Set VMH[6:0] & VML[6:0] for VOMH & VCOML */
+	write_reg(par, CMD_VCOMCTR1, 0x50, 0x63);
+	write_reg(par, CMD_VCOMOFFS, 0);
+
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS, 0, 0, 0, WIDTH);
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS, 0, 0, 0, HEIGHT);
+
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON); /* display ON */
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START); /* Memory Write */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys,
+			 int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+			  xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
+		write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+			  (ys + __OFFSET) >> 8, (ys + __OFFSET) & 0xff,
+			  (ye + __OFFSET) >> 8, (ye + __OFFSET) & 0xff);
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+			  (xs + __OFFSET) >> 8, (xs + __OFFSET) & 0xff,
+			  (xe + __OFFSET) >> 8, (xe + __OFFSET) & 0xff);
+		write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+			  ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
+		break;
+	case 180:
+	case 270:
+		write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+			  xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
+		write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+			  ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
+		break;
+	default:
+		/* Fix incorrect setting */
+		par->info->var.rotate = 0;
+	}
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+/*
+ * 7) MY:  1(bottom to top),	0(top to bottom)    Row Address Order
+ * 6) MX:  1(R to L),		0(L to R)	    Column Address Order
+ * 5) MV:  1(Exchanged),	0(normal)	    Row/Column exchange
+ * 4) ML:  1(bottom to top),	0(top to bottom)    Vertical Refresh Order
+ * 3) RGB: 1(BGR),		0(RGB)		    Color Space
+ * 2) MH:  1(R to L),		0(L to R)	    Horizontal Refresh Order
+ * 1)
+ * 0)
+ *
+ *	MY, MX, MV, ML,RGB, MH, D1, D0
+ *	0 | 0 | 0 | 0 | 1 | 0 | 0 | 0	//normal
+ *	1 | 0 | 0 | 0 | 1 | 0 | 0 | 0	//Y-Mirror
+ *	0 | 1 | 0 | 0 | 1 | 0 | 0 | 0	//X-Mirror
+ *	1 | 1 | 0 | 0 | 1 | 0 | 0 | 0	//X-Y-Mirror
+ *	0 | 0 | 1 | 0 | 1 | 0 | 0 | 0	//X-Y Exchange
+ *	1 | 0 | 1 | 0 | 1 | 0 | 0 | 0	//X-Y Exchange, Y-Mirror
+ *	0 | 1 | 1 | 0 | 1 | 0 | 0 | 0	//XY exchange
+ *	1 | 1 | 1 | 0 | 1 | 0 | 0 | 0
+ */
+static int set_var(struct fbtft_par *par)
+{
+	u8 mactrl_data = 0; /* Avoid compiler warning */
+
+	switch (par->info->var.rotate) {
+	case 0:
+		mactrl_data = 0x08;
+		break;
+	case 180:
+		mactrl_data = 0xC8;
+		break;
+	case 270:
+		mactrl_data = 0xA8;
+		break;
+	case 90:
+		mactrl_data = 0x68;
+		break;
+	}
+
+	/* Colorspcae */
+	if (par->bgr)
+		mactrl_data |= BIT(2);
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, mactrl_data);
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+	return 0;
+}
+
+#ifdef GAMMA_ADJ
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int gamma_adj(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+		0x1f, 0x3f, 0x0f, 0x0f, 0x7f, 0x1f,
+		0x3F, 0x3F, 0x3F, 0x3F, 0x3F};
+	int i, j;
+
+	for (i = 0; i < GAMMA_NUM; i++)
+		for (j = 0; j < GAMMA_LEN; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	write_reg(par, CMD_PGAMMAC,
+		  CURVE(0, 0),
+		  CURVE(0, 1),
+		  CURVE(0, 2),
+		  CURVE(0, 3),
+		  CURVE(0, 4),
+		  CURVE(0, 5),
+		  CURVE(0, 6),
+		  (CURVE(0, 7) << 4) | CURVE(0, 8),
+		  CURVE(0, 9),
+		  CURVE(0, 10),
+		  CURVE(0, 11),
+		  CURVE(0, 12),
+		  CURVE(0, 13),
+		  CURVE(0, 14),
+		  CURVE(0, 15));
+
+	/* Write Data to GRAM mode */
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+
+	return 0;
+}
+
+#undef CURVE
+#endif
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fps = FPS,
+#ifdef GAMMA_ADJ
+	.gamma_num = GAMMA_NUM,
+	.gamma_len = GAMMA_LEN,
+	.gamma = DEFAULT_GAMMA,
+#endif
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+#ifdef GAMMA_ADJ
+		.set_gamma = gamma_adj,
+#endif
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9163", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9163");
+MODULE_ALIAS("platform:ili9163");
+
+MODULE_DESCRIPTION("FB driver for the ILI9163 LCD Controller");
+MODULE_AUTHOR("Kozhevnikov Anatoly");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9320.c b/drivers/staging/fbtft/fb_ili9320.c
new file mode 100644
index 000000000..0be7c2d51
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9320.c
@@ -0,0 +1,267 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9320 LCD Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9320"
+#define WIDTH		240
+#define HEIGHT		320
+#define DEFAULT_GAMMA	"07 07 6 0 0 0 5 5 4 0\n" \
+			"07 08 4 7 5 1 2 0 7 7"
+
+static unsigned int read_devicecode(struct fbtft_par *par)
+{
+	u8 rxbuf[8] = {0, };
+
+	write_reg(par, 0x0000);
+	par->fbtftops.read(par, rxbuf, 4);
+	return (rxbuf[2] << 8) | rxbuf[3];
+}
+
+static int init_display(struct fbtft_par *par)
+{
+	unsigned int devcode;
+
+	par->fbtftops.reset(par);
+
+	devcode = read_devicecode(par);
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "Device code: 0x%04X\n",
+		      devcode);
+	if ((devcode != 0x0000) && (devcode != 0x9320))
+		dev_warn(par->info->device,
+			 "Unrecognized Device code: 0x%04X (expected 0x9320)\n",
+			devcode);
+
+	/* Initialization sequence from ILI9320 Application Notes */
+
+	/* *********** Start Initial Sequence ********* */
+	/* Set the Vcore voltage and this setting is must. */
+	write_reg(par, 0x00E5, 0x8000);
+
+	/* Start internal OSC. */
+	write_reg(par, 0x0000, 0x0001);
+
+	/* set SS and SM bit */
+	write_reg(par, 0x0001, 0x0100);
+
+	/* set 1 line inversion */
+	write_reg(par, 0x0002, 0x0700);
+
+	/* Resize register */
+	write_reg(par, 0x0004, 0x0000);
+
+	/* set the back and front porch */
+	write_reg(par, 0x0008, 0x0202);
+
+	/* set non-display area refresh cycle */
+	write_reg(par, 0x0009, 0x0000);
+
+	/* FMARK function */
+	write_reg(par, 0x000A, 0x0000);
+
+	/* RGB interface setting */
+	write_reg(par, 0x000C, 0x0000);
+
+	/* Frame marker Position */
+	write_reg(par, 0x000D, 0x0000);
+
+	/* RGB interface polarity */
+	write_reg(par, 0x000F, 0x0000);
+
+	/* ***********Power On sequence *************** */
+	/* SAP, BT[3:0], AP, DSTB, SLP, STB */
+	write_reg(par, 0x0010, 0x0000);
+
+	/* DC1[2:0], DC0[2:0], VC[2:0] */
+	write_reg(par, 0x0011, 0x0007);
+
+	/* VREG1OUT voltage */
+	write_reg(par, 0x0012, 0x0000);
+
+	/* VDV[4:0] for VCOM amplitude */
+	write_reg(par, 0x0013, 0x0000);
+
+	/* Dis-charge capacitor power voltage */
+	mdelay(200);
+
+	/* SAP, BT[3:0], AP, DSTB, SLP, STB */
+	write_reg(par, 0x0010, 0x17B0);
+
+	/* R11h=0x0031 at VCI=3.3V DC1[2:0], DC0[2:0], VC[2:0] */
+	write_reg(par, 0x0011, 0x0031);
+	mdelay(50);
+
+	/* R12h=0x0138 at VCI=3.3V VREG1OUT voltage */
+	write_reg(par, 0x0012, 0x0138);
+	mdelay(50);
+
+	/* R13h=0x1800 at VCI=3.3V VDV[4:0] for VCOM amplitude */
+	write_reg(par, 0x0013, 0x1800);
+
+	/* R29h=0x0008 at VCI=3.3V VCM[4:0] for VCOMH */
+	write_reg(par, 0x0029, 0x0008);
+	mdelay(50);
+
+	/* GRAM horizontal Address */
+	write_reg(par, 0x0020, 0x0000);
+
+	/* GRAM Vertical Address */
+	write_reg(par, 0x0021, 0x0000);
+
+	/* ------------------ Set GRAM area --------------- */
+	/* Horizontal GRAM Start Address */
+	write_reg(par, 0x0050, 0x0000);
+
+	/* Horizontal GRAM End Address */
+	write_reg(par, 0x0051, 0x00EF);
+
+	/* Vertical GRAM Start Address */
+	write_reg(par, 0x0052, 0x0000);
+
+	/* Vertical GRAM End Address */
+	write_reg(par, 0x0053, 0x013F);
+
+	/* Gate Scan Line */
+	write_reg(par, 0x0060, 0x2700);
+
+	/* NDL,VLE, REV */
+	write_reg(par, 0x0061, 0x0001);
+
+	/* set scrolling line */
+	write_reg(par, 0x006A, 0x0000);
+
+	/* -------------- Partial Display Control --------- */
+	write_reg(par, 0x0080, 0x0000);
+	write_reg(par, 0x0081, 0x0000);
+	write_reg(par, 0x0082, 0x0000);
+	write_reg(par, 0x0083, 0x0000);
+	write_reg(par, 0x0084, 0x0000);
+	write_reg(par, 0x0085, 0x0000);
+
+	/* -------------- Panel Control ------------------- */
+	write_reg(par, 0x0090, 0x0010);
+	write_reg(par, 0x0092, 0x0000);
+	write_reg(par, 0x0093, 0x0003);
+	write_reg(par, 0x0095, 0x0110);
+	write_reg(par, 0x0097, 0x0000);
+	write_reg(par, 0x0098, 0x0000);
+	write_reg(par, 0x0007, 0x0173); /* 262K color and display ON */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R20h = Horizontal GRAM Start Address */
+	/* R21h = Vertical GRAM Start Address */
+	case 0:
+		write_reg(par, 0x0020, xs);
+		write_reg(par, 0x0021, ys);
+		break;
+	case 180:
+		write_reg(par, 0x0020, WIDTH - 1 - xs);
+		write_reg(par, 0x0021, HEIGHT - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x0020, WIDTH - 1 - ys);
+		write_reg(par, 0x0021, xs);
+		break;
+	case 90:
+		write_reg(par, 0x0020, ys);
+		write_reg(par, 0x0021, HEIGHT - 1 - xs);
+		break;
+	}
+	write_reg(par, 0x0022); /* Write Data to GRAM */
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, 0x3, (par->bgr << 12) | 0x30);
+		break;
+	case 270:
+		write_reg(par, 0x3, (par->bgr << 12) | 0x28);
+		break;
+	case 180:
+		write_reg(par, 0x3, (par->bgr << 12) | 0x00);
+		break;
+	case 90:
+		write_reg(par, 0x3, (par->bgr << 12) | 0x18);
+		break;
+	}
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ *  VRP0 VRP1 RP0 RP1 KP0 KP1 KP2 KP3 KP4 KP5
+ *  VRN0 VRN1 RN0 RN1 KN0 KN1 KN2 KN3 KN4 KN5
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < 2; i++)
+		for (j = 0; j < 10; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	write_reg(par, 0x0030, CURVE(0, 5) << 8 | CURVE(0, 4));
+	write_reg(par, 0x0031, CURVE(0, 7) << 8 | CURVE(0, 6));
+	write_reg(par, 0x0032, CURVE(0, 9) << 8 | CURVE(0, 8));
+	write_reg(par, 0x0035, CURVE(0, 3) << 8 | CURVE(0, 2));
+	write_reg(par, 0x0036, CURVE(0, 1) << 8 | CURVE(0, 0));
+
+	write_reg(par, 0x0037, CURVE(1, 5) << 8 | CURVE(1, 4));
+	write_reg(par, 0x0038, CURVE(1, 7) << 8 | CURVE(1, 6));
+	write_reg(par, 0x0039, CURVE(1, 9) << 8 | CURVE(1, 8));
+	write_reg(par, 0x003C, CURVE(1, 3) << 8 | CURVE(1, 2));
+	write_reg(par, 0x003D, CURVE(1, 1) << 8 | CURVE(1, 0));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 2,
+	.gamma_len = 10,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9320", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9320");
+MODULE_ALIAS("platform:ili9320");
+
+MODULE_DESCRIPTION("FB driver for the ILI9320 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9325.c b/drivers/staging/fbtft/fb_ili9325.c
new file mode 100644
index 000000000..16d3b17ca
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9325.c
@@ -0,0 +1,261 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9325 LCD Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ *
+ * Based on ili9325.c by Jeroen Domburg
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9325"
+#define WIDTH		240
+#define HEIGHT		320
+#define BPP		16
+#define FPS		20
+#define DEFAULT_GAMMA	"0F 00 7 2 0 0 6 5 4 1\n" \
+			"04 16 2 7 6 3 2 1 7 7"
+
+static unsigned int bt = 6; /* VGL=Vci*4 , VGH=Vci*4 */
+module_param(bt, uint, 0000);
+MODULE_PARM_DESC(bt, "Sets the factor used in the step-up circuits");
+
+static unsigned int vc = 0x03; /* Vci1=Vci*0.80 */
+module_param(vc, uint, 0000);
+MODULE_PARM_DESC(vc, "Sets the ratio factor of Vci to generate the reference voltages Vci1");
+
+static unsigned int vrh = 0x0d; /* VREG1OUT=Vci*1.85 */
+module_param(vrh, uint, 0000);
+MODULE_PARM_DESC(vrh, "Set the amplifying rate (1.6 ~ 1.9) of Vci applied to output the VREG1OUT");
+
+static unsigned int vdv = 0x12; /* VCOMH amplitude=VREG1OUT*0.98 */
+module_param(vdv, uint, 0000);
+MODULE_PARM_DESC(vdv, "Select the factor of VREG1OUT to set the amplitude of Vcom");
+
+static unsigned int vcm = 0x0a; /* VCOMH=VREG1OUT*0.735 */
+module_param(vcm, uint, 0000);
+MODULE_PARM_DESC(vcm, "Set the internal VcomH voltage");
+
+/*
+ * Verify that this configuration is within the Voltage limits
+ *
+ * Display module configuration: Vcc = IOVcc = Vci = 3.3V
+ *
+ * Voltages
+ * ----------
+ * Vci                                =   3.3
+ * Vci1           =  Vci * 0.80       =   2.64
+ * DDVDH          =  Vci1 * 2         =   5.28
+ * VCL            = -Vci1             =  -2.64
+ * VREG1OUT       =  Vci * 1.85       =   4.88
+ * VCOMH          =  VREG1OUT * 0.735 =   3.59
+ * VCOM amplitude =  VREG1OUT * 0.98  =   4.79
+ * VGH            =  Vci * 4          =  13.2
+ * VGL            = -Vci * 4          = -13.2
+ *
+ * Limits
+ * --------
+ * Power supplies
+ * 1.65 < IOVcc < 3.30   =>  1.65 < 3.3 < 3.30
+ * 2.40 < Vcc   < 3.30   =>  2.40 < 3.3 < 3.30
+ * 2.50 < Vci   < 3.30   =>  2.50 < 3.3 < 3.30
+ *
+ * Source/VCOM power supply voltage
+ *  4.50 < DDVDH < 6.0   =>  4.50 <  5.28 <  6.0
+ * -3.0  < VCL   < -2.0  =>  -3.0 < -2.64 < -2.0
+ * VCI - VCL < 6.0       =>  5.94 < 6.0
+ *
+ * Gate driver output voltage
+ *  10  < VGH   < 20     =>   10 <  13.2  < 20
+ * -15  < VGL   < -5     =>  -15 < -13.2  < -5
+ * VGH - VGL < 32        =>   26.4 < 32
+ *
+ * VCOM driver output voltage
+ * VCOMH - VCOML < 6.0   =>  4.79 < 6.0
+ */
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	bt &= 0x07;
+	vc &= 0x07;
+	vrh &= 0x0f;
+	vdv &= 0x1f;
+	vcm &= 0x3f;
+
+	/* Initialization sequence from ILI9325 Application Notes */
+
+	/* ----------- Start Initial Sequence ----------- */
+	write_reg(par, 0x00E3, 0x3008); /* Set internal timing */
+	write_reg(par, 0x00E7, 0x0012); /* Set internal timing */
+	write_reg(par, 0x00EF, 0x1231); /* Set internal timing */
+	write_reg(par, 0x0001, 0x0100); /* set SS and SM bit */
+	write_reg(par, 0x0002, 0x0700); /* set 1 line inversion */
+	write_reg(par, 0x0004, 0x0000); /* Resize register */
+	write_reg(par, 0x0008, 0x0207); /* set the back porch and front porch */
+	write_reg(par, 0x0009, 0x0000); /* set non-display area refresh cycle */
+	write_reg(par, 0x000A, 0x0000); /* FMARK function */
+	write_reg(par, 0x000C, 0x0000); /* RGB interface setting */
+	write_reg(par, 0x000D, 0x0000); /* Frame marker Position */
+	write_reg(par, 0x000F, 0x0000); /* RGB interface polarity */
+
+	/* ----------- Power On sequence ----------- */
+	write_reg(par, 0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
+	write_reg(par, 0x0011, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */
+	write_reg(par, 0x0012, 0x0000); /* VREG1OUT voltage */
+	write_reg(par, 0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
+	mdelay(200); /* Dis-charge capacitor power voltage */
+	write_reg(par, 0x0010, /* SAP, BT[3:0], AP, DSTB, SLP, STB */
+		BIT(12) | (bt << 8) | BIT(7) | BIT(4));
+	write_reg(par, 0x0011, 0x220 | vc); /* DC1[2:0], DC0[2:0], VC[2:0] */
+	mdelay(50); /* Delay 50ms */
+	write_reg(par, 0x0012, vrh); /* Internal reference voltage= Vci; */
+	mdelay(50); /* Delay 50ms */
+	write_reg(par, 0x0013, vdv << 8); /* Set VDV[4:0] for VCOM amplitude */
+	write_reg(par, 0x0029, vcm); /* Set VCM[5:0] for VCOMH */
+	write_reg(par, 0x002B, 0x000C); /* Set Frame Rate */
+	mdelay(50); /* Delay 50ms */
+	write_reg(par, 0x0020, 0x0000); /* GRAM horizontal Address */
+	write_reg(par, 0x0021, 0x0000); /* GRAM Vertical Address */
+
+	/*------------------ Set GRAM area --------------- */
+	write_reg(par, 0x0050, 0x0000); /* Horizontal GRAM Start Address */
+	write_reg(par, 0x0051, 0x00EF); /* Horizontal GRAM End Address */
+	write_reg(par, 0x0052, 0x0000); /* Vertical GRAM Start Address */
+	write_reg(par, 0x0053, 0x013F); /* Vertical GRAM Start Address */
+	write_reg(par, 0x0060, 0xA700); /* Gate Scan Line */
+	write_reg(par, 0x0061, 0x0001); /* NDL,VLE, REV */
+	write_reg(par, 0x006A, 0x0000); /* set scrolling line */
+
+	/*-------------- Partial Display Control --------- */
+	write_reg(par, 0x0080, 0x0000);
+	write_reg(par, 0x0081, 0x0000);
+	write_reg(par, 0x0082, 0x0000);
+	write_reg(par, 0x0083, 0x0000);
+	write_reg(par, 0x0084, 0x0000);
+	write_reg(par, 0x0085, 0x0000);
+
+	/*-------------- Panel Control ------------------- */
+	write_reg(par, 0x0090, 0x0010);
+	write_reg(par, 0x0092, 0x0600);
+	write_reg(par, 0x0007, 0x0133); /* 262K color and display ON */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R20h = Horizontal GRAM Start Address */
+	/* R21h = Vertical GRAM Start Address */
+	case 0:
+		write_reg(par, 0x0020, xs);
+		write_reg(par, 0x0021, ys);
+		break;
+	case 180:
+		write_reg(par, 0x0020, WIDTH - 1 - xs);
+		write_reg(par, 0x0021, HEIGHT - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x0020, WIDTH - 1 - ys);
+		write_reg(par, 0x0021, xs);
+		break;
+	case 90:
+		write_reg(par, 0x0020, ys);
+		write_reg(par, 0x0021, HEIGHT - 1 - xs);
+		break;
+	}
+	write_reg(par, 0x0022); /* Write Data to GRAM */
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	/* AM: GRAM update direction */
+	case 0:
+		write_reg(par, 0x03, 0x0030 | (par->bgr << 12));
+		break;
+	case 180:
+		write_reg(par, 0x03, 0x0000 | (par->bgr << 12));
+		break;
+	case 270:
+		write_reg(par, 0x03, 0x0028 | (par->bgr << 12));
+		break;
+	case 90:
+		write_reg(par, 0x03, 0x0018 | (par->bgr << 12));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ *  VRP0 VRP1 RP0 RP1 KP0 KP1 KP2 KP3 KP4 KP5
+ *  VRN0 VRN1 RN0 RN1 KN0 KN1 KN2 KN3 KN4 KN5
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < 2; i++)
+		for (j = 0; j < 10; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	write_reg(par, 0x0030, CURVE(0, 5) << 8 | CURVE(0, 4));
+	write_reg(par, 0x0031, CURVE(0, 7) << 8 | CURVE(0, 6));
+	write_reg(par, 0x0032, CURVE(0, 9) << 8 | CURVE(0, 8));
+	write_reg(par, 0x0035, CURVE(0, 3) << 8 | CURVE(0, 2));
+	write_reg(par, 0x0036, CURVE(0, 1) << 8 | CURVE(0, 0));
+
+	write_reg(par, 0x0037, CURVE(1, 5) << 8 | CURVE(1, 4));
+	write_reg(par, 0x0038, CURVE(1, 7) << 8 | CURVE(1, 6));
+	write_reg(par, 0x0039, CURVE(1, 9) << 8 | CURVE(1, 8));
+	write_reg(par, 0x003C, CURVE(1, 3) << 8 | CURVE(1, 2));
+	write_reg(par, 0x003D, CURVE(1, 1) << 8 | CURVE(1, 0));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fps = FPS,
+	.gamma_num = 2,
+	.gamma_len = 10,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9325", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9325");
+MODULE_ALIAS("platform:ili9325");
+
+MODULE_DESCRIPTION("FB driver for the ILI9325 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9340.c b/drivers/staging/fbtft/fb_ili9340.c
new file mode 100644
index 000000000..704236bca
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9340.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9340 LCD Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9340"
+#define WIDTH		240
+#define HEIGHT		320
+
+/* Init sequence taken from: Arduino Library for the Adafruit 2.2" display */
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0xEF, 0x03, 0x80, 0x02);
+	write_reg(par, 0xCF, 0x00, 0XC1, 0X30);
+	write_reg(par, 0xED, 0x64, 0x03, 0X12, 0X81);
+	write_reg(par, 0xE8, 0x85, 0x00, 0x78);
+	write_reg(par, 0xCB, 0x39, 0x2C, 0x00, 0x34, 0x02);
+	write_reg(par, 0xF7, 0x20);
+	write_reg(par, 0xEA, 0x00, 0x00);
+
+	/* Power Control 1 */
+	write_reg(par, 0xC0, 0x23);
+
+	/* Power Control 2 */
+	write_reg(par, 0xC1, 0x10);
+
+	/* VCOM Control 1 */
+	write_reg(par, 0xC5, 0x3e, 0x28);
+
+	/* VCOM Control 2 */
+	write_reg(par, 0xC7, 0x86);
+
+	/* COLMOD: Pixel Format Set */
+	/* 16 bits/pixel */
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, 0x55);
+
+	/* Frame Rate Control */
+	/* Division ratio = fosc, Frame Rate = 79Hz */
+	write_reg(par, 0xB1, 0x00, 0x18);
+
+	/* Display Function Control */
+	write_reg(par, 0xB6, 0x08, 0x82, 0x27);
+
+	/* Gamma Function Disable */
+	write_reg(par, 0xF2, 0x00);
+
+	/* Gamma curve selection */
+	write_reg(par, MIPI_DCS_SET_GAMMA_CURVE, 0x01);
+
+	/* Positive Gamma Correction */
+	write_reg(par, 0xE0,
+		  0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E, 0xF1,
+		  0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00);
+
+	/* Negative Gamma Correction */
+	write_reg(par, 0xE1,
+		  0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31, 0xC1,
+		  0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F);
+
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+
+	mdelay(120);
+
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define ILI9340_MADCTL_MV  0x20
+#define ILI9340_MADCTL_MX  0x40
+#define ILI9340_MADCTL_MY  0x80
+static int set_var(struct fbtft_par *par)
+{
+	u8 val;
+
+	switch (par->info->var.rotate) {
+	case 270:
+		val = ILI9340_MADCTL_MV;
+		break;
+	case 180:
+		val = ILI9340_MADCTL_MY;
+		break;
+	case 90:
+		val = ILI9340_MADCTL_MV | ILI9340_MADCTL_MY | ILI9340_MADCTL_MX;
+		break;
+	default:
+		val = ILI9340_MADCTL_MX;
+		break;
+	}
+	/* Memory Access Control  */
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, val | (par->bgr << 3));
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9340", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9340");
+MODULE_ALIAS("platform:ili9340");
+
+MODULE_DESCRIPTION("FB driver for the ILI9340 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9341.c b/drivers/staging/fbtft/fb_ili9341.c
new file mode 100644
index 000000000..47e72b87d
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9341.c
@@ -0,0 +1,157 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9341 LCD display controller
+ *
+ * This display uses 9-bit SPI: Data/Command bit + 8 data bits
+ * For platforms that doesn't support 9-bit, the driver is capable
+ * of emulating this using 8-bit transfer.
+ * This is done by transferring eight 9-bit words in 9 bytes.
+ *
+ * Copyright (C) 2013 Christian Vogelgsang
+ * Based on adafruit22fb.c by Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9341"
+#define WIDTH		240
+#define HEIGHT		320
+#define TXBUFLEN	(4 * PAGE_SIZE)
+#define DEFAULT_GAMMA	"1F 1A 18 0A 0F 06 45 87 32 0A 07 02 07 05 00\n" \
+			"00 25 27 05 10 09 3A 78 4D 05 18 0D 38 3A 1F"
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* startup sequence for MI0283QT-9A */
+	write_reg(par, MIPI_DCS_SOFT_RESET);
+	mdelay(5);
+	write_reg(par, MIPI_DCS_SET_DISPLAY_OFF);
+	/* --------------------------------------------------------- */
+	write_reg(par, 0xCF, 0x00, 0x83, 0x30);
+	write_reg(par, 0xED, 0x64, 0x03, 0x12, 0x81);
+	write_reg(par, 0xE8, 0x85, 0x01, 0x79);
+	write_reg(par, 0xCB, 0x39, 0X2C, 0x00, 0x34, 0x02);
+	write_reg(par, 0xF7, 0x20);
+	write_reg(par, 0xEA, 0x00, 0x00);
+	/* ------------power control-------------------------------- */
+	write_reg(par, 0xC0, 0x26);
+	write_reg(par, 0xC1, 0x11);
+	/* ------------VCOM --------- */
+	write_reg(par, 0xC5, 0x35, 0x3E);
+	write_reg(par, 0xC7, 0xBE);
+	/* ------------memory access control------------------------ */
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, 0x55); /* 16bit pixel */
+	/* ------------frame rate----------------------------------- */
+	write_reg(par, 0xB1, 0x00, 0x1B);
+	/* ------------Gamma---------------------------------------- */
+	/* write_reg(par, 0xF2, 0x08); */ /* Gamma Function Disable */
+	write_reg(par, MIPI_DCS_SET_GAMMA_CURVE, 0x01);
+	/* ------------display-------------------------------------- */
+	write_reg(par, 0xB7, 0x07); /* entry mode set */
+	write_reg(par, 0xB6, 0x0A, 0x82, 0x27, 0x00);
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+	mdelay(100);
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+	mdelay(20);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  (xs >> 8) & 0xFF, xs & 0xFF, (xe >> 8) & 0xFF, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  (ys >> 8) & 0xFF, ys & 0xFF, (ye >> 8) & 0xFF, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define MEM_Y   BIT(7) /* MY row address order */
+#define MEM_X   BIT(6) /* MX column address order */
+#define MEM_V   BIT(5) /* MV row / column exchange */
+#define MEM_L   BIT(4) /* ML vertical refresh order */
+#define MEM_H   BIT(2) /* MH horizontal refresh order */
+#define MEM_BGR (3) /* RGB-BGR Order */
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MEM_X | (par->bgr << MEM_BGR));
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MEM_V | MEM_L | (par->bgr << MEM_BGR));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MEM_Y | (par->bgr << MEM_BGR));
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MEM_Y | MEM_X | MEM_V | (par->bgr << MEM_BGR));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ *  Positive: Par1 Par2 [...] Par15
+ *  Negative: Par1 Par2 [...] Par15
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	int i;
+
+	for (i = 0; i < par->gamma.num_curves; i++)
+		write_reg(par, 0xE0 + i,
+			  CURVE(i, 0), CURVE(i, 1), CURVE(i, 2),
+			  CURVE(i, 3), CURVE(i, 4), CURVE(i, 5),
+			  CURVE(i, 6), CURVE(i, 7), CURVE(i, 8),
+			  CURVE(i, 9), CURVE(i, 10), CURVE(i, 11),
+			  CURVE(i, 12), CURVE(i, 13), CURVE(i, 14));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = TXBUFLEN,
+	.gamma_num = 2,
+	.gamma_len = 15,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_SPI_DRIVER(DRVNAME, "ilitek", "ili9341", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9341");
+MODULE_ALIAS("platform:ili9341");
+
+MODULE_DESCRIPTION("FB driver for the ILI9341 LCD display controller");
+MODULE_AUTHOR("Christian Vogelgsang");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9481.c b/drivers/staging/fbtft/fb_ili9481.c
new file mode 100644
index 000000000..19eba085e
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9481.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9481 LCD Controller
+ *
+ * Copyright (c) 2014 Petr Olivka
+ * Copyright (c) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME	"fb_ili9481"
+#define WIDTH		320
+#define HEIGHT		480
+
+static const s16 default_init_sequence[] = {
+	/* SLP_OUT - Sleep out */
+	-1, MIPI_DCS_EXIT_SLEEP_MODE,
+	-2, 50,
+	/* Power setting */
+	-1, 0xD0, 0x07, 0x42, 0x18,
+	/* VCOM */
+	-1, 0xD1, 0x00, 0x07, 0x10,
+	/* Power setting for norm. mode */
+	-1, 0xD2, 0x01, 0x02,
+	/* Panel driving setting */
+	-1, 0xC0, 0x10, 0x3B, 0x00, 0x02, 0x11,
+	/* Frame rate & inv. */
+	-1, 0xC5, 0x03,
+	/* Pixel format */
+	-1, MIPI_DCS_SET_PIXEL_FORMAT, 0x55,
+	/* Gamma */
+	-1, 0xC8, 0x00, 0x32, 0x36, 0x45, 0x06, 0x16,
+		  0x37, 0x75, 0x77, 0x54, 0x0C, 0x00,
+	/* DISP_ON */
+	-1, MIPI_DCS_SET_DISPLAY_ON,
+	-3
+};
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define HFLIP 0x01
+#define VFLIP 0x02
+#define ROW_X_COL 0x20
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  ROW_X_COL | HFLIP | VFLIP | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  VFLIP | (par->bgr << 3));
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  ROW_X_COL | (par->bgr << 3));
+		break;
+	default:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  HFLIP | (par->bgr << 3));
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.init_sequence = default_init_sequence,
+	.fbtftops = {
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9481", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9481");
+MODULE_ALIAS("platform:ili9481");
+
+MODULE_DESCRIPTION("FB driver for the ILI9481 LCD Controller");
+MODULE_AUTHOR("Petr Olivka");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9486.c b/drivers/staging/fbtft/fb_ili9486.c
new file mode 100644
index 000000000..66210a713
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9486.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ILI9486 LCD Controller
+ *
+ * Copyright (C) 2014 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9486"
+#define WIDTH		320
+#define HEIGHT		480
+
+/* this init sequence matches PiScreen */
+static const s16 default_init_sequence[] = {
+	/* Interface Mode Control */
+	-1, 0xb0, 0x0,
+	-1, MIPI_DCS_EXIT_SLEEP_MODE,
+	-2, 250,
+	/* Interface Pixel Format */
+	-1, MIPI_DCS_SET_PIXEL_FORMAT, 0x55,
+	/* Power Control 3 */
+	-1, 0xC2, 0x44,
+	/* VCOM Control 1 */
+	-1, 0xC5, 0x00, 0x00, 0x00, 0x00,
+	/* PGAMCTRL(Positive Gamma Control) */
+	-1, 0xE0, 0x0F, 0x1F, 0x1C, 0x0C, 0x0F, 0x08, 0x48, 0x98,
+		  0x37, 0x0A, 0x13, 0x04, 0x11, 0x0D, 0x00,
+	/* NGAMCTRL(Negative Gamma Control) */
+	-1, 0xE1, 0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75,
+		  0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
+	/* Digital Gamma Control 1 */
+	-1, 0xE2, 0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75,
+		  0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
+	-1, MIPI_DCS_EXIT_SLEEP_MODE,
+	-1, MIPI_DCS_SET_DISPLAY_ON,
+	/* end marker */
+	-3
+};
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  0x80 | (par->bgr << 3));
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  0x20 | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  0x40 | (par->bgr << 3));
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  0xE0 | (par->bgr << 3));
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.init_sequence = default_init_sequence,
+	.fbtftops = {
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9486", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9486");
+MODULE_ALIAS("platform:ili9486");
+
+MODULE_DESCRIPTION("FB driver for the ILI9486 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_pcd8544.c b/drivers/staging/fbtft/fb_pcd8544.c
new file mode 100644
index 000000000..08f8a4bb8
--- /dev/null
+++ b/drivers/staging/fbtft/fb_pcd8544.c
@@ -0,0 +1,167 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the PCD8544 LCD Controller
+ *
+ * The display is monochrome and the video memory is RGB565.
+ * Any pixel value except 0 turns the pixel on.
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME	       "fb_pcd8544"
+#define WIDTH          84
+#define HEIGHT         48
+#define TXBUFLEN       (84 * 6)
+#define DEFAULT_GAMMA  "40" /* gamma controls the contrast in this driver */
+
+static unsigned int tc;
+module_param(tc, uint, 0000);
+MODULE_PARM_DESC(tc, "TC[1:0] Temperature coefficient: 0-3 (default: 0)");
+
+static unsigned int bs = 4;
+module_param(bs, uint, 0000);
+MODULE_PARM_DESC(bs, "BS[2:0] Bias voltage level: 0-7 (default: 4)");
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Function set
+	 *
+	 * 5:1  1
+	 * 2:0  PD - Powerdown control: chip is active
+	 * 1:0  V  - Entry mode: horizontal addressing
+	 * 0:1  H  - Extended instruction set control: extended
+	 */
+	write_reg(par, 0x21);
+
+	/* H=1 Temperature control
+	 *
+	 * 2:1  1
+	 * 1:x  TC1 - Temperature Coefficient: 0x10
+	 * 0:x  TC0
+	 */
+	write_reg(par, 0x04 | (tc & 0x3));
+
+	/* H=1 Bias system
+	 *
+	 * 4:1  1
+	 * 3:0  0
+	 * 2:x  BS2 - Bias System
+	 * 1:x  BS1
+	 * 0:x  BS0
+	 */
+	write_reg(par, 0x10 | (bs & 0x7));
+
+	/* Function set
+	 *
+	 * 5:1  1
+	 * 2:0  PD - Powerdown control: chip is active
+	 * 1:1  V  - Entry mode: vertical addressing
+	 * 0:0  H  - Extended instruction set control: basic
+	 */
+	write_reg(par, 0x22);
+
+	/* H=0 Display control
+	 *
+	 * 3:1  1
+	 * 2:1  D  - DE: 10=normal mode
+	 * 1:0  0
+	 * 0:0  E
+	 */
+	write_reg(par, 0x08 | 4);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* H=0 Set X address of RAM
+	 *
+	 * 7:1  1
+	 * 6-0: X[6:0] - 0x00
+	 */
+	write_reg(par, 0x80);
+
+	/* H=0 Set Y address of RAM
+	 *
+	 * 7:0  0
+	 * 6:1  1
+	 * 2-0: Y[2:0] - 0x0
+	 */
+	write_reg(par, 0x40);
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u8 *buf = par->txbuf.buf;
+	int x, y, i;
+	int ret = 0;
+
+	for (x = 0; x < 84; x++) {
+		for (y = 0; y < 6; y++) {
+			*buf = 0x00;
+			for (i = 0; i < 8; i++)
+				*buf |= (vmem16[(y * 8 + i) * 84 + x] ?
+					 1 : 0) << i;
+			buf++;
+		}
+	}
+
+	/* Write data */
+	gpiod_set_value(par->gpio.dc, 1);
+	ret = par->fbtftops.write(par, par->txbuf.buf, 6 * 84);
+	if (ret < 0)
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
+
+	return ret;
+}
+
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	/* apply mask */
+	curves[0] &= 0x7F;
+
+	write_reg(par, 0x23); /* turn on extended instruction set */
+	write_reg(par, 0x80 | curves[0]);
+	write_reg(par, 0x22); /* turn off extended instruction set */
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = TXBUFLEN,
+	.gamma_num = 1,
+	.gamma_len = 1,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.write_vmem = write_vmem,
+		.set_gamma = set_gamma,
+	},
+	.backlight = 1,
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "philips,pcd8544", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("spi:pcd8544");
+
+MODULE_DESCRIPTION("FB driver for the PCD8544 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ra8875.c b/drivers/staging/fbtft/fb_ra8875.c
new file mode 100644
index 000000000..398bdbf53
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ra8875.c
@@ -0,0 +1,305 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FBTFT driver for the RA8875 LCD Controller
+ * Copyright by Pf@nne & NOTRO
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include <linux/gpio/consumer.h>
+#include "fbtft.h"
+
+#define DRVNAME "fb_ra8875"
+
+static int write_spi(struct fbtft_par *par, void *buf, size_t len)
+{
+	struct spi_transfer t = {
+		.tx_buf = buf,
+		.len = len,
+		.speed_hz = 1000000,
+	};
+	struct spi_message m;
+
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	if (!par->spi) {
+		dev_err(par->info->device,
+			"%s: par->spi is unexpectedly NULL\n", __func__);
+		return -1;
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+	return spi_sync(par->spi, &m);
+}
+
+static int init_display(struct fbtft_par *par)
+{
+	gpiod_set_value(par->gpio.dc, 1);
+
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+		      "%s()\n", __func__);
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+		      "display size %dx%d\n",
+		par->info->var.xres,
+		par->info->var.yres);
+
+	par->fbtftops.reset(par);
+
+	if ((par->info->var.xres == 320) && (par->info->var.yres == 240)) {
+		/* PLL clock frequency */
+		write_reg(par, 0x88, 0x0A);
+		write_reg(par, 0x89, 0x02);
+		mdelay(10);
+		/* color deep / MCU Interface */
+		write_reg(par, 0x10, 0x0C);
+		/* pixel clock period  */
+		write_reg(par, 0x04, 0x03);
+		mdelay(1);
+		/* horizontal settings */
+		write_reg(par, 0x14, 0x27);
+		write_reg(par, 0x15, 0x00);
+		write_reg(par, 0x16, 0x05);
+		write_reg(par, 0x17, 0x04);
+		write_reg(par, 0x18, 0x03);
+		/* vertical settings */
+		write_reg(par, 0x19, 0xEF);
+		write_reg(par, 0x1A, 0x00);
+		write_reg(par, 0x1B, 0x05);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x0E);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x02);
+	} else if ((par->info->var.xres == 480) &&
+		   (par->info->var.yres == 272)) {
+		/* PLL clock frequency  */
+		write_reg(par, 0x88, 0x0A);
+		write_reg(par, 0x89, 0x02);
+		mdelay(10);
+		/* color deep / MCU Interface */
+		write_reg(par, 0x10, 0x0C);
+		/* pixel clock period  */
+		write_reg(par, 0x04, 0x82);
+		mdelay(1);
+		/* horizontal settings */
+		write_reg(par, 0x14, 0x3B);
+		write_reg(par, 0x15, 0x00);
+		write_reg(par, 0x16, 0x01);
+		write_reg(par, 0x17, 0x00);
+		write_reg(par, 0x18, 0x05);
+		/* vertical settings */
+		write_reg(par, 0x19, 0x0F);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x02);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x07);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x09);
+	} else if ((par->info->var.xres == 640) &&
+		   (par->info->var.yres == 480)) {
+		/* PLL clock frequency */
+		write_reg(par, 0x88, 0x0B);
+		write_reg(par, 0x89, 0x02);
+		mdelay(10);
+		/* color deep / MCU Interface */
+		write_reg(par, 0x10, 0x0C);
+		/* pixel clock period */
+		write_reg(par, 0x04, 0x01);
+		mdelay(1);
+		/* horizontal settings */
+		write_reg(par, 0x14, 0x4F);
+		write_reg(par, 0x15, 0x05);
+		write_reg(par, 0x16, 0x0F);
+		write_reg(par, 0x17, 0x01);
+		write_reg(par, 0x18, 0x00);
+		/* vertical settings */
+		write_reg(par, 0x19, 0xDF);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x0A);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x0E);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x01);
+	} else if ((par->info->var.xres == 800) &&
+		   (par->info->var.yres == 480)) {
+		/* PLL clock frequency */
+		write_reg(par, 0x88, 0x0B);
+		write_reg(par, 0x89, 0x02);
+		mdelay(10);
+		/* color deep / MCU Interface */
+		write_reg(par, 0x10, 0x0C);
+		/* pixel clock period */
+		write_reg(par, 0x04, 0x81);
+		mdelay(1);
+		/* horizontal settings */
+		write_reg(par, 0x14, 0x63);
+		write_reg(par, 0x15, 0x03);
+		write_reg(par, 0x16, 0x03);
+		write_reg(par, 0x17, 0x02);
+		write_reg(par, 0x18, 0x00);
+		/* vertical settings */
+		write_reg(par, 0x19, 0xDF);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x14);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x06);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x01);
+	} else {
+		dev_err(par->info->device, "display size is not supported!!");
+		return -1;
+	}
+
+	/* PWM clock */
+	write_reg(par, 0x8a, 0x81);
+	write_reg(par, 0x8b, 0xFF);
+	mdelay(10);
+
+	/* Display ON */
+	write_reg(par, 0x01, 0x80);
+	mdelay(10);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* Set_Active_Window */
+	write_reg(par, 0x30, xs & 0x00FF);
+	write_reg(par, 0x31, (xs & 0xFF00) >> 8);
+	write_reg(par, 0x32, ys & 0x00FF);
+	write_reg(par, 0x33, (ys & 0xFF00) >> 8);
+	write_reg(par, 0x34, (xs + xe) & 0x00FF);
+	write_reg(par, 0x35, ((xs + xe) & 0xFF00) >> 8);
+	write_reg(par, 0x36, (ys + ye) & 0x00FF);
+	write_reg(par, 0x37, ((ys + ye) & 0xFF00) >> 8);
+
+	/* Set_Memory_Write_Cursor */
+	write_reg(par, 0x46,  xs & 0xff);
+	write_reg(par, 0x47, (xs >> 8) & 0x03);
+	write_reg(par, 0x48,  ys & 0xff);
+	write_reg(par, 0x49, (ys >> 8) & 0x01);
+
+	write_reg(par, 0x02);
+}
+
+static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
+{
+	va_list args;
+	int i, ret;
+	u8 *buf = par->buf;
+
+	/* slow down spi-speed for writing registers */
+	par->fbtftops.write = write_spi;
+
+	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {
+		va_start(args, len);
+		for (i = 0; i < len; i++)
+			buf[i] = (u8)va_arg(args, unsigned int);
+		va_end(args);
+		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par, par->info->device,
+				  u8, buf, len, "%s: ", __func__);
+	}
+
+	va_start(args, len);
+	*buf++ = 0x80;
+	*buf = (u8)va_arg(args, unsigned int);
+	ret = par->fbtftops.write(par, par->buf, 2);
+	if (ret < 0) {
+		va_end(args);
+		dev_err(par->info->device, "write() failed and returned %dn",
+			ret);
+		return;
+	}
+	len--;
+
+	udelay(100);
+
+	if (len) {
+		buf = (u8 *)par->buf;
+		*buf++ = 0x00;
+		i = len;
+		while (i--)
+			*buf++ = (u8)va_arg(args, unsigned int);
+
+		ret = par->fbtftops.write(par, par->buf, len + 1);
+		if (ret < 0) {
+			va_end(args);
+			dev_err(par->info->device,
+				"write() failed and returned %dn", ret);
+			return;
+		}
+	}
+	va_end(args);
+
+	/* restore user spi-speed */
+	par->fbtftops.write = fbtft_write_spi;
+	udelay(100);
+}
+
+static int write_vmem16_bus8(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16;
+	__be16 *txbuf16;
+	size_t remain;
+	size_t to_copy;
+	size_t tx_array_size;
+	int i;
+	int ret = 0;
+	size_t startbyte_size = 0;
+
+	fbtft_par_dbg(DEBUG_WRITE_VMEM, par, "%s(offset=%zu, len=%zu)\n",
+		      __func__, offset, len);
+
+	remain = len / 2;
+	vmem16 = (u16 *)(par->info->screen_buffer + offset);
+	tx_array_size = par->txbuf.len / 2;
+	txbuf16 = par->txbuf.buf + 1;
+	tx_array_size -= 2;
+	*(u8 *)(par->txbuf.buf) = 0x00;
+	startbyte_size = 1;
+
+	while (remain) {
+		to_copy = min(tx_array_size, remain);
+		dev_dbg(par->info->device, "    to_copy=%zu, remain=%zu\n",
+			to_copy, remain - to_copy);
+
+		for (i = 0; i < to_copy; i++)
+			txbuf16[i] = cpu_to_be16(vmem16[i]);
+
+		vmem16 = vmem16 + to_copy;
+		ret = par->fbtftops.write(par, par->txbuf.buf,
+			startbyte_size + to_copy * 2);
+		if (ret < 0)
+			return ret;
+		remain -= to_copy;
+	}
+
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.write_register = write_reg8_bus8,
+		.write_vmem = write_vmem16_bus8,
+		.write = write_spi,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "raio,ra8875", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ra8875");
+MODULE_ALIAS("platform:ra8875");
+
+MODULE_DESCRIPTION("FB driver for the RA8875 LCD Controller");
+MODULE_AUTHOR("Pf@nne");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_s6d02a1.c b/drivers/staging/fbtft/fb_s6d02a1.c
new file mode 100644
index 000000000..d3d6871d8
--- /dev/null
+++ b/drivers/staging/fbtft/fb_s6d02a1.c
@@ -0,0 +1,166 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the S6D02A1 LCD Controller
+ *
+ * Based on fb_st7735r.c by Noralf Tronnes
+ * Init code from UTFT library by Henning Karlsen
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME "fb_s6d02a1"
+
+static const s16 default_init_sequence[] = {
+	-1, 0xf0, 0x5a, 0x5a,
+
+	-1, 0xfc, 0x5a, 0x5a,
+
+	-1, 0xfa, 0x02, 0x1f, 0x00, 0x10, 0x22, 0x30, 0x38,
+	0x3A, 0x3A, 0x3A, 0x3A, 0x3A, 0x3d, 0x02, 0x01,
+
+	-1, 0xfb, 0x21, 0x00, 0x02, 0x04, 0x07, 0x0a, 0x0b,
+	0x0c, 0x0c, 0x16, 0x1e, 0x30, 0x3f, 0x01, 0x02,
+
+	/* power setting sequence */
+	-1, 0xfd, 0x00, 0x00, 0x00, 0x17, 0x10, 0x00, 0x01,
+	0x01, 0x00, 0x1f, 0x1f,
+
+	-1, 0xf4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x3f,
+	0x07, 0x00, 0x3C, 0x36, 0x00, 0x3C, 0x36, 0x00,
+
+	-1, 0xf5, 0x00, 0x70, 0x66, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x6d, 0x66, 0x06,
+
+	-1, 0xf6, 0x02, 0x00, 0x3f, 0x00, 0x00, 0x00, 0x02,
+	0x00, 0x06, 0x01, 0x00,
+
+	-1, 0xf2, 0x00, 0x01, 0x03, 0x08, 0x08, 0x04, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x04, 0x08, 0x08,
+
+	-1, 0xf8, 0x11,
+
+	-1, 0xf7, 0xc8, 0x20, 0x00, 0x00,
+
+	-1, 0xf3, 0x00, 0x00,
+
+	-1, MIPI_DCS_EXIT_SLEEP_MODE,
+	-2, 50,
+
+	-1, 0xf3, 0x00, 0x01,
+	-2, 50,
+	-1, 0xf3, 0x00, 0x03,
+	-2, 50,
+	-1, 0xf3, 0x00, 0x07,
+	-2, 50,
+	-1, 0xf3, 0x00, 0x0f,
+	-2, 50,
+
+	-1, 0xf4, 0x00, 0x04, 0x00, 0x00, 0x00, 0x3f, 0x3f,
+	0x07, 0x00, 0x3C, 0x36, 0x00, 0x3C, 0x36, 0x00,
+	-2, 50,
+
+	-1, 0xf3, 0x00, 0x1f,
+	-2, 50,
+	-1, 0xf3, 0x00, 0x7f,
+	-2, 50,
+
+	-1, 0xf3, 0x00, 0xff,
+	-2, 50,
+
+	-1, 0xfd, 0x00, 0x00, 0x00, 0x17, 0x10, 0x00, 0x00,
+	0x01, 0x00, 0x16, 0x16,
+
+	-1, 0xf4, 0x00, 0x09, 0x00, 0x00, 0x00, 0x3f, 0x3f,
+	0x07, 0x00, 0x3C, 0x36, 0x00, 0x3C, 0x36, 0x00,
+
+	/* initializing sequence */
+
+	-1, MIPI_DCS_SET_ADDRESS_MODE, 0x08,
+
+	-1, MIPI_DCS_SET_TEAR_ON, 0x00,
+
+	-1, MIPI_DCS_SET_PIXEL_FORMAT, 0x05,
+
+	/* gamma setting - possible values 0x01, 0x02, 0x04, 0x08 */
+	-1, MIPI_DCS_SET_GAMMA_CURVE, 0x01,
+
+	-2, 150,
+	-1, MIPI_DCS_SET_DISPLAY_ON,
+	-1, MIPI_DCS_WRITE_MEMORY_START,
+	/* end marker */
+	-3
+
+};
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define MY BIT(7)
+#define MX BIT(6)
+#define MV BIT(5)
+static int set_var(struct fbtft_par *par)
+{
+	/*
+	 * Memory data access control (0x36h)
+	 * RGB/BGR:
+	 *	1. Mode selection pin SRGB
+	 *		RGB H/W pin for color filter setting: 0=RGB, 1=BGR
+	 *	2. MADCTL RGB bit
+	 *		RGB-BGR ORDER color filter panel: 0=RGB, 1=BGR
+	 */
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MY | (par->bgr << 3));
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MY | MV | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  par->bgr << 3);
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MV | (par->bgr << 3));
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = 128,
+	.height = 160,
+	.init_sequence = default_init_sequence,
+	.fbtftops = {
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "samsung,s6d02a1", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:s6d02a1");
+MODULE_ALIAS("platform:s6d02a1");
+
+MODULE_DESCRIPTION("FB driver for the S6D02A1 LCD Controller");
+MODULE_AUTHOR("WOLFGANG BUENING");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_s6d1121.c b/drivers/staging/fbtft/fb_s6d1121.c
new file mode 100644
index 000000000..62f27172f
--- /dev/null
+++ b/drivers/staging/fbtft/fb_s6d1121.c
@@ -0,0 +1,182 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the S6D1121 LCD Controller
+ *
+ * Copyright (C) 2013 Roman Rolinsky
+ *
+ * Based on fb_ili9325.c by Noralf Tronnes
+ * Based on ili9325.c by Jeroen Domburg
+ * Init code from UTFT library by Henning Karlsen
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_s6d1121"
+#define WIDTH		240
+#define HEIGHT		320
+#define BPP		16
+#define FPS		20
+#define DEFAULT_GAMMA	"26 09 24 2C 1F 23 24 25 22 26 25 23 0D 00\n" \
+			"1C 1A 13 1D 0B 11 12 10 13 15 36 19 00 0D"
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Initialization sequence from Lib_UTFT */
+
+	write_reg(par, 0x0011, 0x2004);
+	write_reg(par, 0x0013, 0xCC00);
+	write_reg(par, 0x0015, 0x2600);
+	write_reg(par, 0x0014, 0x252A);
+	write_reg(par, 0x0012, 0x0033);
+	write_reg(par, 0x0013, 0xCC04);
+	write_reg(par, 0x0013, 0xCC06);
+	write_reg(par, 0x0013, 0xCC4F);
+	write_reg(par, 0x0013, 0x674F);
+	write_reg(par, 0x0011, 0x2003);
+	write_reg(par, 0x0016, 0x0007);
+	write_reg(par, 0x0002, 0x0013);
+	write_reg(par, 0x0003, 0x0003);
+	write_reg(par, 0x0001, 0x0127);
+	write_reg(par, 0x0008, 0x0303);
+	write_reg(par, 0x000A, 0x000B);
+	write_reg(par, 0x000B, 0x0003);
+	write_reg(par, 0x000C, 0x0000);
+	write_reg(par, 0x0041, 0x0000);
+	write_reg(par, 0x0050, 0x0000);
+	write_reg(par, 0x0060, 0x0005);
+	write_reg(par, 0x0070, 0x000B);
+	write_reg(par, 0x0071, 0x0000);
+	write_reg(par, 0x0078, 0x0000);
+	write_reg(par, 0x007A, 0x0000);
+	write_reg(par, 0x0079, 0x0007);
+	write_reg(par, 0x0007, 0x0051);
+	write_reg(par, 0x0007, 0x0053);
+	write_reg(par, 0x0079, 0x0000);
+
+	write_reg(par, 0x0022); /* Write Data to GRAM */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R20h = Horizontal GRAM Start Address */
+	/* R21h = Vertical GRAM Start Address */
+	case 0:
+		write_reg(par, 0x0020, xs);
+		write_reg(par, 0x0021, ys);
+		break;
+	case 180:
+		write_reg(par, 0x0020, WIDTH - 1 - xs);
+		write_reg(par, 0x0021, HEIGHT - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x0020, WIDTH - 1 - ys);
+		write_reg(par, 0x0021, xs);
+		break;
+	case 90:
+		write_reg(par, 0x0020, ys);
+		write_reg(par, 0x0021, HEIGHT - 1 - xs);
+		break;
+	}
+	write_reg(par, 0x0022); /* Write Data to GRAM */
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	/* AM: GRAM update direction */
+	case 0:
+		write_reg(par, 0x03, 0x0003 | (par->bgr << 12));
+		break;
+	case 180:
+		write_reg(par, 0x03, 0x0000 | (par->bgr << 12));
+		break;
+	case 270:
+		write_reg(par, 0x03, 0x000A | (par->bgr << 12));
+		break;
+	case 90:
+		write_reg(par, 0x03, 0x0009 | (par->bgr << 12));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ * PKP0 PKP1 PKP2 PKP3 PKP4 PKP5 PKP6 PKP7 PKP8 PKP9 PKP10 PKP11 VRP0 VRP1
+ * PKN0 PKN1 PKN2 PKN3 PKN4 PKN5 PKN6 PKN7 PRN8 PRN9 PRN10 PRN11 VRN0 VRN1
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
+		0x3f, 0x3f, 0x1f, 0x1f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x1f, 0x1f,
+	};
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < 2; i++)
+		for (j = 0; j < 14; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	write_reg(par, 0x0030, CURVE(0, 1) << 8 | CURVE(0, 0));
+	write_reg(par, 0x0031, CURVE(0, 3) << 8 | CURVE(0, 2));
+	write_reg(par, 0x0032, CURVE(0, 5) << 8 | CURVE(0, 3));
+	write_reg(par, 0x0033, CURVE(0, 7) << 8 | CURVE(0, 6));
+	write_reg(par, 0x0034, CURVE(0, 9) << 8 | CURVE(0, 8));
+	write_reg(par, 0x0035, CURVE(0, 11) << 8 | CURVE(0, 10));
+
+	write_reg(par, 0x0036, CURVE(1, 1) << 8 | CURVE(1, 0));
+	write_reg(par, 0x0037, CURVE(1, 3) << 8 | CURVE(1, 2));
+	write_reg(par, 0x0038, CURVE(1, 5) << 8 | CURVE(1, 4));
+	write_reg(par, 0x0039, CURVE(1, 7) << 8 | CURVE(1, 6));
+	write_reg(par, 0x003A, CURVE(1, 9) << 8 | CURVE(1, 8));
+	write_reg(par, 0x003B, CURVE(1, 11) << 8 | CURVE(1, 10));
+
+	write_reg(par, 0x003C, CURVE(0, 13) << 8 | CURVE(0, 12));
+	write_reg(par, 0x003D, CURVE(1, 13) << 8 | CURVE(1, 12));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fps = FPS,
+	.gamma_num = 2,
+	.gamma_len = 14,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "samsung,s6d1121", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:s6d1121");
+MODULE_ALIAS("platform:s6d1121");
+
+MODULE_DESCRIPTION("FB driver for the S6D1121 LCD Controller");
+MODULE_AUTHOR("Roman Rolinsky");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_seps525.c b/drivers/staging/fbtft/fb_seps525.c
new file mode 100644
index 000000000..05882e2cd
--- /dev/null
+++ b/drivers/staging/fbtft/fb_seps525.c
@@ -0,0 +1,213 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * FB driver for the NHD-1.69-160128UGC3 (Newhaven Display International, Inc.)
+ * using the SEPS525 (Syncoam) LCD Controller
+ *
+ * Copyright (C) 2016 Analog Devices Inc.
+ *
+ * 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/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_seps525"
+#define WIDTH		160
+#define HEIGHT		128
+
+#define SEPS525_INDEX 0x00
+#define SEPS525_STATUS_RD 0x01
+#define SEPS525_OSC_CTL 0x02
+#define SEPS525_IREF 0x80
+#define SEPS525_CLOCK_DIV 0x03
+#define SEPS525_REDUCE_CURRENT 0x04
+#define SEPS525_SOFT_RST 0x05
+#define SEPS525_DISP_ONOFF 0x06
+#define SEPS525_PRECHARGE_TIME_R 0x08
+#define SEPS525_PRECHARGE_TIME_G 0x09
+#define SEPS525_PRECHARGE_TIME_B 0x0A
+#define SEPS525_PRECHARGE_CURRENT_R 0x0B
+#define SEPS525_PRECHARGE_CURRENT_G 0x0C
+#define SEPS525_PRECHARGE_CURRENT_B 0x0D
+#define SEPS525_DRIVING_CURRENT_R 0x10
+#define SEPS525_DRIVING_CURRENT_G 0x11
+#define SEPS525_DRIVING_CURRENT_B 0x12
+#define SEPS525_DISPLAYMODE_SET 0x13
+#define SEPS525_RGBIF 0x14
+#define SEPS525_RGB_POL 0x15
+#define SEPS525_MEMORY_WRITEMODE 0x16
+#define SEPS525_MX1_ADDR 0x17
+#define SEPS525_MX2_ADDR 0x18
+#define SEPS525_MY1_ADDR 0x19
+#define SEPS525_MY2_ADDR 0x1A
+#define SEPS525_MEMORY_ACCESS_POINTER_X 0x20
+#define SEPS525_MEMORY_ACCESS_POINTER_Y 0x21
+#define SEPS525_DDRAM_DATA_ACCESS_PORT 0x22
+#define SEPS525_GRAY_SCALE_TABLE_INDEX 0x50
+#define SEPS525_GRAY_SCALE_TABLE_DATA 0x51
+#define SEPS525_DUTY 0x28
+#define SEPS525_DSL 0x29
+#define SEPS525_D1_DDRAM_FAC 0x2E
+#define SEPS525_D1_DDRAM_FAR 0x2F
+#define SEPS525_D2_DDRAM_SAC 0x31
+#define SEPS525_D2_DDRAM_SAR 0x32
+#define SEPS525_SCR1_FX1 0x33
+#define SEPS525_SCR1_FX2 0x34
+#define SEPS525_SCR1_FY1 0x35
+#define SEPS525_SCR1_FY2 0x36
+#define SEPS525_SCR2_SX1 0x37
+#define SEPS525_SCR2_SX2 0x38
+#define SEPS525_SCR2_SY1 0x39
+#define SEPS525_SCR2_SY2 0x3A
+#define SEPS525_SCREEN_SAVER_CONTEROL 0x3B
+#define SEPS525_SS_SLEEP_TIMER 0x3C
+#define SEPS525_SCREEN_SAVER_MODE 0x3D
+#define SEPS525_SS_SCR1_FU 0x3E
+#define SEPS525_SS_SCR1_MXY 0x3F
+#define SEPS525_SS_SCR2_FU 0x40
+#define SEPS525_SS_SCR2_MXY 0x41
+#define SEPS525_MOVING_DIRECTION 0x42
+#define SEPS525_SS_SCR2_SX1 0x47
+#define SEPS525_SS_SCR2_SX2 0x48
+#define SEPS525_SS_SCR2_SY1 0x49
+#define SEPS525_SS_SCR2_SY2 0x4A
+
+/* SEPS525_DISPLAYMODE_SET */
+#define MODE_SWAP_BGR	BIT(7)
+#define MODE_SM		BIT(6)
+#define MODE_RD		BIT(5)
+#define MODE_CD		BIT(4)
+
+#define seps525_use_window	0 /* FBTFT doesn't really use it today */
+
+/* Init sequence taken from: Arduino Library for the Adafruit 2.2" display */
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	usleep_range(1000, 5000);
+
+	/* Disable Oscillator Power Down */
+	write_reg(par, SEPS525_REDUCE_CURRENT, 0x03);
+	usleep_range(1000, 5000);
+	/* Set Normal Driving Current */
+	write_reg(par, SEPS525_REDUCE_CURRENT, 0x00);
+	usleep_range(1000, 5000);
+
+	write_reg(par, SEPS525_SCREEN_SAVER_CONTEROL, 0x00);
+	/* Set EXPORT1 Pin at Internal Clock */
+	write_reg(par, SEPS525_OSC_CTL, 0x01);
+	/* Set Clock as 120 Frames/Sec */
+	write_reg(par, SEPS525_CLOCK_DIV, 0x90);
+	/* Set Reference Voltage Controlled by External Resister */
+	write_reg(par, SEPS525_IREF, 0x01);
+
+	/* precharge time R G B */
+	write_reg(par, SEPS525_PRECHARGE_TIME_R, 0x04);
+	write_reg(par, SEPS525_PRECHARGE_TIME_G, 0x05);
+	write_reg(par, SEPS525_PRECHARGE_TIME_B, 0x05);
+
+	/* precharge current R G B (uA) */
+	write_reg(par, SEPS525_PRECHARGE_CURRENT_R, 0x9D);
+	write_reg(par, SEPS525_PRECHARGE_CURRENT_G, 0x8C);
+	write_reg(par, SEPS525_PRECHARGE_CURRENT_B, 0x57);
+
+	/* driving current R G B (uA) */
+	write_reg(par, SEPS525_DRIVING_CURRENT_R, 0x56);
+	write_reg(par, SEPS525_DRIVING_CURRENT_G, 0x4D);
+	write_reg(par, SEPS525_DRIVING_CURRENT_B, 0x46);
+	/* Set Color Sequence */
+	write_reg(par, SEPS525_DISPLAYMODE_SET, 0xA0);
+	write_reg(par, SEPS525_RGBIF, 0x01); /* Set MCU Interface Mode */
+	/* Set Memory Write Mode */
+	write_reg(par, SEPS525_MEMORY_WRITEMODE, 0x66);
+	write_reg(par, SEPS525_DUTY, 0x7F); /* 1/128 Duty (0x0F~0x7F) */
+	/* Set Mapping RAM Display Start Line (0x00~0x7F) */
+	write_reg(par, SEPS525_DSL, 0x00);
+	write_reg(par, SEPS525_DISP_ONOFF, 0x01); /* Display On (0x00/0x01) */
+	/* Set All Internal Register Value as Normal Mode */
+	write_reg(par, SEPS525_SOFT_RST, 0x00);
+	/* Set RGB Interface Polarity as Active Low */
+	write_reg(par, SEPS525_RGB_POL, 0x00);
+
+	write_reg(par, SEPS525_DDRAM_DATA_ACCESS_PORT);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	if (seps525_use_window) {
+		/* Set Window Xs,Ys Xe,Ye*/
+		write_reg(par, SEPS525_MX1_ADDR, xs);
+		write_reg(par, SEPS525_MX2_ADDR, xe);
+		write_reg(par, SEPS525_MY1_ADDR, ys);
+		write_reg(par, SEPS525_MY2_ADDR, ye);
+	}
+	/* start position X,Y */
+	write_reg(par, SEPS525_MEMORY_ACCESS_POINTER_X, xs);
+	write_reg(par, SEPS525_MEMORY_ACCESS_POINTER_Y, ys);
+
+	write_reg(par, SEPS525_DDRAM_DATA_ACCESS_PORT);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	u8 val;
+
+	switch (par->info->var.rotate) {
+	case 0:
+		val = 0;
+		break;
+	case 180:
+		val = MODE_RD | MODE_CD;
+		break;
+	case 90:
+	case 270:
+
+	default:
+		return -EINVAL;
+	}
+	/* Memory Access Control  */
+	write_reg(par, SEPS525_DISPLAYMODE_SET, val |
+		       (par->bgr ? MODE_SWAP_BGR : 0));
+
+	write_reg(par, SEPS525_DDRAM_DATA_ACCESS_PORT);
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "syncoam,seps525", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:seps525");
+MODULE_ALIAS("platform:seps525");
+
+MODULE_DESCRIPTION("FB driver for the SEPS525 LCD Controller");
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_sh1106.c b/drivers/staging/fbtft/fb_sh1106.c
new file mode 100644
index 000000000..9685ca516
--- /dev/null
+++ b/drivers/staging/fbtft/fb_sh1106.c
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the SH1106 OLED Controller
+ * Based on the SSD1306 driver by Noralf Tronnes
+ *
+ * Copyright (C) 2017 Heiner Kallweit
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_sh1106"
+#define WIDTH		128
+#define HEIGHT		64
+
+/* Init sequence based on the Adafruit SSD1306 Arduino library */
+static int init_display(struct fbtft_par *par)
+{
+	if (!par->info->var.xres || par->info->var.xres > WIDTH ||
+	    !par->info->var.yres || par->info->var.yres > HEIGHT ||
+	    par->info->var.yres % 8) {
+		dev_err(par->info->device, "Invalid screen size\n");
+		return -EINVAL;
+	}
+
+	if (par->info->var.rotate) {
+		dev_err(par->info->device, "Display rotation not supported\n");
+		return -EINVAL;
+	}
+
+	par->fbtftops.reset(par);
+
+	/* Set Display OFF */
+	write_reg(par, 0xAE);
+
+	/* Set Display Clock Divide Ratio/ Oscillator Frequency */
+	write_reg(par, 0xD5, 0x80);
+
+	/* Set Multiplex Ratio */
+	write_reg(par, 0xA8, par->info->var.yres - 1);
+
+	/* Set Display Offset */
+	write_reg(par, 0xD3, 0x00);
+
+	/* Set Display Start Line */
+	write_reg(par, 0x40 | 0x0);
+
+	/* Set Segment Re-map */
+	/* column address 127 is mapped to SEG0 */
+	write_reg(par, 0xA0 | 0x1);
+
+	/* Set COM Output Scan Direction */
+	/* remapped mode. Scan from COM[N-1] to COM0 */
+	write_reg(par, 0xC8);
+
+	/* Set COM Pins Hardware Configuration */
+	if (par->info->var.yres == 64)
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0xDA, 0x12);
+	else if (par->info->var.yres == 48)
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0xDA, 0x12);
+	else
+		/* A[4]=0b, Sequential COM pin configuration */
+		write_reg(par, 0xDA, 0x02);
+
+	/* Set Pre-charge Period */
+	write_reg(par, 0xD9, 0xF1);
+
+	/* Set VCOMH Deselect Level */
+	write_reg(par, 0xDB, 0x40);
+
+	/* Set Display ON */
+	write_reg(par, 0xAF);
+
+	msleep(150);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+
+	write_reg(par, on ? 0xAE : 0xAF);
+
+	return 0;
+}
+
+/* Gamma is used to control Contrast */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	/* apply mask */
+	curves[0] &= 0xFF;
+
+	/* Set Contrast Control for BANK0 */
+	write_reg(par, 0x81, curves[0]);
+
+	return 0;
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u32 xres = par->info->var.xres;
+	int page, page_start, page_end, x, i, ret;
+	u8 *buf = par->txbuf.buf;
+
+	/* offset refers to vmem with 2 bytes element size */
+	page_start = offset / (8 * 2 * xres);
+	page_end = DIV_ROUND_UP(offset + len, 8 * 2 * xres);
+
+	for (page = page_start; page < page_end; page++) {
+		/* set page and set column to 2 because of vidmem width 132 */
+		write_reg(par, 0xb0 | page, 0x00 | 2, 0x10 | 0);
+
+		memset(buf, 0, xres);
+		for (x = 0; x < xres; x++)
+			for (i = 0; i < 8; i++)
+				if (vmem16[(page * 8 + i) * xres + x])
+					buf[x] |= BIT(i);
+
+		/* Write data */
+		ret = fbtft_write_buf_dc(par, buf, xres, 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void write_register(struct fbtft_par *par, int len, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, len);
+
+	for (i = 0; i < len; i++)
+		par->buf[i] = va_arg(args, unsigned int);
+
+	/* keep DC low for all command bytes to transfer */
+	fbtft_write_buf_dc(par, par->buf, len, 0);
+
+	va_end(args);
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = WIDTH,
+	.gamma_num = 1,
+	.gamma_len = 1,
+	/* set default contrast to 0xcd = 80% */
+	.gamma = "cd",
+	.fbtftops = {
+		.write_vmem = write_vmem,
+		.write_register = write_register,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.blank = blank,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_SPI_DRIVER(DRVNAME, "sinowealth", "sh1106", &display);
+
+MODULE_DESCRIPTION("SH1106 OLED Driver");
+MODULE_AUTHOR("Heiner Kallweit");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1289.c b/drivers/staging/fbtft/fb_ssd1289.c
new file mode 100644
index 000000000..f27bab38b
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1289.c
@@ -0,0 +1,179 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the SSD1289 LCD Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ *
+ * Init sequence taken from ITDB02_Graph16.cpp - (C)2010-2011 Henning Karlsen
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1289"
+#define WIDTH		240
+#define HEIGHT		320
+#define DEFAULT_GAMMA	"02 03 2 5 7 7 4 2 4 2\n" \
+			"02 03 2 5 7 5 4 2 4 2"
+
+static unsigned int reg11 = 0x6040;
+module_param(reg11, uint, 0000);
+MODULE_PARM_DESC(reg11, "Register 11h value");
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0x00, 0x0001);
+	write_reg(par, 0x03, 0xA8A4);
+	write_reg(par, 0x0C, 0x0000);
+	write_reg(par, 0x0D, 0x080C);
+	write_reg(par, 0x0E, 0x2B00);
+	write_reg(par, 0x1E, 0x00B7);
+	write_reg(par, 0x01,
+		  BIT(13) | (par->bgr << 11) | BIT(9) | (HEIGHT - 1));
+	write_reg(par, 0x02, 0x0600);
+	write_reg(par, 0x10, 0x0000);
+	write_reg(par, 0x05, 0x0000);
+	write_reg(par, 0x06, 0x0000);
+	write_reg(par, 0x16, 0xEF1C);
+	write_reg(par, 0x17, 0x0003);
+	write_reg(par, 0x07, 0x0233);
+	write_reg(par, 0x0B, 0x0000);
+	write_reg(par, 0x0F, 0x0000);
+	write_reg(par, 0x41, 0x0000);
+	write_reg(par, 0x42, 0x0000);
+	write_reg(par, 0x48, 0x0000);
+	write_reg(par, 0x49, 0x013F);
+	write_reg(par, 0x4A, 0x0000);
+	write_reg(par, 0x4B, 0x0000);
+	write_reg(par, 0x44, 0xEF00);
+	write_reg(par, 0x45, 0x0000);
+	write_reg(par, 0x46, 0x013F);
+	write_reg(par, 0x23, 0x0000);
+	write_reg(par, 0x24, 0x0000);
+	write_reg(par, 0x25, 0x8000);
+	write_reg(par, 0x4f, 0x0000);
+	write_reg(par, 0x4e, 0x0000);
+	write_reg(par, 0x22);
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R4Eh - Set GDDRAM X address counter */
+	/* R4Fh - Set GDDRAM Y address counter */
+	case 0:
+		write_reg(par, 0x4e, xs);
+		write_reg(par, 0x4f, ys);
+		break;
+	case 180:
+		write_reg(par, 0x4e, par->info->var.xres - 1 - xs);
+		write_reg(par, 0x4f, par->info->var.yres - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x4e, par->info->var.yres - 1 - ys);
+		write_reg(par, 0x4f, xs);
+		break;
+	case 90:
+		write_reg(par, 0x4e, ys);
+		write_reg(par, 0x4f, par->info->var.xres - 1 - xs);
+		break;
+	}
+
+	/* R22h - RAM data write */
+	write_reg(par, 0x22);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	if (par->fbtftops.init_display != init_display) {
+		/* don't risk messing up register 11h */
+		fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+			      "%s: skipping since custom init_display() is used\n",
+			      __func__);
+		return 0;
+	}
+
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, 0x11, reg11 | 0x30);
+		break;
+	case 270:
+		write_reg(par, 0x11, reg11 | 0x28);
+		break;
+	case 180:
+		write_reg(par, 0x11, reg11 | 0x00);
+		break;
+	case 90:
+		write_reg(par, 0x11, reg11 | 0x18);
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ * VRP0 VRP1 PRP0 PRP1 PKP0 PKP1 PKP2 PKP3 PKP4 PKP5
+ * VRN0 VRN1 PRN0 PRN1 PKN0 PKN1 PKN2 PKN3 PKN4 PKN5
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	static const unsigned long mask[] = {
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < 2; i++)
+		for (j = 0; j < 10; j++)
+			CURVE(i, j) &= mask[i * par->gamma.num_values + j];
+
+	write_reg(par, 0x0030, CURVE(0, 5) << 8 | CURVE(0, 4));
+	write_reg(par, 0x0031, CURVE(0, 7) << 8 | CURVE(0, 6));
+	write_reg(par, 0x0032, CURVE(0, 9) << 8 | CURVE(0, 8));
+	write_reg(par, 0x0033, CURVE(0, 3) << 8 | CURVE(0, 2));
+	write_reg(par, 0x0034, CURVE(1, 5) << 8 | CURVE(1, 4));
+	write_reg(par, 0x0035, CURVE(1, 7) << 8 | CURVE(1, 6));
+	write_reg(par, 0x0036, CURVE(1, 9) << 8 | CURVE(1, 8));
+	write_reg(par, 0x0037, CURVE(1, 3) << 8 | CURVE(1, 2));
+	write_reg(par, 0x003A, CURVE(0, 1) << 8 | CURVE(0, 0));
+	write_reg(par, 0x003B, CURVE(1, 1) << 8 | CURVE(1, 0));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 2,
+	.gamma_len = 10,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1289", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1289");
+MODULE_ALIAS("platform:ssd1289");
+
+MODULE_DESCRIPTION("FB driver for the SSD1289 LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1305.c b/drivers/staging/fbtft/fb_ssd1305.c
new file mode 100644
index 000000000..020fe48fe
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1305.c
@@ -0,0 +1,207 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the SSD1305 OLED Controller
+ *
+ * based on SSD1306 driver by Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1305"
+
+#define WIDTH 128
+#define HEIGHT 64
+
+/*
+ * write_reg() caveat:
+ *
+ *    This doesn't work because D/C has to be LOW for both values:
+ *      write_reg(par, val1, val2);
+ *
+ *    Do it like this:
+ *      write_reg(par, val1);
+ *      write_reg(par, val2);
+ */
+
+/* Init sequence taken from the Adafruit SSD1306 Arduino library */
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	if (par->gamma.curves[0] == 0) {
+		mutex_lock(&par->gamma.lock);
+		if (par->info->var.yres == 64)
+			par->gamma.curves[0] = 0xCF;
+		else
+			par->gamma.curves[0] = 0x8F;
+		mutex_unlock(&par->gamma.lock);
+	}
+
+	/* Set Display OFF */
+	write_reg(par, 0xAE);
+
+	/* Set Display Clock Divide Ratio/ Oscillator Frequency */
+	write_reg(par, 0xD5);
+	write_reg(par, 0x80);
+
+	/* Set Multiplex Ratio */
+	write_reg(par, 0xA8);
+	if (par->info->var.yres == 64)
+		write_reg(par, 0x3F);
+	else
+		write_reg(par, 0x1F);
+
+	/* Set Display Offset */
+	write_reg(par, 0xD3);
+	write_reg(par, 0x0);
+
+	/* Set Display Start Line */
+	write_reg(par, 0x40 | 0x0);
+
+	/* Charge Pump Setting */
+	write_reg(par, 0x8D);
+	/* A[2] = 1b, Enable charge pump during display on */
+	write_reg(par, 0x14);
+
+	/* Set Memory Addressing Mode */
+	write_reg(par, 0x20);
+	/* Vertical addressing mode  */
+	write_reg(par, 0x01);
+
+	/*
+	 * Set Segment Re-map
+	 * column address 127 is mapped to SEG0
+	 */
+	write_reg(par, 0xA0 | ((par->info->var.rotate == 180) ? 0x0 : 0x1));
+
+	/*
+	 * Set COM Output Scan Direction
+	 * remapped mode. Scan from COM[N-1] to COM0
+	 */
+	write_reg(par, ((par->info->var.rotate == 180) ? 0xC8 : 0xC0));
+
+	/* Set COM Pins Hardware Configuration */
+	write_reg(par, 0xDA);
+	if (par->info->var.yres == 64) {
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0x12);
+	} else {
+		/* A[4]=0b, Sequential COM pin configuration */
+		write_reg(par, 0x02);
+	}
+
+	/* Set Pre-charge Period */
+	write_reg(par, 0xD9);
+	write_reg(par, 0xF1);
+
+	/*
+	 * Entire Display ON
+	 * Resume to RAM content display. Output follows RAM content
+	 */
+	write_reg(par, 0xA4);
+
+	/*
+	 * Set Normal Display
+	 *  0 in RAM: OFF in display panel
+	 *  1 in RAM: ON in display panel
+	 */
+	write_reg(par, 0xA6);
+
+	/* Set Display ON */
+	write_reg(par, 0xAF);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* Set Lower Column Start Address for Page Addressing Mode */
+	write_reg(par, 0x00 | ((par->info->var.rotate == 180) ? 0x0 : 0x4));
+	/* Set Higher Column Start Address for Page Addressing Mode */
+	write_reg(par, 0x10 | 0x0);
+	/* Set Display Start Line */
+	write_reg(par, 0x40 | 0x0);
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	if (on)
+		write_reg(par, 0xAE);
+	else
+		write_reg(par, 0xAF);
+	return 0;
+}
+
+/* Gamma is used to control Contrast */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	curves[0] &= 0xFF;
+	/* Set Contrast Control for BANK0 */
+	write_reg(par, 0x81);
+	write_reg(par, curves[0]);
+
+	return 0;
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u8 *buf = par->txbuf.buf;
+	int x, y, i;
+	int ret;
+
+	for (x = 0; x < par->info->var.xres; x++) {
+		for (y = 0; y < par->info->var.yres / 8; y++) {
+			*buf = 0x00;
+			for (i = 0; i < 8; i++)
+				*buf |= (vmem16[(y * 8 + i) *
+						par->info->var.xres + x] ?
+					 1 : 0) << i;
+			buf++;
+		}
+	}
+
+	/* Write data */
+	gpiod_set_value(par->gpio.dc, 1);
+	ret = par->fbtftops.write(par, par->txbuf.buf,
+				  par->info->var.xres * par->info->var.yres /
+				  8);
+	if (ret < 0)
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = WIDTH * HEIGHT / 8,
+	.gamma_num = 1,
+	.gamma_len = 1,
+	.gamma = "00",
+	.fbtftops = {
+		.write_vmem = write_vmem,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.blank = blank,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1305", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1305");
+MODULE_ALIAS("platform:ssd1305");
+
+MODULE_DESCRIPTION("SSD1305 OLED Driver");
+MODULE_AUTHOR("Alexey Mednyy");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1306.c b/drivers/staging/fbtft/fb_ssd1306.c
new file mode 100644
index 000000000..6cf9df579
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1306.c
@@ -0,0 +1,228 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the SSD1306 OLED Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1306"
+#define WIDTH		128
+#define HEIGHT		64
+
+/*
+ * write_reg() caveat:
+ *
+ * This doesn't work because D/C has to be LOW for both values:
+ * write_reg(par, val1, val2);
+ *
+ * Do it like this:
+ * write_reg(par, val1);
+ * write_reg(par, val2);
+ */
+
+/* Init sequence taken from the Adafruit SSD1306 Arduino library */
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	if (par->gamma.curves[0] == 0) {
+		mutex_lock(&par->gamma.lock);
+		if (par->info->var.yres == 64)
+			par->gamma.curves[0] = 0xCF;
+		else
+			par->gamma.curves[0] = 0x8F;
+		mutex_unlock(&par->gamma.lock);
+	}
+
+	/* Set Display OFF */
+	write_reg(par, 0xAE);
+
+	/* Set Display Clock Divide Ratio/ Oscillator Frequency */
+	write_reg(par, 0xD5);
+	write_reg(par, 0x80);
+
+	/* Set Multiplex Ratio */
+	write_reg(par, 0xA8);
+	if (par->info->var.yres == 64)
+		write_reg(par, 0x3F);
+	else if (par->info->var.yres == 48)
+		write_reg(par, 0x2F);
+	else
+		write_reg(par, 0x1F);
+
+	/* Set Display Offset */
+	write_reg(par, 0xD3);
+	write_reg(par, 0x0);
+
+	/* Set Display Start Line */
+	write_reg(par, 0x40 | 0x0);
+
+	/* Charge Pump Setting */
+	write_reg(par, 0x8D);
+	/* A[2] = 1b, Enable charge pump during display on */
+	write_reg(par, 0x14);
+
+	/* Set Memory Addressing Mode */
+	write_reg(par, 0x20);
+	/* Vertical addressing mode  */
+	write_reg(par, 0x01);
+
+	/* Set Segment Re-map */
+	/* column address 127 is mapped to SEG0 */
+	write_reg(par, 0xA0 | 0x1);
+
+	/* Set COM Output Scan Direction */
+	/* remapped mode. Scan from COM[N-1] to COM0 */
+	write_reg(par, 0xC8);
+
+	/* Set COM Pins Hardware Configuration */
+	write_reg(par, 0xDA);
+	if (par->info->var.yres == 64)
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0x12);
+	else if (par->info->var.yres == 48)
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0x12);
+	else
+		/* A[4]=0b, Sequential COM pin configuration */
+		write_reg(par, 0x02);
+
+	/* Set Pre-charge Period */
+	write_reg(par, 0xD9);
+	write_reg(par, 0xF1);
+
+	/* Set VCOMH Deselect Level */
+	write_reg(par, 0xDB);
+	/* according to the datasheet, this value is out of bounds */
+	write_reg(par, 0x40);
+
+	/* Entire Display ON */
+	/* Resume to RAM content display. Output follows RAM content */
+	write_reg(par, 0xA4);
+
+	/* Set Normal Display
+	 * 0 in RAM: OFF in display panel
+	 * 1 in RAM: ON in display panel
+	 */
+	write_reg(par, 0xA6);
+
+	/* Set Display ON */
+	write_reg(par, 0xAF);
+
+	return 0;
+}
+
+static void set_addr_win_64x48(struct fbtft_par *par)
+{
+	/* Set Column Address */
+	write_reg(par, 0x21);
+	write_reg(par, 0x20);
+	write_reg(par, 0x5F);
+
+	/* Set Page Address */
+	write_reg(par, 0x22);
+	write_reg(par, 0x0);
+	write_reg(par, 0x5);
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* Set Lower Column Start Address for Page Addressing Mode */
+	write_reg(par, 0x00 | 0x0);
+	/* Set Higher Column Start Address for Page Addressing Mode */
+	write_reg(par, 0x10 | 0x0);
+	/* Set Display Start Line */
+	write_reg(par, 0x40 | 0x0);
+
+	if (par->info->var.xres == 64 && par->info->var.yres == 48)
+		set_addr_win_64x48(par);
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+
+	if (on)
+		write_reg(par, 0xAE);
+	else
+		write_reg(par, 0xAF);
+	return 0;
+}
+
+/* Gamma is used to control Contrast */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	/* apply mask */
+	curves[0] &= 0xFF;
+
+	/* Set Contrast Control for BANK0 */
+	write_reg(par, 0x81);
+	write_reg(par, curves[0]);
+
+	return 0;
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u32 xres = par->info->var.xres;
+	u32 yres = par->info->var.yres;
+	u8 *buf = par->txbuf.buf;
+	int x, y, i;
+	int ret = 0;
+
+	for (x = 0; x < xres; x++) {
+		for (y = 0; y < yres / 8; y++) {
+			*buf = 0x00;
+			for (i = 0; i < 8; i++)
+				if (vmem16[(y * 8 + i) * xres + x])
+					*buf |= BIT(i);
+			buf++;
+		}
+	}
+
+	/* Write data */
+	gpiod_set_value(par->gpio.dc, 1);
+	ret = par->fbtftops.write(par, par->txbuf.buf, xres * yres / 8);
+	if (ret < 0)
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
+
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 1,
+	.gamma_len = 1,
+	.gamma = "00",
+	.fbtftops = {
+		.write_vmem = write_vmem,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.blank = blank,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1306", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1306");
+MODULE_ALIAS("platform:ssd1306");
+
+MODULE_DESCRIPTION("SSD1306 OLED Driver");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1325.c b/drivers/staging/fbtft/fb_ssd1325.c
new file mode 100644
index 000000000..796a2ac3e
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1325.c
@@ -0,0 +1,194 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the SSD1325 OLED Controller
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1325"
+
+#define WIDTH 128
+#define HEIGHT 64
+#define GAMMA_NUM   1
+#define GAMMA_LEN   15
+#define DEFAULT_GAMMA "7 1 1 1 1 2 2 3 3 4 4 5 5 6 6"
+
+/*
+ * write_reg() caveat:
+ *
+ *    This doesn't work because D/C has to be LOW for both values:
+ *      write_reg(par, val1, val2);
+ *
+ *    Do it like this:
+ *      write_reg(par, val1);
+ *      write_reg(par, val2);
+ */
+
+/* Init sequence taken from the Adafruit SSD1306 Arduino library */
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0xb3);
+	write_reg(par, 0xf0);
+	write_reg(par, 0xae);
+	write_reg(par, 0xa1);
+	write_reg(par, 0x00);
+	write_reg(par, 0xa8);
+	write_reg(par, 0x3f);
+	write_reg(par, 0xa0);
+	write_reg(par, 0x45);
+	write_reg(par, 0xa2);
+	write_reg(par, 0x40);
+	write_reg(par, 0x75);
+	write_reg(par, 0x00);
+	write_reg(par, 0x3f);
+	write_reg(par, 0x15);
+	write_reg(par, 0x00);
+	write_reg(par, 0x7f);
+	write_reg(par, 0xa4);
+	write_reg(par, 0xaf);
+
+	return 0;
+}
+
+static uint8_t rgb565_to_g16(u16 pixel)
+{
+	u16 b = pixel & 0x1f;
+	u16 g = (pixel & (0x3f << 5)) >> 5;
+	u16 r = (pixel & (0x1f << (5 + 6))) >> (5 + 6);
+
+	pixel = (299 * r + 587 * g + 114 * b) / 195;
+	if (pixel > 255)
+		pixel = 255;
+	return (uint8_t)pixel / 16;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		      "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe,
+		      ye);
+
+	write_reg(par, 0x75);
+	write_reg(par, 0x00);
+	write_reg(par, 0x3f);
+	write_reg(par, 0x15);
+	write_reg(par, 0x00);
+	write_reg(par, 0x7f);
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+
+	if (on)
+		write_reg(par, 0xAE);
+	else
+		write_reg(par, 0xAF);
+	return 0;
+}
+
+/*
+ * Grayscale Lookup Table
+ * GS1 - GS15
+ * The "Gamma curve" contains the relative values between the entries
+ * in the Lookup table.
+ *
+ * 0 = Setting of GS1 < Setting of GS2 < Setting of GS3.....<
+ * Setting of GS14 < Setting of GS15
+ */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	int i;
+
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	for (i = 0; i < GAMMA_LEN; i++) {
+		if (i > 0 && curves[i] < 1) {
+			dev_err(par->info->device,
+				"Illegal value in Grayscale Lookup Table at index %d.\n"
+				"Must be greater than 0\n", i);
+			return -EINVAL;
+		}
+		if (curves[i] > 7) {
+			dev_err(par->info->device,
+				"Illegal value(s) in Grayscale Lookup Table.\n"
+				"At index=%d, the accumulated value has exceeded 7\n",
+				i);
+			return -EINVAL;
+		}
+	}
+	write_reg(par, 0xB8);
+	for (i = 0; i < 8; i++)
+		write_reg(par, (curves[i] & 0xFF));
+	return 0;
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u8 *buf = par->txbuf.buf;
+	u8 n1;
+	u8 n2;
+	int y, x;
+	int ret;
+
+	for (x = 0; x < par->info->var.xres; x++) {
+		if (x % 2)
+			continue;
+		for (y = 0; y < par->info->var.yres; y++) {
+			n1 = rgb565_to_g16(vmem16[y * par->info->var.xres + x]);
+			n2 = rgb565_to_g16(vmem16
+					   [y * par->info->var.xres + x + 1]);
+			*buf = (n1 << 4) | n2;
+			buf++;
+		}
+	}
+
+	gpiod_set_value(par->gpio.dc, 1);
+
+	/* Write data */
+	ret = par->fbtftops.write(par, par->txbuf.buf,
+				par->info->var.xres * par->info->var.yres / 2);
+	if (ret < 0)
+		dev_err(par->info->device,
+			"%s: write failed and returned: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = WIDTH * HEIGHT / 2,
+	.gamma_num = GAMMA_NUM,
+	.gamma_len = GAMMA_LEN,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.write_vmem = write_vmem,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.blank = blank,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1325", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1325");
+MODULE_ALIAS("platform:ssd1325");
+
+MODULE_DESCRIPTION("SSD1325 OLED Driver");
+MODULE_AUTHOR("Alexey Mednyy");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1331.c b/drivers/staging/fbtft/fb_ssd1331.c
new file mode 100644
index 000000000..ec5eced7f
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1331.c
@@ -0,0 +1,204 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1331"
+#define WIDTH		96
+#define HEIGHT		64
+#define GAMMA_NUM	1
+#define GAMMA_LEN	63
+#define DEFAULT_GAMMA	"0 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2" \
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0xae); /* Display Off */
+
+	/* Set Column Address Mapping, COM Scan Direction and Colour Depth */
+	if (par->info->var.rotate == 180)
+		write_reg(par, 0xa0, 0x60 | (par->bgr << 2));
+	else
+		write_reg(par, 0xa0, 0x72 | (par->bgr << 2));
+
+	write_reg(par, 0x72); /* RGB colour */
+	write_reg(par, 0xa1, 0x00); /* Set Display Start Line */
+	write_reg(par, 0xa2, 0x00); /* Set Display Offset */
+	write_reg(par, 0xa4); /* NORMALDISPLAY */
+	write_reg(par, 0xa8, 0x3f); /* Set multiplex */
+	write_reg(par, 0xad, 0x8e); /* Set master */
+	/* write_reg(par, 0xb0, 0x0b);  Set power mode */
+	write_reg(par, 0xb1, 0x31); /* Precharge */
+	write_reg(par, 0xb3, 0xf0); /* Clock div */
+	write_reg(par, 0x8a, 0x64); /* Precharge A */
+	write_reg(par, 0x8b, 0x78); /* Precharge B */
+	write_reg(par, 0x8c, 0x64); /* Precharge C */
+	write_reg(par, 0xbb, 0x3a); /* Precharge level */
+	write_reg(par, 0xbe, 0x3e); /* vcomh */
+	write_reg(par, 0x87, 0x06); /* Master current */
+	write_reg(par, 0x81, 0x91); /* Contrast A */
+	write_reg(par, 0x82, 0x50); /* Contrast B */
+	write_reg(par, 0x83, 0x7d); /* Contrast C */
+	write_reg(par, 0xaf); /* Set Sleep Mode Display On */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, 0x15, xs, xe);
+	write_reg(par, 0x75, ys, ye);
+}
+
+static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
+{
+	va_list args;
+	int i, ret;
+	u8 *buf = par->buf;
+
+	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {
+		va_start(args, len);
+		for (i = 0; i < len; i++)
+			buf[i] = (u8)va_arg(args, unsigned int);
+		va_end(args);
+		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par, par->info->device,
+				  u8, buf, len, "%s: ", __func__);
+	}
+
+	va_start(args, len);
+
+	*buf = (u8)va_arg(args, unsigned int);
+	gpiod_set_value(par->gpio.dc, 0);
+	ret = par->fbtftops.write(par, par->buf, sizeof(u8));
+	if (ret < 0) {
+		va_end(args);
+		dev_err(par->info->device,
+			"write() failed and returned %d\n", ret);
+		return;
+	}
+	len--;
+
+	if (len) {
+		i = len;
+		while (i--)
+			*buf++ = (u8)va_arg(args, unsigned int);
+		ret = par->fbtftops.write(par, par->buf, len * (sizeof(u8)));
+		if (ret < 0) {
+			va_end(args);
+			dev_err(par->info->device,
+				"write() failed and returned %d\n", ret);
+			return;
+		}
+	}
+	gpiod_set_value(par->gpio.dc, 1);
+	va_end(args);
+}
+
+/*
+ * Grayscale Lookup Table
+ * GS1 - GS63
+ * The driver Gamma curve contains the relative values between the entries
+ * in the Lookup table.
+ *
+ * From datasheet:
+ * 8.8 Gray Scale Decoder
+ *
+ * there are total 180 Gamma Settings (Setting 0 to Setting 180)
+ * available for the Gray Scale table.
+ *
+ * The gray scale is defined in incremental way, with reference
+ * to the length of previous table entry:
+ * Setting of GS1 has to be >= 0
+ * Setting of GS2 has to be > Setting of GS1 +1
+ * Setting of GS3 has to be > Setting of GS2 +1
+ * :
+ * Setting of GS63 has to be > Setting of GS62 +1
+ *
+ */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	unsigned long tmp[GAMMA_NUM * GAMMA_LEN];
+	int i, acc = 0;
+
+	for (i = 0; i < 63; i++) {
+		if (i > 0 && curves[i] < 2) {
+			dev_err(par->info->device,
+				"Illegal value in Grayscale Lookup Table at index %d. Must be greater than 1\n",
+				i);
+			return -EINVAL;
+		}
+		acc += curves[i];
+		tmp[i] = acc;
+		if (acc > 180) {
+			dev_err(par->info->device,
+				"Illegal value(s) in Grayscale Lookup Table. At index=%d, the accumulated value has exceeded 180\n",
+				i);
+			return -EINVAL;
+		}
+	}
+
+	write_reg(par, 0xB8,
+		  tmp[0], tmp[1], tmp[2], tmp[3], tmp[4], tmp[5], tmp[6],
+		  tmp[7], tmp[8], tmp[9], tmp[10], tmp[11], tmp[12], tmp[13],
+		  tmp[14], tmp[15], tmp[16], tmp[17], tmp[18], tmp[19], tmp[20],
+		  tmp[21], tmp[22], tmp[23], tmp[24], tmp[25], tmp[26],	tmp[27],
+		  tmp[28], tmp[29], tmp[30], tmp[31], tmp[32], tmp[33], tmp[34],
+		  tmp[35], tmp[36], tmp[37], tmp[38], tmp[39], tmp[40], tmp[41],
+		  tmp[42], tmp[43], tmp[44], tmp[45], tmp[46], tmp[47], tmp[48],
+		  tmp[49], tmp[50], tmp[51], tmp[52], tmp[53], tmp[54], tmp[55],
+		  tmp[56], tmp[57], tmp[58], tmp[59], tmp[60], tmp[61],
+		  tmp[62]);
+
+	return 0;
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+	if (on)
+		write_reg(par, 0xAE);
+	else
+		write_reg(par, 0xAF);
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = GAMMA_NUM,
+	.gamma_len = GAMMA_LEN,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.write_register = write_reg8_bus8,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_gamma = set_gamma,
+		.blank = blank,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1331", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1331");
+MODULE_ALIAS("platform:ssd1331");
+
+MODULE_DESCRIPTION("SSD1331 OLED Driver");
+MODULE_AUTHOR("Alec Smecher (adapted from SSD1351 by James Davies)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ssd1351.c b/drivers/staging/fbtft/fb_ssd1351.c
new file mode 100644
index 000000000..b8d55aa8c
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ssd1351.c
@@ -0,0 +1,242 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ssd1351"
+#define WIDTH		128
+#define HEIGHT		128
+#define GAMMA_NUM	1
+#define GAMMA_LEN	63
+#define DEFAULT_GAMMA	"0 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2 2 " \
+			"2 2 2 2 2 2 2" \
+
+static void register_onboard_backlight(struct fbtft_par *par);
+
+static int init_display(struct fbtft_par *par)
+{
+	if (par->pdata &&
+	    par->pdata->display.backlight == FBTFT_ONBOARD_BACKLIGHT) {
+		/* module uses onboard GPIO for panel power */
+		par->fbtftops.register_backlight = register_onboard_backlight;
+	}
+
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0xfd, 0x12); /* Command Lock */
+	write_reg(par, 0xfd, 0xb1); /* Command Lock */
+	write_reg(par, 0xae); /* Display Off */
+	write_reg(par, 0xb3, 0xf1); /* Front Clock Div */
+	write_reg(par, 0xca, 0x7f); /* Set Mux Ratio */
+	write_reg(par, 0x15, 0x00, 0x7f); /* Set Column Address */
+	write_reg(par, 0x75, 0x00, 0x7f); /* Set Row Address */
+	write_reg(par, 0xa1, 0x00); /* Set Display Start Line */
+	write_reg(par, 0xa2, 0x00); /* Set Display Offset */
+	write_reg(par, 0xb5, 0x00); /* Set GPIO */
+	write_reg(par, 0xab, 0x01); /* Set Function Selection */
+	write_reg(par, 0xb1, 0x32); /* Set Phase Length */
+	write_reg(par, 0xb4, 0xa0, 0xb5, 0x55); /* Set Segment Low Voltage */
+	write_reg(par, 0xbb, 0x17); /* Set Precharge Voltage */
+	write_reg(par, 0xbe, 0x05); /* Set VComH Voltage */
+	write_reg(par, 0xc1, 0xc8, 0x80, 0xc8); /* Set Contrast */
+	write_reg(par, 0xc7, 0x0f); /* Set Master Contrast */
+	write_reg(par, 0xb6, 0x01); /* Set Second Precharge Period */
+	write_reg(par, 0xa6); /* Set Display Mode Reset */
+	write_reg(par, 0xaf); /* Set Sleep Mode Display On */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, 0x15, xs, xe);
+	write_reg(par, 0x75, ys, ye);
+	write_reg(par, 0x5c);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	unsigned int remap;
+
+	if (par->fbtftops.init_display != init_display) {
+		/* don't risk messing up register A0h */
+		fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+			      "%s: skipping since custom init_display() is used\n",
+			       __func__);
+		return 0;
+	}
+
+	remap = 0x60 | (par->bgr << 2); /* Set Colour Depth */
+
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, 0xA0, remap | 0x00 | BIT(4));
+		break;
+	case 270:
+		write_reg(par, 0xA0, remap | 0x03 | BIT(4));
+		break;
+	case 180:
+		write_reg(par, 0xA0, remap | 0x02);
+		break;
+	case 90:
+		write_reg(par, 0xA0, remap | 0x01);
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Grayscale Lookup Table
+ * GS1 - GS63
+ * The driver Gamma curve contains the relative values between the entries
+ * in the Lookup table.
+ *
+ * From datasheet:
+ * 8.8 Gray Scale Decoder
+ *
+ *	there are total 180 Gamma Settings (Setting 0 to Setting 180)
+ *	available for the Gray Scale table.
+ *
+ *	The gray scale is defined in incremental way, with reference
+ *	to the length of previous table entry:
+ *		Setting of GS1 has to be >= 0
+ *		Setting of GS2 has to be > Setting of GS1 +1
+ *		Setting of GS3 has to be > Setting of GS2 +1
+ *		:
+ *		Setting of GS63 has to be > Setting of GS62 +1
+ *
+ */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	unsigned long tmp[GAMMA_NUM * GAMMA_LEN];
+	int i, acc = 0;
+
+	for (i = 0; i < 63; i++) {
+		if (i > 0 && curves[i] < 2) {
+			dev_err(par->info->device,
+				"Illegal value in Grayscale Lookup Table at index %d : %d. Must be greater than 1\n",
+				i, curves[i]);
+			return -EINVAL;
+		}
+		acc += curves[i];
+		tmp[i] = acc;
+		if (acc > 180) {
+			dev_err(par->info->device,
+				"Illegal value(s) in Grayscale Lookup Table. At index=%d : %d, the accumulated value has exceeded 180\n",
+				i, acc);
+			return -EINVAL;
+		}
+	}
+
+	write_reg(par, 0xB8,
+		  tmp[0],  tmp[1],  tmp[2],  tmp[3],
+		  tmp[4],  tmp[5],  tmp[6],  tmp[7],
+		  tmp[8],  tmp[9],  tmp[10], tmp[11],
+		  tmp[12], tmp[13], tmp[14], tmp[15],
+		  tmp[16], tmp[17], tmp[18], tmp[19],
+		  tmp[20], tmp[21], tmp[22], tmp[23],
+		  tmp[24], tmp[25], tmp[26], tmp[27],
+		  tmp[28], tmp[29], tmp[30], tmp[31],
+		  tmp[32], tmp[33], tmp[34], tmp[35],
+		  tmp[36], tmp[37], tmp[38], tmp[39],
+		  tmp[40], tmp[41], tmp[42], tmp[43],
+		  tmp[44], tmp[45], tmp[46], tmp[47],
+		  tmp[48], tmp[49], tmp[50], tmp[51],
+		  tmp[52], tmp[53], tmp[54], tmp[55],
+		  tmp[56], tmp[57], tmp[58], tmp[59],
+		  tmp[60], tmp[61], tmp[62]);
+
+	return 0;
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+	if (on)
+		write_reg(par, 0xAE);
+	else
+		write_reg(par, 0xAF);
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = GAMMA_NUM,
+	.gamma_len = GAMMA_LEN,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+		.blank = blank,
+	},
+};
+
+static int update_onboard_backlight(struct backlight_device *bd)
+{
+	struct fbtft_par *par = bl_get_data(bd);
+	bool on;
+
+	fbtft_par_dbg(DEBUG_BACKLIGHT, par,
+		      "%s: power=%d, fb_blank=%d\n",
+		      __func__, bd->props.power, bd->props.fb_blank);
+
+	on = !backlight_is_blank(bd);
+	/* Onboard backlight connected to GPIO0 on SSD1351, GPIO1 unused */
+	write_reg(par, 0xB5, on ? 0x03 : 0x02);
+
+	return 0;
+}
+
+static const struct backlight_ops bl_ops = {
+	.update_status = update_onboard_backlight,
+};
+
+static void register_onboard_backlight(struct fbtft_par *par)
+{
+	struct backlight_device *bd;
+	struct backlight_properties bl_props = { 0, };
+
+	bl_props.type = BACKLIGHT_RAW;
+	bl_props.power = FB_BLANK_POWERDOWN;
+
+	bd = backlight_device_register(dev_driver_string(par->info->device),
+				       par->info->device, par, &bl_ops,
+				       &bl_props);
+	if (IS_ERR(bd)) {
+		dev_err(par->info->device,
+			"cannot register backlight device (%ld)\n",
+			PTR_ERR(bd));
+		return;
+	}
+	par->info->bl_dev = bd;
+
+	if (!par->fbtftops.unregister_backlight)
+		par->fbtftops.unregister_backlight = fbtft_unregister_backlight;
+}
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "solomon,ssd1351", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ssd1351");
+MODULE_ALIAS("platform:ssd1351");
+
+MODULE_DESCRIPTION("SSD1351 OLED Driver");
+MODULE_AUTHOR("James Davies");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_st7735r.c b/drivers/staging/fbtft/fb_st7735r.c
new file mode 100644
index 000000000..9670a8989
--- /dev/null
+++ b/drivers/staging/fbtft/fb_st7735r.c
@@ -0,0 +1,186 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ST7735R LCD Controller
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME "fb_st7735r"
+#define DEFAULT_GAMMA   "0F 1A 0F 18 2F 28 20 22 1F 1B 23 37 00 07 02 10\n" \
+			"0F 1B 0F 17 33 2C 29 2E 30 30 39 3F 00 07 03 10"
+
+static const s16 default_init_sequence[] = {
+	-1, MIPI_DCS_SOFT_RESET,
+	-2, 150,                               /* delay */
+
+	-1, MIPI_DCS_EXIT_SLEEP_MODE,
+	-2, 500,                               /* delay */
+
+	/* FRMCTR1 - frame rate control: normal mode
+	 * frame rate = fosc / (1 x 2 + 40) * (LINE + 2C + 2D)
+	 */
+	-1, 0xB1, 0x01, 0x2C, 0x2D,
+
+	/* FRMCTR2 - frame rate control: idle mode
+	 * frame rate = fosc / (1 x 2 + 40) * (LINE + 2C + 2D)
+	 */
+	-1, 0xB2, 0x01, 0x2C, 0x2D,
+
+	/* FRMCTR3 - frame rate control - partial mode
+	 * dot inversion mode, line inversion mode
+	 */
+	-1, 0xB3, 0x01, 0x2C, 0x2D, 0x01, 0x2C, 0x2D,
+
+	/* INVCTR - display inversion control
+	 * no inversion
+	 */
+	-1, 0xB4, 0x07,
+
+	/* PWCTR1 - Power Control
+	 * -4.6V, AUTO mode
+	 */
+	-1, 0xC0, 0xA2, 0x02, 0x84,
+
+	/* PWCTR2 - Power Control
+	 * VGH25 = 2.4C VGSEL = -10 VGH = 3 * AVDD
+	 */
+	-1, 0xC1, 0xC5,
+
+	/* PWCTR3 - Power Control
+	 * Opamp current small, Boost frequency
+	 */
+	-1, 0xC2, 0x0A, 0x00,
+
+	/* PWCTR4 - Power Control
+	 * BCLK/2, Opamp current small & Medium low
+	 */
+	-1, 0xC3, 0x8A, 0x2A,
+
+	/* PWCTR5 - Power Control */
+	-1, 0xC4, 0x8A, 0xEE,
+
+	/* VMCTR1 - Power Control */
+	-1, 0xC5, 0x0E,
+
+	-1, MIPI_DCS_EXIT_INVERT_MODE,
+
+	-1, MIPI_DCS_SET_PIXEL_FORMAT, MIPI_DCS_PIXEL_FMT_16BIT,
+
+	-1, MIPI_DCS_SET_DISPLAY_ON,
+	-2, 100,                               /* delay */
+
+	-1, MIPI_DCS_ENTER_NORMAL_MODE,
+	-2, 10,                               /* delay */
+
+	/* end marker */
+	-3
+};
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+#define MY BIT(7)
+#define MX BIT(6)
+#define MV BIT(5)
+static int set_var(struct fbtft_par *par)
+{
+	/* MADCTL - Memory data access control
+	 * RGB/BGR:
+	 * 1. Mode selection pin SRGB
+	 *    RGB H/W pin for color filter setting: 0=RGB, 1=BGR
+	 * 2. MADCTL RGB bit
+	 *    RGB-BGR ORDER color filter panel: 0=RGB, 1=BGR
+	 */
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MY | (par->bgr << 3));
+		break;
+	case 270:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MY | MV | (par->bgr << 3));
+		break;
+	case 180:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  par->bgr << 3);
+		break;
+	case 90:
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE,
+			  MX | MV | (par->bgr << 3));
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Gamma string format:
+ * VRF0P VOS0P PK0P PK1P PK2P PK3P PK4P PK5P PK6P PK7P PK8P PK9P SELV0P SELV1P SELV62P SELV63P
+ * VRF0N VOS0N PK0N PK1N PK2N PK3N PK4N PK5N PK6N PK7N PK8N PK9N SELV0N SELV1N SELV62N SELV63N
+ */
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	int i, j;
+
+	/* apply mask */
+	for (i = 0; i < par->gamma.num_curves; i++)
+		for (j = 0; j < par->gamma.num_values; j++)
+			CURVE(i, j) &= 0x3f;
+
+	for (i = 0; i < par->gamma.num_curves; i++)
+		write_reg(par, 0xE0 + i,
+			  CURVE(i, 0),  CURVE(i, 1),
+			  CURVE(i, 2),  CURVE(i, 3),
+			  CURVE(i, 4),  CURVE(i, 5),
+			  CURVE(i, 6),  CURVE(i, 7),
+			  CURVE(i, 8),  CURVE(i, 9),
+			  CURVE(i, 10), CURVE(i, 11),
+			  CURVE(i, 12), CURVE(i, 13),
+			  CURVE(i, 14), CURVE(i, 15));
+
+	return 0;
+}
+
+#undef CURVE
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = 128,
+	.height = 160,
+	.init_sequence = default_init_sequence,
+	.gamma_num = 2,
+	.gamma_len = 16,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "sitronix,st7735r", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:st7735r");
+MODULE_ALIAS("platform:st7735r");
+
+MODULE_DESCRIPTION("FB driver for the ST7735R LCD Controller");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_st7789v.c b/drivers/staging/fbtft/fb_st7789v.c
new file mode 100644
index 000000000..861a15414
--- /dev/null
+++ b/drivers/staging/fbtft/fb_st7789v.c
@@ -0,0 +1,394 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the ST7789V LCD Controller
+ *
+ * Copyright (C) 2015 Dennis Menschel
+ */
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/module.h>
+
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME "fb_st7789v"
+
+#define DEFAULT_GAMMA \
+	"70 2C 2E 15 10 09 48 33 53 0B 19 18 20 25\n" \
+	"70 2C 2E 15 10 09 48 33 53 0B 19 18 20 25"
+
+#define HSD20_IPS_GAMMA \
+	"D0 05 0A 09 08 05 2E 44 45 0F 17 16 2B 33\n" \
+	"D0 05 0A 09 08 05 2E 43 45 0F 16 16 2B 33"
+
+#define HSD20_IPS 1
+
+/**
+ * enum st7789v_command - ST7789V display controller commands
+ *
+ * @PORCTRL: porch setting
+ * @GCTRL: gate control
+ * @VCOMS: VCOM setting
+ * @VDVVRHEN: VDV and VRH command enable
+ * @VRHS: VRH set
+ * @VDVS: VDV set
+ * @VCMOFSET: VCOM offset set
+ * @PWCTRL1: power control 1
+ * @PVGAMCTRL: positive voltage gamma control
+ * @NVGAMCTRL: negative voltage gamma control
+ *
+ * The command names are the same as those found in the datasheet to ease
+ * looking up their semantics and usage.
+ *
+ * Note that the ST7789V display controller offers quite a few more commands
+ * which have been omitted from this list as they are not used at the moment.
+ * Furthermore, commands that are compliant with the MIPI DCS have been left
+ * out as well to avoid duplicate entries.
+ */
+enum st7789v_command {
+	PORCTRL = 0xB2,
+	GCTRL = 0xB7,
+	VCOMS = 0xBB,
+	VDVVRHEN = 0xC2,
+	VRHS = 0xC3,
+	VDVS = 0xC4,
+	VCMOFSET = 0xC5,
+	PWCTRL1 = 0xD0,
+	PVGAMCTRL = 0xE0,
+	NVGAMCTRL = 0xE1,
+};
+
+#define MADCTL_BGR BIT(3) /* bitmask for RGB/BGR order */
+#define MADCTL_MV BIT(5) /* bitmask for page/column order */
+#define MADCTL_MX BIT(6) /* bitmask for column address order */
+#define MADCTL_MY BIT(7) /* bitmask for page address order */
+
+/* 60Hz for 16.6ms, configured as 2*16.6ms */
+#define PANEL_TE_TIMEOUT_MS  33
+
+static struct completion panel_te; /* completion for panel TE line */
+static int irq_te; /* Linux IRQ for LCD TE line */
+
+static irqreturn_t panel_te_handler(int irq, void *data)
+{
+	complete(&panel_te);
+	return IRQ_HANDLED;
+}
+
+/*
+ * init_tearing_effect_line() - init tearing effect line.
+ * @par: FBTFT parameter object.
+ *
+ * Return: 0 on success, or a negative error code otherwise.
+ */
+static int init_tearing_effect_line(struct fbtft_par *par)
+{
+	struct device *dev = par->info->device;
+	struct gpio_desc *te;
+	int rc, irq;
+
+	te = gpiod_get_optional(dev, "te", GPIOD_IN);
+	if (IS_ERR(te))
+		return dev_err_probe(dev, PTR_ERR(te), "Failed to request te GPIO\n");
+
+	/* if te is NULL, indicating no configuration, directly return success */
+	if (!te) {
+		irq_te = 0;
+		return 0;
+	}
+
+	irq = gpiod_to_irq(te);
+
+	/* GPIO is locked as an IRQ, we may drop the reference */
+	gpiod_put(te);
+
+	if (irq < 0)
+		return irq;
+
+	irq_te = irq;
+	init_completion(&panel_te);
+
+	/* The effective state is high and lasts no more than 1000 microseconds */
+	rc = devm_request_irq(dev, irq_te, panel_te_handler,
+			      IRQF_TRIGGER_RISING, "TE_GPIO", par);
+	if (rc)
+		return dev_err_probe(dev, rc, "TE IRQ request failed.\n");
+
+	disable_irq_nosync(irq_te);
+
+	return 0;
+}
+
+/**
+ * init_display() - initialize the display controller
+ *
+ * @par: FBTFT parameter object
+ *
+ * Most of the commands in this init function set their parameters to the
+ * same default values which are already in place after the display has been
+ * powered up. (The main exception to this rule is the pixel format which
+ * would default to 18 instead of 16 bit per pixel.)
+ * Nonetheless, this sequence can be used as a template for concrete
+ * displays which usually need some adjustments.
+ *
+ * Return: 0 on success, < 0 if error occurred.
+ */
+static int init_display(struct fbtft_par *par)
+{
+	int rc;
+
+	par->fbtftops.reset(par);
+
+	rc = init_tearing_effect_line(par);
+	if (rc)
+		return rc;
+
+	/* turn off sleep mode */
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+	mdelay(120);
+
+	/* set pixel format to RGB-565 */
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, MIPI_DCS_PIXEL_FMT_16BIT);
+	if (HSD20_IPS)
+		write_reg(par, PORCTRL, 0x05, 0x05, 0x00, 0x33, 0x33);
+
+	else
+		write_reg(par, PORCTRL, 0x08, 0x08, 0x00, 0x22, 0x22);
+
+	/*
+	 * VGH = 13.26V
+	 * VGL = -10.43V
+	 */
+	if (HSD20_IPS)
+		write_reg(par, GCTRL, 0x75);
+	else
+		write_reg(par, GCTRL, 0x35);
+
+	/*
+	 * VDV and VRH register values come from command write
+	 * (instead of NVM)
+	 */
+	write_reg(par, VDVVRHEN, 0x01, 0xFF);
+
+	/*
+	 * VAP =  4.1V + (VCOM + VCOM offset + 0.5 * VDV)
+	 * VAN = -4.1V + (VCOM + VCOM offset + 0.5 * VDV)
+	 */
+	if (HSD20_IPS)
+		write_reg(par, VRHS, 0x13);
+	else
+		write_reg(par, VRHS, 0x0B);
+
+	/* VDV = 0V */
+	write_reg(par, VDVS, 0x20);
+
+	/* VCOM = 0.9V */
+	if (HSD20_IPS)
+		write_reg(par, VCOMS, 0x22);
+	else
+		write_reg(par, VCOMS, 0x20);
+
+	/* VCOM offset = 0V */
+	write_reg(par, VCMOFSET, 0x20);
+
+	/*
+	 * AVDD = 6.8V
+	 * AVCL = -4.8V
+	 * VDS = 2.3V
+	 */
+	write_reg(par, PWCTRL1, 0xA4, 0xA1);
+
+	/* TE line output is off by default when powering on */
+	if (irq_te)
+		write_reg(par, MIPI_DCS_SET_TEAR_ON, 0x00);
+
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+
+	if (HSD20_IPS)
+		write_reg(par, MIPI_DCS_ENTER_INVERT_MODE);
+
+	return 0;
+}
+
+/*
+ * write_vmem() - write data to display.
+ * @par: FBTFT parameter object.
+ * @offset: offset from screen_buffer.
+ * @len: the length of data to be writte.
+ *
+ * Return: 0 on success, or a negative error code otherwise.
+ */
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	struct device *dev = par->info->device;
+	int ret;
+
+	if (irq_te) {
+		enable_irq(irq_te);
+		reinit_completion(&panel_te);
+		ret = wait_for_completion_timeout(&panel_te,
+						  msecs_to_jiffies(PANEL_TE_TIMEOUT_MS));
+		if (ret == 0)
+			dev_err(dev, "wait panel TE timeout\n");
+
+		disable_irq(irq_te);
+	}
+
+	switch (par->pdata->display.buswidth) {
+	case 8:
+		ret = fbtft_write_vmem16_bus8(par, offset, len);
+		break;
+	case 9:
+		ret = fbtft_write_vmem16_bus9(par, offset, len);
+		break;
+	case 16:
+		ret = fbtft_write_vmem16_bus16(par, offset, len);
+		break;
+	default:
+		dev_err(dev, "Unsupported buswidth %d\n",
+			par->pdata->display.buswidth);
+		ret = 0;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * set_var() - apply LCD properties like rotation and BGR mode
+ *
+ * @par: FBTFT parameter object
+ *
+ * Return: 0 on success, < 0 if error occurred.
+ */
+static int set_var(struct fbtft_par *par)
+{
+	u8 madctl_par = 0;
+
+	if (par->bgr)
+		madctl_par |= MADCTL_BGR;
+	switch (par->info->var.rotate) {
+	case 0:
+		break;
+	case 90:
+		madctl_par |= (MADCTL_MV | MADCTL_MY);
+		break;
+	case 180:
+		madctl_par |= (MADCTL_MX | MADCTL_MY);
+		break;
+	case 270:
+		madctl_par |= (MADCTL_MV | MADCTL_MX);
+		break;
+	default:
+		return -EINVAL;
+	}
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, madctl_par);
+	return 0;
+}
+
+/**
+ * set_gamma() - set gamma curves
+ *
+ * @par: FBTFT parameter object
+ * @curves: gamma curves
+ *
+ * Before the gamma curves are applied, they are preprocessed with a bitmask
+ * to ensure syntactically correct input for the display controller.
+ * This implies that the curves input parameter might be changed by this
+ * function and that illegal gamma values are auto-corrected and not
+ * reported as errors.
+ *
+ * Return: 0 on success, < 0 if error occurred.
+ */
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	int i;
+	int j;
+	int c; /* curve index offset */
+
+	/*
+	 * Bitmasks for gamma curve command parameters.
+	 * The masks are the same for both positive and negative voltage
+	 * gamma curves.
+	 */
+	static const u8 gamma_par_mask[] = {
+		0xFF, /* V63[3:0], V0[3:0]*/
+		0x3F, /* V1[5:0] */
+		0x3F, /* V2[5:0] */
+		0x1F, /* V4[4:0] */
+		0x1F, /* V6[4:0] */
+		0x3F, /* J0[1:0], V13[3:0] */
+		0x7F, /* V20[6:0] */
+		0x77, /* V36[2:0], V27[2:0] */
+		0x7F, /* V43[6:0] */
+		0x3F, /* J1[1:0], V50[3:0] */
+		0x1F, /* V57[4:0] */
+		0x1F, /* V59[4:0] */
+		0x3F, /* V61[5:0] */
+		0x3F, /* V62[5:0] */
+	};
+
+	for (i = 0; i < par->gamma.num_curves; i++) {
+		c = i * par->gamma.num_values;
+		for (j = 0; j < par->gamma.num_values; j++)
+			curves[c + j] &= gamma_par_mask[j];
+		write_reg(par, PVGAMCTRL + i,
+			  curves[c + 0],  curves[c + 1],  curves[c + 2],
+			  curves[c + 3],  curves[c + 4],  curves[c + 5],
+			  curves[c + 6],  curves[c + 7],  curves[c + 8],
+			  curves[c + 9],  curves[c + 10], curves[c + 11],
+			  curves[c + 12], curves[c + 13]);
+	}
+	return 0;
+}
+
+/**
+ * blank() - blank the display
+ *
+ * @par: FBTFT parameter object
+ * @on: whether to enable or disable blanking the display
+ *
+ * Return: 0 on success, < 0 if error occurred.
+ */
+static int blank(struct fbtft_par *par, bool on)
+{
+	if (on)
+		write_reg(par, MIPI_DCS_SET_DISPLAY_OFF);
+	else
+		write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = 240,
+	.height = 320,
+	.gamma_num = 2,
+	.gamma_len = 14,
+	.gamma = HSD20_IPS_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.write_vmem = write_vmem,
+		.set_var = set_var,
+		.set_gamma = set_gamma,
+		.blank = blank,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "sitronix,st7789v", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:st7789v");
+MODULE_ALIAS("platform:st7789v");
+
+MODULE_DESCRIPTION("FB driver for the ST7789V LCD Controller");
+MODULE_AUTHOR("Dennis Menschel");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_tinylcd.c b/drivers/staging/fbtft/fb_tinylcd.c
new file mode 100644
index 000000000..9469248f2
--- /dev/null
+++ b/drivers/staging/fbtft/fb_tinylcd.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Custom FB driver for tinylcd.com display
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_tinylcd"
+#define WIDTH		320
+#define HEIGHT		480
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	write_reg(par, 0xB0, 0x80);
+	write_reg(par, 0xC0, 0x0A, 0x0A);
+	write_reg(par, 0xC1, 0x45, 0x07);
+	write_reg(par, 0xC2, 0x33);
+	write_reg(par, 0xC5, 0x00, 0x42, 0x80);
+	write_reg(par, 0xB1, 0xD0, 0x11);
+	write_reg(par, 0xB4, 0x02);
+	write_reg(par, 0xB6, 0x00, 0x22, 0x3B);
+	write_reg(par, 0xB7, 0x07);
+	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0x58);
+	write_reg(par, 0xF0, 0x36, 0xA5, 0xD3);
+	write_reg(par, 0xE5, 0x80);
+	write_reg(par, 0xE5, 0x01);
+	write_reg(par, 0xB3, 0x00);
+	write_reg(par, 0xE5, 0x00);
+	write_reg(par, 0xF0, 0x36, 0xA5, 0x53);
+	write_reg(par, 0xE0, 0x00, 0x35, 0x33, 0x00, 0x00, 0x00,
+		       0x00, 0x35, 0x33, 0x00, 0x00, 0x00);
+	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, 0x55);
+	write_reg(par, MIPI_DCS_EXIT_SLEEP_MODE);
+	udelay(250);
+	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	case 270:
+		write_reg(par, 0xB6, 0x00, 0x02, 0x3B);
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0x28);
+		break;
+	case 180:
+		write_reg(par, 0xB6, 0x00, 0x22, 0x3B);
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0x58);
+		break;
+	case 90:
+		write_reg(par, 0xB6, 0x00, 0x22, 0x3B);
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0x38);
+		break;
+	default:
+		write_reg(par, 0xB6, 0x00, 0x22, 0x3B);
+		write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, 0x08);
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "neosec,tinylcd", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("spi:tinylcd");
+
+MODULE_DESCRIPTION("Custom FB driver for tinylcd.com display");
+MODULE_AUTHOR("Noralf Tronnes");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_tls8204.c b/drivers/staging/fbtft/fb_tls8204.c
new file mode 100644
index 000000000..bec6dd0ff
--- /dev/null
+++ b/drivers/staging/fbtft/fb_tls8204.c
@@ -0,0 +1,160 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the TLS8204 LCD Controller
+ *
+ * The display is monochrome and the video memory is RGB565.
+ * Any pixel value except 0 turns the pixel on.
+ *
+ * Copyright (C) 2013 Noralf Tronnes
+ * Copyright (C) 2014 Michael Hope (adapted for the TLS8204)
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_tls8204"
+#define WIDTH		84
+#define HEIGHT		48
+#define TXBUFLEN	WIDTH
+
+/* gamma is used to control contrast in this driver */
+#define DEFAULT_GAMMA	"40"
+
+static unsigned int bs = 4;
+module_param(bs, uint, 0000);
+MODULE_PARM_DESC(bs, "BS[2:0] Bias voltage level: 0-7 (default: 4)");
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Enter extended command mode */
+	write_reg(par, 0x21);	/* 5:1  1
+				 * 2:0  PD - Powerdown control: chip is active
+				 * 1:0  V  - Entry mode: horizontal addressing
+				 * 0:1  H  - Extended instruction set control:
+				 *	     extended
+				 */
+
+	/* H=1 Bias system */
+	write_reg(par, 0x10 | (bs & 0x7));
+				/* 4:1  1
+				 * 3:0  0
+				 * 2:x  BS2 - Bias System
+				 * 1:x  BS1
+				 * 0:x  BS0
+				 */
+
+	/* Set the address of the first display line. */
+	write_reg(par, 0x04 | (64 >> 6));
+	write_reg(par, 0x40 | (64 & 0x3F));
+
+	/* Enter H=0 standard command mode */
+	write_reg(par, 0x20);
+
+	/* H=0 Display control */
+	write_reg(par, 0x08 | 4);
+				/* 3:1  1
+				 * 2:1  D - DE: 10=normal mode
+				 * 1:0  0
+				 * 0:0  E
+				 */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* H=0 Set X address of RAM */
+	write_reg(par, 0x80);	/* 7:1  1
+				 * 6-0: X[6:0] - 0x00
+				 */
+
+	/* H=0 Set Y address of RAM */
+	write_reg(par, 0x40);	/* 7:0  0
+				 * 6:1  1
+				 * 2-0: Y[2:0] - 0x0
+				 */
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	int x, y, i;
+	int ret = 0;
+
+	for (y = 0; y < HEIGHT / 8; y++) {
+		u8 *buf = par->txbuf.buf;
+		/* The display is 102x68 but the LCD is 84x48.
+		 * Set the write pointer at the start of each row.
+		 */
+		gpiod_set_value(par->gpio.dc, 0);
+		write_reg(par, 0x80 | 0);
+		write_reg(par, 0x40 | y);
+
+		for (x = 0; x < WIDTH; x++) {
+			u8 ch = 0;
+
+			for (i = 0; i < 8 * WIDTH; i += WIDTH) {
+				ch >>= 1;
+				if (vmem16[(y * 8 * WIDTH) + i + x])
+					ch |= 0x80;
+			}
+			*buf++ = ch;
+		}
+		/* Write the row */
+		gpiod_set_value(par->gpio.dc, 1);
+		ret = par->fbtftops.write(par, par->txbuf.buf, WIDTH);
+		if (ret < 0) {
+			dev_err(par->info->device,
+				"write failed and returned: %d\n", ret);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int set_gamma(struct fbtft_par *par, u32 *curves)
+{
+	/* apply mask */
+	curves[0] &= 0x7F;
+
+	write_reg(par, 0x21); /* turn on extended instruction set */
+	write_reg(par, 0x80 | curves[0]);
+	write_reg(par, 0x20); /* turn off extended instruction set */
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.txbuflen = TXBUFLEN,
+	.gamma_num = 1,
+	.gamma_len = 1,
+	.gamma = DEFAULT_GAMMA,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.write_vmem = write_vmem,
+		.set_gamma = set_gamma,
+	},
+	.backlight = 1,
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "teralane,tls8204", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("spi:tls8204");
+
+MODULE_DESCRIPTION("FB driver for the TLS8204 LCD Controller");
+MODULE_AUTHOR("Michael Hope");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_uc1611.c b/drivers/staging/fbtft/fb_uc1611.c
new file mode 100644
index 000000000..f61e373c7
--- /dev/null
+++ b/drivers/staging/fbtft/fb_uc1611.c
@@ -0,0 +1,337 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the UltraChip UC1611 LCD controller
+ *
+ * The display is 4-bit grayscale (16 shades) 240x160.
+ *
+ * Copyright (C) 2015 Henri Chain
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_uc1611"
+#define WIDTH		240
+#define HEIGHT		160
+#define BPP		8
+#define FPS		40
+
+/*
+ * LCD voltage is a combination of ratio, gain, pot and temp
+ *
+ * V_LCD = V_BIAS * ratio
+ * V_LCD = (C_V0 + C_PM × pot) * (1 + (T - 25) * temp)
+ * C_V0 and C_PM depend on ratio and gain
+ * T is ambient temperature
+ */
+
+/* BR -> actual ratio: 0-3 -> 5, 10, 11, 13 */
+static unsigned int ratio = 2;
+module_param(ratio, uint, 0000);
+MODULE_PARM_DESC(ratio, "BR[1:0] Bias voltage ratio: 0-3 (default: 2)");
+
+static unsigned int gain = 3;
+module_param(gain, uint, 0000);
+MODULE_PARM_DESC(gain, "GN[1:0] Bias voltage gain: 0-3 (default: 3)");
+
+static unsigned int pot = 16;
+module_param(pot, uint, 0000);
+MODULE_PARM_DESC(pot, "PM[6:0] Bias voltage pot.: 0-63 (default: 16)");
+
+/* TC -> % compensation per deg C: 0-3 -> -.05, -.10, -.015, -.20 */
+static unsigned int temp;
+module_param(temp, uint, 0000);
+MODULE_PARM_DESC(temp, "TC[1:0] Temperature compensation: 0-3 (default: 0)");
+
+/* PC[1:0] -> LCD capacitance: 0-3 -> <20nF, 20-28 nF, 29-40 nF, 40-56 nF */
+static unsigned int load = 1;
+module_param(load, uint, 0000);
+MODULE_PARM_DESC(load, "PC[1:0] Panel Loading: 0-3 (default: 1)");
+
+/* PC[3:2] -> V_LCD: 0, 1, 3 -> ext., int. with ratio = 5, int. standard */
+static unsigned int pump = 3;
+module_param(pump, uint, 0000);
+MODULE_PARM_DESC(pump, "PC[3:2] Pump control: 0,1,3 (default: 3)");
+
+static int init_display(struct fbtft_par *par)
+{
+	int ret;
+
+	/*
+	 * Set CS active inverse polarity: just setting SPI_CS_HIGH does not
+	 * work with GPIO based chip selects that are logically active high
+	 * but inverted inside the GPIO library, so enforce inverted
+	 * semantics.
+	 */
+	par->spi->mode ^= SPI_CS_HIGH;
+	ret = spi_setup(par->spi);
+	if (ret) {
+		dev_err(par->info->device,
+			"Could not set inverse CS polarity\n");
+		return ret;
+	}
+
+	/* Reset controller */
+	write_reg(par, 0xE2);
+
+	/* Set bias ratio */
+	write_reg(par, 0xE8 | (ratio & 0x03));
+
+	/* Set bias gain and potentiometer */
+	write_reg(par, 0x81);
+	write_reg(par, (gain & 0x03) << 6 | (pot & 0x3F));
+
+	/* Set temperature compensation */
+	write_reg(par, 0x24 | (temp & 0x03));
+
+	/* Set panel loading */
+	write_reg(par, 0x28 | (load & 0x03));
+
+	/* Set pump control */
+	write_reg(par, 0x2C | (pump & 0x03));
+
+	/* Set inverse display */
+	write_reg(par, 0xA6 | 0x01);
+
+	/* Set 4-bit grayscale mode */
+	write_reg(par, 0xD0 | (0x02 & 0x03));
+
+	/* Set Display enable */
+	write_reg(par, 0xA8 | 0x07);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	case 90:
+	case 270:
+		/* Set column address */
+		write_reg(par, ys & 0x0F);
+		write_reg(par, 0x10 | (ys >> 4));
+
+		/* Set page address (divide xs by 2) (not used by driver) */
+		write_reg(par, 0x60 | ((xs >> 1) & 0x0F));
+		write_reg(par, 0x70 | (xs >> 5));
+		break;
+	default:
+		/* Set column address (not used by driver) */
+		write_reg(par, xs & 0x0F);
+		write_reg(par, 0x10 | (xs >> 4));
+
+		/* Set page address (divide ys by 2) */
+		write_reg(par, 0x60 | ((ys >> 1) & 0x0F));
+		write_reg(par, 0x70 | (ys >> 5));
+		break;
+	}
+}
+
+static int blank(struct fbtft_par *par, bool on)
+{
+	fbtft_par_dbg(DEBUG_BLANK, par, "(%s=%s)\n",
+		      __func__, on ? "true" : "false");
+
+	if (on)
+		write_reg(par, 0xA8 | 0x00);
+	else
+		write_reg(par, 0xA8 | 0x07);
+	return 0;
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	/* par->info->fix.visual = FB_VISUAL_PSEUDOCOLOR; */
+	par->info->var.grayscale = 1;
+	par->info->var.red.offset    = 0;
+	par->info->var.red.length    = 8;
+	par->info->var.green.offset  = 0;
+	par->info->var.green.length  = 8;
+	par->info->var.blue.offset   = 0;
+	par->info->var.blue.length   = 8;
+	par->info->var.transp.offset = 0;
+	par->info->var.transp.length = 0;
+
+	switch (par->info->var.rotate) {
+	case 90:
+		/* Set RAM address control */
+		write_reg(par, 0x88
+			| (0x0 & 0x1) << 2 /* Increment positively */
+			| (0x1 << 1)	   /* Increment page first */
+			| 0x1);		   /* Wrap around (default) */
+
+		/* Set LCD mapping */
+		write_reg(par, 0xC0
+			| (0x0 & 0x1) << 2 /* Mirror Y OFF */
+			| (0x0 & 0x1) << 1 /* Mirror X OFF */
+			| (0x0 & 0x1));    /* MS nibble last (default) */
+		break;
+	case 180:
+		/* Set RAM address control */
+		write_reg(par, 0x88
+			| (0x0 & 0x1) << 2 /* Increment positively */
+			| (0x0 & 0x1) << 1 /* Increment column first */
+			| 0x1);		   /* Wrap around (default) */
+
+		/* Set LCD mapping */
+		write_reg(par, 0xC0
+			| (0x1 << 2)	   /* Mirror Y ON */
+			| (0x0 & 0x1) << 1 /* Mirror X OFF */
+			| (0x0 & 0x1));    /* MS nibble last (default) */
+		break;
+	case 270:
+		/* Set RAM address control */
+		write_reg(par, 0x88
+			| (0x0 & 0x1) << 2 /* Increment positively */
+			| (0x1 << 1)	   /* Increment page first */
+			| 0x1);		   /* Wrap around (default) */
+
+		/* Set LCD mapping */
+		write_reg(par, 0xC0
+			| (0x1 << 2)	   /* Mirror Y ON */
+			| (0x1 << 1)	   /* Mirror X ON */
+			| (0x0 & 0x1));    /* MS nibble last (default) */
+		break;
+	default:
+		/* Set RAM address control */
+		write_reg(par, 0x88
+			| (0x0 & 0x1) << 2 /* Increment positively */
+			| (0x0 & 0x1) << 1 /* Increment column first */
+			| 0x1);		   /* Wrap around (default) */
+
+		/* Set LCD mapping */
+		write_reg(par, 0xC0
+			| (0x0 & 0x1) << 2 /* Mirror Y OFF */
+			| (0x1 << 1)	   /* Mirror X ON */
+			| (0x0 & 0x1));    /* MS nibble last (default) */
+		break;
+	}
+
+	return 0;
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u8 *vmem8 = (u8 *)(par->info->screen_buffer);
+	u8 *buf8 = par->txbuf.buf;
+	u16 *buf16 = par->txbuf.buf;
+	int line_length = par->info->fix.line_length;
+	int y_start = offset / line_length;
+	int y_end = (offset + len - 1) / line_length;
+	int x, y, i;
+	int ret = 0;
+
+	switch (par->pdata->display.buswidth) {
+	case 8:
+		switch (par->info->var.rotate) {
+		case 90:
+		case 270:
+			i = y_start * line_length;
+			for (y = y_start; y <= y_end; y++) {
+				for (x = 0; x < line_length; x += 2) {
+					*buf8 = vmem8[i] >> 4;
+					*buf8 |= vmem8[i + 1] & 0xF0;
+					buf8++;
+					i += 2;
+				}
+			}
+			break;
+		default:
+			/* Must be even because pages are two lines */
+			y_start &= 0xFE;
+			i = y_start * line_length;
+			for (y = y_start; y <= y_end; y += 2) {
+				for (x = 0; x < line_length; x++) {
+					*buf8 = vmem8[i] >> 4;
+					*buf8 |= vmem8[i + line_length] & 0xF0;
+					buf8++;
+					i++;
+				}
+				i += line_length;
+			}
+			break;
+		}
+		gpiod_set_value(par->gpio.dc, 1);
+
+		/* Write data */
+		ret = par->fbtftops.write(par, par->txbuf.buf, len / 2);
+		break;
+	case 9:
+		switch (par->info->var.rotate) {
+		case 90:
+		case 270:
+			i = y_start * line_length;
+			for (y = y_start; y <= y_end; y++) {
+				for (x = 0; x < line_length; x += 2) {
+					*buf16 = 0x100;
+					*buf16 |= vmem8[i] >> 4;
+					*buf16 |= vmem8[i + 1] & 0xF0;
+					buf16++;
+					i += 2;
+				}
+			}
+			break;
+		default:
+			/* Must be even because pages are two lines */
+			y_start &= 0xFE;
+			i = y_start * line_length;
+			for (y = y_start; y <= y_end; y += 2) {
+				for (x = 0; x < line_length; x++) {
+					*buf16 = 0x100;
+					*buf16 |= vmem8[i] >> 4;
+					*buf16 |= vmem8[i + line_length] & 0xF0;
+					buf16++;
+					i++;
+				}
+				i += line_length;
+			}
+			break;
+		}
+
+		/* Write data */
+		ret = par->fbtftops.write(par, par->txbuf.buf, len);
+		break;
+	default:
+		dev_err(par->info->device, "unsupported buswidth %d\n",
+			par->pdata->display.buswidth);
+	}
+
+	if (ret < 0)
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
+
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.txbuflen = -1,
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fps = FPS,
+	.fbtftops = {
+		.write_vmem = write_vmem,
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+		.blank = blank,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ultrachip,uc1611", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:uc1611");
+MODULE_ALIAS("platform:uc1611");
+
+MODULE_DESCRIPTION("FB driver for the UC1611 LCD controller");
+MODULE_AUTHOR("Henri Chain");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_uc1701.c b/drivers/staging/fbtft/fb_uc1701.c
new file mode 100644
index 000000000..e4ccc7386
--- /dev/null
+++ b/drivers/staging/fbtft/fb_uc1701.c
@@ -0,0 +1,170 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the UC1701 LCD Controller
+ *
+ * The display is monochrome and the video memory is RGB565.
+ * Any pixel value except 0 turns the pixel on.
+ *
+ * Copyright (C) 2014 Juergen Holzmann
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME	"fb_uc1701"
+#define WIDTH	  102
+#define HEIGHT	 64
+#define PAGES	  (HEIGHT / 8)
+
+/* 1: Display on/off */
+#define LCD_DISPLAY_ENABLE    0xAE
+/* 2: display start line set */
+#define LCD_START_LINE	0x40
+/* 3: Page address set (lower 4 bits select one of the pages) */
+#define LCD_PAGE_ADDRESS      0xB0
+/* 4: column address */
+#define LCD_COL_ADDRESS       0x10
+/* 8: select orientation */
+#define LCD_BOTTOMVIEW	0xA0
+/* 9: inverted display */
+#define LCD_DISPLAY_INVERT    0xA6
+/* 10: show memory content or switch all pixels on */
+#define LCD_ALL_PIXEL	 0xA4
+/* 11: lcd bias set */
+#define LCD_BIAS	      0xA2
+/* 14: Reset Controller */
+#define LCD_RESET_CMD	 0xE2
+/* 15: output mode select (turns display upside-down) */
+#define LCD_SCAN_DIR	  0xC0
+/* 16: power control set */
+#define LCD_POWER_CONTROL     0x28
+/* 17: voltage regulator resistor ratio set */
+#define LCD_VOLTAGE	   0x20
+/* 18: Volume mode set */
+#define LCD_VOLUME_MODE       0x81
+/* 22: NOP command */
+#define LCD_NO_OP	     0xE3
+/* 25: advanced program control */
+#define LCD_ADV_PROG_CTRL     0xFA
+/* 25: advanced program control2 */
+#define LCD_ADV_PROG_CTRL2    0x10
+#define LCD_TEMPCOMP_HIGH     0x80
+/* column offset for normal orientation */
+#define SHIFT_ADDR_NORMAL     0
+/* column offset for bottom view orientation */
+#define SHIFT_ADDR_TOPVIEW    30
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* softreset of LCD */
+	write_reg(par, LCD_RESET_CMD);
+	mdelay(10);
+
+	/* set startpoint */
+	write_reg(par, LCD_START_LINE);
+
+	/* select orientation BOTTOMVIEW */
+	write_reg(par, LCD_BOTTOMVIEW | 1);
+
+	/* output mode select (turns display upside-down) */
+	write_reg(par, LCD_SCAN_DIR | 0x00);
+
+	/* Normal Pixel mode */
+	write_reg(par, LCD_ALL_PIXEL | 0);
+
+	/* positive display */
+	write_reg(par, LCD_DISPLAY_INVERT | 0);
+
+	/* bias 1/9 */
+	write_reg(par, LCD_BIAS | 0);
+
+	/* power control mode: all features on */
+	write_reg(par, LCD_POWER_CONTROL | 0x07);
+
+	/* set voltage regulator R/R */
+	write_reg(par, LCD_VOLTAGE | 0x07);
+
+	/* volume mode set */
+	write_reg(par, LCD_VOLUME_MODE);
+	write_reg(par, 0x09);
+	write_reg(par, LCD_NO_OP);
+
+	/* advanced program control */
+	write_reg(par, LCD_ADV_PROG_CTRL);
+	write_reg(par, LCD_ADV_PROG_CTRL2 | LCD_TEMPCOMP_HIGH);
+
+	/* enable display */
+	write_reg(par, LCD_DISPLAY_ENABLE | 1);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	/* goto address */
+	write_reg(par, LCD_PAGE_ADDRESS);
+	write_reg(par, 0x00);
+	write_reg(par, LCD_COL_ADDRESS);
+}
+
+static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u8 *buf;
+	int x, y, i;
+	int ret = 0;
+
+	for (y = 0; y < PAGES; y++) {
+		buf = par->txbuf.buf;
+		for (x = 0; x < WIDTH; x++) {
+			*buf = 0x00;
+			for (i = 0; i < 8; i++)
+				*buf |= (vmem16[((y * 8 * WIDTH) +
+						 (i * WIDTH)) + x] ?
+					 1 : 0) << i;
+			buf++;
+		}
+
+		write_reg(par, LCD_PAGE_ADDRESS | (u8)y);
+		write_reg(par, 0x00);
+		write_reg(par, LCD_COL_ADDRESS);
+		gpiod_set_value(par->gpio.dc, 1);
+		ret = par->fbtftops.write(par, par->txbuf.buf, WIDTH);
+		gpiod_set_value(par->gpio.dc, 0);
+	}
+
+	if (ret < 0)
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
+
+	return ret;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.write_vmem = write_vmem,
+	},
+	.backlight = 1,
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "UltraChip,uc1701", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("spi:uc1701");
+
+MODULE_DESCRIPTION("FB driver for the UC1701 LCD Controller");
+MODULE_AUTHOR("Juergen Holzmann");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_upd161704.c b/drivers/staging/fbtft/fb_upd161704.c
new file mode 100644
index 000000000..c680160d6
--- /dev/null
+++ b/drivers/staging/fbtft/fb_upd161704.c
@@ -0,0 +1,184 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * FB driver for the uPD161704 LCD Controller
+ *
+ * Copyright (C) 2014 Seong-Woo Kim
+ *
+ * Based on fb_ili9325.c by Noralf Tronnes
+ * Based on ili9325.c by Jeroen Domburg
+ * Init code from UTFT library by Henning Karlsen
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_upd161704"
+#define WIDTH		240
+#define HEIGHT		320
+#define BPP		16
+
+static int init_display(struct fbtft_par *par)
+{
+	par->fbtftops.reset(par);
+
+	/* Initialization sequence from Lib_UTFT */
+
+	/* register reset */
+	write_reg(par, 0x0003, 0x0001);	/* Soft reset */
+
+	/* oscillator start */
+	write_reg(par, 0x003A, 0x0001);	/*Oscillator 0: stop, 1: operation */
+	udelay(100);
+
+	/* y-setting */
+	write_reg(par, 0x0024, 0x007B);	/* amplitude setting */
+	udelay(10);
+	write_reg(par, 0x0025, 0x003B);	/* amplitude setting */
+	write_reg(par, 0x0026, 0x0034);	/* amplitude setting */
+	udelay(10);
+	write_reg(par, 0x0027, 0x0004);	/* amplitude setting */
+	write_reg(par, 0x0052, 0x0025);	/* circuit setting 1 */
+	udelay(10);
+	write_reg(par, 0x0053, 0x0033);	/* circuit setting 2 */
+	write_reg(par, 0x0061, 0x001C);	/* adjustment V10 positive polarity */
+	udelay(10);
+	write_reg(par, 0x0062, 0x002C);	/* adjustment V9 negative polarity */
+	write_reg(par, 0x0063, 0x0022);	/* adjustment V34 positive polarity */
+	udelay(10);
+	write_reg(par, 0x0064, 0x0027);	/* adjustment V31 negative polarity */
+	udelay(10);
+	write_reg(par, 0x0065, 0x0014);	/* adjustment V61 negative polarity */
+	udelay(10);
+	write_reg(par, 0x0066, 0x0010);	/* adjustment V61 negative polarity */
+
+	/* Basical clock for 1 line (BASECOUNT[7:0]) number specified */
+	write_reg(par, 0x002E, 0x002D);
+
+	/* Power supply setting */
+	write_reg(par, 0x0019, 0x0000);	/* DC/DC output setting */
+	udelay(200);
+	write_reg(par, 0x001A, 0x1000);	/* DC/DC frequency setting */
+	write_reg(par, 0x001B, 0x0023);	/* DC/DC rising setting */
+	write_reg(par, 0x001C, 0x0C01);	/* Regulator voltage setting */
+	write_reg(par, 0x001D, 0x0000);	/* Regulator current setting */
+	write_reg(par, 0x001E, 0x0009);	/* VCOM output setting */
+	write_reg(par, 0x001F, 0x0035);	/* VCOM amplitude setting */
+	write_reg(par, 0x0020, 0x0015);	/* VCOMM cencter setting */
+	write_reg(par, 0x0018, 0x1E7B);	/* DC/DC operation setting */
+
+	/* windows setting */
+	write_reg(par, 0x0008, 0x0000);	/* Minimum X address */
+	write_reg(par, 0x0009, 0x00EF);	/* Maximum X address */
+	write_reg(par, 0x000a, 0x0000);	/* Minimum Y address */
+	write_reg(par, 0x000b, 0x013F);	/* Maximum Y address */
+
+	/* LCD display area setting */
+	write_reg(par, 0x0029, 0x0000);	/* [LCDSIZE]  X MIN. size set */
+	write_reg(par, 0x002A, 0x0000);	/* [LCDSIZE]  Y MIN. size set */
+	write_reg(par, 0x002B, 0x00EF);	/* [LCDSIZE]  X MAX. size set */
+	write_reg(par, 0x002C, 0x013F);	/* [LCDSIZE]  Y MAX. size set */
+
+	/* Gate scan setting */
+	write_reg(par, 0x0032, 0x0002);
+
+	/* n line inversion line number */
+	write_reg(par, 0x0033, 0x0000);
+
+	/* Line inversion/frame inversion/interlace setting */
+	write_reg(par, 0x0037, 0x0000);
+
+	/* Gate scan operation setting register */
+	write_reg(par, 0x003B, 0x0001);
+
+	/* Color mode */
+	/*GS = 0: 260-k color (64 gray scale), GS = 1: 8 color (2 gray scale) */
+	write_reg(par, 0x0004, 0x0000);
+
+	/* RAM control register */
+	write_reg(par, 0x0005, 0x0000);	/*Window access 00:Normal, 10:Window */
+
+	/* Display setting register 2 */
+	write_reg(par, 0x0001, 0x0000);
+
+	/* display setting */
+	write_reg(par, 0x0000, 0x0000);	/* display on */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	switch (par->info->var.rotate) {
+	/* R20h = Horizontal GRAM Start Address */
+	/* R21h = Vertical GRAM Start Address */
+	case 0:
+		write_reg(par, 0x0006, xs);
+		write_reg(par, 0x0007, ys);
+		break;
+	case 180:
+		write_reg(par, 0x0006, WIDTH - 1 - xs);
+		write_reg(par, 0x0007, HEIGHT - 1 - ys);
+		break;
+	case 270:
+		write_reg(par, 0x0006, WIDTH - 1 - ys);
+		write_reg(par, 0x0007, xs);
+		break;
+	case 90:
+		write_reg(par, 0x0006, ys);
+		write_reg(par, 0x0007, HEIGHT - 1 - xs);
+		break;
+	}
+
+	write_reg(par, 0x0e); /* Write Data to GRAM */
+}
+
+static int set_var(struct fbtft_par *par)
+{
+	switch (par->info->var.rotate) {
+	/* AM: GRAM update direction */
+	case 0:
+		write_reg(par, 0x01, 0x0000);
+		write_reg(par, 0x05, 0x0000);
+		break;
+	case 180:
+		write_reg(par, 0x01, 0x00C0);
+		write_reg(par, 0x05, 0x0000);
+		break;
+	case 270:
+		write_reg(par, 0x01, 0x0080);
+		write_reg(par, 0x05, 0x0001);
+		break;
+	case 90:
+		write_reg(par, 0x01, 0x0040);
+		write_reg(par, 0x05, 0x0001);
+		break;
+	}
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 16,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "nec,upd161704", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:upd161704");
+MODULE_ALIAS("platform:upd161704");
+
+MODULE_DESCRIPTION("FB driver for the uPD161704 LCD Controller");
+MODULE_AUTHOR("Seong-Woo Kim");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fbtft-bus.c b/drivers/staging/fbtft/fbtft-bus.c
new file mode 100644
index 000000000..3d422bc11
--- /dev/null
+++ b/drivers/staging/fbtft/fbtft-bus.c
@@ -0,0 +1,243 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/export.h>
+#include <linux/errno.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include "fbtft.h"
+
+/*****************************************************************************
+ *
+ *   void (*write_reg)(struct fbtft_par *par, int len, ...);
+ *
+ *****************************************************************************/
+
+#define define_fbtft_write_reg(func, buffer_type, data_type, modifier)        \
+void func(struct fbtft_par *par, int len, ...)                                \
+{                                                                             \
+	va_list args;                                                         \
+	int i, ret;                                                           \
+	int offset = 0;                                                       \
+	buffer_type *buf = (buffer_type *)par->buf;                           \
+									      \
+	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {                    \
+		va_start(args, len);                                          \
+		for (i = 0; i < len; i++) {                                   \
+			buf[i] = modifier((data_type)va_arg(args,             \
+							    unsigned int));   \
+		}                                                             \
+		va_end(args);                                                 \
+		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par,                  \
+				  par->info->device, buffer_type, buf, len,   \
+				  "%s: ", __func__);                          \
+	}                                                                     \
+									      \
+	va_start(args, len);                                                  \
+									      \
+	if (par->startbyte) {                                                 \
+		*(u8 *)par->buf = par->startbyte;                             \
+		buf = (buffer_type *)(par->buf + 1);                          \
+		offset = 1;                                                   \
+	}                                                                     \
+									      \
+	*buf = modifier((data_type)va_arg(args, unsigned int));               \
+	ret = fbtft_write_buf_dc(par, par->buf, sizeof(data_type) + offset,   \
+				 0);                                          \
+	if (ret < 0)							      \
+		goto out;						      \
+	len--;                                                                \
+									      \
+	if (par->startbyte)                                                   \
+		*(u8 *)par->buf = par->startbyte | 0x2;                       \
+									      \
+	if (len) {                                                            \
+		i = len;                                                      \
+		while (i--)						      \
+			*buf++ = modifier((data_type)va_arg(args,             \
+							    unsigned int));   \
+		fbtft_write_buf_dc(par, par->buf,			      \
+				   len * (sizeof(data_type) + offset), 1);    \
+	}                                                                     \
+out:									      \
+	va_end(args);                                                         \
+}                                                                             \
+EXPORT_SYMBOL(func);
+
+define_fbtft_write_reg(fbtft_write_reg8_bus8, u8, u8, )
+define_fbtft_write_reg(fbtft_write_reg16_bus8, __be16, u16, cpu_to_be16)
+define_fbtft_write_reg(fbtft_write_reg16_bus16, u16, u16, )
+
+void fbtft_write_reg8_bus9(struct fbtft_par *par, int len, ...)
+{
+	va_list args;
+	int i, ret;
+	int pad = 0;
+	u16 *buf = (u16 *)par->buf;
+
+	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {
+		va_start(args, len);
+		for (i = 0; i < len; i++)
+			*(((u8 *)buf) + i) = (u8)va_arg(args, unsigned int);
+		va_end(args);
+		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par,
+				  par->info->device, u8, buf, len, "%s: ",
+				  __func__);
+	}
+	if (len <= 0)
+		return;
+
+	if (par->spi && (par->spi->bits_per_word == 8)) {
+		/* we're emulating 9-bit, pad start of buffer with no-ops
+		 * (assuming here that zero is a no-op)
+		 */
+		pad = (len % 4) ? 4 - (len % 4) : 0;
+		for (i = 0; i < pad; i++)
+			*buf++ = 0x000;
+	}
+
+	va_start(args, len);
+	*buf++ = (u8)va_arg(args, unsigned int);
+	i = len - 1;
+	while (i--) {
+		*buf = (u8)va_arg(args, unsigned int);
+		*buf++ |= 0x100; /* dc=1 */
+	}
+	va_end(args);
+	ret = par->fbtftops.write(par, par->buf, (len + pad) * sizeof(u16));
+	if (ret < 0) {
+		dev_err(par->info->device,
+			"write() failed and returned %d\n", ret);
+		return;
+	}
+}
+EXPORT_SYMBOL(fbtft_write_reg8_bus9);
+
+/*****************************************************************************
+ *
+ *   int (*write_vmem)(struct fbtft_par *par);
+ *
+ *****************************************************************************/
+
+/* 16 bit pixel over 8-bit databus */
+int fbtft_write_vmem16_bus8(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16;
+	__be16 *txbuf16 = par->txbuf.buf;
+	size_t remain;
+	size_t to_copy;
+	size_t tx_array_size;
+	int i;
+	int ret = 0;
+	size_t startbyte_size = 0;
+
+	fbtft_par_dbg(DEBUG_WRITE_VMEM, par, "%s(offset=%zu, len=%zu)\n",
+		      __func__, offset, len);
+
+	remain = len / 2;
+	vmem16 = (u16 *)(par->info->screen_buffer + offset);
+
+	gpiod_set_value(par->gpio.dc, 1);
+
+	/* non buffered write */
+	if (!par->txbuf.buf)
+		return par->fbtftops.write(par, vmem16, len);
+
+	/* buffered write */
+	tx_array_size = par->txbuf.len / 2;
+
+	if (par->startbyte) {
+		txbuf16 = par->txbuf.buf + 1;
+		tx_array_size -= 2;
+		*(u8 *)(par->txbuf.buf) = par->startbyte | 0x2;
+		startbyte_size = 1;
+	}
+
+	while (remain) {
+		to_copy = min(tx_array_size, remain);
+		dev_dbg(par->info->device, "to_copy=%zu, remain=%zu\n",
+			to_copy, remain - to_copy);
+
+		for (i = 0; i < to_copy; i++)
+			txbuf16[i] = cpu_to_be16(vmem16[i]);
+
+		vmem16 = vmem16 + to_copy;
+		ret = par->fbtftops.write(par, par->txbuf.buf,
+						startbyte_size + to_copy * 2);
+		if (ret < 0)
+			return ret;
+		remain -= to_copy;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_write_vmem16_bus8);
+
+/* 16 bit pixel over 9-bit SPI bus: dc + high byte, dc + low byte */
+int fbtft_write_vmem16_bus9(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u8 *vmem8;
+	u16 *txbuf16 = par->txbuf.buf;
+	size_t remain;
+	size_t to_copy;
+	size_t tx_array_size;
+	int i;
+	int ret = 0;
+
+	fbtft_par_dbg(DEBUG_WRITE_VMEM, par, "%s(offset=%zu, len=%zu)\n",
+		      __func__, offset, len);
+
+	if (!par->txbuf.buf) {
+		dev_err(par->info->device, "%s: txbuf.buf is NULL\n", __func__);
+		return -1;
+	}
+
+	remain = len;
+	vmem8 = par->info->screen_buffer + offset;
+
+	tx_array_size = par->txbuf.len / 2;
+
+	while (remain) {
+		to_copy = min(tx_array_size, remain);
+		dev_dbg(par->info->device, "to_copy=%zu, remain=%zu\n",
+			to_copy, remain - to_copy);
+
+#ifdef __LITTLE_ENDIAN
+		for (i = 0; i < to_copy; i += 2) {
+			txbuf16[i]     = 0x0100 | vmem8[i + 1];
+			txbuf16[i + 1] = 0x0100 | vmem8[i];
+		}
+#else
+		for (i = 0; i < to_copy; i++)
+			txbuf16[i]   = 0x0100 | vmem8[i];
+#endif
+		vmem8 = vmem8 + to_copy;
+		ret = par->fbtftops.write(par, par->txbuf.buf, to_copy * 2);
+		if (ret < 0)
+			return ret;
+		remain -= to_copy;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_write_vmem16_bus9);
+
+int fbtft_write_vmem8_bus8(struct fbtft_par *par, size_t offset, size_t len)
+{
+	dev_err(par->info->device, "%s: function not implemented\n", __func__);
+	return -1;
+}
+EXPORT_SYMBOL(fbtft_write_vmem8_bus8);
+
+/* 16 bit pixel over 16-bit databus */
+int fbtft_write_vmem16_bus16(struct fbtft_par *par, size_t offset, size_t len)
+{
+	u16 *vmem16;
+
+	fbtft_par_dbg(DEBUG_WRITE_VMEM, par, "%s(offset=%zu, len=%zu)\n",
+		      __func__, offset, len);
+
+	vmem16 = (u16 *)(par->info->screen_buffer + offset);
+
+	/* no need for buffered write with 16-bit bus */
+	return fbtft_write_buf_dc(par, vmem16, len, 1);
+}
+EXPORT_SYMBOL(fbtft_write_vmem16_bus16);
diff --git a/drivers/staging/fbtft/fbtft-core.c b/drivers/staging/fbtft/fbtft-core.c
new file mode 100644
index 000000000..afaba94d1
--- /dev/null
+++ b/drivers/staging/fbtft/fbtft-core.c
@@ -0,0 +1,1327 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2013 Noralf Tronnes
+ *
+ * This driver is inspired by:
+ *   st7735fb.c, Copyright (C) 2011, Matt Porter
+ *   broadsheetfb.c, Copyright (C) 2008, Jaya Kumar
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/fb.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/backlight.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/spinlock.h>
+
+#include <video/mipi_display.h>
+
+#include "fbtft.h"
+#include "internal.h"
+
+static unsigned long debug;
+module_param(debug, ulong, 0000);
+MODULE_PARM_DESC(debug, "override device debug level");
+
+int fbtft_write_buf_dc(struct fbtft_par *par, void *buf, size_t len, int dc)
+{
+	int ret;
+
+	gpiod_set_value(par->gpio.dc, dc);
+
+	ret = par->fbtftops.write(par, buf, len);
+	if (ret < 0)
+		dev_err(par->info->device,
+			"write() failed and returned %d\n", ret);
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_write_buf_dc);
+
+void fbtft_dbg_hex(const struct device *dev, int groupsize,
+		   const void *buf, size_t len, const char *fmt, ...)
+{
+	va_list args;
+	static char textbuf[512];
+	char *text = textbuf;
+	size_t text_len;
+
+	va_start(args, fmt);
+	text_len = vscnprintf(text, sizeof(textbuf), fmt, args);
+	va_end(args);
+
+	hex_dump_to_buffer(buf, len, 32, groupsize, text + text_len,
+			   512 - text_len, false);
+
+	if (len > 32)
+		dev_info(dev, "%s ...\n", text);
+	else
+		dev_info(dev, "%s\n", text);
+}
+EXPORT_SYMBOL(fbtft_dbg_hex);
+
+static int fbtft_request_one_gpio(struct fbtft_par *par,
+				  const char *name, int index,
+				  struct gpio_desc **gpiop)
+{
+	struct device *dev = par->info->device;
+
+	*gpiop = devm_gpiod_get_index_optional(dev, name, index,
+					       GPIOD_OUT_LOW);
+	if (IS_ERR(*gpiop))
+		return dev_err_probe(dev, PTR_ERR(*gpiop), "Failed to request %s GPIO\n", name);
+
+	fbtft_par_dbg(DEBUG_REQUEST_GPIOS, par, "%s: '%s' GPIO\n",
+		      __func__, name);
+
+	return 0;
+}
+
+static int fbtft_request_gpios(struct fbtft_par *par)
+{
+	int i;
+	int ret;
+
+	ret = fbtft_request_one_gpio(par, "reset", 0, &par->gpio.reset);
+	if (ret)
+		return ret;
+	ret = fbtft_request_one_gpio(par, "dc", 0, &par->gpio.dc);
+	if (ret)
+		return ret;
+	ret = fbtft_request_one_gpio(par, "rd", 0, &par->gpio.rd);
+	if (ret)
+		return ret;
+	ret = fbtft_request_one_gpio(par, "wr", 0, &par->gpio.wr);
+	if (ret)
+		return ret;
+	ret = fbtft_request_one_gpio(par, "cs", 0, &par->gpio.cs);
+	if (ret)
+		return ret;
+	ret = fbtft_request_one_gpio(par, "latch", 0, &par->gpio.latch);
+	if (ret)
+		return ret;
+	for (i = 0; i < 16; i++) {
+		ret = fbtft_request_one_gpio(par, "db", i,
+					     &par->gpio.db[i]);
+		if (ret)
+			return ret;
+		ret = fbtft_request_one_gpio(par, "led", i,
+					     &par->gpio.led[i]);
+		if (ret)
+			return ret;
+		ret = fbtft_request_one_gpio(par, "aux", i,
+					     &par->gpio.aux[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int fbtft_backlight_update_status(struct backlight_device *bd)
+{
+	struct fbtft_par *par = bl_get_data(bd);
+	bool polarity = par->polarity;
+
+	fbtft_par_dbg(DEBUG_BACKLIGHT, par,
+		      "%s: polarity=%d, power=%d, fb_blank=%d\n",
+		      __func__, polarity, bd->props.power, bd->props.fb_blank);
+
+	if (!backlight_is_blank(bd))
+		gpiod_set_value(par->gpio.led[0], polarity);
+	else
+		gpiod_set_value(par->gpio.led[0], !polarity);
+
+	return 0;
+}
+
+static int fbtft_backlight_get_brightness(struct backlight_device *bd)
+{
+	return bd->props.brightness;
+}
+
+void fbtft_unregister_backlight(struct fbtft_par *par)
+{
+	if (par->info->bl_dev) {
+		par->info->bl_dev->props.power = FB_BLANK_POWERDOWN;
+		backlight_update_status(par->info->bl_dev);
+		backlight_device_unregister(par->info->bl_dev);
+		par->info->bl_dev = NULL;
+	}
+}
+EXPORT_SYMBOL(fbtft_unregister_backlight);
+
+static const struct backlight_ops fbtft_bl_ops = {
+	.get_brightness	= fbtft_backlight_get_brightness,
+	.update_status	= fbtft_backlight_update_status,
+};
+
+void fbtft_register_backlight(struct fbtft_par *par)
+{
+	struct backlight_device *bd;
+	struct backlight_properties bl_props = { 0, };
+
+	if (!par->gpio.led[0]) {
+		fbtft_par_dbg(DEBUG_BACKLIGHT, par,
+			      "%s(): led pin not set, exiting.\n", __func__);
+		return;
+	}
+
+	bl_props.type = BACKLIGHT_RAW;
+	/* Assume backlight is off, get polarity from current state of pin */
+	bl_props.power = FB_BLANK_POWERDOWN;
+	if (!gpiod_get_value(par->gpio.led[0]))
+		par->polarity = true;
+
+	bd = backlight_device_register(dev_driver_string(par->info->device),
+				       par->info->device, par,
+				       &fbtft_bl_ops, &bl_props);
+	if (IS_ERR(bd)) {
+		dev_err(par->info->device,
+			"cannot register backlight device (%ld)\n",
+			PTR_ERR(bd));
+		return;
+	}
+	par->info->bl_dev = bd;
+
+	if (!par->fbtftops.unregister_backlight)
+		par->fbtftops.unregister_backlight = fbtft_unregister_backlight;
+}
+EXPORT_SYMBOL(fbtft_register_backlight);
+
+static void fbtft_set_addr_win(struct fbtft_par *par, int xs, int ys, int xe,
+			       int ye)
+{
+	write_reg(par, MIPI_DCS_SET_COLUMN_ADDRESS,
+		  (xs >> 8) & 0xFF, xs & 0xFF, (xe >> 8) & 0xFF, xe & 0xFF);
+
+	write_reg(par, MIPI_DCS_SET_PAGE_ADDRESS,
+		  (ys >> 8) & 0xFF, ys & 0xFF, (ye >> 8) & 0xFF, ye & 0xFF);
+
+	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
+}
+
+static void fbtft_reset(struct fbtft_par *par)
+{
+	if (!par->gpio.reset)
+		return;
+
+	fbtft_par_dbg(DEBUG_RESET, par, "%s()\n", __func__);
+
+	gpiod_set_value_cansleep(par->gpio.reset, 1);
+	usleep_range(20, 40);
+	gpiod_set_value_cansleep(par->gpio.reset, 0);
+	msleep(120);
+
+	gpiod_set_value_cansleep(par->gpio.cs, 1);  /* Activate chip */
+}
+
+static void fbtft_update_display(struct fbtft_par *par, unsigned int start_line,
+				 unsigned int end_line)
+{
+	size_t offset, len;
+	ktime_t ts_start, ts_end;
+	long fps, throughput;
+	bool timeit = false;
+	int ret = 0;
+
+	if (unlikely(par->debug & (DEBUG_TIME_FIRST_UPDATE |
+			DEBUG_TIME_EACH_UPDATE))) {
+		if ((par->debug & DEBUG_TIME_EACH_UPDATE) ||
+		    ((par->debug & DEBUG_TIME_FIRST_UPDATE) &&
+		    !par->first_update_done)) {
+			ts_start = ktime_get();
+			timeit = true;
+		}
+	}
+
+	/* Sanity checks */
+	if (start_line > end_line) {
+		dev_warn(par->info->device,
+			 "%s: start_line=%u is larger than end_line=%u. Shouldn't happen, will do full display update\n",
+			 __func__, start_line, end_line);
+		start_line = 0;
+		end_line = par->info->var.yres - 1;
+	}
+	if (start_line > par->info->var.yres - 1 ||
+	    end_line > par->info->var.yres - 1) {
+		dev_warn(par->info->device,
+			 "%s: start_line=%u or end_line=%u is larger than max=%d. Shouldn't happen, will do full display update\n",
+			 __func__, start_line,
+			 end_line, par->info->var.yres - 1);
+		start_line = 0;
+		end_line = par->info->var.yres - 1;
+	}
+
+	fbtft_par_dbg(DEBUG_UPDATE_DISPLAY, par, "%s(start_line=%u, end_line=%u)\n",
+		      __func__, start_line, end_line);
+
+	if (par->fbtftops.set_addr_win)
+		par->fbtftops.set_addr_win(par, 0, start_line,
+				par->info->var.xres - 1, end_line);
+
+	offset = start_line * par->info->fix.line_length;
+	len = (end_line - start_line + 1) * par->info->fix.line_length;
+	ret = par->fbtftops.write_vmem(par, offset, len);
+	if (ret < 0)
+		dev_err(par->info->device,
+			"%s: write_vmem failed to update display buffer\n",
+			__func__);
+
+	if (unlikely(timeit)) {
+		ts_end = ktime_get();
+		if (!ktime_to_ns(par->update_time))
+			par->update_time = ts_start;
+
+		fps = ktime_us_delta(ts_start, par->update_time);
+		par->update_time = ts_start;
+		fps = fps ? 1000000 / fps : 0;
+
+		throughput = ktime_us_delta(ts_end, ts_start);
+		throughput = throughput ? (len * 1000) / throughput : 0;
+		throughput = throughput * 1000 / 1024;
+
+		dev_info(par->info->device,
+			 "Display update: %ld kB/s, fps=%ld\n",
+			 throughput, fps);
+		par->first_update_done = true;
+	}
+}
+
+static void fbtft_mkdirty(struct fb_info *info, int y, int height)
+{
+	struct fbtft_par *par = info->par;
+	struct fb_deferred_io *fbdefio = info->fbdefio;
+
+	/* special case, needed ? */
+	if (y == -1) {
+		y = 0;
+		height = info->var.yres;
+	}
+
+	/* Mark display lines/area as dirty */
+	spin_lock(&par->dirty_lock);
+	if (y < par->dirty_lines_start)
+		par->dirty_lines_start = y;
+	if (y + height - 1 > par->dirty_lines_end)
+		par->dirty_lines_end = y + height - 1;
+	spin_unlock(&par->dirty_lock);
+
+	/* Schedule deferred_io to update display (no-op if already on queue)*/
+	schedule_delayed_work(&info->deferred_work, fbdefio->delay);
+}
+
+static void fbtft_deferred_io(struct fb_info *info, struct list_head *pagereflist)
+{
+	struct fbtft_par *par = info->par;
+	unsigned int dirty_lines_start, dirty_lines_end;
+	struct fb_deferred_io_pageref *pageref;
+	unsigned int y_low = 0, y_high = 0;
+	int count = 0;
+
+	spin_lock(&par->dirty_lock);
+	dirty_lines_start = par->dirty_lines_start;
+	dirty_lines_end = par->dirty_lines_end;
+	/* set display line markers as clean */
+	par->dirty_lines_start = par->info->var.yres - 1;
+	par->dirty_lines_end = 0;
+	spin_unlock(&par->dirty_lock);
+
+	/* Mark display lines as dirty */
+	list_for_each_entry(pageref, pagereflist, list) {
+		count++;
+		y_low = pageref->offset / info->fix.line_length;
+		y_high = (pageref->offset + PAGE_SIZE - 1) / info->fix.line_length;
+		dev_dbg(info->device,
+			"page->index=%lu y_low=%d y_high=%d\n",
+			pageref->page->index, y_low, y_high);
+		if (y_high > info->var.yres - 1)
+			y_high = info->var.yres - 1;
+		if (y_low < dirty_lines_start)
+			dirty_lines_start = y_low;
+		if (y_high > dirty_lines_end)
+			dirty_lines_end = y_high;
+	}
+
+	par->fbtftops.update_display(info->par,
+					dirty_lines_start, dirty_lines_end);
+}
+
+static void fbtft_fb_fillrect(struct fb_info *info,
+			      const struct fb_fillrect *rect)
+{
+	struct fbtft_par *par = info->par;
+
+	dev_dbg(info->dev,
+		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
+		__func__, rect->dx, rect->dy, rect->width, rect->height);
+	sys_fillrect(info, rect);
+
+	par->fbtftops.mkdirty(info, rect->dy, rect->height);
+}
+
+static void fbtft_fb_copyarea(struct fb_info *info,
+			      const struct fb_copyarea *area)
+{
+	struct fbtft_par *par = info->par;
+
+	dev_dbg(info->dev,
+		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
+		__func__,  area->dx, area->dy, area->width, area->height);
+	sys_copyarea(info, area);
+
+	par->fbtftops.mkdirty(info, area->dy, area->height);
+}
+
+static void fbtft_fb_imageblit(struct fb_info *info,
+			       const struct fb_image *image)
+{
+	struct fbtft_par *par = info->par;
+
+	dev_dbg(info->dev,
+		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
+		__func__,  image->dx, image->dy, image->width, image->height);
+	sys_imageblit(info, image);
+
+	par->fbtftops.mkdirty(info, image->dy, image->height);
+}
+
+static ssize_t fbtft_fb_write(struct fb_info *info, const char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	struct fbtft_par *par = info->par;
+	ssize_t res;
+
+	dev_dbg(info->dev,
+		"%s: count=%zd, ppos=%llu\n", __func__,  count, *ppos);
+	res = fb_sys_write(info, buf, count, ppos);
+
+	/* TODO: only mark changed area update all for now */
+	par->fbtftops.mkdirty(info, -1, 0);
+
+	return res;
+}
+
+/* from pxafb.c */
+static unsigned int chan_to_field(unsigned int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int fbtft_fb_setcolreg(unsigned int regno, unsigned int red,
+			      unsigned int green, unsigned int blue,
+			      unsigned int transp, struct fb_info *info)
+{
+	unsigned int val;
+	int ret = 1;
+
+	dev_dbg(info->dev,
+		"%s(regno=%u, red=0x%X, green=0x%X, blue=0x%X, trans=0x%X)\n",
+		__func__, regno, red, green, blue, transp);
+
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		if (regno < 16) {
+			u32 *pal = info->pseudo_palette;
+
+			val  = chan_to_field(red,   &info->var.red);
+			val |= chan_to_field(green, &info->var.green);
+			val |= chan_to_field(blue,  &info->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+	}
+	return ret;
+}
+
+static int fbtft_fb_blank(int blank, struct fb_info *info)
+{
+	struct fbtft_par *par = info->par;
+	int ret = -EINVAL;
+
+	dev_dbg(info->dev, "%s(blank=%d)\n",
+		__func__, blank);
+
+	if (!par->fbtftops.blank)
+		return ret;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		ret = par->fbtftops.blank(par, true);
+		break;
+	case FB_BLANK_UNBLANK:
+		ret = par->fbtftops.blank(par, false);
+		break;
+	}
+	return ret;
+}
+
+static void fbtft_merge_fbtftops(struct fbtft_ops *dst, struct fbtft_ops *src)
+{
+	if (src->write)
+		dst->write = src->write;
+	if (src->read)
+		dst->read = src->read;
+	if (src->write_vmem)
+		dst->write_vmem = src->write_vmem;
+	if (src->write_register)
+		dst->write_register = src->write_register;
+	if (src->set_addr_win)
+		dst->set_addr_win = src->set_addr_win;
+	if (src->reset)
+		dst->reset = src->reset;
+	if (src->mkdirty)
+		dst->mkdirty = src->mkdirty;
+	if (src->update_display)
+		dst->update_display = src->update_display;
+	if (src->init_display)
+		dst->init_display = src->init_display;
+	if (src->blank)
+		dst->blank = src->blank;
+	if (src->request_gpios_match)
+		dst->request_gpios_match = src->request_gpios_match;
+	if (src->request_gpios)
+		dst->request_gpios = src->request_gpios;
+	if (src->verify_gpios)
+		dst->verify_gpios = src->verify_gpios;
+	if (src->register_backlight)
+		dst->register_backlight = src->register_backlight;
+	if (src->unregister_backlight)
+		dst->unregister_backlight = src->unregister_backlight;
+	if (src->set_var)
+		dst->set_var = src->set_var;
+	if (src->set_gamma)
+		dst->set_gamma = src->set_gamma;
+}
+
+/**
+ * fbtft_framebuffer_alloc - creates a new frame buffer info structure
+ *
+ * @display: pointer to structure describing the display
+ * @dev: pointer to the device for this fb, this can be NULL
+ * @pdata: platform data for the display in use
+ *
+ * Creates a new frame buffer info structure.
+ *
+ * Also creates and populates the following structures:
+ *   info->fbops
+ *   info->fbdefio
+ *   info->pseudo_palette
+ *   par->fbtftops
+ *   par->txbuf
+ *
+ * Returns the new structure, or NULL if an error occurred.
+ *
+ */
+struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
+					struct device *dev,
+					struct fbtft_platform_data *pdata)
+{
+	struct fb_info *info;
+	struct fbtft_par *par;
+	struct fb_ops *fbops = NULL;
+	struct fb_deferred_io *fbdefio = NULL;
+	u8 *vmem = NULL;
+	void *txbuf = NULL;
+	void *buf = NULL;
+	unsigned int width;
+	unsigned int height;
+	int txbuflen = display->txbuflen;
+	unsigned int bpp = display->bpp;
+	unsigned int fps = display->fps;
+	int vmem_size;
+	const s16 *init_sequence = display->init_sequence;
+	char *gamma = display->gamma;
+	u32 *gamma_curves = NULL;
+
+	/* sanity check */
+	if (display->gamma_num * display->gamma_len >
+			FBTFT_GAMMA_MAX_VALUES_TOTAL) {
+		dev_err(dev, "FBTFT_GAMMA_MAX_VALUES_TOTAL=%d is exceeded\n",
+			FBTFT_GAMMA_MAX_VALUES_TOTAL);
+		return NULL;
+	}
+
+	/* defaults */
+	if (!fps)
+		fps = 20;
+	if (!bpp)
+		bpp = 16;
+
+	if (!pdata) {
+		dev_err(dev, "platform data is missing\n");
+		return NULL;
+	}
+
+	/* override driver values? */
+	if (pdata->fps)
+		fps = pdata->fps;
+	if (pdata->txbuflen)
+		txbuflen = pdata->txbuflen;
+	if (pdata->display.init_sequence)
+		init_sequence = pdata->display.init_sequence;
+	if (pdata->gamma)
+		gamma = pdata->gamma;
+	if (pdata->display.debug)
+		display->debug = pdata->display.debug;
+	if (pdata->display.backlight)
+		display->backlight = pdata->display.backlight;
+	if (pdata->display.width)
+		display->width = pdata->display.width;
+	if (pdata->display.height)
+		display->height = pdata->display.height;
+	if (pdata->display.buswidth)
+		display->buswidth = pdata->display.buswidth;
+	if (pdata->display.regwidth)
+		display->regwidth = pdata->display.regwidth;
+
+	display->debug |= debug;
+	fbtft_expand_debug_value(&display->debug);
+
+	switch (pdata->rotate) {
+	case 90:
+	case 270:
+		width =  display->height;
+		height = display->width;
+		break;
+	default:
+		width =  display->width;
+		height = display->height;
+	}
+
+	vmem_size = display->width * display->height * bpp / 8;
+	vmem = vzalloc(vmem_size);
+	if (!vmem)
+		goto alloc_fail;
+
+	fbops = devm_kzalloc(dev, sizeof(struct fb_ops), GFP_KERNEL);
+	if (!fbops)
+		goto alloc_fail;
+
+	fbdefio = devm_kzalloc(dev, sizeof(struct fb_deferred_io), GFP_KERNEL);
+	if (!fbdefio)
+		goto alloc_fail;
+
+	buf = devm_kzalloc(dev, 128, GFP_KERNEL);
+	if (!buf)
+		goto alloc_fail;
+
+	if (display->gamma_num && display->gamma_len) {
+		gamma_curves = devm_kcalloc(dev,
+					    display->gamma_num *
+					    display->gamma_len,
+					    sizeof(gamma_curves[0]),
+					    GFP_KERNEL);
+		if (!gamma_curves)
+			goto alloc_fail;
+	}
+
+	info = framebuffer_alloc(sizeof(struct fbtft_par), dev);
+	if (!info)
+		goto alloc_fail;
+
+	info->screen_buffer = vmem;
+	info->fbops = fbops;
+	info->fbdefio = fbdefio;
+
+	fbops->owner        =      dev->driver->owner;
+	fbops->fb_read      =      fb_sys_read;
+	fbops->fb_write     =      fbtft_fb_write;
+	fbops->fb_fillrect  =      fbtft_fb_fillrect;
+	fbops->fb_copyarea  =      fbtft_fb_copyarea;
+	fbops->fb_imageblit =      fbtft_fb_imageblit;
+	fbops->fb_setcolreg =      fbtft_fb_setcolreg;
+	fbops->fb_blank     =      fbtft_fb_blank;
+	fbops->fb_mmap      =      fb_deferred_io_mmap;
+
+	fbdefio->delay =            HZ / fps;
+	fbdefio->sort_pagereflist = true;
+	fbdefio->deferred_io =      fbtft_deferred_io;
+
+	snprintf(info->fix.id, sizeof(info->fix.id), "%s", dev->driver->name);
+	info->fix.type =           FB_TYPE_PACKED_PIXELS;
+	info->fix.visual =         FB_VISUAL_TRUECOLOR;
+	info->fix.xpanstep =	   0;
+	info->fix.ypanstep =	   0;
+	info->fix.ywrapstep =	   0;
+	info->fix.line_length =    width * bpp / 8;
+	info->fix.accel =          FB_ACCEL_NONE;
+	info->fix.smem_len =       vmem_size;
+	fb_deferred_io_init(info);
+
+	info->var.rotate =         pdata->rotate;
+	info->var.xres =           width;
+	info->var.yres =           height;
+	info->var.xres_virtual =   info->var.xres;
+	info->var.yres_virtual =   info->var.yres;
+	info->var.bits_per_pixel = bpp;
+	info->var.nonstd =         1;
+
+	/* RGB565 */
+	info->var.red.offset =     11;
+	info->var.red.length =     5;
+	info->var.green.offset =   5;
+	info->var.green.length =   6;
+	info->var.blue.offset =    0;
+	info->var.blue.length =    5;
+	info->var.transp.offset =  0;
+	info->var.transp.length =  0;
+
+	info->flags =              FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
+
+	par = info->par;
+	par->info = info;
+	par->pdata = pdata;
+	par->debug = display->debug;
+	par->buf = buf;
+	spin_lock_init(&par->dirty_lock);
+	par->bgr = pdata->bgr;
+	par->startbyte = pdata->startbyte;
+	par->init_sequence = init_sequence;
+	par->gamma.curves = gamma_curves;
+	par->gamma.num_curves = display->gamma_num;
+	par->gamma.num_values = display->gamma_len;
+	mutex_init(&par->gamma.lock);
+	info->pseudo_palette = par->pseudo_palette;
+
+	if (par->gamma.curves && gamma) {
+		if (fbtft_gamma_parse_str(par, par->gamma.curves, gamma,
+					  strlen(gamma)))
+			goto release_framebuf;
+	}
+
+	/* Transmit buffer */
+	if (txbuflen == -1)
+		txbuflen = vmem_size + 2; /* add in case startbyte is used */
+	if (txbuflen >= vmem_size + 2)
+		txbuflen = 0;
+
+#ifdef __LITTLE_ENDIAN
+	if ((!txbuflen) && (bpp > 8))
+		txbuflen = PAGE_SIZE; /* need buffer for byteswapping */
+#endif
+
+	if (txbuflen > 0) {
+		txbuf = devm_kzalloc(par->info->device, txbuflen, GFP_KERNEL);
+		if (!txbuf)
+			goto release_framebuf;
+		par->txbuf.buf = txbuf;
+		par->txbuf.len = txbuflen;
+	}
+
+	/* default fbtft operations */
+	par->fbtftops.write = fbtft_write_spi;
+	par->fbtftops.read = fbtft_read_spi;
+	par->fbtftops.write_vmem = fbtft_write_vmem16_bus8;
+	par->fbtftops.write_register = fbtft_write_reg8_bus8;
+	par->fbtftops.set_addr_win = fbtft_set_addr_win;
+	par->fbtftops.reset = fbtft_reset;
+	par->fbtftops.mkdirty = fbtft_mkdirty;
+	par->fbtftops.update_display = fbtft_update_display;
+	if (display->backlight)
+		par->fbtftops.register_backlight = fbtft_register_backlight;
+
+	/* use driver provided functions */
+	fbtft_merge_fbtftops(&par->fbtftops, &display->fbtftops);
+
+	return info;
+
+release_framebuf:
+	framebuffer_release(info);
+
+alloc_fail:
+	vfree(vmem);
+
+	return NULL;
+}
+EXPORT_SYMBOL(fbtft_framebuffer_alloc);
+
+/**
+ * fbtft_framebuffer_release - frees up all memory used by the framebuffer
+ *
+ * @info: frame buffer info structure
+ *
+ */
+void fbtft_framebuffer_release(struct fb_info *info)
+{
+	fb_deferred_io_cleanup(info);
+	vfree(info->screen_buffer);
+	framebuffer_release(info);
+}
+EXPORT_SYMBOL(fbtft_framebuffer_release);
+
+/**
+ *	fbtft_register_framebuffer - registers a tft frame buffer device
+ *	@fb_info: frame buffer info structure
+ *
+ *  Sets SPI driverdata if needed
+ *  Requests needed gpios.
+ *  Initializes display
+ *  Updates display.
+ *	Registers a frame buffer device @fb_info.
+ *
+ *	Returns negative errno on error, or zero for success.
+ *
+ */
+int fbtft_register_framebuffer(struct fb_info *fb_info)
+{
+	int ret;
+	char text1[50] = "";
+	char text2[50] = "";
+	struct fbtft_par *par = fb_info->par;
+	struct spi_device *spi = par->spi;
+
+	/* sanity checks */
+	if (!par->fbtftops.init_display) {
+		dev_err(fb_info->device, "missing fbtftops.init_display()\n");
+		return -EINVAL;
+	}
+
+	if (spi)
+		spi_set_drvdata(spi, fb_info);
+	if (par->pdev)
+		platform_set_drvdata(par->pdev, fb_info);
+
+	ret = par->fbtftops.request_gpios(par);
+	if (ret < 0)
+		goto reg_fail;
+
+	if (par->fbtftops.verify_gpios) {
+		ret = par->fbtftops.verify_gpios(par);
+		if (ret < 0)
+			goto reg_fail;
+	}
+
+	ret = par->fbtftops.init_display(par);
+	if (ret < 0)
+		goto reg_fail;
+	if (par->fbtftops.set_var) {
+		ret = par->fbtftops.set_var(par);
+		if (ret < 0)
+			goto reg_fail;
+	}
+
+	/* update the entire display */
+	par->fbtftops.update_display(par, 0, par->info->var.yres - 1);
+
+	if (par->fbtftops.set_gamma && par->gamma.curves) {
+		ret = par->fbtftops.set_gamma(par, par->gamma.curves);
+		if (ret)
+			goto reg_fail;
+	}
+
+	if (par->fbtftops.register_backlight)
+		par->fbtftops.register_backlight(par);
+
+	ret = register_framebuffer(fb_info);
+	if (ret < 0)
+		goto reg_fail;
+
+	fbtft_sysfs_init(par);
+
+	if (par->txbuf.buf && par->txbuf.len >= 1024)
+		sprintf(text1, ", %zu KiB buffer memory", par->txbuf.len >> 10);
+	if (spi)
+		sprintf(text2, ", spi%d.%d at %d MHz", spi->master->bus_num,
+			spi->chip_select, spi->max_speed_hz / 1000000);
+	dev_info(fb_info->dev,
+		 "%s frame buffer, %dx%d, %d KiB video memory%s, fps=%lu%s\n",
+		 fb_info->fix.id, fb_info->var.xres, fb_info->var.yres,
+		 fb_info->fix.smem_len >> 10, text1,
+		 HZ / fb_info->fbdefio->delay, text2);
+
+	/* Turn on backlight if available */
+	if (fb_info->bl_dev) {
+		fb_info->bl_dev->props.power = FB_BLANK_UNBLANK;
+		fb_info->bl_dev->ops->update_status(fb_info->bl_dev);
+	}
+
+	return 0;
+
+reg_fail:
+	if (par->fbtftops.unregister_backlight)
+		par->fbtftops.unregister_backlight(par);
+
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_register_framebuffer);
+
+/**
+ *	fbtft_unregister_framebuffer - releases a tft frame buffer device
+ *	@fb_info: frame buffer info structure
+ *
+ *  Frees SPI driverdata if needed
+ *  Frees gpios.
+ *	Unregisters frame buffer device.
+ *
+ */
+int fbtft_unregister_framebuffer(struct fb_info *fb_info)
+{
+	struct fbtft_par *par = fb_info->par;
+
+	if (par->fbtftops.unregister_backlight)
+		par->fbtftops.unregister_backlight(par);
+	fbtft_sysfs_exit(par);
+	unregister_framebuffer(fb_info);
+
+	return 0;
+}
+EXPORT_SYMBOL(fbtft_unregister_framebuffer);
+
+/**
+ * fbtft_init_display_from_property() - Device Tree init_display() function
+ * @par: Driver data
+ *
+ * Return: 0 if successful, negative if error
+ */
+static int fbtft_init_display_from_property(struct fbtft_par *par)
+{
+	struct device *dev = par->info->device;
+	int buf[64], count, index, i, j, ret;
+	u32 *values;
+	u32 val;
+
+	count = device_property_count_u32(dev, "init");
+	if (count < 0)
+		return count;
+	if (count == 0)
+		return -EINVAL;
+
+	values = kmalloc_array(count + 1, sizeof(*values), GFP_KERNEL);
+	if (!values)
+		return -ENOMEM;
+
+	ret = device_property_read_u32_array(dev, "init", values, count);
+	if (ret)
+		goto out_free;
+
+	par->fbtftops.reset(par);
+
+	index = -1;
+	val = values[++index];
+
+	while (index < count) {
+		if (val & FBTFT_OF_INIT_CMD) {
+			val &= 0xFFFF;
+			i = 0;
+			while ((index < count) && !(val & 0xFFFF0000)) {
+				if (i > 63) {
+					dev_err(dev,
+						"%s: Maximum register values exceeded\n",
+						__func__);
+					ret = -EINVAL;
+					goto out_free;
+				}
+				buf[i++] = val;
+				val = values[++index];
+			}
+			/* make debug message */
+			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+				      "init: write_register:\n");
+			for (j = 0; j < i; j++)
+				fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+					      "buf[%d] = %02X\n", j, buf[j]);
+
+			par->fbtftops.write_register(par, i,
+				buf[0], buf[1], buf[2], buf[3],
+				buf[4], buf[5], buf[6], buf[7],
+				buf[8], buf[9], buf[10], buf[11],
+				buf[12], buf[13], buf[14], buf[15],
+				buf[16], buf[17], buf[18], buf[19],
+				buf[20], buf[21], buf[22], buf[23],
+				buf[24], buf[25], buf[26], buf[27],
+				buf[28], buf[29], buf[30], buf[31],
+				buf[32], buf[33], buf[34], buf[35],
+				buf[36], buf[37], buf[38], buf[39],
+				buf[40], buf[41], buf[42], buf[43],
+				buf[44], buf[45], buf[46], buf[47],
+				buf[48], buf[49], buf[50], buf[51],
+				buf[52], buf[53], buf[54], buf[55],
+				buf[56], buf[57], buf[58], buf[59],
+				buf[60], buf[61], buf[62], buf[63]);
+		} else if (val & FBTFT_OF_INIT_DELAY) {
+			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+				      "init: msleep(%u)\n", val & 0xFFFF);
+			msleep(val & 0xFFFF);
+			val = values[++index];
+		} else {
+			dev_err(dev, "illegal init value 0x%X\n", val);
+			ret = -EINVAL;
+			goto out_free;
+		}
+	}
+
+out_free:
+	kfree(values);
+	return ret;
+}
+
+/**
+ * fbtft_init_display() - Generic init_display() function
+ * @par: Driver data
+ *
+ * Uses par->init_sequence to do the initialization
+ *
+ * Return: 0 if successful, negative if error
+ */
+int fbtft_init_display(struct fbtft_par *par)
+{
+	int buf[64];
+	int i;
+	int j;
+
+	/* sanity check */
+	if (!par->init_sequence) {
+		dev_err(par->info->device,
+			"error: init_sequence is not set\n");
+		return -EINVAL;
+	}
+
+	/* make sure stop marker exists */
+	for (i = 0; i < FBTFT_MAX_INIT_SEQUENCE; i++) {
+		if (par->init_sequence[i] == -3)
+			break;
+	}
+
+	if (i == FBTFT_MAX_INIT_SEQUENCE) {
+		dev_err(par->info->device,
+			"missing stop marker at end of init sequence\n");
+		return -EINVAL;
+	}
+
+	par->fbtftops.reset(par);
+
+	i = 0;
+	while (i < FBTFT_MAX_INIT_SEQUENCE) {
+		if (par->init_sequence[i] == -3) {
+			/* done */
+			return 0;
+		}
+		if (par->init_sequence[i] >= 0) {
+			dev_err(par->info->device,
+				"missing delimiter at position %d\n", i);
+			return -EINVAL;
+		}
+		if (par->init_sequence[i + 1] < 0) {
+			dev_err(par->info->device,
+				"missing value after delimiter %d at position %d\n",
+				par->init_sequence[i], i);
+			return -EINVAL;
+		}
+		switch (par->init_sequence[i]) {
+		case -1:
+			i++;
+
+			/* make debug message */
+			for (j = 0; par->init_sequence[i + 1 + j] >= 0; j++)
+				;
+
+			fbtft_par_dbg_hex(DEBUG_INIT_DISPLAY, par, par->info->device,
+					  s16, &par->init_sequence[i + 1], j,
+					  "init: write(0x%02X)", par->init_sequence[i]);
+
+			/* Write */
+			j = 0;
+			while (par->init_sequence[i] >= 0) {
+				if (j > 63) {
+					dev_err(par->info->device,
+						"%s: Maximum register values exceeded\n",
+						__func__);
+					return -EINVAL;
+				}
+				buf[j++] = par->init_sequence[i++];
+			}
+			par->fbtftops.write_register(par, j,
+				buf[0], buf[1], buf[2], buf[3],
+				buf[4], buf[5], buf[6], buf[7],
+				buf[8], buf[9], buf[10], buf[11],
+				buf[12], buf[13], buf[14], buf[15],
+				buf[16], buf[17], buf[18], buf[19],
+				buf[20], buf[21], buf[22], buf[23],
+				buf[24], buf[25], buf[26], buf[27],
+				buf[28], buf[29], buf[30], buf[31],
+				buf[32], buf[33], buf[34], buf[35],
+				buf[36], buf[37], buf[38], buf[39],
+				buf[40], buf[41], buf[42], buf[43],
+				buf[44], buf[45], buf[46], buf[47],
+				buf[48], buf[49], buf[50], buf[51],
+				buf[52], buf[53], buf[54], buf[55],
+				buf[56], buf[57], buf[58], buf[59],
+				buf[60], buf[61], buf[62], buf[63]);
+			break;
+		case -2:
+			i++;
+			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+				      "init: mdelay(%d)\n",
+				      par->init_sequence[i]);
+			mdelay(par->init_sequence[i++]);
+			break;
+		default:
+			dev_err(par->info->device,
+				"unknown delimiter %d at position %d\n",
+				par->init_sequence[i], i);
+			return -EINVAL;
+		}
+	}
+
+	dev_err(par->info->device,
+		"%s: something is wrong. Shouldn't get here.\n", __func__);
+	return -EINVAL;
+}
+EXPORT_SYMBOL(fbtft_init_display);
+
+/**
+ * fbtft_verify_gpios() - Generic verify_gpios() function
+ * @par: Driver data
+ *
+ * Uses @spi, @pdev and @buswidth to determine which GPIOs is needed
+ *
+ * Return: 0 if successful, negative if error
+ */
+static int fbtft_verify_gpios(struct fbtft_par *par)
+{
+	struct fbtft_platform_data *pdata = par->pdata;
+	int i;
+
+	fbtft_par_dbg(DEBUG_VERIFY_GPIOS, par, "%s()\n", __func__);
+
+	if (pdata->display.buswidth != 9 &&  par->startbyte == 0 &&
+	    !par->gpio.dc) {
+		dev_err(par->info->device,
+			"Missing info about 'dc' gpio. Aborting.\n");
+		return -EINVAL;
+	}
+
+	if (!par->pdev)
+		return 0;
+
+	if (!par->gpio.wr) {
+		dev_err(par->info->device, "Missing 'wr' gpio. Aborting.\n");
+		return -EINVAL;
+	}
+	for (i = 0; i < pdata->display.buswidth; i++) {
+		if (!par->gpio.db[i]) {
+			dev_err(par->info->device,
+				"Missing 'db%02d' gpio. Aborting.\n", i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/* returns 0 if the property is not present */
+static u32 fbtft_property_value(struct device *dev, const char *propname)
+{
+	int ret;
+	u32 val = 0;
+
+	ret = device_property_read_u32(dev, propname, &val);
+	if (ret == 0)
+		dev_info(dev, "%s: %s = %u\n", __func__, propname, val);
+
+	return val;
+}
+
+static struct fbtft_platform_data *fbtft_properties_read(struct device *dev)
+{
+	struct fbtft_platform_data *pdata;
+
+	if (!dev_fwnode(dev)) {
+		dev_err(dev, "Missing platform data or properties\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->display.width = fbtft_property_value(dev, "width");
+	pdata->display.height = fbtft_property_value(dev, "height");
+	pdata->display.regwidth = fbtft_property_value(dev, "regwidth");
+	pdata->display.buswidth = fbtft_property_value(dev, "buswidth");
+	pdata->display.backlight = fbtft_property_value(dev, "backlight");
+	pdata->display.bpp = fbtft_property_value(dev, "bpp");
+	pdata->display.debug = fbtft_property_value(dev, "debug");
+	pdata->rotate = fbtft_property_value(dev, "rotate");
+	pdata->bgr = device_property_read_bool(dev, "bgr");
+	pdata->fps = fbtft_property_value(dev, "fps");
+	pdata->txbuflen = fbtft_property_value(dev, "txbuflen");
+	pdata->startbyte = fbtft_property_value(dev, "startbyte");
+	device_property_read_string(dev, "gamma", (const char **)&pdata->gamma);
+
+	if (device_property_present(dev, "led-gpios"))
+		pdata->display.backlight = 1;
+	if (device_property_present(dev, "init"))
+		pdata->display.fbtftops.init_display =
+			fbtft_init_display_from_property;
+
+	pdata->display.fbtftops.request_gpios = fbtft_request_gpios;
+
+	return pdata;
+}
+
+/**
+ * fbtft_probe_common() - Generic device probe() helper function
+ * @display: Display properties
+ * @sdev: SPI device
+ * @pdev: Platform device
+ *
+ * Allocates, initializes and registers a framebuffer
+ *
+ * Either @sdev or @pdev should be NULL
+ *
+ * Return: 0 if successful, negative if error
+ */
+int fbtft_probe_common(struct fbtft_display *display,
+		       struct spi_device *sdev,
+		       struct platform_device *pdev)
+{
+	struct device *dev;
+	struct fb_info *info;
+	struct fbtft_par *par;
+	struct fbtft_platform_data *pdata;
+	int ret;
+
+	if (sdev)
+		dev = &sdev->dev;
+	else
+		dev = &pdev->dev;
+
+	if (unlikely(display->debug & DEBUG_DRIVER_INIT_FUNCTIONS))
+		dev_info(dev, "%s()\n", __func__);
+
+	pdata = dev->platform_data;
+	if (!pdata) {
+		pdata = fbtft_properties_read(dev);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+	}
+
+	info = fbtft_framebuffer_alloc(display, dev, pdata);
+	if (!info)
+		return -ENOMEM;
+
+	par = info->par;
+	par->spi = sdev;
+	par->pdev = pdev;
+
+	if (display->buswidth == 0) {
+		dev_err(dev, "buswidth is not set\n");
+		return -EINVAL;
+	}
+
+	/* write register functions */
+	if (display->regwidth == 8 && display->buswidth == 8)
+		par->fbtftops.write_register = fbtft_write_reg8_bus8;
+	else if (display->regwidth == 8 && display->buswidth == 9 && par->spi)
+		par->fbtftops.write_register = fbtft_write_reg8_bus9;
+	else if (display->regwidth == 16 && display->buswidth == 8)
+		par->fbtftops.write_register = fbtft_write_reg16_bus8;
+	else if (display->regwidth == 16 && display->buswidth == 16)
+		par->fbtftops.write_register = fbtft_write_reg16_bus16;
+	else
+		dev_warn(dev,
+			 "no default functions for regwidth=%d and buswidth=%d\n",
+			 display->regwidth, display->buswidth);
+
+	/* write_vmem() functions */
+	if (display->buswidth == 8)
+		par->fbtftops.write_vmem = fbtft_write_vmem16_bus8;
+	else if (display->buswidth == 9)
+		par->fbtftops.write_vmem = fbtft_write_vmem16_bus9;
+	else if (display->buswidth == 16)
+		par->fbtftops.write_vmem = fbtft_write_vmem16_bus16;
+
+	/* GPIO write() functions */
+	if (par->pdev) {
+		if (display->buswidth == 8)
+			par->fbtftops.write = fbtft_write_gpio8_wr;
+		else if (display->buswidth == 16)
+			par->fbtftops.write = fbtft_write_gpio16_wr;
+	}
+
+	/* 9-bit SPI setup */
+	if (par->spi && display->buswidth == 9) {
+		if (par->spi->master->bits_per_word_mask & SPI_BPW_MASK(9)) {
+			par->spi->bits_per_word = 9;
+		} else {
+			dev_warn(&par->spi->dev,
+				 "9-bit SPI not available, emulating using 8-bit.\n");
+			/* allocate buffer with room for dc bits */
+			par->extra = devm_kzalloc(par->info->device,
+						  par->txbuf.len +
+						  (par->txbuf.len / 8) + 8,
+						  GFP_KERNEL);
+			if (!par->extra) {
+				ret = -ENOMEM;
+				goto out_release;
+			}
+			par->fbtftops.write = fbtft_write_spi_emulate_9;
+		}
+	}
+
+	if (!par->fbtftops.verify_gpios)
+		par->fbtftops.verify_gpios = fbtft_verify_gpios;
+
+	/* make sure we still use the driver provided functions */
+	fbtft_merge_fbtftops(&par->fbtftops, &display->fbtftops);
+
+	/* use init_sequence if provided */
+	if (par->init_sequence)
+		par->fbtftops.init_display = fbtft_init_display;
+
+	/* use platform_data provided functions above all */
+	fbtft_merge_fbtftops(&par->fbtftops, &pdata->display.fbtftops);
+
+	ret = fbtft_register_framebuffer(info);
+	if (ret < 0)
+		goto out_release;
+
+	return 0;
+
+out_release:
+	fbtft_framebuffer_release(info);
+
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_probe_common);
+
+/**
+ * fbtft_remove_common() - Generic device remove() helper function
+ * @dev: Device
+ * @info: Framebuffer
+ *
+ * Unregisters and releases the framebuffer
+ */
+void fbtft_remove_common(struct device *dev, struct fb_info *info)
+{
+	struct fbtft_par *par;
+
+	par = info->par;
+	if (par)
+		fbtft_par_dbg(DEBUG_DRIVER_INIT_FUNCTIONS, par,
+			      "%s()\n", __func__);
+	fbtft_unregister_framebuffer(info);
+	fbtft_framebuffer_release(info);
+}
+EXPORT_SYMBOL(fbtft_remove_common);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fbtft-io.c b/drivers/staging/fbtft/fbtft-io.c
new file mode 100644
index 000000000..de1904a44
--- /dev/null
+++ b/drivers/staging/fbtft/fbtft-io.c
@@ -0,0 +1,236 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/export.h>
+#include <linux/errno.h>
+#include <linux/gpio/consumer.h>
+#include <linux/spi/spi.h>
+#include "fbtft.h"
+
+int fbtft_write_spi(struct fbtft_par *par, void *buf, size_t len)
+{
+	struct spi_transfer t = {
+		.tx_buf = buf,
+		.len = len,
+	};
+	struct spi_message m;
+
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	if (!par->spi) {
+		dev_err(par->info->device,
+			"%s: par->spi is unexpectedly NULL\n", __func__);
+		return -1;
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+	return spi_sync(par->spi, &m);
+}
+EXPORT_SYMBOL(fbtft_write_spi);
+
+/**
+ * fbtft_write_spi_emulate_9() - write SPI emulating 9-bit
+ * @par: Driver data
+ * @buf: Buffer to write
+ * @len: Length of buffer (must be divisible by 8)
+ *
+ * When 9-bit SPI is not available, this function can be used to emulate that.
+ * par->extra must hold a transformation buffer used for transfer.
+ */
+int fbtft_write_spi_emulate_9(struct fbtft_par *par, void *buf, size_t len)
+{
+	u16 *src = buf;
+	u8 *dst = par->extra;
+	size_t size = len / 2;
+	size_t added = 0;
+	int bits, i, j;
+	u64 val, dc, tmp;
+
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	if (!par->extra) {
+		dev_err(par->info->device, "%s: error: par->extra is NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+	if ((len % 8) != 0) {
+		dev_err(par->info->device,
+			"error: len=%zu must be divisible by 8\n", len);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < size; i += 8) {
+		tmp = 0;
+		bits = 63;
+		for (j = 0; j < 7; j++) {
+			dc = (*src & 0x0100) ? 1 : 0;
+			val = *src & 0x00FF;
+			tmp |= dc << bits;
+			bits -= 8;
+			tmp |= val << bits--;
+			src++;
+		}
+		tmp |= ((*src & 0x0100) ? 1 : 0);
+		*(__be64 *)dst = cpu_to_be64(tmp);
+		dst += 8;
+		*dst++ = (u8)(*src++ & 0x00FF);
+		added++;
+	}
+
+	return spi_write(par->spi, par->extra, size + added);
+}
+EXPORT_SYMBOL(fbtft_write_spi_emulate_9);
+
+int fbtft_read_spi(struct fbtft_par *par, void *buf, size_t len)
+{
+	int ret;
+	u8 txbuf[32] = { 0, };
+	struct spi_transfer	t = {
+			.speed_hz = 2000000,
+			.rx_buf		= buf,
+			.len		= len,
+		};
+	struct spi_message	m;
+
+	if (!par->spi) {
+		dev_err(par->info->device,
+			"%s: par->spi is unexpectedly NULL\n", __func__);
+		return -ENODEV;
+	}
+
+	if (par->startbyte) {
+		if (len > 32) {
+			dev_err(par->info->device,
+				"len=%zu can't be larger than 32 when using 'startbyte'\n",
+				len);
+			return -EINVAL;
+		}
+		txbuf[0] = par->startbyte | 0x3;
+		t.tx_buf = txbuf;
+		fbtft_par_dbg_hex(DEBUG_READ, par, par->info->device, u8,
+				  txbuf, len, "%s(len=%zu) txbuf => ",
+				  __func__, len);
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+	ret = spi_sync(par->spi, &m);
+	fbtft_par_dbg_hex(DEBUG_READ, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu) buf <= ", __func__, len);
+
+	return ret;
+}
+EXPORT_SYMBOL(fbtft_read_spi);
+
+/*
+ * Optimized use of gpiolib is twice as fast as no optimization
+ * only one driver can use the optimized version at a time
+ */
+int fbtft_write_gpio8_wr(struct fbtft_par *par, void *buf, size_t len)
+{
+	u8 data;
+	int i;
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+	static u8 prev_data;
+#endif
+
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	while (len--) {
+		data = *(u8 *)buf;
+
+		/* Start writing by pulling down /WR */
+		gpiod_set_value(par->gpio.wr, 1);
+
+		/* Set data */
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+		if (data == prev_data) {
+			gpiod_set_value(par->gpio.wr, 1); /* used as delay */
+		} else {
+			for (i = 0; i < 8; i++) {
+				if ((data & 1) != (prev_data & 1))
+					gpiod_set_value(par->gpio.db[i],
+							data & 1);
+				data >>= 1;
+				prev_data >>= 1;
+			}
+		}
+#else
+		for (i = 0; i < 8; i++) {
+			gpiod_set_value(par->gpio.db[i], data & 1);
+			data >>= 1;
+		}
+#endif
+
+		/* Pullup /WR */
+		gpiod_set_value(par->gpio.wr, 0);
+
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+		prev_data = *(u8 *)buf;
+#endif
+		buf++;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(fbtft_write_gpio8_wr);
+
+int fbtft_write_gpio16_wr(struct fbtft_par *par, void *buf, size_t len)
+{
+	u16 data;
+	int i;
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+	static u16 prev_data;
+#endif
+
+	fbtft_par_dbg_hex(DEBUG_WRITE, par, par->info->device, u8, buf, len,
+			  "%s(len=%zu): ", __func__, len);
+
+	while (len) {
+		data = *(u16 *)buf;
+
+		/* Start writing by pulling down /WR */
+		gpiod_set_value(par->gpio.wr, 1);
+
+		/* Set data */
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+		if (data == prev_data) {
+			gpiod_set_value(par->gpio.wr, 1); /* used as delay */
+		} else {
+			for (i = 0; i < 16; i++) {
+				if ((data & 1) != (prev_data & 1))
+					gpiod_set_value(par->gpio.db[i],
+							data & 1);
+				data >>= 1;
+				prev_data >>= 1;
+			}
+		}
+#else
+		for (i = 0; i < 16; i++) {
+			gpiod_set_value(par->gpio.db[i], data & 1);
+			data >>= 1;
+		}
+#endif
+
+		/* Pullup /WR */
+		gpiod_set_value(par->gpio.wr, 0);
+
+#ifndef DO_NOT_OPTIMIZE_FBTFT_WRITE_GPIO
+		prev_data = *(u16 *)buf;
+#endif
+		buf += 2;
+		len -= 2;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(fbtft_write_gpio16_wr);
+
+int fbtft_write_gpio16_wr_latched(struct fbtft_par *par, void *buf, size_t len)
+{
+	dev_err(par->info->device, "%s: function not implemented\n", __func__);
+	return -1;
+}
+EXPORT_SYMBOL(fbtft_write_gpio16_wr_latched);
diff --git a/drivers/staging/fbtft/fbtft-sysfs.c b/drivers/staging/fbtft/fbtft-sysfs.c
new file mode 100644
index 000000000..39e8d2806
--- /dev/null
+++ b/drivers/staging/fbtft/fbtft-sysfs.c
@@ -0,0 +1,220 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "fbtft.h"
+#include "internal.h"
+
+static int get_next_ulong(char **str_p, unsigned long *val, char *sep, int base)
+{
+	char *p_val;
+
+	if (!str_p || !(*str_p))
+		return -EINVAL;
+
+	p_val = strsep(str_p, sep);
+
+	if (!p_val)
+		return -EINVAL;
+
+	return kstrtoul(p_val, base, val);
+}
+
+int fbtft_gamma_parse_str(struct fbtft_par *par, u32 *curves,
+			  const char *str, int size)
+{
+	char *str_p, *curve_p = NULL;
+	char *tmp;
+	unsigned long val = 0;
+	int ret = 0;
+	int curve_counter, value_counter;
+	int _count;
+
+	fbtft_par_dbg(DEBUG_SYSFS, par, "%s() str=\n", __func__);
+
+	if (!str || !curves)
+		return -EINVAL;
+
+	fbtft_par_dbg(DEBUG_SYSFS, par, "%s\n", str);
+
+	tmp = kmemdup(str, size + 1, GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	/* replace optional separators */
+	str_p = tmp;
+	while (*str_p) {
+		if (*str_p == ',')
+			*str_p = ' ';
+		if (*str_p == ';')
+			*str_p = '\n';
+		str_p++;
+	}
+
+	str_p = strim(tmp);
+
+	curve_counter = 0;
+	while (str_p) {
+		if (curve_counter == par->gamma.num_curves) {
+			dev_err(par->info->device, "Gamma: Too many curves\n");
+			ret = -EINVAL;
+			goto out;
+		}
+		curve_p = strsep(&str_p, "\n");
+		value_counter = 0;
+		while (curve_p) {
+			if (value_counter == par->gamma.num_values) {
+				dev_err(par->info->device,
+					"Gamma: Too many values\n");
+				ret = -EINVAL;
+				goto out;
+			}
+			ret = get_next_ulong(&curve_p, &val, " ", 16);
+			if (ret)
+				goto out;
+
+			_count = curve_counter * par->gamma.num_values +
+				 value_counter;
+			curves[_count] = val;
+			value_counter++;
+		}
+		if (value_counter != par->gamma.num_values) {
+			dev_err(par->info->device, "Gamma: Too few values\n");
+			ret = -EINVAL;
+			goto out;
+		}
+		curve_counter++;
+	}
+	if (curve_counter != par->gamma.num_curves) {
+		dev_err(par->info->device, "Gamma: Too few curves\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+out:
+	kfree(tmp);
+	return ret;
+}
+
+static ssize_t
+sprintf_gamma(struct fbtft_par *par, u32 *curves, char *buf)
+{
+	ssize_t len = 0;
+	unsigned int i, j;
+
+	mutex_lock(&par->gamma.lock);
+	for (i = 0; i < par->gamma.num_curves; i++) {
+		for (j = 0; j < par->gamma.num_values; j++)
+			len += scnprintf(&buf[len], PAGE_SIZE,
+			     "%04x ", curves[i * par->gamma.num_values + j]);
+		buf[len - 1] = '\n';
+	}
+	mutex_unlock(&par->gamma.lock);
+
+	return len;
+}
+
+static ssize_t store_gamma_curve(struct device *device,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+	u32 tmp_curves[FBTFT_GAMMA_MAX_VALUES_TOTAL];
+	int ret;
+
+	ret = fbtft_gamma_parse_str(par, tmp_curves, buf, count);
+	if (ret)
+		return ret;
+
+	ret = par->fbtftops.set_gamma(par, tmp_curves);
+	if (ret)
+		return ret;
+
+	mutex_lock(&par->gamma.lock);
+	memcpy(par->gamma.curves, tmp_curves,
+	       par->gamma.num_curves * par->gamma.num_values *
+	       sizeof(tmp_curves[0]));
+	mutex_unlock(&par->gamma.lock);
+
+	return count;
+}
+
+static ssize_t show_gamma_curve(struct device *device,
+				struct device_attribute *attr, char *buf)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+
+	return sprintf_gamma(par, par->gamma.curves, buf);
+}
+
+static struct device_attribute gamma_device_attrs[] = {
+	__ATTR(gamma, 0660, show_gamma_curve, store_gamma_curve),
+};
+
+void fbtft_expand_debug_value(unsigned long *debug)
+{
+	switch (*debug & 0x7) {
+	case 1:
+		*debug |= DEBUG_LEVEL_1;
+		break;
+	case 2:
+		*debug |= DEBUG_LEVEL_2;
+		break;
+	case 3:
+		*debug |= DEBUG_LEVEL_3;
+		break;
+	case 4:
+		*debug |= DEBUG_LEVEL_4;
+		break;
+	case 5:
+		*debug |= DEBUG_LEVEL_5;
+		break;
+	case 6:
+		*debug |= DEBUG_LEVEL_6;
+		break;
+	case 7:
+		*debug = 0xFFFFFFFF;
+		break;
+	}
+}
+
+static ssize_t store_debug(struct device *device,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &par->debug);
+	if (ret)
+		return ret;
+	fbtft_expand_debug_value(&par->debug);
+
+	return count;
+}
+
+static ssize_t show_debug(struct device *device,
+			  struct device_attribute *attr, char *buf)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+
+	return sysfs_emit(buf, "%lu\n", par->debug);
+}
+
+static struct device_attribute debug_device_attr =
+	__ATTR(debug, 0660, show_debug, store_debug);
+
+void fbtft_sysfs_init(struct fbtft_par *par)
+{
+	device_create_file(par->info->dev, &debug_device_attr);
+	if (par->gamma.curves && par->fbtftops.set_gamma)
+		device_create_file(par->info->dev, &gamma_device_attrs[0]);
+}
+
+void fbtft_sysfs_exit(struct fbtft_par *par)
+{
+	device_remove_file(par->info->dev, &debug_device_attr);
+	if (par->gamma.curves && par->fbtftops.set_gamma)
+		device_remove_file(par->info->dev, &gamma_device_attrs[0]);
+}
diff --git a/drivers/staging/fbtft/fbtft.h b/drivers/staging/fbtft/fbtft.h
new file mode 100644
index 000000000..2c2b5f1c1
--- /dev/null
+++ b/drivers/staging/fbtft/fbtft.h
@@ -0,0 +1,444 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/* Copyright (C) 2013 Noralf Tronnes */
+
+#ifndef __LINUX_FBTFT_H
+#define __LINUX_FBTFT_H
+
+#include <linux/fb.h>
+#include <linux/spinlock.h>
+#include <linux/spi/spi.h>
+#include <linux/platform_device.h>
+
+#define FBTFT_ONBOARD_BACKLIGHT 2
+
+#define FBTFT_GPIO_NO_MATCH		0xFFFF
+#define FBTFT_GPIO_NAME_SIZE	32
+#define FBTFT_MAX_INIT_SEQUENCE      512
+#define FBTFT_GAMMA_MAX_VALUES_TOTAL 128
+
+#define FBTFT_OF_INIT_CMD	BIT(24)
+#define FBTFT_OF_INIT_DELAY	BIT(25)
+
+/**
+ * struct fbtft_gpio - Structure that holds one pinname to gpio mapping
+ * @name: pinname (reset, dc, etc.)
+ * @gpio: GPIO number
+ *
+ */
+struct fbtft_gpio {
+	char name[FBTFT_GPIO_NAME_SIZE];
+	struct gpio_desc *gpio;
+};
+
+struct fbtft_par;
+
+/**
+ * struct fbtft_ops - FBTFT operations structure
+ * @write: Writes to interface bus
+ * @read: Reads from interface bus
+ * @write_vmem: Writes video memory to display
+ * @write_reg: Writes to controller register
+ * @set_addr_win: Set the GRAM update window
+ * @reset: Reset the LCD controller
+ * @mkdirty: Marks display lines for update
+ * @update_display: Updates the display
+ * @init_display: Initializes the display
+ * @blank: Blank the display (optional)
+ * @request_gpios_match: Do pinname to gpio matching
+ * @request_gpios: Request gpios from the kernel
+ * @free_gpios: Free previously requested gpios
+ * @verify_gpios: Verify that necessary gpios is present (optional)
+ * @register_backlight: Used to register backlight device (optional)
+ * @unregister_backlight: Unregister backlight device (optional)
+ * @set_var: Configure LCD with values from variables like @rotate and @bgr
+ *           (optional)
+ * @set_gamma: Set Gamma curve (optional)
+ *
+ * Most of these operations have default functions assigned to them in
+ *     fbtft_framebuffer_alloc()
+ */
+struct fbtft_ops {
+	int (*write)(struct fbtft_par *par, void *buf, size_t len);
+	int (*read)(struct fbtft_par *par, void *buf, size_t len);
+	int (*write_vmem)(struct fbtft_par *par, size_t offset, size_t len);
+	void (*write_register)(struct fbtft_par *par, int len, ...);
+
+	void (*set_addr_win)(struct fbtft_par *par,
+			     int xs, int ys, int xe, int ye);
+	void (*reset)(struct fbtft_par *par);
+	void (*mkdirty)(struct fb_info *info, int from, int to);
+	void (*update_display)(struct fbtft_par *par,
+			       unsigned int start_line, unsigned int end_line);
+	int (*init_display)(struct fbtft_par *par);
+	int (*blank)(struct fbtft_par *par, bool on);
+
+	unsigned long (*request_gpios_match)(struct fbtft_par *par,
+					     const struct fbtft_gpio *gpio);
+	int (*request_gpios)(struct fbtft_par *par);
+	int (*verify_gpios)(struct fbtft_par *par);
+
+	void (*register_backlight)(struct fbtft_par *par);
+	void (*unregister_backlight)(struct fbtft_par *par);
+
+	int (*set_var)(struct fbtft_par *par);
+	int (*set_gamma)(struct fbtft_par *par, u32 *curves);
+};
+
+/**
+ * struct fbtft_display - Describes the display properties
+ * @width: Width of display in pixels
+ * @height: Height of display in pixels
+ * @regwidth: LCD Controller Register width in bits
+ * @buswidth: Display interface bus width in bits
+ * @backlight: Backlight type.
+ * @fbtftops: FBTFT operations provided by driver or device (platform_data)
+ * @bpp: Bits per pixel
+ * @fps: Frames per second
+ * @txbuflen: Size of transmit buffer
+ * @init_sequence: Pointer to LCD initialization array
+ * @gamma: String representation of Gamma curve(s)
+ * @gamma_num: Number of Gamma curves
+ * @gamma_len: Number of values per Gamma curve
+ * @debug: Initial debug value
+ *
+ * This structure is not stored by FBTFT except for init_sequence.
+ */
+struct fbtft_display {
+	unsigned int width;
+	unsigned int height;
+	unsigned int regwidth;
+	unsigned int buswidth;
+	unsigned int backlight;
+	struct fbtft_ops fbtftops;
+	unsigned int bpp;
+	unsigned int fps;
+	int txbuflen;
+	const s16 *init_sequence;
+	char *gamma;
+	int gamma_num;
+	int gamma_len;
+	unsigned long debug;
+};
+
+/**
+ * struct fbtft_platform_data - Passes display specific data to the driver
+ * @display: Display properties
+ * @gpios: Pointer to an array of pinname to gpio mappings
+ * @rotate: Display rotation angle
+ * @bgr: LCD Controller BGR bit
+ * @fps: Frames per second (this will go away, use @fps in @fbtft_display)
+ * @txbuflen: Size of transmit buffer
+ * @startbyte: When set, enables use of Startbyte in transfers
+ * @gamma: String representation of Gamma curve(s)
+ * @extra: A way to pass extra info
+ */
+struct fbtft_platform_data {
+	struct fbtft_display display;
+	unsigned int rotate;
+	bool bgr;
+	unsigned int fps;
+	int txbuflen;
+	u8 startbyte;
+	char *gamma;
+	void *extra;
+};
+
+/**
+ * struct fbtft_par - Main FBTFT data structure
+ *
+ * This structure holds all relevant data to operate the display
+ *
+ * See sourcefile for documentation since nested structs is not
+ * supported by kernel-doc.
+ *
+ */
+/* @spi: Set if it is a SPI device
+ * @pdev: Set if it is a platform device
+ * @info: Pointer to framebuffer fb_info structure
+ * @pdata: Pointer to platform data
+ * @ssbuf: Not used
+ * @pseudo_palette: Used by fb_set_colreg()
+ * @txbuf.buf: Transmit buffer
+ * @txbuf.len: Transmit buffer length
+ * @buf: Small buffer used when writing init data over SPI
+ * @startbyte: Used by some controllers when in SPI mode.
+ *             Format: 6 bit Device id + RS bit + RW bit
+ * @fbtftops: FBTFT operations provided by driver or device (platform_data)
+ * @dirty_lock: Protects dirty_lines_start and dirty_lines_end
+ * @dirty_lines_start: Where to begin updating display
+ * @dirty_lines_end: Where to end updating display
+ * @gpio.reset: GPIO used to reset display
+ * @gpio.dc: Data/Command signal, also known as RS
+ * @gpio.rd: Read latching signal
+ * @gpio.wr: Write latching signal
+ * @gpio.latch: Bus latch signal, eg. 16->8 bit bus latch
+ * @gpio.cs: LCD Chip Select with parallel interface bus
+ * @gpio.db[16]: Parallel databus
+ * @gpio.led[16]: Led control signals
+ * @gpio.aux[16]: Auxiliary signals, not used by core
+ * @init_sequence: Pointer to LCD initialization array
+ * @gamma.lock: Mutex for Gamma curve locking
+ * @gamma.curves: Pointer to Gamma curve array
+ * @gamma.num_values: Number of values per Gamma curve
+ * @gamma.num_curves: Number of Gamma curves
+ * @debug: Pointer to debug value
+ * @current_debug:
+ * @first_update_done: Used to only time the first display update
+ * @update_time: Used to calculate 'fps' in debug output
+ * @bgr: BGR mode/\n
+ * @extra: Extra info needed by driver
+ */
+struct fbtft_par {
+	struct spi_device *spi;
+	struct platform_device *pdev;
+	struct fb_info *info;
+	struct fbtft_platform_data *pdata;
+	u16 *ssbuf;
+	u32 pseudo_palette[16];
+	struct {
+		void *buf;
+		size_t len;
+	} txbuf;
+	u8 *buf;
+	u8 startbyte;
+	struct fbtft_ops fbtftops;
+	spinlock_t dirty_lock;
+	unsigned int dirty_lines_start;
+	unsigned int dirty_lines_end;
+	struct {
+		struct gpio_desc *reset;
+		struct gpio_desc *dc;
+		struct gpio_desc *rd;
+		struct gpio_desc *wr;
+		struct gpio_desc *latch;
+		struct gpio_desc *cs;
+		struct gpio_desc *db[16];
+		struct gpio_desc *led[16];
+		struct gpio_desc *aux[16];
+	} gpio;
+	const s16 *init_sequence;
+	struct {
+		struct mutex lock;
+		u32 *curves;
+		int num_values;
+		int num_curves;
+	} gamma;
+	unsigned long debug;
+	bool first_update_done;
+	ktime_t update_time;
+	bool bgr;
+	void *extra;
+	bool polarity;
+};
+
+#define NUMARGS(...)  (sizeof((int[]){__VA_ARGS__}) / sizeof(int))
+
+#define write_reg(par, ...)                                            \
+	((par)->fbtftops.write_register(par, NUMARGS(__VA_ARGS__), __VA_ARGS__))
+
+/* fbtft-core.c */
+int fbtft_write_buf_dc(struct fbtft_par *par, void *buf, size_t len, int dc);
+__printf(5, 6)
+void fbtft_dbg_hex(const struct device *dev, int groupsize,
+		   const void *buf, size_t len, const char *fmt, ...);
+struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
+					struct device *dev,
+					struct fbtft_platform_data *pdata);
+void fbtft_framebuffer_release(struct fb_info *info);
+int fbtft_register_framebuffer(struct fb_info *fb_info);
+int fbtft_unregister_framebuffer(struct fb_info *fb_info);
+void fbtft_register_backlight(struct fbtft_par *par);
+void fbtft_unregister_backlight(struct fbtft_par *par);
+int fbtft_init_display(struct fbtft_par *par);
+int fbtft_probe_common(struct fbtft_display *display, struct spi_device *sdev,
+		       struct platform_device *pdev);
+void fbtft_remove_common(struct device *dev, struct fb_info *info);
+
+/* fbtft-io.c */
+int fbtft_write_spi(struct fbtft_par *par, void *buf, size_t len);
+int fbtft_write_spi_emulate_9(struct fbtft_par *par, void *buf, size_t len);
+int fbtft_read_spi(struct fbtft_par *par, void *buf, size_t len);
+int fbtft_write_gpio8_wr(struct fbtft_par *par, void *buf, size_t len);
+int fbtft_write_gpio16_wr(struct fbtft_par *par, void *buf, size_t len);
+int fbtft_write_gpio16_wr_latched(struct fbtft_par *par, void *buf, size_t len);
+
+/* fbtft-bus.c */
+int fbtft_write_vmem8_bus8(struct fbtft_par *par, size_t offset, size_t len);
+int fbtft_write_vmem16_bus16(struct fbtft_par *par, size_t offset, size_t len);
+int fbtft_write_vmem16_bus8(struct fbtft_par *par, size_t offset, size_t len);
+int fbtft_write_vmem16_bus9(struct fbtft_par *par, size_t offset, size_t len);
+void fbtft_write_reg8_bus8(struct fbtft_par *par, int len, ...);
+void fbtft_write_reg8_bus9(struct fbtft_par *par, int len, ...);
+void fbtft_write_reg16_bus8(struct fbtft_par *par, int len, ...);
+void fbtft_write_reg16_bus16(struct fbtft_par *par, int len, ...);
+
+#define FBTFT_DT_TABLE(_compatible)						\
+static const struct of_device_id dt_ids[] = {					\
+	{ .compatible = _compatible },						\
+	{},									\
+};										\
+MODULE_DEVICE_TABLE(of, dt_ids);
+
+#define FBTFT_SPI_DRIVER(_name, _compatible, _display, _spi_ids)		\
+										\
+static int fbtft_driver_probe_spi(struct spi_device *spi)			\
+{										\
+	return fbtft_probe_common(_display, spi, NULL);				\
+}										\
+										\
+static void fbtft_driver_remove_spi(struct spi_device *spi)			\
+{										\
+	struct fb_info *info = spi_get_drvdata(spi);				\
+										\
+	fbtft_remove_common(&spi->dev, info);					\
+}										\
+										\
+static struct spi_driver fbtft_driver_spi_driver = {				\
+	.driver = {								\
+		.name = _name,							\
+		.of_match_table = dt_ids,					\
+	},									\
+	.id_table = _spi_ids,							\
+	.probe = fbtft_driver_probe_spi,					\
+	.remove = fbtft_driver_remove_spi,					\
+};
+
+#define FBTFT_REGISTER_DRIVER(_name, _compatible, _display)                \
+									   \
+static int fbtft_driver_probe_pdev(struct platform_device *pdev)           \
+{                                                                          \
+	return fbtft_probe_common(_display, NULL, pdev);                   \
+}                                                                          \
+									   \
+static int fbtft_driver_remove_pdev(struct platform_device *pdev)          \
+{                                                                          \
+	struct fb_info *info = platform_get_drvdata(pdev);                 \
+									   \
+	fbtft_remove_common(&pdev->dev, info);                             \
+	return 0;                                                          \
+}                                                                          \
+									   \
+FBTFT_DT_TABLE(_compatible)						   \
+									   \
+FBTFT_SPI_DRIVER(_name, _compatible, _display, NULL)			   \
+									   \
+static struct platform_driver fbtft_driver_platform_driver = {             \
+	.driver = {                                                        \
+		.name   = _name,                                           \
+		.owner  = THIS_MODULE,                                     \
+		.of_match_table = dt_ids,                                  \
+	},                                                                 \
+	.probe  = fbtft_driver_probe_pdev,                                 \
+	.remove = fbtft_driver_remove_pdev,                                \
+};                                                                         \
+									   \
+static int __init fbtft_driver_module_init(void)                           \
+{                                                                          \
+	int ret;                                                           \
+									   \
+	ret = spi_register_driver(&fbtft_driver_spi_driver);               \
+	if (ret < 0)                                                       \
+		return ret;                                                \
+	ret = platform_driver_register(&fbtft_driver_platform_driver);     \
+	if (ret < 0)                                                       \
+		spi_unregister_driver(&fbtft_driver_spi_driver);           \
+	return ret;                                                        \
+}                                                                          \
+									   \
+static void __exit fbtft_driver_module_exit(void)                          \
+{                                                                          \
+	spi_unregister_driver(&fbtft_driver_spi_driver);                   \
+	platform_driver_unregister(&fbtft_driver_platform_driver);         \
+}                                                                          \
+									   \
+module_init(fbtft_driver_module_init);                                     \
+module_exit(fbtft_driver_module_exit);
+
+#define FBTFT_REGISTER_SPI_DRIVER(_name, _comp_vend, _comp_dev, _display)	\
+										\
+FBTFT_DT_TABLE(_comp_vend "," _comp_dev)					\
+										\
+static const struct spi_device_id spi_ids[] = {					\
+	{ .name = _comp_dev },							\
+	{},									\
+};										\
+MODULE_DEVICE_TABLE(spi, spi_ids);						\
+										\
+FBTFT_SPI_DRIVER(_name, _comp_vend "," _comp_dev, _display, spi_ids)		\
+										\
+module_spi_driver(fbtft_driver_spi_driver);
+
+/* Debug macros */
+
+/* shorthand debug levels */
+#define DEBUG_LEVEL_1	DEBUG_REQUEST_GPIOS
+#define DEBUG_LEVEL_2	(DEBUG_LEVEL_1 | DEBUG_DRIVER_INIT_FUNCTIONS        \
+				       | DEBUG_TIME_FIRST_UPDATE)
+#define DEBUG_LEVEL_3	(DEBUG_LEVEL_2 | DEBUG_RESET | DEBUG_INIT_DISPLAY   \
+				       | DEBUG_BLANK | DEBUG_REQUEST_GPIOS  \
+				       | DEBUG_FREE_GPIOS                   \
+				       | DEBUG_VERIFY_GPIOS                 \
+				       | DEBUG_BACKLIGHT | DEBUG_SYSFS)
+#define DEBUG_LEVEL_4	(DEBUG_LEVEL_2 | DEBUG_FB_READ | DEBUG_FB_WRITE     \
+				       | DEBUG_FB_FILLRECT                  \
+				       | DEBUG_FB_COPYAREA                  \
+				       | DEBUG_FB_IMAGEBLIT | DEBUG_FB_BLANK)
+#define DEBUG_LEVEL_5	(DEBUG_LEVEL_3 | DEBUG_UPDATE_DISPLAY)
+#define DEBUG_LEVEL_6	(DEBUG_LEVEL_4 | DEBUG_LEVEL_5)
+#define DEBUG_LEVEL_7	0xFFFFFFFF
+
+#define DEBUG_DRIVER_INIT_FUNCTIONS BIT(3)
+#define DEBUG_TIME_FIRST_UPDATE     BIT(4)
+#define DEBUG_TIME_EACH_UPDATE      BIT(5)
+#define DEBUG_DEFERRED_IO           BIT(6)
+#define DEBUG_FBTFT_INIT_FUNCTIONS  BIT(7)
+
+/* fbops */
+#define DEBUG_FB_READ               BIT(8)
+#define DEBUG_FB_WRITE              BIT(9)
+#define DEBUG_FB_FILLRECT           BIT(10)
+#define DEBUG_FB_COPYAREA           BIT(11)
+#define DEBUG_FB_IMAGEBLIT          BIT(12)
+#define DEBUG_FB_SETCOLREG          BIT(13)
+#define DEBUG_FB_BLANK              BIT(14)
+
+#define DEBUG_SYSFS                 BIT(16)
+
+/* fbtftops */
+#define DEBUG_BACKLIGHT             BIT(17)
+#define DEBUG_READ                  BIT(18)
+#define DEBUG_WRITE                 BIT(19)
+#define DEBUG_WRITE_VMEM            BIT(20)
+#define DEBUG_WRITE_REGISTER        BIT(21)
+#define DEBUG_SET_ADDR_WIN          BIT(22)
+#define DEBUG_RESET                 BIT(23)
+#define DEBUG_MKDIRTY               BIT(24)
+#define DEBUG_UPDATE_DISPLAY        BIT(25)
+#define DEBUG_INIT_DISPLAY          BIT(26)
+#define DEBUG_BLANK                 BIT(27)
+#define DEBUG_REQUEST_GPIOS         BIT(28)
+#define DEBUG_FREE_GPIOS            BIT(29)
+#define DEBUG_REQUEST_GPIOS_MATCH   BIT(30)
+#define DEBUG_VERIFY_GPIOS          BIT(31)
+
+#define fbtft_init_dbg(dev, format, arg...)                  \
+do {                                                         \
+	if (unlikely((dev)->platform_data &&                 \
+	    (((struct fbtft_platform_data *)(dev)->platform_data)->display.debug & DEBUG_DRIVER_INIT_FUNCTIONS))) \
+		dev_info(dev, format, ##arg);                \
+} while (0)
+
+#define fbtft_par_dbg(level, par, format, arg...)            \
+do {                                                         \
+	if (unlikely((par)->debug & (level)))                    \
+		dev_info((par)->info->device, format, ##arg);  \
+} while (0)
+
+#define fbtft_par_dbg_hex(level, par, dev, type, buf, num, format, arg...) \
+do {                                                                       \
+	if (unlikely((par)->debug & (level)))                                  \
+		fbtft_dbg_hex(dev, sizeof(type), buf,\
+			      (num) * sizeof(type), format, ##arg); \
+} while (0)
+
+#endif /* __LINUX_FBTFT_H */
diff --git a/drivers/staging/fbtft/internal.h b/drivers/staging/fbtft/internal.h
new file mode 100644
index 000000000..ae2ff4a4a
--- /dev/null
+++ b/drivers/staging/fbtft/internal.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/* Copyright (C) 2013 Noralf Tronnes */
+
+#ifndef __LINUX_FBTFT_INTERNAL_H
+#define __LINUX_FBTFT_INTERNAL_H
+
+void fbtft_sysfs_init(struct fbtft_par *par);
+void fbtft_sysfs_exit(struct fbtft_par *par);
+void fbtft_expand_debug_value(unsigned long *debug);
+int fbtft_gamma_parse_str(struct fbtft_par *par, u32 *curves,
+			  const char *str, int size);
+
+#endif /* __LINUX_FBTFT_INTERNAL_H */
diff --git a/drivers/staging/fieldbus/Documentation/ABI/fieldbus-dev-cdev b/drivers/staging/fieldbus/Documentation/ABI/fieldbus-dev-cdev
new file mode 100644
index 000000000..45f631ea3
--- /dev/null
+++ b/drivers/staging/fieldbus/Documentation/ABI/fieldbus-dev-cdev
@@ -0,0 +1,31 @@
+What:		/dev/fieldbus_devX
+Date:		December 2018
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		The cdev interface to drivers for Fieldbus Device Memory
+			(aka. Process Memory).
+
+		The following file operations are supported:
+
+		open(2)
+		Create an I/O context associated with the file descriptor.
+
+		read(2)
+		Read from Process Memory's "read area".
+		Clears POLLERR | POLLPRI from the file descriptor.
+
+		write(2)
+		Write to Process Memory's "write area".
+
+		poll(2), select(2), epoll_wait(2) etc.
+		When a "Process Memory Read Area Changed" event occurs,
+		POLLERR | POLLPRI will be set on the file descriptor.
+		Note that POLLIN | POLLOUT events are always set, because the
+		process memory area is always readable and writable.
+
+		close(2)
+		Free up the I/O context that was associated
+		with the file descriptor.
+
+Users:		TBD
diff --git a/drivers/staging/fieldbus/Documentation/ABI/sysfs-class-fieldbus-dev b/drivers/staging/fieldbus/Documentation/ABI/sysfs-class-fieldbus-dev
new file mode 100644
index 000000000..439f14d33
--- /dev/null
+++ b/drivers/staging/fieldbus/Documentation/ABI/sysfs-class-fieldbus-dev
@@ -0,0 +1,62 @@
+What:		/sys/class/fieldbus_dev/fieldbus_devX/card_name
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		Human-readable name of the Fieldbus Device.
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/fieldbus_type
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		The type of fieldbus implemented by this device.
+		Possible values:
+			'unknown'
+			'profinet'
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/fieldbus_id
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		The unique fieldbus id associated with this device.
+		The exact format of this id is fieldbus type dependent, e.g.
+		a mac address for profinet.
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/read_area_size
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		The size, in bytes, of the Process Memory read area.
+		Note: this area is accessible by reading from the associated
+			character device (/dev/fieldbus_devX).
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/write_area_size
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		The size, in bytes, of the Process Memory write area.
+		Note: this area is accessible by writing to the associated
+			character device (/dev/fieldbus_devX)
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/online
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		Whether the fieldbus is online or offline.
+		Possible values:
+			'1' meaning 'online'
+			'0' meaning 'offline'
+		Note: an uevent is generated when this property changes.
+
+What:		/sys/class/fieldbus_dev/fieldbus_devX/enabled
+KernelVersion:	5.1 (staging)
+Contact:	Sven Van Asbroeck <TheSven73@gmail.com>
+Description:
+		Whether the device is enabled (power on) or
+			disabled (power off).
+		Possible values:
+			'1' meaning enabled
+			'0' meaning disabled
+		Normally a r/o property, but optionally r/w:
+		Writing '1' enables the device (power on) with default
+			settings.
+		Writing '0' disables the card (power off).
diff --git a/drivers/staging/fieldbus/Documentation/devicetree/bindings/fieldbus/arcx,anybus-controller.txt b/drivers/staging/fieldbus/Documentation/devicetree/bindings/fieldbus/arcx,anybus-controller.txt
new file mode 100644
index 000000000..b1f9474f3
--- /dev/null
+++ b/drivers/staging/fieldbus/Documentation/devicetree/bindings/fieldbus/arcx,anybus-controller.txt
@@ -0,0 +1,71 @@
+* Arcx Anybus-S controller
+
+This chip communicates with the SoC over a parallel bus. It is
+expected that its Device Tree node is specified as the child of a node
+corresponding to the parallel bus used for communication.
+
+Required properties:
+--------------------
+
+  - compatible : The following chip-specific string:
+        "arcx,anybus-controller"
+
+  - reg : three areas:
+	index 0: bus memory area where the cpld registers are located.
+	index 1: bus memory area of the first  host's dual-port ram.
+	index 2: bus memory area of the second host's dual-port ram.
+
+  - reset-gpios : the GPIO pin connected to the reset line of the controller.
+
+  - interrupts : two interrupts:
+		index 0: interrupt connected to the first  host
+		index 1: interrupt connected to the second host
+	Generic interrupt client node bindings are described in
+	interrupt-controller/interrupts.txt
+
+Optional: use of subnodes
+-------------------------
+
+The card connected to a host may need additional properties. These can be
+specified in subnodes to the controller node.
+
+The subnodes are identified by the standard 'reg' property. Which information
+exactly can be specified depends on the bindings for the function driver
+for the subnode.
+
+Required controller node properties when using subnodes:
+- #address-cells: should be one.
+- #size-cells: should be zero.
+
+Required subnode properties:
+- reg: Must contain the host index of the card this subnode describes:
+		<0>	for the first  host on the controller
+		<1>	for the second host on the controller
+	Note that only a single card can be plugged into a host, so the host
+	index uniquely describes the card location.
+
+Example of usage:
+-----------------
+
+This example places the bridge on top of the i.MX WEIM parallel bus, see:
+Documentation/devicetree/bindings/bus/imx-weim.txt
+
+&weim {
+	controller@0,0 {
+		compatible = "arcx,anybus-controller";
+		reg = <0 0 0x100>, <0 0x400000 0x800>, <1 0x400000 0x800>;
+		reset-gpios = <&gpio5 2 GPIO_ACTIVE_HIGH>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <1 IRQ_TYPE_LEVEL_LOW>, <5 IRQ_TYPE_LEVEL_LOW>;
+		/* fsl,weim-cs-timing is a i.MX WEIM bus specific property */
+		fsl,weim-cs-timing = <0x024400b1 0x00001010 0x20081100
+				0x00000000 0xa0000240 0x00000000>;
+		/* optional subnode for a card plugged into the first host */
+		#address-cells = <1>;
+		#size-cells = <0>;
+		card@0 {
+			reg = <0>;
+			/* card specific properties go here */
+		};
+	};
+};
diff --git a/drivers/staging/fieldbus/Documentation/fieldbus_dev.txt b/drivers/staging/fieldbus/Documentation/fieldbus_dev.txt
new file mode 100644
index 000000000..89fb8e146
--- /dev/null
+++ b/drivers/staging/fieldbus/Documentation/fieldbus_dev.txt
@@ -0,0 +1,66 @@
+                       Fieldbus-Device Subsystem
+               ============================================
+
+Part 0 - What is a Fieldbus Device ?
+------------------------------------
+
+Fieldbus is the name of a family of industrial computer network protocols used
+for real-time distributed control, standardized as IEC 61158.
+
+A complex automated industrial system -- such as manufacturing assembly line --
+usually needs a distributed control system -- an organized hierarchy of
+controller systems -- to function. In this hierarchy, there is usually a
+Human Machine Interface (HMI) at the top, where an operator can monitor or
+operate the system. This is typically linked to a middle layer of programmable
+logic controllers (PLC) via a non-time-critical communications system
+(e.g. Ethernet). At the bottom of the control chain is the fieldbus that links
+the PLCs to the components that actually do the work, such as sensors,
+actuators, electric motors, console lights, switches, valves and contactors.
+
+(Source: Wikipedia)
+
+A "Fieldbus Device" is such an actuator, motor, console light, switch, ...
+controlled via the Fieldbus by a PLC aka "Fieldbus Controller".
+
+Communication between PLC and device typically happens via process data memory,
+separated into input and output areas. The Fieldbus then cyclically transfers
+the PLC's output area to the device's input area, and vice versa.
+
+Part I - Why do we need this subsystem?
+---------------------------------------
+
+Fieldbus device (client) adapters are commercially available. They allow data
+exchange with a PLC aka "Fieldbus Controller" via process data memory.
+
+They are typically used when a Linux device wants to expose itself as an
+actuator, motor, console light, switch, etc. over the fieldbus.
+
+The purpose of this subsystem is:
+a) present a general, standardized, extensible API/ABI to userspace; and
+b) present a convenient interface to drivers.
+
+Part II - How can drivers use the subsystem?
+--------------------------------------------
+
+Any driver that wants to register as a Fieldbus Device should allocate and
+populate a 'struct fieldbus_dev' (from include/linux/fieldbus_dev.h).
+Registration then happens by calling fieldbus_dev_register().
+
+Part III - How can userspace use the subsystem?
+-----------------------------------------------
+
+Fieldbus protocols and adapters are diverse and varied. However, they share
+a limited few common behaviours and properties. This allows us to define
+a simple interface consisting of a character device and a set of sysfs files:
+
+See:
+drivers/staging/fieldbus/Documentation/ABI/sysfs-class-fieldbus-dev
+drivers/staging/fieldbus/Documentation/ABI/fieldbus-dev-cdev
+
+Note that this simple interface does not provide a way to modify adapter
+configuration settings. It is therefore useful only for adapters that get their
+configuration settings some other way, e.g. non-volatile memory on the adapter,
+through the network, ...
+
+At a later phase, this simple interface can easily co-exist with a future
+(netlink-based ?) configuration settings interface.
diff --git a/drivers/staging/fieldbus/Kconfig b/drivers/staging/fieldbus/Kconfig
new file mode 100644
index 000000000..b0b865acc
--- /dev/null
+++ b/drivers/staging/fieldbus/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+menuconfig FIELDBUS_DEV
+	tristate "Fieldbus Device Support"
+	help
+	  Support for Fieldbus Device Adapters.
+
+	  Fieldbus device (client) adapters allow data exchange with a PLC aka.
+	  "Fieldbus Controller" over a fieldbus (Profinet, FLNet, etc.)
+
+	  They are typically used when a Linux device wants to expose itself
+	  as an actuator, motor, console light, switch, etc. over the fieldbus.
+
+	  This framework is designed to provide a generic interface to Fieldbus
+	  Devices from both the Linux Kernel and the userspace.
+
+	  If unsure, say no.
+
+source "drivers/staging/fieldbus/anybuss/Kconfig"
+
diff --git a/drivers/staging/fieldbus/Makefile b/drivers/staging/fieldbus/Makefile
new file mode 100644
index 000000000..bdf645d41
--- /dev/null
+++ b/drivers/staging/fieldbus/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for fieldbus_dev drivers.
+#
+
+obj-$(CONFIG_FIELDBUS_DEV)	+= fieldbus_dev.o anybuss/
+fieldbus_dev-y			:= dev_core.o
diff --git a/drivers/staging/fieldbus/TODO b/drivers/staging/fieldbus/TODO
new file mode 100644
index 000000000..6d6626af4
--- /dev/null
+++ b/drivers/staging/fieldbus/TODO
@@ -0,0 +1,5 @@
+TODO:
+-Get more people/drivers to use the Fieldbus userspace ABI. It requires
+ verification/sign-off by multiple users.
+
+Contact: Sven Van Asbroeck <TheSven73@gmail.com>
diff --git a/drivers/staging/fieldbus/anybuss/Kconfig b/drivers/staging/fieldbus/anybuss/Kconfig
new file mode 100644
index 000000000..635a0a7b7
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/Kconfig
@@ -0,0 +1,41 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config HMS_ANYBUSS_BUS
+	tristate "HMS Anybus-S Bus Support"
+	select REGMAP
+	depends on OF && FIELDBUS_DEV
+	help
+	  Driver for the HMS Industrial Networks Anybus-S bus.
+	  You can attach a single Anybus-S compatible card to it, which
+	  typically provides fieldbus and industrial ethernet
+	  functionality.
+
+if HMS_ANYBUSS_BUS
+
+config ARCX_ANYBUS_CONTROLLER
+	tristate "Arcx Anybus-S Controller"
+	depends on OF && GPIOLIB && HAS_IOMEM && REGULATOR
+	select REGMAP_MMIO
+	help
+	  Select this to get support for the Arcx Anybus controller.
+	  It connects to the SoC via a parallel memory bus, and
+	  embeds up to two Anybus-S buses (slots).
+	  There is also a CAN power readout, unrelated to the Anybus,
+	  modelled as a regulator.
+
+config HMS_PROFINET
+	tristate "HMS Profinet IRT Controller (Anybus-S)"
+	depends on FIELDBUS_DEV && HMS_ANYBUSS_BUS
+	help
+	  If you say yes here you get support for the HMS Industrial
+	  Networks Profinet IRT Controller.
+
+	  It will be registered with the kernel as a fieldbus_dev,
+	  so userspace can interact with it via the fieldbus_dev userspace
+	  interface(s).
+
+	  This driver can also be built as a module. If so, the module
+	  will be called hms-profinet.
+
+	  If unsure, say N.
+
+endif
diff --git a/drivers/staging/fieldbus/anybuss/Makefile b/drivers/staging/fieldbus/anybuss/Makefile
new file mode 100644
index 000000000..3ad3dcc6b
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/Makefile
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for anybuss drivers.
+#
+
+obj-$(CONFIG_HMS_ANYBUSS_BUS)	+= anybuss_core.o
+anybuss_core-y			+= host.o
+
+obj-$(CONFIG_ARCX_ANYBUS_CONTROLLER) += arcx-anybus.o
+obj-$(CONFIG_HMS_PROFINET)	+= hms-profinet.o
diff --git a/drivers/staging/fieldbus/anybuss/anybuss-client.h b/drivers/staging/fieldbus/anybuss/anybuss-client.h
new file mode 100644
index 000000000..a21968800
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/anybuss-client.h
@@ -0,0 +1,99 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Anybus-S client adapter definitions
+ *
+ * Copyright 2018 Arcx Inc
+ */
+
+#ifndef __LINUX_ANYBUSS_CLIENT_H__
+#define __LINUX_ANYBUSS_CLIENT_H__
+
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/poll.h>
+
+/* move to <linux/fieldbus_dev.h> when taking this out of staging */
+#include "../fieldbus_dev.h"
+
+struct anybuss_host;
+
+struct anybuss_client {
+	struct device dev;
+	struct anybuss_host *host;
+	__be16 anybus_id;
+	/*
+	 * these can be optionally set by the client to receive event
+	 * notifications from the host.
+	 */
+	void (*on_area_updated)(struct anybuss_client *client);
+	void (*on_online_changed)(struct anybuss_client *client, bool online);
+};
+
+struct anybuss_client_driver {
+	struct device_driver driver;
+	int (*probe)(struct anybuss_client *adev);
+	void (*remove)(struct anybuss_client *adev);
+	u16 anybus_id;
+};
+
+int anybuss_client_driver_register(struct anybuss_client_driver *drv);
+void anybuss_client_driver_unregister(struct anybuss_client_driver *drv);
+
+static inline struct anybuss_client *to_anybuss_client(struct device *dev)
+{
+	return container_of(dev, struct anybuss_client, dev);
+}
+
+static inline struct anybuss_client_driver *
+to_anybuss_client_driver(struct device_driver *drv)
+{
+	return container_of(drv, struct anybuss_client_driver, driver);
+}
+
+static inline void *
+anybuss_get_drvdata(const struct anybuss_client *client)
+{
+	return dev_get_drvdata(&client->dev);
+}
+
+static inline void
+anybuss_set_drvdata(struct anybuss_client *client, void *data)
+{
+	dev_set_drvdata(&client->dev, data);
+}
+
+int anybuss_set_power(struct anybuss_client *client, bool power_on);
+
+struct anybuss_memcfg {
+	u16 input_io;
+	u16 input_dpram;
+	u16 input_total;
+
+	u16 output_io;
+	u16 output_dpram;
+	u16 output_total;
+
+	enum fieldbus_dev_offl_mode offl_mode;
+};
+
+int anybuss_start_init(struct anybuss_client *client,
+		       const struct anybuss_memcfg *cfg);
+int anybuss_finish_init(struct anybuss_client *client);
+int anybuss_read_fbctrl(struct anybuss_client *client, u16 addr,
+			void *buf, size_t count);
+int anybuss_send_msg(struct anybuss_client *client, u16 cmd_num,
+		     const void *buf, size_t count);
+int anybuss_send_ext(struct anybuss_client *client, u16 cmd_num,
+		     const void *buf, size_t count);
+int anybuss_recv_msg(struct anybuss_client *client, u16 cmd_num,
+		     void *buf, size_t count);
+
+/* these help clients make a struct file_operations */
+int anybuss_write_input(struct anybuss_client *client,
+			const char __user *buf, size_t size,
+				loff_t *offset);
+int anybuss_read_output(struct anybuss_client *client,
+			char __user *buf, size_t size,
+				loff_t *offset);
+
+#endif /* __LINUX_ANYBUSS_CLIENT_H__ */
diff --git a/drivers/staging/fieldbus/anybuss/anybuss-controller.h b/drivers/staging/fieldbus/anybuss/anybuss-controller.h
new file mode 100644
index 000000000..02fa07490
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/anybuss-controller.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Anybus-S controller definitions
+ *
+ * Copyright 2018 Arcx Inc
+ */
+
+#ifndef __LINUX_ANYBUSS_CONTROLLER_H__
+#define __LINUX_ANYBUSS_CONTROLLER_H__
+
+#include <linux/device.h>
+#include <linux/regmap.h>
+
+/*
+ * To instantiate an Anybus-S host, a controller should provide the following:
+ * - a reset function which resets the attached card;
+ * - a regmap which provides access to the attached card's dpram;
+ * - the irq of the attached card
+ */
+/**
+ * struct anybuss_ops - Controller resources to instantiate an Anybus-S host
+ *
+ * @reset:	asserts/deasserts the anybus card's reset line.
+ * @regmap:	provides access to the card's dual-port RAM area.
+ * @irq:	number of the interrupt connected to the card's interrupt line.
+ * @host_idx:	for multi-host controllers, the host index:
+ *		0 for the first host on the controller, 1 for the second, etc.
+ */
+struct anybuss_ops {
+	void (*reset)(struct device *dev, bool assert);
+	struct regmap *regmap;
+	int irq;
+	int host_idx;
+};
+
+struct anybuss_host;
+
+struct anybuss_host * __must_check
+anybuss_host_common_probe(struct device *dev,
+			  const struct anybuss_ops *ops);
+void anybuss_host_common_remove(struct anybuss_host *host);
+
+struct anybuss_host * __must_check
+devm_anybuss_host_common_probe(struct device *dev,
+			       const struct anybuss_ops *ops);
+
+#endif /* __LINUX_ANYBUSS_CONTROLLER_H__ */
diff --git a/drivers/staging/fieldbus/anybuss/arcx-anybus.c b/drivers/staging/fieldbus/anybuss/arcx-anybus.c
new file mode 100644
index 000000000..9af2e6305
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/arcx-anybus.c
@@ -0,0 +1,377 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Arcx Anybus-S Controller driver
+ *
+ * Copyright (C) 2018 Arcx Inc
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <linux/idr.h>
+#include <linux/mutex.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regmap.h>
+
+/* move to <linux/anybuss-controller.h> when taking this out of staging */
+#include "anybuss-controller.h"
+
+#define CPLD_STATUS1		0x80
+#define CPLD_CONTROL		0x80
+#define CPLD_CONTROL_CRST	0x40
+#define CPLD_CONTROL_RST1	0x04
+#define CPLD_CONTROL_RST2	0x80
+#define CPLD_STATUS1_AB		0x02
+#define CPLD_STATUS1_CAN_POWER	0x01
+#define CPLD_DESIGN_LO		0x81
+#define CPLD_DESIGN_HI		0x82
+#define CPLD_CAP		0x83
+#define CPLD_CAP_COMPAT		0x01
+#define CPLD_CAP_SEP_RESETS	0x02
+
+struct controller_priv {
+	struct device *class_dev;
+	bool common_reset;
+	struct gpio_desc *reset_gpiod;
+	void __iomem *cpld_base;
+	struct mutex ctrl_lock; /* protects CONTROL register */
+	u8 control_reg;
+	char version[3];
+	u16 design_no;
+};
+
+static void do_reset(struct controller_priv *cd, u8 rst_bit, bool reset)
+{
+	mutex_lock(&cd->ctrl_lock);
+	/*
+	 * CPLD_CONTROL is write-only, so cache its value in
+	 * cd->control_reg
+	 */
+	if (reset)
+		cd->control_reg &= ~rst_bit;
+	else
+		cd->control_reg |= rst_bit;
+	writeb(cd->control_reg, cd->cpld_base + CPLD_CONTROL);
+	/*
+	 * h/w work-around:
+	 * the hardware is 'too fast', so a reset followed by an immediate
+	 * not-reset will _not_ change the anybus reset line in any way,
+	 * losing the reset. to prevent this from happening, introduce
+	 * a minimum reset duration.
+	 * Verified minimum safe duration required using a scope
+	 * on 14-June-2018: 100 us.
+	 */
+	if (reset)
+		usleep_range(100, 200);
+	mutex_unlock(&cd->ctrl_lock);
+}
+
+static int anybuss_reset(struct controller_priv *cd,
+			 unsigned long id, bool reset)
+{
+	if (id >= 2)
+		return -EINVAL;
+	if (cd->common_reset)
+		do_reset(cd, CPLD_CONTROL_CRST, reset);
+	else
+		do_reset(cd, id ? CPLD_CONTROL_RST2 : CPLD_CONTROL_RST1, reset);
+	return 0;
+}
+
+static void export_reset_0(struct device *dev, bool assert)
+{
+	struct controller_priv *cd = dev_get_drvdata(dev);
+
+	anybuss_reset(cd, 0, assert);
+}
+
+static void export_reset_1(struct device *dev, bool assert)
+{
+	struct controller_priv *cd = dev_get_drvdata(dev);
+
+	anybuss_reset(cd, 1, assert);
+}
+
+/*
+ * parallel bus limitation:
+ *
+ * the anybus is 8-bit wide. we can't assume that the hardware will translate
+ * word accesses on the parallel bus to multiple byte-accesses on the anybus.
+ *
+ * the imx WEIM bus does not provide this type of translation.
+ *
+ * to be safe, we will limit parallel bus accesses to a single byte
+ * at a time for now.
+ */
+
+static const struct regmap_config arcx_regmap_cfg = {
+	.reg_bits = 16,
+	.val_bits = 8,
+	.max_register = 0x7ff,
+	.use_single_read = true,
+	.use_single_write = true,
+	/*
+	 * single-byte parallel bus accesses are atomic, so don't
+	 * require any synchronization.
+	 */
+	.disable_locking = true,
+};
+
+static struct regmap *create_parallel_regmap(struct platform_device *pdev,
+					     int idx)
+{
+	void __iomem *base;
+	struct device *dev = &pdev->dev;
+
+	base = devm_platform_ioremap_resource(pdev, idx + 1);
+	if (IS_ERR(base))
+		return ERR_CAST(base);
+	return devm_regmap_init_mmio(dev, base, &arcx_regmap_cfg);
+}
+
+static struct anybuss_host *
+create_anybus_host(struct platform_device *pdev, int idx)
+{
+	struct anybuss_ops ops = {};
+
+	switch (idx) {
+	case 0:
+		ops.reset = export_reset_0;
+		break;
+	case 1:
+		ops.reset = export_reset_1;
+		break;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+	ops.host_idx = idx;
+	ops.regmap = create_parallel_regmap(pdev, idx);
+	if (IS_ERR(ops.regmap))
+		return ERR_CAST(ops.regmap);
+	ops.irq = platform_get_irq(pdev, idx);
+	if (ops.irq <= 0)
+		return ERR_PTR(-EINVAL);
+	return devm_anybuss_host_common_probe(&pdev->dev, &ops);
+}
+
+static ssize_t version_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct controller_priv *cd = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", cd->version);
+}
+static DEVICE_ATTR_RO(version);
+
+static ssize_t design_number_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct controller_priv *cd = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", cd->design_no);
+}
+static DEVICE_ATTR_RO(design_number);
+
+static struct attribute *controller_attributes[] = {
+	&dev_attr_version.attr,
+	&dev_attr_design_number.attr,
+	NULL,
+};
+
+static const struct attribute_group controller_attribute_group = {
+	.attrs = controller_attributes,
+};
+
+static const struct attribute_group *controller_attribute_groups[] = {
+	&controller_attribute_group,
+	NULL,
+};
+
+static void controller_device_release(struct device *dev)
+{
+	kfree(dev);
+}
+
+static int can_power_is_enabled(struct regulator_dev *rdev)
+{
+	struct controller_priv *cd = rdev_get_drvdata(rdev);
+
+	return !(readb(cd->cpld_base + CPLD_STATUS1) & CPLD_STATUS1_CAN_POWER);
+}
+
+static const struct regulator_ops can_power_ops = {
+	.is_enabled = can_power_is_enabled,
+};
+
+static const struct regulator_desc can_power_desc = {
+	.name = "regulator-can-power",
+	.id = -1,
+	.type = REGULATOR_VOLTAGE,
+	.owner = THIS_MODULE,
+	.ops = &can_power_ops,
+};
+
+static struct class *controller_class;
+static DEFINE_IDA(controller_index_ida);
+
+static int controller_probe(struct platform_device *pdev)
+{
+	struct controller_priv *cd;
+	struct device *dev = &pdev->dev;
+	struct regulator_config config = { };
+	struct regulator_dev *regulator;
+	int err, id;
+	struct anybuss_host *host;
+	u8 status1, cap;
+
+	cd = devm_kzalloc(dev, sizeof(*cd), GFP_KERNEL);
+	if (!cd)
+		return -ENOMEM;
+	dev_set_drvdata(dev, cd);
+	mutex_init(&cd->ctrl_lock);
+	cd->reset_gpiod = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(cd->reset_gpiod))
+		return PTR_ERR(cd->reset_gpiod);
+
+	/* CPLD control memory, sits at index 0 */
+	cd->cpld_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(cd->cpld_base)) {
+		dev_err(dev,
+			"failed to map cpld base address\n");
+		err = PTR_ERR(cd->cpld_base);
+		goto out_reset;
+	}
+
+	/* identify cpld */
+	status1 = readb(cd->cpld_base + CPLD_STATUS1);
+	cd->design_no = (readb(cd->cpld_base + CPLD_DESIGN_HI) << 8) |
+				readb(cd->cpld_base + CPLD_DESIGN_LO);
+	snprintf(cd->version, sizeof(cd->version), "%c%d",
+		 'A' + ((status1 >> 5) & 0x7),
+		 (status1 >> 2) & 0x7);
+	dev_info(dev, "design number %d, revision %s\n",
+		 cd->design_no,
+		cd->version);
+	cap = readb(cd->cpld_base + CPLD_CAP);
+	if (!(cap & CPLD_CAP_COMPAT)) {
+		dev_err(dev, "unsupported controller [cap=0x%02X]", cap);
+		err = -ENODEV;
+		goto out_reset;
+	}
+
+	if (status1 & CPLD_STATUS1_AB) {
+		dev_info(dev, "has anybus-S slot(s)");
+		cd->common_reset = !(cap & CPLD_CAP_SEP_RESETS);
+		dev_info(dev, "supports %s", cd->common_reset ?
+			"a common reset" : "separate resets");
+		for (id = 0; id < 2; id++) {
+			host = create_anybus_host(pdev, id);
+			if (!IS_ERR(host))
+				continue;
+			err = PTR_ERR(host);
+			/* -ENODEV is fine, it just means no card detected */
+			if (err != -ENODEV)
+				goto out_reset;
+		}
+	}
+
+	id = ida_simple_get(&controller_index_ida, 0, 0, GFP_KERNEL);
+	if (id < 0) {
+		err = id;
+		goto out_reset;
+	}
+	/* export can power readout as a regulator */
+	config.dev = dev;
+	config.driver_data = cd;
+	regulator = devm_regulator_register(dev, &can_power_desc, &config);
+	if (IS_ERR(regulator)) {
+		err = PTR_ERR(regulator);
+		goto out_ida;
+	}
+	/* make controller info visible to userspace */
+	cd->class_dev = kzalloc(sizeof(*cd->class_dev), GFP_KERNEL);
+	if (!cd->class_dev) {
+		err = -ENOMEM;
+		goto out_ida;
+	}
+	cd->class_dev->class = controller_class;
+	cd->class_dev->groups = controller_attribute_groups;
+	cd->class_dev->parent = dev;
+	cd->class_dev->id = id;
+	cd->class_dev->release = controller_device_release;
+	dev_set_name(cd->class_dev, "%d", cd->class_dev->id);
+	dev_set_drvdata(cd->class_dev, cd);
+	err = device_register(cd->class_dev);
+	if (err)
+		goto out_dev;
+	return 0;
+out_dev:
+	put_device(cd->class_dev);
+out_ida:
+	ida_simple_remove(&controller_index_ida, id);
+out_reset:
+	gpiod_set_value_cansleep(cd->reset_gpiod, 1);
+	return err;
+}
+
+static int controller_remove(struct platform_device *pdev)
+{
+	struct controller_priv *cd = platform_get_drvdata(pdev);
+	int id = cd->class_dev->id;
+
+	device_unregister(cd->class_dev);
+	ida_simple_remove(&controller_index_ida, id);
+	gpiod_set_value_cansleep(cd->reset_gpiod, 1);
+	return 0;
+}
+
+static const struct of_device_id controller_of_match[] = {
+	{ .compatible = "arcx,anybus-controller" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, controller_of_match);
+
+static struct platform_driver controller_driver = {
+	.probe = controller_probe,
+	.remove = controller_remove,
+	.driver		= {
+		.name   = "arcx-anybus-controller",
+		.of_match_table	= of_match_ptr(controller_of_match),
+	},
+};
+
+static int __init controller_init(void)
+{
+	int err;
+
+	controller_class = class_create(THIS_MODULE, "arcx_anybus_controller");
+	if (IS_ERR(controller_class))
+		return PTR_ERR(controller_class);
+	err = platform_driver_register(&controller_driver);
+	if (err)
+		class_destroy(controller_class);
+
+	return err;
+}
+
+static void __exit controller_exit(void)
+{
+	platform_driver_unregister(&controller_driver);
+	class_destroy(controller_class);
+	ida_destroy(&controller_index_ida);
+}
+
+module_init(controller_init);
+module_exit(controller_exit);
+
+MODULE_DESCRIPTION("Arcx Anybus-S Controller driver");
+MODULE_AUTHOR("Sven Van Asbroeck <TheSven73@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/fieldbus/anybuss/hms-profinet.c b/drivers/staging/fieldbus/anybuss/hms-profinet.c
new file mode 100644
index 000000000..e691736a5
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/hms-profinet.c
@@ -0,0 +1,224 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * HMS Profinet Client Driver
+ *
+ * Copyright (C) 2018 Arcx Inc
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+
+/* move to <linux/fieldbus_dev.h> when taking this out of staging */
+#include "../fieldbus_dev.h"
+
+/* move to <linux/anybuss-client.h> when taking this out of staging */
+#include "anybuss-client.h"
+
+#define PROFI_DPRAM_SIZE	512
+
+/*
+ * ---------------------------------------------------------------
+ * Anybus Profinet mailbox messages - definitions
+ * ---------------------------------------------------------------
+ * note that we're depending on the layout of these structures being
+ * exactly as advertised.
+ */
+
+struct msg_mac_addr {
+	u8 addr[6];
+};
+
+struct profi_priv {
+	struct fieldbus_dev fbdev;
+	struct anybuss_client *client;
+	struct mutex enable_lock; /* serializes card enable */
+	bool power_on;
+};
+
+static ssize_t
+profi_read_area(struct fieldbus_dev *fbdev, char __user *buf, size_t size,
+		loff_t *offset)
+{
+	struct profi_priv *priv = container_of(fbdev, struct profi_priv, fbdev);
+
+	return anybuss_read_output(priv->client, buf, size, offset);
+}
+
+static ssize_t
+profi_write_area(struct fieldbus_dev *fbdev, const char __user *buf,
+		 size_t size, loff_t *offset)
+{
+	struct profi_priv *priv = container_of(fbdev, struct profi_priv, fbdev);
+
+	return anybuss_write_input(priv->client, buf, size, offset);
+}
+
+static int profi_id_get(struct fieldbus_dev *fbdev, char *buf,
+			size_t max_size)
+{
+	struct profi_priv *priv = container_of(fbdev, struct profi_priv, fbdev);
+	struct msg_mac_addr response;
+	int ret;
+
+	ret = anybuss_recv_msg(priv->client, 0x0010, &response,
+			       sizeof(response));
+	if (ret < 0)
+		return ret;
+	return snprintf(buf, max_size, "%pM\n", response.addr);
+}
+
+static bool profi_enable_get(struct fieldbus_dev *fbdev)
+{
+	struct profi_priv *priv = container_of(fbdev, struct profi_priv, fbdev);
+	bool power_on;
+
+	mutex_lock(&priv->enable_lock);
+	power_on = priv->power_on;
+	mutex_unlock(&priv->enable_lock);
+
+	return power_on;
+}
+
+static int __profi_enable(struct profi_priv *priv)
+{
+	int ret;
+	struct anybuss_client *client = priv->client;
+	/* Initialization Sequence, Generic Anybus Mode */
+	const struct anybuss_memcfg mem_cfg = {
+		.input_io = 220,
+		.input_dpram = PROFI_DPRAM_SIZE,
+		.input_total = PROFI_DPRAM_SIZE,
+		.output_io = 220,
+		.output_dpram = PROFI_DPRAM_SIZE,
+		.output_total = PROFI_DPRAM_SIZE,
+		.offl_mode = FIELDBUS_DEV_OFFL_MODE_CLEAR,
+	};
+
+	/*
+	 * switch anybus off then on, this ensures we can do a complete
+	 * configuration cycle in case anybus was already on.
+	 */
+	anybuss_set_power(client, false);
+	ret = anybuss_set_power(client, true);
+	if (ret)
+		goto err;
+	ret = anybuss_start_init(client, &mem_cfg);
+	if (ret)
+		goto err;
+	ret = anybuss_finish_init(client);
+	if (ret)
+		goto err;
+	priv->power_on = true;
+	return 0;
+
+err:
+	anybuss_set_power(client, false);
+	priv->power_on = false;
+	return ret;
+}
+
+static int __profi_disable(struct profi_priv *priv)
+{
+	struct anybuss_client *client = priv->client;
+
+	anybuss_set_power(client, false);
+	priv->power_on = false;
+	return 0;
+}
+
+static int profi_simple_enable(struct fieldbus_dev *fbdev, bool enable)
+{
+	int ret;
+	struct profi_priv *priv = container_of(fbdev, struct profi_priv, fbdev);
+
+	mutex_lock(&priv->enable_lock);
+	if (enable)
+		ret = __profi_enable(priv);
+	else
+		ret = __profi_disable(priv);
+	mutex_unlock(&priv->enable_lock);
+
+	return ret;
+}
+
+static void profi_on_area_updated(struct anybuss_client *client)
+{
+	struct profi_priv *priv = anybuss_get_drvdata(client);
+
+	fieldbus_dev_area_updated(&priv->fbdev);
+}
+
+static void profi_on_online_changed(struct anybuss_client *client, bool online)
+{
+	struct profi_priv *priv = anybuss_get_drvdata(client);
+
+	fieldbus_dev_online_changed(&priv->fbdev, online);
+}
+
+static int profinet_probe(struct anybuss_client *client)
+{
+	struct profi_priv *priv;
+	struct device *dev = &client->dev;
+	int err;
+
+	client->on_area_updated = profi_on_area_updated;
+	client->on_online_changed = profi_on_online_changed;
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	mutex_init(&priv->enable_lock);
+	priv->client = client;
+	priv->fbdev.read_area_sz = PROFI_DPRAM_SIZE;
+	priv->fbdev.write_area_sz = PROFI_DPRAM_SIZE;
+	priv->fbdev.card_name = "HMS Profinet IRT (Anybus-S)";
+	priv->fbdev.fieldbus_type = FIELDBUS_DEV_TYPE_PROFINET;
+	priv->fbdev.read_area = profi_read_area;
+	priv->fbdev.write_area = profi_write_area;
+	priv->fbdev.fieldbus_id_get = profi_id_get;
+	priv->fbdev.enable_get = profi_enable_get;
+	priv->fbdev.simple_enable_set = profi_simple_enable;
+	priv->fbdev.parent = dev;
+	err = fieldbus_dev_register(&priv->fbdev);
+	if (err < 0)
+		return err;
+	dev_info(dev, "card detected, registered as %s",
+		 dev_name(priv->fbdev.dev));
+	anybuss_set_drvdata(client, priv);
+
+	return 0;
+}
+
+static void profinet_remove(struct anybuss_client *client)
+{
+	struct profi_priv *priv = anybuss_get_drvdata(client);
+
+	fieldbus_dev_unregister(&priv->fbdev);
+}
+
+static struct anybuss_client_driver profinet_driver = {
+	.probe = profinet_probe,
+	.remove = profinet_remove,
+	.driver		= {
+		.name   = "hms-profinet",
+		.owner	= THIS_MODULE,
+	},
+	.anybus_id = 0x0089,
+};
+
+static int __init profinet_init(void)
+{
+	return anybuss_client_driver_register(&profinet_driver);
+}
+module_init(profinet_init);
+
+static void __exit profinet_exit(void)
+{
+	return anybuss_client_driver_unregister(&profinet_driver);
+}
+module_exit(profinet_exit);
+
+MODULE_AUTHOR("Sven Van Asbroeck <TheSven73@gmail.com>");
+MODULE_DESCRIPTION("HMS Profinet IRT Driver (Anybus-S)");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/fieldbus/anybuss/host.c b/drivers/staging/fieldbus/anybuss/host.c
new file mode 100644
index 000000000..cd86b9c9e
--- /dev/null
+++ b/drivers/staging/fieldbus/anybuss/host.c
@@ -0,0 +1,1452 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * HMS Anybus-S Host Driver
+ *
+ * Copyright (C) 2018 Arcx Inc
+ */
+
+/*
+ * Architecture Overview
+ * =====================
+ * This driver (running on the CPU/SoC) and the Anybus-S card communicate
+ * by reading and writing data to/from the Anybus-S Dual-Port RAM (dpram).
+ * This is memory connected to both the SoC and Anybus-S card, which both sides
+ * can access freely and concurrently.
+ *
+ * Synchronization happens by means of two registers located in the dpram:
+ * IND_AB: written exclusively by the Anybus card; and
+ * IND_AP: written exclusively by this driver.
+ *
+ * Communication happens using one of the following mechanisms:
+ * 1. reserve, read/write, release dpram memory areas:
+ *	using an IND_AB/IND_AP protocol, the driver is able to reserve certain
+ *	memory areas. no dpram memory can be read or written except if reserved.
+ *	(with a few limited exceptions)
+ * 2. send and receive data structures via a shared mailbox:
+ *	using an IND_AB/IND_AP protocol, the driver and Anybus card are able to
+ *	exchange commands and responses using a shared mailbox.
+ * 3. receive software interrupts:
+ *	using an IND_AB/IND_AP protocol, the Anybus card is able to notify the
+ *	driver of certain events such as: bus online/offline, data available.
+ *	note that software interrupt event bits are located in a memory area
+ *	which must be reserved before it can be accessed.
+ *
+ * The manual[1] is silent on whether these mechanisms can happen concurrently,
+ * or how they should be synchronized. However, section 13 (Driver Example)
+ * provides the following suggestion for developing a driver:
+ * a) an interrupt handler which updates global variables;
+ * b) a continuously-running task handling area requests (1 above)
+ * c) a continuously-running task handling mailbox requests (2 above)
+ * The example conspicuously leaves out software interrupts (3 above), which
+ * is the thorniest issue to get right (see below).
+ *
+ * The naive, straightforward way to implement this would be:
+ * - create an isr which updates shared variables;
+ * - create a work_struct which handles software interrupts on a queue;
+ * - create a function which does reserve/update/unlock in a loop;
+ * - create a function which does mailbox send/receive in a loop;
+ * - call the above functions from the driver's read/write/ioctl;
+ * - synchronize using mutexes/spinlocks:
+ *	+ only one area request at a time
+ *	+ only one mailbox request at a time
+ *	+ protect AB_IND, AB_IND against data hazards (e.g. read-after-write)
+ *
+ * Unfortunately, the presence of the software interrupt causes subtle yet
+ * considerable synchronization issues; especially problematic is the
+ * requirement to reserve/release the area which contains the status bits.
+ *
+ * The driver architecture presented here sidesteps these synchronization issues
+ * by accessing the dpram from a single kernel thread only. User-space throws
+ * "tasks" (i.e. 1, 2 above) into a task queue, waits for their completion,
+ * and the kernel thread runs them to completion.
+ *
+ * Each task has a task_function, which is called/run by the queue thread.
+ * That function communicates with the Anybus card, and returns either
+ * 0 (OK), a negative error code (error), or -EINPROGRESS (waiting).
+ * On OK or error, the queue thread completes and dequeues the task,
+ * which also releases the user space thread which may still be waiting for it.
+ * On -EINPROGRESS (waiting), the queue thread will leave the task on the queue,
+ * and revisit (call again) whenever an interrupt event comes in.
+ *
+ * Each task has a state machine, which is run by calling its task_function.
+ * It ensures that the task will go through its various stages over time,
+ * returning -EINPROGRESS if it wants to wait for an event to happen.
+ *
+ * Note that according to the manual's driver example, the following operations
+ * may run independent of each other:
+ * - area reserve/read/write/release	(point 1 above)
+ * - mailbox operations			(point 2 above)
+ * - switching power on/off
+ *
+ * To allow them to run independently, each operation class gets its own queue.
+ *
+ * Userspace processes A, B, C, D post tasks to the appropriate queue,
+ * and wait for task completion:
+ *
+ *	process A	B	C	D
+ *		|	|	|	|
+ *		v	v	v	v
+ *	|<-----	========================================
+ *	|		|	   |		|
+ *	|		v	   v		v-------<-------+
+ *	|	+--------------------------------------+	|
+ *	|	| power q     | mbox q    | area q     |	|
+ *	|	|------------|------------|------------|	|
+ *	|	| task       | task       | task       |	|
+ *	|	| task       | task       | task       |	|
+ *	|	| task wait  | task wait  | task wait  |	|
+ *	|	+--------------------------------------+	|
+ *	|		^	   ^		^		|
+ *	|		|	   |		|		^
+ *	|	+--------------------------------------+	|
+ *	|	|	     queue thread	       |	|
+ *	|	|--------------------------------------|	|
+ *	|	| single-threaded:		       |	|
+ *	|	| loop:				       |	|
+ *	v	|   for each queue:		       |	|
+ *	|	|     run task state machine	       |	|
+ *	|	|     if task waiting:		       |	|
+ *	|	|       leave on queue		       |	|
+ *	|	|     if task done:		       |	|
+ *	|	|       complete task, remove from q   |	|
+ *	|	|   if software irq event bits set:    |	|
+ *	|	|     notify userspace		       |	|
+ *	|	|     post clear event bits task------>|>-------+
+ *	|	|   wait for IND_AB changed event OR   |
+ *	|	|            task added event	  OR   |
+ *	|	|	     timeout		       |
+ *	|	| end loop			       |
+ *	|	+--------------------------------------+
+ *	|	+		wake up		       +
+ *	|	+--------------------------------------+
+ *	|		^			^
+ *	|		|			|
+ *	+-------->-------			|
+ *						|
+ *		+--------------------------------------+
+ *		|	interrupt service routine      |
+ *		|--------------------------------------|
+ *		| wake up queue thread on IND_AB change|
+ *		+--------------------------------------+
+ *
+ * Note that the Anybus interrupt is dual-purpose:
+ * - after a reset, triggered when the card becomes ready;
+ * - during normal operation, triggered when AB_IND changes.
+ * This is why the interrupt service routine doesn't just wake up the
+ * queue thread, but also completes the card_boot completion.
+ *
+ * [1] https://www.anybus.com/docs/librariesprovider7/default-document-library/
+ *	manuals-design-guides/hms-hmsi-27-275.pdf
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/atomic.h>
+#include <linux/kthread.h>
+#include <linux/kfifo.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/random.h>
+#include <linux/kref.h>
+#include <linux/of_address.h>
+
+/* move to <linux/anybuss-*.h> when taking this out of staging */
+#include "anybuss-client.h"
+#include "anybuss-controller.h"
+
+#define DPRAM_SIZE		0x800
+#define MAX_MBOX_MSG_SZ		0x0FF
+#define TIMEOUT			(HZ * 2)
+#define MAX_DATA_AREA_SZ	0x200
+#define MAX_FBCTRL_AREA_SZ	0x1BE
+
+#define REG_BOOTLOADER_V	0x7C0
+#define REG_API_V		0x7C2
+#define REG_FIELDBUS_V		0x7C4
+#define REG_SERIAL_NO		0x7C6
+#define REG_FIELDBUS_TYPE	0x7CC
+#define REG_MODULE_SW_V		0x7CE
+#define REG_IND_AB		0x7FF
+#define REG_IND_AP		0x7FE
+#define REG_EVENT_CAUSE		0x7ED
+#define MBOX_IN_AREA		0x400
+#define MBOX_OUT_AREA		0x520
+#define DATA_IN_AREA		0x000
+#define DATA_OUT_AREA		0x200
+#define FBCTRL_AREA		0x640
+
+#define EVENT_CAUSE_DC          0x01
+#define EVENT_CAUSE_FBOF        0x02
+#define EVENT_CAUSE_FBON        0x04
+
+#define IND_AB_UPDATED		0x08
+#define IND_AX_MIN		0x80
+#define IND_AX_MOUT		0x40
+#define IND_AX_IN		0x04
+#define IND_AX_OUT		0x02
+#define IND_AX_FBCTRL		0x01
+#define IND_AP_LOCK		0x08
+#define IND_AP_ACTION		0x10
+#define IND_AX_EVNT		0x20
+#define IND_AP_ABITS		(IND_AX_IN | IND_AX_OUT | \
+					IND_AX_FBCTRL | \
+					IND_AP_ACTION | IND_AP_LOCK)
+
+#define INFO_TYPE_FB		0x0002
+#define INFO_TYPE_APP		0x0001
+#define INFO_COMMAND		0x4000
+
+#define OP_MODE_FBFC		0x0002
+#define OP_MODE_FBS		0x0004
+#define OP_MODE_CD		0x0200
+
+#define CMD_START_INIT		0x0001
+#define CMD_ANYBUS_INIT		0x0002
+#define CMD_END_INIT		0x0003
+
+/*
+ * ---------------------------------------------------------------
+ * Anybus mailbox messages - definitions
+ * ---------------------------------------------------------------
+ * note that we're depending on the layout of these structures being
+ * exactly as advertised.
+ */
+
+struct anybus_mbox_hdr {
+	__be16 id;
+	__be16 info;
+	__be16 cmd_num;
+	__be16 data_size;
+	__be16 frame_count;
+	__be16 frame_num;
+	__be16 offset_high;
+	__be16 offset_low;
+	__be16 extended[8];
+};
+
+struct msg_anybus_init {
+	__be16 input_io_len;
+	__be16 input_dpram_len;
+	__be16 input_total_len;
+	__be16 output_io_len;
+	__be16 output_dpram_len;
+	__be16 output_total_len;
+	__be16 op_mode;
+	__be16 notif_config;
+	__be16 wd_val;
+};
+
+/* ------------- ref counted tasks ------------- */
+
+struct ab_task;
+typedef int (*ab_task_fn_t)(struct anybuss_host *cd,
+					struct ab_task *t);
+typedef void (*ab_done_fn_t)(struct anybuss_host *cd);
+
+struct area_priv {
+	bool is_write;
+	u16 flags;
+	u16 addr;
+	size_t count;
+	u8 buf[MAX_DATA_AREA_SZ];
+};
+
+struct mbox_priv {
+	struct anybus_mbox_hdr hdr;
+	size_t msg_out_sz;
+	size_t msg_in_sz;
+	u8 msg[MAX_MBOX_MSG_SZ];
+};
+
+struct ab_task {
+	struct kmem_cache	*cache;
+	struct kref		refcount;
+	ab_task_fn_t		task_fn;
+	ab_done_fn_t		done_fn;
+	int			result;
+	struct completion	done;
+	unsigned long		start_jiffies;
+	union {
+		struct area_priv area_pd;
+		struct mbox_priv mbox_pd;
+	};
+};
+
+static struct ab_task *ab_task_create_get(struct kmem_cache *cache,
+					  ab_task_fn_t task_fn)
+{
+	struct ab_task *t;
+
+	t = kmem_cache_alloc(cache, GFP_KERNEL);
+	if (!t)
+		return NULL;
+	t->cache = cache;
+	kref_init(&t->refcount);
+	t->task_fn = task_fn;
+	t->done_fn = NULL;
+	t->result = 0;
+	init_completion(&t->done);
+	return t;
+}
+
+static void __ab_task_destroy(struct kref *refcount)
+{
+	struct ab_task *t = container_of(refcount, struct ab_task, refcount);
+	struct kmem_cache *cache = t->cache;
+
+	kmem_cache_free(cache, t);
+}
+
+static void ab_task_put(struct ab_task *t)
+{
+	kref_put(&t->refcount, __ab_task_destroy);
+}
+
+static struct ab_task *__ab_task_get(struct ab_task *t)
+{
+	kref_get(&t->refcount);
+	return t;
+}
+
+static void __ab_task_finish(struct ab_task *t, struct anybuss_host *cd)
+{
+	if (t->done_fn)
+		t->done_fn(cd);
+	complete(&t->done);
+}
+
+static void
+ab_task_dequeue_finish_put(struct kfifo *q, struct anybuss_host *cd)
+{
+	int ret;
+	struct ab_task *t;
+
+	ret = kfifo_out(q, &t, sizeof(t));
+	WARN_ON(!ret);
+	__ab_task_finish(t, cd);
+	ab_task_put(t);
+}
+
+static int
+ab_task_enqueue(struct ab_task *t, struct kfifo *q, spinlock_t *slock,
+		wait_queue_head_t *wq)
+{
+	int ret;
+
+	t->start_jiffies = jiffies;
+	__ab_task_get(t);
+	ret = kfifo_in_spinlocked(q, &t, sizeof(t), slock);
+	if (!ret) {
+		ab_task_put(t);
+		return -ENOMEM;
+	}
+	wake_up(wq);
+	return 0;
+}
+
+static int
+ab_task_enqueue_wait(struct ab_task *t, struct kfifo *q, spinlock_t *slock,
+		     wait_queue_head_t *wq)
+{
+	int ret;
+
+	ret = ab_task_enqueue(t, q, slock, wq);
+	if (ret)
+		return ret;
+	ret = wait_for_completion_interruptible(&t->done);
+	if (ret)
+		return ret;
+	return t->result;
+}
+
+/* ------------------------ anybus hardware ------------------------ */
+
+struct anybuss_host {
+	struct device *dev;
+	struct anybuss_client *client;
+	void (*reset)(struct device *dev, bool assert);
+	struct regmap *regmap;
+	int irq;
+	int host_idx;
+	struct task_struct *qthread;
+	wait_queue_head_t wq;
+	struct completion card_boot;
+	atomic_t ind_ab;
+	spinlock_t qlock; /* protects IN side of powerq, mboxq, areaq */
+	struct kmem_cache *qcache;
+	struct kfifo qs[3];
+	struct kfifo *powerq;
+	struct kfifo *mboxq;
+	struct kfifo *areaq;
+	bool power_on;
+	bool softint_pending;
+};
+
+static void reset_assert(struct anybuss_host *cd)
+{
+	cd->reset(cd->dev, true);
+}
+
+static void reset_deassert(struct anybuss_host *cd)
+{
+	cd->reset(cd->dev, false);
+}
+
+static int test_dpram(struct regmap *regmap)
+{
+	int i;
+	unsigned int val;
+
+	for (i = 0; i < DPRAM_SIZE; i++)
+		regmap_write(regmap, i, (u8)i);
+	for (i = 0; i < DPRAM_SIZE; i++) {
+		regmap_read(regmap, i, &val);
+		if ((u8)val != (u8)i)
+			return -EIO;
+	}
+	return 0;
+}
+
+static int read_ind_ab(struct regmap *regmap)
+{
+	unsigned long timeout = jiffies + HZ / 2;
+	unsigned int a, b, i = 0;
+
+	while (time_before_eq(jiffies, timeout)) {
+		regmap_read(regmap, REG_IND_AB, &a);
+		regmap_read(regmap, REG_IND_AB, &b);
+		if (likely(a == b))
+			return (int)a;
+		if (i < 10) {
+			cpu_relax();
+			i++;
+		} else {
+			usleep_range(500, 1000);
+		}
+	}
+	WARN(1, "IND_AB register not stable");
+	return -ETIMEDOUT;
+}
+
+static int write_ind_ap(struct regmap *regmap, unsigned int ind_ap)
+{
+	unsigned long timeout = jiffies + HZ / 2;
+	unsigned int v, i = 0;
+
+	while (time_before_eq(jiffies, timeout)) {
+		regmap_write(regmap, REG_IND_AP, ind_ap);
+		regmap_read(regmap, REG_IND_AP, &v);
+		if (likely(ind_ap == v))
+			return 0;
+		if (i < 10) {
+			cpu_relax();
+			i++;
+		} else {
+			usleep_range(500, 1000);
+		}
+	}
+	WARN(1, "IND_AP register not stable");
+	return -ETIMEDOUT;
+}
+
+static irqreturn_t irq_handler(int irq, void *data)
+{
+	struct anybuss_host *cd = data;
+	int ind_ab;
+
+	/*
+	 * irq handler needs exclusive access to the IND_AB register,
+	 * because the act of reading the register acks the interrupt.
+	 *
+	 * store the register value in cd->ind_ab (an atomic_t), so that the
+	 * queue thread is able to read it without causing an interrupt ack
+	 * side-effect (and without spuriously acking an interrupt).
+	 */
+	ind_ab = read_ind_ab(cd->regmap);
+	if (ind_ab < 0)
+		return IRQ_NONE;
+	atomic_set(&cd->ind_ab, ind_ab);
+	complete(&cd->card_boot);
+	wake_up(&cd->wq);
+	return IRQ_HANDLED;
+}
+
+/* ------------------------ power on/off tasks --------------------- */
+
+static int task_fn_power_off(struct anybuss_host *cd,
+			     struct ab_task *t)
+{
+	struct anybuss_client *client = cd->client;
+
+	if (!cd->power_on)
+		return 0;
+	disable_irq(cd->irq);
+	reset_assert(cd);
+	atomic_set(&cd->ind_ab, IND_AB_UPDATED);
+	if (client->on_online_changed)
+		client->on_online_changed(client, false);
+	cd->power_on = false;
+	return 0;
+}
+
+static int task_fn_power_on_2(struct anybuss_host *cd,
+			      struct ab_task *t)
+{
+	if (completion_done(&cd->card_boot)) {
+		cd->power_on = true;
+		return 0;
+	}
+	if (time_after(jiffies, t->start_jiffies + TIMEOUT)) {
+		disable_irq(cd->irq);
+		reset_assert(cd);
+		dev_err(cd->dev, "power on timed out");
+		return -ETIMEDOUT;
+	}
+	return -EINPROGRESS;
+}
+
+static int task_fn_power_on(struct anybuss_host *cd,
+			    struct ab_task *t)
+{
+	unsigned int dummy;
+
+	if (cd->power_on)
+		return 0;
+	/*
+	 * anybus docs: prevent false 'init done' interrupt by
+	 * doing a dummy read of IND_AB register while in reset.
+	 */
+	regmap_read(cd->regmap, REG_IND_AB, &dummy);
+	reinit_completion(&cd->card_boot);
+	enable_irq(cd->irq);
+	reset_deassert(cd);
+	t->task_fn = task_fn_power_on_2;
+	return -EINPROGRESS;
+}
+
+int anybuss_set_power(struct anybuss_client *client, bool power_on)
+{
+	struct anybuss_host *cd = client->host;
+	struct ab_task *t;
+	int err;
+
+	t = ab_task_create_get(cd->qcache, power_on ?
+				task_fn_power_on : task_fn_power_off);
+	if (!t)
+		return -ENOMEM;
+	err = ab_task_enqueue_wait(t, cd->powerq, &cd->qlock, &cd->wq);
+	ab_task_put(t);
+	return err;
+}
+EXPORT_SYMBOL_GPL(anybuss_set_power);
+
+/* ---------------------------- area tasks ------------------------ */
+
+static int task_fn_area_3(struct anybuss_host *cd, struct ab_task *t)
+{
+	struct area_priv *pd = &t->area_pd;
+
+	if (!cd->power_on)
+		return -EIO;
+	if (atomic_read(&cd->ind_ab) & pd->flags) {
+		/* area not released yet */
+		if (time_after(jiffies, t->start_jiffies + TIMEOUT))
+			return -ETIMEDOUT;
+		return -EINPROGRESS;
+	}
+	return 0;
+}
+
+static int task_fn_area_2(struct anybuss_host *cd, struct ab_task *t)
+{
+	struct area_priv *pd = &t->area_pd;
+	unsigned int ind_ap;
+	int ret;
+
+	if (!cd->power_on)
+		return -EIO;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	if (!(atomic_read(&cd->ind_ab) & pd->flags)) {
+		/* we don't own the area yet */
+		if (time_after(jiffies, t->start_jiffies + TIMEOUT)) {
+			dev_warn(cd->dev, "timeout waiting for area");
+			dump_stack();
+			return -ETIMEDOUT;
+		}
+		return -EINPROGRESS;
+	}
+	/* we own the area, do what we're here to do */
+	if (pd->is_write)
+		regmap_bulk_write(cd->regmap, pd->addr, pd->buf,
+				  pd->count);
+	else
+		regmap_bulk_read(cd->regmap, pd->addr, pd->buf,
+				 pd->count);
+	/* ask to release the area, must use unlocked release */
+	ind_ap &= ~IND_AP_ABITS;
+	ind_ap |= pd->flags;
+	ret = write_ind_ap(cd->regmap, ind_ap);
+	if (ret)
+		return ret;
+	t->task_fn = task_fn_area_3;
+	return -EINPROGRESS;
+}
+
+static int task_fn_area(struct anybuss_host *cd, struct ab_task *t)
+{
+	struct area_priv *pd = &t->area_pd;
+	unsigned int ind_ap;
+	int ret;
+
+	if (!cd->power_on)
+		return -EIO;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	/* ask to take the area */
+	ind_ap &= ~IND_AP_ABITS;
+	ind_ap |= pd->flags | IND_AP_ACTION | IND_AP_LOCK;
+	ret = write_ind_ap(cd->regmap, ind_ap);
+	if (ret)
+		return ret;
+	t->task_fn = task_fn_area_2;
+	return -EINPROGRESS;
+}
+
+static struct ab_task *
+create_area_reader(struct kmem_cache *qcache, u16 flags, u16 addr,
+		   size_t count)
+{
+	struct ab_task *t;
+	struct area_priv *ap;
+
+	t = ab_task_create_get(qcache, task_fn_area);
+	if (!t)
+		return NULL;
+	ap = &t->area_pd;
+	ap->flags = flags;
+	ap->addr = addr;
+	ap->is_write = false;
+	ap->count = count;
+	return t;
+}
+
+static struct ab_task *
+create_area_writer(struct kmem_cache *qcache, u16 flags, u16 addr,
+		   const void *buf, size_t count)
+{
+	struct ab_task *t;
+	struct area_priv *ap;
+
+	t = ab_task_create_get(qcache, task_fn_area);
+	if (!t)
+		return NULL;
+	ap = &t->area_pd;
+	ap->flags = flags;
+	ap->addr = addr;
+	ap->is_write = true;
+	ap->count = count;
+	memcpy(ap->buf, buf, count);
+	return t;
+}
+
+static struct ab_task *
+create_area_user_writer(struct kmem_cache *qcache, u16 flags, u16 addr,
+			const void __user *buf, size_t count)
+{
+	struct ab_task *t;
+	struct area_priv *ap;
+
+	t = ab_task_create_get(qcache, task_fn_area);
+	if (!t)
+		return ERR_PTR(-ENOMEM);
+	ap = &t->area_pd;
+	ap->flags = flags;
+	ap->addr = addr;
+	ap->is_write = true;
+	ap->count = count;
+	if (copy_from_user(ap->buf, buf, count)) {
+		ab_task_put(t);
+		return ERR_PTR(-EFAULT);
+	}
+	return t;
+}
+
+static bool area_range_ok(u16 addr, size_t count, u16 area_start,
+			  size_t area_sz)
+{
+	u16 area_end_ex = area_start + area_sz;
+	u16 addr_end_ex;
+
+	if (addr < area_start)
+		return false;
+	if (addr >= area_end_ex)
+		return false;
+	addr_end_ex = addr + count;
+	if (addr_end_ex > area_end_ex)
+		return false;
+	return true;
+}
+
+/* -------------------------- mailbox tasks ----------------------- */
+
+static int task_fn_mbox_2(struct anybuss_host *cd, struct ab_task *t)
+{
+	struct mbox_priv *pd = &t->mbox_pd;
+	unsigned int ind_ap;
+
+	if (!cd->power_on)
+		return -EIO;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	if (((atomic_read(&cd->ind_ab) ^ ind_ap) & IND_AX_MOUT) == 0) {
+		/* output message not here */
+		if (time_after(jiffies, t->start_jiffies + TIMEOUT))
+			return -ETIMEDOUT;
+		return -EINPROGRESS;
+	}
+	/* grab the returned header and msg */
+	regmap_bulk_read(cd->regmap, MBOX_OUT_AREA, &pd->hdr,
+			 sizeof(pd->hdr));
+	regmap_bulk_read(cd->regmap, MBOX_OUT_AREA + sizeof(pd->hdr),
+			 pd->msg, pd->msg_in_sz);
+	/* tell anybus we've consumed the message */
+	ind_ap ^= IND_AX_MOUT;
+	return write_ind_ap(cd->regmap, ind_ap);
+}
+
+static int task_fn_mbox(struct anybuss_host *cd, struct ab_task *t)
+{
+	struct mbox_priv *pd = &t->mbox_pd;
+	unsigned int ind_ap;
+	int ret;
+
+	if (!cd->power_on)
+		return -EIO;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	if ((atomic_read(&cd->ind_ab) ^ ind_ap) & IND_AX_MIN) {
+		/* mbox input area busy */
+		if (time_after(jiffies, t->start_jiffies + TIMEOUT))
+			return -ETIMEDOUT;
+		return -EINPROGRESS;
+	}
+	/* write the header and msg to input area */
+	regmap_bulk_write(cd->regmap, MBOX_IN_AREA, &pd->hdr,
+			  sizeof(pd->hdr));
+	regmap_bulk_write(cd->regmap, MBOX_IN_AREA + sizeof(pd->hdr),
+			  pd->msg, pd->msg_out_sz);
+	/* tell anybus we gave it a message */
+	ind_ap ^= IND_AX_MIN;
+	ret = write_ind_ap(cd->regmap, ind_ap);
+	if (ret)
+		return ret;
+	t->start_jiffies = jiffies;
+	t->task_fn = task_fn_mbox_2;
+	return -EINPROGRESS;
+}
+
+static void log_invalid_other(struct device *dev,
+			      struct anybus_mbox_hdr *hdr)
+{
+	size_t ext_offs = ARRAY_SIZE(hdr->extended) - 1;
+	u16 code = be16_to_cpu(hdr->extended[ext_offs]);
+
+	dev_err(dev, "   Invalid other: [0x%02X]", code);
+}
+
+static const char * const EMSGS[] = {
+	"Invalid Message ID",
+	"Invalid Message Type",
+	"Invalid Command",
+	"Invalid Data Size",
+	"Message Header Malformed (offset 008h)",
+	"Message Header Malformed (offset 00Ah)",
+	"Message Header Malformed (offset 00Ch - 00Dh)",
+	"Invalid Address",
+	"Invalid Response",
+	"Flash Config Error",
+};
+
+static int mbox_cmd_err(struct device *dev, struct mbox_priv *mpriv)
+{
+	int i;
+	u8 ecode;
+	struct anybus_mbox_hdr *hdr = &mpriv->hdr;
+	u16 info = be16_to_cpu(hdr->info);
+	u8 *phdr = (u8 *)hdr;
+	u8 *pmsg = mpriv->msg;
+
+	if (!(info & 0x8000))
+		return 0;
+	ecode = (info >> 8) & 0x0F;
+	dev_err(dev, "mailbox command failed:");
+	if (ecode == 0x0F)
+		log_invalid_other(dev, hdr);
+	else if (ecode < ARRAY_SIZE(EMSGS))
+		dev_err(dev, "   Error code: %s (0x%02X)",
+			EMSGS[ecode], ecode);
+	else
+		dev_err(dev, "   Error code: 0x%02X\n", ecode);
+	dev_err(dev, "Failed command:");
+	dev_err(dev, "Message Header:");
+	for (i = 0; i < sizeof(mpriv->hdr); i += 2)
+		dev_err(dev, "%02X%02X", phdr[i], phdr[i + 1]);
+	dev_err(dev, "Message Data:");
+	for (i = 0; i < mpriv->msg_in_sz; i += 2)
+		dev_err(dev, "%02X%02X", pmsg[i], pmsg[i + 1]);
+	dev_err(dev, "Stack dump:");
+	dump_stack();
+	return -EIO;
+}
+
+static int _anybus_mbox_cmd(struct anybuss_host *cd,
+			    u16 cmd_num, bool is_fb_cmd,
+				const void *msg_out, size_t msg_out_sz,
+				void *msg_in, size_t msg_in_sz,
+				const void *ext, size_t ext_sz)
+{
+	struct ab_task *t;
+	struct mbox_priv *pd;
+	struct anybus_mbox_hdr *h;
+	size_t msg_sz = max(msg_in_sz, msg_out_sz);
+	u16 info;
+	int err;
+
+	if (msg_sz > MAX_MBOX_MSG_SZ)
+		return -EINVAL;
+	if (ext && ext_sz > sizeof(h->extended))
+		return -EINVAL;
+	t = ab_task_create_get(cd->qcache, task_fn_mbox);
+	if (!t)
+		return -ENOMEM;
+	pd = &t->mbox_pd;
+	h = &pd->hdr;
+	info = is_fb_cmd ? INFO_TYPE_FB : INFO_TYPE_APP;
+	/*
+	 * prevent uninitialized memory in the header from being sent
+	 * across the anybus
+	 */
+	memset(h, 0, sizeof(*h));
+	h->info = cpu_to_be16(info | INFO_COMMAND);
+	h->cmd_num = cpu_to_be16(cmd_num);
+	h->data_size = cpu_to_be16(msg_out_sz);
+	h->frame_count = cpu_to_be16(1);
+	h->frame_num = cpu_to_be16(1);
+	h->offset_high = cpu_to_be16(0);
+	h->offset_low = cpu_to_be16(0);
+	if (ext)
+		memcpy(h->extended, ext, ext_sz);
+	memcpy(pd->msg, msg_out, msg_out_sz);
+	pd->msg_out_sz = msg_out_sz;
+	pd->msg_in_sz = msg_in_sz;
+	err = ab_task_enqueue_wait(t, cd->powerq, &cd->qlock, &cd->wq);
+	if (err)
+		goto out;
+	/*
+	 * mailbox mechanism worked ok, but maybe the mbox response
+	 * contains an error ?
+	 */
+	err = mbox_cmd_err(cd->dev, pd);
+	if (err)
+		goto out;
+	memcpy(msg_in, pd->msg, msg_in_sz);
+out:
+	ab_task_put(t);
+	return err;
+}
+
+/* ------------------------ anybus queues ------------------------ */
+
+static void process_q(struct anybuss_host *cd, struct kfifo *q)
+{
+	struct ab_task *t;
+	int ret;
+
+	ret = kfifo_out_peek(q, &t, sizeof(t));
+	if (!ret)
+		return;
+	t->result = t->task_fn(cd, t);
+	if (t->result != -EINPROGRESS)
+		ab_task_dequeue_finish_put(q, cd);
+}
+
+static bool qs_have_work(struct kfifo *qs, size_t num)
+{
+	size_t i;
+	struct ab_task *t;
+	int ret;
+
+	for (i = 0; i < num; i++, qs++) {
+		ret = kfifo_out_peek(qs, &t, sizeof(t));
+		if (ret && (t->result != -EINPROGRESS))
+			return true;
+	}
+	return false;
+}
+
+static void process_qs(struct anybuss_host *cd)
+{
+	size_t i;
+	struct kfifo *qs = cd->qs;
+	size_t nqs = ARRAY_SIZE(cd->qs);
+
+	for (i = 0; i < nqs; i++, qs++)
+		process_q(cd, qs);
+}
+
+static void softint_ack(struct anybuss_host *cd)
+{
+	unsigned int ind_ap;
+
+	cd->softint_pending = false;
+	if (!cd->power_on)
+		return;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	ind_ap &= ~IND_AX_EVNT;
+	ind_ap |= atomic_read(&cd->ind_ab) & IND_AX_EVNT;
+	write_ind_ap(cd->regmap, ind_ap);
+}
+
+static void process_softint(struct anybuss_host *cd)
+{
+	struct anybuss_client *client = cd->client;
+	static const u8 zero;
+	int ret;
+	unsigned int ind_ap, ev;
+	struct ab_task *t;
+
+	if (!cd->power_on)
+		return;
+	if (cd->softint_pending)
+		return;
+	regmap_read(cd->regmap, REG_IND_AP, &ind_ap);
+	if (!((atomic_read(&cd->ind_ab) ^ ind_ap) & IND_AX_EVNT))
+		return;
+	/* process software interrupt */
+	regmap_read(cd->regmap, REG_EVENT_CAUSE, &ev);
+	if (ev & EVENT_CAUSE_FBON) {
+		if (client->on_online_changed)
+			client->on_online_changed(client, true);
+		dev_dbg(cd->dev, "Fieldbus ON");
+	}
+	if (ev & EVENT_CAUSE_FBOF) {
+		if (client->on_online_changed)
+			client->on_online_changed(client, false);
+		dev_dbg(cd->dev, "Fieldbus OFF");
+	}
+	if (ev & EVENT_CAUSE_DC) {
+		if (client->on_area_updated)
+			client->on_area_updated(client);
+		dev_dbg(cd->dev, "Fieldbus data changed");
+	}
+	/*
+	 * reset the event cause bits.
+	 * this must be done while owning the fbctrl area, so we'll
+	 * enqueue a task to do that.
+	 */
+	t = create_area_writer(cd->qcache, IND_AX_FBCTRL,
+			       REG_EVENT_CAUSE, &zero, sizeof(zero));
+	if (!t) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	t->done_fn = softint_ack;
+	ret = ab_task_enqueue(t, cd->powerq, &cd->qlock, &cd->wq);
+	ab_task_put(t);
+	cd->softint_pending = true;
+out:
+	WARN_ON(ret);
+	if (ret)
+		softint_ack(cd);
+}
+
+static int qthread_fn(void *data)
+{
+	struct anybuss_host *cd = data;
+	struct kfifo *qs = cd->qs;
+	size_t nqs = ARRAY_SIZE(cd->qs);
+	unsigned int ind_ab;
+
+	/*
+	 * this kernel thread has exclusive access to the anybus's memory.
+	 * only exception: the IND_AB register, which is accessed exclusively
+	 * by the interrupt service routine (ISR). This thread must not touch
+	 * the IND_AB register, but it does require access to its value.
+	 *
+	 * the interrupt service routine stores the register's value in
+	 * cd->ind_ab (an atomic_t), where we may safely access it, with the
+	 * understanding that it can be modified by the ISR at any time.
+	 */
+
+	while (!kthread_should_stop()) {
+		/*
+		 * make a local copy of IND_AB, so we can go around the loop
+		 * again in case it changed while processing queues and softint.
+		 */
+		ind_ab = atomic_read(&cd->ind_ab);
+		process_qs(cd);
+		process_softint(cd);
+		wait_event_timeout(cd->wq,
+				   (atomic_read(&cd->ind_ab) != ind_ab) ||
+				qs_have_work(qs, nqs) ||
+				kthread_should_stop(),
+			HZ);
+		/*
+		 * time out so even 'stuck' tasks will run eventually,
+		 * and can time out.
+		 */
+	}
+
+	return 0;
+}
+
+/* ------------------------ anybus exports ------------------------ */
+
+int anybuss_start_init(struct anybuss_client *client,
+		       const struct anybuss_memcfg *cfg)
+{
+	int ret;
+	u16 op_mode;
+	struct anybuss_host *cd = client->host;
+	struct msg_anybus_init msg = {
+		.input_io_len = cpu_to_be16(cfg->input_io),
+		.input_dpram_len = cpu_to_be16(cfg->input_dpram),
+		.input_total_len = cpu_to_be16(cfg->input_total),
+		.output_io_len = cpu_to_be16(cfg->output_io),
+		.output_dpram_len = cpu_to_be16(cfg->output_dpram),
+		.output_total_len = cpu_to_be16(cfg->output_total),
+		.notif_config = cpu_to_be16(0x000F),
+		.wd_val = cpu_to_be16(0),
+	};
+
+	switch (cfg->offl_mode) {
+	case FIELDBUS_DEV_OFFL_MODE_CLEAR:
+		op_mode = 0;
+		break;
+	case FIELDBUS_DEV_OFFL_MODE_FREEZE:
+		op_mode = OP_MODE_FBFC;
+		break;
+	case FIELDBUS_DEV_OFFL_MODE_SET:
+		op_mode = OP_MODE_FBS;
+		break;
+	default:
+		return -EINVAL;
+	}
+	msg.op_mode = cpu_to_be16(op_mode | OP_MODE_CD);
+	ret = _anybus_mbox_cmd(cd, CMD_START_INIT, false, NULL, 0,
+			       NULL, 0, NULL, 0);
+	if (ret)
+		return ret;
+	return _anybus_mbox_cmd(cd, CMD_ANYBUS_INIT, false,
+			&msg, sizeof(msg), NULL, 0, NULL, 0);
+}
+EXPORT_SYMBOL_GPL(anybuss_start_init);
+
+int anybuss_finish_init(struct anybuss_client *client)
+{
+	struct anybuss_host *cd = client->host;
+
+	return _anybus_mbox_cmd(cd, CMD_END_INIT, false, NULL, 0,
+					NULL, 0, NULL, 0);
+}
+EXPORT_SYMBOL_GPL(anybuss_finish_init);
+
+int anybuss_read_fbctrl(struct anybuss_client *client, u16 addr,
+			void *buf, size_t count)
+{
+	struct anybuss_host *cd = client->host;
+	struct ab_task *t;
+	int ret;
+
+	if (count == 0)
+		return 0;
+	if (!area_range_ok(addr, count, FBCTRL_AREA,
+			   MAX_FBCTRL_AREA_SZ))
+		return -EFAULT;
+	t = create_area_reader(cd->qcache, IND_AX_FBCTRL, addr, count);
+	if (!t)
+		return -ENOMEM;
+	ret = ab_task_enqueue_wait(t, cd->powerq, &cd->qlock, &cd->wq);
+	if (ret)
+		goto out;
+	memcpy(buf, t->area_pd.buf, count);
+out:
+	ab_task_put(t);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(anybuss_read_fbctrl);
+
+int anybuss_write_input(struct anybuss_client *client,
+			const char __user *buf, size_t size,
+				loff_t *offset)
+{
+	ssize_t len = min_t(loff_t, MAX_DATA_AREA_SZ - *offset, size);
+	struct anybuss_host *cd = client->host;
+	struct ab_task *t;
+	int ret;
+
+	if (len <= 0)
+		return 0;
+	t = create_area_user_writer(cd->qcache, IND_AX_IN,
+				    DATA_IN_AREA + *offset, buf, len);
+	if (IS_ERR(t))
+		return PTR_ERR(t);
+	ret = ab_task_enqueue_wait(t, cd->powerq, &cd->qlock, &cd->wq);
+	ab_task_put(t);
+	if (ret)
+		return ret;
+	/* success */
+	*offset += len;
+	return len;
+}
+EXPORT_SYMBOL_GPL(anybuss_write_input);
+
+int anybuss_read_output(struct anybuss_client *client,
+			char __user *buf, size_t size,
+				loff_t *offset)
+{
+	ssize_t len = min_t(loff_t, MAX_DATA_AREA_SZ - *offset, size);
+	struct anybuss_host *cd = client->host;
+	struct ab_task *t;
+	int ret;
+
+	if (len <= 0)
+		return 0;
+	t = create_area_reader(cd->qcache, IND_AX_OUT,
+			       DATA_OUT_AREA + *offset, len);
+	if (!t)
+		return -ENOMEM;
+	ret = ab_task_enqueue_wait(t, cd->powerq, &cd->qlock, &cd->wq);
+	if (ret)
+		goto out;
+	if (copy_to_user(buf, t->area_pd.buf, len))
+		ret = -EFAULT;
+out:
+	ab_task_put(t);
+	if (ret)
+		return ret;
+	/* success */
+	*offset += len;
+	return len;
+}
+EXPORT_SYMBOL_GPL(anybuss_read_output);
+
+int anybuss_send_msg(struct anybuss_client *client, u16 cmd_num,
+		     const void *buf, size_t count)
+{
+	struct anybuss_host *cd = client->host;
+
+	return _anybus_mbox_cmd(cd, cmd_num, true, buf, count, NULL, 0,
+					NULL, 0);
+}
+EXPORT_SYMBOL_GPL(anybuss_send_msg);
+
+int anybuss_send_ext(struct anybuss_client *client, u16 cmd_num,
+		     const void *buf, size_t count)
+{
+	struct anybuss_host *cd = client->host;
+
+	return _anybus_mbox_cmd(cd, cmd_num, true, NULL, 0, NULL, 0,
+					buf, count);
+}
+EXPORT_SYMBOL_GPL(anybuss_send_ext);
+
+int anybuss_recv_msg(struct anybuss_client *client, u16 cmd_num,
+		     void *buf, size_t count)
+{
+	struct anybuss_host *cd = client->host;
+
+	return _anybus_mbox_cmd(cd, cmd_num, true, NULL, 0, buf, count,
+					NULL, 0);
+}
+EXPORT_SYMBOL_GPL(anybuss_recv_msg);
+
+/* ------------------------ bus functions ------------------------ */
+
+static int anybus_bus_match(struct device *dev,
+			    struct device_driver *drv)
+{
+	struct anybuss_client_driver *adrv =
+		to_anybuss_client_driver(drv);
+	struct anybuss_client *adev =
+		to_anybuss_client(dev);
+
+	return adrv->anybus_id == be16_to_cpu(adev->anybus_id);
+}
+
+static int anybus_bus_probe(struct device *dev)
+{
+	struct anybuss_client_driver *adrv =
+		to_anybuss_client_driver(dev->driver);
+	struct anybuss_client *adev =
+		to_anybuss_client(dev);
+
+	return adrv->probe(adev);
+}
+
+static void anybus_bus_remove(struct device *dev)
+{
+	struct anybuss_client_driver *adrv =
+		to_anybuss_client_driver(dev->driver);
+
+	if (adrv->remove)
+		adrv->remove(to_anybuss_client(dev));
+}
+
+static struct bus_type anybus_bus = {
+	.name		= "anybuss",
+	.match		= anybus_bus_match,
+	.probe		= anybus_bus_probe,
+	.remove		= anybus_bus_remove,
+};
+
+int anybuss_client_driver_register(struct anybuss_client_driver *drv)
+{
+	if (!drv->probe)
+		return -ENODEV;
+
+	drv->driver.bus = &anybus_bus;
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(anybuss_client_driver_register);
+
+void anybuss_client_driver_unregister(struct anybuss_client_driver *drv)
+{
+	return driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(anybuss_client_driver_unregister);
+
+static void client_device_release(struct device *dev)
+{
+	kfree(to_anybuss_client(dev));
+}
+
+static int taskq_alloc(struct device *dev, struct kfifo *q)
+{
+	void *buf;
+	size_t size = 64 * sizeof(struct ab_task *);
+
+	buf = devm_kzalloc(dev, size, GFP_KERNEL);
+	if (!buf)
+		return -EIO;
+	return kfifo_init(q, buf, size);
+}
+
+static int anybus_of_get_host_idx(struct device_node *np)
+{
+	const __be32 *host_idx;
+
+	host_idx = of_get_address(np, 0, NULL, NULL);
+	if (!host_idx)
+		return -ENOENT;
+	return __be32_to_cpu(*host_idx);
+}
+
+static struct device_node *
+anybus_of_find_child_device(struct device *dev, int host_idx)
+{
+	struct device_node *node;
+
+	if (!dev || !dev->of_node)
+		return NULL;
+	for_each_child_of_node(dev->of_node, node) {
+		if (anybus_of_get_host_idx(node) == host_idx)
+			return node;
+	}
+	return NULL;
+}
+
+struct anybuss_host * __must_check
+anybuss_host_common_probe(struct device *dev,
+			  const struct anybuss_ops *ops)
+{
+	int ret, i;
+	u8 val[4];
+	__be16 fieldbus_type;
+	struct anybuss_host *cd;
+
+	cd = devm_kzalloc(dev, sizeof(*cd), GFP_KERNEL);
+	if (!cd)
+		return ERR_PTR(-ENOMEM);
+	cd->dev = dev;
+	cd->host_idx = ops->host_idx;
+	init_completion(&cd->card_boot);
+	init_waitqueue_head(&cd->wq);
+	for (i = 0; i < ARRAY_SIZE(cd->qs); i++) {
+		ret = taskq_alloc(dev, &cd->qs[i]);
+		if (ret)
+			return ERR_PTR(ret);
+	}
+	if (WARN_ON(ARRAY_SIZE(cd->qs) < 3))
+		return ERR_PTR(-EINVAL);
+	cd->powerq = &cd->qs[0];
+	cd->mboxq = &cd->qs[1];
+	cd->areaq = &cd->qs[2];
+	cd->reset = ops->reset;
+	if (!cd->reset)
+		return ERR_PTR(-EINVAL);
+	cd->regmap = ops->regmap;
+	if (!cd->regmap)
+		return ERR_PTR(-EINVAL);
+	spin_lock_init(&cd->qlock);
+	cd->qcache = kmem_cache_create(dev_name(dev),
+				       sizeof(struct ab_task), 0, 0, NULL);
+	if (!cd->qcache)
+		return ERR_PTR(-ENOMEM);
+	cd->irq = ops->irq;
+	if (cd->irq <= 0) {
+		ret = -EINVAL;
+		goto err_qcache;
+	}
+	/*
+	 * use a dpram test to check if a card is present, this is only
+	 * possible while in reset.
+	 */
+	reset_assert(cd);
+	if (test_dpram(cd->regmap)) {
+		dev_err(dev, "no Anybus-S card in slot");
+		ret = -ENODEV;
+		goto err_qcache;
+	}
+	ret = devm_request_threaded_irq(dev, cd->irq, NULL, irq_handler,
+					IRQF_ONESHOT, dev_name(dev), cd);
+	if (ret) {
+		dev_err(dev, "could not request irq");
+		goto err_qcache;
+	}
+	/*
+	 * startup sequence:
+	 *   a) perform dummy IND_AB read to prevent false 'init done' irq
+	 *     (already done by test_dpram() above)
+	 *   b) release reset
+	 *   c) wait for first interrupt
+	 *   d) interrupt came in: ready to go !
+	 */
+	reset_deassert(cd);
+	if (!wait_for_completion_timeout(&cd->card_boot, TIMEOUT)) {
+		ret = -ETIMEDOUT;
+		goto err_reset;
+	}
+	/*
+	 * according to the anybus docs, we're allowed to read these
+	 * without handshaking / reserving the area
+	 */
+	dev_info(dev, "Anybus-S card detected");
+	regmap_bulk_read(cd->regmap, REG_BOOTLOADER_V, val, 2);
+	dev_info(dev, "Bootloader version: %02X%02X",
+		 val[0], val[1]);
+	regmap_bulk_read(cd->regmap, REG_API_V, val, 2);
+	dev_info(dev, "API version: %02X%02X", val[0], val[1]);
+	regmap_bulk_read(cd->regmap, REG_FIELDBUS_V, val, 2);
+	dev_info(dev, "Fieldbus version: %02X%02X", val[0], val[1]);
+	regmap_bulk_read(cd->regmap, REG_SERIAL_NO, val, 4);
+	dev_info(dev, "Serial number: %02X%02X%02X%02X",
+		 val[0], val[1], val[2], val[3]);
+	add_device_randomness(&val, 4);
+	regmap_bulk_read(cd->regmap, REG_FIELDBUS_TYPE, &fieldbus_type,
+			 sizeof(fieldbus_type));
+	dev_info(dev, "Fieldbus type: %04X", be16_to_cpu(fieldbus_type));
+	regmap_bulk_read(cd->regmap, REG_MODULE_SW_V, val, 2);
+	dev_info(dev, "Module SW version: %02X%02X",
+		 val[0], val[1]);
+	/* put card back reset until a client driver releases it */
+	disable_irq(cd->irq);
+	reset_assert(cd);
+	atomic_set(&cd->ind_ab, IND_AB_UPDATED);
+	/* fire up the queue thread */
+	cd->qthread = kthread_run(qthread_fn, cd, dev_name(dev));
+	if (IS_ERR(cd->qthread)) {
+		dev_err(dev, "could not create kthread");
+		ret = PTR_ERR(cd->qthread);
+		goto err_reset;
+	}
+	/*
+	 * now advertise that we've detected a client device (card).
+	 * the bus infrastructure will match it to a client driver.
+	 */
+	cd->client = kzalloc(sizeof(*cd->client), GFP_KERNEL);
+	if (!cd->client) {
+		ret = -ENOMEM;
+		goto err_kthread;
+	}
+	cd->client->anybus_id = fieldbus_type;
+	cd->client->host = cd;
+	cd->client->dev.bus = &anybus_bus;
+	cd->client->dev.parent = dev;
+	cd->client->dev.release = client_device_release;
+	cd->client->dev.of_node =
+		anybus_of_find_child_device(dev, cd->host_idx);
+	dev_set_name(&cd->client->dev, "anybuss.card%d", cd->host_idx);
+	ret = device_register(&cd->client->dev);
+	if (ret)
+		goto err_device;
+	return cd;
+err_device:
+	put_device(&cd->client->dev);
+err_kthread:
+	kthread_stop(cd->qthread);
+err_reset:
+	reset_assert(cd);
+err_qcache:
+	kmem_cache_destroy(cd->qcache);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(anybuss_host_common_probe);
+
+void anybuss_host_common_remove(struct anybuss_host *host)
+{
+	struct anybuss_host *cd = host;
+
+	device_unregister(&cd->client->dev);
+	kthread_stop(cd->qthread);
+	reset_assert(cd);
+	kmem_cache_destroy(cd->qcache);
+}
+EXPORT_SYMBOL_GPL(anybuss_host_common_remove);
+
+static void host_release(void *res)
+{
+	anybuss_host_common_remove(res);
+}
+
+struct anybuss_host * __must_check
+devm_anybuss_host_common_probe(struct device *dev,
+			       const struct anybuss_ops *ops)
+{
+	struct anybuss_host *host;
+	int ret;
+
+	host = anybuss_host_common_probe(dev, ops);
+	if (IS_ERR(host))
+		return host;
+
+	ret = devm_add_action_or_reset(dev, host_release, host);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return host;
+}
+EXPORT_SYMBOL_GPL(devm_anybuss_host_common_probe);
+
+static int __init anybus_init(void)
+{
+	int ret;
+
+	ret = bus_register(&anybus_bus);
+	if (ret)
+		pr_err("could not register Anybus-S bus: %d\n", ret);
+	return ret;
+}
+module_init(anybus_init);
+
+static void __exit anybus_exit(void)
+{
+	bus_unregister(&anybus_bus);
+}
+module_exit(anybus_exit);
+
+MODULE_DESCRIPTION("HMS Anybus-S Host Driver");
+MODULE_AUTHOR("Sven Van Asbroeck <TheSven73@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/fieldbus/dev_core.c b/drivers/staging/fieldbus/dev_core.c
new file mode 100644
index 000000000..5aab73460
--- /dev/null
+++ b/drivers/staging/fieldbus/dev_core.c
@@ -0,0 +1,348 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Fieldbus Device Driver Core
+ *
+ */
+
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/idr.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/poll.h>
+
+/* move to <linux/fieldbus_dev.h> when taking this out of staging */
+#include "fieldbus_dev.h"
+
+/* Maximum number of fieldbus devices */
+#define MAX_FIELDBUSES		32
+
+/* the dev_t structure to store the dynamically allocated fieldbus devices */
+static dev_t fieldbus_devt;
+static DEFINE_IDA(fieldbus_ida);
+static DEFINE_MUTEX(fieldbus_mtx);
+
+static ssize_t online_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", !!fb->online);
+}
+static DEVICE_ATTR_RO(online);
+
+static ssize_t enabled_show(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	if (!fb->enable_get)
+		return -EINVAL;
+	return sprintf(buf, "%d\n", !!fb->enable_get(fb));
+}
+
+static ssize_t enabled_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t n)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+	bool value;
+	int ret;
+
+	if (!fb->simple_enable_set)
+		return -ENOTSUPP;
+	ret = kstrtobool(buf, &value);
+	if (ret)
+		return ret;
+	ret = fb->simple_enable_set(fb, value);
+	if (ret < 0)
+		return ret;
+	return n;
+}
+static DEVICE_ATTR_RW(enabled);
+
+static ssize_t card_name_show(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	/*
+	 * card_name was provided by child driver, could potentially be long.
+	 * protect against buffer overrun.
+	 */
+	return snprintf(buf, PAGE_SIZE, "%s\n", fb->card_name);
+}
+static DEVICE_ATTR_RO(card_name);
+
+static ssize_t read_area_size_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%zu\n", fb->read_area_sz);
+}
+static DEVICE_ATTR_RO(read_area_size);
+
+static ssize_t write_area_size_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%zu\n", fb->write_area_sz);
+}
+static DEVICE_ATTR_RO(write_area_size);
+
+static ssize_t fieldbus_id_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+
+	return fb->fieldbus_id_get(fb, buf, PAGE_SIZE);
+}
+static DEVICE_ATTR_RO(fieldbus_id);
+
+static ssize_t fieldbus_type_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+	const char *t;
+
+	switch (fb->fieldbus_type) {
+	case FIELDBUS_DEV_TYPE_PROFINET:
+		t = "profinet";
+		break;
+	default:
+		t = "unknown";
+		break;
+	}
+
+	return sprintf(buf, "%s\n", t);
+}
+static DEVICE_ATTR_RO(fieldbus_type);
+
+static struct attribute *fieldbus_attrs[] = {
+	&dev_attr_enabled.attr,
+	&dev_attr_card_name.attr,
+	&dev_attr_fieldbus_id.attr,
+	&dev_attr_read_area_size.attr,
+	&dev_attr_write_area_size.attr,
+	&dev_attr_online.attr,
+	&dev_attr_fieldbus_type.attr,
+	NULL,
+};
+
+static umode_t fieldbus_is_visible(struct kobject *kobj, struct attribute *attr,
+				   int n)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct fieldbus_dev *fb = dev_get_drvdata(dev);
+	umode_t mode = attr->mode;
+
+	if (attr == &dev_attr_enabled.attr) {
+		mode = 0;
+		if (fb->enable_get)
+			mode |= 0444;
+		if (fb->simple_enable_set)
+			mode |= 0200;
+	}
+
+	return mode;
+}
+
+static const struct attribute_group fieldbus_group = {
+	.attrs = fieldbus_attrs,
+	.is_visible = fieldbus_is_visible,
+};
+__ATTRIBUTE_GROUPS(fieldbus);
+
+static struct class fieldbus_class = {
+	.name =		"fieldbus_dev",
+	.owner =	THIS_MODULE,
+	.dev_groups =	fieldbus_groups,
+};
+
+struct fb_open_file {
+	struct fieldbus_dev *fbdev;
+	int dc_event;
+};
+
+static int fieldbus_open(struct inode *inode, struct file *filp)
+{
+	struct fb_open_file *of;
+	struct fieldbus_dev *fbdev = container_of(inode->i_cdev,
+						struct fieldbus_dev,
+						cdev);
+
+	of = kzalloc(sizeof(*of), GFP_KERNEL);
+	if (!of)
+		return -ENOMEM;
+	of->fbdev = fbdev;
+	filp->private_data = of;
+	return 0;
+}
+
+static int fieldbus_release(struct inode *node, struct file *filp)
+{
+	struct fb_open_file *of = filp->private_data;
+
+	kfree(of);
+	return 0;
+}
+
+static ssize_t fieldbus_read(struct file *filp, char __user *buf, size_t size,
+			     loff_t *offset)
+{
+	struct fb_open_file *of = filp->private_data;
+	struct fieldbus_dev *fbdev = of->fbdev;
+
+	of->dc_event = fbdev->dc_event;
+	return fbdev->read_area(fbdev, buf, size, offset);
+}
+
+static ssize_t fieldbus_write(struct file *filp, const char __user *buf,
+			      size_t size, loff_t *offset)
+{
+	struct fb_open_file *of = filp->private_data;
+	struct fieldbus_dev *fbdev = of->fbdev;
+
+	return fbdev->write_area(fbdev, buf, size, offset);
+}
+
+static __poll_t fieldbus_poll(struct file *filp, poll_table *wait)
+{
+	struct fb_open_file *of = filp->private_data;
+	struct fieldbus_dev *fbdev = of->fbdev;
+	__poll_t mask = EPOLLIN | EPOLLRDNORM | EPOLLOUT | EPOLLWRNORM;
+
+	poll_wait(filp, &fbdev->dc_wq, wait);
+	/* data changed ? */
+	if (fbdev->dc_event != of->dc_event)
+		mask |= EPOLLPRI | EPOLLERR;
+	return mask;
+}
+
+static const struct file_operations fieldbus_fops = {
+	.open		= fieldbus_open,
+	.release	= fieldbus_release,
+	.read		= fieldbus_read,
+	.write		= fieldbus_write,
+	.poll		= fieldbus_poll,
+	.llseek		= generic_file_llseek,
+	.owner		= THIS_MODULE,
+};
+
+void fieldbus_dev_area_updated(struct fieldbus_dev *fb)
+{
+	fb->dc_event++;
+	wake_up_all(&fb->dc_wq);
+}
+EXPORT_SYMBOL_GPL(fieldbus_dev_area_updated);
+
+void fieldbus_dev_online_changed(struct fieldbus_dev *fb, bool online)
+{
+	fb->online = online;
+	kobject_uevent(&fb->dev->kobj, KOBJ_CHANGE);
+}
+EXPORT_SYMBOL_GPL(fieldbus_dev_online_changed);
+
+static void __fieldbus_dev_unregister(struct fieldbus_dev *fb)
+{
+	if (!fb)
+		return;
+	device_destroy(&fieldbus_class, fb->cdev.dev);
+	cdev_del(&fb->cdev);
+	ida_simple_remove(&fieldbus_ida, fb->id);
+}
+
+void fieldbus_dev_unregister(struct fieldbus_dev *fb)
+{
+	mutex_lock(&fieldbus_mtx);
+	__fieldbus_dev_unregister(fb);
+	mutex_unlock(&fieldbus_mtx);
+}
+EXPORT_SYMBOL_GPL(fieldbus_dev_unregister);
+
+static int __fieldbus_dev_register(struct fieldbus_dev *fb)
+{
+	dev_t devno;
+	int err;
+
+	if (!fb)
+		return -EINVAL;
+	if (!fb->read_area || !fb->write_area || !fb->fieldbus_id_get)
+		return -EINVAL;
+	fb->id = ida_simple_get(&fieldbus_ida, 0, MAX_FIELDBUSES, GFP_KERNEL);
+	if (fb->id < 0)
+		return fb->id;
+	devno = MKDEV(MAJOR(fieldbus_devt), fb->id);
+	init_waitqueue_head(&fb->dc_wq);
+	cdev_init(&fb->cdev, &fieldbus_fops);
+	err = cdev_add(&fb->cdev, devno, 1);
+	if (err) {
+		pr_err("fieldbus_dev%d unable to add device %d:%d\n",
+		       fb->id, MAJOR(fieldbus_devt), fb->id);
+		goto err_cdev;
+	}
+	fb->dev = device_create(&fieldbus_class, fb->parent, devno, fb,
+				"fieldbus_dev%d", fb->id);
+	if (IS_ERR(fb->dev)) {
+		err = PTR_ERR(fb->dev);
+		goto err_dev_create;
+	}
+	return 0;
+
+err_dev_create:
+	cdev_del(&fb->cdev);
+err_cdev:
+	ida_simple_remove(&fieldbus_ida, fb->id);
+	return err;
+}
+
+int fieldbus_dev_register(struct fieldbus_dev *fb)
+{
+	int err;
+
+	mutex_lock(&fieldbus_mtx);
+	err = __fieldbus_dev_register(fb);
+	mutex_unlock(&fieldbus_mtx);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(fieldbus_dev_register);
+
+static int __init fieldbus_init(void)
+{
+	int err;
+
+	err = class_register(&fieldbus_class);
+	if (err < 0) {
+		pr_err("fieldbus_dev: could not register class\n");
+		return err;
+	}
+	err = alloc_chrdev_region(&fieldbus_devt, 0,
+				  MAX_FIELDBUSES, "fieldbus_dev");
+	if (err < 0) {
+		pr_err("fieldbus_dev: unable to allocate char dev region\n");
+		goto err_alloc;
+	}
+	return 0;
+
+err_alloc:
+	class_unregister(&fieldbus_class);
+	return err;
+}
+
+static void __exit fieldbus_exit(void)
+{
+	unregister_chrdev_region(fieldbus_devt, MAX_FIELDBUSES);
+	class_unregister(&fieldbus_class);
+	ida_destroy(&fieldbus_ida);
+}
+
+subsys_initcall(fieldbus_init);
+module_exit(fieldbus_exit);
+
+MODULE_AUTHOR("Sven Van Asbroeck <TheSven73@gmail.com>");
+MODULE_AUTHOR("Jonathan Stiles <jonathans@arcx.com>");
+MODULE_DESCRIPTION("Fieldbus Device Driver Core");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/fieldbus/fieldbus_dev.h b/drivers/staging/fieldbus/fieldbus_dev.h
new file mode 100644
index 000000000..301dca3b8
--- /dev/null
+++ b/drivers/staging/fieldbus/fieldbus_dev.h
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Fieldbus Device Driver Core
+ *
+ */
+
+#ifndef __FIELDBUS_DEV_H
+#define __FIELDBUS_DEV_H
+
+#include <linux/cdev.h>
+#include <linux/wait.h>
+
+enum fieldbus_dev_type {
+	FIELDBUS_DEV_TYPE_UNKNOWN = 0,
+	FIELDBUS_DEV_TYPE_PROFINET,
+};
+
+enum fieldbus_dev_offl_mode {
+	FIELDBUS_DEV_OFFL_MODE_CLEAR = 0,
+	FIELDBUS_DEV_OFFL_MODE_FREEZE,
+	FIELDBUS_DEV_OFFL_MODE_SET
+};
+
+/**
+ * struct fieldbus_dev - Fieldbus device
+ * @read_area:		[DRIVER] function to read the process data area of the
+ *				 device. same parameters/return values as
+ *				 the read function in struct file_operations
+ * @write_area:		[DRIVER] function to write to the process data area of
+ *				 the device. same parameters/return values as
+ *				 the write function in struct file_operations
+ * @write_area_sz	[DRIVER] size of the writable process data area
+ * @read_area_sz	[DRIVER] size of the readable process data area
+ * @card_name		[DRIVER] name of the card, e.g. "ACME Inc. profinet"
+ * @fieldbus_type	[DRIVER] fieldbus type of this device, e.g.
+ *					FIELDBUS_DEV_TYPE_PROFINET
+ * @enable_get		[DRIVER] function which returns true if the card
+ *				 is enabled, false otherwise
+ * @fieldbus_id_get	[DRIVER] function to retrieve the unique fieldbus id
+ *				 by which this device can be identified;
+ *				 return value follows the snprintf convention
+ * @simple_enable_set	[DRIVER] (optional) function to enable the device
+ *				 according to its default settings
+ * @parent		[DRIVER] (optional) the device's parent device
+ */
+struct fieldbus_dev {
+	ssize_t (*read_area)(struct fieldbus_dev *fbdev, char __user *buf,
+			     size_t size, loff_t *offset);
+	ssize_t (*write_area)(struct fieldbus_dev *fbdev,
+			      const char __user *buf, size_t size,
+			      loff_t *offset);
+	size_t write_area_sz, read_area_sz;
+	const char *card_name;
+	enum fieldbus_dev_type fieldbus_type;
+	bool (*enable_get)(struct fieldbus_dev *fbdev);
+	int (*fieldbus_id_get)(struct fieldbus_dev *fbdev, char *buf,
+			       size_t max_size);
+	int (*simple_enable_set)(struct fieldbus_dev *fbdev, bool enable);
+	struct device *parent;
+
+	/* private data */
+	int id;
+	struct cdev cdev;
+	struct device *dev;
+	int dc_event;
+	wait_queue_head_t dc_wq;
+	bool online;
+};
+
+#if IS_ENABLED(CONFIG_FIELDBUS_DEV)
+
+/**
+ * fieldbus_dev_unregister()
+ *	- unregister a previously registered fieldbus device
+ * @fb:		Device structure previously registered
+ **/
+void fieldbus_dev_unregister(struct fieldbus_dev *fb);
+
+/**
+ * fieldbus_dev_register()
+ *	- register a device with the fieldbus device subsystem
+ * @fb:		Device structure filled by the device driver
+ **/
+int __must_check fieldbus_dev_register(struct fieldbus_dev *fb);
+
+/**
+ * fieldbus_dev_area_updated()
+ *	- notify the subsystem that an external fieldbus controller updated
+ *			the process data area
+ * @fb:		Device structure
+ **/
+void fieldbus_dev_area_updated(struct fieldbus_dev *fb);
+
+/**
+ * fieldbus_dev_online_changed()
+ *	- notify the subsystem that the fieldbus online status changed
+ * @fb:		Device structure
+ **/
+void fieldbus_dev_online_changed(struct fieldbus_dev *fb, bool online);
+
+#else /* IS_ENABLED(CONFIG_FIELDBUS_DEV) */
+
+static inline void fieldbus_dev_unregister(struct fieldbus_dev *fb) {}
+static inline int __must_check fieldbus_dev_register(struct fieldbus_dev *fb)
+{
+	return -ENOTSUPP;
+}
+
+static inline void fieldbus_dev_area_updated(struct fieldbus_dev *fb) {}
+static inline void fieldbus_dev_online_changed(struct fieldbus_dev *fb,
+					       bool online) {}
+
+#endif /* IS_ENABLED(CONFIG_FIELDBUS_DEV) */
+#endif /* __FIELDBUS_DEV_H */
diff --git a/drivers/staging/gdm724x/Kconfig b/drivers/staging/gdm724x/Kconfig
new file mode 100644
index 000000000..1f403ecd9
--- /dev/null
+++ b/drivers/staging/gdm724x/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# GCT GDM724x LTE driver configuration
+#
+
+config LTE_GDM724X
+	tristate "GCT GDM724x LTE support"
+	depends on NET && USB && TTY && m
+	help
+	  This driver supports GCT GDM724x LTE chip based USB modem devices.
+	  It exposes 4 network devices to be used per PDN and 2 tty devices to be
+	  used for AT commands and DM monitoring applications.
+	  The modules will be called gdmulte.ko and gdmtty.ko
+
+	  GCT-ATCx can be used for AT Commands
+	  GCT-DMx can be used for LTE protocol monitoring
diff --git a/drivers/staging/gdm724x/Makefile b/drivers/staging/gdm724x/Makefile
new file mode 100644
index 000000000..e61b95788
--- /dev/null
+++ b/drivers/staging/gdm724x/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_LTE_GDM724X) := gdmulte.o
+gdmulte-y += gdm_lte.o netlink_k.o
+gdmulte-y += gdm_usb.o gdm_endian.o
+
+obj-$(CONFIG_LTE_GDM724X) += gdmtty.o
+gdmtty-y := gdm_tty.o gdm_mux.o
+
diff --git a/drivers/staging/gdm724x/TODO b/drivers/staging/gdm724x/TODO
new file mode 100644
index 000000000..b2b571ecb
--- /dev/null
+++ b/drivers/staging/gdm724x/TODO
@@ -0,0 +1,16 @@
+TODO:
+- Clean up coding style to meet kernel standard. (80 line limit, netdev_err)
+- Remove test for host endian
+- Remove confusing macros (endian, hci_send, sdu_send, rcv_with_cb)
+- Fixes for every instances of function returning -1
+- Check for skb->len in gdm_lte_emulate_arp()
+- Use ALIGN() macro for dummy_cnt in up_to_host()
+- Error handling in init_usb()
+- Explain reason for multiples of 512 bytes in alloc_tx_struct()
+- Review use of atomic allocation for tx structs
+- No error checking for alloc_tx_struct in do_tx()
+- fix up static tty port allocation to be dynamic
+
+Patches to:
+	Jonathan Kim <jonathankim@gctsemi.com>
+	Dean ahn <deanahn@gctsemi.com>
diff --git a/drivers/staging/gdm724x/gdm_endian.c b/drivers/staging/gdm724x/gdm_endian.c
new file mode 100644
index 000000000..ae39e59da
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_endian.c
@@ -0,0 +1,37 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#include <linux/kernel.h>
+#include "gdm_endian.h"
+
+__dev16 gdm_cpu_to_dev16(u8 dev_ed, u16 x)
+{
+	if (dev_ed == ENDIANNESS_LITTLE)
+		return (__force __dev16)cpu_to_le16(x);
+	else
+		return (__force __dev16)cpu_to_be16(x);
+}
+
+u16 gdm_dev16_to_cpu(u8 dev_ed, __dev16 x)
+{
+	if (dev_ed == ENDIANNESS_LITTLE)
+		return le16_to_cpu((__force __le16)x);
+	else
+		return be16_to_cpu((__force __be16)x);
+}
+
+__dev32 gdm_cpu_to_dev32(u8 dev_ed, u32 x)
+{
+	if (dev_ed == ENDIANNESS_LITTLE)
+		return (__force __dev32)cpu_to_le32(x);
+	else
+		return (__force __dev32)cpu_to_be32(x);
+}
+
+u32 gdm_dev32_to_cpu(u8 dev_ed, __dev32 x)
+{
+	if (dev_ed == ENDIANNESS_LITTLE)
+		return le32_to_cpu((__force __le32)x);
+	else
+		return be32_to_cpu((__force __be32)x);
+}
diff --git a/drivers/staging/gdm724x/gdm_endian.h b/drivers/staging/gdm724x/gdm_endian.h
new file mode 100644
index 000000000..f373dc3a1
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_endian.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef __GDM_ENDIAN_H__
+#define __GDM_ENDIAN_H__
+
+#include <linux/types.h>
+
+/*
+ * For data in "device-endian" byte order (device endianness is model
+ * dependent).  Analogous to __leXX or __beXX.
+ */
+typedef __u32 __bitwise __dev32;
+typedef __u16 __bitwise __dev16;
+
+enum {
+	ENDIANNESS_MIN = 0,
+	ENDIANNESS_UNKNOWN,
+	ENDIANNESS_LITTLE,
+	ENDIANNESS_BIG,
+	ENDIANNESS_MIDDLE,
+	ENDIANNESS_MAX
+};
+
+__dev16 gdm_cpu_to_dev16(u8 dev_ed, u16 x);
+u16 gdm_dev16_to_cpu(u8 dev_ed, __dev16 x);
+__dev32 gdm_cpu_to_dev32(u8 dev_ed, u32 x);
+u32 gdm_dev32_to_cpu(u8 dev_ed, __dev32 x);
+
+#endif /*__GDM_ENDIAN_H__*/
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
new file mode 100644
index 000000000..5703a9ddb
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -0,0 +1,937 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/udp.h>
+#include <linux/in.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/in6.h>
+#include <linux/tcp.h>
+#include <linux/icmp.h>
+#include <linux/icmpv6.h>
+#include <linux/uaccess.h>
+#include <linux/errno.h>
+#include <net/ndisc.h>
+
+#include "gdm_lte.h"
+#include "netlink_k.h"
+#include "hci.h"
+#include "hci_packet.h"
+#include "gdm_endian.h"
+
+/*
+ * Netlink protocol number
+ */
+#define NETLINK_LTE 30
+
+/*
+ * Default MTU Size
+ */
+#define DEFAULT_MTU_SIZE 1500
+
+#define IP_VERSION_4	4
+#define IP_VERSION_6	6
+
+static struct {
+	int ref_cnt;
+	struct sock *sock;
+} lte_event;
+
+static struct device_type wwan_type = {
+	.name   = "wwan",
+};
+
+static int gdm_lte_open(struct net_device *dev)
+{
+	netif_start_queue(dev);
+	return 0;
+}
+
+static int gdm_lte_close(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+	return 0;
+}
+
+static int gdm_lte_set_config(struct net_device *dev, struct ifmap *map)
+{
+	if (dev->flags & IFF_UP)
+		return -EBUSY;
+	return 0;
+}
+
+static void tx_complete(void *arg)
+{
+	struct nic *nic = arg;
+
+	if (netif_queue_stopped(nic->netdev))
+		netif_wake_queue(nic->netdev);
+}
+
+static int gdm_lte_rx(struct sk_buff *skb, struct nic *nic, int nic_type)
+{
+	int ret, len;
+
+	len = skb->len + ETH_HLEN;
+	ret = netif_rx(skb);
+	if (ret == NET_RX_DROP) {
+		nic->stats.rx_dropped++;
+	} else {
+		nic->stats.rx_packets++;
+		nic->stats.rx_bytes += len;
+	}
+
+	return 0;
+}
+
+static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type)
+{
+	struct nic *nic = netdev_priv(skb_in->dev);
+	struct sk_buff *skb_out;
+	struct ethhdr eth;
+	struct vlan_ethhdr vlan_eth;
+	struct arphdr *arp_in;
+	struct arphdr *arp_out;
+	struct arpdata {
+		u8 ar_sha[ETH_ALEN];
+		u8 ar_sip[4];
+		u8 ar_tha[ETH_ALEN];
+		u8 ar_tip[4];
+	};
+	struct arpdata *arp_data_in;
+	struct arpdata *arp_data_out;
+	u8 arp_temp[60];
+	void *mac_header_data;
+	u32 mac_header_len;
+
+	/* Check for skb->len, discard if empty */
+	if (skb_in->len == 0)
+		return -ENODATA;
+
+	/* Format the mac header so that it can be put to skb */
+	if (ntohs(((struct ethhdr *)skb_in->data)->h_proto) == ETH_P_8021Q) {
+		memcpy(&vlan_eth, skb_in->data, sizeof(struct vlan_ethhdr));
+		mac_header_data = &vlan_eth;
+		mac_header_len = VLAN_ETH_HLEN;
+	} else {
+		memcpy(&eth, skb_in->data, sizeof(struct ethhdr));
+		mac_header_data = &eth;
+		mac_header_len = ETH_HLEN;
+	}
+
+	/* Get the pointer of the original request */
+	arp_in = (struct arphdr *)(skb_in->data + mac_header_len);
+	arp_data_in = (struct arpdata *)(skb_in->data + mac_header_len +
+					sizeof(struct arphdr));
+
+	/* Get the pointer of the outgoing response */
+	arp_out = (struct arphdr *)arp_temp;
+	arp_data_out = (struct arpdata *)(arp_temp + sizeof(struct arphdr));
+
+	/* Copy the arp header */
+	memcpy(arp_out, arp_in, sizeof(struct arphdr));
+	arp_out->ar_op = htons(ARPOP_REPLY);
+
+	/* Copy the arp payload: based on 2 bytes of mac and fill the IP */
+	arp_data_out->ar_sha[0] = arp_data_in->ar_sha[0];
+	arp_data_out->ar_sha[1] = arp_data_in->ar_sha[1];
+	memcpy(&arp_data_out->ar_sha[2], &arp_data_in->ar_tip[0], 4);
+	memcpy(&arp_data_out->ar_sip[0], &arp_data_in->ar_tip[0], 4);
+	memcpy(&arp_data_out->ar_tha[0], &arp_data_in->ar_sha[0], 6);
+	memcpy(&arp_data_out->ar_tip[0], &arp_data_in->ar_sip[0], 4);
+
+	/* Fill the destination mac with source mac of the received packet */
+	memcpy(mac_header_data, mac_header_data + ETH_ALEN, ETH_ALEN);
+	/* Fill the source mac with nic's source mac */
+	memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN);
+
+	/* Alloc skb and reserve align */
+	skb_out = dev_alloc_skb(skb_in->len);
+	if (!skb_out)
+		return -ENOMEM;
+	skb_reserve(skb_out, NET_IP_ALIGN);
+
+	skb_put_data(skb_out, mac_header_data, mac_header_len);
+	skb_put_data(skb_out, arp_out, sizeof(struct arphdr));
+	skb_put_data(skb_out, arp_data_out, sizeof(struct arpdata));
+
+	skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
+	skb_out->dev = skb_in->dev;
+	skb_reset_mac_header(skb_out);
+	skb_pull(skb_out, ETH_HLEN);
+
+	gdm_lte_rx(skb_out, nic, nic_type);
+
+	return 0;
+}
+
+static __sum16 icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len)
+{
+	unsigned short *w;
+	__wsum sum = 0;
+	int i;
+	u16 pa;
+
+	union {
+		struct {
+			u8 ph_src[16];
+			u8 ph_dst[16];
+			u32 ph_len;
+			u8 ph_zero[3];
+			u8 ph_nxt;
+		} ph __packed;
+		u16 pa[20];
+	} pseudo_header;
+
+	memset(&pseudo_header, 0, sizeof(pseudo_header));
+	memcpy(&pseudo_header.ph.ph_src, &ipv6->saddr.in6_u.u6_addr8, 16);
+	memcpy(&pseudo_header.ph.ph_dst, &ipv6->daddr.in6_u.u6_addr8, 16);
+	pseudo_header.ph.ph_len = be16_to_cpu(ipv6->payload_len);
+	pseudo_header.ph.ph_nxt = ipv6->nexthdr;
+
+	for (i = 0; i < ARRAY_SIZE(pseudo_header.pa); i++) {
+		pa = pseudo_header.pa[i];
+		sum = csum_add(sum, csum_unfold((__force __sum16)pa));
+	}
+
+	w = ptr;
+	while (len > 1) {
+		sum = csum_add(sum, csum_unfold((__force __sum16)*w++));
+		len -= 2;
+	}
+
+	return csum_fold(sum);
+}
+
+static int gdm_lte_emulate_ndp(struct sk_buff *skb_in, u32 nic_type)
+{
+	struct nic *nic = netdev_priv(skb_in->dev);
+	struct sk_buff *skb_out;
+	struct ethhdr eth;
+	struct vlan_ethhdr vlan_eth;
+	struct neighbour_advertisement {
+		u8 target_address[16];
+		u8 type;
+		u8 length;
+		u8 link_layer_address[6];
+	};
+	struct neighbour_advertisement na;
+	struct neighbour_solicitation {
+		u8 target_address[16];
+	};
+	struct neighbour_solicitation *ns;
+	struct ipv6hdr *ipv6_in;
+	struct ipv6hdr ipv6_out;
+	struct icmp6hdr *icmp6_in;
+	struct icmp6hdr icmp6_out;
+
+	void *mac_header_data;
+	u32 mac_header_len;
+
+	/* Format the mac header so that it can be put to skb */
+	if (ntohs(((struct ethhdr *)skb_in->data)->h_proto) == ETH_P_8021Q) {
+		memcpy(&vlan_eth, skb_in->data, sizeof(struct vlan_ethhdr));
+		if (ntohs(vlan_eth.h_vlan_encapsulated_proto) != ETH_P_IPV6)
+			return -EPROTONOSUPPORT;
+		mac_header_data = &vlan_eth;
+		mac_header_len = VLAN_ETH_HLEN;
+	} else {
+		memcpy(&eth, skb_in->data, sizeof(struct ethhdr));
+		if (ntohs(eth.h_proto) != ETH_P_IPV6)
+			return -EPROTONOSUPPORT;
+		mac_header_data = &eth;
+		mac_header_len = ETH_HLEN;
+	}
+
+	/* Check if this is IPv6 ICMP packet */
+	ipv6_in = (struct ipv6hdr *)(skb_in->data + mac_header_len);
+	if (ipv6_in->version != 6 || ipv6_in->nexthdr != IPPROTO_ICMPV6)
+		return -EPROTONOSUPPORT;
+
+	/* Check if this is NDP packet */
+	icmp6_in = (struct icmp6hdr *)(skb_in->data + mac_header_len +
+					sizeof(struct ipv6hdr));
+	if (icmp6_in->icmp6_type == NDISC_ROUTER_SOLICITATION) { /* Check RS */
+		return -EPROTONOSUPPORT;
+	} else if (icmp6_in->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
+		/* Check NS */
+		u8 icmp_na[sizeof(struct icmp6hdr) +
+			sizeof(struct neighbour_advertisement)];
+		u8 zero_addr8[16] = {0,};
+
+		if (memcmp(ipv6_in->saddr.in6_u.u6_addr8, zero_addr8, 16) == 0)
+			/* Duplicate Address Detection: Source IP is all zero */
+			return 0;
+
+		icmp6_out.icmp6_type = NDISC_NEIGHBOUR_ADVERTISEMENT;
+		icmp6_out.icmp6_code = 0;
+		icmp6_out.icmp6_cksum = 0;
+		/* R=0, S=1, O=1 */
+		icmp6_out.icmp6_dataun.un_data32[0] = htonl(0x60000000);
+
+		ns = (struct neighbour_solicitation *)
+			(skb_in->data + mac_header_len +
+			 sizeof(struct ipv6hdr) + sizeof(struct icmp6hdr));
+		memcpy(&na.target_address, ns->target_address, 16);
+		na.type = 0x02;
+		na.length = 1;
+		na.link_layer_address[0] = 0x00;
+		na.link_layer_address[1] = 0x0a;
+		na.link_layer_address[2] = 0x3b;
+		na.link_layer_address[3] = 0xaf;
+		na.link_layer_address[4] = 0x63;
+		na.link_layer_address[5] = 0xc7;
+
+		memcpy(&ipv6_out, ipv6_in, sizeof(struct ipv6hdr));
+		memcpy(ipv6_out.saddr.in6_u.u6_addr8, &na.target_address, 16);
+		memcpy(ipv6_out.daddr.in6_u.u6_addr8,
+		       ipv6_in->saddr.in6_u.u6_addr8, 16);
+		ipv6_out.payload_len = htons(sizeof(struct icmp6hdr) +
+				sizeof(struct neighbour_advertisement));
+
+		memcpy(icmp_na, &icmp6_out, sizeof(struct icmp6hdr));
+		memcpy(icmp_na + sizeof(struct icmp6hdr), &na,
+		       sizeof(struct neighbour_advertisement));
+
+		icmp6_out.icmp6_cksum = icmp6_checksum(&ipv6_out,
+						       (u16 *)icmp_na,
+						       sizeof(icmp_na));
+	} else {
+		return -EINVAL;
+	}
+
+	/* Fill the destination mac with source mac of the received packet */
+	memcpy(mac_header_data, mac_header_data + ETH_ALEN, ETH_ALEN);
+	/* Fill the source mac with nic's source mac */
+	memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN);
+
+	/* Alloc skb and reserve align */
+	skb_out = dev_alloc_skb(skb_in->len);
+	if (!skb_out)
+		return -ENOMEM;
+	skb_reserve(skb_out, NET_IP_ALIGN);
+
+	skb_put_data(skb_out, mac_header_data, mac_header_len);
+	skb_put_data(skb_out, &ipv6_out, sizeof(struct ipv6hdr));
+	skb_put_data(skb_out, &icmp6_out, sizeof(struct icmp6hdr));
+	skb_put_data(skb_out, &na, sizeof(struct neighbour_advertisement));
+
+	skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
+	skb_out->dev = skb_in->dev;
+	skb_reset_mac_header(skb_out);
+	skb_pull(skb_out, ETH_HLEN);
+
+	gdm_lte_rx(skb_out, nic, nic_type);
+
+	return 0;
+}
+
+static s32 gdm_lte_tx_nic_type(struct net_device *dev, struct sk_buff *skb)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct ethhdr *eth;
+	struct vlan_ethhdr *vlan_eth;
+	struct iphdr *ip;
+	struct ipv6hdr *ipv6;
+	int mac_proto;
+	void *network_data;
+	u32 nic_type;
+
+	/* NIC TYPE is based on the nic_id of this net_device */
+	nic_type = 0x00000010 | nic->nic_id;
+
+	/* Get ethernet protocol */
+	eth = (struct ethhdr *)skb->data;
+	if (ntohs(eth->h_proto) == ETH_P_8021Q) {
+		vlan_eth = skb_vlan_eth_hdr(skb);
+		mac_proto = ntohs(vlan_eth->h_vlan_encapsulated_proto);
+		network_data = skb->data + VLAN_ETH_HLEN;
+		nic_type |= NIC_TYPE_F_VLAN;
+	} else {
+		mac_proto = ntohs(eth->h_proto);
+		network_data = skb->data + ETH_HLEN;
+	}
+
+	/* Process packet for nic type */
+	switch (mac_proto) {
+	case ETH_P_ARP:
+		nic_type |= NIC_TYPE_ARP;
+		break;
+	case ETH_P_IP:
+		nic_type |= NIC_TYPE_F_IPV4;
+		ip = network_data;
+
+		/* Check DHCPv4 */
+		if (ip->protocol == IPPROTO_UDP) {
+			struct udphdr *udp =
+					network_data + sizeof(struct iphdr);
+			if (ntohs(udp->dest) == 67 || ntohs(udp->dest) == 68)
+				nic_type |= NIC_TYPE_F_DHCP;
+		}
+		break;
+	case ETH_P_IPV6:
+		nic_type |= NIC_TYPE_F_IPV6;
+		ipv6 = network_data;
+
+		if (ipv6->nexthdr == IPPROTO_ICMPV6) /* Check NDP request */ {
+			struct icmp6hdr *icmp6 =
+					network_data + sizeof(struct ipv6hdr);
+			if (icmp6->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION)
+				nic_type |= NIC_TYPE_ICMPV6;
+		} else if (ipv6->nexthdr == IPPROTO_UDP) /* Check DHCPv6 */ {
+			struct udphdr *udp =
+					network_data + sizeof(struct ipv6hdr);
+			if (ntohs(udp->dest) == 546 || ntohs(udp->dest) == 547)
+				nic_type |= NIC_TYPE_F_DHCP;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return nic_type;
+}
+
+static netdev_tx_t gdm_lte_tx(struct sk_buff *skb, struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	u32 nic_type;
+	void *data_buf;
+	int data_len;
+	int idx;
+	int ret = 0;
+
+	nic_type = gdm_lte_tx_nic_type(dev, skb);
+	if (nic_type == 0) {
+		netdev_err(dev, "tx - invalid nic_type\n");
+		return -EMEDIUMTYPE;
+	}
+
+	if (nic_type & NIC_TYPE_ARP) {
+		if (gdm_lte_emulate_arp(skb, nic_type) == 0) {
+			dev_kfree_skb(skb);
+			return 0;
+		}
+	}
+
+	if (nic_type & NIC_TYPE_ICMPV6) {
+		if (gdm_lte_emulate_ndp(skb, nic_type) == 0) {
+			dev_kfree_skb(skb);
+			return 0;
+		}
+	}
+
+	/*
+	 * Need byte shift (that is, remove VLAN tag) if there is one
+	 * For the case of ARP, this breaks the offset as vlan_ethhdr+4
+	 * is treated as ethhdr	However, it shouldn't be a problem as
+	 * the response starts from arp_hdr and ethhdr is created by this
+	 * driver based on the NIC mac
+	 */
+	if (nic_type & NIC_TYPE_F_VLAN) {
+		struct vlan_ethhdr *vlan_eth = skb_vlan_eth_hdr(skb);
+
+		nic->vlan_id = ntohs(vlan_eth->h_vlan_TCI) & VLAN_VID_MASK;
+		data_buf = skb->data + (VLAN_ETH_HLEN - ETH_HLEN);
+		data_len = skb->len - (VLAN_ETH_HLEN - ETH_HLEN);
+	} else {
+		nic->vlan_id = 0;
+		data_buf = skb->data;
+		data_len = skb->len;
+	}
+
+	/* If it is a ICMPV6 packet, clear all the other bits :
+	 * for backward compatibility with the firmware
+	 */
+	if (nic_type & NIC_TYPE_ICMPV6)
+		nic_type = NIC_TYPE_ICMPV6;
+
+	/* If it is not a dhcp packet, clear all the flag bits :
+	 * original NIC, otherwise the special flag (IPVX | DHCP)
+	 */
+	if (!(nic_type & NIC_TYPE_F_DHCP))
+		nic_type &= NIC_TYPE_MASK;
+
+	ret = sscanf(dev->name, "lte%d", &idx);
+	if (ret != 1) {
+		dev_kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	ret = nic->phy_dev->send_sdu_func(nic->phy_dev->priv_dev,
+					  data_buf, data_len,
+					  nic->pdn_table.dft_eps_id, 0,
+					  tx_complete, nic, idx,
+					  nic_type);
+
+	if (ret == TX_NO_BUFFER || ret == TX_NO_SPC) {
+		netif_stop_queue(dev);
+		if (ret == TX_NO_BUFFER)
+			ret = 0;
+		else
+			ret = -ENOSPC;
+	} else if (ret == TX_NO_DEV) {
+		ret = -ENODEV;
+	}
+
+	/* Updates tx stats */
+	if (ret) {
+		nic->stats.tx_dropped++;
+	} else {
+		nic->stats.tx_packets++;
+		nic->stats.tx_bytes += data_len;
+	}
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+static struct net_device_stats *gdm_lte_stats(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+
+	return &nic->stats;
+}
+
+static int gdm_lte_event_send(struct net_device *dev, char *buf, int len)
+{
+	struct phy_dev *phy_dev = ((struct nic *)netdev_priv(dev))->phy_dev;
+	struct hci_packet *hci = (struct hci_packet *)buf;
+	int length;
+	int idx;
+	int ret;
+
+	ret = sscanf(dev->name, "lte%d", &idx);
+	if (ret != 1)
+		return -EINVAL;
+
+	length = gdm_dev16_to_cpu(phy_dev->get_endian(phy_dev->priv_dev),
+				  hci->len) + HCI_HEADER_SIZE;
+	return netlink_send(lte_event.sock, idx, 0, buf, length, dev);
+}
+
+static void gdm_lte_event_rcv(struct net_device *dev, u16 type,
+			      void *msg, int len)
+{
+	struct nic *nic = netdev_priv(dev);
+
+	nic->phy_dev->send_hci_func(nic->phy_dev->priv_dev, msg, len, NULL,
+				    NULL);
+}
+
+int gdm_lte_event_init(void)
+{
+	if (lte_event.ref_cnt == 0)
+		lte_event.sock = netlink_init(NETLINK_LTE, gdm_lte_event_rcv);
+
+	if (lte_event.sock) {
+		lte_event.ref_cnt++;
+		return 0;
+	}
+
+	pr_err("event init failed\n");
+	return -ENODATA;
+}
+
+void gdm_lte_event_exit(void)
+{
+	if (lte_event.sock && --lte_event.ref_cnt == 0) {
+		sock_release(lte_event.sock->sk_socket);
+		lte_event.sock = NULL;
+	}
+}
+
+static int find_dev_index(u32 nic_type)
+{
+	u8 index;
+
+	index = (u8)(nic_type & 0x0000000f);
+	if (index >= MAX_NIC_TYPE)
+		return -EINVAL;
+
+	return index;
+}
+
+static void gdm_lte_netif_rx(struct net_device *dev, char *buf,
+			     int len, int flagged_nic_type)
+{
+	u32 nic_type;
+	struct nic *nic;
+	struct sk_buff *skb;
+	struct ethhdr eth;
+	struct vlan_ethhdr vlan_eth;
+	void *mac_header_data;
+	u32 mac_header_len;
+	char ip_version = 0;
+
+	nic_type = flagged_nic_type & NIC_TYPE_MASK;
+	nic = netdev_priv(dev);
+
+	if (flagged_nic_type & NIC_TYPE_F_DHCP) {
+		/* Change the destination mac address
+		 * with the one requested the IP
+		 */
+		if (flagged_nic_type & NIC_TYPE_F_IPV4) {
+			struct dhcp_packet {
+				u8 op;      /* BOOTREQUEST or BOOTREPLY */
+				u8 htype;   /* hardware address type.
+					     * 1 = 10mb ethernet
+					     */
+				u8 hlen;    /* hardware address length */
+				u8 hops;    /* used by relay agents only */
+				u32 xid;    /* unique id */
+				u16 secs;   /* elapsed since client began
+					     * acquisition/renewal
+					     */
+				u16 flags;  /* only one flag so far: */
+				#define BROADCAST_FLAG 0x8000
+				/* "I need broadcast replies" */
+				u32 ciaddr; /* client IP (if client is in
+					     * BOUND, RENEW or REBINDING state)
+					     */
+				u32 yiaddr; /* 'your' (client) IP address */
+				/* IP address of next server to use in
+				 * bootstrap, returned in DHCPOFFER,
+				 * DHCPACK by server
+				 */
+				u32 siaddr_nip;
+				u32 gateway_nip; /* relay agent IP address */
+				u8 chaddr[16];   /* link-layer client hardware
+						  * address (MAC)
+						  */
+				u8 sname[64];    /* server host name (ASCIZ) */
+				u8 file[128];    /* boot file name (ASCIZ) */
+				u32 cookie;      /* fixed first four option
+						  * bytes (99,130,83,99 dec)
+						  */
+			} __packed;
+			int offset = sizeof(struct iphdr) +
+				     sizeof(struct udphdr) +
+				     offsetof(struct dhcp_packet, chaddr);
+			if (offset + ETH_ALEN > len)
+				return;
+			ether_addr_copy(nic->dest_mac_addr, buf + offset);
+		}
+	}
+
+	if (nic->vlan_id > 0) {
+		mac_header_data = (void *)&vlan_eth;
+		mac_header_len = VLAN_ETH_HLEN;
+	} else {
+		mac_header_data = (void *)&eth;
+		mac_header_len = ETH_HLEN;
+	}
+
+	/* Format the data so that it can be put to skb */
+	ether_addr_copy(mac_header_data, nic->dest_mac_addr);
+	memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN);
+
+	vlan_eth.h_vlan_TCI = htons(nic->vlan_id);
+	vlan_eth.h_vlan_proto = htons(ETH_P_8021Q);
+
+	if (nic_type == NIC_TYPE_ARP) {
+		/* Should be response: Only happens because
+		 * there was a request from the host
+		 */
+		eth.h_proto = htons(ETH_P_ARP);
+		vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_ARP);
+	} else {
+		ip_version = buf[0] >> 4;
+		if (ip_version == IP_VERSION_4) {
+			eth.h_proto = htons(ETH_P_IP);
+			vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IP);
+		} else if (ip_version == IP_VERSION_6) {
+			eth.h_proto = htons(ETH_P_IPV6);
+			vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IPV6);
+		} else {
+			netdev_err(dev, "Unknown IP version %d\n", ip_version);
+			return;
+		}
+	}
+
+	/* Alloc skb and reserve align */
+	skb = dev_alloc_skb(len + mac_header_len + NET_IP_ALIGN);
+	if (!skb)
+		return;
+	skb_reserve(skb, NET_IP_ALIGN);
+
+	skb_put_data(skb, mac_header_data, mac_header_len);
+	skb_put_data(skb, buf, len);
+
+	skb->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
+	skb->dev = dev;
+	skb_reset_mac_header(skb);
+	skb_pull(skb, ETH_HLEN);
+
+	gdm_lte_rx(skb, nic, nic_type);
+}
+
+static void gdm_lte_multi_sdu_pkt(struct phy_dev *phy_dev, char *buf, int len)
+{
+	struct net_device *dev;
+	struct multi_sdu *multi_sdu = (struct multi_sdu *)buf;
+	struct sdu *sdu = NULL;
+	u8 endian = phy_dev->get_endian(phy_dev->priv_dev);
+	u8 *data = (u8 *)multi_sdu->data;
+	int copied;
+	u16 i = 0;
+	u16 num_packet;
+	u16 hci_len;
+	u16 cmd_evt;
+	u32 nic_type;
+	int index;
+
+	num_packet = gdm_dev16_to_cpu(endian, multi_sdu->num_packet);
+
+	for (i = 0; i < num_packet; i++) {
+		copied = data - multi_sdu->data;
+		if (len < copied + sizeof(*sdu)) {
+			pr_err("rx prevent buffer overflow");
+			return;
+		}
+
+		sdu = (struct sdu *)data;
+
+		cmd_evt  = gdm_dev16_to_cpu(endian, sdu->cmd_evt);
+		hci_len  = gdm_dev16_to_cpu(endian, sdu->len);
+		nic_type = gdm_dev32_to_cpu(endian, sdu->nic_type);
+
+		if (cmd_evt != LTE_RX_SDU) {
+			pr_err("rx sdu wrong hci %04x\n", cmd_evt);
+			return;
+		}
+		if (hci_len < 12 ||
+		    len < copied + sizeof(*sdu) + (hci_len - 12)) {
+			pr_err("rx sdu invalid len %d\n", hci_len);
+			return;
+		}
+
+		index = find_dev_index(nic_type);
+		if (index < 0) {
+			pr_err("rx sdu invalid nic_type :%x\n", nic_type);
+			return;
+		}
+		dev = phy_dev->dev[index];
+		gdm_lte_netif_rx(dev, (char *)sdu->data,
+				 (int)(hci_len - 12), nic_type);
+
+		data += ((hci_len + 3) & 0xfffc) + HCI_HEADER_SIZE;
+	}
+}
+
+static void gdm_lte_pdn_table(struct net_device *dev, char *buf, int len)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf;
+	u8 ed = nic->phy_dev->get_endian(nic->phy_dev->priv_dev);
+
+	if (!pdn_table->activate) {
+		memset(&nic->pdn_table, 0x00, sizeof(struct pdn_table));
+		netdev_info(dev, "pdn deactivated\n");
+
+		return;
+	}
+
+	nic->pdn_table.activate = pdn_table->activate;
+	nic->pdn_table.dft_eps_id = gdm_dev32_to_cpu(ed, pdn_table->dft_eps_id);
+	nic->pdn_table.nic_type = gdm_dev32_to_cpu(ed, pdn_table->nic_type);
+
+	netdev_info(dev, "pdn activated, nic_type=0x%x\n",
+		    nic->pdn_table.nic_type);
+}
+
+static int gdm_lte_receive_pkt(struct phy_dev *phy_dev, char *buf, int len)
+{
+	struct hci_packet *hci = (struct hci_packet *)buf;
+	struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf;
+	struct sdu *sdu;
+	struct net_device *dev;
+	u8 endian = phy_dev->get_endian(phy_dev->priv_dev);
+	int ret = 0;
+	u16 cmd_evt;
+	u32 nic_type;
+	int index;
+
+	if (!len)
+		return ret;
+
+	cmd_evt = gdm_dev16_to_cpu(endian, hci->cmd_evt);
+
+	dev = phy_dev->dev[0];
+	if (!dev)
+		return 0;
+
+	switch (cmd_evt) {
+	case LTE_RX_SDU:
+		sdu = (struct sdu *)hci->data;
+		nic_type = gdm_dev32_to_cpu(endian, sdu->nic_type);
+		index = find_dev_index(nic_type);
+		if (index < 0)
+			return index;
+		dev = phy_dev->dev[index];
+		gdm_lte_netif_rx(dev, hci->data, len, nic_type);
+		break;
+	case LTE_RX_MULTI_SDU:
+		gdm_lte_multi_sdu_pkt(phy_dev, buf, len);
+		break;
+	case LTE_LINK_ON_OFF_INDICATION:
+		netdev_info(dev, "link %s\n",
+			    ((struct hci_connect_ind *)buf)->connect
+			    ? "on" : "off");
+		break;
+	case LTE_PDN_TABLE_IND:
+		pdn_table = (struct hci_pdn_table_ind *)buf;
+		nic_type = gdm_dev32_to_cpu(endian, pdn_table->nic_type);
+		index = find_dev_index(nic_type);
+		if (index < 0)
+			return index;
+		dev = phy_dev->dev[index];
+		gdm_lte_pdn_table(dev, buf, len);
+		fallthrough;
+	default:
+		ret = gdm_lte_event_send(dev, buf, len);
+		break;
+	}
+
+	return ret;
+}
+
+static int rx_complete(void *arg, void *data, int len, int context)
+{
+	struct phy_dev *phy_dev = arg;
+
+	return gdm_lte_receive_pkt(phy_dev, data, len);
+}
+
+void start_rx_proc(struct phy_dev *phy_dev)
+{
+	int i;
+
+	for (i = 0; i < MAX_RX_SUBMIT_COUNT; i++)
+		phy_dev->rcv_func(phy_dev->priv_dev,
+				rx_complete, phy_dev, USB_COMPLETE);
+}
+
+static const struct net_device_ops gdm_netdev_ops = {
+	.ndo_open			= gdm_lte_open,
+	.ndo_stop			= gdm_lte_close,
+	.ndo_set_config			= gdm_lte_set_config,
+	.ndo_start_xmit			= gdm_lte_tx,
+	.ndo_get_stats			= gdm_lte_stats,
+};
+
+static u8 gdm_lte_macaddr[ETH_ALEN] = {0x00, 0x0a, 0x3b, 0x00, 0x00, 0x00};
+
+static void form_mac_address(u8 *dev_addr, u8 *nic_src, u8 *nic_dest,
+			     u8 *mac_address, u8 index)
+{
+	/* Form the dev_addr */
+	if (!mac_address)
+		ether_addr_copy(dev_addr, gdm_lte_macaddr);
+	else
+		ether_addr_copy(dev_addr, mac_address);
+
+	/* The last byte of the mac address
+	 * should be less than or equal to 0xFC
+	 */
+	dev_addr[ETH_ALEN - 1] += index;
+
+	/* Create random nic src and copy the first
+	 * 3 bytes to be the same as dev_addr
+	 */
+	eth_random_addr(nic_src);
+	memcpy(nic_src, dev_addr, 3);
+
+	/* Copy the nic_dest from dev_addr*/
+	ether_addr_copy(nic_dest, dev_addr);
+}
+
+static void validate_mac_address(u8 *mac_address)
+{
+	/* if zero address or multicast bit set, restore the default value */
+	if (is_zero_ether_addr(mac_address) || (mac_address[0] & 0x01)) {
+		pr_err("MAC invalid, restoring default\n");
+		memcpy(mac_address, gdm_lte_macaddr, 6);
+	}
+}
+
+int register_lte_device(struct phy_dev *phy_dev,
+			struct device *dev, u8 *mac_address)
+{
+	struct nic *nic;
+	struct net_device *net;
+	char pdn_dev_name[16];
+	u8 addr[ETH_ALEN];
+	int ret = 0;
+	u8 index;
+
+	validate_mac_address(mac_address);
+
+	for (index = 0; index < MAX_NIC_TYPE; index++) {
+		/* Create device name lteXpdnX */
+		sprintf(pdn_dev_name, "lte%%dpdn%d", index);
+
+		/* Allocate netdev */
+		net = alloc_netdev(sizeof(struct nic), pdn_dev_name,
+				   NET_NAME_UNKNOWN, ether_setup);
+		if (!net) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		net->netdev_ops = &gdm_netdev_ops;
+		net->flags &= ~IFF_MULTICAST;
+		net->mtu = DEFAULT_MTU_SIZE;
+
+		nic = netdev_priv(net);
+		memset(nic, 0, sizeof(struct nic));
+		nic->netdev = net;
+		nic->phy_dev = phy_dev;
+		nic->nic_id = index;
+
+		form_mac_address(addr,
+				 nic->src_mac_addr,
+				 nic->dest_mac_addr,
+				 mac_address,
+				 index);
+		eth_hw_addr_set(net, addr);
+
+		SET_NETDEV_DEV(net, dev);
+		SET_NETDEV_DEVTYPE(net, &wwan_type);
+
+		ret = register_netdev(net);
+		if (ret)
+			goto err;
+
+		netif_carrier_on(net);
+
+		phy_dev->dev[index] = net;
+	}
+
+	return 0;
+
+err:
+	unregister_lte_device(phy_dev);
+
+	return ret;
+}
+
+void unregister_lte_device(struct phy_dev *phy_dev)
+{
+	struct net_device *net;
+	int index;
+
+	for (index = 0; index < MAX_NIC_TYPE; index++) {
+		net = phy_dev->dev[index];
+		if (!net)
+			continue;
+
+		unregister_netdev(net);
+		free_netdev(net);
+	}
+}
diff --git a/drivers/staging/gdm724x/gdm_lte.h b/drivers/staging/gdm724x/gdm_lte.h
new file mode 100644
index 000000000..f2143a6e0
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_lte.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _GDM_LTE_H_
+#define _GDM_LTE_H_
+
+#include <linux/netdevice.h>
+#include <linux/types.h>
+
+#include "gdm_endian.h"
+
+#define MAX_NIC_TYPE		4
+#define MAX_RX_SUBMIT_COUNT	3
+#define DRIVER_VERSION		"3.7.17.0"
+
+enum TX_ERROR_CODE {
+	TX_NO_ERROR = 0,
+	TX_NO_DEV,
+	TX_NO_SPC,
+	TX_NO_BUFFER,
+};
+
+enum CALLBACK_CONTEXT {
+	KERNEL_THREAD = 0,
+	USB_COMPLETE,
+};
+
+struct pdn_table {
+	u8 activate;
+	u32 dft_eps_id;
+	u32 nic_type;
+} __packed;
+
+struct nic;
+
+struct phy_dev {
+	void	*priv_dev;
+	struct net_device *dev[MAX_NIC_TYPE];
+	int	(*send_hci_func)(void *priv_dev, void *data, int len,
+				 void (*cb)(void *cb_data), void *cb_data);
+	int	(*send_sdu_func)(void *priv_dev, void *data, int len,
+				 unsigned int dft_eps_id, unsigned int eps_id,
+				 void (*cb)(void *cb_data), void *cb_data,
+				 int dev_idx, int nic_type);
+	int	(*rcv_func)(void *priv_dev,
+			    int (*cb)(void *cb_data, void *data, int len,
+				      int context),
+			    void *cb_data, int context);
+	u8 (*get_endian)(void *priv_dev);
+};
+
+struct nic {
+	struct net_device *netdev;
+	struct phy_dev *phy_dev;
+	struct net_device_stats stats;
+	struct pdn_table pdn_table;
+	u8 dest_mac_addr[ETH_ALEN];
+	u8 src_mac_addr[ETH_ALEN];
+	u32 nic_id;
+	u16 vlan_id;
+};
+
+int gdm_lte_event_init(void);
+void gdm_lte_event_exit(void);
+
+void start_rx_proc(struct phy_dev *phy_dev);
+int register_lte_device(struct phy_dev *phy_dev, struct device *dev,
+			u8 *mac_address);
+void unregister_lte_device(struct phy_dev *phy_dev);
+
+#endif /* _GDM_LTE_H_ */
diff --git a/drivers/staging/gdm724x/gdm_mux.c b/drivers/staging/gdm724x/gdm_mux.c
new file mode 100644
index 000000000..9b1261967
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_mux.c
@@ -0,0 +1,668 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/slab.h>
+#include <linux/usb/cdc.h>
+
+#include "gdm_mux.h"
+
+static u16 packet_type_for_tty_index[TTY_MAX_COUNT] = {0xF011, 0xF010};
+
+#define USB_DEVICE_CDC_DATA(vid, pid) \
+	.match_flags = \
+		USB_DEVICE_ID_MATCH_DEVICE |\
+		USB_DEVICE_ID_MATCH_INT_CLASS |\
+		USB_DEVICE_ID_MATCH_INT_SUBCLASS,\
+	.idVendor = vid,\
+	.idProduct = pid,\
+	.bInterfaceClass = USB_CLASS_COMM,\
+	.bInterfaceSubClass = USB_CDC_SUBCLASS_ACM
+
+static const struct usb_device_id id_table[] = {
+	{ USB_DEVICE_CDC_DATA(0x1076, 0x8000) }, /* GCT GDM7240 */
+	{ USB_DEVICE_CDC_DATA(0x1076, 0x8f00) }, /* GCT GDM7243 */
+	{ USB_DEVICE_CDC_DATA(0x1076, 0x9000) }, /* GCT GDM7243 */
+	{ USB_DEVICE_CDC_DATA(0x1d74, 0x2300) }, /* LGIT Phoenix */
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, id_table);
+
+static int packet_type_to_tty_index(u16 packet_type)
+{
+	int i;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++) {
+		if (packet_type_for_tty_index[i] == packet_type)
+			return i;
+	}
+
+	return -1;
+}
+
+static struct mux_tx *alloc_mux_tx(int len)
+{
+	struct mux_tx *t;
+
+	t = kzalloc(sizeof(*t), GFP_ATOMIC);
+	if (!t)
+		return NULL;
+
+	t->urb = usb_alloc_urb(0, GFP_ATOMIC);
+	t->buf = kmalloc(MUX_TX_MAX_SIZE, GFP_ATOMIC);
+	if (!t->urb || !t->buf) {
+		usb_free_urb(t->urb);
+		kfree(t->buf);
+		kfree(t);
+		return NULL;
+	}
+
+	return t;
+}
+
+static void free_mux_tx(struct mux_tx *t)
+{
+	if (t) {
+		usb_free_urb(t->urb);
+		kfree(t->buf);
+		kfree(t);
+	}
+}
+
+static struct mux_rx *alloc_mux_rx(void)
+{
+	struct mux_rx *r;
+
+	r = kzalloc(sizeof(*r), GFP_KERNEL);
+	if (!r)
+		return NULL;
+
+	r->urb = usb_alloc_urb(0, GFP_KERNEL);
+	r->buf = kmalloc(MUX_RX_MAX_SIZE, GFP_KERNEL);
+	if (!r->urb || !r->buf) {
+		usb_free_urb(r->urb);
+		kfree(r->buf);
+		kfree(r);
+		return NULL;
+	}
+
+	return r;
+}
+
+static void free_mux_rx(struct mux_rx *r)
+{
+	if (r) {
+		usb_free_urb(r->urb);
+		kfree(r->buf);
+		kfree(r);
+	}
+}
+
+static struct mux_rx *get_rx_struct(struct rx_cxt *rx)
+{
+	struct mux_rx *r;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rx->free_list_lock, flags);
+
+	if (list_empty(&rx->rx_free_list)) {
+		spin_unlock_irqrestore(&rx->free_list_lock, flags);
+		return NULL;
+	}
+
+	r = list_entry(rx->rx_free_list.prev, struct mux_rx, free_list);
+	list_del(&r->free_list);
+
+	spin_unlock_irqrestore(&rx->free_list_lock, flags);
+
+	return r;
+}
+
+static void put_rx_struct(struct rx_cxt *rx, struct mux_rx *r)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&rx->free_list_lock, flags);
+	list_add_tail(&r->free_list, &rx->rx_free_list);
+	spin_unlock_irqrestore(&rx->free_list_lock, flags);
+}
+
+static int up_to_host(struct mux_rx *r)
+{
+	struct mux_dev *mux_dev = r->mux_dev;
+	struct mux_pkt_header *mux_header;
+	unsigned int start_flag;
+	unsigned int payload_size;
+	unsigned short packet_type;
+	int total_len;
+	u32 packet_size_sum = r->offset;
+	int index;
+	int ret = TO_HOST_INVALID_PACKET;
+	int len = r->len;
+
+	while (1) {
+		mux_header = (struct mux_pkt_header *)(r->buf +
+						       packet_size_sum);
+		start_flag = __le32_to_cpu(mux_header->start_flag);
+		payload_size = __le32_to_cpu(mux_header->payload_size);
+		packet_type = __le16_to_cpu(mux_header->packet_type);
+
+		if (start_flag != START_FLAG) {
+			pr_err("invalid START_FLAG %x\n", start_flag);
+			break;
+		}
+
+		total_len = ALIGN(MUX_HEADER_SIZE + payload_size, 4);
+
+		if (len - packet_size_sum < total_len) {
+			pr_err("invalid payload : %d %d %04x\n",
+			       payload_size, len, packet_type);
+			break;
+		}
+
+		index = packet_type_to_tty_index(packet_type);
+		if (index < 0) {
+			pr_err("invalid index %d\n", index);
+			break;
+		}
+
+		ret = r->callback(mux_header->data,
+				payload_size,
+				index,
+				mux_dev->tty_dev,
+				RECV_PACKET_PROCESS_CONTINUE
+				);
+		if (ret == TO_HOST_BUFFER_REQUEST_FAIL) {
+			r->offset += packet_size_sum;
+			break;
+		}
+
+		packet_size_sum += total_len;
+		if (len - packet_size_sum <= MUX_HEADER_SIZE + 2) {
+			ret = r->callback(NULL,
+					0,
+					index,
+					mux_dev->tty_dev,
+					RECV_PACKET_PROCESS_COMPLETE
+					);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static void do_rx(struct work_struct *work)
+{
+	struct mux_dev *mux_dev =
+		container_of(work, struct mux_dev, work_rx.work);
+	struct mux_rx *r;
+	struct rx_cxt *rx = &mux_dev->rx;
+	unsigned long flags;
+	int ret = 0;
+
+	while (1) {
+		spin_lock_irqsave(&rx->to_host_lock, flags);
+		if (list_empty(&rx->to_host_list)) {
+			spin_unlock_irqrestore(&rx->to_host_lock, flags);
+			break;
+		}
+		r = list_entry(rx->to_host_list.next, struct mux_rx,
+			       to_host_list);
+		list_del(&r->to_host_list);
+		spin_unlock_irqrestore(&rx->to_host_lock, flags);
+
+		ret = up_to_host(r);
+		if (ret == TO_HOST_BUFFER_REQUEST_FAIL)
+			pr_err("failed to send mux data to host\n");
+		else
+			put_rx_struct(rx, r);
+	}
+}
+
+static void remove_rx_submit_list(struct mux_rx *r, struct rx_cxt *rx)
+{
+	unsigned long flags;
+	struct mux_rx	*r_remove, *r_remove_next;
+
+	spin_lock_irqsave(&rx->submit_list_lock, flags);
+	list_for_each_entry_safe(r_remove, r_remove_next, &rx->rx_submit_list,
+				 rx_submit_list) {
+		if (r == r_remove)
+			list_del(&r->rx_submit_list);
+	}
+	spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+}
+
+static void gdm_mux_rcv_complete(struct urb *urb)
+{
+	struct mux_rx *r = urb->context;
+	struct mux_dev *mux_dev = r->mux_dev;
+	struct rx_cxt *rx = &mux_dev->rx;
+	unsigned long flags;
+
+	remove_rx_submit_list(r, rx);
+
+	if (urb->status) {
+		if (mux_dev->usb_state == PM_NORMAL)
+			dev_err(&urb->dev->dev, "%s: urb status error %d\n",
+				__func__, urb->status);
+		put_rx_struct(rx, r);
+	} else {
+		r->len = r->urb->actual_length;
+		spin_lock_irqsave(&rx->to_host_lock, flags);
+		list_add_tail(&r->to_host_list, &rx->to_host_list);
+		schedule_work(&mux_dev->work_rx.work);
+		spin_unlock_irqrestore(&rx->to_host_lock, flags);
+	}
+}
+
+static int gdm_mux_recv(void *priv_dev,
+			int (*cb)(void *data, int len, int tty_index,
+				  struct tty_dev *tty_dev, int complete))
+{
+	struct mux_dev *mux_dev = priv_dev;
+	struct usb_device *usbdev = mux_dev->usbdev;
+	struct mux_rx *r;
+	struct rx_cxt *rx = &mux_dev->rx;
+	unsigned long flags;
+	int ret;
+
+	if (!usbdev) {
+		pr_err("device is disconnected\n");
+		return -ENODEV;
+	}
+
+	r = get_rx_struct(rx);
+	if (!r) {
+		pr_err("get_rx_struct fail\n");
+		return -ENOMEM;
+	}
+
+	r->offset = 0;
+	r->mux_dev = (void *)mux_dev;
+	r->callback = cb;
+	mux_dev->rx_cb = cb;
+
+	usb_fill_bulk_urb(r->urb,
+			  usbdev,
+			  usb_rcvbulkpipe(usbdev, 0x86),
+			  r->buf,
+			  MUX_RX_MAX_SIZE,
+			  gdm_mux_rcv_complete,
+			  r);
+
+	spin_lock_irqsave(&rx->submit_list_lock, flags);
+	list_add_tail(&r->rx_submit_list, &rx->rx_submit_list);
+	spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+
+	ret = usb_submit_urb(r->urb, GFP_KERNEL);
+
+	if (ret) {
+		spin_lock_irqsave(&rx->submit_list_lock, flags);
+		list_del(&r->rx_submit_list);
+		spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+
+		put_rx_struct(rx, r);
+
+		pr_err("usb_submit_urb ret=%d\n", ret);
+	}
+
+	usb_mark_last_busy(usbdev);
+
+	return ret;
+}
+
+static void gdm_mux_send_complete(struct urb *urb)
+{
+	struct mux_tx *t = urb->context;
+
+	if (urb->status == -ECONNRESET) {
+		dev_info(&urb->dev->dev, "CONNRESET\n");
+		free_mux_tx(t);
+		return;
+	}
+
+	if (t->callback)
+		t->callback(t->cb_data);
+
+	free_mux_tx(t);
+}
+
+static int gdm_mux_send(void *priv_dev, void *data, int len, int tty_index,
+			void (*cb)(void *data), void *cb_data)
+{
+	struct mux_dev *mux_dev = priv_dev;
+	struct usb_device *usbdev = mux_dev->usbdev;
+	struct mux_pkt_header *mux_header;
+	struct mux_tx *t = NULL;
+	static u32 seq_num = 1;
+	int total_len;
+	int ret;
+	unsigned long flags;
+
+	if (mux_dev->usb_state == PM_SUSPEND) {
+		ret = usb_autopm_get_interface(mux_dev->intf);
+		if (!ret)
+			usb_autopm_put_interface(mux_dev->intf);
+	}
+
+	spin_lock_irqsave(&mux_dev->write_lock, flags);
+
+	total_len = ALIGN(MUX_HEADER_SIZE + len, 4);
+
+	t = alloc_mux_tx(total_len);
+	if (!t) {
+		pr_err("alloc_mux_tx fail\n");
+		spin_unlock_irqrestore(&mux_dev->write_lock, flags);
+		return -ENOMEM;
+	}
+
+	mux_header = (struct mux_pkt_header *)t->buf;
+	mux_header->start_flag = __cpu_to_le32(START_FLAG);
+	mux_header->seq_num = __cpu_to_le32(seq_num++);
+	mux_header->payload_size = __cpu_to_le32((u32)len);
+	mux_header->packet_type = __cpu_to_le16(packet_type_for_tty_index[tty_index]);
+
+	memcpy(t->buf + MUX_HEADER_SIZE, data, len);
+	memset(t->buf + MUX_HEADER_SIZE + len, 0,
+	       total_len - MUX_HEADER_SIZE - len);
+
+	t->len = total_len;
+	t->callback = cb;
+	t->cb_data = cb_data;
+
+	usb_fill_bulk_urb(t->urb,
+			  usbdev,
+			  usb_sndbulkpipe(usbdev, 5),
+			  t->buf,
+			  total_len,
+			  gdm_mux_send_complete,
+			  t);
+
+	ret = usb_submit_urb(t->urb, GFP_ATOMIC);
+
+	spin_unlock_irqrestore(&mux_dev->write_lock, flags);
+
+	if (ret)
+		pr_err("usb_submit_urb Error: %d\n", ret);
+
+	usb_mark_last_busy(usbdev);
+
+	return ret;
+}
+
+static int gdm_mux_send_control(void *priv_dev, int request, int value,
+				void *buf, int len)
+{
+	struct mux_dev *mux_dev = priv_dev;
+	struct usb_device *usbdev = mux_dev->usbdev;
+	int ret;
+
+	ret = usb_control_msg(usbdev,
+			      usb_sndctrlpipe(usbdev, 0),
+			      request,
+			      USB_RT_ACM,
+			      value,
+			      2,
+			      buf,
+			      len,
+			      5000
+			     );
+
+	if (ret < 0)
+		pr_err("usb_control_msg error: %d\n", ret);
+
+	return min(ret, 0);
+}
+
+static void release_usb(struct mux_dev *mux_dev)
+{
+	struct rx_cxt		*rx = &mux_dev->rx;
+	struct mux_rx		*r, *r_next;
+	unsigned long		flags;
+
+	cancel_delayed_work(&mux_dev->work_rx);
+
+	spin_lock_irqsave(&rx->submit_list_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->rx_submit_list,
+				 rx_submit_list) {
+		spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+		usb_kill_urb(r->urb);
+		spin_lock_irqsave(&rx->submit_list_lock, flags);
+	}
+	spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+
+	spin_lock_irqsave(&rx->free_list_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->rx_free_list, free_list) {
+		list_del(&r->free_list);
+		free_mux_rx(r);
+	}
+	spin_unlock_irqrestore(&rx->free_list_lock, flags);
+
+	spin_lock_irqsave(&rx->to_host_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->to_host_list, to_host_list) {
+		if (r->mux_dev == (void *)mux_dev) {
+			list_del(&r->to_host_list);
+			free_mux_rx(r);
+		}
+	}
+	spin_unlock_irqrestore(&rx->to_host_lock, flags);
+}
+
+static int init_usb(struct mux_dev *mux_dev)
+{
+	struct mux_rx *r;
+	struct rx_cxt *rx = &mux_dev->rx;
+	int ret = 0;
+	int i;
+
+	spin_lock_init(&mux_dev->write_lock);
+	INIT_LIST_HEAD(&rx->to_host_list);
+	INIT_LIST_HEAD(&rx->rx_submit_list);
+	INIT_LIST_HEAD(&rx->rx_free_list);
+	spin_lock_init(&rx->to_host_lock);
+	spin_lock_init(&rx->submit_list_lock);
+	spin_lock_init(&rx->free_list_lock);
+
+	for (i = 0; i < MAX_ISSUE_NUM * 2; i++) {
+		r = alloc_mux_rx();
+		if (!r) {
+			ret = -ENOMEM;
+			break;
+		}
+
+		list_add(&r->free_list, &rx->rx_free_list);
+	}
+
+	INIT_DELAYED_WORK(&mux_dev->work_rx, do_rx);
+
+	return ret;
+}
+
+static int gdm_mux_probe(struct usb_interface *intf,
+			 const struct usb_device_id *id)
+{
+	struct mux_dev *mux_dev;
+	struct tty_dev *tty_dev;
+	u16 idVendor, idProduct;
+	int bInterfaceNumber;
+	int ret;
+	int i;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+
+	bInterfaceNumber = intf->cur_altsetting->desc.bInterfaceNumber;
+
+	idVendor = __le16_to_cpu(usbdev->descriptor.idVendor);
+	idProduct = __le16_to_cpu(usbdev->descriptor.idProduct);
+
+	pr_info("mux vid = 0x%04x pid = 0x%04x\n", idVendor, idProduct);
+
+	if (bInterfaceNumber != 2)
+		return -ENODEV;
+
+	mux_dev = kzalloc(sizeof(*mux_dev), GFP_KERNEL);
+	if (!mux_dev)
+		return -ENOMEM;
+
+	tty_dev = kzalloc(sizeof(*tty_dev), GFP_KERNEL);
+	if (!tty_dev) {
+		ret = -ENOMEM;
+		goto err_free_mux;
+	}
+
+	mux_dev->usbdev = usbdev;
+	mux_dev->control_intf = intf;
+
+	ret = init_usb(mux_dev);
+	if (ret)
+		goto err_free_usb;
+
+	tty_dev->priv_dev = (void *)mux_dev;
+	tty_dev->send_func = gdm_mux_send;
+	tty_dev->recv_func = gdm_mux_recv;
+	tty_dev->send_control = gdm_mux_send_control;
+
+	ret = register_lte_tty_device(tty_dev, &intf->dev);
+	if (ret)
+		goto err_unregister_tty;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++)
+		mux_dev->tty_dev = tty_dev;
+
+	mux_dev->intf = intf;
+	mux_dev->usb_state = PM_NORMAL;
+
+	usb_get_dev(usbdev);
+	usb_set_intfdata(intf, tty_dev);
+
+	return 0;
+
+err_unregister_tty:
+	unregister_lte_tty_device(tty_dev);
+err_free_usb:
+	release_usb(mux_dev);
+	kfree(tty_dev);
+err_free_mux:
+	kfree(mux_dev);
+
+	return ret;
+}
+
+static void gdm_mux_disconnect(struct usb_interface *intf)
+{
+	struct tty_dev *tty_dev;
+	struct mux_dev *mux_dev;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+
+	tty_dev = usb_get_intfdata(intf);
+
+	mux_dev = tty_dev->priv_dev;
+
+	release_usb(mux_dev);
+	unregister_lte_tty_device(tty_dev);
+
+	kfree(mux_dev);
+	kfree(tty_dev);
+
+	usb_put_dev(usbdev);
+}
+
+static int gdm_mux_suspend(struct usb_interface *intf, pm_message_t pm_msg)
+{
+	struct tty_dev *tty_dev;
+	struct mux_dev *mux_dev;
+	struct rx_cxt *rx;
+	struct mux_rx *r, *r_next;
+	unsigned long flags;
+
+	tty_dev = usb_get_intfdata(intf);
+	mux_dev = tty_dev->priv_dev;
+	rx = &mux_dev->rx;
+
+	cancel_work_sync(&mux_dev->work_rx.work);
+
+	if (mux_dev->usb_state != PM_NORMAL) {
+		dev_err(intf->usb_dev, "usb suspend - invalid state\n");
+		return -1;
+	}
+
+	mux_dev->usb_state = PM_SUSPEND;
+
+	spin_lock_irqsave(&rx->submit_list_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->rx_submit_list,
+				 rx_submit_list) {
+		spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+		usb_kill_urb(r->urb);
+		spin_lock_irqsave(&rx->submit_list_lock, flags);
+	}
+	spin_unlock_irqrestore(&rx->submit_list_lock, flags);
+
+	return 0;
+}
+
+static int gdm_mux_resume(struct usb_interface *intf)
+{
+	struct tty_dev *tty_dev;
+	struct mux_dev *mux_dev;
+	u8 i;
+
+	tty_dev = usb_get_intfdata(intf);
+	mux_dev = tty_dev->priv_dev;
+
+	if (mux_dev->usb_state != PM_SUSPEND) {
+		dev_err(intf->usb_dev, "usb resume - invalid state\n");
+		return -1;
+	}
+
+	mux_dev->usb_state = PM_NORMAL;
+
+	for (i = 0; i < MAX_ISSUE_NUM; i++)
+		gdm_mux_recv(mux_dev, mux_dev->rx_cb);
+
+	return 0;
+}
+
+static struct usb_driver gdm_mux_driver = {
+	.name = "gdm_mux",
+	.probe = gdm_mux_probe,
+	.disconnect = gdm_mux_disconnect,
+	.id_table = id_table,
+	.supports_autosuspend = 1,
+	.suspend = gdm_mux_suspend,
+	.resume = gdm_mux_resume,
+	.reset_resume = gdm_mux_resume,
+};
+
+static int __init gdm_usb_mux_init(void)
+{
+	int ret;
+
+	ret = register_lte_tty_driver();
+	if (ret)
+		return ret;
+
+	return usb_register(&gdm_mux_driver);
+}
+
+static void __exit gdm_usb_mux_exit(void)
+{
+	usb_deregister(&gdm_mux_driver);
+	unregister_lte_tty_driver();
+}
+
+module_init(gdm_usb_mux_init);
+module_exit(gdm_usb_mux_exit);
+
+MODULE_DESCRIPTION("GCT LTE TTY Device Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/gdm724x/gdm_mux.h b/drivers/staging/gdm724x/gdm_mux.h
new file mode 100644
index 000000000..87b8d921f
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_mux.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _GDM_MUX_H_
+#define _GDM_MUX_H_
+
+#include <linux/types.h>
+#include <linux/usb.h>
+#include <linux/list.h>
+
+#include "gdm_tty.h"
+
+#define PM_NORMAL 0
+#define PM_SUSPEND 1
+
+#define USB_RT_ACM          (USB_TYPE_CLASS | USB_RECIP_INTERFACE)
+
+#define START_FLAG 0xA512485A
+#define MUX_HEADER_SIZE 14
+#define MUX_TX_MAX_SIZE (1024 * 10)
+#define MUX_RX_MAX_SIZE (1024 * 30)
+#define AT_PKT_TYPE 0xF011
+#define DM_PKT_TYPE 0xF010
+
+#define RETRY_TIMER 30 /* msec */
+
+struct mux_pkt_header {
+	__le32 start_flag;
+	__le32 seq_num;
+	__le32 payload_size;
+	__le16 packet_type;
+	unsigned char data[];
+};
+
+struct mux_tx {
+	struct urb *urb;
+	u8 *buf;
+	int  len;
+	void (*callback)(void *cb_data);
+	void *cb_data;
+};
+
+struct mux_rx {
+	struct list_head free_list;
+	struct list_head rx_submit_list;
+	struct list_head to_host_list;
+	struct urb *urb;
+	u8 *buf;
+	void *mux_dev;
+	u32 offset;
+	u32 len;
+	int (*callback)(void *data,
+			int len,
+			int tty_index,
+			struct tty_dev *tty_dev,
+			int complete);
+};
+
+struct rx_cxt {
+	struct list_head to_host_list;
+	struct list_head rx_submit_list;
+	struct list_head rx_free_list;
+	spinlock_t to_host_lock;
+	spinlock_t submit_list_lock;
+	spinlock_t free_list_lock;
+};
+
+struct mux_dev {
+	struct usb_device *usbdev;
+	struct usb_interface *control_intf;
+	struct usb_interface *data_intf;
+	struct rx_cxt	rx;
+	struct delayed_work work_rx;
+	struct usb_interface *intf;
+	int usb_state;
+	int (*rx_cb)(void *data,
+		     int len,
+		     int tty_index,
+		     struct tty_dev *tty_dev,
+		     int complete);
+	spinlock_t write_lock;
+	struct tty_dev *tty_dev;
+};
+
+#endif /* _GDM_MUX_H_ */
diff --git a/drivers/staging/gdm724x/gdm_tty.c b/drivers/staging/gdm724x/gdm_tty.c
new file mode 100644
index 000000000..cc6d80554
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_tty.c
@@ -0,0 +1,319 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb/cdc.h>
+#include <linux/serial.h>
+#include "gdm_tty.h"
+
+#define GDM_TTY_MAJOR 0
+#define GDM_TTY_MINOR 32
+
+#define WRITE_SIZE 2048
+
+#define MUX_TX_MAX_SIZE 2048
+
+#define GDM_TTY_READY(gdm) (gdm && gdm->tty_dev && gdm->port.count)
+
+static struct tty_driver *gdm_driver[TTY_MAX_COUNT];
+static struct gdm *gdm_table[TTY_MAX_COUNT][GDM_TTY_MINOR];
+static DEFINE_MUTEX(gdm_table_lock);
+
+static const char *DRIVER_STRING[TTY_MAX_COUNT] = {"GCTATC", "GCTDM"};
+static char *DEVICE_STRING[TTY_MAX_COUNT] = {"GCT-ATC", "GCT-DM"};
+
+static void gdm_port_destruct(struct tty_port *port)
+{
+	struct gdm *gdm = container_of(port, struct gdm, port);
+
+	mutex_lock(&gdm_table_lock);
+	gdm_table[gdm->index][gdm->minor] = NULL;
+	mutex_unlock(&gdm_table_lock);
+
+	kfree(gdm);
+}
+
+static const struct tty_port_operations gdm_port_ops = {
+	.destruct = gdm_port_destruct,
+};
+
+static int gdm_tty_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct gdm *gdm = NULL;
+	int ret;
+
+	ret = match_string(DRIVER_STRING, TTY_MAX_COUNT,
+			   tty->driver->driver_name);
+	if (ret < 0)
+		return -ENODEV;
+
+	mutex_lock(&gdm_table_lock);
+	gdm = gdm_table[ret][tty->index];
+	if (!gdm) {
+		mutex_unlock(&gdm_table_lock);
+		return -ENODEV;
+	}
+
+	tty_port_get(&gdm->port);
+
+	ret = tty_standard_install(driver, tty);
+	if (ret) {
+		tty_port_put(&gdm->port);
+		mutex_unlock(&gdm_table_lock);
+		return ret;
+	}
+
+	tty->driver_data = gdm;
+	mutex_unlock(&gdm_table_lock);
+
+	return 0;
+}
+
+static int gdm_tty_open(struct tty_struct *tty, struct file *filp)
+{
+	struct gdm *gdm = tty->driver_data;
+
+	return tty_port_open(&gdm->port, tty, filp);
+}
+
+static void gdm_tty_cleanup(struct tty_struct *tty)
+{
+	struct gdm *gdm = tty->driver_data;
+
+	tty_port_put(&gdm->port);
+}
+
+static void gdm_tty_hangup(struct tty_struct *tty)
+{
+	struct gdm *gdm = tty->driver_data;
+
+	tty_port_hangup(&gdm->port);
+}
+
+static void gdm_tty_close(struct tty_struct *tty, struct file *filp)
+{
+	struct gdm *gdm = tty->driver_data;
+
+	tty_port_close(&gdm->port, tty, filp);
+}
+
+static int gdm_tty_recv_complete(void *data,
+				 int len,
+				 int index,
+				 struct tty_dev *tty_dev,
+				 int complete)
+{
+	struct gdm *gdm = tty_dev->gdm[index];
+
+	if (!GDM_TTY_READY(gdm)) {
+		if (complete == RECV_PACKET_PROCESS_COMPLETE)
+			gdm->tty_dev->recv_func(gdm->tty_dev->priv_dev,
+						gdm_tty_recv_complete);
+		return TO_HOST_PORT_CLOSE;
+	}
+
+	if (data && len) {
+		if (tty_buffer_request_room(&gdm->port, len) == len) {
+			tty_insert_flip_string(&gdm->port, data, len);
+			tty_flip_buffer_push(&gdm->port);
+		} else {
+			return TO_HOST_BUFFER_REQUEST_FAIL;
+		}
+	}
+
+	if (complete == RECV_PACKET_PROCESS_COMPLETE)
+		gdm->tty_dev->recv_func(gdm->tty_dev->priv_dev,
+					gdm_tty_recv_complete);
+
+	return 0;
+}
+
+static void gdm_tty_send_complete(void *arg)
+{
+	struct gdm *gdm = arg;
+
+	if (!GDM_TTY_READY(gdm))
+		return;
+
+	tty_port_tty_wakeup(&gdm->port);
+}
+
+static int gdm_tty_write(struct tty_struct *tty, const unsigned char *buf,
+			 int len)
+{
+	struct gdm *gdm = tty->driver_data;
+	int remain = len;
+	int sent_len = 0;
+	int sending_len = 0;
+
+	if (!GDM_TTY_READY(gdm))
+		return -ENODEV;
+
+	if (!len)
+		return 0;
+
+	while (1) {
+		sending_len = min(MUX_TX_MAX_SIZE, remain);
+		gdm->tty_dev->send_func(gdm->tty_dev->priv_dev,
+					(void *)(buf + sent_len),
+					sending_len,
+					gdm->index,
+					gdm_tty_send_complete,
+					gdm);
+		sent_len += sending_len;
+		remain -= sending_len;
+		if (remain <= 0)
+			break;
+	}
+
+	return len;
+}
+
+static unsigned int gdm_tty_write_room(struct tty_struct *tty)
+{
+	struct gdm *gdm = tty->driver_data;
+
+	if (!GDM_TTY_READY(gdm))
+		return 0;
+
+	return WRITE_SIZE;
+}
+
+int register_lte_tty_device(struct tty_dev *tty_dev, struct device *device)
+{
+	struct gdm *gdm;
+	int i;
+	int j;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++) {
+		gdm = kmalloc(sizeof(*gdm), GFP_KERNEL);
+		if (!gdm)
+			return -ENOMEM;
+
+		mutex_lock(&gdm_table_lock);
+		for (j = 0; j < GDM_TTY_MINOR; j++) {
+			if (!gdm_table[i][j])
+				break;
+		}
+
+		if (j == GDM_TTY_MINOR) {
+			kfree(gdm);
+			mutex_unlock(&gdm_table_lock);
+			return -EINVAL;
+		}
+
+		gdm_table[i][j] = gdm;
+		mutex_unlock(&gdm_table_lock);
+
+		tty_dev->gdm[i] = gdm;
+		tty_port_init(&gdm->port);
+
+		gdm->port.ops = &gdm_port_ops;
+		gdm->index = i;
+		gdm->minor = j;
+		gdm->tty_dev = tty_dev;
+
+		tty_port_register_device(&gdm->port, gdm_driver[i],
+					 gdm->minor, device);
+	}
+
+	for (i = 0; i < MAX_ISSUE_NUM; i++)
+		gdm->tty_dev->recv_func(gdm->tty_dev->priv_dev,
+					gdm_tty_recv_complete);
+
+	return 0;
+}
+
+void unregister_lte_tty_device(struct tty_dev *tty_dev)
+{
+	struct gdm *gdm;
+	struct tty_struct *tty;
+	int i;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++) {
+		gdm = tty_dev->gdm[i];
+		if (!gdm)
+			continue;
+
+		mutex_lock(&gdm_table_lock);
+		gdm_table[gdm->index][gdm->minor] = NULL;
+		mutex_unlock(&gdm_table_lock);
+
+		tty = tty_port_tty_get(&gdm->port);
+		if (tty) {
+			tty_vhangup(tty);
+			tty_kref_put(tty);
+		}
+
+		tty_unregister_device(gdm_driver[i], gdm->minor);
+		tty_port_put(&gdm->port);
+	}
+}
+
+static const struct tty_operations gdm_tty_ops = {
+	.install =	gdm_tty_install,
+	.open =		gdm_tty_open,
+	.close =	gdm_tty_close,
+	.cleanup =	gdm_tty_cleanup,
+	.hangup =	gdm_tty_hangup,
+	.write =	gdm_tty_write,
+	.write_room =	gdm_tty_write_room,
+};
+
+int register_lte_tty_driver(void)
+{
+	struct tty_driver *tty_driver;
+	int i;
+	int ret;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++) {
+		tty_driver = tty_alloc_driver(GDM_TTY_MINOR,
+				TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV);
+		if (IS_ERR(tty_driver))
+			return PTR_ERR(tty_driver);
+
+		tty_driver->owner = THIS_MODULE;
+		tty_driver->driver_name = DRIVER_STRING[i];
+		tty_driver->name = DEVICE_STRING[i];
+		tty_driver->major = GDM_TTY_MAJOR;
+		tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+		tty_driver->subtype = SERIAL_TYPE_NORMAL;
+		tty_driver->init_termios = tty_std_termios;
+		tty_driver->init_termios.c_cflag = B9600 | CS8 | HUPCL | CLOCAL;
+		tty_driver->init_termios.c_lflag = ISIG | ICANON | IEXTEN;
+		tty_set_operations(tty_driver, &gdm_tty_ops);
+
+		ret = tty_register_driver(tty_driver);
+		if (ret) {
+			tty_driver_kref_put(tty_driver);
+			return ret;
+		}
+
+		gdm_driver[i] = tty_driver;
+	}
+
+	return ret;
+}
+
+void unregister_lte_tty_driver(void)
+{
+	struct tty_driver *tty_driver;
+	int i;
+
+	for (i = 0; i < TTY_MAX_COUNT; i++) {
+		tty_driver = gdm_driver[i];
+		if (tty_driver) {
+			tty_unregister_driver(tty_driver);
+			tty_driver_kref_put(tty_driver);
+		}
+	}
+}
+
diff --git a/drivers/staging/gdm724x/gdm_tty.h b/drivers/staging/gdm724x/gdm_tty.h
new file mode 100644
index 000000000..afec97ced
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_tty.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _GDM_TTY_H_
+#define _GDM_TTY_H_
+
+#include <linux/types.h>
+#include <linux/tty.h>
+
+#define TTY_MAX_COUNT		2
+
+#define MAX_ISSUE_NUM 3
+
+enum TO_HOST_RESULT {
+	TO_HOST_BUFFER_REQUEST_FAIL = 1,
+	TO_HOST_PORT_CLOSE = 2,
+	TO_HOST_INVALID_PACKET = 3,
+};
+
+enum RECV_PACKET_PROCESS {
+	RECV_PACKET_PROCESS_COMPLETE = 0,
+	RECV_PACKET_PROCESS_CONTINUE = 1,
+};
+
+struct gdm {
+	struct tty_dev *tty_dev;
+	struct tty_port port;
+	unsigned int index;
+	unsigned int minor;
+};
+
+struct tty_dev {
+	void *priv_dev;
+	int (*send_func)(void *priv_dev,
+			 void *data,
+			 int len,
+			 int tty_index,
+			 void (*cb)(void *cb_data),
+			 void *cb_data);
+	int (*recv_func)(void *priv_dev,
+			 int (*cb)(void *data,
+				   int len,
+				   int tty_index,
+				   struct tty_dev *tty_dev,
+				   int complete));
+	int (*send_control)(void *priv_dev,
+			    int request,
+			    int value,
+			    void *data,
+			    int len);
+	struct gdm *gdm[2];
+};
+
+int register_lte_tty_driver(void);
+void unregister_lte_tty_driver(void);
+int register_lte_tty_device(struct tty_dev *tty_dev, struct device *dev);
+void unregister_lte_tty_device(struct tty_dev *tty_dev);
+
+#endif /* _GDM_USB_H_ */
+
diff --git a/drivers/staging/gdm724x/gdm_usb.c b/drivers/staging/gdm724x/gdm_usb.c
new file mode 100644
index 000000000..54bdb64f5
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_usb.c
@@ -0,0 +1,1012 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <linux/sched.h>
+#include <linux/kthread.h>
+#include <linux/usb/cdc.h>
+#include <linux/wait.h>
+#include <linux/if_ether.h>
+#include <linux/pm_runtime.h>
+
+#include "gdm_usb.h"
+#include "gdm_lte.h"
+#include "hci.h"
+#include "hci_packet.h"
+#include "gdm_endian.h"
+
+#define USB_DEVICE_CDC_DATA(vid, pid) \
+	.match_flags = USB_DEVICE_ID_MATCH_DEVICE | \
+		USB_DEVICE_ID_MATCH_INT_CLASS | \
+		USB_DEVICE_ID_MATCH_INT_SUBCLASS,\
+	.idVendor = vid,\
+	.idProduct = pid,\
+	.bInterfaceClass = USB_CLASS_COMM,\
+	.bInterfaceSubClass = USB_CDC_SUBCLASS_ETHERNET
+
+#define USB_DEVICE_MASS_DATA(vid, pid) \
+	.match_flags = USB_DEVICE_ID_MATCH_DEVICE | \
+		USB_DEVICE_ID_MATCH_INT_INFO,\
+	.idVendor = vid,\
+	.idProduct = pid,\
+	.bInterfaceSubClass = USB_SC_SCSI, \
+	.bInterfaceClass = USB_CLASS_MASS_STORAGE,\
+	.bInterfaceProtocol = USB_PR_BULK
+
+static const struct usb_device_id id_table[] = {
+	{ USB_DEVICE_CDC_DATA(VID_GCT, PID_GDM7240) }, /* GCT GDM7240 */
+	{ USB_DEVICE_CDC_DATA(VID_GCT, PID_GDM7243) }, /* GCT GDM7243 */
+	{ }
+};
+
+MODULE_DEVICE_TABLE(usb, id_table);
+
+static void do_tx(struct work_struct *work);
+static void do_rx(struct work_struct *work);
+
+static int gdm_usb_recv(void *priv_dev,
+			int (*cb)(void *cb_data,
+				  void *data, int len, int context),
+			void *cb_data,
+			int context);
+
+static int request_mac_address(struct lte_udev *udev)
+{
+	struct hci_packet *hci;
+	struct usb_device *usbdev = udev->usbdev;
+	int actual;
+	int ret = -1;
+
+	hci = kmalloc(struct_size(hci, data, 1), GFP_KERNEL);
+	if (!hci)
+		return -ENOMEM;
+
+	hci->cmd_evt = gdm_cpu_to_dev16(udev->gdm_ed, LTE_GET_INFORMATION);
+	hci->len = gdm_cpu_to_dev16(udev->gdm_ed, 1);
+	hci->data[0] = MAC_ADDRESS;
+
+	ret = usb_bulk_msg(usbdev, usb_sndbulkpipe(usbdev, 2), hci, 5,
+			   &actual, 1000);
+
+	udev->request_mac_addr = 1;
+	kfree(hci);
+
+	return ret;
+}
+
+static struct usb_tx *alloc_tx_struct(int len)
+{
+	struct usb_tx *t = NULL;
+	int ret = 0;
+
+	t = kzalloc(sizeof(*t), GFP_ATOMIC);
+	if (!t) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	t->urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!(len % 512))
+		len++;
+
+	t->buf = kmalloc(len, GFP_ATOMIC);
+	if (!t->urb || !t->buf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+out:
+	if (ret < 0) {
+		if (t) {
+			usb_free_urb(t->urb);
+			kfree(t->buf);
+			kfree(t);
+		}
+		return NULL;
+	}
+
+	return t;
+}
+
+static struct usb_tx_sdu *alloc_tx_sdu_struct(void)
+{
+	struct usb_tx_sdu *t_sdu;
+
+	t_sdu = kzalloc(sizeof(*t_sdu), GFP_KERNEL);
+	if (!t_sdu)
+		return NULL;
+
+	t_sdu->buf = kmalloc(SDU_BUF_SIZE, GFP_KERNEL);
+	if (!t_sdu->buf) {
+		kfree(t_sdu);
+		return NULL;
+	}
+
+	return t_sdu;
+}
+
+static void free_tx_struct(struct usb_tx *t)
+{
+	if (t) {
+		usb_free_urb(t->urb);
+		kfree(t->buf);
+		kfree(t);
+	}
+}
+
+static void free_tx_sdu_struct(struct usb_tx_sdu *t_sdu)
+{
+	if (t_sdu) {
+		kfree(t_sdu->buf);
+		kfree(t_sdu);
+	}
+}
+
+static struct usb_tx_sdu *get_tx_sdu_struct(struct tx_cxt *tx, int *no_spc)
+{
+	struct usb_tx_sdu *t_sdu;
+
+	if (list_empty(&tx->free_list))
+		return NULL;
+
+	t_sdu = list_entry(tx->free_list.next, struct usb_tx_sdu, list);
+	list_del(&t_sdu->list);
+
+	tx->avail_count--;
+
+	*no_spc = list_empty(&tx->free_list) ? 1 : 0;
+
+	return t_sdu;
+}
+
+static void put_tx_struct(struct tx_cxt *tx, struct usb_tx_sdu *t_sdu)
+{
+	list_add_tail(&t_sdu->list, &tx->free_list);
+	tx->avail_count++;
+}
+
+static struct usb_rx *alloc_rx_struct(void)
+{
+	struct usb_rx *r = NULL;
+	int ret = 0;
+
+	r = kmalloc(sizeof(*r), GFP_KERNEL);
+	if (!r) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	r->urb = usb_alloc_urb(0, GFP_KERNEL);
+	r->buf = kmalloc(RX_BUF_SIZE, GFP_KERNEL);
+	if (!r->urb || !r->buf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+out:
+
+	if (ret < 0) {
+		if (r) {
+			usb_free_urb(r->urb);
+			kfree(r->buf);
+			kfree(r);
+		}
+		return NULL;
+	}
+
+	return r;
+}
+
+static void free_rx_struct(struct usb_rx *r)
+{
+	if (r) {
+		usb_free_urb(r->urb);
+		kfree(r->buf);
+		kfree(r);
+	}
+}
+
+static struct usb_rx *get_rx_struct(struct rx_cxt *rx, int *no_spc)
+{
+	struct usb_rx *r;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rx->rx_lock, flags);
+
+	if (list_empty(&rx->free_list)) {
+		spin_unlock_irqrestore(&rx->rx_lock, flags);
+		return NULL;
+	}
+
+	r = list_entry(rx->free_list.next, struct usb_rx, free_list);
+	list_del(&r->free_list);
+
+	rx->avail_count--;
+
+	*no_spc = list_empty(&rx->free_list) ? 1 : 0;
+
+	spin_unlock_irqrestore(&rx->rx_lock, flags);
+
+	return r;
+}
+
+static void put_rx_struct(struct rx_cxt *rx, struct usb_rx *r)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&rx->rx_lock, flags);
+
+	list_add_tail(&r->free_list, &rx->free_list);
+	rx->avail_count++;
+
+	spin_unlock_irqrestore(&rx->rx_lock, flags);
+}
+
+static void release_usb(struct lte_udev *udev)
+{
+	struct rx_cxt	*rx = &udev->rx;
+	struct tx_cxt	*tx = &udev->tx;
+	struct usb_tx	*t, *t_next;
+	struct usb_rx	*r, *r_next;
+	struct usb_tx_sdu	*t_sdu, *t_sdu_next;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	list_for_each_entry_safe(t_sdu, t_sdu_next, &tx->sdu_list, list) {
+		list_del(&t_sdu->list);
+		free_tx_sdu_struct(t_sdu);
+	}
+
+	list_for_each_entry_safe(t, t_next, &tx->hci_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(t_sdu, t_sdu_next, &tx->free_list, list) {
+		list_del(&t_sdu->list);
+		free_tx_sdu_struct(t_sdu);
+	}
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	spin_lock_irqsave(&rx->submit_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->rx_submit_list,
+				 rx_submit_list) {
+		spin_unlock_irqrestore(&rx->submit_lock, flags);
+		usb_kill_urb(r->urb);
+		spin_lock_irqsave(&rx->submit_lock, flags);
+	}
+	spin_unlock_irqrestore(&rx->submit_lock, flags);
+
+	spin_lock_irqsave(&rx->rx_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->free_list, free_list) {
+		list_del(&r->free_list);
+		free_rx_struct(r);
+	}
+	spin_unlock_irqrestore(&rx->rx_lock, flags);
+
+	spin_lock_irqsave(&rx->to_host_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->to_host_list, to_host_list) {
+		if (r->index == (void *)udev) {
+			list_del(&r->to_host_list);
+			free_rx_struct(r);
+		}
+	}
+	spin_unlock_irqrestore(&rx->to_host_lock, flags);
+}
+
+static int init_usb(struct lte_udev *udev)
+{
+	int ret = 0;
+	int i;
+	struct tx_cxt *tx = &udev->tx;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_tx_sdu *t_sdu = NULL;
+	struct usb_rx *r = NULL;
+
+	udev->send_complete = 1;
+	udev->tx_stop = 0;
+	udev->request_mac_addr = 0;
+	udev->usb_state = PM_NORMAL;
+
+	INIT_LIST_HEAD(&tx->sdu_list);
+	INIT_LIST_HEAD(&tx->hci_list);
+	INIT_LIST_HEAD(&tx->free_list);
+	INIT_LIST_HEAD(&rx->rx_submit_list);
+	INIT_LIST_HEAD(&rx->free_list);
+	INIT_LIST_HEAD(&rx->to_host_list);
+	spin_lock_init(&tx->lock);
+	spin_lock_init(&rx->rx_lock);
+	spin_lock_init(&rx->submit_lock);
+	spin_lock_init(&rx->to_host_lock);
+
+	tx->avail_count = 0;
+	rx->avail_count = 0;
+
+	udev->rx_cb = NULL;
+
+	for (i = 0; i < MAX_NUM_SDU_BUF; i++) {
+		t_sdu = alloc_tx_sdu_struct();
+		if (!t_sdu) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		list_add(&t_sdu->list, &tx->free_list);
+		tx->avail_count++;
+	}
+
+	for (i = 0; i < MAX_RX_SUBMIT_COUNT * 2; i++) {
+		r = alloc_rx_struct();
+		if (!r) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		list_add(&r->free_list, &rx->free_list);
+		rx->avail_count++;
+	}
+	INIT_DELAYED_WORK(&udev->work_tx, do_tx);
+	INIT_DELAYED_WORK(&udev->work_rx, do_rx);
+	return 0;
+fail:
+	release_usb(udev);
+	return ret;
+}
+
+static int set_mac_address(u8 *data, void *arg)
+{
+	struct phy_dev *phy_dev = arg;
+	struct lte_udev *udev = phy_dev->priv_dev;
+	struct tlv *tlv = (struct tlv *)data;
+	u8 mac_address[ETH_ALEN] = {0, };
+
+	if (tlv->type == MAC_ADDRESS && udev->request_mac_addr) {
+		memcpy(mac_address, tlv->data, tlv->len);
+
+		if (register_lte_device(phy_dev,
+					&udev->intf->dev, mac_address) < 0)
+			pr_err("register lte device failed\n");
+
+		udev->request_mac_addr = 0;
+
+		return 1;
+	}
+
+	return 0;
+}
+
+static void do_rx(struct work_struct *work)
+{
+	struct lte_udev *udev =
+		container_of(work, struct lte_udev, work_rx.work);
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_rx *r;
+	struct hci_packet *hci;
+	struct phy_dev *phy_dev;
+	u16 cmd_evt;
+	int ret;
+	unsigned long flags;
+
+	while (1) {
+		spin_lock_irqsave(&rx->to_host_lock, flags);
+		if (list_empty(&rx->to_host_list)) {
+			spin_unlock_irqrestore(&rx->to_host_lock, flags);
+			break;
+		}
+		r = list_entry(rx->to_host_list.next,
+			       struct usb_rx, to_host_list);
+		list_del(&r->to_host_list);
+		spin_unlock_irqrestore(&rx->to_host_lock, flags);
+
+		phy_dev = r->cb_data;
+		udev = phy_dev->priv_dev;
+		hci = (struct hci_packet *)r->buf;
+		cmd_evt = gdm_dev16_to_cpu(udev->gdm_ed, hci->cmd_evt);
+
+		switch (cmd_evt) {
+		case LTE_GET_INFORMATION_RESULT:
+			if (set_mac_address(hci->data, r->cb_data) == 0) {
+				r->callback(r->cb_data,
+					    r->buf,
+					    r->urb->actual_length,
+					    KERNEL_THREAD);
+			}
+			break;
+
+		default:
+			if (r->callback) {
+				ret = r->callback(r->cb_data,
+						  r->buf,
+						  r->urb->actual_length,
+						  KERNEL_THREAD);
+
+				if (ret == -EAGAIN)
+					pr_err("failed to send received data\n");
+			}
+			break;
+		}
+
+		put_rx_struct(rx, r);
+
+		gdm_usb_recv(udev,
+			     r->callback,
+			     r->cb_data,
+			     USB_COMPLETE);
+	}
+}
+
+static void remove_rx_submit_list(struct usb_rx *r, struct rx_cxt *rx)
+{
+	unsigned long flags;
+	struct usb_rx	*r_remove, *r_remove_next;
+
+	spin_lock_irqsave(&rx->submit_lock, flags);
+	list_for_each_entry_safe(r_remove, r_remove_next,
+				 &rx->rx_submit_list, rx_submit_list) {
+		if (r == r_remove) {
+			list_del(&r->rx_submit_list);
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&rx->submit_lock, flags);
+}
+
+static void gdm_usb_rcv_complete(struct urb *urb)
+{
+	struct usb_rx *r = urb->context;
+	struct rx_cxt *rx = r->rx;
+	unsigned long flags;
+	struct lte_udev *udev = container_of(r->rx, struct lte_udev, rx);
+	struct usb_device *usbdev = udev->usbdev;
+
+	remove_rx_submit_list(r, rx);
+
+	if (!urb->status && r->callback) {
+		spin_lock_irqsave(&rx->to_host_lock, flags);
+		list_add_tail(&r->to_host_list, &rx->to_host_list);
+		schedule_work(&udev->work_rx.work);
+		spin_unlock_irqrestore(&rx->to_host_lock, flags);
+	} else {
+		if (urb->status && udev->usb_state == PM_NORMAL)
+			dev_err(&urb->dev->dev, "%s: urb status error %d\n",
+				__func__, urb->status);
+
+		put_rx_struct(rx, r);
+	}
+
+	usb_mark_last_busy(usbdev);
+}
+
+static int gdm_usb_recv(void *priv_dev,
+			int (*cb)(void *cb_data,
+				  void *data, int len, int context),
+			void *cb_data,
+			int context)
+{
+	struct lte_udev *udev = priv_dev;
+	struct usb_device *usbdev = udev->usbdev;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_rx *r;
+	int no_spc;
+	int ret;
+	unsigned long flags;
+
+	if (!udev->usbdev) {
+		pr_err("invalid device\n");
+		return -ENODEV;
+	}
+
+	r = get_rx_struct(rx, &no_spc);
+	if (!r) {
+		pr_err("Out of Memory\n");
+		return -ENOMEM;
+	}
+
+	udev->rx_cb = cb;
+	r->callback = cb;
+	r->cb_data = cb_data;
+	r->index = (void *)udev;
+	r->rx = rx;
+
+	usb_fill_bulk_urb(r->urb,
+			  usbdev,
+			  usb_rcvbulkpipe(usbdev, 0x83),
+			  r->buf,
+			  RX_BUF_SIZE,
+			  gdm_usb_rcv_complete,
+			  r);
+
+	spin_lock_irqsave(&rx->submit_lock, flags);
+	list_add_tail(&r->rx_submit_list, &rx->rx_submit_list);
+	spin_unlock_irqrestore(&rx->submit_lock, flags);
+
+	if (context == KERNEL_THREAD)
+		ret = usb_submit_urb(r->urb, GFP_KERNEL);
+	else
+		ret = usb_submit_urb(r->urb, GFP_ATOMIC);
+
+	if (ret) {
+		spin_lock_irqsave(&rx->submit_lock, flags);
+		list_del(&r->rx_submit_list);
+		spin_unlock_irqrestore(&rx->submit_lock, flags);
+
+		pr_err("usb_submit_urb failed (%p)\n", r);
+		put_rx_struct(rx, r);
+	}
+
+	return ret;
+}
+
+static void gdm_usb_send_complete(struct urb *urb)
+{
+	struct usb_tx *t = urb->context;
+	struct tx_cxt *tx = t->tx;
+	struct lte_udev *udev = container_of(tx, struct lte_udev, tx);
+	unsigned long flags;
+
+	if (urb->status == -ECONNRESET) {
+		dev_info(&urb->dev->dev, "CONNRESET\n");
+		return;
+	}
+
+	if (t->callback)
+		t->callback(t->cb_data);
+
+	free_tx_struct(t);
+
+	spin_lock_irqsave(&tx->lock, flags);
+	udev->send_complete = 1;
+	schedule_work(&udev->work_tx.work);
+	spin_unlock_irqrestore(&tx->lock, flags);
+}
+
+static int send_tx_packet(struct usb_device *usbdev, struct usb_tx *t, u32 len)
+{
+	int ret = 0;
+
+	if (!(len % 512))
+		len++;
+
+	usb_fill_bulk_urb(t->urb,
+			  usbdev,
+			  usb_sndbulkpipe(usbdev, 2),
+			  t->buf,
+			  len,
+			  gdm_usb_send_complete,
+			  t);
+
+	ret = usb_submit_urb(t->urb, GFP_ATOMIC);
+
+	if (ret)
+		dev_err(&usbdev->dev, "usb_submit_urb failed: %d\n",
+			ret);
+
+	usb_mark_last_busy(usbdev);
+
+	return ret;
+}
+
+static u32 packet_aggregation(struct lte_udev *udev, u8 *send_buf)
+{
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx_sdu *t_sdu = NULL;
+	struct multi_sdu *multi_sdu = (struct multi_sdu *)send_buf;
+	u16 send_len = 0;
+	u16 num_packet = 0;
+	unsigned long flags;
+
+	multi_sdu->cmd_evt = gdm_cpu_to_dev16(udev->gdm_ed, LTE_TX_MULTI_SDU);
+
+	while (num_packet < MAX_PACKET_IN_MULTI_SDU) {
+		spin_lock_irqsave(&tx->lock, flags);
+		if (list_empty(&tx->sdu_list)) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			break;
+		}
+
+		t_sdu = list_entry(tx->sdu_list.next, struct usb_tx_sdu, list);
+		if (send_len + t_sdu->len > MAX_SDU_SIZE) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			break;
+		}
+
+		list_del(&t_sdu->list);
+		spin_unlock_irqrestore(&tx->lock, flags);
+
+		memcpy(multi_sdu->data + send_len, t_sdu->buf, t_sdu->len);
+
+		send_len += (t_sdu->len + 3) & 0xfffc;
+		num_packet++;
+
+		if (tx->avail_count > 10)
+			t_sdu->callback(t_sdu->cb_data);
+
+		spin_lock_irqsave(&tx->lock, flags);
+		put_tx_struct(tx, t_sdu);
+		spin_unlock_irqrestore(&tx->lock, flags);
+	}
+
+	multi_sdu->len = gdm_cpu_to_dev16(udev->gdm_ed, send_len);
+	multi_sdu->num_packet = gdm_cpu_to_dev16(udev->gdm_ed, num_packet);
+
+	return send_len + offsetof(struct multi_sdu, data);
+}
+
+static void do_tx(struct work_struct *work)
+{
+	struct lte_udev *udev =
+		container_of(work, struct lte_udev, work_tx.work);
+	struct usb_device *usbdev = udev->usbdev;
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx *t = NULL;
+	int is_send = 0;
+	u32 len = 0;
+	unsigned long flags;
+
+	if (!usb_autopm_get_interface(udev->intf))
+		usb_autopm_put_interface(udev->intf);
+
+	if (udev->usb_state == PM_SUSPEND)
+		return;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	if (!udev->send_complete) {
+		spin_unlock_irqrestore(&tx->lock, flags);
+		return;
+	}
+	udev->send_complete = 0;
+
+	if (!list_empty(&tx->hci_list)) {
+		t = list_entry(tx->hci_list.next, struct usb_tx, list);
+		list_del(&t->list);
+		len = t->len;
+		t->is_sdu = 0;
+		is_send = 1;
+	} else if (!list_empty(&tx->sdu_list)) {
+		if (udev->tx_stop) {
+			udev->send_complete = 1;
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return;
+		}
+
+		t = alloc_tx_struct(TX_BUF_SIZE);
+		if (!t) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return;
+		}
+		t->callback = NULL;
+		t->tx = tx;
+		t->is_sdu = 1;
+		is_send = 1;
+	}
+
+	if (!is_send) {
+		udev->send_complete = 1;
+		spin_unlock_irqrestore(&tx->lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (t->is_sdu)
+		len = packet_aggregation(udev, t->buf);
+
+	if (send_tx_packet(usbdev, t, len)) {
+		pr_err("send_tx_packet failed\n");
+		t->callback = NULL;
+		gdm_usb_send_complete(t->urb);
+	}
+}
+
+#define SDU_PARAM_LEN 12
+static int gdm_usb_sdu_send(void *priv_dev, void *data, int len,
+			    unsigned int dft_eps_ID, unsigned int eps_ID,
+			    void (*cb)(void *data), void *cb_data,
+			    int dev_idx, int nic_type)
+{
+	struct lte_udev *udev = priv_dev;
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx_sdu *t_sdu;
+	struct sdu *sdu = NULL;
+	unsigned long flags;
+	int no_spc = 0;
+	u16 send_len;
+
+	if (!udev->usbdev) {
+		pr_err("sdu send - invalid device\n");
+		return TX_NO_DEV;
+	}
+
+	spin_lock_irqsave(&tx->lock, flags);
+	t_sdu = get_tx_sdu_struct(tx, &no_spc);
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (!t_sdu) {
+		pr_err("sdu send - free list empty\n");
+		return TX_NO_SPC;
+	}
+
+	sdu = (struct sdu *)t_sdu->buf;
+	sdu->cmd_evt = gdm_cpu_to_dev16(udev->gdm_ed, LTE_TX_SDU);
+	if (nic_type == NIC_TYPE_ARP) {
+		send_len = len + SDU_PARAM_LEN;
+		memcpy(sdu->data, data, len);
+	} else {
+		send_len = len - ETH_HLEN;
+		send_len += SDU_PARAM_LEN;
+		memcpy(sdu->data, data + ETH_HLEN, len - ETH_HLEN);
+	}
+
+	sdu->len = gdm_cpu_to_dev16(udev->gdm_ed, send_len);
+	sdu->dft_eps_ID = gdm_cpu_to_dev32(udev->gdm_ed, dft_eps_ID);
+	sdu->bearer_ID = gdm_cpu_to_dev32(udev->gdm_ed, eps_ID);
+	sdu->nic_type = gdm_cpu_to_dev32(udev->gdm_ed, nic_type);
+
+	t_sdu->len = send_len + HCI_HEADER_SIZE;
+	t_sdu->callback = cb;
+	t_sdu->cb_data = cb_data;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	list_add_tail(&t_sdu->list, &tx->sdu_list);
+	schedule_work(&udev->work_tx.work);
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (no_spc)
+		return TX_NO_BUFFER;
+
+	return 0;
+}
+
+static int gdm_usb_hci_send(void *priv_dev, void *data, int len,
+			    void (*cb)(void *data), void *cb_data)
+{
+	struct lte_udev *udev = priv_dev;
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx *t;
+	unsigned long flags;
+
+	if (!udev->usbdev) {
+		pr_err("hci send - invalid device\n");
+		return -ENODEV;
+	}
+
+	t = alloc_tx_struct(len);
+	if (!t) {
+		pr_err("hci_send - out of memory\n");
+		return -ENOMEM;
+	}
+
+	memcpy(t->buf, data, len);
+	t->callback = cb;
+	t->cb_data = cb_data;
+	t->len = len;
+	t->tx = tx;
+	t->is_sdu = 0;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	list_add_tail(&t->list, &tx->hci_list);
+	schedule_work(&udev->work_tx.work);
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	return 0;
+}
+
+static u8 gdm_usb_get_endian(void *priv_dev)
+{
+	struct lte_udev *udev = priv_dev;
+
+	return udev->gdm_ed;
+}
+
+static int gdm_usb_probe(struct usb_interface *intf,
+			 const struct usb_device_id *id)
+{
+	int ret = 0;
+	struct phy_dev *phy_dev = NULL;
+	struct lte_udev *udev = NULL;
+	u16 idVendor, idProduct;
+	int bInterfaceNumber;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+
+	bInterfaceNumber = intf->cur_altsetting->desc.bInterfaceNumber;
+	idVendor = __le16_to_cpu(usbdev->descriptor.idVendor);
+	idProduct = __le16_to_cpu(usbdev->descriptor.idProduct);
+
+	pr_info("net vid = 0x%04x pid = 0x%04x\n", idVendor, idProduct);
+
+	if (bInterfaceNumber > NETWORK_INTERFACE) {
+		pr_info("not a network device\n");
+		return -ENODEV;
+	}
+
+	phy_dev = kzalloc(sizeof(*phy_dev), GFP_KERNEL);
+	if (!phy_dev)
+		return -ENOMEM;
+
+	udev = kzalloc(sizeof(*udev), GFP_KERNEL);
+	if (!udev) {
+		ret = -ENOMEM;
+		goto err_udev;
+	}
+
+	phy_dev->priv_dev = (void *)udev;
+	phy_dev->send_hci_func = gdm_usb_hci_send;
+	phy_dev->send_sdu_func = gdm_usb_sdu_send;
+	phy_dev->rcv_func = gdm_usb_recv;
+	phy_dev->get_endian = gdm_usb_get_endian;
+
+	udev->usbdev = usbdev;
+	ret = init_usb(udev);
+	if (ret < 0) {
+		dev_err(intf->usb_dev, "init_usb func failed\n");
+		goto err_init_usb;
+	}
+	udev->intf = intf;
+
+	intf->needs_remote_wakeup = 1;
+	usb_enable_autosuspend(usbdev);
+	pm_runtime_set_autosuspend_delay(&usbdev->dev, AUTO_SUSPEND_TIMER);
+
+	/* List up hosts with big endians, otherwise,
+	 * defaults to little endian
+	 */
+	if (idProduct == PID_GDM7243)
+		udev->gdm_ed = ENDIANNESS_BIG;
+	else
+		udev->gdm_ed = ENDIANNESS_LITTLE;
+
+	ret = request_mac_address(udev);
+	if (ret < 0) {
+		dev_err(intf->usb_dev, "request Mac address failed\n");
+		goto err_mac_address;
+	}
+
+	start_rx_proc(phy_dev);
+	usb_get_dev(usbdev);
+	usb_set_intfdata(intf, phy_dev);
+
+	return 0;
+
+err_mac_address:
+	release_usb(udev);
+err_init_usb:
+	kfree(udev);
+err_udev:
+	kfree(phy_dev);
+
+	return ret;
+}
+
+static void gdm_usb_disconnect(struct usb_interface *intf)
+{
+	struct phy_dev *phy_dev;
+	struct lte_udev *udev;
+	struct usb_device *usbdev;
+
+	usbdev = interface_to_usbdev(intf);
+	phy_dev = usb_get_intfdata(intf);
+
+	udev = phy_dev->priv_dev;
+	unregister_lte_device(phy_dev);
+
+	release_usb(udev);
+
+	kfree(udev);
+	udev = NULL;
+
+	kfree(phy_dev);
+	phy_dev = NULL;
+
+	usb_put_dev(usbdev);
+}
+
+static int gdm_usb_suspend(struct usb_interface *intf, pm_message_t pm_msg)
+{
+	struct phy_dev *phy_dev;
+	struct lte_udev *udev;
+	struct rx_cxt *rx;
+	struct usb_rx *r;
+	struct usb_rx *r_next;
+	unsigned long flags;
+
+	phy_dev = usb_get_intfdata(intf);
+	udev = phy_dev->priv_dev;
+	rx = &udev->rx;
+	if (udev->usb_state != PM_NORMAL) {
+		dev_err(intf->usb_dev, "usb suspend - invalid state\n");
+		return -1;
+	}
+
+	udev->usb_state = PM_SUSPEND;
+
+	spin_lock_irqsave(&rx->submit_lock, flags);
+	list_for_each_entry_safe(r, r_next, &rx->rx_submit_list,
+				 rx_submit_list) {
+		spin_unlock_irqrestore(&rx->submit_lock, flags);
+		usb_kill_urb(r->urb);
+		spin_lock_irqsave(&rx->submit_lock, flags);
+	}
+	spin_unlock_irqrestore(&rx->submit_lock, flags);
+
+	cancel_work_sync(&udev->work_tx.work);
+	cancel_work_sync(&udev->work_rx.work);
+
+	return 0;
+}
+
+static int gdm_usb_resume(struct usb_interface *intf)
+{
+	struct phy_dev *phy_dev;
+	struct lte_udev *udev;
+	struct tx_cxt *tx;
+	struct rx_cxt *rx;
+	unsigned long flags;
+	int issue_count;
+	int i;
+
+	phy_dev = usb_get_intfdata(intf);
+	udev = phy_dev->priv_dev;
+	rx = &udev->rx;
+
+	if (udev->usb_state != PM_SUSPEND) {
+		dev_err(intf->usb_dev, "usb resume - invalid state\n");
+		return -1;
+	}
+	udev->usb_state = PM_NORMAL;
+
+	spin_lock_irqsave(&rx->rx_lock, flags);
+	issue_count = rx->avail_count - MAX_RX_SUBMIT_COUNT;
+	spin_unlock_irqrestore(&rx->rx_lock, flags);
+
+	if (issue_count >= 0) {
+		for (i = 0; i < issue_count; i++)
+			gdm_usb_recv(phy_dev->priv_dev,
+				     udev->rx_cb,
+				     phy_dev,
+				     USB_COMPLETE);
+	}
+
+	tx = &udev->tx;
+	spin_lock_irqsave(&tx->lock, flags);
+	schedule_work(&udev->work_tx.work);
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	return 0;
+}
+
+static struct usb_driver gdm_usb_lte_driver = {
+	.name = "gdm_lte",
+	.probe = gdm_usb_probe,
+	.disconnect = gdm_usb_disconnect,
+	.id_table = id_table,
+	.supports_autosuspend = 1,
+	.suspend = gdm_usb_suspend,
+	.resume = gdm_usb_resume,
+	.reset_resume = gdm_usb_resume,
+};
+
+static int __init gdm_usb_lte_init(void)
+{
+	if (gdm_lte_event_init() < 0) {
+		pr_err("error creating event\n");
+		return -1;
+	}
+
+	return usb_register(&gdm_usb_lte_driver);
+}
+
+static void __exit gdm_usb_lte_exit(void)
+{
+	gdm_lte_event_exit();
+
+	usb_deregister(&gdm_usb_lte_driver);
+}
+
+module_init(gdm_usb_lte_init);
+module_exit(gdm_usb_lte_exit);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_DESCRIPTION("GCT LTE USB Device Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/gdm724x/gdm_usb.h b/drivers/staging/gdm724x/gdm_usb.h
new file mode 100644
index 000000000..db689b091
--- /dev/null
+++ b/drivers/staging/gdm724x/gdm_usb.h
@@ -0,0 +1,99 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _GDM_USB_H_
+#define _GDM_USB_H_
+
+#include <linux/types.h>
+#include <linux/usb.h>
+#include <linux/list.h>
+#include <linux/time.h>
+
+#include "gdm_endian.h"
+#include "hci_packet.h"
+
+#define PM_NORMAL 0
+#define PM_SUSPEND 1
+#define AUTO_SUSPEND_TIMER 5000 /* ms */
+
+#define RX_BUF_SIZE		(1024 * 32)
+#define TX_BUF_SIZE		(1024 * 32)
+#define SDU_BUF_SIZE	2048
+#define MAX_SDU_SIZE	(1024 * 30)
+#define MAX_PACKET_IN_MULTI_SDU	256
+
+#define VID_GCT			0x1076
+#define PID_GDM7240		0x8000
+#define PID_GDM7243		0x9000
+
+#define NETWORK_INTERFACE 1
+#define USB_SC_SCSI 0x06
+#define USB_PR_BULK 0x50
+
+#define MAX_NUM_SDU_BUF	64
+
+struct usb_tx {
+	struct list_head list;
+	struct urb *urb;
+	u8 *buf;
+	u32 len;
+	void (*callback)(void *cb_data);
+	void *cb_data;
+	struct tx_cxt *tx;
+	u8 is_sdu;
+};
+
+struct usb_tx_sdu {
+	struct list_head list;
+	u8 *buf;
+	u32 len;
+	void (*callback)(void *cb_data);
+	void *cb_data;
+};
+
+struct usb_rx {
+	struct list_head to_host_list;
+	struct list_head free_list;
+	struct list_head rx_submit_list;
+	struct rx_cxt	*rx;
+	struct urb *urb;
+	u8 *buf;
+	int (*callback)(void *cb_data, void *data, int len, int context);
+	void *cb_data;
+	void *index;
+};
+
+struct tx_cxt {
+	struct list_head sdu_list;
+	struct list_head hci_list;
+	struct list_head free_list;
+	u32 avail_count;
+	spinlock_t lock;
+};
+
+struct rx_cxt {
+	struct list_head to_host_list;
+	struct list_head rx_submit_list;
+	struct list_head free_list;
+	u32	avail_count;
+	spinlock_t to_host_lock;
+	spinlock_t rx_lock;
+	spinlock_t submit_lock;
+};
+
+struct lte_udev {
+	struct usb_device *usbdev;
+	struct tx_cxt tx;
+	struct rx_cxt rx;
+	struct delayed_work work_tx;
+	struct delayed_work work_rx;
+	u8 gdm_ed;
+	u8 send_complete;
+	u8 tx_stop;
+	struct usb_interface *intf;
+	int (*rx_cb)(void *cb_data, void *data, int len, int context);
+	int usb_state;
+	u8 request_mac_addr;
+};
+
+#endif /* _GDM_USB_H_ */
diff --git a/drivers/staging/gdm724x/hci.h b/drivers/staging/gdm724x/hci.h
new file mode 100644
index 000000000..b30945daf
--- /dev/null
+++ b/drivers/staging/gdm724x/hci.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _HCI_H_
+#define _HCI_H_
+
+#define LTE_GET_INFORMATION		0x3002
+#define LTE_GET_INFORMATION_RESULT	0xB003
+	#define MAC_ADDRESS		0xA2
+
+#define LTE_LINK_ON_OFF_INDICATION	0xB133
+#define LTE_PDN_TABLE_IND		0xB143
+
+#define LTE_TX_SDU			0x3200
+#define LTE_RX_SDU			0xB201
+#define LTE_TX_MULTI_SDU		0x3202
+#define LTE_RX_MULTI_SDU		0xB203
+
+#define LTE_DL_SDU_FLOW_CONTROL		0x3305
+#define LTE_UL_SDU_FLOW_CONTROL		0xB306
+
+#define LTE_AT_CMD_TO_DEVICE		0x3307
+#define LTE_AT_CMD_FROM_DEVICE		0xB308
+
+#define LTE_SDIO_DM_SEND_PKT		0x3312
+#define LTE_SDIO_DM_RECV_PKT		0xB313
+
+#define LTE_NV_RESTORE_REQUEST		0xB30C
+#define LTE_NV_RESTORE_RESPONSE		0x330D
+#define LTE_NV_SAVE_REQUEST		0xB30E
+	#define NV_TYPE_LTE_INFO	0x00
+	#define NV_TYPE_BOARD_CONFIG	0x01
+	#define NV_TYPE_RF_CAL		0x02
+	#define NV_TYPE_TEMP		0x03
+	#define NV_TYPE_NET_INFO	0x04
+	#define NV_TYPE_SAFETY_INFO	0x05
+	#define NV_TYPE_CDMA_CAL	0x06
+	#define NV_TYPE_VENDOR		0x07
+	#define NV_TYPE_ALL		0xff
+#define LTE_NV_SAVE_RESPONSE		0x330F
+
+#define LTE_AT_CMD_TO_DEVICE_EXT	0x3323
+#define LTE_AT_CMD_FROM_DEVICE_EXT	0xB324
+
+#endif /* _HCI_H_ */
diff --git a/drivers/staging/gdm724x/hci_packet.h b/drivers/staging/gdm724x/hci_packet.h
new file mode 100644
index 000000000..3bb01e94f
--- /dev/null
+++ b/drivers/staging/gdm724x/hci_packet.h
@@ -0,0 +1,82 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _HCI_PACKET_H_
+#define _HCI_PACKET_H_
+
+#define HCI_HEADER_SIZE 4
+
+/*
+ * The NIC type definition:
+ * For backward compatibility, lower 16 bits used as they were.
+ * Lower 16 bit: NIC_TYPE values
+ * Uppoer 16 bit: NIC_TYPE Flags
+ */
+#define NIC_TYPE_NIC0		0x00000010
+#define NIC_TYPE_NIC1		0x00000011
+#define NIC_TYPE_NIC2		0x00000012
+#define NIC_TYPE_NIC3		0x00000013
+#define NIC_TYPE_ARP		0x00000100
+#define NIC_TYPE_ICMPV6		0x00000200
+#define NIC_TYPE_MASK		0x0000FFFF
+#define NIC_TYPE_F_IPV4		0x00010000
+#define NIC_TYPE_F_IPV6		0x00020000
+#define NIC_TYPE_F_DHCP		0x00040000
+#define NIC_TYPE_F_NDP		0x00080000
+#define NIC_TYPE_F_VLAN		0x00100000
+
+struct hci_packet {
+	__dev16 cmd_evt;
+	__dev16 len;
+	u8 data[];
+} __packed;
+
+struct tlv {
+	u8 type;
+	u8 len;
+	u8 *data[];
+} __packed;
+
+struct sdu_header {
+	__dev16 cmd_evt;
+	__dev16 len;
+	__dev32 dft_eps_id;
+	__dev32 bearer_ID;
+	__dev32 nic_type;
+} __packed;
+
+struct sdu {
+	__dev16 cmd_evt;
+	__dev16 len;
+	__dev32 dft_eps_ID;
+	__dev32 bearer_ID;
+	__dev32 nic_type;
+	u8 data[];
+} __packed;
+
+struct multi_sdu {
+	__dev16 cmd_evt;
+	__dev16 len;
+	__dev16 num_packet;
+	__dev16 reserved;
+	u8 data[];
+} __packed;
+
+struct hci_pdn_table_ind {
+	__dev16 cmd_evt;
+	__dev16 len;
+	u8 activate;
+	__dev32 dft_eps_id;
+	__dev32 nic_type;
+	u8 pdn_type;
+	u8 ipv4_addr[4];
+	u8 ipv6_intf_id[8];
+} __packed;
+
+struct hci_connect_ind {
+	__dev16 cmd_evt;
+	__dev16 len;
+	__dev32 connect;
+} __packed;
+
+#endif /* _HCI_PACKET_H_ */
diff --git a/drivers/staging/gdm724x/netlink_k.c b/drivers/staging/gdm724x/netlink_k.c
new file mode 100644
index 000000000..8f39cc561
--- /dev/null
+++ b/drivers/staging/gdm724x/netlink_k.c
@@ -0,0 +1,128 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/export.h>
+#include <linux/mutex.h>
+#include <linux/etherdevice.h>
+#include <linux/netlink.h>
+#include <asm/byteorder.h>
+#include <net/sock.h>
+
+#include "netlink_k.h"
+
+static DEFINE_MUTEX(netlink_mutex);
+
+#define ND_MAX_GROUP		30
+#define ND_IFINDEX_LEN		sizeof(int)
+#define ND_NLMSG_SPACE(len)	(NLMSG_SPACE(len) + ND_IFINDEX_LEN)
+#define ND_NLMSG_DATA(nlh)	((void *)((char *)NLMSG_DATA(nlh) + \
+						  ND_IFINDEX_LEN))
+#define ND_NLMSG_S_LEN(len)	((len) + ND_IFINDEX_LEN)
+#define ND_NLMSG_R_LEN(nlh)	((nlh)->nlmsg_len - ND_IFINDEX_LEN)
+#define ND_NLMSG_IFIDX(nlh)	NLMSG_DATA(nlh)
+#define ND_MAX_MSG_LEN		(1024 * 32)
+
+static void (*rcv_cb)(struct net_device *dev, u16 type, void *msg, int len);
+
+static void netlink_rcv_cb(struct sk_buff *skb)
+{
+	struct nlmsghdr	*nlh;
+	struct net_device *dev;
+	u32 mlen;
+	void *msg;
+	int ifindex;
+
+	if (!rcv_cb) {
+		pr_err("nl cb - unregistered\n");
+		return;
+	}
+
+	if (skb->len < NLMSG_HDRLEN) {
+		pr_err("nl cb - invalid skb length\n");
+		return;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+
+	if (skb->len < nlh->nlmsg_len || nlh->nlmsg_len > ND_MAX_MSG_LEN) {
+		pr_err("nl cb - invalid length (%d,%d)\n",
+		       skb->len, nlh->nlmsg_len);
+		return;
+	}
+
+	memcpy(&ifindex, ND_NLMSG_IFIDX(nlh), ND_IFINDEX_LEN);
+	msg = ND_NLMSG_DATA(nlh);
+	mlen = ND_NLMSG_R_LEN(nlh);
+
+	dev = dev_get_by_index(&init_net, ifindex);
+	if (dev) {
+		rcv_cb(dev, nlh->nlmsg_type, msg, mlen);
+		dev_put(dev);
+	} else {
+		pr_err("nl cb - dev (%d) not found\n", ifindex);
+	}
+}
+
+static void netlink_rcv(struct sk_buff *skb)
+{
+	mutex_lock(&netlink_mutex);
+	netlink_rcv_cb(skb);
+	mutex_unlock(&netlink_mutex);
+}
+
+struct sock *netlink_init(int unit,
+			  void (*cb)(struct net_device *dev, u16 type,
+				     void *msg, int len))
+{
+	struct sock *sock;
+	struct netlink_kernel_cfg cfg = {
+		.input  = netlink_rcv,
+	};
+
+	sock = netlink_kernel_create(&init_net, unit, &cfg);
+
+	if (sock)
+		rcv_cb = cb;
+
+	return sock;
+}
+
+int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len,
+		 struct net_device *dev)
+{
+	static u32 seq;
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh;
+	int ret = 0;
+
+	if (group > ND_MAX_GROUP)
+		return -EINVAL;
+
+	if (!netlink_has_listeners(sock, group + 1))
+		return -ESRCH;
+
+	skb = alloc_skb(NLMSG_SPACE(len), GFP_ATOMIC);
+	if (!skb)
+		return -ENOMEM;
+
+	seq++;
+
+	nlh = nlmsg_put(skb, 0, seq, type, len, 0);
+	memcpy(NLMSG_DATA(nlh), msg, len);
+	NETLINK_CB(skb).portid = 0;
+	NETLINK_CB(skb).dst_group = 0;
+
+	ret = netlink_broadcast(sock, skb, 0, group + 1, GFP_ATOMIC);
+	if (!ret)
+		return len;
+
+	if (ret != -ESRCH)
+		netdev_err(dev, "nl broadcast g=%d, t=%d, l=%d, r=%d\n",
+			   group, type, len, ret);
+	else if (netlink_has_listeners(sock, group + 1))
+		return -EAGAIN;
+
+	return ret;
+}
diff --git a/drivers/staging/gdm724x/netlink_k.h b/drivers/staging/gdm724x/netlink_k.h
new file mode 100644
index 000000000..d42eea9be
--- /dev/null
+++ b/drivers/staging/gdm724x/netlink_k.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
+
+#ifndef _NETLINK_K_H
+#define _NETLINK_K_H
+
+#include <linux/netdevice.h>
+#include <net/sock.h>
+
+struct sock *netlink_init(int unit,
+			  void (*cb)(struct net_device *dev,
+				     u16 type, void *msg, int len));
+int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len,
+		 struct net_device *dev);
+
+#endif /* _NETLINK_K_H_ */
diff --git a/drivers/staging/greybus/Documentation/firmware/authenticate.c b/drivers/staging/greybus/Documentation/firmware/authenticate.c
new file mode 100644
index 000000000..3d2c6f88a
--- /dev/null
+++ b/drivers/staging/greybus/Documentation/firmware/authenticate.c
@@ -0,0 +1,94 @@
+// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
+/*
+ * Sample code to test CAP protocol
+ *
+ * Copyright(c) 2016 Google Inc. All rights reserved.
+ * Copyright(c) 2016 Linaro Ltd. All rights reserved.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "../../greybus_authentication.h"
+
+struct cap_ioc_get_endpoint_uid uid;
+struct cap_ioc_get_ims_certificate cert = {
+	.certificate_class = 0,
+	.certificate_id = 0,
+};
+
+struct cap_ioc_authenticate authenticate = {
+	.auth_type = 0,
+	.challenge = {0},
+};
+
+int main(int argc, char *argv[])
+{
+	unsigned int timeout = 10000;
+	char *capdev;
+	int fd, ret;
+
+	/* Make sure arguments are correct */
+	if (argc != 2) {
+		printf("\nUsage: ./firmware <Path of the gb-cap-X dev>\n");
+		return 0;
+	}
+
+	capdev = argv[1];
+
+	printf("Opening %s authentication device\n", capdev);
+
+	fd = open(capdev, O_RDWR);
+	if (fd < 0) {
+		printf("Failed to open: %s\n", capdev);
+		return -1;
+	}
+
+	/* Get UID */
+	printf("Get UID\n");
+
+	ret = ioctl(fd, CAP_IOC_GET_ENDPOINT_UID, &uid);
+	if (ret < 0) {
+		printf("Failed to get UID: %s (%d)\n", capdev, ret);
+		ret = -1;
+		goto close_fd;
+	}
+
+	printf("UID received: 0x%llx\n", *(unsigned long long int *)(uid.uid));
+
+	/* Get certificate */
+	printf("Get IMS certificate\n");
+
+	ret = ioctl(fd, CAP_IOC_GET_IMS_CERTIFICATE, &cert);
+	if (ret < 0) {
+		printf("Failed to get IMS certificate: %s (%d)\n", capdev, ret);
+		ret = -1;
+		goto close_fd;
+	}
+
+	printf("IMS Certificate size: %d\n", cert.cert_size);
+
+	/* Authenticate */
+	printf("Authenticate module\n");
+
+	memcpy(authenticate.uid, uid.uid, 8);
+
+	ret = ioctl(fd, CAP_IOC_AUTHENTICATE, &authenticate);
+	if (ret < 0) {
+		printf("Failed to authenticate module: %s (%d)\n", capdev, ret);
+		ret = -1;
+		goto close_fd;
+	}
+
+	printf("Authenticated, result (%02x), sig-size (%02x)\n",
+		authenticate.result_code, authenticate.signature_size);
+
+close_fd:
+	close(fd);
+
+	return ret;
+}
diff --git a/drivers/staging/greybus/Documentation/firmware/firmware-management b/drivers/staging/greybus/Documentation/firmware/firmware-management
new file mode 100644
index 000000000..7918257e5
--- /dev/null
+++ b/drivers/staging/greybus/Documentation/firmware/firmware-management
@@ -0,0 +1,333 @@
+
+Firmware Management
+-------------------
+ Copyright 2016 Google Inc.
+ Copyright 2016 Linaro Ltd.
+
+Interface-Manifest
+------------------
+
+All firmware packages on the Modules or Interfaces are managed by a special
+Firmware Management Protocol. To support Firmware Management by the AP, the
+Interface Manifest shall at least contain the Firmware Management Bundle and a
+Firmware Management Protocol CPort within it.
+
+The bundle may contain additional CPorts based on the extra functionality
+required to manage firmware packages.
+
+For example, this is how the Firmware Management part of the Interface Manifest
+may look like:
+
+	; Firmware Management Bundle (Bundle 1):
+	[bundle-descriptor 1]
+	class = 0x16
+
+	; (Mandatory) Firmware Management Protocol on CPort 1
+	[cport-descriptor 2]
+	bundle = 1
+	protocol = 0x18
+
+	; (Optional) Firmware Download Protocol on CPort 2
+	[cport-descriptor 1]
+	bundle = 1
+	protocol = 0x17
+
+	; (Optional) SPI protocol on CPort 3
+	[cport-descriptor 3]
+	bundle = 1
+	protocol = 0x0b
+
+	; (Optional) Component Authentication Protocol (CAP) on CPort 4
+	[cport-descriptor 4]
+	bundle = 1
+	protocol = 0x19
+
+
+Sysfs Interfaces - Firmware Management
+--------------------------------------
+
+The Firmware Management Protocol interacts with Userspace using the character
+device interface. The character device will be present in /dev/ directory
+and will be named gb-fw-mgmt-<N>. The number <N> is assigned at runtime.
+
+Identifying the Character Device
+================================
+
+There can be multiple devices present in /dev/ directory with name gb-fw-mgmt-N
+and user first needs to identify the character device used for
+firmware-management for a particular interface.
+
+The Firmware Management core creates a device of class 'gb_fw_mgmt', which shall
+be used by the user to identify the right character device for it. The class
+device is created within the Bundle directory for a particular Interface.
+
+For example this is how the class-device can be present:
+
+/sys/bus/greybus/devices/1-1/1-1.1/1-1.1.1/gb_fw_mgmt/gb-fw-mgmt-0
+
+The last name in this path: gb-fw-mgmt-0 is precisely the name of the char
+device and so the device in this case will be:
+
+/dev/gb-fw-mgmt-0.
+
+Operations on the Char device
+=============================
+
+The Character device (gb-fw-mgmt-0 in example) can be opened by the userspace
+application and it can perform various 'ioctl' operations on the device. The
+device doesn't support any read/write operations.
+
+Following are the IOCTLs and their data structures available to the user:
+
+/* IOCTL support */
+#define GB_FW_LOAD_METHOD_UNIPRO		0x01
+#define GB_FW_LOAD_METHOD_INTERNAL		0x02
+
+#define GB_FW_LOAD_STATUS_FAILED		0x00
+#define GB_FW_LOAD_STATUS_UNVALIDATED		0x01
+#define GB_FW_LOAD_STATUS_VALIDATED		0x02
+#define GB_FW_LOAD_STATUS_VALIDATION_FAILED	0x03
+
+#define GB_FW_BACKEND_FW_STATUS_SUCCESS		0x01
+#define GB_FW_BACKEND_FW_STATUS_FAIL_FIND	0x02
+#define GB_FW_BACKEND_FW_STATUS_FAIL_FETCH	0x03
+#define GB_FW_BACKEND_FW_STATUS_FAIL_WRITE	0x04
+#define GB_FW_BACKEND_FW_STATUS_INT		0x05
+#define GB_FW_BACKEND_FW_STATUS_RETRY		0x06
+#define GB_FW_BACKEND_FW_STATUS_NOT_SUPPORTED	0x07
+
+#define GB_FW_BACKEND_VERSION_STATUS_SUCCESS		0x01
+#define GB_FW_BACKEND_VERSION_STATUS_NOT_AVAILABLE	0x02
+#define GB_FW_BACKEND_VERSION_STATUS_NOT_SUPPORTED	0x03
+#define GB_FW_BACKEND_VERSION_STATUS_RETRY		0x04
+#define GB_FW_BACKEND_VERSION_STATUS_FAIL_INT		0x05
+
+
+struct fw_mgmt_ioc_get_intf_version {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u16 major;
+	__u16 minor;
+} __attribute__ ((__packed__));
+
+struct fw_mgmt_ioc_get_backend_version {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u16 major;
+	__u16 minor;
+	__u8 status;
+} __attribute__ ((__packed__));
+
+struct fw_mgmt_ioc_intf_load_and_validate {
+	__u8			firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE];
+	__u8			load_method;
+	__u8			status;
+	__u16			major;
+	__u16			minor;
+} __packed;
+
+struct fw_mgmt_ioc_backend_fw_update {
+	__u8			firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE];
+	__u8			status;
+} __packed;
+
+#define FW_MGMT_IOCTL_BASE			'S'
+#define FW_MGMT_IOC_GET_INTF_FW			_IOR(FW_MGMT_IOCTL_BASE, 0, struct fw_mgmt_ioc_get_intf_version)
+#define FW_MGMT_IOC_GET_BACKEND_FW		_IOWR(FW_MGMT_IOCTL_BASE, 1, struct fw_mgmt_ioc_get_backend_version)
+#define FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE	_IOWR(FW_MGMT_IOCTL_BASE, 2, struct fw_mgmt_ioc_intf_load_and_validate)
+#define FW_MGMT_IOC_INTF_BACKEND_FW_UPDATE	_IOWR(FW_MGMT_IOCTL_BASE, 3, struct fw_mgmt_ioc_backend_fw_update)
+#define FW_MGMT_IOC_SET_TIMEOUT_MS		_IOW(FW_MGMT_IOCTL_BASE, 4, unsigned int)
+#define FW_MGMT_IOC_MODE_SWITCH			_IO(FW_MGMT_IOCTL_BASE, 5)
+
+1. FW_MGMT_IOC_GET_INTF_FW:
+
+   This ioctl shall be used by the user to get the version and firmware-tag of
+   the currently running Interface Firmware. All the fields of the 'struct
+   fw_mgmt_ioc_get_fw' are filled by the kernel.
+
+2. FW_MGMT_IOC_GET_BACKEND_FW:
+
+   This ioctl shall be used by the user to get the version of a currently
+   running Backend Interface Firmware identified by a firmware-tag. The user is
+   required to fill the 'firmware_tag' field of the 'struct fw_mgmt_ioc_get_fw'
+   in this case. The 'major' and 'minor' fields are set by the kernel in
+   response.
+
+3. FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE:
+
+   This ioctl shall be used by the user to load an Interface Firmware package on
+   an Interface. The user needs to fill the 'firmware_tag' and 'load_method'
+   fields of the 'struct fw_mgmt_ioc_intf_load_and_validate'. The 'status',
+   'major' and 'minor' fields are set by the kernel in response.
+
+4. FW_MGMT_IOC_INTF_BACKEND_FW_UPDATE:
+
+   This ioctl shall be used by the user to request an Interface to update a
+   Backend Interface Firmware.  The user is required to fill the 'firmware_tag'
+   field of the 'struct fw_mgmt_ioc_get_fw' in this case. The 'status' field is
+   set by the kernel in response.
+
+5. FW_MGMT_IOC_SET_TIMEOUT_MS:
+
+   This ioctl shall be used by the user to increase the timeout interval within
+   which the firmware must get loaded by the Module. The default timeout is 1
+   second. The user needs to pass the timeout in milliseconds.
+
+6. FW_MGMT_IOC_MODE_SWITCH:
+
+   This ioctl shall be used by the user to mode-switch the module to the
+   previously loaded interface firmware. If the interface firmware isn't loaded
+   previously, or if another unsuccessful FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE
+   operation is started after loading interface firmware, then the firmware core
+   wouldn't allow mode-switch.
+
+
+Sysfs Interfaces - Authentication
+---------------------------------
+
+The Component Authentication Protocol interacts with Userspace using the
+character device interface. The character device will be present in /dev/
+directory and will be named gb-authenticate-<N>. The number <N> is assigned at
+runtime.
+
+Identifying the Character Device
+================================
+
+There can be multiple devices present in /dev/ directory with name
+gb-authenticate-N and user first needs to identify the character device used for
+authentication a of particular interface.
+
+The Authentication core creates a device of class 'gb_authenticate', which shall
+be used by the user to identify the right character device for it. The class
+device is created within the Bundle directory for a particular Interface.
+
+For example this is how the class-device can be present:
+
+/sys/bus/greybus/devices/1-1/1-1.1/1-1.1.1/gb_authenticate/gb-authenticate-0
+
+The last name in this path: gb-authenticate-0 is precisely the name of the char
+device and so the device in this case will be:
+
+/dev/gb-authenticate-0.
+
+Operations on the Char device
+=============================
+
+The Character device (/dev/gb-authenticate-0 in above example) can be opened by
+the userspace application and it can perform various 'ioctl' operations on the
+device. The device doesn't support any read/write operations.
+
+Following are the IOCTLs and their data structures available to the user:
+
+#define CAP_CERTIFICATE_MAX_SIZE	1600
+#define CAP_SIGNATURE_MAX_SIZE		320
+
+/* Certificate class types */
+#define CAP_CERT_IMS_EAPC		0x00000001
+#define CAP_CERT_IMS_EASC		0x00000002
+#define CAP_CERT_IMS_EARC		0x00000003
+#define CAP_CERT_IMS_IAPC		0x00000004
+#define CAP_CERT_IMS_IASC		0x00000005
+#define CAP_CERT_IMS_IARC		0x00000006
+
+/* IMS Certificate response result codes */
+#define CAP_IMS_RESULT_CERT_FOUND	0x00
+#define CAP_IMS_RESULT_CERT_CLASS_INVAL	0x01
+#define CAP_IMS_RESULT_CERT_CORRUPT	0x02
+#define CAP_IMS_RESULT_CERT_NOT_FOUND	0x03
+
+/* Authentication types */
+#define CAP_AUTH_IMS_PRI		0x00000001
+#define CAP_AUTH_IMS_SEC		0x00000002
+#define CAP_AUTH_IMS_RSA		0x00000003
+
+/* Authenticate response result codes */
+#define CAP_AUTH_RESULT_CR_SUCCESS	0x00
+#define CAP_AUTH_RESULT_CR_BAD_TYPE	0x01
+#define CAP_AUTH_RESULT_CR_WRONG_EP	0x02
+#define CAP_AUTH_RESULT_CR_NO_KEY	0x03
+#define CAP_AUTH_RESULT_CR_SIG_FAIL	0x04
+
+
+/* IOCTL support */
+struct cap_ioc_get_endpoint_uid {
+	__u8			uid[8];
+} __attribute__ ((__packed__));
+
+struct cap_ioc_get_ims_certificate {
+	__u32			certificate_class;
+	__u32			certificate_id;
+
+	__u8			result_code;
+	__u32			cert_size;
+	__u8			certificate[CAP_CERTIFICATE_MAX_SIZE];
+} __attribute__ ((__packed__));
+
+struct cap_ioc_authenticate {
+	__u32			auth_type;
+	__u8			uid[8];
+	__u8			challenge[32];
+
+	__u8			result_code;
+	__u8			response[64];
+	__u32			signature_size;
+	__u8			signature[CAP_SIGNATURE_MAX_SIZE];
+} __attribute__ ((__packed__));
+
+#define CAP_IOCTL_BASE			'C'
+#define CAP_IOC_GET_ENDPOINT_UID	_IOR(CAP_IOCTL_BASE, 0, struct cap_ioc_get_endpoint_uid)
+#define CAP_IOC_GET_IMS_CERTIFICATE	_IOWR(CAP_IOCTL_BASE, 1, struct cap_ioc_get_ims_certificate)
+#define CAP_IOC_AUTHENTICATE		_IOWR(CAP_IOCTL_BASE, 2, struct cap_ioc_authenticate)
+
+
+1. CAP_IOC_GET_ENDPOINT_UID:
+
+   This ioctl shall be used by the user to get the endpoint UID associated with
+   the Interface.  All the fields of the 'struct cap_ioc_get_endpoint_uid' are
+   filled by the kernel.
+
+2. CAP_IOC_GET_IMS_CERTIFICATE:
+
+   This ioctl shall be used by the user to retrieve one of the available
+   cryptographic certificates held by the Interface for use in Component
+   Authentication. The user is required to fill the 'certificate_class' and
+   'certificate_id' field of the 'struct cap_ioc_get_ims_certificate' in this
+   case. The other fields will be set by the kernel in response. The first
+   'cert_size' bytes of the 'certificate' shall be read by the user and others
+   must be discarded.
+
+3. CAP_IOC_AUTHENTICATE:
+
+   This ioctl shall be used by the user to authenticate the Module attached to
+   an Interface.  The user needs to fill the 'auth_type', 'uid', and 'challenge'
+   fields of the 'struct cap_ioc_authenticate'. The other fields will be set by
+   the kernel in response.  The first 'signature_size' bytes of the 'signature'
+   shall be read by the user and others must be discarded.
+
+
+Sysfs Interfaces - Firmware Download
+------------------------------------
+
+The Firmware Download Protocol uses the existing Linux Kernel's Firmware class
+and the interface provided to userspace are described in:
+Documentation/firmware_class/.
+
+
+Sysfs Interfaces - SPI Flash
+----------------------------
+
+The SPI flash is exposed in userspace as a MTD device and is created
+within the Bundle directory. For example, this is how the path may look like:
+
+$ ls /sys/bus/greybus/devices/1-1/1-1.1/1-1.1.1/spi_master/spi32766/spi32766.0/mtd
+mtd0    mtd0ro
+
+
+Sample Applications
+-------------------
+
+The current directory also provides a firmware.c test application, which can be
+referenced while developing userspace application to talk to firmware-management
+protocol.
+
+The current directory also provides a authenticate.c test application, which can
+be referenced while developing userspace application to talk to
+component authentication protocol.
diff --git a/drivers/staging/greybus/Documentation/firmware/firmware.c b/drivers/staging/greybus/Documentation/firmware/firmware.c
new file mode 100644
index 000000000..765d69faa
--- /dev/null
+++ b/drivers/staging/greybus/Documentation/firmware/firmware.c
@@ -0,0 +1,218 @@
+// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
+/*
+ * Sample code to test firmware-management protocol
+ *
+ * Copyright(c) 2016 Google Inc. All rights reserved.
+ * Copyright(c) 2016 Linaro Ltd. All rights reserved.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "../../greybus_firmware.h"
+
+#define FW_DEV_DEFAULT		"/dev/gb-fw-mgmt-0"
+#define FW_TAG_INT_DEFAULT	"s3f"
+#define FW_TAG_BCND_DEFAULT	"bf_01"
+#define FW_UPDATE_TYPE_DEFAULT	0
+#define FW_TIMEOUT_DEFAULT	10000
+
+static const char *firmware_tag;
+static const char *fwdev = FW_DEV_DEFAULT;
+static unsigned int fw_update_type = FW_UPDATE_TYPE_DEFAULT;
+static unsigned int fw_timeout = FW_TIMEOUT_DEFAULT;
+
+static struct fw_mgmt_ioc_get_intf_version intf_fw_info;
+static struct fw_mgmt_ioc_get_backend_version backend_fw_info;
+static struct fw_mgmt_ioc_intf_load_and_validate intf_load;
+static struct fw_mgmt_ioc_backend_fw_update backend_update;
+
+static void usage(void)
+{
+	printf("\nUsage: ./firmware <gb-fw-mgmt-X (default: gb-fw-mgmt-0)> <interface: 0, backend: 1 (default: 0)> <firmware-tag> (default: \"s3f\"/\"bf_01\") <timeout (default: 10000 ms)>\n");
+}
+
+static int update_intf_firmware(int fd)
+{
+	int ret;
+
+	/* Get Interface Firmware Version */
+	printf("Get Interface Firmware Version\n");
+
+	ret = ioctl(fd, FW_MGMT_IOC_GET_INTF_FW, &intf_fw_info);
+	if (ret < 0) {
+		printf("Failed to get interface firmware version: %s (%d)\n",
+			fwdev, ret);
+		return -1;
+	}
+
+	printf("Interface Firmware tag (%s), major (%d), minor (%d)\n",
+		intf_fw_info.firmware_tag, intf_fw_info.major,
+		intf_fw_info.minor);
+
+	/* Try Interface Firmware load over Unipro */
+	printf("Loading Interface Firmware\n");
+
+	intf_load.load_method = GB_FW_U_LOAD_METHOD_UNIPRO;
+	intf_load.status = 0;
+	intf_load.major = 0;
+	intf_load.minor = 0;
+
+	strncpy((char *)&intf_load.firmware_tag, firmware_tag,
+		GB_FIRMWARE_U_TAG_MAX_SIZE);
+
+	ret = ioctl(fd, FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE, &intf_load);
+	if (ret < 0) {
+		printf("Failed to load interface firmware: %s (%d)\n", fwdev,
+			ret);
+		return -1;
+	}
+
+	if (intf_load.status != GB_FW_U_LOAD_STATUS_VALIDATED &&
+	    intf_load.status != GB_FW_U_LOAD_STATUS_UNVALIDATED) {
+		printf("Load status says loading failed: %d\n",
+			intf_load.status);
+		return -1;
+	}
+
+	printf("Interface Firmware (%s) Load done: major: %d, minor: %d, status: %d\n",
+		firmware_tag, intf_load.major, intf_load.minor,
+		intf_load.status);
+
+	/* Initiate Mode-switch to the newly loaded firmware */
+	printf("Initiate Mode switch\n");
+
+	ret = ioctl(fd, FW_MGMT_IOC_MODE_SWITCH);
+	if (ret < 0)
+		printf("Failed to initiate mode-switch (%d)\n", ret);
+
+	return ret;
+}
+
+static int update_backend_firmware(int fd)
+{
+	int ret;
+
+	/* Get Backend Firmware Version */
+	printf("Getting Backend Firmware Version\n");
+
+	strncpy((char *)&backend_fw_info.firmware_tag, firmware_tag,
+		GB_FIRMWARE_U_TAG_MAX_SIZE);
+
+retry_fw_version:
+	ret = ioctl(fd, FW_MGMT_IOC_GET_BACKEND_FW, &backend_fw_info);
+	if (ret < 0) {
+		printf("Failed to get backend firmware version: %s (%d)\n",
+			fwdev, ret);
+		return -1;
+	}
+
+	printf("Backend Firmware tag (%s), major (%d), minor (%d), status (%d)\n",
+		backend_fw_info.firmware_tag, backend_fw_info.major,
+		backend_fw_info.minor, backend_fw_info.status);
+
+	if (backend_fw_info.status == GB_FW_U_BACKEND_VERSION_STATUS_RETRY)
+		goto retry_fw_version;
+
+	if ((backend_fw_info.status != GB_FW_U_BACKEND_VERSION_STATUS_SUCCESS)
+	    && (backend_fw_info.status != GB_FW_U_BACKEND_VERSION_STATUS_NOT_AVAILABLE)) {
+		printf("Failed to get backend firmware version: %s (%d)\n",
+			fwdev, backend_fw_info.status);
+		return -1;
+	}
+
+	/* Try Backend Firmware Update over Unipro */
+	printf("Updating Backend Firmware\n");
+
+	strncpy((char *)&backend_update.firmware_tag, firmware_tag,
+		GB_FIRMWARE_U_TAG_MAX_SIZE);
+
+retry_fw_update:
+	backend_update.status = 0;
+
+	ret = ioctl(fd, FW_MGMT_IOC_INTF_BACKEND_FW_UPDATE, &backend_update);
+	if (ret < 0) {
+		printf("Failed to load backend firmware: %s (%d)\n", fwdev, ret);
+		return -1;
+	}
+
+	if (backend_update.status == GB_FW_U_BACKEND_FW_STATUS_RETRY) {
+		printf("Retrying firmware update: %d\n", backend_update.status);
+		goto retry_fw_update;
+	}
+
+	if (backend_update.status != GB_FW_U_BACKEND_FW_STATUS_SUCCESS) {
+		printf("Load status says loading failed: %d\n",
+			backend_update.status);
+	} else {
+		printf("Backend Firmware (%s) Load done: status: %d\n",
+				firmware_tag, backend_update.status);
+	}
+
+	return 0;
+}
+
+int main(int argc, char *argv[])
+{
+	int fd, ret;
+	char *endptr;
+
+	if (argc > 1 &&
+	    (!strcmp(argv[1], "-h") || !strcmp(argv[1], "--help"))) {
+		usage();
+		return -1;
+	}
+
+	if (argc > 1)
+		fwdev = argv[1];
+
+	if (argc > 2)
+		fw_update_type = strtoul(argv[2], &endptr, 10);
+
+	if (argc > 3)
+		firmware_tag = argv[3];
+	else if (!fw_update_type)
+		firmware_tag = FW_TAG_INT_DEFAULT;
+	else
+		firmware_tag = FW_TAG_BCND_DEFAULT;
+
+	if (argc > 4)
+		fw_timeout = strtoul(argv[4], &endptr, 10);
+
+	printf("Trying Firmware update: fwdev: %s, type: %s, tag: %s, timeout: %u\n",
+		fwdev, fw_update_type == 0 ? "interface" : "backend",
+		firmware_tag, fw_timeout);
+
+	printf("Opening %s firmware management device\n", fwdev);
+
+	fd = open(fwdev, O_RDWR);
+	if (fd < 0) {
+		printf("Failed to open: %s\n", fwdev);
+		return -1;
+	}
+
+	/* Set Timeout */
+	printf("Setting timeout to %u ms\n", fw_timeout);
+
+	ret = ioctl(fd, FW_MGMT_IOC_SET_TIMEOUT_MS, &fw_timeout);
+	if (ret < 0) {
+		printf("Failed to set timeout: %s (%d)\n", fwdev, ret);
+		ret = -1;
+		goto close_fd;
+	}
+
+	if (!fw_update_type)
+		ret = update_intf_firmware(fd);
+	else
+		ret = update_backend_firmware(fd);
+
+close_fd:
+	close(fd);
+
+	return ret;
+}
diff --git a/drivers/staging/greybus/Documentation/sysfs-bus-greybus b/drivers/staging/greybus/Documentation/sysfs-bus-greybus
new file mode 100644
index 000000000..2e998966c
--- /dev/null
+++ b/drivers/staging/greybus/Documentation/sysfs-bus-greybus
@@ -0,0 +1,275 @@
+What:		/sys/bus/greybus/devices/greybusN
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The "root" greybus device for the Greybus device tree, or bus,
+		where N is a dynamically assigned 1-based id.
+
+What:		/sys/bus/greybus/devices/greybusN/bus_id
+Date:		April 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The ID of the "root" greybus device, or bus.
+
+What:		/sys/bus/greybus/devices/N-M
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		A Module M on the bus N, where M is the 1-byte interface
+		ID of the module's primary interface.
+
+What:		/sys/bus/greybus/devices/N-M/eject
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Writing a non-zero argument to this attibute disables the
+		module's interfaces before physically ejecting it.
+
+What:		/sys/bus/greybus/devices/N-M/module_id
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The ID of a Greybus module, corresponding to the ID of its
+		primary interface.
+
+What:		/sys/bus/greybus/devices/N-M/num_interfaces
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The number of interfaces of a module.
+
+What:		/sys/bus/greybus/devices/N-M.I
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		An Interface I on the bus N and module N-M, where I is the
+		1-byte interface ID.
+
+What:		/sys/bus/greybus/devices/N-M.I/current_now
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Current measurement of the interface in microamps (uA)
+
+What:		/sys/bus/greybus/devices/N-M.I/ddbl1_manufacturer_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Unipro Device Descriptor Block Level 1 manufacturer ID for the
+		greybus Interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/ddbl1_product_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Unipro Device Descriptor Block Level 1 product ID for the
+		greybus Interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/interface_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The ID of a Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/interface_type
+Date:		June 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The type of a Greybus interface; "dummy", "unipro", "greybus",
+		or "unknown".
+
+What:		/sys/bus/greybus/devices/N-M.I/power_now
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Power measurement of the interface in microwatts (uW)
+
+What:		/sys/bus/greybus/devices/N-M.I/power_state
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		This file reflects the power state of a Greybus interface. If
+		the value read from it is "on", then power is currently
+		supplied to the interface. Otherwise it will read "off" and
+		power is currently not supplied to the interface.
+
+		If the value read is "off", then writing "on" (or '1', 'y',
+		'Y') to this file will enable power to the interface and an
+		attempt to boot and possibly enumerate it will be made. Note
+		that on errors, the interface will again be powered down.
+
+		If the value read is "on", then writing "off" (or '0', 'n',
+		'N') to this file will power down the interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/product_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Product ID of a Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/serial_number
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Serial Number of the Greybus interface, represented by a 64 bit
+		hexadecimal number.
+
+What:		/sys/bus/greybus/devices/N-M.I/vendor_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Vendor ID of a Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I/voltage_now
+Date:		March 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Voltage measurement of the interface in microvolts (uV)
+
+What:		/sys/bus/greybus/devices/N-M.I.ctrl
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Abstract control device for interface I that represents the
+		current mode of an enumerated Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I.ctrl/product_string
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Product ID string of a Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I.ctrl/vendor_string
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Vendor ID string of a Greybus interface.
+
+What:		/sys/bus/greybus/devices/N-M.I.B
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		A bundle B on the Interface I, B is replaced by a 1-byte
+		number representing the bundle.
+
+What:		/sys/bus/greybus/devices/N-M.I.B/bundle_class
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The greybus class of the bundle B.
+
+What:		/sys/bus/greybus/devices/N-M.I.B/bundle_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The interface-unique id of the bundle B.
+
+What:		/sys/bus/greybus/devices/N-M.I.B/gpbX
+Date:		April 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The General Purpose Bridged PHY device of the bundle B,
+		where X is a dynamically assigned 0-based id.
+
+What:		/sys/bus/greybus/devices/N-M.I.B/state
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		A bundle has a state that is managed by the userspace
+		Endo process.  This file allows that Endo to signal
+		other Android HALs that the state of the bundle has
+		changed to a specific value.  When written to, any
+		process watching the file will be woken up, and the new
+		value can be read. It's a "poor-man's IPC", yes, but
+		simplifies the Android userspace code immensely.
+
+What:		/sys/bus/greybus/devices/N-svc
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The singleton SVC device of bus N.
+
+What:		/sys/bus/greybus/devices/N-svc/ap_intf_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The AP interface ID, a 1-byte non-zero integer which
+		defines the position of the AP module on the frame.
+		The interface positions are defined in the GMP
+		Module Developer Kit.
+
+What:		/sys/bus/greybus/devices/N-svc/endo_id
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The Endo ID, which is a 2-byte hexadecimal value
+		defined by the Endo layout scheme, documented in
+		the GMP Module Developer Kit.
+
+What:		/sys/bus/greybus/devices/N-svc/intf_eject
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		Write the number of the interface that you wish to
+		forcibly eject from the system.
+
+What:		/sys/bus/greybus/devices/N-svc/version
+Date:		October 2015
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		The version number of the firmware in the SVC device.
+
+What:		/sys/bus/greybus/devices/N-svc/watchdog
+Date:		October 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		If the SVC watchdog is enabled or not.  Writing 0 to this
+		file will disable the watchdog, writing 1 will enable it.
+
+What:		/sys/bus/greybus/devices/N-svc/watchdog_action
+Date:		July 2016
+KernelVersion:	4.XX
+Contact:	Greg Kroah-Hartman <greg@kroah.com>
+Description:
+		This attribute indicates the action to be performed upon SVC
+		watchdog bite.
+
+		The action can be one of the "reset" or "panic". Writing either
+		one of the "reset" or "panic" will change the behavior of SVC
+		watchdog bite. Default value is "reset".
+
+		"reset" means the UniPro subsystem is to be reset.
+
+		"panic" means SVC watchdog bite will cause kernel to panic.
diff --git a/drivers/staging/greybus/Kconfig b/drivers/staging/greybus/Kconfig
new file mode 100644
index 000000000..927cfa4bc
--- /dev/null
+++ b/drivers/staging/greybus/Kconfig
@@ -0,0 +1,216 @@
+# SPDX-License-Identifier: GPL-2.0
+if GREYBUS
+
+config GREYBUS_AUDIO
+	tristate "Greybus Audio Class driver"
+	depends on SOUND && SND_SOC
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Audio Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-audio.ko
+
+config GREYBUS_AUDIO_APB_CODEC
+	tristate "Greybus APBridge Audio codec driver"
+	depends on SND_SOC && GREYBUS_AUDIO
+	help
+	  Select this option if you have a Toshiba APB device that has I2S
+          ports and acts as a Greybus "Dummy codec". This device is a
+          bridge from an APB-I2S port to a Unipro network.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-audio-codec.ko
+
+
+config GREYBUS_BOOTROM
+	tristate "Greybus Bootrom Class driver"
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Bootrom Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-bootrom.ko
+
+config GREYBUS_CAMERA
+	tristate "Greybus Camera Class driver"
+	depends on MEDIA_SUPPORT && LEDS_CLASS_FLASH && BROKEN
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Camera Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-camera.ko
+
+config GREYBUS_FIRMWARE
+	tristate "Greybus Firmware Download Class driver"
+	depends on SPI
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Firmware Download Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-firmware.ko
+
+config GREYBUS_HID
+	tristate "Greybus HID Class driver"
+	depends on HID && INPUT
+	help
+	  Select this option if you have a device that follows the
+	  Greybus HID Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-hid.ko
+
+config GREYBUS_LIGHT
+	tristate "Greybus LED Class driver"
+	depends on LEDS_CLASS
+	help
+	  Select this option if you have a device that follows the
+	  Greybus LED Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-light.ko
+
+config GREYBUS_LOG
+	tristate "Greybus Debug Log Class driver"
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Debug Log Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-log.ko
+
+config GREYBUS_LOOPBACK
+	tristate "Greybus Loopback Class driver"
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Debug Log Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-log.ko
+
+config GREYBUS_POWER
+	tristate "Greybus Powersupply Class driver"
+	depends on POWER_SUPPLY
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Powersupply Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-power-supply.ko
+
+config GREYBUS_RAW
+	tristate "Greybus Raw Class driver"
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Raw Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-raw.ko
+
+config GREYBUS_VIBRATOR
+	tristate "Greybus Vibrator Motor Class driver"
+	help
+	  Select this option if you have a device that follows the
+	  Greybus Vibrator Motor Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-vibrator.ko
+
+menuconfig GREYBUS_BRIDGED_PHY
+	tristate "Greybus Bridged PHY Class drivers"
+	help
+	  Select this option to pick from a variety of Greybus Bridged
+	  PHY class drivers.  These drivers emulate a number of
+	  different "traditional" busses by tunneling them over Greybus.
+	  Examples of this include serial, SPI, USB, and others.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-phy.ko
+
+if GREYBUS_BRIDGED_PHY
+
+config GREYBUS_GPIO
+	tristate "Greybus GPIO Bridged PHY driver"
+	depends on GPIOLIB
+	select GPIOLIB_IRQCHIP
+	help
+	  Select this option if you have a device that follows the
+	  Greybus GPIO Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-gpio.ko
+
+config GREYBUS_I2C
+	tristate "Greybus I2C Bridged PHY driver"
+	depends on I2C
+	help
+	  Select this option if you have a device that follows the
+	  Greybus I2C Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-i2c.ko
+
+config GREYBUS_PWM
+	tristate "Greybus PWM Bridged PHY driver"
+	depends on PWM
+	help
+	  Select this option if you have a device that follows the
+	  Greybus PWM Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-pwm.ko
+
+config GREYBUS_SDIO
+	tristate "Greybus SDIO Bridged PHY driver"
+	depends on MMC
+	help
+	  Select this option if you have a device that follows the
+	  Greybus SDIO Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-sdio.ko
+
+config GREYBUS_SPI
+	tristate "Greybus SPI Bridged PHY driver"
+	depends on SPI
+	help
+	  Select this option if you have a device that follows the
+	  Greybus SPI Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-spi.ko
+
+config GREYBUS_UART
+	tristate "Greybus UART Bridged PHY driver"
+	depends on TTY
+	help
+	  Select this option if you have a device that follows the
+	  Greybus UART Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-uart.ko
+
+config GREYBUS_USB
+	tristate "Greybus USB Host Bridged PHY driver"
+	depends on USB
+	help
+	  Select this option if you have a device that follows the
+	  Greybus USB Host Bridged PHY Class specification.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-usb.ko
+
+endif	# GREYBUS_BRIDGED_PHY
+
+config GREYBUS_ARCHE
+	tristate "Greybus Arche Platform driver"
+	depends on USB_HSIC_USB3613 || COMPILE_TEST
+	help
+	  Select this option if you have an Arche device.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-arche.ko
+
+endif	# GREYBUS
diff --git a/drivers/staging/greybus/Makefile b/drivers/staging/greybus/Makefile
new file mode 100644
index 000000000..7c5e89622
--- /dev/null
+++ b/drivers/staging/greybus/Makefile
@@ -0,0 +1,73 @@
+# SPDX-License-Identifier: GPL-2.0
+# needed for trace events
+ccflags-y += -I$(src)
+
+# Greybus class drivers
+gb-bootrom-y		:= bootrom.o
+gb-camera-y		:= camera.o
+gb-firmware-y		:= fw-core.o fw-download.o fw-management.o authentication.o
+gb-spilib-y		:= spilib.o
+gb-hid-y		:= hid.o
+gb-light-y		:= light.o
+gb-log-y		:= log.o
+gb-loopback-y		:= loopback.o
+gb-power-supply-y	:= power_supply.o
+gb-raw-y		:= raw.o
+gb-vibrator-y		:= vibrator.o
+
+obj-$(CONFIG_GREYBUS_BOOTROM)	+= gb-bootrom.o
+obj-$(CONFIG_GREYBUS_CAMERA)	+= gb-camera.o
+obj-$(CONFIG_GREYBUS_FIRMWARE)	+= gb-firmware.o gb-spilib.o
+obj-$(CONFIG_GREYBUS_HID)	+= gb-hid.o
+obj-$(CONFIG_GREYBUS_LIGHT)	+= gb-light.o
+obj-$(CONFIG_GREYBUS_LOG)	+= gb-log.o
+obj-$(CONFIG_GREYBUS_LOOPBACK)	+= gb-loopback.o
+obj-$(CONFIG_GREYBUS_POWER)	+= gb-power-supply.o
+obj-$(CONFIG_GREYBUS_RAW)	+= gb-raw.o
+obj-$(CONFIG_GREYBUS_VIBRATOR)	+= gb-vibrator.o
+
+# Greybus Audio is a bunch of modules
+gb-audio-module-y	:= audio_module.o audio_topology.o
+gb-audio-codec-y	:= audio_codec.o audio_helper.o
+gb-audio-gb-y		:= audio_gb.o
+gb-audio-apbridgea-y	:= audio_apbridgea.o
+gb-audio-manager-y	:= audio_manager.o audio_manager_module.o
+
+# Greybus Audio sysfs helpers can be useful when debugging
+#GB_AUDIO_MANAGER_SYSFS ?= true
+#ifeq ($(GB_AUDIO_MANAGER_SYSFS),true)
+#gb-audio-manager-y += audio_manager_sysfs.o
+#ccflags-y += -DGB_AUDIO_MANAGER_SYSFS
+#endif
+
+obj-$(CONFIG_GREYBUS_AUDIO_APB_CODEC)  	+= gb-audio-codec.o
+obj-$(CONFIG_GREYBUS_AUDIO_APB_CODEC)	+= gb-audio-module.o
+obj-$(CONFIG_GREYBUS_AUDIO)		+= gb-audio-gb.o
+obj-$(CONFIG_GREYBUS_AUDIO)		+= gb-audio-apbridgea.o
+obj-$(CONFIG_GREYBUS_AUDIO)		+= gb-audio-manager.o
+
+
+# Greybus Bridged PHY drivers
+gb-gbphy-y := gbphy.o
+gb-gpio-y := gpio.o
+gb-i2c-y := i2c.o
+gb-pwm-y := pwm.o
+gb-sdio-y := sdio.o
+gb-spi-y := spi.o
+gb-uart-y := uart.o
+gb-usb-y := usb.o
+
+obj-$(CONFIG_GREYBUS_BRIDGED_PHY)	+= gb-gbphy.o
+obj-$(CONFIG_GREYBUS_GPIO)		+= gb-gpio.o
+obj-$(CONFIG_GREYBUS_I2C)		+= gb-i2c.o
+obj-$(CONFIG_GREYBUS_PWM)		+= gb-pwm.o
+obj-$(CONFIG_GREYBUS_SDIO)		+= gb-sdio.o
+obj-$(CONFIG_GREYBUS_SPI)		+= gb-spi.o gb-spilib.o
+obj-$(CONFIG_GREYBUS_UART)		+= gb-uart.o
+obj-$(CONFIG_GREYBUS_USB)		+= gb-usb.o
+
+
+# Greybus Platform driver
+gb-arche-y	:= arche-platform.o arche-apb-ctrl.o
+
+obj-$(CONFIG_GREYBUS_ARCHE)	+= gb-arche.o
diff --git a/drivers/staging/greybus/TODO b/drivers/staging/greybus/TODO
new file mode 100644
index 000000000..6461e0132
--- /dev/null
+++ b/drivers/staging/greybus/TODO
@@ -0,0 +1,5 @@
+* Convert all uses of the old GPIO API from <linux/gpio.h> to the
+  GPIO descriptor API in <linux/gpio/consumer.h> and look up GPIO
+  lines from device tree or ACPI.
+* Make pwm.c use the struct pwm_ops::apply instead of ::config, ::set_polarity,
+  ::enable and ::disable.
diff --git a/drivers/staging/greybus/arche-apb-ctrl.c b/drivers/staging/greybus/arche-apb-ctrl.c
new file mode 100644
index 000000000..45afa208d
--- /dev/null
+++ b/drivers/staging/greybus/arche-apb-ctrl.c
@@ -0,0 +1,491 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Arche Platform driver to control APB.
+ *
+ * Copyright 2014-2015 Google Inc.
+ * Copyright 2014-2015 Linaro Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/of_irq.h>
+#include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spinlock.h>
+#include "arche_platform.h"
+
+static void apb_bootret_deassert(struct device *dev);
+
+struct arche_apb_ctrl_drvdata {
+	/* Control GPIO signals to and from AP <=> AP Bridges */
+	struct gpio_desc *resetn;
+	struct gpio_desc *boot_ret;
+	struct gpio_desc *pwroff;
+	struct gpio_desc *wake_in;
+	struct gpio_desc *wake_out;
+	struct gpio_desc *pwrdn;
+
+	enum arche_platform_state state;
+	bool init_disabled;
+
+	struct regulator *vcore;
+	struct regulator *vio;
+
+	struct gpio_desc *clk_en;
+	struct clk *clk;
+
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *pin_default;
+
+	/* V2: SPI Bus control  */
+	struct gpio_desc *spi_en;
+	bool spi_en_polarity_high;
+};
+
+/*
+ * Note that these low level api's are active high
+ */
+static inline void deassert_reset(struct gpio_desc *gpio)
+{
+	gpiod_set_raw_value(gpio, 1);
+}
+
+static inline void assert_reset(struct gpio_desc *gpio)
+{
+	gpiod_set_raw_value(gpio, 0);
+}
+
+/*
+ * Note: Please do not modify the below sequence, as it is as per the spec
+ */
+static int coldboot_seq(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct arche_apb_ctrl_drvdata *apb = platform_get_drvdata(pdev);
+	int ret;
+
+	if (apb->init_disabled ||
+	    apb->state == ARCHE_PLATFORM_STATE_ACTIVE)
+		return 0;
+
+	/* Hold APB in reset state */
+	assert_reset(apb->resetn);
+
+	if (apb->state == ARCHE_PLATFORM_STATE_FW_FLASHING && apb->spi_en)
+		devm_gpiod_put(dev, apb->spi_en);
+
+	/* Enable power to APB */
+	if (!IS_ERR(apb->vcore)) {
+		ret = regulator_enable(apb->vcore);
+		if (ret) {
+			dev_err(dev, "failed to enable core regulator\n");
+			return ret;
+		}
+	}
+
+	if (!IS_ERR(apb->vio)) {
+		ret = regulator_enable(apb->vio);
+		if (ret) {
+			dev_err(dev, "failed to enable IO regulator\n");
+			return ret;
+		}
+	}
+
+	apb_bootret_deassert(dev);
+
+	/* On DB3 clock was not mandatory */
+	if (apb->clk_en)
+		gpiod_set_value(apb->clk_en, 1);
+
+	usleep_range(100, 200);
+
+	/* deassert reset to APB : Active-low signal */
+	deassert_reset(apb->resetn);
+
+	apb->state = ARCHE_PLATFORM_STATE_ACTIVE;
+
+	return 0;
+}
+
+static int fw_flashing_seq(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct arche_apb_ctrl_drvdata *apb = platform_get_drvdata(pdev);
+	int ret;
+
+	if (apb->init_disabled ||
+	    apb->state == ARCHE_PLATFORM_STATE_FW_FLASHING)
+		return 0;
+
+	ret = regulator_enable(apb->vcore);
+	if (ret) {
+		dev_err(dev, "failed to enable core regulator\n");
+		return ret;
+	}
+
+	ret = regulator_enable(apb->vio);
+	if (ret) {
+		dev_err(dev, "failed to enable IO regulator\n");
+		return ret;
+	}
+
+	if (apb->spi_en) {
+		unsigned long flags;
+
+		if (apb->spi_en_polarity_high)
+			flags = GPIOD_OUT_HIGH;
+		else
+			flags = GPIOD_OUT_LOW;
+
+		apb->spi_en = devm_gpiod_get(dev, "spi-en", flags);
+		if (IS_ERR(apb->spi_en)) {
+			ret = PTR_ERR(apb->spi_en);
+			dev_err(dev, "Failed requesting SPI bus en GPIO: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	/* for flashing device should be in reset state */
+	assert_reset(apb->resetn);
+	apb->state = ARCHE_PLATFORM_STATE_FW_FLASHING;
+
+	return 0;
+}
+
+static int standby_boot_seq(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct arche_apb_ctrl_drvdata *apb = platform_get_drvdata(pdev);
+
+	if (apb->init_disabled)
+		return 0;
+
+	/*
+	 * Even if it is in OFF state,
+	 * then we do not want to change the state
+	 */
+	if (apb->state == ARCHE_PLATFORM_STATE_STANDBY ||
+	    apb->state == ARCHE_PLATFORM_STATE_OFF)
+		return 0;
+
+	if (apb->state == ARCHE_PLATFORM_STATE_FW_FLASHING && apb->spi_en)
+		devm_gpiod_put(dev, apb->spi_en);
+
+	/*
+	 * As per WDM spec, do nothing
+	 *
+	 * Pasted from WDM spec,
+	 *  - A falling edge on POWEROFF_L is detected (a)
+	 *  - WDM enters standby mode, but no output signals are changed
+	 */
+
+	/* TODO: POWEROFF_L is input to WDM module  */
+	apb->state = ARCHE_PLATFORM_STATE_STANDBY;
+	return 0;
+}
+
+static void poweroff_seq(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct arche_apb_ctrl_drvdata *apb = platform_get_drvdata(pdev);
+
+	if (apb->init_disabled || apb->state == ARCHE_PLATFORM_STATE_OFF)
+		return;
+
+	if (apb->state == ARCHE_PLATFORM_STATE_FW_FLASHING && apb->spi_en)
+		devm_gpiod_put(dev, apb->spi_en);
+
+	/* disable the clock */
+	if (apb->clk_en)
+		gpiod_set_value(apb->clk_en, 0);
+
+	if (!IS_ERR(apb->vcore) && regulator_is_enabled(apb->vcore) > 0)
+		regulator_disable(apb->vcore);
+
+	if (!IS_ERR(apb->vio) && regulator_is_enabled(apb->vio) > 0)
+		regulator_disable(apb->vio);
+
+	/* As part of exit, put APB back in reset state */
+	assert_reset(apb->resetn);
+	apb->state = ARCHE_PLATFORM_STATE_OFF;
+
+	/* TODO: May have to send an event to SVC about this exit */
+}
+
+static void apb_bootret_deassert(struct device *dev)
+{
+	struct arche_apb_ctrl_drvdata *apb = dev_get_drvdata(dev);
+
+	gpiod_set_value(apb->boot_ret, 0);
+}
+
+int apb_ctrl_coldboot(struct device *dev)
+{
+	return coldboot_seq(to_platform_device(dev));
+}
+
+int apb_ctrl_fw_flashing(struct device *dev)
+{
+	return fw_flashing_seq(to_platform_device(dev));
+}
+
+int apb_ctrl_standby_boot(struct device *dev)
+{
+	return standby_boot_seq(to_platform_device(dev));
+}
+
+void apb_ctrl_poweroff(struct device *dev)
+{
+	poweroff_seq(to_platform_device(dev));
+}
+
+static ssize_t state_store(struct device *dev,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct arche_apb_ctrl_drvdata *apb = platform_get_drvdata(pdev);
+	int ret = 0;
+	bool is_disabled;
+
+	if (sysfs_streq(buf, "off")) {
+		if (apb->state == ARCHE_PLATFORM_STATE_OFF)
+			return count;
+
+		poweroff_seq(pdev);
+	} else if (sysfs_streq(buf, "active")) {
+		if (apb->state == ARCHE_PLATFORM_STATE_ACTIVE)
+			return count;
+
+		poweroff_seq(pdev);
+		is_disabled = apb->init_disabled;
+		apb->init_disabled = false;
+		ret = coldboot_seq(pdev);
+		if (ret)
+			apb->init_disabled = is_disabled;
+	} else if (sysfs_streq(buf, "standby")) {
+		if (apb->state == ARCHE_PLATFORM_STATE_STANDBY)
+			return count;
+
+		ret = standby_boot_seq(pdev);
+	} else if (sysfs_streq(buf, "fw_flashing")) {
+		if (apb->state == ARCHE_PLATFORM_STATE_FW_FLASHING)
+			return count;
+
+		/*
+		 * First we want to make sure we power off everything
+		 * and then enter FW flashing state
+		 */
+		poweroff_seq(pdev);
+		ret = fw_flashing_seq(pdev);
+	} else {
+		dev_err(dev, "unknown state\n");
+		ret = -EINVAL;
+	}
+
+	return ret ? ret : count;
+}
+
+static ssize_t state_show(struct device *dev,
+			  struct device_attribute *attr, char *buf)
+{
+	struct arche_apb_ctrl_drvdata *apb = dev_get_drvdata(dev);
+
+	switch (apb->state) {
+	case ARCHE_PLATFORM_STATE_OFF:
+		return sprintf(buf, "off%s\n",
+				apb->init_disabled ? ",disabled" : "");
+	case ARCHE_PLATFORM_STATE_ACTIVE:
+		return sprintf(buf, "active\n");
+	case ARCHE_PLATFORM_STATE_STANDBY:
+		return sprintf(buf, "standby\n");
+	case ARCHE_PLATFORM_STATE_FW_FLASHING:
+		return sprintf(buf, "fw_flashing\n");
+	default:
+		return sprintf(buf, "unknown state\n");
+	}
+}
+
+static DEVICE_ATTR_RW(state);
+
+static int apb_ctrl_get_devtree_data(struct platform_device *pdev,
+				     struct arche_apb_ctrl_drvdata *apb)
+{
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	apb->resetn = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(apb->resetn)) {
+		ret = PTR_ERR(apb->resetn);
+		dev_err(dev, "Failed requesting reset GPIO: %d\n", ret);
+		return ret;
+	}
+
+	apb->boot_ret = devm_gpiod_get(dev, "boot-ret", GPIOD_OUT_LOW);
+	if (IS_ERR(apb->boot_ret)) {
+		ret = PTR_ERR(apb->boot_ret);
+		dev_err(dev, "Failed requesting bootret GPIO: %d\n", ret);
+		return ret;
+	}
+
+	/* It's not mandatory to support power management interface */
+	apb->pwroff = devm_gpiod_get_optional(dev, "pwr-off", GPIOD_IN);
+	if (IS_ERR(apb->pwroff)) {
+		ret = PTR_ERR(apb->pwroff);
+		dev_err(dev, "Failed requesting pwroff_n GPIO: %d\n", ret);
+		return ret;
+	}
+
+	/* Do not make clock mandatory as of now (for DB3) */
+	apb->clk_en = devm_gpiod_get_optional(dev, "clock-en", GPIOD_OUT_LOW);
+	if (IS_ERR(apb->clk_en)) {
+		ret = PTR_ERR(apb->clk_en);
+		dev_err(dev, "Failed requesting APB clock en GPIO: %d\n", ret);
+		return ret;
+	}
+
+	apb->pwrdn = devm_gpiod_get(dev, "pwr-down", GPIOD_OUT_LOW);
+	if (IS_ERR(apb->pwrdn)) {
+		ret = PTR_ERR(apb->pwrdn);
+		dev_warn(dev, "Failed requesting power down GPIO: %d\n", ret);
+		return ret;
+	}
+
+	/* Regulators are optional, as we may have fixed supply coming in */
+	apb->vcore = devm_regulator_get(dev, "vcore");
+	if (IS_ERR(apb->vcore))
+		dev_warn(dev, "no core regulator found\n");
+
+	apb->vio = devm_regulator_get(dev, "vio");
+	if (IS_ERR(apb->vio))
+		dev_warn(dev, "no IO regulator found\n");
+
+	apb->pinctrl = devm_pinctrl_get(&pdev->dev);
+	if (IS_ERR(apb->pinctrl)) {
+		dev_err(&pdev->dev, "could not get pinctrl handle\n");
+		return PTR_ERR(apb->pinctrl);
+	}
+	apb->pin_default = pinctrl_lookup_state(apb->pinctrl, "default");
+	if (IS_ERR(apb->pin_default)) {
+		dev_err(&pdev->dev, "could not get default pin state\n");
+		return PTR_ERR(apb->pin_default);
+	}
+
+	/* Only applicable for platform >= V2 */
+	if (of_property_read_bool(pdev->dev.of_node, "gb,spi-en-active-high"))
+		apb->spi_en_polarity_high = true;
+
+	return 0;
+}
+
+static int arche_apb_ctrl_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct arche_apb_ctrl_drvdata *apb;
+	struct device *dev = &pdev->dev;
+
+	apb = devm_kzalloc(&pdev->dev, sizeof(*apb), GFP_KERNEL);
+	if (!apb)
+		return -ENOMEM;
+
+	ret = apb_ctrl_get_devtree_data(pdev, apb);
+	if (ret) {
+		dev_err(dev, "failed to get apb devicetree data %d\n", ret);
+		return ret;
+	}
+
+	/* Initially set APB to OFF state */
+	apb->state = ARCHE_PLATFORM_STATE_OFF;
+	/* Check whether device needs to be enabled on boot */
+	if (of_property_read_bool(pdev->dev.of_node, "arche,init-disable"))
+		apb->init_disabled = true;
+
+	platform_set_drvdata(pdev, apb);
+
+	/* Create sysfs interface to allow user to change state dynamically */
+	ret = device_create_file(dev, &dev_attr_state);
+	if (ret) {
+		dev_err(dev, "failed to create state file in sysfs\n");
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "Device registered successfully\n");
+	return 0;
+}
+
+static int arche_apb_ctrl_remove(struct platform_device *pdev)
+{
+	device_remove_file(&pdev->dev, &dev_attr_state);
+	poweroff_seq(pdev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static int __maybe_unused arche_apb_ctrl_suspend(struct device *dev)
+{
+	/*
+	 * If timing profile permits, we may shutdown bridge
+	 * completely
+	 *
+	 * TODO: sequence ??
+	 *
+	 * Also, need to make sure we meet precondition for unipro suspend
+	 * Precondition: Definition ???
+	 */
+	return 0;
+}
+
+static int __maybe_unused arche_apb_ctrl_resume(struct device *dev)
+{
+	/*
+	 * At least for ES2 we have to meet the delay requirement between
+	 * unipro switch and AP bridge init, depending on whether bridge is in
+	 * OFF state or standby state.
+	 *
+	 * Based on whether bridge is in standby or OFF state we may have to
+	 * assert multiple signals. Please refer to WDM spec, for more info.
+	 *
+	 */
+	return 0;
+}
+
+static void arche_apb_ctrl_shutdown(struct platform_device *pdev)
+{
+	apb_ctrl_poweroff(&pdev->dev);
+}
+
+static SIMPLE_DEV_PM_OPS(arche_apb_ctrl_pm_ops, arche_apb_ctrl_suspend,
+			 arche_apb_ctrl_resume);
+
+static const struct of_device_id arche_apb_ctrl_of_match[] = {
+	{ .compatible = "usbffff,2", },
+	{ },
+};
+
+static struct platform_driver arche_apb_ctrl_device_driver = {
+	.probe		= arche_apb_ctrl_probe,
+	.remove		= arche_apb_ctrl_remove,
+	.shutdown	= arche_apb_ctrl_shutdown,
+	.driver		= {
+		.name	= "arche-apb-ctrl",
+		.pm	= &arche_apb_ctrl_pm_ops,
+		.of_match_table = arche_apb_ctrl_of_match,
+	}
+};
+
+int __init arche_apb_init(void)
+{
+	return platform_driver_register(&arche_apb_ctrl_device_driver);
+}
+
+void __exit arche_apb_exit(void)
+{
+	platform_driver_unregister(&arche_apb_ctrl_device_driver);
+}
diff --git a/drivers/staging/greybus/arche-platform.c b/drivers/staging/greybus/arche-platform.c
new file mode 100644
index 000000000..fcbd5f71e
--- /dev/null
+++ b/drivers/staging/greybus/arche-platform.c
@@ -0,0 +1,668 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Arche Platform driver to enable Unipro link.
+ *
+ * Copyright 2014-2015 Google Inc.
+ * Copyright 2014-2015 Linaro Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/suspend.h>
+#include <linux/time.h>
+#include <linux/greybus.h>
+#include "arche_platform.h"
+
+#if IS_ENABLED(CONFIG_USB_HSIC_USB3613)
+#include <linux/usb/usb3613.h>
+#else
+static inline int usb3613_hub_mode_ctrl(bool unused)
+{
+	return 0;
+}
+#endif
+
+#define WD_COLDBOOT_PULSE_WIDTH_MS	30
+
+enum svc_wakedetect_state {
+	WD_STATE_IDLE,			/* Default state = pulled high/low */
+	WD_STATE_BOOT_INIT,		/* WD = falling edge (low) */
+	WD_STATE_COLDBOOT_TRIG,		/* WD = rising edge (high), > 30msec */
+	WD_STATE_STANDBYBOOT_TRIG,	/* As of now not used ?? */
+	WD_STATE_COLDBOOT_START,	/* Cold boot process started */
+	WD_STATE_STANDBYBOOT_START,	/* Not used */
+};
+
+struct arche_platform_drvdata {
+	/* Control GPIO signals to and from AP <=> SVC */
+	struct gpio_desc *svc_reset;
+	bool is_reset_act_hi;
+	struct gpio_desc *svc_sysboot;
+	struct gpio_desc *wake_detect; /* bi-dir,maps to WAKE_MOD & WAKE_FRAME signals */
+
+	enum arche_platform_state state;
+
+	struct gpio_desc *svc_refclk_req;
+	struct clk *svc_ref_clk;
+
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *pin_default;
+
+	int num_apbs;
+
+	enum svc_wakedetect_state wake_detect_state;
+	int wake_detect_irq;
+	spinlock_t wake_lock;			/* Protect wake_detect_state */
+	struct mutex platform_state_mutex;	/* Protect state */
+	unsigned long wake_detect_start;
+	struct notifier_block pm_notifier;
+
+	struct device *dev;
+};
+
+/* Requires calling context to hold arche_pdata->platform_state_mutex */
+static void arche_platform_set_state(struct arche_platform_drvdata *arche_pdata,
+				     enum arche_platform_state state)
+{
+	arche_pdata->state = state;
+}
+
+/* Requires arche_pdata->wake_lock is held by calling context */
+static void arche_platform_set_wake_detect_state(struct arche_platform_drvdata *arche_pdata,
+						 enum svc_wakedetect_state state)
+{
+	arche_pdata->wake_detect_state = state;
+}
+
+static inline void svc_reset_onoff(struct gpio_desc *gpio, bool onoff)
+{
+	gpiod_set_raw_value(gpio, onoff);
+}
+
+static int apb_cold_boot(struct device *dev, void *data)
+{
+	int ret;
+
+	ret = apb_ctrl_coldboot(dev);
+	if (ret)
+		dev_warn(dev, "failed to coldboot\n");
+
+	/*Child nodes are independent, so do not exit coldboot operation */
+	return 0;
+}
+
+static int apb_poweroff(struct device *dev, void *data)
+{
+	apb_ctrl_poweroff(dev);
+
+	/* Enable HUB3613 into HUB mode. */
+	if (usb3613_hub_mode_ctrl(false))
+		dev_warn(dev, "failed to control hub device\n");
+
+	return 0;
+}
+
+static void arche_platform_wd_irq_en(struct arche_platform_drvdata *arche_pdata)
+{
+	/* Enable interrupt here, to read event back from SVC */
+	enable_irq(arche_pdata->wake_detect_irq);
+}
+
+static irqreturn_t arche_platform_wd_irq_thread(int irq, void *devid)
+{
+	struct arche_platform_drvdata *arche_pdata = devid;
+	unsigned long flags;
+
+	spin_lock_irqsave(&arche_pdata->wake_lock, flags);
+	if (arche_pdata->wake_detect_state != WD_STATE_COLDBOOT_TRIG) {
+		/* Something is wrong */
+		spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	arche_platform_set_wake_detect_state(arche_pdata,
+					     WD_STATE_COLDBOOT_START);
+	spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
+
+	/* It should complete power cycle, so first make sure it is poweroff */
+	device_for_each_child(arche_pdata->dev, NULL, apb_poweroff);
+
+	/* Bring APB out of reset: cold boot sequence */
+	device_for_each_child(arche_pdata->dev, NULL, apb_cold_boot);
+
+	/* Enable HUB3613 into HUB mode. */
+	if (usb3613_hub_mode_ctrl(true))
+		dev_warn(arche_pdata->dev, "failed to control hub device\n");
+
+	spin_lock_irqsave(&arche_pdata->wake_lock, flags);
+	arche_platform_set_wake_detect_state(arche_pdata, WD_STATE_IDLE);
+	spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arche_platform_wd_irq(int irq, void *devid)
+{
+	struct arche_platform_drvdata *arche_pdata = devid;
+	unsigned long flags;
+
+	spin_lock_irqsave(&arche_pdata->wake_lock, flags);
+
+	if (gpiod_get_value(arche_pdata->wake_detect)) {
+		/* wake/detect rising */
+
+		/*
+		 * If wake/detect line goes high after low, within less than
+		 * 30msec, then standby boot sequence is initiated, which is not
+		 * supported/implemented as of now. So ignore it.
+		 */
+		if (arche_pdata->wake_detect_state == WD_STATE_BOOT_INIT) {
+			if (time_before(jiffies,
+					arche_pdata->wake_detect_start +
+					msecs_to_jiffies(WD_COLDBOOT_PULSE_WIDTH_MS))) {
+				arche_platform_set_wake_detect_state(arche_pdata,
+								     WD_STATE_IDLE);
+			} else {
+				/*
+				 * Check we are not in middle of irq thread
+				 * already
+				 */
+				if (arche_pdata->wake_detect_state !=
+						WD_STATE_COLDBOOT_START) {
+					arche_platform_set_wake_detect_state(arche_pdata,
+									     WD_STATE_COLDBOOT_TRIG);
+					spin_unlock_irqrestore(&arche_pdata->wake_lock,
+							       flags);
+					return IRQ_WAKE_THREAD;
+				}
+			}
+		}
+	} else {
+		/* wake/detect falling */
+		if (arche_pdata->wake_detect_state == WD_STATE_IDLE) {
+			arche_pdata->wake_detect_start = jiffies;
+			/*
+			 * In the beginning, when wake/detect goes low
+			 * (first time), we assume it is meant for coldboot
+			 * and set the flag. If wake/detect line stays low
+			 * beyond 30msec, then it is coldboot else fallback
+			 * to standby boot.
+			 */
+			arche_platform_set_wake_detect_state(arche_pdata,
+							     WD_STATE_BOOT_INIT);
+		}
+	}
+
+	spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Requires arche_pdata->platform_state_mutex to be held
+ */
+static int
+arche_platform_coldboot_seq(struct arche_platform_drvdata *arche_pdata)
+{
+	int ret;
+
+	if (arche_pdata->state == ARCHE_PLATFORM_STATE_ACTIVE)
+		return 0;
+
+	dev_info(arche_pdata->dev, "Booting from cold boot state\n");
+
+	svc_reset_onoff(arche_pdata->svc_reset, arche_pdata->is_reset_act_hi);
+
+	gpiod_set_value(arche_pdata->svc_sysboot, 0);
+	usleep_range(100, 200);
+
+	ret = clk_prepare_enable(arche_pdata->svc_ref_clk);
+	if (ret) {
+		dev_err(arche_pdata->dev, "failed to enable svc_ref_clk: %d\n",
+			ret);
+		return ret;
+	}
+
+	/* bring SVC out of reset */
+	svc_reset_onoff(arche_pdata->svc_reset, !arche_pdata->is_reset_act_hi);
+
+	arche_platform_set_state(arche_pdata, ARCHE_PLATFORM_STATE_ACTIVE);
+
+	return 0;
+}
+
+/*
+ * Requires arche_pdata->platform_state_mutex to be held
+ */
+static int
+arche_platform_fw_flashing_seq(struct arche_platform_drvdata *arche_pdata)
+{
+	int ret;
+
+	if (arche_pdata->state == ARCHE_PLATFORM_STATE_FW_FLASHING)
+		return 0;
+
+	dev_info(arche_pdata->dev, "Switching to FW flashing state\n");
+
+	svc_reset_onoff(arche_pdata->svc_reset, arche_pdata->is_reset_act_hi);
+
+	gpiod_set_value(arche_pdata->svc_sysboot, 1);
+
+	usleep_range(100, 200);
+
+	ret = clk_prepare_enable(arche_pdata->svc_ref_clk);
+	if (ret) {
+		dev_err(arche_pdata->dev, "failed to enable svc_ref_clk: %d\n",
+			ret);
+		return ret;
+	}
+
+	svc_reset_onoff(arche_pdata->svc_reset,	!arche_pdata->is_reset_act_hi);
+
+	arche_platform_set_state(arche_pdata, ARCHE_PLATFORM_STATE_FW_FLASHING);
+
+	return 0;
+}
+
+/*
+ * Requires arche_pdata->platform_state_mutex to be held
+ */
+static void
+arche_platform_poweroff_seq(struct arche_platform_drvdata *arche_pdata)
+{
+	unsigned long flags;
+
+	if (arche_pdata->state == ARCHE_PLATFORM_STATE_OFF)
+		return;
+
+	/* If in fw_flashing mode, then no need to repeate things again */
+	if (arche_pdata->state != ARCHE_PLATFORM_STATE_FW_FLASHING) {
+		disable_irq(arche_pdata->wake_detect_irq);
+
+		spin_lock_irqsave(&arche_pdata->wake_lock, flags);
+		arche_platform_set_wake_detect_state(arche_pdata,
+						     WD_STATE_IDLE);
+		spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
+	}
+
+	clk_disable_unprepare(arche_pdata->svc_ref_clk);
+
+	/* As part of exit, put APB back in reset state */
+	svc_reset_onoff(arche_pdata->svc_reset,	arche_pdata->is_reset_act_hi);
+
+	arche_platform_set_state(arche_pdata, ARCHE_PLATFORM_STATE_OFF);
+}
+
+static ssize_t state_store(struct device *dev,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct arche_platform_drvdata *arche_pdata = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&arche_pdata->platform_state_mutex);
+
+	if (sysfs_streq(buf, "off")) {
+		if (arche_pdata->state == ARCHE_PLATFORM_STATE_OFF)
+			goto exit;
+
+		/*  If SVC goes down, bring down APB's as well */
+		device_for_each_child(arche_pdata->dev, NULL, apb_poweroff);
+
+		arche_platform_poweroff_seq(arche_pdata);
+
+	} else if (sysfs_streq(buf, "active")) {
+		if (arche_pdata->state == ARCHE_PLATFORM_STATE_ACTIVE)
+			goto exit;
+
+		/* First we want to make sure we power off everything
+		 * and then activate back again
+		 */
+		device_for_each_child(arche_pdata->dev, NULL, apb_poweroff);
+		arche_platform_poweroff_seq(arche_pdata);
+
+		arche_platform_wd_irq_en(arche_pdata);
+		ret = arche_platform_coldboot_seq(arche_pdata);
+		if (ret)
+			goto exit;
+
+	} else if (sysfs_streq(buf, "standby")) {
+		if (arche_pdata->state == ARCHE_PLATFORM_STATE_STANDBY)
+			goto exit;
+
+		dev_warn(arche_pdata->dev, "standby state not supported\n");
+	} else if (sysfs_streq(buf, "fw_flashing")) {
+		if (arche_pdata->state == ARCHE_PLATFORM_STATE_FW_FLASHING)
+			goto exit;
+
+		/*
+		 * Here we only control SVC.
+		 *
+		 * In case of FW_FLASHING mode we do not want to control
+		 * APBs, as in case of V2, SPI bus is shared between both
+		 * the APBs. So let user chose which APB he wants to flash.
+		 */
+		arche_platform_poweroff_seq(arche_pdata);
+
+		ret = arche_platform_fw_flashing_seq(arche_pdata);
+		if (ret)
+			goto exit;
+	} else {
+		dev_err(arche_pdata->dev, "unknown state\n");
+		ret = -EINVAL;
+	}
+
+exit:
+	mutex_unlock(&arche_pdata->platform_state_mutex);
+	return ret ? ret : count;
+}
+
+static ssize_t state_show(struct device *dev,
+			  struct device_attribute *attr, char *buf)
+{
+	struct arche_platform_drvdata *arche_pdata = dev_get_drvdata(dev);
+
+	switch (arche_pdata->state) {
+	case ARCHE_PLATFORM_STATE_OFF:
+		return sprintf(buf, "off\n");
+	case ARCHE_PLATFORM_STATE_ACTIVE:
+		return sprintf(buf, "active\n");
+	case ARCHE_PLATFORM_STATE_STANDBY:
+		return sprintf(buf, "standby\n");
+	case ARCHE_PLATFORM_STATE_FW_FLASHING:
+		return sprintf(buf, "fw_flashing\n");
+	default:
+		return sprintf(buf, "unknown state\n");
+	}
+}
+
+static DEVICE_ATTR_RW(state);
+
+static int arche_platform_pm_notifier(struct notifier_block *notifier,
+				      unsigned long pm_event, void *unused)
+{
+	struct arche_platform_drvdata *arche_pdata =
+		container_of(notifier, struct arche_platform_drvdata,
+			     pm_notifier);
+	int ret = NOTIFY_DONE;
+
+	mutex_lock(&arche_pdata->platform_state_mutex);
+	switch (pm_event) {
+	case PM_SUSPEND_PREPARE:
+		if (arche_pdata->state != ARCHE_PLATFORM_STATE_ACTIVE) {
+			ret = NOTIFY_STOP;
+			break;
+		}
+		device_for_each_child(arche_pdata->dev, NULL, apb_poweroff);
+		arche_platform_poweroff_seq(arche_pdata);
+		break;
+	case PM_POST_SUSPEND:
+		if (arche_pdata->state != ARCHE_PLATFORM_STATE_OFF)
+			break;
+
+		arche_platform_wd_irq_en(arche_pdata);
+		arche_platform_coldboot_seq(arche_pdata);
+		break;
+	default:
+		break;
+	}
+	mutex_unlock(&arche_pdata->platform_state_mutex);
+
+	return ret;
+}
+
+static int arche_platform_probe(struct platform_device *pdev)
+{
+	struct arche_platform_drvdata *arche_pdata;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	int ret;
+	unsigned int flags;
+
+	arche_pdata = devm_kzalloc(&pdev->dev, sizeof(*arche_pdata),
+				   GFP_KERNEL);
+	if (!arche_pdata)
+		return -ENOMEM;
+
+	/* setup svc reset gpio */
+	arche_pdata->is_reset_act_hi = of_property_read_bool(np,
+							     "svc,reset-active-high");
+	if (arche_pdata->is_reset_act_hi)
+		flags = GPIOD_OUT_HIGH;
+	else
+		flags = GPIOD_OUT_LOW;
+
+	arche_pdata->svc_reset = devm_gpiod_get(dev, "svc,reset", flags);
+	if (IS_ERR(arche_pdata->svc_reset)) {
+		ret = PTR_ERR(arche_pdata->svc_reset);
+		dev_err(dev, "failed to request svc-reset GPIO: %d\n", ret);
+		return ret;
+	}
+	arche_platform_set_state(arche_pdata, ARCHE_PLATFORM_STATE_OFF);
+
+	arche_pdata->svc_sysboot = devm_gpiod_get(dev, "svc,sysboot",
+						  GPIOD_OUT_LOW);
+	if (IS_ERR(arche_pdata->svc_sysboot)) {
+		ret = PTR_ERR(arche_pdata->svc_sysboot);
+		dev_err(dev, "failed to request sysboot0 GPIO: %d\n", ret);
+		return ret;
+	}
+
+	/* setup the clock request gpio first */
+	arche_pdata->svc_refclk_req = devm_gpiod_get(dev, "svc,refclk-req",
+						     GPIOD_IN);
+	if (IS_ERR(arche_pdata->svc_refclk_req)) {
+		ret = PTR_ERR(arche_pdata->svc_refclk_req);
+		dev_err(dev, "failed to request svc-clk-req GPIO: %d\n", ret);
+		return ret;
+	}
+
+	/* setup refclk2 to follow the pin */
+	arche_pdata->svc_ref_clk = devm_clk_get(dev, "svc_ref_clk");
+	if (IS_ERR(arche_pdata->svc_ref_clk)) {
+		ret = PTR_ERR(arche_pdata->svc_ref_clk);
+		dev_err(dev, "failed to get svc_ref_clk: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, arche_pdata);
+
+	arche_pdata->num_apbs = of_get_child_count(np);
+	dev_dbg(dev, "Number of APB's available - %d\n", arche_pdata->num_apbs);
+
+	arche_pdata->wake_detect = devm_gpiod_get(dev, "svc,wake-detect",
+						  GPIOD_IN);
+	if (IS_ERR(arche_pdata->wake_detect)) {
+		ret = PTR_ERR(arche_pdata->wake_detect);
+		dev_err(dev, "Failed requesting wake_detect GPIO: %d\n", ret);
+		return ret;
+	}
+
+	arche_platform_set_wake_detect_state(arche_pdata, WD_STATE_IDLE);
+
+	arche_pdata->dev = &pdev->dev;
+
+	spin_lock_init(&arche_pdata->wake_lock);
+	mutex_init(&arche_pdata->platform_state_mutex);
+	arche_pdata->wake_detect_irq =
+		gpiod_to_irq(arche_pdata->wake_detect);
+
+	ret = devm_request_threaded_irq(dev, arche_pdata->wake_detect_irq,
+					arche_platform_wd_irq,
+					arche_platform_wd_irq_thread,
+					IRQF_TRIGGER_FALLING |
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dev_name(dev), arche_pdata);
+	if (ret) {
+		dev_err(dev, "failed to request wake detect IRQ %d\n", ret);
+		return ret;
+	}
+	disable_irq(arche_pdata->wake_detect_irq);
+
+	ret = device_create_file(dev, &dev_attr_state);
+	if (ret) {
+		dev_err(dev, "failed to create state file in sysfs\n");
+		return ret;
+	}
+
+	ret = of_platform_populate(np, NULL, NULL, dev);
+	if (ret) {
+		dev_err(dev, "failed to populate child nodes %d\n", ret);
+		goto err_device_remove;
+	}
+
+	arche_pdata->pm_notifier.notifier_call = arche_platform_pm_notifier;
+	ret = register_pm_notifier(&arche_pdata->pm_notifier);
+
+	if (ret) {
+		dev_err(dev, "failed to register pm notifier %d\n", ret);
+		goto err_device_remove;
+	}
+
+	/* Explicitly power off if requested */
+	if (!of_property_read_bool(pdev->dev.of_node, "arche,init-off")) {
+		mutex_lock(&arche_pdata->platform_state_mutex);
+		ret = arche_platform_coldboot_seq(arche_pdata);
+		if (ret) {
+			dev_err(dev, "Failed to cold boot svc %d\n", ret);
+			goto err_coldboot;
+		}
+		arche_platform_wd_irq_en(arche_pdata);
+		mutex_unlock(&arche_pdata->platform_state_mutex);
+	}
+
+	dev_info(dev, "Device registered successfully\n");
+	return 0;
+
+err_coldboot:
+	mutex_unlock(&arche_pdata->platform_state_mutex);
+err_device_remove:
+	device_remove_file(&pdev->dev, &dev_attr_state);
+	return ret;
+}
+
+static int arche_remove_child(struct device *dev, void *unused)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	platform_device_unregister(pdev);
+
+	return 0;
+}
+
+static int arche_platform_remove(struct platform_device *pdev)
+{
+	struct arche_platform_drvdata *arche_pdata = platform_get_drvdata(pdev);
+
+	unregister_pm_notifier(&arche_pdata->pm_notifier);
+	device_remove_file(&pdev->dev, &dev_attr_state);
+	device_for_each_child(&pdev->dev, NULL, arche_remove_child);
+	arche_platform_poweroff_seq(arche_pdata);
+
+	if (usb3613_hub_mode_ctrl(false))
+		dev_warn(arche_pdata->dev, "failed to control hub device\n");
+		/* TODO: Should we do anything more here ?? */
+	return 0;
+}
+
+static __maybe_unused int arche_platform_suspend(struct device *dev)
+{
+	/*
+	 * If timing profile premits, we may shutdown bridge
+	 * completely
+	 *
+	 * TODO: sequence ??
+	 *
+	 * Also, need to make sure we meet precondition for unipro suspend
+	 * Precondition: Definition ???
+	 */
+	return 0;
+}
+
+static __maybe_unused int arche_platform_resume(struct device *dev)
+{
+	/*
+	 * At least for ES2 we have to meet the delay requirement between
+	 * unipro switch and AP bridge init, depending on whether bridge is in
+	 * OFF state or standby state.
+	 *
+	 * Based on whether bridge is in standby or OFF state we may have to
+	 * assert multiple signals. Please refer to WDM spec, for more info.
+	 *
+	 */
+	return 0;
+}
+
+static void arche_platform_shutdown(struct platform_device *pdev)
+{
+	struct arche_platform_drvdata *arche_pdata = platform_get_drvdata(pdev);
+
+	arche_platform_poweroff_seq(arche_pdata);
+
+	usb3613_hub_mode_ctrl(false);
+}
+
+static SIMPLE_DEV_PM_OPS(arche_platform_pm_ops,
+			arche_platform_suspend,
+			arche_platform_resume);
+
+static const struct of_device_id arche_platform_of_match[] = {
+	/* Use PID/VID of SVC device */
+	{ .compatible = "google,arche-platform", },
+	{ },
+};
+
+static const struct of_device_id arche_combined_id[] = {
+	/* Use PID/VID of SVC device */
+	{ .compatible = "google,arche-platform", },
+	{ .compatible = "usbffff,2", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, arche_combined_id);
+
+static struct platform_driver arche_platform_device_driver = {
+	.probe		= arche_platform_probe,
+	.remove		= arche_platform_remove,
+	.shutdown	= arche_platform_shutdown,
+	.driver		= {
+		.name	= "arche-platform-ctrl",
+		.pm	= &arche_platform_pm_ops,
+		.of_match_table = arche_platform_of_match,
+	}
+};
+
+static int __init arche_init(void)
+{
+	int retval;
+
+	retval = platform_driver_register(&arche_platform_device_driver);
+	if (retval)
+		return retval;
+
+	retval = arche_apb_init();
+	if (retval)
+		platform_driver_unregister(&arche_platform_device_driver);
+
+	return retval;
+}
+module_init(arche_init);
+
+static void __exit arche_exit(void)
+{
+	arche_apb_exit();
+	platform_driver_unregister(&arche_platform_device_driver);
+}
+module_exit(arche_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Vaibhav Hiremath <vaibhav.hiremath@linaro.org>");
+MODULE_DESCRIPTION("Arche Platform Driver");
diff --git a/drivers/staging/greybus/arche_platform.h b/drivers/staging/greybus/arche_platform.h
new file mode 100644
index 000000000..9d997f2d6
--- /dev/null
+++ b/drivers/staging/greybus/arche_platform.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Arche Platform driver to enable Unipro link.
+ *
+ * Copyright 2015-2016 Google Inc.
+ * Copyright 2015-2016 Linaro Ltd.
+ */
+
+#ifndef __ARCHE_PLATFORM_H
+#define __ARCHE_PLATFORM_H
+
+enum arche_platform_state {
+	ARCHE_PLATFORM_STATE_OFF,
+	ARCHE_PLATFORM_STATE_ACTIVE,
+	ARCHE_PLATFORM_STATE_STANDBY,
+	ARCHE_PLATFORM_STATE_FW_FLASHING,
+};
+
+int __init arche_apb_init(void);
+void __exit arche_apb_exit(void);
+
+/* Operational states for the APB device */
+int apb_ctrl_coldboot(struct device *dev);
+int apb_ctrl_fw_flashing(struct device *dev);
+int apb_ctrl_standby_boot(struct device *dev);
+void apb_ctrl_poweroff(struct device *dev);
+
+#endif	/* __ARCHE_PLATFORM_H */
diff --git a/drivers/staging/greybus/audio_apbridgea.c b/drivers/staging/greybus/audio_apbridgea.c
new file mode 100644
index 000000000..26117e390
--- /dev/null
+++ b/drivers/staging/greybus/audio_apbridgea.c
@@ -0,0 +1,205 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Audio Device Class Protocol helpers
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#include <linux/greybus.h>
+#include "audio_apbridgea.h"
+#include "audio_codec.h"
+
+int gb_audio_apbridgea_set_config(struct gb_connection *connection,
+				  __u16 i2s_port, __u32 format, __u32 rate,
+				  __u32 mclk_freq)
+{
+	struct audio_apbridgea_set_config_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_SET_CONFIG;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.format = cpu_to_le32(format);
+	req.rate = cpu_to_le32(rate);
+	req.mclk_freq = cpu_to_le32(mclk_freq);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_set_config);
+
+int gb_audio_apbridgea_register_cport(struct gb_connection *connection,
+				      __u16 i2s_port, __u16 cportid,
+				      __u8 direction)
+{
+	struct audio_apbridgea_register_cport_request req;
+	int ret;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_REGISTER_CPORT;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.cport = cpu_to_le16(cportid);
+	req.direction = direction;
+
+	ret = gb_pm_runtime_get_sync(connection->bundle);
+	if (ret)
+		return ret;
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_register_cport);
+
+int gb_audio_apbridgea_unregister_cport(struct gb_connection *connection,
+					__u16 i2s_port, __u16 cportid,
+					__u8 direction)
+{
+	struct audio_apbridgea_unregister_cport_request req;
+	int ret;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_UNREGISTER_CPORT;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.cport = cpu_to_le16(cportid);
+	req.direction = direction;
+
+	ret = gb_hd_output(connection->hd, &req, sizeof(req),
+			   GB_APB_REQUEST_AUDIO_CONTROL, true);
+
+	gb_pm_runtime_put_autosuspend(connection->bundle);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_unregister_cport);
+
+int gb_audio_apbridgea_set_tx_data_size(struct gb_connection *connection,
+					__u16 i2s_port, __u16 size)
+{
+	struct audio_apbridgea_set_tx_data_size_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_SET_TX_DATA_SIZE;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.size = cpu_to_le16(size);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_set_tx_data_size);
+
+int gb_audio_apbridgea_prepare_tx(struct gb_connection *connection,
+				  __u16 i2s_port)
+{
+	struct audio_apbridgea_prepare_tx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_PREPARE_TX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_prepare_tx);
+
+int gb_audio_apbridgea_start_tx(struct gb_connection *connection,
+				__u16 i2s_port, __u64 timestamp)
+{
+	struct audio_apbridgea_start_tx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_START_TX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.timestamp = cpu_to_le64(timestamp);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_start_tx);
+
+int gb_audio_apbridgea_stop_tx(struct gb_connection *connection, __u16 i2s_port)
+{
+	struct audio_apbridgea_stop_tx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_STOP_TX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_stop_tx);
+
+int gb_audio_apbridgea_shutdown_tx(struct gb_connection *connection,
+				   __u16 i2s_port)
+{
+	struct audio_apbridgea_shutdown_tx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_SHUTDOWN_TX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_shutdown_tx);
+
+int gb_audio_apbridgea_set_rx_data_size(struct gb_connection *connection,
+					__u16 i2s_port, __u16 size)
+{
+	struct audio_apbridgea_set_rx_data_size_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_SET_RX_DATA_SIZE;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+	req.size = cpu_to_le16(size);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_set_rx_data_size);
+
+int gb_audio_apbridgea_prepare_rx(struct gb_connection *connection,
+				  __u16 i2s_port)
+{
+	struct audio_apbridgea_prepare_rx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_PREPARE_RX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_prepare_rx);
+
+int gb_audio_apbridgea_start_rx(struct gb_connection *connection,
+				__u16 i2s_port)
+{
+	struct audio_apbridgea_start_rx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_START_RX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_start_rx);
+
+int gb_audio_apbridgea_stop_rx(struct gb_connection *connection, __u16 i2s_port)
+{
+	struct audio_apbridgea_stop_rx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_STOP_RX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_stop_rx);
+
+int gb_audio_apbridgea_shutdown_rx(struct gb_connection *connection,
+				   __u16 i2s_port)
+{
+	struct audio_apbridgea_shutdown_rx_request req;
+
+	req.hdr.type = AUDIO_APBRIDGEA_TYPE_SHUTDOWN_RX;
+	req.hdr.i2s_port = cpu_to_le16(i2s_port);
+
+	return gb_hd_output(connection->hd, &req, sizeof(req),
+			    GB_APB_REQUEST_AUDIO_CONTROL, true);
+}
+EXPORT_SYMBOL_GPL(gb_audio_apbridgea_shutdown_rx);
+
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("greybus:audio-apbridgea");
+MODULE_DESCRIPTION("Greybus Special APBridgeA Audio Protocol library");
+MODULE_AUTHOR("Mark Greer <mgreer@animalcreek.com>");
diff --git a/drivers/staging/greybus/audio_apbridgea.h b/drivers/staging/greybus/audio_apbridgea.h
new file mode 100644
index 000000000..efec0f815
--- /dev/null
+++ b/drivers/staging/greybus/audio_apbridgea.h
@@ -0,0 +1,133 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2016 Google Inc.
+ */
+/*
+ * This is a special protocol for configuring communication over the
+ * I2S bus between the DSP on the MSM8994 and APBridgeA.  Therefore,
+ * we can predefine several low-level attributes of the communication
+ * because we know that they are supported.  In particular, the following
+ * assumptions are made:
+ *	- there are two channels (i.e., stereo)
+ *	- the low-level protocol is I2S as defined by Philips/NXP
+ *	- the DSP on the MSM8994 is the clock master for MCLK, BCLK, and WCLK
+ *	- WCLK changes on the falling edge of BCLK
+ *	- WCLK low for left channel; high for right channel
+ *	- TX data is sent on the falling edge of BCLK
+ *	- RX data is received/latched on the rising edge of BCLK
+ */
+
+#ifndef __AUDIO_APBRIDGEA_H
+#define __AUDIO_APBRIDGEA_H
+
+#define AUDIO_APBRIDGEA_TYPE_SET_CONFIG			0x01
+#define AUDIO_APBRIDGEA_TYPE_REGISTER_CPORT		0x02
+#define AUDIO_APBRIDGEA_TYPE_UNREGISTER_CPORT		0x03
+#define AUDIO_APBRIDGEA_TYPE_SET_TX_DATA_SIZE		0x04
+							/* 0x05 unused */
+#define AUDIO_APBRIDGEA_TYPE_PREPARE_TX			0x06
+#define AUDIO_APBRIDGEA_TYPE_START_TX			0x07
+#define AUDIO_APBRIDGEA_TYPE_STOP_TX			0x08
+#define AUDIO_APBRIDGEA_TYPE_SHUTDOWN_TX		0x09
+#define AUDIO_APBRIDGEA_TYPE_SET_RX_DATA_SIZE		0x0a
+							/* 0x0b unused */
+#define AUDIO_APBRIDGEA_TYPE_PREPARE_RX			0x0c
+#define AUDIO_APBRIDGEA_TYPE_START_RX			0x0d
+#define AUDIO_APBRIDGEA_TYPE_STOP_RX			0x0e
+#define AUDIO_APBRIDGEA_TYPE_SHUTDOWN_RX		0x0f
+
+#define AUDIO_APBRIDGEA_PCM_FMT_8			BIT(0)
+#define AUDIO_APBRIDGEA_PCM_FMT_16			BIT(1)
+#define AUDIO_APBRIDGEA_PCM_FMT_24			BIT(2)
+#define AUDIO_APBRIDGEA_PCM_FMT_32			BIT(3)
+#define AUDIO_APBRIDGEA_PCM_FMT_64			BIT(4)
+
+#define AUDIO_APBRIDGEA_PCM_RATE_5512			BIT(0)
+#define AUDIO_APBRIDGEA_PCM_RATE_8000			BIT(1)
+#define AUDIO_APBRIDGEA_PCM_RATE_11025			BIT(2)
+#define AUDIO_APBRIDGEA_PCM_RATE_16000			BIT(3)
+#define AUDIO_APBRIDGEA_PCM_RATE_22050			BIT(4)
+#define AUDIO_APBRIDGEA_PCM_RATE_32000			BIT(5)
+#define AUDIO_APBRIDGEA_PCM_RATE_44100			BIT(6)
+#define AUDIO_APBRIDGEA_PCM_RATE_48000			BIT(7)
+#define AUDIO_APBRIDGEA_PCM_RATE_64000			BIT(8)
+#define AUDIO_APBRIDGEA_PCM_RATE_88200			BIT(9)
+#define AUDIO_APBRIDGEA_PCM_RATE_96000			BIT(10)
+#define AUDIO_APBRIDGEA_PCM_RATE_176400			BIT(11)
+#define AUDIO_APBRIDGEA_PCM_RATE_192000			BIT(12)
+
+#define AUDIO_APBRIDGEA_DIRECTION_TX			BIT(0)
+#define AUDIO_APBRIDGEA_DIRECTION_RX			BIT(1)
+
+/* The I2S port is passed in the 'index' parameter of the USB request */
+/* The CPort is passed in the 'value' parameter of the USB request */
+
+struct audio_apbridgea_hdr {
+	__u8	type;
+	__le16	i2s_port;
+	__u8	data[];
+} __packed;
+
+struct audio_apbridgea_set_config_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le32				format;	/* AUDIO_APBRIDGEA_PCM_FMT_* */
+	__le32				rate;	/* AUDIO_APBRIDGEA_PCM_RATE_* */
+	__le32				mclk_freq; /* XXX Remove? */
+} __packed;
+
+struct audio_apbridgea_register_cport_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le16				cport;
+	__u8				direction;
+} __packed;
+
+struct audio_apbridgea_unregister_cport_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le16				cport;
+	__u8				direction;
+} __packed;
+
+struct audio_apbridgea_set_tx_data_size_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le16				size;
+} __packed;
+
+struct audio_apbridgea_prepare_tx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_start_tx_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le64				timestamp;
+} __packed;
+
+struct audio_apbridgea_stop_tx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_shutdown_tx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_set_rx_data_size_request {
+	struct audio_apbridgea_hdr	hdr;
+	__le16				size;
+} __packed;
+
+struct audio_apbridgea_prepare_rx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_start_rx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_stop_rx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+struct audio_apbridgea_shutdown_rx_request {
+	struct audio_apbridgea_hdr	hdr;
+} __packed;
+
+#endif /*__AUDIO_APBRIDGEA_H */
diff --git a/drivers/staging/greybus/audio_codec.c b/drivers/staging/greybus/audio_codec.c
new file mode 100644
index 000000000..0ad8aeabc
--- /dev/null
+++ b/drivers/staging/greybus/audio_codec.c
@@ -0,0 +1,1104 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * APBridge ALSA SoC dummy codec driver
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <sound/soc.h>
+#include <sound/pcm_params.h>
+#include <uapi/linux/input.h>
+
+#include "audio_codec.h"
+#include "audio_apbridgea.h"
+#include "audio_manager.h"
+#include "audio_helper.h"
+
+static struct gbaudio_codec_info *gbcodec;
+
+static struct gbaudio_data_connection *
+find_data(struct gbaudio_module_info *module, int id)
+{
+	struct gbaudio_data_connection *data;
+
+	list_for_each_entry(data, &module->data_list, list) {
+		if (id == data->id)
+			return data;
+	}
+	return NULL;
+}
+
+static struct gbaudio_stream_params *
+find_dai_stream_params(struct gbaudio_codec_info *codec, int id, int stream)
+{
+	struct gbaudio_codec_dai *dai;
+
+	list_for_each_entry(dai, &codec->dai_list, list) {
+		if (dai->id == id)
+			return &dai->params[stream];
+	}
+	return NULL;
+}
+
+static int gbaudio_module_enable_tx(struct gbaudio_codec_info *codec,
+				    struct gbaudio_module_info *module, int id)
+{
+	int module_state, ret = 0;
+	u16 data_cport, i2s_port, cportid;
+	u8 sig_bits, channels;
+	u32 format, rate;
+	struct gbaudio_data_connection *data;
+	struct gbaudio_stream_params *params;
+
+	/* find the dai */
+	data = find_data(module, id);
+	if (!data) {
+		dev_err(module->dev, "%d:DATA connection missing\n", id);
+		return -ENODEV;
+	}
+	module_state = data->state[SNDRV_PCM_STREAM_PLAYBACK];
+
+	params = find_dai_stream_params(codec, id, SNDRV_PCM_STREAM_PLAYBACK);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		return -EINVAL;
+	}
+
+	/* register cport */
+	if (module_state < GBAUDIO_CODEC_STARTUP) {
+		i2s_port = 0;	/* fixed for now */
+		cportid = data->connection->hd_cport_id;
+		ret = gb_audio_apbridgea_register_cport(data->connection,
+							i2s_port, cportid,
+							AUDIO_APBRIDGEA_DIRECTION_TX);
+		if (ret) {
+			dev_err_ratelimited(module->dev, "reg_cport failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_PLAYBACK] = GBAUDIO_CODEC_STARTUP;
+		dev_dbg(module->dev, "Dynamic Register %d DAI\n", cportid);
+	}
+
+	/* hw_params */
+	if (module_state < GBAUDIO_CODEC_HWPARAMS) {
+		format = params->format;
+		channels = params->channels;
+		rate = params->rate;
+		sig_bits = params->sig_bits;
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_set_pcm(module->mgmt_connection, data_cport,
+					  format, rate, channels, sig_bits);
+		if (ret) {
+			dev_err_ratelimited(module->dev, "set_pcm failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_PLAYBACK] = GBAUDIO_CODEC_HWPARAMS;
+		dev_dbg(module->dev, "Dynamic hw_params %d DAI\n", data_cport);
+	}
+
+	/* prepare */
+	if (module_state < GBAUDIO_CODEC_PREPARE) {
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_set_tx_data_size(module->mgmt_connection,
+						   data_cport, 192);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "set_tx_data_size failed:%d\n",
+					    ret);
+			return ret;
+		}
+		ret = gb_audio_gb_activate_tx(module->mgmt_connection, data_cport);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "activate_tx failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_PLAYBACK] = GBAUDIO_CODEC_PREPARE;
+		dev_dbg(module->dev, "Dynamic prepare %d DAI\n", data_cport);
+	}
+
+	return 0;
+}
+
+static int gbaudio_module_disable_tx(struct gbaudio_module_info *module, int id)
+{
+	int ret;
+	u16 data_cport, cportid, i2s_port;
+	int module_state;
+	struct gbaudio_data_connection *data;
+
+	/* find the dai */
+	data = find_data(module, id);
+	if (!data) {
+		dev_err(module->dev, "%d:DATA connection missing\n", id);
+		return -ENODEV;
+	}
+	module_state = data->state[SNDRV_PCM_STREAM_PLAYBACK];
+
+	if (module_state > GBAUDIO_CODEC_HWPARAMS) {
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_deactivate_tx(module->mgmt_connection,
+						data_cport);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "deactivate_tx failed:%d\n", ret);
+			return ret;
+		}
+		dev_dbg(module->dev, "Dynamic deactivate %d DAI\n", data_cport);
+		data->state[SNDRV_PCM_STREAM_PLAYBACK] = GBAUDIO_CODEC_HWPARAMS;
+	}
+
+	if (module_state > GBAUDIO_CODEC_SHUTDOWN) {
+		i2s_port = 0;	/* fixed for now */
+		cportid = data->connection->hd_cport_id;
+		ret = gb_audio_apbridgea_unregister_cport(data->connection,
+							  i2s_port, cportid,
+							  AUDIO_APBRIDGEA_DIRECTION_TX);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "unregister_cport failed:%d\n", ret);
+			return ret;
+		}
+		dev_dbg(module->dev, "Dynamic Unregister %d DAI\n", cportid);
+		data->state[SNDRV_PCM_STREAM_PLAYBACK] = GBAUDIO_CODEC_SHUTDOWN;
+	}
+
+	return 0;
+}
+
+static int gbaudio_module_enable_rx(struct gbaudio_codec_info *codec,
+				    struct gbaudio_module_info *module, int id)
+{
+	int module_state, ret = 0;
+	u16 data_cport, i2s_port, cportid;
+	u8 sig_bits, channels;
+	u32 format, rate;
+	struct gbaudio_data_connection *data;
+	struct gbaudio_stream_params *params;
+
+	/* find the dai */
+	data = find_data(module, id);
+	if (!data) {
+		dev_err(module->dev, "%d:DATA connection missing\n", id);
+		return -ENODEV;
+	}
+	module_state = data->state[SNDRV_PCM_STREAM_CAPTURE];
+
+	params = find_dai_stream_params(codec, id, SNDRV_PCM_STREAM_CAPTURE);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		return -EINVAL;
+	}
+
+	/* register cport */
+	if (module_state < GBAUDIO_CODEC_STARTUP) {
+		i2s_port = 0;	/* fixed for now */
+		cportid = data->connection->hd_cport_id;
+		ret = gb_audio_apbridgea_register_cport(data->connection,
+							i2s_port, cportid,
+							AUDIO_APBRIDGEA_DIRECTION_RX);
+		if (ret) {
+			dev_err_ratelimited(module->dev, "reg_cport failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_CAPTURE] = GBAUDIO_CODEC_STARTUP;
+		dev_dbg(module->dev, "Dynamic Register %d DAI\n", cportid);
+	}
+
+	/* hw_params */
+	if (module_state < GBAUDIO_CODEC_HWPARAMS) {
+		format = params->format;
+		channels = params->channels;
+		rate = params->rate;
+		sig_bits = params->sig_bits;
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_set_pcm(module->mgmt_connection, data_cport,
+					  format, rate, channels, sig_bits);
+		if (ret) {
+			dev_err_ratelimited(module->dev, "set_pcm failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_CAPTURE] = GBAUDIO_CODEC_HWPARAMS;
+		dev_dbg(module->dev, "Dynamic hw_params %d DAI\n", data_cport);
+	}
+
+	/* prepare */
+	if (module_state < GBAUDIO_CODEC_PREPARE) {
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_set_rx_data_size(module->mgmt_connection,
+						   data_cport, 192);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "set_rx_data_size failed:%d\n",
+					    ret);
+			return ret;
+		}
+		ret = gb_audio_gb_activate_rx(module->mgmt_connection,
+					      data_cport);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "activate_rx failed:%d\n", ret);
+			return ret;
+		}
+		data->state[SNDRV_PCM_STREAM_CAPTURE] = GBAUDIO_CODEC_PREPARE;
+		dev_dbg(module->dev, "Dynamic prepare %d DAI\n", data_cport);
+	}
+
+	return 0;
+}
+
+static int gbaudio_module_disable_rx(struct gbaudio_module_info *module, int id)
+{
+	int ret;
+	u16 data_cport, cportid, i2s_port;
+	int module_state;
+	struct gbaudio_data_connection *data;
+
+	/* find the dai */
+	data = find_data(module, id);
+	if (!data) {
+		dev_err(module->dev, "%d:DATA connection missing\n", id);
+		return -ENODEV;
+	}
+	module_state = data->state[SNDRV_PCM_STREAM_CAPTURE];
+
+	if (module_state > GBAUDIO_CODEC_HWPARAMS) {
+		data_cport = data->connection->intf_cport_id;
+		ret = gb_audio_gb_deactivate_rx(module->mgmt_connection,
+						data_cport);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "deactivate_rx failed:%d\n", ret);
+			return ret;
+		}
+		dev_dbg(module->dev, "Dynamic deactivate %d DAI\n", data_cport);
+		data->state[SNDRV_PCM_STREAM_CAPTURE] = GBAUDIO_CODEC_HWPARAMS;
+	}
+
+	if (module_state > GBAUDIO_CODEC_SHUTDOWN) {
+		i2s_port = 0;	/* fixed for now */
+		cportid = data->connection->hd_cport_id;
+		ret = gb_audio_apbridgea_unregister_cport(data->connection,
+							  i2s_port, cportid,
+							  AUDIO_APBRIDGEA_DIRECTION_RX);
+		if (ret) {
+			dev_err_ratelimited(module->dev,
+					    "unregister_cport failed:%d\n", ret);
+			return ret;
+		}
+		dev_dbg(module->dev, "Dynamic Unregister %d DAI\n", cportid);
+		data->state[SNDRV_PCM_STREAM_CAPTURE] = GBAUDIO_CODEC_SHUTDOWN;
+	}
+
+	return 0;
+}
+
+int gbaudio_module_update(struct gbaudio_codec_info *codec,
+			  struct snd_soc_dapm_widget *w,
+			  struct gbaudio_module_info *module, int enable)
+{
+	int dai_id, ret;
+	char intf_name[NAME_SIZE], dir[NAME_SIZE];
+
+	dev_dbg(module->dev, "%s:Module update %s sequence\n", w->name,
+		enable ? "Enable" : "Disable");
+
+	if ((w->id != snd_soc_dapm_aif_in) && (w->id != snd_soc_dapm_aif_out)) {
+		dev_dbg(codec->dev, "No action required for %s\n", w->name);
+		return 0;
+	}
+
+	/* parse dai_id from AIF widget's stream_name */
+	ret = sscanf(w->sname, "%s %d %s", intf_name, &dai_id, dir);
+	if (ret < 3) {
+		dev_err(codec->dev, "Error while parsing dai_id for %s\n", w->name);
+		return -EINVAL;
+	}
+
+	mutex_lock(&codec->lock);
+	if (w->id == snd_soc_dapm_aif_in) {
+		if (enable)
+			ret = gbaudio_module_enable_tx(codec, module, dai_id);
+		else
+			ret = gbaudio_module_disable_tx(module, dai_id);
+	} else if (w->id == snd_soc_dapm_aif_out) {
+		if (enable)
+			ret = gbaudio_module_enable_rx(codec, module, dai_id);
+		else
+			ret = gbaudio_module_disable_rx(module, dai_id);
+	}
+
+	mutex_unlock(&codec->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(gbaudio_module_update);
+
+/*
+ * codec DAI ops
+ */
+static int gbcodec_startup(struct snd_pcm_substream *substream,
+			   struct snd_soc_dai *dai)
+{
+	struct gbaudio_codec_info *codec = dev_get_drvdata(dai->dev);
+	struct gbaudio_stream_params *params;
+
+	mutex_lock(&codec->lock);
+
+	if (list_empty(&codec->module_list)) {
+		dev_err(codec->dev, "No codec module available\n");
+		mutex_unlock(&codec->lock);
+		return -ENODEV;
+	}
+
+	params = find_dai_stream_params(codec, dai->id, substream->stream);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+	params->state = GBAUDIO_CODEC_STARTUP;
+	mutex_unlock(&codec->lock);
+	/* to prevent suspend in case of active audio */
+	pm_stay_awake(dai->dev);
+
+	return 0;
+}
+
+static void gbcodec_shutdown(struct snd_pcm_substream *substream,
+			     struct snd_soc_dai *dai)
+{
+	struct gbaudio_codec_info *codec = dev_get_drvdata(dai->dev);
+	struct gbaudio_stream_params *params;
+
+	mutex_lock(&codec->lock);
+
+	if (list_empty(&codec->module_list))
+		dev_info(codec->dev, "No codec module available during shutdown\n");
+
+	params = find_dai_stream_params(codec, dai->id, substream->stream);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		mutex_unlock(&codec->lock);
+		return;
+	}
+	params->state = GBAUDIO_CODEC_SHUTDOWN;
+	mutex_unlock(&codec->lock);
+	pm_relax(dai->dev);
+}
+
+static int gbcodec_hw_params(struct snd_pcm_substream *substream,
+			     struct snd_pcm_hw_params *hwparams,
+			     struct snd_soc_dai *dai)
+{
+	int ret;
+	u8 sig_bits, channels;
+	u32 format, rate;
+	struct gbaudio_module_info *module;
+	struct gbaudio_data_connection *data;
+	struct gb_bundle *bundle;
+	struct gbaudio_codec_info *codec = dev_get_drvdata(dai->dev);
+	struct gbaudio_stream_params *params;
+
+	mutex_lock(&codec->lock);
+
+	if (list_empty(&codec->module_list)) {
+		dev_err(codec->dev, "No codec module available\n");
+		mutex_unlock(&codec->lock);
+		return -ENODEV;
+	}
+
+	/*
+	 * assuming, currently only 48000 Hz, 16BIT_LE, stereo
+	 * is supported, validate params before configuring codec
+	 */
+	if (params_channels(hwparams) != 2) {
+		dev_err(dai->dev, "Invalid channel count:%d\n",
+			params_channels(hwparams));
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+	channels = params_channels(hwparams);
+
+	if (params_rate(hwparams) != 48000) {
+		dev_err(dai->dev, "Invalid sampling rate:%d\n",
+			params_rate(hwparams));
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+	rate = GB_AUDIO_PCM_RATE_48000;
+
+	if (params_format(hwparams) != SNDRV_PCM_FORMAT_S16_LE) {
+		dev_err(dai->dev, "Invalid format:%d\n", params_format(hwparams));
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+	format = GB_AUDIO_PCM_FMT_S16_LE;
+
+	/* find the data connection */
+	list_for_each_entry(module, &codec->module_list, list) {
+		data = find_data(module, dai->id);
+		if (data)
+			break;
+	}
+
+	if (!data) {
+		dev_err(dai->dev, "DATA connection missing\n");
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+
+	params = find_dai_stream_params(codec, dai->id, substream->stream);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+
+	bundle = to_gb_bundle(module->dev);
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret) {
+		mutex_unlock(&codec->lock);
+		return ret;
+	}
+
+	ret = gb_audio_apbridgea_set_config(data->connection, 0,
+					    AUDIO_APBRIDGEA_PCM_FMT_16,
+					    AUDIO_APBRIDGEA_PCM_RATE_48000,
+					    6144000);
+	if (ret) {
+		dev_err_ratelimited(dai->dev, "%d: Error during set_config\n",
+				    ret);
+		gb_pm_runtime_put_noidle(bundle);
+		mutex_unlock(&codec->lock);
+		return ret;
+	}
+
+	gb_pm_runtime_put_noidle(bundle);
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		sig_bits = dai->driver->playback.sig_bits;
+	else
+		sig_bits = dai->driver->capture.sig_bits;
+
+	params->state = GBAUDIO_CODEC_HWPARAMS;
+	params->format = format;
+	params->rate = rate;
+	params->channels = channels;
+	params->sig_bits = sig_bits;
+
+	mutex_unlock(&codec->lock);
+	return 0;
+}
+
+static int gbcodec_prepare(struct snd_pcm_substream *substream,
+			   struct snd_soc_dai *dai)
+{
+	int ret;
+	struct gbaudio_module_info *module = NULL, *iter;
+	struct gbaudio_data_connection *data;
+	struct gb_bundle *bundle;
+	struct gbaudio_codec_info *codec = dev_get_drvdata(dai->dev);
+	struct gbaudio_stream_params *params;
+
+	mutex_lock(&codec->lock);
+
+	if (list_empty(&codec->module_list)) {
+		dev_err(codec->dev, "No codec module available\n");
+		mutex_unlock(&codec->lock);
+		return -ENODEV;
+	}
+
+	list_for_each_entry(iter, &codec->module_list, list) {
+		/* find the dai */
+		data = find_data(iter, dai->id);
+		if (data) {
+			module = iter;
+			break;
+		}
+	}
+	if (!data) {
+		dev_err(dai->dev, "DATA connection missing\n");
+		mutex_unlock(&codec->lock);
+		return -ENODEV;
+	}
+
+	params = find_dai_stream_params(codec, dai->id, substream->stream);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+
+	bundle = to_gb_bundle(module->dev);
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret) {
+		mutex_unlock(&codec->lock);
+		return ret;
+	}
+
+	switch (substream->stream) {
+	case SNDRV_PCM_STREAM_PLAYBACK:
+		ret = gb_audio_apbridgea_set_tx_data_size(data->connection, 0, 192);
+		break;
+	case SNDRV_PCM_STREAM_CAPTURE:
+		ret = gb_audio_apbridgea_set_rx_data_size(data->connection, 0, 192);
+		break;
+	}
+	if (ret) {
+		gb_pm_runtime_put_noidle(bundle);
+		mutex_unlock(&codec->lock);
+		dev_err_ratelimited(dai->dev, "set_data_size failed:%d\n", ret);
+		return ret;
+	}
+
+	gb_pm_runtime_put_noidle(bundle);
+
+	params->state = GBAUDIO_CODEC_PREPARE;
+	mutex_unlock(&codec->lock);
+	return 0;
+}
+
+static int gbcodec_mute_stream(struct snd_soc_dai *dai, int mute, int stream)
+{
+	int ret;
+	struct gbaudio_data_connection *data;
+	struct gbaudio_module_info *module = NULL, *iter;
+	struct gb_bundle *bundle;
+	struct gbaudio_codec_info *codec = dev_get_drvdata(dai->dev);
+	struct gbaudio_stream_params *params;
+
+	dev_dbg(dai->dev, "Mute:%d, Direction:%s\n", mute,
+		stream ? "CAPTURE" : "PLAYBACK");
+
+	mutex_lock(&codec->lock);
+
+	params = find_dai_stream_params(codec, dai->id, stream);
+	if (!params) {
+		dev_err(codec->dev, "Failed to fetch dai_stream pointer\n");
+		mutex_unlock(&codec->lock);
+		return -EINVAL;
+	}
+
+	if (list_empty(&codec->module_list)) {
+		dev_err(codec->dev, "No codec module available\n");
+		if (mute) {
+			params->state = GBAUDIO_CODEC_STOP;
+			ret = 0;
+		} else {
+			ret = -ENODEV;
+		}
+		mutex_unlock(&codec->lock);
+		return ret;
+	}
+
+	list_for_each_entry(iter, &codec->module_list, list) {
+		/* find the dai */
+		data = find_data(iter, dai->id);
+		if (data) {
+			module = iter;
+			break;
+		}
+	}
+	if (!data) {
+		dev_err(dai->dev, "%s DATA connection missing\n",
+			dai->name);
+		mutex_unlock(&codec->lock);
+		return -ENODEV;
+	}
+
+	bundle = to_gb_bundle(module->dev);
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret) {
+		mutex_unlock(&codec->lock);
+		return ret;
+	}
+
+	if (!mute && !stream) {/* start playback */
+		ret = gb_audio_apbridgea_prepare_tx(data->connection, 0);
+		if (!ret)
+			ret = gb_audio_apbridgea_start_tx(data->connection, 0, 0);
+		params->state = GBAUDIO_CODEC_START;
+	} else if (!mute && stream) {/* start capture */
+		ret = gb_audio_apbridgea_prepare_rx(data->connection, 0);
+		if (!ret)
+			ret = gb_audio_apbridgea_start_rx(data->connection, 0);
+		params->state = GBAUDIO_CODEC_START;
+	} else if (mute && !stream) {/* stop playback */
+		ret = gb_audio_apbridgea_stop_tx(data->connection, 0);
+		if (!ret)
+			ret = gb_audio_apbridgea_shutdown_tx(data->connection, 0);
+		params->state = GBAUDIO_CODEC_STOP;
+	} else if (mute && stream) {/* stop capture */
+		ret = gb_audio_apbridgea_stop_rx(data->connection, 0);
+		if (!ret)
+			ret = gb_audio_apbridgea_shutdown_rx(data->connection, 0);
+		params->state = GBAUDIO_CODEC_STOP;
+	} else {
+		ret = -EINVAL;
+	}
+
+	if (ret)
+		dev_err_ratelimited(dai->dev,
+				    "%s:Error during %s %s stream:%d\n",
+				    module->name, mute ? "Mute" : "Unmute",
+				    stream ? "Capture" : "Playback", ret);
+
+	gb_pm_runtime_put_noidle(bundle);
+	mutex_unlock(&codec->lock);
+	return ret;
+}
+
+static const struct snd_soc_dai_ops gbcodec_dai_ops = {
+	.startup = gbcodec_startup,
+	.shutdown = gbcodec_shutdown,
+	.hw_params = gbcodec_hw_params,
+	.prepare = gbcodec_prepare,
+	.mute_stream = gbcodec_mute_stream,
+};
+
+static struct snd_soc_dai_driver gbaudio_dai[] = {
+	{
+		.name = "apb-i2s0",
+		.id = 0,
+		.playback = {
+			.stream_name = "I2S 0 Playback",
+			.rates = SNDRV_PCM_RATE_48000,
+			.formats = SNDRV_PCM_FMTBIT_S16_LE,
+			.rate_max = 48000,
+			.rate_min = 48000,
+			.channels_min = 1,
+			.channels_max = 2,
+			.sig_bits = 16,
+		},
+		.capture = {
+			.stream_name = "I2S 0 Capture",
+			.rates = SNDRV_PCM_RATE_48000,
+			.formats = SNDRV_PCM_FMTBIT_S16_LE,
+			.rate_max = 48000,
+			.rate_min = 48000,
+			.channels_min = 1,
+			.channels_max = 2,
+			.sig_bits = 16,
+		},
+		.ops = &gbcodec_dai_ops,
+	},
+};
+
+static int gbaudio_init_jack(struct gbaudio_module_info *module,
+			     struct snd_soc_card *card)
+{
+	int ret;
+	struct gbaudio_jack *jack, *n;
+	struct snd_soc_jack_pin *headset, *button;
+
+	if (!module->jack_mask)
+		return 0;
+
+	snprintf(module->jack_name, NAME_SIZE, "GB %d Headset Jack",
+		 module->dev_id);
+
+	headset = devm_kzalloc(module->dev, sizeof(*headset), GFP_KERNEL);
+	if (!headset)
+		return -ENOMEM;
+
+	headset->pin = module->jack_name;
+	headset->mask = module->jack_mask;
+	ret = snd_soc_card_jack_new_pins(card, module->jack_name,
+					 module->jack_mask,
+					 &module->headset.jack, headset, 1);
+	if (ret) {
+		dev_err(module->dev, "Failed to create new jack\n");
+		return ret;
+	}
+
+	/* Add to module's jack list */
+	list_add(&module->headset.list, &module->jack_list);
+
+	if (!module->button_mask)
+		return 0;
+
+	snprintf(module->button_name, NAME_SIZE, "GB %d Button Jack",
+		 module->dev_id);
+	button = devm_kzalloc(module->dev, sizeof(*button), GFP_KERNEL);
+	if (!button) {
+		ret = -ENOMEM;
+		goto free_jacks;
+	}
+
+	button->pin = module->button_name;
+	button->mask = module->button_mask;
+	ret = snd_soc_card_jack_new_pins(card, module->button_name,
+					 module->button_mask,
+					 &module->button.jack,
+					 button, 1);
+	if (ret) {
+		dev_err(module->dev, "Failed to create button jack\n");
+		goto free_jacks;
+	}
+
+	/* Add to module's jack list */
+	list_add(&module->button.list, &module->jack_list);
+
+	/*
+	 * Currently, max 4 buttons are supported with following key mapping
+	 * BTN_0 = KEY_MEDIA
+	 * BTN_1 = KEY_VOICECOMMAND
+	 * BTN_2 = KEY_VOLUMEUP
+	 * BTN_3 = KEY_VOLUMEDOWN
+	 */
+
+	if (module->button_mask & SND_JACK_BTN_0) {
+		ret = snd_jack_set_key(module->button.jack.jack, SND_JACK_BTN_0,
+				       KEY_MEDIA);
+		if (ret) {
+			dev_err(module->dev, "Failed to set BTN_0\n");
+			goto free_jacks;
+		}
+	}
+
+	if (module->button_mask & SND_JACK_BTN_1) {
+		ret = snd_jack_set_key(module->button.jack.jack, SND_JACK_BTN_1,
+				       KEY_VOICECOMMAND);
+		if (ret) {
+			dev_err(module->dev, "Failed to set BTN_1\n");
+			goto free_jacks;
+		}
+	}
+
+	if (module->button_mask & SND_JACK_BTN_2) {
+		ret = snd_jack_set_key(module->button.jack.jack, SND_JACK_BTN_2,
+				       KEY_VOLUMEUP);
+		if (ret) {
+			dev_err(module->dev, "Failed to set BTN_2\n");
+			goto free_jacks;
+		}
+	}
+
+	if (module->button_mask & SND_JACK_BTN_3) {
+		ret = snd_jack_set_key(module->button.jack.jack, SND_JACK_BTN_3,
+				       KEY_VOLUMEDOWN);
+		if (ret) {
+			dev_err(module->dev, "Failed to set BTN_0\n");
+			goto free_jacks;
+		}
+	}
+
+	/* FIXME
+	 * verify if this is really required
+	set_bit(INPUT_PROP_NO_DUMMY_RELEASE,
+		module->button.jack.jack->input_dev->propbit);
+	*/
+
+	return 0;
+
+free_jacks:
+	list_for_each_entry_safe(jack, n, &module->jack_list, list) {
+		snd_device_free(card->snd_card, jack->jack.jack);
+		list_del(&jack->list);
+	}
+
+	return ret;
+}
+
+int gbaudio_register_module(struct gbaudio_module_info *module)
+{
+	int ret;
+	struct snd_soc_component *comp;
+	struct snd_card *card;
+	struct gbaudio_jack *jack = NULL;
+
+	if (!gbcodec) {
+		dev_err(module->dev, "GB Codec not yet probed\n");
+		return -EAGAIN;
+	}
+
+	comp = gbcodec->component;
+	card = comp->card->snd_card;
+
+	down_write(&card->controls_rwsem);
+
+	if (module->num_dais) {
+		dev_err(gbcodec->dev,
+			"%d:DAIs not supported via gbcodec driver\n",
+			module->num_dais);
+		up_write(&card->controls_rwsem);
+		return -EINVAL;
+	}
+
+	ret = gbaudio_init_jack(module, comp->card);
+	if (ret) {
+		up_write(&card->controls_rwsem);
+		return ret;
+	}
+
+	if (module->dapm_widgets)
+		snd_soc_dapm_new_controls(&comp->dapm, module->dapm_widgets,
+					  module->num_dapm_widgets);
+	if (module->controls)
+		snd_soc_add_component_controls(comp, module->controls,
+					       module->num_controls);
+	if (module->dapm_routes)
+		snd_soc_dapm_add_routes(&comp->dapm, module->dapm_routes,
+					module->num_dapm_routes);
+
+	/* card already instantiated, create widgets here only */
+	if (comp->card->instantiated) {
+		gbaudio_dapm_link_component_dai_widgets(comp->card, &comp->dapm);
+#ifdef CONFIG_SND_JACK
+		/*
+		 * register jack devices for this module
+		 * from codec->jack_list
+		 */
+		list_for_each_entry(jack, &module->jack_list, list) {
+			snd_device_register(comp->card->snd_card,
+					    jack->jack.jack);
+		}
+#endif
+	}
+
+	mutex_lock(&gbcodec->lock);
+	list_add(&module->list, &gbcodec->module_list);
+	mutex_unlock(&gbcodec->lock);
+
+	if (comp->card->instantiated)
+		ret = snd_soc_dapm_new_widgets(comp->card);
+	dev_dbg(comp->dev, "Registered %s module\n", module->name);
+
+	up_write(&card->controls_rwsem);
+	return ret;
+}
+EXPORT_SYMBOL(gbaudio_register_module);
+
+static void gbaudio_codec_clean_data_tx(struct gbaudio_data_connection *data)
+{
+	u16 i2s_port, cportid;
+	int ret;
+
+	if (list_is_singular(&gbcodec->module_list)) {
+		ret = gb_audio_apbridgea_stop_tx(data->connection, 0);
+		if (ret)
+			return;
+		ret = gb_audio_apbridgea_shutdown_tx(data->connection, 0);
+		if (ret)
+			return;
+	}
+	i2s_port = 0;	/* fixed for now */
+	cportid = data->connection->hd_cport_id;
+	ret = gb_audio_apbridgea_unregister_cport(data->connection,
+						  i2s_port, cportid,
+						  AUDIO_APBRIDGEA_DIRECTION_TX);
+	data->state[0] = GBAUDIO_CODEC_SHUTDOWN;
+}
+
+static void gbaudio_codec_clean_data_rx(struct gbaudio_data_connection *data)
+{
+	u16 i2s_port, cportid;
+	int ret;
+
+	if (list_is_singular(&gbcodec->module_list)) {
+		ret = gb_audio_apbridgea_stop_rx(data->connection, 0);
+		if (ret)
+			return;
+		ret = gb_audio_apbridgea_shutdown_rx(data->connection, 0);
+		if (ret)
+			return;
+	}
+	i2s_port = 0;	/* fixed for now */
+	cportid = data->connection->hd_cport_id;
+	ret = gb_audio_apbridgea_unregister_cport(data->connection,
+						  i2s_port, cportid,
+						  AUDIO_APBRIDGEA_DIRECTION_RX);
+	data->state[1] = GBAUDIO_CODEC_SHUTDOWN;
+}
+
+static void gbaudio_codec_cleanup(struct gbaudio_module_info *module)
+{
+	struct gbaudio_data_connection *data;
+	int pb_state, cap_state;
+
+	dev_dbg(gbcodec->dev, "%s: removed, cleanup APBridge\n", module->name);
+	list_for_each_entry(data, &module->data_list, list) {
+		pb_state = data->state[0];
+		cap_state = data->state[1];
+
+		if (pb_state > GBAUDIO_CODEC_SHUTDOWN)
+			gbaudio_codec_clean_data_tx(data);
+
+		if (cap_state > GBAUDIO_CODEC_SHUTDOWN)
+			gbaudio_codec_clean_data_rx(data);
+	}
+}
+
+void gbaudio_unregister_module(struct gbaudio_module_info *module)
+{
+	struct snd_soc_component *comp = gbcodec->component;
+	struct snd_card *card = comp->card->snd_card;
+	struct gbaudio_jack *jack, *n;
+	int mask;
+
+	dev_dbg(comp->dev, "Unregister %s module\n", module->name);
+
+	down_write(&card->controls_rwsem);
+	mutex_lock(&gbcodec->lock);
+	gbaudio_codec_cleanup(module);
+	list_del(&module->list);
+	dev_dbg(comp->dev, "Process Unregister %s module\n", module->name);
+	mutex_unlock(&gbcodec->lock);
+
+#ifdef CONFIG_SND_JACK
+	/* free jack devices for this module jack_list */
+	list_for_each_entry_safe(jack, n, &module->jack_list, list) {
+		if (jack == &module->headset)
+			mask = GBCODEC_JACK_MASK;
+		else if (jack == &module->button)
+			mask = GBCODEC_JACK_BUTTON_MASK;
+		else
+			mask = 0;
+		if (mask) {
+			dev_dbg(module->dev, "Report %s removal\n",
+				jack->jack.jack->id);
+			snd_soc_jack_report(&jack->jack, 0, mask);
+			snd_device_free(comp->card->snd_card,
+					jack->jack.jack);
+			list_del(&jack->list);
+		}
+	}
+#endif
+
+	if (module->dapm_routes) {
+		dev_dbg(comp->dev, "Removing %d routes\n",
+			module->num_dapm_routes);
+		snd_soc_dapm_del_routes(&comp->dapm, module->dapm_routes,
+					module->num_dapm_routes);
+	}
+	if (module->controls) {
+		dev_dbg(comp->dev, "Removing %d controls\n",
+			module->num_controls);
+		/* release control semaphore */
+		up_write(&card->controls_rwsem);
+		gbaudio_remove_component_controls(comp, module->controls,
+						  module->num_controls);
+		down_write(&card->controls_rwsem);
+	}
+	if (module->dapm_widgets) {
+		dev_dbg(comp->dev, "Removing %d widgets\n",
+			module->num_dapm_widgets);
+		gbaudio_dapm_free_controls(&comp->dapm, module->dapm_widgets,
+					   module->num_dapm_widgets);
+	}
+
+	dev_dbg(comp->dev, "Unregistered %s module\n", module->name);
+
+	up_write(&card->controls_rwsem);
+}
+EXPORT_SYMBOL(gbaudio_unregister_module);
+
+/*
+ * component driver ops
+ */
+static int gbcodec_probe(struct snd_soc_component *comp)
+{
+	int i;
+	struct gbaudio_codec_info *info;
+	struct gbaudio_codec_dai *dai;
+
+	info = devm_kzalloc(comp->dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->dev = comp->dev;
+	INIT_LIST_HEAD(&info->module_list);
+	mutex_init(&info->lock);
+	INIT_LIST_HEAD(&info->dai_list);
+
+	/* init dai_list used to maintain runtime stream info */
+	for (i = 0; i < ARRAY_SIZE(gbaudio_dai); i++) {
+		dai = devm_kzalloc(comp->dev, sizeof(*dai), GFP_KERNEL);
+		if (!dai)
+			return -ENOMEM;
+		dai->id = gbaudio_dai[i].id;
+		list_add(&dai->list, &info->dai_list);
+	}
+
+	info->component = comp;
+	snd_soc_component_set_drvdata(comp, info);
+	gbcodec = info;
+
+	device_init_wakeup(comp->dev, 1);
+	return 0;
+}
+
+static int gbcodec_write(struct snd_soc_component *comp, unsigned int reg,
+			 unsigned int value)
+{
+	return 0;
+}
+
+static unsigned int gbcodec_read(struct snd_soc_component *comp,
+				 unsigned int reg)
+{
+	return 0;
+}
+
+static const struct snd_soc_component_driver soc_codec_dev_gbaudio = {
+	.probe	= gbcodec_probe,
+	.read = gbcodec_read,
+	.write = gbcodec_write,
+};
+
+#ifdef CONFIG_PM
+static int gbaudio_codec_suspend(struct device *dev)
+{
+	dev_dbg(dev, "%s: suspend\n", __func__);
+	return 0;
+}
+
+static int gbaudio_codec_resume(struct device *dev)
+{
+	dev_dbg(dev, "%s: resume\n", __func__);
+	return 0;
+}
+
+static const struct dev_pm_ops gbaudio_codec_pm_ops = {
+	.suspend	= gbaudio_codec_suspend,
+	.resume		= gbaudio_codec_resume,
+};
+#endif
+
+static int gbaudio_codec_probe(struct platform_device *pdev)
+{
+	return devm_snd_soc_register_component(&pdev->dev,
+			&soc_codec_dev_gbaudio,
+			gbaudio_dai, ARRAY_SIZE(gbaudio_dai));
+}
+
+static int gbaudio_codec_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static const struct of_device_id greybus_asoc_machine_of_match[]  = {
+	{ .compatible = "toshiba,apb-dummy-codec", },
+	{},
+};
+
+static struct platform_driver gbaudio_codec_driver = {
+	.driver = {
+		.name = "apb-dummy-codec",
+#ifdef CONFIG_PM
+		.pm = &gbaudio_codec_pm_ops,
+#endif
+		.of_match_table = greybus_asoc_machine_of_match,
+	},
+	.probe = gbaudio_codec_probe,
+	.remove = gbaudio_codec_remove,
+};
+module_platform_driver(gbaudio_codec_driver);
+
+MODULE_DESCRIPTION("APBridge ALSA SoC dummy codec driver");
+MODULE_AUTHOR("Vaibhav Agarwal <vaibhav.agarwal@linaro.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:apb-dummy-codec");
diff --git a/drivers/staging/greybus/audio_codec.h b/drivers/staging/greybus/audio_codec.h
new file mode 100644
index 000000000..ce15e800e
--- /dev/null
+++ b/drivers/staging/greybus/audio_codec.h
@@ -0,0 +1,242 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus audio driver
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+
+#ifndef __LINUX_GBAUDIO_CODEC_H
+#define __LINUX_GBAUDIO_CODEC_H
+
+#include <linux/greybus.h>
+#include <sound/soc.h>
+#include <sound/jack.h>
+
+#define NAME_SIZE	32
+#define MAX_DAIS	2	/* APB1, APB2 */
+
+enum {
+	APB1_PCM = 0,
+	APB2_PCM,
+	NUM_CODEC_DAIS,
+};
+
+/*
+ * device_type should be same as defined in audio.h
+ * (Android media layer)
+ */
+enum {
+	GBAUDIO_DEVICE_NONE                     = 0x0,
+	/* reserved bits */
+	GBAUDIO_DEVICE_BIT_IN                   = 0x80000000,
+	GBAUDIO_DEVICE_BIT_DEFAULT              = 0x40000000,
+	/* output devices */
+	GBAUDIO_DEVICE_OUT_SPEAKER              = 0x2,
+	GBAUDIO_DEVICE_OUT_WIRED_HEADSET        = 0x4,
+	GBAUDIO_DEVICE_OUT_WIRED_HEADPHONE      = 0x8,
+	/* input devices */
+	GBAUDIO_DEVICE_IN_BUILTIN_MIC           = GBAUDIO_DEVICE_BIT_IN | 0x4,
+	GBAUDIO_DEVICE_IN_WIRED_HEADSET         = GBAUDIO_DEVICE_BIT_IN | 0x10,
+};
+
+#define GBCODEC_JACK_MASK		0x0000FFFF
+#define GBCODEC_JACK_BUTTON_MASK	0xFFFF0000
+
+enum gbaudio_codec_state {
+	GBAUDIO_CODEC_SHUTDOWN = 0,
+	GBAUDIO_CODEC_STARTUP,
+	GBAUDIO_CODEC_HWPARAMS,
+	GBAUDIO_CODEC_PREPARE,
+	GBAUDIO_CODEC_START,
+	GBAUDIO_CODEC_STOP,
+};
+
+struct gbaudio_stream_params {
+	int state;
+	u8 sig_bits, channels;
+	u32 format, rate;
+};
+
+struct gbaudio_codec_dai {
+	int id;
+	/* runtime params for playback/capture streams */
+	struct gbaudio_stream_params params[2];
+	struct list_head list;
+};
+
+struct gbaudio_codec_info {
+	struct device *dev;
+	struct snd_soc_component *component;
+	struct list_head module_list;
+	/* to maintain runtime stream params for each DAI */
+	struct list_head dai_list;
+	struct mutex lock;
+};
+
+struct gbaudio_widget {
+	__u8 id;
+	const char *name;
+	struct list_head list;
+};
+
+struct gbaudio_control {
+	__u8 id;
+	char *name;
+	char *wname;
+	const char * const *texts;
+	int items;
+	struct list_head list;
+};
+
+struct gbaudio_data_connection {
+	int id;
+	__le16 data_cport;
+	struct gb_connection *connection;
+	struct list_head list;
+	/* maintain runtime state for playback/capture stream */
+	int state[2];
+};
+
+/* stream direction */
+#define GB_PLAYBACK	BIT(0)
+#define GB_CAPTURE	BIT(1)
+
+enum gbaudio_module_state {
+	GBAUDIO_MODULE_OFF = 0,
+	GBAUDIO_MODULE_ON,
+};
+
+struct gbaudio_jack {
+	struct snd_soc_jack jack;
+	struct list_head list;
+};
+
+struct gbaudio_module_info {
+	/* module info */
+	struct device *dev;
+	int dev_id;	/* check if it should be bundle_id/hd_cport_id */
+	int vid;
+	int pid;
+	int type;
+	int set_uevent;
+	char vstr[NAME_SIZE];
+	char pstr[NAME_SIZE];
+	struct list_head list;
+	/* need to share this info to above user space */
+	int manager_id;
+	char name[NAME_SIZE];
+	unsigned int ip_devices;
+	unsigned int op_devices;
+
+	/* jack related */
+	char jack_name[NAME_SIZE];
+	char button_name[NAME_SIZE];
+	int jack_type;
+	int jack_mask;
+	int button_mask;
+	int button_status;
+	struct gbaudio_jack headset;
+	struct gbaudio_jack button;
+
+	/* connection info */
+	struct gb_connection *mgmt_connection;
+	size_t num_data_connections;
+	struct list_head data_list;
+
+	/* topology related */
+	int num_dais;
+	int num_controls;
+	int num_dapm_widgets;
+	int num_dapm_routes;
+	unsigned long dai_offset;
+	unsigned long widget_offset;
+	unsigned long control_offset;
+	unsigned long route_offset;
+	struct snd_kcontrol_new *controls;
+	struct snd_soc_dapm_widget *dapm_widgets;
+	struct snd_soc_dapm_route *dapm_routes;
+	struct snd_soc_dai_driver *dais;
+
+	struct list_head widget_list;
+	struct list_head ctl_list;
+	struct list_head widget_ctl_list;
+	struct list_head jack_list;
+
+	struct gb_audio_topology *topology;
+};
+
+int gbaudio_tplg_parse_data(struct gbaudio_module_info *module,
+			    struct gb_audio_topology *tplg_data);
+void gbaudio_tplg_release(struct gbaudio_module_info *module);
+
+int gbaudio_module_update(struct gbaudio_codec_info *codec,
+			  struct snd_soc_dapm_widget *w,
+			  struct gbaudio_module_info *module,
+			  int enable);
+int gbaudio_register_module(struct gbaudio_module_info *module);
+void gbaudio_unregister_module(struct gbaudio_module_info *module);
+
+/* protocol related */
+int gb_audio_gb_get_topology(struct gb_connection *connection,
+			     struct gb_audio_topology **topology);
+int gb_audio_gb_get_control(struct gb_connection *connection,
+			    u8 control_id, u8 index,
+			    struct gb_audio_ctl_elem_value *value);
+int gb_audio_gb_set_control(struct gb_connection *connection,
+			    u8 control_id, u8 index,
+			    struct gb_audio_ctl_elem_value *value);
+int gb_audio_gb_enable_widget(struct gb_connection *connection,
+			      u8 widget_id);
+int gb_audio_gb_disable_widget(struct gb_connection *connection,
+			       u8 widget_id);
+int gb_audio_gb_get_pcm(struct gb_connection *connection,
+			u16 data_cport, u32 *format,
+			u32 *rate, u8 *channels,
+			u8 *sig_bits);
+int gb_audio_gb_set_pcm(struct gb_connection *connection,
+			u16 data_cport, u32 format,
+			u32 rate, u8 channels,
+			u8 sig_bits);
+int gb_audio_gb_set_tx_data_size(struct gb_connection *connection,
+				 u16 data_cport, u16 size);
+int gb_audio_gb_activate_tx(struct gb_connection *connection,
+			    u16 data_cport);
+int gb_audio_gb_deactivate_tx(struct gb_connection *connection,
+			      u16 data_cport);
+int gb_audio_gb_set_rx_data_size(struct gb_connection *connection,
+				 u16 data_cport, u16 size);
+int gb_audio_gb_activate_rx(struct gb_connection *connection,
+			    u16 data_cport);
+int gb_audio_gb_deactivate_rx(struct gb_connection *connection,
+			      u16 data_cport);
+int gb_audio_apbridgea_set_config(struct gb_connection *connection,
+				  __u16 i2s_port, __u32 format,
+				  __u32 rate, __u32 mclk_freq);
+int gb_audio_apbridgea_register_cport(struct gb_connection *connection,
+				      __u16 i2s_port, __u16 cportid,
+				      __u8 direction);
+int gb_audio_apbridgea_unregister_cport(struct gb_connection *connection,
+					__u16 i2s_port, __u16 cportid,
+					__u8 direction);
+int gb_audio_apbridgea_set_tx_data_size(struct gb_connection *connection,
+					__u16 i2s_port, __u16 size);
+int gb_audio_apbridgea_prepare_tx(struct gb_connection *connection,
+				  __u16 i2s_port);
+int gb_audio_apbridgea_start_tx(struct gb_connection *connection,
+				__u16 i2s_port, __u64 timestamp);
+int gb_audio_apbridgea_stop_tx(struct gb_connection *connection,
+			       __u16 i2s_port);
+int gb_audio_apbridgea_shutdown_tx(struct gb_connection *connection,
+				   __u16 i2s_port);
+int gb_audio_apbridgea_set_rx_data_size(struct gb_connection *connection,
+					__u16 i2s_port, __u16 size);
+int gb_audio_apbridgea_prepare_rx(struct gb_connection *connection,
+				  __u16 i2s_port);
+int gb_audio_apbridgea_start_rx(struct gb_connection *connection,
+				__u16 i2s_port);
+int gb_audio_apbridgea_stop_rx(struct gb_connection *connection,
+			       __u16 i2s_port);
+int gb_audio_apbridgea_shutdown_rx(struct gb_connection *connection,
+				   __u16 i2s_port);
+
+#endif /* __LINUX_GBAUDIO_CODEC_H */
diff --git a/drivers/staging/greybus/audio_gb.c b/drivers/staging/greybus/audio_gb.c
new file mode 100644
index 000000000..9d8994fdb
--- /dev/null
+++ b/drivers/staging/greybus/audio_gb.c
@@ -0,0 +1,225 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Audio Device Class Protocol helpers
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#include <linux/greybus.h>
+#include "audio_codec.h"
+
+/* TODO: Split into separate calls */
+int gb_audio_gb_get_topology(struct gb_connection *connection,
+			     struct gb_audio_topology **topology)
+{
+	struct gb_audio_get_topology_size_response size_resp;
+	struct gb_audio_topology *topo;
+	u16 size;
+	int ret;
+
+	ret = gb_operation_sync(connection, GB_AUDIO_TYPE_GET_TOPOLOGY_SIZE,
+				NULL, 0, &size_resp, sizeof(size_resp));
+	if (ret)
+		return ret;
+
+	size = le16_to_cpu(size_resp.size);
+	if (size < sizeof(*topo))
+		return -ENODATA;
+
+	topo = kzalloc(size, GFP_KERNEL);
+	if (!topo)
+		return -ENOMEM;
+
+	ret = gb_operation_sync(connection, GB_AUDIO_TYPE_GET_TOPOLOGY, NULL, 0,
+				topo, size);
+	if (ret) {
+		kfree(topo);
+		return ret;
+	}
+
+	*topology = topo;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_get_topology);
+
+int gb_audio_gb_get_control(struct gb_connection *connection,
+			    u8 control_id, u8 index,
+			    struct gb_audio_ctl_elem_value *value)
+{
+	struct gb_audio_get_control_request req;
+	struct gb_audio_get_control_response resp;
+	int ret;
+
+	req.control_id = control_id;
+	req.index = index;
+
+	ret = gb_operation_sync(connection, GB_AUDIO_TYPE_GET_CONTROL,
+				&req, sizeof(req), &resp, sizeof(resp));
+	if (ret)
+		return ret;
+
+	memcpy(value, &resp.value, sizeof(*value));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_get_control);
+
+int gb_audio_gb_set_control(struct gb_connection *connection,
+			    u8 control_id, u8 index,
+			    struct gb_audio_ctl_elem_value *value)
+{
+	struct gb_audio_set_control_request req;
+
+	req.control_id = control_id;
+	req.index = index;
+	memcpy(&req.value, value, sizeof(req.value));
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_SET_CONTROL,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_set_control);
+
+int gb_audio_gb_enable_widget(struct gb_connection *connection,
+			      u8 widget_id)
+{
+	struct gb_audio_enable_widget_request req;
+
+	req.widget_id = widget_id;
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_ENABLE_WIDGET,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_enable_widget);
+
+int gb_audio_gb_disable_widget(struct gb_connection *connection,
+			       u8 widget_id)
+{
+	struct gb_audio_disable_widget_request req;
+
+	req.widget_id = widget_id;
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_DISABLE_WIDGET,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_disable_widget);
+
+int gb_audio_gb_get_pcm(struct gb_connection *connection, u16 data_cport,
+			u32 *format, u32 *rate, u8 *channels,
+			u8 *sig_bits)
+{
+	struct gb_audio_get_pcm_request req;
+	struct gb_audio_get_pcm_response resp;
+	int ret;
+
+	req.data_cport = cpu_to_le16(data_cport);
+
+	ret = gb_operation_sync(connection, GB_AUDIO_TYPE_GET_PCM,
+				&req, sizeof(req), &resp, sizeof(resp));
+	if (ret)
+		return ret;
+
+	*format = le32_to_cpu(resp.format);
+	*rate = le32_to_cpu(resp.rate);
+	*channels = resp.channels;
+	*sig_bits = resp.sig_bits;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_get_pcm);
+
+int gb_audio_gb_set_pcm(struct gb_connection *connection, u16 data_cport,
+			u32 format, u32 rate, u8 channels,
+			u8 sig_bits)
+{
+	struct gb_audio_set_pcm_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+	req.format = cpu_to_le32(format);
+	req.rate = cpu_to_le32(rate);
+	req.channels = channels;
+	req.sig_bits = sig_bits;
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_SET_PCM,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_set_pcm);
+
+int gb_audio_gb_set_tx_data_size(struct gb_connection *connection,
+				 u16 data_cport, u16 size)
+{
+	struct gb_audio_set_tx_data_size_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+	req.size = cpu_to_le16(size);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_SET_TX_DATA_SIZE,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_set_tx_data_size);
+
+int gb_audio_gb_activate_tx(struct gb_connection *connection,
+			    u16 data_cport)
+{
+	struct gb_audio_activate_tx_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_ACTIVATE_TX,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_activate_tx);
+
+int gb_audio_gb_deactivate_tx(struct gb_connection *connection,
+			      u16 data_cport)
+{
+	struct gb_audio_deactivate_tx_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_DEACTIVATE_TX,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_deactivate_tx);
+
+int gb_audio_gb_set_rx_data_size(struct gb_connection *connection,
+				 u16 data_cport, u16 size)
+{
+	struct gb_audio_set_rx_data_size_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+	req.size = cpu_to_le16(size);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_SET_RX_DATA_SIZE,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_set_rx_data_size);
+
+int gb_audio_gb_activate_rx(struct gb_connection *connection,
+			    u16 data_cport)
+{
+	struct gb_audio_activate_rx_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_ACTIVATE_RX,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_activate_rx);
+
+int gb_audio_gb_deactivate_rx(struct gb_connection *connection,
+			      u16 data_cport)
+{
+	struct gb_audio_deactivate_rx_request req;
+
+	req.data_cport = cpu_to_le16(data_cport);
+
+	return gb_operation_sync(connection, GB_AUDIO_TYPE_DEACTIVATE_RX,
+				 &req, sizeof(req), NULL, 0);
+}
+EXPORT_SYMBOL_GPL(gb_audio_gb_deactivate_rx);
+
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("greybus:audio-gb");
+MODULE_DESCRIPTION("Greybus Audio Device Class Protocol library");
+MODULE_AUTHOR("Mark Greer <mgreer@animalcreek.com>");
diff --git a/drivers/staging/greybus/audio_helper.c b/drivers/staging/greybus/audio_helper.c
new file mode 100644
index 000000000..223987616
--- /dev/null
+++ b/drivers/staging/greybus/audio_helper.c
@@ -0,0 +1,191 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Audio Sound SoC helper APIs
+ */
+
+#include <sound/core.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include "audio_helper.h"
+
+#define gbaudio_dapm_for_each_direction(dir) \
+	for ((dir) = SND_SOC_DAPM_DIR_IN; (dir) <= SND_SOC_DAPM_DIR_OUT; \
+		(dir)++)
+
+static void gbaudio_dapm_link_dai_widget(struct snd_soc_dapm_widget *dai_w,
+					 struct snd_soc_card *card)
+{
+	struct snd_soc_dapm_widget *w;
+	struct snd_soc_dapm_widget *src, *sink;
+	struct snd_soc_dai *dai = dai_w->priv;
+
+	/* ...find all widgets with the same stream and link them */
+	list_for_each_entry(w, &card->widgets, list) {
+		if (w->dapm != dai_w->dapm)
+			continue;
+
+		switch (w->id) {
+		case snd_soc_dapm_dai_in:
+		case snd_soc_dapm_dai_out:
+			continue;
+		default:
+			break;
+		}
+
+		if (!w->sname || !strstr(w->sname, dai_w->sname))
+			continue;
+
+		/*
+		 * check if widget is already linked,
+		 * if (w->linked)
+		 *	return;
+		 */
+
+		if (dai_w->id == snd_soc_dapm_dai_in) {
+			src = dai_w;
+			sink = w;
+		} else {
+			src = w;
+			sink = dai_w;
+		}
+		dev_dbg(dai->dev, "%s -> %s\n", src->name, sink->name);
+		/* Add the DAPM path and set widget's linked status
+		 * snd_soc_dapm_add_path(w->dapm, src, sink, NULL, NULL);
+		 * w->linked = 1;
+		 */
+	}
+}
+
+int gbaudio_dapm_link_component_dai_widgets(struct snd_soc_card *card,
+					    struct snd_soc_dapm_context *dapm)
+{
+	struct snd_soc_dapm_widget *dai_w;
+
+	/* For each DAI widget... */
+	list_for_each_entry(dai_w, &card->widgets, list) {
+		if (dai_w->dapm != dapm)
+			continue;
+		switch (dai_w->id) {
+		case snd_soc_dapm_dai_in:
+		case snd_soc_dapm_dai_out:
+			break;
+		default:
+			continue;
+		}
+		gbaudio_dapm_link_dai_widget(dai_w, card);
+	}
+
+	return 0;
+}
+
+static void gbaudio_dapm_free_path(struct snd_soc_dapm_path *path)
+{
+	list_del(&path->list_node[SND_SOC_DAPM_DIR_IN]);
+	list_del(&path->list_node[SND_SOC_DAPM_DIR_OUT]);
+	list_del(&path->list_kcontrol);
+	list_del(&path->list);
+	kfree(path);
+}
+
+static void gbaudio_dapm_free_widget(struct snd_soc_dapm_widget *w)
+{
+	struct snd_soc_dapm_path *p, *next_p;
+	enum snd_soc_dapm_direction dir;
+
+	list_del(&w->list);
+	/*
+	 * remove source and sink paths associated to this widget.
+	 * While removing the path, remove reference to it from both
+	 * source and sink widgets so that path is removed only once.
+	 */
+	gbaudio_dapm_for_each_direction(dir) {
+		snd_soc_dapm_widget_for_each_path_safe(w, dir, p, next_p)
+			gbaudio_dapm_free_path(p);
+	}
+
+	kfree(w->kcontrols);
+	kfree_const(w->name);
+	kfree_const(w->sname);
+	kfree(w);
+}
+
+int gbaudio_dapm_free_controls(struct snd_soc_dapm_context *dapm,
+			       const struct snd_soc_dapm_widget *widget,
+			       int num)
+{
+	int i;
+	struct snd_soc_dapm_widget *w, *tmp_w;
+
+	mutex_lock(&dapm->card->dapm_mutex);
+	for (i = 0; i < num; i++) {
+		/* below logic can be optimized to identify widget pointer */
+		w = NULL;
+		list_for_each_entry(tmp_w, &dapm->card->widgets, list) {
+			if (tmp_w->dapm == dapm &&
+			    !strcmp(tmp_w->name, widget->name)) {
+				w = tmp_w;
+				break;
+			}
+		}
+		if (!w) {
+			dev_err(dapm->dev, "%s: widget not found\n",
+				widget->name);
+			widget++;
+			continue;
+		}
+		widget++;
+		gbaudio_dapm_free_widget(w);
+	}
+	mutex_unlock(&dapm->card->dapm_mutex);
+	return 0;
+}
+
+static int gbaudio_remove_controls(struct snd_card *card, struct device *dev,
+				   const struct snd_kcontrol_new *controls,
+				   int num_controls, const char *prefix)
+{
+	int i, err;
+
+	for (i = 0; i < num_controls; i++) {
+		const struct snd_kcontrol_new *control = &controls[i];
+		struct snd_ctl_elem_id id;
+		struct snd_kcontrol *kctl;
+
+		if (prefix)
+			snprintf(id.name, sizeof(id.name), "%s %s", prefix,
+				 control->name);
+		else
+			strscpy(id.name, control->name, sizeof(id.name));
+		id.numid = 0;
+		id.iface = control->iface;
+		id.device = control->device;
+		id.subdevice = control->subdevice;
+		id.index = control->index;
+		kctl = snd_ctl_find_id(card, &id);
+		if (!kctl) {
+			dev_err(dev, "Failed to find %s\n", control->name);
+			continue;
+		}
+		err = snd_ctl_remove(card, kctl);
+		if (err < 0) {
+			dev_err(dev, "%d: Failed to remove %s\n", err,
+				control->name);
+			continue;
+		}
+	}
+	return 0;
+}
+
+int gbaudio_remove_component_controls(struct snd_soc_component *component,
+				      const struct snd_kcontrol_new *controls,
+				      unsigned int num_controls)
+{
+	struct snd_card *card = component->card->snd_card;
+	int err;
+
+	down_write(&card->controls_rwsem);
+	err = gbaudio_remove_controls(card, component->dev, controls,
+				      num_controls, component->name_prefix);
+	up_write(&card->controls_rwsem);
+	return err;
+}
diff --git a/drivers/staging/greybus/audio_helper.h b/drivers/staging/greybus/audio_helper.h
new file mode 100644
index 000000000..5cf1c6d7d
--- /dev/null
+++ b/drivers/staging/greybus/audio_helper.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus Audio Sound SoC helper APIs
+ */
+
+#ifndef __LINUX_GBAUDIO_HELPER_H
+#define __LINUX_GBAUDIO_HELPER_H
+
+int gbaudio_dapm_link_component_dai_widgets(struct snd_soc_card *card,
+					    struct snd_soc_dapm_context *dapm);
+int gbaudio_dapm_free_controls(struct snd_soc_dapm_context *dapm,
+			       const struct snd_soc_dapm_widget *widget,
+			       int num);
+int gbaudio_remove_component_controls(struct snd_soc_component *component,
+				      const struct snd_kcontrol_new *controls,
+				      unsigned int num_controls);
+#endif
diff --git a/drivers/staging/greybus/audio_manager.c b/drivers/staging/greybus/audio_manager.c
new file mode 100644
index 000000000..9a3f7c034
--- /dev/null
+++ b/drivers/staging/greybus/audio_manager.c
@@ -0,0 +1,186 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus operations
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#include <linux/string.h>
+#include <linux/sysfs.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/idr.h>
+
+#include "audio_manager.h"
+#include "audio_manager_private.h"
+
+static struct kset *manager_kset;
+
+static LIST_HEAD(modules_list);
+static DECLARE_RWSEM(modules_rwsem);
+static DEFINE_IDA(module_id);
+
+/* helpers */
+static struct gb_audio_manager_module *gb_audio_manager_get_locked(int id)
+{
+	struct gb_audio_manager_module *module;
+
+	if (id < 0)
+		return NULL;
+
+	list_for_each_entry(module, &modules_list, list) {
+		if (module->id == id)
+			return module;
+	}
+
+	return NULL;
+}
+
+/* public API */
+int gb_audio_manager_add(struct gb_audio_manager_module_descriptor *desc)
+{
+	struct gb_audio_manager_module *module;
+	int id;
+	int err;
+
+	id = ida_simple_get(&module_id, 0, 0, GFP_KERNEL);
+	if (id < 0)
+		return id;
+
+	err = gb_audio_manager_module_create(&module, manager_kset,
+					     id, desc);
+	if (err) {
+		ida_simple_remove(&module_id, id);
+		return err;
+	}
+
+	/* Add it to the list */
+	down_write(&modules_rwsem);
+	list_add_tail(&module->list, &modules_list);
+	up_write(&modules_rwsem);
+
+	return module->id;
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_add);
+
+int gb_audio_manager_remove(int id)
+{
+	struct gb_audio_manager_module *module;
+
+	down_write(&modules_rwsem);
+
+	module = gb_audio_manager_get_locked(id);
+	if (!module) {
+		up_write(&modules_rwsem);
+		return -EINVAL;
+	}
+	list_del(&module->list);
+	kobject_put(&module->kobj);
+	up_write(&modules_rwsem);
+	ida_simple_remove(&module_id, id);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_remove);
+
+void gb_audio_manager_remove_all(void)
+{
+	struct gb_audio_manager_module *module, *next;
+	int is_empty;
+
+	down_write(&modules_rwsem);
+
+	list_for_each_entry_safe(module, next, &modules_list, list) {
+		list_del(&module->list);
+		ida_simple_remove(&module_id, module->id);
+		kobject_put(&module->kobj);
+	}
+
+	is_empty = list_empty(&modules_list);
+
+	up_write(&modules_rwsem);
+
+	if (!is_empty)
+		pr_warn("Not all nodes were deleted\n");
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_remove_all);
+
+struct gb_audio_manager_module *gb_audio_manager_get_module(int id)
+{
+	struct gb_audio_manager_module *module;
+
+	down_read(&modules_rwsem);
+	module = gb_audio_manager_get_locked(id);
+	kobject_get(&module->kobj);
+	up_read(&modules_rwsem);
+	return module;
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_get_module);
+
+void gb_audio_manager_put_module(struct gb_audio_manager_module *module)
+{
+	kobject_put(&module->kobj);
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_put_module);
+
+int gb_audio_manager_dump_module(int id)
+{
+	struct gb_audio_manager_module *module;
+
+	down_read(&modules_rwsem);
+	module = gb_audio_manager_get_locked(id);
+	up_read(&modules_rwsem);
+
+	if (!module)
+		return -EINVAL;
+
+	gb_audio_manager_module_dump(module);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_dump_module);
+
+void gb_audio_manager_dump_all(void)
+{
+	struct gb_audio_manager_module *module;
+	int count = 0;
+
+	down_read(&modules_rwsem);
+	list_for_each_entry(module, &modules_list, list) {
+		gb_audio_manager_module_dump(module);
+		count++;
+	}
+	up_read(&modules_rwsem);
+
+	pr_info("Number of connected modules: %d\n", count);
+}
+EXPORT_SYMBOL_GPL(gb_audio_manager_dump_all);
+
+/*
+ * module init/deinit
+ */
+static int __init manager_init(void)
+{
+	manager_kset = kset_create_and_add(GB_AUDIO_MANAGER_NAME, NULL,
+					   kernel_kobj);
+	if (!manager_kset)
+		return -ENOMEM;
+
+#ifdef GB_AUDIO_MANAGER_SYSFS
+	gb_audio_manager_sysfs_init(&manager_kset->kobj);
+#endif
+
+	return 0;
+}
+
+static void __exit manager_exit(void)
+{
+	gb_audio_manager_remove_all();
+	kset_unregister(manager_kset);
+	ida_destroy(&module_id);
+}
+
+module_init(manager_init);
+module_exit(manager_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Svetlin Ankov <ankov_svetlin@projectara.com>");
diff --git a/drivers/staging/greybus/audio_manager.h b/drivers/staging/greybus/audio_manager.h
new file mode 100644
index 000000000..be605485a
--- /dev/null
+++ b/drivers/staging/greybus/audio_manager.h
@@ -0,0 +1,81 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus operations
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#ifndef _GB_AUDIO_MANAGER_H_
+#define _GB_AUDIO_MANAGER_H_
+
+#include <linux/kobject.h>
+#include <linux/list.h>
+
+#define GB_AUDIO_MANAGER_NAME "gb_audio_manager"
+#define GB_AUDIO_MANAGER_MODULE_NAME_LEN 64
+#define GB_AUDIO_MANAGER_MODULE_NAME_LEN_SSCANF "63"
+
+struct gb_audio_manager_module_descriptor {
+	char name[GB_AUDIO_MANAGER_MODULE_NAME_LEN];
+	int vid;
+	int pid;
+	int intf_id;
+	unsigned int ip_devices;
+	unsigned int op_devices;
+};
+
+struct gb_audio_manager_module {
+	struct kobject kobj;
+	struct list_head list;
+	int id;
+	struct gb_audio_manager_module_descriptor desc;
+};
+
+/*
+ * Creates a new gb_audio_manager_module_descriptor, using the specified
+ * descriptor.
+ *
+ * Returns a negative result on error, or the id of the newly created module.
+ *
+ */
+int gb_audio_manager_add(struct gb_audio_manager_module_descriptor *desc);
+
+/*
+ * Removes a connected gb_audio_manager_module_descriptor for the specified ID.
+ *
+ * Returns zero on success, or a negative value on error.
+ */
+int gb_audio_manager_remove(int id);
+
+/*
+ * Removes all connected gb_audio_modules
+ *
+ * Returns zero on success, or a negative value on error.
+ */
+void gb_audio_manager_remove_all(void);
+
+/*
+ * Retrieves a gb_audio_manager_module_descriptor for the specified id.
+ * Returns the gb_audio_manager_module_descriptor structure,
+ * or NULL if there is no module with the specified ID.
+ */
+struct gb_audio_manager_module *gb_audio_manager_get_module(int id);
+
+/*
+ * Decreases the refcount of the module, obtained by the get function.
+ * Modules are removed via gb_audio_manager_remove
+ */
+void gb_audio_manager_put_module(struct gb_audio_manager_module *module);
+
+/*
+ * Dumps the module for the specified id
+ * Return 0 on success
+ */
+int gb_audio_manager_dump_module(int id);
+
+/*
+ * Dumps all connected modules
+ */
+void gb_audio_manager_dump_all(void);
+
+#endif /* _GB_AUDIO_MANAGER_H_ */
diff --git a/drivers/staging/greybus/audio_manager_module.c b/drivers/staging/greybus/audio_manager_module.c
new file mode 100644
index 000000000..0a0f0a394
--- /dev/null
+++ b/drivers/staging/greybus/audio_manager_module.c
@@ -0,0 +1,242 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus operations
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#include <linux/slab.h>
+
+#include "audio_manager.h"
+#include "audio_manager_private.h"
+
+#define to_gb_audio_module_attr(x)	\
+		container_of(x, struct gb_audio_manager_module_attribute, attr)
+#define to_gb_audio_module(x)		\
+		container_of(x, struct gb_audio_manager_module, kobj)
+
+struct gb_audio_manager_module_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct gb_audio_manager_module *module,
+			struct gb_audio_manager_module_attribute *attr,
+			char *buf);
+	ssize_t (*store)(struct gb_audio_manager_module *module,
+			 struct gb_audio_manager_module_attribute *attr,
+			 const char *buf, size_t count);
+};
+
+static ssize_t gb_audio_module_attr_show(struct kobject *kobj,
+					 struct attribute *attr, char *buf)
+{
+	struct gb_audio_manager_module_attribute *attribute;
+	struct gb_audio_manager_module *module;
+
+	attribute = to_gb_audio_module_attr(attr);
+	module = to_gb_audio_module(kobj);
+
+	if (!attribute->show)
+		return -EIO;
+
+	return attribute->show(module, attribute, buf);
+}
+
+static ssize_t gb_audio_module_attr_store(struct kobject *kobj,
+					  struct attribute *attr,
+					  const char *buf, size_t len)
+{
+	struct gb_audio_manager_module_attribute *attribute;
+	struct gb_audio_manager_module *module;
+
+	attribute = to_gb_audio_module_attr(attr);
+	module = to_gb_audio_module(kobj);
+
+	if (!attribute->store)
+		return -EIO;
+
+	return attribute->store(module, attribute, buf, len);
+}
+
+static const struct sysfs_ops gb_audio_module_sysfs_ops = {
+	.show = gb_audio_module_attr_show,
+	.store = gb_audio_module_attr_store,
+};
+
+static void gb_audio_module_release(struct kobject *kobj)
+{
+	struct gb_audio_manager_module *module = to_gb_audio_module(kobj);
+
+	pr_info("Destroying audio module #%d\n", module->id);
+	/* TODO -> delete from list */
+	kfree(module);
+}
+
+static ssize_t gb_audio_module_name_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s", module->desc.name);
+}
+
+static struct gb_audio_manager_module_attribute gb_audio_module_name_attribute =
+	__ATTR(name, 0664, gb_audio_module_name_show, NULL);
+
+static ssize_t gb_audio_module_vid_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d", module->desc.vid);
+}
+
+static struct gb_audio_manager_module_attribute gb_audio_module_vid_attribute =
+	__ATTR(vid, 0664, gb_audio_module_vid_show, NULL);
+
+static ssize_t gb_audio_module_pid_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d", module->desc.pid);
+}
+
+static struct gb_audio_manager_module_attribute gb_audio_module_pid_attribute =
+	__ATTR(pid, 0664, gb_audio_module_pid_show, NULL);
+
+static ssize_t gb_audio_module_intf_id_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d", module->desc.intf_id);
+}
+
+static struct gb_audio_manager_module_attribute
+					gb_audio_module_intf_id_attribute =
+	__ATTR(intf_id, 0664, gb_audio_module_intf_id_show, NULL);
+
+static ssize_t gb_audio_module_ip_devices_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "0x%X", module->desc.ip_devices);
+}
+
+static struct gb_audio_manager_module_attribute
+					gb_audio_module_ip_devices_attribute =
+	__ATTR(ip_devices, 0664, gb_audio_module_ip_devices_show, NULL);
+
+static ssize_t gb_audio_module_op_devices_show(
+	struct gb_audio_manager_module *module,
+	struct gb_audio_manager_module_attribute *attr, char *buf)
+{
+	return sprintf(buf, "0x%X", module->desc.op_devices);
+}
+
+static struct gb_audio_manager_module_attribute
+					gb_audio_module_op_devices_attribute =
+	__ATTR(op_devices, 0664, gb_audio_module_op_devices_show, NULL);
+
+static struct attribute *gb_audio_module_default_attrs[] = {
+	&gb_audio_module_name_attribute.attr,
+	&gb_audio_module_vid_attribute.attr,
+	&gb_audio_module_pid_attribute.attr,
+	&gb_audio_module_intf_id_attribute.attr,
+	&gb_audio_module_ip_devices_attribute.attr,
+	&gb_audio_module_op_devices_attribute.attr,
+	NULL,   /* need to NULL terminate the list of attributes */
+};
+ATTRIBUTE_GROUPS(gb_audio_module_default);
+
+static struct kobj_type gb_audio_module_type = {
+	.sysfs_ops = &gb_audio_module_sysfs_ops,
+	.release = gb_audio_module_release,
+	.default_groups = gb_audio_module_default_groups,
+};
+
+static void send_add_uevent(struct gb_audio_manager_module *module)
+{
+	char name_string[128];
+	char vid_string[64];
+	char pid_string[64];
+	char intf_id_string[64];
+	char ip_devices_string[64];
+	char op_devices_string[64];
+
+	char *envp[] = {
+		name_string,
+		vid_string,
+		pid_string,
+		intf_id_string,
+		ip_devices_string,
+		op_devices_string,
+		NULL
+	};
+
+	snprintf(name_string, 128, "NAME=%s", module->desc.name);
+	snprintf(vid_string, 64, "VID=%d", module->desc.vid);
+	snprintf(pid_string, 64, "PID=%d", module->desc.pid);
+	snprintf(intf_id_string, 64, "INTF_ID=%d", module->desc.intf_id);
+	snprintf(ip_devices_string, 64, "I/P DEVICES=0x%X",
+		 module->desc.ip_devices);
+	snprintf(op_devices_string, 64, "O/P DEVICES=0x%X",
+		 module->desc.op_devices);
+
+	kobject_uevent_env(&module->kobj, KOBJ_ADD, envp);
+}
+
+int gb_audio_manager_module_create(
+	struct gb_audio_manager_module **module,
+	struct kset *manager_kset,
+	int id, struct gb_audio_manager_module_descriptor *desc)
+{
+	int err;
+	struct gb_audio_manager_module *m;
+
+	m = kzalloc(sizeof(*m), GFP_ATOMIC);
+	if (!m)
+		return -ENOMEM;
+
+	/* Initialize the node */
+	INIT_LIST_HEAD(&m->list);
+
+	/* Set the module id */
+	m->id = id;
+
+	/* Copy the provided descriptor */
+	memcpy(&m->desc, desc, sizeof(*desc));
+
+	/* set the kset */
+	m->kobj.kset = manager_kset;
+
+	/*
+	 * Initialize and add the kobject to the kernel.  All the default files
+	 * will be created here.  As we have already specified a kset for this
+	 * kobject, we don't have to set a parent for the kobject, the kobject
+	 * will be placed beneath that kset automatically.
+	 */
+	err = kobject_init_and_add(&m->kobj, &gb_audio_module_type, NULL, "%d",
+				   id);
+	if (err) {
+		pr_err("failed initializing kobject for audio module #%d\n", id);
+		kobject_put(&m->kobj);
+		return err;
+	}
+
+	/*
+	 * Notify the object was created
+	 */
+	send_add_uevent(m);
+
+	*module = m;
+	pr_info("Created audio module #%d\n", id);
+	return 0;
+}
+
+void gb_audio_manager_module_dump(struct gb_audio_manager_module *module)
+{
+	pr_info("audio module #%d name=%s vid=%d pid=%d intf_id=%d i/p devices=0x%X o/p devices=0x%X\n",
+		module->id,
+		module->desc.name,
+		module->desc.vid,
+		module->desc.pid,
+		module->desc.intf_id,
+		module->desc.ip_devices,
+		module->desc.op_devices);
+}
diff --git a/drivers/staging/greybus/audio_manager_private.h b/drivers/staging/greybus/audio_manager_private.h
new file mode 100644
index 000000000..2b3a766c7
--- /dev/null
+++ b/drivers/staging/greybus/audio_manager_private.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus operations
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#ifndef _GB_AUDIO_MANAGER_PRIVATE_H_
+#define _GB_AUDIO_MANAGER_PRIVATE_H_
+
+#include <linux/kobject.h>
+
+#include "audio_manager.h"
+
+int gb_audio_manager_module_create(
+	struct gb_audio_manager_module **module,
+	struct kset *manager_kset,
+	int id, struct gb_audio_manager_module_descriptor *desc);
+
+/* module destroyed via kobject_put */
+
+void gb_audio_manager_module_dump(struct gb_audio_manager_module *module);
+
+/* sysfs control */
+void gb_audio_manager_sysfs_init(struct kobject *kobj);
+
+#endif /* _GB_AUDIO_MANAGER_PRIVATE_H_ */
diff --git a/drivers/staging/greybus/audio_manager_sysfs.c b/drivers/staging/greybus/audio_manager_sysfs.c
new file mode 100644
index 000000000..fcd518f95
--- /dev/null
+++ b/drivers/staging/greybus/audio_manager_sysfs.c
@@ -0,0 +1,101 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus operations
+ *
+ * Copyright 2015-2016 Google Inc.
+ */
+
+#include <linux/string.h>
+#include <linux/sysfs.h>
+
+#include "audio_manager.h"
+#include "audio_manager_private.h"
+
+static ssize_t manager_sysfs_add_store(struct kobject *kobj,
+				       struct kobj_attribute *attr,
+				       const char *buf, size_t count)
+{
+	struct gb_audio_manager_module_descriptor desc = { {0} };
+
+	int num = sscanf(buf,
+			"name=%" GB_AUDIO_MANAGER_MODULE_NAME_LEN_SSCANF
+			"s vid=%d pid=%d intf_id=%d i/p devices=0x%X o/p devices=0x%X",
+			desc.name, &desc.vid, &desc.pid, &desc.intf_id,
+			&desc.ip_devices, &desc.op_devices);
+
+	if (num != 7)
+		return -EINVAL;
+
+	num = gb_audio_manager_add(&desc);
+	if (num < 0)
+		return -EINVAL;
+
+	return count;
+}
+
+static struct kobj_attribute manager_add_attribute =
+	__ATTR(add, 0664, NULL, manager_sysfs_add_store);
+
+static ssize_t manager_sysfs_remove_store(struct kobject *kobj,
+					  struct kobj_attribute *attr,
+					  const char *buf, size_t count)
+{
+	int id;
+
+	int num = kstrtoint(buf, 10, &id);
+
+	if (num != 1)
+		return -EINVAL;
+
+	num = gb_audio_manager_remove(id);
+	if (num)
+		return num;
+
+	return count;
+}
+
+static struct kobj_attribute manager_remove_attribute =
+	__ATTR(remove, 0664, NULL, manager_sysfs_remove_store);
+
+static ssize_t manager_sysfs_dump_store(struct kobject *kobj,
+					struct kobj_attribute *attr,
+					const char *buf, size_t count)
+{
+	int id;
+
+	int num = kstrtoint(buf, 10, &id);
+
+	if (num == 1) {
+		num = gb_audio_manager_dump_module(id);
+		if (num)
+			return num;
+	} else if (!strncmp("all", buf, 3)) {
+		gb_audio_manager_dump_all();
+	} else {
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static struct kobj_attribute manager_dump_attribute =
+	__ATTR(dump, 0664, NULL, manager_sysfs_dump_store);
+
+static void manager_sysfs_init_attribute(struct kobject *kobj,
+					 struct kobj_attribute *kattr)
+{
+	int err;
+
+	err = sysfs_create_file(kobj, &kattr->attr);
+	if (err) {
+		pr_warn("creating the sysfs entry for %s failed: %d\n",
+			kattr->attr.name, err);
+	}
+}
+
+void gb_audio_manager_sysfs_init(struct kobject *kobj)
+{
+	manager_sysfs_init_attribute(kobj, &manager_add_attribute);
+	manager_sysfs_init_attribute(kobj, &manager_remove_attribute);
+	manager_sysfs_init_attribute(kobj, &manager_dump_attribute);
+}
diff --git a/drivers/staging/greybus/audio_module.c b/drivers/staging/greybus/audio_module.c
new file mode 100644
index 000000000..12c376c47
--- /dev/null
+++ b/drivers/staging/greybus/audio_module.c
@@ -0,0 +1,479 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus audio driver
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <sound/soc.h>
+#include <sound/pcm_params.h>
+
+#include "audio_codec.h"
+#include "audio_apbridgea.h"
+#include "audio_manager.h"
+
+/*
+ * gb_snd management functions
+ */
+
+static int gbaudio_request_jack(struct gbaudio_module_info *module,
+				struct gb_audio_jack_event_request *req)
+{
+	int report;
+	struct snd_jack *jack = module->headset.jack.jack;
+	struct snd_jack *btn_jack = module->button.jack.jack;
+
+	if (!jack) {
+		dev_err_ratelimited(module->dev,
+				    "Invalid jack event received:type: %u, event: %u\n",
+				    req->jack_attribute, req->event);
+		return -EINVAL;
+	}
+
+	dev_warn_ratelimited(module->dev,
+			     "Jack Event received: type: %u, event: %u\n",
+			     req->jack_attribute, req->event);
+
+	if (req->event == GB_AUDIO_JACK_EVENT_REMOVAL) {
+		module->jack_type = 0;
+		if (btn_jack && module->button_status) {
+			snd_soc_jack_report(&module->button.jack, 0,
+					    module->button_mask);
+			module->button_status = 0;
+		}
+		snd_soc_jack_report(&module->headset.jack, 0,
+				    module->jack_mask);
+		return 0;
+	}
+
+	report = req->jack_attribute & module->jack_mask;
+	if (!report) {
+		dev_err_ratelimited(module->dev,
+				    "Invalid jack event received:type: %u, event: %u\n",
+				    req->jack_attribute, req->event);
+		return -EINVAL;
+	}
+
+	if (module->jack_type)
+		dev_warn_ratelimited(module->dev,
+				     "Modifying jack from %d to %d\n",
+				     module->jack_type, report);
+
+	module->jack_type = report;
+	snd_soc_jack_report(&module->headset.jack, report, module->jack_mask);
+
+	return 0;
+}
+
+static int gbaudio_request_button(struct gbaudio_module_info *module,
+				  struct gb_audio_button_event_request *req)
+{
+	int soc_button_id, report;
+	struct snd_jack *btn_jack = module->button.jack.jack;
+
+	if (!btn_jack) {
+		dev_err_ratelimited(module->dev,
+				    "Invalid button event received:type: %u, event: %u\n",
+				    req->button_id, req->event);
+		return -EINVAL;
+	}
+
+	dev_warn_ratelimited(module->dev,
+			     "Button Event received: id: %u, event: %u\n",
+			     req->button_id, req->event);
+
+	/* currently supports 4 buttons only */
+	if (!module->jack_type) {
+		dev_err_ratelimited(module->dev,
+				    "Jack not present. Bogus event!!\n");
+		return -EINVAL;
+	}
+
+	report = module->button_status & module->button_mask;
+	soc_button_id = 0;
+
+	switch (req->button_id) {
+	case 1:
+		soc_button_id = SND_JACK_BTN_0 & module->button_mask;
+		break;
+
+	case 2:
+		soc_button_id = SND_JACK_BTN_1 & module->button_mask;
+		break;
+
+	case 3:
+		soc_button_id = SND_JACK_BTN_2 & module->button_mask;
+		break;
+
+	case 4:
+		soc_button_id = SND_JACK_BTN_3 & module->button_mask;
+		break;
+	}
+
+	if (!soc_button_id) {
+		dev_err_ratelimited(module->dev,
+				    "Invalid button request received\n");
+		return -EINVAL;
+	}
+
+	if (req->event == GB_AUDIO_BUTTON_EVENT_PRESS)
+		report = report | soc_button_id;
+	else
+		report = report & ~soc_button_id;
+
+	module->button_status = report;
+
+	snd_soc_jack_report(&module->button.jack, report, module->button_mask);
+
+	return 0;
+}
+
+static int gbaudio_request_stream(struct gbaudio_module_info *module,
+				  struct gb_audio_streaming_event_request *req)
+{
+	dev_warn(module->dev, "Audio Event received: cport: %u, event: %u\n",
+		 le16_to_cpu(req->data_cport), req->event);
+
+	return 0;
+}
+
+static int gbaudio_codec_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gbaudio_module_info *module =
+		greybus_get_drvdata(connection->bundle);
+	struct gb_operation_msg_hdr *header = op->request->header;
+	struct gb_audio_streaming_event_request *stream_req;
+	struct gb_audio_jack_event_request *jack_req;
+	struct gb_audio_button_event_request *button_req;
+	int ret;
+
+	switch (header->type) {
+	case GB_AUDIO_TYPE_STREAMING_EVENT:
+		stream_req = op->request->payload;
+		ret = gbaudio_request_stream(module, stream_req);
+		break;
+
+	case GB_AUDIO_TYPE_JACK_EVENT:
+		jack_req = op->request->payload;
+		ret = gbaudio_request_jack(module, jack_req);
+		break;
+
+	case GB_AUDIO_TYPE_BUTTON_EVENT:
+		button_req = op->request->payload;
+		ret = gbaudio_request_button(module, button_req);
+		break;
+
+	default:
+		dev_err_ratelimited(&connection->bundle->dev,
+				    "Invalid Audio Event received\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int gb_audio_add_mgmt_connection(struct gbaudio_module_info *gbmodule,
+					struct greybus_descriptor_cport *cport_desc,
+					struct gb_bundle *bundle)
+{
+	struct gb_connection *connection;
+
+	/* Management Cport */
+	if (gbmodule->mgmt_connection) {
+		dev_err(&bundle->dev,
+			"Can't have multiple Management connections\n");
+		return -ENODEV;
+	}
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gbaudio_codec_request_handler);
+	if (IS_ERR(connection))
+		return PTR_ERR(connection);
+
+	greybus_set_drvdata(bundle, gbmodule);
+	gbmodule->mgmt_connection = connection;
+
+	return 0;
+}
+
+static int gb_audio_add_data_connection(struct gbaudio_module_info *gbmodule,
+					struct greybus_descriptor_cport *cport_desc,
+					struct gb_bundle *bundle)
+{
+	struct gb_connection *connection;
+	struct gbaudio_data_connection *dai;
+
+	dai = devm_kzalloc(gbmodule->dev, sizeof(*dai), GFP_KERNEL);
+	if (!dai)
+		return -ENOMEM;
+
+	connection = gb_connection_create_offloaded(bundle,
+						    le16_to_cpu(cport_desc->id),
+						    GB_CONNECTION_FLAG_CSD);
+	if (IS_ERR(connection)) {
+		devm_kfree(gbmodule->dev, dai);
+		return PTR_ERR(connection);
+	}
+
+	greybus_set_drvdata(bundle, gbmodule);
+	dai->id = 0;
+	dai->data_cport = cpu_to_le16(connection->intf_cport_id);
+	dai->connection = connection;
+	list_add(&dai->list, &gbmodule->data_list);
+
+	return 0;
+}
+
+/*
+ * This is the basic hook get things initialized and registered w/ gb
+ */
+
+static int gb_audio_probe(struct gb_bundle *bundle,
+			  const struct greybus_bundle_id *id)
+{
+	struct device *dev = &bundle->dev;
+	struct gbaudio_module_info *gbmodule;
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_audio_manager_module_descriptor desc;
+	struct gbaudio_data_connection *dai, *_dai;
+	int ret, i;
+	struct gb_audio_topology *topology;
+
+	/* There should be at least one Management and one Data cport */
+	if (bundle->num_cports < 2)
+		return -ENODEV;
+
+	/*
+	 * There can be only one Management connection and any number of data
+	 * connections.
+	 */
+	gbmodule = devm_kzalloc(dev, sizeof(*gbmodule), GFP_KERNEL);
+	if (!gbmodule)
+		return -ENOMEM;
+
+	gbmodule->num_data_connections = bundle->num_cports - 1;
+	INIT_LIST_HEAD(&gbmodule->data_list);
+	INIT_LIST_HEAD(&gbmodule->widget_list);
+	INIT_LIST_HEAD(&gbmodule->ctl_list);
+	INIT_LIST_HEAD(&gbmodule->widget_ctl_list);
+	INIT_LIST_HEAD(&gbmodule->jack_list);
+	gbmodule->dev = dev;
+	snprintf(gbmodule->name, sizeof(gbmodule->name), "%s.%s", dev->driver->name,
+		 dev_name(dev));
+	greybus_set_drvdata(bundle, gbmodule);
+
+	/* Create all connections */
+	for (i = 0; i < bundle->num_cports; i++) {
+		cport_desc = &bundle->cport_desc[i];
+
+		switch (cport_desc->protocol_id) {
+		case GREYBUS_PROTOCOL_AUDIO_MGMT:
+			ret = gb_audio_add_mgmt_connection(gbmodule, cport_desc,
+							   bundle);
+			if (ret)
+				goto destroy_connections;
+			break;
+		case GREYBUS_PROTOCOL_AUDIO_DATA:
+			ret = gb_audio_add_data_connection(gbmodule, cport_desc,
+							   bundle);
+			if (ret)
+				goto destroy_connections;
+			break;
+		default:
+			dev_err(dev, "Unsupported protocol: 0x%02x\n",
+				cport_desc->protocol_id);
+			ret = -ENODEV;
+			goto destroy_connections;
+		}
+	}
+
+	/* There must be a management cport */
+	if (!gbmodule->mgmt_connection) {
+		ret = -EINVAL;
+		dev_err(dev, "Missing management connection\n");
+		goto destroy_connections;
+	}
+
+	/* Initialize management connection */
+	ret = gb_connection_enable(gbmodule->mgmt_connection);
+	if (ret) {
+		dev_err(dev, "%d: Error while enabling mgmt connection\n", ret);
+		goto destroy_connections;
+	}
+	gbmodule->dev_id = gbmodule->mgmt_connection->intf->interface_id;
+
+	/*
+	 * FIXME: malloc for topology happens via audio_gb driver
+	 * should be done within codec driver itself
+	 */
+	ret = gb_audio_gb_get_topology(gbmodule->mgmt_connection, &topology);
+	if (ret) {
+		dev_err(dev, "%d:Error while fetching topology\n", ret);
+		goto disable_connection;
+	}
+
+	/* process topology data */
+	ret = gbaudio_tplg_parse_data(gbmodule, topology);
+	if (ret) {
+		dev_err(dev, "%d:Error while parsing topology data\n",
+			ret);
+		goto free_topology;
+	}
+	gbmodule->topology = topology;
+
+	/* Initialize data connections */
+	list_for_each_entry(dai, &gbmodule->data_list, list) {
+		ret = gb_connection_enable(dai->connection);
+		if (ret) {
+			dev_err(dev,
+				"%d:Error while enabling %d:data connection\n",
+				ret, le16_to_cpu(dai->data_cport));
+			goto disable_data_connection;
+		}
+	}
+
+	/* register module with gbcodec */
+	ret = gbaudio_register_module(gbmodule);
+	if (ret)
+		goto disable_data_connection;
+
+	/* inform above layer for uevent */
+	dev_dbg(dev, "Inform set_event:%d to above layer\n", 1);
+	/* prepare for the audio manager */
+	strscpy(desc.name, gbmodule->name, sizeof(desc.name));
+	desc.vid = 2; /* todo */
+	desc.pid = 3; /* todo */
+	desc.intf_id = gbmodule->dev_id;
+	desc.op_devices = gbmodule->op_devices;
+	desc.ip_devices = gbmodule->ip_devices;
+	gbmodule->manager_id = gb_audio_manager_add(&desc);
+
+	dev_dbg(dev, "Add GB Audio device:%s\n", gbmodule->name);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+disable_data_connection:
+	list_for_each_entry_safe(dai, _dai, &gbmodule->data_list, list)
+		gb_connection_disable(dai->connection);
+	gbaudio_tplg_release(gbmodule);
+	gbmodule->topology = NULL;
+
+free_topology:
+	kfree(topology);
+
+disable_connection:
+	gb_connection_disable(gbmodule->mgmt_connection);
+
+destroy_connections:
+	list_for_each_entry_safe(dai, _dai, &gbmodule->data_list, list) {
+		gb_connection_destroy(dai->connection);
+		list_del(&dai->list);
+		devm_kfree(dev, dai);
+	}
+
+	if (gbmodule->mgmt_connection)
+		gb_connection_destroy(gbmodule->mgmt_connection);
+
+	devm_kfree(dev, gbmodule);
+
+	return ret;
+}
+
+static void gb_audio_disconnect(struct gb_bundle *bundle)
+{
+	struct gbaudio_module_info *gbmodule = greybus_get_drvdata(bundle);
+	struct gbaudio_data_connection *dai, *_dai;
+
+	gb_pm_runtime_get_sync(bundle);
+
+	/* cleanup module related resources first */
+	gbaudio_unregister_module(gbmodule);
+
+	/* inform uevent to above layers */
+	gb_audio_manager_remove(gbmodule->manager_id);
+
+	gbaudio_tplg_release(gbmodule);
+	kfree(gbmodule->topology);
+	gbmodule->topology = NULL;
+	gb_connection_disable(gbmodule->mgmt_connection);
+	list_for_each_entry_safe(dai, _dai, &gbmodule->data_list, list) {
+		gb_connection_disable(dai->connection);
+		gb_connection_destroy(dai->connection);
+		list_del(&dai->list);
+		devm_kfree(gbmodule->dev, dai);
+	}
+	gb_connection_destroy(gbmodule->mgmt_connection);
+	gbmodule->mgmt_connection = NULL;
+
+	devm_kfree(&bundle->dev, gbmodule);
+}
+
+static const struct greybus_bundle_id gb_audio_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_AUDIO) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_audio_id_table);
+
+#ifdef CONFIG_PM
+static int gb_audio_suspend(struct device *dev)
+{
+	struct gb_bundle *bundle = to_gb_bundle(dev);
+	struct gbaudio_module_info *gbmodule = greybus_get_drvdata(bundle);
+	struct gbaudio_data_connection *dai;
+
+	list_for_each_entry(dai, &gbmodule->data_list, list)
+		gb_connection_disable(dai->connection);
+
+	gb_connection_disable(gbmodule->mgmt_connection);
+
+	return 0;
+}
+
+static int gb_audio_resume(struct device *dev)
+{
+	struct gb_bundle *bundle = to_gb_bundle(dev);
+	struct gbaudio_module_info *gbmodule = greybus_get_drvdata(bundle);
+	struct gbaudio_data_connection *dai;
+	int ret;
+
+	ret = gb_connection_enable(gbmodule->mgmt_connection);
+	if (ret) {
+		dev_err(dev, "%d:Error while enabling mgmt connection\n", ret);
+		return ret;
+	}
+
+	list_for_each_entry(dai, &gbmodule->data_list, list) {
+		ret = gb_connection_enable(dai->connection);
+		if (ret) {
+			dev_err(dev,
+				"%d:Error while enabling %d:data connection\n",
+				ret, le16_to_cpu(dai->data_cport));
+			return ret;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops gb_audio_pm_ops = {
+	SET_RUNTIME_PM_OPS(gb_audio_suspend, gb_audio_resume, NULL)
+};
+
+static struct greybus_driver gb_audio_driver = {
+	.name		= "gb-audio",
+	.probe		= gb_audio_probe,
+	.disconnect	= gb_audio_disconnect,
+	.id_table	= gb_audio_id_table,
+	.driver.pm	= &gb_audio_pm_ops,
+};
+module_greybus_driver(gb_audio_driver);
+
+MODULE_DESCRIPTION("Greybus Audio module driver");
+MODULE_AUTHOR("Vaibhav Agarwal <vaibhav.agarwal@linaro.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:gbaudio-module");
diff --git a/drivers/staging/greybus/audio_topology.c b/drivers/staging/greybus/audio_topology.c
new file mode 100644
index 000000000..62d767485
--- /dev/null
+++ b/drivers/staging/greybus/audio_topology.c
@@ -0,0 +1,1453 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus audio driver
+ * Copyright 2015-2016 Google Inc.
+ * Copyright 2015-2016 Linaro Ltd.
+ */
+
+#include <linux/greybus.h>
+#include "audio_codec.h"
+
+#define GBAUDIO_INVALID_ID	0xFF
+
+/* mixer control */
+struct gb_mixer_control {
+	int min, max;
+	unsigned int reg, rreg, shift, rshift, invert;
+};
+
+struct gbaudio_ctl_pvt {
+	unsigned int ctl_id;
+	unsigned int data_cport;
+	unsigned int access;
+	unsigned int vcount;
+	struct gb_audio_ctl_elem_info *info;
+};
+
+static struct gbaudio_module_info *find_gb_module(
+					struct gbaudio_codec_info *codec,
+					char const *name)
+{
+	int dev_id;
+	char begin[NAME_SIZE];
+	struct gbaudio_module_info *module;
+
+	if (!name)
+		return NULL;
+
+	if (sscanf(name, "%s %d", begin, &dev_id) != 2)
+		return NULL;
+
+	dev_dbg(codec->dev, "%s:Find module#%d\n", __func__, dev_id);
+
+	mutex_lock(&codec->lock);
+	list_for_each_entry(module, &codec->module_list, list) {
+		if (module->dev_id == dev_id) {
+			mutex_unlock(&codec->lock);
+			return module;
+		}
+	}
+	mutex_unlock(&codec->lock);
+	dev_warn(codec->dev, "%s: module#%d missing in codec list\n", name,
+		 dev_id);
+	return NULL;
+}
+
+static const char *gbaudio_map_controlid(struct gbaudio_module_info *module,
+					 __u8 control_id, __u8 index)
+{
+	struct gbaudio_control *control;
+
+	if (control_id == GBAUDIO_INVALID_ID)
+		return NULL;
+
+	list_for_each_entry(control, &module->ctl_list, list) {
+		if (control->id == control_id) {
+			if (index == GBAUDIO_INVALID_ID)
+				return control->name;
+			if (index >= control->items)
+				return NULL;
+			return control->texts[index];
+		}
+	}
+	list_for_each_entry(control, &module->widget_ctl_list, list) {
+		if (control->id == control_id) {
+			if (index == GBAUDIO_INVALID_ID)
+				return control->name;
+			if (index >= control->items)
+				return NULL;
+			return control->texts[index];
+		}
+	}
+	return NULL;
+}
+
+static int gbaudio_map_controlname(struct gbaudio_module_info *module,
+				   const char *name)
+{
+	struct gbaudio_control *control;
+
+	list_for_each_entry(control, &module->ctl_list, list) {
+		if (!strncmp(control->name, name, NAME_SIZE))
+			return control->id;
+	}
+
+	dev_warn(module->dev, "%s: missing in modules controls list\n", name);
+
+	return -EINVAL;
+}
+
+static int gbaudio_map_wcontrolname(struct gbaudio_module_info *module,
+				    const char *name)
+{
+	struct gbaudio_control *control;
+
+	list_for_each_entry(control, &module->widget_ctl_list, list) {
+		if (!strncmp(control->wname, name, NAME_SIZE))
+			return control->id;
+	}
+	dev_warn(module->dev, "%s: missing in modules controls list\n", name);
+
+	return -EINVAL;
+}
+
+static int gbaudio_map_widgetname(struct gbaudio_module_info *module,
+				  const char *name)
+{
+	struct gbaudio_widget *widget;
+
+	list_for_each_entry(widget, &module->widget_list, list) {
+		if (!strncmp(widget->name, name, NAME_SIZE))
+			return widget->id;
+	}
+	dev_warn(module->dev, "%s: missing in modules widgets list\n", name);
+
+	return -EINVAL;
+}
+
+static const char *gbaudio_map_widgetid(struct gbaudio_module_info *module,
+					__u8 widget_id)
+{
+	struct gbaudio_widget *widget;
+
+	list_for_each_entry(widget, &module->widget_list, list) {
+		if (widget->id == widget_id)
+			return widget->name;
+	}
+	return NULL;
+}
+
+static const char **gb_generate_enum_strings(struct gbaudio_module_info *gb,
+					     struct gb_audio_enumerated *gbenum)
+{
+	const char **strings;
+	int i;
+	unsigned int items;
+	__u8 *data;
+
+	items = le32_to_cpu(gbenum->items);
+	strings = devm_kcalloc(gb->dev, items, sizeof(char *), GFP_KERNEL);
+	if (!strings)
+		return NULL;
+
+	data = gbenum->names;
+
+	for (i = 0; i < items; i++) {
+		strings[i] = (const char *)data;
+		while (*data != '\0')
+			data++;
+		data++;
+	}
+
+	return strings;
+}
+
+static int gbcodec_mixer_ctl_info(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_info *uinfo)
+{
+	unsigned int max;
+	const char *name;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_info *info;
+	struct gbaudio_module_info *module;
+	struct snd_soc_component *comp = snd_soc_kcontrol_component(kcontrol);
+	struct gbaudio_codec_info *gbcodec = snd_soc_component_get_drvdata(comp);
+
+	dev_dbg(comp->dev, "Entered %s:%s\n", __func__, kcontrol->id.name);
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	info = (struct gb_audio_ctl_elem_info *)data->info;
+
+	if (!info) {
+		dev_err(comp->dev, "NULL info for %s\n", uinfo->id.name);
+		return -EINVAL;
+	}
+
+	/* update uinfo */
+	uinfo->access = data->access;
+	uinfo->count = data->vcount;
+	uinfo->type = (__force snd_ctl_elem_type_t)info->type;
+
+	switch (info->type) {
+	case GB_AUDIO_CTL_ELEM_TYPE_BOOLEAN:
+	case GB_AUDIO_CTL_ELEM_TYPE_INTEGER:
+		uinfo->value.integer.min = le32_to_cpu(info->value.integer.min);
+		uinfo->value.integer.max = le32_to_cpu(info->value.integer.max);
+		break;
+	case GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED:
+		max = le32_to_cpu(info->value.enumerated.items);
+		uinfo->value.enumerated.items = max;
+		if (uinfo->value.enumerated.item > max - 1)
+			uinfo->value.enumerated.item = max - 1;
+		module = find_gb_module(gbcodec, kcontrol->id.name);
+		if (!module)
+			return -EINVAL;
+		name = gbaudio_map_controlid(module, data->ctl_id,
+					     uinfo->value.enumerated.item);
+		strscpy(uinfo->value.enumerated.name, name, sizeof(uinfo->value.enumerated.name));
+		break;
+	default:
+		dev_err(comp->dev, "Invalid type: %d for %s:kcontrol\n",
+			info->type, kcontrol->id.name);
+		break;
+	}
+	return 0;
+}
+
+static int gbcodec_mixer_ctl_get(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *ucontrol)
+{
+	int ret;
+	struct gb_audio_ctl_elem_info *info;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct snd_soc_component *comp = snd_soc_kcontrol_component(kcontrol);
+	struct gbaudio_codec_info *gb = snd_soc_component_get_drvdata(comp);
+	struct gb_bundle *bundle;
+
+	dev_dbg(comp->dev, "Entered %s:%s\n", __func__, kcontrol->id.name);
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	info = (struct gb_audio_ctl_elem_info *)data->info;
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, data->ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(comp->dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+		return ret;
+	}
+
+	/* update ucontrol */
+	switch (info->type) {
+	case GB_AUDIO_CTL_ELEM_TYPE_BOOLEAN:
+	case GB_AUDIO_CTL_ELEM_TYPE_INTEGER:
+		ucontrol->value.integer.value[0] =
+			le32_to_cpu(gbvalue.value.integer_value[0]);
+		if (data->vcount == 2)
+			ucontrol->value.integer.value[1] =
+				le32_to_cpu(gbvalue.value.integer_value[1]);
+		break;
+	case GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED:
+		ucontrol->value.enumerated.item[0] =
+			le32_to_cpu(gbvalue.value.enumerated_item[0]);
+		if (data->vcount == 2)
+			ucontrol->value.enumerated.item[1] =
+				le32_to_cpu(gbvalue.value.enumerated_item[1]);
+		break;
+	default:
+		dev_err(comp->dev, "Invalid type: %d for %s:kcontrol\n",
+			info->type, kcontrol->id.name);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int gbcodec_mixer_ctl_put(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *ucontrol)
+{
+	int ret = 0;
+	struct gb_audio_ctl_elem_info *info;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct snd_soc_component *comp = snd_soc_kcontrol_component(kcontrol);
+	struct gbaudio_codec_info *gb = snd_soc_component_get_drvdata(comp);
+	struct gb_bundle *bundle;
+
+	dev_dbg(comp->dev, "Entered %s:%s\n", __func__, kcontrol->id.name);
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	info = (struct gb_audio_ctl_elem_info *)data->info;
+	bundle = to_gb_bundle(module->dev);
+
+	/* update ucontrol */
+	switch (info->type) {
+	case GB_AUDIO_CTL_ELEM_TYPE_BOOLEAN:
+	case GB_AUDIO_CTL_ELEM_TYPE_INTEGER:
+		gbvalue.value.integer_value[0] =
+			cpu_to_le32(ucontrol->value.integer.value[0]);
+		if (data->vcount == 2)
+			gbvalue.value.integer_value[1] =
+				cpu_to_le32(ucontrol->value.integer.value[1]);
+		break;
+	case GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED:
+		gbvalue.value.enumerated_item[0] =
+			cpu_to_le32(ucontrol->value.enumerated.item[0]);
+		if (data->vcount == 2)
+			gbvalue.value.enumerated_item[1] =
+				cpu_to_le32(ucontrol->value.enumerated.item[1]);
+		break;
+	default:
+		dev_err(comp->dev, "Invalid type: %d for %s:kcontrol\n",
+			info->type, kcontrol->id.name);
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret)
+		return ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_set_control(module->mgmt_connection, data->ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(comp->dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+	}
+
+	return ret;
+}
+
+#define SOC_MIXER_GB(xname, kcount, data) \
+{	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname, \
+	.count = kcount, .info = gbcodec_mixer_ctl_info, \
+	.get = gbcodec_mixer_ctl_get, .put = gbcodec_mixer_ctl_put, \
+	.private_value = (unsigned long)data }
+
+/*
+ * although below callback functions seems redundant to above functions.
+ * same are kept to allow provision for different handling in case
+ * of DAPM related sequencing, etc.
+ */
+static int gbcodec_mixer_dapm_ctl_info(struct snd_kcontrol *kcontrol,
+				       struct snd_ctl_elem_info *uinfo)
+{
+	int platform_max, platform_min;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_info *info;
+
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	info = (struct gb_audio_ctl_elem_info *)data->info;
+
+	/* update uinfo */
+	platform_max = le32_to_cpu(info->value.integer.max);
+	platform_min = le32_to_cpu(info->value.integer.min);
+
+	if (platform_max == 1 &&
+	    !strnstr(kcontrol->id.name, " Volume", sizeof(kcontrol->id.name)))
+		uinfo->type = SNDRV_CTL_ELEM_TYPE_BOOLEAN;
+	else
+		uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+
+	uinfo->count = data->vcount;
+	uinfo->value.integer.min = platform_min;
+	uinfo->value.integer.max = platform_max;
+
+	return 0;
+}
+
+static int gbcodec_mixer_dapm_ctl_get(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *ucontrol)
+{
+	int ret;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct snd_soc_dapm_widget_list *wlist = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_dapm_widget *widget = wlist->widgets[0];
+	struct device *codec_dev = widget->dapm->dev;
+	struct gbaudio_codec_info *gb = dev_get_drvdata(codec_dev);
+	struct gb_bundle *bundle;
+
+	dev_dbg(codec_dev, "Entered %s:%s\n", __func__, kcontrol->id.name);
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	bundle = to_gb_bundle(module->dev);
+
+	if (data->vcount == 2)
+		dev_warn(widget->dapm->dev,
+			 "GB: Control '%s' is stereo, which is not supported\n",
+			 kcontrol->id.name);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, data->ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(codec_dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+		return ret;
+	}
+	/* update ucontrol */
+	ucontrol->value.integer.value[0] =
+		le32_to_cpu(gbvalue.value.integer_value[0]);
+
+	return ret;
+}
+
+static int gbcodec_mixer_dapm_ctl_put(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *ucontrol)
+{
+	int ret, wi, max, connect;
+	unsigned int mask, val;
+	struct gb_audio_ctl_elem_info *info;
+	struct gbaudio_ctl_pvt *data;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct snd_soc_dapm_widget_list *wlist = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_dapm_widget *widget = wlist->widgets[0];
+	struct device *codec_dev = widget->dapm->dev;
+	struct gbaudio_codec_info *gb = dev_get_drvdata(codec_dev);
+	struct gb_bundle *bundle;
+
+	dev_dbg(codec_dev, "Entered %s:%s\n", __func__, kcontrol->id.name);
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	data = (struct gbaudio_ctl_pvt *)kcontrol->private_value;
+	info = (struct gb_audio_ctl_elem_info *)data->info;
+	bundle = to_gb_bundle(module->dev);
+
+	if (data->vcount == 2)
+		dev_warn(widget->dapm->dev,
+			 "GB: Control '%s' is stereo, which is not supported\n",
+			 kcontrol->id.name);
+
+	max = le32_to_cpu(info->value.integer.max);
+	mask = (1 << fls(max)) - 1;
+	val = ucontrol->value.integer.value[0] & mask;
+	connect = !!val;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, data->ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+	if (ret)
+		goto exit;
+
+	/* update ucontrol */
+	if (le32_to_cpu(gbvalue.value.integer_value[0]) != val) {
+		for (wi = 0; wi < wlist->num_widgets; wi++) {
+			widget = wlist->widgets[wi];
+			snd_soc_dapm_mixer_update_power(widget->dapm, kcontrol,
+							connect, NULL);
+		}
+		gbvalue.value.integer_value[0] =
+			cpu_to_le32(ucontrol->value.integer.value[0]);
+
+		ret = gb_audio_gb_set_control(module->mgmt_connection,
+					      data->ctl_id,
+					      GB_AUDIO_INVALID_INDEX, &gbvalue);
+	}
+
+exit:
+	gb_pm_runtime_put_autosuspend(bundle);
+	if (ret)
+		dev_err_ratelimited(codec_dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+	return ret;
+}
+
+#define SOC_DAPM_MIXER_GB(xname, kcount, data) \
+{	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname, \
+	.count = kcount, .info = gbcodec_mixer_dapm_ctl_info, \
+	.get = gbcodec_mixer_dapm_ctl_get, .put = gbcodec_mixer_dapm_ctl_put, \
+	.private_value = (unsigned long)data}
+
+static int gbcodec_event_spk(struct snd_soc_dapm_widget *w,
+			     struct snd_kcontrol *k, int event)
+{
+	/* Ensure GB speaker is connected */
+
+	return 0;
+}
+
+static int gbcodec_event_hp(struct snd_soc_dapm_widget *w,
+			    struct snd_kcontrol *k, int event)
+{
+	/* Ensure GB module supports jack slot */
+
+	return 0;
+}
+
+static int gbcodec_event_int_mic(struct snd_soc_dapm_widget *w,
+				 struct snd_kcontrol *k, int event)
+{
+	/* Ensure GB module supports jack slot */
+
+	return 0;
+}
+
+static int gbaudio_validate_kcontrol_count(struct gb_audio_widget *w)
+{
+	int ret = 0;
+
+	switch (w->type) {
+	case snd_soc_dapm_spk:
+	case snd_soc_dapm_hp:
+	case snd_soc_dapm_mic:
+	case snd_soc_dapm_output:
+	case snd_soc_dapm_input:
+		if (w->ncontrols)
+			ret = -EINVAL;
+		break;
+	case snd_soc_dapm_switch:
+	case snd_soc_dapm_mux:
+		if (w->ncontrols != 1)
+			ret = -EINVAL;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int gbcodec_enum_ctl_get(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	int ret, ctl_id;
+	struct snd_soc_component *comp = snd_soc_kcontrol_component(kcontrol);
+	struct gbaudio_codec_info *gb = snd_soc_component_get_drvdata(comp);
+	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct gb_bundle *bundle;
+
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	ctl_id = gbaudio_map_controlname(module, kcontrol->id.name);
+	if (ctl_id < 0)
+		return -EINVAL;
+
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(comp->dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+		return ret;
+	}
+
+	ucontrol->value.enumerated.item[0] =
+		le32_to_cpu(gbvalue.value.enumerated_item[0]);
+	if (e->shift_l != e->shift_r)
+		ucontrol->value.enumerated.item[1] =
+			le32_to_cpu(gbvalue.value.enumerated_item[1]);
+
+	return 0;
+}
+
+static int gbcodec_enum_ctl_put(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	int ret, ctl_id;
+	struct snd_soc_component *comp = snd_soc_kcontrol_component(kcontrol);
+	struct gbaudio_codec_info *gb = snd_soc_component_get_drvdata(comp);
+	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct gb_bundle *bundle;
+
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	ctl_id = gbaudio_map_controlname(module, kcontrol->id.name);
+	if (ctl_id < 0)
+		return -EINVAL;
+
+	if (ucontrol->value.enumerated.item[0] > e->items - 1)
+		return -EINVAL;
+	gbvalue.value.enumerated_item[0] =
+		cpu_to_le32(ucontrol->value.enumerated.item[0]);
+
+	if (e->shift_l != e->shift_r) {
+		if (ucontrol->value.enumerated.item[1] > e->items - 1)
+			return -EINVAL;
+		gbvalue.value.enumerated_item[1] =
+			cpu_to_le32(ucontrol->value.enumerated.item[1]);
+	}
+
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_set_control(module->mgmt_connection, ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(comp->dev, "%d:Error in %s for %s\n",
+				    ret, __func__, kcontrol->id.name);
+	}
+
+	return ret;
+}
+
+static int gbaudio_tplg_create_enum_kctl(struct gbaudio_module_info *gb,
+					 struct snd_kcontrol_new *kctl,
+					 struct gb_audio_control *ctl)
+{
+	struct soc_enum *gbe;
+	struct gb_audio_enumerated *gb_enum;
+	int i;
+
+	gbe = devm_kzalloc(gb->dev, sizeof(*gbe), GFP_KERNEL);
+	if (!gbe)
+		return -ENOMEM;
+
+	gb_enum = &ctl->info.value.enumerated;
+
+	/* since count=1, and reg is dummy */
+	gbe->items = le32_to_cpu(gb_enum->items);
+	gbe->texts = gb_generate_enum_strings(gb, gb_enum);
+	if (!gbe->texts)
+		return -ENOMEM;
+
+	/* debug enum info */
+	dev_dbg(gb->dev, "Max:%d, name_length:%d\n", gbe->items,
+		le16_to_cpu(gb_enum->names_length));
+	for (i = 0; i < gbe->items; i++)
+		dev_dbg(gb->dev, "src[%d]: %s\n", i, gbe->texts[i]);
+
+	*kctl = (struct snd_kcontrol_new)
+		SOC_ENUM_EXT(ctl->name, *gbe, gbcodec_enum_ctl_get,
+			     gbcodec_enum_ctl_put);
+	return 0;
+}
+
+static int gbaudio_tplg_create_kcontrol(struct gbaudio_module_info *gb,
+					struct snd_kcontrol_new *kctl,
+					struct gb_audio_control *ctl)
+{
+	int ret = 0;
+	struct gbaudio_ctl_pvt *ctldata;
+
+	switch (ctl->iface) {
+	case (__force int)SNDRV_CTL_ELEM_IFACE_MIXER:
+		switch (ctl->info.type) {
+		case GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED:
+			ret = gbaudio_tplg_create_enum_kctl(gb, kctl, ctl);
+			break;
+		default:
+			ctldata = devm_kzalloc(gb->dev,
+					       sizeof(struct gbaudio_ctl_pvt),
+					       GFP_KERNEL);
+			if (!ctldata)
+				return -ENOMEM;
+			ctldata->ctl_id = ctl->id;
+			ctldata->data_cport = le16_to_cpu(ctl->data_cport);
+			ctldata->access = le32_to_cpu(ctl->access);
+			ctldata->vcount = ctl->count_values;
+			ctldata->info = &ctl->info;
+			*kctl = (struct snd_kcontrol_new)
+				SOC_MIXER_GB(ctl->name, ctl->count, ctldata);
+			ctldata = NULL;
+			break;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(gb->dev, "%s:%d control created\n", ctl->name, ctl->id);
+	return ret;
+}
+
+static int gbcodec_enum_dapm_ctl_get(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *ucontrol)
+{
+	int ret, ctl_id;
+	struct snd_soc_dapm_widget_list *wlist = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_dapm_widget *widget = wlist->widgets[0];
+	struct gbaudio_module_info *module;
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct device *codec_dev = widget->dapm->dev;
+	struct gbaudio_codec_info *gb = dev_get_drvdata(codec_dev);
+	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
+	struct gb_bundle *bundle;
+
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	ctl_id = gbaudio_map_wcontrolname(module, kcontrol->id.name);
+	if (ctl_id < 0)
+		return -EINVAL;
+
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(codec_dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+		return ret;
+	}
+
+	ucontrol->value.enumerated.item[0] = le32_to_cpu(gbvalue.value.enumerated_item[0]);
+	if (e->shift_l != e->shift_r)
+		ucontrol->value.enumerated.item[1] =
+			le32_to_cpu(gbvalue.value.enumerated_item[1]);
+
+	return 0;
+}
+
+static int gbcodec_enum_dapm_ctl_put(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *ucontrol)
+{
+	int ret, wi, ctl_id;
+	unsigned int val, mux, change;
+	unsigned int mask;
+	struct snd_soc_dapm_widget_list *wlist = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_dapm_widget *widget = wlist->widgets[0];
+	struct gb_audio_ctl_elem_value gbvalue;
+	struct gbaudio_module_info *module;
+	struct device *codec_dev = widget->dapm->dev;
+	struct gbaudio_codec_info *gb = dev_get_drvdata(codec_dev);
+	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
+	struct gb_bundle *bundle;
+
+	if (ucontrol->value.enumerated.item[0] > e->items - 1)
+		return -EINVAL;
+
+	module = find_gb_module(gb, kcontrol->id.name);
+	if (!module)
+		return -EINVAL;
+
+	ctl_id = gbaudio_map_wcontrolname(module, kcontrol->id.name);
+	if (ctl_id < 0)
+		return -EINVAL;
+
+	change = 0;
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_audio_gb_get_control(module->mgmt_connection, ctl_id,
+				      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	if (ret) {
+		dev_err_ratelimited(codec_dev, "%d:Error in %s for %s\n", ret,
+				    __func__, kcontrol->id.name);
+		return ret;
+	}
+
+	mux = ucontrol->value.enumerated.item[0];
+	val = mux << e->shift_l;
+	mask = e->mask << e->shift_l;
+
+	if (le32_to_cpu(gbvalue.value.enumerated_item[0]) !=
+	    ucontrol->value.enumerated.item[0]) {
+		change = 1;
+		gbvalue.value.enumerated_item[0] =
+			cpu_to_le32(ucontrol->value.enumerated.item[0]);
+	}
+
+	if (e->shift_l != e->shift_r) {
+		if (ucontrol->value.enumerated.item[1] > e->items - 1)
+			return -EINVAL;
+		val |= ucontrol->value.enumerated.item[1] << e->shift_r;
+		mask |= e->mask << e->shift_r;
+		if (le32_to_cpu(gbvalue.value.enumerated_item[1]) !=
+		    ucontrol->value.enumerated.item[1]) {
+			change = 1;
+			gbvalue.value.enumerated_item[1] =
+				cpu_to_le32(ucontrol->value.enumerated.item[1]);
+		}
+	}
+
+	if (change) {
+		ret = gb_pm_runtime_get_sync(bundle);
+		if (ret)
+			return ret;
+
+		ret = gb_audio_gb_set_control(module->mgmt_connection, ctl_id,
+					      GB_AUDIO_INVALID_INDEX, &gbvalue);
+
+		gb_pm_runtime_put_autosuspend(bundle);
+
+		if (ret) {
+			dev_err_ratelimited(codec_dev,
+					    "%d:Error in %s for %s\n", ret,
+					    __func__, kcontrol->id.name);
+		}
+		for (wi = 0; wi < wlist->num_widgets; wi++) {
+			widget = wlist->widgets[wi];
+			snd_soc_dapm_mux_update_power(widget->dapm, kcontrol,
+						      val, e, NULL);
+		}
+	}
+
+	return change;
+}
+
+static int gbaudio_tplg_create_enum_ctl(struct gbaudio_module_info *gb,
+					struct snd_kcontrol_new *kctl,
+					struct gb_audio_control *ctl)
+{
+	struct soc_enum *gbe;
+	struct gb_audio_enumerated *gb_enum;
+	int i;
+
+	gbe = devm_kzalloc(gb->dev, sizeof(*gbe), GFP_KERNEL);
+	if (!gbe)
+		return -ENOMEM;
+
+	gb_enum = &ctl->info.value.enumerated;
+
+	/* since count=1, and reg is dummy */
+	gbe->items = le32_to_cpu(gb_enum->items);
+	gbe->texts = gb_generate_enum_strings(gb, gb_enum);
+	if (!gbe->texts)
+		return -ENOMEM;
+
+	/* debug enum info */
+	dev_dbg(gb->dev, "Max:%d, name_length:%d\n", gbe->items,
+		le16_to_cpu(gb_enum->names_length));
+	for (i = 0; i < gbe->items; i++)
+		dev_dbg(gb->dev, "src[%d]: %s\n", i, gbe->texts[i]);
+
+	*kctl = (struct snd_kcontrol_new)
+		SOC_DAPM_ENUM_EXT(ctl->name, *gbe, gbcodec_enum_dapm_ctl_get,
+				  gbcodec_enum_dapm_ctl_put);
+	return 0;
+}
+
+static int gbaudio_tplg_create_mixer_ctl(struct gbaudio_module_info *gb,
+					 struct snd_kcontrol_new *kctl,
+					 struct gb_audio_control *ctl)
+{
+	struct gbaudio_ctl_pvt *ctldata;
+
+	ctldata = devm_kzalloc(gb->dev, sizeof(struct gbaudio_ctl_pvt),
+			       GFP_KERNEL);
+	if (!ctldata)
+		return -ENOMEM;
+	ctldata->ctl_id = ctl->id;
+	ctldata->data_cport = le16_to_cpu(ctl->data_cport);
+	ctldata->access = le32_to_cpu(ctl->access);
+	ctldata->vcount = ctl->count_values;
+	ctldata->info = &ctl->info;
+	*kctl = (struct snd_kcontrol_new)
+		SOC_DAPM_MIXER_GB(ctl->name, ctl->count, ctldata);
+
+	return 0;
+}
+
+static int gbaudio_tplg_create_wcontrol(struct gbaudio_module_info *gb,
+					struct snd_kcontrol_new *kctl,
+					struct gb_audio_control *ctl)
+{
+	int ret;
+
+	switch (ctl->iface) {
+	case (__force int)SNDRV_CTL_ELEM_IFACE_MIXER:
+		switch (ctl->info.type) {
+		case GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED:
+			ret = gbaudio_tplg_create_enum_ctl(gb, kctl, ctl);
+			break;
+		default:
+			ret = gbaudio_tplg_create_mixer_ctl(gb, kctl, ctl);
+			break;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(gb->dev, "%s:%d DAPM control created, ret:%d\n", ctl->name,
+		ctl->id, ret);
+	return ret;
+}
+
+static int gbaudio_widget_event(struct snd_soc_dapm_widget *w,
+				struct snd_kcontrol *kcontrol, int event)
+{
+	int wid;
+	int ret;
+	struct device *codec_dev = w->dapm->dev;
+	struct gbaudio_codec_info *gbcodec = dev_get_drvdata(codec_dev);
+	struct gbaudio_module_info *module;
+	struct gb_bundle *bundle;
+
+	dev_dbg(codec_dev, "%s %s %d\n", __func__, w->name, event);
+
+	/* Find relevant module */
+	module = find_gb_module(gbcodec, w->name);
+	if (!module)
+		return -EINVAL;
+
+	/* map name to widget id */
+	wid = gbaudio_map_widgetname(module, w->name);
+	if (wid < 0) {
+		dev_err(codec_dev, "Invalid widget name:%s\n", w->name);
+		return -EINVAL;
+	}
+
+	bundle = to_gb_bundle(module->dev);
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	switch (event) {
+	case SND_SOC_DAPM_PRE_PMU:
+		ret = gb_audio_gb_enable_widget(module->mgmt_connection, wid);
+		if (!ret)
+			ret = gbaudio_module_update(gbcodec, w, module, 1);
+		break;
+	case SND_SOC_DAPM_POST_PMD:
+		ret = gb_audio_gb_disable_widget(module->mgmt_connection, wid);
+		if (!ret)
+			ret = gbaudio_module_update(gbcodec, w, module, 0);
+		break;
+	}
+	if (ret)
+		dev_err_ratelimited(codec_dev,
+				    "%d: widget, event:%d failed:%d\n", wid,
+				    event, ret);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static const struct snd_soc_dapm_widget gbaudio_widgets[] = {
+	[snd_soc_dapm_spk]	= SND_SOC_DAPM_SPK(NULL, gbcodec_event_spk),
+	[snd_soc_dapm_hp]	= SND_SOC_DAPM_HP(NULL, gbcodec_event_hp),
+	[snd_soc_dapm_mic]	= SND_SOC_DAPM_MIC(NULL, gbcodec_event_int_mic),
+	[snd_soc_dapm_output]	= SND_SOC_DAPM_OUTPUT(NULL),
+	[snd_soc_dapm_input]	= SND_SOC_DAPM_INPUT(NULL),
+	[snd_soc_dapm_switch]	= SND_SOC_DAPM_SWITCH_E(NULL, SND_SOC_NOPM,
+					0, 0, NULL,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+	[snd_soc_dapm_pga]	= SND_SOC_DAPM_PGA_E(NULL, SND_SOC_NOPM,
+					0, 0, NULL, 0,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+	[snd_soc_dapm_mixer]	= SND_SOC_DAPM_MIXER_E(NULL, SND_SOC_NOPM,
+					0, 0, NULL, 0,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+	[snd_soc_dapm_mux]	= SND_SOC_DAPM_MUX_E(NULL, SND_SOC_NOPM,
+					0, 0, NULL,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+	[snd_soc_dapm_aif_in]	= SND_SOC_DAPM_AIF_IN_E(NULL, NULL, 0,
+					SND_SOC_NOPM, 0, 0,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+	[snd_soc_dapm_aif_out]	= SND_SOC_DAPM_AIF_OUT_E(NULL, NULL, 0,
+					SND_SOC_NOPM, 0, 0,
+					gbaudio_widget_event,
+					SND_SOC_DAPM_PRE_PMU |
+					SND_SOC_DAPM_POST_PMD),
+};
+
+static int gbaudio_tplg_create_widget(struct gbaudio_module_info *module,
+				      struct snd_soc_dapm_widget *dw,
+				      struct gb_audio_widget *w, int *w_size)
+{
+	int i, ret, csize;
+	struct snd_kcontrol_new *widget_kctls;
+	struct gb_audio_control *curr;
+	struct gbaudio_control *control, *_control;
+	size_t size;
+	char temp_name[NAME_SIZE];
+
+	ret = gbaudio_validate_kcontrol_count(w);
+	if (ret) {
+		dev_err(module->dev, "Invalid kcontrol count=%d for %s\n",
+			w->ncontrols, w->name);
+		return ret;
+	}
+
+	/* allocate memory for kcontrol */
+	if (w->ncontrols) {
+		size = sizeof(struct snd_kcontrol_new) * w->ncontrols;
+		widget_kctls = devm_kzalloc(module->dev, size, GFP_KERNEL);
+		if (!widget_kctls)
+			return -ENOMEM;
+	}
+
+	*w_size = sizeof(struct gb_audio_widget);
+
+	/* create relevant kcontrols */
+	curr = w->ctl;
+	for (i = 0; i < w->ncontrols; i++) {
+		ret = gbaudio_tplg_create_wcontrol(module, &widget_kctls[i],
+						   curr);
+		if (ret) {
+			dev_err(module->dev,
+				"%s:%d type widget_ctl not supported\n",
+				curr->name, curr->iface);
+			goto error;
+		}
+		control = devm_kzalloc(module->dev,
+				       sizeof(struct gbaudio_control),
+				       GFP_KERNEL);
+		if (!control) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		control->id = curr->id;
+		control->name = curr->name;
+		control->wname = w->name;
+
+		if (curr->info.type == GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED) {
+			struct gb_audio_enumerated *gbenum =
+				&curr->info.value.enumerated;
+
+			csize = offsetof(struct gb_audio_control, info);
+			csize += offsetof(struct gb_audio_ctl_elem_info, value);
+			csize += offsetof(struct gb_audio_enumerated, names);
+			csize += le16_to_cpu(gbenum->names_length);
+			control->texts = (const char * const *)
+				gb_generate_enum_strings(module, gbenum);
+			if (!control->texts) {
+				ret = -ENOMEM;
+				goto error;
+			}
+			control->items = le32_to_cpu(gbenum->items);
+		} else {
+			csize = sizeof(struct gb_audio_control);
+		}
+
+		*w_size += csize;
+		curr = (void *)curr + csize;
+		list_add(&control->list, &module->widget_ctl_list);
+		dev_dbg(module->dev, "%s: control of type %d created\n",
+			widget_kctls[i].name, widget_kctls[i].iface);
+	}
+
+	/* Prefix dev_id to widget control_name */
+	strscpy(temp_name, w->name, sizeof(temp_name));
+	snprintf(w->name, sizeof(w->name), "GB %d %s", module->dev_id, temp_name);
+
+	switch (w->type) {
+	case snd_soc_dapm_spk:
+		*dw = gbaudio_widgets[w->type];
+		module->op_devices |= GBAUDIO_DEVICE_OUT_SPEAKER;
+		break;
+	case snd_soc_dapm_hp:
+		*dw = gbaudio_widgets[w->type];
+		module->op_devices |= (GBAUDIO_DEVICE_OUT_WIRED_HEADSET
+					| GBAUDIO_DEVICE_OUT_WIRED_HEADPHONE);
+		module->ip_devices |= GBAUDIO_DEVICE_IN_WIRED_HEADSET;
+		break;
+	case snd_soc_dapm_mic:
+		*dw = gbaudio_widgets[w->type];
+		module->ip_devices |= GBAUDIO_DEVICE_IN_BUILTIN_MIC;
+		break;
+	case snd_soc_dapm_output:
+	case snd_soc_dapm_input:
+	case snd_soc_dapm_switch:
+	case snd_soc_dapm_pga:
+	case snd_soc_dapm_mixer:
+	case snd_soc_dapm_mux:
+		*dw = gbaudio_widgets[w->type];
+		break;
+	case snd_soc_dapm_aif_in:
+	case snd_soc_dapm_aif_out:
+		*dw = gbaudio_widgets[w->type];
+		dw->sname = w->sname;
+		break;
+	default:
+		ret = -EINVAL;
+		goto error;
+	}
+	dw->name = w->name;
+
+	dev_dbg(module->dev, "%s: widget of type %d created\n", dw->name,
+		dw->id);
+	return 0;
+error:
+	list_for_each_entry_safe(control, _control, &module->widget_ctl_list,
+				 list) {
+		list_del(&control->list);
+		devm_kfree(module->dev, control);
+	}
+	return ret;
+}
+
+static int gbaudio_tplg_process_kcontrols(struct gbaudio_module_info *module,
+					  struct gb_audio_control *controls)
+{
+	int i, csize, ret;
+	struct snd_kcontrol_new *dapm_kctls;
+	struct gb_audio_control *curr;
+	struct gbaudio_control *control, *_control;
+	size_t size;
+	char temp_name[NAME_SIZE];
+
+	size = sizeof(struct snd_kcontrol_new) * module->num_controls;
+	dapm_kctls = devm_kzalloc(module->dev, size, GFP_KERNEL);
+	if (!dapm_kctls)
+		return -ENOMEM;
+
+	curr = controls;
+	for (i = 0; i < module->num_controls; i++) {
+		ret = gbaudio_tplg_create_kcontrol(module, &dapm_kctls[i],
+						   curr);
+		if (ret) {
+			dev_err(module->dev, "%s:%d type not supported\n",
+				curr->name, curr->iface);
+			goto error;
+		}
+		control = devm_kzalloc(module->dev, sizeof(struct
+							   gbaudio_control),
+				      GFP_KERNEL);
+		if (!control) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		control->id = curr->id;
+		/* Prefix dev_id to widget_name */
+		strscpy(temp_name, curr->name, sizeof(temp_name));
+		snprintf(curr->name, sizeof(curr->name), "GB %d %s", module->dev_id,
+			 temp_name);
+		control->name = curr->name;
+		if (curr->info.type == GB_AUDIO_CTL_ELEM_TYPE_ENUMERATED) {
+			struct gb_audio_enumerated *gbenum =
+				&curr->info.value.enumerated;
+
+			csize = offsetof(struct gb_audio_control, info);
+			csize += offsetof(struct gb_audio_ctl_elem_info, value);
+			csize += offsetof(struct gb_audio_enumerated, names);
+			csize += le16_to_cpu(gbenum->names_length);
+			control->texts = (const char * const *)
+				gb_generate_enum_strings(module, gbenum);
+			if (!control->texts) {
+				ret = -ENOMEM;
+				goto error;
+			}
+			control->items = le32_to_cpu(gbenum->items);
+		} else {
+			csize = sizeof(struct gb_audio_control);
+		}
+
+		list_add(&control->list, &module->ctl_list);
+		dev_dbg(module->dev, "%d:%s created of type %d\n", curr->id,
+			curr->name, curr->info.type);
+		curr = (void *)curr + csize;
+	}
+	module->controls = dapm_kctls;
+
+	return 0;
+error:
+	list_for_each_entry_safe(control, _control, &module->ctl_list,
+				 list) {
+		list_del(&control->list);
+		devm_kfree(module->dev, control);
+	}
+	devm_kfree(module->dev, dapm_kctls);
+	return ret;
+}
+
+static int gbaudio_tplg_process_widgets(struct gbaudio_module_info *module,
+					struct gb_audio_widget *widgets)
+{
+	int i, ret, w_size;
+	struct snd_soc_dapm_widget *dapm_widgets;
+	struct gb_audio_widget *curr;
+	struct gbaudio_widget *widget, *_widget;
+	size_t size;
+
+	size = sizeof(struct snd_soc_dapm_widget) * module->num_dapm_widgets;
+	dapm_widgets = devm_kzalloc(module->dev, size, GFP_KERNEL);
+	if (!dapm_widgets)
+		return -ENOMEM;
+
+	curr = widgets;
+	for (i = 0; i < module->num_dapm_widgets; i++) {
+		ret = gbaudio_tplg_create_widget(module, &dapm_widgets[i],
+						 curr, &w_size);
+		if (ret) {
+			dev_err(module->dev, "%s:%d type not supported\n",
+				curr->name, curr->type);
+			goto error;
+		}
+		widget = devm_kzalloc(module->dev, sizeof(struct
+							   gbaudio_widget),
+				      GFP_KERNEL);
+		if (!widget) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		widget->id = curr->id;
+		widget->name = curr->name;
+		list_add(&widget->list, &module->widget_list);
+		curr = (void *)curr + w_size;
+	}
+	module->dapm_widgets = dapm_widgets;
+
+	return 0;
+
+error:
+	list_for_each_entry_safe(widget, _widget, &module->widget_list,
+				 list) {
+		list_del(&widget->list);
+		devm_kfree(module->dev, widget);
+	}
+	devm_kfree(module->dev, dapm_widgets);
+	return ret;
+}
+
+static int gbaudio_tplg_process_routes(struct gbaudio_module_info *module,
+				       struct gb_audio_route *routes)
+{
+	int i, ret;
+	struct snd_soc_dapm_route *dapm_routes;
+	struct gb_audio_route *curr;
+	size_t size;
+
+	size = sizeof(struct snd_soc_dapm_route) * module->num_dapm_routes;
+	dapm_routes = devm_kzalloc(module->dev, size, GFP_KERNEL);
+	if (!dapm_routes)
+		return -ENOMEM;
+
+	module->dapm_routes = dapm_routes;
+	curr = routes;
+
+	for (i = 0; i < module->num_dapm_routes; i++) {
+		dapm_routes->sink =
+			gbaudio_map_widgetid(module, curr->destination_id);
+		if (!dapm_routes->sink) {
+			dev_err(module->dev, "%d:%d:%d:%d - Invalid sink\n",
+				curr->source_id, curr->destination_id,
+				curr->control_id, curr->index);
+			ret = -EINVAL;
+			goto error;
+		}
+		dapm_routes->source =
+			gbaudio_map_widgetid(module, curr->source_id);
+		if (!dapm_routes->source) {
+			dev_err(module->dev, "%d:%d:%d:%d - Invalid source\n",
+				curr->source_id, curr->destination_id,
+				curr->control_id, curr->index);
+			ret = -EINVAL;
+			goto error;
+		}
+		dapm_routes->control =
+			gbaudio_map_controlid(module,
+					      curr->control_id,
+					      curr->index);
+		if ((curr->control_id !=  GBAUDIO_INVALID_ID) &&
+		    !dapm_routes->control) {
+			dev_err(module->dev, "%d:%d:%d:%d - Invalid control\n",
+				curr->source_id, curr->destination_id,
+				curr->control_id, curr->index);
+			ret = -EINVAL;
+			goto error;
+		}
+		dev_dbg(module->dev, "Route {%s, %s, %s}\n", dapm_routes->sink,
+			(dapm_routes->control) ? dapm_routes->control : "NULL",
+			dapm_routes->source);
+		dapm_routes++;
+		curr++;
+	}
+
+	return 0;
+
+error:
+	devm_kfree(module->dev, module->dapm_routes);
+	return ret;
+}
+
+static int gbaudio_tplg_process_header(struct gbaudio_module_info *module,
+				       struct gb_audio_topology *tplg_data)
+{
+	/* fetch no. of kcontrols, widgets & routes */
+	module->num_controls = tplg_data->num_controls;
+	module->num_dapm_widgets = tplg_data->num_widgets;
+	module->num_dapm_routes = tplg_data->num_routes;
+
+	/* update block offset */
+	module->dai_offset = (unsigned long)&tplg_data->data;
+	module->control_offset = module->dai_offset +
+					le32_to_cpu(tplg_data->size_dais);
+	module->widget_offset = module->control_offset +
+					le32_to_cpu(tplg_data->size_controls);
+	module->route_offset = module->widget_offset +
+					le32_to_cpu(tplg_data->size_widgets);
+
+	dev_dbg(module->dev, "DAI offset is 0x%lx\n", module->dai_offset);
+	dev_dbg(module->dev, "control offset is %lx\n",
+		module->control_offset);
+	dev_dbg(module->dev, "widget offset is %lx\n", module->widget_offset);
+	dev_dbg(module->dev, "route offset is %lx\n", module->route_offset);
+
+	return 0;
+}
+
+int gbaudio_tplg_parse_data(struct gbaudio_module_info *module,
+			    struct gb_audio_topology *tplg_data)
+{
+	int ret;
+	struct gb_audio_control *controls;
+	struct gb_audio_widget *widgets;
+	struct gb_audio_route *routes;
+	unsigned int jack_type;
+
+	if (!tplg_data)
+		return -EINVAL;
+
+	ret = gbaudio_tplg_process_header(module, tplg_data);
+	if (ret) {
+		dev_err(module->dev, "%d: Error in parsing topology header\n",
+			ret);
+		return ret;
+	}
+
+	/* process control */
+	controls = (struct gb_audio_control *)module->control_offset;
+	ret = gbaudio_tplg_process_kcontrols(module, controls);
+	if (ret) {
+		dev_err(module->dev,
+			"%d: Error in parsing controls data\n", ret);
+		return ret;
+	}
+	dev_dbg(module->dev, "Control parsing finished\n");
+
+	/* process widgets */
+	widgets = (struct gb_audio_widget *)module->widget_offset;
+	ret = gbaudio_tplg_process_widgets(module, widgets);
+	if (ret) {
+		dev_err(module->dev,
+			"%d: Error in parsing widgets data\n", ret);
+		return ret;
+	}
+	dev_dbg(module->dev, "Widget parsing finished\n");
+
+	/* process route */
+	routes = (struct gb_audio_route *)module->route_offset;
+	ret = gbaudio_tplg_process_routes(module, routes);
+	if (ret) {
+		dev_err(module->dev,
+			"%d: Error in parsing routes data\n", ret);
+		return ret;
+	}
+	dev_dbg(module->dev, "Route parsing finished\n");
+
+	/* parse jack capabilities */
+	jack_type = le32_to_cpu(tplg_data->jack_type);
+	if (jack_type) {
+		module->jack_mask = jack_type & GBCODEC_JACK_MASK;
+		module->button_mask = jack_type & GBCODEC_JACK_BUTTON_MASK;
+	}
+
+	return ret;
+}
+
+void gbaudio_tplg_release(struct gbaudio_module_info *module)
+{
+	struct gbaudio_control *control, *_control;
+	struct gbaudio_widget *widget, *_widget;
+
+	if (!module->topology)
+		return;
+
+	/* release kcontrols */
+	list_for_each_entry_safe(control, _control, &module->ctl_list,
+				 list) {
+		list_del(&control->list);
+		devm_kfree(module->dev, control);
+	}
+	if (module->controls)
+		devm_kfree(module->dev, module->controls);
+
+	/* release widget controls */
+	list_for_each_entry_safe(control, _control, &module->widget_ctl_list,
+				 list) {
+		list_del(&control->list);
+		devm_kfree(module->dev, control);
+	}
+
+	/* release widgets */
+	list_for_each_entry_safe(widget, _widget, &module->widget_list,
+				 list) {
+		list_del(&widget->list);
+		devm_kfree(module->dev, widget);
+	}
+	if (module->dapm_widgets)
+		devm_kfree(module->dev, module->dapm_widgets);
+
+	/* release routes */
+	if (module->dapm_routes)
+		devm_kfree(module->dev, module->dapm_routes);
+}
diff --git a/drivers/staging/greybus/authentication.c b/drivers/staging/greybus/authentication.c
new file mode 100644
index 000000000..297e69f01
--- /dev/null
+++ b/drivers/staging/greybus/authentication.c
@@ -0,0 +1,426 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Component Authentication Protocol (CAP) Driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+
+#include <linux/greybus.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+
+#include "greybus_authentication.h"
+#include "firmware.h"
+
+#define CAP_TIMEOUT_MS		1000
+
+/*
+ * Number of minor devices this driver supports.
+ * There will be exactly one required per Interface.
+ */
+#define NUM_MINORS		U8_MAX
+
+struct gb_cap {
+	struct device		*parent;
+	struct gb_connection	*connection;
+	struct kref		kref;
+	struct list_head	node;
+	bool			disabled; /* connection getting disabled */
+
+	struct mutex		mutex;
+	struct cdev		cdev;
+	struct device		*class_device;
+	dev_t			dev_num;
+};
+
+static struct class *cap_class;
+static dev_t cap_dev_num;
+static DEFINE_IDA(cap_minors_map);
+static LIST_HEAD(cap_list);
+static DEFINE_MUTEX(list_mutex);
+
+static void cap_kref_release(struct kref *kref)
+{
+	struct gb_cap *cap = container_of(kref, struct gb_cap, kref);
+
+	kfree(cap);
+}
+
+/*
+ * All users of cap take a reference (from within list_mutex lock), before
+ * they get a pointer to play with. And the structure will be freed only after
+ * the last user has put the reference to it.
+ */
+static void put_cap(struct gb_cap *cap)
+{
+	kref_put(&cap->kref, cap_kref_release);
+}
+
+/* Caller must call put_cap() after using struct gb_cap */
+static struct gb_cap *get_cap(struct cdev *cdev)
+{
+	struct gb_cap *cap;
+
+	mutex_lock(&list_mutex);
+
+	list_for_each_entry(cap, &cap_list, node) {
+		if (&cap->cdev == cdev) {
+			kref_get(&cap->kref);
+			goto unlock;
+		}
+	}
+
+	cap = NULL;
+
+unlock:
+	mutex_unlock(&list_mutex);
+
+	return cap;
+}
+
+static int cap_get_endpoint_uid(struct gb_cap *cap, u8 *euid)
+{
+	struct gb_connection *connection = cap->connection;
+	struct gb_cap_get_endpoint_uid_response response;
+	int ret;
+
+	ret = gb_operation_sync(connection, GB_CAP_TYPE_GET_ENDPOINT_UID, NULL,
+				0, &response, sizeof(response));
+	if (ret) {
+		dev_err(cap->parent, "failed to get endpoint uid (%d)\n", ret);
+		return ret;
+	}
+
+	memcpy(euid, response.uid, sizeof(response.uid));
+
+	return 0;
+}
+
+static int cap_get_ims_certificate(struct gb_cap *cap, u32 class, u32 id,
+				   u8 *certificate, u32 *size, u8 *result)
+{
+	struct gb_connection *connection = cap->connection;
+	struct gb_cap_get_ims_certificate_request *request;
+	struct gb_cap_get_ims_certificate_response *response;
+	size_t max_size = gb_operation_get_payload_size_max(connection);
+	struct gb_operation *op;
+	int ret;
+
+	op = gb_operation_create_flags(connection,
+				       GB_CAP_TYPE_GET_IMS_CERTIFICATE,
+				       sizeof(*request), max_size,
+				       GB_OPERATION_FLAG_SHORT_RESPONSE,
+				       GFP_KERNEL);
+	if (!op)
+		return -ENOMEM;
+
+	request = op->request->payload;
+	request->certificate_class = cpu_to_le32(class);
+	request->certificate_id = cpu_to_le32(id);
+
+	ret = gb_operation_request_send_sync(op);
+	if (ret) {
+		dev_err(cap->parent, "failed to get certificate (%d)\n", ret);
+		goto done;
+	}
+
+	response = op->response->payload;
+	*result = response->result_code;
+	*size = op->response->payload_size - sizeof(*response);
+	memcpy(certificate, response->certificate, *size);
+
+done:
+	gb_operation_put(op);
+	return ret;
+}
+
+static int cap_authenticate(struct gb_cap *cap, u32 auth_type, u8 *uid,
+			    u8 *challenge, u8 *result, u8 *auth_response,
+			    u32 *signature_size, u8 *signature)
+{
+	struct gb_connection *connection = cap->connection;
+	struct gb_cap_authenticate_request *request;
+	struct gb_cap_authenticate_response *response;
+	size_t max_size = gb_operation_get_payload_size_max(connection);
+	struct gb_operation *op;
+	int ret;
+
+	op = gb_operation_create_flags(connection, GB_CAP_TYPE_AUTHENTICATE,
+				       sizeof(*request), max_size,
+				       GB_OPERATION_FLAG_SHORT_RESPONSE,
+				       GFP_KERNEL);
+	if (!op)
+		return -ENOMEM;
+
+	request = op->request->payload;
+	request->auth_type = cpu_to_le32(auth_type);
+	memcpy(request->uid, uid, sizeof(request->uid));
+	memcpy(request->challenge, challenge, sizeof(request->challenge));
+
+	ret = gb_operation_request_send_sync(op);
+	if (ret) {
+		dev_err(cap->parent, "failed to authenticate (%d)\n", ret);
+		goto done;
+	}
+
+	response = op->response->payload;
+	*result = response->result_code;
+	*signature_size = op->response->payload_size - sizeof(*response);
+	memcpy(auth_response, response->response, sizeof(response->response));
+	memcpy(signature, response->signature, *signature_size);
+
+done:
+	gb_operation_put(op);
+	return ret;
+}
+
+/* Char device fops */
+
+static int cap_open(struct inode *inode, struct file *file)
+{
+	struct gb_cap *cap = get_cap(inode->i_cdev);
+
+	/* cap structure can't get freed until file descriptor is closed */
+	if (cap) {
+		file->private_data = cap;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static int cap_release(struct inode *inode, struct file *file)
+{
+	struct gb_cap *cap = file->private_data;
+
+	put_cap(cap);
+	return 0;
+}
+
+static int cap_ioctl(struct gb_cap *cap, unsigned int cmd,
+		     void __user *buf)
+{
+	struct cap_ioc_get_endpoint_uid endpoint_uid;
+	struct cap_ioc_get_ims_certificate *ims_cert;
+	struct cap_ioc_authenticate *authenticate;
+	size_t size;
+	int ret;
+
+	switch (cmd) {
+	case CAP_IOC_GET_ENDPOINT_UID:
+		ret = cap_get_endpoint_uid(cap, endpoint_uid.uid);
+		if (ret)
+			return ret;
+
+		if (copy_to_user(buf, &endpoint_uid, sizeof(endpoint_uid)))
+			return -EFAULT;
+
+		return 0;
+	case CAP_IOC_GET_IMS_CERTIFICATE:
+		size = sizeof(*ims_cert);
+		ims_cert = memdup_user(buf, size);
+		if (IS_ERR(ims_cert))
+			return PTR_ERR(ims_cert);
+
+		ret = cap_get_ims_certificate(cap, ims_cert->certificate_class,
+					      ims_cert->certificate_id,
+					      ims_cert->certificate,
+					      &ims_cert->cert_size,
+					      &ims_cert->result_code);
+		if (!ret && copy_to_user(buf, ims_cert, size))
+			ret = -EFAULT;
+		kfree(ims_cert);
+
+		return ret;
+	case CAP_IOC_AUTHENTICATE:
+		size = sizeof(*authenticate);
+		authenticate = memdup_user(buf, size);
+		if (IS_ERR(authenticate))
+			return PTR_ERR(authenticate);
+
+		ret = cap_authenticate(cap, authenticate->auth_type,
+				       authenticate->uid,
+				       authenticate->challenge,
+				       &authenticate->result_code,
+				       authenticate->response,
+				       &authenticate->signature_size,
+				       authenticate->signature);
+		if (!ret && copy_to_user(buf, authenticate, size))
+			ret = -EFAULT;
+		kfree(authenticate);
+
+		return ret;
+	default:
+		return -ENOTTY;
+	}
+}
+
+static long cap_ioctl_unlocked(struct file *file, unsigned int cmd,
+			       unsigned long arg)
+{
+	struct gb_cap *cap = file->private_data;
+	struct gb_bundle *bundle = cap->connection->bundle;
+	int ret = -ENODEV;
+
+	/*
+	 * Serialize ioctls.
+	 *
+	 * We don't want the user to do multiple authentication operations in
+	 * parallel.
+	 *
+	 * This is also used to protect ->disabled, which is used to check if
+	 * the connection is getting disconnected, so that we don't start any
+	 * new operations.
+	 */
+	mutex_lock(&cap->mutex);
+	if (!cap->disabled) {
+		ret = gb_pm_runtime_get_sync(bundle);
+		if (!ret) {
+			ret = cap_ioctl(cap, cmd, (void __user *)arg);
+			gb_pm_runtime_put_autosuspend(bundle);
+		}
+	}
+	mutex_unlock(&cap->mutex);
+
+	return ret;
+}
+
+static const struct file_operations cap_fops = {
+	.owner		= THIS_MODULE,
+	.open		= cap_open,
+	.release	= cap_release,
+	.unlocked_ioctl	= cap_ioctl_unlocked,
+};
+
+int gb_cap_connection_init(struct gb_connection *connection)
+{
+	struct gb_cap *cap;
+	int ret, minor;
+
+	if (!connection)
+		return 0;
+
+	cap = kzalloc(sizeof(*cap), GFP_KERNEL);
+	if (!cap)
+		return -ENOMEM;
+
+	cap->parent = &connection->bundle->dev;
+	cap->connection = connection;
+	mutex_init(&cap->mutex);
+	gb_connection_set_data(connection, cap);
+	kref_init(&cap->kref);
+
+	mutex_lock(&list_mutex);
+	list_add(&cap->node, &cap_list);
+	mutex_unlock(&list_mutex);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto err_list_del;
+
+	minor = ida_simple_get(&cap_minors_map, 0, NUM_MINORS, GFP_KERNEL);
+	if (minor < 0) {
+		ret = minor;
+		goto err_connection_disable;
+	}
+
+	/* Add a char device to allow userspace to interact with cap */
+	cap->dev_num = MKDEV(MAJOR(cap_dev_num), minor);
+	cdev_init(&cap->cdev, &cap_fops);
+
+	ret = cdev_add(&cap->cdev, cap->dev_num, 1);
+	if (ret)
+		goto err_remove_ida;
+
+	/* Add a soft link to the previously added char-dev within the bundle */
+	cap->class_device = device_create(cap_class, cap->parent, cap->dev_num,
+					  NULL, "gb-authenticate-%d", minor);
+	if (IS_ERR(cap->class_device)) {
+		ret = PTR_ERR(cap->class_device);
+		goto err_del_cdev;
+	}
+
+	return 0;
+
+err_del_cdev:
+	cdev_del(&cap->cdev);
+err_remove_ida:
+	ida_simple_remove(&cap_minors_map, minor);
+err_connection_disable:
+	gb_connection_disable(connection);
+err_list_del:
+	mutex_lock(&list_mutex);
+	list_del(&cap->node);
+	mutex_unlock(&list_mutex);
+
+	put_cap(cap);
+
+	return ret;
+}
+
+void gb_cap_connection_exit(struct gb_connection *connection)
+{
+	struct gb_cap *cap;
+
+	if (!connection)
+		return;
+
+	cap = gb_connection_get_data(connection);
+
+	device_destroy(cap_class, cap->dev_num);
+	cdev_del(&cap->cdev);
+	ida_simple_remove(&cap_minors_map, MINOR(cap->dev_num));
+
+	/*
+	 * Disallow any new ioctl operations on the char device and wait for
+	 * existing ones to finish.
+	 */
+	mutex_lock(&cap->mutex);
+	cap->disabled = true;
+	mutex_unlock(&cap->mutex);
+
+	/* All pending greybus operations should have finished by now */
+	gb_connection_disable(cap->connection);
+
+	/* Disallow new users to get access to the cap structure */
+	mutex_lock(&list_mutex);
+	list_del(&cap->node);
+	mutex_unlock(&list_mutex);
+
+	/*
+	 * All current users of cap would have taken a reference to it by
+	 * now, we can drop our reference and wait the last user will get
+	 * cap freed.
+	 */
+	put_cap(cap);
+}
+
+int cap_init(void)
+{
+	int ret;
+
+	cap_class = class_create(THIS_MODULE, "gb_authenticate");
+	if (IS_ERR(cap_class))
+		return PTR_ERR(cap_class);
+
+	ret = alloc_chrdev_region(&cap_dev_num, 0, NUM_MINORS,
+				  "gb_authenticate");
+	if (ret)
+		goto err_remove_class;
+
+	return 0;
+
+err_remove_class:
+	class_destroy(cap_class);
+	return ret;
+}
+
+void cap_exit(void)
+{
+	unregister_chrdev_region(cap_dev_num, NUM_MINORS);
+	class_destroy(cap_class);
+	ida_destroy(&cap_minors_map);
+}
diff --git a/drivers/staging/greybus/bootrom.c b/drivers/staging/greybus/bootrom.c
new file mode 100644
index 000000000..a8efb86de
--- /dev/null
+++ b/drivers/staging/greybus/bootrom.c
@@ -0,0 +1,527 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * BOOTROM Greybus driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+
+#include <linux/firmware.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/greybus.h>
+
+#include "firmware.h"
+
+/* Timeout, in jiffies, within which the next request must be received */
+#define NEXT_REQ_TIMEOUT_MS	1000
+
+/*
+ * FIXME: Reduce this timeout once svc core handles parallel processing of
+ * events from the SVC, which are handled sequentially today.
+ */
+#define MODE_SWITCH_TIMEOUT_MS	10000
+
+enum next_request_type {
+	NEXT_REQ_FIRMWARE_SIZE,
+	NEXT_REQ_GET_FIRMWARE,
+	NEXT_REQ_READY_TO_BOOT,
+	NEXT_REQ_MODE_SWITCH,
+};
+
+struct gb_bootrom {
+	struct gb_connection	*connection;
+	const struct firmware	*fw;
+	u8			protocol_major;
+	u8			protocol_minor;
+	enum next_request_type	next_request;
+	struct delayed_work	dwork;
+	struct mutex		mutex; /* Protects bootrom->fw */
+};
+
+static void free_firmware(struct gb_bootrom *bootrom)
+{
+	if (!bootrom->fw)
+		return;
+
+	release_firmware(bootrom->fw);
+	bootrom->fw = NULL;
+}
+
+static void gb_bootrom_timedout(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct gb_bootrom *bootrom = container_of(dwork,
+						  struct gb_bootrom, dwork);
+	struct device *dev = &bootrom->connection->bundle->dev;
+	const char *reason;
+
+	switch (bootrom->next_request) {
+	case NEXT_REQ_FIRMWARE_SIZE:
+		reason = "Firmware Size Request";
+		break;
+	case NEXT_REQ_GET_FIRMWARE:
+		reason = "Get Firmware Request";
+		break;
+	case NEXT_REQ_READY_TO_BOOT:
+		reason = "Ready to Boot Request";
+		break;
+	case NEXT_REQ_MODE_SWITCH:
+		reason = "Interface Mode Switch";
+		break;
+	default:
+		reason = NULL;
+		dev_err(dev, "Invalid next-request: %u", bootrom->next_request);
+		break;
+	}
+
+	dev_err(dev, "Timed out waiting for %s from the Module\n", reason);
+
+	mutex_lock(&bootrom->mutex);
+	free_firmware(bootrom);
+	mutex_unlock(&bootrom->mutex);
+
+	/* TODO: Power-off Module ? */
+}
+
+static void gb_bootrom_set_timeout(struct gb_bootrom *bootrom,
+				   enum next_request_type next,
+				   unsigned long timeout)
+{
+	bootrom->next_request = next;
+	schedule_delayed_work(&bootrom->dwork, msecs_to_jiffies(timeout));
+}
+
+static void gb_bootrom_cancel_timeout(struct gb_bootrom *bootrom)
+{
+	cancel_delayed_work_sync(&bootrom->dwork);
+}
+
+/*
+ * The es2 chip doesn't have VID/PID programmed into the hardware and we need to
+ * hack that up to distinguish different modules and their firmware blobs.
+ *
+ * This fetches VID/PID (over bootrom protocol) for es2 chip only, when VID/PID
+ * already sent during hotplug are 0.
+ *
+ * Otherwise, we keep intf->vendor_id/product_id same as what's passed
+ * during hotplug.
+ */
+static void bootrom_es2_fixup_vid_pid(struct gb_bootrom *bootrom)
+{
+	struct gb_bootrom_get_vid_pid_response response;
+	struct gb_connection *connection = bootrom->connection;
+	struct gb_interface *intf = connection->bundle->intf;
+	int ret;
+
+	if (!(intf->quirks & GB_INTERFACE_QUIRK_NO_GMP_IDS))
+		return;
+
+	ret = gb_operation_sync(connection, GB_BOOTROM_TYPE_GET_VID_PID,
+				NULL, 0, &response, sizeof(response));
+	if (ret) {
+		dev_err(&connection->bundle->dev,
+			"Bootrom get vid/pid operation failed (%d)\n", ret);
+		return;
+	}
+
+	/*
+	 * NOTE: This is hacked, so that the same values of VID/PID can be used
+	 * by next firmware level as well. The uevent for bootrom will still
+	 * have VID/PID as 0, though after this point the sysfs files will start
+	 * showing the updated values. But yeah, that's a bit racy as the same
+	 * sysfs files would be showing 0 before this point.
+	 */
+	intf->vendor_id = le32_to_cpu(response.vendor_id);
+	intf->product_id = le32_to_cpu(response.product_id);
+
+	dev_dbg(&connection->bundle->dev, "Bootrom got vid (0x%x)/pid (0x%x)\n",
+		intf->vendor_id, intf->product_id);
+}
+
+/* This returns path of the firmware blob on the disk */
+static int find_firmware(struct gb_bootrom *bootrom, u8 stage)
+{
+	struct gb_connection *connection = bootrom->connection;
+	struct gb_interface *intf = connection->bundle->intf;
+	char firmware_name[49];
+	int rc;
+
+	/* Already have a firmware, free it */
+	free_firmware(bootrom);
+
+	/* Bootrom protocol is only supported for loading Stage 2 firmware */
+	if (stage != 2) {
+		dev_err(&connection->bundle->dev, "Invalid boot stage: %u\n",
+			stage);
+		return -EINVAL;
+	}
+
+	/*
+	 * Create firmware name
+	 *
+	 * XXX Name it properly..
+	 */
+	snprintf(firmware_name, sizeof(firmware_name),
+		 FW_NAME_PREFIX "%08x_%08x_%08x_%08x_s2l.tftf",
+		 intf->ddbl1_manufacturer_id, intf->ddbl1_product_id,
+		 intf->vendor_id, intf->product_id);
+
+	// FIXME:
+	// Turn to dev_dbg later after everyone has valid bootloaders with good
+	// ids, but leave this as dev_info for now to make it easier to track
+	// down "empty" vid/pid modules.
+	dev_info(&connection->bundle->dev, "Firmware file '%s' requested\n",
+		 firmware_name);
+
+	rc = request_firmware(&bootrom->fw, firmware_name,
+			      &connection->bundle->dev);
+	if (rc) {
+		dev_err(&connection->bundle->dev,
+			"failed to find %s firmware (%d)\n", firmware_name, rc);
+	}
+
+	return rc;
+}
+
+static int gb_bootrom_firmware_size_request(struct gb_operation *op)
+{
+	struct gb_bootrom *bootrom = gb_connection_get_data(op->connection);
+	struct gb_bootrom_firmware_size_request *size_request =
+		op->request->payload;
+	struct gb_bootrom_firmware_size_response *size_response;
+	struct device *dev = &op->connection->bundle->dev;
+	int ret;
+
+	/* Disable timeouts */
+	gb_bootrom_cancel_timeout(bootrom);
+
+	if (op->request->payload_size != sizeof(*size_request)) {
+		dev_err(dev, "%s: illegal size of firmware size request (%zu != %zu)\n",
+			__func__, op->request->payload_size,
+			sizeof(*size_request));
+		ret = -EINVAL;
+		goto queue_work;
+	}
+
+	mutex_lock(&bootrom->mutex);
+
+	ret = find_firmware(bootrom, size_request->stage);
+	if (ret)
+		goto unlock;
+
+	if (!gb_operation_response_alloc(op, sizeof(*size_response),
+					 GFP_KERNEL)) {
+		dev_err(dev, "%s: error allocating response\n", __func__);
+		free_firmware(bootrom);
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	size_response = op->response->payload;
+	size_response->size = cpu_to_le32(bootrom->fw->size);
+
+	dev_dbg(dev, "%s: firmware size %d bytes\n",
+		__func__, size_response->size);
+
+unlock:
+	mutex_unlock(&bootrom->mutex);
+
+queue_work:
+	if (!ret) {
+		/* Refresh timeout */
+		gb_bootrom_set_timeout(bootrom, NEXT_REQ_GET_FIRMWARE,
+				       NEXT_REQ_TIMEOUT_MS);
+	}
+
+	return ret;
+}
+
+static int gb_bootrom_get_firmware(struct gb_operation *op)
+{
+	struct gb_bootrom *bootrom = gb_connection_get_data(op->connection);
+	const struct firmware *fw;
+	struct gb_bootrom_get_firmware_request *firmware_request;
+	struct gb_bootrom_get_firmware_response *firmware_response;
+	struct device *dev = &op->connection->bundle->dev;
+	unsigned int offset, size;
+	enum next_request_type next_request;
+	int ret = 0;
+
+	/* Disable timeouts */
+	gb_bootrom_cancel_timeout(bootrom);
+
+	if (op->request->payload_size != sizeof(*firmware_request)) {
+		dev_err(dev, "%s: Illegal size of get firmware request (%zu %zu)\n",
+			__func__, op->request->payload_size,
+			sizeof(*firmware_request));
+		ret = -EINVAL;
+		goto queue_work;
+	}
+
+	mutex_lock(&bootrom->mutex);
+
+	fw = bootrom->fw;
+	if (!fw) {
+		dev_err(dev, "%s: firmware not available\n", __func__);
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	firmware_request = op->request->payload;
+	offset = le32_to_cpu(firmware_request->offset);
+	size = le32_to_cpu(firmware_request->size);
+
+	if (offset >= fw->size || size > fw->size - offset) {
+		dev_warn(dev, "bad firmware request (offs = %u, size = %u)\n",
+			 offset, size);
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (!gb_operation_response_alloc(op, sizeof(*firmware_response) + size,
+					 GFP_KERNEL)) {
+		dev_err(dev, "%s: error allocating response\n", __func__);
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	firmware_response = op->response->payload;
+	memcpy(firmware_response->data, fw->data + offset, size);
+
+	dev_dbg(dev, "responding with firmware (offs = %u, size = %u)\n",
+		offset, size);
+
+unlock:
+	mutex_unlock(&bootrom->mutex);
+
+queue_work:
+	/* Refresh timeout */
+	if (!ret && (offset + size == fw->size))
+		next_request = NEXT_REQ_READY_TO_BOOT;
+	else
+		next_request = NEXT_REQ_GET_FIRMWARE;
+
+	gb_bootrom_set_timeout(bootrom, next_request, NEXT_REQ_TIMEOUT_MS);
+
+	return ret;
+}
+
+static int gb_bootrom_ready_to_boot(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_bootrom *bootrom = gb_connection_get_data(connection);
+	struct gb_bootrom_ready_to_boot_request *rtb_request;
+	struct device *dev = &connection->bundle->dev;
+	u8 status;
+	int ret = 0;
+
+	/* Disable timeouts */
+	gb_bootrom_cancel_timeout(bootrom);
+
+	if (op->request->payload_size != sizeof(*rtb_request)) {
+		dev_err(dev, "%s: Illegal size of ready to boot request (%zu %zu)\n",
+			__func__, op->request->payload_size,
+			sizeof(*rtb_request));
+		ret = -EINVAL;
+		goto queue_work;
+	}
+
+	rtb_request = op->request->payload;
+	status = rtb_request->status;
+
+	/* Return error if the blob was invalid */
+	if (status == GB_BOOTROM_BOOT_STATUS_INVALID) {
+		ret = -EINVAL;
+		goto queue_work;
+	}
+
+	/*
+	 * XXX Should we return error for insecure firmware?
+	 */
+	dev_dbg(dev, "ready to boot: 0x%x, 0\n", status);
+
+queue_work:
+	/*
+	 * Refresh timeout, the Interface shall load the new personality and
+	 * send a new hotplug request, which shall get rid of the bootrom
+	 * connection. As that can take some time, increase the timeout a bit.
+	 */
+	gb_bootrom_set_timeout(bootrom, NEXT_REQ_MODE_SWITCH,
+			       MODE_SWITCH_TIMEOUT_MS);
+
+	return ret;
+}
+
+static int gb_bootrom_request_handler(struct gb_operation *op)
+{
+	u8 type = op->type;
+
+	switch (type) {
+	case GB_BOOTROM_TYPE_FIRMWARE_SIZE:
+		return gb_bootrom_firmware_size_request(op);
+	case GB_BOOTROM_TYPE_GET_FIRMWARE:
+		return gb_bootrom_get_firmware(op);
+	case GB_BOOTROM_TYPE_READY_TO_BOOT:
+		return gb_bootrom_ready_to_boot(op);
+	default:
+		dev_err(&op->connection->bundle->dev,
+			"unsupported request: %u\n", type);
+		return -EINVAL;
+	}
+}
+
+static int gb_bootrom_get_version(struct gb_bootrom *bootrom)
+{
+	struct gb_bundle *bundle = bootrom->connection->bundle;
+	struct gb_bootrom_version_request request;
+	struct gb_bootrom_version_response response;
+	int ret;
+
+	request.major = GB_BOOTROM_VERSION_MAJOR;
+	request.minor = GB_BOOTROM_VERSION_MINOR;
+
+	ret = gb_operation_sync(bootrom->connection,
+				GB_BOOTROM_TYPE_VERSION,
+				&request, sizeof(request), &response,
+				sizeof(response));
+	if (ret) {
+		dev_err(&bundle->dev,
+			"failed to get protocol version: %d\n",
+			ret);
+		return ret;
+	}
+
+	if (response.major > request.major) {
+		dev_err(&bundle->dev,
+			"unsupported major protocol version (%u > %u)\n",
+			response.major, request.major);
+		return -ENOTSUPP;
+	}
+
+	bootrom->protocol_major = response.major;
+	bootrom->protocol_minor = response.minor;
+
+	dev_dbg(&bundle->dev, "%s - %u.%u\n", __func__, response.major,
+		response.minor);
+
+	return 0;
+}
+
+static int gb_bootrom_probe(struct gb_bundle *bundle,
+			    const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_bootrom *bootrom;
+	int ret;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_BOOTROM)
+		return -ENODEV;
+
+	bootrom = kzalloc(sizeof(*bootrom), GFP_KERNEL);
+	if (!bootrom)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle,
+					  le16_to_cpu(cport_desc->id),
+					  gb_bootrom_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto err_free_bootrom;
+	}
+
+	gb_connection_set_data(connection, bootrom);
+
+	bootrom->connection = connection;
+
+	mutex_init(&bootrom->mutex);
+	INIT_DELAYED_WORK(&bootrom->dwork, gb_bootrom_timedout);
+	greybus_set_drvdata(bundle, bootrom);
+
+	ret = gb_connection_enable_tx(connection);
+	if (ret)
+		goto err_connection_destroy;
+
+	ret = gb_bootrom_get_version(bootrom);
+	if (ret)
+		goto err_connection_disable;
+
+	bootrom_es2_fixup_vid_pid(bootrom);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto err_connection_disable;
+
+	/* Refresh timeout */
+	gb_bootrom_set_timeout(bootrom, NEXT_REQ_FIRMWARE_SIZE,
+			       NEXT_REQ_TIMEOUT_MS);
+
+	/* Tell bootrom we're ready. */
+	ret = gb_operation_sync(connection, GB_BOOTROM_TYPE_AP_READY, NULL, 0,
+				NULL, 0);
+	if (ret) {
+		dev_err(&connection->bundle->dev,
+			"failed to send AP READY: %d\n", ret);
+		goto err_cancel_timeout;
+	}
+
+	dev_dbg(&bundle->dev, "AP_READY sent\n");
+
+	return 0;
+
+err_cancel_timeout:
+	gb_bootrom_cancel_timeout(bootrom);
+err_connection_disable:
+	gb_connection_disable(connection);
+err_connection_destroy:
+	gb_connection_destroy(connection);
+err_free_bootrom:
+	kfree(bootrom);
+
+	return ret;
+}
+
+static void gb_bootrom_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_bootrom *bootrom = greybus_get_drvdata(bundle);
+
+	dev_dbg(&bundle->dev, "%s\n", __func__);
+
+	gb_connection_disable(bootrom->connection);
+
+	/* Disable timeouts */
+	gb_bootrom_cancel_timeout(bootrom);
+
+	/*
+	 * Release firmware:
+	 *
+	 * As the connection and the delayed work are already disabled, we don't
+	 * need to lock access to bootrom->fw here.
+	 */
+	free_firmware(bootrom);
+
+	gb_connection_destroy(bootrom->connection);
+	kfree(bootrom);
+}
+
+static const struct greybus_bundle_id gb_bootrom_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_BOOTROM) },
+	{ }
+};
+
+static struct greybus_driver gb_bootrom_driver = {
+	.name		= "bootrom",
+	.probe		= gb_bootrom_probe,
+	.disconnect	= gb_bootrom_disconnect,
+	.id_table	= gb_bootrom_id_table,
+};
+
+module_greybus_driver(gb_bootrom_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/camera.c b/drivers/staging/greybus/camera.c
new file mode 100644
index 000000000..cdbb42cd4
--- /dev/null
+++ b/drivers/staging/greybus/camera.c
@@ -0,0 +1,1382 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Camera protocol driver.
+ *
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <linux/greybus.h>
+
+#include "gb-camera.h"
+#include "greybus_protocols.h"
+
+enum gb_camera_debugs_buffer_id {
+	GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES,
+	GB_CAMERA_DEBUGFS_BUFFER_STREAMS,
+	GB_CAMERA_DEBUGFS_BUFFER_CAPTURE,
+	GB_CAMERA_DEBUGFS_BUFFER_FLUSH,
+	GB_CAMERA_DEBUGFS_BUFFER_MAX,
+};
+
+struct gb_camera_debugfs_buffer {
+	char data[PAGE_SIZE];
+	size_t length;
+};
+
+enum gb_camera_state {
+	GB_CAMERA_STATE_UNCONFIGURED,
+	GB_CAMERA_STATE_CONFIGURED,
+};
+
+/**
+ * struct gb_camera - A Greybus Camera Device
+ * @connection: the greybus connection for camera management
+ * @data_connection: the greybus connection for camera data
+ * @data_cport_id: the data CPort ID on the module side
+ * @mutex: protects the connection and state fields
+ * @state: the current module state
+ * @debugfs: debugfs entries for camera protocol operations testing
+ * @module: Greybus camera module registered to HOST processor.
+ */
+struct gb_camera {
+	struct gb_bundle *bundle;
+	struct gb_connection *connection;
+	struct gb_connection *data_connection;
+	u16 data_cport_id;
+
+	struct mutex mutex;
+	enum gb_camera_state state;
+
+	struct {
+		struct dentry *root;
+		struct gb_camera_debugfs_buffer *buffers;
+	} debugfs;
+
+	struct gb_camera_module module;
+};
+
+struct gb_camera_stream_config {
+	unsigned int width;
+	unsigned int height;
+	unsigned int format;
+	unsigned int vc;
+	unsigned int dt[2];
+	unsigned int max_size;
+};
+
+struct gb_camera_fmt_info {
+	enum v4l2_mbus_pixelcode mbus_code;
+	unsigned int gb_format;
+	unsigned int bpp;
+};
+
+/* GB format to media code map */
+static const struct gb_camera_fmt_info gb_fmt_info[] = {
+	{
+		.mbus_code = V4L2_MBUS_FMT_UYVY8_1X16,
+		.gb_format = 0x01,
+		.bpp	   = 16,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_NV12_1x8,
+		.gb_format = 0x12,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_NV21_1x8,
+		.gb_format = 0x13,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_YU12_1x8,
+		.gb_format = 0x16,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_YV12_1x8,
+		.gb_format = 0x17,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_JPEG_1X8,
+		.gb_format = 0x40,
+		.bpp	   = 0,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_GB_CAM_METADATA_1X8,
+		.gb_format = 0x41,
+		.bpp	   = 0,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_GB_CAM_DEBUG_DATA_1X8,
+		.gb_format = 0x42,
+		.bpp	   = 0,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SBGGR10_1X10,
+		.gb_format = 0x80,
+		.bpp	   = 10,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SGBRG10_1X10,
+		.gb_format = 0x81,
+		.bpp	   = 10,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SGRBG10_1X10,
+		.gb_format = 0x82,
+		.bpp	   = 10,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SRGGB10_1X10,
+		.gb_format = 0x83,
+		.bpp	   = 10,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SBGGR12_1X12,
+		.gb_format = 0x84,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SGBRG12_1X12,
+		.gb_format = 0x85,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SGRBG12_1X12,
+		.gb_format = 0x86,
+		.bpp	   = 12,
+	},
+	{
+		.mbus_code = V4L2_MBUS_FMT_SRGGB12_1X12,
+		.gb_format = 0x87,
+		.bpp	   = 12,
+	},
+};
+
+static const struct gb_camera_fmt_info *gb_camera_get_format_info(u16 gb_fmt)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
+		if (gb_fmt_info[i].gb_format == gb_fmt)
+			return &gb_fmt_info[i];
+	}
+
+	return NULL;
+}
+
+#define ES2_APB_CDSI0_CPORT		16
+#define ES2_APB_CDSI1_CPORT		17
+
+#define GB_CAMERA_MAX_SETTINGS_SIZE	8192
+
+#define gcam_dbg(gcam, format...)	dev_dbg(&gcam->bundle->dev, format)
+#define gcam_info(gcam, format...)	dev_info(&gcam->bundle->dev, format)
+#define gcam_err(gcam, format...)	dev_err(&gcam->bundle->dev, format)
+
+static int gb_camera_operation_sync_flags(struct gb_connection *connection,
+					  int type, unsigned int flags,
+					  void *request, size_t request_size,
+					  void *response, size_t *response_size)
+{
+	struct gb_operation *operation;
+	int ret;
+
+	operation = gb_operation_create_flags(connection, type, request_size,
+					      *response_size, flags,
+					      GFP_KERNEL);
+	if (!operation)
+		return  -ENOMEM;
+
+	if (request_size)
+		memcpy(operation->request->payload, request, request_size);
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret) {
+		dev_err(&connection->hd->dev,
+			"%s: synchronous operation of type 0x%02x failed: %d\n",
+			connection->name, type, ret);
+	} else {
+		*response_size = operation->response->payload_size;
+
+		if (operation->response->payload_size)
+			memcpy(response, operation->response->payload,
+			       operation->response->payload_size);
+	}
+
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static int gb_camera_get_max_pkt_size(struct gb_camera *gcam,
+		struct gb_camera_configure_streams_response *resp)
+{
+	unsigned int max_pkt_size = 0;
+	unsigned int i;
+
+	for (i = 0; i < resp->num_streams; i++) {
+		struct gb_camera_stream_config_response *cfg = &resp->config[i];
+		const struct gb_camera_fmt_info *fmt_info;
+		unsigned int pkt_size;
+
+		fmt_info = gb_camera_get_format_info(cfg->format);
+		if (!fmt_info) {
+			gcam_err(gcam, "unsupported greybus image format: %d\n",
+				 cfg->format);
+			return -EIO;
+		}
+
+		if (fmt_info->bpp == 0) {
+			pkt_size = le32_to_cpu(cfg->max_pkt_size);
+
+			if (pkt_size == 0) {
+				gcam_err(gcam,
+					 "Stream %u: invalid zero maximum packet size\n",
+					 i);
+				return -EIO;
+			}
+		} else {
+			pkt_size = le16_to_cpu(cfg->width) * fmt_info->bpp / 8;
+
+			if (pkt_size != le32_to_cpu(cfg->max_pkt_size)) {
+				gcam_err(gcam,
+					 "Stream %u: maximum packet size mismatch (%u/%u)\n",
+					 i, pkt_size, cfg->max_pkt_size);
+				return -EIO;
+			}
+		}
+
+		max_pkt_size = max(pkt_size, max_pkt_size);
+	}
+
+	return max_pkt_size;
+}
+
+/*
+ * Validate the stream configuration response verifying padding is correctly
+ * set and the returned number of streams is supported
+ */
+static const int gb_camera_configure_streams_validate_response(
+		struct gb_camera *gcam,
+		struct gb_camera_configure_streams_response *resp,
+		unsigned int nstreams)
+{
+	unsigned int i;
+
+	/* Validate the returned response structure */
+	if (resp->padding[0] || resp->padding[1]) {
+		gcam_err(gcam, "response padding != 0\n");
+		return -EIO;
+	}
+
+	if (resp->num_streams > nstreams) {
+		gcam_err(gcam, "got #streams %u > request %u\n",
+			 resp->num_streams, nstreams);
+		return -EIO;
+	}
+
+	for (i = 0; i < resp->num_streams; i++) {
+		struct gb_camera_stream_config_response *cfg = &resp->config[i];
+
+		if (cfg->padding) {
+			gcam_err(gcam, "stream #%u padding != 0\n", i);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware Configuration
+ */
+
+static int gb_camera_set_intf_power_mode(struct gb_camera *gcam, u8 intf_id,
+					 bool hs)
+{
+	struct gb_svc *svc = gcam->connection->hd->svc;
+	int ret;
+
+	if (hs)
+		ret = gb_svc_intf_set_power_mode(svc, intf_id,
+						 GB_SVC_UNIPRO_HS_SERIES_A,
+						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
+						 GB_SVC_SMALL_AMPLITUDE,
+						 GB_SVC_NO_DE_EMPHASIS,
+						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
+						 GB_SVC_PWRM_RXTERMINATION |
+						 GB_SVC_PWRM_TXTERMINATION, 0,
+						 NULL, NULL);
+	else
+		ret = gb_svc_intf_set_power_mode(svc, intf_id,
+						 GB_SVC_UNIPRO_HS_SERIES_A,
+						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
+						 2, 1,
+						 GB_SVC_SMALL_AMPLITUDE,
+						 GB_SVC_NO_DE_EMPHASIS,
+						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
+						 2, 1,
+						 0, 0,
+						 NULL, NULL);
+
+	return ret;
+}
+
+static int gb_camera_set_power_mode(struct gb_camera *gcam, bool hs)
+{
+	struct gb_interface *intf = gcam->connection->intf;
+	struct gb_svc *svc = gcam->connection->hd->svc;
+	int ret;
+
+	ret = gb_camera_set_intf_power_mode(gcam, intf->interface_id, hs);
+	if (ret < 0) {
+		gcam_err(gcam, "failed to set module interface to %s (%d)\n",
+			 hs ? "HS" : "PWM", ret);
+		return ret;
+	}
+
+	ret = gb_camera_set_intf_power_mode(gcam, svc->ap_intf_id, hs);
+	if (ret < 0) {
+		gb_camera_set_intf_power_mode(gcam, intf->interface_id, !hs);
+		gcam_err(gcam, "failed to set AP interface to %s (%d)\n",
+			 hs ? "HS" : "PWM", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+struct ap_csi_config_request {
+	__u8 csi_id;
+	__u8 flags;
+#define GB_CAMERA_CSI_FLAG_CLOCK_CONTINUOUS 0x01
+	__u8 num_lanes;
+	__u8 padding;
+	__le32 csi_clk_freq;
+	__le32 max_pkt_size;
+} __packed;
+
+/*
+ * TODO: Compute the number of lanes dynamically based on bandwidth
+ * requirements.
+ */
+#define GB_CAMERA_CSI_NUM_DATA_LANES		4
+
+#define GB_CAMERA_CSI_CLK_FREQ_MAX		999000000U
+#define GB_CAMERA_CSI_CLK_FREQ_MIN		100000000U
+#define GB_CAMERA_CSI_CLK_FREQ_MARGIN		150000000U
+
+static int gb_camera_setup_data_connection(struct gb_camera *gcam,
+		struct gb_camera_configure_streams_response *resp,
+		struct gb_camera_csi_params *csi_params)
+{
+	struct ap_csi_config_request csi_cfg;
+	struct gb_connection *conn;
+	unsigned int clk_freq;
+	int ret;
+
+	/*
+	 * Create the data connection between the camera module data CPort and
+	 * APB CDSI1. The CDSI1 CPort ID is hardcoded by the ES2 bridge.
+	 */
+	conn = gb_connection_create_offloaded(gcam->bundle, gcam->data_cport_id,
+					      GB_CONNECTION_FLAG_NO_FLOWCTRL |
+					      GB_CONNECTION_FLAG_CDSI1);
+	if (IS_ERR(conn))
+		return PTR_ERR(conn);
+
+	gcam->data_connection = conn;
+	gb_connection_set_data(conn, gcam);
+
+	ret = gb_connection_enable(conn);
+	if (ret)
+		goto error_conn_destroy;
+
+	/* Set the UniPro link to high speed mode. */
+	ret = gb_camera_set_power_mode(gcam, true);
+	if (ret < 0)
+		goto error_conn_disable;
+
+	/*
+	 * Configure the APB-A CSI-2 transmitter.
+	 *
+	 * Hardcode the number of lanes to 4 and compute the bus clock frequency
+	 * based on the module bandwidth requirements with a safety margin.
+	 */
+	memset(&csi_cfg, 0, sizeof(csi_cfg));
+	csi_cfg.csi_id = 1;
+	csi_cfg.flags = 0;
+	csi_cfg.num_lanes = GB_CAMERA_CSI_NUM_DATA_LANES;
+
+	clk_freq = resp->data_rate / 2 / GB_CAMERA_CSI_NUM_DATA_LANES;
+	clk_freq = clamp(clk_freq + GB_CAMERA_CSI_CLK_FREQ_MARGIN,
+			 GB_CAMERA_CSI_CLK_FREQ_MIN,
+			 GB_CAMERA_CSI_CLK_FREQ_MAX);
+	csi_cfg.csi_clk_freq = clk_freq;
+
+	ret = gb_camera_get_max_pkt_size(gcam, resp);
+	if (ret < 0) {
+		ret = -EIO;
+		goto error_power;
+	}
+	csi_cfg.max_pkt_size = ret;
+
+	ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
+			   sizeof(csi_cfg),
+			   GB_APB_REQUEST_CSI_TX_CONTROL, false);
+	if (ret < 0) {
+		gcam_err(gcam, "failed to start the CSI transmitter\n");
+		goto error_power;
+	}
+
+	if (csi_params) {
+		csi_params->clk_freq = csi_cfg.csi_clk_freq;
+		csi_params->num_lanes = csi_cfg.num_lanes;
+	}
+
+	return 0;
+
+error_power:
+	gb_camera_set_power_mode(gcam, false);
+error_conn_disable:
+	gb_connection_disable(gcam->data_connection);
+error_conn_destroy:
+	gb_connection_destroy(gcam->data_connection);
+	gcam->data_connection = NULL;
+	return ret;
+}
+
+static void gb_camera_teardown_data_connection(struct gb_camera *gcam)
+{
+	struct ap_csi_config_request csi_cfg;
+	int ret;
+
+	/* Stop the APB1 CSI transmitter. */
+	memset(&csi_cfg, 0, sizeof(csi_cfg));
+	csi_cfg.csi_id = 1;
+
+	ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
+			   sizeof(csi_cfg),
+			   GB_APB_REQUEST_CSI_TX_CONTROL, false);
+
+	if (ret < 0)
+		gcam_err(gcam, "failed to stop the CSI transmitter\n");
+
+	/* Set the UniPro link to low speed mode. */
+	gb_camera_set_power_mode(gcam, false);
+
+	/* Destroy the data connection. */
+	gb_connection_disable(gcam->data_connection);
+	gb_connection_destroy(gcam->data_connection);
+	gcam->data_connection = NULL;
+}
+
+/* -----------------------------------------------------------------------------
+ * Camera Protocol Operations
+ */
+
+static int gb_camera_capabilities(struct gb_camera *gcam,
+				  u8 *capabilities, size_t *size)
+{
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(gcam->bundle);
+	if (ret)
+		return ret;
+
+	mutex_lock(&gcam->mutex);
+
+	if (!gcam->connection) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = gb_camera_operation_sync_flags(gcam->connection,
+					     GB_CAMERA_TYPE_CAPABILITIES,
+					     GB_OPERATION_FLAG_SHORT_RESPONSE,
+					     NULL, 0,
+					     (void *)capabilities, size);
+	if (ret)
+		gcam_err(gcam, "failed to retrieve capabilities: %d\n", ret);
+
+done:
+	mutex_unlock(&gcam->mutex);
+
+	gb_pm_runtime_put_autosuspend(gcam->bundle);
+
+	return ret;
+}
+
+static int gb_camera_configure_streams(struct gb_camera *gcam,
+				       unsigned int *num_streams,
+				       unsigned int *flags,
+				       struct gb_camera_stream_config *streams,
+				       struct gb_camera_csi_params *csi_params)
+{
+	struct gb_camera_configure_streams_request *req;
+	struct gb_camera_configure_streams_response *resp;
+	unsigned int nstreams = *num_streams;
+	unsigned int i;
+	size_t req_size;
+	size_t resp_size;
+	int ret;
+
+	if (nstreams > GB_CAMERA_MAX_STREAMS)
+		return -EINVAL;
+
+	req_size = sizeof(*req) + nstreams * sizeof(req->config[0]);
+	resp_size = sizeof(*resp) + nstreams * sizeof(resp->config[0]);
+
+	req = kmalloc(req_size, GFP_KERNEL);
+	resp = kmalloc(resp_size, GFP_KERNEL);
+	if (!req || !resp) {
+		kfree(req);
+		kfree(resp);
+		return -ENOMEM;
+	}
+
+	req->num_streams = nstreams;
+	req->flags = *flags;
+	req->padding = 0;
+
+	for (i = 0; i < nstreams; ++i) {
+		struct gb_camera_stream_config_request *cfg = &req->config[i];
+
+		cfg->width = cpu_to_le16(streams[i].width);
+		cfg->height = cpu_to_le16(streams[i].height);
+		cfg->format = cpu_to_le16(streams[i].format);
+		cfg->padding = 0;
+	}
+
+	mutex_lock(&gcam->mutex);
+
+	ret = gb_pm_runtime_get_sync(gcam->bundle);
+	if (ret)
+		goto done_skip_pm_put;
+
+	if (!gcam->connection) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = gb_camera_operation_sync_flags(gcam->connection,
+					     GB_CAMERA_TYPE_CONFIGURE_STREAMS,
+					     GB_OPERATION_FLAG_SHORT_RESPONSE,
+					     req, req_size,
+					     resp, &resp_size);
+	if (ret < 0)
+		goto done;
+
+	ret = gb_camera_configure_streams_validate_response(gcam, resp,
+							    nstreams);
+	if (ret < 0)
+		goto done;
+
+	*flags = resp->flags;
+	*num_streams = resp->num_streams;
+
+	for (i = 0; i < resp->num_streams; ++i) {
+		struct gb_camera_stream_config_response *cfg = &resp->config[i];
+
+		streams[i].width = le16_to_cpu(cfg->width);
+		streams[i].height = le16_to_cpu(cfg->height);
+		streams[i].format = le16_to_cpu(cfg->format);
+		streams[i].vc = cfg->virtual_channel;
+		streams[i].dt[0] = cfg->data_type[0];
+		streams[i].dt[1] = cfg->data_type[1];
+		streams[i].max_size = le32_to_cpu(cfg->max_size);
+	}
+
+	if ((resp->flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED) ||
+	    (req->flags & GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY))
+		goto done;
+
+	if (gcam->state == GB_CAMERA_STATE_CONFIGURED) {
+		gb_camera_teardown_data_connection(gcam);
+		gcam->state = GB_CAMERA_STATE_UNCONFIGURED;
+
+		/*
+		 * When unconfiguring streams release the PM runtime reference
+		 * that was acquired when streams were configured. The bundle
+		 * won't be suspended until the PM runtime reference acquired at
+		 * the beginning of this function gets released right before
+		 * returning.
+		 */
+		gb_pm_runtime_put_noidle(gcam->bundle);
+	}
+
+	if (resp->num_streams == 0)
+		goto done;
+
+	/*
+	 * Make sure the bundle won't be suspended until streams get
+	 * unconfigured after the stream is configured successfully
+	 */
+	gb_pm_runtime_get_noresume(gcam->bundle);
+
+	/* Setup CSI-2 connection from APB-A to AP */
+	ret = gb_camera_setup_data_connection(gcam, resp, csi_params);
+	if (ret < 0) {
+		memset(req, 0, sizeof(*req));
+		gb_operation_sync(gcam->connection,
+				  GB_CAMERA_TYPE_CONFIGURE_STREAMS,
+				  req, sizeof(*req),
+				  resp, sizeof(*resp));
+		*flags = 0;
+		*num_streams = 0;
+		gb_pm_runtime_put_noidle(gcam->bundle);
+		goto done;
+	}
+
+	gcam->state = GB_CAMERA_STATE_CONFIGURED;
+
+done:
+	gb_pm_runtime_put_autosuspend(gcam->bundle);
+
+done_skip_pm_put:
+	mutex_unlock(&gcam->mutex);
+	kfree(req);
+	kfree(resp);
+	return ret;
+}
+
+static int gb_camera_capture(struct gb_camera *gcam, u32 request_id,
+			     unsigned int streams, unsigned int num_frames,
+			     size_t settings_size, const void *settings)
+{
+	struct gb_camera_capture_request *req;
+	size_t req_size;
+	int ret;
+
+	if (settings_size > GB_CAMERA_MAX_SETTINGS_SIZE)
+		return -EINVAL;
+
+	req_size = sizeof(*req) + settings_size;
+	req = kmalloc(req_size, GFP_KERNEL);
+	if (!req)
+		return -ENOMEM;
+
+	req->request_id = cpu_to_le32(request_id);
+	req->streams = streams;
+	req->padding = 0;
+	req->num_frames = cpu_to_le16(num_frames);
+	memcpy(req->settings, settings, settings_size);
+
+	mutex_lock(&gcam->mutex);
+
+	if (!gcam->connection) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_CAPTURE,
+				req, req_size, NULL, 0);
+done:
+	mutex_unlock(&gcam->mutex);
+
+	kfree(req);
+
+	return ret;
+}
+
+static int gb_camera_flush(struct gb_camera *gcam, u32 *request_id)
+{
+	struct gb_camera_flush_response resp;
+	int ret;
+
+	mutex_lock(&gcam->mutex);
+
+	if (!gcam->connection) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_FLUSH, NULL, 0,
+				&resp, sizeof(resp));
+
+	if (ret < 0)
+		goto done;
+
+	if (request_id)
+		*request_id = le32_to_cpu(resp.request_id);
+
+done:
+	mutex_unlock(&gcam->mutex);
+
+	return ret;
+}
+
+static int gb_camera_request_handler(struct gb_operation *op)
+{
+	struct gb_camera *gcam = gb_connection_get_data(op->connection);
+	struct gb_camera_metadata_request *payload;
+	struct gb_message *request;
+
+	if (op->type != GB_CAMERA_TYPE_METADATA) {
+		gcam_err(gcam, "Unsupported unsolicited event: %u\n", op->type);
+		return -EINVAL;
+	}
+
+	request = op->request;
+
+	if (request->payload_size < sizeof(*payload)) {
+		gcam_err(gcam, "Wrong event size received (%zu < %zu)\n",
+			 request->payload_size, sizeof(*payload));
+		return -EINVAL;
+	}
+
+	payload = request->payload;
+
+	gcam_dbg(gcam, "received metadata for request %u, frame %u, stream %u\n",
+		 payload->request_id, payload->frame_number, payload->stream);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Interface with HOST gmp camera.
+ */
+static unsigned int gb_camera_mbus_to_gb(enum v4l2_mbus_pixelcode mbus_code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
+		if (gb_fmt_info[i].mbus_code == mbus_code)
+			return gb_fmt_info[i].gb_format;
+	}
+	return gb_fmt_info[0].gb_format;
+}
+
+static enum v4l2_mbus_pixelcode gb_camera_gb_to_mbus(u16 gb_fmt)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
+		if (gb_fmt_info[i].gb_format == gb_fmt)
+			return gb_fmt_info[i].mbus_code;
+	}
+	return gb_fmt_info[0].mbus_code;
+}
+
+static ssize_t gb_camera_op_capabilities(void *priv, char *data, size_t len)
+{
+	struct gb_camera *gcam = priv;
+	size_t capabilities_len = len;
+	int ret;
+
+	ret = gb_camera_capabilities(gcam, data, &capabilities_len);
+	if (ret)
+		return ret;
+
+	return capabilities_len;
+}
+
+static int gb_camera_op_configure_streams(void *priv, unsigned int *nstreams,
+		unsigned int *flags, struct gb_camera_stream *streams,
+		struct gb_camera_csi_params *csi_params)
+{
+	struct gb_camera *gcam = priv;
+	struct gb_camera_stream_config *gb_streams;
+	unsigned int gb_flags = 0;
+	unsigned int gb_nstreams = *nstreams;
+	unsigned int i;
+	int ret;
+
+	if (gb_nstreams > GB_CAMERA_MAX_STREAMS)
+		return -EINVAL;
+
+	gb_streams = kcalloc(gb_nstreams, sizeof(*gb_streams), GFP_KERNEL);
+	if (!gb_streams)
+		return -ENOMEM;
+
+	for (i = 0; i < gb_nstreams; i++) {
+		gb_streams[i].width = streams[i].width;
+		gb_streams[i].height = streams[i].height;
+		gb_streams[i].format =
+			gb_camera_mbus_to_gb(streams[i].pixel_code);
+	}
+
+	if (*flags & GB_CAMERA_IN_FLAG_TEST)
+		gb_flags |= GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY;
+
+	ret = gb_camera_configure_streams(gcam, &gb_nstreams,
+					  &gb_flags, gb_streams, csi_params);
+	if (ret < 0)
+		goto done;
+	if (gb_nstreams > *nstreams) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	*flags = 0;
+	if (gb_flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED)
+		*flags |= GB_CAMERA_OUT_FLAG_ADJUSTED;
+
+	for (i = 0; i < gb_nstreams; i++) {
+		streams[i].width = gb_streams[i].width;
+		streams[i].height = gb_streams[i].height;
+		streams[i].vc = gb_streams[i].vc;
+		streams[i].dt[0] = gb_streams[i].dt[0];
+		streams[i].dt[1] = gb_streams[i].dt[1];
+		streams[i].max_size = gb_streams[i].max_size;
+		streams[i].pixel_code =
+			gb_camera_gb_to_mbus(gb_streams[i].format);
+	}
+	*nstreams = gb_nstreams;
+
+done:
+	kfree(gb_streams);
+	return ret;
+}
+
+static int gb_camera_op_capture(void *priv, u32 request_id,
+				unsigned int streams, unsigned int num_frames,
+				size_t settings_size, const void *settings)
+{
+	struct gb_camera *gcam = priv;
+
+	return gb_camera_capture(gcam, request_id, streams, num_frames,
+				 settings_size, settings);
+}
+
+static int gb_camera_op_flush(void *priv, u32 *request_id)
+{
+	struct gb_camera *gcam = priv;
+
+	return gb_camera_flush(gcam, request_id);
+}
+
+static const struct gb_camera_ops gb_cam_ops = {
+	.capabilities = gb_camera_op_capabilities,
+	.configure_streams = gb_camera_op_configure_streams,
+	.capture = gb_camera_op_capture,
+	.flush = gb_camera_op_flush,
+};
+
+/* -----------------------------------------------------------------------------
+ * DebugFS
+ */
+
+static ssize_t gb_camera_debugfs_capabilities(struct gb_camera *gcam,
+					      char *buf, size_t len)
+{
+	struct gb_camera_debugfs_buffer *buffer =
+		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES];
+	size_t size = 1024;
+	unsigned int i;
+	u8 *caps;
+	int ret;
+
+	caps = kmalloc(size, GFP_KERNEL);
+	if (!caps)
+		return -ENOMEM;
+
+	ret = gb_camera_capabilities(gcam, caps, &size);
+	if (ret < 0)
+		goto done;
+
+	/*
+	 * hex_dump_to_buffer() doesn't return the number of bytes dumped prior
+	 * to v4.0, we need our own implementation :-(
+	 */
+	buffer->length = 0;
+
+	for (i = 0; i < size; i += 16) {
+		unsigned int nbytes = min_t(unsigned int, size - i, 16);
+
+		buffer->length += sprintf(buffer->data + buffer->length,
+					  "%*ph\n", nbytes, caps + i);
+	}
+
+done:
+	kfree(caps);
+	return ret;
+}
+
+static ssize_t gb_camera_debugfs_configure_streams(struct gb_camera *gcam,
+						   char *buf, size_t len)
+{
+	struct gb_camera_debugfs_buffer *buffer =
+		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_STREAMS];
+	struct gb_camera_stream_config *streams;
+	unsigned int nstreams;
+	unsigned int flags;
+	unsigned int i;
+	char *token;
+	int ret;
+
+	/* Retrieve number of streams to configure */
+	token = strsep(&buf, ";");
+	if (!token)
+		return -EINVAL;
+
+	ret = kstrtouint(token, 10, &nstreams);
+	if (ret < 0)
+		return ret;
+
+	if (nstreams > GB_CAMERA_MAX_STREAMS)
+		return -EINVAL;
+
+	token = strsep(&buf, ";");
+	if (!token)
+		return -EINVAL;
+
+	ret = kstrtouint(token, 10, &flags);
+	if (ret < 0)
+		return ret;
+
+	/* For each stream to configure parse width, height and format */
+	streams = kcalloc(nstreams, sizeof(*streams), GFP_KERNEL);
+	if (!streams)
+		return -ENOMEM;
+
+	for (i = 0; i < nstreams; ++i) {
+		struct gb_camera_stream_config *stream = &streams[i];
+
+		/* width */
+		token = strsep(&buf, ";");
+		if (!token) {
+			ret = -EINVAL;
+			goto done;
+		}
+		ret = kstrtouint(token, 10, &stream->width);
+		if (ret < 0)
+			goto done;
+
+		/* height */
+		token = strsep(&buf, ";");
+		if (!token)
+			goto done;
+
+		ret = kstrtouint(token, 10, &stream->height);
+		if (ret < 0)
+			goto done;
+
+		/* Image format code */
+		token = strsep(&buf, ";");
+		if (!token)
+			goto done;
+
+		ret = kstrtouint(token, 16, &stream->format);
+		if (ret < 0)
+			goto done;
+	}
+
+	ret = gb_camera_configure_streams(gcam, &nstreams, &flags, streams,
+					  NULL);
+	if (ret < 0)
+		goto done;
+
+	buffer->length = sprintf(buffer->data, "%u;%u;", nstreams, flags);
+
+	for (i = 0; i < nstreams; ++i) {
+		struct gb_camera_stream_config *stream = &streams[i];
+
+		buffer->length += sprintf(buffer->data + buffer->length,
+					  "%u;%u;%u;%u;%u;%u;%u;",
+					  stream->width, stream->height,
+					  stream->format, stream->vc,
+					  stream->dt[0], stream->dt[1],
+					  stream->max_size);
+	}
+
+	ret = len;
+
+done:
+	kfree(streams);
+	return ret;
+};
+
+static ssize_t gb_camera_debugfs_capture(struct gb_camera *gcam,
+					 char *buf, size_t len)
+{
+	unsigned int request_id;
+	unsigned int streams_mask;
+	unsigned int num_frames;
+	char *token;
+	int ret;
+
+	/* Request id */
+	token = strsep(&buf, ";");
+	if (!token)
+		return -EINVAL;
+	ret = kstrtouint(token, 10, &request_id);
+	if (ret < 0)
+		return ret;
+
+	/* Stream mask */
+	token = strsep(&buf, ";");
+	if (!token)
+		return -EINVAL;
+	ret = kstrtouint(token, 16, &streams_mask);
+	if (ret < 0)
+		return ret;
+
+	/* number of frames */
+	token = strsep(&buf, ";");
+	if (!token)
+		return -EINVAL;
+	ret = kstrtouint(token, 10, &num_frames);
+	if (ret < 0)
+		return ret;
+
+	ret = gb_camera_capture(gcam, request_id, streams_mask, num_frames, 0,
+				NULL);
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+static ssize_t gb_camera_debugfs_flush(struct gb_camera *gcam,
+				       char *buf, size_t len)
+{
+	struct gb_camera_debugfs_buffer *buffer =
+		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_FLUSH];
+	unsigned int req_id;
+	int ret;
+
+	ret = gb_camera_flush(gcam, &req_id);
+	if (ret < 0)
+		return ret;
+
+	buffer->length = sprintf(buffer->data, "%u", req_id);
+
+	return len;
+}
+
+struct gb_camera_debugfs_entry {
+	const char *name;
+	unsigned int mask;
+	unsigned int buffer;
+	ssize_t (*execute)(struct gb_camera *gcam, char *buf, size_t len);
+};
+
+static const struct gb_camera_debugfs_entry gb_camera_debugfs_entries[] = {
+	{
+		.name = "capabilities",
+		.mask = S_IFREG | 0444,
+		.buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES,
+		.execute = gb_camera_debugfs_capabilities,
+	}, {
+		.name = "configure_streams",
+		.mask = S_IFREG | 0666,
+		.buffer = GB_CAMERA_DEBUGFS_BUFFER_STREAMS,
+		.execute = gb_camera_debugfs_configure_streams,
+	}, {
+		.name = "capture",
+		.mask = S_IFREG | 0666,
+		.buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPTURE,
+		.execute = gb_camera_debugfs_capture,
+	}, {
+		.name = "flush",
+		.mask = S_IFREG | 0666,
+		.buffer = GB_CAMERA_DEBUGFS_BUFFER_FLUSH,
+		.execute = gb_camera_debugfs_flush,
+	},
+};
+
+static ssize_t gb_camera_debugfs_read(struct file *file, char __user *buf,
+				      size_t len, loff_t *offset)
+{
+	const struct gb_camera_debugfs_entry *op = file->private_data;
+	struct gb_camera *gcam = file_inode(file)->i_private;
+	struct gb_camera_debugfs_buffer *buffer;
+	ssize_t ret;
+
+	/* For read-only entries the operation is triggered by a read. */
+	if (!(op->mask & 0222)) {
+		ret = op->execute(gcam, NULL, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	buffer = &gcam->debugfs.buffers[op->buffer];
+
+	return simple_read_from_buffer(buf, len, offset, buffer->data,
+				       buffer->length);
+}
+
+static ssize_t gb_camera_debugfs_write(struct file *file,
+				       const char __user *buf, size_t len,
+				       loff_t *offset)
+{
+	const struct gb_camera_debugfs_entry *op = file->private_data;
+	struct gb_camera *gcam = file_inode(file)->i_private;
+	ssize_t ret;
+	char *kbuf;
+
+	if (len > 1024)
+		return -EINVAL;
+
+	kbuf = memdup_user_nul(buf, len);
+	if (IS_ERR(kbuf))
+		return PTR_ERR(kbuf);
+
+	ret = op->execute(gcam, kbuf, len);
+
+done:
+	kfree(kbuf);
+	return ret;
+}
+
+static int gb_camera_debugfs_open(struct inode *inode, struct file *file)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
+		const struct gb_camera_debugfs_entry *entry =
+			&gb_camera_debugfs_entries[i];
+
+		if (!strcmp(file->f_path.dentry->d_iname, entry->name)) {
+			file->private_data = (void *)entry;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static const struct file_operations gb_camera_debugfs_ops = {
+	.open = gb_camera_debugfs_open,
+	.read = gb_camera_debugfs_read,
+	.write = gb_camera_debugfs_write,
+};
+
+static int gb_camera_debugfs_init(struct gb_camera *gcam)
+{
+	struct gb_connection *connection = gcam->connection;
+	char dirname[27];
+	unsigned int i;
+
+	/*
+	 * Create root debugfs entry and a file entry for each camera operation.
+	 */
+	snprintf(dirname, 27, "camera-%u.%u", connection->intf->interface_id,
+		 gcam->bundle->id);
+
+	gcam->debugfs.root = debugfs_create_dir(dirname, gb_debugfs_get());
+
+	gcam->debugfs.buffers =
+		vmalloc(array_size(GB_CAMERA_DEBUGFS_BUFFER_MAX,
+				   sizeof(*gcam->debugfs.buffers)));
+	if (!gcam->debugfs.buffers)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
+		const struct gb_camera_debugfs_entry *entry =
+			&gb_camera_debugfs_entries[i];
+
+		gcam->debugfs.buffers[i].length = 0;
+
+		debugfs_create_file(entry->name, entry->mask,
+				    gcam->debugfs.root, gcam,
+				    &gb_camera_debugfs_ops);
+	}
+
+	return 0;
+}
+
+static void gb_camera_debugfs_cleanup(struct gb_camera *gcam)
+{
+	debugfs_remove_recursive(gcam->debugfs.root);
+
+	vfree(gcam->debugfs.buffers);
+}
+
+/* -----------------------------------------------------------------------------
+ * Init & Cleanup
+ */
+
+static void gb_camera_cleanup(struct gb_camera *gcam)
+{
+	gb_camera_debugfs_cleanup(gcam);
+
+	mutex_lock(&gcam->mutex);
+	if (gcam->data_connection) {
+		gb_connection_disable(gcam->data_connection);
+		gb_connection_destroy(gcam->data_connection);
+		gcam->data_connection = NULL;
+	}
+
+	if (gcam->connection) {
+		gb_connection_disable(gcam->connection);
+		gb_connection_destroy(gcam->connection);
+		gcam->connection = NULL;
+	}
+	mutex_unlock(&gcam->mutex);
+}
+
+static void gb_camera_release_module(struct kref *ref)
+{
+	struct gb_camera_module *cam_mod =
+		container_of(ref, struct gb_camera_module, refcount);
+	kfree(cam_mod->priv);
+}
+
+static int gb_camera_probe(struct gb_bundle *bundle,
+			   const struct greybus_bundle_id *id)
+{
+	struct gb_connection *conn;
+	struct gb_camera *gcam;
+	u16 mgmt_cport_id = 0;
+	u16 data_cport_id = 0;
+	unsigned int i;
+	int ret;
+
+	/*
+	 * The camera bundle must contain exactly two CPorts, one for the
+	 * camera management protocol and one for the camera data protocol.
+	 */
+	if (bundle->num_cports != 2)
+		return -ENODEV;
+
+	for (i = 0; i < bundle->num_cports; ++i) {
+		struct greybus_descriptor_cport *desc = &bundle->cport_desc[i];
+
+		switch (desc->protocol_id) {
+		case GREYBUS_PROTOCOL_CAMERA_MGMT:
+			mgmt_cport_id = le16_to_cpu(desc->id);
+			break;
+		case GREYBUS_PROTOCOL_CAMERA_DATA:
+			data_cport_id = le16_to_cpu(desc->id);
+			break;
+		default:
+			return -ENODEV;
+		}
+	}
+
+	if (!mgmt_cport_id || !data_cport_id)
+		return -ENODEV;
+
+	gcam = kzalloc(sizeof(*gcam), GFP_KERNEL);
+	if (!gcam)
+		return -ENOMEM;
+
+	mutex_init(&gcam->mutex);
+
+	gcam->bundle = bundle;
+	gcam->state = GB_CAMERA_STATE_UNCONFIGURED;
+	gcam->data_cport_id = data_cport_id;
+
+	conn = gb_connection_create(bundle, mgmt_cport_id,
+				    gb_camera_request_handler);
+	if (IS_ERR(conn)) {
+		ret = PTR_ERR(conn);
+		goto error;
+	}
+
+	gcam->connection = conn;
+	gb_connection_set_data(conn, gcam);
+
+	ret = gb_connection_enable(conn);
+	if (ret)
+		goto error;
+
+	ret = gb_camera_debugfs_init(gcam);
+	if (ret < 0)
+		goto error;
+
+	gcam->module.priv = gcam;
+	gcam->module.ops = &gb_cam_ops;
+	gcam->module.interface_id = gcam->connection->intf->interface_id;
+	gcam->module.release = gb_camera_release_module;
+	ret = gb_camera_register(&gcam->module);
+	if (ret < 0)
+		goto error;
+
+	greybus_set_drvdata(bundle, gcam);
+
+	gb_pm_runtime_put_autosuspend(gcam->bundle);
+
+	return 0;
+
+error:
+	gb_camera_cleanup(gcam);
+	kfree(gcam);
+	return ret;
+}
+
+static void gb_camera_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_camera *gcam = greybus_get_drvdata(bundle);
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		gb_pm_runtime_get_noresume(bundle);
+
+	gb_camera_cleanup(gcam);
+	gb_camera_unregister(&gcam->module);
+}
+
+static const struct greybus_bundle_id gb_camera_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_CAMERA) },
+	{ },
+};
+
+#ifdef CONFIG_PM
+static int gb_camera_suspend(struct device *dev)
+{
+	struct gb_bundle *bundle = to_gb_bundle(dev);
+	struct gb_camera *gcam = greybus_get_drvdata(bundle);
+
+	if (gcam->data_connection)
+		gb_connection_disable(gcam->data_connection);
+
+	gb_connection_disable(gcam->connection);
+
+	return 0;
+}
+
+static int gb_camera_resume(struct device *dev)
+{
+	struct gb_bundle *bundle = to_gb_bundle(dev);
+	struct gb_camera *gcam = greybus_get_drvdata(bundle);
+	int ret;
+
+	ret = gb_connection_enable(gcam->connection);
+	if (ret) {
+		gcam_err(gcam, "failed to enable connection: %d\n", ret);
+		return ret;
+	}
+
+	if (gcam->data_connection) {
+		ret = gb_connection_enable(gcam->data_connection);
+		if (ret) {
+			gcam_err(gcam,
+				 "failed to enable data connection: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops gb_camera_pm_ops = {
+	SET_RUNTIME_PM_OPS(gb_camera_suspend, gb_camera_resume, NULL)
+};
+
+static struct greybus_driver gb_camera_driver = {
+	.name		= "camera",
+	.probe		= gb_camera_probe,
+	.disconnect	= gb_camera_disconnect,
+	.id_table	= gb_camera_id_table,
+	.driver.pm	= &gb_camera_pm_ops,
+};
+
+module_greybus_driver(gb_camera_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/firmware.h b/drivers/staging/greybus/firmware.h
new file mode 100644
index 000000000..5d2564462
--- /dev/null
+++ b/drivers/staging/greybus/firmware.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus Firmware Management Header
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+
+#ifndef __FIRMWARE_H
+#define __FIRMWARE_H
+
+#include <linux/greybus.h>
+
+#define FW_NAME_PREFIX	"gmp_"
+
+/*
+ * Length of the string in format: "FW_NAME_PREFIX""%08x_%08x_%08x_%08x_%s.tftf"
+ *                                  (3 + 1 + 4 * (8 + 1) + 10 + 1 + 4 + 1)
+ */
+#define FW_NAME_SIZE		56
+
+/* Firmware Management Protocol specific functions */
+int fw_mgmt_init(void);
+void fw_mgmt_exit(void);
+struct gb_connection *to_fw_mgmt_connection(struct device *dev);
+int gb_fw_mgmt_request_handler(struct gb_operation *op);
+int gb_fw_mgmt_connection_init(struct gb_connection *connection);
+void gb_fw_mgmt_connection_exit(struct gb_connection *connection);
+
+/* Firmware Download Protocol specific functions */
+int gb_fw_download_request_handler(struct gb_operation *op);
+int gb_fw_download_connection_init(struct gb_connection *connection);
+void gb_fw_download_connection_exit(struct gb_connection *connection);
+
+/* CAP Protocol specific functions */
+int cap_init(void);
+void cap_exit(void);
+int gb_cap_connection_init(struct gb_connection *connection);
+void gb_cap_connection_exit(struct gb_connection *connection);
+
+#endif /* __FIRMWARE_H */
diff --git a/drivers/staging/greybus/fw-core.c b/drivers/staging/greybus/fw-core.c
new file mode 100644
index 000000000..57bebf246
--- /dev/null
+++ b/drivers/staging/greybus/fw-core.c
@@ -0,0 +1,311 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Firmware Core Bundle Driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/firmware.h>
+#include <linux/greybus.h>
+#include "firmware.h"
+#include "spilib.h"
+
+struct gb_fw_core {
+	struct gb_connection	*download_connection;
+	struct gb_connection	*mgmt_connection;
+	struct gb_connection	*spi_connection;
+	struct gb_connection	*cap_connection;
+};
+
+static struct spilib_ops *spilib_ops;
+
+struct gb_connection *to_fw_mgmt_connection(struct device *dev)
+{
+	struct gb_fw_core *fw_core = dev_get_drvdata(dev);
+
+	return fw_core->mgmt_connection;
+}
+
+static int gb_fw_spi_connection_init(struct gb_connection *connection)
+{
+	int ret;
+
+	if (!connection)
+		return 0;
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		return ret;
+
+	ret = gb_spilib_master_init(connection, &connection->bundle->dev,
+				    spilib_ops);
+	if (ret) {
+		gb_connection_disable(connection);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void gb_fw_spi_connection_exit(struct gb_connection *connection)
+{
+	if (!connection)
+		return;
+
+	gb_spilib_master_exit(connection);
+	gb_connection_disable(connection);
+}
+
+static int gb_fw_core_probe(struct gb_bundle *bundle,
+			    const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_fw_core *fw_core;
+	int ret, i;
+	u16 cport_id;
+	u8 protocol_id;
+
+	fw_core = kzalloc(sizeof(*fw_core), GFP_KERNEL);
+	if (!fw_core)
+		return -ENOMEM;
+
+	/* Parse CPorts and create connections */
+	for (i = 0; i < bundle->num_cports; i++) {
+		cport_desc = &bundle->cport_desc[i];
+		cport_id = le16_to_cpu(cport_desc->id);
+		protocol_id = cport_desc->protocol_id;
+
+		switch (protocol_id) {
+		case GREYBUS_PROTOCOL_FW_MANAGEMENT:
+			/* Disallow multiple Firmware Management CPorts */
+			if (fw_core->mgmt_connection) {
+				dev_err(&bundle->dev,
+					"multiple management CPorts found\n");
+				ret = -EINVAL;
+				goto err_destroy_connections;
+			}
+
+			connection = gb_connection_create(bundle, cport_id,
+						gb_fw_mgmt_request_handler);
+			if (IS_ERR(connection)) {
+				ret = PTR_ERR(connection);
+				dev_err(&bundle->dev,
+					"failed to create management connection (%d)\n",
+					ret);
+				goto err_destroy_connections;
+			}
+
+			fw_core->mgmt_connection = connection;
+			break;
+		case GREYBUS_PROTOCOL_FW_DOWNLOAD:
+			/* Disallow multiple Firmware Download CPorts */
+			if (fw_core->download_connection) {
+				dev_err(&bundle->dev,
+					"multiple download CPorts found\n");
+				ret = -EINVAL;
+				goto err_destroy_connections;
+			}
+
+			connection = gb_connection_create(bundle, cport_id,
+						gb_fw_download_request_handler);
+			if (IS_ERR(connection)) {
+				dev_err(&bundle->dev, "failed to create download connection (%ld)\n",
+					PTR_ERR(connection));
+			} else {
+				fw_core->download_connection = connection;
+			}
+
+			break;
+		case GREYBUS_PROTOCOL_SPI:
+			/* Disallow multiple SPI CPorts */
+			if (fw_core->spi_connection) {
+				dev_err(&bundle->dev,
+					"multiple SPI CPorts found\n");
+				ret = -EINVAL;
+				goto err_destroy_connections;
+			}
+
+			connection = gb_connection_create(bundle, cport_id,
+							  NULL);
+			if (IS_ERR(connection)) {
+				dev_err(&bundle->dev, "failed to create SPI connection (%ld)\n",
+					PTR_ERR(connection));
+			} else {
+				fw_core->spi_connection = connection;
+			}
+
+			break;
+		case GREYBUS_PROTOCOL_AUTHENTICATION:
+			/* Disallow multiple CAP CPorts */
+			if (fw_core->cap_connection) {
+				dev_err(&bundle->dev, "multiple Authentication CPorts found\n");
+				ret = -EINVAL;
+				goto err_destroy_connections;
+			}
+
+			connection = gb_connection_create(bundle, cport_id,
+							  NULL);
+			if (IS_ERR(connection)) {
+				dev_err(&bundle->dev, "failed to create Authentication connection (%ld)\n",
+					PTR_ERR(connection));
+			} else {
+				fw_core->cap_connection = connection;
+			}
+
+			break;
+		default:
+			dev_err(&bundle->dev, "invalid protocol id (0x%02x)\n",
+				protocol_id);
+			ret = -EINVAL;
+			goto err_destroy_connections;
+		}
+	}
+
+	/* Firmware Management connection is mandatory */
+	if (!fw_core->mgmt_connection) {
+		dev_err(&bundle->dev, "missing management connection\n");
+		ret = -ENODEV;
+		goto err_destroy_connections;
+	}
+
+	ret = gb_fw_download_connection_init(fw_core->download_connection);
+	if (ret) {
+		/* We may still be able to work with the Interface */
+		dev_err(&bundle->dev, "failed to initialize firmware download connection, disable it (%d)\n",
+			ret);
+		gb_connection_destroy(fw_core->download_connection);
+		fw_core->download_connection = NULL;
+	}
+
+	ret = gb_fw_spi_connection_init(fw_core->spi_connection);
+	if (ret) {
+		/* We may still be able to work with the Interface */
+		dev_err(&bundle->dev, "failed to initialize SPI connection, disable it (%d)\n",
+			ret);
+		gb_connection_destroy(fw_core->spi_connection);
+		fw_core->spi_connection = NULL;
+	}
+
+	ret = gb_cap_connection_init(fw_core->cap_connection);
+	if (ret) {
+		/* We may still be able to work with the Interface */
+		dev_err(&bundle->dev, "failed to initialize CAP connection, disable it (%d)\n",
+			ret);
+		gb_connection_destroy(fw_core->cap_connection);
+		fw_core->cap_connection = NULL;
+	}
+
+	ret = gb_fw_mgmt_connection_init(fw_core->mgmt_connection);
+	if (ret) {
+		/* We may still be able to work with the Interface */
+		dev_err(&bundle->dev, "failed to initialize firmware management connection, disable it (%d)\n",
+			ret);
+		goto err_exit_connections;
+	}
+
+	greybus_set_drvdata(bundle, fw_core);
+
+	/* FIXME: Remove this after S2 Loader gets runtime PM support */
+	if (!(bundle->intf->quirks & GB_INTERFACE_QUIRK_NO_PM))
+		gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+err_exit_connections:
+	gb_cap_connection_exit(fw_core->cap_connection);
+	gb_fw_spi_connection_exit(fw_core->spi_connection);
+	gb_fw_download_connection_exit(fw_core->download_connection);
+err_destroy_connections:
+	gb_connection_destroy(fw_core->mgmt_connection);
+	gb_connection_destroy(fw_core->cap_connection);
+	gb_connection_destroy(fw_core->spi_connection);
+	gb_connection_destroy(fw_core->download_connection);
+	kfree(fw_core);
+
+	return ret;
+}
+
+static void gb_fw_core_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_fw_core *fw_core = greybus_get_drvdata(bundle);
+	int ret;
+
+	/* FIXME: Remove this after S2 Loader gets runtime PM support */
+	if (!(bundle->intf->quirks & GB_INTERFACE_QUIRK_NO_PM)) {
+		ret = gb_pm_runtime_get_sync(bundle);
+		if (ret)
+			gb_pm_runtime_get_noresume(bundle);
+	}
+
+	gb_fw_mgmt_connection_exit(fw_core->mgmt_connection);
+	gb_cap_connection_exit(fw_core->cap_connection);
+	gb_fw_spi_connection_exit(fw_core->spi_connection);
+	gb_fw_download_connection_exit(fw_core->download_connection);
+
+	gb_connection_destroy(fw_core->mgmt_connection);
+	gb_connection_destroy(fw_core->cap_connection);
+	gb_connection_destroy(fw_core->spi_connection);
+	gb_connection_destroy(fw_core->download_connection);
+
+	kfree(fw_core);
+}
+
+static const struct greybus_bundle_id gb_fw_core_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_FW_MANAGEMENT) },
+	{ }
+};
+
+static struct greybus_driver gb_fw_core_driver = {
+	.name		= "gb-firmware",
+	.probe		= gb_fw_core_probe,
+	.disconnect	= gb_fw_core_disconnect,
+	.id_table	= gb_fw_core_id_table,
+};
+
+static int fw_core_init(void)
+{
+	int ret;
+
+	ret = fw_mgmt_init();
+	if (ret) {
+		pr_err("Failed to initialize fw-mgmt core (%d)\n", ret);
+		return ret;
+	}
+
+	ret = cap_init();
+	if (ret) {
+		pr_err("Failed to initialize component authentication core (%d)\n",
+		       ret);
+		goto fw_mgmt_exit;
+	}
+
+	ret = greybus_register(&gb_fw_core_driver);
+	if (ret)
+		goto cap_exit;
+
+	return 0;
+
+cap_exit:
+	cap_exit();
+fw_mgmt_exit:
+	fw_mgmt_exit();
+
+	return ret;
+}
+module_init(fw_core_init);
+
+static void __exit fw_core_exit(void)
+{
+	greybus_deregister(&gb_fw_core_driver);
+	cap_exit();
+	fw_mgmt_exit();
+}
+module_exit(fw_core_exit);
+
+MODULE_ALIAS("greybus:firmware");
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
+MODULE_DESCRIPTION("Greybus Firmware Bundle Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/fw-download.c b/drivers/staging/greybus/fw-download.c
new file mode 100644
index 000000000..543692c56
--- /dev/null
+++ b/drivers/staging/greybus/fw-download.c
@@ -0,0 +1,466 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Firmware Download Protocol Driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+
+#include <linux/firmware.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/greybus.h>
+#include "firmware.h"
+
+/* Estimated minimum buffer size, actual size can be smaller than this */
+#define MIN_FETCH_SIZE		512
+/* Timeout, in jiffies, within which fetch or release firmware must be called */
+#define NEXT_REQ_TIMEOUT_J	msecs_to_jiffies(1000)
+
+struct fw_request {
+	u8			firmware_id;
+	bool			disabled;
+	bool			timedout;
+	char			name[FW_NAME_SIZE];
+	const struct firmware	*fw;
+	struct list_head	node;
+
+	struct delayed_work	dwork;
+	/* Timeout, in jiffies, within which the firmware shall download */
+	unsigned long		release_timeout_j;
+	struct kref		kref;
+	struct fw_download	*fw_download;
+};
+
+struct fw_download {
+	struct device		*parent;
+	struct gb_connection	*connection;
+	struct list_head	fw_requests;
+	struct ida		id_map;
+	struct mutex		mutex;
+};
+
+static void fw_req_release(struct kref *kref)
+{
+	struct fw_request *fw_req = container_of(kref, struct fw_request, kref);
+
+	dev_dbg(fw_req->fw_download->parent, "firmware %s released\n",
+		fw_req->name);
+
+	release_firmware(fw_req->fw);
+
+	/*
+	 * The request timed out and the module may send a fetch-fw or
+	 * release-fw request later. Lets block the id we allocated for this
+	 * request, so that the AP doesn't refer to a later fw-request (with
+	 * same firmware_id) for the old timedout fw-request.
+	 *
+	 * NOTE:
+	 *
+	 * This also means that after 255 timeouts we will fail to service new
+	 * firmware downloads. But what else can we do in that case anyway? Lets
+	 * just hope that it never happens.
+	 */
+	if (!fw_req->timedout)
+		ida_simple_remove(&fw_req->fw_download->id_map,
+				  fw_req->firmware_id);
+
+	kfree(fw_req);
+}
+
+/*
+ * Incoming requests are serialized for a connection, and the only race possible
+ * is between the timeout handler freeing this and an incoming request.
+ *
+ * The operations on the fw-request list are protected by the mutex and
+ * get_fw_req() increments the reference count before returning a fw_req pointer
+ * to the users.
+ *
+ * free_firmware() also takes the mutex while removing an entry from the list,
+ * it guarantees that every user of fw_req has taken a kref-reference by now and
+ * we wouldn't have any new users.
+ *
+ * Once the last user drops the reference, the fw_req structure is freed.
+ */
+static void put_fw_req(struct fw_request *fw_req)
+{
+	kref_put(&fw_req->kref, fw_req_release);
+}
+
+/* Caller must call put_fw_req() after using struct fw_request */
+static struct fw_request *get_fw_req(struct fw_download *fw_download,
+				     u8 firmware_id)
+{
+	struct fw_request *fw_req;
+
+	mutex_lock(&fw_download->mutex);
+
+	list_for_each_entry(fw_req, &fw_download->fw_requests, node) {
+		if (fw_req->firmware_id == firmware_id) {
+			kref_get(&fw_req->kref);
+			goto unlock;
+		}
+	}
+
+	fw_req = NULL;
+
+unlock:
+	mutex_unlock(&fw_download->mutex);
+
+	return fw_req;
+}
+
+static void free_firmware(struct fw_download *fw_download,
+			  struct fw_request *fw_req)
+{
+	/* Already disabled from timeout handlers */
+	if (fw_req->disabled)
+		return;
+
+	mutex_lock(&fw_download->mutex);
+	list_del(&fw_req->node);
+	mutex_unlock(&fw_download->mutex);
+
+	fw_req->disabled = true;
+	put_fw_req(fw_req);
+}
+
+static void fw_request_timedout(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct fw_request *fw_req = container_of(dwork,
+						 struct fw_request, dwork);
+	struct fw_download *fw_download = fw_req->fw_download;
+
+	dev_err(fw_download->parent,
+		"Timed out waiting for fetch / release firmware requests: %u\n",
+		fw_req->firmware_id);
+
+	fw_req->timedout = true;
+	free_firmware(fw_download, fw_req);
+}
+
+static int exceeds_release_timeout(struct fw_request *fw_req)
+{
+	struct fw_download *fw_download = fw_req->fw_download;
+
+	if (time_before(jiffies, fw_req->release_timeout_j))
+		return 0;
+
+	dev_err(fw_download->parent,
+		"Firmware download didn't finish in time, abort: %d\n",
+		fw_req->firmware_id);
+
+	fw_req->timedout = true;
+	free_firmware(fw_download, fw_req);
+
+	return -ETIMEDOUT;
+}
+
+/* This returns path of the firmware blob on the disk */
+static struct fw_request *find_firmware(struct fw_download *fw_download,
+					const char *tag)
+{
+	struct gb_interface *intf = fw_download->connection->bundle->intf;
+	struct fw_request *fw_req;
+	int ret, req_count;
+
+	fw_req = kzalloc(sizeof(*fw_req), GFP_KERNEL);
+	if (!fw_req)
+		return ERR_PTR(-ENOMEM);
+
+	/* Allocate ids from 1 to 255 (u8-max), 0 is an invalid id */
+	ret = ida_simple_get(&fw_download->id_map, 1, 256, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(fw_download->parent,
+			"failed to allocate firmware id (%d)\n", ret);
+		goto err_free_req;
+	}
+	fw_req->firmware_id = ret;
+
+	snprintf(fw_req->name, sizeof(fw_req->name),
+		 FW_NAME_PREFIX "%08x_%08x_%08x_%08x_%s.tftf",
+		 intf->ddbl1_manufacturer_id, intf->ddbl1_product_id,
+		 intf->vendor_id, intf->product_id, tag);
+
+	dev_info(fw_download->parent, "Requested firmware package '%s'\n",
+		 fw_req->name);
+
+	ret = request_firmware(&fw_req->fw, fw_req->name, fw_download->parent);
+	if (ret) {
+		dev_err(fw_download->parent,
+			"firmware request failed for %s (%d)\n", fw_req->name,
+			ret);
+		goto err_free_id;
+	}
+
+	fw_req->fw_download = fw_download;
+	kref_init(&fw_req->kref);
+
+	mutex_lock(&fw_download->mutex);
+	list_add(&fw_req->node, &fw_download->fw_requests);
+	mutex_unlock(&fw_download->mutex);
+
+	/* Timeout, in jiffies, within which firmware should get loaded */
+	req_count = DIV_ROUND_UP(fw_req->fw->size, MIN_FETCH_SIZE);
+	fw_req->release_timeout_j = jiffies + req_count * NEXT_REQ_TIMEOUT_J;
+
+	INIT_DELAYED_WORK(&fw_req->dwork, fw_request_timedout);
+	schedule_delayed_work(&fw_req->dwork, NEXT_REQ_TIMEOUT_J);
+
+	return fw_req;
+
+err_free_id:
+	ida_simple_remove(&fw_download->id_map, fw_req->firmware_id);
+err_free_req:
+	kfree(fw_req);
+
+	return ERR_PTR(ret);
+}
+
+static int fw_download_find_firmware(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct fw_download *fw_download = gb_connection_get_data(connection);
+	struct gb_fw_download_find_firmware_request *request;
+	struct gb_fw_download_find_firmware_response *response;
+	struct fw_request *fw_req;
+	const char *tag;
+
+	if (op->request->payload_size != sizeof(*request)) {
+		dev_err(fw_download->parent,
+			"illegal size of find firmware request (%zu != %zu)\n",
+			op->request->payload_size, sizeof(*request));
+		return -EINVAL;
+	}
+
+	request = op->request->payload;
+	tag = (const char *)request->firmware_tag;
+
+	/* firmware_tag must be null-terminated */
+	if (strnlen(tag, GB_FIRMWARE_TAG_MAX_SIZE) ==
+	    GB_FIRMWARE_TAG_MAX_SIZE) {
+		dev_err(fw_download->parent,
+			"firmware-tag is not null-terminated\n");
+		return -EINVAL;
+	}
+
+	fw_req = find_firmware(fw_download, tag);
+	if (IS_ERR(fw_req))
+		return PTR_ERR(fw_req);
+
+	if (!gb_operation_response_alloc(op, sizeof(*response), GFP_KERNEL)) {
+		dev_err(fw_download->parent, "error allocating response\n");
+		free_firmware(fw_download, fw_req);
+		return -ENOMEM;
+	}
+
+	response = op->response->payload;
+	response->firmware_id = fw_req->firmware_id;
+	response->size = cpu_to_le32(fw_req->fw->size);
+
+	dev_dbg(fw_download->parent,
+		"firmware size is %zu bytes\n", fw_req->fw->size);
+
+	return 0;
+}
+
+static int fw_download_fetch_firmware(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct fw_download *fw_download = gb_connection_get_data(connection);
+	struct gb_fw_download_fetch_firmware_request *request;
+	struct gb_fw_download_fetch_firmware_response *response;
+	struct fw_request *fw_req;
+	const struct firmware *fw;
+	unsigned int offset, size;
+	u8 firmware_id;
+	int ret = 0;
+
+	if (op->request->payload_size != sizeof(*request)) {
+		dev_err(fw_download->parent,
+			"Illegal size of fetch firmware request (%zu %zu)\n",
+			op->request->payload_size, sizeof(*request));
+		return -EINVAL;
+	}
+
+	request = op->request->payload;
+	offset = le32_to_cpu(request->offset);
+	size = le32_to_cpu(request->size);
+	firmware_id = request->firmware_id;
+
+	fw_req = get_fw_req(fw_download, firmware_id);
+	if (!fw_req) {
+		dev_err(fw_download->parent,
+			"firmware not available for id: %02u\n", firmware_id);
+		return -EINVAL;
+	}
+
+	/* Make sure work handler isn't running in parallel */
+	cancel_delayed_work_sync(&fw_req->dwork);
+
+	/* We timed-out before reaching here ? */
+	if (fw_req->disabled) {
+		ret = -ETIMEDOUT;
+		goto put_fw;
+	}
+
+	/*
+	 * Firmware download must finish within a limited time interval. If it
+	 * doesn't, then we might have a buggy Module on the other side. Abort
+	 * download.
+	 */
+	ret = exceeds_release_timeout(fw_req);
+	if (ret)
+		goto put_fw;
+
+	fw = fw_req->fw;
+
+	if (offset >= fw->size || size > fw->size - offset) {
+		dev_err(fw_download->parent,
+			"bad fetch firmware request (offs = %u, size = %u)\n",
+			offset, size);
+		ret = -EINVAL;
+		goto put_fw;
+	}
+
+	if (!gb_operation_response_alloc(op, sizeof(*response) + size,
+					 GFP_KERNEL)) {
+		dev_err(fw_download->parent,
+			"error allocating fetch firmware response\n");
+		ret = -ENOMEM;
+		goto put_fw;
+	}
+
+	response = op->response->payload;
+	memcpy(response->data, fw->data + offset, size);
+
+	dev_dbg(fw_download->parent,
+		"responding with firmware (offs = %u, size = %u)\n", offset,
+		size);
+
+	/* Refresh timeout */
+	schedule_delayed_work(&fw_req->dwork, NEXT_REQ_TIMEOUT_J);
+
+put_fw:
+	put_fw_req(fw_req);
+
+	return ret;
+}
+
+static int fw_download_release_firmware(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct fw_download *fw_download = gb_connection_get_data(connection);
+	struct gb_fw_download_release_firmware_request *request;
+	struct fw_request *fw_req;
+	u8 firmware_id;
+
+	if (op->request->payload_size != sizeof(*request)) {
+		dev_err(fw_download->parent,
+			"Illegal size of release firmware request (%zu %zu)\n",
+			op->request->payload_size, sizeof(*request));
+		return -EINVAL;
+	}
+
+	request = op->request->payload;
+	firmware_id = request->firmware_id;
+
+	fw_req = get_fw_req(fw_download, firmware_id);
+	if (!fw_req) {
+		dev_err(fw_download->parent,
+			"firmware not available for id: %02u\n", firmware_id);
+		return -EINVAL;
+	}
+
+	cancel_delayed_work_sync(&fw_req->dwork);
+
+	free_firmware(fw_download, fw_req);
+	put_fw_req(fw_req);
+
+	dev_dbg(fw_download->parent, "release firmware\n");
+
+	return 0;
+}
+
+int gb_fw_download_request_handler(struct gb_operation *op)
+{
+	u8 type = op->type;
+
+	switch (type) {
+	case GB_FW_DOWNLOAD_TYPE_FIND_FIRMWARE:
+		return fw_download_find_firmware(op);
+	case GB_FW_DOWNLOAD_TYPE_FETCH_FIRMWARE:
+		return fw_download_fetch_firmware(op);
+	case GB_FW_DOWNLOAD_TYPE_RELEASE_FIRMWARE:
+		return fw_download_release_firmware(op);
+	default:
+		dev_err(&op->connection->bundle->dev,
+			"unsupported request: %u\n", type);
+		return -EINVAL;
+	}
+}
+
+int gb_fw_download_connection_init(struct gb_connection *connection)
+{
+	struct fw_download *fw_download;
+	int ret;
+
+	if (!connection)
+		return 0;
+
+	fw_download = kzalloc(sizeof(*fw_download), GFP_KERNEL);
+	if (!fw_download)
+		return -ENOMEM;
+
+	fw_download->parent = &connection->bundle->dev;
+	INIT_LIST_HEAD(&fw_download->fw_requests);
+	ida_init(&fw_download->id_map);
+	gb_connection_set_data(connection, fw_download);
+	fw_download->connection = connection;
+	mutex_init(&fw_download->mutex);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto err_destroy_id_map;
+
+	return 0;
+
+err_destroy_id_map:
+	ida_destroy(&fw_download->id_map);
+	kfree(fw_download);
+
+	return ret;
+}
+
+void gb_fw_download_connection_exit(struct gb_connection *connection)
+{
+	struct fw_download *fw_download;
+	struct fw_request *fw_req, *tmp;
+
+	if (!connection)
+		return;
+
+	fw_download = gb_connection_get_data(connection);
+	gb_connection_disable(fw_download->connection);
+
+	/*
+	 * Make sure we have a reference to the pending requests, before they
+	 * are freed from the timeout handler.
+	 */
+	mutex_lock(&fw_download->mutex);
+	list_for_each_entry(fw_req, &fw_download->fw_requests, node)
+		kref_get(&fw_req->kref);
+	mutex_unlock(&fw_download->mutex);
+
+	/* Release pending firmware packages */
+	list_for_each_entry_safe(fw_req, tmp, &fw_download->fw_requests, node) {
+		cancel_delayed_work_sync(&fw_req->dwork);
+		free_firmware(fw_download, fw_req);
+		put_fw_req(fw_req);
+	}
+
+	ida_destroy(&fw_download->id_map);
+	kfree(fw_download);
+}
diff --git a/drivers/staging/greybus/fw-management.c b/drivers/staging/greybus/fw-management.c
new file mode 100644
index 000000000..3342b8459
--- /dev/null
+++ b/drivers/staging/greybus/fw-management.c
@@ -0,0 +1,720 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Firmware Management Protocol Driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ */
+
+#include <linux/cdev.h>
+#include <linux/completion.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/greybus.h>
+
+#include "firmware.h"
+#include "greybus_firmware.h"
+
+#define FW_MGMT_TIMEOUT_MS		1000
+
+struct fw_mgmt {
+	struct device		*parent;
+	struct gb_connection	*connection;
+	struct kref		kref;
+	struct list_head	node;
+
+	/* Common id-map for interface and backend firmware requests */
+	struct ida		id_map;
+	struct mutex		mutex;
+	struct completion	completion;
+	struct cdev		cdev;
+	struct device		*class_device;
+	dev_t			dev_num;
+	unsigned int		timeout_jiffies;
+	bool			disabled; /* connection getting disabled */
+
+	/* Interface Firmware specific fields */
+	bool			mode_switch_started;
+	bool			intf_fw_loaded;
+	u8			intf_fw_request_id;
+	u8			intf_fw_status;
+	u16			intf_fw_major;
+	u16			intf_fw_minor;
+
+	/* Backend Firmware specific fields */
+	u8			backend_fw_request_id;
+	u8			backend_fw_status;
+};
+
+/*
+ * Number of minor devices this driver supports.
+ * There will be exactly one required per Interface.
+ */
+#define NUM_MINORS		U8_MAX
+
+static struct class *fw_mgmt_class;
+static dev_t fw_mgmt_dev_num;
+static DEFINE_IDA(fw_mgmt_minors_map);
+static LIST_HEAD(fw_mgmt_list);
+static DEFINE_MUTEX(list_mutex);
+
+static void fw_mgmt_kref_release(struct kref *kref)
+{
+	struct fw_mgmt *fw_mgmt = container_of(kref, struct fw_mgmt, kref);
+
+	ida_destroy(&fw_mgmt->id_map);
+	kfree(fw_mgmt);
+}
+
+/*
+ * All users of fw_mgmt take a reference (from within list_mutex lock), before
+ * they get a pointer to play with. And the structure will be freed only after
+ * the last user has put the reference to it.
+ */
+static void put_fw_mgmt(struct fw_mgmt *fw_mgmt)
+{
+	kref_put(&fw_mgmt->kref, fw_mgmt_kref_release);
+}
+
+/* Caller must call put_fw_mgmt() after using struct fw_mgmt */
+static struct fw_mgmt *get_fw_mgmt(struct cdev *cdev)
+{
+	struct fw_mgmt *fw_mgmt;
+
+	mutex_lock(&list_mutex);
+
+	list_for_each_entry(fw_mgmt, &fw_mgmt_list, node) {
+		if (&fw_mgmt->cdev == cdev) {
+			kref_get(&fw_mgmt->kref);
+			goto unlock;
+		}
+	}
+
+	fw_mgmt = NULL;
+
+unlock:
+	mutex_unlock(&list_mutex);
+
+	return fw_mgmt;
+}
+
+static int fw_mgmt_interface_fw_version_operation(struct fw_mgmt *fw_mgmt,
+						  struct fw_mgmt_ioc_get_intf_version *fw_info)
+{
+	struct gb_connection *connection = fw_mgmt->connection;
+	struct gb_fw_mgmt_interface_fw_version_response response;
+	int ret;
+
+	ret = gb_operation_sync(connection,
+				GB_FW_MGMT_TYPE_INTERFACE_FW_VERSION, NULL, 0,
+				&response, sizeof(response));
+	if (ret) {
+		dev_err(fw_mgmt->parent,
+			"failed to get interface firmware version (%d)\n", ret);
+		return ret;
+	}
+
+	fw_info->major = le16_to_cpu(response.major);
+	fw_info->minor = le16_to_cpu(response.minor);
+
+	strncpy(fw_info->firmware_tag, response.firmware_tag,
+		GB_FIRMWARE_TAG_MAX_SIZE);
+
+	/*
+	 * The firmware-tag should be NULL terminated, otherwise throw error but
+	 * don't fail.
+	 */
+	if (fw_info->firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE - 1] != '\0') {
+		dev_err(fw_mgmt->parent,
+			"fw-version: firmware-tag is not NULL terminated\n");
+		fw_info->firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE - 1] = '\0';
+	}
+
+	return 0;
+}
+
+static int fw_mgmt_load_and_validate_operation(struct fw_mgmt *fw_mgmt,
+					       u8 load_method, const char *tag)
+{
+	struct gb_fw_mgmt_load_and_validate_fw_request request;
+	int ret;
+
+	if (load_method != GB_FW_LOAD_METHOD_UNIPRO &&
+	    load_method != GB_FW_LOAD_METHOD_INTERNAL) {
+		dev_err(fw_mgmt->parent,
+			"invalid load-method (%d)\n", load_method);
+		return -EINVAL;
+	}
+
+	request.load_method = load_method;
+	strncpy(request.firmware_tag, tag, GB_FIRMWARE_TAG_MAX_SIZE);
+
+	/*
+	 * The firmware-tag should be NULL terminated, otherwise throw error and
+	 * fail.
+	 */
+	if (request.firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE - 1] != '\0') {
+		dev_err(fw_mgmt->parent, "load-and-validate: firmware-tag is not NULL terminated\n");
+		return -EINVAL;
+	}
+
+	/* Allocate ids from 1 to 255 (u8-max), 0 is an invalid id */
+	ret = ida_simple_get(&fw_mgmt->id_map, 1, 256, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(fw_mgmt->parent, "failed to allocate request id (%d)\n",
+			ret);
+		return ret;
+	}
+
+	fw_mgmt->intf_fw_request_id = ret;
+	fw_mgmt->intf_fw_loaded = false;
+	request.request_id = ret;
+
+	ret = gb_operation_sync(fw_mgmt->connection,
+				GB_FW_MGMT_TYPE_LOAD_AND_VALIDATE_FW, &request,
+				sizeof(request), NULL, 0);
+	if (ret) {
+		ida_simple_remove(&fw_mgmt->id_map,
+				  fw_mgmt->intf_fw_request_id);
+		fw_mgmt->intf_fw_request_id = 0;
+		dev_err(fw_mgmt->parent,
+			"load and validate firmware request failed (%d)\n",
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fw_mgmt_interface_fw_loaded_operation(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct fw_mgmt *fw_mgmt = gb_connection_get_data(connection);
+	struct gb_fw_mgmt_loaded_fw_request *request;
+
+	/* No pending load and validate request ? */
+	if (!fw_mgmt->intf_fw_request_id) {
+		dev_err(fw_mgmt->parent,
+			"unexpected firmware loaded request received\n");
+		return -ENODEV;
+	}
+
+	if (op->request->payload_size != sizeof(*request)) {
+		dev_err(fw_mgmt->parent, "illegal size of firmware loaded request (%zu != %zu)\n",
+			op->request->payload_size, sizeof(*request));
+		return -EINVAL;
+	}
+
+	request = op->request->payload;
+
+	/* Invalid request-id ? */
+	if (request->request_id != fw_mgmt->intf_fw_request_id) {
+		dev_err(fw_mgmt->parent, "invalid request id for firmware loaded request (%02u != %02u)\n",
+			fw_mgmt->intf_fw_request_id, request->request_id);
+		return -ENODEV;
+	}
+
+	ida_simple_remove(&fw_mgmt->id_map, fw_mgmt->intf_fw_request_id);
+	fw_mgmt->intf_fw_request_id = 0;
+	fw_mgmt->intf_fw_status = request->status;
+	fw_mgmt->intf_fw_major = le16_to_cpu(request->major);
+	fw_mgmt->intf_fw_minor = le16_to_cpu(request->minor);
+
+	if (fw_mgmt->intf_fw_status == GB_FW_LOAD_STATUS_FAILED)
+		dev_err(fw_mgmt->parent,
+			"failed to load interface firmware, status:%02x\n",
+			fw_mgmt->intf_fw_status);
+	else if (fw_mgmt->intf_fw_status == GB_FW_LOAD_STATUS_VALIDATION_FAILED)
+		dev_err(fw_mgmt->parent,
+			"failed to validate interface firmware, status:%02x\n",
+			fw_mgmt->intf_fw_status);
+	else
+		fw_mgmt->intf_fw_loaded = true;
+
+	complete(&fw_mgmt->completion);
+
+	return 0;
+}
+
+static int fw_mgmt_backend_fw_version_operation(struct fw_mgmt *fw_mgmt,
+						struct fw_mgmt_ioc_get_backend_version *fw_info)
+{
+	struct gb_connection *connection = fw_mgmt->connection;
+	struct gb_fw_mgmt_backend_fw_version_request request;
+	struct gb_fw_mgmt_backend_fw_version_response response;
+	int ret;
+
+	strncpy(request.firmware_tag, fw_info->firmware_tag,
+		GB_FIRMWARE_TAG_MAX_SIZE);
+
+	/*
+	 * The firmware-tag should be NULL terminated, otherwise throw error and
+	 * fail.
+	 */
+	if (request.firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE - 1] != '\0') {
+		dev_err(fw_mgmt->parent, "backend-version: firmware-tag is not NULL terminated\n");
+		return -EINVAL;
+	}
+
+	ret = gb_operation_sync(connection,
+				GB_FW_MGMT_TYPE_BACKEND_FW_VERSION, &request,
+				sizeof(request), &response, sizeof(response));
+	if (ret) {
+		dev_err(fw_mgmt->parent, "failed to get version of %s backend firmware (%d)\n",
+			fw_info->firmware_tag, ret);
+		return ret;
+	}
+
+	fw_info->status = response.status;
+
+	/* Reset version as that should be non-zero only for success case */
+	fw_info->major = 0;
+	fw_info->minor = 0;
+
+	switch (fw_info->status) {
+	case GB_FW_BACKEND_VERSION_STATUS_SUCCESS:
+		fw_info->major = le16_to_cpu(response.major);
+		fw_info->minor = le16_to_cpu(response.minor);
+		break;
+	case GB_FW_BACKEND_VERSION_STATUS_NOT_AVAILABLE:
+	case GB_FW_BACKEND_VERSION_STATUS_RETRY:
+		break;
+	case GB_FW_BACKEND_VERSION_STATUS_NOT_SUPPORTED:
+		dev_err(fw_mgmt->parent,
+			"Firmware with tag %s is not supported by Interface\n",
+			fw_info->firmware_tag);
+		break;
+	default:
+		dev_err(fw_mgmt->parent, "Invalid status received: %u\n",
+			fw_info->status);
+	}
+
+	return 0;
+}
+
+static int fw_mgmt_backend_fw_update_operation(struct fw_mgmt *fw_mgmt,
+					       char *tag)
+{
+	struct gb_fw_mgmt_backend_fw_update_request request;
+	int ret;
+
+	strncpy(request.firmware_tag, tag, GB_FIRMWARE_TAG_MAX_SIZE);
+
+	/*
+	 * The firmware-tag should be NULL terminated, otherwise throw error and
+	 * fail.
+	 */
+	if (request.firmware_tag[GB_FIRMWARE_TAG_MAX_SIZE - 1] != '\0') {
+		dev_err(fw_mgmt->parent, "backend-update: firmware-tag is not NULL terminated\n");
+		return -EINVAL;
+	}
+
+	/* Allocate ids from 1 to 255 (u8-max), 0 is an invalid id */
+	ret = ida_simple_get(&fw_mgmt->id_map, 1, 256, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(fw_mgmt->parent, "failed to allocate request id (%d)\n",
+			ret);
+		return ret;
+	}
+
+	fw_mgmt->backend_fw_request_id = ret;
+	request.request_id = ret;
+
+	ret = gb_operation_sync(fw_mgmt->connection,
+				GB_FW_MGMT_TYPE_BACKEND_FW_UPDATE, &request,
+				sizeof(request), NULL, 0);
+	if (ret) {
+		ida_simple_remove(&fw_mgmt->id_map,
+				  fw_mgmt->backend_fw_request_id);
+		fw_mgmt->backend_fw_request_id = 0;
+		dev_err(fw_mgmt->parent,
+			"backend %s firmware update request failed (%d)\n", tag,
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fw_mgmt_backend_fw_updated_operation(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct fw_mgmt *fw_mgmt = gb_connection_get_data(connection);
+	struct gb_fw_mgmt_backend_fw_updated_request *request;
+
+	/* No pending load and validate request ? */
+	if (!fw_mgmt->backend_fw_request_id) {
+		dev_err(fw_mgmt->parent, "unexpected backend firmware updated request received\n");
+		return -ENODEV;
+	}
+
+	if (op->request->payload_size != sizeof(*request)) {
+		dev_err(fw_mgmt->parent, "illegal size of backend firmware updated request (%zu != %zu)\n",
+			op->request->payload_size, sizeof(*request));
+		return -EINVAL;
+	}
+
+	request = op->request->payload;
+
+	/* Invalid request-id ? */
+	if (request->request_id != fw_mgmt->backend_fw_request_id) {
+		dev_err(fw_mgmt->parent, "invalid request id for backend firmware updated request (%02u != %02u)\n",
+			fw_mgmt->backend_fw_request_id, request->request_id);
+		return -ENODEV;
+	}
+
+	ida_simple_remove(&fw_mgmt->id_map, fw_mgmt->backend_fw_request_id);
+	fw_mgmt->backend_fw_request_id = 0;
+	fw_mgmt->backend_fw_status = request->status;
+
+	if ((fw_mgmt->backend_fw_status != GB_FW_BACKEND_FW_STATUS_SUCCESS) &&
+	    (fw_mgmt->backend_fw_status != GB_FW_BACKEND_FW_STATUS_RETRY))
+		dev_err(fw_mgmt->parent,
+			"failed to load backend firmware: %02x\n",
+			fw_mgmt->backend_fw_status);
+
+	complete(&fw_mgmt->completion);
+
+	return 0;
+}
+
+/* Char device fops */
+
+static int fw_mgmt_open(struct inode *inode, struct file *file)
+{
+	struct fw_mgmt *fw_mgmt = get_fw_mgmt(inode->i_cdev);
+
+	/* fw_mgmt structure can't get freed until file descriptor is closed */
+	if (fw_mgmt) {
+		file->private_data = fw_mgmt;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static int fw_mgmt_release(struct inode *inode, struct file *file)
+{
+	struct fw_mgmt *fw_mgmt = file->private_data;
+
+	put_fw_mgmt(fw_mgmt);
+	return 0;
+}
+
+static int fw_mgmt_ioctl(struct fw_mgmt *fw_mgmt, unsigned int cmd,
+			 void __user *buf)
+{
+	struct fw_mgmt_ioc_get_intf_version intf_fw_info;
+	struct fw_mgmt_ioc_get_backend_version backend_fw_info;
+	struct fw_mgmt_ioc_intf_load_and_validate intf_load;
+	struct fw_mgmt_ioc_backend_fw_update backend_update;
+	unsigned int timeout;
+	int ret;
+
+	/* Reject any operations after mode-switch has started */
+	if (fw_mgmt->mode_switch_started)
+		return -EBUSY;
+
+	switch (cmd) {
+	case FW_MGMT_IOC_GET_INTF_FW:
+		ret = fw_mgmt_interface_fw_version_operation(fw_mgmt,
+							     &intf_fw_info);
+		if (ret)
+			return ret;
+
+		if (copy_to_user(buf, &intf_fw_info, sizeof(intf_fw_info)))
+			return -EFAULT;
+
+		return 0;
+	case FW_MGMT_IOC_GET_BACKEND_FW:
+		if (copy_from_user(&backend_fw_info, buf,
+				   sizeof(backend_fw_info)))
+			return -EFAULT;
+
+		ret = fw_mgmt_backend_fw_version_operation(fw_mgmt,
+							   &backend_fw_info);
+		if (ret)
+			return ret;
+
+		if (copy_to_user(buf, &backend_fw_info,
+				 sizeof(backend_fw_info)))
+			return -EFAULT;
+
+		return 0;
+	case FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE:
+		if (copy_from_user(&intf_load, buf, sizeof(intf_load)))
+			return -EFAULT;
+
+		ret = fw_mgmt_load_and_validate_operation(fw_mgmt,
+				intf_load.load_method, intf_load.firmware_tag);
+		if (ret)
+			return ret;
+
+		if (!wait_for_completion_timeout(&fw_mgmt->completion,
+						 fw_mgmt->timeout_jiffies)) {
+			dev_err(fw_mgmt->parent, "timed out waiting for firmware load and validation to finish\n");
+			return -ETIMEDOUT;
+		}
+
+		intf_load.status = fw_mgmt->intf_fw_status;
+		intf_load.major = fw_mgmt->intf_fw_major;
+		intf_load.minor = fw_mgmt->intf_fw_minor;
+
+		if (copy_to_user(buf, &intf_load, sizeof(intf_load)))
+			return -EFAULT;
+
+		return 0;
+	case FW_MGMT_IOC_INTF_BACKEND_FW_UPDATE:
+		if (copy_from_user(&backend_update, buf,
+				   sizeof(backend_update)))
+			return -EFAULT;
+
+		ret = fw_mgmt_backend_fw_update_operation(fw_mgmt,
+							  backend_update.firmware_tag);
+		if (ret)
+			return ret;
+
+		if (!wait_for_completion_timeout(&fw_mgmt->completion,
+						 fw_mgmt->timeout_jiffies)) {
+			dev_err(fw_mgmt->parent, "timed out waiting for backend firmware update to finish\n");
+			return -ETIMEDOUT;
+		}
+
+		backend_update.status = fw_mgmt->backend_fw_status;
+
+		if (copy_to_user(buf, &backend_update, sizeof(backend_update)))
+			return -EFAULT;
+
+		return 0;
+	case FW_MGMT_IOC_SET_TIMEOUT_MS:
+		if (get_user(timeout, (unsigned int __user *)buf))
+			return -EFAULT;
+
+		if (!timeout) {
+			dev_err(fw_mgmt->parent, "timeout can't be zero\n");
+			return -EINVAL;
+		}
+
+		fw_mgmt->timeout_jiffies = msecs_to_jiffies(timeout);
+
+		return 0;
+	case FW_MGMT_IOC_MODE_SWITCH:
+		if (!fw_mgmt->intf_fw_loaded) {
+			dev_err(fw_mgmt->parent,
+				"Firmware not loaded for mode-switch\n");
+			return -EPERM;
+		}
+
+		/*
+		 * Disallow new ioctls as the fw-core bundle driver is going to
+		 * get disconnected soon and the character device will get
+		 * removed.
+		 */
+		fw_mgmt->mode_switch_started = true;
+
+		ret = gb_interface_request_mode_switch(fw_mgmt->connection->intf);
+		if (ret) {
+			dev_err(fw_mgmt->parent, "Mode-switch failed: %d\n",
+				ret);
+			fw_mgmt->mode_switch_started = false;
+			return ret;
+		}
+
+		return 0;
+	default:
+		return -ENOTTY;
+	}
+}
+
+static long fw_mgmt_ioctl_unlocked(struct file *file, unsigned int cmd,
+				   unsigned long arg)
+{
+	struct fw_mgmt *fw_mgmt = file->private_data;
+	struct gb_bundle *bundle = fw_mgmt->connection->bundle;
+	int ret = -ENODEV;
+
+	/*
+	 * Serialize ioctls.
+	 *
+	 * We don't want the user to do few operations in parallel. For example,
+	 * updating Interface firmware in parallel for the same Interface. There
+	 * is no need to do things in parallel for speed and we can avoid having
+	 * complicated code for now.
+	 *
+	 * This is also used to protect ->disabled, which is used to check if
+	 * the connection is getting disconnected, so that we don't start any
+	 * new operations.
+	 */
+	mutex_lock(&fw_mgmt->mutex);
+	if (!fw_mgmt->disabled) {
+		ret = gb_pm_runtime_get_sync(bundle);
+		if (!ret) {
+			ret = fw_mgmt_ioctl(fw_mgmt, cmd, (void __user *)arg);
+			gb_pm_runtime_put_autosuspend(bundle);
+		}
+	}
+	mutex_unlock(&fw_mgmt->mutex);
+
+	return ret;
+}
+
+static const struct file_operations fw_mgmt_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fw_mgmt_open,
+	.release	= fw_mgmt_release,
+	.unlocked_ioctl	= fw_mgmt_ioctl_unlocked,
+};
+
+int gb_fw_mgmt_request_handler(struct gb_operation *op)
+{
+	u8 type = op->type;
+
+	switch (type) {
+	case GB_FW_MGMT_TYPE_LOADED_FW:
+		return fw_mgmt_interface_fw_loaded_operation(op);
+	case GB_FW_MGMT_TYPE_BACKEND_FW_UPDATED:
+		return fw_mgmt_backend_fw_updated_operation(op);
+	default:
+		dev_err(&op->connection->bundle->dev,
+			"unsupported request: %u\n", type);
+		return -EINVAL;
+	}
+}
+
+int gb_fw_mgmt_connection_init(struct gb_connection *connection)
+{
+	struct fw_mgmt *fw_mgmt;
+	int ret, minor;
+
+	if (!connection)
+		return 0;
+
+	fw_mgmt = kzalloc(sizeof(*fw_mgmt), GFP_KERNEL);
+	if (!fw_mgmt)
+		return -ENOMEM;
+
+	fw_mgmt->parent = &connection->bundle->dev;
+	fw_mgmt->timeout_jiffies = msecs_to_jiffies(FW_MGMT_TIMEOUT_MS);
+	fw_mgmt->connection = connection;
+
+	gb_connection_set_data(connection, fw_mgmt);
+	init_completion(&fw_mgmt->completion);
+	ida_init(&fw_mgmt->id_map);
+	mutex_init(&fw_mgmt->mutex);
+	kref_init(&fw_mgmt->kref);
+
+	mutex_lock(&list_mutex);
+	list_add(&fw_mgmt->node, &fw_mgmt_list);
+	mutex_unlock(&list_mutex);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto err_list_del;
+
+	minor = ida_simple_get(&fw_mgmt_minors_map, 0, NUM_MINORS, GFP_KERNEL);
+	if (minor < 0) {
+		ret = minor;
+		goto err_connection_disable;
+	}
+
+	/* Add a char device to allow userspace to interact with fw-mgmt */
+	fw_mgmt->dev_num = MKDEV(MAJOR(fw_mgmt_dev_num), minor);
+	cdev_init(&fw_mgmt->cdev, &fw_mgmt_fops);
+
+	ret = cdev_add(&fw_mgmt->cdev, fw_mgmt->dev_num, 1);
+	if (ret)
+		goto err_remove_ida;
+
+	/* Add a soft link to the previously added char-dev within the bundle */
+	fw_mgmt->class_device = device_create(fw_mgmt_class, fw_mgmt->parent,
+					      fw_mgmt->dev_num, NULL,
+					      "gb-fw-mgmt-%d", minor);
+	if (IS_ERR(fw_mgmt->class_device)) {
+		ret = PTR_ERR(fw_mgmt->class_device);
+		goto err_del_cdev;
+	}
+
+	return 0;
+
+err_del_cdev:
+	cdev_del(&fw_mgmt->cdev);
+err_remove_ida:
+	ida_simple_remove(&fw_mgmt_minors_map, minor);
+err_connection_disable:
+	gb_connection_disable(connection);
+err_list_del:
+	mutex_lock(&list_mutex);
+	list_del(&fw_mgmt->node);
+	mutex_unlock(&list_mutex);
+
+	put_fw_mgmt(fw_mgmt);
+
+	return ret;
+}
+
+void gb_fw_mgmt_connection_exit(struct gb_connection *connection)
+{
+	struct fw_mgmt *fw_mgmt;
+
+	if (!connection)
+		return;
+
+	fw_mgmt = gb_connection_get_data(connection);
+
+	device_destroy(fw_mgmt_class, fw_mgmt->dev_num);
+	cdev_del(&fw_mgmt->cdev);
+	ida_simple_remove(&fw_mgmt_minors_map, MINOR(fw_mgmt->dev_num));
+
+	/*
+	 * Disallow any new ioctl operations on the char device and wait for
+	 * existing ones to finish.
+	 */
+	mutex_lock(&fw_mgmt->mutex);
+	fw_mgmt->disabled = true;
+	mutex_unlock(&fw_mgmt->mutex);
+
+	/* All pending greybus operations should have finished by now */
+	gb_connection_disable(fw_mgmt->connection);
+
+	/* Disallow new users to get access to the fw_mgmt structure */
+	mutex_lock(&list_mutex);
+	list_del(&fw_mgmt->node);
+	mutex_unlock(&list_mutex);
+
+	/*
+	 * All current users of fw_mgmt would have taken a reference to it by
+	 * now, we can drop our reference and wait the last user will get
+	 * fw_mgmt freed.
+	 */
+	put_fw_mgmt(fw_mgmt);
+}
+
+int fw_mgmt_init(void)
+{
+	int ret;
+
+	fw_mgmt_class = class_create(THIS_MODULE, "gb_fw_mgmt");
+	if (IS_ERR(fw_mgmt_class))
+		return PTR_ERR(fw_mgmt_class);
+
+	ret = alloc_chrdev_region(&fw_mgmt_dev_num, 0, NUM_MINORS,
+				  "gb_fw_mgmt");
+	if (ret)
+		goto err_remove_class;
+
+	return 0;
+
+err_remove_class:
+	class_destroy(fw_mgmt_class);
+	return ret;
+}
+
+void fw_mgmt_exit(void)
+{
+	unregister_chrdev_region(fw_mgmt_dev_num, NUM_MINORS);
+	class_destroy(fw_mgmt_class);
+	ida_destroy(&fw_mgmt_minors_map);
+}
diff --git a/drivers/staging/greybus/gb-camera.h b/drivers/staging/greybus/gb-camera.h
new file mode 100644
index 000000000..5fc469101
--- /dev/null
+++ b/drivers/staging/greybus/gb-camera.h
@@ -0,0 +1,126 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus Camera protocol driver.
+ *
+ * Copyright 2015 Google Inc.
+ */
+#ifndef __GB_CAMERA_H
+#define __GB_CAMERA_H
+
+#include <linux/v4l2-mediabus.h>
+
+/* Input flags need to be set from the caller */
+#define GB_CAMERA_IN_FLAG_TEST		(1 << 0)
+/* Output flags returned */
+#define GB_CAMERA_OUT_FLAG_ADJUSTED	(1 << 0)
+
+/**
+ * struct gb_camera_stream - Represents greybus camera stream.
+ * @width: Stream width in pixels.
+ * @height: Stream height in pixels.
+ * @pixel_code: Media bus pixel code.
+ * @vc: MIPI CSI virtual channel.
+ * @dt: MIPI CSI data types. Most formats use a single data type, in which case
+ *      the second element will be ignored.
+ * @max_size: Maximum size of a frame in bytes. The camera module guarantees
+ *            that all data between the Frame Start and Frame End packet for
+ *            the associated virtual channel and data type(s) will not exceed
+ *            this size.
+ */
+struct gb_camera_stream {
+	unsigned int width;
+	unsigned int height;
+	enum v4l2_mbus_pixelcode pixel_code;
+	unsigned int vc;
+	unsigned int dt[2];
+	unsigned int max_size;
+};
+
+/**
+ * struct gb_camera_csi_params - CSI configuration parameters
+ * @num_lanes: number of CSI data lanes
+ * @clk_freq: CSI clock frequency in Hz
+ */
+struct gb_camera_csi_params {
+	unsigned int num_lanes;
+	unsigned int clk_freq;
+};
+
+/**
+ * struct gb_camera_ops - Greybus camera operations, used by the Greybus camera
+ *                        driver to expose operations to the host camera driver.
+ * @capabilities: Retrieve camera capabilities and store them in the buffer
+ *                'buf' capabilities. The buffer maximum size is specified by
+ *                the caller in the 'size' parameter, and the effective
+ *                capabilities size is returned from the function. If the buffer
+ *                size is too small to hold the capabilities an error is
+ *                returned and the buffer is left untouched.
+ *
+ * @configure_streams: Negotiate configuration and prepare the module for video
+ *                     capture. The caller specifies the number of streams it
+ *                     requests in the 'nstreams' argument and the associated
+ *                     streams configurations in the 'streams' argument. The
+ *                     GB_CAMERA_IN_FLAG_TEST 'flag' can be set to test a
+ *                     configuration without applying it, otherwise the
+ *                     configuration is applied by the module. The module can
+ *                     decide to modify the requested configuration, including
+ *                     using a different number of streams. In that case the
+ *                     modified configuration won't be applied, the
+ *                     GB_CAMERA_OUT_FLAG_ADJUSTED 'flag' will be set upon
+ *                     return, and the modified configuration and number of
+ *                     streams stored in 'streams' and 'array'. The module
+ *                     returns its CSI-2 bus parameters in the 'csi_params'
+ *                     structure in all cases.
+ *
+ * @capture: Submit a capture request. The supplied 'request_id' must be unique
+ *           and higher than the IDs of all the previously submitted requests.
+ *           The 'streams' argument specifies which streams are affected by the
+ *           request in the form of a bitmask, with bits corresponding to the
+ *           configured streams indexes. If the request contains settings, the
+ *           'settings' argument points to the settings buffer and its size is
+ *           specified by the 'settings_size' argument. Otherwise the 'settings'
+ *           argument should be set to NULL and 'settings_size' to 0.
+ *
+ * @flush: Flush the capture requests queue. Return the ID of the last request
+ *         that will processed by the device before it stops transmitting video
+ *         frames. All queued capture requests with IDs higher than the returned
+ *         ID will be dropped without being processed.
+ */
+struct gb_camera_ops {
+	ssize_t (*capabilities)(void *priv, char *buf, size_t len);
+	int (*configure_streams)(void *priv, unsigned int *nstreams,
+			unsigned int *flags, struct gb_camera_stream *streams,
+			struct gb_camera_csi_params *csi_params);
+	int (*capture)(void *priv, u32 request_id,
+			unsigned int streams, unsigned int num_frames,
+			size_t settings_size, const void *settings);
+	int (*flush)(void *priv, u32 *request_id);
+};
+
+/**
+ * struct gb_camera_module - Represents greybus camera module.
+ * @priv: Module private data, passed to all camera operations.
+ * @ops: Greybus camera operation callbacks.
+ * @interface_id: Interface id of the module.
+ * @refcount: Reference counting object.
+ * @release: Module release function.
+ * @list: List entry in the camera modules list.
+ */
+struct gb_camera_module {
+	void *priv;
+	const struct gb_camera_ops *ops;
+
+	unsigned int interface_id;
+	struct kref refcount;
+	void (*release)(struct kref *kref);
+	struct list_head list; /* Global list */
+};
+
+#define gb_camera_call(f, op, args...)      \
+	(!(f) ? -ENODEV : (((f)->ops->op) ?  \
+	(f)->ops->op((f)->priv, ##args) : -ENOIOCTLCMD))
+
+int gb_camera_register(struct gb_camera_module *module);
+int gb_camera_unregister(struct gb_camera_module *module);
+
+#endif /* __GB_CAMERA_H */
diff --git a/drivers/staging/greybus/gbphy.c b/drivers/staging/greybus/gbphy.c
new file mode 100644
index 000000000..5a5c17a45
--- /dev/null
+++ b/drivers/staging/greybus/gbphy.c
@@ -0,0 +1,357 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Bridged-Phy Bus driver
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+#define GB_GBPHY_AUTOSUSPEND_MS	3000
+
+struct gbphy_host {
+	struct gb_bundle *bundle;
+	struct list_head devices;
+};
+
+static DEFINE_IDA(gbphy_id);
+
+static ssize_t protocol_id_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+
+	return sprintf(buf, "0x%02x\n", gbphy_dev->cport_desc->protocol_id);
+}
+static DEVICE_ATTR_RO(protocol_id);
+
+static struct attribute *gbphy_dev_attrs[] = {
+	&dev_attr_protocol_id.attr,
+	NULL,
+};
+
+ATTRIBUTE_GROUPS(gbphy_dev);
+
+static void gbphy_dev_release(struct device *dev)
+{
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+
+	ida_simple_remove(&gbphy_id, gbphy_dev->id);
+	kfree(gbphy_dev);
+}
+
+#ifdef CONFIG_PM
+static int gb_gbphy_idle(struct device *dev)
+{
+	pm_runtime_mark_last_busy(dev);
+	pm_request_autosuspend(dev);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops gb_gbphy_pm_ops = {
+	SET_RUNTIME_PM_OPS(pm_generic_runtime_suspend,
+			   pm_generic_runtime_resume,
+			   gb_gbphy_idle)
+};
+
+static const struct device_type greybus_gbphy_dev_type = {
+	.name	 =	"gbphy_device",
+	.release =	gbphy_dev_release,
+	.pm	=	&gb_gbphy_pm_ops,
+};
+
+static int gbphy_dev_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+	struct greybus_descriptor_cport *cport_desc = gbphy_dev->cport_desc;
+	struct gb_bundle *bundle = gbphy_dev->bundle;
+	struct gb_interface *intf = bundle->intf;
+	struct gb_module *module = intf->module;
+	struct gb_host_device *hd = intf->hd;
+
+	if (add_uevent_var(env, "BUS=%u", hd->bus_id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "MODULE=%u", module->module_id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "INTERFACE=%u", intf->interface_id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "GREYBUS_ID=%08x/%08x",
+			   intf->vendor_id, intf->product_id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "BUNDLE=%u", gbphy_dev->bundle->id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "BUNDLE_CLASS=%02x", bundle->class))
+		return -ENOMEM;
+	if (add_uevent_var(env, "GBPHY=%u", gbphy_dev->id))
+		return -ENOMEM;
+	if (add_uevent_var(env, "PROTOCOL_ID=%02x", cport_desc->protocol_id))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static const struct gbphy_device_id *
+gbphy_dev_match_id(struct gbphy_device *gbphy_dev,
+		   struct gbphy_driver *gbphy_drv)
+{
+	const struct gbphy_device_id *id = gbphy_drv->id_table;
+
+	if (!id)
+		return NULL;
+
+	for (; id->protocol_id; id++)
+		if (id->protocol_id == gbphy_dev->cport_desc->protocol_id)
+			return id;
+
+	return NULL;
+}
+
+static int gbphy_dev_match(struct device *dev, struct device_driver *drv)
+{
+	struct gbphy_driver *gbphy_drv = to_gbphy_driver(drv);
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+	const struct gbphy_device_id *id;
+
+	id = gbphy_dev_match_id(gbphy_dev, gbphy_drv);
+	if (id)
+		return 1;
+
+	return 0;
+}
+
+static int gbphy_dev_probe(struct device *dev)
+{
+	struct gbphy_driver *gbphy_drv = to_gbphy_driver(dev->driver);
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+	const struct gbphy_device_id *id;
+	int ret;
+
+	id = gbphy_dev_match_id(gbphy_dev, gbphy_drv);
+	if (!id)
+		return -ENODEV;
+
+	/* for old kernels we need get_sync to resume parent devices */
+	ret = gb_pm_runtime_get_sync(gbphy_dev->bundle);
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_set_autosuspend_delay(dev, GB_GBPHY_AUTOSUSPEND_MS);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
+	/*
+	 * Drivers should call put on the gbphy dev before returning
+	 * from probe if they support runtime pm.
+	 */
+	ret = gbphy_drv->probe(gbphy_dev, id);
+	if (ret) {
+		pm_runtime_disable(dev);
+		pm_runtime_set_suspended(dev);
+		pm_runtime_put_noidle(dev);
+		pm_runtime_dont_use_autosuspend(dev);
+	}
+
+	gb_pm_runtime_put_autosuspend(gbphy_dev->bundle);
+
+	return ret;
+}
+
+static void gbphy_dev_remove(struct device *dev)
+{
+	struct gbphy_driver *gbphy_drv = to_gbphy_driver(dev->driver);
+	struct gbphy_device *gbphy_dev = to_gbphy_dev(dev);
+
+	gbphy_drv->remove(gbphy_dev);
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	pm_runtime_put_noidle(dev);
+	pm_runtime_dont_use_autosuspend(dev);
+}
+
+static struct bus_type gbphy_bus_type = {
+	.name =		"gbphy",
+	.match =	gbphy_dev_match,
+	.probe =	gbphy_dev_probe,
+	.remove =	gbphy_dev_remove,
+	.uevent =	gbphy_dev_uevent,
+};
+
+int gb_gbphy_register_driver(struct gbphy_driver *driver,
+			     struct module *owner, const char *mod_name)
+{
+	int retval;
+
+	if (greybus_disabled())
+		return -ENODEV;
+
+	driver->driver.bus = &gbphy_bus_type;
+	driver->driver.name = driver->name;
+	driver->driver.owner = owner;
+	driver->driver.mod_name = mod_name;
+
+	retval = driver_register(&driver->driver);
+	if (retval)
+		return retval;
+
+	pr_info("registered new driver %s\n", driver->name);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gb_gbphy_register_driver);
+
+void gb_gbphy_deregister_driver(struct gbphy_driver *driver)
+{
+	driver_unregister(&driver->driver);
+}
+EXPORT_SYMBOL_GPL(gb_gbphy_deregister_driver);
+
+static struct gbphy_device *gb_gbphy_create_dev(struct gb_bundle *bundle,
+						struct greybus_descriptor_cport *cport_desc)
+{
+	struct gbphy_device *gbphy_dev;
+	int retval;
+	int id;
+
+	id = ida_simple_get(&gbphy_id, 1, 0, GFP_KERNEL);
+	if (id < 0)
+		return ERR_PTR(id);
+
+	gbphy_dev = kzalloc(sizeof(*gbphy_dev), GFP_KERNEL);
+	if (!gbphy_dev) {
+		ida_simple_remove(&gbphy_id, id);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	gbphy_dev->id = id;
+	gbphy_dev->bundle = bundle;
+	gbphy_dev->cport_desc = cport_desc;
+	gbphy_dev->dev.parent = &bundle->dev;
+	gbphy_dev->dev.bus = &gbphy_bus_type;
+	gbphy_dev->dev.type = &greybus_gbphy_dev_type;
+	gbphy_dev->dev.groups = gbphy_dev_groups;
+	gbphy_dev->dev.dma_mask = bundle->dev.dma_mask;
+	dev_set_name(&gbphy_dev->dev, "gbphy%d", id);
+
+	retval = device_register(&gbphy_dev->dev);
+	if (retval) {
+		put_device(&gbphy_dev->dev);
+		return ERR_PTR(retval);
+	}
+
+	return gbphy_dev;
+}
+
+static void gb_gbphy_disconnect(struct gb_bundle *bundle)
+{
+	struct gbphy_host *gbphy_host = greybus_get_drvdata(bundle);
+	struct gbphy_device *gbphy_dev, *temp;
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		gb_pm_runtime_get_noresume(bundle);
+
+	list_for_each_entry_safe(gbphy_dev, temp, &gbphy_host->devices, list) {
+		list_del(&gbphy_dev->list);
+		device_unregister(&gbphy_dev->dev);
+	}
+
+	kfree(gbphy_host);
+}
+
+static int gb_gbphy_probe(struct gb_bundle *bundle,
+			  const struct greybus_bundle_id *id)
+{
+	struct gbphy_host *gbphy_host;
+	struct gbphy_device *gbphy_dev;
+	int i;
+
+	if (bundle->num_cports == 0)
+		return -ENODEV;
+
+	gbphy_host = kzalloc(sizeof(*gbphy_host), GFP_KERNEL);
+	if (!gbphy_host)
+		return -ENOMEM;
+
+	gbphy_host->bundle = bundle;
+	INIT_LIST_HEAD(&gbphy_host->devices);
+	greybus_set_drvdata(bundle, gbphy_host);
+
+	/*
+	 * Create a bunch of children devices, one per cport, and bind the
+	 * bridged phy drivers to them.
+	 */
+	for (i = 0; i < bundle->num_cports; ++i) {
+		gbphy_dev = gb_gbphy_create_dev(bundle, &bundle->cport_desc[i]);
+		if (IS_ERR(gbphy_dev)) {
+			gb_gbphy_disconnect(bundle);
+			return PTR_ERR(gbphy_dev);
+		}
+		list_add(&gbphy_dev->list, &gbphy_host->devices);
+	}
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+}
+
+static const struct greybus_bundle_id gb_gbphy_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_BRIDGED_PHY) },
+	{ },
+};
+MODULE_DEVICE_TABLE(greybus, gb_gbphy_id_table);
+
+static struct greybus_driver gb_gbphy_driver = {
+	.name		= "gbphy",
+	.probe		= gb_gbphy_probe,
+	.disconnect	= gb_gbphy_disconnect,
+	.id_table	= gb_gbphy_id_table,
+};
+
+static int __init gbphy_init(void)
+{
+	int retval;
+
+	retval = bus_register(&gbphy_bus_type);
+	if (retval) {
+		pr_err("gbphy bus register failed (%d)\n", retval);
+		return retval;
+	}
+
+	retval = greybus_register(&gb_gbphy_driver);
+	if (retval) {
+		pr_err("error registering greybus driver\n");
+		goto error_gbphy;
+	}
+
+	return 0;
+
+error_gbphy:
+	bus_unregister(&gbphy_bus_type);
+	ida_destroy(&gbphy_id);
+	return retval;
+}
+module_init(gbphy_init);
+
+static void __exit gbphy_exit(void)
+{
+	greybus_deregister(&gb_gbphy_driver);
+	bus_unregister(&gbphy_bus_type);
+	ida_destroy(&gbphy_id);
+}
+module_exit(gbphy_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/gbphy.h b/drivers/staging/greybus/gbphy.h
new file mode 100644
index 000000000..d4a225b76
--- /dev/null
+++ b/drivers/staging/greybus/gbphy.h
@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus Bridged-Phy Bus driver
+ *
+ * Copyright 2016 Google Inc.
+ */
+
+#ifndef __GBPHY_H
+#define __GBPHY_H
+
+struct gbphy_device {
+	u32 id;
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_bundle *bundle;
+	struct list_head list;
+	struct device dev;
+};
+#define to_gbphy_dev(d) container_of(d, struct gbphy_device, dev)
+
+static inline void *gb_gbphy_get_data(struct gbphy_device *gdev)
+{
+	return dev_get_drvdata(&gdev->dev);
+}
+
+static inline void gb_gbphy_set_data(struct gbphy_device *gdev, void *data)
+{
+	dev_set_drvdata(&gdev->dev, data);
+}
+
+struct gbphy_device_id {
+	__u8 protocol_id;
+};
+
+#define GBPHY_PROTOCOL(p)		\
+	.protocol_id	= (p),
+
+struct gbphy_driver {
+	const char *name;
+	int (*probe)(struct gbphy_device *device,
+		     const struct gbphy_device_id *id);
+	void (*remove)(struct gbphy_device *device);
+	const struct gbphy_device_id *id_table;
+
+	struct device_driver driver;
+};
+#define to_gbphy_driver(d) container_of(d, struct gbphy_driver, driver)
+
+int gb_gbphy_register_driver(struct gbphy_driver *driver,
+			     struct module *owner, const char *mod_name);
+void gb_gbphy_deregister_driver(struct gbphy_driver *driver);
+
+#define gb_gbphy_register(driver) \
+	gb_gbphy_register_driver(driver, THIS_MODULE, KBUILD_MODNAME)
+#define gb_gbphy_deregister(driver) \
+	gb_gbphy_deregister_driver(driver)
+
+/**
+ * module_gbphy_driver() - Helper macro for registering a gbphy driver
+ * @__gbphy_driver: gbphy_driver structure
+ *
+ * Helper macro for gbphy drivers to set up proper module init / exit
+ * functions.  Replaces module_init() and module_exit() and keeps people from
+ * printing pointless things to the kernel log when their driver is loaded.
+ */
+#define module_gbphy_driver(__gbphy_driver)	\
+	module_driver(__gbphy_driver, gb_gbphy_register, gb_gbphy_deregister)
+
+#ifdef CONFIG_PM
+static inline int gbphy_runtime_get_sync(struct gbphy_device *gbphy_dev)
+{
+	struct device *dev = &gbphy_dev->dev;
+	int ret;
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "pm_runtime_get_sync failed: %d\n", ret);
+		pm_runtime_put_noidle(dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline void gbphy_runtime_put_autosuspend(struct gbphy_device *gbphy_dev)
+{
+	struct device *dev = &gbphy_dev->dev;
+
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+}
+
+static inline void gbphy_runtime_get_noresume(struct gbphy_device *gbphy_dev)
+{
+	pm_runtime_get_noresume(&gbphy_dev->dev);
+}
+
+static inline void gbphy_runtime_put_noidle(struct gbphy_device *gbphy_dev)
+{
+	pm_runtime_put_noidle(&gbphy_dev->dev);
+}
+#else
+static inline int gbphy_runtime_get_sync(struct gbphy_device *gbphy_dev) { return 0; }
+static inline void gbphy_runtime_put_autosuspend(struct gbphy_device *gbphy_dev) {}
+static inline void gbphy_runtime_get_noresume(struct gbphy_device *gbphy_dev) {}
+static inline void gbphy_runtime_put_noidle(struct gbphy_device *gbphy_dev) {}
+#endif
+
+#endif /* __GBPHY_H */
+
diff --git a/drivers/staging/greybus/gpio.c b/drivers/staging/greybus/gpio.c
new file mode 100644
index 000000000..8a7cf1d0e
--- /dev/null
+++ b/drivers/staging/greybus/gpio.c
@@ -0,0 +1,627 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * GPIO Greybus driver.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/gpio/driver.h>
+#include <linux/mutex.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+struct gb_gpio_line {
+	/* The following has to be an array of line_max entries */
+	/* --> make them just a flags field */
+	u8			active:    1,
+				direction: 1,	/* 0 = output, 1 = input */
+				value:     1;	/* 0 = low, 1 = high */
+	u16			debounce_usec;
+
+	u8			irq_type;
+	bool			irq_type_pending;
+	bool			masked;
+	bool			masked_pending;
+};
+
+struct gb_gpio_controller {
+	struct gbphy_device	*gbphy_dev;
+	struct gb_connection	*connection;
+	u8			line_max;	/* max line number */
+	struct gb_gpio_line	*lines;
+
+	struct gpio_chip	chip;
+	struct irq_chip		irqc;
+	struct mutex		irq_lock;
+};
+#define gpio_chip_to_gb_gpio_controller(chip) \
+	container_of(chip, struct gb_gpio_controller, chip)
+#define irq_data_to_gpio_chip(d) (d->domain->host_data)
+
+static int gb_gpio_line_count_operation(struct gb_gpio_controller *ggc)
+{
+	struct gb_gpio_line_count_response response;
+	int ret;
+
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_LINE_COUNT,
+				NULL, 0, &response, sizeof(response));
+	if (!ret)
+		ggc->line_max = response.count;
+	return ret;
+}
+
+static int gb_gpio_activate_operation(struct gb_gpio_controller *ggc, u8 which)
+{
+	struct gb_gpio_activate_request request;
+	struct gbphy_device *gbphy_dev = ggc->gbphy_dev;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	request.which = which;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_ACTIVATE,
+				&request, sizeof(request), NULL, 0);
+	if (ret) {
+		gbphy_runtime_put_autosuspend(gbphy_dev);
+		return ret;
+	}
+
+	ggc->lines[which].active = true;
+
+	return 0;
+}
+
+static void gb_gpio_deactivate_operation(struct gb_gpio_controller *ggc,
+					 u8 which)
+{
+	struct gbphy_device *gbphy_dev = ggc->gbphy_dev;
+	struct device *dev = &gbphy_dev->dev;
+	struct gb_gpio_deactivate_request request;
+	int ret;
+
+	request.which = which;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_DEACTIVATE,
+				&request, sizeof(request), NULL, 0);
+	if (ret) {
+		dev_err(dev, "failed to deactivate gpio %u\n", which);
+		goto out_pm_put;
+	}
+
+	ggc->lines[which].active = false;
+
+out_pm_put:
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+}
+
+static int gb_gpio_get_direction_operation(struct gb_gpio_controller *ggc,
+					   u8 which)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_get_direction_request request;
+	struct gb_gpio_get_direction_response response;
+	int ret;
+	u8 direction;
+
+	request.which = which;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_GET_DIRECTION,
+				&request, sizeof(request),
+				&response, sizeof(response));
+	if (ret)
+		return ret;
+
+	direction = response.direction;
+	if (direction && direction != 1) {
+		dev_warn(dev, "gpio %u direction was %u (should be 0 or 1)\n",
+			 which, direction);
+	}
+	ggc->lines[which].direction = direction ? 1 : 0;
+	return 0;
+}
+
+static int gb_gpio_direction_in_operation(struct gb_gpio_controller *ggc,
+					  u8 which)
+{
+	struct gb_gpio_direction_in_request request;
+	int ret;
+
+	request.which = which;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_DIRECTION_IN,
+				&request, sizeof(request), NULL, 0);
+	if (!ret)
+		ggc->lines[which].direction = 1;
+	return ret;
+}
+
+static int gb_gpio_direction_out_operation(struct gb_gpio_controller *ggc,
+					   u8 which, bool value_high)
+{
+	struct gb_gpio_direction_out_request request;
+	int ret;
+
+	request.which = which;
+	request.value = value_high ? 1 : 0;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_DIRECTION_OUT,
+				&request, sizeof(request), NULL, 0);
+	if (!ret)
+		ggc->lines[which].direction = 0;
+	return ret;
+}
+
+static int gb_gpio_get_value_operation(struct gb_gpio_controller *ggc,
+				       u8 which)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_get_value_request request;
+	struct gb_gpio_get_value_response response;
+	int ret;
+	u8 value;
+
+	request.which = which;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_GET_VALUE,
+				&request, sizeof(request),
+				&response, sizeof(response));
+	if (ret) {
+		dev_err(dev, "failed to get value of gpio %u\n", which);
+		return ret;
+	}
+
+	value = response.value;
+	if (value && value != 1) {
+		dev_warn(dev, "gpio %u value was %u (should be 0 or 1)\n",
+			 which, value);
+	}
+	ggc->lines[which].value = value ? 1 : 0;
+	return 0;
+}
+
+static void gb_gpio_set_value_operation(struct gb_gpio_controller *ggc,
+					u8 which, bool value_high)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_set_value_request request;
+	int ret;
+
+	if (ggc->lines[which].direction == 1) {
+		dev_warn(dev, "refusing to set value of input gpio %u\n",
+			 which);
+		return;
+	}
+
+	request.which = which;
+	request.value = value_high ? 1 : 0;
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_SET_VALUE,
+				&request, sizeof(request), NULL, 0);
+	if (ret) {
+		dev_err(dev, "failed to set value of gpio %u\n", which);
+		return;
+	}
+
+	ggc->lines[which].value = request.value;
+}
+
+static int gb_gpio_set_debounce_operation(struct gb_gpio_controller *ggc,
+					  u8 which, u16 debounce_usec)
+{
+	struct gb_gpio_set_debounce_request request;
+	int ret;
+
+	request.which = which;
+	request.usec = cpu_to_le16(debounce_usec);
+	ret = gb_operation_sync(ggc->connection, GB_GPIO_TYPE_SET_DEBOUNCE,
+				&request, sizeof(request), NULL, 0);
+	if (!ret)
+		ggc->lines[which].debounce_usec = debounce_usec;
+	return ret;
+}
+
+static void _gb_gpio_irq_mask(struct gb_gpio_controller *ggc, u8 hwirq)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_irq_mask_request request;
+	int ret;
+
+	request.which = hwirq;
+	ret = gb_operation_sync(ggc->connection,
+				GB_GPIO_TYPE_IRQ_MASK,
+				&request, sizeof(request), NULL, 0);
+	if (ret)
+		dev_err(dev, "failed to mask irq: %d\n", ret);
+}
+
+static void _gb_gpio_irq_unmask(struct gb_gpio_controller *ggc, u8 hwirq)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_irq_unmask_request request;
+	int ret;
+
+	request.which = hwirq;
+	ret = gb_operation_sync(ggc->connection,
+				GB_GPIO_TYPE_IRQ_UNMASK,
+				&request, sizeof(request), NULL, 0);
+	if (ret)
+		dev_err(dev, "failed to unmask irq: %d\n", ret);
+}
+
+static void _gb_gpio_irq_set_type(struct gb_gpio_controller *ggc,
+				  u8 hwirq, u8 type)
+{
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_gpio_irq_type_request request;
+	int ret;
+
+	request.which = hwirq;
+	request.type = type;
+
+	ret = gb_operation_sync(ggc->connection,
+				GB_GPIO_TYPE_IRQ_TYPE,
+				&request, sizeof(request), NULL, 0);
+	if (ret)
+		dev_err(dev, "failed to set irq type: %d\n", ret);
+}
+
+static void gb_gpio_irq_mask(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_to_gpio_chip(d);
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	struct gb_gpio_line *line = &ggc->lines[d->hwirq];
+
+	line->masked = true;
+	line->masked_pending = true;
+}
+
+static void gb_gpio_irq_unmask(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_to_gpio_chip(d);
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	struct gb_gpio_line *line = &ggc->lines[d->hwirq];
+
+	line->masked = false;
+	line->masked_pending = true;
+}
+
+static int gb_gpio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct gpio_chip *chip = irq_data_to_gpio_chip(d);
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	struct gb_gpio_line *line = &ggc->lines[d->hwirq];
+	struct device *dev = &ggc->gbphy_dev->dev;
+	u8 irq_type;
+
+	switch (type) {
+	case IRQ_TYPE_NONE:
+		irq_type = GB_GPIO_IRQ_TYPE_NONE;
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		irq_type = GB_GPIO_IRQ_TYPE_EDGE_RISING;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		irq_type = GB_GPIO_IRQ_TYPE_EDGE_FALLING;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		irq_type = GB_GPIO_IRQ_TYPE_EDGE_BOTH;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		irq_type = GB_GPIO_IRQ_TYPE_LEVEL_LOW;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		irq_type = GB_GPIO_IRQ_TYPE_LEVEL_HIGH;
+		break;
+	default:
+		dev_err(dev, "unsupported irq type: %u\n", type);
+		return -EINVAL;
+	}
+
+	line->irq_type = irq_type;
+	line->irq_type_pending = true;
+
+	return 0;
+}
+
+static void gb_gpio_irq_bus_lock(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_to_gpio_chip(d);
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	mutex_lock(&ggc->irq_lock);
+}
+
+static void gb_gpio_irq_bus_sync_unlock(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_to_gpio_chip(d);
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	struct gb_gpio_line *line = &ggc->lines[d->hwirq];
+
+	if (line->irq_type_pending) {
+		_gb_gpio_irq_set_type(ggc, d->hwirq, line->irq_type);
+		line->irq_type_pending = false;
+	}
+
+	if (line->masked_pending) {
+		if (line->masked)
+			_gb_gpio_irq_mask(ggc, d->hwirq);
+		else
+			_gb_gpio_irq_unmask(ggc, d->hwirq);
+		line->masked_pending = false;
+	}
+
+	mutex_unlock(&ggc->irq_lock);
+}
+
+static int gb_gpio_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_gpio_controller *ggc = gb_connection_get_data(connection);
+	struct device *dev = &ggc->gbphy_dev->dev;
+	struct gb_message *request;
+	struct gb_gpio_irq_event_request *event;
+	u8 type = op->type;
+	int irq, ret;
+
+	if (type != GB_GPIO_TYPE_IRQ_EVENT) {
+		dev_err(dev, "unsupported unsolicited request: %u\n", type);
+		return -EINVAL;
+	}
+
+	request = op->request;
+
+	if (request->payload_size < sizeof(*event)) {
+		dev_err(dev, "short event received (%zu < %zu)\n",
+			request->payload_size, sizeof(*event));
+		return -EINVAL;
+	}
+
+	event = request->payload;
+	if (event->which > ggc->line_max) {
+		dev_err(dev, "invalid hw irq: %d\n", event->which);
+		return -EINVAL;
+	}
+
+	irq = irq_find_mapping(ggc->chip.irq.domain, event->which);
+	if (!irq) {
+		dev_err(dev, "failed to find IRQ\n");
+		return -EINVAL;
+	}
+
+	ret = generic_handle_irq_safe(irq);
+	if (ret)
+		dev_err(dev, "failed to invoke irq handler\n");
+
+	return ret;
+}
+
+static int gb_gpio_request(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	return gb_gpio_activate_operation(ggc, (u8)offset);
+}
+
+static void gb_gpio_free(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	gb_gpio_deactivate_operation(ggc, (u8)offset);
+}
+
+static int gb_gpio_get_direction(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	u8 which;
+	int ret;
+
+	which = (u8)offset;
+	ret = gb_gpio_get_direction_operation(ggc, which);
+	if (ret)
+		return ret;
+
+	return ggc->lines[which].direction ? 1 : 0;
+}
+
+static int gb_gpio_direction_input(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	return gb_gpio_direction_in_operation(ggc, (u8)offset);
+}
+
+static int gb_gpio_direction_output(struct gpio_chip *chip, unsigned int offset,
+				    int value)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	return gb_gpio_direction_out_operation(ggc, (u8)offset, !!value);
+}
+
+static int gb_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	u8 which;
+	int ret;
+
+	which = (u8)offset;
+	ret = gb_gpio_get_value_operation(ggc, which);
+	if (ret)
+		return ret;
+
+	return ggc->lines[which].value;
+}
+
+static void gb_gpio_set(struct gpio_chip *chip, unsigned int offset, int value)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+
+	gb_gpio_set_value_operation(ggc, (u8)offset, !!value);
+}
+
+static int gb_gpio_set_config(struct gpio_chip *chip, unsigned int offset,
+			      unsigned long config)
+{
+	struct gb_gpio_controller *ggc = gpio_chip_to_gb_gpio_controller(chip);
+	u32 debounce;
+
+	if (pinconf_to_config_param(config) != PIN_CONFIG_INPUT_DEBOUNCE)
+		return -ENOTSUPP;
+
+	debounce = pinconf_to_config_argument(config);
+	if (debounce > U16_MAX)
+		return -EINVAL;
+
+	return gb_gpio_set_debounce_operation(ggc, (u8)offset, (u16)debounce);
+}
+
+static int gb_gpio_controller_setup(struct gb_gpio_controller *ggc)
+{
+	int ret;
+
+	/* Now find out how many lines there are */
+	ret = gb_gpio_line_count_operation(ggc);
+	if (ret)
+		return ret;
+
+	ggc->lines = kcalloc(ggc->line_max + 1, sizeof(*ggc->lines),
+			     GFP_KERNEL);
+	if (!ggc->lines)
+		return -ENOMEM;
+
+	return ret;
+}
+
+static int gb_gpio_probe(struct gbphy_device *gbphy_dev,
+			 const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	struct gb_gpio_controller *ggc;
+	struct gpio_chip *gpio;
+	struct gpio_irq_chip *girq;
+	struct irq_chip *irqc;
+	int ret;
+
+	ggc = kzalloc(sizeof(*ggc), GFP_KERNEL);
+	if (!ggc)
+		return -ENOMEM;
+
+	connection =
+		gb_connection_create(gbphy_dev->bundle,
+				     le16_to_cpu(gbphy_dev->cport_desc->id),
+				     gb_gpio_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto exit_ggc_free;
+	}
+
+	ggc->connection = connection;
+	gb_connection_set_data(connection, ggc);
+	ggc->gbphy_dev = gbphy_dev;
+	gb_gbphy_set_data(gbphy_dev, ggc);
+
+	ret = gb_connection_enable_tx(connection);
+	if (ret)
+		goto exit_connection_destroy;
+
+	ret = gb_gpio_controller_setup(ggc);
+	if (ret)
+		goto exit_connection_disable;
+
+	irqc = &ggc->irqc;
+	irqc->irq_mask = gb_gpio_irq_mask;
+	irqc->irq_unmask = gb_gpio_irq_unmask;
+	irqc->irq_set_type = gb_gpio_irq_set_type;
+	irqc->irq_bus_lock = gb_gpio_irq_bus_lock;
+	irqc->irq_bus_sync_unlock = gb_gpio_irq_bus_sync_unlock;
+	irqc->name = "greybus_gpio";
+
+	mutex_init(&ggc->irq_lock);
+
+	gpio = &ggc->chip;
+
+	gpio->label = "greybus_gpio";
+	gpio->parent = &gbphy_dev->dev;
+	gpio->owner = THIS_MODULE;
+
+	gpio->request = gb_gpio_request;
+	gpio->free = gb_gpio_free;
+	gpio->get_direction = gb_gpio_get_direction;
+	gpio->direction_input = gb_gpio_direction_input;
+	gpio->direction_output = gb_gpio_direction_output;
+	gpio->get = gb_gpio_get;
+	gpio->set = gb_gpio_set;
+	gpio->set_config = gb_gpio_set_config;
+	gpio->base = -1;		/* Allocate base dynamically */
+	gpio->ngpio = ggc->line_max + 1;
+	gpio->can_sleep = true;
+
+	girq = &gpio->irq;
+	girq->chip = irqc;
+	/* The event comes from the outside so no parent handler */
+	girq->parent_handler = NULL;
+	girq->num_parents = 0;
+	girq->parents = NULL;
+	girq->default_type = IRQ_TYPE_NONE;
+	girq->handler = handle_level_irq;
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto exit_line_free;
+
+	ret = gpiochip_add(gpio);
+	if (ret) {
+		dev_err(&gbphy_dev->dev, "failed to add gpio chip: %d\n", ret);
+		goto exit_line_free;
+	}
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+	return 0;
+
+exit_line_free:
+	kfree(ggc->lines);
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+exit_ggc_free:
+	kfree(ggc);
+	return ret;
+}
+
+static void gb_gpio_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_gpio_controller *ggc = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = ggc->connection;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	gb_connection_disable_rx(connection);
+	gpiochip_remove(&ggc->chip);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+	kfree(ggc->lines);
+	kfree(ggc);
+}
+
+static const struct gbphy_device_id gb_gpio_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_GPIO) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_gpio_id_table);
+
+static struct gbphy_driver gpio_driver = {
+	.name		= "gpio",
+	.probe		= gb_gpio_probe,
+	.remove		= gb_gpio_remove,
+	.id_table	= gb_gpio_id_table,
+};
+
+module_gbphy_driver(gpio_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/greybus_authentication.h b/drivers/staging/greybus/greybus_authentication.h
new file mode 100644
index 000000000..7edc7295b
--- /dev/null
+++ b/drivers/staging/greybus/greybus_authentication.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause) */
+/*
+ * Greybus Component Authentication User Header
+ *
+ * Copyright(c) 2016 Google Inc. All rights reserved.
+ * Copyright(c) 2016 Linaro Ltd. All rights reserved.
+ */
+
+#ifndef __GREYBUS_AUTHENTICATION_USER_H
+#define __GREYBUS_AUTHENTICATION_USER_H
+
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+#define CAP_CERTIFICATE_MAX_SIZE	1600
+#define CAP_SIGNATURE_MAX_SIZE		320
+
+/* Certificate class types */
+#define CAP_CERT_IMS_EAPC		0x00000001
+#define CAP_CERT_IMS_EASC		0x00000002
+#define CAP_CERT_IMS_EARC		0x00000003
+#define CAP_CERT_IMS_IAPC		0x00000004
+#define CAP_CERT_IMS_IASC		0x00000005
+#define CAP_CERT_IMS_IARC		0x00000006
+
+/* IMS Certificate response result codes */
+#define CAP_IMS_RESULT_CERT_FOUND	0x00
+#define CAP_IMS_RESULT_CERT_CLASS_INVAL	0x01
+#define CAP_IMS_RESULT_CERT_CORRUPT	0x02
+#define CAP_IMS_RESULT_CERT_NOT_FOUND	0x03
+
+/* Authentication types */
+#define CAP_AUTH_IMS_PRI		0x00000001
+#define CAP_AUTH_IMS_SEC		0x00000002
+#define CAP_AUTH_IMS_RSA		0x00000003
+
+/* Authenticate response result codes */
+#define CAP_AUTH_RESULT_CR_SUCCESS	0x00
+#define CAP_AUTH_RESULT_CR_BAD_TYPE	0x01
+#define CAP_AUTH_RESULT_CR_WRONG_EP	0x02
+#define CAP_AUTH_RESULT_CR_NO_KEY	0x03
+#define CAP_AUTH_RESULT_CR_SIG_FAIL	0x04
+
+
+/* IOCTL support */
+struct cap_ioc_get_endpoint_uid {
+	__u8			uid[8];
+} __attribute__ ((__packed__));
+
+struct cap_ioc_get_ims_certificate {
+	__u32			certificate_class;
+	__u32			certificate_id;
+
+	__u8			result_code;
+	__u32			cert_size;
+	__u8			certificate[CAP_CERTIFICATE_MAX_SIZE];
+} __attribute__ ((__packed__));
+
+struct cap_ioc_authenticate {
+	__u32			auth_type;
+	__u8			uid[8];
+	__u8			challenge[32];
+
+	__u8			result_code;
+	__u8			response[64];
+	__u32			signature_size;
+	__u8			signature[CAP_SIGNATURE_MAX_SIZE];
+} __attribute__ ((__packed__));
+
+#define CAP_IOCTL_BASE			'C'
+#define CAP_IOC_GET_ENDPOINT_UID	_IOR(CAP_IOCTL_BASE, 0, struct cap_ioc_get_endpoint_uid)
+#define CAP_IOC_GET_IMS_CERTIFICATE	_IOWR(CAP_IOCTL_BASE, 1, struct cap_ioc_get_ims_certificate)
+#define CAP_IOC_AUTHENTICATE		_IOWR(CAP_IOCTL_BASE, 2, struct cap_ioc_authenticate)
+
+#endif /* __GREYBUS_AUTHENTICATION_USER_H */
diff --git a/drivers/staging/greybus/greybus_firmware.h b/drivers/staging/greybus/greybus_firmware.h
new file mode 100644
index 000000000..f68fd5e25
--- /dev/null
+++ b/drivers/staging/greybus/greybus_firmware.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause) */
+/*
+ * Greybus Firmware Management User Header
+ *
+ * Copyright(c) 2016 Google Inc. All rights reserved.
+ * Copyright(c) 2016 Linaro Ltd. All rights reserved.
+ */
+
+#ifndef __GREYBUS_FIRMWARE_USER_H
+#define __GREYBUS_FIRMWARE_USER_H
+
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+#define GB_FIRMWARE_U_TAG_MAX_SIZE		10
+
+#define GB_FW_U_LOAD_METHOD_UNIPRO		0x01
+#define GB_FW_U_LOAD_METHOD_INTERNAL		0x02
+
+#define GB_FW_U_LOAD_STATUS_FAILED		0x00
+#define GB_FW_U_LOAD_STATUS_UNVALIDATED		0x01
+#define GB_FW_U_LOAD_STATUS_VALIDATED		0x02
+#define GB_FW_U_LOAD_STATUS_VALIDATION_FAILED	0x03
+
+#define GB_FW_U_BACKEND_FW_STATUS_SUCCESS	0x01
+#define GB_FW_U_BACKEND_FW_STATUS_FAIL_FIND	0x02
+#define GB_FW_U_BACKEND_FW_STATUS_FAIL_FETCH	0x03
+#define GB_FW_U_BACKEND_FW_STATUS_FAIL_WRITE	0x04
+#define GB_FW_U_BACKEND_FW_STATUS_INT		0x05
+#define GB_FW_U_BACKEND_FW_STATUS_RETRY		0x06
+#define GB_FW_U_BACKEND_FW_STATUS_NOT_SUPPORTED	0x07
+
+#define GB_FW_U_BACKEND_VERSION_STATUS_SUCCESS		0x01
+#define GB_FW_U_BACKEND_VERSION_STATUS_NOT_AVAILABLE	0x02
+#define GB_FW_U_BACKEND_VERSION_STATUS_NOT_SUPPORTED	0x03
+#define GB_FW_U_BACKEND_VERSION_STATUS_RETRY		0x04
+#define GB_FW_U_BACKEND_VERSION_STATUS_FAIL_INT		0x05
+
+/* IOCTL support */
+struct fw_mgmt_ioc_get_intf_version {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u16 major;
+	__u16 minor;
+} __attribute__ ((__packed__));
+
+struct fw_mgmt_ioc_get_backend_version {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u16 major;
+	__u16 minor;
+	__u8 status;
+} __attribute__ ((__packed__));
+
+struct fw_mgmt_ioc_intf_load_and_validate {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u8 load_method;
+	__u8 status;
+	__u16 major;
+	__u16 minor;
+} __attribute__ ((__packed__));
+
+struct fw_mgmt_ioc_backend_fw_update {
+	__u8 firmware_tag[GB_FIRMWARE_U_TAG_MAX_SIZE];
+	__u8 status;
+} __attribute__ ((__packed__));
+
+#define FW_MGMT_IOCTL_BASE			'F'
+#define FW_MGMT_IOC_GET_INTF_FW			_IOR(FW_MGMT_IOCTL_BASE, 0, struct fw_mgmt_ioc_get_intf_version)
+#define FW_MGMT_IOC_GET_BACKEND_FW		_IOWR(FW_MGMT_IOCTL_BASE, 1, struct fw_mgmt_ioc_get_backend_version)
+#define FW_MGMT_IOC_INTF_LOAD_AND_VALIDATE	_IOWR(FW_MGMT_IOCTL_BASE, 2, struct fw_mgmt_ioc_intf_load_and_validate)
+#define FW_MGMT_IOC_INTF_BACKEND_FW_UPDATE	_IOWR(FW_MGMT_IOCTL_BASE, 3, struct fw_mgmt_ioc_backend_fw_update)
+#define FW_MGMT_IOC_SET_TIMEOUT_MS		_IOW(FW_MGMT_IOCTL_BASE, 4, unsigned int)
+#define FW_MGMT_IOC_MODE_SWITCH			_IO(FW_MGMT_IOCTL_BASE, 5)
+
+#endif /* __GREYBUS_FIRMWARE_USER_H */
+
diff --git a/drivers/staging/greybus/hid.c b/drivers/staging/greybus/hid.c
new file mode 100644
index 000000000..adb912868
--- /dev/null
+++ b/drivers/staging/greybus/hid.c
@@ -0,0 +1,519 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * HID class driver for the Greybus.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#include <linux/bitops.h>
+#include <linux/hid.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/greybus.h>
+
+/* Greybus HID device's structure */
+struct gb_hid {
+	struct gb_bundle *bundle;
+	struct gb_connection		*connection;
+
+	struct hid_device		*hid;
+	struct gb_hid_desc_response	hdesc;
+
+	unsigned long			flags;
+#define GB_HID_STARTED			0x01
+#define GB_HID_READ_PENDING		0x04
+
+	unsigned int			bufsize;
+	char				*inbuf;
+};
+
+/* Routines to get controller's information over greybus */
+
+/* Operations performed on greybus */
+static int gb_hid_get_desc(struct gb_hid *ghid)
+{
+	return gb_operation_sync(ghid->connection, GB_HID_TYPE_GET_DESC, NULL,
+				 0, &ghid->hdesc, sizeof(ghid->hdesc));
+}
+
+static int gb_hid_get_report_desc(struct gb_hid *ghid, char *rdesc)
+{
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(ghid->bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(ghid->connection, GB_HID_TYPE_GET_REPORT_DESC,
+				NULL, 0, rdesc,
+				le16_to_cpu(ghid->hdesc.wReportDescLength));
+
+	gb_pm_runtime_put_autosuspend(ghid->bundle);
+
+	return ret;
+}
+
+static int gb_hid_set_power(struct gb_hid *ghid, int type)
+{
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(ghid->bundle);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(ghid->connection, type, NULL, 0, NULL, 0);
+
+	gb_pm_runtime_put_autosuspend(ghid->bundle);
+
+	return ret;
+}
+
+static int gb_hid_get_report(struct gb_hid *ghid, u8 report_type, u8 report_id,
+			     unsigned char *buf, int len)
+{
+	struct gb_hid_get_report_request request;
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(ghid->bundle);
+	if (ret)
+		return ret;
+
+	request.report_type = report_type;
+	request.report_id = report_id;
+
+	ret = gb_operation_sync(ghid->connection, GB_HID_TYPE_GET_REPORT,
+				&request, sizeof(request), buf, len);
+
+	gb_pm_runtime_put_autosuspend(ghid->bundle);
+
+	return ret;
+}
+
+static int gb_hid_set_report(struct gb_hid *ghid, u8 report_type, u8 report_id,
+			     unsigned char *buf, int len)
+{
+	struct gb_hid_set_report_request *request;
+	struct gb_operation *operation;
+	int ret, size = sizeof(*request) + len - 1;
+
+	ret = gb_pm_runtime_get_sync(ghid->bundle);
+	if (ret)
+		return ret;
+
+	operation = gb_operation_create(ghid->connection,
+					GB_HID_TYPE_SET_REPORT, size, 0,
+					GFP_KERNEL);
+	if (!operation) {
+		gb_pm_runtime_put_autosuspend(ghid->bundle);
+		return -ENOMEM;
+	}
+
+	request = operation->request->payload;
+	request->report_type = report_type;
+	request->report_id = report_id;
+	memcpy(request->report, buf, len);
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret) {
+		dev_err(&operation->connection->bundle->dev,
+			"failed to set report: %d\n", ret);
+	} else {
+		ret = len;
+	}
+
+	gb_operation_put(operation);
+	gb_pm_runtime_put_autosuspend(ghid->bundle);
+
+	return ret;
+}
+
+static int gb_hid_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_hid *ghid = gb_connection_get_data(connection);
+	struct gb_hid_input_report_request *request = op->request->payload;
+
+	if (op->type != GB_HID_TYPE_IRQ_EVENT) {
+		dev_err(&connection->bundle->dev,
+			"unsupported unsolicited request\n");
+		return -EINVAL;
+	}
+
+	if (test_bit(GB_HID_STARTED, &ghid->flags))
+		hid_input_report(ghid->hid, HID_INPUT_REPORT,
+				 request->report, op->request->payload_size, 1);
+
+	return 0;
+}
+
+static int gb_hid_report_len(struct hid_report *report)
+{
+	return ((report->size - 1) >> 3) + 1 +
+		report->device->report_enum[report->type].numbered;
+}
+
+static void gb_hid_find_max_report(struct hid_device *hid, unsigned int type,
+				   unsigned int *max)
+{
+	struct hid_report *report;
+	unsigned int size;
+
+	list_for_each_entry(report, &hid->report_enum[type].report_list, list) {
+		size = gb_hid_report_len(report);
+		if (*max < size)
+			*max = size;
+	}
+}
+
+static void gb_hid_free_buffers(struct gb_hid *ghid)
+{
+	kfree(ghid->inbuf);
+	ghid->inbuf = NULL;
+	ghid->bufsize = 0;
+}
+
+static int gb_hid_alloc_buffers(struct gb_hid *ghid, size_t bufsize)
+{
+	ghid->inbuf = kzalloc(bufsize, GFP_KERNEL);
+	if (!ghid->inbuf)
+		return -ENOMEM;
+
+	ghid->bufsize = bufsize;
+
+	return 0;
+}
+
+/* Routines dealing with reports */
+static void gb_hid_init_report(struct gb_hid *ghid, struct hid_report *report)
+{
+	unsigned int size;
+
+	size = gb_hid_report_len(report);
+	if (gb_hid_get_report(ghid, report->type, report->id, ghid->inbuf,
+			      size))
+		return;
+
+	/*
+	 * hid->driver_lock is held as we are in probe function,
+	 * we just need to setup the input fields, so using
+	 * hid_report_raw_event is safe.
+	 */
+	hid_report_raw_event(ghid->hid, report->type, ghid->inbuf, size, 1);
+}
+
+static void gb_hid_init_reports(struct gb_hid *ghid)
+{
+	struct hid_device *hid = ghid->hid;
+	struct hid_report *report;
+
+	list_for_each_entry(report,
+			    &hid->report_enum[HID_INPUT_REPORT].report_list,
+			    list)
+		gb_hid_init_report(ghid, report);
+
+	list_for_each_entry(report,
+			    &hid->report_enum[HID_FEATURE_REPORT].report_list,
+			    list)
+		gb_hid_init_report(ghid, report);
+}
+
+static int __gb_hid_get_raw_report(struct hid_device *hid,
+				   unsigned char report_number, __u8 *buf, size_t count,
+				   unsigned char report_type)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	int ret;
+
+	if (report_type == HID_OUTPUT_REPORT)
+		return -EINVAL;
+
+	ret = gb_hid_get_report(ghid, report_type, report_number, buf, count);
+	if (!ret)
+		ret = count;
+
+	return ret;
+}
+
+static int __gb_hid_output_raw_report(struct hid_device *hid, __u8 *buf,
+				      size_t len, unsigned char report_type)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	int report_id = buf[0];
+	int ret;
+
+	if (report_type == HID_INPUT_REPORT)
+		return -EINVAL;
+
+	if (report_id) {
+		buf++;
+		len--;
+	}
+
+	ret = gb_hid_set_report(ghid, report_type, report_id, buf, len);
+	if (report_id && ret >= 0)
+		ret++; /* add report_id to the number of transferred bytes */
+
+	return 0;
+}
+
+static int gb_hid_raw_request(struct hid_device *hid, unsigned char reportnum,
+			      __u8 *buf, size_t len, unsigned char rtype,
+			      int reqtype)
+{
+	switch (reqtype) {
+	case HID_REQ_GET_REPORT:
+		return __gb_hid_get_raw_report(hid, reportnum, buf, len, rtype);
+	case HID_REQ_SET_REPORT:
+		if (buf[0] != reportnum)
+			return -EINVAL;
+		return __gb_hid_output_raw_report(hid, buf, len, rtype);
+	default:
+		return -EIO;
+	}
+}
+
+/* HID Callbacks */
+static int gb_hid_parse(struct hid_device *hid)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	unsigned int rsize;
+	char *rdesc;
+	int ret;
+
+	rsize = le16_to_cpu(ghid->hdesc.wReportDescLength);
+	if (!rsize || rsize > HID_MAX_DESCRIPTOR_SIZE) {
+		dbg_hid("weird size of report descriptor (%u)\n", rsize);
+		return -EINVAL;
+	}
+
+	rdesc = kzalloc(rsize, GFP_KERNEL);
+	if (!rdesc)
+		return -ENOMEM;
+
+	ret = gb_hid_get_report_desc(ghid, rdesc);
+	if (ret) {
+		hid_err(hid, "reading report descriptor failed\n");
+		goto free_rdesc;
+	}
+
+	ret = hid_parse_report(hid, rdesc, rsize);
+	if (ret)
+		dbg_hid("parsing report descriptor failed\n");
+
+free_rdesc:
+	kfree(rdesc);
+
+	return ret;
+}
+
+static int gb_hid_start(struct hid_device *hid)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	unsigned int bufsize = HID_MIN_BUFFER_SIZE;
+	int ret;
+
+	gb_hid_find_max_report(hid, HID_INPUT_REPORT, &bufsize);
+	gb_hid_find_max_report(hid, HID_OUTPUT_REPORT, &bufsize);
+	gb_hid_find_max_report(hid, HID_FEATURE_REPORT, &bufsize);
+
+	if (bufsize > HID_MAX_BUFFER_SIZE)
+		bufsize = HID_MAX_BUFFER_SIZE;
+
+	ret = gb_hid_alloc_buffers(ghid, bufsize);
+	if (ret)
+		return ret;
+
+	if (!(hid->quirks & HID_QUIRK_NO_INIT_REPORTS))
+		gb_hid_init_reports(ghid);
+
+	return 0;
+}
+
+static void gb_hid_stop(struct hid_device *hid)
+{
+	struct gb_hid *ghid = hid->driver_data;
+
+	gb_hid_free_buffers(ghid);
+}
+
+static int gb_hid_open(struct hid_device *hid)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	int ret;
+
+	ret = gb_hid_set_power(ghid, GB_HID_TYPE_PWR_ON);
+	if (ret < 0)
+		return ret;
+
+	set_bit(GB_HID_STARTED, &ghid->flags);
+	return 0;
+}
+
+static void gb_hid_close(struct hid_device *hid)
+{
+	struct gb_hid *ghid = hid->driver_data;
+	int ret;
+
+	clear_bit(GB_HID_STARTED, &ghid->flags);
+
+	/* Save some power */
+	ret = gb_hid_set_power(ghid, GB_HID_TYPE_PWR_OFF);
+	if (ret)
+		dev_err(&ghid->connection->bundle->dev,
+			"failed to power off (%d)\n", ret);
+}
+
+static int gb_hid_power(struct hid_device *hid, int lvl)
+{
+	struct gb_hid *ghid = hid->driver_data;
+
+	switch (lvl) {
+	case PM_HINT_FULLON:
+		return gb_hid_set_power(ghid, GB_HID_TYPE_PWR_ON);
+	case PM_HINT_NORMAL:
+		return gb_hid_set_power(ghid, GB_HID_TYPE_PWR_OFF);
+	}
+
+	return 0;
+}
+
+/* HID structure to pass callbacks */
+static struct hid_ll_driver gb_hid_ll_driver = {
+	.parse = gb_hid_parse,
+	.start = gb_hid_start,
+	.stop = gb_hid_stop,
+	.open = gb_hid_open,
+	.close = gb_hid_close,
+	.power = gb_hid_power,
+	.raw_request = gb_hid_raw_request,
+};
+
+static int gb_hid_init(struct gb_hid *ghid)
+{
+	struct hid_device *hid = ghid->hid;
+	int ret;
+
+	ret = gb_hid_get_desc(ghid);
+	if (ret)
+		return ret;
+
+	hid->version = le16_to_cpu(ghid->hdesc.bcdHID);
+	hid->vendor = le16_to_cpu(ghid->hdesc.wVendorID);
+	hid->product = le16_to_cpu(ghid->hdesc.wProductID);
+	hid->country = ghid->hdesc.bCountryCode;
+
+	hid->driver_data = ghid;
+	hid->ll_driver = &gb_hid_ll_driver;
+	hid->dev.parent = &ghid->connection->bundle->dev;
+//	hid->bus = BUS_GREYBUS; /* Need a bustype for GREYBUS in <linux/input.h> */
+
+	/* Set HID device's name */
+	snprintf(hid->name, sizeof(hid->name), "%s %04X:%04X",
+		 dev_name(&ghid->connection->bundle->dev),
+		 hid->vendor, hid->product);
+
+	return 0;
+}
+
+static int gb_hid_probe(struct gb_bundle *bundle,
+			const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct hid_device *hid;
+	struct gb_hid *ghid;
+	int ret;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_HID)
+		return -ENODEV;
+
+	ghid = kzalloc(sizeof(*ghid), GFP_KERNEL);
+	if (!ghid)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_hid_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto err_free_ghid;
+	}
+
+	gb_connection_set_data(connection, ghid);
+	ghid->connection = connection;
+
+	hid = hid_allocate_device();
+	if (IS_ERR(hid)) {
+		ret = PTR_ERR(hid);
+		goto err_connection_destroy;
+	}
+
+	ghid->hid = hid;
+	ghid->bundle = bundle;
+
+	greybus_set_drvdata(bundle, ghid);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto err_destroy_hid;
+
+	ret = gb_hid_init(ghid);
+	if (ret)
+		goto err_connection_disable;
+
+	ret = hid_add_device(hid);
+	if (ret) {
+		hid_err(hid, "can't add hid device: %d\n", ret);
+		goto err_connection_disable;
+	}
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+err_connection_disable:
+	gb_connection_disable(connection);
+err_destroy_hid:
+	hid_destroy_device(hid);
+err_connection_destroy:
+	gb_connection_destroy(connection);
+err_free_ghid:
+	kfree(ghid);
+
+	return ret;
+}
+
+static void gb_hid_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_hid *ghid = greybus_get_drvdata(bundle);
+
+	if (gb_pm_runtime_get_sync(bundle))
+		gb_pm_runtime_get_noresume(bundle);
+
+	hid_destroy_device(ghid->hid);
+	gb_connection_disable(ghid->connection);
+	gb_connection_destroy(ghid->connection);
+	kfree(ghid);
+}
+
+static const struct greybus_bundle_id gb_hid_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_HID) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_hid_id_table);
+
+static struct greybus_driver gb_hid_driver = {
+	.name		= "hid",
+	.probe		= gb_hid_probe,
+	.disconnect	= gb_hid_disconnect,
+	.id_table	= gb_hid_id_table,
+};
+module_greybus_driver(gb_hid_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/i2c.c b/drivers/staging/greybus/i2c.c
new file mode 100644
index 000000000..de2f6516d
--- /dev/null
+++ b/drivers/staging/greybus/i2c.c
@@ -0,0 +1,321 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * I2C bridge driver for the Greybus "generic" I2C module.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+struct gb_i2c_device {
+	struct gb_connection	*connection;
+	struct gbphy_device	*gbphy_dev;
+
+	u32			functionality;
+
+	struct i2c_adapter	adapter;
+};
+
+/*
+ * Map Greybus i2c functionality bits into Linux ones
+ */
+static u32 gb_i2c_functionality_map(u32 gb_i2c_functionality)
+{
+	return gb_i2c_functionality;	/* All bits the same for now */
+}
+
+/*
+ * Do initial setup of the i2c device.  This includes verifying we
+ * can support it (based on the protocol version it advertises).
+ * If that's OK, we get and cached its functionality bits.
+ *
+ * Note: gb_i2c_dev->connection is assumed to have been valid.
+ */
+static int gb_i2c_device_setup(struct gb_i2c_device *gb_i2c_dev)
+{
+	struct gb_i2c_functionality_response response;
+	u32 functionality;
+	int ret;
+
+	ret = gb_operation_sync(gb_i2c_dev->connection,
+				GB_I2C_TYPE_FUNCTIONALITY,
+				NULL, 0, &response, sizeof(response));
+	if (ret)
+		return ret;
+
+	functionality = le32_to_cpu(response.functionality);
+	gb_i2c_dev->functionality = gb_i2c_functionality_map(functionality);
+
+	return 0;
+}
+
+/*
+ * Map Linux i2c_msg flags into Greybus i2c transfer op flags.
+ */
+static u16 gb_i2c_transfer_op_flags_map(u16 flags)
+{
+	return flags;	/* All flags the same for now */
+}
+
+static void
+gb_i2c_fill_transfer_op(struct gb_i2c_transfer_op *op, struct i2c_msg *msg)
+{
+	u16 flags = gb_i2c_transfer_op_flags_map(msg->flags);
+
+	op->addr = cpu_to_le16(msg->addr);
+	op->flags = cpu_to_le16(flags);
+	op->size = cpu_to_le16(msg->len);
+}
+
+static struct gb_operation *
+gb_i2c_operation_create(struct gb_connection *connection,
+			struct i2c_msg *msgs, u32 msg_count)
+{
+	struct gb_i2c_device *gb_i2c_dev = gb_connection_get_data(connection);
+	struct gb_i2c_transfer_request *request;
+	struct gb_operation *operation;
+	struct gb_i2c_transfer_op *op;
+	struct i2c_msg *msg;
+	u32 data_out_size = 0;
+	u32 data_in_size = 0;
+	size_t request_size;
+	void *data;
+	u16 op_count;
+	u32 i;
+
+	if (msg_count > (u32)U16_MAX) {
+		dev_err(&gb_i2c_dev->gbphy_dev->dev, "msg_count (%u) too big\n",
+			msg_count);
+		return NULL;
+	}
+	op_count = (u16)msg_count;
+
+	/*
+	 * In addition to space for all message descriptors we need
+	 * to have enough to hold all outbound message data.
+	 */
+	msg = msgs;
+	for (i = 0; i < msg_count; i++, msg++)
+		if (msg->flags & I2C_M_RD)
+			data_in_size += (u32)msg->len;
+		else
+			data_out_size += (u32)msg->len;
+
+	request_size = sizeof(*request);
+	request_size += msg_count * sizeof(*op);
+	request_size += data_out_size;
+
+	/* Response consists only of incoming data */
+	operation = gb_operation_create(connection, GB_I2C_TYPE_TRANSFER,
+					request_size, data_in_size, GFP_KERNEL);
+	if (!operation)
+		return NULL;
+
+	request = operation->request->payload;
+	request->op_count = cpu_to_le16(op_count);
+	/* Fill in the ops array */
+	op = &request->ops[0];
+	msg = msgs;
+	for (i = 0; i < msg_count; i++)
+		gb_i2c_fill_transfer_op(op++, msg++);
+
+	if (!data_out_size)
+		return operation;
+
+	/* Copy over the outgoing data; it starts after the last op */
+	data = op;
+	msg = msgs;
+	for (i = 0; i < msg_count; i++) {
+		if (!(msg->flags & I2C_M_RD)) {
+			memcpy(data, msg->buf, msg->len);
+			data += msg->len;
+		}
+		msg++;
+	}
+
+	return operation;
+}
+
+static void gb_i2c_decode_response(struct i2c_msg *msgs, u32 msg_count,
+				   struct gb_i2c_transfer_response *response)
+{
+	struct i2c_msg *msg = msgs;
+	u8 *data;
+	u32 i;
+
+	if (!response)
+		return;
+	data = response->data;
+	for (i = 0; i < msg_count; i++) {
+		if (msg->flags & I2C_M_RD) {
+			memcpy(msg->buf, data, msg->len);
+			data += msg->len;
+		}
+		msg++;
+	}
+}
+
+/*
+ * Some i2c transfer operations return results that are expected.
+ */
+static bool gb_i2c_expected_transfer_error(int errno)
+{
+	return errno == -EAGAIN || errno == -ENODEV;
+}
+
+static int gb_i2c_transfer_operation(struct gb_i2c_device *gb_i2c_dev,
+				     struct i2c_msg *msgs, u32 msg_count)
+{
+	struct gb_connection *connection = gb_i2c_dev->connection;
+	struct device *dev = &gb_i2c_dev->gbphy_dev->dev;
+	struct gb_operation *operation;
+	int ret;
+
+	operation = gb_i2c_operation_create(connection, msgs, msg_count);
+	if (!operation)
+		return -ENOMEM;
+
+	ret = gbphy_runtime_get_sync(gb_i2c_dev->gbphy_dev);
+	if (ret)
+		goto exit_operation_put;
+
+	ret = gb_operation_request_send_sync(operation);
+	if (!ret) {
+		struct gb_i2c_transfer_response *response;
+
+		response = operation->response->payload;
+		gb_i2c_decode_response(msgs, msg_count, response);
+		ret = msg_count;
+	} else if (!gb_i2c_expected_transfer_error(ret)) {
+		dev_err(dev, "transfer operation failed (%d)\n", ret);
+	}
+
+	gbphy_runtime_put_autosuspend(gb_i2c_dev->gbphy_dev);
+
+exit_operation_put:
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static int gb_i2c_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+			      int msg_count)
+{
+	struct gb_i2c_device *gb_i2c_dev;
+
+	gb_i2c_dev = i2c_get_adapdata(adap);
+
+	return gb_i2c_transfer_operation(gb_i2c_dev, msgs, msg_count);
+}
+
+static u32 gb_i2c_functionality(struct i2c_adapter *adap)
+{
+	struct gb_i2c_device *gb_i2c_dev = i2c_get_adapdata(adap);
+
+	return gb_i2c_dev->functionality;
+}
+
+static const struct i2c_algorithm gb_i2c_algorithm = {
+	.master_xfer	= gb_i2c_master_xfer,
+	.functionality	= gb_i2c_functionality,
+};
+
+static int gb_i2c_probe(struct gbphy_device *gbphy_dev,
+			const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	struct gb_i2c_device *gb_i2c_dev;
+	struct i2c_adapter *adapter;
+	int ret;
+
+	gb_i2c_dev = kzalloc(sizeof(*gb_i2c_dev), GFP_KERNEL);
+	if (!gb_i2c_dev)
+		return -ENOMEM;
+
+	connection =
+		gb_connection_create(gbphy_dev->bundle,
+				     le16_to_cpu(gbphy_dev->cport_desc->id),
+				     NULL);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto exit_i2cdev_free;
+	}
+
+	gb_i2c_dev->connection = connection;
+	gb_connection_set_data(connection, gb_i2c_dev);
+	gb_i2c_dev->gbphy_dev = gbphy_dev;
+	gb_gbphy_set_data(gbphy_dev, gb_i2c_dev);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto exit_connection_destroy;
+
+	ret = gb_i2c_device_setup(gb_i2c_dev);
+	if (ret)
+		goto exit_connection_disable;
+
+	/* Looks good; up our i2c adapter */
+	adapter = &gb_i2c_dev->adapter;
+	adapter->owner = THIS_MODULE;
+	adapter->class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
+	adapter->algo = &gb_i2c_algorithm;
+
+	adapter->dev.parent = &gbphy_dev->dev;
+	snprintf(adapter->name, sizeof(adapter->name), "Greybus i2c adapter");
+	i2c_set_adapdata(adapter, gb_i2c_dev);
+
+	ret = i2c_add_adapter(adapter);
+	if (ret)
+		goto exit_connection_disable;
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+	return 0;
+
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+exit_i2cdev_free:
+	kfree(gb_i2c_dev);
+
+	return ret;
+}
+
+static void gb_i2c_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_i2c_device *gb_i2c_dev = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = gb_i2c_dev->connection;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	i2c_del_adapter(&gb_i2c_dev->adapter);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+	kfree(gb_i2c_dev);
+}
+
+static const struct gbphy_device_id gb_i2c_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_I2C) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_i2c_id_table);
+
+static struct gbphy_driver i2c_driver = {
+	.name		= "i2c",
+	.probe		= gb_i2c_probe,
+	.remove		= gb_i2c_remove,
+	.id_table	= gb_i2c_id_table,
+};
+
+module_gbphy_driver(i2c_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/light.c b/drivers/staging/greybus/light.c
new file mode 100644
index 000000000..87d36948c
--- /dev/null
+++ b/drivers/staging/greybus/light.c
@@ -0,0 +1,1357 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Lights protocol driver.
+ *
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/led-class-flash.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/greybus.h>
+#include <media/v4l2-flash-led-class.h>
+
+#define NAMES_MAX	32
+
+struct gb_channel {
+	u8				id;
+	u32				flags;
+	u32				color;
+	char				*color_name;
+	u8				fade_in;
+	u8				fade_out;
+	u32				mode;
+	char				*mode_name;
+	struct attribute		**attrs;
+	struct attribute_group		*attr_group;
+	const struct attribute_group	**attr_groups;
+	struct led_classdev		*led;
+#if IS_REACHABLE(CONFIG_LEDS_CLASS_FLASH)
+	struct led_classdev_flash	fled;
+	struct led_flash_setting	intensity_uA;
+	struct led_flash_setting	timeout_us;
+#else
+	struct led_classdev		cled;
+#endif
+	struct gb_light			*light;
+	bool				is_registered;
+	bool				releasing;
+	bool				strobe_state;
+	bool				active;
+	struct mutex			lock;
+};
+
+struct gb_light {
+	u8			id;
+	char			*name;
+	struct gb_lights	*glights;
+	u32			flags;
+	u8			channels_count;
+	struct gb_channel	*channels;
+	bool			has_flash;
+	bool			ready;
+#if IS_REACHABLE(CONFIG_V4L2_FLASH_LED_CLASS)
+	struct v4l2_flash	*v4l2_flash;
+	struct v4l2_flash	*v4l2_flash_ind;
+#endif
+};
+
+struct gb_lights {
+	struct gb_connection	*connection;
+	u8			lights_count;
+	struct gb_light		*lights;
+	struct mutex		lights_lock;
+};
+
+static void gb_lights_channel_free(struct gb_channel *channel);
+
+static struct gb_connection *get_conn_from_channel(struct gb_channel *channel)
+{
+	return channel->light->glights->connection;
+}
+
+static struct gb_connection *get_conn_from_light(struct gb_light *light)
+{
+	return light->glights->connection;
+}
+
+static bool is_channel_flash(struct gb_channel *channel)
+{
+	return !!(channel->mode & (GB_CHANNEL_MODE_FLASH | GB_CHANNEL_MODE_TORCH
+				   | GB_CHANNEL_MODE_INDICATOR));
+}
+
+#if IS_REACHABLE(CONFIG_LEDS_CLASS_FLASH)
+static struct gb_channel *get_channel_from_cdev(struct led_classdev *cdev)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(cdev);
+
+	return container_of(fled_cdev, struct gb_channel, fled);
+}
+
+static struct led_classdev *get_channel_cdev(struct gb_channel *channel)
+{
+	return &channel->fled.led_cdev;
+}
+
+static struct gb_channel *get_channel_from_mode(struct gb_light *light,
+						u32 mode)
+{
+	struct gb_channel *channel = NULL;
+	int i;
+
+	for (i = 0; i < light->channels_count; i++) {
+		channel = &light->channels[i];
+		if (channel && channel->mode == mode)
+			break;
+	}
+	return channel;
+}
+
+static int __gb_lights_flash_intensity_set(struct gb_channel *channel,
+					   u32 intensity)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_set_flash_intensity_request req;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.intensity_uA = cpu_to_le32(intensity);
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_FLASH_INTENSITY,
+				&req, sizeof(req), NULL, 0);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int __gb_lights_flash_brightness_set(struct gb_channel *channel)
+{
+	u32 intensity;
+
+	/* If the channel is flash we need to get the attached torch channel */
+	if (channel->mode & GB_CHANNEL_MODE_FLASH)
+		channel = get_channel_from_mode(channel->light,
+						GB_CHANNEL_MODE_TORCH);
+
+	/* For not flash we need to convert brightness to intensity */
+	intensity = channel->intensity_uA.min +
+			(channel->intensity_uA.step * channel->led->brightness);
+
+	return __gb_lights_flash_intensity_set(channel, intensity);
+}
+#else
+static struct gb_channel *get_channel_from_cdev(struct led_classdev *cdev)
+{
+	return container_of(cdev, struct gb_channel, cled);
+}
+
+static struct led_classdev *get_channel_cdev(struct gb_channel *channel)
+{
+	return &channel->cled;
+}
+
+static int __gb_lights_flash_brightness_set(struct gb_channel *channel)
+{
+	return 0;
+}
+#endif
+
+static int gb_lights_color_set(struct gb_channel *channel, u32 color);
+static int gb_lights_fade_set(struct gb_channel *channel);
+
+static void led_lock(struct led_classdev *cdev)
+{
+	mutex_lock(&cdev->led_access);
+}
+
+static void led_unlock(struct led_classdev *cdev)
+{
+	mutex_unlock(&cdev->led_access);
+}
+
+#define gb_lights_fade_attr(__dir)					\
+static ssize_t fade_##__dir##_show(struct device *dev,			\
+				   struct device_attribute *attr,	\
+				   char *buf)				\
+{									\
+	struct led_classdev *cdev = dev_get_drvdata(dev);		\
+	struct gb_channel *channel = get_channel_from_cdev(cdev);	\
+									\
+	return sprintf(buf, "%u\n", channel->fade_##__dir);		\
+}									\
+									\
+static ssize_t fade_##__dir##_store(struct device *dev,			\
+				    struct device_attribute *attr,	\
+				    const char *buf, size_t size)	\
+{									\
+	struct led_classdev *cdev = dev_get_drvdata(dev);		\
+	struct gb_channel *channel = get_channel_from_cdev(cdev);	\
+	u8 fade;							\
+	int ret;							\
+									\
+	led_lock(cdev);							\
+	if (led_sysfs_is_disabled(cdev)) {				\
+		ret = -EBUSY;						\
+		goto unlock;						\
+	}								\
+									\
+	ret = kstrtou8(buf, 0, &fade);					\
+	if (ret < 0) {							\
+		dev_err(dev, "could not parse fade value %d\n", ret);	\
+		goto unlock;						\
+	}								\
+	if (channel->fade_##__dir == fade)				\
+		goto unlock;						\
+	channel->fade_##__dir = fade;					\
+									\
+	ret = gb_lights_fade_set(channel);				\
+	if (ret < 0)							\
+		goto unlock;						\
+									\
+	ret = size;							\
+unlock:									\
+	led_unlock(cdev);						\
+	return ret;							\
+}									\
+static DEVICE_ATTR_RW(fade_##__dir)
+
+gb_lights_fade_attr(in);
+gb_lights_fade_attr(out);
+
+static ssize_t color_show(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	struct led_classdev *cdev = dev_get_drvdata(dev);
+	struct gb_channel *channel = get_channel_from_cdev(cdev);
+
+	return sprintf(buf, "0x%08x\n", channel->color);
+}
+
+static ssize_t color_store(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t size)
+{
+	struct led_classdev *cdev = dev_get_drvdata(dev);
+	struct gb_channel *channel = get_channel_from_cdev(cdev);
+	u32 color;
+	int ret;
+
+	led_lock(cdev);
+	if (led_sysfs_is_disabled(cdev)) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+	ret = kstrtou32(buf, 0, &color);
+	if (ret < 0) {
+		dev_err(dev, "could not parse color value %d\n", ret);
+		goto unlock;
+	}
+
+	ret = gb_lights_color_set(channel, color);
+	if (ret < 0)
+		goto unlock;
+
+	channel->color = color;
+	ret = size;
+unlock:
+	led_unlock(cdev);
+	return ret;
+}
+static DEVICE_ATTR_RW(color);
+
+static int channel_attr_groups_set(struct gb_channel *channel,
+				   struct led_classdev *cdev)
+{
+	int attr = 0;
+	int size = 0;
+
+	if (channel->flags & GB_LIGHT_CHANNEL_MULTICOLOR)
+		size++;
+	if (channel->flags & GB_LIGHT_CHANNEL_FADER)
+		size += 2;
+
+	if (!size)
+		return 0;
+
+	/* Set attributes based in the channel flags */
+	channel->attrs = kcalloc(size + 1, sizeof(*channel->attrs), GFP_KERNEL);
+	if (!channel->attrs)
+		return -ENOMEM;
+	channel->attr_group = kzalloc(sizeof(*channel->attr_group), GFP_KERNEL);
+	if (!channel->attr_group)
+		return -ENOMEM;
+	channel->attr_groups = kcalloc(2, sizeof(*channel->attr_groups),
+				       GFP_KERNEL);
+	if (!channel->attr_groups)
+		return -ENOMEM;
+
+	if (channel->flags & GB_LIGHT_CHANNEL_MULTICOLOR)
+		channel->attrs[attr++] = &dev_attr_color.attr;
+	if (channel->flags & GB_LIGHT_CHANNEL_FADER) {
+		channel->attrs[attr++] = &dev_attr_fade_in.attr;
+		channel->attrs[attr++] = &dev_attr_fade_out.attr;
+	}
+
+	channel->attr_group->attrs = channel->attrs;
+
+	channel->attr_groups[0] = channel->attr_group;
+
+	cdev->groups = channel->attr_groups;
+
+	return 0;
+}
+
+static int gb_lights_fade_set(struct gb_channel *channel)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_set_fade_request req;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.fade_in = channel->fade_in;
+	req.fade_out = channel->fade_out;
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_FADE,
+				&req, sizeof(req), NULL, 0);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int gb_lights_color_set(struct gb_channel *channel, u32 color)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_set_color_request req;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.color = cpu_to_le32(color);
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_COLOR,
+				&req, sizeof(req), NULL, 0);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int __gb_lights_led_brightness_set(struct gb_channel *channel)
+{
+	struct gb_lights_set_brightness_request req;
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	bool old_active;
+	int ret;
+
+	mutex_lock(&channel->lock);
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		goto out_unlock;
+
+	old_active = channel->active;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.brightness = (u8)channel->led->brightness;
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_BRIGHTNESS,
+				&req, sizeof(req), NULL, 0);
+	if (ret < 0)
+		goto out_pm_put;
+
+	if (channel->led->brightness)
+		channel->active = true;
+	else
+		channel->active = false;
+
+	/* we need to keep module alive when turning to active state */
+	if (!old_active && channel->active)
+		goto out_unlock;
+
+	/*
+	 * on the other hand if going to inactive we still hold a reference and
+	 * need to put it, so we could go to suspend.
+	 */
+	if (old_active && !channel->active)
+		gb_pm_runtime_put_autosuspend(bundle);
+
+out_pm_put:
+	gb_pm_runtime_put_autosuspend(bundle);
+out_unlock:
+	mutex_unlock(&channel->lock);
+
+	return ret;
+}
+
+static int __gb_lights_brightness_set(struct gb_channel *channel)
+{
+	int ret;
+
+	if (channel->releasing)
+		return 0;
+
+	if (is_channel_flash(channel))
+		ret = __gb_lights_flash_brightness_set(channel);
+	else
+		ret = __gb_lights_led_brightness_set(channel);
+
+	return ret;
+}
+
+static int gb_brightness_set(struct led_classdev *cdev,
+			     enum led_brightness value)
+{
+	struct gb_channel *channel = get_channel_from_cdev(cdev);
+
+	channel->led->brightness = value;
+
+	return __gb_lights_brightness_set(channel);
+}
+
+static enum led_brightness gb_brightness_get(struct led_classdev *cdev)
+
+{
+	struct gb_channel *channel = get_channel_from_cdev(cdev);
+
+	return channel->led->brightness;
+}
+
+static int gb_blink_set(struct led_classdev *cdev, unsigned long *delay_on,
+			unsigned long *delay_off)
+{
+	struct gb_channel *channel = get_channel_from_cdev(cdev);
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_blink_request req;
+	bool old_active;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	if (!delay_on || !delay_off)
+		return -EINVAL;
+
+	mutex_lock(&channel->lock);
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		goto out_unlock;
+
+	old_active = channel->active;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.time_on_ms = cpu_to_le16(*delay_on);
+	req.time_off_ms = cpu_to_le16(*delay_off);
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_BLINK, &req,
+				sizeof(req), NULL, 0);
+	if (ret < 0)
+		goto out_pm_put;
+
+	if (*delay_on)
+		channel->active = true;
+	else
+		channel->active = false;
+
+	/* we need to keep module alive when turning to active state */
+	if (!old_active && channel->active)
+		goto out_unlock;
+
+	/*
+	 * on the other hand if going to inactive we still hold a reference and
+	 * need to put it, so we could go to suspend.
+	 */
+	if (old_active && !channel->active)
+		gb_pm_runtime_put_autosuspend(bundle);
+
+out_pm_put:
+	gb_pm_runtime_put_autosuspend(bundle);
+out_unlock:
+	mutex_unlock(&channel->lock);
+
+	return ret;
+}
+
+static void gb_lights_led_operations_set(struct gb_channel *channel,
+					 struct led_classdev *cdev)
+{
+	cdev->brightness_get = gb_brightness_get;
+	cdev->brightness_set_blocking = gb_brightness_set;
+
+	if (channel->flags & GB_LIGHT_CHANNEL_BLINK)
+		cdev->blink_set = gb_blink_set;
+}
+
+#if IS_REACHABLE(CONFIG_V4L2_FLASH_LED_CLASS)
+/* V4L2 specific helpers */
+static const struct v4l2_flash_ops v4l2_flash_ops;
+
+static void __gb_lights_channel_v4l2_config(struct led_flash_setting *channel_s,
+					    struct led_flash_setting *v4l2_s)
+{
+	v4l2_s->min = channel_s->min;
+	v4l2_s->max = channel_s->max;
+	v4l2_s->step = channel_s->step;
+	/* For v4l2 val is the default value */
+	v4l2_s->val = channel_s->max;
+}
+
+static int gb_lights_light_v4l2_register(struct gb_light *light)
+{
+	struct gb_connection *connection = get_conn_from_light(light);
+	struct device *dev = &connection->bundle->dev;
+	struct v4l2_flash_config sd_cfg = { {0} }, sd_cfg_ind = { {0} };
+	struct led_classdev_flash *fled;
+	struct led_classdev *iled = NULL;
+	struct gb_channel *channel_torch, *channel_ind, *channel_flash;
+
+	channel_torch = get_channel_from_mode(light, GB_CHANNEL_MODE_TORCH);
+	if (channel_torch)
+		__gb_lights_channel_v4l2_config(&channel_torch->intensity_uA,
+						&sd_cfg.intensity);
+
+	channel_ind = get_channel_from_mode(light, GB_CHANNEL_MODE_INDICATOR);
+	if (channel_ind) {
+		__gb_lights_channel_v4l2_config(&channel_ind->intensity_uA,
+						&sd_cfg_ind.intensity);
+		iled = &channel_ind->fled.led_cdev;
+	}
+
+	channel_flash = get_channel_from_mode(light, GB_CHANNEL_MODE_FLASH);
+	WARN_ON(!channel_flash);
+
+	fled = &channel_flash->fled;
+
+	snprintf(sd_cfg.dev_name, sizeof(sd_cfg.dev_name), "%s", light->name);
+	snprintf(sd_cfg_ind.dev_name, sizeof(sd_cfg_ind.dev_name),
+		 "%s indicator", light->name);
+
+	/* Set the possible values to faults, in our case all faults */
+	sd_cfg.flash_faults = LED_FAULT_OVER_VOLTAGE | LED_FAULT_TIMEOUT |
+		LED_FAULT_OVER_TEMPERATURE | LED_FAULT_SHORT_CIRCUIT |
+		LED_FAULT_OVER_CURRENT | LED_FAULT_INDICATOR |
+		LED_FAULT_UNDER_VOLTAGE | LED_FAULT_INPUT_VOLTAGE |
+		LED_FAULT_LED_OVER_TEMPERATURE;
+
+	light->v4l2_flash = v4l2_flash_init(dev, NULL, fled, &v4l2_flash_ops,
+					    &sd_cfg);
+	if (IS_ERR(light->v4l2_flash))
+		return PTR_ERR(light->v4l2_flash);
+
+	if (channel_ind) {
+		light->v4l2_flash_ind =
+			v4l2_flash_indicator_init(dev, NULL, iled, &sd_cfg_ind);
+		if (IS_ERR(light->v4l2_flash_ind)) {
+			v4l2_flash_release(light->v4l2_flash);
+			return PTR_ERR(light->v4l2_flash_ind);
+		}
+	}
+
+	return 0;
+}
+
+static void gb_lights_light_v4l2_unregister(struct gb_light *light)
+{
+	v4l2_flash_release(light->v4l2_flash_ind);
+	v4l2_flash_release(light->v4l2_flash);
+}
+#else
+static int gb_lights_light_v4l2_register(struct gb_light *light)
+{
+	struct gb_connection *connection = get_conn_from_light(light);
+
+	dev_err(&connection->bundle->dev, "no support for v4l2 subdevices\n");
+	return 0;
+}
+
+static void gb_lights_light_v4l2_unregister(struct gb_light *light)
+{
+}
+#endif
+
+#if IS_REACHABLE(CONFIG_LEDS_CLASS_FLASH)
+/* Flash specific operations */
+static int gb_lights_flash_intensity_set(struct led_classdev_flash *fcdev,
+					 u32 brightness)
+{
+	struct gb_channel *channel = container_of(fcdev, struct gb_channel,
+						  fled);
+	int ret;
+
+	ret = __gb_lights_flash_intensity_set(channel, brightness);
+	if (ret < 0)
+		return ret;
+
+	fcdev->brightness.val = brightness;
+
+	return 0;
+}
+
+static int gb_lights_flash_intensity_get(struct led_classdev_flash *fcdev,
+					 u32 *brightness)
+{
+	*brightness = fcdev->brightness.val;
+
+	return 0;
+}
+
+static int gb_lights_flash_strobe_set(struct led_classdev_flash *fcdev,
+				      bool state)
+{
+	struct gb_channel *channel = container_of(fcdev, struct gb_channel,
+						  fled);
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_set_flash_strobe_request req;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.state = state ? 1 : 0;
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_FLASH_STROBE,
+				&req, sizeof(req), NULL, 0);
+	if (!ret)
+		channel->strobe_state = state;
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int gb_lights_flash_strobe_get(struct led_classdev_flash *fcdev,
+				      bool *state)
+{
+	struct gb_channel *channel = container_of(fcdev, struct gb_channel,
+						  fled);
+
+	*state = channel->strobe_state;
+	return 0;
+}
+
+static int gb_lights_flash_timeout_set(struct led_classdev_flash *fcdev,
+				       u32 timeout)
+{
+	struct gb_channel *channel = container_of(fcdev, struct gb_channel,
+						  fled);
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_set_flash_timeout_request req;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+	req.timeout_us = cpu_to_le32(timeout);
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_SET_FLASH_TIMEOUT,
+				&req, sizeof(req), NULL, 0);
+	if (!ret)
+		fcdev->timeout.val = timeout;
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int gb_lights_flash_fault_get(struct led_classdev_flash *fcdev,
+				     u32 *fault)
+{
+	struct gb_channel *channel = container_of(fcdev, struct gb_channel,
+						  fled);
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_bundle *bundle = connection->bundle;
+	struct gb_lights_get_flash_fault_request req;
+	struct gb_lights_get_flash_fault_response resp;
+	int ret;
+
+	if (channel->releasing)
+		return -ESHUTDOWN;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret < 0)
+		return ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_GET_FLASH_FAULT,
+				&req, sizeof(req), &resp, sizeof(resp));
+	if (!ret)
+		*fault = le32_to_cpu(resp.fault);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static const struct led_flash_ops gb_lights_flash_ops = {
+	.flash_brightness_set	= gb_lights_flash_intensity_set,
+	.flash_brightness_get	= gb_lights_flash_intensity_get,
+	.strobe_set		= gb_lights_flash_strobe_set,
+	.strobe_get		= gb_lights_flash_strobe_get,
+	.timeout_set		= gb_lights_flash_timeout_set,
+	.fault_get		= gb_lights_flash_fault_get,
+};
+
+static int __gb_lights_channel_torch_attach(struct gb_channel *channel,
+					    struct gb_channel *channel_torch)
+{
+	char *name;
+
+	/* we can only attach torch to a flash channel */
+	if (!(channel->mode & GB_CHANNEL_MODE_FLASH))
+		return 0;
+
+	/* Move torch brightness to the destination */
+	channel->led->max_brightness = channel_torch->led->max_brightness;
+
+	/* append mode name to flash name */
+	name = kasprintf(GFP_KERNEL, "%s_%s", channel->led->name,
+			 channel_torch->mode_name);
+	if (!name)
+		return -ENOMEM;
+	kfree(channel->led->name);
+	channel->led->name = name;
+
+	channel_torch->led = channel->led;
+
+	return 0;
+}
+
+static int __gb_lights_flash_led_register(struct gb_channel *channel)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct led_classdev_flash *fled = &channel->fled;
+	struct led_flash_setting *fset;
+	struct gb_channel *channel_torch;
+	int ret;
+
+	fled->ops = &gb_lights_flash_ops;
+
+	fled->led_cdev.flags |= LED_DEV_CAP_FLASH;
+
+	fset = &fled->brightness;
+	fset->min = channel->intensity_uA.min;
+	fset->max = channel->intensity_uA.max;
+	fset->step = channel->intensity_uA.step;
+	fset->val = channel->intensity_uA.max;
+
+	/* Only the flash mode have the timeout constraints settings */
+	if (channel->mode & GB_CHANNEL_MODE_FLASH) {
+		fset = &fled->timeout;
+		fset->min = channel->timeout_us.min;
+		fset->max = channel->timeout_us.max;
+		fset->step = channel->timeout_us.step;
+		fset->val = channel->timeout_us.max;
+	}
+
+	/*
+	 * If light have torch mode channel, this channel will be the led
+	 * classdev of the registered above flash classdev
+	 */
+	channel_torch = get_channel_from_mode(channel->light,
+					      GB_CHANNEL_MODE_TORCH);
+	if (channel_torch) {
+		ret = __gb_lights_channel_torch_attach(channel, channel_torch);
+		if (ret < 0)
+			goto fail;
+	}
+
+	ret = led_classdev_flash_register(&connection->bundle->dev, fled);
+	if (ret < 0)
+		goto fail;
+
+	channel->is_registered = true;
+	return 0;
+fail:
+	channel->led = NULL;
+	return ret;
+}
+
+static void __gb_lights_flash_led_unregister(struct gb_channel *channel)
+{
+	if (!channel->is_registered)
+		return;
+
+	led_classdev_flash_unregister(&channel->fled);
+}
+
+static int gb_lights_channel_flash_config(struct gb_channel *channel)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct gb_lights_get_channel_flash_config_request req;
+	struct gb_lights_get_channel_flash_config_response conf;
+	struct led_flash_setting *fset;
+	int ret;
+
+	req.light_id = channel->light->id;
+	req.channel_id = channel->id;
+
+	ret = gb_operation_sync(connection,
+				GB_LIGHTS_TYPE_GET_CHANNEL_FLASH_CONFIG,
+				&req, sizeof(req), &conf, sizeof(conf));
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Intensity constraints for flash related modes: flash, torch,
+	 * indicator.  They will be needed for v4l2 registration.
+	 */
+	fset = &channel->intensity_uA;
+	fset->min = le32_to_cpu(conf.intensity_min_uA);
+	fset->max = le32_to_cpu(conf.intensity_max_uA);
+	fset->step = le32_to_cpu(conf.intensity_step_uA);
+
+	/*
+	 * On flash type, max brightness is set as the number of intensity steps
+	 * available.
+	 */
+	channel->led->max_brightness = (fset->max - fset->min) / fset->step;
+
+	/* Only the flash mode have the timeout constraints settings */
+	if (channel->mode & GB_CHANNEL_MODE_FLASH) {
+		fset = &channel->timeout_us;
+		fset->min = le32_to_cpu(conf.timeout_min_us);
+		fset->max = le32_to_cpu(conf.timeout_max_us);
+		fset->step = le32_to_cpu(conf.timeout_step_us);
+	}
+
+	return 0;
+}
+#else
+static int gb_lights_channel_flash_config(struct gb_channel *channel)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+
+	dev_err(&connection->bundle->dev, "no support for flash devices\n");
+	return 0;
+}
+
+static int __gb_lights_flash_led_register(struct gb_channel *channel)
+{
+	return 0;
+}
+
+static void __gb_lights_flash_led_unregister(struct gb_channel *channel)
+{
+}
+
+#endif
+
+static int __gb_lights_led_register(struct gb_channel *channel)
+{
+	struct gb_connection *connection = get_conn_from_channel(channel);
+	struct led_classdev *cdev = get_channel_cdev(channel);
+	int ret;
+
+	ret = led_classdev_register(&connection->bundle->dev, cdev);
+	if (ret < 0)
+		channel->led = NULL;
+	else
+		channel->is_registered = true;
+	return ret;
+}
+
+static int gb_lights_channel_register(struct gb_channel *channel)
+{
+	/* Normal LED channel, just register in led classdev and we are done */
+	if (!is_channel_flash(channel))
+		return __gb_lights_led_register(channel);
+
+	/*
+	 * Flash Type need more work, register flash classdev, indicator as
+	 * flash classdev, torch will be led classdev of the flash classdev.
+	 */
+	if (!(channel->mode & GB_CHANNEL_MODE_TORCH))
+		return __gb_lights_flash_led_register(channel);
+
+	return 0;
+}
+
+static void __gb_lights_led_unregister(struct gb_channel *channel)
+{
+	struct led_classdev *cdev = get_channel_cdev(channel);
+
+	if (!channel->is_registered)
+		return;
+
+	led_classdev_unregister(cdev);
+	kfree(cdev->name);
+	cdev->name = NULL;
+	channel->led = NULL;
+}
+
+static void gb_lights_channel_unregister(struct gb_channel *channel)
+{
+	/* The same as register, handle channels differently */
+	if (!is_channel_flash(channel)) {
+		__gb_lights_led_unregister(channel);
+		return;
+	}
+
+	if (channel->mode & GB_CHANNEL_MODE_TORCH)
+		__gb_lights_led_unregister(channel);
+	else
+		__gb_lights_flash_led_unregister(channel);
+}
+
+static int gb_lights_channel_config(struct gb_light *light,
+				    struct gb_channel *channel)
+{
+	struct gb_lights_get_channel_config_response conf;
+	struct gb_lights_get_channel_config_request req;
+	struct gb_connection *connection = get_conn_from_light(light);
+	struct led_classdev *cdev = get_channel_cdev(channel);
+	char *name;
+	int ret;
+
+	req.light_id = light->id;
+	req.channel_id = channel->id;
+
+	ret = gb_operation_sync(connection, GB_LIGHTS_TYPE_GET_CHANNEL_CONFIG,
+				&req, sizeof(req), &conf, sizeof(conf));
+	if (ret < 0)
+		return ret;
+
+	channel->light = light;
+	channel->mode = le32_to_cpu(conf.mode);
+	channel->flags = le32_to_cpu(conf.flags);
+	channel->color = le32_to_cpu(conf.color);
+	channel->color_name = kstrndup(conf.color_name, NAMES_MAX, GFP_KERNEL);
+	if (!channel->color_name)
+		return -ENOMEM;
+	channel->mode_name = kstrndup(conf.mode_name, NAMES_MAX, GFP_KERNEL);
+	if (!channel->mode_name)
+		return -ENOMEM;
+
+	channel->led = cdev;
+
+	name = kasprintf(GFP_KERNEL, "%s:%s:%s", light->name,
+			 channel->color_name, channel->mode_name);
+	if (!name)
+		return -ENOMEM;
+
+	cdev->name = name;
+
+	cdev->max_brightness = conf.max_brightness;
+
+	ret = channel_attr_groups_set(channel, cdev);
+	if (ret < 0)
+		return ret;
+
+	gb_lights_led_operations_set(channel, cdev);
+
+	/*
+	 * If it is not a flash related channel (flash, torch or indicator) we
+	 * are done here. If not, continue and fetch flash related
+	 * configurations.
+	 */
+	if (!is_channel_flash(channel))
+		return ret;
+
+	light->has_flash = true;
+
+	return gb_lights_channel_flash_config(channel);
+}
+
+static int gb_lights_light_config(struct gb_lights *glights, u8 id)
+{
+	struct gb_light *light = &glights->lights[id];
+	struct gb_lights_get_light_config_request req;
+	struct gb_lights_get_light_config_response conf;
+	int ret;
+	int i;
+
+	light->glights = glights;
+	light->id = id;
+
+	req.id = id;
+
+	ret = gb_operation_sync(glights->connection,
+				GB_LIGHTS_TYPE_GET_LIGHT_CONFIG,
+				&req, sizeof(req), &conf, sizeof(conf));
+	if (ret < 0)
+		return ret;
+
+	if (!conf.channel_count)
+		return -EINVAL;
+	if (!strlen(conf.name))
+		return -EINVAL;
+
+	light->channels_count = conf.channel_count;
+	light->name = kstrndup(conf.name, NAMES_MAX, GFP_KERNEL);
+	if (!light->name)
+		return -ENOMEM;
+	light->channels = kcalloc(light->channels_count,
+				  sizeof(struct gb_channel), GFP_KERNEL);
+	if (!light->channels)
+		return -ENOMEM;
+
+	/* First we collect all the configurations for all channels */
+	for (i = 0; i < light->channels_count; i++) {
+		light->channels[i].id = i;
+		ret = gb_lights_channel_config(light, &light->channels[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int gb_lights_light_register(struct gb_light *light)
+{
+	int ret;
+	int i;
+
+	/*
+	 * Then, if everything went ok in getting configurations, we register
+	 * the classdev, flash classdev and v4l2 subsystem, if a flash device is
+	 * found.
+	 */
+	for (i = 0; i < light->channels_count; i++) {
+		ret = gb_lights_channel_register(&light->channels[i]);
+		if (ret < 0)
+			return ret;
+
+		mutex_init(&light->channels[i].lock);
+	}
+
+	light->ready = true;
+
+	if (light->has_flash) {
+		ret = gb_lights_light_v4l2_register(light);
+		if (ret < 0) {
+			light->has_flash = false;
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void gb_lights_channel_free(struct gb_channel *channel)
+{
+	kfree(channel->attrs);
+	kfree(channel->attr_group);
+	kfree(channel->attr_groups);
+	kfree(channel->color_name);
+	kfree(channel->mode_name);
+	mutex_destroy(&channel->lock);
+}
+
+static void gb_lights_channel_release(struct gb_channel *channel)
+{
+	channel->releasing = true;
+
+	gb_lights_channel_unregister(channel);
+
+	gb_lights_channel_free(channel);
+}
+
+static void gb_lights_light_release(struct gb_light *light)
+{
+	int i;
+
+	light->ready = false;
+
+	if (light->has_flash)
+		gb_lights_light_v4l2_unregister(light);
+	light->has_flash = false;
+
+	for (i = 0; i < light->channels_count; i++)
+		gb_lights_channel_release(&light->channels[i]);
+	light->channels_count = 0;
+
+	kfree(light->channels);
+	light->channels = NULL;
+	kfree(light->name);
+	light->name = NULL;
+}
+
+static void gb_lights_release(struct gb_lights *glights)
+{
+	int i;
+
+	if (!glights)
+		return;
+
+	mutex_lock(&glights->lights_lock);
+	if (!glights->lights)
+		goto free_glights;
+
+	for (i = 0; i < glights->lights_count; i++)
+		gb_lights_light_release(&glights->lights[i]);
+
+	kfree(glights->lights);
+
+free_glights:
+	mutex_unlock(&glights->lights_lock);
+	mutex_destroy(&glights->lights_lock);
+	kfree(glights);
+}
+
+static int gb_lights_get_count(struct gb_lights *glights)
+{
+	struct gb_lights_get_lights_response resp;
+	int ret;
+
+	ret = gb_operation_sync(glights->connection, GB_LIGHTS_TYPE_GET_LIGHTS,
+				NULL, 0, &resp, sizeof(resp));
+	if (ret < 0)
+		return ret;
+
+	if (!resp.lights_count)
+		return -EINVAL;
+
+	glights->lights_count = resp.lights_count;
+
+	return 0;
+}
+
+static int gb_lights_create_all(struct gb_lights *glights)
+{
+	struct gb_connection *connection = glights->connection;
+	int ret;
+	int i;
+
+	mutex_lock(&glights->lights_lock);
+	ret = gb_lights_get_count(glights);
+	if (ret < 0)
+		goto out;
+
+	glights->lights = kcalloc(glights->lights_count,
+				  sizeof(struct gb_light), GFP_KERNEL);
+	if (!glights->lights) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	for (i = 0; i < glights->lights_count; i++) {
+		ret = gb_lights_light_config(glights, i);
+		if (ret < 0) {
+			dev_err(&connection->bundle->dev,
+				"Fail to configure lights device\n");
+			goto out;
+		}
+	}
+
+out:
+	mutex_unlock(&glights->lights_lock);
+	return ret;
+}
+
+static int gb_lights_register_all(struct gb_lights *glights)
+{
+	struct gb_connection *connection = glights->connection;
+	int ret = 0;
+	int i;
+
+	mutex_lock(&glights->lights_lock);
+	for (i = 0; i < glights->lights_count; i++) {
+		ret = gb_lights_light_register(&glights->lights[i]);
+		if (ret < 0) {
+			dev_err(&connection->bundle->dev,
+				"Fail to enable lights device\n");
+			break;
+		}
+	}
+
+	mutex_unlock(&glights->lights_lock);
+	return ret;
+}
+
+static int gb_lights_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct device *dev = &connection->bundle->dev;
+	struct gb_lights *glights = gb_connection_get_data(connection);
+	struct gb_light *light;
+	struct gb_message *request;
+	struct gb_lights_event_request *payload;
+	int ret =  0;
+	u8 light_id;
+	u8 event;
+
+	if (op->type != GB_LIGHTS_TYPE_EVENT) {
+		dev_err(dev, "Unsupported unsolicited event: %u\n", op->type);
+		return -EINVAL;
+	}
+
+	request = op->request;
+
+	if (request->payload_size < sizeof(*payload)) {
+		dev_err(dev, "Wrong event size received (%zu < %zu)\n",
+			request->payload_size, sizeof(*payload));
+		return -EINVAL;
+	}
+
+	payload = request->payload;
+	light_id = payload->light_id;
+
+	if (light_id >= glights->lights_count ||
+	    !glights->lights[light_id].ready) {
+		dev_err(dev, "Event received for unconfigured light id: %d\n",
+			light_id);
+		return -EINVAL;
+	}
+
+	event = payload->event;
+
+	if (event & GB_LIGHTS_LIGHT_CONFIG) {
+		light = &glights->lights[light_id];
+
+		mutex_lock(&glights->lights_lock);
+		gb_lights_light_release(light);
+		ret = gb_lights_light_config(glights, light_id);
+		if (!ret)
+			ret = gb_lights_light_register(light);
+		if (ret < 0)
+			gb_lights_light_release(light);
+		mutex_unlock(&glights->lights_lock);
+	}
+
+	return ret;
+}
+
+static int gb_lights_probe(struct gb_bundle *bundle,
+			   const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_lights *glights;
+	int ret;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_LIGHTS)
+		return -ENODEV;
+
+	glights = kzalloc(sizeof(*glights), GFP_KERNEL);
+	if (!glights)
+		return -ENOMEM;
+
+	mutex_init(&glights->lights_lock);
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_lights_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto out;
+	}
+
+	glights->connection = connection;
+	gb_connection_set_data(connection, glights);
+
+	greybus_set_drvdata(bundle, glights);
+
+	/* We aren't ready to receive an incoming request yet */
+	ret = gb_connection_enable_tx(connection);
+	if (ret)
+		goto error_connection_destroy;
+
+	/*
+	 * Setup all the lights devices over this connection, if anything goes
+	 * wrong tear down all lights
+	 */
+	ret = gb_lights_create_all(glights);
+	if (ret < 0)
+		goto error_connection_disable;
+
+	/* We are ready to receive an incoming request now, enable RX as well */
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto error_connection_disable;
+
+	/* Enable & register lights */
+	ret = gb_lights_register_all(glights);
+	if (ret < 0)
+		goto error_connection_disable;
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+error_connection_disable:
+	gb_connection_disable(connection);
+error_connection_destroy:
+	gb_connection_destroy(connection);
+out:
+	gb_lights_release(glights);
+	return ret;
+}
+
+static void gb_lights_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_lights *glights = greybus_get_drvdata(bundle);
+
+	if (gb_pm_runtime_get_sync(bundle))
+		gb_pm_runtime_get_noresume(bundle);
+
+	gb_connection_disable(glights->connection);
+	gb_connection_destroy(glights->connection);
+
+	gb_lights_release(glights);
+}
+
+static const struct greybus_bundle_id gb_lights_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_LIGHTS) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_lights_id_table);
+
+static struct greybus_driver gb_lights_driver = {
+	.name		= "lights",
+	.probe		= gb_lights_probe,
+	.disconnect	= gb_lights_disconnect,
+	.id_table	= gb_lights_id_table,
+};
+module_greybus_driver(gb_lights_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/log.c b/drivers/staging/greybus/log.c
new file mode 100644
index 000000000..971f36dcc
--- /dev/null
+++ b/drivers/staging/greybus/log.c
@@ -0,0 +1,132 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus driver for the log protocol
+ *
+ * Copyright 2016 Google Inc.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sizes.h>
+#include <linux/uaccess.h>
+#include <linux/greybus.h>
+
+struct gb_log {
+	struct gb_connection *connection;
+};
+
+static int gb_log_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct device *dev = &connection->bundle->dev;
+	struct gb_log_send_log_request *receive;
+	u16 len;
+
+	if (op->type != GB_LOG_TYPE_SEND_LOG) {
+		dev_err(dev, "unknown request type 0x%02x\n", op->type);
+		return -EINVAL;
+	}
+
+	/* Verify size of payload */
+	if (op->request->payload_size < sizeof(*receive)) {
+		dev_err(dev, "log request too small (%zu < %zu)\n",
+			op->request->payload_size, sizeof(*receive));
+		return -EINVAL;
+	}
+	receive = op->request->payload;
+	len = le16_to_cpu(receive->len);
+	if (len != (op->request->payload_size - sizeof(*receive))) {
+		dev_err(dev, "log request wrong size %d vs %zu\n", len,
+			(op->request->payload_size - sizeof(*receive)));
+		return -EINVAL;
+	}
+	if (len == 0) {
+		dev_err(dev, "log request of 0 bytes?\n");
+		return -EINVAL;
+	}
+
+	if (len > GB_LOG_MAX_LEN) {
+		dev_err(dev, "log request too big: %d\n", len);
+		return -EINVAL;
+	}
+
+	/* Ensure the buffer is 0 terminated */
+	receive->msg[len - 1] = '\0';
+
+	/*
+	 * Print with dev_dbg() so that it can be easily turned off using
+	 * dynamic debugging (and prevent any DoS)
+	 */
+	dev_dbg(dev, "%s", receive->msg);
+
+	return 0;
+}
+
+static int gb_log_probe(struct gb_bundle *bundle,
+			const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_log *log;
+	int retval;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_LOG)
+		return -ENODEV;
+
+	log = kzalloc(sizeof(*log), GFP_KERNEL);
+	if (!log)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_log_request_handler);
+	if (IS_ERR(connection)) {
+		retval = PTR_ERR(connection);
+		goto error_free;
+	}
+
+	log->connection = connection;
+	greybus_set_drvdata(bundle, log);
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto error_connection_destroy;
+
+	return 0;
+
+error_connection_destroy:
+	gb_connection_destroy(connection);
+error_free:
+	kfree(log);
+	return retval;
+}
+
+static void gb_log_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_log *log = greybus_get_drvdata(bundle);
+	struct gb_connection *connection = log->connection;
+
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+
+	kfree(log);
+}
+
+static const struct greybus_bundle_id gb_log_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_LOG) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_log_id_table);
+
+static struct greybus_driver gb_log_driver = {
+	.name           = "log",
+	.probe          = gb_log_probe,
+	.disconnect     = gb_log_disconnect,
+	.id_table       = gb_log_id_table,
+};
+module_greybus_driver(gb_log_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/loopback.c b/drivers/staging/greybus/loopback.c
new file mode 100644
index 000000000..1a61fce98
--- /dev/null
+++ b/drivers/staging/greybus/loopback.c
@@ -0,0 +1,1178 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Loopback bridge driver for the Greybus loopback module.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/kthread.h>
+#include <linux/delay.h>
+#include <linux/random.h>
+#include <linux/sizes.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/kfifo.h>
+#include <linux/debugfs.h>
+#include <linux/list_sort.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/atomic.h>
+#include <linux/pm_runtime.h>
+#include <linux/greybus.h>
+#include <asm/div64.h>
+
+#define NSEC_PER_DAY 86400000000000ULL
+
+struct gb_loopback_stats {
+	u32 min;
+	u32 max;
+	u64 sum;
+	u32 count;
+};
+
+struct gb_loopback_device {
+	struct dentry *root;
+	u32 count;
+	size_t size_max;
+
+	/* We need to take a lock in atomic context */
+	spinlock_t lock;
+	wait_queue_head_t wq;
+};
+
+static struct gb_loopback_device gb_dev;
+
+struct gb_loopback_async_operation {
+	struct gb_loopback *gb;
+	struct gb_operation *operation;
+	ktime_t ts;
+	int (*completion)(struct gb_loopback_async_operation *op_async);
+};
+
+struct gb_loopback {
+	struct gb_connection *connection;
+
+	struct dentry *file;
+	struct kfifo kfifo_lat;
+	struct mutex mutex;
+	struct task_struct *task;
+	struct device *dev;
+	wait_queue_head_t wq;
+	wait_queue_head_t wq_completion;
+	atomic_t outstanding_operations;
+
+	/* Per connection stats */
+	ktime_t ts;
+	struct gb_loopback_stats latency;
+	struct gb_loopback_stats throughput;
+	struct gb_loopback_stats requests_per_second;
+	struct gb_loopback_stats apbridge_unipro_latency;
+	struct gb_loopback_stats gbphy_firmware_latency;
+
+	int type;
+	int async;
+	int id;
+	u32 size;
+	u32 iteration_max;
+	u32 iteration_count;
+	int us_wait;
+	u32 error;
+	u32 requests_completed;
+	u32 requests_timedout;
+	u32 timeout;
+	u32 jiffy_timeout;
+	u32 timeout_min;
+	u32 timeout_max;
+	u32 outstanding_operations_max;
+	u64 elapsed_nsecs;
+	u32 apbridge_latency_ts;
+	u32 gbphy_latency_ts;
+
+	u32 send_count;
+};
+
+static struct class loopback_class = {
+	.name		= "gb_loopback",
+	.owner		= THIS_MODULE,
+};
+static DEFINE_IDA(loopback_ida);
+
+/* Min/max values in jiffies */
+#define GB_LOOPBACK_TIMEOUT_MIN				1
+#define GB_LOOPBACK_TIMEOUT_MAX				10000
+
+#define GB_LOOPBACK_FIFO_DEFAULT			8192
+
+static unsigned int kfifo_depth = GB_LOOPBACK_FIFO_DEFAULT;
+module_param(kfifo_depth, uint, 0444);
+
+/* Maximum size of any one send data buffer we support */
+#define MAX_PACKET_SIZE (PAGE_SIZE * 2)
+
+#define GB_LOOPBACK_US_WAIT_MAX				1000000
+
+/* interface sysfs attributes */
+#define gb_loopback_ro_attr(field)				\
+static ssize_t field##_show(struct device *dev,			\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	return sprintf(buf, "%u\n", gb->field);			\
+}									\
+static DEVICE_ATTR_RO(field)
+
+#define gb_loopback_ro_stats_attr(name, field, type)		\
+static ssize_t name##_##field##_show(struct device *dev,	\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	/* Report 0 for min and max if no transfer succeeded */		\
+	if (!gb->requests_completed)					\
+		return sprintf(buf, "0\n");				\
+	return sprintf(buf, "%" #type "\n", gb->name.field);		\
+}									\
+static DEVICE_ATTR_RO(name##_##field)
+
+#define gb_loopback_ro_avg_attr(name)			\
+static ssize_t name##_avg_show(struct device *dev,		\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback_stats *stats;				\
+	struct gb_loopback *gb;						\
+	u64 avg, rem;							\
+	u32 count;							\
+	gb = dev_get_drvdata(dev);			\
+	stats = &gb->name;					\
+	count = stats->count ? stats->count : 1;			\
+	avg = stats->sum + count / 2000000; /* round closest */		\
+	rem = do_div(avg, count);					\
+	rem *= 1000000;							\
+	do_div(rem, count);						\
+	return sprintf(buf, "%llu.%06u\n", avg, (u32)rem);		\
+}									\
+static DEVICE_ATTR_RO(name##_avg)
+
+#define gb_loopback_stats_attrs(field)				\
+	gb_loopback_ro_stats_attr(field, min, u);		\
+	gb_loopback_ro_stats_attr(field, max, u);		\
+	gb_loopback_ro_avg_attr(field)
+
+#define gb_loopback_attr(field, type)					\
+static ssize_t field##_show(struct device *dev,				\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	return sprintf(buf, "%" #type "\n", gb->field);			\
+}									\
+static ssize_t field##_store(struct device *dev,			\
+			    struct device_attribute *attr,		\
+			    const char *buf,				\
+			    size_t len)					\
+{									\
+	int ret;							\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	mutex_lock(&gb->mutex);						\
+	ret = sscanf(buf, "%"#type, &gb->field);			\
+	if (ret != 1)							\
+		len = -EINVAL;						\
+	else								\
+		gb_loopback_check_attr(gb, bundle);			\
+	mutex_unlock(&gb->mutex);					\
+	return len;							\
+}									\
+static DEVICE_ATTR_RW(field)
+
+#define gb_dev_loopback_ro_attr(field, conn)				\
+static ssize_t field##_show(struct device *dev,		\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	return sprintf(buf, "%u\n", gb->field);				\
+}									\
+static DEVICE_ATTR_RO(field)
+
+#define gb_dev_loopback_rw_attr(field, type)				\
+static ssize_t field##_show(struct device *dev,				\
+			    struct device_attribute *attr,		\
+			    char *buf)					\
+{									\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	return sprintf(buf, "%" #type "\n", gb->field);			\
+}									\
+static ssize_t field##_store(struct device *dev,			\
+			    struct device_attribute *attr,		\
+			    const char *buf,				\
+			    size_t len)					\
+{									\
+	int ret;							\
+	struct gb_loopback *gb = dev_get_drvdata(dev);			\
+	mutex_lock(&gb->mutex);						\
+	ret = sscanf(buf, "%"#type, &gb->field);			\
+	if (ret != 1)							\
+		len = -EINVAL;						\
+	else								\
+		gb_loopback_check_attr(gb);		\
+	mutex_unlock(&gb->mutex);					\
+	return len;							\
+}									\
+static DEVICE_ATTR_RW(field)
+
+static void gb_loopback_reset_stats(struct gb_loopback *gb);
+static void gb_loopback_check_attr(struct gb_loopback *gb)
+{
+	if (gb->us_wait > GB_LOOPBACK_US_WAIT_MAX)
+		gb->us_wait = GB_LOOPBACK_US_WAIT_MAX;
+	if (gb->size > gb_dev.size_max)
+		gb->size = gb_dev.size_max;
+	gb->requests_timedout = 0;
+	gb->requests_completed = 0;
+	gb->iteration_count = 0;
+	gb->send_count = 0;
+	gb->error = 0;
+
+	if (kfifo_depth < gb->iteration_max) {
+		dev_warn(gb->dev,
+			 "cannot log bytes %u kfifo_depth %u\n",
+			 gb->iteration_max, kfifo_depth);
+	}
+	kfifo_reset_out(&gb->kfifo_lat);
+
+	switch (gb->type) {
+	case GB_LOOPBACK_TYPE_PING:
+	case GB_LOOPBACK_TYPE_TRANSFER:
+	case GB_LOOPBACK_TYPE_SINK:
+		gb->jiffy_timeout = usecs_to_jiffies(gb->timeout);
+		if (!gb->jiffy_timeout)
+			gb->jiffy_timeout = GB_LOOPBACK_TIMEOUT_MIN;
+		else if (gb->jiffy_timeout > GB_LOOPBACK_TIMEOUT_MAX)
+			gb->jiffy_timeout = GB_LOOPBACK_TIMEOUT_MAX;
+		gb_loopback_reset_stats(gb);
+		wake_up(&gb->wq);
+		break;
+	default:
+		gb->type = 0;
+		break;
+	}
+}
+
+/* Time to send and receive one message */
+gb_loopback_stats_attrs(latency);
+/* Number of requests sent per second on this cport */
+gb_loopback_stats_attrs(requests_per_second);
+/* Quantity of data sent and received on this cport */
+gb_loopback_stats_attrs(throughput);
+/* Latency across the UniPro link from APBridge's perspective */
+gb_loopback_stats_attrs(apbridge_unipro_latency);
+/* Firmware induced overhead in the GPBridge */
+gb_loopback_stats_attrs(gbphy_firmware_latency);
+
+/* Number of errors encountered during loop */
+gb_loopback_ro_attr(error);
+/* Number of requests successfully completed async */
+gb_loopback_ro_attr(requests_completed);
+/* Number of requests timed out async */
+gb_loopback_ro_attr(requests_timedout);
+/* Timeout minimum in useconds */
+gb_loopback_ro_attr(timeout_min);
+/* Timeout minimum in useconds */
+gb_loopback_ro_attr(timeout_max);
+
+/*
+ * Type of loopback message to send based on protocol type definitions
+ * 0 => Don't send message
+ * 2 => Send ping message continuously (message without payload)
+ * 3 => Send transfer message continuously (message with payload,
+ *					   payload returned in response)
+ * 4 => Send a sink message (message with payload, no payload in response)
+ */
+gb_dev_loopback_rw_attr(type, d);
+/* Size of transfer message payload: 0-4096 bytes */
+gb_dev_loopback_rw_attr(size, u);
+/* Time to wait between two messages: 0-1000 ms */
+gb_dev_loopback_rw_attr(us_wait, d);
+/* Maximum iterations for a given operation: 1-(2^32-1), 0 implies infinite */
+gb_dev_loopback_rw_attr(iteration_max, u);
+/* The current index of the for (i = 0; i < iteration_max; i++) loop */
+gb_dev_loopback_ro_attr(iteration_count, false);
+/* A flag to indicate synchronous or asynchronous operations */
+gb_dev_loopback_rw_attr(async, u);
+/* Timeout of an individual asynchronous request */
+gb_dev_loopback_rw_attr(timeout, u);
+/* Maximum number of in-flight operations before back-off */
+gb_dev_loopback_rw_attr(outstanding_operations_max, u);
+
+static struct attribute *loopback_attrs[] = {
+	&dev_attr_latency_min.attr,
+	&dev_attr_latency_max.attr,
+	&dev_attr_latency_avg.attr,
+	&dev_attr_requests_per_second_min.attr,
+	&dev_attr_requests_per_second_max.attr,
+	&dev_attr_requests_per_second_avg.attr,
+	&dev_attr_throughput_min.attr,
+	&dev_attr_throughput_max.attr,
+	&dev_attr_throughput_avg.attr,
+	&dev_attr_apbridge_unipro_latency_min.attr,
+	&dev_attr_apbridge_unipro_latency_max.attr,
+	&dev_attr_apbridge_unipro_latency_avg.attr,
+	&dev_attr_gbphy_firmware_latency_min.attr,
+	&dev_attr_gbphy_firmware_latency_max.attr,
+	&dev_attr_gbphy_firmware_latency_avg.attr,
+	&dev_attr_type.attr,
+	&dev_attr_size.attr,
+	&dev_attr_us_wait.attr,
+	&dev_attr_iteration_count.attr,
+	&dev_attr_iteration_max.attr,
+	&dev_attr_async.attr,
+	&dev_attr_error.attr,
+	&dev_attr_requests_completed.attr,
+	&dev_attr_requests_timedout.attr,
+	&dev_attr_timeout.attr,
+	&dev_attr_outstanding_operations_max.attr,
+	&dev_attr_timeout_min.attr,
+	&dev_attr_timeout_max.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(loopback);
+
+static void gb_loopback_calculate_stats(struct gb_loopback *gb, bool error);
+
+static u32 gb_loopback_nsec_to_usec_latency(u64 elapsed_nsecs)
+{
+	do_div(elapsed_nsecs, NSEC_PER_USEC);
+	return elapsed_nsecs;
+}
+
+static u64 __gb_loopback_calc_latency(u64 t1, u64 t2)
+{
+	if (t2 > t1)
+		return t2 - t1;
+	else
+		return NSEC_PER_DAY - t2 + t1;
+}
+
+static u64 gb_loopback_calc_latency(ktime_t ts, ktime_t te)
+{
+	return __gb_loopback_calc_latency(ktime_to_ns(ts), ktime_to_ns(te));
+}
+
+static int gb_loopback_operation_sync(struct gb_loopback *gb, int type,
+				      void *request, int request_size,
+				      void *response, int response_size)
+{
+	struct gb_operation *operation;
+	ktime_t ts, te;
+	int ret;
+
+	ts = ktime_get();
+	operation = gb_operation_create(gb->connection, type, request_size,
+					response_size, GFP_KERNEL);
+	if (!operation)
+		return -ENOMEM;
+
+	if (request_size)
+		memcpy(operation->request->payload, request, request_size);
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret) {
+		dev_err(&gb->connection->bundle->dev,
+			"synchronous operation failed: %d\n", ret);
+		goto out_put_operation;
+	} else {
+		if (response_size == operation->response->payload_size) {
+			memcpy(response, operation->response->payload,
+			       response_size);
+		} else {
+			dev_err(&gb->connection->bundle->dev,
+				"response size %zu expected %d\n",
+				operation->response->payload_size,
+				response_size);
+			ret = -EINVAL;
+			goto out_put_operation;
+		}
+	}
+
+	te = ktime_get();
+
+	/* Calculate the total time the message took */
+	gb->elapsed_nsecs = gb_loopback_calc_latency(ts, te);
+
+out_put_operation:
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static void gb_loopback_async_wait_all(struct gb_loopback *gb)
+{
+	wait_event(gb->wq_completion,
+		   !atomic_read(&gb->outstanding_operations));
+}
+
+static void gb_loopback_async_operation_callback(struct gb_operation *operation)
+{
+	struct gb_loopback_async_operation *op_async;
+	struct gb_loopback *gb;
+	ktime_t te;
+	int result;
+
+	te = ktime_get();
+	result = gb_operation_result(operation);
+	op_async = gb_operation_get_data(operation);
+	gb = op_async->gb;
+
+	mutex_lock(&gb->mutex);
+
+	if (!result && op_async->completion)
+		result = op_async->completion(op_async);
+
+	if (!result) {
+		gb->elapsed_nsecs = gb_loopback_calc_latency(op_async->ts, te);
+	} else {
+		gb->error++;
+		if (result == -ETIMEDOUT)
+			gb->requests_timedout++;
+	}
+
+	gb->iteration_count++;
+	gb_loopback_calculate_stats(gb, result);
+
+	mutex_unlock(&gb->mutex);
+
+	dev_dbg(&gb->connection->bundle->dev, "complete operation %d\n",
+		operation->id);
+
+	/* Wake up waiters */
+	atomic_dec(&op_async->gb->outstanding_operations);
+	wake_up(&gb->wq_completion);
+
+	/* Release resources */
+	gb_operation_put(operation);
+	kfree(op_async);
+}
+
+static int gb_loopback_async_operation(struct gb_loopback *gb, int type,
+				       void *request, int request_size,
+				       int response_size,
+				       void *completion)
+{
+	struct gb_loopback_async_operation *op_async;
+	struct gb_operation *operation;
+	int ret;
+
+	op_async = kzalloc(sizeof(*op_async), GFP_KERNEL);
+	if (!op_async)
+		return -ENOMEM;
+
+	operation = gb_operation_create(gb->connection, type, request_size,
+					response_size, GFP_KERNEL);
+	if (!operation) {
+		kfree(op_async);
+		return -ENOMEM;
+	}
+
+	if (request_size)
+		memcpy(operation->request->payload, request, request_size);
+
+	gb_operation_set_data(operation, op_async);
+
+	op_async->gb = gb;
+	op_async->operation = operation;
+	op_async->completion = completion;
+
+	op_async->ts = ktime_get();
+
+	atomic_inc(&gb->outstanding_operations);
+	ret = gb_operation_request_send(operation,
+					gb_loopback_async_operation_callback,
+					jiffies_to_msecs(gb->jiffy_timeout),
+					GFP_KERNEL);
+	if (ret) {
+		atomic_dec(&gb->outstanding_operations);
+		gb_operation_put(operation);
+		kfree(op_async);
+	}
+	return ret;
+}
+
+static int gb_loopback_sync_sink(struct gb_loopback *gb, u32 len)
+{
+	struct gb_loopback_transfer_request *request;
+	int retval;
+
+	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+
+	request->len = cpu_to_le32(len);
+	retval = gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_SINK,
+					    request, len + sizeof(*request),
+					    NULL, 0);
+	kfree(request);
+	return retval;
+}
+
+static int gb_loopback_sync_transfer(struct gb_loopback *gb, u32 len)
+{
+	struct gb_loopback_transfer_request *request;
+	struct gb_loopback_transfer_response *response;
+	int retval;
+
+	gb->apbridge_latency_ts = 0;
+	gb->gbphy_latency_ts = 0;
+
+	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+	response = kmalloc(len + sizeof(*response), GFP_KERNEL);
+	if (!response) {
+		kfree(request);
+		return -ENOMEM;
+	}
+
+	memset(request->data, 0x5A, len);
+
+	request->len = cpu_to_le32(len);
+	retval = gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_TRANSFER,
+					    request, len + sizeof(*request),
+					    response, len + sizeof(*response));
+	if (retval)
+		goto gb_error;
+
+	if (memcmp(request->data, response->data, len)) {
+		dev_err(&gb->connection->bundle->dev,
+			"Loopback Data doesn't match\n");
+		retval = -EREMOTEIO;
+	}
+	gb->apbridge_latency_ts = (u32)__le32_to_cpu(response->reserved0);
+	gb->gbphy_latency_ts = (u32)__le32_to_cpu(response->reserved1);
+
+gb_error:
+	kfree(request);
+	kfree(response);
+
+	return retval;
+}
+
+static int gb_loopback_sync_ping(struct gb_loopback *gb)
+{
+	return gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_PING,
+					  NULL, 0, NULL, 0);
+}
+
+static int gb_loopback_async_sink(struct gb_loopback *gb, u32 len)
+{
+	struct gb_loopback_transfer_request *request;
+	int retval;
+
+	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+
+	request->len = cpu_to_le32(len);
+	retval = gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_SINK,
+					     request, len + sizeof(*request),
+					     0, NULL);
+	kfree(request);
+	return retval;
+}
+
+static int gb_loopback_async_transfer_complete(
+				struct gb_loopback_async_operation *op_async)
+{
+	struct gb_loopback *gb;
+	struct gb_operation *operation;
+	struct gb_loopback_transfer_request *request;
+	struct gb_loopback_transfer_response *response;
+	size_t len;
+	int retval = 0;
+
+	gb = op_async->gb;
+	operation = op_async->operation;
+	request = operation->request->payload;
+	response = operation->response->payload;
+	len = le32_to_cpu(request->len);
+
+	if (memcmp(request->data, response->data, len)) {
+		dev_err(&gb->connection->bundle->dev,
+			"Loopback Data doesn't match operation id %d\n",
+			operation->id);
+		retval = -EREMOTEIO;
+	} else {
+		gb->apbridge_latency_ts =
+			(u32)__le32_to_cpu(response->reserved0);
+		gb->gbphy_latency_ts =
+			(u32)__le32_to_cpu(response->reserved1);
+	}
+
+	return retval;
+}
+
+static int gb_loopback_async_transfer(struct gb_loopback *gb, u32 len)
+{
+	struct gb_loopback_transfer_request *request;
+	int retval, response_len;
+
+	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+
+	memset(request->data, 0x5A, len);
+
+	request->len = cpu_to_le32(len);
+	response_len = sizeof(struct gb_loopback_transfer_response);
+	retval = gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_TRANSFER,
+					     request, len + sizeof(*request),
+					     len + response_len,
+					     gb_loopback_async_transfer_complete);
+	if (retval)
+		goto gb_error;
+
+gb_error:
+	kfree(request);
+	return retval;
+}
+
+static int gb_loopback_async_ping(struct gb_loopback *gb)
+{
+	return gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_PING,
+					   NULL, 0, 0, NULL);
+}
+
+static int gb_loopback_request_handler(struct gb_operation *operation)
+{
+	struct gb_connection *connection = operation->connection;
+	struct gb_loopback_transfer_request *request;
+	struct gb_loopback_transfer_response *response;
+	struct device *dev = &connection->bundle->dev;
+	size_t len;
+
+	/* By convention, the AP initiates the version operation */
+	switch (operation->type) {
+	case GB_LOOPBACK_TYPE_PING:
+	case GB_LOOPBACK_TYPE_SINK:
+		return 0;
+	case GB_LOOPBACK_TYPE_TRANSFER:
+		if (operation->request->payload_size < sizeof(*request)) {
+			dev_err(dev, "transfer request too small (%zu < %zu)\n",
+				operation->request->payload_size,
+				sizeof(*request));
+			return -EINVAL;	/* -EMSGSIZE */
+		}
+		request = operation->request->payload;
+		len = le32_to_cpu(request->len);
+		if (len > gb_dev.size_max) {
+			dev_err(dev, "transfer request too large (%zu > %zu)\n",
+				len, gb_dev.size_max);
+			return -EINVAL;
+		}
+
+		if (!gb_operation_response_alloc(operation,
+				len + sizeof(*response), GFP_KERNEL)) {
+			dev_err(dev, "error allocating response\n");
+			return -ENOMEM;
+		}
+		response = operation->response->payload;
+		response->len = cpu_to_le32(len);
+		if (len)
+			memcpy(response->data, request->data, len);
+
+		return 0;
+	default:
+		dev_err(dev, "unsupported request: %u\n", operation->type);
+		return -EINVAL;
+	}
+}
+
+static void gb_loopback_reset_stats(struct gb_loopback *gb)
+{
+	struct gb_loopback_stats reset = {
+		.min = U32_MAX,
+	};
+
+	/* Reset per-connection stats */
+	memcpy(&gb->latency, &reset,
+	       sizeof(struct gb_loopback_stats));
+	memcpy(&gb->throughput, &reset,
+	       sizeof(struct gb_loopback_stats));
+	memcpy(&gb->requests_per_second, &reset,
+	       sizeof(struct gb_loopback_stats));
+	memcpy(&gb->apbridge_unipro_latency, &reset,
+	       sizeof(struct gb_loopback_stats));
+	memcpy(&gb->gbphy_firmware_latency, &reset,
+	       sizeof(struct gb_loopback_stats));
+
+	/* Should be initialized at least once per transaction set */
+	gb->apbridge_latency_ts = 0;
+	gb->gbphy_latency_ts = 0;
+	gb->ts = ktime_set(0, 0);
+}
+
+static void gb_loopback_update_stats(struct gb_loopback_stats *stats, u32 val)
+{
+	if (stats->min > val)
+		stats->min = val;
+	if (stats->max < val)
+		stats->max = val;
+	stats->sum += val;
+	stats->count++;
+}
+
+static void gb_loopback_update_stats_window(struct gb_loopback_stats *stats,
+					    u64 val, u32 count)
+{
+	stats->sum += val;
+	stats->count += count;
+
+	do_div(val, count);
+	if (stats->min > val)
+		stats->min = val;
+	if (stats->max < val)
+		stats->max = val;
+}
+
+static void gb_loopback_requests_update(struct gb_loopback *gb, u32 latency)
+{
+	u64 req = gb->requests_completed * USEC_PER_SEC;
+
+	gb_loopback_update_stats_window(&gb->requests_per_second, req, latency);
+}
+
+static void gb_loopback_throughput_update(struct gb_loopback *gb, u32 latency)
+{
+	u64 aggregate_size = sizeof(struct gb_operation_msg_hdr) * 2;
+
+	switch (gb->type) {
+	case GB_LOOPBACK_TYPE_PING:
+		break;
+	case GB_LOOPBACK_TYPE_SINK:
+		aggregate_size += sizeof(struct gb_loopback_transfer_request) +
+				  gb->size;
+		break;
+	case GB_LOOPBACK_TYPE_TRANSFER:
+		aggregate_size += sizeof(struct gb_loopback_transfer_request) +
+				  sizeof(struct gb_loopback_transfer_response) +
+				  gb->size * 2;
+		break;
+	default:
+		return;
+	}
+
+	aggregate_size *= gb->requests_completed;
+	aggregate_size *= USEC_PER_SEC;
+	gb_loopback_update_stats_window(&gb->throughput, aggregate_size,
+					latency);
+}
+
+static void gb_loopback_calculate_latency_stats(struct gb_loopback *gb)
+{
+	u32 lat;
+
+	/* Express latency in terms of microseconds */
+	lat = gb_loopback_nsec_to_usec_latency(gb->elapsed_nsecs);
+
+	/* Log latency stastic */
+	gb_loopback_update_stats(&gb->latency, lat);
+
+	/* Raw latency log on a per thread basis */
+	kfifo_in(&gb->kfifo_lat, (unsigned char *)&lat, sizeof(lat));
+
+	/* Log the firmware supplied latency values */
+	gb_loopback_update_stats(&gb->apbridge_unipro_latency,
+				 gb->apbridge_latency_ts);
+	gb_loopback_update_stats(&gb->gbphy_firmware_latency,
+				 gb->gbphy_latency_ts);
+}
+
+static void gb_loopback_calculate_stats(struct gb_loopback *gb, bool error)
+{
+	u64 nlat;
+	u32 lat;
+	ktime_t te;
+
+	if (!error) {
+		gb->requests_completed++;
+		gb_loopback_calculate_latency_stats(gb);
+	}
+
+	te = ktime_get();
+	nlat = gb_loopback_calc_latency(gb->ts, te);
+	if (nlat >= NSEC_PER_SEC || gb->iteration_count == gb->iteration_max) {
+		lat = gb_loopback_nsec_to_usec_latency(nlat);
+
+		gb_loopback_throughput_update(gb, lat);
+		gb_loopback_requests_update(gb, lat);
+
+		if (gb->iteration_count != gb->iteration_max) {
+			gb->ts = te;
+			gb->requests_completed = 0;
+		}
+	}
+}
+
+static void gb_loopback_async_wait_to_send(struct gb_loopback *gb)
+{
+	if (!(gb->async && gb->outstanding_operations_max))
+		return;
+	wait_event_interruptible(gb->wq_completion,
+				 (atomic_read(&gb->outstanding_operations) <
+				  gb->outstanding_operations_max) ||
+				  kthread_should_stop());
+}
+
+static int gb_loopback_fn(void *data)
+{
+	int error = 0;
+	int us_wait = 0;
+	int type;
+	int ret;
+	u32 size;
+
+	struct gb_loopback *gb = data;
+	struct gb_bundle *bundle = gb->connection->bundle;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	while (1) {
+		if (!gb->type) {
+			gb_pm_runtime_put_autosuspend(bundle);
+			wait_event_interruptible(gb->wq, gb->type ||
+						 kthread_should_stop());
+			ret = gb_pm_runtime_get_sync(bundle);
+			if (ret)
+				return ret;
+		}
+
+		if (kthread_should_stop())
+			break;
+
+		/* Limit the maximum number of in-flight async operations */
+		gb_loopback_async_wait_to_send(gb);
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&gb->mutex);
+
+		/* Optionally terminate */
+		if (gb->send_count == gb->iteration_max) {
+			mutex_unlock(&gb->mutex);
+
+			/* Wait for synchronous and asynchronous completion */
+			gb_loopback_async_wait_all(gb);
+
+			/* Mark complete unless user-space has poked us */
+			mutex_lock(&gb->mutex);
+			if (gb->iteration_count == gb->iteration_max) {
+				gb->type = 0;
+				gb->send_count = 0;
+				sysfs_notify(&gb->dev->kobj,  NULL,
+					     "iteration_count");
+				dev_dbg(&bundle->dev, "load test complete\n");
+			} else {
+				dev_dbg(&bundle->dev,
+					"continuing on with new test set\n");
+			}
+			mutex_unlock(&gb->mutex);
+			continue;
+		}
+		size = gb->size;
+		us_wait = gb->us_wait;
+		type = gb->type;
+		if (ktime_to_ns(gb->ts) == 0)
+			gb->ts = ktime_get();
+
+		/* Else operations to perform */
+		if (gb->async) {
+			if (type == GB_LOOPBACK_TYPE_PING)
+				error = gb_loopback_async_ping(gb);
+			else if (type == GB_LOOPBACK_TYPE_TRANSFER)
+				error = gb_loopback_async_transfer(gb, size);
+			else if (type == GB_LOOPBACK_TYPE_SINK)
+				error = gb_loopback_async_sink(gb, size);
+
+			if (error) {
+				gb->error++;
+				gb->iteration_count++;
+			}
+		} else {
+			/* We are effectively single threaded here */
+			if (type == GB_LOOPBACK_TYPE_PING)
+				error = gb_loopback_sync_ping(gb);
+			else if (type == GB_LOOPBACK_TYPE_TRANSFER)
+				error = gb_loopback_sync_transfer(gb, size);
+			else if (type == GB_LOOPBACK_TYPE_SINK)
+				error = gb_loopback_sync_sink(gb, size);
+
+			if (error)
+				gb->error++;
+			gb->iteration_count++;
+			gb_loopback_calculate_stats(gb, !!error);
+		}
+		gb->send_count++;
+		mutex_unlock(&gb->mutex);
+
+		if (us_wait) {
+			if (us_wait < 20000)
+				usleep_range(us_wait, us_wait + 100);
+			else
+				msleep(us_wait / 1000);
+		}
+	}
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+}
+
+static int gb_loopback_dbgfs_latency_show_common(struct seq_file *s,
+						 struct kfifo *kfifo,
+						 struct mutex *mutex)
+{
+	u32 latency;
+	int retval;
+
+	if (kfifo_len(kfifo) == 0) {
+		retval = -EAGAIN;
+		goto done;
+	}
+
+	mutex_lock(mutex);
+	retval = kfifo_out(kfifo, &latency, sizeof(latency));
+	if (retval > 0) {
+		seq_printf(s, "%u", latency);
+		retval = 0;
+	}
+	mutex_unlock(mutex);
+done:
+	return retval;
+}
+
+static int gb_loopback_dbgfs_latency_show(struct seq_file *s, void *unused)
+{
+	struct gb_loopback *gb = s->private;
+
+	return gb_loopback_dbgfs_latency_show_common(s, &gb->kfifo_lat,
+						     &gb->mutex);
+}
+DEFINE_SHOW_ATTRIBUTE(gb_loopback_dbgfs_latency);
+
+#define DEBUGFS_NAMELEN 32
+
+static int gb_loopback_probe(struct gb_bundle *bundle,
+			     const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_loopback *gb;
+	struct device *dev;
+	int retval;
+	char name[DEBUGFS_NAMELEN];
+	unsigned long flags;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_LOOPBACK)
+		return -ENODEV;
+
+	gb = kzalloc(sizeof(*gb), GFP_KERNEL);
+	if (!gb)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_loopback_request_handler);
+	if (IS_ERR(connection)) {
+		retval = PTR_ERR(connection);
+		goto out_kzalloc;
+	}
+
+	gb->connection = connection;
+	greybus_set_drvdata(bundle, gb);
+
+	init_waitqueue_head(&gb->wq);
+	init_waitqueue_head(&gb->wq_completion);
+	atomic_set(&gb->outstanding_operations, 0);
+	gb_loopback_reset_stats(gb);
+
+	/* Reported values to user-space for min/max timeouts */
+	gb->timeout_min = jiffies_to_usecs(GB_LOOPBACK_TIMEOUT_MIN);
+	gb->timeout_max = jiffies_to_usecs(GB_LOOPBACK_TIMEOUT_MAX);
+
+	if (!gb_dev.count) {
+		/* Calculate maximum payload */
+		gb_dev.size_max = gb_operation_get_payload_size_max(connection);
+		if (gb_dev.size_max <=
+			sizeof(struct gb_loopback_transfer_request)) {
+			retval = -EINVAL;
+			goto out_connection_destroy;
+		}
+		gb_dev.size_max -= sizeof(struct gb_loopback_transfer_request);
+	}
+
+	/* Create per-connection sysfs and debugfs data-points */
+	snprintf(name, sizeof(name), "raw_latency_%s",
+		 dev_name(&connection->bundle->dev));
+	gb->file = debugfs_create_file(name, S_IFREG | 0444, gb_dev.root, gb,
+				       &gb_loopback_dbgfs_latency_fops);
+
+	gb->id = ida_simple_get(&loopback_ida, 0, 0, GFP_KERNEL);
+	if (gb->id < 0) {
+		retval = gb->id;
+		goto out_debugfs_remove;
+	}
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto out_ida_remove;
+
+	dev = device_create_with_groups(&loopback_class,
+					&connection->bundle->dev,
+					MKDEV(0, 0), gb, loopback_groups,
+					"gb_loopback%d", gb->id);
+	if (IS_ERR(dev)) {
+		retval = PTR_ERR(dev);
+		goto out_connection_disable;
+	}
+	gb->dev = dev;
+
+	/* Allocate kfifo */
+	if (kfifo_alloc(&gb->kfifo_lat, kfifo_depth * sizeof(u32),
+			GFP_KERNEL)) {
+		retval = -ENOMEM;
+		goto out_conn;
+	}
+	/* Fork worker thread */
+	mutex_init(&gb->mutex);
+	gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback");
+	if (IS_ERR(gb->task)) {
+		retval = PTR_ERR(gb->task);
+		goto out_kfifo;
+	}
+
+	spin_lock_irqsave(&gb_dev.lock, flags);
+	gb_dev.count++;
+	spin_unlock_irqrestore(&gb_dev.lock, flags);
+
+	gb_connection_latency_tag_enable(connection);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+out_kfifo:
+	kfifo_free(&gb->kfifo_lat);
+out_conn:
+	device_unregister(dev);
+out_connection_disable:
+	gb_connection_disable(connection);
+out_ida_remove:
+	ida_simple_remove(&loopback_ida, gb->id);
+out_debugfs_remove:
+	debugfs_remove(gb->file);
+out_connection_destroy:
+	gb_connection_destroy(connection);
+out_kzalloc:
+	kfree(gb);
+
+	return retval;
+}
+
+static void gb_loopback_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_loopback *gb = greybus_get_drvdata(bundle);
+	unsigned long flags;
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		gb_pm_runtime_get_noresume(bundle);
+
+	gb_connection_disable(gb->connection);
+
+	if (!IS_ERR_OR_NULL(gb->task))
+		kthread_stop(gb->task);
+
+	kfifo_free(&gb->kfifo_lat);
+	gb_connection_latency_tag_disable(gb->connection);
+	debugfs_remove(gb->file);
+
+	/*
+	 * FIXME: gb_loopback_async_wait_all() is redundant now, as connection
+	 * is disabled at the beginning and so we can't have any more
+	 * incoming/outgoing requests.
+	 */
+	gb_loopback_async_wait_all(gb);
+
+	spin_lock_irqsave(&gb_dev.lock, flags);
+	gb_dev.count--;
+	spin_unlock_irqrestore(&gb_dev.lock, flags);
+
+	device_unregister(gb->dev);
+	ida_simple_remove(&loopback_ida, gb->id);
+
+	gb_connection_destroy(gb->connection);
+	kfree(gb);
+}
+
+static const struct greybus_bundle_id gb_loopback_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_LOOPBACK) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_loopback_id_table);
+
+static struct greybus_driver gb_loopback_driver = {
+	.name		= "loopback",
+	.probe		= gb_loopback_probe,
+	.disconnect	= gb_loopback_disconnect,
+	.id_table	= gb_loopback_id_table,
+};
+
+static int loopback_init(void)
+{
+	int retval;
+
+	spin_lock_init(&gb_dev.lock);
+	gb_dev.root = debugfs_create_dir("gb_loopback", NULL);
+
+	retval = class_register(&loopback_class);
+	if (retval)
+		goto err;
+
+	retval = greybus_register(&gb_loopback_driver);
+	if (retval)
+		goto err_unregister;
+
+	return 0;
+
+err_unregister:
+	class_unregister(&loopback_class);
+err:
+	debugfs_remove_recursive(gb_dev.root);
+	return retval;
+}
+module_init(loopback_init);
+
+static void __exit loopback_exit(void)
+{
+	debugfs_remove_recursive(gb_dev.root);
+	greybus_deregister(&gb_loopback_driver);
+	class_unregister(&loopback_class);
+	ida_destroy(&loopback_ida);
+}
+module_exit(loopback_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/power_supply.c b/drivers/staging/greybus/power_supply.c
new file mode 100644
index 000000000..75cb170e0
--- /dev/null
+++ b/drivers/staging/greybus/power_supply.c
@@ -0,0 +1,1139 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Power Supply driver for a Greybus module.
+ *
+ * Copyright 2014-2015 Google Inc.
+ * Copyright 2014-2015 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/greybus.h>
+
+#define PROP_MAX 32
+
+struct gb_power_supply_prop {
+	enum power_supply_property	prop;
+	u8				gb_prop;
+	int				val;
+	int				previous_val;
+	bool				is_writeable;
+};
+
+struct gb_power_supply {
+	u8				id;
+	bool				registered;
+	struct power_supply		*psy;
+	struct power_supply_desc	desc;
+	char				name[64];
+	struct gb_power_supplies	*supplies;
+	struct delayed_work		work;
+	char				*manufacturer;
+	char				*model_name;
+	char				*serial_number;
+	u8				type;
+	u8				properties_count;
+	u8				properties_count_str;
+	unsigned long			last_update;
+	u8				cache_invalid;
+	unsigned int			update_interval;
+	bool				changed;
+	struct gb_power_supply_prop	*props;
+	enum power_supply_property	*props_raw;
+	bool				pm_acquired;
+	struct mutex			supply_lock;
+};
+
+struct gb_power_supplies {
+	struct gb_connection	*connection;
+	u8			supplies_count;
+	struct gb_power_supply	*supply;
+	struct mutex		supplies_lock;
+};
+
+#define to_gb_power_supply(x) power_supply_get_drvdata(x)
+
+/*
+ * General power supply properties that could be absent from various reasons,
+ * like kernel versions or vendor specific versions
+ */
+#ifndef POWER_SUPPLY_PROP_VOLTAGE_BOOT
+	#define POWER_SUPPLY_PROP_VOLTAGE_BOOT	-1
+#endif
+#ifndef POWER_SUPPLY_PROP_CURRENT_BOOT
+	#define POWER_SUPPLY_PROP_CURRENT_BOOT	-1
+#endif
+#ifndef POWER_SUPPLY_PROP_CALIBRATE
+	#define POWER_SUPPLY_PROP_CALIBRATE	-1
+#endif
+
+/* cache time in milliseconds, if cache_time is set to 0 cache is disable */
+static unsigned int cache_time = 1000;
+/*
+ * update interval initial and maximum value, between the two will
+ * back-off exponential
+ */
+static unsigned int update_interval_init = 1 * HZ;
+static unsigned int update_interval_max = 30 * HZ;
+
+struct gb_power_supply_changes {
+	enum power_supply_property	prop;
+	u32				tolerance_change;
+	void (*prop_changed)(struct gb_power_supply *gbpsy,
+			     struct gb_power_supply_prop *prop);
+};
+
+static void gb_power_supply_state_change(struct gb_power_supply *gbpsy,
+					 struct gb_power_supply_prop *prop);
+
+static const struct gb_power_supply_changes psy_props_changes[] = {
+	{	.prop			= GB_POWER_SUPPLY_PROP_STATUS,
+		.tolerance_change	= 0,
+		.prop_changed		= gb_power_supply_state_change,
+	},
+	{	.prop			= GB_POWER_SUPPLY_PROP_TEMP,
+		.tolerance_change	= 500,
+		.prop_changed		= NULL,
+	},
+	{	.prop			= GB_POWER_SUPPLY_PROP_ONLINE,
+		.tolerance_change	= 0,
+		.prop_changed		= NULL,
+	},
+};
+
+static int get_psp_from_gb_prop(int gb_prop, enum power_supply_property *psp)
+{
+	int prop;
+
+	switch (gb_prop) {
+	case GB_POWER_SUPPLY_PROP_STATUS:
+		prop = POWER_SUPPLY_PROP_STATUS;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_TYPE:
+		prop = POWER_SUPPLY_PROP_CHARGE_TYPE;
+		break;
+	case GB_POWER_SUPPLY_PROP_HEALTH:
+		prop = POWER_SUPPLY_PROP_HEALTH;
+		break;
+	case GB_POWER_SUPPLY_PROP_PRESENT:
+		prop = POWER_SUPPLY_PROP_PRESENT;
+		break;
+	case GB_POWER_SUPPLY_PROP_ONLINE:
+		prop = POWER_SUPPLY_PROP_ONLINE;
+		break;
+	case GB_POWER_SUPPLY_PROP_AUTHENTIC:
+		prop = POWER_SUPPLY_PROP_AUTHENTIC;
+		break;
+	case GB_POWER_SUPPLY_PROP_TECHNOLOGY:
+		prop = POWER_SUPPLY_PROP_TECHNOLOGY;
+		break;
+	case GB_POWER_SUPPLY_PROP_CYCLE_COUNT:
+		prop = POWER_SUPPLY_PROP_CYCLE_COUNT;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_MAX:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_MIN:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_MIN;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_AVG:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_OCV:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_OCV;
+		break;
+	case GB_POWER_SUPPLY_PROP_VOLTAGE_BOOT:
+		prop = POWER_SUPPLY_PROP_VOLTAGE_BOOT;
+		break;
+	case GB_POWER_SUPPLY_PROP_CURRENT_MAX:
+		prop = POWER_SUPPLY_PROP_CURRENT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_CURRENT_NOW:
+		prop = POWER_SUPPLY_PROP_CURRENT_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_CURRENT_AVG:
+		prop = POWER_SUPPLY_PROP_CURRENT_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_CURRENT_BOOT:
+		prop = POWER_SUPPLY_PROP_CURRENT_BOOT;
+		break;
+	case GB_POWER_SUPPLY_PROP_POWER_NOW:
+		prop = POWER_SUPPLY_PROP_POWER_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_POWER_AVG:
+		prop = POWER_SUPPLY_PROP_POWER_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		prop = POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN:
+		prop = POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_FULL:
+		prop = POWER_SUPPLY_PROP_CHARGE_FULL;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_EMPTY:
+		prop = POWER_SUPPLY_PROP_CHARGE_EMPTY;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_NOW:
+		prop = POWER_SUPPLY_PROP_CHARGE_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_AVG:
+		prop = POWER_SUPPLY_PROP_CHARGE_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_COUNTER:
+		prop = POWER_SUPPLY_PROP_CHARGE_COUNTER;
+		break;
+	case GB_POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		prop = POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT;
+		break;
+	case GB_POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		prop = POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		prop = POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE;
+		break;
+	case GB_POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		prop = POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT:
+		prop = POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT_MAX:
+		prop = POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_INPUT_CURRENT_LIMIT:
+		prop = POWER_SUPPLY_PROP_INPUT_CURRENT_LIMIT;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+		prop = POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_EMPTY_DESIGN:
+		prop = POWER_SUPPLY_PROP_ENERGY_EMPTY_DESIGN;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_FULL:
+		prop = POWER_SUPPLY_PROP_ENERGY_FULL;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_EMPTY:
+		prop = POWER_SUPPLY_PROP_ENERGY_EMPTY;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_NOW:
+		prop = POWER_SUPPLY_PROP_ENERGY_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_ENERGY_AVG:
+		prop = POWER_SUPPLY_PROP_ENERGY_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_CAPACITY:
+		prop = POWER_SUPPLY_PROP_CAPACITY;
+		break;
+	case GB_POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN:
+		prop = POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN;
+		break;
+	case GB_POWER_SUPPLY_PROP_CAPACITY_ALERT_MAX:
+		prop = POWER_SUPPLY_PROP_CAPACITY_ALERT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		prop = POWER_SUPPLY_PROP_CAPACITY_LEVEL;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP:
+		prop = POWER_SUPPLY_PROP_TEMP;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_MAX:
+		prop = POWER_SUPPLY_PROP_TEMP_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_MIN:
+		prop = POWER_SUPPLY_PROP_TEMP_MIN;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+		prop = POWER_SUPPLY_PROP_TEMP_ALERT_MIN;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		prop = POWER_SUPPLY_PROP_TEMP_ALERT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_AMBIENT:
+		prop = POWER_SUPPLY_PROP_TEMP_AMBIENT;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_AMBIENT_ALERT_MIN:
+		prop = POWER_SUPPLY_PROP_TEMP_AMBIENT_ALERT_MIN;
+		break;
+	case GB_POWER_SUPPLY_PROP_TEMP_AMBIENT_ALERT_MAX:
+		prop = POWER_SUPPLY_PROP_TEMP_AMBIENT_ALERT_MAX;
+		break;
+	case GB_POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW:
+		prop = POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG:
+		prop = POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_TIME_TO_FULL_NOW:
+		prop = POWER_SUPPLY_PROP_TIME_TO_FULL_NOW;
+		break;
+	case GB_POWER_SUPPLY_PROP_TIME_TO_FULL_AVG:
+		prop = POWER_SUPPLY_PROP_TIME_TO_FULL_AVG;
+		break;
+	case GB_POWER_SUPPLY_PROP_TYPE:
+		prop = POWER_SUPPLY_PROP_TYPE;
+		break;
+	case GB_POWER_SUPPLY_PROP_SCOPE:
+		prop = POWER_SUPPLY_PROP_SCOPE;
+		break;
+	case GB_POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT:
+		prop = POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT;
+		break;
+	case GB_POWER_SUPPLY_PROP_CALIBRATE:
+		prop = POWER_SUPPLY_PROP_CALIBRATE;
+		break;
+	default:
+		prop = -1;
+		break;
+	}
+
+	if (prop < 0)
+		return prop;
+
+	*psp = (enum power_supply_property)prop;
+
+	return 0;
+}
+
+static struct gb_connection *get_conn_from_psy(struct gb_power_supply *gbpsy)
+{
+	return gbpsy->supplies->connection;
+}
+
+static struct gb_power_supply_prop *get_psy_prop(struct gb_power_supply *gbpsy,
+						 enum power_supply_property psp)
+{
+	int i;
+
+	for (i = 0; i < gbpsy->properties_count; i++)
+		if (gbpsy->props[i].prop == psp)
+			return &gbpsy->props[i];
+	return NULL;
+}
+
+static int is_psy_prop_writeable(struct gb_power_supply *gbpsy,
+				     enum power_supply_property psp)
+{
+	struct gb_power_supply_prop *prop;
+
+	prop = get_psy_prop(gbpsy, psp);
+	if (!prop)
+		return -ENOENT;
+	return prop->is_writeable ? 1 : 0;
+}
+
+static int is_prop_valint(enum power_supply_property psp)
+{
+	return ((psp < POWER_SUPPLY_PROP_MODEL_NAME) ? 1 : 0);
+}
+
+static void next_interval(struct gb_power_supply *gbpsy)
+{
+	if (gbpsy->update_interval == update_interval_max)
+		return;
+
+	/* do some exponential back-off in the update interval */
+	gbpsy->update_interval *= 2;
+	if (gbpsy->update_interval > update_interval_max)
+		gbpsy->update_interval = update_interval_max;
+}
+
+static void __gb_power_supply_changed(struct gb_power_supply *gbpsy)
+{
+	power_supply_changed(gbpsy->psy);
+}
+
+static void gb_power_supply_state_change(struct gb_power_supply *gbpsy,
+					 struct gb_power_supply_prop *prop)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	int ret;
+
+	/*
+	 * Check gbpsy->pm_acquired to make sure only one pair of 'get_sync'
+	 * and 'put_autosuspend' runtime pm call for state property change.
+	 */
+	mutex_lock(&gbpsy->supply_lock);
+
+	if ((prop->val == GB_POWER_SUPPLY_STATUS_CHARGING) &&
+	    !gbpsy->pm_acquired) {
+		ret = gb_pm_runtime_get_sync(connection->bundle);
+		if (ret)
+			dev_err(&connection->bundle->dev,
+				"Fail to set wake lock for charging state\n");
+		else
+			gbpsy->pm_acquired = true;
+	} else {
+		if (gbpsy->pm_acquired) {
+			ret = gb_pm_runtime_put_autosuspend(connection->bundle);
+			if (ret)
+				dev_err(&connection->bundle->dev,
+					"Fail to set wake unlock for none charging\n");
+			else
+				gbpsy->pm_acquired = false;
+		}
+	}
+
+	mutex_unlock(&gbpsy->supply_lock);
+}
+
+static void check_changed(struct gb_power_supply *gbpsy,
+			  struct gb_power_supply_prop *prop)
+{
+	const struct gb_power_supply_changes *psyc;
+	int val = prop->val;
+	int prev_val = prop->previous_val;
+	bool changed = false;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(psy_props_changes); i++) {
+		psyc = &psy_props_changes[i];
+		if (prop->prop == psyc->prop) {
+			if (!psyc->tolerance_change)
+				changed = true;
+			else if (val < prev_val &&
+				 prev_val - val > psyc->tolerance_change)
+				changed = true;
+			else if (val > prev_val &&
+				 val - prev_val > psyc->tolerance_change)
+				changed = true;
+
+			if (changed && psyc->prop_changed)
+				psyc->prop_changed(gbpsy, prop);
+
+			if (changed)
+				gbpsy->changed = true;
+			break;
+		}
+	}
+}
+
+static int total_props(struct gb_power_supply *gbpsy)
+{
+	/* this return the intval plus the strval properties */
+	return (gbpsy->properties_count + gbpsy->properties_count_str);
+}
+
+static void prop_append(struct gb_power_supply *gbpsy,
+			enum power_supply_property prop)
+{
+	enum power_supply_property *new_props_raw;
+
+	gbpsy->properties_count_str++;
+	new_props_raw = krealloc(gbpsy->props_raw, total_props(gbpsy) *
+				 sizeof(enum power_supply_property),
+				 GFP_KERNEL);
+	if (!new_props_raw)
+		return;
+	gbpsy->props_raw = new_props_raw;
+	gbpsy->props_raw[total_props(gbpsy) - 1] = prop;
+}
+
+static int __gb_power_supply_set_name(char *init_name, char *name, size_t len)
+{
+	unsigned int i = 0;
+	int ret = 0;
+	struct power_supply *psy;
+
+	if (!strlen(init_name))
+		init_name = "gb_power_supply";
+	strscpy(name, init_name, len);
+
+	while ((ret < len) && (psy = power_supply_get_by_name(name))) {
+		power_supply_put(psy);
+
+		ret = snprintf(name, len, "%s_%u", init_name, ++i);
+	}
+	if (ret >= len)
+		return -ENOMEM;
+	return i;
+}
+
+static void _gb_power_supply_append_props(struct gb_power_supply *gbpsy)
+{
+	if (strlen(gbpsy->manufacturer))
+		prop_append(gbpsy, POWER_SUPPLY_PROP_MANUFACTURER);
+	if (strlen(gbpsy->model_name))
+		prop_append(gbpsy, POWER_SUPPLY_PROP_MODEL_NAME);
+	if (strlen(gbpsy->serial_number))
+		prop_append(gbpsy, POWER_SUPPLY_PROP_SERIAL_NUMBER);
+}
+
+static int gb_power_supply_description_get(struct gb_power_supply *gbpsy)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	struct gb_power_supply_get_description_request req;
+	struct gb_power_supply_get_description_response resp;
+	int ret;
+
+	req.psy_id = gbpsy->id;
+
+	ret = gb_operation_sync(connection,
+				GB_POWER_SUPPLY_TYPE_GET_DESCRIPTION,
+				&req, sizeof(req), &resp, sizeof(resp));
+	if (ret < 0)
+		return ret;
+
+	gbpsy->manufacturer = kstrndup(resp.manufacturer, PROP_MAX, GFP_KERNEL);
+	if (!gbpsy->manufacturer)
+		return -ENOMEM;
+	gbpsy->model_name = kstrndup(resp.model, PROP_MAX, GFP_KERNEL);
+	if (!gbpsy->model_name)
+		return -ENOMEM;
+	gbpsy->serial_number = kstrndup(resp.serial_number, PROP_MAX,
+				       GFP_KERNEL);
+	if (!gbpsy->serial_number)
+		return -ENOMEM;
+
+	gbpsy->type = le16_to_cpu(resp.type);
+	gbpsy->properties_count = resp.properties_count;
+
+	return 0;
+}
+
+static int gb_power_supply_prop_descriptors_get(struct gb_power_supply *gbpsy)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	struct gb_power_supply_get_property_descriptors_request *req;
+	struct gb_power_supply_get_property_descriptors_response *resp;
+	struct gb_operation *op;
+	u8 props_count = gbpsy->properties_count;
+	enum power_supply_property psp;
+	int ret;
+	int i, r = 0;
+
+	if (props_count == 0)
+		return 0;
+
+	op = gb_operation_create(connection,
+				 GB_POWER_SUPPLY_TYPE_GET_PROP_DESCRIPTORS,
+				 sizeof(*req),
+				 struct_size(resp, props, props_count),
+				 GFP_KERNEL);
+	if (!op)
+		return -ENOMEM;
+
+	req = op->request->payload;
+	req->psy_id = gbpsy->id;
+
+	ret = gb_operation_request_send_sync(op);
+	if (ret < 0)
+		goto out_put_operation;
+
+	resp = op->response->payload;
+
+	/* validate received properties */
+	for (i = 0; i < props_count; i++) {
+		ret = get_psp_from_gb_prop(resp->props[i].property, &psp);
+		if (ret < 0) {
+			dev_warn(&connection->bundle->dev,
+				 "greybus property %u it is not supported by this kernel, dropped\n",
+				 resp->props[i].property);
+			gbpsy->properties_count--;
+		}
+	}
+
+	gbpsy->props = kcalloc(gbpsy->properties_count, sizeof(*gbpsy->props),
+			      GFP_KERNEL);
+	if (!gbpsy->props) {
+		ret = -ENOMEM;
+		goto out_put_operation;
+	}
+
+	gbpsy->props_raw = kcalloc(gbpsy->properties_count,
+				   sizeof(*gbpsy->props_raw), GFP_KERNEL);
+	if (!gbpsy->props_raw) {
+		ret = -ENOMEM;
+		goto out_put_operation;
+	}
+
+	/* Store available properties, skip the ones we do not support */
+	for (i = 0; i < props_count; i++) {
+		ret = get_psp_from_gb_prop(resp->props[i].property, &psp);
+		if (ret < 0) {
+			r++;
+			continue;
+		}
+		gbpsy->props[i - r].prop = psp;
+		gbpsy->props[i - r].gb_prop = resp->props[i].property;
+		gbpsy->props_raw[i - r] = psp;
+		if (resp->props[i].is_writeable)
+			gbpsy->props[i - r].is_writeable = true;
+	}
+
+	/*
+	 * now append the properties that we already got information in the
+	 * get_description operation. (char * ones)
+	 */
+	_gb_power_supply_append_props(gbpsy);
+
+	ret = 0;
+out_put_operation:
+	gb_operation_put(op);
+
+	return ret;
+}
+
+static int __gb_power_supply_property_update(struct gb_power_supply *gbpsy,
+					     enum power_supply_property psp)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	struct gb_power_supply_prop *prop;
+	struct gb_power_supply_get_property_request req;
+	struct gb_power_supply_get_property_response resp;
+	int val;
+	int ret;
+
+	prop = get_psy_prop(gbpsy, psp);
+	if (!prop)
+		return -EINVAL;
+	req.psy_id = gbpsy->id;
+	req.property = prop->gb_prop;
+
+	ret = gb_operation_sync(connection, GB_POWER_SUPPLY_TYPE_GET_PROPERTY,
+				&req, sizeof(req), &resp, sizeof(resp));
+	if (ret < 0)
+		return ret;
+
+	val = le32_to_cpu(resp.prop_val);
+	if (val == prop->val)
+		return 0;
+
+	prop->previous_val = prop->val;
+	prop->val = val;
+
+	check_changed(gbpsy, prop);
+
+	return 0;
+}
+
+static int __gb_power_supply_property_get(struct gb_power_supply *gbpsy,
+					  enum power_supply_property psp,
+					  union power_supply_propval *val)
+{
+	struct gb_power_supply_prop *prop;
+
+	prop = get_psy_prop(gbpsy, psp);
+	if (!prop)
+		return -EINVAL;
+
+	val->intval = prop->val;
+	return 0;
+}
+
+static int __gb_power_supply_property_strval_get(struct gb_power_supply *gbpsy,
+						enum power_supply_property psp,
+						union power_supply_propval *val)
+{
+	switch (psp) {
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = gbpsy->model_name;
+		break;
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		val->strval = gbpsy->manufacturer;
+		break;
+	case POWER_SUPPLY_PROP_SERIAL_NUMBER:
+		val->strval = gbpsy->serial_number;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int _gb_power_supply_property_get(struct gb_power_supply *gbpsy,
+					 enum power_supply_property psp,
+					 union power_supply_propval *val)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	int ret;
+
+	/*
+	 * Properties of type const char *, were already fetched on
+	 * get_description operation and should be cached in gb
+	 */
+	if (is_prop_valint(psp))
+		ret = __gb_power_supply_property_get(gbpsy, psp, val);
+	else
+		ret = __gb_power_supply_property_strval_get(gbpsy, psp, val);
+
+	if (ret < 0)
+		dev_err(&connection->bundle->dev, "get property %u\n", psp);
+
+	return 0;
+}
+
+static int is_cache_valid(struct gb_power_supply *gbpsy)
+{
+	/* check if cache is good enough or it has expired */
+	if (gbpsy->cache_invalid) {
+		gbpsy->cache_invalid = 0;
+		return 0;
+	}
+
+	if (gbpsy->last_update &&
+	    time_is_after_jiffies(gbpsy->last_update +
+				  msecs_to_jiffies(cache_time)))
+		return 1;
+
+	return 0;
+}
+
+static int gb_power_supply_status_get(struct gb_power_supply *gbpsy)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	int ret = 0;
+	int i;
+
+	if (is_cache_valid(gbpsy))
+		return 0;
+
+	ret = gb_pm_runtime_get_sync(connection->bundle);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < gbpsy->properties_count; i++) {
+		ret = __gb_power_supply_property_update(gbpsy,
+							gbpsy->props[i].prop);
+		if (ret < 0)
+			break;
+	}
+
+	if (ret == 0)
+		gbpsy->last_update = jiffies;
+
+	gb_pm_runtime_put_autosuspend(connection->bundle);
+	return ret;
+}
+
+static void gb_power_supply_status_update(struct gb_power_supply *gbpsy)
+{
+	/* check if there a change that need to be reported */
+	gb_power_supply_status_get(gbpsy);
+
+	if (!gbpsy->changed)
+		return;
+
+	gbpsy->update_interval = update_interval_init;
+	__gb_power_supply_changed(gbpsy);
+	gbpsy->changed = false;
+}
+
+static void gb_power_supply_work(struct work_struct *work)
+{
+	struct gb_power_supply *gbpsy = container_of(work,
+						     struct gb_power_supply,
+						     work.work);
+
+	/*
+	 * if the poll interval is not set, disable polling, this is helpful
+	 * specially at unregister time.
+	 */
+	if (!gbpsy->update_interval)
+		return;
+
+	gb_power_supply_status_update(gbpsy);
+	next_interval(gbpsy);
+	schedule_delayed_work(&gbpsy->work, gbpsy->update_interval);
+}
+
+static int get_property(struct power_supply *b,
+			enum power_supply_property psp,
+			union power_supply_propval *val)
+{
+	struct gb_power_supply *gbpsy = to_gb_power_supply(b);
+
+	gb_power_supply_status_get(gbpsy);
+
+	return _gb_power_supply_property_get(gbpsy, psp, val);
+}
+
+static int gb_power_supply_property_set(struct gb_power_supply *gbpsy,
+					enum power_supply_property psp,
+					int val)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	struct gb_power_supply_prop *prop;
+	struct gb_power_supply_set_property_request req;
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(connection->bundle);
+	if (ret)
+		return ret;
+
+	prop = get_psy_prop(gbpsy, psp);
+	if (!prop) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	req.psy_id = gbpsy->id;
+	req.property = prop->gb_prop;
+	req.prop_val = cpu_to_le32((s32)val);
+
+	ret = gb_operation_sync(connection, GB_POWER_SUPPLY_TYPE_SET_PROPERTY,
+				&req, sizeof(req), NULL, 0);
+	if (ret < 0)
+		goto out;
+
+	/* cache immediately the new value */
+	prop->val = val;
+
+out:
+	gb_pm_runtime_put_autosuspend(connection->bundle);
+	return ret;
+}
+
+static int set_property(struct power_supply *b,
+			enum power_supply_property psp,
+			const union power_supply_propval *val)
+{
+	struct gb_power_supply *gbpsy = to_gb_power_supply(b);
+
+	return gb_power_supply_property_set(gbpsy, psp, val->intval);
+}
+
+static int property_is_writeable(struct power_supply *b,
+				 enum power_supply_property psp)
+{
+	struct gb_power_supply *gbpsy = to_gb_power_supply(b);
+
+	return is_psy_prop_writeable(gbpsy, psp);
+}
+
+static int gb_power_supply_register(struct gb_power_supply *gbpsy)
+{
+	struct gb_connection *connection = get_conn_from_psy(gbpsy);
+	struct power_supply_config cfg = {};
+
+	cfg.drv_data = gbpsy;
+
+	gbpsy->desc.name		= gbpsy->name;
+	gbpsy->desc.type		= gbpsy->type;
+	gbpsy->desc.properties		= gbpsy->props_raw;
+	gbpsy->desc.num_properties	= total_props(gbpsy);
+	gbpsy->desc.get_property	= get_property;
+	gbpsy->desc.set_property	= set_property;
+	gbpsy->desc.property_is_writeable = property_is_writeable;
+
+	gbpsy->psy = power_supply_register(&connection->bundle->dev,
+					   &gbpsy->desc, &cfg);
+	return PTR_ERR_OR_ZERO(gbpsy->psy);
+}
+
+static void _gb_power_supply_free(struct gb_power_supply *gbpsy)
+{
+	kfree(gbpsy->serial_number);
+	kfree(gbpsy->model_name);
+	kfree(gbpsy->manufacturer);
+	kfree(gbpsy->props_raw);
+	kfree(gbpsy->props);
+}
+
+static void _gb_power_supply_release(struct gb_power_supply *gbpsy)
+{
+	gbpsy->update_interval = 0;
+
+	cancel_delayed_work_sync(&gbpsy->work);
+
+	if (gbpsy->registered)
+		power_supply_unregister(gbpsy->psy);
+
+	_gb_power_supply_free(gbpsy);
+}
+
+static void _gb_power_supplies_release(struct gb_power_supplies *supplies)
+{
+	int i;
+
+	if (!supplies->supply)
+		return;
+
+	mutex_lock(&supplies->supplies_lock);
+	for (i = 0; i < supplies->supplies_count; i++)
+		_gb_power_supply_release(&supplies->supply[i]);
+	kfree(supplies->supply);
+	mutex_unlock(&supplies->supplies_lock);
+	kfree(supplies);
+}
+
+static int gb_power_supplies_get_count(struct gb_power_supplies *supplies)
+{
+	struct gb_power_supply_get_supplies_response resp;
+	int ret;
+
+	ret = gb_operation_sync(supplies->connection,
+				GB_POWER_SUPPLY_TYPE_GET_SUPPLIES,
+				NULL, 0, &resp, sizeof(resp));
+	if (ret < 0)
+		return ret;
+
+	if  (!resp.supplies_count)
+		return -EINVAL;
+
+	supplies->supplies_count = resp.supplies_count;
+
+	return ret;
+}
+
+static int gb_power_supply_config(struct gb_power_supplies *supplies, int id)
+{
+	struct gb_power_supply *gbpsy = &supplies->supply[id];
+	int ret;
+
+	gbpsy->supplies = supplies;
+	gbpsy->id = id;
+
+	ret = gb_power_supply_description_get(gbpsy);
+	if (ret < 0)
+		return ret;
+
+	return gb_power_supply_prop_descriptors_get(gbpsy);
+}
+
+static int gb_power_supply_enable(struct gb_power_supply *gbpsy)
+{
+	int ret;
+
+	/* guarantee that we have an unique name, before register */
+	ret =  __gb_power_supply_set_name(gbpsy->model_name, gbpsy->name,
+					  sizeof(gbpsy->name));
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&gbpsy->supply_lock);
+
+	ret = gb_power_supply_register(gbpsy);
+	if (ret < 0)
+		return ret;
+
+	gbpsy->update_interval = update_interval_init;
+	INIT_DELAYED_WORK(&gbpsy->work, gb_power_supply_work);
+	schedule_delayed_work(&gbpsy->work, 0);
+
+	/* everything went fine, mark it for release code to know */
+	gbpsy->registered = true;
+
+	return 0;
+}
+
+static int gb_power_supplies_setup(struct gb_power_supplies *supplies)
+{
+	struct gb_connection *connection = supplies->connection;
+	int ret;
+	int i;
+
+	mutex_lock(&supplies->supplies_lock);
+
+	ret = gb_power_supplies_get_count(supplies);
+	if (ret < 0)
+		goto out;
+
+	supplies->supply = kcalloc(supplies->supplies_count,
+				     sizeof(struct gb_power_supply),
+				     GFP_KERNEL);
+
+	if (!supplies->supply) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	for (i = 0; i < supplies->supplies_count; i++) {
+		ret = gb_power_supply_config(supplies, i);
+		if (ret < 0) {
+			dev_err(&connection->bundle->dev,
+				"Fail to configure supplies devices\n");
+			goto out;
+		}
+	}
+out:
+	mutex_unlock(&supplies->supplies_lock);
+	return ret;
+}
+
+static int gb_power_supplies_register(struct gb_power_supplies *supplies)
+{
+	struct gb_connection *connection = supplies->connection;
+	int ret = 0;
+	int i;
+
+	mutex_lock(&supplies->supplies_lock);
+
+	for (i = 0; i < supplies->supplies_count; i++) {
+		ret = gb_power_supply_enable(&supplies->supply[i]);
+		if (ret < 0) {
+			dev_err(&connection->bundle->dev,
+				"Fail to enable supplies devices\n");
+			break;
+		}
+	}
+
+	mutex_unlock(&supplies->supplies_lock);
+	return ret;
+}
+
+static int gb_supplies_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_power_supplies *supplies = gb_connection_get_data(connection);
+	struct gb_power_supply *gbpsy;
+	struct gb_message *request;
+	struct gb_power_supply_event_request *payload;
+	u8 psy_id;
+	u8 event;
+	int ret = 0;
+
+	if (op->type != GB_POWER_SUPPLY_TYPE_EVENT) {
+		dev_err(&connection->bundle->dev,
+			"Unsupported unsolicited event: %u\n", op->type);
+		return -EINVAL;
+	}
+
+	request = op->request;
+
+	if (request->payload_size < sizeof(*payload)) {
+		dev_err(&connection->bundle->dev,
+			"Wrong event size received (%zu < %zu)\n",
+			request->payload_size, sizeof(*payload));
+		return -EINVAL;
+	}
+
+	payload = request->payload;
+	psy_id = payload->psy_id;
+	mutex_lock(&supplies->supplies_lock);
+	if (psy_id >= supplies->supplies_count ||
+	    !supplies->supply[psy_id].registered) {
+		dev_err(&connection->bundle->dev,
+			"Event received for unconfigured power_supply id: %d\n",
+			psy_id);
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	event = payload->event;
+	/*
+	 * we will only handle events after setup is done and before release is
+	 * running. For that just check update_interval.
+	 */
+	gbpsy = &supplies->supply[psy_id];
+	if (!gbpsy->update_interval) {
+		ret = -ESHUTDOWN;
+		goto out_unlock;
+	}
+
+	if (event & GB_POWER_SUPPLY_UPDATE) {
+		/*
+		 * we need to make sure we invalidate cache, if not no new
+		 * values for the properties will be fetch and the all propose
+		 * of this event is missed
+		 */
+		gbpsy->cache_invalid = 1;
+		gb_power_supply_status_update(gbpsy);
+	}
+
+out_unlock:
+	mutex_unlock(&supplies->supplies_lock);
+	return ret;
+}
+
+static int gb_power_supply_probe(struct gb_bundle *bundle,
+				 const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_power_supplies *supplies;
+	int ret;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_POWER_SUPPLY)
+		return -ENODEV;
+
+	supplies = kzalloc(sizeof(*supplies), GFP_KERNEL);
+	if (!supplies)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_supplies_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto out;
+	}
+
+	supplies->connection = connection;
+	gb_connection_set_data(connection, supplies);
+
+	mutex_init(&supplies->supplies_lock);
+
+	greybus_set_drvdata(bundle, supplies);
+
+	/* We aren't ready to receive an incoming request yet */
+	ret = gb_connection_enable_tx(connection);
+	if (ret)
+		goto error_connection_destroy;
+
+	ret = gb_power_supplies_setup(supplies);
+	if (ret < 0)
+		goto error_connection_disable;
+
+	/* We are ready to receive an incoming request now, enable RX as well */
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto error_connection_disable;
+
+	ret = gb_power_supplies_register(supplies);
+	if (ret < 0)
+		goto error_connection_disable;
+
+	gb_pm_runtime_put_autosuspend(bundle);
+	return 0;
+
+error_connection_disable:
+	gb_connection_disable(connection);
+error_connection_destroy:
+	gb_connection_destroy(connection);
+out:
+	_gb_power_supplies_release(supplies);
+	return ret;
+}
+
+static void gb_power_supply_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_power_supplies *supplies = greybus_get_drvdata(bundle);
+
+	gb_connection_disable(supplies->connection);
+	gb_connection_destroy(supplies->connection);
+
+	_gb_power_supplies_release(supplies);
+}
+
+static const struct greybus_bundle_id gb_power_supply_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_POWER_SUPPLY) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_power_supply_id_table);
+
+static struct greybus_driver gb_power_supply_driver = {
+	.name		= "power_supply",
+	.probe		= gb_power_supply_probe,
+	.disconnect	= gb_power_supply_disconnect,
+	.id_table	= gb_power_supply_id_table,
+};
+module_greybus_driver(gb_power_supply_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/pwm.c b/drivers/staging/greybus/pwm.c
new file mode 100644
index 000000000..3fda17223
--- /dev/null
+++ b/drivers/staging/greybus/pwm.c
@@ -0,0 +1,351 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * PWM Greybus driver.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/pwm.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+struct gb_pwm_chip {
+	struct gb_connection	*connection;
+	u8			pwm_max;	/* max pwm number */
+
+	struct pwm_chip		chip;
+	struct pwm_chip		*pwm;
+};
+#define pwm_chip_to_gb_pwm_chip(chip) \
+	container_of(chip, struct gb_pwm_chip, chip)
+
+
+static int gb_pwm_count_operation(struct gb_pwm_chip *pwmc)
+{
+	struct gb_pwm_count_response response;
+	int ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_PWM_COUNT,
+				NULL, 0, &response, sizeof(response));
+	if (ret)
+		return ret;
+	pwmc->pwm_max = response.count;
+	return 0;
+}
+
+static int gb_pwm_activate_operation(struct gb_pwm_chip *pwmc,
+				     u8 which)
+{
+	struct gb_pwm_activate_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_ACTIVATE,
+				&request, sizeof(request), NULL, 0);
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_deactivate_operation(struct gb_pwm_chip *pwmc,
+				       u8 which)
+{
+	struct gb_pwm_deactivate_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_DEACTIVATE,
+				&request, sizeof(request), NULL, 0);
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_config_operation(struct gb_pwm_chip *pwmc,
+				   u8 which, u32 duty, u32 period)
+{
+	struct gb_pwm_config_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+	request.duty = cpu_to_le32(duty);
+	request.period = cpu_to_le32(period);
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_CONFIG,
+				&request, sizeof(request), NULL, 0);
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_set_polarity_operation(struct gb_pwm_chip *pwmc,
+					 u8 which, u8 polarity)
+{
+	struct gb_pwm_polarity_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+	request.polarity = polarity;
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_POLARITY,
+				&request, sizeof(request), NULL, 0);
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_enable_operation(struct gb_pwm_chip *pwmc,
+				   u8 which)
+{
+	struct gb_pwm_enable_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_ENABLE,
+				&request, sizeof(request), NULL, 0);
+	if (ret)
+		gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_disable_operation(struct gb_pwm_chip *pwmc,
+				    u8 which)
+{
+	struct gb_pwm_disable_request request;
+	struct gbphy_device *gbphy_dev;
+	int ret;
+
+	if (which > pwmc->pwm_max)
+		return -EINVAL;
+
+	request.which = which;
+
+	ret = gb_operation_sync(pwmc->connection, GB_PWM_TYPE_DISABLE,
+				&request, sizeof(request), NULL, 0);
+
+	gbphy_dev = to_gbphy_dev(pwmc->chip.dev);
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+}
+
+static int gb_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct gb_pwm_chip *pwmc = pwm_chip_to_gb_pwm_chip(chip);
+
+	return gb_pwm_activate_operation(pwmc, pwm->hwpwm);
+};
+
+static void gb_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct gb_pwm_chip *pwmc = pwm_chip_to_gb_pwm_chip(chip);
+
+	if (pwm_is_enabled(pwm))
+		dev_warn(chip->dev, "freeing PWM device without disabling\n");
+
+	gb_pwm_deactivate_operation(pwmc, pwm->hwpwm);
+}
+
+static int gb_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
+			const struct pwm_state *state)
+{
+	int err;
+	bool enabled = pwm->state.enabled;
+	u64 period = state->period;
+	u64 duty_cycle = state->duty_cycle;
+	struct gb_pwm_chip *pwmc = pwm_chip_to_gb_pwm_chip(chip);
+
+	/* Set polarity */
+	if (state->polarity != pwm->state.polarity) {
+		if (enabled) {
+			gb_pwm_disable_operation(pwmc, pwm->hwpwm);
+			enabled = false;
+		}
+		err = gb_pwm_set_polarity_operation(pwmc, pwm->hwpwm, state->polarity);
+		if (err)
+			return err;
+	}
+
+	if (!state->enabled) {
+		if (enabled)
+			gb_pwm_disable_operation(pwmc, pwm->hwpwm);
+		return 0;
+	}
+
+	/*
+	 * Set period and duty cycle
+	 *
+	 * PWM privodes 64-bit period and duty_cycle, but greybus only accepts
+	 * 32-bit, so their values have to be limited to U32_MAX.
+	 */
+	if (period > U32_MAX)
+		period = U32_MAX;
+
+	if (duty_cycle > period)
+		duty_cycle = period;
+
+	err = gb_pwm_config_operation(pwmc, pwm->hwpwm, duty_cycle, period);
+	if (err)
+		return err;
+
+	/* enable/disable */
+	if (!enabled)
+		return gb_pwm_enable_operation(pwmc, pwm->hwpwm);
+
+	return 0;
+}
+
+static const struct pwm_ops gb_pwm_ops = {
+	.request = gb_pwm_request,
+	.free = gb_pwm_free,
+	.apply = gb_pwm_apply,
+	.owner = THIS_MODULE,
+};
+
+static int gb_pwm_probe(struct gbphy_device *gbphy_dev,
+			const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	struct gb_pwm_chip *pwmc;
+	struct pwm_chip *pwm;
+	int ret;
+
+	pwmc = kzalloc(sizeof(*pwmc), GFP_KERNEL);
+	if (!pwmc)
+		return -ENOMEM;
+
+	connection = gb_connection_create(gbphy_dev->bundle,
+					  le16_to_cpu(gbphy_dev->cport_desc->id),
+					  NULL);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto exit_pwmc_free;
+	}
+
+	pwmc->connection = connection;
+	gb_connection_set_data(connection, pwmc);
+	gb_gbphy_set_data(gbphy_dev, pwmc);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto exit_connection_destroy;
+
+	/* Query number of pwms present */
+	ret = gb_pwm_count_operation(pwmc);
+	if (ret)
+		goto exit_connection_disable;
+
+	pwm = &pwmc->chip;
+
+	pwm->dev = &gbphy_dev->dev;
+	pwm->ops = &gb_pwm_ops;
+	pwm->npwm = pwmc->pwm_max + 1;
+
+	ret = pwmchip_add(pwm);
+	if (ret) {
+		dev_err(&gbphy_dev->dev,
+			"failed to register PWM: %d\n", ret);
+		goto exit_connection_disable;
+	}
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+	return 0;
+
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+exit_pwmc_free:
+	kfree(pwmc);
+	return ret;
+}
+
+static void gb_pwm_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_pwm_chip *pwmc = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = pwmc->connection;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	pwmchip_remove(&pwmc->chip);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+	kfree(pwmc);
+}
+
+static const struct gbphy_device_id gb_pwm_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_PWM) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_pwm_id_table);
+
+static struct gbphy_driver pwm_driver = {
+	.name		= "pwm",
+	.probe		= gb_pwm_probe,
+	.remove		= gb_pwm_remove,
+	.id_table	= gb_pwm_id_table,
+};
+
+module_gbphy_driver(pwm_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/raw.c b/drivers/staging/greybus/raw.c
new file mode 100644
index 000000000..2a375f407
--- /dev/null
+++ b/drivers/staging/greybus/raw.c
@@ -0,0 +1,379 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus driver for the Raw protocol
+ *
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sizes.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/uaccess.h>
+#include <linux/greybus.h>
+
+struct gb_raw {
+	struct gb_connection *connection;
+
+	struct list_head list;
+	int list_data;
+	struct mutex list_lock;
+	dev_t dev;
+	struct cdev cdev;
+	struct device *device;
+};
+
+struct raw_data {
+	struct list_head entry;
+	u32 len;
+	u8 data[];
+};
+
+static struct class *raw_class;
+static int raw_major;
+static const struct file_operations raw_fops;
+static DEFINE_IDA(minors);
+
+/* Number of minor devices this driver supports */
+#define NUM_MINORS	256
+
+/* Maximum size of any one send data buffer we support */
+#define MAX_PACKET_SIZE	(PAGE_SIZE * 2)
+
+/*
+ * Maximum size of the data in the receive buffer we allow before we start to
+ * drop messages on the floor
+ */
+#define MAX_DATA_SIZE	(MAX_PACKET_SIZE * 8)
+
+/*
+ * Add the raw data message to the list of received messages.
+ */
+static int receive_data(struct gb_raw *raw, u32 len, u8 *data)
+{
+	struct raw_data *raw_data;
+	struct device *dev = &raw->connection->bundle->dev;
+	int retval = 0;
+
+	if (len > MAX_PACKET_SIZE) {
+		dev_err(dev, "Too big of a data packet, rejected\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&raw->list_lock);
+	if ((raw->list_data + len) > MAX_DATA_SIZE) {
+		dev_err(dev, "Too much data in receive buffer, now dropping packets\n");
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	raw_data = kmalloc(sizeof(*raw_data) + len, GFP_KERNEL);
+	if (!raw_data) {
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	raw->list_data += len;
+	raw_data->len = len;
+	memcpy(&raw_data->data[0], data, len);
+
+	list_add_tail(&raw_data->entry, &raw->list);
+exit:
+	mutex_unlock(&raw->list_lock);
+	return retval;
+}
+
+static int gb_raw_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct device *dev = &connection->bundle->dev;
+	struct gb_raw *raw = greybus_get_drvdata(connection->bundle);
+	struct gb_raw_send_request *receive;
+	u32 len;
+
+	if (op->type != GB_RAW_TYPE_SEND) {
+		dev_err(dev, "unknown request type 0x%02x\n", op->type);
+		return -EINVAL;
+	}
+
+	/* Verify size of payload */
+	if (op->request->payload_size < sizeof(*receive)) {
+		dev_err(dev, "raw receive request too small (%zu < %zu)\n",
+			op->request->payload_size, sizeof(*receive));
+		return -EINVAL;
+	}
+	receive = op->request->payload;
+	len = le32_to_cpu(receive->len);
+	if (len != (int)(op->request->payload_size - sizeof(__le32))) {
+		dev_err(dev, "raw receive request wrong size %d vs %d\n", len,
+			(int)(op->request->payload_size - sizeof(__le32)));
+		return -EINVAL;
+	}
+	if (len == 0) {
+		dev_err(dev, "raw receive request of 0 bytes?\n");
+		return -EINVAL;
+	}
+
+	return receive_data(raw, len, receive->data);
+}
+
+static int gb_raw_send(struct gb_raw *raw, u32 len, const char __user *data)
+{
+	struct gb_connection *connection = raw->connection;
+	struct gb_raw_send_request *request;
+	int retval;
+
+	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+
+	if (copy_from_user(&request->data[0], data, len)) {
+		kfree(request);
+		return -EFAULT;
+	}
+
+	request->len = cpu_to_le32(len);
+
+	retval = gb_operation_sync(connection, GB_RAW_TYPE_SEND,
+				   request, len + sizeof(*request),
+				   NULL, 0);
+
+	kfree(request);
+	return retval;
+}
+
+static int gb_raw_probe(struct gb_bundle *bundle,
+			const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_raw *raw;
+	int retval;
+	int minor;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_RAW)
+		return -ENODEV;
+
+	raw = kzalloc(sizeof(*raw), GFP_KERNEL);
+	if (!raw)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  gb_raw_request_handler);
+	if (IS_ERR(connection)) {
+		retval = PTR_ERR(connection);
+		goto error_free;
+	}
+
+	INIT_LIST_HEAD(&raw->list);
+	mutex_init(&raw->list_lock);
+
+	raw->connection = connection;
+	greybus_set_drvdata(bundle, raw);
+
+	minor = ida_simple_get(&minors, 0, 0, GFP_KERNEL);
+	if (minor < 0) {
+		retval = minor;
+		goto error_connection_destroy;
+	}
+
+	raw->dev = MKDEV(raw_major, minor);
+	cdev_init(&raw->cdev, &raw_fops);
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto error_remove_ida;
+
+	retval = cdev_add(&raw->cdev, raw->dev, 1);
+	if (retval)
+		goto error_connection_disable;
+
+	raw->device = device_create(raw_class, &connection->bundle->dev,
+				    raw->dev, raw, "gb!raw%d", minor);
+	if (IS_ERR(raw->device)) {
+		retval = PTR_ERR(raw->device);
+		goto error_del_cdev;
+	}
+
+	return 0;
+
+error_del_cdev:
+	cdev_del(&raw->cdev);
+
+error_connection_disable:
+	gb_connection_disable(connection);
+
+error_remove_ida:
+	ida_simple_remove(&minors, minor);
+
+error_connection_destroy:
+	gb_connection_destroy(connection);
+
+error_free:
+	kfree(raw);
+	return retval;
+}
+
+static void gb_raw_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_raw *raw = greybus_get_drvdata(bundle);
+	struct gb_connection *connection = raw->connection;
+	struct raw_data *raw_data;
+	struct raw_data *temp;
+
+	// FIXME - handle removing a connection when the char device node is open.
+	device_destroy(raw_class, raw->dev);
+	cdev_del(&raw->cdev);
+	gb_connection_disable(connection);
+	ida_simple_remove(&minors, MINOR(raw->dev));
+	gb_connection_destroy(connection);
+
+	mutex_lock(&raw->list_lock);
+	list_for_each_entry_safe(raw_data, temp, &raw->list, entry) {
+		list_del(&raw_data->entry);
+		kfree(raw_data);
+	}
+	mutex_unlock(&raw->list_lock);
+
+	kfree(raw);
+}
+
+/*
+ * Character device node interfaces.
+ *
+ * Note, we are using read/write to only allow a single read/write per message.
+ * This means for read(), you have to provide a big enough buffer for the full
+ * message to be copied into.  If the buffer isn't big enough, the read() will
+ * fail with -ENOSPC.
+ */
+
+static int raw_open(struct inode *inode, struct file *file)
+{
+	struct cdev *cdev = inode->i_cdev;
+	struct gb_raw *raw = container_of(cdev, struct gb_raw, cdev);
+
+	file->private_data = raw;
+	return 0;
+}
+
+static ssize_t raw_write(struct file *file, const char __user *buf,
+			 size_t count, loff_t *ppos)
+{
+	struct gb_raw *raw = file->private_data;
+	int retval;
+
+	if (!count)
+		return 0;
+
+	if (count > MAX_PACKET_SIZE)
+		return -E2BIG;
+
+	retval = gb_raw_send(raw, count, buf);
+	if (retval)
+		return retval;
+
+	return count;
+}
+
+static ssize_t raw_read(struct file *file, char __user *buf, size_t count,
+			loff_t *ppos)
+{
+	struct gb_raw *raw = file->private_data;
+	int retval = 0;
+	struct raw_data *raw_data;
+
+	mutex_lock(&raw->list_lock);
+	if (list_empty(&raw->list))
+		goto exit;
+
+	raw_data = list_first_entry(&raw->list, struct raw_data, entry);
+	if (raw_data->len > count) {
+		retval = -ENOSPC;
+		goto exit;
+	}
+
+	if (copy_to_user(buf, &raw_data->data[0], raw_data->len)) {
+		retval = -EFAULT;
+		goto exit;
+	}
+
+	list_del(&raw_data->entry);
+	raw->list_data -= raw_data->len;
+	retval = raw_data->len;
+	kfree(raw_data);
+
+exit:
+	mutex_unlock(&raw->list_lock);
+	return retval;
+}
+
+static const struct file_operations raw_fops = {
+	.owner		= THIS_MODULE,
+	.write		= raw_write,
+	.read		= raw_read,
+	.open		= raw_open,
+	.llseek		= noop_llseek,
+};
+
+static const struct greybus_bundle_id gb_raw_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_RAW) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_raw_id_table);
+
+static struct greybus_driver gb_raw_driver = {
+	.name		= "raw",
+	.probe		= gb_raw_probe,
+	.disconnect	= gb_raw_disconnect,
+	.id_table	= gb_raw_id_table,
+};
+
+static int raw_init(void)
+{
+	dev_t dev;
+	int retval;
+
+	raw_class = class_create(THIS_MODULE, "gb_raw");
+	if (IS_ERR(raw_class)) {
+		retval = PTR_ERR(raw_class);
+		goto error_class;
+	}
+
+	retval = alloc_chrdev_region(&dev, 0, NUM_MINORS, "gb_raw");
+	if (retval < 0)
+		goto error_chrdev;
+
+	raw_major = MAJOR(dev);
+
+	retval = greybus_register(&gb_raw_driver);
+	if (retval)
+		goto error_gb;
+
+	return 0;
+
+error_gb:
+	unregister_chrdev_region(dev, NUM_MINORS);
+error_chrdev:
+	class_destroy(raw_class);
+error_class:
+	return retval;
+}
+module_init(raw_init);
+
+static void __exit raw_exit(void)
+{
+	greybus_deregister(&gb_raw_driver);
+	unregister_chrdev_region(MKDEV(raw_major, 0), NUM_MINORS);
+	class_destroy(raw_class);
+	ida_destroy(&minors);
+}
+module_exit(raw_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/sdio.c b/drivers/staging/greybus/sdio.c
new file mode 100644
index 000000000..25bee5335
--- /dev/null
+++ b/drivers/staging/greybus/sdio.c
@@ -0,0 +1,883 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * SD/MMC Greybus driver.
+ *
+ * Copyright 2014-2015 Google Inc.
+ * Copyright 2014-2015 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/mmc/core.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/scatterlist.h>
+#include <linux/workqueue.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+struct gb_sdio_host {
+	struct gb_connection	*connection;
+	struct gbphy_device	*gbphy_dev;
+	struct mmc_host		*mmc;
+	struct mmc_request	*mrq;
+	struct mutex		lock;	/* lock for this host */
+	size_t			data_max;
+	spinlock_t		xfer;	/* lock to cancel ongoing transfer */
+	bool			xfer_stop;
+	struct workqueue_struct	*mrq_workqueue;
+	struct work_struct	mrqwork;
+	u8			queued_events;
+	bool			removed;
+	bool			card_present;
+	bool			read_only;
+};
+
+#define GB_SDIO_RSP_R1_R5_R6_R7	(GB_SDIO_RSP_PRESENT | GB_SDIO_RSP_CRC | \
+				 GB_SDIO_RSP_OPCODE)
+#define GB_SDIO_RSP_R3_R4	(GB_SDIO_RSP_PRESENT)
+#define GB_SDIO_RSP_R2		(GB_SDIO_RSP_PRESENT | GB_SDIO_RSP_CRC | \
+				 GB_SDIO_RSP_136)
+#define GB_SDIO_RSP_R1B		(GB_SDIO_RSP_PRESENT | GB_SDIO_RSP_CRC | \
+				 GB_SDIO_RSP_OPCODE | GB_SDIO_RSP_BUSY)
+
+/* kernel vdd starts at 0x80 and we need to translate to greybus ones 0x01 */
+#define GB_SDIO_VDD_SHIFT	8
+
+#ifndef MMC_CAP2_CORE_RUNTIME_PM
+#define MMC_CAP2_CORE_RUNTIME_PM	0
+#endif
+
+static inline bool single_op(struct mmc_command *cmd)
+{
+	u32 opcode = cmd->opcode;
+
+	return opcode == MMC_WRITE_BLOCK ||
+	       opcode == MMC_READ_SINGLE_BLOCK;
+}
+
+static void _gb_sdio_set_host_caps(struct gb_sdio_host *host, u32 r)
+{
+	u32 caps = 0;
+	u32 caps2 = 0;
+
+	caps = ((r & GB_SDIO_CAP_NONREMOVABLE) ? MMC_CAP_NONREMOVABLE : 0) |
+		((r & GB_SDIO_CAP_4_BIT_DATA) ? MMC_CAP_4_BIT_DATA : 0) |
+		((r & GB_SDIO_CAP_8_BIT_DATA) ? MMC_CAP_8_BIT_DATA : 0) |
+		((r & GB_SDIO_CAP_MMC_HS) ? MMC_CAP_MMC_HIGHSPEED : 0) |
+		((r & GB_SDIO_CAP_SD_HS) ? MMC_CAP_SD_HIGHSPEED : 0) |
+		((r & GB_SDIO_CAP_1_2V_DDR) ? MMC_CAP_1_2V_DDR : 0) |
+		((r & GB_SDIO_CAP_1_8V_DDR) ? MMC_CAP_1_8V_DDR : 0) |
+		((r & GB_SDIO_CAP_POWER_OFF_CARD) ? MMC_CAP_POWER_OFF_CARD : 0) |
+		((r & GB_SDIO_CAP_UHS_SDR12) ? MMC_CAP_UHS_SDR12 : 0) |
+		((r & GB_SDIO_CAP_UHS_SDR25) ? MMC_CAP_UHS_SDR25 : 0) |
+		((r & GB_SDIO_CAP_UHS_SDR50) ? MMC_CAP_UHS_SDR50 : 0) |
+		((r & GB_SDIO_CAP_UHS_SDR104) ? MMC_CAP_UHS_SDR104 : 0) |
+		((r & GB_SDIO_CAP_UHS_DDR50) ? MMC_CAP_UHS_DDR50 : 0) |
+		((r & GB_SDIO_CAP_DRIVER_TYPE_A) ? MMC_CAP_DRIVER_TYPE_A : 0) |
+		((r & GB_SDIO_CAP_DRIVER_TYPE_C) ? MMC_CAP_DRIVER_TYPE_C : 0) |
+		((r & GB_SDIO_CAP_DRIVER_TYPE_D) ? MMC_CAP_DRIVER_TYPE_D : 0);
+
+	caps2 = ((r & GB_SDIO_CAP_HS200_1_2V) ? MMC_CAP2_HS200_1_2V_SDR : 0) |
+		((r & GB_SDIO_CAP_HS400_1_2V) ? MMC_CAP2_HS400_1_2V : 0) |
+		((r & GB_SDIO_CAP_HS400_1_8V) ? MMC_CAP2_HS400_1_8V : 0) |
+		((r & GB_SDIO_CAP_HS200_1_8V) ? MMC_CAP2_HS200_1_8V_SDR : 0);
+
+	host->mmc->caps = caps;
+	host->mmc->caps2 = caps2 | MMC_CAP2_CORE_RUNTIME_PM;
+
+	if (caps & MMC_CAP_NONREMOVABLE)
+		host->card_present = true;
+}
+
+static u32 _gb_sdio_get_host_ocr(u32 ocr)
+{
+	return (((ocr & GB_SDIO_VDD_165_195) ? MMC_VDD_165_195 : 0) |
+		((ocr & GB_SDIO_VDD_20_21) ? MMC_VDD_20_21 : 0) |
+		((ocr & GB_SDIO_VDD_21_22) ? MMC_VDD_21_22 : 0) |
+		((ocr & GB_SDIO_VDD_22_23) ? MMC_VDD_22_23 : 0) |
+		((ocr & GB_SDIO_VDD_23_24) ? MMC_VDD_23_24 : 0) |
+		((ocr & GB_SDIO_VDD_24_25) ? MMC_VDD_24_25 : 0) |
+		((ocr & GB_SDIO_VDD_25_26) ? MMC_VDD_25_26 : 0) |
+		((ocr & GB_SDIO_VDD_26_27) ? MMC_VDD_26_27 : 0) |
+		((ocr & GB_SDIO_VDD_27_28) ? MMC_VDD_27_28 : 0) |
+		((ocr & GB_SDIO_VDD_28_29) ? MMC_VDD_28_29 : 0) |
+		((ocr & GB_SDIO_VDD_29_30) ? MMC_VDD_29_30 : 0) |
+		((ocr & GB_SDIO_VDD_30_31) ? MMC_VDD_30_31 : 0) |
+		((ocr & GB_SDIO_VDD_31_32) ? MMC_VDD_31_32 : 0) |
+		((ocr & GB_SDIO_VDD_32_33) ? MMC_VDD_32_33 : 0) |
+		((ocr & GB_SDIO_VDD_33_34) ? MMC_VDD_33_34 : 0) |
+		((ocr & GB_SDIO_VDD_34_35) ? MMC_VDD_34_35 : 0) |
+		((ocr & GB_SDIO_VDD_35_36) ? MMC_VDD_35_36 : 0)
+		);
+}
+
+static int gb_sdio_get_caps(struct gb_sdio_host *host)
+{
+	struct gb_sdio_get_caps_response response;
+	struct mmc_host *mmc = host->mmc;
+	u16 data_max;
+	u32 blksz;
+	u32 ocr;
+	u32 r;
+	int ret;
+
+	ret = gb_operation_sync(host->connection, GB_SDIO_TYPE_GET_CAPABILITIES,
+				NULL, 0, &response, sizeof(response));
+	if (ret < 0)
+		return ret;
+	r = le32_to_cpu(response.caps);
+
+	_gb_sdio_set_host_caps(host, r);
+
+	/* get the max block size that could fit our payload */
+	data_max = gb_operation_get_payload_size_max(host->connection);
+	data_max = min(data_max - sizeof(struct gb_sdio_transfer_request),
+		       data_max - sizeof(struct gb_sdio_transfer_response));
+
+	blksz = min_t(u16, le16_to_cpu(response.max_blk_size), data_max);
+	blksz = max_t(u32, 512, blksz);
+
+	mmc->max_blk_size = rounddown_pow_of_two(blksz);
+	mmc->max_blk_count = le16_to_cpu(response.max_blk_count);
+	host->data_max = data_max;
+
+	/* get ocr supported values */
+	ocr = _gb_sdio_get_host_ocr(le32_to_cpu(response.ocr));
+	mmc->ocr_avail = ocr;
+	mmc->ocr_avail_sdio = mmc->ocr_avail;
+	mmc->ocr_avail_sd = mmc->ocr_avail;
+	mmc->ocr_avail_mmc = mmc->ocr_avail;
+
+	/* get frequency range values */
+	mmc->f_min = le32_to_cpu(response.f_min);
+	mmc->f_max = le32_to_cpu(response.f_max);
+
+	return 0;
+}
+
+static void _gb_queue_event(struct gb_sdio_host *host, u8 event)
+{
+	if (event & GB_SDIO_CARD_INSERTED)
+		host->queued_events &= ~GB_SDIO_CARD_REMOVED;
+	else if (event & GB_SDIO_CARD_REMOVED)
+		host->queued_events &= ~GB_SDIO_CARD_INSERTED;
+
+	host->queued_events |= event;
+}
+
+static int _gb_sdio_process_events(struct gb_sdio_host *host, u8 event)
+{
+	u8 state_changed = 0;
+
+	if (event & GB_SDIO_CARD_INSERTED) {
+		if (host->mmc->caps & MMC_CAP_NONREMOVABLE)
+			return 0;
+		if (host->card_present)
+			return 0;
+		host->card_present = true;
+		state_changed = 1;
+	}
+
+	if (event & GB_SDIO_CARD_REMOVED) {
+		if (host->mmc->caps & MMC_CAP_NONREMOVABLE)
+			return 0;
+		if (!(host->card_present))
+			return 0;
+		host->card_present = false;
+		state_changed = 1;
+	}
+
+	if (event & GB_SDIO_WP)
+		host->read_only = true;
+
+	if (state_changed) {
+		dev_info(mmc_dev(host->mmc), "card %s now event\n",
+			 (host->card_present ?  "inserted" : "removed"));
+		mmc_detect_change(host->mmc, 0);
+	}
+
+	return 0;
+}
+
+static int gb_sdio_request_handler(struct gb_operation *op)
+{
+	struct gb_sdio_host *host = gb_connection_get_data(op->connection);
+	struct gb_message *request;
+	struct gb_sdio_event_request *payload;
+	u8 type = op->type;
+	int ret =  0;
+	u8 event;
+
+	if (type != GB_SDIO_TYPE_EVENT) {
+		dev_err(mmc_dev(host->mmc),
+			"unsupported unsolicited event: %u\n", type);
+		return -EINVAL;
+	}
+
+	request = op->request;
+
+	if (request->payload_size < sizeof(*payload)) {
+		dev_err(mmc_dev(host->mmc), "wrong event size received (%zu < %zu)\n",
+			request->payload_size, sizeof(*payload));
+		return -EINVAL;
+	}
+
+	payload = request->payload;
+	event = payload->event;
+
+	if (host->removed)
+		_gb_queue_event(host, event);
+	else
+		ret = _gb_sdio_process_events(host, event);
+
+	return ret;
+}
+
+static int gb_sdio_set_ios(struct gb_sdio_host *host,
+			   struct gb_sdio_set_ios_request *request)
+{
+	int ret;
+
+	ret = gbphy_runtime_get_sync(host->gbphy_dev);
+	if (ret)
+		return ret;
+
+	ret = gb_operation_sync(host->connection, GB_SDIO_TYPE_SET_IOS, request,
+				sizeof(*request), NULL, 0);
+
+	gbphy_runtime_put_autosuspend(host->gbphy_dev);
+
+	return ret;
+}
+
+static int _gb_sdio_send(struct gb_sdio_host *host, struct mmc_data *data,
+			 size_t len, u16 nblocks, off_t skip)
+{
+	struct gb_sdio_transfer_request *request;
+	struct gb_sdio_transfer_response *response;
+	struct gb_operation *operation;
+	struct scatterlist *sg = data->sg;
+	unsigned int sg_len = data->sg_len;
+	size_t copied;
+	u16 send_blksz;
+	u16 send_blocks;
+	int ret;
+
+	WARN_ON(len > host->data_max);
+
+	operation = gb_operation_create(host->connection, GB_SDIO_TYPE_TRANSFER,
+					len + sizeof(*request),
+					sizeof(*response), GFP_KERNEL);
+	if (!operation)
+		return -ENOMEM;
+
+	request = operation->request->payload;
+	request->data_flags = data->flags >> 8;
+	request->data_blocks = cpu_to_le16(nblocks);
+	request->data_blksz = cpu_to_le16(data->blksz);
+
+	copied = sg_pcopy_to_buffer(sg, sg_len, &request->data[0], len, skip);
+
+	if (copied != len) {
+		ret = -EINVAL;
+		goto err_put_operation;
+	}
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret < 0)
+		goto err_put_operation;
+
+	response = operation->response->payload;
+
+	send_blocks = le16_to_cpu(response->data_blocks);
+	send_blksz = le16_to_cpu(response->data_blksz);
+
+	if (len != send_blksz * send_blocks) {
+		dev_err(mmc_dev(host->mmc), "send: size received: %zu != %d\n",
+			len, send_blksz * send_blocks);
+		ret = -EINVAL;
+	}
+
+err_put_operation:
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static int _gb_sdio_recv(struct gb_sdio_host *host, struct mmc_data *data,
+			 size_t len, u16 nblocks, off_t skip)
+{
+	struct gb_sdio_transfer_request *request;
+	struct gb_sdio_transfer_response *response;
+	struct gb_operation *operation;
+	struct scatterlist *sg = data->sg;
+	unsigned int sg_len = data->sg_len;
+	size_t copied;
+	u16 recv_blksz;
+	u16 recv_blocks;
+	int ret;
+
+	WARN_ON(len > host->data_max);
+
+	operation = gb_operation_create(host->connection, GB_SDIO_TYPE_TRANSFER,
+					sizeof(*request),
+					len + sizeof(*response), GFP_KERNEL);
+	if (!operation)
+		return -ENOMEM;
+
+	request = operation->request->payload;
+	request->data_flags = data->flags >> 8;
+	request->data_blocks = cpu_to_le16(nblocks);
+	request->data_blksz = cpu_to_le16(data->blksz);
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret < 0)
+		goto err_put_operation;
+
+	response = operation->response->payload;
+	recv_blocks = le16_to_cpu(response->data_blocks);
+	recv_blksz = le16_to_cpu(response->data_blksz);
+
+	if (len != recv_blksz * recv_blocks) {
+		dev_err(mmc_dev(host->mmc), "recv: size received: %d != %zu\n",
+			recv_blksz * recv_blocks, len);
+		ret = -EINVAL;
+		goto err_put_operation;
+	}
+
+	copied = sg_pcopy_from_buffer(sg, sg_len, &response->data[0], len,
+				      skip);
+	if (copied != len)
+		ret = -EINVAL;
+
+err_put_operation:
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static int gb_sdio_transfer(struct gb_sdio_host *host, struct mmc_data *data)
+{
+	size_t left, len;
+	off_t skip = 0;
+	int ret = 0;
+	u16 nblocks;
+
+	if (single_op(data->mrq->cmd) && data->blocks > 1) {
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	left = data->blksz * data->blocks;
+
+	while (left) {
+		/* check is a stop transmission is pending */
+		spin_lock(&host->xfer);
+		if (host->xfer_stop) {
+			host->xfer_stop = false;
+			spin_unlock(&host->xfer);
+			ret = -EINTR;
+			goto out;
+		}
+		spin_unlock(&host->xfer);
+		len = min(left, host->data_max);
+		nblocks = len / data->blksz;
+		len = nblocks * data->blksz;
+
+		if (data->flags & MMC_DATA_READ) {
+			ret = _gb_sdio_recv(host, data, len, nblocks, skip);
+			if (ret < 0)
+				goto out;
+		} else {
+			ret = _gb_sdio_send(host, data, len, nblocks, skip);
+			if (ret < 0)
+				goto out;
+		}
+		data->bytes_xfered += len;
+		left -= len;
+		skip += len;
+	}
+
+out:
+	data->error = ret;
+	return ret;
+}
+
+static int gb_sdio_command(struct gb_sdio_host *host, struct mmc_command *cmd)
+{
+	struct gb_sdio_command_request request = {0};
+	struct gb_sdio_command_response response;
+	struct mmc_data *data = host->mrq->data;
+	unsigned int timeout_ms;
+	u8 cmd_flags;
+	u8 cmd_type;
+	int i;
+	int ret;
+
+	switch (mmc_resp_type(cmd)) {
+	case MMC_RSP_NONE:
+		cmd_flags = GB_SDIO_RSP_NONE;
+		break;
+	case MMC_RSP_R1:
+		cmd_flags = GB_SDIO_RSP_R1_R5_R6_R7;
+		break;
+	case MMC_RSP_R1B:
+		cmd_flags = GB_SDIO_RSP_R1B;
+		break;
+	case MMC_RSP_R2:
+		cmd_flags = GB_SDIO_RSP_R2;
+		break;
+	case MMC_RSP_R3:
+		cmd_flags = GB_SDIO_RSP_R3_R4;
+		break;
+	default:
+		dev_err(mmc_dev(host->mmc), "cmd flag invalid 0x%04x\n",
+			mmc_resp_type(cmd));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	switch (mmc_cmd_type(cmd)) {
+	case MMC_CMD_BC:
+		cmd_type = GB_SDIO_CMD_BC;
+		break;
+	case MMC_CMD_BCR:
+		cmd_type = GB_SDIO_CMD_BCR;
+		break;
+	case MMC_CMD_AC:
+		cmd_type = GB_SDIO_CMD_AC;
+		break;
+	case MMC_CMD_ADTC:
+		cmd_type = GB_SDIO_CMD_ADTC;
+		break;
+	default:
+		dev_err(mmc_dev(host->mmc), "cmd type invalid 0x%04x\n",
+			mmc_cmd_type(cmd));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	request.cmd = cmd->opcode;
+	request.cmd_flags = cmd_flags;
+	request.cmd_type = cmd_type;
+	request.cmd_arg = cpu_to_le32(cmd->arg);
+	/* some controllers need to know at command time data details */
+	if (data) {
+		request.data_blocks = cpu_to_le16(data->blocks);
+		request.data_blksz = cpu_to_le16(data->blksz);
+	}
+
+	timeout_ms = cmd->busy_timeout ? cmd->busy_timeout :
+		GB_OPERATION_TIMEOUT_DEFAULT;
+
+	ret = gb_operation_sync_timeout(host->connection, GB_SDIO_TYPE_COMMAND,
+					&request, sizeof(request), &response,
+					sizeof(response), timeout_ms);
+	if (ret < 0)
+		goto out;
+
+	/* no response expected */
+	if (cmd_flags == GB_SDIO_RSP_NONE)
+		goto out;
+
+	/* long response expected */
+	if (cmd_flags & GB_SDIO_RSP_R2)
+		for (i = 0; i < 4; i++)
+			cmd->resp[i] = le32_to_cpu(response.resp[i]);
+	else
+		cmd->resp[0] = le32_to_cpu(response.resp[0]);
+
+out:
+	cmd->error = ret;
+	return ret;
+}
+
+static void gb_sdio_mrq_work(struct work_struct *work)
+{
+	struct gb_sdio_host *host;
+	struct mmc_request *mrq;
+	int ret;
+
+	host = container_of(work, struct gb_sdio_host, mrqwork);
+
+	ret = gbphy_runtime_get_sync(host->gbphy_dev);
+	if (ret)
+		return;
+
+	mutex_lock(&host->lock);
+	mrq = host->mrq;
+	if (!mrq) {
+		mutex_unlock(&host->lock);
+		gbphy_runtime_put_autosuspend(host->gbphy_dev);
+		dev_err(mmc_dev(host->mmc), "mmc request is NULL");
+		return;
+	}
+
+	if (host->removed) {
+		mrq->cmd->error = -ESHUTDOWN;
+		goto done;
+	}
+
+	if (mrq->sbc) {
+		ret = gb_sdio_command(host, mrq->sbc);
+		if (ret < 0)
+			goto done;
+	}
+
+	ret = gb_sdio_command(host, mrq->cmd);
+	if (ret < 0)
+		goto done;
+
+	if (mrq->data) {
+		ret = gb_sdio_transfer(host, mrq->data);
+		if (ret < 0)
+			goto done;
+	}
+
+	if (mrq->stop) {
+		ret = gb_sdio_command(host, mrq->stop);
+		if (ret < 0)
+			goto done;
+	}
+
+done:
+	host->mrq = NULL;
+	mutex_unlock(&host->lock);
+	mmc_request_done(host->mmc, mrq);
+	gbphy_runtime_put_autosuspend(host->gbphy_dev);
+}
+
+static void gb_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct gb_sdio_host *host = mmc_priv(mmc);
+	struct mmc_command *cmd = mrq->cmd;
+
+	/* Check if it is a cancel to ongoing transfer */
+	if (cmd->opcode == MMC_STOP_TRANSMISSION) {
+		spin_lock(&host->xfer);
+		host->xfer_stop = true;
+		spin_unlock(&host->xfer);
+	}
+
+	mutex_lock(&host->lock);
+
+	WARN_ON(host->mrq);
+	host->mrq = mrq;
+
+	if (host->removed) {
+		mrq->cmd->error = -ESHUTDOWN;
+		goto out;
+	}
+	if (!host->card_present) {
+		mrq->cmd->error = -ENOMEDIUM;
+		goto out;
+	}
+
+	queue_work(host->mrq_workqueue, &host->mrqwork);
+
+	mutex_unlock(&host->lock);
+	return;
+
+out:
+	host->mrq = NULL;
+	mutex_unlock(&host->lock);
+	mmc_request_done(mmc, mrq);
+}
+
+static void gb_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct gb_sdio_host *host = mmc_priv(mmc);
+	struct gb_sdio_set_ios_request request;
+	int ret;
+	u8 power_mode;
+	u8 bus_width;
+	u8 timing;
+	u8 signal_voltage;
+	u8 drv_type;
+	u32 vdd = 0;
+
+	mutex_lock(&host->lock);
+	request.clock = cpu_to_le32(ios->clock);
+
+	if (ios->vdd)
+		vdd = 1 << (ios->vdd - GB_SDIO_VDD_SHIFT);
+	request.vdd = cpu_to_le32(vdd);
+
+	request.bus_mode = ios->bus_mode == MMC_BUSMODE_OPENDRAIN ?
+			    GB_SDIO_BUSMODE_OPENDRAIN :
+			    GB_SDIO_BUSMODE_PUSHPULL;
+
+	switch (ios->power_mode) {
+	case MMC_POWER_OFF:
+	default:
+		power_mode = GB_SDIO_POWER_OFF;
+		break;
+	case MMC_POWER_UP:
+		power_mode = GB_SDIO_POWER_UP;
+		break;
+	case MMC_POWER_ON:
+		power_mode = GB_SDIO_POWER_ON;
+		break;
+	case MMC_POWER_UNDEFINED:
+		power_mode = GB_SDIO_POWER_UNDEFINED;
+		break;
+	}
+	request.power_mode = power_mode;
+
+	switch (ios->bus_width) {
+	case MMC_BUS_WIDTH_1:
+		bus_width = GB_SDIO_BUS_WIDTH_1;
+		break;
+	case MMC_BUS_WIDTH_4:
+	default:
+		bus_width = GB_SDIO_BUS_WIDTH_4;
+		break;
+	case MMC_BUS_WIDTH_8:
+		bus_width = GB_SDIO_BUS_WIDTH_8;
+		break;
+	}
+	request.bus_width = bus_width;
+
+	switch (ios->timing) {
+	case MMC_TIMING_LEGACY:
+	default:
+		timing = GB_SDIO_TIMING_LEGACY;
+		break;
+	case MMC_TIMING_MMC_HS:
+		timing = GB_SDIO_TIMING_MMC_HS;
+		break;
+	case MMC_TIMING_SD_HS:
+		timing = GB_SDIO_TIMING_SD_HS;
+		break;
+	case MMC_TIMING_UHS_SDR12:
+		timing = GB_SDIO_TIMING_UHS_SDR12;
+		break;
+	case MMC_TIMING_UHS_SDR25:
+		timing = GB_SDIO_TIMING_UHS_SDR25;
+		break;
+	case MMC_TIMING_UHS_SDR50:
+		timing = GB_SDIO_TIMING_UHS_SDR50;
+		break;
+	case MMC_TIMING_UHS_SDR104:
+		timing = GB_SDIO_TIMING_UHS_SDR104;
+		break;
+	case MMC_TIMING_UHS_DDR50:
+		timing = GB_SDIO_TIMING_UHS_DDR50;
+		break;
+	case MMC_TIMING_MMC_DDR52:
+		timing = GB_SDIO_TIMING_MMC_DDR52;
+		break;
+	case MMC_TIMING_MMC_HS200:
+		timing = GB_SDIO_TIMING_MMC_HS200;
+		break;
+	case MMC_TIMING_MMC_HS400:
+		timing = GB_SDIO_TIMING_MMC_HS400;
+		break;
+	}
+	request.timing = timing;
+
+	switch (ios->signal_voltage) {
+	case MMC_SIGNAL_VOLTAGE_330:
+		signal_voltage = GB_SDIO_SIGNAL_VOLTAGE_330;
+		break;
+	case MMC_SIGNAL_VOLTAGE_180:
+	default:
+		signal_voltage = GB_SDIO_SIGNAL_VOLTAGE_180;
+		break;
+	case MMC_SIGNAL_VOLTAGE_120:
+		signal_voltage = GB_SDIO_SIGNAL_VOLTAGE_120;
+		break;
+	}
+	request.signal_voltage = signal_voltage;
+
+	switch (ios->drv_type) {
+	case MMC_SET_DRIVER_TYPE_A:
+		drv_type = GB_SDIO_SET_DRIVER_TYPE_A;
+		break;
+	case MMC_SET_DRIVER_TYPE_C:
+		drv_type = GB_SDIO_SET_DRIVER_TYPE_C;
+		break;
+	case MMC_SET_DRIVER_TYPE_D:
+		drv_type = GB_SDIO_SET_DRIVER_TYPE_D;
+		break;
+	case MMC_SET_DRIVER_TYPE_B:
+	default:
+		drv_type = GB_SDIO_SET_DRIVER_TYPE_B;
+		break;
+	}
+	request.drv_type = drv_type;
+
+	ret = gb_sdio_set_ios(host, &request);
+	if (ret < 0)
+		goto out;
+
+	memcpy(&mmc->ios, ios, sizeof(mmc->ios));
+
+out:
+	mutex_unlock(&host->lock);
+}
+
+static int gb_mmc_get_ro(struct mmc_host *mmc)
+{
+	struct gb_sdio_host *host = mmc_priv(mmc);
+
+	mutex_lock(&host->lock);
+	if (host->removed) {
+		mutex_unlock(&host->lock);
+		return -ESHUTDOWN;
+	}
+	mutex_unlock(&host->lock);
+
+	return host->read_only;
+}
+
+static int gb_mmc_get_cd(struct mmc_host *mmc)
+{
+	struct gb_sdio_host *host = mmc_priv(mmc);
+
+	mutex_lock(&host->lock);
+	if (host->removed) {
+		mutex_unlock(&host->lock);
+		return -ESHUTDOWN;
+	}
+	mutex_unlock(&host->lock);
+
+	return host->card_present;
+}
+
+static int gb_mmc_switch_voltage(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	return 0;
+}
+
+static const struct mmc_host_ops gb_sdio_ops = {
+	.request	= gb_mmc_request,
+	.set_ios	= gb_mmc_set_ios,
+	.get_ro		= gb_mmc_get_ro,
+	.get_cd		= gb_mmc_get_cd,
+	.start_signal_voltage_switch	= gb_mmc_switch_voltage,
+};
+
+static int gb_sdio_probe(struct gbphy_device *gbphy_dev,
+			 const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	struct mmc_host *mmc;
+	struct gb_sdio_host *host;
+	int ret = 0;
+
+	mmc = mmc_alloc_host(sizeof(*host), &gbphy_dev->dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	connection = gb_connection_create(gbphy_dev->bundle,
+					  le16_to_cpu(gbphy_dev->cport_desc->id),
+					  gb_sdio_request_handler);
+	if (IS_ERR(connection)) {
+		ret = PTR_ERR(connection);
+		goto exit_mmc_free;
+	}
+
+	host = mmc_priv(mmc);
+	host->mmc = mmc;
+	host->removed = true;
+
+	host->connection = connection;
+	gb_connection_set_data(connection, host);
+	host->gbphy_dev = gbphy_dev;
+	gb_gbphy_set_data(gbphy_dev, host);
+
+	ret = gb_connection_enable_tx(connection);
+	if (ret)
+		goto exit_connection_destroy;
+
+	ret = gb_sdio_get_caps(host);
+	if (ret < 0)
+		goto exit_connection_disable;
+
+	mmc->ops = &gb_sdio_ops;
+
+	mmc->max_segs = host->mmc->max_blk_count;
+
+	/* for now we make a map 1:1 between max request and segment size */
+	mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
+	mmc->max_seg_size = mmc->max_req_size;
+
+	mutex_init(&host->lock);
+	spin_lock_init(&host->xfer);
+	host->mrq_workqueue = alloc_workqueue("mmc-%s", 0, 1,
+					      dev_name(&gbphy_dev->dev));
+	if (!host->mrq_workqueue) {
+		ret = -ENOMEM;
+		goto exit_connection_disable;
+	}
+	INIT_WORK(&host->mrqwork, gb_sdio_mrq_work);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto exit_wq_destroy;
+
+	ret = mmc_add_host(mmc);
+	if (ret < 0)
+		goto exit_wq_destroy;
+	host->removed = false;
+	ret = _gb_sdio_process_events(host, host->queued_events);
+	host->queued_events = 0;
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+
+	return ret;
+
+exit_wq_destroy:
+	destroy_workqueue(host->mrq_workqueue);
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+exit_mmc_free:
+	mmc_free_host(mmc);
+
+	return ret;
+}
+
+static void gb_sdio_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_sdio_host *host = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = host->connection;
+	struct mmc_host *mmc;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	mutex_lock(&host->lock);
+	host->removed = true;
+	mmc = host->mmc;
+	gb_connection_set_data(connection, NULL);
+	mutex_unlock(&host->lock);
+
+	destroy_workqueue(host->mrq_workqueue);
+	gb_connection_disable_rx(connection);
+	mmc_remove_host(mmc);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+	mmc_free_host(mmc);
+}
+
+static const struct gbphy_device_id gb_sdio_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_SDIO) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_sdio_id_table);
+
+static struct gbphy_driver sdio_driver = {
+	.name		= "sdio",
+	.probe		= gb_sdio_probe,
+	.remove		= gb_sdio_remove,
+	.id_table	= gb_sdio_id_table,
+};
+
+module_gbphy_driver(sdio_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/spi.c b/drivers/staging/greybus/spi.c
new file mode 100644
index 000000000..68e8d272d
--- /dev/null
+++ b/drivers/staging/greybus/spi.c
@@ -0,0 +1,78 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * SPI bridge PHY driver.
+ *
+ * Copyright 2014-2016 Google Inc.
+ * Copyright 2014-2016 Linaro Ltd.
+ */
+
+#include <linux/module.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+#include "spilib.h"
+
+static struct spilib_ops *spilib_ops;
+
+static int gb_spi_probe(struct gbphy_device *gbphy_dev,
+			const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	int ret;
+
+	connection = gb_connection_create(gbphy_dev->bundle,
+					  le16_to_cpu(gbphy_dev->cport_desc->id),
+					  NULL);
+	if (IS_ERR(connection))
+		return PTR_ERR(connection);
+
+	ret = gb_connection_enable(connection);
+	if (ret)
+		goto exit_connection_destroy;
+
+	ret = gb_spilib_master_init(connection, &gbphy_dev->dev, spilib_ops);
+	if (ret)
+		goto exit_connection_disable;
+
+	gb_gbphy_set_data(gbphy_dev, connection);
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+	return 0;
+
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+
+	return ret;
+}
+
+static void gb_spi_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_connection *connection = gb_gbphy_get_data(gbphy_dev);
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	gb_spilib_master_exit(connection);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+}
+
+static const struct gbphy_device_id gb_spi_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_SPI) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_spi_id_table);
+
+static struct gbphy_driver spi_driver = {
+	.name		= "spi",
+	.probe		= gb_spi_probe,
+	.remove		= gb_spi_remove,
+	.id_table	= gb_spi_id_table,
+};
+
+module_gbphy_driver(spi_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/spilib.c b/drivers/staging/greybus/spilib.c
new file mode 100644
index 000000000..ad0700a0b
--- /dev/null
+++ b/drivers/staging/greybus/spilib.c
@@ -0,0 +1,570 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus SPI library
+ *
+ * Copyright 2014-2016 Google Inc.
+ * Copyright 2014-2016 Linaro Ltd.
+ */
+
+#include <linux/bitops.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/greybus.h>
+#include <linux/spi/spi.h>
+
+#include "spilib.h"
+
+struct gb_spilib {
+	struct gb_connection	*connection;
+	struct device		*parent;
+	struct spi_transfer	*first_xfer;
+	struct spi_transfer	*last_xfer;
+	struct spilib_ops	*ops;
+	u32			rx_xfer_offset;
+	u32			tx_xfer_offset;
+	u32			last_xfer_size;
+	unsigned int		op_timeout;
+	u16			mode;
+	u16			flags;
+	u32			bits_per_word_mask;
+	u8			num_chipselect;
+	u32			min_speed_hz;
+	u32			max_speed_hz;
+};
+
+#define GB_SPI_STATE_MSG_DONE		((void *)0)
+#define GB_SPI_STATE_MSG_IDLE		((void *)1)
+#define GB_SPI_STATE_MSG_RUNNING	((void *)2)
+#define GB_SPI_STATE_OP_READY		((void *)3)
+#define GB_SPI_STATE_OP_DONE		((void *)4)
+#define GB_SPI_STATE_MSG_ERROR		((void *)-1)
+
+#define XFER_TIMEOUT_TOLERANCE		200
+
+static struct spi_master *get_master_from_spi(struct gb_spilib *spi)
+{
+	return gb_connection_get_data(spi->connection);
+}
+
+static int tx_header_fit_operation(u32 tx_size, u32 count, size_t data_max)
+{
+	size_t headers_size;
+
+	data_max -= sizeof(struct gb_spi_transfer_request);
+	headers_size = (count + 1) * sizeof(struct gb_spi_transfer);
+
+	return tx_size + headers_size > data_max ? 0 : 1;
+}
+
+static size_t calc_rx_xfer_size(u32 rx_size, u32 *tx_xfer_size, u32 len,
+				size_t data_max)
+{
+	size_t rx_xfer_size;
+
+	data_max -= sizeof(struct gb_spi_transfer_response);
+
+	if (rx_size + len > data_max)
+		rx_xfer_size = data_max - rx_size;
+	else
+		rx_xfer_size = len;
+
+	/* if this is a write_read, for symmetry read the same as write */
+	if (*tx_xfer_size && rx_xfer_size > *tx_xfer_size)
+		rx_xfer_size = *tx_xfer_size;
+	if (*tx_xfer_size && rx_xfer_size < *tx_xfer_size)
+		*tx_xfer_size = rx_xfer_size;
+
+	return rx_xfer_size;
+}
+
+static size_t calc_tx_xfer_size(u32 tx_size, u32 count, size_t len,
+				size_t data_max)
+{
+	size_t headers_size;
+
+	data_max -= sizeof(struct gb_spi_transfer_request);
+	headers_size = (count + 1) * sizeof(struct gb_spi_transfer);
+
+	if (tx_size + headers_size + len > data_max)
+		return data_max - (tx_size + sizeof(struct gb_spi_transfer));
+
+	return len;
+}
+
+static void clean_xfer_state(struct gb_spilib *spi)
+{
+	spi->first_xfer = NULL;
+	spi->last_xfer = NULL;
+	spi->rx_xfer_offset = 0;
+	spi->tx_xfer_offset = 0;
+	spi->last_xfer_size = 0;
+	spi->op_timeout = 0;
+}
+
+static bool is_last_xfer_done(struct gb_spilib *spi)
+{
+	struct spi_transfer *last_xfer = spi->last_xfer;
+
+	if ((spi->tx_xfer_offset + spi->last_xfer_size == last_xfer->len) ||
+	    (spi->rx_xfer_offset + spi->last_xfer_size == last_xfer->len))
+		return true;
+
+	return false;
+}
+
+static int setup_next_xfer(struct gb_spilib *spi, struct spi_message *msg)
+{
+	struct spi_transfer *last_xfer = spi->last_xfer;
+
+	if (msg->state != GB_SPI_STATE_OP_DONE)
+		return 0;
+
+	/*
+	 * if we transferred all content of the last transfer, reset values and
+	 * check if this was the last transfer in the message
+	 */
+	if (is_last_xfer_done(spi)) {
+		spi->tx_xfer_offset = 0;
+		spi->rx_xfer_offset = 0;
+		spi->op_timeout = 0;
+		if (last_xfer == list_last_entry(&msg->transfers,
+						 struct spi_transfer,
+						 transfer_list))
+			msg->state = GB_SPI_STATE_MSG_DONE;
+		else
+			spi->first_xfer = list_next_entry(last_xfer,
+							  transfer_list);
+		return 0;
+	}
+
+	spi->first_xfer = last_xfer;
+	if (last_xfer->tx_buf)
+		spi->tx_xfer_offset += spi->last_xfer_size;
+
+	if (last_xfer->rx_buf)
+		spi->rx_xfer_offset += spi->last_xfer_size;
+
+	return 0;
+}
+
+static struct spi_transfer *get_next_xfer(struct spi_transfer *xfer,
+					  struct spi_message *msg)
+{
+	if (xfer == list_last_entry(&msg->transfers, struct spi_transfer,
+				    transfer_list))
+		return NULL;
+
+	return list_next_entry(xfer, transfer_list);
+}
+
+/* Routines to transfer data */
+static struct gb_operation *gb_spi_operation_create(struct gb_spilib *spi,
+		struct gb_connection *connection, struct spi_message *msg)
+{
+	struct gb_spi_transfer_request *request;
+	struct spi_device *dev = msg->spi;
+	struct spi_transfer *xfer;
+	struct gb_spi_transfer *gb_xfer;
+	struct gb_operation *operation;
+	u32 tx_size = 0, rx_size = 0, count = 0, xfer_len = 0, request_size;
+	u32 tx_xfer_size = 0, rx_xfer_size = 0, len;
+	u32 total_len = 0;
+	unsigned int xfer_timeout;
+	size_t data_max;
+	void *tx_data;
+
+	data_max = gb_operation_get_payload_size_max(connection);
+	xfer = spi->first_xfer;
+
+	/* Find number of transfers queued and tx/rx length in the message */
+
+	while (msg->state != GB_SPI_STATE_OP_READY) {
+		msg->state = GB_SPI_STATE_MSG_RUNNING;
+		spi->last_xfer = xfer;
+
+		if (!xfer->tx_buf && !xfer->rx_buf) {
+			dev_err(spi->parent,
+				"bufferless transfer, length %u\n", xfer->len);
+			msg->state = GB_SPI_STATE_MSG_ERROR;
+			return NULL;
+		}
+
+		tx_xfer_size = 0;
+		rx_xfer_size = 0;
+
+		if (xfer->tx_buf) {
+			len = xfer->len - spi->tx_xfer_offset;
+			if (!tx_header_fit_operation(tx_size, count, data_max))
+				break;
+			tx_xfer_size = calc_tx_xfer_size(tx_size, count,
+							 len, data_max);
+			spi->last_xfer_size = tx_xfer_size;
+		}
+
+		if (xfer->rx_buf) {
+			len = xfer->len - spi->rx_xfer_offset;
+			rx_xfer_size = calc_rx_xfer_size(rx_size, &tx_xfer_size,
+							 len, data_max);
+			spi->last_xfer_size = rx_xfer_size;
+		}
+
+		tx_size += tx_xfer_size;
+		rx_size += rx_xfer_size;
+
+		total_len += spi->last_xfer_size;
+		count++;
+
+		xfer = get_next_xfer(xfer, msg);
+		if (!xfer || total_len >= data_max)
+			msg->state = GB_SPI_STATE_OP_READY;
+	}
+
+	/*
+	 * In addition to space for all message descriptors we need
+	 * to have enough to hold all tx data.
+	 */
+	request_size = sizeof(*request);
+	request_size += count * sizeof(*gb_xfer);
+	request_size += tx_size;
+
+	/* Response consists only of incoming data */
+	operation = gb_operation_create(connection, GB_SPI_TYPE_TRANSFER,
+					request_size, rx_size, GFP_KERNEL);
+	if (!operation)
+		return NULL;
+
+	request = operation->request->payload;
+	request->count = cpu_to_le16(count);
+	request->mode = dev->mode;
+	request->chip_select = dev->chip_select;
+
+	gb_xfer = &request->transfers[0];
+	tx_data = gb_xfer + count;	/* place tx data after last gb_xfer */
+
+	/* Fill in the transfers array */
+	xfer = spi->first_xfer;
+	while (msg->state != GB_SPI_STATE_OP_DONE) {
+		int xfer_delay;
+
+		if (xfer == spi->last_xfer)
+			xfer_len = spi->last_xfer_size;
+		else
+			xfer_len = xfer->len;
+
+		/* make sure we do not timeout in a slow transfer */
+		xfer_timeout = xfer_len * 8 * MSEC_PER_SEC / xfer->speed_hz;
+		xfer_timeout += GB_OPERATION_TIMEOUT_DEFAULT;
+
+		if (xfer_timeout > spi->op_timeout)
+			spi->op_timeout = xfer_timeout;
+
+		gb_xfer->speed_hz = cpu_to_le32(xfer->speed_hz);
+		gb_xfer->len = cpu_to_le32(xfer_len);
+		xfer_delay = spi_delay_to_ns(&xfer->delay, xfer) / 1000;
+		xfer_delay = clamp_t(u16, xfer_delay, 0, U16_MAX);
+		gb_xfer->delay_usecs = cpu_to_le16(xfer_delay);
+		gb_xfer->cs_change = xfer->cs_change;
+		gb_xfer->bits_per_word = xfer->bits_per_word;
+
+		/* Copy tx data */
+		if (xfer->tx_buf) {
+			gb_xfer->xfer_flags |= GB_SPI_XFER_WRITE;
+			memcpy(tx_data, xfer->tx_buf + spi->tx_xfer_offset,
+			       xfer_len);
+			tx_data += xfer_len;
+		}
+
+		if (xfer->rx_buf)
+			gb_xfer->xfer_flags |= GB_SPI_XFER_READ;
+
+		if (xfer == spi->last_xfer) {
+			if (!is_last_xfer_done(spi))
+				gb_xfer->xfer_flags |= GB_SPI_XFER_INPROGRESS;
+			msg->state = GB_SPI_STATE_OP_DONE;
+			continue;
+		}
+
+		gb_xfer++;
+		xfer = get_next_xfer(xfer, msg);
+	}
+
+	msg->actual_length += total_len;
+
+	return operation;
+}
+
+static void gb_spi_decode_response(struct gb_spilib *spi,
+				   struct spi_message *msg,
+				   struct gb_spi_transfer_response *response)
+{
+	struct spi_transfer *xfer = spi->first_xfer;
+	void *rx_data = response->data;
+	u32 xfer_len;
+
+	while (xfer) {
+		/* Copy rx data */
+		if (xfer->rx_buf) {
+			if (xfer == spi->first_xfer)
+				xfer_len = xfer->len - spi->rx_xfer_offset;
+			else if (xfer == spi->last_xfer)
+				xfer_len = spi->last_xfer_size;
+			else
+				xfer_len = xfer->len;
+
+			memcpy(xfer->rx_buf + spi->rx_xfer_offset, rx_data,
+			       xfer_len);
+			rx_data += xfer_len;
+		}
+
+		if (xfer == spi->last_xfer)
+			break;
+
+		xfer = list_next_entry(xfer, transfer_list);
+	}
+}
+
+static int gb_spi_transfer_one_message(struct spi_master *master,
+				       struct spi_message *msg)
+{
+	struct gb_spilib *spi = spi_master_get_devdata(master);
+	struct gb_connection *connection = spi->connection;
+	struct gb_spi_transfer_response *response;
+	struct gb_operation *operation;
+	int ret = 0;
+
+	spi->first_xfer = list_first_entry_or_null(&msg->transfers,
+						   struct spi_transfer,
+						   transfer_list);
+	if (!spi->first_xfer) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	msg->state = GB_SPI_STATE_MSG_IDLE;
+
+	while (msg->state != GB_SPI_STATE_MSG_DONE &&
+	       msg->state != GB_SPI_STATE_MSG_ERROR) {
+		operation = gb_spi_operation_create(spi, connection, msg);
+		if (!operation) {
+			msg->state = GB_SPI_STATE_MSG_ERROR;
+			ret = -EINVAL;
+			continue;
+		}
+
+		ret = gb_operation_request_send_sync_timeout(operation,
+							     spi->op_timeout);
+		if (!ret) {
+			response = operation->response->payload;
+			if (response)
+				gb_spi_decode_response(spi, msg, response);
+		} else {
+			dev_err(spi->parent,
+				"transfer operation failed: %d\n", ret);
+			msg->state = GB_SPI_STATE_MSG_ERROR;
+		}
+
+		gb_operation_put(operation);
+		setup_next_xfer(spi, msg);
+	}
+
+out:
+	msg->status = ret;
+	clean_xfer_state(spi);
+	spi_finalize_current_message(master);
+
+	return ret;
+}
+
+static int gb_spi_prepare_transfer_hardware(struct spi_master *master)
+{
+	struct gb_spilib *spi = spi_master_get_devdata(master);
+
+	return spi->ops->prepare_transfer_hardware(spi->parent);
+}
+
+static int gb_spi_unprepare_transfer_hardware(struct spi_master *master)
+{
+	struct gb_spilib *spi = spi_master_get_devdata(master);
+
+	spi->ops->unprepare_transfer_hardware(spi->parent);
+
+	return 0;
+}
+
+static int gb_spi_setup(struct spi_device *spi)
+{
+	/* Nothing to do for now */
+	return 0;
+}
+
+static void gb_spi_cleanup(struct spi_device *spi)
+{
+	/* Nothing to do for now */
+}
+
+/* Routines to get controller information */
+
+/*
+ * Map Greybus spi mode bits/flags/bpw into Linux ones.
+ * All bits are same for now and so these macro's return same values.
+ */
+#define gb_spi_mode_map(mode) mode
+#define gb_spi_flags_map(flags) flags
+
+static int gb_spi_get_master_config(struct gb_spilib *spi)
+{
+	struct gb_spi_master_config_response response;
+	u16 mode, flags;
+	int ret;
+
+	ret = gb_operation_sync(spi->connection, GB_SPI_TYPE_MASTER_CONFIG,
+				NULL, 0, &response, sizeof(response));
+	if (ret < 0)
+		return ret;
+
+	mode = le16_to_cpu(response.mode);
+	spi->mode = gb_spi_mode_map(mode);
+
+	flags = le16_to_cpu(response.flags);
+	spi->flags = gb_spi_flags_map(flags);
+
+	spi->bits_per_word_mask = le32_to_cpu(response.bits_per_word_mask);
+	spi->num_chipselect = response.num_chipselect;
+
+	spi->min_speed_hz = le32_to_cpu(response.min_speed_hz);
+	spi->max_speed_hz = le32_to_cpu(response.max_speed_hz);
+
+	return 0;
+}
+
+static int gb_spi_setup_device(struct gb_spilib *spi, u8 cs)
+{
+	struct spi_master *master = get_master_from_spi(spi);
+	struct gb_spi_device_config_request request;
+	struct gb_spi_device_config_response response;
+	struct spi_board_info spi_board = { {0} };
+	struct spi_device *spidev;
+	int ret;
+	u8 dev_type;
+
+	request.chip_select = cs;
+
+	ret = gb_operation_sync(spi->connection, GB_SPI_TYPE_DEVICE_CONFIG,
+				&request, sizeof(request),
+				&response, sizeof(response));
+	if (ret < 0)
+		return ret;
+
+	dev_type = response.device_type;
+
+	if (dev_type == GB_SPI_SPI_DEV)
+		strscpy(spi_board.modalias, "spidev",
+			sizeof(spi_board.modalias));
+	else if (dev_type == GB_SPI_SPI_NOR)
+		strscpy(spi_board.modalias, "spi-nor",
+			sizeof(spi_board.modalias));
+	else if (dev_type == GB_SPI_SPI_MODALIAS)
+		memcpy(spi_board.modalias, response.name,
+		       sizeof(spi_board.modalias));
+	else
+		return -EINVAL;
+
+	spi_board.mode		= le16_to_cpu(response.mode);
+	spi_board.bus_num	= master->bus_num;
+	spi_board.chip_select	= cs;
+	spi_board.max_speed_hz	= le32_to_cpu(response.max_speed_hz);
+
+	spidev = spi_new_device(master, &spi_board);
+	if (!spidev)
+		return -EINVAL;
+
+	return 0;
+}
+
+int gb_spilib_master_init(struct gb_connection *connection, struct device *dev,
+			  struct spilib_ops *ops)
+{
+	struct gb_spilib *spi;
+	struct spi_master *master;
+	int ret;
+	u8 i;
+
+	/* Allocate master with space for data */
+	master = spi_alloc_master(dev, sizeof(*spi));
+	if (!master) {
+		dev_err(dev, "cannot alloc SPI master\n");
+		return -ENOMEM;
+	}
+
+	spi = spi_master_get_devdata(master);
+	spi->connection = connection;
+	gb_connection_set_data(connection, master);
+	spi->parent = dev;
+	spi->ops = ops;
+
+	/* get master configuration */
+	ret = gb_spi_get_master_config(spi);
+	if (ret)
+		goto exit_spi_put;
+
+	master->bus_num = -1; /* Allow spi-core to allocate it dynamically */
+	master->num_chipselect = spi->num_chipselect;
+	master->mode_bits = spi->mode;
+	master->flags = spi->flags;
+	master->bits_per_word_mask = spi->bits_per_word_mask;
+
+	/* Attach methods */
+	master->cleanup = gb_spi_cleanup;
+	master->setup = gb_spi_setup;
+	master->transfer_one_message = gb_spi_transfer_one_message;
+
+	if (ops && ops->prepare_transfer_hardware) {
+		master->prepare_transfer_hardware =
+			gb_spi_prepare_transfer_hardware;
+	}
+
+	if (ops && ops->unprepare_transfer_hardware) {
+		master->unprepare_transfer_hardware =
+			gb_spi_unprepare_transfer_hardware;
+	}
+
+	master->auto_runtime_pm = true;
+
+	ret = spi_register_master(master);
+	if (ret < 0)
+		goto exit_spi_put;
+
+	/* now, fetch the devices configuration */
+	for (i = 0; i < spi->num_chipselect; i++) {
+		ret = gb_spi_setup_device(spi, i);
+		if (ret < 0) {
+			dev_err(dev, "failed to allocate spi device %d: %d\n",
+				i, ret);
+			goto exit_spi_unregister;
+		}
+	}
+
+	return 0;
+
+exit_spi_put:
+	spi_master_put(master);
+
+	return ret;
+
+exit_spi_unregister:
+	spi_unregister_master(master);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_spilib_master_init);
+
+void gb_spilib_master_exit(struct gb_connection *connection)
+{
+	struct spi_master *master = gb_connection_get_data(connection);
+
+	spi_unregister_master(master);
+}
+EXPORT_SYMBOL_GPL(gb_spilib_master_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/spilib.h b/drivers/staging/greybus/spilib.h
new file mode 100644
index 000000000..9d416839e
--- /dev/null
+++ b/drivers/staging/greybus/spilib.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Greybus SPI library header
+ *
+ * copyright 2016 google inc.
+ * copyright 2016 linaro ltd.
+ *
+ * released under the gplv2 only.
+ */
+
+#ifndef __SPILIB_H
+#define __SPILIB_H
+
+struct device;
+struct gb_connection;
+
+struct spilib_ops {
+	int (*prepare_transfer_hardware)(struct device *dev);
+	void (*unprepare_transfer_hardware)(struct device *dev);
+};
+
+int gb_spilib_master_init(struct gb_connection *connection,
+			  struct device *dev, struct spilib_ops *ops);
+void gb_spilib_master_exit(struct gb_connection *connection);
+
+#endif /* __SPILIB_H */
diff --git a/drivers/staging/greybus/tools/.gitignore b/drivers/staging/greybus/tools/.gitignore
new file mode 100644
index 000000000..1fd364aba
--- /dev/null
+++ b/drivers/staging/greybus/tools/.gitignore
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+loopback_test
diff --git a/drivers/staging/greybus/tools/Android.mk b/drivers/staging/greybus/tools/Android.mk
new file mode 100644
index 000000000..fdadbf611
--- /dev/null
+++ b/drivers/staging/greybus/tools/Android.mk
@@ -0,0 +1,10 @@
+LOCAL_PATH:= $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES:= loopback_test.c
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE := gb_loopback_test
+
+include $(BUILD_EXECUTABLE)
+
diff --git a/drivers/staging/greybus/tools/Makefile b/drivers/staging/greybus/tools/Makefile
new file mode 100644
index 000000000..a3bbd7317
--- /dev/null
+++ b/drivers/staging/greybus/tools/Makefile
@@ -0,0 +1,33 @@
+# SPDX-License-Identifier: GPL-2.0
+ifeq ($(strip $(V)), 1)
+  Q =
+else
+  Q = @
+endif
+
+CFLAGS	+= -std=gnu99 -Wall -Wextra -g \
+	    -D_GNU_SOURCE \
+	    -Wno-unused-parameter \
+	    -Wmaybe-uninitialized \
+	    -Wredundant-decls \
+	    -Wcast-align \
+	    -Wsign-compare \
+	    -Wno-missing-field-initializers \
+	    -Wno-shift-negative-value
+
+CC	:= $(CROSS_COMPILE)gcc
+
+TOOLS = loopback_test
+
+all: $(TOOLS)
+
+%.o: %.c ../greybus_protocols.h
+	@echo '  TARGET_CC $@'
+	$(Q)$(CC) $(CFLAGS) -c $< -o $@
+
+loopback_%: loopback_%.o
+	@echo '  TARGET_LD $@'
+	$(Q)$(CC) $(CFLAGS) $(LDFLAGS) $^ -o $@
+
+clean::
+	rm -f *.o $(TOOLS)
diff --git a/drivers/staging/greybus/tools/README.loopback b/drivers/staging/greybus/tools/README.loopback
new file mode 100644
index 000000000..070a510cb
--- /dev/null
+++ b/drivers/staging/greybus/tools/README.loopback
@@ -0,0 +1,198 @@
+
+
+        1 - LOOPBACK DRIVER
+
+The driver implements the main logic of the loopback test and provides
+sysfs files to configure the test and retrieve the results.
+A user could run a test without the need of the test application given
+that he understands the sysfs interface of the loopback driver.
+
+The loopback kernel driver needs to be loaded and at least one module
+with the loopback feature enabled must be present for the sysfs files to be
+created and for the loopback test application to be able to run.
+
+To load the module:
+# modprobe gb-loopback
+
+
+When the module is probed,  New files are available on the sysfs
+directory of the detected loopback device.
+(typically under "/sys/bus/graybus/devices").
+
+Here is a short summary of the sysfs interface files that should be visible:
+
+* Loopback Configuration Files:
+    async - Use asynchronous operations.
+    iteration_max - Number of tests iterations to perform.
+    size - payload size of the transfer.
+    timeout - The number of microseconds to give an individual
+              asynchronous request before timing out.
+    us_wait - Time to wait between 2 messages
+    type - By writing the test type to this file, the test starts.
+           Valid tests are:
+             0 stop the test
+             2 - ping
+             3 - transfer
+             4 - sink
+
+* Loopback feedback files:
+    error - number of errors that have occurred.
+    iteration_count - Number of iterations performed.
+    requests_completed - Number of requests successfully completed.
+    requests_timedout - Number of requests that have timed out.
+    timeout_max - Max allowed timeout
+    timeout_min - Min allowed timeout.
+
+* Loopback result files:
+    apbridge_unipro_latency_avg
+    apbridge_unipro_latency_max
+    apbridge_unipro_latency_min
+    gpbridge_firmware_latency_avg
+    gpbridge_firmware_latency_max
+    gpbridge_firmware_latency_min
+    requests_per_second_avg
+    requests_per_second_max
+    requests_per_second_min
+    latency_avg
+    latency_max
+    latency_min
+    throughput_avg
+    throughput_max
+    throughput_min
+
+
+
+            2 - LOOPBACK TEST APPLICATION
+
+The loopback test application manages and formats the results provided by
+the loopback kernel module. The purpose of this application
+is to:
+    - Start and manage multiple loopback device tests concurrently.
+    - Calculate the aggregate results for multiple devices.
+    - Gather and format test results (csv or human readable).
+
+The best way to get up to date usage information for the application is
+usually to pass the "-h" parameter.
+Here is the summary of the available options:
+
+ Mandatory arguments
+   -t     must be one of the test names - sink, transfer or ping
+   -i     iteration count - the number of iterations to run the test over
+ Optional arguments
+   -S     sysfs location - location for greybus 'endo' entries default /sys/bus/greybus/devices/
+   -D     debugfs location - location for loopback debugfs entries default /sys/kernel/debug/gb_loopback/
+   -s     size of data packet to send during test - defaults to zero
+   -m     mask - a bit mask of connections to include example: -m 8 = 4th connection -m 9 = 1st and 4th connection etc
+                 default is zero which means broadcast to all connections
+   -v     verbose output
+   -d     debug output
+   -r     raw data output - when specified the full list of latency values are included in the output CSV
+   -p     porcelain - when specified printout is in a user-friendly non-CSV format. This option suppresses writing to CSV file
+   -a     aggregate - show aggregation of all enabled devies
+   -l     list found loopback devices and exit.
+   -x     Async - Enable async transfers.
+   -o     Timeout - Timeout in microseconds for async operations.
+
+
+
+             3 - REAL WORLD EXAMPLE USAGES
+
+ 3.1 - Using the driver sysfs files to run a test on a single device:
+
+* Run a 1000 transfers of a 100 byte packet. Each transfer is started only
+after the previous one finished successfully:
+    echo 0 > /sys/bus/greybus/devices/1-2.17/type
+    echo 0 > /sys/bus/greybus/devices/1-2.17/async
+    echo 2000 > /sys/bus/greybus/devices/1-2.17/us_wait
+    echo 100 > /sys/bus/greybus/devices/1-2.17/size
+    echo 1000 > /sys/bus/greybus/devices/1-2.17/iteration_max
+    echo 0 > /sys/bus/greybus/devices/1-2.17/mask
+    echo 200000 > /sys/bus/greybus/devices/1-2.17/timeout
+    echo 3 > /sys/bus/greybus/devices/1-2.17/type
+
+* Run a 1000 transfers of a 100 byte packet. Transfers are started without
+waiting for the previous one to finish:
+    echo 0 > /sys/bus/greybus/devices/1-2.17/type
+    echo 3 > /sys/bus/greybus/devices/1-2.17/async
+    echo 0 > /sys/bus/greybus/devices/1-2.17/us_wait
+    echo 100 > /sys/bus/greybus/devices/1-2.17/size
+    echo 1000 > /sys/bus/greybus/devices/1-2.17/iteration_max
+    echo 0 > /sys/bus/greybus/devices/1-2.17/mask
+    echo 200000 > /sys/bus/greybus/devices/1-2.17/timeout
+    echo 3 > /sys/bus/greybus/devices/1-2.17/type
+
+* Read the results from sysfs:
+    cat /sys/bus/greybus/devices/1-2.17/requests_per_second_min
+    cat /sys/bus/greybus/devices/1-2.17/requests_per_second_max
+    cat /sys/bus/greybus/devices/1-2.17/requests_per_second_avg
+
+    cat /sys/bus/greybus/devices/1-2.17/latency_min
+    cat /sys/bus/greybus/devices/1-2.17/latency_max
+    cat /sys/bus/greybus/devices/1-2.17/latency_avg
+
+    cat /sys/bus/greybus/devices/1-2.17/apbridge_unipro_latency_min
+    cat /sys/bus/greybus/devices/1-2.17/apbridge_unipro_latency_max
+    cat /sys/bus/greybus/devices/1-2.17/apbridge_unipro_latency_avg
+
+    cat /sys/bus/greybus/devices/1-2.17/gpbridge_firmware_latency_min
+    cat /sys/bus/greybus/devices/1-2.17/gpbridge_firmware_latency_max
+    cat /sys/bus/greybus/devices/1-2.17/gpbridge_firmware_latency_avg
+
+    cat /sys/bus/greybus/devices/1-2.17/error
+    cat /sys/bus/greybus/devices/1-2.17/requests_completed
+    cat /sys/bus/greybus/devices/1-2.17/requests_timedout
+
+
+3.2 - using the test application:
+
+* Run a transfer test 10 iterations of size 100 bytes on all available devices
+    #/loopback_test -t transfer -i 10 -s 100
+    1970-1-1 0:10:7,transfer,1-4.17,100,10,0,443,509,471.700012,66,1963,2256,2124.600098,293,102776,118088,109318.898438,15312,1620,1998,1894.099976,378,56,57,56.799999,1
+    1970-1-1 0:10:7,transfer,1-5.17,100,10,0,399,542,463.399994,143,1845,2505,2175.800049,660,92568,125744,107393.296875,33176,1469,2305,1806.500000,836,56,57,56.799999,1
+
+
+* Show the aggregate results of both devices. ("-a")
+    #/loopback_test -t transfer -i 10 -s 100  -a
+    1970-1-1 0:10:35,transfer,1-4.17,100,10,0,448,580,494.100006,132,1722,2230,2039.400024,508,103936,134560,114515.703125,30624,1513,1980,1806.900024,467,56,57,57.299999,1
+    1970-1-1 0:10:35,transfer,1-5.17,100,10,0,383,558,478.600006,175,1791,2606,2115.199951,815,88856,129456,110919.703125,40600,1457,2246,1773.599976,789,56,57,57.099998,1
+    1970-1-1 0:10:35,transfer,aggregate,100,10,0,383,580,486.000000,197,1722,2606,2077.000000,884,88856,134560,112717.000000,45704,1457,2246,1789.000000,789,56,57,57.000000,1
+
+* Example usage of the mask option to select which devices will
+  run the test (1st, 2nd, or both devices):
+    # /loopback_test -t transfer -i 10 -s 100 -m 1
+    1970-1-1 0:11:56,transfer,1-4.17,100,10,0,514,558,544.900024,44,1791,1943,1836.599976,152,119248,129456,126301.296875,10208,1600,1001609,101613.601562,1000009,56,57,56.900002,1
+    # /loopback_test -t transfer -i 10 -s 100 -m 2
+    1970-1-1 0:12:0,transfer,1-5.17,100,10,0,468,554,539.000000,86,1804,2134,1859.500000,330,108576,128528,124932.500000,19952,1606,1626,1619.300049,20,56,57,57.400002,1
+    # /loopback_test -t transfer -i 10 -s 100 -m 3
+    1970-1-1 0:12:3,transfer,1-4.17,100,10,0,432,510,469.399994,78,1959,2313,2135.800049,354,100224,118320,108785.296875,18096,1610,2024,1893.500000,414,56,57,57.200001,1
+    1970-1-1 0:12:3,transfer,1-5.17,100,10,0,404,542,468.799988,138,1843,2472,2152.500000,629,93728,125744,108646.101562,32016,1504,2247,1853.099976,743,56,57,57.099998,1
+
+* Show output in human readable format ("-p")
+    # /loopback_test -t transfer -i 10 -s 100 -m 3 -p
+
+    1970-1-1 0:12:37
+    test:                       transfer
+    path:                       1-4.17
+    size:                       100
+    iterations:                 10
+    errors:                     0
+    async:                      Disabled
+    requests per-sec:           min=390, max=547, average=469.299988, jitter=157
+    ap-throughput B/s:          min=90480 max=126904 average=108762.101562 jitter=36424
+    ap-latency usec:            min=1826 max=2560 average=2146.000000 jitter=734
+    apbridge-latency usec:      min=1620 max=1982 average=1882.099976 jitter=362
+    gpbridge-latency usec:      min=56 max=57 average=57.099998 jitter=1
+
+
+    1970-1-1 0:12:37
+    test:                       transfer
+    path:                       1-5.17
+    size:                       100
+    iterations:                 10
+    errors:                     0
+    async:                      Disabled
+    requests per-sec:           min=397, max=538, average=461.700012, jitter=141
+    ap-throughput B/s:          min=92104 max=124816 average=106998.898438 jitter=32712
+    ap-latency usec:            min=1856 max=2514 average=2185.699951 jitter=658
+    apbridge-latency usec:      min=1460 max=2296 average=1828.599976 jitter=836
+    gpbridge-latency usec:      min=56 max=57 average=57.099998 jitter=1
diff --git a/drivers/staging/greybus/tools/lbtest b/drivers/staging/greybus/tools/lbtest
new file mode 100755
index 000000000..47c481239
--- /dev/null
+++ b/drivers/staging/greybus/tools/lbtest
@@ -0,0 +1,169 @@
+#!/usr/bin/env python
+# SPDX-License-Identifier: BSD-3-Clause
+
+# Copyright (c) 2015 Google, Inc.
+# Copyright (c) 2015 Linaro, Ltd.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# 3. Neither the name of the copyright holder nor the names of its
+# contributors may be used to endorse or promote products derived from this
+# software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+# PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+# OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+# OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+import csv
+import datetime
+import sys
+import time
+
+dict = {'ping': '2', 'transfer': '3', 'sink': '4'}
+verbose = 1
+
+def abort():
+	sys.exit(1)
+
+def usage():
+	print('Usage: looptest TEST SIZE ITERATIONS PATH\n\n'
+	'  Run TEST for a number of ITERATIONS with operation data SIZE bytes\n'
+	'  TEST may be \'ping\' \'transfer\' or \'sink\'\n'
+	'  SIZE indicates the size of transfer <= greybus max payload bytes\n'
+	'  ITERATIONS indicates the number of times to execute TEST at SIZE bytes\n'
+	'             Note if ITERATIONS is set to zero then this utility will\n'
+	'             initiate an infinite (non terminating) test and exit\n'
+	'             without logging any metrics data\n'
+	'  PATH indicates the sysfs path for the loopback greybus entries e.g.\n'
+	'             /sys/bus/greybus/devices/endo0:1:1:1:1/\n'
+	'Examples:\n'
+	'  looptest transfer 128 10000\n'
+	'  looptest ping 0 128\n'
+	'  looptest sink 2030 32768\n'
+	.format(sys.argv[0]), file=sys.stderr)
+
+	abort()
+
+def read_sysfs_int(path):
+	try:
+		f = open(path, "r");
+		val = f.read();
+		f.close()
+		return int(val)
+	except IOError as e:
+		print("I/O error({0}): {1}".format(e.errno, e.strerror))
+		print("Invalid path %s" % path)
+
+def write_sysfs_val(path, val):
+	try:
+		f = open(path, "r+")
+		f.write(val)
+		f.close()
+	except IOError as e:
+		print("I/O error({0}): {1}".format(e.errno, e.strerror))
+		print("Invalid path %s" % path)
+
+def log_csv(test_name, size, iteration_max, sys_pfx):
+	# file name will test_name_size_iteration_max.csv
+	# every time the same test with the same parameters is run we will then
+	# append to the same CSV with datestamp - representing each test dataset
+	fname = test_name + '_' + size + '_' + str(iteration_max) + '.csv'
+
+	try:
+		# gather data set
+		date = str(datetime.datetime.now())
+		error = read_sysfs_int(sys_pfx + 'error')
+		request_min = read_sysfs_int(sys_pfx + 'requests_per_second_min')
+		request_max = read_sysfs_int(sys_pfx + 'requests_per_second_max')
+		request_avg = read_sysfs_int(sys_pfx + 'requests_per_second_avg')
+		latency_min = read_sysfs_int(sys_pfx + 'latency_min')
+		latency_max = read_sysfs_int(sys_pfx + 'latency_max')
+		latency_avg = read_sysfs_int(sys_pfx + 'latency_avg')
+		throughput_min = read_sysfs_int(sys_pfx + 'throughput_min')
+		throughput_max = read_sysfs_int(sys_pfx + 'throughput_max')
+		throughput_avg = read_sysfs_int(sys_pfx + 'throughput_avg')
+
+		# derive jitter
+		request_jitter = request_max - request_min
+		latency_jitter = latency_max - latency_min
+		throughput_jitter = throughput_max - throughput_min
+
+		# append data set to file
+		with open(fname, 'a') as csvf:
+			row = csv.writer(csvf, delimiter=",", quotechar="'",
+					quoting=csv.QUOTE_MINIMAL)
+			row.writerow([date, test_name, size, iteration_max, error,
+					request_min, request_max, request_avg, request_jitter,
+					latency_min, latency_max, latency_avg, latency_jitter,
+					throughput_min, throughput_max, throughput_avg, throughput_jitter])
+	except IOError as e:
+		print("I/O error({0}): {1}".format(e.errno, e.strerror))
+
+def loopback_run(test_name, size, iteration_max, sys_pfx):
+	test_id = dict[test_name]
+	try:
+		# Terminate any currently running test
+		write_sysfs_val(sys_pfx + 'type', '0')
+		# Set parameter for no wait between messages
+		write_sysfs_val(sys_pfx + 'ms_wait', '0')
+		# Set operation size
+		write_sysfs_val(sys_pfx + 'size', size)
+		# Set iterations
+		write_sysfs_val(sys_pfx + 'iteration_max', str(iteration_max))
+		# Initiate by setting loopback operation type
+		write_sysfs_val(sys_pfx + 'type', test_id)
+		time.sleep(1)
+
+		if iteration_max == 0:
+			print ("Infinite test initiated CSV won't be logged\n")
+			return
+
+		previous = 0
+		err = 0
+		while True:
+			# get current count bail out if it hasn't changed
+			iteration_count = read_sysfs_int(sys_pfx + 'iteration_count')
+			if previous == iteration_count:
+				err = 1
+				break
+			elif iteration_count == iteration_max:
+				break
+			previous = iteration_count
+			if verbose:
+				print('%02d%% complete %d of %d ' %
+					(100 * iteration_count / iteration_max,
+					iteration_count, iteration_max))
+			time.sleep(1)
+		if err:
+			print ('\nError executing test\n')
+		else:
+			log_csv(test_name, size, iteration_max, sys_pfx)
+	except ValueError as ve:
+		print("Error: %s " % format(e.strerror), file=sys.stderr)
+		abort()
+
+def main():
+	if len(sys.argv) < 5:
+		usage()
+
+	if sys.argv[1] in dict.keys():
+		loopback_run(sys.argv[1], sys.argv[2], int(sys.argv[3]), sys.argv[4])
+	else:
+		usage()
+if __name__ == '__main__':
+	main()
diff --git a/drivers/staging/greybus/tools/loopback_test.c b/drivers/staging/greybus/tools/loopback_test.c
new file mode 100644
index 000000000..4c42e393c
--- /dev/null
+++ b/drivers/staging/greybus/tools/loopback_test.c
@@ -0,0 +1,992 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Loopback test application
+ *
+ * Copyright 2015 Google Inc.
+ * Copyright 2015 Linaro Ltd.
+ */
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <poll.h>
+#include <sys/types.h>
+#include <time.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <signal.h>
+
+#define MAX_NUM_DEVICES 10
+#define MAX_SYSFS_PREFIX 0x80
+#define MAX_SYSFS_PATH	0x200
+#define CSV_MAX_LINE	0x1000
+#define SYSFS_MAX_INT	0x20
+#define MAX_STR_LEN	255
+#define DEFAULT_ASYNC_TIMEOUT 200000
+
+struct dict {
+	char *name;
+	int type;
+};
+
+static struct dict dict[] = {
+	{"ping", 2},
+	{"transfer", 3},
+	{"sink", 4},
+	{NULL,}		/* list termination */
+};
+
+struct loopback_results {
+	float latency_avg;
+	uint32_t latency_max;
+	uint32_t latency_min;
+	uint32_t latency_jitter;
+
+	float request_avg;
+	uint32_t request_max;
+	uint32_t request_min;
+	uint32_t request_jitter;
+
+	float throughput_avg;
+	uint32_t throughput_max;
+	uint32_t throughput_min;
+	uint32_t throughput_jitter;
+
+	float apbridge_unipro_latency_avg;
+	uint32_t apbridge_unipro_latency_max;
+	uint32_t apbridge_unipro_latency_min;
+	uint32_t apbridge_unipro_latency_jitter;
+
+	float gbphy_firmware_latency_avg;
+	uint32_t gbphy_firmware_latency_max;
+	uint32_t gbphy_firmware_latency_min;
+	uint32_t gbphy_firmware_latency_jitter;
+
+	uint32_t error;
+};
+
+struct loopback_device {
+	char name[MAX_STR_LEN];
+	char sysfs_entry[MAX_SYSFS_PATH];
+	char debugfs_entry[MAX_SYSFS_PATH];
+	struct loopback_results results;
+};
+
+struct loopback_test {
+	int verbose;
+	int debug;
+	int raw_data_dump;
+	int porcelain;
+	int mask;
+	int size;
+	int iteration_max;
+	int aggregate_output;
+	int test_id;
+	int device_count;
+	int list_devices;
+	int use_async;
+	int async_timeout;
+	int async_outstanding_operations;
+	int us_wait;
+	int file_output;
+	int stop_all;
+	int poll_count;
+	char test_name[MAX_STR_LEN];
+	char sysfs_prefix[MAX_SYSFS_PREFIX];
+	char debugfs_prefix[MAX_SYSFS_PREFIX];
+	struct timespec poll_timeout;
+	struct loopback_device devices[MAX_NUM_DEVICES];
+	struct loopback_results aggregate_results;
+	struct pollfd fds[MAX_NUM_DEVICES];
+};
+
+struct loopback_test t;
+
+/* Helper macros to calculate the aggregate results for all devices */
+static inline int device_enabled(struct loopback_test *t, int dev_idx);
+
+#define GET_MAX(field)							\
+static int get_##field##_aggregate(struct loopback_test *t)		\
+{									\
+	uint32_t max = 0;						\
+	int i;								\
+	for (i = 0; i < t->device_count; i++) {				\
+		if (!device_enabled(t, i))				\
+			continue;					\
+		if (t->devices[i].results.field > max)			\
+			max = t->devices[i].results.field;		\
+	}								\
+	return max;							\
+}									\
+
+#define GET_MIN(field)							\
+static int get_##field##_aggregate(struct loopback_test *t)		\
+{									\
+	uint32_t min = ~0;						\
+	int i;								\
+	for (i = 0; i < t->device_count; i++) {				\
+		if (!device_enabled(t, i))				\
+			continue;					\
+		if (t->devices[i].results.field < min)			\
+			min = t->devices[i].results.field;		\
+	}								\
+	return min;							\
+}									\
+
+#define GET_AVG(field)							\
+static int get_##field##_aggregate(struct loopback_test *t)		\
+{									\
+	uint32_t val = 0;						\
+	uint32_t count = 0;						\
+	int i;								\
+	for (i = 0; i < t->device_count; i++) {				\
+		if (!device_enabled(t, i))				\
+			continue;					\
+		count++;						\
+		val += t->devices[i].results.field;			\
+	}								\
+	if (count)							\
+		val /= count;						\
+	return val;							\
+}									\
+
+GET_MAX(throughput_max);
+GET_MAX(request_max);
+GET_MAX(latency_max);
+GET_MAX(apbridge_unipro_latency_max);
+GET_MAX(gbphy_firmware_latency_max);
+GET_MIN(throughput_min);
+GET_MIN(request_min);
+GET_MIN(latency_min);
+GET_MIN(apbridge_unipro_latency_min);
+GET_MIN(gbphy_firmware_latency_min);
+GET_AVG(throughput_avg);
+GET_AVG(request_avg);
+GET_AVG(latency_avg);
+GET_AVG(apbridge_unipro_latency_avg);
+GET_AVG(gbphy_firmware_latency_avg);
+
+void abort(void)
+{
+	_exit(1);
+}
+
+void usage(void)
+{
+	fprintf(stderr, "Usage: loopback_test TEST [SIZE] ITERATIONS [SYSPATH] [DBGPATH]\n\n"
+	"  Run TEST for a number of ITERATIONS with operation data SIZE bytes\n"
+	"  TEST may be \'ping\' \'transfer\' or \'sink\'\n"
+	"  SIZE indicates the size of transfer <= greybus max payload bytes\n"
+	"  ITERATIONS indicates the number of times to execute TEST at SIZE bytes\n"
+	"             Note if ITERATIONS is set to zero then this utility will\n"
+	"             initiate an infinite (non terminating) test and exit\n"
+	"             without logging any metrics data\n"
+	"  SYSPATH indicates the sysfs path for the loopback greybus entries e.g.\n"
+	"          /sys/bus/greybus/devices\n"
+	"  DBGPATH indicates the debugfs path for the loopback greybus entries e.g.\n"
+	"          /sys/kernel/debug/gb_loopback/\n"
+	" Mandatory arguments\n"
+	"   -t     must be one of the test names - sink, transfer or ping\n"
+	"   -i     iteration count - the number of iterations to run the test over\n"
+	" Optional arguments\n"
+	"   -S     sysfs location - location for greybus 'endo' entries default /sys/bus/greybus/devices/\n"
+	"   -D     debugfs location - location for loopback debugfs entries default /sys/kernel/debug/gb_loopback/\n"
+	"   -s     size of data packet to send during test - defaults to zero\n"
+	"   -m     mask - a bit mask of connections to include example: -m 8 = 4th connection -m 9 = 1st and 4th connection etc\n"
+	"                 default is zero which means broadcast to all connections\n"
+	"   -v     verbose output\n"
+	"   -d     debug output\n"
+	"   -r     raw data output - when specified the full list of latency values are included in the output CSV\n"
+	"   -p     porcelain - when specified printout is in a user-friendly non-CSV format. This option suppresses writing to CSV file\n"
+	"   -a     aggregate - show aggregation of all enabled devices\n"
+	"   -l     list found loopback devices and exit\n"
+	"   -x     Async - Enable async transfers\n"
+	"   -o     Async Timeout - Timeout in uSec for async operations\n"
+	"   -O     Poll loop time out in seconds(max time a test is expected to last, default: 30sec)\n"
+	"   -c     Max number of outstanding operations for async operations\n"
+	"   -w     Wait in uSec between operations\n"
+	"   -z     Enable output to a CSV file (incompatible with -p)\n"
+	"   -f     When starting new loopback test, stop currently running tests on all devices\n"
+	"Examples:\n"
+	"  Send 10000 transfers with a packet size of 128 bytes to all active connections\n"
+	"  loopback_test -t transfer -s 128 -i 10000 -S /sys/bus/greybus/devices/ -D /sys/kernel/debug/gb_loopback/\n"
+	"  loopback_test -t transfer -s 128 -i 10000 -m 0\n"
+	"  Send 10000 transfers with a packet size of 128 bytes to connection 1 and 4\n"
+	"  loopback_test -t transfer -s 128 -i 10000 -m 9\n"
+	"  loopback_test -t ping -s 0 128 -i -S /sys/bus/greybus/devices/ -D /sys/kernel/debug/gb_loopback/\n"
+	"  loopback_test -t sink -s 2030 -i 32768 -S /sys/bus/greybus/devices/ -D /sys/kernel/debug/gb_loopback/\n");
+	abort();
+}
+
+static inline int device_enabled(struct loopback_test *t, int dev_idx)
+{
+	if (!t->mask || (t->mask & (1 << dev_idx)))
+		return 1;
+
+	return 0;
+}
+
+static void show_loopback_devices(struct loopback_test *t)
+{
+	int i;
+
+	if (t->device_count == 0) {
+		printf("No loopback devices.\n");
+		return;
+	}
+
+	for (i = 0; i < t->device_count; i++)
+		printf("device[%d] = %s\n", i, t->devices[i].name);
+
+}
+
+int open_sysfs(const char *sys_pfx, const char *node, int flags)
+{
+	int fd;
+	char path[MAX_SYSFS_PATH];
+
+	snprintf(path, sizeof(path), "%s%s", sys_pfx, node);
+	fd = open(path, flags);
+	if (fd < 0) {
+		fprintf(stderr, "unable to open %s\n", path);
+		abort();
+	}
+	return fd;
+}
+
+int read_sysfs_int_fd(int fd, const char *sys_pfx, const char *node)
+{
+	char buf[SYSFS_MAX_INT];
+
+	if (read(fd, buf, sizeof(buf)) < 0) {
+		fprintf(stderr, "unable to read from %s%s %s\n", sys_pfx, node,
+			strerror(errno));
+		close(fd);
+		abort();
+	}
+	return atoi(buf);
+}
+
+float read_sysfs_float_fd(int fd, const char *sys_pfx, const char *node)
+{
+	char buf[SYSFS_MAX_INT];
+
+	if (read(fd, buf, sizeof(buf)) < 0) {
+
+		fprintf(stderr, "unable to read from %s%s %s\n", sys_pfx, node,
+			strerror(errno));
+		close(fd);
+		abort();
+	}
+	return atof(buf);
+}
+
+int read_sysfs_int(const char *sys_pfx, const char *node)
+{
+	int fd, val;
+
+	fd = open_sysfs(sys_pfx, node, O_RDONLY);
+	val = read_sysfs_int_fd(fd, sys_pfx, node);
+	close(fd);
+	return val;
+}
+
+float read_sysfs_float(const char *sys_pfx, const char *node)
+{
+	int fd;
+	float val;
+
+	fd = open_sysfs(sys_pfx, node, O_RDONLY);
+	val = read_sysfs_float_fd(fd, sys_pfx, node);
+	close(fd);
+	return val;
+}
+
+void write_sysfs_val(const char *sys_pfx, const char *node, int val)
+{
+	int fd, len;
+	char buf[SYSFS_MAX_INT];
+
+	fd = open_sysfs(sys_pfx, node, O_RDWR);
+	len = snprintf(buf, sizeof(buf), "%d", val);
+	if (write(fd, buf, len) < 0) {
+		fprintf(stderr, "unable to write to %s%s %s\n", sys_pfx, node,
+			strerror(errno));
+		close(fd);
+		abort();
+	}
+	close(fd);
+}
+
+static int get_results(struct loopback_test *t)
+{
+	struct loopback_device *d;
+	struct loopback_results *r;
+	int i;
+
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		d = &t->devices[i];
+		r = &d->results;
+
+		r->error = read_sysfs_int(d->sysfs_entry, "error");
+		r->request_min = read_sysfs_int(d->sysfs_entry, "requests_per_second_min");
+		r->request_max = read_sysfs_int(d->sysfs_entry, "requests_per_second_max");
+		r->request_avg = read_sysfs_float(d->sysfs_entry, "requests_per_second_avg");
+
+		r->latency_min = read_sysfs_int(d->sysfs_entry, "latency_min");
+		r->latency_max = read_sysfs_int(d->sysfs_entry, "latency_max");
+		r->latency_avg = read_sysfs_float(d->sysfs_entry, "latency_avg");
+
+		r->throughput_min = read_sysfs_int(d->sysfs_entry, "throughput_min");
+		r->throughput_max = read_sysfs_int(d->sysfs_entry, "throughput_max");
+		r->throughput_avg = read_sysfs_float(d->sysfs_entry, "throughput_avg");
+
+		r->apbridge_unipro_latency_min =
+			read_sysfs_int(d->sysfs_entry, "apbridge_unipro_latency_min");
+		r->apbridge_unipro_latency_max =
+			read_sysfs_int(d->sysfs_entry, "apbridge_unipro_latency_max");
+		r->apbridge_unipro_latency_avg =
+			read_sysfs_float(d->sysfs_entry, "apbridge_unipro_latency_avg");
+
+		r->gbphy_firmware_latency_min =
+			read_sysfs_int(d->sysfs_entry, "gbphy_firmware_latency_min");
+		r->gbphy_firmware_latency_max =
+			read_sysfs_int(d->sysfs_entry, "gbphy_firmware_latency_max");
+		r->gbphy_firmware_latency_avg =
+			read_sysfs_float(d->sysfs_entry, "gbphy_firmware_latency_avg");
+
+		r->request_jitter = r->request_max - r->request_min;
+		r->latency_jitter = r->latency_max - r->latency_min;
+		r->throughput_jitter = r->throughput_max - r->throughput_min;
+		r->apbridge_unipro_latency_jitter =
+			r->apbridge_unipro_latency_max - r->apbridge_unipro_latency_min;
+		r->gbphy_firmware_latency_jitter =
+			r->gbphy_firmware_latency_max - r->gbphy_firmware_latency_min;
+
+	}
+
+	/*calculate the aggregate results of all enabled devices */
+	if (t->aggregate_output) {
+		r = &t->aggregate_results;
+
+		r->request_min = get_request_min_aggregate(t);
+		r->request_max = get_request_max_aggregate(t);
+		r->request_avg = get_request_avg_aggregate(t);
+
+		r->latency_min = get_latency_min_aggregate(t);
+		r->latency_max = get_latency_max_aggregate(t);
+		r->latency_avg = get_latency_avg_aggregate(t);
+
+		r->throughput_min = get_throughput_min_aggregate(t);
+		r->throughput_max = get_throughput_max_aggregate(t);
+		r->throughput_avg = get_throughput_avg_aggregate(t);
+
+		r->apbridge_unipro_latency_min =
+			get_apbridge_unipro_latency_min_aggregate(t);
+		r->apbridge_unipro_latency_max =
+			get_apbridge_unipro_latency_max_aggregate(t);
+		r->apbridge_unipro_latency_avg =
+			get_apbridge_unipro_latency_avg_aggregate(t);
+
+		r->gbphy_firmware_latency_min =
+			get_gbphy_firmware_latency_min_aggregate(t);
+		r->gbphy_firmware_latency_max =
+			get_gbphy_firmware_latency_max_aggregate(t);
+		r->gbphy_firmware_latency_avg =
+			get_gbphy_firmware_latency_avg_aggregate(t);
+
+		r->request_jitter = r->request_max - r->request_min;
+		r->latency_jitter = r->latency_max - r->latency_min;
+		r->throughput_jitter = r->throughput_max - r->throughput_min;
+		r->apbridge_unipro_latency_jitter =
+			r->apbridge_unipro_latency_max - r->apbridge_unipro_latency_min;
+		r->gbphy_firmware_latency_jitter =
+			r->gbphy_firmware_latency_max - r->gbphy_firmware_latency_min;
+
+	}
+
+	return 0;
+}
+
+int format_output(struct loopback_test *t,
+		  struct loopback_results *r,
+		  const char *dev_name,
+		  char *buf, int buf_len,
+		  struct tm *tm)
+{
+	int len = 0;
+
+	memset(buf, 0x00, buf_len);
+	len = snprintf(buf, buf_len, "%u-%u-%u %u:%u:%u",
+		       tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday,
+		       tm->tm_hour, tm->tm_min, tm->tm_sec);
+
+	if (t->porcelain) {
+		len += snprintf(&buf[len], buf_len - len,
+			"\n test:\t\t\t%s\n path:\t\t\t%s\n size:\t\t\t%u\n iterations:\t\t%u\n errors:\t\t%u\n async:\t\t\t%s\n",
+			t->test_name,
+			dev_name,
+			t->size,
+			t->iteration_max,
+			r->error,
+			t->use_async ? "Enabled" : "Disabled");
+
+		len += snprintf(&buf[len], buf_len - len,
+			" requests per-sec:\tmin=%u, max=%u, average=%f, jitter=%u\n",
+			r->request_min,
+			r->request_max,
+			r->request_avg,
+			r->request_jitter);
+
+		len += snprintf(&buf[len], buf_len - len,
+			" ap-throughput B/s:\tmin=%u max=%u average=%f jitter=%u\n",
+			r->throughput_min,
+			r->throughput_max,
+			r->throughput_avg,
+			r->throughput_jitter);
+		len += snprintf(&buf[len], buf_len - len,
+			" ap-latency usec:\tmin=%u max=%u average=%f jitter=%u\n",
+			r->latency_min,
+			r->latency_max,
+			r->latency_avg,
+			r->latency_jitter);
+		len += snprintf(&buf[len], buf_len - len,
+			" apbridge-latency usec:\tmin=%u max=%u average=%f jitter=%u\n",
+			r->apbridge_unipro_latency_min,
+			r->apbridge_unipro_latency_max,
+			r->apbridge_unipro_latency_avg,
+			r->apbridge_unipro_latency_jitter);
+
+		len += snprintf(&buf[len], buf_len - len,
+			" gbphy-latency usec:\tmin=%u max=%u average=%f jitter=%u\n",
+			r->gbphy_firmware_latency_min,
+			r->gbphy_firmware_latency_max,
+			r->gbphy_firmware_latency_avg,
+			r->gbphy_firmware_latency_jitter);
+
+	} else {
+		len += snprintf(&buf[len], buf_len - len, ",%s,%s,%u,%u,%u",
+			t->test_name, dev_name, t->size, t->iteration_max,
+			r->error);
+
+		len += snprintf(&buf[len], buf_len - len, ",%u,%u,%f,%u",
+			r->request_min,
+			r->request_max,
+			r->request_avg,
+			r->request_jitter);
+
+		len += snprintf(&buf[len], buf_len - len, ",%u,%u,%f,%u",
+			r->latency_min,
+			r->latency_max,
+			r->latency_avg,
+			r->latency_jitter);
+
+		len += snprintf(&buf[len], buf_len - len, ",%u,%u,%f,%u",
+			r->throughput_min,
+			r->throughput_max,
+			r->throughput_avg,
+			r->throughput_jitter);
+
+		len += snprintf(&buf[len], buf_len - len, ",%u,%u,%f,%u",
+			r->apbridge_unipro_latency_min,
+			r->apbridge_unipro_latency_max,
+			r->apbridge_unipro_latency_avg,
+			r->apbridge_unipro_latency_jitter);
+
+		len += snprintf(&buf[len], buf_len - len, ",%u,%u,%f,%u",
+			r->gbphy_firmware_latency_min,
+			r->gbphy_firmware_latency_max,
+			r->gbphy_firmware_latency_avg,
+			r->gbphy_firmware_latency_jitter);
+	}
+
+	printf("\n%s\n", buf);
+
+	return len;
+}
+
+static int log_results(struct loopback_test *t)
+{
+	int fd, i, len, ret;
+	struct tm tm;
+	time_t local_time;
+	char file_name[MAX_SYSFS_PATH];
+	char data[CSV_MAX_LINE];
+
+	local_time = time(NULL);
+	tm = *localtime(&local_time);
+
+	/*
+	 * file name will test_name_size_iteration_max.csv
+	 * every time the same test with the same parameters is run we will then
+	 * append to the same CSV with datestamp - representing each test
+	 * dataset.
+	 */
+	if (t->file_output && !t->porcelain) {
+		snprintf(file_name, sizeof(file_name), "%s_%d_%d.csv",
+			 t->test_name, t->size, t->iteration_max);
+
+		fd = open(file_name, O_WRONLY | O_CREAT | O_APPEND, 0644);
+		if (fd < 0) {
+			fprintf(stderr, "unable to open %s for appending\n", file_name);
+			abort();
+		}
+
+	}
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		len = format_output(t, &t->devices[i].results,
+				    t->devices[i].name,
+				    data, sizeof(data), &tm);
+		if (t->file_output && !t->porcelain) {
+			ret = write(fd, data, len);
+			if (ret == -1)
+				fprintf(stderr, "unable to write %d bytes to csv.\n", len);
+		}
+
+	}
+
+
+	if (t->aggregate_output) {
+		len = format_output(t, &t->aggregate_results, "aggregate",
+				    data, sizeof(data), &tm);
+		if (t->file_output && !t->porcelain) {
+			ret = write(fd, data, len);
+			if (ret == -1)
+				fprintf(stderr, "unable to write %d bytes to csv.\n", len);
+		}
+	}
+
+	if (t->file_output && !t->porcelain)
+		close(fd);
+
+	return 0;
+}
+
+int is_loopback_device(const char *path, const char *node)
+{
+	char file[MAX_SYSFS_PATH];
+
+	snprintf(file, MAX_SYSFS_PATH, "%s%s/iteration_count", path, node);
+	if (access(file, F_OK) == 0)
+		return 1;
+	return 0;
+}
+
+int find_loopback_devices(struct loopback_test *t)
+{
+	struct dirent **namelist;
+	int i, n, ret;
+	unsigned int dev_id;
+	struct loopback_device *d;
+
+	n = scandir(t->sysfs_prefix, &namelist, NULL, alphasort);
+	if (n < 0) {
+		perror("scandir");
+		ret = -ENODEV;
+		goto baddir;
+	}
+
+	/* Don't include '.' and '..' */
+	if (n <= 2) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	for (i = 0; i < n; i++) {
+		ret = sscanf(namelist[i]->d_name, "gb_loopback%u", &dev_id);
+		if (ret != 1)
+			continue;
+
+		if (!is_loopback_device(t->sysfs_prefix, namelist[i]->d_name))
+			continue;
+
+		if (t->device_count == MAX_NUM_DEVICES) {
+			fprintf(stderr, "max number of devices reached!\n");
+			break;
+		}
+
+		d = &t->devices[t->device_count++];
+		snprintf(d->name, MAX_STR_LEN, "gb_loopback%u", dev_id);
+
+		snprintf(d->sysfs_entry, MAX_SYSFS_PATH, "%s%s/",
+			 t->sysfs_prefix, d->name);
+
+		snprintf(d->debugfs_entry, MAX_SYSFS_PATH, "%sraw_latency_%s",
+			 t->debugfs_prefix, d->name);
+
+		if (t->debug)
+			printf("add %s %s\n", d->sysfs_entry, d->debugfs_entry);
+	}
+
+	ret = 0;
+done:
+	for (i = 0; i < n; i++)
+		free(namelist[i]);
+	free(namelist);
+baddir:
+	return ret;
+}
+
+static int open_poll_files(struct loopback_test *t)
+{
+	struct loopback_device *dev;
+	char buf[MAX_SYSFS_PATH + MAX_STR_LEN];
+	char dummy;
+	int fds_idx = 0;
+	int i;
+
+	for (i = 0; i < t->device_count; i++) {
+		dev = &t->devices[i];
+
+		if (!device_enabled(t, i))
+			continue;
+
+		snprintf(buf, sizeof(buf), "%s%s", dev->sysfs_entry, "iteration_count");
+		t->fds[fds_idx].fd = open(buf, O_RDONLY);
+		if (t->fds[fds_idx].fd < 0) {
+			fprintf(stderr, "Error opening poll file!\n");
+			goto err;
+		}
+		read(t->fds[fds_idx].fd, &dummy, 1);
+		t->fds[fds_idx].events = POLLERR | POLLPRI;
+		t->fds[fds_idx].revents = 0;
+		fds_idx++;
+	}
+
+	t->poll_count = fds_idx;
+
+	return 0;
+
+err:
+	for (i = 0; i < fds_idx; i++)
+		close(t->fds[i].fd);
+
+	return -1;
+}
+
+static int close_poll_files(struct loopback_test *t)
+{
+	int i;
+	for (i = 0; i < t->poll_count; i++)
+		close(t->fds[i].fd);
+
+	return 0;
+}
+static int is_complete(struct loopback_test *t)
+{
+	int iteration_count;
+	int i;
+
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		iteration_count = read_sysfs_int(t->devices[i].sysfs_entry,
+						 "iteration_count");
+
+		/* at least one device did not finish yet */
+		if (iteration_count != t->iteration_max)
+			return 0;
+	}
+
+	return 1;
+}
+
+static void stop_tests(struct loopback_test *t)
+{
+	int i;
+
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+		write_sysfs_val(t->devices[i].sysfs_entry, "type", 0);
+	}
+}
+
+static void handler(int sig) { /* do nothing */  }
+
+static int wait_for_complete(struct loopback_test *t)
+{
+	int number_of_events = 0;
+	char dummy;
+	int ret;
+	int i;
+	struct timespec *ts = NULL;
+	struct sigaction sa;
+	sigset_t mask_old, mask;
+
+	sigemptyset(&mask);
+	sigemptyset(&mask_old);
+	sigaddset(&mask, SIGINT);
+	sigprocmask(SIG_BLOCK, &mask, &mask_old);
+
+	sa.sa_handler = handler;
+	sa.sa_flags = 0;
+	sigemptyset(&sa.sa_mask);
+	if (sigaction(SIGINT, &sa, NULL) == -1) {
+		fprintf(stderr, "sigaction error\n");
+		return -1;
+	}
+
+	if (t->poll_timeout.tv_sec != 0)
+		ts = &t->poll_timeout;
+
+	while (1) {
+
+		ret = ppoll(t->fds, t->poll_count, ts, &mask_old);
+		if (ret <= 0) {
+			stop_tests(t);
+			fprintf(stderr, "Poll exit with errno %d\n", errno);
+			return -1;
+		}
+
+		for (i = 0; i < t->poll_count; i++) {
+			if (t->fds[i].revents & POLLPRI) {
+				/* Dummy read to clear the event */
+				read(t->fds[i].fd, &dummy, 1);
+				number_of_events++;
+			}
+		}
+
+		if (number_of_events == t->poll_count)
+			break;
+	}
+
+	if (!is_complete(t)) {
+		fprintf(stderr, "Iteration count did not finish!\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static void prepare_devices(struct loopback_test *t)
+{
+	int i;
+
+	/*
+	 * Cancel any running tests on enabled devices. If
+	 * stop_all option is given, stop test on all devices.
+	 */
+	for (i = 0; i < t->device_count; i++)
+		if (t->stop_all || device_enabled(t, i))
+			write_sysfs_val(t->devices[i].sysfs_entry, "type", 0);
+
+
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		write_sysfs_val(t->devices[i].sysfs_entry, "us_wait",
+				t->us_wait);
+
+		/* Set operation size */
+		write_sysfs_val(t->devices[i].sysfs_entry, "size", t->size);
+
+		/* Set iterations */
+		write_sysfs_val(t->devices[i].sysfs_entry, "iteration_max",
+				t->iteration_max);
+
+		if (t->use_async) {
+			write_sysfs_val(t->devices[i].sysfs_entry, "async", 1);
+			write_sysfs_val(t->devices[i].sysfs_entry,
+					"timeout", t->async_timeout);
+			write_sysfs_val(t->devices[i].sysfs_entry,
+					"outstanding_operations_max",
+					t->async_outstanding_operations);
+		} else {
+			write_sysfs_val(t->devices[i].sysfs_entry, "async", 0);
+		}
+	}
+}
+
+static int start(struct loopback_test *t)
+{
+	int i;
+
+	/* the test starts by writing test_id to the type file. */
+	for (i = 0; i < t->device_count; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		write_sysfs_val(t->devices[i].sysfs_entry, "type", t->test_id);
+	}
+
+	return 0;
+}
+
+
+void loopback_run(struct loopback_test *t)
+{
+	int i;
+	int ret;
+
+	for (i = 0; dict[i].name != NULL; i++) {
+		if (strstr(dict[i].name, t->test_name))
+			t->test_id = dict[i].type;
+	}
+	if (!t->test_id) {
+		fprintf(stderr, "invalid test %s\n", t->test_name);
+		usage();
+		return;
+	}
+
+	prepare_devices(t);
+
+	ret = open_poll_files(t);
+	if (ret)
+		goto err;
+
+	start(t);
+
+	ret = wait_for_complete(t);
+	close_poll_files(t);
+	if (ret)
+		goto err;
+
+
+	get_results(t);
+
+	log_results(t);
+
+	return;
+
+err:
+	printf("Error running test\n");
+	return;
+}
+
+static int sanity_check(struct loopback_test *t)
+{
+	int i;
+
+	if (t->device_count == 0) {
+		fprintf(stderr, "No loopback devices found\n");
+		return -1;
+	}
+
+	for (i = 0; i < MAX_NUM_DEVICES; i++) {
+		if (!device_enabled(t, i))
+			continue;
+
+		if (t->mask && !strcmp(t->devices[i].name, "")) {
+			fprintf(stderr, "Bad device mask %x\n", (1 << i));
+			return -1;
+		}
+
+	}
+
+
+	return 0;
+}
+
+int main(int argc, char *argv[])
+{
+	int o, ret;
+	char *sysfs_prefix = "/sys/class/gb_loopback/";
+	char *debugfs_prefix = "/sys/kernel/debug/gb_loopback/";
+
+	memset(&t, 0, sizeof(t));
+
+	while ((o = getopt(argc, argv,
+			   "t:s:i:S:D:m:v::d::r::p::a::l::x::o:O:c:w:z::f::")) != -1) {
+		switch (o) {
+		case 't':
+			snprintf(t.test_name, MAX_STR_LEN, "%s", optarg);
+			break;
+		case 's':
+			t.size = atoi(optarg);
+			break;
+		case 'i':
+			t.iteration_max = atoi(optarg);
+			break;
+		case 'S':
+			snprintf(t.sysfs_prefix, MAX_SYSFS_PREFIX, "%s", optarg);
+			break;
+		case 'D':
+			snprintf(t.debugfs_prefix, MAX_SYSFS_PREFIX, "%s", optarg);
+			break;
+		case 'm':
+			t.mask = atol(optarg);
+			break;
+		case 'v':
+			t.verbose = 1;
+			break;
+		case 'd':
+			t.debug = 1;
+			break;
+		case 'r':
+			t.raw_data_dump = 1;
+			break;
+		case 'p':
+			t.porcelain = 1;
+			break;
+		case 'a':
+			t.aggregate_output = 1;
+			break;
+		case 'l':
+			t.list_devices = 1;
+			break;
+		case 'x':
+			t.use_async = 1;
+			break;
+		case 'o':
+			t.async_timeout = atoi(optarg);
+			break;
+		case 'O':
+			t.poll_timeout.tv_sec = atoi(optarg);
+			break;
+		case 'c':
+			t.async_outstanding_operations = atoi(optarg);
+			break;
+		case 'w':
+			t.us_wait = atoi(optarg);
+			break;
+		case 'z':
+			t.file_output = 1;
+			break;
+		case 'f':
+			t.stop_all = 1;
+			break;
+		default:
+			usage();
+			return -EINVAL;
+		}
+	}
+
+	if (!strcmp(t.sysfs_prefix, ""))
+		snprintf(t.sysfs_prefix, MAX_SYSFS_PREFIX, "%s", sysfs_prefix);
+
+	if (!strcmp(t.debugfs_prefix, ""))
+		snprintf(t.debugfs_prefix, MAX_SYSFS_PREFIX, "%s", debugfs_prefix);
+
+	ret = find_loopback_devices(&t);
+	if (ret)
+		return ret;
+	ret = sanity_check(&t);
+	if (ret)
+		return ret;
+
+	if (t.list_devices) {
+		show_loopback_devices(&t);
+		return 0;
+	}
+
+	if (t.test_name[0] == '\0' || t.iteration_max == 0)
+		usage();
+
+	if (t.async_timeout == 0)
+		t.async_timeout = DEFAULT_ASYNC_TIMEOUT;
+
+	loopback_run(&t);
+
+	return 0;
+}
diff --git a/drivers/staging/greybus/uart.c b/drivers/staging/greybus/uart.c
new file mode 100644
index 000000000..90ff07f2c
--- /dev/null
+++ b/drivers/staging/greybus/uart.c
@@ -0,0 +1,1028 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * UART driver for the Greybus "generic" UART module.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ *
+ * Heavily based on drivers/usb/class/cdc-acm.c and
+ * drivers/usb/serial/usb-serial.c.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/module.h>
+#include <linux/sched/signal.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/mutex.h>
+#include <linux/tty.h>
+#include <linux/serial.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/idr.h>
+#include <linux/fs.h>
+#include <linux/kdev_t.h>
+#include <linux/kfifo.h>
+#include <linux/workqueue.h>
+#include <linux/completion.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+#define GB_NUM_MINORS	16	/* 16 is more than enough */
+#define GB_NAME		"ttyGB"
+
+#define GB_UART_WRITE_FIFO_SIZE		PAGE_SIZE
+#define GB_UART_WRITE_ROOM_MARGIN	1	/* leave some space in fifo */
+#define GB_UART_FIRMWARE_CREDITS	4096
+#define GB_UART_CREDIT_WAIT_TIMEOUT_MSEC	10000
+
+struct gb_tty {
+	struct gbphy_device *gbphy_dev;
+	struct tty_port port;
+	void *buffer;
+	size_t buffer_payload_max;
+	struct gb_connection *connection;
+	u16 cport_id;
+	unsigned int minor;
+	unsigned char clocal;
+	bool disconnected;
+	spinlock_t read_lock;
+	spinlock_t write_lock;
+	struct async_icount iocount;
+	struct async_icount oldcount;
+	wait_queue_head_t wioctl;
+	struct mutex mutex;
+	u8 ctrlin;	/* input control lines */
+	u8 ctrlout;	/* output control lines */
+	struct gb_uart_set_line_coding_request line_coding;
+	struct work_struct tx_work;
+	struct kfifo write_fifo;
+	bool close_pending;
+	unsigned int credits;
+	struct completion credits_complete;
+};
+
+static struct tty_driver *gb_tty_driver;
+static DEFINE_IDR(tty_minors);
+static DEFINE_MUTEX(table_lock);
+
+static int gb_uart_receive_data_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_tty *gb_tty = gb_connection_get_data(connection);
+	struct tty_port *port = &gb_tty->port;
+	struct gb_message *request = op->request;
+	struct gb_uart_recv_data_request *receive_data;
+	u16 recv_data_size;
+	int count;
+	unsigned long tty_flags = TTY_NORMAL;
+
+	if (request->payload_size < sizeof(*receive_data)) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"short receive-data request received (%zu < %zu)\n",
+			request->payload_size, sizeof(*receive_data));
+		return -EINVAL;
+	}
+
+	receive_data = op->request->payload;
+	recv_data_size = le16_to_cpu(receive_data->size);
+
+	if (recv_data_size != request->payload_size - sizeof(*receive_data)) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"malformed receive-data request received (%u != %zu)\n",
+			recv_data_size,
+			request->payload_size - sizeof(*receive_data));
+		return -EINVAL;
+	}
+
+	if (!recv_data_size)
+		return -EINVAL;
+
+	if (receive_data->flags) {
+		if (receive_data->flags & GB_UART_RECV_FLAG_BREAK)
+			tty_flags = TTY_BREAK;
+		else if (receive_data->flags & GB_UART_RECV_FLAG_PARITY)
+			tty_flags = TTY_PARITY;
+		else if (receive_data->flags & GB_UART_RECV_FLAG_FRAMING)
+			tty_flags = TTY_FRAME;
+
+		/* overrun is special, not associated with a char */
+		if (receive_data->flags & GB_UART_RECV_FLAG_OVERRUN)
+			tty_insert_flip_char(port, 0, TTY_OVERRUN);
+	}
+	count = tty_insert_flip_string_fixed_flag(port, receive_data->data,
+						  tty_flags, recv_data_size);
+	if (count != recv_data_size) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"UART: RX 0x%08x bytes only wrote 0x%08x\n",
+			recv_data_size, count);
+	}
+	if (count)
+		tty_flip_buffer_push(port);
+	return 0;
+}
+
+static int gb_uart_serial_state_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_tty *gb_tty = gb_connection_get_data(connection);
+	struct gb_message *request = op->request;
+	struct gb_uart_serial_state_request *serial_state;
+
+	if (request->payload_size < sizeof(*serial_state)) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"short serial-state event received (%zu < %zu)\n",
+			request->payload_size, sizeof(*serial_state));
+		return -EINVAL;
+	}
+
+	serial_state = request->payload;
+	gb_tty->ctrlin = serial_state->control;
+
+	return 0;
+}
+
+static int gb_uart_receive_credits_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_tty *gb_tty = gb_connection_get_data(connection);
+	struct gb_message *request = op->request;
+	struct gb_uart_receive_credits_request *credit_request;
+	unsigned long flags;
+	unsigned int incoming_credits;
+	int ret = 0;
+
+	if (request->payload_size < sizeof(*credit_request)) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"short receive_credits event received (%zu < %zu)\n",
+			request->payload_size,
+			sizeof(*credit_request));
+		return -EINVAL;
+	}
+
+	credit_request = request->payload;
+	incoming_credits = le16_to_cpu(credit_request->count);
+
+	spin_lock_irqsave(&gb_tty->write_lock, flags);
+	gb_tty->credits += incoming_credits;
+	if (gb_tty->credits > GB_UART_FIRMWARE_CREDITS) {
+		gb_tty->credits -= incoming_credits;
+		ret = -EINVAL;
+	}
+	spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+	if (ret) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"invalid number of incoming credits: %d\n",
+			incoming_credits);
+		return ret;
+	}
+
+	if (!gb_tty->close_pending)
+		schedule_work(&gb_tty->tx_work);
+
+	/*
+	 * the port the tty layer may be waiting for credits
+	 */
+	tty_port_tty_wakeup(&gb_tty->port);
+
+	if (gb_tty->credits == GB_UART_FIRMWARE_CREDITS)
+		complete(&gb_tty->credits_complete);
+
+	return ret;
+}
+
+static int gb_uart_request_handler(struct gb_operation *op)
+{
+	struct gb_connection *connection = op->connection;
+	struct gb_tty *gb_tty = gb_connection_get_data(connection);
+	int type = op->type;
+	int ret;
+
+	switch (type) {
+	case GB_UART_TYPE_RECEIVE_DATA:
+		ret = gb_uart_receive_data_handler(op);
+		break;
+	case GB_UART_TYPE_SERIAL_STATE:
+		ret = gb_uart_serial_state_handler(op);
+		break;
+	case GB_UART_TYPE_RECEIVE_CREDITS:
+		ret = gb_uart_receive_credits_handler(op);
+		break;
+	default:
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"unsupported unsolicited request: 0x%02x\n", type);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static void  gb_uart_tx_write_work(struct work_struct *work)
+{
+	struct gb_uart_send_data_request *request;
+	struct gb_tty *gb_tty;
+	unsigned long flags;
+	unsigned int send_size;
+	int ret;
+
+	gb_tty = container_of(work, struct gb_tty, tx_work);
+	request = gb_tty->buffer;
+
+	while (1) {
+		if (gb_tty->close_pending)
+			break;
+
+		spin_lock_irqsave(&gb_tty->write_lock, flags);
+		send_size = gb_tty->buffer_payload_max;
+		if (send_size > gb_tty->credits)
+			send_size = gb_tty->credits;
+
+		send_size = kfifo_out_peek(&gb_tty->write_fifo,
+					   &request->data[0],
+					   send_size);
+		if (!send_size) {
+			spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+			break;
+		}
+
+		gb_tty->credits -= send_size;
+		spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+		request->size = cpu_to_le16(send_size);
+		ret = gb_operation_sync(gb_tty->connection,
+					GB_UART_TYPE_SEND_DATA,
+					request, sizeof(*request) + send_size,
+					NULL, 0);
+		if (ret) {
+			dev_err(&gb_tty->gbphy_dev->dev,
+				"send data error: %d\n", ret);
+			spin_lock_irqsave(&gb_tty->write_lock, flags);
+			gb_tty->credits += send_size;
+			spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+			if (!gb_tty->close_pending)
+				schedule_work(work);
+			return;
+		}
+
+		spin_lock_irqsave(&gb_tty->write_lock, flags);
+		ret = kfifo_out(&gb_tty->write_fifo, &request->data[0],
+				send_size);
+		spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+		tty_port_tty_wakeup(&gb_tty->port);
+	}
+}
+
+static int send_line_coding(struct gb_tty *tty)
+{
+	return gb_operation_sync(tty->connection, GB_UART_TYPE_SET_LINE_CODING,
+				 &tty->line_coding, sizeof(tty->line_coding),
+				 NULL, 0);
+}
+
+static int send_control(struct gb_tty *gb_tty, u8 control)
+{
+	struct gb_uart_set_control_line_state_request request;
+
+	request.control = control;
+	return gb_operation_sync(gb_tty->connection,
+				 GB_UART_TYPE_SET_CONTROL_LINE_STATE,
+				 &request, sizeof(request), NULL, 0);
+}
+
+static int send_break(struct gb_tty *gb_tty, u8 state)
+{
+	struct gb_uart_set_break_request request;
+
+	if ((state != 0) && (state != 1)) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"invalid break state of %d\n", state);
+		return -EINVAL;
+	}
+
+	request.state = state;
+	return gb_operation_sync(gb_tty->connection, GB_UART_TYPE_SEND_BREAK,
+				 &request, sizeof(request), NULL, 0);
+}
+
+static int gb_uart_wait_for_all_credits(struct gb_tty *gb_tty)
+{
+	int ret;
+
+	if (gb_tty->credits == GB_UART_FIRMWARE_CREDITS)
+		return 0;
+
+	ret = wait_for_completion_timeout(&gb_tty->credits_complete,
+			msecs_to_jiffies(GB_UART_CREDIT_WAIT_TIMEOUT_MSEC));
+	if (!ret) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"time out waiting for credits\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int gb_uart_flush(struct gb_tty *gb_tty, u8 flags)
+{
+	struct gb_uart_serial_flush_request request;
+
+	request.flags = flags;
+	return gb_operation_sync(gb_tty->connection, GB_UART_TYPE_FLUSH_FIFOS,
+				 &request, sizeof(request), NULL, 0);
+}
+
+static struct gb_tty *get_gb_by_minor(unsigned int minor)
+{
+	struct gb_tty *gb_tty;
+
+	mutex_lock(&table_lock);
+	gb_tty = idr_find(&tty_minors, minor);
+	if (gb_tty) {
+		mutex_lock(&gb_tty->mutex);
+		if (gb_tty->disconnected) {
+			mutex_unlock(&gb_tty->mutex);
+			gb_tty = NULL;
+		} else {
+			tty_port_get(&gb_tty->port);
+			mutex_unlock(&gb_tty->mutex);
+		}
+	}
+	mutex_unlock(&table_lock);
+	return gb_tty;
+}
+
+static int alloc_minor(struct gb_tty *gb_tty)
+{
+	int minor;
+
+	mutex_lock(&table_lock);
+	minor = idr_alloc(&tty_minors, gb_tty, 0, GB_NUM_MINORS, GFP_KERNEL);
+	mutex_unlock(&table_lock);
+	if (minor >= 0)
+		gb_tty->minor = minor;
+	return minor;
+}
+
+static void release_minor(struct gb_tty *gb_tty)
+{
+	int minor = gb_tty->minor;
+
+	gb_tty->minor = 0;	/* Maybe should use an invalid value instead */
+	mutex_lock(&table_lock);
+	idr_remove(&tty_minors, minor);
+	mutex_unlock(&table_lock);
+}
+
+static int gb_tty_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty;
+	int retval;
+
+	gb_tty = get_gb_by_minor(tty->index);
+	if (!gb_tty)
+		return -ENODEV;
+
+	retval = tty_standard_install(driver, tty);
+	if (retval)
+		goto error;
+
+	tty->driver_data = gb_tty;
+	return 0;
+error:
+	tty_port_put(&gb_tty->port);
+	return retval;
+}
+
+static int gb_tty_open(struct tty_struct *tty, struct file *file)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	return tty_port_open(&gb_tty->port, tty, file);
+}
+
+static void gb_tty_close(struct tty_struct *tty, struct file *file)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	tty_port_close(&gb_tty->port, tty, file);
+}
+
+static void gb_tty_cleanup(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	tty_port_put(&gb_tty->port);
+}
+
+static void gb_tty_hangup(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	tty_port_hangup(&gb_tty->port);
+}
+
+static int gb_tty_write(struct tty_struct *tty, const unsigned char *buf,
+			int count)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	count =  kfifo_in_spinlocked(&gb_tty->write_fifo, buf, count,
+				     &gb_tty->write_lock);
+	if (count && !gb_tty->close_pending)
+		schedule_work(&gb_tty->tx_work);
+
+	return count;
+}
+
+static unsigned int gb_tty_write_room(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	unsigned long flags;
+	int room;
+
+	spin_lock_irqsave(&gb_tty->write_lock, flags);
+	room = kfifo_avail(&gb_tty->write_fifo);
+	spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+	room -= GB_UART_WRITE_ROOM_MARGIN;
+	if (room < 0)
+		return 0;
+
+	return room;
+}
+
+static unsigned int gb_tty_chars_in_buffer(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	unsigned long flags;
+	unsigned int chars;
+
+	spin_lock_irqsave(&gb_tty->write_lock, flags);
+	chars = kfifo_len(&gb_tty->write_fifo);
+	if (gb_tty->credits < GB_UART_FIRMWARE_CREDITS)
+		chars += GB_UART_FIRMWARE_CREDITS - gb_tty->credits;
+	spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+	return chars;
+}
+
+static int gb_tty_break_ctl(struct tty_struct *tty, int state)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	return send_break(gb_tty, state ? 1 : 0);
+}
+
+static void gb_tty_set_termios(struct tty_struct *tty,
+			       const struct ktermios *termios_old)
+{
+	struct gb_uart_set_line_coding_request newline;
+	struct gb_tty *gb_tty = tty->driver_data;
+	struct ktermios *termios = &tty->termios;
+	u8 newctrl = gb_tty->ctrlout;
+
+	newline.rate = cpu_to_le32(tty_get_baud_rate(tty));
+	newline.format = termios->c_cflag & CSTOPB ?
+				GB_SERIAL_2_STOP_BITS : GB_SERIAL_1_STOP_BITS;
+	newline.parity = termios->c_cflag & PARENB ?
+				(termios->c_cflag & PARODD ? 1 : 2) +
+				(termios->c_cflag & CMSPAR ? 2 : 0) : 0;
+
+	newline.data_bits = tty_get_char_size(termios->c_cflag);
+
+	/* FIXME: needs to clear unsupported bits in the termios */
+	gb_tty->clocal = ((termios->c_cflag & CLOCAL) != 0);
+
+	if (C_BAUD(tty) == B0) {
+		newline.rate = gb_tty->line_coding.rate;
+		newctrl &= ~(GB_UART_CTRL_DTR | GB_UART_CTRL_RTS);
+	} else if (termios_old && (termios_old->c_cflag & CBAUD) == B0) {
+		newctrl |= (GB_UART_CTRL_DTR | GB_UART_CTRL_RTS);
+	}
+
+	if (newctrl != gb_tty->ctrlout) {
+		gb_tty->ctrlout = newctrl;
+		send_control(gb_tty, newctrl);
+	}
+
+	if (C_CRTSCTS(tty) && C_BAUD(tty) != B0)
+		newline.flow_control = GB_SERIAL_AUTO_RTSCTS_EN;
+	else
+		newline.flow_control = 0;
+
+	if (memcmp(&gb_tty->line_coding, &newline, sizeof(newline))) {
+		memcpy(&gb_tty->line_coding, &newline, sizeof(newline));
+		send_line_coding(gb_tty);
+	}
+}
+
+static int gb_tty_tiocmget(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	return (gb_tty->ctrlout & GB_UART_CTRL_DTR ? TIOCM_DTR : 0) |
+	       (gb_tty->ctrlout & GB_UART_CTRL_RTS ? TIOCM_RTS : 0) |
+	       (gb_tty->ctrlin  & GB_UART_CTRL_DSR ? TIOCM_DSR : 0) |
+	       (gb_tty->ctrlin  & GB_UART_CTRL_RI  ? TIOCM_RI  : 0) |
+	       (gb_tty->ctrlin  & GB_UART_CTRL_DCD ? TIOCM_CD  : 0) |
+	       TIOCM_CTS;
+}
+
+static int gb_tty_tiocmset(struct tty_struct *tty, unsigned int set,
+			   unsigned int clear)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	u8 newctrl = gb_tty->ctrlout;
+
+	set = (set & TIOCM_DTR ? GB_UART_CTRL_DTR : 0) |
+	      (set & TIOCM_RTS ? GB_UART_CTRL_RTS : 0);
+	clear = (clear & TIOCM_DTR ? GB_UART_CTRL_DTR : 0) |
+		(clear & TIOCM_RTS ? GB_UART_CTRL_RTS : 0);
+
+	newctrl = (newctrl & ~clear) | set;
+	if (gb_tty->ctrlout == newctrl)
+		return 0;
+
+	gb_tty->ctrlout = newctrl;
+	return send_control(gb_tty, newctrl);
+}
+
+static void gb_tty_throttle(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	unsigned char stop_char;
+	int retval;
+
+	if (I_IXOFF(tty)) {
+		stop_char = STOP_CHAR(tty);
+		retval = gb_tty_write(tty, &stop_char, 1);
+		if (retval <= 0)
+			return;
+	}
+
+	if (tty->termios.c_cflag & CRTSCTS) {
+		gb_tty->ctrlout &= ~GB_UART_CTRL_RTS;
+		retval = send_control(gb_tty, gb_tty->ctrlout);
+	}
+}
+
+static void gb_tty_unthrottle(struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	unsigned char start_char;
+	int retval;
+
+	if (I_IXOFF(tty)) {
+		start_char = START_CHAR(tty);
+		retval = gb_tty_write(tty, &start_char, 1);
+		if (retval <= 0)
+			return;
+	}
+
+	if (tty->termios.c_cflag & CRTSCTS) {
+		gb_tty->ctrlout |= GB_UART_CTRL_RTS;
+		retval = send_control(gb_tty, gb_tty->ctrlout);
+	}
+}
+
+static int get_serial_info(struct tty_struct *tty,
+			   struct serial_struct *ss)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	ss->line = gb_tty->minor;
+	ss->close_delay = jiffies_to_msecs(gb_tty->port.close_delay) / 10;
+	ss->closing_wait =
+		gb_tty->port.closing_wait == ASYNC_CLOSING_WAIT_NONE ?
+		ASYNC_CLOSING_WAIT_NONE :
+		jiffies_to_msecs(gb_tty->port.closing_wait) / 10;
+
+	return 0;
+}
+
+static int set_serial_info(struct tty_struct *tty,
+			   struct serial_struct *ss)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+	unsigned int closing_wait;
+	unsigned int close_delay;
+	int retval = 0;
+
+	close_delay = msecs_to_jiffies(ss->close_delay * 10);
+	closing_wait = ss->closing_wait == ASYNC_CLOSING_WAIT_NONE ?
+			ASYNC_CLOSING_WAIT_NONE :
+			msecs_to_jiffies(ss->closing_wait * 10);
+
+	mutex_lock(&gb_tty->port.mutex);
+	if (!capable(CAP_SYS_ADMIN)) {
+		if ((close_delay != gb_tty->port.close_delay) ||
+		    (closing_wait != gb_tty->port.closing_wait))
+			retval = -EPERM;
+	} else {
+		gb_tty->port.close_delay = close_delay;
+		gb_tty->port.closing_wait = closing_wait;
+	}
+	mutex_unlock(&gb_tty->port.mutex);
+	return retval;
+}
+
+static int wait_serial_change(struct gb_tty *gb_tty, unsigned long arg)
+{
+	int retval = 0;
+	DECLARE_WAITQUEUE(wait, current);
+	struct async_icount old;
+	struct async_icount new;
+
+	if (!(arg & (TIOCM_DSR | TIOCM_RI | TIOCM_CD)))
+		return -EINVAL;
+
+	do {
+		spin_lock_irq(&gb_tty->read_lock);
+		old = gb_tty->oldcount;
+		new = gb_tty->iocount;
+		gb_tty->oldcount = new;
+		spin_unlock_irq(&gb_tty->read_lock);
+
+		if ((arg & TIOCM_DSR) && (old.dsr != new.dsr))
+			break;
+		if ((arg & TIOCM_CD) && (old.dcd != new.dcd))
+			break;
+		if ((arg & TIOCM_RI) && (old.rng != new.rng))
+			break;
+
+		add_wait_queue(&gb_tty->wioctl, &wait);
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule();
+		remove_wait_queue(&gb_tty->wioctl, &wait);
+		if (gb_tty->disconnected) {
+			if (arg & TIOCM_CD)
+				break;
+			retval = -ENODEV;
+		} else if (signal_pending(current)) {
+			retval = -ERESTARTSYS;
+		}
+	} while (!retval);
+
+	return retval;
+}
+
+static int gb_tty_get_icount(struct tty_struct *tty,
+			     struct serial_icounter_struct *icount)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	icount->dsr = gb_tty->iocount.dsr;
+	icount->rng = gb_tty->iocount.rng;
+	icount->dcd = gb_tty->iocount.dcd;
+	icount->frame = gb_tty->iocount.frame;
+	icount->overrun = gb_tty->iocount.overrun;
+	icount->parity = gb_tty->iocount.parity;
+	icount->brk = gb_tty->iocount.brk;
+
+	return 0;
+}
+
+static int gb_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
+			unsigned long arg)
+{
+	struct gb_tty *gb_tty = tty->driver_data;
+
+	switch (cmd) {
+	case TIOCMIWAIT:
+		return wait_serial_change(gb_tty, arg);
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+static void gb_tty_dtr_rts(struct tty_port *port, int on)
+{
+	struct gb_tty *gb_tty;
+	u8 newctrl;
+
+	gb_tty = container_of(port, struct gb_tty, port);
+	newctrl = gb_tty->ctrlout;
+
+	if (on)
+		newctrl |= (GB_UART_CTRL_DTR | GB_UART_CTRL_RTS);
+	else
+		newctrl &= ~(GB_UART_CTRL_DTR | GB_UART_CTRL_RTS);
+
+	gb_tty->ctrlout = newctrl;
+	send_control(gb_tty, newctrl);
+}
+
+static int gb_tty_port_activate(struct tty_port *port,
+				struct tty_struct *tty)
+{
+	struct gb_tty *gb_tty;
+
+	gb_tty = container_of(port, struct gb_tty, port);
+
+	return gbphy_runtime_get_sync(gb_tty->gbphy_dev);
+}
+
+static void gb_tty_port_shutdown(struct tty_port *port)
+{
+	struct gb_tty *gb_tty;
+	unsigned long flags;
+	int ret;
+
+	gb_tty = container_of(port, struct gb_tty, port);
+
+	gb_tty->close_pending = true;
+
+	cancel_work_sync(&gb_tty->tx_work);
+
+	spin_lock_irqsave(&gb_tty->write_lock, flags);
+	kfifo_reset_out(&gb_tty->write_fifo);
+	spin_unlock_irqrestore(&gb_tty->write_lock, flags);
+
+	if (gb_tty->credits == GB_UART_FIRMWARE_CREDITS)
+		goto out;
+
+	ret = gb_uart_flush(gb_tty, GB_SERIAL_FLAG_FLUSH_TRANSMITTER);
+	if (ret) {
+		dev_err(&gb_tty->gbphy_dev->dev,
+			"error flushing transmitter: %d\n", ret);
+	}
+
+	gb_uart_wait_for_all_credits(gb_tty);
+
+out:
+	gb_tty->close_pending = false;
+
+	gbphy_runtime_put_autosuspend(gb_tty->gbphy_dev);
+}
+
+static void gb_tty_port_destruct(struct tty_port *port)
+{
+	struct gb_tty *gb_tty = container_of(port, struct gb_tty, port);
+
+	if (gb_tty->minor != GB_NUM_MINORS)
+		release_minor(gb_tty);
+	kfifo_free(&gb_tty->write_fifo);
+	kfree(gb_tty->buffer);
+	kfree(gb_tty);
+}
+
+static const struct tty_operations gb_ops = {
+	.install =		gb_tty_install,
+	.open =			gb_tty_open,
+	.close =		gb_tty_close,
+	.cleanup =		gb_tty_cleanup,
+	.hangup =		gb_tty_hangup,
+	.write =		gb_tty_write,
+	.write_room =		gb_tty_write_room,
+	.ioctl =		gb_tty_ioctl,
+	.throttle =		gb_tty_throttle,
+	.unthrottle =		gb_tty_unthrottle,
+	.chars_in_buffer =	gb_tty_chars_in_buffer,
+	.break_ctl =		gb_tty_break_ctl,
+	.set_termios =		gb_tty_set_termios,
+	.tiocmget =		gb_tty_tiocmget,
+	.tiocmset =		gb_tty_tiocmset,
+	.get_icount =		gb_tty_get_icount,
+	.set_serial =		set_serial_info,
+	.get_serial =		get_serial_info,
+};
+
+static const struct tty_port_operations gb_port_ops = {
+	.dtr_rts =		gb_tty_dtr_rts,
+	.activate =		gb_tty_port_activate,
+	.shutdown =		gb_tty_port_shutdown,
+	.destruct =		gb_tty_port_destruct,
+};
+
+static int gb_uart_probe(struct gbphy_device *gbphy_dev,
+			 const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	size_t max_payload;
+	struct gb_tty *gb_tty;
+	struct device *tty_dev;
+	int retval;
+	int minor;
+
+	connection = gb_connection_create(gbphy_dev->bundle,
+					  le16_to_cpu(gbphy_dev->cport_desc->id),
+					  gb_uart_request_handler);
+	if (IS_ERR(connection))
+		return PTR_ERR(connection);
+
+	max_payload = gb_operation_get_payload_size_max(connection);
+	if (max_payload < sizeof(struct gb_uart_send_data_request)) {
+		retval = -EINVAL;
+		goto exit_connection_destroy;
+	}
+
+	gb_tty = kzalloc(sizeof(*gb_tty), GFP_KERNEL);
+	if (!gb_tty) {
+		retval = -ENOMEM;
+		goto exit_connection_destroy;
+	}
+
+	tty_port_init(&gb_tty->port);
+	gb_tty->port.ops = &gb_port_ops;
+	gb_tty->minor = GB_NUM_MINORS;
+
+	gb_tty->buffer_payload_max = max_payload -
+			sizeof(struct gb_uart_send_data_request);
+
+	gb_tty->buffer = kzalloc(gb_tty->buffer_payload_max, GFP_KERNEL);
+	if (!gb_tty->buffer) {
+		retval = -ENOMEM;
+		goto exit_put_port;
+	}
+
+	INIT_WORK(&gb_tty->tx_work, gb_uart_tx_write_work);
+
+	retval = kfifo_alloc(&gb_tty->write_fifo, GB_UART_WRITE_FIFO_SIZE,
+			     GFP_KERNEL);
+	if (retval)
+		goto exit_put_port;
+
+	gb_tty->credits = GB_UART_FIRMWARE_CREDITS;
+	init_completion(&gb_tty->credits_complete);
+
+	minor = alloc_minor(gb_tty);
+	if (minor < 0) {
+		if (minor == -ENOSPC) {
+			dev_err(&gbphy_dev->dev,
+				"no more free minor numbers\n");
+			retval = -ENODEV;
+		} else {
+			retval = minor;
+		}
+		goto exit_put_port;
+	}
+
+	gb_tty->minor = minor;
+	spin_lock_init(&gb_tty->write_lock);
+	spin_lock_init(&gb_tty->read_lock);
+	init_waitqueue_head(&gb_tty->wioctl);
+	mutex_init(&gb_tty->mutex);
+
+	gb_tty->connection = connection;
+	gb_tty->gbphy_dev = gbphy_dev;
+	gb_connection_set_data(connection, gb_tty);
+	gb_gbphy_set_data(gbphy_dev, gb_tty);
+
+	retval = gb_connection_enable_tx(connection);
+	if (retval)
+		goto exit_put_port;
+
+	send_control(gb_tty, gb_tty->ctrlout);
+
+	/* initialize the uart to be 9600n81 */
+	gb_tty->line_coding.rate = cpu_to_le32(9600);
+	gb_tty->line_coding.format = GB_SERIAL_1_STOP_BITS;
+	gb_tty->line_coding.parity = GB_SERIAL_NO_PARITY;
+	gb_tty->line_coding.data_bits = 8;
+	send_line_coding(gb_tty);
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto exit_connection_disable;
+
+	tty_dev = tty_port_register_device(&gb_tty->port, gb_tty_driver, minor,
+					   &gbphy_dev->dev);
+	if (IS_ERR(tty_dev)) {
+		retval = PTR_ERR(tty_dev);
+		goto exit_connection_disable;
+	}
+
+	gbphy_runtime_put_autosuspend(gbphy_dev);
+	return 0;
+
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_put_port:
+	tty_port_put(&gb_tty->port);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+
+	return retval;
+}
+
+static void gb_uart_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_tty *gb_tty = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = gb_tty->connection;
+	struct tty_struct *tty;
+	int ret;
+
+	ret = gbphy_runtime_get_sync(gbphy_dev);
+	if (ret)
+		gbphy_runtime_get_noresume(gbphy_dev);
+
+	mutex_lock(&gb_tty->mutex);
+	gb_tty->disconnected = true;
+
+	wake_up_all(&gb_tty->wioctl);
+	mutex_unlock(&gb_tty->mutex);
+
+	tty = tty_port_tty_get(&gb_tty->port);
+	if (tty) {
+		tty_vhangup(tty);
+		tty_kref_put(tty);
+	}
+
+	gb_connection_disable_rx(connection);
+	tty_unregister_device(gb_tty_driver, gb_tty->minor);
+
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+
+	tty_port_put(&gb_tty->port);
+}
+
+static int gb_tty_init(void)
+{
+	int retval = 0;
+
+	gb_tty_driver = tty_alloc_driver(GB_NUM_MINORS, 0);
+	if (IS_ERR(gb_tty_driver)) {
+		pr_err("Can not allocate tty driver\n");
+		retval = -ENOMEM;
+		goto fail_unregister_dev;
+	}
+
+	gb_tty_driver->driver_name = "gb";
+	gb_tty_driver->name = GB_NAME;
+	gb_tty_driver->major = 0;
+	gb_tty_driver->minor_start = 0;
+	gb_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	gb_tty_driver->subtype = SERIAL_TYPE_NORMAL;
+	gb_tty_driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	gb_tty_driver->init_termios = tty_std_termios;
+	gb_tty_driver->init_termios.c_cflag = B9600 | CS8 |
+		CREAD | HUPCL | CLOCAL;
+	tty_set_operations(gb_tty_driver, &gb_ops);
+
+	retval = tty_register_driver(gb_tty_driver);
+	if (retval) {
+		pr_err("Can not register tty driver: %d\n", retval);
+		goto fail_put_gb_tty;
+	}
+
+	return 0;
+
+fail_put_gb_tty:
+	tty_driver_kref_put(gb_tty_driver);
+fail_unregister_dev:
+	return retval;
+}
+
+static void gb_tty_exit(void)
+{
+	tty_unregister_driver(gb_tty_driver);
+	tty_driver_kref_put(gb_tty_driver);
+	idr_destroy(&tty_minors);
+}
+
+static const struct gbphy_device_id gb_uart_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_UART) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_uart_id_table);
+
+static struct gbphy_driver uart_driver = {
+	.name		= "uart",
+	.probe		= gb_uart_probe,
+	.remove		= gb_uart_remove,
+	.id_table	= gb_uart_id_table,
+};
+
+static int gb_uart_driver_init(void)
+{
+	int ret;
+
+	ret = gb_tty_init();
+	if (ret)
+		return ret;
+
+	ret = gb_gbphy_register(&uart_driver);
+	if (ret) {
+		gb_tty_exit();
+		return ret;
+	}
+
+	return 0;
+}
+module_init(gb_uart_driver_init);
+
+static void gb_uart_driver_exit(void)
+{
+	gb_gbphy_deregister(&uart_driver);
+	gb_tty_exit();
+}
+
+module_exit(gb_uart_driver_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/usb.c b/drivers/staging/greybus/usb.c
new file mode 100644
index 000000000..8e9d9d59a
--- /dev/null
+++ b/drivers/staging/greybus/usb.c
@@ -0,0 +1,245 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * USB host driver for the Greybus "generic" USB module.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/greybus.h>
+
+#include "gbphy.h"
+
+/* Greybus USB request types */
+#define GB_USB_TYPE_HCD_START		0x02
+#define GB_USB_TYPE_HCD_STOP		0x03
+#define GB_USB_TYPE_HUB_CONTROL		0x04
+
+struct gb_usb_hub_control_request {
+	__le16 typeReq;
+	__le16 wValue;
+	__le16 wIndex;
+	__le16 wLength;
+};
+
+struct gb_usb_hub_control_response {
+	u8 buf[0];
+};
+
+struct gb_usb_device {
+	struct gb_connection *connection;
+	struct gbphy_device *gbphy_dev;
+};
+
+static inline struct gb_usb_device *to_gb_usb_device(struct usb_hcd *hcd)
+{
+	return (struct gb_usb_device *)hcd->hcd_priv;
+}
+
+static inline struct usb_hcd *gb_usb_device_to_hcd(struct gb_usb_device *dev)
+{
+	return container_of((void *)dev, struct usb_hcd, hcd_priv);
+}
+
+static void hcd_stop(struct usb_hcd *hcd)
+{
+	struct gb_usb_device *dev = to_gb_usb_device(hcd);
+	int ret;
+
+	ret = gb_operation_sync(dev->connection, GB_USB_TYPE_HCD_STOP,
+				NULL, 0, NULL, 0);
+	if (ret)
+		dev_err(&dev->gbphy_dev->dev, "HCD stop failed '%d'\n", ret);
+}
+
+static int hcd_start(struct usb_hcd *hcd)
+{
+	struct usb_bus *bus = hcd_to_bus(hcd);
+	struct gb_usb_device *dev = to_gb_usb_device(hcd);
+	int ret;
+
+	ret = gb_operation_sync(dev->connection, GB_USB_TYPE_HCD_START,
+				NULL, 0, NULL, 0);
+	if (ret) {
+		dev_err(&dev->gbphy_dev->dev, "HCD start failed '%d'\n", ret);
+		return ret;
+	}
+
+	hcd->state = HC_STATE_RUNNING;
+	if (bus->root_hub)
+		usb_hcd_resume_root_hub(hcd);
+	return 0;
+}
+
+static int urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
+{
+	return -ENXIO;
+}
+
+static int urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
+{
+	return -ENXIO;
+}
+
+static int get_frame_number(struct usb_hcd *hcd)
+{
+	return 0;
+}
+
+static int hub_status_data(struct usb_hcd *hcd, char *buf)
+{
+	return 0;
+}
+
+static int hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,
+		       char *buf, u16 wLength)
+{
+	struct gb_usb_device *dev = to_gb_usb_device(hcd);
+	struct gb_operation *operation;
+	struct gb_usb_hub_control_request *request;
+	struct gb_usb_hub_control_response *response;
+	size_t response_size;
+	int ret;
+
+	/* FIXME: handle unspecified lengths */
+	response_size = sizeof(*response) + wLength;
+
+	operation = gb_operation_create(dev->connection,
+					GB_USB_TYPE_HUB_CONTROL,
+					sizeof(*request),
+					response_size,
+					GFP_KERNEL);
+	if (!operation)
+		return -ENOMEM;
+
+	request = operation->request->payload;
+	request->typeReq = cpu_to_le16(typeReq);
+	request->wValue = cpu_to_le16(wValue);
+	request->wIndex = cpu_to_le16(wIndex);
+	request->wLength = cpu_to_le16(wLength);
+
+	ret = gb_operation_request_send_sync(operation);
+	if (ret)
+		goto out;
+
+	if (wLength) {
+		/* Greybus core has verified response size */
+		response = operation->response->payload;
+		memcpy(buf, response->buf, wLength);
+	}
+out:
+	gb_operation_put(operation);
+
+	return ret;
+}
+
+static const struct hc_driver usb_gb_hc_driver = {
+	.description = "greybus-hcd",
+	.product_desc = "Greybus USB Host Controller",
+	.hcd_priv_size = sizeof(struct gb_usb_device),
+
+	.flags = HCD_USB2,
+
+	.start = hcd_start,
+	.stop = hcd_stop,
+
+	.urb_enqueue = urb_enqueue,
+	.urb_dequeue = urb_dequeue,
+
+	.get_frame_number = get_frame_number,
+	.hub_status_data = hub_status_data,
+	.hub_control = hub_control,
+};
+
+static int gb_usb_probe(struct gbphy_device *gbphy_dev,
+			const struct gbphy_device_id *id)
+{
+	struct gb_connection *connection;
+	struct device *dev = &gbphy_dev->dev;
+	struct gb_usb_device *gb_usb_dev;
+	struct usb_hcd *hcd;
+	int retval;
+
+	hcd = usb_create_hcd(&usb_gb_hc_driver, dev, dev_name(dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	connection = gb_connection_create(gbphy_dev->bundle,
+					  le16_to_cpu(gbphy_dev->cport_desc->id),
+					  NULL);
+	if (IS_ERR(connection)) {
+		retval = PTR_ERR(connection);
+		goto exit_usb_put;
+	}
+
+	gb_usb_dev = to_gb_usb_device(hcd);
+	gb_usb_dev->connection = connection;
+	gb_connection_set_data(connection, gb_usb_dev);
+	gb_usb_dev->gbphy_dev = gbphy_dev;
+	gb_gbphy_set_data(gbphy_dev, gb_usb_dev);
+
+	hcd->has_tt = 1;
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto exit_connection_destroy;
+
+	/*
+	 * FIXME: The USB bridged-PHY protocol driver depends on changes to
+	 *        USB core which are not yet upstream.
+	 *
+	 *        Disable for now.
+	 */
+	if (1) {
+		dev_warn(dev, "USB protocol disabled\n");
+		retval = -EPROTONOSUPPORT;
+		goto exit_connection_disable;
+	}
+
+	retval = usb_add_hcd(hcd, 0, 0);
+	if (retval)
+		goto exit_connection_disable;
+
+	return 0;
+
+exit_connection_disable:
+	gb_connection_disable(connection);
+exit_connection_destroy:
+	gb_connection_destroy(connection);
+exit_usb_put:
+	usb_put_hcd(hcd);
+
+	return retval;
+}
+
+static void gb_usb_remove(struct gbphy_device *gbphy_dev)
+{
+	struct gb_usb_device *gb_usb_dev = gb_gbphy_get_data(gbphy_dev);
+	struct gb_connection *connection = gb_usb_dev->connection;
+	struct usb_hcd *hcd = gb_usb_device_to_hcd(gb_usb_dev);
+
+	usb_remove_hcd(hcd);
+	gb_connection_disable(connection);
+	gb_connection_destroy(connection);
+	usb_put_hcd(hcd);
+}
+
+static const struct gbphy_device_id gb_usb_id_table[] = {
+	{ GBPHY_PROTOCOL(GREYBUS_PROTOCOL_USB) },
+	{ },
+};
+MODULE_DEVICE_TABLE(gbphy, gb_usb_id_table);
+
+static struct gbphy_driver usb_driver = {
+	.name		= "usb",
+	.probe		= gb_usb_probe,
+	.remove		= gb_usb_remove,
+	.id_table	= gb_usb_id_table,
+};
+
+module_gbphy_driver(usb_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/greybus/vibrator.c b/drivers/staging/greybus/vibrator.c
new file mode 100644
index 000000000..0e2b188e5
--- /dev/null
+++ b/drivers/staging/greybus/vibrator.c
@@ -0,0 +1,249 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Greybus Vibrator protocol driver.
+ *
+ * Copyright 2014 Google Inc.
+ * Copyright 2014 Linaro Ltd.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/kdev_t.h>
+#include <linux/idr.h>
+#include <linux/pm_runtime.h>
+#include <linux/greybus.h>
+
+struct gb_vibrator_device {
+	struct gb_connection	*connection;
+	struct device		*dev;
+	int			minor;		/* vibrator minor number */
+	struct delayed_work     delayed_work;
+};
+
+/* Greybus Vibrator operation types */
+#define	GB_VIBRATOR_TYPE_ON			0x02
+#define	GB_VIBRATOR_TYPE_OFF			0x03
+
+static int turn_off(struct gb_vibrator_device *vib)
+{
+	struct gb_bundle *bundle = vib->connection->bundle;
+	int ret;
+
+	ret = gb_operation_sync(vib->connection, GB_VIBRATOR_TYPE_OFF,
+				NULL, 0, NULL, 0);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return ret;
+}
+
+static int turn_on(struct gb_vibrator_device *vib, u16 timeout_ms)
+{
+	struct gb_bundle *bundle = vib->connection->bundle;
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		return ret;
+
+	/* Vibrator was switched ON earlier */
+	if (cancel_delayed_work_sync(&vib->delayed_work))
+		turn_off(vib);
+
+	ret = gb_operation_sync(vib->connection, GB_VIBRATOR_TYPE_ON,
+				NULL, 0, NULL, 0);
+	if (ret) {
+		gb_pm_runtime_put_autosuspend(bundle);
+		return ret;
+	}
+
+	schedule_delayed_work(&vib->delayed_work, msecs_to_jiffies(timeout_ms));
+
+	return 0;
+}
+
+static void gb_vibrator_worker(struct work_struct *work)
+{
+	struct delayed_work *delayed_work = to_delayed_work(work);
+	struct gb_vibrator_device *vib =
+		container_of(delayed_work,
+			     struct gb_vibrator_device,
+			     delayed_work);
+
+	turn_off(vib);
+}
+
+static ssize_t timeout_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct gb_vibrator_device *vib = dev_get_drvdata(dev);
+	unsigned long val;
+	int retval;
+
+	retval = kstrtoul(buf, 10, &val);
+	if (retval < 0) {
+		dev_err(dev, "could not parse timeout value %d\n", retval);
+		return retval;
+	}
+
+	if (val)
+		retval = turn_on(vib, (u16)val);
+	else
+		retval = turn_off(vib);
+	if (retval)
+		return retval;
+
+	return count;
+}
+static DEVICE_ATTR_WO(timeout);
+
+static struct attribute *vibrator_attrs[] = {
+	&dev_attr_timeout.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(vibrator);
+
+static struct class vibrator_class = {
+	.name		= "vibrator",
+	.owner		= THIS_MODULE,
+	.dev_groups	= vibrator_groups,
+};
+
+static DEFINE_IDA(minors);
+
+static int gb_vibrator_probe(struct gb_bundle *bundle,
+			     const struct greybus_bundle_id *id)
+{
+	struct greybus_descriptor_cport *cport_desc;
+	struct gb_connection *connection;
+	struct gb_vibrator_device *vib;
+	struct device *dev;
+	int retval;
+
+	if (bundle->num_cports != 1)
+		return -ENODEV;
+
+	cport_desc = &bundle->cport_desc[0];
+	if (cport_desc->protocol_id != GREYBUS_PROTOCOL_VIBRATOR)
+		return -ENODEV;
+
+	vib = kzalloc(sizeof(*vib), GFP_KERNEL);
+	if (!vib)
+		return -ENOMEM;
+
+	connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id),
+					  NULL);
+	if (IS_ERR(connection)) {
+		retval = PTR_ERR(connection);
+		goto err_free_vib;
+	}
+	gb_connection_set_data(connection, vib);
+
+	vib->connection = connection;
+
+	greybus_set_drvdata(bundle, vib);
+
+	retval = gb_connection_enable(connection);
+	if (retval)
+		goto err_connection_destroy;
+
+	/*
+	 * For now we create a device in sysfs for the vibrator, but odds are
+	 * there is a "real" device somewhere in the kernel for this, but I
+	 * can't find it at the moment...
+	 */
+	vib->minor = ida_simple_get(&minors, 0, 0, GFP_KERNEL);
+	if (vib->minor < 0) {
+		retval = vib->minor;
+		goto err_connection_disable;
+	}
+	dev = device_create(&vibrator_class, &bundle->dev,
+			    MKDEV(0, 0), vib, "vibrator%d", vib->minor);
+	if (IS_ERR(dev)) {
+		retval = -EINVAL;
+		goto err_ida_remove;
+	}
+	vib->dev = dev;
+
+	INIT_DELAYED_WORK(&vib->delayed_work, gb_vibrator_worker);
+
+	gb_pm_runtime_put_autosuspend(bundle);
+
+	return 0;
+
+err_ida_remove:
+	ida_simple_remove(&minors, vib->minor);
+err_connection_disable:
+	gb_connection_disable(connection);
+err_connection_destroy:
+	gb_connection_destroy(connection);
+err_free_vib:
+	kfree(vib);
+
+	return retval;
+}
+
+static void gb_vibrator_disconnect(struct gb_bundle *bundle)
+{
+	struct gb_vibrator_device *vib = greybus_get_drvdata(bundle);
+	int ret;
+
+	ret = gb_pm_runtime_get_sync(bundle);
+	if (ret)
+		gb_pm_runtime_get_noresume(bundle);
+
+	if (cancel_delayed_work_sync(&vib->delayed_work))
+		turn_off(vib);
+
+	device_unregister(vib->dev);
+	ida_simple_remove(&minors, vib->minor);
+	gb_connection_disable(vib->connection);
+	gb_connection_destroy(vib->connection);
+	kfree(vib);
+}
+
+static const struct greybus_bundle_id gb_vibrator_id_table[] = {
+	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_VIBRATOR) },
+	{ }
+};
+MODULE_DEVICE_TABLE(greybus, gb_vibrator_id_table);
+
+static struct greybus_driver gb_vibrator_driver = {
+	.name		= "vibrator",
+	.probe		= gb_vibrator_probe,
+	.disconnect	= gb_vibrator_disconnect,
+	.id_table	= gb_vibrator_id_table,
+};
+
+static __init int gb_vibrator_init(void)
+{
+	int retval;
+
+	retval = class_register(&vibrator_class);
+	if (retval)
+		return retval;
+
+	retval = greybus_register(&gb_vibrator_driver);
+	if (retval)
+		goto err_class_unregister;
+
+	return 0;
+
+err_class_unregister:
+	class_unregister(&vibrator_class);
+
+	return retval;
+}
+module_init(gb_vibrator_init);
+
+static __exit void gb_vibrator_exit(void)
+{
+	greybus_deregister(&gb_vibrator_driver);
+	class_unregister(&vibrator_class);
+	ida_destroy(&minors);
+}
+module_exit(gb_vibrator_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/Documentation/inkernel.txt b/drivers/staging/iio/Documentation/inkernel.txt
new file mode 100644
index 000000000..ab528409b
--- /dev/null
+++ b/drivers/staging/iio/Documentation/inkernel.txt
@@ -0,0 +1,58 @@
+Industrial I/O Subsystem in kernel consumers.
+
+The IIO subsystem can act as a layer under other elements of the kernel
+providing a means of obtaining ADC type readings or of driving DAC type
+signals.  The functionality supported will grow as use cases arise.
+
+Describing the channel mapping (iio/machine.h)
+
+Channel associations are described using:
+
+struct iio_map {
+	const char *adc_channel_label;
+	const char *consumer_dev_name;
+	const char *consumer_channel;
+};
+
+adc_channel_label identifies the channel on the IIO device by being
+matched against the datasheet_name field of the iio_chan_spec.
+
+consumer_dev_name allows identification of the consumer device.
+This are then used to find the channel mapping from the consumer device (see
+below).
+
+Finally consumer_channel is a string identifying the channel to the consumer.
+(Perhaps 'battery_voltage' or similar).
+
+An array of these structures is then passed to the IIO driver.
+
+Supporting in kernel interfaces in the driver (driver.h)
+
+The driver must provide datasheet_name values for its channels and
+must pass the iio_map structures and a pointer to its own iio_dev structure
+ on to the core via a call to iio_map_array_register.  On removal,
+iio_map_array_unregister reverses this process.
+
+The result of this is that the IIO core now has all the information needed
+to associate a given channel with the consumer requesting it.
+
+Acting as an IIO consumer (consumer.h)
+
+The consumer first has to obtain an iio_channel structure from the core
+by calling iio_channel_get().  The correct channel is identified by:
+
+* matching dev or dev_name against consumer_dev and consumer_dev_name
+* matching consumer_channel against consumer_channel in the map
+
+There are then a number of functions that can be used to get information
+about this channel such as it's current reading.
+
+e.g.
+iio_read_channel_raw() - get a reading
+iio_get_channel_type() - get the type of channel
+
+There is also provision for retrieving all of the channels associated
+with a given consumer.  This is useful for generic drivers such as
+iio_hwmon where the number and naming of channels is not known by the
+consumer driver.  To do this, use iio_channel_get_all.
+
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-adc-ad7280a b/drivers/staging/iio/Documentation/sysfs-bus-iio-adc-ad7280a
new file mode 100644
index 000000000..863d38567
--- /dev/null
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-adc-ad7280a
@@ -0,0 +1,21 @@
+What:		/sys/bus/iio/devices/deviceX/inY-inZ_balance_switch_en
+KernelVersion:	3.0.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Writing 1 enables the cell balance output switch corresponding
+		to input Y. Writing 0 disables it. If the inY-inZ_balance_timer
+		is set to a none zero value, the corresponding switch will
+		enable for the programmed amount of time, before it
+		automatically disables.
+
+What:		/sys/bus/iio/devices/deviceX/inY-inZ_balance_timer
+KernelVersion:	3.0.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		The inY-inZ_balance_timer file allows the user to program
+		individual times for each cell balance output. The AD7280A
+		allows the user to set the timer to a value from 0 minutes to
+		36.9 minutes. The resolution of the timer is 71.5 sec.
+		The value written is the on-time in milliseconds. When the
+		timer value is set 0, the timer is disabled. The cell balance
+		outputs are controlled only by inY-inZ_balance_switch_en.
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-dds b/drivers/staging/iio/Documentation/sysfs-bus-iio-dds
new file mode 100644
index 000000000..ee8c509c6
--- /dev/null
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-dds
@@ -0,0 +1,96 @@
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencyY
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Stores frequency into tuning word Y.
+		There will be more than one out_altvoltageX_frequencyY file,
+		which allows for pin controlled FSK Frequency Shift Keying
+		(out_altvoltageX_pincontrol_frequency_en is active) or the user
+		can control the desired active tuning word by writing Y to the
+		out_altvoltageX_frequencysymbol file.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencyY_scale
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Scale to be applied to out_altvoltageX_frequencyY in order to
+		obtain the desired value in Hz. If shared across all frequency
+		registers Y is not present. It is also possible X is not present
+		if shared across all channels.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencysymbol
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Specifies the active output frequency tuning word. The value
+		corresponds to the Y in out_altvoltageX_frequencyY.
+		To exit this mode the user can write
+		out_altvoltageX_pincontrol_frequency_en or
+		out_altvoltageX_out_enable file.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phaseY
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Stores phase into Y.
+		There will be more than one out_altvoltageX_phaseY file, which
+		allows for pin controlled PSK Phase Shift Keying
+		(out_altvoltageX_pincontrol_phase_en is active) or the user can
+		control the desired phase Y which is added to the phase
+		accumulator output by writing Y to the phase_en file.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phaseY_scale
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Scale to be applied to out_altvoltageX_phaseY in order to obtain
+		the desired value in rad. If shared across all phase registers
+		Y is not present. It is also possible X is not present if
+		shared across all channels.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phasesymbol
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Specifies the active phase Y which is added to the phase
+		accumulator output. The value corresponds to the Y in
+		out_altvoltageX_phaseY. To exit this mode the user can write
+		out_altvoltageX_pincontrol_phase_en or disable file.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_en
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_frequency_en
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_phase_en
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		out_altvoltageX_pincontrol_en: Both, the active frequency and
+		phase is controlled by the respective phase and frequency
+		control inputs. In case the device in features independent
+		controls, then there are dedicated files
+		(out_altvoltageX_pincontrol_frequency_en,
+		out_altvoltageX_pincontrol_phase_en).
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_out_enable
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_enable
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		out_altvoltageX_outY_enable controls signal generation on
+		output Y of channel X. Y may be suppressed if all channels are
+		controlled together.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Specifies the output waveform.
+		(sine, triangle, ramp, square, ...)
+		For a list of available output waveform options read
+		available_output_modes.
+
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Lists all available output waveform options.
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
new file mode 100644
index 000000000..afd05bf33
--- /dev/null
+++ b/drivers/staging/iio/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Industrial I/O subsystem configuration
+#
+menu "IIO staging drivers"
+	depends on IIO
+
+source "drivers/staging/iio/accel/Kconfig"
+source "drivers/staging/iio/adc/Kconfig"
+source "drivers/staging/iio/addac/Kconfig"
+source "drivers/staging/iio/frequency/Kconfig"
+source "drivers/staging/iio/impedance-analyzer/Kconfig"
+source "drivers/staging/iio/meter/Kconfig"
+source "drivers/staging/iio/resolver/Kconfig"
+
+endmenu
diff --git a/drivers/staging/iio/Makefile b/drivers/staging/iio/Makefile
new file mode 100644
index 000000000..5ed56fe57
--- /dev/null
+++ b/drivers/staging/iio/Makefile
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the industrial I/O core.
+#
+
+obj-y += accel/
+obj-y += adc/
+obj-y += addac/
+obj-y += frequency/
+obj-y += impedance-analyzer/
+obj-y += meter/
+obj-y += resolver/
diff --git a/drivers/staging/iio/TODO b/drivers/staging/iio/TODO
new file mode 100644
index 000000000..0fa6a5500
--- /dev/null
+++ b/drivers/staging/iio/TODO
@@ -0,0 +1,5 @@
+2020-02-25
+
+
+Contact: Jonathan Cameron <jic23@kernel.org>.
+Mailing list: linux-iio@vger.kernel.org
diff --git a/drivers/staging/iio/accel/Kconfig b/drivers/staging/iio/accel/Kconfig
new file mode 100644
index 000000000..3318997a7
--- /dev/null
+++ b/drivers/staging/iio/accel/Kconfig
@@ -0,0 +1,31 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Accelerometer drivers
+#
+menu "Accelerometers"
+
+config ADIS16203
+	tristate "Analog Devices ADIS16203 Programmable 360 Degrees Inclinometer"
+	depends on SPI
+	select IIO_ADIS_LIB
+	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
+	help
+	  Say Y here to build support for Analog Devices adis16203 Programmable
+	  360 Degrees Inclinometer.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16203.
+
+config ADIS16240
+	tristate "Analog Devices ADIS16240 Programmable Impact Sensor and Recorder"
+	depends on SPI
+	select IIO_ADIS_LIB
+	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
+	help
+	  Say Y here to build support for Analog Devices adis16240 programmable
+	  impact Sensor and recorder.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16240.
+
+endmenu
diff --git a/drivers/staging/iio/accel/Makefile b/drivers/staging/iio/accel/Makefile
new file mode 100644
index 000000000..094cc9be3
--- /dev/null
+++ b/drivers/staging/iio/accel/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for industrial I/O accelerometer drivers
+#
+
+obj-$(CONFIG_ADIS16203) += adis16203.o
+obj-$(CONFIG_ADIS16240) += adis16240.o
diff --git a/drivers/staging/iio/accel/adis16203.c b/drivers/staging/iio/accel/adis16203.c
new file mode 100644
index 000000000..62d5397ff
--- /dev/null
+++ b/drivers/staging/iio/accel/adis16203.c
@@ -0,0 +1,315 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADIS16203 Programmable 360 Degrees Inclinometer
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/imu/adis.h>
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+
+#define ADIS16203_STARTUP_DELAY 220 /* ms */
+
+/* Flash memory write count */
+#define ADIS16203_FLASH_CNT      0x00
+
+/* Output, power supply */
+#define ADIS16203_SUPPLY_OUT     0x02
+
+/* Output, auxiliary ADC input */
+#define ADIS16203_AUX_ADC        0x08
+
+/* Output, temperature */
+#define ADIS16203_TEMP_OUT       0x0A
+
+/* Output, x-axis inclination */
+#define ADIS16203_XINCL_OUT      0x0C
+
+/* Output, y-axis inclination */
+#define ADIS16203_YINCL_OUT      0x0E
+
+/* Incline null calibration */
+#define ADIS16203_INCL_NULL      0x18
+
+/* Alarm 1 amplitude threshold */
+#define ADIS16203_ALM_MAG1       0x20
+
+/* Alarm 2 amplitude threshold */
+#define ADIS16203_ALM_MAG2       0x22
+
+/* Alarm 1, sample period */
+#define ADIS16203_ALM_SMPL1      0x24
+
+/* Alarm 2, sample period */
+#define ADIS16203_ALM_SMPL2      0x26
+
+/* Alarm control */
+#define ADIS16203_ALM_CTRL       0x28
+
+/* Auxiliary DAC data */
+#define ADIS16203_AUX_DAC        0x30
+
+/* General-purpose digital input/output control */
+#define ADIS16203_GPIO_CTRL      0x32
+
+/* Miscellaneous control */
+#define ADIS16203_MSC_CTRL       0x34
+
+/* Internal sample period (rate) control */
+#define ADIS16203_SMPL_PRD       0x36
+
+/* Operation, filter configuration */
+#define ADIS16203_AVG_CNT        0x38
+
+/* Operation, sleep mode control */
+#define ADIS16203_SLP_CNT        0x3A
+
+/* Diagnostics, system status register */
+#define ADIS16203_DIAG_STAT      0x3C
+
+/* Operation, system command register */
+#define ADIS16203_GLOB_CMD       0x3E
+
+/* MSC_CTRL */
+
+/* Self-test at power-on: 1 = disabled, 0 = enabled */
+#define ADIS16203_MSC_CTRL_PWRUP_SELF_TEST      BIT(10)
+
+/* Reverses rotation of both inclination outputs */
+#define ADIS16203_MSC_CTRL_REVERSE_ROT_EN       BIT(9)
+
+/* Self-test enable */
+#define ADIS16203_MSC_CTRL_SELF_TEST_EN         BIT(8)
+
+/* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16203_MSC_CTRL_DATA_RDY_EN          BIT(2)
+
+/* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16203_MSC_CTRL_ACTIVE_HIGH          BIT(1)
+
+/* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
+#define ADIS16203_MSC_CTRL_DATA_RDY_DIO1        BIT(0)
+
+/* DIAG_STAT */
+
+/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16203_DIAG_STAT_ALARM2        BIT(9)
+
+/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16203_DIAG_STAT_ALARM1        BIT(8)
+
+/* Self-test diagnostic error flag */
+#define ADIS16203_DIAG_STAT_SELFTEST_FAIL_BIT 5
+
+/* SPI communications failure */
+#define ADIS16203_DIAG_STAT_SPI_FAIL_BIT      3
+
+/* Flash update failure */
+#define ADIS16203_DIAG_STAT_FLASH_UPT_BIT     2
+
+/* Power supply above 3.625 V */
+#define ADIS16203_DIAG_STAT_POWER_HIGH_BIT    1
+
+/* Power supply below 2.975 V */
+#define ADIS16203_DIAG_STAT_POWER_LOW_BIT     0
+
+/* GLOB_CMD */
+
+#define ADIS16203_GLOB_CMD_SW_RESET     BIT(7)
+#define ADIS16203_GLOB_CMD_CLEAR_STAT   BIT(4)
+#define ADIS16203_GLOB_CMD_FACTORY_CAL  BIT(1)
+
+#define ADIS16203_ERROR_ACTIVE          BIT(14)
+
+enum adis16203_scan {
+	 ADIS16203_SCAN_INCLI_X,
+	 ADIS16203_SCAN_INCLI_Y,
+	 ADIS16203_SCAN_SUPPLY,
+	 ADIS16203_SCAN_AUX_ADC,
+	 ADIS16203_SCAN_TEMP,
+};
+
+#define DRIVER_NAME		"adis16203"
+
+static const u8 adis16203_addresses[] = {
+	[ADIS16203_SCAN_INCLI_X] = ADIS16203_INCL_NULL,
+};
+
+static int adis16203_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct adis *st = iio_priv(indio_dev);
+	/* currently only one writable parameter which keeps this simple */
+	u8 addr = adis16203_addresses[chan->scan_index];
+
+	return adis_write_reg_16(st, addr, val & 0x3FFF);
+}
+
+static int adis16203_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct adis *st = iio_priv(indio_dev);
+	int ret;
+	u8 addr;
+	s16 val16;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return adis_single_conversion(indio_dev, chan,
+				ADIS16203_ERROR_ACTIVE, val);
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			if (chan->channel == 0) {
+				*val = 1;
+				*val2 = 220000; /* 1.22 mV */
+			} else {
+				*val = 0;
+				*val2 = 610000; /* 0.61 mV */
+			}
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_TEMP:
+			*val = -470; /* -0.47 C */
+			*val2 = 0;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_INCLI:
+			*val = 0;
+			*val2 = 25000; /* 0.025 degree */
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 25000 / -470 - 1278; /* 25 C = 1278 */
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		addr = adis16203_addresses[chan->scan_index];
+		ret = adis_read_reg_16(st, addr, &val16);
+		if (ret)
+			return ret;
+		*val = sign_extend32(val16, 13);
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_chan_spec adis16203_channels[] = {
+	ADIS_SUPPLY_CHAN(ADIS16203_SUPPLY_OUT, ADIS16203_SCAN_SUPPLY, 0, 12),
+	ADIS_AUX_ADC_CHAN(ADIS16203_AUX_ADC, ADIS16203_SCAN_AUX_ADC, 0, 12),
+	ADIS_INCLI_CHAN(X, ADIS16203_XINCL_OUT, ADIS16203_SCAN_INCLI_X,
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+	/* Fixme: Not what it appears to be - see data sheet */
+	ADIS_INCLI_CHAN(Y, ADIS16203_YINCL_OUT, ADIS16203_SCAN_INCLI_Y,
+			0, 0, 14),
+	ADIS_TEMP_CHAN(ADIS16203_TEMP_OUT, ADIS16203_SCAN_TEMP, 0, 12),
+	IIO_CHAN_SOFT_TIMESTAMP(5),
+};
+
+static const struct iio_info adis16203_info = {
+	.read_raw = adis16203_read_raw,
+	.write_raw = adis16203_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
+};
+
+static const char * const adis16203_status_error_msgs[] = {
+	[ADIS16203_DIAG_STAT_SELFTEST_FAIL_BIT] = "Self test failure",
+	[ADIS16203_DIAG_STAT_SPI_FAIL_BIT] = "SPI failure",
+	[ADIS16203_DIAG_STAT_FLASH_UPT_BIT] = "Flash update failed",
+	[ADIS16203_DIAG_STAT_POWER_HIGH_BIT] = "Power supply above 3.625V",
+	[ADIS16203_DIAG_STAT_POWER_LOW_BIT] = "Power supply below 2.975V",
+};
+
+static const struct adis_timeout adis16203_timeouts = {
+	.reset_ms = ADIS16203_STARTUP_DELAY,
+	.sw_reset_ms = ADIS16203_STARTUP_DELAY,
+	.self_test_ms = ADIS16203_STARTUP_DELAY
+};
+
+static const struct adis_data adis16203_data = {
+	.read_delay = 20,
+	.msc_ctrl_reg = ADIS16203_MSC_CTRL,
+	.glob_cmd_reg = ADIS16203_GLOB_CMD,
+	.diag_stat_reg = ADIS16203_DIAG_STAT,
+
+	.self_test_mask = ADIS16203_MSC_CTRL_SELF_TEST_EN,
+	.self_test_reg = ADIS16203_MSC_CTRL,
+	.self_test_no_autoclear = true,
+	.timeouts = &adis16203_timeouts,
+
+	.status_error_msgs = adis16203_status_error_msgs,
+	.status_error_mask = BIT(ADIS16203_DIAG_STAT_SELFTEST_FAIL_BIT) |
+		BIT(ADIS16203_DIAG_STAT_SPI_FAIL_BIT) |
+		BIT(ADIS16203_DIAG_STAT_FLASH_UPT_BIT) |
+		BIT(ADIS16203_DIAG_STAT_POWER_HIGH_BIT) |
+		BIT(ADIS16203_DIAG_STAT_POWER_LOW_BIT),
+};
+
+static int adis16203_probe(struct spi_device *spi)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct adis *st;
+
+	/* setup the industrialio driver allocated elements */
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+	st = iio_priv(indio_dev);
+	/* this is only used for removal purposes */
+	spi_set_drvdata(spi, indio_dev);
+
+	indio_dev->name = spi->dev.driver->name;
+	indio_dev->channels = adis16203_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adis16203_channels);
+	indio_dev->info = &adis16203_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = adis_init(st, indio_dev, spi, &adis16203_data);
+	if (ret)
+		return ret;
+
+	ret = devm_adis_setup_buffer_and_trigger(st, indio_dev, NULL);
+	if (ret)
+		return ret;
+
+	/* Get the device into a sane initial state */
+	ret = adis_initial_startup(st);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct of_device_id adis16203_of_match[] = {
+	{ .compatible = "adi,adis16203" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, adis16203_of_match);
+
+static struct spi_driver adis16203_driver = {
+	.driver = {
+		.name = "adis16203",
+		.of_match_table = adis16203_of_match,
+	},
+	.probe = adis16203_probe,
+};
+module_spi_driver(adis16203_driver);
+
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices ADIS16203 Programmable 360 Degrees Inclinometer");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16203");
+MODULE_IMPORT_NS(IIO_ADISLIB);
diff --git a/drivers/staging/iio/accel/adis16240.c b/drivers/staging/iio/accel/adis16240.c
new file mode 100644
index 000000000..bca857eef
--- /dev/null
+++ b/drivers/staging/iio/accel/adis16240.c
@@ -0,0 +1,443 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADIS16240 Programmable Impact Sensor and Recorder driver
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/sysfs.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/imu/adis.h>
+
+#define ADIS16240_STARTUP_DELAY	220 /* ms */
+
+/* Flash memory write count */
+#define ADIS16240_FLASH_CNT      0x00
+
+/* Output, power supply */
+#define ADIS16240_SUPPLY_OUT     0x02
+
+/* Output, x-axis accelerometer */
+#define ADIS16240_XACCL_OUT      0x04
+
+/* Output, y-axis accelerometer */
+#define ADIS16240_YACCL_OUT      0x06
+
+/* Output, z-axis accelerometer */
+#define ADIS16240_ZACCL_OUT      0x08
+
+/* Output, auxiliary ADC input */
+#define ADIS16240_AUX_ADC        0x0A
+
+/* Output, temperature */
+#define ADIS16240_TEMP_OUT       0x0C
+
+/* Output, x-axis acceleration peak */
+#define ADIS16240_XPEAK_OUT      0x0E
+
+/* Output, y-axis acceleration peak */
+#define ADIS16240_YPEAK_OUT      0x10
+
+/* Output, z-axis acceleration peak */
+#define ADIS16240_ZPEAK_OUT      0x12
+
+/* Output, sum-of-squares acceleration peak */
+#define ADIS16240_XYZPEAK_OUT    0x14
+
+/* Output, Capture Buffer 1, X and Y acceleration */
+#define ADIS16240_CAPT_BUF1      0x16
+
+/* Output, Capture Buffer 2, Z acceleration */
+#define ADIS16240_CAPT_BUF2      0x18
+
+/* Diagnostic, error flags */
+#define ADIS16240_DIAG_STAT      0x1A
+
+/* Diagnostic, event counter */
+#define ADIS16240_EVNT_CNTR      0x1C
+
+/* Diagnostic, check sum value from firmware test */
+#define ADIS16240_CHK_SUM        0x1E
+
+/* Calibration, x-axis acceleration offset adjustment */
+#define ADIS16240_XACCL_OFF      0x20
+
+/* Calibration, y-axis acceleration offset adjustment */
+#define ADIS16240_YACCL_OFF      0x22
+
+/* Calibration, z-axis acceleration offset adjustment */
+#define ADIS16240_ZACCL_OFF      0x24
+
+/* Clock, hour and minute */
+#define ADIS16240_CLK_TIME       0x2E
+
+/* Clock, month and day */
+#define ADIS16240_CLK_DATE       0x30
+
+/* Clock, year */
+#define ADIS16240_CLK_YEAR       0x32
+
+/* Wake-up setting, hour and minute */
+#define ADIS16240_WAKE_TIME      0x34
+
+/* Wake-up setting, month and day */
+#define ADIS16240_WAKE_DATE      0x36
+
+/* Alarm 1 amplitude threshold */
+#define ADIS16240_ALM_MAG1       0x38
+
+/* Alarm 2 amplitude threshold */
+#define ADIS16240_ALM_MAG2       0x3A
+
+/* Alarm control */
+#define ADIS16240_ALM_CTRL       0x3C
+
+/* Capture, external trigger control */
+#define ADIS16240_XTRIG_CTRL     0x3E
+
+/* Capture, address pointer */
+#define ADIS16240_CAPT_PNTR      0x40
+
+/* Capture, configuration and control */
+#define ADIS16240_CAPT_CTRL      0x42
+
+/* General-purpose digital input/output control */
+#define ADIS16240_GPIO_CTRL      0x44
+
+/* Miscellaneous control */
+#define ADIS16240_MSC_CTRL       0x46
+
+/* Internal sample period (rate) control */
+#define ADIS16240_SMPL_PRD       0x48
+
+/* System command */
+#define ADIS16240_GLOB_CMD       0x4A
+
+/* MSC_CTRL */
+
+/* Enables sum-of-squares output (XYZPEAK_OUT) */
+#define ADIS16240_MSC_CTRL_XYZPEAK_OUT_EN	BIT(15)
+
+/* Enables peak tracking output (XPEAK_OUT, YPEAK_OUT, and ZPEAK_OUT) */
+#define ADIS16240_MSC_CTRL_X_Y_ZPEAK_OUT_EN	BIT(14)
+
+/* Self-test enable: 1 = apply electrostatic force, 0 = disabled */
+#define ADIS16240_MSC_CTRL_SELF_TEST_EN	        BIT(8)
+
+/* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16240_MSC_CTRL_DATA_RDY_EN	        BIT(2)
+
+/* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16240_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
+
+/* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
+#define ADIS16240_MSC_CTRL_DATA_RDY_DIO2	BIT(0)
+
+/* DIAG_STAT */
+
+/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16240_DIAG_STAT_ALARM2      BIT(9)
+
+/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16240_DIAG_STAT_ALARM1      BIT(8)
+
+/* Capture buffer full: 1 = capture buffer is full */
+#define ADIS16240_DIAG_STAT_CPT_BUF_FUL BIT(7)
+
+/* Flash test, checksum flag: 1 = mismatch, 0 = match */
+#define ADIS16240_DIAG_STAT_CHKSUM      BIT(6)
+
+/* Power-on, self-test flag: 1 = failure, 0 = pass */
+#define ADIS16240_DIAG_STAT_PWRON_FAIL_BIT  5
+
+/* Power-on self-test: 1 = in-progress, 0 = complete */
+#define ADIS16240_DIAG_STAT_PWRON_BUSY  BIT(4)
+
+/* SPI communications failure */
+#define ADIS16240_DIAG_STAT_SPI_FAIL_BIT	3
+
+/* Flash update failure */
+#define ADIS16240_DIAG_STAT_FLASH_UPT_BIT	2
+
+/* Power supply above 3.625 V */
+#define ADIS16240_DIAG_STAT_POWER_HIGH_BIT	1
+
+ /* Power supply below 2.225 V */
+#define ADIS16240_DIAG_STAT_POWER_LOW_BIT	0
+
+/* GLOB_CMD */
+
+#define ADIS16240_GLOB_CMD_RESUME	BIT(8)
+#define ADIS16240_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16240_GLOB_CMD_STANDBY	BIT(2)
+
+#define ADIS16240_ERROR_ACTIVE          BIT(14)
+
+/* At the moment triggers are only used for ring buffer
+ * filling. This may change!
+ */
+
+enum adis16240_scan {
+	ADIS16240_SCAN_ACC_X,
+	ADIS16240_SCAN_ACC_Y,
+	ADIS16240_SCAN_ACC_Z,
+	ADIS16240_SCAN_SUPPLY,
+	ADIS16240_SCAN_AUX_ADC,
+	ADIS16240_SCAN_TEMP,
+};
+
+static ssize_t adis16240_spi_read_signed(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf,
+					 unsigned int bits)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct adis *st = iio_priv(indio_dev);
+	int ret;
+	s16 val = 0;
+	unsigned int shift = 16 - bits;
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+
+	ret = adis_read_reg_16(st,
+			       this_attr->address, (u16 *)&val);
+	if (ret)
+		return ret;
+
+	if (val & ADIS16240_ERROR_ACTIVE)
+		adis_check_status(st);
+
+	val = (s16)(val << shift) >> shift;
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t adis16240_read_12bit_signed(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	return adis16240_spi_read_signed(dev, attr, buf, 12);
+}
+
+static IIO_DEVICE_ATTR(in_accel_xyz_squared_peak_raw, 0444,
+		       adis16240_read_12bit_signed, NULL,
+		       ADIS16240_XYZPEAK_OUT);
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("4096");
+
+static const u8 adis16240_addresses[][2] = {
+	[ADIS16240_SCAN_ACC_X] = { ADIS16240_XACCL_OFF, ADIS16240_XPEAK_OUT },
+	[ADIS16240_SCAN_ACC_Y] = { ADIS16240_YACCL_OFF, ADIS16240_YPEAK_OUT },
+	[ADIS16240_SCAN_ACC_Z] = { ADIS16240_ZACCL_OFF, ADIS16240_ZPEAK_OUT },
+};
+
+static int adis16240_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct adis *st = iio_priv(indio_dev);
+	int ret;
+	u8 addr;
+	s16 val16;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return adis_single_conversion(indio_dev, chan,
+				ADIS16240_ERROR_ACTIVE, val);
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			if (chan->channel == 0) {
+				*val = 4;
+				*val2 = 880000; /* 4.88 mV */
+				return IIO_VAL_INT_PLUS_MICRO;
+			}
+			return -EINVAL;
+		case IIO_TEMP:
+			*val = 244; /* 0.244 C */
+			*val2 = 0;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_ACCEL:
+			*val = 0;
+			*val2 = IIO_G_TO_M_S_2(51400); /* 51.4 mg */
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case IIO_CHAN_INFO_PEAK_SCALE:
+		*val = 0;
+		*val2 = IIO_G_TO_M_S_2(51400); /* 51.4 mg */
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 25000 / 244 - 0x133; /* 25 C = 0x133 */
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		addr = adis16240_addresses[chan->scan_index][0];
+		ret = adis_read_reg_16(st, addr, &val16);
+		if (ret)
+			return ret;
+		*val = sign_extend32(val16, 9);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_PEAK:
+		addr = adis16240_addresses[chan->scan_index][1];
+		ret = adis_read_reg_16(st, addr, &val16);
+		if (ret)
+			return ret;
+		*val = sign_extend32(val16, 9);
+		return IIO_VAL_INT;
+	}
+	return -EINVAL;
+}
+
+static int adis16240_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct adis *st = iio_priv(indio_dev);
+	u8 addr;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBBIAS:
+		addr = adis16240_addresses[chan->scan_index][0];
+		return adis_write_reg_16(st, addr, val & GENMASK(9, 0));
+	}
+	return -EINVAL;
+}
+
+static const struct iio_chan_spec adis16240_channels[] = {
+	ADIS_SUPPLY_CHAN(ADIS16240_SUPPLY_OUT, ADIS16240_SCAN_SUPPLY, 0, 10),
+	ADIS_AUX_ADC_CHAN(ADIS16240_AUX_ADC, ADIS16240_SCAN_AUX_ADC, 0, 10),
+	ADIS_ACCEL_CHAN(X, ADIS16240_XACCL_OUT, ADIS16240_SCAN_ACC_X,
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
+	ADIS_ACCEL_CHAN(Y, ADIS16240_YACCL_OUT, ADIS16240_SCAN_ACC_Y,
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
+	ADIS_ACCEL_CHAN(Z, ADIS16240_ZACCL_OUT, ADIS16240_SCAN_ACC_Z,
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
+	ADIS_TEMP_CHAN(ADIS16240_TEMP_OUT, ADIS16240_SCAN_TEMP, 0, 10),
+	IIO_CHAN_SOFT_TIMESTAMP(6)
+};
+
+static struct attribute *adis16240_attributes[] = {
+	&iio_dev_attr_in_accel_xyz_squared_peak_raw.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group adis16240_attribute_group = {
+	.attrs = adis16240_attributes,
+};
+
+static const struct iio_info adis16240_info = {
+	.attrs = &adis16240_attribute_group,
+	.read_raw = adis16240_read_raw,
+	.write_raw = adis16240_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
+};
+
+static const char * const adis16240_status_error_msgs[] = {
+	[ADIS16240_DIAG_STAT_PWRON_FAIL_BIT] = "Power on, self-test failed",
+	[ADIS16240_DIAG_STAT_SPI_FAIL_BIT] = "SPI failure",
+	[ADIS16240_DIAG_STAT_FLASH_UPT_BIT] = "Flash update failed",
+	[ADIS16240_DIAG_STAT_POWER_HIGH_BIT] = "Power supply above 3.625V",
+	[ADIS16240_DIAG_STAT_POWER_LOW_BIT] = "Power supply below 2.225V",
+};
+
+static const struct adis_timeout adis16240_timeouts = {
+	.reset_ms = ADIS16240_STARTUP_DELAY,
+	.sw_reset_ms = ADIS16240_STARTUP_DELAY,
+	.self_test_ms = ADIS16240_STARTUP_DELAY,
+};
+
+static const struct adis_data adis16240_data = {
+	.write_delay = 35,
+	.read_delay = 35,
+	.msc_ctrl_reg = ADIS16240_MSC_CTRL,
+	.glob_cmd_reg = ADIS16240_GLOB_CMD,
+	.diag_stat_reg = ADIS16240_DIAG_STAT,
+
+	.self_test_mask = ADIS16240_MSC_CTRL_SELF_TEST_EN,
+	.self_test_reg = ADIS16240_MSC_CTRL,
+	.self_test_no_autoclear = true,
+	.timeouts = &adis16240_timeouts,
+
+	.status_error_msgs = adis16240_status_error_msgs,
+	.status_error_mask = BIT(ADIS16240_DIAG_STAT_PWRON_FAIL_BIT) |
+		BIT(ADIS16240_DIAG_STAT_SPI_FAIL_BIT) |
+		BIT(ADIS16240_DIAG_STAT_FLASH_UPT_BIT) |
+		BIT(ADIS16240_DIAG_STAT_POWER_HIGH_BIT) |
+		BIT(ADIS16240_DIAG_STAT_POWER_LOW_BIT),
+};
+
+static int adis16240_probe(struct spi_device *spi)
+{
+	int ret;
+	struct adis *st;
+	struct iio_dev *indio_dev;
+
+	/* setup the industrialio driver allocated elements */
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+	st = iio_priv(indio_dev);
+	/* this is only used for removal purposes */
+	spi_set_drvdata(spi, indio_dev);
+
+	indio_dev->name = spi->dev.driver->name;
+	indio_dev->info = &adis16240_info;
+	indio_dev->channels = adis16240_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adis16240_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	spi->mode = SPI_MODE_3;
+	ret = spi_setup(spi);
+	if (ret) {
+		dev_err(&spi->dev, "spi_setup failed!\n");
+		return ret;
+	}
+
+	ret = adis_init(st, indio_dev, spi, &adis16240_data);
+	if (ret)
+		return ret;
+	ret = devm_adis_setup_buffer_and_trigger(st, indio_dev, NULL);
+	if (ret)
+		return ret;
+
+	/* Get the device into a sane initial state */
+	ret = adis_initial_startup(st);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct of_device_id adis16240_of_match[] = {
+	{ .compatible = "adi,adis16240" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, adis16240_of_match);
+
+static struct spi_driver adis16240_driver = {
+	.driver = {
+		.name = "adis16240",
+		.of_match_table = adis16240_of_match,
+	},
+	.probe = adis16240_probe,
+};
+module_spi_driver(adis16240_driver);
+
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices Programmable Impact Sensor and Recorder");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16240");
+MODULE_IMPORT_NS(IIO_ADISLIB);
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
new file mode 100644
index 000000000..2f0d6cf04
--- /dev/null
+++ b/drivers/staging/iio/adc/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# ADC drivers
+#
+menu "Analog to digital converters"
+
+config AD7816
+	tristate "Analog Devices AD7816/7/8 temperature sensor and ADC driver"
+	depends on SPI
+	depends on GPIOLIB || COMPILE_TEST
+	help
+	  Say yes here to build support for Analog Devices AD7816/7/8
+	  temperature sensors and ADC.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad7816.
+
+endmenu
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
new file mode 100644
index 000000000..1e2a94c4d
--- /dev/null
+++ b/drivers/staging/iio/adc/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for industrial I/O ADC drivers
+#
+
+obj-$(CONFIG_AD7816) += ad7816.o
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
new file mode 100644
index 000000000..6c14d7bcd
--- /dev/null
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -0,0 +1,451 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * AD7816 digital temperature sensor driver supporting AD7816/7/8
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/gpio/consumer.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/list.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+
+/*
+ * AD7816 config masks
+ */
+#define AD7816_FULL			0x1
+#define AD7816_PD			0x2
+#define AD7816_CS_MASK			0x7
+#define AD7816_CS_MAX			0x4
+
+/*
+ * AD7816 temperature masks
+ */
+#define AD7816_VALUE_OFFSET		6
+#define AD7816_BOUND_VALUE_BASE		0x8
+#define AD7816_BOUND_VALUE_MIN		-95
+#define AD7816_BOUND_VALUE_MAX		152
+#define AD7816_TEMP_FLOAT_OFFSET	2
+#define AD7816_TEMP_FLOAT_MASK		0x3
+
+/*
+ * struct ad7816_chip_info - chip specific information
+ */
+
+struct ad7816_chip_info {
+	kernel_ulong_t id;
+	struct spi_device *spi_dev;
+	struct gpio_desc *rdwr_pin;
+	struct gpio_desc *convert_pin;
+	struct gpio_desc *busy_pin;
+	u8  oti_data[AD7816_CS_MAX + 1];
+	u8  channel_id;	/* 0 always be temperature */
+	u8  mode;
+};
+
+enum ad7816_type {
+	ID_AD7816,
+	ID_AD7817,
+	ID_AD7818,
+};
+
+/*
+ * ad7816 data access by SPI
+ */
+static int ad7816_spi_read(struct ad7816_chip_info *chip, u16 *data)
+{
+	struct spi_device *spi_dev = chip->spi_dev;
+	int ret;
+	__be16 buf;
+
+	gpiod_set_value(chip->rdwr_pin, 1);
+	gpiod_set_value(chip->rdwr_pin, 0);
+	ret = spi_write(spi_dev, &chip->channel_id, sizeof(chip->channel_id));
+	if (ret < 0) {
+		dev_err(&spi_dev->dev, "SPI channel setting error\n");
+		return ret;
+	}
+	gpiod_set_value(chip->rdwr_pin, 1);
+
+	if (chip->mode == AD7816_PD) { /* operating mode 2 */
+		gpiod_set_value(chip->convert_pin, 1);
+		gpiod_set_value(chip->convert_pin, 0);
+	} else { /* operating mode 1 */
+		gpiod_set_value(chip->convert_pin, 0);
+		gpiod_set_value(chip->convert_pin, 1);
+	}
+
+	if (chip->id == ID_AD7816 || chip->id == ID_AD7817) {
+		while (gpiod_get_value(chip->busy_pin))
+			cpu_relax();
+	}
+
+	gpiod_set_value(chip->rdwr_pin, 0);
+	gpiod_set_value(chip->rdwr_pin, 1);
+	ret = spi_read(spi_dev, &buf, sizeof(*data));
+	if (ret < 0) {
+		dev_err(&spi_dev->dev, "SPI data read error\n");
+		return ret;
+	}
+
+	*data = be16_to_cpu(buf);
+
+	return ret;
+}
+
+static int ad7816_spi_write(struct ad7816_chip_info *chip, u8 data)
+{
+	struct spi_device *spi_dev = chip->spi_dev;
+	int ret;
+
+	gpiod_set_value(chip->rdwr_pin, 1);
+	gpiod_set_value(chip->rdwr_pin, 0);
+	ret = spi_write(spi_dev, &data, sizeof(data));
+	if (ret < 0)
+		dev_err(&spi_dev->dev, "SPI oti data write error\n");
+
+	return ret;
+}
+
+static ssize_t ad7816_show_mode(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+
+	if (chip->mode)
+		return sprintf(buf, "power-save\n");
+	return sprintf(buf, "full\n");
+}
+
+static ssize_t ad7816_store_mode(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf,
+				 size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+
+	if (strcmp(buf, "full")) {
+		gpiod_set_value(chip->rdwr_pin, 1);
+		chip->mode = AD7816_FULL;
+	} else {
+		gpiod_set_value(chip->rdwr_pin, 0);
+		chip->mode = AD7816_PD;
+	}
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(mode, 0644,
+		ad7816_show_mode,
+		ad7816_store_mode,
+		0);
+
+static ssize_t ad7816_show_available_modes(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	return sprintf(buf, "full\npower-save\n");
+}
+
+static IIO_DEVICE_ATTR(available_modes, 0444, ad7816_show_available_modes,
+			NULL, 0);
+
+static ssize_t ad7816_show_channel(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n", chip->channel_id);
+}
+
+static ssize_t ad7816_store_channel(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf,
+				    size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+	unsigned long data;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &data);
+	if (ret)
+		return ret;
+
+	if (data > AD7816_CS_MAX && data != AD7816_CS_MASK) {
+		dev_err(&chip->spi_dev->dev, "Invalid channel id %lu for %s.\n",
+			data, indio_dev->name);
+		return -EINVAL;
+	} else if (strcmp(indio_dev->name, "ad7818") == 0 && data > 1) {
+		dev_err(&chip->spi_dev->dev,
+			"Invalid channel id %lu for ad7818.\n", data);
+		return -EINVAL;
+	} else if (strcmp(indio_dev->name, "ad7816") == 0 && data > 0) {
+		dev_err(&chip->spi_dev->dev,
+			"Invalid channel id %lu for ad7816.\n", data);
+		return -EINVAL;
+	}
+
+	chip->channel_id = data;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(channel, 0644,
+		ad7816_show_channel,
+		ad7816_store_channel,
+		0);
+
+static ssize_t ad7816_show_value(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+	u16 data;
+	s8 value;
+	int ret;
+
+	ret = ad7816_spi_read(chip, &data);
+	if (ret)
+		return -EIO;
+
+	data >>= AD7816_VALUE_OFFSET;
+
+	if (chip->channel_id == 0) {
+		value = (s8)((data >> AD7816_TEMP_FLOAT_OFFSET) - 103);
+		data &= AD7816_TEMP_FLOAT_MASK;
+		if (value < 0)
+			data = BIT(AD7816_TEMP_FLOAT_OFFSET) - data;
+		return sprintf(buf, "%d.%.2d\n", value, data * 25);
+	}
+	return sprintf(buf, "%u\n", data);
+}
+
+static IIO_DEVICE_ATTR(value, 0444, ad7816_show_value, NULL, 0);
+
+static struct attribute *ad7816_attributes[] = {
+	&iio_dev_attr_available_modes.dev_attr.attr,
+	&iio_dev_attr_mode.dev_attr.attr,
+	&iio_dev_attr_channel.dev_attr.attr,
+	&iio_dev_attr_value.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad7816_attribute_group = {
+	.attrs = ad7816_attributes,
+};
+
+/*
+ * temperature bound events
+ */
+
+#define IIO_EVENT_CODE_AD7816_OTI IIO_UNMOD_EVENT_CODE(IIO_TEMP,	\
+						       0,		\
+						       IIO_EV_TYPE_THRESH, \
+						       IIO_EV_DIR_FALLING)
+
+static irqreturn_t ad7816_event_handler(int irq, void *private)
+{
+	iio_push_event(private, IIO_EVENT_CODE_AD7816_OTI,
+		       iio_get_time_ns(private));
+	return IRQ_HANDLED;
+}
+
+static ssize_t ad7816_show_oti(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+	int value;
+
+	if (chip->channel_id > AD7816_CS_MAX) {
+		dev_err(dev, "Invalid oti channel id %d.\n", chip->channel_id);
+		return -EINVAL;
+	} else if (chip->channel_id == 0) {
+		value = AD7816_BOUND_VALUE_MIN +
+			(chip->oti_data[chip->channel_id] -
+			AD7816_BOUND_VALUE_BASE);
+		return sprintf(buf, "%d\n", value);
+	}
+	return sprintf(buf, "%u\n", chip->oti_data[chip->channel_id]);
+}
+
+static inline ssize_t ad7816_set_oti(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7816_chip_info *chip = iio_priv(indio_dev);
+	long value;
+	u8 data;
+	int ret;
+
+	ret = kstrtol(buf, 10, &value);
+	if (ret)
+		return ret;
+
+	if (chip->channel_id > AD7816_CS_MAX) {
+		dev_err(dev, "Invalid oti channel id %d.\n", chip->channel_id);
+		return -EINVAL;
+	} else if (chip->channel_id == 0) {
+		if (value < AD7816_BOUND_VALUE_MIN ||
+		    value > AD7816_BOUND_VALUE_MAX)
+			return -EINVAL;
+
+		data = (u8)(value - AD7816_BOUND_VALUE_MIN +
+			AD7816_BOUND_VALUE_BASE);
+	} else {
+		if (value < AD7816_BOUND_VALUE_BASE || value > 255)
+			return -EINVAL;
+
+		data = (u8)value;
+	}
+
+	ret = ad7816_spi_write(chip, data);
+	if (ret)
+		return -EIO;
+
+	chip->oti_data[chip->channel_id] = data;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(oti, 0644,
+		       ad7816_show_oti, ad7816_set_oti, 0);
+
+static struct attribute *ad7816_event_attributes[] = {
+	&iio_dev_attr_oti.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad7816_event_attribute_group = {
+	.attrs = ad7816_event_attributes,
+	.name = "events",
+};
+
+static const struct iio_info ad7816_info = {
+	.attrs = &ad7816_attribute_group,
+	.event_attrs = &ad7816_event_attribute_group,
+};
+
+/*
+ * device probe and remove
+ */
+
+static int ad7816_probe(struct spi_device *spi_dev)
+{
+	struct ad7816_chip_info *chip;
+	struct iio_dev *indio_dev;
+	int i, ret;
+
+	indio_dev = devm_iio_device_alloc(&spi_dev->dev, sizeof(*chip));
+	if (!indio_dev)
+		return -ENOMEM;
+	chip = iio_priv(indio_dev);
+	/* this is only used for device removal purposes */
+	dev_set_drvdata(&spi_dev->dev, indio_dev);
+
+	chip->spi_dev = spi_dev;
+	for (i = 0; i <= AD7816_CS_MAX; i++)
+		chip->oti_data[i] = 203;
+
+	chip->id = spi_get_device_id(spi_dev)->driver_data;
+	chip->rdwr_pin = devm_gpiod_get(&spi_dev->dev, "rdwr", GPIOD_OUT_HIGH);
+	if (IS_ERR(chip->rdwr_pin)) {
+		ret = PTR_ERR(chip->rdwr_pin);
+		dev_err(&spi_dev->dev, "Failed to request rdwr GPIO: %d\n",
+			ret);
+		return ret;
+	}
+	chip->convert_pin = devm_gpiod_get(&spi_dev->dev, "convert",
+					   GPIOD_OUT_HIGH);
+	if (IS_ERR(chip->convert_pin)) {
+		ret = PTR_ERR(chip->convert_pin);
+		dev_err(&spi_dev->dev, "Failed to request convert GPIO: %d\n",
+			ret);
+		return ret;
+	}
+	if (chip->id == ID_AD7816 || chip->id == ID_AD7817) {
+		chip->busy_pin = devm_gpiod_get(&spi_dev->dev, "busy",
+						GPIOD_IN);
+		if (IS_ERR(chip->busy_pin)) {
+			ret = PTR_ERR(chip->busy_pin);
+			dev_err(&spi_dev->dev, "Failed to request busy GPIO: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	indio_dev->name = spi_get_device_id(spi_dev)->name;
+	indio_dev->info = &ad7816_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	if (spi_dev->irq) {
+		/* Only low trigger is supported in ad7816/7/8 */
+		ret = devm_request_threaded_irq(&spi_dev->dev, spi_dev->irq,
+						NULL,
+						&ad7816_event_handler,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						indio_dev->name,
+						indio_dev);
+		if (ret)
+			return ret;
+	}
+
+	ret = devm_iio_device_register(&spi_dev->dev, indio_dev);
+	if (ret)
+		return ret;
+
+	dev_info(&spi_dev->dev, "%s temperature sensor and ADC registered.\n",
+		 indio_dev->name);
+
+	return 0;
+}
+
+static const struct of_device_id ad7816_of_match[] = {
+	{ .compatible = "adi,ad7816", },
+	{ .compatible = "adi,ad7817", },
+	{ .compatible = "adi,ad7818", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ad7816_of_match);
+
+static const struct spi_device_id ad7816_id[] = {
+	{ "ad7816", ID_AD7816 },
+	{ "ad7817", ID_AD7817 },
+	{ "ad7818", ID_AD7818 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(spi, ad7816_id);
+
+static struct spi_driver ad7816_driver = {
+	.driver = {
+		.name = "ad7816",
+		.of_match_table = ad7816_of_match,
+	},
+	.probe = ad7816_probe,
+	.id_table = ad7816_id,
+};
+module_spi_driver(ad7816_driver);
+
+MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
+MODULE_DESCRIPTION("Analog Devices AD7816/7/8 digital temperature sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/addac/Kconfig b/drivers/staging/iio/addac/Kconfig
new file mode 100644
index 000000000..b7c3c4a7d
--- /dev/null
+++ b/drivers/staging/iio/addac/Kconfig
@@ -0,0 +1,38 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# ADDAC drivers
+#
+menu "Analog digital bi-direction converters"
+
+config ADT7316
+	tristate "Analog Devices ADT7316/7/8 ADT7516/7/9 temperature sensor, ADC and DAC driver"
+	depends on GPIOLIB || COMPILE_TEST
+	help
+	  Say yes here to build support for Analog Devices ADT7316, ADT7317, ADT7318
+	  and ADT7516, ADT7517, ADT7519 temperature sensors, ADC and DAC.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316.
+
+config ADT7316_SPI
+	tristate "support SPI bus connection"
+	depends on SPI && ADT7316
+	default y
+	help
+	  Say yes here to build SPI bus support for Analog Devices ADT7316/7/8
+	  and ADT7516/7/9.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316_spi.
+
+config ADT7316_I2C
+	tristate "support I2C bus connection"
+	depends on I2C && ADT7316
+	help
+	  Say yes here to build I2C bus support for Analog Devices ADT7316/7/8
+	  and ADT7516/7/9.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316_i2c.
+
+endmenu
diff --git a/drivers/staging/iio/addac/Makefile b/drivers/staging/iio/addac/Makefile
new file mode 100644
index 000000000..8fdbd8cab
--- /dev/null
+++ b/drivers/staging/iio/addac/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for industrial I/O ADDAC drivers
+#
+
+obj-$(CONFIG_ADT7316) += adt7316.o
+obj-$(CONFIG_ADT7316_SPI) += adt7316-spi.o
+obj-$(CONFIG_ADT7316_I2C) += adt7316-i2c.o
diff --git a/drivers/staging/iio/addac/adt7316-i2c.c b/drivers/staging/iio/addac/adt7316-i2c.c
new file mode 100644
index 000000000..5543cc909
--- /dev/null
+++ b/drivers/staging/iio/addac/adt7316-i2c.c
@@ -0,0 +1,148 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * I2C bus driver for ADT7316/7/8 ADT7516/7/9 digital temperature
+ * sensor, ADC and DAC
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+
+#include "adt7316.h"
+
+/*
+ * adt7316 register access by I2C
+ */
+static int adt7316_i2c_read(void *client, u8 reg, u8 *data)
+{
+	struct i2c_client *cl = client;
+	int ret;
+
+	ret = i2c_smbus_write_byte(cl, reg);
+	if (ret < 0) {
+		dev_err(&cl->dev, "I2C fail to select reg\n");
+		return ret;
+	}
+
+	ret = i2c_smbus_read_byte(client);
+	if (ret < 0) {
+		dev_err(&cl->dev, "I2C read error\n");
+		return ret;
+	}
+
+	*data = ret;
+
+	return 0;
+}
+
+static int adt7316_i2c_write(void *client, u8 reg, u8 data)
+{
+	struct i2c_client *cl = client;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(cl, reg, data);
+	if (ret < 0)
+		dev_err(&cl->dev, "I2C write error\n");
+
+	return ret;
+}
+
+static int adt7316_i2c_multi_read(void *client, u8 reg, u8 count, u8 *data)
+{
+	struct i2c_client *cl = client;
+	int i, ret;
+
+	if (count > ADT7316_REG_MAX_ADDR)
+		count = ADT7316_REG_MAX_ADDR;
+
+	for (i = 0; i < count; i++) {
+		ret = adt7316_i2c_read(cl, reg, &data[i]);
+		if (ret < 0) {
+			dev_err(&cl->dev, "I2C multi read error\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int adt7316_i2c_multi_write(void *client, u8 reg, u8 count, u8 *data)
+{
+	struct i2c_client *cl = client;
+	int i, ret;
+
+	if (count > ADT7316_REG_MAX_ADDR)
+		count = ADT7316_REG_MAX_ADDR;
+
+	for (i = 0; i < count; i++) {
+		ret = adt7316_i2c_write(cl, reg, data[i]);
+		if (ret < 0) {
+			dev_err(&cl->dev, "I2C multi write error\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * device probe and remove
+ */
+
+static int adt7316_i2c_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct adt7316_bus bus = {
+		.client = client,
+		.irq = client->irq,
+		.read = adt7316_i2c_read,
+		.write = adt7316_i2c_write,
+		.multi_read = adt7316_i2c_multi_read,
+		.multi_write = adt7316_i2c_multi_write,
+	};
+
+	return adt7316_probe(&client->dev, &bus, id->name);
+}
+
+static const struct i2c_device_id adt7316_i2c_id[] = {
+	{ "adt7316", 0 },
+	{ "adt7317", 0 },
+	{ "adt7318", 0 },
+	{ "adt7516", 0 },
+	{ "adt7517", 0 },
+	{ "adt7519", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, adt7316_i2c_id);
+
+static const struct of_device_id adt7316_of_match[] = {
+	{ .compatible = "adi,adt7316" },
+	{ .compatible = "adi,adt7317" },
+	{ .compatible = "adi,adt7318" },
+	{ .compatible = "adi,adt7516" },
+	{ .compatible = "adi,adt7517" },
+	{ .compatible = "adi,adt7519" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, adt7316_of_match);
+
+static struct i2c_driver adt7316_driver = {
+	.driver = {
+		.name = "adt7316",
+		.of_match_table = adt7316_of_match,
+		.pm = ADT7316_PM_OPS,
+	},
+	.probe = adt7316_i2c_probe,
+	.id_table = adt7316_i2c_id,
+};
+module_i2c_driver(adt7316_driver);
+
+MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
+MODULE_DESCRIPTION("I2C bus driver for Analog Devices ADT7316/7/9 and ADT7516/7/8 digital temperature sensor, ADC and DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/addac/adt7316-spi.c b/drivers/staging/iio/addac/adt7316-spi.c
new file mode 100644
index 000000000..af513e003
--- /dev/null
+++ b/drivers/staging/iio/addac/adt7316-spi.c
@@ -0,0 +1,154 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * API bus driver for ADT7316/7/8 ADT7516/7/9 digital temperature
+ * sensor, ADC and DAC
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/spi/spi.h>
+
+#include "adt7316.h"
+
+#define ADT7316_SPI_MAX_FREQ_HZ		5000000
+#define ADT7316_SPI_CMD_READ		0x91
+#define ADT7316_SPI_CMD_WRITE		0x90
+
+/*
+ * adt7316 register access by SPI
+ */
+
+static int adt7316_spi_multi_read(void *client, u8 reg, u8 count, u8 *data)
+{
+	struct spi_device *spi_dev = client;
+	u8 cmd[2];
+	int ret;
+
+	if (count > ADT7316_REG_MAX_ADDR)
+		count = ADT7316_REG_MAX_ADDR;
+
+	cmd[0] = ADT7316_SPI_CMD_WRITE;
+	cmd[1] = reg;
+
+	ret = spi_write(spi_dev, cmd, 2);
+	if (ret < 0) {
+		dev_err(&spi_dev->dev, "SPI fail to select reg\n");
+		return ret;
+	}
+
+	cmd[0] = ADT7316_SPI_CMD_READ;
+
+	ret = spi_write_then_read(spi_dev, cmd, 1, data, count);
+	if (ret < 0) {
+		dev_err(&spi_dev->dev, "SPI read data error\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int adt7316_spi_multi_write(void *client, u8 reg, u8 count, u8 *data)
+{
+	struct spi_device *spi_dev = client;
+	u8 buf[ADT7316_REG_MAX_ADDR + 2];
+	int i, ret;
+
+	if (count > ADT7316_REG_MAX_ADDR)
+		count = ADT7316_REG_MAX_ADDR;
+
+	buf[0] = ADT7316_SPI_CMD_WRITE;
+	buf[1] = reg;
+	for (i = 0; i < count; i++)
+		buf[i + 2] = data[i];
+
+	ret = spi_write(spi_dev, buf, count + 2);
+	if (ret < 0) {
+		dev_err(&spi_dev->dev, "SPI write error\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int adt7316_spi_read(void *client, u8 reg, u8 *data)
+{
+	return adt7316_spi_multi_read(client, reg, 1, data);
+}
+
+static int adt7316_spi_write(void *client, u8 reg, u8 val)
+{
+	return adt7316_spi_multi_write(client, reg, 1, &val);
+}
+
+/*
+ * device probe and remove
+ */
+
+static int adt7316_spi_probe(struct spi_device *spi_dev)
+{
+	struct adt7316_bus bus = {
+		.client = spi_dev,
+		.irq = spi_dev->irq,
+		.read = adt7316_spi_read,
+		.write = adt7316_spi_write,
+		.multi_read = adt7316_spi_multi_read,
+		.multi_write = adt7316_spi_multi_write,
+	};
+
+	/* don't exceed max specified SPI CLK frequency */
+	if (spi_dev->max_speed_hz > ADT7316_SPI_MAX_FREQ_HZ) {
+		dev_err(&spi_dev->dev, "SPI CLK %d Hz?\n",
+			spi_dev->max_speed_hz);
+		return -EINVAL;
+	}
+
+	/* switch from default I2C protocol to SPI protocol */
+	adt7316_spi_write(spi_dev, 0, 0);
+	adt7316_spi_write(spi_dev, 0, 0);
+	adt7316_spi_write(spi_dev, 0, 0);
+
+	return adt7316_probe(&spi_dev->dev, &bus, spi_dev->modalias);
+}
+
+static const struct spi_device_id adt7316_spi_id[] = {
+	{ "adt7316", 0 },
+	{ "adt7317", 0 },
+	{ "adt7318", 0 },
+	{ "adt7516", 0 },
+	{ "adt7517", 0 },
+	{ "adt7519", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(spi, adt7316_spi_id);
+
+static const struct of_device_id adt7316_of_spi_match[] = {
+	{ .compatible = "adi,adt7316" },
+	{ .compatible = "adi,adt7317" },
+	{ .compatible = "adi,adt7318" },
+	{ .compatible = "adi,adt7516" },
+	{ .compatible = "adi,adt7517" },
+	{ .compatible = "adi,adt7519" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, adt7316_of_spi_match);
+
+static struct spi_driver adt7316_driver = {
+	.driver = {
+		.name = "adt7316",
+		.of_match_table = adt7316_of_spi_match,
+		.pm = ADT7316_PM_OPS,
+	},
+	.probe = adt7316_spi_probe,
+	.id_table = adt7316_spi_id,
+};
+module_spi_driver(adt7316_driver);
+
+MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
+MODULE_DESCRIPTION("SPI bus driver for Analog Devices ADT7316/7/8 and ADT7516/7/9 digital temperature sensor, ADC and DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
new file mode 100644
index 000000000..79467f056
--- /dev/null
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -0,0 +1,2208 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADT7316 digital temperature sensor driver supporting ADT7316/7/8 ADT7516/7/9
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/gpio/consumer.h>
+#include <linux/irq.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/rtc.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/events.h>
+#include <linux/iio/sysfs.h>
+#include "adt7316.h"
+
+/*
+ * ADT7316 registers definition
+ */
+#define ADT7316_INT_STAT1		0x0
+#define ADT7316_INT_STAT2		0x1
+#define ADT7316_LSB_IN_TEMP_VDD		0x3
+#define ADT7316_LSB_IN_TEMP_MASK	0x3
+#define ADT7316_LSB_VDD_MASK		0xC
+#define ADT7316_LSB_VDD_OFFSET		2
+#define ADT7316_LSB_EX_TEMP_AIN		0x4
+#define ADT7316_LSB_EX_TEMP_MASK	0x3
+#define ADT7516_LSB_AIN_SHIFT		2
+#define ADT7316_AD_MSB_DATA_BASE        0x6
+#define ADT7316_AD_MSB_DATA_REGS        3
+#define ADT7516_AD_MSB_DATA_REGS        6
+#define ADT7316_MSB_VDD			0x6
+#define ADT7316_MSB_IN_TEMP		0x7
+#define ADT7316_MSB_EX_TEMP		0x8
+#define ADT7516_MSB_AIN1		0x8
+#define ADT7516_MSB_AIN2		0x9
+#define ADT7516_MSB_AIN3		0xA
+#define ADT7516_MSB_AIN4		0xB
+#define ADT7316_DA_DATA_BASE		0x10
+#define ADT7316_DA_10_BIT_LSB_SHIFT	6
+#define ADT7316_DA_12_BIT_LSB_SHIFT	4
+#define ADT7316_DA_MSB_DATA_REGS	4
+#define ADT7316_LSB_DAC_A		0x10
+#define ADT7316_MSB_DAC_A		0x11
+#define ADT7316_LSB_DAC_B		0x12
+#define ADT7316_MSB_DAC_B		0x13
+#define ADT7316_LSB_DAC_C		0x14
+#define ADT7316_MSB_DAC_C		0x15
+#define ADT7316_LSB_DAC_D		0x16
+#define ADT7316_MSB_DAC_D		0x17
+#define ADT7316_CONFIG1			0x18
+#define ADT7316_CONFIG2			0x19
+#define ADT7316_CONFIG3			0x1A
+#define ADT7316_DAC_CONFIG		0x1B
+#define ADT7316_LDAC_CONFIG		0x1C
+#define ADT7316_INT_MASK1		0x1D
+#define ADT7316_INT_MASK2		0x1E
+#define ADT7316_IN_TEMP_OFFSET		0x1F
+#define ADT7316_EX_TEMP_OFFSET		0x20
+#define ADT7316_IN_ANALOG_TEMP_OFFSET	0x21
+#define ADT7316_EX_ANALOG_TEMP_OFFSET	0x22
+#define ADT7316_VDD_HIGH		0x23
+#define ADT7316_VDD_LOW			0x24
+#define ADT7316_IN_TEMP_HIGH		0x25
+#define ADT7316_IN_TEMP_LOW		0x26
+#define ADT7316_EX_TEMP_HIGH		0x27
+#define ADT7316_EX_TEMP_LOW		0x28
+#define ADT7516_AIN2_HIGH		0x2B
+#define ADT7516_AIN2_LOW		0x2C
+#define ADT7516_AIN3_HIGH		0x2D
+#define ADT7516_AIN3_LOW		0x2E
+#define ADT7516_AIN4_HIGH		0x2F
+#define ADT7516_AIN4_LOW		0x30
+#define ADT7316_DEVICE_ID		0x4D
+#define ADT7316_MANUFACTURE_ID		0x4E
+#define ADT7316_DEVICE_REV		0x4F
+#define ADT7316_SPI_LOCK_STAT		0x7F
+
+/*
+ * ADT7316 config1
+ */
+#define ADT7316_EN			0x1
+#define ADT7516_SEL_EX_TEMP		0x4
+#define ADT7516_SEL_AIN1_2_EX_TEMP_MASK	0x6
+#define ADT7516_SEL_AIN3		0x8
+#define ADT7316_INT_EN			0x20
+#define ADT7316_INT_POLARITY		0x40
+#define ADT7316_PD			0x80
+
+/*
+ * ADT7316 config2
+ */
+#define ADT7316_AD_SINGLE_CH_MASK	0x3
+#define ADT7516_AD_SINGLE_CH_MASK	0x7
+#define ADT7316_AD_SINGLE_CH_VDD	0
+#define ADT7316_AD_SINGLE_CH_IN		1
+#define ADT7316_AD_SINGLE_CH_EX		2
+#define ADT7516_AD_SINGLE_CH_AIN1	2
+#define ADT7516_AD_SINGLE_CH_AIN2	3
+#define ADT7516_AD_SINGLE_CH_AIN3	4
+#define ADT7516_AD_SINGLE_CH_AIN4	5
+#define ADT7316_AD_SINGLE_CH_MODE	0x10
+#define ADT7316_DISABLE_AVERAGING	0x20
+#define ADT7316_EN_SMBUS_TIMEOUT	0x40
+#define ADT7316_RESET			0x80
+
+/*
+ * ADT7316 config3
+ */
+#define ADT7316_ADCLK_22_5		0x1
+#define ADT7316_DA_HIGH_RESOLUTION	0x2
+#define ADT7316_DA_EN_VIA_DAC_LDAC	0x8
+#define ADT7516_AIN_IN_VREF		0x10
+#define ADT7316_EN_IN_TEMP_PROP_DACA	0x20
+#define ADT7316_EN_EX_TEMP_PROP_DACB	0x40
+
+/*
+ * ADT7316 DAC config
+ */
+#define ADT7316_DA_2VREF_CH_MASK	0xF
+#define ADT7316_DA_EN_MODE_MASK		0x30
+#define ADT7316_DA_EN_MODE_SHIFT	4
+#define ADT7316_DA_EN_MODE_SINGLE	0x00
+#define ADT7316_DA_EN_MODE_AB_CD	0x10
+#define ADT7316_DA_EN_MODE_ABCD		0x20
+#define ADT7316_DA_EN_MODE_LDAC		0x30
+#define ADT7316_VREF_BYPASS_DAC_AB	0x40
+#define ADT7316_VREF_BYPASS_DAC_CD	0x80
+
+/*
+ * ADT7316 LDAC config
+ */
+#define ADT7316_LDAC_EN_DA_MASK		0xF
+#define ADT7316_DAC_IN_VREF		0x10
+#define ADT7516_DAC_AB_IN_VREF		0x10
+#define ADT7516_DAC_CD_IN_VREF		0x20
+#define ADT7516_DAC_IN_VREF_OFFSET	4
+#define ADT7516_DAC_IN_VREF_MASK	0x30
+
+/*
+ * ADT7316 INT_MASK2
+ */
+#define ADT7316_INT_MASK2_VDD		0x10
+
+/*
+ * ADT7316 value masks
+ */
+#define ADT7316_VALUE_MASK		0xfff
+#define ADT7316_T_VALUE_SIGN		0x400
+#define ADT7316_T_VALUE_FLOAT_OFFSET	2
+#define ADT7316_T_VALUE_FLOAT_MASK	0x2
+
+/*
+ * Chip ID
+ */
+#define ID_ADT7316		0x1
+#define ID_ADT7317		0x2
+#define ID_ADT7318		0x3
+#define ID_ADT7516		0x11
+#define ID_ADT7517		0x12
+#define ID_ADT7519		0x14
+
+#define ID_FAMILY_MASK		0xF0
+#define ID_ADT73XX		0x0
+#define ID_ADT75XX		0x10
+
+/*
+ * struct adt7316_chip_info - chip specific information
+ */
+
+struct adt7316_chip_info {
+	struct adt7316_bus	bus;
+	struct gpio_desc	*ldac_pin;
+	u16			int_mask;	/* 0x2f */
+	u8			config1;
+	u8			config2;
+	u8			config3;
+	u8			dac_config;	/* DAC config */
+	u8			ldac_config;	/* LDAC config */
+	u8			dac_bits;	/* 8, 10, 12 */
+	u8			id;		/* chip id */
+};
+
+/*
+ * Logic interrupt mask for user application to enable
+ * interrupts.
+ */
+#define ADT7316_IN_TEMP_HIGH_INT_MASK	0x1
+#define ADT7316_IN_TEMP_LOW_INT_MASK	0x2
+#define ADT7316_EX_TEMP_HIGH_INT_MASK	0x4
+#define ADT7316_EX_TEMP_LOW_INT_MASK	0x8
+#define ADT7316_EX_TEMP_FAULT_INT_MASK	0x10
+#define ADT7516_AIN1_INT_MASK		0x4
+#define ADT7516_AIN2_INT_MASK		0x20
+#define ADT7516_AIN3_INT_MASK		0x40
+#define ADT7516_AIN4_INT_MASK		0x80
+#define ADT7316_VDD_INT_MASK		0x100
+#define ADT7316_TEMP_INT_MASK		0x1F
+#define ADT7516_AIN_INT_MASK		0xE0
+#define ADT7316_TEMP_AIN_INT_MASK	\
+	(ADT7316_TEMP_INT_MASK)
+
+/*
+ * struct adt7316_chip_info - chip specific information
+ */
+
+struct adt7316_limit_regs {
+	u16	data_high;
+	u16	data_low;
+};
+
+static ssize_t adt7316_show_enabled(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_EN));
+}
+
+static ssize_t _adt7316_store_enabled(struct adt7316_chip_info *chip,
+				      int enable)
+{
+	u8 config1;
+	int ret;
+
+	if (enable)
+		config1 = chip->config1 | ADT7316_EN;
+	else
+		config1 = chip->config1 & ~ADT7316_EN;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
+	if (ret)
+		return -EIO;
+
+	chip->config1 = config1;
+
+	return ret;
+}
+
+static ssize_t adt7316_store_enabled(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	int enable;
+
+	if (buf[0] == '1')
+		enable = 1;
+	else
+		enable = 0;
+
+	if (_adt7316_store_enabled(chip, enable) < 0)
+		return -EIO;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(enabled, 0644,
+		adt7316_show_enabled,
+		adt7316_store_enabled,
+		0);
+
+static ssize_t adt7316_show_select_ex_temp(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
+		return -EPERM;
+
+	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7516_SEL_EX_TEMP));
+}
+
+static ssize_t adt7316_store_select_ex_temp(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf,
+					    size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config1;
+	int ret;
+
+	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
+		return -EPERM;
+
+	config1 = chip->config1 & (~ADT7516_SEL_EX_TEMP);
+	if (buf[0] == '1')
+		config1 |= ADT7516_SEL_EX_TEMP;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
+	if (ret)
+		return -EIO;
+
+	chip->config1 = config1;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(select_ex_temp, 0644,
+		adt7316_show_select_ex_temp,
+		adt7316_store_select_ex_temp,
+		0);
+
+static ssize_t adt7316_show_mode(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (chip->config2 & ADT7316_AD_SINGLE_CH_MODE)
+		return sprintf(buf, "single_channel\n");
+
+	return sprintf(buf, "round_robin\n");
+}
+
+static ssize_t adt7316_store_mode(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf,
+				  size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config2;
+	int ret;
+
+	config2 = chip->config2 & (~ADT7316_AD_SINGLE_CH_MODE);
+	if (!memcmp(buf, "single_channel", 14))
+		config2 |= ADT7316_AD_SINGLE_CH_MODE;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
+	if (ret)
+		return -EIO;
+
+	chip->config2 = config2;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(mode, 0644,
+		adt7316_show_mode,
+		adt7316_store_mode,
+		0);
+
+static ssize_t adt7316_show_all_modes(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	return sprintf(buf, "single_channel\nround_robin\n");
+}
+
+static IIO_DEVICE_ATTR(all_modes, 0444, adt7316_show_all_modes, NULL, 0);
+
+static ssize_t adt7316_show_ad_channel(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (!(chip->config2 & ADT7316_AD_SINGLE_CH_MODE))
+		return -EPERM;
+
+	switch (chip->config2 & ADT7516_AD_SINGLE_CH_MASK) {
+	case ADT7316_AD_SINGLE_CH_VDD:
+		return sprintf(buf, "0 - VDD\n");
+	case ADT7316_AD_SINGLE_CH_IN:
+		return sprintf(buf, "1 - Internal Temperature\n");
+	case ADT7316_AD_SINGLE_CH_EX:
+		if (((chip->id & ID_FAMILY_MASK) == ID_ADT75XX) &&
+		    (chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0)
+			return sprintf(buf, "2 - AIN1\n");
+
+		return sprintf(buf, "2 - External Temperature\n");
+	case ADT7516_AD_SINGLE_CH_AIN2:
+		if ((chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0)
+			return sprintf(buf, "3 - AIN2\n");
+
+		return sprintf(buf, "N/A\n");
+	case ADT7516_AD_SINGLE_CH_AIN3:
+		if (chip->config1 & ADT7516_SEL_AIN3)
+			return sprintf(buf, "4 - AIN3\n");
+
+		return sprintf(buf, "N/A\n");
+	case ADT7516_AD_SINGLE_CH_AIN4:
+		return sprintf(buf, "5 - AIN4\n");
+	default:
+		return sprintf(buf, "N/A\n");
+	}
+}
+
+static ssize_t adt7316_store_ad_channel(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf,
+					size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config2;
+	u8 data;
+	int ret;
+
+	if (!(chip->config2 & ADT7316_AD_SINGLE_CH_MODE))
+		return -EPERM;
+
+	ret = kstrtou8(buf, 10, &data);
+	if (ret)
+		return -EINVAL;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX) {
+		if (data > 5)
+			return -EINVAL;
+
+		config2 = chip->config2 & (~ADT7516_AD_SINGLE_CH_MASK);
+	} else {
+		if (data > 2)
+			return -EINVAL;
+
+		config2 = chip->config2 & (~ADT7316_AD_SINGLE_CH_MASK);
+	}
+
+	config2 |= data;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
+	if (ret)
+		return -EIO;
+
+	chip->config2 = config2;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(ad_channel, 0644,
+		adt7316_show_ad_channel,
+		adt7316_store_ad_channel,
+		0);
+
+static ssize_t adt7316_show_all_ad_channels(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (!(chip->config2 & ADT7316_AD_SINGLE_CH_MODE))
+		return -EPERM;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+		return sprintf(buf, "0 - VDD\n1 - Internal Temperature\n"
+				"2 - External Temperature or AIN1\n"
+				"3 - AIN2\n4 - AIN3\n5 - AIN4\n");
+	return sprintf(buf, "0 - VDD\n1 - Internal Temperature\n"
+			"2 - External Temperature\n");
+}
+
+static IIO_DEVICE_ATTR(all_ad_channels, 0444,
+		adt7316_show_all_ad_channels, NULL, 0);
+
+static ssize_t adt7316_show_disable_averaging(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->config2 & ADT7316_DISABLE_AVERAGING));
+}
+
+static ssize_t adt7316_store_disable_averaging(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config2;
+	int ret;
+
+	config2 = chip->config2 & (~ADT7316_DISABLE_AVERAGING);
+	if (buf[0] == '1')
+		config2 |= ADT7316_DISABLE_AVERAGING;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
+	if (ret)
+		return -EIO;
+
+	chip->config2 = config2;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(disable_averaging, 0644,
+		adt7316_show_disable_averaging,
+		adt7316_store_disable_averaging,
+		0);
+
+static ssize_t adt7316_show_enable_smbus_timeout(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->config2 & ADT7316_EN_SMBUS_TIMEOUT));
+}
+
+static ssize_t adt7316_store_enable_smbus_timeout(struct device *dev,
+						  struct device_attribute *attr,
+						  const char *buf,
+						  size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config2;
+	int ret;
+
+	config2 = chip->config2 & (~ADT7316_EN_SMBUS_TIMEOUT);
+	if (buf[0] == '1')
+		config2 |= ADT7316_EN_SMBUS_TIMEOUT;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
+	if (ret)
+		return -EIO;
+
+	chip->config2 = config2;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(enable_smbus_timeout, 0644,
+		adt7316_show_enable_smbus_timeout,
+		adt7316_store_enable_smbus_timeout,
+		0);
+
+static ssize_t adt7316_show_powerdown(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_PD));
+}
+
+static ssize_t adt7316_store_powerdown(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config1;
+	int ret;
+
+	config1 = chip->config1 & (~ADT7316_PD);
+	if (buf[0] == '1')
+		config1 |= ADT7316_PD;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
+	if (ret)
+		return -EIO;
+
+	chip->config1 = config1;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(powerdown, 0644,
+		adt7316_show_powerdown,
+		adt7316_store_powerdown,
+		0);
+
+static ssize_t adt7316_show_fast_ad_clock(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n", !!(chip->config3 & ADT7316_ADCLK_22_5));
+}
+
+static ssize_t adt7316_store_fast_ad_clock(struct device *dev,
+					   struct device_attribute *attr,
+					   const char *buf,
+					   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config3;
+	int ret;
+
+	config3 = chip->config3 & (~ADT7316_ADCLK_22_5);
+	if (buf[0] == '1')
+		config3 |= ADT7316_ADCLK_22_5;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
+	if (ret)
+		return -EIO;
+
+	chip->config3 = config3;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(fast_ad_clock, 0644,
+		adt7316_show_fast_ad_clock,
+		adt7316_store_fast_ad_clock,
+		0);
+
+static ssize_t adt7316_show_da_high_resolution(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (chip->config3 & ADT7316_DA_HIGH_RESOLUTION) {
+		if (chip->id != ID_ADT7318 && chip->id != ID_ADT7519)
+			return sprintf(buf, "1 (10 bits)\n");
+	}
+
+	return sprintf(buf, "0 (8 bits)\n");
+}
+
+static ssize_t adt7316_store_da_high_resolution(struct device *dev,
+						struct device_attribute *attr,
+						const char *buf,
+						size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config3;
+	int ret;
+
+	if (chip->id == ID_ADT7318 || chip->id == ID_ADT7519)
+		return -EPERM;
+
+	config3 = chip->config3 & (~ADT7316_DA_HIGH_RESOLUTION);
+	if (buf[0] == '1')
+		config3 |= ADT7316_DA_HIGH_RESOLUTION;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
+	if (ret)
+		return -EIO;
+
+	chip->config3 = config3;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(da_high_resolution, 0644,
+		adt7316_show_da_high_resolution,
+		adt7316_store_da_high_resolution,
+		0);
+
+static ssize_t adt7316_show_AIN_internal_Vref(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
+		return -EPERM;
+
+	return sprintf(buf, "%d\n",
+		!!(chip->config3 & ADT7516_AIN_IN_VREF));
+}
+
+static ssize_t adt7316_store_AIN_internal_Vref(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config3;
+	int ret;
+
+	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
+		return -EPERM;
+
+	if (buf[0] != '1')
+		config3 = chip->config3 & (~ADT7516_AIN_IN_VREF);
+	else
+		config3 = chip->config3 | ADT7516_AIN_IN_VREF;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
+	if (ret)
+		return -EIO;
+
+	chip->config3 = config3;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(AIN_internal_Vref, 0644,
+		adt7316_show_AIN_internal_Vref,
+		adt7316_store_AIN_internal_Vref,
+		0);
+
+static ssize_t adt7316_show_enable_prop_DACA(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->config3 & ADT7316_EN_IN_TEMP_PROP_DACA));
+}
+
+static ssize_t adt7316_store_enable_prop_DACA(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf,
+					      size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config3;
+	int ret;
+
+	config3 = chip->config3 & (~ADT7316_EN_IN_TEMP_PROP_DACA);
+	if (buf[0] == '1')
+		config3 |= ADT7316_EN_IN_TEMP_PROP_DACA;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
+	if (ret)
+		return -EIO;
+
+	chip->config3 = config3;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(enable_proportion_DACA, 0644,
+		       adt7316_show_enable_prop_DACA,
+		       adt7316_store_enable_prop_DACA,
+		       0);
+
+static ssize_t adt7316_show_enable_prop_DACB(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->config3 & ADT7316_EN_EX_TEMP_PROP_DACB));
+}
+
+static ssize_t adt7316_store_enable_prop_DACB(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf,
+					      size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config3;
+	int ret;
+
+	config3 = chip->config3 & (~ADT7316_EN_EX_TEMP_PROP_DACB);
+	if (buf[0] == '1')
+		config3 |= ADT7316_EN_EX_TEMP_PROP_DACB;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
+	if (ret)
+		return -EIO;
+
+	chip->config3 = config3;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(enable_proportion_DACB, 0644,
+		       adt7316_show_enable_prop_DACB,
+		       adt7316_store_enable_prop_DACB,
+		       0);
+
+static ssize_t adt7316_show_DAC_2Vref_ch_mask(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "0x%x\n",
+		chip->dac_config & ADT7316_DA_2VREF_CH_MASK);
+}
+
+static ssize_t adt7316_store_DAC_2Vref_ch_mask(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 dac_config;
+	u8 data;
+	int ret;
+
+	ret = kstrtou8(buf, 16, &data);
+	if (ret || data > ADT7316_DA_2VREF_CH_MASK)
+		return -EINVAL;
+
+	dac_config = chip->dac_config & (~ADT7316_DA_2VREF_CH_MASK);
+	dac_config |= data;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
+	if (ret)
+		return -EIO;
+
+	chip->dac_config = dac_config;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(DAC_2Vref_channels_mask, 0644,
+		       adt7316_show_DAC_2Vref_ch_mask,
+		       adt7316_store_DAC_2Vref_ch_mask,
+		       0);
+
+static ssize_t adt7316_show_DAC_update_mode(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (!(chip->config3 & ADT7316_DA_EN_VIA_DAC_LDAC))
+		return sprintf(buf, "manual\n");
+
+	switch (chip->dac_config & ADT7316_DA_EN_MODE_MASK) {
+	case ADT7316_DA_EN_MODE_SINGLE:
+		return sprintf(buf,
+			"0 - auto at any MSB DAC writing\n");
+	case ADT7316_DA_EN_MODE_AB_CD:
+		return sprintf(buf,
+			"1 - auto at MSB DAC AB and CD writing\n");
+	case ADT7316_DA_EN_MODE_ABCD:
+		return sprintf(buf,
+			"2 - auto at MSB DAC ABCD writing\n");
+	default: /* ADT7316_DA_EN_MODE_LDAC */
+		return sprintf(buf, "3 - manual\n");
+	}
+}
+
+static ssize_t adt7316_store_DAC_update_mode(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf,
+					     size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 dac_config;
+	u8 data;
+	int ret;
+
+	if (!(chip->config3 & ADT7316_DA_EN_VIA_DAC_LDAC))
+		return -EPERM;
+
+	ret = kstrtou8(buf, 10, &data);
+	if (ret || data > (ADT7316_DA_EN_MODE_MASK >> ADT7316_DA_EN_MODE_SHIFT))
+		return -EINVAL;
+
+	dac_config = chip->dac_config & (~ADT7316_DA_EN_MODE_MASK);
+	dac_config |= data << ADT7316_DA_EN_MODE_SHIFT;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
+	if (ret)
+		return -EIO;
+
+	chip->dac_config = dac_config;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(DAC_update_mode, 0644,
+		       adt7316_show_DAC_update_mode,
+		       adt7316_store_DAC_update_mode,
+		       0);
+
+static ssize_t adt7316_show_all_DAC_update_modes(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if (chip->config3 & ADT7316_DA_EN_VIA_DAC_LDAC)
+		return sprintf(buf, "0 - auto at any MSB DAC writing\n"
+				"1 - auto at MSB DAC AB and CD writing\n"
+				"2 - auto at MSB DAC ABCD writing\n"
+				"3 - manual\n");
+	return sprintf(buf, "manual\n");
+}
+
+static IIO_DEVICE_ATTR(all_DAC_update_modes, 0444,
+		       adt7316_show_all_DAC_update_modes, NULL, 0);
+
+static ssize_t adt7316_store_update_DAC(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf,
+					size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 ldac_config;
+	u8 data;
+	int ret;
+
+	if (chip->config3 & ADT7316_DA_EN_VIA_DAC_LDAC) {
+		if ((chip->dac_config & ADT7316_DA_EN_MODE_MASK) !=
+			ADT7316_DA_EN_MODE_LDAC)
+			return -EPERM;
+
+		ret = kstrtou8(buf, 16, &data);
+		if (ret || data > ADT7316_LDAC_EN_DA_MASK)
+			return -EINVAL;
+
+		ldac_config = chip->ldac_config & (~ADT7316_LDAC_EN_DA_MASK);
+		ldac_config |= data;
+
+		ret = chip->bus.write(chip->bus.client, ADT7316_LDAC_CONFIG,
+			ldac_config);
+		if (ret)
+			return -EIO;
+	} else {
+		gpiod_set_value(chip->ldac_pin, 0);
+		gpiod_set_value(chip->ldac_pin, 1);
+	}
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(update_DAC, 0644,
+		       NULL,
+		       adt7316_store_update_DAC,
+		       0);
+
+static ssize_t adt7316_show_DA_AB_Vref_bypass(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->dac_config & ADT7316_VREF_BYPASS_DAC_AB));
+}
+
+static ssize_t adt7316_store_DA_AB_Vref_bypass(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 dac_config;
+	int ret;
+
+	dac_config = chip->dac_config & (~ADT7316_VREF_BYPASS_DAC_AB);
+	if (buf[0] == '1')
+		dac_config |= ADT7316_VREF_BYPASS_DAC_AB;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
+	if (ret)
+		return -EIO;
+
+	chip->dac_config = dac_config;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(DA_AB_Vref_bypass, 0644,
+		       adt7316_show_DA_AB_Vref_bypass,
+		       adt7316_store_DA_AB_Vref_bypass,
+		       0);
+
+static ssize_t adt7316_show_DA_CD_Vref_bypass(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n",
+		!!(chip->dac_config & ADT7316_VREF_BYPASS_DAC_CD));
+}
+
+static ssize_t adt7316_store_DA_CD_Vref_bypass(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 dac_config;
+	int ret;
+
+	dac_config = chip->dac_config & (~ADT7316_VREF_BYPASS_DAC_CD);
+	if (buf[0] == '1')
+		dac_config |= ADT7316_VREF_BYPASS_DAC_CD;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
+	if (ret)
+		return -EIO;
+
+	chip->dac_config = dac_config;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(DA_CD_Vref_bypass, 0644,
+		       adt7316_show_DA_CD_Vref_bypass,
+		       adt7316_store_DA_CD_Vref_bypass,
+		       0);
+
+static ssize_t adt7316_show_DAC_internal_Vref(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+		return sprintf(buf, "0x%x\n",
+			(chip->ldac_config & ADT7516_DAC_IN_VREF_MASK) >>
+			ADT7516_DAC_IN_VREF_OFFSET);
+	return sprintf(buf, "%d\n",
+		       !!(chip->ldac_config & ADT7316_DAC_IN_VREF));
+}
+
+static ssize_t adt7316_store_DAC_internal_Vref(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf,
+					       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 ldac_config;
+	u8 data;
+	int ret;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX) {
+		ret = kstrtou8(buf, 16, &data);
+		if (ret || data > 3)
+			return -EINVAL;
+
+		ldac_config = chip->ldac_config & (~ADT7516_DAC_IN_VREF_MASK);
+		if (data & 0x1)
+			ldac_config |= ADT7516_DAC_AB_IN_VREF;
+		if (data & 0x2)
+			ldac_config |= ADT7516_DAC_CD_IN_VREF;
+	} else {
+		ret = kstrtou8(buf, 16, &data);
+		if (ret)
+			return -EINVAL;
+
+		ldac_config = chip->ldac_config & (~ADT7316_DAC_IN_VREF);
+		if (data)
+			ldac_config = chip->ldac_config | ADT7316_DAC_IN_VREF;
+	}
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_LDAC_CONFIG,
+			ldac_config);
+	if (ret)
+		return -EIO;
+
+	chip->ldac_config = ldac_config;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(DAC_internal_Vref, 0644,
+		       adt7316_show_DAC_internal_Vref,
+		       adt7316_store_DAC_internal_Vref,
+		       0);
+
+static ssize_t adt7316_show_ad(struct adt7316_chip_info *chip,
+			       int channel, char *buf)
+{
+	u16 data;
+	u8 msb, lsb;
+	char sign = ' ';
+	int ret;
+
+	if ((chip->config2 & ADT7316_AD_SINGLE_CH_MODE) &&
+	    channel != (chip->config2 & ADT7516_AD_SINGLE_CH_MASK))
+		return -EPERM;
+
+	switch (channel) {
+	case ADT7316_AD_SINGLE_CH_IN:
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_LSB_IN_TEMP_VDD, &lsb);
+		if (ret)
+			return -EIO;
+
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_AD_MSB_DATA_BASE + channel, &msb);
+		if (ret)
+			return -EIO;
+
+		data = msb << ADT7316_T_VALUE_FLOAT_OFFSET;
+		data |= lsb & ADT7316_LSB_IN_TEMP_MASK;
+		break;
+	case ADT7316_AD_SINGLE_CH_VDD:
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_LSB_IN_TEMP_VDD, &lsb);
+		if (ret)
+			return -EIO;
+
+		ret = chip->bus.read(chip->bus.client,
+
+			ADT7316_AD_MSB_DATA_BASE + channel, &msb);
+		if (ret)
+			return -EIO;
+
+		data = msb << ADT7316_T_VALUE_FLOAT_OFFSET;
+		data |= (lsb & ADT7316_LSB_VDD_MASK) >> ADT7316_LSB_VDD_OFFSET;
+		return sprintf(buf, "%d\n", data);
+	default: /* ex_temp and ain */
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_LSB_EX_TEMP_AIN, &lsb);
+		if (ret)
+			return -EIO;
+
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_AD_MSB_DATA_BASE + channel, &msb);
+		if (ret)
+			return -EIO;
+
+		data = msb << ADT7316_T_VALUE_FLOAT_OFFSET;
+		data |= lsb & (ADT7316_LSB_EX_TEMP_MASK <<
+			(ADT7516_LSB_AIN_SHIFT * (channel -
+			(ADT7316_MSB_EX_TEMP - ADT7316_AD_MSB_DATA_BASE))));
+
+		if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+			return sprintf(buf, "%d\n", data);
+
+		break;
+	}
+
+	if (data & ADT7316_T_VALUE_SIGN) {
+		/* convert supplement to positive value */
+		data = (ADT7316_T_VALUE_SIGN << 1) - data;
+		sign = '-';
+	}
+
+	return sprintf(buf, "%c%d.%.2d\n", sign,
+		(data >> ADT7316_T_VALUE_FLOAT_OFFSET),
+		(data & ADT7316_T_VALUE_FLOAT_MASK) * 25);
+}
+
+static ssize_t adt7316_show_VDD(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_VDD, buf);
+}
+static IIO_DEVICE_ATTR(VDD, 0444, adt7316_show_VDD, NULL, 0);
+
+static ssize_t adt7316_show_in_temp(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_IN, buf);
+}
+
+static IIO_DEVICE_ATTR(in_temp, 0444, adt7316_show_in_temp, NULL, 0);
+
+static ssize_t adt7316_show_ex_temp_AIN1(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_EX, buf);
+}
+
+static IIO_DEVICE_ATTR(ex_temp_AIN1, 0444, adt7316_show_ex_temp_AIN1,
+		       NULL, 0);
+static IIO_DEVICE_ATTR(ex_temp, 0444, adt7316_show_ex_temp_AIN1, NULL, 0);
+
+static ssize_t adt7316_show_AIN2(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN2, buf);
+}
+static IIO_DEVICE_ATTR(AIN2, 0444, adt7316_show_AIN2, NULL, 0);
+
+static ssize_t adt7316_show_AIN3(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN3, buf);
+}
+static IIO_DEVICE_ATTR(AIN3, 0444, adt7316_show_AIN3, NULL, 0);
+
+static ssize_t adt7316_show_AIN4(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN4, buf);
+}
+static IIO_DEVICE_ATTR(AIN4, 0444, adt7316_show_AIN4, NULL, 0);
+
+static ssize_t adt7316_show_temp_offset(struct adt7316_chip_info *chip,
+					int offset_addr, char *buf)
+{
+	int data;
+	u8 val;
+	int ret;
+
+	ret = chip->bus.read(chip->bus.client, offset_addr, &val);
+	if (ret)
+		return -EIO;
+
+	data = (int)val;
+	if (val & 0x80)
+		data -= 256;
+
+	return sprintf(buf, "%d\n", data);
+}
+
+static ssize_t adt7316_store_temp_offset(struct adt7316_chip_info *chip,
+					 int offset_addr,
+					 const char *buf,
+					 size_t len)
+{
+	int data;
+	u8 val;
+	int ret;
+
+	ret = kstrtoint(buf, 10, &data);
+	if (ret || data > 127 || data < -128)
+		return -EINVAL;
+
+	if (data < 0)
+		data += 256;
+
+	val = (u8)data;
+
+	ret = chip->bus.write(chip->bus.client, offset_addr, val);
+	if (ret)
+		return -EIO;
+
+	return len;
+}
+
+static ssize_t adt7316_show_in_temp_offset(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_temp_offset(chip, ADT7316_IN_TEMP_OFFSET, buf);
+}
+
+static ssize_t adt7316_store_in_temp_offset(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf,
+					    size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_temp_offset(chip, ADT7316_IN_TEMP_OFFSET, buf,
+			len);
+}
+
+static IIO_DEVICE_ATTR(in_temp_offset, 0644,
+		       adt7316_show_in_temp_offset,
+		       adt7316_store_in_temp_offset, 0);
+
+static ssize_t adt7316_show_ex_temp_offset(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_temp_offset(chip, ADT7316_EX_TEMP_OFFSET, buf);
+}
+
+static ssize_t adt7316_store_ex_temp_offset(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf,
+					    size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_temp_offset(chip, ADT7316_EX_TEMP_OFFSET, buf,
+			len);
+}
+
+static IIO_DEVICE_ATTR(ex_temp_offset, 0644,
+		       adt7316_show_ex_temp_offset,
+		       adt7316_store_ex_temp_offset, 0);
+
+static ssize_t adt7316_show_in_analog_temp_offset(struct device *dev,
+						  struct device_attribute *attr,
+						  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_temp_offset(chip,
+			ADT7316_IN_ANALOG_TEMP_OFFSET, buf);
+}
+
+static ssize_t adt7316_store_in_analog_temp_offset(struct device *dev,
+						   struct device_attribute *attr,
+						   const char *buf,
+						   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_temp_offset(chip,
+			ADT7316_IN_ANALOG_TEMP_OFFSET, buf, len);
+}
+
+static IIO_DEVICE_ATTR(in_analog_temp_offset, 0644,
+		       adt7316_show_in_analog_temp_offset,
+		       adt7316_store_in_analog_temp_offset, 0);
+
+static ssize_t adt7316_show_ex_analog_temp_offset(struct device *dev,
+						  struct device_attribute *attr,
+						  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_temp_offset(chip,
+			ADT7316_EX_ANALOG_TEMP_OFFSET, buf);
+}
+
+static ssize_t adt7316_store_ex_analog_temp_offset(struct device *dev,
+						   struct device_attribute *attr,
+						   const char *buf,
+						   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_temp_offset(chip,
+			ADT7316_EX_ANALOG_TEMP_OFFSET, buf, len);
+}
+
+static IIO_DEVICE_ATTR(ex_analog_temp_offset, 0644,
+		       adt7316_show_ex_analog_temp_offset,
+		       adt7316_store_ex_analog_temp_offset, 0);
+
+static ssize_t adt7316_show_DAC(struct adt7316_chip_info *chip,
+				int channel, char *buf)
+{
+	u16 data = 0;
+	u8 msb, lsb, offset;
+	int ret;
+
+	if (channel >= ADT7316_DA_MSB_DATA_REGS ||
+	    (channel == 0 &&
+	    (chip->config3 & ADT7316_EN_IN_TEMP_PROP_DACA)) ||
+	    (channel == 1 &&
+	    (chip->config3 & ADT7316_EN_EX_TEMP_PROP_DACB)))
+		return -EPERM;
+
+	offset = chip->dac_bits - 8;
+
+	if (chip->dac_bits > 8) {
+		ret = chip->bus.read(chip->bus.client,
+			ADT7316_DA_DATA_BASE + channel * 2, &lsb);
+		if (ret)
+			return -EIO;
+	}
+
+	ret = chip->bus.read(chip->bus.client,
+		ADT7316_DA_DATA_BASE + 1 + channel * 2, &msb);
+	if (ret)
+		return -EIO;
+
+	if (chip->dac_bits == 12)
+		data = lsb >> ADT7316_DA_12_BIT_LSB_SHIFT;
+	else if (chip->dac_bits == 10)
+		data = lsb >> ADT7316_DA_10_BIT_LSB_SHIFT;
+	data |= msb << offset;
+
+	return sprintf(buf, "%d\n", data);
+}
+
+static ssize_t adt7316_store_DAC(struct adt7316_chip_info *chip,
+				 int channel, const char *buf, size_t len)
+{
+	u8 msb, lsb, lsb_reg, offset;
+	u16 data;
+	int ret;
+
+	if (channel >= ADT7316_DA_MSB_DATA_REGS ||
+	    (channel == 0 &&
+	    (chip->config3 & ADT7316_EN_IN_TEMP_PROP_DACA)) ||
+	    (channel == 1 &&
+	    (chip->config3 & ADT7316_EN_EX_TEMP_PROP_DACB)))
+		return -EPERM;
+
+	offset = chip->dac_bits - 8;
+
+	ret = kstrtou16(buf, 10, &data);
+	if (ret || data >= (1 << chip->dac_bits))
+		return -EINVAL;
+
+	if (chip->dac_bits > 8) {
+		lsb = data & ((1 << offset) - 1);
+		if (chip->dac_bits == 12)
+			lsb_reg = lsb << ADT7316_DA_12_BIT_LSB_SHIFT;
+		else
+			lsb_reg = lsb << ADT7316_DA_10_BIT_LSB_SHIFT;
+		ret = chip->bus.write(chip->bus.client,
+			ADT7316_DA_DATA_BASE + channel * 2, lsb_reg);
+		if (ret)
+			return -EIO;
+	}
+
+	msb = data >> offset;
+	ret = chip->bus.write(chip->bus.client,
+		ADT7316_DA_DATA_BASE + 1 + channel * 2, msb);
+	if (ret)
+		return -EIO;
+
+	return len;
+}
+
+static ssize_t adt7316_show_DAC_A(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_DAC(chip, 0, buf);
+}
+
+static ssize_t adt7316_store_DAC_A(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_DAC(chip, 0, buf, len);
+}
+
+static IIO_DEVICE_ATTR(DAC_A, 0644, adt7316_show_DAC_A,
+		       adt7316_store_DAC_A, 0);
+
+static ssize_t adt7316_show_DAC_B(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_DAC(chip, 1, buf);
+}
+
+static ssize_t adt7316_store_DAC_B(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_DAC(chip, 1, buf, len);
+}
+
+static IIO_DEVICE_ATTR(DAC_B, 0644, adt7316_show_DAC_B,
+		       adt7316_store_DAC_B, 0);
+
+static ssize_t adt7316_show_DAC_C(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_DAC(chip, 2, buf);
+}
+
+static ssize_t adt7316_store_DAC_C(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_DAC(chip, 2, buf, len);
+}
+
+static IIO_DEVICE_ATTR(DAC_C, 0644, adt7316_show_DAC_C,
+		       adt7316_store_DAC_C, 0);
+
+static ssize_t adt7316_show_DAC_D(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_show_DAC(chip, 3, buf);
+}
+
+static ssize_t adt7316_store_DAC_D(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return adt7316_store_DAC(chip, 3, buf, len);
+}
+
+static IIO_DEVICE_ATTR(DAC_D, 0644, adt7316_show_DAC_D,
+		       adt7316_store_DAC_D, 0);
+
+static ssize_t adt7316_show_device_id(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 id;
+	int ret;
+
+	ret = chip->bus.read(chip->bus.client, ADT7316_DEVICE_ID, &id);
+	if (ret)
+		return -EIO;
+
+	return sprintf(buf, "%d\n", id);
+}
+
+static IIO_DEVICE_ATTR(device_id, 0444, adt7316_show_device_id, NULL, 0);
+
+static ssize_t adt7316_show_manufactorer_id(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 id;
+	int ret;
+
+	ret = chip->bus.read(chip->bus.client, ADT7316_MANUFACTURE_ID, &id);
+	if (ret)
+		return -EIO;
+
+	return sprintf(buf, "%d\n", id);
+}
+
+static IIO_DEVICE_ATTR(manufactorer_id, 0444,
+		       adt7316_show_manufactorer_id, NULL, 0);
+
+static ssize_t adt7316_show_device_rev(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 rev;
+	int ret;
+
+	ret = chip->bus.read(chip->bus.client, ADT7316_DEVICE_REV, &rev);
+	if (ret)
+		return -EIO;
+
+	return sprintf(buf, "%d\n", rev);
+}
+
+static IIO_DEVICE_ATTR(device_rev, 0444, adt7316_show_device_rev, NULL, 0);
+
+static ssize_t adt7316_show_bus_type(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 stat;
+	int ret;
+
+	ret = chip->bus.read(chip->bus.client, ADT7316_SPI_LOCK_STAT, &stat);
+	if (ret)
+		return -EIO;
+
+	if (stat)
+		return sprintf(buf, "spi\n");
+
+	return sprintf(buf, "i2c\n");
+}
+
+static IIO_DEVICE_ATTR(bus_type, 0444, adt7316_show_bus_type, NULL, 0);
+
+static struct attribute *adt7316_attributes[] = {
+	&iio_dev_attr_all_modes.dev_attr.attr,
+	&iio_dev_attr_mode.dev_attr.attr,
+	&iio_dev_attr_enabled.dev_attr.attr,
+	&iio_dev_attr_ad_channel.dev_attr.attr,
+	&iio_dev_attr_all_ad_channels.dev_attr.attr,
+	&iio_dev_attr_disable_averaging.dev_attr.attr,
+	&iio_dev_attr_enable_smbus_timeout.dev_attr.attr,
+	&iio_dev_attr_powerdown.dev_attr.attr,
+	&iio_dev_attr_fast_ad_clock.dev_attr.attr,
+	&iio_dev_attr_da_high_resolution.dev_attr.attr,
+	&iio_dev_attr_enable_proportion_DACA.dev_attr.attr,
+	&iio_dev_attr_enable_proportion_DACB.dev_attr.attr,
+	&iio_dev_attr_DAC_2Vref_channels_mask.dev_attr.attr,
+	&iio_dev_attr_DAC_update_mode.dev_attr.attr,
+	&iio_dev_attr_all_DAC_update_modes.dev_attr.attr,
+	&iio_dev_attr_update_DAC.dev_attr.attr,
+	&iio_dev_attr_DA_AB_Vref_bypass.dev_attr.attr,
+	&iio_dev_attr_DA_CD_Vref_bypass.dev_attr.attr,
+	&iio_dev_attr_DAC_internal_Vref.dev_attr.attr,
+	&iio_dev_attr_VDD.dev_attr.attr,
+	&iio_dev_attr_in_temp.dev_attr.attr,
+	&iio_dev_attr_ex_temp.dev_attr.attr,
+	&iio_dev_attr_in_temp_offset.dev_attr.attr,
+	&iio_dev_attr_ex_temp_offset.dev_attr.attr,
+	&iio_dev_attr_in_analog_temp_offset.dev_attr.attr,
+	&iio_dev_attr_ex_analog_temp_offset.dev_attr.attr,
+	&iio_dev_attr_DAC_A.dev_attr.attr,
+	&iio_dev_attr_DAC_B.dev_attr.attr,
+	&iio_dev_attr_DAC_C.dev_attr.attr,
+	&iio_dev_attr_DAC_D.dev_attr.attr,
+	&iio_dev_attr_device_id.dev_attr.attr,
+	&iio_dev_attr_manufactorer_id.dev_attr.attr,
+	&iio_dev_attr_device_rev.dev_attr.attr,
+	&iio_dev_attr_bus_type.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group adt7316_attribute_group = {
+	.attrs = adt7316_attributes,
+};
+
+static struct attribute *adt7516_attributes[] = {
+	&iio_dev_attr_all_modes.dev_attr.attr,
+	&iio_dev_attr_mode.dev_attr.attr,
+	&iio_dev_attr_select_ex_temp.dev_attr.attr,
+	&iio_dev_attr_enabled.dev_attr.attr,
+	&iio_dev_attr_ad_channel.dev_attr.attr,
+	&iio_dev_attr_all_ad_channels.dev_attr.attr,
+	&iio_dev_attr_disable_averaging.dev_attr.attr,
+	&iio_dev_attr_enable_smbus_timeout.dev_attr.attr,
+	&iio_dev_attr_powerdown.dev_attr.attr,
+	&iio_dev_attr_fast_ad_clock.dev_attr.attr,
+	&iio_dev_attr_AIN_internal_Vref.dev_attr.attr,
+	&iio_dev_attr_da_high_resolution.dev_attr.attr,
+	&iio_dev_attr_enable_proportion_DACA.dev_attr.attr,
+	&iio_dev_attr_enable_proportion_DACB.dev_attr.attr,
+	&iio_dev_attr_DAC_2Vref_channels_mask.dev_attr.attr,
+	&iio_dev_attr_DAC_update_mode.dev_attr.attr,
+	&iio_dev_attr_all_DAC_update_modes.dev_attr.attr,
+	&iio_dev_attr_update_DAC.dev_attr.attr,
+	&iio_dev_attr_DAC_internal_Vref.dev_attr.attr,
+	&iio_dev_attr_VDD.dev_attr.attr,
+	&iio_dev_attr_in_temp.dev_attr.attr,
+	&iio_dev_attr_ex_temp_AIN1.dev_attr.attr,
+	&iio_dev_attr_AIN2.dev_attr.attr,
+	&iio_dev_attr_AIN3.dev_attr.attr,
+	&iio_dev_attr_AIN4.dev_attr.attr,
+	&iio_dev_attr_in_temp_offset.dev_attr.attr,
+	&iio_dev_attr_ex_temp_offset.dev_attr.attr,
+	&iio_dev_attr_in_analog_temp_offset.dev_attr.attr,
+	&iio_dev_attr_ex_analog_temp_offset.dev_attr.attr,
+	&iio_dev_attr_DAC_A.dev_attr.attr,
+	&iio_dev_attr_DAC_B.dev_attr.attr,
+	&iio_dev_attr_DAC_C.dev_attr.attr,
+	&iio_dev_attr_DAC_D.dev_attr.attr,
+	&iio_dev_attr_device_id.dev_attr.attr,
+	&iio_dev_attr_manufactorer_id.dev_attr.attr,
+	&iio_dev_attr_device_rev.dev_attr.attr,
+	&iio_dev_attr_bus_type.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group adt7516_attribute_group = {
+	.attrs = adt7516_attributes,
+};
+
+static irqreturn_t adt7316_event_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct adt7316_chip_info *chip = iio_priv(indio_dev);
+	u8 stat1, stat2;
+	int ret;
+	s64 time;
+
+	ret = chip->bus.read(chip->bus.client, ADT7316_INT_STAT1, &stat1);
+	if (!ret) {
+		if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
+			stat1 &= 0x1F;
+
+		time = iio_get_time_ns(indio_dev);
+		if (stat1 & BIT(0))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 0,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_RISING),
+				       time);
+		if (stat1 & BIT(1))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 0,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_FALLING),
+				       time);
+		if (stat1 & BIT(2))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 1,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_RISING),
+				       time);
+		if (stat1 & BIT(3))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 1,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_FALLING),
+				       time);
+		if (stat1 & BIT(5))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_VOLTAGE, 1,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_EITHER),
+				       time);
+		if (stat1 & BIT(6))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_VOLTAGE, 2,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_EITHER),
+				       time);
+		if (stat1 & BIT(7))
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_VOLTAGE, 3,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_EITHER),
+				       time);
+		}
+	ret = chip->bus.read(chip->bus.client, ADT7316_INT_STAT2, &stat2);
+	if (!ret) {
+		if (stat2 & ADT7316_INT_MASK2_VDD)
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(IIO_VOLTAGE,
+							    0,
+							    IIO_EV_TYPE_THRESH,
+							    IIO_EV_DIR_RISING),
+				       iio_get_time_ns(indio_dev));
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int adt7316_setup_irq(struct iio_dev *indio_dev)
+{
+	struct adt7316_chip_info *chip = iio_priv(indio_dev);
+	int irq_type, ret;
+
+	irq_type = irqd_get_trigger_type(irq_get_irq_data(chip->bus.irq));
+
+	switch (irq_type) {
+	case IRQF_TRIGGER_HIGH:
+	case IRQF_TRIGGER_RISING:
+		break;
+	case IRQF_TRIGGER_LOW:
+	case IRQF_TRIGGER_FALLING:
+		break;
+	default:
+		dev_info(&indio_dev->dev, "mode %d unsupported, using IRQF_TRIGGER_LOW\n",
+			 irq_type);
+		irq_type = IRQF_TRIGGER_LOW;
+		break;
+	}
+
+	ret = devm_request_threaded_irq(&indio_dev->dev, chip->bus.irq,
+					NULL, adt7316_event_handler,
+					irq_type | IRQF_ONESHOT,
+					indio_dev->name, indio_dev);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to request irq %d\n",
+			chip->bus.irq);
+		return ret;
+	}
+
+	if (irq_type & IRQF_TRIGGER_HIGH)
+		chip->config1 |= ADT7316_INT_POLARITY;
+
+	return 0;
+}
+
+/*
+ * Show mask of enabled interrupts in Hex.
+ */
+static ssize_t adt7316_show_int_mask(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "0x%x\n", chip->int_mask);
+}
+
+/*
+ * Set 1 to the mask in Hex to enabled interrupts.
+ */
+static ssize_t adt7316_set_int_mask(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf,
+				    size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u16 data;
+	int ret;
+	u8 mask;
+
+	ret = kstrtou16(buf, 16, &data);
+	if (ret || data >= ADT7316_VDD_INT_MASK + 1)
+		return -EINVAL;
+
+	if (data & ADT7316_VDD_INT_MASK)
+		mask = 0;			/* enable vdd int */
+	else
+		mask = ADT7316_INT_MASK2_VDD;	/* disable vdd int */
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_INT_MASK2, mask);
+	if (!ret) {
+		chip->int_mask &= ~ADT7316_VDD_INT_MASK;
+		chip->int_mask |= data & ADT7316_VDD_INT_MASK;
+	}
+
+	if (data & ADT7316_TEMP_AIN_INT_MASK) {
+		if ((chip->id & ID_FAMILY_MASK) == ID_ADT73XX)
+			/* mask in reg is opposite, set 1 to disable */
+			mask = (~data) & ADT7316_TEMP_INT_MASK;
+		else
+			/* mask in reg is opposite, set 1 to disable */
+			mask = (~data) & ADT7316_TEMP_AIN_INT_MASK;
+	}
+	ret = chip->bus.write(chip->bus.client, ADT7316_INT_MASK1, mask);
+
+	chip->int_mask = mask;
+
+	return len;
+}
+
+static inline ssize_t adt7316_show_ad_bound(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 val;
+	int data;
+	int ret;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT73XX &&
+	    this_attr->address > ADT7316_EX_TEMP_LOW)
+		return -EPERM;
+
+	ret = chip->bus.read(chip->bus.client, this_attr->address, &val);
+	if (ret)
+		return -EIO;
+
+	data = (int)val;
+
+	if (!((chip->id & ID_FAMILY_MASK) == ID_ADT75XX &&
+	      (chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0)) {
+		if (data & 0x80)
+			data -= 256;
+	}
+
+	return sprintf(buf, "%d\n", data);
+}
+
+static inline ssize_t adt7316_set_ad_bound(struct device *dev,
+					   struct device_attribute *attr,
+					   const char *buf,
+					   size_t len)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	int data;
+	u8 val;
+	int ret;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT73XX &&
+	    this_attr->address > ADT7316_EX_TEMP_LOW)
+		return -EPERM;
+
+	ret = kstrtoint(buf, 10, &data);
+	if (ret)
+		return -EINVAL;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX &&
+	    (chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0) {
+		if (data > 255 || data < 0)
+			return -EINVAL;
+	} else {
+		if (data > 127 || data < -128)
+			return -EINVAL;
+
+		if (data < 0)
+			data += 256;
+	}
+
+	val = (u8)data;
+
+	ret = chip->bus.write(chip->bus.client, this_attr->address, val);
+	if (ret)
+		return -EIO;
+
+	return len;
+}
+
+static ssize_t adt7316_show_int_enabled(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_INT_EN));
+}
+
+static ssize_t adt7316_set_int_enabled(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t len)
+{
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+	u8 config1;
+	int ret;
+
+	config1 = chip->config1 & (~ADT7316_INT_EN);
+	if (buf[0] == '1')
+		config1 |= ADT7316_INT_EN;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
+	if (ret)
+		return -EIO;
+
+	chip->config1 = config1;
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(int_mask,
+		       0644,
+		       adt7316_show_int_mask, adt7316_set_int_mask,
+		       0);
+static IIO_DEVICE_ATTR(in_temp_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_IN_TEMP_HIGH);
+static IIO_DEVICE_ATTR(in_temp_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_IN_TEMP_LOW);
+static IIO_DEVICE_ATTR(ex_temp_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_EX_TEMP_HIGH);
+static IIO_DEVICE_ATTR(ex_temp_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_EX_TEMP_LOW);
+
+/* NASTY duplication to be fixed */
+static IIO_DEVICE_ATTR(ex_temp_ain1_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_EX_TEMP_HIGH);
+static IIO_DEVICE_ATTR(ex_temp_ain1_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7316_EX_TEMP_LOW);
+static IIO_DEVICE_ATTR(ain2_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN2_HIGH);
+static IIO_DEVICE_ATTR(ain2_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN2_LOW);
+static IIO_DEVICE_ATTR(ain3_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN3_HIGH);
+static IIO_DEVICE_ATTR(ain3_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN3_LOW);
+static IIO_DEVICE_ATTR(ain4_high_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN4_HIGH);
+static IIO_DEVICE_ATTR(ain4_low_value,
+		       0644,
+		       adt7316_show_ad_bound, adt7316_set_ad_bound,
+		       ADT7516_AIN4_LOW);
+static IIO_DEVICE_ATTR(int_enabled,
+		       0644,
+		       adt7316_show_int_enabled,
+		       adt7316_set_int_enabled, 0);
+
+static struct attribute *adt7316_event_attributes[] = {
+	&iio_dev_attr_int_mask.dev_attr.attr,
+	&iio_dev_attr_in_temp_high_value.dev_attr.attr,
+	&iio_dev_attr_in_temp_low_value.dev_attr.attr,
+	&iio_dev_attr_ex_temp_high_value.dev_attr.attr,
+	&iio_dev_attr_ex_temp_low_value.dev_attr.attr,
+	&iio_dev_attr_int_enabled.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group adt7316_event_attribute_group = {
+	.attrs = adt7316_event_attributes,
+	.name = "events",
+};
+
+static struct attribute *adt7516_event_attributes[] = {
+	&iio_dev_attr_int_mask.dev_attr.attr,
+	&iio_dev_attr_in_temp_high_value.dev_attr.attr,
+	&iio_dev_attr_in_temp_low_value.dev_attr.attr,
+	&iio_dev_attr_ex_temp_ain1_high_value.dev_attr.attr,
+	&iio_dev_attr_ex_temp_ain1_low_value.dev_attr.attr,
+	&iio_dev_attr_ain2_high_value.dev_attr.attr,
+	&iio_dev_attr_ain2_low_value.dev_attr.attr,
+	&iio_dev_attr_ain3_high_value.dev_attr.attr,
+	&iio_dev_attr_ain3_low_value.dev_attr.attr,
+	&iio_dev_attr_ain4_high_value.dev_attr.attr,
+	&iio_dev_attr_ain4_low_value.dev_attr.attr,
+	&iio_dev_attr_int_enabled.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group adt7516_event_attribute_group = {
+	.attrs = adt7516_event_attributes,
+	.name = "events",
+};
+
+#ifdef CONFIG_PM_SLEEP
+static int adt7316_disable(struct device *dev)
+{
+	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return _adt7316_store_enabled(chip, 0);
+}
+
+static int adt7316_enable(struct device *dev)
+{
+	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct adt7316_chip_info *chip = iio_priv(dev_info);
+
+	return _adt7316_store_enabled(chip, 1);
+}
+EXPORT_SYMBOL_GPL(adt7316_pm_ops);
+SIMPLE_DEV_PM_OPS(adt7316_pm_ops, adt7316_disable, adt7316_enable);
+#endif
+
+static const struct iio_info adt7316_info = {
+	.attrs = &adt7316_attribute_group,
+	.event_attrs = &adt7316_event_attribute_group,
+};
+
+static const struct iio_info adt7516_info = {
+	.attrs = &adt7516_attribute_group,
+	.event_attrs = &adt7516_event_attribute_group,
+};
+
+/*
+ * device probe and remove
+ */
+int adt7316_probe(struct device *dev, struct adt7316_bus *bus,
+		  const char *name)
+{
+	struct adt7316_chip_info *chip;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*chip));
+	if (!indio_dev)
+		return -ENOMEM;
+	chip = iio_priv(indio_dev);
+	/* this is only used for device removal purposes */
+	dev_set_drvdata(dev, indio_dev);
+
+	chip->bus = *bus;
+
+	if (name[4] == '3')
+		chip->id = ID_ADT7316 + (name[6] - '6');
+	else if (name[4] == '5')
+		chip->id = ID_ADT7516 + (name[6] - '6');
+	else
+		return -ENODEV;
+
+	if (chip->id == ID_ADT7316 || chip->id == ID_ADT7516)
+		chip->dac_bits = 12;
+	else if (chip->id == ID_ADT7317 || chip->id == ID_ADT7517)
+		chip->dac_bits = 10;
+	else
+		chip->dac_bits = 8;
+
+	chip->ldac_pin = devm_gpiod_get_optional(dev, "adi,ldac",
+						 GPIOD_OUT_LOW);
+	if (IS_ERR(chip->ldac_pin)) {
+		ret = PTR_ERR(chip->ldac_pin);
+		dev_err(dev, "Failed to request ldac GPIO: %d\n", ret);
+		return ret;
+	}
+
+	if (!chip->ldac_pin) {
+		chip->config3 |= ADT7316_DA_EN_VIA_DAC_LDAC;
+		if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+			chip->config1 |= ADT7516_SEL_AIN3;
+	}
+	chip->int_mask = ADT7316_TEMP_INT_MASK | ADT7316_VDD_INT_MASK;
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+		chip->int_mask |= ADT7516_AIN_INT_MASK;
+
+	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
+		indio_dev->info = &adt7516_info;
+	else
+		indio_dev->info = &adt7316_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	if (chip->bus.irq > 0) {
+		ret = adt7316_setup_irq(indio_dev);
+		if (ret)
+			return ret;
+	}
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, chip->config1);
+	if (ret)
+		return -EIO;
+
+	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, chip->config3);
+	if (ret)
+		return -EIO;
+
+	ret = devm_iio_device_register(dev, indio_dev);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "%s temperature sensor, ADC and DAC registered.\n",
+		 indio_dev->name);
+
+	return 0;
+}
+EXPORT_SYMBOL(adt7316_probe);
+
+MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
+MODULE_DESCRIPTION("Analog Devices ADT7316/7/8 and ADT7516/7/9 digital temperature sensor, ADC and DAC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/addac/adt7316.h b/drivers/staging/iio/addac/adt7316.h
new file mode 100644
index 000000000..8c2a92ae7
--- /dev/null
+++ b/drivers/staging/iio/addac/adt7316.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * ADT7316 digital temperature sensor driver supporting ADT7316/7/8 ADT7516/7/9
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#ifndef _ADT7316_H_
+#define _ADT7316_H_
+
+#include <linux/types.h>
+#include <linux/pm.h>
+
+#define ADT7316_REG_MAX_ADDR		0x3F
+
+struct adt7316_bus {
+	void *client;
+	int irq;
+	int (*read)(void *client, u8 reg, u8 *data);
+	int (*write)(void *client, u8 reg, u8 val);
+	int (*multi_read)(void *client, u8 first_reg, u8 count, u8 *data);
+	int (*multi_write)(void *client, u8 first_reg, u8 count, u8 *data);
+};
+
+#ifdef CONFIG_PM_SLEEP
+extern const struct dev_pm_ops adt7316_pm_ops;
+#define ADT7316_PM_OPS (&adt7316_pm_ops)
+#else
+#define ADT7316_PM_OPS NULL
+#endif
+int adt7316_probe(struct device *dev, struct adt7316_bus *bus,
+		  const char *name);
+
+#endif
diff --git a/drivers/staging/iio/frequency/Kconfig b/drivers/staging/iio/frequency/Kconfig
new file mode 100644
index 000000000..72d899cbe
--- /dev/null
+++ b/drivers/staging/iio/frequency/Kconfig
@@ -0,0 +1,27 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Direct Digital Synthesis drivers
+#
+menu "Direct Digital Synthesis"
+
+config AD9832
+	tristate "Analog Devices ad9832/5 driver"
+	depends on SPI
+	help
+	  Say yes here to build support for Analog Devices DDS chip
+	  AD9832 and AD9835, provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad9832.
+
+config AD9834
+	tristate "Analog Devices AD9833/4/7/8 driver"
+	depends on SPI
+	help
+	  Say yes here to build support for Analog Devices DDS chip
+	  AD9833, AD9834, AD9837 and AD9838, provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad9834.
+
+endmenu
diff --git a/drivers/staging/iio/frequency/Makefile b/drivers/staging/iio/frequency/Makefile
new file mode 100644
index 000000000..b8c5cf98a
--- /dev/null
+++ b/drivers/staging/iio/frequency/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for Direct Digital Synthesis drivers
+#
+
+obj-$(CONFIG_AD9832) += ad9832.o
+obj-$(CONFIG_AD9834) += ad9834.o
diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c
new file mode 100644
index 000000000..6f9eebd6c
--- /dev/null
+++ b/drivers/staging/iio/frequency/ad9832.c
@@ -0,0 +1,462 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * AD9832 SPI DDS driver
+ *
+ * Copyright 2011 Analog Devices Inc.
+ */
+
+#include <asm/div64.h>
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/sysfs.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#include "ad9832.h"
+
+#include "dds.h"
+
+/* Registers */
+
+#define AD9832_FREQ0LL		0x0
+#define AD9832_FREQ0HL		0x1
+#define AD9832_FREQ0LM		0x2
+#define AD9832_FREQ0HM		0x3
+#define AD9832_FREQ1LL		0x4
+#define AD9832_FREQ1HL		0x5
+#define AD9832_FREQ1LM		0x6
+#define AD9832_FREQ1HM		0x7
+#define AD9832_PHASE0L		0x8
+#define AD9832_PHASE0H		0x9
+#define AD9832_PHASE1L		0xA
+#define AD9832_PHASE1H		0xB
+#define AD9832_PHASE2L		0xC
+#define AD9832_PHASE2H		0xD
+#define AD9832_PHASE3L		0xE
+#define AD9832_PHASE3H		0xF
+
+#define AD9832_PHASE_SYM	0x10
+#define AD9832_FREQ_SYM		0x11
+#define AD9832_PINCTRL_EN	0x12
+#define AD9832_OUTPUT_EN	0x13
+
+/* Command Control Bits */
+
+#define AD9832_CMD_PHA8BITSW	0x1
+#define AD9832_CMD_PHA16BITSW	0x0
+#define AD9832_CMD_FRE8BITSW	0x3
+#define AD9832_CMD_FRE16BITSW	0x2
+#define AD9832_CMD_FPSELECT	0x6
+#define AD9832_CMD_SYNCSELSRC	0x8
+#define AD9832_CMD_SLEEPRESCLR	0xC
+
+#define AD9832_FREQ		BIT(11)
+#define AD9832_PHASE(x)		(((x) & 3) << 9)
+#define AD9832_SYNC		BIT(13)
+#define AD9832_SELSRC		BIT(12)
+#define AD9832_SLEEP		BIT(13)
+#define AD9832_RESET		BIT(12)
+#define AD9832_CLR		BIT(11)
+#define CMD_SHIFT		12
+#define ADD_SHIFT		8
+#define AD9832_FREQ_BITS	32
+#define AD9832_PHASE_BITS	12
+#define RES_MASK(bits)		((1 << (bits)) - 1)
+
+/**
+ * struct ad9832_state - driver instance specific data
+ * @spi:		spi_device
+ * @avdd:		supply regulator for the analog section
+ * @dvdd:		supply regulator for the digital section
+ * @mclk:		external master clock
+ * @ctrl_fp:		cached frequency/phase control word
+ * @ctrl_ss:		cached sync/selsrc control word
+ * @ctrl_src:		cached sleep/reset/clr word
+ * @xfer:		default spi transfer
+ * @msg:		default spi message
+ * @freq_xfer:		tuning word spi transfer
+ * @freq_msg:		tuning word spi message
+ * @phase_xfer:		tuning word spi transfer
+ * @phase_msg:		tuning word spi message
+ * @lock:		protect sensor state
+ * @data:		spi transmit buffer
+ * @phase_data:		tuning word spi transmit buffer
+ * @freq_data:		tuning word spi transmit buffer
+ */
+
+struct ad9832_state {
+	struct spi_device		*spi;
+	struct regulator		*avdd;
+	struct regulator		*dvdd;
+	struct clk			*mclk;
+	unsigned short			ctrl_fp;
+	unsigned short			ctrl_ss;
+	unsigned short			ctrl_src;
+	struct spi_transfer		xfer;
+	struct spi_message		msg;
+	struct spi_transfer		freq_xfer[4];
+	struct spi_message		freq_msg;
+	struct spi_transfer		phase_xfer[2];
+	struct spi_message		phase_msg;
+	struct mutex			lock;	/* protect sensor state */
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	union {
+		__be16			freq_data[4];
+		__be16			phase_data[2];
+		__be16			data;
+	} __aligned(IIO_DMA_MINALIGN);
+};
+
+static unsigned long ad9832_calc_freqreg(unsigned long mclk, unsigned long fout)
+{
+	unsigned long long freqreg = (u64)fout *
+				     (u64)((u64)1L << AD9832_FREQ_BITS);
+	do_div(freqreg, mclk);
+	return freqreg;
+}
+
+static int ad9832_write_frequency(struct ad9832_state *st,
+				  unsigned int addr, unsigned long fout)
+{
+	unsigned long regval;
+
+	if (fout > (clk_get_rate(st->mclk) / 2))
+		return -EINVAL;
+
+	regval = ad9832_calc_freqreg(clk_get_rate(st->mclk), fout);
+
+	st->freq_data[0] = cpu_to_be16((AD9832_CMD_FRE8BITSW << CMD_SHIFT) |
+					(addr << ADD_SHIFT) |
+					((regval >> 24) & 0xFF));
+	st->freq_data[1] = cpu_to_be16((AD9832_CMD_FRE16BITSW << CMD_SHIFT) |
+					((addr - 1) << ADD_SHIFT) |
+					((regval >> 16) & 0xFF));
+	st->freq_data[2] = cpu_to_be16((AD9832_CMD_FRE8BITSW << CMD_SHIFT) |
+					((addr - 2) << ADD_SHIFT) |
+					((regval >> 8) & 0xFF));
+	st->freq_data[3] = cpu_to_be16((AD9832_CMD_FRE16BITSW << CMD_SHIFT) |
+					((addr - 3) << ADD_SHIFT) |
+					((regval >> 0) & 0xFF));
+
+	return spi_sync(st->spi, &st->freq_msg);
+}
+
+static int ad9832_write_phase(struct ad9832_state *st,
+			      unsigned long addr, unsigned long phase)
+{
+	if (phase > BIT(AD9832_PHASE_BITS))
+		return -EINVAL;
+
+	st->phase_data[0] = cpu_to_be16((AD9832_CMD_PHA8BITSW << CMD_SHIFT) |
+					(addr << ADD_SHIFT) |
+					((phase >> 8) & 0xFF));
+	st->phase_data[1] = cpu_to_be16((AD9832_CMD_PHA16BITSW << CMD_SHIFT) |
+					((addr - 1) << ADD_SHIFT) |
+					(phase & 0xFF));
+
+	return spi_sync(st->spi, &st->phase_msg);
+}
+
+static ssize_t ad9832_write(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad9832_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret;
+	unsigned long val;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		goto error_ret;
+
+	mutex_lock(&st->lock);
+	switch ((u32)this_attr->address) {
+	case AD9832_FREQ0HM:
+	case AD9832_FREQ1HM:
+		ret = ad9832_write_frequency(st, this_attr->address, val);
+		break;
+	case AD9832_PHASE0H:
+	case AD9832_PHASE1H:
+	case AD9832_PHASE2H:
+	case AD9832_PHASE3H:
+		ret = ad9832_write_phase(st, this_attr->address, val);
+		break;
+	case AD9832_PINCTRL_EN:
+		if (val)
+			st->ctrl_ss &= ~AD9832_SELSRC;
+		else
+			st->ctrl_ss |= AD9832_SELSRC;
+		st->data = cpu_to_be16((AD9832_CMD_SYNCSELSRC << CMD_SHIFT) |
+					st->ctrl_ss);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9832_FREQ_SYM:
+		if (val == 1) {
+			st->ctrl_fp |= AD9832_FREQ;
+		} else if (val == 0) {
+			st->ctrl_fp &= ~AD9832_FREQ;
+		} else {
+			ret = -EINVAL;
+			break;
+		}
+		st->data = cpu_to_be16((AD9832_CMD_FPSELECT << CMD_SHIFT) |
+					st->ctrl_fp);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9832_PHASE_SYM:
+		if (val > 3) {
+			ret = -EINVAL;
+			break;
+		}
+
+		st->ctrl_fp &= ~AD9832_PHASE(3);
+		st->ctrl_fp |= AD9832_PHASE(val);
+
+		st->data = cpu_to_be16((AD9832_CMD_FPSELECT << CMD_SHIFT) |
+					st->ctrl_fp);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9832_OUTPUT_EN:
+		if (val)
+			st->ctrl_src &= ~(AD9832_RESET | AD9832_SLEEP |
+					AD9832_CLR);
+		else
+			st->ctrl_src |= AD9832_RESET;
+
+		st->data = cpu_to_be16((AD9832_CMD_SLEEPRESCLR << CMD_SHIFT) |
+					st->ctrl_src);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	default:
+		ret = -ENODEV;
+	}
+	mutex_unlock(&st->lock);
+
+error_ret:
+	return ret ? ret : len;
+}
+
+/*
+ * see dds.h for further information
+ */
+
+static IIO_DEV_ATTR_FREQ(0, 0, 0200, NULL, ad9832_write, AD9832_FREQ0HM);
+static IIO_DEV_ATTR_FREQ(0, 1, 0200, NULL, ad9832_write, AD9832_FREQ1HM);
+static IIO_DEV_ATTR_FREQSYMBOL(0, 0200, NULL, ad9832_write, AD9832_FREQ_SYM);
+static IIO_CONST_ATTR_FREQ_SCALE(0, "1"); /* 1Hz */
+
+static IIO_DEV_ATTR_PHASE(0, 0, 0200, NULL, ad9832_write, AD9832_PHASE0H);
+static IIO_DEV_ATTR_PHASE(0, 1, 0200, NULL, ad9832_write, AD9832_PHASE1H);
+static IIO_DEV_ATTR_PHASE(0, 2, 0200, NULL, ad9832_write, AD9832_PHASE2H);
+static IIO_DEV_ATTR_PHASE(0, 3, 0200, NULL, ad9832_write, AD9832_PHASE3H);
+static IIO_DEV_ATTR_PHASESYMBOL(0, 0200, NULL,
+				ad9832_write, AD9832_PHASE_SYM);
+static IIO_CONST_ATTR_PHASE_SCALE(0, "0.0015339808"); /* 2PI/2^12 rad*/
+
+static IIO_DEV_ATTR_PINCONTROL_EN(0, 0200, NULL,
+				ad9832_write, AD9832_PINCTRL_EN);
+static IIO_DEV_ATTR_OUT_ENABLE(0, 0200, NULL,
+				ad9832_write, AD9832_OUTPUT_EN);
+
+static struct attribute *ad9832_attributes[] = {
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase2.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase3.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_pincontrol_en.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad9832_attribute_group = {
+	.attrs = ad9832_attributes,
+};
+
+static const struct iio_info ad9832_info = {
+	.attrs = &ad9832_attribute_group,
+};
+
+static void ad9832_reg_disable(void *reg)
+{
+	regulator_disable(reg);
+}
+
+static void ad9832_clk_disable(void *clk)
+{
+	clk_disable_unprepare(clk);
+}
+
+static int ad9832_probe(struct spi_device *spi)
+{
+	struct ad9832_platform_data *pdata = dev_get_platdata(&spi->dev);
+	struct iio_dev *indio_dev;
+	struct ad9832_state *st;
+	int ret;
+
+	if (!pdata) {
+		dev_dbg(&spi->dev, "no platform data?\n");
+		return -ENODEV;
+	}
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->avdd = devm_regulator_get(&spi->dev, "avdd");
+	if (IS_ERR(st->avdd))
+		return PTR_ERR(st->avdd);
+
+	ret = regulator_enable(st->avdd);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to enable specified AVDD supply\n");
+		return ret;
+	}
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9832_reg_disable, st->avdd);
+	if (ret)
+		return ret;
+
+	st->dvdd = devm_regulator_get(&spi->dev, "dvdd");
+	if (IS_ERR(st->dvdd))
+		return PTR_ERR(st->dvdd);
+
+	ret = regulator_enable(st->dvdd);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to enable specified DVDD supply\n");
+		return ret;
+	}
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9832_reg_disable, st->dvdd);
+	if (ret)
+		return ret;
+
+	st->mclk = devm_clk_get(&spi->dev, "mclk");
+	if (IS_ERR(st->mclk))
+		return PTR_ERR(st->mclk);
+
+	ret = clk_prepare_enable(st->mclk);
+	if (ret < 0)
+		return ret;
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9832_clk_disable, st->mclk);
+	if (ret)
+		return ret;
+
+	st->spi = spi;
+	mutex_init(&st->lock);
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->info = &ad9832_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	/* Setup default messages */
+
+	st->xfer.tx_buf = &st->data;
+	st->xfer.len = 2;
+
+	spi_message_init(&st->msg);
+	spi_message_add_tail(&st->xfer, &st->msg);
+
+	st->freq_xfer[0].tx_buf = &st->freq_data[0];
+	st->freq_xfer[0].len = 2;
+	st->freq_xfer[0].cs_change = 1;
+	st->freq_xfer[1].tx_buf = &st->freq_data[1];
+	st->freq_xfer[1].len = 2;
+	st->freq_xfer[1].cs_change = 1;
+	st->freq_xfer[2].tx_buf = &st->freq_data[2];
+	st->freq_xfer[2].len = 2;
+	st->freq_xfer[2].cs_change = 1;
+	st->freq_xfer[3].tx_buf = &st->freq_data[3];
+	st->freq_xfer[3].len = 2;
+
+	spi_message_init(&st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[0], &st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[1], &st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[2], &st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[3], &st->freq_msg);
+
+	st->phase_xfer[0].tx_buf = &st->phase_data[0];
+	st->phase_xfer[0].len = 2;
+	st->phase_xfer[0].cs_change = 1;
+	st->phase_xfer[1].tx_buf = &st->phase_data[1];
+	st->phase_xfer[1].len = 2;
+
+	spi_message_init(&st->phase_msg);
+	spi_message_add_tail(&st->phase_xfer[0], &st->phase_msg);
+	spi_message_add_tail(&st->phase_xfer[1], &st->phase_msg);
+
+	st->ctrl_src = AD9832_SLEEP | AD9832_RESET | AD9832_CLR;
+	st->data = cpu_to_be16((AD9832_CMD_SLEEPRESCLR << CMD_SHIFT) |
+					st->ctrl_src);
+	ret = spi_sync(st->spi, &st->msg);
+	if (ret) {
+		dev_err(&spi->dev, "device init failed\n");
+		return ret;
+	}
+
+	ret = ad9832_write_frequency(st, AD9832_FREQ0HM, pdata->freq0);
+	if (ret)
+		return ret;
+
+	ret = ad9832_write_frequency(st, AD9832_FREQ1HM, pdata->freq1);
+	if (ret)
+		return ret;
+
+	ret = ad9832_write_phase(st, AD9832_PHASE0H, pdata->phase0);
+	if (ret)
+		return ret;
+
+	ret = ad9832_write_phase(st, AD9832_PHASE1H, pdata->phase1);
+	if (ret)
+		return ret;
+
+	ret = ad9832_write_phase(st, AD9832_PHASE2H, pdata->phase2);
+	if (ret)
+		return ret;
+
+	ret = ad9832_write_phase(st, AD9832_PHASE3H, pdata->phase3);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct spi_device_id ad9832_id[] = {
+	{"ad9832", 0},
+	{"ad9835", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad9832_id);
+
+static struct spi_driver ad9832_driver = {
+	.driver = {
+		.name	= "ad9832",
+	},
+	.probe		= ad9832_probe,
+	.id_table	= ad9832_id,
+};
+module_spi_driver(ad9832_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Analog Devices AD9832/AD9835 DDS");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/frequency/ad9832.h b/drivers/staging/iio/frequency/ad9832.h
new file mode 100644
index 000000000..98dfbd928
--- /dev/null
+++ b/drivers/staging/iio/frequency/ad9832.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * AD9832 SPI DDS driver
+ *
+ * Copyright 2011 Analog Devices Inc.
+ */
+#ifndef IIO_DDS_AD9832_H_
+#define IIO_DDS_AD9832_H_
+
+/*
+ * TODO: struct ad9832_platform_data needs to go into include/linux/iio
+ */
+
+/**
+ * struct ad9832_platform_data - platform specific information
+ * @mclk:		master clock in Hz
+ * @freq0:		power up freq0 tuning word in Hz
+ * @freq1:		power up freq1 tuning word in Hz
+ * @phase0:		power up phase0 value [0..4095] correlates with 0..2PI
+ * @phase1:		power up phase1 value [0..4095] correlates with 0..2PI
+ * @phase2:		power up phase2 value [0..4095] correlates with 0..2PI
+ * @phase3:		power up phase3 value [0..4095] correlates with 0..2PI
+ */
+
+struct ad9832_platform_data {
+	unsigned long		freq0;
+	unsigned long		freq1;
+	unsigned short		phase0;
+	unsigned short		phase1;
+	unsigned short		phase2;
+	unsigned short		phase3;
+};
+
+#endif /* IIO_DDS_AD9832_H_ */
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
new file mode 100644
index 000000000..2b4267a87
--- /dev/null
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -0,0 +1,545 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * AD9833/AD9834/AD9837/AD9838 SPI DDS driver
+ *
+ * Copyright 2010-2011 Analog Devices Inc.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/list.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <asm/div64.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include "dds.h"
+
+#include "ad9834.h"
+
+/* Registers */
+
+#define AD9834_REG_CMD		0
+#define AD9834_REG_FREQ0	BIT(14)
+#define AD9834_REG_FREQ1	BIT(15)
+#define AD9834_REG_PHASE0	(BIT(15) | BIT(14))
+#define AD9834_REG_PHASE1	(BIT(15) | BIT(14) | BIT(13))
+
+/* Command Control Bits */
+
+#define AD9834_B28		BIT(13)
+#define AD9834_HLB		BIT(12)
+#define AD9834_FSEL		BIT(11)
+#define AD9834_PSEL		BIT(10)
+#define AD9834_PIN_SW		BIT(9)
+#define AD9834_RESET		BIT(8)
+#define AD9834_SLEEP1		BIT(7)
+#define AD9834_SLEEP12		BIT(6)
+#define AD9834_OPBITEN		BIT(5)
+#define AD9834_SIGN_PIB		BIT(4)
+#define AD9834_DIV2		BIT(3)
+#define AD9834_MODE		BIT(1)
+
+#define AD9834_FREQ_BITS	28
+#define AD9834_PHASE_BITS	12
+
+#define RES_MASK(bits)	(BIT(bits) - 1)
+
+/**
+ * struct ad9834_state - driver instance specific data
+ * @spi:		spi_device
+ * @mclk:		external master clock
+ * @control:		cached control word
+ * @devid:		device id
+ * @xfer:		default spi transfer
+ * @msg:		default spi message
+ * @freq_xfer:		tuning word spi transfer
+ * @freq_msg:		tuning word spi message
+ * @lock:		protect sensor state
+ * @data:		spi transmit buffer
+ * @freq_data:		tuning word spi transmit buffer
+ */
+
+struct ad9834_state {
+	struct spi_device		*spi;
+	struct clk			*mclk;
+	unsigned short			control;
+	unsigned short			devid;
+	struct spi_transfer		xfer;
+	struct spi_message		msg;
+	struct spi_transfer		freq_xfer[2];
+	struct spi_message		freq_msg;
+	struct mutex                    lock;   /* protect sensor state */
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	__be16				data __aligned(IIO_DMA_MINALIGN);
+	__be16				freq_data[2];
+};
+
+/*
+ * ad9834_supported_device_ids:
+ */
+
+enum ad9834_supported_device_ids {
+	ID_AD9833,
+	ID_AD9834,
+	ID_AD9837,
+	ID_AD9838,
+};
+
+static unsigned int ad9834_calc_freqreg(unsigned long mclk, unsigned long fout)
+{
+	unsigned long long freqreg = (u64)fout * (u64)BIT(AD9834_FREQ_BITS);
+
+	do_div(freqreg, mclk);
+	return freqreg;
+}
+
+static int ad9834_write_frequency(struct ad9834_state *st,
+				  unsigned long addr, unsigned long fout)
+{
+	unsigned long clk_freq;
+	unsigned long regval;
+
+	clk_freq = clk_get_rate(st->mclk);
+
+	if (fout > (clk_freq / 2))
+		return -EINVAL;
+
+	regval = ad9834_calc_freqreg(clk_freq, fout);
+
+	st->freq_data[0] = cpu_to_be16(addr | (regval &
+				       RES_MASK(AD9834_FREQ_BITS / 2)));
+	st->freq_data[1] = cpu_to_be16(addr | ((regval >>
+				       (AD9834_FREQ_BITS / 2)) &
+				       RES_MASK(AD9834_FREQ_BITS / 2)));
+
+	return spi_sync(st->spi, &st->freq_msg);
+}
+
+static int ad9834_write_phase(struct ad9834_state *st,
+			      unsigned long addr, unsigned long phase)
+{
+	if (phase > BIT(AD9834_PHASE_BITS))
+		return -EINVAL;
+	st->data = cpu_to_be16(addr | phase);
+
+	return spi_sync(st->spi, &st->msg);
+}
+
+static ssize_t ad9834_write(struct device *dev,
+			    struct device_attribute *attr,
+			    const char *buf,
+			    size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad9834_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret;
+	unsigned long val;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	mutex_lock(&st->lock);
+	switch ((u32)this_attr->address) {
+	case AD9834_REG_FREQ0:
+	case AD9834_REG_FREQ1:
+		ret = ad9834_write_frequency(st, this_attr->address, val);
+		break;
+	case AD9834_REG_PHASE0:
+	case AD9834_REG_PHASE1:
+		ret = ad9834_write_phase(st, this_attr->address, val);
+		break;
+	case AD9834_OPBITEN:
+		if (st->control & AD9834_MODE) {
+			ret = -EINVAL;  /* AD9843 reserved mode */
+			break;
+		}
+
+		if (val)
+			st->control |= AD9834_OPBITEN;
+		else
+			st->control &= ~AD9834_OPBITEN;
+
+		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9834_PIN_SW:
+		if (val)
+			st->control |= AD9834_PIN_SW;
+		else
+			st->control &= ~AD9834_PIN_SW;
+		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9834_FSEL:
+	case AD9834_PSEL:
+		if (!val) {
+			st->control &= ~(this_attr->address | AD9834_PIN_SW);
+		} else if (val == 1) {
+			st->control |= this_attr->address;
+			st->control &= ~AD9834_PIN_SW;
+		} else {
+			ret = -EINVAL;
+			break;
+		}
+		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	case AD9834_RESET:
+		if (val)
+			st->control &= ~AD9834_RESET;
+		else
+			st->control |= AD9834_RESET;
+
+		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+		ret = spi_sync(st->spi, &st->msg);
+		break;
+	default:
+		ret = -ENODEV;
+	}
+	mutex_unlock(&st->lock);
+
+	return ret ? ret : len;
+}
+
+static ssize_t ad9834_store_wavetype(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad9834_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret = 0;
+	bool is_ad9833_7 = (st->devid == ID_AD9833) || (st->devid == ID_AD9837);
+
+	mutex_lock(&st->lock);
+
+	switch ((u32)this_attr->address) {
+	case 0:
+		if (sysfs_streq(buf, "sine")) {
+			st->control &= ~AD9834_MODE;
+			if (is_ad9833_7)
+				st->control &= ~AD9834_OPBITEN;
+		} else if (sysfs_streq(buf, "triangle")) {
+			if (is_ad9833_7) {
+				st->control &= ~AD9834_OPBITEN;
+				st->control |= AD9834_MODE;
+			} else if (st->control & AD9834_OPBITEN) {
+				ret = -EINVAL;	/* AD9843 reserved mode */
+			} else {
+				st->control |= AD9834_MODE;
+			}
+		} else if (is_ad9833_7 && sysfs_streq(buf, "square")) {
+			st->control &= ~AD9834_MODE;
+			st->control |= AD9834_OPBITEN;
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+	case 1:
+		if (sysfs_streq(buf, "square") &&
+		    !(st->control & AD9834_MODE)) {
+			st->control &= ~AD9834_MODE;
+			st->control |= AD9834_OPBITEN;
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (!ret) {
+		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+		ret = spi_sync(st->spi, &st->msg);
+	}
+	mutex_unlock(&st->lock);
+
+	return ret ? ret : len;
+}
+
+static
+ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad9834_state *st = iio_priv(indio_dev);
+	char *str;
+
+	if (st->devid == ID_AD9833 || st->devid == ID_AD9837)
+		str = "sine triangle square";
+	else if (st->control & AD9834_OPBITEN)
+		str = "sine";
+	else
+		str = "sine triangle";
+
+	return sprintf(buf, "%s\n", str);
+}
+
+static IIO_DEVICE_ATTR(out_altvoltage0_out0_wavetype_available, 0444,
+		       ad9834_show_out0_wavetype_available, NULL, 0);
+
+static
+ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad9834_state *st = iio_priv(indio_dev);
+	char *str;
+
+	if (st->control & AD9834_MODE)
+		str = "";
+	else
+		str = "square";
+
+	return sprintf(buf, "%s\n", str);
+}
+
+static IIO_DEVICE_ATTR(out_altvoltage0_out1_wavetype_available, 0444,
+		       ad9834_show_out1_wavetype_available, NULL, 0);
+
+/*
+ * see dds.h for further information
+ */
+
+static IIO_DEV_ATTR_FREQ(0, 0, 0200, NULL, ad9834_write, AD9834_REG_FREQ0);
+static IIO_DEV_ATTR_FREQ(0, 1, 0200, NULL, ad9834_write, AD9834_REG_FREQ1);
+static IIO_DEV_ATTR_FREQSYMBOL(0, 0200, NULL, ad9834_write, AD9834_FSEL);
+static IIO_CONST_ATTR_FREQ_SCALE(0, "1"); /* 1Hz */
+
+static IIO_DEV_ATTR_PHASE(0, 0, 0200, NULL, ad9834_write, AD9834_REG_PHASE0);
+static IIO_DEV_ATTR_PHASE(0, 1, 0200, NULL, ad9834_write, AD9834_REG_PHASE1);
+static IIO_DEV_ATTR_PHASESYMBOL(0, 0200, NULL, ad9834_write, AD9834_PSEL);
+static IIO_CONST_ATTR_PHASE_SCALE(0, "0.0015339808"); /* 2PI/2^12 rad*/
+
+static IIO_DEV_ATTR_PINCONTROL_EN(0, 0200, NULL,
+	ad9834_write, AD9834_PIN_SW);
+static IIO_DEV_ATTR_OUT_ENABLE(0, 0200, NULL, ad9834_write, AD9834_RESET);
+static IIO_DEV_ATTR_OUTY_ENABLE(0, 1, 0200, NULL,
+	ad9834_write, AD9834_OPBITEN);
+static IIO_DEV_ATTR_OUT_WAVETYPE(0, 0, ad9834_store_wavetype, 0);
+static IIO_DEV_ATTR_OUT_WAVETYPE(0, 1, ad9834_store_wavetype, 1);
+
+static struct attribute *ad9834_attributes[] = {
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_pincontrol_en.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype_available.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_wavetype_available.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute *ad9833_attributes[] = {
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad9834_attribute_group = {
+	.attrs = ad9834_attributes,
+};
+
+static const struct attribute_group ad9833_attribute_group = {
+	.attrs = ad9833_attributes,
+};
+
+static const struct iio_info ad9834_info = {
+	.attrs = &ad9834_attribute_group,
+};
+
+static const struct iio_info ad9833_info = {
+	.attrs = &ad9833_attribute_group,
+};
+
+static void ad9834_disable_reg(void *data)
+{
+	struct regulator *reg = data;
+
+	regulator_disable(reg);
+}
+
+static void ad9834_disable_clk(void *data)
+{
+	struct clk *clk = data;
+
+	clk_disable_unprepare(clk);
+}
+
+static int ad9834_probe(struct spi_device *spi)
+{
+	struct ad9834_state *st;
+	struct iio_dev *indio_dev;
+	struct regulator *reg;
+	int ret;
+
+	reg = devm_regulator_get(&spi->dev, "avdd");
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
+
+	ret = regulator_enable(reg);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to enable specified AVDD supply\n");
+		return ret;
+	}
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9834_disable_reg, reg);
+	if (ret)
+		return ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev) {
+		ret = -ENOMEM;
+		return ret;
+	}
+	st = iio_priv(indio_dev);
+	mutex_init(&st->lock);
+	st->mclk = devm_clk_get(&spi->dev, NULL);
+	if (IS_ERR(st->mclk)) {
+		ret = PTR_ERR(st->mclk);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(st->mclk);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to enable master clock\n");
+		return ret;
+	}
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9834_disable_clk, st->mclk);
+	if (ret)
+		return ret;
+
+	st->spi = spi;
+	st->devid = spi_get_device_id(spi)->driver_data;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	switch (st->devid) {
+	case ID_AD9833:
+	case ID_AD9837:
+		indio_dev->info = &ad9833_info;
+		break;
+	default:
+		indio_dev->info = &ad9834_info;
+		break;
+	}
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	/* Setup default messages */
+
+	st->xfer.tx_buf = &st->data;
+	st->xfer.len = 2;
+
+	spi_message_init(&st->msg);
+	spi_message_add_tail(&st->xfer, &st->msg);
+
+	st->freq_xfer[0].tx_buf = &st->freq_data[0];
+	st->freq_xfer[0].len = 2;
+	st->freq_xfer[0].cs_change = 1;
+	st->freq_xfer[1].tx_buf = &st->freq_data[1];
+	st->freq_xfer[1].len = 2;
+
+	spi_message_init(&st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[0], &st->freq_msg);
+	spi_message_add_tail(&st->freq_xfer[1], &st->freq_msg);
+
+	st->control = AD9834_B28 | AD9834_RESET;
+	st->control |= AD9834_DIV2;
+
+	if (st->devid == ID_AD9834)
+		st->control |= AD9834_SIGN_PIB;
+
+	st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
+	ret = spi_sync(st->spi, &st->msg);
+	if (ret) {
+		dev_err(&spi->dev, "device init failed\n");
+		return ret;
+	}
+
+	ret = ad9834_write_frequency(st, AD9834_REG_FREQ0, 1000000);
+	if (ret)
+		return ret;
+
+	ret = ad9834_write_frequency(st, AD9834_REG_FREQ1, 5000000);
+	if (ret)
+		return ret;
+
+	ret = ad9834_write_phase(st, AD9834_REG_PHASE0, 512);
+	if (ret)
+		return ret;
+
+	ret = ad9834_write_phase(st, AD9834_REG_PHASE1, 1024);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct spi_device_id ad9834_id[] = {
+	{"ad9833", ID_AD9833},
+	{"ad9834", ID_AD9834},
+	{"ad9837", ID_AD9837},
+	{"ad9838", ID_AD9838},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad9834_id);
+
+static const struct of_device_id ad9834_of_match[] = {
+	{.compatible = "adi,ad9833"},
+	{.compatible = "adi,ad9834"},
+	{.compatible = "adi,ad9837"},
+	{.compatible = "adi,ad9838"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, ad9834_of_match);
+
+static struct spi_driver ad9834_driver = {
+	.driver = {
+		.name	= "ad9834",
+		.of_match_table = ad9834_of_match
+	},
+	.probe		= ad9834_probe,
+	.id_table	= ad9834_id,
+};
+module_spi_driver(ad9834_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Analog Devices AD9833/AD9834/AD9837/AD9838 DDS");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/frequency/ad9834.h b/drivers/staging/iio/frequency/ad9834.h
new file mode 100644
index 000000000..521943aa0
--- /dev/null
+++ b/drivers/staging/iio/frequency/ad9834.h
@@ -0,0 +1,10 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * AD9833/AD9834/AD9837/AD9838 SPI DDS driver
+ *
+ * Copyright 2010-2011 Analog Devices Inc.
+ */
+#ifndef IIO_DDS_AD9834_H_
+#define IIO_DDS_AD9834_H_
+
+#endif /* IIO_DDS_AD9834_H_ */
diff --git a/drivers/staging/iio/frequency/dds.h b/drivers/staging/iio/frequency/dds.h
new file mode 100644
index 000000000..2ebe68eb7
--- /dev/null
+++ b/drivers/staging/iio/frequency/dds.h
@@ -0,0 +1,113 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * dds.h - sysfs attributes associated with DDS devices
+ *
+ * Copyright (c) 2010 Analog Devices Inc.
+ */
+#ifndef IIO_DDS_H_
+#define IIO_DDS_H_
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencyY
+ */
+
+#define IIO_DEV_ATTR_FREQ(_channel, _num, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_frequency##_num,	\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencyY_scale
+ */
+
+#define IIO_CONST_ATTR_FREQ_SCALE(_channel, _string)			\
+	IIO_CONST_ATTR(out_altvoltage##_channel##_frequency_scale, _string)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencysymbol
+ */
+
+#define IIO_DEV_ATTR_FREQSYMBOL(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_frequencysymbol,	\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phaseY
+ */
+
+#define IIO_DEV_ATTR_PHASE(_channel, _num, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_phase##_num,		\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phaseY_scale
+ */
+
+#define IIO_CONST_ATTR_PHASE_SCALE(_channel, _string)			\
+	IIO_CONST_ATTR(out_altvoltage##_channel##_phase_scale, _string)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phasesymbol
+ */
+
+#define IIO_DEV_ATTR_PHASESYMBOL(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_phasesymbol,		\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_en,	\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_frequency_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_FREQ_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_frequency_en,\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_phase_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_PHASE_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_phase_en,	\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_out_enable
+ */
+
+#define IIO_DEV_ATTR_OUT_ENABLE(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out_enable,		\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_enable
+ */
+
+#define IIO_DEV_ATTR_OUTY_ENABLE(_channel, _output,			\
+			_mode, _show, _store, _addr)			\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_enable,\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype
+ */
+
+#define IIO_DEV_ATTR_OUT_WAVETYPE(_channel, _output, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_wavetype,\
+			0200, NULL, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
+ */
+
+#define IIO_CONST_ATTR_OUT_WAVETYPES_AVAILABLE(_channel, _output, _modes)\
+	IIO_CONST_ATTR(							\
+	out_altvoltage##_channel##_out##_output##_wavetype_available, _modes)
+
+#endif /* IIO_DDS_H_ */
diff --git a/drivers/staging/iio/impedance-analyzer/Kconfig b/drivers/staging/iio/impedance-analyzer/Kconfig
new file mode 100644
index 000000000..841648847
--- /dev/null
+++ b/drivers/staging/iio/impedance-analyzer/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Impedance Converter, Network Analyzer drivers
+#
+menu "Network Analyzer, Impedance Converters"
+
+config AD5933
+	tristate "Analog Devices AD5933, AD5934 driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_KFIFO_BUF
+	help
+	  Say yes here to build support for Analog Devices Impedance Converter,
+	  Network Analyzer, AD5933/4.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad5933.
+
+endmenu
diff --git a/drivers/staging/iio/impedance-analyzer/Makefile b/drivers/staging/iio/impedance-analyzer/Makefile
new file mode 100644
index 000000000..b4e657a1a
--- /dev/null
+++ b/drivers/staging/iio/impedance-analyzer/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for Impedance Converter, Network Analyzer drivers
+#
+
+obj-$(CONFIG_AD5933) += ad5933.o
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
new file mode 100644
index 000000000..f177b20f0
--- /dev/null
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -0,0 +1,791 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * AD5933 AD5934 Impedance Converter, Network Analyzer
+ *
+ * Copyright 2011 Analog Devices Inc.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sysfs.h>
+#include <linux/types.h>
+
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/sysfs.h>
+
+/* AD5933/AD5934 Registers */
+#define AD5933_REG_CONTROL_HB		0x80	/* R/W, 1 byte */
+#define AD5933_REG_CONTROL_LB		0x81	/* R/W, 1 byte */
+#define AD5933_REG_FREQ_START		0x82	/* R/W, 3 bytes */
+#define AD5933_REG_FREQ_INC		0x85	/* R/W, 3 bytes */
+#define AD5933_REG_INC_NUM		0x88	/* R/W, 2 bytes, 9 bit */
+#define AD5933_REG_SETTLING_CYCLES	0x8A	/* R/W, 2 bytes */
+#define AD5933_REG_STATUS		0x8F	/* R, 1 byte */
+#define AD5933_REG_TEMP_DATA		0x92	/* R, 2 bytes*/
+#define AD5933_REG_REAL_DATA		0x94	/* R, 2 bytes*/
+#define AD5933_REG_IMAG_DATA		0x96	/* R, 2 bytes*/
+
+/* AD5933_REG_CONTROL_HB Bits */
+#define AD5933_CTRL_INIT_START_FREQ	(0x1 << 4)
+#define AD5933_CTRL_START_SWEEP		(0x2 << 4)
+#define AD5933_CTRL_INC_FREQ		(0x3 << 4)
+#define AD5933_CTRL_REPEAT_FREQ		(0x4 << 4)
+#define AD5933_CTRL_MEASURE_TEMP	(0x9 << 4)
+#define AD5933_CTRL_POWER_DOWN		(0xA << 4)
+#define AD5933_CTRL_STANDBY		(0xB << 4)
+
+#define AD5933_CTRL_RANGE_2000mVpp	(0x0 << 1)
+#define AD5933_CTRL_RANGE_200mVpp	(0x1 << 1)
+#define AD5933_CTRL_RANGE_400mVpp	(0x2 << 1)
+#define AD5933_CTRL_RANGE_1000mVpp	(0x3 << 1)
+#define AD5933_CTRL_RANGE(x)		((x) << 1)
+
+#define AD5933_CTRL_PGA_GAIN_1		(0x1 << 0)
+#define AD5933_CTRL_PGA_GAIN_5		(0x0 << 0)
+
+/* AD5933_REG_CONTROL_LB Bits */
+#define AD5933_CTRL_RESET		(0x1 << 4)
+#define AD5933_CTRL_INT_SYSCLK		(0x0 << 3)
+#define AD5933_CTRL_EXT_SYSCLK		(0x1 << 3)
+
+/* AD5933_REG_STATUS Bits */
+#define AD5933_STAT_TEMP_VALID		(0x1 << 0)
+#define AD5933_STAT_DATA_VALID		(0x1 << 1)
+#define AD5933_STAT_SWEEP_DONE		(0x1 << 2)
+
+/* I2C Block Commands */
+#define AD5933_I2C_BLOCK_WRITE		0xA0
+#define AD5933_I2C_BLOCK_READ		0xA1
+#define AD5933_I2C_ADDR_POINTER		0xB0
+
+/* Device Specs */
+#define AD5933_INT_OSC_FREQ_Hz		16776000
+#define AD5933_MAX_OUTPUT_FREQ_Hz	100000
+#define AD5933_MAX_RETRIES		100
+
+#define AD5933_OUT_RANGE		1
+#define AD5933_OUT_RANGE_AVAIL		2
+#define AD5933_OUT_SETTLING_CYCLES	3
+#define AD5933_IN_PGA_GAIN		4
+#define AD5933_IN_PGA_GAIN_AVAIL	5
+#define AD5933_FREQ_POINTS		6
+
+#define AD5933_POLL_TIME_ms		10
+#define AD5933_INIT_EXCITATION_TIME_ms	100
+
+struct ad5933_state {
+	struct i2c_client		*client;
+	struct regulator		*reg;
+	struct clk			*mclk;
+	struct delayed_work		work;
+	struct mutex			lock; /* Protect sensor state */
+	unsigned long			mclk_hz;
+	unsigned char			ctrl_hb;
+	unsigned char			ctrl_lb;
+	unsigned int			range_avail[4];
+	unsigned short			vref_mv;
+	unsigned short			settling_cycles;
+	unsigned short			freq_points;
+	unsigned int			freq_start;
+	unsigned int			freq_inc;
+	unsigned int			state;
+	unsigned int			poll_time_jiffies;
+};
+
+#define AD5933_CHANNEL(_type, _extend_name, _info_mask_separate, _address, \
+		_scan_index, _realbits) { \
+	.type = (_type), \
+	.extend_name = (_extend_name), \
+	.info_mask_separate = (_info_mask_separate), \
+	.address = (_address), \
+	.scan_index = (_scan_index), \
+	.scan_type = { \
+		.sign = 's', \
+		.realbits = (_realbits), \
+		.storagebits = 16, \
+	}, \
+}
+
+static const struct iio_chan_spec ad5933_channels[] = {
+	AD5933_CHANNEL(IIO_TEMP, NULL, BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_SCALE), AD5933_REG_TEMP_DATA, -1, 14),
+	/* Ring Channels */
+	AD5933_CHANNEL(IIO_VOLTAGE, "real", 0, AD5933_REG_REAL_DATA, 0, 16),
+	AD5933_CHANNEL(IIO_VOLTAGE, "imag", 0, AD5933_REG_IMAG_DATA, 1, 16),
+};
+
+static int ad5933_i2c_write(struct i2c_client *client, u8 reg, u8 len, u8 *data)
+{
+	int ret;
+
+	while (len--) {
+		ret = i2c_smbus_write_byte_data(client, reg++, *data++);
+		if (ret < 0) {
+			dev_err(&client->dev, "I2C write error\n");
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static int ad5933_i2c_read(struct i2c_client *client, u8 reg, u8 len, u8 *data)
+{
+	int ret;
+
+	while (len--) {
+		ret = i2c_smbus_read_byte_data(client, reg++);
+		if (ret < 0) {
+			dev_err(&client->dev, "I2C read error\n");
+			return ret;
+		}
+		*data++ = ret;
+	}
+	return 0;
+}
+
+static int ad5933_cmd(struct ad5933_state *st, unsigned char cmd)
+{
+	unsigned char dat = st->ctrl_hb | cmd;
+
+	return ad5933_i2c_write(st->client,
+			AD5933_REG_CONTROL_HB, 1, &dat);
+}
+
+static int ad5933_reset(struct ad5933_state *st)
+{
+	unsigned char dat = st->ctrl_lb | AD5933_CTRL_RESET;
+
+	return ad5933_i2c_write(st->client,
+			AD5933_REG_CONTROL_LB, 1, &dat);
+}
+
+static int ad5933_wait_busy(struct ad5933_state *st, unsigned char event)
+{
+	unsigned char val, timeout = AD5933_MAX_RETRIES;
+	int ret;
+
+	while (timeout--) {
+		ret =  ad5933_i2c_read(st->client, AD5933_REG_STATUS, 1, &val);
+		if (ret < 0)
+			return ret;
+		if (val & event)
+			return val;
+		cpu_relax();
+		mdelay(1);
+	}
+
+	return -EAGAIN;
+}
+
+static int ad5933_set_freq(struct ad5933_state *st,
+			   unsigned int reg, unsigned long freq)
+{
+	unsigned long long freqreg;
+	union {
+		__be32 d32;
+		u8 d8[4];
+	} dat;
+
+	freqreg = (u64)freq * (u64)(1 << 27);
+	do_div(freqreg, st->mclk_hz / 4);
+
+	switch (reg) {
+	case AD5933_REG_FREQ_START:
+		st->freq_start = freq;
+		break;
+	case AD5933_REG_FREQ_INC:
+		st->freq_inc = freq;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dat.d32 = cpu_to_be32(freqreg);
+	return ad5933_i2c_write(st->client, reg, 3, &dat.d8[1]);
+}
+
+static int ad5933_setup(struct ad5933_state *st)
+{
+	__be16 dat;
+	int ret;
+
+	ret = ad5933_reset(st);
+	if (ret < 0)
+		return ret;
+
+	ret = ad5933_set_freq(st, AD5933_REG_FREQ_START, 10000);
+	if (ret < 0)
+		return ret;
+
+	ret = ad5933_set_freq(st, AD5933_REG_FREQ_INC, 200);
+	if (ret < 0)
+		return ret;
+
+	st->settling_cycles = 10;
+	dat = cpu_to_be16(st->settling_cycles);
+
+	ret = ad5933_i2c_write(st->client,
+			       AD5933_REG_SETTLING_CYCLES,
+			       2, (u8 *)&dat);
+	if (ret < 0)
+		return ret;
+
+	st->freq_points = 100;
+	dat = cpu_to_be16(st->freq_points);
+
+	return ad5933_i2c_write(st->client, AD5933_REG_INC_NUM, 2, (u8 *)&dat);
+}
+
+static void ad5933_calc_out_ranges(struct ad5933_state *st)
+{
+	int i;
+	unsigned int normalized_3v3[4] = {1980, 198, 383, 970};
+
+	for (i = 0; i < 4; i++)
+		st->range_avail[i] = normalized_3v3[i] * st->vref_mv / 3300;
+}
+
+/*
+ * handles: AD5933_REG_FREQ_START and AD5933_REG_FREQ_INC
+ */
+
+static ssize_t ad5933_show_frequency(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad5933_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret;
+	unsigned long long freqreg;
+	union {
+		__be32 d32;
+		u8 d8[4];
+	} dat;
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+	ret = ad5933_i2c_read(st->client, this_attr->address, 3, &dat.d8[1]);
+	iio_device_release_direct_mode(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	freqreg = be32_to_cpu(dat.d32) & 0xFFFFFF;
+
+	freqreg = (u64)freqreg * (u64)(st->mclk_hz / 4);
+	do_div(freqreg, BIT(27));
+
+	return sprintf(buf, "%d\n", (int)freqreg);
+}
+
+static ssize_t ad5933_store_frequency(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf,
+				      size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad5933_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (val > AD5933_MAX_OUTPUT_FREQ_Hz)
+		return -EINVAL;
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+	ret = ad5933_set_freq(st, this_attr->address, val);
+	iio_device_release_direct_mode(indio_dev);
+
+	return ret ? ret : len;
+}
+
+static IIO_DEVICE_ATTR(out_altvoltage0_frequency_start, 0644,
+			ad5933_show_frequency,
+			ad5933_store_frequency,
+			AD5933_REG_FREQ_START);
+
+static IIO_DEVICE_ATTR(out_altvoltage0_frequency_increment, 0644,
+			ad5933_show_frequency,
+			ad5933_store_frequency,
+			AD5933_REG_FREQ_INC);
+
+static ssize_t ad5933_show(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad5933_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret = 0, len = 0;
+
+	mutex_lock(&st->lock);
+	switch ((u32)this_attr->address) {
+	case AD5933_OUT_RANGE:
+		len = sprintf(buf, "%u\n",
+			      st->range_avail[(st->ctrl_hb >> 1) & 0x3]);
+		break;
+	case AD5933_OUT_RANGE_AVAIL:
+		len = sprintf(buf, "%u %u %u %u\n", st->range_avail[0],
+			      st->range_avail[3], st->range_avail[2],
+			      st->range_avail[1]);
+		break;
+	case AD5933_OUT_SETTLING_CYCLES:
+		len = sprintf(buf, "%d\n", st->settling_cycles);
+		break;
+	case AD5933_IN_PGA_GAIN:
+		len = sprintf(buf, "%s\n",
+			      (st->ctrl_hb & AD5933_CTRL_PGA_GAIN_1) ?
+			      "1" : "0.2");
+		break;
+	case AD5933_IN_PGA_GAIN_AVAIL:
+		len = sprintf(buf, "1 0.2\n");
+		break;
+	case AD5933_FREQ_POINTS:
+		len = sprintf(buf, "%d\n", st->freq_points);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&st->lock);
+	return ret ? ret : len;
+}
+
+static ssize_t ad5933_store(struct device *dev,
+			    struct device_attribute *attr,
+			    const char *buf,
+			    size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad5933_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	u16 val;
+	int i, ret = 0;
+	__be16 dat;
+
+	if (this_attr->address != AD5933_IN_PGA_GAIN) {
+		ret = kstrtou16(buf, 10, &val);
+		if (ret)
+			return ret;
+	}
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+	mutex_lock(&st->lock);
+	switch ((u32)this_attr->address) {
+	case AD5933_OUT_RANGE:
+		ret = -EINVAL;
+		for (i = 0; i < 4; i++)
+			if (val == st->range_avail[i]) {
+				st->ctrl_hb &= ~AD5933_CTRL_RANGE(0x3);
+				st->ctrl_hb |= AD5933_CTRL_RANGE(i);
+				ret = ad5933_cmd(st, 0);
+				break;
+			}
+		break;
+	case AD5933_IN_PGA_GAIN:
+		if (sysfs_streq(buf, "1")) {
+			st->ctrl_hb |= AD5933_CTRL_PGA_GAIN_1;
+		} else if (sysfs_streq(buf, "0.2")) {
+			st->ctrl_hb &= ~AD5933_CTRL_PGA_GAIN_1;
+		} else {
+			ret = -EINVAL;
+			break;
+		}
+		ret = ad5933_cmd(st, 0);
+		break;
+	case AD5933_OUT_SETTLING_CYCLES:
+		val = clamp(val, (u16)0, (u16)0x7FF);
+		st->settling_cycles = val;
+
+		/* 2x, 4x handling, see datasheet */
+		if (val > 1022)
+			val = (val >> 2) | (3 << 9);
+		else if (val > 511)
+			val = (val >> 1) | BIT(9);
+
+		dat = cpu_to_be16(val);
+		ret = ad5933_i2c_write(st->client,
+				       AD5933_REG_SETTLING_CYCLES,
+				       2, (u8 *)&dat);
+		break;
+	case AD5933_FREQ_POINTS:
+		val = clamp(val, (u16)0, (u16)511);
+		st->freq_points = val;
+
+		dat = cpu_to_be16(val);
+		ret = ad5933_i2c_write(st->client, AD5933_REG_INC_NUM, 2,
+				       (u8 *)&dat);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&st->lock);
+	iio_device_release_direct_mode(indio_dev);
+	return ret ? ret : len;
+}
+
+static IIO_DEVICE_ATTR(out_altvoltage0_raw, 0644,
+			ad5933_show,
+			ad5933_store,
+			AD5933_OUT_RANGE);
+
+static IIO_DEVICE_ATTR(out_altvoltage0_scale_available, 0444,
+			ad5933_show,
+			NULL,
+			AD5933_OUT_RANGE_AVAIL);
+
+static IIO_DEVICE_ATTR(in_voltage0_scale, 0644,
+			ad5933_show,
+			ad5933_store,
+			AD5933_IN_PGA_GAIN);
+
+static IIO_DEVICE_ATTR(in_voltage0_scale_available, 0444,
+			ad5933_show,
+			NULL,
+			AD5933_IN_PGA_GAIN_AVAIL);
+
+static IIO_DEVICE_ATTR(out_altvoltage0_frequency_points, 0644,
+			ad5933_show,
+			ad5933_store,
+			AD5933_FREQ_POINTS);
+
+static IIO_DEVICE_ATTR(out_altvoltage0_settling_cycles, 0644,
+			ad5933_show,
+			ad5933_store,
+			AD5933_OUT_SETTLING_CYCLES);
+
+/*
+ * note:
+ * ideally we would handle the scale attributes via the iio_info
+ * (read|write)_raw methods, however this part is a untypical since we
+ * don't create dedicated sysfs channel attributes for out0 and in0.
+ */
+static struct attribute *ad5933_attributes[] = {
+	&iio_dev_attr_out_altvoltage0_raw.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_scale_available.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency_start.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency_increment.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency_points.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_settling_cycles.dev_attr.attr,
+	&iio_dev_attr_in_voltage0_scale.dev_attr.attr,
+	&iio_dev_attr_in_voltage0_scale_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group ad5933_attribute_group = {
+	.attrs = ad5933_attributes,
+};
+
+static int ad5933_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct ad5933_state *st = iio_priv(indio_dev);
+	__be16 dat;
+	int ret;
+
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+		ret = ad5933_cmd(st, AD5933_CTRL_MEASURE_TEMP);
+		if (ret < 0)
+			goto out;
+		ret = ad5933_wait_busy(st, AD5933_STAT_TEMP_VALID);
+		if (ret < 0)
+			goto out;
+
+		ret = ad5933_i2c_read(st->client,
+				      AD5933_REG_TEMP_DATA,
+				      2, (u8 *)&dat);
+		if (ret < 0)
+			goto out;
+		iio_device_release_direct_mode(indio_dev);
+		*val = sign_extend32(be16_to_cpu(dat), 13);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 1000;
+		*val2 = 5;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
+
+	return -EINVAL;
+out:
+	iio_device_release_direct_mode(indio_dev);
+	return ret;
+}
+
+static const struct iio_info ad5933_info = {
+	.read_raw = ad5933_read_raw,
+	.attrs = &ad5933_attribute_group,
+};
+
+static int ad5933_ring_preenable(struct iio_dev *indio_dev)
+{
+	struct ad5933_state *st = iio_priv(indio_dev);
+	int ret;
+
+	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
+		return -EINVAL;
+
+	ret = ad5933_reset(st);
+	if (ret < 0)
+		return ret;
+
+	ret = ad5933_cmd(st, AD5933_CTRL_STANDBY);
+	if (ret < 0)
+		return ret;
+
+	ret = ad5933_cmd(st, AD5933_CTRL_INIT_START_FREQ);
+	if (ret < 0)
+		return ret;
+
+	st->state = AD5933_CTRL_INIT_START_FREQ;
+
+	return 0;
+}
+
+static int ad5933_ring_postenable(struct iio_dev *indio_dev)
+{
+	struct ad5933_state *st = iio_priv(indio_dev);
+
+	/*
+	 * AD5933_CTRL_INIT_START_FREQ:
+	 * High Q complex circuits require a long time to reach steady state.
+	 * To facilitate the measurement of such impedances, this mode allows
+	 * the user full control of the settling time requirement before
+	 * entering start frequency sweep mode where the impedance measurement
+	 * takes place. In this mode the impedance is excited with the
+	 * programmed start frequency (ad5933_ring_preenable),
+	 * but no measurement takes place.
+	 */
+
+	schedule_delayed_work(&st->work,
+			      msecs_to_jiffies(AD5933_INIT_EXCITATION_TIME_ms));
+	return 0;
+}
+
+static int ad5933_ring_postdisable(struct iio_dev *indio_dev)
+{
+	struct ad5933_state *st = iio_priv(indio_dev);
+
+	cancel_delayed_work_sync(&st->work);
+	return ad5933_cmd(st, AD5933_CTRL_POWER_DOWN);
+}
+
+static const struct iio_buffer_setup_ops ad5933_ring_setup_ops = {
+	.preenable = ad5933_ring_preenable,
+	.postenable = ad5933_ring_postenable,
+	.postdisable = ad5933_ring_postdisable,
+};
+
+static void ad5933_work(struct work_struct *work)
+{
+	struct ad5933_state *st = container_of(work,
+		struct ad5933_state, work.work);
+	struct iio_dev *indio_dev = i2c_get_clientdata(st->client);
+	__be16 buf[2];
+	int val[2];
+	unsigned char status;
+	int ret;
+
+	if (st->state == AD5933_CTRL_INIT_START_FREQ) {
+		/* start sweep */
+		ad5933_cmd(st, AD5933_CTRL_START_SWEEP);
+		st->state = AD5933_CTRL_START_SWEEP;
+		schedule_delayed_work(&st->work, st->poll_time_jiffies);
+		return;
+	}
+
+	ret = ad5933_i2c_read(st->client, AD5933_REG_STATUS, 1, &status);
+	if (ret)
+		return;
+
+	if (status & AD5933_STAT_DATA_VALID) {
+		int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+					       indio_dev->masklength);
+		ret = ad5933_i2c_read(st->client,
+				test_bit(1, indio_dev->active_scan_mask) ?
+				AD5933_REG_REAL_DATA : AD5933_REG_IMAG_DATA,
+				scan_count * 2, (u8 *)buf);
+		if (ret)
+			return;
+
+		if (scan_count == 2) {
+			val[0] = be16_to_cpu(buf[0]);
+			val[1] = be16_to_cpu(buf[1]);
+		} else {
+			val[0] = be16_to_cpu(buf[0]);
+		}
+		iio_push_to_buffers(indio_dev, val);
+	} else {
+		/* no data available - try again later */
+		schedule_delayed_work(&st->work, st->poll_time_jiffies);
+		return;
+	}
+
+	if (status & AD5933_STAT_SWEEP_DONE) {
+		/*
+		 * last sample received - power down do
+		 * nothing until the ring enable is toggled
+		 */
+		ad5933_cmd(st, AD5933_CTRL_POWER_DOWN);
+	} else {
+		/* we just received a valid datum, move on to the next */
+		ad5933_cmd(st, AD5933_CTRL_INC_FREQ);
+		schedule_delayed_work(&st->work, st->poll_time_jiffies);
+	}
+}
+
+static void ad5933_reg_disable(void *data)
+{
+	struct ad5933_state *st = data;
+
+	regulator_disable(st->reg);
+}
+
+static void ad5933_clk_disable(void *data)
+{
+	struct ad5933_state *st = data;
+
+	clk_disable_unprepare(st->mclk);
+}
+
+static int ad5933_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct ad5933_state *st;
+	struct iio_dev *indio_dev;
+	unsigned long ext_clk_hz = 0;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	st->client = client;
+
+	mutex_init(&st->lock);
+
+	st->reg = devm_regulator_get(&client->dev, "vdd");
+	if (IS_ERR(st->reg))
+		return PTR_ERR(st->reg);
+
+	ret = regulator_enable(st->reg);
+	if (ret) {
+		dev_err(&client->dev, "Failed to enable specified VDD supply\n");
+		return ret;
+	}
+
+	ret = devm_add_action_or_reset(&client->dev, ad5933_reg_disable, st);
+	if (ret)
+		return ret;
+
+	ret = regulator_get_voltage(st->reg);
+	if (ret < 0)
+		return ret;
+
+	st->vref_mv = ret / 1000;
+
+	st->mclk = devm_clk_get(&client->dev, "mclk");
+	if (IS_ERR(st->mclk) && PTR_ERR(st->mclk) != -ENOENT)
+		return PTR_ERR(st->mclk);
+
+	if (!IS_ERR(st->mclk)) {
+		ret = clk_prepare_enable(st->mclk);
+		if (ret < 0)
+			return ret;
+
+		ret = devm_add_action_or_reset(&client->dev,
+					       ad5933_clk_disable,
+					       st);
+		if (ret)
+			return ret;
+
+		ext_clk_hz = clk_get_rate(st->mclk);
+	}
+
+	if (ext_clk_hz) {
+		st->mclk_hz = ext_clk_hz;
+		st->ctrl_lb = AD5933_CTRL_EXT_SYSCLK;
+	} else {
+		st->mclk_hz = AD5933_INT_OSC_FREQ_Hz;
+		st->ctrl_lb = AD5933_CTRL_INT_SYSCLK;
+	}
+
+	ad5933_calc_out_ranges(st);
+	INIT_DELAYED_WORK(&st->work, ad5933_work);
+	st->poll_time_jiffies = msecs_to_jiffies(AD5933_POLL_TIME_ms);
+
+	indio_dev->info = &ad5933_info;
+	indio_dev->name = id->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ad5933_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ad5933_channels);
+
+	ret = devm_iio_kfifo_buffer_setup(&client->dev, indio_dev,
+					  &ad5933_ring_setup_ops);
+	if (ret)
+		return ret;
+
+	ret = ad5933_setup(st);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id ad5933_id[] = {
+	{ "ad5933", 0 },
+	{ "ad5934", 0 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ad5933_id);
+
+static const struct of_device_id ad5933_of_match[] = {
+	{ .compatible = "adi,ad5933" },
+	{ .compatible = "adi,ad5934" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, ad5933_of_match);
+
+static struct i2c_driver ad5933_driver = {
+	.driver = {
+		.name = "ad5933",
+		.of_match_table = ad5933_of_match,
+	},
+	.probe = ad5933_probe,
+	.id_table = ad5933_id,
+};
+module_i2c_driver(ad5933_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Analog Devices AD5933 Impedance Conv. Network Analyzer");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/meter/Kconfig b/drivers/staging/iio/meter/Kconfig
new file mode 100644
index 000000000..aa6a3e7f6
--- /dev/null
+++ b/drivers/staging/iio/meter/Kconfig
@@ -0,0 +1,37 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# IIO meter drivers configuration
+#
+menu "Active energy metering IC"
+
+config ADE7854
+	tristate "Analog Devices ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC Driver"
+	depends on SPI || I2C
+	help
+	  Say yes here to build support for Analog Devices ADE7854/58/68/78 Polyphase
+	  Multifunction Energy Metering IC Driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7854.
+
+config ADE7854_I2C
+	tristate "support I2C bus connection"
+	depends on ADE7854 && I2C
+	default y
+	help
+	  Say Y here if you have ADE7854/58/68/78 hooked to an I2C bus.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7854-i2c.
+
+config ADE7854_SPI
+	tristate "support SPI bus connection"
+	depends on ADE7854 && SPI
+	default y
+	help
+	  Say Y here if you have ADE7854/58/68/78 hooked to a SPI bus.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7854-spi.
+
+endmenu
diff --git a/drivers/staging/iio/meter/Makefile b/drivers/staging/iio/meter/Makefile
new file mode 100644
index 000000000..ed4547e38
--- /dev/null
+++ b/drivers/staging/iio/meter/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for metering ic drivers
+#
+
+obj-$(CONFIG_ADE7854) += ade7854.o
+obj-$(CONFIG_ADE7854_I2C) += ade7854-i2c.o
+obj-$(CONFIG_ADE7854_SPI) += ade7854-spi.o
diff --git a/drivers/staging/iio/meter/ade7854-i2c.c b/drivers/staging/iio/meter/ade7854-i2c.c
new file mode 100644
index 000000000..a9a06e8dd
--- /dev/null
+++ b/drivers/staging/iio/meter/ade7854-i2c.c
@@ -0,0 +1,151 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC Driver (I2C Bus)
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include "ade7854.h"
+
+static int ade7854_i2c_write_reg(struct device *dev,
+				 u16 reg_address,
+				 u32 val,
+				 int bits)
+{
+	int ret;
+	int count;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	mutex_lock(&st->buf_lock);
+	st->tx[0] = (reg_address >> 8) & 0xFF;
+	st->tx[1] = reg_address & 0xFF;
+
+	switch (bits) {
+	case 8:
+		st->tx[2] = val & 0xFF;
+		count = 3;
+		break;
+	case 16:
+		st->tx[2] = (val >> 8) & 0xFF;
+		st->tx[3] = val & 0xFF;
+		count = 4;
+		break;
+	case 24:
+		st->tx[2] = (val >> 16) & 0xFF;
+		st->tx[3] = (val >> 8) & 0xFF;
+		st->tx[4] = val & 0xFF;
+		count = 5;
+		break;
+	case 32:
+		st->tx[2] = (val >> 24) & 0xFF;
+		st->tx[3] = (val >> 16) & 0xFF;
+		st->tx[4] = (val >> 8) & 0xFF;
+		st->tx[5] = val & 0xFF;
+		count = 6;
+		break;
+	default:
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	ret = i2c_master_send(st->i2c, st->tx, count);
+
+unlock:
+	mutex_unlock(&st->buf_lock);
+
+	return ret < 0 ? ret : 0;
+}
+
+static int ade7854_i2c_read_reg(struct device *dev,
+				u16 reg_address,
+				u32 *val,
+				int bits)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&st->buf_lock);
+	st->tx[0] = (reg_address >> 8) & 0xFF;
+	st->tx[1] = reg_address & 0xFF;
+
+	ret = i2c_master_send(st->i2c, st->tx, 2);
+	if (ret < 0)
+		goto unlock;
+
+	ret = i2c_master_recv(st->i2c, st->rx, bits);
+	if (ret < 0)
+		goto unlock;
+
+	switch (bits) {
+	case 8:
+		*val = st->rx[0];
+		break;
+	case 16:
+		*val = (st->rx[0] << 8) | st->rx[1];
+		break;
+	case 24:
+		*val = (st->rx[0] << 16) | (st->rx[1] << 8) | st->rx[2];
+		break;
+	case 32:
+		*val = (st->rx[0] << 24) | (st->rx[1] << 16) |
+			(st->rx[2] << 8) | st->rx[3];
+		break;
+	default:
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+unlock:
+	mutex_unlock(&st->buf_lock);
+	return ret;
+}
+
+static int ade7854_i2c_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct ade7854_state *st;
+	struct iio_dev *indio_dev;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+	st = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	st->read_reg = ade7854_i2c_read_reg;
+	st->write_reg = ade7854_i2c_write_reg;
+	st->i2c = client;
+	st->irq = client->irq;
+
+	return ade7854_probe(indio_dev, &client->dev);
+}
+
+static const struct i2c_device_id ade7854_id[] = {
+	{ "ade7854", 0 },
+	{ "ade7858", 0 },
+	{ "ade7868", 0 },
+	{ "ade7878", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ade7854_id);
+
+static struct i2c_driver ade7854_i2c_driver = {
+	.driver = {
+		.name = "ade7854",
+	},
+	.probe    = ade7854_i2c_probe,
+	.id_table = ade7854_id,
+};
+module_i2c_driver(ade7854_i2c_driver);
+
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC I2C Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/meter/ade7854-spi.c b/drivers/staging/iio/meter/ade7854-spi.c
new file mode 100644
index 000000000..f12a6c8b3
--- /dev/null
+++ b/drivers/staging/iio/meter/ade7854-spi.c
@@ -0,0 +1,160 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC Driver (SPI Bus)
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include "ade7854.h"
+
+static int ade7854_spi_write_reg(struct device *dev,
+				 u16 reg_address,
+				 u32 val,
+				 int bits)
+{
+	int ret;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	struct spi_transfer xfer = {
+		.tx_buf = st->tx,
+		.bits_per_word = 8,
+		.len = 4,
+	};
+
+	mutex_lock(&st->buf_lock);
+	st->tx[0] = ADE7854_WRITE_REG;
+	st->tx[1] = (reg_address >> 8) & 0xFF;
+	st->tx[2] = reg_address & 0xFF;
+	switch (bits) {
+	case 8:
+		st->tx[3] = val & 0xFF;
+		break;
+	case 16:
+		xfer.len = 5;
+		st->tx[3] = (val >> 8) & 0xFF;
+		st->tx[4] = val & 0xFF;
+		break;
+	case 24:
+		xfer.len = 6;
+		st->tx[3] = (val >> 16) & 0xFF;
+		st->tx[4] = (val >> 8) & 0xFF;
+		st->tx[5] = val & 0xFF;
+		break;
+	case 32:
+		xfer.len = 7;
+		st->tx[3] = (val >> 24) & 0xFF;
+		st->tx[4] = (val >> 16) & 0xFF;
+		st->tx[5] = (val >> 8) & 0xFF;
+		st->tx[6] = val & 0xFF;
+		break;
+	default:
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	ret = spi_sync_transfer(st->spi, &xfer, 1);
+unlock:
+	mutex_unlock(&st->buf_lock);
+
+	return ret;
+}
+
+static int ade7854_spi_read_reg(struct device *dev,
+				u16 reg_address,
+				u32 *val,
+				int bits)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	int ret;
+	struct spi_transfer xfers[] = {
+		{
+			.tx_buf = st->tx,
+			.bits_per_word = 8,
+			.len = 3,
+		}, {
+			.rx_buf = st->rx,
+			.bits_per_word = 8,
+			.len = bits,
+		}
+	};
+
+	mutex_lock(&st->buf_lock);
+
+	st->tx[0] = ADE7854_READ_REG;
+	st->tx[1] = (reg_address >> 8) & 0xFF;
+	st->tx[2] = reg_address & 0xFF;
+
+	ret = spi_sync_transfer(st->spi, xfers, ARRAY_SIZE(xfers));
+	if (ret < 0) {
+		dev_err(&st->spi->dev, "problem when reading register 0x%02X",
+			reg_address);
+		goto unlock;
+	}
+
+	switch (bits) {
+	case 8:
+		*val = st->rx[0];
+		break;
+	case 16:
+		*val = be16_to_cpup((const __be16 *)st->rx);
+		break;
+	case 24:
+		*val = (st->rx[0] << 16) | (st->rx[1] << 8) | st->rx[2];
+		break;
+	case 32:
+		*val = be32_to_cpup((const __be32 *)st->rx);
+		break;
+	}
+
+unlock:
+	mutex_unlock(&st->buf_lock);
+	return ret;
+}
+
+static int ade7854_spi_probe(struct spi_device *spi)
+{
+	struct ade7854_state *st;
+	struct iio_dev *indio_dev;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+	st = iio_priv(indio_dev);
+	spi_set_drvdata(spi, indio_dev);
+	st->read_reg = ade7854_spi_read_reg;
+	st->write_reg = ade7854_spi_write_reg;
+	st->irq = spi->irq;
+	st->spi = spi;
+
+	return ade7854_probe(indio_dev, &spi->dev);
+}
+
+static const struct spi_device_id ade7854_id[] = {
+	{ "ade7854", 0 },
+	{ "ade7858", 0 },
+	{ "ade7868", 0 },
+	{ "ade7878", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ade7854_id);
+
+static struct spi_driver ade7854_driver = {
+	.driver = {
+		.name = "ade7854",
+	},
+	.probe = ade7854_spi_probe,
+	.id_table = ade7854_id,
+};
+module_spi_driver(ade7854_driver);
+
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices ADE7854/58/68/78 SPI Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/meter/ade7854.c b/drivers/staging/iio/meter/ade7854.c
new file mode 100644
index 000000000..68da6ecde
--- /dev/null
+++ b/drivers/staging/iio/meter/ade7854.c
@@ -0,0 +1,556 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC Driver
+ *
+ * Copyright 2010 Analog Devices Inc.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/list.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include "meter.h"
+#include "ade7854.h"
+
+static ssize_t ade7854_read_8bit(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	int ret;
+	u32 val = 0;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+
+	ret = st->read_reg(dev, this_attr->address, &val, 8);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%u\n", val);
+}
+
+static ssize_t ade7854_read_16bit(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	int ret;
+	u32 val = 0;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+
+	ret = st->read_reg(dev, this_attr->address, &val, 16);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%u\n", val);
+}
+
+static ssize_t ade7854_read_24bit(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	int ret;
+	u32 val;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+
+	ret = st->read_reg(dev, this_attr->address, &val, 24);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%u\n", val);
+}
+
+static ssize_t ade7854_read_32bit(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	int ret;
+	u32 val = 0;
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	ret = st->read_reg(dev, this_attr->address, &val, 32);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%u\n", val);
+}
+
+static ssize_t ade7854_write_8bit(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf,
+				  size_t len)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	int ret;
+	u8 val;
+
+	ret = kstrtou8(buf, 10, &val);
+	if (ret)
+		goto error_ret;
+	ret = st->write_reg(dev, this_attr->address, val, 8);
+
+error_ret:
+	return ret ? ret : len;
+}
+
+static ssize_t ade7854_write_16bit(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	int ret;
+	u16 val;
+
+	ret = kstrtou16(buf, 10, &val);
+	if (ret)
+		goto error_ret;
+	ret = st->write_reg(dev, this_attr->address, val, 16);
+
+error_ret:
+	return ret ? ret : len;
+}
+
+static ssize_t ade7854_write_24bit(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	int ret;
+	u32 val;
+
+	ret = kstrtou32(buf, 10, &val);
+	if (ret)
+		goto error_ret;
+	ret = st->write_reg(dev, this_attr->address, val, 24);
+
+error_ret:
+	return ret ? ret : len;
+}
+
+static ssize_t ade7854_write_32bit(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
+{
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	int ret;
+	u32 val;
+
+	ret = kstrtou32(buf, 10, &val);
+	if (ret)
+		goto error_ret;
+	ret = st->write_reg(dev, this_attr->address, val, 32);
+
+error_ret:
+	return ret ? ret : len;
+}
+
+static int ade7854_reset(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+	u32 val;
+
+	st->read_reg(dev, ADE7854_CONFIG, &val, 16);
+	val |= BIT(7); /* Software Chip Reset */
+
+	return st->write_reg(dev, ADE7854_CONFIG, val, 16);
+}
+
+static IIO_DEV_ATTR_AIGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AIGAIN);
+static IIO_DEV_ATTR_BIGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BIGAIN);
+static IIO_DEV_ATTR_CIGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CIGAIN);
+static IIO_DEV_ATTR_NIGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_NIGAIN);
+static IIO_DEV_ATTR_AVGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AVGAIN);
+static IIO_DEV_ATTR_BVGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BVGAIN);
+static IIO_DEV_ATTR_CVGAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CVGAIN);
+static IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AVAGAIN);
+static IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BVAGAIN);
+static IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CVAGAIN);
+static IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AWATTOS);
+static IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BWATTOS);
+static IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CWATTOS);
+static IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AVARGAIN);
+static IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BVARGAIN);
+static IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CVARGAIN);
+static IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_AVAROS);
+static IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_BVAROS);
+static IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(0644,
+		ade7854_read_24bit,
+		ade7854_write_24bit,
+		ADE7854_CVAROS);
+static IIO_DEV_ATTR_VPEAK(0644,
+		ade7854_read_32bit,
+		ade7854_write_32bit,
+		ADE7854_VPEAK);
+static IIO_DEV_ATTR_IPEAK(0644,
+		ade7854_read_32bit,
+		ade7854_write_32bit,
+		ADE7854_IPEAK);
+static IIO_DEV_ATTR_APHCAL(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_APHCAL);
+static IIO_DEV_ATTR_BPHCAL(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_BPHCAL);
+static IIO_DEV_ATTR_CPHCAL(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CPHCAL);
+static IIO_DEV_ATTR_CF1DEN(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CF1DEN);
+static IIO_DEV_ATTR_CF2DEN(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CF2DEN);
+static IIO_DEV_ATTR_CF3DEN(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CF3DEN);
+static IIO_DEV_ATTR_LINECYC(0644,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_LINECYC);
+static IIO_DEV_ATTR_SAGCYC(0644,
+		ade7854_read_8bit,
+		ade7854_write_8bit,
+		ADE7854_SAGCYC);
+static IIO_DEV_ATTR_CFCYC(0644,
+		ade7854_read_8bit,
+		ade7854_write_8bit,
+		ADE7854_CFCYC);
+static IIO_DEV_ATTR_PEAKCYC(0644,
+		ade7854_read_8bit,
+		ade7854_write_8bit,
+		ADE7854_PEAKCYC);
+static IIO_DEV_ATTR_CHKSUM(ade7854_read_24bit,
+		ADE7854_CHECKSUM);
+static IIO_DEV_ATTR_ANGLE0(ade7854_read_24bit,
+		ADE7854_ANGLE0);
+static IIO_DEV_ATTR_ANGLE1(ade7854_read_24bit,
+		ADE7854_ANGLE1);
+static IIO_DEV_ATTR_ANGLE2(ade7854_read_24bit,
+		ADE7854_ANGLE2);
+static IIO_DEV_ATTR_AIRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_AIRMS);
+static IIO_DEV_ATTR_BIRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_BIRMS);
+static IIO_DEV_ATTR_CIRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_CIRMS);
+static IIO_DEV_ATTR_NIRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_NIRMS);
+static IIO_DEV_ATTR_AVRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_AVRMS);
+static IIO_DEV_ATTR_BVRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_BVRMS);
+static IIO_DEV_ATTR_CVRMS(0444,
+		ade7854_read_24bit,
+		NULL,
+		ADE7854_CVRMS);
+static IIO_DEV_ATTR_AIRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_AIRMSOS);
+static IIO_DEV_ATTR_BIRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_BIRMSOS);
+static IIO_DEV_ATTR_CIRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CIRMSOS);
+static IIO_DEV_ATTR_AVRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_AVRMSOS);
+static IIO_DEV_ATTR_BVRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_BVRMSOS);
+static IIO_DEV_ATTR_CVRMSOS(0444,
+		ade7854_read_16bit,
+		ade7854_write_16bit,
+		ADE7854_CVRMSOS);
+static IIO_DEV_ATTR_VOLT_A(ade7854_read_24bit,
+		ADE7854_VAWV);
+static IIO_DEV_ATTR_VOLT_B(ade7854_read_24bit,
+		ADE7854_VBWV);
+static IIO_DEV_ATTR_VOLT_C(ade7854_read_24bit,
+		ADE7854_VCWV);
+static IIO_DEV_ATTR_CURRENT_A(ade7854_read_24bit,
+		ADE7854_IAWV);
+static IIO_DEV_ATTR_CURRENT_B(ade7854_read_24bit,
+		ADE7854_IBWV);
+static IIO_DEV_ATTR_CURRENT_C(ade7854_read_24bit,
+		ADE7854_ICWV);
+static IIO_DEV_ATTR_AWATTHR(ade7854_read_32bit,
+		ADE7854_AWATTHR);
+static IIO_DEV_ATTR_BWATTHR(ade7854_read_32bit,
+		ADE7854_BWATTHR);
+static IIO_DEV_ATTR_CWATTHR(ade7854_read_32bit,
+		ADE7854_CWATTHR);
+static IIO_DEV_ATTR_AFWATTHR(ade7854_read_32bit,
+		ADE7854_AFWATTHR);
+static IIO_DEV_ATTR_BFWATTHR(ade7854_read_32bit,
+		ADE7854_BFWATTHR);
+static IIO_DEV_ATTR_CFWATTHR(ade7854_read_32bit,
+		ADE7854_CFWATTHR);
+static IIO_DEV_ATTR_AVARHR(ade7854_read_32bit,
+		ADE7854_AVARHR);
+static IIO_DEV_ATTR_BVARHR(ade7854_read_32bit,
+		ADE7854_BVARHR);
+static IIO_DEV_ATTR_CVARHR(ade7854_read_32bit,
+		ADE7854_CVARHR);
+static IIO_DEV_ATTR_AVAHR(ade7854_read_32bit,
+		ADE7854_AVAHR);
+static IIO_DEV_ATTR_BVAHR(ade7854_read_32bit,
+		ADE7854_BVAHR);
+static IIO_DEV_ATTR_CVAHR(ade7854_read_32bit,
+		ADE7854_CVAHR);
+
+static int ade7854_set_irq(struct device *dev, bool enable)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7854_state *st = iio_priv(indio_dev);
+
+	int ret;
+	u32 irqen;
+
+	ret = st->read_reg(dev, ADE7854_MASK0, &irqen, 32);
+	if (ret < 0)
+		return ret;
+
+	if (enable)
+		irqen |= BIT(17); /* 1: interrupt enabled when all periodical
+				   * (at 8 kHz rate) DSP computations finish.
+				   */
+	else
+		irqen &= ~BIT(17);
+
+	return st->write_reg(dev, ADE7854_MASK0, irqen, 32);
+}
+
+static int ade7854_initial_setup(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct device *dev = &indio_dev->dev;
+
+	/* Disable IRQ */
+	ret = ade7854_set_irq(dev, false);
+	if (ret) {
+		dev_err(dev, "disable irq failed");
+		goto err_ret;
+	}
+
+	ade7854_reset(dev);
+	usleep_range(ADE7854_STARTUP_DELAY, ADE7854_STARTUP_DELAY + 100);
+
+err_ret:
+	return ret;
+}
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("8000");
+
+static IIO_CONST_ATTR(name, "ade7854");
+
+static struct attribute *ade7854_attributes[] = {
+	&iio_dev_attr_aigain.dev_attr.attr,
+	&iio_dev_attr_bigain.dev_attr.attr,
+	&iio_dev_attr_cigain.dev_attr.attr,
+	&iio_dev_attr_nigain.dev_attr.attr,
+	&iio_dev_attr_avgain.dev_attr.attr,
+	&iio_dev_attr_bvgain.dev_attr.attr,
+	&iio_dev_attr_cvgain.dev_attr.attr,
+	&iio_dev_attr_linecyc.dev_attr.attr,
+	&iio_dev_attr_sagcyc.dev_attr.attr,
+	&iio_dev_attr_cfcyc.dev_attr.attr,
+	&iio_dev_attr_peakcyc.dev_attr.attr,
+	&iio_dev_attr_chksum.dev_attr.attr,
+	&iio_dev_attr_apparent_power_a_gain.dev_attr.attr,
+	&iio_dev_attr_apparent_power_b_gain.dev_attr.attr,
+	&iio_dev_attr_apparent_power_c_gain.dev_attr.attr,
+	&iio_dev_attr_active_power_a_offset.dev_attr.attr,
+	&iio_dev_attr_active_power_b_offset.dev_attr.attr,
+	&iio_dev_attr_active_power_c_offset.dev_attr.attr,
+	&iio_dev_attr_reactive_power_a_gain.dev_attr.attr,
+	&iio_dev_attr_reactive_power_b_gain.dev_attr.attr,
+	&iio_dev_attr_reactive_power_c_gain.dev_attr.attr,
+	&iio_dev_attr_reactive_power_a_offset.dev_attr.attr,
+	&iio_dev_attr_reactive_power_b_offset.dev_attr.attr,
+	&iio_dev_attr_reactive_power_c_offset.dev_attr.attr,
+	&iio_dev_attr_awatthr.dev_attr.attr,
+	&iio_dev_attr_bwatthr.dev_attr.attr,
+	&iio_dev_attr_cwatthr.dev_attr.attr,
+	&iio_dev_attr_afwatthr.dev_attr.attr,
+	&iio_dev_attr_bfwatthr.dev_attr.attr,
+	&iio_dev_attr_cfwatthr.dev_attr.attr,
+	&iio_dev_attr_avarhr.dev_attr.attr,
+	&iio_dev_attr_bvarhr.dev_attr.attr,
+	&iio_dev_attr_cvarhr.dev_attr.attr,
+	&iio_dev_attr_angle0.dev_attr.attr,
+	&iio_dev_attr_angle1.dev_attr.attr,
+	&iio_dev_attr_angle2.dev_attr.attr,
+	&iio_dev_attr_avahr.dev_attr.attr,
+	&iio_dev_attr_bvahr.dev_attr.attr,
+	&iio_dev_attr_cvahr.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_const_attr_name.dev_attr.attr,
+	&iio_dev_attr_vpeak.dev_attr.attr,
+	&iio_dev_attr_ipeak.dev_attr.attr,
+	&iio_dev_attr_aphcal.dev_attr.attr,
+	&iio_dev_attr_bphcal.dev_attr.attr,
+	&iio_dev_attr_cphcal.dev_attr.attr,
+	&iio_dev_attr_cf1den.dev_attr.attr,
+	&iio_dev_attr_cf2den.dev_attr.attr,
+	&iio_dev_attr_cf3den.dev_attr.attr,
+	&iio_dev_attr_airms.dev_attr.attr,
+	&iio_dev_attr_birms.dev_attr.attr,
+	&iio_dev_attr_cirms.dev_attr.attr,
+	&iio_dev_attr_nirms.dev_attr.attr,
+	&iio_dev_attr_avrms.dev_attr.attr,
+	&iio_dev_attr_bvrms.dev_attr.attr,
+	&iio_dev_attr_cvrms.dev_attr.attr,
+	&iio_dev_attr_airmsos.dev_attr.attr,
+	&iio_dev_attr_birmsos.dev_attr.attr,
+	&iio_dev_attr_cirmsos.dev_attr.attr,
+	&iio_dev_attr_avrmsos.dev_attr.attr,
+	&iio_dev_attr_bvrmsos.dev_attr.attr,
+	&iio_dev_attr_cvrmsos.dev_attr.attr,
+	&iio_dev_attr_volt_a.dev_attr.attr,
+	&iio_dev_attr_volt_b.dev_attr.attr,
+	&iio_dev_attr_volt_c.dev_attr.attr,
+	&iio_dev_attr_current_a.dev_attr.attr,
+	&iio_dev_attr_current_b.dev_attr.attr,
+	&iio_dev_attr_current_c.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ade7854_attribute_group = {
+	.attrs = ade7854_attributes,
+};
+
+static const struct iio_info ade7854_info = {
+	.attrs = &ade7854_attribute_group,
+};
+
+int ade7854_probe(struct iio_dev *indio_dev, struct device *dev)
+{
+	int ret;
+	struct ade7854_state *st = iio_priv(indio_dev);
+	/* setup the industrialio driver allocated elements */
+	mutex_init(&st->buf_lock);
+
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &ade7854_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = devm_iio_device_register(dev, indio_dev);
+	if (ret)
+		return ret;
+
+	/* Get the device into a sane initial state */
+	return ade7854_initial_setup(indio_dev);
+}
+EXPORT_SYMBOL(ade7854_probe);
+
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices ADE7854/58/68/78 Polyphase Energy Meter");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/meter/ade7854.h b/drivers/staging/iio/meter/ade7854.h
new file mode 100644
index 000000000..7a49f8f10
--- /dev/null
+++ b/drivers/staging/iio/meter/ade7854.h
@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _ADE7854_H
+#define _ADE7854_H
+
+#define ADE7854_AIGAIN    0x4380
+#define ADE7854_AVGAIN    0x4381
+#define ADE7854_BIGAIN    0x4382
+#define ADE7854_BVGAIN    0x4383
+#define ADE7854_CIGAIN    0x4384
+#define ADE7854_CVGAIN    0x4385
+#define ADE7854_NIGAIN    0x4386
+#define ADE7854_AIRMSOS   0x4387
+#define ADE7854_AVRMSOS   0x4388
+#define ADE7854_BIRMSOS   0x4389
+#define ADE7854_BVRMSOS   0x438A
+#define ADE7854_CIRMSOS   0x438B
+#define ADE7854_CVRMSOS   0x438C
+#define ADE7854_NIRMSOS   0x438D
+#define ADE7854_AVAGAIN   0x438E
+#define ADE7854_BVAGAIN   0x438F
+#define ADE7854_CVAGAIN   0x4390
+#define ADE7854_AWGAIN    0x4391
+#define ADE7854_AWATTOS   0x4392
+#define ADE7854_BWGAIN    0x4393
+#define ADE7854_BWATTOS   0x4394
+#define ADE7854_CWGAIN    0x4395
+#define ADE7854_CWATTOS   0x4396
+#define ADE7854_AVARGAIN  0x4397
+#define ADE7854_AVAROS    0x4398
+#define ADE7854_BVARGAIN  0x4399
+#define ADE7854_BVAROS    0x439A
+#define ADE7854_CVARGAIN  0x439B
+#define ADE7854_CVAROS    0x439C
+#define ADE7854_AFWGAIN   0x439D
+#define ADE7854_AFWATTOS  0x439E
+#define ADE7854_BFWGAIN   0x439F
+#define ADE7854_BFWATTOS  0x43A0
+#define ADE7854_CFWGAIN   0x43A1
+#define ADE7854_CFWATTOS  0x43A2
+#define ADE7854_AFVARGAIN 0x43A3
+#define ADE7854_AFVAROS   0x43A4
+#define ADE7854_BFVARGAIN 0x43A5
+#define ADE7854_BFVAROS   0x43A6
+#define ADE7854_CFVARGAIN 0x43A7
+#define ADE7854_CFVAROS   0x43A8
+#define ADE7854_VATHR1    0x43A9
+#define ADE7854_VATHR0    0x43AA
+#define ADE7854_WTHR1     0x43AB
+#define ADE7854_WTHR0     0x43AC
+#define ADE7854_VARTHR1   0x43AD
+#define ADE7854_VARTHR0   0x43AE
+#define ADE7854_RSV       0x43AF
+#define ADE7854_VANOLOAD  0x43B0
+#define ADE7854_APNOLOAD  0x43B1
+#define ADE7854_VARNOLOAD 0x43B2
+#define ADE7854_VLEVEL    0x43B3
+#define ADE7854_DICOEFF   0x43B5
+#define ADE7854_HPFDIS    0x43B6
+#define ADE7854_ISUMLVL   0x43B8
+#define ADE7854_ISUM      0x43BF
+#define ADE7854_AIRMS     0x43C0
+#define ADE7854_AVRMS     0x43C1
+#define ADE7854_BIRMS     0x43C2
+#define ADE7854_BVRMS     0x43C3
+#define ADE7854_CIRMS     0x43C4
+#define ADE7854_CVRMS     0x43C5
+#define ADE7854_NIRMS     0x43C6
+#define ADE7854_RUN       0xE228
+#define ADE7854_AWATTHR   0xE400
+#define ADE7854_BWATTHR   0xE401
+#define ADE7854_CWATTHR   0xE402
+#define ADE7854_AFWATTHR  0xE403
+#define ADE7854_BFWATTHR  0xE404
+#define ADE7854_CFWATTHR  0xE405
+#define ADE7854_AVARHR    0xE406
+#define ADE7854_BVARHR    0xE407
+#define ADE7854_CVARHR    0xE408
+#define ADE7854_AFVARHR   0xE409
+#define ADE7854_BFVARHR   0xE40A
+#define ADE7854_CFVARHR   0xE40B
+#define ADE7854_AVAHR     0xE40C
+#define ADE7854_BVAHR     0xE40D
+#define ADE7854_CVAHR     0xE40E
+#define ADE7854_IPEAK     0xE500
+#define ADE7854_VPEAK     0xE501
+#define ADE7854_STATUS0   0xE502
+#define ADE7854_STATUS1   0xE503
+#define ADE7854_OILVL     0xE507
+#define ADE7854_OVLVL     0xE508
+#define ADE7854_SAGLVL    0xE509
+#define ADE7854_MASK0     0xE50A
+#define ADE7854_MASK1     0xE50B
+#define ADE7854_IAWV      0xE50C
+#define ADE7854_IBWV      0xE50D
+#define ADE7854_ICWV      0xE50E
+#define ADE7854_VAWV      0xE510
+#define ADE7854_VBWV      0xE511
+#define ADE7854_VCWV      0xE512
+#define ADE7854_AWATT     0xE513
+#define ADE7854_BWATT     0xE514
+#define ADE7854_CWATT     0xE515
+#define ADE7854_AVA       0xE519
+#define ADE7854_BVA       0xE51A
+#define ADE7854_CVA       0xE51B
+#define ADE7854_CHECKSUM  0xE51F
+#define ADE7854_VNOM      0xE520
+#define ADE7854_PHSTATUS  0xE600
+#define ADE7854_ANGLE0    0xE601
+#define ADE7854_ANGLE1    0xE602
+#define ADE7854_ANGLE2    0xE603
+#define ADE7854_PERIOD    0xE607
+#define ADE7854_PHNOLOAD  0xE608
+#define ADE7854_LINECYC   0xE60C
+#define ADE7854_ZXTOUT    0xE60D
+#define ADE7854_COMPMODE  0xE60E
+#define ADE7854_GAIN      0xE60F
+#define ADE7854_CFMODE    0xE610
+#define ADE7854_CF1DEN    0xE611
+#define ADE7854_CF2DEN    0xE612
+#define ADE7854_CF3DEN    0xE613
+#define ADE7854_APHCAL    0xE614
+#define ADE7854_BPHCAL    0xE615
+#define ADE7854_CPHCAL    0xE616
+#define ADE7854_PHSIGN    0xE617
+#define ADE7854_CONFIG    0xE618
+#define ADE7854_MMODE     0xE700
+#define ADE7854_ACCMODE   0xE701
+#define ADE7854_LCYCMODE  0xE702
+#define ADE7854_PEAKCYC   0xE703
+#define ADE7854_SAGCYC    0xE704
+#define ADE7854_CFCYC     0xE705
+#define ADE7854_HSDC_CFG  0xE706
+#define ADE7854_CONFIG2   0xEC01
+
+#define ADE7854_READ_REG   0x1
+#define ADE7854_WRITE_REG  0x0
+
+#define ADE7854_MAX_TX    7
+#define ADE7854_MAX_RX    7
+#define ADE7854_STARTUP_DELAY 1000
+
+#define ADE7854_SPI_SLOW	(u32)(300 * 1000)
+#define ADE7854_SPI_BURST	(u32)(1000 * 1000)
+#define ADE7854_SPI_FAST	(u32)(2000 * 1000)
+
+/**
+ * struct ade7854_state - device instance specific data
+ * @spi:		actual spi_device
+ * @read_reg		Wrapper function for I2C and SPI read
+ * @write_reg		Wrapper function for I2C and SPI write
+ * @indio_dev:		industrial I/O device structure
+ * @buf_lock:		mutex to protect tx and rx
+ * @tx:			transmit buffer
+ * @rx:			receive buffer
+ **/
+struct ade7854_state {
+	struct spi_device *spi;
+	struct i2c_client *i2c;
+	int (*read_reg)(struct device *dev, u16 reg_address, u32 *val,
+			int bits);
+	int (*write_reg)(struct device *dev, u16 reg_address, u32 val,
+			 int bits);
+	int irq;
+	struct mutex buf_lock;
+	u8 tx[ADE7854_MAX_TX] __aligned(IIO_DMA_MINALIGN);
+	u8 rx[ADE7854_MAX_RX];
+
+};
+
+int ade7854_probe(struct iio_dev *indio_dev, struct device *dev);
+int ade7854_remove(struct iio_dev *indio_dev);
+
+#endif
diff --git a/drivers/staging/iio/meter/meter.h b/drivers/staging/iio/meter/meter.h
new file mode 100644
index 000000000..5ed59bf30
--- /dev/null
+++ b/drivers/staging/iio/meter/meter.h
@@ -0,0 +1,398 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _METER_H
+#define _METER_H
+
+#include <linux/iio/sysfs.h>
+
+/* metering ic types of attribute */
+
+#define IIO_DEV_ATTR_CURRENT_A_OFFSET(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_a_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_B_OFFSET(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_b_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_C_OFFSET(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_c_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VOLT_A_OFFSET(_mode, _show, _store, _addr)      \
+	IIO_DEVICE_ATTR(volt_a_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VOLT_B_OFFSET(_mode, _show, _store, _addr)      \
+	IIO_DEVICE_ATTR(volt_b_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VOLT_C_OFFSET(_mode, _show, _store, _addr)      \
+	IIO_DEVICE_ATTR(volt_c_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(reactive_power_a_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(reactive_power_b_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(reactive_power_c_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(active_power_a_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(active_power_b_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(active_power_c_offset, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_A_GAIN(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_a_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_B_GAIN(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_b_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_C_GAIN(_mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(current_c_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(apparent_power_a_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(apparent_power_b_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(apparent_power_c_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(active_power_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(active_power_a_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(active_power_b_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_ACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)   \
+	IIO_DEVICE_ATTR(active_power_c_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(reactive_power_a_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(reactive_power_b_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)     \
+	IIO_DEVICE_ATTR(reactive_power_c_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_A(_show, _addr)			\
+	IIO_DEVICE_ATTR(current_a, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_B(_show, _addr)			\
+	IIO_DEVICE_ATTR(current_b, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CURRENT_C(_show, _addr)			\
+	IIO_DEVICE_ATTR(current_c, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_VOLT_A(_show, _addr)			\
+	IIO_DEVICE_ATTR(volt_a, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_VOLT_B(_show, _addr)			\
+	IIO_DEVICE_ATTR(volt_b, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_VOLT_C(_show, _addr)			\
+	IIO_DEVICE_ATTR(volt_c, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_AENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(aenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_LENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(lenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_RAENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(raenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_LAENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(laenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_VAENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(vaenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_LVAENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(lvaenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_RVAENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(rvaenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_LVARENERGY(_show, _addr)			\
+	IIO_DEVICE_ATTR(lvarenergy, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CHKSUM(_show, _addr)                       \
+	IIO_DEVICE_ATTR(chksum, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_ANGLE0(_show, _addr)                       \
+	IIO_DEVICE_ATTR(angle0, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_ANGLE1(_show, _addr)                       \
+	IIO_DEVICE_ATTR(angle1, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_ANGLE2(_show, _addr)                       \
+	IIO_DEVICE_ATTR(angle2, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_AWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(awatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_BWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(bwatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(cwatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_AFWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(afwatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_BFWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(bfwatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CFWATTHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(cfwatthr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_AVARHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(avarhr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_BVARHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(bvarhr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CVARHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(cvarhr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_AVAHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(avahr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_BVAHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(bvahr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_CVAHR(_show, _addr)			\
+	IIO_DEVICE_ATTR(cvahr, 0444, _show, NULL, _addr)
+
+#define IIO_DEV_ATTR_IOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(ios, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_PHCAL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(phcal, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_APHCAL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(aphcal, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BPHCAL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bphcal, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CPHCAL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cphcal, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_APOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(apos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AAPOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(aapos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BAPOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bapos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CAPOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(capos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AVRMSGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(avrmsgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BVRMSGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bvrmsgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CVRMSGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cvrmsgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AIGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(aigain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BIGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bigain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CIGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cigain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_NIGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(nigain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AVGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(avgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BVGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bvgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CVGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cvgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_WGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(wgain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_WDIV(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(wdiv, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CFNUM(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cfnum, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CFDEN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cfden, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CF1DEN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cf1den, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CF2DEN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cf2den, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CF3DEN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cf3den, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_IRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(irms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vrms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AIRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(airms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BIRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(birms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CIRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cirms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_NIRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(nirms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AVRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(avrms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BVRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bvrms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CVRMS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cvrms, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_IRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(irmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vrmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AIRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(airmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BIRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(birmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CIRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cirmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_AVRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(avrmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_BVRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(bvrmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CVRMSOS(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cvrmsos, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VAGAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vagain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_PGA_GAIN(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(pga_gain, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VADIV(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vadiv, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_LINECYC(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(linecyc, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_SAGCYC(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(sagcyc, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_CFCYC(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(cfcyc, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_PEAKCYC(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(peakcyc, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_SAGLVL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(saglvl, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_IPKLVL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(ipklvl, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VPKLVL(_mode, _show, _store, _addr)                \
+	IIO_DEVICE_ATTR(vpklvl, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_IPEAK(_mode, _show, _store, _addr)			\
+	IIO_DEVICE_ATTR(ipeak, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_RIPEAK(_mode, _show, _store, _addr)		\
+	IIO_DEVICE_ATTR(ripeak, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VPEAK(_mode, _show, _store, _addr)			\
+	IIO_DEVICE_ATTR(vpeak, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_RVPEAK(_mode, _show, _store, _addr)		\
+	IIO_DEVICE_ATTR(rvpeak, _mode, _show, _store, _addr)
+
+#define IIO_DEV_ATTR_VPERIOD(_mode, _show, _store, _addr)		\
+	IIO_DEVICE_ATTR(vperiod, _mode, _show, _store, _addr)
+
+/* active energy register, AENERGY, is more than half full */
+#define IIO_EVENT_ATTR_AENERGY_HALF_FULL(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(aenergy_half_full, _evlist, _show, _store, _mask)
+
+/* a SAG on the line voltage */
+#define IIO_EVENT_ATTR_LINE_VOLT_SAG(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(line_volt_sag, _evlist, _show, _store, _mask)
+
+/*
+ * Indicates the end of energy accumulation over an integer number
+ * of half line cycles
+ */
+#define IIO_EVENT_ATTR_CYCEND(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(cycend, _evlist, _show, _store, _mask)
+
+/* on the rising and falling edge of the voltage waveform */
+#define IIO_EVENT_ATTR_ZERO_CROSS(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(zero_cross, _evlist, _show, _store, _mask)
+
+/* the active energy register has overflowed */
+#define IIO_EVENT_ATTR_AENERGY_OVERFLOW(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(aenergy_overflow, _evlist, _show, _store, _mask)
+
+/* the apparent energy register has overflowed */
+#define IIO_EVENT_ATTR_VAENERGY_OVERFLOW(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(vaenergy_overflow, _evlist, _show, _store, _mask)
+
+/* the active energy register, VAENERGY, is more than half full */
+#define IIO_EVENT_ATTR_VAENERGY_HALF_FULL(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(vaenergy_half_full, _evlist, _show, _store, _mask)
+
+/* the power has gone from negative to positive */
+#define IIO_EVENT_ATTR_PPOS(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(ppos, _evlist, _show, _store, _mask)
+
+/* the power has gone from positive to negative */
+#define IIO_EVENT_ATTR_PNEG(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(pneg, _evlist, _show, _store, _mask)
+
+/* waveform sample from Channel 1 has exceeded the IPKLVL value */
+#define IIO_EVENT_ATTR_IPKLVL_EXC(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(ipklvl_exc, _evlist, _show, _store, _mask)
+
+/* waveform sample from Channel 2 has exceeded the VPKLVL value */
+#define IIO_EVENT_ATTR_VPKLVL_EXC(_evlist, _show, _store, _mask) \
+	IIO_EVENT_ATTR_SH(vpklvl_exc, _evlist, _show, _store, _mask)
+
+#endif /* _METER_H */
diff --git a/drivers/staging/iio/resolver/Kconfig b/drivers/staging/iio/resolver/Kconfig
new file mode 100644
index 000000000..6d1e2622e
--- /dev/null
+++ b/drivers/staging/iio/resolver/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Resolver/Synchro drivers
+#
+menu "Resolver to digital converters"
+
+config AD2S1210
+	tristate "Analog Devices ad2s1210 driver"
+	depends on SPI
+	depends on GPIOLIB || COMPILE_TEST
+	help
+	  Say yes here to build support for Analog Devices spi resolver
+	  to digital converters, ad2s1210, provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad2s1210.
+
+endmenu
diff --git a/drivers/staging/iio/resolver/Makefile b/drivers/staging/iio/resolver/Makefile
new file mode 100644
index 000000000..398631f7e
--- /dev/null
+++ b/drivers/staging/iio/resolver/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for Resolver/Synchro drivers
+#
+
+obj-$(CONFIG_AD2S1210) += ad2s1210.o
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
new file mode 100644
index 000000000..636c45b12
--- /dev/null
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -0,0 +1,715 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ad2s1210.c support for the ADI Resolver to Digital Converters: AD2S1210
+ *
+ * Copyright (c) 2010-2010 Analog Devices Inc.
+ */
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define DRV_NAME "ad2s1210"
+
+#define AD2S1210_DEF_CONTROL		0x7E
+
+#define AD2S1210_MSB_IS_HIGH		0x80
+#define AD2S1210_MSB_IS_LOW		0x7F
+#define AD2S1210_PHASE_LOCK_RANGE_44	0x20
+#define AD2S1210_ENABLE_HYSTERESIS	0x10
+#define AD2S1210_SET_ENRES1		0x08
+#define AD2S1210_SET_ENRES0		0x04
+#define AD2S1210_SET_RES1		0x02
+#define AD2S1210_SET_RES0		0x01
+
+#define AD2S1210_SET_RESOLUTION		(AD2S1210_SET_RES1 | AD2S1210_SET_RES0)
+
+#define AD2S1210_REG_POSITION		0x80
+#define AD2S1210_REG_VELOCITY		0x82
+#define AD2S1210_REG_LOS_THRD		0x88
+#define AD2S1210_REG_DOS_OVR_THRD	0x89
+#define AD2S1210_REG_DOS_MIS_THRD	0x8A
+#define AD2S1210_REG_DOS_RST_MAX_THRD	0x8B
+#define AD2S1210_REG_DOS_RST_MIN_THRD	0x8C
+#define AD2S1210_REG_LOT_HIGH_THRD	0x8D
+#define AD2S1210_REG_LOT_LOW_THRD	0x8E
+#define AD2S1210_REG_EXCIT_FREQ		0x91
+#define AD2S1210_REG_CONTROL		0x92
+#define AD2S1210_REG_SOFT_RESET		0xF0
+#define AD2S1210_REG_FAULT		0xFF
+
+#define AD2S1210_MIN_CLKIN	6144000
+#define AD2S1210_MAX_CLKIN	10240000
+#define AD2S1210_MIN_EXCIT	2000
+#define AD2S1210_MAX_EXCIT	20000
+#define AD2S1210_MIN_FCW	0x4
+#define AD2S1210_MAX_FCW	0x50
+
+#define AD2S1210_DEF_EXCIT	10000
+
+enum ad2s1210_mode {
+	MOD_POS = 0,
+	MOD_VEL,
+	MOD_CONFIG,
+	MOD_RESERVED,
+};
+
+enum ad2s1210_gpios {
+	AD2S1210_SAMPLE,
+	AD2S1210_A0,
+	AD2S1210_A1,
+	AD2S1210_RES0,
+	AD2S1210_RES1,
+};
+
+struct ad2s1210_gpio {
+	const char *name;
+	unsigned long flags;
+};
+
+static const struct ad2s1210_gpio gpios[] = {
+	[AD2S1210_SAMPLE] = { .name = "adi,sample", .flags = GPIOD_OUT_LOW },
+	[AD2S1210_A0] = { .name = "adi,a0", .flags = GPIOD_OUT_LOW },
+	[AD2S1210_A1] = { .name = "adi,a1", .flags = GPIOD_OUT_LOW },
+	[AD2S1210_RES0] = { .name = "adi,res0", .flags = GPIOD_OUT_LOW },
+	[AD2S1210_RES1] = { .name = "adi,res1", .flags = GPIOD_OUT_LOW },
+};
+
+static const unsigned int ad2s1210_resolution_value[] = { 10, 12, 14, 16 };
+
+struct ad2s1210_state {
+	struct mutex lock;
+	struct spi_device *sdev;
+	struct gpio_desc *gpios[5];
+	unsigned int fclkin;
+	unsigned int fexcit;
+	bool hysteresis;
+	u8 resolution;
+	enum ad2s1210_mode mode;
+	u8 rx[2] __aligned(IIO_DMA_MINALIGN);
+	u8 tx[2];
+};
+
+static const int ad2s1210_mode_vals[4][2] = {
+	[MOD_POS] = { 0, 0 },
+	[MOD_VEL] = { 0, 1 },
+	[MOD_CONFIG] = { 1, 1 },
+};
+
+static inline void ad2s1210_set_mode(enum ad2s1210_mode mode,
+				     struct ad2s1210_state *st)
+{
+	gpiod_set_value(st->gpios[AD2S1210_A0], ad2s1210_mode_vals[mode][0]);
+	gpiod_set_value(st->gpios[AD2S1210_A1], ad2s1210_mode_vals[mode][1]);
+	st->mode = mode;
+}
+
+/* write 1 bytes (address or data) to the chip */
+static int ad2s1210_config_write(struct ad2s1210_state *st, u8 data)
+{
+	int ret;
+
+	ad2s1210_set_mode(MOD_CONFIG, st);
+	st->tx[0] = data;
+	ret = spi_write(st->sdev, st->tx, 1);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/* read value from one of the registers */
+static int ad2s1210_config_read(struct ad2s1210_state *st,
+				unsigned char address)
+{
+	struct spi_transfer xfers[] = {
+		{
+			.len = 1,
+			.rx_buf = &st->rx[0],
+			.tx_buf = &st->tx[0],
+			.cs_change = 1,
+		}, {
+			.len = 1,
+			.rx_buf = &st->rx[1],
+			.tx_buf = &st->tx[1],
+		},
+	};
+	int ret = 0;
+
+	ad2s1210_set_mode(MOD_CONFIG, st);
+	st->tx[0] = address | AD2S1210_MSB_IS_HIGH;
+	st->tx[1] = AD2S1210_REG_FAULT;
+	ret = spi_sync_transfer(st->sdev, xfers, 2);
+	if (ret < 0)
+		return ret;
+
+	return st->rx[1];
+}
+
+static inline
+int ad2s1210_update_frequency_control_word(struct ad2s1210_state *st)
+{
+	int ret;
+	unsigned char fcw;
+
+	fcw = (unsigned char)(st->fexcit * (1 << 15) / st->fclkin);
+	if (fcw < AD2S1210_MIN_FCW || fcw > AD2S1210_MAX_FCW) {
+		dev_err(&st->sdev->dev, "ad2s1210: FCW out of range\n");
+		return -ERANGE;
+	}
+
+	ret = ad2s1210_config_write(st, AD2S1210_REG_EXCIT_FREQ);
+	if (ret < 0)
+		return ret;
+
+	return ad2s1210_config_write(st, fcw);
+}
+
+static const int ad2s1210_res_pins[4][2] = {
+	{ 0, 0 }, {0, 1}, {1, 0}, {1, 1}
+};
+
+static inline void ad2s1210_set_resolution_pin(struct ad2s1210_state *st)
+{
+	gpiod_set_value(st->gpios[AD2S1210_RES0],
+			ad2s1210_res_pins[(st->resolution - 10) / 2][0]);
+	gpiod_set_value(st->gpios[AD2S1210_RES1],
+			ad2s1210_res_pins[(st->resolution - 10) / 2][1]);
+}
+
+static inline int ad2s1210_soft_reset(struct ad2s1210_state *st)
+{
+	int ret;
+
+	ret = ad2s1210_config_write(st, AD2S1210_REG_SOFT_RESET);
+	if (ret < 0)
+		return ret;
+
+	return ad2s1210_config_write(st, 0x0);
+}
+
+static ssize_t ad2s1210_show_fclkin(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "%u\n", st->fclkin);
+}
+
+static ssize_t ad2s1210_store_fclkin(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned int fclkin;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &fclkin);
+	if (ret)
+		return ret;
+	if (fclkin < AD2S1210_MIN_CLKIN || fclkin > AD2S1210_MAX_CLKIN) {
+		dev_err(dev, "ad2s1210: fclkin out of range\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&st->lock);
+	st->fclkin = fclkin;
+
+	ret = ad2s1210_update_frequency_control_word(st);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_soft_reset(st);
+error_ret:
+	mutex_unlock(&st->lock);
+
+	return ret < 0 ? ret : len;
+}
+
+static ssize_t ad2s1210_show_fexcit(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "%u\n", st->fexcit);
+}
+
+static ssize_t ad2s1210_store_fexcit(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned int fexcit;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &fexcit);
+	if (ret < 0)
+		return ret;
+	if (fexcit < AD2S1210_MIN_EXCIT || fexcit > AD2S1210_MAX_EXCIT) {
+		dev_err(dev,
+			"ad2s1210: excitation frequency out of range\n");
+		return -EINVAL;
+	}
+	mutex_lock(&st->lock);
+	st->fexcit = fexcit;
+	ret = ad2s1210_update_frequency_control_word(st);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_soft_reset(st);
+error_ret:
+	mutex_unlock(&st->lock);
+
+	return ret < 0 ? ret : len;
+}
+
+static ssize_t ad2s1210_show_control(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	int ret;
+
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
+	mutex_unlock(&st->lock);
+	return ret < 0 ? ret : sprintf(buf, "0x%x\n", ret);
+}
+
+static ssize_t ad2s1210_store_control(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned char udata;
+	unsigned char data;
+	int ret;
+
+	ret = kstrtou8(buf, 16, &udata);
+	if (ret)
+		return -EINVAL;
+
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_write(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	data = udata & AD2S1210_MSB_IS_LOW;
+	ret = ad2s1210_config_write(st, data);
+	if (ret < 0)
+		goto error_ret;
+
+	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	if (ret & AD2S1210_MSB_IS_HIGH) {
+		ret = -EIO;
+		dev_err(dev,
+			"ad2s1210: write control register fail\n");
+		goto error_ret;
+	}
+	st->resolution =
+		ad2s1210_resolution_value[data & AD2S1210_SET_RESOLUTION];
+	ad2s1210_set_resolution_pin(st);
+	ret = len;
+	st->hysteresis = !!(data & AD2S1210_ENABLE_HYSTERESIS);
+
+error_ret:
+	mutex_unlock(&st->lock);
+	return ret;
+}
+
+static ssize_t ad2s1210_show_resolution(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "%d\n", st->resolution);
+}
+
+static ssize_t ad2s1210_store_resolution(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned char data;
+	unsigned char udata;
+	int ret;
+
+	ret = kstrtou8(buf, 10, &udata);
+	if (ret || udata < 10 || udata > 16) {
+		dev_err(dev, "ad2s1210: resolution out of range\n");
+		return -EINVAL;
+	}
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	data = ret;
+	data &= ~AD2S1210_SET_RESOLUTION;
+	data |= (udata - 10) >> 1;
+	ret = ad2s1210_config_write(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_config_write(st, data & AD2S1210_MSB_IS_LOW);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	data = ret;
+	if (data & AD2S1210_MSB_IS_HIGH) {
+		ret = -EIO;
+		dev_err(dev, "ad2s1210: setting resolution fail\n");
+		goto error_ret;
+	}
+	st->resolution =
+		ad2s1210_resolution_value[data & AD2S1210_SET_RESOLUTION];
+	ad2s1210_set_resolution_pin(st);
+	ret = len;
+error_ret:
+	mutex_unlock(&st->lock);
+	return ret;
+}
+
+/* read the fault register since last sample */
+static ssize_t ad2s1210_show_fault(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	int ret;
+
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_read(st, AD2S1210_REG_FAULT);
+	mutex_unlock(&st->lock);
+
+	return ret ? ret : sprintf(buf, "0x%x\n", ret);
+}
+
+static ssize_t ad2s1210_clear_fault(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf,
+				    size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	int ret;
+
+	mutex_lock(&st->lock);
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 0);
+	/* delay (2 * tck + 20) nano seconds */
+	udelay(1);
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 1);
+	ret = ad2s1210_config_read(st, AD2S1210_REG_FAULT);
+	if (ret < 0)
+		goto error_ret;
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 0);
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 1);
+error_ret:
+	mutex_unlock(&st->lock);
+
+	return ret < 0 ? ret : len;
+}
+
+static ssize_t ad2s1210_show_reg(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	struct iio_dev_attr *iattr = to_iio_dev_attr(attr);
+	int ret;
+
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_read(st, iattr->address);
+	mutex_unlock(&st->lock);
+
+	return ret < 0 ? ret : sprintf(buf, "%d\n", ret);
+}
+
+static ssize_t ad2s1210_store_reg(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t len)
+{
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned char data;
+	int ret;
+	struct iio_dev_attr *iattr = to_iio_dev_attr(attr);
+
+	ret = kstrtou8(buf, 10, &data);
+	if (ret)
+		return -EINVAL;
+	mutex_lock(&st->lock);
+	ret = ad2s1210_config_write(st, iattr->address);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_config_write(st, data & AD2S1210_MSB_IS_LOW);
+error_ret:
+	mutex_unlock(&st->lock);
+	return ret < 0 ? ret : len;
+}
+
+static int ad2s1210_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int *val,
+			     int *val2,
+			     long m)
+{
+	struct ad2s1210_state *st = iio_priv(indio_dev);
+	u16 negative;
+	int ret = 0;
+	u16 pos;
+	s16 vel;
+
+	mutex_lock(&st->lock);
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 0);
+	/* delay (6 * tck + 20) nano seconds */
+	udelay(1);
+
+	switch (chan->type) {
+	case IIO_ANGL:
+		ad2s1210_set_mode(MOD_POS, st);
+		break;
+	case IIO_ANGL_VEL:
+		ad2s1210_set_mode(MOD_VEL, st);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (ret < 0)
+		goto error_ret;
+	ret = spi_read(st->sdev, st->rx, 2);
+	if (ret < 0)
+		goto error_ret;
+
+	switch (chan->type) {
+	case IIO_ANGL:
+		pos = be16_to_cpup((__be16 *)st->rx);
+		if (st->hysteresis)
+			pos >>= 16 - st->resolution;
+		*val = pos;
+		ret = IIO_VAL_INT;
+		break;
+	case IIO_ANGL_VEL:
+		vel = be16_to_cpup((__be16 *)st->rx);
+		vel >>= 16 - st->resolution;
+		if (vel & 0x8000) {
+			negative = (0xffff >> st->resolution) << st->resolution;
+			vel |= negative;
+		}
+		*val = vel;
+		ret = IIO_VAL_INT;
+		break;
+	default:
+		mutex_unlock(&st->lock);
+		return -EINVAL;
+	}
+
+error_ret:
+	gpiod_set_value(st->gpios[AD2S1210_SAMPLE], 1);
+	/* delay (2 * tck + 20) nano seconds */
+	udelay(1);
+	mutex_unlock(&st->lock);
+	return ret;
+}
+
+static IIO_DEVICE_ATTR(fclkin, 0644,
+		       ad2s1210_show_fclkin, ad2s1210_store_fclkin, 0);
+static IIO_DEVICE_ATTR(fexcit, 0644,
+		       ad2s1210_show_fexcit,	ad2s1210_store_fexcit, 0);
+static IIO_DEVICE_ATTR(control, 0644,
+		       ad2s1210_show_control, ad2s1210_store_control, 0);
+static IIO_DEVICE_ATTR(bits, 0644,
+		       ad2s1210_show_resolution, ad2s1210_store_resolution, 0);
+static IIO_DEVICE_ATTR(fault, 0644,
+		       ad2s1210_show_fault, ad2s1210_clear_fault, 0);
+
+static IIO_DEVICE_ATTR(los_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_LOS_THRD);
+static IIO_DEVICE_ATTR(dos_ovr_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_DOS_OVR_THRD);
+static IIO_DEVICE_ATTR(dos_mis_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_DOS_MIS_THRD);
+static IIO_DEVICE_ATTR(dos_rst_max_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_DOS_RST_MAX_THRD);
+static IIO_DEVICE_ATTR(dos_rst_min_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_DOS_RST_MIN_THRD);
+static IIO_DEVICE_ATTR(lot_high_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_LOT_HIGH_THRD);
+static IIO_DEVICE_ATTR(lot_low_thrd, 0644,
+		       ad2s1210_show_reg, ad2s1210_store_reg,
+		       AD2S1210_REG_LOT_LOW_THRD);
+
+static const struct iio_chan_spec ad2s1210_channels[] = {
+	{
+		.type = IIO_ANGL,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+	}, {
+		.type = IIO_ANGL_VEL,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+	}
+};
+
+static struct attribute *ad2s1210_attributes[] = {
+	&iio_dev_attr_fclkin.dev_attr.attr,
+	&iio_dev_attr_fexcit.dev_attr.attr,
+	&iio_dev_attr_control.dev_attr.attr,
+	&iio_dev_attr_bits.dev_attr.attr,
+	&iio_dev_attr_fault.dev_attr.attr,
+	&iio_dev_attr_los_thrd.dev_attr.attr,
+	&iio_dev_attr_dos_ovr_thrd.dev_attr.attr,
+	&iio_dev_attr_dos_mis_thrd.dev_attr.attr,
+	&iio_dev_attr_dos_rst_max_thrd.dev_attr.attr,
+	&iio_dev_attr_dos_rst_min_thrd.dev_attr.attr,
+	&iio_dev_attr_lot_high_thrd.dev_attr.attr,
+	&iio_dev_attr_lot_low_thrd.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad2s1210_attribute_group = {
+	.attrs = ad2s1210_attributes,
+};
+
+static int ad2s1210_initial(struct ad2s1210_state *st)
+{
+	unsigned char data;
+	int ret;
+
+	mutex_lock(&st->lock);
+	ad2s1210_set_resolution_pin(st);
+
+	ret = ad2s1210_config_write(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+	data = AD2S1210_DEF_CONTROL & ~(AD2S1210_SET_RESOLUTION);
+	data |= (st->resolution - 10) >> 1;
+	ret = ad2s1210_config_write(st, data);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
+	if (ret < 0)
+		goto error_ret;
+
+	if (ret & AD2S1210_MSB_IS_HIGH) {
+		ret = -EIO;
+		goto error_ret;
+	}
+
+	ret = ad2s1210_update_frequency_control_word(st);
+	if (ret < 0)
+		goto error_ret;
+	ret = ad2s1210_soft_reset(st);
+error_ret:
+	mutex_unlock(&st->lock);
+	return ret;
+}
+
+static const struct iio_info ad2s1210_info = {
+	.read_raw = ad2s1210_read_raw,
+	.attrs = &ad2s1210_attribute_group,
+};
+
+static int ad2s1210_setup_gpios(struct ad2s1210_state *st)
+{
+	struct spi_device *spi = st->sdev;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(gpios); i++) {
+		st->gpios[i] = devm_gpiod_get(&spi->dev, gpios[i].name,
+					      gpios[i].flags);
+		if (IS_ERR(st->gpios[i])) {
+			ret = PTR_ERR(st->gpios[i]);
+			dev_err(&spi->dev,
+				"ad2s1210: failed to request %s GPIO: %d\n",
+				gpios[i].name, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ad2s1210_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct ad2s1210_state *st;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+	st = iio_priv(indio_dev);
+	ret = ad2s1210_setup_gpios(st);
+	if (ret < 0)
+		return ret;
+
+	spi_set_drvdata(spi, indio_dev);
+
+	mutex_init(&st->lock);
+	st->sdev = spi;
+	st->hysteresis = true;
+	st->mode = MOD_CONFIG;
+	st->resolution = 12;
+	st->fexcit = AD2S1210_DEF_EXCIT;
+
+	indio_dev->info = &ad2s1210_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ad2s1210_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ad2s1210_channels);
+	indio_dev->name = spi_get_device_id(spi)->name;
+
+	ret = devm_iio_device_register(&spi->dev, indio_dev);
+	if (ret)
+		return ret;
+
+	st->fclkin = spi->max_speed_hz;
+	spi->mode = SPI_MODE_3;
+	spi_setup(spi);
+	ad2s1210_initial(st);
+
+	return 0;
+}
+
+static const struct of_device_id ad2s1210_of_match[] = {
+	{ .compatible = "adi,ad2s1210", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ad2s1210_of_match);
+
+static const struct spi_device_id ad2s1210_id[] = {
+	{ "ad2s1210" },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad2s1210_id);
+
+static struct spi_driver ad2s1210_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = of_match_ptr(ad2s1210_of_match),
+	},
+	.probe = ad2s1210_probe,
+	.id_table = ad2s1210_id,
+};
+module_spi_driver(ad2s1210_driver);
+
+MODULE_AUTHOR("Graff Yang <graff.yang@gmail.com>");
+MODULE_DESCRIPTION("Analog Devices AD2S1210 Resolver to Digital SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/ks7010/Kconfig b/drivers/staging/ks7010/Kconfig
new file mode 100644
index 000000000..8ea6c0928
--- /dev/null
+++ b/drivers/staging/ks7010/Kconfig
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+config KS7010
+	tristate "KeyStream KS7010 SDIO support"
+	depends on MMC && WIRELESS
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select FW_LOADER
+	select CRYPTO
+	select CRYPTO_HASH
+	select CRYPTO_MICHAEL_MIC
+	help
+	  This is a driver for KeyStream KS7010 based SDIO WIFI cards. It is
+	  found on at least later Spectec SDW-821 (FCC-ID "S2Y-WLAN-11G-K" only,
+	  sadly not FCC-ID "S2Y-WLAN-11B-G") and Spectec SDW-823 microSD cards.
diff --git a/drivers/staging/ks7010/Makefile b/drivers/staging/ks7010/Makefile
new file mode 100644
index 000000000..009851a32
--- /dev/null
+++ b/drivers/staging/ks7010/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_KS7010) += ks7010.o
+
+ks7010-y	:= ks_hostif.o ks_wlan_net.o ks7010_sdio.o
diff --git a/drivers/staging/ks7010/TODO b/drivers/staging/ks7010/TODO
new file mode 100644
index 000000000..ab6f39175
--- /dev/null
+++ b/drivers/staging/ks7010/TODO
@@ -0,0 +1,33 @@
+KS7010 Linux driver
+===================
+
+This driver is based on source code from the Ben Nanonote extra repository [1]
+which is based on the original v007 release from Renesas [2]. Some more
+background info about the chipset can be found here [3] and here [4]. Thank
+you to all which already participated in cleaning up the driver so far!
+
+[1] http://projects.qi-hardware.com/index.php/p/openwrt-packages/source/tree/master/ks7010/src
+[2] http://downloads.qi-hardware.com/software/ks7010_sdio_v007.tar.bz2
+[3] http://en.qi-hardware.com/wiki/Ben_NanoNote_Wi-Fi
+[4] https://wikidevi.com/wiki/Renesas
+
+TODO
+----
+
+First a few words what not to do (at least not blindly):
+
+- don't be overly strict with the 80 char limit. Only if it REALLY makes the
+  code more readable
+
+Now the TODOs:
+
+- fix codechecker warnings (checkpatch, sparse, smatch). But PLEASE make sure
+  that you are not only silencing the warning but really fixing code. You
+  should understand the change you submit.
+- fix the 'card removal' event when card is inserted when booting
+- check what other upstream wireless mechanisms can be used instead of the
+  custom ones here
+
+Please send any patches to:
+Greg Kroah-Hartman <gregkh@linuxfoundation.org>
+Linux Driver Project Developer List <driverdev-devel@linuxdriverproject.org>
diff --git a/drivers/staging/ks7010/eap_packet.h b/drivers/staging/ks7010/eap_packet.h
new file mode 100644
index 000000000..1eee77431
--- /dev/null
+++ b/drivers/staging/ks7010/eap_packet.h
@@ -0,0 +1,70 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef EAP_PACKET_H
+#define EAP_PACKET_H
+
+#include <linux/compiler.h>
+#include <linux/bitops.h>
+#include <uapi/linux/if_ether.h>
+
+struct ether_hdr {
+	unsigned char h_dest[ETH_ALEN];	/* destination eth addr */
+	unsigned char h_source[ETH_ALEN];	/* source ether addr    */
+	unsigned char h_dest_snap;
+	unsigned char h_source_snap;
+	unsigned char h_command;
+	unsigned char h_vendor_id[3];
+	__be16 h_proto;	/* packet type ID field */
+	/* followed by length octets of data */
+} __packed;
+
+#define ETHER_HDR_SIZE sizeof(struct ether_hdr)
+
+struct ieee802_1x_hdr {
+	unsigned char version;
+	unsigned char type;
+	unsigned short length;
+	/* followed by length octets of data */
+} __packed;
+
+enum {
+	IEEE802_1X_TYPE_EAP_PACKET = 0,
+	IEEE802_1X_TYPE_EAPOL_START = 1,
+	IEEE802_1X_TYPE_EAPOL_LOGOFF = 2,
+	IEEE802_1X_TYPE_EAPOL_KEY = 3,
+	IEEE802_1X_TYPE_EAPOL_ENCAPSULATED_ASF_ALERT = 4
+};
+
+#define WPA_NONCE_LEN 32
+#define WPA_REPLAY_COUNTER_LEN 8
+
+struct wpa_eapol_key {
+	unsigned char type;
+	__be16 key_info;
+	unsigned short key_length;
+	unsigned char replay_counter[WPA_REPLAY_COUNTER_LEN];
+	unsigned char key_nonce[WPA_NONCE_LEN];
+	unsigned char key_iv[16];
+	unsigned char key_rsc[8];
+	unsigned char key_id[8];	/* Reserved in IEEE 802.11i/RSN */
+	unsigned char key_mic[16];
+	unsigned short key_data_length;
+	/* followed by key_data_length bytes of key_data */
+} __packed;
+
+#define WPA_KEY_INFO_TYPE_MASK GENMASK(2, 0)
+#define WPA_KEY_INFO_TYPE_HMAC_MD5_RC4 BIT(0)
+#define WPA_KEY_INFO_TYPE_HMAC_SHA1_AES BIT(1)
+#define WPA_KEY_INFO_KEY_TYPE BIT(3)	/* 1 = Pairwise, 0 = Group key */
+/* bit4..5 is used in WPA, but is reserved in IEEE 802.11i/RSN */
+#define WPA_KEY_INFO_KEY_INDEX_MASK GENMASK(5, 4)
+#define WPA_KEY_INFO_KEY_INDEX_SHIFT 4
+#define WPA_KEY_INFO_INSTALL BIT(6)	/* pairwise */
+#define WPA_KEY_INFO_TXRX BIT(6)	/* group */
+#define WPA_KEY_INFO_ACK BIT(7)
+#define WPA_KEY_INFO_MIC BIT(8)
+#define WPA_KEY_INFO_SECURE BIT(9)
+#define WPA_KEY_INFO_ERROR BIT(10)
+#define WPA_KEY_INFO_REQUEST BIT(11)
+#define WPA_KEY_INFO_ENCR_KEY_DATA BIT(12)	/* IEEE 802.11i/RSN only */
+
+#endif /* EAP_PACKET_H */
diff --git a/drivers/staging/ks7010/ks7010_sdio.c b/drivers/staging/ks7010/ks7010_sdio.c
new file mode 100644
index 000000000..9fb118e77
--- /dev/null
+++ b/drivers/staging/ks7010/ks7010_sdio.c
@@ -0,0 +1,1143 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *   Driver for KeyStream, KS7010 based SDIO cards.
+ *
+ *   Copyright (C) 2006-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ *   Copyright (C) 2016 Sang Engineering, Wolfram Sang
+ */
+
+#include <linux/atomic.h>
+#include <linux/firmware.h>
+#include <linux/jiffies.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/module.h>
+#include <linux/workqueue.h>
+#include "ks_wlan.h"
+#include "ks_hostif.h"
+
+#define ROM_FILE "ks7010sd.rom"
+
+/*  SDIO KeyStream vendor and device */
+#define SDIO_VENDOR_ID_KS_CODE_A	0x005b
+#define SDIO_VENDOR_ID_KS_CODE_B	0x0023
+
+/* Older sources suggest earlier versions were named 7910 or 79xx */
+#define SDIO_DEVICE_ID_KS_7010		0x7910
+
+/* Read/Write Status Register */
+#define READ_STATUS_REG		0x000000
+#define WRITE_STATUS_REG	0x00000C
+enum reg_status_type {
+	REG_STATUS_BUSY,
+	REG_STATUS_IDLE
+};
+
+/* Read Index Register */
+#define READ_INDEX_REG		0x000004
+
+/* Read Data Size Register */
+#define READ_DATA_SIZE_REG	0x000008
+
+/* Write Index Register */
+#define WRITE_INDEX_REG		0x000010
+
+/*
+ * Write Status/Read Data Size Register
+ * for network packet (less than 2048 bytes data)
+ */
+#define WSTATUS_RSIZE_REG	0x000014
+
+/* Write Status Register value */
+#define WSTATUS_MASK		0x80
+
+/* Read Data Size Register value [10:4] */
+#define RSIZE_MASK		0x7F
+
+/* ARM to SD interrupt Enable */
+#define INT_ENABLE_REG		0x000020
+/* ARM to SD interrupt Pending */
+#define INT_PENDING_REG		0x000024
+
+#define INT_GCR_B              BIT(7)
+#define INT_GCR_A              BIT(6)
+#define INT_WRITE_STATUS       BIT(5)
+#define INT_WRITE_INDEX        BIT(4)
+#define INT_WRITE_SIZE         BIT(3)
+#define INT_READ_STATUS        BIT(2)
+#define INT_READ_INDEX         BIT(1)
+#define INT_READ_SIZE          BIT(0)
+
+/* General Communication Register A */
+#define GCR_A_REG		0x000028
+enum gen_com_reg_a {
+	GCR_A_INIT,
+	GCR_A_REMAP,
+	GCR_A_RUN
+};
+
+/* General Communication Register B */
+#define GCR_B_REG		0x00002C
+enum gen_com_reg_b {
+	GCR_B_ACTIVE,
+	GCR_B_DOZE
+};
+
+/* Wakeup Register */
+#define WAKEUP_REG		0x008018
+#define WAKEUP_REQ		0x5a
+
+/* AHB Data Window  0x010000-0x01FFFF */
+#define DATA_WINDOW		0x010000
+#define WINDOW_SIZE		(64 * 1024)
+
+#define KS7010_IRAM_ADDRESS	0x06000000
+
+#define KS7010_IO_BLOCK_SIZE 512
+
+/**
+ * struct ks_sdio_card - SDIO device data.
+ *
+ * Structure is used as the &struct sdio_func private data.
+ *
+ * @func: Pointer to the SDIO function device.
+ * @priv: Pointer to the &struct net_device private data.
+ */
+struct ks_sdio_card {
+	struct sdio_func *func;
+	struct ks_wlan_private *priv;
+};
+
+static struct sdio_func *ks7010_to_func(struct ks_wlan_private *priv)
+{
+	struct ks_sdio_card *ks_sdio = priv->if_hw;
+
+	return ks_sdio->func;
+}
+
+/* Read single byte from device address into byte (CMD52) */
+static int ks7010_sdio_readb(struct ks_wlan_private *priv,
+			     u32 address, u8 *byte)
+{
+	struct sdio_func *func = ks7010_to_func(priv);
+	int ret;
+
+	*byte = sdio_readb(func, address, &ret);
+
+	return ret;
+}
+
+/* Read length bytes from device address into buffer (CMD53) */
+static int ks7010_sdio_read(struct ks_wlan_private *priv, u32 address,
+			    u8 *buffer, unsigned int length)
+{
+	struct sdio_func *func = ks7010_to_func(priv);
+
+	return sdio_memcpy_fromio(func, buffer, address, length);
+}
+
+/* Write single byte to device address (CMD52) */
+static int ks7010_sdio_writeb(struct ks_wlan_private *priv,
+			      u32 address, u8 byte)
+{
+	struct sdio_func *func = ks7010_to_func(priv);
+	int ret;
+
+	sdio_writeb(func, byte, address, &ret);
+
+	return ret;
+}
+
+/* Write length bytes to device address from buffer (CMD53) */
+static int ks7010_sdio_write(struct ks_wlan_private *priv, u32 address,
+			     u8 *buffer, unsigned int length)
+{
+	struct sdio_func *func = ks7010_to_func(priv);
+
+	return sdio_memcpy_toio(func, address, buffer, length);
+}
+
+static void ks_wlan_hw_sleep_doze_request(struct ks_wlan_private *priv)
+{
+	int ret;
+
+	/* clear request */
+	atomic_set(&priv->sleepstatus.doze_request, 0);
+
+	if (atomic_read(&priv->sleepstatus.status) == 0) {
+		ret = ks7010_sdio_writeb(priv, GCR_B_REG, GCR_B_DOZE);
+		if (ret) {
+			netdev_err(priv->net_dev, "write GCR_B_REG\n");
+			goto set_sleep_mode;
+		}
+		atomic_set(&priv->sleepstatus.status, 1);
+		priv->last_doze = jiffies;
+	}
+
+set_sleep_mode:
+	priv->sleep_mode = atomic_read(&priv->sleepstatus.status);
+}
+
+static void ks_wlan_hw_sleep_wakeup_request(struct ks_wlan_private *priv)
+{
+	int ret;
+
+	/* clear request */
+	atomic_set(&priv->sleepstatus.wakeup_request, 0);
+
+	if (atomic_read(&priv->sleepstatus.status) == 1) {
+		ret = ks7010_sdio_writeb(priv, WAKEUP_REG, WAKEUP_REQ);
+		if (ret) {
+			netdev_err(priv->net_dev, "write WAKEUP_REG\n");
+			goto set_sleep_mode;
+		}
+		atomic_set(&priv->sleepstatus.status, 0);
+		priv->last_wakeup = jiffies;
+		++priv->wakeup_count;
+	}
+
+set_sleep_mode:
+	priv->sleep_mode = atomic_read(&priv->sleepstatus.status);
+}
+
+void ks_wlan_hw_wakeup_request(struct ks_wlan_private *priv)
+{
+	int ret;
+
+	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+		ret = ks7010_sdio_writeb(priv, WAKEUP_REG, WAKEUP_REQ);
+		if (ret)
+			netdev_err(priv->net_dev, "write WAKEUP_REG\n");
+
+		priv->last_wakeup = jiffies;
+		++priv->wakeup_count;
+	}
+}
+
+static void _ks_wlan_hw_power_save(struct ks_wlan_private *priv)
+{
+	u8 byte;
+	int ret;
+
+	if (priv->reg.power_mgmt == POWER_MGMT_ACTIVE)
+		return;
+
+	if (priv->reg.operation_mode != MODE_INFRASTRUCTURE)
+		return;
+
+	if (!is_connect_status(priv->connect_status))
+		return;
+
+	if (priv->dev_state != DEVICE_STATE_SLEEP)
+		return;
+
+	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE)
+		return;
+
+	netdev_dbg(priv->net_dev,
+		   "STATUS:\n"
+		   "- psstatus.status = %d\n"
+		   "- psstatus.confirm_wait = %d\n"
+		   "- psstatus.snooze_guard = %d\n"
+		   "- txq_count = %d\n",
+		   atomic_read(&priv->psstatus.status),
+		   atomic_read(&priv->psstatus.confirm_wait),
+		   atomic_read(&priv->psstatus.snooze_guard),
+		   txq_count(priv));
+
+	if (atomic_read(&priv->psstatus.confirm_wait) ||
+	    atomic_read(&priv->psstatus.snooze_guard) ||
+	    txq_has_space(priv)) {
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+		return;
+	}
+
+	ret = ks7010_sdio_readb(priv, INT_PENDING_REG, &byte);
+	if (ret) {
+		netdev_err(priv->net_dev, "read INT_PENDING_REG\n");
+		goto queue_delayed_work;
+	}
+	if (byte)
+		goto queue_delayed_work;
+
+	ret = ks7010_sdio_writeb(priv, GCR_B_REG, GCR_B_DOZE);
+	if (ret) {
+		netdev_err(priv->net_dev, "write GCR_B_REG\n");
+		goto queue_delayed_work;
+	}
+	atomic_set(&priv->psstatus.status, PS_SNOOZE);
+
+	return;
+
+queue_delayed_work:
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+}
+
+int ks_wlan_hw_power_save(struct ks_wlan_private *priv)
+{
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+	return 0;
+}
+
+static int enqueue_txdev(struct ks_wlan_private *priv, unsigned char *p,
+			 unsigned long size,
+			 void (*complete_handler)(struct ks_wlan_private *priv,
+						  struct sk_buff *skb),
+			 struct sk_buff *skb)
+{
+	struct tx_device_buffer *sp;
+	int ret;
+
+	if (priv->dev_state < DEVICE_STATE_BOOT) {
+		ret = -EPERM;
+		goto err_complete;
+	}
+
+	if ((TX_DEVICE_BUFF_SIZE - 1) <= txq_count(priv)) {
+		netdev_err(priv->net_dev, "tx buffer overflow\n");
+		ret = -EOVERFLOW;
+		goto err_complete;
+	}
+
+	sp = &priv->tx_dev.tx_dev_buff[priv->tx_dev.qtail];
+	sp->sendp = p;
+	sp->size = size;
+	sp->complete_handler = complete_handler;
+	sp->skb = skb;
+	inc_txqtail(priv);
+
+	return 0;
+
+err_complete:
+	kfree(p);
+	if (complete_handler)
+		(*complete_handler)(priv, skb);
+
+	return ret;
+}
+
+/* write data */
+static int write_to_device(struct ks_wlan_private *priv, u8 *buffer,
+			   unsigned long size)
+{
+	struct hostif_hdr *hdr;
+	int ret;
+
+	hdr = (struct hostif_hdr *)buffer;
+
+	if (le16_to_cpu(hdr->event) < HIF_DATA_REQ ||
+	    le16_to_cpu(hdr->event) > HIF_REQ_MAX) {
+		netdev_err(priv->net_dev, "unknown event=%04X\n", hdr->event);
+		return 0;
+	}
+
+	ret = ks7010_sdio_write(priv, DATA_WINDOW, buffer, size);
+	if (ret) {
+		netdev_err(priv->net_dev, "write DATA_WINDOW\n");
+		return ret;
+	}
+
+	ret = ks7010_sdio_writeb(priv, WRITE_STATUS_REG, REG_STATUS_BUSY);
+	if (ret) {
+		netdev_err(priv->net_dev, "write WRITE_STATUS_REG\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void tx_device_task(struct ks_wlan_private *priv)
+{
+	struct tx_device_buffer *sp;
+	int ret;
+
+	if (!txq_has_space(priv) ||
+	    atomic_read(&priv->psstatus.status) == PS_SNOOZE)
+		return;
+
+	sp = &priv->tx_dev.tx_dev_buff[priv->tx_dev.qhead];
+	if (priv->dev_state >= DEVICE_STATE_BOOT) {
+		ret = write_to_device(priv, sp->sendp, sp->size);
+		if (ret) {
+			netdev_err(priv->net_dev,
+				   "write_to_device error !!(%d)\n", ret);
+			queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+			return;
+		}
+	}
+	kfree(sp->sendp);
+	if (sp->complete_handler)	/* TX Complete */
+		(*sp->complete_handler)(priv, sp->skb);
+	inc_txqhead(priv);
+
+	if (txq_has_space(priv))
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+}
+
+int ks_wlan_hw_tx(struct ks_wlan_private *priv, void *p, unsigned long size,
+		  void (*complete_handler)(struct ks_wlan_private *priv,
+					   struct sk_buff *skb),
+		  struct sk_buff *skb)
+{
+	int result;
+	struct hostif_hdr *hdr;
+
+	hdr = (struct hostif_hdr *)p;
+
+	if (le16_to_cpu(hdr->event) < HIF_DATA_REQ ||
+	    le16_to_cpu(hdr->event) > HIF_REQ_MAX) {
+		netdev_err(priv->net_dev, "unknown event=%04X\n", hdr->event);
+		return 0;
+	}
+
+	/* add event to hostt buffer */
+	priv->hostt.buff[priv->hostt.qtail] = le16_to_cpu(hdr->event);
+	priv->hostt.qtail = (priv->hostt.qtail + 1) % SME_EVENT_BUFF_SIZE;
+
+	spin_lock(&priv->tx_dev.tx_dev_lock);
+	result = enqueue_txdev(priv, p, size, complete_handler, skb);
+	spin_unlock(&priv->tx_dev.tx_dev_lock);
+
+	if (txq_has_space(priv))
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+
+	return result;
+}
+
+static void rx_event_task(struct tasklet_struct *t)
+{
+	struct ks_wlan_private *priv = from_tasklet(priv, t, rx_bh_task);
+	struct rx_device_buffer *rp;
+
+	if (rxq_has_space(priv) && priv->dev_state >= DEVICE_STATE_BOOT) {
+		rp = &priv->rx_dev.rx_dev_buff[priv->rx_dev.qhead];
+		hostif_receive(priv, rp->data, rp->size);
+		inc_rxqhead(priv);
+
+		if (rxq_has_space(priv))
+			tasklet_schedule(&priv->rx_bh_task);
+	}
+}
+
+static void ks_wlan_hw_rx(struct ks_wlan_private *priv, size_t size)
+{
+	int ret;
+	struct rx_device_buffer *rx_buffer;
+	struct hostif_hdr *hdr;
+	u16 event = 0;
+
+	/* receive data */
+	if (rxq_count(priv) >= (RX_DEVICE_BUFF_SIZE - 1)) {
+		netdev_err(priv->net_dev, "rx buffer overflow\n");
+		return;
+	}
+	rx_buffer = &priv->rx_dev.rx_dev_buff[priv->rx_dev.qtail];
+
+	ret = ks7010_sdio_read(priv, DATA_WINDOW, &rx_buffer->data[0],
+			       hif_align_size(size));
+	if (ret)
+		return;
+
+	/* length check */
+	if (size > 2046 || size == 0) {
+#ifdef DEBUG
+		print_hex_dump_bytes("INVALID DATA dump: ",
+				     DUMP_PREFIX_OFFSET,
+				     rx_buffer->data, 32);
+#endif
+		ret = ks7010_sdio_writeb(priv, READ_STATUS_REG,
+					 REG_STATUS_IDLE);
+		if (ret)
+			netdev_err(priv->net_dev, "write READ_STATUS_REG\n");
+
+		/* length check fail */
+		return;
+	}
+
+	hdr = (struct hostif_hdr *)&rx_buffer->data[0];
+	rx_buffer->size = le16_to_cpu(hdr->size) + sizeof(hdr->size);
+	event = le16_to_cpu(hdr->event);
+	inc_rxqtail(priv);
+
+	ret = ks7010_sdio_writeb(priv, READ_STATUS_REG, REG_STATUS_IDLE);
+	if (ret)
+		netdev_err(priv->net_dev, "write READ_STATUS_REG\n");
+
+	if (atomic_read(&priv->psstatus.confirm_wait) && is_hif_conf(event)) {
+		netdev_dbg(priv->net_dev, "IS_HIF_CONF true !!\n");
+		atomic_dec(&priv->psstatus.confirm_wait);
+	}
+
+	tasklet_schedule(&priv->rx_bh_task);
+}
+
+static void ks7010_rw_function(struct work_struct *work)
+{
+	struct ks_wlan_private *priv = container_of(work,
+						    struct ks_wlan_private,
+						    rw_dwork.work);
+	struct sdio_func *func = ks7010_to_func(priv);
+	u8 byte;
+	int ret;
+
+	/* wait after DOZE */
+	if (time_after(priv->last_doze + msecs_to_jiffies(30), jiffies)) {
+		netdev_dbg(priv->net_dev, "wait after DOZE\n");
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+		return;
+	}
+
+	/* wait after WAKEUP */
+	while (time_after(priv->last_wakeup + msecs_to_jiffies(30), jiffies)) {
+		netdev_dbg(priv->net_dev, "wait after WAKEUP\n");
+		dev_info(&func->dev, "wake: %lu %lu\n",
+			 priv->last_wakeup + msecs_to_jiffies(30), jiffies);
+		msleep(30);
+	}
+
+	sdio_claim_host(func);
+
+	/* power save wakeup */
+	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+		if (txq_has_space(priv)) {
+			ks_wlan_hw_wakeup_request(priv);
+			queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+		}
+		goto release_host;
+	}
+
+	/* sleep mode doze */
+	if (atomic_read(&priv->sleepstatus.doze_request) == 1) {
+		ks_wlan_hw_sleep_doze_request(priv);
+		goto release_host;
+	}
+	/* sleep mode wakeup */
+	if (atomic_read(&priv->sleepstatus.wakeup_request) == 1) {
+		ks_wlan_hw_sleep_wakeup_request(priv);
+		goto release_host;
+	}
+
+	/* read (WriteStatus/ReadDataSize FN1:00_0014) */
+	ret = ks7010_sdio_readb(priv, WSTATUS_RSIZE_REG, &byte);
+	if (ret) {
+		netdev_err(priv->net_dev, "read WSTATUS_RSIZE_REG psstatus=%d\n",
+			   atomic_read(&priv->psstatus.status));
+		goto release_host;
+	}
+
+	if (byte & RSIZE_MASK) {	/* Read schedule */
+		ks_wlan_hw_rx(priv, (size_t)((byte & RSIZE_MASK) << 4));
+	}
+	if ((byte & WSTATUS_MASK))
+		tx_device_task(priv);
+
+	_ks_wlan_hw_power_save(priv);
+
+release_host:
+	sdio_release_host(func);
+}
+
+static void ks_sdio_interrupt(struct sdio_func *func)
+{
+	int ret;
+	struct ks_sdio_card *card;
+	struct ks_wlan_private *priv;
+	u8 status, rsize, byte;
+
+	card = sdio_get_drvdata(func);
+	priv = card->priv;
+
+	if (priv->dev_state < DEVICE_STATE_BOOT)
+		goto queue_delayed_work;
+
+	ret = ks7010_sdio_readb(priv, INT_PENDING_REG, &status);
+	if (ret) {
+		netdev_err(priv->net_dev, "read INT_PENDING_REG\n");
+		goto queue_delayed_work;
+	}
+
+	/* schedule task for interrupt status */
+	/* bit7 -> Write General Communication B register */
+	/* read (General Communication B register) */
+	/* bit5 -> Write Status Idle */
+	/* bit2 -> Read Status Busy  */
+	if (status & INT_GCR_B ||
+	    atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+		ret = ks7010_sdio_readb(priv, GCR_B_REG, &byte);
+		if (ret) {
+			netdev_err(priv->net_dev, "read GCR_B_REG\n");
+			goto queue_delayed_work;
+		}
+		if (byte == GCR_B_ACTIVE) {
+			if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+				atomic_set(&priv->psstatus.status, PS_WAKEUP);
+				priv->wakeup_count = 0;
+			}
+			complete(&priv->psstatus.wakeup_wait);
+		}
+	}
+
+	do {
+		/* read (WriteStatus/ReadDataSize FN1:00_0014) */
+		ret = ks7010_sdio_readb(priv, WSTATUS_RSIZE_REG, &byte);
+		if (ret) {
+			netdev_err(priv->net_dev, "read WSTATUS_RSIZE_REG\n");
+			goto queue_delayed_work;
+		}
+		rsize = byte & RSIZE_MASK;
+		if (rsize != 0)		/* Read schedule */
+			ks_wlan_hw_rx(priv, (size_t)(rsize << 4));
+
+		if (byte & WSTATUS_MASK) {
+			if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+				if (txq_has_space(priv)) {
+					ks_wlan_hw_wakeup_request(priv);
+					queue_delayed_work(priv->wq,
+							   &priv->rw_dwork, 1);
+					return;
+				}
+			} else {
+				tx_device_task(priv);
+			}
+		}
+	} while (rsize);
+
+queue_delayed_work:
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+}
+
+static int trx_device_init(struct ks_wlan_private *priv)
+{
+	priv->tx_dev.qhead = 0;
+	priv->tx_dev.qtail = 0;
+
+	priv->rx_dev.qhead = 0;
+	priv->rx_dev.qtail = 0;
+
+	spin_lock_init(&priv->tx_dev.tx_dev_lock);
+	spin_lock_init(&priv->rx_dev.rx_dev_lock);
+
+	tasklet_setup(&priv->rx_bh_task, rx_event_task);
+
+	return 0;
+}
+
+static void trx_device_exit(struct ks_wlan_private *priv)
+{
+	struct tx_device_buffer *sp;
+
+	/* tx buffer clear */
+	while (txq_has_space(priv)) {
+		sp = &priv->tx_dev.tx_dev_buff[priv->tx_dev.qhead];
+		kfree(sp->sendp);
+		if (sp->complete_handler)	/* TX Complete */
+			(*sp->complete_handler)(priv, sp->skb);
+		inc_txqhead(priv);
+	}
+
+	tasklet_kill(&priv->rx_bh_task);
+}
+
+static int ks7010_sdio_update_index(struct ks_wlan_private *priv, u32 index)
+{
+	int ret;
+	unsigned char *data_buf;
+
+	data_buf = kmemdup(&index, sizeof(u32), GFP_KERNEL);
+	if (!data_buf)
+		return -ENOMEM;
+
+	ret = ks7010_sdio_write(priv, WRITE_INDEX_REG, data_buf, sizeof(index));
+	if (ret)
+		goto err_free_data_buf;
+
+	ret = ks7010_sdio_write(priv, READ_INDEX_REG, data_buf, sizeof(index));
+	if (ret)
+		goto err_free_data_buf;
+
+	return 0;
+
+err_free_data_buf:
+	kfree(data_buf);
+
+	return ret;
+}
+
+#define ROM_BUFF_SIZE (64 * 1024)
+static int ks7010_sdio_data_compare(struct ks_wlan_private *priv, u32 address,
+				    u8 *data, unsigned int size)
+{
+	int ret;
+	u8 *read_buf;
+
+	read_buf = kmalloc(ROM_BUFF_SIZE, GFP_KERNEL);
+	if (!read_buf)
+		return -ENOMEM;
+
+	ret = ks7010_sdio_read(priv, address, read_buf, size);
+	if (ret)
+		goto err_free_read_buf;
+
+	if (memcmp(data, read_buf, size) != 0) {
+		ret = -EIO;
+		netdev_err(priv->net_dev, "data compare error (%d)\n", ret);
+		goto err_free_read_buf;
+	}
+
+	return 0;
+
+err_free_read_buf:
+	kfree(read_buf);
+
+	return ret;
+}
+
+static int ks7010_copy_firmware(struct ks_wlan_private *priv,
+				const struct firmware *fw_entry)
+{
+	unsigned int length;
+	unsigned int size;
+	unsigned int offset;
+	unsigned int n = 0;
+	u8 *rom_buf;
+	int ret;
+
+	rom_buf = kmalloc(ROM_BUFF_SIZE, GFP_KERNEL);
+	if (!rom_buf)
+		return -ENOMEM;
+
+	length = fw_entry->size;
+
+	do {
+		if (length >= ROM_BUFF_SIZE) {
+			size = ROM_BUFF_SIZE;
+			length = length - ROM_BUFF_SIZE;
+		} else {
+			size = length;
+			length = 0;
+		}
+		if (size == 0)
+			break;
+
+		memcpy(rom_buf, fw_entry->data + n, size);
+
+		offset = n;
+		ret = ks7010_sdio_update_index(priv,
+					       KS7010_IRAM_ADDRESS + offset);
+		if (ret)
+			goto free_rom_buf;
+
+		ret = ks7010_sdio_write(priv, DATA_WINDOW, rom_buf, size);
+		if (ret)
+			goto free_rom_buf;
+
+		ret = ks7010_sdio_data_compare(priv,
+					       DATA_WINDOW, rom_buf, size);
+		if (ret)
+			goto free_rom_buf;
+
+		n += size;
+
+	} while (size);
+
+	ret = ks7010_sdio_writeb(priv, GCR_A_REG, GCR_A_REMAP);
+
+free_rom_buf:
+	kfree(rom_buf);
+	return ret;
+}
+
+static int ks7010_upload_firmware(struct ks_sdio_card *card)
+{
+	struct ks_wlan_private *priv = card->priv;
+	struct sdio_func *func = ks7010_to_func(priv);
+	unsigned int n;
+	u8 byte = 0;
+	int ret;
+	const struct firmware *fw_entry = NULL;
+
+	sdio_claim_host(func);
+
+	/* Firmware running ? */
+	ret = ks7010_sdio_readb(priv, GCR_A_REG, &byte);
+	if (ret)
+		goto release_host;
+	if (byte == GCR_A_RUN) {
+		netdev_dbg(priv->net_dev, "MAC firmware running ...\n");
+		ret = -EBUSY;
+		goto release_host;
+	}
+
+	ret = request_firmware(&fw_entry, ROM_FILE,
+			       &func->dev);
+	if (ret)
+		goto release_host;
+
+	ret = ks7010_copy_firmware(priv, fw_entry);
+	if (ret)
+		goto release_firmware;
+
+	/* Firmware running check */
+	for (n = 0; n < 50; ++n) {
+		usleep_range(10000, 11000);	/* wait_ms(10); */
+		ret = ks7010_sdio_readb(priv, GCR_A_REG, &byte);
+		if (ret)
+			goto release_firmware;
+
+		if (byte == GCR_A_RUN)
+			break;
+	}
+	if ((50) <= n) {
+		netdev_err(priv->net_dev, "firmware can't start\n");
+		ret = -EIO;
+		goto release_firmware;
+	}
+
+	ret = 0;
+
+ release_firmware:
+	release_firmware(fw_entry);
+ release_host:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+static void ks7010_sme_enqueue_events(struct ks_wlan_private *priv)
+{
+	static const u16 init_events[] = {
+		SME_GET_EEPROM_CKSUM, SME_STOP_REQUEST,
+		SME_RTS_THRESHOLD_REQUEST, SME_FRAGMENTATION_THRESHOLD_REQUEST,
+		SME_WEP_INDEX_REQUEST, SME_WEP_KEY1_REQUEST,
+		SME_WEP_KEY2_REQUEST, SME_WEP_KEY3_REQUEST,
+		SME_WEP_KEY4_REQUEST, SME_WEP_FLAG_REQUEST,
+		SME_RSN_ENABLED_REQUEST, SME_MODE_SET_REQUEST,
+		SME_START_REQUEST
+	};
+	int ev;
+
+	for (ev = 0; ev < ARRAY_SIZE(init_events); ev++)
+		hostif_sme_enqueue(priv, init_events[ev]);
+}
+
+static void ks7010_card_init(struct ks_wlan_private *priv)
+{
+	init_completion(&priv->confirm_wait);
+
+	/* get mac address & firmware version */
+	hostif_sme_enqueue(priv, SME_START);
+
+	if (!wait_for_completion_interruptible_timeout
+	    (&priv->confirm_wait, 5 * HZ)) {
+		netdev_dbg(priv->net_dev, "wait time out!! SME_START\n");
+	}
+
+	if (priv->mac_address_valid && priv->version_size != 0)
+		priv->dev_state = DEVICE_STATE_PREINIT;
+
+	ks7010_sme_enqueue_events(priv);
+
+	if (!wait_for_completion_interruptible_timeout
+	    (&priv->confirm_wait, 5 * HZ)) {
+		netdev_dbg(priv->net_dev, "wait time out!! wireless parameter set\n");
+	}
+
+	if (priv->dev_state >= DEVICE_STATE_PREINIT) {
+		netdev_dbg(priv->net_dev, "DEVICE READY!!\n");
+		priv->dev_state = DEVICE_STATE_READY;
+	}
+}
+
+static void ks7010_init_defaults(struct ks_wlan_private *priv)
+{
+	priv->reg.tx_rate = TX_RATE_AUTO;
+	priv->reg.preamble = LONG_PREAMBLE;
+	priv->reg.power_mgmt = POWER_MGMT_ACTIVE;
+	priv->reg.scan_type = ACTIVE_SCAN;
+	priv->reg.beacon_lost_count = 20;
+	priv->reg.rts = 2347UL;
+	priv->reg.fragment = 2346UL;
+	priv->reg.phy_type = D_11BG_COMPATIBLE_MODE;
+	priv->reg.cts_mode = CTS_MODE_FALSE;
+	priv->reg.rate_set.body[11] = TX_RATE_54M;
+	priv->reg.rate_set.body[10] = TX_RATE_48M;
+	priv->reg.rate_set.body[9] = TX_RATE_36M;
+	priv->reg.rate_set.body[8] = TX_RATE_18M;
+	priv->reg.rate_set.body[7] = TX_RATE_9M;
+	priv->reg.rate_set.body[6] = TX_RATE_24M | BASIC_RATE;
+	priv->reg.rate_set.body[5] = TX_RATE_12M | BASIC_RATE;
+	priv->reg.rate_set.body[4] = TX_RATE_6M | BASIC_RATE;
+	priv->reg.rate_set.body[3] = TX_RATE_11M | BASIC_RATE;
+	priv->reg.rate_set.body[2] = TX_RATE_5M | BASIC_RATE;
+	priv->reg.rate_set.body[1] = TX_RATE_2M | BASIC_RATE;
+	priv->reg.rate_set.body[0] = TX_RATE_1M | BASIC_RATE;
+	priv->reg.tx_rate = TX_RATE_FULL_AUTO;
+	priv->reg.rate_set.size = 12;
+}
+
+static int ks7010_sdio_setup_irqs(struct sdio_func *func)
+{
+	int ret;
+
+	/* interrupt disable */
+	sdio_writeb(func, 0, INT_ENABLE_REG, &ret);
+	if (ret)
+		goto irq_error;
+
+	sdio_writeb(func, 0xff, INT_PENDING_REG, &ret);
+	if (ret)
+		goto irq_error;
+
+	/* setup interrupt handler */
+	ret = sdio_claim_irq(func, ks_sdio_interrupt);
+
+irq_error:
+	return ret;
+}
+
+static void ks7010_sdio_init_irqs(struct sdio_func *func,
+				  struct ks_wlan_private *priv)
+{
+	u8 byte;
+	int ret;
+
+	/*
+	 * interrupt setting
+	 * clear Interrupt status write
+	 * (ARMtoSD_InterruptPending FN1:00_0024)
+	 */
+	sdio_claim_host(func);
+	ret = ks7010_sdio_writeb(priv, INT_PENDING_REG, 0xff);
+	sdio_release_host(func);
+	if (ret)
+		netdev_err(priv->net_dev, "write INT_PENDING_REG\n");
+
+	/* enable ks7010sdio interrupt */
+	byte = (INT_GCR_B | INT_READ_STATUS | INT_WRITE_STATUS);
+	sdio_claim_host(func);
+	ret = ks7010_sdio_writeb(priv, INT_ENABLE_REG, byte);
+	sdio_release_host(func);
+	if (ret)
+		netdev_err(priv->net_dev, "write INT_ENABLE_REG\n");
+}
+
+static void ks7010_private_init(struct ks_wlan_private *priv,
+				struct ks_sdio_card *card,
+				struct net_device *netdev)
+{
+	/* private memory initialize */
+	priv->if_hw = card;
+
+	priv->dev_state = DEVICE_STATE_PREBOOT;
+	priv->net_dev = netdev;
+	priv->firmware_version[0] = '\0';
+	priv->version_size = 0;
+	priv->last_doze = jiffies;
+	priv->last_wakeup = jiffies;
+	memset(&priv->nstats, 0, sizeof(priv->nstats));
+	memset(&priv->wstats, 0, sizeof(priv->wstats));
+
+	/* sleep mode */
+	atomic_set(&priv->sleepstatus.status, 0);
+	atomic_set(&priv->sleepstatus.doze_request, 0);
+	atomic_set(&priv->sleepstatus.wakeup_request, 0);
+
+	trx_device_init(priv);
+	hostif_init(priv);
+	ks_wlan_net_start(netdev);
+	ks7010_init_defaults(priv);
+}
+
+static int ks7010_sdio_probe(struct sdio_func *func,
+			     const struct sdio_device_id *device)
+{
+	struct ks_wlan_private *priv = NULL;
+	struct net_device *netdev = NULL;
+	struct ks_sdio_card *card;
+	int ret;
+
+	card = kzalloc(sizeof(*card), GFP_KERNEL);
+	if (!card)
+		return -ENOMEM;
+
+	card->func = func;
+
+	sdio_claim_host(func);
+
+	ret = sdio_set_block_size(func, KS7010_IO_BLOCK_SIZE);
+	if (ret)
+		goto err_free_card;
+
+	dev_dbg(&card->func->dev, "multi_block=%d sdio_set_block_size()=%d %d\n",
+		func->card->cccr.multi_block, func->cur_blksize, ret);
+
+	ret = sdio_enable_func(func);
+	if (ret)
+		goto err_free_card;
+
+	ret = ks7010_sdio_setup_irqs(func);
+	if (ret)
+		goto err_disable_func;
+
+	sdio_release_host(func);
+
+	sdio_set_drvdata(func, card);
+
+	dev_dbg(&card->func->dev, "class = 0x%X, vendor = 0x%X, device = 0x%X\n",
+		func->class, func->vendor, func->device);
+
+	/* private memory allocate */
+	netdev = alloc_etherdev(sizeof(*priv));
+	if (!netdev) {
+		dev_err(&card->func->dev, "Unable to alloc new net device\n");
+		goto err_release_irq;
+	}
+
+	ret = dev_alloc_name(netdev, "wlan%d");
+	if (ret < 0) {
+		dev_err(&card->func->dev, "Couldn't get name!\n");
+		goto err_free_netdev;
+	}
+
+	priv = netdev_priv(netdev);
+
+	card->priv = priv;
+	SET_NETDEV_DEV(netdev, &card->func->dev);
+
+	ks7010_private_init(priv, card, netdev);
+
+	ret = ks7010_upload_firmware(card);
+	if (ret) {
+		netdev_err(priv->net_dev,
+			   "firmware load failed !! ret = %d\n", ret);
+		goto err_free_netdev;
+	}
+
+	ks7010_sdio_init_irqs(func, priv);
+
+	priv->dev_state = DEVICE_STATE_BOOT;
+
+	priv->wq = alloc_workqueue("wq", WQ_MEM_RECLAIM, 1);
+	if (!priv->wq) {
+		netdev_err(priv->net_dev, "create_workqueue failed !!\n");
+		goto err_free_netdev;
+	}
+
+	INIT_DELAYED_WORK(&priv->rw_dwork, ks7010_rw_function);
+	ks7010_card_init(priv);
+
+	ret = register_netdev(priv->net_dev);
+	if (ret)
+		goto err_destroy_wq;
+
+	return 0;
+
+ err_destroy_wq:
+	destroy_workqueue(priv->wq);
+ err_free_netdev:
+	free_netdev(netdev);
+ err_release_irq:
+	sdio_claim_host(func);
+	sdio_release_irq(func);
+ err_disable_func:
+	sdio_disable_func(func);
+ err_free_card:
+	sdio_release_host(func);
+	sdio_set_drvdata(func, NULL);
+	kfree(card);
+
+	return -ENODEV;
+}
+
+/* send stop request to MAC */
+static int send_stop_request(struct sdio_func *func)
+{
+	struct hostif_stop_request *pp;
+	struct ks_sdio_card *card;
+	size_t size;
+
+	card = sdio_get_drvdata(func);
+
+	pp = kzalloc(hif_align_size(sizeof(*pp)), GFP_KERNEL);
+	if (!pp)
+		return -ENOMEM;
+
+	size = sizeof(*pp) - sizeof(pp->header.size);
+	pp->header.size = cpu_to_le16(size);
+	pp->header.event = cpu_to_le16(HIF_STOP_REQ);
+
+	sdio_claim_host(func);
+	write_to_device(card->priv, (u8 *)pp, hif_align_size(sizeof(*pp)));
+	sdio_release_host(func);
+
+	kfree(pp);
+	return 0;
+}
+
+static void ks7010_sdio_remove(struct sdio_func *func)
+{
+	int ret;
+	struct ks_sdio_card *card;
+	struct ks_wlan_private *priv;
+
+	card = sdio_get_drvdata(func);
+
+	if (!card)
+		return;
+
+	priv = card->priv;
+	if (!priv)
+		goto err_free_card;
+
+	ks_wlan_net_stop(priv->net_dev);
+
+	/* interrupt disable */
+	sdio_claim_host(func);
+	sdio_writeb(func, 0, INT_ENABLE_REG, &ret);
+	sdio_writeb(func, 0xff, INT_PENDING_REG, &ret);
+	sdio_release_host(func);
+
+	ret = send_stop_request(func);
+	if (ret)	/* memory allocation failure */
+		goto err_free_card;
+
+	if (priv->wq)
+		destroy_workqueue(priv->wq);
+
+	hostif_exit(priv);
+
+	unregister_netdev(priv->net_dev);
+
+	trx_device_exit(priv);
+	free_netdev(priv->net_dev);
+	card->priv = NULL;
+
+	sdio_claim_host(func);
+	sdio_release_irq(func);
+	sdio_disable_func(func);
+	sdio_release_host(func);
+err_free_card:
+	sdio_set_drvdata(func, NULL);
+	kfree(card);
+}
+
+static const struct sdio_device_id ks7010_sdio_ids[] = {
+	{SDIO_DEVICE(SDIO_VENDOR_ID_KS_CODE_A, SDIO_DEVICE_ID_KS_7010)},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_KS_CODE_B, SDIO_DEVICE_ID_KS_7010)},
+	{ /* all zero */ }
+};
+MODULE_DEVICE_TABLE(sdio, ks7010_sdio_ids);
+
+static struct sdio_driver ks7010_sdio_driver = {
+	.name = "ks7010_sdio",
+	.id_table = ks7010_sdio_ids,
+	.probe = ks7010_sdio_probe,
+	.remove = ks7010_sdio_remove,
+};
+
+module_driver(ks7010_sdio_driver, sdio_register_driver, sdio_unregister_driver);
+MODULE_AUTHOR("Sang Engineering, Qi-Hardware, KeyStream");
+MODULE_DESCRIPTION("Driver for KeyStream KS7010 based SDIO cards");
+MODULE_LICENSE("GPL v2");
+MODULE_FIRMWARE(ROM_FILE);
diff --git a/drivers/staging/ks7010/ks_hostif.c b/drivers/staging/ks7010/ks_hostif.c
new file mode 100644
index 000000000..9429ee155
--- /dev/null
+++ b/drivers/staging/ks7010/ks_hostif.c
@@ -0,0 +1,2313 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *   Driver for KeyStream wireless LAN cards.
+ *
+ *   Copyright (C) 2005-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ */
+
+#include <crypto/hash.h>
+#include <linux/circ_buf.h>
+#include <linux/if_arp.h>
+#include <net/iw_handler.h>
+#include <uapi/linux/llc.h>
+#include "eap_packet.h"
+#include "ks_wlan.h"
+#include "ks_hostif.h"
+
+#define MICHAEL_MIC_KEY_LEN 8
+#define MICHAEL_MIC_LEN     8
+
+static inline void inc_smeqhead(struct ks_wlan_private *priv)
+{
+	priv->sme_i.qhead = (priv->sme_i.qhead + 1) % SME_EVENT_BUFF_SIZE;
+}
+
+static inline void inc_smeqtail(struct ks_wlan_private *priv)
+{
+	priv->sme_i.qtail = (priv->sme_i.qtail + 1) % SME_EVENT_BUFF_SIZE;
+}
+
+static inline unsigned int cnt_smeqbody(struct ks_wlan_private *priv)
+{
+	return CIRC_CNT_TO_END(priv->sme_i.qhead, priv->sme_i.qtail,
+			       SME_EVENT_BUFF_SIZE);
+}
+
+static inline u8 get_byte(struct ks_wlan_private *priv)
+{
+	u8 data;
+
+	data = *priv->rxp++;
+	/* length check in advance ! */
+	--(priv->rx_size);
+	return data;
+}
+
+static inline u16 get_word(struct ks_wlan_private *priv)
+{
+	u16 data;
+
+	data = (get_byte(priv) & 0xff);
+	data |= ((get_byte(priv) << 8) & 0xff00);
+	return data;
+}
+
+static inline u32 get_dword(struct ks_wlan_private *priv)
+{
+	u32 data;
+
+	data = (get_byte(priv) & 0xff);
+	data |= ((get_byte(priv) << 8) & 0x0000ff00);
+	data |= ((get_byte(priv) << 16) & 0x00ff0000);
+	data |= ((get_byte(priv) << 24) & 0xff000000);
+	return data;
+}
+
+static void ks_wlan_hw_wakeup_task(struct work_struct *work)
+{
+	struct ks_wlan_private *priv;
+	int ps_status;
+	long time_left;
+
+	priv = container_of(work, struct ks_wlan_private, wakeup_work);
+	ps_status = atomic_read(&priv->psstatus.status);
+
+	if (ps_status == PS_SNOOZE) {
+		ks_wlan_hw_wakeup_request(priv);
+		time_left = wait_for_completion_interruptible_timeout(
+				&priv->psstatus.wakeup_wait,
+				msecs_to_jiffies(20));
+		if (time_left <= 0) {
+			netdev_dbg(priv->net_dev, "wake up timeout or interrupted !!!\n");
+			schedule_work(&priv->wakeup_work);
+			return;
+		}
+	}
+}
+
+static void ks_wlan_do_power_save(struct ks_wlan_private *priv)
+{
+	if (is_connect_status(priv->connect_status))
+		hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
+	else
+		priv->dev_state = DEVICE_STATE_READY;
+}
+
+static
+int get_current_ap(struct ks_wlan_private *priv, struct link_ap_info *ap_info)
+{
+	struct local_ap *ap;
+	union iwreq_data wrqu;
+	struct net_device *netdev = priv->net_dev;
+	u8 size;
+
+	ap = &priv->current_ap;
+
+	if (is_disconnect_status(priv->connect_status)) {
+		memset(ap, 0, sizeof(struct local_ap));
+		return -EPERM;
+	}
+
+	ether_addr_copy(ap->bssid, ap_info->bssid);
+	memcpy(ap->ssid.body, priv->reg.ssid.body,
+	       priv->reg.ssid.size);
+	ap->ssid.size = priv->reg.ssid.size;
+	memcpy(ap->rate_set.body, ap_info->rate_set.body,
+	       ap_info->rate_set.size);
+	ap->rate_set.size = ap_info->rate_set.size;
+	if (ap_info->ext_rate_set.size != 0) {
+		memcpy(&ap->rate_set.body[ap->rate_set.size],
+		       ap_info->ext_rate_set.body,
+		       ap_info->ext_rate_set.size);
+		ap->rate_set.size += ap_info->ext_rate_set.size;
+	}
+	ap->channel = ap_info->ds_parameter.channel;
+	ap->rssi = ap_info->rssi;
+	ap->sq = ap_info->sq;
+	ap->noise = ap_info->noise;
+	ap->capability = le16_to_cpu(ap_info->capability);
+	size = (ap_info->rsn.size <= RSN_IE_BODY_MAX) ?
+		ap_info->rsn.size : RSN_IE_BODY_MAX;
+	if ((ap_info->rsn_mode & RSN_MODE_WPA2) &&
+	    (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)) {
+		ap->rsn_ie.id = RSN_INFO_ELEM_ID;
+		ap->rsn_ie.size = size;
+		memcpy(ap->rsn_ie.body, ap_info->rsn.body, size);
+	} else if ((ap_info->rsn_mode & RSN_MODE_WPA) &&
+		   (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA)) {
+		ap->wpa_ie.id = WPA_INFO_ELEM_ID;
+		ap->wpa_ie.size = size;
+		memcpy(ap->wpa_ie.body, ap_info->rsn.body, size);
+	} else {
+		ap->rsn_ie.id = 0;
+		ap->rsn_ie.size = 0;
+		ap->wpa_ie.id = 0;
+		ap->wpa_ie.size = 0;
+	}
+
+	wrqu.data.length = 0;
+	wrqu.data.flags = 0;
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	if (is_connect_status(priv->connect_status)) {
+		ether_addr_copy(wrqu.ap_addr.sa_data, priv->current_ap.bssid);
+		netdev_dbg(priv->net_dev,
+			   "IWEVENT: connect bssid=%pM\n",
+			   wrqu.ap_addr.sa_data);
+		wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
+	}
+	netdev_dbg(priv->net_dev, "Link AP\n"
+		   "- bssid=%pM\n"
+		   "- essid=%s\n"
+		   "- rate_set=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X\n"
+		   "- channel=%d\n"
+		   "- rssi=%d\n"
+		   "- sq=%d\n"
+		   "- capability=%04X\n"
+		   "- rsn.mode=%d\n"
+		   "- rsn.size=%d\n"
+		   "- ext_rate_set_size=%d\n"
+		   "- rate_set_size=%d\n",
+		   ap->bssid,
+		   &ap->ssid.body[0],
+		   ap->rate_set.body[0], ap->rate_set.body[1],
+		   ap->rate_set.body[2], ap->rate_set.body[3],
+		   ap->rate_set.body[4], ap->rate_set.body[5],
+		   ap->rate_set.body[6], ap->rate_set.body[7],
+		   ap->channel, ap->rssi, ap->sq, ap->capability,
+		   ap_info->rsn_mode, ap_info->rsn.size,
+		   ap_info->ext_rate_set.size, ap_info->rate_set.size);
+
+	return 0;
+}
+
+static u8 read_ie(unsigned char *bp, u8 max, u8 *body)
+{
+	u8 size = (*(bp + 1) <= max) ? *(bp + 1) : max;
+
+	memcpy(body, bp + 2, size);
+	return size;
+}
+
+static int
+michael_mic(u8 *key, u8 *data, unsigned int len, u8 priority, u8 *result)
+{
+	u8 pad_data[4] = { priority, 0, 0, 0 };
+	struct crypto_shash *tfm = NULL;
+	struct shash_desc *desc = NULL;
+	int ret;
+
+	tfm = crypto_alloc_shash("michael_mic", 0, 0);
+	if (IS_ERR(tfm)) {
+		ret = PTR_ERR(tfm);
+		goto err;
+	}
+
+	ret = crypto_shash_setkey(tfm, key, MICHAEL_MIC_KEY_LEN);
+	if (ret < 0)
+		goto err_free_tfm;
+
+	desc = kmalloc(sizeof(*desc) + crypto_shash_descsize(tfm), GFP_KERNEL);
+	if (!desc) {
+		ret = -ENOMEM;
+		goto err_free_tfm;
+	}
+
+	desc->tfm = tfm;
+
+	ret = crypto_shash_init(desc);
+	if (ret < 0)
+		goto err_free_desc;
+
+	// Compute the MIC value
+	/*
+	 * IEEE802.11i  page 47
+	 * Figure 43g TKIP MIC processing format
+	 * +--+--+--------+--+----+--+--+--+--+--+--+--+--+
+	 * |6 |6 |1       |3 |M   |1 |1 |1 |1 |1 |1 |1 |1 | Octet
+	 * +--+--+--------+--+----+--+--+--+--+--+--+--+--+
+	 * |DA|SA|Priority|0 |Data|M0|M1|M2|M3|M4|M5|M6|M7|
+	 * +--+--+--------+--+----+--+--+--+--+--+--+--+--+
+	 */
+
+	ret = crypto_shash_update(desc, data, 12);
+	if (ret < 0)
+		goto err_free_desc;
+
+	ret = crypto_shash_update(desc, pad_data, 4);
+	if (ret < 0)
+		goto err_free_desc;
+
+	ret = crypto_shash_finup(desc, data + 12, len - 12, result);
+
+err_free_desc:
+	kfree_sensitive(desc);
+
+err_free_tfm:
+	crypto_free_shash(tfm);
+
+err:
+	return ret;
+}
+
+static
+int get_ap_information(struct ks_wlan_private *priv, struct ap_info *ap_info,
+		       struct local_ap *ap)
+{
+	unsigned char *bp;
+	int bsize, offset;
+
+	memset(ap, 0, sizeof(struct local_ap));
+
+	ether_addr_copy(ap->bssid, ap_info->bssid);
+	ap->rssi = ap_info->rssi;
+	ap->sq = ap_info->sq;
+	ap->noise = ap_info->noise;
+	ap->capability = le16_to_cpu(ap_info->capability);
+	ap->channel = ap_info->ch_info;
+
+	bp = ap_info->body;
+	bsize = le16_to_cpu(ap_info->body_size);
+	offset = 0;
+
+	while (bsize > offset) {
+		switch (*bp) { /* Information Element ID */
+		case WLAN_EID_SSID:
+			ap->ssid.size = read_ie(bp, IEEE80211_MAX_SSID_LEN,
+						ap->ssid.body);
+			break;
+		case WLAN_EID_SUPP_RATES:
+		case WLAN_EID_EXT_SUPP_RATES:
+			if ((*(bp + 1) + ap->rate_set.size) <=
+			    RATE_SET_MAX_SIZE) {
+				memcpy(&ap->rate_set.body[ap->rate_set.size],
+				       bp + 2, *(bp + 1));
+				ap->rate_set.size += *(bp + 1);
+			} else {
+				memcpy(&ap->rate_set.body[ap->rate_set.size],
+				       bp + 2,
+				       RATE_SET_MAX_SIZE - ap->rate_set.size);
+				ap->rate_set.size +=
+				    (RATE_SET_MAX_SIZE - ap->rate_set.size);
+			}
+			break;
+		case WLAN_EID_RSN:
+			ap->rsn_ie.id = *bp;
+			ap->rsn_ie.size = read_ie(bp, RSN_IE_BODY_MAX,
+						  ap->rsn_ie.body);
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC: /* WPA */
+			/* WPA OUI check */
+			if (memcmp(bp + 2, CIPHER_ID_WPA_WEP40, 4) == 0) {
+				ap->wpa_ie.id = *bp;
+				ap->wpa_ie.size = read_ie(bp, RSN_IE_BODY_MAX,
+							  ap->wpa_ie.body);
+			}
+			break;
+		case WLAN_EID_DS_PARAMS:
+		case WLAN_EID_FH_PARAMS:
+		case WLAN_EID_CF_PARAMS:
+		case WLAN_EID_TIM:
+		case WLAN_EID_IBSS_PARAMS:
+		case WLAN_EID_COUNTRY:
+		case WLAN_EID_ERP_INFO:
+			break;
+		default:
+			netdev_err(priv->net_dev,
+				   "unknown Element ID=%d\n", *bp);
+			break;
+		}
+
+		offset += 2;	/* id & size field */
+		offset += *(bp + 1);	/* +size offset */
+		bp += (*(bp + 1) + 2);	/* pointer update */
+	}
+
+	return 0;
+}
+
+static
+int hostif_data_indication_wpa(struct ks_wlan_private *priv,
+			       unsigned short auth_type)
+{
+	struct ether_hdr *eth_hdr;
+	unsigned short eth_proto;
+	unsigned char recv_mic[MICHAEL_MIC_LEN];
+	char buf[128];
+	unsigned long now;
+	struct mic_failure *mic_failure;
+	u8 mic[MICHAEL_MIC_LEN];
+	union iwreq_data wrqu;
+	unsigned int key_index = auth_type - 1;
+	struct wpa_key *key = &priv->wpa.key[key_index];
+
+	eth_hdr = (struct ether_hdr *)(priv->rxp);
+	eth_proto = ntohs(eth_hdr->h_proto);
+
+	if (eth_hdr->h_dest_snap != eth_hdr->h_source_snap) {
+		netdev_err(priv->net_dev, "invalid data format\n");
+		priv->nstats.rx_errors++;
+		return -EINVAL;
+	}
+	if (((auth_type == TYPE_PMK1 &&
+	      priv->wpa.pairwise_suite == IW_AUTH_CIPHER_TKIP) ||
+	     (auth_type == TYPE_GMK1 &&
+	      priv->wpa.group_suite == IW_AUTH_CIPHER_TKIP) ||
+	     (auth_type == TYPE_GMK2 &&
+	      priv->wpa.group_suite == IW_AUTH_CIPHER_TKIP)) &&
+	    key->key_len) {
+		int ret;
+
+		netdev_dbg(priv->net_dev, "TKIP: protocol=%04X: size=%u\n",
+			   eth_proto, priv->rx_size);
+		/* MIC save */
+		memcpy(&recv_mic[0],
+		       (priv->rxp) + ((priv->rx_size) - sizeof(recv_mic)),
+		       sizeof(recv_mic));
+		priv->rx_size = priv->rx_size - sizeof(recv_mic);
+
+		ret = michael_mic(key->rx_mic_key, priv->rxp, priv->rx_size,
+				  0, mic);
+		if (ret < 0)
+			return ret;
+		if (memcmp(mic, recv_mic, sizeof(mic)) != 0) {
+			now = jiffies;
+			mic_failure = &priv->wpa.mic_failure;
+			/* MIC FAILURE */
+			if (mic_failure->last_failure_time &&
+			    (now - mic_failure->last_failure_time) / HZ >= 60) {
+				mic_failure->failure = 0;
+			}
+			netdev_err(priv->net_dev, "MIC FAILURE\n");
+			if (mic_failure->failure == 0) {
+				mic_failure->failure = 1;
+				mic_failure->counter = 0;
+			} else if (mic_failure->failure == 1) {
+				mic_failure->failure = 2;
+				mic_failure->counter =
+					(u16)((now - mic_failure->last_failure_time) / HZ);
+				/*  range 1-60 */
+				if (!mic_failure->counter)
+					mic_failure->counter = 1;
+			}
+			priv->wpa.mic_failure.last_failure_time = now;
+
+			/*  needed parameters: count, keyid, key type, TSC */
+			sprintf(buf,
+				"MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr=%pM)",
+				key_index,
+				eth_hdr->h_dest[0] & 0x01 ? "broad" : "uni",
+				eth_hdr->h_source);
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.length = strlen(buf);
+			wireless_send_event(priv->net_dev, IWEVCUSTOM, &wrqu,
+					    buf);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static
+void hostif_data_indication(struct ks_wlan_private *priv)
+{
+	unsigned int rx_ind_size;	/* indicate data size */
+	struct sk_buff *skb;
+	u16 auth_type;
+	unsigned char temp[256];
+	struct ether_hdr *eth_hdr;
+	struct ieee802_1x_hdr *aa1x_hdr;
+	size_t size;
+	int ret;
+
+	/* min length check */
+	if (priv->rx_size <= ETH_HLEN) {
+		priv->nstats.rx_errors++;
+		return;
+	}
+
+	auth_type = get_word(priv);	/* AuthType */
+	get_word(priv);	/* Reserve Area */
+
+	eth_hdr = (struct ether_hdr *)(priv->rxp);
+
+	/* source address check */
+	if (ether_addr_equal(&priv->eth_addr[0], eth_hdr->h_source)) {
+		netdev_err(priv->net_dev, "invalid : source is own mac address !!\n");
+		netdev_err(priv->net_dev, "eth_hdrernet->h_dest=%pM\n", eth_hdr->h_source);
+		priv->nstats.rx_errors++;
+		return;
+	}
+
+	/*  for WPA */
+	if (auth_type != TYPE_DATA && priv->wpa.rsn_enabled) {
+		ret = hostif_data_indication_wpa(priv, auth_type);
+		if (ret)
+			return;
+	}
+
+	if ((priv->connect_status & FORCE_DISCONNECT) ||
+	    priv->wpa.mic_failure.failure == 2) {
+		return;
+	}
+
+	/* check 13th byte at rx data */
+	switch (*(priv->rxp + 12)) {
+	case LLC_SAP_SNAP:
+		rx_ind_size = priv->rx_size - 6;
+		skb = dev_alloc_skb(rx_ind_size);
+		if (!skb) {
+			priv->nstats.rx_dropped++;
+			return;
+		}
+		netdev_dbg(priv->net_dev, "SNAP, rx_ind_size = %d\n",
+			   rx_ind_size);
+
+		size = ETH_ALEN * 2;
+		skb_put_data(skb, priv->rxp, size);
+
+		/* (SNAP+UI..) skip */
+
+		size = rx_ind_size - (ETH_ALEN * 2);
+		skb_put_data(skb, &eth_hdr->h_proto, size);
+
+		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + ETHER_HDR_SIZE);
+		break;
+	case LLC_SAP_NETBEUI:
+		rx_ind_size = (priv->rx_size + 2);
+		skb = dev_alloc_skb(rx_ind_size);
+		if (!skb) {
+			priv->nstats.rx_dropped++;
+			return;
+		}
+		netdev_dbg(priv->net_dev, "NETBEUI/NetBIOS rx_ind_size=%d\n",
+			   rx_ind_size);
+
+		/* 8802/FDDI MAC copy */
+		skb_put_data(skb, priv->rxp, 12);
+
+		/* NETBEUI size add */
+		temp[0] = (((rx_ind_size - 12) >> 8) & 0xff);
+		temp[1] = ((rx_ind_size - 12) & 0xff);
+		skb_put_data(skb, temp, 2);
+
+		/* copy after Type */
+		skb_put_data(skb, priv->rxp + 12, rx_ind_size - 14);
+
+		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + 14);
+		break;
+	default:	/* other rx data */
+		netdev_err(priv->net_dev, "invalid data format\n");
+		priv->nstats.rx_errors++;
+		return;
+	}
+
+	if (aa1x_hdr->type == IEEE802_1X_TYPE_EAPOL_KEY &&
+	    priv->wpa.rsn_enabled)
+		atomic_set(&priv->psstatus.snooze_guard, 1);
+
+	/* rx indication */
+	skb->dev = priv->net_dev;
+	skb->protocol = eth_type_trans(skb, skb->dev);
+	priv->nstats.rx_packets++;
+	priv->nstats.rx_bytes += rx_ind_size;
+	netif_rx(skb);
+}
+
+static
+void hostif_mib_get_confirm(struct ks_wlan_private *priv)
+{
+	struct net_device *dev = priv->net_dev;
+	u32 mib_status;
+	u32 mib_attribute;
+
+	mib_status = get_dword(priv);
+	mib_attribute = get_dword(priv);
+	get_word(priv); /* mib_val_size */
+	get_word(priv); /* mib_val_type */
+
+	if (mib_status) {
+		netdev_err(priv->net_dev, "attribute=%08X, status=%08X\n",
+			   mib_attribute, mib_status);
+		return;
+	}
+
+	switch (mib_attribute) {
+	case DOT11_MAC_ADDRESS:
+		hostif_sme_enqueue(priv, SME_GET_MAC_ADDRESS);
+		ether_addr_copy(priv->eth_addr, priv->rxp);
+		priv->mac_address_valid = true;
+		eth_hw_addr_set(dev, priv->eth_addr);
+		netdev_info(dev, "MAC ADDRESS = %pM\n", priv->eth_addr);
+		break;
+	case DOT11_PRODUCT_VERSION:
+		priv->version_size = priv->rx_size;
+		memcpy(priv->firmware_version, priv->rxp, priv->rx_size);
+		priv->firmware_version[priv->rx_size] = '\0';
+		netdev_info(dev, "firmware ver. = %s\n",
+			    priv->firmware_version);
+		hostif_sme_enqueue(priv, SME_GET_PRODUCT_VERSION);
+		/* wake_up_interruptible_all(&priv->confirm_wait); */
+		complete(&priv->confirm_wait);
+		break;
+	case LOCAL_GAIN:
+		memcpy(&priv->gain, priv->rxp, sizeof(priv->gain));
+		netdev_dbg(priv->net_dev, "tx_mode=%d, rx_mode=%d, tx_gain=%d, rx_gain=%d\n",
+			   priv->gain.tx_mode, priv->gain.rx_mode,
+			   priv->gain.tx_gain, priv->gain.rx_gain);
+		break;
+	case LOCAL_EEPROM_SUM:
+		memcpy(&priv->eeprom_sum, priv->rxp, sizeof(priv->eeprom_sum));
+		if (priv->eeprom_sum.type != 0 &&
+		    priv->eeprom_sum.type != 1) {
+			netdev_err(dev, "LOCAL_EEPROM_SUM error!\n");
+			return;
+		}
+		priv->eeprom_checksum = (priv->eeprom_sum.type == 0) ?
+					 EEPROM_CHECKSUM_NONE :
+					 (priv->eeprom_sum.result == 0) ?
+					 EEPROM_NG : EEPROM_OK;
+		break;
+	default:
+		netdev_err(priv->net_dev, "mib_attribute=%08x\n",
+			   (unsigned int)mib_attribute);
+		break;
+	}
+}
+
+static
+void hostif_mib_set_confirm(struct ks_wlan_private *priv)
+{
+	u32 mib_status;
+	u32 mib_attribute;
+
+	mib_status = get_dword(priv);
+	mib_attribute = get_dword(priv);
+
+	if (mib_status) {
+		/* in case of error */
+		netdev_err(priv->net_dev, "error :: attribute=%08X, status=%08X\n",
+			   mib_attribute, mib_status);
+	}
+
+	switch (mib_attribute) {
+	case DOT11_RTS_THRESHOLD:
+		hostif_sme_enqueue(priv, SME_RTS_THRESHOLD_CONFIRM);
+		break;
+	case DOT11_FRAGMENTATION_THRESHOLD:
+		hostif_sme_enqueue(priv, SME_FRAGMENTATION_THRESHOLD_CONFIRM);
+		break;
+	case DOT11_WEP_DEFAULT_KEY_ID:
+		if (!priv->wpa.wpa_enabled)
+			hostif_sme_enqueue(priv, SME_WEP_INDEX_CONFIRM);
+		break;
+	case DOT11_WEP_DEFAULT_KEY_VALUE1:
+		if (priv->wpa.rsn_enabled)
+			hostif_sme_enqueue(priv, SME_SET_PMK_TSC);
+		else
+			hostif_sme_enqueue(priv, SME_WEP_KEY1_CONFIRM);
+		break;
+	case DOT11_WEP_DEFAULT_KEY_VALUE2:
+		if (priv->wpa.rsn_enabled)
+			hostif_sme_enqueue(priv, SME_SET_GMK1_TSC);
+		else
+			hostif_sme_enqueue(priv, SME_WEP_KEY2_CONFIRM);
+		break;
+	case DOT11_WEP_DEFAULT_KEY_VALUE3:
+		if (priv->wpa.rsn_enabled)
+			hostif_sme_enqueue(priv, SME_SET_GMK2_TSC);
+		else
+			hostif_sme_enqueue(priv, SME_WEP_KEY3_CONFIRM);
+		break;
+	case DOT11_WEP_DEFAULT_KEY_VALUE4:
+		if (!priv->wpa.rsn_enabled)
+			hostif_sme_enqueue(priv, SME_WEP_KEY4_CONFIRM);
+		break;
+	case DOT11_PRIVACY_INVOKED:
+		if (!priv->wpa.rsn_enabled)
+			hostif_sme_enqueue(priv, SME_WEP_FLAG_CONFIRM);
+		break;
+	case DOT11_RSN_ENABLED:
+		hostif_sme_enqueue(priv, SME_RSN_ENABLED_CONFIRM);
+		break;
+	case LOCAL_RSN_MODE:
+		hostif_sme_enqueue(priv, SME_RSN_MODE_CONFIRM);
+		break;
+	case LOCAL_MULTICAST_ADDRESS:
+		hostif_sme_enqueue(priv, SME_MULTICAST_REQUEST);
+		break;
+	case LOCAL_MULTICAST_FILTER:
+		hostif_sme_enqueue(priv, SME_MULTICAST_CONFIRM);
+		break;
+	case LOCAL_CURRENTADDRESS:
+		priv->mac_address_valid = true;
+		break;
+	case DOT11_RSN_CONFIG_MULTICAST_CIPHER:
+		hostif_sme_enqueue(priv, SME_RSN_MCAST_CONFIRM);
+		break;
+	case DOT11_RSN_CONFIG_UNICAST_CIPHER:
+		hostif_sme_enqueue(priv, SME_RSN_UCAST_CONFIRM);
+		break;
+	case DOT11_RSN_CONFIG_AUTH_SUITE:
+		hostif_sme_enqueue(priv, SME_RSN_AUTH_CONFIRM);
+		break;
+	case DOT11_GMK1_TSC:
+		if (atomic_read(&priv->psstatus.snooze_guard))
+			atomic_set(&priv->psstatus.snooze_guard, 0);
+		break;
+	case DOT11_GMK2_TSC:
+		if (atomic_read(&priv->psstatus.snooze_guard))
+			atomic_set(&priv->psstatus.snooze_guard, 0);
+		break;
+	case DOT11_PMK_TSC:
+	case LOCAL_PMK:
+	case LOCAL_GAIN:
+	case LOCAL_WPS_ENABLE:
+	case LOCAL_WPS_PROBE_REQ:
+	case LOCAL_REGION:
+	default:
+		break;
+	}
+}
+
+static
+void hostif_power_mgmt_confirm(struct ks_wlan_private *priv)
+{
+	if (priv->reg.power_mgmt > POWER_MGMT_ACTIVE &&
+	    priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
+		atomic_set(&priv->psstatus.confirm_wait, 0);
+		priv->dev_state = DEVICE_STATE_SLEEP;
+		ks_wlan_hw_power_save(priv);
+	} else {
+		priv->dev_state = DEVICE_STATE_READY;
+	}
+}
+
+static
+void hostif_sleep_confirm(struct ks_wlan_private *priv)
+{
+	atomic_set(&priv->sleepstatus.doze_request, 1);
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+}
+
+static
+void hostif_start_confirm(struct ks_wlan_private *priv)
+{
+	union iwreq_data wrqu;
+
+	wrqu.data.length = 0;
+	wrqu.data.flags = 0;
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	if (is_connect_status(priv->connect_status)) {
+		eth_zero_addr(wrqu.ap_addr.sa_data);
+		wireless_send_event(priv->net_dev, SIOCGIWAP, &wrqu, NULL);
+	}
+	netdev_dbg(priv->net_dev, " scan_ind_count=%d\n", priv->scan_ind_count);
+	hostif_sme_enqueue(priv, SME_START_CONFIRM);
+}
+
+static
+void hostif_connect_indication(struct ks_wlan_private *priv)
+{
+	u16 connect_code;
+	unsigned int tmp = 0;
+	unsigned int old_status = priv->connect_status;
+	struct net_device *netdev = priv->net_dev;
+	union iwreq_data wrqu0;
+
+	connect_code = get_word(priv);
+
+	switch (connect_code) {
+	case RESULT_CONNECT:
+		if (!(priv->connect_status & FORCE_DISCONNECT))
+			netif_carrier_on(netdev);
+		tmp = FORCE_DISCONNECT & priv->connect_status;
+		priv->connect_status = tmp + CONNECT_STATUS;
+		break;
+	case RESULT_DISCONNECT:
+		netif_carrier_off(netdev);
+		tmp = FORCE_DISCONNECT & priv->connect_status;
+		priv->connect_status = tmp + DISCONNECT_STATUS;
+		break;
+	default:
+		netdev_dbg(priv->net_dev, "unknown connect_code=%d :: scan_ind_count=%d\n",
+			   connect_code, priv->scan_ind_count);
+		netif_carrier_off(netdev);
+		tmp = FORCE_DISCONNECT & priv->connect_status;
+		priv->connect_status = tmp + DISCONNECT_STATUS;
+		break;
+	}
+
+	get_current_ap(priv, (struct link_ap_info *)priv->rxp);
+	if (is_connect_status(priv->connect_status) &&
+	    is_disconnect_status(old_status)) {
+		/* for power save */
+		atomic_set(&priv->psstatus.snooze_guard, 0);
+		atomic_set(&priv->psstatus.confirm_wait, 0);
+	}
+	ks_wlan_do_power_save(priv);
+
+	wrqu0.data.length = 0;
+	wrqu0.data.flags = 0;
+	wrqu0.ap_addr.sa_family = ARPHRD_ETHER;
+	if (is_disconnect_status(priv->connect_status) &&
+	    is_connect_status(old_status)) {
+		eth_zero_addr(wrqu0.ap_addr.sa_data);
+		netdev_dbg(priv->net_dev, "disconnect :: scan_ind_count=%d\n",
+			   priv->scan_ind_count);
+		wireless_send_event(netdev, SIOCGIWAP, &wrqu0, NULL);
+	}
+	priv->scan_ind_count = 0;
+}
+
+static
+void hostif_scan_indication(struct ks_wlan_private *priv)
+{
+	int i;
+	struct ap_info *ap_info;
+
+	netdev_dbg(priv->net_dev,
+		   "scan_ind_count = %d\n", priv->scan_ind_count);
+	ap_info = (struct ap_info *)(priv->rxp);
+
+	if (priv->scan_ind_count) {
+		/* bssid check */
+		for (i = 0; i < priv->aplist.size; i++) {
+			u8 *bssid = priv->aplist.ap[i].bssid;
+
+			if (ether_addr_equal(ap_info->bssid, bssid))
+				continue;
+
+			if (ap_info->frame_type == IEEE80211_STYPE_PROBE_RESP)
+				get_ap_information(priv, ap_info,
+						   &priv->aplist.ap[i]);
+			return;
+		}
+	}
+	priv->scan_ind_count++;
+	if (priv->scan_ind_count < LOCAL_APLIST_MAX + 1) {
+		netdev_dbg(priv->net_dev, " scan_ind_count=%d :: aplist.size=%d\n",
+			   priv->scan_ind_count, priv->aplist.size);
+		get_ap_information(priv, (struct ap_info *)(priv->rxp),
+				   &priv->aplist.ap[priv->scan_ind_count - 1]);
+		priv->aplist.size = priv->scan_ind_count;
+	} else {
+		netdev_dbg(priv->net_dev, " count over :: scan_ind_count=%d\n",
+			   priv->scan_ind_count);
+	}
+}
+
+static
+void hostif_stop_confirm(struct ks_wlan_private *priv)
+{
+	unsigned int tmp = 0;
+	unsigned int old_status = priv->connect_status;
+	struct net_device *netdev = priv->net_dev;
+	union iwreq_data wrqu0;
+
+	if (priv->dev_state == DEVICE_STATE_SLEEP)
+		priv->dev_state = DEVICE_STATE_READY;
+
+	/* disconnect indication */
+	if (is_connect_status(priv->connect_status)) {
+		netif_carrier_off(netdev);
+		tmp = FORCE_DISCONNECT & priv->connect_status;
+		priv->connect_status = tmp | DISCONNECT_STATUS;
+		netdev_info(netdev, "IWEVENT: disconnect\n");
+
+		wrqu0.data.length = 0;
+		wrqu0.data.flags = 0;
+		wrqu0.ap_addr.sa_family = ARPHRD_ETHER;
+		if (is_disconnect_status(priv->connect_status) &&
+		    is_connect_status(old_status)) {
+			eth_zero_addr(wrqu0.ap_addr.sa_data);
+			netdev_info(netdev, "IWEVENT: disconnect\n");
+			wireless_send_event(netdev, SIOCGIWAP, &wrqu0, NULL);
+		}
+		priv->scan_ind_count = 0;
+	}
+
+	hostif_sme_enqueue(priv, SME_STOP_CONFIRM);
+}
+
+static
+void hostif_ps_adhoc_set_confirm(struct ks_wlan_private *priv)
+{
+	priv->infra_status = 0;	/* infrastructure mode cancel */
+	hostif_sme_enqueue(priv, SME_MODE_SET_CONFIRM);
+}
+
+static
+void hostif_infrastructure_set_confirm(struct ks_wlan_private *priv)
+{
+	get_word(priv); /* result_code */
+	priv->infra_status = 1;	/* infrastructure mode set */
+	hostif_sme_enqueue(priv, SME_MODE_SET_CONFIRM);
+}
+
+static
+void hostif_adhoc_set_confirm(struct ks_wlan_private *priv)
+{
+	priv->infra_status = 1;	/* infrastructure mode set */
+	hostif_sme_enqueue(priv, SME_MODE_SET_CONFIRM);
+}
+
+static
+void hostif_associate_indication(struct ks_wlan_private *priv)
+{
+	struct association_request *assoc_req;
+	struct association_response *assoc_resp;
+	unsigned char *pb;
+	union iwreq_data wrqu;
+	char buf[IW_CUSTOM_MAX];
+	char *pbuf = &buf[0];
+	int i;
+
+	static const char associnfo_leader0[] = "ASSOCINFO(ReqIEs=";
+	static const char associnfo_leader1[] = " RespIEs=";
+
+	assoc_req = (struct association_request *)(priv->rxp);
+	assoc_resp = (struct association_response *)(assoc_req + 1);
+	pb = (unsigned char *)(assoc_resp + 1);
+
+	memset(&wrqu, 0, sizeof(wrqu));
+	memcpy(pbuf, associnfo_leader0, sizeof(associnfo_leader0) - 1);
+	wrqu.data.length += sizeof(associnfo_leader0) - 1;
+	pbuf += sizeof(associnfo_leader0) - 1;
+
+	for (i = 0; i < le16_to_cpu(assoc_req->req_ies_size); i++)
+		pbuf += sprintf(pbuf, "%02x", *(pb + i));
+	wrqu.data.length += (le16_to_cpu(assoc_req->req_ies_size)) * 2;
+
+	memcpy(pbuf, associnfo_leader1, sizeof(associnfo_leader1) - 1);
+	wrqu.data.length += sizeof(associnfo_leader1) - 1;
+	pbuf += sizeof(associnfo_leader1) - 1;
+
+	pb += le16_to_cpu(assoc_req->req_ies_size);
+	for (i = 0; i < le16_to_cpu(assoc_resp->resp_ies_size); i++)
+		pbuf += sprintf(pbuf, "%02x", *(pb + i));
+	wrqu.data.length += (le16_to_cpu(assoc_resp->resp_ies_size)) * 2;
+
+	pbuf += sprintf(pbuf, ")");
+	wrqu.data.length += 1;
+
+	wireless_send_event(priv->net_dev, IWEVCUSTOM, &wrqu, buf);
+}
+
+static
+void hostif_bss_scan_confirm(struct ks_wlan_private *priv)
+{
+	u32 result_code;
+	struct net_device *dev = priv->net_dev;
+	union iwreq_data wrqu;
+
+	result_code = get_dword(priv);
+	netdev_dbg(priv->net_dev, "result=%d :: scan_ind_count=%d\n",
+		   result_code, priv->scan_ind_count);
+
+	priv->sme_i.sme_flag &= ~SME_AP_SCAN;
+	hostif_sme_enqueue(priv, SME_BSS_SCAN_CONFIRM);
+
+	wrqu.data.length = 0;
+	wrqu.data.flags = 0;
+	wireless_send_event(dev, SIOCGIWSCAN, &wrqu, NULL);
+	priv->scan_ind_count = 0;
+}
+
+static
+void hostif_phy_information_confirm(struct ks_wlan_private *priv)
+{
+	struct iw_statistics *wstats = &priv->wstats;
+	u8 rssi, signal;
+	u8 link_speed;
+	u32 transmitted_frame_count, received_fragment_count;
+	u32 failed_count, fcs_error_count;
+
+	rssi = get_byte(priv);
+	signal = get_byte(priv);
+	get_byte(priv); /* noise */
+	link_speed = get_byte(priv);
+	transmitted_frame_count = get_dword(priv);
+	received_fragment_count = get_dword(priv);
+	failed_count = get_dword(priv);
+	fcs_error_count = get_dword(priv);
+
+	netdev_dbg(priv->net_dev, "phyinfo confirm rssi=%d signal=%d\n",
+		   rssi, signal);
+	priv->current_rate = (link_speed & RATE_MASK);
+	wstats->qual.qual = signal;
+	wstats->qual.level = 256 - rssi;
+	wstats->qual.noise = 0;	/* invalid noise value */
+	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+
+	netdev_dbg(priv->net_dev, "\n    rssi=%u\n"
+		   "    signal=%u\n"
+		   "    link_speed=%ux500Kbps\n"
+		   "    transmitted_frame_count=%u\n"
+		   "    received_fragment_count=%u\n"
+		   "    failed_count=%u\n"
+		   "    fcs_error_count=%u\n",
+		   rssi, signal, link_speed, transmitted_frame_count,
+		   received_fragment_count, failed_count, fcs_error_count);
+	/* wake_up_interruptible_all(&priv->confirm_wait); */
+	complete(&priv->confirm_wait);
+}
+
+static
+void hostif_mic_failure_confirm(struct ks_wlan_private *priv)
+{
+	netdev_dbg(priv->net_dev, "mic_failure=%u\n",
+		   priv->wpa.mic_failure.failure);
+	hostif_sme_enqueue(priv, SME_MIC_FAILURE_CONFIRM);
+}
+
+static
+void hostif_event_check(struct ks_wlan_private *priv)
+{
+	u16 event;
+
+	event = get_word(priv);
+	switch (event) {
+	case HIF_DATA_IND:
+		hostif_data_indication(priv);
+		break;
+	case HIF_MIB_GET_CONF:
+		hostif_mib_get_confirm(priv);
+		break;
+	case HIF_MIB_SET_CONF:
+		hostif_mib_set_confirm(priv);
+		break;
+	case HIF_POWER_MGMT_CONF:
+		hostif_power_mgmt_confirm(priv);
+		break;
+	case HIF_SLEEP_CONF:
+		hostif_sleep_confirm(priv);
+		break;
+	case HIF_START_CONF:
+		hostif_start_confirm(priv);
+		break;
+	case HIF_CONNECT_IND:
+		hostif_connect_indication(priv);
+		break;
+	case HIF_STOP_CONF:
+		hostif_stop_confirm(priv);
+		break;
+	case HIF_PS_ADH_SET_CONF:
+		hostif_ps_adhoc_set_confirm(priv);
+		break;
+	case HIF_INFRA_SET_CONF:
+	case HIF_INFRA_SET2_CONF:
+		hostif_infrastructure_set_confirm(priv);
+		break;
+	case HIF_ADH_SET_CONF:
+	case HIF_ADH_SET2_CONF:
+		hostif_adhoc_set_confirm(priv);
+		break;
+	case HIF_ASSOC_INFO_IND:
+		hostif_associate_indication(priv);
+		break;
+	case HIF_MIC_FAILURE_CONF:
+		hostif_mic_failure_confirm(priv);
+		break;
+	case HIF_SCAN_CONF:
+		hostif_bss_scan_confirm(priv);
+		break;
+	case HIF_PHY_INFO_CONF:
+	case HIF_PHY_INFO_IND:
+		hostif_phy_information_confirm(priv);
+		break;
+	case HIF_SCAN_IND:
+		hostif_scan_indication(priv);
+		break;
+	case HIF_AP_SET_CONF:
+	default:
+		netdev_err(priv->net_dev, "undefined event[%04X]\n", event);
+		/* wake_up_all(&priv->confirm_wait); */
+		complete(&priv->confirm_wait);
+		break;
+	}
+
+	/* add event to hostt buffer */
+	priv->hostt.buff[priv->hostt.qtail] = event;
+	priv->hostt.qtail = (priv->hostt.qtail + 1) % SME_EVENT_BUFF_SIZE;
+}
+
+/* allocate size bytes, set header size and event */
+static void *hostif_generic_request(size_t size, int event)
+{
+	struct hostif_hdr *p;
+
+	p = kzalloc(hif_align_size(size), GFP_ATOMIC);
+	if (!p)
+		return NULL;
+
+	p->size = cpu_to_le16(size - sizeof(p->size));
+	p->event = cpu_to_le16(event);
+
+	return p;
+}
+
+int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *skb)
+{
+	unsigned int skb_len = 0;
+	unsigned char *buffer = NULL;
+	unsigned int length = 0;
+	struct hostif_data_request *pp;
+	unsigned char *p;
+	unsigned short eth_proto;
+	struct ether_hdr *eth_hdr;
+	unsigned short keyinfo = 0;
+	struct ieee802_1x_hdr *aa1x_hdr;
+	struct wpa_eapol_key *eap_key;
+	struct ethhdr *eth;
+	size_t size;
+	int ret;
+
+	skb_len = skb->len;
+	if (skb_len > ETH_FRAME_LEN) {
+		netdev_err(priv->net_dev, "bad length skb_len=%d\n", skb_len);
+		ret = -EOVERFLOW;
+		goto err_kfree_skb;
+	}
+
+	if (is_disconnect_status(priv->connect_status) ||
+	    (priv->connect_status & FORCE_DISCONNECT) ||
+	    priv->wpa.mic_failure.stop) {
+		if (netif_queue_stopped(priv->net_dev))
+			netif_wake_queue(priv->net_dev);
+
+		dev_kfree_skb(skb);
+
+		return 0;
+	}
+
+	/* power save wakeup */
+	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+		if (!netif_queue_stopped(priv->net_dev))
+			netif_stop_queue(priv->net_dev);
+	}
+
+	size = sizeof(*pp) + 6 + skb_len + 8;
+	pp = kmalloc(hif_align_size(size), GFP_ATOMIC);
+	if (!pp) {
+		ret = -ENOMEM;
+		goto err_kfree_skb;
+	}
+
+	p = (unsigned char *)pp->data;
+
+	buffer = skb->data;
+	length = skb->len;
+
+	/* skb check */
+	eth = (struct ethhdr *)skb->data;
+	if (!ether_addr_equal(&priv->eth_addr[0], eth->h_source)) {
+		netdev_err(priv->net_dev,
+			   "Invalid mac address: ethernet->h_source=%pM\n",
+			   eth->h_source);
+		ret = -ENXIO;
+		goto err_kfree;
+	}
+
+	/* dest and src MAC address copy */
+	size = ETH_ALEN * 2;
+	memcpy(p, buffer, size);
+	p += size;
+	buffer += size;
+	length -= size;
+
+	/* EtherType/Length check */
+	if (*(buffer + 1) + (*buffer << 8) > 1500) {
+		/* ProtocolEAP = *(buffer+1) + (*buffer << 8); */
+		/* SAP/CTL/OUI(6 byte) add */
+		*p++ = 0xAA;	/* DSAP */
+		*p++ = 0xAA;	/* SSAP */
+		*p++ = 0x03;	/* CTL */
+		*p++ = 0x00;	/* OUI ("000000") */
+		*p++ = 0x00;	/* OUI ("000000") */
+		*p++ = 0x00;	/* OUI ("000000") */
+		skb_len += 6;
+	} else {
+		/* Length(2 byte) delete */
+		buffer += 2;
+		length -= 2;
+		skb_len -= 2;
+	}
+
+	/* pp->data copy */
+	memcpy(p, buffer, length);
+
+	p += length;
+
+	/* for WPA */
+	eth_hdr = (struct ether_hdr *)&pp->data[0];
+	eth_proto = ntohs(eth_hdr->h_proto);
+
+	/* for MIC FAILURE REPORT check */
+	if (eth_proto == ETH_P_PAE &&
+	    priv->wpa.mic_failure.failure > 0) {
+		aa1x_hdr = (struct ieee802_1x_hdr *)(eth_hdr + 1);
+		if (aa1x_hdr->type == IEEE802_1X_TYPE_EAPOL_KEY) {
+			eap_key = (struct wpa_eapol_key *)(aa1x_hdr + 1);
+			keyinfo = ntohs(eap_key->key_info);
+		}
+	}
+
+	if (priv->wpa.rsn_enabled && priv->wpa.key[0].key_len) {
+		/* no encryption */
+		if (eth_proto == ETH_P_PAE &&
+		    priv->wpa.key[1].key_len == 0 &&
+		    priv->wpa.key[2].key_len == 0 &&
+		    priv->wpa.key[3].key_len == 0) {
+			pp->auth_type = cpu_to_le16(TYPE_AUTH);
+		} else {
+			if (priv->wpa.pairwise_suite == IW_AUTH_CIPHER_TKIP) {
+				u8 mic[MICHAEL_MIC_LEN];
+
+				ret = michael_mic(priv->wpa.key[0].tx_mic_key,
+						  &pp->data[0], skb_len,
+						  0, mic);
+				if (ret < 0)
+					goto err_kfree;
+
+				memcpy(p, mic, sizeof(mic));
+				length += sizeof(mic);
+				skb_len += sizeof(mic);
+				p += sizeof(mic);
+				pp->auth_type =
+				    cpu_to_le16(TYPE_DATA);
+			} else if (priv->wpa.pairwise_suite ==
+				   IW_AUTH_CIPHER_CCMP) {
+				pp->auth_type =
+				    cpu_to_le16(TYPE_DATA);
+			}
+		}
+	} else {
+		if (eth_proto == ETH_P_PAE)
+			pp->auth_type = cpu_to_le16(TYPE_AUTH);
+		else
+			pp->auth_type = cpu_to_le16(TYPE_DATA);
+	}
+
+	/* header value set */
+	pp->header.size =
+	    cpu_to_le16((sizeof(*pp) - sizeof(pp->header.size) + skb_len));
+	pp->header.event = cpu_to_le16(HIF_DATA_REQ);
+
+	/* tx request */
+	ret = ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp) + skb_len),
+			    send_packet_complete, skb);
+
+	/* MIC FAILURE REPORT check */
+	if (eth_proto == ETH_P_PAE &&
+	    priv->wpa.mic_failure.failure > 0) {
+		if (keyinfo & WPA_KEY_INFO_ERROR &&
+		    keyinfo & WPA_KEY_INFO_REQUEST) {
+			netdev_err(priv->net_dev,
+				   "MIC ERROR Report SET : %04X\n", keyinfo);
+			hostif_sme_enqueue(priv, SME_MIC_FAILURE_REQUEST);
+		}
+		if (priv->wpa.mic_failure.failure == 2)
+			priv->wpa.mic_failure.stop = 1;
+	}
+
+	return ret;
+
+err_kfree:
+	kfree(pp);
+err_kfree_skb:
+	dev_kfree_skb(skb);
+
+	return ret;
+}
+
+static inline void ps_confirm_wait_inc(struct ks_wlan_private *priv)
+{
+	if (atomic_read(&priv->psstatus.status) > PS_ACTIVE_SET)
+		atomic_inc(&priv->psstatus.confirm_wait);
+}
+
+static inline void send_request_to_device(struct ks_wlan_private *priv,
+					  void *data, size_t size)
+{
+	ps_confirm_wait_inc(priv);
+	ks_wlan_hw_tx(priv, data, size, NULL, NULL);
+}
+
+static void hostif_mib_get_request(struct ks_wlan_private *priv,
+				   u32 mib_attribute)
+{
+	struct hostif_mib_get_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIB_GET_REQ);
+	if (!pp)
+		return;
+
+	pp->mib_attribute = cpu_to_le32(mib_attribute);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static void hostif_mib_set_request(struct ks_wlan_private *priv,
+				   enum mib_attribute attr,
+				   enum mib_data_type type,
+				   void *data, size_t size)
+{
+	struct hostif_mib_set_request_t *pp;
+
+	if (priv->dev_state < DEVICE_STATE_BOOT)
+		return;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIB_SET_REQ);
+	if (!pp)
+		return;
+
+	pp->mib_attribute = cpu_to_le32(attr);
+	pp->mib_value.size = cpu_to_le16(size);
+	pp->mib_value.type = cpu_to_le16(type);
+	memcpy(&pp->mib_value.body, data, size);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp) + size));
+}
+
+static inline void hostif_mib_set_request_int(struct ks_wlan_private *priv,
+					      enum mib_attribute attr, int val)
+{
+	__le32 v = cpu_to_le32(val);
+	size_t size = sizeof(v);
+
+	hostif_mib_set_request(priv, attr, MIB_VALUE_TYPE_INT, &v, size);
+}
+
+static inline void hostif_mib_set_request_bool(struct ks_wlan_private *priv,
+					       enum mib_attribute attr,
+					       bool val)
+{
+	__le32 v = cpu_to_le32(val);
+	size_t size = sizeof(v);
+
+	hostif_mib_set_request(priv, attr, MIB_VALUE_TYPE_BOOL, &v, size);
+}
+
+static inline void hostif_mib_set_request_ostring(struct ks_wlan_private *priv,
+						  enum mib_attribute attr,
+						  void *data, size_t size)
+{
+	hostif_mib_set_request(priv, attr, MIB_VALUE_TYPE_OSTRING, data, size);
+}
+
+static
+void hostif_start_request(struct ks_wlan_private *priv, unsigned char mode)
+{
+	struct hostif_start_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_START_REQ);
+	if (!pp)
+		return;
+
+	pp->mode = cpu_to_le16(mode);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+
+	priv->aplist.size = 0;
+	priv->scan_ind_count = 0;
+}
+
+static __le16 ks_wlan_cap(struct ks_wlan_private *priv)
+{
+	u16 capability = 0x0000;
+
+	if (priv->reg.preamble == SHORT_PREAMBLE)
+		capability |= WLAN_CAPABILITY_SHORT_PREAMBLE;
+
+	capability &= ~(WLAN_CAPABILITY_PBCC);	/* pbcc not support */
+
+	if (priv->reg.phy_type != D_11B_ONLY_MODE) {
+		capability |= WLAN_CAPABILITY_SHORT_SLOT_TIME;
+		capability &= ~(WLAN_CAPABILITY_DSSS_OFDM);
+	}
+
+	return cpu_to_le16(capability);
+}
+
+static void init_request(struct ks_wlan_private *priv,
+			 struct hostif_request *req)
+{
+	req->phy_type = cpu_to_le16(priv->reg.phy_type);
+	req->cts_mode = cpu_to_le16(priv->reg.cts_mode);
+	req->scan_type = cpu_to_le16(priv->reg.scan_type);
+	req->rate_set.size = priv->reg.rate_set.size;
+	req->capability = ks_wlan_cap(priv);
+	memcpy(&req->rate_set.body[0], &priv->reg.rate_set.body[0],
+	       priv->reg.rate_set.size);
+}
+
+static
+void hostif_ps_adhoc_set_request(struct ks_wlan_private *priv)
+{
+	struct hostif_ps_adhoc_set_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_PS_ADH_SET_REQ);
+	if (!pp)
+		return;
+
+	init_request(priv, &pp->request);
+	pp->channel = cpu_to_le16(priv->reg.channel);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_infrastructure_set_request(struct ks_wlan_private *priv, int event)
+{
+	struct hostif_infrastructure_set_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), event);
+	if (!pp)
+		return;
+
+	init_request(priv, &pp->request);
+	pp->ssid.size = priv->reg.ssid.size;
+	memcpy(&pp->ssid.body[0], &priv->reg.ssid.body[0], priv->reg.ssid.size);
+	pp->beacon_lost_count =
+	    cpu_to_le16(priv->reg.beacon_lost_count);
+	pp->auth_type = cpu_to_le16(priv->reg.authenticate_type);
+
+	pp->channel_list.body[0] = 1;
+	pp->channel_list.body[1] = 8;
+	pp->channel_list.body[2] = 2;
+	pp->channel_list.body[3] = 9;
+	pp->channel_list.body[4] = 3;
+	pp->channel_list.body[5] = 10;
+	pp->channel_list.body[6] = 4;
+	pp->channel_list.body[7] = 11;
+	pp->channel_list.body[8] = 5;
+	pp->channel_list.body[9] = 12;
+	pp->channel_list.body[10] = 6;
+	pp->channel_list.body[11] = 13;
+	pp->channel_list.body[12] = 7;
+	if (priv->reg.phy_type == D_11G_ONLY_MODE) {
+		pp->channel_list.size = 13;
+	} else {
+		pp->channel_list.body[13] = 14;
+		pp->channel_list.size = 14;
+	}
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_adhoc_set_request(struct ks_wlan_private *priv)
+{
+	struct hostif_adhoc_set_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_ADH_SET_REQ);
+	if (!pp)
+		return;
+
+	init_request(priv, &pp->request);
+	pp->channel = cpu_to_le16(priv->reg.channel);
+	pp->ssid.size = priv->reg.ssid.size;
+	memcpy(&pp->ssid.body[0], &priv->reg.ssid.body[0], priv->reg.ssid.size);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_adhoc_set2_request(struct ks_wlan_private *priv)
+{
+	struct hostif_adhoc_set2_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_ADH_SET_REQ);
+	if (!pp)
+		return;
+
+	init_request(priv, &pp->request);
+	pp->ssid.size = priv->reg.ssid.size;
+	memcpy(&pp->ssid.body[0], &priv->reg.ssid.body[0], priv->reg.ssid.size);
+
+	pp->channel_list.body[0] = priv->reg.channel;
+	pp->channel_list.size = 1;
+	memcpy(pp->bssid, priv->reg.bssid, ETH_ALEN);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_stop_request(struct ks_wlan_private *priv)
+{
+	struct hostif_stop_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_STOP_REQ);
+	if (!pp)
+		return;
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_phy_information_request(struct ks_wlan_private *priv)
+{
+	struct hostif_phy_information_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_PHY_INFO_REQ);
+	if (!pp)
+		return;
+
+	if (priv->reg.phy_info_timer) {
+		pp->type = cpu_to_le16(TIME_TYPE);
+		pp->time = cpu_to_le16(priv->reg.phy_info_timer);
+	} else {
+		pp->type = cpu_to_le16(NORMAL_TYPE);
+		pp->time = cpu_to_le16(0);
+	}
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_power_mgmt_request(struct ks_wlan_private *priv,
+			       u32 mode, u32 wake_up, u32 receive_dtims)
+{
+	struct hostif_power_mgmt_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_POWER_MGMT_REQ);
+	if (!pp)
+		return;
+
+	pp->mode = cpu_to_le32(mode);
+	pp->wake_up = cpu_to_le32(wake_up);
+	pp->receive_dtims = cpu_to_le32(receive_dtims);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+static
+void hostif_sleep_request(struct ks_wlan_private *priv,
+			  enum sleep_mode_type mode)
+{
+	struct hostif_sleep_request *pp;
+
+	if (mode == SLP_SLEEP) {
+		pp = hostif_generic_request(sizeof(*pp), HIF_SLEEP_REQ);
+		if (!pp)
+			return;
+
+		send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+	} else if (mode == SLP_ACTIVE) {
+		atomic_set(&priv->sleepstatus.wakeup_request, 1);
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+	} else {
+		netdev_err(priv->net_dev, "invalid mode %ld\n", (long)mode);
+		return;
+	}
+}
+
+static
+void hostif_bss_scan_request(struct ks_wlan_private *priv,
+			     unsigned long scan_type, u8 *scan_ssid,
+			     u8 scan_ssid_len)
+{
+	struct hostif_bss_scan_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_SCAN_REQ);
+	if (!pp)
+		return;
+
+	pp->scan_type = scan_type;
+
+	pp->ch_time_min = cpu_to_le32(110);	/* default value */
+	pp->ch_time_max = cpu_to_le32(130);	/* default value */
+	pp->channel_list.body[0] = 1;
+	pp->channel_list.body[1] = 8;
+	pp->channel_list.body[2] = 2;
+	pp->channel_list.body[3] = 9;
+	pp->channel_list.body[4] = 3;
+	pp->channel_list.body[5] = 10;
+	pp->channel_list.body[6] = 4;
+	pp->channel_list.body[7] = 11;
+	pp->channel_list.body[8] = 5;
+	pp->channel_list.body[9] = 12;
+	pp->channel_list.body[10] = 6;
+	pp->channel_list.body[11] = 13;
+	pp->channel_list.body[12] = 7;
+	if (priv->reg.phy_type == D_11G_ONLY_MODE) {
+		pp->channel_list.size = 13;
+	} else {
+		pp->channel_list.body[13] = 14;
+		pp->channel_list.size = 14;
+	}
+	pp->ssid.size = 0;
+
+	/* specified SSID SCAN */
+	if (scan_ssid_len > 0 && scan_ssid_len <= 32) {
+		pp->ssid.size = scan_ssid_len;
+		memcpy(&pp->ssid.body[0], scan_ssid, scan_ssid_len);
+	}
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+
+	priv->aplist.size = 0;
+	priv->scan_ind_count = 0;
+}
+
+static
+void hostif_mic_failure_request(struct ks_wlan_private *priv,
+				u16 failure_count, u16 timer)
+{
+	struct hostif_mic_failure_request *pp;
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIC_FAILURE_REQ);
+	if (!pp)
+		return;
+
+	pp->failure_count = cpu_to_le16(failure_count);
+	pp->timer = cpu_to_le16(timer);
+
+	send_request_to_device(priv, pp, hif_align_size(sizeof(*pp)));
+}
+
+/* Device I/O Receive indicate */
+static void devio_rec_ind(struct ks_wlan_private *priv, unsigned char *p,
+			  unsigned int size)
+{
+	if (!priv->is_device_open)
+		return;
+
+	spin_lock(&priv->dev_read_lock);
+	priv->dev_data[atomic_read(&priv->rec_count)] = p;
+	priv->dev_size[atomic_read(&priv->rec_count)] = size;
+
+	if (atomic_read(&priv->event_count) != DEVICE_STOCK_COUNT) {
+		/* rx event count inc */
+		atomic_inc(&priv->event_count);
+	}
+	atomic_inc(&priv->rec_count);
+	if (atomic_read(&priv->rec_count) == DEVICE_STOCK_COUNT)
+		atomic_set(&priv->rec_count, 0);
+
+	wake_up_interruptible_all(&priv->devread_wait);
+
+	spin_unlock(&priv->dev_read_lock);
+}
+
+void hostif_receive(struct ks_wlan_private *priv, unsigned char *p,
+		    unsigned int size)
+{
+	devio_rec_ind(priv, p, size);
+
+	priv->rxp = p;
+	priv->rx_size = size;
+
+	if (get_word(priv) == priv->rx_size)
+		hostif_event_check(priv);
+}
+
+static void hostif_sme_set_wep(struct ks_wlan_private *priv, int type)
+{
+	switch (type) {
+	case SME_WEP_INDEX_REQUEST:
+		hostif_mib_set_request_int(priv, DOT11_WEP_DEFAULT_KEY_ID,
+					   priv->reg.wep_index);
+		break;
+	case SME_WEP_KEY1_REQUEST:
+		if (priv->wpa.wpa_enabled)
+			return;
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE1,
+					       &priv->reg.wep_key[0].val[0],
+					       priv->reg.wep_key[0].size);
+		break;
+	case SME_WEP_KEY2_REQUEST:
+		if (priv->wpa.wpa_enabled)
+			return;
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE2,
+					       &priv->reg.wep_key[1].val[0],
+					       priv->reg.wep_key[1].size);
+		break;
+	case SME_WEP_KEY3_REQUEST:
+		if (priv->wpa.wpa_enabled)
+			return;
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE3,
+					       &priv->reg.wep_key[2].val[0],
+					       priv->reg.wep_key[2].size);
+		break;
+	case SME_WEP_KEY4_REQUEST:
+		if (priv->wpa.wpa_enabled)
+			return;
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE4,
+					       &priv->reg.wep_key[3].val[0],
+					       priv->reg.wep_key[3].size);
+		break;
+	case SME_WEP_FLAG_REQUEST:
+		hostif_mib_set_request_bool(priv, DOT11_PRIVACY_INVOKED,
+					    priv->reg.privacy_invoked);
+		break;
+	}
+}
+
+struct wpa_suite {
+	__le16 size;
+	unsigned char suite[4][CIPHER_ID_LEN];
+} __packed;
+
+struct rsn_mode {
+	__le32 rsn_mode;
+	__le16 rsn_capability;
+} __packed;
+
+static void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
+{
+	struct wpa_suite wpa_suite;
+	struct rsn_mode rsn_mode;
+	size_t size;
+	u32 mode;
+	const u8 *buf = NULL;
+
+	memset(&wpa_suite, 0, sizeof(wpa_suite));
+
+	switch (type) {
+	case SME_RSN_UCAST_REQUEST:
+		wpa_suite.size = cpu_to_le16(1);
+		switch (priv->wpa.pairwise_suite) {
+		case IW_AUTH_CIPHER_NONE:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_NONE : CIPHER_ID_WPA_NONE;
+			break;
+		case IW_AUTH_CIPHER_WEP40:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_WEP40 : CIPHER_ID_WPA_WEP40;
+			break;
+		case IW_AUTH_CIPHER_TKIP:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_TKIP : CIPHER_ID_WPA_TKIP;
+			break;
+		case IW_AUTH_CIPHER_CCMP:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_CCMP : CIPHER_ID_WPA_CCMP;
+			break;
+		case IW_AUTH_CIPHER_WEP104:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_WEP104 : CIPHER_ID_WPA_WEP104;
+			break;
+		}
+
+		if (buf)
+			memcpy(&wpa_suite.suite[0][0], buf, CIPHER_ID_LEN);
+		size = sizeof(wpa_suite.size) +
+		       (CIPHER_ID_LEN * le16_to_cpu(wpa_suite.size));
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_RSN_CONFIG_UNICAST_CIPHER,
+					       &wpa_suite, size);
+		break;
+	case SME_RSN_MCAST_REQUEST:
+		switch (priv->wpa.group_suite) {
+		case IW_AUTH_CIPHER_NONE:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_NONE : CIPHER_ID_WPA_NONE;
+			break;
+		case IW_AUTH_CIPHER_WEP40:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_WEP40 : CIPHER_ID_WPA_WEP40;
+			break;
+		case IW_AUTH_CIPHER_TKIP:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_TKIP : CIPHER_ID_WPA_TKIP;
+			break;
+		case IW_AUTH_CIPHER_CCMP:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_CCMP : CIPHER_ID_WPA_CCMP;
+			break;
+		case IW_AUTH_CIPHER_WEP104:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				CIPHER_ID_WPA2_WEP104 : CIPHER_ID_WPA_WEP104;
+			break;
+		}
+		if (buf)
+			memcpy(&wpa_suite.suite[0][0], buf, CIPHER_ID_LEN);
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_RSN_CONFIG_MULTICAST_CIPHER,
+					       &wpa_suite.suite[0][0],
+					       CIPHER_ID_LEN);
+		break;
+	case SME_RSN_AUTH_REQUEST:
+		wpa_suite.size = cpu_to_le16(1);
+		switch (priv->wpa.key_mgmt_suite) {
+		case IW_AUTH_KEY_MGMT_802_1X:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				KEY_MGMT_ID_WPA2_1X : KEY_MGMT_ID_WPA_1X;
+			break;
+		case IW_AUTH_KEY_MGMT_PSK:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				KEY_MGMT_ID_WPA2_PSK : KEY_MGMT_ID_WPA_PSK;
+			break;
+		case 0:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				KEY_MGMT_ID_WPA2_NONE : KEY_MGMT_ID_WPA_NONE;
+			break;
+		case 4:
+			buf = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+				KEY_MGMT_ID_WPA2_WPANONE :
+				KEY_MGMT_ID_WPA_WPANONE;
+			break;
+		}
+
+		if (buf)
+			memcpy(&wpa_suite.suite[0][0], buf, KEY_MGMT_ID_LEN);
+		size = sizeof(wpa_suite.size) +
+		       (KEY_MGMT_ID_LEN * le16_to_cpu(wpa_suite.size));
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_RSN_CONFIG_AUTH_SUITE,
+					       &wpa_suite, size);
+		break;
+	case SME_RSN_ENABLED_REQUEST:
+		hostif_mib_set_request_bool(priv, DOT11_RSN_ENABLED,
+					    priv->wpa.rsn_enabled);
+		break;
+	case SME_RSN_MODE_REQUEST:
+		mode = (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) ?
+			RSN_MODE_WPA2 :
+			(priv->wpa.version == IW_AUTH_WPA_VERSION_WPA) ?
+			 RSN_MODE_WPA : RSN_MODE_NONE;
+		rsn_mode.rsn_mode = cpu_to_le32(mode);
+		rsn_mode.rsn_capability = cpu_to_le16(0);
+		hostif_mib_set_request_ostring(priv, LOCAL_RSN_MODE,
+					       &rsn_mode, sizeof(rsn_mode));
+		break;
+	}
+}
+
+static
+void hostif_sme_mode_setup(struct ks_wlan_private *priv)
+{
+	unsigned char rate_size;
+	unsigned char rate_octet[RATE_SET_MAX_SIZE];
+	int i = 0;
+
+	/* rate setting if rate segging is auto for changing phy_type (#94) */
+	if (priv->reg.tx_rate == TX_RATE_FULL_AUTO) {
+		if (priv->reg.phy_type == D_11B_ONLY_MODE) {
+			priv->reg.rate_set.body[3] = TX_RATE_11M;
+			priv->reg.rate_set.body[2] = TX_RATE_5M;
+			priv->reg.rate_set.body[1] = TX_RATE_2M | BASIC_RATE;
+			priv->reg.rate_set.body[0] = TX_RATE_1M | BASIC_RATE;
+			priv->reg.rate_set.size = 4;
+		} else {	/* D_11G_ONLY_MODE or D_11BG_COMPATIBLE_MODE */
+			priv->reg.rate_set.body[11] = TX_RATE_54M;
+			priv->reg.rate_set.body[10] = TX_RATE_48M;
+			priv->reg.rate_set.body[9] = TX_RATE_36M;
+			priv->reg.rate_set.body[8] = TX_RATE_18M;
+			priv->reg.rate_set.body[7] = TX_RATE_9M;
+			priv->reg.rate_set.body[6] = TX_RATE_24M | BASIC_RATE;
+			priv->reg.rate_set.body[5] = TX_RATE_12M | BASIC_RATE;
+			priv->reg.rate_set.body[4] = TX_RATE_6M | BASIC_RATE;
+			priv->reg.rate_set.body[3] = TX_RATE_11M | BASIC_RATE;
+			priv->reg.rate_set.body[2] = TX_RATE_5M | BASIC_RATE;
+			priv->reg.rate_set.body[1] = TX_RATE_2M | BASIC_RATE;
+			priv->reg.rate_set.body[0] = TX_RATE_1M | BASIC_RATE;
+			priv->reg.rate_set.size = 12;
+		}
+	}
+
+	/* rate mask by phy setting */
+	if (priv->reg.phy_type == D_11B_ONLY_MODE) {
+		for (i = 0; i < priv->reg.rate_set.size; i++) {
+			if (!is_11b_rate(priv->reg.rate_set.body[i]))
+				break;
+
+			if ((priv->reg.rate_set.body[i] & RATE_MASK) >= TX_RATE_5M) {
+				rate_octet[i] = priv->reg.rate_set.body[i] &
+						RATE_MASK;
+			} else {
+				rate_octet[i] = priv->reg.rate_set.body[i];
+			}
+		}
+
+	} else {	/* D_11G_ONLY_MODE or D_11BG_COMPATIBLE_MODE */
+		for (i = 0; i < priv->reg.rate_set.size; i++) {
+			if (!is_11bg_rate(priv->reg.rate_set.body[i]))
+				break;
+
+			if (is_ofdm_ext_rate(priv->reg.rate_set.body[i])) {
+				rate_octet[i] = priv->reg.rate_set.body[i] &
+						RATE_MASK;
+			} else {
+				rate_octet[i] = priv->reg.rate_set.body[i];
+			}
+		}
+	}
+	rate_size = i;
+	if (rate_size == 0) {
+		if (priv->reg.phy_type == D_11G_ONLY_MODE)
+			rate_octet[0] = TX_RATE_6M | BASIC_RATE;
+		else
+			rate_octet[0] = TX_RATE_2M | BASIC_RATE;
+		rate_size = 1;
+	}
+
+	/* rate set update */
+	priv->reg.rate_set.size = rate_size;
+	memcpy(&priv->reg.rate_set.body[0], &rate_octet[0], rate_size);
+
+	switch (priv->reg.operation_mode) {
+	case MODE_PSEUDO_ADHOC:
+		hostif_ps_adhoc_set_request(priv);
+		break;
+	case MODE_INFRASTRUCTURE:
+		if (!is_valid_ether_addr((u8 *)priv->reg.bssid)) {
+			hostif_infrastructure_set_request(priv,
+							  HIF_INFRA_SET_REQ);
+		} else {
+			hostif_infrastructure_set_request(priv,
+							  HIF_INFRA_SET2_REQ);
+			netdev_dbg(priv->net_dev,
+				   "Infra bssid = %pM\n", priv->reg.bssid);
+		}
+		break;
+	case MODE_ADHOC:
+		if (!is_valid_ether_addr((u8 *)priv->reg.bssid)) {
+			hostif_adhoc_set_request(priv);
+		} else {
+			hostif_adhoc_set2_request(priv);
+			netdev_dbg(priv->net_dev,
+				   "Adhoc bssid = %pM\n", priv->reg.bssid);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static
+void hostif_sme_multicast_set(struct ks_wlan_private *priv)
+{
+	struct net_device *dev = priv->net_dev;
+	int mc_count;
+	struct netdev_hw_addr *ha;
+	char set_address[NIC_MAX_MCAST_LIST * ETH_ALEN];
+	int i = 0;
+
+	spin_lock(&priv->multicast_spin);
+
+	memset(set_address, 0, NIC_MAX_MCAST_LIST * ETH_ALEN);
+
+	if (dev->flags & IFF_PROMISC) {
+		hostif_mib_set_request_int(priv, LOCAL_MULTICAST_FILTER,
+					   MCAST_FILTER_PROMISC);
+		goto spin_unlock;
+	}
+
+	if ((netdev_mc_count(dev) > NIC_MAX_MCAST_LIST) ||
+	    (dev->flags & IFF_ALLMULTI)) {
+		hostif_mib_set_request_int(priv, LOCAL_MULTICAST_FILTER,
+					   MCAST_FILTER_MCASTALL);
+		goto spin_unlock;
+	}
+
+	if (priv->sme_i.sme_flag & SME_MULTICAST) {
+		mc_count = netdev_mc_count(dev);
+		netdev_for_each_mc_addr(ha, dev) {
+			ether_addr_copy(&set_address[i * ETH_ALEN], ha->addr);
+			i++;
+		}
+		priv->sme_i.sme_flag &= ~SME_MULTICAST;
+		hostif_mib_set_request_ostring(priv, LOCAL_MULTICAST_ADDRESS,
+					       &set_address[0],
+					       ETH_ALEN * mc_count);
+	} else {
+		priv->sme_i.sme_flag |= SME_MULTICAST;
+		hostif_mib_set_request_int(priv, LOCAL_MULTICAST_FILTER,
+					   MCAST_FILTER_MCAST);
+	}
+
+spin_unlock:
+	spin_unlock(&priv->multicast_spin);
+}
+
+static void hostif_sme_power_mgmt_set(struct ks_wlan_private *priv)
+{
+	u32 mode, wake_up, receive_dtims;
+
+	if (priv->reg.power_mgmt != POWER_MGMT_SAVE1 &&
+	    priv->reg.power_mgmt != POWER_MGMT_SAVE2) {
+		mode = POWER_ACTIVE;
+		wake_up = 0;
+		receive_dtims = 0;
+	} else {
+		mode = (priv->reg.operation_mode == MODE_INFRASTRUCTURE) ?
+			POWER_SAVE : POWER_ACTIVE;
+		wake_up = 0;
+		receive_dtims = (priv->reg.operation_mode == MODE_INFRASTRUCTURE &&
+				 priv->reg.power_mgmt == POWER_MGMT_SAVE2);
+	}
+
+	hostif_power_mgmt_request(priv, mode, wake_up, receive_dtims);
+}
+
+static void hostif_sme_sleep_set(struct ks_wlan_private *priv)
+{
+	if (priv->sleep_mode != SLP_SLEEP &&
+	    priv->sleep_mode != SLP_ACTIVE)
+		return;
+
+	hostif_sleep_request(priv, priv->sleep_mode);
+}
+
+static
+void hostif_sme_set_key(struct ks_wlan_private *priv, int type)
+{
+	switch (type) {
+	case SME_SET_FLAG:
+		hostif_mib_set_request_bool(priv, DOT11_PRIVACY_INVOKED,
+					    priv->reg.privacy_invoked);
+		break;
+	case SME_SET_TXKEY:
+		hostif_mib_set_request_int(priv, DOT11_WEP_DEFAULT_KEY_ID,
+					   priv->wpa.txkey);
+		break;
+	case SME_SET_KEY1:
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE1,
+					       &priv->wpa.key[0].key_val[0],
+					       priv->wpa.key[0].key_len);
+		break;
+	case SME_SET_KEY2:
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE2,
+					       &priv->wpa.key[1].key_val[0],
+					       priv->wpa.key[1].key_len);
+		break;
+	case SME_SET_KEY3:
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE3,
+					       &priv->wpa.key[2].key_val[0],
+					       priv->wpa.key[2].key_len);
+		break;
+	case SME_SET_KEY4:
+		hostif_mib_set_request_ostring(priv,
+					       DOT11_WEP_DEFAULT_KEY_VALUE4,
+					       &priv->wpa.key[3].key_val[0],
+					       priv->wpa.key[3].key_len);
+		break;
+	case SME_SET_PMK_TSC:
+		hostif_mib_set_request_ostring(priv, DOT11_PMK_TSC,
+					       &priv->wpa.key[0].rx_seq[0],
+					       WPA_RX_SEQ_LEN);
+		break;
+	case SME_SET_GMK1_TSC:
+		hostif_mib_set_request_ostring(priv, DOT11_GMK1_TSC,
+					       &priv->wpa.key[1].rx_seq[0],
+					       WPA_RX_SEQ_LEN);
+		break;
+	case SME_SET_GMK2_TSC:
+		hostif_mib_set_request_ostring(priv, DOT11_GMK2_TSC,
+					       &priv->wpa.key[2].rx_seq[0],
+					       WPA_RX_SEQ_LEN);
+		break;
+	}
+}
+
+static
+void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
+{
+	struct pmk_cache {
+		__le16 size;
+		struct {
+			u8 bssid[ETH_ALEN];
+			u8 pmkid[IW_PMKID_LEN];
+		} __packed list[PMK_LIST_MAX];
+	} __packed pmkcache;
+	struct pmk *pmk;
+	size_t size;
+	int i = 0;
+
+	list_for_each_entry(pmk, &priv->pmklist.head, list) {
+		if (i >= PMK_LIST_MAX)
+			break;
+		ether_addr_copy(pmkcache.list[i].bssid, pmk->bssid);
+		memcpy(pmkcache.list[i].pmkid, pmk->pmkid, IW_PMKID_LEN);
+		i++;
+	}
+	pmkcache.size = cpu_to_le16(priv->pmklist.size);
+	size = sizeof(priv->pmklist.size) +
+	       ((ETH_ALEN + IW_PMKID_LEN) * priv->pmklist.size);
+	hostif_mib_set_request_ostring(priv, LOCAL_PMK, &pmkcache, size);
+}
+
+/* execute sme */
+static void hostif_sme_execute(struct ks_wlan_private *priv, int event)
+{
+	u16 failure;
+
+	switch (event) {
+	case SME_START:
+		if (priv->dev_state == DEVICE_STATE_BOOT)
+			hostif_mib_get_request(priv, DOT11_MAC_ADDRESS);
+		break;
+	case SME_MULTICAST_REQUEST:
+		hostif_sme_multicast_set(priv);
+		break;
+	case SME_MACADDRESS_SET_REQUEST:
+		hostif_mib_set_request_ostring(priv, LOCAL_CURRENTADDRESS,
+					       &priv->eth_addr[0], ETH_ALEN);
+		break;
+	case SME_BSS_SCAN_REQUEST:
+		hostif_bss_scan_request(priv, priv->reg.scan_type,
+					priv->scan_ssid, priv->scan_ssid_len);
+		break;
+	case SME_POW_MNGMT_REQUEST:
+		hostif_sme_power_mgmt_set(priv);
+		break;
+	case SME_PHY_INFO_REQUEST:
+		hostif_phy_information_request(priv);
+		break;
+	case SME_MIC_FAILURE_REQUEST:
+		failure = priv->wpa.mic_failure.failure;
+		if (failure != 1 && failure != 2) {
+			netdev_err(priv->net_dev,
+				   "SME_MIC_FAILURE_REQUEST: failure count=%u error?\n",
+				   failure);
+			return;
+		}
+		hostif_mic_failure_request(priv, failure - 1, (failure == 1) ?
+					    0 : priv->wpa.mic_failure.counter);
+		break;
+	case SME_MIC_FAILURE_CONFIRM:
+		if (priv->wpa.mic_failure.failure == 2) {
+			if (priv->wpa.mic_failure.stop)
+				priv->wpa.mic_failure.stop = 0;
+			priv->wpa.mic_failure.failure = 0;
+			hostif_start_request(priv, priv->reg.operation_mode);
+		}
+		break;
+	case SME_GET_MAC_ADDRESS:
+		if (priv->dev_state == DEVICE_STATE_BOOT)
+			hostif_mib_get_request(priv, DOT11_PRODUCT_VERSION);
+		break;
+	case SME_GET_PRODUCT_VERSION:
+		if (priv->dev_state == DEVICE_STATE_BOOT)
+			priv->dev_state = DEVICE_STATE_PREINIT;
+		break;
+	case SME_STOP_REQUEST:
+		hostif_stop_request(priv);
+		break;
+	case SME_RTS_THRESHOLD_REQUEST:
+		hostif_mib_set_request_int(priv, DOT11_RTS_THRESHOLD,
+					   priv->reg.rts);
+		break;
+	case SME_FRAGMENTATION_THRESHOLD_REQUEST:
+		hostif_mib_set_request_int(priv, DOT11_FRAGMENTATION_THRESHOLD,
+					   priv->reg.fragment);
+		break;
+	case SME_WEP_INDEX_REQUEST:
+	case SME_WEP_KEY1_REQUEST:
+	case SME_WEP_KEY2_REQUEST:
+	case SME_WEP_KEY3_REQUEST:
+	case SME_WEP_KEY4_REQUEST:
+	case SME_WEP_FLAG_REQUEST:
+		hostif_sme_set_wep(priv, event);
+		break;
+	case SME_RSN_UCAST_REQUEST:
+	case SME_RSN_MCAST_REQUEST:
+	case SME_RSN_AUTH_REQUEST:
+	case SME_RSN_ENABLED_REQUEST:
+	case SME_RSN_MODE_REQUEST:
+		hostif_sme_set_rsn(priv, event);
+		break;
+	case SME_SET_FLAG:
+	case SME_SET_TXKEY:
+	case SME_SET_KEY1:
+	case SME_SET_KEY2:
+	case SME_SET_KEY3:
+	case SME_SET_KEY4:
+	case SME_SET_PMK_TSC:
+	case SME_SET_GMK1_TSC:
+	case SME_SET_GMK2_TSC:
+		hostif_sme_set_key(priv, event);
+		break;
+	case SME_SET_PMKSA:
+		hostif_sme_set_pmksa(priv);
+		break;
+	case SME_WPS_ENABLE_REQUEST:
+		hostif_mib_set_request_int(priv, LOCAL_WPS_ENABLE,
+					   priv->wps.wps_enabled);
+		break;
+	case SME_WPS_PROBE_REQUEST:
+		hostif_mib_set_request_ostring(priv, LOCAL_WPS_PROBE_REQ,
+					       priv->wps.ie, priv->wps.ielen);
+		break;
+	case SME_MODE_SET_REQUEST:
+		hostif_sme_mode_setup(priv);
+		break;
+	case SME_SET_GAIN:
+		hostif_mib_set_request_ostring(priv, LOCAL_GAIN,
+					       &priv->gain, sizeof(priv->gain));
+		break;
+	case SME_GET_GAIN:
+		hostif_mib_get_request(priv, LOCAL_GAIN);
+		break;
+	case SME_GET_EEPROM_CKSUM:
+		priv->eeprom_checksum = EEPROM_FW_NOT_SUPPORT;	/* initialize */
+		hostif_mib_get_request(priv, LOCAL_EEPROM_SUM);
+		break;
+	case SME_START_REQUEST:
+		hostif_start_request(priv, priv->reg.operation_mode);
+		break;
+	case SME_START_CONFIRM:
+		/* for power save */
+		atomic_set(&priv->psstatus.snooze_guard, 0);
+		atomic_set(&priv->psstatus.confirm_wait, 0);
+		if (priv->dev_state == DEVICE_STATE_PREINIT)
+			priv->dev_state = DEVICE_STATE_INIT;
+		/* wake_up_interruptible_all(&priv->confirm_wait); */
+		complete(&priv->confirm_wait);
+		break;
+	case SME_SLEEP_REQUEST:
+		hostif_sme_sleep_set(priv);
+		break;
+	case SME_SET_REGION:
+		hostif_mib_set_request_int(priv, LOCAL_REGION, priv->region);
+		break;
+	case SME_MULTICAST_CONFIRM:
+	case SME_BSS_SCAN_CONFIRM:
+	case SME_POW_MNGMT_CONFIRM:
+	case SME_PHY_INFO_CONFIRM:
+	case SME_STOP_CONFIRM:
+	case SME_RTS_THRESHOLD_CONFIRM:
+	case SME_FRAGMENTATION_THRESHOLD_CONFIRM:
+	case SME_WEP_INDEX_CONFIRM:
+	case SME_WEP_KEY1_CONFIRM:
+	case SME_WEP_KEY2_CONFIRM:
+	case SME_WEP_KEY3_CONFIRM:
+	case SME_WEP_KEY4_CONFIRM:
+	case SME_WEP_FLAG_CONFIRM:
+	case SME_RSN_UCAST_CONFIRM:
+	case SME_RSN_MCAST_CONFIRM:
+	case SME_RSN_AUTH_CONFIRM:
+	case SME_RSN_ENABLED_CONFIRM:
+	case SME_RSN_MODE_CONFIRM:
+	case SME_MODE_SET_CONFIRM:
+	case SME_TERMINATE:
+	default:
+		break;
+	}
+}
+
+static void hostif_sme_work(struct work_struct *work)
+{
+	struct ks_wlan_private *priv;
+
+	priv = container_of(work, struct ks_wlan_private, sme_work);
+
+	if (priv->dev_state < DEVICE_STATE_BOOT)
+		return;
+
+	if (cnt_smeqbody(priv) <= 0)
+		return;
+
+	hostif_sme_execute(priv, priv->sme_i.event_buff[priv->sme_i.qhead]);
+	inc_smeqhead(priv);
+	if (cnt_smeqbody(priv) > 0)
+		schedule_work(&priv->sme_work);
+}
+
+/* send to Station Management Entity module */
+void hostif_sme_enqueue(struct ks_wlan_private *priv, u16 event)
+{
+	/* enqueue sme event */
+	if (cnt_smeqbody(priv) < (SME_EVENT_BUFF_SIZE - 1)) {
+		priv->sme_i.event_buff[priv->sme_i.qtail] = event;
+		inc_smeqtail(priv);
+	} else {
+		/* in case of buffer overflow */
+		netdev_err(priv->net_dev, "sme queue buffer overflow\n");
+	}
+
+	schedule_work(&priv->sme_work);
+}
+
+static inline void hostif_aplist_init(struct ks_wlan_private *priv)
+{
+	size_t size = LOCAL_APLIST_MAX * sizeof(struct local_ap);
+
+	priv->aplist.size = 0;
+	memset(&priv->aplist.ap[0], 0, size);
+}
+
+static inline void hostif_status_init(struct ks_wlan_private *priv)
+{
+	priv->infra_status = 0;
+	priv->current_rate = 4;
+	priv->connect_status = DISCONNECT_STATUS;
+}
+
+static inline void hostif_sme_init(struct ks_wlan_private *priv)
+{
+	priv->sme_i.sme_status = SME_IDLE;
+	priv->sme_i.qhead = 0;
+	priv->sme_i.qtail = 0;
+	spin_lock_init(&priv->sme_i.sme_spin);
+	priv->sme_i.sme_flag = 0;
+	INIT_WORK(&priv->sme_work, hostif_sme_work);
+}
+
+static inline void hostif_wpa_init(struct ks_wlan_private *priv)
+{
+	memset(&priv->wpa, 0, sizeof(priv->wpa));
+	priv->wpa.rsn_enabled = false;
+	priv->wpa.mic_failure.failure = 0;
+	priv->wpa.mic_failure.last_failure_time = 0;
+	priv->wpa.mic_failure.stop = 0;
+}
+
+static inline void hostif_power_save_init(struct ks_wlan_private *priv)
+{
+	atomic_set(&priv->psstatus.status, PS_NONE);
+	atomic_set(&priv->psstatus.confirm_wait, 0);
+	atomic_set(&priv->psstatus.snooze_guard, 0);
+	init_completion(&priv->psstatus.wakeup_wait);
+	INIT_WORK(&priv->wakeup_work, ks_wlan_hw_wakeup_task);
+}
+
+static inline void hostif_pmklist_init(struct ks_wlan_private *priv)
+{
+	int i;
+
+	memset(&priv->pmklist, 0, sizeof(priv->pmklist));
+	INIT_LIST_HEAD(&priv->pmklist.head);
+	for (i = 0; i < PMK_LIST_MAX; i++)
+		INIT_LIST_HEAD(&priv->pmklist.pmk[i].list);
+}
+
+static inline void hostif_counters_init(struct ks_wlan_private *priv)
+{
+	priv->dev_count = 0;
+	atomic_set(&priv->event_count, 0);
+	atomic_set(&priv->rec_count, 0);
+}
+
+int hostif_init(struct ks_wlan_private *priv)
+{
+	hostif_aplist_init(priv);
+	hostif_status_init(priv);
+
+	spin_lock_init(&priv->multicast_spin);
+	spin_lock_init(&priv->dev_read_lock);
+	init_waitqueue_head(&priv->devread_wait);
+
+	hostif_counters_init(priv);
+	hostif_power_save_init(priv);
+	hostif_wpa_init(priv);
+	hostif_pmklist_init(priv);
+	hostif_sme_init(priv);
+
+	return 0;
+}
+
+void hostif_exit(struct ks_wlan_private *priv)
+{
+	cancel_work_sync(&priv->sme_work);
+}
diff --git a/drivers/staging/ks7010/ks_hostif.h b/drivers/staging/ks7010/ks_hostif.h
new file mode 100644
index 000000000..c62a494ed
--- /dev/null
+++ b/drivers/staging/ks7010/ks_hostif.h
@@ -0,0 +1,617 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *   Driver for KeyStream wireless LAN
+ *
+ *   Copyright (c) 2005-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ */
+
+#ifndef _KS_HOSTIF_H_
+#define _KS_HOSTIF_H_
+
+#include <linux/compiler.h>
+#include <linux/ieee80211.h>
+
+/*
+ * HOST-MAC I/F events
+ */
+#define HIF_DATA_REQ		0xE001
+#define HIF_DATA_IND		0xE801
+#define HIF_MIB_GET_REQ		0xE002
+#define HIF_MIB_GET_CONF	0xE802
+#define HIF_MIB_SET_REQ		0xE003
+#define HIF_MIB_SET_CONF	0xE803
+#define HIF_POWER_MGMT_REQ	0xE004
+#define HIF_POWER_MGMT_CONF	0xE804
+#define HIF_START_REQ		0xE005
+#define HIF_START_CONF		0xE805
+#define HIF_CONNECT_IND		0xE806
+#define HIF_STOP_REQ		0xE006
+#define HIF_STOP_CONF		0xE807
+#define HIF_PS_ADH_SET_REQ	0xE007
+#define HIF_PS_ADH_SET_CONF	0xE808
+#define HIF_INFRA_SET_REQ	0xE008
+#define HIF_INFRA_SET_CONF	0xE809
+#define HIF_ADH_SET_REQ		0xE009
+#define HIF_ADH_SET_CONF	0xE80A
+#define HIF_AP_SET_REQ		0xE00A
+#define HIF_AP_SET_CONF		0xE80B
+#define HIF_ASSOC_INFO_IND	0xE80C
+#define HIF_MIC_FAILURE_REQ	0xE00B
+#define HIF_MIC_FAILURE_CONF	0xE80D
+#define HIF_SCAN_REQ		0xE00C
+#define HIF_SCAN_CONF		0xE80E
+#define HIF_PHY_INFO_REQ	0xE00D
+#define HIF_PHY_INFO_CONF	0xE80F
+#define HIF_SLEEP_REQ		0xE00E
+#define HIF_SLEEP_CONF		0xE810
+#define HIF_PHY_INFO_IND	0xE811
+#define HIF_SCAN_IND		0xE812
+#define HIF_INFRA_SET2_REQ	0xE00F
+#define HIF_INFRA_SET2_CONF	0xE813
+#define HIF_ADH_SET2_REQ	0xE010
+#define HIF_ADH_SET2_CONF	0xE814
+
+#define HIF_REQ_MAX		0xE010
+
+/*
+ * HOST-MAC I/F data structure
+ * Byte alignment Little Endian
+ */
+
+struct hostif_hdr {
+	__le16 size;
+	__le16 event;
+} __packed;
+
+struct hostif_data_request {
+	struct hostif_hdr header;
+	__le16 auth_type;
+#define TYPE_DATA 0x0000
+#define TYPE_AUTH 0x0001
+	__le16 reserved;
+	u8 data[];
+} __packed;
+
+#define TYPE_PMK1 0x0001
+#define TYPE_GMK1 0x0002
+#define TYPE_GMK2 0x0003
+
+#define CHANNEL_LIST_MAX_SIZE 14
+struct channel_list {
+	u8 size;
+	u8 body[CHANNEL_LIST_MAX_SIZE];
+	u8 pad;
+} __packed;
+
+/**
+ * enum mib_attribute - Management Information Base attribute
+ * Attribute value used for accessing and updating MIB
+ *
+ * @DOT11_MAC_ADDRESS: MAC Address (R)
+ * @DOT11_PRODUCT_VERSION: FirmWare Version (R)
+ * @DOT11_RTS_THRESHOLD: RTS Threshold (R/W)
+ * @DOT11_FRAGMENTATION_THRESHOLD: Fragment Threshold (R/W)
+ * @DOT11_PRIVACY_INVOKED: WEP ON/OFF (W)
+ * @DOT11_WEP_DEFAULT_KEY_ID: WEP Index (W)
+ * @DOT11_WEP_DEFAULT_KEY_VALUE1: WEP Key#1(TKIP AES: PairwiseTemporalKey) (W)
+ * @DOT11_WEP_DEFAULT_KEY_VALUE2: WEP Key#2(TKIP AES: GroupKey1) (W)
+ * @DOT11_WEP_DEFAULT_KEY_VALUE3: WEP Key#3(TKIP AES: GroupKey2) (W)
+ * @DOT11_WEP_DEFAULT_KEY_VALUE4: WEP Key#4 (W)
+ * @DOT11_WEP_LIST: WEP LIST
+ * @DOT11_DESIRED_SSID: SSID
+ * @DOT11_CURRENT_CHANNEL: channel set
+ * @DOT11_OPERATION_RATE_SET: rate set
+ * @LOCAL_AP_SEARCH_INTERVAL: AP search interval (R/W)
+ * @LOCAL_CURRENTADDRESS: MAC Address change (W)
+ * @LOCAL_MULTICAST_ADDRESS: Multicast Address (W)
+ * @LOCAL_MULTICAST_FILTER: Multicast Address Filter enable/disable (W)
+ * @LOCAL_SEARCHED_AP_LIST: AP list (R)
+ * @LOCAL_LINK_AP_STATUS: Link AP status (R)
+ * @LOCAL_PACKET_STATISTICS: tx,rx packets statistics
+ * @LOCAL_AP_SCAN_LIST_TYPE_SET: AP_SCAN_LIST_TYPE
+ * @DOT11_RSN_ENABLED: WPA enable/disable (W)
+ * @LOCAL_RSN_MODE: RSN mode WPA/WPA2 (W)
+ * @DOT11_RSN_CONFIG_MULTICAST_CIPHER: GroupKeyCipherSuite (W)
+ * @DOT11_RSN_CONFIG_UNICAST_CIPHER: PairwiseKeyCipherSuite (W)
+ * @DOT11_RSN_CONFIG_AUTH_SUITE: AuthenticationKeyManagementSuite (W)
+ * @DOT11_RSN_CONFIG_VERSION: RSN version (W)
+ * @LOCAL_RSN_CONFIG_ALL: RSN CONFIG ALL (W)
+ * @DOT11_PMK_TSC: PMK_TSC (W)
+ * @DOT11_GMK1_TSC: GMK1_TSC (W)
+ * @DOT11_GMK2_TSC: GMK2_TSC (W)
+ * @DOT11_GMK3_TSC: GMK3_TSC
+ * @LOCAL_PMK: Pairwise Master Key cache (W)
+ * @LOCAL_REGION: Region setting
+ * @LOCAL_WPS_ENABLE: WiFi Protected Setup
+ * @LOCAL_WPS_PROBE_REQ: WPS Probe Request
+ * @LOCAL_GAIN: Carrer sense threshold for demo ato show
+ * @LOCAL_EEPROM_SUM: EEPROM checksum information
+ */
+enum mib_attribute {
+	DOT11_MAC_ADDRESS                 = 0x21010100,
+	DOT11_PRODUCT_VERSION             = 0x31024100,
+	DOT11_RTS_THRESHOLD               = 0x21020100,
+	DOT11_FRAGMENTATION_THRESHOLD     = 0x21050100,
+	DOT11_PRIVACY_INVOKED             = 0x15010100,
+	DOT11_WEP_DEFAULT_KEY_ID          = 0x15020100,
+	DOT11_WEP_DEFAULT_KEY_VALUE1      = 0x13020101,
+	DOT11_WEP_DEFAULT_KEY_VALUE2      = 0x13020102,
+	DOT11_WEP_DEFAULT_KEY_VALUE3      = 0x13020103,
+	DOT11_WEP_DEFAULT_KEY_VALUE4      = 0x13020104,
+	DOT11_WEP_LIST                    = 0x13020100,
+	DOT11_DESIRED_SSID		  = 0x11090100,
+	DOT11_CURRENT_CHANNEL		  = 0x45010100,
+	DOT11_OPERATION_RATE_SET	  = 0x11110100,
+	LOCAL_AP_SEARCH_INTERVAL          = 0xF1010100,
+	LOCAL_CURRENTADDRESS              = 0xF1050100,
+	LOCAL_MULTICAST_ADDRESS           = 0xF1060100,
+	LOCAL_MULTICAST_FILTER            = 0xF1060200,
+	LOCAL_SEARCHED_AP_LIST            = 0xF1030100,
+	LOCAL_LINK_AP_STATUS              = 0xF1040100,
+	LOCAL_PACKET_STATISTICS		  = 0xF1020100,
+	LOCAL_AP_SCAN_LIST_TYPE_SET	  = 0xF1030200,
+	DOT11_RSN_ENABLED                 = 0x15070100,
+	LOCAL_RSN_MODE                    = 0x56010100,
+	DOT11_RSN_CONFIG_MULTICAST_CIPHER = 0x51040100,
+	DOT11_RSN_CONFIG_UNICAST_CIPHER   = 0x52020100,
+	DOT11_RSN_CONFIG_AUTH_SUITE       = 0x53020100,
+	DOT11_RSN_CONFIG_VERSION          = 0x51020100,
+	LOCAL_RSN_CONFIG_ALL              = 0x5F010100,
+	DOT11_PMK_TSC                     = 0x55010100,
+	DOT11_GMK1_TSC                    = 0x55010101,
+	DOT11_GMK2_TSC                    = 0x55010102,
+	DOT11_GMK3_TSC                    = 0x55010103,
+	LOCAL_PMK                         = 0x58010100,
+	LOCAL_REGION                      = 0xF10A0100,
+	LOCAL_WPS_ENABLE                  = 0xF10B0100,
+	LOCAL_WPS_PROBE_REQ               = 0xF10C0100,
+	LOCAL_GAIN                        = 0xF10D0100,
+	LOCAL_EEPROM_SUM                  = 0xF10E0100
+};
+
+struct hostif_mib_get_request {
+	struct hostif_hdr header;
+	__le32 mib_attribute;
+} __packed;
+
+/**
+ * enum mib_data_type - Message Information Base data type.
+ * @MIB_VALUE_TYPE_NULL: NULL type
+ * @MIB_VALUE_TYPE_INT: INTEGER type
+ * @MIB_VALUE_TYPE_BOOL: BOOL type
+ * @MIB_VALUE_TYPE_COUNT32: unused
+ * @MIB_VALUE_TYPE_OSTRING: Chunk of memory
+ */
+enum mib_data_type {
+	MIB_VALUE_TYPE_NULL = 0,
+	MIB_VALUE_TYPE_INT,
+	MIB_VALUE_TYPE_BOOL,
+	MIB_VALUE_TYPE_COUNT32,
+	MIB_VALUE_TYPE_OSTRING
+};
+
+struct hostif_mib_value {
+	__le16 size;
+	__le16 type;
+	u8 body[];
+} __packed;
+
+struct hostif_mib_get_confirm_t {
+	struct hostif_hdr header;
+	__le32 mib_status;
+#define MIB_SUCCESS    0
+#define MIB_INVALID    1
+#define MIB_READ_ONLY  2
+#define MIB_WRITE_ONLY 3
+	__le32 mib_attribute;
+	struct hostif_mib_value mib_value;
+} __packed;
+
+struct hostif_mib_set_request_t {
+	struct hostif_hdr header;
+	__le32 mib_attribute;
+	struct hostif_mib_value mib_value;
+} __packed;
+
+struct hostif_power_mgmt_request {
+	struct hostif_hdr header;
+	__le32 mode;
+#define POWER_ACTIVE  1
+#define POWER_SAVE    2
+	__le32 wake_up;
+#define SLEEP_FALSE 0
+#define SLEEP_TRUE  1	/* not used */
+	__le32 receive_dtims;
+#define DTIM_FALSE 0
+#define DTIM_TRUE  1
+} __packed;
+
+enum power_mgmt_mode_type {
+	POWER_MGMT_ACTIVE,
+	POWER_MGMT_SAVE1,
+	POWER_MGMT_SAVE2
+};
+
+#define	RESULT_SUCCESS            0
+#define	RESULT_INVALID_PARAMETERS 1
+#define	RESULT_NOT_SUPPORTED      2
+/* #define	RESULT_ALREADY_RUNNING    3 */
+#define	RESULT_ALREADY_RUNNING    7
+
+struct hostif_start_request {
+	struct hostif_hdr header;
+	__le16 mode;
+#define MODE_PSEUDO_ADHOC   0
+#define MODE_INFRASTRUCTURE 1
+#define MODE_AP             2	/* not used */
+#define MODE_ADHOC          3
+} __packed;
+
+struct ssid {
+	u8 size;
+	u8 body[IEEE80211_MAX_SSID_LEN];
+	u8 ssid_pad;
+} __packed;
+
+#define RATE_SET_MAX_SIZE 16
+struct rate_set8 {
+	u8 size;
+	u8 body[8];
+	u8 rate_pad;
+} __packed;
+
+struct fh_parms {
+	__le16 dwell_time;
+	u8 hop_set;
+	u8 hop_pattern;
+	u8 hop_index;
+} __packed;
+
+struct ds_parms {
+	u8 channel;
+} __packed;
+
+struct cf_parms {
+	u8 count;
+	u8 period;
+	__le16 max_duration;
+	__le16 dur_remaining;
+} __packed;
+
+struct ibss_parms {
+	__le16 atim_window;
+} __packed;
+
+struct rsn_t {
+	u8 size;
+#define RSN_BODY_SIZE 64
+	u8 body[RSN_BODY_SIZE];
+} __packed;
+
+struct erp_params_t {
+	u8 erp_info;
+} __packed;
+
+struct rate_set16 {
+	u8 size;
+	u8 body[16];
+	u8 rate_pad;
+} __packed;
+
+struct ap_info {
+	u8 bssid[6];	/* +00 */
+	u8 rssi;	/* +06 */
+	u8 sq;	/* +07 */
+	u8 noise;	/* +08 */
+	u8 pad0;	/* +09 */
+	__le16 beacon_period;	/* +10 */
+	__le16 capability;	/* +12 */
+	u8 frame_type;	/* +14 */
+	u8 ch_info;	/* +15 */
+	__le16 body_size;	/* +16 */
+	u8 body[1024];	/* +18 */
+	/* +1032 */
+} __packed;
+
+struct link_ap_info {
+	u8 bssid[6];	/* +00 */
+	u8 rssi;	/* +06 */
+	u8 sq;	/* +07 */
+	u8 noise;	/* +08 */
+	u8 pad0;	/* +09 */
+	__le16 beacon_period;	/* +10 */
+	__le16 capability;	/* +12 */
+	struct rate_set8 rate_set;	/* +14 */
+	struct fh_parms fh_parameter;	/* +24 */
+	struct ds_parms ds_parameter;	/* +29 */
+	struct cf_parms cf_parameter;	/* +30 */
+	struct ibss_parms ibss_parameter;	/* +36 */
+	struct erp_params_t erp_parameter;	/* +38 */
+	u8 pad1;	/* +39 */
+	struct rate_set8 ext_rate_set;	/* +40 */
+	u8 DTIM_period;	/* +50 */
+	u8 rsn_mode;	/* +51 */
+#define RSN_MODE_NONE	0
+#define RSN_MODE_WPA	1
+#define RSN_MODE_WPA2	2
+	struct {
+		u8 size;	/* +52 */
+		u8 body[128];	/* +53 */
+	} __packed rsn;
+} __packed;
+
+#define RESULT_CONNECT    0
+#define RESULT_DISCONNECT 1
+
+struct hostif_stop_request {
+	struct hostif_hdr header;
+} __packed;
+
+#define D_11B_ONLY_MODE		0
+#define D_11G_ONLY_MODE		1
+#define D_11BG_COMPATIBLE_MODE	2
+#define D_11A_ONLY_MODE		3
+
+#define CTS_MODE_FALSE	0
+#define CTS_MODE_TRUE	1
+
+struct hostif_request {
+	__le16 phy_type;
+	__le16 cts_mode;
+	__le16 scan_type;
+	__le16 capability;
+	struct rate_set16 rate_set;
+} __packed;
+
+/**
+ * struct hostif_ps_adhoc_set_request - pseudo adhoc mode
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
+struct hostif_ps_adhoc_set_request {
+	struct hostif_hdr header;
+	struct hostif_request request;
+	__le16 channel;
+} __packed;
+
+#define AUTH_TYPE_OPEN_SYSTEM 0
+#define AUTH_TYPE_SHARED_KEY  1
+
+/**
+ * struct hostif_infrastructure_set_request
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
+struct hostif_infrastructure_set_request {
+	struct hostif_hdr header;
+	struct hostif_request request;
+	struct ssid ssid;
+	__le16 beacon_lost_count;
+	__le16 auth_type;
+	struct channel_list channel_list;
+	u8 bssid[ETH_ALEN];
+} __packed;
+
+/**
+ * struct hostif_adhoc_set_request
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
+struct hostif_adhoc_set_request {
+	struct hostif_hdr header;
+	struct hostif_request request;
+	struct ssid ssid;
+	__le16 channel;
+} __packed;
+
+/**
+ * struct hostif_adhoc_set2_request
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
+struct hostif_adhoc_set2_request {
+	struct hostif_hdr header;
+	struct hostif_request request;
+	__le16 reserved;
+	struct ssid ssid;
+	struct channel_list channel_list;
+	u8 bssid[ETH_ALEN];
+} __packed;
+
+struct association_request {
+	u8 type;
+	u8 pad;
+	__le16 capability;
+	__le16 listen_interval;
+	u8 ap_address[6];
+	__le16 req_ies_size;
+} __packed;
+
+struct association_response {
+	u8 type;
+	u8 pad;
+	__le16 capability;
+	__le16 status;
+	__le16 association_id;
+	__le16 resp_ies_size;
+} __packed;
+
+struct hostif_bss_scan_request {
+	struct hostif_hdr header;
+	u8 scan_type;
+#define ACTIVE_SCAN  0
+#define PASSIVE_SCAN 1
+	u8 pad[3];
+	__le32 ch_time_min;
+	__le32 ch_time_max;
+	struct channel_list channel_list;
+	struct ssid ssid;
+} __packed;
+
+struct hostif_phy_information_request {
+	struct hostif_hdr header;
+	__le16 type;
+#define NORMAL_TYPE	0
+#define TIME_TYPE	1
+	__le16 time;	/* unit 100ms */
+} __packed;
+
+enum sleep_mode_type {
+	SLP_ACTIVE,
+	SLP_SLEEP
+};
+
+struct hostif_sleep_request {
+	struct hostif_hdr header;
+} __packed;
+
+struct hostif_mic_failure_request {
+	struct hostif_hdr header;
+	__le16 failure_count;
+	__le16 timer;
+} __packed;
+
+#define BASIC_RATE	0x80
+#define RATE_MASK	0x7F
+
+#define TX_RATE_AUTO      0xff
+#define TX_RATE_1M_FIXED  0
+#define TX_RATE_2M_FIXED  1
+#define TX_RATE_1_2M_AUTO 2
+#define TX_RATE_5M_FIXED  3
+#define TX_RATE_11M_FIXED 4
+
+#define TX_RATE_FULL_AUTO	0
+#define TX_RATE_11_AUTO		1
+#define TX_RATE_11B_AUTO	2
+#define TX_RATE_11BG_AUTO	3
+#define TX_RATE_MANUAL_AUTO	4
+#define TX_RATE_FIXED		5
+
+/* 11b rate */
+#define TX_RATE_1M	((u8)(10 / 5))	/* 11b 11g basic rate */
+#define TX_RATE_2M	((u8)(20 / 5))	/* 11b 11g basic rate */
+#define TX_RATE_5M	((u8)(55 / 5))	/* 11g basic rate */
+#define TX_RATE_11M	((u8)(110 / 5))	/* 11g basic rate */
+
+/* 11g rate */
+#define TX_RATE_6M	((u8)(60 / 5))	/* 11g basic rate */
+#define TX_RATE_12M	((u8)(120 / 5))	/* 11g basic rate */
+#define TX_RATE_24M	((u8)(240 / 5))	/* 11g basic rate */
+#define TX_RATE_9M	((u8)(90 / 5))
+#define TX_RATE_18M	((u8)(180 / 5))
+#define TX_RATE_36M	((u8)(360 / 5))
+#define TX_RATE_48M	((u8)(480 / 5))
+#define TX_RATE_54M	((u8)(540 / 5))
+
+static inline bool is_11b_rate(u8 rate)
+{
+	return (((rate & RATE_MASK) == TX_RATE_1M) ||
+		((rate & RATE_MASK) == TX_RATE_2M) ||
+		((rate & RATE_MASK) == TX_RATE_5M) ||
+		((rate & RATE_MASK) == TX_RATE_11M));
+}
+
+static inline bool is_ofdm_rate(u8 rate)
+{
+	return (((rate & RATE_MASK) == TX_RATE_6M)  ||
+		((rate & RATE_MASK) == TX_RATE_12M) ||
+		((rate & RATE_MASK) == TX_RATE_24M) ||
+		((rate & RATE_MASK) == TX_RATE_9M)  ||
+		((rate & RATE_MASK) == TX_RATE_18M) ||
+		((rate & RATE_MASK) == TX_RATE_36M) ||
+		((rate & RATE_MASK) == TX_RATE_48M) ||
+		((rate & RATE_MASK) == TX_RATE_54M));
+}
+
+static inline bool is_11bg_rate(u8 rate)
+{
+	return (is_11b_rate(rate) || is_ofdm_rate(rate));
+}
+
+static inline bool is_ofdm_ext_rate(u8 rate)
+{
+	return (((rate & RATE_MASK) == TX_RATE_9M)  ||
+		((rate & RATE_MASK) == TX_RATE_18M) ||
+		((rate & RATE_MASK) == TX_RATE_36M) ||
+		((rate & RATE_MASK) == TX_RATE_48M) ||
+		((rate & RATE_MASK) == TX_RATE_54M));
+}
+
+enum connect_status_type {
+	CONNECT_STATUS,
+	DISCONNECT_STATUS
+};
+
+enum preamble_type {
+	LONG_PREAMBLE,
+	SHORT_PREAMBLE
+};
+
+enum multicast_filter_type {
+	MCAST_FILTER_MCAST,
+	MCAST_FILTER_MCASTALL,
+	MCAST_FILTER_PROMISC,
+};
+
+#define NIC_MAX_MCAST_LIST 32
+
+#define HIF_EVENT_MASK 0xE800
+
+static inline bool is_hif_ind(unsigned short event)
+{
+	return (((event & HIF_EVENT_MASK) == HIF_EVENT_MASK) &&
+		(((event & ~HIF_EVENT_MASK) == 0x0001) ||
+		 ((event & ~HIF_EVENT_MASK) == 0x0006) ||
+		 ((event & ~HIF_EVENT_MASK) == 0x000C) ||
+		 ((event & ~HIF_EVENT_MASK) == 0x0011) ||
+		 ((event & ~HIF_EVENT_MASK) == 0x0012)));
+}
+
+static inline bool is_hif_conf(unsigned short event)
+{
+	return (((event & HIF_EVENT_MASK) == HIF_EVENT_MASK) &&
+		((event & ~HIF_EVENT_MASK) > 0x0000) &&
+		((event & ~HIF_EVENT_MASK) < 0x0012) &&
+		!is_hif_ind(event));
+}
+
+#ifdef __KERNEL__
+
+#include "ks_wlan.h"
+
+/* function prototype */
+int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *skb);
+void hostif_receive(struct ks_wlan_private *priv, unsigned char *p,
+		    unsigned int size);
+void hostif_sme_enqueue(struct ks_wlan_private *priv, u16 event);
+int hostif_init(struct ks_wlan_private *priv);
+void hostif_exit(struct ks_wlan_private *priv);
+int ks_wlan_hw_tx(struct ks_wlan_private *priv, void *p, unsigned long size,
+		  void (*complete_handler)(struct ks_wlan_private *priv,
+					   struct sk_buff *skb),
+		  struct sk_buff *skb);
+void send_packet_complete(struct ks_wlan_private *priv, struct sk_buff *skb);
+
+void ks_wlan_hw_wakeup_request(struct ks_wlan_private *priv);
+int ks_wlan_hw_power_save(struct ks_wlan_private *priv);
+
+#define KS7010_SIZE_ALIGNMENT	32
+
+static inline size_t hif_align_size(size_t size)
+{
+	return ALIGN(size, KS7010_SIZE_ALIGNMENT);
+}
+
+#endif /* __KERNEL__ */
+
+#endif /* _KS_HOSTIF_H_ */
diff --git a/drivers/staging/ks7010/ks_wlan.h b/drivers/staging/ks7010/ks_wlan.h
new file mode 100644
index 000000000..3e9a91b51
--- /dev/null
+++ b/drivers/staging/ks7010/ks_wlan.h
@@ -0,0 +1,567 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *   Driver for KeyStream IEEE802.11 b/g wireless LAN cards.
+ *
+ *   Copyright (C) 2006-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ */
+
+#ifndef _KS_WLAN_H
+#define _KS_WLAN_H
+
+#include <linux/atomic.h>
+#include <linux/circ_buf.h>
+#include <linux/completion.h>
+#include <linux/netdevice.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/wireless.h>
+
+struct ks_wlan_parameter {
+	u8 operation_mode;
+	u8 channel;
+	u8 tx_rate;
+	struct {
+		u8 size;
+		u8 body[16];
+	} rate_set;
+	u8 bssid[ETH_ALEN];
+	struct {
+		u8 size;
+		u8 body[32 + 1];
+	} ssid;
+	u8 preamble;
+	u8 power_mgmt;
+	u32 scan_type;
+#define BEACON_LOST_COUNT_MAX 65535
+	u32 beacon_lost_count;
+	u32 rts;
+	u32 fragment;
+	u32 privacy_invoked;
+	u32 wep_index;
+	struct {
+		u8 size;
+		u8 val[13 * 2 + 1];
+	} wep_key[4];
+	u16 authenticate_type;
+	u16 phy_type;
+	u16 cts_mode;
+	u16 phy_info_timer;
+};
+
+enum {
+	DEVICE_STATE_OFF = 0,	/* this means hw_unavailable is != 0 */
+	DEVICE_STATE_PREBOOT,	/* we are in a pre-boot state (empty RAM) */
+	DEVICE_STATE_BOOT,	/* boot state (fw upload, run fw) */
+	DEVICE_STATE_PREINIT,	/* pre-init state */
+	DEVICE_STATE_INIT,	/* init state (restore MIB backup to device) */
+	DEVICE_STATE_READY,	/* driver&device are in operational state */
+	DEVICE_STATE_SLEEP	/* device in sleep mode */
+};
+
+/* SME flag */
+#define SME_MODE_SET	    BIT(0)
+#define SME_RTS             BIT(1)
+#define SME_FRAG            BIT(2)
+#define SME_WEP_FLAG        BIT(3)
+#define SME_WEP_INDEX       BIT(4)
+#define SME_WEP_VAL1        BIT(5)
+#define SME_WEP_VAL2        BIT(6)
+#define SME_WEP_VAL3        BIT(7)
+#define SME_WEP_VAL4        BIT(8)
+#define SME_WEP_VAL_MASK    GENMASK(8, 5)
+#define SME_RSN             BIT(9)
+#define SME_RSN_MULTICAST   BIT(10)
+#define SME_RSN_UNICAST	    BIT(11)
+#define SME_RSN_AUTH	    BIT(12)
+
+#define SME_AP_SCAN         BIT(13)
+#define SME_MULTICAST       BIT(14)
+
+/* SME Event */
+enum {
+	SME_START,
+
+	SME_MULTICAST_REQUEST,
+	SME_MACADDRESS_SET_REQUEST,
+	SME_BSS_SCAN_REQUEST,
+	SME_SET_FLAG,
+	SME_SET_TXKEY,
+	SME_SET_KEY1,
+	SME_SET_KEY2,
+	SME_SET_KEY3,
+	SME_SET_KEY4,
+	SME_SET_PMK_TSC,
+	SME_SET_GMK1_TSC,
+	SME_SET_GMK2_TSC,
+	SME_SET_GMK3_TSC,
+	SME_SET_PMKSA,
+	SME_POW_MNGMT_REQUEST,
+	SME_PHY_INFO_REQUEST,
+	SME_MIC_FAILURE_REQUEST,
+	SME_GET_MAC_ADDRESS,
+	SME_GET_PRODUCT_VERSION,
+	SME_STOP_REQUEST,
+	SME_RTS_THRESHOLD_REQUEST,
+	SME_FRAGMENTATION_THRESHOLD_REQUEST,
+	SME_WEP_INDEX_REQUEST,
+	SME_WEP_KEY1_REQUEST,
+	SME_WEP_KEY2_REQUEST,
+	SME_WEP_KEY3_REQUEST,
+	SME_WEP_KEY4_REQUEST,
+	SME_WEP_FLAG_REQUEST,
+	SME_RSN_UCAST_REQUEST,
+	SME_RSN_MCAST_REQUEST,
+	SME_RSN_AUTH_REQUEST,
+	SME_RSN_ENABLED_REQUEST,
+	SME_RSN_MODE_REQUEST,
+	SME_WPS_ENABLE_REQUEST,
+	SME_WPS_PROBE_REQUEST,
+	SME_SET_GAIN,
+	SME_GET_GAIN,
+	SME_SLEEP_REQUEST,
+	SME_SET_REGION,
+	SME_MODE_SET_REQUEST,
+	SME_START_REQUEST,
+	SME_GET_EEPROM_CKSUM,
+
+	SME_MIC_FAILURE_CONFIRM,
+	SME_START_CONFIRM,
+
+	SME_MULTICAST_CONFIRM,
+	SME_BSS_SCAN_CONFIRM,
+	SME_GET_CURRENT_AP,
+	SME_POW_MNGMT_CONFIRM,
+	SME_PHY_INFO_CONFIRM,
+	SME_STOP_CONFIRM,
+	SME_RTS_THRESHOLD_CONFIRM,
+	SME_FRAGMENTATION_THRESHOLD_CONFIRM,
+	SME_WEP_INDEX_CONFIRM,
+	SME_WEP_KEY1_CONFIRM,
+	SME_WEP_KEY2_CONFIRM,
+	SME_WEP_KEY3_CONFIRM,
+	SME_WEP_KEY4_CONFIRM,
+	SME_WEP_FLAG_CONFIRM,
+	SME_RSN_UCAST_CONFIRM,
+	SME_RSN_MCAST_CONFIRM,
+	SME_RSN_AUTH_CONFIRM,
+	SME_RSN_ENABLED_CONFIRM,
+	SME_RSN_MODE_CONFIRM,
+	SME_MODE_SET_CONFIRM,
+	SME_SLEEP_CONFIRM,
+
+	SME_RSN_SET_CONFIRM,
+	SME_WEP_SET_CONFIRM,
+	SME_TERMINATE,
+
+	SME_EVENT_SIZE
+};
+
+/* SME Status */
+enum {
+	SME_IDLE,
+	SME_SETUP,
+	SME_DISCONNECT,
+	SME_CONNECT
+};
+
+#define	SME_EVENT_BUFF_SIZE	128
+
+struct sme_info {
+	int sme_status;
+	int event_buff[SME_EVENT_BUFF_SIZE];
+	unsigned int qhead;
+	unsigned int qtail;
+	spinlock_t sme_spin;
+	unsigned long sme_flag;
+};
+
+struct hostt {
+	int buff[SME_EVENT_BUFF_SIZE];
+	unsigned int qhead;
+	unsigned int qtail;
+};
+
+#define RSN_IE_BODY_MAX 64
+struct rsn_ie {
+	u8 id;	/* 0xdd = WPA or 0x30 = RSN */
+	u8 size;	/* max ? 255 ? */
+	u8 body[RSN_IE_BODY_MAX];
+} __packed;
+
+#define WPA_INFO_ELEM_ID 0xdd
+#define RSN_INFO_ELEM_ID 0x30
+
+#define WPS_IE_BODY_MAX 255
+struct wps_ie {
+	u8 id;	/* 221 'dd <len> 00 50 F2 04' */
+	u8 size;	/* max ? 255 ? */
+	u8 body[WPS_IE_BODY_MAX];
+} __packed;
+
+struct local_ap {
+	u8 bssid[6];
+	u8 rssi;
+	u8 sq;
+	struct {
+		u8 size;
+		u8 body[32];
+		u8 ssid_pad;
+	} ssid;
+	struct {
+		u8 size;
+		u8 body[16];
+		u8 rate_pad;
+	} rate_set;
+	u16 capability;
+	u8 channel;
+	u8 noise;
+	struct rsn_ie wpa_ie;
+	struct rsn_ie rsn_ie;
+	struct wps_ie wps_ie;
+};
+
+#define LOCAL_APLIST_MAX 31
+#define LOCAL_CURRENT_AP LOCAL_APLIST_MAX
+struct local_aplist {
+	int size;
+	struct local_ap ap[LOCAL_APLIST_MAX + 1];
+};
+
+struct local_gain {
+	u8 tx_mode;
+	u8 rx_mode;
+	u8 tx_gain;
+	u8 rx_gain;
+};
+
+struct local_eeprom_sum {
+	u8 type;
+	u8 result;
+};
+
+enum {
+	EEPROM_OK,
+	EEPROM_CHECKSUM_NONE,
+	EEPROM_FW_NOT_SUPPORT,
+	EEPROM_NG,
+};
+
+/* Power Save Status */
+enum {
+	PS_NONE,
+	PS_ACTIVE_SET,
+	PS_SAVE_SET,
+	PS_CONF_WAIT,
+	PS_SNOOZE,
+	PS_WAKEUP
+};
+
+struct power_save_status {
+	atomic_t status;	/* initialvalue 0 */
+	struct completion wakeup_wait;
+	atomic_t confirm_wait;
+	atomic_t snooze_guard;
+};
+
+struct sleep_status {
+	atomic_t status;	/* initialvalue 0 */
+	atomic_t doze_request;
+	atomic_t wakeup_request;
+};
+
+/* WPA */
+struct scan_ext {
+	unsigned int flag;
+	char ssid[IW_ESSID_MAX_SIZE + 1];
+};
+
+#define CIPHER_ID_WPA_NONE    "\x00\x50\xf2\x00"
+#define CIPHER_ID_WPA_WEP40   "\x00\x50\xf2\x01"
+#define CIPHER_ID_WPA_TKIP    "\x00\x50\xf2\x02"
+#define CIPHER_ID_WPA_CCMP    "\x00\x50\xf2\x04"
+#define CIPHER_ID_WPA_WEP104  "\x00\x50\xf2\x05"
+
+#define CIPHER_ID_WPA2_NONE   "\x00\x0f\xac\x00"
+#define CIPHER_ID_WPA2_WEP40  "\x00\x0f\xac\x01"
+#define CIPHER_ID_WPA2_TKIP   "\x00\x0f\xac\x02"
+#define CIPHER_ID_WPA2_CCMP   "\x00\x0f\xac\x04"
+#define CIPHER_ID_WPA2_WEP104 "\x00\x0f\xac\x05"
+
+#define CIPHER_ID_LEN    4
+
+enum {
+	KEY_MGMT_802_1X,
+	KEY_MGMT_PSK,
+	KEY_MGMT_WPANONE,
+};
+
+#define KEY_MGMT_ID_WPA_NONE     "\x00\x50\xf2\x00"
+#define KEY_MGMT_ID_WPA_1X       "\x00\x50\xf2\x01"
+#define KEY_MGMT_ID_WPA_PSK      "\x00\x50\xf2\x02"
+#define KEY_MGMT_ID_WPA_WPANONE  "\x00\x50\xf2\xff"
+
+#define KEY_MGMT_ID_WPA2_NONE    "\x00\x0f\xac\x00"
+#define KEY_MGMT_ID_WPA2_1X      "\x00\x0f\xac\x01"
+#define KEY_MGMT_ID_WPA2_PSK     "\x00\x0f\xac\x02"
+#define KEY_MGMT_ID_WPA2_WPANONE "\x00\x0f\xac\xff"
+
+#define KEY_MGMT_ID_LEN  4
+
+#define MIC_KEY_SIZE 8
+
+struct wpa_key {
+	u32 ext_flags;	/* IW_ENCODE_EXT_xxx */
+	u8 tx_seq[IW_ENCODE_SEQ_MAX_SIZE];	/* LSB first */
+	u8 rx_seq[IW_ENCODE_SEQ_MAX_SIZE];	/* LSB first */
+	struct sockaddr addr;	/* ff:ff:ff:ff:ff:ff for broadcast/multicast
+				 * (group) keys or unicast address for
+				 * individual keys
+				 */
+	u16 alg;
+	u16 key_len;	/* WEP: 5 or 13, TKIP: 32, CCMP: 16 */
+	u8 key_val[IW_ENCODING_TOKEN_MAX];
+	u8 tx_mic_key[MIC_KEY_SIZE];
+	u8 rx_mic_key[MIC_KEY_SIZE];
+};
+
+#define WPA_KEY_INDEX_MAX 4
+#define WPA_RX_SEQ_LEN 6
+
+struct mic_failure {
+	u16 failure;	/* MIC Failure counter 0 or 1 or 2 */
+	u16 counter;	/* 1sec counter 0-60 */
+	u32 last_failure_time;
+	int stop;
+};
+
+struct wpa_status {
+	int wpa_enabled;
+	bool rsn_enabled;
+	int version;
+	int pairwise_suite;	/* unicast cipher */
+	int group_suite;	/* multicast cipher */
+	int key_mgmt_suite;
+	int auth_alg;
+	int txkey;
+	struct wpa_key key[WPA_KEY_INDEX_MAX];
+	struct scan_ext scan_ext;
+	struct mic_failure mic_failure;
+};
+
+#include <linux/list.h>
+#define PMK_LIST_MAX 8
+struct pmk_list {
+	u16 size;
+	struct list_head head;
+	struct pmk {
+		struct list_head list;
+		u8 bssid[ETH_ALEN];
+		u8 pmkid[IW_PMKID_LEN];
+	} pmk[PMK_LIST_MAX];
+};
+
+struct wps_status {
+	int wps_enabled;
+	int ielen;
+	u8 ie[255];
+};
+
+/* Tx Device struct */
+#define	TX_DEVICE_BUFF_SIZE	1024
+
+struct ks_wlan_private;
+
+/**
+ * struct tx_device_buffer - Queue item for the tx queue.
+ * @sendp: Pointer to the send request data.
+ * @size: Size of @sendp data.
+ * @complete_handler: Function called once data write to device is complete.
+ * @arg1: First argument to @complete_handler.
+ * @arg2: Second argument to @complete_handler.
+ */
+struct tx_device_buffer {
+	unsigned char *sendp;
+	unsigned int size;
+	void (*complete_handler)(struct ks_wlan_private *priv,
+				 struct sk_buff *skb);
+	struct sk_buff *skb;
+};
+
+/**
+ * struct tx_device - Tx buffer queue.
+ * @tx_device_buffer: Queue buffer.
+ * @qhead: Head of tx queue.
+ * @qtail: Tail of tx queue.
+ * @tx_dev_lock: Queue lock.
+ */
+struct tx_device {
+	struct tx_device_buffer tx_dev_buff[TX_DEVICE_BUFF_SIZE];
+	unsigned int qhead;
+	unsigned int qtail;
+	spinlock_t tx_dev_lock;	/* protect access to the queue */
+};
+
+/* Rx Device struct */
+#define	RX_DATA_SIZE	(2 + 2 + 2347 + 1)
+#define	RX_DEVICE_BUFF_SIZE	32
+
+/**
+ * struct rx_device_buffer - Queue item for the rx queue.
+ * @data: rx data.
+ * @size: Size of @data.
+ */
+struct rx_device_buffer {
+	unsigned char data[RX_DATA_SIZE];
+	unsigned int size;
+};
+
+/**
+ * struct rx_device - Rx buffer queue.
+ * @rx_device_buffer: Queue buffer.
+ * @qhead: Head of rx queue.
+ * @qtail: Tail of rx queue.
+ * @rx_dev_lock: Queue lock.
+ */
+struct rx_device {
+	struct rx_device_buffer rx_dev_buff[RX_DEVICE_BUFF_SIZE];
+	unsigned int qhead;
+	unsigned int qtail;
+	spinlock_t rx_dev_lock;	/* protect access to the queue */
+};
+
+struct ks_wlan_private {
+	/* hardware information */
+	void *if_hw;
+	struct workqueue_struct *wq;
+	struct delayed_work rw_dwork;
+	struct tasklet_struct rx_bh_task;
+
+	struct net_device *net_dev;
+	struct net_device_stats nstats;
+	struct iw_statistics wstats;
+
+	struct completion confirm_wait;
+
+	/* trx device & sme */
+	struct tx_device tx_dev;
+	struct rx_device rx_dev;
+	struct sme_info sme_i;
+	u8 *rxp;
+	unsigned int rx_size;
+	struct work_struct sme_work;
+	struct work_struct wakeup_work;
+	int scan_ind_count;
+
+	unsigned char eth_addr[ETH_ALEN];
+
+	struct local_aplist aplist;
+	struct local_ap current_ap;
+	struct power_save_status psstatus;
+	struct sleep_status sleepstatus;
+	struct wpa_status wpa;
+	struct pmk_list pmklist;
+	/* wireless parameter */
+	struct ks_wlan_parameter reg;
+	u8 current_rate;
+
+	char nick[IW_ESSID_MAX_SIZE + 1];
+
+	spinlock_t multicast_spin;
+
+	spinlock_t dev_read_lock;
+	wait_queue_head_t devread_wait;
+
+	unsigned int need_commit;	/* for ioctl */
+
+	/* DeviceIoControl */
+	bool is_device_open;
+	atomic_t event_count;
+	atomic_t rec_count;
+	int dev_count;
+#define DEVICE_STOCK_COUNT 20
+	unsigned char *dev_data[DEVICE_STOCK_COUNT];
+	int dev_size[DEVICE_STOCK_COUNT];
+
+	/* ioctl : IOCTL_FIRMWARE_VERSION */
+	unsigned char firmware_version[128 + 1];
+	int version_size;
+
+	bool mac_address_valid;
+
+	int dev_state;
+
+	struct sk_buff *skb;
+	unsigned int cur_rx;	/* Index into the Rx buffer of next Rx pkt. */
+#define FORCE_DISCONNECT    0x80000000
+#define CONNECT_STATUS_MASK 0x7FFFFFFF
+	u32 connect_status;
+	int infra_status;
+	u8 scan_ssid_len;
+	u8 scan_ssid[IW_ESSID_MAX_SIZE + 1];
+	struct local_gain gain;
+	struct wps_status wps;
+	u8 sleep_mode;
+
+	u8 region;
+	struct local_eeprom_sum eeprom_sum;
+	u8 eeprom_checksum;
+
+	struct hostt hostt;
+
+	unsigned long last_doze;
+	unsigned long last_wakeup;
+
+	unsigned int wakeup_count;	/* for detect wakeup loop */
+};
+
+static inline void inc_txqhead(struct ks_wlan_private *priv)
+{
+	priv->tx_dev.qhead = (priv->tx_dev.qhead + 1) % TX_DEVICE_BUFF_SIZE;
+}
+
+static inline void inc_txqtail(struct ks_wlan_private *priv)
+{
+	priv->tx_dev.qtail = (priv->tx_dev.qtail + 1) % TX_DEVICE_BUFF_SIZE;
+}
+
+static inline bool txq_has_space(struct ks_wlan_private *priv)
+{
+	return (CIRC_SPACE(priv->tx_dev.qhead, priv->tx_dev.qtail,
+			   TX_DEVICE_BUFF_SIZE) > 0);
+}
+
+static inline void inc_rxqhead(struct ks_wlan_private *priv)
+{
+	priv->rx_dev.qhead = (priv->rx_dev.qhead + 1) % RX_DEVICE_BUFF_SIZE;
+}
+
+static inline void inc_rxqtail(struct ks_wlan_private *priv)
+{
+	priv->rx_dev.qtail = (priv->rx_dev.qtail + 1) % RX_DEVICE_BUFF_SIZE;
+}
+
+static inline bool rxq_has_space(struct ks_wlan_private *priv)
+{
+	return (CIRC_SPACE(priv->rx_dev.qhead, priv->rx_dev.qtail,
+			   RX_DEVICE_BUFF_SIZE) > 0);
+}
+
+static inline unsigned int txq_count(struct ks_wlan_private *priv)
+{
+	return CIRC_CNT_TO_END(priv->tx_dev.qhead, priv->tx_dev.qtail,
+			       TX_DEVICE_BUFF_SIZE);
+}
+
+static inline unsigned int rxq_count(struct ks_wlan_private *priv)
+{
+	return CIRC_CNT_TO_END(priv->rx_dev.qhead, priv->rx_dev.qtail,
+			       RX_DEVICE_BUFF_SIZE);
+}
+
+int ks_wlan_net_start(struct net_device *dev);
+int ks_wlan_net_stop(struct net_device *dev);
+bool is_connect_status(u32 status);
+bool is_disconnect_status(u32 status);
+
+#endif /* _KS_WLAN_H */
diff --git a/drivers/staging/ks7010/ks_wlan_ioctl.h b/drivers/staging/ks7010/ks_wlan_ioctl.h
new file mode 100644
index 000000000..97c7d95de
--- /dev/null
+++ b/drivers/staging/ks7010/ks_wlan_ioctl.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *   Driver for KeyStream 11b/g wireless LAN
+ *
+ *   Copyright (c) 2005-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ */
+
+#ifndef _KS_WLAN_IOCTL_H
+#define _KS_WLAN_IOCTL_H
+
+#include <linux/wireless.h>
+/* The low order bit identify a SET (0) or a GET (1) ioctl.  */
+
+/*					(SIOCIWFIRSTPRIV + 0) */
+/* former KS_WLAN_GET_DRIVER_VERSION	(SIOCIWFIRSTPRIV + 1) */
+/*					(SIOCIWFIRSTPRIV + 2) */
+#define KS_WLAN_GET_FIRM_VERSION	(SIOCIWFIRSTPRIV + 3)
+#define KS_WLAN_SET_WPS_ENABLE		(SIOCIWFIRSTPRIV + 4)
+#define KS_WLAN_GET_WPS_ENABLE		(SIOCIWFIRSTPRIV + 5)
+#define KS_WLAN_SET_WPS_PROBE_REQ	(SIOCIWFIRSTPRIV + 6)
+#define KS_WLAN_GET_EEPROM_CKSUM	(SIOCIWFIRSTPRIV + 7)
+#define KS_WLAN_SET_PREAMBLE		(SIOCIWFIRSTPRIV + 8)
+#define KS_WLAN_GET_PREAMBLE		(SIOCIWFIRSTPRIV + 9)
+#define KS_WLAN_SET_POWER_SAVE		(SIOCIWFIRSTPRIV + 10)
+#define KS_WLAN_GET_POWER_SAVE		(SIOCIWFIRSTPRIV + 11)
+#define KS_WLAN_SET_SCAN_TYPE		(SIOCIWFIRSTPRIV + 12)
+#define KS_WLAN_GET_SCAN_TYPE		(SIOCIWFIRSTPRIV + 13)
+#define KS_WLAN_SET_RX_GAIN		(SIOCIWFIRSTPRIV + 14)
+#define KS_WLAN_GET_RX_GAIN		(SIOCIWFIRSTPRIV + 15)
+#define KS_WLAN_HOSTT			(SIOCIWFIRSTPRIV + 16)	/* unused */
+//#define KS_WLAN_SET_REGION            (SIOCIWFIRSTPRIV + 17)
+#define KS_WLAN_SET_BEACON_LOST		(SIOCIWFIRSTPRIV + 18)
+#define KS_WLAN_GET_BEACON_LOST		(SIOCIWFIRSTPRIV + 19)
+
+#define KS_WLAN_SET_TX_GAIN		(SIOCIWFIRSTPRIV + 20)
+#define KS_WLAN_GET_TX_GAIN		(SIOCIWFIRSTPRIV + 21)
+
+/* for KS7010 */
+#define KS_WLAN_SET_PHY_TYPE		(SIOCIWFIRSTPRIV + 22)
+#define KS_WLAN_GET_PHY_TYPE		(SIOCIWFIRSTPRIV + 23)
+#define KS_WLAN_SET_CTS_MODE		(SIOCIWFIRSTPRIV + 24)
+#define KS_WLAN_GET_CTS_MODE		(SIOCIWFIRSTPRIV + 25)
+/*					(SIOCIWFIRSTPRIV + 26) */
+/*					(SIOCIWFIRSTPRIV + 27) */
+#define KS_WLAN_SET_SLEEP_MODE		(SIOCIWFIRSTPRIV + 28)	/* sleep mode */
+#define KS_WLAN_GET_SLEEP_MODE		(SIOCIWFIRSTPRIV + 29)	/* sleep mode */
+/*					(SIOCIWFIRSTPRIV + 30) */
+/*					(SIOCIWFIRSTPRIV + 31) */
+
+#ifdef __KERNEL__
+
+#include "ks_wlan.h"
+#include <linux/netdevice.h>
+
+int ks_wlan_setup_parameter(struct ks_wlan_private *priv,
+			    unsigned int commit_flag);
+
+#endif /* __KERNEL__ */
+
+#endif /* _KS_WLAN_IOCTL_H */
diff --git a/drivers/staging/ks7010/ks_wlan_net.c b/drivers/staging/ks7010/ks_wlan_net.c
new file mode 100644
index 000000000..f389e227a
--- /dev/null
+++ b/drivers/staging/ks7010/ks_wlan_net.c
@@ -0,0 +1,2673 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *   Driver for KeyStream 11b/g wireless LAN
+ *
+ *   Copyright (C) 2005-2008 KeyStream Corp.
+ *   Copyright (C) 2009 Renesas Technology Corp.
+ */
+
+#include <linux/atomic.h>
+#include <linux/completion.h>
+#include <linux/if_arp.h>
+#include <linux/netdevice.h>
+#include <linux/timer.h>
+#include <linux/uaccess.h>
+
+static int wep_on_off;
+#define	WEP_OFF		0
+#define	WEP_ON_64BIT	1
+#define	WEP_ON_128BIT	2
+
+#include "ks_wlan.h"
+#include "ks_hostif.h"
+#include "ks_wlan_ioctl.h"
+
+/* Include Wireless Extension definition and check version */
+#include <linux/wireless.h>
+#define WIRELESS_SPY	/* enable iwspy support */
+#include <net/iw_handler.h>	/* New driver API */
+
+/* Frequency list (map channels to frequencies) */
+static const long frequency_list[] = {
+	2412, 2417, 2422, 2427, 2432, 2437, 2442,
+	2447, 2452, 2457, 2462, 2467, 2472, 2484
+};
+
+/* A few details needed for WEP (Wireless Equivalent Privacy) */
+#define MAX_KEY_SIZE 13	/* 128 (?) bits */
+#define MIN_KEY_SIZE  5	/* 40 bits RC4 - WEP */
+struct wep_key {
+	u16 len;
+	u8 key[16];	/* 40-bit and 104-bit keys */
+};
+
+/*
+ *	function prototypes
+ */
+static int ks_wlan_open(struct net_device *dev);
+static void ks_wlan_tx_timeout(struct net_device *dev, unsigned int txqueue);
+static netdev_tx_t ks_wlan_start_xmit(struct sk_buff *skb, struct net_device *dev);
+static int ks_wlan_close(struct net_device *dev);
+static void ks_wlan_set_rx_mode(struct net_device *dev);
+static struct net_device_stats *ks_wlan_get_stats(struct net_device *dev);
+static int ks_wlan_set_mac_address(struct net_device *dev, void *addr);
+static int ks_wlan_netdev_ioctl(struct net_device *dev, struct ifreq *rq,
+				int cmd);
+
+static atomic_t update_phyinfo;
+static struct timer_list update_phyinfo_timer;
+static
+int ks_wlan_update_phy_information(struct ks_wlan_private *priv)
+{
+	struct iw_statistics *wstats = &priv->wstats;
+
+	netdev_dbg(priv->net_dev, "in_interrupt = %ld\n", in_interrupt());
+
+	if (priv->dev_state < DEVICE_STATE_READY)
+		return -EBUSY;	/* not finished initialize */
+
+	if (atomic_read(&update_phyinfo))
+		return -EPERM;
+
+	/* The status */
+	wstats->status = priv->reg.operation_mode;	/* Operation mode */
+
+	/* Signal quality and co. But where is the noise level ??? */
+	hostif_sme_enqueue(priv, SME_PHY_INFO_REQUEST);
+
+	/* interruptible_sleep_on_timeout(&priv->confirm_wait, HZ/2); */
+	if (!wait_for_completion_interruptible_timeout
+	    (&priv->confirm_wait, HZ / 2)) {
+		netdev_dbg(priv->net_dev, "wait time out!!\n");
+	}
+
+	atomic_inc(&update_phyinfo);
+	update_phyinfo_timer.expires = jiffies + HZ;	/* 1sec */
+	add_timer(&update_phyinfo_timer);
+
+	return 0;
+}
+
+static
+void ks_wlan_update_phyinfo_timeout(struct timer_list *unused)
+{
+	pr_debug("in_interrupt = %ld\n", in_interrupt());
+	atomic_set(&update_phyinfo, 0);
+}
+
+int ks_wlan_setup_parameter(struct ks_wlan_private *priv,
+			    unsigned int commit_flag)
+{
+	hostif_sme_enqueue(priv, SME_STOP_REQUEST);
+
+	if (commit_flag & SME_RTS)
+		hostif_sme_enqueue(priv, SME_RTS_THRESHOLD_REQUEST);
+	if (commit_flag & SME_FRAG)
+		hostif_sme_enqueue(priv, SME_FRAGMENTATION_THRESHOLD_REQUEST);
+
+	if (commit_flag & SME_WEP_INDEX)
+		hostif_sme_enqueue(priv, SME_WEP_INDEX_REQUEST);
+	if (commit_flag & SME_WEP_VAL1)
+		hostif_sme_enqueue(priv, SME_WEP_KEY1_REQUEST);
+	if (commit_flag & SME_WEP_VAL2)
+		hostif_sme_enqueue(priv, SME_WEP_KEY2_REQUEST);
+	if (commit_flag & SME_WEP_VAL3)
+		hostif_sme_enqueue(priv, SME_WEP_KEY3_REQUEST);
+	if (commit_flag & SME_WEP_VAL4)
+		hostif_sme_enqueue(priv, SME_WEP_KEY4_REQUEST);
+	if (commit_flag & SME_WEP_FLAG)
+		hostif_sme_enqueue(priv, SME_WEP_FLAG_REQUEST);
+
+	if (commit_flag & SME_RSN) {
+		hostif_sme_enqueue(priv, SME_RSN_ENABLED_REQUEST);
+		hostif_sme_enqueue(priv, SME_RSN_MODE_REQUEST);
+	}
+	if (commit_flag & SME_RSN_MULTICAST)
+		hostif_sme_enqueue(priv, SME_RSN_MCAST_REQUEST);
+	if (commit_flag & SME_RSN_UNICAST)
+		hostif_sme_enqueue(priv, SME_RSN_UCAST_REQUEST);
+	if (commit_flag & SME_RSN_AUTH)
+		hostif_sme_enqueue(priv, SME_RSN_AUTH_REQUEST);
+
+	hostif_sme_enqueue(priv, SME_MODE_SET_REQUEST);
+
+	hostif_sme_enqueue(priv, SME_START_REQUEST);
+
+	return 0;
+}
+
+/*
+ * Initial Wireless Extension code for Ks_Wlannet driver by :
+ *	Jean Tourrilhes <jt@hpl.hp.com> - HPL - 17 November 00
+ * Conversion to new driver API by :
+ *	Jean Tourrilhes <jt@hpl.hp.com> - HPL - 26 March 02
+ * Javier also did a good amount of work here, adding some new extensions
+ * and fixing my code. Let's just say that without him this code just
+ * would not work at all... - Jean II
+ */
+
+static int ks_wlan_get_name(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *cwrq,
+			    char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (priv->dev_state < DEVICE_STATE_READY)
+		strscpy(cwrq->name, "NOT READY!", sizeof(cwrq->name));
+	else if (priv->reg.phy_type == D_11B_ONLY_MODE)
+		strscpy(cwrq->name, "IEEE 802.11b", sizeof(cwrq->name));
+	else if (priv->reg.phy_type == D_11G_ONLY_MODE)
+		strscpy(cwrq->name, "IEEE 802.11g", sizeof(cwrq->name));
+	else
+		strscpy(cwrq->name, "IEEE 802.11b/g", sizeof(cwrq->name));
+
+	return 0;
+}
+
+static int ks_wlan_set_freq(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *fwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int channel;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* If setting by frequency, convert to a channel */
+	if ((fwrq->freq.e == 1) &&
+	    (fwrq->freq.m >= 241200000) && (fwrq->freq.m <= 248700000)) {
+		int f = fwrq->freq.m / 100000;
+		int c = 0;
+
+		while ((c < 14) && (f != frequency_list[c]))
+			c++;
+		/* Hack to fall through... */
+		fwrq->freq.e = 0;
+		fwrq->freq.m = c + 1;
+	}
+	/* Setting by channel number */
+	if ((fwrq->freq.m > 1000) || (fwrq->freq.e > 0))
+		return -EOPNOTSUPP;
+
+	channel = fwrq->freq.m;
+	/* We should do a better check than that,
+	 * based on the card capability !!!
+	 */
+	if ((channel < 1) || (channel > 14)) {
+		netdev_dbg(dev, "%s: New channel value of %d is invalid!\n",
+			   dev->name, fwrq->freq.m);
+		return -EINVAL;
+	}
+
+	/* Yes ! We can set it !!! */
+	priv->reg.channel = (u8)(channel);
+	priv->need_commit |= SME_MODE_SET;
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_freq(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *fwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int f;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (is_connect_status(priv->connect_status))
+		f = (int)priv->current_ap.channel;
+	else
+		f = (int)priv->reg.channel;
+
+	fwrq->freq.m = frequency_list[f - 1] * 100000;
+	fwrq->freq.e = 1;
+
+	return 0;
+}
+
+static int ks_wlan_set_essid(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	size_t len;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* Check if we asked for `any' */
+	if (!dwrq->essid.flags) {
+		/* Just send an empty SSID list */
+		memset(priv->reg.ssid.body, 0, sizeof(priv->reg.ssid.body));
+		priv->reg.ssid.size = 0;
+	} else {
+		len = dwrq->essid.length;
+		/* iwconfig uses nul termination in SSID.. */
+		if (len > 0 && extra[len - 1] == '\0')
+			len--;
+
+		/* Check the size of the string */
+		if (len > IW_ESSID_MAX_SIZE)
+			return -EINVAL;
+
+		/* Set the SSID */
+		memset(priv->reg.ssid.body, 0, sizeof(priv->reg.ssid.body));
+		memcpy(priv->reg.ssid.body, extra, len);
+		priv->reg.ssid.size = len;
+	}
+	/* Write it to the card */
+	priv->need_commit |= SME_MODE_SET;
+
+	ks_wlan_setup_parameter(priv, priv->need_commit);
+	priv->need_commit = 0;
+	return 0;
+}
+
+static int ks_wlan_get_essid(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* Note : if dwrq->flags != 0, we should
+	 * get the relevant SSID from the SSID list...
+	 */
+	if (priv->reg.ssid.size != 0) {
+		/* Get the current SSID */
+		memcpy(extra, priv->reg.ssid.body, priv->reg.ssid.size);
+
+		/* If none, we may want to get the one that was set */
+
+		/* Push it out ! */
+		dwrq->essid.length = priv->reg.ssid.size;
+		dwrq->essid.flags = 1;	/* active */
+	} else {
+		dwrq->essid.length = 0;
+		dwrq->essid.flags = 0;	/* ANY */
+	}
+
+	return 0;
+}
+
+static int ks_wlan_set_wap(struct net_device *dev, struct iw_request_info *info,
+			   union iwreq_data *awrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (priv->reg.operation_mode != MODE_ADHOC &&
+	    priv->reg.operation_mode != MODE_INFRASTRUCTURE) {
+		eth_zero_addr(priv->reg.bssid);
+		return -EOPNOTSUPP;
+	}
+
+	ether_addr_copy(priv->reg.bssid, awrq->ap_addr.sa_data);
+	if (is_valid_ether_addr((u8 *)priv->reg.bssid))
+		priv->need_commit |= SME_MODE_SET;
+
+	netdev_dbg(dev, "bssid = %pM\n", priv->reg.bssid);
+
+	/* Write it to the card */
+	if (priv->need_commit) {
+		priv->need_commit |= SME_MODE_SET;
+		return -EINPROGRESS;	/* Call commit handler */
+	}
+	return 0;
+}
+
+static int ks_wlan_get_wap(struct net_device *dev, struct iw_request_info *info,
+			   union iwreq_data *awrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (is_connect_status(priv->connect_status))
+		ether_addr_copy(awrq->ap_addr.sa_data, priv->current_ap.bssid);
+	else
+		eth_zero_addr(awrq->ap_addr.sa_data);
+
+	awrq->ap_addr.sa_family = ARPHRD_ETHER;
+
+	return 0;
+}
+
+static int ks_wlan_set_nick(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* Check the size of the string */
+	if (dwrq->data.length > 16 + 1)
+		return -E2BIG;
+
+	memset(priv->nick, 0, sizeof(priv->nick));
+	memcpy(priv->nick, extra, dwrq->data.length);
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_nick(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	strncpy(extra, priv->nick, 16);
+	extra[16] = '\0';
+	dwrq->data.length = strlen(extra) + 1;
+
+	return 0;
+}
+
+static int ks_wlan_set_rate(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int i = 0;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (priv->reg.phy_type == D_11B_ONLY_MODE) {
+		if (vwrq->bitrate.fixed == 1) {
+			switch (vwrq->bitrate.value) {
+			case 11000000:
+			case 5500000:
+				priv->reg.rate_set.body[0] =
+				    (u8)(vwrq->bitrate.value / 500000);
+				break;
+			case 2000000:
+			case 1000000:
+				priv->reg.rate_set.body[0] =
+				    ((u8)(vwrq->bitrate.value / 500000)) |
+				    BASIC_RATE;
+				break;
+			default:
+				return -EINVAL;
+			}
+			priv->reg.tx_rate = TX_RATE_FIXED;
+			priv->reg.rate_set.size = 1;
+		} else {	/* vwrq->fixed == 0 */
+			if (vwrq->bitrate.value > 0) {
+				switch (vwrq->bitrate.value) {
+				case 11000000:
+					priv->reg.rate_set.body[3] =
+					    TX_RATE_11M;
+					i++;
+					fallthrough;
+				case 5500000:
+					priv->reg.rate_set.body[2] = TX_RATE_5M;
+					i++;
+					fallthrough;
+				case 2000000:
+					priv->reg.rate_set.body[1] =
+					    TX_RATE_2M | BASIC_RATE;
+					i++;
+					fallthrough;
+				case 1000000:
+					priv->reg.rate_set.body[0] =
+					    TX_RATE_1M | BASIC_RATE;
+					i++;
+					break;
+				default:
+					return -EINVAL;
+				}
+				priv->reg.tx_rate = TX_RATE_MANUAL_AUTO;
+				priv->reg.rate_set.size = i;
+			} else {
+				priv->reg.rate_set.body[3] = TX_RATE_11M;
+				priv->reg.rate_set.body[2] = TX_RATE_5M;
+				priv->reg.rate_set.body[1] =
+				    TX_RATE_2M | BASIC_RATE;
+				priv->reg.rate_set.body[0] =
+				    TX_RATE_1M | BASIC_RATE;
+				priv->reg.tx_rate = TX_RATE_FULL_AUTO;
+				priv->reg.rate_set.size = 4;
+			}
+		}
+	} else {	/* D_11B_ONLY_MODE or  D_11BG_COMPATIBLE_MODE */
+		if (vwrq->bitrate.fixed == 1) {
+			switch (vwrq->bitrate.value) {
+			case 54000000:
+			case 48000000:
+			case 36000000:
+			case 18000000:
+			case 9000000:
+				priv->reg.rate_set.body[0] =
+				    (u8)(vwrq->bitrate.value / 500000);
+				break;
+			case 24000000:
+			case 12000000:
+			case 11000000:
+			case 6000000:
+			case 5500000:
+			case 2000000:
+			case 1000000:
+				priv->reg.rate_set.body[0] =
+				    ((u8)(vwrq->bitrate.value / 500000)) |
+				    BASIC_RATE;
+				break;
+			default:
+				return -EINVAL;
+			}
+			priv->reg.tx_rate = TX_RATE_FIXED;
+			priv->reg.rate_set.size = 1;
+		} else {	/* vwrq->fixed == 0 */
+			if (vwrq->bitrate.value > 0) {
+				switch (vwrq->bitrate.value) {
+				case 54000000:
+					priv->reg.rate_set.body[11] =
+					    TX_RATE_54M;
+					i++;
+					fallthrough;
+				case 48000000:
+					priv->reg.rate_set.body[10] =
+					    TX_RATE_48M;
+					i++;
+					fallthrough;
+				case 36000000:
+					priv->reg.rate_set.body[9] =
+					    TX_RATE_36M;
+					i++;
+					fallthrough;
+				case 24000000:
+				case 18000000:
+				case 12000000:
+				case 11000000:
+				case 9000000:
+				case 6000000:
+					if (vwrq->bitrate.value == 24000000) {
+						priv->reg.rate_set.body[8] =
+						    TX_RATE_18M;
+						i++;
+						priv->reg.rate_set.body[7] =
+						    TX_RATE_9M;
+						i++;
+						priv->reg.rate_set.body[6] =
+						    TX_RATE_24M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[5] =
+						    TX_RATE_12M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[4] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_11M | BASIC_RATE;
+						i++;
+					} else if (vwrq->bitrate.value == 18000000) {
+						priv->reg.rate_set.body[7] =
+						    TX_RATE_18M;
+						i++;
+						priv->reg.rate_set.body[6] =
+						    TX_RATE_9M;
+						i++;
+						priv->reg.rate_set.body[5] =
+						    TX_RATE_12M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[4] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_11M | BASIC_RATE;
+						i++;
+					} else if (vwrq->bitrate.value == 12000000) {
+						priv->reg.rate_set.body[6] =
+						    TX_RATE_9M;
+						i++;
+						priv->reg.rate_set.body[5] =
+						    TX_RATE_12M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[4] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_11M | BASIC_RATE;
+						i++;
+					} else if (vwrq->bitrate.value == 11000000) {
+						priv->reg.rate_set.body[5] =
+						    TX_RATE_9M;
+						i++;
+						priv->reg.rate_set.body[4] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_11M | BASIC_RATE;
+						i++;
+					} else if (vwrq->bitrate.value == 9000000) {
+						priv->reg.rate_set.body[4] =
+						    TX_RATE_9M;
+						i++;
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+					} else {	/* vwrq->value == 6000000 */
+						priv->reg.rate_set.body[3] =
+						    TX_RATE_6M | BASIC_RATE;
+						i++;
+					}
+					fallthrough;
+				case 5500000:
+					priv->reg.rate_set.body[2] =
+					    TX_RATE_5M | BASIC_RATE;
+					i++;
+					fallthrough;
+				case 2000000:
+					priv->reg.rate_set.body[1] =
+					    TX_RATE_2M | BASIC_RATE;
+					i++;
+					fallthrough;
+				case 1000000:
+					priv->reg.rate_set.body[0] =
+					    TX_RATE_1M | BASIC_RATE;
+					i++;
+					break;
+				default:
+					return -EINVAL;
+				}
+				priv->reg.tx_rate = TX_RATE_MANUAL_AUTO;
+				priv->reg.rate_set.size = i;
+			} else {
+				priv->reg.rate_set.body[11] = TX_RATE_54M;
+				priv->reg.rate_set.body[10] = TX_RATE_48M;
+				priv->reg.rate_set.body[9] = TX_RATE_36M;
+				priv->reg.rate_set.body[8] = TX_RATE_18M;
+				priv->reg.rate_set.body[7] = TX_RATE_9M;
+				priv->reg.rate_set.body[6] =
+				    TX_RATE_24M | BASIC_RATE;
+				priv->reg.rate_set.body[5] =
+				    TX_RATE_12M | BASIC_RATE;
+				priv->reg.rate_set.body[4] =
+				    TX_RATE_6M | BASIC_RATE;
+				priv->reg.rate_set.body[3] =
+				    TX_RATE_11M | BASIC_RATE;
+				priv->reg.rate_set.body[2] =
+				    TX_RATE_5M | BASIC_RATE;
+				priv->reg.rate_set.body[1] =
+				    TX_RATE_2M | BASIC_RATE;
+				priv->reg.rate_set.body[0] =
+				    TX_RATE_1M | BASIC_RATE;
+				priv->reg.tx_rate = TX_RATE_FULL_AUTO;
+				priv->reg.rate_set.size = 12;
+			}
+		}
+	}
+
+	priv->need_commit |= SME_MODE_SET;
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_rate(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	netdev_dbg(dev, "in_interrupt = %ld update_phyinfo = %d\n",
+		   in_interrupt(), atomic_read(&update_phyinfo));
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (!atomic_read(&update_phyinfo))
+		ks_wlan_update_phy_information(priv);
+
+	vwrq->bitrate.value = ((priv->current_rate) & RATE_MASK) * 500000;
+	vwrq->bitrate.fixed = (priv->reg.tx_rate == TX_RATE_FIXED) ? 1 : 0;
+
+	return 0;
+}
+
+static int ks_wlan_set_rts(struct net_device *dev, struct iw_request_info *info,
+			   union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int rthr = vwrq->rts.value;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (vwrq->rts.disabled)
+		rthr = 2347;
+	if ((rthr < 0) || (rthr > 2347))
+		return -EINVAL;
+
+	priv->reg.rts = rthr;
+	priv->need_commit |= SME_RTS;
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_rts(struct net_device *dev, struct iw_request_info *info,
+			   union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	vwrq->rts.value = priv->reg.rts;
+	vwrq->rts.disabled = (vwrq->rts.value >= 2347);
+	vwrq->rts.fixed = 1;
+
+	return 0;
+}
+
+static int ks_wlan_set_frag(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int fthr = vwrq->frag.value;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (vwrq->frag.disabled)
+		fthr = 2346;
+	if ((fthr < 256) || (fthr > 2346))
+		return -EINVAL;
+
+	fthr &= ~0x1;	/* Get an even value - is it really needed ??? */
+	priv->reg.fragment = fthr;
+	priv->need_commit |= SME_FRAG;
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_frag(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	vwrq->frag.value = priv->reg.fragment;
+	vwrq->frag.disabled = (vwrq->frag.value >= 2346);
+	vwrq->frag.fixed = 1;
+
+	return 0;
+}
+
+static int ks_wlan_set_mode(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *uwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (uwrq->mode != IW_MODE_ADHOC &&
+	    uwrq->mode != IW_MODE_INFRA)
+		return -EINVAL;
+
+	priv->reg.operation_mode = (uwrq->mode == IW_MODE_ADHOC) ?
+				    MODE_ADHOC : MODE_INFRASTRUCTURE;
+	priv->need_commit |= SME_MODE_SET;
+
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_mode(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *uwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* If not managed, assume it's ad-hoc */
+	uwrq->mode = (priv->reg.operation_mode == MODE_INFRASTRUCTURE) ?
+		      IW_MODE_INFRA : IW_MODE_ADHOC;
+
+	return 0;
+}
+
+static int ks_wlan_set_encode(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_point *enc = &dwrq->encoding;
+	struct wep_key key;
+	int index = (enc->flags & IW_ENCODE_INDEX);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (enc->length > MAX_KEY_SIZE)
+		return -EINVAL;
+
+	/* for SLEEP MODE */
+	if ((index < 0) || (index > 4))
+		return -EINVAL;
+
+	index = (index == 0) ? priv->reg.wep_index : (index - 1);
+
+	/* Is WEP supported ? */
+	/* Basic checking: do we have a key to set ? */
+	if (enc->length > 0) {
+		key.len = (enc->length > MIN_KEY_SIZE) ?
+			   MAX_KEY_SIZE : MIN_KEY_SIZE;
+		priv->reg.privacy_invoked = 0x01;
+		priv->need_commit |= SME_WEP_FLAG;
+		wep_on_off = (enc->length > MIN_KEY_SIZE) ?
+			      WEP_ON_128BIT : WEP_ON_64BIT;
+		/* Check if the key is not marked as invalid */
+		if (enc->flags & IW_ENCODE_NOKEY)
+			return 0;
+
+		/* Cleanup */
+		memset(key.key, 0, MAX_KEY_SIZE);
+		/* Copy the key in the driver */
+		if (copy_from_user(key.key, enc->pointer, enc->length)) {
+			key.len = 0;
+			return -EFAULT;
+		}
+		/* Send the key to the card */
+		priv->reg.wep_key[index].size = key.len;
+		memcpy(&priv->reg.wep_key[index].val[0], &key.key[0],
+		       priv->reg.wep_key[index].size);
+		priv->need_commit |= (SME_WEP_VAL1 << index);
+		priv->reg.wep_index = index;
+		priv->need_commit |= SME_WEP_INDEX;
+	} else {
+		if (enc->flags & IW_ENCODE_DISABLED) {
+			priv->reg.wep_key[0].size = 0;
+			priv->reg.wep_key[1].size = 0;
+			priv->reg.wep_key[2].size = 0;
+			priv->reg.wep_key[3].size = 0;
+			priv->reg.privacy_invoked = 0x00;
+			if (priv->reg.authenticate_type == AUTH_TYPE_SHARED_KEY)
+				priv->need_commit |= SME_MODE_SET;
+
+			priv->reg.authenticate_type = AUTH_TYPE_OPEN_SYSTEM;
+			wep_on_off = WEP_OFF;
+			priv->need_commit |= SME_WEP_FLAG;
+		} else {
+			/* set_wep_key(priv, index, 0, 0, 1);   xxx */
+			if (priv->reg.wep_key[index].size == 0)
+				return -EINVAL;
+			priv->reg.wep_index = index;
+			priv->need_commit |= SME_WEP_INDEX;
+		}
+	}
+
+	/* Commit the changes if needed */
+	if (enc->flags & IW_ENCODE_MODE)
+		priv->need_commit |= SME_WEP_FLAG;
+
+	if (enc->flags & IW_ENCODE_OPEN) {
+		if (priv->reg.authenticate_type == AUTH_TYPE_SHARED_KEY)
+			priv->need_commit |= SME_MODE_SET;
+
+		priv->reg.authenticate_type = AUTH_TYPE_OPEN_SYSTEM;
+	} else if (enc->flags & IW_ENCODE_RESTRICTED) {
+		if (priv->reg.authenticate_type == AUTH_TYPE_OPEN_SYSTEM)
+			priv->need_commit |= SME_MODE_SET;
+
+		priv->reg.authenticate_type = AUTH_TYPE_SHARED_KEY;
+	}
+	if (priv->need_commit) {
+		ks_wlan_setup_parameter(priv, priv->need_commit);
+		priv->need_commit = 0;
+	}
+	return 0;
+}
+
+static int ks_wlan_get_encode(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_point *enc = &dwrq->encoding;
+	int index = (enc->flags & IW_ENCODE_INDEX) - 1;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	enc->flags = IW_ENCODE_DISABLED;
+
+	/* Check encryption mode */
+	switch (priv->reg.authenticate_type) {
+	case AUTH_TYPE_OPEN_SYSTEM:
+		enc->flags = IW_ENCODE_OPEN;
+		break;
+	case AUTH_TYPE_SHARED_KEY:
+		enc->flags = IW_ENCODE_RESTRICTED;
+		break;
+	}
+
+	/* Which key do we want ? -1 -> tx index */
+	if ((index < 0) || (index >= 4))
+		index = priv->reg.wep_index;
+	if (priv->reg.privacy_invoked) {
+		enc->flags &= ~IW_ENCODE_DISABLED;
+		/* dwrq->flags |= IW_ENCODE_NOKEY; */
+	}
+	enc->flags |= index + 1;
+	/* Copy the key to the user buffer */
+	if (index >= 0 && index < 4) {
+		enc->length = (priv->reg.wep_key[index].size <= 16) ?
+				priv->reg.wep_key[index].size : 0;
+		memcpy(extra, priv->reg.wep_key[index].val, enc->length);
+	}
+
+	return 0;
+}
+
+static int ks_wlan_get_range(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_range *range = (struct iw_range *)extra;
+	int i, k;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	dwrq->data.length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(*range));
+	range->min_nwid = 0x0000;
+	range->max_nwid = 0x0000;
+	range->num_channels = 14;
+	/* Should be based on cap_rid.country to give only
+	 * what the current card support
+	 */
+	k = 0;
+	for (i = 0; i < 13; i++) {	/* channel 1 -- 13 */
+		range->freq[k].i = i + 1;	/* List index */
+		range->freq[k].m = frequency_list[i] * 100000;
+		range->freq[k++].e = 1;	/* Values in table in MHz -> * 10^5 * 10 */
+	}
+	range->num_frequency = k;
+	if (priv->reg.phy_type == D_11B_ONLY_MODE ||
+	    priv->reg.phy_type == D_11BG_COMPATIBLE_MODE) {	/* channel 14 */
+		range->freq[13].i = 14;	/* List index */
+		range->freq[13].m = frequency_list[13] * 100000;
+		range->freq[13].e = 1;	/* Values in table in MHz -> * 10^5 * 10 */
+		range->num_frequency = 14;
+	}
+
+	/* Hum... Should put the right values there */
+	range->max_qual.qual = 100;
+	range->max_qual.level = 256 - 128;	/* 0 dBm? */
+	range->max_qual.noise = 256 - 128;
+	range->sensitivity = 1;
+
+	if (priv->reg.phy_type == D_11B_ONLY_MODE) {
+		range->bitrate[0] = 1e6;
+		range->bitrate[1] = 2e6;
+		range->bitrate[2] = 5.5e6;
+		range->bitrate[3] = 11e6;
+		range->num_bitrates = 4;
+	} else {	/* D_11G_ONLY_MODE or D_11BG_COMPATIBLE_MODE */
+		range->bitrate[0] = 1e6;
+		range->bitrate[1] = 2e6;
+		range->bitrate[2] = 5.5e6;
+		range->bitrate[3] = 11e6;
+
+		range->bitrate[4] = 6e6;
+		range->bitrate[5] = 9e6;
+		range->bitrate[6] = 12e6;
+		if (IW_MAX_BITRATES < 9) {
+			range->bitrate[7] = 54e6;
+			range->num_bitrates = 8;
+		} else {
+			range->bitrate[7] = 18e6;
+			range->bitrate[8] = 24e6;
+			range->bitrate[9] = 36e6;
+			range->bitrate[10] = 48e6;
+			range->bitrate[11] = 54e6;
+
+			range->num_bitrates = 12;
+		}
+	}
+
+	/* Set an indication of the max TCP throughput
+	 * in bit/s that we can expect using this interface.
+	 * May be use for QoS stuff... Jean II
+	 */
+	if (i > 2)
+		range->throughput = 5000 * 1000;
+	else
+		range->throughput = 1500 * 1000;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+	range->encoding_size[0] = 5;	/* WEP: RC4 40 bits */
+	range->encoding_size[1] = 13;	/* WEP: RC4 ~128 bits */
+	range->num_encoding_sizes = 2;
+	range->max_encoding_tokens = 4;
+
+	/* power management not support */
+	range->pmp_flags = IW_POWER_ON;
+	range->pmt_flags = IW_POWER_ON;
+	range->pm_capa = 0;
+
+	/* Transmit Power - values are in dBm( or mW) */
+	range->txpower[0] = -256;
+	range->num_txpower = 1;
+	range->txpower_capa = IW_TXPOW_DBM;
+	/* range->txpower_capa = IW_TXPOW_MWATT; */
+
+	range->we_version_source = 21;
+	range->we_version_compiled = WIRELESS_EXT;
+
+	range->retry_capa = IW_RETRY_ON;
+	range->retry_flags = IW_RETRY_ON;
+	range->r_time_flags = IW_RETRY_ON;
+
+	/* Experimental measurements - boundary 11/5.5 Mb/s
+	 *
+	 * Note : with or without the (local->rssi), results
+	 * are somewhat different. - Jean II
+	 */
+	range->avg_qual.qual = 50;
+	range->avg_qual.level = 186;	/* -70 dBm */
+	range->avg_qual.noise = 0;
+
+	/* Event capability (kernel + driver) */
+	range->event_capa[0] = (IW_EVENT_CAPA_K_0 |
+				IW_EVENT_CAPA_MASK(SIOCGIWAP) |
+				IW_EVENT_CAPA_MASK(SIOCGIWSCAN));
+	range->event_capa[1] = IW_EVENT_CAPA_K_1;
+	range->event_capa[4] = (IW_EVENT_CAPA_MASK(IWEVCUSTOM) |
+				IW_EVENT_CAPA_MASK(IWEVMICHAELMICFAILURE));
+
+	/* encode extension (WPA) capability */
+	range->enc_capa = (IW_ENC_CAPA_WPA |
+			   IW_ENC_CAPA_WPA2 |
+			   IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP);
+	return 0;
+}
+
+static int ks_wlan_set_power(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (vwrq->power.disabled) {
+		priv->reg.power_mgmt = POWER_MGMT_ACTIVE;
+	} else {
+		if (priv->reg.operation_mode != MODE_INFRASTRUCTURE)
+			return -EINVAL;
+		priv->reg.power_mgmt = POWER_MGMT_SAVE1;
+	}
+
+	hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
+
+	return 0;
+}
+
+static int ks_wlan_get_power(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	vwrq->power.disabled = (priv->reg.power_mgmt <= 0);
+
+	return 0;
+}
+
+static int ks_wlan_get_iwstats(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	vwrq->qual.qual = 0;	/* not supported */
+	vwrq->qual.level = priv->wstats.qual.level;
+	vwrq->qual.noise = 0;	/* not supported */
+	vwrq->qual.updated = 0;
+
+	return 0;
+}
+
+/* Note : this is deprecated in favor of IWSCAN */
+static int ks_wlan_get_aplist(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct sockaddr *address = (struct sockaddr *)extra;
+	struct iw_quality qual[LOCAL_APLIST_MAX];
+	int i;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	for (i = 0; i < priv->aplist.size; i++) {
+		ether_addr_copy(address[i].sa_data, priv->aplist.ap[i].bssid);
+		address[i].sa_family = ARPHRD_ETHER;
+		qual[i].level = 256 - priv->aplist.ap[i].rssi;
+		qual[i].qual = priv->aplist.ap[i].sq;
+		qual[i].noise = 0;	/* invalid noise value */
+		qual[i].updated = 7;
+	}
+	if (i) {
+		dwrq->data.flags = 1;	/* Should be define'd */
+		memcpy(extra + sizeof(struct sockaddr) * i,
+		       &qual, sizeof(struct iw_quality) * i);
+	}
+	dwrq->data.length = i;
+
+	return 0;
+}
+
+static int ks_wlan_set_scan(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_scan_req *req = NULL;
+	int len;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* specified SSID SCAN */
+	if (wrqu->data.length == sizeof(struct iw_scan_req) &&
+	    wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+		req = (struct iw_scan_req *)extra;
+		len = min_t(int, req->essid_len, IW_ESSID_MAX_SIZE);
+		priv->scan_ssid_len = len;
+		memcpy(priv->scan_ssid, req->essid, len);
+	} else {
+		priv->scan_ssid_len = 0;
+	}
+
+	priv->sme_i.sme_flag |= SME_AP_SCAN;
+	hostif_sme_enqueue(priv, SME_BSS_SCAN_REQUEST);
+
+	/* At this point, just return to the user. */
+
+	return 0;
+}
+
+static char *ks_wlan_add_leader_event(const char *rsn_leader, char *end_buf,
+				      char *current_ev, struct rsn_ie *rsn,
+				      struct iw_event *iwe,
+				      struct iw_request_info *info)
+{
+	char buffer[RSN_IE_BODY_MAX * 2 + 30];
+	char *pbuf;
+	int i;
+
+	pbuf = &buffer[0];
+	memset(iwe, 0, sizeof(*iwe));
+	iwe->cmd = IWEVCUSTOM;
+	memcpy(buffer, rsn_leader, sizeof(rsn_leader) - 1);
+	iwe->u.data.length += sizeof(rsn_leader) - 1;
+	pbuf += sizeof(rsn_leader) - 1;
+	pbuf += sprintf(pbuf, "%02x", rsn->id);
+	pbuf += sprintf(pbuf, "%02x", rsn->size);
+	iwe->u.data.length += 4;
+
+	for (i = 0; i < rsn->size; i++)
+		pbuf += sprintf(pbuf, "%02x", rsn->body[i]);
+
+	iwe->u.data.length += rsn->size * 2;
+
+	return iwe_stream_add_point(info, current_ev, end_buf, iwe, &buffer[0]);
+}
+
+/*
+ * Translate scan data returned from the card to a card independent
+ * format that the Wireless Tools will understand - Jean II
+ */
+static inline char *ks_wlan_translate_scan(struct net_device *dev,
+					   struct iw_request_info *info,
+					   char *current_ev, char *end_buf,
+					   struct local_ap *ap)
+{
+	/* struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv; */
+	static const char rsn_leader[] = "rsn_ie=";
+	static const char wpa_leader[] = "wpa_ie=";
+	struct iw_event iwe;	/* Temporary buffer */
+	u16 capabilities;
+	char *current_val;	/* For rates */
+	int i;
+
+	/* First entry *MUST* be the AP MAC address */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+	ether_addr_copy(iwe.u.ap_addr.sa_data, ap->bssid);
+	current_ev = iwe_stream_add_event(info, current_ev,
+					  end_buf, &iwe, IW_EV_ADDR_LEN);
+
+	/* Other entries will be displayed in the order we give them */
+
+	/* Add the ESSID */
+	iwe.u.data.length = ap->ssid.size;
+	if (iwe.u.data.length > 32)
+		iwe.u.data.length = 32;
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	current_ev = iwe_stream_add_point(info, current_ev,
+					  end_buf, &iwe, ap->ssid.body);
+
+	/* Add mode */
+	iwe.cmd = SIOCGIWMODE;
+	capabilities = ap->capability;
+	if (capabilities & (WLAN_CAPABILITY_ESS | WLAN_CAPABILITY_IBSS)) {
+		iwe.u.mode = (capabilities & WLAN_CAPABILITY_ESS) ?
+			      IW_MODE_INFRA : IW_MODE_ADHOC;
+		current_ev = iwe_stream_add_event(info, current_ev,
+						  end_buf, &iwe, IW_EV_UINT_LEN);
+	}
+
+	/* Add frequency */
+	iwe.cmd = SIOCGIWFREQ;
+	iwe.u.freq.m = ap->channel;
+	iwe.u.freq.m = frequency_list[iwe.u.freq.m - 1] * 100000;
+	iwe.u.freq.e = 1;
+	current_ev = iwe_stream_add_event(info, current_ev,
+					  end_buf, &iwe, IW_EV_FREQ_LEN);
+
+	/* Add quality statistics */
+	iwe.cmd = IWEVQUAL;
+	iwe.u.qual.level = 256 - ap->rssi;
+	iwe.u.qual.qual = ap->sq;
+	iwe.u.qual.noise = 0;	/* invalid noise value */
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_QUAL_LEN);
+
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	iwe.u.data.flags = (capabilities & WLAN_CAPABILITY_PRIVACY) ?
+			    (IW_ENCODE_ENABLED | IW_ENCODE_NOKEY) :
+			     IW_ENCODE_DISABLED;
+	iwe.u.data.length = 0;
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, ap->ssid.body);
+
+	/*
+	 * Rate : stuffing multiple values in a single event
+	 * require a bit more of magic - Jean II
+	 */
+	current_val = current_ev + IW_EV_LCP_LEN;
+
+	iwe.cmd = SIOCGIWRATE;
+
+	/* These two flags are ignored... */
+	iwe.u.bitrate.fixed = 0;
+	iwe.u.bitrate.disabled = 0;
+
+	/* Max 16 values */
+	for (i = 0; i < 16; i++) {
+		/* NULL terminated */
+		if (i >= ap->rate_set.size)
+			break;
+		/* Bit rate given in 500 kb/s units (+ 0x80) */
+		iwe.u.bitrate.value = ((ap->rate_set.body[i] & 0x7f) * 500000);
+		/* Add new value to event */
+		current_val = iwe_stream_add_value(info, current_ev,
+						   current_val, end_buf, &iwe,
+						   IW_EV_PARAM_LEN);
+	}
+	/* Check if we added any event */
+	if ((current_val - current_ev) > IW_EV_LCP_LEN)
+		current_ev = current_val;
+
+	if (ap->rsn_ie.id == RSN_INFO_ELEM_ID && ap->rsn_ie.size != 0)
+		current_ev = ks_wlan_add_leader_event(rsn_leader, end_buf,
+						      current_ev, &ap->rsn_ie,
+						      &iwe, info);
+
+	if (ap->wpa_ie.id == WPA_INFO_ELEM_ID && ap->wpa_ie.size != 0)
+		current_ev = ks_wlan_add_leader_event(wpa_leader, end_buf,
+						      current_ev, &ap->wpa_ie,
+						      &iwe, info);
+
+	/*
+	 * The other data in the scan result are not really
+	 * interesting, so for now drop it - Jean II
+	 */
+	return current_ev;
+}
+
+static int ks_wlan_get_scan(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int i;
+	char *current_ev = extra;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (priv->sme_i.sme_flag & SME_AP_SCAN)
+		return -EAGAIN;
+
+	if (priv->aplist.size == 0) {
+		/* Client error, no scan results...
+		 * The caller need to restart the scan.
+		 */
+		return -ENODATA;
+	}
+
+	/* Read and parse all entries */
+	for (i = 0; i < priv->aplist.size; i++) {
+		if ((extra + dwrq->data.length) - current_ev <= IW_EV_ADDR_LEN) {
+			dwrq->data.length = 0;
+			return -E2BIG;
+		}
+		/* Translate to WE format this entry */
+		current_ev = ks_wlan_translate_scan(dev, info, current_ev,
+						    extra + dwrq->data.length,
+						    &priv->aplist.ap[i]);
+	}
+	/* Length of data */
+	dwrq->data.length = (current_ev - extra);
+	dwrq->data.flags = 0;
+
+	return 0;
+}
+
+/* called after a bunch of SET operations */
+static int ks_wlan_config_commit(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *zwrq,
+				 char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (!priv->need_commit)
+		return 0;
+
+	ks_wlan_setup_parameter(priv, priv->need_commit);
+	priv->need_commit = 0;
+	return 0;
+}
+
+/* set association ie params */
+static int ks_wlan_set_genie(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	return 0;
+//      return -EOPNOTSUPP;
+}
+
+static int ks_wlan_set_auth_mode(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_param *param = &vwrq->param;
+	int index = (param->flags & IW_AUTH_INDEX);
+	int value = param->value;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	switch (index) {
+	case IW_AUTH_WPA_VERSION:	/* 0 */
+		switch (value) {
+		case IW_AUTH_WPA_VERSION_DISABLED:
+			priv->wpa.version = value;
+			if (priv->wpa.rsn_enabled)
+				priv->wpa.rsn_enabled = false;
+			priv->need_commit |= SME_RSN;
+			break;
+		case IW_AUTH_WPA_VERSION_WPA:
+		case IW_AUTH_WPA_VERSION_WPA2:
+			priv->wpa.version = value;
+			if (!(priv->wpa.rsn_enabled))
+				priv->wpa.rsn_enabled = true;
+			priv->need_commit |= SME_RSN;
+			break;
+		default:
+			return -EOPNOTSUPP;
+		}
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:	/* 1 */
+		switch (value) {
+		case IW_AUTH_CIPHER_NONE:
+			if (priv->reg.privacy_invoked) {
+				priv->reg.privacy_invoked = 0x00;
+				priv->need_commit |= SME_WEP_FLAG;
+			}
+			break;
+		case IW_AUTH_CIPHER_WEP40:
+		case IW_AUTH_CIPHER_TKIP:
+		case IW_AUTH_CIPHER_CCMP:
+		case IW_AUTH_CIPHER_WEP104:
+			if (!priv->reg.privacy_invoked) {
+				priv->reg.privacy_invoked = 0x01;
+				priv->need_commit |= SME_WEP_FLAG;
+			}
+			priv->wpa.pairwise_suite = value;
+			priv->need_commit |= SME_RSN_UNICAST;
+			break;
+		default:
+			return -EOPNOTSUPP;
+		}
+		break;
+	case IW_AUTH_CIPHER_GROUP:	/* 2 */
+		switch (value) {
+		case IW_AUTH_CIPHER_NONE:
+			if (priv->reg.privacy_invoked) {
+				priv->reg.privacy_invoked = 0x00;
+				priv->need_commit |= SME_WEP_FLAG;
+			}
+			break;
+		case IW_AUTH_CIPHER_WEP40:
+		case IW_AUTH_CIPHER_TKIP:
+		case IW_AUTH_CIPHER_CCMP:
+		case IW_AUTH_CIPHER_WEP104:
+			if (!priv->reg.privacy_invoked) {
+				priv->reg.privacy_invoked = 0x01;
+				priv->need_commit |= SME_WEP_FLAG;
+			}
+			priv->wpa.group_suite = value;
+			priv->need_commit |= SME_RSN_MULTICAST;
+			break;
+		default:
+			return -EOPNOTSUPP;
+		}
+		break;
+	case IW_AUTH_KEY_MGMT:	/* 3 */
+		switch (value) {
+		case IW_AUTH_KEY_MGMT_802_1X:
+		case IW_AUTH_KEY_MGMT_PSK:
+		case 0:	/* NONE or 802_1X_NO_WPA */
+		case 4:	/* WPA_NONE */
+			priv->wpa.key_mgmt_suite = value;
+			priv->need_commit |= SME_RSN_AUTH;
+			break;
+		default:
+			return -EOPNOTSUPP;
+		}
+		break;
+	case IW_AUTH_80211_AUTH_ALG:	/* 6 */
+		switch (value) {
+		case IW_AUTH_ALG_OPEN_SYSTEM:
+			priv->wpa.auth_alg = value;
+			priv->reg.authenticate_type = AUTH_TYPE_OPEN_SYSTEM;
+			break;
+		case IW_AUTH_ALG_SHARED_KEY:
+			priv->wpa.auth_alg = value;
+			priv->reg.authenticate_type = AUTH_TYPE_SHARED_KEY;
+			break;
+		case IW_AUTH_ALG_LEAP:
+		default:
+			return -EOPNOTSUPP;
+		}
+		priv->need_commit |= SME_MODE_SET;
+		break;
+	case IW_AUTH_WPA_ENABLED:	/* 7 */
+		priv->wpa.wpa_enabled = value;
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:	/* 10 */
+		if ((value && !priv->reg.privacy_invoked) ||
+		    (!value && priv->reg.privacy_invoked)) {
+			priv->reg.privacy_invoked = value ? 0x01 : 0x00;
+			priv->need_commit |= SME_WEP_FLAG;
+		}
+		break;
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:	/* 4 */
+	case IW_AUTH_TKIP_COUNTERMEASURES:	/* 5 */
+	case IW_AUTH_DROP_UNENCRYPTED:	/* 8 */
+	case IW_AUTH_ROAMING_CONTROL:	/* 9 */
+	default:
+		break;
+	}
+
+	/* return -EINPROGRESS; */
+	if (priv->need_commit) {
+		ks_wlan_setup_parameter(priv, priv->need_commit);
+		priv->need_commit = 0;
+	}
+	return 0;
+}
+
+static int ks_wlan_get_auth_mode(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *vwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_param *param = &vwrq->param;
+	int index = (param->flags & IW_AUTH_INDEX);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/*  WPA (not used ?? wpa_supplicant) */
+	switch (index) {
+	case IW_AUTH_WPA_VERSION:
+		param->value = priv->wpa.version;
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+		param->value = priv->wpa.pairwise_suite;
+		break;
+	case IW_AUTH_CIPHER_GROUP:
+		param->value = priv->wpa.group_suite;
+		break;
+	case IW_AUTH_KEY_MGMT:
+		param->value = priv->wpa.key_mgmt_suite;
+		break;
+	case IW_AUTH_80211_AUTH_ALG:
+		param->value = priv->wpa.auth_alg;
+		break;
+	case IW_AUTH_WPA_ENABLED:
+		param->value = priv->wpa.rsn_enabled;
+		break;
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:	/* OK??? */
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+	case IW_AUTH_DROP_UNENCRYPTED:
+	default:
+		/* return -EOPNOTSUPP; */
+		break;
+	}
+	return 0;
+}
+
+/* set encoding token & mode (WPA)*/
+static int ks_wlan_set_encode_ext(struct net_device *dev,
+				  struct iw_request_info *info,
+				  union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_encode_ext *enc;
+	int index = dwrq->encoding.flags & IW_ENCODE_INDEX;
+	unsigned int commit = 0;
+	struct wpa_key *key;
+
+	enc = (struct iw_encode_ext *)extra;
+	if (!enc)
+		return -EINVAL;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (index < 1 || index > 4)
+		return -EINVAL;
+	index--;
+	key = &priv->wpa.key[index];
+
+	if (dwrq->encoding.flags & IW_ENCODE_DISABLED)
+		key->key_len = 0;
+
+	key->ext_flags = enc->ext_flags;
+	if (enc->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+		priv->wpa.txkey = index;
+		commit |= SME_WEP_INDEX;
+	} else if (enc->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+		memcpy(&key->rx_seq[0], &enc->rx_seq[0], IW_ENCODE_SEQ_MAX_SIZE);
+	}
+
+	ether_addr_copy(&key->addr.sa_data[0], &enc->addr.sa_data[0]);
+
+	switch (enc->alg) {
+	case IW_ENCODE_ALG_NONE:
+		if (priv->reg.privacy_invoked) {
+			priv->reg.privacy_invoked = 0x00;
+			commit |= SME_WEP_FLAG;
+		}
+		key->key_len = 0;
+
+		break;
+	case IW_ENCODE_ALG_WEP:
+	case IW_ENCODE_ALG_CCMP:
+		if (!priv->reg.privacy_invoked) {
+			priv->reg.privacy_invoked = 0x01;
+			commit |= SME_WEP_FLAG;
+		}
+		if (enc->key_len) {
+			int key_len = clamp_val(enc->key_len, 0, IW_ENCODING_TOKEN_MAX);
+
+			memcpy(&key->key_val[0], &enc->key[0], key_len);
+			key->key_len = key_len;
+			commit |= (SME_WEP_VAL1 << index);
+		}
+		break;
+	case IW_ENCODE_ALG_TKIP:
+		if (!priv->reg.privacy_invoked) {
+			priv->reg.privacy_invoked = 0x01;
+			commit |= SME_WEP_FLAG;
+		}
+		if (enc->key_len == 32) {
+			memcpy(&key->key_val[0], &enc->key[0], enc->key_len - 16);
+			key->key_len = enc->key_len - 16;
+			if (priv->wpa.key_mgmt_suite == 4) {	/* WPA_NONE */
+				memcpy(&key->tx_mic_key[0], &enc->key[16], 8);
+				memcpy(&key->rx_mic_key[0], &enc->key[16], 8);
+			} else {
+				memcpy(&key->tx_mic_key[0], &enc->key[16], 8);
+				memcpy(&key->rx_mic_key[0], &enc->key[24], 8);
+			}
+			commit |= (SME_WEP_VAL1 << index);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	key->alg = enc->alg;
+
+	if (commit) {
+		if (commit & SME_WEP_INDEX)
+			hostif_sme_enqueue(priv, SME_SET_TXKEY);
+		if (commit & SME_WEP_VAL_MASK)
+			hostif_sme_enqueue(priv, SME_SET_KEY1 + index);
+		if (commit & SME_WEP_FLAG)
+			hostif_sme_enqueue(priv, SME_WEP_FLAG_REQUEST);
+	}
+
+	return 0;
+}
+
+/* get encoding token & mode (WPA)*/
+static int ks_wlan_get_encode_ext(struct net_device *dev,
+				  struct iw_request_info *info,
+				  union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	/* WPA (not used ?? wpa_supplicant)
+	 * struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
+	 * struct iw_encode_ext *enc;
+	 * enc = (struct iw_encode_ext *)extra;
+	 * int index = dwrq->flags & IW_ENCODE_INDEX;
+	 * WPA (not used ?? wpa_supplicant)
+	 */
+	return 0;
+}
+
+static int ks_wlan_set_pmksa(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_pmksa *pmksa;
+	int i;
+	struct pmk *pmk;
+	struct list_head *ptr;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (!extra)
+		return -EINVAL;
+
+	pmksa = (struct iw_pmksa *)extra;
+
+	switch (pmksa->cmd) {
+	case IW_PMKSA_ADD:
+		if (list_empty(&priv->pmklist.head)) {
+			for (i = 0; i < PMK_LIST_MAX; i++) {
+				pmk = &priv->pmklist.pmk[i];
+				if (is_zero_ether_addr(pmk->bssid))
+					break;
+			}
+			ether_addr_copy(pmk->bssid, pmksa->bssid.sa_data);
+			memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
+			list_add(&pmk->list, &priv->pmklist.head);
+			priv->pmklist.size++;
+			break;
+		}
+		/* search cache data */
+		list_for_each(ptr, &priv->pmklist.head) {
+			pmk = list_entry(ptr, struct pmk, list);
+			if (ether_addr_equal(pmksa->bssid.sa_data, pmk->bssid)) {
+				memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
+				list_move(&pmk->list, &priv->pmklist.head);
+				break;
+			}
+		}
+		/* not find address. */
+		if (ptr != &priv->pmklist.head)
+			break;
+		/* new cache data */
+		if (priv->pmklist.size < PMK_LIST_MAX) {
+			for (i = 0; i < PMK_LIST_MAX; i++) {
+				pmk = &priv->pmklist.pmk[i];
+				if (is_zero_ether_addr(pmk->bssid))
+					break;
+			}
+			ether_addr_copy(pmk->bssid, pmksa->bssid.sa_data);
+			memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
+			list_add(&pmk->list, &priv->pmklist.head);
+			priv->pmklist.size++;
+		} else { /* overwrite old cache data */
+			pmk = list_entry(priv->pmklist.head.prev, struct pmk,
+					 list);
+			ether_addr_copy(pmk->bssid, pmksa->bssid.sa_data);
+			memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
+			list_move(&pmk->list, &priv->pmklist.head);
+		}
+		break;
+	case IW_PMKSA_REMOVE:
+		if (list_empty(&priv->pmklist.head))
+			return -EINVAL;
+		/* search cache data */
+		list_for_each(ptr, &priv->pmklist.head) {
+			pmk = list_entry(ptr, struct pmk, list);
+			if (ether_addr_equal(pmksa->bssid.sa_data, pmk->bssid)) {
+				eth_zero_addr(pmk->bssid);
+				memset(pmk->pmkid, 0, IW_PMKID_LEN);
+				list_del_init(&pmk->list);
+				break;
+			}
+		}
+		/* not find address. */
+		if (ptr == &priv->pmklist.head)
+			return 0;
+		break;
+	case IW_PMKSA_FLUSH:
+		memset(&priv->pmklist, 0, sizeof(priv->pmklist));
+		INIT_LIST_HEAD(&priv->pmklist.head);
+		for (i = 0; i < PMK_LIST_MAX; i++)
+			INIT_LIST_HEAD(&priv->pmklist.pmk[i].list);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	hostif_sme_enqueue(priv, SME_SET_PMKSA);
+	return 0;
+}
+
+static struct iw_statistics *ks_get_wireless_stats(struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_statistics *wstats = &priv->wstats;
+
+	if (!atomic_read(&update_phyinfo))
+		return (priv->dev_state < DEVICE_STATE_READY) ? NULL : wstats;
+
+	/*
+	 * Packets discarded in the wireless adapter due to wireless
+	 * specific problems
+	 */
+	wstats->discard.nwid = 0;	/* Rx invalid nwid      */
+	wstats->discard.code = 0;	/* Rx invalid crypt     */
+	wstats->discard.fragment = 0;	/* Rx invalid frag      */
+	wstats->discard.retries = 0;	/* Tx excessive retries */
+	wstats->discard.misc = 0;	/* Invalid misc         */
+	wstats->miss.beacon = 0;	/* Missed beacon        */
+
+	return wstats;
+}
+
+static int ks_wlan_set_stop_request(struct net_device *dev,
+				    struct iw_request_info *info, __u32 *uwrq,
+				    char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (!(*uwrq))
+		return -EINVAL;
+
+	hostif_sme_enqueue(priv, SME_STOP_REQUEST);
+	return 0;
+}
+
+#include <linux/ieee80211.h>
+static int ks_wlan_set_mlme(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct iw_mlme *mlme = (struct iw_mlme *)extra;
+	__u32 mode = 1;
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (mlme->cmd != IW_MLME_DEAUTH &&
+	    mlme->cmd != IW_MLME_DISASSOC)
+		return -EOPNOTSUPP;
+
+	if (mlme->cmd == IW_MLME_DEAUTH &&
+	    mlme->reason_code == WLAN_REASON_MIC_FAILURE)
+		return 0;
+
+	return ks_wlan_set_stop_request(dev, NULL, &mode, NULL);
+}
+
+static int ks_wlan_get_firmware_version(struct net_device *dev,
+					struct iw_request_info *info,
+					struct iw_point *dwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	dwrq->length = priv->version_size + 1;
+	strscpy(extra, priv->firmware_version, dwrq->length);
+	return 0;
+}
+
+static int ks_wlan_set_preamble(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	if (*uwrq != LONG_PREAMBLE && *uwrq != SHORT_PREAMBLE)
+		return -EINVAL;
+
+	priv->reg.preamble = *uwrq;
+	priv->need_commit |= SME_MODE_SET;
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_preamble(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.preamble;
+	return 0;
+}
+
+static int ks_wlan_set_power_mgmt(struct net_device *dev,
+				  struct iw_request_info *info, __u32 *uwrq,
+				  char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (*uwrq != POWER_MGMT_ACTIVE &&
+	    *uwrq != POWER_MGMT_SAVE1 &&
+	    *uwrq != POWER_MGMT_SAVE2)
+		return -EINVAL;
+
+	if ((*uwrq == POWER_MGMT_SAVE1 || *uwrq == POWER_MGMT_SAVE2) &&
+	    (priv->reg.operation_mode != MODE_INFRASTRUCTURE))
+		return -EINVAL;
+
+	priv->reg.power_mgmt = *uwrq;
+	hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
+
+	return 0;
+}
+
+static int ks_wlan_get_power_mgmt(struct net_device *dev,
+				  struct iw_request_info *info, __u32 *uwrq,
+				  char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.power_mgmt;
+	return 0;
+}
+
+static int ks_wlan_set_scan_type(struct net_device *dev,
+				 struct iw_request_info *info, __u32 *uwrq,
+				 char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+
+	if (*uwrq != ACTIVE_SCAN && *uwrq != PASSIVE_SCAN)
+		return -EINVAL;
+
+	priv->reg.scan_type = *uwrq;
+	return 0;
+}
+
+static int ks_wlan_get_scan_type(struct net_device *dev,
+				 struct iw_request_info *info, __u32 *uwrq,
+				 char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.scan_type;
+	return 0;
+}
+
+static int ks_wlan_set_beacon_lost(struct net_device *dev,
+				   struct iw_request_info *info, __u32 *uwrq,
+				   char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (*uwrq > BEACON_LOST_COUNT_MAX)
+		return -EINVAL;
+
+	priv->reg.beacon_lost_count = *uwrq;
+
+	if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
+		priv->need_commit |= SME_MODE_SET;
+		return -EINPROGRESS;	/* Call commit handler */
+	}
+
+	return 0;
+}
+
+static int ks_wlan_get_beacon_lost(struct net_device *dev,
+				   struct iw_request_info *info, __u32 *uwrq,
+				   char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.beacon_lost_count;
+	return 0;
+}
+
+static int ks_wlan_set_phy_type(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	if (*uwrq != D_11B_ONLY_MODE &&
+	    *uwrq != D_11G_ONLY_MODE &&
+	    *uwrq != D_11BG_COMPATIBLE_MODE)
+		return -EINVAL;
+
+	/* for SLEEP MODE */
+	priv->reg.phy_type = *uwrq;
+	priv->need_commit |= SME_MODE_SET;
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_phy_type(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.phy_type;
+	return 0;
+}
+
+static int ks_wlan_set_cts_mode(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (*uwrq != CTS_MODE_FALSE && *uwrq != CTS_MODE_TRUE)
+		return -EINVAL;
+
+	priv->reg.cts_mode = (*uwrq == CTS_MODE_FALSE) ? *uwrq :
+			      (priv->reg.phy_type == D_11G_ONLY_MODE ||
+			       priv->reg.phy_type == D_11BG_COMPATIBLE_MODE) ?
+			       *uwrq : !*uwrq;
+
+	priv->need_commit |= SME_MODE_SET;
+	return -EINPROGRESS;	/* Call commit handler */
+}
+
+static int ks_wlan_get_cts_mode(struct net_device *dev,
+				struct iw_request_info *info, __u32 *uwrq,
+				char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->reg.cts_mode;
+	return 0;
+}
+
+static int ks_wlan_set_sleep_mode(struct net_device *dev,
+				  struct iw_request_info *info,
+				  __u32 *uwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (*uwrq != SLP_SLEEP &&
+	    *uwrq != SLP_ACTIVE) {
+		netdev_err(dev, "SET_SLEEP_MODE %d error\n", *uwrq);
+		return -EINVAL;
+	}
+
+	priv->sleep_mode = *uwrq;
+	netdev_info(dev, "SET_SLEEP_MODE %d\n", priv->sleep_mode);
+
+	if (*uwrq == SLP_SLEEP)
+		hostif_sme_enqueue(priv, SME_STOP_REQUEST);
+
+	hostif_sme_enqueue(priv, SME_SLEEP_REQUEST);
+
+	return 0;
+}
+
+static int ks_wlan_get_sleep_mode(struct net_device *dev,
+				  struct iw_request_info *info,
+				  __u32 *uwrq, char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	*uwrq = priv->sleep_mode;
+
+	return 0;
+}
+
+static int ks_wlan_set_wps_enable(struct net_device *dev,
+				  struct iw_request_info *info, __u32 *uwrq,
+				  char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (*uwrq != 0 && *uwrq != 1)
+		return -EINVAL;
+
+	priv->wps.wps_enabled = *uwrq;
+	hostif_sme_enqueue(priv, SME_WPS_ENABLE_REQUEST);
+
+	return 0;
+}
+
+static int ks_wlan_get_wps_enable(struct net_device *dev,
+				  struct iw_request_info *info, __u32 *uwrq,
+				  char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->wps.wps_enabled;
+	netdev_info(dev, "return=%d\n", *uwrq);
+
+	return 0;
+}
+
+static int ks_wlan_set_wps_probe_req(struct net_device *dev,
+				     struct iw_request_info *info,
+				     struct iw_point *dwrq, char *extra)
+{
+	u8 *p = extra;
+	unsigned char len;
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+
+	/* length check */
+	if (p[1] + 2 != dwrq->length || dwrq->length > 256)
+		return -EINVAL;
+
+	priv->wps.ielen = p[1] + 2 + 1;	/* IE header + IE + sizeof(len) */
+	len = p[1] + 2;	/* IE header + IE */
+
+	memcpy(priv->wps.ie, &len, sizeof(len));
+	p = memcpy(priv->wps.ie + 1, p, len);
+
+	netdev_dbg(dev, "%d(%#x): %02X %02X %02X %02X ... %02X %02X %02X\n",
+		   priv->wps.ielen, priv->wps.ielen, p[0], p[1], p[2], p[3],
+		   p[priv->wps.ielen - 3], p[priv->wps.ielen - 2],
+		   p[priv->wps.ielen - 1]);
+
+	hostif_sme_enqueue(priv, SME_WPS_PROBE_REQUEST);
+
+	return 0;
+}
+
+static int ks_wlan_set_tx_gain(struct net_device *dev,
+			       struct iw_request_info *info, __u32 *uwrq,
+			       char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (*uwrq > 0xFF)
+		return -EINVAL;
+
+	priv->gain.tx_gain = (u8)*uwrq;
+	priv->gain.tx_mode = (priv->gain.tx_gain < 0xFF) ? 1 : 0;
+	hostif_sme_enqueue(priv, SME_SET_GAIN);
+	return 0;
+}
+
+static int ks_wlan_get_tx_gain(struct net_device *dev,
+			       struct iw_request_info *info, __u32 *uwrq,
+			       char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->gain.tx_gain;
+	hostif_sme_enqueue(priv, SME_GET_GAIN);
+	return 0;
+}
+
+static int ks_wlan_set_rx_gain(struct net_device *dev,
+			       struct iw_request_info *info, __u32 *uwrq,
+			       char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	if (*uwrq > 0xFF)
+		return -EINVAL;
+
+	priv->gain.rx_gain = (u8)*uwrq;
+	priv->gain.rx_mode = (priv->gain.rx_gain < 0xFF) ? 1 : 0;
+	hostif_sme_enqueue(priv, SME_SET_GAIN);
+	return 0;
+}
+
+static int ks_wlan_get_rx_gain(struct net_device *dev,
+			       struct iw_request_info *info, __u32 *uwrq,
+			       char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->sleep_mode == SLP_SLEEP)
+		return -EPERM;
+	/* for SLEEP MODE */
+	*uwrq = priv->gain.rx_gain;
+	hostif_sme_enqueue(priv, SME_GET_GAIN);
+	return 0;
+}
+
+static int ks_wlan_get_eeprom_cksum(struct net_device *dev,
+				    struct iw_request_info *info, __u32 *uwrq,
+				    char *extra)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	*uwrq = priv->eeprom_checksum;
+	return 0;
+}
+
+static void print_hif_event(struct net_device *dev, int event)
+{
+	switch (event) {
+	case HIF_DATA_REQ:
+		netdev_info(dev, "HIF_DATA_REQ\n");
+		break;
+	case HIF_DATA_IND:
+		netdev_info(dev, "HIF_DATA_IND\n");
+		break;
+	case HIF_MIB_GET_REQ:
+		netdev_info(dev, "HIF_MIB_GET_REQ\n");
+		break;
+	case HIF_MIB_GET_CONF:
+		netdev_info(dev, "HIF_MIB_GET_CONF\n");
+		break;
+	case HIF_MIB_SET_REQ:
+		netdev_info(dev, "HIF_MIB_SET_REQ\n");
+		break;
+	case HIF_MIB_SET_CONF:
+		netdev_info(dev, "HIF_MIB_SET_CONF\n");
+		break;
+	case HIF_POWER_MGMT_REQ:
+		netdev_info(dev, "HIF_POWER_MGMT_REQ\n");
+		break;
+	case HIF_POWER_MGMT_CONF:
+		netdev_info(dev, "HIF_POWER_MGMT_CONF\n");
+		break;
+	case HIF_START_REQ:
+		netdev_info(dev, "HIF_START_REQ\n");
+		break;
+	case HIF_START_CONF:
+		netdev_info(dev, "HIF_START_CONF\n");
+		break;
+	case HIF_CONNECT_IND:
+		netdev_info(dev, "HIF_CONNECT_IND\n");
+		break;
+	case HIF_STOP_REQ:
+		netdev_info(dev, "HIF_STOP_REQ\n");
+		break;
+	case HIF_STOP_CONF:
+		netdev_info(dev, "HIF_STOP_CONF\n");
+		break;
+	case HIF_PS_ADH_SET_REQ:
+		netdev_info(dev, "HIF_PS_ADH_SET_REQ\n");
+		break;
+	case HIF_PS_ADH_SET_CONF:
+		netdev_info(dev, "HIF_PS_ADH_SET_CONF\n");
+		break;
+	case HIF_INFRA_SET_REQ:
+		netdev_info(dev, "HIF_INFRA_SET_REQ\n");
+		break;
+	case HIF_INFRA_SET_CONF:
+		netdev_info(dev, "HIF_INFRA_SET_CONF\n");
+		break;
+	case HIF_ADH_SET_REQ:
+		netdev_info(dev, "HIF_ADH_SET_REQ\n");
+		break;
+	case HIF_ADH_SET_CONF:
+		netdev_info(dev, "HIF_ADH_SET_CONF\n");
+		break;
+	case HIF_AP_SET_REQ:
+		netdev_info(dev, "HIF_AP_SET_REQ\n");
+		break;
+	case HIF_AP_SET_CONF:
+		netdev_info(dev, "HIF_AP_SET_CONF\n");
+		break;
+	case HIF_ASSOC_INFO_IND:
+		netdev_info(dev, "HIF_ASSOC_INFO_IND\n");
+		break;
+	case HIF_MIC_FAILURE_REQ:
+		netdev_info(dev, "HIF_MIC_FAILURE_REQ\n");
+		break;
+	case HIF_MIC_FAILURE_CONF:
+		netdev_info(dev, "HIF_MIC_FAILURE_CONF\n");
+		break;
+	case HIF_SCAN_REQ:
+		netdev_info(dev, "HIF_SCAN_REQ\n");
+		break;
+	case HIF_SCAN_CONF:
+		netdev_info(dev, "HIF_SCAN_CONF\n");
+		break;
+	case HIF_PHY_INFO_REQ:
+		netdev_info(dev, "HIF_PHY_INFO_REQ\n");
+		break;
+	case HIF_PHY_INFO_CONF:
+		netdev_info(dev, "HIF_PHY_INFO_CONF\n");
+		break;
+	case HIF_SLEEP_REQ:
+		netdev_info(dev, "HIF_SLEEP_REQ\n");
+		break;
+	case HIF_SLEEP_CONF:
+		netdev_info(dev, "HIF_SLEEP_CONF\n");
+		break;
+	case HIF_PHY_INFO_IND:
+		netdev_info(dev, "HIF_PHY_INFO_IND\n");
+		break;
+	case HIF_SCAN_IND:
+		netdev_info(dev, "HIF_SCAN_IND\n");
+		break;
+	case HIF_INFRA_SET2_REQ:
+		netdev_info(dev, "HIF_INFRA_SET2_REQ\n");
+		break;
+	case HIF_INFRA_SET2_CONF:
+		netdev_info(dev, "HIF_INFRA_SET2_CONF\n");
+		break;
+	case HIF_ADH_SET2_REQ:
+		netdev_info(dev, "HIF_ADH_SET2_REQ\n");
+		break;
+	case HIF_ADH_SET2_CONF:
+		netdev_info(dev, "HIF_ADH_SET2_CONF\n");
+	}
+}
+
+/* get host command history */
+static int ks_wlan_hostt(struct net_device *dev, struct iw_request_info *info,
+			 __u32 *uwrq, char *extra)
+{
+	int i, event;
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	for (i = 63; i >= 0; i--) {
+		event =
+		    priv->hostt.buff[(priv->hostt.qtail - 1 - i) %
+				     SME_EVENT_BUFF_SIZE];
+		print_hif_event(dev, event);
+	}
+	return 0;
+}
+
+/* Structures to export the Wireless Handlers */
+
+static const struct iw_priv_args ks_wlan_private_args[] = {
+/*{ cmd, set_args, get_args, name[16] } */
+	{KS_WLAN_GET_FIRM_VERSION, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_CHAR | (128 + 1), "GetFirmwareVer"},
+	{KS_WLAN_SET_WPS_ENABLE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetWPSEnable"},
+	{KS_WLAN_GET_WPS_ENABLE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetW"},
+	{KS_WLAN_SET_WPS_PROBE_REQ, IW_PRIV_TYPE_BYTE | 2047, IW_PRIV_TYPE_NONE,
+	 "SetWPSProbeReq"},
+	{KS_WLAN_SET_PREAMBLE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetPreamble"},
+	{KS_WLAN_GET_PREAMBLE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetPreamble"},
+	{KS_WLAN_SET_POWER_SAVE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetPowerSave"},
+	{KS_WLAN_GET_POWER_SAVE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetPowerSave"},
+	{KS_WLAN_SET_SCAN_TYPE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetScanType"},
+	{KS_WLAN_GET_SCAN_TYPE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetScanType"},
+	{KS_WLAN_SET_RX_GAIN, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetRxGain"},
+	{KS_WLAN_GET_RX_GAIN, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetRxGain"},
+	{KS_WLAN_HOSTT, IW_PRIV_TYPE_NONE, IW_PRIV_TYPE_CHAR | (128 + 1),
+	 "hostt"},
+	{KS_WLAN_SET_BEACON_LOST, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetBeaconLost"},
+	{KS_WLAN_GET_BEACON_LOST, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetBeaconLost"},
+	{KS_WLAN_SET_SLEEP_MODE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetSleepMode"},
+	{KS_WLAN_GET_SLEEP_MODE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetSleepMode"},
+	{KS_WLAN_SET_TX_GAIN, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetTxGain"},
+	{KS_WLAN_GET_TX_GAIN, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetTxGain"},
+	{KS_WLAN_SET_PHY_TYPE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetPhyType"},
+	{KS_WLAN_GET_PHY_TYPE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetPhyType"},
+	{KS_WLAN_SET_CTS_MODE, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 IW_PRIV_TYPE_NONE, "SetCtsMode"},
+	{KS_WLAN_GET_CTS_MODE, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetCtsMode"},
+	{KS_WLAN_GET_EEPROM_CKSUM, IW_PRIV_TYPE_NONE,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "GetChecksum"},
+};
+
+static const iw_handler ks_wlan_handler[] = {
+	IW_HANDLER(SIOCSIWCOMMIT, ks_wlan_config_commit),
+	IW_HANDLER(SIOCGIWNAME, ks_wlan_get_name),
+	IW_HANDLER(SIOCSIWFREQ, ks_wlan_set_freq),
+	IW_HANDLER(SIOCGIWFREQ, ks_wlan_get_freq),
+	IW_HANDLER(SIOCSIWMODE, ks_wlan_set_mode),
+	IW_HANDLER(SIOCGIWMODE, ks_wlan_get_mode),
+	IW_HANDLER(SIOCGIWRANGE, ks_wlan_get_range),
+	IW_HANDLER(SIOCGIWSTATS, ks_wlan_get_iwstats),
+	IW_HANDLER(SIOCSIWAP, ks_wlan_set_wap),
+	IW_HANDLER(SIOCGIWAP, ks_wlan_get_wap),
+	IW_HANDLER(SIOCSIWMLME, ks_wlan_set_mlme),
+	IW_HANDLER(SIOCGIWAPLIST, ks_wlan_get_aplist),
+	IW_HANDLER(SIOCSIWSCAN, ks_wlan_set_scan),
+	IW_HANDLER(SIOCGIWSCAN, ks_wlan_get_scan),
+	IW_HANDLER(SIOCSIWESSID, ks_wlan_set_essid),
+	IW_HANDLER(SIOCGIWESSID, ks_wlan_get_essid),
+	IW_HANDLER(SIOCSIWNICKN, ks_wlan_set_nick),
+	IW_HANDLER(SIOCGIWNICKN, ks_wlan_get_nick),
+	IW_HANDLER(SIOCSIWRATE, ks_wlan_set_rate),
+	IW_HANDLER(SIOCGIWRATE, ks_wlan_get_rate),
+	IW_HANDLER(SIOCSIWRTS, ks_wlan_set_rts),
+	IW_HANDLER(SIOCGIWRTS, ks_wlan_get_rts),
+	IW_HANDLER(SIOCSIWFRAG, ks_wlan_set_frag),
+	IW_HANDLER(SIOCGIWFRAG, ks_wlan_get_frag),
+	IW_HANDLER(SIOCSIWENCODE, ks_wlan_set_encode),
+	IW_HANDLER(SIOCGIWENCODE, ks_wlan_get_encode),
+	IW_HANDLER(SIOCSIWPOWER, ks_wlan_set_power),
+	IW_HANDLER(SIOCGIWPOWER, ks_wlan_get_power),
+	IW_HANDLER(SIOCSIWGENIE, ks_wlan_set_genie),
+	IW_HANDLER(SIOCSIWAUTH, ks_wlan_set_auth_mode),
+	IW_HANDLER(SIOCGIWAUTH, ks_wlan_get_auth_mode),
+	IW_HANDLER(SIOCSIWENCODEEXT, ks_wlan_set_encode_ext),
+	IW_HANDLER(SIOCGIWENCODEEXT, ks_wlan_get_encode_ext),
+	IW_HANDLER(SIOCSIWPMKSA, ks_wlan_set_pmksa),
+};
+
+/* private_handler */
+static const iw_handler ks_wlan_private_handler[] = {
+	(iw_handler)NULL,			/* 0 */
+	(iw_handler)NULL,			/* 1, KS_WLAN_GET_DRIVER_VERSION */
+	(iw_handler)NULL,			/* 2 */
+	(iw_handler)ks_wlan_get_firmware_version,/* 3 KS_WLAN_GET_FIRM_VERSION */
+	(iw_handler)ks_wlan_set_wps_enable,	/* 4 KS_WLAN_SET_WPS_ENABLE */
+	(iw_handler)ks_wlan_get_wps_enable,	/* 5 KS_WLAN_GET_WPS_ENABLE */
+	(iw_handler)ks_wlan_set_wps_probe_req,	/* 6 KS_WLAN_SET_WPS_PROBE_REQ */
+	(iw_handler)ks_wlan_get_eeprom_cksum,	/* 7 KS_WLAN_GET_CONNECT */
+	(iw_handler)ks_wlan_set_preamble,	/* 8 KS_WLAN_SET_PREAMBLE */
+	(iw_handler)ks_wlan_get_preamble,	/* 9 KS_WLAN_GET_PREAMBLE */
+	(iw_handler)ks_wlan_set_power_mgmt,	/* 10 KS_WLAN_SET_POWER_SAVE */
+	(iw_handler)ks_wlan_get_power_mgmt,	/* 11 KS_WLAN_GET_POWER_SAVE */
+	(iw_handler)ks_wlan_set_scan_type,	/* 12 KS_WLAN_SET_SCAN_TYPE */
+	(iw_handler)ks_wlan_get_scan_type,	/* 13 KS_WLAN_GET_SCAN_TYPE */
+	(iw_handler)ks_wlan_set_rx_gain,	/* 14 KS_WLAN_SET_RX_GAIN */
+	(iw_handler)ks_wlan_get_rx_gain,	/* 15 KS_WLAN_GET_RX_GAIN */
+	(iw_handler)ks_wlan_hostt,		/* 16 KS_WLAN_HOSTT */
+	(iw_handler)NULL,			/* 17 */
+	(iw_handler)ks_wlan_set_beacon_lost,	/* 18 KS_WLAN_SET_BECAN_LOST */
+	(iw_handler)ks_wlan_get_beacon_lost,	/* 19 KS_WLAN_GET_BECAN_LOST */
+	(iw_handler)ks_wlan_set_tx_gain,	/* 20 KS_WLAN_SET_TX_GAIN */
+	(iw_handler)ks_wlan_get_tx_gain,	/* 21 KS_WLAN_GET_TX_GAIN */
+	(iw_handler)ks_wlan_set_phy_type,	/* 22 KS_WLAN_SET_PHY_TYPE */
+	(iw_handler)ks_wlan_get_phy_type,	/* 23 KS_WLAN_GET_PHY_TYPE */
+	(iw_handler)ks_wlan_set_cts_mode,	/* 24 KS_WLAN_SET_CTS_MODE */
+	(iw_handler)ks_wlan_get_cts_mode,	/* 25 KS_WLAN_GET_CTS_MODE */
+	(iw_handler)NULL,			/* 26 */
+	(iw_handler)NULL,			/* 27 */
+	(iw_handler)ks_wlan_set_sleep_mode,	/* 28 KS_WLAN_SET_SLEEP_MODE */
+	(iw_handler)ks_wlan_get_sleep_mode,	/* 29 KS_WLAN_GET_SLEEP_MODE */
+	(iw_handler)NULL,			/* 30 */
+	(iw_handler)NULL,			/* 31 */
+};
+
+static const struct iw_handler_def ks_wlan_handler_def = {
+	.num_standard = ARRAY_SIZE(ks_wlan_handler),
+	.num_private = ARRAY_SIZE(ks_wlan_private_handler),
+	.num_private_args = ARRAY_SIZE(ks_wlan_private_args),
+	.standard = ks_wlan_handler,
+	.private = ks_wlan_private_handler,
+	.private_args = ks_wlan_private_args,
+	.get_wireless_stats = ks_get_wireless_stats,
+};
+
+static int ks_wlan_netdev_ioctl(struct net_device *dev, struct ifreq *rq,
+				int cmd)
+{
+	int ret;
+	struct iwreq *wrq = (struct iwreq *)rq;
+
+	switch (cmd) {
+	case SIOCIWFIRSTPRIV + 20:	/* KS_WLAN_SET_STOP_REQ */
+		ret = ks_wlan_set_stop_request(dev, NULL, &wrq->u.mode, NULL);
+		break;
+		// All other calls are currently unsupported
+	default:
+		ret = -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+static
+struct net_device_stats *ks_wlan_get_stats(struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->dev_state < DEVICE_STATE_READY)
+		return NULL;	/* not finished initialize */
+
+	return &priv->nstats;
+}
+
+static
+int ks_wlan_set_mac_address(struct net_device *dev, void *addr)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	struct sockaddr *mac_addr = (struct sockaddr *)addr;
+
+	if (netif_running(dev))
+		return -EBUSY;
+	eth_hw_addr_set(dev, mac_addr->sa_data);
+	ether_addr_copy(priv->eth_addr, mac_addr->sa_data);
+
+	priv->mac_address_valid = false;
+	hostif_sme_enqueue(priv, SME_MACADDRESS_SET_REQUEST);
+	netdev_info(dev, "ks_wlan:  MAC ADDRESS = %pM\n", priv->eth_addr);
+	return 0;
+}
+
+static
+void ks_wlan_tx_timeout(struct net_device *dev, unsigned int txqueue)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	netdev_dbg(dev, "head(%d) tail(%d)!!\n", priv->tx_dev.qhead,
+		   priv->tx_dev.qtail);
+	if (!netif_queue_stopped(dev))
+		netif_stop_queue(dev);
+	priv->nstats.tx_errors++;
+	netif_wake_queue(dev);
+}
+
+static
+netdev_tx_t ks_wlan_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int ret;
+
+	netdev_dbg(dev, "in_interrupt()=%ld\n", in_interrupt());
+
+	if (!skb) {
+		netdev_err(dev, "ks_wlan:  skb == NULL!!!\n");
+		return 0;
+	}
+	if (priv->dev_state < DEVICE_STATE_READY) {
+		dev_kfree_skb(skb);
+		return 0;	/* not finished initialize */
+	}
+
+	if (netif_running(dev))
+		netif_stop_queue(dev);
+
+	ret = hostif_data_request(priv, skb);
+	netif_trans_update(dev);
+
+	if (ret)
+		netdev_err(dev, "hostif_data_request error: =%d\n", ret);
+
+	return 0;
+}
+
+void send_packet_complete(struct ks_wlan_private *priv, struct sk_buff *skb)
+{
+	priv->nstats.tx_packets++;
+
+	if (netif_queue_stopped(priv->net_dev))
+		netif_wake_queue(priv->net_dev);
+
+	if (skb) {
+		priv->nstats.tx_bytes += skb->len;
+		dev_kfree_skb(skb);
+	}
+}
+
+/*
+ * Set or clear the multicast filter for this adaptor.
+ * This routine is not state sensitive and need not be SMP locked.
+ */
+static
+void ks_wlan_set_rx_mode(struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	if (priv->dev_state < DEVICE_STATE_READY)
+		return;	/* not finished initialize */
+	hostif_sme_enqueue(priv, SME_MULTICAST_REQUEST);
+}
+
+static
+int ks_wlan_open(struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	priv->cur_rx = 0;
+
+	if (!priv->mac_address_valid) {
+		netdev_err(dev, "ks_wlan : %s Not READY !!\n", dev->name);
+		return -EBUSY;
+	}
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+static
+int ks_wlan_close(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+
+	return 0;
+}
+
+/* Operational parameters that usually are not changed. */
+/* Time in jiffies before concluding the transmitter is hung. */
+#define TX_TIMEOUT  (3 * HZ)
+static const unsigned char dummy_addr[] = {
+	0x00, 0x0b, 0xe3, 0x00, 0x00, 0x00
+};
+
+static const struct net_device_ops ks_wlan_netdev_ops = {
+	.ndo_start_xmit = ks_wlan_start_xmit,
+	.ndo_open = ks_wlan_open,
+	.ndo_stop = ks_wlan_close,
+	.ndo_do_ioctl = ks_wlan_netdev_ioctl,
+	.ndo_set_mac_address = ks_wlan_set_mac_address,
+	.ndo_get_stats = ks_wlan_get_stats,
+	.ndo_tx_timeout = ks_wlan_tx_timeout,
+	.ndo_set_rx_mode = ks_wlan_set_rx_mode,
+};
+
+int ks_wlan_net_start(struct net_device *dev)
+{
+	struct ks_wlan_private *priv;
+	/* int rc; */
+
+	priv = netdev_priv(dev);
+	priv->mac_address_valid = false;
+	priv->is_device_open = true;
+	priv->need_commit = 0;
+	/* phy information update timer */
+	atomic_set(&update_phyinfo, 0);
+	timer_setup(&update_phyinfo_timer, ks_wlan_update_phyinfo_timeout, 0);
+
+	/* dummy address set */
+	ether_addr_copy(priv->eth_addr, dummy_addr);
+	eth_hw_addr_set(dev, priv->eth_addr);
+
+	/* The ks_wlan-specific entries in the device structure. */
+	dev->netdev_ops = &ks_wlan_netdev_ops;
+	dev->wireless_handlers = &ks_wlan_handler_def;
+	dev->watchdog_timeo = TX_TIMEOUT;
+
+	netif_carrier_off(dev);
+
+	return 0;
+}
+
+int ks_wlan_net_stop(struct net_device *dev)
+{
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
+	priv->is_device_open = false;
+	del_timer_sync(&update_phyinfo_timer);
+
+	if (netif_running(dev))
+		netif_stop_queue(dev);
+
+	return 0;
+}
+
+/**
+ * is_connect_status() - return true if status is 'connected'
+ * @status: high bit is used as FORCE_DISCONNECT, low bits used for
+ *	connect status.
+ */
+bool is_connect_status(u32 status)
+{
+	return (status & CONNECT_STATUS_MASK) == CONNECT_STATUS;
+}
+
+/**
+ * is_disconnect_status() - return true if status is 'disconnected'
+ * @status: high bit is used as FORCE_DISCONNECT, low bits used for
+ *	disconnect status.
+ */
+bool is_disconnect_status(u32 status)
+{
+	return (status & CONNECT_STATUS_MASK) == DISCONNECT_STATUS;
+}
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
new file mode 100644
index 000000000..d4f03b203
--- /dev/null
+++ b/drivers/staging/media/Kconfig
@@ -0,0 +1,64 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig STAGING_MEDIA
+	bool "Media staging drivers"
+	default n
+	help
+	  This option allows you to select a number of media drivers that
+	  don't have the "normal" Linux kernel quality level.
+	  Most of them don't follow properly the V4L, DVB and/or RC API's,
+	  so, they won't likely work fine with the existing applications.
+	  That also means that, once fixed, their API's will change to match
+	  the existing ones.
+
+	  If you wish to work on these drivers, to help improve them, or
+	  to report problems you have with them, please use the
+	  linux-media@vger.kernel.org mailing list.
+
+	  If in doubt, say N here.
+
+
+if STAGING_MEDIA && MEDIA_SUPPORT
+
+# Please keep them in alphabetic order
+source "drivers/staging/media/atomisp/Kconfig"
+
+source "drivers/staging/media/imx/Kconfig"
+
+source "drivers/staging/media/ipu3/Kconfig"
+
+source "drivers/staging/media/max96712/Kconfig"
+
+source "drivers/staging/media/meson/vdec/Kconfig"
+
+source "drivers/staging/media/omap4iss/Kconfig"
+
+source "drivers/staging/media/rkvdec/Kconfig"
+
+source "drivers/staging/media/sunxi/Kconfig"
+
+source "drivers/staging/media/tegra-video/Kconfig"
+
+menuconfig STAGING_MEDIA_DEPRECATED
+	bool "Media staging drivers (DEPRECATED)"
+	default n
+	help
+	  This option enables deprecated media drivers that are
+	  scheduled for future removal from the kernel.
+
+	  If you wish to work on these drivers to prevent their removal,
+	  then contact the linux-media@vger.kernel.org mailing list.
+
+	  If in doubt, say N here.
+
+if STAGING_MEDIA_DEPRECATED
+source "drivers/staging/media/deprecated/cpia2/Kconfig"
+source "drivers/staging/media/deprecated/fsl-viu/Kconfig"
+source "drivers/staging/media/deprecated/meye/Kconfig"
+source "drivers/staging/media/deprecated/saa7146/Kconfig"
+source "drivers/staging/media/deprecated/stkwebcam/Kconfig"
+source "drivers/staging/media/deprecated/tm6000/Kconfig"
+source "drivers/staging/media/deprecated/vpfe_capture/Kconfig"
+source "drivers/staging/media/deprecated/zr364xx/Kconfig"
+endif
+
+endif
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
new file mode 100644
index 000000000..a387692b8
--- /dev/null
+++ b/drivers/staging/media/Makefile
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_INTEL_ATOMISP)     += atomisp/
+obj-$(CONFIG_VIDEO_CPIA2)	+= deprecated/cpia2/
+obj-$(CONFIG_VIDEO_IMX_MEDIA)	+= imx/
+obj-$(CONFIG_VIDEO_MAX96712)	+= max96712/
+obj-$(CONFIG_VIDEO_MESON_VDEC)	+= meson/vdec/
+obj-$(CONFIG_VIDEO_MEYE)	+= deprecated/meye/
+obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
+obj-$(CONFIG_VIDEO_ROCKCHIP_VDEC)	+= rkvdec/
+obj-$(CONFIG_VIDEO_STKWEBCAM)	+= deprecated/stkwebcam/
+obj-$(CONFIG_VIDEO_SUNXI)	+= sunxi/
+obj-$(CONFIG_VIDEO_TEGRA)	+= tegra-video/
+obj-$(CONFIG_VIDEO_IPU3_IMGU)	+= ipu3/
+obj-$(CONFIG_VIDEO_TM6000)	+= deprecated/tm6000/
+obj-$(CONFIG_VIDEO_VIU)		+= deprecated/fsl-viu/
+obj-$(CONFIG_USB_ZR364XX)	+= deprecated/zr364xx/
+obj-y += deprecated/vpfe_capture/
+obj-y += deprecated/saa7146/
diff --git a/drivers/staging/media/atomisp/Kconfig b/drivers/staging/media/atomisp/Kconfig
new file mode 100644
index 000000000..4881b6ea5
--- /dev/null
+++ b/drivers/staging/media/atomisp/Kconfig
@@ -0,0 +1,40 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig INTEL_ATOMISP
+	bool "Enable support to Intel Atom ISP camera drivers"
+	depends on X86 && EFI && PCI && ACPI
+	depends on COMMON_CLK
+	select IOSF_MBI
+	select MEDIA_CONTROLLER
+	help
+	  Enable support for the Intel ISP2 camera interfaces and MIPI
+	  sensor drivers.
+
+config VIDEO_ATOMISP
+	tristate "Intel Atom Image Signal Processor Driver"
+	depends on VIDEO_DEV && INTEL_ATOMISP
+	depends on PMIC_OPREGION
+	select V4L2_FWNODE
+	select IOSF_MBI
+	select VIDEOBUF_VMALLOC
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Say Y here if your platform supports Intel Atom SoC
+	  camera imaging subsystem.
+	  To compile this driver as a module, choose M here: the
+	  module will be called atomisp
+
+config VIDEO_ATOMISP_ISP2401
+	bool "Use Intel Atom ISP on Cherrytail/Anniedale (ISP2401)"
+	depends on VIDEO_ATOMISP
+	help
+	  Enable support for Atom ISP2401-based boards.
+
+	  Select this option for  Anniedale (Merrifield+ / Moorefield)
+	  and Cherrytrail SoCs.
+
+	  Disabling it enables support for Atom ISP2400-based boards
+	  (Merrifield and Baytrail SoCs).
+
+if VIDEO_ATOMISP
+source "drivers/staging/media/atomisp/i2c/Kconfig"
+endif
diff --git a/drivers/staging/media/atomisp/Makefile b/drivers/staging/media/atomisp/Makefile
new file mode 100644
index 000000000..532e12ed7
--- /dev/null
+++ b/drivers/staging/media/atomisp/Makefile
@@ -0,0 +1,315 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for camera drivers.
+#
+obj-$(CONFIG_INTEL_ATOMISP) += i2c/
+obj-$(CONFIG_VIDEO_ATOMISP) += atomisp.o
+obj-$(CONFIG_VIDEO_ATOMISP) += pci/atomisp_gmin_platform.o
+
+# While on staging, keep debug enabled
+DEFINES += -DDEBUG
+
+atomisp = $(srctree)/drivers/staging/media/atomisp/
+
+# SPDX-License-Identifier: GPL-2.0
+atomisp-objs += \
+	pci/atomisp_cmd.o \
+	pci/atomisp_compat_css20.o \
+	pci/atomisp_csi2.o \
+	pci/atomisp_drvfs.o \
+	pci/atomisp_fops.o \
+	pci/atomisp_ioctl.o \
+	pci/atomisp_subdev.o \
+	pci/atomisp_tpg.o \
+	pci/atomisp_v4l2.o \
+	pci/sh_css_firmware.o \
+	pci/sh_css_host_data.o \
+	pci/sh_css_hrt.o \
+	pci/sh_css_metrics.o \
+	pci/sh_css_mipi.o \
+	pci/sh_css_mmu.o \
+	pci/sh_css.o \
+	pci/sh_css_param_dvs.o \
+	pci/sh_css_param_shading.o \
+	pci/sh_css_params.o \
+	pci/sh_css_properties.o \
+	pci/sh_css_sp.o \
+	pci/sh_css_stream_format.o \
+	pci/sh_css_version.o \
+	pci/base/circbuf/src/circbuf.o \
+	pci/base/refcount/src/refcount.o \
+	pci/camera/pipe/src/pipe_binarydesc.o \
+	pci/camera/pipe/src/pipe_stagedesc.o \
+	pci/camera/pipe/src/pipe_util.o \
+	pci/camera/util/src/util.o \
+	pci/hmm/hmm_bo.o \
+	pci/hmm/hmm.o \
+	pci/ia_css_device_access.o \
+	pci/ia_css_isp_configs.o \
+	pci/ia_css_isp_states.o \
+	pci/ia_css_isp_params.o \
+	pci/isp/kernels/aa/aa_2/ia_css_aa2.host.o \
+	pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.o \
+	pci/isp/kernels/anr/anr_2/ia_css_anr2.host.o \
+	pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.o \
+	pci/isp/kernels/bh/bh_2/ia_css_bh.host.o \
+	pci/isp/kernels/bnlm/ia_css_bnlm.host.o \
+	pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.o \
+	pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.o \
+	pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.o \
+	pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.o \
+	pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.o \
+	pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.o \
+	pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.o \
+	pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.o \
+	pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.o \
+	pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.o \
+	pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.o \
+	pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.o \
+	pci/isp/kernels/de/de_1.0/ia_css_de.host.o \
+	pci/isp/kernels/de/de_2/ia_css_de2.host.o \
+	pci/isp/kernels/dpc2/ia_css_dpc2.host.o \
+	pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.o \
+	pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.o \
+	pci/isp/kernels/eed1_8/ia_css_eed1_8.host.o \
+	pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.o \
+	pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.o \
+	pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.o \
+	pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.o \
+	pci/isp/kernels/gc/gc_2/ia_css_gc2.host.o \
+	pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.o \
+	pci/isp/kernels/hdr/ia_css_hdr.host.o \
+	pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.o \
+	pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.o \
+	pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.o \
+	pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.o \
+	pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.o \
+	pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.o \
+	pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.o \
+	pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.o \
+	pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.o \
+	pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.o \
+	pci/isp/kernels/ob/ob2/ia_css_ob2.host.o \
+	pci/isp/kernels/output/output_1.0/ia_css_output.host.o \
+	pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.o \
+	pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.o \
+	pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.o \
+	pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.o \
+	pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.o \
+	pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.o \
+	pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.o \
+	pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.o \
+	pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.o \
+	pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.o \
+	pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.o \
+	pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.o \
+	pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.o \
+	pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.o \
+	pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.o \
+	pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.o \
+	pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.o \
+	pci/mmu/isp_mmu.o \
+	pci/mmu/sh_mmu_mrfld.o \
+	pci/runtime/binary/src/binary.o \
+	pci/runtime/bufq/src/bufq.o \
+	pci/runtime/debug/src/ia_css_debug.o \
+	pci/runtime/eventq/src/eventq.o \
+	pci/runtime/event/src/event.o \
+	pci/runtime/frame/src/frame.o \
+	pci/runtime/ifmtr/src/ifmtr.o \
+	pci/runtime/inputfifo/src/inputfifo.o \
+	pci/runtime/isp_param/src/isp_param.o \
+	pci/runtime/isys/src/csi_rx_rmgr.o \
+	pci/runtime/isys/src/isys_dma_rmgr.o \
+	pci/runtime/isys/src/isys_init.o \
+	pci/runtime/isys/src/isys_stream2mmio_rmgr.o \
+	pci/runtime/isys/src/rx.o \
+	pci/runtime/isys/src/virtual_isys.o \
+	pci/runtime/pipeline/src/pipeline.o \
+	pci/runtime/queue/src/queue_access.o \
+	pci/runtime/queue/src/queue.o \
+	pci/runtime/rmgr/src/rmgr.o \
+	pci/runtime/rmgr/src/rmgr_vbuf.o \
+	pci/runtime/spctrl/src/spctrl.o \
+	pci/runtime/timer/src/timer.o \
+	pci/hive_isp_css_common/host/debug.o \
+	pci/hive_isp_css_common/host/dma.o \
+	pci/hive_isp_css_common/host/event_fifo.o \
+	pci/hive_isp_css_common/host/fifo_monitor.o \
+	pci/hive_isp_css_common/host/gdc.o \
+	pci/hive_isp_css_common/host/gp_device.o \
+	pci/hive_isp_css_common/host/gp_timer.o \
+	pci/hive_isp_css_common/host/hmem.o \
+	pci/hive_isp_css_common/host/input_formatter.o \
+	pci/hive_isp_css_common/host/input_system.o \
+	pci/hive_isp_css_common/host/irq.o \
+	pci/hive_isp_css_common/host/isp.o \
+	pci/hive_isp_css_common/host/mmu.o \
+	pci/hive_isp_css_common/host/sp.o \
+	pci/hive_isp_css_common/host/timed_ctrl.o \
+	pci/hive_isp_css_common/host/vmem.o \
+	pci/hive_isp_css_shared/host/tag.o \
+	pci/system_local.o
+
+# These will be needed when clean merge CHT support nicely into the driver
+# Keep them here handy for when we get to that point
+#
+
+obj-cht = \
+	pci/css_2401_system/host/csi_rx.o \
+	pci/css_2401_system/host/ibuf_ctrl.o \
+	pci/css_2401_system/host/isys_dma.o \
+	pci/css_2401_system/host/isys_irq.o \
+	pci/css_2401_system/host/isys_stream2mmio.o
+
+INCLUDES += \
+	-I$(atomisp)/ \
+	-I$(atomisp)/include/ \
+	-I$(atomisp)/include/hmm/ \
+	-I$(atomisp)/include/mmu/ \
+	-I$(atomisp)/pci/ \
+	-I$(atomisp)/pci/base/circbuf/interface/ \
+	-I$(atomisp)/pci/base/refcount/interface/ \
+	-I$(atomisp)/pci/camera/pipe/interface/ \
+	-I$(atomisp)/pci/camera/util/interface/ \
+	-I$(atomisp)/pci/hive_isp_css_common/ \
+	-I$(atomisp)/pci/hive_isp_css_common/host/ \
+	-I$(atomisp)/pci/hive_isp_css_include/ \
+	-I$(atomisp)/pci/hive_isp_css_include/device_access/ \
+	-I$(atomisp)/pci/hive_isp_css_include/host/ \
+	-I$(atomisp)/pci/hive_isp_css_shared/ \
+	-I$(atomisp)/pci/hive_isp_css_shared/host/ \
+	-I$(atomisp)/pci/isp/kernels/ \
+	-I$(atomisp)/pci/isp/kernels/aa/aa_2/ \
+	-I$(atomisp)/pci/isp/kernels/anr/anr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/anr/anr_2/ \
+	-I$(atomisp)/pci/isp/kernels/bh/bh_2/ \
+	-I$(atomisp)/pci/isp/kernels/bnlm/ \
+	-I$(atomisp)/pci/isp/kernels/bnr/ \
+	-I$(atomisp)/pci/isp/kernels/bnr/bnr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/bnr/bnr2_2/ \
+	-I$(atomisp)/pci/isp/kernels/cnr/ \
+	-I$(atomisp)/pci/isp/kernels/cnr/cnr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/cnr/cnr_2/ \
+	-I$(atomisp)/pci/isp/kernels/conversion/ \
+	-I$(atomisp)/pci/isp/kernels/conversion/conversion_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/copy_output/ \
+	-I$(atomisp)/pci/isp/kernels/copy_output/copy_output_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/crop/ \
+	-I$(atomisp)/pci/isp/kernels/crop/crop_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/csc/ \
+	-I$(atomisp)/pci/isp/kernels/csc/csc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ctc/ \
+	-I$(atomisp)/pci/isp/kernels/ctc/ctc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ctc/ctc1_5/ \
+	-I$(atomisp)/pci/isp/kernels/ctc/ctc2/ \
+	-I$(atomisp)/pci/isp/kernels/de/ \
+	-I$(atomisp)/pci/isp/kernels/de/de_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/de/de_2/ \
+	-I$(atomisp)/pci/isp/kernels/dp/ \
+	-I$(atomisp)/pci/isp/kernels/dpc2/ \
+	-I$(atomisp)/pci/isp/kernels/dp/dp_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/dvs/ \
+	-I$(atomisp)/pci/isp/kernels/dvs/dvs_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/eed1_8/ \
+	-I$(atomisp)/pci/isp/kernels/fc/ \
+	-I$(atomisp)/pci/isp/kernels/fc/fc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/fixedbds/ \
+	-I$(atomisp)/pci/isp/kernels/fixedbds/fixedbds_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/fpn/ \
+	-I$(atomisp)/pci/isp/kernels/fpn/fpn_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/gc/ \
+	-I$(atomisp)/pci/isp/kernels/gc/gc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/gc/gc_2/ \
+	-I$(atomisp)/pci/isp/kernels/hdr/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/common/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/common/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ \
+	-I$(atomisp)/pci/isp/kernels/iterator/ \
+	-I$(atomisp)/pci/isp/kernels/iterator/iterator_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/macc/ \
+	-I$(atomisp)/pci/isp/kernels/macc/macc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/macc/macc1_5/ \
+	-I$(atomisp)/pci/isp/kernels/norm/ \
+	-I$(atomisp)/pci/isp/kernels/norm/norm_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ob/ \
+	-I$(atomisp)/pci/isp/kernels/ob/ob_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ob/ob2/ \
+	-I$(atomisp)/pci/isp/kernels/output/ \
+	-I$(atomisp)/pci/isp/kernels/output/output_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/qplane/ \
+	-I$(atomisp)/pci/isp/kernels/qplane/qplane_2/ \
+	-I$(atomisp)/pci/isp/kernels/raw/ \
+	-I$(atomisp)/pci/isp/kernels/raw_aa_binning/ \
+	-I$(atomisp)/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/raw/raw_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ref/ \
+	-I$(atomisp)/pci/isp/kernels/ref/ref_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/s3a/ \
+	-I$(atomisp)/pci/isp/kernels/s3a/s3a_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/sc/ \
+	-I$(atomisp)/pci/isp/kernels/sc/sc_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/sdis/ \
+	-I$(atomisp)/pci/isp/kernels/sdis/common/ \
+	-I$(atomisp)/pci/isp/kernels/sdis/sdis_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/sdis/sdis_2/ \
+	-I$(atomisp)/pci/isp/kernels/tdf/ \
+	-I$(atomisp)/pci/isp/kernels/tdf/tdf_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/tnr/ \
+	-I$(atomisp)/pci/isp/kernels/tnr/tnr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/tnr/tnr3/ \
+	-I$(atomisp)/pci/isp/kernels/uds/ \
+	-I$(atomisp)/pci/isp/kernels/uds/uds_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/vf/ \
+	-I$(atomisp)/pci/isp/kernels/vf/vf_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/wb/ \
+	-I$(atomisp)/pci/isp/kernels/wb/wb_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/xnr/ \
+	-I$(atomisp)/pci/isp/kernels/xnr/xnr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/xnr/xnr_3.0/ \
+	-I$(atomisp)/pci/isp/kernels/ynr/ \
+	-I$(atomisp)/pci/isp/kernels/ynr/ynr_1.0/ \
+	-I$(atomisp)/pci/isp/kernels/ynr/ynr_2/ \
+	-I$(atomisp)/pci/isp/modes/interface/ \
+	-I$(atomisp)/pci/runtime/binary/interface/ \
+	-I$(atomisp)/pci/runtime/bufq/interface/ \
+	-I$(atomisp)/pci/runtime/debug/interface/ \
+	-I$(atomisp)/pci/runtime/event/interface/ \
+	-I$(atomisp)/pci/runtime/eventq/interface/ \
+	-I$(atomisp)/pci/runtime/frame/interface/ \
+	-I$(atomisp)/pci/runtime/ifmtr/interface/ \
+	-I$(atomisp)/pci/runtime/inputfifo/interface/ \
+	-I$(atomisp)/pci/runtime/isp_param/interface/ \
+	-I$(atomisp)/pci/runtime/isys/interface/ \
+	-I$(atomisp)/pci/runtime/isys/src/ \
+	-I$(atomisp)/pci/runtime/pipeline/interface/ \
+	-I$(atomisp)/pci/runtime/queue/interface/ \
+	-I$(atomisp)/pci/runtime/queue/src/ \
+	-I$(atomisp)/pci/runtime/rmgr/interface/ \
+	-I$(atomisp)/pci/runtime/spctrl/interface/ \
+	-I$(atomisp)/pci/runtime/tagger/interface/ \
+	-I$(atomisp)/pci/css_2401_system/ \
+	-I$(atomisp)/pci/css_2401_system/host/ \
+	-I$(atomisp)/pci/css_2401_system/hrt/
+
+DEFINES := -DHRT_HW -DHRT_ISP_CSS_CUSTOM_HOST -DHRT_USE_VIR_ADDRS -D__HOST__
+#DEFINES += -DUSE_DYNAMIC_BIN
+#DEFINES += -DISP_POWER_GATING
+#DEFINES += -DUSE_INTERRUPTS
+#DEFINES += -DUSE_SSSE3
+#DEFINES += -DPUNIT_CAMERA_BUSY
+#DEFINES += -DUSE_KMEM_CACHE
+
+ifeq ($(CONFIG_VIDEO_ATOMISP_ISP2401),y)
+atomisp-objs += \
+	$(obj-cht) \
+	pci/runtime/isys/src/ibuf_ctrl_rmgr.o
+DEFINES += -DISP2401
+endif
+
+ccflags-y += $(INCLUDES) $(DEFINES) -fno-common
diff --git a/drivers/staging/media/atomisp/TODO b/drivers/staging/media/atomisp/TODO
new file mode 100644
index 000000000..43b842043
--- /dev/null
+++ b/drivers/staging/media/atomisp/TODO
@@ -0,0 +1,213 @@
+For both Cherrytrail (CHT) and Baytrail (BHT) the driver
+requires the "candrpv_0415_20150521_0458" firmware version.
+It should be noticed that the firmware file is different,
+depending on the ISP model, so they're stored with different
+names:
+
+- for BHT: /lib/firmware/shisp_2400b0_v21.bin
+
+  Warning: The driver was not tested yet for BHT.
+
+- for CHT: /lib/firmware/shisp_2401a0_v21.bin
+
+  https://github.com/intel-aero/meta-intel-aero-base/blob/master/recipes-kernel/linux/linux-yocto/shisp_2401a0_v21.bin
+
+NOTE:
+=====
+
+This driver currently doesn't work with most V4L2 applications,
+as there are still some issues with regards to implementing
+certain APIs at the standard way.
+
+Also, currently only USERPTR streaming mode is working.
+
+In order to test, it is needed to know what's the sensor's
+resolution. This can be checked with:
+
+$ v4l2-ctl --get-fmt-video
+  Format Video Capture:
+	Width/Height      : 1600/1200
+	...
+
+It is known to work with:
+
+- v4l2grab at contrib/test directory at https://git.linuxtv.org/v4l-utils.git/
+
+  The resolution should not be bigger than the max resolution
+  supported by the sensor, or it will fail. So, if the sensor
+  reports:
+
+  The driver can be tested with:
+
+  v4l2grab -f YUYV -x 1600 -y 1200 -d /dev/video2 -u
+
+- NVT at https://github.com/intel/nvt
+
+  $ ./v4l2n -o testimage_@.raw \
+		 --device /dev/video2 \
+		 --input 0 \
+		 --exposure=30000,30000,30000,30000 \
+		 --parm type=1,capturemode=CI_MODE_PREVIEW \
+		 --fmt type=1,width=1600,height=1200,pixelformat=YUYV \
+		 --reqbufs count=2,memory=USERPTR \
+		 --parameters=wb_config.r=32768,wb_config.gr=21043,wb_config.gb=21043,wb_config.b=30863 \
+		 --capture=20
+
+  As the output is in raw format, images need to be converted with:
+
+  $ for i in $(seq 0 19); do
+	name="testimage_$(printf "%03i" $i)"
+	./raw2pnm -x$WIDTH -y$HEIGHT -f$FORMAT $name.raw $name.pnm
+	rm $name.raw
+    done
+
+TODO
+====
+
+1.  Fix support for MMAP streaming mode. This is required for most
+    V4L2 applications;
+
+2.  Implement and/or fix V4L2 ioctls in order to allow a normal app to
+    use it;
+
+3.  Ensure that the driver will pass v4l2-compliance tests;
+
+4.  Get manufacturer's authorization to redistribute the binaries for
+    the firmware files;
+
+5.  remove VIDEO_ATOMISP_ISP2401, making the driver to auto-detect the
+    register address differences between ISP2400 and ISP2401;
+
+6.  Cleanup the driver code, removing the abstraction layers inside it;
+
+7.  The atomisp doesn't rely at the usual i2c stuff to discover the
+    sensors. Instead, it calls a function from atomisp_gmin_platform.c.
+    There are some hacks added there for it to wait for sensors to be
+    probed (with a timeout of 2 seconds or so). This should be converted
+    to the usual way, using V4L2 async subdev framework to wait for
+    cameras to be probed;
+
+8.  Switch to standard V4L2 sub-device API for sensor and lens. In
+    particular, the user space API needs to support V4L2 controls as
+    defined in the V4L2 spec and references to atomisp must be removed from
+    these drivers.
+
+9.  Use LED flash API for flash LED drivers such as LM3554 (which already
+    has a LED class driver).
+
+10. Migrate the sensor drivers out of staging or re-using existing
+    drivers;
+
+11. Switch the driver to use pm_runtime stuff. Right now, it probes the
+    existing PMIC code and sensors call it directly.
+
+12. There's a problem on sensor drivers: when trying to set a video
+    format, the atomisp main driver calls the sensor drivers with the
+    sensor turned off. This causes them to fail.
+
+    This was fixed at atomisp-ov2880, which has a hack inside it
+    to turn it on when VIDIOC_S_FMT is called, but this has to be
+    cheked on other drivers as well.
+
+    The right fix seems to power on the sensor when a video device is
+    opened (or at the first VIDIOC_ ioctl - except for VIDIOC_QUERYCAP),
+    powering it down at close() syscall.
+
+    Such kind of control would need to be done inside the atomisp driver,
+    not at the sensors code.
+
+13. There are several issues related to memory management, that can
+    cause crashes and/or memory leaks. The atomisp splits the memory
+    management on three separate regions:
+
+	- dynamic pool;
+	- reserved pool;
+	- generic pool
+
+    The code implementing it is at:
+
+	drivers/staging/media/atomisp/pci/hmm/
+
+    It also has a separate code for managing DMA buffers at:
+
+	drivers/staging/media/atomisp/pci/mmu/
+
+    The code there is really dirty, ugly and probably wrong. I fixed
+    one bug there already, but the best would be to just trash it and use
+    something else. Maybe the code from the newer intel driver could
+    serve as a model:
+
+	drivers/staging/media/ipu3/ipu3-mmu.c
+
+    But converting it to use something like that is painful and may
+    cause some breakages.
+
+14. The file structure needs to get tidied up to resemble a normal Linux
+    driver.
+
+15. Lots of the midlayer glue. Unused code and abstraction needs removing.
+
+16. The AtomISP driver includes some special IOCTLS (ATOMISP_IOC_XXXX_XXXX)
+    and controls that require some cleanup. Some of those code may have
+    been removed during the cleanups. They could be needed in order to
+    properly support 3A algorithms.
+
+    Such IOCTL interface needs more documentation. The better would
+    be to use something close to the interface used by the IPU3 IMGU driver.
+
+17. The ISP code has some dependencies of the exact FW version.
+    The version defined in pci/sh_css_firmware.c:
+
+    BYT (isp2400): "irci_stable_candrpv_0415_20150521_0458"
+
+    CHT (isp2401): "irci_ecr - master_20150911_0724"
+
+    Those versions don't seem to be available anymore. On the tests we've
+    done so far, this version also seems to work for CHT:
+
+		"irci_stable_candrpv_0415_20150521_0458"
+
+    Which can be obtainable from Yocto Atom ISP respository.
+
+    but this was not thoroughly tested.
+
+    At some point we may need to round up a few driver versions and see if
+    there are any specific things that can be done to fold in support for
+    multiple firmware versions.
+
+
+18. Switch from videobuf1 to videobuf2. Videobuf1 is being removed!
+
+19. Correct Coding Style. Please refrain sending coding style patches
+    for this driver until the other work is done, as there will be a lot
+    of code churn until this driver becomes functional again.
+
+20. Remove the logic which sets up pipelines inside it, moving it to
+    libcamera and implement MC support.
+
+
+Limitations
+===========
+
+1. To test the patches, you also need the ISP firmware
+
+   for BYT: /lib/firmware/shisp_2400b0_v21.bin
+   for CHT: /lib/firmware/shisp_2401a0_v21.bin
+
+   The firmware files will usually be found in /etc/firmware on an Android
+   device but can also be extracted from the upgrade kit if you've managed
+   to lose them somehow.
+
+2. Without a 3A library the capture behaviour is not very good. To take a good
+   picture, you need tune ISP parameters by IOCTL functions or use a 3A library
+   such as libxcam.
+
+3. The driver is intended to drive the PCI exposed versions of the device.
+   It will not detect those devices enumerated via ACPI as a field of the
+   i915 GPU driver.
+
+   There are some patches adding i915 GPU support floating at the Yocto's
+   Aero repository (so far, untested upstream).
+
+4. The driver supports only v2 of the IPU/Camera. It will not work with the
+   versions of the hardware in other SoCs.
diff --git a/drivers/staging/media/atomisp/i2c/Kconfig b/drivers/staging/media/atomisp/i2c/Kconfig
new file mode 100644
index 000000000..e726101b2
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/Kconfig
@@ -0,0 +1,97 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Kconfig for sensor drivers
+#
+
+config VIDEO_ATOMISP_OV2722
+	tristate "OVT ov2722 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the OVT
+	  OV2722 raw camera.
+
+	  OVT is a 2M raw sensor.
+
+	  It currently only works with the atomisp driver.
+
+config VIDEO_ATOMISP_GC2235
+	tristate "Galaxy gc2235 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the OVT
+	  GC2235 raw camera.
+
+	  GC2235 is a 2M raw sensor.
+
+	  It currently only works with the atomisp driver.
+
+config VIDEO_ATOMISP_MSRLIST_HELPER
+	tristate "Helper library to load, parse and apply large register lists."
+	depends on I2C
+	help
+	  This is a helper library to be used from a sensor driver to load, parse
+	  and apply large register lists.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called libmsrlisthelper.
+
+config VIDEO_ATOMISP_MT9M114
+	tristate "Aptina mt9m114 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the Micron
+	  mt9m114 1.3 Mpixel camera.
+
+	  mt9m114 is video camera sensor.
+
+	  It currently only works with the atomisp driver.
+
+config VIDEO_ATOMISP_GC0310
+	tristate "GC0310 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the Galaxycore
+	  GC0310 0.3MP sensor.
+
+config VIDEO_ATOMISP_OV2680
+	tristate "Omnivision OV2680 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the Omnivision
+	  OV2680 raw camera.
+
+	  ov2680 is a 2M raw sensor.
+
+	  It currently only works with the atomisp driver.
+
+config VIDEO_ATOMISP_OV5693
+	tristate "Omnivision ov5693 sensor support"
+	depends on ACPI
+	depends on I2C && VIDEO_DEV
+	help
+	  This is a Video4Linux2 sensor-level driver for the Micron
+	  ov5693 5 Mpixel camera.
+
+	  ov5693 is video camera sensor.
+
+	  It currently only works with the atomisp driver.
+
+#
+# Kconfig for flash drivers
+#
+
+config VIDEO_ATOMISP_LM3554
+       tristate "LM3554 flash light driver"
+	depends on ACPI
+       depends on VIDEO_DEV && I2C
+	help
+	 This is a Video4Linux2 sub-dev driver for the LM3554
+	 flash light driver.
+
+	 To compile this driver as a module, choose M here: the
+	 module will be called lm3554
diff --git a/drivers/staging/media/atomisp/i2c/Makefile b/drivers/staging/media/atomisp/i2c/Makefile
new file mode 100644
index 000000000..8d022986e
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/Makefile
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for sensor drivers
+#
+
+obj-$(CONFIG_VIDEO_ATOMISP_OV5693)     += ov5693/
+obj-$(CONFIG_VIDEO_ATOMISP_MT9M114)    += atomisp-mt9m114.o
+obj-$(CONFIG_VIDEO_ATOMISP_GC2235)     += atomisp-gc2235.o
+obj-$(CONFIG_VIDEO_ATOMISP_OV2722)     += atomisp-ov2722.o
+obj-$(CONFIG_VIDEO_ATOMISP_OV2680)     += atomisp-ov2680.o
+obj-$(CONFIG_VIDEO_ATOMISP_GC0310)     += atomisp-gc0310.o
+
+obj-$(CONFIG_VIDEO_ATOMISP_MSRLIST_HELPER) += atomisp-libmsrlisthelper.o
+
+# Makefile for flash drivers
+#
+
+obj-$(CONFIG_VIDEO_ATOMISP_LM3554) += atomisp-lm3554.o
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-gc0310.c b/drivers/staging/media/atomisp/i2c/atomisp-gc0310.c
new file mode 100644
index 000000000..783f1b88e
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-gc0310.c
@@ -0,0 +1,1299 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for GalaxyCore GC0310 VGA camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/moduleparam.h>
+#include <media/v4l2-device.h>
+#include <linux/io.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+
+#include "gc0310.h"
+
+/* i2c read/write stuff */
+static int gc0310_read_reg(struct i2c_client *client,
+			   u16 data_length, u8 reg, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[1];
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no client->adapter\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	if (data_length != GC0310_8BIT) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(msg, 0, sizeof(msg));
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = I2C_MSG_LENGTH;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8)(reg & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].len = data_length;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		dev_err(&client->dev,
+			"read from offset 0x%x error %d", reg, err);
+		return err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	if (data_length == GC0310_8BIT)
+		*val = (u8)data[0];
+
+	return 0;
+}
+
+static int gc0310_i2c_write(struct i2c_client *client, u16 len, u8 *data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+static int gc0310_write_reg(struct i2c_client *client, u16 data_length,
+			    u8 reg, u8 val)
+{
+	int ret;
+	unsigned char data[2] = {0};
+	u8 *wreg = (u8 *)data;
+	const u16 len = data_length + sizeof(u8); /* 8-bit address + data */
+
+	if (data_length != GC0310_8BIT) {
+		dev_err(&client->dev,
+			"%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	*wreg = (u8)(reg & 0xff);
+
+	if (data_length == GC0310_8BIT)
+		data[1] = (u8)(val);
+
+	ret = gc0310_i2c_write(client, len, data);
+	if (ret)
+		dev_err(&client->dev,
+			"write error: wrote 0x%x to offset 0x%x error %d",
+			val, reg, ret);
+
+	return ret;
+}
+
+/*
+ * gc0310_write_reg_array - Initializes a list of GC0310 registers
+ * @client: i2c driver client structure
+ * @reglist: list of registers to be written
+ *
+ * This function initializes a list of registers. When consecutive addresses
+ * are found in a row on the list, this function creates a buffer and sends
+ * consecutive data in a single i2c_transfer().
+ *
+ * __gc0310_flush_reg_array, __gc0310_buf_reg_array() and
+ * __gc0310_write_reg_is_consecutive() are internal functions to
+ * gc0310_write_reg_array_fast() and should be not used anywhere else.
+ *
+ */
+
+static int __gc0310_flush_reg_array(struct i2c_client *client,
+				    struct gc0310_write_ctrl *ctrl)
+{
+	u16 size;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u8) + ctrl->index; /* 8-bit address + data */
+	ctrl->buffer.addr = (u8)(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return gc0310_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __gc0310_buf_reg_array(struct i2c_client *client,
+				  struct gc0310_write_ctrl *ctrl,
+				  const struct gc0310_reg *next)
+{
+	int size;
+
+	switch (next->type) {
+	case GC0310_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* When first item is added, we need to store its starting address */
+	if (ctrl->index == 0)
+		ctrl->buffer.addr = next->reg;
+
+	ctrl->index += size;
+
+	/*
+	 * Buffer cannot guarantee free space for u32? Better flush it to avoid
+	 * possible lack of memory for next item.
+	 */
+	if (ctrl->index + sizeof(u8) >= GC0310_MAX_WRITE_BUF_SIZE)
+		return __gc0310_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int __gc0310_write_reg_is_consecutive(struct i2c_client *client,
+					     struct gc0310_write_ctrl *ctrl,
+					     const struct gc0310_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+static int gc0310_write_reg_array(struct i2c_client *client,
+				  const struct gc0310_reg *reglist)
+{
+	const struct gc0310_reg *next = reglist;
+	struct gc0310_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != GC0310_TOK_TERM; next++) {
+		switch (next->type & GC0310_TOK_MASK) {
+		case GC0310_TOK_DELAY:
+			err = __gc0310_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			msleep(next->val);
+			break;
+		default:
+			/*
+			 * If next address is not consecutive, data needs to be
+			 * flushed before proceed.
+			 */
+			if (!__gc0310_write_reg_is_consecutive(client, &ctrl,
+							       next)) {
+				err = __gc0310_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __gc0310_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev, "%s: write error, aborted\n",
+					__func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __gc0310_flush_reg_array(client, &ctrl);
+}
+
+static int gc0310_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (GC0310_FOCAL_LENGTH_NUM << 16) | GC0310_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int gc0310_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/*const f number for imx*/
+	*val = (GC0310_F_NUMBER_DEFAULT_NUM << 16) | GC0310_F_NUMBER_DEM;
+	return 0;
+}
+
+static int gc0310_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (GC0310_F_NUMBER_DEFAULT_NUM << 24) |
+	       (GC0310_F_NUMBER_DEM << 16) |
+	       (GC0310_F_NUMBER_DEFAULT_NUM << 8) | GC0310_F_NUMBER_DEM;
+	return 0;
+}
+
+static int gc0310_g_bin_factor_x(struct v4l2_subdev *sd, s32 *val)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	*val = dev->res->bin_factor_x;
+
+	return 0;
+}
+
+static int gc0310_g_bin_factor_y(struct v4l2_subdev *sd, s32 *val)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	*val = dev->res->bin_factor_y;
+
+	return 0;
+}
+
+static int gc0310_get_intg_factor(struct i2c_client *client,
+				  struct camera_mipi_info *info,
+				  const struct gc0310_resolution *res)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	u16 val;
+	u8 reg_val;
+	int ret;
+	unsigned int hori_blanking;
+	unsigned int vert_blanking;
+	unsigned int sh_delay;
+
+	if (!info)
+		return -EINVAL;
+
+	/* pixel clock calculattion */
+	dev->vt_pix_clk_freq_mhz = 14400000; // 16.8MHz
+	buf->vt_pix_clk_freq_mhz = dev->vt_pix_clk_freq_mhz;
+	dev_dbg(&client->dev, "vt_pix_clk_freq_mhz=%d\n", buf->vt_pix_clk_freq_mhz);
+
+	/* get integration time */
+	buf->coarse_integration_time_min = GC0310_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    GC0310_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = GC0310_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    GC0310_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = GC0310_FINE_INTG_TIME_MIN;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	/* Getting crop_horizontal_start */
+	ret =  gc0310_read_reg(client, GC0310_8BIT,
+			       GC0310_H_CROP_START_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret =  gc0310_read_reg(client, GC0310_8BIT,
+			       GC0310_H_CROP_START_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = val | (reg_val & 0xFF);
+	dev_dbg(&client->dev, "crop_horizontal_start=%d\n", buf->crop_horizontal_start);
+
+	/* Getting crop_vertical_start */
+	ret =  gc0310_read_reg(client, GC0310_8BIT,
+			       GC0310_V_CROP_START_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret =  gc0310_read_reg(client, GC0310_8BIT,
+			       GC0310_V_CROP_START_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = val | (reg_val & 0xFF);
+	dev_dbg(&client->dev, "crop_vertical_start=%d\n", buf->crop_vertical_start);
+
+	/* Getting output_width */
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_H_OUTSIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_H_OUTSIZE_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = val | (reg_val & 0xFF);
+	dev_dbg(&client->dev, "output_width=%d\n", buf->output_width);
+
+	/* Getting output_height */
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_V_OUTSIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_V_OUTSIZE_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = val | (reg_val & 0xFF);
+	dev_dbg(&client->dev, "output_height=%d\n", buf->output_height);
+
+	buf->crop_horizontal_end = buf->crop_horizontal_start + buf->output_width - 1;
+	buf->crop_vertical_end = buf->crop_vertical_start + buf->output_height - 1;
+	dev_dbg(&client->dev, "crop_horizontal_end=%d\n", buf->crop_horizontal_end);
+	dev_dbg(&client->dev, "crop_vertical_end=%d\n", buf->crop_vertical_end);
+
+	/* Getting line_length_pck */
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_H_BLANKING_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_H_BLANKING_L, &reg_val);
+	if (ret)
+		return ret;
+	hori_blanking = val | (reg_val & 0xFF);
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_SH_DELAY, &reg_val);
+	if (ret)
+		return ret;
+	sh_delay = reg_val;
+	buf->line_length_pck = buf->output_width + hori_blanking + sh_delay + 4;
+	dev_dbg(&client->dev, "hori_blanking=%d sh_delay=%d line_length_pck=%d\n", hori_blanking,
+		sh_delay, buf->line_length_pck);
+
+	/* Getting frame_length_lines */
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_V_BLANKING_H, &reg_val);
+	if (ret)
+		return ret;
+	val = (reg_val & 0xFF) << 8;
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_V_BLANKING_L, &reg_val);
+	if (ret)
+		return ret;
+	vert_blanking = val | (reg_val & 0xFF);
+	buf->frame_length_lines = buf->output_height + vert_blanking;
+	dev_dbg(&client->dev, "vert_blanking=%d frame_length_lines=%d\n", vert_blanking,
+		buf->frame_length_lines);
+
+	buf->binning_factor_x = res->bin_factor_x ?
+				res->bin_factor_x : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				res->bin_factor_y : 1;
+	return 0;
+}
+
+static int gc0310_set_gain(struct v4l2_subdev *sd, int gain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 again, dgain;
+
+	if (gain < 0x20)
+		gain = 0x20;
+	if (gain > 0x80)
+		gain = 0x80;
+
+	if (gain >= 0x20 && gain < 0x40) {
+		again = 0x0; /* sqrt(2) */
+		dgain = gain;
+	} else {
+		again = 0x2; /* 2 * sqrt(2) */
+		dgain = gain / 2;
+	}
+
+	dev_dbg(&client->dev, "gain=0x%x again=0x%x dgain=0x%x\n", gain, again, dgain);
+
+	/* set analog gain */
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_AGC_ADJ, again);
+	if (ret)
+		return ret;
+
+	/* set digital gain */
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_DGC_ADJ, dgain);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __gc0310_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
+				 int gain, int digitgain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	dev_dbg(&client->dev, "coarse_itg=%d gain=%d digitgain=%d\n", coarse_itg, gain, digitgain);
+
+	/* set exposure */
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_AEC_PK_EXPO_L,
+			       coarse_itg & 0xff);
+	if (ret)
+		return ret;
+
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_AEC_PK_EXPO_H,
+			       (coarse_itg >> 8) & 0x0f);
+	if (ret)
+		return ret;
+
+	ret = gc0310_set_gain(sd, gain);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+
+static int gc0310_set_exposure(struct v4l2_subdev *sd, int exposure,
+			       int gain, int digitgain)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __gc0310_set_exposure(sd, exposure, gain, digitgain);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long gc0310_s_exposure(struct v4l2_subdev *sd,
+			      struct atomisp_exposure *exposure)
+{
+	int exp = exposure->integration_time[0];
+	int gain = exposure->gain[0];
+	int digitgain = exposure->gain[1];
+
+	/* we should not accept the invalid value below. */
+	if (gain == 0) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		v4l2_err(client, "%s: invalid value\n", __func__);
+		return -EINVAL;
+	}
+
+	return gc0310_set_exposure(sd, exp, gain, digitgain);
+}
+
+/* TO DO */
+static int gc0310_v_flip(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+/* TO DO */
+static int gc0310_h_flip(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+static long gc0310_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return gc0310_s_exposure(sd, arg);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int gc0310_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 reg_v;
+	int ret;
+
+	/* get exposure */
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_AEC_PK_EXPO_L,
+			      &reg_v);
+	if (ret)
+		goto err;
+
+	*value = reg_v;
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_AEC_PK_EXPO_H,
+			      &reg_v);
+	if (ret)
+		goto err;
+
+	*value = *value + (reg_v << 8);
+err:
+	return ret;
+}
+
+static int gc0310_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gc0310_device *dev =
+	    container_of(ctrl->handler, struct gc0310_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		dev_dbg(&client->dev, "%s: CID_VFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = gc0310_v_flip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&client->dev, "%s: CID_HFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = gc0310_h_flip(&dev->sd, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int gc0310_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gc0310_device *dev =
+	    container_of(ctrl->handler, struct gc0310_device, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = gc0310_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = gc0310_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = gc0310_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = gc0310_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		ret = gc0310_g_bin_factor_x(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_VERT:
+		ret = gc0310_g_bin_factor_y(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = gc0310_s_ctrl,
+	.g_volatile_ctrl = gc0310_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config gc0310_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.min = 0x0,
+		.max = 0xffff,
+		.step = 0x01,
+		.def = 0x00,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VFLIP,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Flip",
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HFLIP,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Mirror",
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = GC0310_FOCAL_LENGTH_DEFAULT,
+		.max = GC0310_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = GC0310_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = GC0310_F_NUMBER_DEFAULT,
+		.max = GC0310_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = GC0310_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = GC0310_F_NUMBER_RANGE,
+		.max = GC0310_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = GC0310_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "horizontal binning factor",
+		.min = 0,
+		.max = GC0310_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vertical binning factor",
+		.min = 0,
+		.max = GC0310_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+};
+
+static int gc0310_init(struct v4l2_subdev *sd)
+{
+	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+
+	/* set initial registers */
+	ret  = gc0310_write_reg_array(client, gc0310_reset_register);
+
+	/* restore settings */
+	gc0310_res = gc0310_res_preview;
+	N_RES = N_RES_PREVIEW;
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = 0;
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	if (flag) {
+		/* The upstream module driver (written to Crystal
+		 * Cove) had this logic to pulse the rails low first.
+		 * This appears to break things on the MRD7 with the
+		 * X-Powers PMIC...
+		 *
+		 *     ret = dev->platform_data->v1p8_ctrl(sd, 0);
+		 *     ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+		 *     mdelay(50);
+		 */
+		ret |= dev->platform_data->v1p8_ctrl(sd, 1);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+	}
+
+	if (!flag || ret) {
+		ret |= dev->platform_data->v1p8_ctrl(sd, 0);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret;
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* GPIO0 == "reset" (active low), GPIO1 == "power down" */
+	if (flag) {
+		/* Pulse reset, then release power down */
+		ret = dev->platform_data->gpio0_ctrl(sd, 0);
+		usleep_range(5000, 10000);
+		ret |= dev->platform_data->gpio0_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+		ret |= dev->platform_data->gpio1_ctrl(sd, 0);
+		usleep_range(10000, 15000);
+	} else {
+		ret = dev->platform_data->gpio1_ctrl(sd, 1);
+		ret |= dev->platform_data->gpio0_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd);
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* flis clock control */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret) {
+		ret = gpio_ctrl(sd, 1);
+		if (ret)
+			goto fail_gpio;
+	}
+
+	msleep(100);
+
+	return 0;
+
+fail_gpio:
+	dev->platform_data->flisclk_ctrl(sd, 0);
+fail_clk:
+	power_ctrl(sd, 0);
+fail_power:
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			dev_err(&client->dev, "gpio failed 2\n");
+	}
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	return ret;
+}
+
+static int gc0310_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	if (on == 0)
+		return power_down(sd);
+
+	ret = power_up(sd);
+	if (ret)
+		return ret;
+
+	return gc0310_init(sd);
+}
+
+/* TODO: remove it. */
+static int startup(struct v4l2_subdev *sd)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	ret = gc0310_write_reg_array(client, dev->res->regs);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 write register err.\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int gc0310_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *gc0310_info = NULL;
+	struct gc0310_resolution *res;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	gc0310_info = v4l2_get_subdev_hostdata(sd);
+	if (!gc0310_info)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+
+	res = v4l2_find_nearest_size(gc0310_res_preview,
+				     ARRAY_SIZE(gc0310_res_preview), width,
+				     height, fmt->width, fmt->height);
+	if (!res)
+		res = &gc0310_res_preview[N_RES - 1];
+
+	fmt->width = res->width;
+	fmt->height = res->height;
+	dev->res = res;
+
+	fmt->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+	}
+
+	dev_dbg(&client->dev, "%s: before gc0310_write_reg_array %s\n",
+		__func__, dev->res->desc);
+	ret = startup(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 startup err\n");
+		goto err;
+	}
+
+	ret = gc0310_get_intg_factor(client, gc0310_info, dev->res);
+	if (ret) {
+		dev_err(&client->dev, "failed to get integration_factor\n");
+		goto err;
+	}
+
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc0310_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	fmt->width = dev->res->width;
+	fmt->height = dev->res->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+
+	return 0;
+}
+
+static int gc0310_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u8 high, low;
+	int ret;
+	u16 id;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_SC_CMMN_CHIP_ID_H, &high);
+	if (ret) {
+		dev_err(&client->dev, "read sensor_id_high failed\n");
+		return -ENODEV;
+	}
+	ret = gc0310_read_reg(client, GC0310_8BIT,
+			      GC0310_SC_CMMN_CHIP_ID_L, &low);
+	if (ret) {
+		dev_err(&client->dev, "read sensor_id_low failed\n");
+		return -ENODEV;
+	}
+	id = ((((u16)high) << 8) | (u16)low);
+	dev_dbg(&client->dev, "sensor ID = 0x%x\n", id);
+
+	if (id != GC0310_ID) {
+		dev_err(&client->dev, "sensor ID error, read id = 0x%x, target id = 0x%x\n", id,
+			GC0310_ID);
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "detect gc0310 success\n");
+
+	return 0;
+}
+
+static int gc0310_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	dev_dbg(&client->dev, "%s S enable=%d\n", __func__, enable);
+	mutex_lock(&dev->input_lock);
+
+	if (enable) {
+		/* enable per frame MIPI and sensor ctrl reset  */
+		ret = gc0310_write_reg(client, GC0310_8BIT,
+				       0xFE, 0x30);
+		if (ret) {
+			mutex_unlock(&dev->input_lock);
+			return ret;
+		}
+	}
+
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_RESET_RELATED, GC0310_REGISTER_PAGE_3);
+	if (ret) {
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	ret = gc0310_write_reg(client, GC0310_8BIT, GC0310_SW_STREAM,
+			       enable ? GC0310_START_STREAMING :
+			       GC0310_STOP_STREAMING);
+	if (ret) {
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	ret = gc0310_write_reg(client, GC0310_8BIT,
+			       GC0310_RESET_RELATED, GC0310_REGISTER_PAGE_0);
+	if (ret) {
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc0310_s_config(struct v4l2_subdev *sd,
+			   int irq, void *platform_data)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+	/* power off the module, then power on it in future
+	 * as first power on by board may not fulfill the
+	 * power on sequqence needed by the module
+	 */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 power-off err.\n");
+		goto fail_power_off;
+	}
+
+	ret = power_up(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 power-up err.\n");
+		goto fail_power_on;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = gc0310_detect(client);
+	if (ret) {
+		dev_err(&client->dev, "gc0310_detect err s_config.\n");
+		goto fail_csi_cfg;
+	}
+
+	/* turn off sensor, after probed */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 power-off err.\n");
+		goto fail_csi_cfg;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_power_on:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+fail_power_off:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc0310_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = dev->res->fps;
+
+	return 0;
+}
+
+static int gc0310_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	return 0;
+}
+
+static int gc0310_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = gc0310_res[index].width;
+	fse->min_height = gc0310_res[index].height;
+	fse->max_width = gc0310_res[index].width;
+	fse->max_height = gc0310_res[index].height;
+
+	return 0;
+}
+
+static int gc0310_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->res->skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_sensor_ops gc0310_sensor_ops = {
+	.g_skip_frames	= gc0310_g_skip_frames,
+};
+
+static const struct v4l2_subdev_video_ops gc0310_video_ops = {
+	.s_stream = gc0310_s_stream,
+	.g_frame_interval = gc0310_g_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops gc0310_core_ops = {
+	.s_power = gc0310_s_power,
+	.ioctl = gc0310_ioctl,
+};
+
+static const struct v4l2_subdev_pad_ops gc0310_pad_ops = {
+	.enum_mbus_code = gc0310_enum_mbus_code,
+	.enum_frame_size = gc0310_enum_frame_size,
+	.get_fmt = gc0310_get_fmt,
+	.set_fmt = gc0310_set_fmt,
+};
+
+static const struct v4l2_subdev_ops gc0310_ops = {
+	.core = &gc0310_core_ops,
+	.video = &gc0310_video_ops,
+	.pad = &gc0310_pad_ops,
+	.sensor = &gc0310_sensor_ops,
+};
+
+static void gc0310_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+
+	dev_dbg(&client->dev, "gc0310_remove...\n");
+
+	dev->platform_data->csi_cfg(sd, 0);
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+}
+
+static int gc0310_probe(struct i2c_client *client)
+{
+	struct gc0310_device *dev;
+	int ret;
+	void *pdata;
+	unsigned int i;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	mutex_init(&dev->input_lock);
+
+	dev->res = &gc0310_res_preview[0];
+	v4l2_i2c_subdev_init(&dev->sd, client, &gc0310_ops);
+
+	pdata = gmin_camera_platform_data(&dev->sd,
+					  ATOMISP_INPUT_FORMAT_RAW_8,
+					  atomisp_bayer_order_grbg);
+	if (!pdata) {
+		ret = -EINVAL;
+		goto out_free;
+	}
+
+	ret = gc0310_s_config(&dev->sd, client->irq, pdata);
+	if (ret)
+		goto out_free;
+
+	ret = atomisp_register_i2c_module(&dev->sd, pdata, RAW_CAMERA);
+	if (ret)
+		goto out_free;
+
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->format.code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret =
+	    v4l2_ctrl_handler_init(&dev->ctrl_handler,
+				   ARRAY_SIZE(gc0310_controls));
+	if (ret) {
+		gc0310_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(gc0310_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &gc0310_controls[i],
+				     NULL);
+
+	if (dev->ctrl_handler.error) {
+		gc0310_remove(client);
+		return dev->ctrl_handler.error;
+	}
+
+	/* Use same lock for controls as for everything else. */
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = &dev->ctrl_handler;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret)
+		gc0310_remove(client);
+
+	return ret;
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static const struct acpi_device_id gc0310_acpi_match[] = {
+	{"XXGC0310"},
+	{"INT0310"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, gc0310_acpi_match);
+
+static struct i2c_driver gc0310_driver = {
+	.driver = {
+		.name = "gc0310",
+		.acpi_match_table = gc0310_acpi_match,
+	},
+	.probe_new = gc0310_probe,
+	.remove = gc0310_remove,
+};
+module_i2c_driver(gc0310_driver);
+
+MODULE_AUTHOR("Lai, Angie <angie.lai@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for GalaxyCore GC0310 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-gc2235.c b/drivers/staging/media/atomisp/i2c/atomisp-gc2235.c
new file mode 100644
index 000000000..4d5a7e335
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-gc2235.c
@@ -0,0 +1,1050 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for GalaxyCore GC2235 2M camera sensor.
+ *
+ * Copyright (c) 2014 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/moduleparam.h>
+#include <media/v4l2-device.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+#include <linux/acpi.h>
+#include <linux/io.h>
+
+#include "gc2235.h"
+
+/* i2c read/write stuff */
+static int gc2235_read_reg(struct i2c_client *client,
+			   u16 data_length, u16 reg, u16 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[6];
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no client->adapter\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	if (data_length != GC2235_8BIT) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(msg, 0, sizeof(msg));
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8)(reg & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].len = data_length;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		dev_err(&client->dev,
+			"read from offset 0x%x error %d", reg, err);
+		return err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	if (data_length == GC2235_8BIT)
+		*val = (u8)data[0];
+
+	return 0;
+}
+
+static int gc2235_i2c_write(struct i2c_client *client, u16 len, u8 *data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+static int gc2235_write_reg(struct i2c_client *client, u16 data_length,
+			    u8 reg, u8 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	const u16 len = data_length + sizeof(u8); /* 16-bit address + data */
+
+	if (data_length != GC2235_8BIT) {
+		dev_err(&client->dev,
+			"%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	data[0] = reg;
+	data[1] = val;
+
+	ret = gc2235_i2c_write(client, len, data);
+	if (ret)
+		dev_err(&client->dev,
+			"write error: wrote 0x%x to offset 0x%x error %d",
+			val, reg, ret);
+
+	return ret;
+}
+
+static int __gc2235_flush_reg_array(struct i2c_client *client,
+				    struct gc2235_write_ctrl *ctrl)
+{
+	u16 size;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u8) + ctrl->index; /* 8-bit address + data */
+	ctrl->index = 0;
+
+	return gc2235_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __gc2235_buf_reg_array(struct i2c_client *client,
+				  struct gc2235_write_ctrl *ctrl,
+				  const struct gc2235_reg *next)
+{
+	int size;
+
+	if (next->type != GC2235_8BIT)
+		return -EINVAL;
+
+	size = 1;
+	ctrl->buffer.data[ctrl->index] = (u8)next->val;
+
+	/* When first item is added, we need to store its starting address */
+	if (ctrl->index == 0)
+		ctrl->buffer.addr = next->reg;
+
+	ctrl->index += size;
+
+	/*
+	 * Buffer cannot guarantee free space for u32? Better flush it to avoid
+	 * possible lack of memory for next item.
+	 */
+	if (ctrl->index + sizeof(u8) >= GC2235_MAX_WRITE_BUF_SIZE)
+		return __gc2235_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int __gc2235_write_reg_is_consecutive(struct i2c_client *client,
+					     struct gc2235_write_ctrl *ctrl,
+					     const struct gc2235_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+static int gc2235_write_reg_array(struct i2c_client *client,
+				  const struct gc2235_reg *reglist)
+{
+	const struct gc2235_reg *next = reglist;
+	struct gc2235_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != GC2235_TOK_TERM; next++) {
+		switch (next->type & GC2235_TOK_MASK) {
+		case GC2235_TOK_DELAY:
+			err = __gc2235_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			msleep(next->val);
+			break;
+		default:
+			/*
+			 * If next address is not consecutive, data needs to be
+			 * flushed before proceed.
+			 */
+			if (!__gc2235_write_reg_is_consecutive(client, &ctrl,
+							       next)) {
+				err = __gc2235_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __gc2235_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev, "%s: write error, aborted\n",
+					__func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __gc2235_flush_reg_array(client, &ctrl);
+}
+
+static int gc2235_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (GC2235_FOCAL_LENGTH_NUM << 16) | GC2235_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int gc2235_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/* const f number for imx */
+	*val = (GC2235_F_NUMBER_DEFAULT_NUM << 16) | GC2235_F_NUMBER_DEM;
+	return 0;
+}
+
+static int gc2235_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (GC2235_F_NUMBER_DEFAULT_NUM << 24) |
+	       (GC2235_F_NUMBER_DEM << 16) |
+	       (GC2235_F_NUMBER_DEFAULT_NUM << 8) | GC2235_F_NUMBER_DEM;
+	return 0;
+}
+
+static int gc2235_get_intg_factor(struct i2c_client *client,
+				  struct camera_mipi_info *info,
+				  const struct gc2235_resolution *res)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	u16 reg_val, reg_val_h;
+	int ret;
+
+	if (!info)
+		return -EINVAL;
+
+	/* pixel clock calculattion */
+	buf->vt_pix_clk_freq_mhz = dev->vt_pix_clk_freq_mhz = 30000000;
+
+	/* get integration time */
+	buf->coarse_integration_time_min = GC2235_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    GC2235_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = GC2235_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    GC2235_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = GC2235_FINE_INTG_TIME_MIN;
+	buf->frame_length_lines = res->lines_per_frame;
+	buf->line_length_pck = res->pixels_per_line;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	ret =  gc2235_read_reg(client, GC2235_8BIT,
+			       GC2235_H_CROP_START_H, &reg_val_h);
+	ret =  gc2235_read_reg(client, GC2235_8BIT,
+			       GC2235_H_CROP_START_L, &reg_val);
+	if (ret)
+		return ret;
+
+	buf->crop_horizontal_start = (reg_val_h << 8) | reg_val;
+
+	ret =  gc2235_read_reg(client, GC2235_8BIT,
+			       GC2235_V_CROP_START_H, &reg_val_h);
+	ret =  gc2235_read_reg(client, GC2235_8BIT,
+			       GC2235_V_CROP_START_L, &reg_val);
+	if (ret)
+		return ret;
+
+	buf->crop_vertical_start = (reg_val_h << 8) | reg_val;
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_H_OUTSIZE_H, &reg_val_h);
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_H_OUTSIZE_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = (reg_val_h << 8) | reg_val;
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_V_OUTSIZE_H, &reg_val_h);
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_V_OUTSIZE_L, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = (reg_val_h << 8) | reg_val;
+
+	buf->crop_horizontal_end = buf->crop_horizontal_start +
+				   buf->output_width - 1;
+	buf->crop_vertical_end = buf->crop_vertical_start +
+				 buf->output_height - 1;
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_HB_H, &reg_val_h);
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_HB_L, &reg_val);
+	if (ret)
+		return ret;
+
+#if 0
+	u16 dummy = (reg_val_h << 8) | reg_val;
+#endif
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_SH_DELAY_H, &reg_val_h);
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_SH_DELAY_L, &reg_val);
+
+#if 0
+	buf->line_length_pck = buf->output_width + 16 + dummy +
+			       (((u16)reg_val_h << 8) | (u16)reg_val) + 4;
+#endif
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_VB_H, &reg_val_h);
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_VB_L, &reg_val);
+	if (ret)
+		return ret;
+
+#if 0
+	buf->frame_length_lines = buf->output_height + 32 +
+				  (((u16)reg_val_h << 8) | (u16)reg_val);
+#endif
+	buf->binning_factor_x = res->bin_factor_x ?
+				res->bin_factor_x : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				res->bin_factor_y : 1;
+	return 0;
+}
+
+static long __gc2235_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
+				  int gain, int digitgain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 coarse_integration = (u16)coarse_itg;
+	int ret = 0;
+	u16 expo_coarse_h, expo_coarse_l, gain_val = 0xF0, gain_val2 = 0xF0;
+
+	expo_coarse_h = coarse_integration >> 8;
+	expo_coarse_l = coarse_integration & 0xff;
+
+	ret = gc2235_write_reg(client, GC2235_8BIT,
+			       GC2235_EXPOSURE_H, expo_coarse_h);
+	ret = gc2235_write_reg(client, GC2235_8BIT,
+			       GC2235_EXPOSURE_L, expo_coarse_l);
+
+	if (gain <= 0x58) {
+		gain_val = 0x40;
+		gain_val2 = 0x58;
+	} else if (gain < 256) {
+		gain_val = 0x40;
+		gain_val2 = gain;
+	} else {
+		gain_val2 = 64 * gain / 256;
+		gain_val = 0xff;
+	}
+
+	ret = gc2235_write_reg(client, GC2235_8BIT,
+			       GC2235_GLOBAL_GAIN, (u8)gain_val);
+	ret = gc2235_write_reg(client, GC2235_8BIT,
+			       GC2235_PRE_GAIN, (u8)gain_val2);
+
+	return ret;
+}
+
+static int gc2235_set_exposure(struct v4l2_subdev *sd, int exposure,
+			       int gain, int digitgain)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __gc2235_set_exposure(sd, exposure, gain, digitgain);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long gc2235_s_exposure(struct v4l2_subdev *sd,
+			      struct atomisp_exposure *exposure)
+{
+	int exp = exposure->integration_time[0];
+	int gain = exposure->gain[0];
+	int digitgain = exposure->gain[1];
+
+	/* we should not accept the invalid value below. */
+	if (gain == 0) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		v4l2_err(client, "%s: invalid value\n", __func__);
+		return -EINVAL;
+	}
+
+	return gc2235_set_exposure(sd, exp, gain, digitgain);
+}
+
+static long gc2235_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return gc2235_s_exposure(sd, arg);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int gc2235_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 reg_v, reg_v2;
+	int ret;
+
+	/* get exposure */
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_EXPOSURE_L,
+			      &reg_v);
+	if (ret)
+		goto err;
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+			      GC2235_EXPOSURE_H,
+			      &reg_v2);
+	if (ret)
+		goto err;
+
+	reg_v += reg_v2 << 8;
+
+	*value = reg_v;
+err:
+	return ret;
+}
+
+static int gc2235_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gc2235_device *dev =
+	    container_of(ctrl->handler, struct gc2235_device, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = gc2235_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = gc2235_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = gc2235_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = gc2235_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.g_volatile_ctrl = gc2235_g_volatile_ctrl
+};
+
+static struct v4l2_ctrl_config gc2235_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.min = 0x0,
+		.max = 0xffff,
+		.step = 0x01,
+		.def = 0x00,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = GC2235_FOCAL_LENGTH_DEFAULT,
+		.max = GC2235_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = GC2235_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = GC2235_F_NUMBER_DEFAULT,
+		.max = GC2235_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = GC2235_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = GC2235_F_NUMBER_RANGE,
+		.max = GC2235_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = GC2235_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+};
+
+static int __gc2235_init(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* restore settings */
+	gc2235_res = gc2235_res_preview;
+	N_RES = N_RES_PREVIEW;
+
+	return gc2235_write_reg_array(client, gc2235_init_settings);
+}
+
+static int is_init;
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = -1;
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	if (flag) {
+		ret = dev->platform_data->v1p8_ctrl(sd, 1);
+		usleep_range(60, 90);
+		if (ret == 0)
+			ret |= dev->platform_data->v2p8_ctrl(sd, 1);
+	} else {
+		ret = dev->platform_data->v1p8_ctrl(sd, 0);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	int ret;
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	ret = dev->platform_data->gpio1_ctrl(sd, !flag);
+	usleep_range(60, 90);
+	ret |= dev->platform_data->gpio0_ctrl(sd, flag);
+
+	return ret;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* according to DS, at least 5ms is needed between DOVDD and PWDN */
+	usleep_range(5000, 6000);
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+	usleep_range(5000, 6000);
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret) {
+		ret = gpio_ctrl(sd, 1);
+		if (ret)
+			goto fail_power;
+	}
+
+	msleep(5);
+	return 0;
+
+fail_clk:
+	gpio_ctrl(sd, 0);
+fail_power:
+	power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			dev_err(&client->dev, "gpio failed 2\n");
+	}
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	return ret;
+}
+
+static int gc2235_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	if (on == 0) {
+		ret = power_down(sd);
+	} else {
+		ret = power_up(sd);
+		if (!ret)
+			ret = __gc2235_init(sd);
+		is_init = 1;
+	}
+	return ret;
+}
+
+static int startup(struct v4l2_subdev *sd)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (is_init == 0) {
+		/*
+		 * force gc2235 to do a reset in res change, otherwise it
+		 * can not output normal after switching res. and it is not
+		 * necessary for first time run up after power on, for the sack
+		 * of performance
+		 */
+		power_down(sd);
+		power_up(sd);
+		gc2235_write_reg_array(client, gc2235_init_settings);
+	}
+
+	ret = gc2235_write_reg_array(client, dev->res->regs);
+	if (ret) {
+		dev_err(&client->dev, "gc2235 write register err.\n");
+		return ret;
+	}
+	is_init = 0;
+
+	return ret;
+}
+
+static int gc2235_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *gc2235_info = NULL;
+	struct gc2235_resolution *res;
+	int ret = 0;
+
+	gc2235_info = v4l2_get_subdev_hostdata(sd);
+	if (!gc2235_info)
+		return -EINVAL;
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	res = v4l2_find_nearest_size(gc2235_res_preview,
+				     ARRAY_SIZE(gc2235_res_preview), width,
+				     height, fmt->width, fmt->height);
+	if (!res)
+		res = &gc2235_res_preview[N_RES - 1];
+
+	fmt->width = res->width;
+	fmt->height = res->height;
+	dev->res = res;
+
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+	}
+
+	ret = startup(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc2235 startup err\n");
+		goto err;
+	}
+
+	ret = gc2235_get_intg_factor(client, gc2235_info,
+				     dev->res);
+	if (ret)
+		dev_err(&client->dev, "failed to get integration_factor\n");
+
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc2235_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	fmt->width = dev->res->width;
+	fmt->height = dev->res->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int gc2235_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u16 high = 0, low = 0;
+	u16 id;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	gc2235_read_reg(client, GC2235_8BIT, GC2235_SENSOR_ID_H, &high);
+	gc2235_read_reg(client, GC2235_8BIT, GC2235_SENSOR_ID_L, &low);
+	id = ((high << 8) | low);
+
+	if (id != GC2235_ID) {
+		dev_err(&client->dev, "sensor ID error, 0x%x\n", id);
+		return -ENODEV;
+	}
+
+	dev_info(&client->dev, "detect gc2235 success\n");
+	return 0;
+}
+
+static int gc2235_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	if (enable)
+		ret = gc2235_write_reg_array(client, gc2235_stream_on);
+	else
+		ret = gc2235_write_reg_array(client, gc2235_stream_off);
+
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc2235_s_config(struct v4l2_subdev *sd,
+			   int irq, void *platform_data)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+	/*
+	 * power off the module, then power on it in future
+	 * as first power on by board may not fulfill the
+	 * power on sequqence needed by the module
+	 */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc2235 power-off err.\n");
+		goto fail_power_off;
+	}
+
+	ret = power_up(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc2235 power-up err.\n");
+		goto fail_power_on;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = gc2235_detect(client);
+	if (ret) {
+		dev_err(&client->dev, "gc2235_detect err s_config.\n");
+		goto fail_csi_cfg;
+	}
+
+	/* turn off sensor, after probed */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc2235 power-off err.\n");
+		goto fail_csi_cfg;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_power_on:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+fail_power_off:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc2235_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = dev->res->fps;
+
+	return 0;
+}
+
+static int gc2235_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int gc2235_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = gc2235_res[index].width;
+	fse->min_height = gc2235_res[index].height;
+	fse->max_width = gc2235_res[index].width;
+	fse->max_height = gc2235_res[index].height;
+
+	return 0;
+}
+
+static int gc2235_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->res->skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_sensor_ops gc2235_sensor_ops = {
+	.g_skip_frames	= gc2235_g_skip_frames,
+};
+
+static const struct v4l2_subdev_video_ops gc2235_video_ops = {
+	.s_stream = gc2235_s_stream,
+	.g_frame_interval = gc2235_g_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops gc2235_core_ops = {
+	.s_power = gc2235_s_power,
+	.ioctl = gc2235_ioctl,
+};
+
+static const struct v4l2_subdev_pad_ops gc2235_pad_ops = {
+	.enum_mbus_code = gc2235_enum_mbus_code,
+	.enum_frame_size = gc2235_enum_frame_size,
+	.get_fmt = gc2235_get_fmt,
+	.set_fmt = gc2235_set_fmt,
+};
+
+static const struct v4l2_subdev_ops gc2235_ops = {
+	.core = &gc2235_core_ops,
+	.video = &gc2235_video_ops,
+	.pad = &gc2235_pad_ops,
+	.sensor = &gc2235_sensor_ops,
+};
+
+static void gc2235_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+
+	dev_dbg(&client->dev, "gc2235_remove...\n");
+
+	dev->platform_data->csi_cfg(sd, 0);
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+}
+
+static int gc2235_probe(struct i2c_client *client)
+{
+	struct gc2235_device *dev;
+	void *gcpdev;
+	int ret;
+	unsigned int i;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	mutex_init(&dev->input_lock);
+
+	dev->res = &gc2235_res_preview[0];
+	v4l2_i2c_subdev_init(&dev->sd, client, &gc2235_ops);
+
+	gcpdev = gmin_camera_platform_data(&dev->sd,
+					   ATOMISP_INPUT_FORMAT_RAW_10,
+					   atomisp_bayer_order_grbg);
+
+	ret = gc2235_s_config(&dev->sd, client->irq, gcpdev);
+	if (ret)
+		goto out_free;
+
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret =
+	    v4l2_ctrl_handler_init(&dev->ctrl_handler,
+				   ARRAY_SIZE(gc2235_controls));
+	if (ret) {
+		gc2235_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(gc2235_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &gc2235_controls[i],
+				     NULL);
+
+	if (dev->ctrl_handler.error) {
+		gc2235_remove(client);
+		return dev->ctrl_handler.error;
+	}
+
+	/* Use same lock for controls as for everything else. */
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = &dev->ctrl_handler;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret)
+		gc2235_remove(client);
+
+	return atomisp_register_i2c_module(&dev->sd, gcpdev, RAW_CAMERA);
+
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+
+	return ret;
+}
+
+static const struct acpi_device_id gc2235_acpi_match[] = {
+	{ "INT33F8" },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, gc2235_acpi_match);
+
+static struct i2c_driver gc2235_driver = {
+	.driver = {
+		.name = "gc2235",
+		.acpi_match_table = gc2235_acpi_match,
+	},
+	.probe_new = gc2235_probe,
+	.remove = gc2235_remove,
+};
+module_i2c_driver(gc2235_driver);
+
+MODULE_AUTHOR("Shuguang Gong <Shuguang.Gong@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for GC2235 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-libmsrlisthelper.c b/drivers/staging/media/atomisp/i2c/atomisp-libmsrlisthelper.c
new file mode 100644
index 000000000..7a20d918a
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-libmsrlisthelper.c
@@ -0,0 +1,210 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/i2c.h>
+#include <linux/firmware.h>
+#include <linux/device.h>
+#include <linux/export.h>
+#include "../include/linux/libmsrlisthelper.h"
+#include <linux/module.h>
+#include <linux/slab.h>
+
+/* Tagged binary data container structure definitions. */
+struct tbd_header {
+	u32 tag;          /*!< Tag identifier, also checks endianness */
+	u32 size;         /*!< Container size including this header */
+	u32 version;      /*!< Version, format 0xYYMMDDVV */
+	u32 revision;     /*!< Revision, format 0xYYMMDDVV */
+	u32 config_bits;  /*!< Configuration flag bits set */
+	u32 checksum;     /*!< Global checksum, header included */
+} __packed;
+
+struct tbd_record_header {
+	u32 size;        /*!< Size of record including header */
+	u8 format_id;    /*!< tbd_format_t enumeration values used */
+	u8 packing_key;  /*!< Packing method; 0 = no packing */
+	u16 class_id;    /*!< tbd_class_t enumeration values used */
+} __packed;
+
+struct tbd_data_record_header {
+	u16 next_offset;
+	u16 flags;
+	u16 data_offset;
+	u16 data_size;
+} __packed;
+
+#define TBD_CLASS_DRV_ID 2
+
+static int set_msr_configuration(struct i2c_client *client, uint8_t *bufptr,
+				 unsigned int size)
+{
+	/*
+	 * The configuration data contains any number of sequences where
+	 * the first byte (that is, uint8_t) that marks the number of bytes
+	 * in the sequence to follow, is indeed followed by the indicated
+	 * number of bytes of actual data to be written to sensor.
+	 * By convention, the first two bytes of actual data should be
+	 * understood as an address in the sensor address space (hibyte
+	 * followed by lobyte) where the remaining data in the sequence
+	 * will be written.
+	 */
+
+	u8 *ptr = bufptr;
+
+	while (ptr < bufptr + size) {
+		struct i2c_msg msg = {
+			.addr = client->addr,
+			.flags = 0,
+		};
+		int ret;
+
+		/* How many bytes */
+		msg.len = *ptr++;
+		/* Where the bytes are located */
+		msg.buf = ptr;
+		ptr += msg.len;
+
+		if (ptr > bufptr + size)
+			/* Accessing data beyond bounds is not tolerated */
+			return -EINVAL;
+
+		ret = i2c_transfer(client->adapter, &msg, 1);
+		if (ret < 0) {
+			dev_err(&client->dev, "i2c write error: %d", ret);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static int parse_and_apply(struct i2c_client *client, uint8_t *buffer,
+			   unsigned int size)
+{
+	u8 *endptr8 = buffer + size;
+	struct tbd_data_record_header *header =
+	    (struct tbd_data_record_header *)buffer;
+
+	/* There may be any number of datasets present */
+	unsigned int dataset = 0;
+
+	do {
+		/* In below, four variables are read from buffer */
+		if ((uint8_t *)header + sizeof(*header) > endptr8)
+			return -EINVAL;
+
+		/* All data should be located within given buffer */
+		if ((uint8_t *)header + header->data_offset +
+		    header->data_size > endptr8)
+			return -EINVAL;
+
+		/* We have a new valid dataset */
+		dataset++;
+		/* See whether there is MSR data */
+		/* If yes, update the reg info */
+		if (header->data_size && (header->flags & 1)) {
+			int ret;
+
+			dev_info(&client->dev,
+				 "New MSR data for sensor driver (dataset %02d) size:%d\n",
+				 dataset, header->data_size);
+			ret = set_msr_configuration(client,
+						    buffer + header->data_offset,
+						    header->data_size);
+			if (ret)
+				return ret;
+		}
+		header = (struct tbd_data_record_header *)(buffer +
+			 header->next_offset);
+	} while (header->next_offset);
+
+	return 0;
+}
+
+int apply_msr_data(struct i2c_client *client, const struct firmware *fw)
+{
+	struct tbd_header *header;
+	struct tbd_record_header *record;
+
+	if (!fw) {
+		dev_warn(&client->dev, "Drv data is not loaded.\n");
+		return -EINVAL;
+	}
+
+	if (sizeof(*header) > fw->size)
+		return -EINVAL;
+
+	header = (struct tbd_header *)fw->data;
+	/* Check that we have drvb block. */
+	if (memcmp(&header->tag, "DRVB", 4))
+		return -EINVAL;
+
+	/* Check the size */
+	if (header->size != fw->size)
+		return -EINVAL;
+
+	if (sizeof(*header) + sizeof(*record) > fw->size)
+		return -EINVAL;
+
+	record = (struct tbd_record_header *)(header + 1);
+	/* Check that class id mathes tbd's drv id. */
+	if (record->class_id != TBD_CLASS_DRV_ID)
+		return -EINVAL;
+
+	/* Size 0 shall not be treated as an error */
+	if (!record->size)
+		return 0;
+
+	return parse_and_apply(client, (uint8_t *)(record + 1), record->size);
+}
+EXPORT_SYMBOL_GPL(apply_msr_data);
+
+int load_msr_list(struct i2c_client *client, char *name,
+		  const struct firmware **fw)
+{
+	int ret = request_firmware(fw, name, &client->dev);
+
+	if (ret) {
+		dev_err(&client->dev,
+			"Error %d while requesting firmware %s\n",
+			ret, name);
+		return ret;
+	}
+	dev_info(&client->dev, "Received %lu bytes drv data\n",
+		 (unsigned long)(*fw)->size);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(load_msr_list);
+
+void release_msr_list(struct i2c_client *client, const struct firmware *fw)
+{
+	release_firmware(fw);
+}
+EXPORT_SYMBOL_GPL(release_msr_list);
+
+static int init_msrlisthelper(void)
+{
+	return 0;
+}
+
+static void exit_msrlisthelper(void)
+{
+}
+
+module_init(init_msrlisthelper);
+module_exit(exit_msrlisthelper);
+
+MODULE_AUTHOR("Jukka Kaartinen <jukka.o.kaartinen@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-lm3554.c b/drivers/staging/media/atomisp/i2c/atomisp-lm3554.c
new file mode 100644
index 000000000..75d16b525
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-lm3554.c
@@ -0,0 +1,955 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * LED flash driver for LM3554
+ *
+ * Copyright (c) 2010-2012 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/slab.h>
+
+#include "../include/media/lm3554.h"
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <linux/acpi.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+#include "../include/linux/atomisp.h"
+
+/* Registers */
+
+#define LM3554_TORCH_BRIGHTNESS_REG	0xA0
+#define LM3554_TORCH_MODE_SHIFT		0
+#define LM3554_TORCH_CURRENT_SHIFT	3
+#define LM3554_INDICATOR_CURRENT_SHIFT	6
+
+#define LM3554_FLASH_BRIGHTNESS_REG	0xB0
+#define LM3554_FLASH_MODE_SHIFT		0
+#define LM3554_FLASH_CURRENT_SHIFT	3
+#define LM3554_STROBE_SENSITIVITY_SHIFT	7
+
+#define LM3554_FLASH_DURATION_REG	0xC0
+#define LM3554_FLASH_TIMEOUT_SHIFT	0
+#define LM3554_CURRENT_LIMIT_SHIFT	5
+
+#define LM3554_FLAGS_REG		0xD0
+#define LM3554_FLAG_TIMEOUT		BIT(0)
+#define LM3554_FLAG_THERMAL_SHUTDOWN	BIT(1)
+#define LM3554_FLAG_LED_FAULT		BIT(2)
+#define LM3554_FLAG_TX1_INTERRUPT	BIT(3)
+#define LM3554_FLAG_TX2_INTERRUPT	BIT(4)
+#define LM3554_FLAG_LED_THERMAL_FAULT	BIT(5)
+#define LM3554_FLAG_UNUSED		BIT(6)
+#define LM3554_FLAG_INPUT_VOLTAGE_LOW	BIT(7)
+
+#define LM3554_CONFIG_REG_1		0xE0
+#define LM3554_ENVM_TX2_SHIFT		5
+#define LM3554_TX2_POLARITY_SHIFT	6
+
+struct lm3554 {
+	struct v4l2_subdev sd;
+
+	struct mutex power_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+	int power_count;
+
+	unsigned int mode;
+	int timeout;
+	u8 torch_current;
+	u8 indicator_current;
+	u8 flash_current;
+
+	struct timer_list flash_off_delay;
+	struct lm3554_platform_data *pdata;
+};
+
+#define to_lm3554(p_sd)	container_of(p_sd, struct lm3554, sd)
+
+/* Return negative errno else zero on success */
+static int lm3554_write(struct lm3554 *flash, u8 addr, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->sd);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, addr, val);
+
+	dev_dbg(&client->dev, "Write Addr:%02X Val:%02X %s\n", addr, val,
+		ret < 0 ? "fail" : "ok");
+
+	return ret;
+}
+
+/* Return negative errno else a data byte received from the device. */
+static int lm3554_read(struct lm3554 *flash, u8 addr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->sd);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, addr);
+
+	dev_dbg(&client->dev, "Read Addr:%02X Val:%02X %s\n", addr, ret,
+		ret < 0 ? "fail" : "ok");
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware configuration
+ */
+
+static int lm3554_set_mode(struct lm3554 *flash, unsigned int mode)
+{
+	u8 val;
+	int ret;
+
+	val = (mode << LM3554_FLASH_MODE_SHIFT) |
+	      (flash->flash_current << LM3554_FLASH_CURRENT_SHIFT);
+
+	ret = lm3554_write(flash, LM3554_FLASH_BRIGHTNESS_REG, val);
+	if (ret == 0)
+		flash->mode = mode;
+	return ret;
+}
+
+static int lm3554_set_torch(struct lm3554 *flash)
+{
+	u8 val;
+
+	val = (flash->mode << LM3554_TORCH_MODE_SHIFT) |
+	      (flash->torch_current << LM3554_TORCH_CURRENT_SHIFT) |
+	      (flash->indicator_current << LM3554_INDICATOR_CURRENT_SHIFT);
+
+	return lm3554_write(flash, LM3554_TORCH_BRIGHTNESS_REG, val);
+}
+
+static int lm3554_set_flash(struct lm3554 *flash)
+{
+	u8 val;
+
+	val = (flash->mode << LM3554_FLASH_MODE_SHIFT) |
+	      (flash->flash_current << LM3554_FLASH_CURRENT_SHIFT);
+
+	return lm3554_write(flash, LM3554_FLASH_BRIGHTNESS_REG, val);
+}
+
+static int lm3554_set_duration(struct lm3554 *flash)
+{
+	u8 val;
+
+	val = (flash->timeout << LM3554_FLASH_TIMEOUT_SHIFT) |
+	      (flash->pdata->current_limit << LM3554_CURRENT_LIMIT_SHIFT);
+
+	return lm3554_write(flash, LM3554_FLASH_DURATION_REG, val);
+}
+
+static int lm3554_set_config1(struct lm3554 *flash)
+{
+	u8 val;
+
+	val = (flash->pdata->envm_tx2 << LM3554_ENVM_TX2_SHIFT) |
+	      (flash->pdata->tx2_polarity << LM3554_TX2_POLARITY_SHIFT);
+	return lm3554_write(flash, LM3554_CONFIG_REG_1, val);
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware trigger
+ */
+static void lm3554_flash_off_delay(struct timer_list *t)
+{
+	struct lm3554 *flash = from_timer(flash, t, flash_off_delay);
+	struct lm3554_platform_data *pdata = flash->pdata;
+
+	gpiod_set_value(pdata->gpio_strobe, 0);
+}
+
+static int lm3554_hw_strobe(struct i2c_client *client, bool strobe)
+{
+	int ret, timer_pending;
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(sd);
+	struct lm3554_platform_data *pdata = flash->pdata;
+
+	/*
+	 * An abnormal high flash current is observed when strobe off the
+	 * flash. Workaround here is firstly set flash current to lower level,
+	 * wait a short moment, and then strobe off the flash.
+	 */
+
+	timer_pending = del_timer_sync(&flash->flash_off_delay);
+
+	/* Flash off */
+	if (!strobe) {
+		/* set current to 70mA and wait a while */
+		ret = lm3554_write(flash, LM3554_FLASH_BRIGHTNESS_REG, 0);
+		if (ret < 0)
+			goto err;
+		mod_timer(&flash->flash_off_delay,
+			  jiffies + msecs_to_jiffies(LM3554_TIMER_DELAY));
+		return 0;
+	}
+
+	/* Flash on */
+
+	/*
+	 * If timer is killed before run, flash is not strobe off,
+	 * so must strobe off here
+	 */
+	if (timer_pending)
+		gpiod_set_value(pdata->gpio_strobe, 0);
+
+	/* Restore flash current settings */
+	ret = lm3554_set_flash(flash);
+	if (ret < 0)
+		goto err;
+
+	/* Strobe on Flash */
+	gpiod_set_value(pdata->gpio_strobe, 1);
+
+	return 0;
+err:
+	dev_err(&client->dev, "failed to %s flash strobe (%d)\n",
+		strobe ? "on" : "off", ret);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 controls
+ */
+
+static int lm3554_read_status(struct lm3554 *flash)
+{
+	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->sd);
+
+	/* NOTE: reading register clear fault status */
+	ret = lm3554_read(flash, LM3554_FLAGS_REG);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Accordingly to datasheet we read back '1' in bit 6.
+	 * Clear it first.
+	 */
+	ret &= ~LM3554_FLAG_UNUSED;
+
+	/*
+	 * Do not take TX1/TX2 signal as an error
+	 * because MSIC will not turn off flash, but turn to
+	 * torch mode according to gsm modem signal by hardware.
+	 */
+	ret &= ~(LM3554_FLAG_TX1_INTERRUPT | LM3554_FLAG_TX2_INTERRUPT);
+
+	if (ret > 0)
+		dev_dbg(&client->dev, "LM3554 flag status: %02x\n", ret);
+
+	return ret;
+}
+
+static int lm3554_s_flash_timeout(struct v4l2_subdev *sd, u32 val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	val = clamp(val, LM3554_MIN_TIMEOUT, LM3554_MAX_TIMEOUT);
+	val = val / LM3554_TIMEOUT_STEPSIZE - 1;
+
+	flash->timeout = val;
+
+	return lm3554_set_duration(flash);
+}
+
+static int lm3554_g_flash_timeout(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	*val = (u32)(flash->timeout + 1) * LM3554_TIMEOUT_STEPSIZE;
+
+	return 0;
+}
+
+static int lm3554_s_flash_intensity(struct v4l2_subdev *sd, u32 intensity)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	intensity = LM3554_CLAMP_PERCENTAGE(intensity);
+	intensity = LM3554_PERCENT_TO_VALUE(intensity, LM3554_FLASH_STEP);
+
+	flash->flash_current = intensity;
+
+	return lm3554_set_flash(flash);
+}
+
+static int lm3554_g_flash_intensity(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	*val = LM3554_VALUE_TO_PERCENT((u32)flash->flash_current,
+				       LM3554_FLASH_STEP);
+
+	return 0;
+}
+
+static int lm3554_s_torch_intensity(struct v4l2_subdev *sd, u32 intensity)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	intensity = LM3554_CLAMP_PERCENTAGE(intensity);
+	intensity = LM3554_PERCENT_TO_VALUE(intensity, LM3554_TORCH_STEP);
+
+	flash->torch_current = intensity;
+
+	return lm3554_set_torch(flash);
+}
+
+static int lm3554_g_torch_intensity(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	*val = LM3554_VALUE_TO_PERCENT((u32)flash->torch_current,
+				       LM3554_TORCH_STEP);
+
+	return 0;
+}
+
+static int lm3554_s_indicator_intensity(struct v4l2_subdev *sd, u32 intensity)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	intensity = LM3554_CLAMP_PERCENTAGE(intensity);
+	intensity = LM3554_PERCENT_TO_VALUE(intensity, LM3554_INDICATOR_STEP);
+
+	flash->indicator_current = intensity;
+
+	return lm3554_set_torch(flash);
+}
+
+static int lm3554_g_indicator_intensity(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+
+	*val = LM3554_VALUE_TO_PERCENT((u32)flash->indicator_current,
+				       LM3554_INDICATOR_STEP);
+
+	return 0;
+}
+
+static int lm3554_s_flash_strobe(struct v4l2_subdev *sd, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return lm3554_hw_strobe(client, val);
+}
+
+static int lm3554_s_flash_mode(struct v4l2_subdev *sd, u32 new_mode)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+	unsigned int mode;
+
+	switch (new_mode) {
+	case ATOMISP_FLASH_MODE_OFF:
+		mode = LM3554_MODE_SHUTDOWN;
+		break;
+	case ATOMISP_FLASH_MODE_FLASH:
+		mode = LM3554_MODE_FLASH;
+		break;
+	case ATOMISP_FLASH_MODE_INDICATOR:
+		mode = LM3554_MODE_INDICATOR;
+		break;
+	case ATOMISP_FLASH_MODE_TORCH:
+		mode = LM3554_MODE_TORCH;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return lm3554_set_mode(flash, mode);
+}
+
+static int lm3554_g_flash_mode(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+	*val = flash->mode;
+	return 0;
+}
+
+static int lm3554_g_flash_status(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+	int value;
+
+	value = lm3554_read_status(flash);
+	if (value < 0)
+		return value;
+
+	if (value & LM3554_FLAG_TIMEOUT)
+		*val = ATOMISP_FLASH_STATUS_TIMEOUT;
+	else if (value > 0)
+		*val = ATOMISP_FLASH_STATUS_HW_ERROR;
+	else
+		*val = ATOMISP_FLASH_STATUS_OK;
+
+	return 0;
+}
+
+static int lm3554_g_flash_status_register(struct v4l2_subdev *sd, s32 *val)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+	int ret;
+
+	ret = lm3554_read(flash, LM3554_FLAGS_REG);
+
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+	return 0;
+}
+
+static int lm3554_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct lm3554 *dev =
+	    container_of(ctrl->handler, struct lm3554, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_TIMEOUT:
+		ret = lm3554_s_flash_timeout(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FLASH_INTENSITY:
+		ret = lm3554_s_flash_intensity(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+		ret = lm3554_s_torch_intensity(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		ret = lm3554_s_indicator_intensity(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FLASH_STROBE:
+		ret = lm3554_s_flash_strobe(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FLASH_MODE:
+		ret = lm3554_s_flash_mode(&dev->sd, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int lm3554_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct lm3554 *dev =
+	    container_of(ctrl->handler, struct lm3554, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_TIMEOUT:
+		ret = lm3554_g_flash_timeout(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_INTENSITY:
+		ret = lm3554_g_flash_intensity(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+		ret = lm3554_g_torch_intensity(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		ret = lm3554_g_indicator_intensity(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_MODE:
+		ret = lm3554_g_flash_mode(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_STATUS:
+		ret = lm3554_g_flash_status(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FLASH_STATUS_REGISTER:
+		ret = lm3554_g_flash_status_register(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = lm3554_s_ctrl,
+	.g_volatile_ctrl = lm3554_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config lm3554_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_TIMEOUT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Flash Timeout",
+		.min = 0x0,
+		.max = LM3554_MAX_TIMEOUT,
+		.step = 0x01,
+		.def = LM3554_DEFAULT_TIMEOUT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_INTENSITY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Flash Intensity",
+		.min = LM3554_MIN_PERCENT,
+		.max = LM3554_MAX_PERCENT,
+		.step = 0x01,
+		.def = LM3554_FLASH_DEFAULT_BRIGHTNESS,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_TORCH_INTENSITY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Torch Intensity",
+		.min = LM3554_MIN_PERCENT,
+		.max = LM3554_MAX_PERCENT,
+		.step = 0x01,
+		.def = LM3554_TORCH_DEFAULT_BRIGHTNESS,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_INDICATOR_INTENSITY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Indicator Intensity",
+		.min = LM3554_MIN_PERCENT,
+		.max = LM3554_MAX_PERCENT,
+		.step = 0x01,
+		.def = LM3554_INDICATOR_DEFAULT_BRIGHTNESS,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_STROBE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Flash Strobe",
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_MODE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Flash Mode",
+		.min = 0,
+		.max = 100,
+		.step = 1,
+		.def = ATOMISP_FLASH_MODE_OFF,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_STATUS,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Flash Status",
+		.min = ATOMISP_FLASH_STATUS_OK,
+		.max = ATOMISP_FLASH_STATUS_TIMEOUT,
+		.step = 1,
+		.def = ATOMISP_FLASH_STATUS_OK,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FLASH_STATUS_REGISTER,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Flash Status Register",
+		.min = 0,
+		.max = 255,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+/* Put device into known state. */
+static int lm3554_setup(struct lm3554 *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->sd);
+	int ret;
+
+	/* clear the flags register */
+	ret = lm3554_read(flash, LM3554_FLAGS_REG);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(&client->dev, "Fault info: %02x\n", ret);
+
+	ret = lm3554_set_config1(flash);
+	if (ret < 0)
+		return ret;
+
+	ret = lm3554_set_duration(flash);
+	if (ret < 0)
+		return ret;
+
+	ret = lm3554_set_torch(flash);
+	if (ret < 0)
+		return ret;
+
+	ret = lm3554_set_flash(flash);
+	if (ret < 0)
+		return ret;
+
+	/* read status */
+	ret = lm3554_read_status(flash);
+	if (ret < 0)
+		return ret;
+
+	return ret ? -EIO : 0;
+}
+
+static int __lm3554_s_power(struct lm3554 *flash, int power)
+{
+	struct lm3554_platform_data *pdata = flash->pdata;
+	int ret;
+
+	/*initialize flash driver*/
+	gpiod_set_value(pdata->gpio_reset, power);
+	usleep_range(100, 100 + 1);
+
+	if (power) {
+		/* Setup default values. This makes sure that the chip
+		 * is in a known state.
+		 */
+		ret = lm3554_setup(flash);
+		if (ret < 0) {
+			__lm3554_s_power(flash, 0);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int lm3554_s_power(struct v4l2_subdev *sd, int power)
+{
+	struct lm3554 *flash = to_lm3554(sd);
+	int ret = 0;
+
+	mutex_lock(&flash->power_lock);
+
+	if (flash->power_count == !power) {
+		ret = __lm3554_s_power(flash, !!power);
+		if (ret < 0)
+			goto done;
+	}
+
+	flash->power_count += power ? 1 : -1;
+	WARN_ON(flash->power_count < 0);
+
+done:
+	mutex_unlock(&flash->power_lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops lm3554_core_ops = {
+	.s_power = lm3554_s_power,
+};
+
+static const struct v4l2_subdev_ops lm3554_ops = {
+	.core = &lm3554_core_ops,
+};
+
+static int lm3554_detect(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_adapter *adapter = client->adapter;
+	struct lm3554 *flash = to_lm3554(sd);
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev, "lm3554_detect i2c error\n");
+		return -ENODEV;
+	}
+
+	/* Power up the flash driver and reset it */
+	ret = lm3554_s_power(&flash->sd, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to power on lm3554 LED flash\n");
+	} else {
+		dev_dbg(&client->dev, "Successfully detected lm3554 LED flash\n");
+		lm3554_s_power(&flash->sd, 0);
+	}
+
+	return ret;
+}
+
+static int lm3554_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return lm3554_s_power(sd, 1);
+}
+
+static int lm3554_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return lm3554_s_power(sd, 0);
+}
+
+static const struct v4l2_subdev_internal_ops lm3554_internal_ops = {
+	.registered = lm3554_detect,
+	.open = lm3554_open,
+	.close = lm3554_close,
+};
+
+/* -----------------------------------------------------------------------------
+ *  I2C driver
+ */
+#ifdef CONFIG_PM
+
+static int lm3554_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(subdev);
+	int rval;
+
+	if (flash->power_count == 0)
+		return 0;
+
+	rval = __lm3554_s_power(flash, 0);
+
+	dev_dbg(&client->dev, "Suspend %s\n", rval < 0 ? "failed" : "ok");
+
+	return rval;
+}
+
+static int lm3554_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(subdev);
+	int rval;
+
+	if (flash->power_count == 0)
+		return 0;
+
+	rval = __lm3554_s_power(flash, 1);
+
+	dev_dbg(&client->dev, "Resume %s\n", rval < 0 ? "fail" : "ok");
+
+	return rval;
+}
+
+#else
+
+#define lm3554_suspend NULL
+#define lm3554_resume  NULL
+
+#endif /* CONFIG_PM */
+
+static int lm3554_gpio_init(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(sd);
+	struct lm3554_platform_data *pdata = flash->pdata;
+	int ret;
+
+	if (!pdata->gpio_reset)
+		return -EINVAL;
+
+	ret = gpiod_direction_output(pdata->gpio_reset, 0);
+	if (ret < 0)
+		return ret;
+
+	if (!pdata->gpio_strobe)
+		return -EINVAL;
+
+	ret = gpiod_direction_output(pdata->gpio_strobe, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static void lm3554_gpio_uninit(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(sd);
+	struct lm3554_platform_data *pdata = flash->pdata;
+	int ret;
+
+	ret = gpiod_direction_output(pdata->gpio_strobe, 0);
+	if (ret < 0)
+		dev_err(&client->dev,
+			"gpio request/direction_output fail for gpio_strobe");
+
+	ret = gpiod_direction_output(pdata->gpio_reset, 0);
+	if (ret < 0)
+		dev_err(&client->dev,
+			"gpio request/direction_output fail for gpio_reset");
+}
+
+static void *lm3554_platform_data_func(struct i2c_client *client)
+{
+	static struct lm3554_platform_data platform_data;
+
+	platform_data.gpio_reset = gpiod_get_index(&client->dev,
+						   NULL, 2, GPIOD_OUT_LOW);
+	if (IS_ERR(platform_data.gpio_reset))
+		return ERR_CAST(platform_data.gpio_reset);
+	platform_data.gpio_strobe = gpiod_get_index(&client->dev,
+						    NULL, 0, GPIOD_OUT_LOW);
+	if (IS_ERR(platform_data.gpio_strobe))
+		return ERR_CAST(platform_data.gpio_strobe);
+	platform_data.gpio_torch = gpiod_get_index(&client->dev,
+						   NULL, 1, GPIOD_OUT_LOW);
+	if (IS_ERR(platform_data.gpio_torch))
+		return ERR_CAST(platform_data.gpio_torch);
+
+	/* Set to TX2 mode, then ENVM/TX2 pin is a power amplifier sync input:
+	 * ENVM/TX pin asserted, flash forced into torch;
+	 * ENVM/TX pin desserted, flash set back;
+	 */
+	platform_data.envm_tx2 = 1;
+	platform_data.tx2_polarity = 0;
+
+	/* set peak current limit to be 1000mA */
+	platform_data.current_limit = 0;
+
+	return &platform_data;
+}
+
+static int lm3554_probe(struct i2c_client *client)
+{
+	int err = 0;
+	struct lm3554 *flash;
+	unsigned int i;
+
+	flash = kzalloc(sizeof(*flash), GFP_KERNEL);
+	if (!flash)
+		return -ENOMEM;
+
+	flash->pdata = lm3554_platform_data_func(client);
+	if (IS_ERR(flash->pdata)) {
+		err = PTR_ERR(flash->pdata);
+		goto free_flash;
+	}
+
+	v4l2_i2c_subdev_init(&flash->sd, client, &lm3554_ops);
+	flash->sd.internal_ops = &lm3554_internal_ops;
+	flash->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	flash->mode = ATOMISP_FLASH_MODE_OFF;
+	flash->timeout = LM3554_MAX_TIMEOUT / LM3554_TIMEOUT_STEPSIZE - 1;
+	err =
+	    v4l2_ctrl_handler_init(&flash->ctrl_handler,
+				   ARRAY_SIZE(lm3554_controls));
+	if (err) {
+		dev_err(&client->dev, "error initialize a ctrl_handler.\n");
+		goto unregister_subdev;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lm3554_controls); i++)
+		v4l2_ctrl_new_custom(&flash->ctrl_handler, &lm3554_controls[i],
+				     NULL);
+
+	if (flash->ctrl_handler.error) {
+		dev_err(&client->dev, "ctrl_handler error.\n");
+		err = flash->ctrl_handler.error;
+		goto free_handler;
+	}
+
+	flash->sd.ctrl_handler = &flash->ctrl_handler;
+	err = media_entity_pads_init(&flash->sd.entity, 0, NULL);
+	if (err) {
+		dev_err(&client->dev, "error initialize a media entity.\n");
+		goto free_handler;
+	}
+
+	flash->sd.entity.function = MEDIA_ENT_F_FLASH;
+
+	mutex_init(&flash->power_lock);
+
+	timer_setup(&flash->flash_off_delay, lm3554_flash_off_delay, 0);
+
+	err = lm3554_gpio_init(client);
+	if (err) {
+		dev_err(&client->dev, "gpio request/direction_output fail.\n");
+		goto cleanup_media;
+	}
+
+	err = atomisp_register_i2c_module(&flash->sd, NULL, LED_FLASH);
+	if (err) {
+		dev_err(&client->dev, "fail to register atomisp i2c module.\n");
+		goto uninit_gpio;
+	}
+
+	return 0;
+
+uninit_gpio:
+	lm3554_gpio_uninit(client);
+cleanup_media:
+	media_entity_cleanup(&flash->sd.entity);
+free_handler:
+	v4l2_ctrl_handler_free(&flash->ctrl_handler);
+unregister_subdev:
+	v4l2_device_unregister_subdev(&flash->sd);
+free_flash:
+	kfree(flash);
+
+	return err;
+}
+
+static void lm3554_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(sd);
+
+	media_entity_cleanup(&flash->sd.entity);
+	v4l2_ctrl_handler_free(&flash->ctrl_handler);
+	v4l2_device_unregister_subdev(sd);
+
+	atomisp_gmin_remove_subdev(sd);
+
+	del_timer_sync(&flash->flash_off_delay);
+
+	lm3554_gpio_uninit(client);
+
+	kfree(flash);
+}
+
+static const struct dev_pm_ops lm3554_pm_ops = {
+	.suspend = lm3554_suspend,
+	.resume = lm3554_resume,
+};
+
+static const struct acpi_device_id lm3554_acpi_match[] = {
+	{ "INTCF1C" },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, lm3554_acpi_match);
+
+static struct i2c_driver lm3554_driver = {
+	.driver = {
+		.name = "lm3554",
+		.pm   = &lm3554_pm_ops,
+		.acpi_match_table = lm3554_acpi_match,
+	},
+	.probe_new = lm3554_probe,
+	.remove = lm3554_remove,
+};
+module_i2c_driver(lm3554_driver);
+
+MODULE_AUTHOR("Jing Tao <jing.tao@intel.com>");
+MODULE_DESCRIPTION("LED flash driver for LM3554");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c b/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c
new file mode 100644
index 000000000..a0e8e94b2
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-mt9m114.c
@@ -0,0 +1,1815 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for mt9m114 Camera Sensor.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/acpi.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+#include <media/v4l2-device.h>
+
+#include "mt9m114.h"
+
+#define to_mt9m114_sensor(sd) container_of(sd, struct mt9m114_device, sd)
+
+/*
+ * TODO: use debug parameter to actually define when debug messages should
+ * be printed.
+ */
+static int debug;
+static int aaalock;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+static int mt9m114_t_vflip(struct v4l2_subdev *sd, int value);
+static int mt9m114_t_hflip(struct v4l2_subdev *sd, int value);
+static int mt9m114_wait_state(struct i2c_client *client, int timeout);
+
+static int
+mt9m114_read_reg(struct i2c_client *client, u16 data_length, u32 reg, u32 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[4];
+
+	if (!client->adapter) {
+		v4l2_err(client, "%s error, no client->adapter\n", __func__);
+		return -ENODEV;
+	}
+
+	if (data_length != MISENSOR_8BIT && data_length != MISENSOR_16BIT
+	    && data_length != MISENSOR_32BIT) {
+		v4l2_err(client, "%s error, invalid data length\n", __func__);
+		return -EINVAL;
+	}
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = MSG_LEN_OFFSET;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u16)(reg >> 8);
+	data[1] = (u16)(reg & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].len = data_length;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err >= 0) {
+		*val = 0;
+		/* high byte comes first */
+		if (data_length == MISENSOR_8BIT)
+			*val = data[0];
+		else if (data_length == MISENSOR_16BIT)
+			*val = data[1] + (data[0] << 8);
+		else
+			*val = data[3] + (data[2] << 8) +
+			       (data[1] << 16) + (data[0] << 24);
+
+		return 0;
+	}
+
+	dev_err(&client->dev, "read from offset 0x%x error %d", reg, err);
+	return err;
+}
+
+static int
+mt9m114_write_reg(struct i2c_client *client, u16 data_length, u16 reg, u32 val)
+{
+	int num_msg;
+	struct i2c_msg msg;
+	unsigned char data[6] = {0};
+	__be16 *wreg;
+	int retry = 0;
+
+	if (!client->adapter) {
+		v4l2_err(client, "%s error, no client->adapter\n", __func__);
+		return -ENODEV;
+	}
+
+	if (data_length != MISENSOR_8BIT && data_length != MISENSOR_16BIT
+	    && data_length != MISENSOR_32BIT) {
+		v4l2_err(client, "%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	memset(&msg, 0, sizeof(msg));
+
+again:
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2 + data_length;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	wreg = (void *)data;
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == MISENSOR_8BIT) {
+		data[2] = (u8)(val);
+	} else if (data_length == MISENSOR_16BIT) {
+		u16 *wdata = (void *)&data[2];
+
+		*wdata = be16_to_cpu(*(__be16 *)&data[2]);
+	} else {
+		/* MISENSOR_32BIT */
+		u32 *wdata = (void *)&data[2];
+
+		*wdata = be32_to_cpu(*(__be32 *)&data[2]);
+	}
+
+	num_msg = i2c_transfer(client->adapter, &msg, 1);
+
+	/*
+	 * HACK: Need some delay here for Rev 2 sensors otherwise some
+	 * registers do not seem to load correctly.
+	 */
+	mdelay(1);
+
+	if (num_msg >= 0)
+		return 0;
+
+	dev_err(&client->dev, "write error: wrote 0x%x to offset 0x%x error %d",
+		val, reg, num_msg);
+	if (retry <= I2C_RETRY_COUNT) {
+		dev_dbg(&client->dev, "retrying... %d", retry);
+		retry++;
+		msleep(20);
+		goto again;
+	}
+
+	return num_msg;
+}
+
+/**
+ * misensor_rmw_reg - Read/Modify/Write a value to a register in the sensor
+ * device
+ * @client: i2c driver client structure
+ * @data_length: 8/16/32-bits length
+ * @reg: register address
+ * @mask: masked out bits
+ * @set: bits set
+ *
+ * Read/modify/write a value to a register in the  sensor device.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int
+misensor_rmw_reg(struct i2c_client *client, u16 data_length, u16 reg,
+		 u32 mask, u32 set)
+{
+	int err;
+	u32 val;
+
+	/* Exit when no mask */
+	if (mask == 0)
+		return 0;
+
+	/* @mask must not exceed data length */
+	switch (data_length) {
+	case MISENSOR_8BIT:
+		if (mask & ~0xff)
+			return -EINVAL;
+		break;
+	case MISENSOR_16BIT:
+		if (mask & ~0xffff)
+			return -EINVAL;
+		break;
+	case MISENSOR_32BIT:
+		break;
+	default:
+		/* Wrong @data_length */
+		return -EINVAL;
+	}
+
+	err = mt9m114_read_reg(client, data_length, reg, &val);
+	if (err) {
+		v4l2_err(client, "%s error exit, read failed\n", __func__);
+		return -EINVAL;
+	}
+
+	val &= ~mask;
+
+	/*
+	 * Perform the OR function if the @set exists.
+	 * Shift @set value to target bit location. @set should set only
+	 * bits included in @mask.
+	 *
+	 * REVISIT: This function expects @set to be non-shifted. Its shift
+	 * value is then defined to be equal to mask's LSB position.
+	 * How about to inform values in their right offset position and avoid
+	 * this unneeded shift operation?
+	 */
+	set <<= ffs(mask) - 1;
+	val |= set & mask;
+
+	err = mt9m114_write_reg(client, data_length, reg, val);
+	if (err) {
+		v4l2_err(client, "%s error exit, write failed\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __mt9m114_flush_reg_array(struct i2c_client *client,
+				     struct mt9m114_write_ctrl *ctrl)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+	int retry = 0;
+	__be16 *data16 = (void *)&ctrl->buffer.addr;
+
+	if (ctrl->index == 0)
+		return 0;
+
+again:
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2 + ctrl->index;
+	*data16 = cpu_to_be16(ctrl->buffer.addr);
+	msg.buf = (u8 *)&ctrl->buffer;
+
+	ret = i2c_transfer(client->adapter, &msg, num_msg);
+	if (ret != num_msg) {
+		if (++retry <= I2C_RETRY_COUNT) {
+			dev_dbg(&client->dev, "retrying... %d\n", retry);
+			msleep(20);
+			goto again;
+		}
+		dev_err(&client->dev, "%s: i2c transfer error\n", __func__);
+		return -EIO;
+	}
+
+	ctrl->index = 0;
+
+	/*
+	 * REVISIT: Previously we had a delay after writing data to sensor.
+	 * But it was removed as our tests have shown it is not necessary
+	 * anymore.
+	 */
+
+	return 0;
+}
+
+static int __mt9m114_buf_reg_array(struct i2c_client *client,
+				   struct mt9m114_write_ctrl *ctrl,
+				   const struct misensor_reg *next)
+{
+	__be16 *data16;
+	__be32 *data32;
+	int err;
+
+	/* Insufficient buffer? Let's flush and get more free space. */
+	if (ctrl->index + next->length >= MT9M114_MAX_WRITE_BUF_SIZE) {
+		err = __mt9m114_flush_reg_array(client, ctrl);
+		if (err)
+			return err;
+	}
+
+	switch (next->length) {
+	case MISENSOR_8BIT:
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case MISENSOR_16BIT:
+		data16 = (__be16 *)&ctrl->buffer.data[ctrl->index];
+		*data16 = cpu_to_be16((u16)next->val);
+		break;
+	case MISENSOR_32BIT:
+		data32 = (__be32 *)&ctrl->buffer.data[ctrl->index];
+		*data32 = cpu_to_be32(next->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* When first item is added, we need to store its starting address */
+	if (ctrl->index == 0)
+		ctrl->buffer.addr = next->reg;
+
+	ctrl->index += next->length;
+
+	return 0;
+}
+
+static int
+__mt9m114_write_reg_is_consecutive(struct i2c_client *client,
+				   struct mt9m114_write_ctrl *ctrl,
+				   const struct misensor_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+/*
+ * mt9m114_write_reg_array - Initializes a list of mt9m114 registers
+ * @client: i2c driver client structure
+ * @reglist: list of registers to be written
+ * @poll: completion polling requirement
+ * This function initializes a list of registers. When consecutive addresses
+ * are found in a row on the list, this function creates a buffer and sends
+ * consecutive data in a single i2c_transfer().
+ *
+ * __mt9m114_flush_reg_array, __mt9m114_buf_reg_array() and
+ * __mt9m114_write_reg_is_consecutive() are internal functions to
+ * mt9m114_write_reg_array() and should be not used anywhere else.
+ *
+ */
+static int mt9m114_write_reg_array(struct i2c_client *client,
+				   const struct misensor_reg *reglist,
+				   int poll)
+{
+	const struct misensor_reg *next = reglist;
+	struct mt9m114_write_ctrl ctrl;
+	int err;
+
+	if (poll == PRE_POLLING) {
+		err = mt9m114_wait_state(client, MT9M114_WAIT_STAT_TIMEOUT);
+		if (err)
+			return err;
+	}
+
+	ctrl.index = 0;
+	for (; next->length != MISENSOR_TOK_TERM; next++) {
+		switch (next->length & MISENSOR_TOK_MASK) {
+		case MISENSOR_TOK_DELAY:
+			err = __mt9m114_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			msleep(next->val);
+			break;
+		case MISENSOR_TOK_RMW:
+			err = __mt9m114_flush_reg_array(client, &ctrl);
+			err |= misensor_rmw_reg(client,
+						next->length &
+						~MISENSOR_TOK_RMW,
+						next->reg, next->val,
+						next->val2);
+			if (err) {
+				dev_err(&client->dev, "%s read err. aborted\n",
+					__func__);
+				return -EINVAL;
+			}
+			break;
+		default:
+			/*
+			 * If next address is not consecutive, data needs to be
+			 * flushed before proceed.
+			 */
+			if (!__mt9m114_write_reg_is_consecutive(client, &ctrl,
+								next)) {
+				err = __mt9m114_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __mt9m114_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				v4l2_err(client, "%s: write error, aborted\n",
+					 __func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	err = __mt9m114_flush_reg_array(client, &ctrl);
+	if (err)
+		return err;
+
+	if (poll == POST_POLLING)
+		return mt9m114_wait_state(client, MT9M114_WAIT_STAT_TIMEOUT);
+
+	return 0;
+}
+
+static int mt9m114_wait_state(struct i2c_client *client, int timeout)
+{
+	int ret;
+	unsigned int val;
+
+	while (timeout-- > 0) {
+		ret = mt9m114_read_reg(client, MISENSOR_16BIT, 0x0080, &val);
+		if (ret)
+			return ret;
+		if ((val & 0x2) == 0)
+			return 0;
+		msleep(20);
+	}
+
+	return -EINVAL;
+}
+
+static int mt9m114_set_suspend(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return mt9m114_write_reg_array(client,
+				       mt9m114_standby_reg, POST_POLLING);
+}
+
+static int mt9m114_init_common(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return mt9m114_write_reg_array(client, mt9m114_common, PRE_POLLING);
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret;
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	if (flag) {
+		ret = dev->platform_data->v2p8_ctrl(sd, 1);
+		if (ret == 0) {
+			ret = dev->platform_data->v1p8_ctrl(sd, 1);
+			if (ret)
+				ret = dev->platform_data->v2p8_ctrl(sd, 0);
+		}
+	} else {
+		ret = dev->platform_data->v2p8_ctrl(sd, 0);
+		ret = dev->platform_data->v1p8_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret;
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/*
+	 * Note: current modules wire only one GPIO signal (RESET#),
+	 * but the schematic wires up two to the connector.  BIOS
+	 * versions have been unfortunately inconsistent with which
+	 * ACPI index RESET# is on, so hit both
+	 */
+
+	if (flag) {
+		ret = dev->platform_data->gpio0_ctrl(sd, 0);
+		ret = dev->platform_data->gpio1_ctrl(sd, 0);
+		msleep(60);
+		ret |= dev->platform_data->gpio0_ctrl(sd, 1);
+		ret |= dev->platform_data->gpio1_ctrl(sd, 1);
+	} else {
+		ret = dev->platform_data->gpio0_ctrl(sd, 0);
+		ret = dev->platform_data->gpio1_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev, "no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* flis clock control */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret)
+		dev_err(&client->dev, "gpio failed 1\n");
+	/*
+	 * according to DS, 44ms is needed between power up and first i2c
+	 * commend
+	 */
+	msleep(50);
+
+	return 0;
+
+fail_clk:
+	dev->platform_data->flisclk_ctrl(sd, 0);
+fail_power:
+	power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev, "no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "gpio failed 1\n");
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	/* according to DS, 20ms is needed after power down */
+	msleep(20);
+
+	return ret;
+}
+
+static int mt9m114_s_power(struct v4l2_subdev *sd, int power)
+{
+	if (power == 0)
+		return power_down(sd);
+
+	if (power_up(sd))
+		return -EINVAL;
+
+	return mt9m114_init_common(sd);
+}
+
+static int mt9m114_res2size(struct v4l2_subdev *sd, int *h_size, int *v_size)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	unsigned short hsize;
+	unsigned short vsize;
+
+	switch (dev->res) {
+	case MT9M114_RES_736P:
+		hsize = MT9M114_RES_736P_SIZE_H;
+		vsize = MT9M114_RES_736P_SIZE_V;
+		break;
+	case MT9M114_RES_864P:
+		hsize = MT9M114_RES_864P_SIZE_H;
+		vsize = MT9M114_RES_864P_SIZE_V;
+		break;
+	case MT9M114_RES_960P:
+		hsize = MT9M114_RES_960P_SIZE_H;
+		vsize = MT9M114_RES_960P_SIZE_V;
+		break;
+	default:
+		v4l2_err(sd, "%s: Resolution 0x%08x unknown\n", __func__,
+			 dev->res);
+		return -EINVAL;
+	}
+
+	if (h_size)
+		*h_size = hsize;
+	if (v_size)
+		*v_size = vsize;
+
+	return 0;
+}
+
+static int mt9m114_get_intg_factor(struct i2c_client *client,
+				   struct camera_mipi_info *info,
+				   const struct mt9m114_res_struct *res)
+{
+	struct atomisp_sensor_mode_data *buf;
+	u32 reg_val;
+	int ret;
+
+	if (!info)
+		return -EINVAL;
+
+	buf = &info->data;
+
+	ret =  mt9m114_read_reg(client, MISENSOR_32BIT,
+				REG_PIXEL_CLK, &reg_val);
+	if (ret)
+		return ret;
+	buf->vt_pix_clk_freq_mhz = reg_val;
+
+	/* get integration time */
+	buf->coarse_integration_time_min = MT9M114_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    MT9M114_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = MT9M114_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    MT9M114_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = MT9M114_FINE_INTG_TIME_MIN;
+
+	buf->frame_length_lines = res->lines_per_frame;
+	buf->line_length_pck = res->pixels_per_line;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	ret =  mt9m114_read_reg(client, MISENSOR_16BIT,
+				REG_H_START, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = reg_val;
+
+	ret =  mt9m114_read_reg(client, MISENSOR_16BIT,
+				REG_V_START, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_H_END, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_V_END, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_WIDTH, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_HEIGHT, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_TIMING_HTS, &reg_val);
+	if (ret)
+		return ret;
+	buf->line_length_pck = reg_val;
+
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_TIMING_VTS, &reg_val);
+	if (ret)
+		return ret;
+	buf->frame_length_lines = reg_val;
+
+	buf->binning_factor_x = res->bin_factor_x ?
+				res->bin_factor_x : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				res->bin_factor_y : 1;
+	return 0;
+}
+
+static int mt9m114_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	int width, height;
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	ret = mt9m114_res2size(sd, &width, &height);
+	if (ret)
+		return ret;
+	fmt->width = width;
+	fmt->height = height;
+
+	return 0;
+}
+
+static int mt9m114_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	struct mt9m114_res_struct *res;
+	u32 width = fmt->width;
+	u32 height = fmt->height;
+	struct camera_mipi_info *mt9m114_info = NULL;
+
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
+	dev->streamon = 0;
+	dev->first_exp = MT9M114_DEFAULT_FIRST_EXP;
+
+	mt9m114_info = v4l2_get_subdev_hostdata(sd);
+	if (!mt9m114_info)
+		return -EINVAL;
+
+	res = v4l2_find_nearest_size(mt9m114_res,
+				     ARRAY_SIZE(mt9m114_res), width,
+				     height, fmt->width, fmt->height);
+	if (!res)
+		res = &mt9m114_res[N_RES - 1];
+
+	fmt->width = res->width;
+	fmt->height = res->height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		return 0;
+	}
+
+	switch (res->res) {
+	case MT9M114_RES_736P:
+		ret = mt9m114_write_reg_array(c, mt9m114_736P_init, NO_POLLING);
+		ret += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_R_MODE_MASK, MISENSOR_NORMAL_SET);
+		break;
+	case MT9M114_RES_864P:
+		ret = mt9m114_write_reg_array(c, mt9m114_864P_init, NO_POLLING);
+		ret += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_R_MODE_MASK, MISENSOR_NORMAL_SET);
+		break;
+	case MT9M114_RES_960P:
+		ret = mt9m114_write_reg_array(c, mt9m114_976P_init, NO_POLLING);
+		/* set sensor read_mode to Normal */
+		ret += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_R_MODE_MASK, MISENSOR_NORMAL_SET);
+		break;
+	default:
+		v4l2_err(sd, "set resolution: %d failed!\n", res->res);
+		return -EINVAL;
+	}
+
+	if (ret)
+		return -EINVAL;
+
+	ret = mt9m114_write_reg_array(c, mt9m114_chgstat_reg, POST_POLLING);
+	if (ret < 0)
+		return ret;
+
+	if (mt9m114_set_suspend(sd))
+		return -EINVAL;
+
+	if (dev->res != res->res) {
+		int index;
+
+		/* Switch to different size */
+		if (width <= 640) {
+			dev->nctx = 0x00; /* Set for context A */
+		} else {
+			/*
+			 * Context B is used for resolutions larger than 640x480
+			 * Using YUV for Context B.
+			 */
+			dev->nctx = 0x01; /* set for context B */
+		}
+
+		/*
+		 * Marked current sensor res as being "used"
+		 *
+		 * REVISIT: We don't need to use an "used" field on each mode
+		 * list entry to know which mode is selected. If this
+		 * information is really necessary, how about to use a single
+		 * variable on sensor dev struct?
+		 */
+		for (index = 0; index < N_RES; index++) {
+			if ((width == mt9m114_res[index].width) &&
+			    (height == mt9m114_res[index].height)) {
+				mt9m114_res[index].used = true;
+				continue;
+			}
+			mt9m114_res[index].used = false;
+		}
+	}
+	ret = mt9m114_get_intg_factor(c, mt9m114_info,
+				      &mt9m114_res[res->res]);
+	if (ret) {
+		dev_err(&c->dev, "failed to get integration_factor\n");
+		return -EINVAL;
+	}
+	/*
+	 * mt9m114 - we don't poll for context switch
+	 * because it does not happen with streaming disabled.
+	 */
+	dev->res = res->res;
+
+	fmt->width = width;
+	fmt->height = height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	return 0;
+}
+
+/* TODO: Update to SOC functions, remove exposure and gain */
+static int mt9m114_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (MT9M114_FOCAL_LENGTH_NUM << 16) | MT9M114_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int mt9m114_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/* const f number for mt9m114 */
+	*val = (MT9M114_F_NUMBER_DEFAULT_NUM << 16) | MT9M114_F_NUMBER_DEM;
+	return 0;
+}
+
+static int mt9m114_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (MT9M114_F_NUMBER_DEFAULT_NUM << 24) |
+	       (MT9M114_F_NUMBER_DEM << 16) |
+	       (MT9M114_F_NUMBER_DEFAULT_NUM << 8) | MT9M114_F_NUMBER_DEM;
+	return 0;
+}
+
+/* Horizontal flip the image. */
+static int mt9m114_g_hflip(struct v4l2_subdev *sd, s32 *val)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int ret;
+	u32 data;
+
+	ret = mt9m114_read_reg(c, MISENSOR_16BIT,
+			       (u32)MISENSOR_READ_MODE, &data);
+	if (ret)
+		return ret;
+	*val = !!(data & MISENSOR_HFLIP_MASK);
+
+	return 0;
+}
+
+static int mt9m114_g_vflip(struct v4l2_subdev *sd, s32 *val)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int ret;
+	u32 data;
+
+	ret = mt9m114_read_reg(c, MISENSOR_16BIT,
+			       (u32)MISENSOR_READ_MODE, &data);
+	if (ret)
+		return ret;
+	*val = !!(data & MISENSOR_VFLIP_MASK);
+
+	return 0;
+}
+
+static long mt9m114_s_exposure(struct v4l2_subdev *sd,
+			       struct atomisp_exposure *exposure)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	int ret = 0;
+	unsigned int coarse_integration = 0;
+	unsigned int f_lines = 0;
+	unsigned int frame_len_lines = 0; /* ExposureTime.FrameLengthLines; */
+	unsigned int analog_gain, digital_gain;
+	u32 analog_gain_to_write = 0;
+
+	dev_dbg(&client->dev, "%s(0x%X 0x%X 0x%X)\n", __func__,
+		exposure->integration_time[0], exposure->gain[0],
+		exposure->gain[1]);
+
+	coarse_integration = exposure->integration_time[0];
+	/*
+	 * fine_integration = ExposureTime.FineIntegrationTime;
+	 * frame_len_lines = ExposureTime.FrameLengthLines;
+	 */
+	f_lines = mt9m114_res[dev->res].lines_per_frame;
+	analog_gain = exposure->gain[0];
+	digital_gain = exposure->gain[1];
+	if (!dev->streamon) {
+		/*Save the first exposure values while stream is off*/
+		dev->first_exp = coarse_integration;
+		dev->first_gain = analog_gain;
+		dev->first_diggain = digital_gain;
+	}
+	/* digital_gain = 0x400 * (((u16) digital_gain) >> 8) +		*/
+	/* ((unsigned int)(0x400 * (((u16) digital_gain) & 0xFF)) >>8); */
+
+	/* set frame length */
+	if (f_lines < coarse_integration + 6)
+		f_lines = coarse_integration + 6;
+	if (f_lines < frame_len_lines)
+		f_lines = frame_len_lines;
+	ret = mt9m114_write_reg(client, MISENSOR_16BIT, 0x300A, f_lines);
+	if (ret) {
+		v4l2_err(client, "%s: fail to set f_lines\n", __func__);
+		return -EINVAL;
+	}
+
+	/* set coarse integration */
+	/*
+	 * 3A provide real exposure time.
+	 * should not translate to any value here.
+	 */
+	ret = mt9m114_write_reg(client, MISENSOR_16BIT,
+				REG_EXPO_COARSE, (u16)(coarse_integration));
+	if (ret) {
+		v4l2_err(client, "%s: fail to set exposure time\n", __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * set analog/digital gain
+	switch(analog_gain)
+	{
+	case 0:
+	  analog_gain_to_write = 0x0;
+	  break;
+	case 1:
+	  analog_gain_to_write = 0x20;
+	  break;
+	case 2:
+	  analog_gain_to_write = 0x60;
+	  break;
+	case 4:
+	  analog_gain_to_write = 0xA0;
+	  break;
+	case 8:
+	  analog_gain_to_write = 0xE0;
+	  break;
+	default:
+	  analog_gain_to_write = 0x20;
+	  break;
+	}
+	*/
+	if (digital_gain >= 16 || digital_gain <= 1)
+		digital_gain = 1;
+	/*
+	 * analog_gain_to_write = (u16)((digital_gain << 12)
+	 *				| analog_gain_to_write);
+	 */
+	analog_gain_to_write = (u16)((digital_gain << 12) | (u16)analog_gain);
+	ret = mt9m114_write_reg(client, MISENSOR_16BIT,
+				REG_GAIN, analog_gain_to_write);
+	if (ret) {
+		v4l2_err(client, "%s: fail to set analog_gain_to_write\n",
+			 __func__);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static long mt9m114_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return mt9m114_s_exposure(sd, arg);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int mt9m114_g_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 coarse;
+	int ret;
+
+	/* the fine integration time is currently not calculated */
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT,
+			       REG_EXPO_COARSE, &coarse);
+	if (ret)
+		return ret;
+
+	*value = coarse;
+	return 0;
+}
+
+/*
+ * This function will return the sensor supported max exposure zone number.
+ * the sensor which supports max exposure zone number is 1.
+ */
+static int mt9m114_g_exposure_zone_num(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = 1;
+
+	return 0;
+}
+
+/*
+ * set exposure metering, average/center_weighted/spot/matrix.
+ */
+static int mt9m114_s_exposure_metering(struct v4l2_subdev *sd, s32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	switch (val) {
+	case V4L2_EXPOSURE_METERING_SPOT:
+		ret = mt9m114_write_reg_array(client, mt9m114_exp_average,
+					      NO_POLLING);
+		if (ret) {
+			dev_err(&client->dev, "write exp_average reg err.\n");
+			return ret;
+		}
+		break;
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+	default:
+		ret = mt9m114_write_reg_array(client, mt9m114_exp_center,
+					      NO_POLLING);
+		if (ret) {
+			dev_err(&client->dev, "write exp_default reg err");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * This function is for touch exposure feature.
+ */
+static int mt9m114_s_exposure_selection(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *sd_state,
+					struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct misensor_reg exp_reg;
+	int width, height;
+	int grid_width, grid_height;
+	int grid_left, grid_top, grid_right, grid_bottom;
+	int win_left, win_top, win_right, win_bottom;
+	int i, j;
+	int ret;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_TRY &&
+	    sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	grid_left = sel->r.left;
+	grid_top = sel->r.top;
+	grid_right = sel->r.left + sel->r.width - 1;
+	grid_bottom = sel->r.top + sel->r.height - 1;
+
+	ret = mt9m114_res2size(sd, &width, &height);
+	if (ret)
+		return ret;
+
+	grid_width = width / 5;
+	grid_height = height / 5;
+
+	if (grid_width && grid_height) {
+		win_left = grid_left / grid_width;
+		win_top = grid_top / grid_height;
+		win_right = grid_right / grid_width;
+		win_bottom = grid_bottom / grid_height;
+	} else {
+		dev_err(&client->dev, "Incorrect exp grid.\n");
+		return -EINVAL;
+	}
+
+	win_left   = clamp_t(int, win_left, 0, 4);
+	win_top    = clamp_t(int, win_top, 0, 4);
+	win_right  = clamp_t(int, win_right, 0, 4);
+	win_bottom = clamp_t(int, win_bottom, 0, 4);
+
+	ret = mt9m114_write_reg_array(client, mt9m114_exp_average, NO_POLLING);
+	if (ret) {
+		dev_err(&client->dev, "write exp_average reg err.\n");
+		return ret;
+	}
+
+	for (i = win_top; i <= win_bottom; i++) {
+		for (j = win_left; j <= win_right; j++) {
+			exp_reg = mt9m114_exp_win[i][j];
+
+			ret = mt9m114_write_reg(client, exp_reg.length,
+						exp_reg.reg, exp_reg.val);
+			if (ret) {
+				dev_err(&client->dev, "write exp_reg err.\n");
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int mt9m114_g_bin_factor_x(struct v4l2_subdev *sd, s32 *val)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+
+	*val = mt9m114_res[dev->res].bin_factor_x;
+
+	return 0;
+}
+
+static int mt9m114_g_bin_factor_y(struct v4l2_subdev *sd, s32 *val)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+
+	*val = mt9m114_res[dev->res].bin_factor_y;
+
+	return 0;
+}
+
+static int mt9m114_s_ev(struct v4l2_subdev *sd, s32 val)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	s32 luma = 0x37;
+	int err;
+
+	/*
+	 * EV value only support -2 to 2
+	 * 0: 0x37, 1:0x47, 2:0x57, -1:0x27, -2:0x17
+	 */
+	if (val < -2 || val > 2)
+		return -EINVAL;
+	luma += 0x10 * val;
+	dev_dbg(&c->dev, "%s val:%d luma:0x%x\n", __func__, val, luma);
+	err = mt9m114_write_reg(c, MISENSOR_16BIT, 0x098E, 0xC87A);
+	if (err) {
+		dev_err(&c->dev, "%s logic addr access error\n", __func__);
+		return err;
+	}
+	err = mt9m114_write_reg(c, MISENSOR_8BIT, 0xC87A, (u32)luma);
+	if (err) {
+		dev_err(&c->dev, "%s write target_average_luma failed\n",
+			__func__);
+		return err;
+	}
+	udelay(10);
+
+	return 0;
+}
+
+static int mt9m114_g_ev(struct v4l2_subdev *sd, s32 *val)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int err;
+	u32 luma;
+
+	err = mt9m114_write_reg(c, MISENSOR_16BIT, 0x098E, 0xC87A);
+	if (err) {
+		dev_err(&c->dev, "%s logic addr access error\n", __func__);
+		return err;
+	}
+	err = mt9m114_read_reg(c, MISENSOR_8BIT, 0xC87A, &luma);
+	if (err) {
+		dev_err(&c->dev, "%s read target_average_luma failed\n",
+			__func__);
+		return err;
+	}
+	luma -= 0x17;
+	luma /= 0x10;
+	*val = (s32)luma - 2;
+	dev_dbg(&c->dev, "%s val:%d\n", __func__, *val);
+
+	return 0;
+}
+
+/*
+ * Fake interface
+ * mt9m114 now can not support 3a_lock
+ */
+static int mt9m114_s_3a_lock(struct v4l2_subdev *sd, s32 val)
+{
+	aaalock = val;
+	return 0;
+}
+
+static int mt9m114_g_3a_lock(struct v4l2_subdev *sd, s32 *val)
+{
+	if (aaalock)
+		return V4L2_LOCK_EXPOSURE | V4L2_LOCK_WHITE_BALANCE
+		       | V4L2_LOCK_FOCUS;
+	return 0;
+}
+
+static int mt9m114_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m114_device *dev =
+	    container_of(ctrl->handler, struct mt9m114_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		dev_dbg(&client->dev, "%s: CID_VFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = mt9m114_t_vflip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&client->dev, "%s: CID_HFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = mt9m114_t_hflip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_METERING:
+		ret = mt9m114_s_exposure_metering(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = mt9m114_s_ev(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_3A_LOCK:
+		ret = mt9m114_s_3a_lock(&dev->sd, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int mt9m114_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m114_device *dev =
+	    container_of(ctrl->handler, struct mt9m114_device, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		ret = mt9m114_g_vflip(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = mt9m114_g_hflip(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = mt9m114_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = mt9m114_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = mt9m114_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = mt9m114_g_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_ZONE_NUM:
+		ret = mt9m114_g_exposure_zone_num(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		ret = mt9m114_g_bin_factor_x(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_VERT:
+		ret = mt9m114_g_bin_factor_y(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = mt9m114_g_ev(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_3A_LOCK:
+		ret = mt9m114_g_3a_lock(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = mt9m114_s_ctrl,
+	.g_volatile_ctrl = mt9m114_g_volatile_ctrl
+};
+
+static struct v4l2_ctrl_config mt9m114_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VFLIP,
+		.name = "Image v-Flip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HFLIP,
+		.name = "Image h-Flip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.name = "focal length",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = MT9M114_FOCAL_LENGTH_DEFAULT,
+		.max = MT9M114_FOCAL_LENGTH_DEFAULT,
+		.step = 1,
+		.def = MT9M114_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.name = "f-number",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = MT9M114_F_NUMBER_DEFAULT,
+		.max = MT9M114_F_NUMBER_DEFAULT,
+		.step = 1,
+		.def = MT9M114_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.name = "f-number range",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = MT9M114_F_NUMBER_RANGE,
+		.max = MT9M114_F_NUMBER_RANGE,
+		.step = 1,
+		.def = MT9M114_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.name = "exposure",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 0xffff,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ZONE_NUM,
+		.name = "one-time exposure zone number",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 0xffff,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_METERING,
+		.name = "metering",
+		.type = V4L2_CTRL_TYPE_MENU,
+		.min = 0,
+		.max = 3,
+		.step = 0,
+		.def = 1,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.name = "horizontal binning factor",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = MT9M114_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.name = "vertical binning factor",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = MT9M114_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE,
+		.name = "exposure biasx",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = -2,
+		.max = 2,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_3A_LOCK,
+		.name = "3a lock",
+		.type = V4L2_CTRL_TYPE_BITMASK,
+		.min = 0,
+		.max = V4L2_LOCK_EXPOSURE | V4L2_LOCK_WHITE_BALANCE | V4L2_LOCK_FOCUS,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+};
+
+static int mt9m114_detect(struct mt9m114_device *dev, struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u32 model;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "%s: i2c error", __func__);
+		return -ENODEV;
+	}
+	ret = mt9m114_read_reg(client, MISENSOR_16BIT, MT9M114_PID, &model);
+	if (ret)
+		return ret;
+	dev->real_model_id = model;
+
+	if (model != MT9M114_MOD_ID) {
+		dev_err(&client->dev, "%s: failed: client->addr = %x\n",
+			__func__, client->addr);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int
+mt9m114_s_config(struct v4l2_subdev *sd, int irq, void *platform_data)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	ret = power_up(sd);
+	if (ret) {
+		v4l2_err(client, "mt9m114 power-up err");
+		return ret;
+	}
+
+	/* config & detect sensor */
+	ret = mt9m114_detect(dev, client);
+	if (ret) {
+		v4l2_err(client, "mt9m114_detect err s_config.\n");
+		goto fail_detect;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	ret = mt9m114_set_suspend(sd);
+	if (ret) {
+		v4l2_err(client, "mt9m114 suspend err");
+		return ret;
+	}
+
+	ret = power_down(sd);
+	if (ret) {
+		v4l2_err(client, "mt9m114 power down err");
+		return ret;
+	}
+
+	return ret;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_detect:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+	return ret;
+}
+
+/* Horizontal flip the image. */
+static int mt9m114_t_hflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	int err;
+	/* set for direct mode */
+	err = mt9m114_write_reg(c, MISENSOR_16BIT, 0x098E, 0xC850);
+	if (value) {
+		/* enable H flip ctx A */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC850, 0x01, 0x01);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC851, 0x01, 0x01);
+		/* ctx B */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC888, 0x01, 0x01);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC889, 0x01, 0x01);
+
+		err += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_HFLIP_MASK, MISENSOR_FLIP_EN);
+
+		dev->bpat = MT9M114_BPAT_GRGRBGBG;
+	} else {
+		/* disable H flip ctx A */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC850, 0x01, 0x00);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC851, 0x01, 0x00);
+		/* ctx B */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC888, 0x01, 0x00);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC889, 0x01, 0x00);
+
+		err += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_HFLIP_MASK, MISENSOR_FLIP_DIS);
+
+		dev->bpat = MT9M114_BPAT_BGBGGRGR;
+	}
+
+	err += mt9m114_write_reg(c, MISENSOR_8BIT, 0x8404, 0x06);
+	udelay(10);
+
+	return !!err;
+}
+
+/* Vertically flip the image */
+static int mt9m114_t_vflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int err;
+	/* set for direct mode */
+	err = mt9m114_write_reg(c, MISENSOR_16BIT, 0x098E, 0xC850);
+	if (value >= 1) {
+		/* enable H flip - ctx A */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC850, 0x02, 0x01);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC851, 0x02, 0x01);
+		/* ctx B */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC888, 0x02, 0x01);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC889, 0x02, 0x01);
+
+		err += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_VFLIP_MASK, MISENSOR_FLIP_EN);
+	} else {
+		/* disable H flip - ctx A */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC850, 0x02, 0x00);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC851, 0x02, 0x00);
+		/* ctx B */
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC888, 0x02, 0x00);
+		err += misensor_rmw_reg(c, MISENSOR_8BIT, 0xC889, 0x02, 0x00);
+
+		err += misensor_rmw_reg(c, MISENSOR_16BIT, MISENSOR_READ_MODE,
+					MISENSOR_VFLIP_MASK, MISENSOR_FLIP_DIS);
+	}
+
+	err += mt9m114_write_reg(c, MISENSOR_8BIT, 0x8404, 0x06);
+	udelay(10);
+
+	return !!err;
+}
+
+static int mt9m114_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *interval)
+{
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = mt9m114_res[dev->res].fps;
+
+	return 0;
+}
+
+static int mt9m114_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int ret;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct mt9m114_device *dev = to_mt9m114_sensor(sd);
+	struct atomisp_exposure exposure;
+
+	if (enable) {
+		ret = mt9m114_write_reg_array(c, mt9m114_chgstat_reg,
+					      POST_POLLING);
+		if (ret < 0)
+			return ret;
+
+		if (dev->first_exp > MT9M114_MAX_FIRST_EXP) {
+			exposure.integration_time[0] = dev->first_exp;
+			exposure.gain[0] = dev->first_gain;
+			exposure.gain[1] = dev->first_diggain;
+			mt9m114_s_exposure(sd, &exposure);
+		}
+		dev->streamon = 1;
+
+	} else {
+		dev->streamon = 0;
+		ret = mt9m114_set_suspend(sd);
+	}
+
+	return ret;
+}
+
+static int mt9m114_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index)
+		return -EINVAL;
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int mt9m114_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	unsigned int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = mt9m114_res[index].width;
+	fse->min_height = mt9m114_res[index].height;
+	fse->max_width = mt9m114_res[index].width;
+	fse->max_height = mt9m114_res[index].height;
+
+	return 0;
+}
+
+static int mt9m114_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	int index;
+	struct mt9m114_device *snr = to_mt9m114_sensor(sd);
+
+	if (!frames)
+		return -EINVAL;
+
+	for (index = 0; index < N_RES; index++) {
+		if (mt9m114_res[index].res == snr->res)
+			break;
+	}
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	*frames = mt9m114_res[index].skip_frames;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops mt9m114_video_ops = {
+	.s_stream = mt9m114_s_stream,
+	.g_frame_interval = mt9m114_g_frame_interval,
+};
+
+static const struct v4l2_subdev_sensor_ops mt9m114_sensor_ops = {
+	.g_skip_frames	= mt9m114_g_skip_frames,
+};
+
+static const struct v4l2_subdev_core_ops mt9m114_core_ops = {
+	.s_power = mt9m114_s_power,
+	.ioctl = mt9m114_ioctl,
+};
+
+/* REVISIT: Do we need pad operations? */
+static const struct v4l2_subdev_pad_ops mt9m114_pad_ops = {
+	.enum_mbus_code = mt9m114_enum_mbus_code,
+	.enum_frame_size = mt9m114_enum_frame_size,
+	.get_fmt = mt9m114_get_fmt,
+	.set_fmt = mt9m114_set_fmt,
+	.set_selection = mt9m114_s_exposure_selection,
+};
+
+static const struct v4l2_subdev_ops mt9m114_ops = {
+	.core = &mt9m114_core_ops,
+	.video = &mt9m114_video_ops,
+	.pad = &mt9m114_pad_ops,
+	.sensor = &mt9m114_sensor_ops,
+};
+
+static void mt9m114_remove(struct i2c_client *client)
+{
+	struct mt9m114_device *dev;
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	dev = container_of(sd, struct mt9m114_device, sd);
+	dev->platform_data->csi_cfg(sd, 0);
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+}
+
+static int mt9m114_probe(struct i2c_client *client)
+{
+	struct mt9m114_device *dev;
+	int ret = 0;
+	unsigned int i;
+	void *pdata;
+
+	/* Setup sensor configuration structure */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&dev->sd, client, &mt9m114_ops);
+	pdata = gmin_camera_platform_data(&dev->sd,
+					  ATOMISP_INPUT_FORMAT_RAW_10,
+					  atomisp_bayer_order_grbg);
+	if (pdata)
+		ret = mt9m114_s_config(&dev->sd, client->irq, pdata);
+	if (!pdata || ret) {
+		v4l2_device_unregister_subdev(&dev->sd);
+		kfree(dev);
+		return ret;
+	}
+
+	ret = atomisp_register_i2c_module(&dev->sd, pdata, RAW_CAMERA);
+	if (ret) {
+		v4l2_device_unregister_subdev(&dev->sd);
+		kfree(dev);
+		/* Coverity CID 298095 - return on error */
+		return ret;
+	}
+
+	/* TODO add format code here */
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret =
+	    v4l2_ctrl_handler_init(&dev->ctrl_handler,
+				   ARRAY_SIZE(mt9m114_controls));
+	if (ret) {
+		mt9m114_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mt9m114_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &mt9m114_controls[i],
+				     NULL);
+
+	if (dev->ctrl_handler.error) {
+		mt9m114_remove(client);
+		return dev->ctrl_handler.error;
+	}
+
+	/* Use same lock for controls as for everything else. */
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = &dev->ctrl_handler;
+
+	/* REVISIT: Do we need media controller? */
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret) {
+		mt9m114_remove(client);
+		return ret;
+	}
+	return 0;
+}
+
+static const struct acpi_device_id mt9m114_acpi_match[] = {
+	{ "INT33F0" },
+	{ "CRMT1040" },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, mt9m114_acpi_match);
+
+static struct i2c_driver mt9m114_driver = {
+	.driver = {
+		.name = "mt9m114",
+		.acpi_match_table = mt9m114_acpi_match,
+	},
+	.probe_new = mt9m114_probe,
+	.remove = mt9m114_remove,
+};
+module_i2c_driver(mt9m114_driver);
+
+MODULE_AUTHOR("Shuguang Gong <Shuguang.gong@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c b/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c
new file mode 100644
index 000000000..fa1de45b7
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c
@@ -0,0 +1,1242 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for OmniVision OV2680 1080p HD camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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 <asm/unaligned.h>
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/moduleparam.h>
+#include <media/v4l2-device.h>
+#include <linux/io.h>
+#include <linux/acpi.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+
+#include "ov2680.h"
+
+static int h_flag;
+static int v_flag;
+static enum atomisp_bayer_order ov2680_bayer_order_mapping[] = {
+	atomisp_bayer_order_bggr,
+	atomisp_bayer_order_grbg,
+	atomisp_bayer_order_gbrg,
+	atomisp_bayer_order_rggb,
+};
+
+/* i2c read/write stuff */
+static int ov2680_read_reg(struct i2c_client *client,
+			   int len, u16 reg, u32 *val)
+{
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
+	u8 data_buf[4] = { 0, };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs)) {
+		dev_err(&client->dev, "read error: reg=0x%4x: %d\n", reg, ret);
+		return -EIO;
+	}
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov2680_write_reg(struct i2c_client *client, unsigned int len,
+			    u16 reg, u16 val)
+{
+	u8 buf[6];
+	int ret;
+
+	if (len == 2)
+		put_unaligned_be16(val, buf + 2);
+	else if (len == 1)
+		buf[2] = val;
+	else
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2) {
+		dev_err(&client->dev, "write error %d reg 0x%04x, val 0x%02x: buf sent: %*ph\n",
+			ret, reg, val, len + 2, &buf);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int ov2680_write_reg_array(struct i2c_client *client,
+				  const struct ov2680_reg *reglist)
+{
+	const struct ov2680_reg *next = reglist;
+	int ret;
+
+	for (; next->reg != 0; next++) {
+		ret = ov2680_write_reg(client, 1, next->reg, next->val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov2680_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV2680_FOCAL_LENGTH_NUM << 16) | OV2680_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int ov2680_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/* const f number for ov2680 */
+
+	*val = (OV2680_F_NUMBER_DEFAULT_NUM << 16) | OV2680_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov2680_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV2680_F_NUMBER_DEFAULT_NUM << 24) |
+	       (OV2680_F_NUMBER_DEM << 16) |
+	       (OV2680_F_NUMBER_DEFAULT_NUM << 8) | OV2680_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov2680_g_bin_factor_x(struct v4l2_subdev *sd, s32 *val)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	dev_dbg(&client->dev,  "++++ov2680_g_bin_factor_x\n");
+	*val = dev->res->bin_factor_x;
+
+	return 0;
+}
+
+static int ov2680_g_bin_factor_y(struct v4l2_subdev *sd, s32 *val)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	*val = dev->res->bin_factor_y;
+	dev_dbg(&client->dev,  "++++ov2680_g_bin_factor_y\n");
+	return 0;
+}
+
+static int ov2680_get_intg_factor(struct i2c_client *client,
+				  struct camera_mipi_info *info,
+				  const struct ov2680_resolution *res)
+{
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	unsigned int pix_clk_freq_hz;
+	u32 reg_val;
+	int ret;
+
+	dev_dbg(&client->dev,  "++++ov2680_get_intg_factor\n");
+	if (!info)
+		return -EINVAL;
+
+	/* pixel clock */
+	pix_clk_freq_hz = res->pix_clk_freq * 1000000;
+
+	buf->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
+
+	/* get integration time */
+	buf->coarse_integration_time_min = OV2680_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    OV2680_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = OV2680_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    OV2680_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = OV2680_FINE_INTG_TIME_MIN;
+	buf->frame_length_lines = res->lines_per_frame;
+	buf->line_length_pck = res->pixels_per_line;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	ret =  ov2680_read_reg(client, 2,
+			       OV2680_HORIZONTAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = reg_val;
+
+	ret =  ov2680_read_reg(client, 2,
+			       OV2680_VERTICAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = reg_val;
+
+	ret = ov2680_read_reg(client, 2,
+			      OV2680_HORIZONTAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = reg_val;
+
+	ret = ov2680_read_reg(client, 2,
+			      OV2680_VERTICAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = reg_val;
+
+	ret = ov2680_read_reg(client, 2,
+			      OV2680_HORIZONTAL_OUTPUT_SIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = reg_val;
+
+	ret = ov2680_read_reg(client, 2,
+			      OV2680_VERTICAL_OUTPUT_SIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = reg_val;
+
+	buf->binning_factor_x = res->bin_factor_x ?
+				(res->bin_factor_x * 2) : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				(res->bin_factor_y * 2) : 1;
+	return 0;
+}
+
+static long __ov2680_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
+				  int gain, int digitgain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	u16 vts;
+	int ret, exp_val;
+
+	dev_dbg(&client->dev,
+		"+++++++__ov2680_set_exposure coarse_itg %d, gain %d, digitgain %d++\n",
+		coarse_itg, gain, digitgain);
+
+	vts = dev->res->lines_per_frame;
+
+	/* group hold */
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_GROUP_ACCESS, 0x00);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_GROUP_ACCESS);
+		return ret;
+	}
+
+	/* Increase the VTS to match exposure + MARGIN */
+	if (coarse_itg > vts - OV2680_INTEGRATION_TIME_MARGIN)
+		vts = (u16)coarse_itg + OV2680_INTEGRATION_TIME_MARGIN;
+
+	ret = ov2680_write_reg(client, 2, OV2680_TIMING_VTS_H, vts);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_TIMING_VTS_H);
+		return ret;
+	}
+
+	/* set exposure */
+
+	/* Lower four bit should be 0*/
+	exp_val = coarse_itg << 4;
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_EXPOSURE_L, exp_val & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_EXPOSURE_L);
+		return ret;
+	}
+
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_EXPOSURE_M, (exp_val >> 8) & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_EXPOSURE_M);
+		return ret;
+	}
+
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_EXPOSURE_H, (exp_val >> 16) & 0x0F);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_EXPOSURE_H);
+		return ret;
+	}
+
+	/* Analog gain */
+	ret = ov2680_write_reg(client, 2, OV2680_AGC_H, gain);
+	if (ret) {
+		dev_err(&client->dev, "%s: write 0x%02x: error, aborted\n",
+			__func__, OV2680_AGC_H);
+		return ret;
+	}
+	/* Digital gain */
+	if (digitgain) {
+		ret = ov2680_write_reg(client, 2,
+				       OV2680_MWB_RED_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev,
+				"%s: write 0x%02x: error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov2680_write_reg(client, 2,
+				       OV2680_MWB_GREEN_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev,
+				"%s: write 0x%02x: error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov2680_write_reg(client, 2,
+				       OV2680_MWB_BLUE_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev,
+				"%s: write 0x%02x: error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+	}
+
+	/* End group */
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_GROUP_ACCESS, 0x10);
+	if (ret)
+		return ret;
+
+	/* Delay launch group */
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_GROUP_ACCESS, 0xa0);
+	if (ret)
+		return ret;
+	return ret;
+}
+
+static int ov2680_set_exposure(struct v4l2_subdev *sd, int exposure,
+			       int gain, int digitgain)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	int ret = 0;
+
+	mutex_lock(&dev->input_lock);
+
+	dev->exposure = exposure;
+	dev->gain = gain;
+	dev->digitgain = digitgain;
+
+	if (dev->power_on)
+		ret = __ov2680_set_exposure(sd, exposure, gain, digitgain);
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long ov2680_s_exposure(struct v4l2_subdev *sd,
+			      struct atomisp_exposure *exposure)
+{
+	u16 coarse_itg = exposure->integration_time[0];
+	u16 analog_gain = exposure->gain[0];
+	u16 digital_gain = exposure->gain[1];
+
+	/* we should not accept the invalid value below */
+	if (analog_gain == 0) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		v4l2_err(client, "%s: invalid value\n", __func__);
+		return -EINVAL;
+	}
+
+	return ov2680_set_exposure(sd, coarse_itg, analog_gain, digital_gain);
+}
+
+static long ov2680_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return ov2680_s_exposure(sd, arg);
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int ov2680_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 reg_val;
+	int ret;
+
+	/* get exposure */
+	ret = ov2680_read_reg(client, 3, OV2680_EXPOSURE_H, &reg_val);
+	if (ret)
+		return ret;
+
+	/* Lower four bits are not part of the exposure val (always 0) */
+	*value = reg_val >> 4;
+	return 0;
+}
+
+static int ov2680_v_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct camera_mipi_info *ov2680_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u32 val;
+	u8 index;
+
+	dev_dbg(&client->dev, "@%s: value:%d\n", __func__, value);
+	ret = ov2680_read_reg(client, 1, OV2680_FLIP_REG, &val);
+	if (ret)
+		return ret;
+	if (value)
+		val |= OV2680_FLIP_MIRROR_BIT_ENABLE;
+	else
+		val &= ~OV2680_FLIP_MIRROR_BIT_ENABLE;
+
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_FLIP_REG, val);
+	if (ret)
+		return ret;
+	index = (v_flag > 0 ? OV2680_FLIP_BIT : 0) | (h_flag > 0 ? OV2680_MIRROR_BIT :
+		0);
+	ov2680_info = v4l2_get_subdev_hostdata(sd);
+	if (ov2680_info) {
+		ov2680_info->raw_bayer_order = ov2680_bayer_order_mapping[index];
+	}
+	return ret;
+}
+
+static int ov2680_h_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct camera_mipi_info *ov2680_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u32 val;
+	u8 index;
+
+	dev_dbg(&client->dev, "@%s: value:%d\n", __func__, value);
+
+	ret = ov2680_read_reg(client, 1, OV2680_MIRROR_REG, &val);
+	if (ret)
+		return ret;
+	if (value)
+		val |= OV2680_FLIP_MIRROR_BIT_ENABLE;
+	else
+		val &= ~OV2680_FLIP_MIRROR_BIT_ENABLE;
+
+	ret = ov2680_write_reg(client, 1,
+			       OV2680_MIRROR_REG, val);
+	if (ret)
+		return ret;
+	index = (v_flag > 0 ? OV2680_FLIP_BIT : 0) | (h_flag > 0 ? OV2680_MIRROR_BIT :
+		0);
+	ov2680_info = v4l2_get_subdev_hostdata(sd);
+	if (ov2680_info) {
+		ov2680_info->raw_bayer_order = ov2680_bayer_order_mapping[index];
+	}
+	return ret;
+}
+
+static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2680_device *dev =
+	    container_of(ctrl->handler, struct ov2680_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		dev_dbg(&client->dev, "%s: CID_VFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = ov2680_v_flip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&client->dev, "%s: CID_HFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = ov2680_h_flip(&dev->sd, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int ov2680_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2680_device *dev =
+	    container_of(ctrl->handler, struct ov2680_device, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = ov2680_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = ov2680_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = ov2680_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = ov2680_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		ret = ov2680_g_bin_factor_x(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_VERT:
+		ret = ov2680_g_bin_factor_y(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = ov2680_s_ctrl,
+	.g_volatile_ctrl = ov2680_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config ov2680_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.min = 0x0,
+		.max = 0xffff,
+		.step = 0x01,
+		.def = 0x00,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = OV2680_FOCAL_LENGTH_DEFAULT,
+		.max = OV2680_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = OV2680_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = OV2680_F_NUMBER_DEFAULT,
+		.max = OV2680_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = OV2680_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = OV2680_F_NUMBER_RANGE,
+		.max = OV2680_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = OV2680_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "horizontal binning factor",
+		.min = 0,
+		.max = OV2680_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vertical binning factor",
+		.min = 0,
+		.max = OV2680_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VFLIP,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Flip",
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HFLIP,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Mirror",
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+	},
+};
+
+static int ov2680_init_registers(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = ov2680_write_reg(client, 1, OV2680_SW_RESET, 0x01);
+	ret |= ov2680_write_reg_array(client, ov2680_global_setting);
+
+	return ret;
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = 0;
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	dev_dbg(&client->dev, "%s: %s", __func__, flag ? "on" : "off");
+
+	if (flag) {
+		ret |= dev->platform_data->v1p8_ctrl(sd, 1);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+	}
+
+	if (!flag || ret) {
+		ret |= dev->platform_data->v1p8_ctrl(sd, 0);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret;
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/*
+	 * The OV2680 documents only one GPIO input (#XSHUTDN), but
+	 * existing integrations often wire two (reset/power_down)
+	 * because that is the way other sensors work.  There is no
+	 * way to tell how it is wired internally, so existing
+	 * firmwares expose both and we drive them symmetrically.
+	 */
+	if (flag) {
+		ret = dev->platform_data->gpio0_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+		/* Ignore return from second gpio, it may not be there */
+		dev->platform_data->gpio1_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+	} else {
+		dev->platform_data->gpio1_ctrl(sd, 0);
+		ret = dev->platform_data->gpio0_ctrl(sd, 0);
+	}
+	return ret;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	if (dev->power_on)
+		return 0; /* Already on */
+
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* according to DS, at least 5ms is needed between DOVDD and PWDN */
+	usleep_range(5000, 6000);
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret) {
+		ret = gpio_ctrl(sd, 1);
+		if (ret)
+			goto fail_power;
+	}
+
+	/* flis clock control */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+
+	/* according to DS, 20ms is needed between PWDN and i2c access */
+	msleep(20);
+
+	ret = ov2680_init_registers(sd);
+	if (ret)
+		goto fail_init_registers;
+
+	ret = __ov2680_set_exposure(sd, dev->exposure, dev->gain, dev->digitgain);
+	if (ret)
+		goto fail_init_registers;
+
+	dev->power_on = true;
+	return 0;
+
+fail_init_registers:
+	dev->platform_data->flisclk_ctrl(sd, 0);
+fail_clk:
+	gpio_ctrl(sd, 0);
+fail_power:
+	power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	h_flag = 0;
+	v_flag = 0;
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	if (!dev->power_on)
+		return 0; /* Already off */
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			dev_err(&client->dev, "gpio failed 2\n");
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret) {
+		dev_err(&client->dev, "vprog failed.\n");
+		return ret;
+	}
+
+	dev->power_on = false;
+	return 0;
+}
+
+static int ov2680_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	if (on == 0) {
+		ret = power_down(sd);
+	} else {
+		ret = power_up(sd);
+	}
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ov2680_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *ov2680_info = NULL;
+	struct ov2680_resolution *res;
+	int vts, ret = 0;
+
+	dev_dbg(&client->dev, "%s: %s: pad: %d, fmt: %p\n",
+		__func__,
+		(format->which == V4L2_SUBDEV_FORMAT_TRY) ? "try" : "set",
+		format->pad, fmt);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	ov2680_info = v4l2_get_subdev_hostdata(sd);
+	if (!ov2680_info)
+		return -EINVAL;
+
+	res = v4l2_find_nearest_size(ov2680_res_preview,
+				     ARRAY_SIZE(ov2680_res_preview), width,
+				     height, fmt->width, fmt->height);
+	if (!res)
+		res = &ov2680_res_preview[N_RES_PREVIEW - 1];
+
+	fmt->width = res->width;
+	fmt->height = res->height;
+
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		return 0;
+	}
+
+	dev_dbg(&client->dev, "%s: %dx%d\n",
+		__func__, fmt->width, fmt->height);
+
+	mutex_lock(&dev->input_lock);
+
+	/* s_power has not been called yet for std v4l2 clients (camorama) */
+	power_up(sd);
+	ret = ov2680_write_reg_array(client, dev->res->regs);
+	if (ret) {
+		dev_err(&client->dev,
+			"ov2680 write resolution register err: %d\n", ret);
+		goto err;
+	}
+
+	vts = dev->res->lines_per_frame;
+
+	/* If necessary increase the VTS to match exposure + MARGIN */
+	if (dev->exposure > vts - OV2680_INTEGRATION_TIME_MARGIN)
+		vts = dev->exposure + OV2680_INTEGRATION_TIME_MARGIN;
+
+	ret = ov2680_write_reg(client, 2, OV2680_TIMING_VTS_H, vts);
+	if (ret) {
+		dev_err(&client->dev, "ov2680 write vts err: %d\n", ret);
+		goto err;
+	}
+
+	ret = ov2680_get_intg_factor(client, ov2680_info, res);
+	if (ret) {
+		dev_err(&client->dev, "failed to get integration factor\n");
+		goto err;
+	}
+
+	/*
+	 * recall flip functions to avoid flip registers
+	 * were overridden by default setting
+	 */
+	if (h_flag)
+		ov2680_h_flip(sd, h_flag);
+	if (v_flag)
+		ov2680_v_flip(sd, v_flag);
+
+	dev->res = res;
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2680_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	fmt->width = dev->res->width;
+	fmt->height = dev->res->height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov2680_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u32 high, low;
+	int ret;
+	u16 id;
+	u8 revision;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ret = ov2680_read_reg(client, 1,
+			      OV2680_SC_CMMN_CHIP_ID_H, &high);
+	if (ret) {
+		dev_err(&client->dev, "sensor_id_high = 0x%x\n", high);
+		return -ENODEV;
+	}
+	ret = ov2680_read_reg(client, 1,
+			      OV2680_SC_CMMN_CHIP_ID_L, &low);
+	id = ((((u16)high) << 8) | (u16)low);
+
+	if (id != OV2680_ID) {
+		dev_err(&client->dev, "sensor ID error 0x%x\n", id);
+		return -ENODEV;
+	}
+
+	ret = ov2680_read_reg(client, 1,
+			      OV2680_SC_CMMN_SUB_ID, &high);
+	revision = (u8)high & 0x0f;
+
+	dev_info(&client->dev, "sensor_revision id = 0x%x, rev= %d\n",
+		 id, revision);
+
+	return 0;
+}
+
+static int ov2680_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	if (enable)
+		dev_dbg(&client->dev, "ov2680_s_stream one\n");
+	else
+		dev_dbg(&client->dev, "ov2680_s_stream off\n");
+
+	ret = ov2680_write_reg(client, 1, OV2680_SW_STREAM,
+			       enable ? OV2680_START_STREAMING :
+			       OV2680_STOP_STREAMING);
+
+	//otp valid at stream on state
+	//if(!dev->otp_data)
+	//	dev->otp_data = ov2680_otp_read(sd);
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ov2680_s_config(struct v4l2_subdev *sd,
+			   int irq, void *platform_data)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+
+	ret = power_up(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov2680 power-up err.\n");
+		goto fail_power_on;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = ov2680_detect(client);
+	if (ret) {
+		dev_err(&client->dev, "ov2680_detect err s_config.\n");
+		goto fail_csi_cfg;
+	}
+
+	/* turn off sensor, after probed */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov2680 power-off err.\n");
+		goto fail_csi_cfg;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_power_on:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2680_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = dev->res->fps;
+
+	return 0;
+}
+
+static int ov2680_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int ov2680_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES_PREVIEW)
+		return -EINVAL;
+
+	fse->min_width = ov2680_res_preview[index].width;
+	fse->min_height = ov2680_res_preview[index].height;
+	fse->max_width = ov2680_res_preview[index].width;
+	fse->max_height = ov2680_res_preview[index].height;
+
+	return 0;
+}
+
+static int ov2680_enum_frame_interval(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct v4l2_fract fract;
+
+	if (fie->index >= N_RES_PREVIEW ||
+	    fie->width > ov2680_res_preview[0].width ||
+	    fie->height > ov2680_res_preview[0].height ||
+	    fie->which > V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	fract.denominator = ov2680_res_preview[fie->index].fps;
+	fract.numerator = 1;
+
+	fie->interval = fract;
+
+	return 0;
+}
+
+static int ov2680_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->res->skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov2680_video_ops = {
+	.s_stream = ov2680_s_stream,
+	.g_frame_interval = ov2680_g_frame_interval,
+};
+
+static const struct v4l2_subdev_sensor_ops ov2680_sensor_ops = {
+	.g_skip_frames	= ov2680_g_skip_frames,
+};
+
+static const struct v4l2_subdev_core_ops ov2680_core_ops = {
+	.s_power = ov2680_s_power,
+	.ioctl = ov2680_ioctl,
+};
+
+static const struct v4l2_subdev_pad_ops ov2680_pad_ops = {
+	.enum_mbus_code = ov2680_enum_mbus_code,
+	.enum_frame_size = ov2680_enum_frame_size,
+	.enum_frame_interval = ov2680_enum_frame_interval,
+	.get_fmt = ov2680_get_fmt,
+	.set_fmt = ov2680_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov2680_ops = {
+	.core = &ov2680_core_ops,
+	.video = &ov2680_video_ops,
+	.pad = &ov2680_pad_ops,
+	.sensor = &ov2680_sensor_ops,
+};
+
+static void ov2680_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	dev_dbg(&client->dev, "ov2680_remove...\n");
+
+	dev->platform_data->csi_cfg(sd, 0);
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+}
+
+static int ov2680_probe(struct i2c_client *client)
+{
+	struct ov2680_device *dev;
+	int ret;
+	void *pdata;
+	unsigned int i;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	mutex_init(&dev->input_lock);
+
+	dev->res = &ov2680_res_preview[0];
+	dev->exposure = dev->res->lines_per_frame - OV2680_INTEGRATION_TIME_MARGIN;
+	dev->gain = 250; /* 0-2047 */
+	v4l2_i2c_subdev_init(&dev->sd, client, &ov2680_ops);
+
+	pdata = gmin_camera_platform_data(&dev->sd,
+					  ATOMISP_INPUT_FORMAT_RAW_10,
+					  atomisp_bayer_order_bggr);
+	if (!pdata) {
+		ret = -EINVAL;
+		goto out_free;
+	}
+
+	ret = ov2680_s_config(&dev->sd, client->irq, pdata);
+	if (ret)
+		goto out_free;
+
+	ret = atomisp_register_i2c_module(&dev->sd, pdata, RAW_CAMERA);
+	if (ret)
+		goto out_free;
+
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret =
+	    v4l2_ctrl_handler_init(&dev->ctrl_handler,
+				   ARRAY_SIZE(ov2680_controls));
+	if (ret) {
+		ov2680_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ov2680_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &ov2680_controls[i],
+				     NULL);
+
+	if (dev->ctrl_handler.error) {
+		ov2680_remove(client);
+		return dev->ctrl_handler.error;
+	}
+
+	/* Use same lock for controls as for everything else. */
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = &dev->ctrl_handler;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret) {
+		ov2680_remove(client);
+		dev_dbg(&client->dev, "+++ remove ov2680\n");
+	}
+	return ret;
+out_free:
+	dev_dbg(&client->dev, "+++ out free\n");
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static const struct acpi_device_id ov2680_acpi_match[] = {
+	{"XXOV2680"},
+	{"OVTI2680"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov2680_acpi_match);
+
+static struct i2c_driver ov2680_driver = {
+	.driver = {
+		.name = "ov2680",
+		.acpi_match_table = ov2680_acpi_match,
+	},
+	.probe_new = ov2680_probe,
+	.remove = ov2680_remove,
+};
+module_i2c_driver(ov2680_driver);
+
+MODULE_AUTHOR("Jacky Wang <Jacky_wang@ovt.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 2680 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/atomisp-ov2722.c b/drivers/staging/media/atomisp/i2c/atomisp-ov2722.c
new file mode 100644
index 000000000..887b6f99f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/atomisp-ov2722.c
@@ -0,0 +1,1194 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for OmniVision OV2722 1080p HD camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/moduleparam.h>
+#include <media/v4l2-device.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+#include <linux/acpi.h>
+#include <linux/io.h>
+
+#include "ov2722.h"
+
+/* i2c read/write stuff */
+static int ov2722_read_reg(struct i2c_client *client,
+			   u16 data_length, u16 reg, u16 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[6];
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no client->adapter\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	if (data_length != OV2722_8BIT && data_length != OV2722_16BIT &&
+	    data_length != OV2722_32BIT) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(msg, 0, sizeof(msg));
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = I2C_MSG_LENGTH;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8)(reg >> 8);
+	data[1] = (u8)(reg & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].len = data_length;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		dev_err(&client->dev,
+			"read from offset 0x%x error %d", reg, err);
+		return err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	if (data_length == OV2722_8BIT)
+		*val = (u8)data[0];
+	else if (data_length == OV2722_16BIT)
+		*val = be16_to_cpu(*(__be16 *)&data[0]);
+	else
+		*val = be32_to_cpu(*(__be32 *)&data[0]);
+
+	return 0;
+}
+
+static int ov2722_i2c_write(struct i2c_client *client, u16 len, u8 *data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+static int ov2722_write_reg(struct i2c_client *client, u16 data_length,
+			    u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	__be16 *wreg = (__be16 *)data;
+	const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
+
+	if (data_length != OV2722_8BIT && data_length != OV2722_16BIT) {
+		dev_err(&client->dev,
+			"%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == OV2722_8BIT) {
+		data[2] = (u8)(val);
+	} else {
+		/* OV2722_16BIT */
+		__be16 *wdata = (__be16 *)&data[2];
+
+		*wdata = cpu_to_be16(val);
+	}
+
+	ret = ov2722_i2c_write(client, len, data);
+	if (ret)
+		dev_err(&client->dev,
+			"write error: wrote 0x%x to offset 0x%x error %d",
+			val, reg, ret);
+
+	return ret;
+}
+
+/*
+ * ov2722_write_reg_array - Initializes a list of OV2722 registers
+ * @client: i2c driver client structure
+ * @reglist: list of registers to be written
+ *
+ * This function initializes a list of registers. When consecutive addresses
+ * are found in a row on the list, this function creates a buffer and sends
+ * consecutive data in a single i2c_transfer().
+ *
+ * __ov2722_flush_reg_array, __ov2722_buf_reg_array() and
+ * __ov2722_write_reg_is_consecutive() are internal functions to
+ * ov2722_write_reg_array_fast() and should be not used anywhere else.
+ *
+ */
+
+static int __ov2722_flush_reg_array(struct i2c_client *client,
+				    struct ov2722_write_ctrl *ctrl)
+{
+	u16 size;
+	__be16 *data16 = (void *)&ctrl->buffer.addr;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	*data16 = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return ov2722_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __ov2722_buf_reg_array(struct i2c_client *client,
+				  struct ov2722_write_ctrl *ctrl,
+				  const struct ov2722_reg *next)
+{
+	int size;
+	__be16 *data16;
+
+	switch (next->type) {
+	case OV2722_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV2722_16BIT:
+		size = 2;
+		data16 = (void *)&ctrl->buffer.data[ctrl->index];
+		*data16 = cpu_to_be16((u16)next->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* When first item is added, we need to store its starting address */
+	if (ctrl->index == 0)
+		ctrl->buffer.addr = next->reg;
+
+	ctrl->index += size;
+
+	/*
+	 * Buffer cannot guarantee free space for u32? Better flush it to avoid
+	 * possible lack of memory for next item.
+	 */
+	if (ctrl->index + sizeof(u16) >= OV2722_MAX_WRITE_BUF_SIZE)
+		return __ov2722_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int __ov2722_write_reg_is_consecutive(struct i2c_client *client,
+					     struct ov2722_write_ctrl *ctrl,
+					     const struct ov2722_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+static int ov2722_write_reg_array(struct i2c_client *client,
+				  const struct ov2722_reg *reglist)
+{
+	const struct ov2722_reg *next = reglist;
+	struct ov2722_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != OV2722_TOK_TERM; next++) {
+		switch (next->type & OV2722_TOK_MASK) {
+		case OV2722_TOK_DELAY:
+			err = __ov2722_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			msleep(next->val);
+			break;
+		default:
+			/*
+			 * If next address is not consecutive, data needs to be
+			 * flushed before proceed.
+			 */
+			if (!__ov2722_write_reg_is_consecutive(client, &ctrl,
+							       next)) {
+				err = __ov2722_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __ov2722_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev, "%s: write error, aborted\n",
+					__func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __ov2722_flush_reg_array(client, &ctrl);
+}
+
+static int ov2722_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV2722_FOCAL_LENGTH_NUM << 16) | OV2722_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int ov2722_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/*const f number for imx*/
+	*val = (OV2722_F_NUMBER_DEFAULT_NUM << 16) | OV2722_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov2722_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV2722_F_NUMBER_DEFAULT_NUM << 24) |
+	       (OV2722_F_NUMBER_DEM << 16) |
+	       (OV2722_F_NUMBER_DEFAULT_NUM << 8) | OV2722_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov2722_get_intg_factor(struct i2c_client *client,
+				  struct camera_mipi_info *info,
+				  const struct ov2722_resolution *res)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2722_device *dev = NULL;
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	const unsigned int ext_clk_freq_hz = 19200000;
+	const unsigned int pll_invariant_div = 10;
+	unsigned int pix_clk_freq_hz;
+	u16 pre_pll_clk_div;
+	u16 pll_multiplier;
+	u16 op_pix_clk_div;
+	u16 reg_val;
+	int ret;
+
+	if (!info)
+		return -EINVAL;
+
+	dev = to_ov2722_sensor(sd);
+
+	/* pixel clock calculattion */
+	ret =  ov2722_read_reg(client, OV2722_8BIT,
+			       OV2722_SC_CMMN_PLL_CTRL3, &pre_pll_clk_div);
+	if (ret)
+		return ret;
+
+	ret =  ov2722_read_reg(client, OV2722_8BIT,
+			       OV2722_SC_CMMN_PLL_MULTIPLIER, &pll_multiplier);
+	if (ret)
+		return ret;
+
+	ret =  ov2722_read_reg(client, OV2722_8BIT,
+			       OV2722_SC_CMMN_PLL_DEBUG_OPT, &op_pix_clk_div);
+	if (ret)
+		return ret;
+
+	pre_pll_clk_div = (pre_pll_clk_div & 0x70) >> 4;
+	if (!pre_pll_clk_div)
+		return -EINVAL;
+
+	pll_multiplier = pll_multiplier & 0x7f;
+	op_pix_clk_div = op_pix_clk_div & 0x03;
+	pix_clk_freq_hz = ext_clk_freq_hz / pre_pll_clk_div * pll_multiplier
+			  * op_pix_clk_div / pll_invariant_div;
+
+	dev->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
+	buf->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
+
+	/* get integration time */
+	buf->coarse_integration_time_min = OV2722_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    OV2722_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = OV2722_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    OV2722_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = OV2722_FINE_INTG_TIME_MIN;
+	buf->frame_length_lines = res->lines_per_frame;
+	buf->line_length_pck = res->pixels_per_line;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	ret =  ov2722_read_reg(client, OV2722_16BIT,
+			       OV2722_H_CROP_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = reg_val;
+
+	ret =  ov2722_read_reg(client, OV2722_16BIT,
+			       OV2722_V_CROP_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = reg_val;
+
+	ret = ov2722_read_reg(client, OV2722_16BIT,
+			      OV2722_H_CROP_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = reg_val;
+
+	ret = ov2722_read_reg(client, OV2722_16BIT,
+			      OV2722_V_CROP_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = reg_val;
+
+	ret = ov2722_read_reg(client, OV2722_16BIT,
+			      OV2722_H_OUTSIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = reg_val;
+
+	ret = ov2722_read_reg(client, OV2722_16BIT,
+			      OV2722_V_OUTSIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = reg_val;
+
+	buf->binning_factor_x = res->bin_factor_x ?
+				res->bin_factor_x : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				res->bin_factor_y : 1;
+	return 0;
+}
+
+static long __ov2722_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
+				  int gain, int digitgain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	u16 hts, vts;
+	int ret;
+
+	dev_dbg(&client->dev, "set_exposure without group hold\n");
+
+	/* clear VTS_DIFF on manual mode */
+	ret = ov2722_write_reg(client, OV2722_16BIT, OV2722_VTS_DIFF_H, 0);
+	if (ret)
+		return ret;
+
+	hts = dev->pixels_per_line;
+	vts = dev->lines_per_frame;
+
+	if ((coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN) > vts)
+		vts = coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN;
+
+	coarse_itg <<= 4;
+	digitgain <<= 2;
+
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_VTS_H, vts);
+	if (ret)
+		return ret;
+
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_HTS_H, hts);
+	if (ret)
+		return ret;
+
+	/* set exposure */
+	ret = ov2722_write_reg(client, OV2722_8BIT,
+			       OV2722_AEC_PK_EXPO_L,
+			       coarse_itg & 0xff);
+	if (ret)
+		return ret;
+
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_AEC_PK_EXPO_H,
+			       (coarse_itg >> 8) & 0xfff);
+	if (ret)
+		return ret;
+
+	/* set analog gain */
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_AGC_ADJ_H, gain);
+	if (ret)
+		return ret;
+
+	/* set digital gain */
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_MWB_GAIN_R_H, digitgain);
+	if (ret)
+		return ret;
+
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_MWB_GAIN_G_H, digitgain);
+	if (ret)
+		return ret;
+
+	ret = ov2722_write_reg(client, OV2722_16BIT,
+			       OV2722_MWB_GAIN_B_H, digitgain);
+
+	return ret;
+}
+
+static int ov2722_set_exposure(struct v4l2_subdev *sd, int exposure,
+			       int gain, int digitgain)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __ov2722_set_exposure(sd, exposure, gain, digitgain);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long ov2722_s_exposure(struct v4l2_subdev *sd,
+			      struct atomisp_exposure *exposure)
+{
+	int exp = exposure->integration_time[0];
+	int gain = exposure->gain[0];
+	int digitgain = exposure->gain[1];
+
+	/* we should not accept the invalid value below. */
+	if (gain == 0) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		v4l2_err(client, "%s: invalid value\n", __func__);
+		return -EINVAL;
+	}
+
+	return ov2722_set_exposure(sd, exp, gain, digitgain);
+}
+
+static long ov2722_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return ov2722_s_exposure(sd, arg);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int ov2722_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 reg_v, reg_v2;
+	int ret;
+
+	/* get exposure */
+	ret = ov2722_read_reg(client, OV2722_8BIT,
+			      OV2722_AEC_PK_EXPO_L,
+			      &reg_v);
+	if (ret)
+		goto err;
+
+	ret = ov2722_read_reg(client, OV2722_8BIT,
+			      OV2722_AEC_PK_EXPO_M,
+			      &reg_v2);
+	if (ret)
+		goto err;
+
+	reg_v += reg_v2 << 8;
+	ret = ov2722_read_reg(client, OV2722_8BIT,
+			      OV2722_AEC_PK_EXPO_H,
+			      &reg_v2);
+	if (ret)
+		goto err;
+
+	*value = reg_v + (((u32)reg_v2 << 16));
+err:
+	return ret;
+}
+
+static int ov2722_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2722_device *dev =
+	    container_of(ctrl->handler, struct ov2722_device, ctrl_handler);
+	int ret = 0;
+	unsigned int val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = ov2722_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = ov2722_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = ov2722_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = ov2722_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_LINK_FREQ:
+		val = dev->res->mipi_freq;
+		if (val == 0)
+			return -EINVAL;
+
+		ctrl->val = val * 1000;	/* To Hz */
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.g_volatile_ctrl = ov2722_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config ov2722_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.min = 0x0,
+		.max = 0xffff,
+		.step = 0x01,
+		.def = 0x00,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = OV2722_FOCAL_LENGTH_DEFAULT,
+		.max = OV2722_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = OV2722_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = OV2722_F_NUMBER_DEFAULT,
+		.max = OV2722_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = OV2722_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = OV2722_F_NUMBER_RANGE,
+		.max = OV2722_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = OV2722_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_LINK_FREQ,
+		.name = "Link Frequency",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 1,
+		.max = 1500000 * 1000,
+		.step = 1,
+		.def = 1,
+		.flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_READ_ONLY,
+	},
+};
+
+static int ov2722_init(struct v4l2_subdev *sd)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+
+	/* restore settings */
+	ov2722_res = ov2722_res_preview;
+	N_RES = N_RES_PREVIEW;
+
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = -1;
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	if (flag) {
+		ret = dev->platform_data->v1p8_ctrl(sd, 1);
+		if (ret == 0) {
+			ret = dev->platform_data->v2p8_ctrl(sd, 1);
+			if (ret)
+				dev->platform_data->v1p8_ctrl(sd, 0);
+		}
+	} else {
+		ret = dev->platform_data->v1p8_ctrl(sd, 0);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+	}
+
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	int ret = -1;
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Note: the GPIO order is asymmetric: always RESET#
+	 * before PWDN# when turning it on or off.
+	 */
+	ret = dev->platform_data->gpio0_ctrl(sd, flag);
+	/*
+	 *ov2722 PWDN# active high when pull down,opposite to the convention
+	 */
+	ret |= dev->platform_data->gpio1_ctrl(sd, !flag);
+	return ret;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* according to DS, at least 5ms is needed between DOVDD and PWDN */
+	usleep_range(5000, 6000);
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			goto fail_power;
+	}
+
+	/* flis clock control */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+
+	/* according to DS, 20ms is needed between PWDN and i2c access */
+	msleep(20);
+
+	return 0;
+
+fail_clk:
+	gpio_ctrl(sd, 0);
+fail_power:
+	power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			dev_err(&client->dev, "gpio failed 2\n");
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	return ret;
+}
+
+static int ov2722_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	if (on == 0)
+		return power_down(sd);
+
+	ret = power_up(sd);
+	if (!ret)
+		return ov2722_init(sd);
+
+	return ret;
+}
+
+/* TODO: remove it. */
+static int startup(struct v4l2_subdev *sd)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	ret = ov2722_write_reg(client, OV2722_8BIT,
+			       OV2722_SW_RESET, 0x01);
+	if (ret) {
+		dev_err(&client->dev, "ov2722 reset err.\n");
+		return ret;
+	}
+
+	ret = ov2722_write_reg_array(client, dev->res->regs);
+	if (ret) {
+		dev_err(&client->dev, "ov2722 write register err.\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int ov2722_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2722_resolution *res;
+	struct camera_mipi_info *ov2722_info = NULL;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+	ov2722_info = v4l2_get_subdev_hostdata(sd);
+	if (!ov2722_info)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	res = v4l2_find_nearest_size(ov2722_res_preview,
+				     ARRAY_SIZE(ov2722_res_preview), width,
+				     height, fmt->width, fmt->height);
+	if (!res)
+		res = &ov2722_res_preview[N_RES - 1];
+
+	fmt->width = res->width;
+	fmt->height = res->height;
+	dev->res = res;
+
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+	}
+
+
+	dev->pixels_per_line = dev->res->pixels_per_line;
+	dev->lines_per_frame = dev->res->lines_per_frame;
+
+	ret = startup(sd);
+	if (ret) {
+		int i = 0;
+
+		dev_err(&client->dev, "ov2722 startup err, retry to power up\n");
+		for (i = 0; i < OV2722_POWER_UP_RETRY_NUM; i++) {
+			dev_err(&client->dev,
+				"ov2722 retry to power up %d/%d times, result: ",
+				i + 1, OV2722_POWER_UP_RETRY_NUM);
+			power_down(sd);
+			ret = power_up(sd);
+			if (ret) {
+				dev_err(&client->dev, "power up failed, continue\n");
+				continue;
+			}
+			ret = startup(sd);
+			if (ret) {
+				dev_err(&client->dev, " startup FAILED!\n");
+			} else {
+				dev_err(&client->dev, " startup SUCCESS!\n");
+				break;
+			}
+		}
+		if (ret) {
+			dev_err(&client->dev, "ov2722 startup err\n");
+			goto err;
+		}
+	}
+
+	ret = ov2722_get_intg_factor(client, ov2722_info, dev->res);
+	if (ret)
+		dev_err(&client->dev, "failed to get integration_factor\n");
+
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2722_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	fmt->width = dev->res->width;
+	fmt->height = dev->res->height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov2722_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u16 high = 0, low = 0;
+	u16 id;
+	u8 revision;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ov2722_read_reg(client, OV2722_8BIT,
+			OV2722_SC_CMMN_CHIP_ID_H, &high);
+	ov2722_read_reg(client, OV2722_8BIT,
+			OV2722_SC_CMMN_CHIP_ID_L, &low);
+	id = (high << 8) | low;
+
+	if ((id != OV2722_ID) && (id != OV2720_ID)) {
+		dev_err(&client->dev, "sensor ID error\n");
+		return -ENODEV;
+	}
+
+	high = 0;
+	ov2722_read_reg(client, OV2722_8BIT,
+			OV2722_SC_CMMN_SUB_ID, &high);
+	revision = (u8)high & 0x0f;
+
+	dev_dbg(&client->dev, "sensor_revision = 0x%x\n", revision);
+	dev_dbg(&client->dev, "detect ov2722 success\n");
+	return 0;
+}
+
+static int ov2722_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	ret = ov2722_write_reg(client, OV2722_8BIT, OV2722_SW_STREAM,
+			       enable ? OV2722_START_STREAMING :
+			       OV2722_STOP_STREAMING);
+
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2722_s_config(struct v4l2_subdev *sd,
+			   int irq, void *platform_data)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+
+	/* power off the module, then power on it in future
+	 * as first power on by board may not fulfill the
+	 * power on sequqence needed by the module
+	 */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov2722 power-off err.\n");
+		goto fail_power_off;
+	}
+
+	ret = power_up(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov2722 power-up err.\n");
+		goto fail_power_on;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = ov2722_detect(client);
+	if (ret) {
+		dev_err(&client->dev, "ov2722_detect err s_config.\n");
+		goto fail_csi_cfg;
+	}
+
+	/* turn off sensor, after probed */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov2722 power-off err.\n");
+		goto fail_csi_cfg;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_power_on:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+fail_power_off:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2722_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = dev->res->fps;
+
+	return 0;
+}
+
+static int ov2722_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int ov2722_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = ov2722_res[index].width;
+	fse->min_height = ov2722_res[index].height;
+	fse->max_width = ov2722_res[index].width;
+	fse->max_height = ov2722_res[index].height;
+
+	return 0;
+}
+
+static int ov2722_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->res->skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_sensor_ops ov2722_sensor_ops = {
+	.g_skip_frames	= ov2722_g_skip_frames,
+};
+
+static const struct v4l2_subdev_video_ops ov2722_video_ops = {
+	.s_stream = ov2722_s_stream,
+	.g_frame_interval = ov2722_g_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops ov2722_core_ops = {
+	.s_power = ov2722_s_power,
+	.ioctl = ov2722_ioctl,
+};
+
+static const struct v4l2_subdev_pad_ops ov2722_pad_ops = {
+	.enum_mbus_code = ov2722_enum_mbus_code,
+	.enum_frame_size = ov2722_enum_frame_size,
+	.get_fmt = ov2722_get_fmt,
+	.set_fmt = ov2722_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov2722_ops = {
+	.core = &ov2722_core_ops,
+	.video = &ov2722_video_ops,
+	.pad = &ov2722_pad_ops,
+	.sensor = &ov2722_sensor_ops,
+};
+
+static void ov2722_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+
+	dev->platform_data->csi_cfg(sd, 0);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_device_unregister_subdev(sd);
+
+	atomisp_gmin_remove_subdev(sd);
+
+	media_entity_cleanup(&dev->sd.entity);
+	kfree(dev);
+}
+
+static int __ov2722_init_ctrl_handler(struct ov2722_device *dev)
+{
+	struct v4l2_ctrl_handler *hdl;
+	unsigned int i;
+
+	hdl = &dev->ctrl_handler;
+	v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(ov2722_controls));
+	for (i = 0; i < ARRAY_SIZE(ov2722_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &ov2722_controls[i],
+				     NULL);
+
+	dev->link_freq = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_LINK_FREQ);
+
+	if (dev->ctrl_handler.error || !dev->link_freq)
+		return dev->ctrl_handler.error;
+
+	dev->sd.ctrl_handler = hdl;
+
+	return 0;
+}
+
+static int ov2722_probe(struct i2c_client *client)
+{
+	struct ov2722_device *dev;
+	void *ovpdev;
+	int ret;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	mutex_init(&dev->input_lock);
+
+	dev->res = &ov2722_res_preview[0];
+	v4l2_i2c_subdev_init(&dev->sd, client, &ov2722_ops);
+
+	ovpdev = gmin_camera_platform_data(&dev->sd,
+					   ATOMISP_INPUT_FORMAT_RAW_10,
+					   atomisp_bayer_order_grbg);
+
+	ret = ov2722_s_config(&dev->sd, client->irq, ovpdev);
+	if (ret)
+		goto out_free;
+
+	ret = __ov2722_init_ctrl_handler(dev);
+	if (ret)
+		goto out_ctrl_handler_free;
+
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret)
+		ov2722_remove(client);
+
+	return atomisp_register_i2c_module(&dev->sd, ovpdev, RAW_CAMERA);
+
+out_ctrl_handler_free:
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static const struct acpi_device_id ov2722_acpi_match[] = {
+	{ "INT33FB" },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov2722_acpi_match);
+
+static struct i2c_driver ov2722_driver = {
+	.driver = {
+		.name = "ov2722",
+		.acpi_match_table = ov2722_acpi_match,
+	},
+	.probe_new = ov2722_probe,
+	.remove = ov2722_remove,
+};
+module_i2c_driver(ov2722_driver);
+
+MODULE_AUTHOR("Wei Liu <wei.liu@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 2722 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/gc0310.h b/drivers/staging/media/atomisp/i2c/gc0310.h
new file mode 100644
index 000000000..db643ebc3
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc0310.h
@@ -0,0 +1,404 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for GalaxyCore GC0310 VGA camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __GC0310_H__
+#define __GC0310_H__
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+
+#include "../include/linux/atomisp_platform.h"
+
+/* Defines for register writes and register array processing */
+#define I2C_MSG_LENGTH		1
+#define I2C_RETRY_COUNT		5
+
+#define GC0310_FOCAL_LENGTH_NUM	278	/*2.78mm*/
+#define GC0310_FOCAL_LENGTH_DEM	100
+#define GC0310_F_NUMBER_DEFAULT_NUM	26
+#define GC0310_F_NUMBER_DEM	10
+
+#define MAX_FMTS		1
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define GC0310_FOCAL_LENGTH_DEFAULT 0x1160064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define GC0310_F_NUMBER_DEFAULT 0x1a000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define GC0310_F_NUMBER_RANGE 0x1a0a1a0a
+#define GC0310_ID	0xa310
+
+#define GC0310_RESET_RELATED		0xFE
+#define GC0310_REGISTER_PAGE_0		0x0
+#define GC0310_REGISTER_PAGE_3		0x3
+
+#define GC0310_FINE_INTG_TIME_MIN 0
+#define GC0310_FINE_INTG_TIME_MAX_MARGIN 0
+#define GC0310_COARSE_INTG_TIME_MIN 1
+#define GC0310_COARSE_INTG_TIME_MAX_MARGIN 6
+
+/*
+ * GC0310 System control registers
+ */
+#define GC0310_SW_STREAM			0x10
+
+#define GC0310_SC_CMMN_CHIP_ID_H		0xf0
+#define GC0310_SC_CMMN_CHIP_ID_L		0xf1
+
+#define GC0310_AEC_PK_EXPO_H			0x03
+#define GC0310_AEC_PK_EXPO_L			0x04
+#define GC0310_AGC_ADJ			0x48
+#define GC0310_DGC_ADJ			0x71
+#if 0
+#define GC0310_GROUP_ACCESS			0x3208
+#endif
+
+#define GC0310_H_CROP_START_H			0x09
+#define GC0310_H_CROP_START_L			0x0A
+#define GC0310_V_CROP_START_H			0x0B
+#define GC0310_V_CROP_START_L			0x0C
+#define GC0310_H_OUTSIZE_H			0x0F
+#define GC0310_H_OUTSIZE_L			0x10
+#define GC0310_V_OUTSIZE_H			0x0D
+#define GC0310_V_OUTSIZE_L			0x0E
+#define GC0310_H_BLANKING_H			0x05
+#define GC0310_H_BLANKING_L			0x06
+#define GC0310_V_BLANKING_H			0x07
+#define GC0310_V_BLANKING_L			0x08
+#define GC0310_SH_DELAY			0x11
+
+#define GC0310_START_STREAMING			0x94 /* 8-bit enable */
+#define GC0310_STOP_STREAMING			0x0 /* 8-bit disable */
+
+#define GC0310_BIN_FACTOR_MAX			3
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct gc0310_resolution {
+	u8 *desc;
+	const struct gc0310_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int pix_clk_freq;
+	u32 skip_frames;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+	bool used;
+};
+
+struct gc0310_format {
+	u8 *desc;
+	u32 pixelformat;
+	struct gc0310_reg *regs;
+};
+
+/*
+ * gc0310 device structure.
+ */
+struct gc0310_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	struct mutex input_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct camera_sensor_platform_data *platform_data;
+	int vt_pix_clk_freq_mhz;
+	struct gc0310_resolution *res;
+	u8 type;
+};
+
+enum gc0310_tok_type {
+	GC0310_8BIT  = 0x0001,
+	GC0310_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+	GC0310_TOK_DELAY  = 0xfe00,	/* delay token for reg list */
+	GC0310_TOK_MASK = 0xfff0
+};
+
+/**
+ * struct gc0310_reg - MI sensor  register format
+ * @type: type of the register
+ * @reg: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct gc0310_reg {
+	enum gc0310_tok_type type;
+	u8 reg;
+	u8 val;	/* @set value for read/mod/write, @mask */
+};
+
+#define to_gc0310_sensor(x) container_of(x, struct gc0310_device, sd)
+
+#define GC0310_MAX_WRITE_BUF_SIZE	30
+
+struct gc0310_write_buffer {
+	u8 addr;
+	u8 data[GC0310_MAX_WRITE_BUF_SIZE];
+};
+
+struct gc0310_write_ctrl {
+	int index;
+	struct gc0310_write_buffer buffer;
+};
+
+/*
+ * Register settings for various resolution
+ */
+static const struct gc0310_reg gc0310_reset_register[] = {
+/////////////////////////////////////////////////
+/////////////////	system reg	/////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0xfe, 0xf0},
+	{GC0310_8BIT, 0xfe, 0xf0},
+	{GC0310_8BIT, 0xfe, 0x00},
+
+	{GC0310_8BIT, 0xfc, 0x0e}, //4e
+	{GC0310_8BIT, 0xfc, 0x0e}, //16//4e // [0]apwd [6]regf_clk_gate
+	{GC0310_8BIT, 0xf2, 0x80}, //sync output
+	{GC0310_8BIT, 0xf3, 0x00}, //1f//01 data output
+	{GC0310_8BIT, 0xf7, 0x33}, //f9
+	{GC0310_8BIT, 0xf8, 0x05}, //00
+	{GC0310_8BIT, 0xf9, 0x0e}, // 0x8e //0f
+	{GC0310_8BIT, 0xfa, 0x11},
+
+/////////////////////////////////////////////////
+///////////////////   MIPI	 ////////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0xfe, 0x03},
+	{GC0310_8BIT, 0x01, 0x03}, ///mipi 1lane
+	{GC0310_8BIT, 0x02, 0x22}, // 0x33
+	{GC0310_8BIT, 0x03, 0x94},
+	{GC0310_8BIT, 0x04, 0x01}, // fifo_prog
+	{GC0310_8BIT, 0x05, 0x00}, //fifo_prog
+	{GC0310_8BIT, 0x06, 0x80}, //b0  //YUV ISP data
+	{GC0310_8BIT, 0x11, 0x2a},//1e //LDI set YUV422
+	{GC0310_8BIT, 0x12, 0x90},//00 //04 //00 //04//00 //LWC[7:0]  //
+	{GC0310_8BIT, 0x13, 0x02},//05 //05 //LWC[15:8]
+	{GC0310_8BIT, 0x15, 0x12}, // 0x10 //DPHYY_MODE read_ready
+	{GC0310_8BIT, 0x17, 0x01},
+	{GC0310_8BIT, 0x40, 0x08},
+	{GC0310_8BIT, 0x41, 0x00},
+	{GC0310_8BIT, 0x42, 0x00},
+	{GC0310_8BIT, 0x43, 0x00},
+	{GC0310_8BIT, 0x21, 0x02}, // 0x01
+	{GC0310_8BIT, 0x22, 0x02}, // 0x01
+	{GC0310_8BIT, 0x23, 0x01}, // 0x05 //Nor:0x05 DOU:0x06
+	{GC0310_8BIT, 0x29, 0x00},
+	{GC0310_8BIT, 0x2A, 0x25}, // 0x05 //data zero 0x7a de
+	{GC0310_8BIT, 0x2B, 0x02},
+
+	{GC0310_8BIT, 0xfe, 0x00},
+
+/////////////////////////////////////////////////
+/////////////////	CISCTL reg	/////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0x00, 0x2f}, //2f//0f//02//01
+	{GC0310_8BIT, 0x01, 0x0f}, //06
+	{GC0310_8BIT, 0x02, 0x04},
+	{GC0310_8BIT, 0x4f, 0x00}, //AEC 0FF
+	{GC0310_8BIT, 0x03, 0x01}, // 0x03 //04
+	{GC0310_8BIT, 0x04, 0xc0}, // 0xe8 //58
+	{GC0310_8BIT, 0x05, 0x00},
+	{GC0310_8BIT, 0x06, 0xb2}, // 0x0a //HB
+	{GC0310_8BIT, 0x07, 0x00},
+	{GC0310_8BIT, 0x08, 0x0c}, // 0x89 //VB
+	{GC0310_8BIT, 0x09, 0x00}, //row start
+	{GC0310_8BIT, 0x0a, 0x00}, //
+	{GC0310_8BIT, 0x0b, 0x00}, //col start
+	{GC0310_8BIT, 0x0c, 0x00},
+	{GC0310_8BIT, 0x0d, 0x01}, //height
+	{GC0310_8BIT, 0x0e, 0xf2}, // 0xf7 //height
+	{GC0310_8BIT, 0x0f, 0x02}, //width
+	{GC0310_8BIT, 0x10, 0x94}, // 0xa0 //height
+	{GC0310_8BIT, 0x17, 0x14},
+	{GC0310_8BIT, 0x18, 0x1a}, //0a//[4]double reset
+	{GC0310_8BIT, 0x19, 0x14}, //AD pipeline
+	{GC0310_8BIT, 0x1b, 0x48},
+	{GC0310_8BIT, 0x1e, 0x6b}, //3b//col bias
+	{GC0310_8BIT, 0x1f, 0x28}, //20//00//08//txlow
+	{GC0310_8BIT, 0x20, 0x89}, //88//0c//[3:2]DA15
+	{GC0310_8BIT, 0x21, 0x49}, //48//[3] txhigh
+	{GC0310_8BIT, 0x22, 0xb0},
+	{GC0310_8BIT, 0x23, 0x04}, //[1:0]vcm_r
+	{GC0310_8BIT, 0x24, 0x16}, //15
+	{GC0310_8BIT, 0x34, 0x20}, //[6:4] rsg high//range
+
+/////////////////////////////////////////////////
+////////////////////   BLK	 ////////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0x26, 0x23}, //[1]dark_current_en [0]offset_en
+	{GC0310_8BIT, 0x28, 0xff}, //BLK_limie_value
+	{GC0310_8BIT, 0x29, 0x00}, //global offset
+	{GC0310_8BIT, 0x33, 0x18}, //offset_ratio
+	{GC0310_8BIT, 0x37, 0x20}, //dark_current_ratio
+	{GC0310_8BIT, 0x2a, 0x00},
+	{GC0310_8BIT, 0x2b, 0x00},
+	{GC0310_8BIT, 0x2c, 0x00},
+	{GC0310_8BIT, 0x2d, 0x00},
+	{GC0310_8BIT, 0x2e, 0x00},
+	{GC0310_8BIT, 0x2f, 0x00},
+	{GC0310_8BIT, 0x30, 0x00},
+	{GC0310_8BIT, 0x31, 0x00},
+	{GC0310_8BIT, 0x47, 0x80}, //a7
+	{GC0310_8BIT, 0x4e, 0x66}, //select_row
+	{GC0310_8BIT, 0xa8, 0x02}, //win_width_dark, same with crop_win_width
+	{GC0310_8BIT, 0xa9, 0x80},
+
+/////////////////////////////////////////////////
+//////////////////	 ISP reg  ///////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0x40, 0x06}, // 0xff //ff //48
+	{GC0310_8BIT, 0x41, 0x00}, // 0x21 //00//[0]curve_en
+	{GC0310_8BIT, 0x42, 0x04}, // 0xcf //0a//[1]awn_en
+	{GC0310_8BIT, 0x44, 0x18}, // 0x18 //02
+	{GC0310_8BIT, 0x46, 0x02}, // 0x03 //sync
+	{GC0310_8BIT, 0x49, 0x03},
+	{GC0310_8BIT, 0x4c, 0x20}, //00[5]pretect exp
+	{GC0310_8BIT, 0x50, 0x01}, //crop enable
+	{GC0310_8BIT, 0x51, 0x00},
+	{GC0310_8BIT, 0x52, 0x00},
+	{GC0310_8BIT, 0x53, 0x00},
+	{GC0310_8BIT, 0x54, 0x01},
+	{GC0310_8BIT, 0x55, 0x01}, //crop window height
+	{GC0310_8BIT, 0x56, 0xf0},
+	{GC0310_8BIT, 0x57, 0x02}, //crop window width
+	{GC0310_8BIT, 0x58, 0x90},
+
+/////////////////////////////////////////////////
+///////////////////   GAIN	 ////////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0x70, 0x70}, //70 //80//global gain
+	{GC0310_8BIT, 0x71, 0x20}, // pregain gain
+	{GC0310_8BIT, 0x72, 0x40}, // post gain
+	{GC0310_8BIT, 0x5a, 0x84}, //84//analog gain 0
+	{GC0310_8BIT, 0x5b, 0xc9}, //c9
+	{GC0310_8BIT, 0x5c, 0xed}, //ed//not use pga gain highest level
+	{GC0310_8BIT, 0x77, 0x40}, // R gain 0x74 //awb gain
+	{GC0310_8BIT, 0x78, 0x40}, // G gain
+	{GC0310_8BIT, 0x79, 0x40}, // B gain 0x5f
+
+	{GC0310_8BIT, 0x48, 0x00},
+	{GC0310_8BIT, 0xfe, 0x01},
+	{GC0310_8BIT, 0x0a, 0x45}, //[7]col gain mode
+
+	{GC0310_8BIT, 0x3e, 0x40},
+	{GC0310_8BIT, 0x3f, 0x5c},
+	{GC0310_8BIT, 0x40, 0x7b},
+	{GC0310_8BIT, 0x41, 0xbd},
+	{GC0310_8BIT, 0x42, 0xf6},
+	{GC0310_8BIT, 0x43, 0x63},
+	{GC0310_8BIT, 0x03, 0x60},
+	{GC0310_8BIT, 0x44, 0x03},
+
+/////////////////////////////////////////////////
+/////////////////	dark sun   //////////////////
+/////////////////////////////////////////////////
+	{GC0310_8BIT, 0xfe, 0x01},
+	{GC0310_8BIT, 0x45, 0xa4}, // 0xf7
+	{GC0310_8BIT, 0x46, 0xf0}, // 0xff //f0//sun value th
+	{GC0310_8BIT, 0x48, 0x03}, //sun mode
+	{GC0310_8BIT, 0x4f, 0x60}, //sun_clamp
+	{GC0310_8BIT, 0xfe, 0x00},
+
+	{GC0310_TOK_TERM, 0, 0},
+};
+
+static struct gc0310_reg const gc0310_VGA_30fps[] = {
+	{GC0310_8BIT, 0xfe, 0x00},
+	{GC0310_8BIT, 0x0d, 0x01}, //height
+	{GC0310_8BIT, 0x0e, 0xf2}, // 0xf7 //height
+	{GC0310_8BIT, 0x0f, 0x02}, //width
+	{GC0310_8BIT, 0x10, 0x94}, // 0xa0 //height
+
+	{GC0310_8BIT, 0x50, 0x01}, //crop enable
+	{GC0310_8BIT, 0x51, 0x00},
+	{GC0310_8BIT, 0x52, 0x00},
+	{GC0310_8BIT, 0x53, 0x00},
+	{GC0310_8BIT, 0x54, 0x01},
+	{GC0310_8BIT, 0x55, 0x01}, //crop window height
+	{GC0310_8BIT, 0x56, 0xf0},
+	{GC0310_8BIT, 0x57, 0x02}, //crop window width
+	{GC0310_8BIT, 0x58, 0x90},
+
+	{GC0310_8BIT, 0xfe, 0x03},
+	{GC0310_8BIT, 0x12, 0x90},//00 //04 //00 //04//00 //LWC[7:0]  //
+	{GC0310_8BIT, 0x13, 0x02},//05 //05 //LWC[15:8]
+
+	{GC0310_8BIT, 0xfe, 0x00},
+
+	{GC0310_TOK_TERM, 0, 0},
+};
+
+static struct gc0310_resolution gc0310_res_preview[] = {
+	{
+		.desc = "gc0310_VGA_30fps",
+		.width = 656, // 648,
+		.height = 496, // 488,
+		.fps = 30,
+		//.pix_clk_freq = 73,
+		.used = 0,
+#if 0
+		.pixels_per_line = 0x0314,
+		.lines_per_frame = 0x0213,
+#endif
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 2,
+		.regs = gc0310_VGA_30fps,
+	},
+};
+
+#define N_RES_PREVIEW (ARRAY_SIZE(gc0310_res_preview))
+
+static struct gc0310_resolution *gc0310_res = gc0310_res_preview;
+static unsigned long N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/gc2235.h b/drivers/staging/media/atomisp/i2c/gc2235.h
new file mode 100644
index 000000000..806be5dff
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc2235.h
@@ -0,0 +1,680 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for GalaxyCore GC2235 2M camera sensor.
+ *
+ * Copyright (c) 2014 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.
+ *
+ */
+
+#ifndef __GC2235_H__
+#define __GC2235_H__
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+
+#include "../include/linux/atomisp_platform.h"
+
+/*
+ * FIXME: non-preview resolutions are currently broken
+ */
+#define ENABLE_NON_PREVIEW     0
+
+/* Defines for register writes and register array processing */
+#define I2C_MSG_LENGTH		0x2
+#define I2C_RETRY_COUNT		5
+
+#define GC2235_FOCAL_LENGTH_NUM	278	/*2.78mm*/
+#define GC2235_FOCAL_LENGTH_DEM	100
+#define GC2235_F_NUMBER_DEFAULT_NUM	26
+#define GC2235_F_NUMBER_DEM	10
+
+#define MAX_FMTS		1
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define GC2235_FOCAL_LENGTH_DEFAULT 0x1160064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define GC2235_F_NUMBER_DEFAULT 0x1a000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define GC2235_F_NUMBER_RANGE 0x1a0a1a0a
+#define GC2235_ID	0x2235
+
+#define GC2235_FINE_INTG_TIME_MIN 0
+#define GC2235_FINE_INTG_TIME_MAX_MARGIN 0
+#define GC2235_COARSE_INTG_TIME_MIN 1
+#define GC2235_COARSE_INTG_TIME_MAX_MARGIN 6
+
+/*
+ * GC2235 System control registers
+ */
+/*
+ * GC2235 System control registers
+ */
+#define GC2235_SENSOR_ID_H		0xF0
+#define GC2235_SENSOR_ID_L		0xF1
+#define GC2235_RESET_RELATED		0xFE
+#define GC2235_SW_RESET			0x8
+#define GC2235_MIPI_RESET		0x3
+#define GC2235_RESET_BIT		0x4
+#define GC2235_REGISTER_PAGE_0		0x0
+#define GC2235_REGISTER_PAGE_3		0x3
+
+#define GC2235_V_CROP_START_H		0x91
+#define GC2235_V_CROP_START_L		0x92
+#define GC2235_H_CROP_START_H		0x93
+#define GC2235_H_CROP_START_L		0x94
+#define GC2235_V_OUTSIZE_H		0x95
+#define GC2235_V_OUTSIZE_L		0x96
+#define GC2235_H_OUTSIZE_H		0x97
+#define GC2235_H_OUTSIZE_L		0x98
+
+#define GC2235_HB_H			0x5
+#define GC2235_HB_L			0x6
+#define GC2235_VB_H			0x7
+#define GC2235_VB_L			0x8
+#define GC2235_SH_DELAY_H		0x11
+#define GC2235_SH_DELAY_L		0x12
+
+#define GC2235_CSI2_MODE		0x10
+
+#define GC2235_EXPOSURE_H		0x3
+#define GC2235_EXPOSURE_L		0x4
+#define GC2235_GLOBAL_GAIN		0xB0
+#define GC2235_PRE_GAIN			0xB1
+#define GC2235_AWB_R_GAIN		0xB3
+#define GC2235_AWB_G_GAIN		0xB4
+#define GC2235_AWB_B_GAIN		0xB5
+
+#define GC2235_START_STREAMING		0x91
+#define GC2235_STOP_STREAMING		0x0
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct gc2235_resolution {
+	u8 *desc;
+	const struct gc2235_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int pix_clk_freq;
+	u32 skip_frames;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+	bool used;
+};
+
+struct gc2235_format {
+	u8 *desc;
+	u32 pixelformat;
+	struct gc2235_reg *regs;
+};
+
+/*
+ * gc2235 device structure.
+ */
+struct gc2235_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	struct mutex input_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct gc2235_resolution *res;
+
+	struct camera_sensor_platform_data *platform_data;
+	int vt_pix_clk_freq_mhz;
+	u8 type;
+};
+
+enum gc2235_tok_type {
+	GC2235_8BIT  = 0x0001,
+	GC2235_16BIT = 0x0002,
+	GC2235_32BIT = 0x0004,
+	GC2235_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+	GC2235_TOK_DELAY  = 0xfe00,	/* delay token for reg list */
+	GC2235_TOK_MASK = 0xfff0
+};
+
+/**
+ * struct gc2235_reg - MI sensor  register format
+ * @type: type of the register
+ * @reg: 8-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct gc2235_reg {
+	enum gc2235_tok_type type;
+	u8 reg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+#define to_gc2235_sensor(x) container_of(x, struct gc2235_device, sd)
+
+#define GC2235_MAX_WRITE_BUF_SIZE	30
+
+struct gc2235_write_buffer {
+	u8 addr;
+	u8 data[GC2235_MAX_WRITE_BUF_SIZE];
+};
+
+struct gc2235_write_ctrl {
+	int index;
+	struct gc2235_write_buffer buffer;
+};
+
+static struct gc2235_reg const gc2235_stream_on[] = {
+	{ GC2235_8BIT, 0xfe, 0x03}, /* switch to P3 */
+	{ GC2235_8BIT, 0x10, 0x91}, /* start mipi */
+	{ GC2235_8BIT, 0xfe, 0x00}, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_reg const gc2235_stream_off[] = {
+	{ GC2235_8BIT, 0xfe, 0x03}, /* switch to P3 */
+	{ GC2235_8BIT, 0x10, 0x01}, /* stop mipi */
+	{ GC2235_8BIT, 0xfe, 0x00}, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_reg const gc2235_init_settings[] = {
+	/* System */
+	{ GC2235_8BIT, 0xfe, 0x80 },
+	{ GC2235_8BIT, 0xfe, 0x80 },
+	{ GC2235_8BIT, 0xfe, 0x80 },
+	{ GC2235_8BIT, 0xf2, 0x00 },
+	{ GC2235_8BIT, 0xf6, 0x00 },
+	{ GC2235_8BIT, 0xfc, 0x06 },
+	{ GC2235_8BIT, 0xf7, 0x15 },
+	{ GC2235_8BIT, 0xf8, 0x84 },
+	{ GC2235_8BIT, 0xf9, 0xfe },
+	{ GC2235_8BIT, 0xfa, 0x00 },
+	{ GC2235_8BIT, 0xfe, 0x00 },
+	/* Analog & cisctl */
+	{ GC2235_8BIT, 0x03, 0x04 },
+	{ GC2235_8BIT, 0x04, 0x9E },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x06, 0xfd },
+	{ GC2235_8BIT, 0x07, 0x00 },
+	{ GC2235_8BIT, 0x08, 0x14 },
+	{ GC2235_8BIT, 0x0a, 0x02 }, /* row start */
+	{ GC2235_8BIT, 0x0c, 0x00 }, /* col start */
+	{ GC2235_8BIT, 0x0d, 0x04 }, /* win height 1232 */
+	{ GC2235_8BIT, 0x0e, 0xd0 },
+	{ GC2235_8BIT, 0x0f, 0x06 }, /* win width: 1616 */
+	{ GC2235_8BIT, 0x10, 0x60 },
+	{ GC2235_8BIT, 0x17, 0x15 }, /* mirror flip */
+	{ GC2235_8BIT, 0x18, 0x1a },
+	{ GC2235_8BIT, 0x19, 0x06 },
+	{ GC2235_8BIT, 0x1a, 0x01 },
+	{ GC2235_8BIT, 0x1b, 0x4d },
+	{ GC2235_8BIT, 0x1e, 0x88 },
+	{ GC2235_8BIT, 0x1f, 0x48 },
+	{ GC2235_8BIT, 0x20, 0x03 },
+	{ GC2235_8BIT, 0x21, 0x7f },
+	{ GC2235_8BIT, 0x22, 0x83 },
+	{ GC2235_8BIT, 0x23, 0x42 },
+	{ GC2235_8BIT, 0x24, 0x16 },
+	{ GC2235_8BIT, 0x26, 0x01 }, /*analog gain*/
+	{ GC2235_8BIT, 0x27, 0x30 },
+	{ GC2235_8BIT, 0x3f, 0x00 }, /* PRC */
+	/* blk */
+	{ GC2235_8BIT, 0x40, 0xa3 },
+	{ GC2235_8BIT, 0x41, 0x82 },
+	{ GC2235_8BIT, 0x43, 0x20 },
+	{ GC2235_8BIT, 0x5e, 0x18 },
+	{ GC2235_8BIT, 0x5f, 0x18 },
+	{ GC2235_8BIT, 0x60, 0x18 },
+	{ GC2235_8BIT, 0x61, 0x18 },
+	{ GC2235_8BIT, 0x62, 0x18 },
+	{ GC2235_8BIT, 0x63, 0x18 },
+	{ GC2235_8BIT, 0x64, 0x18 },
+	{ GC2235_8BIT, 0x65, 0x18 },
+	{ GC2235_8BIT, 0x66, 0x20 },
+	{ GC2235_8BIT, 0x67, 0x20 },
+	{ GC2235_8BIT, 0x68, 0x20 },
+	{ GC2235_8BIT, 0x69, 0x20 },
+	/* Gain */
+	{ GC2235_8BIT, 0xb2, 0x00 },
+	{ GC2235_8BIT, 0xb3, 0x40 },
+	{ GC2235_8BIT, 0xb4, 0x40 },
+	{ GC2235_8BIT, 0xb5, 0x40 },
+	/* Dark sun */
+	{ GC2235_8BIT, 0xbc, 0x00 },
+
+	{ GC2235_8BIT, 0xfe, 0x03 },
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+/*
+ * Register settings for various resolution
+ */
+#if ENABLE_NON_PREVIEW
+static struct gc2235_reg const gc2235_1296_736_30fps[] = {
+	{ GC2235_8BIT, 0x8b, 0xa0 },
+	{ GC2235_8BIT, 0x8c, 0x02 },
+
+	{ GC2235_8BIT, 0x07, 0x01 }, /* VBI */
+	{ GC2235_8BIT, 0x08, 0x44 },
+	{ GC2235_8BIT, 0x09, 0x00 }, /* row start */
+	{ GC2235_8BIT, 0x0a, 0xf0 },
+	{ GC2235_8BIT, 0x0b, 0x00 }, /* col start */
+	{ GC2235_8BIT, 0x0c, 0xa0 },
+	{ GC2235_8BIT, 0x0d, 0x02 }, /* win height 736 */
+	{ GC2235_8BIT, 0x0e, 0xf0 },
+	{ GC2235_8BIT, 0x0f, 0x05 }, /* win width: 1296 */
+	{ GC2235_8BIT, 0x10, 0x20 },
+
+	{ GC2235_8BIT, 0x90, 0x01 },
+	{ GC2235_8BIT, 0x92, 0x08 },
+	{ GC2235_8BIT, 0x94, 0x08 },
+	{ GC2235_8BIT, 0x95, 0x02 }, /* crop win height 736 */
+	{ GC2235_8BIT, 0x96, 0xe0 },
+	{ GC2235_8BIT, 0x97, 0x05 }, /* crop win width 1296 */
+	{ GC2235_8BIT, 0x98, 0x10 },
+	/* mimi init */
+	{ GC2235_8BIT, 0xfe, 0x03 }, /* switch to P3 */
+	{ GC2235_8BIT, 0x01, 0x07 },
+	{ GC2235_8BIT, 0x02, 0x11 },
+	{ GC2235_8BIT, 0x03, 0x11 },
+	{ GC2235_8BIT, 0x06, 0x80 },
+	{ GC2235_8BIT, 0x11, 0x2b },
+	/* set mipi buffer */
+	{ GC2235_8BIT, 0x12, 0x54 }, /* val_low = (width * 10 / 8) & 0xFF */
+	{ GC2235_8BIT, 0x13, 0x06 }, /* val_high = (width * 10 / 8) >> 8 */
+
+	{ GC2235_8BIT, 0x15, 0x12 }, /* DPHY mode*/
+	{ GC2235_8BIT, 0x04, 0x10 },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x17, 0x01 },
+
+	{ GC2235_8BIT, 0x22, 0x01 },
+	{ GC2235_8BIT, 0x23, 0x05 },
+	{ GC2235_8BIT, 0x24, 0x10 },
+	{ GC2235_8BIT, 0x25, 0x10 },
+	{ GC2235_8BIT, 0x26, 0x02 },
+	{ GC2235_8BIT, 0x21, 0x10 },
+	{ GC2235_8BIT, 0x29, 0x01 },
+	{ GC2235_8BIT, 0x2a, 0x02 },
+	{ GC2235_8BIT, 0x2b, 0x02 },
+
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_reg const gc2235_960_640_30fps[] = {
+	{ GC2235_8BIT, 0x8b, 0xa0 },
+	{ GC2235_8BIT, 0x8c, 0x02 },
+
+	{ GC2235_8BIT, 0x07, 0x02 }, /* VBI */
+	{ GC2235_8BIT, 0x08, 0xA4 },
+	{ GC2235_8BIT, 0x09, 0x01 }, /* row start */
+	{ GC2235_8BIT, 0x0a, 0x18 },
+	{ GC2235_8BIT, 0x0b, 0x01 }, /* col start */
+	{ GC2235_8BIT, 0x0c, 0x40 },
+	{ GC2235_8BIT, 0x0d, 0x02 }, /* win height 656 */
+	{ GC2235_8BIT, 0x0e, 0x90 },
+	{ GC2235_8BIT, 0x0f, 0x03 }, /* win width: 976 */
+	{ GC2235_8BIT, 0x10, 0xd0 },
+
+	{ GC2235_8BIT, 0x90, 0x01 },
+	{ GC2235_8BIT, 0x92, 0x02 },
+	{ GC2235_8BIT, 0x94, 0x06 },
+	{ GC2235_8BIT, 0x95, 0x02 }, /* crop win height 640 */
+	{ GC2235_8BIT, 0x96, 0x80 },
+	{ GC2235_8BIT, 0x97, 0x03 }, /* crop win width 960 */
+	{ GC2235_8BIT, 0x98, 0xc0 },
+	/* mimp init */
+	{ GC2235_8BIT, 0xfe, 0x03 }, /* switch to P3 */
+	{ GC2235_8BIT, 0x01, 0x07 },
+	{ GC2235_8BIT, 0x02, 0x11 },
+	{ GC2235_8BIT, 0x03, 0x11 },
+	{ GC2235_8BIT, 0x06, 0x80 },
+	{ GC2235_8BIT, 0x11, 0x2b },
+	/* set mipi buffer */
+	{ GC2235_8BIT, 0x12, 0xb0 }, /* val_low = (width * 10 / 8) & 0xFF */
+	{ GC2235_8BIT, 0x13, 0x04 }, /* val_high = (width * 10 / 8) >> 8 */
+
+	{ GC2235_8BIT, 0x15, 0x12 }, /* DPHY mode*/
+	{ GC2235_8BIT, 0x04, 0x10 },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x17, 0x01 },
+	{ GC2235_8BIT, 0x22, 0x01 },
+	{ GC2235_8BIT, 0x23, 0x05 },
+	{ GC2235_8BIT, 0x24, 0x10 },
+	{ GC2235_8BIT, 0x25, 0x10 },
+	{ GC2235_8BIT, 0x26, 0x02 },
+	{ GC2235_8BIT, 0x21, 0x10 },
+	{ GC2235_8BIT, 0x29, 0x01 },
+	{ GC2235_8BIT, 0x2a, 0x02 },
+	{ GC2235_8BIT, 0x2b, 0x02 },
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+#endif
+
+static struct gc2235_reg const gc2235_1600_900_30fps[] = {
+	{ GC2235_8BIT, 0x8b, 0xa0 },
+	{ GC2235_8BIT, 0x8c, 0x02 },
+
+	{ GC2235_8BIT, 0x0d, 0x03 }, /* win height 932 */
+	{ GC2235_8BIT, 0x0e, 0xa4 },
+	{ GC2235_8BIT, 0x0f, 0x06 }, /* win width: 1632 */
+	{ GC2235_8BIT, 0x10, 0x50 },
+
+	{ GC2235_8BIT, 0x90, 0x01 },
+	{ GC2235_8BIT, 0x92, 0x02 },
+	{ GC2235_8BIT, 0x94, 0x06 },
+	{ GC2235_8BIT, 0x95, 0x03 }, /* crop win height 900 */
+	{ GC2235_8BIT, 0x96, 0x84 },
+	{ GC2235_8BIT, 0x97, 0x06 }, /* crop win width 1600 */
+	{ GC2235_8BIT, 0x98, 0x40 },
+	/* mimi init */
+	{ GC2235_8BIT, 0xfe, 0x03 }, /* switch to P3 */
+	{ GC2235_8BIT, 0x01, 0x07 },
+	{ GC2235_8BIT, 0x02, 0x11 },
+	{ GC2235_8BIT, 0x03, 0x11 },
+	{ GC2235_8BIT, 0x06, 0x80 },
+	{ GC2235_8BIT, 0x11, 0x2b },
+	/* set mipi buffer */
+	{ GC2235_8BIT, 0x12, 0xd0 }, /* val_low = (width * 10 / 8) & 0xFF */
+	{ GC2235_8BIT, 0x13, 0x07 }, /* val_high = (width * 10 / 8) >> 8 */
+
+	{ GC2235_8BIT, 0x15, 0x12 }, /* DPHY mode*/
+	{ GC2235_8BIT, 0x04, 0x10 },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x17, 0x01 },
+	{ GC2235_8BIT, 0x22, 0x01 },
+	{ GC2235_8BIT, 0x23, 0x05 },
+	{ GC2235_8BIT, 0x24, 0x10 },
+	{ GC2235_8BIT, 0x25, 0x10 },
+	{ GC2235_8BIT, 0x26, 0x02 },
+	{ GC2235_8BIT, 0x21, 0x10 },
+	{ GC2235_8BIT, 0x29, 0x01 },
+	{ GC2235_8BIT, 0x2a, 0x02 },
+	{ GC2235_8BIT, 0x2b, 0x02 },
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_reg const gc2235_1616_1082_30fps[] = {
+	{ GC2235_8BIT, 0x8b, 0xa0 },
+	{ GC2235_8BIT, 0x8c, 0x02 },
+
+	{ GC2235_8BIT, 0x0d, 0x04 }, /* win height 1232 */
+	{ GC2235_8BIT, 0x0e, 0xd0 },
+	{ GC2235_8BIT, 0x0f, 0x06 }, /* win width: 1616 */
+	{ GC2235_8BIT, 0x10, 0x50 },
+
+	{ GC2235_8BIT, 0x90, 0x01 },
+	{ GC2235_8BIT, 0x92, 0x4a },
+	{ GC2235_8BIT, 0x94, 0x00 },
+	{ GC2235_8BIT, 0x95, 0x04 }, /* crop win height 1082 */
+	{ GC2235_8BIT, 0x96, 0x3a },
+	{ GC2235_8BIT, 0x97, 0x06 }, /* crop win width 1616 */
+	{ GC2235_8BIT, 0x98, 0x50 },
+	/* mimp init */
+	{ GC2235_8BIT, 0xfe, 0x03 }, /* switch to P3 */
+	{ GC2235_8BIT, 0x01, 0x07 },
+	{ GC2235_8BIT, 0x02, 0x11 },
+	{ GC2235_8BIT, 0x03, 0x11 },
+	{ GC2235_8BIT, 0x06, 0x80 },
+	{ GC2235_8BIT, 0x11, 0x2b },
+	/* set mipi buffer */
+	{ GC2235_8BIT, 0x12, 0xe4 }, /* val_low = (width * 10 / 8) & 0xFF */
+	{ GC2235_8BIT, 0x13, 0x07 }, /* val_high = (width * 10 / 8) >> 8 */
+
+	{ GC2235_8BIT, 0x15, 0x12 }, /* DPHY mode*/
+	{ GC2235_8BIT, 0x04, 0x10 },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x17, 0x01 },
+	{ GC2235_8BIT, 0x22, 0x01 },
+	{ GC2235_8BIT, 0x23, 0x05 },
+	{ GC2235_8BIT, 0x24, 0x10 },
+	{ GC2235_8BIT, 0x25, 0x10 },
+	{ GC2235_8BIT, 0x26, 0x02 },
+	{ GC2235_8BIT, 0x21, 0x10 },
+	{ GC2235_8BIT, 0x29, 0x01 },
+	{ GC2235_8BIT, 0x2a, 0x02 },
+	{ GC2235_8BIT, 0x2b, 0x02 },
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_reg const gc2235_1616_1216_30fps[] = {
+	{ GC2235_8BIT, 0x8b, 0xa0 },
+	{ GC2235_8BIT, 0x8c, 0x02 },
+
+	{ GC2235_8BIT, 0x0d, 0x04 }, /* win height 1232 */
+	{ GC2235_8BIT, 0x0e, 0xd0 },
+	{ GC2235_8BIT, 0x0f, 0x06 }, /* win width: 1616 */
+	{ GC2235_8BIT, 0x10, 0x50 },
+
+	{ GC2235_8BIT, 0x90, 0x01 },
+	{ GC2235_8BIT, 0x92, 0x02 },
+	{ GC2235_8BIT, 0x94, 0x00 },
+	{ GC2235_8BIT, 0x95, 0x04 }, /* crop win height 1216 */
+	{ GC2235_8BIT, 0x96, 0xc0 },
+	{ GC2235_8BIT, 0x97, 0x06 }, /* crop win width 1616 */
+	{ GC2235_8BIT, 0x98, 0x50 },
+	/* mimi init */
+	{ GC2235_8BIT, 0xfe, 0x03 }, /* switch to P3 */
+	{ GC2235_8BIT, 0x01, 0x07 },
+	{ GC2235_8BIT, 0x02, 0x11 },
+	{ GC2235_8BIT, 0x03, 0x11 },
+	{ GC2235_8BIT, 0x06, 0x80 },
+	{ GC2235_8BIT, 0x11, 0x2b },
+	/* set mipi buffer */
+	{ GC2235_8BIT, 0x12, 0xe4 }, /* val_low = (width * 10 / 8) & 0xFF */
+	{ GC2235_8BIT, 0x13, 0x07 }, /* val_high = (width * 10 / 8) >> 8 */
+	{ GC2235_8BIT, 0x15, 0x12 }, /* DPHY mode*/
+	{ GC2235_8BIT, 0x04, 0x10 },
+	{ GC2235_8BIT, 0x05, 0x00 },
+	{ GC2235_8BIT, 0x17, 0x01 },
+	{ GC2235_8BIT, 0x22, 0x01 },
+	{ GC2235_8BIT, 0x23, 0x05 },
+	{ GC2235_8BIT, 0x24, 0x10 },
+	{ GC2235_8BIT, 0x25, 0x10 },
+	{ GC2235_8BIT, 0x26, 0x02 },
+	{ GC2235_8BIT, 0x21, 0x10 },
+	{ GC2235_8BIT, 0x29, 0x01 },
+	{ GC2235_8BIT, 0x2a, 0x02 },
+	{ GC2235_8BIT, 0x2b, 0x02 },
+	{ GC2235_8BIT, 0x10, 0x01 }, /* disable mipi */
+	{ GC2235_8BIT, 0xfe, 0x00 }, /* switch to P0 */
+	{ GC2235_TOK_TERM, 0, 0 }
+};
+
+static struct gc2235_resolution gc2235_res_preview[] = {
+	{
+		.desc = "gc2235_1600_900_30fps",
+		.width = 1600,
+		.height = 900,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1068,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1600_900_30fps,
+	},
+
+	{
+		.desc = "gc2235_1600_1066_30fps",
+		.width = 1616,
+		.height = 1082,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1368,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1616_1082_30fps,
+	},
+	{
+		.desc = "gc2235_1600_1200_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1368,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1616_1216_30fps,
+	},
+
+};
+
+#define N_RES_PREVIEW (ARRAY_SIZE(gc2235_res_preview))
+
+/*
+ * Disable non-preview configurations until the configuration selection is
+ * improved.
+ */
+#if ENABLE_NON_PREVIEW
+static struct gc2235_resolution gc2235_res_still[] = {
+	{
+		.desc = "gc2235_1600_900_30fps",
+		.width = 1600,
+		.height = 900,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1068,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1600_900_30fps,
+	},
+	{
+		.desc = "gc2235_1600_1066_30fps",
+		.width = 1616,
+		.height = 1082,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1368,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1616_1082_30fps,
+	},
+	{
+		.desc = "gc2235_1600_1200_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2132,
+		.lines_per_frame = 1368,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1616_1216_30fps,
+	},
+
+};
+
+#define N_RES_STILL (ARRAY_SIZE(gc2235_res_still))
+
+static struct gc2235_resolution gc2235_res_video[] = {
+	{
+		.desc = "gc2235_1296_736_30fps",
+		.width = 1296,
+		.height = 736,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 1828,
+		.lines_per_frame = 888,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_1296_736_30fps,
+	},
+	{
+		.desc = "gc2235_960_640_30fps",
+		.width = 960,
+		.height = 640,
+		.pix_clk_freq = 30,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 1492,
+		.lines_per_frame = 792,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = gc2235_960_640_30fps,
+	},
+
+};
+
+#define N_RES_VIDEO (ARRAY_SIZE(gc2235_res_video))
+#endif
+
+static struct gc2235_resolution *gc2235_res = gc2235_res_preview;
+static unsigned long N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/mt9m114.h b/drivers/staging/media/atomisp/i2c/mt9m114.h
new file mode 100644
index 000000000..bcce18b65
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/mt9m114.h
@@ -0,0 +1,1794 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for mt9m114 Camera Sensor.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __A1040_H__
+#define __A1040_H__
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include "../include/linux/atomisp_platform.h"
+#include "../include/linux/atomisp.h"
+
+#define V4L2_IDENT_MT9M114 8245
+
+#define MT9P111_REV3
+#define FULLINISUPPORT
+
+/* #defines for register writes and register array processing */
+#define MISENSOR_8BIT		1
+#define MISENSOR_16BIT		2
+#define MISENSOR_32BIT		4
+
+#define MISENSOR_FWBURST0	0x80
+#define MISENSOR_FWBURST1	0x81
+#define MISENSOR_FWBURST4	0x84
+#define MISENSOR_FWBURST	0x88
+
+#define MISENSOR_TOK_TERM	0xf000	/* terminating token for reg list */
+#define MISENSOR_TOK_DELAY	0xfe00	/* delay token for reg list */
+#define MISENSOR_TOK_FWLOAD	0xfd00	/* token indicating load FW */
+#define MISENSOR_TOK_POLL	0xfc00	/* token indicating poll instruction */
+#define MISENSOR_TOK_RMW	0x0010  /* RMW operation */
+#define MISENSOR_TOK_MASK	0xfff0
+#define MISENSOR_AWB_STEADY	BIT(0)	/* awb steady */
+#define MISENSOR_AE_READY	BIT(3)	/* ae status ready */
+
+/* mask to set sensor read_mode via misensor_rmw_reg */
+#define MISENSOR_R_MODE_MASK	0x0330
+/* mask to set sensor vert_flip and horz_mirror */
+#define MISENSOR_VFLIP_MASK	0x0002
+#define MISENSOR_HFLIP_MASK	0x0001
+#define MISENSOR_FLIP_EN	1
+#define MISENSOR_FLIP_DIS	0
+
+/* bits set to set sensor read_mode via misensor_rmw_reg */
+#define MISENSOR_SKIPPING_SET	0x0011
+#define MISENSOR_SUMMING_SET	0x0033
+#define MISENSOR_NORMAL_SET	0x0000
+
+/* sensor register that control sensor read-mode and mirror */
+#define MISENSOR_READ_MODE	0xC834
+/* sensor ae-track status register */
+#define MISENSOR_AE_TRACK_STATUS	0xA800
+/* sensor awb status register */
+#define MISENSOR_AWB_STATUS	0xAC00
+/* sensor coarse integration time register */
+#define MISENSOR_COARSE_INTEGRATION_TIME 0xC83C
+
+/* registers */
+#define REG_SW_RESET                    0x301A
+#define REG_SW_STREAM                   0xDC00
+#define REG_SCCB_CTRL                   0x3100
+#define REG_SC_CMMN_CHIP_ID             0x0000
+#define REG_V_START                     0xc800 /* 16bits */
+#define REG_H_START                     0xc802 /* 16bits */
+#define REG_V_END                       0xc804 /* 16bits */
+#define REG_H_END                       0xc806 /* 16bits */
+#define REG_PIXEL_CLK                   0xc808 /* 32bits */
+#define REG_TIMING_VTS                  0xc812 /* 16bits */
+#define REG_TIMING_HTS                  0xc814 /* 16bits */
+#define REG_WIDTH                       0xC868 /* 16bits */
+#define REG_HEIGHT                      0xC86A /* 16bits */
+#define REG_EXPO_COARSE                 0x3012 /* 16bits */
+#define REG_EXPO_FINE                   0x3014 /* 16bits */
+#define REG_GAIN                        0x305E
+#define REG_ANALOGGAIN                  0x305F
+#define REG_ADDR_ACESSS                 0x098E /* logical_address_access */
+#define REG_COMM_Register               0x0080 /* command_register */
+
+#define SENSOR_DETECTED		1
+#define SENSOR_NOT_DETECTED	0
+
+#define I2C_RETRY_COUNT		5
+#define MSG_LEN_OFFSET		2
+
+#ifndef MIPI_CONTROL
+#define MIPI_CONTROL		0x3400	/* MIPI_Control */
+#endif
+
+/* GPIO pin on Moorestown */
+#define GPIO_SCLK_25		44
+#define GPIO_STB_PIN		47
+
+#define GPIO_STDBY_PIN		49   /* ab:new */
+#define GPIO_RESET_PIN		50
+
+/* System control register for Aptina A-1040SOC*/
+#define MT9M114_PID		0x0
+
+/* MT9P111_DEVICE_ID */
+#define MT9M114_MOD_ID		0x2481
+
+#define MT9M114_FINE_INTG_TIME_MIN 0
+#define MT9M114_FINE_INTG_TIME_MAX_MARGIN 0
+#define MT9M114_COARSE_INTG_TIME_MIN 1
+#define MT9M114_COARSE_INTG_TIME_MAX_MARGIN 6
+
+/* ulBPat; */
+
+#define MT9M114_BPAT_RGRGGBGB	BIT(0)
+#define MT9M114_BPAT_GRGRBGBG	BIT(1)
+#define MT9M114_BPAT_GBGBRGRG	BIT(2)
+#define MT9M114_BPAT_BGBGGRGR	BIT(3)
+
+#define MT9M114_FOCAL_LENGTH_NUM	208	/*2.08mm*/
+#define MT9M114_FOCAL_LENGTH_DEM	100
+#define MT9M114_F_NUMBER_DEFAULT_NUM	24
+#define MT9M114_F_NUMBER_DEM	10
+#define MT9M114_WAIT_STAT_TIMEOUT	100
+#define MT9M114_FLICKER_MODE_50HZ	1
+#define MT9M114_FLICKER_MODE_60HZ	2
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define MT9M114_FOCAL_LENGTH_DEFAULT 0xD00064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define MT9M114_F_NUMBER_DEFAULT 0x18000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define MT9M114_F_NUMBER_RANGE 0x180a180a
+
+/* Supported resolutions */
+enum {
+	MT9M114_RES_736P,
+	MT9M114_RES_864P,
+	MT9M114_RES_960P,
+};
+
+#define MT9M114_RES_960P_SIZE_H		1296
+#define MT9M114_RES_960P_SIZE_V		976
+#define MT9M114_RES_720P_SIZE_H		1280
+#define MT9M114_RES_720P_SIZE_V		720
+#define MT9M114_RES_576P_SIZE_H		1024
+#define MT9M114_RES_576P_SIZE_V		576
+#define MT9M114_RES_480P_SIZE_H		768
+#define MT9M114_RES_480P_SIZE_V		480
+#define MT9M114_RES_VGA_SIZE_H		640
+#define MT9M114_RES_VGA_SIZE_V		480
+#define MT9M114_RES_QVGA_SIZE_H		320
+#define MT9M114_RES_QVGA_SIZE_V		240
+#define MT9M114_RES_QCIF_SIZE_H		176
+#define MT9M114_RES_QCIF_SIZE_V		144
+
+#define MT9M114_RES_720_480p_768_SIZE_H 736
+#define MT9M114_RES_720_480p_768_SIZE_V 496
+#define MT9M114_RES_736P_SIZE_H 1296
+#define MT9M114_RES_736P_SIZE_V 736
+#define MT9M114_RES_864P_SIZE_H 1296
+#define MT9M114_RES_864P_SIZE_V 864
+#define MT9M114_RES_976P_SIZE_H 1296
+#define MT9M114_RES_976P_SIZE_V 976
+
+#define MT9M114_BIN_FACTOR_MAX			3
+
+#define MT9M114_DEFAULT_FIRST_EXP 0x10
+#define MT9M114_MAX_FIRST_EXP 0x302
+
+/* completion status polling requirements, usage based on Aptina .INI Rev2 */
+enum poll_reg {
+	NO_POLLING,
+	PRE_POLLING,
+	POST_POLLING,
+};
+
+/*
+ * struct misensor_reg - MI sensor  register format
+ * @length: length of the register
+ * @reg: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ * Define a structure for sensor register initialization values
+ */
+struct misensor_reg {
+	u32 length;
+	u32 reg;
+	u32 val;	/* value or for read/mod/write, AND mask */
+	u32 val2;	/* optional; for rmw, OR mask */
+};
+
+/*
+ * struct misensor_fwreg - Firmware burst command
+ * @type: FW burst or 8/16 bit register
+ * @addr: 16-bit offset to register or other values depending on type
+ * @valx: data value for burst (or other commands)
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct misensor_fwreg {
+	u32	type;	/* type of value, register or FW burst string */
+	u32	addr;	/* target address */
+	u32	val0;
+	u32	val1;
+	u32	val2;
+	u32	val3;
+	u32	val4;
+	u32	val5;
+	u32	val6;
+	u32	val7;
+};
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct mt9m114_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+
+	struct camera_sensor_platform_data *platform_data;
+	struct mutex input_lock;	/* serialize sensor's ioctl */
+	struct v4l2_ctrl_handler ctrl_handler;
+	int real_model_id;
+	int nctx;
+	int power;
+
+	unsigned int bus_width;
+	unsigned int mode;
+	unsigned int field_inv;
+	unsigned int field_sel;
+	unsigned int ycseq;
+	unsigned int conv422;
+	unsigned int bpat;
+	unsigned int hpol;
+	unsigned int vpol;
+	unsigned int edge;
+	unsigned int bls;
+	unsigned int gamma;
+	unsigned int cconv;
+	unsigned int res;
+	unsigned int dwn_sz;
+	unsigned int blc;
+	unsigned int agc;
+	unsigned int awb;
+	unsigned int aec;
+	/* extension SENSOR version 2 */
+	unsigned int cie_profile;
+
+	/* extension SENSOR version 3 */
+	unsigned int flicker_freq;
+
+	/* extension SENSOR version 4 */
+	unsigned int smia_mode;
+	unsigned int mipi_mode;
+
+	/* Add name here to load shared library */
+	unsigned int type;
+
+	/*Number of MIPI lanes*/
+	unsigned int mipi_lanes;
+	/*WA for low light AE*/
+	unsigned int first_exp;
+	unsigned int first_gain;
+	unsigned int first_diggain;
+	char name[32];
+
+	u8 lightfreq;
+	u8 streamon;
+};
+
+struct mt9m114_format_struct {
+	u8 *desc;
+	u32 pixelformat;
+	struct regval_list *regs;
+};
+
+struct mt9m114_res_struct {
+	u8 *desc;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int skip_frames;
+	bool used;
+	struct regval_list *regs;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+};
+
+/* 2 bytes used for address: 256 bytes total */
+#define MT9M114_MAX_WRITE_BUF_SIZE	254
+struct mt9m114_write_buffer {
+	u16 addr;
+	u8 data[MT9M114_MAX_WRITE_BUF_SIZE];
+};
+
+struct mt9m114_write_ctrl {
+	int index;
+	struct mt9m114_write_buffer buffer;
+};
+
+/*
+ * Modes supported by the mt9m114 driver.
+ * Please, keep them in ascending order.
+ */
+static struct mt9m114_res_struct mt9m114_res[] = {
+	{
+		.desc	= "720P",
+		.res	= MT9M114_RES_736P,
+		.width	= 1296,
+		.height = 736,
+		.fps	= 30,
+		.used	= false,
+		.regs	= NULL,
+		.skip_frames = 1,
+
+		.pixels_per_line = 0x0640,
+		.lines_per_frame = 0x0307,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+	},
+	{
+		.desc	= "848P",
+		.res	= MT9M114_RES_864P,
+		.width	= 1296,
+		.height = 864,
+		.fps	= 30,
+		.used	= false,
+		.regs	= NULL,
+		.skip_frames = 1,
+
+		.pixels_per_line = 0x0640,
+		.lines_per_frame = 0x03E8,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+	},
+	{
+		.desc	= "960P",
+		.res	= MT9M114_RES_960P,
+		.width	= 1296,
+		.height	= 976,
+		.fps	= 30,
+		.used	= false,
+		.regs	= NULL,
+		.skip_frames = 1,
+
+		.pixels_per_line = 0x0644, /* consistent with regs arrays */
+		.lines_per_frame = 0x03E5, /* consistent with regs arrays */
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+	},
+};
+
+#define N_RES (ARRAY_SIZE(mt9m114_res))
+
+#if 0 /* Currently unused */
+static struct misensor_reg const mt9m114_exitstandby[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	/* exit-standby */
+	{MISENSOR_8BIT,  0xDC00, 0x54},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct misensor_reg const mt9m114_exp_win[5][5] = {
+	{
+		{MISENSOR_8BIT,  0xA407, 0x64},
+		{MISENSOR_8BIT,  0xA408, 0x64},
+		{MISENSOR_8BIT,  0xA409, 0x64},
+		{MISENSOR_8BIT,  0xA40A, 0x64},
+		{MISENSOR_8BIT,  0xA40B, 0x64},
+	},
+	{
+		{MISENSOR_8BIT,  0xA40C, 0x64},
+		{MISENSOR_8BIT,  0xA40D, 0x64},
+		{MISENSOR_8BIT,  0xA40E, 0x64},
+		{MISENSOR_8BIT,  0xA40F, 0x64},
+		{MISENSOR_8BIT,  0xA410, 0x64},
+	},
+	{
+		{MISENSOR_8BIT,  0xA411, 0x64},
+		{MISENSOR_8BIT,  0xA412, 0x64},
+		{MISENSOR_8BIT,  0xA413, 0x64},
+		{MISENSOR_8BIT,  0xA414, 0x64},
+		{MISENSOR_8BIT,  0xA415, 0x64},
+	},
+	{
+		{MISENSOR_8BIT,  0xA416, 0x64},
+		{MISENSOR_8BIT,  0xA417, 0x64},
+		{MISENSOR_8BIT,  0xA418, 0x64},
+		{MISENSOR_8BIT,  0xA419, 0x64},
+		{MISENSOR_8BIT,  0xA41A, 0x64},
+	},
+	{
+		{MISENSOR_8BIT,  0xA41B, 0x64},
+		{MISENSOR_8BIT,  0xA41C, 0x64},
+		{MISENSOR_8BIT,  0xA41D, 0x64},
+		{MISENSOR_8BIT,  0xA41E, 0x64},
+		{MISENSOR_8BIT,  0xA41F, 0x64},
+	},
+};
+
+static struct misensor_reg const mt9m114_exp_average[] = {
+	{MISENSOR_8BIT,  0xA407, 0x00},
+	{MISENSOR_8BIT,  0xA408, 0x00},
+	{MISENSOR_8BIT,  0xA409, 0x00},
+	{MISENSOR_8BIT,  0xA40A, 0x00},
+	{MISENSOR_8BIT,  0xA40B, 0x00},
+	{MISENSOR_8BIT,  0xA40C, 0x00},
+	{MISENSOR_8BIT,  0xA40D, 0x00},
+	{MISENSOR_8BIT,  0xA40E, 0x00},
+	{MISENSOR_8BIT,  0xA40F, 0x00},
+	{MISENSOR_8BIT,  0xA410, 0x00},
+	{MISENSOR_8BIT,  0xA411, 0x00},
+	{MISENSOR_8BIT,  0xA412, 0x00},
+	{MISENSOR_8BIT,  0xA413, 0x00},
+	{MISENSOR_8BIT,  0xA414, 0x00},
+	{MISENSOR_8BIT,  0xA415, 0x00},
+	{MISENSOR_8BIT,  0xA416, 0x00},
+	{MISENSOR_8BIT,  0xA417, 0x00},
+	{MISENSOR_8BIT,  0xA418, 0x00},
+	{MISENSOR_8BIT,  0xA419, 0x00},
+	{MISENSOR_8BIT,  0xA41A, 0x00},
+	{MISENSOR_8BIT,  0xA41B, 0x00},
+	{MISENSOR_8BIT,  0xA41C, 0x00},
+	{MISENSOR_8BIT,  0xA41D, 0x00},
+	{MISENSOR_8BIT,  0xA41E, 0x00},
+	{MISENSOR_8BIT,  0xA41F, 0x00},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+static struct misensor_reg const mt9m114_exp_center[] = {
+	{MISENSOR_8BIT,  0xA407, 0x19},
+	{MISENSOR_8BIT,  0xA408, 0x19},
+	{MISENSOR_8BIT,  0xA409, 0x19},
+	{MISENSOR_8BIT,  0xA40A, 0x19},
+	{MISENSOR_8BIT,  0xA40B, 0x19},
+	{MISENSOR_8BIT,  0xA40C, 0x19},
+	{MISENSOR_8BIT,  0xA40D, 0x4B},
+	{MISENSOR_8BIT,  0xA40E, 0x4B},
+	{MISENSOR_8BIT,  0xA40F, 0x4B},
+	{MISENSOR_8BIT,  0xA410, 0x19},
+	{MISENSOR_8BIT,  0xA411, 0x19},
+	{MISENSOR_8BIT,  0xA412, 0x4B},
+	{MISENSOR_8BIT,  0xA413, 0x64},
+	{MISENSOR_8BIT,  0xA414, 0x4B},
+	{MISENSOR_8BIT,  0xA415, 0x19},
+	{MISENSOR_8BIT,  0xA416, 0x19},
+	{MISENSOR_8BIT,  0xA417, 0x4B},
+	{MISENSOR_8BIT,  0xA418, 0x4B},
+	{MISENSOR_8BIT,  0xA419, 0x4B},
+	{MISENSOR_8BIT,  0xA41A, 0x19},
+	{MISENSOR_8BIT,  0xA41B, 0x19},
+	{MISENSOR_8BIT,  0xA41C, 0x19},
+	{MISENSOR_8BIT,  0xA41D, 0x19},
+	{MISENSOR_8BIT,  0xA41E, 0x19},
+	{MISENSOR_8BIT,  0xA41F, 0x19},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+#if 0 /* Currently unused */
+static struct misensor_reg const mt9m114_suspend[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	{MISENSOR_8BIT,  0xDC00, 0x40},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+static struct misensor_reg const mt9m114_streaming[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	{MISENSOR_8BIT,  0xDC00, 0x34},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct misensor_reg const mt9m114_standby_reg[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	{MISENSOR_8BIT,  0xDC00, 0x50},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+#if 0 /* Currently unused */
+static struct misensor_reg const mt9m114_wakeup_reg[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	{MISENSOR_8BIT,  0xDC00, 0x54},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct misensor_reg const mt9m114_chgstat_reg[] = {
+	{MISENSOR_16BIT,  0x098E, 0xDC00},
+	{MISENSOR_8BIT,  0xDC00, 0x28},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+/* [1296x976_30fps] - Intel */
+#if 0
+static struct misensor_reg const mt9m114_960P_init[] = {
+	{MISENSOR_16BIT, 0x098E, 0x1000},
+	{MISENSOR_8BIT, 0xC97E, 0x01},	  /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x0128}, /* cam_sysctl_pll_divider_m_n = 276 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x0000}, /* cam_sensor_cfg_y_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC802, 0x0000}, /* cam_sensor_cfg_x_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC804, 0x03CF}, /* cam_sensor_cfg_y_addr_end = 971 */
+	{MISENSOR_16BIT, 0xC806, 0x050F}, /* cam_sensor_cfg_x_addr_end = 1291 */
+	{MISENSOR_16BIT, 0xC808, 0x02DC}, /* cam_sensor_cfg_pixclk = 48000000 */
+	{MISENSOR_16BIT, 0xC80A, 0x6C00},
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x00DB},
+	/* cam_sensor_cfg_fine_integ_time_max = 1459 */
+	{MISENSOR_16BIT, 0xC810, 0x05B3},
+	/* cam_sensor_cfg_frame_length_lines = 1006 */
+	{MISENSOR_16BIT, 0xC812, 0x03F6},
+	/* cam_sensor_cfg_line_length_pck = 1590 */
+	{MISENSOR_16BIT, 0xC814, 0x063E},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x0060},
+	/* cam_sensor_cfg_cpipe_last_row = 963 */
+	{MISENSOR_16BIT, 0xC818, 0x03C3},
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0000}, /* cam_sensor_control_read_mode = 0 */
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x0508}, /* cam_crop_window_width = 1280 */
+	{MISENSOR_16BIT, 0xC85A, 0x03C8}, /* cam_crop_window_height = 960 */
+	{MISENSOR_8BIT,  0xC85C, 0x03},   /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x0508}, /* cam_output_width = 1280 */
+	{MISENSOR_16BIT, 0xC86A, 0x03C8}, /* cam_output_height = 960 */
+	{MISENSOR_TOK_TERM, 0, 0},
+};
+#endif
+
+/* [1296x976_30fps_768Mbps] */
+static struct misensor_reg const mt9m114_976P_init[] = {
+	{MISENSOR_16BIT, 0x98E, 0x1000},
+	{MISENSOR_8BIT, 0xC97E, 0x01},	  /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x0128}, /* cam_sysctl_pll_divider_m_n = 276 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x0000}, /* cam_sensor_cfg_y_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC802, 0x0000}, /* cam_sensor_cfg_x_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC804, 0x03CF}, /* cam_sensor_cfg_y_addr_end = 975 */
+	{MISENSOR_16BIT, 0xC806, 0x050F}, /* cam_sensor_cfg_x_addr_end = 1295 */
+	{MISENSOR_32BIT, 0xC808, 0x2DC6C00},/* cam_sensor_cfg_pixclk = 480000*/
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x00DB},
+	/* 0x062E //cam_sensor_cfg_fine_integ_time_max = 1459 */
+	{MISENSOR_16BIT, 0xC810, 0x05B3},
+	/* 0x074C //cam_sensor_cfg_frame_length_lines = 1006 */
+	{MISENSOR_16BIT, 0xC812, 0x03E5},
+	/* 0x06B1 /cam_sensor_cfg_line_length_pck = 1590 */
+	{MISENSOR_16BIT, 0xC814, 0x0644},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x0060},
+	/* cam_sensor_cfg_cpipe_last_row = 963 */
+	{MISENSOR_16BIT, 0xC818, 0x03C3},
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0000}, /* cam_sensor_control_read_mode = 0 */
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x0508}, /* cam_crop_window_width = 1288 */
+	{MISENSOR_16BIT, 0xC85A, 0x03C8}, /* cam_crop_window_height = 968 */
+	{MISENSOR_8BIT, 0xC85C, 0x03}, /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x0508}, /* cam_output_width = 1288 */
+	{MISENSOR_16BIT, 0xC86A, 0x03C8}, /* cam_output_height = 968 */
+	{MISENSOR_8BIT, 0xC878, 0x00}, /* 0x0E //cam_aet_aemode = 0 */
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+/* [1296x864_30fps] */
+static struct misensor_reg const mt9m114_864P_init[] = {
+	{MISENSOR_16BIT, 0x98E, 0x1000},
+	{MISENSOR_8BIT, 0xC97E, 0x01},	  /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x0128}, /* cam_sysctl_pll_divider_m_n = 276 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x0038}, /* cam_sensor_cfg_y_addr_start = 56 */
+	{MISENSOR_16BIT, 0xC802, 0x0000}, /* cam_sensor_cfg_x_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC804, 0x0397}, /* cam_sensor_cfg_y_addr_end = 919 */
+	{MISENSOR_16BIT, 0xC806, 0x050F}, /* cam_sensor_cfg_x_addr_end = 1295 */
+	/* cam_sensor_cfg_pixclk = 48000000 */
+	{MISENSOR_32BIT, 0xC808, 0x2DC6C00},
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x00DB},
+	/* cam_sensor_cfg_fine_integ_time_max = 1469 */
+	{MISENSOR_16BIT, 0xC810, 0x05BD},
+	/* cam_sensor_cfg_frame_length_lines = 1000 */
+	{MISENSOR_16BIT, 0xC812, 0x03E8},
+	/* cam_sensor_cfg_line_length_pck = 1600 */
+	{MISENSOR_16BIT, 0xC814, 0x0640},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x0060},
+	/* cam_sensor_cfg_cpipe_last_row = 859 */
+	{MISENSOR_16BIT, 0xC818, 0x035B},
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0000}, /* cam_sensor_control_read_mode = 0 */
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x0508}, /* cam_crop_window_width = 1288 */
+	{MISENSOR_16BIT, 0xC85A, 0x0358}, /* cam_crop_window_height = 856 */
+	{MISENSOR_8BIT, 0xC85C, 0x03}, /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x0508}, /* cam_output_width = 1288 */
+	{MISENSOR_16BIT, 0xC86A, 0x0358}, /* cam_output_height = 856 */
+	{MISENSOR_8BIT, 0xC878, 0x00}, /* 0x0E //cam_aet_aemode = 0 */
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+/* [1296x736_30fps] */
+static struct misensor_reg const mt9m114_736P_init[] = {
+	{MISENSOR_16BIT, 0x98E, 0x1000},
+	{MISENSOR_8BIT, 0xC97E, 0x01},	  /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x011F}, /* cam_sysctl_pll_divider_m_n = 287 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x0078}, /* cam_sensor_cfg_y_addr_start = 120*/
+	{MISENSOR_16BIT, 0xC802, 0x0000}, /* cam_sensor_cfg_x_addr_start = 0 */
+	{MISENSOR_16BIT, 0xC804, 0x0357}, /* cam_sensor_cfg_y_addr_end = 855 */
+	{MISENSOR_16BIT, 0xC806, 0x050F}, /* cam_sensor_cfg_x_addr_end = 1295 */
+	{MISENSOR_32BIT, 0xC808, 0x237A07F}, /* cam_sensor_cfg_pixclk=37199999*/
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x00DB},
+	/* 0x062E //cam_sensor_cfg_fine_integ_time_max = 1469 */
+	{MISENSOR_16BIT, 0xC810, 0x05BD},
+	/* 0x074C //cam_sensor_cfg_frame_length_lines = 775 */
+	{MISENSOR_16BIT, 0xC812, 0x0307},
+	/* 0x06B1 /cam_sensor_cfg_line_length_pck = 1600 */
+	{MISENSOR_16BIT, 0xC814, 0x0640},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x0060},
+	/* cam_sensor_cfg_cpipe_last_row = 731 */
+	{MISENSOR_16BIT, 0xC818, 0x02DB},
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0000}, /* cam_sensor_control_read_mode = 0 */
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x0508}, /* cam_crop_window_width = 1288 */
+	{MISENSOR_16BIT, 0xC85A, 0x02D8}, /* cam_crop_window_height = 728 */
+	{MISENSOR_8BIT, 0xC85C, 0x03}, /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x0508}, /* cam_output_width = 1288 */
+	{MISENSOR_16BIT, 0xC86A, 0x02D8}, /* cam_output_height = 728 */
+	{MISENSOR_8BIT, 0xC878, 0x00}, /* 0x0E //cam_aet_aemode = 0 */
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+/* [736x496_30fps_768Mbps] */
+#if 0 /* Currently unused */
+static struct misensor_reg const mt9m114_720_480P_init[] = {
+	{MISENSOR_16BIT, 0x98E, 0x1000},
+	{MISENSOR_8BIT, 0xC97E, 0x01},	  /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x0128}, /* cam_sysctl_pll_divider_m_n = 276 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x00F0}, /* cam_sensor_cfg_y_addr_start = 240*/
+	{MISENSOR_16BIT, 0xC802, 0x0118}, /* cam_sensor_cfg_x_addr_start = 280*/
+	{MISENSOR_16BIT, 0xC804, 0x02DF}, /* cam_sensor_cfg_y_addr_end = 735 */
+	{MISENSOR_16BIT, 0xC806, 0x03F7}, /* cam_sensor_cfg_x_addr_end = 1015 */
+	/* cam_sensor_cfg_pixclk = 48000000 */
+	{MISENSOR_32BIT, 0xC808, 0x2DC6C00},
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x00DB},
+	/* 0x062E //cam_sensor_cfg_fine_integ_time_max = 1459 */
+	{MISENSOR_16BIT, 0xC810, 0x05B3},
+	/* 0x074C //cam_sensor_cfg_frame_length_lines = 997 */
+	{MISENSOR_16BIT, 0xC812, 0x03E5},
+	/* 0x06B1 /cam_sensor_cfg_line_length_pck = 1604 */
+	{MISENSOR_16BIT, 0xC814, 0x0644},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x0060},
+	{MISENSOR_16BIT, 0xC818, 0x03C3}, /* cam_sensor_cfg_cpipe_last_row=963*/
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0000}, /* cam_sensor_control_read_mode = 0*/
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x02D8}, /* cam_crop_window_width = 728 */
+	{MISENSOR_16BIT, 0xC85A, 0x01E8}, /* cam_crop_window_height = 488 */
+	{MISENSOR_8BIT, 0xC85C, 0x03}, /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x02D8}, /* cam_output_width = 728 */
+	{MISENSOR_16BIT, 0xC86A, 0x01E8}, /* cam_output_height = 488 */
+	{MISENSOR_8BIT, 0xC878, 0x00}, /* 0x0E //cam_aet_aemode = 0 */
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct misensor_reg const mt9m114_common[] = {
+	/* reset */
+	{MISENSOR_16BIT,  0x301A, 0x0234},
+	/* LOAD = Step2-PLL_Timing      //PLL and Timing */
+	{MISENSOR_16BIT, 0x098E, 0x1000}, /* LOGICAL_ADDRESS_ACCESS */
+	{MISENSOR_8BIT, 0xC97E, 0x01},    /* cam_sysctl_pll_enable = 1 */
+	{MISENSOR_16BIT, 0xC980, 0x0128}, /* cam_sysctl_pll_divider_m_n = 276 */
+	{MISENSOR_16BIT, 0xC982, 0x0700}, /* cam_sysctl_pll_divider_p = 1792 */
+	{MISENSOR_16BIT, 0xC800, 0x0000}, /* cam_sensor_cfg_y_addr_start = 216*/
+	{MISENSOR_16BIT, 0xC802, 0x0000}, /* cam_sensor_cfg_x_addr_start = 168*/
+	{MISENSOR_16BIT, 0xC804, 0x03CD}, /* cam_sensor_cfg_y_addr_end = 761 */
+	{MISENSOR_16BIT, 0xC806, 0x050D}, /* cam_sensor_cfg_x_addr_end = 1127 */
+	{MISENSOR_16BIT, 0xC808, 0x02DC}, /* cam_sensor_cfg_pixclk = 24000000 */
+	{MISENSOR_16BIT, 0xC80A, 0x6C00},
+	{MISENSOR_16BIT, 0xC80C, 0x0001}, /* cam_sensor_cfg_row_speed = 1 */
+	/* cam_sensor_cfg_fine_integ_time_min = 219 */
+	{MISENSOR_16BIT, 0xC80E, 0x01C3},
+	/* cam_sensor_cfg_fine_integ_time_max = 1149 */
+	{MISENSOR_16BIT, 0xC810, 0x03F7},
+	/* cam_sensor_cfg_frame_length_lines = 625 */
+	{MISENSOR_16BIT, 0xC812, 0x0500},
+	/* cam_sensor_cfg_line_length_pck = 1280 */
+	{MISENSOR_16BIT, 0xC814, 0x04E2},
+	/* cam_sensor_cfg_fine_correction = 96 */
+	{MISENSOR_16BIT, 0xC816, 0x00E0},
+	/* cam_sensor_cfg_cpipe_last_row = 541 */
+	{MISENSOR_16BIT, 0xC818, 0x01E3},
+	{MISENSOR_16BIT, 0xC826, 0x0020}, /* cam_sensor_cfg_reg_0_data = 32 */
+	{MISENSOR_16BIT, 0xC834, 0x0330}, /* cam_sensor_control_read_mode = 0 */
+	{MISENSOR_16BIT, 0xC854, 0x0000}, /* cam_crop_window_xoffset = 0 */
+	{MISENSOR_16BIT, 0xC856, 0x0000}, /* cam_crop_window_yoffset = 0 */
+	{MISENSOR_16BIT, 0xC858, 0x0280}, /* cam_crop_window_width = 952 */
+	{MISENSOR_16BIT, 0xC85A, 0x01E0}, /* cam_crop_window_height = 538 */
+	{MISENSOR_8BIT, 0xC85C, 0x03},    /* cam_crop_cropmode = 3 */
+	{MISENSOR_16BIT, 0xC868, 0x0280}, /* cam_output_width = 952 */
+	{MISENSOR_16BIT, 0xC86A, 0x01E0}, /* cam_output_height = 538 */
+	/*
+	 * LOAD = Step3-Recommended
+	 * Patch, Errata and Sensor optimization Setting
+	 */
+	{MISENSOR_16BIT, 0x316A, 0x8270}, /* DAC_TXLO_ROW */
+	{MISENSOR_16BIT, 0x316C, 0x8270}, /* DAC_TXLO */
+	{MISENSOR_16BIT, 0x3ED0, 0x2305}, /* DAC_LD_4_5 */
+	{MISENSOR_16BIT, 0x3ED2, 0x77CF}, /* DAC_LD_6_7 */
+	{MISENSOR_16BIT, 0x316E, 0x8202}, /* DAC_ECL */
+	{MISENSOR_16BIT, 0x3180, 0x87FF}, /* DELTA_DK_CONTROL */
+	{MISENSOR_16BIT, 0x30D4, 0x6080}, /* COLUMN_CORRECTION */
+	{MISENSOR_16BIT, 0xA802, 0x0008}, /* AE_TRACK_MODE */
+	{MISENSOR_16BIT, 0x3E14, 0xFF39}, /* SAMP_COL_PUP2 */
+	{MISENSOR_16BIT, 0x31E0, 0x0003}, /* PIX_DEF_ID */
+	/* LOAD = Step8-Features	//Ports, special features, etc. */
+	{MISENSOR_16BIT, 0x098E, 0x0000}, /* LOGICAL_ADDRESS_ACCESS */
+	{MISENSOR_16BIT, 0x001E, 0x0777}, /* PAD_SLEW */
+	{MISENSOR_16BIT, 0x098E, 0x0000}, /* LOGICAL_ADDRESS_ACCESS */
+	{MISENSOR_16BIT, 0xC984, 0x8001}, /* CAM_PORT_OUTPUT_CONTROL */
+	{MISENSOR_16BIT, 0xC988, 0x0F00}, /* CAM_PORT_MIPI_TIMING_T_HS_ZERO */
+	/* CAM_PORT_MIPI_TIMING_T_HS_EXIT_HS_TRAIL */
+	{MISENSOR_16BIT, 0xC98A, 0x0B07},
+	/* CAM_PORT_MIPI_TIMING_T_CLK_POST_CLK_PRE */
+	{MISENSOR_16BIT, 0xC98C, 0x0D01},
+	/* CAM_PORT_MIPI_TIMING_T_CLK_TRAIL_CLK_ZERO */
+	{MISENSOR_16BIT, 0xC98E, 0x071D},
+	{MISENSOR_16BIT, 0xC990, 0x0006}, /* CAM_PORT_MIPI_TIMING_T_LPX */
+	{MISENSOR_16BIT, 0xC992, 0x0A0C}, /* CAM_PORT_MIPI_TIMING_INIT_TIMING */
+	{MISENSOR_16BIT, 0x3C5A, 0x0009}, /* MIPI_DELAY_TRIM */
+	{MISENSOR_16BIT, 0xC86C, 0x0210}, /* CAM_OUTPUT_FORMAT */
+	{MISENSOR_16BIT, 0xA804, 0x0000}, /* AE_TRACK_ALGO */
+	/* default exposure */
+	{MISENSOR_16BIT, 0x3012, 0x0110}, /* COMMAND_REGISTER */
+	{MISENSOR_TOK_TERM, 0, 0},
+
+};
+
+#if 0 /* Currently unused */
+static struct misensor_reg const mt9m114_antiflicker_50hz[] = {
+	{MISENSOR_16BIT,  0x098E, 0xC88B},
+	{MISENSOR_8BIT,  0xC88B, 0x32},
+	{MISENSOR_8BIT,  0xDC00, 0x28},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+static struct misensor_reg const mt9m114_antiflicker_60hz[] = {
+	{MISENSOR_16BIT,  0x098E, 0xC88B},
+	{MISENSOR_8BIT,  0xC88B, 0x3C},
+	{MISENSOR_8BIT,  0xDC00, 0x28},
+	{MISENSOR_16BIT,  0x0080, 0x8002},
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+static struct misensor_reg const mt9m114_iq[] = {
+	/* [Step3-Recommended] [Sensor optimization] */
+	{MISENSOR_16BIT,	0x316A, 0x8270},
+	{MISENSOR_16BIT,	0x316C, 0x8270},
+	{MISENSOR_16BIT,	0x3ED0, 0x2305},
+	{MISENSOR_16BIT,	0x3ED2, 0x77CF},
+	{MISENSOR_16BIT,	0x316E, 0x8202},
+	{MISENSOR_16BIT,	0x3180, 0x87FF},
+	{MISENSOR_16BIT,	0x30D4, 0x6080},
+	{MISENSOR_16BIT,	0xA802, 0x0008},
+
+	/* This register is from vender to avoid low light color noise */
+	{MISENSOR_16BIT,	0x31E0, 0x0001},
+
+	/* LOAD=Errata item 1 */
+	{MISENSOR_16BIT,	0x3E14, 0xFF39},
+
+	/* LOAD=Errata item 2 */
+	{MISENSOR_16BIT,	0x301A, 0x8234},
+
+	/*
+	 * LOAD=Errata item 3
+	 * LOAD=Patch 0202;
+	 * Feature Recommended; Black level correction fix
+	 */
+	{MISENSOR_16BIT,	0x0982, 0x0001},
+	{MISENSOR_16BIT,	0x098A, 0x5000},
+	{MISENSOR_16BIT,	0xD000, 0x70CF},
+	{MISENSOR_16BIT,	0xD002, 0xFFFF},
+	{MISENSOR_16BIT,	0xD004, 0xC5D4},
+	{MISENSOR_16BIT,	0xD006, 0x903A},
+	{MISENSOR_16BIT,	0xD008, 0x2144},
+	{MISENSOR_16BIT,	0xD00A, 0x0C00},
+	{MISENSOR_16BIT,	0xD00C, 0x2186},
+	{MISENSOR_16BIT,	0xD00E, 0x0FF3},
+	{MISENSOR_16BIT,	0xD010, 0xB844},
+	{MISENSOR_16BIT,	0xD012, 0xB948},
+	{MISENSOR_16BIT,	0xD014, 0xE082},
+	{MISENSOR_16BIT,	0xD016, 0x20CC},
+	{MISENSOR_16BIT,	0xD018, 0x80E2},
+	{MISENSOR_16BIT,	0xD01A, 0x21CC},
+	{MISENSOR_16BIT,	0xD01C, 0x80A2},
+	{MISENSOR_16BIT,	0xD01E, 0x21CC},
+	{MISENSOR_16BIT,	0xD020, 0x80E2},
+	{MISENSOR_16BIT,	0xD022, 0xF404},
+	{MISENSOR_16BIT,	0xD024, 0xD801},
+	{MISENSOR_16BIT,	0xD026, 0xF003},
+	{MISENSOR_16BIT,	0xD028, 0xD800},
+	{MISENSOR_16BIT,	0xD02A, 0x7EE0},
+	{MISENSOR_16BIT,	0xD02C, 0xC0F1},
+	{MISENSOR_16BIT,	0xD02E, 0x08BA},
+
+	{MISENSOR_16BIT,	0xD030, 0x0600},
+	{MISENSOR_16BIT,	0xD032, 0xC1A1},
+	{MISENSOR_16BIT,	0xD034, 0x76CF},
+	{MISENSOR_16BIT,	0xD036, 0xFFFF},
+	{MISENSOR_16BIT,	0xD038, 0xC130},
+	{MISENSOR_16BIT,	0xD03A, 0x6E04},
+	{MISENSOR_16BIT,	0xD03C, 0xC040},
+	{MISENSOR_16BIT,	0xD03E, 0x71CF},
+	{MISENSOR_16BIT,	0xD040, 0xFFFF},
+	{MISENSOR_16BIT,	0xD042, 0xC790},
+	{MISENSOR_16BIT,	0xD044, 0x8103},
+	{MISENSOR_16BIT,	0xD046, 0x77CF},
+	{MISENSOR_16BIT,	0xD048, 0xFFFF},
+	{MISENSOR_16BIT,	0xD04A, 0xC7C0},
+	{MISENSOR_16BIT,	0xD04C, 0xE001},
+	{MISENSOR_16BIT,	0xD04E, 0xA103},
+	{MISENSOR_16BIT,	0xD050, 0xD800},
+	{MISENSOR_16BIT,	0xD052, 0x0C6A},
+	{MISENSOR_16BIT,	0xD054, 0x04E0},
+	{MISENSOR_16BIT,	0xD056, 0xB89E},
+	{MISENSOR_16BIT,	0xD058, 0x7508},
+	{MISENSOR_16BIT,	0xD05A, 0x8E1C},
+	{MISENSOR_16BIT,	0xD05C, 0x0809},
+	{MISENSOR_16BIT,	0xD05E, 0x0191},
+
+	{MISENSOR_16BIT,	0xD060, 0xD801},
+	{MISENSOR_16BIT,	0xD062, 0xAE1D},
+	{MISENSOR_16BIT,	0xD064, 0xE580},
+	{MISENSOR_16BIT,	0xD066, 0x20CA},
+	{MISENSOR_16BIT,	0xD068, 0x0022},
+	{MISENSOR_16BIT,	0xD06A, 0x20CF},
+	{MISENSOR_16BIT,	0xD06C, 0x0522},
+	{MISENSOR_16BIT,	0xD06E, 0x0C5C},
+	{MISENSOR_16BIT,	0xD070, 0x04E2},
+	{MISENSOR_16BIT,	0xD072, 0x21CA},
+	{MISENSOR_16BIT,	0xD074, 0x0062},
+	{MISENSOR_16BIT,	0xD076, 0xE580},
+	{MISENSOR_16BIT,	0xD078, 0xD901},
+	{MISENSOR_16BIT,	0xD07A, 0x79C0},
+	{MISENSOR_16BIT,	0xD07C, 0xD800},
+	{MISENSOR_16BIT,	0xD07E, 0x0BE6},
+	{MISENSOR_16BIT,	0xD080, 0x04E0},
+	{MISENSOR_16BIT,	0xD082, 0xB89E},
+	{MISENSOR_16BIT,	0xD084, 0x70CF},
+	{MISENSOR_16BIT,	0xD086, 0xFFFF},
+	{MISENSOR_16BIT,	0xD088, 0xC8D4},
+	{MISENSOR_16BIT,	0xD08A, 0x9002},
+	{MISENSOR_16BIT,	0xD08C, 0x0857},
+	{MISENSOR_16BIT,	0xD08E, 0x025E},
+
+	{MISENSOR_16BIT,	0xD090, 0xFFDC},
+	{MISENSOR_16BIT,	0xD092, 0xE080},
+	{MISENSOR_16BIT,	0xD094, 0x25CC},
+	{MISENSOR_16BIT,	0xD096, 0x9022},
+	{MISENSOR_16BIT,	0xD098, 0xF225},
+	{MISENSOR_16BIT,	0xD09A, 0x1700},
+	{MISENSOR_16BIT,	0xD09C, 0x108A},
+	{MISENSOR_16BIT,	0xD09E, 0x73CF},
+	{MISENSOR_16BIT,	0xD0A0, 0xFF00},
+	{MISENSOR_16BIT,	0xD0A2, 0x3174},
+	{MISENSOR_16BIT,	0xD0A4, 0x9307},
+	{MISENSOR_16BIT,	0xD0A6, 0x2A04},
+	{MISENSOR_16BIT,	0xD0A8, 0x103E},
+	{MISENSOR_16BIT,	0xD0AA, 0x9328},
+	{MISENSOR_16BIT,	0xD0AC, 0x2942},
+	{MISENSOR_16BIT,	0xD0AE, 0x7140},
+	{MISENSOR_16BIT,	0xD0B0, 0x2A04},
+	{MISENSOR_16BIT,	0xD0B2, 0x107E},
+	{MISENSOR_16BIT,	0xD0B4, 0x9349},
+	{MISENSOR_16BIT,	0xD0B6, 0x2942},
+	{MISENSOR_16BIT,	0xD0B8, 0x7141},
+	{MISENSOR_16BIT,	0xD0BA, 0x2A04},
+	{MISENSOR_16BIT,	0xD0BC, 0x10BE},
+	{MISENSOR_16BIT,	0xD0BE, 0x934A},
+
+	{MISENSOR_16BIT,	0xD0C0, 0x2942},
+	{MISENSOR_16BIT,	0xD0C2, 0x714B},
+	{MISENSOR_16BIT,	0xD0C4, 0x2A04},
+	{MISENSOR_16BIT,	0xD0C6, 0x10BE},
+	{MISENSOR_16BIT,	0xD0C8, 0x130C},
+	{MISENSOR_16BIT,	0xD0CA, 0x010A},
+	{MISENSOR_16BIT,	0xD0CC, 0x2942},
+	{MISENSOR_16BIT,	0xD0CE, 0x7142},
+	{MISENSOR_16BIT,	0xD0D0, 0x2250},
+	{MISENSOR_16BIT,	0xD0D2, 0x13CA},
+	{MISENSOR_16BIT,	0xD0D4, 0x1B0C},
+	{MISENSOR_16BIT,	0xD0D6, 0x0284},
+	{MISENSOR_16BIT,	0xD0D8, 0xB307},
+	{MISENSOR_16BIT,	0xD0DA, 0xB328},
+	{MISENSOR_16BIT,	0xD0DC, 0x1B12},
+	{MISENSOR_16BIT,	0xD0DE, 0x02C4},
+	{MISENSOR_16BIT,	0xD0E0, 0xB34A},
+	{MISENSOR_16BIT,	0xD0E2, 0xED88},
+	{MISENSOR_16BIT,	0xD0E4, 0x71CF},
+	{MISENSOR_16BIT,	0xD0E6, 0xFF00},
+	{MISENSOR_16BIT,	0xD0E8, 0x3174},
+	{MISENSOR_16BIT,	0xD0EA, 0x9106},
+	{MISENSOR_16BIT,	0xD0EC, 0xB88F},
+	{MISENSOR_16BIT,	0xD0EE, 0xB106},
+
+	{MISENSOR_16BIT,	0xD0F0, 0x210A},
+	{MISENSOR_16BIT,	0xD0F2, 0x8340},
+	{MISENSOR_16BIT,	0xD0F4, 0xC000},
+	{MISENSOR_16BIT,	0xD0F6, 0x21CA},
+	{MISENSOR_16BIT,	0xD0F8, 0x0062},
+	{MISENSOR_16BIT,	0xD0FA, 0x20F0},
+	{MISENSOR_16BIT,	0xD0FC, 0x0040},
+	{MISENSOR_16BIT,	0xD0FE, 0x0B02},
+	{MISENSOR_16BIT,	0xD100, 0x0320},
+	{MISENSOR_16BIT,	0xD102, 0xD901},
+	{MISENSOR_16BIT,	0xD104, 0x07F1},
+	{MISENSOR_16BIT,	0xD106, 0x05E0},
+	{MISENSOR_16BIT,	0xD108, 0xC0A1},
+	{MISENSOR_16BIT,	0xD10A, 0x78E0},
+	{MISENSOR_16BIT,	0xD10C, 0xC0F1},
+	{MISENSOR_16BIT,	0xD10E, 0x71CF},
+	{MISENSOR_16BIT,	0xD110, 0xFFFF},
+	{MISENSOR_16BIT,	0xD112, 0xC7C0},
+	{MISENSOR_16BIT,	0xD114, 0xD840},
+	{MISENSOR_16BIT,	0xD116, 0xA900},
+	{MISENSOR_16BIT,	0xD118, 0x71CF},
+	{MISENSOR_16BIT,	0xD11A, 0xFFFF},
+	{MISENSOR_16BIT,	0xD11C, 0xD02C},
+	{MISENSOR_16BIT,	0xD11E, 0xD81E},
+
+	{MISENSOR_16BIT,	0xD120, 0x0A5A},
+	{MISENSOR_16BIT,	0xD122, 0x04E0},
+	{MISENSOR_16BIT,	0xD124, 0xDA00},
+	{MISENSOR_16BIT,	0xD126, 0xD800},
+	{MISENSOR_16BIT,	0xD128, 0xC0D1},
+	{MISENSOR_16BIT,	0xD12A, 0x7EE0},
+
+	{MISENSOR_16BIT,	0x098E, 0x0000},
+	{MISENSOR_16BIT,	0xE000, 0x010C},
+	{MISENSOR_16BIT,	0xE002, 0x0202},
+	{MISENSOR_16BIT,	0xE004, 0x4103},
+	{MISENSOR_16BIT,	0xE006, 0x0202},
+	{MISENSOR_16BIT,	0x0080, 0xFFF0},
+	{MISENSOR_16BIT,	0x0080, 0xFFF1},
+
+	/* LOAD=Patch 0302; Feature Recommended; Adaptive Sensitivity */
+	{MISENSOR_16BIT,	0x0982, 0x0001},
+	{MISENSOR_16BIT,	0x098A, 0x512C},
+	{MISENSOR_16BIT,	0xD12C, 0x70CF},
+	{MISENSOR_16BIT,	0xD12E, 0xFFFF},
+	{MISENSOR_16BIT,	0xD130, 0xC5D4},
+	{MISENSOR_16BIT,	0xD132, 0x903A},
+	{MISENSOR_16BIT,	0xD134, 0x2144},
+	{MISENSOR_16BIT,	0xD136, 0x0C00},
+	{MISENSOR_16BIT,	0xD138, 0x2186},
+	{MISENSOR_16BIT,	0xD13A, 0x0FF3},
+	{MISENSOR_16BIT,	0xD13C, 0xB844},
+	{MISENSOR_16BIT,	0xD13E, 0x262F},
+	{MISENSOR_16BIT,	0xD140, 0xF008},
+	{MISENSOR_16BIT,	0xD142, 0xB948},
+	{MISENSOR_16BIT,	0xD144, 0x21CC},
+	{MISENSOR_16BIT,	0xD146, 0x8021},
+	{MISENSOR_16BIT,	0xD148, 0xD801},
+	{MISENSOR_16BIT,	0xD14A, 0xF203},
+	{MISENSOR_16BIT,	0xD14C, 0xD800},
+	{MISENSOR_16BIT,	0xD14E, 0x7EE0},
+	{MISENSOR_16BIT,	0xD150, 0xC0F1},
+	{MISENSOR_16BIT,	0xD152, 0x71CF},
+	{MISENSOR_16BIT,	0xD154, 0xFFFF},
+	{MISENSOR_16BIT,	0xD156, 0xC610},
+	{MISENSOR_16BIT,	0xD158, 0x910E},
+	{MISENSOR_16BIT,	0xD15A, 0x208C},
+	{MISENSOR_16BIT,	0xD15C, 0x8014},
+	{MISENSOR_16BIT,	0xD15E, 0xF418},
+	{MISENSOR_16BIT,	0xD160, 0x910F},
+	{MISENSOR_16BIT,	0xD162, 0x208C},
+	{MISENSOR_16BIT,	0xD164, 0x800F},
+	{MISENSOR_16BIT,	0xD166, 0xF414},
+	{MISENSOR_16BIT,	0xD168, 0x9116},
+	{MISENSOR_16BIT,	0xD16A, 0x208C},
+	{MISENSOR_16BIT,	0xD16C, 0x800A},
+	{MISENSOR_16BIT,	0xD16E, 0xF410},
+	{MISENSOR_16BIT,	0xD170, 0x9117},
+	{MISENSOR_16BIT,	0xD172, 0x208C},
+	{MISENSOR_16BIT,	0xD174, 0x8807},
+	{MISENSOR_16BIT,	0xD176, 0xF40C},
+	{MISENSOR_16BIT,	0xD178, 0x9118},
+	{MISENSOR_16BIT,	0xD17A, 0x2086},
+	{MISENSOR_16BIT,	0xD17C, 0x0FF3},
+	{MISENSOR_16BIT,	0xD17E, 0xB848},
+	{MISENSOR_16BIT,	0xD180, 0x080D},
+	{MISENSOR_16BIT,	0xD182, 0x0090},
+	{MISENSOR_16BIT,	0xD184, 0xFFEA},
+	{MISENSOR_16BIT,	0xD186, 0xE081},
+	{MISENSOR_16BIT,	0xD188, 0xD801},
+	{MISENSOR_16BIT,	0xD18A, 0xF203},
+	{MISENSOR_16BIT,	0xD18C, 0xD800},
+	{MISENSOR_16BIT,	0xD18E, 0xC0D1},
+	{MISENSOR_16BIT,	0xD190, 0x7EE0},
+	{MISENSOR_16BIT,	0xD192, 0x78E0},
+	{MISENSOR_16BIT,	0xD194, 0xC0F1},
+	{MISENSOR_16BIT,	0xD196, 0x71CF},
+	{MISENSOR_16BIT,	0xD198, 0xFFFF},
+	{MISENSOR_16BIT,	0xD19A, 0xC610},
+	{MISENSOR_16BIT,	0xD19C, 0x910E},
+	{MISENSOR_16BIT,	0xD19E, 0x208C},
+	{MISENSOR_16BIT,	0xD1A0, 0x800A},
+	{MISENSOR_16BIT,	0xD1A2, 0xF418},
+	{MISENSOR_16BIT,	0xD1A4, 0x910F},
+	{MISENSOR_16BIT,	0xD1A6, 0x208C},
+	{MISENSOR_16BIT,	0xD1A8, 0x8807},
+	{MISENSOR_16BIT,	0xD1AA, 0xF414},
+	{MISENSOR_16BIT,	0xD1AC, 0x9116},
+	{MISENSOR_16BIT,	0xD1AE, 0x208C},
+	{MISENSOR_16BIT,	0xD1B0, 0x800A},
+	{MISENSOR_16BIT,	0xD1B2, 0xF410},
+	{MISENSOR_16BIT,	0xD1B4, 0x9117},
+	{MISENSOR_16BIT,	0xD1B6, 0x208C},
+	{MISENSOR_16BIT,	0xD1B8, 0x8807},
+	{MISENSOR_16BIT,	0xD1BA, 0xF40C},
+	{MISENSOR_16BIT,	0xD1BC, 0x9118},
+	{MISENSOR_16BIT,	0xD1BE, 0x2086},
+	{MISENSOR_16BIT,	0xD1C0, 0x0FF3},
+	{MISENSOR_16BIT,	0xD1C2, 0xB848},
+	{MISENSOR_16BIT,	0xD1C4, 0x080D},
+	{MISENSOR_16BIT,	0xD1C6, 0x0090},
+	{MISENSOR_16BIT,	0xD1C8, 0xFFD9},
+	{MISENSOR_16BIT,	0xD1CA, 0xE080},
+	{MISENSOR_16BIT,	0xD1CC, 0xD801},
+	{MISENSOR_16BIT,	0xD1CE, 0xF203},
+	{MISENSOR_16BIT,	0xD1D0, 0xD800},
+	{MISENSOR_16BIT,	0xD1D2, 0xF1DF},
+	{MISENSOR_16BIT,	0xD1D4, 0x9040},
+	{MISENSOR_16BIT,	0xD1D6, 0x71CF},
+	{MISENSOR_16BIT,	0xD1D8, 0xFFFF},
+	{MISENSOR_16BIT,	0xD1DA, 0xC5D4},
+	{MISENSOR_16BIT,	0xD1DC, 0xB15A},
+	{MISENSOR_16BIT,	0xD1DE, 0x9041},
+	{MISENSOR_16BIT,	0xD1E0, 0x73CF},
+	{MISENSOR_16BIT,	0xD1E2, 0xFFFF},
+	{MISENSOR_16BIT,	0xD1E4, 0xC7D0},
+	{MISENSOR_16BIT,	0xD1E6, 0xB140},
+	{MISENSOR_16BIT,	0xD1E8, 0x9042},
+	{MISENSOR_16BIT,	0xD1EA, 0xB141},
+	{MISENSOR_16BIT,	0xD1EC, 0x9043},
+	{MISENSOR_16BIT,	0xD1EE, 0xB142},
+	{MISENSOR_16BIT,	0xD1F0, 0x9044},
+	{MISENSOR_16BIT,	0xD1F2, 0xB143},
+	{MISENSOR_16BIT,	0xD1F4, 0x9045},
+	{MISENSOR_16BIT,	0xD1F6, 0xB147},
+	{MISENSOR_16BIT,	0xD1F8, 0x9046},
+	{MISENSOR_16BIT,	0xD1FA, 0xB148},
+	{MISENSOR_16BIT,	0xD1FC, 0x9047},
+	{MISENSOR_16BIT,	0xD1FE, 0xB14B},
+	{MISENSOR_16BIT,	0xD200, 0x9048},
+	{MISENSOR_16BIT,	0xD202, 0xB14C},
+	{MISENSOR_16BIT,	0xD204, 0x9049},
+	{MISENSOR_16BIT,	0xD206, 0x1958},
+	{MISENSOR_16BIT,	0xD208, 0x0084},
+	{MISENSOR_16BIT,	0xD20A, 0x904A},
+	{MISENSOR_16BIT,	0xD20C, 0x195A},
+	{MISENSOR_16BIT,	0xD20E, 0x0084},
+	{MISENSOR_16BIT,	0xD210, 0x8856},
+	{MISENSOR_16BIT,	0xD212, 0x1B36},
+	{MISENSOR_16BIT,	0xD214, 0x8082},
+	{MISENSOR_16BIT,	0xD216, 0x8857},
+	{MISENSOR_16BIT,	0xD218, 0x1B37},
+	{MISENSOR_16BIT,	0xD21A, 0x8082},
+	{MISENSOR_16BIT,	0xD21C, 0x904C},
+	{MISENSOR_16BIT,	0xD21E, 0x19A7},
+	{MISENSOR_16BIT,	0xD220, 0x009C},
+	{MISENSOR_16BIT,	0xD222, 0x881A},
+	{MISENSOR_16BIT,	0xD224, 0x7FE0},
+	{MISENSOR_16BIT,	0xD226, 0x1B54},
+	{MISENSOR_16BIT,	0xD228, 0x8002},
+	{MISENSOR_16BIT,	0xD22A, 0x78E0},
+	{MISENSOR_16BIT,	0xD22C, 0x71CF},
+	{MISENSOR_16BIT,	0xD22E, 0xFFFF},
+	{MISENSOR_16BIT,	0xD230, 0xC350},
+	{MISENSOR_16BIT,	0xD232, 0xD828},
+	{MISENSOR_16BIT,	0xD234, 0xA90B},
+	{MISENSOR_16BIT,	0xD236, 0x8100},
+	{MISENSOR_16BIT,	0xD238, 0x01C5},
+	{MISENSOR_16BIT,	0xD23A, 0x0320},
+	{MISENSOR_16BIT,	0xD23C, 0xD900},
+	{MISENSOR_16BIT,	0xD23E, 0x78E0},
+	{MISENSOR_16BIT,	0xD240, 0x220A},
+	{MISENSOR_16BIT,	0xD242, 0x1F80},
+	{MISENSOR_16BIT,	0xD244, 0xFFFF},
+	{MISENSOR_16BIT,	0xD246, 0xD4E0},
+	{MISENSOR_16BIT,	0xD248, 0xC0F1},
+	{MISENSOR_16BIT,	0xD24A, 0x0811},
+	{MISENSOR_16BIT,	0xD24C, 0x0051},
+	{MISENSOR_16BIT,	0xD24E, 0x2240},
+	{MISENSOR_16BIT,	0xD250, 0x1200},
+	{MISENSOR_16BIT,	0xD252, 0xFFE1},
+	{MISENSOR_16BIT,	0xD254, 0xD801},
+	{MISENSOR_16BIT,	0xD256, 0xF006},
+	{MISENSOR_16BIT,	0xD258, 0x2240},
+	{MISENSOR_16BIT,	0xD25A, 0x1900},
+	{MISENSOR_16BIT,	0xD25C, 0xFFDE},
+	{MISENSOR_16BIT,	0xD25E, 0xD802},
+	{MISENSOR_16BIT,	0xD260, 0x1A05},
+	{MISENSOR_16BIT,	0xD262, 0x1002},
+	{MISENSOR_16BIT,	0xD264, 0xFFF2},
+	{MISENSOR_16BIT,	0xD266, 0xF195},
+	{MISENSOR_16BIT,	0xD268, 0xC0F1},
+	{MISENSOR_16BIT,	0xD26A, 0x0E7E},
+	{MISENSOR_16BIT,	0xD26C, 0x05C0},
+	{MISENSOR_16BIT,	0xD26E, 0x75CF},
+	{MISENSOR_16BIT,	0xD270, 0xFFFF},
+	{MISENSOR_16BIT,	0xD272, 0xC84C},
+	{MISENSOR_16BIT,	0xD274, 0x9502},
+	{MISENSOR_16BIT,	0xD276, 0x77CF},
+	{MISENSOR_16BIT,	0xD278, 0xFFFF},
+	{MISENSOR_16BIT,	0xD27A, 0xC344},
+	{MISENSOR_16BIT,	0xD27C, 0x2044},
+	{MISENSOR_16BIT,	0xD27E, 0x008E},
+	{MISENSOR_16BIT,	0xD280, 0xB8A1},
+	{MISENSOR_16BIT,	0xD282, 0x0926},
+	{MISENSOR_16BIT,	0xD284, 0x03E0},
+	{MISENSOR_16BIT,	0xD286, 0xB502},
+	{MISENSOR_16BIT,	0xD288, 0x9502},
+	{MISENSOR_16BIT,	0xD28A, 0x952E},
+	{MISENSOR_16BIT,	0xD28C, 0x7E05},
+	{MISENSOR_16BIT,	0xD28E, 0xB5C2},
+	{MISENSOR_16BIT,	0xD290, 0x70CF},
+	{MISENSOR_16BIT,	0xD292, 0xFFFF},
+	{MISENSOR_16BIT,	0xD294, 0xC610},
+	{MISENSOR_16BIT,	0xD296, 0x099A},
+	{MISENSOR_16BIT,	0xD298, 0x04A0},
+	{MISENSOR_16BIT,	0xD29A, 0xB026},
+	{MISENSOR_16BIT,	0xD29C, 0x0E02},
+	{MISENSOR_16BIT,	0xD29E, 0x0560},
+	{MISENSOR_16BIT,	0xD2A0, 0xDE00},
+	{MISENSOR_16BIT,	0xD2A2, 0x0A12},
+	{MISENSOR_16BIT,	0xD2A4, 0x0320},
+	{MISENSOR_16BIT,	0xD2A6, 0xB7C4},
+	{MISENSOR_16BIT,	0xD2A8, 0x0B36},
+	{MISENSOR_16BIT,	0xD2AA, 0x03A0},
+	{MISENSOR_16BIT,	0xD2AC, 0x70C9},
+	{MISENSOR_16BIT,	0xD2AE, 0x9502},
+	{MISENSOR_16BIT,	0xD2B0, 0x7608},
+	{MISENSOR_16BIT,	0xD2B2, 0xB8A8},
+	{MISENSOR_16BIT,	0xD2B4, 0xB502},
+	{MISENSOR_16BIT,	0xD2B6, 0x70CF},
+	{MISENSOR_16BIT,	0xD2B8, 0x0000},
+	{MISENSOR_16BIT,	0xD2BA, 0x5536},
+	{MISENSOR_16BIT,	0xD2BC, 0x7860},
+	{MISENSOR_16BIT,	0xD2BE, 0x2686},
+	{MISENSOR_16BIT,	0xD2C0, 0x1FFB},
+	{MISENSOR_16BIT,	0xD2C2, 0x9502},
+	{MISENSOR_16BIT,	0xD2C4, 0x78C5},
+	{MISENSOR_16BIT,	0xD2C6, 0x0631},
+	{MISENSOR_16BIT,	0xD2C8, 0x05E0},
+	{MISENSOR_16BIT,	0xD2CA, 0xB502},
+	{MISENSOR_16BIT,	0xD2CC, 0x72CF},
+	{MISENSOR_16BIT,	0xD2CE, 0xFFFF},
+	{MISENSOR_16BIT,	0xD2D0, 0xC5D4},
+	{MISENSOR_16BIT,	0xD2D2, 0x923A},
+	{MISENSOR_16BIT,	0xD2D4, 0x73CF},
+	{MISENSOR_16BIT,	0xD2D6, 0xFFFF},
+	{MISENSOR_16BIT,	0xD2D8, 0xC7D0},
+	{MISENSOR_16BIT,	0xD2DA, 0xB020},
+	{MISENSOR_16BIT,	0xD2DC, 0x9220},
+	{MISENSOR_16BIT,	0xD2DE, 0xB021},
+	{MISENSOR_16BIT,	0xD2E0, 0x9221},
+	{MISENSOR_16BIT,	0xD2E2, 0xB022},
+	{MISENSOR_16BIT,	0xD2E4, 0x9222},
+	{MISENSOR_16BIT,	0xD2E6, 0xB023},
+	{MISENSOR_16BIT,	0xD2E8, 0x9223},
+	{MISENSOR_16BIT,	0xD2EA, 0xB024},
+	{MISENSOR_16BIT,	0xD2EC, 0x9227},
+	{MISENSOR_16BIT,	0xD2EE, 0xB025},
+	{MISENSOR_16BIT,	0xD2F0, 0x9228},
+	{MISENSOR_16BIT,	0xD2F2, 0xB026},
+	{MISENSOR_16BIT,	0xD2F4, 0x922B},
+	{MISENSOR_16BIT,	0xD2F6, 0xB027},
+	{MISENSOR_16BIT,	0xD2F8, 0x922C},
+	{MISENSOR_16BIT,	0xD2FA, 0xB028},
+	{MISENSOR_16BIT,	0xD2FC, 0x1258},
+	{MISENSOR_16BIT,	0xD2FE, 0x0101},
+	{MISENSOR_16BIT,	0xD300, 0xB029},
+	{MISENSOR_16BIT,	0xD302, 0x125A},
+	{MISENSOR_16BIT,	0xD304, 0x0101},
+	{MISENSOR_16BIT,	0xD306, 0xB02A},
+	{MISENSOR_16BIT,	0xD308, 0x1336},
+	{MISENSOR_16BIT,	0xD30A, 0x8081},
+	{MISENSOR_16BIT,	0xD30C, 0xA836},
+	{MISENSOR_16BIT,	0xD30E, 0x1337},
+	{MISENSOR_16BIT,	0xD310, 0x8081},
+	{MISENSOR_16BIT,	0xD312, 0xA837},
+	{MISENSOR_16BIT,	0xD314, 0x12A7},
+	{MISENSOR_16BIT,	0xD316, 0x0701},
+	{MISENSOR_16BIT,	0xD318, 0xB02C},
+	{MISENSOR_16BIT,	0xD31A, 0x1354},
+	{MISENSOR_16BIT,	0xD31C, 0x8081},
+	{MISENSOR_16BIT,	0xD31E, 0x7FE0},
+	{MISENSOR_16BIT,	0xD320, 0xA83A},
+	{MISENSOR_16BIT,	0xD322, 0x78E0},
+	{MISENSOR_16BIT,	0xD324, 0xC0F1},
+	{MISENSOR_16BIT,	0xD326, 0x0DC2},
+	{MISENSOR_16BIT,	0xD328, 0x05C0},
+	{MISENSOR_16BIT,	0xD32A, 0x7608},
+	{MISENSOR_16BIT,	0xD32C, 0x09BB},
+	{MISENSOR_16BIT,	0xD32E, 0x0010},
+	{MISENSOR_16BIT,	0xD330, 0x75CF},
+	{MISENSOR_16BIT,	0xD332, 0xFFFF},
+	{MISENSOR_16BIT,	0xD334, 0xD4E0},
+	{MISENSOR_16BIT,	0xD336, 0x8D21},
+	{MISENSOR_16BIT,	0xD338, 0x8D00},
+	{MISENSOR_16BIT,	0xD33A, 0x2153},
+	{MISENSOR_16BIT,	0xD33C, 0x0003},
+	{MISENSOR_16BIT,	0xD33E, 0xB8C0},
+	{MISENSOR_16BIT,	0xD340, 0x8D45},
+	{MISENSOR_16BIT,	0xD342, 0x0B23},
+	{MISENSOR_16BIT,	0xD344, 0x0000},
+	{MISENSOR_16BIT,	0xD346, 0xEA8F},
+	{MISENSOR_16BIT,	0xD348, 0x0915},
+	{MISENSOR_16BIT,	0xD34A, 0x001E},
+	{MISENSOR_16BIT,	0xD34C, 0xFF81},
+	{MISENSOR_16BIT,	0xD34E, 0xE808},
+	{MISENSOR_16BIT,	0xD350, 0x2540},
+	{MISENSOR_16BIT,	0xD352, 0x1900},
+	{MISENSOR_16BIT,	0xD354, 0xFFDE},
+	{MISENSOR_16BIT,	0xD356, 0x8D00},
+	{MISENSOR_16BIT,	0xD358, 0xB880},
+	{MISENSOR_16BIT,	0xD35A, 0xF004},
+	{MISENSOR_16BIT,	0xD35C, 0x8D00},
+	{MISENSOR_16BIT,	0xD35E, 0xB8A0},
+	{MISENSOR_16BIT,	0xD360, 0xAD00},
+	{MISENSOR_16BIT,	0xD362, 0x8D05},
+	{MISENSOR_16BIT,	0xD364, 0xE081},
+	{MISENSOR_16BIT,	0xD366, 0x20CC},
+	{MISENSOR_16BIT,	0xD368, 0x80A2},
+	{MISENSOR_16BIT,	0xD36A, 0xDF00},
+	{MISENSOR_16BIT,	0xD36C, 0xF40A},
+	{MISENSOR_16BIT,	0xD36E, 0x71CF},
+	{MISENSOR_16BIT,	0xD370, 0xFFFF},
+	{MISENSOR_16BIT,	0xD372, 0xC84C},
+	{MISENSOR_16BIT,	0xD374, 0x9102},
+	{MISENSOR_16BIT,	0xD376, 0x7708},
+	{MISENSOR_16BIT,	0xD378, 0xB8A6},
+	{MISENSOR_16BIT,	0xD37A, 0x2786},
+	{MISENSOR_16BIT,	0xD37C, 0x1FFE},
+	{MISENSOR_16BIT,	0xD37E, 0xB102},
+	{MISENSOR_16BIT,	0xD380, 0x0B42},
+	{MISENSOR_16BIT,	0xD382, 0x0180},
+	{MISENSOR_16BIT,	0xD384, 0x0E3E},
+	{MISENSOR_16BIT,	0xD386, 0x0180},
+	{MISENSOR_16BIT,	0xD388, 0x0F4A},
+	{MISENSOR_16BIT,	0xD38A, 0x0160},
+	{MISENSOR_16BIT,	0xD38C, 0x70C9},
+	{MISENSOR_16BIT,	0xD38E, 0x8D05},
+	{MISENSOR_16BIT,	0xD390, 0xE081},
+	{MISENSOR_16BIT,	0xD392, 0x20CC},
+	{MISENSOR_16BIT,	0xD394, 0x80A2},
+	{MISENSOR_16BIT,	0xD396, 0xF429},
+	{MISENSOR_16BIT,	0xD398, 0x76CF},
+	{MISENSOR_16BIT,	0xD39A, 0xFFFF},
+	{MISENSOR_16BIT,	0xD39C, 0xC84C},
+	{MISENSOR_16BIT,	0xD39E, 0x082D},
+	{MISENSOR_16BIT,	0xD3A0, 0x0051},
+	{MISENSOR_16BIT,	0xD3A2, 0x70CF},
+	{MISENSOR_16BIT,	0xD3A4, 0xFFFF},
+	{MISENSOR_16BIT,	0xD3A6, 0xC90C},
+	{MISENSOR_16BIT,	0xD3A8, 0x8805},
+	{MISENSOR_16BIT,	0xD3AA, 0x09B6},
+	{MISENSOR_16BIT,	0xD3AC, 0x0360},
+	{MISENSOR_16BIT,	0xD3AE, 0xD908},
+	{MISENSOR_16BIT,	0xD3B0, 0x2099},
+	{MISENSOR_16BIT,	0xD3B2, 0x0802},
+	{MISENSOR_16BIT,	0xD3B4, 0x9634},
+	{MISENSOR_16BIT,	0xD3B6, 0xB503},
+	{MISENSOR_16BIT,	0xD3B8, 0x7902},
+	{MISENSOR_16BIT,	0xD3BA, 0x1523},
+	{MISENSOR_16BIT,	0xD3BC, 0x1080},
+	{MISENSOR_16BIT,	0xD3BE, 0xB634},
+	{MISENSOR_16BIT,	0xD3C0, 0xE001},
+	{MISENSOR_16BIT,	0xD3C2, 0x1D23},
+	{MISENSOR_16BIT,	0xD3C4, 0x1002},
+	{MISENSOR_16BIT,	0xD3C6, 0xF00B},
+	{MISENSOR_16BIT,	0xD3C8, 0x9634},
+	{MISENSOR_16BIT,	0xD3CA, 0x9503},
+	{MISENSOR_16BIT,	0xD3CC, 0x6038},
+	{MISENSOR_16BIT,	0xD3CE, 0xB614},
+	{MISENSOR_16BIT,	0xD3D0, 0x153F},
+	{MISENSOR_16BIT,	0xD3D2, 0x1080},
+	{MISENSOR_16BIT,	0xD3D4, 0xE001},
+	{MISENSOR_16BIT,	0xD3D6, 0x1D3F},
+	{MISENSOR_16BIT,	0xD3D8, 0x1002},
+	{MISENSOR_16BIT,	0xD3DA, 0xFFA4},
+	{MISENSOR_16BIT,	0xD3DC, 0x9602},
+	{MISENSOR_16BIT,	0xD3DE, 0x7F05},
+	{MISENSOR_16BIT,	0xD3E0, 0xD800},
+	{MISENSOR_16BIT,	0xD3E2, 0xB6E2},
+	{MISENSOR_16BIT,	0xD3E4, 0xAD05},
+	{MISENSOR_16BIT,	0xD3E6, 0x0511},
+	{MISENSOR_16BIT,	0xD3E8, 0x05E0},
+	{MISENSOR_16BIT,	0xD3EA, 0xD800},
+	{MISENSOR_16BIT,	0xD3EC, 0xC0F1},
+	{MISENSOR_16BIT,	0xD3EE, 0x0CFE},
+	{MISENSOR_16BIT,	0xD3F0, 0x05C0},
+	{MISENSOR_16BIT,	0xD3F2, 0x0A96},
+	{MISENSOR_16BIT,	0xD3F4, 0x05A0},
+	{MISENSOR_16BIT,	0xD3F6, 0x7608},
+	{MISENSOR_16BIT,	0xD3F8, 0x0C22},
+	{MISENSOR_16BIT,	0xD3FA, 0x0240},
+	{MISENSOR_16BIT,	0xD3FC, 0xE080},
+	{MISENSOR_16BIT,	0xD3FE, 0x20CA},
+	{MISENSOR_16BIT,	0xD400, 0x0F82},
+	{MISENSOR_16BIT,	0xD402, 0x0000},
+	{MISENSOR_16BIT,	0xD404, 0x190B},
+	{MISENSOR_16BIT,	0xD406, 0x0C60},
+	{MISENSOR_16BIT,	0xD408, 0x05A2},
+	{MISENSOR_16BIT,	0xD40A, 0x21CA},
+	{MISENSOR_16BIT,	0xD40C, 0x0022},
+	{MISENSOR_16BIT,	0xD40E, 0x0C56},
+	{MISENSOR_16BIT,	0xD410, 0x0240},
+	{MISENSOR_16BIT,	0xD412, 0xE806},
+	{MISENSOR_16BIT,	0xD414, 0x0E0E},
+	{MISENSOR_16BIT,	0xD416, 0x0220},
+	{MISENSOR_16BIT,	0xD418, 0x70C9},
+	{MISENSOR_16BIT,	0xD41A, 0xF048},
+	{MISENSOR_16BIT,	0xD41C, 0x0896},
+	{MISENSOR_16BIT,	0xD41E, 0x0440},
+	{MISENSOR_16BIT,	0xD420, 0x0E96},
+	{MISENSOR_16BIT,	0xD422, 0x0400},
+	{MISENSOR_16BIT,	0xD424, 0x0966},
+	{MISENSOR_16BIT,	0xD426, 0x0380},
+	{MISENSOR_16BIT,	0xD428, 0x75CF},
+	{MISENSOR_16BIT,	0xD42A, 0xFFFF},
+	{MISENSOR_16BIT,	0xD42C, 0xD4E0},
+	{MISENSOR_16BIT,	0xD42E, 0x8D00},
+	{MISENSOR_16BIT,	0xD430, 0x084D},
+	{MISENSOR_16BIT,	0xD432, 0x001E},
+	{MISENSOR_16BIT,	0xD434, 0xFF47},
+	{MISENSOR_16BIT,	0xD436, 0x080D},
+	{MISENSOR_16BIT,	0xD438, 0x0050},
+	{MISENSOR_16BIT,	0xD43A, 0xFF57},
+	{MISENSOR_16BIT,	0xD43C, 0x0841},
+	{MISENSOR_16BIT,	0xD43E, 0x0051},
+	{MISENSOR_16BIT,	0xD440, 0x8D04},
+	{MISENSOR_16BIT,	0xD442, 0x9521},
+	{MISENSOR_16BIT,	0xD444, 0xE064},
+	{MISENSOR_16BIT,	0xD446, 0x790C},
+	{MISENSOR_16BIT,	0xD448, 0x702F},
+	{MISENSOR_16BIT,	0xD44A, 0x0CE2},
+	{MISENSOR_16BIT,	0xD44C, 0x05E0},
+	{MISENSOR_16BIT,	0xD44E, 0xD964},
+	{MISENSOR_16BIT,	0xD450, 0x72CF},
+	{MISENSOR_16BIT,	0xD452, 0xFFFF},
+	{MISENSOR_16BIT,	0xD454, 0xC700},
+	{MISENSOR_16BIT,	0xD456, 0x9235},
+	{MISENSOR_16BIT,	0xD458, 0x0811},
+	{MISENSOR_16BIT,	0xD45A, 0x0043},
+	{MISENSOR_16BIT,	0xD45C, 0xFF3D},
+	{MISENSOR_16BIT,	0xD45E, 0x080D},
+	{MISENSOR_16BIT,	0xD460, 0x0051},
+	{MISENSOR_16BIT,	0xD462, 0xD801},
+	{MISENSOR_16BIT,	0xD464, 0xFF77},
+	{MISENSOR_16BIT,	0xD466, 0xF025},
+	{MISENSOR_16BIT,	0xD468, 0x9501},
+	{MISENSOR_16BIT,	0xD46A, 0x9235},
+	{MISENSOR_16BIT,	0xD46C, 0x0911},
+	{MISENSOR_16BIT,	0xD46E, 0x0003},
+	{MISENSOR_16BIT,	0xD470, 0xFF49},
+	{MISENSOR_16BIT,	0xD472, 0x080D},
+	{MISENSOR_16BIT,	0xD474, 0x0051},
+	{MISENSOR_16BIT,	0xD476, 0xD800},
+	{MISENSOR_16BIT,	0xD478, 0xFF72},
+	{MISENSOR_16BIT,	0xD47A, 0xF01B},
+	{MISENSOR_16BIT,	0xD47C, 0x0886},
+	{MISENSOR_16BIT,	0xD47E, 0x03E0},
+	{MISENSOR_16BIT,	0xD480, 0xD801},
+	{MISENSOR_16BIT,	0xD482, 0x0EF6},
+	{MISENSOR_16BIT,	0xD484, 0x03C0},
+	{MISENSOR_16BIT,	0xD486, 0x0F52},
+	{MISENSOR_16BIT,	0xD488, 0x0340},
+	{MISENSOR_16BIT,	0xD48A, 0x0DBA},
+	{MISENSOR_16BIT,	0xD48C, 0x0200},
+	{MISENSOR_16BIT,	0xD48E, 0x0AF6},
+	{MISENSOR_16BIT,	0xD490, 0x0440},
+	{MISENSOR_16BIT,	0xD492, 0x0C22},
+	{MISENSOR_16BIT,	0xD494, 0x0400},
+	{MISENSOR_16BIT,	0xD496, 0x0D72},
+	{MISENSOR_16BIT,	0xD498, 0x0440},
+	{MISENSOR_16BIT,	0xD49A, 0x0DC2},
+	{MISENSOR_16BIT,	0xD49C, 0x0200},
+	{MISENSOR_16BIT,	0xD49E, 0x0972},
+	{MISENSOR_16BIT,	0xD4A0, 0x0440},
+	{MISENSOR_16BIT,	0xD4A2, 0x0D3A},
+	{MISENSOR_16BIT,	0xD4A4, 0x0220},
+	{MISENSOR_16BIT,	0xD4A6, 0xD820},
+	{MISENSOR_16BIT,	0xD4A8, 0x0BFA},
+	{MISENSOR_16BIT,	0xD4AA, 0x0260},
+	{MISENSOR_16BIT,	0xD4AC, 0x70C9},
+	{MISENSOR_16BIT,	0xD4AE, 0x0451},
+	{MISENSOR_16BIT,	0xD4B0, 0x05C0},
+	{MISENSOR_16BIT,	0xD4B2, 0x78E0},
+	{MISENSOR_16BIT,	0xD4B4, 0xD900},
+	{MISENSOR_16BIT,	0xD4B6, 0xF00A},
+	{MISENSOR_16BIT,	0xD4B8, 0x70CF},
+	{MISENSOR_16BIT,	0xD4BA, 0xFFFF},
+	{MISENSOR_16BIT,	0xD4BC, 0xD520},
+	{MISENSOR_16BIT,	0xD4BE, 0x7835},
+	{MISENSOR_16BIT,	0xD4C0, 0x8041},
+	{MISENSOR_16BIT,	0xD4C2, 0x8000},
+	{MISENSOR_16BIT,	0xD4C4, 0xE102},
+	{MISENSOR_16BIT,	0xD4C6, 0xA040},
+	{MISENSOR_16BIT,	0xD4C8, 0x09F1},
+	{MISENSOR_16BIT,	0xD4CA, 0x8114},
+	{MISENSOR_16BIT,	0xD4CC, 0x71CF},
+	{MISENSOR_16BIT,	0xD4CE, 0xFFFF},
+	{MISENSOR_16BIT,	0xD4D0, 0xD4E0},
+	{MISENSOR_16BIT,	0xD4D2, 0x70CF},
+	{MISENSOR_16BIT,	0xD4D4, 0xFFFF},
+	{MISENSOR_16BIT,	0xD4D6, 0xC594},
+	{MISENSOR_16BIT,	0xD4D8, 0xB03A},
+	{MISENSOR_16BIT,	0xD4DA, 0x7FE0},
+	{MISENSOR_16BIT,	0xD4DC, 0xD800},
+	{MISENSOR_16BIT,	0xD4DE, 0x0000},
+	{MISENSOR_16BIT,	0xD4E0, 0x0000},
+	{MISENSOR_16BIT,	0xD4E2, 0x0500},
+	{MISENSOR_16BIT,	0xD4E4, 0x0500},
+	{MISENSOR_16BIT,	0xD4E6, 0x0200},
+	{MISENSOR_16BIT,	0xD4E8, 0x0330},
+	{MISENSOR_16BIT,	0xD4EA, 0x0000},
+	{MISENSOR_16BIT,	0xD4EC, 0x0000},
+	{MISENSOR_16BIT,	0xD4EE, 0x03CD},
+	{MISENSOR_16BIT,	0xD4F0, 0x050D},
+	{MISENSOR_16BIT,	0xD4F2, 0x01C5},
+	{MISENSOR_16BIT,	0xD4F4, 0x03B3},
+	{MISENSOR_16BIT,	0xD4F6, 0x00E0},
+	{MISENSOR_16BIT,	0xD4F8, 0x01E3},
+	{MISENSOR_16BIT,	0xD4FA, 0x0280},
+	{MISENSOR_16BIT,	0xD4FC, 0x01E0},
+	{MISENSOR_16BIT,	0xD4FE, 0x0109},
+	{MISENSOR_16BIT,	0xD500, 0x0080},
+	{MISENSOR_16BIT,	0xD502, 0x0500},
+	{MISENSOR_16BIT,	0xD504, 0x0000},
+	{MISENSOR_16BIT,	0xD506, 0x0000},
+	{MISENSOR_16BIT,	0xD508, 0x0000},
+	{MISENSOR_16BIT,	0xD50A, 0x0000},
+	{MISENSOR_16BIT,	0xD50C, 0x0000},
+	{MISENSOR_16BIT,	0xD50E, 0x0000},
+	{MISENSOR_16BIT,	0xD510, 0x0000},
+	{MISENSOR_16BIT,	0xD512, 0x0000},
+	{MISENSOR_16BIT,	0xD514, 0x0000},
+	{MISENSOR_16BIT,	0xD516, 0x0000},
+	{MISENSOR_16BIT,	0xD518, 0x0000},
+	{MISENSOR_16BIT,	0xD51A, 0x0000},
+	{MISENSOR_16BIT,	0xD51C, 0x0000},
+	{MISENSOR_16BIT,	0xD51E, 0x0000},
+	{MISENSOR_16BIT,	0xD520, 0xFFFF},
+	{MISENSOR_16BIT,	0xD522, 0xC9B4},
+	{MISENSOR_16BIT,	0xD524, 0xFFFF},
+	{MISENSOR_16BIT,	0xD526, 0xD324},
+	{MISENSOR_16BIT,	0xD528, 0xFFFF},
+	{MISENSOR_16BIT,	0xD52A, 0xCA34},
+	{MISENSOR_16BIT,	0xD52C, 0xFFFF},
+	{MISENSOR_16BIT,	0xD52E, 0xD3EC},
+	{MISENSOR_16BIT,	0x098E, 0x0000},
+	{MISENSOR_16BIT,	0xE000, 0x04B4},
+	{MISENSOR_16BIT,	0xE002, 0x0302},
+	{MISENSOR_16BIT,	0xE004, 0x4103},
+	{MISENSOR_16BIT,	0xE006, 0x0202},
+	{MISENSOR_16BIT,	0x0080, 0xFFF0},
+	{MISENSOR_16BIT,	0x0080, 0xFFF1},
+
+	/* PGA parameter and APGA
+	 * [Step4-APGA] [TP101_MT9M114_APGA]
+	 */
+	{MISENSOR_16BIT,	0x098E, 0x495E},
+	{MISENSOR_16BIT,	0xC95E, 0x0000},
+	{MISENSOR_16BIT,	0x3640, 0x02B0},
+	{MISENSOR_16BIT,	0x3642, 0x8063},
+	{MISENSOR_16BIT,	0x3644, 0x78D0},
+	{MISENSOR_16BIT,	0x3646, 0x50CC},
+	{MISENSOR_16BIT,	0x3648, 0x3511},
+	{MISENSOR_16BIT,	0x364A, 0x0110},
+	{MISENSOR_16BIT,	0x364C, 0xBD8A},
+	{MISENSOR_16BIT,	0x364E, 0x0CD1},
+	{MISENSOR_16BIT,	0x3650, 0x24ED},
+	{MISENSOR_16BIT,	0x3652, 0x7C11},
+	{MISENSOR_16BIT,	0x3654, 0x0150},
+	{MISENSOR_16BIT,	0x3656, 0x124C},
+	{MISENSOR_16BIT,	0x3658, 0x3130},
+	{MISENSOR_16BIT,	0x365A, 0x508C},
+	{MISENSOR_16BIT,	0x365C, 0x21F1},
+	{MISENSOR_16BIT,	0x365E, 0x0090},
+	{MISENSOR_16BIT,	0x3660, 0xBFCA},
+	{MISENSOR_16BIT,	0x3662, 0x0A11},
+	{MISENSOR_16BIT,	0x3664, 0x4F4B},
+	{MISENSOR_16BIT,	0x3666, 0x28B1},
+	{MISENSOR_16BIT,	0x3680, 0x50A9},
+	{MISENSOR_16BIT,	0x3682, 0xA04B},
+	{MISENSOR_16BIT,	0x3684, 0x0E2D},
+	{MISENSOR_16BIT,	0x3686, 0x73EC},
+	{MISENSOR_16BIT,	0x3688, 0x164F},
+	{MISENSOR_16BIT,	0x368A, 0xF829},
+	{MISENSOR_16BIT,	0x368C, 0xC1A8},
+	{MISENSOR_16BIT,	0x368E, 0xB0EC},
+	{MISENSOR_16BIT,	0x3690, 0xE76A},
+	{MISENSOR_16BIT,	0x3692, 0x69AF},
+	{MISENSOR_16BIT,	0x3694, 0x378C},
+	{MISENSOR_16BIT,	0x3696, 0xA70D},
+	{MISENSOR_16BIT,	0x3698, 0x884F},
+	{MISENSOR_16BIT,	0x369A, 0xEE8B},
+	{MISENSOR_16BIT,	0x369C, 0x5DEF},
+	{MISENSOR_16BIT,	0x369E, 0x27CC},
+	{MISENSOR_16BIT,	0x36A0, 0xCAAC},
+	{MISENSOR_16BIT,	0x36A2, 0x840E},
+	{MISENSOR_16BIT,	0x36A4, 0xDAA9},
+	{MISENSOR_16BIT,	0x36A6, 0xF00C},
+	{MISENSOR_16BIT,	0x36C0, 0x1371},
+	{MISENSOR_16BIT,	0x36C2, 0x272F},
+	{MISENSOR_16BIT,	0x36C4, 0x2293},
+	{MISENSOR_16BIT,	0x36C6, 0xE6D0},
+	{MISENSOR_16BIT,	0x36C8, 0xEC32},
+	{MISENSOR_16BIT,	0x36CA, 0x11B1},
+	{MISENSOR_16BIT,	0x36CC, 0x7BAF},
+	{MISENSOR_16BIT,	0x36CE, 0x5813},
+	{MISENSOR_16BIT,	0x36D0, 0xB871},
+	{MISENSOR_16BIT,	0x36D2, 0x8913},
+	{MISENSOR_16BIT,	0x36D4, 0x4610},
+	{MISENSOR_16BIT,	0x36D6, 0x7EEE},
+	{MISENSOR_16BIT,	0x36D8, 0x0DF3},
+	{MISENSOR_16BIT,	0x36DA, 0xB84F},
+	{MISENSOR_16BIT,	0x36DC, 0xB532},
+	{MISENSOR_16BIT,	0x36DE, 0x1171},
+	{MISENSOR_16BIT,	0x36E0, 0x13CF},
+	{MISENSOR_16BIT,	0x36E2, 0x22F3},
+	{MISENSOR_16BIT,	0x36E4, 0xE090},
+	{MISENSOR_16BIT,	0x36E6, 0x8133},
+	{MISENSOR_16BIT,	0x3700, 0x88AE},
+	{MISENSOR_16BIT,	0x3702, 0x00EA},
+	{MISENSOR_16BIT,	0x3704, 0x344F},
+	{MISENSOR_16BIT,	0x3706, 0xEC88},
+	{MISENSOR_16BIT,	0x3708, 0x3E91},
+	{MISENSOR_16BIT,	0x370A, 0xF12D},
+	{MISENSOR_16BIT,	0x370C, 0xB0EF},
+	{MISENSOR_16BIT,	0x370E, 0x77CD},
+	{MISENSOR_16BIT,	0x3710, 0x7930},
+	{MISENSOR_16BIT,	0x3712, 0x5C12},
+	{MISENSOR_16BIT,	0x3714, 0x500C},
+	{MISENSOR_16BIT,	0x3716, 0x22CE},
+	{MISENSOR_16BIT,	0x3718, 0x2370},
+	{MISENSOR_16BIT,	0x371A, 0x258F},
+	{MISENSOR_16BIT,	0x371C, 0x3D30},
+	{MISENSOR_16BIT,	0x371E, 0x370C},
+	{MISENSOR_16BIT,	0x3720, 0x03ED},
+	{MISENSOR_16BIT,	0x3722, 0x9AD0},
+	{MISENSOR_16BIT,	0x3724, 0x7ECF},
+	{MISENSOR_16BIT,	0x3726, 0x1093},
+	{MISENSOR_16BIT,	0x3740, 0x2391},
+	{MISENSOR_16BIT,	0x3742, 0xAAD0},
+	{MISENSOR_16BIT,	0x3744, 0x28F2},
+	{MISENSOR_16BIT,	0x3746, 0xBA4F},
+	{MISENSOR_16BIT,	0x3748, 0xC536},
+	{MISENSOR_16BIT,	0x374A, 0x1472},
+	{MISENSOR_16BIT,	0x374C, 0xD110},
+	{MISENSOR_16BIT,	0x374E, 0x2933},
+	{MISENSOR_16BIT,	0x3750, 0xD0D1},
+	{MISENSOR_16BIT,	0x3752, 0x9F37},
+	{MISENSOR_16BIT,	0x3754, 0x34D1},
+	{MISENSOR_16BIT,	0x3756, 0x1C6C},
+	{MISENSOR_16BIT,	0x3758, 0x3FD2},
+	{MISENSOR_16BIT,	0x375A, 0xCB72},
+	{MISENSOR_16BIT,	0x375C, 0xBA96},
+	{MISENSOR_16BIT,	0x375E, 0x1551},
+	{MISENSOR_16BIT,	0x3760, 0xB74F},
+	{MISENSOR_16BIT,	0x3762, 0x1672},
+	{MISENSOR_16BIT,	0x3764, 0x84F1},
+	{MISENSOR_16BIT,	0x3766, 0xC2D6},
+	{MISENSOR_16BIT,	0x3782, 0x01E0},
+	{MISENSOR_16BIT,	0x3784, 0x0280},
+	{MISENSOR_16BIT,	0x37C0, 0xA6EA},
+	{MISENSOR_16BIT,	0x37C2, 0x874B},
+	{MISENSOR_16BIT,	0x37C4, 0x85CB},
+	{MISENSOR_16BIT,	0x37C6, 0x968A},
+	{MISENSOR_16BIT,	0x098E, 0x0000},
+	{MISENSOR_16BIT,	0xC960, 0x0AF0},
+	{MISENSOR_16BIT,	0xC962, 0x79E2},
+	{MISENSOR_16BIT,	0xC964, 0x5EC8},
+	{MISENSOR_16BIT,	0xC966, 0x791F},
+	{MISENSOR_16BIT,	0xC968, 0x76EE},
+	{MISENSOR_16BIT,	0xC96A, 0x0FA0},
+	{MISENSOR_16BIT,	0xC96C, 0x7DFA},
+	{MISENSOR_16BIT,	0xC96E, 0x7DAF},
+	{MISENSOR_16BIT,	0xC970, 0x7E02},
+	{MISENSOR_16BIT,	0xC972, 0x7E0A},
+	{MISENSOR_16BIT,	0xC974, 0x1964},
+	{MISENSOR_16BIT,	0xC976, 0x7CDC},
+	{MISENSOR_16BIT,	0xC978, 0x7838},
+	{MISENSOR_16BIT,	0xC97A, 0x7C2F},
+	{MISENSOR_16BIT,	0xC97C, 0x7792},
+	{MISENSOR_16BIT,	0xC95E, 0x0003},
+
+	/* [Step4-APGA] */
+	{MISENSOR_16BIT,	0x098E, 0x0000},
+	{MISENSOR_16BIT,	0xC95E, 0x0003},
+
+	/* [Step5-AWB_CCM]1: LOAD=CCM */
+	{MISENSOR_16BIT,	0xC892, 0x0267},
+	{MISENSOR_16BIT,	0xC894, 0xFF1A},
+	{MISENSOR_16BIT,	0xC896, 0xFFB3},
+	{MISENSOR_16BIT,	0xC898, 0xFF80},
+	{MISENSOR_16BIT,	0xC89A, 0x0166},
+	{MISENSOR_16BIT,	0xC89C, 0x0003},
+	{MISENSOR_16BIT,	0xC89E, 0xFF9A},
+	{MISENSOR_16BIT,	0xC8A0, 0xFEB4},
+	{MISENSOR_16BIT,	0xC8A2, 0x024D},
+	{MISENSOR_16BIT,	0xC8A4, 0x01BF},
+	{MISENSOR_16BIT,	0xC8A6, 0xFF01},
+	{MISENSOR_16BIT,	0xC8A8, 0xFFF3},
+	{MISENSOR_16BIT,	0xC8AA, 0xFF75},
+	{MISENSOR_16BIT,	0xC8AC, 0x0198},
+	{MISENSOR_16BIT,	0xC8AE, 0xFFFD},
+	{MISENSOR_16BIT,	0xC8B0, 0xFF9A},
+	{MISENSOR_16BIT,	0xC8B2, 0xFEE7},
+	{MISENSOR_16BIT,	0xC8B4, 0x02A8},
+	{MISENSOR_16BIT,	0xC8B6, 0x01D9},
+	{MISENSOR_16BIT,	0xC8B8, 0xFF26},
+	{MISENSOR_16BIT,	0xC8BA, 0xFFF3},
+	{MISENSOR_16BIT,	0xC8BC, 0xFFB3},
+	{MISENSOR_16BIT,	0xC8BE, 0x0132},
+	{MISENSOR_16BIT,	0xC8C0, 0xFFE8},
+	{MISENSOR_16BIT,	0xC8C2, 0xFFDA},
+	{MISENSOR_16BIT,	0xC8C4, 0xFECD},
+	{MISENSOR_16BIT,	0xC8C6, 0x02C2},
+	{MISENSOR_16BIT,	0xC8C8, 0x0075},
+	{MISENSOR_16BIT,	0xC8CA, 0x011C},
+	{MISENSOR_16BIT,	0xC8CC, 0x009A},
+	{MISENSOR_16BIT,	0xC8CE, 0x0105},
+	{MISENSOR_16BIT,	0xC8D0, 0x00A4},
+	{MISENSOR_16BIT,	0xC8D2, 0x00AC},
+	{MISENSOR_16BIT,	0xC8D4, 0x0A8C},
+	{MISENSOR_16BIT,	0xC8D6, 0x0F0A},
+	{MISENSOR_16BIT,	0xC8D8, 0x1964},
+
+	/* LOAD=AWB */
+	{MISENSOR_16BIT,	0xC914, 0x0000},
+	{MISENSOR_16BIT,	0xC916, 0x0000},
+	{MISENSOR_16BIT,	0xC918, 0x04FF},
+	{MISENSOR_16BIT,	0xC91A, 0x02CF},
+	{MISENSOR_16BIT,	0xC904, 0x0033},
+	{MISENSOR_16BIT,	0xC906, 0x0040},
+	{MISENSOR_8BIT,   0xC8F2, 0x03},
+	{MISENSOR_8BIT,   0xC8F3, 0x02},
+	{MISENSOR_16BIT,	0xC906, 0x003C},
+	{MISENSOR_16BIT,	0xC8F4, 0x0000},
+	{MISENSOR_16BIT,	0xC8F6, 0x0000},
+	{MISENSOR_16BIT,	0xC8F8, 0x0000},
+	{MISENSOR_16BIT,	0xC8FA, 0xE724},
+	{MISENSOR_16BIT,	0xC8FC, 0x1583},
+	{MISENSOR_16BIT,	0xC8FE, 0x2045},
+	{MISENSOR_16BIT,	0xC900, 0x05DC},
+	{MISENSOR_16BIT,	0xC902, 0x007C},
+	{MISENSOR_8BIT,   0xC90C, 0x80},
+	{MISENSOR_8BIT,   0xC90D, 0x80},
+	{MISENSOR_8BIT,   0xC90E, 0x80},
+	{MISENSOR_8BIT,   0xC90F, 0x88},
+	{MISENSOR_8BIT,   0xC910, 0x80},
+	{MISENSOR_8BIT,   0xC911, 0x80},
+
+	/* LOAD=Step7-CPIPE_Preference */
+	{MISENSOR_16BIT,	0xC926, 0x0020},
+	{MISENSOR_16BIT,	0xC928, 0x009A},
+	{MISENSOR_16BIT,	0xC946, 0x0070},
+	{MISENSOR_16BIT,	0xC948, 0x00F3},
+	{MISENSOR_16BIT,	0xC952, 0x0020},
+	{MISENSOR_16BIT,	0xC954, 0x009A},
+	{MISENSOR_8BIT,   0xC92A, 0x80},
+	{MISENSOR_8BIT,   0xC92B, 0x4B},
+	{MISENSOR_8BIT,   0xC92C, 0x00},
+	{MISENSOR_8BIT,   0xC92D, 0xFF},
+	{MISENSOR_8BIT,   0xC92E, 0x3C},
+	{MISENSOR_8BIT,   0xC92F, 0x02},
+	{MISENSOR_8BIT,   0xC930, 0x06},
+	{MISENSOR_8BIT,   0xC931, 0x64},
+	{MISENSOR_8BIT,   0xC932, 0x01},
+	{MISENSOR_8BIT,   0xC933, 0x0C},
+	{MISENSOR_8BIT,   0xC934, 0x3C},
+	{MISENSOR_8BIT,   0xC935, 0x3C},
+	{MISENSOR_8BIT,   0xC936, 0x3C},
+	{MISENSOR_8BIT,   0xC937, 0x0F},
+	{MISENSOR_8BIT,   0xC938, 0x64},
+	{MISENSOR_8BIT,   0xC939, 0x64},
+	{MISENSOR_8BIT,   0xC93A, 0x64},
+	{MISENSOR_8BIT,   0xC93B, 0x32},
+	{MISENSOR_16BIT,	0xC93C, 0x0020},
+	{MISENSOR_16BIT,	0xC93E, 0x009A},
+	{MISENSOR_16BIT,	0xC940, 0x00DC},
+	{MISENSOR_8BIT,   0xC942, 0x38},
+	{MISENSOR_8BIT,   0xC943, 0x30},
+	{MISENSOR_8BIT,   0xC944, 0x50},
+	{MISENSOR_8BIT,   0xC945, 0x19},
+	{MISENSOR_16BIT,	0xC94A, 0x0230},
+	{MISENSOR_16BIT,	0xC94C, 0x0010},
+	{MISENSOR_16BIT,	0xC94E, 0x01CD},
+	{MISENSOR_8BIT,   0xC950, 0x05},
+	{MISENSOR_8BIT,   0xC951, 0x40},
+	{MISENSOR_8BIT,   0xC87B, 0x1B},
+	{MISENSOR_8BIT,   0xC878, 0x0E},
+	{MISENSOR_16BIT,	0xC890, 0x0080},
+	{MISENSOR_16BIT,	0xC886, 0x0100},
+	{MISENSOR_16BIT,	0xC87C, 0x005A},
+	{MISENSOR_8BIT,   0xB42A, 0x05},
+	{MISENSOR_8BIT,   0xA80A, 0x20},
+
+	/* Speed up AE/AWB */
+	{MISENSOR_16BIT,	0x098E, 0x2802},
+	{MISENSOR_16BIT,	0xA802, 0x0008},
+	{MISENSOR_8BIT,   0xC908, 0x01},
+	{MISENSOR_8BIT,   0xC879, 0x01},
+	{MISENSOR_8BIT,   0xC909, 0x02},
+	{MISENSOR_8BIT,   0xA80A, 0x18},
+	{MISENSOR_8BIT,   0xA80B, 0x18},
+	{MISENSOR_8BIT,   0xAC16, 0x18},
+	{MISENSOR_8BIT,   0xC878, 0x0E},
+
+	{MISENSOR_TOK_TERM, 0, 0}
+};
+
+#endif
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov2680.h b/drivers/staging/media/atomisp/i2c/ov2680.h
new file mode 100644
index 000000000..4e351196f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2680.h
@@ -0,0 +1,917 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for OmniVision OV2680 5M camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __OV2680_H__
+#define __OV2680_H__
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+
+#include "../include/linux/atomisp_platform.h"
+
+/* Defines for register writes and register array processing */
+#define I2C_MSG_LENGTH		0x2
+#define I2C_RETRY_COUNT		5
+
+#define OV2680_FOCAL_LENGTH_NUM	334	/*3.34mm*/
+#define OV2680_FOCAL_LENGTH_DEM	100
+#define OV2680_F_NUMBER_DEFAULT_NUM	24
+#define OV2680_F_NUMBER_DEM	10
+
+#define OV2680_BIN_FACTOR_MAX 4
+
+#define MAX_FMTS		1
+
+/* sensor_mode_data read_mode adaptation */
+#define OV2680_READ_MODE_BINNING_ON	0x0400
+#define OV2680_READ_MODE_BINNING_OFF	0x00
+#define OV2680_INTEGRATION_TIME_MARGIN	8
+
+#define OV2680_MAX_EXPOSURE_VALUE	0xFFF1
+#define OV2680_MAX_GAIN_VALUE		0xFF
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV2680_FOCAL_LENGTH_DEFAULT 0x1B70064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV2680_F_NUMBER_DEFAULT 0x18000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define OV2680_F_NUMBER_RANGE 0x180a180a
+#define OV2680_ID	0x2680
+
+#define OV2680_FINE_INTG_TIME_MIN 0
+#define OV2680_FINE_INTG_TIME_MAX_MARGIN 0
+#define OV2680_COARSE_INTG_TIME_MIN 1
+#define OV2680_COARSE_INTG_TIME_MAX_MARGIN 6
+
+/*
+ * OV2680 System control registers
+ */
+#define OV2680_SW_SLEEP				0x0100
+#define OV2680_SW_RESET				0x0103
+#define OV2680_SW_STREAM			0x0100
+
+#define OV2680_SC_CMMN_CHIP_ID_H		0x300A
+#define OV2680_SC_CMMN_CHIP_ID_L		0x300B
+#define OV2680_SC_CMMN_SCCB_ID			0x302B /* 0x300C*/
+#define OV2680_SC_CMMN_SUB_ID			0x302A /* process, version*/
+
+#define OV2680_GROUP_ACCESS							0x3208 /*Bit[7:4] Group control, Bit[3:0] Group ID*/
+
+#define OV2680_EXPOSURE_H							0x3500 /*Bit[3:0] Bit[19:16] of exposure, remaining 16 bits lies in Reg0x3501&Reg0x3502*/
+#define OV2680_EXPOSURE_M							0x3501
+#define OV2680_EXPOSURE_L							0x3502
+#define OV2680_AGC_H								0x350A /*Bit[1:0] means Bit[9:8] of gain*/
+#define OV2680_AGC_L								0x350B /*Bit[7:0] of gain*/
+
+#define OV2680_HORIZONTAL_START_H					0x3800 /*Bit[11:8]*/
+#define OV2680_HORIZONTAL_START_L					0x3801 /*Bit[7:0]*/
+#define OV2680_VERTICAL_START_H						0x3802 /*Bit[11:8]*/
+#define OV2680_VERTICAL_START_L						0x3803 /*Bit[7:0]*/
+#define OV2680_HORIZONTAL_END_H						0x3804 /*Bit[11:8]*/
+#define OV2680_HORIZONTAL_END_L						0x3805 /*Bit[7:0]*/
+#define OV2680_VERTICAL_END_H						0x3806 /*Bit[11:8]*/
+#define OV2680_VERTICAL_END_L						0x3807 /*Bit[7:0]*/
+#define OV2680_HORIZONTAL_OUTPUT_SIZE_H				0x3808 /*Bit[3:0]*/
+#define OV2680_HORIZONTAL_OUTPUT_SIZE_L				0x3809 /*Bit[7:0]*/
+#define OV2680_VERTICAL_OUTPUT_SIZE_H				0x380a /*Bit[3:0]*/
+#define OV2680_VERTICAL_OUTPUT_SIZE_L				0x380b /*Bit[7:0]*/
+#define OV2680_TIMING_HTS_H							0x380C  /*High 8-bit, and low 8-bit HTS address is 0x380d*/
+#define OV2680_TIMING_HTS_L							0x380D  /*High 8-bit, and low 8-bit HTS address is 0x380d*/
+#define OV2680_TIMING_VTS_H							0x380e  /*High 8-bit, and low 8-bit HTS address is 0x380f*/
+#define OV2680_TIMING_VTS_L							0x380f  /*High 8-bit, and low 8-bit HTS address is 0x380f*/
+#define OV2680_FRAME_OFF_NUM						0x4202
+
+/*Flip/Mirror*/
+#define OV2680_FLIP_REG				0x3820
+#define OV2680_MIRROR_REG			0x3821
+#define OV2680_FLIP_BIT				1
+#define OV2680_MIRROR_BIT			2
+#define OV2680_FLIP_MIRROR_BIT_ENABLE		4
+
+#define OV2680_MWB_RED_GAIN_H			0x5004/*0x3400*/
+#define OV2680_MWB_GREEN_GAIN_H			0x5006/*0x3402*/
+#define OV2680_MWB_BLUE_GAIN_H			0x5008/*0x3404*/
+#define OV2680_MWB_GAIN_MAX				0x0fff
+
+#define OV2680_START_STREAMING			0x01
+#define OV2680_STOP_STREAMING			0x00
+
+#define OV2680_INVALID_CONFIG	0xffffffff
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct ov2680_resolution {
+	const struct ov2680_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int pix_clk_freq;
+	u32 skip_frames;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+};
+
+struct ov2680_format {
+	u8 *desc;
+	u32 pixelformat;
+	struct ov2680_reg *regs;
+};
+
+/*
+ * ov2680 device structure.
+ */
+struct ov2680_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct mutex input_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct ov2680_resolution *res;
+	struct camera_sensor_platform_data *platform_data;
+	bool power_on;
+	u16 exposure;
+	u16 gain;
+	u16 digitgain;
+};
+
+/**
+ * struct ov2680_reg - MI sensor  register format
+ * @type: type of the register
+ * @reg: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct ov2680_reg {
+	u16 reg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+#define to_ov2680_sensor(x) container_of(x, struct ov2680_device, sd)
+
+#define OV2680_MAX_WRITE_BUF_SIZE	30
+
+struct ov2680_write_buffer {
+	u16 addr;
+	u8 data[OV2680_MAX_WRITE_BUF_SIZE];
+};
+
+struct ov2680_write_ctrl {
+	int index;
+	struct ov2680_write_buffer buffer;
+};
+
+static struct ov2680_reg const ov2680_global_setting[] = {
+	{0x0103, 0x01},
+	{0x3002, 0x00},
+	{0x3016, 0x1c},
+	{0x3018, 0x44},
+	{0x3020, 0x00},
+	{0x3080, 0x02},
+	{0x3082, 0x45},
+	{0x3084, 0x09},
+	{0x3085, 0x04},
+	{0x3503, 0x03},
+	{0x350b, 0x36},
+	{0x3600, 0xb4},
+	{0x3603, 0x39},
+	{0x3604, 0x24},
+	{0x3605, 0x00},
+	{0x3620, 0x26},
+	{0x3621, 0x37},
+	{0x3622, 0x04},
+	{0x3628, 0x00},
+	{0x3705, 0x3c},
+	{0x370c, 0x50},
+	{0x370d, 0xc0},
+	{0x3718, 0x88},
+	{0x3720, 0x00},
+	{0x3721, 0x00},
+	{0x3722, 0x00},
+	{0x3723, 0x00},
+	{0x3738, 0x00},
+	{0x3717, 0x58},
+	{0x3781, 0x80},
+	{0x3789, 0x60},
+	{0x3800, 0x00},
+	{0x3819, 0x04},
+	{0x4000, 0x81},
+	{0x4001, 0x40},
+	{0x4602, 0x02},
+	{0x481f, 0x36},
+	{0x4825, 0x36},
+	{0x4837, 0x18},
+	{0x5002, 0x30},
+	{0x5004, 0x04},//manual awb 1x
+	{0x5005, 0x00},
+	{0x5006, 0x04},
+	{0x5007, 0x00},
+	{0x5008, 0x04},
+	{0x5009, 0x00},
+	{0x5080, 0x00},
+	{0x3701, 0x64},  //add on 14/05/13
+	{0x3784, 0x0c},  //based OV2680_R1A_AM10.ovt add on 14/06/13
+	{0x5780, 0x3e},  //based OV2680_R1A_AM10.ovt,Adjust DPC setting (57xx) on 14/06/13
+	{0x5781, 0x0f},
+	{0x5782, 0x04},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x00},
+	{0x578a, 0x01},
+	{0x578b, 0x02},
+	{0x578c, 0x03},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x04},
+	{0x5792, 0x00},
+	{0x5793, 0x00},
+	{0x5794, 0x03}, //based OV2680_R1A_AM10.ovt,Adjust DPC setting (57xx) on 14/06/13
+	{0x0100, 0x00},	//stream off
+
+	{}
+};
+
+/*
+ * 176x144 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_QCIF_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x05},
+	{0x3805, 0xaf},
+	{0x3806, 0x04},
+	{0x3807, 0x47},
+	{0x3808, 0x00},
+	{0x3809, 0xC0},
+	{0x380a, 0x00},
+	{0x380b, 0xa0},
+	{0x380c, 0x06},
+	{0x380d, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x4000, 0x81},
+	{0x4001, 0x40},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc2},
+	{0x3821, 0x01},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ * 352x288 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_CIF_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x03},
+	{0x3805, 0x8f},
+	{0x3806, 0x02},
+	{0x3807, 0xe7},
+	{0x3808, 0x01},
+	{0x3809, 0x70},
+	{0x380a, 0x01},
+	{0x380b, 0x30},
+	{0x380c, 0x06},
+	{0x380d, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc2},
+	{0x3821, 0x01},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ * 336x256 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_QVGA_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x03},
+	{0x3805, 0x4f},
+	{0x3806, 0x02},
+	{0x3807, 0x87},
+	{0x3808, 0x01},
+	{0x3809, 0x50},
+	{0x380a, 0x01},
+	{0x380b, 0x00},
+	{0x380c, 0x06},
+	{0x380d, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc2},
+	{0x3821, 0x01},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ * 656x496 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_656x496_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x05},
+	{0x3805, 0xcf},
+	{0x3806, 0x04},
+	{0x3807, 0x67},
+	{0x3808, 0x02},
+	{0x3809, 0x90},
+	{0x380a, 0x01},
+	{0x380b, 0xf0},
+	{0x380c, 0x06},
+	{0x380d, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc2},
+	{0x3821, 0x01},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ * 720x592 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_720x592_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0x00}, // X_ADDR_START;
+	{0x3802, 0x00},
+	{0x3803, 0x00}, // Y_ADDR_START;
+	{0x3804, 0x05},
+	{0x3805, 0xaf}, // X_ADDR_END;
+	{0x3806, 0x04},
+	{0x3807, 0xaf}, // Y_ADDR_END;
+	{0x3808, 0x02},
+	{0x3809, 0xd0}, // X_OUTPUT_SIZE;
+	{0x380a, 0x02},
+	{0x380b, 0x50}, // Y_OUTPUT_SIZE;
+	{0x380c, 0x06},
+	{0x380d, 0xac}, // HTS;
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x00},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5708, 0x00},
+	{0x5704, 0x02},
+	{0x5705, 0xd0}, // X_WIN;
+	{0x5706, 0x02},
+	{0x5707, 0x50}, // Y_WIN;
+	{0x3820, 0xc2}, // FLIP_FORMAT;
+	{0x3821, 0x01}, // MIRROR_FORMAT;
+	{0x5090, 0x00}, // PRE ISP CTRL16, default value is 0x0C;
+	// BIT[3]: Mirror order, BG or GB;
+	// BIT[2]: Flip order, BR or RB;
+	{0x5081, 0x41},
+	{}
+};
+
+/*
+ * 800x600 30fps  VBlanking 1lane 10Bit (binning)
+ */
+static struct ov2680_reg const ov2680_800x600_30fps[] = {
+	{0x3086, 0x01},
+	{0x370a, 0x23},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0xbf},
+	{0x3808, 0x03},
+	{0x3809, 0x20},
+	{0x380a, 0x02},
+	{0x380b, 0x58},
+	{0x380c, 0x06},
+	{0x380d, 0xac},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x00},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x5708, 0x00},
+	{0x5704, 0x03},
+	{0x5705, 0x20},
+	{0x5706, 0x02},
+	{0x5707, 0x58},
+	{0x3820, 0xc2},
+	{0x3821, 0x01},
+	{0x5090, 0x00},
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+	{0x5081, 0x41},
+	{}
+};
+
+/*
+ * 720p=1280*720 30fps  VBlanking 1lane 10Bit (no-Scaling)
+ */
+static struct ov2680_reg const ov2680_720p_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0xf2},
+	{0x3804, 0x05},
+	{0x3805, 0xbf},
+	{0x3806, 0x03},
+	{0x3807, 0xdd},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x02},
+	{0x380b, 0xe0},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ * 1296x976 30fps  VBlanking 1lane 10Bit(no-scaling)
+ */
+static struct ov2680_reg const ov2680_1296x976_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0xa0},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x05},
+	{0x3805, 0xbf},
+	{0x3806, 0x04},
+	{0x3807, 0x57},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xd0},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc0},
+	{0x3821, 0x00}, //mirror/flip
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ *   1456*1096 30fps  VBlanking 1lane 10bit(no-scaling)
+ */
+static struct ov2680_reg const ov2680_1456x1096_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0x90},
+	{0x3802, 0x00},
+	{0x3803, 0x78},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0xC0},
+	{0x3808, 0x05},
+	{0x3809, 0xb0},
+	{0x380a, 0x04},
+	{0x380b, 0x48},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x00},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x5081, 0x41},
+	{0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x10},
+	{0x5705, 0xa0},
+	{0x5706, 0x0c},
+	{0x5707, 0x78},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	// {0x5090, 0x0c},
+	{}
+};
+
+/*
+ *1616x916  30fps  VBlanking 1lane 10bit
+ */
+
+static struct ov2680_reg const ov2680_1616x916_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x96},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0x39},
+	{0x3808, 0x06},
+	{0x3809, 0x50},
+	{0x380a, 0x03},
+	{0x380b, 0x94},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x5081, 0x41},
+	{0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x06},
+	{0x5705, 0x50},
+	{0x5706, 0x03},
+	{0x5707, 0x94},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	// {0x5090, 0x0C},
+	{}
+};
+
+/*
+ * 1616x1082 30fps VBlanking 1lane 10Bit
+ */
+static struct ov2680_reg const ov2680_1616x1082_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x86},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0xbf},
+	{0x3808, 0x06},
+	{0x3809, 0x50},
+	{0x380a, 0x04},
+	{0x380b, 0x3a},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x00},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x06},
+	{0x5705, 0x50},
+	{0x5706, 0x04},
+	{0x5707, 0x3a},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	// {0x5090, 0x0C},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x5081, 0x41},
+	{}
+};
+
+/*
+ * 1616x1216 30fps VBlanking 1lane 10Bit
+ */
+static struct ov2680_reg const ov2680_1616x1216_30fps[] = {
+	{0x3086, 0x00},
+	{0x370a, 0x21},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0xbf},
+	{0x3808, 0x06},
+	{0x3809, 0x50},//50},//4line for mirror and flip
+	{0x380a, 0x04},
+	{0x380b, 0xc0},//c0},
+	{0x380c, 0x06},
+	{0x380d, 0xa8},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x00},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x4008, 0x00},
+	{0x4009, 0x0b},
+	{0x5081, 0x01},
+	{0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+	{0x5704, 0x06},
+	{0x5705, 0x50},
+	{0x5706, 0x04},
+	{0x5707, 0xcc},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	// {0x5090, 0x0C},
+	{}
+};
+
+static struct ov2680_resolution ov2680_res_preview[] = {
+	{
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 66,
+		.fps = 30,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_1616x1216_30fps,
+	},
+	{
+		.width = 1616,
+		.height = 1082,
+		.pix_clk_freq = 66,
+		.fps = 30,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_1616x1082_30fps,
+	},
+	{
+		.width = 1616,
+		.height = 916,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_1616x916_30fps,
+	},
+	{
+		.width = 1456,
+		.height = 1096,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_1456x1096_30fps,
+	},
+	{
+		.width = 1296,
+		.height = 976,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_1296x976_30fps,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_720p_30fps,
+	},
+	{
+		.width = 800,
+		.height = 600,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_800x600_30fps,
+	},
+	{
+		.width = 720,
+		.height = 592,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_720x592_30fps,
+	},
+	{
+		.width = 656,
+		.height = 496,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_656x496_30fps,
+	},
+	{
+		.width = 336,
+		.height = 256,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_QVGA_30fps,
+	},
+	{
+		.width = 352,
+		.height = 288,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_CIF_30fps,
+	},
+	{
+		.width = 176,
+		.height = 144,
+		.fps = 60,
+		.pix_clk_freq = 66,
+		.pixels_per_line = 1698,//1704,
+		.lines_per_frame = 1294,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2680_QCIF_30fps,
+	},
+};
+
+#define N_RES_PREVIEW (ARRAY_SIZE(ov2680_res_preview))
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov2722.h b/drivers/staging/media/atomisp/i2c/ov2722.h
new file mode 100644
index 000000000..d6e2510bc
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2722.h
@@ -0,0 +1,1272 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for OmniVision OV2722 1080p HD camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __OV2722_H__
+#define __OV2722_H__
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+
+#include "../include/linux/atomisp_platform.h"
+
+#define OV2722_POWER_UP_RETRY_NUM 5
+
+/* Defines for register writes and register array processing */
+#define I2C_MSG_LENGTH		0x2
+#define I2C_RETRY_COUNT		5
+
+#define OV2722_FOCAL_LENGTH_NUM	278	/*2.78mm*/
+#define OV2722_FOCAL_LENGTH_DEM	100
+#define OV2722_F_NUMBER_DEFAULT_NUM	26
+#define OV2722_F_NUMBER_DEM	10
+
+#define MAX_FMTS		1
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV2722_FOCAL_LENGTH_DEFAULT 0x1160064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV2722_F_NUMBER_DEFAULT 0x1a000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define OV2722_F_NUMBER_RANGE 0x1a0a1a0a
+#define OV2720_ID	0x2720
+#define OV2722_ID	0x2722
+
+#define OV2722_FINE_INTG_TIME_MIN 0
+#define OV2722_FINE_INTG_TIME_MAX_MARGIN 0
+#define OV2722_COARSE_INTG_TIME_MIN 1
+#define OV2722_COARSE_INTG_TIME_MAX_MARGIN 4
+
+/*
+ * OV2722 System control registers
+ */
+#define OV2722_SW_SLEEP				0x0100
+#define OV2722_SW_RESET				0x0103
+#define OV2722_SW_STREAM			0x0100
+
+#define OV2722_SC_CMMN_CHIP_ID_H		0x300A
+#define OV2722_SC_CMMN_CHIP_ID_L		0x300B
+#define OV2722_SC_CMMN_SCCB_ID			0x300C
+#define OV2722_SC_CMMN_SUB_ID			0x302A /* process, version*/
+
+#define OV2722_SC_CMMN_PAD_OEN0			0x3000
+#define OV2722_SC_CMMN_PAD_OEN1			0x3001
+#define OV2722_SC_CMMN_PAD_OEN2			0x3002
+#define OV2722_SC_CMMN_PAD_OUT0			0x3008
+#define OV2722_SC_CMMN_PAD_OUT1			0x3009
+#define OV2722_SC_CMMN_PAD_OUT2			0x300D
+#define OV2722_SC_CMMN_PAD_SEL0			0x300E
+#define OV2722_SC_CMMN_PAD_SEL1			0x300F
+#define OV2722_SC_CMMN_PAD_SEL2			0x3010
+
+#define OV2722_SC_CMMN_PAD_PK			0x3011
+#define OV2722_SC_CMMN_A_PWC_PK_O_13		0x3013
+#define OV2722_SC_CMMN_A_PWC_PK_O_14		0x3014
+
+#define OV2722_SC_CMMN_CLKRST0			0x301A
+#define OV2722_SC_CMMN_CLKRST1			0x301B
+#define OV2722_SC_CMMN_CLKRST2			0x301C
+#define OV2722_SC_CMMN_CLKRST3			0x301D
+#define OV2722_SC_CMMN_CLKRST4			0x301E
+#define OV2722_SC_CMMN_CLKRST5			0x3005
+#define OV2722_SC_CMMN_PCLK_DIV_CTRL		0x3007
+#define OV2722_SC_CMMN_CLOCK_SEL		0x3020
+#define OV2722_SC_SOC_CLKRST5			0x3040
+
+#define OV2722_SC_CMMN_PLL_CTRL0		0x3034
+#define OV2722_SC_CMMN_PLL_CTRL1		0x3035
+#define OV2722_SC_CMMN_PLL_CTRL2		0x3039
+#define OV2722_SC_CMMN_PLL_CTRL3		0x3037
+#define OV2722_SC_CMMN_PLL_MULTIPLIER		0x3036
+#define OV2722_SC_CMMN_PLL_DEBUG_OPT		0x3038
+#define OV2722_SC_CMMN_PLLS_CTRL0		0x303A
+#define OV2722_SC_CMMN_PLLS_CTRL1		0x303B
+#define OV2722_SC_CMMN_PLLS_CTRL2		0x303C
+#define OV2722_SC_CMMN_PLLS_CTRL3		0x303D
+
+#define OV2722_SC_CMMN_MIPI_PHY_16		0x3016
+#define OV2722_SC_CMMN_MIPI_PHY_17		0x3017
+#define OV2722_SC_CMMN_MIPI_SC_CTRL_18		0x3018
+#define OV2722_SC_CMMN_MIPI_SC_CTRL_19		0x3019
+#define OV2722_SC_CMMN_MIPI_SC_CTRL_21		0x3021
+#define OV2722_SC_CMMN_MIPI_SC_CTRL_22		0x3022
+
+#define OV2722_AEC_PK_EXPO_H			0x3500
+#define OV2722_AEC_PK_EXPO_M			0x3501
+#define OV2722_AEC_PK_EXPO_L			0x3502
+#define OV2722_AEC_MANUAL_CTRL			0x3503
+#define OV2722_AGC_ADJ_H			0x3508
+#define OV2722_AGC_ADJ_L			0x3509
+#define OV2722_VTS_DIFF_H			0x350c
+#define OV2722_VTS_DIFF_L			0x350d
+#define OV2722_GROUP_ACCESS			0x3208
+#define OV2722_HTS_H				0x380c
+#define OV2722_HTS_L				0x380d
+#define OV2722_VTS_H				0x380e
+#define OV2722_VTS_L				0x380f
+
+#define OV2722_MWB_GAIN_R_H			0x5186
+#define OV2722_MWB_GAIN_R_L			0x5187
+#define OV2722_MWB_GAIN_G_H			0x5188
+#define OV2722_MWB_GAIN_G_L			0x5189
+#define OV2722_MWB_GAIN_B_H			0x518a
+#define OV2722_MWB_GAIN_B_L			0x518b
+
+#define OV2722_H_CROP_START_H			0x3800
+#define OV2722_H_CROP_START_L			0x3801
+#define OV2722_V_CROP_START_H			0x3802
+#define OV2722_V_CROP_START_L			0x3803
+#define OV2722_H_CROP_END_H			0x3804
+#define OV2722_H_CROP_END_L			0x3805
+#define OV2722_V_CROP_END_H			0x3806
+#define OV2722_V_CROP_END_L			0x3807
+#define OV2722_H_OUTSIZE_H			0x3808
+#define OV2722_H_OUTSIZE_L			0x3809
+#define OV2722_V_OUTSIZE_H			0x380a
+#define OV2722_V_OUTSIZE_L			0x380b
+
+#define OV2722_START_STREAMING			0x01
+#define OV2722_STOP_STREAMING			0x00
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct ov2722_resolution {
+	u8 *desc;
+	const struct ov2722_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int pix_clk_freq;
+	u32 skip_frames;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+	bool used;
+	int mipi_freq;
+};
+
+struct ov2722_format {
+	u8 *desc;
+	u32 pixelformat;
+	struct ov2722_reg *regs;
+};
+
+/*
+ * ov2722 device structure.
+ */
+struct ov2722_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	struct mutex input_lock;
+	struct ov2722_resolution *res;
+
+	struct camera_sensor_platform_data *platform_data;
+	int vt_pix_clk_freq_mhz;
+	int run_mode;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 type;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq;
+};
+
+enum ov2722_tok_type {
+	OV2722_8BIT  = 0x0001,
+	OV2722_16BIT = 0x0002,
+	OV2722_32BIT = 0x0004,
+	OV2722_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+	OV2722_TOK_DELAY  = 0xfe00,	/* delay token for reg list */
+	OV2722_TOK_MASK = 0xfff0
+};
+
+/**
+ * struct ov2722_reg - MI sensor  register format
+ * @type: type of the register
+ * @reg: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct ov2722_reg {
+	enum ov2722_tok_type type;
+	u16 reg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+#define to_ov2722_sensor(x) container_of(x, struct ov2722_device, sd)
+
+#define OV2722_MAX_WRITE_BUF_SIZE	30
+
+struct ov2722_write_buffer {
+	u16 addr;
+	u8 data[OV2722_MAX_WRITE_BUF_SIZE];
+};
+
+struct ov2722_write_ctrl {
+	int index;
+	struct ov2722_write_buffer buffer;
+};
+
+/*
+ * Register settings for various resolution
+ */
+#if 0
+static struct ov2722_reg const ov2722_QVGA_30fps[] = {
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x0c},
+	{OV2722_8BIT, 0x373a, 0x1c},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x0c},
+	{OV2722_8BIT, 0x3705, 0x06},
+	{OV2722_8BIT, 0x3730, 0x0e},
+	{OV2722_8BIT, 0x3704, 0x1c},
+	{OV2722_8BIT, 0x3f06, 0x00},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0x46},
+	{OV2722_8BIT, 0x371e, 0x00},
+	{OV2722_8BIT, 0x371f, 0x63},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x01},
+	{OV2722_8BIT, 0x3801, 0x42}, /* H crop start: 322 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x20}, /* V crop start: 32 */
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0x95}, /* H crop end:  1685 */
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x27}, /* V crop end:  1063 */
+	{OV2722_8BIT, 0x3808, 0x01},
+	{OV2722_8BIT, 0x3809, 0x50}, /* H output size: 336 */
+	{OV2722_8BIT, 0x380a, 0x01},
+	{OV2722_8BIT, 0x380b, 0x00}, /* V output size: 256 */
+
+	/* H blank timing */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x00}, /* H total size: 2048 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xa0}, /* V total size: 1184 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x04}, /* H window offset: 5 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x01}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0xc0},
+	{OV2722_8BIT, 0x3821, 0x06}, /* flip isp*/
+	{OV2722_8BIT, 0x3814, 0x71},
+	{OV2722_8BIT, 0x3815, 0x71},
+	{OV2722_8BIT, 0x3612, 0x49},
+	{OV2722_8BIT, 0x3618, 0x00},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0xc3},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x77},
+	{OV2722_8BIT, 0x3a0d, 0x00},
+	{OV2722_8BIT, 0x3a0e, 0x00},
+	{OV2722_8BIT, 0x4520, 0x09},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3000, 0xff},
+	{OV2722_8BIT, 0x3001, 0xff},
+	{OV2722_8BIT, 0x3002, 0xf0},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0x53}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x63},
+	{OV2722_8BIT, 0x3634, 0x24},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1}, /* v_en, h_en, blc_en */
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xff},
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0}, /* AWB red */
+	{OV2722_8BIT, 0x5184, 0xb0}, /* AWB green */
+	{OV2722_8BIT, 0x5185, 0xb0}, /* AWB blue */
+	{OV2722_8BIT, 0x5180, 0x03}, /* AWB manual mode */
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x4800, 0x24}, /* clk lane gate enable */
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x26},
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+
+	/* Added for power optimization */
+	{OV2722_8BIT, 0x3000, 0x00},
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3011, 0x22},
+	{OV2722_8BIT, 0x3a00, 0x58},
+	{OV2722_8BIT, 0x3503, 0x17},
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x46},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x10},
+	{OV2722_TOK_TERM, 0, 0},
+
+};
+
+static struct ov2722_reg const ov2722_480P_30fps[] = {
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x18},
+	{OV2722_8BIT, 0x373a, 0x3c},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x1d},
+	{OV2722_8BIT, 0x3705, 0x12},
+	{OV2722_8BIT, 0x3730, 0x1f},
+	{OV2722_8BIT, 0x3704, 0x3f},
+	{OV2722_8BIT, 0x3f06, 0x1d},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0x83},
+	{OV2722_8BIT, 0x371e, 0x00},
+	{OV2722_8BIT, 0x371f, 0xbd},
+	{OV2722_8BIT, 0x3708, 0x63},
+	{OV2722_8BIT, 0x3709, 0x52},
+	{OV2722_8BIT, 0x3800, 0x00},
+	{OV2722_8BIT, 0x3801, 0xf2}, /* H crop start: 322 - 80 = 242*/
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x20}, /* V crop start:  32*/
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0xBB}, /* H crop end:   1643 + 80 = 1723*/
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x03}, /* V crop end:   1027*/
+	{OV2722_8BIT, 0x3808, 0x02},
+	{OV2722_8BIT, 0x3809, 0xE0}, /* H output size: 656 +80 = 736*/
+	{OV2722_8BIT, 0x380a, 0x01},
+	{OV2722_8BIT, 0x380b, 0xF0}, /* V output size: 496 */
+
+	/* H blank timing */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x00}, /* H total size: 2048 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xa0}, /* V total size: 1184 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x04}, /* H window offset: 5 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x01}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /* flip isp*/
+	{OV2722_8BIT, 0x3814, 0x31},
+	{OV2722_8BIT, 0x3815, 0x31},
+	{OV2722_8BIT, 0x3612, 0x4b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x02},
+	{OV2722_8BIT, 0x3a09, 0x67},
+	{OV2722_8BIT, 0x3a0a, 0x02},
+	{OV2722_8BIT, 0x3a0b, 0x00},
+	{OV2722_8BIT, 0x3a0d, 0x00},
+	{OV2722_8BIT, 0x3a0e, 0x00},
+	{OV2722_8BIT, 0x4520, 0x0a},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3000, 0xff},
+	{OV2722_8BIT, 0x3001, 0xff},
+	{OV2722_8BIT, 0x3002, 0xf0},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0x53}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x63},
+	{OV2722_8BIT, 0x3634, 0x24},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1}, /* v_en, h_en, blc_en */
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xff},
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0}, /* AWB red */
+	{OV2722_8BIT, 0x5184, 0xb0}, /* AWB green */
+	{OV2722_8BIT, 0x5185, 0xb0}, /* AWB blue */
+	{OV2722_8BIT, 0x5180, 0x03}, /* AWB manual mode */
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x4800, 0x24}, /* clk lane gate enable */
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x26},
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+
+	/* Added for power optimization */
+	{OV2722_8BIT, 0x3000, 0x00},
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3011, 0x22},
+	{OV2722_8BIT, 0x3a00, 0x58},
+	{OV2722_8BIT, 0x3503, 0x17},
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x46},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x10},
+	{OV2722_TOK_TERM, 0, 0},
+};
+
+static struct ov2722_reg const ov2722_VGA_30fps[] = {
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x18},
+	{OV2722_8BIT, 0x373a, 0x3c},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x1d},
+	{OV2722_8BIT, 0x3705, 0x12},
+	{OV2722_8BIT, 0x3730, 0x1f},
+	{OV2722_8BIT, 0x3704, 0x3f},
+	{OV2722_8BIT, 0x3f06, 0x1d},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0x83},
+	{OV2722_8BIT, 0x371e, 0x00},
+	{OV2722_8BIT, 0x371f, 0xbd},
+	{OV2722_8BIT, 0x3708, 0x63},
+	{OV2722_8BIT, 0x3709, 0x52},
+	{OV2722_8BIT, 0x3800, 0x01},
+	{OV2722_8BIT, 0x3801, 0x42}, /* H crop start: 322 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x20}, /* V crop start:  32*/
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0x6B}, /* H crop end:   1643*/
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x03}, /* V crop end:   1027*/
+	{OV2722_8BIT, 0x3808, 0x02},
+	{OV2722_8BIT, 0x3809, 0x90}, /* H output size: 656 */
+	{OV2722_8BIT, 0x380a, 0x01},
+	{OV2722_8BIT, 0x380b, 0xF0}, /* V output size: 496 */
+
+	/* H blank timing */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x00}, /* H total size: 2048 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xa0}, /* V total size: 1184 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x04}, /* H window offset: 5 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x01}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /* flip isp*/
+	{OV2722_8BIT, 0x3814, 0x31},
+	{OV2722_8BIT, 0x3815, 0x31},
+	{OV2722_8BIT, 0x3612, 0x4b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x02},
+	{OV2722_8BIT, 0x3a09, 0x67},
+	{OV2722_8BIT, 0x3a0a, 0x02},
+	{OV2722_8BIT, 0x3a0b, 0x00},
+	{OV2722_8BIT, 0x3a0d, 0x00},
+	{OV2722_8BIT, 0x3a0e, 0x00},
+	{OV2722_8BIT, 0x4520, 0x0a},
+	{OV2722_8BIT, 0x4837, 0x29},
+	{OV2722_8BIT, 0x3000, 0xff},
+	{OV2722_8BIT, 0x3001, 0xff},
+	{OV2722_8BIT, 0x3002, 0xf0},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0x53}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x63},
+	{OV2722_8BIT, 0x3634, 0x24},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1}, /* v_en, h_en, blc_en */
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xff},
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0}, /* AWB red */
+	{OV2722_8BIT, 0x5184, 0xb0}, /* AWB green */
+	{OV2722_8BIT, 0x5185, 0xb0}, /* AWB blue */
+	{OV2722_8BIT, 0x5180, 0x03}, /* AWB manual mode */
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x4800, 0x24}, /* clk lane gate enable */
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x26},
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+
+	/* Added for power optimization */
+	{OV2722_8BIT, 0x3000, 0x00},
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3011, 0x22},
+	{OV2722_8BIT, 0x3a00, 0x58},
+	{OV2722_8BIT, 0x3503, 0x17},
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x46},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x10},
+	{OV2722_TOK_TERM, 0, 0},
+};
+#endif
+
+static struct ov2722_reg const ov2722_1632_1092_30fps[] = {
+	{OV2722_8BIT, 0x3021, 0x03}, /* For stand wait for
+				a whole frame complete.(vblank) */
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x24},
+	{OV2722_8BIT, 0x373a, 0x60},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x2e},
+	{OV2722_8BIT, 0x3705, 0x10},
+	{OV2722_8BIT, 0x3730, 0x30},
+	{OV2722_8BIT, 0x3704, 0x62},
+	{OV2722_8BIT, 0x3f06, 0x3a},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0xc4},
+	{OV2722_8BIT, 0x371e, 0x01},
+	{OV2722_8BIT, 0x371f, 0x0d},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x00},
+	{OV2722_8BIT, 0x3801, 0x9E}, /* H crop start: 158 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x01}, /* V crop start: 1 */
+	{OV2722_8BIT, 0x3804, 0x07},
+	{OV2722_8BIT, 0x3805, 0x05}, /* H crop end: 1797 */
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x45}, /* V crop end: 1093 */
+
+	{OV2722_8BIT, 0x3808, 0x06},
+	{OV2722_8BIT, 0x3809, 0x60}, /* H output size: 1632 */
+	{OV2722_8BIT, 0x380a, 0x04},
+	{OV2722_8BIT, 0x380b, 0x44}, /* V output size: 1092 */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0xd4}, /* H timing: 2260 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xdc}, /* V timing: 1244 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x03}, /* H window offset: 3 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x02}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /*  mirror */
+	{OV2722_8BIT, 0x3814, 0x11},
+	{OV2722_8BIT, 0x3815, 0x11},
+	{OV2722_8BIT, 0x3612, 0x0b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0x50},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x18},
+	{OV2722_8BIT, 0x3a0d, 0x03},
+	{OV2722_8BIT, 0x3a0e, 0x03},
+	{OV2722_8BIT, 0x4520, 0x00},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0xd2}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x23},
+	{OV2722_8BIT, 0x3634, 0x54},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1},
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xcf}, /* manual 3a */
+	{OV2722_8BIT, 0x301d, 0xf0}, /* enable group hold */
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0},
+	{OV2722_8BIT, 0x5184, 0xb0},
+	{OV2722_8BIT, 0x5185, 0xb0},
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x2c}, /* 422.4 MHz */
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+	{OV2722_8BIT, 0x3000, 0x00}, /* added for power optimization */
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3503, 0x17}, /* manual 3a */
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x3F},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x00},
+	{OV2722_TOK_TERM, 0, 0}
+};
+
+static struct ov2722_reg const ov2722_1452_1092_30fps[] = {
+	{OV2722_8BIT, 0x3021, 0x03}, /* For stand wait for
+				a whole frame complete.(vblank) */
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x24},
+	{OV2722_8BIT, 0x373a, 0x60},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x2e},
+	{OV2722_8BIT, 0x3705, 0x10},
+	{OV2722_8BIT, 0x3730, 0x30},
+	{OV2722_8BIT, 0x3704, 0x62},
+	{OV2722_8BIT, 0x3f06, 0x3a},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0xc4},
+	{OV2722_8BIT, 0x371e, 0x01},
+	{OV2722_8BIT, 0x371f, 0x0d},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x00},
+	{OV2722_8BIT, 0x3801, 0xF8}, /* H crop start: 248 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x01}, /* V crop start: 1 */
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0xab}, /* H crop end: 1707 */
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x45}, /* V crop end: 1093 */
+	{OV2722_8BIT, 0x3808, 0x05},
+	{OV2722_8BIT, 0x3809, 0xac}, /* H output size: 1452 */
+	{OV2722_8BIT, 0x380a, 0x04},
+	{OV2722_8BIT, 0x380b, 0x44}, /* V output size: 1092 */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0xd4}, /* H timing: 2260 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xdc}, /* V timing: 1244 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x03}, /* H window offset: 3 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x02}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /*  mirror */
+	{OV2722_8BIT, 0x3814, 0x11},
+	{OV2722_8BIT, 0x3815, 0x11},
+	{OV2722_8BIT, 0x3612, 0x0b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0x50},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x18},
+	{OV2722_8BIT, 0x3a0d, 0x03},
+	{OV2722_8BIT, 0x3a0e, 0x03},
+	{OV2722_8BIT, 0x4520, 0x00},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0xd2}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x23},
+	{OV2722_8BIT, 0x3634, 0x54},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1},
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xcf}, /* manual 3a */
+	{OV2722_8BIT, 0x301d, 0xf0}, /* enable group hold */
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0},
+	{OV2722_8BIT, 0x5184, 0xb0},
+	{OV2722_8BIT, 0x5185, 0xb0},
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x2c}, /* 422.4 MHz */
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+	{OV2722_8BIT, 0x3000, 0x00}, /* added for power optimization */
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3503, 0x17}, /* manual 3a */
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x3F},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x00},
+	{OV2722_TOK_TERM, 0, 0}
+};
+
+#if 0
+static struct ov2722_reg const ov2722_1M3_30fps[] = {
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x24},
+	{OV2722_8BIT, 0x373a, 0x60},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x2e},
+	{OV2722_8BIT, 0x3705, 0x10},
+	{OV2722_8BIT, 0x3730, 0x30},
+	{OV2722_8BIT, 0x3704, 0x62},
+	{OV2722_8BIT, 0x3f06, 0x3a},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0xc4},
+	{OV2722_8BIT, 0x371e, 0x01},
+	{OV2722_8BIT, 0x371f, 0x0d},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x01},
+	{OV2722_8BIT, 0x3801, 0x4a},	/* H crop start: 330 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x03},	/* V crop start: 3 */
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0xe1},	/* H crop end:  1761 */
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x47},	/* V crop end:  1095 */
+	{OV2722_8BIT, 0x3808, 0x05},
+	{OV2722_8BIT, 0x3809, 0x88},	/* H output size: 1416 */
+	{OV2722_8BIT, 0x380a, 0x04},
+	{OV2722_8BIT, 0x380b, 0x0a},	/* V output size: 1034 */
+
+	/* H blank timing */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x00},	/* H total size: 2048 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xa0},	/* V total size: 1184 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x05},	/* H window offset: 5 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x02},	/* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06},	/* flip isp */
+	{OV2722_8BIT, 0x3814, 0x11},
+	{OV2722_8BIT, 0x3815, 0x11},
+	{OV2722_8BIT, 0x3612, 0x0b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0x50},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x18},
+	{OV2722_8BIT, 0x3a0d, 0x03},
+	{OV2722_8BIT, 0x3a0e, 0x03},
+	{OV2722_8BIT, 0x4520, 0x00},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3000, 0xff},
+	{OV2722_8BIT, 0x3001, 0xff},
+	{OV2722_8BIT, 0x3002, 0xf0},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0xd2},	/* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x23},
+	{OV2722_8BIT, 0x3634, 0x54},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1},	/* v_en, h_en, blc_en */
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xcf},
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0},	/* AWB red */
+	{OV2722_8BIT, 0x5184, 0xb0},	/* AWB green */
+	{OV2722_8BIT, 0x5185, 0xb0},	/* AWB blue */
+	{OV2722_8BIT, 0x5180, 0x03},	/* AWB manual mode */
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x4800, 0x24},	/* clk lane gate enable */
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x26},
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+
+	/* Added for power optimization */
+	{OV2722_8BIT, 0x3000, 0x00},
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3503, 0x17},
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x46},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x10},
+	{OV2722_TOK_TERM, 0, 0},
+};
+#endif
+
+static struct ov2722_reg const ov2722_1080p_30fps[] = {
+	{OV2722_8BIT, 0x3021, 0x03}, /* For stand wait for a whole
+					frame complete.(vblank) */
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x24},
+	{OV2722_8BIT, 0x373a, 0x60},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x2e},
+	{OV2722_8BIT, 0x3705, 0x2b},
+	{OV2722_8BIT, 0x3730, 0x30},
+	{OV2722_8BIT, 0x3704, 0x62},
+	{OV2722_8BIT, 0x3f06, 0x3a},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0xc4},
+	{OV2722_8BIT, 0x371e, 0x01},
+	{OV2722_8BIT, 0x371f, 0x28},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x00},
+	{OV2722_8BIT, 0x3801, 0x08}, /* H crop start: 8 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0x01}, /* V crop start: 1 */
+	{OV2722_8BIT, 0x3804, 0x07},
+	{OV2722_8BIT, 0x3805, 0x9b}, /* H crop end: 1947 */
+	{OV2722_8BIT, 0x3806, 0x04},
+	{OV2722_8BIT, 0x3807, 0x45}, /* V crop end: 1093 */
+	{OV2722_8BIT, 0x3808, 0x07},
+	{OV2722_8BIT, 0x3809, 0x8c}, /* H output size: 1932 */
+	{OV2722_8BIT, 0x380a, 0x04},
+	{OV2722_8BIT, 0x380b, 0x44}, /* V output size: 1092 */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x14}, /* H timing: 2068 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0x5a}, /* V timing: 1114 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x03}, /* H window offset: 3 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x02}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /*  mirror */
+	{OV2722_8BIT, 0x3814, 0x11},
+	{OV2722_8BIT, 0x3815, 0x11},
+	{OV2722_8BIT, 0x3612, 0x4b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0x50},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x18},
+	{OV2722_8BIT, 0x3a0d, 0x03},
+	{OV2722_8BIT, 0x3a0e, 0x03},
+	{OV2722_8BIT, 0x4520, 0x00},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3000, 0xff},
+	{OV2722_8BIT, 0x3001, 0xff},
+	{OV2722_8BIT, 0x3002, 0xf0},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0x53}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x63},
+	{OV2722_8BIT, 0x3634, 0x24},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1},
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xcd}, /* manual 3a */
+	{OV2722_8BIT, 0x301d, 0xf0}, /* enable group hold */
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x3503, 0x17},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0},
+	{OV2722_8BIT, 0x5184, 0xb0},
+	{OV2722_8BIT, 0x5185, 0xb0},
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x24}, /* 345.6 MHz */
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+	{OV2722_8BIT, 0x3000, 0x00}, /* added for power optimization */
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3011, 0x22},
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x3F},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x00},
+	{OV2722_TOK_TERM, 0, 0}
+};
+
+#if 0 /* Currently unused */
+static struct ov2722_reg const ov2722_720p_30fps[] = {
+	{OV2722_8BIT, 0x3021, 0x03},
+	{OV2722_8BIT, 0x3718, 0x10},
+	{OV2722_8BIT, 0x3702, 0x24},
+	{OV2722_8BIT, 0x373a, 0x60},
+	{OV2722_8BIT, 0x3715, 0x01},
+	{OV2722_8BIT, 0x3703, 0x2e},
+	{OV2722_8BIT, 0x3705, 0x10},
+	{OV2722_8BIT, 0x3730, 0x30},
+	{OV2722_8BIT, 0x3704, 0x62},
+	{OV2722_8BIT, 0x3f06, 0x3a},
+	{OV2722_8BIT, 0x371c, 0x00},
+	{OV2722_8BIT, 0x371d, 0xc4},
+	{OV2722_8BIT, 0x371e, 0x01},
+	{OV2722_8BIT, 0x371f, 0x0d},
+	{OV2722_8BIT, 0x3708, 0x61},
+	{OV2722_8BIT, 0x3709, 0x12},
+	{OV2722_8BIT, 0x3800, 0x01},
+	{OV2722_8BIT, 0x3801, 0x40}, /* H crop start: 320 */
+	{OV2722_8BIT, 0x3802, 0x00},
+	{OV2722_8BIT, 0x3803, 0xb1}, /* V crop start: 177 */
+	{OV2722_8BIT, 0x3804, 0x06},
+	{OV2722_8BIT, 0x3805, 0x55}, /* H crop end: 1621 */
+	{OV2722_8BIT, 0x3806, 0x03},
+	{OV2722_8BIT, 0x3807, 0x95}, /* V crop end: 918 */
+	{OV2722_8BIT, 0x3808, 0x05},
+	{OV2722_8BIT, 0x3809, 0x10}, /* H output size: 0x0788==1928 */
+	{OV2722_8BIT, 0x380a, 0x02},
+	{OV2722_8BIT, 0x380b, 0xe0}, /* output size: 0x02DE==734 */
+	{OV2722_8BIT, 0x380c, 0x08},
+	{OV2722_8BIT, 0x380d, 0x00}, /* H timing: 2048 */
+	{OV2722_8BIT, 0x380e, 0x04},
+	{OV2722_8BIT, 0x380f, 0xa3}, /* V timing: 1187 */
+	{OV2722_8BIT, 0x3810, 0x00},
+	{OV2722_8BIT, 0x3811, 0x03}, /* H window offset: 3 */
+	{OV2722_8BIT, 0x3812, 0x00},
+	{OV2722_8BIT, 0x3813, 0x02}, /* V window offset: 2 */
+	{OV2722_8BIT, 0x3820, 0x80},
+	{OV2722_8BIT, 0x3821, 0x06}, /* mirror */
+	{OV2722_8BIT, 0x3814, 0x11},
+	{OV2722_8BIT, 0x3815, 0x11},
+	{OV2722_8BIT, 0x3612, 0x0b},
+	{OV2722_8BIT, 0x3618, 0x04},
+	{OV2722_8BIT, 0x3a08, 0x01},
+	{OV2722_8BIT, 0x3a09, 0x50},
+	{OV2722_8BIT, 0x3a0a, 0x01},
+	{OV2722_8BIT, 0x3a0b, 0x18},
+	{OV2722_8BIT, 0x3a0d, 0x03},
+	{OV2722_8BIT, 0x3a0e, 0x03},
+	{OV2722_8BIT, 0x4520, 0x00},
+	{OV2722_8BIT, 0x4837, 0x1b},
+	{OV2722_8BIT, 0x3600, 0x08},
+	{OV2722_8BIT, 0x3621, 0xc0},
+	{OV2722_8BIT, 0x3632, 0xd2}, /* added for power opt */
+	{OV2722_8BIT, 0x3633, 0x23},
+	{OV2722_8BIT, 0x3634, 0x54},
+	{OV2722_8BIT, 0x3f01, 0x0c},
+	{OV2722_8BIT, 0x5001, 0xc1},
+	{OV2722_8BIT, 0x3614, 0xf0},
+	{OV2722_8BIT, 0x3630, 0x2d},
+	{OV2722_8BIT, 0x370b, 0x62},
+	{OV2722_8BIT, 0x3706, 0x61},
+	{OV2722_8BIT, 0x4000, 0x02},
+	{OV2722_8BIT, 0x4002, 0xc5},
+	{OV2722_8BIT, 0x4005, 0x08},
+	{OV2722_8BIT, 0x404f, 0x84},
+	{OV2722_8BIT, 0x4051, 0x00},
+	{OV2722_8BIT, 0x5000, 0xcf}, /* manual 3a */
+	{OV2722_8BIT, 0x301d, 0xf0}, /* enable group hold */
+	{OV2722_8BIT, 0x3a18, 0x00},
+	{OV2722_8BIT, 0x3a19, 0x80},
+	{OV2722_8BIT, 0x4521, 0x00},
+	{OV2722_8BIT, 0x5183, 0xb0},
+	{OV2722_8BIT, 0x5184, 0xb0},
+	{OV2722_8BIT, 0x5185, 0xb0},
+	{OV2722_8BIT, 0x370c, 0x0c},
+	{OV2722_8BIT, 0x3035, 0x00},
+	{OV2722_8BIT, 0x3036, 0x26}, /* {0x3036, 0x2c}, //422.4 MHz */
+	{OV2722_8BIT, 0x3037, 0xa1},
+	{OV2722_8BIT, 0x303e, 0x19},
+	{OV2722_8BIT, 0x3038, 0x06},
+	{OV2722_8BIT, 0x3018, 0x04},
+	{OV2722_8BIT, 0x3000, 0x00}, /* added for power optimization */
+	{OV2722_8BIT, 0x3001, 0x00},
+	{OV2722_8BIT, 0x3002, 0x00},
+	{OV2722_8BIT, 0x3a0f, 0x40},
+	{OV2722_8BIT, 0x3a10, 0x38},
+	{OV2722_8BIT, 0x3a1b, 0x48},
+	{OV2722_8BIT, 0x3a1e, 0x30},
+	{OV2722_8BIT, 0x3a11, 0x90},
+	{OV2722_8BIT, 0x3a1f, 0x10},
+	{OV2722_8BIT, 0x3503, 0x17}, /* manual 3a */
+	{OV2722_8BIT, 0x3500, 0x00},
+	{OV2722_8BIT, 0x3501, 0x3F},
+	{OV2722_8BIT, 0x3502, 0x00},
+	{OV2722_8BIT, 0x3508, 0x00},
+	{OV2722_8BIT, 0x3509, 0x00},
+	{OV2722_TOK_TERM, 0, 0},
+};
+#endif
+
+static struct ov2722_resolution ov2722_res_preview[] = {
+	{
+		.desc = "ov2722_1632_1092_30fps",
+		.width = 1632,
+		.height = 1092,
+		.fps = 30,
+		.pix_clk_freq = 85,
+		.used = 0,
+		.pixels_per_line = 2260,
+		.lines_per_frame = 1244,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1632_1092_30fps,
+		.mipi_freq = 422400,
+	},
+	{
+		.desc = "ov2722_1452_1092_30fps",
+		.width = 1452,
+		.height = 1092,
+		.fps = 30,
+		.pix_clk_freq = 85,
+		.used = 0,
+		.pixels_per_line = 2260,
+		.lines_per_frame = 1244,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1452_1092_30fps,
+		.mipi_freq = 422400,
+	},
+	{
+		.desc = "ov2722_1080P_30fps",
+		.width = 1932,
+		.height = 1092,
+		.pix_clk_freq = 69,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2068,
+		.lines_per_frame = 1114,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1080p_30fps,
+		.mipi_freq = 345600,
+	},
+};
+
+#define N_RES_PREVIEW (ARRAY_SIZE(ov2722_res_preview))
+
+/*
+ * Disable non-preview configurations until the configuration selection is
+ * improved.
+ */
+#if 0
+struct ov2722_resolution ov2722_res_still[] = {
+	{
+		.desc = "ov2722_480P_30fps",
+		.width = 1632,
+		.height = 1092,
+		.fps = 30,
+		.pix_clk_freq = 85,
+		.used = 0,
+		.pixels_per_line = 2260,
+		.lines_per_frame = 1244,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1632_1092_30fps,
+		.mipi_freq = 422400,
+	},
+	{
+		.desc = "ov2722_1452_1092_30fps",
+		.width = 1452,
+		.height = 1092,
+		.fps = 30,
+		.pix_clk_freq = 85,
+		.used = 0,
+		.pixels_per_line = 2260,
+		.lines_per_frame = 1244,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1452_1092_30fps,
+		.mipi_freq = 422400,
+	},
+	{
+		.desc = "ov2722_1080P_30fps",
+		.width = 1932,
+		.height = 1092,
+		.pix_clk_freq = 69,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2068,
+		.lines_per_frame = 1114,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1080p_30fps,
+		.mipi_freq = 345600,
+	},
+};
+
+#define N_RES_STILL (ARRAY_SIZE(ov2722_res_still))
+
+struct ov2722_resolution ov2722_res_video[] = {
+	{
+		.desc = "ov2722_QVGA_30fps",
+		.width = 336,
+		.height = 256,
+		.fps = 30,
+		.pix_clk_freq = 73,
+		.used = 0,
+		.pixels_per_line = 2048,
+		.lines_per_frame = 1184,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_QVGA_30fps,
+		.mipi_freq = 364800,
+	},
+	{
+		.desc = "ov2722_480P_30fps",
+		.width = 736,
+		.height = 496,
+		.fps = 30,
+		.pix_clk_freq = 73,
+		.used = 0,
+		.pixels_per_line = 2048,
+		.lines_per_frame = 1184,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_480P_30fps,
+	},
+	{
+		.desc = "ov2722_1080P_30fps",
+		.width = 1932,
+		.height = 1092,
+		.pix_clk_freq = 69,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2068,
+		.lines_per_frame = 1114,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.skip_frames = 3,
+		.regs = ov2722_1080p_30fps,
+		.mipi_freq = 345600,
+	},
+};
+
+#define N_RES_VIDEO (ARRAY_SIZE(ov2722_res_video))
+#endif
+
+static struct ov2722_resolution *ov2722_res = ov2722_res_preview;
+static unsigned long N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/Makefile b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
new file mode 100644
index 000000000..3275f2be2
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_VIDEO_ATOMISP_OV5693) += atomisp-ov5693.o
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/ad5823.h b/drivers/staging/media/atomisp/i2c/ov5693/ad5823.h
new file mode 100644
index 000000000..f1362cd69
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ad5823.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for AD5823 VCM.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __AD5823_H__
+#define __AD5823_H__
+
+#include <linux/types.h>
+
+#define AD5823_VCM_ADDR	0x0c
+
+#define AD5823_REG_RESET		0x01
+#define AD5823_REG_MODE			0x02
+#define AD5823_REG_VCM_MOVE_TIME	0x03
+#define AD5823_REG_VCM_CODE_MSB		0x04
+#define AD5823_REG_VCM_CODE_LSB		0x05
+#define AD5823_REG_VCM_THRESHOLD_MSB	0x06
+#define AD5823_REG_VCM_THRESHOLD_LSB	0x07
+
+#define AD5823_REG_LENGTH		0x1
+
+#define AD5823_RING_CTRL_ENABLE		0x04
+#define AD5823_RING_CTRL_DISABLE	0x00
+
+#define AD5823_RESONANCE_PERIOD		100000
+#define AD5823_RESONANCE_COEF		512
+#define AD5823_HIGH_FREQ_RANGE		0x80
+
+#define VCM_CODE_MSB_MASK		0xfc
+#define AD5823_INIT_FOCUS_POS           350
+
+enum ad5823_tok_type {
+	AD5823_8BIT  = 0x1,
+	AD5823_16BIT = 0x2,
+};
+
+enum ad5823_vcm_mode {
+	AD5823_ARC_RES0 = 0x0,	/* Actuator response control RES1 */
+	AD5823_ARC_RES1 = 0x1,	/* Actuator response control RES0.5 */
+	AD5823_ARC_RES2 = 0x2,	/* Actuator response control RES2 */
+	AD5823_ESRC = 0x3,	/* Enhanced slew rate control */
+	AD5823_DIRECT = 0x4,	/* Direct control */
+};
+
+#define AD5823_INVALID_CONFIG	0xffffffff
+#define AD5823_MAX_FOCUS_POS	1023
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 1023)
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/atomisp-ov5693.c b/drivers/staging/media/atomisp/i2c/ov5693/atomisp-ov5693.c
new file mode 100644
index 000000000..c1cd63145
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/atomisp-ov5693.c
@@ -0,0 +1,1998 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for OmniVision OV5693 1080p HD camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/moduleparam.h>
+#include <media/v4l2-device.h>
+#include <linux/io.h>
+#include <linux/acpi.h>
+#include "../../include/linux/atomisp_gmin_platform.h"
+
+#include "ov5693.h"
+#include "ad5823.h"
+
+#define __cci_delay(t) \
+	do { \
+		if ((t) < 10) { \
+			usleep_range((t) * 1000, ((t) + 1) * 1000); \
+		} else { \
+			msleep((t)); \
+		} \
+	} while (0)
+
+/* Value 30ms reached through experimentation on byt ecs.
+ * The DS specifies a much lower value but when using a smaller value
+ * the I2C bus sometimes locks up permanently when starting the camera.
+ * This issue could not be reproduced on cht, so we can reduce the
+ * delay value to a lower value when insmod.
+ */
+static uint up_delay = 30;
+module_param(up_delay, uint, 0644);
+MODULE_PARM_DESC(up_delay,
+		 "Delay prior to the first CCI transaction for ov5693");
+
+static int vcm_ad_i2c_wr8(struct i2c_client *client, u8 reg, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+
+	msg.addr = VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err != 1) {
+		dev_err(&client->dev, "%s: vcm i2c fail, err code = %d\n",
+			__func__, err);
+		return -EIO;
+	}
+	return 0;
+}
+
+static int ad5823_i2c_write(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = AD5823_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 0x02;
+	msg.buf = &buf[0];
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int ad5823_i2c_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = 0;
+
+	msg[0].addr = AD5823_VCM_ADDR;
+	msg[0].flags = 0;
+	msg[0].len = 0x01;
+	msg[0].buf = &buf[0];
+
+	msg[1].addr = 0x0c;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 0x01;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+	return 0;
+}
+
+static const u32 ov5693_embedded_effective_size = 28;
+
+/* i2c read/write stuff */
+static int ov5693_read_reg(struct i2c_client *client,
+			   u16 data_length, u16 reg, u16 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[6];
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no client->adapter\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	if (data_length != OV5693_8BIT && data_length != OV5693_16BIT
+	    && data_length != OV5693_32BIT) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(msg, 0, sizeof(msg));
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = I2C_MSG_LENGTH;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8)(reg >> 8);
+	data[1] = (u8)(reg & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].len = data_length;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		dev_err(&client->dev,
+			"read from offset 0x%x error %d", reg, err);
+		return err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	if (data_length == OV5693_8BIT)
+		*val = (u8)data[0];
+	else if (data_length == OV5693_16BIT)
+		*val = be16_to_cpu(*(__be16 *)&data[0]);
+	else
+		*val = be32_to_cpu(*(__be32 *)&data[0]);
+
+	return 0;
+}
+
+static int ov5693_i2c_write(struct i2c_client *client, u16 len, u8 *data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+static int vcm_dw_i2c_write(struct i2c_client *client, u16 data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+	__be16 val;
+
+	val = cpu_to_be16(data);
+	msg.addr = VCM_ADDR;
+	msg.flags = 0;
+	msg.len = OV5693_16BIT;
+	msg.buf = (void *)&val;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+/*
+ * Theory: per datasheet, the two VCMs both allow for a 2-byte read.
+ * The DW9714 doesn't actually specify what this does (it has a
+ * two-byte write-only protocol, but specifies the read sequence as
+ * legal), but it returns the same data (zeroes) always, after an
+ * undocumented initial NAK.  The AD5823 has a one-byte address
+ * register to which all writes go, and subsequent reads will cycle
+ * through the 8 bytes of registers.  Notably, the default values (the
+ * device is always power-cycled affirmatively, so we can rely on
+ * these) in AD5823 are not pairwise repetitions of the same 16 bit
+ * word.  So all we have to do is sequentially read two bytes at a
+ * time and see if we detect a difference in any of the first four
+ * pairs.
+ */
+static int vcm_detect(struct i2c_client *client)
+{
+	int i, ret;
+	struct i2c_msg msg;
+	u16 data0 = 0, data;
+
+	for (i = 0; i < 4; i++) {
+		msg.addr = VCM_ADDR;
+		msg.flags = I2C_M_RD;
+		msg.len = sizeof(data);
+		msg.buf = (u8 *)&data;
+		ret = i2c_transfer(client->adapter, &msg, 1);
+
+		/*
+		 * DW9714 always fails the first read and returns
+		 * zeroes for subsequent ones
+		 */
+		if (i == 0 && ret == -EREMOTEIO) {
+			data0 = 0;
+			continue;
+		}
+
+		if (i == 0)
+			data0 = data;
+
+		if (data != data0)
+			return VCM_AD5823;
+	}
+	return ret == 1 ? VCM_DW9714 : ret;
+}
+
+static int ov5693_write_reg(struct i2c_client *client, u16 data_length,
+			    u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	__be16 *wreg = (void *)data;
+	const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
+
+	if (data_length != OV5693_8BIT && data_length != OV5693_16BIT) {
+		dev_err(&client->dev,
+			"%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == OV5693_8BIT) {
+		data[2] = (u8)(val);
+	} else {
+		/* OV5693_16BIT */
+		__be16 *wdata = (void *)&data[2];
+
+		*wdata = cpu_to_be16(val);
+	}
+
+	ret = ov5693_i2c_write(client, len, data);
+	if (ret)
+		dev_err(&client->dev,
+			"write error: wrote 0x%x to offset 0x%x error %d",
+			val, reg, ret);
+
+	return ret;
+}
+
+/*
+ * ov5693_write_reg_array - Initializes a list of OV5693 registers
+ * @client: i2c driver client structure
+ * @reglist: list of registers to be written
+ *
+ * This function initializes a list of registers. When consecutive addresses
+ * are found in a row on the list, this function creates a buffer and sends
+ * consecutive data in a single i2c_transfer().
+ *
+ * __ov5693_flush_reg_array, __ov5693_buf_reg_array() and
+ * __ov5693_write_reg_is_consecutive() are internal functions to
+ * ov5693_write_reg_array_fast() and should be not used anywhere else.
+ *
+ */
+
+static int __ov5693_flush_reg_array(struct i2c_client *client,
+				    struct ov5693_write_ctrl *ctrl)
+{
+	u16 size;
+	__be16 *reg = (void *)&ctrl->buffer.addr;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+
+	*reg = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return ov5693_i2c_write(client, size, (u8 *)reg);
+}
+
+static int __ov5693_buf_reg_array(struct i2c_client *client,
+				  struct ov5693_write_ctrl *ctrl,
+				  const struct ov5693_reg *next)
+{
+	int size;
+	__be16 *data16;
+
+	switch (next->type) {
+	case OV5693_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV5693_16BIT:
+		size = 2;
+
+		data16 = (void *)&ctrl->buffer.data[ctrl->index];
+		*data16 = cpu_to_be16((u16)next->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* When first item is added, we need to store its starting address */
+	if (ctrl->index == 0)
+		ctrl->buffer.addr = next->reg;
+
+	ctrl->index += size;
+
+	/*
+	 * Buffer cannot guarantee free space for u32? Better flush it to avoid
+	 * possible lack of memory for next item.
+	 */
+	if (ctrl->index + sizeof(u16) >= OV5693_MAX_WRITE_BUF_SIZE)
+		return __ov5693_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int __ov5693_write_reg_is_consecutive(struct i2c_client *client,
+	struct ov5693_write_ctrl *ctrl,
+	const struct ov5693_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+static int ov5693_write_reg_array(struct i2c_client *client,
+				  const struct ov5693_reg *reglist)
+{
+	const struct ov5693_reg *next = reglist;
+	struct ov5693_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != OV5693_TOK_TERM; next++) {
+		switch (next->type & OV5693_TOK_MASK) {
+		case OV5693_TOK_DELAY:
+			err = __ov5693_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			msleep(next->val);
+			break;
+		default:
+			/*
+			 * If next address is not consecutive, data needs to be
+			 * flushed before proceed.
+			 */
+			if (!__ov5693_write_reg_is_consecutive(client, &ctrl,
+							       next)) {
+				err = __ov5693_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __ov5693_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev,
+					"%s: write error, aborted\n",
+					__func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __ov5693_flush_reg_array(client, &ctrl);
+}
+
+static int ov5693_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV5693_FOCAL_LENGTH_NUM << 16) | OV5693_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int ov5693_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/*const f number for imx*/
+	*val = (OV5693_F_NUMBER_DEFAULT_NUM << 16) | OV5693_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov5693_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (OV5693_F_NUMBER_DEFAULT_NUM << 24) |
+	       (OV5693_F_NUMBER_DEM << 16) |
+	       (OV5693_F_NUMBER_DEFAULT_NUM << 8) | OV5693_F_NUMBER_DEM;
+	return 0;
+}
+
+static int ov5693_g_bin_factor_x(struct v4l2_subdev *sd, s32 *val)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	*val = ov5693_res[dev->fmt_idx].bin_factor_x;
+
+	return 0;
+}
+
+static int ov5693_g_bin_factor_y(struct v4l2_subdev *sd, s32 *val)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	*val = ov5693_res[dev->fmt_idx].bin_factor_y;
+
+	return 0;
+}
+
+static int ov5693_get_intg_factor(struct i2c_client *client,
+				  struct camera_mipi_info *info,
+				  const struct ov5693_resolution *res)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	unsigned int pix_clk_freq_hz;
+	u16 reg_val;
+	int ret;
+
+	if (!info)
+		return -EINVAL;
+
+	/* pixel clock */
+	pix_clk_freq_hz = res->pix_clk_freq * 1000000;
+
+	dev->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
+	buf->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
+
+	/* get integration time */
+	buf->coarse_integration_time_min = OV5693_COARSE_INTG_TIME_MIN;
+	buf->coarse_integration_time_max_margin =
+	    OV5693_COARSE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_min = OV5693_FINE_INTG_TIME_MIN;
+	buf->fine_integration_time_max_margin =
+	    OV5693_FINE_INTG_TIME_MAX_MARGIN;
+
+	buf->fine_integration_time_def = OV5693_FINE_INTG_TIME_MIN;
+	buf->frame_length_lines = res->lines_per_frame;
+	buf->line_length_pck = res->pixels_per_line;
+	buf->read_mode = res->bin_mode;
+
+	/* get the cropping and output resolution to ISP for this mode. */
+	ret =  ov5693_read_reg(client, OV5693_16BIT,
+			       OV5693_HORIZONTAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = reg_val;
+
+	ret =  ov5693_read_reg(client, OV5693_16BIT,
+			       OV5693_VERTICAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = reg_val;
+
+	ret = ov5693_read_reg(client, OV5693_16BIT,
+			      OV5693_HORIZONTAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = reg_val;
+
+	ret = ov5693_read_reg(client, OV5693_16BIT,
+			      OV5693_VERTICAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = reg_val;
+
+	ret = ov5693_read_reg(client, OV5693_16BIT,
+			      OV5693_HORIZONTAL_OUTPUT_SIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = reg_val;
+
+	ret = ov5693_read_reg(client, OV5693_16BIT,
+			      OV5693_VERTICAL_OUTPUT_SIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_height = reg_val;
+
+	buf->binning_factor_x = res->bin_factor_x ?
+				res->bin_factor_x : 1;
+	buf->binning_factor_y = res->bin_factor_y ?
+				res->bin_factor_y : 1;
+	return 0;
+}
+
+static long __ov5693_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
+				  int gain, int digitgain)
+
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	u16 vts, hts;
+	int ret, exp_val;
+
+	hts = ov5693_res[dev->fmt_idx].pixels_per_line;
+	vts = ov5693_res[dev->fmt_idx].lines_per_frame;
+	/*
+	 * If coarse_itg is larger than 1<<15, can not write to reg directly.
+	 * The way is to write coarse_itg/2 to the reg, meanwhile write 2*hts
+	 * to the reg.
+	 */
+	if (coarse_itg > (1 << 15)) {
+		hts = hts * 2;
+		coarse_itg = (int)coarse_itg / 2;
+	}
+	/* group hold */
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_GROUP_ACCESS, 0x00);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_GROUP_ACCESS);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_TIMING_HTS_H, (hts >> 8) & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_TIMING_HTS_H);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_TIMING_HTS_L, hts & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_TIMING_HTS_L);
+		return ret;
+	}
+	/* Increase the VTS to match exposure + MARGIN */
+	if (coarse_itg > vts - OV5693_INTEGRATION_TIME_MARGIN)
+		vts = (u16)coarse_itg + OV5693_INTEGRATION_TIME_MARGIN;
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_TIMING_VTS_H, (vts >> 8) & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_TIMING_VTS_H);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_TIMING_VTS_L, vts & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_TIMING_VTS_L);
+		return ret;
+	}
+
+	/* set exposure */
+
+	/* Lower four bit should be 0*/
+	exp_val = coarse_itg << 4;
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_EXPOSURE_L, exp_val & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_EXPOSURE_L);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_EXPOSURE_M, (exp_val >> 8) & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_EXPOSURE_M);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_EXPOSURE_H, (exp_val >> 16) & 0x0F);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_EXPOSURE_H);
+		return ret;
+	}
+
+	/* Analog gain */
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_AGC_L, gain & 0xff);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_AGC_L);
+		return ret;
+	}
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_AGC_H, (gain >> 8) & 0xff);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV5693_AGC_H);
+		return ret;
+	}
+
+	/* Digital gain */
+	if (digitgain) {
+		ret = ov5693_write_reg(client, OV5693_16BIT,
+				       OV5693_MWB_RED_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV5693_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov5693_write_reg(client, OV5693_16BIT,
+				       OV5693_MWB_GREEN_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV5693_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov5693_write_reg(client, OV5693_16BIT,
+				       OV5693_MWB_BLUE_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV5693_MWB_RED_GAIN_H);
+			return ret;
+		}
+	}
+
+	/* End group */
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_GROUP_ACCESS, 0x10);
+	if (ret)
+		return ret;
+
+	/* Delay launch group */
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_GROUP_ACCESS, 0xa0);
+	if (ret)
+		return ret;
+	return ret;
+}
+
+static int ov5693_set_exposure(struct v4l2_subdev *sd, int exposure,
+			       int gain, int digitgain)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __ov5693_set_exposure(sd, exposure, gain, digitgain);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long ov5693_s_exposure(struct v4l2_subdev *sd,
+			      struct atomisp_exposure *exposure)
+{
+	u16 coarse_itg = exposure->integration_time[0];
+	u16 analog_gain = exposure->gain[0];
+	u16 digital_gain = exposure->gain[1];
+
+	/* we should not accept the invalid value below */
+	if (analog_gain == 0) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		v4l2_err(client, "%s: invalid value\n", __func__);
+		return -EINVAL;
+	}
+	return ov5693_set_exposure(sd, coarse_itg, analog_gain, digital_gain);
+}
+
+static int ov5693_read_otp_reg_array(struct i2c_client *client, u16 size,
+				     u16 addr, u8 *buf)
+{
+	u16 index;
+	int ret;
+	u16 *pVal = NULL;
+
+	for (index = 0; index <= size; index++) {
+		pVal = (u16 *)(buf + index);
+		ret =
+		    ov5693_read_reg(client, OV5693_8BIT, addr + index,
+				    pVal);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int __ov5693_otp_read(struct v4l2_subdev *sd, u8 *buf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	int ret;
+	int i;
+	u8 *b = buf;
+
+	dev->otp_size = 0;
+	for (i = 1; i < OV5693_OTP_BANK_MAX; i++) {
+		/*set bank NO and OTP read mode. */
+		ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_OTP_BANK_REG,
+				       (i | 0xc0));	//[7:6] 2'b11 [5:0] bank no
+		if (ret) {
+			dev_err(&client->dev, "failed to prepare OTP page\n");
+			return ret;
+		}
+		//pr_debug("write 0x%x->0x%x\n",OV5693_OTP_BANK_REG,(i|0xc0));
+
+		/*enable read */
+		ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_OTP_READ_REG,
+				       OV5693_OTP_MODE_READ);	// enable :1
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to set OTP reading mode page");
+			return ret;
+		}
+		//pr_debug("write 0x%x->0x%x\n",OV5693_OTP_READ_REG,OV5693_OTP_MODE_READ);
+
+		/* Reading the OTP data array */
+		ret = ov5693_read_otp_reg_array(client, OV5693_OTP_BANK_SIZE,
+						OV5693_OTP_START_ADDR,
+						b);
+		if (ret) {
+			dev_err(&client->dev, "failed to read OTP data\n");
+			return ret;
+		}
+
+		//pr_debug("BANK[%2d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", i, *b, *(b+1), *(b+2), *(b+3), *(b+4), *(b+5), *(b+6), *(b+7), *(b+8), *(b+9), *(b+10), *(b+11), *(b+12), *(b+13), *(b+14), *(b+15));
+
+		//Intel OTP map, try to read 320byts first.
+		if (i == 21) {
+			if ((*b) == 0) {
+				dev->otp_size = 320;
+				break;
+			} else {
+				b = buf;
+				continue;
+			}
+		} else if (i ==
+			   24) {		//if the first 320bytes data doesn't not exist, try to read the next 32bytes data.
+			if ((*b) == 0) {
+				dev->otp_size = 32;
+				break;
+			} else {
+				b = buf;
+				continue;
+			}
+		} else if (i ==
+			   27) {		//if the prvious 32bytes data doesn't exist, try to read the next 32bytes data again.
+			if ((*b) == 0) {
+				dev->otp_size = 32;
+				break;
+			} else {
+				dev->otp_size = 0;	// no OTP data.
+				break;
+			}
+		}
+
+		b = b + OV5693_OTP_BANK_SIZE;
+	}
+	return 0;
+}
+
+/*
+ * Read otp data and store it into a kmalloced buffer.
+ * The caller must kfree the buffer when no more needed.
+ * @size: set to the size of the returned otp data.
+ */
+static void *ov5693_otp_read(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 *buf;
+	int ret;
+
+	buf = devm_kzalloc(&client->dev, (OV5693_OTP_DATA_SIZE + 16), GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	//otp valid after mipi on and sw stream on
+	ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_FRAME_OFF_NUM, 0x00);
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_SW_STREAM, OV5693_START_STREAMING);
+
+	ret = __ov5693_otp_read(sd, buf);
+
+	//mipi off and sw stream off after otp read
+	ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_FRAME_OFF_NUM, 0x0f);
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_SW_STREAM, OV5693_STOP_STREAMING);
+
+	/* Driver has failed to find valid data */
+	if (ret) {
+		dev_err(&client->dev, "sensor found no valid OTP data\n");
+		return ERR_PTR(ret);
+	}
+
+	return buf;
+}
+
+static int ov5693_g_priv_int_data(struct v4l2_subdev *sd,
+				  struct v4l2_private_int_data *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	u8 __user *to = priv->data;
+	u32 read_size = priv->size;
+	int ret;
+
+	/* No need to copy data if size is 0 */
+	if (!read_size)
+		goto out;
+
+	if (IS_ERR(dev->otp_data)) {
+		dev_err(&client->dev, "OTP data not available");
+		return PTR_ERR(dev->otp_data);
+	}
+
+	/* Correct read_size value only if bigger than maximum */
+	if (read_size > OV5693_OTP_DATA_SIZE)
+		read_size = OV5693_OTP_DATA_SIZE;
+
+	ret = copy_to_user(to, dev->otp_data, read_size);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to copy OTP data to user\n",
+			__func__);
+		return -EFAULT;
+	}
+
+	pr_debug("%s read_size:%d\n", __func__, read_size);
+
+out:
+	/* Return correct size */
+	priv->size = dev->otp_size;
+
+	return 0;
+}
+
+static long ov5693_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return ov5693_s_exposure(sd, arg);
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+		return ov5693_g_priv_int_data(sd, arg);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * This returns the exposure time being used. This should only be used
+ * for filling in EXIF data, not for actual image processing.
+ */
+static int ov5693_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 reg_v, reg_v2;
+	int ret;
+
+	/* get exposure */
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_EXPOSURE_L,
+			      &reg_v);
+	if (ret)
+		goto err;
+
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_EXPOSURE_M,
+			      &reg_v2);
+	if (ret)
+		goto err;
+
+	reg_v += reg_v2 << 8;
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_EXPOSURE_H,
+			      &reg_v2);
+	if (ret)
+		goto err;
+
+	*value = reg_v + (((u32)reg_v2 << 16));
+err:
+	return ret;
+}
+
+static int ad5823_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 vcm_code;
+
+	ret = ad5823_i2c_read(client, AD5823_REG_VCM_CODE_MSB, &vcm_code);
+	if (ret)
+		return ret;
+
+	/* set reg VCM_CODE_MSB Bit[1:0] */
+	vcm_code = (vcm_code & VCM_CODE_MSB_MASK) |
+		   ((val >> 8) & ~VCM_CODE_MSB_MASK);
+	ret = ad5823_i2c_write(client, AD5823_REG_VCM_CODE_MSB, vcm_code);
+	if (ret)
+		return ret;
+
+	/* set reg VCM_CODE_LSB Bit[7:0] */
+	ret = ad5823_i2c_write(client, AD5823_REG_VCM_CODE_LSB, (val & 0xff));
+	if (ret)
+		return ret;
+
+	/* set required vcm move time */
+	vcm_code = AD5823_RESONANCE_PERIOD / AD5823_RESONANCE_COEF
+		   - AD5823_HIGH_FREQ_RANGE;
+	ret = ad5823_i2c_write(client, AD5823_REG_VCM_MOVE_TIME, vcm_code);
+
+	return ret;
+}
+
+static int ad5823_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	value = min(value, AD5823_MAX_FOCUS_POS);
+	return ad5823_t_focus_vcm(sd, value);
+}
+
+static int ov5693_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	dev_dbg(&client->dev, "%s: FOCUS_POS: 0x%x\n", __func__, value);
+	value = clamp(value, 0, OV5693_VCM_MAX_FOCUS_POS);
+	if (dev->vcm == VCM_DW9714) {
+		if (dev->vcm_update) {
+			ret = vcm_dw_i2c_write(client, VCM_PROTECTION_OFF);
+			if (ret)
+				return ret;
+			ret = vcm_dw_i2c_write(client, DIRECT_VCM);
+			if (ret)
+				return ret;
+			ret = vcm_dw_i2c_write(client, VCM_PROTECTION_ON);
+			if (ret)
+				return ret;
+			dev->vcm_update = false;
+		}
+		ret = vcm_dw_i2c_write(client,
+				       vcm_val(value, VCM_DEFAULT_S));
+	} else if (dev->vcm == VCM_AD5823) {
+		ad5823_t_focus_abs(sd, value);
+	}
+	if (ret == 0) {
+		dev->number_of_steps = value - dev->focus;
+		dev->focus = value;
+		dev->timestamp_t_focus_abs = ktime_get();
+	} else
+		dev_err(&client->dev,
+			"%s: i2c failed. ret %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int ov5693_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	return ov5693_t_focus_abs(sd, dev->focus + value);
+}
+
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 1023)
+static int ov5693_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	u32 status = 0;
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	ktime_t temptime;
+	ktime_t timedelay = ns_to_ktime(min_t(u32,
+					      abs(dev->number_of_steps) * DELAY_PER_STEP_NS,
+					      DELAY_MAX_PER_STEP_NS));
+
+	temptime = ktime_sub(ktime_get(), (dev->timestamp_t_focus_abs));
+	if (ktime_compare(temptime, timedelay) <= 0) {
+		status |= ATOMISP_FOCUS_STATUS_MOVING;
+		status |= ATOMISP_FOCUS_HP_IN_PROGRESS;
+	} else {
+		status |= ATOMISP_FOCUS_STATUS_ACCEPTS_NEW_MOVE;
+		status |= ATOMISP_FOCUS_HP_COMPLETE;
+	}
+
+	*value = status;
+
+	return 0;
+}
+
+static int ov5693_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	s32 val;
+
+	ov5693_q_focus_status(sd, &val);
+
+	if (val & ATOMISP_FOCUS_STATUS_MOVING)
+		*value  = dev->focus - dev->number_of_steps;
+	else
+		*value  = dev->focus;
+
+	return 0;
+}
+
+static int ov5693_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	dev->number_of_steps = value;
+	dev->vcm_update = true;
+	return 0;
+}
+
+static int ov5693_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	dev->number_of_steps = value;
+	dev->vcm_update = true;
+	return 0;
+}
+
+static int ov5693_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5693_device *dev =
+	    container_of(ctrl->handler, struct ov5693_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		dev_dbg(&client->dev, "%s: CID_FOCUS_ABSOLUTE:%d.\n",
+			__func__, ctrl->val);
+		ret = ov5693_t_focus_abs(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_RELATIVE:
+		dev_dbg(&client->dev, "%s: CID_FOCUS_RELATIVE:%d.\n",
+			__func__, ctrl->val);
+		ret = ov5693_t_focus_rel(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_VCM_SLEW:
+		ret = ov5693_t_vcm_slew(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_VCM_TIMING:
+		ret = ov5693_t_vcm_timing(&dev->sd, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int ov5693_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5693_device *dev =
+	    container_of(ctrl->handler, struct ov5693_device, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = ov5693_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = ov5693_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = ov5693_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = ov5693_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		ret = ov5693_q_focus_abs(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_STATUS:
+		ret = ov5693_q_focus_status(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		ret = ov5693_g_bin_factor_x(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_VERT:
+		ret = ov5693_g_bin_factor_y(&dev->sd, &ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = ov5693_s_ctrl,
+	.g_volatile_ctrl = ov5693_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config ov5693_controls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.min = 0x0,
+		.max = 0xffff,
+		.step = 0x01,
+		.def = 0x00,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = OV5693_FOCAL_LENGTH_DEFAULT,
+		.max = OV5693_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = OV5693_FOCAL_LENGTH_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = OV5693_F_NUMBER_DEFAULT,
+		.max = OV5693_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = OV5693_F_NUMBER_DEFAULT,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = OV5693_F_NUMBER_RANGE,
+		.max = OV5693_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = OV5693_F_NUMBER_RANGE,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focus move absolute",
+		.min = 0,
+		.max = OV5693_VCM_MAX_FOCUS_POS,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_RELATIVE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focus move relative",
+		.min = OV5693_VCM_MAX_FOCUS_NEG,
+		.max = OV5693_VCM_MAX_FOCUS_POS,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_STATUS,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focus status",
+		.min = 0,
+		.max = 100,		/* allow enum to grow in the future */
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VCM_SLEW,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vcm slew",
+		.min = 0,
+		.max = OV5693_VCM_SLEW_STEP_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VCM_TIMING,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vcm step time",
+		.min = 0,
+		.max = OV5693_VCM_SLEW_TIME_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "horizontal binning factor",
+		.min = 0,
+		.max = OV5693_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vertical binning factor",
+		.min = 0,
+		.max = OV5693_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+};
+
+static int ov5693_init(struct v4l2_subdev *sd)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	pr_info("%s\n", __func__);
+	mutex_lock(&dev->input_lock);
+	dev->vcm_update = false;
+
+	if (dev->vcm == VCM_AD5823) {
+		ret = vcm_ad_i2c_wr8(client, 0x01, 0x01); /* vcm init test */
+		if (ret)
+			dev_err(&client->dev,
+				"vcm reset failed\n");
+		/*change the mode*/
+		ret = ad5823_i2c_write(client, AD5823_REG_VCM_CODE_MSB,
+				       AD5823_RING_CTRL_ENABLE);
+		if (ret)
+			dev_err(&client->dev,
+				"vcm enable ringing failed\n");
+		ret = ad5823_i2c_write(client, AD5823_REG_MODE,
+				       AD5823_ARC_RES1);
+		if (ret)
+			dev_err(&client->dev,
+				"vcm change mode failed\n");
+	}
+
+	/*change initial focus value for ad5823*/
+	if (dev->vcm == VCM_AD5823) {
+		dev->focus = AD5823_INIT_FOCUS_POS;
+		ov5693_t_focus_abs(sd, AD5823_INIT_FOCUS_POS);
+	} else {
+		dev->focus = 0;
+		ov5693_t_focus_abs(sd, 0);
+	}
+
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret;
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/*
+	 * This driver assumes "internal DVDD, PWDNB tied to DOVDD".
+	 * In this set up only gpio0 (XSHUTDN) should be available
+	 * but in some products (for example ECS) gpio1 (PWDNB) is
+	 * also available. If gpio1 is available we emulate it being
+	 * tied to DOVDD here.
+	 */
+	if (flag) {
+		ret = dev->platform_data->v2p8_ctrl(sd, 1);
+		dev->platform_data->gpio1_ctrl(sd, 1);
+		if (ret == 0) {
+			ret = dev->platform_data->v1p8_ctrl(sd, 1);
+			if (ret) {
+				dev->platform_data->gpio1_ctrl(sd, 0);
+				ret = dev->platform_data->v2p8_ctrl(sd, 0);
+			}
+		}
+	} else {
+		dev->platform_data->gpio1_ctrl(sd, 0);
+		ret = dev->platform_data->v1p8_ctrl(sd, 0);
+		ret |= dev->platform_data->v2p8_ctrl(sd, 0);
+	}
+
+	return ret;
+}
+
+static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	return dev->platform_data->gpio0_ctrl(sd, flag);
+}
+
+static int __power_up(struct v4l2_subdev *sd)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 1);
+	if (ret)
+		goto fail_power;
+
+	/* according to DS, at least 5ms is needed between DOVDD and PWDN */
+	/* add this delay time to 10~11ms*/
+	usleep_range(10000, 11000);
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 1);
+	if (ret) {
+		ret = gpio_ctrl(sd, 1);
+		if (ret)
+			goto fail_power;
+	}
+
+	/* flis clock control */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret)
+		goto fail_clk;
+
+	__cci_delay(up_delay);
+
+	return 0;
+
+fail_clk:
+	gpio_ctrl(sd, 0);
+fail_power:
+	power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	dev->focus = OV5693_INVALID_CONFIG;
+	if (!dev->platform_data) {
+		dev_err(&client->dev,
+			"no camera_sensor_platform_data");
+		return -ENODEV;
+	}
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* gpio ctrl */
+	ret = gpio_ctrl(sd, 0);
+	if (ret) {
+		ret = gpio_ctrl(sd, 0);
+		if (ret)
+			dev_err(&client->dev, "gpio failed 2\n");
+	}
+
+	/* power control */
+	ret = power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	return ret;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	static const int retry_count = 4;
+	int i, ret;
+
+	for (i = 0; i < retry_count; i++) {
+		ret = __power_up(sd);
+		if (!ret)
+			return 0;
+
+		power_down(sd);
+	}
+	return ret;
+}
+
+static int ov5693_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	pr_info("%s: on %d\n", __func__, on);
+	if (on == 0)
+		return power_down(sd);
+	else {
+		ret = power_up(sd);
+		if (!ret) {
+			ret = ov5693_init(sd);
+			/* restore settings */
+			ov5693_res = ov5693_res_preview;
+			N_RES = N_RES_PREVIEW;
+		}
+	}
+	return ret;
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between res_w/res_h and w/h.
+ * distance = (res_w/res_h - w/h) / (w/h) * 8192
+ * res->width/height smaller than w/h wouldn't be considered.
+ * The gap of ratio larger than 1/8 wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 1024
+static int distance(struct ov5693_resolution *res, u32 w, u32 h)
+{
+	int ratio;
+	int distance;
+
+	if (w == 0 || h == 0 ||
+	    res->width < w || res->height < h)
+		return -1;
+
+	ratio = res->width << 13;
+	ratio /= w;
+	ratio *= h;
+	ratio /= res->height;
+
+	distance = abs(ratio - 8192);
+
+	if (distance > LARGEST_ALLOWED_RATIO_MISMATCH)
+		return -1;
+
+	return distance;
+}
+
+/* Return the nearest higher resolution index
+ * Firstly try to find the approximate aspect ratio resolution
+ * If we find multiple same AR resolutions, choose the
+ * minimal size.
+ */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	int min_res_w = INT_MAX;
+	struct ov5693_resolution *tmp_res = NULL;
+
+	for (i = 0; i < N_RES; i++) {
+		tmp_res = &ov5693_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+			min_res_w = ov5693_res[i].width;
+			continue;
+		}
+		if (dist == min_dist && ov5693_res[i].width < min_res_w)
+			idx = i;
+	}
+
+	return idx;
+}
+
+static int get_resolution_index(int w, int h)
+{
+	int i;
+
+	for (i = 0; i < N_RES; i++) {
+		if (w != ov5693_res[i].width)
+			continue;
+		if (h != ov5693_res[i].height)
+			continue;
+
+		return i;
+	}
+
+	return -1;
+}
+
+/* TODO: remove it. */
+static int startup(struct v4l2_subdev *sd)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	ret = ov5693_write_reg(client, OV5693_8BIT,
+			       OV5693_SW_RESET, 0x01);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 reset err.\n");
+		return ret;
+	}
+
+	ret = ov5693_write_reg_array(client, ov5693_global_setting);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 write register err.\n");
+		return ret;
+	}
+
+	ret = ov5693_write_reg_array(client, ov5693_res[dev->fmt_idx].regs);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 write register err.\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int ov5693_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *ov5693_info = NULL;
+	int ret = 0;
+	int idx;
+
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+	ov5693_info = v4l2_get_subdev_hostdata(sd);
+	if (!ov5693_info)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	idx = nearest_resolution_index(fmt->width, fmt->height);
+	if (idx == -1) {
+		/* return the largest resolution */
+		fmt->width = ov5693_res[N_RES - 1].width;
+		fmt->height = ov5693_res[N_RES - 1].height;
+	} else {
+		fmt->width = ov5693_res[idx].width;
+		fmt->height = ov5693_res[idx].height;
+	}
+
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+	}
+
+	dev->fmt_idx = get_resolution_index(fmt->width, fmt->height);
+	if (dev->fmt_idx == -1) {
+		dev_err(&client->dev, "get resolution fail\n");
+		mutex_unlock(&dev->input_lock);
+		return -EINVAL;
+	}
+
+	ret = startup(sd);
+	if (ret) {
+		int i = 0;
+
+		dev_err(&client->dev, "ov5693 startup err, retry to power up\n");
+		for (i = 0; i < OV5693_POWER_UP_RETRY_NUM; i++) {
+			dev_err(&client->dev,
+				"ov5693 retry to power up %d/%d times, result: ",
+				i + 1, OV5693_POWER_UP_RETRY_NUM);
+			power_down(sd);
+			ret = power_up(sd);
+			if (!ret) {
+				mutex_unlock(&dev->input_lock);
+				ov5693_init(sd);
+				mutex_lock(&dev->input_lock);
+			} else {
+				dev_err(&client->dev, "power up failed, continue\n");
+				continue;
+			}
+			ret = startup(sd);
+			if (ret) {
+				dev_err(&client->dev, " startup FAILED!\n");
+			} else {
+				dev_err(&client->dev, " startup SUCCESS!\n");
+				break;
+			}
+		}
+	}
+
+	/*
+	 * After sensor settings are set to HW, sometimes stream is started.
+	 * This would cause ISP timeout because ISP is not ready to receive
+	 * data yet. So add stop streaming here.
+	 */
+	ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_SW_STREAM,
+			       OV5693_STOP_STREAMING);
+	if (ret)
+		dev_warn(&client->dev, "ov5693 stream off err\n");
+
+	ret = ov5693_get_intg_factor(client, ov5693_info,
+				     &ov5693_res[dev->fmt_idx]);
+	if (ret) {
+		dev_err(&client->dev, "failed to get integration_factor\n");
+		goto err;
+	}
+
+	ov5693_info->metadata_width = fmt->width * 10 / 8;
+	ov5693_info->metadata_height = 1;
+	ov5693_info->metadata_effective_width = &ov5693_embedded_effective_size;
+
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov5693_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	fmt->width = ov5693_res[dev->fmt_idx].width;
+	fmt->height = ov5693_res[dev->fmt_idx].height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov5693_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u16 high, low;
+	int ret;
+	u16 id;
+	u8 revision;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_SC_CMMN_CHIP_ID_H, &high);
+	if (ret) {
+		dev_err(&client->dev, "sensor_id_high = 0x%x\n", high);
+		return -ENODEV;
+	}
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_SC_CMMN_CHIP_ID_L, &low);
+	if (ret)
+		return ret;
+	id = ((((u16)high) << 8) | (u16)low);
+
+	if (id != OV5693_ID) {
+		dev_err(&client->dev, "sensor ID error 0x%x\n", id);
+		return -ENODEV;
+	}
+
+	ret = ov5693_read_reg(client, OV5693_8BIT,
+			      OV5693_SC_CMMN_SUB_ID, &high);
+	revision = (u8)high & 0x0f;
+
+	dev_dbg(&client->dev, "sensor_revision = 0x%x\n", revision);
+	dev_dbg(&client->dev, "detect ov5693 success\n");
+	return 0;
+}
+
+static int ov5693_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	ret = ov5693_write_reg(client, OV5693_8BIT, OV5693_SW_STREAM,
+			       enable ? OV5693_START_STREAMING :
+			       OV5693_STOP_STREAMING);
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ov5693_s_config(struct v4l2_subdev *sd,
+			   int irq, void *platform_data)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+	/* power off the module, then power on it in future
+	 * as first power on by board may not fulfill the
+	 * power on sequqence needed by the module
+	 */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 power-off err.\n");
+		goto fail_power_off;
+	}
+
+	ret = power_up(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 power-up err.\n");
+		goto fail_power_on;
+	}
+
+	if (!dev->vcm)
+		dev->vcm = vcm_detect(client);
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = ov5693_detect(client);
+	if (ret) {
+		dev_err(&client->dev, "ov5693_detect err s_config.\n");
+		goto fail_csi_cfg;
+	}
+
+	dev->otp_data = ov5693_otp_read(sd);
+
+	/* turn off sensor, after probed */
+	ret = power_down(sd);
+	if (ret) {
+		dev_err(&client->dev, "ov5693 power-off err.\n");
+		goto fail_csi_cfg;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+
+fail_csi_cfg:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_power_on:
+	power_down(sd);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+fail_power_off:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov5693_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = ov5693_res[dev->fmt_idx].fps;
+
+	return 0;
+}
+
+static int ov5693_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int ov5693_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = ov5693_res[index].width;
+	fse->min_height = ov5693_res[index].height;
+	fse->max_width = ov5693_res[index].width;
+	fse->max_height = ov5693_res[index].height;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov5693_video_ops = {
+	.s_stream = ov5693_s_stream,
+	.g_frame_interval = ov5693_g_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops ov5693_core_ops = {
+	.s_power = ov5693_s_power,
+	.ioctl = ov5693_ioctl,
+};
+
+static const struct v4l2_subdev_pad_ops ov5693_pad_ops = {
+	.enum_mbus_code = ov5693_enum_mbus_code,
+	.enum_frame_size = ov5693_enum_frame_size,
+	.get_fmt = ov5693_get_fmt,
+	.set_fmt = ov5693_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov5693_ops = {
+	.core = &ov5693_core_ops,
+	.video = &ov5693_video_ops,
+	.pad = &ov5693_pad_ops,
+};
+
+static void ov5693_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	dev_dbg(&client->dev, "ov5693_remove...\n");
+
+	dev->platform_data->csi_cfg(sd, 0);
+
+	v4l2_device_unregister_subdev(sd);
+
+	atomisp_gmin_remove_subdev(sd);
+
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+}
+
+static int ov5693_probe(struct i2c_client *client)
+{
+	struct ov5693_device *dev;
+	int i2c;
+	int ret;
+	void *pdata;
+	unsigned int i;
+
+	/*
+	 * Firmware workaround: Some modules use a "secondary default"
+	 * address of 0x10 which doesn't appear on schematics, and
+	 * some BIOS versions haven't gotten the memo.  Work around
+	 * via config.
+	 */
+	i2c = gmin_get_var_int(&client->dev, false, "I2CAddr", -1);
+	if (i2c != -1) {
+		dev_info(&client->dev,
+			 "Overriding firmware-provided I2C address (0x%x) with 0x%x\n",
+			 client->addr, i2c);
+		client->addr = i2c;
+	}
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&dev->sd, client, &ov5693_ops);
+
+	pdata = gmin_camera_platform_data(&dev->sd,
+					  ATOMISP_INPUT_FORMAT_RAW_10,
+					  atomisp_bayer_order_bggr);
+	if (!pdata) {
+		ret = -EINVAL;
+		goto out_free;
+	}
+
+	ret = ov5693_s_config(&dev->sd, client->irq, pdata);
+	if (ret)
+		goto out_free;
+
+	ret = atomisp_register_i2c_module(&dev->sd, pdata, RAW_CAMERA);
+	if (ret)
+		goto out_free;
+
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret =
+	    v4l2_ctrl_handler_init(&dev->ctrl_handler,
+				   ARRAY_SIZE(ov5693_controls));
+	if (ret) {
+		ov5693_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ov5693_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &ov5693_controls[i],
+				     NULL);
+
+	if (dev->ctrl_handler.error) {
+		ov5693_remove(client);
+		return dev->ctrl_handler.error;
+	}
+
+	/* Use same lock for controls as for everything else. */
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = &dev->ctrl_handler;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret)
+		ov5693_remove(client);
+
+	return ret;
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static const struct acpi_device_id ov5693_acpi_match[] = {
+	{"INT33BE"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov5693_acpi_match);
+
+static struct i2c_driver ov5693_driver = {
+	.driver = {
+		.name = "ov5693",
+		.acpi_match_table = ov5693_acpi_match,
+	},
+	.probe_new = ov5693_probe,
+	.remove = ov5693_remove,
+};
+module_i2c_driver(ov5693_driver);
+
+MODULE_DESCRIPTION("A low-level driver for OmniVision 5693 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/ov5693.h b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.h
new file mode 100644
index 000000000..a1366666f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.h
@@ -0,0 +1,1392 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for OmniVision OV5693 5M camera sensor.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __OV5693_H__
+#define __OV5693_H__
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+
+#include "../../include/linux/atomisp_platform.h"
+
+/*
+ * FIXME: non-preview resolutions are currently broken
+ */
+#define ENABLE_NON_PREVIEW	0
+
+#define OV5693_POWER_UP_RETRY_NUM 5
+
+/* Defines for register writes and register array processing */
+#define I2C_MSG_LENGTH		0x2
+#define I2C_RETRY_COUNT		5
+
+#define OV5693_FOCAL_LENGTH_NUM	334	/*3.34mm*/
+#define OV5693_FOCAL_LENGTH_DEM	100
+#define OV5693_F_NUMBER_DEFAULT_NUM	24
+#define OV5693_F_NUMBER_DEM	10
+
+#define MAX_FMTS		1
+
+/* sensor_mode_data read_mode adaptation */
+#define OV5693_READ_MODE_BINNING_ON	0x0400
+#define OV5693_READ_MODE_BINNING_OFF	0x00
+#define OV5693_INTEGRATION_TIME_MARGIN	8
+
+#define OV5693_MAX_EXPOSURE_VALUE	0xFFF1
+#define OV5693_MAX_GAIN_VALUE		0xFF
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV5693_FOCAL_LENGTH_DEFAULT 0x1B70064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define OV5693_F_NUMBER_DEFAULT 0x18000a
+
+/*
+ * f-number range bits definition:
+ * bits 31-24: max f-number numerator
+ * bits 23-16: max f-number denominator
+ * bits 15-8: min f-number numerator
+ * bits 7-0: min f-number denominator
+ */
+#define OV5693_F_NUMBER_RANGE 0x180a180a
+#define OV5693_ID	0x5690
+
+#define OV5693_FINE_INTG_TIME_MIN 0
+#define OV5693_FINE_INTG_TIME_MAX_MARGIN 0
+#define OV5693_COARSE_INTG_TIME_MIN 1
+#define OV5693_COARSE_INTG_TIME_MAX_MARGIN 6
+
+#define OV5693_BIN_FACTOR_MAX 4
+/*
+ * OV5693 System control registers
+ */
+#define OV5693_SW_SLEEP				0x0100
+#define OV5693_SW_RESET				0x0103
+#define OV5693_SW_STREAM			0x0100
+
+#define OV5693_SC_CMMN_CHIP_ID_H		0x300A
+#define OV5693_SC_CMMN_CHIP_ID_L		0x300B
+#define OV5693_SC_CMMN_SCCB_ID			0x300C
+#define OV5693_SC_CMMN_SUB_ID			0x302A /* process, version*/
+/*Bit[7:4] Group control, Bit[3:0] Group ID*/
+#define OV5693_GROUP_ACCESS			0x3208
+/*
+*Bit[3:0] Bit[19:16] of exposure,
+*remaining 16 bits lies in Reg0x3501&Reg0x3502
+*/
+#define OV5693_EXPOSURE_H			0x3500
+#define OV5693_EXPOSURE_M			0x3501
+#define OV5693_EXPOSURE_L			0x3502
+/*Bit[1:0] means Bit[9:8] of gain*/
+#define OV5693_AGC_H				0x350A
+#define OV5693_AGC_L				0x350B /*Bit[7:0] of gain*/
+
+#define OV5693_HORIZONTAL_START_H		0x3800 /*Bit[11:8]*/
+#define OV5693_HORIZONTAL_START_L		0x3801 /*Bit[7:0]*/
+#define OV5693_VERTICAL_START_H			0x3802 /*Bit[11:8]*/
+#define OV5693_VERTICAL_START_L			0x3803 /*Bit[7:0]*/
+#define OV5693_HORIZONTAL_END_H			0x3804 /*Bit[11:8]*/
+#define OV5693_HORIZONTAL_END_L			0x3805 /*Bit[7:0]*/
+#define OV5693_VERTICAL_END_H			0x3806 /*Bit[11:8]*/
+#define OV5693_VERTICAL_END_L			0x3807 /*Bit[7:0]*/
+#define OV5693_HORIZONTAL_OUTPUT_SIZE_H		0x3808 /*Bit[3:0]*/
+#define OV5693_HORIZONTAL_OUTPUT_SIZE_L		0x3809 /*Bit[7:0]*/
+#define OV5693_VERTICAL_OUTPUT_SIZE_H		0x380a /*Bit[3:0]*/
+#define OV5693_VERTICAL_OUTPUT_SIZE_L		0x380b /*Bit[7:0]*/
+/*High 8-bit, and low 8-bit HTS address is 0x380d*/
+#define OV5693_TIMING_HTS_H			0x380C
+/*High 8-bit, and low 8-bit HTS address is 0x380d*/
+#define OV5693_TIMING_HTS_L			0x380D
+/*High 8-bit, and low 8-bit HTS address is 0x380f*/
+#define OV5693_TIMING_VTS_H			0x380e
+/*High 8-bit, and low 8-bit HTS address is 0x380f*/
+#define OV5693_TIMING_VTS_L			0x380f
+
+#define OV5693_MWB_RED_GAIN_H			0x3400
+#define OV5693_MWB_GREEN_GAIN_H			0x3402
+#define OV5693_MWB_BLUE_GAIN_H			0x3404
+#define OV5693_MWB_GAIN_MAX			0x0fff
+
+#define OV5693_START_STREAMING			0x01
+#define OV5693_STOP_STREAMING			0x00
+
+#define VCM_ADDR           0x0c
+#define VCM_CODE_MSB       0x04
+
+#define OV5693_INVALID_CONFIG	0xffffffff
+
+#define OV5693_VCM_SLEW_STEP			0x30F0
+#define OV5693_VCM_SLEW_STEP_MAX		0x7
+#define OV5693_VCM_SLEW_STEP_MASK		0x7
+#define OV5693_VCM_CODE				0x30F2
+#define OV5693_VCM_SLEW_TIME			0x30F4
+#define OV5693_VCM_SLEW_TIME_MAX		0xffff
+#define OV5693_VCM_ENABLE			0x8000
+
+#define OV5693_VCM_MAX_FOCUS_NEG       -1023
+#define OV5693_VCM_MAX_FOCUS_POS       1023
+
+#define DLC_ENABLE 1
+#define DLC_DISABLE 0
+#define VCM_PROTECTION_OFF     0xeca3
+#define VCM_PROTECTION_ON      0xdc51
+#define VCM_DEFAULT_S 0x0
+#define vcm_step_s(a) (u8)(a & 0xf)
+#define vcm_step_mclk(a) (u8)((a >> 4) & 0x3)
+#define vcm_dlc_mclk(dlc, mclk) (u16)((dlc << 3) | mclk | 0xa104)
+#define vcm_tsrc(tsrc) (u16)(tsrc << 3 | 0xf200)
+#define vcm_val(data, s) (u16)(data << 4 | s)
+#define DIRECT_VCM vcm_dlc_mclk(0, 0)
+
+/* Defines for OTP Data Registers */
+#define OV5693_FRAME_OFF_NUM		0x4202
+#define OV5693_OTP_BYTE_MAX		32	//change to 32 as needed by otpdata
+#define OV5693_OTP_SHORT_MAX		16
+#define OV5693_OTP_START_ADDR		0x3D00
+#define OV5693_OTP_END_ADDR		0x3D0F
+#define OV5693_OTP_DATA_SIZE		320
+#define OV5693_OTP_PROGRAM_REG		0x3D80
+#define OV5693_OTP_READ_REG		0x3D81	// 1:Enable 0:disable
+#define OV5693_OTP_BANK_REG		0x3D84	//otp bank and mode
+#define OV5693_OTP_READY_REG_DONE	1
+#define OV5693_OTP_BANK_MAX		28
+#define OV5693_OTP_BANK_SIZE		16	//16 bytes per bank
+#define OV5693_OTP_READ_ONETIME		16
+#define OV5693_OTP_MODE_READ		1
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+struct ov5693_resolution {
+	u8 *desc;
+	const struct ov5693_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	int pix_clk_freq;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	u8 bin_mode;
+	bool used;
+};
+
+struct ov5693_format {
+	u8 *desc;
+	u32 pixelformat;
+	struct ov5693_reg *regs;
+};
+
+enum vcm_type {
+	VCM_UNKNOWN,
+	VCM_AD5823,
+	VCM_DW9714,
+};
+
+/*
+ * ov5693 device structure.
+ */
+struct ov5693_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	struct mutex input_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct camera_sensor_platform_data *platform_data;
+	ktime_t timestamp_t_focus_abs;
+	int vt_pix_clk_freq_mhz;
+	int fmt_idx;
+	int run_mode;
+	int otp_size;
+	u8 *otp_data;
+	u32 focus;
+	s16 number_of_steps;
+	u8 res;
+	u8 type;
+	bool vcm_update;
+	enum vcm_type vcm;
+};
+
+enum ov5693_tok_type {
+	OV5693_8BIT  = 0x0001,
+	OV5693_16BIT = 0x0002,
+	OV5693_32BIT = 0x0004,
+	OV5693_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+	OV5693_TOK_DELAY  = 0xfe00,	/* delay token for reg list */
+	OV5693_TOK_MASK = 0xfff0
+};
+
+/**
+ * struct ov5693_reg - MI sensor  register format
+ * @type: type of the register
+ * @reg: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for sensor register initialization values
+ */
+struct ov5693_reg {
+	enum ov5693_tok_type type;
+	u16 reg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+#define to_ov5693_sensor(x) container_of(x, struct ov5693_device, sd)
+
+#define OV5693_MAX_WRITE_BUF_SIZE	30
+
+struct ov5693_write_buffer {
+	u16 addr;
+	u8 data[OV5693_MAX_WRITE_BUF_SIZE];
+};
+
+struct ov5693_write_ctrl {
+	int index;
+	struct ov5693_write_buffer buffer;
+};
+
+static struct ov5693_reg const ov5693_global_setting[] = {
+	{OV5693_8BIT, 0x0103, 0x01},
+	{OV5693_8BIT, 0x3001, 0x0a},
+	{OV5693_8BIT, 0x3002, 0x80},
+	{OV5693_8BIT, 0x3006, 0x00},
+	{OV5693_8BIT, 0x3011, 0x21},
+	{OV5693_8BIT, 0x3012, 0x09},
+	{OV5693_8BIT, 0x3013, 0x10},
+	{OV5693_8BIT, 0x3014, 0x00},
+	{OV5693_8BIT, 0x3015, 0x08},
+	{OV5693_8BIT, 0x3016, 0xf0},
+	{OV5693_8BIT, 0x3017, 0xf0},
+	{OV5693_8BIT, 0x3018, 0xf0},
+	{OV5693_8BIT, 0x301b, 0xb4},
+	{OV5693_8BIT, 0x301d, 0x02},
+	{OV5693_8BIT, 0x3021, 0x00},
+	{OV5693_8BIT, 0x3022, 0x01},
+	{OV5693_8BIT, 0x3028, 0x44},
+	{OV5693_8BIT, 0x3098, 0x02},
+	{OV5693_8BIT, 0x3099, 0x19},
+	{OV5693_8BIT, 0x309a, 0x02},
+	{OV5693_8BIT, 0x309b, 0x01},
+	{OV5693_8BIT, 0x309c, 0x00},
+	{OV5693_8BIT, 0x30a0, 0xd2},
+	{OV5693_8BIT, 0x30a2, 0x01},
+	{OV5693_8BIT, 0x30b2, 0x00},
+	{OV5693_8BIT, 0x30b3, 0x7d},
+	{OV5693_8BIT, 0x30b4, 0x03},
+	{OV5693_8BIT, 0x30b5, 0x04},
+	{OV5693_8BIT, 0x30b6, 0x01},
+	{OV5693_8BIT, 0x3104, 0x21},
+	{OV5693_8BIT, 0x3106, 0x00},
+	{OV5693_8BIT, 0x3400, 0x04},
+	{OV5693_8BIT, 0x3401, 0x00},
+	{OV5693_8BIT, 0x3402, 0x04},
+	{OV5693_8BIT, 0x3403, 0x00},
+	{OV5693_8BIT, 0x3404, 0x04},
+	{OV5693_8BIT, 0x3405, 0x00},
+	{OV5693_8BIT, 0x3406, 0x01},
+	{OV5693_8BIT, 0x3500, 0x00},
+	{OV5693_8BIT, 0x3503, 0x07},
+	{OV5693_8BIT, 0x3504, 0x00},
+	{OV5693_8BIT, 0x3505, 0x00},
+	{OV5693_8BIT, 0x3506, 0x00},
+	{OV5693_8BIT, 0x3507, 0x02},
+	{OV5693_8BIT, 0x3508, 0x00},
+	{OV5693_8BIT, 0x3509, 0x10},
+	{OV5693_8BIT, 0x350a, 0x00},
+	{OV5693_8BIT, 0x350b, 0x40},
+	{OV5693_8BIT, 0x3601, 0x0a},
+	{OV5693_8BIT, 0x3602, 0x38},
+	{OV5693_8BIT, 0x3612, 0x80},
+	{OV5693_8BIT, 0x3620, 0x54},
+	{OV5693_8BIT, 0x3621, 0xc7},
+	{OV5693_8BIT, 0x3622, 0x0f},
+	{OV5693_8BIT, 0x3625, 0x10},
+	{OV5693_8BIT, 0x3630, 0x55},
+	{OV5693_8BIT, 0x3631, 0xf4},
+	{OV5693_8BIT, 0x3632, 0x00},
+	{OV5693_8BIT, 0x3633, 0x34},
+	{OV5693_8BIT, 0x3634, 0x02},
+	{OV5693_8BIT, 0x364d, 0x0d},
+	{OV5693_8BIT, 0x364f, 0xdd},
+	{OV5693_8BIT, 0x3660, 0x04},
+	{OV5693_8BIT, 0x3662, 0x10},
+	{OV5693_8BIT, 0x3663, 0xf1},
+	{OV5693_8BIT, 0x3665, 0x00},
+	{OV5693_8BIT, 0x3666, 0x20},
+	{OV5693_8BIT, 0x3667, 0x00},
+	{OV5693_8BIT, 0x366a, 0x80},
+	{OV5693_8BIT, 0x3680, 0xe0},
+	{OV5693_8BIT, 0x3681, 0x00},
+	{OV5693_8BIT, 0x3700, 0x42},
+	{OV5693_8BIT, 0x3701, 0x14},
+	{OV5693_8BIT, 0x3702, 0xa0},
+	{OV5693_8BIT, 0x3703, 0xd8},
+	{OV5693_8BIT, 0x3704, 0x78},
+	{OV5693_8BIT, 0x3705, 0x02},
+	{OV5693_8BIT, 0x370a, 0x00},
+	{OV5693_8BIT, 0x370b, 0x20},
+	{OV5693_8BIT, 0x370c, 0x0c},
+	{OV5693_8BIT, 0x370d, 0x11},
+	{OV5693_8BIT, 0x370e, 0x00},
+	{OV5693_8BIT, 0x370f, 0x40},
+	{OV5693_8BIT, 0x3710, 0x00},
+	{OV5693_8BIT, 0x371a, 0x1c},
+	{OV5693_8BIT, 0x371b, 0x05},
+	{OV5693_8BIT, 0x371c, 0x01},
+	{OV5693_8BIT, 0x371e, 0xa1},
+	{OV5693_8BIT, 0x371f, 0x0c},
+	{OV5693_8BIT, 0x3721, 0x00},
+	{OV5693_8BIT, 0x3724, 0x10},
+	{OV5693_8BIT, 0x3726, 0x00},
+	{OV5693_8BIT, 0x372a, 0x01},
+	{OV5693_8BIT, 0x3730, 0x10},
+	{OV5693_8BIT, 0x3738, 0x22},
+	{OV5693_8BIT, 0x3739, 0xe5},
+	{OV5693_8BIT, 0x373a, 0x50},
+	{OV5693_8BIT, 0x373b, 0x02},
+	{OV5693_8BIT, 0x373c, 0x41},
+	{OV5693_8BIT, 0x373f, 0x02},
+	{OV5693_8BIT, 0x3740, 0x42},
+	{OV5693_8BIT, 0x3741, 0x02},
+	{OV5693_8BIT, 0x3742, 0x18},
+	{OV5693_8BIT, 0x3743, 0x01},
+	{OV5693_8BIT, 0x3744, 0x02},
+	{OV5693_8BIT, 0x3747, 0x10},
+	{OV5693_8BIT, 0x374c, 0x04},
+	{OV5693_8BIT, 0x3751, 0xf0},
+	{OV5693_8BIT, 0x3752, 0x00},
+	{OV5693_8BIT, 0x3753, 0x00},
+	{OV5693_8BIT, 0x3754, 0xc0},
+	{OV5693_8BIT, 0x3755, 0x00},
+	{OV5693_8BIT, 0x3756, 0x1a},
+	{OV5693_8BIT, 0x3758, 0x00},
+	{OV5693_8BIT, 0x3759, 0x0f},
+	{OV5693_8BIT, 0x376b, 0x44},
+	{OV5693_8BIT, 0x375c, 0x04},
+	{OV5693_8BIT, 0x3774, 0x10},
+	{OV5693_8BIT, 0x3776, 0x00},
+	{OV5693_8BIT, 0x377f, 0x08},
+	{OV5693_8BIT, 0x3780, 0x22},
+	{OV5693_8BIT, 0x3781, 0x0c},
+	{OV5693_8BIT, 0x3784, 0x2c},
+	{OV5693_8BIT, 0x3785, 0x1e},
+	{OV5693_8BIT, 0x378f, 0xf5},
+	{OV5693_8BIT, 0x3791, 0xb0},
+	{OV5693_8BIT, 0x3795, 0x00},
+	{OV5693_8BIT, 0x3796, 0x64},
+	{OV5693_8BIT, 0x3797, 0x11},
+	{OV5693_8BIT, 0x3798, 0x30},
+	{OV5693_8BIT, 0x3799, 0x41},
+	{OV5693_8BIT, 0x379a, 0x07},
+	{OV5693_8BIT, 0x379b, 0xb0},
+	{OV5693_8BIT, 0x379c, 0x0c},
+	{OV5693_8BIT, 0x37c5, 0x00},
+	{OV5693_8BIT, 0x37c6, 0x00},
+	{OV5693_8BIT, 0x37c7, 0x00},
+	{OV5693_8BIT, 0x37c9, 0x00},
+	{OV5693_8BIT, 0x37ca, 0x00},
+	{OV5693_8BIT, 0x37cb, 0x00},
+	{OV5693_8BIT, 0x37de, 0x00},
+	{OV5693_8BIT, 0x37df, 0x00},
+	{OV5693_8BIT, 0x3800, 0x00},
+	{OV5693_8BIT, 0x3801, 0x00},
+	{OV5693_8BIT, 0x3802, 0x00},
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3810, 0x00},
+	{OV5693_8BIT, 0x3812, 0x00},
+	{OV5693_8BIT, 0x3823, 0x00},
+	{OV5693_8BIT, 0x3824, 0x00},
+	{OV5693_8BIT, 0x3825, 0x00},
+	{OV5693_8BIT, 0x3826, 0x00},
+	{OV5693_8BIT, 0x3827, 0x00},
+	{OV5693_8BIT, 0x382a, 0x04},
+	{OV5693_8BIT, 0x3a04, 0x06},
+	{OV5693_8BIT, 0x3a05, 0x14},
+	{OV5693_8BIT, 0x3a06, 0x00},
+	{OV5693_8BIT, 0x3a07, 0xfe},
+	{OV5693_8BIT, 0x3b00, 0x00},
+	{OV5693_8BIT, 0x3b02, 0x00},
+	{OV5693_8BIT, 0x3b03, 0x00},
+	{OV5693_8BIT, 0x3b04, 0x00},
+	{OV5693_8BIT, 0x3b05, 0x00},
+	{OV5693_8BIT, 0x3e07, 0x20},
+	{OV5693_8BIT, 0x4000, 0x08},
+	{OV5693_8BIT, 0x4001, 0x04},
+	{OV5693_8BIT, 0x4002, 0x45},
+	{OV5693_8BIT, 0x4004, 0x08},
+	{OV5693_8BIT, 0x4005, 0x18},
+	{OV5693_8BIT, 0x4006, 0x20},
+	{OV5693_8BIT, 0x4008, 0x24},
+	{OV5693_8BIT, 0x4009, 0x10},
+	{OV5693_8BIT, 0x400c, 0x00},
+	{OV5693_8BIT, 0x400d, 0x00},
+	{OV5693_8BIT, 0x4058, 0x00},
+	{OV5693_8BIT, 0x404e, 0x37},
+	{OV5693_8BIT, 0x404f, 0x8f},
+	{OV5693_8BIT, 0x4058, 0x00},
+	{OV5693_8BIT, 0x4101, 0xb2},
+	{OV5693_8BIT, 0x4303, 0x00},
+	{OV5693_8BIT, 0x4304, 0x08},
+	{OV5693_8BIT, 0x4307, 0x31},
+	{OV5693_8BIT, 0x4311, 0x04},
+	{OV5693_8BIT, 0x4315, 0x01},
+	{OV5693_8BIT, 0x4511, 0x05},
+	{OV5693_8BIT, 0x4512, 0x01},
+	{OV5693_8BIT, 0x4806, 0x00},
+	{OV5693_8BIT, 0x4816, 0x52},
+	{OV5693_8BIT, 0x481f, 0x30},
+	{OV5693_8BIT, 0x4826, 0x2c},
+	{OV5693_8BIT, 0x4831, 0x64},
+	{OV5693_8BIT, 0x4d00, 0x04},
+	{OV5693_8BIT, 0x4d01, 0x71},
+	{OV5693_8BIT, 0x4d02, 0xfd},
+	{OV5693_8BIT, 0x4d03, 0xf5},
+	{OV5693_8BIT, 0x4d04, 0x0c},
+	{OV5693_8BIT, 0x4d05, 0xcc},
+	{OV5693_8BIT, 0x4837, 0x0a},
+	{OV5693_8BIT, 0x5000, 0x06},
+	{OV5693_8BIT, 0x5001, 0x01},
+	{OV5693_8BIT, 0x5003, 0x20},
+	{OV5693_8BIT, 0x5046, 0x0a},
+	{OV5693_8BIT, 0x5013, 0x00},
+	{OV5693_8BIT, 0x5046, 0x0a},
+	{OV5693_8BIT, 0x5780, 0x1c},
+	{OV5693_8BIT, 0x5786, 0x20},
+	{OV5693_8BIT, 0x5787, 0x10},
+	{OV5693_8BIT, 0x5788, 0x18},
+	{OV5693_8BIT, 0x578a, 0x04},
+	{OV5693_8BIT, 0x578b, 0x02},
+	{OV5693_8BIT, 0x578c, 0x02},
+	{OV5693_8BIT, 0x578e, 0x06},
+	{OV5693_8BIT, 0x578f, 0x02},
+	{OV5693_8BIT, 0x5790, 0x02},
+	{OV5693_8BIT, 0x5791, 0xff},
+	{OV5693_8BIT, 0x5842, 0x01},
+	{OV5693_8BIT, 0x5843, 0x2b},
+	{OV5693_8BIT, 0x5844, 0x01},
+	{OV5693_8BIT, 0x5845, 0x92},
+	{OV5693_8BIT, 0x5846, 0x01},
+	{OV5693_8BIT, 0x5847, 0x8f},
+	{OV5693_8BIT, 0x5848, 0x01},
+	{OV5693_8BIT, 0x5849, 0x0c},
+	{OV5693_8BIT, 0x5e00, 0x00},
+	{OV5693_8BIT, 0x5e10, 0x0c},
+	{OV5693_8BIT, 0x0100, 0x00},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+#if ENABLE_NON_PREVIEW
+/*
+ * 654x496 30fps 17ms VBlanking 2lane 10Bit (Scaling)
+ */
+static struct ov5693_reg const ov5693_654x496[] = {
+	{OV5693_8BIT, 0x3501, 0x3d},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe6},
+	{OV5693_8BIT, 0x3709, 0xc7},
+	{OV5693_8BIT, 0x3803, 0x00},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xa3},
+	{OV5693_8BIT, 0x3808, 0x02},
+	{OV5693_8BIT, 0x3809, 0x90},
+	{OV5693_8BIT, 0x380a, 0x01},
+	{OV5693_8BIT, 0x380b, 0xf0},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x08},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x04},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*
+ * 1296x976 30fps 17ms VBlanking 2lane 10Bit (Scaling)
+*DS from 2592x1952
+*/
+static struct ov5693_reg const ov5693_1296x976[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+
+	{OV5693_8BIT, 0x3800, 0x00},
+	{OV5693_8BIT, 0x3801, 0x00},
+	{OV5693_8BIT, 0x3802, 0x00},
+	{OV5693_8BIT, 0x3803, 0x00},
+
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xA3},
+
+	{OV5693_8BIT, 0x3808, 0x05},
+	{OV5693_8BIT, 0x3809, 0x10},
+	{OV5693_8BIT, 0x380a, 0x03},
+	{OV5693_8BIT, 0x380b, 0xD0},
+
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+
+	{OV5693_8BIT, 0x3810, 0x00},
+	{OV5693_8BIT, 0x3811, 0x10},
+	{OV5693_8BIT, 0x3812, 0x00},
+	{OV5693_8BIT, 0x3813, 0x02},
+
+	{OV5693_8BIT, 0x3814, 0x11},	/*X subsample control*/
+	{OV5693_8BIT, 0x3815, 0x11},	/*Y subsample control*/
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x5041, 0x84}, /* scale is auto enabled */
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+
+};
+
+/*
+ * 336x256 30fps 17ms VBlanking 2lane 10Bit (Scaling)
+ DS from 2564x1956
+ */
+static struct ov5693_reg const ov5693_336x256[] = {
+	{OV5693_8BIT, 0x3501, 0x3d},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe6},
+	{OV5693_8BIT, 0x3709, 0xc7},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xa3},
+	{OV5693_8BIT, 0x3808, 0x01},
+	{OV5693_8BIT, 0x3809, 0x50},
+	{OV5693_8BIT, 0x380a, 0x01},
+	{OV5693_8BIT, 0x380b, 0x00},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x1E},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x04},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*
+ * 336x256 30fps 17ms VBlanking 2lane 10Bit (Scaling)
+ DS from 2368x1956
+ */
+static struct ov5693_reg const ov5693_368x304[] = {
+	{OV5693_8BIT, 0x3501, 0x3d},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe6},
+	{OV5693_8BIT, 0x3709, 0xc7},
+	{OV5693_8BIT, 0x3808, 0x01},
+	{OV5693_8BIT, 0x3809, 0x70},
+	{OV5693_8BIT, 0x380a, 0x01},
+	{OV5693_8BIT, 0x380b, 0x30},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x80},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x04},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*
+ * ov5693_192x160 30fps 17ms VBlanking 2lane 10Bit (Scaling)
+ DS from 2460x1956
+ */
+static struct ov5693_reg const ov5693_192x160[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x80},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xA3},
+	{OV5693_8BIT, 0x3808, 0x00},
+	{OV5693_8BIT, 0x3809, 0xC0},
+	{OV5693_8BIT, 0x380a, 0x00},
+	{OV5693_8BIT, 0x380b, 0xA0},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x40},
+	{OV5693_8BIT, 0x3813, 0x00},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x04},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+static struct ov5693_reg const ov5693_736x496[] = {
+	{OV5693_8BIT, 0x3501, 0x3d},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe6},
+	{OV5693_8BIT, 0x3709, 0xc7},
+	{OV5693_8BIT, 0x3803, 0x68},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0x3b},
+	{OV5693_8BIT, 0x3808, 0x02},
+	{OV5693_8BIT, 0x3809, 0xe0},
+	{OV5693_8BIT, 0x380a, 0x01},
+	{OV5693_8BIT, 0x380b, 0xf0},
+	{OV5693_8BIT, 0x380c, 0x0a}, /*hts*/
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07}, /*vts*/
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x08},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x04},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+#endif
+
+/*
+static struct ov5693_reg const ov5693_736x496[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe6},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0x00},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xa3},
+	{OV5693_8BIT, 0x3808, 0x02},
+	{OV5693_8BIT, 0x3809, 0xe0},
+	{OV5693_8BIT, 0x380a, 0x01},
+	{OV5693_8BIT, 0x380b, 0xf0},
+	{OV5693_8BIT, 0x380c, 0x0d},
+	{OV5693_8BIT, 0x380d, 0xb0},
+	{OV5693_8BIT, 0x380e, 0x05},
+	{OV5693_8BIT, 0x380f, 0xf2},
+	{OV5693_8BIT, 0x3811, 0x08},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x31},
+	{OV5693_8BIT, 0x3815, 0x31},
+	{OV5693_8BIT, 0x3820, 0x01},
+	{OV5693_8BIT, 0x3821, 0x1f},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+*/
+/*
+ * 976x556 30fps 8.8ms VBlanking 2lane 10Bit (Scaling)
+ */
+#if ENABLE_NON_PREVIEW
+static struct ov5693_reg const ov5693_976x556[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0xf0},
+	{OV5693_8BIT, 0x3806, 0x06},
+	{OV5693_8BIT, 0x3807, 0xa7},
+	{OV5693_8BIT, 0x3808, 0x03},
+	{OV5693_8BIT, 0x3809, 0xd0},
+	{OV5693_8BIT, 0x380a, 0x02},
+	{OV5693_8BIT, 0x380b, 0x2C},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x10},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*DS from 2624x1492*/
+static struct ov5693_reg const ov5693_1296x736[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+
+	{OV5693_8BIT, 0x3800, 0x00},
+	{OV5693_8BIT, 0x3801, 0x00},
+	{OV5693_8BIT, 0x3802, 0x00},
+	{OV5693_8BIT, 0x3803, 0x00},
+
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xA3},
+
+	{OV5693_8BIT, 0x3808, 0x05},
+	{OV5693_8BIT, 0x3809, 0x10},
+	{OV5693_8BIT, 0x380a, 0x02},
+	{OV5693_8BIT, 0x380b, 0xe0},
+
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+
+	{OV5693_8BIT, 0x3813, 0xE8},
+
+	{OV5693_8BIT, 0x3814, 0x11},	/*X subsample control*/
+	{OV5693_8BIT, 0x3815, 0x11},	/*Y subsample control*/
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x5041, 0x84}, /* scale is auto enabled */
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+static struct ov5693_reg const ov5693_1636p_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0xf0},
+	{OV5693_8BIT, 0x3806, 0x06},
+	{OV5693_8BIT, 0x3807, 0xa7},
+	{OV5693_8BIT, 0x3808, 0x06},
+	{OV5693_8BIT, 0x3809, 0x64},
+	{OV5693_8BIT, 0x380a, 0x04},
+	{OV5693_8BIT, 0x380b, 0x48},
+	{OV5693_8BIT, 0x380c, 0x0a}, /*hts*/
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07}, /*vts*/
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x02},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct ov5693_reg const ov5693_1616x1216_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x80},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3800, 0x00},	/*{3800,3801} Array X start*/
+	{OV5693_8BIT, 0x3801, 0x08},	/* 04 //{3800,3801} Array X start*/
+	{OV5693_8BIT, 0x3802, 0x00},	/*{3802,3803} Array Y start*/
+	{OV5693_8BIT, 0x3803, 0x04},	/* 00  //{3802,3803} Array Y start*/
+	{OV5693_8BIT, 0x3804, 0x0a},	/*{3804,3805} Array X end*/
+	{OV5693_8BIT, 0x3805, 0x37},	/* 3b  //{3804,3805} Array X end*/
+	{OV5693_8BIT, 0x3806, 0x07},	/*{3806,3807} Array Y end*/
+	{OV5693_8BIT, 0x3807, 0x9f},	/* a3  //{3806,3807} Array Y end*/
+	{OV5693_8BIT, 0x3808, 0x06},	/*{3808,3809} Final output H size*/
+	{OV5693_8BIT, 0x3809, 0x50},	/*{3808,3809} Final output H size*/
+	{OV5693_8BIT, 0x380a, 0x04},	/*{380a,380b} Final output V size*/
+	{OV5693_8BIT, 0x380b, 0xc0},	/*{380a,380b} Final output V size*/
+	{OV5693_8BIT, 0x380c, 0x0a},	/*{380c,380d} HTS*/
+	{OV5693_8BIT, 0x380d, 0x80},	/*{380c,380d} HTS*/
+	{OV5693_8BIT, 0x380e, 0x07},	/*{380e,380f} VTS*/
+	{OV5693_8BIT, 0x380f, 0xc0},	/* bc	//{380e,380f} VTS*/
+	{OV5693_8BIT, 0x3810, 0x00},	/*{3810,3811} windowing X offset*/
+	{OV5693_8BIT, 0x3811, 0x10},	/*{3810,3811} windowing X offset*/
+	{OV5693_8BIT, 0x3812, 0x00},	/*{3812,3813} windowing Y offset*/
+	{OV5693_8BIT, 0x3813, 0x06},	/*{3812,3813} windowing Y offset*/
+	{OV5693_8BIT, 0x3814, 0x11},	/*X subsample control*/
+	{OV5693_8BIT, 0x3815, 0x11},	/*Y subsample control*/
+	{OV5693_8BIT, 0x3820, 0x00},	/*FLIP/Binning control*/
+	{OV5693_8BIT, 0x3821, 0x1e},	/*MIRROR control*/
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x5041, 0x84},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*
+ * 1940x1096 30fps 8.8ms VBlanking 2lane 10bit (Scaling)
+ */
+#if ENABLE_NON_PREVIEW
+static struct ov5693_reg const ov5693_1940x1096[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0xf0},
+	{OV5693_8BIT, 0x3806, 0x06},
+	{OV5693_8BIT, 0x3807, 0xa7},
+	{OV5693_8BIT, 0x3808, 0x07},
+	{OV5693_8BIT, 0x3809, 0x94},
+	{OV5693_8BIT, 0x380a, 0x04},
+	{OV5693_8BIT, 0x380b, 0x48},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x02},
+	{OV5693_8BIT, 0x3813, 0x02},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x80},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+static struct ov5693_reg const ov5693_2592x1456_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3800, 0x00},
+	{OV5693_8BIT, 0x3801, 0x00},
+	{OV5693_8BIT, 0x3802, 0x00},
+	{OV5693_8BIT, 0x3803, 0xf0},
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3806, 0x06},
+	{OV5693_8BIT, 0x3807, 0xa4},
+	{OV5693_8BIT, 0x3808, 0x0a},
+	{OV5693_8BIT, 0x3809, 0x20},
+	{OV5693_8BIT, 0x380a, 0x05},
+	{OV5693_8BIT, 0x380b, 0xb0},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x10},
+	{OV5693_8BIT, 0x3813, 0x00},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_TOK_TERM, 0, 0}
+};
+#endif
+
+static struct ov5693_reg const ov5693_2576x1456_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3800, 0x00},
+	{OV5693_8BIT, 0x3801, 0x00},
+	{OV5693_8BIT, 0x3802, 0x00},
+	{OV5693_8BIT, 0x3803, 0xf0},
+	{OV5693_8BIT, 0x3804, 0x0a},
+	{OV5693_8BIT, 0x3805, 0x3f},
+	{OV5693_8BIT, 0x3806, 0x06},
+	{OV5693_8BIT, 0x3807, 0xa4},
+	{OV5693_8BIT, 0x3808, 0x0a},
+	{OV5693_8BIT, 0x3809, 0x10},
+	{OV5693_8BIT, 0x380a, 0x05},
+	{OV5693_8BIT, 0x380b, 0xb0},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x18},
+	{OV5693_8BIT, 0x3813, 0x00},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+/*
+ * 2592x1944 30fps 0.6ms VBlanking 2lane 10Bit
+ */
+#if ENABLE_NON_PREVIEW
+static struct ov5693_reg const ov5693_2592x1944_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0x00},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xa3},
+	{OV5693_8BIT, 0x3808, 0x0a},
+	{OV5693_8BIT, 0x3809, 0x20},
+	{OV5693_8BIT, 0x380a, 0x07},
+	{OV5693_8BIT, 0x380b, 0x98},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x10},
+	{OV5693_8BIT, 0x3813, 0x00},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+#endif
+
+/*
+ * 11:9 Full FOV Output, expected FOV Res: 2346x1920
+ * ISP Effect Res: 1408x1152
+ * Sensor out: 1424x1168, DS From: 2380x1952
+ *
+ * WA: Left Offset: 8, Hor scal: 64
+ */
+#if ENABLE_NON_PREVIEW
+static struct ov5693_reg const ov5693_1424x1168_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x3b}, /* long exposure[15:8] */
+	{OV5693_8BIT, 0x3502, 0x80}, /* long exposure[7:0] */
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3800, 0x00}, /* TIMING_X_ADDR_START */
+	{OV5693_8BIT, 0x3801, 0x50}, /* 80 */
+	{OV5693_8BIT, 0x3802, 0x00}, /* TIMING_Y_ADDR_START */
+	{OV5693_8BIT, 0x3803, 0x02}, /* 2 */
+	{OV5693_8BIT, 0x3804, 0x09}, /* TIMING_X_ADDR_END */
+	{OV5693_8BIT, 0x3805, 0xdd}, /* 2525 */
+	{OV5693_8BIT, 0x3806, 0x07}, /* TIMING_Y_ADDR_END */
+	{OV5693_8BIT, 0x3807, 0xa1}, /* 1953 */
+	{OV5693_8BIT, 0x3808, 0x05}, /* TIMING_X_OUTPUT_SIZE */
+	{OV5693_8BIT, 0x3809, 0x90}, /* 1424 */
+	{OV5693_8BIT, 0x380a, 0x04}, /* TIMING_Y_OUTPUT_SIZE */
+	{OV5693_8BIT, 0x380b, 0x90}, /* 1168 */
+	{OV5693_8BIT, 0x380c, 0x0a}, /* TIMING_HTS */
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07}, /* TIMING_VTS */
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3810, 0x00}, /* TIMING_ISP_X_WIN */
+	{OV5693_8BIT, 0x3811, 0x02}, /* 2 */
+	{OV5693_8BIT, 0x3812, 0x00}, /* TIMING_ISP_Y_WIN */
+	{OV5693_8BIT, 0x3813, 0x00}, /* 0 */
+	{OV5693_8BIT, 0x3814, 0x11}, /* TIME_X_INC */
+	{OV5693_8BIT, 0x3815, 0x11}, /* TIME_Y_INC */
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x5041, 0x84}, /* scale is auto enabled */
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+#endif
+
+/*
+ * 3:2 Full FOV Output, expected FOV Res: 2560x1706
+ * ISP Effect Res: 720x480
+ * Sensor out: 736x496, DS From 2616x1764
+ */
+static struct ov5693_reg const ov5693_736x496_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x3b}, /* long exposure[15:8] */
+	{OV5693_8BIT, 0x3502, 0x80}, /* long exposure[7:0] */
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3800, 0x00}, /* TIMING_X_ADDR_START */
+	{OV5693_8BIT, 0x3801, 0x02}, /* 2 */
+	{OV5693_8BIT, 0x3802, 0x00}, /* TIMING_Y_ADDR_START */
+	{OV5693_8BIT, 0x3803, 0x62}, /* 98 */
+	{OV5693_8BIT, 0x3804, 0x0a}, /* TIMING_X_ADDR_END */
+	{OV5693_8BIT, 0x3805, 0x3b}, /* 2619 */
+	{OV5693_8BIT, 0x3806, 0x07}, /* TIMING_Y_ADDR_END */
+	{OV5693_8BIT, 0x3807, 0x43}, /* 1859 */
+	{OV5693_8BIT, 0x3808, 0x02}, /* TIMING_X_OUTPUT_SIZE */
+	{OV5693_8BIT, 0x3809, 0xe0}, /* 736 */
+	{OV5693_8BIT, 0x380a, 0x01}, /* TIMING_Y_OUTPUT_SIZE */
+	{OV5693_8BIT, 0x380b, 0xf0}, /* 496 */
+	{OV5693_8BIT, 0x380c, 0x0a}, /* TIMING_HTS */
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07}, /* TIMING_VTS */
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3810, 0x00}, /* TIMING_ISP_X_WIN */
+	{OV5693_8BIT, 0x3811, 0x02}, /* 2 */
+	{OV5693_8BIT, 0x3812, 0x00}, /* TIMING_ISP_Y_WIN */
+	{OV5693_8BIT, 0x3813, 0x00}, /* 0 */
+	{OV5693_8BIT, 0x3814, 0x11}, /* TIME_X_INC */
+	{OV5693_8BIT, 0x3815, 0x11}, /* TIME_Y_INC */
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x5041, 0x84}, /* scale is auto enabled */
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+static struct ov5693_reg const ov5693_2576x1936_30fps[] = {
+	{OV5693_8BIT, 0x3501, 0x7b},
+	{OV5693_8BIT, 0x3502, 0x00},
+	{OV5693_8BIT, 0x3708, 0xe2},
+	{OV5693_8BIT, 0x3709, 0xc3},
+	{OV5693_8BIT, 0x3803, 0x00},
+	{OV5693_8BIT, 0x3806, 0x07},
+	{OV5693_8BIT, 0x3807, 0xa3},
+	{OV5693_8BIT, 0x3808, 0x0a},
+	{OV5693_8BIT, 0x3809, 0x10},
+	{OV5693_8BIT, 0x380a, 0x07},
+	{OV5693_8BIT, 0x380b, 0x90},
+	{OV5693_8BIT, 0x380c, 0x0a},
+	{OV5693_8BIT, 0x380d, 0x80},
+	{OV5693_8BIT, 0x380e, 0x07},
+	{OV5693_8BIT, 0x380f, 0xc0},
+	{OV5693_8BIT, 0x3811, 0x18},
+	{OV5693_8BIT, 0x3813, 0x00},
+	{OV5693_8BIT, 0x3814, 0x11},
+	{OV5693_8BIT, 0x3815, 0x11},
+	{OV5693_8BIT, 0x3820, 0x00},
+	{OV5693_8BIT, 0x3821, 0x1e},
+	{OV5693_8BIT, 0x5002, 0x00},
+	{OV5693_8BIT, 0x0100, 0x01},
+	{OV5693_TOK_TERM, 0, 0}
+};
+
+static struct ov5693_resolution ov5693_res_preview[] = {
+	{
+		.desc = "ov5693_736x496_30fps",
+		.width = 736,
+		.height = 496,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_736x496_30fps,
+	},
+	{
+		.desc = "ov5693_1616x1216_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_1616x1216_30fps,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2576,
+		.height = 1456,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2576x1456_30fps,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2576,
+		.height = 1936,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2576x1936_30fps,
+	},
+};
+
+#define N_RES_PREVIEW (ARRAY_SIZE(ov5693_res_preview))
+
+/*
+ * Disable non-preview configurations until the configuration selection is
+ * improved.
+ */
+#if ENABLE_NON_PREVIEW
+struct ov5693_resolution ov5693_res_still[] = {
+	{
+		.desc = "ov5693_736x496_30fps",
+		.width = 736,
+		.height = 496,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_736x496_30fps,
+	},
+	{
+		.desc = "ov5693_1424x1168_30fps",
+		.width = 1424,
+		.height = 1168,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_1424x1168_30fps,
+	},
+	{
+		.desc = "ov5693_1616x1216_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_1616x1216_30fps,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2592,
+		.height = 1456,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2592x1456_30fps,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2592,
+		.height = 1944,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2592x1944_30fps,
+	},
+};
+
+#define N_RES_STILL (ARRAY_SIZE(ov5693_res_still))
+
+struct ov5693_resolution ov5693_res_video[] = {
+	{
+		.desc = "ov5693_736x496_30fps",
+		.width = 736,
+		.height = 496,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 1,
+		.regs = ov5693_736x496,
+	},
+	{
+		.desc = "ov5693_336x256_30fps",
+		.width = 336,
+		.height = 256,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 1,
+		.regs = ov5693_336x256,
+	},
+	{
+		.desc = "ov5693_368x304_30fps",
+		.width = 368,
+		.height = 304,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 1,
+		.regs = ov5693_368x304,
+	},
+	{
+		.desc = "ov5693_192x160_30fps",
+		.width = 192,
+		.height = 160,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 1,
+		.regs = ov5693_192x160,
+	},
+	{
+		.desc = "ov5693_1296x736_30fps",
+		.width = 1296,
+		.height = 736,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 0,
+		.regs = ov5693_1296x736,
+	},
+	{
+		.desc = "ov5693_1296x976_30fps",
+		.width = 1296,
+		.height = 976,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.bin_mode = 0,
+		.regs = ov5693_1296x976,
+	},
+	{
+		.desc = "ov5693_1636P_30fps",
+		.width = 1636,
+		.height = 1096,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_1636p_30fps,
+	},
+	{
+		.desc = "ov5693_1080P_30fps",
+		.width = 1940,
+		.height = 1096,
+		.fps = 30,
+		.pix_clk_freq = 160,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_1940x1096,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2592,
+		.height = 1456,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2592x1456_30fps,
+	},
+	{
+		.desc = "ov5693_5M_30fps",
+		.width = 2592,
+		.height = 1944,
+		.pix_clk_freq = 160,
+		.fps = 30,
+		.used = 0,
+		.pixels_per_line = 2688,
+		.lines_per_frame = 1984,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.bin_mode = 0,
+		.regs = ov5693_2592x1944_30fps,
+	},
+};
+
+#define N_RES_VIDEO (ARRAY_SIZE(ov5693_res_video))
+#endif
+
+static struct ov5693_resolution *ov5693_res = ov5693_res_preview;
+static unsigned long N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/include/hmm/hmm.h b/drivers/staging/media/atomisp/include/hmm/hmm.h
new file mode 100644
index 000000000..c0384bb0a
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/hmm/hmm.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__HMM_H__
+#define	__HMM_H__
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+
+#include "hmm_common.h"
+#include "hmm/hmm_bo.h"
+#include "ia_css_types.h"
+
+#define mmgr_NULL              ((ia_css_ptr)0)
+#define mmgr_EXCEPTION         ((ia_css_ptr) - 1)
+
+int hmm_init(void);
+void hmm_cleanup(void);
+
+ia_css_ptr hmm_alloc(size_t bytes);
+ia_css_ptr hmm_create_from_userdata(size_t bytes, const void __user *userptr);
+void hmm_free(ia_css_ptr ptr);
+int hmm_load(ia_css_ptr virt, void *data, unsigned int bytes);
+int hmm_store(ia_css_ptr virt, const void *data, unsigned int bytes);
+int hmm_set(ia_css_ptr virt, int c, unsigned int bytes);
+int hmm_flush(ia_css_ptr virt, unsigned int bytes);
+
+/*
+ * get kernel memory physical address from ISP virtual address.
+ */
+phys_addr_t hmm_virt_to_phys(ia_css_ptr virt);
+
+/*
+ * map ISP memory starts with virt to kernel virtual address
+ * by using vmap. return NULL if failed.
+ *
+ * virt must be the start address of ISP memory (return by hmm_alloc),
+ * do not pass any other address.
+ */
+void *hmm_vmap(ia_css_ptr virt, bool cached);
+void hmm_vunmap(ia_css_ptr virt);
+
+/*
+ * flush the cache for the vmapped buffer.
+ * if the buffer has not been vmapped, return directly.
+ */
+void hmm_flush_vmap(ia_css_ptr virt);
+
+/*
+ * map ISP memory starts with virt to specific vma.
+ *
+ * used for mmap operation.
+ *
+ * virt must be the start address of ISP memory (return by hmm_alloc),
+ * do not pass any other address.
+ */
+int hmm_mmap(struct vm_area_struct *vma, ia_css_ptr virt);
+
+extern struct hmm_bo_device bo_device;
+
+#endif
diff --git a/drivers/staging/media/atomisp/include/hmm/hmm_bo.h b/drivers/staging/media/atomisp/include/hmm/hmm_bo.h
new file mode 100644
index 000000000..c5cbae1d9
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/hmm/hmm_bo.h
@@ -0,0 +1,272 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__HMM_BO_H__
+#define	__HMM_BO_H__
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include "mmu/isp_mmu.h"
+#include "hmm/hmm_common.h"
+#include "ia_css_types.h"
+
+#define	check_bodev_null_return(bdev, exp)	\
+		check_null_return(bdev, exp, \
+			"NULL hmm_bo_device.\n")
+
+#define	check_bodev_null_return_void(bdev)	\
+		check_null_return_void(bdev, \
+			"NULL hmm_bo_device.\n")
+
+#define	check_bo_status_yes_goto(bo, _status, label) \
+	var_not_equal_goto((bo->status & (_status)), (_status), \
+			label, \
+			"HMM buffer status not contain %s.\n", \
+			#_status)
+
+#define	check_bo_status_no_goto(bo, _status, label) \
+	var_equal_goto((bo->status & (_status)), (_status), \
+			label, \
+			"HMM buffer status contains %s.\n", \
+			#_status)
+
+#define rbtree_node_to_hmm_bo(root_node)	\
+	container_of((root_node), struct hmm_buffer_object, node)
+
+#define	list_to_hmm_bo(list_ptr)	\
+	list_entry((list_ptr), struct hmm_buffer_object, list)
+
+#define	kref_to_hmm_bo(kref_ptr)	\
+	list_entry((kref_ptr), struct hmm_buffer_object, kref)
+
+#define	check_bo_null_return(bo, exp)	\
+	check_null_return(bo, exp, "NULL hmm buffer object.\n")
+
+#define	check_bo_null_return_void(bo)	\
+	check_null_return_void(bo, "NULL hmm buffer object.\n")
+
+#define	ISP_VM_START	0x0
+#define	ISP_VM_SIZE	(0x7FFFFFFF)	/* 2G address space */
+#define	ISP_PTR_NULL	NULL
+
+#define	HMM_BO_DEVICE_INITED	0x1
+
+enum hmm_bo_type {
+	HMM_BO_PRIVATE,
+	HMM_BO_USER,
+	HMM_BO_LAST,
+};
+
+#define	HMM_BO_MASK		0x1
+#define	HMM_BO_FREE		0x0
+#define	HMM_BO_ALLOCED	0x1
+#define	HMM_BO_PAGE_ALLOCED	0x2
+#define	HMM_BO_BINDED		0x4
+#define	HMM_BO_MMAPED		0x8
+#define	HMM_BO_VMAPED		0x10
+#define	HMM_BO_VMAPED_CACHED	0x20
+#define	HMM_BO_ACTIVE		0x1000
+
+struct hmm_bo_device {
+	struct isp_mmu		mmu;
+
+	/* start/pgnr/size is used to record the virtual memory of this bo */
+	unsigned int start;
+	unsigned int pgnr;
+	unsigned int size;
+
+	/* list lock is used to protect the entire_bo_list */
+	spinlock_t	list_lock;
+	int flag;
+
+	/* linked list for entire buffer object */
+	struct list_head entire_bo_list;
+	/* rbtree for maintain entire allocated vm */
+	struct rb_root allocated_rbtree;
+	/* rbtree for maintain entire free vm */
+	struct rb_root free_rbtree;
+	struct mutex rbtree_mutex;
+	struct kmem_cache *bo_cache;
+};
+
+struct hmm_buffer_object {
+	struct hmm_bo_device	*bdev;
+	struct list_head	list;
+	struct kref	kref;
+
+	struct page **pages;
+
+	/* mutex protecting this BO */
+	struct mutex		mutex;
+	enum hmm_bo_type	type;
+	int		mmap_count;
+	int		status;
+	void		*vmap_addr; /* kernel virtual address by vmap */
+
+	struct rb_node	node;
+	unsigned int	start;
+	unsigned int	end;
+	unsigned int	pgnr;
+	/*
+	 * When insert a bo which has the same pgnr with an existed
+	 * bo node in the free_rbtree, using "prev & next" pointer
+	 * to maintain a bo linked list instead of insert this bo
+	 * into free_rbtree directly, it will make sure each node
+	 * in free_rbtree has different pgnr.
+	 * "prev & next" default is NULL.
+	 */
+	struct hmm_buffer_object	*prev;
+	struct hmm_buffer_object	*next;
+};
+
+struct hmm_buffer_object *hmm_bo_alloc(struct hmm_bo_device *bdev,
+				       unsigned int pgnr);
+
+void hmm_bo_release(struct hmm_buffer_object *bo);
+
+int hmm_bo_device_init(struct hmm_bo_device *bdev,
+		       struct isp_mmu_client *mmu_driver,
+		       unsigned int vaddr_start, unsigned int size);
+
+/*
+ * clean up all hmm_bo_device related things.
+ */
+void hmm_bo_device_exit(struct hmm_bo_device *bdev);
+
+/*
+ * whether the bo device is inited or not.
+ */
+int hmm_bo_device_inited(struct hmm_bo_device *bdev);
+
+/*
+ * increse buffer object reference.
+ */
+void hmm_bo_ref(struct hmm_buffer_object *bo);
+
+/*
+ * decrese buffer object reference. if reference reaches 0,
+ * release function of the buffer object will be called.
+ *
+ * this call is also used to release hmm_buffer_object or its
+ * upper level object with it embedded in. you need to call
+ * this function when it is no longer used.
+ *
+ * Note:
+ *
+ * user dont need to care about internal resource release of
+ * the buffer object in the release callback, it will be
+ * handled internally.
+ *
+ * this call will only release internal resource of the buffer
+ * object but will not free the buffer object itself, as the
+ * buffer object can be both pre-allocated statically or
+ * dynamically allocated. so user need to deal with the release
+ * of the buffer object itself manually. below example shows
+ * the normal case of using the buffer object.
+ *
+ *	struct hmm_buffer_object *bo = hmm_bo_create(bdev, pgnr);
+ *	......
+ *	hmm_bo_unref(bo);
+ *
+ * or:
+ *
+ *	struct hmm_buffer_object bo;
+ *
+ *	hmm_bo_init(bdev, &bo, pgnr, NULL);
+ *	...
+ *	hmm_bo_unref(&bo);
+ */
+void hmm_bo_unref(struct hmm_buffer_object *bo);
+
+int hmm_bo_allocated(struct hmm_buffer_object *bo);
+
+/*
+ * Allocate/Free physical pages for the bo. Type indicates if the
+ * pages will be allocated by using video driver (for share buffer)
+ * or by ISP driver itself.
+ */
+int hmm_bo_alloc_pages(struct hmm_buffer_object *bo,
+		       enum hmm_bo_type type,
+		       const void __user *userptr);
+void hmm_bo_free_pages(struct hmm_buffer_object *bo);
+int hmm_bo_page_allocated(struct hmm_buffer_object *bo);
+
+/*
+ * bind/unbind the physical pages to a virtual address space.
+ */
+int hmm_bo_bind(struct hmm_buffer_object *bo);
+void hmm_bo_unbind(struct hmm_buffer_object *bo);
+int hmm_bo_binded(struct hmm_buffer_object *bo);
+
+/*
+ * vmap buffer object's pages to contiguous kernel virtual address.
+ * if the buffer has been vmaped, return the virtual address directly.
+ */
+void *hmm_bo_vmap(struct hmm_buffer_object *bo, bool cached);
+
+/*
+ * flush the cache for the vmapped buffer object's pages,
+ * if the buffer has not been vmapped, return directly.
+ */
+void hmm_bo_flush_vmap(struct hmm_buffer_object *bo);
+
+/*
+ * vunmap buffer object's kernel virtual address.
+ */
+void hmm_bo_vunmap(struct hmm_buffer_object *bo);
+
+/*
+ * mmap the bo's physical pages to specific vma.
+ *
+ * vma's address space size must be the same as bo's size,
+ * otherwise it will return -EINVAL.
+ *
+ * vma->vm_flags will be set to (VM_RESERVED | VM_IO).
+ */
+int hmm_bo_mmap(struct vm_area_struct *vma,
+		struct hmm_buffer_object *bo);
+
+/*
+ * find the buffer object by its virtual address vaddr.
+ * return NULL if no such buffer object found.
+ */
+struct hmm_buffer_object *hmm_bo_device_search_start(
+    struct hmm_bo_device *bdev, ia_css_ptr vaddr);
+
+/*
+ * find the buffer object by its virtual address.
+ * it does not need to be the start address of one bo,
+ * it can be an address within the range of one bo.
+ * return NULL if no such buffer object found.
+ */
+struct hmm_buffer_object *hmm_bo_device_search_in_range(
+    struct hmm_bo_device *bdev, ia_css_ptr vaddr);
+
+/*
+ * find the buffer object with kernel virtual address vaddr.
+ * return NULL if no such buffer object found.
+ */
+struct hmm_buffer_object *hmm_bo_device_search_vmap_start(
+    struct hmm_bo_device *bdev, const void *vaddr);
+
+#endif
diff --git a/drivers/staging/media/atomisp/include/hmm/hmm_common.h b/drivers/staging/media/atomisp/include/hmm/hmm_common.h
new file mode 100644
index 000000000..d8610b135
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/hmm/hmm_common.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__HMM_BO_COMMON_H__
+#define	__HMM_BO_COMMON_H__
+
+#define	HMM_BO_NAME	"HMM"
+
+/*
+ * some common use micros
+ */
+#define	var_equal_return(var1, var2, exp, fmt, arg ...)	\
+	do { \
+		if ((var1) == (var2)) { \
+			dev_err(atomisp_dev, \
+			fmt, ## arg); \
+			return exp;\
+		} \
+	} while (0)
+
+#define	var_equal_return_void(var1, var2, fmt, arg ...)	\
+	do { \
+		if ((var1) == (var2)) { \
+			dev_err(atomisp_dev, \
+			fmt, ## arg); \
+			return;\
+		} \
+	} while (0)
+
+#define	var_equal_goto(var1, var2, label, fmt, arg ...)	\
+	do { \
+		if ((var1) == (var2)) { \
+			dev_err(atomisp_dev, \
+			fmt, ## arg); \
+			goto label;\
+		} \
+	} while (0)
+
+#define	var_not_equal_goto(var1, var2, label, fmt, arg ...)	\
+	do { \
+		if ((var1) != (var2)) { \
+			dev_err(atomisp_dev, \
+			fmt, ## arg); \
+			goto label;\
+		} \
+	} while (0)
+
+#define	check_null_return(ptr, exp, fmt, arg ...)	\
+		var_equal_return(ptr, NULL, exp, fmt, ## arg)
+
+#define	check_null_return_void(ptr, fmt, arg ...)	\
+		var_equal_return_void(ptr, NULL, fmt, ## arg)
+
+#endif
diff --git a/drivers/staging/media/atomisp/include/linux/atomisp.h b/drivers/staging/media/atomisp/include/linux/atomisp.h
new file mode 100644
index 000000000..3f602b5aa
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp.h
@@ -0,0 +1,1199 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef _ATOM_ISP_H
+#define _ATOM_ISP_H
+
+#include <linux/types.h>
+#include <linux/version.h>
+
+/* struct media_device_info.hw_revision */
+#define ATOMISP_HW_REVISION_MASK	0x0000ff00
+#define ATOMISP_HW_REVISION_SHIFT	8
+#define ATOMISP_HW_REVISION_ISP2300	0x00
+#define ATOMISP_HW_REVISION_ISP2400	0x10
+#define ATOMISP_HW_REVISION_ISP2401_LEGACY 0x11
+#define ATOMISP_HW_REVISION_ISP2401	0x20
+
+#define ATOMISP_HW_STEPPING_MASK	0x000000ff
+#define ATOMISP_HW_STEPPING_A0		0x00
+#define ATOMISP_HW_STEPPING_B0		0x10
+
+/*ISP binary running mode*/
+#define CI_MODE_PREVIEW		0x8000
+#define CI_MODE_VIDEO		0x4000
+#define CI_MODE_STILL_CAPTURE	0x2000
+#define CI_MODE_CONTINUOUS	0x1000
+#define CI_MODE_NONE		0x0000
+
+#define OUTPUT_MODE_FILE 0x0100
+#define OUTPUT_MODE_TEXT 0x0200
+
+/*
+ * Camera HAL sets this flag in v4l2_buffer reserved2 to indicate this
+ * buffer has a per-frame parameter.
+ */
+#define ATOMISP_BUFFER_HAS_PER_FRAME_SETTING	0x80000000
+
+/* Custom format for RAW capture from M10MO 0x3130314d */
+#define V4L2_PIX_FMT_CUSTOM_M10MO_RAW	v4l2_fourcc('M', '1', '0', '1')
+
+/* Custom media bus formats being used in atomisp */
+#define V4L2_MBUS_FMT_CUSTOM_YUV420	0x8001
+#define V4L2_MBUS_FMT_CUSTOM_YVU420	0x8002
+#define V4L2_MBUS_FMT_CUSTOM_YUV422P	0x8003
+#define V4L2_MBUS_FMT_CUSTOM_YUV444	0x8004
+#define V4L2_MBUS_FMT_CUSTOM_NV12	0x8005
+#define V4L2_MBUS_FMT_CUSTOM_NV21	0x8006
+#define V4L2_MBUS_FMT_CUSTOM_NV16	0x8007
+#define V4L2_MBUS_FMT_CUSTOM_YUYV	0x8008
+#define V4L2_MBUS_FMT_CUSTOM_SBGGR16	0x8009
+#define V4L2_MBUS_FMT_CUSTOM_RGB32	0x800a
+
+/* Custom media bus format for M10MO RAW capture */
+#if 0
+#define V4L2_MBUS_FMT_CUSTOM_M10MO_RAW	0x800b
+#endif
+
+/* Configuration used by Bayer noise reduction and YCC noise reduction */
+struct atomisp_nr_config {
+	/* [gain] Strength of noise reduction for Bayer NR (Used by Bayer NR) */
+	unsigned int bnr_gain;
+	/* [gain] Strength of noise reduction for YCC NR (Used by YCC NR) */
+	unsigned int ynr_gain;
+	/* [intensity] Sensitivity of Edge (Used by Bayer NR) */
+	unsigned int direction;
+	/* [intensity] coring threshold for Cb (Used by YCC NR) */
+	unsigned int threshold_cb;
+	/* [intensity] coring threshold for Cr (Used by YCC NR) */
+	unsigned int threshold_cr;
+};
+
+/* Temporal noise reduction configuration */
+struct atomisp_tnr_config {
+	unsigned int gain;	 /* [gain] Strength of NR */
+	unsigned int threshold_y;/* [intensity] Motion sensitivity for Y */
+	unsigned int threshold_uv;/* [intensity] Motion sensitivity for U/V */
+};
+
+/* Histogram. This contains num_elements values of type unsigned int.
+ * The data pointer is a DDR pointer (virtual address).
+ */
+struct atomisp_histogram {
+	unsigned int num_elements;
+	void __user *data;
+};
+
+enum atomisp_ob_mode {
+	atomisp_ob_mode_none,
+	atomisp_ob_mode_fixed,
+	atomisp_ob_mode_raster
+};
+
+/* Optical black level configuration */
+struct atomisp_ob_config {
+	/* Obtical black level mode (Fixed / Raster) */
+	enum atomisp_ob_mode mode;
+	/* [intensity] optical black level for GR (relevant for fixed mode) */
+	unsigned int level_gr;
+	/* [intensity] optical black level for R (relevant for fixed mode) */
+	unsigned int level_r;
+	/* [intensity] optical black level for B (relevant for fixed mode) */
+	unsigned int level_b;
+	/* [intensity] optical black level for GB (relevant for fixed mode) */
+	unsigned int level_gb;
+	/* [BQ] 0..63 start position of OB area (relevant for raster mode) */
+	unsigned short start_position;
+	/* [BQ] start..63 end position of OB area (relevant for raster mode) */
+	unsigned short end_position;
+};
+
+/* Edge enhancement (sharpen) configuration */
+struct atomisp_ee_config {
+	/* [gain] The strength of sharpness. u5_11 */
+	unsigned int gain;
+	/* [intensity] The threshold that divides noises from edge. u8_8 */
+	unsigned int threshold;
+	/* [gain] The strength of sharpness in pell-mell area. u5_11 */
+	unsigned int detail_gain;
+};
+
+struct atomisp_3a_output {
+	int ae_y;
+	int awb_cnt;
+	int awb_gr;
+	int awb_r;
+	int awb_b;
+	int awb_gb;
+	int af_hpf1;
+	int af_hpf2;
+};
+
+enum atomisp_calibration_type {
+	calibration_type1,
+	calibration_type2,
+	calibration_type3
+};
+
+struct atomisp_calibration_group {
+	unsigned int size;
+	unsigned int type;
+	unsigned short *calb_grp_values;
+};
+
+struct atomisp_gc_config {
+	__u16 gain_k1;
+	__u16 gain_k2;
+};
+
+struct atomisp_3a_config {
+	unsigned int ae_y_coef_r;	/* [gain] Weight of R for Y */
+	unsigned int ae_y_coef_g;	/* [gain] Weight of G for Y */
+	unsigned int ae_y_coef_b;	/* [gain] Weight of B for Y */
+	unsigned int awb_lg_high_raw;	/* [intensity]
+					   AWB level gate high for raw */
+	unsigned int awb_lg_low;	/* [intensity] AWB level gate low */
+	unsigned int awb_lg_high;	/* [intensity] AWB level gate high */
+	int af_fir1_coef[7];	/* [factor] AF FIR coefficients of fir1 */
+	int af_fir2_coef[7];	/* [factor] AF FIR coefficients of fir2 */
+};
+
+struct atomisp_dvs_grid_info {
+	u32 enable;
+	u32 width;
+	u32 aligned_width;
+	u32 height;
+	u32 aligned_height;
+	u32 bqs_per_grid_cell;
+	u32 num_hor_coefs;
+	u32 num_ver_coefs;
+};
+
+struct atomisp_dvs_envelop {
+	unsigned int width;
+	unsigned int height;
+};
+
+struct atomisp_grid_info {
+	u32 enable;
+	u32 use_dmem;
+	u32 has_histogram;
+	u32 s3a_width;
+	u32 s3a_height;
+	u32 aligned_width;
+	u32 aligned_height;
+	u32 s3a_bqs_per_grid_cell;
+	u32 deci_factor_log2;
+	u32 elem_bit_depth;
+};
+
+struct atomisp_dis_vector {
+	int x;
+	int y;
+};
+
+/* DVS 2.0 Coefficient types. This structure contains 4 pointers to
+ *  arrays that contain the coeffients for each type.
+ */
+struct atomisp_dvs2_coef_types {
+	short __user *odd_real; /** real part of the odd coefficients*/
+	short __user *odd_imag; /** imaginary part of the odd coefficients*/
+	short __user *even_real;/** real part of the even coefficients*/
+	short __user *even_imag;/** imaginary part of the even coefficients*/
+};
+
+/*
+ * DVS 2.0 Statistic types. This structure contains 4 pointers to
+ * arrays that contain the statistics for each type.
+ */
+struct atomisp_dvs2_stat_types {
+	int __user *odd_real; /** real part of the odd statistics*/
+	int __user *odd_imag; /** imaginary part of the odd statistics*/
+	int __user *even_real;/** real part of the even statistics*/
+	int __user *even_imag;/** imaginary part of the even statistics*/
+};
+
+struct atomisp_dis_coefficients {
+	struct atomisp_dvs_grid_info grid_info;
+	struct atomisp_dvs2_coef_types hor_coefs;
+	struct atomisp_dvs2_coef_types ver_coefs;
+};
+
+struct atomisp_dvs2_statistics {
+	struct atomisp_dvs_grid_info grid_info;
+	struct atomisp_dvs2_stat_types hor_prod;
+	struct atomisp_dvs2_stat_types ver_prod;
+};
+
+struct atomisp_dis_statistics {
+	struct atomisp_dvs2_statistics dvs2_stat;
+	u32 exp_id;
+};
+
+struct atomisp_3a_rgby_output {
+	u32 r;
+	u32 g;
+	u32 b;
+	u32 y;
+};
+
+/*
+ * Because we have 2 pipes at max to output metadata, therefore driver will use
+ * ATOMISP_MAIN_METADATA to specify the metadata from the pipe which keeps
+ * streaming always and use ATOMISP_SEC_METADATA to specify the metadata from
+ * the pipe which is streaming by request like capture pipe of ZSL or SDV mode
+ * as secondary metadata. And for the use case which has only one pipe
+ * streaming like online capture, ATOMISP_MAIN_METADATA will be used.
+ */
+enum atomisp_metadata_type {
+	ATOMISP_MAIN_METADATA = 0,
+	ATOMISP_SEC_METADATA,
+	ATOMISP_METADATA_TYPE_NUM,
+};
+
+struct atomisp_metadata_with_type {
+	/* to specify which type of metadata to get */
+	enum atomisp_metadata_type type;
+	void __user *data;
+	u32 width;
+	u32 height;
+	u32 stride; /* in bytes */
+	u32 exp_id; /* exposure ID */
+	u32 *effective_width; /* mipi packets valid data size */
+};
+
+struct atomisp_metadata {
+	void __user *data;
+	u32 width;
+	u32 height;
+	u32 stride; /* in bytes */
+	u32 exp_id; /* exposure ID */
+	u32 *effective_width; /* mipi packets valid data size */
+};
+
+struct atomisp_ext_isp_ctrl {
+	u32 id;
+	u32 data;
+};
+
+struct atomisp_3a_statistics {
+	struct atomisp_grid_info  grid_info;
+	struct atomisp_3a_output __user *data;
+	struct atomisp_3a_rgby_output __user *rgby_data;
+	u32 exp_id; /* exposure ID */
+	u32 isp_config_id; /* isp config ID */
+};
+
+/**
+ * struct atomisp_cont_capture_conf - continuous capture parameters
+ * @num_captures: number of still images to capture
+ * @skip_frames: number of frames to skip between 2 captures
+ * @offset: offset in ring buffer to start capture
+ *
+ * For example, to capture 1 frame from past, current, and 1 from future
+ * and skip one frame between each capture, parameters would be:
+ * num_captures:3
+ * skip_frames:1
+ * offset:-2
+ */
+
+struct atomisp_cont_capture_conf {
+	int num_captures;
+	unsigned int skip_frames;
+	int offset;
+	__u32 reserved[5];
+};
+
+struct atomisp_ae_window {
+	int x_left;
+	int x_right;
+	int y_top;
+	int y_bottom;
+	int weight;
+};
+
+/* White Balance (Gain Adjust) */
+struct atomisp_wb_config {
+	unsigned int integer_bits;
+	unsigned int gr;	/* unsigned <integer_bits>.<16-integer_bits> */
+	unsigned int r;		/* unsigned <integer_bits>.<16-integer_bits> */
+	unsigned int b;		/* unsigned <integer_bits>.<16-integer_bits> */
+	unsigned int gb;	/* unsigned <integer_bits>.<16-integer_bits> */
+};
+
+/* Color Space Conversion settings */
+struct atomisp_cc_config {
+	unsigned int fraction_bits;
+	int matrix[3 * 3];	/* RGB2YUV Color matrix, signed
+				   <13-fraction_bits>.<fraction_bits> */
+};
+
+/* De pixel noise configuration */
+struct atomisp_de_config {
+	unsigned int pixelnoise;
+	unsigned int c1_coring_threshold;
+	unsigned int c2_coring_threshold;
+};
+
+/* Chroma enhancement */
+struct atomisp_ce_config {
+	unsigned char uv_level_min;
+	unsigned char uv_level_max;
+};
+
+/* Defect pixel correction configuration */
+struct atomisp_dp_config {
+	/* [intensity] The threshold of defect Pixel Correction, representing
+	 * the permissible difference of intensity between one pixel and its
+	 * surrounding pixels. Smaller values result in more frequent pixel
+	 * corrections. u0_16
+	 */
+	unsigned int threshold;
+	/* [gain] The sensitivity of mis-correction. ISP will miss a lot of
+	 * defects if the value is set too large. u8_8
+	 */
+	unsigned int gain;
+	unsigned int gr;
+	unsigned int r;
+	unsigned int b;
+	unsigned int gb;
+};
+
+/* XNR threshold */
+struct atomisp_xnr_config {
+	__u16 threshold;
+};
+
+/* metadata config */
+struct atomisp_metadata_config {
+	u32 metadata_height;
+	u32 metadata_stride;
+};
+
+/*
+ * Generic resolution structure.
+ */
+struct atomisp_resolution {
+	u32 width;  /** Width */
+	u32 height; /** Height */
+};
+
+/*
+ * This specifies the coordinates (x,y)
+ */
+struct atomisp_zoom_point {
+	s32 x; /** x coordinate */
+	s32 y; /** y coordinate */
+};
+
+/*
+ * This specifies the region
+ */
+struct atomisp_zoom_region {
+	struct atomisp_zoom_point
+		origin; /* Starting point coordinates for the region */
+	struct atomisp_resolution resolution; /* Region resolution */
+};
+
+struct atomisp_dz_config {
+	u32 dx; /** Horizontal zoom factor */
+	u32 dy; /** Vertical zoom factor */
+	struct atomisp_zoom_region zoom_region; /** region for zoom */
+};
+
+struct atomisp_parm {
+	struct atomisp_grid_info info;
+	struct atomisp_dvs_grid_info dvs_grid;
+	struct atomisp_dvs_envelop dvs_envelop;
+	struct atomisp_wb_config wb_config;
+	struct atomisp_cc_config cc_config;
+	struct atomisp_ob_config ob_config;
+	struct atomisp_de_config de_config;
+	struct atomisp_dz_config dz_config;
+	struct atomisp_ce_config ce_config;
+	struct atomisp_dp_config dp_config;
+	struct atomisp_nr_config nr_config;
+	struct atomisp_ee_config ee_config;
+	struct atomisp_tnr_config tnr_config;
+	struct atomisp_metadata_config metadata_config;
+};
+
+struct dvs2_bq_resolution {
+	int width_bq;         /* width [BQ] */
+	int height_bq;        /* height [BQ] */
+};
+
+struct atomisp_dvs2_bq_resolutions {
+	/* GDC source image size [BQ] */
+	struct dvs2_bq_resolution source_bq;
+	/* GDC output image size [BQ] */
+	struct dvs2_bq_resolution output_bq;
+	/* GDC effective envelope size [BQ] */
+	struct dvs2_bq_resolution envelope_bq;
+	/* isp pipe filter size [BQ] */
+	struct dvs2_bq_resolution ispfilter_bq;
+	/* GDC shit size [BQ] */
+	struct dvs2_bq_resolution gdc_shift_bq;
+};
+
+struct atomisp_dvs_6axis_config {
+	u32 exp_id;
+	u32 width_y;
+	u32 height_y;
+	u32 width_uv;
+	u32 height_uv;
+	u32 *xcoords_y;
+	u32 *ycoords_y;
+	u32 *xcoords_uv;
+	u32 *ycoords_uv;
+};
+
+struct atomisp_formats_config {
+	u32 video_full_range_flag;
+};
+
+struct atomisp_parameters {
+	struct atomisp_wb_config   *wb_config;  /* White Balance config */
+	struct atomisp_cc_config   *cc_config;  /* Color Correction config */
+	struct atomisp_tnr_config  *tnr_config; /* Temporal Noise Reduction */
+	struct atomisp_ecd_config  *ecd_config; /* Eigen Color Demosaicing */
+	struct atomisp_ynr_config  *ynr_config; /* Y(Luma) Noise Reduction */
+	struct atomisp_fc_config   *fc_config;  /* Fringe Control */
+	struct atomisp_formats_config *formats_config; /* Formats Control */
+	struct atomisp_cnr_config  *cnr_config; /* Chroma Noise Reduction */
+	struct atomisp_macc_config *macc_config;  /* MACC */
+	struct atomisp_ctc_config  *ctc_config; /* Chroma Tone Control */
+	struct atomisp_aa_config   *aa_config;  /* Anti-Aliasing */
+	struct atomisp_aa_config   *baa_config;  /* Anti-Aliasing */
+	struct atomisp_ce_config   *ce_config;
+	struct atomisp_dvs_6axis_config *dvs_6axis_config;
+	struct atomisp_ob_config   *ob_config;  /* Objective Black config */
+	struct atomisp_dp_config   *dp_config;  /* Dead Pixel config */
+	struct atomisp_nr_config   *nr_config;  /* Noise Reduction config */
+	struct atomisp_ee_config   *ee_config;  /* Edge Enhancement config */
+	struct atomisp_de_config   *de_config;  /* Demosaic config */
+	struct atomisp_gc_config   *gc_config;  /* Gamma Correction config */
+	struct atomisp_anr_config  *anr_config; /* Advanced Noise Reduction */
+	struct atomisp_3a_config   *a3a_config; /* 3A Statistics config */
+	struct atomisp_xnr_config  *xnr_config; /* eXtra Noise Reduction */
+	struct atomisp_dz_config   *dz_config;  /* Digital Zoom */
+	struct atomisp_cc_config *yuv2rgb_cc_config; /* Color
+							Correction config */
+	struct atomisp_cc_config *rgb2yuv_cc_config; /* Color
+							Correction config */
+	struct atomisp_macc_table  *macc_table;
+	struct atomisp_gamma_table *gamma_table;
+	struct atomisp_ctc_table   *ctc_table;
+	struct atomisp_xnr_table   *xnr_table;
+	struct atomisp_rgb_gamma_table *r_gamma_table;
+	struct atomisp_rgb_gamma_table *g_gamma_table;
+	struct atomisp_rgb_gamma_table *b_gamma_table;
+	struct atomisp_vector      *motion_vector; /* For 2-axis DVS */
+	struct atomisp_shading_table *shading_table;
+	struct atomisp_morph_table   *morph_table;
+	struct atomisp_dvs_coefficients *dvs_coefs; /* DVS 1.0 coefficients */
+	struct atomisp_dis_coefficients *dvs2_coefs; /* DVS 2.0 coefficients */
+	struct atomisp_capture_config   *capture_config;
+	struct atomisp_anr_thres   *anr_thres;
+
+	void	*lin_2500_config;       /* Skylake: Linearization config */
+	void	*obgrid_2500_config;    /* Skylake: OBGRID config */
+	void	*bnr_2500_config;       /* Skylake: bayer denoise config */
+	void	*shd_2500_config;       /* Skylake: shading config */
+	void	*dm_2500_config;        /* Skylake: demosaic config */
+	void	*rgbpp_2500_config;     /* Skylake: RGBPP config */
+	void	*dvs_stat_2500_config;  /* Skylake: DVS STAT config */
+	void	*lace_stat_2500_config; /* Skylake: LACE STAT config */
+	void	*yuvp1_2500_config;     /* Skylake: yuvp1 config */
+	void	*yuvp2_2500_config;     /* Skylake: yuvp2 config */
+	void	*tnr_2500_config;       /* Skylake: TNR config */
+	void	*dpc_2500_config;       /* Skylake: DPC config */
+	void	*awb_2500_config;       /* Skylake: auto white balance config */
+	void	*awb_fr_2500_config;    /* Skylake: auto white balance filter response config */
+	void	*anr_2500_config;       /* Skylake: ANR config */
+	void	*af_2500_config;        /* Skylake: auto focus config */
+	void	*ae_2500_config;        /* Skylake: auto exposure config */
+	void	*bds_2500_config;       /* Skylake: bayer downscaler config */
+	void	*dvs_2500_config;       /* Skylake: digital video stabilization config */
+	void	*res_mgr_2500_config;
+
+	/*
+	 * Output frame pointer the config is to be applied to (optional),
+	 * set to NULL to make this config is applied as global.
+	 */
+	void	*output_frame;
+	/*
+	 * Unique ID to track which config was actually applied to a particular
+	 * frame, driver will send this id back with output frame together.
+	 */
+	u32	isp_config_id;
+
+	/*
+	 * Switch to control per_frame setting:
+	 * 0: this is a global setting
+	 * 1: this is a per_frame setting
+	 * PLEASE KEEP THIS AT THE END OF THE STRUCTURE!!
+	 */
+	u32	per_frame_setting;
+};
+
+#define ATOMISP_GAMMA_TABLE_SIZE        1024
+struct atomisp_gamma_table {
+	unsigned short data[ATOMISP_GAMMA_TABLE_SIZE];
+};
+
+/* Morphing table for advanced ISP.
+ * Each line of width elements takes up COORD_TABLE_EXT_WIDTH elements
+ * in memory.
+ */
+#define ATOMISP_MORPH_TABLE_NUM_PLANES  6
+struct atomisp_morph_table {
+	unsigned int enabled;
+
+	unsigned int height;
+	unsigned int width;	/* number of valid elements per line */
+	unsigned short __user *coordinates_x[ATOMISP_MORPH_TABLE_NUM_PLANES];
+	unsigned short __user *coordinates_y[ATOMISP_MORPH_TABLE_NUM_PLANES];
+};
+
+#define ATOMISP_NUM_SC_COLORS	4
+#define ATOMISP_SC_FLAG_QUERY	BIT(0)
+
+struct atomisp_shading_table {
+	__u32 enable;
+
+	__u32 sensor_width;
+	__u32 sensor_height;
+	__u32 width;
+	__u32 height;
+	__u32 fraction_bits;
+
+	__u16 *data[ATOMISP_NUM_SC_COLORS];
+};
+
+struct atomisp_makernote_info {
+	/* bits 31-16: numerator, bits 15-0: denominator */
+	unsigned int focal_length;
+	/* bits 31-16: numerator, bits 15-0: denominator*/
+	unsigned int f_number_curr;
+	/*
+	* bits 31-24: max f-number numerator
+	* bits 23-16: max f-number denominator
+	* bits 15-8: min f-number numerator
+	* bits 7-0: min f-number denominator
+	*/
+	unsigned int f_number_range;
+};
+
+/* parameter for MACC */
+#define ATOMISP_NUM_MACC_AXES           16
+struct atomisp_macc_table {
+	short data[4 * ATOMISP_NUM_MACC_AXES];
+};
+
+struct atomisp_macc_config {
+	int color_effect;
+	struct atomisp_macc_table table;
+};
+
+/* Parameter for ctc parameter control */
+#define ATOMISP_CTC_TABLE_SIZE          1024
+struct atomisp_ctc_table {
+	unsigned short data[ATOMISP_CTC_TABLE_SIZE];
+};
+
+/* Parameter for overlay image loading */
+struct atomisp_overlay {
+	/* the frame containing the overlay data The overlay frame width should
+	 * be the multiples of 2*ISP_VEC_NELEMS. The overlay frame height
+	 * should be the multiples of 2.
+	 */
+	struct v4l2_framebuffer *frame;
+	/* Y value of overlay background */
+	unsigned char bg_y;
+	/* U value of overlay background */
+	char bg_u;
+	/* V value of overlay background */
+	char bg_v;
+	/* the blending percent of input data for Y subpixels */
+	unsigned char blend_input_perc_y;
+	/* the blending percent of input data for U subpixels */
+	unsigned char blend_input_perc_u;
+	/* the blending percent of input data for V subpixels */
+	unsigned char blend_input_perc_v;
+	/* the blending percent of overlay data for Y subpixels */
+	unsigned char blend_overlay_perc_y;
+	/* the blending percent of overlay data for U subpixels */
+	unsigned char blend_overlay_perc_u;
+	/* the blending percent of overlay data for V subpixels */
+	unsigned char blend_overlay_perc_v;
+	/* the overlay start x pixel position on output frame It should be the
+	   multiples of 2*ISP_VEC_NELEMS. */
+	unsigned int overlay_start_x;
+	/* the overlay start y pixel position on output frame It should be the
+	   multiples of 2. */
+	unsigned int overlay_start_y;
+};
+
+/* Sensor resolution specific data for AE calculation.*/
+struct atomisp_sensor_mode_data {
+	unsigned int coarse_integration_time_min;
+	unsigned int coarse_integration_time_max_margin;
+	unsigned int fine_integration_time_min;
+	unsigned int fine_integration_time_max_margin;
+	unsigned int fine_integration_time_def;
+	unsigned int frame_length_lines;
+	unsigned int line_length_pck;
+	unsigned int read_mode;
+	unsigned int vt_pix_clk_freq_mhz;
+	unsigned int crop_horizontal_start; /* Sensor crop start cord. (x0,y0)*/
+	unsigned int crop_vertical_start;
+	unsigned int crop_horizontal_end; /* Sensor crop end cord. (x1,y1)*/
+	unsigned int crop_vertical_end;
+	unsigned int output_width; /* input size to ISP after binning/scaling */
+	unsigned int output_height;
+	u8 binning_factor_x; /* horizontal binning factor used */
+	u8 binning_factor_y; /* vertical binning factor used */
+	u16 hts;
+};
+
+struct atomisp_exposure {
+	unsigned int integration_time[8];
+	unsigned int shutter_speed[8];
+	unsigned int gain[4];
+	unsigned int aperture;
+};
+
+/* For texture streaming. */
+struct atomisp_bc_video_package {
+	int ioctl_cmd;
+	int device_id;
+	int inputparam;
+	int outputparam;
+};
+
+enum atomisp_focus_hp {
+	ATOMISP_FOCUS_HP_IN_PROGRESS = (1U << 2),
+	ATOMISP_FOCUS_HP_COMPLETE    = (2U << 2),
+	ATOMISP_FOCUS_HP_FAILED      = (3U << 2)
+};
+
+/* Masks */
+#define ATOMISP_FOCUS_STATUS_MOVING           BIT(0)
+#define ATOMISP_FOCUS_STATUS_ACCEPTS_NEW_MOVE BIT(1)
+#define ATOMISP_FOCUS_STATUS_HOME_POSITION    (3U << 2)
+
+enum atomisp_camera_port {
+	ATOMISP_CAMERA_PORT_SECONDARY,
+	ATOMISP_CAMERA_PORT_PRIMARY,
+	ATOMISP_CAMERA_PORT_TERTIARY,
+	ATOMISP_CAMERA_NR_PORTS
+};
+
+/* Flash modes. Default is off.
+ * Setting a flash to TORCH or INDICATOR mode will automatically
+ * turn it on. Setting it to FLASH mode will not turn on the flash
+ * until the FLASH_STROBE command is sent. */
+enum atomisp_flash_mode {
+	ATOMISP_FLASH_MODE_OFF,
+	ATOMISP_FLASH_MODE_FLASH,
+	ATOMISP_FLASH_MODE_TORCH,
+	ATOMISP_FLASH_MODE_INDICATOR,
+};
+
+/* Flash statuses, used by atomisp driver to check before starting
+ * flash and after having started flash. */
+enum atomisp_flash_status {
+	ATOMISP_FLASH_STATUS_OK,
+	ATOMISP_FLASH_STATUS_HW_ERROR,
+	ATOMISP_FLASH_STATUS_INTERRUPTED,
+	ATOMISP_FLASH_STATUS_TIMEOUT,
+};
+
+/* Frame status. This is used to detect corrupted frames and flash
+ * exposed frames. Usually, the first 2 frames coming out of the sensor
+ * are corrupted. When using flash, the frame before and the frame after
+ * the flash exposed frame may be partially exposed by flash. The ISP
+ * statistics for these frames should not be used by the 3A library.
+ * The frame status value can be found in the "reserved" field in the
+ * v4l2_buffer struct. */
+enum atomisp_frame_status {
+	ATOMISP_FRAME_STATUS_OK,
+	ATOMISP_FRAME_STATUS_CORRUPTED,
+	ATOMISP_FRAME_STATUS_FLASH_EXPOSED,
+	ATOMISP_FRAME_STATUS_FLASH_PARTIAL,
+	ATOMISP_FRAME_STATUS_FLASH_FAILED,
+};
+
+enum atomisp_ext_isp_id {
+	EXT_ISP_CID_ISO = 0,
+	EXT_ISP_CID_CAPTURE_HDR,
+	EXT_ISP_CID_CAPTURE_LLS,
+	EXT_ISP_CID_FOCUS_MODE,
+	EXT_ISP_CID_FOCUS_EXECUTION,
+	EXT_ISP_CID_TOUCH_POSX,
+	EXT_ISP_CID_TOUCH_POSY,
+	EXT_ISP_CID_CAF_STATUS,
+	EXT_ISP_CID_AF_STATUS,
+	EXT_ISP_CID_GET_AF_MODE,
+	EXT_ISP_CID_CAPTURE_BURST,
+	EXT_ISP_CID_FLASH_MODE,
+	EXT_ISP_CID_ZOOM,
+	EXT_ISP_CID_SHOT_MODE
+};
+
+#define EXT_ISP_FOCUS_MODE_NORMAL	0
+#define EXT_ISP_FOCUS_MODE_MACRO	1
+#define EXT_ISP_FOCUS_MODE_TOUCH_AF	2
+#define EXT_ISP_FOCUS_MODE_PREVIEW_CAF	3
+#define EXT_ISP_FOCUS_MODE_MOVIE_CAF	4
+#define EXT_ISP_FOCUS_MODE_FACE_CAF	5
+#define EXT_ISP_FOCUS_MODE_TOUCH_MACRO	6
+#define EXT_ISP_FOCUS_MODE_TOUCH_CAF	7
+
+#define EXT_ISP_FOCUS_STOP		0
+#define EXT_ISP_FOCUS_SEARCH		1
+#define EXT_ISP_PAN_FOCUSING		2
+
+#define EXT_ISP_CAF_RESTART_CHECK	1
+#define EXT_ISP_CAF_STATUS_FOCUSING	2
+#define EXT_ISP_CAF_STATUS_SUCCESS	3
+#define EXT_ISP_CAF_STATUS_FAIL         4
+
+#define EXT_ISP_AF_STATUS_INVALID	1
+#define EXT_ISP_AF_STATUS_FOCUSING	2
+#define EXT_ISP_AF_STATUS_SUCCESS	3
+#define EXT_ISP_AF_STATUS_FAIL		4
+
+enum atomisp_burst_capture_options {
+	EXT_ISP_BURST_CAPTURE_CTRL_START = 0,
+	EXT_ISP_BURST_CAPTURE_CTRL_STOP
+};
+
+#define EXT_ISP_FLASH_MODE_OFF		0
+#define EXT_ISP_FLASH_MODE_ON		1
+#define EXT_ISP_FLASH_MODE_AUTO		2
+#define EXT_ISP_LED_TORCH_OFF		3
+#define EXT_ISP_LED_TORCH_ON		4
+
+#define EXT_ISP_SHOT_MODE_AUTO		0
+#define EXT_ISP_SHOT_MODE_BEAUTY_FACE	1
+#define EXT_ISP_SHOT_MODE_BEST_PHOTO	2
+#define EXT_ISP_SHOT_MODE_DRAMA		3
+#define EXT_ISP_SHOT_MODE_BEST_FACE	4
+#define EXT_ISP_SHOT_MODE_ERASER	5
+#define EXT_ISP_SHOT_MODE_PANORAMA	6
+#define EXT_ISP_SHOT_MODE_RICH_TONE_HDR	7
+#define EXT_ISP_SHOT_MODE_NIGHT		8
+#define EXT_ISP_SHOT_MODE_SOUND_SHOT	9
+#define EXT_ISP_SHOT_MODE_ANIMATED_PHOTO	10
+#define EXT_ISP_SHOT_MODE_SPORTS	11
+
+/*
+ * Set Senor run mode
+ */
+struct atomisp_s_runmode {
+	__u32 mode;
+};
+
+struct atomisp_update_exposure {
+	unsigned int gain;
+	unsigned int digi_gain;
+	unsigned int update_gain;
+	unsigned int update_digi_gain;
+};
+
+/*
+ * V4L2 private internal data interface.
+ * -----------------------------------------------------------------------------
+ * struct v4l2_private_int_data - request private data stored in video device
+ * internal memory.
+ * @size: sanity check to ensure userspace's buffer fits whole private data.
+ *	  If not, kernel will make partial copy (or nothing if @size == 0).
+ *	  @size is always corrected for the minimum necessary if IOCTL returns
+ *	  no error.
+ * @data: pointer to userspace buffer.
+ */
+struct v4l2_private_int_data {
+	__u32 size;
+	void __user *data;
+	__u32 reserved[2];
+};
+
+enum atomisp_sensor_ae_bracketing_mode {
+	SENSOR_AE_BRACKETING_MODE_OFF = 0,
+	SENSOR_AE_BRACKETING_MODE_SINGLE, /* back to SW standby after bracketing */
+	SENSOR_AE_BRACKETING_MODE_SINGLE_TO_STREAMING, /* back to normal streaming after bracketing */
+	SENSOR_AE_BRACKETING_MODE_LOOP, /* continue AE bracketing in loop mode */
+};
+
+struct atomisp_sensor_ae_bracketing_info {
+	unsigned int modes; /* bit mask to indicate supported modes  */
+	unsigned int lut_depth;
+};
+
+struct atomisp_sensor_ae_bracketing_lut_entry {
+	__u16 coarse_integration_time;
+	__u16 analog_gain;
+	__u16 digital_gain;
+};
+
+struct atomisp_sensor_ae_bracketing_lut {
+	struct atomisp_sensor_ae_bracketing_lut_entry *lut;
+	unsigned int lut_size;
+};
+
+/*Private IOCTLs for ISP */
+#define ATOMISP_IOC_G_XNR \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 0, int)
+#define ATOMISP_IOC_S_XNR \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 0, int)
+#define ATOMISP_IOC_G_NR \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 1, struct atomisp_nr_config)
+#define ATOMISP_IOC_S_NR \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 1, struct atomisp_nr_config)
+#define ATOMISP_IOC_G_TNR \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 2, struct atomisp_tnr_config)
+#define ATOMISP_IOC_S_TNR \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 2, struct atomisp_tnr_config)
+#define ATOMISP_IOC_G_HISTOGRAM \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 3, struct atomisp_histogram)
+#define ATOMISP_IOC_S_HISTOGRAM \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 3, struct atomisp_histogram)
+#define ATOMISP_IOC_G_BLACK_LEVEL_COMP \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 4, struct atomisp_ob_config)
+#define ATOMISP_IOC_S_BLACK_LEVEL_COMP \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 4, struct atomisp_ob_config)
+#define ATOMISP_IOC_G_EE \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 5, struct atomisp_ee_config)
+#define ATOMISP_IOC_S_EE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 5, struct atomisp_ee_config)
+/* Digital Image Stabilization:
+ * 1. get dis statistics: reads DIS statistics from ISP (every frame)
+ * 2. set dis coefficients: set DIS filter coefficients (one time)
+ * 3. set dis motion vecotr: set motion vector (result of DIS, every frame)
+ */
+#define ATOMISP_IOC_G_DIS_STAT \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dis_statistics)
+
+#define ATOMISP_IOC_G_DVS2_BQ_RESOLUTIONS \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dvs2_bq_resolutions)
+
+#define ATOMISP_IOC_S_DIS_COEFS \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dis_coefficients)
+
+#define ATOMISP_IOC_S_DIS_VECTOR \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dvs_6axis_config)
+
+#define ATOMISP_IOC_G_3A_STAT \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 7, struct atomisp_3a_statistics)
+#define ATOMISP_IOC_G_ISP_PARM \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 8, struct atomisp_parm)
+#define ATOMISP_IOC_S_ISP_PARM \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 8, struct atomisp_parm)
+#define ATOMISP_IOC_G_ISP_GAMMA \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 9, struct atomisp_gamma_table)
+#define ATOMISP_IOC_S_ISP_GAMMA \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 9, struct atomisp_gamma_table)
+#define ATOMISP_IOC_G_ISP_GDC_TAB \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 10, struct atomisp_morph_table)
+#define ATOMISP_IOC_S_ISP_GDC_TAB \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 10, struct atomisp_morph_table)
+#define ATOMISP_IOC_ISP_MAKERNOTE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 11, struct atomisp_makernote_info)
+
+/* macc parameter control*/
+#define ATOMISP_IOC_G_ISP_MACC \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 12, struct atomisp_macc_config)
+#define ATOMISP_IOC_S_ISP_MACC \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 12, struct atomisp_macc_config)
+
+/* Defect pixel detection & Correction */
+#define ATOMISP_IOC_G_ISP_BAD_PIXEL_DETECTION \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 13, struct atomisp_dp_config)
+#define ATOMISP_IOC_S_ISP_BAD_PIXEL_DETECTION \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 13, struct atomisp_dp_config)
+
+/* False Color Correction */
+#define ATOMISP_IOC_G_ISP_FALSE_COLOR_CORRECTION \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 14, struct atomisp_de_config)
+#define ATOMISP_IOC_S_ISP_FALSE_COLOR_CORRECTION \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 14, struct atomisp_de_config)
+
+/* ctc parameter control */
+#define ATOMISP_IOC_G_ISP_CTC \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 15, struct atomisp_ctc_table)
+#define ATOMISP_IOC_S_ISP_CTC \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 15, struct atomisp_ctc_table)
+
+/* white balance Correction */
+#define ATOMISP_IOC_G_ISP_WHITE_BALANCE \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 16, struct atomisp_wb_config)
+#define ATOMISP_IOC_S_ISP_WHITE_BALANCE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 16, struct atomisp_wb_config)
+
+/* fpn table loading */
+#define ATOMISP_IOC_S_ISP_FPN_TABLE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 17, struct v4l2_framebuffer)
+
+/* overlay image loading */
+#define ATOMISP_IOC_G_ISP_OVERLAY \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 18, struct atomisp_overlay)
+#define ATOMISP_IOC_S_ISP_OVERLAY \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 18, struct atomisp_overlay)
+
+/* bcd driver bridge */
+#define ATOMISP_IOC_CAMERA_BRIDGE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 19, struct atomisp_bc_video_package)
+
+/* Sensor resolution specific info for AE */
+#define ATOMISP_IOC_G_SENSOR_MODE_DATA \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 20, struct atomisp_sensor_mode_data)
+
+#define ATOMISP_IOC_S_EXPOSURE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 21, struct atomisp_exposure)
+
+/* sensor calibration registers group */
+#define ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 22, struct atomisp_calibration_group)
+
+/* white balance Correction */
+#define ATOMISP_IOC_G_3A_CONFIG \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 23, struct atomisp_3a_config)
+#define ATOMISP_IOC_S_3A_CONFIG \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 23, struct atomisp_3a_config)
+
+/* sensor OTP memory read */
+#define ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 26, struct v4l2_private_int_data)
+
+/* LCS (shading) table write */
+#define ATOMISP_IOC_S_ISP_SHD_TAB \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 27, struct atomisp_shading_table)
+
+/* Gamma Correction */
+#define ATOMISP_IOC_G_ISP_GAMMA_CORRECTION \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 28, struct atomisp_gc_config)
+
+#define ATOMISP_IOC_S_ISP_GAMMA_CORRECTION \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 28, struct atomisp_gc_config)
+
+/* motor internal memory read */
+#define ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 29, struct v4l2_private_int_data)
+
+#define ATOMISP_IOC_S_PARAMETERS \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 32, struct atomisp_parameters)
+
+#define ATOMISP_IOC_S_CONT_CAPTURE_CONFIG \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 33, struct atomisp_cont_capture_conf)
+
+#define ATOMISP_IOC_G_METADATA \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 34, struct atomisp_metadata)
+
+#define ATOMISP_IOC_G_METADATA_BY_TYPE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 34, struct atomisp_metadata_with_type)
+
+#define ATOMISP_IOC_EXT_ISP_CTRL \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 35, struct atomisp_ext_isp_ctrl)
+
+#define ATOMISP_IOC_EXP_ID_UNLOCK \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 36, int)
+
+#define ATOMISP_IOC_EXP_ID_CAPTURE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 37, int)
+
+#define ATOMISP_IOC_S_ENABLE_DZ_CAPT_PIPE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 38, unsigned int)
+
+#define ATOMISP_IOC_G_FORMATS_CONFIG \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 39, struct atomisp_formats_config)
+
+#define ATOMISP_IOC_S_FORMATS_CONFIG \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 39, struct atomisp_formats_config)
+
+#define ATOMISP_IOC_S_EXPOSURE_WINDOW \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 40, struct atomisp_ae_window)
+
+#define ATOMISP_IOC_INJECT_A_FAKE_EVENT \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 42, int)
+
+#define ATOMISP_IOC_G_SENSOR_AE_BRACKETING_INFO \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 43, struct atomisp_sensor_ae_bracketing_info)
+
+#define ATOMISP_IOC_S_SENSOR_AE_BRACKETING_MODE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 43, unsigned int)
+
+#define ATOMISP_IOC_G_SENSOR_AE_BRACKETING_MODE \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 43, unsigned int)
+
+#define ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 43, struct atomisp_sensor_ae_bracketing_lut)
+
+#define ATOMISP_IOC_G_INVALID_FRAME_NUM \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 44, unsigned int)
+
+#define ATOMISP_IOC_S_ARRAY_RESOLUTION \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 45, struct atomisp_resolution)
+
+/* for depth mode sensor frame sync compensation */
+#define ATOMISP_IOC_G_DEPTH_SYNC_COMP \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 46, unsigned int)
+
+#define ATOMISP_IOC_S_SENSOR_EE_CONFIG \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 47, unsigned int)
+
+#define ATOMISP_IOC_S_SENSOR_RUNMODE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 48, struct atomisp_s_runmode)
+
+#define ATOMISP_IOC_G_UPDATE_EXPOSURE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 49, struct atomisp_update_exposure)
+
+/*
+ * Reserved ioctls. We have customer implementing it internally.
+ * We can't use both numbers to not cause ABI conflict.
+ * Anyway, those ioctls are hacks and not implemented by us:
+ *
+ * #define ATOMISP_IOC_G_SENSOR_REG \
+ *	_IOW('v', BASE_VIDIOC_PRIVATE + 55, struct atomisp_sensor_regs)
+ * #define ATOMISP_IOC_S_SENSOR_REG \
+ *	_IOW('v', BASE_VIDIOC_PRIVATE + 56, struct atomisp_sensor_regs)
+ */
+
+/*  ISP Private control IDs */
+#define V4L2_CID_ATOMISP_BAD_PIXEL_DETECTION \
+	(V4L2_CID_PRIVATE_BASE + 0)
+#define V4L2_CID_ATOMISP_POSTPROCESS_GDC_CAC \
+	(V4L2_CID_PRIVATE_BASE + 1)
+#define V4L2_CID_ATOMISP_VIDEO_STABLIZATION \
+	(V4L2_CID_PRIVATE_BASE + 2)
+#define V4L2_CID_ATOMISP_FIXED_PATTERN_NR \
+	(V4L2_CID_PRIVATE_BASE + 3)
+#define V4L2_CID_ATOMISP_FALSE_COLOR_CORRECTION \
+	(V4L2_CID_PRIVATE_BASE + 4)
+#define V4L2_CID_ATOMISP_LOW_LIGHT \
+	(V4L2_CID_PRIVATE_BASE + 5)
+
+/* Camera class:
+ * Exposure, Flash and privacy (indicator) light controls, to be upstreamed */
+#define V4L2_CID_CAMERA_LASTP1             (V4L2_CID_CAMERA_CLASS_BASE + 1024)
+
+#define V4L2_CID_FOCAL_ABSOLUTE            (V4L2_CID_CAMERA_LASTP1 + 0)
+#define V4L2_CID_FNUMBER_ABSOLUTE          (V4L2_CID_CAMERA_LASTP1 + 1)
+#define V4L2_CID_FNUMBER_RANGE             (V4L2_CID_CAMERA_LASTP1 + 2)
+
+/* Flash related CIDs, see also:
+ * http://linuxtv.org/downloads/v4l-dvb-apis/extended-controls.html\
+ * #flash-controls */
+
+/* Request a number of flash-exposed frames. The frame status can be
+ * found in the reserved field in the v4l2_buffer struct. */
+#define V4L2_CID_REQUEST_FLASH             (V4L2_CID_CAMERA_LASTP1 + 3)
+/* Query flash driver status. See enum atomisp_flash_status above. */
+#define V4L2_CID_FLASH_STATUS              (V4L2_CID_CAMERA_LASTP1 + 5)
+/* Set the flash mode (see enum atomisp_flash_mode) */
+#define V4L2_CID_FLASH_MODE                (V4L2_CID_CAMERA_LASTP1 + 10)
+
+/* VCM slew control */
+#define V4L2_CID_VCM_SLEW                  (V4L2_CID_CAMERA_LASTP1 + 11)
+/* VCM step time */
+#define V4L2_CID_VCM_TIMING                (V4L2_CID_CAMERA_LASTP1 + 12)
+
+/* Query Focus Status */
+#define V4L2_CID_FOCUS_STATUS              (V4L2_CID_CAMERA_LASTP1 + 14)
+
+/* Query sensor's binning factor */
+#define V4L2_CID_BIN_FACTOR_HORZ	   (V4L2_CID_CAMERA_LASTP1 + 15)
+#define V4L2_CID_BIN_FACTOR_VERT	   (V4L2_CID_CAMERA_LASTP1 + 16)
+
+/* number of frames to skip at stream start */
+#define V4L2_CID_G_SKIP_FRAMES		   (V4L2_CID_CAMERA_LASTP1 + 17)
+
+/* Query sensor's 2A status */
+#define V4L2_CID_2A_STATUS                 (V4L2_CID_CAMERA_LASTP1 + 18)
+#define V4L2_2A_STATUS_AE_READY            BIT(0)
+#define V4L2_2A_STATUS_AWB_READY           BIT(1)
+
+#define V4L2_CID_FMT_AUTO			(V4L2_CID_CAMERA_LASTP1 + 19)
+
+#define V4L2_CID_RUN_MODE			(V4L2_CID_CAMERA_LASTP1 + 20)
+#define ATOMISP_RUN_MODE_VIDEO			1
+#define ATOMISP_RUN_MODE_STILL_CAPTURE		2
+#define ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE	3
+#define ATOMISP_RUN_MODE_PREVIEW		4
+#define ATOMISP_RUN_MODE_SDV			5
+
+#define V4L2_CID_ENABLE_VFPP			(V4L2_CID_CAMERA_LASTP1 + 21)
+#define V4L2_CID_ATOMISP_CONTINUOUS_MODE	(V4L2_CID_CAMERA_LASTP1 + 22)
+#define V4L2_CID_ATOMISP_CONTINUOUS_RAW_BUFFER_SIZE \
+						(V4L2_CID_CAMERA_LASTP1 + 23)
+#define V4L2_CID_ATOMISP_CONTINUOUS_VIEWFINDER \
+						(V4L2_CID_CAMERA_LASTP1 + 24)
+
+#define V4L2_CID_VFPP				(V4L2_CID_CAMERA_LASTP1 + 25)
+#define ATOMISP_VFPP_ENABLE			0
+#define ATOMISP_VFPP_DISABLE_SCALER		1
+#define ATOMISP_VFPP_DISABLE_LOWLAT		2
+
+/* Query real flash status register value */
+#define V4L2_CID_FLASH_STATUS_REGISTER  (V4L2_CID_CAMERA_LASTP1 + 26)
+
+#define V4L2_CID_START_ZSL_CAPTURE	(V4L2_CID_CAMERA_LASTP1 + 28)
+/* Lock and unlock raw buffer */
+#define V4L2_CID_ENABLE_RAW_BUFFER_LOCK (V4L2_CID_CAMERA_LASTP1 + 29)
+
+#define V4L2_CID_DEPTH_MODE		(V4L2_CID_CAMERA_LASTP1 + 30)
+
+#define V4L2_CID_EXPOSURE_ZONE_NUM	(V4L2_CID_CAMERA_LASTP1 + 31)
+/* Disable digital zoom */
+#define V4L2_CID_DISABLE_DZ		(V4L2_CID_CAMERA_LASTP1 + 32)
+
+#define V4L2_CID_TEST_PATTERN_COLOR_R	(V4L2_CID_CAMERA_LASTP1 + 33)
+#define V4L2_CID_TEST_PATTERN_COLOR_GR	(V4L2_CID_CAMERA_LASTP1 + 34)
+#define V4L2_CID_TEST_PATTERN_COLOR_GB	(V4L2_CID_CAMERA_LASTP1 + 35)
+#define V4L2_CID_TEST_PATTERN_COLOR_B	(V4L2_CID_CAMERA_LASTP1 + 36)
+
+#define V4L2_CID_ATOMISP_SELECT_ISP_VERSION	(V4L2_CID_CAMERA_LASTP1 + 38)
+
+#define V4L2_BUF_FLAG_BUFFER_INVALID       0x0400
+#define V4L2_BUF_FLAG_BUFFER_VALID         0x0800
+
+#define V4L2_BUF_TYPE_VIDEO_CAPTURE_ION  (V4L2_BUF_TYPE_PRIVATE + 1024)
+
+#define V4L2_EVENT_ATOMISP_3A_STATS_READY   (V4L2_EVENT_PRIVATE_START + 1)
+#define V4L2_EVENT_ATOMISP_METADATA_READY   (V4L2_EVENT_PRIVATE_START + 2)
+#define V4L2_EVENT_ATOMISP_RAW_BUFFERS_ALLOC_DONE   (V4L2_EVENT_PRIVATE_START + 3)
+#define V4L2_EVENT_ATOMISP_ACC_COMPLETE     (V4L2_EVENT_PRIVATE_START + 4)
+#define V4L2_EVENT_ATOMISP_PAUSE_BUFFER	    (V4L2_EVENT_PRIVATE_START + 5)
+#define V4L2_EVENT_ATOMISP_CSS_RESET	    (V4L2_EVENT_PRIVATE_START + 6)
+/* Nonstandard color effects for V4L2_CID_COLORFX */
+enum {
+	V4L2_COLORFX_SKIN_WHITEN_LOW = 1001,
+	V4L2_COLORFX_SKIN_WHITEN_HIGH = 1002,
+	V4L2_COLORFX_WARM = 1003,
+	V4L2_COLORFX_COLD = 1004,
+	V4L2_COLORFX_WASHED = 1005,
+	V4L2_COLORFX_RED = 1006,
+	V4L2_COLORFX_GREEN = 1007,
+	V4L2_COLORFX_BLUE = 1008,
+	V4L2_COLORFX_PINK = 1009,
+	V4L2_COLORFX_YELLOW = 1010,
+	V4L2_COLORFX_PURPLE = 1011,
+};
+
+#endif /* _ATOM_ISP_H */
diff --git a/drivers/staging/media/atomisp/include/linux/atomisp_gmin_platform.h b/drivers/staging/media/atomisp/include/linux/atomisp_gmin_platform.h
new file mode 100644
index 000000000..5463d11d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp_gmin_platform.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel MID SoC Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2014 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ */
+#ifndef ATOMISP_GMIN_PLATFORM_H_
+#define ATOMISP_GMIN_PLATFORM_H_
+
+#include "atomisp_platform.h"
+
+int atomisp_register_i2c_module(struct v4l2_subdev *subdev,
+				struct camera_sensor_platform_data *plat_data,
+				enum intel_v4l2_subdev_type type);
+struct v4l2_subdev *atomisp_gmin_find_subdev(struct i2c_adapter *adapter,
+	struct i2c_board_info *board_info);
+int atomisp_gmin_remove_subdev(struct v4l2_subdev *sd);
+int gmin_get_var_int(struct device *dev, bool is_gmin,
+		     const char *var, int def);
+struct camera_sensor_platform_data *
+gmin_camera_platform_data(
+    struct v4l2_subdev *subdev,
+    enum atomisp_input_format csi_format,
+    enum atomisp_bayer_order csi_bayer);
+
+int atomisp_gmin_register_vcm_control(struct camera_vcm_control *);
+
+#endif
diff --git a/drivers/staging/media/atomisp/include/linux/atomisp_platform.h b/drivers/staging/media/atomisp/include/linux/atomisp_platform.h
new file mode 100644
index 000000000..0253661d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp_platform.h
@@ -0,0 +1,241 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef ATOMISP_PLATFORM_H_
+#define ATOMISP_PLATFORM_H_
+
+#include <asm/intel-family.h>
+#include <asm/processor.h>
+
+#include <linux/i2c.h>
+#include <media/v4l2-subdev.h>
+#include "atomisp.h"
+
+#define MAX_SENSORS_PER_PORT 4
+#define MAX_STREAMS_PER_CHANNEL 2
+
+#define CAMERA_MODULE_ID_LEN 64
+
+enum atomisp_bayer_order {
+	atomisp_bayer_order_grbg,
+	atomisp_bayer_order_rggb,
+	atomisp_bayer_order_bggr,
+	atomisp_bayer_order_gbrg
+};
+
+enum atomisp_input_stream_id {
+	ATOMISP_INPUT_STREAM_GENERAL = 0,
+	ATOMISP_INPUT_STREAM_CAPTURE = 0,
+	ATOMISP_INPUT_STREAM_POSTVIEW,
+	ATOMISP_INPUT_STREAM_PREVIEW,
+	ATOMISP_INPUT_STREAM_VIDEO,
+	ATOMISP_INPUT_STREAM_NUM
+};
+
+enum atomisp_input_format {
+	ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY,/* 8 bits per subpixel (legacy) */
+	ATOMISP_INPUT_FORMAT_YUV420_8, /* 8 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_YUV420_10,/* 10 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_YUV420_16,/* 16 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_YUV422_8, /* UYVY..UVYV, 8 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_YUV422_10,/* UYVY..UVYV, 10 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_YUV422_16,/* UYVY..UVYV, 16 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_RGB_444,  /* BGR..BGR, 4 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_RGB_555,  /* BGR..BGR, 5 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_RGB_565,  /* BGR..BGR, 5 bits B and R, 6 bits G */
+	ATOMISP_INPUT_FORMAT_RGB_666,  /* BGR..BGR, 6 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_RGB_888,  /* BGR..BGR, 8 bits per subpixel */
+	ATOMISP_INPUT_FORMAT_RAW_6,    /* RAW data, 6 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_7,    /* RAW data, 7 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_8,    /* RAW data, 8 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_10,   /* RAW data, 10 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_12,   /* RAW data, 12 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_14,   /* RAW data, 14 bits per pixel */
+	ATOMISP_INPUT_FORMAT_RAW_16,   /* RAW data, 16 bits per pixel */
+	ATOMISP_INPUT_FORMAT_BINARY_8, /* Binary byte stream. */
+
+	/* CSI2-MIPI specific format: Generic short packet data. It is used to
+	 * keep the timing information for the opening/closing of shutters,
+	 * triggering of flashes and etc.
+	 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT1,  /* Generic Short Packet Code 1 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT2,  /* Generic Short Packet Code 2 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT3,  /* Generic Short Packet Code 3 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT4,  /* Generic Short Packet Code 4 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT5,  /* Generic Short Packet Code 5 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT6,  /* Generic Short Packet Code 6 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT7,  /* Generic Short Packet Code 7 */
+	ATOMISP_INPUT_FORMAT_GENERIC_SHORT8,  /* Generic Short Packet Code 8 */
+
+	/* CSI2-MIPI specific format: YUV data.
+	 */
+	ATOMISP_INPUT_FORMAT_YUV420_8_SHIFT,  /* YUV420 8-bit (Chroma Shifted
+						 Pixel Sampling) */
+	ATOMISP_INPUT_FORMAT_YUV420_10_SHIFT, /* YUV420 8-bit (Chroma Shifted
+						 Pixel Sampling) */
+
+	/* CSI2-MIPI specific format: Generic long packet data
+	 */
+	ATOMISP_INPUT_FORMAT_EMBEDDED, /* Embedded 8-bit non Image Data */
+
+	/* CSI2-MIPI specific format: User defined byte-based data. For example,
+	 * the data transmitter (e.g. the SoC sensor) can keep the JPEG data as
+	 * the User Defined Data Type 4 and the MPEG data as the
+	 * User Defined Data Type 7.
+	 */
+	ATOMISP_INPUT_FORMAT_USER_DEF1,  /* User defined 8-bit data type 1 */
+	ATOMISP_INPUT_FORMAT_USER_DEF2,  /* User defined 8-bit data type 2 */
+	ATOMISP_INPUT_FORMAT_USER_DEF3,  /* User defined 8-bit data type 3 */
+	ATOMISP_INPUT_FORMAT_USER_DEF4,  /* User defined 8-bit data type 4 */
+	ATOMISP_INPUT_FORMAT_USER_DEF5,  /* User defined 8-bit data type 5 */
+	ATOMISP_INPUT_FORMAT_USER_DEF6,  /* User defined 8-bit data type 6 */
+	ATOMISP_INPUT_FORMAT_USER_DEF7,  /* User defined 8-bit data type 7 */
+	ATOMISP_INPUT_FORMAT_USER_DEF8,  /* User defined 8-bit data type 8 */
+};
+
+#define N_ATOMISP_INPUT_FORMAT (ATOMISP_INPUT_FORMAT_USER_DEF8 + 1)
+
+enum intel_v4l2_subdev_type {
+	RAW_CAMERA = 1,
+	SOC_CAMERA = 2,
+	CAMERA_MOTOR = 3,
+	LED_FLASH = 4,
+	XENON_FLASH = 5,
+	FILE_INPUT = 6,
+	TEST_PATTERN = 7,
+};
+
+struct intel_v4l2_subdev_id {
+	char name[17];
+	enum intel_v4l2_subdev_type type;
+	enum atomisp_camera_port    port;
+};
+
+struct intel_v4l2_subdev_i2c_board_info {
+	struct i2c_board_info board_info;
+	int i2c_adapter_id;
+};
+
+struct intel_v4l2_subdev_table {
+	struct intel_v4l2_subdev_i2c_board_info v4l2_subdev;
+	enum intel_v4l2_subdev_type type;
+	enum atomisp_camera_port port;
+	struct v4l2_subdev *subdev;
+};
+
+struct atomisp_platform_data {
+	struct intel_v4l2_subdev_table *subdevs;
+};
+
+/*
+ *  Sensor of external ISP can send multiple steams with different mipi data
+ * type in the same virtual channel. This information needs to come from the
+ * sensor or external ISP
+ */
+struct atomisp_isys_config_info {
+	u8 input_format;
+	u16 width;
+	u16 height;
+};
+
+struct atomisp_input_stream_info {
+	enum atomisp_input_stream_id stream;
+	u8 enable;
+	/* Sensor driver fills ch_id with the id
+	   of the virtual channel. */
+	u8 ch_id;
+	/* Tells how many streams in this virtual channel. If 0 ignore rest
+	 * and the input format will be from mipi_info */
+	u8 isys_configs;
+	/*
+	 * if more isys_configs is more than 0, sensor needs to configure the
+	 * input format differently. width and height can be 0. If width and
+	 * height is not zero, then the corresponsing data needs to be set
+	 */
+	struct atomisp_isys_config_info isys_info[MAX_STREAMS_PER_CHANNEL];
+};
+
+struct camera_vcm_control;
+struct camera_vcm_ops {
+	int (*power_up)(struct v4l2_subdev *sd, struct camera_vcm_control *vcm);
+	int (*power_down)(struct v4l2_subdev *sd,
+			  struct camera_vcm_control *vcm);
+	int (*queryctrl)(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc,
+			 struct camera_vcm_control *vcm);
+	int (*g_ctrl)(struct v4l2_subdev *sd, struct v4l2_control *ctrl,
+		      struct camera_vcm_control *vcm);
+	int (*s_ctrl)(struct v4l2_subdev *sd, struct v4l2_control *ctrl,
+		      struct camera_vcm_control *vcm);
+};
+
+struct camera_vcm_control {
+	char camera_module[CAMERA_MODULE_ID_LEN];
+	struct camera_vcm_ops *ops;
+	struct list_head list;
+};
+
+struct camera_sensor_platform_data {
+	int (*flisclk_ctrl)(struct v4l2_subdev *subdev, int flag);
+	int (*csi_cfg)(struct v4l2_subdev *subdev, int flag);
+
+	/*
+	 * New G-Min power and GPIO interface to control individual
+	 * lines as implemented on all known camera modules.
+	 */
+	int (*gpio0_ctrl)(struct v4l2_subdev *subdev, int on);
+	int (*gpio1_ctrl)(struct v4l2_subdev *subdev, int on);
+	int (*v1p8_ctrl)(struct v4l2_subdev *subdev, int on);
+	int (*v2p8_ctrl)(struct v4l2_subdev *subdev, int on);
+	int (*v1p2_ctrl)(struct v4l2_subdev *subdev, int on);
+	struct camera_vcm_control *(*get_vcm_ctrl)(struct v4l2_subdev *subdev,
+		char *module_id);
+};
+
+struct camera_mipi_info {
+	enum atomisp_camera_port        port;
+	unsigned int                    num_lanes;
+	enum atomisp_input_format       input_format;
+	enum atomisp_bayer_order        raw_bayer_order;
+	struct atomisp_sensor_mode_data data;
+	enum atomisp_input_format       metadata_format;
+	u32                             metadata_width;
+	u32                             metadata_height;
+	const u32                       *metadata_effective_width;
+};
+
+const struct atomisp_platform_data *atomisp_get_platform_data(void);
+
+/* API from old platform_camera.h, new CPUID implementation */
+#define __IS_SOC(x) (boot_cpu_data.x86_vendor == X86_VENDOR_INTEL && \
+		     boot_cpu_data.x86 == 6 &&                       \
+		     boot_cpu_data.x86_model == (x))
+#define __IS_SOCS(x,y) (boot_cpu_data.x86_vendor == X86_VENDOR_INTEL && \
+		        boot_cpu_data.x86 == 6 &&                       \
+		        (boot_cpu_data.x86_model == (x) || \
+		         boot_cpu_data.x86_model == (y)))
+
+#define IS_MFLD	__IS_SOC(INTEL_FAM6_ATOM_SALTWELL_MID)
+#define IS_BYT	__IS_SOC(INTEL_FAM6_ATOM_SILVERMONT)
+#define IS_CHT	__IS_SOC(INTEL_FAM6_ATOM_AIRMONT)
+#define IS_MRFD	__IS_SOC(INTEL_FAM6_ATOM_SILVERMONT_MID)
+#define IS_MOFD	__IS_SOC(INTEL_FAM6_ATOM_AIRMONT_MID)
+
+/* Both CHT and MOFD come with ISP2401 */
+#define IS_ISP2401 __IS_SOCS(INTEL_FAM6_ATOM_AIRMONT, \
+			     INTEL_FAM6_ATOM_AIRMONT_MID)
+
+#endif /* ATOMISP_PLATFORM_H_ */
diff --git a/drivers/staging/media/atomisp/include/linux/libmsrlisthelper.h b/drivers/staging/media/atomisp/include/linux/libmsrlisthelper.h
new file mode 100644
index 000000000..abc8fa809
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/libmsrlisthelper.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef __LIBMSRLISTHELPER_H__
+#define __LIBMSRLISTHELPER_H__
+
+struct i2c_client;
+struct firmware;
+
+int load_msr_list(struct i2c_client *client, char *path,
+		  const struct firmware **fw);
+int apply_msr_data(struct i2c_client *client, const struct firmware *fw);
+void release_msr_list(struct i2c_client *client,
+		      const struct firmware *fw);
+
+#endif /* ifndef __LIBMSRLISTHELPER_H__ */
diff --git a/drivers/staging/media/atomisp/include/media/lm3554.h b/drivers/staging/media/atomisp/include/media/lm3554.h
new file mode 100644
index 000000000..711b7d7c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/media/lm3554.h
@@ -0,0 +1,132 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * include/media/lm3554.h
+ *
+ * Copyright (c) 2010-2012 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef _LM3554_H_
+#define _LM3554_H_
+
+#include <linux/gpio/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+
+#define LM3554_ID      3554
+
+#define	v4l2_queryctrl_entry_integer(_id, _name,\
+		_minimum, _maximum, _step, \
+		_default_value, _flags)	\
+	{\
+		.id = (_id), \
+		.type = V4L2_CTRL_TYPE_INTEGER, \
+		.name = _name, \
+		.minimum = (_minimum), \
+		.maximum = (_maximum), \
+		.step = (_step), \
+		.default_value = (_default_value),\
+		.flags = (_flags),\
+	}
+#define	v4l2_queryctrl_entry_boolean(_id, _name,\
+		_default_value, _flags)	\
+	{\
+		.id = (_id), \
+		.type = V4L2_CTRL_TYPE_BOOLEAN, \
+		.name = _name, \
+		.minimum = 0, \
+		.maximum = 1, \
+		.step = 1, \
+		.default_value = (_default_value),\
+		.flags = (_flags),\
+	}
+
+#define	s_ctrl_id_entry_integer(_id, _name, \
+		_minimum, _maximum, _step, \
+		_default_value, _flags, \
+		_s_ctrl, _g_ctrl)	\
+	{\
+		.qc = v4l2_queryctrl_entry_integer(_id, _name,\
+				_minimum, _maximum, _step,\
+				_default_value, _flags), \
+		.s_ctrl = _s_ctrl, \
+		.g_ctrl = _g_ctrl, \
+	}
+
+#define	s_ctrl_id_entry_boolean(_id, _name, \
+		_default_value, _flags, \
+		_s_ctrl, _g_ctrl)	\
+	{\
+		.qc = v4l2_queryctrl_entry_boolean(_id, _name,\
+				_default_value, _flags), \
+		.s_ctrl = _s_ctrl, \
+		.g_ctrl = _g_ctrl, \
+	}
+
+/* Value settings for Flash Time-out Duration*/
+#define LM3554_DEFAULT_TIMEOUT          512U
+#define LM3554_MIN_TIMEOUT              32U
+#define LM3554_MAX_TIMEOUT              1024U
+#define LM3554_TIMEOUT_STEPSIZE         32U
+
+/* Flash modes */
+#define LM3554_MODE_SHUTDOWN            0
+#define LM3554_MODE_INDICATOR           1
+#define LM3554_MODE_TORCH               2
+#define LM3554_MODE_FLASH               3
+
+/* timer delay time */
+#define LM3554_TIMER_DELAY		5
+
+/* Percentage <-> value macros */
+#define LM3554_MIN_PERCENT                   0U
+#define LM3554_MAX_PERCENT                   100U
+#define LM3554_CLAMP_PERCENTAGE(val) \
+	clamp(val, LM3554_MIN_PERCENT, LM3554_MAX_PERCENT)
+
+#define LM3554_VALUE_TO_PERCENT(v, step)     (((((unsigned long)(v)) * (step)) + 50) / 100)
+#define LM3554_PERCENT_TO_VALUE(p, step)     (((((unsigned long)(p)) * 100) + (step >> 1)) / (step))
+
+/* Product specific limits
+ * TODO: get these from platform data */
+#define LM3554_FLASH_MAX_LVL   0x0F /* 1191mA */
+
+/* Flash brightness, input is percentage, output is [0..15] */
+#define LM3554_FLASH_STEP	\
+	((100ul * (LM3554_MAX_PERCENT) + ((LM3554_FLASH_MAX_LVL) >> 1)) / ((LM3554_FLASH_MAX_LVL)))
+#define LM3554_FLASH_DEFAULT_BRIGHTNESS \
+	LM3554_VALUE_TO_PERCENT(13, LM3554_FLASH_STEP)
+
+/* Torch brightness, input is percentage, output is [0..7] */
+#define LM3554_TORCH_STEP                    1250
+#define LM3554_TORCH_DEFAULT_BRIGHTNESS \
+	LM3554_VALUE_TO_PERCENT(2, LM3554_TORCH_STEP)
+
+/* Indicator brightness, input is percentage, output is [0..3] */
+#define LM3554_INDICATOR_STEP                2500
+#define LM3554_INDICATOR_DEFAULT_BRIGHTNESS \
+	LM3554_VALUE_TO_PERCENT(1, LM3554_INDICATOR_STEP)
+
+/*
+ * lm3554_platform_data - Flash controller platform data
+ */
+struct lm3554_platform_data {
+	struct gpio_desc *gpio_torch;
+	struct gpio_desc *gpio_strobe;
+	struct gpio_desc *gpio_reset;
+
+	unsigned int current_limit;
+	unsigned int envm_tx2;
+	unsigned int tx2_polarity;
+};
+
+#endif /* _LM3554_H_ */
diff --git a/drivers/staging/media/atomisp/include/mmu/isp_mmu.h b/drivers/staging/media/atomisp/include/mmu/isp_mmu.h
new file mode 100644
index 000000000..268560954
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/mmu/isp_mmu.h
@@ -0,0 +1,169 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+/*
+ * ISP MMU driver for classic two-level page tables
+ */
+#ifndef	__ISP_MMU_H__
+#define	__ISP_MMU_H__
+
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+
+/*
+ * do not change these values, the page size for ISP must be the
+ * same as kernel's page size.
+ */
+#define	ISP_PAGE_OFFSET		12
+#define	ISP_PAGE_SIZE		BIT(ISP_PAGE_OFFSET)
+#define	ISP_PAGE_MASK		(~(phys_addr_t)(ISP_PAGE_SIZE - 1))
+
+#define	ISP_L1PT_OFFSET		22
+#define	ISP_L1PT_MASK		(~((1U << ISP_L1PT_OFFSET) - 1))
+
+#define	ISP_L2PT_OFFSET		12
+#define	ISP_L2PT_MASK		(~(ISP_L1PT_MASK | (~(ISP_PAGE_MASK))))
+
+#define	ISP_L1PT_PTES		1024
+#define	ISP_L2PT_PTES		1024
+
+#define	ISP_PTR_TO_L1_IDX(x)	((((x) & ISP_L1PT_MASK)) \
+					>> ISP_L1PT_OFFSET)
+
+#define	ISP_PTR_TO_L2_IDX(x)	((((x) & ISP_L2PT_MASK)) \
+					>> ISP_L2PT_OFFSET)
+
+#define	ISP_PAGE_ALIGN(x)	(((x) + (ISP_PAGE_SIZE - 1)) \
+					& ISP_PAGE_MASK)
+
+#define	ISP_PT_TO_VIRT(l1_idx, l2_idx, offset) do {\
+		((l1_idx) << ISP_L1PT_OFFSET) | \
+		((l2_idx) << ISP_L2PT_OFFSET) | \
+		(offset)\
+} while (0)
+
+#define	pgnr_to_size(pgnr)	((pgnr) << ISP_PAGE_OFFSET)
+#define	size_to_pgnr_ceil(size)	(((size) + (1 << ISP_PAGE_OFFSET) - 1)\
+						>> ISP_PAGE_OFFSET)
+#define	size_to_pgnr_bottom(size)	((size) >> ISP_PAGE_OFFSET)
+
+struct isp_mmu;
+
+struct isp_mmu_client {
+	/*
+	 * const value
+	 *
+	 * @name:
+	 *      driver name
+	 * @pte_valid_mask:
+	 *      should be 1 bit valid data, meaning the value should
+	 *      be power of 2.
+	 */
+	char *name;
+	unsigned int pte_valid_mask;
+	unsigned int null_pte;
+
+	/*
+	 * get page directory base address (physical address).
+	 *
+	 * must be provided.
+	 */
+	unsigned int (*get_pd_base)(struct isp_mmu *mmu, phys_addr_t pd_base);
+	/*
+	 * callback to flush tlb.
+	 *
+	 * tlb_flush_range will at least flush TLBs containing
+	 * address mapping from addr to addr + size.
+	 *
+	 * tlb_flush_all will flush all TLBs.
+	 *
+	 * tlb_flush_all is must be provided. if tlb_flush_range is
+	 * not valid, it will set to tlb_flush_all by default.
+	 */
+	void (*tlb_flush_range)(struct isp_mmu *mmu,
+				unsigned int addr, unsigned int size);
+	void (*tlb_flush_all)(struct isp_mmu *mmu);
+	unsigned int (*phys_to_pte)(struct isp_mmu *mmu,
+				    phys_addr_t phys);
+	phys_addr_t (*pte_to_phys)(struct isp_mmu *mmu,
+				   unsigned int pte);
+
+};
+
+struct isp_mmu {
+	struct isp_mmu_client *driver;
+	unsigned int l1_pte;
+	int l2_pgt_refcount[ISP_L1PT_PTES];
+	phys_addr_t base_address;
+
+	struct mutex pt_mutex;
+};
+
+/* flags for PDE and PTE */
+#define	ISP_PTE_VALID_MASK(mmu)	\
+	((mmu)->driver->pte_valid_mask)
+
+#define	ISP_PTE_VALID(mmu, pte)	\
+	((pte) & ISP_PTE_VALID_MASK(mmu))
+
+#define	NULL_PAGE	((phys_addr_t)(-1) & ISP_PAGE_MASK)
+#define	PAGE_VALID(page)	((page) != NULL_PAGE)
+
+/*
+ * init mmu with specific mmu driver.
+ */
+int isp_mmu_init(struct isp_mmu *mmu, struct isp_mmu_client *driver);
+/*
+ * cleanup all mmu related things.
+ */
+void isp_mmu_exit(struct isp_mmu *mmu);
+
+/*
+ * setup/remove address mapping for pgnr continuous physical pages
+ * and isp_virt.
+ *
+ * map/unmap is mutex lock protected, and caller does not have
+ * to do lock/unlock operation.
+ *
+ * map/unmap will not flush tlb, and caller needs to deal with
+ * this itself.
+ */
+int isp_mmu_map(struct isp_mmu *mmu, unsigned int isp_virt,
+		phys_addr_t phys, unsigned int pgnr);
+
+void isp_mmu_unmap(struct isp_mmu *mmu, unsigned int isp_virt,
+		   unsigned int pgnr);
+
+static inline void isp_mmu_flush_tlb_all(struct isp_mmu *mmu)
+{
+	if (mmu->driver && mmu->driver->tlb_flush_all)
+		mmu->driver->tlb_flush_all(mmu);
+}
+
+#define isp_mmu_flush_tlb isp_mmu_flush_tlb_all
+
+static inline void isp_mmu_flush_tlb_range(struct isp_mmu *mmu,
+	unsigned int start, unsigned int size)
+{
+	if (mmu->driver && mmu->driver->tlb_flush_range)
+		mmu->driver->tlb_flush_range(mmu, start, size);
+}
+
+#endif /* ISP_MMU_H_ */
diff --git a/drivers/staging/media/atomisp/include/mmu/sh_mmu_mrfld.h b/drivers/staging/media/atomisp/include/mmu/sh_mmu_mrfld.h
new file mode 100644
index 000000000..84fe7a368
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/mmu/sh_mmu_mrfld.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Merrifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__SH_MMU_MRFLD_H__
+#define	__SH_MMU_MRFLD_H__
+
+extern struct isp_mmu_client sh_mmu_mrfld;
+#endif
diff --git a/drivers/staging/media/atomisp/notes.txt b/drivers/staging/media/atomisp/notes.txt
new file mode 100644
index 000000000..d3cf6ed54
--- /dev/null
+++ b/drivers/staging/media/atomisp/notes.txt
@@ -0,0 +1,49 @@
+Some notes about the working of the atomisp drivers (learned while working
+on cleaning it up).
+
+The atomisp seems to be a generic DSP(ISP) like processor without a fixed
+pipeline. It does not have its own memory, but instead uses main memory.
+The ISP has its own address-space and main memory needs to be mapped into
+its address space through the ISP's MMU.
+
+Memory is allocated by the hmm code. hmm_alloc() returns an ISP virtual
+address. The hmm code keeps a list of all allocations and when necessary
+the hmm code finds the backing hmm-buffer-object (hmm_bo) by looking
+up the hmm_bo based on the ISP virtual address.
+
+The actual processing pipeline is made by loading one or more programs,
+called binaries. The shisp_240??0_v21.bin firmware file contains many
+different binaries. Binaries are picked by filling a ia_css_binary_descr
+struct with various input and output parameters and then calling
+ia_css_binary_find(). Some binaries support creating multiple outputs
+(preview + video frame?) at the same time.
+
+For example for the /dev/video0 preview node load_preview_binaries()
+from atomisp/pci/sh_css.c is called and then loads a preview and
+optionally a scalar binary. Note when digital zoom is disabled
+(it is enabled by default) only the preview binary is loaded.
+So in this case a single binary handles the entire pipeline.
+
+Since getting a picture requires multiple processing steps,
+this means that unlike in fixed pipelines the soft pipelines
+on the ISP can do multiple processing steps in a single pipeline
+element (in a single binary).
+
+###
+
+The sensor drivers use of v4l2_get_subdev_hostdata(), which returns
+a camera_mipi_info struct. This struct is allocated/managed by
+the core atomisp code. The most important parts of the struct
+are filled by the atomisp core itself, like e.g. the port number.
+
+The sensor drivers on a set_fmt call do fill in camera_mipi_info.data
+which is a atomisp_sensor_mode_data struct. This gets filled from
+a function called <sensor_name>_get_intg_factor(). This struct is not
+used by the atomisp code at all. It is returned to userspace by
+a ATOMISP_IOC_G_SENSOR_MODE_DATA and the Android userspace does use this.
+
+Other members of camera_mipi_info which are set by some drivers are:
+-metadata_width, metadata_height, metadata_effective_width, set by
+ the ov5693 driver (and used by the atomisp core)
+-raw_bayer_order, adjusted by the ov2680 driver when flipping since
+ flipping can change the bayer order
diff --git a/drivers/staging/media/atomisp/pci/atomisp-regs.h b/drivers/staging/media/atomisp/pci/atomisp-regs.h
new file mode 100644
index 000000000..022997f47
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp-regs.h
@@ -0,0 +1,197 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2012 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef ATOMISP_REGS_H
+#define ATOMISP_REGS_H
+
+/* common register definitions */
+#define PCICMDSTS		0x01
+#define INTR			0x0f
+#define MSI_CAPID		0x24
+#define MSI_ADDRESS		0x25
+#define MSI_DATA		0x26
+#define INTR_CTL		0x27
+
+#define PCI_MSI_CAPID		0x90
+#define PCI_MSI_ADDR		0x94
+#define PCI_MSI_DATA		0x98
+#define PCI_INTERRUPT_CTRL	0x9C
+#define PCI_I_CONTROL		0xfc
+
+/* MRFLD specific register definitions */
+#define MRFLD_CSI_AFE		0x39
+#define MRFLD_CSI_CONTROL	0x3a
+#define MRFLD_CSI_RCOMP		0x3d
+
+#define MRFLD_PCI_PMCS		0x84
+#define MRFLD_PCI_CSI_ACCESS_CTRL_VIOL	0xd4
+#define MRFLD_PCI_CSI_AFE_HS_CONTROL	0xdc
+#define MRFLD_PCI_CSI_AFE_RCOMP_CONTROL	0xe0
+#define MRFLD_PCI_CSI_CONTROL		0xe8
+#define MRFLD_PCI_CSI_AFE_TRIM_CONTROL	0xe4
+#define MRFLD_PCI_CSI_DEADLINE_CONTROL	0xec
+#define MRFLD_PCI_CSI_RCOMP_CONTROL	0xf4
+
+/* Select Arasan (legacy)/Intel input system */
+#define MRFLD_PCI_CSI_CONTROL_PARPATHEN	BIT(24)
+/* Enable CSI interface (ANN B0/K0) */
+#define MRFLD_PCI_CSI_CONTROL_CSI_READY	BIT(25)
+
+/*
+ * Enables the combining of adjacent 32-byte read requests to the same
+ * cache line. When cleared, each 32-byte read request is sent as a
+ * separate request on the IB interface.
+ */
+#define MRFLD_PCI_I_CONTROL_ENABLE_READ_COMBINING	0x1
+
+/*
+ * Register: MRFLD_PCI_CSI_RCOMP_CONTROL
+ * If cleared, the high speed clock going to the digital logic is gated when
+ * RCOMP update is happening. The clock is gated for a minimum of 100 nsec.
+ * If this bit is set, then the high speed clock is not gated during the
+ * update cycle.
+ */
+#define MRFLD_PCI_CSI_HS_OVR_CLK_GATE_ON_UPDATE		0x800000
+
+/*
+ * Enables the combining of adjacent 32-byte write requests to the same
+ * cache line. When cleared, each 32-byte write request is sent as a
+ * separate request on the IB interface.
+ */
+#define MRFLD_PCI_I_CONTROL_ENABLE_WRITE_COMBINING	0x2
+
+#define MRFLD_PCI_I_CONTROL_SRSE_RESET_MASK		0xc
+
+#define MRFLD_PCI_CSI1_HSRXCLKTRIM		0x2
+#define MRFLD_PCI_CSI1_HSRXCLKTRIM_SHIFT	16
+#define MRFLD_PCI_CSI2_HSRXCLKTRIM		0x3
+#define MRFLD_PCI_CSI2_HSRXCLKTRIM_SHIFT	24
+#define MRFLD_PCI_CSI3_HSRXCLKTRIM		0x2
+#define MRFLD_PCI_CSI3_HSRXCLKTRIM_SHIFT	28
+#define MRFLD_PCI_CSI_HSRXCLKTRIM_MASK		0xf
+
+/*
+ * This register is IUINT MMIO register, it is used to select the CSI
+ * receiver backend.
+ * 1: SH CSI backend
+ * 0: Arasan CSI backend
+ */
+#define MRFLD_CSI_RECEIVER_SELECTION_REG       0x8081c
+
+#define MRFLD_INTR_CLEAR_REG		       0x50c
+#define MRFLD_INTR_STATUS_REG		       0x508
+#define MRFLD_INTR_ENABLE_REG		       0x510
+
+#define MRFLD_MAX_ZOOM_FACTOR	1024
+
+/* MRFLD ISP POWER related */
+#define MRFLD_ISPSSPM0         0x39
+#define MRFLD_ISPSSPM0_ISPSSC_OFFSET   0
+#define MRFLD_ISPSSPM0_ISPSSS_OFFSET   24
+#define MRFLD_ISPSSPM0_ISPSSC_MASK     0x3
+#define MRFLD_ISPSSPM0_IUNIT_POWER_ON  0
+#define MRFLD_ISPSSPM0_IUNIT_POWER_OFF 0x3
+#define MRFLD_ISPSSDVFS			0x13F
+#define MRFLD_BIT0			0x0001
+#define MRFLD_BIT1			0x0002
+
+/* MRFLD CSI lane configuration related */
+#define MRFLD_PORT_CONFIG_NUM  8
+#define MRFLD_PORT_NUM         3
+#define MRFLD_PORT1_ENABLE_SHIFT       0
+#define MRFLD_PORT2_ENABLE_SHIFT       1
+#define MRFLD_PORT3_ENABLE_SHIFT       2
+#define MRFLD_PORT1_LANES_SHIFT        3
+#define MRFLD_PORT2_LANES_SHIFT        7
+#define MRFLD_PORT3_LANES_SHIFT        8
+#define MRFLD_PORT_CONFIG_MASK 0x000f03ff
+#define MRFLD_PORT_CONFIGCODE_SHIFT    16
+#define MRFLD_ALL_CSI_PORTS_OFF_MASK   0x7
+
+#define CHV_PORT3_LANES_SHIFT		9
+#define CHV_PORT_CONFIG_MASK		0x1f07ff
+
+#define ISPSSPM1				0x3a
+#define ISP_FREQ_STAT_MASK			(0x1f << ISP_FREQ_STAT_OFFSET)
+#define ISP_REQ_FREQ_MASK			0x1f
+#define ISP_FREQ_VALID_MASK			(0x1 << ISP_FREQ_VALID_OFFSET)
+#define ISP_FREQ_STAT_OFFSET			0x18
+#define ISP_REQ_GUAR_FREQ_OFFSET		0x8
+#define ISP_REQ_FREQ_OFFSET			0x0
+#define ISP_FREQ_VALID_OFFSET			0x7
+#define ISP_FREQ_RULE_ANY			0x0
+
+#define ISP_FREQ_457MHZ				0x1C9
+#define ISP_FREQ_400MHZ				0x190
+#define ISP_FREQ_356MHZ				0x164
+#define ISP_FREQ_320MHZ				0x140
+#define ISP_FREQ_266MHZ				0x10a
+#define ISP_FREQ_200MHZ				0xc8
+#define ISP_FREQ_100MHZ				0x64
+
+#define HPLL_FREQ_800MHZ			0x320
+#define HPLL_FREQ_1600MHZ			0x640
+#define HPLL_FREQ_2000MHZ			0x7D0
+
+#define CCK_FUSE_REG_0			0x08
+#define CCK_FUSE_HPLL_FREQ_MASK		0x03
+
+/* ISP2401 CSI2+ receiver delay settings */
+#define CSI2_PORT_A_BASE					0xC0000
+#define CSI2_PORT_B_BASE					0xC2000
+#define CSI2_PORT_C_BASE					0xC4000
+
+#define CSI2_LANE_CL_BASE					0x418
+#define CSI2_LANE_D0_BASE					0x420
+#define CSI2_LANE_D1_BASE					0x428
+#define CSI2_LANE_D2_BASE					0x430
+#define CSI2_LANE_D3_BASE					0x438
+
+#define CSI2_REG_RX_CSI_DLY_CNT_TERMEN				0
+#define CSI2_REG_RX_CSI_DLY_CNT_SETTLE				0x4
+
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_TERMEN_CLANE			0xC0418
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_SETTLE_CLANE			0xC041C
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_TERMEN_DLANE0		0xC0420
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_SETTLE_DLANE0		0xC0424
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_TERMEN_DLANE1		0xC0428
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_SETTLE_DLANE1		0xC042C
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_TERMEN_DLANE2		0xC0430
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_SETTLE_DLANE2		0xC0434
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_TERMEN_DLANE3		0xC0438
+#define CSI2_PORT_A_RX_CSI_DLY_CNT_SETTLE_DLANE3		0xC043C
+
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_TERMEN_CLANE			0xC2418
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_SETTLE_CLANE			0xC241C
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_TERMEN_DLANE0		0xC2420
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_SETTLE_DLANE0		0xC2424
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_TERMEN_DLANE1		0xC2428
+#define CSI2_PORT_B_RX_CSI_DLY_CNT_SETTLE_DLANE1		0xC242C
+
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_TERMEN_CLANE			0xC4418
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_SETTLE_CLANE			0xC441C
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_TERMEN_DLANE0		0xC4420
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_SETTLE_DLANE0		0xC4424
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_TERMEN_DLANE1		0xC4428
+#define CSI2_PORT_C_RX_CSI_DLY_CNT_SETTLE_DLANE1		0xC442C
+
+#define DMA_BURST_SIZE_REG					0xCD408
+
+#define ISP_DFS_TRY_TIMES	2
+
+#endif /* ATOMISP_REGS_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_cmd.c b/drivers/staging/media/atomisp/pci/atomisp_cmd.c
new file mode 100644
index 000000000..c72d0e344
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_cmd.c
@@ -0,0 +1,6129 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/errno.h>
+#include <linux/firmware.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/kfifo.h>
+#include <linux/pm_runtime.h>
+#include <linux/timer.h>
+
+#include <asm/iosf_mbi.h>
+
+#include <media/v4l2-event.h>
+#include <media/videobuf-vmalloc.h>
+
+#define CREATE_TRACE_POINTS
+#include "atomisp_trace_event.h"
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_fops.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+#include "atomisp-regs.h"
+#include "atomisp_tables.h"
+#include "atomisp_compat.h"
+#include "atomisp_subdev.h"
+#include "atomisp_dfs_tables.h"
+
+#include <hmm/hmm.h>
+
+#include "sh_css_hrt.h"
+#include "sh_css_defs.h"
+#include "system_global.h"
+#include "sh_css_internal.h"
+#include "sh_css_sp.h"
+#include "gp_device.h"
+#include "device_access.h"
+#include "irq.h"
+
+#include "ia_css_types.h"
+#include "ia_css_stream.h"
+#include "ia_css_debug.h"
+#include "bits.h"
+
+/* We should never need to run the flash for more than 2 frames.
+ * At 15fps this means 133ms. We set the timeout a bit longer.
+ * Each flash driver is supposed to set its own timeout, but
+ * just in case someone else changed the timeout, we set it
+ * here to make sure we don't damage the flash hardware. */
+#define FLASH_TIMEOUT 800 /* ms */
+
+union host {
+	struct {
+		void *kernel_ptr;
+		void __user *user_ptr;
+		int size;
+	} scalar;
+	struct {
+		void *hmm_ptr;
+	} ptr;
+};
+
+static int atomisp_set_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id);
+
+/*
+ * get sensor:dis71430/ov2720 related info from v4l2_subdev->priv data field.
+ * subdev->priv is set in mrst.c
+ */
+struct camera_mipi_info *atomisp_to_sensor_mipi_info(struct v4l2_subdev *sd)
+{
+	return (struct camera_mipi_info *)v4l2_get_subdev_hostdata(sd);
+}
+
+/*
+ * get struct atomisp_video_pipe from v4l2 video_device
+ */
+struct atomisp_video_pipe *atomisp_to_video_pipe(struct video_device *dev)
+{
+	return (struct atomisp_video_pipe *)
+	       container_of(dev, struct atomisp_video_pipe, vdev);
+}
+
+static unsigned short atomisp_get_sensor_fps(struct atomisp_sub_device *asd)
+{
+	struct v4l2_subdev_frame_interval fi = { 0 };
+	struct atomisp_device *isp = asd->isp;
+
+	unsigned short fps = 0;
+	int ret;
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       video, g_frame_interval, &fi);
+
+	if (!ret && fi.interval.numerator)
+		fps = fi.interval.denominator / fi.interval.numerator;
+
+	return fps;
+}
+
+/*
+ * DFS progress is shown as follows:
+ * 1. Target frequency is calculated according to FPS/Resolution/ISP running
+ *    mode.
+ * 2. Ratio is calculated using formula: 2 * HPLL / target frequency - 1
+ *    with proper rounding.
+ * 3. Set ratio to ISPFREQ40, 1 to FREQVALID and ISPFREQGUAR40
+ *    to 200MHz in ISPSSPM1.
+ * 4. Wait for FREQVALID to be cleared by P-Unit.
+ * 5. Wait for field ISPFREQSTAT40 in ISPSSPM1 turn to ratio set in 3.
+ */
+static int write_target_freq_to_hw(struct atomisp_device *isp,
+				   unsigned int new_freq)
+{
+	unsigned int ratio, timeout, guar_ratio;
+	u32 isp_sspm1 = 0;
+	int i;
+
+	if (!isp->hpll_freq) {
+		dev_err(isp->dev, "failed to get hpll_freq. no change to freq\n");
+		return -EINVAL;
+	}
+
+	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+	if (isp_sspm1 & ISP_FREQ_VALID_MASK) {
+		dev_dbg(isp->dev, "clearing ISPSSPM1 valid bit.\n");
+		iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, ISPSSPM1,
+			       isp_sspm1 & ~(1 << ISP_FREQ_VALID_OFFSET));
+	}
+
+	ratio = (2 * isp->hpll_freq + new_freq / 2) / new_freq - 1;
+	guar_ratio = (2 * isp->hpll_freq + 200 / 2) / 200 - 1;
+
+	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+	isp_sspm1 &= ~(0x1F << ISP_REQ_FREQ_OFFSET);
+
+	for (i = 0; i < ISP_DFS_TRY_TIMES; i++) {
+		iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, ISPSSPM1,
+			       isp_sspm1
+			       | ratio << ISP_REQ_FREQ_OFFSET
+			       | 1 << ISP_FREQ_VALID_OFFSET
+			       | guar_ratio << ISP_REQ_GUAR_FREQ_OFFSET);
+
+		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+		timeout = 20;
+		while ((isp_sspm1 & ISP_FREQ_VALID_MASK) && timeout) {
+			iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+			dev_dbg(isp->dev, "waiting for ISPSSPM1 valid bit to be 0.\n");
+			udelay(100);
+			timeout--;
+		}
+
+		if (timeout != 0)
+			break;
+	}
+
+	if (timeout == 0) {
+		dev_err(isp->dev, "DFS failed due to HW error.\n");
+		return -EINVAL;
+	}
+
+	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+	timeout = 10;
+	while (((isp_sspm1 >> ISP_FREQ_STAT_OFFSET) != ratio) && timeout) {
+		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
+		dev_dbg(isp->dev, "waiting for ISPSSPM1 status bit to be 0x%x.\n",
+			new_freq);
+		udelay(100);
+		timeout--;
+	}
+	if (timeout == 0) {
+		dev_err(isp->dev, "DFS target freq is rejected by HW.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_freq_scaling(struct atomisp_device *isp,
+			 enum atomisp_dfs_mode mode,
+			 bool force)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	/* FIXME! Only use subdev[0] status yet */
+	struct atomisp_sub_device *asd = &isp->asd[0];
+	const struct atomisp_dfs_config *dfs;
+	unsigned int new_freq;
+	struct atomisp_freq_scaling_rule curr_rules;
+	int i, ret;
+	unsigned short fps = 0;
+
+	if (isp->sw_contex.power_state != ATOM_ISP_POWER_UP) {
+		dev_err(isp->dev, "DFS cannot proceed due to no power.\n");
+		return -EINVAL;
+	}
+
+	if ((pdev->device & ATOMISP_PCI_DEVICE_SOC_MASK) ==
+	    ATOMISP_PCI_DEVICE_SOC_CHT && ATOMISP_USE_YUVPP(asd))
+		isp->dfs = &dfs_config_cht_soc;
+
+	dfs = isp->dfs;
+
+	if (dfs->lowest_freq == 0 || dfs->max_freq_at_vmin == 0 ||
+	    dfs->highest_freq == 0 || dfs->dfs_table_size == 0 ||
+	    !dfs->dfs_table) {
+		dev_err(isp->dev, "DFS configuration is invalid.\n");
+		return -EINVAL;
+	}
+
+	if (mode == ATOMISP_DFS_MODE_LOW) {
+		new_freq = dfs->lowest_freq;
+		goto done;
+	}
+
+	if (mode == ATOMISP_DFS_MODE_MAX) {
+		new_freq = dfs->highest_freq;
+		goto done;
+	}
+
+	fps = atomisp_get_sensor_fps(asd);
+	if (fps == 0) {
+		dev_info(isp->dev,
+			 "Sensor didn't report FPS. Using DFS max mode.\n");
+		new_freq = dfs->highest_freq;
+		goto done;
+	}
+
+	curr_rules.width = asd->fmt[asd->capture_pad].fmt.width;
+	curr_rules.height = asd->fmt[asd->capture_pad].fmt.height;
+	curr_rules.fps = fps;
+	curr_rules.run_mode = asd->run_mode->val;
+	/*
+	 * For continuous mode, we need to make the capture setting applied
+	 * since preview mode, because there is no chance to do this when
+	 * starting image capture.
+	 */
+	if (asd->continuous_mode->val) {
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			curr_rules.run_mode = ATOMISP_RUN_MODE_SDV;
+		else
+			curr_rules.run_mode =
+			    ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE;
+	}
+
+	/* search for the target frequency by looping freq rules*/
+	for (i = 0; i < dfs->dfs_table_size; i++) {
+		if (curr_rules.width != dfs->dfs_table[i].width &&
+		    dfs->dfs_table[i].width != ISP_FREQ_RULE_ANY)
+			continue;
+		if (curr_rules.height != dfs->dfs_table[i].height &&
+		    dfs->dfs_table[i].height != ISP_FREQ_RULE_ANY)
+			continue;
+		if (curr_rules.fps != dfs->dfs_table[i].fps &&
+		    dfs->dfs_table[i].fps != ISP_FREQ_RULE_ANY)
+			continue;
+		if (curr_rules.run_mode != dfs->dfs_table[i].run_mode &&
+		    dfs->dfs_table[i].run_mode != ISP_FREQ_RULE_ANY)
+			continue;
+		break;
+	}
+
+	if (i == dfs->dfs_table_size)
+		new_freq = dfs->max_freq_at_vmin;
+	else
+		new_freq = dfs->dfs_table[i].isp_freq;
+
+done:
+	dev_dbg(isp->dev, "DFS target frequency=%d.\n", new_freq);
+
+	if ((new_freq == isp->sw_contex.running_freq) && !force)
+		return 0;
+
+	dev_dbg(isp->dev, "Programming DFS frequency to %d\n", new_freq);
+
+	ret = write_target_freq_to_hw(isp, new_freq);
+	if (!ret) {
+		isp->sw_contex.running_freq = new_freq;
+		trace_ipu_pstate(new_freq, -1);
+	}
+	return ret;
+}
+
+/*
+ * reset and restore ISP
+ */
+int atomisp_reset(struct atomisp_device *isp)
+{
+	/* Reset ISP by power-cycling it */
+	int ret = 0;
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+	atomisp_css_suspend(isp);
+	ret = atomisp_runtime_suspend(isp->dev);
+	if (ret < 0)
+		dev_err(isp->dev, "atomisp_runtime_suspend failed, %d\n", ret);
+	ret = atomisp_mrfld_power_down(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "can not disable ISP power\n");
+	} else {
+		ret = atomisp_mrfld_power_up(isp);
+		if (ret < 0)
+			dev_err(isp->dev, "can not enable ISP power\n");
+		ret = atomisp_runtime_resume(isp->dev);
+		if (ret < 0)
+			dev_err(isp->dev, "atomisp_runtime_resume failed, %d\n", ret);
+	}
+	ret = atomisp_css_resume(isp);
+	if (ret)
+		isp->isp_fatal_error = true;
+
+	return ret;
+}
+
+/*
+ * interrupt disable functions
+ */
+static void disable_isp_irq(enum hrt_isp_css_irq irq)
+{
+	irq_disable_channel(IRQ0_ID, irq);
+
+	if (irq != hrt_isp_css_irq_sp)
+		return;
+
+	cnd_sp_irq_enable(SP0_ID, false);
+}
+
+/*
+ * interrupt clean function
+ */
+static void clear_isp_irq(enum hrt_isp_css_irq irq)
+{
+	irq_clear_all(IRQ0_ID);
+}
+
+void atomisp_msi_irq_init(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	u32 msg32;
+	u16 msg16;
+
+	pci_read_config_dword(pdev, PCI_MSI_CAPID, &msg32);
+	msg32 |= 1 << MSI_ENABLE_BIT;
+	pci_write_config_dword(pdev, PCI_MSI_CAPID, msg32);
+
+	msg32 = (1 << INTR_IER) | (1 << INTR_IIR);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg32);
+
+	pci_read_config_word(pdev, PCI_COMMAND, &msg16);
+	msg16 |= (PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
+		  PCI_COMMAND_INTX_DISABLE);
+	pci_write_config_word(pdev, PCI_COMMAND, msg16);
+}
+
+void atomisp_msi_irq_uninit(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	u32 msg32;
+	u16 msg16;
+
+	pci_read_config_dword(pdev, PCI_MSI_CAPID, &msg32);
+	msg32 &=  ~(1 << MSI_ENABLE_BIT);
+	pci_write_config_dword(pdev, PCI_MSI_CAPID, msg32);
+
+	msg32 = 0x0;
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg32);
+
+	pci_read_config_word(pdev, PCI_COMMAND, &msg16);
+	msg16 &= ~(PCI_COMMAND_MASTER);
+	pci_write_config_word(pdev, PCI_COMMAND, msg16);
+}
+
+static void atomisp_sof_event(struct atomisp_sub_device *asd)
+{
+	struct v4l2_event event = {0};
+
+	event.type = V4L2_EVENT_FRAME_SYNC;
+	event.u.frame_sync.frame_sequence = atomic_read(&asd->sof_count);
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+void atomisp_eof_event(struct atomisp_sub_device *asd, uint8_t exp_id)
+{
+	struct v4l2_event event = {0};
+
+	event.type = V4L2_EVENT_FRAME_END;
+	event.u.frame_sync.frame_sequence = exp_id;
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+static void atomisp_3a_stats_ready_event(struct atomisp_sub_device *asd,
+	uint8_t exp_id)
+{
+	struct v4l2_event event = {0};
+
+	event.type = V4L2_EVENT_ATOMISP_3A_STATS_READY;
+	event.u.frame_sync.frame_sequence = exp_id;
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+static void atomisp_metadata_ready_event(struct atomisp_sub_device *asd,
+	enum atomisp_metadata_type md_type)
+{
+	struct v4l2_event event = {0};
+
+	event.type = V4L2_EVENT_ATOMISP_METADATA_READY;
+	event.u.data[0] = md_type;
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+static void atomisp_reset_event(struct atomisp_sub_device *asd)
+{
+	struct v4l2_event event = {0};
+
+	event.type = V4L2_EVENT_ATOMISP_CSS_RESET;
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+static void print_csi_rx_errors(enum mipi_port_id port,
+				struct atomisp_device *isp)
+{
+	u32 infos = 0;
+
+	atomisp_css_rx_get_irq_info(port, &infos);
+
+	dev_err(isp->dev, "CSI Receiver port %d errors:\n", port);
+	if (infos & IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
+		dev_err(isp->dev, "  buffer overrun");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT)
+		dev_err(isp->dev, "  start-of-transmission error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
+		dev_err(isp->dev, "  start-of-transmission sync error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CONTROL)
+		dev_err(isp->dev, "  control error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
+		dev_err(isp->dev, "  2 or more ECC errors");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CRC)
+		dev_err(isp->dev, "  CRC mismatch");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
+		dev_err(isp->dev, "  unknown error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
+		dev_err(isp->dev, "  frame sync error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
+		dev_err(isp->dev, "  frame data error");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
+		dev_err(isp->dev, "  data timeout");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
+		dev_err(isp->dev, "  unknown escape command entry");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
+		dev_err(isp->dev, "  line sync error");
+}
+
+/* Clear irq reg */
+static void clear_irq_reg(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	u32 msg_ret;
+
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &msg_ret);
+	msg_ret |= 1 << INTR_IIR;
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg_ret);
+}
+
+static struct atomisp_sub_device *
+__get_asd_from_port(struct atomisp_device *isp, enum mipi_port_id port)
+{
+	int i;
+
+	/* Check which isp subdev to send eof */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		struct camera_mipi_info *mipi_info;
+
+		mipi_info = atomisp_to_sensor_mipi_info(
+				isp->inputs[asd->input_curr].camera);
+
+		if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED &&
+		    __get_mipi_port(isp, mipi_info->port) == port) {
+			return asd;
+		}
+	}
+
+	return NULL;
+}
+
+/* interrupt handling function*/
+irqreturn_t atomisp_isr(int irq, void *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)dev;
+	struct atomisp_sub_device *asd;
+	struct atomisp_css_event eof_event;
+	unsigned int irq_infos = 0;
+	unsigned long flags;
+	unsigned int i;
+	int err;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (isp->sw_contex.power_state != ATOM_ISP_POWER_UP ||
+	    !isp->css_initialized) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return IRQ_HANDLED;
+	}
+	err = atomisp_css_irq_translate(isp, &irq_infos);
+	if (err) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return IRQ_NONE;
+	}
+
+	clear_irq_reg(isp);
+
+	if (!atomisp_streaming_count(isp))
+		goto out_nowake;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+			continue;
+		/*
+		 * Current SOF only support one stream, so the SOF only valid
+		 * either solely one stream is running
+		 */
+		if (irq_infos & IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF) {
+			atomic_inc(&asd->sof_count);
+			atomisp_sof_event(asd);
+
+			/* If sequence_temp and sequence are the same
+			 * there where no frames lost so we can increase
+			 * sequence_temp.
+			 * If not then processing of frame is still in progress
+			 * and driver needs to keep old sequence_temp value.
+			 * NOTE: There is assumption here that ISP will not
+			 * start processing next frame from sensor before old
+			 * one is completely done. */
+			if (atomic_read(&asd->sequence) == atomic_read(
+				&asd->sequence_temp))
+				atomic_set(&asd->sequence_temp,
+					   atomic_read(&asd->sof_count));
+		}
+		if (irq_infos & IA_CSS_IRQ_INFO_EVENTS_READY)
+			atomic_set(&asd->sequence,
+				   atomic_read(&asd->sequence_temp));
+	}
+
+	if (irq_infos & IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF) {
+		dev_dbg_ratelimited(isp->dev,
+				    "irq:0x%x (SOF)\n",
+				    irq_infos);
+		irq_infos &= ~IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF;
+	}
+
+	if ((irq_infos & IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR) ||
+	    (irq_infos & IA_CSS_IRQ_INFO_IF_ERROR)) {
+		/* handle mipi receiver error */
+		u32 rx_infos;
+		enum mipi_port_id port;
+
+		for (port = MIPI_PORT0_ID; port <= MIPI_PORT2_ID;
+		     port++) {
+			print_csi_rx_errors(port, isp);
+			atomisp_css_rx_get_irq_info(port, &rx_infos);
+			atomisp_css_rx_clear_irq_info(port, rx_infos);
+		}
+	}
+
+	if (irq_infos & IA_CSS_IRQ_INFO_ISYS_EVENTS_READY) {
+		while (ia_css_dequeue_isys_event(&eof_event.event) ==
+		       0) {
+			/* EOF Event does not have the css_pipe returned */
+			asd = __get_asd_from_port(isp, eof_event.event.port);
+			if (!asd) {
+				dev_err(isp->dev, "%s: ISYS event, but no subdev.event:%d",
+					__func__, eof_event.event.type);
+				continue;
+			}
+
+			atomisp_eof_event(asd, eof_event.event.exp_id);
+			dev_dbg_ratelimited(isp->dev,
+					    "%s ISYS event: EOF exp_id %d, asd %d\n",
+					    __func__, eof_event.event.exp_id,
+					    asd->index);
+		}
+
+		irq_infos &= ~IA_CSS_IRQ_INFO_ISYS_EVENTS_READY;
+		if (irq_infos == 0)
+			goto out_nowake;
+	}
+
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	dev_dbg_ratelimited(isp->dev, "irq:0x%x (unhandled)\n", irq_infos);
+
+	return IRQ_WAKE_THREAD;
+
+out_nowake:
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	if (irq_infos)
+		dev_dbg_ratelimited(isp->dev, "irq:0x%x (ignored, as not streaming anymore)\n",
+				    irq_infos);
+
+	return IRQ_HANDLED;
+}
+
+void atomisp_clear_css_buffer_counters(struct atomisp_sub_device *asd)
+{
+	int i;
+
+	memset(asd->s3a_bufs_in_css, 0, sizeof(asd->s3a_bufs_in_css));
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++)
+		memset(asd->metadata_bufs_in_css[i], 0,
+		       sizeof(asd->metadata_bufs_in_css[i]));
+	asd->dis_bufs_in_css = 0;
+	asd->video_out_capture.buffers_in_css = 0;
+	asd->video_out_vf.buffers_in_css = 0;
+	asd->video_out_preview.buffers_in_css = 0;
+	asd->video_out_video_capture.buffers_in_css = 0;
+}
+
+/* ISP2400 */
+bool atomisp_buffers_queued(struct atomisp_sub_device *asd)
+{
+	return asd->video_out_capture.buffers_in_css ||
+	       asd->video_out_vf.buffers_in_css ||
+	       asd->video_out_preview.buffers_in_css ||
+	       asd->video_out_video_capture.buffers_in_css;
+}
+
+/* ISP2401 */
+bool atomisp_buffers_queued_pipe(struct atomisp_video_pipe *pipe)
+{
+	return pipe->buffers_in_css ? true : false;
+}
+
+/* 0x100000 is the start of dmem inside SP */
+#define SP_DMEM_BASE	0x100000
+
+void dump_sp_dmem(struct atomisp_device *isp, unsigned int addr,
+		  unsigned int size)
+{
+	unsigned int data = 0;
+	unsigned int size32 = DIV_ROUND_UP(size, sizeof(u32));
+
+	dev_dbg(isp->dev, "atomisp mmio base: %p\n", isp->base);
+	dev_dbg(isp->dev, "%s, addr:0x%x, size: %d, size32: %d\n", __func__,
+		addr, size, size32);
+	if (size32 * 4 + addr > 0x4000) {
+		dev_err(isp->dev, "illegal size (%d) or addr (0x%x)\n",
+			size32, addr);
+		return;
+	}
+	addr += SP_DMEM_BASE;
+	addr &= 0x003FFFFF;
+	do {
+		data = readl(isp->base + addr);
+		dev_dbg(isp->dev, "%s, \t [0x%x]:0x%x\n", __func__, addr, data);
+		addr += sizeof(u32);
+	} while (--size32);
+}
+
+static struct videobuf_buffer *atomisp_css_frame_to_vbuf(
+    struct atomisp_video_pipe *pipe, struct ia_css_frame *frame)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct ia_css_frame *handle;
+	int i;
+
+	for (i = 0; pipe->capq.bufs[i]; i++) {
+		vm_mem = pipe->capq.bufs[i]->priv;
+		handle = vm_mem->vaddr;
+		if (handle && handle->data == frame->data)
+			return pipe->capq.bufs[i];
+	}
+
+	return NULL;
+}
+
+static void atomisp_flush_video_pipe(struct atomisp_sub_device *asd,
+				     struct atomisp_video_pipe *pipe)
+{
+	unsigned long irqflags;
+	int i;
+
+	if (!pipe->users)
+		return;
+
+	for (i = 0; pipe->capq.bufs[i]; i++) {
+		spin_lock_irqsave(&pipe->irq_lock, irqflags);
+		if (pipe->capq.bufs[i]->state == VIDEOBUF_ACTIVE ||
+		    pipe->capq.bufs[i]->state == VIDEOBUF_QUEUED) {
+			pipe->capq.bufs[i]->ts = ktime_get_ns();
+			pipe->capq.bufs[i]->field_count =
+			    atomic_read(&asd->sequence) << 1;
+			dev_dbg(asd->isp->dev, "release buffers on device %s\n",
+				pipe->vdev.name);
+			if (pipe->capq.bufs[i]->state == VIDEOBUF_QUEUED)
+				list_del_init(&pipe->capq.bufs[i]->queue);
+			pipe->capq.bufs[i]->state = VIDEOBUF_ERROR;
+			wake_up(&pipe->capq.bufs[i]->done);
+		}
+		spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+	}
+}
+
+/* Returns queued buffers back to video-core */
+void atomisp_flush_bufs_and_wakeup(struct atomisp_sub_device *asd)
+{
+	atomisp_flush_video_pipe(asd, &asd->video_out_capture);
+	atomisp_flush_video_pipe(asd, &asd->video_out_vf);
+	atomisp_flush_video_pipe(asd, &asd->video_out_preview);
+	atomisp_flush_video_pipe(asd, &asd->video_out_video_capture);
+}
+
+/* clean out the parameters that did not apply */
+void atomisp_flush_params_queue(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_css_params_with_list *param;
+
+	while (!list_empty(&pipe->per_frame_params)) {
+		param = list_entry(pipe->per_frame_params.next,
+				   struct atomisp_css_params_with_list, list);
+		list_del(&param->list);
+		atomisp_free_css_parameters(&param->params);
+		kvfree(param);
+	}
+}
+
+/* Re-queue per-frame parameters */
+static void atomisp_recover_params_queue(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_css_params_with_list *param;
+	int i;
+
+	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+		param = pipe->frame_params[i];
+		if (param)
+			list_add_tail(&param->list, &pipe->per_frame_params);
+		pipe->frame_params[i] = NULL;
+	}
+	atomisp_handle_parameter_and_buffer(pipe);
+}
+
+/* find atomisp_video_pipe with css pipe id, buffer type and atomisp run_mode */
+static struct atomisp_video_pipe *__atomisp_get_pipe(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    enum ia_css_pipe_id css_pipe_id,
+    enum ia_css_buffer_type buf_type)
+{
+	/* video is same in online as in continuouscapture mode */
+	if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+		/*
+		 * Disable vf_pp and run CSS in still capture mode. In this
+		 * mode, CSS does not cause extra latency with buffering, but
+		 * scaling is not available.
+		 */
+		return &asd->video_out_capture;
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+		/*
+		 * Disable vf_pp and run CSS in video mode. This allows using
+		 * ISP scaling but it has one frame delay due to CSS internal
+		 * buffering.
+		 */
+		return &asd->video_out_video_capture;
+	} else if (css_pipe_id == IA_CSS_PIPE_ID_YUVPP) {
+		/*
+		 * to SOC camera, yuvpp pipe is run for capture/video/SDV/ZSL.
+		 */
+		if (asd->continuous_mode->val) {
+			if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+				/* SDV case */
+				switch (buf_type) {
+				case IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+					return &asd->video_out_video_capture;
+				case IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
+					return &asd->video_out_preview;
+				case IA_CSS_BUFFER_TYPE_OUTPUT_FRAME:
+					return &asd->video_out_capture;
+				default:
+					return &asd->video_out_vf;
+				}
+			} else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
+				/* ZSL case */
+				switch (buf_type) {
+				case IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+					return &asd->video_out_preview;
+				case IA_CSS_BUFFER_TYPE_OUTPUT_FRAME:
+					return &asd->video_out_capture;
+				default:
+					return &asd->video_out_vf;
+				}
+			}
+		} else if (buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME) {
+			switch (asd->run_mode->val) {
+			case ATOMISP_RUN_MODE_VIDEO:
+				return &asd->video_out_video_capture;
+			case ATOMISP_RUN_MODE_PREVIEW:
+				return &asd->video_out_preview;
+			default:
+				return &asd->video_out_capture;
+			}
+		} else if (buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
+			if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+				return &asd->video_out_preview;
+			else
+				return &asd->video_out_vf;
+		}
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+		/* For online video or SDV video pipe. */
+		if (css_pipe_id == IA_CSS_PIPE_ID_VIDEO ||
+		    css_pipe_id == IA_CSS_PIPE_ID_COPY ||
+		    css_pipe_id == IA_CSS_PIPE_ID_YUVPP) {
+			if (buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME)
+				return &asd->video_out_video_capture;
+			return &asd->video_out_preview;
+		}
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
+		/* For online preview or ZSL preview pipe. */
+		if (css_pipe_id == IA_CSS_PIPE_ID_PREVIEW ||
+		    css_pipe_id == IA_CSS_PIPE_ID_COPY ||
+		    css_pipe_id == IA_CSS_PIPE_ID_YUVPP)
+			return &asd->video_out_preview;
+	}
+	/* For capture pipe. */
+	if (buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME)
+		return &asd->video_out_vf;
+	return &asd->video_out_capture;
+}
+
+enum atomisp_metadata_type
+atomisp_get_metadata_type(struct atomisp_sub_device *asd,
+			  enum ia_css_pipe_id pipe_id)
+{
+	if (!asd->continuous_mode->val)
+		return ATOMISP_MAIN_METADATA;
+
+	if (pipe_id == IA_CSS_PIPE_ID_CAPTURE) /* online capture pipe */
+		return ATOMISP_SEC_METADATA;
+	else
+		return ATOMISP_MAIN_METADATA;
+}
+
+void atomisp_buf_done(struct atomisp_sub_device *asd, int error,
+		      enum ia_css_buffer_type buf_type,
+		      enum ia_css_pipe_id css_pipe_id,
+		      bool q_buffers, enum atomisp_input_stream_id stream_id)
+{
+	struct videobuf_buffer *vb = NULL;
+	struct atomisp_video_pipe *pipe = NULL;
+	struct atomisp_css_buffer buffer;
+	bool requeue = false;
+	int err;
+	unsigned long irqflags;
+	struct ia_css_frame *frame = NULL;
+	struct atomisp_s3a_buf *s3a_buf = NULL, *_s3a_buf_tmp, *s3a_iter;
+	struct atomisp_dis_buf *dis_buf = NULL, *_dis_buf_tmp, *dis_iter;
+	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf_tmp, *md_iter;
+	enum atomisp_metadata_type md_type;
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_control ctrl;
+
+	lockdep_assert_held(&isp->mutex);
+
+	if (
+	    buf_type != IA_CSS_BUFFER_TYPE_METADATA &&
+	    buf_type != IA_CSS_BUFFER_TYPE_3A_STATISTICS &&
+	    buf_type != IA_CSS_BUFFER_TYPE_DIS_STATISTICS &&
+	    buf_type != IA_CSS_BUFFER_TYPE_OUTPUT_FRAME &&
+	    buf_type != IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
+	    buf_type != IA_CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME &&
+	    buf_type != IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
+	    buf_type != IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
+		dev_err(isp->dev, "%s, unsupported buffer type: %d\n",
+			__func__, buf_type);
+		return;
+	}
+
+	memset(&buffer, 0, sizeof(struct atomisp_css_buffer));
+	buffer.css_buffer.type = buf_type;
+	err = atomisp_css_dequeue_buffer(asd, stream_id, css_pipe_id,
+					 buf_type, &buffer);
+	if (err) {
+		dev_err(isp->dev,
+			"atomisp_css_dequeue_buffer failed: 0x%x\n", err);
+		return;
+	}
+
+	/* need to know the atomisp pipe for frame buffers */
+	pipe = __atomisp_get_pipe(asd, stream_id, css_pipe_id, buf_type);
+	if (!pipe) {
+		dev_err(isp->dev, "error getting atomisp pipe\n");
+		return;
+	}
+
+	switch (buf_type) {
+	case IA_CSS_BUFFER_TYPE_3A_STATISTICS:
+		list_for_each_entry_safe(s3a_iter, _s3a_buf_tmp,
+					 &asd->s3a_stats_in_css, list) {
+			if (s3a_iter->s3a_data ==
+			    buffer.css_buffer.data.stats_3a) {
+				list_del_init(&s3a_iter->list);
+				list_add_tail(&s3a_iter->list,
+					      &asd->s3a_stats_ready);
+				s3a_buf = s3a_iter;
+				break;
+			}
+		}
+
+		asd->s3a_bufs_in_css[css_pipe_id]--;
+		atomisp_3a_stats_ready_event(asd, buffer.css_buffer.exp_id);
+		if (s3a_buf)
+			dev_dbg(isp->dev, "%s: s3a stat with exp_id %d is ready\n",
+				__func__, s3a_buf->s3a_data->exp_id);
+		else
+			dev_dbg(isp->dev, "%s: s3a stat is ready with no exp_id found\n",
+				__func__);
+		break;
+	case IA_CSS_BUFFER_TYPE_METADATA:
+		if (error)
+			break;
+
+		md_type = atomisp_get_metadata_type(asd, css_pipe_id);
+		list_for_each_entry_safe(md_iter, _md_buf_tmp,
+					 &asd->metadata_in_css[md_type], list) {
+			if (md_iter->metadata ==
+			    buffer.css_buffer.data.metadata) {
+				list_del_init(&md_iter->list);
+				list_add_tail(&md_iter->list,
+					      &asd->metadata_ready[md_type]);
+				md_buf = md_iter;
+				break;
+			}
+		}
+		asd->metadata_bufs_in_css[stream_id][css_pipe_id]--;
+		atomisp_metadata_ready_event(asd, md_type);
+		if (md_buf)
+			dev_dbg(isp->dev, "%s: metadata with exp_id %d is ready\n",
+				__func__, md_buf->metadata->exp_id);
+		else
+			dev_dbg(isp->dev, "%s: metadata is ready with no exp_id found\n",
+				__func__);
+		break;
+	case IA_CSS_BUFFER_TYPE_DIS_STATISTICS:
+		list_for_each_entry_safe(dis_iter, _dis_buf_tmp,
+					 &asd->dis_stats_in_css, list) {
+			if (dis_iter->dis_data ==
+			    buffer.css_buffer.data.stats_dvs) {
+				spin_lock_irqsave(&asd->dis_stats_lock,
+						  irqflags);
+				list_del_init(&dis_iter->list);
+				list_add(&dis_iter->list, &asd->dis_stats);
+				asd->params.dis_proj_data_valid = true;
+				spin_unlock_irqrestore(&asd->dis_stats_lock,
+						       irqflags);
+				dis_buf = dis_iter;
+				break;
+			}
+		}
+		asd->dis_bufs_in_css--;
+		if (dis_buf)
+			dev_dbg(isp->dev, "%s: dis stat with exp_id %d is ready\n",
+				__func__, dis_buf->dis_data->exp_id);
+		else
+			dev_dbg(isp->dev, "%s: dis stat is ready with no exp_id found\n",
+				__func__);
+		break;
+	case IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME:
+	case IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
+		pipe->buffers_in_css--;
+		frame = buffer.css_buffer.data.frame;
+		if (!frame) {
+			WARN_ON(1);
+			break;
+		}
+		if (!frame->valid)
+			error = true;
+
+		/* FIXME:
+		 * YUVPP doesn't set postview exp_id correctlly in SDV mode.
+		 * This is a WORKAROUND to set exp_id. see HSDES-1503911606.
+		 */
+		if (IS_BYT && buf_type == IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
+		    asd->continuous_mode->val && ATOMISP_USE_YUVPP(asd))
+			frame->exp_id = (asd->postview_exp_id++) %
+					(ATOMISP_MAX_EXP_ID + 1);
+
+		dev_dbg(isp->dev, "%s: vf frame with exp_id %d is ready\n",
+			__func__, frame->exp_id);
+		if (asd->params.flash_state == ATOMISP_FLASH_ONGOING) {
+			if (frame->flash_state
+			    == IA_CSS_FRAME_FLASH_STATE_PARTIAL)
+				dev_dbg(isp->dev, "%s thumb partially flashed\n",
+					__func__);
+			else if (frame->flash_state
+				 == IA_CSS_FRAME_FLASH_STATE_FULL)
+				dev_dbg(isp->dev, "%s thumb completely flashed\n",
+					__func__);
+			else
+				dev_dbg(isp->dev, "%s thumb no flash in this frame\n",
+					__func__);
+		}
+		vb = atomisp_css_frame_to_vbuf(pipe, frame);
+		WARN_ON(!vb);
+		if (vb)
+			pipe->frame_config_id[vb->i] = frame->isp_config_id;
+		if (css_pipe_id == IA_CSS_PIPE_ID_CAPTURE &&
+		    asd->pending_capture_request > 0) {
+			err = atomisp_css_offline_capture_configure(asd,
+				asd->params.offline_parm.num_captures,
+				asd->params.offline_parm.skip_frames,
+				asd->params.offline_parm.offset);
+
+			asd->pending_capture_request--;
+
+			dev_dbg(isp->dev, "Trigger capture again for new buffer. err=%d\n",
+				err);
+		}
+		break;
+	case IA_CSS_BUFFER_TYPE_OUTPUT_FRAME:
+	case IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+		pipe->buffers_in_css--;
+		frame = buffer.css_buffer.data.frame;
+		if (!frame) {
+			WARN_ON(1);
+			break;
+		}
+
+		if (!frame->valid)
+			error = true;
+
+		/* FIXME:
+		 * YUVPP doesn't set preview exp_id correctlly in ZSL mode.
+		 * This is a WORKAROUND to set exp_id. see HSDES-1503911606.
+		 */
+		if (IS_BYT && buf_type == IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
+		    asd->continuous_mode->val && ATOMISP_USE_YUVPP(asd))
+			frame->exp_id = (asd->preview_exp_id++) %
+					(ATOMISP_MAX_EXP_ID + 1);
+
+		dev_dbg(isp->dev, "%s: main frame with exp_id %d is ready\n",
+			__func__, frame->exp_id);
+		vb = atomisp_css_frame_to_vbuf(pipe, frame);
+		if (!vb) {
+			WARN_ON(1);
+			break;
+		}
+
+		/* free the parameters */
+		if (pipe->frame_params[vb->i]) {
+			if (asd->params.dvs_6axis ==
+			    pipe->frame_params[vb->i]->params.dvs_6axis)
+				asd->params.dvs_6axis = NULL;
+			atomisp_free_css_parameters(
+			    &pipe->frame_params[vb->i]->params);
+			kvfree(pipe->frame_params[vb->i]);
+			pipe->frame_params[vb->i] = NULL;
+		}
+
+		pipe->frame_config_id[vb->i] = frame->isp_config_id;
+		ctrl.id = V4L2_CID_FLASH_MODE;
+		if (asd->params.flash_state == ATOMISP_FLASH_ONGOING) {
+			if (frame->flash_state
+			    == IA_CSS_FRAME_FLASH_STATE_PARTIAL) {
+				asd->frame_status[vb->i] =
+				    ATOMISP_FRAME_STATUS_FLASH_PARTIAL;
+				dev_dbg(isp->dev, "%s partially flashed\n",
+					__func__);
+			} else if (frame->flash_state
+				   == IA_CSS_FRAME_FLASH_STATE_FULL) {
+				asd->frame_status[vb->i] =
+				    ATOMISP_FRAME_STATUS_FLASH_EXPOSED;
+				asd->params.num_flash_frames--;
+				dev_dbg(isp->dev, "%s completely flashed\n",
+					__func__);
+			} else {
+				asd->frame_status[vb->i] =
+				    ATOMISP_FRAME_STATUS_OK;
+				dev_dbg(isp->dev,
+					"%s no flash in this frame\n",
+					__func__);
+			}
+
+			/* Check if flashing sequence is done */
+			if (asd->frame_status[vb->i] ==
+			    ATOMISP_FRAME_STATUS_FLASH_EXPOSED)
+				asd->params.flash_state = ATOMISP_FLASH_DONE;
+		} else if (isp->flash) {
+			if (v4l2_g_ctrl(isp->flash->ctrl_handler, &ctrl) ==
+			    0 && ctrl.value == ATOMISP_FLASH_MODE_TORCH) {
+				ctrl.id = V4L2_CID_FLASH_TORCH_INTENSITY;
+				if (v4l2_g_ctrl(isp->flash->ctrl_handler, &ctrl)
+				    == 0 && ctrl.value > 0) {
+					asd->frame_status[vb->i] =
+					    ATOMISP_FRAME_STATUS_FLASH_EXPOSED;
+				} else {
+					asd->frame_status[vb->i] =
+					    ATOMISP_FRAME_STATUS_OK;
+				}
+			} else {
+				asd->frame_status[vb->i] =
+				    ATOMISP_FRAME_STATUS_OK;
+			}
+		} else {
+			asd->frame_status[vb->i] = ATOMISP_FRAME_STATUS_OK;
+		}
+
+		asd->params.last_frame_status = asd->frame_status[vb->i];
+
+		if (asd->continuous_mode->val) {
+			if (css_pipe_id == IA_CSS_PIPE_ID_PREVIEW ||
+			    css_pipe_id == IA_CSS_PIPE_ID_VIDEO) {
+				asd->latest_preview_exp_id = frame->exp_id;
+			} else if (css_pipe_id ==
+				   IA_CSS_PIPE_ID_CAPTURE) {
+				if (asd->run_mode->val ==
+				    ATOMISP_RUN_MODE_VIDEO)
+					dev_dbg(isp->dev, "SDV capture raw buffer id: %u\n",
+						frame->exp_id);
+				else
+					dev_dbg(isp->dev, "ZSL capture raw buffer id: %u\n",
+						frame->exp_id);
+			}
+		}
+		/*
+		 * Only after enabled the raw buffer lock
+		 * and in continuous mode.
+		 * in preview/video pipe, each buffer will
+		 * be locked automatically, so record it here.
+		 */
+		if (((css_pipe_id == IA_CSS_PIPE_ID_PREVIEW) ||
+		     (css_pipe_id == IA_CSS_PIPE_ID_VIDEO)) &&
+		    asd->enable_raw_buffer_lock->val &&
+		    asd->continuous_mode->val) {
+			atomisp_set_raw_buffer_bitmap(asd, frame->exp_id);
+			WARN_ON(frame->exp_id > ATOMISP_MAX_EXP_ID);
+		}
+
+		if (asd->params.css_update_params_needed) {
+			atomisp_apply_css_parameters(asd,
+						     &asd->params.css_param);
+			if (asd->params.css_param.update_flag.dz_config)
+				asd->params.config.dz_config = &asd->params.css_param.dz_config;
+			/* New global dvs 6axis config should be blocked
+			 * here if there's a buffer with per-frame parameters
+			 * pending in CSS frame buffer queue.
+			 * This is to aviod zooming vibration since global
+			 * parameters take effect immediately while
+			 * per-frame parameters are taken after previous
+			 * buffers in CSS got processed.
+			 */
+			if (asd->params.dvs_6axis)
+				atomisp_css_set_dvs_6axis(asd,
+							  asd->params.dvs_6axis);
+			else
+				asd->params.css_update_params_needed = false;
+			/* The update flag should not be cleaned here
+			 * since it is still going to be used to make up
+			 * following per-frame parameters.
+			 * This will introduce more copy work since each
+			 * time when updating global parameters, the whole
+			 * parameter set are applied.
+			 * FIXME: A new set of parameter copy functions can
+			 * be added to make up per-frame parameters based on
+			 * solid structures stored in asd->params.css_param
+			 * instead of using shadow pointers in update flag.
+			 */
+			atomisp_css_update_isp_params(asd);
+		}
+		break;
+	default:
+		break;
+	}
+	if (vb) {
+		vb->ts = ktime_get_ns();
+		vb->field_count = atomic_read(&asd->sequence) << 1;
+		/*mark videobuffer done for dequeue*/
+		spin_lock_irqsave(&pipe->irq_lock, irqflags);
+		vb->state = !error ? VIDEOBUF_DONE : VIDEOBUF_ERROR;
+		spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+
+		/*
+		 * Frame capture done, wake up any process block on
+		 * current active buffer
+		 * possibly hold by videobuf_dqbuf()
+		 */
+		wake_up(&vb->done);
+	}
+
+	/*
+	 * Requeue should only be done for 3a and dis buffers.
+	 * Queue/dequeue order will change if driver recycles image buffers.
+	 */
+	if (requeue) {
+		err = atomisp_css_queue_buffer(asd,
+					       stream_id, css_pipe_id,
+					       buf_type, &buffer);
+		if (err)
+			dev_err(isp->dev, "%s, q to css fails: %d\n",
+				__func__, err);
+		return;
+	}
+	if (!error && q_buffers)
+		atomisp_qbuffers_to_css(asd);
+}
+
+void atomisp_delayed_init_work(struct work_struct *work)
+{
+	struct atomisp_sub_device *asd = container_of(work,
+					 struct atomisp_sub_device,
+					 delayed_init_work);
+	/*
+	 * to SOC camera, use yuvpp pipe and no support continuous mode.
+	 */
+	if (!ATOMISP_USE_YUVPP(asd)) {
+		struct v4l2_event event = {0};
+		struct ia_css_stream *stream;
+
+		stream = asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
+
+
+		if (ia_css_alloc_continuous_frame_remain(stream))
+			return;
+
+		ia_css_update_continuous_frames(stream);
+
+		event.type = V4L2_EVENT_ATOMISP_RAW_BUFFERS_ALLOC_DONE;
+		v4l2_event_queue(asd->subdev.devnode, &event);
+	}
+
+	/* signal streamon after delayed init is done */
+	asd->delayed_init = ATOMISP_DELAYED_INIT_DONE;
+	complete(&asd->init_done);
+}
+
+static void __atomisp_css_recover(struct atomisp_device *isp, bool isp_timeout)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	enum ia_css_pipe_id css_pipe_id;
+	bool stream_restart[MAX_STREAM_NUM] = {0};
+	bool depth_mode = false;
+	int i, ret, depth_cnt = 0;
+	unsigned long flags;
+
+	lockdep_assert_held(&isp->mutex);
+
+	if (!atomisp_streaming_count(isp))
+		return;
+
+	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF, false);
+
+	BUG_ON(isp->num_of_streams > MAX_STREAM_NUM);
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED &&
+		    !asd->stream_prepared)
+			continue;
+
+		depth_cnt++;
+
+		if (asd->delayed_init == ATOMISP_DELAYED_INIT_QUEUED)
+			cancel_work_sync(&asd->delayed_init_work);
+
+		complete(&asd->init_done);
+		asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
+
+		stream_restart[asd->index] = true;
+
+		spin_lock_irqsave(&isp->lock, flags);
+		asd->streaming = ATOMISP_DEVICE_STREAMING_STOPPING;
+		spin_unlock_irqrestore(&isp->lock, flags);
+
+		/* stream off sensor */
+		ret = v4l2_subdev_call(
+			  isp->inputs[asd->input_curr].
+			  camera, video, s_stream, 0);
+		if (ret)
+			dev_warn(isp->dev,
+				 "can't stop streaming on sensor!\n");
+
+		atomisp_clear_css_buffer_counters(asd);
+
+		css_pipe_id = atomisp_get_css_pipe_id(asd);
+		atomisp_css_stop(asd, css_pipe_id, true);
+
+		spin_lock_irqsave(&isp->lock, flags);
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+		spin_unlock_irqrestore(&isp->lock, flags);
+
+		asd->preview_exp_id = 1;
+		asd->postview_exp_id = 1;
+		/* notify HAL the CSS reset */
+		dev_dbg(isp->dev,
+			"send reset event to %s\n", asd->subdev.devnode->name);
+		atomisp_reset_event(asd);
+	}
+
+	/* clear irq */
+	disable_isp_irq(hrt_isp_css_irq_sp);
+	clear_isp_irq(hrt_isp_css_irq_sp);
+
+	/* Set the SRSE to 3 before resetting */
+	pci_write_config_dword(pdev, PCI_I_CONTROL,
+			       isp->saved_regs.i_control | MRFLD_PCI_I_CONTROL_SRSE_RESET_MASK);
+
+	/* reset ISP and restore its state */
+	isp->isp_timeout = true;
+	atomisp_reset(isp);
+	isp->isp_timeout = false;
+
+	if (!isp_timeout) {
+		for (i = 0; i < isp->num_of_streams; i++) {
+			if (isp->asd[i].depth_mode->val)
+				return;
+		}
+	}
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		if (!stream_restart[i])
+			continue;
+
+		if (isp->inputs[asd->input_curr].type != FILE_INPUT)
+			atomisp_css_input_set_mode(asd,
+						   IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
+
+		css_pipe_id = atomisp_get_css_pipe_id(asd);
+		if (atomisp_css_start(asd, css_pipe_id, true)) {
+			dev_warn(isp->dev,
+				 "start SP failed, so do not set streaming to be enable!\n");
+		} else {
+			spin_lock_irqsave(&isp->lock, flags);
+			asd->streaming = ATOMISP_DEVICE_STREAMING_ENABLED;
+			spin_unlock_irqrestore(&isp->lock, flags);
+		}
+
+		atomisp_csi2_configure(asd);
+	}
+
+	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF,
+			       atomisp_css_valid_sof(isp));
+
+	if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_AUTO, true) < 0)
+		dev_dbg(isp->dev, "DFS auto failed while recovering!\n");
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd;
+
+		asd = &isp->asd[i];
+
+		if (!stream_restart[i])
+			continue;
+
+		if (asd->continuous_mode->val &&
+		    asd->delayed_init == ATOMISP_DELAYED_INIT_NOT_QUEUED) {
+			reinit_completion(&asd->init_done);
+			asd->delayed_init = ATOMISP_DELAYED_INIT_QUEUED;
+			queue_work(asd->delayed_init_workq,
+				   &asd->delayed_init_work);
+		}
+		/*
+		 * dequeueing buffers is not needed. CSS will recycle
+		 * buffers that it has.
+		 */
+		atomisp_flush_bufs_and_wakeup(asd);
+
+		/* Requeue unprocessed per-frame parameters. */
+		atomisp_recover_params_queue(&asd->video_out_capture);
+		atomisp_recover_params_queue(&asd->video_out_preview);
+		atomisp_recover_params_queue(&asd->video_out_video_capture);
+
+		if ((asd->depth_mode->val) &&
+		    (depth_cnt == ATOMISP_DEPTH_SENSOR_STREAMON_COUNT)) {
+			depth_mode = true;
+			continue;
+		}
+
+		ret = v4l2_subdev_call(
+			  isp->inputs[asd->input_curr].camera, video,
+			  s_stream, 1);
+		if (ret)
+			dev_warn(isp->dev,
+				 "can't start streaming on sensor!\n");
+	}
+
+	if (depth_mode) {
+		if (atomisp_stream_on_master_slave_sensor(isp, true))
+			dev_warn(isp->dev,
+				 "master slave sensor stream on failed!\n");
+	}
+}
+
+void atomisp_assert_recovery_work(struct work_struct *work)
+{
+	struct atomisp_device *isp = container_of(work, struct atomisp_device,
+						  assert_recovery_work);
+
+	mutex_lock(&isp->mutex);
+	__atomisp_css_recover(isp, true);
+	mutex_unlock(&isp->mutex);
+}
+
+void atomisp_css_flush(struct atomisp_device *isp)
+{
+	/* Start recover */
+	__atomisp_css_recover(isp, false);
+
+	dev_dbg(isp->dev, "atomisp css flush done\n");
+}
+
+void atomisp_setup_flash(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_control ctrl;
+
+	if (!isp->flash)
+		return;
+
+	if (asd->params.flash_state != ATOMISP_FLASH_REQUESTED &&
+	    asd->params.flash_state != ATOMISP_FLASH_DONE)
+		return;
+
+	if (asd->params.num_flash_frames) {
+		/* make sure the timeout is set before setting flash mode */
+		ctrl.id = V4L2_CID_FLASH_TIMEOUT;
+		ctrl.value = FLASH_TIMEOUT;
+
+		if (v4l2_s_ctrl(NULL, isp->flash->ctrl_handler, &ctrl)) {
+			dev_err(isp->dev, "flash timeout configure failed\n");
+			return;
+		}
+
+		ia_css_stream_request_flash(asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream);
+
+		asd->params.flash_state = ATOMISP_FLASH_ONGOING;
+	} else {
+		asd->params.flash_state = ATOMISP_FLASH_IDLE;
+	}
+}
+
+irqreturn_t atomisp_isr_thread(int irq, void *isp_ptr)
+{
+	struct atomisp_device *isp = isp_ptr;
+	unsigned long flags;
+	bool frame_done_found[MAX_STREAM_NUM] = {0};
+	bool css_pipe_done[MAX_STREAM_NUM] = {0};
+	unsigned int i;
+	struct atomisp_sub_device *asd;
+
+	dev_dbg(isp->dev, ">%s\n", __func__);
+
+	spin_lock_irqsave(&isp->lock, flags);
+
+	if (!atomisp_streaming_count(isp)) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	/*
+	 * The standard CSS2.0 API tells the following calling sequence of
+	 * dequeue ready buffers:
+	 * while (ia_css_dequeue_psys_event(...)) {
+	 *	switch (event.type) {
+	 *	...
+	 *	ia_css_pipe_dequeue_buffer()
+	 *	}
+	 * }
+	 * That is, dequeue event and buffer are one after another.
+	 *
+	 * But the following implementation is to first deuque all the event
+	 * to a FIFO, then process the event in the FIFO.
+	 * This will not have issue in single stream mode, but it do have some
+	 * issue in multiple stream case. The issue is that
+	 * ia_css_pipe_dequeue_buffer() will not return the corrent buffer in
+	 * a specific pipe.
+	 *
+	 * This is due to ia_css_pipe_dequeue_buffer() does not take the
+	 * ia_css_pipe parameter.
+	 *
+	 * So:
+	 * For CSS2.0: we change the way to not dequeue all the event at one
+	 * time, instead, dequue one and process one, then another
+	 */
+	mutex_lock(&isp->mutex);
+	if (atomisp_css_isr_thread(isp, frame_done_found, css_pipe_done))
+		goto out;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+			continue;
+		atomisp_setup_flash(asd);
+	}
+out:
+	mutex_unlock(&isp->mutex);
+	dev_dbg(isp->dev, "<%s\n", __func__);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Get internal fmt according to V4L2 fmt
+ */
+static enum ia_css_frame_format
+v4l2_fmt_to_sh_fmt(u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_YUV420:
+				return IA_CSS_FRAME_FORMAT_YUV420;
+	case V4L2_PIX_FMT_YVU420:
+		return IA_CSS_FRAME_FORMAT_YV12;
+	case V4L2_PIX_FMT_YUV422P:
+		return IA_CSS_FRAME_FORMAT_YUV422;
+	case V4L2_PIX_FMT_YUV444:
+		return IA_CSS_FRAME_FORMAT_YUV444;
+	case V4L2_PIX_FMT_NV12:
+		return IA_CSS_FRAME_FORMAT_NV12;
+	case V4L2_PIX_FMT_NV21:
+		return IA_CSS_FRAME_FORMAT_NV21;
+	case V4L2_PIX_FMT_NV16:
+		return IA_CSS_FRAME_FORMAT_NV16;
+	case V4L2_PIX_FMT_NV61:
+		return IA_CSS_FRAME_FORMAT_NV61;
+	case V4L2_PIX_FMT_UYVY:
+		return IA_CSS_FRAME_FORMAT_UYVY;
+	case V4L2_PIX_FMT_YUYV:
+		return IA_CSS_FRAME_FORMAT_YUYV;
+	case V4L2_PIX_FMT_RGB24:
+		return IA_CSS_FRAME_FORMAT_PLANAR_RGB888;
+	case V4L2_PIX_FMT_RGB32:
+		return IA_CSS_FRAME_FORMAT_RGBA888;
+	case V4L2_PIX_FMT_RGB565:
+		return IA_CSS_FRAME_FORMAT_RGB565;
+#if 0
+	case V4L2_PIX_FMT_JPEG:
+	case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
+		return IA_CSS_FRAME_FORMAT_BINARY_8;
+#endif
+	case V4L2_PIX_FMT_SBGGR16:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+		return IA_CSS_FRAME_FORMAT_RAW;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * raw format match between SH format and V4L2 format
+ */
+static int raw_output_format_match_input(u32 input, u32 output)
+{
+	if ((input == ATOMISP_INPUT_FORMAT_RAW_12) &&
+	    ((output == V4L2_PIX_FMT_SRGGB12) ||
+	     (output == V4L2_PIX_FMT_SGRBG12) ||
+	     (output == V4L2_PIX_FMT_SBGGR12) ||
+	     (output == V4L2_PIX_FMT_SGBRG12)))
+		return 0;
+
+	if ((input == ATOMISP_INPUT_FORMAT_RAW_10) &&
+	    ((output == V4L2_PIX_FMT_SRGGB10) ||
+	     (output == V4L2_PIX_FMT_SGRBG10) ||
+	     (output == V4L2_PIX_FMT_SBGGR10) ||
+	     (output == V4L2_PIX_FMT_SGBRG10)))
+		return 0;
+
+	if ((input == ATOMISP_INPUT_FORMAT_RAW_8) &&
+	    ((output == V4L2_PIX_FMT_SRGGB8) ||
+	     (output == V4L2_PIX_FMT_SGRBG8) ||
+	     (output == V4L2_PIX_FMT_SBGGR8) ||
+	     (output == V4L2_PIX_FMT_SGBRG8)))
+		return 0;
+
+	if ((input == ATOMISP_INPUT_FORMAT_RAW_16) && (output == V4L2_PIX_FMT_SBGGR16))
+		return 0;
+
+	return -EINVAL;
+}
+
+u32 atomisp_get_pixel_depth(u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_YVU420:
+		return 12;
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_SBGGR16:
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+		return 16;
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_YUV444:
+		return 24;
+	case V4L2_PIX_FMT_RGB32:
+		return 32;
+	case V4L2_PIX_FMT_JPEG:
+	case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+		return 8;
+	default:
+		return 8 * 2;	/* raw type now */
+	}
+}
+
+bool atomisp_is_mbuscode_raw(uint32_t code)
+{
+	return code >= 0x3000 && code < 0x4000;
+}
+
+/*
+ * ISP features control function
+ */
+
+/*
+ * Set ISP capture mode based on current settings
+ */
+static void atomisp_update_capture_mode(struct atomisp_sub_device *asd)
+{
+	if (asd->params.gdc_cac_en)
+		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_ADVANCED);
+	else if (asd->params.low_light)
+		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_LOW_LIGHT);
+	else if (asd->video_out_capture.sh_fmt == IA_CSS_FRAME_FORMAT_RAW)
+		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_RAW);
+	else
+		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_PRIMARY);
+}
+
+/* ISP2401 */
+int atomisp_set_sensor_runmode(struct atomisp_sub_device *asd,
+			       struct atomisp_s_runmode *runmode)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_ctrl *c;
+	int ret = 0;
+
+	if (!(runmode && (runmode->mode & RUNMODE_MASK)))
+		return -EINVAL;
+
+	mutex_lock(asd->ctrl_handler.lock);
+	c = v4l2_ctrl_find(isp->inputs[asd->input_curr].camera->ctrl_handler,
+			   V4L2_CID_RUN_MODE);
+
+	if (c)
+		ret = v4l2_ctrl_s_ctrl(c, runmode->mode);
+
+	mutex_unlock(asd->ctrl_handler.lock);
+	return ret;
+}
+
+/*
+ * Function to enable/disable lens geometry distortion correction (GDC) and
+ * chromatic aberration correction (CAC)
+ */
+int atomisp_gdc_cac(struct atomisp_sub_device *asd, int flag,
+		    __s32 *value)
+{
+	if (flag == 0) {
+		*value = asd->params.gdc_cac_en;
+		return 0;
+	}
+
+	asd->params.gdc_cac_en = !!*value;
+	if (asd->params.gdc_cac_en) {
+		asd->params.config.morph_table = asd->params.css_param.morph_table;
+	} else {
+		asd->params.config.morph_table = NULL;
+	}
+	asd->params.css_update_params_needed = true;
+	atomisp_update_capture_mode(asd);
+	return 0;
+}
+
+/*
+ * Function to enable/disable low light mode including ANR
+ */
+int atomisp_low_light(struct atomisp_sub_device *asd, int flag,
+		      __s32 *value)
+{
+	if (flag == 0) {
+		*value = asd->params.low_light;
+		return 0;
+	}
+
+	asd->params.low_light = (*value != 0);
+	atomisp_update_capture_mode(asd);
+	return 0;
+}
+
+/*
+ * Function to enable/disable extra noise reduction (XNR) in low light
+ * condition
+ */
+int atomisp_xnr(struct atomisp_sub_device *asd, int flag,
+		int *xnr_enable)
+{
+	if (flag == 0) {
+		*xnr_enable = asd->params.xnr_en;
+		return 0;
+	}
+
+	atomisp_css_capture_enable_xnr(asd, !!*xnr_enable);
+
+	return 0;
+}
+
+/*
+ * Function to configure bayer noise reduction
+ */
+int atomisp_nr(struct atomisp_sub_device *asd, int flag,
+	       struct atomisp_nr_config *arg)
+{
+	if (flag == 0) {
+		/* Get nr config from current setup */
+		if (atomisp_css_get_nr_config(asd, arg))
+			return -EINVAL;
+	} else {
+		/* Set nr config to isp parameters */
+		memcpy(&asd->params.css_param.nr_config, arg,
+		       sizeof(struct ia_css_nr_config));
+		asd->params.config.nr_config = &asd->params.css_param.nr_config;
+		asd->params.css_update_params_needed = true;
+	}
+	return 0;
+}
+
+/*
+ * Function to configure temporal noise reduction (TNR)
+ */
+int atomisp_tnr(struct atomisp_sub_device *asd, int flag,
+		struct atomisp_tnr_config *config)
+{
+	/* Get tnr config from current setup */
+	if (flag == 0) {
+		/* Get tnr config from current setup */
+		if (atomisp_css_get_tnr_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set tnr config to isp parameters */
+		memcpy(&asd->params.css_param.tnr_config, config,
+		       sizeof(struct ia_css_tnr_config));
+		asd->params.config.tnr_config = &asd->params.css_param.tnr_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to configure black level compensation
+ */
+int atomisp_black_level(struct atomisp_sub_device *asd, int flag,
+			struct atomisp_ob_config *config)
+{
+	if (flag == 0) {
+		/* Get ob config from current setup */
+		if (atomisp_css_get_ob_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set ob config to isp parameters */
+		memcpy(&asd->params.css_param.ob_config, config,
+		       sizeof(struct ia_css_ob_config));
+		asd->params.config.ob_config = &asd->params.css_param.ob_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to configure edge enhancement
+ */
+int atomisp_ee(struct atomisp_sub_device *asd, int flag,
+	       struct atomisp_ee_config *config)
+{
+	if (flag == 0) {
+		/* Get ee config from current setup */
+		if (atomisp_css_get_ee_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set ee config to isp parameters */
+		memcpy(&asd->params.css_param.ee_config, config,
+		       sizeof(asd->params.css_param.ee_config));
+		asd->params.config.ee_config = &asd->params.css_param.ee_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to update Gamma table for gamma, brightness and contrast config
+ */
+int atomisp_gamma(struct atomisp_sub_device *asd, int flag,
+		  struct atomisp_gamma_table *config)
+{
+	if (flag == 0) {
+		/* Get gamma table from current setup */
+		if (atomisp_css_get_gamma_table(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set gamma table to isp parameters */
+		memcpy(&asd->params.css_param.gamma_table, config,
+		       sizeof(asd->params.css_param.gamma_table));
+		asd->params.config.gamma_table = &asd->params.css_param.gamma_table;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to update Ctc table for Chroma Enhancement
+ */
+int atomisp_ctc(struct atomisp_sub_device *asd, int flag,
+		struct atomisp_ctc_table *config)
+{
+	if (flag == 0) {
+		/* Get ctc table from current setup */
+		if (atomisp_css_get_ctc_table(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set ctc table to isp parameters */
+		memcpy(&asd->params.css_param.ctc_table, config,
+		       sizeof(asd->params.css_param.ctc_table));
+		atomisp_css_set_ctc_table(asd, &asd->params.css_param.ctc_table);
+	}
+
+	return 0;
+}
+
+/*
+ * Function to update gamma correction parameters
+ */
+int atomisp_gamma_correction(struct atomisp_sub_device *asd, int flag,
+			     struct atomisp_gc_config *config)
+{
+	if (flag == 0) {
+		/* Get gamma correction params from current setup */
+		if (atomisp_css_get_gc_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set gamma correction params to isp parameters */
+		memcpy(&asd->params.css_param.gc_config, config,
+		       sizeof(asd->params.css_param.gc_config));
+		asd->params.config.gc_config = &asd->params.css_param.gc_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to update narrow gamma flag
+ */
+int atomisp_formats(struct atomisp_sub_device *asd, int flag,
+		    struct atomisp_formats_config *config)
+{
+	if (flag == 0) {
+		/* Get narrow gamma flag from current setup */
+		if (atomisp_css_get_formats_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set narrow gamma flag to isp parameters */
+		memcpy(&asd->params.css_param.formats_config, config,
+		       sizeof(asd->params.css_param.formats_config));
+		asd->params.config.formats_config = &asd->params.css_param.formats_config;
+	}
+
+	return 0;
+}
+
+void atomisp_free_internal_buffers(struct atomisp_sub_device *asd)
+{
+	atomisp_free_css_parameters(&asd->params.css_param);
+
+	if (asd->raw_output_frame) {
+		ia_css_frame_free(asd->raw_output_frame);
+		asd->raw_output_frame = NULL;
+	}
+}
+
+static void atomisp_update_grid_info(struct atomisp_sub_device *asd,
+				     enum ia_css_pipe_id pipe_id,
+				     int source_pad)
+{
+	struct atomisp_device *isp = asd->isp;
+	int err;
+
+	if (atomisp_css_get_grid_info(asd, pipe_id, source_pad))
+		return;
+
+	/* We must free all buffers because they no longer match
+	   the grid size. */
+	atomisp_css_free_stat_buffers(asd);
+
+	err = atomisp_alloc_css_stat_bufs(asd, ATOMISP_INPUT_STREAM_GENERAL);
+	if (err) {
+		dev_err(isp->dev, "stat_buf allocate error\n");
+		goto err;
+	}
+
+	if (atomisp_alloc_3a_output_buf(asd)) {
+		/* Failure for 3A buffers does not influence DIS buffers */
+		if (asd->params.s3a_output_bytes != 0) {
+			/* For SOC sensor happens s3a_output_bytes == 0,
+			 * using if condition to exclude false error log */
+			dev_err(isp->dev, "Failed to allocate memory for 3A statistics\n");
+		}
+		goto err;
+	}
+
+	if (atomisp_alloc_dis_coef_buf(asd)) {
+		dev_err(isp->dev,
+			"Failed to allocate memory for DIS statistics\n");
+		goto err;
+	}
+
+	if (atomisp_alloc_metadata_output_buf(asd)) {
+		dev_err(isp->dev, "Failed to allocate memory for metadata\n");
+		goto err;
+	}
+
+	return;
+
+err:
+	atomisp_css_free_stat_buffers(asd);
+	return;
+}
+
+static void atomisp_curr_user_grid_info(struct atomisp_sub_device *asd,
+					struct atomisp_grid_info *info)
+{
+	memcpy(info, &asd->params.curr_grid_info.s3a_grid,
+	       sizeof(struct ia_css_3a_grid_info));
+}
+
+int atomisp_compare_grid(struct atomisp_sub_device *asd,
+			 struct atomisp_grid_info *atomgrid)
+{
+	struct atomisp_grid_info tmp = {0};
+
+	atomisp_curr_user_grid_info(asd, &tmp);
+	return memcmp(atomgrid, &tmp, sizeof(tmp));
+}
+
+/*
+ * Function to update Gdc table for gdc
+ */
+int atomisp_gdc_cac_table(struct atomisp_sub_device *asd, int flag,
+			  struct atomisp_morph_table *config)
+{
+	int ret;
+	int i;
+	struct atomisp_device *isp = asd->isp;
+
+	if (flag == 0) {
+		/* Get gdc table from current setup */
+		struct ia_css_morph_table tab = {0};
+
+		atomisp_css_get_morph_table(asd, &tab);
+
+		config->width = tab.width;
+		config->height = tab.height;
+
+		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+			ret = copy_to_user(config->coordinates_x[i],
+					   tab.coordinates_x[i], tab.height *
+					   tab.width * sizeof(*tab.coordinates_x[i]));
+			if (ret) {
+				dev_err(isp->dev,
+					"Failed to copy to User for x\n");
+				return -EFAULT;
+			}
+			ret = copy_to_user(config->coordinates_y[i],
+					   tab.coordinates_y[i], tab.height *
+					   tab.width * sizeof(*tab.coordinates_y[i]));
+			if (ret) {
+				dev_err(isp->dev,
+					"Failed to copy to User for y\n");
+				return -EFAULT;
+			}
+		}
+	} else {
+		struct ia_css_morph_table *tab =
+			    asd->params.css_param.morph_table;
+
+		/* free first if we have one */
+		if (tab) {
+			atomisp_css_morph_table_free(tab);
+			asd->params.css_param.morph_table = NULL;
+		}
+
+		/* allocate new one */
+		tab = atomisp_css_morph_table_allocate(config->width,
+						       config->height);
+
+		if (!tab) {
+			dev_err(isp->dev, "out of memory\n");
+			return -EINVAL;
+		}
+
+		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+			ret = copy_from_user(tab->coordinates_x[i],
+					     config->coordinates_x[i],
+					     config->height * config->width *
+					     sizeof(*config->coordinates_x[i]));
+			if (ret) {
+				dev_err(isp->dev,
+					"Failed to copy from User for x, ret %d\n",
+					ret);
+				atomisp_css_morph_table_free(tab);
+				return -EFAULT;
+			}
+			ret = copy_from_user(tab->coordinates_y[i],
+					     config->coordinates_y[i],
+					     config->height * config->width *
+					     sizeof(*config->coordinates_y[i]));
+			if (ret) {
+				dev_err(isp->dev,
+					"Failed to copy from User for y, ret is %d\n",
+					ret);
+				atomisp_css_morph_table_free(tab);
+				return -EFAULT;
+			}
+		}
+		asd->params.css_param.morph_table = tab;
+		if (asd->params.gdc_cac_en)
+			asd->params.config.morph_table = tab;
+	}
+
+	return 0;
+}
+
+int atomisp_macc_table(struct atomisp_sub_device *asd, int flag,
+		       struct atomisp_macc_config *config)
+{
+	struct ia_css_macc_table *macc_table;
+
+	switch (config->color_effect) {
+	case V4L2_COLORFX_NONE:
+		macc_table = &asd->params.css_param.macc_table;
+		break;
+	case V4L2_COLORFX_SKY_BLUE:
+		macc_table = &blue_macc_table;
+		break;
+	case V4L2_COLORFX_GRASS_GREEN:
+		macc_table = &green_macc_table;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN_LOW:
+		macc_table = &skin_low_macc_table;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN:
+		macc_table = &skin_medium_macc_table;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN_HIGH:
+		macc_table = &skin_high_macc_table;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (flag == 0) {
+		/* Get macc table from current setup */
+		memcpy(&config->table, macc_table,
+		       sizeof(struct ia_css_macc_table));
+	} else {
+		memcpy(macc_table, &config->table,
+		       sizeof(struct ia_css_macc_table));
+		if (config->color_effect == asd->params.color_effect)
+			asd->params.config.macc_table = macc_table;
+	}
+
+	return 0;
+}
+
+int atomisp_set_dis_vector(struct atomisp_sub_device *asd,
+			   struct atomisp_dis_vector *vector)
+{
+	atomisp_css_video_set_dis_vector(asd, vector);
+
+	asd->params.dis_proj_data_valid = false;
+	asd->params.css_update_params_needed = true;
+	return 0;
+}
+
+/*
+ * Function to set/get image stablization statistics
+ */
+int atomisp_get_dis_stat(struct atomisp_sub_device *asd,
+			 struct atomisp_dis_statistics *stats)
+{
+	return atomisp_css_get_dis_stat(asd, stats);
+}
+
+/*
+ * Function  set camrea_prefiles.xml current sensor pixel array size
+ */
+int atomisp_set_array_res(struct atomisp_sub_device *asd,
+			  struct atomisp_resolution  *config)
+{
+	dev_dbg(asd->isp->dev, ">%s start\n", __func__);
+	if (!config) {
+		dev_err(asd->isp->dev, "Set sensor array size is not valid\n");
+		return -EINVAL;
+	}
+
+	asd->sensor_array_res.width = config->width;
+	asd->sensor_array_res.height = config->height;
+	return 0;
+}
+
+/*
+ * Function to get DVS2 BQ resolution settings
+ */
+int atomisp_get_dvs2_bq_resolutions(struct atomisp_sub_device *asd,
+				    struct atomisp_dvs2_bq_resolutions *bq_res)
+{
+	struct ia_css_pipe_config *pipe_cfg = NULL;
+	struct ia_css_stream_config *stream_cfg = NULL;
+	struct ia_css_stream_input_config *input_config = NULL;
+
+	struct ia_css_stream *stream =
+		    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
+	if (!stream) {
+		dev_warn(asd->isp->dev, "stream is not created");
+		return -EAGAIN;
+	}
+
+	pipe_cfg = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		   .pipe_configs[IA_CSS_PIPE_ID_VIDEO];
+	stream_cfg = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		     .stream_config;
+	input_config = &stream_cfg->input_config;
+
+	if (!bq_res)
+		return -EINVAL;
+
+	/* the GDC output resolution */
+	bq_res->output_bq.width_bq = pipe_cfg->output_info[0].res.width / 2;
+	bq_res->output_bq.height_bq = pipe_cfg->output_info[0].res.height / 2;
+
+	bq_res->envelope_bq.width_bq = 0;
+	bq_res->envelope_bq.height_bq = 0;
+	/* the GDC input resolution */
+	if (!asd->continuous_mode->val) {
+		bq_res->source_bq.width_bq = bq_res->output_bq.width_bq +
+					     pipe_cfg->dvs_envelope.width / 2;
+		bq_res->source_bq.height_bq = bq_res->output_bq.height_bq +
+					      pipe_cfg->dvs_envelope.height / 2;
+		/*
+		 * Bad pixels caused by spatial filter processing
+		 * ISP filter resolution should be given by CSS/FW, but for now
+		 * there is not such API to query, and it is fixed value, so
+		 * hardcoded here.
+		 */
+		bq_res->ispfilter_bq.width_bq = 12 / 2;
+		bq_res->ispfilter_bq.height_bq = 12 / 2;
+		/* spatial filter shift, always 4 pixels */
+		bq_res->gdc_shift_bq.width_bq = 4 / 2;
+		bq_res->gdc_shift_bq.height_bq = 4 / 2;
+
+		if (asd->params.video_dis_en) {
+			bq_res->envelope_bq.width_bq = pipe_cfg->dvs_envelope.width
+						       / 2 - bq_res->ispfilter_bq.width_bq;
+			bq_res->envelope_bq.height_bq = pipe_cfg->dvs_envelope.height
+							/ 2 - bq_res->ispfilter_bq.height_bq;
+		}
+	} else {
+		unsigned int w_padding;
+		unsigned int gdc_effective_input = 0;
+
+		/* For GDC:
+		 * gdc_effective_input = effective_input + envelope
+		 *
+		 * From the comment and formula in BZ1786,
+		 * we see the source_bq should be:
+		 * effective_input / bayer_ds_ratio
+		 */
+		bq_res->source_bq.width_bq =
+		    (input_config->effective_res.width *
+		     pipe_cfg->bayer_ds_out_res.width /
+		     input_config->effective_res.width + 1) / 2;
+		bq_res->source_bq.height_bq =
+		    (input_config->effective_res.height *
+		     pipe_cfg->bayer_ds_out_res.height /
+		     input_config->effective_res.height + 1) / 2;
+
+		if (!asd->params.video_dis_en) {
+			/*
+			 * We adjust the ispfilter_bq to:
+			 * ispfilter_bq = 128/BDS
+			 * we still need firmware team to provide an offical
+			 * formula for SDV.
+			 */
+			bq_res->ispfilter_bq.width_bq = 128 *
+							pipe_cfg->bayer_ds_out_res.width /
+							input_config->effective_res.width / 2;
+			bq_res->ispfilter_bq.height_bq = 128 *
+							 pipe_cfg->bayer_ds_out_res.width /
+							 input_config->effective_res.width / 2;
+
+			if (IS_HWREVISION(asd->isp, ATOMISP_HW_REVISION_ISP2401)) {
+				/* No additional left padding for ISYS2401 */
+				bq_res->gdc_shift_bq.width_bq = 4 / 2;
+				bq_res->gdc_shift_bq.height_bq = 4 / 2;
+			} else {
+				/*
+				 * For the w_padding and gdc_shift_bq cacluation
+				 * Please see the BZ 1786 and 4358 for more info.
+				 * Just test that this formula can work now,
+				 * but we still have no offical formula.
+				 *
+				 * w_padding = ceiling(gdc_effective_input
+				 *             /128, 1) * 128 - effective_width
+				 * gdc_shift_bq = w_padding/BDS/2 + ispfilter_bq/2
+				 */
+				gdc_effective_input =
+				    input_config->effective_res.width +
+				    pipe_cfg->dvs_envelope.width;
+				w_padding = roundup(gdc_effective_input, 128) -
+					    input_config->effective_res.width;
+				w_padding = w_padding *
+					    pipe_cfg->bayer_ds_out_res.width /
+					    input_config->effective_res.width + 1;
+				w_padding = roundup(w_padding / 2, 1);
+
+				bq_res->gdc_shift_bq.width_bq = bq_res->ispfilter_bq.width_bq / 2
+								+ w_padding;
+				bq_res->gdc_shift_bq.height_bq = 4 / 2;
+			}
+		} else {
+			unsigned int dvs_w, dvs_h, dvs_w_max, dvs_h_max;
+
+			bq_res->ispfilter_bq.width_bq = 8 / 2;
+			bq_res->ispfilter_bq.height_bq = 8 / 2;
+
+			if (IS_HWREVISION(asd->isp, ATOMISP_HW_REVISION_ISP2401)) {
+				/* No additional left padding for ISYS2401 */
+				bq_res->gdc_shift_bq.width_bq = 4 / 2;
+				bq_res->gdc_shift_bq.height_bq = 4 / 2;
+			} else {
+				w_padding =
+				    roundup(input_config->effective_res.width, 128) -
+				    input_config->effective_res.width;
+				if (w_padding < 12)
+					w_padding = 12;
+				bq_res->gdc_shift_bq.width_bq = 4 / 2 +
+								((w_padding - 12) *
+								 pipe_cfg->bayer_ds_out_res.width /
+								 input_config->effective_res.width + 1) / 2;
+				bq_res->gdc_shift_bq.height_bq = 4 / 2;
+			}
+
+			dvs_w = pipe_cfg->bayer_ds_out_res.width -
+				pipe_cfg->output_info[0].res.width;
+			dvs_h = pipe_cfg->bayer_ds_out_res.height -
+				pipe_cfg->output_info[0].res.height;
+			dvs_w_max = rounddown(
+					pipe_cfg->output_info[0].res.width / 5,
+					ATOM_ISP_STEP_WIDTH);
+			dvs_h_max = rounddown(
+					pipe_cfg->output_info[0].res.height / 5,
+					ATOM_ISP_STEP_HEIGHT);
+			bq_res->envelope_bq.width_bq =
+			    min((dvs_w / 2), (dvs_w_max / 2)) -
+			    bq_res->ispfilter_bq.width_bq;
+			bq_res->envelope_bq.height_bq =
+			    min((dvs_h / 2), (dvs_h_max / 2)) -
+			    bq_res->ispfilter_bq.height_bq;
+		}
+	}
+
+	dev_dbg(asd->isp->dev,
+		"source_bq.width_bq %d, source_bq.height_bq %d,\nispfilter_bq.width_bq %d, ispfilter_bq.height_bq %d,\ngdc_shift_bq.width_bq %d, gdc_shift_bq.height_bq %d,\nenvelope_bq.width_bq %d, envelope_bq.height_bq %d,\noutput_bq.width_bq %d, output_bq.height_bq %d\n",
+		bq_res->source_bq.width_bq, bq_res->source_bq.height_bq,
+		bq_res->ispfilter_bq.width_bq, bq_res->ispfilter_bq.height_bq,
+		bq_res->gdc_shift_bq.width_bq, bq_res->gdc_shift_bq.height_bq,
+		bq_res->envelope_bq.width_bq, bq_res->envelope_bq.height_bq,
+		bq_res->output_bq.width_bq, bq_res->output_bq.height_bq);
+
+	return 0;
+}
+
+int atomisp_set_dis_coefs(struct atomisp_sub_device *asd,
+			  struct atomisp_dis_coefficients *coefs)
+{
+	return atomisp_css_set_dis_coefs(asd, coefs);
+}
+
+/*
+ * Function to set/get 3A stat from isp
+ */
+int atomisp_3a_stat(struct atomisp_sub_device *asd, int flag,
+		    struct atomisp_3a_statistics *config)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_s3a_buf *s3a_buf;
+	unsigned long ret;
+
+	if (flag != 0)
+		return -EINVAL;
+
+	/* sanity check to avoid writing into unallocated memory. */
+	if (asd->params.s3a_output_bytes == 0)
+		return -EINVAL;
+
+	if (atomisp_compare_grid(asd, &config->grid_info) != 0) {
+		/* If the grid info in the argument differs from the current
+		   grid info, we tell the caller to reset the grid size and
+		   try again. */
+		return -EAGAIN;
+	}
+
+	if (list_empty(&asd->s3a_stats_ready)) {
+		dev_err(isp->dev, "3a statistics is not valid.\n");
+		return -EAGAIN;
+	}
+
+	s3a_buf = list_entry(asd->s3a_stats_ready.next,
+			     struct atomisp_s3a_buf, list);
+	if (s3a_buf->s3a_map)
+		ia_css_translate_3a_statistics(
+		    asd->params.s3a_user_stat, s3a_buf->s3a_map);
+	else
+		ia_css_get_3a_statistics(asd->params.s3a_user_stat,
+					 s3a_buf->s3a_data);
+
+	config->exp_id = s3a_buf->s3a_data->exp_id;
+	config->isp_config_id = s3a_buf->s3a_data->isp_config_id;
+
+	ret = copy_to_user(config->data, asd->params.s3a_user_stat->data,
+			   asd->params.s3a_output_bytes);
+	if (ret) {
+		dev_err(isp->dev, "copy to user failed: copied %lu bytes\n",
+			ret);
+		return -EFAULT;
+	}
+
+	/* Move to free buffer list */
+	list_del_init(&s3a_buf->list);
+	list_add_tail(&s3a_buf->list, &asd->s3a_stats);
+	dev_dbg(isp->dev, "%s: finish getting exp_id %d 3a stat, isp_config_id %d\n",
+		__func__,
+		config->exp_id, config->isp_config_id);
+	return 0;
+}
+
+int atomisp_get_metadata(struct atomisp_sub_device *asd, int flag,
+			 struct atomisp_metadata *md)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_stream_info *stream_info;
+	struct camera_mipi_info *mipi_info;
+	struct atomisp_metadata_buf *md_buf;
+	enum atomisp_metadata_type md_type = ATOMISP_MAIN_METADATA;
+	int ret, i;
+
+	if (flag != 0)
+		return -EINVAL;
+
+	stream_info = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		      stream_info;
+
+	/* We always return the resolution and stride even if there is
+	 * no valid metadata. This allows the caller to get the information
+	 * needed to allocate user-space buffers. */
+	md->width  = stream_info->metadata_info.resolution.width;
+	md->height = stream_info->metadata_info.resolution.height;
+	md->stride = stream_info->metadata_info.stride;
+
+	/* sanity check to avoid writing into unallocated memory.
+	 * This does not return an error because it is a valid way
+	 * for applications to detect that metadata is not enabled. */
+	if (md->width == 0 || md->height == 0 || !md->data)
+		return 0;
+
+	/* This is done in the atomisp_buf_done() */
+	if (list_empty(&asd->metadata_ready[md_type])) {
+		dev_warn(isp->dev, "Metadata queue is empty now!\n");
+		return -EAGAIN;
+	}
+
+	mipi_info = atomisp_to_sensor_mipi_info(
+			isp->inputs[asd->input_curr].camera);
+	if (!mipi_info)
+		return -EINVAL;
+
+	if (mipi_info->metadata_effective_width) {
+		for (i = 0; i < md->height; i++)
+			md->effective_width[i] =
+			    mipi_info->metadata_effective_width[i];
+	}
+
+	md_buf = list_entry(asd->metadata_ready[md_type].next,
+			    struct atomisp_metadata_buf, list);
+	md->exp_id = md_buf->metadata->exp_id;
+	if (md_buf->md_vptr) {
+		ret = copy_to_user(md->data,
+				   md_buf->md_vptr,
+				   stream_info->metadata_info.size);
+	} else {
+		hmm_load(md_buf->metadata->address,
+			 asd->params.metadata_user[md_type],
+			 stream_info->metadata_info.size);
+
+		ret = copy_to_user(md->data,
+				   asd->params.metadata_user[md_type],
+				   stream_info->metadata_info.size);
+	}
+	if (ret) {
+		dev_err(isp->dev, "copy to user failed: copied %d bytes\n",
+			ret);
+		return -EFAULT;
+	}
+
+	list_del_init(&md_buf->list);
+	list_add_tail(&md_buf->list, &asd->metadata[md_type]);
+
+	dev_dbg(isp->dev, "%s: HAL de-queued metadata type %d with exp_id %d\n",
+		__func__, md_type, md->exp_id);
+	return 0;
+}
+
+int atomisp_get_metadata_by_type(struct atomisp_sub_device *asd, int flag,
+				 struct atomisp_metadata_with_type *md)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_stream_info *stream_info;
+	struct camera_mipi_info *mipi_info;
+	struct atomisp_metadata_buf *md_buf;
+	enum atomisp_metadata_type md_type;
+	int ret, i;
+
+	if (flag != 0)
+		return -EINVAL;
+
+	stream_info = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		      stream_info;
+
+	/* We always return the resolution and stride even if there is
+	 * no valid metadata. This allows the caller to get the information
+	 * needed to allocate user-space buffers. */
+	md->width  = stream_info->metadata_info.resolution.width;
+	md->height = stream_info->metadata_info.resolution.height;
+	md->stride = stream_info->metadata_info.stride;
+
+	/* sanity check to avoid writing into unallocated memory.
+	 * This does not return an error because it is a valid way
+	 * for applications to detect that metadata is not enabled. */
+	if (md->width == 0 || md->height == 0 || !md->data)
+		return 0;
+
+	md_type = md->type;
+	if (md_type < 0 || md_type >= ATOMISP_METADATA_TYPE_NUM)
+		return -EINVAL;
+
+	/* This is done in the atomisp_buf_done() */
+	if (list_empty(&asd->metadata_ready[md_type])) {
+		dev_warn(isp->dev, "Metadata queue is empty now!\n");
+		return -EAGAIN;
+	}
+
+	mipi_info = atomisp_to_sensor_mipi_info(
+			isp->inputs[asd->input_curr].camera);
+	if (!mipi_info)
+		return -EINVAL;
+
+	if (mipi_info->metadata_effective_width) {
+		for (i = 0; i < md->height; i++)
+			md->effective_width[i] =
+			    mipi_info->metadata_effective_width[i];
+	}
+
+	md_buf = list_entry(asd->metadata_ready[md_type].next,
+			    struct atomisp_metadata_buf, list);
+	md->exp_id = md_buf->metadata->exp_id;
+	if (md_buf->md_vptr) {
+		ret = copy_to_user(md->data,
+				   md_buf->md_vptr,
+				   stream_info->metadata_info.size);
+	} else {
+		hmm_load(md_buf->metadata->address,
+			 asd->params.metadata_user[md_type],
+			 stream_info->metadata_info.size);
+
+		ret = copy_to_user(md->data,
+				   asd->params.metadata_user[md_type],
+				   stream_info->metadata_info.size);
+	}
+	if (ret) {
+		dev_err(isp->dev, "copy to user failed: copied %d bytes\n",
+			ret);
+		return -EFAULT;
+	} else {
+		list_del_init(&md_buf->list);
+		list_add_tail(&md_buf->list, &asd->metadata[md_type]);
+	}
+	dev_dbg(isp->dev, "%s: HAL de-queued metadata type %d with exp_id %d\n",
+		__func__, md_type, md->exp_id);
+	return 0;
+}
+
+/*
+ * Function to calculate real zoom region for every pipe
+ */
+int atomisp_calculate_real_zoom_region(struct atomisp_sub_device *asd,
+				       struct ia_css_dz_config   *dz_config,
+				       enum ia_css_pipe_id css_pipe_id)
+
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct atomisp_resolution  eff_res, out_res;
+	int w_offset, h_offset;
+
+	memset(&eff_res, 0, sizeof(eff_res));
+	memset(&out_res, 0, sizeof(out_res));
+
+	if (dz_config->dx || dz_config->dy)
+		return 0;
+
+	if (css_pipe_id != IA_CSS_PIPE_ID_PREVIEW
+	    && css_pipe_id != IA_CSS_PIPE_ID_CAPTURE) {
+		dev_err(asd->isp->dev, "%s the set pipe no support crop region"
+			, __func__);
+		return -EINVAL;
+	}
+
+	eff_res.width =
+	    stream_env->stream_config.input_config.effective_res.width;
+	eff_res.height =
+	    stream_env->stream_config.input_config.effective_res.height;
+	if (eff_res.width == 0 || eff_res.height == 0) {
+		dev_err(asd->isp->dev, "%s err effective resolution"
+			, __func__);
+		return -EINVAL;
+	}
+
+	if (dz_config->zoom_region.resolution.width
+	    == asd->sensor_array_res.width
+	    || dz_config->zoom_region.resolution.height
+	    == asd->sensor_array_res.height) {
+		/*no need crop region*/
+		dz_config->zoom_region.origin.x = 0;
+		dz_config->zoom_region.origin.y = 0;
+		dz_config->zoom_region.resolution.width = eff_res.width;
+		dz_config->zoom_region.resolution.height = eff_res.height;
+		return 0;
+	}
+
+	/* FIXME:
+	 * This is not the correct implementation with Google's definition, due
+	 * to firmware limitation.
+	 * map real crop region base on above calculating base max crop region.
+	 */
+
+	if (!IS_ISP2401) {
+		dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x
+						  * eff_res.width
+						  / asd->sensor_array_res.width;
+		dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y
+						  * eff_res.height
+						  / asd->sensor_array_res.height;
+		dz_config->zoom_region.resolution.width = dz_config->zoom_region.resolution.width
+							  * eff_res.width
+							  / asd->sensor_array_res.width;
+		dz_config->zoom_region.resolution.height = dz_config->zoom_region.resolution.height
+							  * eff_res.height
+							  / asd->sensor_array_res.height;
+		/*
+		 * Set same ratio of crop region resolution and current pipe output
+		 * resolution
+		 */
+		out_res.width = stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
+		out_res.height = stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
+		if (out_res.width == 0 || out_res.height == 0) {
+			dev_err(asd->isp->dev, "%s err current pipe output resolution"
+				, __func__);
+			return -EINVAL;
+		}
+	} else {
+		out_res.width = stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
+		out_res.height = stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
+		if (out_res.width == 0 || out_res.height == 0) {
+			dev_err(asd->isp->dev, "%s err current pipe output resolution"
+				, __func__);
+			return -EINVAL;
+		}
+
+		if (asd->sensor_array_res.width * out_res.height
+		    < out_res.width * asd->sensor_array_res.height) {
+			h_offset = asd->sensor_array_res.height
+				   - asd->sensor_array_res.width
+				   * out_res.height / out_res.width;
+			h_offset = h_offset / 2;
+			if (dz_config->zoom_region.origin.y < h_offset)
+				dz_config->zoom_region.origin.y = 0;
+			else
+				dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y - h_offset;
+			w_offset = 0;
+		} else {
+			w_offset = asd->sensor_array_res.width
+				   - asd->sensor_array_res.height
+				   * out_res.width / out_res.height;
+			w_offset = w_offset / 2;
+			if (dz_config->zoom_region.origin.x < w_offset)
+				dz_config->zoom_region.origin.x = 0;
+			else
+				dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x - w_offset;
+			h_offset = 0;
+		}
+		dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x
+						  * eff_res.width
+						  / (asd->sensor_array_res.width - 2 * w_offset);
+		dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y
+						  * eff_res.height
+						  / (asd->sensor_array_res.height - 2 * h_offset);
+		dz_config->zoom_region.resolution.width = dz_config->zoom_region.resolution.width
+						  * eff_res.width
+						  / (asd->sensor_array_res.width - 2 * w_offset);
+		dz_config->zoom_region.resolution.height = dz_config->zoom_region.resolution.height
+						  * eff_res.height
+						  / (asd->sensor_array_res.height - 2 * h_offset);
+	}
+
+	if (out_res.width * dz_config->zoom_region.resolution.height
+	    > dz_config->zoom_region.resolution.width * out_res.height) {
+		dz_config->zoom_region.resolution.height =
+		    dz_config->zoom_region.resolution.width
+		    * out_res.height / out_res.width;
+	} else {
+		dz_config->zoom_region.resolution.width =
+		    dz_config->zoom_region.resolution.height
+		    * out_res.width / out_res.height;
+	}
+	dev_dbg(asd->isp->dev,
+		"%s crop region:(%d,%d),(%d,%d) eff_res(%d, %d) array_size(%d,%d) out_res(%d, %d)\n",
+		__func__, dz_config->zoom_region.origin.x,
+		dz_config->zoom_region.origin.y,
+		dz_config->zoom_region.resolution.width,
+		dz_config->zoom_region.resolution.height,
+		eff_res.width, eff_res.height,
+		asd->sensor_array_res.width,
+		asd->sensor_array_res.height,
+		out_res.width, out_res.height);
+
+	if ((dz_config->zoom_region.origin.x +
+	     dz_config->zoom_region.resolution.width
+	     > eff_res.width) ||
+	    (dz_config->zoom_region.origin.y +
+	     dz_config->zoom_region.resolution.height
+	     > eff_res.height))
+		return -EINVAL;
+
+	return 0;
+}
+
+/*
+ * Function to check the zoom region whether is effective
+ */
+static bool atomisp_check_zoom_region(
+    struct atomisp_sub_device *asd,
+    struct ia_css_dz_config *dz_config)
+{
+	struct atomisp_resolution  config;
+	bool flag = false;
+	unsigned int w, h;
+
+	memset(&config, 0, sizeof(struct atomisp_resolution));
+
+	if (dz_config->dx && dz_config->dy)
+		return true;
+
+	config.width = asd->sensor_array_res.width;
+	config.height = asd->sensor_array_res.height;
+	w = dz_config->zoom_region.origin.x +
+	    dz_config->zoom_region.resolution.width;
+	h = dz_config->zoom_region.origin.y +
+	    dz_config->zoom_region.resolution.height;
+
+	if ((w <= config.width) && (h <= config.height) && w > 0 && h > 0)
+		flag = true;
+	else
+		/* setting error zoom region */
+		dev_err(asd->isp->dev,
+			"%s zoom region ERROR:dz_config:(%d,%d),(%d,%d)array_res(%d, %d)\n",
+			__func__, dz_config->zoom_region.origin.x,
+			dz_config->zoom_region.origin.y,
+			dz_config->zoom_region.resolution.width,
+			dz_config->zoom_region.resolution.height,
+			config.width, config.height);
+
+	return flag;
+}
+
+void atomisp_apply_css_parameters(
+    struct atomisp_sub_device *asd,
+    struct atomisp_css_params *css_param)
+{
+	if (css_param->update_flag.wb_config)
+		asd->params.config.wb_config = &css_param->wb_config;
+
+	if (css_param->update_flag.ob_config)
+		asd->params.config.ob_config = &css_param->ob_config;
+
+	if (css_param->update_flag.dp_config)
+		asd->params.config.dp_config = &css_param->dp_config;
+
+	if (css_param->update_flag.nr_config)
+		asd->params.config.nr_config = &css_param->nr_config;
+
+	if (css_param->update_flag.ee_config)
+		asd->params.config.ee_config = &css_param->ee_config;
+
+	if (css_param->update_flag.tnr_config)
+		asd->params.config.tnr_config = &css_param->tnr_config;
+
+	if (css_param->update_flag.a3a_config)
+		asd->params.config.s3a_config = &css_param->s3a_config;
+
+	if (css_param->update_flag.ctc_config)
+		asd->params.config.ctc_config = &css_param->ctc_config;
+
+	if (css_param->update_flag.cnr_config)
+		asd->params.config.cnr_config = &css_param->cnr_config;
+
+	if (css_param->update_flag.ecd_config)
+		asd->params.config.ecd_config = &css_param->ecd_config;
+
+	if (css_param->update_flag.ynr_config)
+		asd->params.config.ynr_config = &css_param->ynr_config;
+
+	if (css_param->update_flag.fc_config)
+		asd->params.config.fc_config = &css_param->fc_config;
+
+	if (css_param->update_flag.macc_config)
+		asd->params.config.macc_config = &css_param->macc_config;
+
+	if (css_param->update_flag.aa_config)
+		asd->params.config.aa_config = &css_param->aa_config;
+
+	if (css_param->update_flag.anr_config)
+		asd->params.config.anr_config = &css_param->anr_config;
+
+	if (css_param->update_flag.xnr_config)
+		asd->params.config.xnr_config = &css_param->xnr_config;
+
+	if (css_param->update_flag.yuv2rgb_cc_config)
+		asd->params.config.yuv2rgb_cc_config = &css_param->yuv2rgb_cc_config;
+
+	if (css_param->update_flag.rgb2yuv_cc_config)
+		asd->params.config.rgb2yuv_cc_config = &css_param->rgb2yuv_cc_config;
+
+	if (css_param->update_flag.macc_table)
+		asd->params.config.macc_table = &css_param->macc_table;
+
+	if (css_param->update_flag.xnr_table)
+		asd->params.config.xnr_table = &css_param->xnr_table;
+
+	if (css_param->update_flag.r_gamma_table)
+		asd->params.config.r_gamma_table = &css_param->r_gamma_table;
+
+	if (css_param->update_flag.g_gamma_table)
+		asd->params.config.g_gamma_table = &css_param->g_gamma_table;
+
+	if (css_param->update_flag.b_gamma_table)
+		asd->params.config.b_gamma_table = &css_param->b_gamma_table;
+
+	if (css_param->update_flag.anr_thres)
+		atomisp_css_set_anr_thres(asd, &css_param->anr_thres);
+
+	if (css_param->update_flag.shading_table)
+		asd->params.config.shading_table = css_param->shading_table;
+
+	if (css_param->update_flag.morph_table && asd->params.gdc_cac_en)
+		asd->params.config.morph_table = css_param->morph_table;
+
+	if (css_param->update_flag.dvs2_coefs) {
+		struct ia_css_dvs_grid_info *dvs_grid_info =
+		    atomisp_css_get_dvs_grid_info(
+			&asd->params.curr_grid_info);
+
+		if (dvs_grid_info && dvs_grid_info->enable)
+			atomisp_css_set_dvs2_coefs(asd, css_param->dvs2_coeff);
+	}
+
+	if (css_param->update_flag.dvs_6axis_config)
+		atomisp_css_set_dvs_6axis(asd, css_param->dvs_6axis);
+
+	atomisp_css_set_isp_config_id(asd, css_param->isp_config_id);
+	/*
+	 * These configurations are on used by ISP1.x, not for ISP2.x,
+	 * so do not handle them. see comments of ia_css_isp_config.
+	 * 1 cc_config
+	 * 2 ce_config
+	 * 3 de_config
+	 * 4 gc_config
+	 * 5 gamma_table
+	 * 6 ctc_table
+	 * 7 dvs_coefs
+	 */
+}
+
+static unsigned int long copy_from_compatible(void *to, const void *from,
+	unsigned long n, bool from_user)
+{
+	if (from_user)
+		return copy_from_user(to, (void __user *)from, n);
+	else
+		memcpy(to, from, n);
+	return 0;
+}
+
+int atomisp_cp_general_isp_parameters(struct atomisp_sub_device *asd,
+				      struct atomisp_parameters *arg,
+				      struct atomisp_css_params *css_param,
+				      bool from_user)
+{
+	struct atomisp_parameters *cur_config = &css_param->update_flag;
+
+	if (!arg || !asd || !css_param)
+		return -EINVAL;
+
+	if (arg->wb_config && (from_user || !cur_config->wb_config)) {
+		if (copy_from_compatible(&css_param->wb_config, arg->wb_config,
+					 sizeof(struct ia_css_wb_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.wb_config =
+		    (struct atomisp_wb_config *)&css_param->wb_config;
+	}
+
+	if (arg->ob_config && (from_user || !cur_config->ob_config)) {
+		if (copy_from_compatible(&css_param->ob_config, arg->ob_config,
+					 sizeof(struct ia_css_ob_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.ob_config =
+		    (struct atomisp_ob_config *)&css_param->ob_config;
+	}
+
+	if (arg->dp_config && (from_user || !cur_config->dp_config)) {
+		if (copy_from_compatible(&css_param->dp_config, arg->dp_config,
+					 sizeof(struct ia_css_dp_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.dp_config =
+		    (struct atomisp_dp_config *)&css_param->dp_config;
+	}
+
+	if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
+		if (arg->dz_config && (from_user || !cur_config->dz_config)) {
+			if (copy_from_compatible(&css_param->dz_config,
+						 arg->dz_config,
+						 sizeof(struct ia_css_dz_config),
+						 from_user))
+				return -EFAULT;
+			if (!atomisp_check_zoom_region(asd,
+						       &css_param->dz_config)) {
+				dev_err(asd->isp->dev, "crop region error!");
+				return -EINVAL;
+			}
+			css_param->update_flag.dz_config =
+			    (struct atomisp_dz_config *)
+			    &css_param->dz_config;
+		}
+	}
+
+	if (arg->nr_config && (from_user || !cur_config->nr_config)) {
+		if (copy_from_compatible(&css_param->nr_config, arg->nr_config,
+					 sizeof(struct ia_css_nr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.nr_config =
+		    (struct atomisp_nr_config *)&css_param->nr_config;
+	}
+
+	if (arg->ee_config && (from_user || !cur_config->ee_config)) {
+		if (copy_from_compatible(&css_param->ee_config, arg->ee_config,
+					 sizeof(struct ia_css_ee_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.ee_config =
+		    (struct atomisp_ee_config *)&css_param->ee_config;
+	}
+
+	if (arg->tnr_config && (from_user || !cur_config->tnr_config)) {
+		if (copy_from_compatible(&css_param->tnr_config,
+					 arg->tnr_config,
+					 sizeof(struct ia_css_tnr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.tnr_config =
+		    (struct atomisp_tnr_config *)
+		    &css_param->tnr_config;
+	}
+
+	if (arg->a3a_config && (from_user || !cur_config->a3a_config)) {
+		if (copy_from_compatible(&css_param->s3a_config,
+					 arg->a3a_config,
+					 sizeof(struct ia_css_3a_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.a3a_config =
+		    (struct atomisp_3a_config *)&css_param->s3a_config;
+	}
+
+	if (arg->ctc_config && (from_user || !cur_config->ctc_config)) {
+		if (copy_from_compatible(&css_param->ctc_config,
+					 arg->ctc_config,
+					 sizeof(struct ia_css_ctc_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.ctc_config =
+		    (struct atomisp_ctc_config *)
+		    &css_param->ctc_config;
+	}
+
+	if (arg->cnr_config && (from_user || !cur_config->cnr_config)) {
+		if (copy_from_compatible(&css_param->cnr_config,
+					 arg->cnr_config,
+					 sizeof(struct ia_css_cnr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.cnr_config =
+		    (struct atomisp_cnr_config *)
+		    &css_param->cnr_config;
+	}
+
+	if (arg->ecd_config && (from_user || !cur_config->ecd_config)) {
+		if (copy_from_compatible(&css_param->ecd_config,
+					 arg->ecd_config,
+					 sizeof(struct ia_css_ecd_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.ecd_config =
+		    (struct atomisp_ecd_config *)
+		    &css_param->ecd_config;
+	}
+
+	if (arg->ynr_config && (from_user || !cur_config->ynr_config)) {
+		if (copy_from_compatible(&css_param->ynr_config,
+					 arg->ynr_config,
+					 sizeof(struct ia_css_ynr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.ynr_config =
+		    (struct atomisp_ynr_config *)
+		    &css_param->ynr_config;
+	}
+
+	if (arg->fc_config && (from_user || !cur_config->fc_config)) {
+		if (copy_from_compatible(&css_param->fc_config,
+					 arg->fc_config,
+					 sizeof(struct ia_css_fc_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.fc_config =
+		    (struct atomisp_fc_config *)&css_param->fc_config;
+	}
+
+	if (arg->macc_config && (from_user || !cur_config->macc_config)) {
+		if (copy_from_compatible(&css_param->macc_config,
+					 arg->macc_config,
+					 sizeof(struct ia_css_macc_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.macc_config =
+		    (struct atomisp_macc_config *)
+		    &css_param->macc_config;
+	}
+
+	if (arg->aa_config && (from_user || !cur_config->aa_config)) {
+		if (copy_from_compatible(&css_param->aa_config, arg->aa_config,
+					 sizeof(struct ia_css_aa_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.aa_config =
+		    (struct atomisp_aa_config *)&css_param->aa_config;
+	}
+
+	if (arg->anr_config && (from_user || !cur_config->anr_config)) {
+		if (copy_from_compatible(&css_param->anr_config,
+					 arg->anr_config,
+					 sizeof(struct ia_css_anr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.anr_config =
+		    (struct atomisp_anr_config *)
+		    &css_param->anr_config;
+	}
+
+	if (arg->xnr_config && (from_user || !cur_config->xnr_config)) {
+		if (copy_from_compatible(&css_param->xnr_config,
+					 arg->xnr_config,
+					 sizeof(struct ia_css_xnr_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.xnr_config =
+		    (struct atomisp_xnr_config *)
+		    &css_param->xnr_config;
+	}
+
+	if (arg->yuv2rgb_cc_config &&
+	    (from_user || !cur_config->yuv2rgb_cc_config)) {
+		if (copy_from_compatible(&css_param->yuv2rgb_cc_config,
+					 arg->yuv2rgb_cc_config,
+					 sizeof(struct ia_css_cc_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.yuv2rgb_cc_config =
+		    (struct atomisp_cc_config *)
+		    &css_param->yuv2rgb_cc_config;
+	}
+
+	if (arg->rgb2yuv_cc_config &&
+	    (from_user || !cur_config->rgb2yuv_cc_config)) {
+		if (copy_from_compatible(&css_param->rgb2yuv_cc_config,
+					 arg->rgb2yuv_cc_config,
+					 sizeof(struct ia_css_cc_config),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.rgb2yuv_cc_config =
+		    (struct atomisp_cc_config *)
+		    &css_param->rgb2yuv_cc_config;
+	}
+
+	if (arg->macc_table && (from_user || !cur_config->macc_table)) {
+		if (copy_from_compatible(&css_param->macc_table,
+					 arg->macc_table,
+					 sizeof(struct ia_css_macc_table),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.macc_table =
+		    (struct atomisp_macc_table *)
+		    &css_param->macc_table;
+	}
+
+	if (arg->xnr_table && (from_user || !cur_config->xnr_table)) {
+		if (copy_from_compatible(&css_param->xnr_table,
+					 arg->xnr_table,
+					 sizeof(struct ia_css_xnr_table),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.xnr_table =
+		    (struct atomisp_xnr_table *)&css_param->xnr_table;
+	}
+
+	if (arg->r_gamma_table && (from_user || !cur_config->r_gamma_table)) {
+		if (copy_from_compatible(&css_param->r_gamma_table,
+					 arg->r_gamma_table,
+					 sizeof(struct ia_css_rgb_gamma_table),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.r_gamma_table =
+		    (struct atomisp_rgb_gamma_table *)
+		    &css_param->r_gamma_table;
+	}
+
+	if (arg->g_gamma_table && (from_user || !cur_config->g_gamma_table)) {
+		if (copy_from_compatible(&css_param->g_gamma_table,
+					 arg->g_gamma_table,
+					 sizeof(struct ia_css_rgb_gamma_table),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.g_gamma_table =
+		    (struct atomisp_rgb_gamma_table *)
+		    &css_param->g_gamma_table;
+	}
+
+	if (arg->b_gamma_table && (from_user || !cur_config->b_gamma_table)) {
+		if (copy_from_compatible(&css_param->b_gamma_table,
+					 arg->b_gamma_table,
+					 sizeof(struct ia_css_rgb_gamma_table),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.b_gamma_table =
+		    (struct atomisp_rgb_gamma_table *)
+		    &css_param->b_gamma_table;
+	}
+
+	if (arg->anr_thres && (from_user || !cur_config->anr_thres)) {
+		if (copy_from_compatible(&css_param->anr_thres, arg->anr_thres,
+					 sizeof(struct ia_css_anr_thres),
+					 from_user))
+			return -EFAULT;
+		css_param->update_flag.anr_thres =
+		    (struct atomisp_anr_thres *)&css_param->anr_thres;
+	}
+
+	if (from_user)
+		css_param->isp_config_id = arg->isp_config_id;
+	/*
+	 * These configurations are on used by ISP1.x, not for ISP2.x,
+	 * so do not handle them. see comments of ia_css_isp_config.
+	 * 1 cc_config
+	 * 2 ce_config
+	 * 3 de_config
+	 * 4 gc_config
+	 * 5 gamma_table
+	 * 6 ctc_table
+	 * 7 dvs_coefs
+	 */
+	return 0;
+}
+
+int atomisp_cp_lsc_table(struct atomisp_sub_device *asd,
+			 struct atomisp_shading_table *source_st,
+			 struct atomisp_css_params *css_param,
+			 bool from_user)
+{
+	unsigned int i;
+	unsigned int len_table;
+	struct ia_css_shading_table *shading_table;
+	struct ia_css_shading_table *old_table;
+	struct atomisp_shading_table *st, dest_st;
+
+	if (!source_st)
+		return 0;
+
+	if (!css_param)
+		return -EINVAL;
+
+	if (!from_user && css_param->update_flag.shading_table)
+		return 0;
+
+	if (IS_ISP2401) {
+		if (copy_from_compatible(&dest_st, source_st,
+					sizeof(struct atomisp_shading_table),
+					from_user)) {
+			dev_err(asd->isp->dev, "copy shading table failed!");
+			return -EFAULT;
+		}
+		st = &dest_st;
+	} else {
+		st = source_st;
+	}
+
+	old_table = css_param->shading_table;
+
+	/* user config is to disable the shading table. */
+	if (!st->enable) {
+		/* Generate a minimum table with enable = 0. */
+		shading_table = atomisp_css_shading_table_alloc(1, 1);
+		if (!shading_table)
+			return -ENOMEM;
+		shading_table->enable = 0;
+		goto set_lsc;
+	}
+
+	/* Setting a new table. Validate first - all tables must be set */
+	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+		if (!st->data[i]) {
+			dev_err(asd->isp->dev, "shading table validate failed");
+			return -EINVAL;
+		}
+	}
+
+	/* Shading table size per color */
+	if (st->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
+	    st->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR) {
+		dev_err(asd->isp->dev, "shading table w/h validate failed!");
+		return -EINVAL;
+	}
+
+	shading_table = atomisp_css_shading_table_alloc(st->width, st->height);
+	if (!shading_table)
+		return -ENOMEM;
+
+	len_table = st->width * st->height * ATOMISP_SC_TYPE_SIZE;
+	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+		if (copy_from_compatible(shading_table->data[i],
+					 st->data[i], len_table, from_user)) {
+			atomisp_css_shading_table_free(shading_table);
+			return -EFAULT;
+		}
+	}
+	shading_table->sensor_width = st->sensor_width;
+	shading_table->sensor_height = st->sensor_height;
+	shading_table->fraction_bits = st->fraction_bits;
+	shading_table->enable = st->enable;
+
+	/* No need to update shading table if it is the same */
+	if (old_table &&
+	    old_table->sensor_width == shading_table->sensor_width &&
+	    old_table->sensor_height == shading_table->sensor_height &&
+	    old_table->width == shading_table->width &&
+	    old_table->height == shading_table->height &&
+	    old_table->fraction_bits == shading_table->fraction_bits &&
+	    old_table->enable == shading_table->enable) {
+		bool data_is_same = true;
+
+		for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+			if (memcmp(shading_table->data[i], old_table->data[i],
+				   len_table) != 0) {
+				data_is_same = false;
+				break;
+			}
+		}
+
+		if (data_is_same) {
+			atomisp_css_shading_table_free(shading_table);
+			return 0;
+		}
+	}
+
+set_lsc:
+	/* set LSC to CSS */
+	css_param->shading_table = shading_table;
+	css_param->update_flag.shading_table = (struct atomisp_shading_table *)shading_table;
+	asd->params.sc_en = shading_table;
+
+	if (old_table)
+		atomisp_css_shading_table_free(old_table);
+
+	return 0;
+}
+
+int atomisp_css_cp_dvs2_coefs(struct atomisp_sub_device *asd,
+			      struct ia_css_dvs2_coefficients *coefs,
+			      struct atomisp_css_params *css_param,
+			      bool from_user)
+{
+	struct ia_css_dvs_grid_info *cur =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	int dvs_hor_coef_bytes, dvs_ver_coef_bytes;
+	struct ia_css_dvs2_coefficients dvs2_coefs;
+
+	if (!coefs || !cur)
+		return 0;
+
+	if (!from_user && css_param->update_flag.dvs2_coefs)
+		return 0;
+
+	if (!IS_ISP2401) {
+		if (sizeof(*cur) != sizeof(coefs->grid) ||
+		    memcmp(&coefs->grid, cur, sizeof(coefs->grid))) {
+			dev_err(asd->isp->dev, "dvs grid mis-match!\n");
+			/* If the grid info in the argument differs from the current
+			grid info, we tell the caller to reset the grid size and
+			try again. */
+			return -EAGAIN;
+		}
+
+		if (!coefs->hor_coefs.odd_real ||
+		    !coefs->hor_coefs.odd_imag ||
+		    !coefs->hor_coefs.even_real ||
+		    !coefs->hor_coefs.even_imag ||
+		    !coefs->ver_coefs.odd_real ||
+		    !coefs->ver_coefs.odd_imag ||
+		    !coefs->ver_coefs.even_real ||
+		    !coefs->ver_coefs.even_imag)
+			return -EINVAL;
+
+		if (!css_param->dvs2_coeff) {
+			/* DIS coefficients. */
+			css_param->dvs2_coeff = ia_css_dvs2_coefficients_allocate(cur);
+			if (!css_param->dvs2_coeff)
+				return -ENOMEM;
+		}
+
+		dvs_hor_coef_bytes = asd->params.dvs_hor_coef_bytes;
+		dvs_ver_coef_bytes = asd->params.dvs_ver_coef_bytes;
+		if (copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_real,
+					coefs->hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
+					coefs->hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
+					coefs->hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
+					coefs->hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
+					coefs->ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
+					coefs->ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
+					coefs->ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
+					coefs->ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
+			ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
+			css_param->dvs2_coeff = NULL;
+			return -EFAULT;
+		}
+	} else {
+		if (copy_from_compatible(&dvs2_coefs, coefs,
+					sizeof(struct ia_css_dvs2_coefficients),
+					from_user)) {
+			dev_err(asd->isp->dev, "copy dvs2 coef failed");
+			return -EFAULT;
+		}
+
+		if (sizeof(*cur) != sizeof(dvs2_coefs.grid) ||
+		    memcmp(&dvs2_coefs.grid, cur, sizeof(dvs2_coefs.grid))) {
+			dev_err(asd->isp->dev, "dvs grid mis-match!\n");
+			/* If the grid info in the argument differs from the current
+			grid info, we tell the caller to reset the grid size and
+			try again. */
+			return -EAGAIN;
+		}
+
+		if (!dvs2_coefs.hor_coefs.odd_real ||
+		    !dvs2_coefs.hor_coefs.odd_imag ||
+		    !dvs2_coefs.hor_coefs.even_real ||
+		    !dvs2_coefs.hor_coefs.even_imag ||
+		    !dvs2_coefs.ver_coefs.odd_real ||
+		    !dvs2_coefs.ver_coefs.odd_imag ||
+		    !dvs2_coefs.ver_coefs.even_real ||
+		    !dvs2_coefs.ver_coefs.even_imag)
+			return -EINVAL;
+
+		if (!css_param->dvs2_coeff) {
+			/* DIS coefficients. */
+			css_param->dvs2_coeff = ia_css_dvs2_coefficients_allocate(cur);
+			if (!css_param->dvs2_coeff)
+				return -ENOMEM;
+		}
+
+		dvs_hor_coef_bytes = asd->params.dvs_hor_coef_bytes;
+		dvs_ver_coef_bytes = asd->params.dvs_ver_coef_bytes;
+		if (copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_real,
+					dvs2_coefs.hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
+					dvs2_coefs.hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
+					dvs2_coefs.hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
+					dvs2_coefs.hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
+					dvs2_coefs.ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
+					dvs2_coefs.ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
+					dvs2_coefs.ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
+		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
+					dvs2_coefs.ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
+			ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
+			css_param->dvs2_coeff = NULL;
+			return -EFAULT;
+		}
+	}
+
+	css_param->update_flag.dvs2_coefs =
+	    (struct atomisp_dis_coefficients *)css_param->dvs2_coeff;
+	return 0;
+}
+
+int atomisp_cp_dvs_6axis_config(struct atomisp_sub_device *asd,
+				struct atomisp_dvs_6axis_config *source_6axis_config,
+				struct atomisp_css_params *css_param,
+				bool from_user)
+{
+	struct ia_css_dvs_6axis_config *dvs_6axis_config;
+	struct ia_css_dvs_6axis_config *old_6axis_config;
+	struct ia_css_stream *stream =
+		    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
+	struct ia_css_dvs_grid_info *dvs_grid_info =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	int ret = -EFAULT;
+
+	if (!stream) {
+		dev_err(asd->isp->dev, "%s: internal error!", __func__);
+		return -EINVAL;
+	}
+
+	if (!source_6axis_config || !dvs_grid_info)
+		return 0;
+
+	if (!dvs_grid_info->enable)
+		return 0;
+
+	if (!from_user && css_param->update_flag.dvs_6axis_config)
+		return 0;
+
+	/* check whether need to reallocate for 6 axis config */
+	old_6axis_config = css_param->dvs_6axis;
+	dvs_6axis_config = old_6axis_config;
+
+	if (IS_ISP2401) {
+		struct ia_css_dvs_6axis_config t_6axis_config;
+
+		if (copy_from_compatible(&t_6axis_config, source_6axis_config,
+					sizeof(struct atomisp_dvs_6axis_config),
+					from_user)) {
+			dev_err(asd->isp->dev, "copy morph table failed!");
+			return -EFAULT;
+		}
+
+		if (old_6axis_config &&
+		    (old_6axis_config->width_y != t_6axis_config.width_y ||
+		    old_6axis_config->height_y != t_6axis_config.height_y ||
+		    old_6axis_config->width_uv != t_6axis_config.width_uv ||
+		    old_6axis_config->height_uv != t_6axis_config.height_uv)) {
+			ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
+			css_param->dvs_6axis = NULL;
+
+			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
+			if (!dvs_6axis_config)
+				return -ENOMEM;
+		} else if (!dvs_6axis_config) {
+			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
+			if (!dvs_6axis_config)
+				return -ENOMEM;
+		}
+
+		dvs_6axis_config->exp_id = t_6axis_config.exp_id;
+
+		if (copy_from_compatible(dvs_6axis_config->xcoords_y,
+					t_6axis_config.xcoords_y,
+					t_6axis_config.width_y *
+					t_6axis_config.height_y *
+					sizeof(*dvs_6axis_config->xcoords_y),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->ycoords_y,
+					t_6axis_config.ycoords_y,
+					t_6axis_config.width_y *
+					t_6axis_config.height_y *
+					sizeof(*dvs_6axis_config->ycoords_y),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
+					t_6axis_config.xcoords_uv,
+					t_6axis_config.width_uv *
+					t_6axis_config.height_uv *
+					sizeof(*dvs_6axis_config->xcoords_uv),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
+					t_6axis_config.ycoords_uv,
+					t_6axis_config.width_uv *
+					t_6axis_config.height_uv *
+					sizeof(*dvs_6axis_config->ycoords_uv),
+					from_user))
+			goto error;
+	} else {
+		if (old_6axis_config &&
+		    (old_6axis_config->width_y != source_6axis_config->width_y ||
+		    old_6axis_config->height_y != source_6axis_config->height_y ||
+		    old_6axis_config->width_uv != source_6axis_config->width_uv ||
+		    old_6axis_config->height_uv != source_6axis_config->height_uv)) {
+			ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
+			css_param->dvs_6axis = NULL;
+
+			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
+			if (!dvs_6axis_config)
+				return -ENOMEM;
+		} else if (!dvs_6axis_config) {
+			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
+			if (!dvs_6axis_config)
+				return -ENOMEM;
+		}
+
+		dvs_6axis_config->exp_id = source_6axis_config->exp_id;
+
+		if (copy_from_compatible(dvs_6axis_config->xcoords_y,
+					source_6axis_config->xcoords_y,
+					source_6axis_config->width_y *
+					source_6axis_config->height_y *
+					sizeof(*source_6axis_config->xcoords_y),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->ycoords_y,
+					source_6axis_config->ycoords_y,
+					source_6axis_config->width_y *
+					source_6axis_config->height_y *
+					sizeof(*source_6axis_config->ycoords_y),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
+					source_6axis_config->xcoords_uv,
+					source_6axis_config->width_uv *
+					source_6axis_config->height_uv *
+					sizeof(*source_6axis_config->xcoords_uv),
+					from_user))
+			goto error;
+		if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
+					source_6axis_config->ycoords_uv,
+					source_6axis_config->width_uv *
+					source_6axis_config->height_uv *
+					sizeof(*source_6axis_config->ycoords_uv),
+					from_user))
+			goto error;
+	}
+	css_param->dvs_6axis = dvs_6axis_config;
+	css_param->update_flag.dvs_6axis_config =
+	    (struct atomisp_dvs_6axis_config *)dvs_6axis_config;
+	return 0;
+
+error:
+	if (dvs_6axis_config)
+		ia_css_dvs2_6axis_config_free(dvs_6axis_config);
+	return ret;
+}
+
+int atomisp_cp_morph_table(struct atomisp_sub_device *asd,
+			   struct atomisp_morph_table *source_morph_table,
+			   struct atomisp_css_params *css_param,
+			   bool from_user)
+{
+	int ret = -EFAULT;
+	unsigned int i;
+	struct ia_css_morph_table *morph_table;
+	struct ia_css_morph_table *old_morph_table;
+
+	if (!source_morph_table)
+		return 0;
+
+	if (!from_user && css_param->update_flag.morph_table)
+		return 0;
+
+	old_morph_table = css_param->morph_table;
+
+	if (IS_ISP2401) {
+		struct ia_css_morph_table mtbl;
+
+		if (copy_from_compatible(&mtbl, source_morph_table,
+				sizeof(struct atomisp_morph_table),
+				from_user)) {
+			dev_err(asd->isp->dev, "copy morph table failed!");
+			return -EFAULT;
+		}
+
+		morph_table = atomisp_css_morph_table_allocate(
+				mtbl.width,
+				mtbl.height);
+		if (!morph_table)
+			return -ENOMEM;
+
+		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+			if (copy_from_compatible(morph_table->coordinates_x[i],
+						(__force void *)source_morph_table->coordinates_x[i],
+						mtbl.height * mtbl.width *
+						sizeof(*morph_table->coordinates_x[i]),
+						from_user))
+				goto error;
+
+			if (copy_from_compatible(morph_table->coordinates_y[i],
+						(__force void *)source_morph_table->coordinates_y[i],
+						mtbl.height * mtbl.width *
+						sizeof(*morph_table->coordinates_y[i]),
+						from_user))
+				goto error;
+		}
+	} else {
+		morph_table = atomisp_css_morph_table_allocate(
+				source_morph_table->width,
+				source_morph_table->height);
+		if (!morph_table)
+			return -ENOMEM;
+
+		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+			if (copy_from_compatible(morph_table->coordinates_x[i],
+						(__force void *)source_morph_table->coordinates_x[i],
+						source_morph_table->height * source_morph_table->width *
+						sizeof(*source_morph_table->coordinates_x[i]),
+						from_user))
+				goto error;
+
+			if (copy_from_compatible(morph_table->coordinates_y[i],
+						(__force void *)source_morph_table->coordinates_y[i],
+						source_morph_table->height * source_morph_table->width *
+						sizeof(*source_morph_table->coordinates_y[i]),
+						from_user))
+				goto error;
+		}
+	}
+
+	css_param->morph_table = morph_table;
+	if (old_morph_table)
+		atomisp_css_morph_table_free(old_morph_table);
+	css_param->update_flag.morph_table =
+	    (struct atomisp_morph_table *)morph_table;
+	return 0;
+
+error:
+	if (morph_table)
+		atomisp_css_morph_table_free(morph_table);
+	return ret;
+}
+
+int atomisp_makeup_css_parameters(struct atomisp_sub_device *asd,
+				  struct atomisp_parameters *arg,
+				  struct atomisp_css_params *css_param)
+{
+	int ret;
+
+	ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, false);
+	if (ret)
+		return ret;
+	ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, false);
+	if (ret)
+		return ret;
+	ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, false);
+	if (ret)
+		return ret;
+	ret = atomisp_css_cp_dvs2_coefs(asd,
+					(struct ia_css_dvs2_coefficients *)arg->dvs2_coefs,
+					css_param, false);
+	if (ret)
+		return ret;
+	ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
+					  css_param, false);
+	return ret;
+}
+
+void atomisp_free_css_parameters(struct atomisp_css_params *css_param)
+{
+	if (css_param->dvs_6axis) {
+		ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
+		css_param->dvs_6axis = NULL;
+	}
+	if (css_param->dvs2_coeff) {
+		ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
+		css_param->dvs2_coeff = NULL;
+	}
+	if (css_param->shading_table) {
+		ia_css_shading_table_free(css_param->shading_table);
+		css_param->shading_table = NULL;
+	}
+	if (css_param->morph_table) {
+		ia_css_morph_table_free(css_param->morph_table);
+		css_param->morph_table = NULL;
+	}
+}
+
+/*
+ * Check parameter queue list and buffer queue list to find out if matched items
+ * and then set parameter to CSS and enqueue buffer to CSS.
+ * Of course, if the buffer in buffer waiting list is not bound to a per-frame
+ * parameter, it will be enqueued into CSS as long as the per-frame setting
+ * buffers before it get enqueued.
+ */
+void atomisp_handle_parameter_and_buffer(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct videobuf_buffer *vb = NULL, *vb_tmp;
+	struct atomisp_css_params_with_list *param = NULL, *param_tmp;
+	struct videobuf_vmalloc_memory *vm_mem = NULL;
+	unsigned long irqflags;
+	bool need_to_enqueue_buffer = false;
+
+	lockdep_assert_held(&asd->isp->mutex);
+
+	if (!asd) {
+		dev_err(pipe->isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, pipe->vdev.name);
+		return;
+	}
+
+	if (atomisp_is_vf_pipe(pipe))
+		return;
+
+	/*
+	 * CSS/FW requires set parameter and enqueue buffer happen after ISP
+	 * is streamon.
+	 */
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+		return;
+
+	if (list_empty(&pipe->per_frame_params) ||
+	    list_empty(&pipe->buffers_waiting_for_param))
+		return;
+
+	list_for_each_entry_safe(vb, vb_tmp,
+				 &pipe->buffers_waiting_for_param, queue) {
+		if (pipe->frame_request_config_id[vb->i]) {
+			list_for_each_entry_safe(param, param_tmp,
+						 &pipe->per_frame_params, list) {
+				if (pipe->frame_request_config_id[vb->i] !=
+				    param->params.isp_config_id)
+					continue;
+
+				list_del(&param->list);
+				list_del(&vb->queue);
+				/*
+				 * clear the request config id as the buffer
+				 * will be handled and enqueued into CSS soon
+				 */
+				pipe->frame_request_config_id[vb->i] = 0;
+				pipe->frame_params[vb->i] = param;
+				vm_mem = vb->priv;
+				BUG_ON(!vm_mem);
+				break;
+			}
+
+			if (vm_mem) {
+				spin_lock_irqsave(&pipe->irq_lock, irqflags);
+				list_add_tail(&vb->queue, &pipe->activeq);
+				spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+				vm_mem = NULL;
+				need_to_enqueue_buffer = true;
+			} else {
+				/* The is the end, stop further loop */
+				break;
+			}
+		} else {
+			list_del(&vb->queue);
+			pipe->frame_params[vb->i] = NULL;
+			spin_lock_irqsave(&pipe->irq_lock, irqflags);
+			list_add_tail(&vb->queue, &pipe->activeq);
+			spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+			need_to_enqueue_buffer = true;
+		}
+	}
+
+	if (!need_to_enqueue_buffer)
+		return;
+
+	atomisp_qbuffers_to_css(asd);
+}
+
+/*
+* Function to configure ISP parameters
+*/
+int atomisp_set_parameters(struct video_device *vdev,
+			   struct atomisp_parameters *arg)
+{
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct atomisp_css_params_with_list *param = NULL;
+	struct atomisp_css_params *css_param = &asd->params.css_param;
+	int ret;
+
+	lockdep_assert_held(&asd->isp->mutex);
+
+	if (!asd) {
+		dev_err(pipe->isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(asd->isp->dev, "%s: internal error!\n", __func__);
+		return -EINVAL;
+	}
+
+	dev_dbg(asd->isp->dev,
+		"%s: set parameter(per_frame_setting %d) for asd%d with isp_config_id %d of %s\n",
+		__func__, arg->per_frame_setting, asd->index,
+		arg->isp_config_id, vdev->name);
+
+	if (IS_ISP2401) {
+		if (atomisp_is_vf_pipe(pipe) && arg->per_frame_setting) {
+			dev_err(asd->isp->dev, "%s: vf pipe not support per_frame_setting",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	if (arg->per_frame_setting && !atomisp_is_vf_pipe(pipe)) {
+		/*
+		 * Per-frame setting enabled, we allocate a new parameter
+		 * buffer to cache the parameters and only when frame buffers
+		 * are ready, the parameters will be set to CSS.
+		 * per-frame setting only works for the main output frame.
+		 */
+		param = kvzalloc(sizeof(*param), GFP_KERNEL);
+		if (!param) {
+			dev_err(asd->isp->dev, "%s: failed to alloc params buffer\n",
+				__func__);
+			return -ENOMEM;
+		}
+		css_param = &param->params;
+	}
+
+	ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, true);
+	if (ret)
+		goto apply_parameter_failed;
+
+	ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, true);
+	if (ret)
+		goto apply_parameter_failed;
+
+	ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, true);
+	if (ret)
+		goto apply_parameter_failed;
+
+	ret = atomisp_css_cp_dvs2_coefs(asd,
+					(struct ia_css_dvs2_coefficients *)arg->dvs2_coefs,
+					css_param, true);
+	if (ret)
+		goto apply_parameter_failed;
+
+	ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
+					  css_param, true);
+	if (ret)
+		goto apply_parameter_failed;
+
+	if (!(arg->per_frame_setting && !atomisp_is_vf_pipe(pipe))) {
+		/* indicate to CSS that we have parameters to be updated */
+		asd->params.css_update_params_needed = true;
+	} else {
+		list_add_tail(&param->list, &pipe->per_frame_params);
+		atomisp_handle_parameter_and_buffer(pipe);
+	}
+
+	return 0;
+
+apply_parameter_failed:
+	if (css_param)
+		atomisp_free_css_parameters(css_param);
+	kvfree(param);
+
+	return ret;
+}
+
+/*
+ * Function to set/get isp parameters to isp
+ */
+int atomisp_param(struct atomisp_sub_device *asd, int flag,
+		  struct atomisp_parm *config)
+{
+	struct ia_css_pipe_config *vp_cfg =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		    pipe_configs[IA_CSS_PIPE_ID_VIDEO];
+
+	/* Read parameter for 3A binary info */
+	if (flag == 0) {
+		struct ia_css_dvs_grid_info *dvs_grid_info =
+		    atomisp_css_get_dvs_grid_info(
+			&asd->params.curr_grid_info);
+
+		atomisp_curr_user_grid_info(asd, &config->info);
+
+		/* We always return the resolution and stride even if there is
+		 * no valid metadata. This allows the caller to get the
+		 * information needed to allocate user-space buffers. */
+		config->metadata_config.metadata_height = asd->
+			stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
+			metadata_info.resolution.height;
+		config->metadata_config.metadata_stride = asd->
+			stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
+			metadata_info.stride;
+
+		/* update dvs grid info */
+		if (dvs_grid_info)
+			memcpy(&config->dvs_grid,
+			       dvs_grid_info,
+			       sizeof(struct ia_css_dvs_grid_info));
+
+		if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
+			config->dvs_envelop.width = 0;
+			config->dvs_envelop.height = 0;
+			return 0;
+		}
+
+		/* update dvs envelop info */
+		if (!asd->continuous_mode->val) {
+			config->dvs_envelop.width = vp_cfg->dvs_envelope.width;
+			config->dvs_envelop.height =
+			    vp_cfg->dvs_envelope.height;
+		} else {
+			unsigned int dvs_w, dvs_h, dvs_w_max, dvs_h_max;
+
+			dvs_w = vp_cfg->bayer_ds_out_res.width -
+				vp_cfg->output_info[0].res.width;
+			dvs_h = vp_cfg->bayer_ds_out_res.height -
+				vp_cfg->output_info[0].res.height;
+			dvs_w_max = rounddown(
+					vp_cfg->output_info[0].res.width / 5,
+					ATOM_ISP_STEP_WIDTH);
+			dvs_h_max = rounddown(
+					vp_cfg->output_info[0].res.height / 5,
+					ATOM_ISP_STEP_HEIGHT);
+
+			config->dvs_envelop.width = min(dvs_w, dvs_w_max);
+			config->dvs_envelop.height = min(dvs_h, dvs_h_max);
+		}
+
+		return 0;
+	}
+
+	memcpy(&asd->params.css_param.wb_config, &config->wb_config,
+	       sizeof(struct ia_css_wb_config));
+	memcpy(&asd->params.css_param.ob_config, &config->ob_config,
+	       sizeof(struct ia_css_ob_config));
+	memcpy(&asd->params.css_param.dp_config, &config->dp_config,
+	       sizeof(struct ia_css_dp_config));
+	memcpy(&asd->params.css_param.de_config, &config->de_config,
+	       sizeof(struct ia_css_de_config));
+	memcpy(&asd->params.css_param.dz_config, &config->dz_config,
+	       sizeof(struct ia_css_dz_config));
+	memcpy(&asd->params.css_param.ce_config, &config->ce_config,
+	       sizeof(struct ia_css_ce_config));
+	memcpy(&asd->params.css_param.nr_config, &config->nr_config,
+	       sizeof(struct ia_css_nr_config));
+	memcpy(&asd->params.css_param.ee_config, &config->ee_config,
+	       sizeof(struct ia_css_ee_config));
+	memcpy(&asd->params.css_param.tnr_config, &config->tnr_config,
+	       sizeof(struct ia_css_tnr_config));
+
+	if (asd->params.color_effect == V4L2_COLORFX_NEGATIVE) {
+		asd->params.css_param.cc_config.matrix[3] = -config->cc_config.matrix[3];
+		asd->params.css_param.cc_config.matrix[4] = -config->cc_config.matrix[4];
+		asd->params.css_param.cc_config.matrix[5] = -config->cc_config.matrix[5];
+		asd->params.css_param.cc_config.matrix[6] = -config->cc_config.matrix[6];
+		asd->params.css_param.cc_config.matrix[7] = -config->cc_config.matrix[7];
+		asd->params.css_param.cc_config.matrix[8] = -config->cc_config.matrix[8];
+	}
+
+	if (asd->params.color_effect != V4L2_COLORFX_SEPIA &&
+	    asd->params.color_effect != V4L2_COLORFX_BW) {
+		memcpy(&asd->params.css_param.cc_config, &config->cc_config,
+		       sizeof(struct ia_css_cc_config));
+		asd->params.config.cc_config = &asd->params.css_param.cc_config;
+	}
+
+	asd->params.config.wb_config = &asd->params.css_param.wb_config;
+	asd->params.config.ob_config = &asd->params.css_param.ob_config;
+	asd->params.config.de_config = &asd->params.css_param.de_config;
+	asd->params.config.dz_config = &asd->params.css_param.dz_config;
+	asd->params.config.ce_config = &asd->params.css_param.ce_config;
+	asd->params.config.dp_config = &asd->params.css_param.dp_config;
+	asd->params.config.nr_config = &asd->params.css_param.nr_config;
+	asd->params.config.ee_config = &asd->params.css_param.ee_config;
+	asd->params.config.tnr_config = &asd->params.css_param.tnr_config;
+	asd->params.css_update_params_needed = true;
+
+	return 0;
+}
+
+/*
+ * Function to configure color effect of the image
+ */
+int atomisp_color_effect(struct atomisp_sub_device *asd, int flag,
+			 __s32 *effect)
+{
+	struct ia_css_cc_config *cc_config = NULL;
+	struct ia_css_macc_table *macc_table = NULL;
+	struct ia_css_ctc_table *ctc_table = NULL;
+	int ret = 0;
+	struct v4l2_control control;
+	struct atomisp_device *isp = asd->isp;
+
+	if (flag == 0) {
+		*effect = asd->params.color_effect;
+		return 0;
+	}
+
+	control.id = V4L2_CID_COLORFX;
+	control.value = *effect;
+	ret =
+	    v4l2_s_ctrl(NULL, isp->inputs[asd->input_curr].camera->ctrl_handler,
+			&control);
+	/*
+	 * if set color effect to sensor successfully, return
+	 * 0 directly.
+	 */
+	if (!ret) {
+		asd->params.color_effect = (u32)*effect;
+		return 0;
+	}
+
+	if (*effect == asd->params.color_effect)
+		return 0;
+
+	/*
+	 * isp_subdev->params.macc_en should be set to false.
+	 */
+	asd->params.macc_en = false;
+
+	switch (*effect) {
+	case V4L2_COLORFX_NONE:
+		macc_table = &asd->params.css_param.macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		cc_config = &sepia_cc_config;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		cc_config = &nega_cc_config;
+		break;
+	case V4L2_COLORFX_BW:
+		cc_config = &mono_cc_config;
+		break;
+	case V4L2_COLORFX_SKY_BLUE:
+		macc_table = &blue_macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_GRASS_GREEN:
+		macc_table = &green_macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN_LOW:
+		macc_table = &skin_low_macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN:
+		macc_table = &skin_medium_macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_SKIN_WHITEN_HIGH:
+		macc_table = &skin_high_macc_table;
+		asd->params.macc_en = true;
+		break;
+	case V4L2_COLORFX_VIVID:
+		ctc_table = &vivid_ctc_table;
+		break;
+	default:
+		return -EINVAL;
+	}
+	atomisp_update_capture_mode(asd);
+
+	if (cc_config)
+		asd->params.config.cc_config = cc_config;
+	if (macc_table)
+		asd->params.config.macc_table = macc_table;
+	if (ctc_table)
+		atomisp_css_set_ctc_table(asd, ctc_table);
+	asd->params.color_effect = (u32)*effect;
+	asd->params.css_update_params_needed = true;
+	return 0;
+}
+
+/*
+ * Function to configure bad pixel correction
+ */
+int atomisp_bad_pixel(struct atomisp_sub_device *asd, int flag,
+		      __s32 *value)
+{
+	if (flag == 0) {
+		*value = asd->params.bad_pixel_en;
+		return 0;
+	}
+	asd->params.bad_pixel_en = !!*value;
+
+	return 0;
+}
+
+/*
+ * Function to configure bad pixel correction params
+ */
+int atomisp_bad_pixel_param(struct atomisp_sub_device *asd, int flag,
+			    struct atomisp_dp_config *config)
+{
+	if (flag == 0) {
+		/* Get bad pixel from current setup */
+		if (atomisp_css_get_dp_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set bad pixel to isp parameters */
+		memcpy(&asd->params.css_param.dp_config, config,
+		       sizeof(asd->params.css_param.dp_config));
+		asd->params.config.dp_config = &asd->params.css_param.dp_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to enable/disable video image stablization
+ */
+int atomisp_video_stable(struct atomisp_sub_device *asd, int flag,
+			 __s32 *value)
+{
+	if (flag == 0)
+		*value = asd->params.video_dis_en;
+	else
+		asd->params.video_dis_en = !!*value;
+
+	return 0;
+}
+
+/*
+ * Function to configure fixed pattern noise
+ */
+int atomisp_fixed_pattern(struct atomisp_sub_device *asd, int flag,
+			  __s32 *value)
+{
+	if (flag == 0) {
+		*value = asd->params.fpn_en;
+		return 0;
+	}
+
+	if (*value == 0) {
+		asd->params.fpn_en = false;
+		return 0;
+	}
+
+	/* Add function to get black from from sensor with shutter off */
+	return 0;
+}
+
+static unsigned int
+atomisp_bytesperline_to_padded_width(unsigned int bytesperline,
+				     enum ia_css_frame_format format)
+{
+	switch (format) {
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_RAW:
+	case IA_CSS_FRAME_FORMAT_RGB565:
+		return bytesperline / 2;
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+		return bytesperline / 4;
+	/* The following cases could be removed, but we leave them
+	   in to document the formats that are included. */
+	case IA_CSS_FRAME_FORMAT_NV11:
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV16:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_NV61:
+	case IA_CSS_FRAME_FORMAT_YV12:
+	case IA_CSS_FRAME_FORMAT_YV16:
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+	case IA_CSS_FRAME_FORMAT_YUV422:
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+	case IA_CSS_FRAME_FORMAT_YUV444:
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+	default:
+		return bytesperline;
+	}
+}
+
+static int
+atomisp_v4l2_framebuffer_to_css_frame(const struct v4l2_framebuffer *arg,
+				      struct ia_css_frame **result)
+{
+	struct ia_css_frame *res = NULL;
+	unsigned int padded_width;
+	enum ia_css_frame_format sh_format;
+	char *tmp_buf = NULL;
+	int ret = 0;
+
+	sh_format = v4l2_fmt_to_sh_fmt(arg->fmt.pixelformat);
+	padded_width = atomisp_bytesperline_to_padded_width(
+			   arg->fmt.bytesperline, sh_format);
+
+	/* Note: the padded width on an ia_css_frame is in elements, not in
+	   bytes. The RAW frame we use here should always be a 16bit RAW
+	   frame. This is why we bytesperline/2 is equal to the padded with */
+	if (ia_css_frame_allocate(&res, arg->fmt.width, arg->fmt.height,
+				       sh_format, padded_width, 0)) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	tmp_buf = vmalloc(arg->fmt.sizeimage);
+	if (!tmp_buf) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	if (copy_from_user(tmp_buf, (void __user __force *)arg->base,
+			   arg->fmt.sizeimage)) {
+		ret = -EFAULT;
+		goto err;
+	}
+
+	if (hmm_store(res->data, tmp_buf, arg->fmt.sizeimage)) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+err:
+	if (ret && res)
+		ia_css_frame_free(res);
+	vfree(tmp_buf);
+	if (ret == 0)
+		*result = res;
+	return ret;
+}
+
+/*
+ * Function to configure fixed pattern noise table
+ */
+int atomisp_fixed_pattern_table(struct atomisp_sub_device *asd,
+				struct v4l2_framebuffer *arg)
+{
+	struct ia_css_frame *raw_black_frame = NULL;
+	int ret;
+
+	if (!arg)
+		return -EINVAL;
+
+	ret = atomisp_v4l2_framebuffer_to_css_frame(arg, &raw_black_frame);
+	if (ret)
+		return ret;
+
+	if (sh_css_set_black_frame(asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+				   raw_black_frame) != 0)
+		return -ENOMEM;
+
+	ia_css_frame_free(raw_black_frame);
+	return ret;
+}
+
+/*
+ * Function to configure false color correction
+ */
+int atomisp_false_color(struct atomisp_sub_device *asd, int flag,
+			__s32 *value)
+{
+	/* Get nr config from current setup */
+	if (flag == 0) {
+		*value = asd->params.false_color;
+		return 0;
+	}
+
+	/* Set nr config to isp parameters */
+	if (*value) {
+		asd->params.config.de_config = NULL;
+	} else {
+		asd->params.css_param.de_config.pixelnoise = 0;
+		asd->params.config.de_config = &asd->params.css_param.de_config;
+	}
+	asd->params.css_update_params_needed = true;
+	asd->params.false_color = *value;
+	return 0;
+}
+
+/*
+ * Function to configure bad pixel correction params
+ */
+int atomisp_false_color_param(struct atomisp_sub_device *asd, int flag,
+			      struct atomisp_de_config *config)
+{
+	if (flag == 0) {
+		/* Get false color from current setup */
+		if (atomisp_css_get_de_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set false color to isp parameters */
+		memcpy(&asd->params.css_param.de_config, config,
+		       sizeof(asd->params.css_param.de_config));
+		asd->params.config.de_config = &asd->params.css_param.de_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to configure white balance params
+ */
+int atomisp_white_balance_param(struct atomisp_sub_device *asd, int flag,
+				struct atomisp_wb_config *config)
+{
+	if (flag == 0) {
+		/* Get white balance from current setup */
+		if (atomisp_css_get_wb_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set white balance to isp parameters */
+		memcpy(&asd->params.css_param.wb_config, config,
+		       sizeof(asd->params.css_param.wb_config));
+		asd->params.config.wb_config = &asd->params.css_param.wb_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+int atomisp_3a_config_param(struct atomisp_sub_device *asd, int flag,
+			    struct atomisp_3a_config *config)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	dev_dbg(isp->dev, ">%s %d\n", __func__, flag);
+
+	if (flag == 0) {
+		/* Get white balance from current setup */
+		if (atomisp_css_get_3a_config(asd, config))
+			return -EINVAL;
+	} else {
+		/* Set white balance to isp parameters */
+		memcpy(&asd->params.css_param.s3a_config, config,
+		       sizeof(asd->params.css_param.s3a_config));
+		asd->params.config.s3a_config = &asd->params.css_param.s3a_config;
+		asd->params.css_update_params_needed = true;
+	}
+
+	dev_dbg(isp->dev, "<%s %d\n", __func__, flag);
+	return 0;
+}
+
+/*
+ * Function to setup digital zoom
+ */
+int atomisp_digital_zoom(struct atomisp_sub_device *asd, int flag,
+			 __s32 *value)
+{
+	u32 zoom;
+	struct atomisp_device *isp = asd->isp;
+
+	unsigned int max_zoom = MRFLD_MAX_ZOOM_FACTOR;
+
+	if (flag == 0) {
+		atomisp_css_get_zoom_factor(asd, &zoom);
+		*value = max_zoom - zoom;
+	} else {
+		if (*value < 0)
+			return -EINVAL;
+
+		zoom = max_zoom - min_t(u32, max_zoom - 1, *value);
+		atomisp_css_set_zoom_factor(asd, zoom);
+
+		dev_dbg(isp->dev, "%s, zoom: %d\n", __func__, zoom);
+		asd->params.css_update_params_needed = true;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to get sensor specific info for current resolution,
+ * which will be used for auto exposure conversion.
+ */
+int atomisp_get_sensor_mode_data(struct atomisp_sub_device *asd,
+				 struct atomisp_sensor_mode_data *config)
+{
+	struct camera_mipi_info *mipi_info;
+	struct atomisp_device *isp = asd->isp;
+
+	mipi_info = atomisp_to_sensor_mipi_info(
+			isp->inputs[asd->input_curr].camera);
+	if (!mipi_info)
+		return -EINVAL;
+
+	memcpy(config, &mipi_info->data, sizeof(*config));
+	return 0;
+}
+
+static void __atomisp_update_stream_env(struct atomisp_sub_device *asd,
+					u16 stream_index, struct atomisp_input_stream_info *stream_info)
+{
+	int i;
+
+	/* assign virtual channel id return from sensor driver query */
+	asd->stream_env[stream_index].ch_id = stream_info->ch_id;
+	asd->stream_env[stream_index].isys_configs = stream_info->isys_configs;
+	for (i = 0; i < stream_info->isys_configs; i++) {
+		asd->stream_env[stream_index].isys_info[i].input_format =
+		    stream_info->isys_info[i].input_format;
+		asd->stream_env[stream_index].isys_info[i].width =
+		    stream_info->isys_info[i].width;
+		asd->stream_env[stream_index].isys_info[i].height =
+		    stream_info->isys_info[i].height;
+	}
+}
+
+static void __atomisp_init_stream_info(u16 stream_index,
+				       struct atomisp_input_stream_info *stream_info)
+{
+	int i;
+
+	stream_info->enable = 1;
+	stream_info->stream = stream_index;
+	stream_info->ch_id = 0;
+	stream_info->isys_configs = 0;
+	for (i = 0; i < MAX_STREAMS_PER_CHANNEL; i++) {
+		stream_info->isys_info[i].input_format = 0;
+		stream_info->isys_info[i].width = 0;
+		stream_info->isys_info[i].height = 0;
+	}
+}
+
+/* This function looks up the closest available resolution. */
+int atomisp_try_fmt(struct video_device *vdev, struct v4l2_pix_format *f,
+		    bool *res_overflow)
+{
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_state pad_state = {
+		.pads = &pad_cfg
+		};
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+
+	struct v4l2_mbus_framefmt *snr_mbus_fmt = &format.format;
+	const struct atomisp_format_bridge *fmt;
+	struct atomisp_input_stream_info *stream_info =
+	    (struct atomisp_input_stream_info *)snr_mbus_fmt->reserved;
+	int ret;
+
+	if (!asd) {
+		dev_err(isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	if (!isp->inputs[asd->input_curr].camera)
+		return -EINVAL;
+
+	fmt = atomisp_get_format_bridge(f->pixelformat);
+	if (!fmt) {
+		dev_err(isp->dev, "unsupported pixelformat!\n");
+		fmt = atomisp_output_fmts;
+	}
+
+	if (f->width <= 0 || f->height <= 0)
+		return -EINVAL;
+
+	snr_mbus_fmt->code = fmt->mbus_code;
+	snr_mbus_fmt->width = f->width;
+	snr_mbus_fmt->height = f->height;
+
+	__atomisp_init_stream_info(ATOMISP_INPUT_STREAM_GENERAL, stream_info);
+
+	dev_dbg(isp->dev, "try_mbus_fmt: asking for %ux%u\n",
+		snr_mbus_fmt->width, snr_mbus_fmt->height);
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       pad, set_fmt, &pad_state, &format);
+	if (ret)
+		return ret;
+
+	dev_dbg(isp->dev, "try_mbus_fmt: got %ux%u\n",
+		snr_mbus_fmt->width, snr_mbus_fmt->height);
+
+	fmt = atomisp_get_format_bridge_from_mbus(snr_mbus_fmt->code);
+	if (!fmt) {
+		dev_err(isp->dev, "unknown sensor format 0x%8.8x\n",
+			snr_mbus_fmt->code);
+		return -EINVAL;
+	}
+
+	f->pixelformat = fmt->pixelformat;
+
+	/*
+	 * If the format is jpeg or custom RAW, then the width and height will
+	 * not satisfy the normal atomisp requirements and no need to check
+	 * the below conditions. So just assign to what is being returned from
+	 * the sensor driver.
+	 */
+	if (f->pixelformat == V4L2_PIX_FMT_JPEG ||
+	    f->pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
+		f->width = snr_mbus_fmt->width;
+		f->height = snr_mbus_fmt->height;
+		return 0;
+	}
+
+	if (!res_overflow || (snr_mbus_fmt->width < f->width &&
+			      snr_mbus_fmt->height < f->height)) {
+		f->width = snr_mbus_fmt->width;
+		f->height = snr_mbus_fmt->height;
+		/* Set the flag when resolution requested is
+		 * beyond the max value supported by sensor
+		 */
+		if (res_overflow)
+			*res_overflow = true;
+	}
+
+	/* app vs isp */
+	f->width = rounddown(clamp_t(u32, f->width, ATOM_ISP_MIN_WIDTH,
+				     ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
+	f->height = rounddown(clamp_t(u32, f->height, ATOM_ISP_MIN_HEIGHT,
+				      ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
+
+	return 0;
+}
+
+enum mipi_port_id __get_mipi_port(struct atomisp_device *isp,
+				  enum atomisp_camera_port port)
+{
+	switch (port) {
+	case ATOMISP_CAMERA_PORT_PRIMARY:
+		return MIPI_PORT0_ID;
+	case ATOMISP_CAMERA_PORT_SECONDARY:
+		return MIPI_PORT1_ID;
+	case ATOMISP_CAMERA_PORT_TERTIARY:
+		if (MIPI_PORT1_ID + 1 != N_MIPI_PORT_ID)
+			return MIPI_PORT1_ID + 1;
+		fallthrough;
+	default:
+		dev_err(isp->dev, "unsupported port: %d\n", port);
+		return MIPI_PORT0_ID;
+	}
+}
+
+static inline int atomisp_set_sensor_mipi_to_isp(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    struct camera_mipi_info *mipi_info)
+{
+	struct v4l2_control ctrl;
+	struct atomisp_device *isp = asd->isp;
+	const struct atomisp_in_fmt_conv *fc;
+	int mipi_freq = 0;
+	unsigned int input_format, bayer_order;
+
+	ctrl.id = V4L2_CID_LINK_FREQ;
+	if (v4l2_g_ctrl
+	    (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl) == 0)
+		mipi_freq = ctrl.value;
+
+	if (asd->stream_env[stream_id].isys_configs == 1) {
+		input_format =
+		    asd->stream_env[stream_id].isys_info[0].input_format;
+		atomisp_css_isys_set_format(asd, stream_id,
+					    input_format, IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
+	} else if (asd->stream_env[stream_id].isys_configs == 2) {
+		atomisp_css_isys_two_stream_cfg_update_stream1(
+		    asd, stream_id,
+		    asd->stream_env[stream_id].isys_info[0].input_format,
+		    asd->stream_env[stream_id].isys_info[0].width,
+		    asd->stream_env[stream_id].isys_info[0].height);
+
+		atomisp_css_isys_two_stream_cfg_update_stream2(
+		    asd, stream_id,
+		    asd->stream_env[stream_id].isys_info[1].input_format,
+		    asd->stream_env[stream_id].isys_info[1].width,
+		    asd->stream_env[stream_id].isys_info[1].height);
+	}
+
+	/* Compatibility for sensors which provide no media bus code
+	 * in s_mbus_framefmt() nor support pad formats. */
+	if (mipi_info->input_format != -1) {
+		bayer_order = mipi_info->raw_bayer_order;
+
+		/* Input stream config is still needs configured */
+		/* TODO: Check if this is necessary */
+		fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+			 mipi_info->input_format);
+		if (!fc)
+			return -EINVAL;
+		input_format = fc->atomisp_in_fmt;
+	} else {
+		struct v4l2_mbus_framefmt *sink;
+
+		sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+					       V4L2_SUBDEV_FORMAT_ACTIVE,
+					       ATOMISP_SUBDEV_PAD_SINK);
+		fc = atomisp_find_in_fmt_conv(sink->code);
+		if (!fc)
+			return -EINVAL;
+		input_format = fc->atomisp_in_fmt;
+		bayer_order = fc->bayer_order;
+	}
+
+	atomisp_css_input_set_format(asd, stream_id, input_format);
+	atomisp_css_input_set_bayer_order(asd, stream_id, bayer_order);
+
+	fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+		 mipi_info->metadata_format);
+	if (!fc)
+		return -EINVAL;
+	input_format = fc->atomisp_in_fmt;
+	atomisp_css_input_configure_port(asd,
+					 __get_mipi_port(asd->isp, mipi_info->port),
+					 mipi_info->num_lanes,
+					 0xffff4, mipi_freq,
+					 input_format,
+					 mipi_info->metadata_width,
+					 mipi_info->metadata_height);
+	return 0;
+}
+
+static int __enable_continuous_mode(struct atomisp_sub_device *asd,
+				    bool enable)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	dev_dbg(isp->dev,
+		"continuous mode %d, raw buffers %d, stop preview %d\n",
+		enable, asd->continuous_raw_buffer_size->val,
+		!asd->continuous_viewfinder->val);
+
+	if (!IS_ISP2401)
+		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_PRIMARY);
+	else
+		atomisp_update_capture_mode(asd);
+
+	/* in case of ANR, force capture pipe to offline mode */
+	atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
+					  asd->params.low_light ? false : !enable);
+	atomisp_css_preview_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
+					  !enable);
+	atomisp_css_enable_continuous(asd, enable);
+	atomisp_css_enable_cvf(asd, asd->continuous_viewfinder->val);
+
+	atomisp_css_continuous_set_num_raw_frames(asd,
+		asd->continuous_raw_buffer_size->val);
+
+	if (!enable) {
+		atomisp_css_enable_raw_binning(asd, false);
+		atomisp_css_input_set_two_pixels_per_clock(asd, false);
+	}
+
+	if (isp->inputs[asd->input_curr].type != FILE_INPUT)
+		atomisp_css_input_set_mode(asd, IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
+
+	return atomisp_update_run_mode(asd);
+}
+
+static int configure_pp_input_nop(struct atomisp_sub_device *asd,
+				  unsigned int width, unsigned int height)
+{
+	return 0;
+}
+
+static int configure_output_nop(struct atomisp_sub_device *asd,
+				unsigned int width, unsigned int height,
+				unsigned int min_width,
+				enum ia_css_frame_format sh_fmt)
+{
+	return 0;
+}
+
+static int get_frame_info_nop(struct atomisp_sub_device *asd,
+			      struct ia_css_frame_info *finfo)
+{
+	return 0;
+}
+
+/*
+ * Resets CSS parameters that depend on input resolution.
+ *
+ * Update params like CSS RAW binning, 2ppc mode and pp_input
+ * which depend on input size, but are not automatically
+ * handled in CSS when the input resolution is changed.
+ */
+static int css_input_resolution_changed(struct atomisp_sub_device *asd,
+					struct v4l2_mbus_framefmt *ffmt)
+{
+	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf;
+	unsigned int i;
+
+	dev_dbg(asd->isp->dev, "css_input_resolution_changed to %ux%u\n",
+		ffmt->width, ffmt->height);
+
+	if (IS_ISP2401)
+		atomisp_css_input_set_two_pixels_per_clock(asd, false);
+	else
+		atomisp_css_input_set_two_pixels_per_clock(asd, true);
+
+	if (asd->continuous_mode->val) {
+		/* Note for all checks: ffmt includes pad_w+pad_h */
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO ||
+		    (ffmt->width >= 2048 || ffmt->height >= 1536)) {
+			/*
+			 * For preview pipe, enable only if resolution
+			 * is >= 3M for ISP2400.
+			 */
+			atomisp_css_enable_raw_binning(asd, true);
+		}
+	}
+	/*
+	 * If sensor input changed, which means metadata resolution changed
+	 * together. Release all metadata buffers here to let it re-allocated
+	 * next time in reqbufs.
+	 */
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		list_for_each_entry_safe(md_buf, _md_buf, &asd->metadata[i],
+					 list) {
+			atomisp_css_free_metadata_buffer(md_buf);
+			list_del(&md_buf->list);
+			kfree(md_buf);
+		}
+	}
+	return 0;
+
+	/*
+	 * TODO: atomisp_css_preview_configure_pp_input() not
+	 *       reset due to CSS bug tracked as PSI BZ 115124
+	 */
+}
+
+static int atomisp_set_fmt_to_isp(struct video_device *vdev,
+				  struct ia_css_frame_info *output_info,
+				  struct ia_css_frame_info *raw_output_info,
+				  struct v4l2_pix_format *pix,
+				  unsigned int source_pad)
+{
+	struct camera_mipi_info *mipi_info;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	const struct atomisp_format_bridge *format;
+	struct v4l2_rect *isp_sink_crop;
+	enum ia_css_pipe_id pipe_id;
+	struct v4l2_subdev_fh fh;
+	int (*configure_output)(struct atomisp_sub_device *asd,
+				unsigned int width, unsigned int height,
+				unsigned int min_width,
+				enum ia_css_frame_format sh_fmt) =
+				    configure_output_nop;
+	int (*get_frame_info)(struct atomisp_sub_device *asd,
+			      struct ia_css_frame_info *finfo) =
+				  get_frame_info_nop;
+	int (*configure_pp_input)(struct atomisp_sub_device *asd,
+				  unsigned int width, unsigned int height) =
+				      configure_pp_input_nop;
+	const struct atomisp_in_fmt_conv *fc;
+	int ret, i;
+
+	if (!asd) {
+		dev_err(isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	isp_sink_crop = atomisp_subdev_get_rect(
+			    &asd->subdev, NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
+			    ATOMISP_SUBDEV_PAD_SINK, V4L2_SEL_TGT_CROP);
+
+	format = atomisp_get_format_bridge(pix->pixelformat);
+	if (!format)
+		return -EINVAL;
+
+	if (isp->inputs[asd->input_curr].type != TEST_PATTERN &&
+	    isp->inputs[asd->input_curr].type != FILE_INPUT) {
+		mipi_info = atomisp_to_sensor_mipi_info(
+				isp->inputs[asd->input_curr].camera);
+		if (!mipi_info) {
+			dev_err(isp->dev, "mipi_info is NULL\n");
+			return -EINVAL;
+		}
+		if (atomisp_set_sensor_mipi_to_isp(asd, ATOMISP_INPUT_STREAM_GENERAL,
+						   mipi_info))
+			return -EINVAL;
+		fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+			 mipi_info->input_format);
+		if (!fc)
+			fc = atomisp_find_in_fmt_conv(
+				 atomisp_subdev_get_ffmt(&asd->subdev,
+							 NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
+							 ATOMISP_SUBDEV_PAD_SINK)->code);
+		if (!fc)
+			return -EINVAL;
+		if (format->sh_fmt == IA_CSS_FRAME_FORMAT_RAW &&
+		    raw_output_format_match_input(fc->atomisp_in_fmt,
+						  pix->pixelformat))
+			return -EINVAL;
+	}
+
+	/*
+	 * Configure viewfinder also when vfpp is disabled: the
+	 * CSS still requires viewfinder configuration.
+	 */
+	if (asd->fmt_auto->val ||
+	    asd->vfpp->val != ATOMISP_VFPP_ENABLE) {
+		struct v4l2_rect vf_size = {0};
+		struct v4l2_mbus_framefmt vf_ffmt = {0};
+
+		if (pix->width < 640 || pix->height < 480) {
+			vf_size.width = pix->width;
+			vf_size.height = pix->height;
+		} else {
+			vf_size.width = 640;
+			vf_size.height = 480;
+		}
+
+		/* FIXME: proper format name for this one. See
+		   atomisp_output_fmts[] in atomisp_v4l2.c */
+		vf_ffmt.code = V4L2_MBUS_FMT_CUSTOM_YUV420;
+
+		atomisp_subdev_set_selection(&asd->subdev, fh.state,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     ATOMISP_SUBDEV_PAD_SOURCE_VF,
+					     V4L2_SEL_TGT_COMPOSE, 0, &vf_size);
+		atomisp_subdev_set_ffmt(&asd->subdev, fh.state,
+					V4L2_SUBDEV_FORMAT_ACTIVE,
+					ATOMISP_SUBDEV_PAD_SOURCE_VF, &vf_ffmt);
+		asd->video_out_vf.sh_fmt = IA_CSS_FRAME_FORMAT_NV12;
+
+		if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+			atomisp_css_video_configure_viewfinder(asd,
+							       vf_size.width, vf_size.height, 0,
+							       asd->video_out_vf.sh_fmt);
+		} else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+			if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
+			    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO)
+				atomisp_css_video_configure_viewfinder(asd,
+								       vf_size.width, vf_size.height, 0,
+								       asd->video_out_vf.sh_fmt);
+			else
+				atomisp_css_capture_configure_viewfinder(asd,
+					vf_size.width, vf_size.height, 0,
+					asd->video_out_vf.sh_fmt);
+		} else if (source_pad != ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
+			   asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+			atomisp_css_capture_configure_viewfinder(asd,
+				vf_size.width, vf_size.height, 0,
+				asd->video_out_vf.sh_fmt);
+		}
+	}
+
+	if (asd->continuous_mode->val) {
+		ret = __enable_continuous_mode(asd, true);
+		if (ret)
+			return -EINVAL;
+	}
+
+	atomisp_css_input_set_mode(asd, IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].pipe_extra_configs[i].disable_vf_pp = asd->vfpp->val != ATOMISP_VFPP_ENABLE;
+
+	/* ISP2401 new input system need to use copy pipe */
+	if (asd->copy_mode) {
+		pipe_id = IA_CSS_PIPE_ID_COPY;
+		atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL, false);
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+		/* video same in continuouscapture and online modes */
+		configure_output = atomisp_css_video_configure_output;
+		get_frame_info = atomisp_css_video_get_output_frame_info;
+		pipe_id = IA_CSS_PIPE_ID_VIDEO;
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+		if (!asd->continuous_mode->val) {
+			configure_output = atomisp_css_video_configure_output;
+			get_frame_info =
+			    atomisp_css_video_get_output_frame_info;
+			pipe_id = IA_CSS_PIPE_ID_VIDEO;
+		} else {
+			if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
+			    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
+				configure_output =
+				    atomisp_css_video_configure_output;
+				get_frame_info =
+				    atomisp_css_video_get_output_frame_info;
+				configure_pp_input =
+				    atomisp_css_video_configure_pp_input;
+				pipe_id = IA_CSS_PIPE_ID_VIDEO;
+			} else {
+				configure_output =
+				    atomisp_css_capture_configure_output;
+				get_frame_info =
+				    atomisp_css_capture_get_output_frame_info;
+				configure_pp_input =
+				    atomisp_css_capture_configure_pp_input;
+				pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+				atomisp_update_capture_mode(asd);
+				atomisp_css_capture_enable_online(asd,
+								  ATOMISP_INPUT_STREAM_GENERAL,
+								  false);
+			}
+		}
+	} else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW) {
+		configure_output = atomisp_css_preview_configure_output;
+		get_frame_info = atomisp_css_preview_get_output_frame_info;
+		configure_pp_input = atomisp_css_preview_configure_pp_input;
+		pipe_id = IA_CSS_PIPE_ID_PREVIEW;
+	} else {
+		/* CSS doesn't support low light mode on SOC cameras, so disable
+		 * it. FIXME: if this is done elsewhere, it gives corrupted
+		 * colors into thumbnail image.
+		 */
+		if (isp->inputs[asd->input_curr].type == SOC_CAMERA)
+			asd->params.low_light = false;
+
+		if (format->sh_fmt == IA_CSS_FRAME_FORMAT_RAW) {
+			atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_RAW);
+			atomisp_css_enable_dz(asd, false);
+		} else {
+			atomisp_update_capture_mode(asd);
+		}
+
+		if (!asd->continuous_mode->val)
+			/* in case of ANR, force capture pipe to offline mode */
+			atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
+							  asd->params.low_light ?
+							  false : asd->params.online_process);
+
+		configure_output = atomisp_css_capture_configure_output;
+		get_frame_info = atomisp_css_capture_get_output_frame_info;
+		configure_pp_input = atomisp_css_capture_configure_pp_input;
+		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+		if (!asd->params.online_process &&
+		    !asd->continuous_mode->val) {
+			ret = atomisp_css_capture_get_output_raw_frame_info(asd,
+				raw_output_info);
+			if (ret)
+				return ret;
+		}
+		if (!asd->continuous_mode->val && asd->run_mode->val
+		    != ATOMISP_RUN_MODE_STILL_CAPTURE) {
+			dev_err(isp->dev,
+				"Need to set the running mode first\n");
+			asd->run_mode->val = ATOMISP_RUN_MODE_STILL_CAPTURE;
+		}
+	}
+
+	/*
+	 * to SOC camera, use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+
+	if (asd->copy_mode)
+		ret = atomisp_css_copy_configure_output(asd, ATOMISP_INPUT_STREAM_GENERAL,
+							pix->width, pix->height,
+							format->planar ? pix->bytesperline :
+							pix->bytesperline * 8 / format->depth,
+							format->sh_fmt);
+	else
+		ret = configure_output(asd, pix->width, pix->height,
+				       format->planar ? pix->bytesperline :
+				       pix->bytesperline * 8 / format->depth,
+				       format->sh_fmt);
+	if (ret) {
+		dev_err(isp->dev, "configure_output %ux%u, format %8.8x\n",
+			pix->width, pix->height, format->sh_fmt);
+		return -EINVAL;
+	}
+
+	ret = configure_pp_input(asd, isp_sink_crop->width, isp_sink_crop->height);
+	if (ret) {
+		dev_err(isp->dev, "configure_pp_input %ux%u\n",
+			isp_sink_crop->width,
+			isp_sink_crop->height);
+		return -EINVAL;
+	}
+	if (asd->copy_mode)
+		ret = atomisp_css_copy_get_output_frame_info(asd,
+							     ATOMISP_INPUT_STREAM_GENERAL,
+							     output_info);
+	else
+		ret = get_frame_info(asd, output_info);
+	if (ret) {
+		dev_err(isp->dev, "__get_frame_info %ux%u (padded to %u) returned %d\n",
+			pix->width, pix->height, pix->bytesperline, ret);
+		return ret;
+	}
+
+	atomisp_update_grid_info(asd, pipe_id, source_pad);
+
+	/* Free the raw_dump buffer first */
+	ia_css_frame_free(asd->raw_output_frame);
+	asd->raw_output_frame = NULL;
+
+	if (!asd->continuous_mode->val && !asd->params.online_process &&
+	    ia_css_frame_allocate_from_info(&asd->raw_output_frame,
+		    raw_output_info))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void atomisp_get_dis_envelop(struct atomisp_sub_device *asd,
+				    unsigned int width, unsigned int height,
+				    unsigned int *dvs_env_w, unsigned int *dvs_env_h)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	/* if subdev type is SOC camera,we do not need to set DVS */
+	if (isp->inputs[asd->input_curr].type == SOC_CAMERA)
+		asd->params.video_dis_en = false;
+
+	if (asd->params.video_dis_en &&
+	    asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+		/* envelope is 20% of the output resolution */
+		/*
+		 * dvs envelope cannot be round up.
+		 * it would cause ISP timeout and color switch issue
+		 */
+		*dvs_env_w = rounddown(width / 5, ATOM_ISP_STEP_WIDTH);
+		*dvs_env_h = rounddown(height / 5, ATOM_ISP_STEP_HEIGHT);
+	}
+
+	asd->params.dis_proj_data_valid = false;
+	asd->params.css_update_params_needed = true;
+}
+
+static void atomisp_check_copy_mode(struct atomisp_sub_device *asd,
+				    int source_pad, struct v4l2_pix_format *f)
+{
+	struct v4l2_mbus_framefmt *sink, *src;
+
+	if (!IS_ISP2401) {
+		/* Only used for the new input system */
+		asd->copy_mode = false;
+		return;
+	}
+
+	sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				       V4L2_SUBDEV_FORMAT_ACTIVE, ATOMISP_SUBDEV_PAD_SINK);
+	src = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				      V4L2_SUBDEV_FORMAT_ACTIVE, source_pad);
+
+	if (sink->code == src->code && sink->width == f->width && sink->height == f->height)
+		asd->copy_mode = true;
+	else
+		asd->copy_mode = false;
+
+	dev_dbg(asd->isp->dev, "copy_mode: %d\n", asd->copy_mode);
+}
+
+static int atomisp_set_fmt_to_snr(struct video_device *vdev,
+				  struct v4l2_pix_format *f, unsigned int pixelformat,
+				  unsigned int padding_w, unsigned int padding_h,
+				  unsigned int dvs_env_w, unsigned int dvs_env_h)
+{
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	const struct atomisp_format_bridge *format;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_state pad_state = {
+		.pads = &pad_cfg
+		};
+	struct v4l2_subdev_format vformat = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *ffmt = &vformat.format;
+	struct v4l2_mbus_framefmt *req_ffmt;
+	struct atomisp_device *isp;
+	struct atomisp_input_stream_info *stream_info =
+	    (struct atomisp_input_stream_info *)ffmt->reserved;
+	int source_pad = atomisp_subdev_source_pad(vdev);
+	struct v4l2_subdev_fh fh;
+	int ret;
+
+	if (!asd) {
+		dev_err(pipe->isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	isp = asd->isp;
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	format = atomisp_get_format_bridge(pixelformat);
+	if (!format)
+		return -EINVAL;
+
+	v4l2_fill_mbus_format(ffmt, f, format->mbus_code);
+	ffmt->height += padding_h + dvs_env_h;
+	ffmt->width += padding_w + dvs_env_w;
+
+	dev_dbg(isp->dev, "s_mbus_fmt: ask %ux%u (padding %ux%u, dvs %ux%u)\n",
+		ffmt->width, ffmt->height, padding_w, padding_h,
+		dvs_env_w, dvs_env_h);
+
+	__atomisp_init_stream_info(ATOMISP_INPUT_STREAM_GENERAL, stream_info);
+
+	req_ffmt = ffmt;
+
+	/* Disable dvs if resolution can't be supported by sensor */
+	if (asd->params.video_dis_en &&
+	    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
+		vformat.which = V4L2_SUBDEV_FORMAT_TRY;
+		ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				       pad, set_fmt, &pad_state, &vformat);
+		if (ret)
+			return ret;
+
+		dev_dbg(isp->dev, "video dis: sensor width: %d, height: %d\n",
+			ffmt->width, ffmt->height);
+
+		if (ffmt->width < req_ffmt->width ||
+		    ffmt->height < req_ffmt->height) {
+			req_ffmt->height -= dvs_env_h;
+			req_ffmt->width -= dvs_env_w;
+			ffmt = req_ffmt;
+			dev_warn(isp->dev,
+				 "can not enable video dis due to sensor limitation.");
+			asd->params.video_dis_en = false;
+		}
+	}
+	vformat.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera, pad,
+			       set_fmt, NULL, &vformat);
+	if (ret)
+		return ret;
+
+	__atomisp_update_stream_env(asd, ATOMISP_INPUT_STREAM_GENERAL, stream_info);
+
+	dev_dbg(isp->dev, "sensor width: %d, height: %d\n",
+		ffmt->width, ffmt->height);
+
+	if (ffmt->width < ATOM_ISP_STEP_WIDTH ||
+	    ffmt->height < ATOM_ISP_STEP_HEIGHT)
+		return -EINVAL;
+
+	if (asd->params.video_dis_en &&
+	    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO &&
+	    (ffmt->width < req_ffmt->width || ffmt->height < req_ffmt->height)) {
+		dev_warn(isp->dev,
+			 "can not enable video dis due to sensor limitation.");
+		asd->params.video_dis_en = false;
+	}
+
+	atomisp_subdev_set_ffmt(&asd->subdev, fh.state,
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+				ATOMISP_SUBDEV_PAD_SINK, ffmt);
+
+	return css_input_resolution_changed(asd, ffmt);
+}
+
+int atomisp_set_fmt(struct file *file, void *unused, struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	const struct atomisp_format_bridge *format_bridge;
+	const struct atomisp_format_bridge *snr_format_bridge;
+	struct ia_css_frame_info output_info, raw_output_info;
+	struct v4l2_pix_format snr_fmt;
+	struct v4l2_pix_format backup_fmt, s_fmt;
+	unsigned int dvs_env_w = 0, dvs_env_h = 0;
+	unsigned int padding_w = pad_w, padding_h = pad_h;
+	bool res_overflow = false, crop_needs_override = false;
+	struct v4l2_mbus_framefmt *isp_sink_fmt;
+	struct v4l2_mbus_framefmt isp_source_fmt = {0};
+	struct v4l2_subdev_format vformat = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *ffmt = &vformat.format;
+	struct v4l2_rect isp_sink_crop;
+	u16 source_pad = atomisp_subdev_source_pad(vdev);
+	struct v4l2_subdev_fh fh;
+	int ret;
+
+	ret = atomisp_pipe_check(pipe, true);
+	if (ret)
+		return ret;
+
+	if (source_pad >= ATOMISP_SUBDEV_PADS_NUM)
+		return -EINVAL;
+
+	dev_dbg(isp->dev,
+		"setting resolution %ux%u on pad %u for asd%d, bytesperline %u\n",
+		f->fmt.pix.width, f->fmt.pix.height, source_pad,
+		asd->index, f->fmt.pix.bytesperline);
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+	if (!format_bridge)
+		return -EINVAL;
+
+	/* Currently, raw formats are broken!!! */
+
+	if (format_bridge->sh_fmt == IA_CSS_FRAME_FORMAT_RAW) {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+
+		format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+		if (!format_bridge)
+			return -EINVAL;
+	}
+	pipe->sh_fmt = format_bridge->sh_fmt;
+	pipe->pix.pixelformat = f->fmt.pix.pixelformat;
+
+	/* Ensure that the resolution is equal or below the maximum supported */
+
+	vformat.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	v4l2_fill_mbus_format(ffmt, &f->fmt.pix, format_bridge->mbus_code);
+	ffmt->height += padding_h;
+	ffmt->width += padding_w;
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera, pad,
+			       set_fmt, NULL, &vformat);
+	if (ret)
+		return ret;
+
+	f->fmt.pix.width = ffmt->width - padding_w;
+	f->fmt.pix.height = ffmt->height - padding_h;
+
+	snr_fmt = f->fmt.pix;
+	backup_fmt = snr_fmt;
+
+	/**********************************************************************/
+
+	if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VF ||
+	    (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW
+	     && asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)) {
+		if (asd->fmt_auto->val) {
+			struct v4l2_rect *capture_comp;
+			struct v4l2_rect r = {0};
+
+			r.width = f->fmt.pix.width;
+			r.height = f->fmt.pix.height;
+
+			if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
+				capture_comp = atomisp_subdev_get_rect(
+						   &asd->subdev, NULL,
+						   V4L2_SUBDEV_FORMAT_ACTIVE,
+						   ATOMISP_SUBDEV_PAD_SOURCE_VIDEO,
+						   V4L2_SEL_TGT_COMPOSE);
+			else
+				capture_comp = atomisp_subdev_get_rect(
+						   &asd->subdev, NULL,
+						   V4L2_SUBDEV_FORMAT_ACTIVE,
+						   ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE,
+						   V4L2_SEL_TGT_COMPOSE);
+
+			if (capture_comp->width < r.width
+			    || capture_comp->height < r.height) {
+				r.width = capture_comp->width;
+				r.height = capture_comp->height;
+			}
+
+			atomisp_subdev_set_selection(
+			    &asd->subdev, fh.state,
+			    V4L2_SUBDEV_FORMAT_ACTIVE, source_pad,
+			    V4L2_SEL_TGT_COMPOSE, 0, &r);
+
+			f->fmt.pix.width = r.width;
+			f->fmt.pix.height = r.height;
+		}
+
+		if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW) {
+			atomisp_css_video_configure_viewfinder(asd,
+							       f->fmt.pix.width, f->fmt.pix.height,
+							       format_bridge->planar ? f->fmt.pix.bytesperline
+							       : f->fmt.pix.bytesperline * 8
+							       / format_bridge->depth,	format_bridge->sh_fmt);
+			atomisp_css_video_get_viewfinder_frame_info(asd,
+				&output_info);
+			asd->copy_mode = false;
+		} else {
+			atomisp_css_capture_configure_viewfinder(asd,
+				f->fmt.pix.width, f->fmt.pix.height,
+				format_bridge->planar ? f->fmt.pix.bytesperline
+				: f->fmt.pix.bytesperline * 8
+				/ format_bridge->depth,	format_bridge->sh_fmt);
+			atomisp_css_capture_get_viewfinder_frame_info(asd,
+				&output_info);
+			asd->copy_mode = false;
+		}
+
+		goto done;
+	}
+	/*
+	 * Check whether main resolution configured smaller
+	 * than snapshot resolution. If so, force main resolution
+	 * to be the same as snapshot resolution
+	 */
+	if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE) {
+		struct v4l2_rect *r;
+
+		r = atomisp_subdev_get_rect(
+			&asd->subdev, NULL,
+			V4L2_SUBDEV_FORMAT_ACTIVE,
+			ATOMISP_SUBDEV_PAD_SOURCE_VF, V4L2_SEL_TGT_COMPOSE);
+
+		if (r->width && r->height
+		    && (r->width > f->fmt.pix.width
+			|| r->height > f->fmt.pix.height))
+			dev_warn(isp->dev,
+				 "Main Resolution config smaller then Vf Resolution. Force to be equal with Vf Resolution.");
+	}
+
+	/* Pipeline configuration done through subdevs. Bail out now. */
+	if (!asd->fmt_auto->val)
+		goto set_fmt_to_isp;
+
+	/* get sensor resolution and format */
+	ret = atomisp_try_fmt(vdev, &snr_fmt, &res_overflow);
+	if (ret) {
+		dev_warn(isp->dev, "Try format failed with error %d\n", ret);
+		return ret;
+	}
+	f->fmt.pix.width = snr_fmt.width;
+	f->fmt.pix.height = snr_fmt.height;
+
+	snr_format_bridge = atomisp_get_format_bridge(snr_fmt.pixelformat);
+	if (!snr_format_bridge) {
+		dev_warn(isp->dev, "Can't find bridge format\n");
+		return -EINVAL;
+	}
+
+	atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+				ATOMISP_SUBDEV_PAD_SINK)->code =
+				    snr_format_bridge->mbus_code;
+
+	isp_sink_fmt = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+						V4L2_SUBDEV_FORMAT_ACTIVE,
+						ATOMISP_SUBDEV_PAD_SINK);
+
+	isp_source_fmt.code = format_bridge->mbus_code;
+	atomisp_subdev_set_ffmt(&asd->subdev, fh.state,
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+				source_pad, &isp_source_fmt);
+
+	if (!atomisp_subdev_format_conversion(asd, source_pad)) {
+		padding_w = 0;
+		padding_h = 0;
+	} else if (IS_BYT) {
+		padding_w = 12;
+		padding_h = 12;
+	}
+
+	/* construct resolution supported by isp */
+	if (res_overflow && !asd->continuous_mode->val) {
+		f->fmt.pix.width = rounddown(
+				       clamp_t(u32, f->fmt.pix.width - padding_w,
+					       ATOM_ISP_MIN_WIDTH,
+					       ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
+		f->fmt.pix.height = rounddown(
+					clamp_t(u32, f->fmt.pix.height - padding_h,
+						ATOM_ISP_MIN_HEIGHT,
+						ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
+	}
+
+	atomisp_get_dis_envelop(asd, f->fmt.pix.width, f->fmt.pix.height,
+				&dvs_env_w, &dvs_env_h);
+
+	if (asd->continuous_mode->val) {
+		struct v4l2_rect *r;
+
+		r = atomisp_subdev_get_rect(
+			&asd->subdev, NULL,
+			V4L2_SUBDEV_FORMAT_ACTIVE,
+			ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE,
+			V4L2_SEL_TGT_COMPOSE);
+		/*
+		 * The ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE should get resolutions
+		 * properly set otherwise, it should not be the capture_pad.
+		 */
+		if (r->width && r->height)
+			asd->capture_pad = ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE;
+		else
+			asd->capture_pad = source_pad;
+	} else {
+		asd->capture_pad = source_pad;
+	}
+	/*
+	 * set format info to sensor
+	 * In continuous mode, resolution is set only if it is higher than
+	 * existing value. This because preview pipe will be configured after
+	 * capture pipe and usually has lower resolution than capture pipe.
+	 */
+	if (!asd->continuous_mode->val ||
+	    isp_sink_fmt->width < (f->fmt.pix.width + padding_w + dvs_env_w) ||
+	    isp_sink_fmt->height < (f->fmt.pix.height + padding_h +
+				    dvs_env_h)) {
+		/*
+		 * For jpeg or custom raw format the sensor will return constant
+		 * width and height. Because we already had quried try_mbus_fmt,
+		 * f->fmt.pix.width and f->fmt.pix.height has been changed to
+		 * this fixed width and height. So we cannot select the correct
+		 * resolution with that information. So use the original width
+		 * and height while set_mbus_fmt() so actual resolutions are
+		 * being used in while set media bus format.
+		 */
+		s_fmt = f->fmt.pix;
+		if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG ||
+		    f->fmt.pix.pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
+			s_fmt.width = backup_fmt.width;
+			s_fmt.height = backup_fmt.height;
+		}
+		ret = atomisp_set_fmt_to_snr(vdev, &s_fmt,
+					     f->fmt.pix.pixelformat, padding_w,
+					     padding_h, dvs_env_w, dvs_env_h);
+		if (ret) {
+			dev_warn(isp->dev,
+				 "Set format to sensor failed with %d\n", ret);
+			return -EINVAL;
+		}
+
+		atomisp_csi_lane_config(isp);
+		crop_needs_override = true;
+	}
+
+	atomisp_check_copy_mode(asd, source_pad, &backup_fmt);
+	asd->yuvpp_mode = false;			/* Reset variable */
+
+	isp_sink_crop = *atomisp_subdev_get_rect(&asd->subdev, NULL,
+			V4L2_SUBDEV_FORMAT_ACTIVE,
+			ATOMISP_SUBDEV_PAD_SINK,
+			V4L2_SEL_TGT_CROP);
+
+	/* Try to enable YUV downscaling if ISP input is 10 % (either
+	 * width or height) bigger than the desired result. */
+	if (isp_sink_crop.width * 9 / 10 < f->fmt.pix.width ||
+	    isp_sink_crop.height * 9 / 10 < f->fmt.pix.height ||
+	    (atomisp_subdev_format_conversion(asd, source_pad) &&
+	     ((asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+	       !asd->continuous_mode->val) ||
+	      asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER))) {
+		/* for continuous mode, preview size might be smaller than
+		 * still capture size. if preview size still needs crop,
+		 * pick the larger one between crop size of preview and
+		 * still capture.
+		 */
+		if (asd->continuous_mode->val
+		    && source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW
+		    && !crop_needs_override) {
+			isp_sink_crop.width =
+			    max_t(unsigned int, f->fmt.pix.width,
+				  isp_sink_crop.width);
+			isp_sink_crop.height =
+			    max_t(unsigned int, f->fmt.pix.height,
+				  isp_sink_crop.height);
+		} else {
+			isp_sink_crop.width = f->fmt.pix.width;
+			isp_sink_crop.height = f->fmt.pix.height;
+		}
+
+		atomisp_subdev_set_selection(&asd->subdev, fh.state,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     ATOMISP_SUBDEV_PAD_SINK,
+					     V4L2_SEL_TGT_CROP,
+					     V4L2_SEL_FLAG_KEEP_CONFIG,
+					     &isp_sink_crop);
+		atomisp_subdev_set_selection(&asd->subdev, fh.state,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     source_pad, V4L2_SEL_TGT_COMPOSE,
+					     0, &isp_sink_crop);
+	} else if (IS_MOFD) {
+		struct v4l2_rect main_compose = {0};
+
+		main_compose.width = isp_sink_crop.width;
+		main_compose.height =
+		    DIV_ROUND_UP(main_compose.width * f->fmt.pix.height,
+				 f->fmt.pix.width);
+		if (main_compose.height > isp_sink_crop.height) {
+			main_compose.height = isp_sink_crop.height;
+			main_compose.width =
+			    DIV_ROUND_UP(main_compose.height *
+					 f->fmt.pix.width,
+					 f->fmt.pix.height);
+		}
+
+		atomisp_subdev_set_selection(&asd->subdev, fh.state,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     source_pad,
+					     V4L2_SEL_TGT_COMPOSE, 0,
+					     &main_compose);
+	} else {
+		struct v4l2_rect sink_crop = {0};
+		struct v4l2_rect main_compose = {0};
+
+		main_compose.width = f->fmt.pix.width;
+		main_compose.height = f->fmt.pix.height;
+
+		/* WORKAROUND: this override is universally enabled in
+		 * GMIN to work around a CTS failures (GMINL-539)
+		 * which appears to be related by a hardware
+		 * performance limitation.  It's unclear why this
+		 * particular code triggers the issue. */
+		if (crop_needs_override) {
+			if (isp_sink_crop.width * main_compose.height >
+			    isp_sink_crop.height * main_compose.width) {
+				sink_crop.height = isp_sink_crop.height;
+				sink_crop.width = DIV_NEAREST_STEP(
+						      sink_crop.height *
+						      f->fmt.pix.width,
+						      f->fmt.pix.height,
+						      ATOM_ISP_STEP_WIDTH);
+			} else {
+				sink_crop.width = isp_sink_crop.width;
+				sink_crop.height = DIV_NEAREST_STEP(
+						       sink_crop.width *
+						       f->fmt.pix.height,
+						       f->fmt.pix.width,
+						       ATOM_ISP_STEP_HEIGHT);
+			}
+			atomisp_subdev_set_selection(&asd->subdev, fh.state,
+						     V4L2_SUBDEV_FORMAT_ACTIVE,
+						     ATOMISP_SUBDEV_PAD_SINK,
+						     V4L2_SEL_TGT_CROP,
+						     V4L2_SEL_FLAG_KEEP_CONFIG,
+						     &sink_crop);
+		}
+		atomisp_subdev_set_selection(&asd->subdev, fh.state,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     source_pad,
+					     V4L2_SEL_TGT_COMPOSE, 0,
+					     &main_compose);
+	}
+
+set_fmt_to_isp:
+	ret = atomisp_set_fmt_to_isp(vdev, &output_info, &raw_output_info,
+				     &f->fmt.pix, source_pad);
+	if (ret) {
+		dev_warn(isp->dev, "Can't set format on ISP. Error %d\n", ret);
+		return -EINVAL;
+	}
+done:
+	pipe->pix.width = f->fmt.pix.width;
+	pipe->pix.height = f->fmt.pix.height;
+	pipe->pix.pixelformat = f->fmt.pix.pixelformat;
+	/*
+	 * FIXME: do we need to setup this differently, depending on the
+	 * sensor or the pipeline?
+	 */
+	pipe->pix.colorspace = V4L2_COLORSPACE_REC709;
+	pipe->pix.ycbcr_enc = V4L2_YCBCR_ENC_709;
+	pipe->pix.xfer_func = V4L2_XFER_FUNC_709;
+
+	if (format_bridge->planar) {
+		pipe->pix.bytesperline = output_info.padded_width;
+		pipe->pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height *
+						 DIV_ROUND_UP(format_bridge->depth *
+							 output_info.padded_width, 8));
+	} else {
+		pipe->pix.bytesperline =
+		    DIV_ROUND_UP(format_bridge->depth *
+				 output_info.padded_width, 8);
+		pipe->pix.sizeimage =
+		    PAGE_ALIGN(f->fmt.pix.height * pipe->pix.bytesperline);
+	}
+	dev_dbg(isp->dev, "%s: image size: %d, %d bytes per line\n",
+		__func__, pipe->pix.sizeimage, pipe->pix.bytesperline);
+
+	if (f->fmt.pix.field == V4L2_FIELD_ANY)
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+	pipe->pix.field = f->fmt.pix.field;
+
+	f->fmt.pix = pipe->pix;
+	f->fmt.pix.priv = PAGE_ALIGN(pipe->pix.width *
+				     pipe->pix.height * 2);
+
+	pipe->capq.field = f->fmt.pix.field;
+
+	/*
+	 * If in video 480P case, no GFX throttle
+	 */
+	if (asd->run_mode->val == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO &&
+	    f->fmt.pix.width == 720 && f->fmt.pix.height == 480)
+		isp->need_gfx_throttle = false;
+	else
+		isp->need_gfx_throttle = true;
+
+	/* Report the needed sizes */
+	f->fmt.pix.sizeimage = pipe->pix.sizeimage;
+	f->fmt.pix.bytesperline = pipe->pix.bytesperline;
+
+	dev_dbg(isp->dev, "%s: %dx%d, image size: %d, %d bytes per line\n",
+		__func__,
+		f->fmt.pix.width, f->fmt.pix.height,
+		f->fmt.pix.sizeimage, f->fmt.pix.bytesperline);
+
+	return 0;
+}
+
+int atomisp_set_shading_table(struct atomisp_sub_device *asd,
+			      struct atomisp_shading_table *user_shading_table)
+{
+	struct ia_css_shading_table *shading_table;
+	struct ia_css_shading_table *free_table;
+	unsigned int len_table;
+	int i;
+	int ret = 0;
+
+	if (!user_shading_table)
+		return -EINVAL;
+
+	if (!user_shading_table->enable) {
+		asd->params.config.shading_table = NULL;
+		asd->params.sc_en = false;
+		return 0;
+	}
+
+	/* If enabling, all tables must be set */
+	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+		if (!user_shading_table->data[i])
+			return -EINVAL;
+	}
+
+	/* Shading table size per color */
+	if (user_shading_table->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
+	    user_shading_table->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR)
+		return -EINVAL;
+
+	shading_table = atomisp_css_shading_table_alloc(
+			    user_shading_table->width, user_shading_table->height);
+	if (!shading_table)
+		return -ENOMEM;
+
+	len_table = user_shading_table->width * user_shading_table->height *
+		    ATOMISP_SC_TYPE_SIZE;
+	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+		ret = copy_from_user(shading_table->data[i],
+				     (void __user *)user_shading_table->data[i],
+				     len_table);
+		if (ret) {
+			free_table = shading_table;
+			ret = -EFAULT;
+			goto out;
+		}
+	}
+	shading_table->sensor_width = user_shading_table->sensor_width;
+	shading_table->sensor_height = user_shading_table->sensor_height;
+	shading_table->fraction_bits = user_shading_table->fraction_bits;
+
+	free_table = asd->params.css_param.shading_table;
+	asd->params.css_param.shading_table = shading_table;
+	asd->params.config.shading_table = shading_table;
+	asd->params.sc_en = true;
+
+out:
+	if (free_table)
+		atomisp_css_shading_table_free(free_table);
+
+	return ret;
+}
+
+/*Turn off ISP dphy */
+int atomisp_ospm_dphy_down(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	unsigned long flags;
+	u32 reg;
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	/* if ISP timeout, we can force powerdown */
+	if (isp->isp_timeout)
+		goto done;
+
+	if (!atomisp_dev_users(isp))
+		goto done;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	isp->sw_contex.power_state = ATOM_ISP_POWER_DOWN;
+	spin_unlock_irqrestore(&isp->lock, flags);
+done:
+	/*
+	 * MRFLD IUNIT DPHY is located in an always-power-on island
+	 * MRFLD HW design need all CSI ports are disabled before
+	 * powering down the IUNIT.
+	 */
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_CONTROL, &reg);
+	reg |= MRFLD_ALL_CSI_PORTS_OFF_MASK;
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_CONTROL, reg);
+	return 0;
+}
+
+/*Turn on ISP dphy */
+int atomisp_ospm_dphy_up(struct atomisp_device *isp)
+{
+	unsigned long flags;
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	spin_lock_irqsave(&isp->lock, flags);
+	isp->sw_contex.power_state = ATOM_ISP_POWER_UP;
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	return 0;
+}
+
+int atomisp_exif_makernote(struct atomisp_sub_device *asd,
+			   struct atomisp_makernote_info *config)
+{
+	struct v4l2_control ctrl;
+	struct atomisp_device *isp = asd->isp;
+
+	ctrl.id = V4L2_CID_FOCAL_ABSOLUTE;
+	if (v4l2_g_ctrl
+	    (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
+		dev_warn(isp->dev, "failed to g_ctrl for focal length\n");
+		return -EINVAL;
+	} else {
+		config->focal_length = ctrl.value;
+	}
+
+	ctrl.id = V4L2_CID_FNUMBER_ABSOLUTE;
+	if (v4l2_g_ctrl
+	    (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
+		dev_warn(isp->dev, "failed to g_ctrl for f-number\n");
+		return -EINVAL;
+	} else {
+		config->f_number_curr = ctrl.value;
+	}
+
+	ctrl.id = V4L2_CID_FNUMBER_RANGE;
+	if (v4l2_g_ctrl
+	    (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
+		dev_warn(isp->dev, "failed to g_ctrl for f number range\n");
+		return -EINVAL;
+	} else {
+		config->f_number_range = ctrl.value;
+	}
+
+	return 0;
+}
+
+int atomisp_offline_capture_configure(struct atomisp_sub_device *asd,
+				      struct atomisp_cont_capture_conf *cvf_config)
+{
+	struct v4l2_ctrl *c;
+
+	lockdep_assert_held(&asd->isp->mutex);
+
+	/*
+	* In case of M10MO ZSL capture case, we need to issue a separate
+	* capture request to M10MO which will output captured jpeg image
+	*/
+	c = v4l2_ctrl_find(
+		asd->isp->inputs[asd->input_curr].camera->ctrl_handler,
+		V4L2_CID_START_ZSL_CAPTURE);
+	if (c) {
+		int ret;
+
+		dev_dbg(asd->isp->dev, "%s trigger ZSL capture request\n",
+			__func__);
+		/* TODO: use the cvf_config */
+		ret = v4l2_ctrl_s_ctrl(c, 1);
+		if (ret)
+			return ret;
+
+		return v4l2_ctrl_s_ctrl(c, 0);
+	}
+
+	asd->params.offline_parm = *cvf_config;
+
+	if (asd->params.offline_parm.num_captures) {
+		if (asd->streaming == ATOMISP_DEVICE_STREAMING_DISABLED) {
+			unsigned int init_raw_num;
+
+			if (asd->enable_raw_buffer_lock->val) {
+				init_raw_num =
+				    ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES_LOCK_EN;
+				if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+				    asd->params.video_dis_en)
+					init_raw_num +=
+					    ATOMISP_CSS2_NUM_DVS_FRAME_DELAY;
+			} else {
+				init_raw_num =
+				    ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES;
+			}
+
+			/* TODO: this can be removed once user-space
+			 *       has been updated to use control API */
+			asd->continuous_raw_buffer_size->val =
+			    max_t(int,
+				  asd->continuous_raw_buffer_size->val,
+				  asd->params.offline_parm.
+				  num_captures + init_raw_num);
+			asd->continuous_raw_buffer_size->val =
+			    min_t(int, ATOMISP_CONT_RAW_FRAMES,
+				  asd->continuous_raw_buffer_size->val);
+		}
+		asd->continuous_mode->val = true;
+	} else {
+		asd->continuous_mode->val = false;
+		__enable_continuous_mode(asd, false);
+	}
+
+	return 0;
+}
+
+/*
+ * set auto exposure metering window to camera sensor
+ */
+int atomisp_s_ae_window(struct atomisp_sub_device *asd,
+			struct atomisp_ae_window *arg)
+{
+	struct atomisp_device *isp = asd->isp;
+	/* Coverity CID 298071 - initialzize struct */
+	struct v4l2_subdev_selection sel = { 0 };
+
+	sel.r.left = arg->x_left;
+	sel.r.top = arg->y_top;
+	sel.r.width = arg->x_right - arg->x_left + 1;
+	sel.r.height = arg->y_bottom - arg->y_top + 1;
+
+	if (v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			     pad, set_selection, NULL, &sel)) {
+		dev_err(isp->dev, "failed to call sensor set_selection.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_flash_enable(struct atomisp_sub_device *asd, int num_frames)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (num_frames < 0) {
+		dev_dbg(isp->dev, "%s ERROR: num_frames: %d\n", __func__,
+			num_frames);
+		return -EINVAL;
+	}
+	/* a requested flash is still in progress. */
+	if (num_frames && asd->params.flash_state != ATOMISP_FLASH_IDLE) {
+		dev_dbg(isp->dev, "%s flash busy: %d frames left: %d\n",
+			__func__, asd->params.flash_state,
+			asd->params.num_flash_frames);
+		return -EBUSY;
+	}
+
+	asd->params.num_flash_frames = num_frames;
+	asd->params.flash_state = ATOMISP_FLASH_REQUESTED;
+	return 0;
+}
+
+bool atomisp_is_vf_pipe(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_sub_device *asd = pipe->asd;
+
+	if (!asd) {
+		dev_err(pipe->isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, pipe->vdev.name);
+		return false;
+	}
+
+	if (pipe == &asd->video_out_vf)
+		return true;
+
+	if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+	    pipe == &asd->video_out_preview)
+		return true;
+
+	return false;
+}
+
+static int __checking_exp_id(struct atomisp_sub_device *asd, int exp_id)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->enable_raw_buffer_lock->val) {
+		dev_warn(isp->dev, "%s Raw Buffer Lock is disable.\n", __func__);
+		return -EINVAL;
+	}
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED) {
+		dev_err(isp->dev, "%s streaming %d invalid exp_id %d.\n",
+			__func__, exp_id, asd->streaming);
+		return -EINVAL;
+	}
+	if ((exp_id > ATOMISP_MAX_EXP_ID) || (exp_id <= 0)) {
+		dev_err(isp->dev, "%s exp_id %d invalid.\n", __func__, exp_id);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+void atomisp_init_raw_buffer_bitmap(struct atomisp_sub_device *asd)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
+	memset(asd->raw_buffer_bitmap, 0, sizeof(asd->raw_buffer_bitmap));
+	asd->raw_buffer_locked_count = 0;
+	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
+}
+
+static int atomisp_set_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id)
+{
+	int *bitmap, bit;
+	unsigned long flags;
+
+	if (__checking_exp_id(asd, exp_id))
+		return -EINVAL;
+
+	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
+	bit = exp_id % 32;
+	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
+	(*bitmap) |= (1 << bit);
+	asd->raw_buffer_locked_count++;
+	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
+
+	dev_dbg(asd->isp->dev, "%s: exp_id %d,  raw_buffer_locked_count %d\n",
+		__func__, exp_id, asd->raw_buffer_locked_count);
+
+	/* Check if the raw buffer after next is still locked!!! */
+	exp_id += 2;
+	if (exp_id > ATOMISP_MAX_EXP_ID)
+		exp_id -= ATOMISP_MAX_EXP_ID;
+	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
+	bit = exp_id % 32;
+	if ((*bitmap) & (1 << bit)) {
+		int ret;
+
+		/* WORKAROUND unlock the raw buffer compulsively */
+		ret = atomisp_css_exp_id_unlock(asd, exp_id);
+		if (ret) {
+			dev_err(asd->isp->dev,
+				"%s exp_id is wrapping back to %d but force unlock failed, err %d.\n",
+				__func__, exp_id, ret);
+			return ret;
+		}
+
+		spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
+		(*bitmap) &= ~(1 << bit);
+		asd->raw_buffer_locked_count--;
+		spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
+		dev_warn(asd->isp->dev,
+			 "%s exp_id is wrapping back to %d but it is still locked so force unlock it, raw_buffer_locked_count %d\n",
+			 __func__, exp_id, asd->raw_buffer_locked_count);
+	}
+	return 0;
+}
+
+static int __is_raw_buffer_locked(struct atomisp_sub_device *asd, int exp_id)
+{
+	int *bitmap, bit;
+	unsigned long flags;
+	int ret;
+
+	if (__checking_exp_id(asd, exp_id))
+		return -EINVAL;
+
+	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
+	bit = exp_id % 32;
+	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
+	ret = ((*bitmap) & (1 << bit));
+	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
+	return !ret;
+}
+
+static int __clear_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id)
+{
+	int *bitmap, bit;
+	unsigned long flags;
+
+	if (__is_raw_buffer_locked(asd, exp_id))
+		return -EINVAL;
+
+	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
+	bit = exp_id % 32;
+	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
+	(*bitmap) &= ~(1 << bit);
+	asd->raw_buffer_locked_count--;
+	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
+
+	dev_dbg(asd->isp->dev, "%s: exp_id %d,  raw_buffer_locked_count %d\n",
+		__func__, exp_id, asd->raw_buffer_locked_count);
+	return 0;
+}
+
+int atomisp_exp_id_capture(struct atomisp_sub_device *asd, int *exp_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	int value = *exp_id;
+	int ret;
+
+	lockdep_assert_held(&isp->mutex);
+
+	ret = __is_raw_buffer_locked(asd, value);
+	if (ret) {
+		dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
+		return -EINVAL;
+	}
+
+	dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
+	ret = atomisp_css_exp_id_capture(asd, value);
+	if (ret) {
+		dev_err(isp->dev, "%s exp_id %d failed.\n", __func__, value);
+		return -EIO;
+	}
+	return 0;
+}
+
+int atomisp_exp_id_unlock(struct atomisp_sub_device *asd, int *exp_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	int value = *exp_id;
+	int ret;
+
+	lockdep_assert_held(&isp->mutex);
+
+	ret = __clear_raw_buffer_bitmap(asd, value);
+	if (ret) {
+		dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
+		return -EINVAL;
+	}
+
+	dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
+	ret = atomisp_css_exp_id_unlock(asd, value);
+	if (ret)
+		dev_err(isp->dev, "%s exp_id %d failed, err %d.\n",
+			__func__, value, ret);
+
+	return ret;
+}
+
+int atomisp_enable_dz_capt_pipe(struct atomisp_sub_device *asd,
+				unsigned int *enable)
+{
+	bool value;
+
+	if (!enable)
+		return -EINVAL;
+
+	value = *enable > 0;
+
+	atomisp_en_dz_capt_pipe(asd, value);
+
+	return 0;
+}
+
+int atomisp_inject_a_fake_event(struct atomisp_sub_device *asd, int *event)
+{
+	if (!event || asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+		return -EINVAL;
+
+	lockdep_assert_held(&asd->isp->mutex);
+
+	dev_dbg(asd->isp->dev, "%s: trying to inject a fake event 0x%x\n",
+		__func__, *event);
+
+	switch (*event) {
+	case V4L2_EVENT_FRAME_SYNC:
+		atomisp_sof_event(asd);
+		break;
+	case V4L2_EVENT_FRAME_END:
+		atomisp_eof_event(asd, 0);
+		break;
+	case V4L2_EVENT_ATOMISP_3A_STATS_READY:
+		atomisp_3a_stats_ready_event(asd, 0);
+		break;
+	case V4L2_EVENT_ATOMISP_METADATA_READY:
+		atomisp_metadata_ready_event(asd, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int atomisp_get_pipe_id(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_sub_device *asd = pipe->asd;
+
+	if (!asd) {
+		dev_err(pipe->isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, pipe->vdev.name);
+		return -EINVAL;
+	}
+
+	if (ATOMISP_USE_YUVPP(asd)) {
+		return IA_CSS_PIPE_ID_YUVPP;
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+		return IA_CSS_PIPE_ID_VIDEO;
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+		return IA_CSS_PIPE_ID_CAPTURE;
+	} else if (pipe == &asd->video_out_video_capture) {
+		return IA_CSS_PIPE_ID_VIDEO;
+	} else if (pipe == &asd->video_out_vf) {
+		return IA_CSS_PIPE_ID_CAPTURE;
+	} else if (pipe == &asd->video_out_preview) {
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			return IA_CSS_PIPE_ID_VIDEO;
+		else
+			return IA_CSS_PIPE_ID_PREVIEW;
+	} else if (pipe == &asd->video_out_capture) {
+		if (asd->copy_mode)
+			return IA_CSS_PIPE_ID_COPY;
+		else
+			return IA_CSS_PIPE_ID_CAPTURE;
+	}
+
+	/* fail through */
+	dev_warn(asd->isp->dev, "%s failed to find proper pipe\n",
+		 __func__);
+	return IA_CSS_PIPE_ID_CAPTURE;
+}
+
+int atomisp_get_invalid_frame_num(struct video_device *vdev,
+				  int *invalid_frame_num)
+{
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	enum ia_css_pipe_id pipe_id;
+	struct ia_css_pipe_info p_info;
+	int ret;
+
+	pipe_id = atomisp_get_pipe_id(pipe);
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].pipes[pipe_id]) {
+		dev_warn(asd->isp->dev,
+			 "%s pipe %d has not been created yet, do SET_FMT first!\n",
+			 __func__, pipe_id);
+		return -EINVAL;
+	}
+
+	ret = ia_css_pipe_get_info(
+		  asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		  .pipes[pipe_id], &p_info);
+	if (!ret) {
+		*invalid_frame_num = p_info.num_invalid_frames;
+		return 0;
+	} else {
+		dev_warn(asd->isp->dev, "%s get pipe infor failed %d\n",
+			 __func__, ret);
+		return -EINVAL;
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_cmd.h b/drivers/staging/media/atomisp/pci/atomisp_cmd.h
new file mode 100644
index 000000000..c9f92f132
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_cmd.h
@@ -0,0 +1,349 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__ATOMISP_CMD_H__
+#define	__ATOMISP_CMD_H__
+
+#include "../../include/linux/atomisp.h"
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "atomisp_internal.h"
+
+#include "ia_css_types.h"
+#include "ia_css.h"
+
+struct atomisp_device;
+struct ia_css_frame;
+
+#define MSI_ENABLE_BIT		16
+#define INTR_DISABLE_BIT	10
+#define BUS_MASTER_ENABLE	2
+#define MEMORY_SPACE_ENABLE	1
+#define INTR_IER		24
+#define INTR_IIR		16
+
+/* ISP2401 */
+#define RUNMODE_MASK (ATOMISP_RUN_MODE_VIDEO | ATOMISP_RUN_MODE_STILL_CAPTURE \
+			| ATOMISP_RUN_MODE_PREVIEW)
+
+/* FIXME: check if can go */
+extern int atomisp_punit_hpll_freq;
+
+/* Helper function */
+void dump_sp_dmem(struct atomisp_device *isp, unsigned int addr,
+		  unsigned int size);
+struct camera_mipi_info *atomisp_to_sensor_mipi_info(struct v4l2_subdev *sd);
+struct atomisp_video_pipe *atomisp_to_video_pipe(struct video_device *dev);
+int atomisp_reset(struct atomisp_device *isp);
+void atomisp_flush_bufs_and_wakeup(struct atomisp_sub_device *asd);
+void atomisp_clear_css_buffer_counters(struct atomisp_sub_device *asd);
+/* ISP2400 */
+bool atomisp_buffers_queued(struct atomisp_sub_device *asd);
+/* ISP2401 */
+bool atomisp_buffers_queued_pipe(struct atomisp_video_pipe *pipe);
+
+/* Interrupt functions */
+void atomisp_msi_irq_init(struct atomisp_device *isp);
+void atomisp_msi_irq_uninit(struct atomisp_device *isp);
+void atomisp_assert_recovery_work(struct work_struct *work);
+void atomisp_setup_flash(struct atomisp_sub_device *asd);
+irqreturn_t atomisp_isr(int irq, void *dev);
+irqreturn_t atomisp_isr_thread(int irq, void *isp_ptr);
+const struct atomisp_format_bridge *get_atomisp_format_bridge_from_mbus(
+    u32 mbus_code);
+bool atomisp_is_mbuscode_raw(uint32_t code);
+void atomisp_delayed_init_work(struct work_struct *work);
+
+/* Get internal fmt according to V4L2 fmt */
+bool atomisp_is_viewfinder_support(struct atomisp_device *isp);
+
+/* ISP features control function */
+
+/*
+ * Function to set sensor runmode by user when
+ * ATOMISP_IOC_S_SENSOR_RUNMODE ioctl was called
+ */
+int atomisp_set_sensor_runmode(struct atomisp_sub_device *asd,
+			       struct atomisp_s_runmode *runmode);
+/*
+ * Function to enable/disable lens geometry distortion correction (GDC) and
+ * chromatic aberration correction (CAC)
+ */
+int atomisp_gdc_cac(struct atomisp_sub_device *asd, int flag,
+		    __s32 *value);
+
+/* Function to enable/disable low light mode (including ANR) */
+int atomisp_low_light(struct atomisp_sub_device *asd, int flag,
+		      __s32 *value);
+
+/*
+ * Function to enable/disable extra noise reduction (XNR) in low light
+ * condition
+ */
+int atomisp_xnr(struct atomisp_sub_device *asd, int flag, int *arg);
+
+int atomisp_formats(struct atomisp_sub_device *asd, int flag,
+		    struct atomisp_formats_config *config);
+
+/* Function to configure noise reduction */
+int atomisp_nr(struct atomisp_sub_device *asd, int flag,
+	       struct atomisp_nr_config *config);
+
+/* Function to configure temporal noise reduction (TNR) */
+int atomisp_tnr(struct atomisp_sub_device *asd, int flag,
+		struct atomisp_tnr_config *config);
+
+/* Function to configure black level compensation */
+int atomisp_black_level(struct atomisp_sub_device *asd, int flag,
+			struct atomisp_ob_config *config);
+
+/* Function to configure edge enhancement */
+int atomisp_ee(struct atomisp_sub_device *asd, int flag,
+	       struct atomisp_ee_config *config);
+
+/* Function to update Gamma table for gamma, brightness and contrast config */
+int atomisp_gamma(struct atomisp_sub_device *asd, int flag,
+		  struct atomisp_gamma_table *config);
+
+/* Function to update Ctc table for Chroma Enhancement */
+int atomisp_ctc(struct atomisp_sub_device *asd, int flag,
+		struct atomisp_ctc_table *config);
+
+/* Function to update gamma correction parameters */
+int atomisp_gamma_correction(struct atomisp_sub_device *asd, int flag,
+			     struct atomisp_gc_config *config);
+
+/* Function to update Gdc table for gdc */
+int atomisp_gdc_cac_table(struct atomisp_sub_device *asd, int flag,
+			  struct atomisp_morph_table *config);
+
+/* Function to update table for macc */
+int atomisp_macc_table(struct atomisp_sub_device *asd, int flag,
+		       struct atomisp_macc_config *config);
+
+/* Function to get DIS statistics. */
+int atomisp_get_dis_stat(struct atomisp_sub_device *asd,
+			 struct atomisp_dis_statistics *stats);
+
+/* Function to get DVS2 BQ resolution settings */
+int atomisp_get_dvs2_bq_resolutions(struct atomisp_sub_device *asd,
+				    struct atomisp_dvs2_bq_resolutions *bq_res);
+
+/* Function to set the DIS coefficients. */
+int atomisp_set_dis_coefs(struct atomisp_sub_device *asd,
+			  struct atomisp_dis_coefficients *coefs);
+
+/* Function to set the DIS motion vector. */
+int atomisp_set_dis_vector(struct atomisp_sub_device *asd,
+			   struct atomisp_dis_vector *vector);
+
+/* Function to set/get 3A stat from isp */
+int atomisp_3a_stat(struct atomisp_sub_device *asd, int flag,
+		    struct atomisp_3a_statistics *config);
+
+/* Function to get metadata from isp */
+int atomisp_get_metadata(struct atomisp_sub_device *asd, int flag,
+			 struct atomisp_metadata *config);
+
+int atomisp_get_metadata_by_type(struct atomisp_sub_device *asd, int flag,
+				 struct atomisp_metadata_with_type *config);
+
+int atomisp_set_parameters(struct video_device *vdev,
+			   struct atomisp_parameters *arg);
+
+/* Function to set/get isp parameters to isp */
+int atomisp_param(struct atomisp_sub_device *asd, int flag,
+		  struct atomisp_parm *config);
+
+/* Function to configure color effect of the image */
+int atomisp_color_effect(struct atomisp_sub_device *asd, int flag,
+			 __s32 *effect);
+
+/* Function to configure bad pixel correction */
+int atomisp_bad_pixel(struct atomisp_sub_device *asd, int flag,
+		      __s32 *value);
+
+/* Function to configure bad pixel correction params */
+int atomisp_bad_pixel_param(struct atomisp_sub_device *asd, int flag,
+			    struct atomisp_dp_config *config);
+
+/* Function to enable/disable video image stablization */
+int atomisp_video_stable(struct atomisp_sub_device *asd, int flag,
+			 __s32 *value);
+
+/* Function to configure fixed pattern noise */
+int atomisp_fixed_pattern(struct atomisp_sub_device *asd, int flag,
+			  __s32 *value);
+
+/* Function to configure fixed pattern noise table */
+int atomisp_fixed_pattern_table(struct atomisp_sub_device *asd,
+				struct v4l2_framebuffer *config);
+
+/* Function to configure false color correction */
+int atomisp_false_color(struct atomisp_sub_device *asd, int flag,
+			__s32 *value);
+
+/* Function to configure false color correction params */
+int atomisp_false_color_param(struct atomisp_sub_device *asd, int flag,
+			      struct atomisp_de_config *config);
+
+/* Function to configure white balance params */
+int atomisp_white_balance_param(struct atomisp_sub_device *asd, int flag,
+				struct atomisp_wb_config *config);
+
+int atomisp_3a_config_param(struct atomisp_sub_device *asd, int flag,
+			    struct atomisp_3a_config *config);
+
+/* Function to setup digital zoom */
+int atomisp_digital_zoom(struct atomisp_sub_device *asd, int flag,
+			 __s32 *value);
+
+/* Function  set camera_prefiles.xml current sensor pixel array size */
+int atomisp_set_array_res(struct atomisp_sub_device *asd,
+			  struct atomisp_resolution  *config);
+
+/* Function to calculate real zoom region for every pipe */
+int atomisp_calculate_real_zoom_region(struct atomisp_sub_device *asd,
+				       struct ia_css_dz_config   *dz_config,
+				       enum ia_css_pipe_id css_pipe_id);
+
+int atomisp_cp_general_isp_parameters(struct atomisp_sub_device *asd,
+				      struct atomisp_parameters *arg,
+				      struct atomisp_css_params *css_param,
+				      bool from_user);
+
+int atomisp_cp_lsc_table(struct atomisp_sub_device *asd,
+			 struct atomisp_shading_table *source_st,
+			 struct atomisp_css_params *css_param,
+			 bool from_user);
+
+int atomisp_css_cp_dvs2_coefs(struct atomisp_sub_device *asd,
+			      struct ia_css_dvs2_coefficients *coefs,
+			      struct atomisp_css_params *css_param,
+			      bool from_user);
+
+int atomisp_cp_morph_table(struct atomisp_sub_device *asd,
+			   struct atomisp_morph_table *source_morph_table,
+			   struct atomisp_css_params *css_param,
+			   bool from_user);
+
+int atomisp_cp_dvs_6axis_config(struct atomisp_sub_device *asd,
+				struct atomisp_dvs_6axis_config *user_6axis_config,
+				struct atomisp_css_params *css_param,
+				bool from_user);
+
+int atomisp_makeup_css_parameters(struct atomisp_sub_device *asd,
+				  struct atomisp_parameters *arg,
+				  struct atomisp_css_params *css_param);
+
+int atomisp_compare_grid(struct atomisp_sub_device *asd,
+			 struct atomisp_grid_info *atomgrid);
+
+int atomisp_get_sensor_mode_data(struct atomisp_sub_device *asd,
+				 struct atomisp_sensor_mode_data *config);
+
+/* This function looks up the closest available resolution. */
+int atomisp_try_fmt(struct video_device *vdev, struct v4l2_pix_format *f,
+		    bool *res_overflow);
+
+int atomisp_set_fmt(struct file *file, void *fh, struct v4l2_format *f);
+
+int atomisp_set_shading_table(struct atomisp_sub_device *asd,
+			      struct atomisp_shading_table *shading_table);
+
+int atomisp_offline_capture_configure(struct atomisp_sub_device *asd,
+				      struct atomisp_cont_capture_conf *cvf_config);
+
+int atomisp_ospm_dphy_down(struct atomisp_device *isp);
+int atomisp_ospm_dphy_up(struct atomisp_device *isp);
+int atomisp_exif_makernote(struct atomisp_sub_device *asd,
+			   struct atomisp_makernote_info *config);
+
+void atomisp_free_internal_buffers(struct atomisp_sub_device *asd);
+
+int atomisp_s_ae_window(struct atomisp_sub_device *asd,
+			struct atomisp_ae_window *arg);
+
+int  atomisp_flash_enable(struct atomisp_sub_device *asd,
+			  int num_frames);
+
+int atomisp_freq_scaling(struct atomisp_device *vdev,
+			 enum atomisp_dfs_mode mode,
+			 bool force);
+
+void atomisp_buf_done(struct atomisp_sub_device *asd, int error,
+		      enum ia_css_buffer_type buf_type,
+		      enum ia_css_pipe_id css_pipe_id,
+		      bool q_buffers, enum atomisp_input_stream_id stream_id);
+
+void atomisp_css_flush(struct atomisp_device *isp);
+
+/* Events. Only one event has to be exported for now. */
+void atomisp_eof_event(struct atomisp_sub_device *asd, uint8_t exp_id);
+
+enum mipi_port_id __get_mipi_port(struct atomisp_device *isp,
+				  enum atomisp_camera_port port);
+
+bool atomisp_is_vf_pipe(struct atomisp_video_pipe *pipe);
+
+void atomisp_apply_css_parameters(
+    struct atomisp_sub_device *asd,
+    struct atomisp_css_params *css_param);
+void atomisp_free_css_parameters(struct atomisp_css_params *css_param);
+
+void atomisp_handle_parameter_and_buffer(struct atomisp_video_pipe *pipe);
+
+void atomisp_flush_params_queue(struct atomisp_video_pipe *asd);
+
+/* Function to do Raw Buffer related operation, after enable Lock Unlock Raw Buffer */
+int atomisp_exp_id_unlock(struct atomisp_sub_device *asd, int *exp_id);
+int atomisp_exp_id_capture(struct atomisp_sub_device *asd, int *exp_id);
+
+void atomisp_init_raw_buffer_bitmap(struct atomisp_sub_device *asd);
+
+/* Function to enable/disable zoom for capture pipe */
+int atomisp_enable_dz_capt_pipe(struct atomisp_sub_device *asd,
+				unsigned int *enable);
+
+/* Function to get metadata type bu pipe id */
+enum atomisp_metadata_type
+atomisp_get_metadata_type(struct atomisp_sub_device *asd,
+			  enum ia_css_pipe_id pipe_id);
+
+u32 atomisp_get_pixel_depth(u32 pixelformat);
+
+/* Function for HAL to inject a fake event to wake up poll thread */
+int atomisp_inject_a_fake_event(struct atomisp_sub_device *asd, int *event);
+
+/*
+ * Function for HAL to query how many invalid frames at the beginning of ISP
+ * pipeline output
+ */
+int atomisp_get_invalid_frame_num(struct video_device *vdev,
+				  int *invalid_frame_num);
+
+int atomisp_mrfld_power_up(struct atomisp_device *isp);
+int atomisp_mrfld_power_down(struct atomisp_device *isp);
+int atomisp_runtime_suspend(struct device *dev);
+int atomisp_runtime_resume(struct device *dev);
+#endif /* __ATOMISP_CMD_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_common.h b/drivers/staging/media/atomisp/pci/atomisp_common.h
new file mode 100644
index 000000000..b29874f2b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_common.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__ATOMISP_COMMON_H__
+#define	__ATOMISP_COMMON_H__
+
+#include "../../include/linux/atomisp.h"
+
+#include <linux/v4l2-mediabus.h>
+
+#include <media/videobuf-core.h>
+
+#include "atomisp_compat.h"
+
+#include "ia_css.h"
+
+extern int dbg_level;
+extern int dbg_func;
+extern int mipicsi_flag;
+extern int pad_w;
+extern int pad_h;
+
+#define CSS_DTRACE_VERBOSITY_LEVEL	5	/* Controls trace verbosity */
+#define CSS_DTRACE_VERBOSITY_TIMEOUT	9	/* Verbosity on ISP timeout */
+#define MRFLD_MAX_ZOOM_FACTOR	1024
+
+/* ISP2401 */
+#define ATOMISP_CSS_ISP_PIPE_VERSION_2_7    1
+
+struct atomisp_format_bridge {
+	unsigned int pixelformat;
+	unsigned int depth;
+	u32 mbus_code;
+	enum ia_css_frame_format sh_fmt;
+	unsigned char description[32];	/* the same as struct v4l2_fmtdesc */
+	bool planar;
+};
+
+struct atomisp_fmt {
+	u32 pixelformat;
+	u32 depth;
+	u32 bytesperline;
+	u32 framesize;
+	u32 imagesize;
+	u32 width;
+	u32 height;
+	u32 bayer_order;
+};
+
+struct atomisp_buffer {
+	struct videobuf_buffer	vb;
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp_compat.h b/drivers/staging/media/atomisp/pci/atomisp_compat.h
new file mode 100644
index 000000000..a6d85d0f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_compat.h
@@ -0,0 +1,446 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Clovertrail PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2012 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __ATOMISP_COMPAT_H__
+#define __ATOMISP_COMPAT_H__
+
+#include "atomisp_compat_css20.h"
+
+#include "../../include/linux/atomisp.h"
+#include <media/videobuf-vmalloc.h>
+
+struct atomisp_device;
+struct atomisp_sub_device;
+struct video_device;
+enum atomisp_input_stream_id;
+
+struct atomisp_metadata_buf {
+	struct ia_css_metadata *metadata;
+	void *md_vptr;
+	struct list_head list;
+};
+
+void atomisp_css2_hw_store_32(hrt_address addr, uint32_t data);
+void atomisp_load_uint32(hrt_address addr, uint32_t *data);
+
+int atomisp_css_init(struct atomisp_device *isp);
+
+void atomisp_css_uninit(struct atomisp_device *isp);
+
+void atomisp_css_suspend(struct atomisp_device *isp);
+
+int atomisp_css_resume(struct atomisp_device *isp);
+
+void atomisp_css_init_struct(struct atomisp_sub_device *asd);
+
+int atomisp_css_irq_translate(struct atomisp_device *isp,
+			      unsigned int *infos);
+
+void atomisp_css_rx_get_irq_info(enum mipi_port_id port,
+				 unsigned int *infos);
+
+void atomisp_css_rx_clear_irq_info(enum mipi_port_id port,
+				   unsigned int infos);
+
+int atomisp_css_irq_enable(struct atomisp_device *isp,
+			   enum ia_css_irq_info info, bool enable);
+
+int atomisp_q_video_buffer_to_css(struct atomisp_sub_device *asd,
+				  struct videobuf_vmalloc_memory *vm_mem,
+				  enum atomisp_input_stream_id stream_id,
+				  enum ia_css_buffer_type css_buf_type,
+				  enum ia_css_pipe_id css_pipe_id);
+
+int atomisp_q_s3a_buffer_to_css(struct atomisp_sub_device *asd,
+				struct atomisp_s3a_buf *s3a_buf,
+				enum atomisp_input_stream_id stream_id,
+				enum ia_css_pipe_id css_pipe_id);
+
+int atomisp_q_metadata_buffer_to_css(struct atomisp_sub_device *asd,
+				     struct atomisp_metadata_buf *metadata_buf,
+				     enum atomisp_input_stream_id stream_id,
+				     enum ia_css_pipe_id css_pipe_id);
+
+int atomisp_q_dis_buffer_to_css(struct atomisp_sub_device *asd,
+				struct atomisp_dis_buf *dis_buf,
+				enum atomisp_input_stream_id stream_id,
+				enum ia_css_pipe_id css_pipe_id);
+
+void ia_css_mmu_invalidate_cache(void);
+
+int atomisp_css_start(struct atomisp_sub_device *asd,
+		      enum ia_css_pipe_id pipe_id, bool in_reset);
+
+void atomisp_css_update_isp_params(struct atomisp_sub_device *asd);
+void atomisp_css_update_isp_params_on_pipe(struct atomisp_sub_device *asd,
+	struct ia_css_pipe *pipe);
+
+int atomisp_css_queue_buffer(struct atomisp_sub_device *asd,
+			     enum atomisp_input_stream_id stream_id,
+			     enum ia_css_pipe_id pipe_id,
+			     enum ia_css_buffer_type buf_type,
+			     struct atomisp_css_buffer *isp_css_buffer);
+
+int atomisp_css_dequeue_buffer(struct atomisp_sub_device *asd,
+			       enum atomisp_input_stream_id stream_id,
+			       enum ia_css_pipe_id pipe_id,
+			       enum ia_css_buffer_type buf_type,
+			       struct atomisp_css_buffer *isp_css_buffer);
+
+int atomisp_css_allocate_stat_buffers(struct atomisp_sub_device *asd,
+				      u16 stream_id,
+				      struct atomisp_s3a_buf *s3a_buf,
+				      struct atomisp_dis_buf *dis_buf,
+				      struct atomisp_metadata_buf *md_buf);
+
+void atomisp_css_free_stat_buffers(struct atomisp_sub_device *asd);
+
+void atomisp_css_free_3a_buffer(struct atomisp_s3a_buf *s3a_buf);
+
+void atomisp_css_free_dis_buffer(struct atomisp_dis_buf *dis_buf);
+
+void atomisp_css_free_metadata_buffer(struct atomisp_metadata_buf
+				      *metadata_buf);
+
+int atomisp_css_get_grid_info(struct atomisp_sub_device *asd,
+			      enum ia_css_pipe_id pipe_id,
+			      int source_pad);
+
+int atomisp_alloc_3a_output_buf(struct atomisp_sub_device *asd);
+
+int atomisp_alloc_dis_coef_buf(struct atomisp_sub_device *asd);
+
+int atomisp_alloc_metadata_output_buf(struct atomisp_sub_device *asd);
+
+void atomisp_free_metadata_output_buf(struct atomisp_sub_device *asd);
+
+void atomisp_css_temp_pipe_to_pipe_id(struct atomisp_sub_device *asd,
+				      struct atomisp_css_event *current_event);
+
+int atomisp_css_isys_set_resolution(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    struct v4l2_mbus_framefmt *ffmt,
+				    int isys_stream);
+
+void atomisp_css_isys_set_link(struct atomisp_sub_device *asd,
+			       enum atomisp_input_stream_id stream_id,
+			       int link,
+			       int isys_stream);
+
+void atomisp_css_isys_set_valid(struct atomisp_sub_device *asd,
+				enum atomisp_input_stream_id stream_id,
+				bool valid,
+				int isys_stream);
+
+void atomisp_css_isys_set_format(struct atomisp_sub_device *asd,
+				 enum atomisp_input_stream_id stream_id,
+				 enum atomisp_input_format format,
+				 int isys_stream);
+
+int atomisp_css_set_default_isys_config(struct atomisp_sub_device *asd,
+					enum atomisp_input_stream_id stream_id,
+					struct v4l2_mbus_framefmt *ffmt);
+
+int atomisp_css_isys_two_stream_cfg(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum atomisp_input_format input_format);
+
+void atomisp_css_isys_two_stream_cfg_update_stream1(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    enum atomisp_input_format input_format,
+    unsigned int width, unsigned int height);
+
+void atomisp_css_isys_two_stream_cfg_update_stream2(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    enum atomisp_input_format input_format,
+    unsigned int width, unsigned int height);
+
+int atomisp_css_input_set_resolution(struct atomisp_sub_device *asd,
+				     enum atomisp_input_stream_id stream_id,
+				     struct v4l2_mbus_framefmt *ffmt);
+
+void atomisp_css_input_set_binning_factor(struct atomisp_sub_device *asd,
+	enum atomisp_input_stream_id stream_id,
+	unsigned int bin_factor);
+
+void atomisp_css_input_set_bayer_order(struct atomisp_sub_device *asd,
+				       enum atomisp_input_stream_id stream_id,
+				       enum ia_css_bayer_order bayer_order);
+
+void atomisp_css_input_set_format(struct atomisp_sub_device *asd,
+				  enum atomisp_input_stream_id stream_id,
+				  enum atomisp_input_format format);
+
+int atomisp_css_input_set_effective_resolution(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    unsigned int width,
+    unsigned int height);
+
+void atomisp_css_video_set_dis_envelope(struct atomisp_sub_device *asd,
+					unsigned int dvs_w, unsigned int dvs_h);
+
+void atomisp_css_input_set_two_pixels_per_clock(
+    struct atomisp_sub_device *asd,
+    bool two_ppc);
+
+void atomisp_css_enable_raw_binning(struct atomisp_sub_device *asd,
+				    bool enable);
+
+void atomisp_css_enable_dz(struct atomisp_sub_device *asd, bool enable);
+
+void atomisp_css_capture_set_mode(struct atomisp_sub_device *asd,
+				  enum ia_css_capture_mode mode);
+
+void atomisp_css_input_set_mode(struct atomisp_sub_device *asd,
+				enum ia_css_input_mode mode);
+
+void atomisp_css_capture_enable_online(struct atomisp_sub_device *asd,
+				       unsigned short stream_index, bool enable);
+
+void atomisp_css_preview_enable_online(struct atomisp_sub_device *asd,
+				       unsigned short stream_index, bool enable);
+
+void atomisp_css_video_enable_online(struct atomisp_sub_device *asd,
+				     bool enable);
+
+void atomisp_css_enable_continuous(struct atomisp_sub_device *asd,
+				   bool enable);
+
+void atomisp_css_enable_cvf(struct atomisp_sub_device *asd,
+			    bool enable);
+
+int atomisp_css_input_configure_port(struct atomisp_sub_device *asd,
+				     enum mipi_port_id port,
+				     unsigned int num_lanes,
+				     unsigned int timeout,
+				     unsigned int mipi_freq,
+				     enum atomisp_input_format metadata_format,
+				     unsigned int metadata_width,
+				     unsigned int metadata_height);
+
+int atomisp_create_pipes_stream(struct atomisp_sub_device *asd);
+void atomisp_destroy_pipes_stream_force(struct atomisp_sub_device *asd);
+
+void atomisp_css_stop(struct atomisp_sub_device *asd,
+		      enum ia_css_pipe_id pipe_id, bool in_reset);
+
+void atomisp_css_continuous_set_num_raw_frames(
+     struct atomisp_sub_device *asd,
+     int num_frames);
+
+int atomisp_css_copy_configure_output(struct atomisp_sub_device *asd,
+				      unsigned int stream_index,
+				      unsigned int width, unsigned int height,
+				      unsigned int padded_width,
+				      enum ia_css_frame_format format);
+
+int atomisp_css_yuvpp_configure_output(struct atomisp_sub_device *asd,
+				       unsigned int stream_index,
+				       unsigned int width, unsigned int height,
+				       unsigned int padded_width,
+				       enum ia_css_frame_format format);
+
+int atomisp_css_yuvpp_configure_viewfinder(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    unsigned int width, unsigned int height,
+    unsigned int min_width,
+    enum ia_css_frame_format format);
+
+int atomisp_css_yuvpp_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_yuvpp_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_preview_configure_output(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format);
+
+int atomisp_css_capture_configure_output(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format);
+
+int atomisp_css_video_configure_output(struct atomisp_sub_device *asd,
+				       unsigned int width, unsigned int height,
+				       unsigned int min_width,
+				       enum ia_css_frame_format format);
+
+int atomisp_get_css_frame_info(struct atomisp_sub_device *asd,
+			       u16 source_pad,
+			       struct ia_css_frame_info *frame_info);
+
+int atomisp_css_video_configure_viewfinder(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format);
+
+int atomisp_css_capture_configure_viewfinder(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height,
+    unsigned int min_width,
+    enum ia_css_frame_format format);
+
+int atomisp_css_video_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_capture_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_copy_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_capture_get_output_raw_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_preview_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_capture_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_video_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info);
+
+int atomisp_css_preview_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height);
+
+int atomisp_css_capture_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height);
+
+int atomisp_css_video_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height);
+
+int atomisp_css_offline_capture_configure(struct atomisp_sub_device *asd,
+	int num_captures, unsigned int skip, int offset);
+int atomisp_css_exp_id_capture(struct atomisp_sub_device *asd, int exp_id);
+int atomisp_css_exp_id_unlock(struct atomisp_sub_device *asd, int exp_id);
+
+int atomisp_css_capture_enable_xnr(struct atomisp_sub_device *asd,
+				   bool enable);
+
+void atomisp_css_set_ctc_table(struct atomisp_sub_device *asd,
+			       struct ia_css_ctc_table *ctc_table);
+
+void atomisp_css_video_set_dis_vector(struct atomisp_sub_device *asd,
+				      struct atomisp_dis_vector *vector);
+
+void atomisp_css_set_dvs2_coefs(struct atomisp_sub_device *asd,
+				struct ia_css_dvs2_coefficients *coefs);
+
+int atomisp_css_set_dis_coefs(struct atomisp_sub_device *asd,
+			      struct atomisp_dis_coefficients *coefs);
+
+void atomisp_css_set_zoom_factor(struct atomisp_sub_device *asd,
+				 unsigned int zoom);
+
+int atomisp_css_get_wb_config(struct atomisp_sub_device *asd,
+			      struct atomisp_wb_config *config);
+
+int atomisp_css_get_ob_config(struct atomisp_sub_device *asd,
+			      struct atomisp_ob_config *config);
+
+int atomisp_css_get_dp_config(struct atomisp_sub_device *asd,
+			      struct atomisp_dp_config *config);
+
+int atomisp_css_get_de_config(struct atomisp_sub_device *asd,
+			      struct atomisp_de_config *config);
+
+int atomisp_css_get_nr_config(struct atomisp_sub_device *asd,
+			      struct atomisp_nr_config *config);
+
+int atomisp_css_get_ee_config(struct atomisp_sub_device *asd,
+			      struct atomisp_ee_config *config);
+
+int atomisp_css_get_tnr_config(struct atomisp_sub_device *asd,
+			       struct atomisp_tnr_config *config);
+
+int atomisp_css_get_ctc_table(struct atomisp_sub_device *asd,
+			      struct atomisp_ctc_table *config);
+
+int atomisp_css_get_gamma_table(struct atomisp_sub_device *asd,
+				struct atomisp_gamma_table *config);
+
+int atomisp_css_get_gc_config(struct atomisp_sub_device *asd,
+			      struct atomisp_gc_config *config);
+
+int atomisp_css_get_3a_config(struct atomisp_sub_device *asd,
+			      struct atomisp_3a_config *config);
+
+int atomisp_css_get_formats_config(struct atomisp_sub_device *asd,
+				   struct atomisp_formats_config *formats_config);
+
+void atomisp_css_set_formats_config(struct atomisp_sub_device *asd,
+				    struct ia_css_formats_config *formats_config);
+
+int atomisp_css_get_zoom_factor(struct atomisp_sub_device *asd,
+				unsigned int *zoom);
+
+struct ia_css_shading_table *atomisp_css_shading_table_alloc(
+    unsigned int width, unsigned int height);
+
+void atomisp_css_set_shading_table(struct atomisp_sub_device *asd,
+				   struct ia_css_shading_table *table);
+
+void atomisp_css_shading_table_free(struct ia_css_shading_table *table);
+
+struct ia_css_morph_table *atomisp_css_morph_table_allocate(
+    unsigned int width, unsigned int height);
+
+void atomisp_css_set_morph_table(struct atomisp_sub_device *asd,
+				 struct ia_css_morph_table *table);
+
+void atomisp_css_get_morph_table(struct atomisp_sub_device *asd,
+				 struct ia_css_morph_table *table);
+
+void atomisp_css_morph_table_free(struct ia_css_morph_table *table);
+
+int atomisp_css_get_dis_stat(struct atomisp_sub_device *asd,
+			     struct atomisp_dis_statistics *stats);
+
+int atomisp_css_update_stream(struct atomisp_sub_device *asd);
+
+int atomisp_css_isr_thread(struct atomisp_device *isp,
+			   bool *frame_done_found,
+			   bool *css_pipe_done);
+
+bool atomisp_css_valid_sof(struct atomisp_device *isp);
+
+void atomisp_en_dz_capt_pipe(struct atomisp_sub_device *asd, bool enable);
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp_compat_css20.c b/drivers/staging/media/atomisp/pci/atomisp_compat_css20.c
new file mode 100644
index 000000000..fdc05548d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_compat_css20.c
@@ -0,0 +1,4016 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Clovertrail PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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 <media/videobuf-vmalloc.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-event.h>
+
+#include "mmu/isp_mmu.h"
+#include "mmu/sh_mmu_mrfld.h"
+#include "hmm/hmm_bo.h"
+#include "hmm/hmm.h"
+
+#include "atomisp_compat.h"
+#include "atomisp_internal.h"
+#include "atomisp_cmd.h"
+#include "atomisp-regs.h"
+#include "atomisp_fops.h"
+#include "atomisp_ioctl.h"
+
+#include "ia_css_debug.h"
+#include "ia_css_isp_param.h"
+#include "sh_css_hrt.h"
+#include "ia_css_isys.h"
+
+#include <linux/io.h>
+#include <linux/pm_runtime.h>
+
+/* Assume max number of ACC stages */
+#define MAX_ACC_STAGES	20
+
+/* Ideally, this should come from CSS headers */
+#define NO_LINK -1
+
+/*
+ * to serialize MMIO access , this is due to ISP2400 silicon issue Sighting
+ * #4684168, if concurrency access happened, system may hard hang.
+ */
+static DEFINE_SPINLOCK(mmio_lock);
+
+enum frame_info_type {
+	ATOMISP_CSS_VF_FRAME,
+	ATOMISP_CSS_SECOND_VF_FRAME,
+	ATOMISP_CSS_OUTPUT_FRAME,
+	ATOMISP_CSS_SECOND_OUTPUT_FRAME,
+	ATOMISP_CSS_RAW_FRAME,
+};
+
+struct bayer_ds_factor {
+	unsigned int numerator;
+	unsigned int denominator;
+};
+
+static void atomisp_css2_hw_store_8(hrt_address addr, uint8_t data)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	writeb(data, isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static void atomisp_css2_hw_store_16(hrt_address addr, uint16_t data)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	writew(data, isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+void atomisp_css2_hw_store_32(hrt_address addr, uint32_t data)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	writel(data, isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static uint8_t atomisp_css2_hw_load_8(hrt_address addr)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+	u8 ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = readb(isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+
+static uint16_t atomisp_css2_hw_load_16(hrt_address addr)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+	u16 ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = readw(isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+
+static uint32_t atomisp_css2_hw_load_32(hrt_address addr)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+	u32 ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = readl(isp->base + (addr & 0x003FFFFF));
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+
+static void atomisp_css2_hw_store(hrt_address addr, const void *from, uint32_t n)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+	unsigned int i;
+
+	addr &= 0x003FFFFF;
+	spin_lock_irqsave(&mmio_lock, flags);
+	for (i = 0; i < n; i++, from++)
+		writeb(*(s8 *)from, isp->base + addr + i);
+
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static void atomisp_css2_hw_load(hrt_address addr, void *to, uint32_t n)
+{
+	struct atomisp_device *isp = dev_get_drvdata(atomisp_dev);
+	unsigned long flags;
+	unsigned int i;
+
+	addr &= 0x003FFFFF;
+	spin_lock_irqsave(&mmio_lock, flags);
+	for (i = 0; i < n; i++, to++)
+		*(s8 *)to = readb(isp->base + addr + i);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static int  __printf(1, 0) atomisp_css2_dbg_ftrace_print(const char *fmt,
+							 va_list args)
+{
+	ftrace_vprintk(fmt, args);
+	return 0;
+}
+
+static int  __printf(1, 0) atomisp_vprintk(const char *fmt, va_list args)
+{
+	vprintk(fmt, args);
+	return 0;
+}
+
+void atomisp_load_uint32(hrt_address addr, uint32_t *data)
+{
+	*data = atomisp_css2_hw_load_32(addr);
+}
+
+static int hmm_get_mmu_base_addr(struct device *dev, unsigned int *mmu_base_addr)
+{
+	if (!sh_mmu_mrfld.get_pd_base) {
+		dev_err(dev, "get mmu base address failed.\n");
+		return -EINVAL;
+	}
+
+	*mmu_base_addr = sh_mmu_mrfld.get_pd_base(&bo_device.mmu,
+			 bo_device.mmu.base_address);
+	return 0;
+}
+
+static void __dump_pipe_config(struct atomisp_sub_device *asd,
+			       struct atomisp_stream_env *stream_env,
+			       unsigned int pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (stream_env->pipes[pipe_id]) {
+		struct ia_css_pipe_config *p_config;
+		struct ia_css_pipe_extra_config *pe_config;
+
+		p_config = &stream_env->pipe_configs[pipe_id];
+		pe_config = &stream_env->pipe_extra_configs[pipe_id];
+		dev_dbg(isp->dev, "dumping pipe[%d] config:\n", pipe_id);
+		dev_dbg(isp->dev,
+			"pipe_config.pipe_mode:%d.\n", p_config->mode);
+		dev_dbg(isp->dev,
+			"pipe_config.output_info[0] w=%d, h=%d.\n",
+			p_config->output_info[0].res.width,
+			p_config->output_info[0].res.height);
+		dev_dbg(isp->dev,
+			"pipe_config.vf_pp_in_res w=%d, h=%d.\n",
+			p_config->vf_pp_in_res.width,
+			p_config->vf_pp_in_res.height);
+		dev_dbg(isp->dev,
+			"pipe_config.capt_pp_in_res w=%d, h=%d.\n",
+			p_config->capt_pp_in_res.width,
+			p_config->capt_pp_in_res.height);
+		dev_dbg(isp->dev,
+			"pipe_config.output.padded w=%d.\n",
+			p_config->output_info[0].padded_width);
+		dev_dbg(isp->dev,
+			"pipe_config.vf_output_info[0] w=%d, h=%d.\n",
+			p_config->vf_output_info[0].res.width,
+			p_config->vf_output_info[0].res.height);
+		dev_dbg(isp->dev,
+			"pipe_config.bayer_ds_out_res w=%d, h=%d.\n",
+			p_config->bayer_ds_out_res.width,
+			p_config->bayer_ds_out_res.height);
+		dev_dbg(isp->dev,
+			"pipe_config.envelope w=%d, h=%d.\n",
+			p_config->dvs_envelope.width,
+			p_config->dvs_envelope.height);
+		dev_dbg(isp->dev,
+			"pipe_config.dvs_frame_delay=%d.\n",
+			p_config->dvs_frame_delay);
+		dev_dbg(isp->dev,
+			"pipe_config.isp_pipe_version:%d.\n",
+			p_config->isp_pipe_version);
+		dev_dbg(isp->dev,
+			"pipe_config.acc_extension=%p.\n",
+			p_config->acc_extension);
+		dev_dbg(isp->dev,
+			"pipe_config.acc_stages=%p.\n",
+			p_config->acc_stages);
+		dev_dbg(isp->dev,
+			"pipe_config.num_acc_stages=%d.\n",
+			p_config->num_acc_stages);
+		dev_dbg(isp->dev,
+			"pipe_config.acc_num_execs=%d.\n",
+			p_config->acc_num_execs);
+		dev_dbg(isp->dev,
+			"pipe_config.default_capture_config.capture_mode=%d.\n",
+			p_config->default_capture_config.mode);
+		dev_dbg(isp->dev,
+			"pipe_config.enable_dz=%d.\n",
+			p_config->enable_dz);
+		dev_dbg(isp->dev,
+			"pipe_config.default_capture_config.enable_xnr=%d.\n",
+			p_config->default_capture_config.enable_xnr);
+		dev_dbg(isp->dev,
+			"dumping pipe[%d] extra config:\n", pipe_id);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.enable_raw_binning:%d.\n",
+			pe_config->enable_raw_binning);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.enable_yuv_ds:%d.\n",
+			pe_config->enable_yuv_ds);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.enable_high_speed:%d.\n",
+			pe_config->enable_high_speed);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.enable_dvs_6axis:%d.\n",
+			pe_config->enable_dvs_6axis);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.enable_reduced_pipe:%d.\n",
+			pe_config->enable_reduced_pipe);
+		dev_dbg(isp->dev,
+			"pipe_(extra_)config.enable_dz:%d.\n",
+			p_config->enable_dz);
+		dev_dbg(isp->dev,
+			"pipe_extra_config.disable_vf_pp:%d.\n",
+			pe_config->disable_vf_pp);
+	}
+}
+
+static void __dump_stream_config(struct atomisp_sub_device *asd,
+				 struct atomisp_stream_env *stream_env)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_stream_config *s_config;
+	int j;
+	bool valid_stream = false;
+
+	for (j = 0; j < IA_CSS_PIPE_ID_NUM; j++) {
+		if (stream_env->pipes[j]) {
+			__dump_pipe_config(asd, stream_env, j);
+			valid_stream = true;
+		}
+	}
+	if (!valid_stream)
+		return;
+	s_config = &stream_env->stream_config;
+	dev_dbg(isp->dev, "stream_config.mode=%d.\n", s_config->mode);
+
+	if (s_config->mode == IA_CSS_INPUT_MODE_SENSOR ||
+	    s_config->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		dev_dbg(isp->dev, "stream_config.source.port.port=%d.\n",
+			s_config->source.port.port);
+		dev_dbg(isp->dev, "stream_config.source.port.num_lanes=%d.\n",
+			s_config->source.port.num_lanes);
+		dev_dbg(isp->dev, "stream_config.source.port.timeout=%d.\n",
+			s_config->source.port.timeout);
+		dev_dbg(isp->dev, "stream_config.source.port.rxcount=0x%x.\n",
+			s_config->source.port.rxcount);
+		dev_dbg(isp->dev, "stream_config.source.port.compression.type=%d.\n",
+			s_config->source.port.compression.type);
+		dev_dbg(isp->dev,
+			"stream_config.source.port.compression.compressed_bits_per_pixel=%d.\n",
+			s_config->source.port.compression.
+			compressed_bits_per_pixel);
+		dev_dbg(isp->dev,
+			"stream_config.source.port.compression.uncompressed_bits_per_pixel=%d.\n",
+			s_config->source.port.compression.
+			uncompressed_bits_per_pixel);
+	} else if (s_config->mode == IA_CSS_INPUT_MODE_TPG) {
+		dev_dbg(isp->dev, "stream_config.source.tpg.id=%d.\n",
+			s_config->source.tpg.id);
+		dev_dbg(isp->dev, "stream_config.source.tpg.mode=%d.\n",
+			s_config->source.tpg.mode);
+		dev_dbg(isp->dev, "stream_config.source.tpg.x_mask=%d.\n",
+			s_config->source.tpg.x_mask);
+		dev_dbg(isp->dev, "stream_config.source.tpg.x_delta=%d.\n",
+			s_config->source.tpg.x_delta);
+		dev_dbg(isp->dev, "stream_config.source.tpg.y_mask=%d.\n",
+			s_config->source.tpg.y_mask);
+		dev_dbg(isp->dev, "stream_config.source.tpg.y_delta=%d.\n",
+			s_config->source.tpg.y_delta);
+		dev_dbg(isp->dev, "stream_config.source.tpg.xy_mask=%d.\n",
+			s_config->source.tpg.xy_mask);
+	} else if (s_config->mode == IA_CSS_INPUT_MODE_PRBS) {
+		dev_dbg(isp->dev, "stream_config.source.prbs.id=%d.\n",
+			s_config->source.prbs.id);
+		dev_dbg(isp->dev, "stream_config.source.prbs.h_blank=%d.\n",
+			s_config->source.prbs.h_blank);
+		dev_dbg(isp->dev, "stream_config.source.prbs.v_blank=%d.\n",
+			s_config->source.prbs.v_blank);
+		dev_dbg(isp->dev, "stream_config.source.prbs.seed=%d.\n",
+			s_config->source.prbs.seed);
+		dev_dbg(isp->dev, "stream_config.source.prbs.seed1=%d.\n",
+			s_config->source.prbs.seed1);
+	}
+
+	for (j = 0; j < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH; j++) {
+		dev_dbg(isp->dev, "stream_configisys_config[%d].input_res w=%d, h=%d.\n",
+			j,
+			s_config->isys_config[j].input_res.width,
+			s_config->isys_config[j].input_res.height);
+
+		dev_dbg(isp->dev, "stream_configisys_config[%d].linked_isys_stream_id=%d\n",
+			j,
+			s_config->isys_config[j].linked_isys_stream_id);
+
+		dev_dbg(isp->dev, "stream_configisys_config[%d].format=%d\n",
+			j,
+			s_config->isys_config[j].format);
+
+		dev_dbg(isp->dev, "stream_configisys_config[%d].valid=%d.\n",
+			j,
+			s_config->isys_config[j].valid);
+	}
+
+	dev_dbg(isp->dev, "stream_config.input_config.input_res w=%d, h=%d.\n",
+		s_config->input_config.input_res.width,
+		s_config->input_config.input_res.height);
+
+	dev_dbg(isp->dev, "stream_config.input_config.effective_res w=%d, h=%d.\n",
+		s_config->input_config.effective_res.width,
+		s_config->input_config.effective_res.height);
+
+	dev_dbg(isp->dev, "stream_config.input_config.format=%d\n",
+		s_config->input_config.format);
+
+	dev_dbg(isp->dev, "stream_config.input_config.bayer_order=%d.\n",
+		s_config->input_config.bayer_order);
+
+	dev_dbg(isp->dev, "stream_config.pixels_per_clock=%d.\n",
+		s_config->pixels_per_clock);
+	dev_dbg(isp->dev, "stream_config.online=%d.\n", s_config->online);
+	dev_dbg(isp->dev, "stream_config.continuous=%d.\n",
+		s_config->continuous);
+	dev_dbg(isp->dev, "stream_config.disable_cont_viewfinder=%d.\n",
+		s_config->disable_cont_viewfinder);
+	dev_dbg(isp->dev, "stream_config.channel_id=%d.\n",
+		s_config->channel_id);
+	dev_dbg(isp->dev, "stream_config.init_num_cont_raw_buf=%d.\n",
+		s_config->init_num_cont_raw_buf);
+	dev_dbg(isp->dev, "stream_config.target_num_cont_raw_buf=%d.\n",
+		s_config->target_num_cont_raw_buf);
+	dev_dbg(isp->dev, "stream_config.left_padding=%d.\n",
+		s_config->left_padding);
+	dev_dbg(isp->dev, "stream_config.sensor_binning_factor=%d.\n",
+		s_config->sensor_binning_factor);
+	dev_dbg(isp->dev, "stream_config.pixels_per_clock=%d.\n",
+		s_config->pixels_per_clock);
+	dev_dbg(isp->dev, "stream_config.pack_raw_pixels=%d.\n",
+		s_config->pack_raw_pixels);
+	dev_dbg(isp->dev, "stream_config.flash_gpio_pin=%d.\n",
+		s_config->flash_gpio_pin);
+	dev_dbg(isp->dev, "stream_config.mipi_buffer_config.size_mem_words=%d.\n",
+		s_config->mipi_buffer_config.size_mem_words);
+	dev_dbg(isp->dev, "stream_config.mipi_buffer_config.contiguous=%d.\n",
+		s_config->mipi_buffer_config.contiguous);
+	dev_dbg(isp->dev, "stream_config.metadata_config.data_type=%d.\n",
+		s_config->metadata_config.data_type);
+	dev_dbg(isp->dev, "stream_config.metadata_config.resolution w=%d, h=%d.\n",
+		s_config->metadata_config.resolution.width,
+		s_config->metadata_config.resolution.height);
+}
+
+static int __destroy_stream(struct atomisp_sub_device *asd,
+			    struct atomisp_stream_env *stream_env)
+{
+	struct atomisp_device *isp = asd->isp;
+	unsigned long timeout;
+
+	if (!stream_env->stream)
+		return 0;
+
+	if (stream_env->stream_state == CSS_STREAM_STARTED
+	    && ia_css_stream_stop(stream_env->stream) != 0) {
+		dev_err(isp->dev, "stop stream failed.\n");
+		return -EINVAL;
+	}
+
+	if (stream_env->stream_state == CSS_STREAM_STARTED) {
+		timeout = jiffies + msecs_to_jiffies(40);
+		while (1) {
+			if (ia_css_stream_has_stopped(stream_env->stream))
+				break;
+
+			if (time_after(jiffies, timeout)) {
+				dev_warn(isp->dev, "stop stream timeout.\n");
+				break;
+			}
+
+			usleep_range(100, 200);
+		}
+	}
+
+	stream_env->stream_state = CSS_STREAM_STOPPED;
+
+	if (ia_css_stream_destroy(stream_env->stream)) {
+		dev_err(isp->dev, "destroy stream failed.\n");
+		return -EINVAL;
+	}
+	stream_env->stream_state = CSS_STREAM_UNINIT;
+	stream_env->stream = NULL;
+
+	return 0;
+}
+
+static int __destroy_streams(struct atomisp_sub_device *asd)
+{
+	int ret, i;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		ret = __destroy_stream(asd, &asd->stream_env[i]);
+		if (ret)
+			return ret;
+	}
+	asd->stream_prepared = false;
+	return 0;
+}
+
+static int __create_stream(struct atomisp_sub_device *asd,
+			   struct atomisp_stream_env *stream_env)
+{
+	int pipe_index = 0, i;
+	struct ia_css_pipe *multi_pipes[IA_CSS_PIPE_ID_NUM];
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		if (stream_env->pipes[i])
+			multi_pipes[pipe_index++] = stream_env->pipes[i];
+	}
+	if (pipe_index == 0)
+		return 0;
+
+	stream_env->stream_config.target_num_cont_raw_buf =
+	    asd->continuous_raw_buffer_size->val;
+	stream_env->stream_config.channel_id = stream_env->ch_id;
+	stream_env->stream_config.ia_css_enable_raw_buffer_locking =
+	    asd->enable_raw_buffer_lock->val;
+
+	__dump_stream_config(asd, stream_env);
+	if (ia_css_stream_create(&stream_env->stream_config,
+				 pipe_index, multi_pipes, &stream_env->stream) != 0)
+		return -EINVAL;
+	if (ia_css_stream_get_info(stream_env->stream,
+				   &stream_env->stream_info) != 0) {
+		ia_css_stream_destroy(stream_env->stream);
+		stream_env->stream = NULL;
+		return -EINVAL;
+	}
+
+	stream_env->stream_state = CSS_STREAM_CREATED;
+	return 0;
+}
+
+static int __create_streams(struct atomisp_sub_device *asd)
+{
+	int ret, i;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		ret = __create_stream(asd, &asd->stream_env[i]);
+		if (ret)
+			goto rollback;
+	}
+	asd->stream_prepared = true;
+	return 0;
+rollback:
+	for (i--; i >= 0; i--)
+		__destroy_stream(asd, &asd->stream_env[i]);
+	return ret;
+}
+
+static int __destroy_stream_pipes(struct atomisp_sub_device *asd,
+				  struct atomisp_stream_env *stream_env)
+{
+	struct atomisp_device *isp = asd->isp;
+	int ret = 0;
+	int i;
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		if (!stream_env->pipes[i])
+			continue;
+		if (ia_css_pipe_destroy(stream_env->pipes[i])
+		    != 0) {
+			dev_err(isp->dev,
+				"destroy pipe[%d]failed.cannot recover.\n", i);
+			ret = -EINVAL;
+		}
+		stream_env->pipes[i] = NULL;
+		stream_env->update_pipe[i] = false;
+	}
+	return ret;
+}
+
+static int __destroy_pipes(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		if (asd->stream_env[i].stream) {
+			dev_err(isp->dev,
+				"cannot destroy css pipes for stream[%d].\n",
+				i);
+			continue;
+		}
+
+		ret = __destroy_stream_pipes(asd, &asd->stream_env[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+void atomisp_destroy_pipes_stream_force(struct atomisp_sub_device *asd)
+{
+	if (__destroy_streams(asd))
+		dev_warn(asd->isp->dev, "destroy stream failed.\n");
+
+	if (__destroy_pipes(asd))
+		dev_warn(asd->isp->dev, "destroy pipe failed.\n");
+}
+
+static void __apply_additional_pipe_config(
+    struct atomisp_sub_device *asd,
+    struct atomisp_stream_env *stream_env,
+    enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (pipe_id < 0 || pipe_id >= IA_CSS_PIPE_ID_NUM) {
+		dev_err(isp->dev,
+			"wrong pipe_id for additional pipe config.\n");
+		return;
+	}
+
+	/* apply default pipe config */
+	stream_env->pipe_configs[pipe_id].isp_pipe_version = 2;
+	stream_env->pipe_configs[pipe_id].enable_dz =
+	    asd->disable_dz->val ? false : true;
+	/* apply isp 2.2 specific config for baytrail*/
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_CAPTURE:
+		/* enable capture pp/dz manually or digital zoom would
+		 * fail*/
+		if (stream_env->pipe_configs[pipe_id].
+		    default_capture_config.mode == IA_CSS_CAPTURE_MODE_RAW)
+			stream_env->pipe_configs[pipe_id].enable_dz = false;
+		break;
+	case IA_CSS_PIPE_ID_VIDEO:
+		/* enable reduced pipe to have binary
+		 * video_dz_2_min selected*/
+		stream_env->pipe_extra_configs[pipe_id]
+		.enable_reduced_pipe = true;
+		stream_env->pipe_configs[pipe_id]
+		.enable_dz = false;
+		if (ATOMISP_SOC_CAMERA(asd))
+			stream_env->pipe_configs[pipe_id].enable_dz = true;
+
+		if (asd->params.video_dis_en) {
+			stream_env->pipe_extra_configs[pipe_id]
+			.enable_dvs_6axis = true;
+			stream_env->pipe_configs[pipe_id]
+			.dvs_frame_delay =
+			    ATOMISP_CSS2_NUM_DVS_FRAME_DELAY;
+		}
+		break;
+	case IA_CSS_PIPE_ID_PREVIEW:
+		break;
+	case IA_CSS_PIPE_ID_YUVPP:
+	case IA_CSS_PIPE_ID_COPY:
+		if (ATOMISP_SOC_CAMERA(asd))
+			stream_env->pipe_configs[pipe_id].enable_dz = true;
+		else
+			stream_env->pipe_configs[pipe_id].enable_dz = false;
+		break;
+	case IA_CSS_PIPE_ID_ACC:
+		stream_env->pipe_configs[pipe_id].mode = IA_CSS_PIPE_MODE_ACC;
+		stream_env->pipe_configs[pipe_id].enable_dz = false;
+		break;
+	default:
+		break;
+	}
+}
+
+static bool is_pipe_valid_to_current_run_mode(struct atomisp_sub_device *asd,
+	enum ia_css_pipe_id pipe_id)
+{
+	if (!asd)
+		return false;
+
+	if (pipe_id == IA_CSS_PIPE_ID_ACC || pipe_id == IA_CSS_PIPE_ID_YUVPP)
+		return true;
+
+	if (asd->vfpp) {
+		if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+			if (pipe_id == IA_CSS_PIPE_ID_VIDEO)
+				return true;
+			else
+				return false;
+		} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+			if (pipe_id == IA_CSS_PIPE_ID_CAPTURE)
+				return true;
+			else
+				return false;
+		}
+	}
+
+	if (!asd->run_mode)
+		return false;
+
+	if (asd->copy_mode && pipe_id == IA_CSS_PIPE_ID_COPY)
+		return true;
+
+	switch (asd->run_mode->val) {
+	case ATOMISP_RUN_MODE_STILL_CAPTURE:
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE)
+			return true;
+
+		return false;
+	case ATOMISP_RUN_MODE_PREVIEW:
+		if (!asd->continuous_mode->val) {
+			if (pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+				return true;
+
+			return false;
+		}
+		fallthrough;
+	case ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE:
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE ||
+		    pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			return true;
+
+		return false;
+	case ATOMISP_RUN_MODE_VIDEO:
+		if (!asd->continuous_mode->val) {
+			if (pipe_id == IA_CSS_PIPE_ID_VIDEO ||
+			    pipe_id == IA_CSS_PIPE_ID_YUVPP)
+				return true;
+			else
+				return false;
+		}
+		fallthrough;
+	case ATOMISP_RUN_MODE_SDV:
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE ||
+		    pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			return true;
+
+		return false;
+	}
+
+	return false;
+}
+
+static int __create_pipe(struct atomisp_sub_device *asd,
+			 struct atomisp_stream_env *stream_env,
+			 enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_pipe_extra_config extra_config;
+	int ret;
+
+	if (pipe_id >= IA_CSS_PIPE_ID_NUM)
+		return -EINVAL;
+
+	if (pipe_id != IA_CSS_PIPE_ID_ACC &&
+	    !stream_env->pipe_configs[pipe_id].output_info[0].res.width)
+		return 0;
+
+	if (pipe_id == IA_CSS_PIPE_ID_ACC &&
+	    !stream_env->pipe_configs[pipe_id].acc_extension)
+		return 0;
+
+	if (!is_pipe_valid_to_current_run_mode(asd, pipe_id))
+		return 0;
+
+	ia_css_pipe_extra_config_defaults(&extra_config);
+
+	__apply_additional_pipe_config(asd, stream_env, pipe_id);
+	if (!memcmp(&extra_config,
+		    &stream_env->pipe_extra_configs[pipe_id],
+		    sizeof(extra_config)))
+		ret = ia_css_pipe_create(
+			  &stream_env->pipe_configs[pipe_id],
+			  &stream_env->pipes[pipe_id]);
+	else
+		ret = ia_css_pipe_create_extra(
+			  &stream_env->pipe_configs[pipe_id],
+			  &stream_env->pipe_extra_configs[pipe_id],
+			  &stream_env->pipes[pipe_id]);
+	if (ret)
+		dev_err(isp->dev, "create pipe[%d] error.\n", pipe_id);
+	return ret;
+}
+
+static int __create_pipes(struct atomisp_sub_device *asd)
+{
+	int ret;
+	int i, j;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		for (j = 0; j < IA_CSS_PIPE_ID_NUM; j++) {
+			ret = __create_pipe(asd, &asd->stream_env[i], j);
+			if (ret)
+				break;
+		}
+		if (j < IA_CSS_PIPE_ID_NUM)
+			goto pipe_err;
+	}
+	return 0;
+pipe_err:
+	for (; i >= 0; i--) {
+		for (j--; j >= 0; j--) {
+			if (asd->stream_env[i].pipes[j]) {
+				ia_css_pipe_destroy(asd->stream_env[i].pipes[j]);
+				asd->stream_env[i].pipes[j] = NULL;
+			}
+		}
+		j = IA_CSS_PIPE_ID_NUM;
+	}
+	return -EINVAL;
+}
+
+int atomisp_create_pipes_stream(struct atomisp_sub_device *asd)
+{
+	int ret;
+
+	ret = __create_pipes(asd);
+	if (ret) {
+		dev_err(asd->isp->dev, "create pipe failed %d.\n", ret);
+		return ret;
+	}
+
+	ret = __create_streams(asd);
+	if (ret) {
+		dev_warn(asd->isp->dev, "create stream failed %d.\n", ret);
+		__destroy_pipes(asd);
+		return ret;
+	}
+
+	return 0;
+}
+
+int atomisp_css_update_stream(struct atomisp_sub_device *asd)
+{
+	atomisp_destroy_pipes_stream_force(asd);
+	return atomisp_create_pipes_stream(asd);
+}
+
+int atomisp_css_init(struct atomisp_device *isp)
+{
+	unsigned int mmu_base_addr;
+	int ret;
+	int err;
+
+	ret = hmm_get_mmu_base_addr(isp->dev, &mmu_base_addr);
+	if (ret)
+		return ret;
+
+	/* Init ISP */
+	err = ia_css_init(isp->dev, &isp->css_env.isp_css_env, NULL,
+			  (uint32_t)mmu_base_addr, IA_CSS_IRQ_TYPE_PULSE);
+	if (err) {
+		dev_err(isp->dev, "css init failed --- bad firmware?\n");
+		return -EINVAL;
+	}
+	ia_css_enable_isys_event_queue(true);
+
+	isp->css_initialized = true;
+	dev_dbg(isp->dev, "sh_css_init success\n");
+
+	return 0;
+}
+
+static inline int __set_css_print_env(struct atomisp_device *isp, int opt)
+{
+	int ret = 0;
+
+	if (opt == 0)
+		isp->css_env.isp_css_env.print_env.debug_print = NULL;
+	else if (opt == 1)
+		isp->css_env.isp_css_env.print_env.debug_print =
+		    atomisp_css2_dbg_ftrace_print;
+	else if (opt == 2)
+		isp->css_env.isp_css_env.print_env.debug_print = atomisp_vprintk;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+int atomisp_css_load_firmware(struct atomisp_device *isp)
+{
+	int err;
+
+	/* set css env */
+	isp->css_env.isp_css_fw.data = (void *)isp->firmware->data;
+	isp->css_env.isp_css_fw.bytes = isp->firmware->size;
+
+	isp->css_env.isp_css_env.hw_access_env.store_8 =
+	    atomisp_css2_hw_store_8;
+	isp->css_env.isp_css_env.hw_access_env.store_16 =
+	    atomisp_css2_hw_store_16;
+	isp->css_env.isp_css_env.hw_access_env.store_32 =
+	    atomisp_css2_hw_store_32;
+
+	isp->css_env.isp_css_env.hw_access_env.load_8 = atomisp_css2_hw_load_8;
+	isp->css_env.isp_css_env.hw_access_env.load_16 =
+	    atomisp_css2_hw_load_16;
+	isp->css_env.isp_css_env.hw_access_env.load_32 =
+	    atomisp_css2_hw_load_32;
+
+	isp->css_env.isp_css_env.hw_access_env.load = atomisp_css2_hw_load;
+	isp->css_env.isp_css_env.hw_access_env.store = atomisp_css2_hw_store;
+
+	__set_css_print_env(isp, dbg_func);
+
+	isp->css_env.isp_css_env.print_env.error_print = atomisp_vprintk;
+
+	/* load isp fw into ISP memory */
+	err = ia_css_load_firmware(isp->dev, &isp->css_env.isp_css_env,
+				   &isp->css_env.isp_css_fw);
+	if (err) {
+		dev_err(isp->dev, "css load fw failed.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void atomisp_css_uninit(struct atomisp_device *isp)
+{
+	struct atomisp_sub_device *asd;
+	unsigned int i;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		memset(&asd->params.config, 0, sizeof(asd->params.config));
+		asd->params.css_update_params_needed = false;
+	}
+
+	isp->css_initialized = false;
+	ia_css_uninit();
+}
+
+void atomisp_css_suspend(struct atomisp_device *isp)
+{
+	isp->css_initialized = false;
+	ia_css_uninit();
+}
+
+int atomisp_css_resume(struct atomisp_device *isp)
+{
+	unsigned int mmu_base_addr;
+	int ret;
+
+	ret = hmm_get_mmu_base_addr(isp->dev, &mmu_base_addr);
+	if (ret) {
+		dev_err(isp->dev, "get base address error.\n");
+		return -EINVAL;
+	}
+
+	ret = ia_css_init(isp->dev, &isp->css_env.isp_css_env, NULL,
+			  mmu_base_addr, IA_CSS_IRQ_TYPE_PULSE);
+	if (ret) {
+		dev_err(isp->dev, "re-init css failed.\n");
+		return -EINVAL;
+	}
+	ia_css_enable_isys_event_queue(true);
+
+	isp->css_initialized = true;
+	return 0;
+}
+
+int atomisp_css_irq_translate(struct atomisp_device *isp,
+			      unsigned int *infos)
+{
+	int err;
+
+	err = ia_css_irq_translate(infos);
+	if (err) {
+		dev_warn(isp->dev,
+			 "%s:failed to translate irq (err = %d,infos = %d)\n",
+			 __func__, err, *infos);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void atomisp_css_rx_get_irq_info(enum mipi_port_id port,
+				 unsigned int *infos)
+{
+#ifndef ISP2401
+	ia_css_isys_rx_get_irq_info(port, infos);
+#else
+	*infos = 0;
+#endif
+}
+
+void atomisp_css_rx_clear_irq_info(enum mipi_port_id port,
+				   unsigned int infos)
+{
+#ifndef ISP2401
+	ia_css_isys_rx_clear_irq_info(port, infos);
+#endif
+}
+
+int atomisp_css_irq_enable(struct atomisp_device *isp,
+			   enum ia_css_irq_info info, bool enable)
+{
+	dev_dbg(isp->dev, "%s: css irq info 0x%08x: %s (%d).\n",
+		__func__, info,
+		enable ? "enable" : "disable", enable);
+	if (ia_css_irq_enable(info, enable)) {
+		dev_warn(isp->dev, "%s:Invalid irq info: 0x%08x when %s.\n",
+			 __func__, info,
+			 enable ? "enabling" : "disabling");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void atomisp_css_init_struct(struct atomisp_sub_device *asd)
+{
+	int i, j;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		asd->stream_env[i].stream = NULL;
+		for (j = 0; j < IA_CSS_PIPE_MODE_NUM; j++) {
+			asd->stream_env[i].pipes[j] = NULL;
+			asd->stream_env[i].update_pipe[j] = false;
+			ia_css_pipe_config_defaults(
+			    &asd->stream_env[i].pipe_configs[j]);
+			ia_css_pipe_extra_config_defaults(
+			    &asd->stream_env[i].pipe_extra_configs[j]);
+		}
+		ia_css_stream_config_defaults(&asd->stream_env[i].stream_config);
+	}
+}
+
+int atomisp_q_video_buffer_to_css(struct atomisp_sub_device *asd,
+				  struct videobuf_vmalloc_memory *vm_mem,
+				  enum atomisp_input_stream_id stream_id,
+				  enum ia_css_buffer_type css_buf_type,
+				  enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_stream_env *stream_env = &asd->stream_env[stream_id];
+	struct ia_css_buffer css_buf = {0};
+	int err;
+
+	css_buf.type = css_buf_type;
+	css_buf.data.frame = vm_mem->vaddr;
+
+	err = ia_css_pipe_enqueue_buffer(
+		  stream_env->pipes[css_pipe_id], &css_buf);
+	if (err)
+		return -EINVAL;
+
+	return 0;
+}
+
+int atomisp_q_metadata_buffer_to_css(struct atomisp_sub_device *asd,
+				     struct atomisp_metadata_buf *metadata_buf,
+				     enum atomisp_input_stream_id stream_id,
+				     enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_stream_env *stream_env = &asd->stream_env[stream_id];
+	struct ia_css_buffer buffer = {0};
+	struct atomisp_device *isp = asd->isp;
+
+	buffer.type = IA_CSS_BUFFER_TYPE_METADATA;
+	buffer.data.metadata = metadata_buf->metadata;
+	if (ia_css_pipe_enqueue_buffer(stream_env->pipes[css_pipe_id],
+				       &buffer)) {
+		dev_err(isp->dev, "failed to q meta data buffer\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_q_s3a_buffer_to_css(struct atomisp_sub_device *asd,
+				struct atomisp_s3a_buf *s3a_buf,
+				enum atomisp_input_stream_id stream_id,
+				enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_stream_env *stream_env = &asd->stream_env[stream_id];
+	struct ia_css_buffer buffer = {0};
+	struct atomisp_device *isp = asd->isp;
+
+	buffer.type = IA_CSS_BUFFER_TYPE_3A_STATISTICS;
+	buffer.data.stats_3a = s3a_buf->s3a_data;
+	if (ia_css_pipe_enqueue_buffer(
+		stream_env->pipes[css_pipe_id],
+		&buffer)) {
+		dev_dbg(isp->dev, "failed to q s3a stat buffer\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_q_dis_buffer_to_css(struct atomisp_sub_device *asd,
+				struct atomisp_dis_buf *dis_buf,
+				enum atomisp_input_stream_id stream_id,
+				enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_stream_env *stream_env = &asd->stream_env[stream_id];
+	struct ia_css_buffer buffer = {0};
+	struct atomisp_device *isp = asd->isp;
+
+	buffer.type = IA_CSS_BUFFER_TYPE_DIS_STATISTICS;
+	buffer.data.stats_dvs = dis_buf->dis_data;
+	if (ia_css_pipe_enqueue_buffer(
+		stream_env->pipes[css_pipe_id],
+		&buffer)) {
+		dev_dbg(isp->dev, "failed to q dvs stat buffer\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_css_start(struct atomisp_sub_device *asd,
+		      enum ia_css_pipe_id pipe_id, bool in_reset)
+{
+	struct atomisp_device *isp = asd->isp;
+	bool sp_is_started = false;
+	int ret = 0, i = 0;
+
+	if (in_reset) {
+		ret = atomisp_css_update_stream(asd);
+		if (ret)
+			return ret;
+
+		/* Invalidate caches. FIXME: should flush only necessary buffers */
+		wbinvd();
+	}
+
+	/*
+	 * For dual steam case, it is possible that:
+	 * 1: for this stream, it is at the stage that:
+	 * - after set_fmt is called
+	 * - before stream on is called
+	 * 2: for the other stream, the stream off is called which css reset
+	 * has been done.
+	 *
+	 * Thus the stream created in set_fmt get destroyed and need to be
+	 * recreated in the next stream on.
+	 */
+	if (!asd->stream_prepared) {
+		ret = atomisp_create_pipes_stream(asd);
+		if (ret)
+			return ret;
+	}
+	/*
+	 * SP can only be started one time
+	 * if atomisp_subdev_streaming_count() tell there already has some
+	 * subdev at streamming, then SP should already be started previously,
+	 * so need to skip start sp procedure
+	 */
+	if (atomisp_streaming_count(isp)) {
+		dev_dbg(isp->dev, "skip start sp\n");
+	} else {
+		if (!sh_css_hrt_system_is_idle())
+			dev_err(isp->dev, "CSS HW not idle before starting SP\n");
+		if (ia_css_start_sp()) {
+			dev_err(isp->dev, "start sp error.\n");
+			ret = -EINVAL;
+			goto start_err;
+		} else {
+			sp_is_started = true;
+		}
+	}
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		if (asd->stream_env[i].stream) {
+			if (ia_css_stream_start(asd->stream_env[i]
+						.stream) != 0) {
+				dev_err(isp->dev, "stream[%d] start error.\n", i);
+				ret = -EINVAL;
+				goto start_err;
+			} else {
+				asd->stream_env[i].stream_state = CSS_STREAM_STARTED;
+				dev_dbg(isp->dev, "stream[%d] started.\n", i);
+			}
+		}
+	}
+
+	return 0;
+
+start_err:
+	atomisp_destroy_pipes_stream_force(asd);
+
+	/* css 2.0 API limitation: ia_css_stop_sp() could be only called after
+	 * destroy all pipes
+	 */
+	/*
+	 * SP can not be stop if other streams are in use
+	 */
+	if ((atomisp_streaming_count(isp) == 0) && sp_is_started)
+		ia_css_stop_sp();
+
+	return ret;
+}
+
+void atomisp_css_update_isp_params(struct atomisp_sub_device *asd)
+{
+	/*
+	 * FIXME!
+	 * for ISP2401 new input system, this api is under development.
+	 * Calling it would cause kernel panic.
+	 *
+	 * VIED BZ: 1458
+	 *
+	 * Check if it is Cherry Trail and also new input system
+	 */
+	if (asd->copy_mode) {
+		dev_warn(asd->isp->dev,
+			 "%s: ia_css_stream_set_isp_config() not supported in copy mode!.\n",
+			 __func__);
+		return;
+	}
+
+	ia_css_stream_set_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &asd->params.config);
+	memset(&asd->params.config, 0, sizeof(asd->params.config));
+}
+
+void atomisp_css_update_isp_params_on_pipe(struct atomisp_sub_device *asd,
+	struct ia_css_pipe *pipe)
+{
+	int ret;
+
+	if (!pipe) {
+		atomisp_css_update_isp_params(asd);
+		return;
+	}
+
+	dev_dbg(asd->isp->dev,
+		"%s: apply parameter for ia_css_frame %p with isp_config_id %d on pipe %p.\n",
+		__func__, asd->params.config.output_frame,
+		asd->params.config.isp_config_id, pipe);
+
+	ret = ia_css_stream_set_isp_config_on_pipe(
+		  asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		  &asd->params.config, pipe);
+	if (ret)
+		dev_warn(asd->isp->dev, "%s: ia_css_stream_set_isp_config_on_pipe failed %d\n",
+			 __func__, ret);
+	memset(&asd->params.config, 0, sizeof(asd->params.config));
+}
+
+int atomisp_css_queue_buffer(struct atomisp_sub_device *asd,
+			     enum atomisp_input_stream_id stream_id,
+			     enum ia_css_pipe_id pipe_id,
+			     enum ia_css_buffer_type buf_type,
+			     struct atomisp_css_buffer *isp_css_buffer)
+{
+	if (ia_css_pipe_enqueue_buffer(
+		asd->stream_env[stream_id].pipes[pipe_id],
+		&isp_css_buffer->css_buffer)
+	    != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int atomisp_css_dequeue_buffer(struct atomisp_sub_device *asd,
+			       enum atomisp_input_stream_id stream_id,
+			       enum ia_css_pipe_id pipe_id,
+			       enum ia_css_buffer_type buf_type,
+			       struct atomisp_css_buffer *isp_css_buffer)
+{
+	struct atomisp_device *isp = asd->isp;
+	int err;
+
+	err = ia_css_pipe_dequeue_buffer(
+		  asd->stream_env[stream_id].pipes[pipe_id],
+		  &isp_css_buffer->css_buffer);
+	if (err) {
+		dev_err(isp->dev,
+			"ia_css_pipe_dequeue_buffer failed: 0x%x\n", err);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int atomisp_css_allocate_stat_buffers(struct atomisp_sub_device   *asd,
+				      u16 stream_id,
+				      struct atomisp_s3a_buf      *s3a_buf,
+				      struct atomisp_dis_buf      *dis_buf,
+				      struct atomisp_metadata_buf *md_buf)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_dvs_grid_info *dvs_grid_info =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+
+	if (s3a_buf && asd->params.curr_grid_info.s3a_grid.enable) {
+		void *s3a_ptr;
+
+		s3a_buf->s3a_data = ia_css_isp_3a_statistics_allocate(
+					&asd->params.curr_grid_info.s3a_grid);
+		if (!s3a_buf->s3a_data) {
+			dev_err(isp->dev, "3a buf allocation failed.\n");
+			return -EINVAL;
+		}
+
+		s3a_ptr = hmm_vmap(s3a_buf->s3a_data->data_ptr, true);
+		s3a_buf->s3a_map = ia_css_isp_3a_statistics_map_allocate(
+				       s3a_buf->s3a_data, s3a_ptr);
+	}
+
+	if (dis_buf && dvs_grid_info && dvs_grid_info->enable) {
+		void *dvs_ptr;
+
+		dis_buf->dis_data = ia_css_isp_dvs2_statistics_allocate(
+					dvs_grid_info);
+		if (!dis_buf->dis_data) {
+			dev_err(isp->dev, "dvs buf allocation failed.\n");
+			if (s3a_buf)
+				ia_css_isp_3a_statistics_free(s3a_buf->s3a_data);
+			return -EINVAL;
+		}
+
+		dvs_ptr = hmm_vmap(dis_buf->dis_data->data_ptr, true);
+		dis_buf->dvs_map = ia_css_isp_dvs_statistics_map_allocate(
+				       dis_buf->dis_data, dvs_ptr);
+	}
+
+	if (asd->stream_env[stream_id].stream_info.
+	    metadata_info.size && md_buf) {
+		md_buf->metadata = ia_css_metadata_allocate(
+				       &asd->stream_env[stream_id].stream_info.metadata_info);
+		if (!md_buf->metadata) {
+			if (s3a_buf)
+				ia_css_isp_3a_statistics_free(s3a_buf->s3a_data);
+			if (dis_buf)
+				ia_css_isp_dvs2_statistics_free(dis_buf->dis_data);
+			dev_err(isp->dev, "metadata buf allocation failed.\n");
+			return -EINVAL;
+		}
+		md_buf->md_vptr = hmm_vmap(md_buf->metadata->address, false);
+	}
+
+	return 0;
+}
+
+void atomisp_css_free_3a_buffer(struct atomisp_s3a_buf *s3a_buf)
+{
+	if (s3a_buf->s3a_data)
+		hmm_vunmap(s3a_buf->s3a_data->data_ptr);
+
+	ia_css_isp_3a_statistics_map_free(s3a_buf->s3a_map);
+	s3a_buf->s3a_map = NULL;
+	ia_css_isp_3a_statistics_free(s3a_buf->s3a_data);
+}
+
+void atomisp_css_free_dis_buffer(struct atomisp_dis_buf *dis_buf)
+{
+	if (dis_buf->dis_data)
+		hmm_vunmap(dis_buf->dis_data->data_ptr);
+
+	ia_css_isp_dvs_statistics_map_free(dis_buf->dvs_map);
+	dis_buf->dvs_map = NULL;
+	ia_css_isp_dvs2_statistics_free(dis_buf->dis_data);
+}
+
+void atomisp_css_free_metadata_buffer(struct atomisp_metadata_buf *metadata_buf)
+{
+	if (metadata_buf->md_vptr) {
+		hmm_vunmap(metadata_buf->metadata->address);
+		metadata_buf->md_vptr = NULL;
+	}
+	ia_css_metadata_free(metadata_buf->metadata);
+}
+
+void atomisp_css_free_stat_buffers(struct atomisp_sub_device *asd)
+{
+	struct atomisp_s3a_buf *s3a_buf, *_s3a_buf;
+	struct atomisp_dis_buf *dis_buf, *_dis_buf;
+	struct atomisp_metadata_buf *md_buf, *_md_buf;
+	struct ia_css_dvs_grid_info *dvs_grid_info =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	unsigned int i;
+
+	/* 3A statistics use vmalloc, DIS use kmalloc */
+	if (dvs_grid_info && dvs_grid_info->enable) {
+		ia_css_dvs2_coefficients_free(asd->params.css_param.dvs2_coeff);
+		ia_css_dvs2_statistics_free(asd->params.dvs_stat);
+		asd->params.css_param.dvs2_coeff = NULL;
+		asd->params.dvs_stat = NULL;
+		asd->params.dvs_hor_proj_bytes = 0;
+		asd->params.dvs_ver_proj_bytes = 0;
+		asd->params.dvs_hor_coef_bytes = 0;
+		asd->params.dvs_ver_coef_bytes = 0;
+		asd->params.dis_proj_data_valid = false;
+		list_for_each_entry_safe(dis_buf, _dis_buf,
+					 &asd->dis_stats, list) {
+			atomisp_css_free_dis_buffer(dis_buf);
+			list_del(&dis_buf->list);
+			kfree(dis_buf);
+		}
+		list_for_each_entry_safe(dis_buf, _dis_buf,
+					 &asd->dis_stats_in_css, list) {
+			atomisp_css_free_dis_buffer(dis_buf);
+			list_del(&dis_buf->list);
+			kfree(dis_buf);
+		}
+	}
+	if (asd->params.curr_grid_info.s3a_grid.enable) {
+		ia_css_3a_statistics_free(asd->params.s3a_user_stat);
+		asd->params.s3a_user_stat = NULL;
+		asd->params.s3a_output_bytes = 0;
+		list_for_each_entry_safe(s3a_buf, _s3a_buf,
+					 &asd->s3a_stats, list) {
+			atomisp_css_free_3a_buffer(s3a_buf);
+			list_del(&s3a_buf->list);
+			kfree(s3a_buf);
+		}
+		list_for_each_entry_safe(s3a_buf, _s3a_buf,
+					 &asd->s3a_stats_in_css, list) {
+			atomisp_css_free_3a_buffer(s3a_buf);
+			list_del(&s3a_buf->list);
+			kfree(s3a_buf);
+		}
+		list_for_each_entry_safe(s3a_buf, _s3a_buf,
+					 &asd->s3a_stats_ready, list) {
+			atomisp_css_free_3a_buffer(s3a_buf);
+			list_del(&s3a_buf->list);
+			kfree(s3a_buf);
+		}
+	}
+
+	if (asd->params.css_param.dvs_6axis) {
+		ia_css_dvs2_6axis_config_free(asd->params.css_param.dvs_6axis);
+		asd->params.css_param.dvs_6axis = NULL;
+	}
+
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		list_for_each_entry_safe(md_buf, _md_buf,
+					 &asd->metadata[i], list) {
+			atomisp_css_free_metadata_buffer(md_buf);
+			list_del(&md_buf->list);
+			kfree(md_buf);
+		}
+		list_for_each_entry_safe(md_buf, _md_buf,
+					 &asd->metadata_in_css[i], list) {
+			atomisp_css_free_metadata_buffer(md_buf);
+			list_del(&md_buf->list);
+			kfree(md_buf);
+		}
+		list_for_each_entry_safe(md_buf, _md_buf,
+					 &asd->metadata_ready[i], list) {
+			atomisp_css_free_metadata_buffer(md_buf);
+			list_del(&md_buf->list);
+			kfree(md_buf);
+		}
+	}
+	asd->params.metadata_width_size = 0;
+	atomisp_free_metadata_output_buf(asd);
+}
+
+int atomisp_css_get_grid_info(struct atomisp_sub_device *asd,
+			      enum ia_css_pipe_id pipe_id,
+			      int source_pad)
+{
+	struct ia_css_pipe_info p_info;
+	struct ia_css_grid_info old_info;
+	struct atomisp_device *isp = asd->isp;
+	int md_width = asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		       stream_config.metadata_config.resolution.width;
+
+	memset(&p_info, 0, sizeof(struct ia_css_pipe_info));
+	memset(&old_info, 0, sizeof(struct ia_css_grid_info));
+
+	if (ia_css_pipe_get_info(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].pipes[pipe_id],
+		&p_info) != 0) {
+		dev_err(isp->dev, "ia_css_pipe_get_info failed\n");
+		return -EINVAL;
+	}
+
+	memcpy(&old_info, &asd->params.curr_grid_info,
+	       sizeof(struct ia_css_grid_info));
+	memcpy(&asd->params.curr_grid_info, &p_info.grid_info,
+	       sizeof(struct ia_css_grid_info));
+	/*
+	 * Record which css pipe enables s3a_grid.
+	 * Currently would have one css pipe that need it
+	 */
+	if (asd->params.curr_grid_info.s3a_grid.enable) {
+		if (asd->params.s3a_enabled_pipe != IA_CSS_PIPE_ID_NUM)
+			dev_dbg(isp->dev, "css pipe %d enabled s3a grid replaced by: %d.\n",
+				asd->params.s3a_enabled_pipe, pipe_id);
+		asd->params.s3a_enabled_pipe = pipe_id;
+	}
+
+	/* If the grid info has not changed and the buffers for 3A and
+	 * DIS statistics buffers are allocated or buffer size would be zero
+	 * then no need to do anything. */
+	if (((!memcmp(&old_info, &asd->params.curr_grid_info, sizeof(old_info))
+	      && asd->params.s3a_user_stat && asd->params.dvs_stat)
+	     || asd->params.curr_grid_info.s3a_grid.width == 0
+	     || asd->params.curr_grid_info.s3a_grid.height == 0)
+	    && asd->params.metadata_width_size == md_width) {
+		dev_dbg(isp->dev,
+			"grid info change escape. memcmp=%d, s3a_user_stat=%d,dvs_stat=%d, s3a.width=%d, s3a.height=%d, metadata width =%d\n",
+			!memcmp(&old_info, &asd->params.curr_grid_info,
+				sizeof(old_info)),
+			!!asd->params.s3a_user_stat, !!asd->params.dvs_stat,
+			asd->params.curr_grid_info.s3a_grid.width,
+			asd->params.curr_grid_info.s3a_grid.height,
+			asd->params.metadata_width_size);
+		return -EINVAL;
+	}
+	asd->params.metadata_width_size = md_width;
+
+	return 0;
+}
+
+int atomisp_alloc_3a_output_buf(struct atomisp_sub_device *asd)
+{
+	if (!asd->params.curr_grid_info.s3a_grid.width ||
+	    !asd->params.curr_grid_info.s3a_grid.height)
+		return 0;
+
+	asd->params.s3a_user_stat = ia_css_3a_statistics_allocate(
+					&asd->params.curr_grid_info.s3a_grid);
+	if (!asd->params.s3a_user_stat)
+		return -ENOMEM;
+	/* 3A statistics. These can be big, so we use vmalloc. */
+	asd->params.s3a_output_bytes =
+	    asd->params.curr_grid_info.s3a_grid.width *
+	    asd->params.curr_grid_info.s3a_grid.height *
+	    sizeof(*asd->params.s3a_user_stat->data);
+
+	return 0;
+}
+
+int atomisp_alloc_dis_coef_buf(struct atomisp_sub_device *asd)
+{
+	struct ia_css_dvs_grid_info *dvs_grid =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+
+	if (!dvs_grid)
+		return 0;
+
+	if (!dvs_grid->enable) {
+		dev_dbg(asd->isp->dev, "%s: dvs_grid not enabled.\n", __func__);
+		return 0;
+	}
+
+	/* DIS coefficients. */
+	asd->params.css_param.dvs2_coeff = ia_css_dvs2_coefficients_allocate(
+					       dvs_grid);
+	if (!asd->params.css_param.dvs2_coeff)
+		return -ENOMEM;
+
+	asd->params.dvs_hor_coef_bytes = dvs_grid->num_hor_coefs *
+					 sizeof(*asd->params.css_param.dvs2_coeff->hor_coefs.odd_real);
+
+	asd->params.dvs_ver_coef_bytes = dvs_grid->num_ver_coefs *
+					 sizeof(*asd->params.css_param.dvs2_coeff->ver_coefs.odd_real);
+
+	/* DIS projections. */
+	asd->params.dis_proj_data_valid = false;
+	asd->params.dvs_stat = ia_css_dvs2_statistics_allocate(dvs_grid);
+	if (!asd->params.dvs_stat)
+		return -ENOMEM;
+
+	asd->params.dvs_hor_proj_bytes =
+	    dvs_grid->aligned_height * dvs_grid->aligned_width *
+	    sizeof(*asd->params.dvs_stat->hor_prod.odd_real);
+
+	asd->params.dvs_ver_proj_bytes =
+	    dvs_grid->aligned_height * dvs_grid->aligned_width *
+	    sizeof(*asd->params.dvs_stat->ver_prod.odd_real);
+
+	return 0;
+}
+
+int atomisp_alloc_metadata_output_buf(struct atomisp_sub_device *asd)
+{
+	int i;
+
+	/* We allocate the cpu-side buffer used for communication with user
+	 * space */
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		asd->params.metadata_user[i] = kvmalloc(
+						   asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+						   stream_info.metadata_info.size, GFP_KERNEL);
+		if (!asd->params.metadata_user[i]) {
+			while (--i >= 0) {
+				kvfree(asd->params.metadata_user[i]);
+				asd->params.metadata_user[i] = NULL;
+			}
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+void atomisp_free_metadata_output_buf(struct atomisp_sub_device *asd)
+{
+	unsigned int i;
+
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		if (asd->params.metadata_user[i]) {
+			kvfree(asd->params.metadata_user[i]);
+			asd->params.metadata_user[i] = NULL;
+		}
+	}
+}
+
+void atomisp_css_temp_pipe_to_pipe_id(struct atomisp_sub_device *asd,
+				      struct atomisp_css_event *current_event)
+{
+	/*
+	 * FIXME!
+	 * Pipe ID reported in CSS event is not correct for new system's
+	 * copy pipe.
+	 * VIED BZ: 1463
+	 */
+	ia_css_temp_pipe_to_pipe_id(current_event->event.pipe,
+				    &current_event->pipe);
+	if (asd && asd->copy_mode &&
+	    current_event->pipe == IA_CSS_PIPE_ID_CAPTURE)
+		current_event->pipe = IA_CSS_PIPE_ID_COPY;
+}
+
+int atomisp_css_isys_set_resolution(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    struct v4l2_mbus_framefmt *ffmt,
+				    int isys_stream)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	if (isys_stream >= IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH)
+		return -EINVAL;
+
+	s_config->isys_config[isys_stream].input_res.width = ffmt->width;
+	s_config->isys_config[isys_stream].input_res.height = ffmt->height;
+	return 0;
+}
+
+int atomisp_css_input_set_resolution(struct atomisp_sub_device *asd,
+				     enum atomisp_input_stream_id stream_id,
+				     struct v4l2_mbus_framefmt *ffmt)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->input_config.input_res.width = ffmt->width;
+	s_config->input_config.input_res.height = ffmt->height;
+	return 0;
+}
+
+void atomisp_css_input_set_binning_factor(struct atomisp_sub_device *asd,
+	enum atomisp_input_stream_id stream_id,
+	unsigned int bin_factor)
+{
+	asd->stream_env[stream_id]
+	.stream_config.sensor_binning_factor = bin_factor;
+}
+
+void atomisp_css_input_set_bayer_order(struct atomisp_sub_device *asd,
+				       enum atomisp_input_stream_id stream_id,
+				       enum ia_css_bayer_order bayer_order)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+	s_config->input_config.bayer_order = bayer_order;
+}
+
+void atomisp_css_isys_set_link(struct atomisp_sub_device *asd,
+			       enum atomisp_input_stream_id stream_id,
+			       int link,
+			       int isys_stream)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[isys_stream].linked_isys_stream_id = link;
+}
+
+void atomisp_css_isys_set_valid(struct atomisp_sub_device *asd,
+				enum atomisp_input_stream_id stream_id,
+				bool valid,
+				int isys_stream)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[isys_stream].valid = valid;
+}
+
+void atomisp_css_isys_set_format(struct atomisp_sub_device *asd,
+				 enum atomisp_input_stream_id stream_id,
+				 enum atomisp_input_format format,
+				 int isys_stream)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[isys_stream].format = format;
+}
+
+void atomisp_css_input_set_format(struct atomisp_sub_device *asd,
+				  enum atomisp_input_stream_id stream_id,
+				  enum atomisp_input_format format)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->input_config.format = format;
+}
+
+int atomisp_css_set_default_isys_config(struct atomisp_sub_device *asd,
+					enum atomisp_input_stream_id stream_id,
+					struct v4l2_mbus_framefmt *ffmt)
+{
+	int i;
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+	/*
+	 * Set all isys configs to not valid.
+	 * Currently we support only one stream per channel
+	 */
+	for (i = IA_CSS_STREAM_ISYS_STREAM_0;
+	     i < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH; i++)
+		s_config->isys_config[i].valid = false;
+
+	atomisp_css_isys_set_resolution(asd, stream_id, ffmt,
+					IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
+	atomisp_css_isys_set_format(asd, stream_id,
+				    s_config->input_config.format,
+				    IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
+	atomisp_css_isys_set_link(asd, stream_id, NO_LINK,
+				  IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
+	atomisp_css_isys_set_valid(asd, stream_id, true,
+				   IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
+
+	return 0;
+}
+
+int atomisp_css_isys_two_stream_cfg(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum atomisp_input_format input_format)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].input_res.width =
+	    s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].input_res.width;
+
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].input_res.height =
+	    s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].input_res.height / 2;
+
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].linked_isys_stream_id
+	    = IA_CSS_STREAM_ISYS_STREAM_0;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].format =
+	    ATOMISP_INPUT_FORMAT_USER_DEF1;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].format =
+	    ATOMISP_INPUT_FORMAT_USER_DEF2;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].valid = true;
+	return 0;
+}
+
+void atomisp_css_isys_two_stream_cfg_update_stream1(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    enum atomisp_input_format input_format,
+    unsigned int width, unsigned int height)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].input_res.width =
+	    width;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].input_res.height =
+	    height;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].format =
+	    input_format;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_0].valid = true;
+}
+
+void atomisp_css_isys_two_stream_cfg_update_stream2(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    enum atomisp_input_format input_format,
+    unsigned int width, unsigned int height)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].input_res.width =
+	    width;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].input_res.height =
+	    height;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].linked_isys_stream_id
+	    = IA_CSS_STREAM_ISYS_STREAM_0;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].format =
+	    input_format;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].valid = true;
+}
+
+int atomisp_css_input_set_effective_resolution(
+    struct atomisp_sub_device *asd,
+    enum atomisp_input_stream_id stream_id,
+    unsigned int width, unsigned int height)
+{
+	struct ia_css_stream_config *s_config =
+		    &asd->stream_env[stream_id].stream_config;
+	s_config->input_config.effective_res.width = width;
+	s_config->input_config.effective_res.height = height;
+	return 0;
+}
+
+void atomisp_css_video_set_dis_envelope(struct atomisp_sub_device *asd,
+					unsigned int dvs_w, unsigned int dvs_h)
+{
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.pipe_configs[IA_CSS_PIPE_ID_VIDEO].dvs_envelope.width = dvs_w;
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.pipe_configs[IA_CSS_PIPE_ID_VIDEO].dvs_envelope.height = dvs_h;
+}
+
+void atomisp_css_input_set_two_pixels_per_clock(
+    struct atomisp_sub_device *asd,
+    bool two_ppc)
+{
+	int i;
+
+	if (asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	    .stream_config.pixels_per_clock == (two_ppc ? 2 : 1))
+		return;
+
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.stream_config.pixels_per_clock = (two_ppc ? 2 : 1);
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.update_pipe[i] = true;
+}
+
+void atomisp_css_enable_raw_binning(struct atomisp_sub_device *asd,
+				    bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	unsigned int pipe;
+
+	if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+		pipe = IA_CSS_PIPE_ID_VIDEO;
+	else
+		pipe = IA_CSS_PIPE_ID_PREVIEW;
+
+	stream_env->pipe_extra_configs[pipe].enable_raw_binning = enable;
+	stream_env->update_pipe[pipe] = true;
+	if (enable)
+		stream_env->pipe_configs[pipe].output_info[0].padded_width =
+		    stream_env->stream_config.input_config.effective_res.width;
+}
+
+void atomisp_css_enable_dz(struct atomisp_sub_device *asd, bool enable)
+{
+	int i;
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.pipe_configs[i].enable_dz = enable;
+}
+
+void atomisp_css_capture_set_mode(struct atomisp_sub_device *asd,
+				  enum ia_css_capture_mode mode)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+
+	if (stream_env->pipe_configs[IA_CSS_PIPE_ID_CAPTURE]
+	    .default_capture_config.mode == mode)
+		return;
+
+	stream_env->pipe_configs[IA_CSS_PIPE_ID_CAPTURE].
+	default_capture_config.mode = mode;
+	stream_env->update_pipe[IA_CSS_PIPE_ID_CAPTURE] = true;
+}
+
+void atomisp_css_input_set_mode(struct atomisp_sub_device *asd,
+				enum ia_css_input_mode mode)
+{
+	int i;
+	struct atomisp_device *isp = asd->isp;
+	unsigned int size_mem_words;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++)
+		asd->stream_env[i].stream_config.mode = mode;
+
+	if (isp->inputs[asd->input_curr].type == TEST_PATTERN) {
+		struct ia_css_stream_config *s_config =
+			    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_config;
+		s_config->mode = IA_CSS_INPUT_MODE_TPG;
+		s_config->source.tpg.mode = IA_CSS_TPG_MODE_CHECKERBOARD;
+		s_config->source.tpg.x_mask = (1 << 4) - 1;
+		s_config->source.tpg.x_delta = -2;
+		s_config->source.tpg.y_mask = (1 << 4) - 1;
+		s_config->source.tpg.y_delta = 3;
+		s_config->source.tpg.xy_mask = (1 << 8) - 1;
+		return;
+	}
+
+	if (mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR)
+		return;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		/*
+		 * TODO: sensor needs to export the embedded_data_size_words
+		 * information to atomisp for each setting.
+		 * Here using a large safe value.
+		 */
+		struct ia_css_stream_config *s_config =
+			    &asd->stream_env[i].stream_config;
+
+		if (s_config->input_config.input_res.width == 0)
+			continue;
+
+		if (ia_css_mipi_frame_calculate_size(
+			s_config->input_config.input_res.width,
+			s_config->input_config.input_res.height,
+			s_config->input_config.format,
+			true,
+			0x13000,
+			&size_mem_words) != 0) {
+			if (IS_MRFD)
+				size_mem_words = CSS_MIPI_FRAME_BUFFER_SIZE_2;
+			else
+				size_mem_words = CSS_MIPI_FRAME_BUFFER_SIZE_1;
+			dev_warn(asd->isp->dev,
+				 "ia_css_mipi_frame_calculate_size failed,applying pre-defined MIPI buffer size %u.\n",
+				 size_mem_words);
+		}
+		s_config->mipi_buffer_config.size_mem_words = size_mem_words;
+		s_config->mipi_buffer_config.nof_mipi_buffers = 2;
+	}
+}
+
+void atomisp_css_capture_enable_online(struct atomisp_sub_device *asd,
+				       unsigned short stream_index, bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[stream_index];
+
+	if (stream_env->stream_config.online == !!enable)
+		return;
+
+	stream_env->stream_config.online = !!enable;
+	stream_env->update_pipe[IA_CSS_PIPE_ID_CAPTURE] = true;
+}
+
+void atomisp_css_preview_enable_online(struct atomisp_sub_device *asd,
+				       unsigned short stream_index, bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[stream_index];
+	int i;
+
+	if (stream_env->stream_config.online != !!enable) {
+		stream_env->stream_config.online = !!enable;
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+			stream_env->update_pipe[i] = true;
+	}
+}
+
+void atomisp_css_video_enable_online(struct atomisp_sub_device *asd,
+				     bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_VIDEO];
+	int i;
+
+	if (stream_env->stream_config.online != enable) {
+		stream_env->stream_config.online = enable;
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+			stream_env->update_pipe[i] = true;
+	}
+}
+
+void atomisp_css_enable_continuous(struct atomisp_sub_device *asd,
+				   bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	int i;
+
+	/*
+	 * To SOC camera, there is only one YUVPP pipe in any case
+	 * including ZSL/SDV/continuous viewfinder, so always set
+	 * stream_config.continuous to 0.
+	 */
+	if (ATOMISP_USE_YUVPP(asd)) {
+		stream_env->stream_config.continuous = 0;
+		stream_env->stream_config.online = 1;
+		return;
+	}
+
+	if (stream_env->stream_config.continuous != !!enable) {
+		stream_env->stream_config.continuous = !!enable;
+		stream_env->stream_config.pack_raw_pixels = true;
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+			stream_env->update_pipe[i] = true;
+	}
+}
+
+void atomisp_css_enable_cvf(struct atomisp_sub_device *asd,
+			    bool enable)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	int i;
+
+	if (stream_env->stream_config.disable_cont_viewfinder != !enable) {
+		stream_env->stream_config.disable_cont_viewfinder = !enable;
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+			stream_env->update_pipe[i] = true;
+	}
+}
+
+int atomisp_css_input_configure_port(
+    struct atomisp_sub_device *asd,
+    enum mipi_port_id port,
+    unsigned int num_lanes,
+    unsigned int timeout,
+    unsigned int mipi_freq,
+    enum atomisp_input_format metadata_format,
+    unsigned int metadata_width,
+    unsigned int metadata_height)
+{
+	int i;
+	struct atomisp_stream_env *stream_env;
+	/*
+	 * Calculate rx_count as follows:
+	 * Input: mipi_freq                 : CSI-2 bus frequency in Hz
+	 * UI = 1 / (2 * mipi_freq)         : period of one bit on the bus
+	 * min = 85e-9 + 6 * UI             : Limits for rx_count in seconds
+	 * max = 145e-9 + 10 * UI
+	 * rxcount0 = min / (4 / mipi_freq) : convert seconds to byte clocks
+	 * rxcount = rxcount0 - 2           : adjust for better results
+	 * The formula below is simplified version of the above with
+	 * 10-bit fixed points for improved accuracy.
+	 */
+	const unsigned int rxcount =
+	    min(((mipi_freq / 46000) - 1280) >> 10, 0xffU) * 0x01010101U;
+
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		stream_env = &asd->stream_env[i];
+		stream_env->stream_config.source.port.port = port;
+		stream_env->stream_config.source.port.num_lanes = num_lanes;
+		stream_env->stream_config.source.port.timeout = timeout;
+		if (mipi_freq)
+			stream_env->stream_config.source.port.rxcount = rxcount;
+		stream_env->stream_config.
+		metadata_config.data_type = metadata_format;
+		stream_env->stream_config.
+		metadata_config.resolution.width = metadata_width;
+		stream_env->stream_config.
+		metadata_config.resolution.height = metadata_height;
+	}
+
+	return 0;
+}
+
+void atomisp_css_stop(struct atomisp_sub_device *asd,
+		      enum ia_css_pipe_id pipe_id, bool in_reset)
+{
+	struct atomisp_device *isp = asd->isp;
+	unsigned long irqflags;
+	unsigned int i;
+
+	/* if is called in atomisp_reset(), force destroy streams and pipes */
+	atomisp_destroy_pipes_stream_force(asd);
+
+	atomisp_init_raw_buffer_bitmap(asd);
+
+	/*
+	 * SP can not be stop if other streams are in use
+	 */
+	if (atomisp_streaming_count(isp) == 0)
+		ia_css_stop_sp();
+
+	if (!in_reset) {
+		struct atomisp_stream_env *stream_env;
+		int i, j;
+
+		for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+			stream_env = &asd->stream_env[i];
+			for (j = 0; j < IA_CSS_PIPE_ID_NUM; j++) {
+				ia_css_pipe_config_defaults(
+				    &stream_env->pipe_configs[j]);
+				ia_css_pipe_extra_config_defaults(
+				    &stream_env->pipe_extra_configs[j]);
+			}
+			ia_css_stream_config_defaults(
+			    &stream_env->stream_config);
+		}
+		memset(&asd->params.config, 0, sizeof(asd->params.config));
+		asd->params.css_update_params_needed = false;
+	}
+
+	/* move stats buffers to free queue list */
+	list_splice_init(&asd->s3a_stats_in_css, &asd->s3a_stats);
+	list_splice_init(&asd->s3a_stats_ready, &asd->s3a_stats);
+
+	spin_lock_irqsave(&asd->dis_stats_lock, irqflags);
+	list_splice_init(&asd->dis_stats_in_css, &asd->dis_stats);
+	asd->params.dis_proj_data_valid = false;
+	spin_unlock_irqrestore(&asd->dis_stats_lock, irqflags);
+
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		list_splice_init(&asd->metadata_in_css[i], &asd->metadata[i]);
+		list_splice_init(&asd->metadata_ready[i], &asd->metadata[i]);
+	}
+
+	atomisp_flush_params_queue(&asd->video_out_capture);
+	atomisp_flush_params_queue(&asd->video_out_vf);
+	atomisp_flush_params_queue(&asd->video_out_preview);
+	atomisp_flush_params_queue(&asd->video_out_video_capture);
+	atomisp_free_css_parameters(&asd->params.css_param);
+	memset(&asd->params.css_param, 0, sizeof(asd->params.css_param));
+}
+
+void atomisp_css_continuous_set_num_raw_frames(
+     struct atomisp_sub_device *asd,
+     int num_frames)
+{
+	if (asd->enable_raw_buffer_lock->val) {
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.stream_config.init_num_cont_raw_buf =
+		    ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES_LOCK_EN;
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+		    asd->params.video_dis_en)
+			asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+			.stream_config.init_num_cont_raw_buf +=
+			    ATOMISP_CSS2_NUM_DVS_FRAME_DELAY;
+	} else {
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.stream_config.init_num_cont_raw_buf =
+		    ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES;
+	}
+
+	if (asd->params.video_dis_en)
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.stream_config.init_num_cont_raw_buf +=
+		    ATOMISP_CSS2_NUM_DVS_FRAME_DELAY;
+
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.stream_config.target_num_cont_raw_buf = num_frames;
+}
+
+static enum ia_css_pipe_mode __pipe_id_to_pipe_mode(
+    struct atomisp_sub_device *asd,
+    enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct camera_mipi_info *mipi_info = atomisp_to_sensor_mipi_info(
+		isp->inputs[asd->input_curr].camera);
+
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_COPY:
+		/* Currently only YUVPP mode supports YUV420_Legacy format.
+		 * Revert this when other pipe modes can support
+		 * YUV420_Legacy format.
+		 */
+		if (mipi_info && mipi_info->input_format ==
+		    ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY)
+			return IA_CSS_PIPE_MODE_YUVPP;
+		return IA_CSS_PIPE_MODE_COPY;
+	case IA_CSS_PIPE_ID_PREVIEW:
+		return IA_CSS_PIPE_MODE_PREVIEW;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		return IA_CSS_PIPE_MODE_CAPTURE;
+	case IA_CSS_PIPE_ID_VIDEO:
+		return IA_CSS_PIPE_MODE_VIDEO;
+	case IA_CSS_PIPE_ID_ACC:
+		return IA_CSS_PIPE_MODE_ACC;
+	case IA_CSS_PIPE_ID_YUVPP:
+		return IA_CSS_PIPE_MODE_YUVPP;
+	default:
+		WARN_ON(1);
+		return IA_CSS_PIPE_MODE_PREVIEW;
+	}
+}
+
+static void __configure_output(struct atomisp_sub_device *asd,
+			       unsigned int stream_index,
+			       unsigned int width, unsigned int height,
+			       unsigned int min_width,
+			       enum ia_css_frame_format format,
+			       enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[stream_index];
+	struct ia_css_stream_config *s_config = &stream_env->stream_config;
+
+	stream_env->pipe_configs[pipe_id].mode =
+	    __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	stream_env->pipe_configs[pipe_id].output_info[0].res.width = width;
+	stream_env->pipe_configs[pipe_id].output_info[0].res.height = height;
+	stream_env->pipe_configs[pipe_id].output_info[0].format = format;
+	stream_env->pipe_configs[pipe_id].output_info[0].padded_width = min_width;
+
+	/* isp binary 2.2 specific setting*/
+	if (width > s_config->input_config.effective_res.width ||
+	    height > s_config->input_config.effective_res.height) {
+		s_config->input_config.effective_res.width = width;
+		s_config->input_config.effective_res.height = height;
+	}
+
+	dev_dbg(isp->dev, "configuring pipe[%d] output info w=%d.h=%d.f=%d.\n",
+		pipe_id, width, height, format);
+}
+
+static void __configure_video_preview_output(struct atomisp_sub_device *asd,
+	unsigned int stream_index,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format,
+	enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[stream_index];
+	struct ia_css_frame_info *css_output_info;
+	struct ia_css_stream_config *stream_config = &stream_env->stream_config;
+
+	stream_env->pipe_configs[pipe_id].mode =
+	    __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	/*
+	 * second_output will be as video main output in SDV mode
+	 * with SOC camera. output will be as video main output in
+	 * normal video mode.
+	 */
+	if (asd->continuous_mode->val)
+		css_output_info = &stream_env->pipe_configs[pipe_id].
+				  output_info[ATOMISP_CSS_OUTPUT_SECOND_INDEX];
+	else
+		css_output_info = &stream_env->pipe_configs[pipe_id].
+				  output_info[ATOMISP_CSS_OUTPUT_DEFAULT_INDEX];
+
+	css_output_info->res.width = width;
+	css_output_info->res.height = height;
+	css_output_info->format = format;
+	css_output_info->padded_width = min_width;
+
+	/* isp binary 2.2 specific setting*/
+	if (width > stream_config->input_config.effective_res.width ||
+	    height > stream_config->input_config.effective_res.height) {
+		stream_config->input_config.effective_res.width = width;
+		stream_config->input_config.effective_res.height = height;
+	}
+
+	dev_dbg(isp->dev, "configuring pipe[%d] output info w=%d.h=%d.f=%d.\n",
+		pipe_id, width, height, format);
+}
+
+/*
+ * For CSS2.1, capture pipe uses capture_pp_in_res to configure yuv
+ * downscaling input resolution.
+ */
+static void __configure_capture_pp_input(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct ia_css_stream_config *stream_config = &stream_env->stream_config;
+	struct ia_css_pipe_config *pipe_configs =
+		    &stream_env->pipe_configs[pipe_id];
+	struct ia_css_pipe_extra_config *pipe_extra_configs =
+		    &stream_env->pipe_extra_configs[pipe_id];
+	unsigned int hor_ds_factor = 0, ver_ds_factor = 0;
+
+	if (width == 0 && height == 0)
+		return;
+
+	if (width * 9 / 10 < pipe_configs->output_info[0].res.width ||
+	    height * 9 / 10 < pipe_configs->output_info[0].res.height)
+		return;
+	/* here just copy the calculation in css */
+	hor_ds_factor = CEIL_DIV(width >> 1,
+				 pipe_configs->output_info[0].res.width);
+	ver_ds_factor = CEIL_DIV(height >> 1,
+				 pipe_configs->output_info[0].res.height);
+
+	if ((asd->isp->media_dev.hw_revision <
+	     (ATOMISP_HW_REVISION_ISP2401 << ATOMISP_HW_REVISION_SHIFT) ||
+	     IS_CHT) && hor_ds_factor != ver_ds_factor) {
+		dev_warn(asd->isp->dev,
+			 "Cropping for capture due to FW limitation");
+		return;
+	}
+
+	pipe_configs->mode = __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	pipe_extra_configs->enable_yuv_ds = true;
+
+	pipe_configs->capt_pp_in_res.width =
+	    stream_config->input_config.effective_res.width;
+	pipe_configs->capt_pp_in_res.height =
+	    stream_config->input_config.effective_res.height;
+
+	dev_dbg(isp->dev, "configuring pipe[%d]capture pp input w=%d.h=%d.\n",
+		pipe_id, width, height);
+}
+
+/*
+ * For CSS2.1, preview pipe could support bayer downscaling, yuv decimation and
+ * yuv downscaling, which needs addtional configurations.
+ */
+static void __configure_preview_pp_input(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	int out_width, out_height, yuv_ds_in_width, yuv_ds_in_height;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct ia_css_stream_config *stream_config = &stream_env->stream_config;
+	struct ia_css_pipe_config *pipe_configs =
+		    &stream_env->pipe_configs[pipe_id];
+	struct ia_css_pipe_extra_config *pipe_extra_configs =
+		    &stream_env->pipe_extra_configs[pipe_id];
+	struct ia_css_resolution *bayer_ds_out_res =
+		    &pipe_configs->bayer_ds_out_res;
+	struct ia_css_resolution *vf_pp_in_res =
+		    &pipe_configs->vf_pp_in_res;
+	struct ia_css_resolution  *effective_res =
+		    &stream_config->input_config.effective_res;
+
+	static const struct bayer_ds_factor bds_fct[] = {{2, 1}, {3, 2}, {5, 4} };
+	/*
+	 * BZ201033: YUV decimation factor of 4 causes couple of rightmost
+	 * columns to be shaded. Remove this factor to work around the CSS bug.
+	 * const unsigned int yuv_dec_fct[] = {4, 2};
+	 */
+	static const unsigned int yuv_dec_fct[] = { 2 };
+	unsigned int i;
+
+	if (width == 0 && height == 0)
+		return;
+
+	pipe_configs->mode = __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	out_width = pipe_configs->output_info[0].res.width;
+	out_height = pipe_configs->output_info[0].res.height;
+
+	/*
+	 * The ISP could do bayer downscaling, yuv decimation and yuv
+	 * downscaling:
+	 * 1: Bayer Downscaling: between effective resolution and
+	 * bayer_ds_res_out;
+	 * 2: YUV Decimation: between bayer_ds_res_out and vf_pp_in_res;
+	 * 3: YUV Downscaling: between vf_pp_in_res and final vf output
+	 *
+	 * Rule for Bayer Downscaling: support factor 2, 1.5 and 1.25
+	 * Rule for YUV Decimation: support factor 2, 4
+	 * Rule for YUV Downscaling: arbitrary value below 2
+	 *
+	 * General rule of factor distribution among these stages:
+	 * 1: try to do Bayer downscaling first if not in online mode.
+	 * 2: try to do maximum of 2 for YUV downscaling
+	 * 3: the remainling for YUV decimation
+	 *
+	 * Note:
+	 * Do not configure bayer_ds_out_res if:
+	 * online == 1 or continuous == 0 or raw_binning = 0
+	 */
+	if (stream_config->online || !stream_config->continuous ||
+	    !pipe_extra_configs->enable_raw_binning) {
+		bayer_ds_out_res->width = 0;
+		bayer_ds_out_res->height = 0;
+	} else {
+		bayer_ds_out_res->width = effective_res->width;
+		bayer_ds_out_res->height = effective_res->height;
+
+		for (i = 0; i < ARRAY_SIZE(bds_fct); i++) {
+			if (effective_res->width >= out_width *
+			    bds_fct[i].numerator / bds_fct[i].denominator &&
+			    effective_res->height >= out_height *
+			    bds_fct[i].numerator / bds_fct[i].denominator) {
+				bayer_ds_out_res->width =
+				    effective_res->width *
+				    bds_fct[i].denominator /
+				    bds_fct[i].numerator;
+				bayer_ds_out_res->height =
+				    effective_res->height *
+				    bds_fct[i].denominator /
+				    bds_fct[i].numerator;
+				break;
+			}
+		}
+	}
+	/*
+	 * calculate YUV Decimation, YUV downscaling facor:
+	 * YUV Downscaling factor must not exceed 2.
+	 * YUV Decimation factor could be 2, 4.
+	 */
+	/* first decide the yuv_ds input resolution */
+	if (bayer_ds_out_res->width == 0) {
+		yuv_ds_in_width = effective_res->width;
+		yuv_ds_in_height = effective_res->height;
+	} else {
+		yuv_ds_in_width = bayer_ds_out_res->width;
+		yuv_ds_in_height = bayer_ds_out_res->height;
+	}
+
+	vf_pp_in_res->width = yuv_ds_in_width;
+	vf_pp_in_res->height = yuv_ds_in_height;
+
+	/* find out the yuv decimation factor */
+	for (i = 0; i < ARRAY_SIZE(yuv_dec_fct); i++) {
+		if (yuv_ds_in_width >= out_width * yuv_dec_fct[i] &&
+		    yuv_ds_in_height >= out_height * yuv_dec_fct[i]) {
+			vf_pp_in_res->width = yuv_ds_in_width / yuv_dec_fct[i];
+			vf_pp_in_res->height = yuv_ds_in_height / yuv_dec_fct[i];
+			break;
+		}
+	}
+
+	if (vf_pp_in_res->width == out_width &&
+	    vf_pp_in_res->height == out_height) {
+		pipe_extra_configs->enable_yuv_ds = false;
+		vf_pp_in_res->width = 0;
+		vf_pp_in_res->height = 0;
+	} else {
+		pipe_extra_configs->enable_yuv_ds = true;
+	}
+
+	dev_dbg(isp->dev, "configuring pipe[%d]preview pp input w=%d.h=%d.\n",
+		pipe_id, width, height);
+}
+
+/*
+ * For CSS2.1, offline video pipe could support bayer decimation, and
+ * yuv downscaling, which needs addtional configurations.
+ */
+static void __configure_video_pp_input(struct atomisp_sub_device *asd,
+				       unsigned int width, unsigned int height,
+				       enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	int out_width, out_height;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct ia_css_stream_config *stream_config = &stream_env->stream_config;
+	struct ia_css_pipe_config *pipe_configs =
+		    &stream_env->pipe_configs[pipe_id];
+	struct ia_css_pipe_extra_config *pipe_extra_configs =
+		    &stream_env->pipe_extra_configs[pipe_id];
+	struct ia_css_resolution *bayer_ds_out_res =
+		    &pipe_configs->bayer_ds_out_res;
+	struct ia_css_resolution  *effective_res =
+		    &stream_config->input_config.effective_res;
+
+	static const struct bayer_ds_factor bds_factors[] = {
+		{8, 1}, {6, 1}, {4, 1}, {3, 1}, {2, 1}, {3, 2}
+	};
+	unsigned int i;
+
+	if (width == 0 && height == 0)
+		return;
+
+	pipe_configs->mode = __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	pipe_extra_configs->enable_yuv_ds = false;
+
+	/*
+	 * If DVS is enabled,  video binary will take care the dvs envelope
+	 * and usually the bayer_ds_out_res should be larger than 120% of
+	 * destination resolution, the extra 20% will be cropped as DVS
+	 * envelope. But,  if the bayer_ds_out_res is less than 120% of the
+	 * destination. The ISP can still work,  but DVS quality is not good.
+	 */
+	/* taking at least 10% as envelope */
+	if (asd->params.video_dis_en) {
+		out_width = pipe_configs->output_info[0].res.width * 110 / 100;
+		out_height = pipe_configs->output_info[0].res.height * 110 / 100;
+	} else {
+		out_width = pipe_configs->output_info[0].res.width;
+		out_height = pipe_configs->output_info[0].res.height;
+	}
+
+	/*
+	 * calculate bayer decimate factor:
+	 * 1: only 1.5, 2, 4 and 8 get supported
+	 * 2: Do not configure bayer_ds_out_res if:
+	 *    online == 1 or continuous == 0 or raw_binning = 0
+	 */
+	if (stream_config->online || !stream_config->continuous) {
+		bayer_ds_out_res->width = 0;
+		bayer_ds_out_res->height = 0;
+		goto done;
+	}
+
+	pipe_extra_configs->enable_raw_binning = true;
+	bayer_ds_out_res->width = effective_res->width;
+	bayer_ds_out_res->height = effective_res->height;
+
+	for (i = 0; i < sizeof(bds_factors) / sizeof(struct bayer_ds_factor);
+	     i++) {
+		if (effective_res->width >= out_width *
+		    bds_factors[i].numerator / bds_factors[i].denominator &&
+		    effective_res->height >= out_height *
+		    bds_factors[i].numerator / bds_factors[i].denominator) {
+			bayer_ds_out_res->width = effective_res->width *
+						  bds_factors[i].denominator /
+						  bds_factors[i].numerator;
+			bayer_ds_out_res->height = effective_res->height *
+						   bds_factors[i].denominator /
+						   bds_factors[i].numerator;
+			break;
+		}
+	}
+
+	/*
+	 * DVS is cropped from BDS output, so we do not really need to set the
+	 * envelope to 20% of output resolution here. always set it to 12x12
+	 * per firmware requirement.
+	 */
+	pipe_configs->dvs_envelope.width = 12;
+	pipe_configs->dvs_envelope.height = 12;
+
+done:
+	if (pipe_id == IA_CSS_PIPE_ID_YUVPP)
+		stream_config->left_padding = -1;
+	else
+		stream_config->left_padding = 12;
+	dev_dbg(isp->dev, "configuring pipe[%d]video pp input w=%d.h=%d.\n",
+		pipe_id, width, height);
+}
+
+static void __configure_vf_output(struct atomisp_sub_device *asd,
+				  unsigned int width, unsigned int height,
+				  unsigned int min_width,
+				  enum ia_css_frame_format format,
+				  enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	stream_env->pipe_configs[pipe_id].mode =
+	    __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].res.width = width;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].res.height = height;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].format = format;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].padded_width =
+	    min_width;
+	dev_dbg(isp->dev,
+		"configuring pipe[%d] vf output info w=%d.h=%d.f=%d.\n",
+		pipe_id, width, height, format);
+}
+
+static void __configure_video_vf_output(struct atomisp_sub_device *asd,
+					unsigned int width, unsigned int height,
+					unsigned int min_width,
+					enum ia_css_frame_format format,
+					enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct ia_css_frame_info *css_output_info;
+
+	stream_env->pipe_configs[pipe_id].mode =
+	    __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	/*
+	 * second_vf_output will be as video viewfinder in SDV mode
+	 * with SOC camera. vf_output will be as video viewfinder in
+	 * normal video mode.
+	 */
+	if (asd->continuous_mode->val)
+		css_output_info = &stream_env->pipe_configs[pipe_id].
+				  vf_output_info[ATOMISP_CSS_OUTPUT_SECOND_INDEX];
+	else
+		css_output_info = &stream_env->pipe_configs[pipe_id].
+				  vf_output_info[ATOMISP_CSS_OUTPUT_DEFAULT_INDEX];
+
+	css_output_info->res.width = width;
+	css_output_info->res.height = height;
+	css_output_info->format = format;
+	css_output_info->padded_width = min_width;
+	dev_dbg(isp->dev,
+		"configuring pipe[%d] vf output info w=%d.h=%d.f=%d.\n",
+		pipe_id, width, height, format);
+}
+
+static int __get_frame_info(struct atomisp_sub_device *asd,
+			    unsigned int stream_index,
+			    struct ia_css_frame_info *info,
+			    enum frame_info_type type,
+			    enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	int ret;
+	struct ia_css_pipe_info p_info;
+
+	/* FIXME! No need to destroy/recreate all streams */
+	ret = atomisp_css_update_stream(asd);
+	if (ret)
+		return ret;
+
+	ret = ia_css_pipe_get_info(asd->stream_env[stream_index].pipes[pipe_id],
+				   &p_info);
+	if (ret) {
+		dev_err(isp->dev, "can't get info from pipe\n");
+		goto get_info_err;
+	}
+
+	switch (type) {
+	case ATOMISP_CSS_VF_FRAME:
+		*info = p_info.vf_output_info[0];
+		dev_dbg(isp->dev, "getting vf frame info.\n");
+		break;
+	case ATOMISP_CSS_SECOND_VF_FRAME:
+		*info = p_info.vf_output_info[1];
+		dev_dbg(isp->dev, "getting second vf frame info.\n");
+		break;
+	case ATOMISP_CSS_OUTPUT_FRAME:
+		*info = p_info.output_info[0];
+		dev_dbg(isp->dev, "getting main frame info.\n");
+		break;
+	case ATOMISP_CSS_SECOND_OUTPUT_FRAME:
+		*info = p_info.output_info[1];
+		dev_dbg(isp->dev, "getting second main frame info.\n");
+		break;
+	default:
+	case ATOMISP_CSS_RAW_FRAME:
+		*info = p_info.raw_output_info;
+		dev_dbg(isp->dev, "getting raw frame info.\n");
+		break;
+	}
+	dev_dbg(isp->dev, "get frame info: w=%d, h=%d, num_invalid_frames %d.\n",
+		info->res.width, info->res.height, p_info.num_invalid_frames);
+
+	return 0;
+
+get_info_err:
+	atomisp_destroy_pipes_stream_force(asd);
+	return -EINVAL;
+}
+
+static unsigned int atomisp_get_pipe_index(struct atomisp_sub_device *asd,
+	uint16_t source_pad)
+{
+	struct atomisp_device *isp = asd->isp;
+	/*
+	 * to SOC camera, use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		return IA_CSS_PIPE_ID_YUVPP;
+
+	switch (source_pad) {
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO:
+		if (asd->yuvpp_mode)
+			return IA_CSS_PIPE_ID_YUVPP;
+		if (asd->copy_mode)
+			return IA_CSS_PIPE_ID_COPY;
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO
+		    || asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER)
+			return IA_CSS_PIPE_ID_VIDEO;
+
+		return IA_CSS_PIPE_ID_CAPTURE;
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
+		if (asd->copy_mode)
+			return IA_CSS_PIPE_ID_COPY;
+
+		return IA_CSS_PIPE_ID_CAPTURE;
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF:
+		if (!atomisp_is_mbuscode_raw(asd->fmt[asd->capture_pad].fmt.code)) {
+			return IA_CSS_PIPE_ID_CAPTURE;
+		}
+		fallthrough;
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
+		if (asd->yuvpp_mode)
+			return IA_CSS_PIPE_ID_YUVPP;
+		if (asd->copy_mode)
+			return IA_CSS_PIPE_ID_COPY;
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			return IA_CSS_PIPE_ID_VIDEO;
+
+		return IA_CSS_PIPE_ID_PREVIEW;
+	}
+	dev_warn(isp->dev,
+		 "invalid source pad:%d, return default preview pipe index.\n",
+		 source_pad);
+	return IA_CSS_PIPE_ID_PREVIEW;
+}
+
+int atomisp_get_css_frame_info(struct atomisp_sub_device *asd,
+			       u16 source_pad,
+			       struct ia_css_frame_info *frame_info)
+{
+	struct ia_css_pipe_info info;
+	int pipe_index = atomisp_get_pipe_index(asd, source_pad);
+	int stream_index;
+	struct atomisp_device *isp = asd->isp;
+
+	if (ATOMISP_SOC_CAMERA(asd)) {
+		stream_index = ATOMISP_INPUT_STREAM_GENERAL;
+	} else {
+		stream_index = (pipe_index == IA_CSS_PIPE_ID_YUVPP) ?
+			       ATOMISP_INPUT_STREAM_VIDEO :
+			       ATOMISP_INPUT_STREAM_GENERAL;
+	}
+
+	if (0 != ia_css_pipe_get_info(asd->stream_env[stream_index]
+		.pipes[pipe_index], &info)) {
+		dev_err(isp->dev, "ia_css_pipe_get_info FAILED");
+		return -EINVAL;
+	}
+
+	switch (source_pad) {
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
+		*frame_info = info.output_info[0];
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO:
+		if (ATOMISP_USE_YUVPP(asd) && asd->continuous_mode->val)
+			*frame_info = info.
+				      output_info[ATOMISP_CSS_OUTPUT_SECOND_INDEX];
+		else
+			*frame_info = info.
+				      output_info[ATOMISP_CSS_OUTPUT_DEFAULT_INDEX];
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF:
+		if (stream_index == ATOMISP_INPUT_STREAM_POSTVIEW)
+			*frame_info = info.output_info[0];
+		else
+			*frame_info = info.vf_output_info[0];
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+		    (pipe_index == IA_CSS_PIPE_ID_VIDEO ||
+		     pipe_index == IA_CSS_PIPE_ID_YUVPP))
+			if (ATOMISP_USE_YUVPP(asd) && asd->continuous_mode->val)
+				*frame_info = info.
+					      vf_output_info[ATOMISP_CSS_OUTPUT_SECOND_INDEX];
+			else
+				*frame_info = info.
+					      vf_output_info[ATOMISP_CSS_OUTPUT_DEFAULT_INDEX];
+		else if (ATOMISP_USE_YUVPP(asd) && asd->continuous_mode->val)
+			*frame_info =
+			    info.output_info[ATOMISP_CSS_OUTPUT_SECOND_INDEX];
+		else
+			*frame_info =
+			    info.output_info[ATOMISP_CSS_OUTPUT_DEFAULT_INDEX];
+
+		break;
+	default:
+		frame_info = NULL;
+		break;
+	}
+	return frame_info ? 0 : -EINVAL;
+}
+
+int atomisp_css_copy_configure_output(struct atomisp_sub_device *asd,
+				      unsigned int stream_index,
+				      unsigned int width, unsigned int height,
+				      unsigned int padded_width,
+				      enum ia_css_frame_format format)
+{
+	asd->stream_env[stream_index].pipe_configs[IA_CSS_PIPE_ID_COPY].
+	default_capture_config.mode =
+	    IA_CSS_CAPTURE_MODE_RAW;
+
+	__configure_output(asd, stream_index, width, height, padded_width,
+			   format, IA_CSS_PIPE_ID_COPY);
+	return 0;
+}
+
+int atomisp_css_yuvpp_configure_output(struct atomisp_sub_device *asd,
+				       unsigned int stream_index,
+				       unsigned int width, unsigned int height,
+				       unsigned int padded_width,
+				       enum ia_css_frame_format format)
+{
+	asd->stream_env[stream_index].pipe_configs[IA_CSS_PIPE_ID_YUVPP].
+	default_capture_config.mode =
+	    IA_CSS_CAPTURE_MODE_RAW;
+
+	__configure_output(asd, stream_index, width, height, padded_width,
+			   format, IA_CSS_PIPE_ID_YUVPP);
+	return 0;
+}
+
+int atomisp_css_yuvpp_configure_viewfinder(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    unsigned int width, unsigned int height,
+    unsigned int min_width,
+    enum ia_css_frame_format format)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[stream_index];
+	enum ia_css_pipe_id pipe_id = IA_CSS_PIPE_ID_YUVPP;
+
+	stream_env->pipe_configs[pipe_id].mode =
+	    __pipe_id_to_pipe_mode(asd, pipe_id);
+	stream_env->update_pipe[pipe_id] = true;
+
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].res.width = width;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].res.height = height;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].format = format;
+	stream_env->pipe_configs[pipe_id].vf_output_info[0].padded_width =
+	    min_width;
+	return 0;
+}
+
+int atomisp_css_yuvpp_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info)
+{
+	return __get_frame_info(asd, stream_index, info,
+				ATOMISP_CSS_OUTPUT_FRAME, IA_CSS_PIPE_ID_YUVPP);
+}
+
+int atomisp_css_yuvpp_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info)
+{
+	return __get_frame_info(asd, stream_index, info,
+				ATOMISP_CSS_VF_FRAME, IA_CSS_PIPE_ID_YUVPP);
+}
+
+int atomisp_css_preview_configure_output(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format)
+{
+	/*
+	 * to SOC camera, use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		__configure_video_preview_output(asd, ATOMISP_INPUT_STREAM_GENERAL, width,
+						 height,
+						 min_width, format, IA_CSS_PIPE_ID_YUVPP);
+	else
+		__configure_output(asd, ATOMISP_INPUT_STREAM_GENERAL, width, height,
+				   min_width, format, IA_CSS_PIPE_ID_PREVIEW);
+	return 0;
+}
+
+int atomisp_css_capture_configure_output(struct atomisp_sub_device *asd,
+	unsigned int width, unsigned int height,
+	unsigned int min_width,
+	enum ia_css_frame_format format)
+{
+	enum ia_css_pipe_id pipe_id;
+
+	/*
+	 * to SOC camera, use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+	else
+		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+	__configure_output(asd, ATOMISP_INPUT_STREAM_GENERAL, width, height,
+			   min_width, format, pipe_id);
+	return 0;
+}
+
+int atomisp_css_video_configure_output(struct atomisp_sub_device *asd,
+				       unsigned int width, unsigned int height,
+				       unsigned int min_width,
+				       enum ia_css_frame_format format)
+{
+	/*
+	 * to SOC camera, use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		__configure_video_preview_output(asd, ATOMISP_INPUT_STREAM_GENERAL, width,
+						 height,
+						 min_width, format, IA_CSS_PIPE_ID_YUVPP);
+	else
+		__configure_output(asd, ATOMISP_INPUT_STREAM_GENERAL, width, height,
+				   min_width, format, IA_CSS_PIPE_ID_VIDEO);
+	return 0;
+}
+
+int atomisp_css_video_configure_viewfinder(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height,
+    unsigned int min_width,
+    enum ia_css_frame_format format)
+{
+	/*
+	 * to SOC camera, video will use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		__configure_video_vf_output(asd, width, height, min_width, format,
+					    IA_CSS_PIPE_ID_YUVPP);
+	else
+		__configure_vf_output(asd, width, height, min_width, format,
+				      IA_CSS_PIPE_ID_VIDEO);
+	return 0;
+}
+
+int atomisp_css_capture_configure_viewfinder(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height,
+    unsigned int min_width,
+    enum ia_css_frame_format format)
+{
+	enum ia_css_pipe_id pipe_id;
+
+	/*
+	 * to SOC camera, video will use yuvpp pipe.
+	 */
+	if (ATOMISP_USE_YUVPP(asd))
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+	else
+		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+	__configure_vf_output(asd, width, height, min_width, format,
+			      pipe_id);
+	return 0;
+}
+
+int atomisp_css_video_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	enum ia_css_pipe_id pipe_id;
+	enum frame_info_type frame_type = ATOMISP_CSS_VF_FRAME;
+
+	if (ATOMISP_USE_YUVPP(asd)) {
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+		if (asd->continuous_mode->val)
+			frame_type = ATOMISP_CSS_SECOND_VF_FRAME;
+	} else {
+		pipe_id = IA_CSS_PIPE_ID_VIDEO;
+	}
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				frame_type, pipe_id);
+}
+
+int atomisp_css_capture_get_viewfinder_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	enum ia_css_pipe_id pipe_id;
+
+	if (ATOMISP_USE_YUVPP(asd))
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+	else
+		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				ATOMISP_CSS_VF_FRAME, pipe_id);
+}
+
+int atomisp_css_capture_get_output_raw_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	if (ATOMISP_USE_YUVPP(asd))
+		return 0;
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				ATOMISP_CSS_RAW_FRAME, IA_CSS_PIPE_ID_CAPTURE);
+}
+
+int atomisp_css_copy_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    unsigned int stream_index,
+    struct ia_css_frame_info *info)
+{
+	return __get_frame_info(asd, stream_index, info,
+				ATOMISP_CSS_OUTPUT_FRAME, IA_CSS_PIPE_ID_COPY);
+}
+
+int atomisp_css_preview_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	enum ia_css_pipe_id pipe_id;
+	enum frame_info_type frame_type = ATOMISP_CSS_OUTPUT_FRAME;
+
+	if (ATOMISP_USE_YUVPP(asd)) {
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+		if (asd->continuous_mode->val)
+			frame_type = ATOMISP_CSS_SECOND_OUTPUT_FRAME;
+	} else {
+		pipe_id = IA_CSS_PIPE_ID_PREVIEW;
+	}
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				frame_type, pipe_id);
+}
+
+int atomisp_css_capture_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	enum ia_css_pipe_id pipe_id;
+
+	if (ATOMISP_USE_YUVPP(asd))
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+	else
+		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				ATOMISP_CSS_OUTPUT_FRAME, pipe_id);
+}
+
+int atomisp_css_video_get_output_frame_info(
+    struct atomisp_sub_device *asd,
+    struct ia_css_frame_info *info)
+{
+	enum ia_css_pipe_id pipe_id;
+	enum frame_info_type frame_type = ATOMISP_CSS_OUTPUT_FRAME;
+
+	if (ATOMISP_USE_YUVPP(asd)) {
+		pipe_id = IA_CSS_PIPE_ID_YUVPP;
+		if (asd->continuous_mode->val)
+			frame_type = ATOMISP_CSS_SECOND_OUTPUT_FRAME;
+	} else {
+		pipe_id = IA_CSS_PIPE_ID_VIDEO;
+	}
+
+	return __get_frame_info(asd, ATOMISP_INPUT_STREAM_GENERAL, info,
+				frame_type, pipe_id);
+}
+
+int atomisp_css_preview_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	__configure_preview_pp_input(asd, width, height,
+				     ATOMISP_USE_YUVPP(asd) ?
+				     IA_CSS_PIPE_ID_YUVPP : IA_CSS_PIPE_ID_PREVIEW);
+
+	if (width > stream_env->pipe_configs[IA_CSS_PIPE_ID_CAPTURE].
+	    capt_pp_in_res.width)
+		__configure_capture_pp_input(asd, width, height,
+					     ATOMISP_USE_YUVPP(asd) ?
+					     IA_CSS_PIPE_ID_YUVPP : IA_CSS_PIPE_ID_CAPTURE);
+	return 0;
+}
+
+int atomisp_css_capture_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height)
+{
+	__configure_capture_pp_input(asd, width, height,
+				     ATOMISP_USE_YUVPP(asd) ?
+				     IA_CSS_PIPE_ID_YUVPP : IA_CSS_PIPE_ID_CAPTURE);
+	return 0;
+}
+
+int atomisp_css_video_configure_pp_input(
+    struct atomisp_sub_device *asd,
+    unsigned int width, unsigned int height)
+{
+	struct atomisp_stream_env *stream_env =
+		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+
+	__configure_video_pp_input(asd, width, height,
+				   ATOMISP_USE_YUVPP(asd) ?
+				   IA_CSS_PIPE_ID_YUVPP : IA_CSS_PIPE_ID_VIDEO);
+
+	if (width > stream_env->pipe_configs[IA_CSS_PIPE_ID_CAPTURE].
+	    capt_pp_in_res.width)
+		__configure_capture_pp_input(asd, width, height,
+					     ATOMISP_USE_YUVPP(asd) ?
+					     IA_CSS_PIPE_ID_YUVPP : IA_CSS_PIPE_ID_CAPTURE);
+	return 0;
+}
+
+int atomisp_css_offline_capture_configure(struct atomisp_sub_device *asd,
+	int num_captures, unsigned int skip, int offset)
+{
+	int ret;
+
+	dev_dbg(asd->isp->dev, "%s num_capture:%d skip:%d offset:%d\n",
+		__func__, num_captures, skip, offset);
+
+	ret = ia_css_stream_capture(
+		  asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		  num_captures, skip, offset);
+	if (ret)
+		return -EINVAL;
+
+	return 0;
+}
+
+int atomisp_css_exp_id_capture(struct atomisp_sub_device *asd, int exp_id)
+{
+	int ret;
+
+	ret = ia_css_stream_capture_frame(
+		  asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		  exp_id);
+	if (ret == -ENOBUFS) {
+		/* capture cmd queue is full */
+		return -EBUSY;
+	} else if (ret) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int atomisp_css_exp_id_unlock(struct atomisp_sub_device *asd, int exp_id)
+{
+	int ret;
+
+	ret = ia_css_unlock_raw_frame(
+		  asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		  exp_id);
+	if (ret == -ENOBUFS)
+		return -EAGAIN;
+	else if (ret)
+		return -EIO;
+
+	return 0;
+}
+
+int atomisp_css_capture_enable_xnr(struct atomisp_sub_device *asd,
+				   bool enable)
+{
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.pipe_configs[IA_CSS_PIPE_ID_CAPTURE]
+	.default_capture_config.enable_xnr = enable;
+	asd->params.capture_config.enable_xnr = enable;
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+	.update_pipe[IA_CSS_PIPE_ID_CAPTURE] = true;
+
+	return 0;
+}
+
+void atomisp_css_set_ctc_table(struct atomisp_sub_device *asd,
+			       struct ia_css_ctc_table *ctc_table)
+{
+	int i;
+	u16 *vamem_ptr = ctc_table->data.vamem_1;
+	int data_size = IA_CSS_VAMEM_1_CTC_TABLE_SIZE;
+	bool valid = false;
+
+	/* workaround: if ctc_table is all 0, do not apply it */
+	if (ctc_table->vamem_type == IA_CSS_VAMEM_TYPE_2) {
+		vamem_ptr = ctc_table->data.vamem_2;
+		data_size = IA_CSS_VAMEM_2_CTC_TABLE_SIZE;
+	}
+
+	for (i = 0; i < data_size; i++) {
+		if (*(vamem_ptr + i)) {
+			valid = true;
+			break;
+		}
+	}
+
+	if (valid)
+		asd->params.config.ctc_table = ctc_table;
+	else
+		dev_warn(asd->isp->dev, "Bypass the invalid ctc_table.\n");
+}
+
+void atomisp_css_set_anr_thres(struct atomisp_sub_device *asd,
+			       struct ia_css_anr_thres *anr_thres)
+{
+	asd->params.config.anr_thres = anr_thres;
+}
+
+void atomisp_css_set_dvs_6axis(struct atomisp_sub_device *asd,
+			       struct ia_css_dvs_6axis_config *dvs_6axis)
+{
+	asd->params.config.dvs_6axis_config = dvs_6axis;
+}
+
+void atomisp_css_video_set_dis_vector(struct atomisp_sub_device *asd,
+				      struct atomisp_dis_vector *vector)
+{
+	if (!asd->params.config.motion_vector)
+		asd->params.config.motion_vector = &asd->params.css_param.motion_vector;
+
+	memset(asd->params.config.motion_vector,
+	       0, sizeof(struct ia_css_vector));
+	asd->params.css_param.motion_vector.x = vector->x;
+	asd->params.css_param.motion_vector.y = vector->y;
+}
+
+static int atomisp_compare_dvs_grid(struct atomisp_sub_device *asd,
+				    struct atomisp_dvs_grid_info *atomgrid)
+{
+	struct ia_css_dvs_grid_info *cur =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+
+	if (!cur) {
+		dev_err(asd->isp->dev, "dvs grid not available!\n");
+		return -EINVAL;
+	}
+
+	if (sizeof(*cur) != sizeof(*atomgrid)) {
+		dev_err(asd->isp->dev, "dvs grid mis-match!\n");
+		return -EINVAL;
+	}
+
+	if (!cur->enable) {
+		dev_err(asd->isp->dev, "dvs not enabled!\n");
+		return -EINVAL;
+	}
+
+	return memcmp(atomgrid, cur, sizeof(*cur));
+}
+
+void  atomisp_css_set_dvs2_coefs(struct atomisp_sub_device *asd,
+				 struct ia_css_dvs2_coefficients *coefs)
+{
+	asd->params.config.dvs2_coefs = coefs;
+}
+
+int atomisp_css_set_dis_coefs(struct atomisp_sub_device *asd,
+			      struct atomisp_dis_coefficients *coefs)
+{
+	if (atomisp_compare_dvs_grid(asd, &coefs->grid_info) != 0)
+		/* If the grid info in the argument differs from the current
+		   grid info, we tell the caller to reset the grid size and
+		   try again. */
+		return -EAGAIN;
+
+	if (!coefs->hor_coefs.odd_real ||
+	    !coefs->hor_coefs.odd_imag ||
+	    !coefs->hor_coefs.even_real ||
+	    !coefs->hor_coefs.even_imag ||
+	    !coefs->ver_coefs.odd_real ||
+	    !coefs->ver_coefs.odd_imag ||
+	    !coefs->ver_coefs.even_real ||
+	    !coefs->ver_coefs.even_imag ||
+	    !asd->params.css_param.dvs2_coeff->hor_coefs.odd_real ||
+	    !asd->params.css_param.dvs2_coeff->hor_coefs.odd_imag ||
+	    !asd->params.css_param.dvs2_coeff->hor_coefs.even_real ||
+	    !asd->params.css_param.dvs2_coeff->hor_coefs.even_imag ||
+	    !asd->params.css_param.dvs2_coeff->ver_coefs.odd_real ||
+	    !asd->params.css_param.dvs2_coeff->ver_coefs.odd_imag ||
+	    !asd->params.css_param.dvs2_coeff->ver_coefs.even_real ||
+	    !asd->params.css_param.dvs2_coeff->ver_coefs.even_imag)
+		return -EINVAL;
+
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->hor_coefs.odd_real,
+			   coefs->hor_coefs.odd_real, asd->params.dvs_hor_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->hor_coefs.odd_imag,
+			   coefs->hor_coefs.odd_imag, asd->params.dvs_hor_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->hor_coefs.even_real,
+			   coefs->hor_coefs.even_real, asd->params.dvs_hor_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->hor_coefs.even_imag,
+			   coefs->hor_coefs.even_imag, asd->params.dvs_hor_coef_bytes))
+		return -EFAULT;
+
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->ver_coefs.odd_real,
+			   coefs->ver_coefs.odd_real, asd->params.dvs_ver_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->ver_coefs.odd_imag,
+			   coefs->ver_coefs.odd_imag, asd->params.dvs_ver_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->ver_coefs.even_real,
+			   coefs->ver_coefs.even_real, asd->params.dvs_ver_coef_bytes))
+		return -EFAULT;
+	if (copy_from_user(asd->params.css_param.dvs2_coeff->ver_coefs.even_imag,
+			   coefs->ver_coefs.even_imag, asd->params.dvs_ver_coef_bytes))
+		return -EFAULT;
+
+	asd->params.css_param.update_flag.dvs2_coefs =
+		(struct atomisp_dis_coefficients *)
+		asd->params.css_param.dvs2_coeff;
+	/* FIXME! */
+	/*	asd->params.dis_proj_data_valid = false; */
+	asd->params.css_update_params_needed = true;
+
+	return 0;
+}
+
+void atomisp_css_set_zoom_factor(struct atomisp_sub_device *asd,
+				 unsigned int zoom)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (zoom == asd->params.css_param.dz_config.dx &&
+	    zoom == asd->params.css_param.dz_config.dy) {
+		dev_dbg(isp->dev, "same zoom scale. skipped.\n");
+		return;
+	}
+
+	memset(&asd->params.css_param.dz_config, 0,
+	       sizeof(struct ia_css_dz_config));
+	asd->params.css_param.dz_config.dx = zoom;
+	asd->params.css_param.dz_config.dy = zoom;
+
+	asd->params.css_param.update_flag.dz_config =
+	    (struct atomisp_dz_config *)&asd->params.css_param.dz_config;
+	asd->params.css_update_params_needed = true;
+}
+
+void atomisp_css_set_formats_config(struct atomisp_sub_device *asd,
+				    struct ia_css_formats_config *formats_config)
+{
+	asd->params.config.formats_config = formats_config;
+}
+
+int atomisp_css_get_wb_config(struct atomisp_sub_device *asd,
+			      struct atomisp_wb_config *config)
+{
+	struct ia_css_wb_config wb_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&wb_config, 0, sizeof(struct ia_css_wb_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.wb_config = &wb_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &wb_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_ob_config(struct atomisp_sub_device *asd,
+			      struct atomisp_ob_config *config)
+{
+	struct ia_css_ob_config ob_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&ob_config, 0, sizeof(struct ia_css_ob_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.ob_config = &ob_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &ob_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_dp_config(struct atomisp_sub_device *asd,
+			      struct atomisp_dp_config *config)
+{
+	struct ia_css_dp_config dp_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&dp_config, 0, sizeof(struct ia_css_dp_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.dp_config = &dp_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &dp_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_de_config(struct atomisp_sub_device *asd,
+			      struct atomisp_de_config *config)
+{
+	struct ia_css_de_config de_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&de_config, 0, sizeof(struct ia_css_de_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.de_config = &de_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &de_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_nr_config(struct atomisp_sub_device *asd,
+			      struct atomisp_nr_config *config)
+{
+	struct ia_css_nr_config nr_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&nr_config, 0, sizeof(struct ia_css_nr_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+
+	isp_config.nr_config = &nr_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &nr_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_ee_config(struct atomisp_sub_device *asd,
+			      struct atomisp_ee_config *config)
+{
+	struct ia_css_ee_config ee_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&ee_config, 0, sizeof(struct ia_css_ee_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.ee_config = &ee_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &ee_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_tnr_config(struct atomisp_sub_device *asd,
+			       struct atomisp_tnr_config *config)
+{
+	struct ia_css_tnr_config tnr_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&tnr_config, 0, sizeof(struct ia_css_tnr_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.tnr_config = &tnr_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, &tnr_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_ctc_table(struct atomisp_sub_device *asd,
+			      struct atomisp_ctc_table *config)
+{
+	struct ia_css_ctc_table *tab;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	tab = vzalloc(sizeof(struct ia_css_ctc_table));
+	if (!tab)
+		return -ENOMEM;
+
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.ctc_table = tab;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, tab, sizeof(*tab));
+	vfree(tab);
+
+	return 0;
+}
+
+int atomisp_css_get_gamma_table(struct atomisp_sub_device *asd,
+				struct atomisp_gamma_table *config)
+{
+	struct ia_css_gamma_table *tab;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	tab = vzalloc(sizeof(struct ia_css_gamma_table));
+	if (!tab)
+		return -ENOMEM;
+
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.gamma_table = tab;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	memcpy(config, tab, sizeof(*tab));
+	vfree(tab);
+
+	return 0;
+}
+
+int atomisp_css_get_gc_config(struct atomisp_sub_device *asd,
+			      struct atomisp_gc_config *config)
+{
+	struct ia_css_gc_config gc_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&gc_config, 0, sizeof(struct ia_css_gc_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.gc_config = &gc_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	/* Get gamma correction params from current setup */
+	memcpy(config, &gc_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_3a_config(struct atomisp_sub_device *asd,
+			      struct atomisp_3a_config *config)
+{
+	struct ia_css_3a_config s3a_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&s3a_config, 0, sizeof(struct ia_css_3a_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.s3a_config = &s3a_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	/* Get white balance from current setup */
+	memcpy(config, &s3a_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_formats_config(struct atomisp_sub_device *asd,
+				   struct atomisp_formats_config *config)
+{
+	struct ia_css_formats_config formats_config;
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&formats_config, 0, sizeof(formats_config));
+	memset(&isp_config, 0, sizeof(isp_config));
+	isp_config.formats_config = &formats_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	/* Get narrow gamma from current setup */
+	memcpy(config, &formats_config, sizeof(*config));
+
+	return 0;
+}
+
+int atomisp_css_get_zoom_factor(struct atomisp_sub_device *asd,
+				unsigned int *zoom)
+{
+	struct ia_css_dz_config dz_config;  /** Digital Zoom */
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev, "%s called after streamoff, skipping.\n",
+			__func__);
+		return -EINVAL;
+	}
+	memset(&dz_config, 0, sizeof(struct ia_css_dz_config));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.dz_config = &dz_config;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+	*zoom = dz_config.dx;
+
+	return 0;
+}
+
+/*
+ * Function to set/get image stablization statistics
+ */
+int atomisp_css_get_dis_stat(struct atomisp_sub_device *asd,
+			     struct atomisp_dis_statistics *stats)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_dis_buf *dis_buf;
+	unsigned long flags;
+
+	lockdep_assert_held(&isp->mutex);
+
+	if (!asd->params.dvs_stat->hor_prod.odd_real ||
+	    !asd->params.dvs_stat->hor_prod.odd_imag ||
+	    !asd->params.dvs_stat->hor_prod.even_real ||
+	    !asd->params.dvs_stat->hor_prod.even_imag ||
+	    !asd->params.dvs_stat->ver_prod.odd_real ||
+	    !asd->params.dvs_stat->ver_prod.odd_imag ||
+	    !asd->params.dvs_stat->ver_prod.even_real ||
+	    !asd->params.dvs_stat->ver_prod.even_imag)
+		return -EINVAL;
+
+	/* isp needs to be streaming to get DIS statistics */
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+		return -EINVAL;
+
+	if (atomisp_compare_dvs_grid(asd, &stats->dvs2_stat.grid_info) != 0)
+		/* If the grid info in the argument differs from the current
+		   grid info, we tell the caller to reset the grid size and
+		   try again. */
+		return -EAGAIN;
+
+	spin_lock_irqsave(&asd->dis_stats_lock, flags);
+	if (!asd->params.dis_proj_data_valid || list_empty(&asd->dis_stats)) {
+		spin_unlock_irqrestore(&asd->dis_stats_lock, flags);
+		dev_err(isp->dev, "dis statistics is not valid.\n");
+		return -EAGAIN;
+	}
+
+	dis_buf = list_entry(asd->dis_stats.next,
+			     struct atomisp_dis_buf, list);
+	list_del_init(&dis_buf->list);
+	spin_unlock_irqrestore(&asd->dis_stats_lock, flags);
+
+	if (dis_buf->dvs_map)
+		ia_css_translate_dvs2_statistics(
+		    asd->params.dvs_stat, dis_buf->dvs_map);
+	else
+		ia_css_get_dvs2_statistics(asd->params.dvs_stat,
+					   dis_buf->dis_data);
+	stats->exp_id = dis_buf->dis_data->exp_id;
+
+	spin_lock_irqsave(&asd->dis_stats_lock, flags);
+	list_add_tail(&dis_buf->list, &asd->dis_stats);
+	spin_unlock_irqrestore(&asd->dis_stats_lock, flags);
+
+	if (copy_to_user(stats->dvs2_stat.ver_prod.odd_real,
+			 asd->params.dvs_stat->ver_prod.odd_real,
+			 asd->params.dvs_ver_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.ver_prod.odd_imag,
+			 asd->params.dvs_stat->ver_prod.odd_imag,
+			 asd->params.dvs_ver_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.ver_prod.even_real,
+			 asd->params.dvs_stat->ver_prod.even_real,
+			 asd->params.dvs_ver_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.ver_prod.even_imag,
+			 asd->params.dvs_stat->ver_prod.even_imag,
+			 asd->params.dvs_ver_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.hor_prod.odd_real,
+			 asd->params.dvs_stat->hor_prod.odd_real,
+			 asd->params.dvs_hor_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.hor_prod.odd_imag,
+			 asd->params.dvs_stat->hor_prod.odd_imag,
+			 asd->params.dvs_hor_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.hor_prod.even_real,
+			 asd->params.dvs_stat->hor_prod.even_real,
+			 asd->params.dvs_hor_proj_bytes))
+		return -EFAULT;
+	if (copy_to_user(stats->dvs2_stat.hor_prod.even_imag,
+			 asd->params.dvs_stat->hor_prod.even_imag,
+			 asd->params.dvs_hor_proj_bytes))
+		return -EFAULT;
+
+	return 0;
+}
+
+struct ia_css_shading_table *atomisp_css_shading_table_alloc(
+    unsigned int width, unsigned int height)
+{
+	return ia_css_shading_table_alloc(width, height);
+}
+
+void atomisp_css_set_shading_table(struct atomisp_sub_device *asd,
+				   struct ia_css_shading_table *table)
+{
+	asd->params.config.shading_table = table;
+}
+
+void atomisp_css_shading_table_free(struct ia_css_shading_table *table)
+{
+	ia_css_shading_table_free(table);
+}
+
+struct ia_css_morph_table *atomisp_css_morph_table_allocate(
+    unsigned int width, unsigned int height)
+{
+	return ia_css_morph_table_allocate(width, height);
+}
+
+void atomisp_css_set_morph_table(struct atomisp_sub_device *asd,
+				 struct ia_css_morph_table *table)
+{
+	asd->params.config.morph_table = table;
+}
+
+void atomisp_css_get_morph_table(struct atomisp_sub_device *asd,
+				 struct ia_css_morph_table *table)
+{
+	struct ia_css_isp_config isp_config;
+	struct atomisp_device *isp = asd->isp;
+
+	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
+		dev_err(isp->dev,
+			"%s called after streamoff, skipping.\n", __func__);
+		return;
+	}
+	memset(table, 0, sizeof(struct ia_css_morph_table));
+	memset(&isp_config, 0, sizeof(struct ia_css_isp_config));
+	isp_config.morph_table = table;
+	ia_css_stream_get_isp_config(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    &isp_config);
+}
+
+void atomisp_css_morph_table_free(struct ia_css_morph_table *table)
+{
+	ia_css_morph_table_free(table);
+}
+
+static struct atomisp_sub_device *__get_atomisp_subdev(
+    struct ia_css_pipe *css_pipe,
+    struct atomisp_device *isp,
+    enum atomisp_input_stream_id *stream_id)
+{
+	int i, j, k;
+	struct atomisp_sub_device *asd;
+	struct atomisp_stream_env *stream_env;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		if (asd->streaming == ATOMISP_DEVICE_STREAMING_DISABLED)
+			continue;
+		for (j = 0; j < ATOMISP_INPUT_STREAM_NUM; j++) {
+			stream_env = &asd->stream_env[j];
+			for (k = 0; k < IA_CSS_PIPE_ID_NUM; k++) {
+				if (stream_env->pipes[k] &&
+				    stream_env->pipes[k] == css_pipe) {
+					*stream_id = j;
+					return asd;
+				}
+			}
+		}
+	}
+
+	return NULL;
+}
+
+int atomisp_css_isr_thread(struct atomisp_device *isp,
+			   bool *frame_done_found,
+			   bool *css_pipe_done)
+{
+	enum atomisp_input_stream_id stream_id = 0;
+	struct atomisp_css_event current_event;
+	struct atomisp_sub_device *asd;
+
+	lockdep_assert_held(&isp->mutex);
+
+	while (!ia_css_dequeue_psys_event(&current_event.event)) {
+		if (current_event.event.type ==
+		    IA_CSS_EVENT_TYPE_FW_ASSERT) {
+			/*
+			 * Received FW assertion signal,
+			 * trigger WDT to recover
+			 */
+			dev_err(isp->dev,
+				"%s: ISP reports FW_ASSERT event! fw_assert_module_id %d fw_assert_line_no %d\n",
+				__func__,
+				current_event.event.fw_assert_module_id,
+				current_event.event.fw_assert_line_no);
+
+			queue_work(system_long_wq, &isp->assert_recovery_work);
+			return -EINVAL;
+		} else if (current_event.event.type == IA_CSS_EVENT_TYPE_FW_WARNING) {
+			dev_warn(isp->dev, "%s: ISP reports warning, code is %d, exp_id %d\n",
+				 __func__, current_event.event.fw_warning,
+				 current_event.event.exp_id);
+			continue;
+		}
+
+		asd = __get_atomisp_subdev(current_event.event.pipe,
+					   isp, &stream_id);
+		if (!asd) {
+			if (current_event.event.type == IA_CSS_EVENT_TYPE_TIMER)
+				dev_dbg(isp->dev,
+					"event: Timer event.");
+			else
+				dev_warn(isp->dev, "%s:no subdev.event:%d",
+					 __func__,
+					 current_event.event.type);
+			continue;
+		}
+
+		atomisp_css_temp_pipe_to_pipe_id(asd, &current_event);
+		switch (current_event.event.type) {
+		case IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE:
+			dev_dbg(isp->dev, "event: Output frame done");
+			frame_done_found[asd->index] = true;
+			atomisp_buf_done(asd, 0, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_SECOND_OUTPUT_FRAME_DONE:
+			dev_dbg(isp->dev, "event: Second output frame done");
+			frame_done_found[asd->index] = true;
+			atomisp_buf_done(asd, 0, IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE:
+			dev_dbg(isp->dev, "event: 3A stats frame done");
+			atomisp_buf_done(asd, 0,
+					 IA_CSS_BUFFER_TYPE_3A_STATISTICS,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_METADATA_DONE:
+			dev_dbg(isp->dev, "event: metadata frame done");
+			atomisp_buf_done(asd, 0,
+					 IA_CSS_BUFFER_TYPE_METADATA,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_VF_OUTPUT_FRAME_DONE:
+			dev_dbg(isp->dev, "event: VF output frame done");
+			atomisp_buf_done(asd, 0,
+					 IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_SECOND_VF_OUTPUT_FRAME_DONE:
+			dev_dbg(isp->dev, "event: second VF output frame done");
+			atomisp_buf_done(asd, 0,
+					 IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE:
+			dev_dbg(isp->dev, "event: dis stats frame done");
+			atomisp_buf_done(asd, 0,
+					 IA_CSS_BUFFER_TYPE_DIS_STATISTICS,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case IA_CSS_EVENT_TYPE_PIPELINE_DONE:
+			dev_dbg(isp->dev, "event: pipeline done");
+			css_pipe_done[asd->index] = true;
+			break;
+		case IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE:
+			dev_warn(isp->dev, "unexpected event: acc stage done");
+			break;
+		default:
+			dev_dbg(isp->dev, "unhandled css stored event: 0x%x\n",
+				current_event.event.type);
+			break;
+		}
+	}
+
+	return 0;
+}
+
+bool atomisp_css_valid_sof(struct atomisp_device *isp)
+{
+	unsigned int i, j;
+
+	/* Loop for each css stream */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		/* Loop for each css vc stream */
+		for (j = 0; j < ATOMISP_INPUT_STREAM_NUM; j++) {
+			if (!asd->stream_env[j].stream)
+				continue;
+
+			dev_dbg(isp->dev,
+				"stream #%d: mode: %d\n", j,
+				asd->stream_env[j].stream_config.mode);
+			if (asd->stream_env[j].stream_config.mode ==
+			    IA_CSS_INPUT_MODE_BUFFERED_SENSOR)
+				return false;
+		}
+	}
+
+	return true;
+}
+
+int atomisp_css_debug_dump_isp_binary(void)
+{
+	ia_css_debug_dump_isp_binary();
+	return 0;
+}
+
+int atomisp_css_dump_sp_raw_copy_linecount(bool reduced)
+{
+	sh_css_dump_sp_raw_copy_linecount(reduced);
+	return 0;
+}
+
+static const char * const fw_type_name[] = {
+	[ia_css_sp_firmware]		= "SP",
+	[ia_css_isp_firmware]		= "ISP",
+	[ia_css_bootloader_firmware]	= "BootLoader",
+	[ia_css_acc_firmware]		= "accel",
+};
+
+static const char * const fw_acc_type_name[] = {
+	[IA_CSS_ACC_NONE] =		"Normal",
+	[IA_CSS_ACC_OUTPUT] =		"Accel stage on output",
+	[IA_CSS_ACC_VIEWFINDER] =	"Accel stage on viewfinder",
+	[IA_CSS_ACC_STANDALONE] =	"Stand-alone acceleration",
+};
+
+int atomisp_css_dump_blob_infor(struct atomisp_device *isp)
+{
+	struct ia_css_blob_descr *bd = sh_css_blob_info;
+	unsigned int i, nm = sh_css_num_binaries;
+
+	if (nm == 0)
+		return -EPERM;
+	if (!bd)
+		return -EPERM;
+
+	/*
+	 * The sh_css_load_firmware function discard the initial
+	 * "SPS" binaries
+	 */
+	for (i = 0; i < sh_css_num_binaries - NUM_OF_SPS; i++) {
+		switch (bd[i].header.type) {
+		case ia_css_isp_firmware:
+			dev_dbg(isp->dev, "Num%2d type %s (%s), binary id is %2d, name is %s\n",
+				i + NUM_OF_SPS,
+				fw_type_name[bd[i].header.type],
+				fw_acc_type_name[bd[i].header.info.isp.type],
+				bd[i].header.info.isp.sp.id,
+				bd[i].name);
+			break;
+		default:
+			dev_dbg(isp->dev, "Num%2d type %s, name is %s\n",
+				i + NUM_OF_SPS, fw_type_name[bd[i].header.type],
+				bd[i].name);
+		}
+	}
+
+	return 0;
+}
+
+void atomisp_css_set_isp_config_id(struct atomisp_sub_device *asd,
+				   uint32_t isp_config_id)
+{
+	asd->params.config.isp_config_id = isp_config_id;
+}
+
+void atomisp_css_set_isp_config_applied_frame(struct atomisp_sub_device *asd,
+	struct ia_css_frame *output_frame)
+{
+	asd->params.config.output_frame = output_frame;
+}
+
+int atomisp_get_css_dbgfunc(void)
+{
+	return dbg_func;
+}
+
+int atomisp_set_css_dbgfunc(struct atomisp_device *isp, int opt)
+{
+	int ret;
+
+	ret = __set_css_print_env(isp, opt);
+	if (ret == 0)
+		dbg_func = opt;
+
+	return ret;
+}
+
+void atomisp_en_dz_capt_pipe(struct atomisp_sub_device *asd, bool enable)
+{
+	ia_css_en_dz_capt_pipe(
+	    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+	    enable);
+}
+
+struct ia_css_dvs_grid_info *atomisp_css_get_dvs_grid_info(
+    struct ia_css_grid_info *grid_info)
+{
+	if (!grid_info)
+		return NULL;
+
+#ifdef IA_CSS_DVS_STAT_GRID_INFO_SUPPORTED
+	return &grid_info->dvs_grid.dvs_grid_info;
+#else
+	return &grid_info->dvs_grid;
+#endif
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_compat_css20.h b/drivers/staging/media/atomisp/pci/atomisp_compat_css20.h
new file mode 100644
index 000000000..e06015343
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_compat_css20.h
@@ -0,0 +1,169 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Clovertrail PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __ATOMISP_COMPAT_CSS20_H__
+#define __ATOMISP_COMPAT_CSS20_H__
+
+#include <media/v4l2-mediabus.h>
+
+#include "ia_css.h"
+#include "ia_css_types.h"
+#include "ia_css_acc_types.h"
+#include "sh_css_legacy.h"
+
+#define ATOMISP_CSS2_PIPE_MAX	2
+#define ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES     3
+#define ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES_LOCK_EN     4
+#define ATOMISP_CSS2_NUM_DVS_FRAME_DELAY     2
+
+#define CSS_MIPI_FRAME_BUFFER_SIZE_1	0x60000
+#define CSS_MIPI_FRAME_BUFFER_SIZE_2	0x80000
+
+struct atomisp_device;
+struct atomisp_sub_device;
+
+#define MAX_STREAMS_PER_CHANNEL	2
+
+/*
+ * These are used to indicate the css stream state, corresponding
+ * stream handling can be done via judging the different state.
+ */
+enum atomisp_css_stream_state {
+	CSS_STREAM_UNINIT,
+	CSS_STREAM_CREATED,
+	CSS_STREAM_STARTED,
+	CSS_STREAM_STOPPED,
+};
+
+/*
+ *  Sensor of external ISP can send multiple steams with different mipi data
+ * type in the same virtual channel. This information needs to come from the
+ * sensor or external ISP
+ */
+struct atomisp_css_isys_config_info {
+	unsigned int input_format;
+	unsigned int width;
+	unsigned int height;
+};
+
+struct atomisp_stream_env {
+	struct ia_css_stream *stream;
+	struct ia_css_stream_config stream_config;
+	struct ia_css_stream_info stream_info;
+	struct ia_css_pipe *pipes[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_pipe *multi_pipes[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_pipe_config pipe_configs[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_pipe_extra_config pipe_extra_configs[IA_CSS_PIPE_ID_NUM];
+	bool update_pipe[IA_CSS_PIPE_ID_NUM];
+	enum atomisp_css_stream_state stream_state;
+	struct ia_css_stream *acc_stream;
+	enum atomisp_css_stream_state acc_stream_state;
+	struct ia_css_stream_config acc_stream_config;
+	unsigned int ch_id; /* virtual channel ID */
+	unsigned int isys_configs;
+	struct atomisp_css_isys_config_info isys_info[MAX_STREAMS_PER_CHANNEL];
+};
+
+struct atomisp_css_env {
+	struct ia_css_env isp_css_env;
+	struct ia_css_fw isp_css_fw;
+};
+
+struct atomisp_s3a_buf {
+	struct ia_css_isp_3a_statistics *s3a_data;
+	struct ia_css_isp_3a_statistics_map *s3a_map;
+	struct list_head list;
+};
+
+struct atomisp_dis_buf {
+	struct ia_css_isp_dvs_statistics *dis_data;
+	struct ia_css_isp_dvs_statistics_map *dvs_map;
+	struct list_head list;
+};
+
+struct atomisp_css_buffer {
+	struct ia_css_buffer css_buffer;
+};
+
+struct atomisp_css_event {
+	enum ia_css_pipe_id pipe;
+	struct ia_css_event event;
+};
+
+void atomisp_css_set_macc_config(struct atomisp_sub_device *asd,
+				 struct ia_css_macc_config *macc_config);
+
+void atomisp_css_set_ecd_config(struct atomisp_sub_device *asd,
+				struct ia_css_ecd_config *ecd_config);
+
+void atomisp_css_set_ynr_config(struct atomisp_sub_device *asd,
+				struct ia_css_ynr_config *ynr_config);
+
+void atomisp_css_set_fc_config(struct atomisp_sub_device *asd,
+			       struct ia_css_fc_config *fc_config);
+
+void atomisp_css_set_aa_config(struct atomisp_sub_device *asd,
+			       struct ia_css_aa_config *aa_config);
+
+void atomisp_css_set_baa_config(struct atomisp_sub_device *asd,
+				struct ia_css_aa_config *baa_config);
+
+void atomisp_css_set_anr_config(struct atomisp_sub_device *asd,
+				struct ia_css_anr_config *anr_config);
+
+void atomisp_css_set_xnr_config(struct atomisp_sub_device *asd,
+				struct ia_css_xnr_config *xnr_config);
+
+void atomisp_css_set_cnr_config(struct atomisp_sub_device *asd,
+				struct ia_css_cnr_config *cnr_config);
+
+void atomisp_css_set_ctc_config(struct atomisp_sub_device *asd,
+				struct ia_css_ctc_config *ctc_config);
+
+void atomisp_css_set_yuv2rgb_cc_config(struct atomisp_sub_device *asd,
+				       struct ia_css_cc_config *yuv2rgb_cc_config);
+
+void atomisp_css_set_rgb2yuv_cc_config(struct atomisp_sub_device *asd,
+				       struct ia_css_cc_config *rgb2yuv_cc_config);
+
+void atomisp_css_set_anr_thres(struct atomisp_sub_device *asd,
+			       struct ia_css_anr_thres *anr_thres);
+
+int atomisp_css_load_firmware(struct atomisp_device *isp);
+
+void atomisp_css_set_dvs_6axis(struct atomisp_sub_device *asd,
+			       struct ia_css_dvs_6axis_config *dvs_6axis);
+
+int atomisp_css_debug_dump_isp_binary(void);
+
+int atomisp_css_dump_sp_raw_copy_linecount(bool reduced);
+
+int atomisp_css_dump_blob_infor(struct atomisp_device *isp);
+
+void atomisp_css_set_isp_config_id(struct atomisp_sub_device *asd,
+				   uint32_t isp_config_id);
+
+void atomisp_css_set_isp_config_applied_frame(struct atomisp_sub_device *asd,
+	struct ia_css_frame *output_frame);
+
+int atomisp_get_css_dbgfunc(void);
+
+int atomisp_set_css_dbgfunc(struct atomisp_device *isp, int opt);
+struct ia_css_dvs_grid_info *atomisp_css_get_dvs_grid_info(
+    struct ia_css_grid_info *grid_info);
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp_compat_ioctl32.h b/drivers/staging/media/atomisp/pci/atomisp_compat_ioctl32.h
new file mode 100644
index 000000000..33821b51d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_compat_ioctl32.h
@@ -0,0 +1,310 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef __ATOMISP_COMPAT_IOCTL32_H__
+#define __ATOMISP_COMPAT_IOCTL32_H__
+
+#include <linux/compat.h>
+#include <linux/videodev2.h>
+
+#include "atomisp_compat.h"
+
+struct atomisp_histogram32 {
+	unsigned int num_elements;
+	compat_uptr_t data;
+};
+
+struct atomisp_dvs2_stat_types32 {
+	compat_uptr_t odd_real; /** real part of the odd statistics*/
+	compat_uptr_t odd_imag; /** imaginary part of the odd statistics*/
+	compat_uptr_t even_real;/** real part of the even statistics*/
+	compat_uptr_t even_imag;/** imaginary part of the even statistics*/
+};
+
+struct atomisp_dvs2_coef_types32 {
+	compat_uptr_t odd_real; /** real part of the odd coefficients*/
+	compat_uptr_t odd_imag; /** imaginary part of the odd coefficients*/
+	compat_uptr_t even_real;/** real part of the even coefficients*/
+	compat_uptr_t even_imag;/** imaginary part of the even coefficients*/
+};
+
+struct atomisp_dvs2_statistics32 {
+	struct atomisp_dvs_grid_info grid_info;
+	struct atomisp_dvs2_stat_types32 hor_prod;
+	struct atomisp_dvs2_stat_types32 ver_prod;
+};
+
+struct atomisp_dis_statistics32 {
+	struct atomisp_dvs2_statistics32 dvs2_stat;
+	u32 exp_id;
+};
+
+struct atomisp_dis_coefficients32 {
+	struct atomisp_dvs_grid_info grid_info;
+	struct atomisp_dvs2_coef_types32 hor_coefs;
+	struct atomisp_dvs2_coef_types32 ver_coefs;
+};
+
+struct atomisp_3a_statistics32 {
+	struct atomisp_grid_info  grid_info;
+	compat_uptr_t data;
+	compat_uptr_t rgby_data;
+	u32 exp_id;
+	u32 isp_config_id;
+};
+
+struct atomisp_metadata_with_type32 {
+	/* to specify which type of metadata to get */
+	enum atomisp_metadata_type type;
+	compat_uptr_t data;
+	u32 width;
+	u32 height;
+	u32 stride; /* in bytes */
+	u32 exp_id; /* exposure ID */
+	compat_uptr_t effective_width;
+};
+
+struct atomisp_metadata32 {
+	compat_uptr_t data;
+	u32 width;
+	u32 height;
+	u32 stride;
+	u32 exp_id;
+	compat_uptr_t effective_width;
+};
+
+struct atomisp_morph_table32 {
+	unsigned int enabled;
+	unsigned int height;
+	unsigned int width;	/* number of valid elements per line */
+	compat_uptr_t coordinates_x[ATOMISP_MORPH_TABLE_NUM_PLANES];
+	compat_uptr_t coordinates_y[ATOMISP_MORPH_TABLE_NUM_PLANES];
+};
+
+struct v4l2_framebuffer32 {
+	__u32			capability;
+	__u32			flags;
+	compat_uptr_t		base;
+	struct v4l2_pix_format	fmt;
+};
+
+struct atomisp_overlay32 {
+	/* the frame containing the overlay data The overlay frame width should
+	 * be the multiples of 2*ISP_VEC_NELEMS. The overlay frame height
+	 * should be the multiples of 2.
+	 */
+	compat_uptr_t frame;
+	/* Y value of overlay background */
+	unsigned char bg_y;
+	/* U value of overlay background */
+	char bg_u;
+	/* V value of overlay background */
+	char bg_v;
+	/* the blending percent of input data for Y subpixels */
+	unsigned char blend_input_perc_y;
+	/* the blending percent of input data for U subpixels */
+	unsigned char blend_input_perc_u;
+	/* the blending percent of input data for V subpixels */
+	unsigned char blend_input_perc_v;
+	/* the blending percent of overlay data for Y subpixels */
+	unsigned char blend_overlay_perc_y;
+	/* the blending percent of overlay data for U subpixels */
+	unsigned char blend_overlay_perc_u;
+	/* the blending percent of overlay data for V subpixels */
+	unsigned char blend_overlay_perc_v;
+	/* the overlay start x pixel position on output frame It should be the
+	   multiples of 2*ISP_VEC_NELEMS. */
+	unsigned int overlay_start_x;
+	/* the overlay start y pixel position on output frame It should be the
+	   multiples of 2. */
+	unsigned int overlay_start_y;
+};
+
+struct atomisp_calibration_group32 {
+	unsigned int size;
+	unsigned int type;
+	compat_uptr_t calb_grp_values;
+};
+
+struct v4l2_private_int_data32 {
+	__u32 size;
+	compat_uptr_t data;
+	__u32 reserved[2];
+};
+
+struct atomisp_shading_table32 {
+	__u32 enable;
+	__u32 sensor_width;
+	__u32 sensor_height;
+	__u32 width;
+	__u32 height;
+	__u32 fraction_bits;
+
+	compat_uptr_t data[ATOMISP_NUM_SC_COLORS];
+};
+
+struct atomisp_parameters32 {
+	compat_uptr_t wb_config;  /* White Balance config */
+	compat_uptr_t cc_config;  /* Color Correction config */
+	compat_uptr_t tnr_config; /* Temporal Noise Reduction */
+	compat_uptr_t ecd_config; /* Eigen Color Demosaicing */
+	compat_uptr_t ynr_config; /* Y(Luma) Noise Reduction */
+	compat_uptr_t fc_config;  /* Fringe Control */
+	compat_uptr_t formats_config;  /* Formats Control */
+	compat_uptr_t cnr_config; /* Chroma Noise Reduction */
+	compat_uptr_t macc_config;  /* MACC */
+	compat_uptr_t ctc_config; /* Chroma Tone Control */
+	compat_uptr_t aa_config;  /* Anti-Aliasing */
+	compat_uptr_t baa_config;  /* Anti-Aliasing */
+	compat_uptr_t ce_config;
+	compat_uptr_t dvs_6axis_config;
+	compat_uptr_t ob_config;  /* Objective Black config */
+	compat_uptr_t dp_config;  /* Dead Pixel config */
+	compat_uptr_t nr_config;  /* Noise Reduction config */
+	compat_uptr_t ee_config;  /* Edge Enhancement config */
+	compat_uptr_t de_config;  /* Demosaic config */
+	compat_uptr_t gc_config;  /* Gamma Correction config */
+	compat_uptr_t anr_config; /* Advanced Noise Reduction */
+	compat_uptr_t a3a_config; /* 3A Statistics config */
+	compat_uptr_t xnr_config; /* eXtra Noise Reduction */
+	compat_uptr_t dz_config;  /* Digital Zoom */
+	compat_uptr_t yuv2rgb_cc_config; /* Color
+							Correction config */
+	compat_uptr_t rgb2yuv_cc_config; /* Color
+							Correction config */
+	compat_uptr_t macc_table;
+	compat_uptr_t gamma_table;
+	compat_uptr_t ctc_table;
+	compat_uptr_t xnr_table;
+	compat_uptr_t r_gamma_table;
+	compat_uptr_t g_gamma_table;
+	compat_uptr_t b_gamma_table;
+	compat_uptr_t motion_vector; /* For 2-axis DVS */
+	compat_uptr_t shading_table;
+	compat_uptr_t morph_table;
+	compat_uptr_t dvs_coefs; /* DVS 1.0 coefficients */
+	compat_uptr_t dvs2_coefs; /* DVS 2.0 coefficients */
+	compat_uptr_t capture_config;
+	compat_uptr_t anr_thres;
+
+	compat_uptr_t	lin_2500_config;       /* Skylake: Linearization config */
+	compat_uptr_t	obgrid_2500_config;    /* Skylake: OBGRID config */
+	compat_uptr_t	bnr_2500_config;       /* Skylake: bayer denoise config */
+	compat_uptr_t	shd_2500_config;       /* Skylake: shading config */
+	compat_uptr_t	dm_2500_config;        /* Skylake: demosaic config */
+	compat_uptr_t	rgbpp_2500_config;     /* Skylake: RGBPP config */
+	compat_uptr_t	dvs_stat_2500_config;  /* Skylake: DVS STAT config */
+	compat_uptr_t	lace_stat_2500_config; /* Skylake: LACE STAT config */
+	compat_uptr_t	yuvp1_2500_config;     /* Skylake: yuvp1 config */
+	compat_uptr_t	yuvp2_2500_config;     /* Skylake: yuvp2 config */
+	compat_uptr_t	tnr_2500_config;       /* Skylake: TNR config */
+	compat_uptr_t	dpc_2500_config;       /* Skylake: DPC config */
+	compat_uptr_t	awb_2500_config;       /* Skylake: auto white balance config */
+	compat_uptr_t
+	awb_fr_2500_config;    /* Skylake: auto white balance filter response config */
+	compat_uptr_t	anr_2500_config;       /* Skylake: ANR config */
+	compat_uptr_t	af_2500_config;        /* Skylake: auto focus config */
+	compat_uptr_t	ae_2500_config;        /* Skylake: auto exposure config */
+	compat_uptr_t	bds_2500_config;       /* Skylake: bayer downscaler config */
+	compat_uptr_t
+	dvs_2500_config;       /* Skylake: digital video stabilization config */
+	compat_uptr_t	res_mgr_2500_config;
+
+	/*
+	 * Output frame pointer the config is to be applied to (optional),
+	 * set to NULL to make this config is applied as global.
+	 */
+	compat_uptr_t	output_frame;
+	/*
+	 * Unique ID to track which config was actually applied to a particular
+	 * frame, driver will send this id back with output frame together.
+	 */
+	u32	isp_config_id;
+	u32	per_frame_setting;
+};
+
+struct atomisp_dvs_6axis_config32 {
+	u32 exp_id;
+	u32 width_y;
+	u32 height_y;
+	u32 width_uv;
+	u32 height_uv;
+	compat_uptr_t xcoords_y;
+	compat_uptr_t ycoords_y;
+	compat_uptr_t xcoords_uv;
+	compat_uptr_t ycoords_uv;
+};
+
+struct atomisp_sensor_ae_bracketing_lut32 {
+	compat_uptr_t lut;
+	unsigned int lut_size;
+};
+
+#define ATOMISP_IOC_G_HISTOGRAM32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 3, struct atomisp_histogram32)
+#define ATOMISP_IOC_S_HISTOGRAM32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 3, struct atomisp_histogram32)
+
+#define ATOMISP_IOC_G_DIS_STAT32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dis_statistics32)
+#define ATOMISP_IOC_S_DIS_COEFS32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dis_coefficients32)
+
+#define ATOMISP_IOC_S_DIS_VECTOR32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 6, struct atomisp_dvs_6axis_config32)
+
+#define ATOMISP_IOC_G_3A_STAT32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 7, struct atomisp_3a_statistics32)
+
+#define ATOMISP_IOC_G_ISP_GDC_TAB32 \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 10, struct atomisp_morph_table32)
+#define ATOMISP_IOC_S_ISP_GDC_TAB32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 10, struct atomisp_morph_table32)
+
+#define ATOMISP_IOC_S_ISP_FPN_TABLE32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 17, struct v4l2_framebuffer32)
+
+#define ATOMISP_IOC_G_ISP_OVERLAY32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 18, struct atomisp_overlay32)
+#define ATOMISP_IOC_S_ISP_OVERLAY32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 18, struct atomisp_overlay32)
+
+#define ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 22, struct atomisp_calibration_group32)
+
+#define ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 26, struct v4l2_private_int_data32)
+
+#define ATOMISP_IOC_S_ISP_SHD_TAB32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 27, struct atomisp_shading_table32)
+
+#define ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 29, struct v4l2_private_int_data32)
+
+#define ATOMISP_IOC_S_PARAMETERS32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 32, struct atomisp_parameters32)
+
+#define ATOMISP_IOC_G_METADATA32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 34, struct atomisp_metadata32)
+
+#define ATOMISP_IOC_G_METADATA_BY_TYPE32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 34, struct atomisp_metadata_with_type32)
+
+#define ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 43, struct atomisp_sensor_ae_bracketing_lut32)
+
+#endif /* __ATOMISP_COMPAT_IOCTL32_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_csi2.c b/drivers/staging/media/atomisp/pci/atomisp_csi2.c
new file mode 100644
index 000000000..4a9268bac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_csi2.c
@@ -0,0 +1,430 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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 <media/v4l2-event.h>
+#include <media/v4l2-mediabus.h>
+#include "atomisp_cmd.h"
+#include "atomisp_internal.h"
+#include "atomisp-regs.h"
+
+static struct
+v4l2_mbus_framefmt *__csi2_get_format(struct atomisp_mipi_csi2_device *csi2,
+				      struct v4l2_subdev_state *sd_state,
+				      enum v4l2_subdev_format_whence which,
+				      unsigned int pad)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->subdev, sd_state,
+						  pad);
+	else
+		return &csi2->formats[pad];
+}
+
+/*
+ * csi2_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @fh     : V4L2 subdev file handle
+ * @code   : pointer to v4l2_subdev_pad_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct atomisp_in_fmt_conv *ic = atomisp_in_fmt_conv;
+	unsigned int i = 0;
+
+	while (ic->code) {
+		if (i == code->index) {
+			code->code = ic->code;
+			return 0;
+		}
+		i++, ic++;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * csi2_get_format - Handle get format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @pad: pad num
+ * @fmt: pointer to v4l2 format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_get_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct atomisp_mipi_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, sd_state, fmt->which, fmt->pad);
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+int atomisp_csi2_set_ffmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  unsigned int which, uint16_t pad,
+			  struct v4l2_mbus_framefmt *ffmt)
+{
+	struct atomisp_mipi_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *actual_ffmt = __csi2_get_format(csi2,
+								   sd_state,
+								   which, pad);
+
+	if (pad == CSI2_PAD_SINK) {
+		const struct atomisp_in_fmt_conv *ic;
+		struct v4l2_mbus_framefmt tmp_ffmt;
+
+		ic = atomisp_find_in_fmt_conv(ffmt->code);
+		if (ic)
+			actual_ffmt->code = ic->code;
+		else
+			actual_ffmt->code = atomisp_in_fmt_conv[0].code;
+
+		actual_ffmt->width = clamp_t(u32, ffmt->width,
+					     ATOM_ISP_MIN_WIDTH,
+					     ATOM_ISP_MAX_WIDTH);
+		actual_ffmt->height = clamp_t(u32, ffmt->height,
+					      ATOM_ISP_MIN_HEIGHT,
+					      ATOM_ISP_MAX_HEIGHT);
+
+		tmp_ffmt = *ffmt = *actual_ffmt;
+
+		return atomisp_csi2_set_ffmt(sd, sd_state, which,
+					     CSI2_PAD_SOURCE,
+					     &tmp_ffmt);
+	}
+
+	/* FIXME: DPCM decompression */
+	*actual_ffmt = *ffmt = *__csi2_get_format(csi2, sd_state, which,
+						  CSI2_PAD_SINK);
+
+	return 0;
+}
+
+/*
+ * csi2_set_format - Handle set format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @pad: pad num
+ * @fmt: pointer to v4l2 format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_set_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	return atomisp_csi2_set_ffmt(sd, sd_state, fmt->which, fmt->pad,
+				     &fmt->format);
+}
+
+/*
+ * csi2_set_stream - Enable/Disable streaming on the CSI2 module
+ * @sd: ISP CSI2 V4L2 subdevice
+ * @enable: Enable/disable stream (1/0)
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	return 0;
+}
+
+/* subdev core operations */
+static const struct v4l2_subdev_core_ops csi2_core_ops = {
+};
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.enum_mbus_code = csi2_enum_mbus_code,
+	.get_fmt = csi2_get_format,
+	.set_fmt = csi2_set_format,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops csi2_ops = {
+	.core = &csi2_core_ops,
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+/*
+ * csi2_link_setup - Setup CSI2 connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct atomisp_mipi_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	u32 result = local->index | is_media_entity_v4l2_subdev(remote->entity);
+
+	switch (result) {
+	case CSI2_PAD_SOURCE | MEDIA_ENT_F_OLD_BASE:
+		/* not supported yet */
+		return -EINVAL;
+
+	case CSI2_PAD_SOURCE | MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_ISP_SUBDEV)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_ISP_SUBDEV;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_ISP_SUBDEV;
+		}
+		break;
+
+	default:
+		/* Link from camera to CSI2 is fixed... */
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations csi2_media_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * ispcsi2_init_entities - Initialize subdev and media entity.
+ * @csi2: Pointer to ispcsi2 structure.
+ * return -ENOMEM or zero on success
+ */
+static int mipi_csi2_init_entities(struct atomisp_mipi_csi2_device *csi2,
+				   int port)
+{
+	struct v4l2_subdev *sd = &csi2->subdev;
+	struct media_pad *pads = csi2->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	v4l2_subdev_init(sd, &csi2_ops);
+	snprintf(sd->name, sizeof(sd->name), "ATOM ISP CSI2-port%d", port);
+
+	v4l2_set_subdevdata(sd, csi2);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+
+	me->ops = &csi2_media_ops;
+	me->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+	ret = media_entity_pads_init(me, CSI2_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	csi2->formats[CSI2_PAD_SINK].code = atomisp_in_fmt_conv[0].code;
+	csi2->formats[CSI2_PAD_SOURCE].code = atomisp_in_fmt_conv[0].code;
+
+	return 0;
+}
+
+void
+atomisp_mipi_csi2_unregister_entities(struct atomisp_mipi_csi2_device *csi2)
+{
+	media_entity_cleanup(&csi2->subdev.entity);
+	v4l2_device_unregister_subdev(&csi2->subdev);
+}
+
+int atomisp_mipi_csi2_register_entities(struct atomisp_mipi_csi2_device *csi2,
+					struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	atomisp_mipi_csi2_unregister_entities(csi2);
+	return ret;
+}
+
+static const int LIMIT_SHIFT = 6;	/* Limit numeric range into 31 bits */
+
+static int
+atomisp_csi2_configure_calc(const short int coeffs[2], int mipi_freq, int def)
+{
+	/* Delay counter accuracy, 1/0.0625 for ANN/CHT, 1/0.125 for BXT */
+	static const int accinv = 16;		/* 1 / COUNT_ACC */
+	int r;
+
+	if (mipi_freq >> LIMIT_SHIFT <= 0)
+		return def;
+
+	r = accinv * coeffs[1] * (500000000 >> LIMIT_SHIFT);
+	r /= mipi_freq >> LIMIT_SHIFT;
+	r += accinv * coeffs[0];
+
+	return r;
+}
+
+static void atomisp_csi2_configure_isp2401(struct atomisp_sub_device *asd)
+{
+	/*
+	 * The ISP2401 new input system CSI2+ receiver has several
+	 * parameters affecting the receiver timings. These depend
+	 * on the MIPI bus frequency F in Hz (sensor transmitter rate)
+	 * as follows:
+	 *	register value = (A/1e9 + B * UI) / COUNT_ACC
+	 * where
+	 *	UI = 1 / (2 * F) in seconds
+	 *	COUNT_ACC = counter accuracy in seconds
+	 *	For ANN and CHV, COUNT_ACC = 0.0625 ns
+	 *	For BXT,  COUNT_ACC = 0.125 ns
+	 * A and B are coefficients from the table below,
+	 * depending whether the register minimum or maximum value is
+	 * calculated.
+	 *				       Minimum     Maximum
+	 * Clock lane			       A     B     A     B
+	 * reg_rx_csi_dly_cnt_termen_clane     0     0    38     0
+	 * reg_rx_csi_dly_cnt_settle_clane    95    -8   300   -16
+	 * Data lanes
+	 * reg_rx_csi_dly_cnt_termen_dlane0    0     0    35     4
+	 * reg_rx_csi_dly_cnt_settle_dlane0   85    -2   145    -6
+	 * reg_rx_csi_dly_cnt_termen_dlane1    0     0    35     4
+	 * reg_rx_csi_dly_cnt_settle_dlane1   85    -2   145    -6
+	 * reg_rx_csi_dly_cnt_termen_dlane2    0     0    35     4
+	 * reg_rx_csi_dly_cnt_settle_dlane2   85    -2   145    -6
+	 * reg_rx_csi_dly_cnt_termen_dlane3    0     0    35     4
+	 * reg_rx_csi_dly_cnt_settle_dlane3   85    -2   145    -6
+	 *
+	 * We use the minimum values in the calculations below.
+	 */
+	static const short int coeff_clk_termen[] = { 0, 0 };
+	static const short int coeff_clk_settle[] = { 95, -8 };
+	static const short int coeff_dat_termen[] = { 0, 0 };
+	static const short int coeff_dat_settle[] = { 85, -2 };
+	static const int TERMEN_DEFAULT		  = 0 * 0;
+	static const int SETTLE_DEFAULT		  = 0x480;
+
+	static const hrt_address csi2_port_base[] = {
+		[ATOMISP_CAMERA_PORT_PRIMARY]     = CSI2_PORT_A_BASE,
+		[ATOMISP_CAMERA_PORT_SECONDARY]   = CSI2_PORT_B_BASE,
+		[ATOMISP_CAMERA_PORT_TERTIARY]    = CSI2_PORT_C_BASE,
+	};
+	/* Number of lanes on each port, excluding clock lane */
+	static const unsigned char csi2_port_lanes[] = {
+		[ATOMISP_CAMERA_PORT_PRIMARY]     = 4,
+		[ATOMISP_CAMERA_PORT_SECONDARY]   = 2,
+		[ATOMISP_CAMERA_PORT_TERTIARY]    = 2,
+	};
+	static const hrt_address csi2_lane_base[] = {
+		CSI2_LANE_CL_BASE,
+		CSI2_LANE_D0_BASE,
+		CSI2_LANE_D1_BASE,
+		CSI2_LANE_D2_BASE,
+		CSI2_LANE_D3_BASE,
+	};
+
+	int clk_termen;
+	int clk_settle;
+	int dat_termen;
+	int dat_settle;
+
+	struct v4l2_control ctrl;
+	struct atomisp_device *isp = asd->isp;
+	struct camera_mipi_info *mipi_info;
+	int mipi_freq = 0;
+	enum atomisp_camera_port port;
+
+	int n;
+
+	mipi_info = atomisp_to_sensor_mipi_info(
+			isp->inputs[asd->input_curr].camera);
+	port = mipi_info->port;
+
+	ctrl.id = V4L2_CID_LINK_FREQ;
+	if (v4l2_g_ctrl
+	    (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl) == 0)
+		mipi_freq = ctrl.value;
+
+	clk_termen = atomisp_csi2_configure_calc(coeff_clk_termen, mipi_freq,
+						 TERMEN_DEFAULT);
+	clk_settle = atomisp_csi2_configure_calc(coeff_clk_settle, mipi_freq,
+						 SETTLE_DEFAULT);
+	dat_termen = atomisp_csi2_configure_calc(coeff_dat_termen, mipi_freq,
+						 TERMEN_DEFAULT);
+	dat_settle = atomisp_csi2_configure_calc(coeff_dat_settle, mipi_freq,
+						 SETTLE_DEFAULT);
+
+	for (n = 0; n < csi2_port_lanes[port] + 1; n++) {
+		hrt_address base = csi2_port_base[port] + csi2_lane_base[n];
+
+		atomisp_css2_hw_store_32(base + CSI2_REG_RX_CSI_DLY_CNT_TERMEN,
+					 n == 0 ? clk_termen : dat_termen);
+		atomisp_css2_hw_store_32(base + CSI2_REG_RX_CSI_DLY_CNT_SETTLE,
+					 n == 0 ? clk_settle : dat_settle);
+	}
+}
+
+void atomisp_csi2_configure(struct atomisp_sub_device *asd)
+{
+	if (IS_HWREVISION(asd->isp, ATOMISP_HW_REVISION_ISP2401))
+		atomisp_csi2_configure_isp2401(asd);
+}
+
+/*
+ * atomisp_mipi_csi2_cleanup - Routine for module driver cleanup
+ */
+void atomisp_mipi_csi2_cleanup(struct atomisp_device *isp)
+{
+}
+
+int atomisp_mipi_csi2_init(struct atomisp_device *isp)
+{
+	struct atomisp_mipi_csi2_device *csi2_port;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++) {
+		csi2_port = &isp->csi2_port[i];
+		csi2_port->isp = isp;
+		ret = mipi_csi2_init_entities(csi2_port, i);
+		if (ret < 0)
+			goto fail;
+	}
+
+	return 0;
+
+fail:
+	atomisp_mipi_csi2_cleanup(isp);
+	return ret;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_csi2.h b/drivers/staging/media/atomisp/pci/atomisp_csi2.h
new file mode 100644
index 000000000..e35711be8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_csi2.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef __ATOMISP_CSI2_H__
+#define __ATOMISP_CSI2_H__
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+
+#define CSI2_PAD_SINK		0
+#define CSI2_PAD_SOURCE		1
+#define CSI2_PADS_NUM		2
+
+#define CSI2_OUTPUT_ISP_SUBDEV	BIT(0)
+#define CSI2_OUTPUT_MEMORY	BIT(1)
+
+struct atomisp_device;
+struct v4l2_device;
+struct atomisp_sub_device;
+
+struct atomisp_mipi_csi2_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[CSI2_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[CSI2_PADS_NUM];
+
+	struct v4l2_ctrl_handler ctrls;
+	struct atomisp_device *isp;
+
+	u32 output; /* output direction */
+};
+
+int atomisp_csi2_set_ffmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  unsigned int which, uint16_t pad,
+			  struct v4l2_mbus_framefmt *ffmt);
+int atomisp_mipi_csi2_init(struct atomisp_device *isp);
+void atomisp_mipi_csi2_cleanup(struct atomisp_device *isp);
+void atomisp_mipi_csi2_unregister_entities(
+    struct atomisp_mipi_csi2_device *csi2);
+int atomisp_mipi_csi2_register_entities(struct atomisp_mipi_csi2_device *csi2,
+					struct v4l2_device *vdev);
+
+void atomisp_csi2_configure(struct atomisp_sub_device *asd);
+
+#endif /* __ATOMISP_CSI2_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_dfs_tables.h b/drivers/staging/media/atomisp/pci/atomisp_dfs_tables.h
new file mode 100644
index 000000000..8f1cc3fca
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_dfs_tables.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef	__ATOMISP_DFS_TABLES_H__
+#define	__ATOMISP_DFS_TABLES_H__
+
+#include <linux/kernel.h>
+
+struct atomisp_freq_scaling_rule {
+	unsigned int width;
+	unsigned int height;
+	unsigned short fps;
+	unsigned int isp_freq;
+	unsigned int run_mode;
+};
+
+struct atomisp_dfs_config {
+	unsigned int lowest_freq;
+	unsigned int max_freq_at_vmin;
+	unsigned int highest_freq;
+	const struct atomisp_freq_scaling_rule *dfs_table;
+	unsigned int dfs_table_size;
+};
+
+extern const struct atomisp_dfs_config dfs_config_cht_soc;
+
+#endif /* __ATOMISP_DFS_TABLES_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_drvfs.c b/drivers/staging/media/atomisp/pci/atomisp_drvfs.c
new file mode 100644
index 000000000..3ddc935ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_drvfs.c
@@ -0,0 +1,202 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for atomisp driver sysfs interface
+ *
+ * Copyright (c) 2014 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/device.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+
+#include "atomisp_compat.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+#include "atomisp_drvfs.h"
+#include "hmm/hmm.h"
+#include "ia_css_debug.h"
+
+/*
+ * _iunit_debug:
+ * dbglvl: iunit css driver trace level
+ * dbgopt: iunit debug option:
+ *        bit 0: binary list
+ *        bit 1: running binary
+ *        bit 2: memory statistic
+*/
+struct _iunit_debug {
+	struct device_driver	*drv;
+	struct atomisp_device	*isp;
+	unsigned int		dbglvl;
+	unsigned int		dbgfun;
+	unsigned int		dbgopt;
+};
+
+#define OPTION_BIN_LIST			BIT(0)
+#define OPTION_BIN_RUN			BIT(1)
+#define OPTION_VALID			(OPTION_BIN_LIST \
+					| OPTION_BIN_RUN)
+
+static struct _iunit_debug iunit_debug = {
+	.dbglvl = 0,
+	.dbgopt = OPTION_BIN_LIST,
+};
+
+static inline int iunit_dump_dbgopt(struct atomisp_device *isp,
+				    unsigned int opt)
+{
+	int ret = 0;
+
+	if (opt & OPTION_VALID) {
+		if (opt & OPTION_BIN_LIST) {
+			ret = atomisp_css_dump_blob_infor(isp);
+			if (ret) {
+				dev_err(isp->dev, "%s dump blob infor err[ret:%d]\n",
+					__func__, ret);
+				goto opt_err;
+			}
+		}
+
+		if (opt & OPTION_BIN_RUN) {
+			if (atomisp_streaming_count(isp)) {
+				atomisp_css_dump_sp_raw_copy_linecount(true);
+				atomisp_css_debug_dump_isp_binary();
+			} else {
+				ret = -EPERM;
+				dev_err(isp->dev, "%s dump running bin err[ret:%d]\n",
+					__func__, ret);
+				goto opt_err;
+			}
+		}
+	} else {
+		ret = -EINVAL;
+		dev_err(isp->dev, "%s dump nothing[ret=%d]\n", __func__, ret);
+	}
+
+opt_err:
+	return ret;
+}
+
+static ssize_t iunit_dbglvl_show(struct device_driver *drv, char *buf)
+{
+	iunit_debug.dbglvl = dbg_level;
+	return sysfs_emit(buf, "dtrace level:%u\n", iunit_debug.dbglvl);
+}
+
+static ssize_t iunit_dbglvl_store(struct device_driver *drv, const char *buf,
+				  size_t size)
+{
+	if (kstrtouint(buf, 10, &iunit_debug.dbglvl)
+	    || iunit_debug.dbglvl < 1
+	    || iunit_debug.dbglvl > 9) {
+		return -ERANGE;
+	}
+	ia_css_debug_set_dtrace_level(iunit_debug.dbglvl);
+
+	return size;
+}
+
+static ssize_t iunit_dbgfun_show(struct device_driver *drv, char *buf)
+{
+	iunit_debug.dbgfun = atomisp_get_css_dbgfunc();
+	return sysfs_emit(buf, "dbgfun opt:%u\n", iunit_debug.dbgfun);
+}
+
+static ssize_t iunit_dbgfun_store(struct device_driver *drv, const char *buf,
+				  size_t size)
+{
+	unsigned int opt;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &opt);
+	if (ret)
+		return ret;
+
+	ret = atomisp_set_css_dbgfunc(iunit_debug.isp, opt);
+	if (ret)
+		return ret;
+
+	iunit_debug.dbgfun = opt;
+
+	return size;
+}
+
+static ssize_t iunit_dbgopt_show(struct device_driver *drv, char *buf)
+{
+	return sysfs_emit(buf, "option:0x%x\n", iunit_debug.dbgopt);
+}
+
+static ssize_t iunit_dbgopt_store(struct device_driver *drv, const char *buf,
+				  size_t size)
+{
+	unsigned int opt;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &opt);
+	if (ret)
+		return ret;
+
+	iunit_debug.dbgopt = opt;
+	ret = iunit_dump_dbgopt(iunit_debug.isp, iunit_debug.dbgopt);
+	if (ret)
+		return ret;
+
+	return size;
+}
+
+static const struct driver_attribute iunit_drvfs_attrs[] = {
+	__ATTR(dbglvl, 0644, iunit_dbglvl_show, iunit_dbglvl_store),
+	__ATTR(dbgfun, 0644, iunit_dbgfun_show, iunit_dbgfun_store),
+	__ATTR(dbgopt, 0644, iunit_dbgopt_show, iunit_dbgopt_store),
+};
+
+static int iunit_drvfs_create_files(struct device_driver *drv)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(iunit_drvfs_attrs); i++)
+		ret |= driver_create_file(drv, &iunit_drvfs_attrs[i]);
+
+	return ret;
+}
+
+static void iunit_drvfs_remove_files(struct device_driver *drv)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(iunit_drvfs_attrs); i++)
+		driver_remove_file(drv, &iunit_drvfs_attrs[i]);
+}
+
+int atomisp_drvfs_init(struct atomisp_device *isp)
+{
+	struct device_driver *drv = isp->dev->driver;
+	int ret;
+
+	iunit_debug.isp = isp;
+	iunit_debug.drv = drv;
+
+	ret = iunit_drvfs_create_files(iunit_debug.drv);
+	if (ret) {
+		dev_err(isp->dev, "drvfs_create_files error: %d\n", ret);
+		iunit_drvfs_remove_files(iunit_debug.drv);
+	}
+
+	return ret;
+}
+
+void atomisp_drvfs_exit(void)
+{
+	iunit_drvfs_remove_files(iunit_debug.drv);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_drvfs.h b/drivers/staging/media/atomisp/pci/atomisp_drvfs.h
new file mode 100644
index 000000000..8f4cc722b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_drvfs.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for atomisp driver sysfs interface.
+ *
+ * Copyright (c) 2014 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__ATOMISP_DRVFS_H__
+#define	__ATOMISP_DRVFS_H__
+
+int atomisp_drvfs_init(struct atomisp_device *isp);
+void atomisp_drvfs_exit(void);
+
+#endif /* __ATOMISP_DRVFS_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_fops.c b/drivers/staging/media/atomisp/pci/atomisp_fops.c
new file mode 100644
index 000000000..5fa2e2596
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_fops.c
@@ -0,0 +1,1128 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf-vmalloc.h>
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_fops.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+#include "atomisp_compat.h"
+#include "atomisp_subdev.h"
+#include "atomisp_v4l2.h"
+#include "atomisp-regs.h"
+#include "hmm/hmm.h"
+
+#include "type_support.h"
+#include "device_access/device_access.h"
+
+#define ISP_LEFT_PAD			128	/* equal to 2*NWAY */
+
+/*
+ * input image data, and current frame resolution for test
+ */
+#define	ISP_PARAM_MMAP_OFFSET	0xfffff000
+
+#define MAGIC_CHECK(is, should)	\
+	do { \
+		if (unlikely((is) != (should))) { \
+			pr_err("magic mismatch: %x (expected %x)\n", \
+				is, should); \
+			BUG(); \
+		} \
+	} while (0)
+
+/*
+ * Videobuf ops
+ */
+static int atomisp_buf_setup(struct videobuf_queue *vq, unsigned int *count,
+			     unsigned int *size)
+{
+	struct atomisp_video_pipe *pipe = vq->priv_data;
+
+	*size = pipe->pix.sizeimage;
+
+	return 0;
+}
+
+static int atomisp_buf_prepare(struct videobuf_queue *vq,
+			       struct videobuf_buffer *vb,
+			       enum v4l2_field field)
+{
+	struct atomisp_video_pipe *pipe = vq->priv_data;
+
+	vb->size = pipe->pix.sizeimage;
+	vb->width = pipe->pix.width;
+	vb->height = pipe->pix.height;
+	vb->field = field;
+	vb->state = VIDEOBUF_PREPARED;
+
+	return 0;
+}
+
+static int atomisp_q_one_metadata_buffer(struct atomisp_sub_device *asd,
+	enum atomisp_input_stream_id stream_id,
+	enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_metadata_buf *metadata_buf;
+	enum atomisp_metadata_type md_type =
+	    atomisp_get_metadata_type(asd, css_pipe_id);
+	struct list_head *metadata_list;
+
+	if (asd->metadata_bufs_in_css[stream_id][css_pipe_id] >=
+	    ATOMISP_CSS_Q_DEPTH)
+		return 0; /* we have reached CSS queue depth */
+
+	if (!list_empty(&asd->metadata[md_type])) {
+		metadata_list = &asd->metadata[md_type];
+	} else if (!list_empty(&asd->metadata_ready[md_type])) {
+		metadata_list = &asd->metadata_ready[md_type];
+	} else {
+		dev_warn(asd->isp->dev, "%s: No metadata buffers available for type %d!\n",
+			 __func__, md_type);
+		return -EINVAL;
+	}
+
+	metadata_buf = list_entry(metadata_list->next,
+				  struct atomisp_metadata_buf, list);
+	list_del_init(&metadata_buf->list);
+
+	if (atomisp_q_metadata_buffer_to_css(asd, metadata_buf,
+					     stream_id, css_pipe_id)) {
+		list_add(&metadata_buf->list, metadata_list);
+		return -EINVAL;
+	} else {
+		list_add_tail(&metadata_buf->list,
+			      &asd->metadata_in_css[md_type]);
+	}
+	asd->metadata_bufs_in_css[stream_id][css_pipe_id]++;
+
+	return 0;
+}
+
+static int atomisp_q_one_s3a_buffer(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_s3a_buf *s3a_buf;
+	struct list_head *s3a_list;
+	unsigned int exp_id;
+
+	if (asd->s3a_bufs_in_css[css_pipe_id] >= ATOMISP_CSS_Q_DEPTH)
+		return 0; /* we have reached CSS queue depth */
+
+	if (!list_empty(&asd->s3a_stats)) {
+		s3a_list = &asd->s3a_stats;
+	} else if (!list_empty(&asd->s3a_stats_ready)) {
+		s3a_list = &asd->s3a_stats_ready;
+	} else {
+		dev_warn(asd->isp->dev, "%s: No s3a buffers available!\n",
+			 __func__);
+		return -EINVAL;
+	}
+
+	s3a_buf = list_entry(s3a_list->next, struct atomisp_s3a_buf, list);
+	list_del_init(&s3a_buf->list);
+	exp_id = s3a_buf->s3a_data->exp_id;
+
+	hmm_flush_vmap(s3a_buf->s3a_data->data_ptr);
+	if (atomisp_q_s3a_buffer_to_css(asd, s3a_buf,
+					stream_id, css_pipe_id)) {
+		/* got from head, so return back to the head */
+		list_add(&s3a_buf->list, s3a_list);
+		return -EINVAL;
+	} else {
+		list_add_tail(&s3a_buf->list, &asd->s3a_stats_in_css);
+		if (s3a_list == &asd->s3a_stats_ready)
+			dev_warn(asd->isp->dev, "%s: drop one s3a stat which has exp_id %d!\n",
+				 __func__, exp_id);
+	}
+
+	asd->s3a_bufs_in_css[css_pipe_id]++;
+	return 0;
+}
+
+static int atomisp_q_one_dis_buffer(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum ia_css_pipe_id css_pipe_id)
+{
+	struct atomisp_dis_buf *dis_buf;
+	unsigned long irqflags;
+
+	if (asd->dis_bufs_in_css >=  ATOMISP_CSS_Q_DEPTH)
+		return 0; /* we have reached CSS queue depth */
+
+	spin_lock_irqsave(&asd->dis_stats_lock, irqflags);
+	if (list_empty(&asd->dis_stats)) {
+		spin_unlock_irqrestore(&asd->dis_stats_lock, irqflags);
+		dev_warn(asd->isp->dev, "%s: No dis buffers available!\n",
+			 __func__);
+		return -EINVAL;
+	}
+
+	dis_buf = list_entry(asd->dis_stats.prev,
+			     struct atomisp_dis_buf, list);
+	list_del_init(&dis_buf->list);
+	spin_unlock_irqrestore(&asd->dis_stats_lock, irqflags);
+
+	hmm_flush_vmap(dis_buf->dis_data->data_ptr);
+	if (atomisp_q_dis_buffer_to_css(asd, dis_buf,
+					stream_id, css_pipe_id)) {
+		spin_lock_irqsave(&asd->dis_stats_lock, irqflags);
+		/* got from tail, so return back to the tail */
+		list_add_tail(&dis_buf->list, &asd->dis_stats);
+		spin_unlock_irqrestore(&asd->dis_stats_lock, irqflags);
+		return -EINVAL;
+	} else {
+		spin_lock_irqsave(&asd->dis_stats_lock, irqflags);
+		list_add_tail(&dis_buf->list, &asd->dis_stats_in_css);
+		spin_unlock_irqrestore(&asd->dis_stats_lock, irqflags);
+	}
+
+	asd->dis_bufs_in_css++;
+
+	return 0;
+}
+
+int atomisp_q_video_buffers_to_css(struct atomisp_sub_device *asd,
+				   struct atomisp_video_pipe *pipe,
+				   enum atomisp_input_stream_id stream_id,
+				   enum ia_css_buffer_type css_buf_type,
+				   enum ia_css_pipe_id css_pipe_id)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct atomisp_css_params_with_list *param;
+	struct ia_css_dvs_grid_info *dvs_grid =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	unsigned long irqflags;
+	int err = 0;
+
+	if (WARN_ON(css_pipe_id >= IA_CSS_PIPE_ID_NUM))
+		return -EINVAL;
+
+	while (pipe->buffers_in_css < ATOMISP_CSS_Q_DEPTH) {
+		struct videobuf_buffer *vb;
+
+		spin_lock_irqsave(&pipe->irq_lock, irqflags);
+		if (list_empty(&pipe->activeq)) {
+			spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+			return -EINVAL;
+		}
+		vb = list_entry(pipe->activeq.next,
+				struct videobuf_buffer, queue);
+		list_del_init(&vb->queue);
+		vb->state = VIDEOBUF_ACTIVE;
+		spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+
+		/*
+		 * If there is a per_frame setting to apply on the buffer,
+		 * do it before buffer en-queueing.
+		 */
+		vm_mem = vb->priv;
+
+		param = pipe->frame_params[vb->i];
+		if (param) {
+			atomisp_makeup_css_parameters(asd,
+						      &asd->params.css_param.update_flag,
+						      &param->params);
+			atomisp_apply_css_parameters(asd, &param->params);
+
+			if (param->params.update_flag.dz_config &&
+			    asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
+				err = atomisp_calculate_real_zoom_region(asd,
+					&param->params.dz_config, css_pipe_id);
+				if (!err)
+					asd->params.config.dz_config = &param->params.dz_config;
+			}
+			atomisp_css_set_isp_config_applied_frame(asd,
+				vm_mem->vaddr);
+			atomisp_css_update_isp_params_on_pipe(asd,
+							      asd->stream_env[stream_id].pipes[css_pipe_id]);
+			asd->params.dvs_6axis = (struct ia_css_dvs_6axis_config *)
+						param->params.dvs_6axis;
+
+			/*
+			 * WORKAROUND:
+			 * Because the camera halv3 can't ensure to set zoom
+			 * region to per_frame setting and global setting at
+			 * same time and only set zoom region to pre_frame
+			 * setting now.so when the pre_frame setting include
+			 * zoom region,I will set it to global setting.
+			 */
+			if (param->params.update_flag.dz_config &&
+			    asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO
+			    && !err) {
+				memcpy(&asd->params.css_param.dz_config,
+				       &param->params.dz_config,
+				       sizeof(struct ia_css_dz_config));
+				asd->params.css_param.update_flag.dz_config =
+				    (struct atomisp_dz_config *)
+				    &asd->params.css_param.dz_config;
+				asd->params.css_update_params_needed = true;
+			}
+		}
+		/* Enqueue buffer */
+		err = atomisp_q_video_buffer_to_css(asd, vm_mem, stream_id,
+						    css_buf_type, css_pipe_id);
+		if (err) {
+			spin_lock_irqsave(&pipe->irq_lock, irqflags);
+			list_add_tail(&vb->queue, &pipe->activeq);
+			vb->state = VIDEOBUF_QUEUED;
+			spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+			dev_err(asd->isp->dev, "%s, css q fails: %d\n",
+				__func__, err);
+			return -EINVAL;
+		}
+		pipe->buffers_in_css++;
+
+		/* enqueue 3A/DIS/metadata buffers */
+		if (asd->params.curr_grid_info.s3a_grid.enable &&
+		    css_pipe_id == asd->params.s3a_enabled_pipe &&
+		    css_buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME)
+			atomisp_q_one_s3a_buffer(asd, stream_id,
+						 css_pipe_id);
+
+		if (asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
+		    metadata_info.size &&
+		    css_buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME)
+			atomisp_q_one_metadata_buffer(asd, stream_id,
+						      css_pipe_id);
+
+		if (dvs_grid && dvs_grid->enable &&
+		    css_pipe_id == IA_CSS_PIPE_ID_VIDEO &&
+		    css_buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME)
+			atomisp_q_one_dis_buffer(asd, stream_id,
+						 css_pipe_id);
+	}
+
+	return 0;
+}
+
+static int atomisp_get_css_buf_type(struct atomisp_sub_device *asd,
+				    enum ia_css_pipe_id pipe_id,
+				    uint16_t source_pad)
+{
+	if (ATOMISP_USE_YUVPP(asd)) {
+		/* when run ZSL case */
+		if (asd->continuous_mode->val &&
+		    asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
+			if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE)
+				return IA_CSS_BUFFER_TYPE_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
+				return IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME;
+			else
+				return IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+		}
+
+		/*when run SDV case*/
+		if (asd->continuous_mode->val &&
+		    asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+			if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE)
+				return IA_CSS_BUFFER_TYPE_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
+				return IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO)
+				return IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME;
+			else
+				return IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+		}
+
+		/*other case: default setting*/
+		if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE ||
+		    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO ||
+		    (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW &&
+		     asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO))
+			return IA_CSS_BUFFER_TYPE_OUTPUT_FRAME;
+		else
+			return IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+	}
+
+	if (pipe_id == IA_CSS_PIPE_ID_COPY ||
+	    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE ||
+	    source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO ||
+	    (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW &&
+	     asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO))
+		return IA_CSS_BUFFER_TYPE_OUTPUT_FRAME;
+	else
+		return IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+}
+
+/* queue all available buffers to css */
+int atomisp_qbuffers_to_css(struct atomisp_sub_device *asd)
+{
+	enum ia_css_buffer_type buf_type;
+	enum ia_css_pipe_id css_capture_pipe_id = IA_CSS_PIPE_ID_NUM;
+	enum ia_css_pipe_id css_preview_pipe_id = IA_CSS_PIPE_ID_NUM;
+	enum ia_css_pipe_id css_video_pipe_id = IA_CSS_PIPE_ID_NUM;
+	enum atomisp_input_stream_id input_stream_id;
+	struct atomisp_video_pipe *capture_pipe = NULL;
+	struct atomisp_video_pipe *vf_pipe = NULL;
+	struct atomisp_video_pipe *preview_pipe = NULL;
+	struct atomisp_video_pipe *video_pipe = NULL;
+	bool raw_mode = atomisp_is_mbuscode_raw(
+			    asd->fmt[asd->capture_pad].fmt.code);
+
+	if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+		video_pipe = &asd->video_out_video_capture;
+		css_video_pipe_id = IA_CSS_PIPE_ID_VIDEO;
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+		preview_pipe = &asd->video_out_capture;
+		css_preview_pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+		if (asd->continuous_mode->val) {
+			capture_pipe = &asd->video_out_capture;
+			vf_pipe = &asd->video_out_vf;
+			css_capture_pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+		}
+		video_pipe = &asd->video_out_video_capture;
+		preview_pipe = &asd->video_out_preview;
+		css_video_pipe_id = IA_CSS_PIPE_ID_VIDEO;
+		css_preview_pipe_id = IA_CSS_PIPE_ID_VIDEO;
+	} else if (asd->continuous_mode->val) {
+		capture_pipe = &asd->video_out_capture;
+		vf_pipe = &asd->video_out_vf;
+		preview_pipe = &asd->video_out_preview;
+
+		css_preview_pipe_id = IA_CSS_PIPE_ID_PREVIEW;
+		css_capture_pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
+		preview_pipe = &asd->video_out_preview;
+		css_preview_pipe_id = IA_CSS_PIPE_ID_PREVIEW;
+	} else {
+		/* ATOMISP_RUN_MODE_STILL_CAPTURE */
+		capture_pipe = &asd->video_out_capture;
+		if (!raw_mode)
+			vf_pipe = &asd->video_out_vf;
+		css_capture_pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+	}
+
+	if (IS_ISP2401 && asd->copy_mode) {
+		css_capture_pipe_id = IA_CSS_PIPE_ID_COPY;
+		css_preview_pipe_id = IA_CSS_PIPE_ID_COPY;
+		css_video_pipe_id = IA_CSS_PIPE_ID_COPY;
+	}
+
+	if (asd->yuvpp_mode) {
+		capture_pipe = &asd->video_out_capture;
+		video_pipe   = &asd->video_out_video_capture;
+		preview_pipe = &asd->video_out_preview;
+		css_capture_pipe_id = IA_CSS_PIPE_ID_COPY;
+		css_video_pipe_id   = IA_CSS_PIPE_ID_YUVPP;
+		css_preview_pipe_id = IA_CSS_PIPE_ID_YUVPP;
+	}
+
+	if (capture_pipe) {
+		buf_type = atomisp_get_css_buf_type(
+			       asd, css_capture_pipe_id,
+			       atomisp_subdev_source_pad(&capture_pipe->vdev));
+		input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+
+		/*
+		 * use yuvpp pipe for SOC camera.
+		 */
+		if (ATOMISP_USE_YUVPP(asd))
+			css_capture_pipe_id = IA_CSS_PIPE_ID_YUVPP;
+
+		atomisp_q_video_buffers_to_css(asd, capture_pipe,
+					       input_stream_id,
+					       buf_type, css_capture_pipe_id);
+	}
+
+	if (vf_pipe) {
+		buf_type = atomisp_get_css_buf_type(
+			       asd, css_capture_pipe_id,
+			       atomisp_subdev_source_pad(&vf_pipe->vdev));
+		if (asd->stream_env[ATOMISP_INPUT_STREAM_POSTVIEW].stream)
+			input_stream_id = ATOMISP_INPUT_STREAM_POSTVIEW;
+		else
+			input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+
+		/*
+		 * use yuvpp pipe for SOC camera.
+		 */
+		if (ATOMISP_USE_YUVPP(asd))
+			css_capture_pipe_id = IA_CSS_PIPE_ID_YUVPP;
+		atomisp_q_video_buffers_to_css(asd, vf_pipe,
+					       input_stream_id,
+					       buf_type, css_capture_pipe_id);
+	}
+
+	if (preview_pipe) {
+		buf_type = atomisp_get_css_buf_type(
+			       asd, css_preview_pipe_id,
+			       atomisp_subdev_source_pad(&preview_pipe->vdev));
+		if (ATOMISP_SOC_CAMERA(asd) && css_preview_pipe_id == IA_CSS_PIPE_ID_YUVPP)
+			input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+		/* else for ext isp use case */
+		else if (css_preview_pipe_id == IA_CSS_PIPE_ID_YUVPP)
+			input_stream_id = ATOMISP_INPUT_STREAM_VIDEO;
+		else if (asd->stream_env[ATOMISP_INPUT_STREAM_PREVIEW].stream)
+			input_stream_id = ATOMISP_INPUT_STREAM_PREVIEW;
+		else
+			input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+
+		/*
+		 * use yuvpp pipe for SOC camera.
+		 */
+		if (ATOMISP_USE_YUVPP(asd))
+			css_preview_pipe_id = IA_CSS_PIPE_ID_YUVPP;
+
+		atomisp_q_video_buffers_to_css(asd, preview_pipe,
+					       input_stream_id,
+					       buf_type, css_preview_pipe_id);
+	}
+
+	if (video_pipe) {
+		buf_type = atomisp_get_css_buf_type(
+			       asd, css_video_pipe_id,
+			       atomisp_subdev_source_pad(&video_pipe->vdev));
+		if (asd->stream_env[ATOMISP_INPUT_STREAM_VIDEO].stream)
+			input_stream_id = ATOMISP_INPUT_STREAM_VIDEO;
+		else
+			input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+
+		/*
+		 * use yuvpp pipe for SOC camera.
+		 */
+		if (ATOMISP_USE_YUVPP(asd))
+			css_video_pipe_id = IA_CSS_PIPE_ID_YUVPP;
+
+		atomisp_q_video_buffers_to_css(asd, video_pipe,
+					       input_stream_id,
+					       buf_type, css_video_pipe_id);
+	}
+
+	return 0;
+}
+
+static void atomisp_buf_queue(struct videobuf_queue *vq,
+			      struct videobuf_buffer *vb)
+{
+	struct atomisp_video_pipe *pipe = vq->priv_data;
+
+	/*
+	 * when a frame buffer meets following conditions, it should be put into
+	 * the waiting list:
+	 * 1.  It is not a main output frame, and it has a per-frame parameter
+	 *     to go with it.
+	 * 2.  It is not a main output frame, and the waiting buffer list is not
+	 *     empty, to keep the FIFO sequence of frame buffer processing, it
+	 *     is put to waiting list until previous per-frame parameter buffers
+	 *     get enqueued.
+	 */
+	if (!atomisp_is_vf_pipe(pipe) &&
+	    (pipe->frame_request_config_id[vb->i] ||
+	     !list_empty(&pipe->buffers_waiting_for_param)))
+		list_add_tail(&vb->queue, &pipe->buffers_waiting_for_param);
+	else
+		list_add_tail(&vb->queue, &pipe->activeq);
+
+	vb->state = VIDEOBUF_QUEUED;
+}
+
+static void atomisp_buf_release(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb)
+{
+	vb->state = VIDEOBUF_NEEDS_INIT;
+	atomisp_videobuf_free_buf(vb);
+}
+
+static const struct videobuf_queue_ops videobuf_qops = {
+	.buf_setup	= atomisp_buf_setup,
+	.buf_prepare	= atomisp_buf_prepare,
+	.buf_queue	= atomisp_buf_queue,
+	.buf_release	= atomisp_buf_release,
+};
+
+static int atomisp_init_pipe(struct atomisp_video_pipe *pipe)
+{
+	/* init locks */
+	spin_lock_init(&pipe->irq_lock);
+
+	videobuf_queue_vmalloc_init(&pipe->capq, &videobuf_qops, NULL,
+				    &pipe->irq_lock,
+				    V4L2_BUF_TYPE_VIDEO_CAPTURE,
+				    V4L2_FIELD_NONE,
+				    sizeof(struct atomisp_buffer), pipe,
+				    NULL);	/* ext_lock: NULL */
+
+	INIT_LIST_HEAD(&pipe->activeq);
+	INIT_LIST_HEAD(&pipe->buffers_waiting_for_param);
+	INIT_LIST_HEAD(&pipe->per_frame_params);
+	memset(pipe->frame_request_config_id, 0,
+	       VIDEO_MAX_FRAME * sizeof(unsigned int));
+	memset(pipe->frame_params, 0,
+	       VIDEO_MAX_FRAME *
+	       sizeof(struct atomisp_css_params_with_list *));
+
+	return 0;
+}
+
+static void atomisp_dev_init_struct(struct atomisp_device *isp)
+{
+	unsigned int i;
+
+	isp->need_gfx_throttle = true;
+	isp->isp_fatal_error = false;
+	isp->mipi_frame_size = 0;
+
+	for (i = 0; i < isp->input_cnt; i++)
+		isp->inputs[i].asd = NULL;
+	/*
+	 * For Merrifield, frequency is scalable.
+	 * After boot-up, the default frequency is 200MHz.
+	 */
+	isp->sw_contex.running_freq = ISP_FREQ_200MHZ;
+}
+
+static void atomisp_subdev_init_struct(struct atomisp_sub_device *asd)
+{
+	v4l2_ctrl_s_ctrl(asd->run_mode, ATOMISP_RUN_MODE_STILL_CAPTURE);
+	memset(&asd->params.css_param, 0, sizeof(asd->params.css_param));
+	asd->params.color_effect = V4L2_COLORFX_NONE;
+	asd->params.bad_pixel_en = true;
+	asd->params.gdc_cac_en = false;
+	asd->params.video_dis_en = false;
+	asd->params.sc_en = false;
+	asd->params.fpn_en = false;
+	asd->params.xnr_en = false;
+	asd->params.false_color = 0;
+	asd->params.online_process = 1;
+	asd->params.yuv_ds_en = 0;
+	/* s3a grid not enabled for any pipe */
+	asd->params.s3a_enabled_pipe = IA_CSS_PIPE_ID_NUM;
+
+	asd->params.offline_parm.num_captures = 1;
+	asd->params.offline_parm.skip_frames = 0;
+	asd->params.offline_parm.offset = 0;
+	asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
+	/* Add for channel */
+	asd->input_curr = 0;
+
+	asd->mipi_frame_size = 0;
+	asd->copy_mode = false;
+	asd->yuvpp_mode = false;
+
+	asd->stream_prepared = false;
+	asd->high_speed_mode = false;
+	asd->sensor_array_res.height = 0;
+	asd->sensor_array_res.width = 0;
+	atomisp_css_init_struct(asd);
+}
+
+/*
+ * file operation functions
+ */
+static unsigned int atomisp_subdev_users(struct atomisp_sub_device *asd)
+{
+	return asd->video_out_preview.users +
+	       asd->video_out_vf.users +
+	       asd->video_out_capture.users +
+	       asd->video_out_video_capture.users;
+}
+
+unsigned int atomisp_dev_users(struct atomisp_device *isp)
+{
+	unsigned int i, sum;
+
+	for (i = 0, sum = 0; i < isp->num_of_streams; i++)
+		sum += atomisp_subdev_users(&isp->asd[i]);
+
+	return sum;
+}
+
+static int atomisp_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	int ret;
+
+	dev_dbg(isp->dev, "open device %s\n", vdev->name);
+
+	ret = v4l2_fh_open(file);
+	if (ret)
+		return ret;
+
+	mutex_lock(&isp->mutex);
+
+	asd->subdev.devnode = vdev;
+	/* Deferred firmware loading case. */
+	if (isp->css_env.isp_css_fw.bytes == 0) {
+		dev_err(isp->dev, "Deferred firmware load.\n");
+		isp->firmware = atomisp_load_firmware(isp);
+		if (!isp->firmware) {
+			dev_err(isp->dev, "Failed to load ISP firmware.\n");
+			ret = -ENOENT;
+			goto error;
+		}
+		ret = atomisp_css_load_firmware(isp);
+		if (ret) {
+			dev_err(isp->dev, "Failed to init css.\n");
+			goto error;
+		}
+		/* No need to keep FW in memory anymore. */
+		release_firmware(isp->firmware);
+		isp->firmware = NULL;
+		isp->css_env.isp_css_fw.data = NULL;
+	}
+
+	if (!isp->input_cnt) {
+		dev_err(isp->dev, "no camera attached\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * atomisp does not allow multiple open
+	 */
+	if (pipe->users) {
+		dev_dbg(isp->dev, "video node already opened\n");
+		mutex_unlock(&isp->mutex);
+		return -EBUSY;
+	}
+
+	ret = atomisp_init_pipe(pipe);
+	if (ret)
+		goto error;
+
+	if (atomisp_dev_users(isp)) {
+		dev_dbg(isp->dev, "skip init isp in open\n");
+		goto init_subdev;
+	}
+
+	/* runtime power management, turn on ISP */
+	ret = pm_runtime_resume_and_get(vdev->v4l2_dev->dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "Failed to power on device\n");
+		goto error;
+	}
+
+	/* Init ISP */
+	if (atomisp_css_init(isp)) {
+		ret = -EINVAL;
+		/* Need to clean up CSS init if it fails. */
+		goto css_error;
+	}
+
+	atomisp_dev_init_struct(isp);
+
+	ret = v4l2_subdev_call(isp->flash, core, s_power, 1);
+	if (ret < 0 && ret != -ENODEV && ret != -ENOIOCTLCMD) {
+		dev_err(isp->dev, "Failed to power-on flash\n");
+		goto css_error;
+	}
+
+init_subdev:
+	if (atomisp_subdev_users(asd))
+		goto done;
+
+	atomisp_subdev_init_struct(asd);
+	/* Ensure that a mode is set */
+	v4l2_ctrl_s_ctrl(asd->run_mode, pipe->default_run_mode);
+
+done:
+	pipe->users++;
+	mutex_unlock(&isp->mutex);
+
+
+	return 0;
+
+css_error:
+	atomisp_css_uninit(isp);
+	pm_runtime_put(vdev->v4l2_dev->dev);
+error:
+	mutex_unlock(&isp->mutex);
+	v4l2_fh_release(file);
+	return ret;
+}
+
+static int atomisp_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct v4l2_requestbuffers req;
+	struct v4l2_subdev_fh fh;
+	struct v4l2_rect clear_compose = {0};
+	unsigned long flags;
+	int ret = 0;
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	req.count = 0;
+	if (!isp)
+		return -EBADF;
+
+	mutex_lock(&isp->mutex);
+
+	dev_dbg(isp->dev, "release device %s\n", vdev->name);
+
+	asd->subdev.devnode = vdev;
+
+	pipe->users--;
+
+	if (pipe->capq.streaming)
+		dev_warn(isp->dev,
+			 "%s: ISP still streaming while closing!",
+			 __func__);
+
+	if (pipe->capq.streaming &&
+	    atomisp_streamoff(file, NULL, V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_err(isp->dev, "atomisp_streamoff failed on release, driver bug");
+		goto done;
+	}
+
+	if (pipe->users)
+		goto done;
+
+	if (atomisp_reqbufs(file, NULL, &req)) {
+		dev_err(isp->dev, "atomisp_reqbufs failed on release, driver bug");
+		goto done;
+	}
+
+	/*
+	 * A little trick here:
+	 * file injection input resolution is recorded in the sink pad,
+	 * therefore can not be cleared when releaseing one device node.
+	 * The sink pad setting can only be cleared when all device nodes
+	 * get released.
+	 */
+	if (asd->fmt_auto->val) {
+		struct v4l2_mbus_framefmt isp_sink_fmt = { 0 };
+
+		atomisp_subdev_set_ffmt(&asd->subdev, fh.state,
+					V4L2_SUBDEV_FORMAT_ACTIVE,
+					ATOMISP_SUBDEV_PAD_SINK, &isp_sink_fmt);
+	}
+
+	if (atomisp_subdev_users(asd))
+		goto done;
+
+	atomisp_css_free_stat_buffers(asd);
+	atomisp_free_internal_buffers(asd);
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       core, s_power, 0);
+	if (ret)
+		dev_warn(isp->dev, "Failed to power-off sensor\n");
+
+	/* clear the asd field to show this camera is not used */
+	isp->inputs[asd->input_curr].asd = NULL;
+	spin_lock_irqsave(&isp->lock, flags);
+	asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	if (atomisp_dev_users(isp))
+		goto done;
+
+	atomisp_destroy_pipes_stream_force(asd);
+	atomisp_css_uninit(isp);
+
+	if (defer_fw_load) {
+		ia_css_unload_firmware();
+		isp->css_env.isp_css_fw.data = NULL;
+		isp->css_env.isp_css_fw.bytes = 0;
+	}
+
+	ret = v4l2_subdev_call(isp->flash, core, s_power, 0);
+	if (ret < 0 && ret != -ENODEV && ret != -ENOIOCTLCMD)
+		dev_warn(isp->dev, "Failed to power-off flash\n");
+
+	if (pm_runtime_put_sync(vdev->v4l2_dev->dev) < 0)
+		dev_err(isp->dev, "Failed to power off device\n");
+
+done:
+	atomisp_subdev_set_selection(&asd->subdev, fh.state,
+				     V4L2_SUBDEV_FORMAT_ACTIVE,
+				     atomisp_subdev_source_pad(vdev),
+				     V4L2_SEL_TGT_COMPOSE, 0,
+				     &clear_compose);
+	mutex_unlock(&isp->mutex);
+
+	return v4l2_fh_release(file);
+}
+
+/*
+ * Memory help functions for image frame and private parameters
+ */
+static int do_isp_mm_remap(struct atomisp_device *isp,
+			   struct vm_area_struct *vma,
+			   ia_css_ptr isp_virt, u32 host_virt, u32 pgnr)
+{
+	u32 pfn;
+
+	while (pgnr) {
+		pfn = hmm_virt_to_phys(isp_virt) >> PAGE_SHIFT;
+		if (remap_pfn_range(vma, host_virt, pfn,
+				    PAGE_SIZE, PAGE_SHARED)) {
+			dev_err(isp->dev, "remap_pfn_range err.\n");
+			return -EAGAIN;
+		}
+
+		isp_virt += PAGE_SIZE;
+		host_virt += PAGE_SIZE;
+		pgnr--;
+	}
+
+	return 0;
+}
+
+static int frame_mmap(struct atomisp_device *isp,
+		      const struct ia_css_frame *frame, struct vm_area_struct *vma)
+{
+	ia_css_ptr isp_virt;
+	u32 host_virt;
+	u32 pgnr;
+
+	if (!frame) {
+		dev_err(isp->dev, "%s: NULL frame pointer.\n", __func__);
+		return -EINVAL;
+	}
+
+	host_virt = vma->vm_start;
+	isp_virt = frame->data;
+	pgnr = DIV_ROUND_UP(frame->data_bytes, PAGE_SIZE);
+
+	if (do_isp_mm_remap(isp, vma, isp_virt, host_virt, pgnr))
+		return -EAGAIN;
+
+	return 0;
+}
+
+int atomisp_videobuf_mmap_mapper(struct videobuf_queue *q,
+				 struct vm_area_struct *vma)
+{
+	u32 offset = vma->vm_pgoff << PAGE_SHIFT;
+	int ret = -EINVAL, i;
+	struct atomisp_device *isp =
+	    ((struct atomisp_video_pipe *)(q->priv_data))->isp;
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct videobuf_mapping *map;
+
+	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
+	if (!(vma->vm_flags & VM_WRITE) || !(vma->vm_flags & VM_SHARED)) {
+		dev_err(isp->dev, "map appl bug: PROT_WRITE and MAP_SHARED are required\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&q->vb_lock);
+	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+		struct videobuf_buffer *buf = q->bufs[i];
+
+		if (!buf)
+			continue;
+
+		map = kzalloc(sizeof(struct videobuf_mapping), GFP_KERNEL);
+		if (!map) {
+			mutex_unlock(&q->vb_lock);
+			return -ENOMEM;
+		}
+
+		buf->map = map;
+		map->q = q;
+
+		buf->baddr = vma->vm_start;
+
+		if (buf && buf->memory == V4L2_MEMORY_MMAP &&
+		    buf->boff == offset) {
+			vm_mem = buf->priv;
+			ret = frame_mmap(isp, vm_mem->vaddr, vma);
+			vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
+			break;
+		}
+	}
+	mutex_unlock(&q->vb_lock);
+
+	return ret;
+}
+
+/* The input frame contains left and right padding that need to be removed.
+ * There is always ISP_LEFT_PAD padding on the left side.
+ * There is also padding on the right (padded_width - width).
+ */
+static int remove_pad_from_frame(struct atomisp_device *isp,
+				 struct ia_css_frame *in_frame, __u32 width, __u32 height)
+{
+	unsigned int i;
+	unsigned short *buffer;
+	int ret = 0;
+	ia_css_ptr load = in_frame->data;
+	ia_css_ptr store = load;
+
+	buffer = kmalloc_array(width, sizeof(load), GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	load += ISP_LEFT_PAD;
+	for (i = 0; i < height; i++) {
+		ret = hmm_load(load, buffer, width * sizeof(load));
+		if (ret < 0)
+			goto remove_pad_error;
+
+		ret = hmm_store(store, buffer, width * sizeof(store));
+		if (ret < 0)
+			goto remove_pad_error;
+
+		load  += in_frame->info.padded_width;
+		store += width;
+	}
+
+remove_pad_error:
+	kfree(buffer);
+	return ret;
+}
+
+static int atomisp_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct ia_css_frame *raw_virt_addr;
+	u32 start = vma->vm_start;
+	u32 end = vma->vm_end;
+	u32 size = end - start;
+	u32 origin_size, new_size;
+	int ret;
+
+	if (!asd) {
+		dev_err(isp->dev, "%s(): asd is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	if (!(vma->vm_flags & (VM_WRITE | VM_READ)))
+		return -EACCES;
+
+	mutex_lock(&isp->mutex);
+
+	if (!(vma->vm_flags & VM_SHARED)) {
+		/* Map private buffer.
+		 * Set VM_SHARED to the flags since we need
+		 * to map the buffer page by page.
+		 * Without VM_SHARED, remap_pfn_range() treats
+		 * this kind of mapping as invalid.
+		 */
+		vma->vm_flags |= VM_SHARED;
+		ret = hmm_mmap(vma, vma->vm_pgoff << PAGE_SHIFT);
+		mutex_unlock(&isp->mutex);
+		return ret;
+	}
+
+	/* mmap for ISP offline raw data */
+	if (atomisp_subdev_source_pad(vdev)
+	    == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE &&
+	    vma->vm_pgoff == (ISP_PARAM_MMAP_OFFSET >> PAGE_SHIFT)) {
+		new_size = pipe->pix.width * pipe->pix.height * 2;
+		if (asd->params.online_process != 0) {
+			ret = -EINVAL;
+			goto error;
+		}
+		raw_virt_addr = asd->raw_output_frame;
+		if (!raw_virt_addr) {
+			dev_err(isp->dev, "Failed to request RAW frame\n");
+			ret = -EINVAL;
+			goto error;
+		}
+
+		ret = remove_pad_from_frame(isp, raw_virt_addr,
+					    pipe->pix.width, pipe->pix.height);
+		if (ret < 0) {
+			dev_err(isp->dev, "remove pad failed.\n");
+			goto error;
+		}
+		origin_size = raw_virt_addr->data_bytes;
+		raw_virt_addr->data_bytes = new_size;
+
+		if (size != PAGE_ALIGN(new_size)) {
+			dev_err(isp->dev, "incorrect size for mmap ISP  Raw Frame\n");
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (frame_mmap(isp, raw_virt_addr, vma)) {
+			dev_err(isp->dev, "frame_mmap failed.\n");
+			raw_virt_addr->data_bytes = origin_size;
+			ret = -EAGAIN;
+			goto error;
+		}
+		raw_virt_addr->data_bytes = origin_size;
+		vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
+		mutex_unlock(&isp->mutex);
+		return 0;
+	}
+
+	/*
+	 * mmap for normal frames
+	 */
+	if (size != pipe->pix.sizeimage) {
+		dev_err(isp->dev, "incorrect size for mmap ISP frames\n");
+		ret = -EINVAL;
+		goto error;
+	}
+	mutex_unlock(&isp->mutex);
+
+	return atomisp_videobuf_mmap_mapper(&pipe->capq, vma);
+
+error:
+	mutex_unlock(&isp->mutex);
+
+	return ret;
+}
+
+static __poll_t atomisp_poll(struct file *file,
+			     struct poll_table_struct *pt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+
+	mutex_lock(&isp->mutex);
+	if (pipe->capq.streaming != 1) {
+		mutex_unlock(&isp->mutex);
+		return EPOLLERR;
+	}
+	mutex_unlock(&isp->mutex);
+
+	return videobuf_poll_stream(file, &pipe->capq, pt);
+}
+
+const struct v4l2_file_operations atomisp_fops = {
+	.owner = THIS_MODULE,
+	.open = atomisp_open,
+	.release = atomisp_release,
+	.mmap = atomisp_mmap,
+	.unlocked_ioctl = video_ioctl2,
+#ifdef CONFIG_COMPAT
+	/*
+	 * this was removed because of bugs, the interface
+	 * needs to be made safe for compat tasks instead.
+	.compat_ioctl32 = atomisp_compat_ioctl32,
+	 */
+#endif
+	.poll = atomisp_poll,
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp_fops.h b/drivers/staging/media/atomisp/pci/atomisp_fops.h
new file mode 100644
index 000000000..3f1e442ba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_fops.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__ATOMISP_FOPS_H__
+#define	__ATOMISP_FOPS_H__
+#include "atomisp_subdev.h"
+
+int atomisp_q_video_buffers_to_css(struct atomisp_sub_device *asd,
+				   struct atomisp_video_pipe *pipe,
+				   enum atomisp_input_stream_id stream_id,
+				   enum ia_css_buffer_type css_buf_type,
+				   enum ia_css_pipe_id css_pipe_id);
+
+unsigned int atomisp_dev_users(struct atomisp_device *isp);
+unsigned int atomisp_sub_dev_users(struct atomisp_sub_device *asd);
+
+/*
+ * Memory help functions for image frame and private parameters
+ */
+
+int atomisp_videobuf_mmap_mapper(struct videobuf_queue *q,
+				 struct vm_area_struct *vma);
+
+int atomisp_qbuf_to_css(struct atomisp_device *isp,
+			struct atomisp_video_pipe *pipe,
+			struct videobuf_buffer *vb);
+
+int atomisp_qbuffers_to_css(struct atomisp_sub_device *asd);
+
+extern const struct v4l2_file_operations atomisp_fops;
+
+extern bool defer_fw_load;
+
+#endif /* __ATOMISP_FOPS_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_gmin_platform.c b/drivers/staging/media/atomisp/pci/atomisp_gmin_platform.c
new file mode 100644
index 000000000..789b13db8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_gmin_platform.c
@@ -0,0 +1,1391 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/dmi.h>
+#include <linux/efi.h>
+#include <linux/pci.h>
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <media/v4l2-subdev.h>
+#include <linux/mfd/intel_soc_pmic.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio/consumer.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include "../../include/linux/atomisp_platform.h"
+#include "../../include/linux/atomisp_gmin_platform.h"
+
+#define MAX_SUBDEVS 8
+
+enum clock_rate {
+	VLV2_CLK_XTAL_25_0MHz = 0,
+	VLV2_CLK_PLL_19P2MHZ = 1
+};
+
+#define CLK_RATE_19_2MHZ	19200000
+#define CLK_RATE_25_0MHZ	25000000
+
+/* Valid clock number range from 0 to 5 */
+#define MAX_CLK_COUNT                   5
+
+/* X-Powers AXP288 register set */
+#define ALDO1_SEL_REG	0x28
+#define ALDO1_CTRL3_REG	0x13
+#define ALDO1_2P8V	0x16
+#define ALDO1_CTRL3_SHIFT 0x05
+
+#define ELDO_CTRL_REG   0x12
+
+#define ELDO1_SEL_REG	0x19
+#define ELDO1_1P6V	0x12
+#define ELDO1_CTRL_SHIFT 0x00
+
+#define ELDO2_SEL_REG	0x1a
+#define ELDO2_1P8V	0x16
+#define ELDO2_CTRL_SHIFT 0x01
+
+/* TI SND9039 PMIC register set */
+#define LDO9_REG	0x49
+#define LDO10_REG	0x4a
+#define LDO11_REG	0x4b
+
+#define LDO_2P8V_ON	0x2f /* 0x2e selects 2.85V ...      */
+#define LDO_2P8V_OFF	0x2e /* ... bottom bit is "enabled" */
+
+#define LDO_1P8V_ON	0x59 /* 0x58 selects 1.80V ...      */
+#define LDO_1P8V_OFF	0x58 /* ... bottom bit is "enabled" */
+
+/* CRYSTAL COVE PMIC register set */
+#define CRYSTAL_1P8V_REG	0x57
+#define CRYSTAL_2P8V_REG	0x5d
+#define CRYSTAL_ON		0x63
+#define CRYSTAL_OFF		0x62
+
+struct gmin_subdev {
+	struct v4l2_subdev *subdev;
+	enum clock_rate clock_src;
+	struct clk *pmc_clk;
+	struct gpio_desc *gpio0;
+	struct gpio_desc *gpio1;
+	struct regulator *v1p8_reg;
+	struct regulator *v2p8_reg;
+	struct regulator *v1p2_reg;
+	struct regulator *v2p8_vcm_reg;
+	enum atomisp_camera_port csi_port;
+	unsigned int csi_lanes;
+	enum atomisp_input_format csi_fmt;
+	enum atomisp_bayer_order csi_bayer;
+
+	bool clock_on;
+	bool v1p8_on;
+	bool v2p8_on;
+	bool v1p2_on;
+	bool v2p8_vcm_on;
+
+	int v1p8_gpio;
+	int v2p8_gpio;
+
+	u8 pwm_i2c_addr;
+
+	/* For PMIC AXP */
+	int eldo1_sel_reg, eldo1_1p6v, eldo1_ctrl_shift;
+	int eldo2_sel_reg, eldo2_1p8v, eldo2_ctrl_shift;
+};
+
+static struct gmin_subdev gmin_subdevs[MAX_SUBDEVS];
+
+/* ACPI HIDs for the PMICs that could be used by this driver */
+#define PMIC_ACPI_AXP		"INT33F4"	/* XPower AXP288 PMIC */
+#define PMIC_ACPI_TI		"INT33F5"	/* Dollar Cove TI PMIC */
+#define PMIC_ACPI_CRYSTALCOVE	"INT33FD"	/* Crystal Cove PMIC */
+
+#define PMIC_PLATFORM_TI	"intel_soc_pmic_chtdc_ti"
+
+static enum {
+	PMIC_UNSET = 0,
+	PMIC_REGULATOR,
+	PMIC_AXP,
+	PMIC_TI,
+	PMIC_CRYSTALCOVE
+} pmic_id;
+
+static const char *pmic_name[] = {
+	[PMIC_UNSET]		= "ACPI device PM",
+	[PMIC_REGULATOR]	= "regulator driver",
+	[PMIC_AXP]		= "XPower AXP288 PMIC",
+	[PMIC_TI]		= "Dollar Cove TI PMIC",
+	[PMIC_CRYSTALCOVE]	= "Crystal Cove PMIC",
+};
+
+static DEFINE_MUTEX(gmin_regulator_mutex);
+static int gmin_v1p8_enable_count;
+static int gmin_v2p8_enable_count;
+
+/* The atomisp uses type==0 for the end-of-list marker, so leave space. */
+static struct intel_v4l2_subdev_table pdata_subdevs[MAX_SUBDEVS + 1];
+
+static const struct atomisp_platform_data pdata = {
+	.subdevs = pdata_subdevs,
+};
+
+static LIST_HEAD(vcm_devices);
+static DEFINE_MUTEX(vcm_lock);
+
+static struct gmin_subdev *find_gmin_subdev(struct v4l2_subdev *subdev);
+
+const struct atomisp_platform_data *atomisp_get_platform_data(void)
+{
+	return &pdata;
+}
+EXPORT_SYMBOL_GPL(atomisp_get_platform_data);
+
+int atomisp_register_i2c_module(struct v4l2_subdev *subdev,
+				struct camera_sensor_platform_data *plat_data,
+				enum intel_v4l2_subdev_type type)
+{
+	int i;
+	struct i2c_board_info *bi;
+	struct gmin_subdev *gs;
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct acpi_device *adev = ACPI_COMPANION(&client->dev);
+
+	dev_info(&client->dev, "register atomisp i2c module type %d\n", type);
+
+	/* The windows driver model (and thus most BIOSes by default)
+	 * uses ACPI runtime power management for camera devices, but
+	 * we don't.  Disable it, or else the rails will be needlessly
+	 * tickled during suspend/resume.  This has caused power and
+	 * performance issues on multiple devices.
+	 */
+	adev->power.flags.power_resources = 0;
+
+	for (i = 0; i < MAX_SUBDEVS; i++)
+		if (!pdata.subdevs[i].type)
+			break;
+
+	if (pdata.subdevs[i].type)
+		return -ENOMEM;
+
+	/* Note subtlety of initialization order: at the point where
+	 * this registration API gets called, the platform data
+	 * callbacks have probably already been invoked, so the
+	 * gmin_subdev struct is already initialized for us.
+	 */
+	gs = find_gmin_subdev(subdev);
+	if (!gs)
+		return -ENODEV;
+
+	pdata.subdevs[i].type = type;
+	pdata.subdevs[i].port = gs->csi_port;
+	pdata.subdevs[i].subdev = subdev;
+	pdata.subdevs[i].v4l2_subdev.i2c_adapter_id = client->adapter->nr;
+
+	/* Convert i2c_client to i2c_board_info */
+	bi = &pdata.subdevs[i].v4l2_subdev.board_info;
+	memcpy(bi->type, client->name, I2C_NAME_SIZE);
+	bi->flags = client->flags;
+	bi->addr = client->addr;
+	bi->irq = client->irq;
+	bi->platform_data = plat_data;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(atomisp_register_i2c_module);
+
+struct v4l2_subdev *atomisp_gmin_find_subdev(struct i2c_adapter *adapter,
+	struct i2c_board_info *board_info)
+{
+	int i;
+
+	for (i = 0; i < MAX_SUBDEVS && pdata.subdevs[i].type; i++) {
+		struct intel_v4l2_subdev_table *sd = &pdata.subdevs[i];
+
+		if (sd->v4l2_subdev.i2c_adapter_id == adapter->nr &&
+		    sd->v4l2_subdev.board_info.addr == board_info->addr)
+			return sd->subdev;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(atomisp_gmin_find_subdev);
+
+int atomisp_gmin_remove_subdev(struct v4l2_subdev *sd)
+{
+	int i, j;
+
+	if (!sd)
+		return 0;
+
+	for (i = 0; i < MAX_SUBDEVS; i++) {
+		if (pdata.subdevs[i].subdev == sd) {
+			for (j = i + 1; j <= MAX_SUBDEVS; j++)
+				pdata.subdevs[j - 1] = pdata.subdevs[j];
+		}
+		if (gmin_subdevs[i].subdev == sd) {
+			if (gmin_subdevs[i].gpio0)
+				gpiod_put(gmin_subdevs[i].gpio0);
+			gmin_subdevs[i].gpio0 = NULL;
+			if (gmin_subdevs[i].gpio1)
+				gpiod_put(gmin_subdevs[i].gpio1);
+			gmin_subdevs[i].gpio1 = NULL;
+			if (pmic_id == PMIC_REGULATOR) {
+				regulator_put(gmin_subdevs[i].v1p8_reg);
+				regulator_put(gmin_subdevs[i].v2p8_reg);
+				regulator_put(gmin_subdevs[i].v1p2_reg);
+				regulator_put(gmin_subdevs[i].v2p8_vcm_reg);
+			}
+			gmin_subdevs[i].subdev = NULL;
+		}
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(atomisp_gmin_remove_subdev);
+
+struct gmin_cfg_var {
+	const char *name, *val;
+};
+
+static struct gmin_cfg_var ffrd8_vars[] = {
+	{ "INTCF1B:00_ImxId",    "0x134" },
+	{ "INTCF1B:00_CsiPort",  "1" },
+	{ "INTCF1B:00_CsiLanes", "4" },
+	{ "INTCF1B:00_CamClk", "0" },
+	{},
+};
+
+/* Cribbed from MCG defaults in the mt9m114 driver, not actually verified
+ * vs. T100 hardware
+ */
+static struct gmin_cfg_var t100_vars[] = {
+	{ "INT33F0:00_CsiPort",  "0" },
+	{ "INT33F0:00_CsiLanes", "1" },
+	{ "INT33F0:00_CamClk",   "1" },
+	{},
+};
+
+static struct gmin_cfg_var mrd7_vars[] = {
+	{"INT33F8:00_CamType", "1"},
+	{"INT33F8:00_CsiPort", "1"},
+	{"INT33F8:00_CsiLanes", "2"},
+	{"INT33F8:00_CsiFmt", "13"},
+	{"INT33F8:00_CsiBayer", "0"},
+	{"INT33F8:00_CamClk", "0"},
+
+	{"INT33F9:00_CamType", "1"},
+	{"INT33F9:00_CsiPort", "0"},
+	{"INT33F9:00_CsiLanes", "1"},
+	{"INT33F9:00_CsiFmt", "13"},
+	{"INT33F9:00_CsiBayer", "0"},
+	{"INT33F9:00_CamClk", "1"},
+	{},
+};
+
+static struct gmin_cfg_var ecs7_vars[] = {
+	{"INT33BE:00_CsiPort", "1"},
+	{"INT33BE:00_CsiLanes", "2"},
+	{"INT33BE:00_CsiFmt", "13"},
+	{"INT33BE:00_CsiBayer", "2"},
+	{"INT33BE:00_CamClk", "0"},
+
+	{"INT33F0:00_CsiPort", "0"},
+	{"INT33F0:00_CsiLanes", "1"},
+	{"INT33F0:00_CsiFmt", "13"},
+	{"INT33F0:00_CsiBayer", "0"},
+	{"INT33F0:00_CamClk", "1"},
+	{"gmin_V2P8GPIO", "402"},
+	{},
+};
+
+static struct gmin_cfg_var i8880_vars[] = {
+	{"XXOV2680:00_CsiPort", "1"},
+	{"XXOV2680:00_CsiLanes", "1"},
+	{"XXOV2680:00_CamClk", "0"},
+
+	{"XXGC0310:00_CsiPort", "0"},
+	{"XXGC0310:00_CsiLanes", "1"},
+	{"XXGC0310:00_CamClk", "1"},
+	{},
+};
+
+/*
+ * Surface 3 does not describe CsiPort/CsiLanes in both DSDT and EFI.
+ */
+static struct gmin_cfg_var surface3_vars[] = {
+	{"APTA0330:00_CsiPort", "0"},
+	{"APTA0330:00_CsiLanes", "2"},
+
+	{"OVTI8835:00_CsiPort", "1"},
+	{"OVTI8835:00_CsiLanes", "4"},
+	{},
+};
+
+static const struct dmi_system_id gmin_vars[] = {
+	{
+		.ident = "BYT-T FFD8",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "BYT-T FFD8"),
+		},
+		.driver_data = ffrd8_vars,
+	},
+	{
+		.ident = "T100TA",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "T100TA"),
+		},
+		.driver_data = t100_vars,
+	},
+	{
+		.ident = "MRD7",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "TABLET"),
+			DMI_MATCH(DMI_BOARD_VERSION, "MRD 7"),
+		},
+		.driver_data = mrd7_vars,
+	},
+	{
+		.ident = "ST70408",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "ST70408"),
+		},
+		.driver_data = ecs7_vars,
+	},
+	{
+		.ident = "VTA0803",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "VTA0803"),
+		},
+		.driver_data = i8880_vars,
+	},
+	{
+		.ident = "Surface 3",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "Surface 3"),
+		},
+		.driver_data = surface3_vars,
+	},
+	{}
+};
+
+#define GMIN_CFG_VAR_EFI_GUID EFI_GUID(0xecb54cd9, 0xe5ae, 0x4fdc, \
+				       0xa9, 0x71, 0xe8, 0x77,	   \
+				       0x75, 0x60, 0x68, 0xf7)
+
+static const guid_t atomisp_dsm_guid = GUID_INIT(0xdc2f6c4f, 0x045b, 0x4f1d,
+						 0x97, 0xb9, 0x88, 0x2a,
+						 0x68, 0x60, 0xa4, 0xbe);
+
+#define CFG_VAR_NAME_MAX 64
+
+#define GMIN_PMC_CLK_NAME 14 /* "pmc_plt_clk_[0..5]" */
+static char gmin_pmc_clk_name[GMIN_PMC_CLK_NAME];
+
+static struct i2c_client *gmin_i2c_dev_exists(struct device *dev, char *name,
+					      struct i2c_client **client)
+{
+	struct acpi_device *adev;
+	struct device *d;
+
+	adev = acpi_dev_get_first_match_dev(name, NULL, -1);
+	if (!adev)
+		return NULL;
+
+	d = bus_find_device_by_acpi_dev(&i2c_bus_type, adev);
+	acpi_dev_put(adev);
+	if (!d)
+		return NULL;
+
+	*client = i2c_verify_client(d);
+	put_device(d);
+
+	dev_dbg(dev, "found '%s' at address 0x%02x, adapter %d\n",
+		(*client)->name, (*client)->addr, (*client)->adapter->nr);
+	return *client;
+}
+
+static int gmin_i2c_write(struct device *dev, u16 i2c_addr, u8 reg,
+			  u32 value, u32 mask)
+{
+	int ret;
+
+	/*
+	 * FIXME: Right now, the intel_pmic driver just write values
+	 * directly at the regmap, instead of properly implementing
+	 * i2c_transfer() mechanism. Let's use the same interface here,
+	 * as otherwise we may face issues.
+	 */
+
+	dev_dbg(dev,
+		"I2C write, addr: 0x%02x, reg: 0x%02x, value: 0x%02x, mask: 0x%02x\n",
+		i2c_addr, reg, value, mask);
+
+	ret = intel_soc_pmic_exec_mipi_pmic_seq_element(i2c_addr, reg, value, mask);
+	if (ret == -EOPNOTSUPP)
+		dev_err(dev,
+			"ACPI didn't mapped the OpRegion needed to access I2C address 0x%02x.\n"
+			"Need to compile the kernel using CONFIG_*_PMIC_OPREGION settings\n",
+			i2c_addr);
+
+	return ret;
+}
+
+static int atomisp_get_acpi_power(struct device *dev)
+{
+	char name[5];
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_buffer b_name = { sizeof(name), name };
+	union acpi_object *package, *element;
+	acpi_handle handle = ACPI_HANDLE(dev);
+	acpi_handle rhandle;
+	acpi_status status;
+	int clock_num = -1;
+	int i;
+
+	status = acpi_evaluate_object(handle, "_PR0", NULL, &buffer);
+	if (!ACPI_SUCCESS(status))
+		return -1;
+
+	package = buffer.pointer;
+
+	if (!buffer.length || !package
+	    || package->type != ACPI_TYPE_PACKAGE
+	    || !package->package.count)
+		goto fail;
+
+	for (i = 0; i < package->package.count; i++) {
+		element = &package->package.elements[i];
+
+		if (element->type != ACPI_TYPE_LOCAL_REFERENCE)
+			continue;
+
+		rhandle = element->reference.handle;
+		if (!rhandle)
+			goto fail;
+
+		acpi_get_name(rhandle, ACPI_SINGLE_NAME, &b_name);
+
+		dev_dbg(dev, "Found PM resource '%s'\n", name);
+		if (strlen(name) == 4 && !strncmp(name, "CLK", 3)) {
+			if (name[3] >= '0' && name[3] <= '4')
+				clock_num = name[3] - '0';
+#if 0
+			/*
+			 * We could abort here, but let's parse all resources,
+			 * as this is helpful for debugging purposes
+			 */
+			if (clock_num >= 0)
+				break;
+#endif
+		}
+	}
+
+fail:
+	ACPI_FREE(buffer.pointer);
+
+	return clock_num;
+}
+
+static u8 gmin_get_pmic_id_and_addr(struct device *dev)
+{
+	struct i2c_client *power = NULL;
+	static u8 pmic_i2c_addr;
+
+	if (pmic_id)
+		return pmic_i2c_addr;
+
+	if (gmin_i2c_dev_exists(dev, PMIC_ACPI_TI, &power))
+		pmic_id = PMIC_TI;
+	else if (gmin_i2c_dev_exists(dev, PMIC_ACPI_AXP, &power))
+		pmic_id = PMIC_AXP;
+	else if (gmin_i2c_dev_exists(dev, PMIC_ACPI_CRYSTALCOVE, &power))
+		pmic_id = PMIC_CRYSTALCOVE;
+	else
+		pmic_id = PMIC_REGULATOR;
+
+	pmic_i2c_addr = power ? power->addr : 0;
+	return pmic_i2c_addr;
+}
+
+static int gmin_detect_pmic(struct v4l2_subdev *subdev)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct device *dev = &client->dev;
+	u8 pmic_i2c_addr;
+
+	pmic_i2c_addr = gmin_get_pmic_id_and_addr(dev);
+	dev_info(dev, "gmin: power management provided via %s (i2c addr 0x%02x)\n",
+		 pmic_name[pmic_id], pmic_i2c_addr);
+	return pmic_i2c_addr;
+}
+
+static int gmin_subdev_add(struct gmin_subdev *gs)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(gs->subdev);
+	struct device *dev = &client->dev;
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+	int ret, default_val, clock_num = -1;
+
+	dev_info(dev, "%s: ACPI path is %pfw\n", __func__, dev_fwnode(dev));
+
+	/*WA:CHT requires XTAL clock as PLL is not stable.*/
+	gs->clock_src = gmin_get_var_int(dev, false, "ClkSrc",
+				         VLV2_CLK_PLL_19P2MHZ);
+
+	/*
+	 * Get ACPI _PR0 derived clock here already because it is used
+	 * to determine the csi_port default.
+	 */
+	if (acpi_device_power_manageable(adev))
+		clock_num = atomisp_get_acpi_power(dev);
+
+	/* Compare clock to CsiPort 1 pmc-clock used in the CHT/BYT reference designs */
+	if (IS_ISP2401)
+		default_val = clock_num == 4 ? 1 : 0;
+	else
+		default_val = clock_num == 0 ? 1 : 0;
+
+	gs->csi_port = gmin_get_var_int(dev, false, "CsiPort", default_val);
+	gs->csi_lanes = gmin_get_var_int(dev, false, "CsiLanes", 1);
+
+	gs->gpio0 = gpiod_get_index(dev, NULL, 0, GPIOD_OUT_LOW);
+	if (IS_ERR(gs->gpio0))
+		gs->gpio0 = NULL;
+	else
+		dev_info(dev, "will handle gpio0 via ACPI\n");
+
+	gs->gpio1 = gpiod_get_index(dev, NULL, 1, GPIOD_OUT_LOW);
+	if (IS_ERR(gs->gpio1))
+		gs->gpio1 = NULL;
+	else
+		dev_info(dev, "will handle gpio1 via ACPI\n");
+
+	/*
+	 * Those are used only when there is an external regulator apart
+	 * from the PMIC that would be providing power supply, like on the
+	 * two cases below:
+	 *
+	 * The ECS E7 board drives camera 2.8v from an external regulator
+	 * instead of the PMIC.  There's a gmin_CamV2P8 config variable
+	 * that specifies the GPIO to handle this particular case,
+	 * but this needs a broader architecture for handling camera power.
+	 *
+	 * The CHT RVP board drives camera 1.8v from an* external regulator
+	 * instead of the PMIC just like ECS E7 board.
+	 */
+
+	gs->v1p8_gpio = gmin_get_var_int(dev, true, "V1P8GPIO", -1);
+	gs->v2p8_gpio = gmin_get_var_int(dev, true, "V2P8GPIO", -1);
+
+	/*
+	 * FIXME:
+	 *
+	 * The ACPI handling code checks for the _PR? tables in order to
+	 * know what is required to switch the device from power state
+	 * D0 (_PR0) up to D3COLD (_PR3).
+	 *
+	 * The adev->flags.power_manageable is set to true if the device
+	 * has a _PR0 table, which can be checked by calling
+	 * acpi_device_power_manageable(adev).
+	 *
+	 * However, this only says that the device can be set to power off
+	 * mode.
+	 *
+	 * At least on the DSDT tables we've seen so far, there's no _PR3,
+	 * nor _PS3 (which would have a somewhat similar effect).
+	 * So, using ACPI for power management won't work, except if adding
+	 * an ACPI override logic somewhere.
+	 *
+	 * So, at least for the existing devices we know, the check below
+	 * will always be false.
+	 */
+	if (acpi_device_can_wakeup(adev) &&
+	    acpi_device_can_poweroff(adev)) {
+		dev_info(dev,
+			 "gmin: power management provided via device PM\n");
+		return 0;
+	}
+
+	/*
+	 * The code below is here due to backward compatibility with devices
+	 * whose ACPI BIOS may not contain everything that would be needed
+	 * in order to set clocks and do power management.
+	 */
+
+	/*
+	 * According with :
+	 *   https://github.com/projectceladon/hardware-intel-kernelflinger/blob/master/doc/fastboot.md
+	 *
+	 * The "CamClk" EFI var is set via fastboot on some Android devices,
+	 * and seems to contain the number of the clock used to feed the
+	 * sensor.
+	 *
+	 * On systems with a proper ACPI table, this is given via the _PR0
+	 * power resource table. The logic below should first check if there
+	 * is a power resource already, falling back to the EFI vars detection
+	 * otherwise.
+	 */
+
+	/* If getting the clock from _PR0 above failed, fall-back to EFI and/or DMI match */
+	if (clock_num < 0)
+		clock_num = gmin_get_var_int(dev, false, "CamClk", 0);
+
+	if (clock_num < 0 || clock_num > MAX_CLK_COUNT) {
+		dev_err(dev, "Invalid clock number\n");
+		return -EINVAL;
+	}
+
+	snprintf(gmin_pmc_clk_name, sizeof(gmin_pmc_clk_name),
+		 "%s_%d", "pmc_plt_clk", clock_num);
+
+	gs->pmc_clk = devm_clk_get(dev, gmin_pmc_clk_name);
+	if (IS_ERR(gs->pmc_clk)) {
+		ret = PTR_ERR(gs->pmc_clk);
+		dev_err(dev, "Failed to get clk from %s: %d\n", gmin_pmc_clk_name, ret);
+		return ret;
+	}
+	dev_info(dev, "Will use CLK%d (%s)\n", clock_num, gmin_pmc_clk_name);
+
+	/*
+	 * The firmware might enable the clock at
+	 * boot (this information may or may not
+	 * be reflected in the enable clock register).
+	 * To change the rate we must disable the clock
+	 * first to cover these cases. Due to common
+	 * clock framework restrictions that do not allow
+	 * to disable a clock that has not been enabled,
+	 * we need to enable the clock first.
+	 */
+	ret = clk_prepare_enable(gs->pmc_clk);
+	if (!ret)
+		clk_disable_unprepare(gs->pmc_clk);
+
+	switch (pmic_id) {
+	case PMIC_REGULATOR:
+		gs->v1p8_reg = regulator_get(dev, "V1P8SX");
+		gs->v2p8_reg = regulator_get(dev, "V2P8SX");
+
+		gs->v1p2_reg = regulator_get(dev, "V1P2A");
+		gs->v2p8_vcm_reg = regulator_get(dev, "VPROG4B");
+
+		/* Note: ideally we would initialize v[12]p8_on to the
+		 * output of regulator_is_enabled(), but sadly that
+		 * API is broken with the current drivers, returning
+		 * "1" for a regulator that will then emit a
+		 * "unbalanced disable" WARNing if we try to disable
+		 * it.
+		 */
+		break;
+
+	case PMIC_AXP:
+		gs->eldo1_1p6v = gmin_get_var_int(dev, false,
+						  "eldo1_1p8v",
+						  ELDO1_1P6V);
+		gs->eldo1_sel_reg = gmin_get_var_int(dev, false,
+						     "eldo1_sel_reg",
+						     ELDO1_SEL_REG);
+		gs->eldo1_ctrl_shift = gmin_get_var_int(dev, false,
+							"eldo1_ctrl_shift",
+							ELDO1_CTRL_SHIFT);
+		gs->eldo2_1p8v = gmin_get_var_int(dev, false,
+						  "eldo2_1p8v",
+						  ELDO2_1P8V);
+		gs->eldo2_sel_reg = gmin_get_var_int(dev, false,
+						     "eldo2_sel_reg",
+						     ELDO2_SEL_REG);
+		gs->eldo2_ctrl_shift = gmin_get_var_int(dev, false,
+							"eldo2_ctrl_shift",
+							ELDO2_CTRL_SHIFT);
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static struct gmin_subdev *find_gmin_subdev(struct v4l2_subdev *subdev)
+{
+	int i;
+
+	for (i = 0; i < MAX_SUBDEVS; i++)
+		if (gmin_subdevs[i].subdev == subdev)
+			return &gmin_subdevs[i];
+	return NULL;
+}
+
+static struct gmin_subdev *find_free_gmin_subdev_slot(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < MAX_SUBDEVS; i++)
+		if (gmin_subdevs[i].subdev == NULL)
+			return &gmin_subdevs[i];
+	return NULL;
+}
+
+static int axp_regulator_set(struct device *dev, struct gmin_subdev *gs,
+			     int sel_reg, u8 setting,
+			     int ctrl_reg, int shift, bool on)
+{
+	int ret;
+	int val;
+
+	ret = gmin_i2c_write(dev, gs->pwm_i2c_addr, sel_reg, setting, 0xff);
+	if (ret)
+		return ret;
+
+	val = on ? 1 << shift : 0;
+
+	ret = gmin_i2c_write(dev, gs->pwm_i2c_addr, ctrl_reg, val, 1 << shift);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * Some boards contain a hw-bug where turning eldo2 back on after having turned
+ * it off causes the CPLM3218 ambient-light-sensor on the image-sensor's I2C bus
+ * to crash, hanging the bus. Do not turn eldo2 off on these systems.
+ */
+static const struct dmi_system_id axp_leave_eldo2_on_ids[] = {
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "TrekStor"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "SurfTab duo W1 10.1 (VT4)"),
+		},
+	},
+	{ }
+};
+
+static int axp_v1p8_on(struct device *dev, struct gmin_subdev *gs)
+{
+	int ret;
+
+	ret = axp_regulator_set(dev, gs, gs->eldo2_sel_reg, gs->eldo2_1p8v,
+				ELDO_CTRL_REG, gs->eldo2_ctrl_shift, true);
+	if (ret)
+		return ret;
+
+	/*
+	 * This sleep comes out of the gc2235 driver, which is the
+	 * only one I currently see that wants to set both 1.8v rails.
+	 */
+	usleep_range(110, 150);
+
+	ret = axp_regulator_set(dev, gs, gs->eldo1_sel_reg, gs->eldo1_1p6v,
+				ELDO_CTRL_REG, gs->eldo1_ctrl_shift, true);
+	return ret;
+}
+
+static int axp_v1p8_off(struct device *dev, struct gmin_subdev *gs)
+{
+	int ret;
+
+	ret = axp_regulator_set(dev, gs, gs->eldo1_sel_reg, gs->eldo1_1p6v,
+				ELDO_CTRL_REG, gs->eldo1_ctrl_shift, false);
+	if (ret)
+		return ret;
+
+	if (dmi_check_system(axp_leave_eldo2_on_ids))
+		return 0;
+
+	ret = axp_regulator_set(dev, gs, gs->eldo2_sel_reg, gs->eldo2_1p8v,
+				ELDO_CTRL_REG, gs->eldo2_ctrl_shift, false);
+	return ret;
+}
+
+static int gmin_gpio0_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
+	if (gs) {
+		gpiod_set_value(gs->gpio0, on);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int gmin_gpio1_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
+	if (gs) {
+		gpiod_set_value(gs->gpio1, on);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int gmin_v1p2_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
+	if (!gs || gs->v1p2_on == on)
+		return 0;
+	gs->v1p2_on = on;
+
+	/* use regulator for PMIC */
+	if (gs->v1p2_reg) {
+		if (on)
+			return regulator_enable(gs->v1p2_reg);
+		else
+			return regulator_disable(gs->v1p2_reg);
+	}
+
+	/* TODO:v1p2 may need to extend to other PMICs */
+
+	return -EINVAL;
+}
+
+static int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	int ret;
+	int value;
+
+	if (!gs || gs->v1p8_on == on)
+		return 0;
+
+	if (gs->v1p8_gpio >= 0) {
+		pr_info("atomisp_gmin_platform: 1.8v power on GPIO %d\n",
+			gs->v1p8_gpio);
+		ret = gpio_request(gs->v1p8_gpio, "camera_v1p8_en");
+		if (!ret)
+			ret = gpio_direction_output(gs->v1p8_gpio, 0);
+		if (ret)
+			pr_err("V1P8 GPIO initialization failed\n");
+	}
+
+	gs->v1p8_on = on;
+
+	ret = 0;
+	mutex_lock(&gmin_regulator_mutex);
+	if (on) {
+		gmin_v1p8_enable_count++;
+		if (gmin_v1p8_enable_count > 1)
+			goto out; /* Already on */
+	} else {
+		gmin_v1p8_enable_count--;
+		if (gmin_v1p8_enable_count > 0)
+			goto out; /* Still needed */
+	}
+
+	if (gs->v1p8_gpio >= 0)
+		gpio_set_value(gs->v1p8_gpio, on);
+
+	if (gs->v1p8_reg) {
+		regulator_set_voltage(gs->v1p8_reg, 1800000, 1800000);
+		if (on)
+			ret = regulator_enable(gs->v1p8_reg);
+		else
+			ret = regulator_disable(gs->v1p8_reg);
+
+		goto out;
+	}
+
+	switch (pmic_id) {
+	case PMIC_AXP:
+		if (on)
+			ret = axp_v1p8_on(subdev->dev, gs);
+		else
+			ret = axp_v1p8_off(subdev->dev, gs);
+		break;
+	case PMIC_TI:
+		value = on ? LDO_1P8V_ON : LDO_1P8V_OFF;
+
+		ret = gmin_i2c_write(subdev->dev, gs->pwm_i2c_addr,
+				     LDO10_REG, value, 0xff);
+		break;
+	case PMIC_CRYSTALCOVE:
+		value = on ? CRYSTAL_ON : CRYSTAL_OFF;
+
+		ret = gmin_i2c_write(subdev->dev, gs->pwm_i2c_addr,
+				     CRYSTAL_1P8V_REG, value, 0xff);
+		break;
+	default:
+		dev_err(subdev->dev, "Couldn't set power mode for v1p8\n");
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&gmin_regulator_mutex);
+	return ret;
+}
+
+static int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	int ret;
+	int value;
+
+	if (WARN_ON(!gs))
+		return -ENODEV;
+
+	if (gs->v2p8_gpio >= 0) {
+		pr_info("atomisp_gmin_platform: 2.8v power on GPIO %d\n",
+			gs->v2p8_gpio);
+		ret = gpio_request(gs->v2p8_gpio, "camera_v2p8");
+		if (!ret)
+			ret = gpio_direction_output(gs->v2p8_gpio, 0);
+		if (ret)
+			pr_err("V2P8 GPIO initialization failed\n");
+	}
+
+	if (gs->v2p8_on == on)
+		return 0;
+	gs->v2p8_on = on;
+
+	ret = 0;
+	mutex_lock(&gmin_regulator_mutex);
+	if (on) {
+		gmin_v2p8_enable_count++;
+		if (gmin_v2p8_enable_count > 1)
+			goto out; /* Already on */
+	} else {
+		gmin_v2p8_enable_count--;
+		if (gmin_v2p8_enable_count > 0)
+			goto out; /* Still needed */
+	}
+
+	if (gs->v2p8_gpio >= 0)
+		gpio_set_value(gs->v2p8_gpio, on);
+
+	if (gs->v2p8_reg) {
+		regulator_set_voltage(gs->v2p8_reg, 2900000, 2900000);
+		if (on)
+			ret = regulator_enable(gs->v2p8_reg);
+		else
+			ret = regulator_disable(gs->v2p8_reg);
+
+		goto out;
+	}
+
+	switch (pmic_id) {
+	case PMIC_AXP:
+		ret = axp_regulator_set(subdev->dev, gs, ALDO1_SEL_REG,
+					ALDO1_2P8V, ALDO1_CTRL3_REG,
+					ALDO1_CTRL3_SHIFT, on);
+		break;
+	case PMIC_TI:
+		value = on ? LDO_2P8V_ON : LDO_2P8V_OFF;
+
+		ret = gmin_i2c_write(subdev->dev, gs->pwm_i2c_addr,
+				     LDO9_REG, value, 0xff);
+		break;
+	case PMIC_CRYSTALCOVE:
+		value = on ? CRYSTAL_ON : CRYSTAL_OFF;
+
+		ret = gmin_i2c_write(subdev->dev, gs->pwm_i2c_addr,
+				     CRYSTAL_2P8V_REG, value, 0xff);
+		break;
+	default:
+		dev_err(subdev->dev, "Couldn't set power mode for v2p8\n");
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&gmin_regulator_mutex);
+	return ret;
+}
+
+static int gmin_acpi_pm_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	int ret = 0;
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct acpi_device *adev = ACPI_COMPANION(&client->dev);
+
+	/* Use the ACPI power management to control it */
+	on = !!on;
+	if (gs->clock_on == on)
+		return 0;
+
+	dev_dbg(subdev->dev, "Setting power state to %s\n",
+		on ? "on" : "off");
+
+	if (on)
+		ret = acpi_device_set_power(adev,
+					    ACPI_STATE_D0);
+	else
+		ret = acpi_device_set_power(adev,
+					    ACPI_STATE_D3_COLD);
+
+	if (!ret)
+		gs->clock_on = on;
+	else
+		dev_err(subdev->dev, "Couldn't set power state to %s\n",
+			on ? "on" : "off");
+
+	return ret;
+}
+
+static int gmin_flisclk_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	int ret = 0;
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+
+	if (gs->clock_on == !!on)
+		return 0;
+
+	if (on) {
+		ret = clk_set_rate(gs->pmc_clk,
+				   gs->clock_src ? CLK_RATE_19_2MHZ : CLK_RATE_25_0MHZ);
+
+		if (ret)
+			dev_err(&client->dev, "unable to set PMC rate %d\n",
+				gs->clock_src);
+
+		ret = clk_prepare_enable(gs->pmc_clk);
+		if (ret == 0)
+			gs->clock_on = true;
+	} else {
+		clk_disable_unprepare(gs->pmc_clk);
+		gs->clock_on = false;
+	}
+
+	return ret;
+}
+
+static int camera_sensor_csi_alloc(struct v4l2_subdev *sd, u32 port, u32 lanes,
+				   u32 format, u32 bayer_order)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *csi;
+
+	csi = kzalloc(sizeof(*csi), GFP_KERNEL);
+	if (!csi)
+		return -ENOMEM;
+
+	csi->port = port;
+	csi->num_lanes = lanes;
+	csi->input_format = format;
+	csi->raw_bayer_order = bayer_order;
+	v4l2_set_subdev_hostdata(sd, csi);
+	csi->metadata_format = ATOMISP_INPUT_FORMAT_EMBEDDED;
+	csi->metadata_effective_width = NULL;
+	dev_info(&client->dev,
+		 "camera pdata: port: %d lanes: %d order: %8.8x\n",
+		 port, lanes, bayer_order);
+
+	return 0;
+}
+
+static void camera_sensor_csi_free(struct v4l2_subdev *sd)
+{
+	struct camera_mipi_info *csi;
+
+	csi = v4l2_get_subdev_hostdata(sd);
+	kfree(csi);
+}
+
+static int gmin_csi_cfg(struct v4l2_subdev *sd, int flag)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct gmin_subdev *gs = find_gmin_subdev(sd);
+
+	if (!client || !gs)
+		return -ENODEV;
+
+	if (flag)
+		return camera_sensor_csi_alloc(sd, gs->csi_port, gs->csi_lanes,
+					       gs->csi_fmt, gs->csi_bayer);
+	camera_sensor_csi_free(sd);
+	return 0;
+}
+
+static struct camera_vcm_control *gmin_get_vcm_ctrl(struct v4l2_subdev *subdev,
+	char *camera_module)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	struct camera_vcm_control *vcm;
+
+	if (!client || !gs)
+		return NULL;
+
+	if (!camera_module)
+		return NULL;
+
+	mutex_lock(&vcm_lock);
+	list_for_each_entry(vcm, &vcm_devices, list) {
+		if (!strcmp(camera_module, vcm->camera_module)) {
+			mutex_unlock(&vcm_lock);
+			return vcm;
+		}
+	}
+
+	mutex_unlock(&vcm_lock);
+	return NULL;
+}
+
+static struct camera_sensor_platform_data pmic_gmin_plat = {
+	.gpio0_ctrl = gmin_gpio0_ctrl,
+	.gpio1_ctrl = gmin_gpio1_ctrl,
+	.v1p8_ctrl = gmin_v1p8_ctrl,
+	.v2p8_ctrl = gmin_v2p8_ctrl,
+	.v1p2_ctrl = gmin_v1p2_ctrl,
+	.flisclk_ctrl = gmin_flisclk_ctrl,
+	.csi_cfg = gmin_csi_cfg,
+	.get_vcm_ctrl = gmin_get_vcm_ctrl,
+};
+
+static struct camera_sensor_platform_data acpi_gmin_plat = {
+	.gpio0_ctrl = gmin_gpio0_ctrl,
+	.gpio1_ctrl = gmin_gpio1_ctrl,
+	.v1p8_ctrl = gmin_acpi_pm_ctrl,
+	.v2p8_ctrl = gmin_acpi_pm_ctrl,
+	.v1p2_ctrl = gmin_acpi_pm_ctrl,
+	.flisclk_ctrl = gmin_acpi_pm_ctrl,
+	.csi_cfg = gmin_csi_cfg,
+	.get_vcm_ctrl = gmin_get_vcm_ctrl,
+};
+
+struct camera_sensor_platform_data *
+gmin_camera_platform_data(struct v4l2_subdev *subdev,
+			  enum atomisp_input_format csi_format,
+			  enum atomisp_bayer_order csi_bayer)
+{
+	u8 pmic_i2c_addr = gmin_detect_pmic(subdev);
+	struct gmin_subdev *gs;
+
+	gs = find_free_gmin_subdev_slot();
+	gs->subdev = subdev;
+	gs->csi_fmt = csi_format;
+	gs->csi_bayer = csi_bayer;
+	gs->pwm_i2c_addr = pmic_i2c_addr;
+
+	gmin_subdev_add(gs);
+	if (gs->pmc_clk)
+		return &pmic_gmin_plat;
+	else
+		return &acpi_gmin_plat;
+}
+EXPORT_SYMBOL_GPL(gmin_camera_platform_data);
+
+int atomisp_gmin_register_vcm_control(struct camera_vcm_control *vcmCtrl)
+{
+	if (!vcmCtrl)
+		return -EINVAL;
+
+	mutex_lock(&vcm_lock);
+	list_add_tail(&vcmCtrl->list, &vcm_devices);
+	mutex_unlock(&vcm_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(atomisp_gmin_register_vcm_control);
+
+static int gmin_get_hardcoded_var(struct device *dev,
+				  struct gmin_cfg_var *varlist,
+				  const char *var8, char *out, size_t *out_len)
+{
+	struct gmin_cfg_var *gv;
+
+	for (gv = varlist; gv->name; gv++) {
+		size_t vl;
+
+		if (strcmp(var8, gv->name))
+			continue;
+
+		dev_info(dev, "Found DMI entry for '%s'\n", var8);
+
+		vl = strlen(gv->val);
+		if (vl > *out_len - 1)
+			return -ENOSPC;
+
+		strscpy(out, gv->val, *out_len);
+		*out_len = vl;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+
+static int gmin_get_config_dsm_var(struct device *dev,
+				   const char *var,
+				   char *out, size_t *out_len)
+{
+	acpi_handle handle = ACPI_HANDLE(dev);
+	union acpi_object *obj, *cur = NULL;
+	int i;
+
+	/*
+	 * The data reported by "CamClk" seems to be either 0 or 1 at the
+	 * _DSM table.
+	 *
+	 * At the ACPI tables we looked so far, this is not related to the
+	 * actual clock source for the sensor, which is given by the
+	 * _PR0 ACPI table. So, ignore it, as otherwise this will be
+	 * set to a wrong value.
+	 */
+	if (!strcmp(var, "CamClk"))
+		return -EINVAL;
+
+	/* Return on unexpected object type */
+	obj = acpi_evaluate_dsm_typed(handle, &atomisp_dsm_guid, 0, 0, NULL,
+				      ACPI_TYPE_PACKAGE);
+	if (!obj) {
+		dev_info_once(dev, "Didn't find ACPI _DSM table.\n");
+		return -EINVAL;
+	}
+
+#if 0 /* Just for debugging purposes */
+	for (i = 0; i < obj->package.count; i++) {
+		union acpi_object *cur = &obj->package.elements[i];
+
+		if (cur->type == ACPI_TYPE_INTEGER)
+			dev_info(dev, "object #%d, type %d, value: %lld\n",
+				 i, cur->type, cur->integer.value);
+		else if (cur->type == ACPI_TYPE_STRING)
+			dev_info(dev, "object #%d, type %d, string: %s\n",
+				 i, cur->type, cur->string.pointer);
+		else
+			dev_info(dev, "object #%d, type %d\n",
+				 i, cur->type);
+	}
+#endif
+
+	/* Seek for the desired var */
+	for (i = 0; i < obj->package.count - 1; i += 2) {
+		if (obj->package.elements[i].type == ACPI_TYPE_STRING &&
+		    !strcmp(obj->package.elements[i].string.pointer, var)) {
+			/* Next element should be the required value */
+			cur = &obj->package.elements[i + 1];
+			break;
+		}
+	}
+
+	if (!cur) {
+		dev_info(dev, "didn't found _DSM entry for '%s'\n", var);
+		ACPI_FREE(obj);
+		return -EINVAL;
+	}
+
+	/*
+	 * While it could be possible to have an ACPI_TYPE_INTEGER,
+	 * and read the value from cur->integer.value, the table
+	 * seen so far uses the string type. So, produce a warning
+	 * if it founds something different than string, letting it
+	 * to fall back to the old code.
+	 */
+	if (cur && cur->type != ACPI_TYPE_STRING) {
+		dev_info(dev, "found non-string _DSM entry for '%s'\n", var);
+		ACPI_FREE(obj);
+		return -EINVAL;
+	}
+
+	dev_info(dev, "found _DSM entry for '%s': %s\n", var,
+		 cur->string.pointer);
+	strscpy(out, cur->string.pointer, *out_len);
+	*out_len = strlen(out);
+
+	ACPI_FREE(obj);
+	return 0;
+}
+
+/* Retrieves a device-specific configuration variable.  The dev
+ * argument should be a device with an ACPI companion, as all
+ * configuration is based on firmware ID.
+ */
+static int gmin_get_config_var(struct device *maindev,
+			       bool is_gmin,
+			       const char *var,
+			       char *out, size_t *out_len)
+{
+	efi_char16_t var16[CFG_VAR_NAME_MAX];
+	const struct dmi_system_id *id;
+	struct device *dev = maindev;
+	char var8[CFG_VAR_NAME_MAX];
+	efi_status_t status;
+	int i, ret;
+
+	/* For sensors, try first to use the _DSM table */
+	if (!is_gmin) {
+		ret = gmin_get_config_dsm_var(maindev, var, out, out_len);
+		if (!ret)
+			return 0;
+	}
+
+	/* Fall-back to other approaches */
+
+	if (!is_gmin && ACPI_COMPANION(dev))
+		dev = &ACPI_COMPANION(dev)->dev;
+
+	if (!is_gmin)
+		ret = snprintf(var8, sizeof(var8), "%s_%s", dev_name(dev), var);
+	else
+		ret = snprintf(var8, sizeof(var8), "gmin_%s", var);
+
+	if (ret < 0 || ret >= sizeof(var8) - 1)
+		return -EINVAL;
+
+	/* First check a hard-coded list of board-specific variables.
+	 * Some device firmwares lack the ability to set EFI variables at
+	 * runtime.
+	 */
+	id = dmi_first_match(gmin_vars);
+	if (id) {
+		ret = gmin_get_hardcoded_var(maindev, id->driver_data, var8,
+					     out, out_len);
+		if (!ret)
+			return 0;
+	}
+
+	/* Our variable names are ASCII by construction, but EFI names
+	 * are wide chars.  Convert and zero-pad.
+	 */
+	memset(var16, 0, sizeof(var16));
+	for (i = 0; i < sizeof(var8) && var8[i]; i++)
+		var16[i] = var8[i];
+
+	status = EFI_UNSUPPORTED;
+	if (efi_rt_services_supported(EFI_RT_SUPPORTED_GET_VARIABLE))
+		status = efi.get_variable(var16, &GMIN_CFG_VAR_EFI_GUID, NULL,
+					  (unsigned long *)out_len, out);
+	if (status == EFI_SUCCESS) {
+		dev_info(maindev, "found EFI entry for '%s'\n", var8);
+	} else if (is_gmin) {
+		dev_info(maindev, "Failed to find EFI gmin variable %s\n", var8);
+	} else {
+		dev_info(maindev, "Failed to find EFI variable %s\n", var8);
+	}
+
+	return ret;
+}
+
+int gmin_get_var_int(struct device *dev, bool is_gmin, const char *var, int def)
+{
+	char val[CFG_VAR_NAME_MAX];
+	size_t len = sizeof(val);
+	long result;
+	int ret;
+
+	ret = gmin_get_config_var(dev, is_gmin, var, val, &len);
+	if (!ret) {
+		val[len] = 0;
+		ret = kstrtol(val, 0, &result);
+	} else {
+		dev_info(dev, "%s: using default (%d)\n", var, def);
+	}
+
+	return ret ? def : result;
+}
+EXPORT_SYMBOL_GPL(gmin_get_var_int);
+
+/* PCI quirk: The BYT ISP advertises PCI runtime PM but it doesn't
+ * work.  Disable so the kernel framework doesn't hang the device
+ * trying.  The driver itself does direct calls to the PUNIT to manage
+ * ISP power.
+ */
+static void isp_pm_cap_fixup(struct pci_dev *pdev)
+{
+	dev_info(&pdev->dev, "Disabling PCI power management on camera ISP\n");
+	pdev->pm_cap = 0;
+}
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0f38, isp_pm_cap_fixup);
+
+MODULE_DESCRIPTION("Ancillary routines for binding ACPI devices");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/pci/atomisp_internal.h b/drivers/staging/media/atomisp/pci/atomisp_internal.h
new file mode 100644
index 000000000..d9d158cdf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_internal.h
@@ -0,0 +1,270 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef __ATOMISP_INTERNAL_H__
+#define __ATOMISP_INTERNAL_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/firmware.h>
+#include <linux/kernel.h>
+#include <linux/pm_qos.h>
+#include <linux/idr.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-subdev.h>
+
+/* ISP2400*/
+#include "ia_css_types.h"
+#include "sh_css_legacy.h"
+
+#include "atomisp_csi2.h"
+#include "atomisp_subdev.h"
+#include "atomisp_tpg.h"
+#include "atomisp_compat.h"
+
+#include "gp_device.h"
+#include "irq.h"
+#include <linux/vmalloc.h>
+
+#define V4L2_EVENT_FRAME_END          5
+
+#define IS_HWREVISION(isp, rev) \
+	(((isp)->media_dev.hw_revision & ATOMISP_HW_REVISION_MASK) == \
+	 ((rev) << ATOMISP_HW_REVISION_SHIFT))
+
+#define MAX_STREAM_NUM	2
+
+#define ATOMISP_PCI_DEVICE_SOC_MASK	0xfff8
+/* MRFLD with 0x1178: ISP freq can burst to 457MHz */
+#define ATOMISP_PCI_DEVICE_SOC_MRFLD	0x1178
+/* MRFLD with 0x1179: max ISP freq limited to 400MHz */
+#define ATOMISP_PCI_DEVICE_SOC_MRFLD_1179	0x1179
+/* MRFLD with 0x117a: max ISP freq is 400MHz and max freq at Vmin is 200MHz */
+#define ATOMISP_PCI_DEVICE_SOC_MRFLD_117A	0x117a
+#define ATOMISP_PCI_DEVICE_SOC_BYT	0x0f38
+#define ATOMISP_PCI_DEVICE_SOC_ANN	0x1478
+#define ATOMISP_PCI_DEVICE_SOC_CHT	0x22b8
+
+#define ATOMISP_PCI_REV_MRFLD_A0_MAX	0
+#define ATOMISP_PCI_REV_BYT_A0_MAX	4
+
+#define ATOM_ISP_STEP_WIDTH	2
+#define ATOM_ISP_STEP_HEIGHT	2
+
+#define ATOM_ISP_MIN_WIDTH	4
+#define ATOM_ISP_MIN_HEIGHT	4
+#define ATOM_ISP_MAX_WIDTH	UINT_MAX
+#define ATOM_ISP_MAX_HEIGHT	UINT_MAX
+
+/* sub-QCIF resolution */
+#define ATOM_RESOLUTION_SUBQCIF_WIDTH	128
+#define ATOM_RESOLUTION_SUBQCIF_HEIGHT	96
+
+#define ATOM_ISP_MAX_WIDTH_TMP	1280
+#define ATOM_ISP_MAX_HEIGHT_TMP	720
+
+#define ATOM_ISP_I2C_BUS_1	4
+#define ATOM_ISP_I2C_BUS_2	5
+
+#define ATOM_ISP_POWER_DOWN	0
+#define ATOM_ISP_POWER_UP	1
+
+#define ATOM_ISP_MAX_INPUTS	3
+
+#define ATOMISP_SC_TYPE_SIZE	2
+
+#define ATOMISP_ISP_TIMEOUT_DURATION		(2 * HZ)
+#define ATOMISP_EXT_ISP_TIMEOUT_DURATION        (6 * HZ)
+#define ATOMISP_WDT_KEEP_CURRENT_DELAY          0
+#define ATOMISP_ISP_MAX_TIMEOUT_COUNT	2
+#define ATOMISP_CSS_STOP_TIMEOUT_US	200000
+
+#define ATOMISP_CSS_Q_DEPTH	3
+#define ATOMISP_CSS_EVENTS_MAX  16
+#define ATOMISP_CONT_RAW_FRAMES 15
+#define ATOMISP_METADATA_QUEUE_DEPTH_FOR_HAL	8
+#define ATOMISP_S3A_BUF_QUEUE_DEPTH_FOR_HAL	8
+
+#define ATOMISP_DELAYED_INIT_NOT_QUEUED	0
+#define ATOMISP_DELAYED_INIT_QUEUED	1
+#define ATOMISP_DELAYED_INIT_DONE	2
+
+/*
+ * Define how fast CPU should be able to serve ISP interrupts.
+ * The bigger the value, the higher risk that the ISP is not
+ * triggered sufficiently fast for it to process image during
+ * vertical blanking time, increasing risk of dropped frames.
+ * 1000 us is a reasonable value considering that the processing
+ * time is typically ~2000 us.
+ */
+#define ATOMISP_MAX_ISR_LATENCY	1000
+
+/* Add new YUVPP pipe for SOC sensor. */
+#define ATOMISP_CSS_SUPPORT_YUVPP     1
+
+#define ATOMISP_CSS_OUTPUT_SECOND_INDEX     1
+#define ATOMISP_CSS_OUTPUT_DEFAULT_INDEX    0
+
+/*
+ * ATOMISP_SOC_CAMERA
+ * This is to differentiate between ext-isp and soc camera in
+ * Moorefield/Baytrail platform.
+ */
+#define ATOMISP_SOC_CAMERA(asd)  \
+	(asd->isp->inputs[asd->input_curr].type == SOC_CAMERA)
+
+#define ATOMISP_USE_YUVPP(asd)  \
+	(ATOMISP_SOC_CAMERA(asd) && ATOMISP_CSS_SUPPORT_YUVPP && \
+	!asd->copy_mode)
+
+#define ATOMISP_DEPTH_SENSOR_STREAMON_COUNT 2
+
+#define ATOMISP_DEPTH_DEFAULT_MASTER_SENSOR 0
+#define ATOMISP_DEPTH_DEFAULT_SLAVE_SENSOR 1
+
+/* ISP2401 */
+#define ATOMISP_ION_DEVICE_FD_OFFSET   16
+#define ATOMISP_ION_SHARED_FD_MASK     (0xFFFF)
+#define ATOMISP_ION_DEVICE_FD_MASK     (~ATOMISP_ION_SHARED_FD_MASK)
+#define ION_FD_UNSET (-1)
+
+#define DIV_NEAREST_STEP(n, d, step) \
+	round_down((2 * (n) + (d) * (step)) / (2 * (d)), (step))
+
+struct atomisp_input_subdev {
+	unsigned int type;
+	enum atomisp_camera_port port;
+	struct v4l2_subdev *camera;
+	struct v4l2_subdev *motor;
+	struct v4l2_frmsizeenum frame_size;
+
+	/*
+	 * To show this resource is used by
+	 * which stream, in ISP multiple stream mode
+	 */
+	struct atomisp_sub_device *asd;
+
+	int sensor_index;
+};
+
+enum atomisp_dfs_mode {
+	ATOMISP_DFS_MODE_AUTO = 0,
+	ATOMISP_DFS_MODE_LOW,
+	ATOMISP_DFS_MODE_MAX,
+};
+
+struct atomisp_regs {
+	/* PCI config space info */
+	u16 pcicmdsts;
+	u32 ispmmadr;
+	u32 msicap;
+	u32 msi_addr;
+	u16 msi_data;
+	u8 intr;
+	u32 interrupt_control;
+	u32 pmcs;
+	u32 cg_dis;
+	u32 i_control;
+
+	/* I-Unit PHY related info */
+	u32 csi_rcomp_config;
+	u32 csi_afe_dly;
+	u32 csi_control;
+
+	/* New for MRFLD */
+	u32 csi_afe_rcomp_config;
+	u32 csi_afe_hs_control;
+	u32 csi_deadline_control;
+	u32 csi_access_viol;
+};
+
+struct atomisp_sw_contex {
+	int power_state;
+	int running_freq;
+};
+
+#define ATOMISP_DEVICE_STREAMING_DISABLED	0
+#define ATOMISP_DEVICE_STREAMING_ENABLED	1
+#define ATOMISP_DEVICE_STREAMING_STOPPING	2
+
+/*
+ * ci device struct
+ */
+struct atomisp_device {
+	struct device *dev;
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct atomisp_platform_data *pdata;
+	void *mmu_l1_base;
+	void __iomem *base;
+	const struct firmware *firmware;
+
+	struct pm_qos_request pm_qos;
+	s32 max_isr_latency;
+
+	/*
+	 * ISP modules
+	 * Multiple streams are represents by multiple
+	 * atomisp_sub_device instances
+	 */
+	struct atomisp_sub_device *asd;
+	/*
+	 * this will be assigned dyanamically.
+	 * For Merr/BTY(ISP2400), 2 streams are supported.
+	 */
+	unsigned int num_of_streams;
+
+	struct atomisp_mipi_csi2_device csi2_port[ATOMISP_CAMERA_NR_PORTS];
+	struct atomisp_tpg_device tpg;
+
+	/* Purpose of mutex is to protect and serialize use of isp data
+	 * structures and css API calls. */
+	struct mutex mutex;
+
+	unsigned int input_cnt;
+	struct atomisp_input_subdev inputs[ATOM_ISP_MAX_INPUTS];
+	struct v4l2_subdev *flash;
+	struct v4l2_subdev *motor;
+
+	struct atomisp_regs saved_regs;
+	struct atomisp_sw_contex sw_contex;
+	struct atomisp_css_env css_env;
+
+	/* isp timeout status flag */
+	bool isp_timeout;
+	bool isp_fatal_error;
+	struct work_struct assert_recovery_work;
+
+	spinlock_t lock; /* Protects asd[i].streaming */
+
+	bool need_gfx_throttle;
+
+	unsigned int mipi_frame_size;
+	const struct atomisp_dfs_config *dfs;
+	unsigned int hpll_freq;
+
+	bool css_initialized;
+};
+
+#define v4l2_dev_to_atomisp_device(dev) \
+	container_of(dev, struct atomisp_device, v4l2_dev)
+
+extern struct device *atomisp_dev;
+
+#endif /* __ATOMISP_INTERNAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_ioctl.c b/drivers/staging/media/atomisp/pci/atomisp_ioctl.c
new file mode 100644
index 000000000..0ddb0ed42
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_ioctl.c
@@ -0,0 +1,2738 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/delay.h>
+#include <linux/pci.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf-vmalloc.h>
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_fops.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+#include "atomisp-regs.h"
+#include "atomisp_compat.h"
+
+#include "sh_css_hrt.h"
+
+#include "gp_device.h"
+#include "device_access.h"
+#include "irq.h"
+
+static const char *DRIVER = "atomisp";	/* max size 15 */
+static const char *CARD = "ATOM ISP";	/* max size 31 */
+
+/*
+ * FIXME: ISP should not know beforehand all CIDs supported by sensor.
+ * Instead, it needs to propagate to sensor unkonwn CIDs.
+ */
+static struct v4l2_queryctrl ci_v4l2_controls[] = {
+	{
+		.id = V4L2_CID_AUTO_WHITE_BALANCE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Automatic White Balance",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_RED_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Red Balance",
+		.minimum = 0x00,
+		.maximum = 0xff,
+		.step = 1,
+		.default_value = 0x00,
+	},
+	{
+		.id = V4L2_CID_BLUE_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Blue Balance",
+		.minimum = 0x00,
+		.maximum = 0xff,
+		.step = 1,
+		.default_value = 0x00,
+	},
+	{
+		.id = V4L2_CID_GAMMA,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Gamma",
+		.minimum = 0x00,
+		.maximum = 0xff,
+		.step = 1,
+		.default_value = 0x00,
+	},
+	{
+		.id = V4L2_CID_POWER_LINE_FREQUENCY,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.name = "Light frequency filter",
+		.minimum = 1,
+		.maximum = 2,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_COLORFX,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Image Color Effect",
+		.minimum = 0,
+		.maximum = 9,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_COLORFX_CBCR,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Image Color Effect CbCr",
+		.minimum = 0,
+		.maximum = 0xffff,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_BAD_PIXEL_DETECTION,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Bad Pixel Correction",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_POSTPROCESS_GDC_CAC,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "GDC/CAC",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_VIDEO_STABLIZATION,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Video Stablization",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_FIXED_PATTERN_NR,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Fixed Pattern Noise Reduction",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_FALSE_COLOR_CORRECTION,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "False Color Correction",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_REQUEST_FLASH,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Request flash frames",
+		.minimum = 0,
+		.maximum = 10,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_ATOMISP_LOW_LIGHT,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Low light mode",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Horizontal binning factor",
+		.minimum = 0,
+		.maximum = 10,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Vertical binning factor",
+		.minimum = 0,
+		.maximum = 10,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_2A_STATUS,
+		.type = V4L2_CTRL_TYPE_BITMASK,
+		.name = "AE and AWB status",
+		.minimum = 0,
+		.maximum = V4L2_2A_STATUS_AE_READY | V4L2_2A_STATUS_AWB_READY,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "exposure",
+		.minimum = -4,
+		.maximum = 4,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_ZONE_NUM,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "one-time exposure zone number",
+		.minimum = 0x0,
+		.maximum = 0xffff,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_AUTO_PRIORITY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Exposure auto priority",
+		.minimum = V4L2_EXPOSURE_AUTO,
+		.maximum = V4L2_EXPOSURE_APERTURE_PRIORITY,
+		.step = 1,
+		.default_value = V4L2_EXPOSURE_AUTO,
+	},
+	{
+		.id = V4L2_CID_SCENE_MODE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "scene mode",
+		.minimum = 0,
+		.maximum = 13,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ISO_SENSITIVITY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "iso",
+		.minimum = -4,
+		.maximum = 4,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_ISO_SENSITIVITY_AUTO,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "iso mode",
+		.minimum = V4L2_ISO_SENSITIVITY_MANUAL,
+		.maximum = V4L2_ISO_SENSITIVITY_AUTO,
+		.step = 1,
+		.default_value = V4L2_ISO_SENSITIVITY_AUTO,
+	},
+	{
+		.id = V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "white balance",
+		.minimum = 0,
+		.maximum = 9,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_METERING,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.name = "metering",
+		.minimum = 0,
+		.maximum = 3,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_3A_LOCK,
+		.type = V4L2_CTRL_TYPE_BITMASK,
+		.name = "3a lock",
+		.minimum = 0,
+		.maximum = V4L2_LOCK_EXPOSURE | V4L2_LOCK_WHITE_BALANCE
+		| V4L2_LOCK_FOCUS,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_TEST_PATTERN,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test Pattern",
+		.minimum = 0,
+		.maximum = 0xffff,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_TEST_PATTERN_COLOR_R,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test Pattern Solid Color R",
+		.minimum = INT_MIN,
+		.maximum = INT_MAX,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_TEST_PATTERN_COLOR_GR,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test Pattern Solid Color GR",
+		.minimum = INT_MIN,
+		.maximum = INT_MAX,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_TEST_PATTERN_COLOR_GB,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test Pattern Solid Color GB",
+		.minimum = INT_MIN,
+		.maximum = INT_MAX,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_TEST_PATTERN_COLOR_B,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test Pattern Solid Color B",
+		.minimum = INT_MIN,
+		.maximum = INT_MAX,
+		.step = 1,
+		.default_value = 0,
+	},
+};
+
+static const u32 ctrls_num = ARRAY_SIZE(ci_v4l2_controls);
+
+/*
+ * supported V4L2 fmts and resolutions
+ */
+const struct atomisp_format_bridge atomisp_output_fmts[] = {
+	{
+		.pixelformat = V4L2_PIX_FMT_YUV420,
+		.depth = 12,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_YUV420,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_YUV420,
+		.description = "YUV420, planar",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_YVU420,
+		.depth = 12,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_YVU420,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_YV12,
+		.description = "YVU420, planar",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_YUV422P,
+		.depth = 16,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_YUV422P,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_YUV422,
+		.description = "YUV422, planar",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_YUV444,
+		.depth = 24,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_YUV444,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_YUV444,
+		.description = "YUV444"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_NV12,
+		.depth = 12,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_NV12,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_NV12,
+		.description = "NV12, Y-plane, CbCr interleaved",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_NV21,
+		.depth = 12,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_NV21,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_NV21,
+		.description = "NV21, Y-plane, CbCr interleaved",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_NV16,
+		.depth = 16,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_NV16,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_NV16,
+		.description = "NV16, Y-plane, CbCr interleaved",
+		.planar = true
+	}, {
+		.pixelformat = V4L2_PIX_FMT_YUYV,
+		.depth = 16,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_YUYV,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_YUYV,
+		.description = "YUYV, interleaved"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_UYVY,
+		.description = "UYVY, interleaved"
+	}, { /* This one is for parallel sensors! DO NOT USE! */
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_UYVY,
+		.description = "UYVY, interleaved"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR16,
+		.depth = 16,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_SBGGR16,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 16"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_RGB32,
+		.depth = 32,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_RGB32,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RGBA888,
+		.description = "32 RGB 8-8-8-8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_BGR565_2X8_LE,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_RGB565,
+		.description = "16 RGB 5-6-5"
+#if 0
+	}, {
+		.pixelformat = V4L2_PIX_FMT_JPEG,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_JPEG_1X8,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_BINARY_8,
+		.description = "JPEG"
+	}, {
+		/* This is a custom format being used by M10MO to send the RAW data */
+		.pixelformat = V4L2_PIX_FMT_CUSTOM_M10MO_RAW,
+		.depth = 8,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_M10MO_RAW,
+		.sh_fmt = IA_CSS_FRAME_FORMAT_BINARY_8,
+		.description = "Custom RAW for M10MO"
+#endif
+	},
+};
+
+const struct atomisp_format_bridge *
+atomisp_get_format_bridge(unsigned int pixelformat)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_output_fmts); i++) {
+		if (atomisp_output_fmts[i].pixelformat == pixelformat)
+			return &atomisp_output_fmts[i];
+	}
+
+	return NULL;
+}
+
+const struct atomisp_format_bridge *
+atomisp_get_format_bridge_from_mbus(u32 mbus_code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_output_fmts); i++) {
+		if (mbus_code == atomisp_output_fmts[i].mbus_code)
+			return &atomisp_output_fmts[i];
+	}
+
+	return NULL;
+}
+
+int atomisp_pipe_check(struct atomisp_video_pipe *pipe, bool settings_change)
+{
+	lockdep_assert_held(&pipe->isp->mutex);
+
+	if (pipe->isp->isp_fatal_error)
+		return -EIO;
+
+	switch (pipe->asd->streaming) {
+	case ATOMISP_DEVICE_STREAMING_DISABLED:
+		break;
+	case ATOMISP_DEVICE_STREAMING_ENABLED:
+		if (settings_change) {
+			dev_err(pipe->isp->dev, "Set fmt/input IOCTL while streaming\n");
+			return -EBUSY;
+		}
+		break;
+	case ATOMISP_DEVICE_STREAMING_STOPPING:
+		dev_err(pipe->isp->dev, "IOCTL issued while stopping\n");
+		return -EBUSY;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * v4l2 ioctls
+ * return ISP capabilities
+ */
+static int atomisp_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+
+	strscpy(cap->driver, DRIVER, sizeof(cap->driver));
+	strscpy(cap->card, CARD, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s", dev_name(isp->dev));
+
+	return 0;
+}
+
+/*
+ * enum input are used to check primary/secondary camera
+ */
+static int atomisp_enum_input(struct file *file, void *fh,
+			      struct v4l2_input *input)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int index = input->index;
+	struct v4l2_subdev *motor;
+
+	if (index >= isp->input_cnt)
+		return -EINVAL;
+
+	if (!isp->inputs[index].camera)
+		return -EINVAL;
+
+	memset(input, 0, sizeof(struct v4l2_input));
+	strscpy(input->name, isp->inputs[index].camera->name,
+		sizeof(input->name));
+
+	/*
+	 * HACK: append actuator's name to sensor's
+	 * As currently userspace can't talk directly to subdev nodes, this
+	 * ioctl is the only way to enum inputs + possible external actuators
+	 * for 3A tuning purpose.
+	 */
+	if (!IS_ISP2401)
+		motor = isp->inputs[index].motor;
+	else
+		motor = isp->motor;
+
+	if (motor && strlen(motor->name) > 0) {
+		const int cur_len = strlen(input->name);
+		const int max_size = sizeof(input->name) - cur_len - 1;
+
+		if (max_size > 1) {
+			input->name[cur_len] = '+';
+			strscpy(&input->name[cur_len + 1],
+				motor->name, max_size);
+		}
+	}
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->index = index;
+	input->reserved[0] = isp->inputs[index].type;
+	input->reserved[1] = isp->inputs[index].port;
+
+	return 0;
+}
+
+static unsigned int
+atomisp_subdev_streaming_count(struct atomisp_sub_device *asd)
+{
+	return asd->video_out_preview.capq.streaming
+	       + asd->video_out_capture.capq.streaming
+	       + asd->video_out_video_capture.capq.streaming
+	       + asd->video_out_vf.capq.streaming;
+}
+
+unsigned int atomisp_streaming_count(struct atomisp_device *isp)
+{
+	unsigned int i, sum;
+
+	for (i = 0, sum = 0; i < isp->num_of_streams; i++)
+		sum += isp->asd[i].streaming ==
+		       ATOMISP_DEVICE_STREAMING_ENABLED;
+
+	return sum;
+}
+
+/*
+ * get input are used to get current primary/secondary camera
+ */
+static int atomisp_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+
+	*input = asd->input_curr;
+	return 0;
+}
+
+/*
+ * set input are used to set current primary/secondary camera
+ */
+static int atomisp_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct v4l2_subdev *camera = NULL;
+	struct v4l2_subdev *motor;
+	int ret;
+
+	ret = atomisp_pipe_check(pipe, true);
+	if (ret)
+		return ret;
+
+	if (input >= ATOM_ISP_MAX_INPUTS || input >= isp->input_cnt) {
+		dev_dbg(isp->dev, "input_cnt: %d\n", isp->input_cnt);
+		return -EINVAL;
+	}
+
+	/*
+	 * check whether the request camera:
+	 * 1: already in use
+	 * 2: if in use, whether it is used by other streams
+	 */
+	if (isp->inputs[input].asd && isp->inputs[input].asd != asd) {
+		dev_err(isp->dev,
+			"%s, camera is already used by stream: %d\n", __func__,
+			isp->inputs[input].asd->index);
+		return -EBUSY;
+	}
+
+	camera = isp->inputs[input].camera;
+	if (!camera) {
+		dev_err(isp->dev, "%s, no camera\n", __func__);
+		return -EINVAL;
+	}
+
+	/* power off the current owned sensor, as it is not used this time */
+	if (isp->inputs[asd->input_curr].asd == asd &&
+	    asd->input_curr != input) {
+		ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				       core, s_power, 0);
+		if (ret)
+			dev_warn(isp->dev,
+				 "Failed to power-off sensor\n");
+		/* clear the asd field to show this camera is not used */
+		isp->inputs[asd->input_curr].asd = NULL;
+	}
+
+	/* powe on the new sensor */
+	ret = v4l2_subdev_call(isp->inputs[input].camera, core, s_power, 1);
+	if (ret) {
+		dev_err(isp->dev, "Failed to power-on sensor\n");
+		return ret;
+	}
+	/*
+	 * Some sensor driver resets the run mode during power-on, thus force
+	 * update the run mode to sensor after power-on.
+	 */
+	atomisp_update_run_mode(asd);
+
+	/* select operating sensor */
+	ret = v4l2_subdev_call(isp->inputs[input].camera, video, s_routing,
+			       0, isp->inputs[input].sensor_index, 0);
+	if (ret && (ret != -ENOIOCTLCMD)) {
+		dev_err(isp->dev, "Failed to select sensor\n");
+		return ret;
+	}
+
+	if (!IS_ISP2401) {
+		motor = isp->inputs[input].motor;
+	} else {
+		motor = isp->motor;
+		if (motor)
+			ret = v4l2_subdev_call(motor, core, s_power, 1);
+	}
+
+	if (motor)
+		ret = v4l2_subdev_call(motor, core, init, 1);
+
+	asd->input_curr = input;
+	/* mark this camera is used by the current stream */
+	isp->inputs[input].asd = asd;
+
+	return 0;
+}
+
+static int atomisp_enum_framesizes(struct file *file, void *priv,
+				   struct v4l2_frmsizeenum *fsize)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = fse.max_width - pad_w;
+	fsize->discrete.height = fse.max_height - pad_h;
+
+	return 0;
+}
+
+static int atomisp_enum_frameintervals(struct file *file, void *priv,
+				       struct v4l2_frmivalenum *fival)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.code	= atomisp_in_fmt_conv[0].code,
+		.index = fival->index,
+		.width = fival->width,
+		.height = fival->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       pad, enum_frame_interval, NULL,
+			       &fie);
+	if (ret)
+		return ret;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return ret;
+}
+
+static int atomisp_enum_fmt_cap(struct file *file, void *fh,
+				struct v4l2_fmtdesc *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	const struct atomisp_format_bridge *format;
+	struct v4l2_subdev *camera;
+	unsigned int i, fi = 0;
+	int rval;
+
+	camera = isp->inputs[asd->input_curr].camera;
+	if(!camera) {
+		dev_err(isp->dev, "%s(): camera is NULL, device is %s\n",
+			__func__, vdev->name);
+		return -EINVAL;
+	}
+
+	rval = v4l2_subdev_call(camera, pad, enum_mbus_code, NULL, &code);
+	if (rval == -ENOIOCTLCMD) {
+		dev_warn(isp->dev,
+			 "enum_mbus_code pad op not supported by %s. Please fix your sensor driver!\n",
+			 camera->name);
+	}
+
+	if (rval)
+		return rval;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_output_fmts); i++) {
+		format = &atomisp_output_fmts[i];
+
+		/*
+		 * Is the atomisp-supported format is valid for the
+		 * sensor (configuration)? If not, skip it.
+		 *
+		 * FIXME: fix the pipeline to allow sensor format too.
+		 */
+		if (format->sh_fmt == IA_CSS_FRAME_FORMAT_RAW)
+			continue;
+
+		/* Found a match. Now let's pick f->index'th one. */
+		if (fi < f->index) {
+			fi++;
+			continue;
+		}
+
+		strscpy(f->description, format->description,
+			sizeof(f->description));
+		f->pixelformat = format->pixelformat;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int atomisp_adjust_fmt(struct v4l2_format *f)
+{
+	const struct atomisp_format_bridge *format_bridge;
+	u32 padded_width;
+
+	format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+
+	padded_width = f->fmt.pix.width + pad_w;
+
+	if (format_bridge->planar) {
+		f->fmt.pix.bytesperline = padded_width;
+		f->fmt.pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height *
+						  DIV_ROUND_UP(format_bridge->depth *
+						  padded_width, 8));
+	} else {
+		f->fmt.pix.bytesperline = DIV_ROUND_UP(format_bridge->depth *
+						      padded_width, 8);
+		f->fmt.pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height * f->fmt.pix.bytesperline);
+	}
+
+	if (f->fmt.pix.field == V4L2_FIELD_ANY)
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+	if (!format_bridge)
+		return -EINVAL;
+
+	/* Currently, raw formats are broken!!! */
+	if (format_bridge->sh_fmt == IA_CSS_FRAME_FORMAT_RAW) {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+
+		format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+		if (!format_bridge)
+			return -EINVAL;
+	}
+
+	padded_width = f->fmt.pix.width + pad_w;
+
+	if (format_bridge->planar) {
+		f->fmt.pix.bytesperline = padded_width;
+		f->fmt.pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height *
+						  DIV_ROUND_UP(format_bridge->depth *
+						  padded_width, 8));
+	} else {
+		f->fmt.pix.bytesperline = DIV_ROUND_UP(format_bridge->depth *
+						      padded_width, 8);
+		f->fmt.pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height * f->fmt.pix.bytesperline);
+	}
+
+	if (f->fmt.pix.field == V4L2_FIELD_ANY)
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	/*
+	 * FIXME: do we need to setup this differently, depending on the
+	 * sensor or the pipeline?
+	 */
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+	f->fmt.pix.ycbcr_enc = V4L2_YCBCR_ENC_709;
+	f->fmt.pix.xfer_func = V4L2_XFER_FUNC_709;
+
+	f->fmt.pix.width -= pad_w;
+	f->fmt.pix.height -= pad_h;
+
+	return 0;
+}
+
+/* This function looks up the closest available resolution. */
+static int atomisp_try_fmt_cap(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	int ret;
+
+	/*
+	 * atomisp_try_fmt() gived results with padding included, note
+	 * (this gets removed again by the atomisp_adjust_fmt() call below.
+	 */
+	f->fmt.pix.width += pad_w;
+	f->fmt.pix.height += pad_h;
+
+	ret = atomisp_try_fmt(vdev, &f->fmt.pix, NULL);
+	if (ret)
+		return ret;
+
+	return atomisp_adjust_fmt(f);
+}
+
+static int atomisp_g_fmt_cap(struct file *file, void *fh,
+			     struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe;
+
+	pipe = atomisp_to_video_pipe(vdev);
+
+	f->fmt.pix = pipe->pix;
+
+	/* If s_fmt was issued, just return whatever is was previouly set */
+	if (f->fmt.pix.sizeimage)
+		return 0;
+
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+	f->fmt.pix.width = 10000;
+	f->fmt.pix.height = 10000;
+
+	return atomisp_try_fmt_cap(file, fh, f);
+}
+
+/*
+ * Free videobuffer buffer priv data
+ */
+void atomisp_videobuf_free_buf(struct videobuf_buffer *vb)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+
+	if (!vb)
+		return;
+
+	vm_mem = vb->priv;
+	if (vm_mem && vm_mem->vaddr) {
+		ia_css_frame_free(vm_mem->vaddr);
+		vm_mem->vaddr = NULL;
+	}
+}
+
+/*
+ * this function is used to free video buffer queue
+ */
+static void atomisp_videobuf_free_queue(struct videobuf_queue *q)
+{
+	int i;
+
+	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+		atomisp_videobuf_free_buf(q->bufs[i]);
+		kfree(q->bufs[i]);
+		q->bufs[i] = NULL;
+	}
+}
+
+int atomisp_alloc_css_stat_bufs(struct atomisp_sub_device *asd,
+				uint16_t stream_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_s3a_buf *s3a_buf = NULL, *_s3a_buf;
+	struct atomisp_dis_buf *dis_buf = NULL, *_dis_buf;
+	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf;
+	int count;
+	struct ia_css_dvs_grid_info *dvs_grid_info =
+	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	unsigned int i;
+
+	if (list_empty(&asd->s3a_stats) &&
+	    asd->params.curr_grid_info.s3a_grid.enable) {
+		count = ATOMISP_CSS_Q_DEPTH +
+			ATOMISP_S3A_BUF_QUEUE_DEPTH_FOR_HAL;
+		dev_dbg(isp->dev, "allocating %d 3a buffers\n", count);
+		while (count--) {
+			s3a_buf = kzalloc(sizeof(struct atomisp_s3a_buf), GFP_KERNEL);
+			if (!s3a_buf)
+				goto error;
+
+			if (atomisp_css_allocate_stat_buffers(
+				asd, stream_id, s3a_buf, NULL, NULL)) {
+				kfree(s3a_buf);
+				goto error;
+			}
+
+			list_add_tail(&s3a_buf->list, &asd->s3a_stats);
+		}
+	}
+
+	if (list_empty(&asd->dis_stats) && dvs_grid_info &&
+	    dvs_grid_info->enable) {
+		count = ATOMISP_CSS_Q_DEPTH + 1;
+		dev_dbg(isp->dev, "allocating %d dis buffers\n", count);
+		while (count--) {
+			dis_buf = kzalloc(sizeof(struct atomisp_dis_buf), GFP_KERNEL);
+			if (!dis_buf)
+				goto error;
+			if (atomisp_css_allocate_stat_buffers(
+				asd, stream_id, NULL, dis_buf, NULL)) {
+				kfree(dis_buf);
+				goto error;
+			}
+
+			list_add_tail(&dis_buf->list, &asd->dis_stats);
+		}
+	}
+
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		if (list_empty(&asd->metadata[i]) &&
+		    list_empty(&asd->metadata_ready[i]) &&
+		    list_empty(&asd->metadata_in_css[i])) {
+			count = ATOMISP_CSS_Q_DEPTH +
+				ATOMISP_METADATA_QUEUE_DEPTH_FOR_HAL;
+			dev_dbg(isp->dev, "allocating %d metadata buffers for type %d\n",
+				count, i);
+			while (count--) {
+				md_buf = kzalloc(sizeof(struct atomisp_metadata_buf),
+						 GFP_KERNEL);
+				if (!md_buf)
+					goto error;
+
+				if (atomisp_css_allocate_stat_buffers(
+					asd, stream_id, NULL, NULL, md_buf)) {
+					kfree(md_buf);
+					goto error;
+				}
+				list_add_tail(&md_buf->list, &asd->metadata[i]);
+			}
+		}
+	}
+	return 0;
+
+error:
+	dev_err(isp->dev, "failed to allocate statistics buffers\n");
+
+	list_for_each_entry_safe(dis_buf, _dis_buf, &asd->dis_stats, list) {
+		atomisp_css_free_dis_buffer(dis_buf);
+		list_del(&dis_buf->list);
+		kfree(dis_buf);
+	}
+
+	list_for_each_entry_safe(s3a_buf, _s3a_buf, &asd->s3a_stats, list) {
+		atomisp_css_free_3a_buffer(s3a_buf);
+		list_del(&s3a_buf->list);
+		kfree(s3a_buf);
+	}
+
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		list_for_each_entry_safe(md_buf, _md_buf, &asd->metadata[i],
+					 list) {
+			atomisp_css_free_metadata_buffer(md_buf);
+			list_del(&md_buf->list);
+			kfree(md_buf);
+		}
+	}
+	return -ENOMEM;
+}
+
+/*
+ * Initiate Memory Mapping or User Pointer I/O
+ */
+int atomisp_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *req)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct ia_css_frame_info frame_info;
+	struct ia_css_frame *frame;
+	struct videobuf_vmalloc_memory *vm_mem;
+	u16 source_pad = atomisp_subdev_source_pad(vdev);
+	int ret = 0, i = 0;
+
+	if (req->count == 0) {
+		mutex_lock(&pipe->capq.vb_lock);
+		if (!list_empty(&pipe->capq.stream))
+			videobuf_queue_cancel(&pipe->capq);
+
+		atomisp_videobuf_free_queue(&pipe->capq);
+		mutex_unlock(&pipe->capq.vb_lock);
+		/* clear request config id */
+		memset(pipe->frame_request_config_id, 0,
+		       VIDEO_MAX_FRAME * sizeof(unsigned int));
+		memset(pipe->frame_params, 0,
+		       VIDEO_MAX_FRAME *
+		       sizeof(struct atomisp_css_params_with_list *));
+		return 0;
+	}
+
+	ret = videobuf_reqbufs(&pipe->capq, req);
+	if (ret)
+		return ret;
+
+	atomisp_alloc_css_stat_bufs(asd, ATOMISP_INPUT_STREAM_GENERAL);
+
+	/*
+	 * for user pointer type, buffers are not really allocated here,
+	 * buffers are setup in QBUF operation through v4l2_buffer structure
+	 */
+	if (req->memory == V4L2_MEMORY_USERPTR)
+		return 0;
+
+	ret = atomisp_get_css_frame_info(asd, source_pad, &frame_info);
+	if (ret)
+		return ret;
+
+	/*
+	 * Allocate the real frame here for selected node using our
+	 * memory management function
+	 */
+	for (i = 0; i < req->count; i++) {
+		if (ia_css_frame_allocate_from_info(&frame, &frame_info))
+			goto error;
+		vm_mem = pipe->capq.bufs[i]->priv;
+		vm_mem->vaddr = frame;
+	}
+
+	return ret;
+
+error:
+	while (i--) {
+		vm_mem = pipe->capq.bufs[i]->priv;
+		ia_css_frame_free(vm_mem->vaddr);
+	}
+
+	if (asd->vf_frame)
+		ia_css_frame_free(asd->vf_frame);
+
+	return -ENOMEM;
+}
+
+/* application query the status of a buffer */
+static int atomisp_querybuf(struct file *file, void *fh,
+			    struct v4l2_buffer *buf)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+
+	return videobuf_querybuf(&pipe->capq, buf);
+}
+
+/*
+ * Applications call the VIDIOC_QBUF ioctl to enqueue an empty (capturing) or
+ * filled (output) buffer in the drivers incoming queue.
+ */
+static int atomisp_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	static const int NOFLUSH_FLAGS = V4L2_BUF_FLAG_NO_CACHE_INVALIDATE |
+					 V4L2_BUF_FLAG_NO_CACHE_CLEAN;
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct videobuf_buffer *vb;
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct ia_css_frame_info frame_info;
+	struct ia_css_frame *handle = NULL;
+	u32 length;
+	u32 pgnr;
+	int ret;
+
+	ret = atomisp_pipe_check(pipe, false);
+	if (ret)
+		return ret;
+
+	if (!buf || buf->index >= VIDEO_MAX_FRAME ||
+	    !pipe->capq.bufs[buf->index]) {
+		dev_err(isp->dev, "Invalid index for qbuf.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * For userptr type frame, we convert user space address to physic
+	 * address and reprograme out page table properly
+	 */
+	if (buf->memory == V4L2_MEMORY_USERPTR) {
+		if (offset_in_page(buf->m.userptr)) {
+			dev_err(isp->dev, "Error userptr is not page aligned.\n");
+			return -EINVAL;
+		}
+
+		vb = pipe->capq.bufs[buf->index];
+		vm_mem = vb->priv;
+		if (!vm_mem)
+			return -EINVAL;
+
+		length = vb->bsize;
+		pgnr = (length + (PAGE_SIZE - 1)) >> PAGE_SHIFT;
+
+		if (vb->baddr == buf->m.userptr && vm_mem->vaddr)
+			goto done;
+
+		if (atomisp_get_css_frame_info(asd,
+					       atomisp_subdev_source_pad(vdev), &frame_info))
+			return -EIO;
+
+		ret = ia_css_frame_map(&handle, &frame_info,
+					    (void __user *)buf->m.userptr,
+					    pgnr);
+		if (ret) {
+			dev_err(isp->dev, "Failed to map user buffer\n");
+			return ret;
+		}
+
+		if (vm_mem->vaddr) {
+			mutex_lock(&pipe->capq.vb_lock);
+			ia_css_frame_free(vm_mem->vaddr);
+			vm_mem->vaddr = NULL;
+			vb->state = VIDEOBUF_NEEDS_INIT;
+			mutex_unlock(&pipe->capq.vb_lock);
+		}
+
+		vm_mem->vaddr = handle;
+
+		buf->flags &= ~V4L2_BUF_FLAG_MAPPED;
+		buf->flags |= V4L2_BUF_FLAG_QUEUED;
+		buf->flags &= ~V4L2_BUF_FLAG_DONE;
+	} else if (buf->memory == V4L2_MEMORY_MMAP) {
+		buf->flags |= V4L2_BUF_FLAG_MAPPED;
+		buf->flags |= V4L2_BUF_FLAG_QUEUED;
+		buf->flags &= ~V4L2_BUF_FLAG_DONE;
+
+		/*
+		 * For mmap, frames were allocated at request buffers
+		 */
+	}
+
+done:
+	if (!((buf->flags & NOFLUSH_FLAGS) == NOFLUSH_FLAGS))
+		wbinvd();
+
+	if (!atomisp_is_vf_pipe(pipe) &&
+	    (buf->reserved2 & ATOMISP_BUFFER_HAS_PER_FRAME_SETTING)) {
+		/* this buffer will have a per-frame parameter */
+		pipe->frame_request_config_id[buf->index] = buf->reserved2 &
+			~ATOMISP_BUFFER_HAS_PER_FRAME_SETTING;
+		dev_dbg(isp->dev,
+			"This buffer requires per_frame setting which has isp_config_id %d\n",
+			pipe->frame_request_config_id[buf->index]);
+	} else {
+		pipe->frame_request_config_id[buf->index] = 0;
+	}
+
+	pipe->frame_params[buf->index] = NULL;
+
+	mutex_unlock(&isp->mutex);
+	ret = videobuf_qbuf(&pipe->capq, buf);
+	mutex_lock(&isp->mutex);
+	if (ret)
+		return ret;
+
+	/* TODO: do this better, not best way to queue to css */
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
+		if (!list_empty(&pipe->buffers_waiting_for_param)) {
+			atomisp_handle_parameter_and_buffer(pipe);
+		} else {
+			atomisp_qbuffers_to_css(asd);
+		}
+	}
+
+	/*
+	 * Workaround: Due to the design of HALv3,
+	 * sometimes in ZSL or SDV mode HAL needs to
+	 * capture multiple images within one streaming cycle.
+	 * But the capture number cannot be determined by HAL.
+	 * So HAL only sets the capture number to be 1 and queue multiple
+	 * buffers. Atomisp driver needs to check this case and re-trigger
+	 * CSS to do capture when new buffer is queued.
+	 */
+	if (asd->continuous_mode->val &&
+	    atomisp_subdev_source_pad(vdev)
+	    == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE &&
+	    pipe->capq.streaming &&
+	    !asd->enable_raw_buffer_lock->val &&
+	    asd->params.offline_parm.num_captures == 1) {
+			asd->pending_capture_request++;
+			dev_dbg(isp->dev, "Add one pending capture request.\n");
+	}
+
+	dev_dbg(isp->dev, "qbuf buffer %d (%s) for asd%d\n", buf->index,
+		vdev->name, asd->index);
+
+	return 0;
+}
+
+static int __get_frame_exp_id(struct atomisp_video_pipe *pipe,
+			      struct v4l2_buffer *buf)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct ia_css_frame *handle;
+	int i;
+
+	for (i = 0; pipe->capq.bufs[i]; i++) {
+		vm_mem = pipe->capq.bufs[i]->priv;
+		handle = vm_mem->vaddr;
+		if (buf->index == pipe->capq.bufs[i]->i && handle)
+			return handle->exp_id;
+	}
+	return -EINVAL;
+}
+
+/*
+ * Applications call the VIDIOC_DQBUF ioctl to dequeue a filled (capturing) or
+ * displayed (output buffer)from the driver's outgoing queue
+ */
+static int atomisp_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int ret;
+
+	ret = atomisp_pipe_check(pipe, false);
+	if (ret)
+		return ret;
+
+	mutex_unlock(&isp->mutex);
+	ret = videobuf_dqbuf(&pipe->capq, buf, file->f_flags & O_NONBLOCK);
+	mutex_lock(&isp->mutex);
+	if (ret) {
+		if (ret != -EAGAIN)
+			dev_dbg(isp->dev, "<%s: %d\n", __func__, ret);
+		return ret;
+	}
+
+	buf->bytesused = pipe->pix.sizeimage;
+	buf->reserved = asd->frame_status[buf->index];
+
+	/*
+	 * Hack:
+	 * Currently frame_status in the enum type which takes no more lower
+	 * 8 bit.
+	 * use bit[31:16] for exp_id as it is only in the range of 1~255
+	 */
+	buf->reserved &= 0x0000ffff;
+	if (!(buf->flags & V4L2_BUF_FLAG_ERROR))
+		buf->reserved |= __get_frame_exp_id(pipe, buf) << 16;
+	buf->reserved2 = pipe->frame_config_id[buf->index];
+
+	dev_dbg(isp->dev,
+		"dqbuf buffer %d (%s) for asd%d with exp_id %d, isp_config_id %d\n",
+		buf->index, vdev->name, asd->index, buf->reserved >> 16,
+		buf->reserved2);
+	return 0;
+}
+
+enum ia_css_pipe_id atomisp_get_css_pipe_id(struct atomisp_sub_device *asd)
+{
+	if (ATOMISP_USE_YUVPP(asd))
+		return IA_CSS_PIPE_ID_YUVPP;
+
+	if (asd->continuous_mode->val) {
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			return IA_CSS_PIPE_ID_VIDEO;
+		else
+			return IA_CSS_PIPE_ID_PREVIEW;
+	}
+
+	/*
+	 * Disable vf_pp and run CSS in video mode. This allows using ISP
+	 * scaling but it has one frame delay due to CSS internal buffering.
+	 */
+	if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER)
+		return IA_CSS_PIPE_ID_VIDEO;
+
+	/*
+	 * Disable vf_pp and run CSS in still capture mode. In this mode
+	 * CSS does not cause extra latency with buffering, but scaling
+	 * is not available.
+	 */
+	if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT)
+		return IA_CSS_PIPE_ID_CAPTURE;
+
+	switch (asd->run_mode->val) {
+	case ATOMISP_RUN_MODE_PREVIEW:
+		return IA_CSS_PIPE_ID_PREVIEW;
+	case ATOMISP_RUN_MODE_VIDEO:
+		return IA_CSS_PIPE_ID_VIDEO;
+	case ATOMISP_RUN_MODE_STILL_CAPTURE:
+	default:
+		return IA_CSS_PIPE_ID_CAPTURE;
+	}
+}
+
+static unsigned int atomisp_sensor_start_stream(struct atomisp_sub_device *asd)
+{
+	if (asd->vfpp->val != ATOMISP_VFPP_ENABLE ||
+	    asd->copy_mode)
+		return 1;
+
+	if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO ||
+	    (asd->run_mode->val == ATOMISP_RUN_MODE_STILL_CAPTURE &&
+	     !atomisp_is_mbuscode_raw(
+		 asd->fmt[
+		  asd->capture_pad].fmt.code) &&
+	     !asd->continuous_mode->val))
+		return 2;
+	else
+		return 1;
+}
+
+int atomisp_stream_on_master_slave_sensor(struct atomisp_device *isp,
+	bool isp_timeout)
+{
+	unsigned int master, slave, delay_slave = 0;
+	int ret;
+
+	master = ATOMISP_DEPTH_DEFAULT_MASTER_SENSOR;
+	slave = ATOMISP_DEPTH_DEFAULT_SLAVE_SENSOR;
+	dev_warn(isp->dev,
+		 "depth mode use default master=%s.slave=%s.\n",
+		 isp->inputs[master].camera->name,
+		 isp->inputs[slave].camera->name);
+
+	ret = v4l2_subdev_call(isp->inputs[master].camera, core,
+			       ioctl, ATOMISP_IOC_G_DEPTH_SYNC_COMP,
+			       &delay_slave);
+	if (ret)
+		dev_warn(isp->dev,
+			 "get depth sensor %s compensation delay failed.\n",
+			 isp->inputs[master].camera->name);
+
+	ret = v4l2_subdev_call(isp->inputs[master].camera,
+			       video, s_stream, 1);
+	if (ret) {
+		dev_err(isp->dev, "depth mode master sensor %s stream-on failed.\n",
+			isp->inputs[master].camera->name);
+		return -EINVAL;
+	}
+
+	if (delay_slave != 0)
+		udelay(delay_slave);
+
+	ret = v4l2_subdev_call(isp->inputs[slave].camera,
+			       video, s_stream, 1);
+	if (ret) {
+		dev_err(isp->dev, "depth mode slave sensor %s stream-on failed.\n",
+			isp->inputs[slave].camera->name);
+		v4l2_subdev_call(isp->inputs[master].camera, video, s_stream, 0);
+
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Input system HW workaround */
+/* Input system address translation corrupts burst during */
+/* invalidate. SW workaround for this is to set burst length */
+/* manually to 128 in case of 13MPx snapshot and to 1 otherwise. */
+static void atomisp_dma_burst_len_cfg(struct atomisp_sub_device *asd)
+{
+	struct v4l2_mbus_framefmt *sink;
+
+	sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				       V4L2_SUBDEV_FORMAT_ACTIVE,
+				       ATOMISP_SUBDEV_PAD_SINK);
+
+	if (sink->width * sink->height >= 4096 * 3072)
+		atomisp_css2_hw_store_32(DMA_BURST_SIZE_REG, 0x7F);
+	else
+		atomisp_css2_hw_store_32(DMA_BURST_SIZE_REG, 0x00);
+}
+
+/*
+ * This ioctl start the capture during streaming I/O.
+ */
+static int atomisp_streamon(struct file *file, void *fh,
+			    enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	enum ia_css_pipe_id css_pipe_id;
+	unsigned int sensor_start_stream;
+	unsigned long irqflags;
+	int ret;
+
+	dev_dbg(isp->dev, "Start stream on pad %d for asd%d\n",
+		atomisp_subdev_source_pad(vdev), asd->index);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_dbg(isp->dev, "unsupported v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	ret = atomisp_pipe_check(pipe, false);
+	if (ret)
+		return ret;
+
+	if (pipe->capq.streaming)
+		return 0;
+
+	/* Input system HW workaround */
+	atomisp_dma_burst_len_cfg(asd);
+
+	/*
+	 * The number of streaming video nodes is based on which
+	 * binary is going to be run.
+	 */
+	sensor_start_stream = atomisp_sensor_start_stream(asd);
+
+	spin_lock_irqsave(&pipe->irq_lock, irqflags);
+	if (list_empty(&pipe->capq.stream)) {
+		spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+		dev_dbg(isp->dev, "no buffer in the queue\n");
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+
+	ret = videobuf_streamon(&pipe->capq);
+	if (ret)
+		return ret;
+
+	/* Reset pending capture request count. */
+	asd->pending_capture_request = 0;
+
+	if (atomisp_subdev_streaming_count(asd) > sensor_start_stream) {
+		/* trigger still capture */
+		if (asd->continuous_mode->val &&
+		    atomisp_subdev_source_pad(vdev)
+		    == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE) {
+			if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+				dev_dbg(isp->dev, "SDV last video raw buffer id: %u\n",
+					asd->latest_preview_exp_id);
+			else
+				dev_dbg(isp->dev, "ZSL last preview raw buffer id: %u\n",
+					asd->latest_preview_exp_id);
+
+			if (asd->delayed_init == ATOMISP_DELAYED_INIT_QUEUED) {
+				flush_work(&asd->delayed_init_work);
+				mutex_unlock(&isp->mutex);
+				ret = wait_for_completion_interruptible(&asd->init_done);
+				mutex_lock(&isp->mutex);
+				if (ret != 0)
+					return -ERESTARTSYS;
+			}
+
+			/* handle per_frame_setting parameter and buffers */
+			atomisp_handle_parameter_and_buffer(pipe);
+
+			/*
+			 * only ZSL/SDV capture request will be here, raise
+			 * the ISP freq to the highest possible to minimize
+			 * the S2S latency.
+			 */
+			atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_MAX, false);
+			/*
+			 * When asd->enable_raw_buffer_lock->val is true,
+			 * An extra IOCTL is needed to call
+			 * atomisp_css_exp_id_capture and trigger real capture
+			 */
+			if (!asd->enable_raw_buffer_lock->val) {
+				ret = atomisp_css_offline_capture_configure(asd,
+					asd->params.offline_parm.num_captures,
+					asd->params.offline_parm.skip_frames,
+					asd->params.offline_parm.offset);
+				if (ret)
+					return -EINVAL;
+			}
+		}
+		atomisp_qbuffers_to_css(asd);
+		return 0;
+	}
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
+		atomisp_qbuffers_to_css(asd);
+		goto start_sensor;
+	}
+
+	css_pipe_id = atomisp_get_css_pipe_id(asd);
+
+	/* Invalidate caches. FIXME: should flush only necessary buffers */
+	wbinvd();
+
+	if (asd->params.css_update_params_needed) {
+		atomisp_apply_css_parameters(asd, &asd->params.css_param);
+		if (asd->params.css_param.update_flag.dz_config)
+			asd->params.config.dz_config = &asd->params.css_param.dz_config;
+		atomisp_css_update_isp_params(asd);
+		asd->params.css_update_params_needed = false;
+		memset(&asd->params.css_param.update_flag, 0,
+		       sizeof(struct atomisp_parameters));
+	}
+	asd->params.dvs_6axis = NULL;
+
+	ret = atomisp_css_start(asd, css_pipe_id, false);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&isp->lock, irqflags);
+	asd->streaming = ATOMISP_DEVICE_STREAMING_ENABLED;
+	spin_unlock_irqrestore(&isp->lock, irqflags);
+	atomic_set(&asd->sof_count, -1);
+	atomic_set(&asd->sequence, -1);
+	atomic_set(&asd->sequence_temp, -1);
+
+	asd->params.dis_proj_data_valid = false;
+	asd->latest_preview_exp_id = 0;
+	asd->postview_exp_id = 1;
+	asd->preview_exp_id = 1;
+
+	/* handle per_frame_setting parameter and buffers */
+	atomisp_handle_parameter_and_buffer(pipe);
+
+	atomisp_qbuffers_to_css(asd);
+
+	/* Only start sensor when the last streaming instance started */
+	if (atomisp_subdev_streaming_count(asd) < sensor_start_stream)
+		return 0;
+
+start_sensor:
+	if (isp->flash) {
+		asd->params.num_flash_frames = 0;
+		asd->params.flash_state = ATOMISP_FLASH_IDLE;
+		atomisp_setup_flash(asd);
+	}
+
+	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF,
+			       atomisp_css_valid_sof(isp));
+	atomisp_csi2_configure(asd);
+	/*
+	 * set freq to max when streaming count > 1 which indicate
+	 * dual camera would run
+	 */
+	if (atomisp_streaming_count(isp) > 1) {
+		if (atomisp_freq_scaling(isp,
+					 ATOMISP_DFS_MODE_MAX, false) < 0)
+			dev_dbg(isp->dev, "DFS max mode failed!\n");
+	} else {
+		if (atomisp_freq_scaling(isp,
+					 ATOMISP_DFS_MODE_AUTO, false) < 0)
+			dev_dbg(isp->dev, "DFS auto mode failed!\n");
+	}
+
+	if (asd->depth_mode->val && atomisp_streaming_count(isp) ==
+	    ATOMISP_DEPTH_SENSOR_STREAMON_COUNT) {
+		ret = atomisp_stream_on_master_slave_sensor(isp, false);
+		if (ret) {
+			dev_err(isp->dev, "master slave sensor stream on failed!\n");
+			return ret;
+		}
+		goto start_delay_wq;
+	} else if (asd->depth_mode->val && (atomisp_streaming_count(isp) <
+					    ATOMISP_DEPTH_SENSOR_STREAMON_COUNT)) {
+		goto start_delay_wq;
+	}
+
+	/* Enable the CSI interface on ANN B0/K0 */
+	if (isp->media_dev.hw_revision >= ((ATOMISP_HW_REVISION_ISP2401 <<
+					    ATOMISP_HW_REVISION_SHIFT) | ATOMISP_HW_STEPPING_B0)) {
+		pci_write_config_word(pdev, MRFLD_PCI_CSI_CONTROL,
+				      isp->saved_regs.csi_control | MRFLD_PCI_CSI_CONTROL_CSI_READY);
+	}
+
+	/* stream on the sensor */
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       video, s_stream, 1);
+	if (ret) {
+		spin_lock_irqsave(&isp->lock, irqflags);
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+		spin_unlock_irqrestore(&isp->lock, irqflags);
+		return -EINVAL;
+	}
+
+start_delay_wq:
+	if (asd->continuous_mode->val) {
+		atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				        V4L2_SUBDEV_FORMAT_ACTIVE,
+				        ATOMISP_SUBDEV_PAD_SINK);
+
+		reinit_completion(&asd->init_done);
+		asd->delayed_init = ATOMISP_DELAYED_INIT_QUEUED;
+		queue_work(asd->delayed_init_workq, &asd->delayed_init_work);
+	} else {
+		asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
+	}
+
+	return 0;
+}
+
+int atomisp_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct atomisp_video_pipe *capture_pipe = NULL;
+	struct atomisp_video_pipe *vf_pipe = NULL;
+	struct atomisp_video_pipe *preview_pipe = NULL;
+	struct atomisp_video_pipe *video_pipe = NULL;
+	struct videobuf_buffer *vb, *_vb;
+	enum ia_css_pipe_id css_pipe_id;
+	int ret;
+	unsigned long flags;
+	bool first_streamoff = false;
+
+	dev_dbg(isp->dev, "Stop stream on pad %d for asd%d\n",
+		atomisp_subdev_source_pad(vdev), asd->index);
+
+	lockdep_assert_held(&isp->mutex);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_dbg(isp->dev, "unsupported v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * do only videobuf_streamoff for capture & vf pipes in
+	 * case of continuous capture
+	 */
+	if (asd->continuous_mode->val &&
+	    atomisp_subdev_source_pad(vdev) != ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW &&
+	    atomisp_subdev_source_pad(vdev) != ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
+		if (atomisp_subdev_source_pad(vdev) == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE) {
+			/* stop continuous still capture if needed */
+			if (asd->params.offline_parm.num_captures == -1)
+				atomisp_css_offline_capture_configure(asd,
+								      0, 0, 0);
+			atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_AUTO, false);
+		}
+		/*
+		 * Currently there is no way to flush buffers queued to css.
+		 * When doing videobuf_streamoff, active buffers will be
+		 * marked as VIDEOBUF_NEEDS_INIT. HAL will be able to use
+		 * these buffers again, and these buffers might be queued to
+		 * css more than once! Warn here, if HAL has not dequeued all
+		 * buffers back before calling streamoff.
+		 */
+		if (pipe->buffers_in_css != 0) {
+			WARN(1, "%s: buffers of vdev %s still in CSS!\n",
+			     __func__, pipe->vdev.name);
+
+			/*
+			 * Buffers remained in css maybe dequeued out in the
+			 * next stream on, while this will causes serious
+			 * issues as buffers already get invalid after
+			 * previous stream off.
+			 *
+			 * No way to flush buffers but to reset the whole css
+			 */
+			dev_warn(isp->dev, "Reset CSS to clean up css buffers.\n");
+			atomisp_css_flush(isp);
+		}
+
+		return videobuf_streamoff(&pipe->capq);
+	}
+
+	if (!pipe->capq.streaming)
+		return 0;
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED)
+		first_streamoff = true;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (atomisp_subdev_streaming_count(asd) == 1)
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+	else
+		asd->streaming = ATOMISP_DEVICE_STREAMING_STOPPING;
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	if (!first_streamoff) {
+		ret = videobuf_streamoff(&pipe->capq);
+		if (ret)
+			return ret;
+		goto stopsensor;
+	}
+
+	atomisp_clear_css_buffer_counters(asd);
+	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF, false);
+
+	if (asd->delayed_init == ATOMISP_DELAYED_INIT_QUEUED) {
+		cancel_work_sync(&asd->delayed_init_work);
+		asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
+	}
+
+	css_pipe_id = atomisp_get_css_pipe_id(asd);
+	atomisp_css_stop(asd, css_pipe_id, false);
+
+	/* cancel work queue*/
+	if (asd->video_out_capture.users) {
+		capture_pipe = &asd->video_out_capture;
+		wake_up_interruptible(&capture_pipe->capq.wait);
+	}
+	if (asd->video_out_vf.users) {
+		vf_pipe = &asd->video_out_vf;
+		wake_up_interruptible(&vf_pipe->capq.wait);
+	}
+	if (asd->video_out_preview.users) {
+		preview_pipe = &asd->video_out_preview;
+		wake_up_interruptible(&preview_pipe->capq.wait);
+	}
+	if (asd->video_out_video_capture.users) {
+		video_pipe = &asd->video_out_video_capture;
+		wake_up_interruptible(&video_pipe->capq.wait);
+	}
+	ret = videobuf_streamoff(&pipe->capq);
+	if (ret)
+		return ret;
+
+	/* cleanup css here */
+	/* no need for this, as ISP will be reset anyway */
+	/*atomisp_flush_bufs_in_css(isp);*/
+
+	spin_lock_irqsave(&pipe->irq_lock, flags);
+	list_for_each_entry_safe(vb, _vb, &pipe->activeq, queue) {
+		vb->state = VIDEOBUF_PREPARED;
+		list_del(&vb->queue);
+	}
+	list_for_each_entry_safe(vb, _vb, &pipe->buffers_waiting_for_param, queue) {
+		vb->state = VIDEOBUF_PREPARED;
+		list_del(&vb->queue);
+		pipe->frame_request_config_id[vb->i] = 0;
+	}
+	spin_unlock_irqrestore(&pipe->irq_lock, flags);
+
+	atomisp_subdev_cleanup_pending_events(asd);
+stopsensor:
+	if (atomisp_subdev_streaming_count(asd) + 1
+	    != atomisp_sensor_start_stream(asd))
+		return 0;
+
+	ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+			       video, s_stream, 0);
+
+	if (isp->flash) {
+		asd->params.num_flash_frames = 0;
+		asd->params.flash_state = ATOMISP_FLASH_IDLE;
+	}
+
+	/* if other streams are running, isp should not be powered off */
+	if (atomisp_streaming_count(isp)) {
+		atomisp_css_flush(isp);
+		return 0;
+	}
+
+	/* Disable the CSI interface on ANN B0/K0 */
+	if (isp->media_dev.hw_revision >= ((ATOMISP_HW_REVISION_ISP2401 <<
+					    ATOMISP_HW_REVISION_SHIFT) | ATOMISP_HW_STEPPING_B0)) {
+		pci_write_config_word(pdev, MRFLD_PCI_CSI_CONTROL,
+				      isp->saved_regs.csi_control & ~MRFLD_PCI_CSI_CONTROL_CSI_READY);
+	}
+
+	if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_LOW, false))
+		dev_warn(isp->dev, "DFS failed.\n");
+	/*
+	 * ISP work around, need to reset isp
+	 * Is it correct time to reset ISP when first node does streamoff?
+	 */
+	if (isp->sw_contex.power_state == ATOM_ISP_POWER_UP) {
+		unsigned int i;
+		bool recreate_streams[MAX_STREAM_NUM] = {0};
+
+		if (isp->isp_timeout)
+			dev_err(isp->dev, "%s: Resetting with WA activated",
+				__func__);
+		/*
+		 * It is possible that the other asd stream is in the stage
+		 * that v4l2_setfmt is just get called on it, which will
+		 * create css stream on that stream. But at this point, there
+		 * is no way to destroy the css stream created on that stream.
+		 *
+		 * So force stream destroy here.
+		 */
+		for (i = 0; i < isp->num_of_streams; i++) {
+			if (isp->asd[i].stream_prepared) {
+				atomisp_destroy_pipes_stream_force(&isp->
+								   asd[i]);
+				recreate_streams[i] = true;
+			}
+		}
+
+		/* disable  PUNIT/ISP acknowlede/handshake - SRSE=3 */
+		pci_write_config_dword(pdev, PCI_I_CONTROL,
+				       isp->saved_regs.i_control | MRFLD_PCI_I_CONTROL_SRSE_RESET_MASK);
+		dev_err(isp->dev, "atomisp_reset");
+		atomisp_reset(isp);
+		for (i = 0; i < isp->num_of_streams; i++) {
+			if (recreate_streams[i]) {
+				int ret2;
+
+				ret2 = atomisp_create_pipes_stream(&isp->asd[i]);
+				if (ret2) {
+					dev_err(isp->dev, "%s error re-creating streams: %d\n",
+						__func__, ret2);
+					if (!ret)
+						ret = ret2;
+				}
+			}
+		}
+		isp->isp_timeout = false;
+	}
+	return ret;
+}
+
+/*
+ * To get the current value of a control.
+ * applications initialize the id field of a struct v4l2_control and
+ * call this ioctl with a pointer to this structure
+ */
+static int atomisp_g_ctrl(struct file *file, void *fh,
+			  struct v4l2_control *control)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int i, ret = -EINVAL;
+
+	for (i = 0; i < ctrls_num; i++) {
+		if (ci_v4l2_controls[i].id == control->id) {
+			ret = 0;
+			break;
+		}
+	}
+
+	if (ret)
+		return ret;
+
+	switch (control->id) {
+	case V4L2_CID_IRIS_ABSOLUTE:
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+	case V4L2_CID_2A_STATUS:
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+	case V4L2_CID_EXPOSURE:
+	case V4L2_CID_EXPOSURE_AUTO:
+	case V4L2_CID_SCENE_MODE:
+	case V4L2_CID_ISO_SENSITIVITY:
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+	case V4L2_CID_SHARPNESS:
+	case V4L2_CID_3A_LOCK:
+	case V4L2_CID_EXPOSURE_ZONE_NUM:
+	case V4L2_CID_TEST_PATTERN:
+	case V4L2_CID_TEST_PATTERN_COLOR_R:
+	case V4L2_CID_TEST_PATTERN_COLOR_GR:
+	case V4L2_CID_TEST_PATTERN_COLOR_GB:
+	case V4L2_CID_TEST_PATTERN_COLOR_B:
+		return v4l2_g_ctrl(isp->inputs[asd->input_curr].camera->
+				   ctrl_handler, control);
+	case V4L2_CID_COLORFX:
+		ret = atomisp_color_effect(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_BAD_PIXEL_DETECTION:
+		ret = atomisp_bad_pixel(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_POSTPROCESS_GDC_CAC:
+		ret = atomisp_gdc_cac(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_VIDEO_STABLIZATION:
+		ret = atomisp_video_stable(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_FIXED_PATTERN_NR:
+		ret = atomisp_fixed_pattern(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_FALSE_COLOR_CORRECTION:
+		ret = atomisp_false_color(asd, 0, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_LOW_LIGHT:
+		ret = atomisp_low_light(asd, 0, &control->value);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * To change the value of a control.
+ * applications initialize the id and value fields of a struct v4l2_control
+ * and call this ioctl.
+ */
+static int atomisp_s_ctrl(struct file *file, void *fh,
+			  struct v4l2_control *control)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int i, ret = -EINVAL;
+
+	for (i = 0; i < ctrls_num; i++) {
+		if (ci_v4l2_controls[i].id == control->id) {
+			ret = 0;
+			break;
+		}
+	}
+
+	if (ret)
+		return ret;
+
+	switch (control->id) {
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+	case V4L2_CID_EXPOSURE:
+	case V4L2_CID_EXPOSURE_AUTO:
+	case V4L2_CID_EXPOSURE_AUTO_PRIORITY:
+	case V4L2_CID_SCENE_MODE:
+	case V4L2_CID_ISO_SENSITIVITY:
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+	case V4L2_CID_EXPOSURE_METERING:
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+	case V4L2_CID_SHARPNESS:
+	case V4L2_CID_3A_LOCK:
+	case V4L2_CID_COLORFX_CBCR:
+	case V4L2_CID_TEST_PATTERN:
+	case V4L2_CID_TEST_PATTERN_COLOR_R:
+	case V4L2_CID_TEST_PATTERN_COLOR_GR:
+	case V4L2_CID_TEST_PATTERN_COLOR_GB:
+	case V4L2_CID_TEST_PATTERN_COLOR_B:
+		return v4l2_s_ctrl(NULL,
+				   isp->inputs[asd->input_curr].camera->
+				   ctrl_handler, control);
+	case V4L2_CID_COLORFX:
+		ret = atomisp_color_effect(asd, 1, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_BAD_PIXEL_DETECTION:
+		ret = atomisp_bad_pixel(asd, 1, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_POSTPROCESS_GDC_CAC:
+		ret = atomisp_gdc_cac(asd, 1, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_VIDEO_STABLIZATION:
+		ret = atomisp_video_stable(asd, 1, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_FIXED_PATTERN_NR:
+		ret = atomisp_fixed_pattern(asd, 1, &control->value);
+		break;
+	case V4L2_CID_ATOMISP_FALSE_COLOR_CORRECTION:
+		ret = atomisp_false_color(asd, 1, &control->value);
+		break;
+	case V4L2_CID_REQUEST_FLASH:
+		ret = atomisp_flash_enable(asd, control->value);
+		break;
+	case V4L2_CID_ATOMISP_LOW_LIGHT:
+		ret = atomisp_low_light(asd, 1, &control->value);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+/*
+ * To query the attributes of a control.
+ * applications set the id field of a struct v4l2_queryctrl and call the
+ * this ioctl with a pointer to this structure. The driver fills
+ * the rest of the structure.
+ */
+static int atomisp_queryctl(struct file *file, void *fh,
+			    struct v4l2_queryctrl *qc)
+{
+	int i, ret = -EINVAL;
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+
+	switch (qc->id) {
+	case V4L2_CID_FOCUS_ABSOLUTE:
+	case V4L2_CID_FOCUS_RELATIVE:
+	case V4L2_CID_FOCUS_STATUS:
+		if (!IS_ISP2401) {
+			return v4l2_queryctrl(isp->inputs[asd->input_curr].camera->
+					    ctrl_handler, qc);
+		}
+		/* ISP2401 */
+		if (isp->motor)
+			return v4l2_queryctrl(isp->motor->ctrl_handler, qc);
+		else
+			return v4l2_queryctrl(isp->inputs[asd->input_curr].
+					      camera->ctrl_handler, qc);
+	}
+
+	if (qc->id & V4L2_CTRL_FLAG_NEXT_CTRL)
+		return ret;
+
+	for (i = 0; i < ctrls_num; i++) {
+		if (ci_v4l2_controls[i].id == qc->id) {
+			memcpy(qc, &ci_v4l2_controls[i],
+			       sizeof(struct v4l2_queryctrl));
+			qc->reserved[0] = 0;
+			ret = 0;
+			break;
+		}
+	}
+	if (ret != 0)
+		qc->flags = V4L2_CTRL_FLAG_DISABLED;
+
+	return ret;
+}
+
+static int atomisp_camera_g_ext_ctrls(struct file *file, void *fh,
+				      struct v4l2_ext_controls *c)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct v4l2_subdev *motor;
+	struct v4l2_control ctrl;
+	int i;
+	int ret = 0;
+
+	if (!IS_ISP2401)
+		motor = isp->inputs[asd->input_curr].motor;
+	else
+		motor = isp->motor;
+
+	for (i = 0; i < c->count; i++) {
+		ctrl.id = c->controls[i].id;
+		ctrl.value = c->controls[i].value;
+		switch (ctrl.id) {
+		case V4L2_CID_EXPOSURE_ABSOLUTE:
+		case V4L2_CID_EXPOSURE_AUTO:
+		case V4L2_CID_IRIS_ABSOLUTE:
+		case V4L2_CID_FNUMBER_ABSOLUTE:
+		case V4L2_CID_BIN_FACTOR_HORZ:
+		case V4L2_CID_BIN_FACTOR_VERT:
+		case V4L2_CID_3A_LOCK:
+		case V4L2_CID_TEST_PATTERN:
+		case V4L2_CID_TEST_PATTERN_COLOR_R:
+		case V4L2_CID_TEST_PATTERN_COLOR_GR:
+		case V4L2_CID_TEST_PATTERN_COLOR_GB:
+		case V4L2_CID_TEST_PATTERN_COLOR_B:
+			/*
+			 * Exposure related control will be handled by sensor
+			 * driver
+			 */
+			ret =
+			    v4l2_g_ctrl(isp->inputs[asd->input_curr].camera->
+					ctrl_handler, &ctrl);
+			break;
+		case V4L2_CID_FOCUS_ABSOLUTE:
+		case V4L2_CID_FOCUS_RELATIVE:
+		case V4L2_CID_FOCUS_STATUS:
+		case V4L2_CID_FOCUS_AUTO:
+			if (motor)
+				ret = v4l2_g_ctrl(motor->ctrl_handler, &ctrl);
+			break;
+		case V4L2_CID_FLASH_STATUS:
+		case V4L2_CID_FLASH_INTENSITY:
+		case V4L2_CID_FLASH_TORCH_INTENSITY:
+		case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		case V4L2_CID_FLASH_TIMEOUT:
+		case V4L2_CID_FLASH_STROBE:
+		case V4L2_CID_FLASH_MODE:
+		case V4L2_CID_FLASH_STATUS_REGISTER:
+			if (isp->flash)
+				ret =
+				    v4l2_g_ctrl(isp->flash->ctrl_handler,
+						&ctrl);
+			break;
+		case V4L2_CID_ZOOM_ABSOLUTE:
+			ret = atomisp_digital_zoom(asd, 0, &ctrl.value);
+			break;
+		case V4L2_CID_G_SKIP_FRAMES:
+			ret = v4l2_subdev_call(
+				  isp->inputs[asd->input_curr].camera,
+				  sensor, g_skip_frames, (u32 *)&ctrl.value);
+			break;
+		default:
+			ret = -EINVAL;
+		}
+
+		if (ret) {
+			c->error_idx = i;
+			break;
+		}
+		c->controls[i].value = ctrl.value;
+	}
+	return ret;
+}
+
+/* This ioctl allows the application to get multiple controls by class */
+static int atomisp_g_ext_ctrls(struct file *file, void *fh,
+			       struct v4l2_ext_controls *c)
+{
+	struct v4l2_control ctrl;
+	int i, ret = 0;
+
+	/*
+	 * input_lock is not need for the Camera related IOCTLs
+	 * The input_lock downgrade the FPS of 3A
+	 */
+	ret = atomisp_camera_g_ext_ctrls(file, fh, c);
+	if (ret != -EINVAL)
+		return ret;
+
+	for (i = 0; i < c->count; i++) {
+		ctrl.id = c->controls[i].id;
+		ctrl.value = c->controls[i].value;
+		ret = atomisp_g_ctrl(file, fh, &ctrl);
+		c->controls[i].value = ctrl.value;
+		if (ret) {
+			c->error_idx = i;
+			break;
+		}
+	}
+	return ret;
+}
+
+static int atomisp_camera_s_ext_ctrls(struct file *file, void *fh,
+				      struct v4l2_ext_controls *c)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct v4l2_subdev *motor;
+	struct v4l2_control ctrl;
+	int i;
+	int ret = 0;
+
+	if (!IS_ISP2401)
+		motor = isp->inputs[asd->input_curr].motor;
+	else
+		motor = isp->motor;
+
+	for (i = 0; i < c->count; i++) {
+		struct v4l2_ctrl *ctr;
+
+		ctrl.id = c->controls[i].id;
+		ctrl.value = c->controls[i].value;
+		switch (ctrl.id) {
+		case V4L2_CID_EXPOSURE_ABSOLUTE:
+		case V4L2_CID_EXPOSURE_AUTO:
+		case V4L2_CID_EXPOSURE_METERING:
+		case V4L2_CID_IRIS_ABSOLUTE:
+		case V4L2_CID_FNUMBER_ABSOLUTE:
+		case V4L2_CID_VCM_TIMING:
+		case V4L2_CID_VCM_SLEW:
+		case V4L2_CID_3A_LOCK:
+		case V4L2_CID_TEST_PATTERN:
+		case V4L2_CID_TEST_PATTERN_COLOR_R:
+		case V4L2_CID_TEST_PATTERN_COLOR_GR:
+		case V4L2_CID_TEST_PATTERN_COLOR_GB:
+		case V4L2_CID_TEST_PATTERN_COLOR_B:
+			ret = v4l2_s_ctrl(NULL,
+					  isp->inputs[asd->input_curr].camera->
+					  ctrl_handler, &ctrl);
+			break;
+		case V4L2_CID_FOCUS_ABSOLUTE:
+		case V4L2_CID_FOCUS_RELATIVE:
+		case V4L2_CID_FOCUS_STATUS:
+		case V4L2_CID_FOCUS_AUTO:
+			if (motor)
+				ret = v4l2_s_ctrl(NULL, motor->ctrl_handler,
+						  &ctrl);
+			else
+				ret = v4l2_s_ctrl(NULL,
+						  isp->inputs[asd->input_curr].
+						  camera->ctrl_handler, &ctrl);
+			break;
+		case V4L2_CID_FLASH_STATUS:
+		case V4L2_CID_FLASH_INTENSITY:
+		case V4L2_CID_FLASH_TORCH_INTENSITY:
+		case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		case V4L2_CID_FLASH_TIMEOUT:
+		case V4L2_CID_FLASH_STROBE:
+		case V4L2_CID_FLASH_MODE:
+		case V4L2_CID_FLASH_STATUS_REGISTER:
+			if (isp->flash) {
+				ret =
+				    v4l2_s_ctrl(NULL, isp->flash->ctrl_handler,
+						&ctrl);
+				/*
+				 * When flash mode is changed we need to reset
+				 * flash state
+				 */
+				if (ctrl.id == V4L2_CID_FLASH_MODE) {
+					asd->params.flash_state =
+					    ATOMISP_FLASH_IDLE;
+					asd->params.num_flash_frames = 0;
+				}
+			}
+			break;
+		case V4L2_CID_ZOOM_ABSOLUTE:
+			ret = atomisp_digital_zoom(asd, 1, &ctrl.value);
+			break;
+		default:
+			ctr = v4l2_ctrl_find(&asd->ctrl_handler, ctrl.id);
+			if (ctr)
+				ret = v4l2_ctrl_s_ctrl(ctr, ctrl.value);
+			else
+				ret = -EINVAL;
+		}
+
+		if (ret) {
+			c->error_idx = i;
+			break;
+		}
+		c->controls[i].value = ctrl.value;
+	}
+	return ret;
+}
+
+/* This ioctl allows the application to set multiple controls by class */
+static int atomisp_s_ext_ctrls(struct file *file, void *fh,
+			       struct v4l2_ext_controls *c)
+{
+	struct v4l2_control ctrl;
+	int i, ret = 0;
+
+	/*
+	 * input_lock is not need for the Camera related IOCTLs
+	 * The input_lock downgrade the FPS of 3A
+	 */
+	ret = atomisp_camera_s_ext_ctrls(file, fh, c);
+	if (ret != -EINVAL)
+		return ret;
+
+	for (i = 0; i < c->count; i++) {
+		ctrl.id = c->controls[i].id;
+		ctrl.value = c->controls[i].value;
+		ret = atomisp_s_ctrl(file, fh, &ctrl);
+		c->controls[i].value = ctrl.value;
+		if (ret) {
+			c->error_idx = i;
+			break;
+		}
+	}
+	return ret;
+}
+
+/*
+ * vidioc_g/s_param are used to switch isp running mode
+ */
+static int atomisp_g_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(isp->dev, "unsupported v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	parm->parm.capture.capturemode = asd->run_mode->val;
+
+	return 0;
+}
+
+static int atomisp_s_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	int mode;
+	int rval;
+	int fps;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(isp->dev, "unsupported v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	asd->high_speed_mode = false;
+	switch (parm->parm.capture.capturemode) {
+	case CI_MODE_NONE: {
+		struct v4l2_subdev_frame_interval fi = {0};
+
+		fi.interval = parm->parm.capture.timeperframe;
+
+		rval = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					video, s_frame_interval, &fi);
+		if (!rval)
+			parm->parm.capture.timeperframe = fi.interval;
+
+		if (fi.interval.numerator != 0) {
+			fps = fi.interval.denominator / fi.interval.numerator;
+			if (fps > 30)
+				asd->high_speed_mode = true;
+		}
+
+		return rval == -ENOIOCTLCMD ? 0 : rval;
+	}
+	case CI_MODE_VIDEO:
+		mode = ATOMISP_RUN_MODE_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		mode = ATOMISP_RUN_MODE_STILL_CAPTURE;
+		break;
+	case CI_MODE_CONTINUOUS:
+		mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE;
+		break;
+	case CI_MODE_PREVIEW:
+		mode = ATOMISP_RUN_MODE_PREVIEW;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	rval = v4l2_ctrl_s_ctrl(asd->run_mode, mode);
+
+	return rval == -ENOIOCTLCMD ? 0 : rval;
+}
+
+static long atomisp_vidioc_default(struct file *file, void *fh,
+				   bool valid_prio, unsigned int cmd, void *arg)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev *motor;
+	int err;
+
+	if (!IS_ISP2401)
+		motor = isp->inputs[asd->input_curr].motor;
+	else
+		motor = isp->motor;
+
+	switch (cmd) {
+	case ATOMISP_IOC_S_SENSOR_RUNMODE:
+		if (IS_ISP2401)
+			err = atomisp_set_sensor_runmode(asd, arg);
+		else
+			err = -EINVAL;
+		break;
+
+	case ATOMISP_IOC_G_XNR:
+		err = atomisp_xnr(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_XNR:
+		err = atomisp_xnr(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_NR:
+		err = atomisp_nr(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_NR:
+		err = atomisp_nr(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_TNR:
+		err = atomisp_tnr(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_TNR:
+		err = atomisp_tnr(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_BLACK_LEVEL_COMP:
+		err = atomisp_black_level(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_BLACK_LEVEL_COMP:
+		err = atomisp_black_level(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_EE:
+		err = atomisp_ee(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_EE:
+		err = atomisp_ee(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_DIS_STAT:
+		err = atomisp_get_dis_stat(asd, arg);
+		break;
+
+	case ATOMISP_IOC_G_DVS2_BQ_RESOLUTIONS:
+		err = atomisp_get_dvs2_bq_resolutions(asd, arg);
+		break;
+
+	case ATOMISP_IOC_S_DIS_COEFS:
+		err = atomisp_css_cp_dvs2_coefs(asd, arg,
+						&asd->params.css_param, true);
+		if (!err && arg)
+			asd->params.css_update_params_needed = true;
+		break;
+
+	case ATOMISP_IOC_S_DIS_VECTOR:
+		err = atomisp_cp_dvs_6axis_config(asd, arg,
+						  &asd->params.css_param, true);
+		if (!err && arg)
+			asd->params.css_update_params_needed = true;
+		break;
+
+	case ATOMISP_IOC_G_ISP_PARM:
+		err = atomisp_param(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_PARM:
+		err = atomisp_param(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_3A_STAT:
+		err = atomisp_3a_stat(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_GAMMA:
+		err = atomisp_gamma(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_GAMMA:
+		err = atomisp_gamma(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_GDC_TAB:
+		err = atomisp_gdc_cac_table(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_GDC_TAB:
+		err = atomisp_gdc_cac_table(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_MACC:
+		err = atomisp_macc_table(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_MACC:
+		err = atomisp_macc_table(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_BAD_PIXEL_DETECTION:
+		err = atomisp_bad_pixel_param(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_BAD_PIXEL_DETECTION:
+		err = atomisp_bad_pixel_param(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_FALSE_COLOR_CORRECTION:
+		err = atomisp_false_color_param(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_FALSE_COLOR_CORRECTION:
+		err = atomisp_false_color_param(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_CTC:
+		err = atomisp_ctc(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_CTC:
+		err = atomisp_ctc(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_WHITE_BALANCE:
+		err = atomisp_white_balance_param(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_WHITE_BALANCE:
+		err = atomisp_white_balance_param(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_G_3A_CONFIG:
+		err = atomisp_3a_config_param(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_3A_CONFIG:
+		err = atomisp_3a_config_param(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_FPN_TABLE:
+		err = atomisp_fixed_pattern_table(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ISP_MAKERNOTE:
+		err = atomisp_exif_makernote(asd, arg);
+		break;
+
+	case ATOMISP_IOC_G_SENSOR_MODE_DATA:
+		err = atomisp_get_sensor_mode_data(asd, arg);
+		break;
+
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA:
+		if (motor)
+			err = v4l2_subdev_call(motor, core, ioctl, cmd, arg);
+		else
+			err = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					       core, ioctl, cmd, arg);
+		break;
+
+	case ATOMISP_IOC_S_EXPOSURE:
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP:
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_G_SENSOR_AE_BRACKETING_INFO:
+	case ATOMISP_IOC_S_SENSOR_AE_BRACKETING_MODE:
+	case ATOMISP_IOC_G_SENSOR_AE_BRACKETING_MODE:
+	case ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT:
+		err = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				       core, ioctl, cmd, arg);
+		break;
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+		if (IS_ISP2401)
+			err = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					       core, ioctl, cmd, arg);
+		else
+			err = -EINVAL;
+		break;
+
+	case ATOMISP_IOC_S_ISP_SHD_TAB:
+		err = atomisp_set_shading_table(asd, arg);
+		break;
+
+	case ATOMISP_IOC_G_ISP_GAMMA_CORRECTION:
+		err = atomisp_gamma_correction(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_ISP_GAMMA_CORRECTION:
+		err = atomisp_gamma_correction(asd, 1, arg);
+		break;
+
+	case ATOMISP_IOC_S_PARAMETERS:
+		err = atomisp_set_parameters(vdev, arg);
+		break;
+
+	case ATOMISP_IOC_S_CONT_CAPTURE_CONFIG:
+		err = atomisp_offline_capture_configure(asd, arg);
+		break;
+	case ATOMISP_IOC_G_METADATA:
+		err = atomisp_get_metadata(asd, 0, arg);
+		break;
+	case ATOMISP_IOC_G_METADATA_BY_TYPE:
+		err = atomisp_get_metadata_by_type(asd, 0, arg);
+		break;
+	case ATOMISP_IOC_EXT_ISP_CTRL:
+		err = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				       core, ioctl, cmd, arg);
+		break;
+	case ATOMISP_IOC_EXP_ID_UNLOCK:
+		err = atomisp_exp_id_unlock(asd, arg);
+		break;
+	case ATOMISP_IOC_EXP_ID_CAPTURE:
+		err = atomisp_exp_id_capture(asd, arg);
+		break;
+	case ATOMISP_IOC_S_ENABLE_DZ_CAPT_PIPE:
+		err = atomisp_enable_dz_capt_pipe(asd, arg);
+		break;
+	case ATOMISP_IOC_G_FORMATS_CONFIG:
+		err = atomisp_formats(asd, 0, arg);
+		break;
+
+	case ATOMISP_IOC_S_FORMATS_CONFIG:
+		err = atomisp_formats(asd, 1, arg);
+		break;
+	case ATOMISP_IOC_S_EXPOSURE_WINDOW:
+		err = atomisp_s_ae_window(asd, arg);
+		break;
+	case ATOMISP_IOC_INJECT_A_FAKE_EVENT:
+		err = atomisp_inject_a_fake_event(asd, arg);
+		break;
+	case ATOMISP_IOC_G_INVALID_FRAME_NUM:
+		err = atomisp_get_invalid_frame_num(vdev, arg);
+		break;
+	case ATOMISP_IOC_S_ARRAY_RESOLUTION:
+		err = atomisp_set_array_res(asd, arg);
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+const struct v4l2_ioctl_ops atomisp_ioctl_ops = {
+	.vidioc_querycap = atomisp_querycap,
+	.vidioc_enum_input = atomisp_enum_input,
+	.vidioc_g_input = atomisp_g_input,
+	.vidioc_s_input = atomisp_s_input,
+	.vidioc_queryctrl = atomisp_queryctl,
+	.vidioc_s_ctrl = atomisp_s_ctrl,
+	.vidioc_g_ctrl = atomisp_g_ctrl,
+	.vidioc_s_ext_ctrls = atomisp_s_ext_ctrls,
+	.vidioc_g_ext_ctrls = atomisp_g_ext_ctrls,
+	.vidioc_enum_framesizes   = atomisp_enum_framesizes,
+	.vidioc_enum_frameintervals = atomisp_enum_frameintervals,
+	.vidioc_enum_fmt_vid_cap = atomisp_enum_fmt_cap,
+	.vidioc_try_fmt_vid_cap = atomisp_try_fmt_cap,
+	.vidioc_g_fmt_vid_cap = atomisp_g_fmt_cap,
+	.vidioc_s_fmt_vid_cap = atomisp_set_fmt,
+	.vidioc_reqbufs = atomisp_reqbufs,
+	.vidioc_querybuf = atomisp_querybuf,
+	.vidioc_qbuf = atomisp_qbuf,
+	.vidioc_dqbuf = atomisp_dqbuf,
+	.vidioc_streamon = atomisp_streamon,
+	.vidioc_streamoff = atomisp_streamoff,
+	.vidioc_default = atomisp_vidioc_default,
+	.vidioc_s_parm = atomisp_s_parm,
+	.vidioc_g_parm = atomisp_g_parm,
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp_ioctl.h b/drivers/staging/media/atomisp/pci/atomisp_ioctl.h
new file mode 100644
index 000000000..c660f631d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_ioctl.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef	__ATOMISP_IOCTL_H__
+#define	__ATOMISP_IOCTL_H__
+
+#include "ia_css.h"
+
+struct atomisp_device;
+struct atomisp_video_pipe;
+
+extern const struct atomisp_format_bridge atomisp_output_fmts[];
+
+const struct
+atomisp_format_bridge *atomisp_get_format_bridge(unsigned int pixelformat);
+
+const struct
+atomisp_format_bridge *atomisp_get_format_bridge_from_mbus(u32 mbus_code);
+
+int atomisp_pipe_check(struct atomisp_video_pipe *pipe, bool streaming_ok);
+
+int atomisp_alloc_css_stat_bufs(struct atomisp_sub_device *asd,
+				uint16_t stream_id);
+
+int atomisp_streamoff(struct file *file, void *fh, enum v4l2_buf_type type);
+int atomisp_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *req);
+
+enum ia_css_pipe_id atomisp_get_css_pipe_id(struct atomisp_sub_device
+	*asd);
+
+void atomisp_videobuf_free_buf(struct videobuf_buffer *vb);
+
+extern const struct v4l2_ioctl_ops atomisp_ioctl_ops;
+
+unsigned int atomisp_streaming_count(struct atomisp_device *isp);
+
+/* compat_ioctl for 32bit userland app and 64bit kernel */
+long atomisp_compat_ioctl32(struct file *file,
+			    unsigned int cmd, unsigned long arg);
+
+int atomisp_stream_on_master_slave_sensor(struct atomisp_device *isp,
+	bool isp_timeout);
+#endif /* __ATOMISP_IOCTL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_subdev.c b/drivers/staging/media/atomisp/pci/atomisp_subdev.c
new file mode 100644
index 000000000..847dfee6a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_subdev.c
@@ -0,0 +1,1369 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mediabus.h>
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_compat.h"
+#include "atomisp_internal.h"
+
+const struct atomisp_in_fmt_conv atomisp_in_fmt_conv[] = {
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, IA_CSS_BAYER_ORDER_BGGR },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, IA_CSS_BAYER_ORDER_GBRG },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, IA_CSS_BAYER_ORDER_GRBG },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, IA_CSS_BAYER_ORDER_RGGB },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, IA_CSS_BAYER_ORDER_BGGR },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, IA_CSS_BAYER_ORDER_GBRG },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, IA_CSS_BAYER_ORDER_GRBG },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, IA_CSS_BAYER_ORDER_RGGB },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, IA_CSS_BAYER_ORDER_BGGR },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, IA_CSS_BAYER_ORDER_GBRG },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, IA_CSS_BAYER_ORDER_GRBG },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, IA_CSS_BAYER_ORDER_RGGB },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, 8, ATOMISP_INPUT_FORMAT_YUV422_8, 0 },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, 8, ATOMISP_INPUT_FORMAT_YUV422_8, 0 },
+#if 0 // disabled due to clang warnings
+	{ MEDIA_BUS_FMT_JPEG_1X8, 8, 8, IA_CSS_FRAME_FORMAT_BINARY_8, 0 },
+	{ V4L2_MBUS_FMT_CUSTOM_NV12, 12, 12, IA_CSS_FRAME_FORMAT_NV12, 0 },
+	{ V4L2_MBUS_FMT_CUSTOM_NV21, 12, 12, IA_CSS_FRAME_FORMAT_NV21, 0 },
+#endif
+	{ V4L2_MBUS_FMT_CUSTOM_YUV420, 12, 12, ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY, 0 },
+#if 0
+	{ V4L2_MBUS_FMT_CUSTOM_M10MO_RAW, 8, 8, IA_CSS_FRAME_FORMAT_BINARY_8, 0 },
+#endif
+	/* no valid V4L2 MBUS code for metadata format, so leave it 0. */
+	{ 0, 0, 0, ATOMISP_INPUT_FORMAT_EMBEDDED, 0 },
+	{}
+};
+
+static const struct {
+	u32 code;
+	u32 compressed;
+} compressed_codes[] = {
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8 },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8 },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8 },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8 },
+};
+
+u32 atomisp_subdev_uncompressed_code(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(compressed_codes); i++)
+		if (code == compressed_codes[i].compressed)
+			return compressed_codes[i].code;
+
+	return code;
+}
+
+bool atomisp_subdev_is_compressed(u32 code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_in_fmt_conv) - 1; i++)
+		if (code == atomisp_in_fmt_conv[i].code)
+			return atomisp_in_fmt_conv[i].bpp !=
+			       atomisp_in_fmt_conv[i].depth;
+
+	return false;
+}
+
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv(u32 code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_in_fmt_conv) - 1; i++)
+		if (code == atomisp_in_fmt_conv[i].code)
+			return atomisp_in_fmt_conv + i;
+
+	return NULL;
+}
+
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+    enum atomisp_input_format atomisp_in_fmt)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_in_fmt_conv) - 1; i++)
+		if (atomisp_in_fmt_conv[i].atomisp_in_fmt == atomisp_in_fmt)
+			return atomisp_in_fmt_conv + i;
+
+	return NULL;
+}
+
+bool atomisp_subdev_format_conversion(struct atomisp_sub_device *asd,
+				      unsigned int source_pad)
+{
+	struct v4l2_mbus_framefmt *sink, *src;
+
+	sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				       V4L2_SUBDEV_FORMAT_ACTIVE,
+				       ATOMISP_SUBDEV_PAD_SINK);
+	src = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+				      V4L2_SUBDEV_FORMAT_ACTIVE, source_pad);
+
+	return atomisp_is_mbuscode_raw(sink->code)
+	       && !atomisp_is_mbuscode_raw(src->code);
+}
+
+uint16_t atomisp_subdev_source_pad(struct video_device *vdev)
+{
+	struct media_link *link;
+	u16 ret = 0;
+
+	list_for_each_entry(link, &vdev->entity.links, list) {
+		if (link->source) {
+			ret = link->source->index;
+			break;
+		}
+	}
+	return ret;
+}
+
+/*
+ * V4L2 subdev operations
+ */
+
+/*
+ * isp_subdev_ioctl - CCDC module private ioctl's
+ * @sd: ISP V4L2 subdevice
+ * @cmd: ioctl command
+ * @arg: ioctl argument
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static long isp_subdev_ioctl(struct v4l2_subdev *sd,
+			     unsigned int cmd, void *arg)
+{
+	return 0;
+}
+
+/*
+ * isp_subdev_set_power - Power on/off the CCDC module
+ * @sd: ISP V4L2 subdevice
+ * @on: power on/off
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int isp_subdev_set_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static int isp_subdev_subscribe_event(struct v4l2_subdev *sd,
+				      struct v4l2_fh *fh,
+				      struct v4l2_event_subscription *sub)
+{
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = isp_sd->isp;
+
+	if (sub->type != V4L2_EVENT_FRAME_SYNC &&
+	    sub->type != V4L2_EVENT_FRAME_END &&
+	    sub->type != V4L2_EVENT_ATOMISP_3A_STATS_READY &&
+	    sub->type != V4L2_EVENT_ATOMISP_METADATA_READY &&
+	    sub->type != V4L2_EVENT_ATOMISP_PAUSE_BUFFER &&
+	    sub->type != V4L2_EVENT_ATOMISP_CSS_RESET &&
+	    sub->type != V4L2_EVENT_ATOMISP_RAW_BUFFERS_ALLOC_DONE &&
+	    sub->type != V4L2_EVENT_ATOMISP_ACC_COMPLETE)
+		return -EINVAL;
+
+	if (sub->type == V4L2_EVENT_FRAME_SYNC &&
+	    !atomisp_css_valid_sof(isp))
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, 16, NULL);
+}
+
+static int isp_subdev_unsubscribe_event(struct v4l2_subdev *sd,
+					struct v4l2_fh *fh,
+					struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
+
+/*
+ * isp_subdev_enum_mbus_code - Handle pixel format enumeration
+ * @sd: pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @code: pointer to v4l2_subdev_pad_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int isp_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(atomisp_in_fmt_conv) - 1)
+		return -EINVAL;
+
+	code->code = atomisp_in_fmt_conv[code->index].code;
+
+	return 0;
+}
+
+static int isp_subdev_validate_rect(struct v4l2_subdev *sd, uint32_t pad,
+				    uint32_t target)
+{
+	switch (pad) {
+	case ATOMISP_SUBDEV_PAD_SINK:
+		switch (target) {
+		case V4L2_SEL_TGT_CROP:
+			return 0;
+		}
+		break;
+	default:
+		switch (target) {
+		case V4L2_SEL_TGT_COMPOSE:
+			return 0;
+		}
+		break;
+	}
+
+	return -EINVAL;
+}
+
+struct v4l2_rect *atomisp_subdev_get_rect(struct v4l2_subdev *sd,
+	struct v4l2_subdev_state *sd_state,
+	u32 which, uint32_t pad,
+	uint32_t target)
+{
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY) {
+		switch (target) {
+		case V4L2_SEL_TGT_CROP:
+			return v4l2_subdev_get_try_crop(sd, sd_state, pad);
+		case V4L2_SEL_TGT_COMPOSE:
+			return v4l2_subdev_get_try_compose(sd, sd_state, pad);
+		}
+	}
+
+	switch (target) {
+	case V4L2_SEL_TGT_CROP:
+		return &isp_sd->fmt[pad].crop;
+	case V4L2_SEL_TGT_COMPOSE:
+		return &isp_sd->fmt[pad].compose;
+	}
+
+	return NULL;
+}
+
+struct v4l2_mbus_framefmt
+*atomisp_subdev_get_ffmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *sd_state, uint32_t which,
+			 uint32_t pad)
+{
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(sd, sd_state, pad);
+
+	return &isp_sd->fmt[pad].fmt;
+}
+
+static void isp_get_fmt_rect(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     uint32_t which,
+			     struct v4l2_mbus_framefmt **ffmt,
+			     struct v4l2_rect *crop[ATOMISP_SUBDEV_PADS_NUM],
+			     struct v4l2_rect *comp[ATOMISP_SUBDEV_PADS_NUM])
+{
+	unsigned int i;
+
+	for (i = 0; i < ATOMISP_SUBDEV_PADS_NUM; i++) {
+		ffmt[i] = atomisp_subdev_get_ffmt(sd, sd_state, which, i);
+		crop[i] = atomisp_subdev_get_rect(sd, sd_state, which, i,
+						  V4L2_SEL_TGT_CROP);
+		comp[i] = atomisp_subdev_get_rect(sd, sd_state, which, i,
+						  V4L2_SEL_TGT_COMPOSE);
+	}
+}
+
+static void isp_subdev_propagate(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 u32 which, uint32_t pad, uint32_t target,
+				 uint32_t flags)
+{
+	struct v4l2_mbus_framefmt *ffmt[ATOMISP_SUBDEV_PADS_NUM];
+	struct v4l2_rect *crop[ATOMISP_SUBDEV_PADS_NUM],
+		       *comp[ATOMISP_SUBDEV_PADS_NUM];
+
+	if (flags & V4L2_SEL_FLAG_KEEP_CONFIG)
+		return;
+
+	isp_get_fmt_rect(sd, sd_state, which, ffmt, crop, comp);
+
+	switch (pad) {
+	case ATOMISP_SUBDEV_PAD_SINK: {
+		struct v4l2_rect r = {0};
+
+		/* Only crop target supported on sink pad. */
+		r.width = ffmt[pad]->width;
+		r.height = ffmt[pad]->height;
+
+		atomisp_subdev_set_selection(sd, sd_state, which, pad,
+					     target, flags, &r);
+		break;
+	}
+	}
+}
+
+static int isp_subdev_get_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_rect *rec;
+	int rval = isp_subdev_validate_rect(sd, sel->pad, sel->target);
+
+	if (rval)
+		return rval;
+
+	rec = atomisp_subdev_get_rect(sd, sd_state, sel->which, sel->pad,
+				      sel->target);
+	if (!rec)
+		return -EINVAL;
+
+	sel->r = *rec;
+	return 0;
+}
+
+static const char *atomisp_pad_str(unsigned int pad)
+{
+	static const char *const pad_str[] = {
+		"ATOMISP_SUBDEV_PAD_SINK",
+		"ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE",
+		"ATOMISP_SUBDEV_PAD_SOURCE_VF",
+		"ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW",
+		"ATOMISP_SUBDEV_PAD_SOURCE_VIDEO",
+	};
+
+	if (pad >= ARRAY_SIZE(pad_str))
+		return "ATOMISP_INVALID_PAD";
+	return pad_str[pad];
+}
+
+int atomisp_subdev_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 u32 which, uint32_t pad, uint32_t target,
+				 u32 flags, struct v4l2_rect *r)
+{
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = isp_sd->isp;
+	struct v4l2_mbus_framefmt *ffmt[ATOMISP_SUBDEV_PADS_NUM];
+	struct v4l2_rect *crop[ATOMISP_SUBDEV_PADS_NUM],
+		       *comp[ATOMISP_SUBDEV_PADS_NUM];
+	unsigned int i;
+	unsigned int padding_w = pad_w;
+	unsigned int padding_h = pad_h;
+
+	isp_get_fmt_rect(sd, sd_state, which, ffmt, crop, comp);
+
+	dev_dbg(isp->dev,
+		"sel: pad %s tgt %s l %d t %d w %d h %d which %s f 0x%8.8x\n",
+		atomisp_pad_str(pad), target == V4L2_SEL_TGT_CROP
+		? "V4L2_SEL_TGT_CROP" : "V4L2_SEL_TGT_COMPOSE",
+		r->left, r->top, r->width, r->height,
+		which == V4L2_SUBDEV_FORMAT_TRY ? "V4L2_SUBDEV_FORMAT_TRY"
+		: "V4L2_SUBDEV_FORMAT_ACTIVE", flags);
+
+	r->width = rounddown(r->width, ATOM_ISP_STEP_WIDTH);
+	r->height = rounddown(r->height, ATOM_ISP_STEP_HEIGHT);
+
+	switch (pad) {
+	case ATOMISP_SUBDEV_PAD_SINK: {
+		/* Only crop target supported on sink pad. */
+		unsigned int dvs_w, dvs_h;
+
+		crop[pad]->width = ffmt[pad]->width;
+		crop[pad]->height = ffmt[pad]->height;
+
+		/* Workaround for BYT 1080p perfectshot since the maxinum resolution of
+		 * front camera ov2722 is 1932x1092 and cannot use pad_w > 12*/
+		if (!strncmp(isp->inputs[isp_sd->input_curr].camera->name,
+			     "ov2722", 6) && crop[pad]->height == 1092) {
+			padding_w = 12;
+			padding_h = 12;
+		}
+
+		if (isp->inputs[isp_sd->input_curr].type == SOC_CAMERA) {
+			padding_w = 0;
+			padding_h = 0;
+		}
+
+		if (atomisp_subdev_format_conversion(isp_sd,
+						     isp_sd->capture_pad)
+		    && crop[pad]->width && crop[pad]->height) {
+			crop[pad]->width -= padding_w;
+			crop[pad]->height -= padding_h;
+		}
+
+		/* if subdev type is SOC camera,we do not need to set DVS */
+		if (isp->inputs[isp_sd->input_curr].type == SOC_CAMERA)
+			isp_sd->params.video_dis_en = 0;
+
+		if (isp_sd->params.video_dis_en &&
+		    isp_sd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+		    !isp_sd->continuous_mode->val) {
+			/* This resolution contains 20 % of DVS slack
+			 * (of the desired captured image before
+			 * scaling, or 1 / 6 of what we get from the
+			 * sensor) in both width and height. Remove
+			 * it. */
+			crop[pad]->width = roundup(crop[pad]->width * 5 / 6,
+						   ATOM_ISP_STEP_WIDTH);
+			crop[pad]->height = roundup(crop[pad]->height * 5 / 6,
+						    ATOM_ISP_STEP_HEIGHT);
+		}
+
+		crop[pad]->width = min(crop[pad]->width, r->width);
+		crop[pad]->height = min(crop[pad]->height, r->height);
+
+		if (!(flags & V4L2_SEL_FLAG_KEEP_CONFIG)) {
+			for (i = ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE;
+			     i < ATOMISP_SUBDEV_PADS_NUM; i++) {
+				struct v4l2_rect tmp = *crop[pad];
+
+				atomisp_subdev_set_selection(
+				    sd, sd_state, which, i,
+				    V4L2_SEL_TGT_COMPOSE,
+				    flags, &tmp);
+			}
+		}
+
+		if (which == V4L2_SUBDEV_FORMAT_TRY)
+			break;
+
+		if (isp_sd->params.video_dis_en &&
+		    isp_sd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
+		    !isp_sd->continuous_mode->val) {
+			dvs_w = rounddown(crop[pad]->width / 5,
+					  ATOM_ISP_STEP_WIDTH);
+			dvs_h = rounddown(crop[pad]->height / 5,
+					  ATOM_ISP_STEP_HEIGHT);
+		} else if (!isp_sd->params.video_dis_en &&
+			   isp_sd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
+			/*
+			 * For CSS2.0, digital zoom needs to set dvs envelope to 12
+			 * when dvs is disabled.
+			 */
+			dvs_w = dvs_h = 12;
+		} else {
+			dvs_w = dvs_h = 0;
+		}
+		atomisp_css_video_set_dis_envelope(isp_sd, dvs_w, dvs_h);
+		atomisp_css_input_set_effective_resolution(isp_sd,
+							   ATOMISP_INPUT_STREAM_GENERAL,
+							   crop[pad]->width,
+							   crop[pad]->height);
+		break;
+	}
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO: {
+		/* Only compose target is supported on source pads. */
+
+		if (isp_sd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+			/* Scaling is disabled in this mode */
+			r->width = crop[ATOMISP_SUBDEV_PAD_SINK]->width;
+			r->height = crop[ATOMISP_SUBDEV_PAD_SINK]->height;
+		}
+
+		if (crop[ATOMISP_SUBDEV_PAD_SINK]->width == r->width
+		    && crop[ATOMISP_SUBDEV_PAD_SINK]->height == r->height)
+			isp_sd->params.yuv_ds_en = false;
+		else
+			isp_sd->params.yuv_ds_en = true;
+
+		comp[pad]->width = r->width;
+		comp[pad]->height = r->height;
+
+		if (r->width == 0 || r->height == 0 ||
+		    crop[ATOMISP_SUBDEV_PAD_SINK]->width == 0 ||
+		    crop[ATOMISP_SUBDEV_PAD_SINK]->height == 0)
+			break;
+		/*
+		 * do cropping on sensor input if ratio of required resolution
+		 * is different with sensor output resolution ratio:
+		 *
+		 * ratio = width / height
+		 *
+		 * if ratio_output < ratio_sensor:
+		 *	effect_width = sensor_height * out_width / out_height;
+		 *	effect_height = sensor_height;
+		 * else
+		 *	effect_width = sensor_width;
+		 *	effect_height = sensor_width * out_height / out_width;
+		 *
+		 */
+		if (r->width * crop[ATOMISP_SUBDEV_PAD_SINK]->height <
+		    crop[ATOMISP_SUBDEV_PAD_SINK]->width * r->height)
+			atomisp_css_input_set_effective_resolution(isp_sd,
+				ATOMISP_INPUT_STREAM_GENERAL,
+				rounddown(crop[ATOMISP_SUBDEV_PAD_SINK]->
+					  height * r->width / r->height,
+					  ATOM_ISP_STEP_WIDTH),
+				crop[ATOMISP_SUBDEV_PAD_SINK]->height);
+		else
+			atomisp_css_input_set_effective_resolution(isp_sd,
+				ATOMISP_INPUT_STREAM_GENERAL,
+				crop[ATOMISP_SUBDEV_PAD_SINK]->width,
+				rounddown(crop[ATOMISP_SUBDEV_PAD_SINK]->
+					  width * r->height / r->width,
+					  ATOM_ISP_STEP_WIDTH));
+
+		break;
+	}
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF:
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
+		comp[pad]->width = r->width;
+		comp[pad]->height = r->height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Set format dimensions on non-sink pads as well. */
+	if (pad != ATOMISP_SUBDEV_PAD_SINK) {
+		ffmt[pad]->width = comp[pad]->width;
+		ffmt[pad]->height = comp[pad]->height;
+	}
+
+	if (!atomisp_subdev_get_rect(sd, sd_state, which, pad, target))
+		return -EINVAL;
+	*r = *atomisp_subdev_get_rect(sd, sd_state, which, pad, target);
+
+	dev_dbg(isp->dev, "sel actual: l %d t %d w %d h %d\n",
+		r->left, r->top, r->width, r->height);
+
+	return 0;
+}
+
+static int isp_subdev_set_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_selection *sel)
+{
+	int rval = isp_subdev_validate_rect(sd, sel->pad, sel->target);
+
+	if (rval)
+		return rval;
+
+	return atomisp_subdev_set_selection(sd, sd_state, sel->which,
+					    sel->pad,
+					    sel->target, sel->flags, &sel->r);
+}
+
+static int atomisp_get_sensor_bin_factor(struct atomisp_sub_device *asd)
+{
+	struct v4l2_control ctrl = {0};
+	struct atomisp_device *isp = asd->isp;
+	int hbin, vbin;
+	int ret;
+
+	if (isp->inputs[asd->input_curr].type == FILE_INPUT ||
+	    isp->inputs[asd->input_curr].type == TEST_PATTERN)
+		return 0;
+
+	ctrl.id = V4L2_CID_BIN_FACTOR_HORZ;
+	ret =
+	    v4l2_g_ctrl(isp->inputs[asd->input_curr].camera->ctrl_handler,
+			&ctrl);
+	hbin = ctrl.value;
+	ctrl.id = V4L2_CID_BIN_FACTOR_VERT;
+	ret |=
+	    v4l2_g_ctrl(isp->inputs[asd->input_curr].camera->ctrl_handler,
+			&ctrl);
+	vbin = ctrl.value;
+
+	/*
+	 * ISP needs to know binning factor from sensor.
+	 * In case horizontal and vertical sensor's binning factors
+	 * are different or sensor does not support binning factor CID,
+	 * ISP will apply default 0 value.
+	 */
+	if (ret || hbin != vbin)
+		hbin = 0;
+
+	return hbin;
+}
+
+void atomisp_subdev_set_ffmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     uint32_t which,
+			     u32 pad, struct v4l2_mbus_framefmt *ffmt)
+{
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = isp_sd->isp;
+	struct v4l2_mbus_framefmt *__ffmt =
+	    atomisp_subdev_get_ffmt(sd, sd_state, which, pad);
+
+	dev_dbg(isp->dev, "ffmt: pad %s w %d h %d code 0x%8.8x which %s\n",
+		atomisp_pad_str(pad), ffmt->width, ffmt->height, ffmt->code,
+		which == V4L2_SUBDEV_FORMAT_TRY ? "V4L2_SUBDEV_FORMAT_TRY"
+		: "V4L2_SUBDEV_FORMAT_ACTIVE");
+
+	switch (pad) {
+	case ATOMISP_SUBDEV_PAD_SINK: {
+		const struct atomisp_in_fmt_conv *fc =
+		    atomisp_find_in_fmt_conv(ffmt->code);
+
+		if (!fc) {
+			fc = atomisp_in_fmt_conv;
+			ffmt->code = fc->code;
+			dev_dbg(isp->dev, "using 0x%8.8x instead\n",
+				ffmt->code);
+		}
+
+		*__ffmt = *ffmt;
+
+		isp_subdev_propagate(sd, sd_state, which, pad,
+				     V4L2_SEL_TGT_CROP, 0);
+
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			atomisp_css_input_set_resolution(isp_sd,
+							 ATOMISP_INPUT_STREAM_GENERAL, ffmt);
+			atomisp_css_input_set_binning_factor(isp_sd,
+							     ATOMISP_INPUT_STREAM_GENERAL,
+							     atomisp_get_sensor_bin_factor(isp_sd));
+			atomisp_css_input_set_bayer_order(isp_sd, ATOMISP_INPUT_STREAM_GENERAL,
+							  fc->bayer_order);
+			atomisp_css_input_set_format(isp_sd, ATOMISP_INPUT_STREAM_GENERAL,
+						     fc->atomisp_in_fmt);
+			atomisp_css_set_default_isys_config(isp_sd, ATOMISP_INPUT_STREAM_GENERAL,
+							    ffmt);
+		}
+
+		break;
+	}
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF:
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO:
+		__ffmt->code = ffmt->code;
+		break;
+	}
+}
+
+/*
+ * isp_subdev_get_format - Retrieve the video format on a pad
+ * @sd : ISP V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @pad: Pad number
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int isp_subdev_get_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	fmt->format = *atomisp_subdev_get_ffmt(sd, sd_state, fmt->which,
+					       fmt->pad);
+
+	return 0;
+}
+
+/*
+ * isp_subdev_set_format - Set the video format on a pad
+ * @sd : ISP subdev V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @pad: Pad number
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int isp_subdev_set_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	atomisp_subdev_set_ffmt(sd, sd_state, fmt->which, fmt->pad,
+				&fmt->format);
+
+	return 0;
+}
+
+/* V4L2 subdev core operations */
+static const struct v4l2_subdev_core_ops isp_subdev_v4l2_core_ops = {
+	.ioctl = isp_subdev_ioctl, .s_power = isp_subdev_set_power,
+	.subscribe_event = isp_subdev_subscribe_event,
+	.unsubscribe_event = isp_subdev_unsubscribe_event,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops isp_subdev_v4l2_pad_ops = {
+	.enum_mbus_code = isp_subdev_enum_mbus_code,
+	.get_fmt = isp_subdev_get_format,
+	.set_fmt = isp_subdev_set_format,
+	.get_selection = isp_subdev_get_selection,
+	.set_selection = isp_subdev_set_selection,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops isp_subdev_v4l2_ops = {
+	.core = &isp_subdev_v4l2_core_ops,
+	.pad = &isp_subdev_v4l2_pad_ops,
+};
+
+static void isp_subdev_init_params(struct atomisp_sub_device *asd)
+{
+	unsigned int i;
+
+	/* parameters initialization */
+	INIT_LIST_HEAD(&asd->s3a_stats);
+	INIT_LIST_HEAD(&asd->s3a_stats_in_css);
+	INIT_LIST_HEAD(&asd->s3a_stats_ready);
+	INIT_LIST_HEAD(&asd->dis_stats);
+	INIT_LIST_HEAD(&asd->dis_stats_in_css);
+	spin_lock_init(&asd->dis_stats_lock);
+	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
+		INIT_LIST_HEAD(&asd->metadata[i]);
+		INIT_LIST_HEAD(&asd->metadata_in_css[i]);
+		INIT_LIST_HEAD(&asd->metadata_ready[i]);
+	}
+}
+
+/*
+* isp_subdev_link_setup - Setup isp subdev connections
+* @entity: ispsubdev media entity
+* @local: Pad at the local end of the link
+* @remote: Pad at the remote end of the link
+* @flags: Link flags
+*
+* return -EINVAL or zero on success
+*/
+static int isp_subdev_link_setup(struct media_entity *entity,
+				 const struct media_pad *local,
+				 const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct atomisp_sub_device *isp_sd = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = isp_sd->isp;
+	unsigned int i;
+
+	switch (local->index | is_media_entity_v4l2_subdev(remote->entity)) {
+	case ATOMISP_SUBDEV_PAD_SINK | MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN:
+		/* Read from the sensor CSI2-ports. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			isp_sd->input = ATOMISP_SUBDEV_INPUT_NONE;
+			break;
+		}
+
+		if (isp_sd->input != ATOMISP_SUBDEV_INPUT_NONE)
+			return -EBUSY;
+
+		for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++) {
+			if (remote->entity != &isp->csi2_port[i].subdev.entity)
+				continue;
+
+			isp_sd->input = ATOMISP_SUBDEV_INPUT_CSI2_PORT1 + i;
+			return 0;
+		}
+
+		return -EINVAL;
+
+	case ATOMISP_SUBDEV_PAD_SINK | MEDIA_ENT_F_OLD_BASE:
+		/* read from memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (isp_sd->input >= ATOMISP_SUBDEV_INPUT_CSI2_PORT1 &&
+			    isp_sd->input < (ATOMISP_SUBDEV_INPUT_CSI2_PORT1
+					     + ATOMISP_CAMERA_NR_PORTS))
+				return -EBUSY;
+			isp_sd->input = ATOMISP_SUBDEV_INPUT_MEMORY;
+		} else {
+			if (isp_sd->input == ATOMISP_SUBDEV_INPUT_MEMORY)
+				isp_sd->input = ATOMISP_SUBDEV_INPUT_NONE;
+		}
+		break;
+
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW | MEDIA_ENT_F_OLD_BASE:
+		/* always write to memory */
+		break;
+
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF | MEDIA_ENT_F_OLD_BASE:
+		/* always write to memory */
+		break;
+
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE | MEDIA_ENT_F_OLD_BASE:
+		/* always write to memory */
+		break;
+
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO | MEDIA_ENT_F_OLD_BASE:
+		/* always write to memory */
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations isp_subdev_media_ops = {
+	.link_setup = isp_subdev_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+	/*	 .set_power = v4l2_subdev_set_power,	*/
+};
+
+static int __atomisp_update_run_mode(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_ctrl *ctrl = asd->run_mode;
+	struct v4l2_ctrl *c;
+	s32 mode;
+
+	if (ctrl->val != ATOMISP_RUN_MODE_VIDEO &&
+	    asd->continuous_mode->val)
+		mode = ATOMISP_RUN_MODE_PREVIEW;
+	else
+		mode = ctrl->val;
+
+	c = v4l2_ctrl_find(
+		isp->inputs[asd->input_curr].camera->ctrl_handler,
+		V4L2_CID_RUN_MODE);
+
+	if (c)
+		return v4l2_ctrl_s_ctrl(c, mode);
+
+	return 0;
+}
+
+int atomisp_update_run_mode(struct atomisp_sub_device *asd)
+{
+	int rval;
+
+	mutex_lock(asd->ctrl_handler.lock);
+	rval = __atomisp_update_run_mode(asd);
+	mutex_unlock(asd->ctrl_handler.lock);
+
+	return rval;
+}
+
+static int s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct atomisp_sub_device *asd = container_of(
+					     ctrl->handler, struct atomisp_sub_device, ctrl_handler);
+	unsigned int streaming;
+	unsigned long flags;
+
+	switch (ctrl->id) {
+	case V4L2_CID_RUN_MODE:
+		return __atomisp_update_run_mode(asd);
+	case V4L2_CID_DEPTH_MODE:
+		/* Use spinlock instead of mutex to avoid possible locking issues */
+		spin_lock_irqsave(&asd->isp->lock, flags);
+		streaming = asd->streaming;
+		spin_unlock_irqrestore(&asd->isp->lock, flags);
+		if (streaming != ATOMISP_DEVICE_STREAMING_DISABLED) {
+			dev_err(asd->isp->dev,
+				"ISP is streaming, it is not supported to change the depth mode\n");
+			return -EINVAL;
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = &s_ctrl,
+};
+
+static const struct v4l2_ctrl_config ctrl_fmt_auto = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_FMT_AUTO,
+	.name = "Automatic format guessing",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static const char *const ctrl_run_mode_menu[] = {
+	NULL,
+	"Video",
+	"Still capture",
+	"Continuous capture",
+	"Preview",
+};
+
+static const struct v4l2_ctrl_config ctrl_run_mode = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_RUN_MODE,
+	.name = "Atomisp run mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.def = 1,
+	.max = 4,
+	.qmenu = ctrl_run_mode_menu,
+};
+
+static const char *const ctrl_vfpp_mode_menu[] = {
+	"Enable",			/* vfpp always enabled */
+	"Disable to scaler mode",	/* CSS into video mode and disable */
+	"Disable to low latency mode",	/* CSS into still mode and disable */
+};
+
+static const struct v4l2_ctrl_config ctrl_vfpp = {
+	.id = V4L2_CID_VFPP,
+	.name = "Atomisp vf postprocess",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 0,
+	.def = 0,
+	.max = 2,
+	.qmenu = ctrl_vfpp_mode_menu,
+};
+
+/*
+ * Control for ISP continuous mode
+ *
+ * When enabled, capture processing is possible without
+ * stopping the preview pipeline. When disabled, ISP needs
+ * to be restarted between preview and capture.
+ */
+static const struct v4l2_ctrl_config ctrl_continuous_mode = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_ATOMISP_CONTINUOUS_MODE,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Continuous mode",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+/*
+ * Control for continuous mode raw buffer size
+ *
+ * The size of the RAW ringbuffer sets limit on how much
+ * back in time application can go when requesting capture
+ * frames to be rendered, and how many frames can be rendered
+ * in a burst at full sensor rate.
+ *
+ * Note: this setting has a big impact on memory consumption of
+ * the CSS subsystem.
+ */
+static const struct v4l2_ctrl_config ctrl_continuous_raw_buffer_size = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_ATOMISP_CONTINUOUS_RAW_BUFFER_SIZE,
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.name = "Continuous raw ringbuffer size",
+	.min = 1,
+	.max = 100, /* depends on CSS version, runtime checked */
+	.step = 1,
+	.def = 3,
+};
+
+/*
+ * Control for enabling continuous viewfinder
+ *
+ * When enabled, and ISP is in continuous mode (see ctrl_continuous_mode ),
+ * preview pipeline continues concurrently with capture
+ * processing. When disabled, and continuous mode is used,
+ * preview is paused while captures are processed, but
+ * full pipeline restart is not needed.
+ *
+ * By setting this to disabled, capture processing is
+ * essentially given priority over preview, and the effective
+ * capture output rate may be higher than with continuous
+ * viewfinder enabled.
+ */
+static const struct v4l2_ctrl_config ctrl_continuous_viewfinder = {
+	.id = V4L2_CID_ATOMISP_CONTINUOUS_VIEWFINDER,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Continuous viewfinder",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+/*
+ * Control for enabling Lock&Unlock Raw Buffer mechanism
+ *
+ * When enabled, Raw Buffer can be locked and unlocked.
+ * Application can hold the exp_id of Raw Buffer
+ * and unlock it when no longer needed.
+ * Note: Make sure set this configuration before creating stream.
+ */
+static const struct v4l2_ctrl_config ctrl_enable_raw_buffer_lock = {
+	.id = V4L2_CID_ENABLE_RAW_BUFFER_LOCK,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Lock Unlock Raw Buffer",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+/*
+ * Control to disable digital zoom of the whole stream
+ *
+ * When it is true, pipe configuration enable_dz will be set to false.
+ * This can help get a better performance by disabling pp binary.
+ *
+ * Note: Make sure set this configuration before creating stream.
+ */
+static const struct v4l2_ctrl_config ctrl_disable_dz = {
+	.id = V4L2_CID_DISABLE_DZ,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Disable digital zoom",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+/*
+ * Control for ISP depth mode
+ *
+ * When enabled, that means ISP will deal with dual streams and sensors will be
+ * in slave/master mode.
+ * slave sensor will have no output until master sensor is streamed on.
+ */
+static const struct v4l2_ctrl_config ctrl_depth_mode = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_DEPTH_MODE,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Depth mode",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+static void atomisp_init_subdev_pipe(struct atomisp_sub_device *asd,
+				     struct atomisp_video_pipe *pipe, enum v4l2_buf_type buf_type)
+{
+	pipe->type = buf_type;
+	pipe->asd = asd;
+	pipe->isp = asd->isp;
+	spin_lock_init(&pipe->irq_lock);
+	INIT_LIST_HEAD(&pipe->activeq);
+	INIT_LIST_HEAD(&pipe->buffers_waiting_for_param);
+	INIT_LIST_HEAD(&pipe->per_frame_params);
+	memset(pipe->frame_request_config_id,
+	       0, VIDEO_MAX_FRAME * sizeof(unsigned int));
+	memset(pipe->frame_params,
+	       0, VIDEO_MAX_FRAME *
+	       sizeof(struct atomisp_css_params_with_list *));
+}
+
+/*
+ * isp_subdev_init_entities - Initialize V4L2 subdev and media entity
+ * @asd: ISP CCDC module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int isp_subdev_init_entities(struct atomisp_sub_device *asd)
+{
+	struct v4l2_subdev *sd = &asd->subdev;
+	struct media_pad *pads = asd->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	asd->input = ATOMISP_SUBDEV_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &isp_subdev_v4l2_ops);
+	sprintf(sd->name, "ATOMISP_SUBDEV_%d", asd->index);
+	v4l2_set_subdevdata(sd, asd);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[ATOMISP_SUBDEV_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW].flags = MEDIA_PAD_FL_SOURCE;
+	pads[ATOMISP_SUBDEV_PAD_SOURCE_VF].flags = MEDIA_PAD_FL_SOURCE;
+	pads[ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE].flags = MEDIA_PAD_FL_SOURCE;
+	pads[ATOMISP_SUBDEV_PAD_SOURCE_VIDEO].flags = MEDIA_PAD_FL_SOURCE;
+
+	asd->fmt[ATOMISP_SUBDEV_PAD_SINK].fmt.code =
+	    MEDIA_BUS_FMT_SBGGR10_1X10;
+	asd->fmt[ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW].fmt.code =
+	    MEDIA_BUS_FMT_SBGGR10_1X10;
+	asd->fmt[ATOMISP_SUBDEV_PAD_SOURCE_VF].fmt.code =
+	    MEDIA_BUS_FMT_SBGGR10_1X10;
+	asd->fmt[ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE].fmt.code =
+	    MEDIA_BUS_FMT_SBGGR10_1X10;
+	asd->fmt[ATOMISP_SUBDEV_PAD_SOURCE_VIDEO].fmt.code =
+	    MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	me->ops = &isp_subdev_media_ops;
+	me->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+	ret = media_entity_pads_init(me, ATOMISP_SUBDEV_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	atomisp_init_subdev_pipe(asd, &asd->video_out_preview,
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	atomisp_init_subdev_pipe(asd, &asd->video_out_vf,
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	atomisp_init_subdev_pipe(asd, &asd->video_out_capture,
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	atomisp_init_subdev_pipe(asd, &asd->video_out_video_capture,
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	ret = atomisp_video_init(&asd->video_out_capture, "CAPTURE",
+				 ATOMISP_RUN_MODE_STILL_CAPTURE);
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_vf, "VIEWFINDER",
+				 ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE);
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_preview, "PREVIEW",
+				 ATOMISP_RUN_MODE_PREVIEW);
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_video_capture, "VIDEO",
+				 ATOMISP_RUN_MODE_VIDEO);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_ctrl_handler_init(&asd->ctrl_handler, 1);
+	if (ret)
+		return ret;
+
+	asd->fmt_auto = v4l2_ctrl_new_custom(&asd->ctrl_handler,
+					     &ctrl_fmt_auto, NULL);
+	asd->run_mode = v4l2_ctrl_new_custom(&asd->ctrl_handler,
+					     &ctrl_run_mode, NULL);
+	asd->vfpp = v4l2_ctrl_new_custom(&asd->ctrl_handler,
+					 &ctrl_vfpp, NULL);
+	asd->continuous_mode = v4l2_ctrl_new_custom(&asd->ctrl_handler,
+			       &ctrl_continuous_mode, NULL);
+	asd->continuous_viewfinder = v4l2_ctrl_new_custom(&asd->ctrl_handler,
+				     &ctrl_continuous_viewfinder,
+				     NULL);
+	asd->continuous_raw_buffer_size =
+	    v4l2_ctrl_new_custom(&asd->ctrl_handler,
+				 &ctrl_continuous_raw_buffer_size,
+				 NULL);
+
+	asd->enable_raw_buffer_lock =
+	    v4l2_ctrl_new_custom(&asd->ctrl_handler,
+				 &ctrl_enable_raw_buffer_lock,
+				 NULL);
+	asd->depth_mode =
+	    v4l2_ctrl_new_custom(&asd->ctrl_handler,
+				 &ctrl_depth_mode,
+				 NULL);
+	asd->disable_dz =
+	    v4l2_ctrl_new_custom(&asd->ctrl_handler,
+				 &ctrl_disable_dz,
+				 NULL);
+
+	/* Make controls visible on subdev as well. */
+	asd->subdev.ctrl_handler = &asd->ctrl_handler;
+	spin_lock_init(&asd->raw_buffer_bitmap_lock);
+	return asd->ctrl_handler.error;
+}
+
+int atomisp_create_pads_links(struct atomisp_device *isp)
+{
+	struct atomisp_sub_device *asd;
+	int i, j, ret = 0;
+
+	isp->num_of_streams = 2;
+	for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++) {
+		for (j = 0; j < isp->num_of_streams; j++) {
+			ret =
+			    media_create_pad_link(&isp->csi2_port[i].subdev.
+						  entity, CSI2_PAD_SOURCE,
+						  &isp->asd[j].subdev.entity,
+						  ATOMISP_SUBDEV_PAD_SINK, 0);
+			if (ret < 0)
+				return ret;
+		}
+	}
+	for (i = 0; i < isp->input_cnt; i++) {
+		/* Don't create links for the test-pattern-generator */
+		if (isp->inputs[i].type == TEST_PATTERN)
+			continue;
+
+		ret = media_create_pad_link(&isp->inputs[i].camera->entity, 0,
+					    &isp->csi2_port[isp->inputs[i].
+						    port].subdev.entity,
+					    CSI2_PAD_SINK,
+					    MEDIA_LNK_FL_ENABLED |
+					    MEDIA_LNK_FL_IMMUTABLE);
+		if (ret < 0)
+			return ret;
+	}
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		ret = media_create_pad_link(&asd->subdev.entity,
+					    ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW,
+					    &asd->video_out_preview.vdev.entity,
+					    0, 0);
+		if (ret < 0)
+			return ret;
+		ret = media_create_pad_link(&asd->subdev.entity,
+					    ATOMISP_SUBDEV_PAD_SOURCE_VF,
+					    &asd->video_out_vf.vdev.entity, 0,
+					    0);
+		if (ret < 0)
+			return ret;
+		ret = media_create_pad_link(&asd->subdev.entity,
+					    ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE,
+					    &asd->video_out_capture.vdev.entity,
+					    0, 0);
+		if (ret < 0)
+			return ret;
+		ret = media_create_pad_link(&asd->subdev.entity,
+					    ATOMISP_SUBDEV_PAD_SOURCE_VIDEO,
+					    &asd->video_out_video_capture.vdev.
+					    entity, 0, 0);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static void atomisp_subdev_cleanup_entities(struct atomisp_sub_device *asd)
+{
+	v4l2_ctrl_handler_free(&asd->ctrl_handler);
+
+	media_entity_cleanup(&asd->subdev.entity);
+}
+
+void atomisp_subdev_cleanup_pending_events(struct atomisp_sub_device *asd)
+{
+	struct v4l2_fh *fh, *fh_tmp;
+	struct v4l2_event event;
+	unsigned int i, pending_event;
+
+	list_for_each_entry_safe(fh, fh_tmp,
+				 &asd->subdev.devnode->fh_list, list) {
+		pending_event = v4l2_event_pending(fh);
+		for (i = 0; i < pending_event; i++)
+			v4l2_event_dequeue(fh, &event, 1);
+	}
+}
+
+void atomisp_subdev_unregister_entities(struct atomisp_sub_device *asd)
+{
+	atomisp_subdev_cleanup_entities(asd);
+	v4l2_device_unregister_subdev(&asd->subdev);
+	atomisp_video_unregister(&asd->video_out_preview);
+	atomisp_video_unregister(&asd->video_out_vf);
+	atomisp_video_unregister(&asd->video_out_capture);
+	atomisp_video_unregister(&asd->video_out_video_capture);
+}
+
+int atomisp_subdev_register_subdev(struct atomisp_sub_device *asd,
+				   struct v4l2_device *vdev)
+{
+	return v4l2_device_register_subdev(vdev, &asd->subdev);
+}
+
+int atomisp_subdev_register_video_nodes(struct atomisp_sub_device *asd,
+					struct v4l2_device *vdev)
+{
+	int ret;
+
+	/*
+	 * FIXME: check if all device caps are properly initialized.
+	 * Should any of those use V4L2_CAP_META_CAPTURE? Probably yes.
+	 */
+
+	asd->video_out_preview.vdev.v4l2_dev = vdev;
+	asd->video_out_preview.vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	ret = video_register_device(&asd->video_out_preview.vdev,
+				    VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		goto error;
+
+	asd->video_out_capture.vdev.v4l2_dev = vdev;
+	asd->video_out_capture.vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	ret = video_register_device(&asd->video_out_capture.vdev,
+				    VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		goto error;
+
+	asd->video_out_vf.vdev.v4l2_dev = vdev;
+	asd->video_out_vf.vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	ret = video_register_device(&asd->video_out_vf.vdev,
+				    VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		goto error;
+
+	asd->video_out_video_capture.vdev.v4l2_dev = vdev;
+	asd->video_out_video_capture.vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	ret = video_register_device(&asd->video_out_video_capture.vdev,
+				    VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	atomisp_subdev_unregister_entities(asd);
+	return ret;
+}
+
+/*
+ * atomisp_subdev_init - ISP Subdevice  initialization.
+ * @dev: Device pointer specific to the ATOM ISP.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int atomisp_subdev_init(struct atomisp_device *isp)
+{
+	struct atomisp_sub_device *asd;
+	int i, ret = 0;
+
+	/*
+	 * CSS2.0 running ISP2400 support
+	 * multiple streams
+	 */
+	isp->num_of_streams = 2;
+	isp->asd = devm_kzalloc(isp->dev, sizeof(struct atomisp_sub_device) *
+				isp->num_of_streams, GFP_KERNEL);
+	if (!isp->asd)
+		return -ENOMEM;
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		asd->isp = isp;
+		isp_subdev_init_params(asd);
+		asd->index = i;
+		ret = isp_subdev_init_entities(asd);
+		if (ret < 0) {
+			atomisp_subdev_cleanup_entities(asd);
+			break;
+		}
+	}
+
+	return ret;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_subdev.h b/drivers/staging/media/atomisp/pci/atomisp_subdev.h
new file mode 100644
index 000000000..a1f4da352
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_subdev.h
@@ -0,0 +1,414 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef __ATOMISP_SUBDEV_H__
+#define __ATOMISP_SUBDEV_H__
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf-core.h>
+
+#include "atomisp_common.h"
+#include "atomisp_compat.h"
+#include "atomisp_v4l2.h"
+
+#include "ia_css.h"
+
+/* EXP_ID's ranger is 1 ~ 250 */
+#define ATOMISP_MAX_EXP_ID     (250)
+enum atomisp_subdev_input_entity {
+	ATOMISP_SUBDEV_INPUT_NONE,
+	ATOMISP_SUBDEV_INPUT_MEMORY,
+	ATOMISP_SUBDEV_INPUT_CSI2,
+	/*
+	 * The following enum for CSI2 port must go together in one row.
+	 * Otherwise it breaks the code logic.
+	 */
+	ATOMISP_SUBDEV_INPUT_CSI2_PORT1,
+	ATOMISP_SUBDEV_INPUT_CSI2_PORT2,
+	ATOMISP_SUBDEV_INPUT_CSI2_PORT3,
+};
+
+#define ATOMISP_SUBDEV_PAD_SINK			0
+/* capture output for still frames */
+#define ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE	1
+/* viewfinder output for downscaled capture output */
+#define ATOMISP_SUBDEV_PAD_SOURCE_VF		2
+/* preview output for display */
+#define ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW	3
+/* main output for video pipeline */
+#define ATOMISP_SUBDEV_PAD_SOURCE_VIDEO	4
+#define ATOMISP_SUBDEV_PADS_NUM			5
+
+struct atomisp_in_fmt_conv {
+	u32     code;
+	u8 bpp; /* bits per pixel */
+	u8 depth; /* uncompressed */
+	enum atomisp_input_format atomisp_in_fmt;
+	enum ia_css_bayer_order bayer_order;
+};
+
+struct atomisp_sub_device;
+
+struct atomisp_video_pipe {
+	struct video_device vdev;
+	enum v4l2_buf_type type;
+	struct media_pad pad;
+	struct videobuf_queue capq;
+	struct list_head activeq;
+	/*
+	 * the buffers waiting for per-frame parameters, this is only valid
+	 * in per-frame setting mode.
+	 */
+	struct list_head buffers_waiting_for_param;
+	/* the link list to store per_frame parameters */
+	struct list_head per_frame_params;
+
+	/* Store here the initial run mode */
+	unsigned int default_run_mode;
+
+	unsigned int buffers_in_css;
+
+	/*
+	 * irq_lock is used to protect video buffer state change operations and
+	 * also to make activeq and capq operations atomic.
+	 */
+	spinlock_t irq_lock;
+	unsigned int users;
+
+	struct atomisp_device *isp;
+	struct v4l2_pix_format pix;
+	u32 sh_fmt;
+
+	struct atomisp_sub_device *asd;
+
+	/*
+	 * This frame_config_id is got from CSS when dequueues buffers from CSS,
+	 * it is used to indicate which parameter it has applied.
+	 */
+	unsigned int frame_config_id[VIDEO_MAX_FRAME];
+	/*
+	 * This config id is set when camera HAL enqueues buffer, it has a
+	 * non-zero value to indicate which parameter it needs to applu
+	 */
+	unsigned int frame_request_config_id[VIDEO_MAX_FRAME];
+	struct atomisp_css_params_with_list *frame_params[VIDEO_MAX_FRAME];
+};
+
+struct atomisp_pad_format {
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_rect crop;
+	struct v4l2_rect compose;
+};
+
+/* Internal states for flash process */
+enum atomisp_flash_state {
+	ATOMISP_FLASH_IDLE,
+	ATOMISP_FLASH_REQUESTED,
+	ATOMISP_FLASH_ONGOING,
+	ATOMISP_FLASH_DONE
+};
+
+/*
+ * This structure is used to cache the CSS parameters, it aligns to
+ * struct ia_css_isp_config but without un-supported and deprecated parts.
+ */
+struct atomisp_css_params {
+	struct ia_css_wb_config   wb_config;
+	struct ia_css_cc_config   cc_config;
+	struct ia_css_tnr_config  tnr_config;
+	struct ia_css_ecd_config  ecd_config;
+	struct ia_css_ynr_config  ynr_config;
+	struct ia_css_fc_config   fc_config;
+	struct ia_css_formats_config formats_config;
+	struct ia_css_cnr_config  cnr_config;
+	struct ia_css_macc_config macc_config;
+	struct ia_css_ctc_config  ctc_config;
+	struct ia_css_aa_config   aa_config;
+	struct ia_css_aa_config   baa_config;
+	struct ia_css_ce_config   ce_config;
+	struct ia_css_ob_config   ob_config;
+	struct ia_css_dp_config   dp_config;
+	struct ia_css_de_config   de_config;
+	struct ia_css_gc_config   gc_config;
+	struct ia_css_nr_config   nr_config;
+	struct ia_css_ee_config   ee_config;
+	struct ia_css_anr_config  anr_config;
+	struct ia_css_3a_config   s3a_config;
+	struct ia_css_xnr_config  xnr_config;
+	struct ia_css_dz_config   dz_config;
+	struct ia_css_cc_config yuv2rgb_cc_config;
+	struct ia_css_cc_config rgb2yuv_cc_config;
+	struct ia_css_macc_table  macc_table;
+	struct ia_css_gamma_table gamma_table;
+	struct ia_css_ctc_table   ctc_table;
+
+	struct ia_css_xnr_table   xnr_table;
+	struct ia_css_rgb_gamma_table r_gamma_table;
+	struct ia_css_rgb_gamma_table g_gamma_table;
+	struct ia_css_rgb_gamma_table b_gamma_table;
+
+	struct ia_css_vector      motion_vector;
+	struct ia_css_anr_thres   anr_thres;
+
+	struct ia_css_dvs_6axis_config *dvs_6axis;
+	struct ia_css_dvs2_coefficients *dvs2_coeff;
+	struct ia_css_shading_table *shading_table;
+	struct ia_css_morph_table   *morph_table;
+
+	/*
+	 * Used to store the user pointer address of the frame. driver needs to
+	 * translate to ia_css_frame * and then set to CSS.
+	 */
+	void		*output_frame;
+	u32	isp_config_id;
+
+	/* Indicates which parameters need to be updated. */
+	struct atomisp_parameters update_flag;
+};
+
+struct atomisp_subdev_params {
+	/* FIXME: Determines whether raw capture buffer are being passed to
+	 * user space. Unimplemented for now. */
+	int online_process;
+	int yuv_ds_en;
+	unsigned int color_effect;
+	bool gdc_cac_en;
+	bool macc_en;
+	bool bad_pixel_en;
+	bool video_dis_en;
+	bool sc_en;
+	bool fpn_en;
+	bool xnr_en;
+	bool low_light;
+	int false_color;
+	unsigned int histogram_elenum;
+
+	/* Current grid info */
+	struct ia_css_grid_info curr_grid_info;
+	enum ia_css_pipe_id s3a_enabled_pipe;
+
+	int s3a_output_bytes;
+
+	bool dis_proj_data_valid;
+
+	struct ia_css_dz_config   dz_config;  /** Digital Zoom */
+	struct ia_css_capture_config   capture_config;
+
+	struct ia_css_isp_config config;
+
+	/* current configurations */
+	struct atomisp_css_params css_param;
+
+	/*
+	 * Intermediate buffers used to communicate data between
+	 * CSS and user space.
+	 */
+	struct ia_css_3a_statistics *s3a_user_stat;
+
+	void *metadata_user[ATOMISP_METADATA_TYPE_NUM];
+	u32 metadata_width_size;
+
+	struct ia_css_dvs2_statistics *dvs_stat;
+	struct ia_css_dvs_6axis_config *dvs_6axis;
+	u32 exp_id;
+	int  dvs_hor_coef_bytes;
+	int  dvs_ver_coef_bytes;
+	int  dvs_ver_proj_bytes;
+	int  dvs_hor_proj_bytes;
+
+	/* Flash */
+	int num_flash_frames;
+	enum atomisp_flash_state flash_state;
+	enum atomisp_frame_status last_frame_status;
+
+	/* continuous capture */
+	struct atomisp_cont_capture_conf offline_parm;
+	/* Flag to check if driver needs to update params to css */
+	bool css_update_params_needed;
+};
+
+struct atomisp_css_params_with_list {
+	/* parameters for CSS */
+	struct atomisp_css_params params;
+	struct list_head list;
+};
+
+struct atomisp_sub_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[ATOMISP_SUBDEV_PADS_NUM];
+	struct atomisp_pad_format fmt[ATOMISP_SUBDEV_PADS_NUM];
+	u16 capture_pad; /* main capture pad; defines much of isp config */
+
+	enum atomisp_subdev_input_entity input;
+	unsigned int output;
+	struct atomisp_video_pipe video_out_capture; /* capture output */
+	struct atomisp_video_pipe video_out_vf;      /* viewfinder output */
+	struct atomisp_video_pipe video_out_preview; /* preview output */
+	/* video pipe main output */
+	struct atomisp_video_pipe video_out_video_capture;
+	/* struct isp_subdev_params params; */
+	struct atomisp_device *isp;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *fmt_auto;
+	struct v4l2_ctrl *run_mode;
+	struct v4l2_ctrl *depth_mode;
+	struct v4l2_ctrl *vfpp;
+	struct v4l2_ctrl *continuous_mode;
+	struct v4l2_ctrl *continuous_raw_buffer_size;
+	struct v4l2_ctrl *continuous_viewfinder;
+	struct v4l2_ctrl *enable_raw_buffer_lock;
+
+	/* ISP2401 */
+	struct v4l2_ctrl *ion_dev_fd;
+
+	struct v4l2_ctrl *disable_dz;
+
+	struct atomisp_subdev_params params;
+
+	struct atomisp_stream_env stream_env[ATOMISP_INPUT_STREAM_NUM];
+
+	struct v4l2_pix_format dvs_envelop;
+	unsigned int s3a_bufs_in_css[IA_CSS_PIPE_ID_NUM];
+	unsigned int dis_bufs_in_css;
+
+	unsigned int metadata_bufs_in_css
+	[ATOMISP_INPUT_STREAM_NUM][IA_CSS_PIPE_ID_NUM];
+	/* The list of free and available metadata buffers for CSS */
+	struct list_head metadata[ATOMISP_METADATA_TYPE_NUM];
+	/* The list of metadata buffers which have been en-queued to CSS */
+	struct list_head metadata_in_css[ATOMISP_METADATA_TYPE_NUM];
+	/* The list of metadata buffers which are ready for userspace to get */
+	struct list_head metadata_ready[ATOMISP_METADATA_TYPE_NUM];
+
+	/* The list of free and available s3a stat buffers for CSS */
+	struct list_head s3a_stats;
+	/* The list of s3a stat buffers which have been en-queued to CSS */
+	struct list_head s3a_stats_in_css;
+	/* The list of s3a stat buffers which are ready for userspace to get */
+	struct list_head s3a_stats_ready;
+
+	struct list_head dis_stats;
+	struct list_head dis_stats_in_css;
+	spinlock_t dis_stats_lock;
+
+	struct ia_css_frame *vf_frame; /* TODO: needed? */
+	struct ia_css_frame *raw_output_frame;
+	enum atomisp_frame_status frame_status[VIDEO_MAX_FRAME];
+
+	/* This field specifies which camera (v4l2 input) is selected. */
+	int input_curr;
+
+	atomic_t sof_count;
+	atomic_t sequence;      /* Sequence value that is assigned to buffer. */
+	atomic_t sequence_temp;
+
+	/*
+	 * Writers of streaming must hold both isp->mutex and isp->lock.
+	 * Readers of streaming need to hold only one of the two locks.
+	 */
+	unsigned int streaming;
+	bool stream_prepared; /* whether css stream is created */
+
+	/* subdev index: will be used to show which subdev is holding the
+	 * resource, like which camera is used by which subdev
+	 */
+	unsigned int index;
+
+	/* delayed memory allocation for css */
+	struct completion init_done;
+	struct workqueue_struct *delayed_init_workq;
+	unsigned int delayed_init;
+	struct work_struct delayed_init_work;
+
+	unsigned int latest_preview_exp_id; /* CSS ZSL/SDV raw buffer id */
+
+	unsigned int mipi_frame_size;
+
+	bool copy_mode; /* CSI2+ use copy mode */
+	bool yuvpp_mode;	/* CSI2+ yuvpp pipe */
+
+	int raw_buffer_bitmap[ATOMISP_MAX_EXP_ID / 32 +
+						 1]; /* Record each Raw Buffer lock status */
+	int raw_buffer_locked_count;
+	spinlock_t raw_buffer_bitmap_lock;
+
+	/* ISP2401 */
+	bool re_trigger_capture;
+
+	struct atomisp_resolution sensor_array_res;
+	bool high_speed_mode; /* Indicate whether now is a high speed mode */
+	int pending_capture_request; /* Indicates the number of pending capture requests. */
+
+	unsigned int preview_exp_id;
+	unsigned int postview_exp_id;
+};
+
+extern const struct atomisp_in_fmt_conv atomisp_in_fmt_conv[];
+
+u32 atomisp_subdev_uncompressed_code(u32 code);
+bool atomisp_subdev_is_compressed(u32 code);
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv(u32 code);
+
+/* ISP2400 */
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+    enum atomisp_input_format atomisp_in_fmt);
+
+/* ISP2401 */
+const struct atomisp_in_fmt_conv
+*atomisp_find_in_fmt_conv_by_atomisp_in_fmt(enum atomisp_input_format
+	atomisp_in_fmt);
+
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv_compressed(u32 code);
+bool atomisp_subdev_format_conversion(struct atomisp_sub_device *asd,
+				      unsigned int source_pad);
+uint16_t atomisp_subdev_source_pad(struct video_device *vdev);
+
+/* Get pointer to appropriate format */
+struct v4l2_mbus_framefmt
+*atomisp_subdev_get_ffmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *sd_state, uint32_t which,
+			 uint32_t pad);
+struct v4l2_rect *atomisp_subdev_get_rect(struct v4l2_subdev *sd,
+	struct v4l2_subdev_state *sd_state,
+	u32 which, uint32_t pad,
+	uint32_t target);
+int atomisp_subdev_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 u32 which, uint32_t pad, uint32_t target,
+				 u32 flags, struct v4l2_rect *r);
+/* Actually set the format */
+void atomisp_subdev_set_ffmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     uint32_t which,
+			     u32 pad, struct v4l2_mbus_framefmt *ffmt);
+
+int atomisp_update_run_mode(struct atomisp_sub_device *asd);
+
+void atomisp_subdev_cleanup_pending_events(struct atomisp_sub_device *asd);
+
+void atomisp_subdev_unregister_entities(struct atomisp_sub_device *asd);
+int atomisp_subdev_register_subdev(struct atomisp_sub_device *asd,
+				   struct v4l2_device *vdev);
+int atomisp_subdev_register_video_nodes(struct atomisp_sub_device *asd,
+					struct v4l2_device *vdev);
+int atomisp_subdev_init(struct atomisp_device *isp);
+void atomisp_subdev_cleanup(struct atomisp_device *isp);
+int atomisp_create_pads_links(struct atomisp_device *isp);
+
+#endif /* __ATOMISP_SUBDEV_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_tables.h b/drivers/staging/media/atomisp/pci/atomisp_tables.h
new file mode 100644
index 000000000..e718a3f66
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_tables.h
@@ -0,0 +1,188 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#ifndef	__ATOMISP_TABLES_H__
+#define	__ATOMISP_TABLES_H__
+
+#include "sh_css_params.h"
+
+/*Sepia image effect table*/
+static struct ia_css_cc_config sepia_cc_config = {
+	.fraction_bits  = 8,
+	.matrix	 = {141, 18, 68, -40, -5, -19, 35, 4, 16},
+};
+
+/*Negative image effect table*/
+static struct ia_css_cc_config nega_cc_config = {
+	.fraction_bits  = 8,
+	.matrix	 = {255, 29, 120, 0, 374, 342, 0, 672, -301},
+};
+
+/*Mono image effect table*/
+static struct ia_css_cc_config mono_cc_config = {
+	.fraction_bits  = 8,
+	.matrix	 = {255, 29, 120, 0, 0, 0, 0, 0, 0},
+};
+
+/*Skin whiten image effect table*/
+static struct ia_css_macc_table skin_low_macc_table = {
+	.data = {
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		7168, 0, 2048, 8192,
+		5120, -1024, 2048, 8192,
+		8192, 2048, -1024, 5120,
+		8192, 2048, 0, 7168,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192
+	}
+};
+
+static struct ia_css_macc_table skin_medium_macc_table = {
+	.data = {
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		5120, 0, 6144, 8192,
+		3072, -1024, 2048, 6144,
+		6144, 2048, -1024, 3072,
+		8192, 6144, 0, 5120,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192
+	}
+};
+
+static struct ia_css_macc_table skin_high_macc_table = {
+	.data = {
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		4096, 0, 8192, 8192,
+		0, -2048, 4096, 6144,
+		6144, 4096, -2048, 0,
+		8192, 8192, 0, 4096,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192
+	}
+};
+
+/*Blue enhencement image effect table*/
+static struct ia_css_macc_table blue_macc_table = {
+	.data = {
+		9728, -3072, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		9728, 0, -3072, 8192,
+		12800, 1536, -3072, 8192,
+		11264, 0, 0, 11264,
+		9728, -3072, 0, 11264
+	}
+};
+
+/*Green enhencement image effect table*/
+static struct ia_css_macc_table green_macc_table = {
+	.data = {
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		10240, 4096, 0, 8192,
+		10240, 4096, 0, 12288,
+		12288, 0, 0, 12288,
+		14336, -2048, 4096, 8192,
+		10240, 0, 4096, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192,
+		8192, 0, 0, 8192
+	}
+};
+
+static struct ia_css_ctc_table vivid_ctc_table = {
+	.data.vamem_2 = {
+		0,  384,  837,  957, 1011, 1062, 1083, 1080,
+		1078, 1077, 1053, 1039, 1012,  992,  969,  951,
+		929,  906,  886,  866,  845,  823,  809,  790,
+		772,  758,  741,  726,  711,  701,  688,  675,
+		666,  656,  648,  639,  633,  626,  618,  612,
+		603,  594,  582,  572,  557,  545,  529,  516,
+		504,  491,  480,  467,  459,  447,  438,  429,
+		419,  412,  404,  397,  389,  382,  376,  368,
+		363,  357,  351,  345,  340,  336,  330,  326,
+		321,  318,  312,  308,  304,  300,  297,  294,
+		291,  286,  284,  281,  278,  275,  271,  268,
+		261,  257,  251,  245,  240,  235,  232,  225,
+		223,  218,  213,  209,  206,  204,  199,  197,
+		193,  189,  186,  185,  183,  179,  177,  175,
+		172,  170,  169,  167,  164,  164,  162,  160,
+		158,  157,  156,  154,  154,  152,  151,  150,
+		149,  148,  146,  147,  146,  144,  143,  143,
+		142,  141,  140,  141,  139,  138,  138,  138,
+		137,  136,  136,  135,  134,  134,  134,  133,
+		132,  132,  131,  130,  131,  130,  129,  128,
+		129,  127,  127,  127,  127,  125,  125,  125,
+		123,  123,  122,  120,  118,  115,  114,  111,
+		110,  108,  106,  105,  103,  102,  100,   99,
+		97,   97,   96,   95,   94,   93,   93,   91,
+		91,   91,   90,   90,   89,   89,   88,   88,
+		89,   88,   88,   87,   87,   87,   87,   86,
+		87,   87,   86,   87,   86,   86,   84,   84,
+		82,   80,   78,   76,   74,   72,   70,   68,
+		67,   65,   62,   60,   58,   56,   55,   54,
+		53,   51,   49,   49,   47,   45,   45,   45,
+		41,   40,   39,   39,   34,   33,   34,   32,
+		25,   23,   24,   20,   13,    9,   12,    0,
+		0
+	}
+};
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp_tpg.c b/drivers/staging/media/atomisp/pci/atomisp_tpg.c
new file mode 100644
index 000000000..e29a96da5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_tpg.c
@@ -0,0 +1,164 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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 <media/v4l2-event.h>
+#include <media/v4l2-mediabus.h>
+#include "atomisp_internal.h"
+#include "atomisp_tpg.h"
+
+static int tpg_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	return 0;
+}
+
+static int tpg_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *format)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+	/* only raw8 grbg is supported by TPG */
+	fmt->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *fmt;
+		return 0;
+	}
+	return 0;
+}
+
+static int tpg_log_status(struct v4l2_subdev *sd)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_s_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static int tpg_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_enum_frame_ival(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_interval_enum *fie)
+{
+	/*to fake*/
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops tpg_video_ops = {
+	.s_stream = tpg_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops tpg_core_ops = {
+	.log_status = tpg_log_status,
+	.s_power = tpg_s_power,
+};
+
+static const struct v4l2_subdev_pad_ops tpg_pad_ops = {
+	.enum_mbus_code = tpg_enum_mbus_code,
+	.enum_frame_size = tpg_enum_frame_size,
+	.enum_frame_interval = tpg_enum_frame_ival,
+	.get_fmt = tpg_get_fmt,
+	.set_fmt = tpg_set_fmt,
+};
+
+static const struct v4l2_subdev_ops tpg_ops = {
+	.core = &tpg_core_ops,
+	.video = &tpg_video_ops,
+	.pad = &tpg_pad_ops,
+};
+
+void atomisp_tpg_unregister_entities(struct atomisp_tpg_device *tpg)
+{
+	media_entity_cleanup(&tpg->sd.entity);
+	v4l2_device_unregister_subdev(&tpg->sd);
+}
+
+int atomisp_tpg_register_entities(struct atomisp_tpg_device *tpg,
+				  struct v4l2_device *vdev)
+{
+	int ret;
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &tpg->sd);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	atomisp_tpg_unregister_entities(tpg);
+	return ret;
+}
+
+void atomisp_tpg_cleanup(struct atomisp_device *isp)
+{
+}
+
+int atomisp_tpg_init(struct atomisp_device *isp)
+{
+	struct atomisp_tpg_device *tpg = &isp->tpg;
+	struct v4l2_subdev *sd = &tpg->sd;
+	struct media_pad *pads = tpg->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	tpg->isp = isp;
+	v4l2_subdev_init(sd, &tpg_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strscpy(sd->name, "tpg_subdev", sizeof(sd->name));
+	v4l2_set_subdevdata(sd, tpg);
+
+	pads[0].flags = MEDIA_PAD_FL_SINK;
+	me->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+
+	ret = media_entity_pads_init(me, 1, pads);
+	if (ret < 0)
+		goto fail;
+	return 0;
+fail:
+	atomisp_tpg_cleanup(isp);
+	return ret;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp_tpg.h b/drivers/staging/media/atomisp/pci/atomisp_tpg.h
new file mode 100644
index 000000000..4176e076f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_tpg.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __ATOMISP_TPG_H__
+#define __ATOMISP_TPG_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+struct atomisp_tpg_device {
+	struct v4l2_subdev sd;
+	struct atomisp_device *isp;
+	struct media_pad pads[1];
+};
+
+void atomisp_tpg_cleanup(struct atomisp_device *isp);
+int atomisp_tpg_init(struct atomisp_device *isp);
+void atomisp_tpg_unregister_entities(struct atomisp_tpg_device *tpg);
+int atomisp_tpg_register_entities(struct atomisp_tpg_device *tpg,
+				  struct v4l2_device *vdev);
+
+#endif /* __ATOMISP_TPG_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp_trace_event.h b/drivers/staging/media/atomisp/pci/atomisp_trace_event.h
new file mode 100644
index 000000000..538d45e00
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_trace_event.h
@@ -0,0 +1,128 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support Camera Imaging tracer core.
+ *
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM atomisp
+
+#if !defined(ATOMISP_TRACE_EVENT_H) || defined(TRACE_HEADER_MULTI_READ)
+#define ATOMISP_TRACE_EVENT_H
+
+#include <linux/tracepoint.h>
+#include <linux/string.h>
+TRACE_EVENT(camera_meminfo,
+
+	    TP_PROTO(const char *name, int uptr_size, int counter, int sys_size,
+		     int sys_res_size, int cam_sys_use, int cam_dyc_use,
+		     int cam_res_use),
+
+	    TP_ARGS(name, uptr_size, counter, sys_size, sys_res_size, cam_sys_use,
+		    cam_dyc_use, cam_res_use),
+
+	    TP_STRUCT__entry(
+		__array(char, name, 24)
+		__field(int, uptr_size)
+		__field(int, counter)
+		__field(int, sys_size)
+		__field(int, sys_res_size)
+		__field(int, cam_res_use)
+		__field(int, cam_dyc_use)
+		__field(int, cam_sys_use)
+	    ),
+
+	    TP_fast_assign(
+		strscpy(__entry->name, name, 24);
+		__entry->uptr_size = uptr_size;
+		__entry->counter = counter;
+		__entry->sys_size = sys_size;
+		__entry->sys_res_size = sys_res_size;
+		__entry->cam_res_use = cam_res_use;
+		__entry->cam_dyc_use = cam_dyc_use;
+		__entry->cam_sys_use = cam_sys_use;
+	    ),
+
+	    TP_printk(
+		"<%s> User ptr memory:%d pages,\tISP private memory used:%d pages:\tsysFP system size:%d,\treserved size:%d\tcamFP sysUse:%d,\tdycUse:%d,\tresUse:%d.\n",
+		__entry->name, __entry->uptr_size, __entry->counter,
+		__entry->sys_size, __entry->sys_res_size, __entry->cam_sys_use,
+		__entry->cam_dyc_use, __entry->cam_res_use)
+	   );
+
+TRACE_EVENT(camera_debug,
+
+	    TP_PROTO(const char *name, char *info, const int line),
+
+	    TP_ARGS(name, info, line),
+
+	    TP_STRUCT__entry(
+		__array(char, name, 24)
+		__array(char, info, 24)
+		__field(int, line)
+	    ),
+
+	    TP_fast_assign(
+		strscpy(__entry->name, name, 24);
+		strscpy(__entry->info, info, 24);
+		__entry->line = line;
+	    ),
+
+	    TP_printk("<%s>-<%d> %s\n", __entry->name, __entry->line,
+		      __entry->info)
+	   );
+
+TRACE_EVENT(ipu_cstate,
+
+	    TP_PROTO(int cstate),
+
+	    TP_ARGS(cstate),
+
+	    TP_STRUCT__entry(
+		__field(int, cstate)
+	    ),
+
+	    TP_fast_assign(
+		__entry->cstate = cstate;
+	    ),
+
+	    TP_printk("cstate=%d", __entry->cstate)
+	   );
+
+TRACE_EVENT(ipu_pstate,
+
+	    TP_PROTO(int freq, int util),
+
+	    TP_ARGS(freq, util),
+
+	    TP_STRUCT__entry(
+		__field(int, freq)
+		__field(int, util)
+	    ),
+
+	    TP_fast_assign(
+		__entry->freq = freq;
+		__entry->util = util;
+	    ),
+
+	    TP_printk("freq=%d util=%d", __entry->freq, __entry->util)
+	   );
+#endif
+
+#undef TRACE_INCLUDE_PATH
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_PATH .
+#define TRACE_INCLUDE_FILE   atomisp_trace_event
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/staging/media/atomisp/pci/atomisp_v4l2.c b/drivers/staging/media/atomisp/pci/atomisp_v4l2.c
new file mode 100644
index 000000000..d5bb9906c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_v4l2.c
@@ -0,0 +1,1798 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010-2017 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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/module.h>
+#include <linux/pci.h>
+#include <linux/pm_runtime.h>
+#include <linux/pm_qos.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/dmi.h>
+#include <linux/interrupt.h>
+#include <linux/bits.h>
+
+#include <asm/iosf_mbi.h>
+
+#include "../../include/linux/atomisp_gmin_platform.h"
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_fops.h"
+#include "atomisp_ioctl.h"
+#include "atomisp_internal.h"
+#include "atomisp-regs.h"
+#include "atomisp_dfs_tables.h"
+#include "atomisp_drvfs.h"
+#include "hmm/hmm.h"
+#include "atomisp_trace_event.h"
+
+#include "sh_css_firmware.h"
+
+#include "device_access.h"
+
+/* Timeouts to wait for all subdevs to be registered */
+#define SUBDEV_WAIT_TIMEOUT		50 /* ms */
+#define SUBDEV_WAIT_TIMEOUT_MAX_COUNT	40 /* up to 2 seconds */
+
+/* G-Min addition: pull this in from intel_mid_pm.h */
+#define CSTATE_EXIT_LATENCY_C1  1
+
+static uint skip_fwload;
+module_param(skip_fwload, uint, 0644);
+MODULE_PARM_DESC(skip_fwload, "Skip atomisp firmware load");
+
+/* memory optimization: deferred firmware loading */
+bool defer_fw_load;
+module_param(defer_fw_load, bool, 0644);
+MODULE_PARM_DESC(defer_fw_load,
+		 "Defer FW loading until device is opened (default:disable)");
+
+/* cross componnet debug message flag */
+int dbg_level;
+module_param(dbg_level, int, 0644);
+MODULE_PARM_DESC(dbg_level, "debug message level (default:0)");
+
+/* log function switch */
+int dbg_func = 2;
+module_param(dbg_func, int, 0644);
+MODULE_PARM_DESC(dbg_func,
+		 "log function switch non/trace_printk/printk (default:printk)");
+
+int mipicsi_flag;
+module_param(mipicsi_flag, int, 0644);
+MODULE_PARM_DESC(mipicsi_flag, "mipi csi compression predictor algorithm");
+
+static char firmware_name[256];
+module_param_string(firmware_name, firmware_name, sizeof(firmware_name), 0);
+MODULE_PARM_DESC(firmware_name, "Firmware file name. Allows overriding the default firmware name.");
+
+/*set to 16x16 since this is the amount of lines and pixels the sensor
+exports extra. If these are kept at the 10x8 that they were on, in yuv
+downscaling modes incorrect resolutions where requested to the sensor
+driver with strange outcomes as a result. The proper way tot do this
+would be to have a list of tables the specify the sensor res, mipi rec,
+output res, and isp output res. however since we do not have this yet,
+the chosen solution is the next best thing. */
+int pad_w = 16;
+module_param(pad_w, int, 0644);
+MODULE_PARM_DESC(pad_w, "extra data for ISP processing");
+
+int pad_h = 16;
+module_param(pad_h, int, 0644);
+MODULE_PARM_DESC(pad_h, "extra data for ISP processing");
+
+/*
+ * FIXME: this is a hack to make easier to support ISP2401 variant.
+ * As a given system will either be ISP2401 or not, we can just use
+ * a boolean, in order to replace existing #ifdef ISP2401 everywhere.
+ *
+ * Once this driver gets into a better shape, however, the best would
+ * be to replace this to something stored inside atomisp allocated
+ * structures.
+ */
+
+struct device *atomisp_dev;
+
+static const struct atomisp_freq_scaling_rule dfs_rules_merr[] = {
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_457MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+/* Merrifield and Moorefield DFS rules */
+static const struct atomisp_dfs_config dfs_config_merr = {
+	.lowest_freq = ISP_FREQ_200MHZ,
+	.max_freq_at_vmin = ISP_FREQ_400MHZ,
+	.highest_freq = ISP_FREQ_457MHZ,
+	.dfs_table = dfs_rules_merr,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_merr),
+};
+
+static const struct atomisp_freq_scaling_rule dfs_rules_merr_1179[] = {
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static const struct atomisp_dfs_config dfs_config_merr_1179 = {
+	.lowest_freq = ISP_FREQ_200MHZ,
+	.max_freq_at_vmin = ISP_FREQ_400MHZ,
+	.highest_freq = ISP_FREQ_400MHZ,
+	.dfs_table = dfs_rules_merr_1179,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_merr_1179),
+};
+
+static const struct atomisp_freq_scaling_rule dfs_rules_merr_117a[] = {
+	{
+		.width = 1920,
+		.height = 1080,
+		.fps = 30,
+		.isp_freq = ISP_FREQ_266MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = 1080,
+		.height = 1920,
+		.fps = 30,
+		.isp_freq = ISP_FREQ_266MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fps = 45,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = 1080,
+		.height = 1920,
+		.fps = 45,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = 60,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_200MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_200MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static struct atomisp_dfs_config dfs_config_merr_117a = {
+	.lowest_freq = ISP_FREQ_200MHZ,
+	.max_freq_at_vmin = ISP_FREQ_200MHZ,
+	.highest_freq = ISP_FREQ_400MHZ,
+	.dfs_table = dfs_rules_merr_117a,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_merr_117a),
+};
+
+static const struct atomisp_freq_scaling_rule dfs_rules_byt[] = {
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_400MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static const struct atomisp_dfs_config dfs_config_byt = {
+	.lowest_freq = ISP_FREQ_200MHZ,
+	.max_freq_at_vmin = ISP_FREQ_400MHZ,
+	.highest_freq = ISP_FREQ_400MHZ,
+	.dfs_table = dfs_rules_byt,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_byt),
+};
+
+static const struct atomisp_freq_scaling_rule dfs_rules_cht[] = {
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static const struct atomisp_freq_scaling_rule dfs_rules_cht_soc[] = {
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_VIDEO,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_STILL_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_PREVIEW,
+	},
+	{
+		.width = ISP_FREQ_RULE_ANY,
+		.height = ISP_FREQ_RULE_ANY,
+		.fps = ISP_FREQ_RULE_ANY,
+		.isp_freq = ISP_FREQ_356MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static const struct atomisp_dfs_config dfs_config_cht = {
+	.lowest_freq = ISP_FREQ_100MHZ,
+	.max_freq_at_vmin = ISP_FREQ_356MHZ,
+	.highest_freq = ISP_FREQ_356MHZ,
+	.dfs_table = dfs_rules_cht,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_cht),
+};
+
+/* This one should be visible also by atomisp_cmd.c */
+const struct atomisp_dfs_config dfs_config_cht_soc = {
+	.lowest_freq = ISP_FREQ_100MHZ,
+	.max_freq_at_vmin = ISP_FREQ_356MHZ,
+	.highest_freq = ISP_FREQ_356MHZ,
+	.dfs_table = dfs_rules_cht_soc,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_cht_soc),
+};
+
+int atomisp_video_init(struct atomisp_video_pipe *video, const char *name,
+		       unsigned int run_mode)
+{
+	int ret;
+	const char *direction;
+
+	switch (video->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		direction = "output";
+		video->pad.flags = MEDIA_PAD_FL_SINK;
+		video->vdev.fops = &atomisp_fops;
+		video->vdev.ioctl_ops = &atomisp_ioctl_ops;
+		video->vdev.lock = &video->isp->mutex;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = media_entity_pads_init(&video->vdev.entity, 1, &video->pad);
+	if (ret < 0)
+		return ret;
+
+	/* Initialize the video device. */
+	snprintf(video->vdev.name, sizeof(video->vdev.name),
+		 "ATOMISP ISP %s %s", name, direction);
+	video->vdev.release = video_device_release_empty;
+	video_set_drvdata(&video->vdev, video->isp);
+	video->default_run_mode = run_mode;
+
+	return 0;
+}
+
+void atomisp_video_unregister(struct atomisp_video_pipe *video)
+{
+	if (video_is_registered(&video->vdev)) {
+		media_entity_cleanup(&video->vdev.entity);
+		video_unregister_device(&video->vdev);
+	}
+}
+
+static int atomisp_save_iunit_reg(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	pci_read_config_word(pdev, PCI_COMMAND, &isp->saved_regs.pcicmdsts);
+	/* isp->saved_regs.ispmmadr is set from the atomisp_pci_probe() */
+	pci_read_config_dword(pdev, PCI_MSI_CAPID, &isp->saved_regs.msicap);
+	pci_read_config_dword(pdev, PCI_MSI_ADDR, &isp->saved_regs.msi_addr);
+	pci_read_config_word(pdev, PCI_MSI_DATA,  &isp->saved_regs.msi_data);
+	pci_read_config_byte(pdev, PCI_INTERRUPT_LINE, &isp->saved_regs.intr);
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &isp->saved_regs.interrupt_control);
+
+	pci_read_config_dword(pdev, MRFLD_PCI_PMCS, &isp->saved_regs.pmcs);
+	/* Ensure read/write combining is enabled. */
+	pci_read_config_dword(pdev, PCI_I_CONTROL, &isp->saved_regs.i_control);
+	isp->saved_regs.i_control |=
+	    MRFLD_PCI_I_CONTROL_ENABLE_READ_COMBINING |
+	    MRFLD_PCI_I_CONTROL_ENABLE_WRITE_COMBINING;
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_ACCESS_CTRL_VIOL,
+			      &isp->saved_regs.csi_access_viol);
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_RCOMP_CONTROL,
+			      &isp->saved_regs.csi_rcomp_config);
+	/*
+	 * Hardware bugs require setting CSI_HS_OVR_CLK_GATE_ON_UPDATE.
+	 * ANN/CHV: RCOMP updates do not happen when using CSI2+ path
+	 * and sensor sending "continuous clock".
+	 * TNG/ANN/CHV: MIPI packets are lost if the HS entry sequence
+	 * is missed, and IUNIT can hang.
+	 * For both issues, setting this bit is a workaround.
+	 */
+	isp->saved_regs.csi_rcomp_config |= MRFLD_PCI_CSI_HS_OVR_CLK_GATE_ON_UPDATE;
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL,
+			      &isp->saved_regs.csi_afe_dly);
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_CONTROL,
+			      &isp->saved_regs.csi_control);
+	if (isp->media_dev.hw_revision >=
+	    (ATOMISP_HW_REVISION_ISP2401 << ATOMISP_HW_REVISION_SHIFT))
+		isp->saved_regs.csi_control |= MRFLD_PCI_CSI_CONTROL_PARPATHEN;
+	/*
+	 * On CHT CSI_READY bit should be enabled before stream on
+	 */
+	if (IS_CHT && (isp->media_dev.hw_revision >= ((ATOMISP_HW_REVISION_ISP2401 <<
+		       ATOMISP_HW_REVISION_SHIFT) | ATOMISP_HW_STEPPING_B0)))
+		isp->saved_regs.csi_control |= MRFLD_PCI_CSI_CONTROL_CSI_READY;
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_AFE_RCOMP_CONTROL,
+			      &isp->saved_regs.csi_afe_rcomp_config);
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_AFE_HS_CONTROL,
+			      &isp->saved_regs.csi_afe_hs_control);
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_DEADLINE_CONTROL,
+			      &isp->saved_regs.csi_deadline_control);
+	return 0;
+}
+
+static int __maybe_unused atomisp_restore_iunit_reg(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	pci_write_config_word(pdev, PCI_COMMAND, isp->saved_regs.pcicmdsts);
+	pci_write_config_dword(pdev, PCI_BASE_ADDRESS_0, isp->saved_regs.ispmmadr);
+	pci_write_config_dword(pdev, PCI_MSI_CAPID, isp->saved_regs.msicap);
+	pci_write_config_dword(pdev, PCI_MSI_ADDR, isp->saved_regs.msi_addr);
+	pci_write_config_word(pdev, PCI_MSI_DATA, isp->saved_regs.msi_data);
+	pci_write_config_byte(pdev, PCI_INTERRUPT_LINE, isp->saved_regs.intr);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, isp->saved_regs.interrupt_control);
+	pci_write_config_dword(pdev, PCI_I_CONTROL, isp->saved_regs.i_control);
+
+	pci_write_config_dword(pdev, MRFLD_PCI_PMCS, isp->saved_regs.pmcs);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_ACCESS_CTRL_VIOL,
+			       isp->saved_regs.csi_access_viol);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_RCOMP_CONTROL,
+			       isp->saved_regs.csi_rcomp_config);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL,
+			       isp->saved_regs.csi_afe_dly);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_CONTROL,
+			       isp->saved_regs.csi_control);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_AFE_RCOMP_CONTROL,
+			       isp->saved_regs.csi_afe_rcomp_config);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_AFE_HS_CONTROL,
+			       isp->saved_regs.csi_afe_hs_control);
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_DEADLINE_CONTROL,
+			       isp->saved_regs.csi_deadline_control);
+
+	/*
+	 * for MRFLD, Software/firmware needs to write a 1 to bit0
+	 * of the register at CSI_RECEIVER_SELECTION_REG to enable
+	 * SH CSI backend write 0 will enable Arasan CSI backend,
+	 * which has bugs(like sighting:4567697 and 4567699) and
+	 * will be removed in B0
+	 */
+	atomisp_css2_hw_store_32(MRFLD_CSI_RECEIVER_SELECTION_REG, 1);
+	return 0;
+}
+
+static int atomisp_mrfld_pre_power_down(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	u32 irq;
+	unsigned long flags;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (isp->sw_contex.power_state == ATOM_ISP_POWER_DOWN) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		dev_dbg(isp->dev, "<%s %d.\n", __func__, __LINE__);
+		return 0;
+	}
+	/*
+	 * MRFLD HAS requirement: cannot power off i-unit if
+	 * ISP has IRQ not serviced.
+	 * So, here we need to check if there is any pending
+	 * IRQ, if so, waiting for it to be served
+	 */
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= BIT(INTR_IIR);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, irq);
+
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+	if (!(irq & BIT(INTR_IIR)))
+		goto done;
+
+	atomisp_css2_hw_store_32(MRFLD_INTR_CLEAR_REG, 0xFFFFFFFF);
+	atomisp_load_uint32(MRFLD_INTR_STATUS_REG, &irq);
+	if (irq != 0) {
+		dev_err(isp->dev,
+			"%s: fail to clear isp interrupt status reg=0x%x\n",
+			__func__, irq);
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return -EAGAIN;
+	} else {
+		pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+		irq &= BIT(INTR_IIR);
+		pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, irq);
+
+		pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+		if (!(irq & BIT(INTR_IIR))) {
+			atomisp_css2_hw_store_32(MRFLD_INTR_ENABLE_REG, 0x0);
+			goto done;
+		}
+		dev_err(isp->dev,
+			"%s: error in iunit interrupt. status reg=0x%x\n",
+			__func__, irq);
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return -EAGAIN;
+	}
+done:
+	/*
+	* MRFLD WORKAROUND:
+	* before powering off IUNIT, clear the pending interrupts
+	* and disable the interrupt. driver should avoid writing 0
+	* to IIR. It could block subsequent interrupt messages.
+	* HW sighting:4568410.
+	*/
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= ~BIT(INTR_IER);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, irq);
+
+	atomisp_msi_irq_uninit(isp);
+	atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_LOW, true);
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	return 0;
+}
+
+/*
+* WA for DDR DVFS enable/disable
+* By default, ISP will force DDR DVFS 1600MHz before disable DVFS
+*/
+static void punit_ddr_dvfs_enable(bool enable)
+{
+	int door_bell = 1 << 8;
+	int max_wait = 30;
+	int reg;
+
+	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, MRFLD_ISPSSDVFS, &reg);
+	if (enable) {
+		reg &= ~(MRFLD_BIT0 | MRFLD_BIT1);
+	} else {
+		reg |= (MRFLD_BIT1 | door_bell);
+		reg &= ~(MRFLD_BIT0);
+	}
+	iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, MRFLD_ISPSSDVFS, reg);
+
+	/* Check Req_ACK to see freq status, wait until door_bell is cleared */
+	while ((reg & door_bell) && max_wait--) {
+		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, MRFLD_ISPSSDVFS, &reg);
+		usleep_range(100, 500);
+	}
+
+	if (max_wait == -1)
+		pr_info("DDR DVFS, door bell is not cleared within 3ms\n");
+}
+
+static int atomisp_mrfld_power(struct atomisp_device *isp, bool enable)
+{
+	unsigned long timeout;
+	u32 val = enable ? MRFLD_ISPSSPM0_IUNIT_POWER_ON :
+			   MRFLD_ISPSSPM0_IUNIT_POWER_OFF;
+
+	dev_dbg(isp->dev, "IUNIT power-%s.\n", enable ? "on" : "off");
+
+	/* WA for P-Unit, if DVFS enabled, ISP timeout observed */
+	if (IS_CHT && enable)
+		punit_ddr_dvfs_enable(false);
+
+	/*
+	 * FIXME:WA for ECS28A, with this sleep, CTS
+	 * android.hardware.camera2.cts.CameraDeviceTest#testCameraDeviceAbort
+	 * PASS, no impact on other platforms
+	 */
+	if (IS_BYT && enable)
+		msleep(10);
+
+	/* Write to ISPSSPM0 bit[1:0] to power on/off the IUNIT */
+	iosf_mbi_modify(BT_MBI_UNIT_PMC, MBI_REG_READ, MRFLD_ISPSSPM0,
+			val, MRFLD_ISPSSPM0_ISPSSC_MASK);
+
+	/* WA:Enable DVFS */
+	if (IS_CHT && !enable)
+		punit_ddr_dvfs_enable(true);
+
+	/*
+	 * There should be no IUNIT access while power-down is
+	 * in progress. HW sighting: 4567865.
+	 * Wait up to 50 ms for the IUNIT to shut down.
+	 * And we do the same for power on.
+	 */
+	timeout = jiffies + msecs_to_jiffies(50);
+	do {
+		u32 tmp;
+
+		/* Wait until ISPSSPM0 bit[25:24] shows the right value */
+		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, MRFLD_ISPSSPM0, &tmp);
+		tmp = (tmp >> MRFLD_ISPSSPM0_ISPSSS_OFFSET) & MRFLD_ISPSSPM0_ISPSSC_MASK;
+		if (tmp == val) {
+			trace_ipu_cstate(enable);
+			return 0;
+		}
+
+		if (time_after(jiffies, timeout))
+			break;
+
+		/* FIXME: experienced value for delay */
+		usleep_range(100, 150);
+	} while (1);
+
+	if (enable)
+		msleep(10);
+
+	dev_err(isp->dev, "IUNIT power-%s timeout.\n", enable ? "on" : "off");
+	return -EBUSY;
+}
+
+/* Workaround for pmu_nc_set_power_state not ready in MRFLD */
+int atomisp_mrfld_power_down(struct atomisp_device *isp)
+{
+	return atomisp_mrfld_power(isp, false);
+}
+
+/* Workaround for pmu_nc_set_power_state not ready in MRFLD */
+int atomisp_mrfld_power_up(struct atomisp_device *isp)
+{
+	return atomisp_mrfld_power(isp, true);
+}
+
+int atomisp_runtime_suspend(struct device *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)
+				     dev_get_drvdata(dev);
+	int ret;
+
+	ret = atomisp_mrfld_pre_power_down(isp);
+	if (ret)
+		return ret;
+
+	/*Turn off the ISP d-phy*/
+	ret = atomisp_ospm_dphy_down(isp);
+	if (ret)
+		return ret;
+	cpu_latency_qos_update_request(&isp->pm_qos, PM_QOS_DEFAULT_VALUE);
+	return atomisp_mrfld_power_down(isp);
+}
+
+int atomisp_runtime_resume(struct device *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)
+				     dev_get_drvdata(dev);
+	int ret;
+
+	ret = atomisp_mrfld_power_up(isp);
+	if (ret)
+		return ret;
+
+	cpu_latency_qos_update_request(&isp->pm_qos, isp->max_isr_latency);
+	if (isp->sw_contex.power_state == ATOM_ISP_POWER_DOWN) {
+		/*Turn on ISP d-phy */
+		ret = atomisp_ospm_dphy_up(isp);
+		if (ret) {
+			dev_err(isp->dev, "Failed to power up ISP!.\n");
+			return -EINVAL;
+		}
+	}
+
+	/*restore register values for iUnit and iUnitPHY registers*/
+	if (isp->saved_regs.pcicmdsts)
+		atomisp_restore_iunit_reg(isp);
+
+	atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_LOW, true);
+	return 0;
+}
+
+static int __maybe_unused atomisp_suspend(struct device *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)
+				     dev_get_drvdata(dev);
+	/* FIXME: only has one isp_subdev at present */
+	struct atomisp_sub_device *asd = &isp->asd[0];
+	unsigned long flags;
+	int ret;
+
+	/*
+	 * FIXME: Suspend is not supported by sensors. Abort if any video
+	 * node was opened.
+	 */
+	if (atomisp_dev_users(isp))
+		return -EBUSY;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_DISABLED) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		dev_err(isp->dev, "atomisp cannot suspend at this time.\n");
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	ret = atomisp_mrfld_pre_power_down(isp);
+	if (ret)
+		return ret;
+
+	/*Turn off the ISP d-phy */
+	ret = atomisp_ospm_dphy_down(isp);
+	if (ret) {
+		dev_err(isp->dev, "fail to power off ISP\n");
+		return ret;
+	}
+	cpu_latency_qos_update_request(&isp->pm_qos, PM_QOS_DEFAULT_VALUE);
+	return atomisp_mrfld_power_down(isp);
+}
+
+static int __maybe_unused atomisp_resume(struct device *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)
+				     dev_get_drvdata(dev);
+	int ret;
+
+	ret = atomisp_mrfld_power_up(isp);
+	if (ret)
+		return ret;
+
+	cpu_latency_qos_update_request(&isp->pm_qos, isp->max_isr_latency);
+
+	/*Turn on ISP d-phy */
+	ret = atomisp_ospm_dphy_up(isp);
+	if (ret) {
+		dev_err(isp->dev, "Failed to power up ISP!.\n");
+		return -EINVAL;
+	}
+
+	/*restore register values for iUnit and iUnitPHY registers*/
+	if (isp->saved_regs.pcicmdsts)
+		atomisp_restore_iunit_reg(isp);
+
+	atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_LOW, true);
+	return 0;
+}
+
+int atomisp_csi_lane_config(struct atomisp_device *isp)
+{
+	struct pci_dev *pdev = to_pci_dev(isp->dev);
+	static const struct {
+		u8 code;
+		u8 lanes[MRFLD_PORT_NUM];
+	} portconfigs[] = {
+		/* Tangier/Merrifield available lane configurations */
+		{ 0x00, { 4, 1, 0 } },		/* 00000 */
+		{ 0x01, { 3, 1, 0 } },		/* 00001 */
+		{ 0x02, { 2, 1, 0 } },		/* 00010 */
+		{ 0x03, { 1, 1, 0 } },		/* 00011 */
+		{ 0x04, { 2, 1, 2 } },		/* 00100 */
+		{ 0x08, { 3, 1, 1 } },		/* 01000 */
+		{ 0x09, { 2, 1, 1 } },		/* 01001 */
+		{ 0x0a, { 1, 1, 1 } },		/* 01010 */
+
+		/* Anniedale/Moorefield only configurations */
+		{ 0x10, { 4, 2, 0 } },		/* 10000 */
+		{ 0x11, { 3, 2, 0 } },		/* 10001 */
+		{ 0x12, { 2, 2, 0 } },		/* 10010 */
+		{ 0x13, { 1, 2, 0 } },		/* 10011 */
+		{ 0x14, { 2, 2, 2 } },		/* 10100 */
+		{ 0x18, { 3, 2, 1 } },		/* 11000 */
+		{ 0x19, { 2, 2, 1 } },		/* 11001 */
+		{ 0x1a, { 1, 2, 1 } },		/* 11010 */
+	};
+
+	unsigned int i, j;
+	u8 sensor_lanes[MRFLD_PORT_NUM] = { 0 };
+	u32 csi_control;
+	int nportconfigs;
+	u32 port_config_mask;
+	int port3_lanes_shift;
+
+	if (isp->media_dev.hw_revision <
+	    ATOMISP_HW_REVISION_ISP2401_LEGACY <<
+	    ATOMISP_HW_REVISION_SHIFT) {
+		/* Merrifield */
+		port_config_mask = MRFLD_PORT_CONFIG_MASK;
+		port3_lanes_shift = MRFLD_PORT3_LANES_SHIFT;
+	} else {
+		/* Moorefield / Cherryview */
+		port_config_mask = CHV_PORT_CONFIG_MASK;
+		port3_lanes_shift = CHV_PORT3_LANES_SHIFT;
+	}
+
+	if (isp->media_dev.hw_revision <
+	    ATOMISP_HW_REVISION_ISP2401 <<
+	    ATOMISP_HW_REVISION_SHIFT) {
+		/* Merrifield / Moorefield legacy input system */
+		nportconfigs = MRFLD_PORT_CONFIG_NUM;
+	} else {
+		/* Moorefield / Cherryview new input system */
+		nportconfigs = ARRAY_SIZE(portconfigs);
+	}
+
+	for (i = 0; i < isp->input_cnt; i++) {
+		struct camera_mipi_info *mipi_info;
+
+		if (isp->inputs[i].type != RAW_CAMERA &&
+		    isp->inputs[i].type != SOC_CAMERA)
+			continue;
+
+		mipi_info = atomisp_to_sensor_mipi_info(isp->inputs[i].camera);
+		if (!mipi_info)
+			continue;
+
+		switch (mipi_info->port) {
+		case ATOMISP_CAMERA_PORT_PRIMARY:
+			sensor_lanes[0] = mipi_info->num_lanes;
+			break;
+		case ATOMISP_CAMERA_PORT_SECONDARY:
+			sensor_lanes[1] = mipi_info->num_lanes;
+			break;
+		case ATOMISP_CAMERA_PORT_TERTIARY:
+			sensor_lanes[2] = mipi_info->num_lanes;
+			break;
+		default:
+			dev_err(isp->dev,
+				"%s: invalid port: %d for the %dth sensor\n",
+				__func__, mipi_info->port, i);
+			return -EINVAL;
+		}
+	}
+
+	for (i = 0; i < nportconfigs; i++) {
+		for (j = 0; j < MRFLD_PORT_NUM; j++)
+			if (sensor_lanes[j] &&
+			    sensor_lanes[j] != portconfigs[i].lanes[j])
+				break;
+
+		if (j == MRFLD_PORT_NUM)
+			break;			/* Found matching setting */
+	}
+
+	if (i >= nportconfigs) {
+		dev_err(isp->dev,
+			"%s: could not find the CSI port setting for %d-%d-%d\n",
+			__func__,
+			sensor_lanes[0], sensor_lanes[1], sensor_lanes[2]);
+		return -EINVAL;
+	}
+
+	pci_read_config_dword(pdev, MRFLD_PCI_CSI_CONTROL, &csi_control);
+	csi_control &= ~port_config_mask;
+	csi_control |= (portconfigs[i].code << MRFLD_PORT_CONFIGCODE_SHIFT)
+		       | (portconfigs[i].lanes[0] ? 0 : (1 << MRFLD_PORT1_ENABLE_SHIFT))
+		       | (portconfigs[i].lanes[1] ? 0 : (1 << MRFLD_PORT2_ENABLE_SHIFT))
+		       | (portconfigs[i].lanes[2] ? 0 : (1 << MRFLD_PORT3_ENABLE_SHIFT))
+		       | (((1 << portconfigs[i].lanes[0]) - 1) << MRFLD_PORT1_LANES_SHIFT)
+		       | (((1 << portconfigs[i].lanes[1]) - 1) << MRFLD_PORT2_LANES_SHIFT)
+		       | (((1 << portconfigs[i].lanes[2]) - 1) << port3_lanes_shift);
+
+	pci_write_config_dword(pdev, MRFLD_PCI_CSI_CONTROL, csi_control);
+
+	dev_dbg(isp->dev,
+		"%s: the portconfig is %d-%d-%d, CSI_CONTROL is 0x%08X\n",
+		__func__, portconfigs[i].lanes[0], portconfigs[i].lanes[1],
+		portconfigs[i].lanes[2], csi_control);
+
+	return 0;
+}
+
+static int atomisp_subdev_probe(struct atomisp_device *isp)
+{
+	const struct atomisp_platform_data *pdata;
+	struct intel_v4l2_subdev_table *subdevs;
+	int ret, raw_index = -1, count;
+
+	pdata = atomisp_get_platform_data();
+	if (!pdata) {
+		dev_err(isp->dev, "no platform data available\n");
+		return 0;
+	}
+
+	/* FIXME: should return -EPROBE_DEFER if not all subdevs were probed */
+	for (count = 0; count < SUBDEV_WAIT_TIMEOUT_MAX_COUNT; count++) {
+		int camera_count = 0;
+
+		for (subdevs = pdata->subdevs; subdevs->type; ++subdevs) {
+			if (subdevs->type == RAW_CAMERA ||
+			    subdevs->type == SOC_CAMERA)
+				camera_count++;
+		}
+		if (camera_count)
+			break;
+		msleep(SUBDEV_WAIT_TIMEOUT);
+	}
+	/* Wait more time to give more time for subdev init code to finish */
+	msleep(5 * SUBDEV_WAIT_TIMEOUT);
+
+	/* FIXME: should, instead, use I2C probe */
+
+	for (subdevs = pdata->subdevs; subdevs->type; ++subdevs) {
+		struct v4l2_subdev *subdev;
+		struct i2c_board_info *board_info =
+			    &subdevs->v4l2_subdev.board_info;
+		struct i2c_adapter *adapter =
+		    i2c_get_adapter(subdevs->v4l2_subdev.i2c_adapter_id);
+
+		dev_info(isp->dev, "Probing Subdev %s\n", board_info->type);
+
+		if (!adapter) {
+			dev_err(isp->dev,
+				"Failed to find i2c adapter for subdev %s\n",
+				board_info->type);
+			break;
+		}
+
+		/* In G-Min, the sensor devices will already be probed
+		 * (via ACPI) and registered, do not create new
+		 * ones */
+		subdev = atomisp_gmin_find_subdev(adapter, board_info);
+		if (!subdev) {
+			dev_warn(isp->dev, "Subdev %s not found\n",
+				 board_info->type);
+			continue;
+		}
+		ret = v4l2_device_register_subdev(&isp->v4l2_dev, subdev);
+		if (ret) {
+			dev_warn(isp->dev, "Subdev %s detection fail\n",
+				 board_info->type);
+			continue;
+		}
+
+		if (!subdev) {
+			dev_warn(isp->dev, "Subdev %s detection fail\n",
+				 board_info->type);
+			continue;
+		}
+
+		dev_info(isp->dev, "Subdev %s successfully register\n",
+			 board_info->type);
+
+		switch (subdevs->type) {
+		case RAW_CAMERA:
+			dev_dbg(isp->dev, "raw_index: %d\n", raw_index);
+			raw_index = isp->input_cnt;
+			fallthrough;
+		case SOC_CAMERA:
+			dev_dbg(isp->dev, "SOC_INDEX: %d\n", isp->input_cnt);
+			if (isp->input_cnt >= ATOM_ISP_MAX_INPUTS) {
+				dev_warn(isp->dev,
+					 "too many atomisp inputs, ignored\n");
+				break;
+			}
+
+			isp->inputs[isp->input_cnt].type = subdevs->type;
+			isp->inputs[isp->input_cnt].port = subdevs->port;
+			isp->inputs[isp->input_cnt].camera = subdev;
+			isp->inputs[isp->input_cnt].sensor_index = 0;
+			/*
+			 * initialize the subdev frame size, then next we can
+			 * judge whether frame_size store effective value via
+			 * pixel_format.
+			 */
+			isp->inputs[isp->input_cnt].frame_size.pixel_format = 0;
+			isp->input_cnt++;
+			break;
+		case CAMERA_MOTOR:
+			if (isp->motor) {
+				dev_warn(isp->dev,
+					 "too many atomisp motors, ignored %s\n",
+					 board_info->type);
+				continue;
+			}
+			isp->motor = subdev;
+			break;
+		case LED_FLASH:
+		case XENON_FLASH:
+			if (isp->flash) {
+				dev_warn(isp->dev,
+					 "too many atomisp flash devices, ignored %s\n",
+					 board_info->type);
+				continue;
+			}
+			isp->flash = subdev;
+			break;
+		default:
+			dev_dbg(isp->dev, "unknown subdev probed\n");
+			break;
+		}
+	}
+
+	/*
+	 * HACK: Currently VCM belongs to primary sensor only, but correct
+	 * approach must be to acquire from platform code which sensor
+	 * owns it.
+	 */
+	if (isp->motor && raw_index >= 0)
+		isp->inputs[raw_index].motor = isp->motor;
+
+	/* Proceed even if no modules detected. For COS mode and no modules. */
+	if (!isp->input_cnt)
+		dev_warn(isp->dev, "no camera attached or fail to detect\n");
+	else
+		dev_info(isp->dev, "detected %d camera sensors\n",
+			 isp->input_cnt);
+
+	return atomisp_csi_lane_config(isp);
+}
+
+static void atomisp_unregister_entities(struct atomisp_device *isp)
+{
+	unsigned int i;
+	struct v4l2_subdev *sd, *next;
+
+	for (i = 0; i < isp->num_of_streams; i++)
+		atomisp_subdev_unregister_entities(&isp->asd[i]);
+	atomisp_tpg_unregister_entities(&isp->tpg);
+	for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++)
+		atomisp_mipi_csi2_unregister_entities(&isp->csi2_port[i]);
+
+	list_for_each_entry_safe(sd, next, &isp->v4l2_dev.subdevs, list)
+		v4l2_device_unregister_subdev(sd);
+
+	v4l2_device_unregister(&isp->v4l2_dev);
+	media_device_unregister(&isp->media_dev);
+	media_device_cleanup(&isp->media_dev);
+}
+
+static int atomisp_register_entities(struct atomisp_device *isp)
+{
+	int ret = 0;
+	unsigned int i;
+
+	isp->media_dev.dev = isp->dev;
+
+	strscpy(isp->media_dev.model, "Intel Atom ISP",
+		sizeof(isp->media_dev.model));
+
+	media_device_init(&isp->media_dev);
+	isp->v4l2_dev.mdev = &isp->media_dev;
+	ret = v4l2_device_register(isp->dev, &isp->v4l2_dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "%s: V4L2 device registration failed (%d)\n",
+			__func__, ret);
+		goto v4l2_device_failed;
+	}
+
+	ret = atomisp_subdev_probe(isp);
+	if (ret < 0)
+		goto csi_and_subdev_probe_failed;
+
+	/* Register internal entities */
+	for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++) {
+		ret = atomisp_mipi_csi2_register_entities(&isp->csi2_port[i],
+			&isp->v4l2_dev);
+		if (ret == 0)
+			continue;
+
+		/* error case */
+		dev_err(isp->dev, "failed to register the CSI port: %d\n", i);
+		/* deregister all registered CSI ports */
+		while (i--)
+			atomisp_mipi_csi2_unregister_entities(
+			    &isp->csi2_port[i]);
+
+		goto csi_and_subdev_probe_failed;
+	}
+
+	ret = atomisp_tpg_register_entities(&isp->tpg, &isp->v4l2_dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "atomisp_tpg_register_entities\n");
+		goto tpg_register_failed;
+	}
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		ret = atomisp_subdev_register_subdev(asd, &isp->v4l2_dev);
+		if (ret < 0) {
+			dev_err(isp->dev, "atomisp_subdev_register_subdev fail\n");
+			for (; i > 0; i--)
+				atomisp_subdev_unregister_entities(
+				    &isp->asd[i - 1]);
+			goto subdev_register_failed;
+		}
+	}
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		init_completion(&asd->init_done);
+
+		asd->delayed_init_workq =
+		    alloc_workqueue(isp->v4l2_dev.name, WQ_CPU_INTENSIVE,
+				    1);
+		if (!asd->delayed_init_workq) {
+			dev_err(isp->dev,
+				"Failed to initialize delayed init workq\n");
+			ret = -ENOMEM;
+
+			for (; i > 0; i--)
+				destroy_workqueue(isp->asd[i - 1].
+						  delayed_init_workq);
+			goto wq_alloc_failed;
+		}
+		INIT_WORK(&asd->delayed_init_work, atomisp_delayed_init_work);
+	}
+
+	for (i = 0; i < isp->input_cnt; i++) {
+		if (isp->inputs[i].port >= ATOMISP_CAMERA_NR_PORTS) {
+			dev_err(isp->dev, "isp->inputs port %d not supported\n",
+				isp->inputs[i].port);
+			ret = -EINVAL;
+			goto link_failed;
+		}
+	}
+
+	if (isp->input_cnt < ATOM_ISP_MAX_INPUTS) {
+		dev_dbg(isp->dev,
+			"TPG detected, camera_cnt: %d\n", isp->input_cnt);
+		isp->inputs[isp->input_cnt].type = TEST_PATTERN;
+		isp->inputs[isp->input_cnt].port = -1;
+		isp->inputs[isp->input_cnt++].camera = &isp->tpg.sd;
+	} else {
+		dev_warn(isp->dev, "too many atomisp inputs, TPG ignored.\n");
+	}
+
+	return 0;
+
+link_failed:
+	for (i = 0; i < isp->num_of_streams; i++)
+		destroy_workqueue(isp->asd[i].
+				  delayed_init_workq);
+wq_alloc_failed:
+	for (i = 0; i < isp->num_of_streams; i++)
+		atomisp_subdev_unregister_entities(
+		    &isp->asd[i]);
+subdev_register_failed:
+	atomisp_tpg_unregister_entities(&isp->tpg);
+tpg_register_failed:
+	for (i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++)
+		atomisp_mipi_csi2_unregister_entities(&isp->csi2_port[i]);
+csi_and_subdev_probe_failed:
+	v4l2_device_unregister(&isp->v4l2_dev);
+v4l2_device_failed:
+	media_device_unregister(&isp->media_dev);
+	media_device_cleanup(&isp->media_dev);
+	return ret;
+}
+
+static int atomisp_register_device_nodes(struct atomisp_device *isp)
+{
+	int i, err;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		err = atomisp_subdev_register_video_nodes(&isp->asd[i], &isp->v4l2_dev);
+		if (err)
+			return err;
+	}
+
+	err = atomisp_create_pads_links(isp);
+	if (err)
+		return err;
+
+	err = v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+	if (err)
+		return err;
+
+	return media_device_register(&isp->media_dev);
+}
+
+static int atomisp_initialize_modules(struct atomisp_device *isp)
+{
+	int ret;
+
+	ret = atomisp_mipi_csi2_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "mipi csi2 initialization failed\n");
+		goto error_mipi_csi2;
+	}
+
+	ret = atomisp_tpg_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "tpg initialization failed\n");
+		goto error_tpg;
+	}
+
+	ret = atomisp_subdev_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "ISP subdev initialization failed\n");
+		goto error_isp_subdev;
+	}
+
+	return 0;
+
+error_isp_subdev:
+error_tpg:
+	atomisp_tpg_cleanup(isp);
+error_mipi_csi2:
+	atomisp_mipi_csi2_cleanup(isp);
+	return ret;
+}
+
+static void atomisp_uninitialize_modules(struct atomisp_device *isp)
+{
+	atomisp_tpg_cleanup(isp);
+	atomisp_mipi_csi2_cleanup(isp);
+}
+
+const struct firmware *
+atomisp_load_firmware(struct atomisp_device *isp)
+{
+	const struct firmware *fw;
+	int rc;
+	char *fw_path = NULL;
+
+	if (skip_fwload)
+		return NULL;
+
+	if (firmware_name[0] != '\0') {
+		fw_path = firmware_name;
+	} else {
+		if ((isp->media_dev.hw_revision  >> ATOMISP_HW_REVISION_SHIFT)
+		    == ATOMISP_HW_REVISION_ISP2401)
+			fw_path = "shisp_2401a0_v21.bin";
+
+		if (isp->media_dev.hw_revision ==
+		    ((ATOMISP_HW_REVISION_ISP2401_LEGACY << ATOMISP_HW_REVISION_SHIFT)
+		    | ATOMISP_HW_STEPPING_A0))
+			fw_path = "shisp_2401a0_legacy_v21.bin";
+
+		if (isp->media_dev.hw_revision ==
+		    ((ATOMISP_HW_REVISION_ISP2400 << ATOMISP_HW_REVISION_SHIFT)
+		    | ATOMISP_HW_STEPPING_B0))
+			fw_path = "shisp_2400b0_v21.bin";
+	}
+
+	if (!fw_path) {
+		dev_err(isp->dev, "Unsupported hw_revision 0x%x\n",
+			isp->media_dev.hw_revision);
+		return NULL;
+	}
+
+	rc = request_firmware(&fw, fw_path, isp->dev);
+	if (rc) {
+		dev_err(isp->dev,
+			"atomisp: Error %d while requesting firmware %s\n",
+			rc, fw_path);
+		return NULL;
+	}
+
+	return fw;
+}
+
+/*
+ * Check for flags the driver was compiled with against the PCI
+ * device. Always returns true on other than ISP 2400.
+ */
+static bool is_valid_device(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	const char *name;
+	const char *product;
+
+	product = dmi_get_system_info(DMI_PRODUCT_NAME);
+
+	switch (id->device & ATOMISP_PCI_DEVICE_SOC_MASK) {
+	case ATOMISP_PCI_DEVICE_SOC_MRFLD:
+		name = "Merrifield";
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_BYT:
+		name = "Baytrail";
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_ANN:
+		name = "Anniedale";
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_CHT:
+		name = "Cherrytrail";
+		break;
+	default:
+		dev_err(&pdev->dev, "%s: unknown device ID %x04:%x04\n",
+			product, id->vendor, id->device);
+		return false;
+	}
+
+	if (pdev->revision <= ATOMISP_PCI_REV_BYT_A0_MAX) {
+		dev_err(&pdev->dev, "%s revision %d is not unsupported\n",
+			name, pdev->revision);
+		return false;
+	}
+
+	/*
+	 * FIXME:
+	 * remove the if once the driver become generic
+	 */
+
+#ifndef ISP2401
+	if (IS_ISP2401) {
+		dev_err(&pdev->dev, "Support for %s (ISP2401) was disabled at compile time\n",
+			name);
+		return false;
+	}
+#else
+	if (!IS_ISP2401) {
+		dev_err(&pdev->dev, "Support for %s (ISP2400) was disabled at compile time\n",
+			name);
+		return false;
+	}
+#endif
+
+	dev_info(&pdev->dev, "Detected %s version %d (ISP240%c) on %s\n",
+		 name, pdev->revision, IS_ISP2401 ? '1' : '0', product);
+
+	return true;
+}
+
+#define ATOM_ISP_PCI_BAR	0
+
+static int atomisp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	const struct atomisp_platform_data *pdata;
+	struct atomisp_device *isp;
+	unsigned int start;
+	int err, val;
+	u32 irq;
+
+	if (!is_valid_device(pdev, id))
+		return -ENODEV;
+
+	/* Pointer to struct device. */
+	atomisp_dev = &pdev->dev;
+
+	pdata = atomisp_get_platform_data();
+	if (!pdata)
+		dev_warn(&pdev->dev, "no platform data available\n");
+
+	err = pcim_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to enable CI ISP device (%d)\n", err);
+		return err;
+	}
+
+	start = pci_resource_start(pdev, ATOM_ISP_PCI_BAR);
+	dev_dbg(&pdev->dev, "start: 0x%x\n", start);
+
+	err = pcim_iomap_regions(pdev, BIT(ATOM_ISP_PCI_BAR), pci_name(pdev));
+	if (err) {
+		dev_err(&pdev->dev, "Failed to I/O memory remapping (%d)\n", err);
+		goto ioremap_fail;
+	}
+
+	isp = devm_kzalloc(&pdev->dev, sizeof(*isp), GFP_KERNEL);
+	if (!isp) {
+		err = -ENOMEM;
+		goto atomisp_dev_alloc_fail;
+	}
+
+	isp->dev = &pdev->dev;
+	isp->base = pcim_iomap_table(pdev)[ATOM_ISP_PCI_BAR];
+	isp->sw_contex.power_state = ATOM_ISP_POWER_UP;
+	isp->saved_regs.ispmmadr = start;
+
+	dev_dbg(&pdev->dev, "atomisp mmio base: %p\n", isp->base);
+
+	mutex_init(&isp->mutex);
+	spin_lock_init(&isp->lock);
+
+	/* This is not a true PCI device on SoC, so the delay is not needed. */
+	pdev->d3hot_delay = 0;
+
+	pci_set_drvdata(pdev, isp);
+
+	switch (id->device & ATOMISP_PCI_DEVICE_SOC_MASK) {
+	case ATOMISP_PCI_DEVICE_SOC_MRFLD:
+		isp->media_dev.hw_revision =
+		    (ATOMISP_HW_REVISION_ISP2400
+		     << ATOMISP_HW_REVISION_SHIFT) |
+		    ATOMISP_HW_STEPPING_B0;
+
+		switch (id->device) {
+		case ATOMISP_PCI_DEVICE_SOC_MRFLD_1179:
+			isp->dfs = &dfs_config_merr_1179;
+			break;
+		case ATOMISP_PCI_DEVICE_SOC_MRFLD_117A:
+			isp->dfs = &dfs_config_merr_117a;
+
+			break;
+		default:
+			isp->dfs = &dfs_config_merr;
+			break;
+		}
+		isp->hpll_freq = HPLL_FREQ_1600MHZ;
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_BYT:
+		isp->media_dev.hw_revision =
+		    (ATOMISP_HW_REVISION_ISP2400
+		     << ATOMISP_HW_REVISION_SHIFT) |
+		    ATOMISP_HW_STEPPING_B0;
+
+		/*
+		 * Note: some Intel-based tablets with Android use a different
+		 * DFS table. Based on the comments at the Yocto Aero meta
+		 * version of this driver (at the ssid.h header), they're
+		 * identified via a "spid" var:
+		 *
+		 *	androidboot.spid=vend:cust:manu:plat:prod:hard
+		 *
+		 * As we don't have this upstream, nor we know enough details
+		 * to use a DMI or PCI match table, the old code was just
+		 * removed, but let's keep a note here as a reminder that,
+		 * for certain devices, we may need to limit the max DFS
+		 * frequency to be below certain values, adjusting the
+		 * resolution accordingly.
+		 */
+		isp->dfs = &dfs_config_byt;
+
+		/*
+		 * HPLL frequency is known to be device-specific, but we don't
+		 * have specs yet for exactly how it varies.  Default to
+		 * BYT-CR but let provisioning set it via EFI variable
+		 */
+		isp->hpll_freq = gmin_get_var_int(&pdev->dev, false, "HpllFreq", HPLL_FREQ_2000MHZ);
+
+		/*
+		 * for BYT/CHT we are put isp into D3cold to avoid pci registers access
+		 * in power off. Set d3cold_delay to 0 since default 100ms is not
+		 * necessary.
+		 */
+		pdev->d3cold_delay = 0;
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_ANN:
+		isp->media_dev.hw_revision = (	 ATOMISP_HW_REVISION_ISP2401
+						 << ATOMISP_HW_REVISION_SHIFT);
+		isp->media_dev.hw_revision |= pdev->revision < 2 ?
+					      ATOMISP_HW_STEPPING_A0 : ATOMISP_HW_STEPPING_B0;
+		isp->dfs = &dfs_config_merr;
+		isp->hpll_freq = HPLL_FREQ_1600MHZ;
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_CHT:
+		isp->media_dev.hw_revision = (	 ATOMISP_HW_REVISION_ISP2401
+						 << ATOMISP_HW_REVISION_SHIFT);
+		isp->media_dev.hw_revision |= pdev->revision < 2 ?
+					      ATOMISP_HW_STEPPING_A0 : ATOMISP_HW_STEPPING_B0;
+
+		isp->dfs = &dfs_config_cht;
+		pdev->d3cold_delay = 0;
+
+		iosf_mbi_read(BT_MBI_UNIT_CCK, MBI_REG_READ, CCK_FUSE_REG_0, &val);
+		switch (val & CCK_FUSE_HPLL_FREQ_MASK) {
+		case 0x00:
+			isp->hpll_freq = HPLL_FREQ_800MHZ;
+			break;
+		case 0x01:
+			isp->hpll_freq = HPLL_FREQ_1600MHZ;
+			break;
+		case 0x02:
+			isp->hpll_freq = HPLL_FREQ_2000MHZ;
+			break;
+		default:
+			isp->hpll_freq = HPLL_FREQ_1600MHZ;
+			dev_warn(&pdev->dev, "read HPLL from cck failed. Default to 1600 MHz.\n");
+		}
+		break;
+	default:
+		dev_err(&pdev->dev, "un-supported IUNIT device\n");
+		err = -ENODEV;
+		goto atomisp_dev_alloc_fail;
+	}
+
+	dev_info(&pdev->dev, "ISP HPLL frequency base = %d MHz\n", isp->hpll_freq);
+
+	isp->max_isr_latency = ATOMISP_MAX_ISR_LATENCY;
+
+	/* Load isp firmware from user space */
+	if (!defer_fw_load) {
+		isp->firmware = atomisp_load_firmware(isp);
+		if (!isp->firmware) {
+			err = -ENOENT;
+			dev_dbg(&pdev->dev, "Firmware load failed\n");
+			goto load_fw_fail;
+		}
+
+		err = sh_css_check_firmware_version(isp->dev, isp->firmware->data);
+		if (err) {
+			dev_dbg(&pdev->dev, "Firmware version check failed\n");
+			goto fw_validation_fail;
+		}
+	} else {
+		dev_info(&pdev->dev, "Firmware load will be deferred\n");
+	}
+
+	pci_set_master(pdev);
+
+	err = pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSI);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Failed to enable msi (%d)\n", err);
+		goto enable_msi_fail;
+	}
+
+	atomisp_msi_irq_init(isp);
+
+	cpu_latency_qos_add_request(&isp->pm_qos, PM_QOS_DEFAULT_VALUE);
+
+	/*
+	 * for MRFLD, Software/firmware needs to write a 1 to bit 0 of
+	 * the register at CSI_RECEIVER_SELECTION_REG to enable SH CSI
+	 * backend write 0 will enable Arasan CSI backend, which has
+	 * bugs(like sighting:4567697 and 4567699) and will be removed
+	 * in B0
+	 */
+	atomisp_css2_hw_store_32(MRFLD_CSI_RECEIVER_SELECTION_REG, 1);
+
+	if ((id->device & ATOMISP_PCI_DEVICE_SOC_MASK) ==
+	    ATOMISP_PCI_DEVICE_SOC_MRFLD) {
+		u32 csi_afe_trim;
+
+		/*
+		 * Workaround for imbalance data eye issue which is observed
+		 * on TNG B0.
+		 */
+		pci_read_config_dword(pdev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL, &csi_afe_trim);
+		csi_afe_trim &= ~((MRFLD_PCI_CSI_HSRXCLKTRIM_MASK <<
+				   MRFLD_PCI_CSI1_HSRXCLKTRIM_SHIFT) |
+				  (MRFLD_PCI_CSI_HSRXCLKTRIM_MASK <<
+				   MRFLD_PCI_CSI2_HSRXCLKTRIM_SHIFT) |
+				  (MRFLD_PCI_CSI_HSRXCLKTRIM_MASK <<
+				   MRFLD_PCI_CSI3_HSRXCLKTRIM_SHIFT));
+		csi_afe_trim |= (MRFLD_PCI_CSI1_HSRXCLKTRIM <<
+				 MRFLD_PCI_CSI1_HSRXCLKTRIM_SHIFT) |
+				(MRFLD_PCI_CSI2_HSRXCLKTRIM <<
+				 MRFLD_PCI_CSI2_HSRXCLKTRIM_SHIFT) |
+				(MRFLD_PCI_CSI3_HSRXCLKTRIM <<
+				 MRFLD_PCI_CSI3_HSRXCLKTRIM_SHIFT);
+		pci_write_config_dword(pdev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL, csi_afe_trim);
+	}
+
+	err = atomisp_initialize_modules(isp);
+	if (err < 0) {
+		dev_err(&pdev->dev, "atomisp_initialize_modules (%d)\n", err);
+		goto initialize_modules_fail;
+	}
+
+	err = atomisp_register_entities(isp);
+	if (err < 0) {
+		dev_err(&pdev->dev, "atomisp_register_entities failed (%d)\n", err);
+		goto register_entities_fail;
+	}
+
+	INIT_WORK(&isp->assert_recovery_work, atomisp_assert_recovery_work);
+
+	/* save the iunit context only once after all the values are init'ed. */
+	atomisp_save_iunit_reg(isp);
+
+	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_allow(&pdev->dev);
+
+	/* Init ISP memory management */
+	hmm_init();
+
+	err = devm_request_threaded_irq(&pdev->dev, pdev->irq,
+					atomisp_isr, atomisp_isr_thread,
+					IRQF_SHARED, "isp_irq", isp);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to request irq (%d)\n", err);
+		goto request_irq_fail;
+	}
+
+	/* Load firmware into ISP memory */
+	if (!defer_fw_load) {
+		err = atomisp_css_load_firmware(isp);
+		if (err) {
+			dev_err(&pdev->dev, "Failed to init css.\n");
+			goto css_init_fail;
+		}
+	} else {
+		dev_dbg(&pdev->dev, "Skip css init.\n");
+	}
+	/* Clear FW image from memory */
+	release_firmware(isp->firmware);
+	isp->firmware = NULL;
+	isp->css_env.isp_css_fw.data = NULL;
+
+	err = atomisp_register_device_nodes(isp);
+	if (err)
+		goto css_init_fail;
+
+	atomisp_drvfs_init(isp);
+
+	return 0;
+
+css_init_fail:
+	devm_free_irq(&pdev->dev, pdev->irq, isp);
+request_irq_fail:
+	hmm_cleanup();
+	pm_runtime_get_noresume(&pdev->dev);
+	atomisp_unregister_entities(isp);
+register_entities_fail:
+	atomisp_uninitialize_modules(isp);
+initialize_modules_fail:
+	cpu_latency_qos_remove_request(&isp->pm_qos);
+	atomisp_msi_irq_uninit(isp);
+	pci_free_irq_vectors(pdev);
+enable_msi_fail:
+fw_validation_fail:
+	release_firmware(isp->firmware);
+load_fw_fail:
+	/*
+	 * Switch off ISP, as keeping it powered on would prevent
+	 * reaching S0ix states.
+	 *
+	 * The following lines have been copied from atomisp suspend path
+	 */
+
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= BIT(INTR_IIR);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, irq);
+
+	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= ~BIT(INTR_IER);
+	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, irq);
+
+	atomisp_msi_irq_uninit(isp);
+
+	atomisp_ospm_dphy_down(isp);
+
+	/* Address later when we worry about the ...field chips */
+	if (IS_ENABLED(CONFIG_PM) && atomisp_mrfld_power_down(isp))
+		dev_err(&pdev->dev, "Failed to switch off ISP\n");
+
+atomisp_dev_alloc_fail:
+	pcim_iounmap_regions(pdev, BIT(ATOM_ISP_PCI_BAR));
+
+ioremap_fail:
+	return err;
+}
+
+static void atomisp_pci_remove(struct pci_dev *pdev)
+{
+	struct atomisp_device *isp = pci_get_drvdata(pdev);
+
+	dev_info(&pdev->dev, "Removing atomisp driver\n");
+
+	atomisp_drvfs_exit();
+
+	ia_css_unload_firmware();
+	hmm_cleanup();
+
+	pm_runtime_forbid(&pdev->dev);
+	pm_runtime_get_noresume(&pdev->dev);
+	cpu_latency_qos_remove_request(&isp->pm_qos);
+
+	atomisp_msi_irq_uninit(isp);
+	atomisp_unregister_entities(isp);
+
+	release_firmware(isp->firmware);
+}
+
+static const struct pci_device_id atomisp_pci_tbl[] = {
+	/* Merrifield */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_MRFLD)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_MRFLD_1179)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_MRFLD_117A)},
+	/* Baytrail */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_BYT)},
+	/* Anniedale (Merrifield+ / Moorefield) */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_ANN)},
+	/* Cherrytrail */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, ATOMISP_PCI_DEVICE_SOC_CHT)},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, atomisp_pci_tbl);
+
+static const struct dev_pm_ops atomisp_pm_ops = {
+	.runtime_suspend = atomisp_runtime_suspend,
+	.runtime_resume = atomisp_runtime_resume,
+	.suspend = atomisp_suspend,
+	.resume = atomisp_resume,
+};
+
+static struct pci_driver atomisp_pci_driver = {
+	.driver = {
+		.pm = &atomisp_pm_ops,
+	},
+	.name = "atomisp-isp2",
+	.id_table = atomisp_pci_tbl,
+	.probe = atomisp_pci_probe,
+	.remove = atomisp_pci_remove,
+};
+
+module_pci_driver(atomisp_pci_driver);
+
+MODULE_AUTHOR("Wen Wang <wen.w.wang@intel.com>");
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Intel ATOM Platform ISP Driver");
diff --git a/drivers/staging/media/atomisp/pci/atomisp_v4l2.h b/drivers/staging/media/atomisp/pci/atomisp_v4l2.h
new file mode 100644
index 000000000..ccf1c0ac1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp_v4l2.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+
+#ifndef __ATOMISP_V4L2_H__
+#define __ATOMISP_V4L2_H__
+
+struct atomisp_video_pipe;
+struct v4l2_device;
+struct atomisp_device;
+struct firmware;
+
+int atomisp_video_init(struct atomisp_video_pipe *video, const char *name,
+		       unsigned int run_mode);
+void atomisp_video_unregister(struct atomisp_video_pipe *video);
+const struct firmware *atomisp_load_firmware(struct atomisp_device *isp);
+int atomisp_csi_lane_config(struct atomisp_device *isp);
+
+#endif /* __ATOMISP_V4L2_H__ */
diff --git a/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf.h b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf.h
new file mode 100644
index 000000000..0579deac5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf.h
@@ -0,0 +1,377 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_CIRCBUF_H
+#define _IA_CSS_CIRCBUF_H
+
+#include <sp.h>
+#include <type_support.h>
+#include <math_support.h>
+#include <assert_support.h>
+#include <platform_support.h>
+#include "ia_css_circbuf_comm.h"
+#include "ia_css_circbuf_desc.h"
+
+/****************************************************************
+ *
+ * Data structures.
+ *
+ ****************************************************************/
+/**
+ * @brief Data structure for the circular buffer.
+ */
+typedef struct ia_css_circbuf_s ia_css_circbuf_t;
+struct ia_css_circbuf_s {
+	ia_css_circbuf_desc_t *desc;    /* Pointer to the descriptor of the circbuf */
+	ia_css_circbuf_elem_t *elems;	/* an array of elements    */
+};
+
+/**
+ * @brief Create the circular buffer.
+ *
+ * @param cb	The pointer to the circular buffer.
+ * @param elems	An array of elements.
+ * @param desc	The descriptor set to the size using ia_css_circbuf_desc_init().
+ */
+void ia_css_circbuf_create(
+    ia_css_circbuf_t *cb,
+    ia_css_circbuf_elem_t *elems,
+    ia_css_circbuf_desc_t *desc);
+
+/**
+ * @brief Destroy the circular buffer.
+ *
+ * @param cb The pointer to the circular buffer.
+ */
+void ia_css_circbuf_destroy(
+    ia_css_circbuf_t *cb);
+
+/**
+ * @brief Pop a value out of the circular buffer.
+ * Get a value at the head of the circular buffer.
+ * The user should call "ia_css_circbuf_is_empty()"
+ * to avoid accessing to an empty buffer.
+ *
+ * @param cb	The pointer to the circular buffer.
+ *
+ * @return the pop-out value.
+ */
+uint32_t ia_css_circbuf_pop(
+    ia_css_circbuf_t *cb);
+
+/**
+ * @brief Extract a value out of the circular buffer.
+ * Get a value at an arbitrary poistion in the circular
+ * buffer. The user should call "ia_css_circbuf_is_empty()"
+ * to avoid accessing to an empty buffer.
+ *
+ * @param cb	 The pointer to the circular buffer.
+ * @param offset The offset from "start" to the target position.
+ *
+ * @return the extracted value.
+ */
+uint32_t ia_css_circbuf_extract(
+    ia_css_circbuf_t *cb,
+    int offset);
+
+/****************************************************************
+ *
+ * Inline functions.
+ *
+ ****************************************************************/
+/**
+ * @brief Set the "val" field in the element.
+ *
+ * @param elem The pointer to the element.
+ * @param val  The value to be set.
+ */
+static inline void ia_css_circbuf_elem_set_val(
+    ia_css_circbuf_elem_t *elem,
+    uint32_t val)
+{
+	OP___assert(elem);
+
+	elem->val = val;
+}
+
+/**
+ * @brief Initialize the element.
+ *
+ * @param elem The pointer to the element.
+ */
+static inline void ia_css_circbuf_elem_init(
+    ia_css_circbuf_elem_t *elem)
+{
+	OP___assert(elem);
+	ia_css_circbuf_elem_set_val(elem, 0);
+}
+
+/**
+ * @brief Copy an element.
+ *
+ * @param src  The element as the copy source.
+ * @param dest The element as the copy destination.
+ */
+static inline void ia_css_circbuf_elem_cpy(
+    ia_css_circbuf_elem_t *src,
+    ia_css_circbuf_elem_t *dest)
+{
+	OP___assert(src);
+	OP___assert(dest);
+
+	ia_css_circbuf_elem_set_val(dest, src->val);
+}
+
+/**
+ * @brief Get position in the circular buffer.
+ *
+ * @param cb		The pointer to the circular buffer.
+ * @param base		The base position.
+ * @param offset	The offset.
+ *
+ * @return the position at offset.
+ */
+static inline uint8_t ia_css_circbuf_get_pos_at_offset(
+    ia_css_circbuf_t *cb,
+    u32 base,
+    int offset)
+{
+	u8 dest;
+
+	OP___assert(cb);
+	OP___assert(cb->desc);
+	OP___assert(cb->desc->size > 0);
+
+	/* step 1: adjudst the offset  */
+	while (offset < 0) {
+		offset += cb->desc->size;
+	}
+
+	/* step 2: shift and round by the upper limit */
+	dest = OP_std_modadd(base, offset, cb->desc->size);
+
+	return dest;
+}
+
+/**
+ * @brief Get the offset between two positions in the circular buffer.
+ * Get the offset from the source position to the terminal position,
+ * along the direction in which the new elements come in.
+ *
+ * @param cb		The pointer to the circular buffer.
+ * @param src_pos	The source position.
+ * @param dest_pos	The terminal position.
+ *
+ * @return the offset.
+ */
+static inline int ia_css_circbuf_get_offset(
+    ia_css_circbuf_t *cb,
+    u32 src_pos,
+    uint32_t dest_pos)
+{
+	int offset;
+
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	offset = (int)(dest_pos - src_pos);
+	offset += (offset < 0) ? cb->desc->size : 0;
+
+	return offset;
+}
+
+/**
+ * @brief Get the maximum number of elements.
+ *
+ * @param cb The pointer to the circular buffer.
+ *
+ * @return the maximum number of elements.
+ *
+ * TODO: Test this API.
+ */
+static inline uint32_t ia_css_circbuf_get_size(
+    ia_css_circbuf_t *cb)
+{
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	return cb->desc->size;
+}
+
+/**
+ * @brief Get the number of available elements.
+ *
+ * @param cb The pointer to the circular buffer.
+ *
+ * @return the number of available elements.
+ */
+static inline uint32_t ia_css_circbuf_get_num_elems(
+    ia_css_circbuf_t *cb)
+{
+	int num;
+
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	num = ia_css_circbuf_get_offset(cb, cb->desc->start, cb->desc->end);
+
+	return (uint32_t)num;
+}
+
+/**
+ * @brief Test if the circular buffer is empty.
+ *
+ * @param cb	The pointer to the circular buffer.
+ *
+ * @return
+ *	- true when it is empty.
+ *	- false when it is not empty.
+ */
+static inline bool ia_css_circbuf_is_empty(
+    ia_css_circbuf_t *cb)
+{
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	return ia_css_circbuf_desc_is_empty(cb->desc);
+}
+
+/**
+ * @brief Test if the circular buffer is full.
+ *
+ * @param cb	The pointer to the circular buffer.
+ *
+ * @return
+ *	- true when it is full.
+ *	- false when it is not full.
+ */
+static inline bool ia_css_circbuf_is_full(ia_css_circbuf_t *cb)
+{
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	return ia_css_circbuf_desc_is_full(cb->desc);
+}
+
+/**
+ * @brief Write a new element into the circular buffer.
+ * Write a new element WITHOUT checking whether the
+ * circular buffer is full or not. So it also overwrites
+ * the oldest element when the buffer is full.
+ *
+ * @param cb	The pointer to the circular buffer.
+ * @param elem	The new element.
+ */
+static inline void ia_css_circbuf_write(
+    ia_css_circbuf_t *cb,
+    ia_css_circbuf_elem_t elem)
+{
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	/* Cannot continue as the queue is full*/
+	assert(!ia_css_circbuf_is_full(cb));
+
+	ia_css_circbuf_elem_cpy(&elem, &cb->elems[cb->desc->end]);
+
+	cb->desc->end = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->end, 1);
+}
+
+/**
+ * @brief Push a value in the circular buffer.
+ * Put a new value at the tail of the circular buffer.
+ * The user should call "ia_css_circbuf_is_full()"
+ * to avoid accessing to a full buffer.
+ *
+ * @param cb	The pointer to the circular buffer.
+ * @param val	The value to be pushed in.
+ */
+static inline void ia_css_circbuf_push(
+    ia_css_circbuf_t *cb,
+    uint32_t val)
+{
+	ia_css_circbuf_elem_t elem;
+
+	OP___assert(cb);
+
+	/* set up an element */
+	ia_css_circbuf_elem_init(&elem);
+	ia_css_circbuf_elem_set_val(&elem, val);
+
+	/* write the element into the buffer */
+	ia_css_circbuf_write(cb, elem);
+}
+
+/**
+ * @brief Get the number of free elements.
+ *
+ * @param cb The pointer to the circular buffer.
+ *
+ * @return: The number of free elements.
+ */
+static inline uint32_t ia_css_circbuf_get_free_elems(
+    ia_css_circbuf_t *cb)
+{
+	OP___assert(cb);
+	OP___assert(cb->desc);
+
+	return ia_css_circbuf_desc_get_free_elems(cb->desc);
+}
+
+/**
+ * @brief Peek an element in Circular Buffer.
+ *
+ * @param cb	 The pointer to the circular buffer.
+ * @param offset Offset to the element.
+ *
+ * @return the elements value.
+ */
+uint32_t ia_css_circbuf_peek(
+    ia_css_circbuf_t *cb,
+    int offset);
+
+/**
+ * @brief Get an element in Circular Buffer.
+ *
+ * @param cb	 The pointer to the circular buffer.
+ * @param offset Offset to the element.
+ *
+ * @return the elements value.
+ */
+uint32_t ia_css_circbuf_peek_from_start(
+    ia_css_circbuf_t *cb,
+    int offset);
+
+/**
+ * @brief Increase Size of a Circular Buffer.
+ * Use 'CAUTION' before using this function, This was added to
+ * support / fix issue with increasing size for tagger only
+ *
+ * @param cb The pointer to the circular buffer.
+ * @param sz_delta delta increase for new size
+ * @param elems (optional) pointers to new additional elements
+ *		cb element array size will not be increased dynamically,
+ *		but new elements should be added at the end to existing
+ *		cb element array which if of max_size >= new size
+ *
+ * @return	true on successfully increasing the size
+ *			false on failure
+ */
+bool ia_css_circbuf_increase_size(
+    ia_css_circbuf_t *cb,
+    unsigned int sz_delta,
+    ia_css_circbuf_elem_t *elems);
+
+#endif /*_IA_CSS_CIRCBUF_H */
diff --git a/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_comm.h b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_comm.h
new file mode 100644
index 000000000..6fa6da859
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_comm.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_CIRCBUF_COMM_H
+#define _IA_CSS_CIRCBUF_COMM_H
+
+#include <type_support.h>  /* uint8_t, uint32_t */
+
+#define IA_CSS_CIRCBUF_PADDING 1 /* The circular buffer is implemented in lock-less manner, wherein
+				   * the head and tail can advance independently without any locks.
+				   * But to achieve this, an extra buffer element is required to detect
+				   * queue full & empty conditions, wherein the tail trails the head for
+				   * full and is equal to head for empty condition. This causes 1 buffer
+				   * not being available for use.
+				   */
+
+/****************************************************************
+ *
+ * Portable Data structures
+ *
+ ****************************************************************/
+/**
+ * @brief Data structure for the circular descriptor.
+ */
+typedef struct ia_css_circbuf_desc_s ia_css_circbuf_desc_t;
+struct ia_css_circbuf_desc_s {
+	u8 size;	/* the maximum number of elements*/
+	u8 step;   /* number of bytes per element */
+	u8 start;	/* index of the oldest element */
+	u8 end;	/* index at which to write the new element */
+};
+
+#define SIZE_OF_IA_CSS_CIRCBUF_DESC_S_STRUCT				\
+	(4 * sizeof(uint8_t))
+
+/**
+ * @brief Data structure for the circular buffer element.
+ */
+typedef struct ia_css_circbuf_elem_s ia_css_circbuf_elem_t;
+struct ia_css_circbuf_elem_s {
+	u32 val;	/* the value stored in the element */
+};
+
+#define SIZE_OF_IA_CSS_CIRCBUF_ELEM_S_STRUCT				\
+	(sizeof(uint32_t))
+
+#endif /*_IA_CSS_CIRCBUF_COMM_H*/
diff --git a/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_desc.h b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_desc.h
new file mode 100644
index 000000000..1071813a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/circbuf/interface/ia_css_circbuf_desc.h
@@ -0,0 +1,174 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_CIRCBUF_DESC_H_
+#define _IA_CSS_CIRCBUF_DESC_H_
+
+#include <type_support.h>
+#include <math_support.h>
+#include <platform_support.h>
+#include <sp.h>
+#include "ia_css_circbuf_comm.h"
+/****************************************************************
+ *
+ * Inline functions.
+ *
+ ****************************************************************/
+/**
+ * @brief Test if the circular buffer is empty.
+ *
+ * @param cb_desc The pointer to the circular buffer descriptor.
+ *
+ * @return
+ *	- true when it is empty.
+ *	- false when it is not empty.
+ */
+static inline bool ia_css_circbuf_desc_is_empty(
+    ia_css_circbuf_desc_t *cb_desc)
+{
+	OP___assert(cb_desc);
+	return (cb_desc->end == cb_desc->start);
+}
+
+/**
+ * @brief Test if the circular buffer descriptor is full.
+ *
+ * @param cb_desc	The pointer to the circular buffer
+ *			descriptor.
+ *
+ * @return
+ *	- true when it is full.
+ *	- false when it is not full.
+ */
+static inline bool ia_css_circbuf_desc_is_full(
+    ia_css_circbuf_desc_t *cb_desc)
+{
+	OP___assert(cb_desc);
+	return (OP_std_modadd(cb_desc->end, 1, cb_desc->size) == cb_desc->start);
+}
+
+/**
+ * @brief Initialize the circular buffer descriptor
+ *
+ * @param cb_desc	The pointer circular buffer descriptor
+ * @param size		The size of the circular buffer
+ */
+static inline void ia_css_circbuf_desc_init(
+    ia_css_circbuf_desc_t *cb_desc,
+    int8_t size)
+{
+	OP___assert(cb_desc);
+	cb_desc->size = size;
+}
+
+/**
+ * @brief Get a position in the circular buffer descriptor.
+ *
+ * @param cb     The pointer to the circular buffer descriptor.
+ * @param base   The base position.
+ * @param offset The offset.
+ *
+ * @return the position in the circular buffer descriptor.
+ */
+static inline uint8_t ia_css_circbuf_desc_get_pos_at_offset(
+    ia_css_circbuf_desc_t *cb_desc,
+    u32 base,
+    int offset)
+{
+	u8 dest;
+
+	OP___assert(cb_desc);
+	OP___assert(cb_desc->size > 0);
+
+	/* step 1: adjust the offset  */
+	while (offset < 0) {
+		offset += cb_desc->size;
+	}
+
+	/* step 2: shift and round by the upper limit */
+	dest = OP_std_modadd(base, offset, cb_desc->size);
+
+	return dest;
+}
+
+/**
+ * @brief Get the offset between two positions in the circular buffer
+ * descriptor.
+ * Get the offset from the source position to the terminal position,
+ * along the direction in which the new elements come in.
+ *
+ * @param cb_desc	The pointer to the circular buffer descriptor.
+ * @param src_pos	The source position.
+ * @param dest_pos	The terminal position.
+ *
+ * @return the offset.
+ */
+static inline int ia_css_circbuf_desc_get_offset(
+    ia_css_circbuf_desc_t *cb_desc,
+    u32 src_pos,
+    uint32_t dest_pos)
+{
+	int offset;
+
+	OP___assert(cb_desc);
+
+	offset = (int)(dest_pos - src_pos);
+	offset += (offset < 0) ? cb_desc->size : 0;
+
+	return offset;
+}
+
+/**
+ * @brief Get the number of available elements.
+ *
+ * @param cb_desc The pointer to the circular buffer.
+ *
+ * @return The number of available elements.
+ */
+static inline uint32_t ia_css_circbuf_desc_get_num_elems(
+    ia_css_circbuf_desc_t *cb_desc)
+{
+	int num;
+
+	OP___assert(cb_desc);
+
+	num = ia_css_circbuf_desc_get_offset(cb_desc,
+					     cb_desc->start,
+					     cb_desc->end);
+
+	return (uint32_t)num;
+}
+
+/**
+ * @brief Get the number of free elements.
+ *
+ * @param cb_desc The pointer to the circular buffer descriptor.
+ *
+ * @return: The number of free elements.
+ */
+static inline uint32_t ia_css_circbuf_desc_get_free_elems(
+    ia_css_circbuf_desc_t *cb_desc)
+{
+	u32 num;
+
+	OP___assert(cb_desc);
+
+	num = ia_css_circbuf_desc_get_offset(cb_desc,
+					     cb_desc->start,
+					     cb_desc->end);
+
+	return (cb_desc->size - num);
+}
+#endif /*_IA_CSS_CIRCBUF_DESC_H_ */
diff --git a/drivers/staging/media/atomisp/pci/base/circbuf/src/circbuf.c b/drivers/staging/media/atomisp/pci/base/circbuf/src/circbuf.c
new file mode 100644
index 000000000..d9f7c1437
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/circbuf/src/circbuf.c
@@ -0,0 +1,321 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_circbuf.h"
+
+#include <assert_support.h>
+
+/**********************************************************************
+ *
+ * Forward declarations.
+ *
+ **********************************************************************/
+/*
+ * @brief Read the oldest element from the circular buffer.
+ * Read the oldest element WITHOUT checking whehter the
+ * circular buffer is empty or not. The oldest element is
+ * also removed out from the circular buffer.
+ *
+ * @param cb The pointer to the circular buffer.
+ *
+ * @return the oldest element.
+ */
+static inline ia_css_circbuf_elem_t
+ia_css_circbuf_read(ia_css_circbuf_t *cb);
+
+/*
+ * @brief Shift a chunk of elements in the circular buffer.
+ * A chunk of elements (i.e. the ones from the "start" position
+ * to the "chunk_src" position) are shifted in the circular buffer,
+ * along the direction of new elements coming.
+ *
+ * @param cb	     The pointer to the circular buffer.
+ * @param chunk_src  The position at which the first element in the chunk is.
+ * @param chunk_dest The position to which the first element in the chunk would be shift.
+ */
+static inline void ia_css_circbuf_shift_chunk(ia_css_circbuf_t *cb,
+	u32 chunk_src,
+	uint32_t chunk_dest);
+
+/*
+ * @brief Get the "val" field in the element.
+ *
+ * @param elem The pointer to the element.
+ *
+ * @return the "val" field.
+ */
+static inline uint32_t
+ia_css_circbuf_elem_get_val(ia_css_circbuf_elem_t *elem);
+
+/**********************************************************************
+ *
+ * Non-inline functions.
+ *
+ **********************************************************************/
+/*
+ * @brief Create the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+void
+ia_css_circbuf_create(ia_css_circbuf_t *cb,
+		      ia_css_circbuf_elem_t *elems,
+		      ia_css_circbuf_desc_t *desc)
+{
+	u32 i;
+
+	OP___assert(desc);
+
+	cb->desc = desc;
+	/* Initialize to defaults */
+	cb->desc->start = 0;
+	cb->desc->end = 0;
+	cb->desc->step = 0;
+
+	for (i = 0; i < cb->desc->size; i++)
+		ia_css_circbuf_elem_init(&elems[i]);
+
+	cb->elems = elems;
+}
+
+/*
+ * @brief Destroy the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+void ia_css_circbuf_destroy(ia_css_circbuf_t *cb)
+{
+	cb->desc = NULL;
+
+	cb->elems = NULL;
+}
+
+/*
+ * @brief Pop a value out of the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+uint32_t ia_css_circbuf_pop(ia_css_circbuf_t *cb)
+{
+	u32 ret;
+	ia_css_circbuf_elem_t elem;
+
+	assert(!ia_css_circbuf_is_empty(cb));
+
+	/* read an element from the buffer */
+	elem = ia_css_circbuf_read(cb);
+	ret = ia_css_circbuf_elem_get_val(&elem);
+	return ret;
+}
+
+/*
+ * @brief Extract a value out of the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+uint32_t ia_css_circbuf_extract(ia_css_circbuf_t *cb, int offset)
+{
+	int max_offset;
+	u32 val;
+	u32 pos;
+	u32 src_pos;
+	u32 dest_pos;
+
+	/* get the maximum offest */
+	max_offset = ia_css_circbuf_get_offset(cb, cb->desc->start, cb->desc->end);
+	max_offset--;
+
+	/*
+	 * Step 1: When the target element is at the "start" position.
+	 */
+	if (offset == 0) {
+		val = ia_css_circbuf_pop(cb);
+		return val;
+	}
+
+	/*
+	 * Step 2: When the target element is out of the range.
+	 */
+	if (offset > max_offset) {
+		val = 0;
+		return val;
+	}
+
+	/*
+	 * Step 3: When the target element is between the "start" and
+	 * "end" position.
+	 */
+	/* get the position of the target element */
+	pos = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->start, offset);
+
+	/* get the value from the target element */
+	val = ia_css_circbuf_elem_get_val(&cb->elems[pos]);
+
+	/* shift the elements */
+	src_pos = ia_css_circbuf_get_pos_at_offset(cb, pos, -1);
+	dest_pos = pos;
+	ia_css_circbuf_shift_chunk(cb, src_pos, dest_pos);
+
+	return val;
+}
+
+/*
+ * @brief Peek an element from the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+uint32_t ia_css_circbuf_peek(ia_css_circbuf_t *cb, int offset)
+{
+	int pos;
+
+	pos = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->end, offset);
+
+	/* get the value at the position */
+	return cb->elems[pos].val;
+}
+
+/*
+ * @brief Get the value of an element from the circular buffer.
+ * Refer to "ia_css_circbuf.h" for details.
+ */
+uint32_t ia_css_circbuf_peek_from_start(ia_css_circbuf_t *cb, int offset)
+{
+	int pos;
+
+	pos = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->start, offset);
+
+	/* get the value at the position */
+	return cb->elems[pos].val;
+}
+
+/* @brief increase size of a circular buffer.
+ * Use 'CAUTION' before using this function. This was added to
+ * support / fix issue with increasing size for tagger only
+ * Please refer to "ia_css_circbuf.h" for details.
+ */
+bool ia_css_circbuf_increase_size(
+    ia_css_circbuf_t *cb,
+    unsigned int sz_delta,
+    ia_css_circbuf_elem_t *elems)
+{
+	u8 curr_size;
+	u8 curr_end;
+	unsigned int i = 0;
+
+	if (!cb || sz_delta == 0)
+		return false;
+
+	curr_size = cb->desc->size;
+	curr_end = cb->desc->end;
+	/* We assume cb was pre defined as global to allow
+	 * increase in size */
+	/* FM: are we sure this cannot cause size to become too big? */
+	if (((uint8_t)(cb->desc->size + (uint8_t)sz_delta) > cb->desc->size) &&
+	    ((uint8_t)sz_delta == sz_delta))
+		cb->desc->size += (uint8_t)sz_delta;
+	else
+		return false; /* overflow in size */
+
+	/* If elems are passed update them else we assume its been taken
+	 * care before calling this function */
+	if (elems) {
+		/* cb element array size will not be increased dynamically,
+		 * but pointers to new elements can be added at the end
+		 * of existing pre defined cb element array of
+		 * size >= new size if not already added */
+		for (i = curr_size; i <  cb->desc->size; i++)
+			cb->elems[i] = elems[i - curr_size];
+	}
+	/* Fix Start / End */
+	if (curr_end < cb->desc->start) {
+		if (curr_end == 0) {
+			/* Easily fix End */
+			cb->desc->end = curr_size;
+		} else {
+			/* Move elements and fix Start*/
+			ia_css_circbuf_shift_chunk(cb,
+						   curr_size - 1,
+						   curr_size + sz_delta - 1);
+		}
+	}
+
+	return true;
+}
+
+/****************************************************************
+ *
+ * Inline functions.
+ *
+ ****************************************************************/
+/*
+ * @brief Get the "val" field in the element.
+ * Refer to "Forward declarations" for details.
+ */
+static inline uint32_t
+ia_css_circbuf_elem_get_val(ia_css_circbuf_elem_t *elem)
+{
+	return elem->val;
+}
+
+/*
+ * @brief Read the oldest element from the circular buffer.
+ * Refer to "Forward declarations" for details.
+ */
+static inline ia_css_circbuf_elem_t
+ia_css_circbuf_read(ia_css_circbuf_t *cb)
+{
+	ia_css_circbuf_elem_t elem;
+
+	/* get the element from the target position */
+	elem = cb->elems[cb->desc->start];
+
+	/* clear the target position */
+	ia_css_circbuf_elem_init(&cb->elems[cb->desc->start]);
+
+	/* adjust the "start" position */
+	cb->desc->start = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->start, 1);
+	return elem;
+}
+
+/*
+ * @brief Shift a chunk of elements in the circular buffer.
+ * Refer to "Forward declarations" for details.
+ */
+static inline void
+ia_css_circbuf_shift_chunk(ia_css_circbuf_t *cb,
+			   u32 chunk_src, uint32_t chunk_dest)
+{
+	int chunk_offset;
+	int chunk_sz;
+	int i;
+
+	/* get the chunk offset and size */
+	chunk_offset = ia_css_circbuf_get_offset(cb,
+		       chunk_src, chunk_dest);
+	chunk_sz = ia_css_circbuf_get_offset(cb, cb->desc->start, chunk_src) + 1;
+
+	/* shift each element to its terminal position */
+	for (i = 0; i < chunk_sz; i++) {
+		/* copy the element from the source to the destination */
+		ia_css_circbuf_elem_cpy(&cb->elems[chunk_src],
+					&cb->elems[chunk_dest]);
+
+		/* clear the source position */
+		ia_css_circbuf_elem_init(&cb->elems[chunk_src]);
+
+		/* adjust the source/terminal positions */
+		chunk_src = ia_css_circbuf_get_pos_at_offset(cb, chunk_src, -1);
+		chunk_dest = ia_css_circbuf_get_pos_at_offset(cb, chunk_dest, -1);
+	}
+
+	/* adjust the index "start" */
+	cb->desc->start = ia_css_circbuf_get_pos_at_offset(cb, cb->desc->start,
+			  chunk_offset);
+}
diff --git a/drivers/staging/media/atomisp/pci/base/refcount/interface/ia_css_refcount.h b/drivers/staging/media/atomisp/pci/base/refcount/interface/ia_css_refcount.h
new file mode 100644
index 000000000..78cf0cbfb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/refcount/interface/ia_css_refcount.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_REFCOUNT_H_
+#define _IA_CSS_REFCOUNT_H_
+
+#include <type_support.h>
+#include <system_local.h>
+#include <ia_css_err.h>
+#include <ia_css_types.h>
+
+typedef void (*clear_func)(ia_css_ptr ptr);
+
+/*! \brief Function for initializing refcount list
+ *
+ * \param[in]	size		Size of the refcount list.
+ * \return				ia_css_err
+ */
+int ia_css_refcount_init(uint32_t size);
+
+/*! \brief Function for de-initializing refcount list
+ *
+ * \return				None
+ */
+void ia_css_refcount_uninit(void);
+
+/*! \brief Function for increasing reference by 1.
+ *
+ * \param[in]	id		ID of the object.
+ * \param[in]	ptr		Data of the object (ptr).
+ * \return				ia_css_ptr (saved address)
+ */
+ia_css_ptr ia_css_refcount_increment(s32 id, ia_css_ptr ptr);
+
+/*! \brief Function for decrease reference by 1.
+ *
+ * \param[in]	id		ID of the object.
+ * \param[in]	ptr		Data of the object (ptr).
+ *
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool ia_css_refcount_decrement(s32 id, ia_css_ptr ptr);
+
+/*! \brief Function to check if reference count is 1.
+ *
+ * \param[in]	ptr		Data of the object (ptr).
+ *
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool ia_css_refcount_is_single(ia_css_ptr ptr);
+
+/*! \brief Function to clear reference list objects.
+ *
+ * \param[in]	id			ID of the object.
+ * \param[in] clear_func	function to be run to free reference objects.
+ *
+ *  return				None
+ */
+void ia_css_refcount_clear(s32 id,
+			   clear_func clear_func_ptr);
+
+/*! \brief Function to verify if object is valid
+ *
+ * \param[in] ptr       Data of the object (ptr)
+ *
+ *      - true, if valid
+ *      - false, if invalid
+ */
+bool ia_css_refcount_is_valid(ia_css_ptr ptr);
+
+#endif /* _IA_CSS_REFCOUNT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/base/refcount/src/refcount.c b/drivers/staging/media/atomisp/pci/base/refcount/src/refcount.c
new file mode 100644
index 000000000..a9c881631
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/base/refcount/src/refcount.c
@@ -0,0 +1,277 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_refcount.h"
+#include "sh_css_defs.h"
+
+#include "platform_support.h"
+
+#include "assert_support.h"
+
+#include "ia_css_debug.h"
+
+/* TODO: enable for other memory aswell
+	 now only for ia_css_ptr */
+struct ia_css_refcount_entry {
+	u32 count;
+	ia_css_ptr data;
+	s32 id;
+};
+
+struct ia_css_refcount_list {
+	u32 size;
+	struct ia_css_refcount_entry *items;
+};
+
+static struct ia_css_refcount_list myrefcount;
+
+static struct ia_css_refcount_entry *refcount_find_entry(ia_css_ptr ptr,
+	bool firstfree)
+{
+	u32 i;
+
+	if (ptr == 0)
+		return NULL;
+	if (!myrefcount.items) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "%s(): Ref count not initialized!\n", __func__);
+		return NULL;
+	}
+
+	for (i = 0; i < myrefcount.size; i++) {
+		if ((&myrefcount.items[i])->data == 0) {
+			if (firstfree) {
+				/* for new entry */
+				return &myrefcount.items[i];
+			}
+		}
+		if ((&myrefcount.items[i])->data == ptr) {
+			/* found entry */
+			return &myrefcount.items[i];
+		}
+	}
+	return NULL;
+}
+
+int ia_css_refcount_init(uint32_t size)
+{
+	int err = 0;
+
+	if (size == 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "%s(): Size of 0 for Ref count init!\n", __func__);
+		return -EINVAL;
+	}
+	if (myrefcount.items) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "%s(): Ref count is already initialized\n", __func__);
+		return -EINVAL;
+	}
+	myrefcount.items =
+	    kvmalloc(sizeof(struct ia_css_refcount_entry) * size, GFP_KERNEL);
+	if (!myrefcount.items)
+		err = -ENOMEM;
+	if (!err) {
+		memset(myrefcount.items, 0,
+		       sizeof(struct ia_css_refcount_entry) * size);
+		myrefcount.size = size;
+	}
+	return err;
+}
+
+void ia_css_refcount_uninit(void)
+{
+	struct ia_css_refcount_entry *entry;
+	u32 i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "%s() entry\n", __func__);
+	for (i = 0; i < myrefcount.size; i++) {
+		/* driver verifier tool has issues with &arr[i]
+		   and prefers arr + i; as these are actually equivalent
+		   the line below uses + i
+		*/
+		entry = myrefcount.items + i;
+		if (entry->data != mmgr_NULL) {
+			/*	ia_css_debug_dtrace(IA_CSS_DBG_TRACE,
+				"ia_css_refcount_uninit: freeing (%x)\n",
+				entry->data);*/
+			hmm_free(entry->data);
+			entry->data = mmgr_NULL;
+			entry->count = 0;
+			entry->id = 0;
+		}
+	}
+	kvfree(myrefcount.items);
+	myrefcount.items = NULL;
+	myrefcount.size = 0;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "%s() leave\n", __func__);
+}
+
+ia_css_ptr ia_css_refcount_increment(s32 id, ia_css_ptr ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	if (ptr == mmgr_NULL)
+		return ptr;
+
+	entry = refcount_find_entry(ptr, false);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "%s(%x) 0x%x\n", __func__, id, ptr);
+
+	if (!entry) {
+		entry = refcount_find_entry(ptr, true);
+		assert(entry);
+		if (!entry)
+			return mmgr_NULL;
+		entry->id = id;
+	}
+
+	if (entry->id != id) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "%s(): Ref count IDS do not match!\n", __func__);
+		return mmgr_NULL;
+	}
+
+	if (entry->data == ptr)
+		entry->count += 1;
+	else if (entry->data == mmgr_NULL) {
+		entry->data = ptr;
+		entry->count = 1;
+	} else
+		return mmgr_NULL;
+
+	return ptr;
+}
+
+bool ia_css_refcount_decrement(s32 id, ia_css_ptr ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "%s(%x) 0x%x\n", __func__, id, ptr);
+
+	if (ptr == mmgr_NULL)
+		return false;
+
+	entry = refcount_find_entry(ptr, false);
+
+	if (entry) {
+		if (entry->id != id) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+					    "%s(): Ref count IDS do not match!\n", __func__);
+			return false;
+		}
+		if (entry->count > 0) {
+			entry->count -= 1;
+			if (entry->count == 0) {
+				/* ia_css_debug_dtrace(IA_CSS_DBEUG_TRACE,
+				   "ia_css_refcount_decrement: freeing\n");*/
+				hmm_free(ptr);
+				entry->data = mmgr_NULL;
+				entry->id = 0;
+			}
+			return true;
+		}
+	}
+
+	/* SHOULD NOT HAPPEN: ptr not managed by refcount, or not
+	   valid anymore */
+	if (entry)
+		IA_CSS_ERROR("id %x, ptr 0x%x entry %p entry->id %x entry->count %d\n",
+			     id, ptr, entry, entry->id, entry->count);
+	else
+		IA_CSS_ERROR("entry NULL\n");
+	assert(false);
+
+	return false;
+}
+
+bool ia_css_refcount_is_single(ia_css_ptr ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	if (ptr == mmgr_NULL)
+		return false;
+
+	entry = refcount_find_entry(ptr, false);
+
+	if (entry)
+		return (entry->count == 1);
+
+	return true;
+}
+
+void ia_css_refcount_clear(s32 id, clear_func clear_func_ptr)
+{
+	struct ia_css_refcount_entry *entry;
+	u32 i;
+	u32 count = 0;
+
+	assert(clear_func_ptr);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s(%x)\n",
+			    __func__, id);
+
+	for (i = 0; i < myrefcount.size; i++) {
+		/* driver verifier tool has issues with &arr[i]
+		   and prefers arr + i; as these are actually equivalent
+		   the line below uses + i
+		*/
+		entry = myrefcount.items + i;
+		if ((entry->data != mmgr_NULL) && (entry->id == id)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "%s: %x: 0x%x\n", __func__,
+					    id, entry->data);
+			if (clear_func_ptr) {
+				/* clear using provided function */
+				clear_func_ptr(entry->data);
+			} else {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "%s: using hmm_free: no clear_func\n", __func__);
+				hmm_free(entry->data);
+			}
+
+			if (entry->count != 0) {
+				IA_CSS_WARNING("Ref count for entry %x is not zero!", entry->id);
+			}
+
+			assert(entry->count == 0);
+
+			entry->data = mmgr_NULL;
+			entry->count = 0;
+			entry->id = 0;
+			count++;
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "%s(%x): cleared %d\n", __func__, id,
+			    count);
+}
+
+bool ia_css_refcount_is_valid(ia_css_ptr ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	if (ptr == mmgr_NULL)
+		return false;
+
+	entry = refcount_find_entry(ptr, false);
+
+	return entry;
+}
diff --git a/drivers/staging/media/atomisp/pci/bits.h b/drivers/staging/media/atomisp/pci/bits.h
new file mode 100644
index 000000000..9fab02ebd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/bits.h
@@ -0,0 +1,105 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _HRT_BITS_H
+#define _HRT_BITS_H
+
+#include "defs.h"
+
+#define _hrt_ones(n) HRTCAT(_hrt_ones_, n)
+#define _hrt_ones_0x0  0x00000000U
+#define _hrt_ones_0x1  0x00000001U
+#define _hrt_ones_0x2  0x00000003U
+#define _hrt_ones_0x3  0x00000007U
+#define _hrt_ones_0x4  0x0000000FU
+#define _hrt_ones_0x5  0x0000001FU
+#define _hrt_ones_0x6  0x0000003FU
+#define _hrt_ones_0x7  0x0000007FU
+#define _hrt_ones_0x8  0x000000FFU
+#define _hrt_ones_0x9  0x000001FFU
+#define _hrt_ones_0xA  0x000003FFU
+#define _hrt_ones_0xB  0x000007FFU
+#define _hrt_ones_0xC  0x00000FFFU
+#define _hrt_ones_0xD  0x00001FFFU
+#define _hrt_ones_0xE  0x00003FFFU
+#define _hrt_ones_0xF  0x00007FFFU
+#define _hrt_ones_0x10 0x0000FFFFU
+#define _hrt_ones_0x11 0x0001FFFFU
+#define _hrt_ones_0x12 0x0003FFFFU
+#define _hrt_ones_0x13 0x0007FFFFU
+#define _hrt_ones_0x14 0x000FFFFFU
+#define _hrt_ones_0x15 0x001FFFFFU
+#define _hrt_ones_0x16 0x003FFFFFU
+#define _hrt_ones_0x17 0x007FFFFFU
+#define _hrt_ones_0x18 0x00FFFFFFU
+#define _hrt_ones_0x19 0x01FFFFFFU
+#define _hrt_ones_0x1A 0x03FFFFFFU
+#define _hrt_ones_0x1B 0x07FFFFFFU
+#define _hrt_ones_0x1C 0x0FFFFFFFU
+#define _hrt_ones_0x1D 0x1FFFFFFFU
+#define _hrt_ones_0x1E 0x3FFFFFFFU
+#define _hrt_ones_0x1F 0x7FFFFFFFU
+#define _hrt_ones_0x20 0xFFFFFFFFU
+
+#define _hrt_ones_0  _hrt_ones_0x0
+#define _hrt_ones_1  _hrt_ones_0x1
+#define _hrt_ones_2  _hrt_ones_0x2
+#define _hrt_ones_3  _hrt_ones_0x3
+#define _hrt_ones_4  _hrt_ones_0x4
+#define _hrt_ones_5  _hrt_ones_0x5
+#define _hrt_ones_6  _hrt_ones_0x6
+#define _hrt_ones_7  _hrt_ones_0x7
+#define _hrt_ones_8  _hrt_ones_0x8
+#define _hrt_ones_9  _hrt_ones_0x9
+#define _hrt_ones_10 _hrt_ones_0xA
+#define _hrt_ones_11 _hrt_ones_0xB
+#define _hrt_ones_12 _hrt_ones_0xC
+#define _hrt_ones_13 _hrt_ones_0xD
+#define _hrt_ones_14 _hrt_ones_0xE
+#define _hrt_ones_15 _hrt_ones_0xF
+#define _hrt_ones_16 _hrt_ones_0x10
+#define _hrt_ones_17 _hrt_ones_0x11
+#define _hrt_ones_18 _hrt_ones_0x12
+#define _hrt_ones_19 _hrt_ones_0x13
+#define _hrt_ones_20 _hrt_ones_0x14
+#define _hrt_ones_21 _hrt_ones_0x15
+#define _hrt_ones_22 _hrt_ones_0x16
+#define _hrt_ones_23 _hrt_ones_0x17
+#define _hrt_ones_24 _hrt_ones_0x18
+#define _hrt_ones_25 _hrt_ones_0x19
+#define _hrt_ones_26 _hrt_ones_0x1A
+#define _hrt_ones_27 _hrt_ones_0x1B
+#define _hrt_ones_28 _hrt_ones_0x1C
+#define _hrt_ones_29 _hrt_ones_0x1D
+#define _hrt_ones_30 _hrt_ones_0x1E
+#define _hrt_ones_31 _hrt_ones_0x1F
+#define _hrt_ones_32 _hrt_ones_0x20
+
+#define _hrt_mask(b, n) \
+  (_hrt_ones(n) << (b))
+#define _hrt_get_bits(w, b, n) \
+  (((w) >> (b)) & _hrt_ones(n))
+#define _hrt_set_bits(w, b, n, v) \
+  (((w) & ~_hrt_mask(b, n)) | (((v) & _hrt_ones(n)) << (b)))
+#define _hrt_get_bit(w, b) \
+  (((w) >> (b)) & 1)
+#define _hrt_set_bit(w, b, v) \
+  (((w) & (~(1 << (b)))) | (((v) & 1) << (b)))
+#define _hrt_set_lower_half(w, v) \
+  _hrt_set_bits(w, 0, 16, v)
+#define _hrt_set_upper_half(w, v) \
+  _hrt_set_bits(w, 16, 16, v)
+
+#endif /* _HRT_BITS_H */
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_binarydesc.h b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_binarydesc.h
new file mode 100644
index 000000000..965cfda50
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_binarydesc.h
@@ -0,0 +1,298 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPE_BINARYDESC_H__
+#define __IA_CSS_PIPE_BINARYDESC_H__
+
+#include <ia_css_types.h>		/* ia_css_pipe */
+#include <ia_css_frame_public.h>	/* ia_css_frame_info */
+#include <ia_css_binary.h>		/* ia_css_binary_descr */
+
+/* @brief Get a binary descriptor for copy.
+ *
+ * @param[in] pipe
+ * @param[out] copy_desc
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_copy_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *copy_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for vfpp.
+ *
+ * @param[in] pipe
+ * @param[out] vfpp_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_vfpp_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *vf_pp_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get numerator and denominator of bayer downscaling factor.
+ *
+ * @param[in] bds_factor: The bayer downscaling factor.
+ *		(= The bds_factor member in the sh_css_bds_factor structure.)
+ * @param[out] bds_factor_numerator: The numerator of the bayer downscaling factor.
+ *		(= The numerator member in the sh_css_bds_factor structure.)
+ * @param[out] bds_factor_denominator: The denominator of the bayer downscaling factor.
+ *		(= The denominator member in the sh_css_bds_factor structure.)
+ * @return	0 or error code upon error.
+ *
+ */
+int sh_css_bds_factor_get_numerator_denominator(
+    unsigned int bds_factor,
+    unsigned int *bds_factor_numerator,
+    unsigned int *bds_factor_denominator);
+
+/* @brief Get a binary descriptor for preview stage.
+ *
+ * @param[in] pipe
+ * @param[out] preview_descr
+ * @param[in/out] in_info
+ * @param[in/out] bds_out_info
+ * @param[in/out] out_info
+ * @param[in/out] vf_info
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_pipe_get_preview_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *preview_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *bds_out_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for video stage.
+ *
+ * @param[in/out] pipe
+ * @param[out] video_descr
+ * @param[in/out] in_info
+ * @param[in/out] bds_out_info
+ * @param[in/out] vf_info
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_pipe_get_video_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *video_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *bds_out_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info,
+    int stream_config_left_padding);
+
+/* @brief Get a binary descriptor for yuv scaler stage.
+ *
+ * @param[in/out] pipe
+ * @param[out] yuv_scaler_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @param[in/out] internal_out_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_yuvscaler_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *yuv_scaler_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *internal_out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for capture pp stage.
+ *
+ * @param[in/out] pipe
+ * @param[out] capture_pp_descr
+ * @param[in/out] in_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_capturepp_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *capture_pp_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for primary capture.
+ *
+ * @param[in] pipe
+ * @param[out] prim_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_primary_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *prim_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info,
+    unsigned int stage_idx);
+
+/* @brief Get a binary descriptor for pre gdc stage.
+ *
+ * @param[in] pipe
+ * @param[out] pre_gdc_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_pre_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get a binary descriptor for gdc stage.
+ *
+ * @param[in] pipe
+ * @param[out] gdc_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get a binary descriptor for post gdc.
+ *
+ * @param[in] pipe
+ * @param[out] post_gdc_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_post_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *post_gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for de.
+ *
+ * @param[in] pipe
+ * @param[out] pre_de_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_pre_de_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *pre_de_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get a binary descriptor for pre anr stage.
+ *
+ * @param[in] pipe
+ * @param[out] pre_anr_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_pre_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *pre_anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get a binary descriptor for ANR stage.
+ *
+ * @param[in] pipe
+ * @param[out] anr_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Get a binary descriptor for post anr stage.
+ *
+ * @param[in] pipe
+ * @param[out] post_anr_descr
+ * @param[in/out] in_info
+ * @param[in/out] out_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_post_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *post_anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info);
+
+/* @brief Get a binary descriptor for ldc stage.
+ *
+ * @param[in/out] pipe
+ * @param[out] capture_pp_descr
+ * @param[in/out] in_info
+ * @param[in/out] vf_info
+ * @return    None
+ *
+ */
+void ia_css_pipe_get_ldc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *ldc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info);
+
+/* @brief Calculates the required BDS factor
+ *
+ * @param[in] input_res
+ * @param[in] output_res
+ * @param[in/out] bds_factor
+ * @return	0 or error code upon error.
+ */
+int binarydesc_calculate_bds_factor(
+    struct ia_css_resolution input_res,
+    struct ia_css_resolution output_res,
+    unsigned int *bds_factor);
+
+#endif /* __IA_CSS_PIPE_BINARYDESC_H__ */
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_stagedesc.h b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_stagedesc.h
new file mode 100644
index 000000000..40c8145a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_stagedesc.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPE_STAGEDESC_H__
+#define __IA_CSS_PIPE_STAGEDESC_H__
+
+#include <ia_css_acc_types.h> /* ia_css_fw_info */
+#include <ia_css_frame_public.h>
+#include <ia_css_binary.h>
+#include "ia_css_pipeline.h"
+#include "ia_css_pipeline_common.h"
+
+void ia_css_pipe_get_generic_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_frame *out_frame[],
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *vf_frame);
+
+void ia_css_pipe_get_firmwares_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_frame *out_frame[],
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *vf_frame,
+    const struct ia_css_fw_info *fw,
+    unsigned int mode);
+
+void ia_css_pipe_get_acc_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_fw_info *fw);
+
+void ia_css_pipe_get_sp_func_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_frame *out_frame,
+    enum ia_css_pipeline_stage_sp_func sp_func,
+    unsigned int max_input_width);
+
+#endif /*__IA_CSS_PIPE_STAGEDESC__H__ */
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_util.h b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_util.h
new file mode 100644
index 000000000..c23d1bd91
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/interface/ia_css_pipe_util.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPE_UTIL_H__
+#define __IA_CSS_PIPE_UTIL_H__
+
+#include <ia_css_types.h>
+#include <ia_css_frame_public.h>
+
+/* @brief Get Input format bits per pixel based on stream configuration of this
+ * pipe.
+ *
+ * @param[in] pipe
+ * @return   bits per pixel for the underlying stream
+ *
+ */
+unsigned int ia_css_pipe_util_pipe_input_format_bpp(
+    const struct ia_css_pipe *const pipe);
+
+void ia_css_pipe_util_create_output_frames(
+    struct ia_css_frame *frames[]);
+
+void ia_css_pipe_util_set_output_frames(
+    struct ia_css_frame *frames[],
+    unsigned int idx,
+    struct ia_css_frame *frame);
+
+#endif /* __IA_CSS_PIPE_UTIL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_binarydesc.c b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_binarydesc.c
new file mode 100644
index 000000000..7dd0e4a53
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_binarydesc.c
@@ -0,0 +1,853 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_pipe_binarydesc.h"
+#include "ia_css_frame_format.h"
+#include "ia_css_pipe.h"
+#include "ia_css_pipe_util.h"
+#include "ia_css_util.h"
+#include "ia_css_debug.h"
+#include "sh_css_params.h"
+#include <assert_support.h>
+/* HRT_GDC_N */
+#include "gdc_device.h"
+#include <linux/kernel.h>
+
+/* This module provides a binary descriptions to used to find a binary. Since,
+ * every stage is associated with a binary, it implicity helps stage
+ * description. Apart from providing a binary description, this module also
+ * populates the frame info's when required.*/
+
+/* Generic descriptor for offline binaries. Internal function. */
+static void pipe_binarydesc_get_offline(
+    struct ia_css_pipe const *const pipe,
+    const int mode,
+    struct ia_css_binary_descr *descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info[],
+    struct ia_css_frame_info *vf_info)
+{
+	unsigned int i;
+	/* in_info, out_info, vf_info can be NULL */
+	assert(pipe);
+	assert(descr);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "pipe_binarydesc_get_offline() enter:\n");
+
+	descr->mode = mode;
+	descr->online = false;
+	descr->continuous = pipe->stream->config.continuous;
+	descr->striped = false;
+	descr->two_ppc = false;
+	descr->enable_yuv_ds = false;
+	descr->enable_high_speed = false;
+	descr->enable_dvs_6axis = false;
+	descr->enable_reduced_pipe = false;
+	descr->enable_dz = true;
+	descr->enable_xnr = false;
+	descr->enable_dpc = false;
+	descr->enable_tnr = false;
+	descr->enable_capture_pp_bli = false;
+	descr->enable_fractional_ds = false;
+	descr->dvs_env.width = 0;
+	descr->dvs_env.height = 0;
+	descr->stream_format = pipe->stream->config.input_config.format;
+	descr->in_info = in_info;
+	descr->bds_out_info = NULL;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		descr->out_info[i] = out_info[i];
+	descr->vf_info = vf_info;
+	descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	descr->required_bds_factor = SH_CSS_BDS_FACTOR_1_00;
+	descr->stream_config_left_padding = -1;
+}
+
+void ia_css_pipe_get_copy_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *copy_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	unsigned int i;
+	/* out_info can be NULL */
+	assert(pipe);
+	assert(in_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_COPY,
+				    copy_descr, in_info, out_infos, vf_info);
+	copy_descr->online = true;
+	copy_descr->continuous = false;
+	copy_descr->two_ppc = (pipe->stream->config.pixels_per_clock == 2);
+	copy_descr->enable_dz = false;
+	copy_descr->isp_pipe_version = IA_CSS_PIPE_VERSION_1;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_vfpp_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *vf_pp_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	unsigned int i;
+	/* out_info can be NULL ??? */
+	assert(pipe);
+	assert(in_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	in_info->raw_bit_depth = 0;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_VF_PP,
+				    vf_pp_descr, in_info, out_infos, NULL);
+	vf_pp_descr->enable_fractional_ds = true;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+static struct sh_css_bds_factor bds_factors_list[] = {
+	{1, 1, SH_CSS_BDS_FACTOR_1_00},
+	{5, 4, SH_CSS_BDS_FACTOR_1_25},
+	{3, 2, SH_CSS_BDS_FACTOR_1_50},
+	{2, 1, SH_CSS_BDS_FACTOR_2_00},
+	{9, 4, SH_CSS_BDS_FACTOR_2_25},
+	{5, 2, SH_CSS_BDS_FACTOR_2_50},
+	{3, 1, SH_CSS_BDS_FACTOR_3_00},
+	{4, 1, SH_CSS_BDS_FACTOR_4_00},
+	{9, 2, SH_CSS_BDS_FACTOR_4_50},
+	{5, 1, SH_CSS_BDS_FACTOR_5_00},
+	{6, 1, SH_CSS_BDS_FACTOR_6_00},
+	{8, 1, SH_CSS_BDS_FACTOR_8_00}
+};
+
+int sh_css_bds_factor_get_numerator_denominator(
+    unsigned int bds_factor,
+    unsigned int *bds_factor_numerator,
+    unsigned int *bds_factor_denominator)
+{
+	unsigned int i;
+
+	/* Loop over all bds factors until a match is found */
+	for (i = 0; i < ARRAY_SIZE(bds_factors_list); i++) {
+		if (bds_factors_list[i].bds_factor == bds_factor) {
+			*bds_factor_numerator = bds_factors_list[i].numerator;
+			*bds_factor_denominator = bds_factors_list[i].denominator;
+			return 0;
+		}
+	}
+
+	/* Throw an error since bds_factor cannot be found
+	in bds_factors_list */
+	return -EINVAL;
+}
+
+int binarydesc_calculate_bds_factor(
+    struct ia_css_resolution input_res,
+    struct ia_css_resolution output_res,
+    unsigned int *bds_factor)
+{
+	unsigned int i;
+	unsigned int in_w = input_res.width,
+		     in_h = input_res.height,
+		     out_w = output_res.width, out_h = output_res.height;
+
+	unsigned int max_bds_factor = 8;
+	unsigned int max_rounding_margin = 2;
+	/* delta in pixels to account for rounding margin in the calculation */
+	unsigned int delta = max_bds_factor * max_rounding_margin;
+
+	/* Assert if the resolutions are not set */
+	assert(in_w != 0 && in_h != 0);
+	assert(out_w != 0 && out_h != 0);
+
+	/* Loop over all bds factors until a match is found */
+	for (i = 0; i < ARRAY_SIZE(bds_factors_list); i++) {
+		unsigned int num = bds_factors_list[i].numerator;
+		unsigned int den = bds_factors_list[i].denominator;
+
+		/* See width-wise and height-wise if this bds_factor
+		 * satisfies the condition */
+		bool cond = (out_w * num / den + delta > in_w) &&
+			    (out_w * num / den <= in_w) &&
+			    (out_h * num / den + delta > in_h) &&
+			    (out_h * num / den <= in_h);
+
+		if (cond) {
+			*bds_factor = bds_factors_list[i].bds_factor;
+			return 0;
+		}
+	}
+
+	/* Throw an error since a suitable bds_factor cannot be found */
+	return -EINVAL;
+}
+
+int ia_css_pipe_get_preview_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *preview_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *bds_out_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	int err;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	int mode = IA_CSS_BINARY_MODE_PREVIEW;
+	unsigned int i;
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	assert(vf_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	/*
+	 * Set up the info of the input frame with
+	 * the ISP required resolution
+	 */
+	in_info->res = pipe->config.input_effective_res;
+	in_info->padded_width = in_info->res.width;
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+
+	if (ia_css_util_is_input_format_yuv(pipe->stream->config.input_config.format))
+		mode = IA_CSS_BINARY_MODE_COPY;
+	else
+		in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, mode,
+				    preview_descr, in_info, out_infos, vf_info);
+	if (pipe->stream->config.online) {
+		preview_descr->online = pipe->stream->config.online;
+		preview_descr->two_ppc =
+		    (pipe->stream->config.pixels_per_clock == 2);
+	}
+	preview_descr->stream_format = pipe->stream->config.input_config.format;
+
+	/* TODO: Remove this when bds_out_info is available! */
+	*bds_out_info = *in_info;
+
+	if (pipe->extra_config.enable_raw_binning) {
+		if (pipe->config.bayer_ds_out_res.width != 0 &&
+		    pipe->config.bayer_ds_out_res.height != 0) {
+			bds_out_info->res.width =
+			    pipe->config.bayer_ds_out_res.width;
+			bds_out_info->res.height =
+			    pipe->config.bayer_ds_out_res.height;
+			bds_out_info->padded_width =
+			    pipe->config.bayer_ds_out_res.width;
+			err =
+			    binarydesc_calculate_bds_factor(in_info->res,
+							    bds_out_info->res,
+							    &preview_descr->required_bds_factor);
+			if (err)
+				return err;
+		} else {
+			bds_out_info->res.width = in_info->res.width / 2;
+			bds_out_info->res.height = in_info->res.height / 2;
+			bds_out_info->padded_width = in_info->padded_width / 2;
+			preview_descr->required_bds_factor =
+			    SH_CSS_BDS_FACTOR_2_00;
+		}
+	} else {
+		/* TODO: Remove this when bds_out_info->is available! */
+		bds_out_info->res.width = in_info->res.width;
+		bds_out_info->res.height = in_info->res.height;
+		bds_out_info->padded_width = in_info->padded_width;
+		preview_descr->required_bds_factor = SH_CSS_BDS_FACTOR_1_00;
+	}
+	pipe->required_bds_factor = preview_descr->required_bds_factor;
+
+	/* bayer ds and fractional ds cannot be enabled at the same time,
+	so we disable bds_out_info when fractional ds is used */
+	if (!pipe->extra_config.enable_fractional_ds)
+		preview_descr->bds_out_info = bds_out_info;
+	else
+		preview_descr->bds_out_info = NULL;
+	/*
+	   ----Preview binary-----
+	   --in-->|--out->|vf_veceven|--|--->vf
+	   -----------------------
+	   * Preview binary normally doesn't have a vf_port but
+	   * instead it has an output port. However, the output is
+	   * generated by vf_veceven module in which we might have
+	   * a downscaling (by 1x, 2x, or 4x). Because the resolution
+	   * might change, we need two different info, namely out_info
+	   * & vf_info. In fill_binary_info we use out&vf info to
+	   * calculate vf decimation factor.
+	 */
+	*out_info = *vf_info;
+
+	/* In case of preview_ds binary, we can do any fractional amount
+	 * of downscale, so there is no DS needed in vf_veceven. Therefore,
+	 * out and vf infos will be the same. Otherwise, we set out resolution
+	 * equal to in resolution. */
+	if (!pipe->extra_config.enable_fractional_ds) {
+		/* TODO: Change this when bds_out_info is available! */
+		out_info->res.width = bds_out_info->res.width;
+		out_info->res.height = bds_out_info->res.height;
+		out_info->padded_width = bds_out_info->padded_width;
+	}
+	preview_descr->enable_fractional_ds =
+	    pipe->extra_config.enable_fractional_ds;
+
+	preview_descr->enable_dpc = pipe->config.enable_dpc;
+
+	preview_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+int ia_css_pipe_get_video_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *video_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *bds_out_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info,
+    int stream_config_left_padding)
+{
+	int mode = IA_CSS_BINARY_MODE_VIDEO;
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	int err = 0;
+	bool stream_dz_config = false;
+
+	/* vf_info can be NULL */
+	assert(pipe);
+	assert(in_info);
+	/* assert(vf_info != NULL); */
+	IA_CSS_ENTER_PRIVATE("");
+
+	/* The solution below is not optimal; we should move to using ia_css_pipe_get_copy_binarydesc()
+	 * But for now this fixes things; this code used to be there but was removed
+	 * with gerrit 8908 as this was wrong for Skycam; however 240x still needs this
+	 */
+	if (ia_css_util_is_input_format_yuv(pipe->stream->config.input_config.format))
+		mode = IA_CSS_BINARY_MODE_COPY;
+
+	in_info->res = pipe->config.input_effective_res;
+	in_info->padded_width = in_info->res.width;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, mode,
+				    video_descr, in_info, out_infos, vf_info);
+
+	if (pipe->stream->config.online) {
+		video_descr->online = pipe->stream->config.online;
+		video_descr->two_ppc =
+		    (pipe->stream->config.pixels_per_clock == 2);
+	}
+
+	if (mode == IA_CSS_BINARY_MODE_VIDEO) {
+		stream_dz_config =
+		    ((pipe->stream->isp_params_configs->dz_config.dx !=
+		      HRT_GDC_N)
+		     || (pipe->stream->isp_params_configs->dz_config.dy !=
+			 HRT_GDC_N));
+
+		video_descr->enable_dz = pipe->config.enable_dz
+					 || stream_dz_config;
+		video_descr->dvs_env = pipe->config.dvs_envelope;
+		video_descr->enable_yuv_ds = pipe->extra_config.enable_yuv_ds;
+		video_descr->enable_high_speed =
+		    pipe->extra_config.enable_high_speed;
+		video_descr->enable_dvs_6axis =
+		    pipe->extra_config.enable_dvs_6axis;
+		video_descr->enable_reduced_pipe =
+		    pipe->extra_config.enable_reduced_pipe;
+		video_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+		video_descr->enable_fractional_ds =
+		    pipe->extra_config.enable_fractional_ds;
+		video_descr->enable_dpc =
+		    pipe->config.enable_dpc;
+		video_descr->enable_tnr =
+		    pipe->config.enable_tnr;
+
+		if (pipe->extra_config.enable_raw_binning) {
+			if (pipe->config.bayer_ds_out_res.width != 0 &&
+			    pipe->config.bayer_ds_out_res.height != 0) {
+				bds_out_info->res.width =
+				    pipe->config.bayer_ds_out_res.width;
+				bds_out_info->res.height =
+				    pipe->config.bayer_ds_out_res.height;
+				bds_out_info->padded_width =
+				    pipe->config.bayer_ds_out_res.width;
+				err =
+				    binarydesc_calculate_bds_factor(
+					in_info->res, bds_out_info->res,
+					&video_descr->required_bds_factor);
+				if (err)
+					return err;
+			} else {
+				bds_out_info->res.width =
+				    in_info->res.width / 2;
+				bds_out_info->res.height =
+				    in_info->res.height / 2;
+				bds_out_info->padded_width =
+				    in_info->padded_width / 2;
+				video_descr->required_bds_factor =
+				    SH_CSS_BDS_FACTOR_2_00;
+			}
+		} else {
+			bds_out_info->res.width = in_info->res.width;
+			bds_out_info->res.height = in_info->res.height;
+			bds_out_info->padded_width = in_info->padded_width;
+			video_descr->required_bds_factor =
+			    SH_CSS_BDS_FACTOR_1_00;
+		}
+
+		pipe->required_bds_factor = video_descr->required_bds_factor;
+
+		/* bayer ds and fractional ds cannot be enabled
+		at the same time, so we disable bds_out_info when
+		fractional ds is used */
+		if (!pipe->extra_config.enable_fractional_ds)
+			video_descr->bds_out_info = bds_out_info;
+		else
+			video_descr->bds_out_info = NULL;
+
+		video_descr->enable_fractional_ds =
+		    pipe->extra_config.enable_fractional_ds;
+		video_descr->stream_config_left_padding = stream_config_left_padding;
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+void ia_css_pipe_get_yuvscaler_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *yuv_scaler_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *internal_out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame_info *this_vf_info = NULL;
+
+	assert(pipe);
+	assert(in_info);
+	/* Note: if the following assert fails, the number of ports has been
+	 * changed; in that case an additional initializer must be added
+	 * a few lines below after which this assert can be updated.
+	 */
+	assert(IA_CSS_BINARY_MAX_OUTPUT_PORTS == 2);
+	IA_CSS_ENTER_PRIVATE("");
+
+	in_info->padded_width = in_info->res.width;
+	in_info->raw_bit_depth = 0;
+	ia_css_frame_info_set_width(in_info, in_info->res.width, 0);
+	out_infos[0] = out_info;
+	out_infos[1] = internal_out_info;
+	/* add initializers here if
+	 * assert(IA_CSS_BINARY_MAX_OUTPUT_PORTS == ...);
+	 * fails
+	 */
+
+	if (vf_info) {
+		this_vf_info = (vf_info->res.width == 0 &&
+				vf_info->res.height == 0) ? NULL : vf_info;
+	}
+
+	pipe_binarydesc_get_offline(pipe,
+				    IA_CSS_BINARY_MODE_CAPTURE_PP,
+				    yuv_scaler_descr,
+				    in_info, out_infos, this_vf_info);
+
+	yuv_scaler_descr->enable_fractional_ds = true;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_capturepp_binarydesc(
+    struct ia_css_pipe *const pipe,
+    struct ia_css_binary_descr *capture_pp_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(vf_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	/* the in_info is only used for resolution to enable
+	   bayer down scaling. */
+	if (pipe->out_yuv_ds_input_info.res.width)
+		*in_info = pipe->out_yuv_ds_input_info;
+	else
+		*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_YUV420;
+	in_info->raw_bit_depth = 0;
+	ia_css_frame_info_set_width(in_info, in_info->res.width, 0);
+
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe,
+				    IA_CSS_BINARY_MODE_CAPTURE_PP,
+				    capture_pp_descr,
+				    in_info, out_infos, vf_info);
+
+	capture_pp_descr->enable_capture_pp_bli =
+	    pipe->config.default_capture_config.enable_capture_pp_bli;
+	capture_pp_descr->enable_fractional_ds = true;
+	capture_pp_descr->enable_xnr =
+	    pipe->config.default_capture_config.enable_xnr != 0;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+/* lookup table for high quality primary binaries */
+static unsigned int primary_hq_binary_modes[NUM_PRIMARY_HQ_STAGES] = {
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE0,
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE1,
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE2,
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE3,
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE4,
+	IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE5
+};
+
+void ia_css_pipe_get_primary_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *prim_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info,
+    unsigned int stage_idx)
+{
+	enum ia_css_pipe_version pipe_version = pipe->config.isp_pipe_version;
+	int mode;
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	assert(stage_idx < NUM_PRIMARY_HQ_STAGES);
+	/* vf_info can be NULL - example video_binarydescr */
+	/*assert(vf_info != NULL);*/
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (pipe_version == IA_CSS_PIPE_VERSION_2_6_1)
+		mode = primary_hq_binary_modes[stage_idx];
+	else
+		mode = IA_CSS_BINARY_MODE_PRIMARY;
+
+	if (ia_css_util_is_input_format_yuv(pipe->stream->config.input_config.format))
+		mode = IA_CSS_BINARY_MODE_COPY;
+
+	in_info->res = pipe->config.input_effective_res;
+	in_info->padded_width = in_info->res.width;
+
+	if (pipe->stream->config.pack_raw_pixels)
+		in_info->format = IA_CSS_FRAME_FORMAT_RAW_PACKED;
+	else
+		in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, mode,
+				    prim_descr, in_info, out_infos, vf_info);
+
+	if (pipe->stream->config.online &&
+	    pipe->stream->config.mode != IA_CSS_INPUT_MODE_MEMORY) {
+		prim_descr->online = true;
+		prim_descr->two_ppc =
+		    (pipe->stream->config.pixels_per_clock == 2);
+		prim_descr->stream_format = pipe->stream->config.input_config.format;
+	}
+	if (mode == IA_CSS_BINARY_MODE_PRIMARY) {
+		prim_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+		prim_descr->enable_fractional_ds =
+		    pipe->extra_config.enable_fractional_ds;
+		/* We have both striped and non-striped primary binaries,
+		 * if continuous viewfinder is required, then we must select
+		 * a striped one. Otherwise we prefer to use a non-striped
+		 * since it has better performance. */
+		if (pipe_version == IA_CSS_PIPE_VERSION_2_6_1)
+			prim_descr->striped = false;
+		else
+			prim_descr->striped = prim_descr->continuous &&
+					      (!pipe->stream->stop_copy_preview || !pipe->stream->disable_cont_vf);
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_pre_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *pre_gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_PRE_ISP,
+				    pre_gdc_descr, in_info, out_infos, NULL);
+	pre_gdc_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_QPLANE6;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_GDC,
+				    gdc_descr, in_info, out_infos, NULL);
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_post_gdc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *post_gdc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	assert(vf_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_YUV420_16;
+	in_info->raw_bit_depth = 16;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_POST_ISP,
+				    post_gdc_descr, in_info, out_infos, vf_info);
+
+	post_gdc_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_pre_de_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *pre_de_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_1)
+		pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_PRE_ISP,
+					    pre_de_descr, in_info, out_infos, NULL);
+	else if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_2_2) {
+		pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_PRE_DE,
+					    pre_de_descr, in_info, out_infos, NULL);
+	}
+
+	if (pipe->stream->config.online) {
+		pre_de_descr->online = true;
+		pre_de_descr->two_ppc =
+		    (pipe->stream->config.pixels_per_clock == 2);
+		pre_de_descr->stream_format = pipe->stream->config.input_config.format;
+	}
+	pre_de_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_pre_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *pre_anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_PRE_ISP,
+				    pre_anr_descr, in_info, out_infos, NULL);
+
+	if (pipe->stream->config.online) {
+		pre_anr_descr->online = true;
+		pre_anr_descr->two_ppc =
+		    (pipe->stream->config.pixels_per_clock == 2);
+		pre_anr_descr->stream_format = pipe->stream->config.input_config.format;
+	}
+	pre_anr_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ANR_ELEMENT_BITS;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_ANR,
+				    anr_descr, in_info, out_infos, NULL);
+
+	anr_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_post_anr_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *post_anr_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	assert(vf_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+	in_info->format = IA_CSS_FRAME_FORMAT_RAW;
+	in_info->raw_bit_depth = ANR_ELEMENT_BITS;
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_POST_ISP,
+				    post_anr_descr, in_info, out_infos, vf_info);
+
+	post_anr_descr->isp_pipe_version = pipe->config.isp_pipe_version;
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_ldc_binarydesc(
+    struct ia_css_pipe const *const pipe,
+    struct ia_css_binary_descr *ldc_descr,
+    struct ia_css_frame_info *in_info,
+    struct ia_css_frame_info *out_info)
+{
+	unsigned int i;
+	struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+
+	assert(pipe);
+	assert(in_info);
+	assert(out_info);
+	IA_CSS_ENTER_PRIVATE("");
+
+	*in_info = *out_info;
+
+	in_info->format = IA_CSS_FRAME_FORMAT_YUV420;
+	in_info->raw_bit_depth = 0;
+	ia_css_frame_info_set_width(in_info, in_info->res.width, 0);
+
+	out_infos[0] = out_info;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		out_infos[i] = NULL;
+
+	pipe_binarydesc_get_offline(pipe, IA_CSS_BINARY_MODE_CAPTURE_PP,
+				    ldc_descr, in_info, out_infos, NULL);
+	ldc_descr->enable_dvs_6axis =
+	    pipe->extra_config.enable_dvs_6axis;
+	IA_CSS_LEAVE_PRIVATE("");
+}
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_stagedesc.c b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_stagedesc.c
new file mode 100644
index 000000000..82a24aabe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_stagedesc.c
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_pipe_stagedesc.h"
+#include "assert_support.h"
+#include "ia_css_debug.h"
+
+void ia_css_pipe_get_generic_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_frame *out_frame[],
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *vf_frame)
+{
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("stage_desc = %p, binary = %p, out_frame = %p, in_frame = %p, vf_frame = %p",
+			     stage_desc, binary, out_frame, in_frame, vf_frame);
+
+	assert(stage_desc && binary && binary->info);
+	if (!stage_desc || !binary || !binary->info) {
+		IA_CSS_ERROR("invalid arguments");
+		goto ERR;
+	}
+
+	stage_desc->binary = binary;
+	stage_desc->firmware = NULL;
+	stage_desc->sp_func = IA_CSS_PIPELINE_NO_FUNC;
+	stage_desc->max_input_width = 0;
+	stage_desc->mode = binary->info->sp.pipeline.mode;
+	stage_desc->in_frame = in_frame;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		stage_desc->out_frame[i] = out_frame[i];
+	}
+	stage_desc->vf_frame = vf_frame;
+ERR:
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_pipe_get_firmwares_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_frame *out_frame[],
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *vf_frame,
+    const struct ia_css_fw_info *fw,
+    unsigned int mode)
+{
+	unsigned int i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_get_firmwares_stage_desc() enter:\n");
+	stage_desc->binary = binary;
+	stage_desc->firmware = fw;
+	stage_desc->sp_func = IA_CSS_PIPELINE_NO_FUNC;
+	stage_desc->max_input_width = 0;
+	stage_desc->mode = mode;
+	stage_desc->in_frame = in_frame;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		stage_desc->out_frame[i] = out_frame[i];
+	}
+	stage_desc->vf_frame = vf_frame;
+}
+
+void ia_css_pipe_get_acc_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_binary *binary,
+    struct ia_css_fw_info *fw)
+{
+	unsigned int i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_get_acc_stage_desc() enter:\n");
+	stage_desc->binary = binary;
+	stage_desc->firmware = fw;
+	stage_desc->sp_func = IA_CSS_PIPELINE_NO_FUNC;
+	stage_desc->max_input_width = 0;
+	stage_desc->mode = IA_CSS_BINARY_MODE_VF_PP;
+	stage_desc->in_frame = NULL;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		stage_desc->out_frame[i] = NULL;
+	}
+	stage_desc->vf_frame = NULL;
+}
+
+void ia_css_pipe_get_sp_func_stage_desc(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_frame *out_frame,
+    enum ia_css_pipeline_stage_sp_func sp_func,
+    unsigned int max_input_width)
+{
+	unsigned int i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_get_sp_func_stage_desc() enter:\n");
+	stage_desc->binary = NULL;
+	stage_desc->firmware = NULL;
+	stage_desc->sp_func = sp_func;
+	stage_desc->max_input_width = max_input_width;
+	stage_desc->mode = (unsigned int)-1;
+	stage_desc->in_frame = NULL;
+	stage_desc->out_frame[0] = out_frame;
+	for (i = 1; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		stage_desc->out_frame[i] = NULL;
+	}
+	stage_desc->vf_frame = NULL;
+}
diff --git a/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_util.c b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_util.c
new file mode 100644
index 000000000..03d9d168f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/pipe/src/pipe_util.c
@@ -0,0 +1,51 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_pipe_util.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_pipe.h"
+#include "ia_css_util.h"
+#include "assert_support.h"
+
+unsigned int ia_css_pipe_util_pipe_input_format_bpp(
+    const struct ia_css_pipe *const pipe)
+{
+	assert(pipe);
+	assert(pipe->stream);
+
+	return ia_css_util_input_format_bpp(pipe->stream->config.input_config.format,
+					    pipe->stream->config.pixels_per_clock == 2);
+}
+
+void ia_css_pipe_util_create_output_frames(
+    struct ia_css_frame *frames[])
+{
+	unsigned int i;
+
+	assert(frames);
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		frames[i] = NULL;
+	}
+}
+
+void ia_css_pipe_util_set_output_frames(
+    struct ia_css_frame *frames[],
+    unsigned int idx,
+    struct ia_css_frame *frame)
+{
+	assert(idx < IA_CSS_BINARY_MAX_OUTPUT_PORTS);
+
+	frames[idx] = frame;
+}
diff --git a/drivers/staging/media/atomisp/pci/camera/util/interface/ia_css_util.h b/drivers/staging/media/atomisp/pci/camera/util/interface/ia_css_util.h
new file mode 100644
index 000000000..59df44d69
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/util/interface/ia_css_util.h
@@ -0,0 +1,143 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_UTIL_H__
+#define __IA_CSS_UTIL_H__
+
+#include <linux/errno.h>
+
+#include <ia_css_err.h>
+#include <type_support.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_stream_public.h>
+#include <ia_css_stream_format.h>
+
+/* @brief convert "errno" error code to "ia_css_err" error code
+ *
+ * @param[in]	"errno" error code
+ * @return	"ia_css_err" error code
+ *
+ */
+int ia_css_convert_errno(
+    int in_err);
+
+/* @brief check vf frame info.
+ *
+ * @param[in] info
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_util_check_vf_info(
+    const struct ia_css_frame_info *const info);
+
+/* @brief check input configuration.
+ *
+ * @param[in] stream_config
+ * @param[in] must_be_raw
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_util_check_input(
+    const struct ia_css_stream_config *const stream_config,
+    bool must_be_raw,
+    bool must_be_yuv);
+
+/* @brief check vf and out frame info.
+ *
+ * @param[in] out_info
+ * @param[in] vf_info
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_util_check_vf_out_info(
+    const struct ia_css_frame_info *const out_info,
+    const struct ia_css_frame_info *const vf_info);
+
+/* @brief check width and height
+ *
+ * @param[in] width
+ * @param[in] height
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_util_check_res(
+    unsigned int width,
+    unsigned int height);
+
+/* ISP2401 */
+/* @brief compare resolutions (less or equal)
+ *
+ * @param[in] a resolution
+ * @param[in] b resolution
+ * @return    true if both dimensions of a are less or
+ *            equal than those of b, false otherwise
+ *
+ */
+bool ia_css_util_res_leq(
+    struct ia_css_resolution a,
+    struct ia_css_resolution b);
+
+/* ISP2401 */
+/**
+ * @brief Check if resolution is zero
+ *
+ * @param[in] resolution The resolution to check
+ *
+ * @returns true if resolution is zero
+ */
+bool ia_css_util_resolution_is_zero(
+    const struct ia_css_resolution resolution);
+
+/* ISP2401 */
+/**
+ * @brief Check if resolution is even
+ *
+ * @param[in] resolution The resolution to check
+ *
+ * @returns true if resolution is even
+ */
+bool ia_css_util_resolution_is_even(
+    const struct ia_css_resolution resolution);
+
+/* @brief check width and height
+ *
+ * @param[in] stream_format
+ * @param[in] two_ppc
+ * @return bits per pixel based on given parameters.
+ *
+ */
+unsigned int ia_css_util_input_format_bpp(
+    enum atomisp_input_format stream_format,
+    bool two_ppc);
+
+/* @brief check if input format it raw
+ *
+ * @param[in] stream_format
+ * @return true if the input format is raw or false otherwise
+ *
+ */
+bool ia_css_util_is_input_format_raw(
+    enum atomisp_input_format stream_format);
+
+/* @brief check if input format it yuv
+ *
+ * @param[in] stream_format
+ * @return true if the input format is yuv or false otherwise
+ *
+ */
+bool ia_css_util_is_input_format_yuv(
+    enum atomisp_input_format stream_format);
+
+#endif /* __IA_CSS_UTIL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/camera/util/src/util.c b/drivers/staging/media/atomisp/pci/camera/util/src/util.c
new file mode 100644
index 000000000..40a71e37c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/camera/util/src/util.c
@@ -0,0 +1,197 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_util.h"
+#include <ia_css_frame.h>
+#include <assert_support.h>
+#include <math_support.h>
+
+/* for ia_css_binary_max_vf_width() */
+#include "ia_css_binary.h"
+
+/* MW: Table look-up ??? */
+unsigned int ia_css_util_input_format_bpp(
+    enum atomisp_input_format format,
+    bool two_ppc)
+{
+	unsigned int rval = 0;
+
+	switch (format) {
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+	case ATOMISP_INPUT_FORMAT_EMBEDDED:
+		rval = 8;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+		rval = 10;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		rval = 16;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+		rval = 4;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+		rval = 5;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+		rval = 65;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+		rval = 6;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+		rval = 7;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+		rval = 12;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+		if (two_ppc)
+			rval = 14;
+		else
+			rval = 12;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		if (two_ppc)
+			rval = 16;
+		else
+			rval = 12;
+		break;
+	default:
+		rval = 0;
+		break;
+	}
+	return rval;
+}
+
+int ia_css_util_check_vf_info(
+    const struct ia_css_frame_info *const info)
+{
+	int err;
+	unsigned int max_vf_width;
+
+	assert(info);
+	err = ia_css_frame_check_info(info);
+	if (err)
+		return err;
+	max_vf_width = ia_css_binary_max_vf_width();
+	if (max_vf_width != 0 && info->res.width > max_vf_width * 2)
+		return -EINVAL;
+	return 0;
+}
+
+int ia_css_util_check_vf_out_info(
+    const struct ia_css_frame_info *const out_info,
+    const struct ia_css_frame_info *const vf_info)
+{
+	int err;
+
+	assert(out_info);
+	assert(vf_info);
+
+	err = ia_css_frame_check_info(out_info);
+	if (err)
+		return err;
+	err = ia_css_util_check_vf_info(vf_info);
+	if (err)
+		return err;
+	return 0;
+}
+
+int ia_css_util_check_res(unsigned int width, unsigned int height)
+{
+	/* height can be odd number for jpeg/embedded data from ISYS2401 */
+	if (((width  == 0)   ||
+	     (height == 0)   ||
+	     IS_ODD(width))) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* ISP2401 */
+bool ia_css_util_res_leq(struct ia_css_resolution a, struct ia_css_resolution b)
+{
+	return a.width <= b.width && a.height <= b.height;
+}
+
+/* ISP2401 */
+bool ia_css_util_resolution_is_zero(const struct ia_css_resolution resolution)
+{
+	return (resolution.width == 0) || (resolution.height == 0);
+}
+
+/* ISP2401 */
+bool ia_css_util_resolution_is_even(const struct ia_css_resolution resolution)
+{
+	return IS_EVEN(resolution.height) && IS_EVEN(resolution.width);
+}
+
+bool ia_css_util_is_input_format_raw(enum atomisp_input_format format)
+{
+	return ((format == ATOMISP_INPUT_FORMAT_RAW_6) ||
+		(format == ATOMISP_INPUT_FORMAT_RAW_7) ||
+		(format == ATOMISP_INPUT_FORMAT_RAW_8) ||
+		(format == ATOMISP_INPUT_FORMAT_RAW_10) ||
+		(format == ATOMISP_INPUT_FORMAT_RAW_12));
+	/* raw_14 and raw_16 are not supported as input formats to the ISP.
+	 * They can only be copied to a frame in memory using the
+	 * copy binary.
+	 */
+}
+
+bool ia_css_util_is_input_format_yuv(enum atomisp_input_format format)
+{
+	return format == ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY ||
+	       format == ATOMISP_INPUT_FORMAT_YUV420_8  ||
+	       format == ATOMISP_INPUT_FORMAT_YUV420_10 ||
+	       format == ATOMISP_INPUT_FORMAT_YUV420_16 ||
+	       format == ATOMISP_INPUT_FORMAT_YUV422_8  ||
+	       format == ATOMISP_INPUT_FORMAT_YUV422_10 ||
+	       format == ATOMISP_INPUT_FORMAT_YUV422_16;
+}
+
+int ia_css_util_check_input(
+    const struct ia_css_stream_config *const stream_config,
+    bool must_be_raw,
+    bool must_be_yuv)
+{
+	assert(stream_config);
+
+	if (!stream_config)
+		return -EINVAL;
+
+	if (stream_config->input_config.effective_res.width == 0 ||
+	    stream_config->input_config.effective_res.height == 0)
+		return -EINVAL;
+	if (must_be_raw &&
+	    !ia_css_util_is_input_format_raw(stream_config->input_config.format))
+		return -EINVAL;
+
+	if (must_be_yuv &&
+	    !ia_css_util_is_input_format_yuv(stream_config->input_config.format))
+		return -EINVAL;
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/cell_params.h b/drivers/staging/media/atomisp/pci/cell_params.h
new file mode 100644
index 000000000..3c21a1899
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/cell_params.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _cell_params_h
+#define _cell_params_h
+
+#define SP_PMEM_LOG_WIDTH_BITS           6  /*Width of PC, 64 bits, 8 bytes*/
+#define SP_ICACHE_TAG_BITS               4  /*size of tag*/
+#define SP_ICACHE_SET_BITS               8  /* 256 sets*/
+#define SP_ICACHE_BLOCKS_PER_SET_BITS    1  /* 2 way associative*/
+#define SP_ICACHE_BLOCK_ADDRESS_BITS     11 /* 2048 lines capacity*/
+
+#define SP_ICACHE_ADDRESS_BITS \
+			    (SP_ICACHE_TAG_BITS + SP_ICACHE_BLOCK_ADDRESS_BITS)
+
+#define SP_PMEM_DEPTH        BIT(SP_ICACHE_ADDRESS_BITS)
+
+#define SP_FIFO_0_DEPTH      0
+#define SP_FIFO_1_DEPTH      0
+#define SP_FIFO_2_DEPTH      0
+#define SP_FIFO_3_DEPTH      0
+#define SP_FIFO_4_DEPTH      0
+#define SP_FIFO_5_DEPTH      0
+#define SP_FIFO_6_DEPTH      0
+#define SP_FIFO_7_DEPTH      0
+
+#define SP_SLV_BUS_MAXBURSTSIZE        1
+
+#endif /* _cell_params_h */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/csi_rx_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/csi_rx_global.h
new file mode 100644
index 000000000..3aabd0248
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/csi_rx_global.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_GLOBAL_H_INCLUDED__
+#define __CSI_RX_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+typedef enum {
+	CSI_MIPI_PACKET_TYPE_UNDEFINED = 0,
+	CSI_MIPI_PACKET_TYPE_LONG,
+	CSI_MIPI_PACKET_TYPE_SHORT,
+	CSI_MIPI_PACKET_TYPE_RESERVED,
+	N_CSI_MIPI_PACKET_TYPE
+} csi_mipi_packet_type_t;
+
+typedef struct csi_rx_backend_lut_entry_s	csi_rx_backend_lut_entry_t;
+struct csi_rx_backend_lut_entry_s {
+	u32	long_packet_entry;
+	u32	short_packet_entry;
+};
+
+typedef struct csi_rx_backend_cfg_s csi_rx_backend_cfg_t;
+struct csi_rx_backend_cfg_s {
+	/* LUT entry for the packet */
+	csi_rx_backend_lut_entry_t lut_entry;
+
+	/* can be derived from the Data Type */
+	csi_mipi_packet_type_t csi_mipi_packet_type;
+
+	struct {
+		bool     comp_enable;
+		u32 virtual_channel;
+		u32 data_type;
+		u32 comp_scheme;
+		u32 comp_predictor;
+		u32 comp_bit_idx;
+	} csi_mipi_cfg;
+};
+
+typedef struct csi_rx_frontend_cfg_s csi_rx_frontend_cfg_t;
+struct csi_rx_frontend_cfg_s {
+	u32 active_lanes;
+};
+
+extern const u32 N_SHORT_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID];
+extern const u32 N_LONG_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID];
+extern const u32 N_CSI_RX_FE_CTRL_DLANES[N_CSI_RX_FRONTEND_ID];
+/* sid_width for CSI_RX_BACKEND<N>_ID */
+extern const u32 N_CSI_RX_BE_SID_WIDTH[N_CSI_RX_BACKEND_ID];
+
+#endif /* __CSI_RX_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx.c b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx.c
new file mode 100644
index 000000000..9a8d8f546
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx.c
@@ -0,0 +1,42 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+#include "csi_rx_global.h"
+
+const u32 N_SHORT_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID] = {
+	4,	/* 4 entries at CSI_RX_BACKEND0_ID*/
+	4,	/* 4 entries at CSI_RX_BACKEND1_ID*/
+	4	/* 4 entries at CSI_RX_BACKEND2_ID*/
+};
+
+const u32 N_LONG_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID] = {
+	8,	/* 8 entries at CSI_RX_BACKEND0_ID*/
+	4,	/* 4 entries at CSI_RX_BACKEND1_ID*/
+	4	/* 4 entries at CSI_RX_BACKEND2_ID*/
+};
+
+const u32 N_CSI_RX_FE_CTRL_DLANES[N_CSI_RX_FRONTEND_ID] = {
+	N_CSI_RX_DLANE_ID,	/* 4 dlanes for CSI_RX_FR0NTEND0_ID */
+	N_CSI_RX_DLANE_ID,	/* 4 dlanes for CSI_RX_FR0NTEND1_ID */
+	N_CSI_RX_DLANE_ID	/* 4 dlanes for CSI_RX_FR0NTEND2_ID */
+};
+
+/* sid_width for CSI_RX_BACKEND<N>_ID */
+const u32 N_CSI_RX_BE_SID_WIDTH[N_CSI_RX_BACKEND_ID] = {
+	3,
+	2,
+	2
+};
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_local.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_local.h
new file mode 100644
index 000000000..6489ee644
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_local.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_LOCAL_H_INCLUDED__
+#define __CSI_RX_LOCAL_H_INCLUDED__
+
+#include "csi_rx_global.h"
+#define N_CSI_RX_BE_MIPI_COMP_FMT_REG		4
+#define N_CSI_RX_BE_MIPI_CUSTOM_PEC		12
+#define N_CSI_RX_BE_SHORT_PKT_LUT		4
+#define N_CSI_RX_BE_LONG_PKT_LUT		8
+typedef struct csi_rx_fe_ctrl_state_s		csi_rx_fe_ctrl_state_t;
+typedef struct csi_rx_fe_ctrl_lane_s		csi_rx_fe_ctrl_lane_t;
+typedef struct csi_rx_be_ctrl_state_s		csi_rx_be_ctrl_state_t;
+/*mipi_backend_custom_mode_pixel_extraction_config*/
+typedef struct csi_rx_be_ctrl_pec_s		csi_rx_be_ctrl_pec_t;
+
+struct csi_rx_fe_ctrl_lane_s {
+	hrt_data	termen;
+	hrt_data	settle;
+};
+
+struct csi_rx_fe_ctrl_state_s {
+	hrt_data		enable;
+	hrt_data		nof_enable_lanes;
+	hrt_data		error_handling;
+	hrt_data		status;
+	hrt_data		status_dlane_hs;
+	hrt_data		status_dlane_lp;
+	csi_rx_fe_ctrl_lane_t	clane;
+	csi_rx_fe_ctrl_lane_t	dlane[N_CSI_RX_DLANE_ID];
+};
+
+struct csi_rx_be_ctrl_state_s {
+	hrt_data		enable;
+	hrt_data		status;
+	hrt_data		comp_format_reg[N_CSI_RX_BE_MIPI_COMP_FMT_REG];
+	hrt_data		raw16;
+	hrt_data		raw18;
+	hrt_data		force_raw8;
+	hrt_data		irq_status;
+	hrt_data		custom_mode_enable;
+	hrt_data		custom_mode_data_state;
+	hrt_data		pec[N_CSI_RX_BE_MIPI_CUSTOM_PEC];
+	hrt_data		custom_mode_valid_eop_config;
+	hrt_data		global_lut_disregard_reg;
+	hrt_data		packet_status_stall;
+	hrt_data		short_packet_lut_entry[N_CSI_RX_BE_SHORT_PKT_LUT];
+	hrt_data		long_packet_lut_entry[N_CSI_RX_BE_LONG_PKT_LUT];
+};
+#endif /* __CSI_RX_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_private.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_private.h
new file mode 100644
index 000000000..ece45d80e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/csi_rx_private.h
@@ -0,0 +1,306 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_PRIVATE_H_INCLUDED__
+#define __CSI_RX_PRIVATE_H_INCLUDED__
+
+#include "rx_csi_defs.h"
+#include "mipi_backend_defs.h"
+#include "csi_rx.h"
+
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+
+#include "assert_support.h" /* assert */
+#include "print_support.h" /* print */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline hrt_data csi_rx_fe_ctrl_reg_load(
+    const csi_rx_frontend_ID_t ID,
+    const hrt_address reg)
+{
+	assert(ID < N_CSI_RX_FRONTEND_ID);
+	assert(CSI_RX_FE_CTRL_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(CSI_RX_FE_CTRL_BASE[ID] + reg * sizeof(
+					     hrt_data));
+}
+
+/**
+ * @brief Store a value to the register.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+static inline void csi_rx_fe_ctrl_reg_store(
+    const csi_rx_frontend_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value)
+{
+	assert(ID < N_CSI_RX_FRONTEND_ID);
+	assert(CSI_RX_FE_CTRL_BASE[ID] != (hrt_address)-1);
+
+	ia_css_device_store_uint32(CSI_RX_FE_CTRL_BASE[ID] + reg * sizeof(hrt_data),
+				   value);
+}
+
+/**
+ * @brief Load the register value.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline hrt_data csi_rx_be_ctrl_reg_load(
+    const csi_rx_backend_ID_t ID,
+    const hrt_address reg)
+{
+	assert(ID < N_CSI_RX_BACKEND_ID);
+	assert(CSI_RX_BE_CTRL_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(CSI_RX_BE_CTRL_BASE[ID] + reg * sizeof(
+					     hrt_data));
+}
+
+/**
+ * @brief Store a value to the register.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+static inline void csi_rx_be_ctrl_reg_store(
+    const csi_rx_backend_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value)
+{
+	assert(ID < N_CSI_RX_BACKEND_ID);
+	assert(CSI_RX_BE_CTRL_BASE[ID] != (hrt_address)-1);
+
+	ia_css_device_store_uint32(CSI_RX_BE_CTRL_BASE[ID] + reg * sizeof(hrt_data),
+				   value);
+}
+
+/* end of DLI */
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the state of the csi rx fe dlane process.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline void csi_rx_fe_ctrl_get_dlane_state(
+    const csi_rx_frontend_ID_t ID,
+    const u32 lane,
+    csi_rx_fe_ctrl_lane_t *dlane_state)
+{
+	dlane_state->termen =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_DLY_CNT_TERMEN_DLANE_REG_IDX(lane));
+	dlane_state->settle =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_DLY_CNT_SETTLE_DLANE_REG_IDX(lane));
+}
+
+/**
+ * @brief Get the csi rx fe state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline void csi_rx_fe_ctrl_get_state(
+    const csi_rx_frontend_ID_t ID,
+    csi_rx_fe_ctrl_state_t *state)
+{
+	u32 i;
+
+	state->enable =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_ENABLE_REG_IDX);
+	state->nof_enable_lanes =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_NOF_ENABLED_LANES_REG_IDX);
+	state->error_handling =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_ERROR_HANDLING_REG_IDX);
+	state->status =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_STATUS_REG_IDX);
+	state->status_dlane_hs =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_STATUS_DLANE_HS_REG_IDX);
+	state->status_dlane_lp =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_STATUS_DLANE_LP_REG_IDX);
+	state->clane.termen =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_DLY_CNT_TERMEN_CLANE_REG_IDX);
+	state->clane.settle =
+	    csi_rx_fe_ctrl_reg_load(ID, _HRT_CSI_RX_DLY_CNT_SETTLE_CLANE_REG_IDX);
+
+	/*
+	 * Get the values of the register-set per
+	 * dlane.
+	 */
+	for (i = 0; i < N_CSI_RX_FE_CTRL_DLANES[ID]; i++) {
+		csi_rx_fe_ctrl_get_dlane_state(
+		    ID,
+		    i,
+		    &state->dlane[i]);
+	}
+}
+
+/**
+ * @brief dump the csi rx fe state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline void csi_rx_fe_ctrl_dump_state(
+    const csi_rx_frontend_ID_t ID,
+    csi_rx_fe_ctrl_state_t *state)
+{
+	u32 i;
+
+	ia_css_print("CSI RX FE STATE Controller %d Enable state 0x%x\n", ID,
+		     state->enable);
+	ia_css_print("CSI RX FE STATE Controller %d No Of enable lanes 0x%x\n", ID,
+		     state->nof_enable_lanes);
+	ia_css_print("CSI RX FE STATE Controller %d Error handling 0x%x\n", ID,
+		     state->error_handling);
+	ia_css_print("CSI RX FE STATE Controller %d Status 0x%x\n", ID, state->status);
+	ia_css_print("CSI RX FE STATE Controller %d Status Dlane HS 0x%x\n", ID,
+		     state->status_dlane_hs);
+	ia_css_print("CSI RX FE STATE Controller %d Status Dlane LP 0x%x\n", ID,
+		     state->status_dlane_lp);
+	ia_css_print("CSI RX FE STATE Controller %d Status term enable LP 0x%x\n", ID,
+		     state->clane.termen);
+	ia_css_print("CSI RX FE STATE Controller %d Status term settle LP 0x%x\n", ID,
+		     state->clane.settle);
+
+	/*
+	 * Get the values of the register-set per
+	 * dlane.
+	 */
+	for (i = 0; i < N_CSI_RX_FE_CTRL_DLANES[ID]; i++) {
+		ia_css_print("CSI RX FE STATE Controller %d DLANE ID %d termen 0x%x\n", ID, i,
+			     state->dlane[i].termen);
+		ia_css_print("CSI RX FE STATE Controller %d DLANE ID %d settle 0x%x\n", ID, i,
+			     state->dlane[i].settle);
+	}
+}
+
+/**
+ * @brief Get the csi rx be state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline void csi_rx_be_ctrl_get_state(
+    const csi_rx_backend_ID_t ID,
+    csi_rx_be_ctrl_state_t *state)
+{
+	u32 i;
+
+	state->enable =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_ENABLE_REG_IDX);
+
+	state->status =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_STATUS_REG_IDX);
+
+	for (i = 0; i < N_CSI_RX_BE_MIPI_COMP_FMT_REG ; i++) {
+		state->comp_format_reg[i] =
+		    csi_rx_be_ctrl_reg_load(ID,
+					    _HRT_MIPI_BACKEND_COMP_FORMAT_REG0_IDX + i);
+	}
+
+	state->raw16 =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_RAW16_CONFIG_REG_IDX);
+
+	state->raw18 =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_RAW18_CONFIG_REG_IDX);
+	state->force_raw8 =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_FORCE_RAW8_REG_IDX);
+	state->irq_status =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_IRQ_STATUS_REG_IDX);
+#if 0 /* device access error for these registers */
+	/* ToDo: rootcause this failure */
+	state->custom_mode_enable =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_CUST_EN_REG_IDX);
+
+	state->custom_mode_data_state =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_CUST_DATA_STATE_REG_IDX);
+	for (i = 0; i < N_CSI_RX_BE_MIPI_CUSTOM_PEC ; i++) {
+		state->pec[i] =
+		    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_CUST_PIX_EXT_S0P0_REG_IDX + i);
+	}
+	state->custom_mode_valid_eop_config =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_REG_IDX);
+#endif
+	state->global_lut_disregard_reg =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_GLOBAL_LUT_DISREGARD_REG_IDX);
+	state->packet_status_stall =
+	    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_PKT_STALL_STATUS_REG_IDX);
+	/*
+	 * Get the values of the register-set per
+	 * lut.
+	 */
+	for (i = 0; i < N_SHORT_PACKET_LUT_ENTRIES[ID]; i++) {
+		state->short_packet_lut_entry[i] =
+		    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_SP_LUT_ENTRY_0_REG_IDX + i);
+	}
+	for (i = 0; i < N_LONG_PACKET_LUT_ENTRIES[ID]; i++) {
+		state->long_packet_lut_entry[i] =
+		    csi_rx_be_ctrl_reg_load(ID, _HRT_MIPI_BACKEND_LP_LUT_ENTRY_0_REG_IDX + i);
+	}
+}
+
+/**
+ * @brief Dump the csi rx be state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+static inline void csi_rx_be_ctrl_dump_state(
+    const csi_rx_backend_ID_t ID,
+    csi_rx_be_ctrl_state_t *state)
+{
+	u32 i;
+
+	ia_css_print("CSI RX BE STATE Controller %d Enable 0x%x\n", ID, state->enable);
+	ia_css_print("CSI RX BE STATE Controller %d Status 0x%x\n", ID, state->status);
+
+	for (i = 0; i < N_CSI_RX_BE_MIPI_COMP_FMT_REG ; i++) {
+		ia_css_print("CSI RX BE STATE Controller %d comp format reg vc%d value 0x%x\n",
+			     ID, i, state->status);
+	}
+	ia_css_print("CSI RX BE STATE Controller %d RAW16 0x%x\n", ID, state->raw16);
+	ia_css_print("CSI RX BE STATE Controller %d RAW18 0x%x\n", ID, state->raw18);
+	ia_css_print("CSI RX BE STATE Controller %d Force RAW8 0x%x\n", ID,
+		     state->force_raw8);
+	ia_css_print("CSI RX BE STATE Controller %d IRQ state 0x%x\n", ID,
+		     state->irq_status);
+#if 0   /* ToDo:Getting device access error for this register */
+	for (i = 0; i < N_CSI_RX_BE_MIPI_CUSTOM_PEC ; i++) {
+		ia_css_print("CSI RX BE STATE Controller %d PEC ID %d custom pec 0x%x\n", ID, i,
+			     state->pec[i]);
+	}
+#endif
+	ia_css_print("CSI RX BE STATE Controller %d Global LUT disregard reg 0x%x\n",
+		     ID, state->global_lut_disregard_reg);
+	ia_css_print("CSI RX BE STATE Controller %d packet stall reg 0x%x\n", ID,
+		     state->packet_status_stall);
+	/*
+	 * Get the values of the register-set per
+	 * lut.
+	 */
+	for (i = 0; i < N_SHORT_PACKET_LUT_ENTRIES[ID]; i++) {
+		ia_css_print("CSI RX BE STATE Controller ID %d Short packet entry %d short packet lut id 0x%x\n",
+			     ID, i,
+			     state->short_packet_lut_entry[i]);
+	}
+	for (i = 0; i < N_LONG_PACKET_LUT_ENTRIES[ID]; i++) {
+		ia_css_print("CSI RX BE STATE Controller ID %d Long packet entry %d long packet lut id 0x%x\n",
+			     ID, i,
+			     state->long_packet_lut_entry[i]);
+	}
+}
+
+/* end of NCI */
+
+#endif /* __CSI_RX_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl.c b/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl.c
new file mode 100644
index 000000000..8d19c9875
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <type_support.h>
+#include "system_global.h"
+#include "ibuf_ctrl_global.h"
+
+const u32 N_IBUF_CTRL_PROCS[N_IBUF_CTRL_ID] = {
+	8,	/* IBUF_CTRL0_ID supports at most 8 processes */
+	4,	/* IBUF_CTRL1_ID supports at most 4 processes */
+	4	/* IBUF_CTRL2_ID supports at most 4 processes */
+};
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl_local.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl_local.h
new file mode 100644
index 000000000..f71841195
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/ibuf_ctrl_local.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IBUF_CTRL_LOCAL_H_INCLUDED__
+#define __IBUF_CTRL_LOCAL_H_INCLUDED__
+
+#include "ibuf_ctrl_global.h"
+#include "ibuf_ctrl_local.h"
+
+typedef struct ibuf_ctrl_proc_state_s	ibuf_ctrl_proc_state_t;
+typedef struct ibuf_ctrl_state_s		ibuf_ctrl_state_t;
+
+struct ibuf_ctrl_proc_state_s {
+	hrt_data num_items;
+	hrt_data num_stores;
+	hrt_data dma_channel;
+	hrt_data dma_command;
+	hrt_data ibuf_st_addr;
+	hrt_data ibuf_stride;
+	hrt_data ibuf_end_addr;
+	hrt_data dest_st_addr;
+	hrt_data dest_stride;
+	hrt_data dest_end_addr;
+	hrt_data sync_frame;
+	hrt_data sync_command;
+	hrt_data store_command;
+	hrt_data shift_returned_items;
+	hrt_data elems_ibuf;
+	hrt_data elems_dest;
+	hrt_data cur_stores;
+	hrt_data cur_acks;
+	hrt_data cur_s2m_ibuf_addr;
+	hrt_data cur_dma_ibuf_addr;
+	hrt_data cur_dma_dest_addr;
+	hrt_data cur_isp_dest_addr;
+	hrt_data dma_cmds_send;
+	hrt_data main_cntrl_state;
+	hrt_data dma_sync_state;
+	hrt_data isp_sync_state;
+};
+
+struct ibuf_ctrl_state_s {
+	hrt_data	recalc_words;
+	hrt_data	arbiters;
+	ibuf_ctrl_proc_state_t	proc_state[N_STREAM2MMIO_SID_ID];
+};
+
+#endif /* __IBUF_CTRL_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma.c b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma.c
new file mode 100644
index 000000000..2a5159945
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma.c
@@ -0,0 +1,35 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_local.h"
+#include "isys_dma_global.h"
+#include "assert_support.h"
+#include "isys_dma_private.h"
+
+const isys2401_dma_channel N_ISYS2401_DMA_CHANNEL_PROCS[N_ISYS2401_DMA_ID] = {
+	N_ISYS2401_DMA_CHANNEL
+};
+
+void isys2401_dma_set_max_burst_size(
+    const isys2401_dma_ID_t	dma_id,
+    uint32_t		max_burst_size)
+{
+	assert(dma_id < N_ISYS2401_DMA_ID);
+	assert((max_burst_size > 0x00) && (max_burst_size <= 0xFF));
+
+	isys2401_dma_reg_store(dma_id,
+			       DMA_DEV_INFO_REG_IDX(_DMA_V2_DEV_INTERF_MAX_BURST_IDX, HIVE_DMA_BUS_DDR_CONN),
+			       (max_burst_size - 1));
+}
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma_private.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma_private.h
new file mode 100644
index 000000000..a313e1dc7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_dma_private.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_DMA_PRIVATE_H_INCLUDED__
+#define __ISYS_DMA_PRIVATE_H_INCLUDED__
+
+#include "isys_dma_public.h"
+#include "device_access.h"
+#include "assert_support.h"
+#include "dma.h"
+#include "dma_v2_defs.h"
+#include "print_support.h"
+
+void isys2401_dma_reg_store(const isys2401_dma_ID_t	dma_id,
+			    const unsigned int		reg,
+			    const hrt_data		value)
+{
+	unsigned int reg_loc;
+
+	assert(dma_id < N_ISYS2401_DMA_ID);
+	assert(ISYS2401_DMA_BASE[dma_id] != (hrt_address) - 1);
+
+	reg_loc = ISYS2401_DMA_BASE[dma_id] + (reg * sizeof(hrt_data));
+
+	ia_css_print("isys dma store at addr(0x%x) val(%u)\n", reg_loc,
+		     (unsigned int)value);
+	ia_css_device_store_uint32(reg_loc, value);
+}
+
+hrt_data isys2401_dma_reg_load(const isys2401_dma_ID_t	dma_id,
+			       const unsigned int	reg)
+{
+	unsigned int reg_loc;
+	hrt_data value;
+
+	assert(dma_id < N_ISYS2401_DMA_ID);
+	assert(ISYS2401_DMA_BASE[dma_id] != (hrt_address) - 1);
+
+	reg_loc = ISYS2401_DMA_BASE[dma_id] + (reg * sizeof(hrt_data));
+
+	value = ia_css_device_load_uint32(reg_loc);
+	ia_css_print("isys dma load from addr(0x%x) val(%u)\n", reg_loc,
+		     (unsigned int)value);
+
+	return value;
+}
+
+#endif /* __ISYS_DMA_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq.c b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq.c
new file mode 100644
index 000000000..b6135c4b6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq.c
@@ -0,0 +1,43 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <system_local.h>
+#include "device_access.h"
+#include "assert_support.h"
+#include "ia_css_debug.h"
+#include "isys_irq.h"
+
+#ifndef __INLINE_ISYS2401_IRQ__
+/*
+ * Include definitions for isys irq private functions. isys_irq.h includes
+ * declarations of these functions by including isys_irq_public.h.
+ */
+#include "isys_irq_private.h"
+#endif
+
+/* Public interface */
+void isys_irqc_status_enable(const isys_irq_ID_t	isys_irqc_id)
+{
+	assert(isys_irqc_id < N_ISYS_IRQ_ID);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "Setting irq mask for port %u\n",
+			    isys_irqc_id);
+	isys_irqc_reg_store(isys_irqc_id, ISYS_IRQ_MASK_REG_IDX,
+			    ISYS_IRQ_MASK_REG_VALUE);
+	isys_irqc_reg_store(isys_irqc_id, ISYS_IRQ_CLEAR_REG_IDX,
+			    ISYS_IRQ_CLEAR_REG_VALUE);
+	isys_irqc_reg_store(isys_irqc_id, ISYS_IRQ_ENABLE_REG_IDX,
+			    ISYS_IRQ_ENABLE_REG_VALUE);
+}
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_local.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_local.h
new file mode 100644
index 000000000..a76987190
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_local.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_IRQ_LOCAL_H__
+#define __ISYS_IRQ_LOCAL_H__
+
+#include <type_support.h>
+
+#if defined(ISP2401)
+
+typedef struct isys_irqc_state_s isys_irqc_state_t;
+
+struct isys_irqc_state_s {
+	hrt_data edge;
+	hrt_data mask;
+	hrt_data status;
+	hrt_data enable;
+	hrt_data level_no;
+	/*hrt_data clear;	*/	/* write-only register */
+};
+
+#endif /* defined(ISP2401) */
+
+#endif	/* __ISYS_IRQ_LOCAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_private.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_private.h
new file mode 100644
index 000000000..fb168c25b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_irq_private.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_IRQ_PRIVATE_H__
+#define __ISYS_IRQ_PRIVATE_H__
+
+#include "isys_irq_global.h"
+#include "isys_irq_local.h"
+
+#if defined(ISP2401)
+
+/* -------------------------------------------------------+
+ |             Native command interface (NCI)             |
+ + -------------------------------------------------------*/
+
+/**
+* @brief Get the isys irq status.
+* Refer to "isys_irq.h" for details.
+*/
+void isys_irqc_state_get(
+    const isys_irq_ID_t	isys_irqc_id,
+    isys_irqc_state_t *state)
+{
+	state->edge     = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_EDGE_REG_IDX);
+	state->mask     = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_MASK_REG_IDX);
+	state->status   = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_STATUS_REG_IDX);
+	state->enable   = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_ENABLE_REG_IDX);
+	state->level_no = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_LEVEL_NO_REG_IDX);
+	/*
+	** Invalid to read/load from write-only register 'clear'
+	** state->clear = isys_irqc_reg_load(isys_irqc_id, ISYS_IRQ_CLEAR_REG_IDX);
+	*/
+}
+
+/**
+* @brief Dump the isys irq status.
+* Refer to "isys_irq.h" for details.
+*/
+void isys_irqc_state_dump(
+    const isys_irq_ID_t	isys_irqc_id,
+    const isys_irqc_state_t *state)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "isys irq controller id %d\n\tstatus:0x%x\n\tedge:0x%x\n\tmask:0x%x\n\tenable:0x%x\n\tlevel_not_pulse:0x%x\n",
+			    isys_irqc_id,
+			    state->status, state->edge, state->mask, state->enable, state->level_no);
+}
+
+/* end of NCI */
+
+/* -------------------------------------------------------+
+ |              Device level interface (DLI)              |
+ + -------------------------------------------------------*/
+
+/* Support functions */
+void isys_irqc_reg_store(
+    const isys_irq_ID_t	isys_irqc_id,
+    const unsigned int	reg_idx,
+    const hrt_data	value)
+{
+	unsigned int reg_addr;
+
+	assert(isys_irqc_id < N_ISYS_IRQ_ID);
+	assert(reg_idx <= ISYS_IRQ_LEVEL_NO_REG_IDX);
+
+	reg_addr = ISYS_IRQ_BASE[isys_irqc_id] + (reg_idx * sizeof(hrt_data));
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "isys irq store at addr(0x%x) val(%u)\n", reg_addr, (unsigned int)value);
+
+	ia_css_device_store_uint32(reg_addr, value);
+}
+
+hrt_data isys_irqc_reg_load(
+    const isys_irq_ID_t	isys_irqc_id,
+    const unsigned int	reg_idx)
+{
+	unsigned int reg_addr;
+	hrt_data value;
+
+	assert(isys_irqc_id < N_ISYS_IRQ_ID);
+	assert(reg_idx <= ISYS_IRQ_LEVEL_NO_REG_IDX);
+
+	reg_addr = ISYS_IRQ_BASE[isys_irqc_id] + (reg_idx * sizeof(hrt_data));
+	value = ia_css_device_load_uint32(reg_addr);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "isys irq load from addr(0x%x) val(%u)\n", reg_addr, (unsigned int)value);
+
+	return value;
+}
+
+/* end of DLI */
+
+#endif /* defined(ISP2401) */
+
+#endif	/* __ISYS_IRQ_PRIVATE_H__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio.c b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio.c
new file mode 100644
index 000000000..b7d893aea
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio.c
@@ -0,0 +1,22 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "isys_stream2mmio.h"
+
+const stream2mmio_sid_ID_t N_STREAM2MMIO_SID_PROCS[N_STREAM2MMIO_ID] = {
+	N_STREAM2MMIO_SID_ID,
+	STREAM2MMIO_SID4_ID,
+	STREAM2MMIO_SID4_ID
+};
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_local.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_local.h
new file mode 100644
index 000000000..4fbbcc233
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_local.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_LOCAL_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_LOCAL_H_INCLUDED__
+
+#include "isys_stream2mmio_global.h"
+
+typedef struct stream2mmio_state_s		stream2mmio_state_t;
+typedef struct stream2mmio_sid_state_s	stream2mmio_sid_state_t;
+
+struct stream2mmio_sid_state_s {
+	hrt_data rcv_ack;
+	hrt_data pix_width_id;
+	hrt_data start_addr;
+	hrt_data end_addr;
+	hrt_data strides;
+	hrt_data num_items;
+	hrt_data block_when_no_cmd;
+};
+
+struct stream2mmio_state_s {
+	stream2mmio_sid_state_t	sid_state[N_STREAM2MMIO_SID_ID];
+};
+#endif /* __ISYS_STREAM2MMIO_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_private.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_private.h
new file mode 100644
index 000000000..4a5646a22
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/isys_stream2mmio_private.h
@@ -0,0 +1,168 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_PRIVATE_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_PRIVATE_H_INCLUDED__
+
+#include "isys_stream2mmio_public.h"
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+#include "assert_support.h"	/* assert */
+#include "print_support.h"	/* print */
+
+#define STREAM2MMIO_COMMAND_REG_ID             0
+#define STREAM2MMIO_ACKNOWLEDGE_REG_ID         1
+#define STREAM2MMIO_PIX_WIDTH_ID_REG_ID        2
+#define STREAM2MMIO_START_ADDR_REG_ID          3      /* master port address,NOT Byte */
+#define STREAM2MMIO_END_ADDR_REG_ID            4      /* master port address,NOT Byte */
+#define STREAM2MMIO_STRIDE_REG_ID              5      /* stride in master port words, increment is per packet for long sids, stride is not used for short sid's*/
+#define STREAM2MMIO_NUM_ITEMS_REG_ID           6      /* number of packets for store packets cmd, number of words for store_words cmd */
+#define STREAM2MMIO_BLOCK_WHEN_NO_CMD_REG_ID   7      /* if this register is 1, input will be stalled if there is no pending command for this sid */
+#define STREAM2MMIO_REGS_PER_SID               8
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the stream2mmio-controller state.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C void stream2mmio_get_state(
+    const stream2mmio_ID_t ID,
+    stream2mmio_state_t *state)
+{
+	stream2mmio_sid_ID_t i;
+
+	/*
+	 * Get the values of the register-set per
+	 * stream2mmio-controller sids.
+	 */
+	for (i = STREAM2MMIO_SID0_ID; i < N_STREAM2MMIO_SID_PROCS[ID]; i++) {
+		stream2mmio_get_sid_state(ID, i, &state->sid_state[i]);
+	}
+}
+
+/**
+ * @brief Get the state of the stream2mmio-controller sidess.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C void stream2mmio_get_sid_state(
+    const stream2mmio_ID_t ID,
+    const stream2mmio_sid_ID_t sid_id,
+    stream2mmio_sid_state_t	*state)
+{
+	state->rcv_ack =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_ACKNOWLEDGE_REG_ID);
+
+	state->pix_width_id =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_PIX_WIDTH_ID_REG_ID);
+
+	state->start_addr =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_START_ADDR_REG_ID);
+
+	state->end_addr =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_END_ADDR_REG_ID);
+
+	state->strides =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_STRIDE_REG_ID);
+
+	state->num_items =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_NUM_ITEMS_REG_ID);
+
+	state->block_when_no_cmd =
+	    stream2mmio_reg_load(ID, sid_id, STREAM2MMIO_BLOCK_WHEN_NO_CMD_REG_ID);
+}
+
+/**
+ * @brief Dump the state of the stream2mmio-controller sidess.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C void stream2mmio_print_sid_state(
+    stream2mmio_sid_state_t	*state)
+{
+	ia_css_print("\t \t Receive acks 0x%x\n", state->rcv_ack);
+	ia_css_print("\t \t Pixel width 0x%x\n", state->pix_width_id);
+	ia_css_print("\t \t Startaddr 0x%x\n", state->start_addr);
+	ia_css_print("\t \t Endaddr 0x%x\n", state->end_addr);
+	ia_css_print("\t \t Strides 0x%x\n", state->strides);
+	ia_css_print("\t \t Num Items 0x%x\n", state->num_items);
+	ia_css_print("\t \t block when no cmd 0x%x\n", state->block_when_no_cmd);
+}
+
+/**
+ * @brief Dump the ibuf-controller state.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C void stream2mmio_dump_state(
+    const stream2mmio_ID_t ID,
+    stream2mmio_state_t *state)
+{
+	stream2mmio_sid_ID_t i;
+
+	/*
+	 * Get the values of the register-set per
+	 * stream2mmio-controller sids.
+	 */
+	for (i = STREAM2MMIO_SID0_ID; i < N_STREAM2MMIO_SID_PROCS[ID]; i++) {
+		ia_css_print("StREAM2MMIO ID %d SID %d\n", ID, i);
+		stream2mmio_print_sid_state(&state->sid_state[i]);
+	}
+}
+
+/* end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C hrt_data stream2mmio_reg_load(
+    const stream2mmio_ID_t ID,
+    const stream2mmio_sid_ID_t sid_id,
+    const uint32_t reg_idx)
+{
+	u32 reg_bank_offset;
+
+	assert(ID < N_STREAM2MMIO_ID);
+
+	reg_bank_offset = STREAM2MMIO_REGS_PER_SID * sid_id;
+	return ia_css_device_load_uint32(STREAM2MMIO_CTRL_BASE[ID] +
+					 (reg_bank_offset + reg_idx) * sizeof(hrt_data));
+}
+
+/**
+ * @brief Store a value to the register.
+ * Refer to "stream2mmio_public.h" for details.
+ */
+STORAGE_CLASS_STREAM2MMIO_C void stream2mmio_reg_store(
+    const stream2mmio_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value)
+{
+	assert(ID < N_STREAM2MMIO_ID);
+	assert(STREAM2MMIO_CTRL_BASE[ID] != (hrt_address)-1);
+
+	ia_css_device_store_uint32(STREAM2MMIO_CTRL_BASE[ID] +
+				   reg * sizeof(hrt_data), value);
+}
+
+/* end of DLI */
+
+#endif /* __ISYS_STREAM2MMIO_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_local.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_local.h
new file mode 100644
index 000000000..efaa4da8d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_local.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PIXELGEN_LOCAL_H_INCLUDED__
+#define __PIXELGEN_LOCAL_H_INCLUDED__
+
+#include "pixelgen_global.h"
+
+typedef struct pixelgen_ctrl_state_s	pixelgen_ctrl_state_t;
+struct pixelgen_ctrl_state_s {
+	hrt_data	com_enable;
+	hrt_data	prbs_rstval0;
+	hrt_data	prbs_rstval1;
+	hrt_data	syng_sid;
+	hrt_data	syng_free_run;
+	hrt_data	syng_pause;
+	hrt_data	syng_nof_frames;
+	hrt_data	syng_nof_pixels;
+	hrt_data	syng_nof_line;
+	hrt_data	syng_hblank_cyc;
+	hrt_data	syng_vblank_cyc;
+	hrt_data	syng_stat_hcnt;
+	hrt_data	syng_stat_vcnt;
+	hrt_data	syng_stat_fcnt;
+	hrt_data	syng_stat_done;
+	hrt_data	tpg_mode;
+	hrt_data	tpg_hcnt_mask;
+	hrt_data	tpg_vcnt_mask;
+	hrt_data	tpg_xycnt_mask;
+	hrt_data	tpg_hcnt_delta;
+	hrt_data	tpg_vcnt_delta;
+	hrt_data	tpg_r1;
+	hrt_data	tpg_g1;
+	hrt_data	tpg_b1;
+	hrt_data	tpg_r2;
+	hrt_data	tpg_g2;
+	hrt_data	tpg_b2;
+};
+#endif /* __PIXELGEN_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_private.h b/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_private.h
new file mode 100644
index 000000000..1c7938d8c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/host/pixelgen_private.h
@@ -0,0 +1,184 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PIXELGEN_PRIVATE_H_INCLUDED__
+#define __PIXELGEN_PRIVATE_H_INCLUDED__
+#include "pixelgen_public.h"
+#include "PixelGen_SysBlock_defs.h"
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+#include "assert_support.h" /* assert */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Refer to "pixelgen_public.h" for details.
+ */
+STORAGE_CLASS_PIXELGEN_C hrt_data pixelgen_ctrl_reg_load(
+    const pixelgen_ID_t ID,
+    const hrt_address reg)
+{
+	assert(ID < N_PIXELGEN_ID);
+	assert(PIXELGEN_CTRL_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(PIXELGEN_CTRL_BASE[ID] + reg * sizeof(
+					     hrt_data));
+}
+
+/**
+ * @brief Store a value to the register.
+ * Refer to "pixelgen_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_PIXELGEN_C void pixelgen_ctrl_reg_store(
+    const pixelgen_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value)
+{
+	assert(ID < N_PIXELGEN_ID);
+	assert(PIXELGEN_CTRL_BASE[ID] != (hrt_address)-1);
+
+	ia_css_device_store_uint32(PIXELGEN_CTRL_BASE[ID] + reg * sizeof(hrt_data),
+				   value);
+}
+
+/* end of DLI */
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the pixelgen state.
+ * Refer to "pixelgen_public.h" for details.
+ */
+STORAGE_CLASS_PIXELGEN_C void pixelgen_ctrl_get_state(
+    const pixelgen_ID_t ID,
+    pixelgen_ctrl_state_t *state)
+{
+	state->com_enable =
+	    pixelgen_ctrl_reg_load(ID, _PXG_COM_ENABLE_REG_IDX);
+	state->prbs_rstval0 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_PRBS_RSTVAL_REG0_IDX);
+	state->prbs_rstval1 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_PRBS_RSTVAL_REG1_IDX);
+	state->syng_sid =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_SID_REG_IDX);
+	state->syng_free_run =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_FREE_RUN_REG_IDX);
+	state->syng_pause =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_PAUSE_REG_IDX);
+	state->syng_nof_frames =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_NOF_FRAME_REG_IDX);
+	state->syng_nof_pixels =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_NOF_PIXEL_REG_IDX);
+	state->syng_nof_line =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_NOF_LINE_REG_IDX);
+	state->syng_hblank_cyc =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_HBLANK_CYC_REG_IDX);
+	state->syng_vblank_cyc =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_VBLANK_CYC_REG_IDX);
+	state->syng_stat_hcnt =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_STAT_HCNT_REG_IDX);
+	state->syng_stat_vcnt =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_STAT_VCNT_REG_IDX);
+	state->syng_stat_fcnt =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_STAT_FCNT_REG_IDX);
+	state->syng_stat_done =
+	    pixelgen_ctrl_reg_load(ID, _PXG_SYNG_STAT_DONE_REG_IDX);
+	state->tpg_mode =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_MODE_REG_IDX);
+	state->tpg_hcnt_mask =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_HCNT_MASK_REG_IDX);
+	state->tpg_vcnt_mask =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_VCNT_MASK_REG_IDX);
+	state->tpg_xycnt_mask =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_XYCNT_MASK_REG_IDX);
+	state->tpg_hcnt_delta =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_HCNT_DELTA_REG_IDX);
+	state->tpg_vcnt_delta =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_VCNT_DELTA_REG_IDX);
+	state->tpg_r1 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_R1_REG_IDX);
+	state->tpg_g1 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_G1_REG_IDX);
+	state->tpg_b1 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_B1_REG_IDX);
+	state->tpg_r2 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_R2_REG_IDX);
+	state->tpg_g2 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_G2_REG_IDX);
+	state->tpg_b2 =
+	    pixelgen_ctrl_reg_load(ID, _PXG_TPG_B2_REG_IDX);
+}
+
+/**
+ * @brief Dump the pixelgen state.
+ * Refer to "pixelgen_public.h" for details.
+ */
+STORAGE_CLASS_PIXELGEN_C void pixelgen_ctrl_dump_state(
+    const pixelgen_ID_t ID,
+    pixelgen_ctrl_state_t *state)
+{
+	ia_css_print("Pixel Generator ID %d Enable  0x%x\n", ID, state->com_enable);
+	ia_css_print("Pixel Generator ID %d PRBS reset value 0 0x%x\n", ID,
+		     state->prbs_rstval0);
+	ia_css_print("Pixel Generator ID %d PRBS reset value 1 0x%x\n", ID,
+		     state->prbs_rstval1);
+	ia_css_print("Pixel Generator ID %d SYNC SID 0x%x\n", ID, state->syng_sid);
+	ia_css_print("Pixel Generator ID %d syng free run 0x%x\n", ID,
+		     state->syng_free_run);
+	ia_css_print("Pixel Generator ID %d syng pause 0x%x\n", ID, state->syng_pause);
+	ia_css_print("Pixel Generator ID %d syng no of frames 0x%x\n", ID,
+		     state->syng_nof_frames);
+	ia_css_print("Pixel Generator ID %d syng no of pixels 0x%x\n", ID,
+		     state->syng_nof_pixels);
+	ia_css_print("Pixel Generator ID %d syng no of line 0x%x\n", ID,
+		     state->syng_nof_line);
+	ia_css_print("Pixel Generator ID %d syng hblank cyc  0x%x\n", ID,
+		     state->syng_hblank_cyc);
+	ia_css_print("Pixel Generator ID %d syng vblank cyc  0x%x\n", ID,
+		     state->syng_vblank_cyc);
+	ia_css_print("Pixel Generator ID %d syng stat hcnt  0x%x\n", ID,
+		     state->syng_stat_hcnt);
+	ia_css_print("Pixel Generator ID %d syng stat vcnt  0x%x\n", ID,
+		     state->syng_stat_vcnt);
+	ia_css_print("Pixel Generator ID %d syng stat fcnt  0x%x\n", ID,
+		     state->syng_stat_fcnt);
+	ia_css_print("Pixel Generator ID %d syng stat done  0x%x\n", ID,
+		     state->syng_stat_done);
+	ia_css_print("Pixel Generator ID %d tpg modee  0x%x\n", ID, state->tpg_mode);
+	ia_css_print("Pixel Generator ID %d tpg hcnt mask  0x%x\n", ID,
+		     state->tpg_hcnt_mask);
+	ia_css_print("Pixel Generator ID %d tpg hcnt mask  0x%x\n", ID,
+		     state->tpg_hcnt_mask);
+	ia_css_print("Pixel Generator ID %d tpg xycnt mask  0x%x\n", ID,
+		     state->tpg_xycnt_mask);
+	ia_css_print("Pixel Generator ID %d tpg hcnt delta  0x%x\n", ID,
+		     state->tpg_hcnt_delta);
+	ia_css_print("Pixel Generator ID %d tpg vcnt delta  0x%x\n", ID,
+		     state->tpg_vcnt_delta);
+	ia_css_print("Pixel Generator ID %d tpg r1 0x%x\n", ID, state->tpg_r1);
+	ia_css_print("Pixel Generator ID %d tpg g1 0x%x\n", ID, state->tpg_g1);
+	ia_css_print("Pixel Generator ID %d tpg b1 0x%x\n", ID, state->tpg_b1);
+	ia_css_print("Pixel Generator ID %d tpg r2 0x%x\n", ID, state->tpg_r2);
+	ia_css_print("Pixel Generator ID %d tpg g2 0x%x\n", ID, state->tpg_g2);
+	ia_css_print("Pixel Generator ID %d tpg b2 0x%x\n", ID, state->tpg_b2);
+}
+
+/* end of NCI */
+#endif /* __PIXELGEN_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/PixelGen_SysBlock_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/PixelGen_SysBlock_defs.h
new file mode 100644
index 000000000..ae471dd51
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/PixelGen_SysBlock_defs.h
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _PixelGen_SysBlock_defs_h
+#define _PixelGen_SysBlock_defs_h
+
+/* Parematers and User_Parameters for HSS */
+#define _PXG_PPC                       Ppc
+#define _PXG_PIXEL_BITS                PixelWidth
+#define _PXG_MAX_NOF_SID               MaxNofSids
+#define _PXG_DATA_BITS                 DataWidth
+#define _PXG_CNT_BITS                  CntWidth
+#define _PXG_FIFODEPTH                 FifoDepth
+#define _PXG_DBG                       Dbg_device_not_included
+
+/* ID's and Address */
+#define _PXG_ADRRESS_ALIGN_REG         4
+
+#define _PXG_COM_ENABLE_REG_IDX        0
+#define _PXG_PRBS_RSTVAL_REG0_IDX      1
+#define _PXG_PRBS_RSTVAL_REG1_IDX      2
+#define _PXG_SYNG_SID_REG_IDX          3
+#define _PXG_SYNG_FREE_RUN_REG_IDX     4
+#define _PXG_SYNG_PAUSE_REG_IDX        5
+#define _PXG_SYNG_NOF_FRAME_REG_IDX    6
+#define _PXG_SYNG_NOF_PIXEL_REG_IDX    7
+#define _PXG_SYNG_NOF_LINE_REG_IDX     8
+#define _PXG_SYNG_HBLANK_CYC_REG_IDX   9
+#define _PXG_SYNG_VBLANK_CYC_REG_IDX  10
+#define _PXG_SYNG_STAT_HCNT_REG_IDX   11
+#define _PXG_SYNG_STAT_VCNT_REG_IDX   12
+#define _PXG_SYNG_STAT_FCNT_REG_IDX   13
+#define _PXG_SYNG_STAT_DONE_REG_IDX   14
+#define _PXG_TPG_MODE_REG_IDX         15
+#define _PXG_TPG_HCNT_MASK_REG_IDX    16
+#define _PXG_TPG_VCNT_MASK_REG_IDX    17
+#define _PXG_TPG_XYCNT_MASK_REG_IDX   18
+#define _PXG_TPG_HCNT_DELTA_REG_IDX   19
+#define _PXG_TPG_VCNT_DELTA_REG_IDX   20
+#define _PXG_TPG_R1_REG_IDX           21
+#define _PXG_TPG_G1_REG_IDX           22
+#define _PXG_TPG_B1_REG_IDX           23
+#define _PXG_TPG_R2_REG_IDX           24
+#define _PXG_TPG_G2_REG_IDX           25
+#define _PXG_TPG_B2_REG_IDX           26
+/* */
+#define _PXG_SYNG_PAUSE_CYCLES        0
+/* Subblock ID's */
+#define _PXG_DISABLE_IDX              0
+#define _PXG_PRBS_IDX                 0
+#define _PXG_TPG_IDX                  1
+#define _PXG_SYNG_IDX                 2
+#define _PXG_SMUX_IDX                 3
+/* Register Widths */
+#define _PXG_COM_ENABLE_REG_WIDTH     2
+#define _PXG_COM_SRST_REG_WIDTH       4
+#define _PXG_PRBS_RSTVAL_REG0_WIDTH  31
+#define _PXG_PRBS_RSTVAL_REG1_WIDTH  31
+
+#define _PXG_SYNG_SID_REG_WIDTH        3
+
+#define _PXG_SYNG_FREE_RUN_REG_WIDTH   1
+#define _PXG_SYNG_PAUSE_REG_WIDTH      1
+/*
+#define _PXG_SYNG_NOF_FRAME_REG_WIDTH  <sync_gen_cnt_width>
+#define _PXG_SYNG_NOF_PIXEL_REG_WIDTH  <sync_gen_cnt_width>
+#define _PXG_SYNG_NOF_LINE_REG_WIDTH   <sync_gen_cnt_width>
+#define _PXG_SYNG_HBLANK_CYC_REG_WIDTH <sync_gen_cnt_width>
+#define _PXG_SYNG_VBLANK_CYC_REG_WIDTH <sync_gen_cnt_width>
+#define _PXG_SYNG_STAT_HCNT_REG_WIDTH  <sync_gen_cnt_width>
+#define _PXG_SYNG_STAT_VCNT_REG_WIDTH  <sync_gen_cnt_width>
+#define _PXG_SYNG_STAT_FCNT_REG_WIDTH  <sync_gen_cnt_width>
+*/
+#define _PXG_SYNG_STAT_DONE_REG_WIDTH  1
+#define _PXG_TPG_MODE_REG_WIDTH        2
+/*
+#define _PXG_TPG_HCNT_MASK_REG_WIDTH   <sync_gen_cnt_width>
+#define _PXG_TPG_VCNT_MASK_REG_WIDTH   <sync_gen_cnt_width>
+#define _PXG_TPG_XYCNT_MASK_REG_WIDTH  <pixle_width>
+*/
+#define _PXG_TPG_HCNT_DELTA_REG_WIDTH  4
+#define _PXG_TPG_VCNT_DELTA_REG_WIDTH  4
+/*
+#define _PXG_TPG_R1_REG_WIDTH          <pixle_width>
+#define _PXG_TPG_G1_REG_WIDTH          <pixle_width>
+#define _PXG_TPG_B1_REG_WIDTH          <pixle_width>
+#define _PXG_TPG_R2_REG_WIDTH          <pixle_width>
+#define _PXG_TPG_G2_REG_WIDTH          <pixle_width>
+#define _PXG_TPG_B2_REG_WIDTH          <pixle_width>
+*/
+#define _PXG_FIFO_DEPTH                2
+/* MISC */
+#define _PXG_ENABLE_REG_VAL            1
+#define _PXG_PRBS_ENABLE_REG_VAL       1
+#define _PXG_TPG_ENABLE_REG_VAL        2
+#define _PXG_SYNG_ENABLE_REG_VAL       4
+#define _PXG_FIFO_ENABLE_REG_VAL       8
+#define _PXG_PXL_BITS                 14
+#define _PXG_INVALID_FLAG              0xDEADBEEF
+#define _PXG_CAFE_FLAG                 0xCAFEBABE
+
+#endif /* _PixelGen_SysBlock_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/ibuf_cntrl_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/ibuf_cntrl_defs.h
new file mode 100644
index 000000000..374466e6b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/ibuf_cntrl_defs.h
@@ -0,0 +1,135 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _ibuf_cntrl_defs_h_
+#define _ibuf_cntrl_defs_h_
+
+#include <stream2mmio_defs.h>
+#include <dma_v2_defs.h>
+
+#define _IBUF_CNTRL_REG_ALIGN 4
+/* alignment of register banks, first bank are shared configuration and status registers: */
+#define _IBUF_CNTRL_PROC_REG_ALIGN        32
+
+/* the actual amount of configuration registers per proc: */
+#define _IBUF_CNTRL_CONFIG_REGS_PER_PROC 18
+/* the actual amount of shared configuration registers: */
+#define _IBUF_CNTRL_CONFIG_REGS_NO_PROC  0
+
+/* the actual amount of status registers per proc */
+#define _IBUF_CNTRL_STATUS_REGS_PER_PROC (_IBUF_CNTRL_CONFIG_REGS_PER_PROC + 10)
+/* the actual amount shared status registers */
+#define _IBUF_CNTRL_STATUS_REGS_NO_PROC  (_IBUF_CNTRL_CONFIG_REGS_NO_PROC + 2)
+
+/* time out bits, maximum time out value is 2^_IBUF_CNTRL_TIME_OUT_BITS - 1 */
+#define _IBUF_CNTRL_TIME_OUT_BITS         5
+
+/* command token definition */
+#define _IBUF_CNTRL_CMD_TOKEN_LSB          0
+#define _IBUF_CNTRL_CMD_TOKEN_MSB          1
+
+/* Str2MMIO defines */
+#define _IBUF_CNTRL_STREAM2MMIO_CMD_TOKEN_MSB        _STREAM2MMIO_CMD_TOKEN_CMD_MSB
+#define _IBUF_CNTRL_STREAM2MMIO_CMD_TOKEN_LSB        _STREAM2MMIO_CMD_TOKEN_CMD_LSB
+#define _IBUF_CNTRL_STREAM2MMIO_NUM_ITEMS_BITS       _STREAM2MMIO_PACK_NUM_ITEMS_BITS
+#define _IBUF_CNTRL_STREAM2MMIO_ACK_EOF_BIT          _STREAM2MMIO_PACK_ACK_EOF_BIT
+#define _IBUF_CNTRL_STREAM2MMIO_ACK_TOKEN_VALID_BIT  _STREAM2MMIO_ACK_TOKEN_VALID_BIT
+
+/* acknowledge token definition */
+#define _IBUF_CNTRL_ACK_TOKEN_STORES_IDX    0
+#define _IBUF_CNTRL_ACK_TOKEN_STORES_BITS   15
+#define _IBUF_CNTRL_ACK_TOKEN_ITEMS_IDX     (_IBUF_CNTRL_ACK_TOKEN_STORES_BITS + _IBUF_CNTRL_ACK_TOKEN_STORES_IDX)
+#define _IBUF_CNTRL_ACK_TOKEN_ITEMS_BITS    _STREAM2MMIO_PACK_NUM_ITEMS_BITS
+#define _IBUF_CNTRL_ACK_TOKEN_LSB          _IBUF_CNTRL_ACK_TOKEN_STORES_IDX
+#define _IBUF_CNTRL_ACK_TOKEN_MSB          (_IBUF_CNTRL_ACK_TOKEN_ITEMS_BITS + _IBUF_CNTRL_ACK_TOKEN_ITEMS_IDX - 1)
+/* bit 31 indicates a valid ack: */
+#define _IBUF_CNTRL_ACK_TOKEN_VALID_BIT    (_IBUF_CNTRL_ACK_TOKEN_ITEMS_BITS + _IBUF_CNTRL_ACK_TOKEN_ITEMS_IDX)
+
+/*shared registers:*/
+#define _IBUF_CNTRL_RECALC_WORDS_STATUS     0
+#define _IBUF_CNTRL_ARBITERS_STATUS         1
+
+#define _IBUF_CNTRL_SET_CRUN                2 /* NO PHYSICAL REGISTER!! Only used in HSS model */
+
+/*register addresses for each proc: */
+#define _IBUF_CNTRL_CMD                   0
+#define _IBUF_CNTRL_ACK                   1
+
+/* number of items (packets or words) per frame: */
+#define _IBUF_CNTRL_NUM_ITEMS_PER_STORE   2
+
+/* number of stores (packets or words) per store/buffer: */
+#define _IBUF_CNTRL_NUM_STORES_PER_FRAME  3
+
+/* the channel and command in the DMA */
+#define _IBUF_CNTRL_DMA_CHANNEL           4
+#define _IBUF_CNTRL_DMA_CMD               5
+
+/* the start address and stride of the buffers */
+#define _IBUF_CNTRL_BUFFER_START_ADDRESS  6
+#define _IBUF_CNTRL_BUFFER_STRIDE         7
+#define _IBUF_CNTRL_BUFFER_END_ADDRESS    8
+
+/* destination start address, stride and end address; should be the same as in the DMA */
+#define _IBUF_CNTRL_DEST_START_ADDRESS    9
+#define _IBUF_CNTRL_DEST_STRIDE           10
+#define _IBUF_CNTRL_DEST_END_ADDRESS      11
+
+/* send a frame sync or not, default 1 */
+#define _IBUF_CNTRL_SYNC_FRAME            12
+
+/* str2mmio cmds */
+#define _IBUF_CNTRL_STR2MMIO_SYNC_CMD     13
+#define _IBUF_CNTRL_STR2MMIO_STORE_CMD    14
+
+/* num elems p word*/
+#define _IBUF_CNTRL_SHIFT_ITEMS           15
+#define _IBUF_CNTRL_ELEMS_P_WORD_IBUF     16
+#define _IBUF_CNTRL_ELEMS_P_WORD_DEST     17
+
+/* STATUS */
+/* current frame and stores in buffer */
+#define _IBUF_CNTRL_CUR_STORES            18
+#define _IBUF_CNTRL_CUR_ACKS              19
+
+/* current buffer and destination address for DMA cmd's */
+#define _IBUF_CNTRL_CUR_S2M_IBUF_ADDR     20
+#define _IBUF_CNTRL_CUR_DMA_IBUF_ADDR     21
+#define _IBUF_CNTRL_CUR_DMA_DEST_ADDR     22
+#define _IBUF_CNTRL_CUR_ISP_DEST_ADDR     23
+
+#define _IBUF_CNTRL_CUR_NR_DMA_CMDS_SEND  24
+
+#define _IBUF_CNTRL_MAIN_CNTRL_STATE      25
+#define _IBUF_CNTRL_DMA_SYNC_STATE        26
+#define _IBUF_CNTRL_ISP_SYNC_STATE        27
+
+/*Commands: */
+#define _IBUF_CNTRL_CMD_STORE_FRAME_IDX     0
+#define _IBUF_CNTRL_CMD_ONLINE_IDX          1
+
+/* initialize, copy st_addr to cur_addr etc */
+#define _IBUF_CNTRL_CMD_INITIALIZE          0
+
+/* store an online frame (sync with ISP, use end cfg start, stride and end address: */
+#define _IBUF_CNTRL_CMD_STORE_ONLINE_FRAME  ((1 << _IBUF_CNTRL_CMD_STORE_FRAME_IDX) | (1 << _IBUF_CNTRL_CMD_ONLINE_IDX))
+
+/* store an offline frame (don't sync with ISP, requires start address as 2nd token, no end address: */
+#define _IBUF_CNTRL_CMD_STORE_OFFLINE_FRAME  BIT(_IBUF_CNTRL_CMD_STORE_FRAME_IDX)
+
+/* false command token, should be different then commands. Use online bit, not store frame: */
+#define _IBUF_CNTRL_FALSE_ACK               2
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_common_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_common_defs.h
new file mode 100644
index 000000000..ac8be2d49
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_common_defs.h
@@ -0,0 +1,206 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _css_receiver_2400_common_defs_h_
+#define _css_receiver_2400_common_defs_h_
+#ifndef _mipi_backend_common_defs_h_
+#define _mipi_backend_common_defs_h_
+
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_DATA_WIDTH     16
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_CH_ID_WIDTH     2
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_FMT_TYPE_WIDTH  3
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_REAL_WIDTH (_HRT_CSS_RECEIVER_2400_GEN_SHORT_DATA_WIDTH + _HRT_CSS_RECEIVER_2400_GEN_SHORT_CH_ID_WIDTH + _HRT_CSS_RECEIVER_2400_GEN_SHORT_FMT_TYPE_WIDTH)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_WIDTH      32 /* use 32 to be compatibel with streaming monitor !, MSB's of interface are tied to '0' */
+
+/* Definition of data format ID at the interface CSS_receiver capture/acquisition units */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8          24   /* 01 1000 YUV420 8-bit                                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_10         25   /* 01 1001  YUV420 10-bit                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8L         26   /* 01 1010   YUV420 8-bit legacy                               */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV422_8          30   /* 01 1110   YUV422 8-bit                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV422_10         31   /* 01 1111   YUV422 10-bit                                     */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB444            32   /* 10 0000   RGB444                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB555            33   /* 10 0001   RGB555                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB565            34   /* 10 0010   RGB565                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB666            35   /* 10 0011   RGB666                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB888            36   /* 10 0100   RGB888                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW6              40   /* 10 1000   RAW6                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW7              41   /* 10 1001   RAW7                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW8              42   /* 10 1010   RAW8                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW10             43   /* 10 1011   RAW10                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW12             44   /* 10 1100   RAW12                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW14             45   /* 10 1101   RAW14                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_1         48   /* 11 0000    JPEG [User Defined 8-bit Data Type 1]            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_2         49   /* 11 0001    User Defined 8-bit Data Type 2                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_3         50   /* 11 0010    User Defined 8-bit Data Type 3                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_4         51   /* 11 0011    User Defined 8-bit Data Type 4                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_5         52   /* 11 0100    User Defined 8-bit Data Type 5                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_6         53   /* 11 0101    User Defined 8-bit Data Type 6                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_7         54   /* 11 0110    User Defined 8-bit Data Type 7                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_8         55   /* 11 0111    User Defined 8-bit Data Type 8                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_Emb               18   /* 01 0010    embedded eight bit non image data                */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_SOF                0   /* 00 0000    frame start                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_EOF                1   /* 00 0001    frame end                                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_SOL                2   /* 00 0010    line start                                       */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_EOL                3   /* 00 0011    line end                                         */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH1            8   /* 00 1000  Generic Short Packet Code 1                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH2            9   /* 00 1001    Generic Short Packet Code 2                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH3           10   /* 00 1010    Generic Short Packet Code 3                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH4           11   /* 00 1011    Generic Short Packet Code 4                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH5           12   /* 00 1100    Generic Short Packet Code 5                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH6           13   /* 00 1101    Generic Short Packet Code 6                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH7           14   /* 00 1110    Generic Short Packet Code 7                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH8           15   /* 00 1111    Generic Short Packet Code 8                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8_CSPS     28   /* 01 1100   YUV420 8-bit (Chroma Shifted Pixel Sampling)      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_10_CSPS    29   /* 01 1101   YUV420 10-bit (Chroma Shifted Pixel Sampling)     */
+/* used reserved mipi positions for these */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW16             46
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18             47
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18_2           37
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18_3           38
+
+//_HRT_CSS_RECEIVER_2400_FMT_TYPE_CUSTOM 63
+#define _HRT_MIPI_BACKEND_FMT_TYPE_CUSTOM                       63
+
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_WIDTH              6
+
+/* Definition of format_types at the interface CSS --> input_selector*/
+/* !! Changes here should be copied to systems/isp/isp_css/bin/conv_transmitter_cmd.tcl !! */
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB888           0  // 36 'h24
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB555           1  // 33 'h
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB444           2  // 32
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB565           3  // 34
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB666           4  // 35
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW8             5  // 42
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW10            6  // 43
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW6             7  // 40
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW7             8  // 41
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW12            9  // 43
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW14           10  // 45
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8        11  // 30
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_10       12  // 25
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV422_8        13  // 30
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV422_10       14  // 31
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_1       15  // 48
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8L       16  // 26
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_Emb             17  // 18
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_2       18  // 49
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_3       19  // 50
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_4       20  // 51
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_5       21  // 52
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_6       22  // 53
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_7       23  // 54
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_8       24  // 55
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8_CSPS   25  // 28
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_10_CSPS  26  // 29
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW16           27  // ?
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18           28  // ?
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18_2         29  // ? Option 2 for depacketiser
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18_3         30  // ? Option 3 for depacketiser
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_CUSTOM          31  // to signal custom decoding
+
+/* definition for state machine of data FIFO for decode different type of data */
+#define _HRT_CSS_RECEIVER_2400_YUV420_8_REPEAT_PTN                 1
+#define _HRT_CSS_RECEIVER_2400_YUV420_10_REPEAT_PTN                5
+#define _HRT_CSS_RECEIVER_2400_YUV420_8L_REPEAT_PTN                1
+#define _HRT_CSS_RECEIVER_2400_YUV422_8_REPEAT_PTN                 1
+#define _HRT_CSS_RECEIVER_2400_YUV422_10_REPEAT_PTN                5
+#define _HRT_CSS_RECEIVER_2400_RGB444_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB555_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB565_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB666_REPEAT_PTN                   9
+#define _HRT_CSS_RECEIVER_2400_RGB888_REPEAT_PTN                   3
+#define _HRT_CSS_RECEIVER_2400_RAW6_REPEAT_PTN                     3
+#define _HRT_CSS_RECEIVER_2400_RAW7_REPEAT_PTN                     7
+#define _HRT_CSS_RECEIVER_2400_RAW8_REPEAT_PTN                     1
+#define _HRT_CSS_RECEIVER_2400_RAW10_REPEAT_PTN                    5
+#define _HRT_CSS_RECEIVER_2400_RAW12_REPEAT_PTN                    3
+#define _HRT_CSS_RECEIVER_2400_RAW14_REPEAT_PTN                    7
+
+#define _HRT_CSS_RECEIVER_2400_MAX_REPEAT_PTN                      _HRT_CSS_RECEIVER_2400_RGB666_REPEAT_PTN
+
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FMT_IDX                     0
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FMT_WIDTH                   3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_PRED_IDX                    3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_PRED_WIDTH                  1
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_USD_BITS                    4  /* bits per USD type */
+
+#define _HRT_CSS_RECEIVER_2400_BE_RAW16_DATAID_IDX                 0
+#define _HRT_CSS_RECEIVER_2400_BE_RAW16_EN_IDX                     6
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_DATAID_IDX                 0
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_OPTION_IDX                 6
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_EN_IDX                     8
+
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_NO_COMP                     0
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_6_10                     1
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_7_10                     2
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_8_10                     3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_6_12                     4
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_7_12                     5
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_8_12                     6
+
+/* packet bit definition */
+#define _HRT_CSS_RECEIVER_2400_PKT_SOP_IDX                        32
+#define _HRT_CSS_RECEIVER_2400_PKT_SOP_BITS                        1
+#define _HRT_CSS_RECEIVER_2400_PKT_CH_ID_IDX                      22
+#define _HRT_CSS_RECEIVER_2400_PKT_CH_ID_BITS                      2
+#define _HRT_CSS_RECEIVER_2400_PKT_FMT_ID_IDX                     16
+#define _HRT_CSS_RECEIVER_2400_PKT_FMT_ID_BITS                     6
+#define _HRT_CSS_RECEIVER_2400_PH_DATA_FIELD_IDX                   0
+#define _HRT_CSS_RECEIVER_2400_PH_DATA_FIELD_BITS                 16
+#define _HRT_CSS_RECEIVER_2400_PKT_PAYLOAD_IDX                     0
+#define _HRT_CSS_RECEIVER_2400_PKT_PAYLOAD_BITS                   32
+
+/*************************************************************************************************/
+/* Custom Decoding                                                                               */
+/* These Custom Defs are defined based on design-time config in "mipi_backend_pixel_formatter.chdl" !! */
+/*************************************************************************************************/
+/*
+#define BE_CUST_EN_IDX                     0     // 2bits
+#define BE_CUST_EN_DATAID_IDX              2     // 6bits MIPI DATA ID
+#define BE_CUST_EN_WIDTH                   8
+#define BE_CUST_MODE_ALL                   1     // Enable Custom Decoding for all DATA IDs
+#define BE_CUST_MODE_ONE                   3     // Enable Custom Decoding for ONE DATA ID, programmed in CUST_EN_DATA_ID
+
+// Data State config = {get_bits(6bits), valid(1bit)}  //
+#define BE_CUST_DATA_STATE_S0_IDX          0     // 7bits
+#define BE_CUST_DATA_STATE_S1_IDX          8 //7      // 7bits
+#define BE_CUST_DATA_STATE_S2_IDX          16//14    // 7bits /
+#define BE_CUST_DATA_STATE_WIDTH           24//21
+#define BE_CUST_DATA_STATE_VALID_IDX       0     // 1bits
+#define BE_CUST_DATA_STATE_GETBITS_IDX     1     // 6bits
+
+// Pixel Extractor config
+#define BE_CUST_PIX_EXT_DATA_ALIGN_IDX     0     // 6bits
+#define BE_CUST_PIX_EXT_PIX_ALIGN_IDX      6//5     // 5bits
+#define BE_CUST_PIX_EXT_PIX_MASK_IDX       11//10    // 18bits
+#define BE_CUST_PIX_EXT_PIX_EN_IDX         29 //28    // 1bits
+
+#define BE_CUST_PIX_EXT_WIDTH              30//29
+
+// Pixel Valid & EoP config = {[eop,valid](especial), [eop,valid](normal)}
+#define BE_CUST_PIX_VALID_EOP_P0_IDX        0    // 4bits
+#define BE_CUST_PIX_VALID_EOP_P1_IDX        4    // 4bits
+#define BE_CUST_PIX_VALID_EOP_P2_IDX        8    // 4bits
+#define BE_CUST_PIX_VALID_EOP_P3_IDX        12   // 4bits
+#define BE_CUST_PIX_VALID_EOP_WIDTH         16
+#define BE_CUST_PIX_VALID_EOP_NOR_VALID_IDX 0    // Normal (NO less get_bits case) Valid - 1bits
+#define BE_CUST_PIX_VALID_EOP_NOR_EOP_IDX   1    // Normal (NO less get_bits case) EoP - 1bits
+#define BE_CUST_PIX_VALID_EOP_ESP_VALID_IDX 2    // Especial (less get_bits case) Valid - 1bits
+#define BE_CUST_PIX_VALID_EOP_ESP_EOP_IDX   3    // Especial (less get_bits case) EoP - 1bits
+
+*/
+
+#endif /* _mipi_backend_common_defs_h_ */
+#endif /* _css_receiver_2400_common_defs_h_ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_defs.h
new file mode 100644
index 000000000..6fae1c262
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/mipi_backend_defs.h
@@ -0,0 +1,209 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _mipi_backend_defs_h
+#define _mipi_backend_defs_h
+
+#include "mipi_backend_common_defs.h"
+
+#define MIPI_BACKEND_REG_ALIGN                    4 // assuming 32 bit control bus width
+
+#define _HRT_MIPI_BACKEND_NOF_IRQS                         3 // sid_lut
+
+// SH Backend Register IDs
+#define _HRT_MIPI_BACKEND_ENABLE_REG_IDX                   0
+#define _HRT_MIPI_BACKEND_STATUS_REG_IDX                   1
+//#define _HRT_MIPI_BACKEND_HIGH_PREC_REG_IDX                2
+#define _HRT_MIPI_BACKEND_COMP_FORMAT_REG0_IDX             2
+#define _HRT_MIPI_BACKEND_COMP_FORMAT_REG1_IDX             3
+#define _HRT_MIPI_BACKEND_COMP_FORMAT_REG2_IDX             4
+#define _HRT_MIPI_BACKEND_COMP_FORMAT_REG3_IDX             5
+#define _HRT_MIPI_BACKEND_RAW16_CONFIG_REG_IDX             6
+#define _HRT_MIPI_BACKEND_RAW18_CONFIG_REG_IDX             7
+#define _HRT_MIPI_BACKEND_FORCE_RAW8_REG_IDX               8
+#define _HRT_MIPI_BACKEND_IRQ_STATUS_REG_IDX               9
+#define _HRT_MIPI_BACKEND_IRQ_CLEAR_REG_IDX               10
+////
+#define _HRT_MIPI_BACKEND_CUST_EN_REG_IDX                 11
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_REG_IDX         12
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S0P0_REG_IDX       13
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S0P1_REG_IDX       14
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S0P2_REG_IDX       15
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S0P3_REG_IDX       16
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S1P0_REG_IDX       17
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S1P1_REG_IDX       18
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S1P2_REG_IDX       19
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S1P3_REG_IDX       20
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S2P0_REG_IDX       21
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S2P1_REG_IDX       22
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S2P2_REG_IDX       23
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_S2P3_REG_IDX       24
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_REG_IDX      25
+////
+#define _HRT_MIPI_BACKEND_GLOBAL_LUT_DISREGARD_REG_IDX    26
+#define _HRT_MIPI_BACKEND_PKT_STALL_STATUS_REG_IDX        27
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENABLE_REG_IDX           28
+#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_0_REG_IDX          28
+#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_1_REG_IDX          29
+#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_2_REG_IDX          30
+#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_3_REG_IDX          31
+
+#define _HRT_MIPI_BACKEND_NOF_REGISTERS                   32 // excluding the LP LUT entries
+
+#define _HRT_MIPI_BACKEND_LP_LUT_ENTRY_0_REG_IDX          32
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+#define _HRT_MIPI_BACKEND_ENABLE_REG_WIDTH                 1
+#define _HRT_MIPI_BACKEND_STATUS_REG_WIDTH                 1
+//#define _HRT_MIPI_BACKEND_HIGH_PREC_REG_WIDTH              1
+#define _HRT_MIPI_BACKEND_COMP_FORMAT_REG_WIDTH           32
+#define _HRT_MIPI_BACKEND_RAW16_CONFIG_REG_WIDTH           7
+#define _HRT_MIPI_BACKEND_RAW18_CONFIG_REG_WIDTH           9
+#define _HRT_MIPI_BACKEND_FORCE_RAW8_REG_WIDTH             8
+#define _HRT_MIPI_BACKEND_IRQ_STATUS_REG_WIDTH            _HRT_MIPI_BACKEND_NOF_IRQS
+#define _HRT_MIPI_BACKEND_IRQ_CLEAR_REG_WIDTH              0
+#define _HRT_MIPI_BACKEND_GLOBAL_LUT_DISREGARD_REG_WIDTH   1
+#define _HRT_MIPI_BACKEND_PKT_STALL_STATUS_REG_WIDTH       1 + 2 + 6
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENABLE_REG_WIDTH          1
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_0_REG_WIDTH         7
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_1_REG_WIDTH         7
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_2_REG_WIDTH         7
+//#define _HRT_MIPI_BACKEND_SP_LUT_ENTRY_3_REG_WIDTH         7
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define _HRT_MIPI_BACKEND_NOF_SP_LUT_ENTRIES               4
+
+//#define _HRT_MIPI_BACKEND_MAX_NOF_LP_LUT_ENTRIES           16  // to satisfy hss model static array declaration
+
+#define _HRT_MIPI_BACKEND_CHANNEL_ID_WIDTH                 2
+#define _HRT_MIPI_BACKEND_FORMAT_TYPE_WIDTH                6
+#define _HRT_MIPI_BACKEND_PACKET_ID_WIDTH                  _HRT_MIPI_BACKEND_CHANNEL_ID_WIDTH + _HRT_MIPI_BACKEND_FORMAT_TYPE_WIDTH
+
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_A_LSB                 0
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_A_MSB(pix_width)     (_HRT_MIPI_BACKEND_STREAMING_PIX_A_LSB + (pix_width) - 1)
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_A_VAL_BIT(pix_width) (_HRT_MIPI_BACKEND_STREAMING_PIX_A_MSB(pix_width) + 1)
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_B_LSB(pix_width)     (_HRT_MIPI_BACKEND_STREAMING_PIX_A_VAL_BIT(pix_width) + 1)
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_B_MSB(pix_width)     (_HRT_MIPI_BACKEND_STREAMING_PIX_B_LSB(pix_width) + (pix_width) - 1)
+#define _HRT_MIPI_BACKEND_STREAMING_PIX_B_VAL_BIT(pix_width) (_HRT_MIPI_BACKEND_STREAMING_PIX_B_MSB(pix_width) + 1)
+#define _HRT_MIPI_BACKEND_STREAMING_SOP_BIT(pix_width)       (_HRT_MIPI_BACKEND_STREAMING_PIX_B_VAL_BIT(pix_width) + 1)
+#define _HRT_MIPI_BACKEND_STREAMING_EOP_BIT(pix_width)       (_HRT_MIPI_BACKEND_STREAMING_SOP_BIT(pix_width) + 1)
+#define _HRT_MIPI_BACKEND_STREAMING_WIDTH(pix_width)         (_HRT_MIPI_BACKEND_STREAMING_EOP_BIT(pix_width) + 1)
+
+/*************************************************************************************************/
+/* Custom Decoding                                                                               */
+/* These Custom Defs are defined based on design-time config in "mipi_backend_pixel_formatter.chdl" !! */
+/*************************************************************************************************/
+#define _HRT_MIPI_BACKEND_CUST_EN_IDX                     0     /* 2bits */
+#define _HRT_MIPI_BACKEND_CUST_EN_DATAID_IDX              2     /* 6bits MIPI DATA ID */
+#define _HRT_MIPI_BACKEND_CUST_EN_HIGH_PREC_IDX           8     // 1 bit
+#define _HRT_MIPI_BACKEND_CUST_EN_WIDTH                   9
+#define _HRT_MIPI_BACKEND_CUST_MODE_ALL                   1     /* Enable Custom Decoding for all DATA IDs */
+#define _HRT_MIPI_BACKEND_CUST_MODE_ONE                   3     /* Enable Custom Decoding for ONE DATA ID, programmed in CUST_EN_DATA_ID */
+
+#define _HRT_MIPI_BACKEND_CUST_EN_OPTION_IDX              1
+
+/* Data State config = {get_bits(6bits), valid(1bit)}  */
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_S0_IDX          0     /* 7bits */
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_S1_IDX          8     /* 7bits */
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_S2_IDX          16    /* was 14 7bits */
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_WIDTH           24    /* was 21*/
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_VALID_IDX       0     /* 1bits */
+#define _HRT_MIPI_BACKEND_CUST_DATA_STATE_GETBITS_IDX     1     /* 6bits */
+
+/* Pixel Extractor config */
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_DATA_ALIGN_IDX     0     /* 6bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_PIX_ALIGN_IDX      6     /* 5bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_PIX_MASK_IDX       11    /* was 10 18bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_PIX_EN_IDX         29    /* was 28 1bits */
+
+#define _HRT_MIPI_BACKEND_CUST_PIX_EXT_WIDTH              30    /* was 29 */
+
+/* Pixel Valid & EoP config = {[eop,valid](especial), [eop,valid](normal)} */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_P0_IDX        0    /* 4bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_P1_IDX        4    /* 4bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_P2_IDX        8    /* 4bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_P3_IDX        12   /* 4bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_WIDTH         16
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_NOR_VALID_IDX 0    /* Normal (NO less get_bits case) Valid - 1bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_NOR_EOP_IDX   1    /* Normal (NO less get_bits case) EoP - 1bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_ESP_VALID_IDX 2    /* Especial (less get_bits case) Valid - 1bits */
+#define _HRT_MIPI_BACKEND_CUST_PIX_VALID_EOP_ESP_EOP_IDX   3    /* Especial (less get_bits case) EoP - 1bits */
+
+/*************************************************************************************************/
+/* MIPI backend output streaming interface definition                                            */
+/* These parameters define the fields within the streaming bus. These should also be used by the */
+/* subsequent block, ie stream2mmio.                                                             */
+/*************************************************************************************************/
+/* The pipe backend - stream2mmio should be design time configurable in                          */
+/*   PixWidth - Number of bits per pixel                                                         */
+/*   PPC      - Pixel per Clocks                                                                 */
+/*   NumSids  - Max number of source Ids (ifc's)  and derived from that:                         */
+/*   SidWidth - Number of bits required for the sid parameter                                    */
+/* In order to keep this configurability, below Macro's have these as a parameter                */
+/*************************************************************************************************/
+
+#define HRT_MIPI_BACKEND_STREAM_EOP_BIT                      0
+#define HRT_MIPI_BACKEND_STREAM_SOP_BIT                      1
+#define HRT_MIPI_BACKEND_STREAM_EOF_BIT                      2
+#define HRT_MIPI_BACKEND_STREAM_SOF_BIT                      3
+#define HRT_MIPI_BACKEND_STREAM_CHID_LS_BIT                  4
+#define HRT_MIPI_BACKEND_STREAM_CHID_MS_BIT(sid_width)      (HRT_MIPI_BACKEND_STREAM_CHID_LS_BIT + (sid_width) - 1)
+#define HRT_MIPI_BACKEND_STREAM_PIX_VAL_BIT(sid_width, p)    (HRT_MIPI_BACKEND_STREAM_CHID_MS_BIT(sid_width) + 1 + p)
+
+#define HRT_MIPI_BACKEND_STREAM_PIX_LS_BIT(sid_width, ppc, pix_width, p) (HRT_MIPI_BACKEND_STREAM_PIX_VAL_BIT(sid_width, ppc) + ((pix_width) * p))
+#define HRT_MIPI_BACKEND_STREAM_PIX_MS_BIT(sid_width, ppc, pix_width, p) (HRT_MIPI_BACKEND_STREAM_PIX_LS_BIT(sid_width, ppc, pix_width, p) + (pix_width) - 1)
+
+#if 0
+//#define HRT_MIPI_BACKEND_STREAM_PIX_BITS                    14
+//#define HRT_MIPI_BACKEND_STREAM_CHID_BITS                    4
+//#define HRT_MIPI_BACKEND_STREAM_PPC                          4
+#endif
+
+#define HRT_MIPI_BACKEND_STREAM_BITS(sid_width, ppc, pix_width)         (HRT_MIPI_BACKEND_STREAM_PIX_MS_BIT(sid_width, ppc, pix_width, (ppc - 1)) + 1)
+
+/* SP and LP LUT BIT POSITIONS */
+#define HRT_MIPI_BACKEND_LUT_PKT_DISREGARD_BIT              0                                                                                           // 0
+#define HRT_MIPI_BACKEND_LUT_SID_LS_BIT                     HRT_MIPI_BACKEND_LUT_PKT_DISREGARD_BIT + 1                                                  // 1
+#define HRT_MIPI_BACKEND_LUT_SID_MS_BIT(sid_width)          (HRT_MIPI_BACKEND_LUT_SID_LS_BIT + (sid_width) - 1)                                             // 1 + (4) - 1 = 4
+#define HRT_MIPI_BACKEND_LUT_MIPI_CH_ID_LS_BIT(sid_width)   HRT_MIPI_BACKEND_LUT_SID_MS_BIT(sid_width) + 1                                              // 5
+#define HRT_MIPI_BACKEND_LUT_MIPI_CH_ID_MS_BIT(sid_width)   HRT_MIPI_BACKEND_LUT_MIPI_CH_ID_LS_BIT(sid_width) + _HRT_MIPI_BACKEND_CHANNEL_ID_WIDTH - 1  // 6
+#define HRT_MIPI_BACKEND_LUT_MIPI_FMT_LS_BIT(sid_width)     HRT_MIPI_BACKEND_LUT_MIPI_CH_ID_MS_BIT(sid_width) + 1                                       // 7
+#define HRT_MIPI_BACKEND_LUT_MIPI_FMT_MS_BIT(sid_width)     HRT_MIPI_BACKEND_LUT_MIPI_FMT_LS_BIT(sid_width) + _HRT_MIPI_BACKEND_FORMAT_TYPE_WIDTH - 1   // 12
+
+/* #define HRT_MIPI_BACKEND_SP_LUT_BITS(sid_width)             HRT_MIPI_BACKEND_LUT_MIPI_CH_ID_MS_BIT(sid_width) + 1                                       // 7          */
+
+#define HRT_MIPI_BACKEND_SP_LUT_BITS(sid_width)             HRT_MIPI_BACKEND_LUT_SID_MS_BIT(sid_width) + 1
+#define HRT_MIPI_BACKEND_LP_LUT_BITS(sid_width)             HRT_MIPI_BACKEND_LUT_MIPI_FMT_MS_BIT(sid_width) + 1                                         // 13
+
+// temp solution
+//#define HRT_MIPI_BACKEND_STREAM_PIXA_VAL_BIT                HRT_MIPI_BACKEND_STREAM_CHID_MS_BIT  + 1                                    // 8
+//#define HRT_MIPI_BACKEND_STREAM_PIXB_VAL_BIT                HRT_MIPI_BACKEND_STREAM_PIXA_VAL_BIT + 1                                    // 9
+//#define HRT_MIPI_BACKEND_STREAM_PIXC_VAL_BIT                HRT_MIPI_BACKEND_STREAM_PIXB_VAL_BIT + 1                                    // 10
+//#define HRT_MIPI_BACKEND_STREAM_PIXD_VAL_BIT                HRT_MIPI_BACKEND_STREAM_PIXC_VAL_BIT + 1                                    // 11
+//#define HRT_MIPI_BACKEND_STREAM_PIXA_LS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXD_VAL_BIT + 1                                    // 12
+//#define HRT_MIPI_BACKEND_STREAM_PIXA_MS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXA_LS_BIT  + HRT_MIPI_BACKEND_STREAM_PIX_BITS - 1 // 25
+//#define HRT_MIPI_BACKEND_STREAM_PIXB_LS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXA_MS_BIT + 1                                     // 26
+//#define HRT_MIPI_BACKEND_STREAM_PIXB_MS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXB_LS_BIT  + HRT_MIPI_BACKEND_STREAM_PIX_BITS - 1 // 39
+//#define HRT_MIPI_BACKEND_STREAM_PIXC_LS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXB_MS_BIT + 1                                     // 40
+//#define HRT_MIPI_BACKEND_STREAM_PIXC_MS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXC_LS_BIT  + HRT_MIPI_BACKEND_STREAM_PIX_BITS - 1 // 53
+//#define HRT_MIPI_BACKEND_STREAM_PIXD_LS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXC_MS_BIT + 1                                     // 54
+//#define HRT_MIPI_BACKEND_STREAM_PIXD_MS_BIT                 HRT_MIPI_BACKEND_STREAM_PIXD_LS_BIT  + HRT_MIPI_BACKEND_STREAM_PIX_BITS - 1 // 67
+
+// vc hidden in pixb data (passed as raw12 the pipe)
+#define HRT_MIPI_BACKEND_STREAM_VC_LS_BIT(sid_width, ppc, pix_width)  HRT_MIPI_BACKEND_STREAM_PIX_LS_BIT(sid_width, ppc, pix_width, 1) + 10  //HRT_MIPI_BACKEND_STREAM_PIXB_LS_BIT + 10 // 36
+#define HRT_MIPI_BACKEND_STREAM_VC_MS_BIT(sid_width, ppc, pix_width)  HRT_MIPI_BACKEND_STREAM_VC_LS_BIT(sid_width, ppc, pix_width) + 1    // 37
+
+#endif /* _mipi_backend_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/rx_csi_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/rx_csi_defs.h
new file mode 100644
index 000000000..d0e5b54d1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/rx_csi_defs.h
@@ -0,0 +1,170 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _csi_rx_defs_h
+#define _csi_rx_defs_h
+
+//#include "rx_csi_common_defs.h"
+
+#define MIPI_PKT_DATA_WIDTH                         32
+//#define CLK_CROSSING_FIFO_DEPTH                     16
+#define _CSI_RX_REG_ALIGN                            4
+
+//define number of IRQ (see below for definition of each IRQ bits)
+#define CSI_RX_NOF_IRQS_BYTE_DOMAIN                11
+#define CSI_RX_NOF_IRQS_ISP_DOMAIN                 15 // CSI_RX_NOF_IRQS_BYTE_DOMAIN + remaining from Dphy_rx already on ISP clock domain
+
+// REGISTER DESCRIPTION
+//#define _HRT_CSI_RX_SOFTRESET_REG_IDX                0
+#define _HRT_CSI_RX_ENABLE_REG_IDX                   0
+#define _HRT_CSI_RX_NOF_ENABLED_LANES_REG_IDX        1
+#define _HRT_CSI_RX_ERROR_HANDLING_REG_IDX           2
+#define _HRT_CSI_RX_STATUS_REG_IDX                   3
+#define _HRT_CSI_RX_STATUS_DLANE_HS_REG_IDX          4
+#define _HRT_CSI_RX_STATUS_DLANE_LP_REG_IDX          5
+//#define _HRT_CSI_RX_IRQ_CONFIG_REG_IDX               6
+#define _HRT_CSI_RX_DLY_CNT_TERMEN_CLANE_REG_IDX     6
+#define _HRT_CSI_RX_DLY_CNT_SETTLE_CLANE_REG_IDX     7
+#define _HRT_CSI_RX_DLY_CNT_TERMEN_DLANE_REG_IDX(lane_idx)    (8 + (2 * lane_idx))
+#define _HRT_CSI_RX_DLY_CNT_SETTLE_DLANE_REG_IDX(lane_idx)    (8 + (2 * lane_idx) + 1)
+
+#define _HRT_CSI_RX_NOF_REGISTERS(nof_dlanes)      (8 + 2 * (nof_dlanes))
+
+//#define _HRT_CSI_RX_SOFTRESET_REG_WIDTH              1
+#define _HRT_CSI_RX_ENABLE_REG_WIDTH                 1
+#define _HRT_CSI_RX_NOF_ENABLED_LANES_REG_WIDTH      3
+#define _HRT_CSI_RX_ERROR_HANDLING_REG_WIDTH         4
+#define _HRT_CSI_RX_STATUS_REG_WIDTH                 1
+#define _HRT_CSI_RX_STATUS_DLANE_HS_REG_WIDTH        8
+#define _HRT_CSI_RX_STATUS_DLANE_LP_REG_WIDTH        24
+#define _HRT_CSI_RX_IRQ_CONFIG_REG_WIDTH             (CSI_RX_NOF_IRQS_ISP_DOMAIN)
+#define _HRT_CSI_RX_DLY_CNT_REG_WIDTH                24
+//#define _HRT_CSI_RX_IRQ_STATUS_REG_WIDTH            NOF_IRQS
+//#define _HRT_CSI_RX_IRQ_CLEAR_REG_WIDTH             0
+
+#define ONE_LANE_ENABLED                             0
+#define TWO_LANES_ENABLED                            1
+#define THREE_LANES_ENABLED                          2
+#define FOUR_LANES_ENABLED                           3
+
+// Error handling reg bit positions
+#define ERR_DECISION_BIT      0
+#define DISC_RESERVED_SP_BIT  1
+#define DISC_RESERVED_LP_BIT  2
+#define DIS_INCOMP_PKT_CHK_BIT	3
+
+#define _HRT_CSI_RX_IRQ_CONFIG_REG_VAL_POSEDGE      0
+#define _HRT_CSI_RX_IRQ_CONFIG_REG_VAL_ORIGINAL     1
+
+// Interrupt bits
+#define _HRT_RX_CSI_IRQ_SINGLE_PH_ERROR_CORRECTED   0
+#define _HRT_RX_CSI_IRQ_MULTIPLE_PH_ERROR_DETECTED  1
+#define _HRT_RX_CSI_IRQ_PAYLOAD_CHECKSUM_ERROR      2
+#define _HRT_RX_CSI_IRQ_FIFO_FULL_ERROR             3
+#define _HRT_RX_CSI_IRQ_RESERVED_SP_DETECTED        4
+#define _HRT_RX_CSI_IRQ_RESERVED_LP_DETECTED        5
+//#define _HRT_RX_CSI_IRQ_PREMATURE_SOP               6
+#define _HRT_RX_CSI_IRQ_INCOMPLETE_PACKET           6
+#define _HRT_RX_CSI_IRQ_FRAME_SYNC_ERROR            7
+#define _HRT_RX_CSI_IRQ_LINE_SYNC_ERROR             8
+#define _HRT_RX_CSI_IRQ_DLANE_HS_SOT_ERROR          9
+#define _HRT_RX_CSI_IRQ_DLANE_HS_SOT_SYNC_ERROR    10
+
+#define _HRT_RX_CSI_IRQ_DLANE_ESC_ERROR            11
+#define _HRT_RX_CSI_IRQ_DLANE_TRIGGERESC           12
+#define _HRT_RX_CSI_IRQ_DLANE_ULPSESC              13
+#define _HRT_RX_CSI_IRQ_CLANE_ULPSCLKNOT           14
+
+/* OLD ARASAN FRONTEND IRQs
+#define _HRT_RX_CSI_IRQ_OVERRUN_BIT                0
+#define _HRT_RX_CSI_IRQ_RESERVED_BIT               1
+#define _HRT_RX_CSI_IRQ_SLEEP_MODE_ENTRY_BIT       2
+#define _HRT_RX_CSI_IRQ_SLEEP_MODE_EXIT_BIT        3
+#define _HRT_RX_CSI_IRQ_ERR_SOT_HS_BIT             4
+#define _HRT_RX_CSI_IRQ_ERR_SOT_SYNC_HS_BIT        5
+#define _HRT_RX_CSI_IRQ_ERR_CONTROL_BIT            6
+#define _HRT_RX_CSI_IRQ_ERR_ECC_DOUBLE_BIT         7
+#define _HRT_RX_CSI_IRQ_ERR_ECC_CORRECTED_BIT      8
+#define _HRT_RX_CSI_IRQ_ERR_ECC_NO_CORRECTION_BIT  9
+#define _HRT_RX_CSI_IRQ_ERR_CRC_BIT               10
+#define _HRT_RX_CSI_IRQ_ERR_ID_BIT                11
+#define _HRT_RX_CSI_IRQ_ERR_FRAME_SYNC_BIT        12
+#define _HRT_RX_CSI_IRQ_ERR_FRAME_DATA_BIT        13
+#define _HRT_RX_CSI_IRQ_DATA_TIMEOUT_BIT          14
+#define _HRT_RX_CSI_IRQ_ERR_ESCAPE_BIT            15
+#define _HRT_RX_CSI_IRQ_ERR_LINE_SYNC_BIT         16
+*/
+
+////Bit Description for reg _HRT_CSI_RX_STATUS_DLANE_HS_REG_IDX
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_ERR_LANE0        0
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_ERR_LANE1        1
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_ERR_LANE2        2
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_ERR_LANE3        3
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_SYNC_ERR_LANE0   4
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_SYNC_ERR_LANE1   5
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_SYNC_ERR_LANE2   6
+#define _HRT_CSI_RX_STATUS_DLANE_HS_SOT_SYNC_ERR_LANE3   7
+
+////Bit Description for reg _HRT_CSI_RX_STATUS_DLANE_LP_REG_IDX
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ESC_ERR_LANE0        0
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ESC_ERR_LANE1        1
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ESC_ERR_LANE2        2
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ESC_ERR_LANE3        3
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC0_LANE0    4
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC1_LANE0    5
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC2_LANE0    6
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC3_LANE0    7
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC0_LANE1    8
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC1_LANE1    9
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC2_LANE1    10
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC3_LANE1    11
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC0_LANE2    12
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC1_LANE2    13
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC2_LANE2    14
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC3_LANE2    15
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC0_LANE3    16
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC1_LANE3    17
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC2_LANE3    18
+#define _HRT_CSI_RX_STATUS_DLANE_LP_TRIGGERESC3_LANE3    19
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ULPSESC_LANE0        20
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ULPSESC_LANE1        21
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ULPSESC_LANE2        22
+#define _HRT_CSI_RX_STATUS_DLANE_LP_ULPSESC_LANE3        23
+
+/*********************************************************/
+/*** Relevant declarations from rx_csi_common_defs.h *****/
+/*********************************************************/
+/* packet bit definition */
+#define _HRT_RX_CSI_PKT_SOP_BITPOS                       32
+#define _HRT_RX_CSI_PKT_EOP_BITPOS                       33
+#define _HRT_RX_CSI_PKT_PAYLOAD_BITPOS                    0
+#define _HRT_RX_CSI_PH_CH_ID_BITPOS                      22
+#define _HRT_RX_CSI_PH_FMT_ID_BITPOS                     16
+#define _HRT_RX_CSI_PH_DATA_FIELD_BITPOS                  0
+
+#define _HRT_RX_CSI_PKT_SOP_BITS                          1
+#define _HRT_RX_CSI_PKT_EOP_BITS                          1
+#define _HRT_RX_CSI_PKT_PAYLOAD_BITS                     32
+#define _HRT_RX_CSI_PH_CH_ID_BITS                         2
+#define _HRT_RX_CSI_PH_FMT_ID_BITS                        6
+#define _HRT_RX_CSI_PH_DATA_FIELD_BITS                   16
+
+/* Definition of data format ID at the interface CSS_receiver units */
+#define _HRT_RX_CSI_DATA_FORMAT_ID_SOF                0   /* 00 0000    frame start                                      */
+#define _HRT_RX_CSI_DATA_FORMAT_ID_EOF                1   /* 00 0001    frame end                                        */
+#define _HRT_RX_CSI_DATA_FORMAT_ID_SOL                2   /* 00 0010    line start                                       */
+#define _HRT_RX_CSI_DATA_FORMAT_ID_EOL                3   /* 00 0011    line end                                         */
+
+#endif /* _csi_rx_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/hrt/stream2mmio_defs.h b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/stream2mmio_defs.h
new file mode 100644
index 000000000..e17783f96
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/hrt/stream2mmio_defs.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _STREAM2MMMIO_DEFS_H
+#define _STREAM2MMMIO_DEFS_H
+
+#include <mipi_backend_defs.h>
+
+#define _STREAM2MMIO_REG_ALIGN                  4
+
+#define _STREAM2MMIO_COMMAND_REG_ID             0
+#define _STREAM2MMIO_ACKNOWLEDGE_REG_ID         1
+#define _STREAM2MMIO_PIX_WIDTH_ID_REG_ID        2
+#define _STREAM2MMIO_START_ADDR_REG_ID          3      /* master port address,NOT Byte */
+#define _STREAM2MMIO_END_ADDR_REG_ID            4      /* master port address,NOT Byte */
+#define _STREAM2MMIO_STRIDE_REG_ID              5      /* stride in master port words, increment is per packet for long sids, stride is not used for short sid's*/
+#define _STREAM2MMIO_NUM_ITEMS_REG_ID           6      /* number of packets for store packets cmd, number of words for store_words cmd */
+#define _STREAM2MMIO_BLOCK_WHEN_NO_CMD_REG_ID   7      /* if this register is 1, input will be stalled if there is no pending command for this sid */
+#define _STREAM2MMIO_REGS_PER_SID               8
+
+#define _STREAM2MMIO_SID_REG_OFFSET             8
+#define _STREAM2MMIO_MAX_NOF_SIDS              64      /* value used in hss model */
+
+/* command token definition     */
+#define _STREAM2MMIO_CMD_TOKEN_CMD_LSB          0      /* bits 1-0 is for the command field */
+#define _STREAM2MMIO_CMD_TOKEN_CMD_MSB          1
+
+#define _STREAM2MMIO_CMD_TOKEN_WIDTH           (_STREAM2MMIO_CMD_TOKEN_CMD_MSB + 1 - _STREAM2MMIO_CMD_TOKEN_CMD_LSB)
+
+#define _STREAM2MMIO_CMD_TOKEN_STORE_WORDS              0      /* command for storing a number of output words indicated by reg _STREAM2MMIO_NUM_ITEMS */
+#define _STREAM2MMIO_CMD_TOKEN_STORE_PACKETS            1      /* command for storing a number of packets indicated by reg _STREAM2MMIO_NUM_ITEMS      */
+#define _STREAM2MMIO_CMD_TOKEN_SYNC_FRAME               2      /* command for waiting for a frame start                                                */
+
+/* acknowledges from packer module */
+/* fields: eof   - indicates whether last (short) packet received was an eof packet */
+/*         eop   - indicates whether command has ended due to packet end or due to no of words requested has been received */
+/*         count - indicates number of words stored */
+#define _STREAM2MMIO_PACK_NUM_ITEMS_BITS        16
+#define _STREAM2MMIO_PACK_ACK_EOP_BIT           _STREAM2MMIO_PACK_NUM_ITEMS_BITS
+#define _STREAM2MMIO_PACK_ACK_EOF_BIT           (_STREAM2MMIO_PACK_ACK_EOP_BIT + 1)
+
+/* acknowledge token definition */
+#define _STREAM2MMIO_ACK_TOKEN_NUM_ITEMS_LSB    0      /* bits 3-0 is for the command field */
+#define _STREAM2MMIO_ACK_TOKEN_NUM_ITEMS_MSB   (_STREAM2MMIO_PACK_NUM_ITEMS_BITS - 1)
+#define _STREAM2MMIO_ACK_TOKEN_EOP_BIT         _STREAM2MMIO_PACK_ACK_EOP_BIT
+#define _STREAM2MMIO_ACK_TOKEN_EOF_BIT         _STREAM2MMIO_PACK_ACK_EOF_BIT
+#define _STREAM2MMIO_ACK_TOKEN_VALID_BIT       (_STREAM2MMIO_ACK_TOKEN_EOF_BIT + 1)      /* this bit indicates a valid ack    */
+/* if there is no valid ack, a read  */
+/* on the ack register returns 0     */
+#define _STREAM2MMIO_ACK_TOKEN_WIDTH           (_STREAM2MMIO_ACK_TOKEN_VALID_BIT + 1)
+
+/* commands for packer module */
+#define _STREAM2MMIO_PACK_CMD_STORE_WORDS        0
+#define _STREAM2MMIO_PACK_CMD_STORE_LONG_PACKET  1
+#define _STREAM2MMIO_PACK_CMD_STORE_SHORT_PACKET 2
+
+#endif /* _STREAM2MMIO_DEFS_H */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/ibuf_ctrl_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/ibuf_ctrl_global.h
new file mode 100644
index 000000000..56c5ed89b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/ibuf_ctrl_global.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IBUF_CTRL_GLOBAL_H_INCLUDED__
+#define __IBUF_CTRL_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#include <ibuf_cntrl_defs.h>	/* _IBUF_CNTRL_RECALC_WORDS_STATUS,
+				 * _IBUF_CNTRL_ARBITERS_STATUS,
+				 * _IBUF_CNTRL_PROC_REG_ALIGN,
+				 * etc.
+				 */
+
+/* Definition of contents of main controller state register is lacking
+ * in ibuf_cntrl_defs.h, so define these here:
+ */
+#define _IBUF_CNTRL_MAIN_CNTRL_FSM_MASK			0xf
+#define _IBUF_CNTRL_MAIN_CNTRL_FSM_NEXT_COMMAND_CHECK	0x9
+#define _IBUF_CNTRL_MAIN_CNTRL_MEM_INP_BUF_ALLOC	BIT(8)
+#define _IBUF_CNTRL_DMA_SYNC_WAIT_FOR_SYNC		1
+#define _IBUF_CNTRL_DMA_SYNC_FSM_WAIT_FOR_ACK		(0x3 << 1)
+
+struct	isp2401_ib_buffer_s {
+	u32	start_addr;	/* start address of the buffer in the
+					 * "input-buffer hardware block"
+					 */
+
+	u32	stride;		/* stride per buffer line (in bytes) */
+	u32	lines;		/* lines in the buffer */
+};
+typedef struct isp2401_ib_buffer_s	isp2401_ib_buffer_t;
+
+typedef struct ibuf_ctrl_cfg_s ibuf_ctrl_cfg_t;
+struct ibuf_ctrl_cfg_s {
+	bool online;
+
+	struct {
+		/* DMA configuration */
+		u32 channel;
+		u32 cmd; /* must be _DMA_V2_MOVE_A2B_NO_SYNC_CHK_COMMAND */
+
+		/* DMA reconfiguration */
+		u32 shift_returned_items;
+		u32 elems_per_word_in_ibuf;
+		u32 elems_per_word_in_dest;
+	} dma_cfg;
+
+	isp2401_ib_buffer_t ib_buffer;
+
+	struct {
+		u32 stride;
+		u32 start_addr;
+		u32 lines;
+	} dest_buf_cfg;
+
+	u32 items_per_store;
+	u32 stores_per_frame;
+
+	struct {
+		u32 sync_cmd;	/* must be _STREAM2MMIO_CMD_TOKEN_SYNC_FRAME */
+		u32 store_cmd;	/* must be _STREAM2MMIO_CMD_TOKEN_STORE_PACKETS */
+	} stream2mmio_cfg;
+};
+
+extern const u32 N_IBUF_CTRL_PROCS[N_IBUF_CTRL_ID];
+
+#endif /* __IBUF_CTRL_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/isys_dma_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/isys_dma_global.h
new file mode 100644
index 000000000..f423f3413
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/isys_dma_global.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_DMA_GLOBAL_H_INCLUDED__
+#define __ISYS_DMA_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#define HIVE_ISYS2401_DMA_IBUF_DDR_CONN	0
+#define HIVE_ISYS2401_DMA_IBUF_VMEM_CONN	1
+#define _DMA_V2_ZERO_EXTEND		0
+#define _DMA_V2_SIGN_EXTEND		1
+
+#define _DMA_ZERO_EXTEND     _DMA_V2_ZERO_EXTEND
+#define _DMA_SIGN_EXTEND     _DMA_V2_SIGN_EXTEND
+
+/********************************************************
+ *
+ * DMA Port.
+ *
+ * The DMA port definition for the input system
+ * 2401 DMA is the duplication of the DMA port
+ * definition for the CSS system DMA. It is duplicated
+ * here just as the temporal step before the device library
+ * is available. The device library is suppose to provide
+ * the capability of reusing the control interface of the
+ * same device prototypes. The refactor team will work on
+ * this, right?
+ *
+ ********************************************************/
+typedef struct isys2401_dma_port_cfg_s isys2401_dma_port_cfg_t;
+struct isys2401_dma_port_cfg_s {
+	u32 stride;
+	u32 elements;
+	u32 cropping;
+	u32 width;
+};
+
+/* end of DMA Port */
+
+/************************************************
+ *
+ * DMA Device.
+ *
+ * The DMA device definition for the input system
+ * 2401 DMA is the duplicattion of the DMA device
+ * definition for the CSS system DMA. It is duplicated
+ * here just as the temporal step before the device library
+ * is available. The device library is suppose to provide
+ * the capability of reusing the control interface of the
+ * same device prototypes. The refactor team will work on
+ * this, right?
+ *
+ ************************************************/
+typedef enum {
+	isys2401_dma_ibuf_to_ddr_connection	= HIVE_ISYS2401_DMA_IBUF_DDR_CONN,
+	isys2401_dma_ibuf_to_vmem_connection	= HIVE_ISYS2401_DMA_IBUF_VMEM_CONN
+} isys2401_dma_connection;
+
+typedef enum {
+	isys2401_dma_zero_extension = _DMA_ZERO_EXTEND,
+	isys2401_dma_sign_extension = _DMA_SIGN_EXTEND
+} isys2401_dma_extension;
+
+typedef struct isys2401_dma_cfg_s isys2401_dma_cfg_t;
+struct isys2401_dma_cfg_s {
+	isys2401_dma_channel	channel;
+	isys2401_dma_connection	connection;
+	isys2401_dma_extension	extension;
+	u32		height;
+};
+
+/* end of DMA Device */
+
+/* isys2401_dma_channel limits per DMA ID */
+extern const isys2401_dma_channel
+N_ISYS2401_DMA_CHANNEL_PROCS[N_ISYS2401_DMA_ID];
+
+#endif /* __ISYS_DMA_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/isys_irq_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/isys_irq_global.h
new file mode 100644
index 000000000..a81e4d13a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/isys_irq_global.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_IRQ_GLOBAL_H__
+#define __ISYS_IRQ_GLOBAL_H__
+
+#if defined(ISP2401)
+
+/* Register offset/index from base location */
+#define ISYS_IRQ_EDGE_REG_IDX		(0)
+#define ISYS_IRQ_MASK_REG_IDX		(ISYS_IRQ_EDGE_REG_IDX + 1)
+#define ISYS_IRQ_STATUS_REG_IDX		(ISYS_IRQ_EDGE_REG_IDX + 2)
+#define ISYS_IRQ_CLEAR_REG_IDX		(ISYS_IRQ_EDGE_REG_IDX + 3)
+#define ISYS_IRQ_ENABLE_REG_IDX		(ISYS_IRQ_EDGE_REG_IDX + 4)
+#define ISYS_IRQ_LEVEL_NO_REG_IDX	(ISYS_IRQ_EDGE_REG_IDX + 5)
+
+/* Register values */
+#define ISYS_IRQ_MASK_REG_VALUE		(0xFFFF)
+#define ISYS_IRQ_CLEAR_REG_VALUE	(0xFFFF)
+#define ISYS_IRQ_ENABLE_REG_VALUE	(0xFFFF)
+
+#endif /* defined(ISP2401) */
+
+#endif	/* __ISYS_IRQ_GLOBAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/isys_stream2mmio_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/isys_stream2mmio_global.h
new file mode 100644
index 000000000..0611047ea
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/isys_stream2mmio_global.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_GLOBAL_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+typedef struct stream2mmio_cfg_s stream2mmio_cfg_t;
+struct stream2mmio_cfg_s {
+	u32				bits_per_pixel;
+	u32				enable_blocking;
+};
+
+/* Stream2MMIO limits  per ID*/
+/*
+ * Stream2MMIO 0 has 8 SIDs that are indexed by
+ * [STREAM2MMIO_SID0_ID...STREAM2MMIO_SID7_ID].
+ *
+ * Stream2MMIO 1 has 4 SIDs that are indexed by
+ * [STREAM2MMIO_SID0_ID...TREAM2MMIO_SID3_ID].
+ *
+ * Stream2MMIO 2 has 4 SIDs that are indexed by
+ * [STREAM2MMIO_SID0_ID...STREAM2MMIO_SID3_ID].
+ */
+extern const stream2mmio_sid_ID_t N_STREAM2MMIO_SID_PROCS[N_STREAM2MMIO_ID];
+
+#endif /* __ISYS_STREAM2MMIO_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_2401_system/pixelgen_global.h b/drivers/staging/media/atomisp/pci/css_2401_system/pixelgen_global.h
new file mode 100644
index 000000000..f131f03cb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_2401_system/pixelgen_global.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PIXELGEN_GLOBAL_H_INCLUDED__
+#define __PIXELGEN_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+/**
+ * Pixel-generator. ("pixelgen_global.h")
+ */
+/*
+ * Duplicates "sync_generator_cfg_t" in "input_system_global.h".
+ */
+typedef struct isp2401_sync_generator_cfg_s isp2401_sync_generator_cfg_t;
+struct isp2401_sync_generator_cfg_s {
+	u32	hblank_cycles;
+	u32	vblank_cycles;
+	u32	pixels_per_clock;
+	u32	nr_of_frames;
+	u32	pixels_per_line;
+	u32	lines_per_frame;
+};
+
+typedef enum {
+	PIXELGEN_TPG_MODE_RAMP = 0,
+	PIXELGEN_TPG_MODE_CHBO,
+	PIXELGEN_TPG_MODE_MONO,
+	N_PIXELGEN_TPG_MODE
+} pixelgen_tpg_mode_t;
+
+/*
+ * "pixelgen_tpg_cfg_t" duplicates parts of
+ * "tpg_cfg_t" in "input_system_global.h".
+ */
+typedef struct pixelgen_tpg_cfg_s pixelgen_tpg_cfg_t;
+struct pixelgen_tpg_cfg_s {
+	pixelgen_tpg_mode_t	mode;	/* CHBO, MONO */
+
+	struct {
+		/* be used by CHBO and MON */
+		u32 R1;
+		u32 G1;
+		u32 B1;
+
+		/* be used by CHBO only */
+		u32 R2;
+		u32 G2;
+		u32 B2;
+	} color_cfg;
+
+	struct {
+		u32	h_mask;		/* horizontal mask */
+		u32	v_mask;		/* vertical mask */
+		u32	hv_mask;	/* horizontal+vertical mask? */
+	} mask_cfg;
+
+	struct {
+		s32	h_delta;	/* horizontal delta? */
+		s32	v_delta;	/* vertical delta? */
+	} delta_cfg;
+
+	isp2401_sync_generator_cfg_t	 sync_gen_cfg;
+};
+
+/*
+ * "pixelgen_prbs_cfg_t" duplicates parts of
+ * prbs_cfg_t" in "input_system_global.h".
+ */
+typedef struct pixelgen_prbs_cfg_s pixelgen_prbs_cfg_t;
+struct pixelgen_prbs_cfg_s {
+	s32	seed0;
+	s32	seed1;
+
+	isp2401_sync_generator_cfg_t	sync_gen_cfg;
+};
+
+/* end of Pixel-generator: TPG. ("pixelgen_global.h") */
+#endif /* __PIXELGEN_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/css_receiver_2400_common_defs.h b/drivers/staging/media/atomisp/pci/css_receiver_2400_common_defs.h
new file mode 100644
index 000000000..d2c39f960
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_receiver_2400_common_defs.h
@@ -0,0 +1,199 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _css_receiver_2400_common_defs_h_
+#define _css_receiver_2400_common_defs_h_
+#ifndef _mipi_backend_common_defs_h_
+#define _mipi_backend_common_defs_h_
+
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_DATA_WIDTH     16
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_CH_ID_WIDTH     2
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_FMT_TYPE_WIDTH  3
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_REAL_WIDTH (_HRT_CSS_RECEIVER_2400_GEN_SHORT_DATA_WIDTH + _HRT_CSS_RECEIVER_2400_GEN_SHORT_CH_ID_WIDTH + _HRT_CSS_RECEIVER_2400_GEN_SHORT_FMT_TYPE_WIDTH)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_WIDTH      32 /* use 32 to be compatibel with streaming monitor !, MSB's of interface are tied to '0' */
+
+/* Definition of data format ID at the interface CSS_receiver capture/acquisition units */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8          24   /* 01 1000 YUV420 8-bit                                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_10         25   /* 01 1001  YUV420 10-bit                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8L         26   /* 01 1010   YUV420 8-bit legacy                               */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV422_8          30   /* 01 1110   YUV422 8-bit                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV422_10         31   /* 01 1111   YUV422 10-bit                                     */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB444            32   /* 10 0000   RGB444                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB555            33   /* 10 0001   RGB555                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB565            34   /* 10 0010   RGB565                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB666            35   /* 10 0011   RGB666                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RGB888            36   /* 10 0100   RGB888                                            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW6              40   /* 10 1000   RAW6                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW7              41   /* 10 1001   RAW7                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW8              42   /* 10 1010   RAW8                                              */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW10             43   /* 10 1011   RAW10                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW12             44   /* 10 1100   RAW12                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW14             45   /* 10 1101   RAW14                                             */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_1         48   /* 11 0000    JPEG [User Defined 8-bit Data Type 1]            */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_2         49   /* 11 0001    User Defined 8-bit Data Type 2                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_3         50   /* 11 0010    User Defined 8-bit Data Type 3                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_4         51   /* 11 0011    User Defined 8-bit Data Type 4                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_5         52   /* 11 0100    User Defined 8-bit Data Type 5                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_6         53   /* 11 0101    User Defined 8-bit Data Type 6                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_7         54   /* 11 0110    User Defined 8-bit Data Type 7                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_USR_DEF_8         55   /* 11 0111    User Defined 8-bit Data Type 8                   */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_Emb               18   /* 01 0010    embedded eight bit non image data                */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_SOF                0   /* 00 0000    frame start                                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_EOF                1   /* 00 0001    frame end                                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_SOL                2   /* 00 0010    line start                                       */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_EOL                3   /* 00 0011    line end                                         */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH1            8   /* 00 1000  Generic Short Packet Code 1                        */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH2            9   /* 00 1001    Generic Short Packet Code 2                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH3           10   /* 00 1010    Generic Short Packet Code 3                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH4           11   /* 00 1011    Generic Short Packet Code 4                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH5           12   /* 00 1100    Generic Short Packet Code 5                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH6           13   /* 00 1101    Generic Short Packet Code 6                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH7           14   /* 00 1110    Generic Short Packet Code 7                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_GEN_SH8           15   /* 00 1111    Generic Short Packet Code 8                      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_8_CSPS     28   /* 01 1100   YUV420 8-bit (Chroma Shifted Pixel Sampling)      */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_YUV420_10_CSPS    29   /* 01 1101   YUV420 10-bit (Chroma Shifted Pixel Sampling)     */
+/* used reserved mipi positions for these */
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW16             46
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18             47
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18_2           37
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_RAW18_3           38
+
+#define _HRT_CSS_RECEIVER_2400_DATA_FORMAT_ID_WIDTH              6
+
+/* Definition of format_types at the interface CSS --> input_selector*/
+/* !! Changes here should be copied to systems/isp/isp_css/bin/conv_transmitter_cmd.tcl !! */
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB888           0  // 36 'h24
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB555           1  // 33 'h
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB444           2  // 32
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB565           3  // 34
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RGB666           4  // 35
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW8             5  // 42
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW10            6  // 43
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW6             7  // 40
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW7             8  // 41
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW12            9  // 43
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW14           10  // 45
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8        11  // 30
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_10       12  // 25
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV422_8        13  // 30
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV422_10       14  // 31
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_1       15  // 48
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8L       16  // 26
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_Emb             17  // 18
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_2       18  // 49
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_3       19  // 50
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_4       20  // 51
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_5       21  // 52
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_6       22  // 53
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_7       23  // 54
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_USR_DEF_8       24  // 55
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_8_CSPS   25  // 28
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_YUV420_10_CSPS  26  // 29
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW16           27  // ?
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18           28  // ?
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18_2         29  // ? Option 2 for depacketiser
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_RAW18_3         30  // ? Option 3 for depacketiser
+#define _HRT_CSS_RECEIVER_2400_FMT_TYPE_CUSTOM          31  // to signal custom decoding
+
+/* definition for state machine of data FIFO for decode different type of data */
+#define _HRT_CSS_RECEIVER_2400_YUV420_8_REPEAT_PTN                 1
+#define _HRT_CSS_RECEIVER_2400_YUV420_10_REPEAT_PTN                5
+#define _HRT_CSS_RECEIVER_2400_YUV420_8L_REPEAT_PTN                1
+#define _HRT_CSS_RECEIVER_2400_YUV422_8_REPEAT_PTN                 1
+#define _HRT_CSS_RECEIVER_2400_YUV422_10_REPEAT_PTN                5
+#define _HRT_CSS_RECEIVER_2400_RGB444_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB555_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB565_REPEAT_PTN                   2
+#define _HRT_CSS_RECEIVER_2400_RGB666_REPEAT_PTN                   9
+#define _HRT_CSS_RECEIVER_2400_RGB888_REPEAT_PTN                   3
+#define _HRT_CSS_RECEIVER_2400_RAW6_REPEAT_PTN                     3
+#define _HRT_CSS_RECEIVER_2400_RAW7_REPEAT_PTN                     7
+#define _HRT_CSS_RECEIVER_2400_RAW8_REPEAT_PTN                     1
+#define _HRT_CSS_RECEIVER_2400_RAW10_REPEAT_PTN                    5
+#define _HRT_CSS_RECEIVER_2400_RAW12_REPEAT_PTN                    3
+#define _HRT_CSS_RECEIVER_2400_RAW14_REPEAT_PTN                    7
+
+#define _HRT_CSS_RECEIVER_2400_MAX_REPEAT_PTN                      _HRT_CSS_RECEIVER_2400_RGB666_REPEAT_PTN
+
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FMT_IDX                     0
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FMT_WIDTH                   3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_PRED_IDX                    3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_PRED_WIDTH                  1
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_USD_BITS                    4  /* bits per USD type */
+
+#define _HRT_CSS_RECEIVER_2400_BE_RAW16_DATAID_IDX                 0
+#define _HRT_CSS_RECEIVER_2400_BE_RAW16_EN_IDX                     6
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_DATAID_IDX                 0
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_OPTION_IDX                 6
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_EN_IDX                     8
+
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_NO_COMP                     0
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_6_10                     1
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_7_10                     2
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_10_8_10                     3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_6_12                     4
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_7_12                     5
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_12_8_12                     6
+
+/* packet bit definition */
+#define _HRT_CSS_RECEIVER_2400_PKT_SOP_IDX                        32
+#define _HRT_CSS_RECEIVER_2400_PKT_SOP_BITS                        1
+#define _HRT_CSS_RECEIVER_2400_PKT_CH_ID_IDX                      22
+#define _HRT_CSS_RECEIVER_2400_PKT_CH_ID_BITS                      2
+#define _HRT_CSS_RECEIVER_2400_PKT_FMT_ID_IDX                     16
+#define _HRT_CSS_RECEIVER_2400_PKT_FMT_ID_BITS                     6
+#define _HRT_CSS_RECEIVER_2400_PH_DATA_FIELD_IDX                   0
+#define _HRT_CSS_RECEIVER_2400_PH_DATA_FIELD_BITS                 16
+#define _HRT_CSS_RECEIVER_2400_PKT_PAYLOAD_IDX                     0
+#define _HRT_CSS_RECEIVER_2400_PKT_PAYLOAD_BITS                   32
+
+/*************************************************************************************************/
+/* Custom Decoding                                                                               */
+/* These Custom Defs are defined based on design-time config in "csi_be_pixel_formatter.chdl" !! */
+/*************************************************************************************************/
+#define BE_CUST_EN_IDX                     0     /* 2bits */
+#define BE_CUST_EN_DATAID_IDX              2     /* 6bits MIPI DATA ID */
+#define BE_CUST_EN_WIDTH                   8
+#define BE_CUST_MODE_ALL                   1     /* Enable Custom Decoding for all DATA IDs */
+#define BE_CUST_MODE_ONE                   3     /* Enable Custom Decoding for ONE DATA ID, programmed in CUST_EN_DATA_ID */
+
+/* Data State config = {get_bits(6bits), valid(1bit)}  */
+#define BE_CUST_DATA_STATE_S0_IDX          0     /* 7bits */
+#define BE_CUST_DATA_STATE_S1_IDX          7     /* 7bits */
+#define BE_CUST_DATA_STATE_S2_IDX          14    /* 7bits */
+#define BE_CUST_DATA_STATE_WIDTH           21
+#define BE_CUST_DATA_STATE_VALID_IDX       0     /* 1bits */
+#define BE_CUST_DATA_STATE_GETBITS_IDX     1     /* 6bits */
+
+/* Pixel Extractor config */
+#define BE_CUST_PIX_EXT_DATA_ALIGN_IDX     0     /* 5bits */
+#define BE_CUST_PIX_EXT_PIX_ALIGN_IDX      5     /* 5bits */
+#define BE_CUST_PIX_EXT_PIX_MASK_IDX       10    /* 18bits */
+#define BE_CUST_PIX_EXT_PIX_EN_IDX         28    /* 1bits */
+#define BE_CUST_PIX_EXT_WIDTH              29
+
+/* Pixel Valid & EoP config = {[eop,valid](especial), [eop,valid](normal)} */
+#define BE_CUST_PIX_VALID_EOP_P0_IDX        0    /* 4bits */
+#define BE_CUST_PIX_VALID_EOP_P1_IDX        4    /* 4bits */
+#define BE_CUST_PIX_VALID_EOP_P2_IDX        8    /* 4bits */
+#define BE_CUST_PIX_VALID_EOP_P3_IDX        12   /* 4bits */
+#define BE_CUST_PIX_VALID_EOP_WIDTH         16
+#define BE_CUST_PIX_VALID_EOP_NOR_VALID_IDX 0    /* Normal (NO less get_bits case) Valid - 1bits */
+#define BE_CUST_PIX_VALID_EOP_NOR_EOP_IDX   1    /* Normal (NO less get_bits case) EoP - 1bits */
+#define BE_CUST_PIX_VALID_EOP_ESP_VALID_IDX 2    /* Especial (less get_bits case) Valid - 1bits */
+#define BE_CUST_PIX_VALID_EOP_ESP_EOP_IDX   3    /* Especial (less get_bits case) EoP - 1bits */
+
+#endif /* _mipi_backend_common_defs_h_ */
+#endif /* _css_receiver_2400_common_defs_h_ */
diff --git a/drivers/staging/media/atomisp/pci/css_receiver_2400_defs.h b/drivers/staging/media/atomisp/pci/css_receiver_2400_defs.h
new file mode 100644
index 000000000..180ff7cd9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_receiver_2400_defs.h
@@ -0,0 +1,257 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _css_receiver_2400_defs_h_
+#define _css_receiver_2400_defs_h_
+
+#include "css_receiver_2400_common_defs.h"
+
+#define CSS_RECEIVER_DATA_WIDTH                8
+#define CSS_RECEIVER_RX_TRIG                   4
+#define CSS_RECEIVER_RF_WORD                  32
+#define CSS_RECEIVER_IMG_PROC_RF_ADDR         10
+#define CSS_RECEIVER_CSI_RF_ADDR               4
+#define CSS_RECEIVER_DATA_OUT                 12
+#define CSS_RECEIVER_CHN_NO                    2
+#define CSS_RECEIVER_DWORD_CNT                11
+#define CSS_RECEIVER_FORMAT_TYP                5
+#define CSS_RECEIVER_HRESPONSE                 2
+#define CSS_RECEIVER_STATE_WIDTH               3
+#define CSS_RECEIVER_FIFO_DAT                 32
+#define CSS_RECEIVER_CNT_VAL                   2
+#define CSS_RECEIVER_PRED10_VAL               10
+#define CSS_RECEIVER_PRED12_VAL               12
+#define CSS_RECEIVER_CNT_WIDTH                 8
+#define CSS_RECEIVER_WORD_CNT                 16
+#define CSS_RECEIVER_PIXEL_LEN                 6
+#define CSS_RECEIVER_PIXEL_CNT                 5
+#define CSS_RECEIVER_COMP_8_BIT                8
+#define CSS_RECEIVER_COMP_7_BIT                7
+#define CSS_RECEIVER_COMP_6_BIT                6
+
+#define CSI_CONFIG_WIDTH                       4
+
+/* division of gen_short data, ch_id and fmt_type over streaming data interface */
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_DATA_BIT_LSB     0
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_FMT_TYPE_BIT_LSB (_HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_DATA_BIT_LSB     + _HRT_CSS_RECEIVER_2400_GEN_SHORT_DATA_WIDTH)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_CH_ID_BIT_LSB    (_HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_FMT_TYPE_BIT_LSB + _HRT_CSS_RECEIVER_2400_GEN_SHORT_FMT_TYPE_WIDTH)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_DATA_BIT_MSB     (_HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_FMT_TYPE_BIT_LSB - 1)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_FMT_TYPE_BIT_MSB (_HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_CH_ID_BIT_LSB    - 1)
+#define _HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_CH_ID_BIT_MSB    (_HRT_CSS_RECEIVER_2400_GEN_SHORT_STR_REAL_WIDTH       - 1)
+
+#define _HRT_CSS_RECEIVER_2400_REG_ALIGN 4
+#define _HRT_CSS_RECEIVER_2400_BYTES_PER_PKT             4
+
+#define hrt_css_receiver_2400_4_lane_port_offset  0x100
+#define hrt_css_receiver_2400_1_lane_port_offset  0x200
+#define hrt_css_receiver_2400_2_lane_port_offset  0x300
+#define hrt_css_receiver_2400_backend_port_offset 0x100
+
+#define _HRT_CSS_RECEIVER_2400_DEVICE_READY_REG_IDX      0
+#define _HRT_CSS_RECEIVER_2400_IRQ_STATUS_REG_IDX        1
+#define _HRT_CSS_RECEIVER_2400_IRQ_ENABLE_REG_IDX        2
+#define _HRT_CSS_RECEIVER_2400_CSI2_FUNC_PROG_REG_IDX    3
+#define _HRT_CSS_RECEIVER_2400_INIT_COUNT_REG_IDX        4
+#define _HRT_CSS_RECEIVER_2400_FS_TO_LS_DELAY_REG_IDX    7
+#define _HRT_CSS_RECEIVER_2400_LS_TO_DATA_DELAY_REG_IDX  8
+#define _HRT_CSS_RECEIVER_2400_DATA_TO_LE_DELAY_REG_IDX  9
+#define _HRT_CSS_RECEIVER_2400_LE_TO_FE_DELAY_REG_IDX   10
+#define _HRT_CSS_RECEIVER_2400_FE_TO_FS_DELAY_REG_IDX   11
+#define _HRT_CSS_RECEIVER_2400_LE_TO_LS_DELAY_REG_IDX   12
+#define _HRT_CSS_RECEIVER_2400_TWO_PIXEL_EN_REG_IDX     13
+#define _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_REG_IDX  14
+#define _HRT_CSS_RECEIVER_2400_SYNC_COUNT_REG_IDX       15
+#define _HRT_CSS_RECEIVER_2400_RX_COUNT_REG_IDX         16
+#define _HRT_CSS_RECEIVER_2400_BACKEND_RST_REG_IDX      17
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG0_IDX 18
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG1_IDX 19
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG0_IDX 20
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG1_IDX 21
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG0_IDX 22
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG1_IDX 23
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG0_IDX 24
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG1_IDX 25
+#define _HRT_CSS_RECEIVER_2400_RAW18_REG_IDX            26
+#define _HRT_CSS_RECEIVER_2400_FORCE_RAW8_REG_IDX       27
+#define _HRT_CSS_RECEIVER_2400_RAW16_REG_IDX            28
+
+/* Interrupt bits for IRQ_STATUS and IRQ_ENABLE registers */
+#define _HRT_CSS_RECEIVER_2400_IRQ_OVERRUN_BIT                0
+#define _HRT_CSS_RECEIVER_2400_IRQ_RESERVED_BIT               1
+#define _HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_ENTRY_BIT       2
+#define _HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_EXIT_BIT        3
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_HS_BIT             4
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_SYNC_HS_BIT        5
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_CONTROL_BIT            6
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_DOUBLE_BIT         7
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_CORRECTED_BIT      8
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_NO_CORRECTION_BIT  9
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_CRC_BIT               10
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ID_BIT                11
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_SYNC_BIT        12
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_DATA_BIT        13
+#define _HRT_CSS_RECEIVER_2400_IRQ_DATA_TIMEOUT_BIT          14
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ESCAPE_BIT            15
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_LINE_SYNC_BIT         16
+
+#define _HRT_CSS_RECEIVER_2400_IRQ_OVERRUN_CAUSE_                  "Fifo Overrun"
+#define _HRT_CSS_RECEIVER_2400_IRQ_RESERVED_CAUSE_                 "Reserved"
+#define _HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_ENTRY_CAUSE_         "Sleep mode entry"
+#define _HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_EXIT_CAUSE_          "Sleep mode exit"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_HS_CAUSE_               "Error high speed SOT"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_SYNC_HS_CAUSE_          "Error high speed sync SOT"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_CONTROL_CAUSE_              "Error control"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_DOUBLE_CAUSE_           "Error correction double bit"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_CORRECTED_CAUSE_        "Error correction single bit"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_NO_CORRECTION_CAUSE_    "No error"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_CRC_CAUSE_                  "Error cyclic redundancy check"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ID_CAUSE_                   "Error id"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_SYNC_CAUSE_           "Error frame sync"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_DATA_CAUSE_           "Error frame data"
+#define _HRT_CSS_RECEIVER_2400_IRQ_DATA_TIMEOUT_CAUSE_             "Data time-out"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_ESCAPE_CAUSE_               "Error escape"
+#define _HRT_CSS_RECEIVER_2400_IRQ_ERR_LINE_SYNC_CAUSE_            "Error line sync"
+
+/* Bits for CSI2_DEVICE_READY register */
+#define _HRT_CSS_RECEIVER_2400_CSI2_DEVICE_READY_IDX                          0
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_INIT_TIME_OUT_ERR_IDX                2
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_OVER_RUN_ERR_IDX                     3
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_SOT_SYNC_ERR_IDX                     4
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_RECEIVE_DATA_TIME_OUT_ERR_IDX        5
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_ECC_TWO_BIT_ERR_IDX                  6
+#define _HRT_CSS_RECEIVER_2400_CSI2_MASK_DATA_ID_ERR_IDX                      7
+
+/* Bits for CSI2_FUNC_PROG register */
+#define _HRT_CSS_RECEIVER_2400_CSI2_DATA_TIMEOUT_IDX    0
+#define _HRT_CSS_RECEIVER_2400_CSI2_DATA_TIMEOUT_BITS   19
+
+/* Bits for INIT_COUNT register */
+#define _HRT_CSS_RECEIVER_2400_INIT_TIMER_IDX  0
+#define _HRT_CSS_RECEIVER_2400_INIT_TIMER_BITS 16
+
+/* Bits for COUNT registers */
+#define _HRT_CSS_RECEIVER_2400_SYNC_COUNT_IDX     0
+#define _HRT_CSS_RECEIVER_2400_SYNC_COUNT_BITS    8
+#define _HRT_CSS_RECEIVER_2400_RX_COUNT_IDX       0
+#define _HRT_CSS_RECEIVER_2400_RX_COUNT_BITS      8
+
+/* Bits for RAW116_18_DATAID register */
+#define _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_RAW16_BITS_IDX   0
+#define _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_RAW16_BITS_BITS  6
+#define _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_RAW18_BITS_IDX   8
+#define _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_RAW18_BITS_BITS  6
+
+/* Bits for COMP_FORMAT register, this selects the compression data format */
+#define _HRT_CSS_RECEIVER_2400_COMP_RAW_BITS_IDX  0
+#define _HRT_CSS_RECEIVER_2400_COMP_RAW_BITS_BITS 8
+#define _HRT_CSS_RECEIVER_2400_COMP_NUM_BITS_IDX  (_HRT_CSS_RECEIVER_2400_COMP_RAW_BITS_IDX + _HRT_CSS_RECEIVER_2400_COMP_RAW_BITS_BITS)
+#define _HRT_CSS_RECEIVER_2400_COMP_NUM_BITS_BITS 8
+
+/* Bits for COMP_PREDICT register, this selects the predictor algorithm */
+#define _HRT_CSS_RECEIVER_2400_PREDICT_NO_COMP 0
+#define _HRT_CSS_RECEIVER_2400_PREDICT_1       1
+#define _HRT_CSS_RECEIVER_2400_PREDICT_2       2
+
+/* Number of bits used for the delay registers */
+#define _HRT_CSS_RECEIVER_2400_DELAY_BITS 8
+
+/* Bits for COMP_SCHEME register, this  selects the compression scheme for a VC */
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD1_BITS_IDX  0
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD2_BITS_IDX  5
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD3_BITS_IDX  10
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD4_BITS_IDX  15
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD5_BITS_IDX  20
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD6_BITS_IDX  25
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD7_BITS_IDX  0
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD8_BITS_IDX  5
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD_BITS_BITS  5
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD_FMT_BITS_IDX   0
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD_FMT_BITS_BITS  3
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD_PRED_BITS_IDX  3
+#define _HRT_CSS_RECEIVER_2400_COMP_SCHEME_USD_PRED_BITS_BITS 2
+
+/* BITS for backend RAW16 and RAW 18 registers */
+
+#define _HRT_CSS_RECEIVER_2400_RAW18_DATAID_IDX    0
+#define _HRT_CSS_RECEIVER_2400_RAW18_DATAID_BITS   6
+#define _HRT_CSS_RECEIVER_2400_RAW18_OPTION_IDX    6
+#define _HRT_CSS_RECEIVER_2400_RAW18_OPTION_BITS   2
+#define _HRT_CSS_RECEIVER_2400_RAW18_EN_IDX        8
+#define _HRT_CSS_RECEIVER_2400_RAW18_EN_BITS       1
+
+#define _HRT_CSS_RECEIVER_2400_RAW16_DATAID_IDX    0
+#define _HRT_CSS_RECEIVER_2400_RAW16_DATAID_BITS   6
+#define _HRT_CSS_RECEIVER_2400_RAW16_OPTION_IDX    6
+#define _HRT_CSS_RECEIVER_2400_RAW16_OPTION_BITS   2
+#define _HRT_CSS_RECEIVER_2400_RAW16_EN_IDX        8
+#define _HRT_CSS_RECEIVER_2400_RAW16_EN_BITS       1
+
+/* These hsync and vsync values are for HSS simulation only */
+#define _HRT_CSS_RECEIVER_2400_HSYNC_VAL BIT(16)
+#define _HRT_CSS_RECEIVER_2400_VSYNC_VAL BIT(17)
+
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_WIDTH                 28
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_LSB              0
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_MSB             (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_LSB + CSS_RECEIVER_DATA_OUT - 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_VAL_BIT         (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_MSB + 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_LSB             (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_A_VAL_BIT + 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_MSB             (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_LSB + CSS_RECEIVER_DATA_OUT - 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_VAL_BIT         (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_MSB + 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_SOP_BIT               (_HRT_CSS_RECEIVER_2400_BE_STREAMING_PIX_B_VAL_BIT + 1)
+#define _HRT_CSS_RECEIVER_2400_BE_STREAMING_EOP_BIT               (_HRT_CSS_RECEIVER_2400_BE_STREAMING_SOP_BIT + 1)
+
+// SH Backend Register IDs
+#define _HRT_CSS_RECEIVER_2400_BE_GSP_ACC_OVL_REG_IDX              0
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_REG_IDX                     1
+#define _HRT_CSS_RECEIVER_2400_BE_TWO_PPC_REG_IDX                  2
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG0_IDX             3
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG1_IDX             4
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG2_IDX             5
+#define _HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG3_IDX             6
+#define _HRT_CSS_RECEIVER_2400_BE_SEL_REG_IDX                      7
+#define _HRT_CSS_RECEIVER_2400_BE_RAW16_CONFIG_REG_IDX             8
+#define _HRT_CSS_RECEIVER_2400_BE_RAW18_CONFIG_REG_IDX             9
+#define _HRT_CSS_RECEIVER_2400_BE_FORCE_RAW8_REG_IDX              10
+#define _HRT_CSS_RECEIVER_2400_BE_IRQ_STATUS_REG_IDX              11
+#define _HRT_CSS_RECEIVER_2400_BE_IRQ_CLEAR_REG_IDX               12
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_EN_REG_IDX                 13
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_DATA_STATE_REG_IDX         14    /* Data State 0,1,2 config */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S0P0_REG_IDX       15    /* Pixel Extractor config for Data State 0 & Pix 0 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S0P1_REG_IDX       16    /* Pixel Extractor config for Data State 0 & Pix 1 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S0P2_REG_IDX       17    /* Pixel Extractor config for Data State 0 & Pix 2 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S0P3_REG_IDX       18    /* Pixel Extractor config for Data State 0 & Pix 3 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S1P0_REG_IDX       19    /* Pixel Extractor config for Data State 1 & Pix 0 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S1P1_REG_IDX       20    /* Pixel Extractor config for Data State 1 & Pix 1 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S1P2_REG_IDX       21    /* Pixel Extractor config for Data State 1 & Pix 2 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S1P3_REG_IDX       22    /* Pixel Extractor config for Data State 1 & Pix 3 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S2P0_REG_IDX       23    /* Pixel Extractor config for Data State 2 & Pix 0 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S2P1_REG_IDX       24    /* Pixel Extractor config for Data State 2 & Pix 1 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S2P2_REG_IDX       25    /* Pixel Extractor config for Data State 2 & Pix 2 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_EXT_S2P3_REG_IDX       26    /* Pixel Extractor config for Data State 2 & Pix 3 */
+#define _HRT_CSS_RECEIVER_2400_BE_CUST_PIX_VALID_EOP_REG_IDX      27    /* Pixel Valid & EoP config for Pix 0,1,2,3 */
+
+#define _HRT_CSS_RECEIVER_2400_BE_NOF_REGISTERS                   28
+
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_HE                          0
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_RCF                         1
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_PF                          2
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_SM                          3
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_PD                          4
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_SD                          5
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_OT                          6
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_BC                          7
+#define _HRT_CSS_RECEIVER_2400_BE_SRST_WIDTH                       8
+
+#endif /* _css_receiver_2400_defs_h_ */
diff --git a/drivers/staging/media/atomisp/pci/css_trace.h b/drivers/staging/media/atomisp/pci/css_trace.h
new file mode 100644
index 000000000..d2ce50fcf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/css_trace.h
@@ -0,0 +1,278 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSS_TRACE_H_
+#define __CSS_TRACE_H_
+
+#include <type_support.h>
+#include "sh_css_internal.h"	/* for SH_CSS_MAX_SP_THREADS */
+
+/*
+	structs and constants for tracing
+*/
+
+/* one tracer item: major, minor and counter. The counter value can be used for GP data */
+struct trace_item_t {
+	u8   major;
+	u8   minor;
+	u16  counter;
+};
+
+#define MAX_SCRATCH_DATA	4
+#define MAX_CMD_DATA		2
+
+/* trace header: holds the version and the topology of the tracer. */
+struct trace_header_t {
+	/* 1st dword: descriptor */
+	u8   version;
+	u8   max_threads;
+	u16  max_tracer_points;
+	/* 2nd field: command + data */
+	/* 2nd dword */
+	u32  command;
+	/* 3rd & 4th dword */
+	u32  data[MAX_CMD_DATA];
+	/* 3rd field: debug pointer */
+	/* 5th & 6th dword: debug pointer mechanism */
+	u32  debug_ptr_signature;
+	u32  debug_ptr_value;
+	/* Rest of the header: status & scratch data */
+	u8   thr_status_byte[SH_CSS_MAX_SP_THREADS];
+	u16  thr_status_word[SH_CSS_MAX_SP_THREADS];
+	u32  thr_status_dword[SH_CSS_MAX_SP_THREADS];
+	u32  scratch_debug[MAX_SCRATCH_DATA];
+};
+
+/* offsets for master_port read/write */
+#define HDR_HDR_OFFSET              0	/* offset of the header */
+#define HDR_COMMAND_OFFSET          offsetof(struct trace_header_t, command)
+#define HDR_DATA_OFFSET             offsetof(struct trace_header_t, data)
+#define HDR_DEBUG_SIGNATURE_OFFSET  offsetof(struct trace_header_t, debug_ptr_signature)
+#define HDR_DEBUG_POINTER_OFFSET    offsetof(struct trace_header_t, debug_ptr_value)
+#define HDR_STATUS_OFFSET           offsetof(struct trace_header_t, thr_status_byte)
+#define HDR_STATUS_OFFSET_BYTE      offsetof(struct trace_header_t, thr_status_byte)
+#define HDR_STATUS_OFFSET_WORD      offsetof(struct trace_header_t, thr_status_word)
+#define HDR_STATUS_OFFSET_DWORD     offsetof(struct trace_header_t, thr_status_dword)
+#define HDR_STATUS_OFFSET_SCRATCH   offsetof(struct trace_header_t, scratch_debug)
+
+/*
+Trace version history:
+ 1: initial version, hdr = descr, command & ptr.
+ 2: added ISP + 24-bit fields.
+ 3: added thread ID.
+ 4: added status in header.
+*/
+#define TRACER_VER			4
+
+#define TRACE_BUFF_ADDR       0xA000
+#define TRACE_BUFF_SIZE       0x1000	/* 4K allocated */
+
+#define TRACE_ENABLE_SP0 0
+#define TRACE_ENABLE_SP1 0
+#define TRACE_ENABLE_ISP 0
+
+enum TRACE_CORE_ID {
+	TRACE_SP0_ID,
+	TRACE_SP1_ID,
+	TRACE_ISP_ID
+};
+
+/* TODO: add timing format? */
+enum TRACE_DUMP_FORMAT {
+	TRACE_DUMP_FORMAT_POINT_NO_TID,
+	TRACE_DUMP_FORMAT_VALUE24,
+	TRACE_DUMP_FORMAT_VALUE24_TIMING,
+	TRACE_DUMP_FORMAT_VALUE24_TIMING_DELTA,
+	TRACE_DUMP_FORMAT_POINT
+};
+
+/* currently divided as follows:*/
+#if (TRACE_ENABLE_SP0 + TRACE_ENABLE_SP1 + TRACE_ENABLE_ISP == 3)
+/* can be divided as needed */
+#define TRACE_SP0_SIZE (TRACE_BUFF_SIZE / 4)
+#define TRACE_SP1_SIZE (TRACE_BUFF_SIZE / 4)
+#define TRACE_ISP_SIZE (TRACE_BUFF_SIZE / 2)
+#elif (TRACE_ENABLE_SP0 + TRACE_ENABLE_SP1 + TRACE_ENABLE_ISP == 2)
+#if TRACE_ENABLE_SP0
+#define TRACE_SP0_SIZE (TRACE_BUFF_SIZE / 2)
+#else
+#define TRACE_SP0_SIZE (0)
+#endif
+#if TRACE_ENABLE_SP1
+#define TRACE_SP1_SIZE (TRACE_BUFF_SIZE / 2)
+#else
+#define TRACE_SP1_SIZE (0)
+#endif
+#if TRACE_ENABLE_ISP
+#define TRACE_ISP_SIZE (TRACE_BUFF_SIZE / 2)
+#else
+#define TRACE_ISP_SIZE (0)
+#endif
+#elif (TRACE_ENABLE_SP0 + TRACE_ENABLE_SP1 + TRACE_ENABLE_ISP == 1)
+#if TRACE_ENABLE_SP0
+#define TRACE_SP0_SIZE (TRACE_BUFF_SIZE)
+#else
+#define TRACE_SP0_SIZE (0)
+#endif
+#if TRACE_ENABLE_SP1
+#define TRACE_SP1_SIZE (TRACE_BUFF_SIZE)
+#else
+#define TRACE_SP1_SIZE (0)
+#endif
+#if TRACE_ENABLE_ISP
+#define TRACE_ISP_SIZE (TRACE_BUFF_SIZE)
+#else
+#define TRACE_ISP_SIZE (0)
+#endif
+#else
+#define TRACE_SP0_SIZE (0)
+#define TRACE_SP1_SIZE (0)
+#define TRACE_ISP_SIZE (0)
+#endif
+
+#define TRACE_SP0_ADDR (TRACE_BUFF_ADDR)
+#define TRACE_SP1_ADDR (TRACE_SP0_ADDR + TRACE_SP0_SIZE)
+#define TRACE_ISP_ADDR (TRACE_SP1_ADDR + TRACE_SP1_SIZE)
+
+/* check if it's a legal division */
+#if (TRACE_BUFF_SIZE < TRACE_SP0_SIZE + TRACE_SP1_SIZE + TRACE_ISP_SIZE)
+#error trace sizes are not divided correctly and are above limit
+#endif
+
+#define TRACE_SP0_HEADER_ADDR (TRACE_SP0_ADDR)
+#define TRACE_SP0_HEADER_SIZE (sizeof(struct trace_header_t))
+#define TRACE_SP0_ITEM_SIZE   (sizeof(struct trace_item_t))
+#define TRACE_SP0_DATA_ADDR   (TRACE_SP0_HEADER_ADDR + TRACE_SP0_HEADER_SIZE)
+#define TRACE_SP0_DATA_SIZE   (TRACE_SP0_SIZE - TRACE_SP0_HEADER_SIZE)
+#define TRACE_SP0_MAX_POINTS  (TRACE_SP0_DATA_SIZE / TRACE_SP0_ITEM_SIZE)
+
+#define TRACE_SP1_HEADER_ADDR (TRACE_SP1_ADDR)
+#define TRACE_SP1_HEADER_SIZE (sizeof(struct trace_header_t))
+#define TRACE_SP1_ITEM_SIZE   (sizeof(struct trace_item_t))
+#define TRACE_SP1_DATA_ADDR   (TRACE_SP1_HEADER_ADDR + TRACE_SP1_HEADER_SIZE)
+#define TRACE_SP1_DATA_SIZE   (TRACE_SP1_SIZE - TRACE_SP1_HEADER_SIZE)
+#define TRACE_SP1_MAX_POINTS  (TRACE_SP1_DATA_SIZE / TRACE_SP1_ITEM_SIZE)
+
+#define TRACE_ISP_HEADER_ADDR (TRACE_ISP_ADDR)
+#define TRACE_ISP_HEADER_SIZE (sizeof(struct trace_header_t))
+#define TRACE_ISP_ITEM_SIZE   (sizeof(struct trace_item_t))
+#define TRACE_ISP_DATA_ADDR   (TRACE_ISP_HEADER_ADDR + TRACE_ISP_HEADER_SIZE)
+#define TRACE_ISP_DATA_SIZE   (TRACE_ISP_SIZE - TRACE_ISP_HEADER_SIZE)
+#define TRACE_ISP_MAX_POINTS  (TRACE_ISP_DATA_SIZE / TRACE_ISP_ITEM_SIZE)
+
+/* common majors */
+/* SP0 */
+#define MAJOR_MAIN              1
+#define MAJOR_ISP_STAGE_ENTRY   2
+#define MAJOR_DMA_PRXY          3
+#define MAJOR_START_ISP         4
+/* SP1 */
+#define MAJOR_OBSERVER_ISP0_EVENT          21
+#define MAJOR_OBSERVER_OUTPUT_FORM_EVENT   22
+#define MAJOR_OBSERVER_OUTPUT_SCAL_EVENT   23
+#define MAJOR_OBSERVER_IF_ACK              24
+#define MAJOR_OBSERVER_SP0_EVENT           25
+#define MAJOR_OBSERVER_SP_TERMINATE_EVENT  26
+#define MAJOR_OBSERVER_DMA_ACK             27
+#define MAJOR_OBSERVER_ACC_ACK             28
+
+#define DEBUG_PTR_SIGNATURE     0xABCD	/* signature for the debug parameter pointer */
+
+/* command codes (1st byte) */
+typedef enum {
+	CMD_SET_ONE_MAJOR = 1,		/* mask in one major. 2nd byte in the command is the major code */
+	CMD_UNSET_ONE_MAJOR = 2,	/* mask out one major. 2nd byte in the command is the major code */
+	CMD_SET_ALL_MAJORS = 3,		/* set the major print mask. the full mask is in the data DWORD */
+	CMD_SET_VERBOSITY = 4		/* set verbosity level */
+} DBG_commands;
+
+/* command signature */
+#define CMD_SIGNATURE	0xAABBCC00
+
+/* shared macros in traces infrastructure */
+/* increment the pointer cyclicly */
+#define DBG_NEXT_ITEM(x, max_items) (((x + 1) >= max_items) ? 0 : x + 1)
+#define DBG_PREV_ITEM(x, max_items) ((x) ? x - 1 : max_items - 1)
+
+#define FIELD_MASK(width) (((1 << (width)) - 1))
+#define FIELD_PACK(value, mask, offset) (((value) & (mask)) << (offset))
+#define FIELD_UNPACK(value, mask, offset) (((value) >> (offset)) & (mask))
+
+#define FIELD_VALUE_OFFSET		(0)
+#define FIELD_VALUE_WIDTH		(16)
+#define FIELD_VALUE_MASK		FIELD_MASK(FIELD_VALUE_WIDTH)
+#define FIELD_VALUE_PACK(f)		FIELD_PACK(f, FIELD_VALUE_MASK, FIELD_VALUE_OFFSET)
+#define FIELD_VALUE_UNPACK(f)		FIELD_UNPACK(f, FIELD_VALUE_MASK, FIELD_VALUE_OFFSET)
+
+#define FIELD_MINOR_OFFSET		(FIELD_VALUE_OFFSET + FIELD_VALUE_WIDTH)
+#define FIELD_MINOR_WIDTH		(8)
+#define FIELD_MINOR_MASK		FIELD_MASK(FIELD_MINOR_WIDTH)
+#define FIELD_MINOR_PACK(f)		FIELD_PACK(f, FIELD_MINOR_MASK, FIELD_MINOR_OFFSET)
+#define FIELD_MINOR_UNPACK(f)		FIELD_UNPACK(f, FIELD_MINOR_MASK, FIELD_MINOR_OFFSET)
+
+#define FIELD_MAJOR_OFFSET		(FIELD_MINOR_OFFSET + FIELD_MINOR_WIDTH)
+#define FIELD_MAJOR_WIDTH		(5)
+#define FIELD_MAJOR_MASK		FIELD_MASK(FIELD_MAJOR_WIDTH)
+#define FIELD_MAJOR_PACK(f)		FIELD_PACK(f, FIELD_MAJOR_MASK, FIELD_MAJOR_OFFSET)
+#define FIELD_MAJOR_UNPACK(f)		FIELD_UNPACK(f, FIELD_MAJOR_MASK, FIELD_MAJOR_OFFSET)
+
+/* for quick traces - only insertion, compatible with the regular point */
+#define FIELD_FULL_MAJOR_WIDTH		(8)
+#define FIELD_FULL_MAJOR_MASK		FIELD_MASK(FIELD_FULL_MAJOR_WIDTH)
+#define FIELD_FULL_MAJOR_PACK(f)	FIELD_PACK(f, FIELD_FULL_MAJOR_MASK, FIELD_MAJOR_OFFSET)
+
+/* The following 2 fields are used only when FIELD_TID value is 111b.
+ * it means we don't want to use thread id, but format. In this case,
+ * the last 2 MSB bits of the major field will indicates the format
+ */
+#define FIELD_MAJOR_W_FMT_OFFSET	FIELD_MAJOR_OFFSET
+#define FIELD_MAJOR_W_FMT_WIDTH		(3)
+#define FIELD_MAJOR_W_FMT_MASK		FIELD_MASK(FIELD_MAJOR_W_FMT_WIDTH)
+#define FIELD_MAJOR_W_FMT_PACK(f)	FIELD_PACK(f, FIELD_MAJOR_W_FMT_MASK, FIELD_MAJOR_W_FMT_OFFSET)
+#define FIELD_MAJOR_W_FMT_UNPACK(f)	FIELD_UNPACK(f, FIELD_MAJOR_W_FMT_MASK, FIELD_MAJOR_W_FMT_OFFSET)
+
+#define FIELD_FORMAT_OFFSET		(FIELD_MAJOR_OFFSET + FIELD_MAJOR_W_FMT_WIDTH)
+#define FIELD_FORMAT_WIDTH		(2)
+#define FIELD_FORMAT_MASK		FIELD_MASK(FIELD_MAJOR_W_FMT_WIDTH)
+#define FIELD_FORMAT_PACK(f)		FIELD_PACK(f, FIELD_FORMAT_MASK, FIELD_FORMAT_OFFSET)
+#define FIELD_FORMAT_UNPACK(f)		FIELD_UNPACK(f, FIELD_FORMAT_MASK, FIELD_FORMAT_OFFSET)
+
+#define FIELD_TID_SEL_FORMAT_PAT	(7)
+
+#define FIELD_TID_OFFSET		(FIELD_MAJOR_OFFSET + FIELD_MAJOR_WIDTH)
+#define FIELD_TID_WIDTH			(3)
+#define FIELD_TID_MASK			FIELD_MASK(FIELD_TID_WIDTH)
+#define FIELD_TID_PACK(f)		FIELD_PACK(f, FIELD_TID_MASK, FIELD_TID_OFFSET)
+#define FIELD_TID_UNPACK(f)		FIELD_UNPACK(f, FIELD_TID_MASK, FIELD_TID_OFFSET)
+
+#define FIELD_VALUE_24_OFFSET		(0)
+#define FIELD_VALUE_24_WIDTH		(24)
+#define FIELD_VALUE_24_MASK		FIELD_MASK(FIELD_VALUE_24_WIDTH)
+#define FIELD_VALUE_24_PACK(f)		FIELD_PACK(f, FIELD_VALUE_24_MASK, FIELD_VALUE_24_OFFSET)
+#define FIELD_VALUE_24_UNPACK(f)	FIELD_UNPACK(f, FIELD_VALUE_24_MASK, FIELD_VALUE_24_OFFSET)
+
+#define PACK_TRACEPOINT(tid, major, minor, value)	\
+	(FIELD_TID_PACK(tid) | FIELD_MAJOR_PACK(major) | FIELD_MINOR_PACK(minor) | FIELD_VALUE_PACK(value))
+
+#define PACK_QUICK_TRACEPOINT(major, minor)	\
+	(FIELD_FULL_MAJOR_PACK(major) | FIELD_MINOR_PACK(minor))
+
+#define PACK_FORMATTED_TRACEPOINT(format, major, minor, value)	\
+	(FIELD_TID_PACK(FIELD_TID_SEL_FORMAT_PAT) | FIELD_FORMAT_PACK(format) | FIELD_MAJOR_PACK(major) | FIELD_MINOR_PACK(minor) | FIELD_VALUE_PACK(value))
+
+#define PACK_TRACE_VALUE24(major, value)	\
+	(FIELD_TID_PACK(FIELD_TID_SEL_FORMAT_PAT) | FIELD_MAJOR_PACK(major) | FIELD_VALUE_24_PACK(value))
+
+#endif /* __CSS_TRACE_H_ */
diff --git a/drivers/staging/media/atomisp/pci/defs.h b/drivers/staging/media/atomisp/pci/defs.h
new file mode 100644
index 000000000..785e7a670
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/defs.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _HRT_DEFS_H_
+#define _HRT_DEFS_H_
+
+#ifndef HRTCAT
+#define _HRTCAT(m, n)     m##n
+#define HRTCAT(m, n)      _HRTCAT(m, n)
+#endif
+
+#ifndef HRTSTR
+#define _HRTSTR(x)   #x
+#define HRTSTR(x)    _HRTSTR(x)
+#endif
+
+#ifndef HRTMIN
+#define HRTMIN(a, b) (((a) < (b)) ? (a) : (b))
+#endif
+
+#ifndef HRTMAX
+#define HRTMAX(a, b) (((a) > (b)) ? (a) : (b))
+#endif
+
+#endif /* _HRT_DEFS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/dma_v2_defs.h b/drivers/staging/media/atomisp/pci/dma_v2_defs.h
new file mode 100644
index 000000000..27299e3a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/dma_v2_defs.h
@@ -0,0 +1,200 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _dma_v2_defs_h
+#define _dma_v2_defs_h
+
+#define _DMA_V2_NUM_CHANNELS_ID               MaxNumChannels
+#define _DMA_V2_CONNECTIONS_ID                Connections
+#define _DMA_V2_DEV_ELEM_WIDTHS_ID            DevElemWidths
+#define _DMA_V2_DEV_FIFO_DEPTH_ID             DevFifoDepth
+#define _DMA_V2_DEV_FIFO_RD_LAT_ID            DevFifoRdLat
+#define _DMA_V2_DEV_FIFO_LAT_BYPASS_ID        DevFifoRdLatBypass
+#define _DMA_V2_DEV_NO_BURST_ID               DevNoBurst
+#define _DMA_V2_DEV_RD_ACCEPT_ID              DevRdAccept
+#define _DMA_V2_DEV_SRMD_ID                   DevSRMD
+#define _DMA_V2_DEV_HAS_CRUN_ID               CRunMasters
+#define _DMA_V2_CTRL_ACK_FIFO_DEPTH_ID        CtrlAckFifoDepth
+#define _DMA_V2_CMD_FIFO_DEPTH_ID             CommandFifoDepth
+#define _DMA_V2_CMD_FIFO_RD_LAT_ID            CommandFifoRdLat
+#define _DMA_V2_CMD_FIFO_LAT_BYPASS_ID        CommandFifoRdLatBypass
+#define _DMA_V2_NO_PACK_ID                    has_no_pack
+
+#define _DMA_V2_REG_ALIGN                4
+#define _DMA_V2_REG_ADDR_BITS            2
+
+/* Command word */
+#define _DMA_V2_CMD_IDX            0
+#define _DMA_V2_CMD_BITS           6
+#define _DMA_V2_CHANNEL_IDX        (_DMA_V2_CMD_IDX + _DMA_V2_CMD_BITS)
+#define _DMA_V2_CHANNEL_BITS       5
+
+/* The command to set a parameter contains the PARAM field next */
+#define _DMA_V2_PARAM_IDX          (_DMA_V2_CHANNEL_IDX + _DMA_V2_CHANNEL_BITS)
+#define _DMA_V2_PARAM_BITS         4
+
+/* Commands to read, write or init specific blocks contain these
+   three values */
+#define _DMA_V2_SPEC_DEV_A_XB_IDX  (_DMA_V2_CHANNEL_IDX + _DMA_V2_CHANNEL_BITS)
+#define _DMA_V2_SPEC_DEV_A_XB_BITS 8
+#define _DMA_V2_SPEC_DEV_B_XB_IDX  (_DMA_V2_SPEC_DEV_A_XB_IDX + _DMA_V2_SPEC_DEV_A_XB_BITS)
+#define _DMA_V2_SPEC_DEV_B_XB_BITS 8
+#define _DMA_V2_SPEC_YB_IDX        (_DMA_V2_SPEC_DEV_B_XB_IDX + _DMA_V2_SPEC_DEV_B_XB_BITS)
+#define _DMA_V2_SPEC_YB_BITS       (32 - _DMA_V2_SPEC_DEV_B_XB_BITS - _DMA_V2_SPEC_DEV_A_XB_BITS - _DMA_V2_CMD_BITS - _DMA_V2_CHANNEL_BITS)
+
+/* */
+#define _DMA_V2_CMD_CTRL_IDX       4
+#define _DMA_V2_CMD_CTRL_BITS      4
+
+/* Packing setup word */
+#define _DMA_V2_CONNECTION_IDX     0
+#define _DMA_V2_CONNECTION_BITS    4
+#define _DMA_V2_EXTENSION_IDX      (_DMA_V2_CONNECTION_IDX + _DMA_V2_CONNECTION_BITS)
+#define _DMA_V2_EXTENSION_BITS     1
+
+/* Elements packing word */
+#define _DMA_V2_ELEMENTS_IDX        0
+#define _DMA_V2_ELEMENTS_BITS       8
+#define _DMA_V2_LEFT_CROPPING_IDX  (_DMA_V2_ELEMENTS_IDX + _DMA_V2_ELEMENTS_BITS)
+#define _DMA_V2_LEFT_CROPPING_BITS  8
+
+#define _DMA_V2_WIDTH_IDX           0
+#define _DMA_V2_WIDTH_BITS         16
+
+#define _DMA_V2_HEIGHT_IDX          0
+#define _DMA_V2_HEIGHT_BITS        16
+
+#define _DMA_V2_STRIDE_IDX          0
+#define _DMA_V2_STRIDE_BITS        32
+
+/* Command IDs */
+#define _DMA_V2_MOVE_B2A_COMMAND                             0
+#define _DMA_V2_MOVE_B2A_BLOCK_COMMAND                       1
+#define _DMA_V2_MOVE_B2A_NO_SYNC_CHK_COMMAND                 2
+#define _DMA_V2_MOVE_B2A_BLOCK_NO_SYNC_CHK_COMMAND           3
+#define _DMA_V2_MOVE_A2B_COMMAND                             4
+#define _DMA_V2_MOVE_A2B_BLOCK_COMMAND                       5
+#define _DMA_V2_MOVE_A2B_NO_SYNC_CHK_COMMAND                 6
+#define _DMA_V2_MOVE_A2B_BLOCK_NO_SYNC_CHK_COMMAND           7
+#define _DMA_V2_INIT_A_COMMAND                               8
+#define _DMA_V2_INIT_A_BLOCK_COMMAND                         9
+#define _DMA_V2_INIT_A_NO_SYNC_CHK_COMMAND                  10
+#define _DMA_V2_INIT_A_BLOCK_NO_SYNC_CHK_COMMAND            11
+#define _DMA_V2_INIT_B_COMMAND                              12
+#define _DMA_V2_INIT_B_BLOCK_COMMAND                        13
+#define _DMA_V2_INIT_B_NO_SYNC_CHK_COMMAND                  14
+#define _DMA_V2_INIT_B_BLOCK_NO_SYNC_CHK_COMMAND            15
+#define _DMA_V2_NO_ACK_MOVE_B2A_NO_SYNC_CHK_COMMAND         (_DMA_V2_MOVE_B2A_NO_SYNC_CHK_COMMAND       + 16)
+#define _DMA_V2_NO_ACK_MOVE_B2A_BLOCK_NO_SYNC_CHK_COMMAND   (_DMA_V2_MOVE_B2A_BLOCK_NO_SYNC_CHK_COMMAND + 16)
+#define _DMA_V2_NO_ACK_MOVE_A2B_NO_SYNC_CHK_COMMAND         (_DMA_V2_MOVE_A2B_NO_SYNC_CHK_COMMAND       + 16)
+#define _DMA_V2_NO_ACK_MOVE_A2B_BLOCK_NO_SYNC_CHK_COMMAND   (_DMA_V2_MOVE_A2B_BLOCK_NO_SYNC_CHK_COMMAND + 16)
+#define _DMA_V2_NO_ACK_INIT_A_NO_SYNC_CHK_COMMAND           (_DMA_V2_INIT_A_NO_SYNC_CHK_COMMAND         + 16)
+#define _DMA_V2_NO_ACK_INIT_A_BLOCK_NO_SYNC_CHK_COMMAND     (_DMA_V2_INIT_A_BLOCK_NO_SYNC_CHK_COMMAND   + 16)
+#define _DMA_V2_NO_ACK_INIT_B_NO_SYNC_CHK_COMMAND           (_DMA_V2_INIT_B_NO_SYNC_CHK_COMMAND         + 16)
+#define _DMA_V2_NO_ACK_INIT_B_BLOCK_NO_SYNC_CHK_COMMAND     (_DMA_V2_INIT_B_BLOCK_NO_SYNC_CHK_COMMAND   + 16)
+#define _DMA_V2_CONFIG_CHANNEL_COMMAND                      32
+#define _DMA_V2_SET_CHANNEL_PARAM_COMMAND                   33
+#define _DMA_V2_SET_CRUN_COMMAND                            62
+
+/* Channel Parameter IDs */
+#define _DMA_V2_PACKING_SETUP_PARAM                     0
+#define _DMA_V2_STRIDE_A_PARAM                          1
+#define _DMA_V2_ELEM_CROPPING_A_PARAM                   2
+#define _DMA_V2_WIDTH_A_PARAM                           3
+#define _DMA_V2_STRIDE_B_PARAM                          4
+#define _DMA_V2_ELEM_CROPPING_B_PARAM                   5
+#define _DMA_V2_WIDTH_B_PARAM                           6
+#define _DMA_V2_HEIGHT_PARAM                            7
+#define _DMA_V2_QUEUED_CMDS                             8
+
+/* Parameter Constants */
+#define _DMA_V2_ZERO_EXTEND                             0
+#define _DMA_V2_SIGN_EXTEND                             1
+
+/* SLAVE address map */
+#define _DMA_V2_SEL_FSM_CMD                             0
+#define _DMA_V2_SEL_CH_REG                              1
+#define _DMA_V2_SEL_CONN_GROUP                          2
+#define _DMA_V2_SEL_DEV_INTERF                          3
+
+#define _DMA_V2_ADDR_SEL_COMP_IDX                      12
+#define _DMA_V2_ADDR_SEL_COMP_BITS                      4
+#define _DMA_V2_ADDR_SEL_CH_REG_IDX                     2
+#define _DMA_V2_ADDR_SEL_CH_REG_BITS                    6
+#define _DMA_V2_ADDR_SEL_PARAM_IDX                      (_DMA_V2_ADDR_SEL_CH_REG_BITS + _DMA_V2_ADDR_SEL_CH_REG_IDX)
+#define _DMA_V2_ADDR_SEL_PARAM_BITS                     4
+
+#define _DMA_V2_ADDR_SEL_GROUP_COMP_IDX                 2
+#define _DMA_V2_ADDR_SEL_GROUP_COMP_BITS                6
+#define _DMA_V2_ADDR_SEL_GROUP_COMP_INFO_IDX            (_DMA_V2_ADDR_SEL_GROUP_COMP_BITS + _DMA_V2_ADDR_SEL_GROUP_COMP_IDX)
+#define _DMA_V2_ADDR_SEL_GROUP_COMP_INFO_BITS           4
+
+#define _DMA_V2_ADDR_SEL_DEV_INTERF_IDX_IDX             2
+#define _DMA_V2_ADDR_SEL_DEV_INTERF_IDX_BITS            6
+#define _DMA_V2_ADDR_SEL_DEV_INTERF_INFO_IDX            (_DMA_V2_ADDR_SEL_DEV_INTERF_IDX_IDX + _DMA_V2_ADDR_SEL_DEV_INTERF_IDX_BITS)
+#define _DMA_V2_ADDR_SEL_DEV_INTERF_INFO_BITS           4
+
+#define _DMA_V2_FSM_GROUP_CMD_IDX                       0
+#define _DMA_V2_FSM_GROUP_ADDR_SRC_IDX                  1
+#define _DMA_V2_FSM_GROUP_ADDR_DEST_IDX                 2
+#define _DMA_V2_FSM_GROUP_CMD_CTRL_IDX                  3
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_IDX                  4
+#define _DMA_V2_FSM_GROUP_FSM_PACK_IDX                  5
+#define _DMA_V2_FSM_GROUP_FSM_REQ_IDX                   6
+#define _DMA_V2_FSM_GROUP_FSM_WR_IDX                    7
+
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_STATE_IDX            0
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_REQ_DEV_IDX          1
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_REQ_ADDR_IDX         2
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_REQ_STRIDE_IDX       3
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_REQ_XB_IDX           4
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_REQ_YB_IDX           5
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_REQ_DEV_IDX     6
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_DEV_IDX      7
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_WR_ADDR_IDX          8
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_WR_STRIDE_IDX        9
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_REQ_XB_IDX     10
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_YB_IDX      11
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_XB_IDX      12
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_ELEM_REQ_IDX   13
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_ELEM_WR_IDX    14
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_PACK_S_Z_IDX        15
+#define _DMA_V2_FSM_GROUP_FSM_CTRL_CMD_CTRL_IDX        15
+
+#define _DMA_V2_FSM_GROUP_FSM_PACK_STATE_IDX            0
+#define _DMA_V2_FSM_GROUP_FSM_PACK_CNT_YB_IDX           1
+#define _DMA_V2_FSM_GROUP_FSM_PACK_CNT_XB_REQ_IDX       2
+#define _DMA_V2_FSM_GROUP_FSM_PACK_CNT_XB_WR_IDX        3
+
+#define _DMA_V2_FSM_GROUP_FSM_REQ_STATE_IDX             0
+#define _DMA_V2_FSM_GROUP_FSM_REQ_CNT_YB_IDX            1
+#define _DMA_V2_FSM_GROUP_FSM_REQ_CNT_XB_IDX            2
+#define _DMA_V2_FSM_GROUP_FSM_REQ_XB_REMAINING_IDX      3
+#define _DMA_V2_FSM_GROUP_FSM_REQ_CNT_BURST_IDX         4
+
+#define _DMA_V2_FSM_GROUP_FSM_WR_STATE_IDX              0
+#define _DMA_V2_FSM_GROUP_FSM_WR_CNT_YB_IDX             1
+#define _DMA_V2_FSM_GROUP_FSM_WR_CNT_XB_IDX             2
+#define _DMA_V2_FSM_GROUP_FSM_WR_XB_REMAINING_IDX       3
+#define _DMA_V2_FSM_GROUP_FSM_WR_CNT_BURST_IDX          4
+
+#define _DMA_V2_DEV_INTERF_REQ_SIDE_STATUS_IDX          0
+#define _DMA_V2_DEV_INTERF_SEND_SIDE_STATUS_IDX         1
+#define _DMA_V2_DEV_INTERF_FIFO_STATUS_IDX              2
+#define _DMA_V2_DEV_INTERF_REQ_ONLY_COMPLETE_BURST_IDX  3
+#define _DMA_V2_DEV_INTERF_MAX_BURST_IDX                4
+#define _DMA_V2_DEV_INTERF_CHK_ADDR_ALIGN               5
+
+#endif /* _dma_v2_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/gdc_v2_defs.h b/drivers/staging/media/atomisp/pci/gdc_v2_defs.h
new file mode 100644
index 000000000..804df8179
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/gdc_v2_defs.h
@@ -0,0 +1,164 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef HRT_GDC_v2_defs_h_
+#define HRT_GDC_v2_defs_h_
+
+#define HRT_GDC_IS_V2
+
+#define HRT_GDC_N                     1024 /* Top-level design constant, equal to the number of entries in the LUT      */
+#define HRT_GDC_FRAC_BITS               10 /* Number of fractional bits in the GDC block, driven by the size of the LUT */
+
+#define HRT_GDC_BLI_FRAC_BITS            4 /* Number of fractional bits for the bi-linear interpolation type            */
+#define HRT_GDC_BLI_COEF_ONE             BIT(HRT_GDC_BLI_FRAC_BITS)
+
+#define HRT_GDC_BCI_COEF_BITS           14 /* 14 bits per coefficient                                                   */
+#define HRT_GDC_BCI_COEF_ONE             (1 << (HRT_GDC_BCI_COEF_BITS - 2))  /* We represent signed 10 bit coefficients.  */
+/* The supported range is [-256, .., +256]      */
+/* in 14-bit signed notation,                   */
+/* We need all ten bits (MSB must be zero).     */
+/* -s is inserted to solve this issue, and      */
+/* therefore "1" is equal to +256.              */
+#define HRT_GDC_BCI_COEF_MASK            ((1 << HRT_GDC_BCI_COEF_BITS) - 1)
+
+#define HRT_GDC_LUT_BYTES                (HRT_GDC_N * 4 * 2)                /* 1024 addresses, 4 coefficients per address,  */
+/* 2 bytes per coefficient                      */
+
+#define _HRT_GDC_REG_ALIGN               4
+
+//     31  30  29    25 24                     0
+//  |-----|---|--------|------------------------|
+//  | CMD | C | Reg_ID |        Value           |
+
+// There are just two commands possible for the GDC block:
+// 1 - Configure reg
+// 0 - Data token
+
+// C      - Reserved bit
+//          Used in protocol to indicate whether it is C-run or other type of runs
+//          In case of C-run, this bit has a value of 1, for all the other runs, it is 0.
+
+// Reg_ID - Address of the register to be configured
+
+// Value  - Value to store to the addressed register, maximum of 24 bits
+
+// Configure reg command is not followed by any other token.
+// The address of the register and the data to be filled in is contained in the same token
+
+// When the first data token is received, it must be:
+//   1. FRX and FRY (device configured in one of the  scaling modes) ***DEFAULT MODE***, or,
+//   2. P0'X        (device configured in one of the tetragon modes)
+// After the first data token is received, pre-defined number of tokens with the following meaning follow:
+//   1. two  tokens: SRC address ; DST address
+//   2. nine tokens: P0'Y, .., P3'Y ; SRC address ; DST address
+
+#define HRT_GDC_CONFIG_CMD             1
+#define HRT_GDC_DATA_CMD               0
+
+#define HRT_GDC_CMD_POS               31
+#define HRT_GDC_CMD_BITS               1
+#define HRT_GDC_CRUN_POS              30
+#define HRT_GDC_REG_ID_POS            25
+#define HRT_GDC_REG_ID_BITS            5
+#define HRT_GDC_DATA_POS               0
+#define HRT_GDC_DATA_BITS             25
+
+#define HRT_GDC_FRYIPXFRX_BITS        26
+#define HRT_GDC_P0X_BITS              23
+
+#define HRT_GDC_MAX_OXDIM           (8192 - 64)
+#define HRT_GDC_MAX_OYDIM           4095
+#define HRT_GDC_MAX_IXDIM           (8192 - 64)
+#define HRT_GDC_MAX_IYDIM           4095
+#define HRT_GDC_MAX_DS_FAC            16
+#define HRT_GDC_MAX_DX                 (HRT_GDC_MAX_DS_FAC * HRT_GDC_N - 1)
+#define HRT_GDC_MAX_DY                 HRT_GDC_MAX_DX
+
+/* GDC lookup tables entries are 10 bits values, but they're
+   stored 2 by 2 as 32 bit values, yielding 16 bits per entry.
+   A GDC lookup table contains 64 * 4 elements */
+
+#define HRT_GDC_PERF_1_1_pix          0
+#define HRT_GDC_PERF_2_1_pix          1
+#define HRT_GDC_PERF_1_2_pix          2
+#define HRT_GDC_PERF_2_2_pix          3
+
+#define HRT_GDC_NND_MODE              0
+#define HRT_GDC_BLI_MODE              1
+#define HRT_GDC_BCI_MODE              2
+#define HRT_GDC_LUT_MODE              3
+
+#define HRT_GDC_SCAN_STB              0
+#define HRT_GDC_SCAN_STR              1
+
+#define HRT_GDC_MODE_SCALING          0
+#define HRT_GDC_MODE_TETRAGON         1
+
+#define HRT_GDC_LUT_COEFF_OFFSET     16
+#define HRT_GDC_FRY_BIT_OFFSET       16
+// FRYIPXFRX is the only register where we store two values in one field,
+// to save one token in the scaling protocol.
+// Like this, we have three tokens in the scaling protocol,
+// Otherwise, we would have had four.
+// The register bit-map is:
+//   31  26 25      16 15  10 9        0
+//  |------|----------|------|----------|
+//  | XXXX |   FRY    |  IPX |   FRX    |
+
+#define HRT_GDC_CE_FSM0_POS           0
+#define HRT_GDC_CE_FSM0_LEN           2
+#define HRT_GDC_CE_OPY_POS            2
+#define HRT_GDC_CE_OPY_LEN           14
+#define HRT_GDC_CE_OPX_POS           16
+#define HRT_GDC_CE_OPX_LEN           16
+// CHK_ENGINE register bit-map:
+//   31            16 15        2 1  0
+//  |----------------|-----------|----|
+//  |      OPX       |    OPY    |FSM0|
+// However, for the time being at least,
+// this implementation is meaningless in hss model,
+// So, we just return 0
+
+#define HRT_GDC_CHK_ENGINE_IDX        0
+#define HRT_GDC_WOIX_IDX              1
+#define HRT_GDC_WOIY_IDX              2
+#define HRT_GDC_BPP_IDX               3
+#define HRT_GDC_FRYIPXFRX_IDX         4
+#define HRT_GDC_OXDIM_IDX             5
+#define HRT_GDC_OYDIM_IDX             6
+#define HRT_GDC_SRC_ADDR_IDX          7
+#define HRT_GDC_SRC_END_ADDR_IDX      8
+#define HRT_GDC_SRC_WRAP_ADDR_IDX     9
+#define HRT_GDC_SRC_STRIDE_IDX       10
+#define HRT_GDC_DST_ADDR_IDX         11
+#define HRT_GDC_DST_STRIDE_IDX       12
+#define HRT_GDC_DX_IDX               13
+#define HRT_GDC_DY_IDX               14
+#define HRT_GDC_P0X_IDX              15
+#define HRT_GDC_P0Y_IDX              16
+#define HRT_GDC_P1X_IDX              17
+#define HRT_GDC_P1Y_IDX              18
+#define HRT_GDC_P2X_IDX              19
+#define HRT_GDC_P2Y_IDX              20
+#define HRT_GDC_P3X_IDX              21
+#define HRT_GDC_P3Y_IDX              22
+#define HRT_GDC_PERF_POINT_IDX       23  // 1x1 ; 1x2 ; 2x1 ; 2x2 pixels per cc
+#define HRT_GDC_INTERP_TYPE_IDX      24  // NND ; BLI ; BCI ; LUT
+#define HRT_GDC_SCAN_IDX             25  // 0 = STB (Slide To Bottom) ; 1 = STR (Slide To Right)
+#define HRT_GDC_PROC_MODE_IDX        26  // 0 = Scaling ; 1 = Tetragon
+
+#define HRT_GDC_LUT_IDX              32
+
+#endif /* HRT_GDC_v2_defs_h_ */
diff --git a/drivers/staging/media/atomisp/pci/gp_timer_defs.h b/drivers/staging/media/atomisp/pci/gp_timer_defs.h
new file mode 100644
index 000000000..9bc04e5b4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/gp_timer_defs.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _gp_timer_defs_h
+#define _gp_timer_defs_h
+
+#define _HRT_GP_TIMER_REG_ALIGN 4
+
+#define HIVE_GP_TIMER_RESET_REG_IDX                              0
+#define HIVE_GP_TIMER_OVERALL_ENABLE_REG_IDX                     1
+#define HIVE_GP_TIMER_ENABLE_REG_IDX(timer)                     (HIVE_GP_TIMER_OVERALL_ENABLE_REG_IDX + 1 + timer)
+#define HIVE_GP_TIMER_VALUE_REG_IDX(timer, timers)               (HIVE_GP_TIMER_ENABLE_REG_IDX(timers) + timer)
+#define HIVE_GP_TIMER_COUNT_TYPE_REG_IDX(timer, timers)          (HIVE_GP_TIMER_VALUE_REG_IDX(timers, timers) + timer)
+#define HIVE_GP_TIMER_SIGNAL_SELECT_REG_IDX(timer, timers)       (HIVE_GP_TIMER_COUNT_TYPE_REG_IDX(timers, timers) + timer)
+#define HIVE_GP_TIMER_IRQ_TRIGGER_VALUE_REG_IDX(irq, timers)     (HIVE_GP_TIMER_SIGNAL_SELECT_REG_IDX(timers, timers) + irq)
+#define HIVE_GP_TIMER_IRQ_TIMER_SELECT_REG_IDX(irq, timers, irqs) (HIVE_GP_TIMER_IRQ_TRIGGER_VALUE_REG_IDX(irqs, timers) + irq)
+#define HIVE_GP_TIMER_IRQ_ENABLE_REG_IDX(irq, timers, irqs)       (HIVE_GP_TIMER_IRQ_TIMER_SELECT_REG_IDX(irqs, timers, irqs) + irq)
+
+#define HIVE_GP_TIMER_COUNT_TYPE_HIGH                            0
+#define HIVE_GP_TIMER_COUNT_TYPE_LOW                             1
+#define HIVE_GP_TIMER_COUNT_TYPE_POSEDGE                         2
+#define HIVE_GP_TIMER_COUNT_TYPE_NEGEDGE                         3
+#define HIVE_GP_TIMER_COUNT_TYPES                                4
+
+#endif /* _gp_timer_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/gpio_block_defs.h b/drivers/staging/media/atomisp/pci/gpio_block_defs.h
new file mode 100644
index 000000000..e1bd638d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/gpio_block_defs.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _gpio_block_defs_h_
+#define _gpio_block_defs_h_
+
+#define _HRT_GPIO_BLOCK_REG_ALIGN 4
+
+/* R/W registers */
+#define _gpio_block_reg_do_e				 0
+#define _gpio_block_reg_do_select		       1
+#define _gpio_block_reg_do_0				 2
+#define _gpio_block_reg_do_1				 3
+#define _gpio_block_reg_do_pwm_cnt_0	     4
+#define _gpio_block_reg_do_pwm_cnt_1	     5
+#define _gpio_block_reg_do_pwm_cnt_2	     6
+#define _gpio_block_reg_do_pwm_cnt_3	     7
+#define _gpio_block_reg_do_pwm_main_cnt    8
+#define _gpio_block_reg_do_pwm_enable      9
+#define _gpio_block_reg_di_debounce_sel	  10
+#define _gpio_block_reg_di_debounce_cnt_0	11
+#define _gpio_block_reg_di_debounce_cnt_1	12
+#define _gpio_block_reg_di_debounce_cnt_2	13
+#define _gpio_block_reg_di_debounce_cnt_3	14
+#define _gpio_block_reg_di_active_level	  15
+
+/* read-only registers */
+#define _gpio_block_reg_di				  16
+
+#endif /* _gpio_block_defs_h_ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/debug_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/debug_global.h
new file mode 100644
index 000000000..b6538beca
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/debug_global.h
@@ -0,0 +1,82 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DEBUG_GLOBAL_H_INCLUDED__
+#define __DEBUG_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#define DEBUG_BUF_SIZE	1024
+#define DEBUG_BUF_MASK	(DEBUG_BUF_SIZE - 1)
+
+#define DEBUG_DATA_ENABLE_ADDR		0x00
+#define DEBUG_DATA_BUF_MODE_ADDR	0x04
+#define DEBUG_DATA_HEAD_ADDR		0x08
+#define DEBUG_DATA_TAIL_ADDR		0x0C
+#define DEBUG_DATA_BUF_ADDR			0x10
+
+#define DEBUG_DATA_ENABLE_DDR_ADDR		0x00
+#define DEBUG_DATA_BUF_MODE_DDR_ADDR	HIVE_ISP_DDR_WORD_BYTES
+#define DEBUG_DATA_HEAD_DDR_ADDR		(2 * HIVE_ISP_DDR_WORD_BYTES)
+#define DEBUG_DATA_TAIL_DDR_ADDR		(3 * HIVE_ISP_DDR_WORD_BYTES)
+#define DEBUG_DATA_BUF_DDR_ADDR			(4 * HIVE_ISP_DDR_WORD_BYTES)
+
+#define DEBUG_BUFFER_ISP_DMEM_ADDR       0x0
+
+/*
+ * Enable HAS_WATCHDOG_SP_THREAD_DEBUG for additional SP thread and
+ * pipe information on watchdog output
+ * #undef HAS_WATCHDOG_SP_THREAD_DEBUG
+ * #define HAS_WATCHDOG_SP_THREAD_DEBUG
+ */
+
+/*
+ * The linear buffer mode will accept data until the first
+ * overflow and then stop accepting new data
+ * The circular buffer mode will accept if there is place
+ * and discard the data if the buffer is full
+ */
+typedef enum {
+	DEBUG_BUFFER_MODE_LINEAR = 0,
+	DEBUG_BUFFER_MODE_CIRCULAR,
+	N_DEBUG_BUFFER_MODE
+} debug_buf_mode_t;
+
+struct debug_data_s {
+	u32			enable;
+	u32			bufmode;
+	u32			head;
+	u32			tail;
+	u32			buf[DEBUG_BUF_SIZE];
+};
+
+/* thread.sp.c doesn't have a notion of HIVE_ISP_DDR_WORD_BYTES
+   still one point of control is needed for debug purposes */
+
+#ifdef HIVE_ISP_DDR_WORD_BYTES
+struct debug_data_ddr_s {
+	u32			enable;
+	s8				padding1[HIVE_ISP_DDR_WORD_BYTES - sizeof(uint32_t)];
+	u32			bufmode;
+	s8				padding2[HIVE_ISP_DDR_WORD_BYTES - sizeof(uint32_t)];
+	u32			head;
+	s8				padding3[HIVE_ISP_DDR_WORD_BYTES - sizeof(uint32_t)];
+	u32			tail;
+	s8				padding4[HIVE_ISP_DDR_WORD_BYTES - sizeof(uint32_t)];
+	u32			buf[DEBUG_BUF_SIZE];
+};
+#endif
+
+#endif /* __DEBUG_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/dma_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/dma_global.h
new file mode 100644
index 000000000..135034c72
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/dma_global.h
@@ -0,0 +1,255 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DMA_GLOBAL_H_INCLUDED__
+#define __DMA_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#define IS_DMA_VERSION_2
+
+#define HIVE_ISP_NUM_DMA_CONNS		3
+#define HIVE_ISP_NUM_DMA_CHANNELS	32
+
+#define N_DMA_CHANNEL_ID	HIVE_ISP_NUM_DMA_CHANNELS
+
+#include "dma_v2_defs.h"
+
+/*
+ * Command token bit mappings
+ *
+ * transfer / config
+ *    param id[4] channel id[5] cmd id[6]
+ *	| b14 .. b11 | b10 ... b6 | b5 ... b0 |
+ *
+ *
+ * fast transfer:
+ *    height[5]     width[8]      width[8]  channel id[5] cmd id[6]
+ *	| b31 .. b26 | b25 .. b18 | b17 .. b11 | b10 ... b6 | b5 ... b0 |
+ *
+ */
+
+#define _DMA_PACKING_SETUP_PARAM	_DMA_V2_PACKING_SETUP_PARAM
+#define _DMA_HEIGHT_PARAM			_DMA_V2_HEIGHT_PARAM
+#define _DMA_STRIDE_A_PARAM			_DMA_V2_STRIDE_A_PARAM
+#define _DMA_ELEM_CROPPING_A_PARAM	_DMA_V2_ELEM_CROPPING_A_PARAM
+#define _DMA_WIDTH_A_PARAM			_DMA_V2_WIDTH_A_PARAM
+#define _DMA_STRIDE_B_PARAM			_DMA_V2_STRIDE_B_PARAM
+#define _DMA_ELEM_CROPPING_B_PARAM	_DMA_V2_ELEM_CROPPING_B_PARAM
+#define _DMA_WIDTH_B_PARAM			_DMA_V2_WIDTH_B_PARAM
+
+#define _DMA_ZERO_EXTEND     _DMA_V2_ZERO_EXTEND
+#define _DMA_SIGN_EXTEND     _DMA_V2_SIGN_EXTEND
+
+typedef unsigned int dma_channel;
+
+typedef enum {
+	dma_isp_to_bus_connection = HIVE_DMA_ISP_BUS_CONN,
+	dma_isp_to_ddr_connection = HIVE_DMA_ISP_DDR_CONN,
+	dma_bus_to_ddr_connection = HIVE_DMA_BUS_DDR_CONN,
+} dma_connection;
+
+typedef enum {
+	dma_zero_extension = _DMA_ZERO_EXTEND,
+	dma_sign_extension = _DMA_SIGN_EXTEND
+} dma_extension;
+
+#define DMA_PROP_SHIFT(val, param)       ((val) << _DMA_V2_ ## param ## _IDX)
+#define DMA_PROP_MASK(param)             ((1U << _DMA_V2_ ## param ## _BITS) - 1)
+#define DMA_PACK(val, param)             DMA_PROP_SHIFT((val) & DMA_PROP_MASK(param), param)
+
+#define DMA_PACK_COMMAND(cmd)            DMA_PACK(cmd, CMD)
+#define DMA_PACK_CHANNEL(ch)             DMA_PACK(ch,  CHANNEL)
+#define DMA_PACK_PARAM(par)              DMA_PACK(par, PARAM)
+#define DMA_PACK_EXTENSION(ext)          DMA_PACK(ext, EXTENSION)
+#define DMA_PACK_LEFT_CROPPING(lc)       DMA_PACK(lc,  LEFT_CROPPING)
+#define DMA_PACK_WIDTH_A(w)              DMA_PACK(w,   SPEC_DEV_A_XB)
+#define DMA_PACK_WIDTH_B(w)              DMA_PACK(w,   SPEC_DEV_B_XB)
+#define DMA_PACK_HEIGHT(h)               DMA_PACK(h,   SPEC_YB)
+
+#define DMA_PACK_CMD_CHANNEL(cmd, ch)	 (DMA_PACK_COMMAND(cmd) | DMA_PACK_CHANNEL(ch))
+#define DMA_PACK_SETUP(conn, ext)        ((conn) | DMA_PACK_EXTENSION(ext))
+#define DMA_PACK_CROP_ELEMS(elems, crop) ((elems) | DMA_PACK_LEFT_CROPPING(crop))
+
+#define hive_dma_snd(dma_id, token) OP_std_snd(dma_id, (unsigned int)(token))
+
+#define DMA_PACK_BLOCK_CMD(cmd, ch, width_a, width_b, height) \
+  (DMA_PACK_COMMAND(cmd)     | \
+   DMA_PACK_CHANNEL(ch)      | \
+   DMA_PACK_WIDTH_A(width_a) | \
+   DMA_PACK_WIDTH_B(width_b) | \
+   DMA_PACK_HEIGHT(height))
+
+#define hive_dma_move_data(dma_id, read, channel, addr_a, addr_b, to_is_var, from_is_var) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK(_DMA_V2_SET_CRUN_COMMAND, CMD)); \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(read ? _DMA_V2_MOVE_B2A_COMMAND : _DMA_V2_MOVE_A2B_COMMAND, channel)); \
+  hive_dma_snd(dma_id, read ? (unsigned int)(addr_b) : (unsigned int)(addr_a)); \
+  hive_dma_snd(dma_id, read ? (unsigned int)(addr_a) : (unsigned int)(addr_b)); \
+  hive_dma_snd(dma_id, to_is_var); \
+  hive_dma_snd(dma_id, from_is_var); \
+}
+
+#define hive_dma_move_data_no_ack(dma_id, read, channel, addr_a, addr_b, to_is_var, from_is_var) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK(_DMA_V2_SET_CRUN_COMMAND, CMD)); \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(read ? _DMA_V2_NO_ACK_MOVE_B2A_NO_SYNC_CHK_COMMAND : _DMA_V2_NO_ACK_MOVE_A2B_NO_SYNC_CHK_COMMAND, channel)); \
+  hive_dma_snd(dma_id, read ? (unsigned int)(addr_b) : (unsigned int)(addr_a)); \
+  hive_dma_snd(dma_id, read ? (unsigned int)(addr_a) : (unsigned int)(addr_b)); \
+  hive_dma_snd(dma_id, to_is_var); \
+  hive_dma_snd(dma_id, from_is_var); \
+}
+
+#define hive_dma_move_b2a_data(dma_id, channel, to_addr, from_addr, to_is_var, from_is_var) \
+{ \
+  hive_dma_move_data(dma_id, true, channel, to_addr, from_addr, to_is_var, from_is_var) \
+}
+
+#define hive_dma_move_a2b_data(dma_id, channel, from_addr, to_addr, from_is_var, to_is_var) \
+{ \
+  hive_dma_move_data(dma_id, false, channel, from_addr, to_addr, from_is_var, to_is_var) \
+}
+
+#define hive_dma_set_data(dma_id, channel, address, value, is_var) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK(_DMA_V2_SET_CRUN_COMMAND, CMD)); \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(_DMA_V2_INIT_A_COMMAND, channel)); \
+  hive_dma_snd(dma_id, value); \
+  hive_dma_snd(dma_id, address); \
+  hive_dma_snd(dma_id, is_var); \
+}
+
+#define hive_dma_clear_data(dma_id, channel, address, is_var) hive_dma_set_data(dma_id, channel, address, 0, is_var)
+
+#define hive_dma_configure(dma_id, channel, connection, extension, height, \
+	stride_A, elems_A, cropping_A, width_A, \
+	stride_B, elems_B, cropping_B, width_B) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(_DMA_V2_CONFIG_CHANNEL_COMMAND, channel)); \
+  hive_dma_snd(dma_id, DMA_PACK_SETUP(connection, extension)); \
+  hive_dma_snd(dma_id, stride_A); \
+  hive_dma_snd(dma_id, DMA_PACK_CROP_ELEMS(elems_A, cropping_A)); \
+  hive_dma_snd(dma_id, width_A); \
+  hive_dma_snd(dma_id, stride_B); \
+  hive_dma_snd(dma_id, DMA_PACK_CROP_ELEMS(elems_B, cropping_B)); \
+  hive_dma_snd(dma_id, width_B); \
+  hive_dma_snd(dma_id, height); \
+}
+
+#define hive_dma_execute(dma_id, channel, cmd, to_addr, from_addr_value, to_is_var, from_is_var) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK(_DMA_V2_SET_CRUN_COMMAND, CMD)); \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(cmd, channel)); \
+  hive_dma_snd(dma_id, to_addr); \
+  hive_dma_snd(dma_id, from_addr_value); \
+  hive_dma_snd(dma_id, to_is_var); \
+  if ((cmd & DMA_CLEAR_CMDBIT) == 0) { \
+	hive_dma_snd(dma_id, from_is_var); \
+  } \
+}
+
+#define hive_dma_configure_fast(dma_id, channel, connection, extension, elems_A, elems_B) \
+{ \
+  hive_dma_snd(dma_id, DMA_PACK_CMD_CHANNEL(_DMA_V2_CONFIG_CHANNEL_COMMAND, channel)); \
+  hive_dma_snd(dma_id, DMA_PACK_SETUP(connection, extension)); \
+  hive_dma_snd(dma_id, 0); \
+  hive_dma_snd(dma_id, DMA_PACK_CROP_ELEMS(elems_A, 0)); \
+  hive_dma_snd(dma_id, 0); \
+  hive_dma_snd(dma_id, 0); \
+  hive_dma_snd(dma_id, DMA_PACK_CROP_ELEMS(elems_B, 0)); \
+  hive_dma_snd(dma_id, 0); \
+  hive_dma_snd(dma_id, 1); \
+}
+
+#define hive_dma_set_parameter(dma_id, channel, param, value) \
+{ \
+  hive_dma_snd(dma_id, _DMA_V2_SET_CHANNEL_PARAM_COMMAND | DMA_PACK_CHANNEL(channel) | DMA_PACK_PARAM(param)); \
+  hive_dma_snd(dma_id, value); \
+}
+
+#define	DMA_SPECIFIC_CMDBIT	0x01
+#define	DMA_CHECK_CMDBIT	0x02
+#define	DMA_RW_CMDBIT		0x04
+#define	DMA_CLEAR_CMDBIT	0x08
+#define	DMA_ACK_CMDBIT		0x10
+#define	DMA_CFG_CMDBIT		0x20
+#define	DMA_PARAM_CMDBIT	0x01
+
+/* Write complete check not necessary if there's no ack */
+#define	DMA_NOACK_CMD		(DMA_ACK_CMDBIT | DMA_CHECK_CMDBIT)
+#define	DMA_CFG_CMD			(DMA_CFG_CMDBIT)
+#define	DMA_CFGPARAM_CMD	(DMA_CFG_CMDBIT | DMA_PARAM_CMDBIT)
+
+#define DMA_CMD_NEEDS_ACK(cmd) ((cmd & DMA_NOACK_CMD) == 0)
+#define DMA_CMD_IS_TRANSFER(cmd) ((cmd & DMA_CFG_CMDBIT) == 0)
+#define DMA_CMD_IS_WR(cmd) ((cmd & DMA_RW_CMDBIT) != 0)
+#define DMA_CMD_IS_RD(cmd) ((cmd & DMA_RW_CMDBIT) == 0)
+#define DMA_CMD_IS_CLR(cmd) ((cmd & DMA_CLEAR_CMDBIT) != 0)
+#define DMA_CMD_IS_CFG(cmd) ((cmd & DMA_CFG_CMDBIT) != 0)
+#define DMA_CMD_IS_PARAMCFG(cmd) ((cmd & DMA_CFGPARAM_CMD) == DMA_CFGPARAM_CMD)
+
+/* As a matter of convention */
+#define DMA_TRANSFER_READ		DMA_TRANSFER_B2A
+#define DMA_TRANSFER_WRITE		DMA_TRANSFER_A2B
+/* store/load from the PoV of the system(memory) */
+#define DMA_TRANSFER_STORE		DMA_TRANSFER_B2A
+#define DMA_TRANSFER_LOAD		DMA_TRANSFER_A2B
+#define DMA_TRANSFER_CLEAR		DMA_TRANSFER_CLEAR_A
+
+typedef enum {
+	DMA_TRANSFER_CLEAR_A = DMA_CLEAR_CMDBIT,                                       /* 8 */
+	DMA_TRANSFER_CLEAR_B = DMA_CLEAR_CMDBIT | DMA_RW_CMDBIT,                       /* 12 */
+	DMA_TRANSFER_A2B = DMA_RW_CMDBIT,                                              /* 4 */
+	DMA_TRANSFER_B2A = 0,                                                          /* 0 */
+	DMA_TRANSFER_CLEAR_A_NOACK = DMA_CLEAR_CMDBIT | DMA_NOACK_CMD,                 /* 26 */
+	DMA_TRANSFER_CLEAR_B_NOACK = DMA_CLEAR_CMDBIT | DMA_RW_CMDBIT | DMA_NOACK_CMD, /* 30 */
+	DMA_TRANSFER_A2B_NOACK = DMA_RW_CMDBIT | DMA_NOACK_CMD,                        /* 22 */
+	DMA_TRANSFER_B2A_NOACK = DMA_NOACK_CMD,                                        /* 18 */
+	DMA_FASTTRANSFER_CLEAR_A = DMA_CLEAR_CMDBIT | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_CLEAR_B = DMA_CLEAR_CMDBIT | DMA_RW_CMDBIT | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_A2B = DMA_RW_CMDBIT | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_B2A = DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_CLEAR_A_NOACK = DMA_CLEAR_CMDBIT | DMA_NOACK_CMD | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_CLEAR_B_NOACK = DMA_CLEAR_CMDBIT | DMA_RW_CMDBIT | DMA_NOACK_CMD | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_A2B_NOACK = DMA_RW_CMDBIT | DMA_NOACK_CMD | DMA_SPECIFIC_CMDBIT,
+	DMA_FASTTRANSFER_B2A_NOACK = DMA_NOACK_CMD | DMA_SPECIFIC_CMDBIT,
+} dma_transfer_type_t;
+
+typedef enum {
+	DMA_CONFIG_SETUP = _DMA_V2_PACKING_SETUP_PARAM,
+	DMA_CONFIG_HEIGHT = _DMA_V2_HEIGHT_PARAM,
+	DMA_CONFIG_STRIDE_A_ = _DMA_V2_STRIDE_A_PARAM,
+	DMA_CONFIG_CROP_ELEM_A = _DMA_V2_ELEM_CROPPING_A_PARAM,
+	DMA_CONFIG_WIDTH_A = _DMA_V2_WIDTH_A_PARAM,
+	DMA_CONFIG_STRIDE_B_ = _DMA_V2_STRIDE_B_PARAM,
+	DMA_CONFIG_CROP_ELEM_B = _DMA_V2_ELEM_CROPPING_B_PARAM,
+	DMA_CONFIG_WIDTH_B = _DMA_V2_WIDTH_B_PARAM,
+} dma_config_type_t;
+
+struct dma_port_config {
+	u8  crop, elems;
+	u16 width;
+	u32 stride;
+};
+
+/* Descriptor for dma configuration */
+struct dma_channel_config {
+	u8  connection;
+	u8  extension;
+	u8  height;
+	struct dma_port_config a, b;
+};
+
+#endif /* __DMA_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/event_fifo_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/event_fifo_global.h
new file mode 100644
index 000000000..a50635b71
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/event_fifo_global.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __EVENT_FIFO_GLOBAL_H
+#define __EVENT_FIFO_GLOBAL_H
+
+/*#error "event_global.h: No global event information permitted"*/
+
+#endif /* __EVENT_FIFO_GLOBAL_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/fifo_monitor_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/fifo_monitor_global.h
new file mode 100644
index 000000000..d941c82d5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/fifo_monitor_global.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __FIFO_MONITOR_GLOBAL_H_INCLUDED__
+#define __FIFO_MONITOR_GLOBAL_H_INCLUDED__
+
+#define IS_FIFO_MONITOR_VERSION_2
+
+/*
+#define HIVE_ISP_CSS_STREAM_SWITCH_NONE      0
+#define HIVE_ISP_CSS_STREAM_SWITCH_SP        1
+#define HIVE_ISP_CSS_STREAM_SWITCH_ISP       2
+ *
+ * Actually, "HIVE_ISP_CSS_STREAM_SWITCH_SP = 1", "HIVE_ISP_CSS_STREAM_SWITCH_ISP = 0"
+ * "hive_isp_css_stream_switch_hrt.h"
+ */
+#define HIVE_ISP_CSS_STREAM_SWITCH_ISP       0
+#define HIVE_ISP_CSS_STREAM_SWITCH_SP        1
+#define HIVE_ISP_CSS_STREAM_SWITCH_NONE      2
+
+#endif /* __FIFO_MONITOR_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/gdc_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gdc_global.h
new file mode 100644
index 000000000..599d993b8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gdc_global.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GDC_GLOBAL_H_INCLUDED__
+#define __GDC_GLOBAL_H_INCLUDED__
+
+#define IS_GDC_VERSION_2
+
+#include <type_support.h>
+#include "gdc_v2_defs.h"
+
+/*
+ * Storage addresses for packed data transfer
+ */
+#define GDC_PARAM_ICX_LEFT_ROUNDED_IDX            0
+#define GDC_PARAM_OXDIM_FLOORED_IDX               1
+#define GDC_PARAM_OXDIM_LAST_IDX                  2
+#define GDC_PARAM_WOIX_LAST_IDX                   3
+#define GDC_PARAM_IY_TOPLEFT_IDX                  4
+#define GDC_PARAM_CHUNK_CNT_IDX                   5
+/*#define GDC_PARAM_ELEMENTS_PER_XMEM_ADDR_IDX    6 */		/* Derived from bpp */
+#define GDC_PARAM_BPP_IDX                         6
+#define GDC_PARAM_BLOCK_HEIGHT_IDX                7
+/*#define GDC_PARAM_DMA_CHANNEL_STRIDE_A_IDX      8*/		/* The DMA stride == the GDC buffer stride */
+#define GDC_PARAM_WOIX_IDX                        8
+#define GDC_PARAM_DMA_CHANNEL_STRIDE_B_IDX        9
+#define GDC_PARAM_DMA_CHANNEL_WIDTH_A_IDX        10
+#define GDC_PARAM_DMA_CHANNEL_WIDTH_B_IDX        11
+#define GDC_PARAM_VECTORS_PER_LINE_IN_IDX        12
+#define GDC_PARAM_VECTORS_PER_LINE_OUT_IDX       13
+#define GDC_PARAM_VMEM_IN_DIMY_IDX               14
+#define GDC_PARAM_COMMAND_IDX                    15
+#define N_GDC_PARAM                              16
+
+/* Because of the packed parameter transfer max(params) == max(fragments) */
+#define	N_GDC_FRAGMENTS		N_GDC_PARAM
+
+/* The GDC is capable of higher internal precision than the parameter data structures */
+#define HRT_GDC_COORD_SCALE_BITS	6
+#define HRT_GDC_COORD_SCALE			BIT(HRT_GDC_COORD_SCALE_BITS)
+
+typedef enum {
+	GDC_CH0_ID = 0,
+	N_GDC_CHANNEL_ID
+} gdc_channel_ID_t;
+
+typedef enum {
+	gdc_8_bpp  = 8,
+	gdc_10_bpp = 10,
+	gdc_12_bpp = 12,
+	gdc_14_bpp = 14
+} gdc_bits_per_pixel_t;
+
+typedef struct gdc_scale_param_mem_s {
+	u16  params[N_GDC_PARAM];
+	u16  ipx_start_array[N_GDC_PARAM];
+	u16  ibuf_offset[N_GDC_PARAM];
+	u16  obuf_offset[N_GDC_PARAM];
+} gdc_scale_param_mem_t;
+
+typedef struct gdc_warp_param_mem_s {
+	u32      origin_x;
+	u32      origin_y;
+	u32      in_addr_offset;
+	u32      in_block_width;
+	u32      in_block_height;
+	u32      p0_x;
+	u32      p0_y;
+	u32      p1_x;
+	u32      p1_y;
+	u32      p2_x;
+	u32      p2_y;
+	u32      p3_x;
+	u32      p3_y;
+	u32      padding[3];
+} gdc_warp_param_mem_t;
+
+#endif /* __GDC_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_device_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_device_global.h
new file mode 100644
index 000000000..c8f416515
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_device_global.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_DEVICE_GLOBAL_H_INCLUDED__
+#define __GP_DEVICE_GLOBAL_H_INCLUDED__
+
+#define IS_GP_DEVICE_VERSION_2
+
+#define _REG_GP_IRQ_REQ0_ADDR				0x08
+#define _REG_GP_IRQ_REQ1_ADDR				0x0C
+/* The SP sends SW interrupt info to this register */
+#define _REG_GP_IRQ_REQUEST0_ADDR			_REG_GP_IRQ_REQ0_ADDR
+#define _REG_GP_IRQ_REQUEST1_ADDR			_REG_GP_IRQ_REQ1_ADDR
+
+/* The SP configures FIFO switches in these registers */
+#define _REG_GP_SWITCH_IF_ADDR						0x40
+#define _REG_GP_SWITCH_GDC1_ADDR					0x44
+#define _REG_GP_SWITCH_GDC2_ADDR					0x48
+/* @ INPUT_FORMATTER_BASE -> GP_DEVICE_BASE */
+#define _REG_GP_IFMT_input_switch_lut_reg0			0x00030800
+#define _REG_GP_IFMT_input_switch_lut_reg1			0x00030804
+#define _REG_GP_IFMT_input_switch_lut_reg2			0x00030808
+#define _REG_GP_IFMT_input_switch_lut_reg3			0x0003080C
+#define _REG_GP_IFMT_input_switch_lut_reg4			0x00030810
+#define _REG_GP_IFMT_input_switch_lut_reg5			0x00030814
+#define _REG_GP_IFMT_input_switch_lut_reg6			0x00030818
+#define _REG_GP_IFMT_input_switch_lut_reg7			0x0003081C
+#define _REG_GP_IFMT_input_switch_fsync_lut			0x00030820
+#define _REG_GP_IFMT_srst							0x00030824
+#define _REG_GP_IFMT_slv_reg_srst					0x00030828
+#define _REG_GP_IFMT_input_switch_ch_id_fmt_type	0x0003082C
+
+/* @ GP_DEVICE_BASE */
+#define _REG_GP_SYNCGEN_ENABLE_ADDR					0x00090000
+#define _REG_GP_SYNCGEN_FREE_RUNNING_ADDR			0x00090004
+#define _REG_GP_SYNCGEN_PAUSE_ADDR					0x00090008
+#define _REG_GP_NR_FRAMES_ADDR						0x0009000C
+#define _REG_GP_SYNGEN_NR_PIX_ADDR					0x00090010
+#define _REG_GP_SYNGEN_NR_LINES_ADDR				0x00090014
+#define _REG_GP_SYNGEN_HBLANK_CYCLES_ADDR			0x00090018
+#define _REG_GP_SYNGEN_VBLANK_CYCLES_ADDR			0x0009001C
+#define _REG_GP_ISEL_SOF_ADDR						0x00090020
+#define _REG_GP_ISEL_EOF_ADDR						0x00090024
+#define _REG_GP_ISEL_SOL_ADDR						0x00090028
+#define _REG_GP_ISEL_EOL_ADDR						0x0009002C
+#define _REG_GP_ISEL_LFSR_ENABLE_ADDR				0x00090030
+#define _REG_GP_ISEL_LFSR_ENABLE_B_ADDR				0x00090034
+#define _REG_GP_ISEL_LFSR_RESET_VALUE_ADDR			0x00090038
+#define _REG_GP_ISEL_TPG_ENABLE_ADDR				0x0009003C
+#define _REG_GP_ISEL_TPG_ENABLE_B_ADDR				0x00090040
+#define _REG_GP_ISEL_HOR_CNT_MASK_ADDR				0x00090044
+#define _REG_GP_ISEL_VER_CNT_MASK_ADDR				0x00090048
+#define _REG_GP_ISEL_XY_CNT_MASK_ADDR				0x0009004C
+#define _REG_GP_ISEL_HOR_CNT_DELTA_ADDR				0x00090050
+#define _REG_GP_ISEL_VER_CNT_DELTA_ADDR				0x00090054
+#define _REG_GP_ISEL_TPG_MODE_ADDR					0x00090058
+#define _REG_GP_ISEL_TPG_RED1_ADDR					0x0009005C
+#define _REG_GP_ISEL_TPG_GREEN1_ADDR				0x00090060
+#define _REG_GP_ISEL_TPG_BLUE1_ADDR					0x00090064
+#define _REG_GP_ISEL_TPG_RED2_ADDR					0x00090068
+#define _REG_GP_ISEL_TPG_GREEN2_ADDR				0x0009006C
+#define _REG_GP_ISEL_TPG_BLUE2_ADDR					0x00090070
+#define _REG_GP_ISEL_CH_ID_ADDR						0x00090074
+#define _REG_GP_ISEL_FMT_TYPE_ADDR					0x00090078
+#define _REG_GP_ISEL_DATA_SEL_ADDR					0x0009007C
+#define _REG_GP_ISEL_SBAND_SEL_ADDR					0x00090080
+#define _REG_GP_ISEL_SYNC_SEL_ADDR					0x00090084
+#define _REG_GP_SYNCGEN_HOR_CNT_ADDR				0x00090088
+#define _REG_GP_SYNCGEN_VER_CNT_ADDR				0x0009008C
+#define _REG_GP_SYNCGEN_FRAME_CNT_ADDR				0x00090090
+#define _REG_GP_SOFT_RESET_ADDR						0x00090094
+
+#endif /* __GP_DEVICE_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_timer_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_timer_global.h
new file mode 100644
index 000000000..163003f2c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gp_timer_global.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_TIMER_GLOBAL_H_INCLUDED__
+#define __GP_TIMER_GLOBAL_H_INCLUDED__
+
+#include "hive_isp_css_defs.h" /*HIVE_GP_TIMER_SP_DMEM_ERROR_IRQ */
+
+/* from gp_timer_defs.h*/
+#define GP_TIMER_COUNT_TYPE_HIGH             0
+#define GP_TIMER_COUNT_TYPE_LOW              1
+#define GP_TIMER_COUNT_TYPE_POSEDGE          2
+#define GP_TIMER_COUNT_TYPE_NEGEDGE          3
+#define GP_TIMER_COUNT_TYPE_TYPES            4
+
+/* timer - 3 is selected */
+#define GP_TIMER_SEL                         3
+
+/*HIVE_GP_TIMER_SP_DMEM_ERROR_IRQ is selected*/
+#define GP_TIMER_SIGNAL_SELECT  HIVE_GP_TIMER_SP_DMEM_ERROR_IRQ
+
+#endif /* __GP_TIMER_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/gpio_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gpio_global.h
new file mode 100644
index 000000000..b5f017482
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/gpio_global.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GPIO_GLOBAL_H_INCLUDED__
+#define __GPIO_GLOBAL_H_INCLUDED__
+
+#define IS_GPIO_VERSION_1
+
+#include <gpio_block_defs.h>
+
+/* pqiao: following part only defines in hive_isp_css_defs.h in fpga system.
+	port it here
+*/
+
+/* GPIO pin defines */
+/*#define HIVE_GPIO_CAMERA_BOARD_RESET_PIN_NR                   0
+#define HIVE_GPIO_LCD_CLOCK_SELECT_PIN_NR                     7
+#define HIVE_GPIO_HDMI_CLOCK_SELECT_PIN_NR                    8
+#define HIVE_GPIO_LCD_VERT_FLIP_PIN_NR                        8
+#define HIVE_GPIO_LCD_HOR_FLIP_PIN_NR                         9
+#define HIVE_GPIO_AS3683_GPIO_P0_PIN_NR                       1
+#define HIVE_GPIO_AS3683_DATA_P1_PIN_NR                       2
+#define HIVE_GPIO_AS3683_CLK_P2_PIN_NR                        3
+#define HIVE_GPIO_AS3683_T1_F0_PIN_NR                         4
+#define HIVE_GPIO_AS3683_SFL_F1_PIN_NR                        5
+#define HIVE_GPIO_AS3683_STROBE_F2_PIN_NR                     6
+#define HIVE_GPIO_MAX1577_EN1_PIN_NR                          1
+#define HIVE_GPIO_MAX1577_EN2_PIN_NR                          2
+#define HIVE_GPIO_MAX8685A_EN_PIN_NR                          3
+#define HIVE_GPIO_MAX8685A_TRIG_PIN_NR                        4*/
+
+#define HIVE_GPIO_STROBE_TRIGGER_PIN		2
+
+#endif /* __GPIO_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/hmem_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/hmem_global.h
new file mode 100644
index 000000000..746b07097
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/hmem_global.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __HMEM_GLOBAL_H_INCLUDED__
+#define __HMEM_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#define IS_HMEM_VERSION_1
+
+#include "isp.h"
+
+/*
+#define ISP_HIST_ADDRESS_BITS                  12
+#define ISP_HIST_ALIGNMENT                     4
+#define ISP_HIST_COMP_IN_PREC                  12
+#define ISP_HIST_DEPTH                         1024
+#define ISP_HIST_WIDTH                         24
+#define ISP_HIST_COMPONENTS                    4
+*/
+#define ISP_HIST_ALIGNMENT_LOG2		2
+
+#define HMEM_SIZE_LOG2		(ISP_HIST_ADDRESS_BITS - ISP_HIST_ALIGNMENT_LOG2)
+#define HMEM_SIZE			ISP_HIST_DEPTH
+
+#define HMEM_UNIT_SIZE		(HMEM_SIZE / ISP_HIST_COMPONENTS)
+#define HMEM_UNIT_COUNT		ISP_HIST_COMPONENTS
+
+#define HMEM_RANGE_LOG2		ISP_HIST_WIDTH
+#define HMEM_RANGE			BIT(HMEM_RANGE_LOG2)
+
+typedef u32			hmem_data_t;
+
+#endif /* __HMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug.c
new file mode 100644
index 000000000..a502ba9f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug.c
@@ -0,0 +1,72 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2016, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "debug.h"
+
+#include "hmm.h"
+
+#ifndef __INLINE_DEBUG__
+#include "debug_private.h"
+#endif /* __INLINE_DEBUG__ */
+
+#define __INLINE_SP__
+#include "sp.h"
+
+#include "assert_support.h"
+
+/* The address of the remote copy */
+hrt_address	debug_buffer_address = (hrt_address) - 1;
+ia_css_ptr	debug_buffer_ddr_address = (ia_css_ptr)-1;
+/* The local copy */
+static debug_data_t		debug_data;
+debug_data_t		*debug_data_ptr = &debug_data;
+
+void debug_buffer_init(const hrt_address addr)
+{
+	debug_buffer_address = addr;
+
+	debug_data.head = 0;
+	debug_data.tail = 0;
+}
+
+void debug_buffer_ddr_init(const ia_css_ptr addr)
+{
+	debug_buf_mode_t mode = DEBUG_BUFFER_MODE_LINEAR;
+	u32 enable = 1;
+	u32 head = 0;
+	u32 tail = 0;
+	/* set the ddr queue */
+	debug_buffer_ddr_address = addr;
+	hmm_store(addr + DEBUG_DATA_BUF_MODE_DDR_ADDR,
+		   &mode, sizeof(debug_buf_mode_t));
+	hmm_store(addr + DEBUG_DATA_HEAD_DDR_ADDR,
+		   &head, sizeof(uint32_t));
+	hmm_store(addr + DEBUG_DATA_TAIL_DDR_ADDR,
+		   &tail, sizeof(uint32_t));
+	hmm_store(addr + DEBUG_DATA_ENABLE_DDR_ADDR,
+		   &enable, sizeof(uint32_t));
+
+	/* set the local copy */
+	debug_data.head = 0;
+	debug_data.tail = 0;
+}
+
+void debug_buffer_setmode(const debug_buf_mode_t mode)
+{
+	assert(debug_buffer_address != ((hrt_address)-1));
+
+	sp_dmem_store_uint32(SP0_ID,
+			     debug_buffer_address + DEBUG_DATA_BUF_MODE_ADDR, mode);
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_local.h
new file mode 100644
index 000000000..536a4dcf0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DEBUG_LOCAL_H_INCLUDED__
+#define __DEBUG_LOCAL_H_INCLUDED__
+
+#include "debug_global.h"
+
+#endif /* __DEBUG_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_private.h
new file mode 100644
index 000000000..3fea43a21
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/debug_private.h
@@ -0,0 +1,125 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DEBUG_PRIVATE_H_INCLUDED__
+#define __DEBUG_PRIVATE_H_INCLUDED__
+
+#include "debug_public.h"
+
+#include "sp.h"
+
+#define __INLINE_ISP__
+#include "isp.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_DEBUG_C bool is_debug_buffer_empty(void)
+{
+	return (debug_data_ptr->head == debug_data_ptr->tail);
+}
+
+STORAGE_CLASS_DEBUG_C hrt_data debug_dequeue(void)
+{
+	hrt_data value = 0;
+
+	assert(debug_buffer_address != ((hrt_address) - 1));
+
+	debug_synch_queue();
+
+	if (!is_debug_buffer_empty()) {
+		value = debug_data_ptr->buf[debug_data_ptr->head];
+		debug_data_ptr->head = (debug_data_ptr->head + 1) & DEBUG_BUF_MASK;
+		sp_dmem_store_uint32(SP0_ID, debug_buffer_address + DEBUG_DATA_HEAD_ADDR,
+				     debug_data_ptr->head);
+	}
+
+	return value;
+}
+
+STORAGE_CLASS_DEBUG_C void debug_synch_queue(void)
+{
+	u32 remote_tail = sp_dmem_load_uint32(SP0_ID,
+					      debug_buffer_address + DEBUG_DATA_TAIL_ADDR);
+	/* We could move the remote head after the upload, but we would have to limit the upload w.r.t. the local head. This is easier */
+	if (remote_tail > debug_data_ptr->tail) {
+		size_t	delta = remote_tail - debug_data_ptr->tail;
+
+		sp_dmem_load(SP0_ID, debug_buffer_address + DEBUG_DATA_BUF_ADDR +
+			     debug_data_ptr->tail * sizeof(uint32_t),
+			     (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+	} else if (remote_tail < debug_data_ptr->tail) {
+		size_t	delta = DEBUG_BUF_SIZE - debug_data_ptr->tail;
+
+		sp_dmem_load(SP0_ID, debug_buffer_address + DEBUG_DATA_BUF_ADDR +
+			     debug_data_ptr->tail * sizeof(uint32_t),
+			     (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+		sp_dmem_load(SP0_ID, debug_buffer_address + DEBUG_DATA_BUF_ADDR,
+			     (void *)&debug_data_ptr->buf[0],
+			     remote_tail * sizeof(uint32_t));
+	} /* else we are up to date */
+	debug_data_ptr->tail = remote_tail;
+}
+
+STORAGE_CLASS_DEBUG_C void debug_synch_queue_isp(void)
+{
+	u32 remote_tail = isp_dmem_load_uint32(ISP0_ID,
+					       DEBUG_BUFFER_ISP_DMEM_ADDR + DEBUG_DATA_TAIL_ADDR);
+	/* We could move the remote head after the upload, but we would have to limit the upload w.r.t. the local head. This is easier */
+	if (remote_tail > debug_data_ptr->tail) {
+		size_t	delta = remote_tail - debug_data_ptr->tail;
+
+		isp_dmem_load(ISP0_ID, DEBUG_BUFFER_ISP_DMEM_ADDR + DEBUG_DATA_BUF_ADDR +
+			      debug_data_ptr->tail * sizeof(uint32_t),
+			      (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+	} else if (remote_tail < debug_data_ptr->tail) {
+		size_t	delta = DEBUG_BUF_SIZE - debug_data_ptr->tail;
+
+		isp_dmem_load(ISP0_ID, DEBUG_BUFFER_ISP_DMEM_ADDR + DEBUG_DATA_BUF_ADDR +
+			      debug_data_ptr->tail * sizeof(uint32_t),
+			      (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+		isp_dmem_load(ISP0_ID, DEBUG_BUFFER_ISP_DMEM_ADDR + DEBUG_DATA_BUF_ADDR,
+			      (void *)&debug_data_ptr->buf[0],
+			      remote_tail * sizeof(uint32_t));
+	} /* else we are up to date */
+	debug_data_ptr->tail = remote_tail;
+}
+
+STORAGE_CLASS_DEBUG_C void debug_synch_queue_ddr(void)
+{
+	u32	remote_tail;
+
+	hmm_load(debug_buffer_ddr_address + DEBUG_DATA_TAIL_DDR_ADDR, &remote_tail,
+		  sizeof(uint32_t));
+	/* We could move the remote head after the upload, but we would have to limit the upload w.r.t. the local head. This is easier */
+	if (remote_tail > debug_data_ptr->tail) {
+		size_t	delta = remote_tail - debug_data_ptr->tail;
+
+		hmm_load(debug_buffer_ddr_address + DEBUG_DATA_BUF_DDR_ADDR +
+			  debug_data_ptr->tail * sizeof(uint32_t),
+			  (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+	} else if (remote_tail < debug_data_ptr->tail) {
+		size_t	delta = DEBUG_BUF_SIZE - debug_data_ptr->tail;
+
+		hmm_load(debug_buffer_ddr_address + DEBUG_DATA_BUF_DDR_ADDR +
+			  debug_data_ptr->tail * sizeof(uint32_t),
+			  (void *)&debug_data_ptr->buf[debug_data_ptr->tail], delta * sizeof(uint32_t));
+		hmm_load(debug_buffer_ddr_address + DEBUG_DATA_BUF_DDR_ADDR,
+			  (void *)&debug_data_ptr->buf[0],
+			  remote_tail * sizeof(uint32_t));
+	} /* else we are up to date */
+	debug_data_ptr->tail = remote_tail;
+}
+
+#endif /* __DEBUG_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma.c
new file mode 100644
index 000000000..f85950c47
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma.c
@@ -0,0 +1,300 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2016, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/kernel.h>
+
+#include "dma.h"
+
+#include "assert_support.h"
+
+#ifndef __INLINE_DMA__
+#include "dma_private.h"
+#endif /* __INLINE_DMA__ */
+
+void dma_get_state(const dma_ID_t ID, dma_state_t *state)
+{
+	int			i;
+	hrt_data	tmp;
+
+	assert(ID < N_DMA_ID);
+	assert(state);
+
+	tmp = dma_reg_load(ID, DMA_COMMAND_FSM_REG_IDX);
+	//reg  [3:0] : flags error [3], stall, run, idle [0]
+	//reg  [9:4] : command
+	//reg[14:10] : channel
+	//reg [23:15] : param
+	state->fsm_command_idle = tmp & 0x1;
+	state->fsm_command_run = tmp & 0x2;
+	state->fsm_command_stalling = tmp & 0x4;
+	state->fsm_command_error    = tmp & 0x8;
+	state->last_command_channel = (tmp >> 10 & 0x1F);
+	state->last_command_param =  (tmp >> 15 & 0x0F);
+	tmp = (tmp >> 4) & 0x3F;
+	/* state->last_command = (dma_commands_t)tmp; */
+	/* if the enumerator is made non-linear */
+	/* AM: the list below does not cover all the cases*/
+	/*  and these are not correct */
+	/* therefore for just dumpinmg this command*/
+	state->last_command = tmp;
+
+	/*
+		if (tmp == 0)
+			state->last_command = DMA_COMMAND_READ;
+		if (tmp == 1)
+			state->last_command = DMA_COMMAND_WRITE;
+		if (tmp == 2)
+			state->last_command = DMA_COMMAND_SET_CHANNEL;
+		if (tmp == 3)
+			state->last_command = DMA_COMMAND_SET_PARAM;
+		if (tmp == 4)
+			state->last_command = DMA_COMMAND_READ_SPECIFIC;
+		if (tmp == 5)
+			state->last_command = DMA_COMMAND_WRITE_SPECIFIC;
+		if (tmp == 8)
+			state->last_command = DMA_COMMAND_INIT;
+		if (tmp == 12)
+			state->last_command = DMA_COMMAND_INIT_SPECIFIC;
+		if (tmp == 15)
+			state->last_command = DMA_COMMAND_RST;
+	*/
+
+	/* No sub-fields, idx = 0 */
+	state->current_command = dma_reg_load(ID,
+					      DMA_CG_INFO_REG_IDX(0, _DMA_FSM_GROUP_CMD_IDX));
+	state->current_addr_a = dma_reg_load(ID,
+					     DMA_CG_INFO_REG_IDX(0, _DMA_FSM_GROUP_ADDR_A_IDX));
+	state->current_addr_b = dma_reg_load(ID,
+					     DMA_CG_INFO_REG_IDX(0, _DMA_FSM_GROUP_ADDR_B_IDX));
+
+	tmp =  dma_reg_load(ID,
+			    DMA_CG_INFO_REG_IDX(
+				_DMA_FSM_GROUP_FSM_CTRL_STATE_IDX,
+				_DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_idle = tmp & 0x1;
+	state->fsm_ctrl_run = tmp & 0x2;
+	state->fsm_ctrl_stalling = tmp & 0x4;
+	state->fsm_ctrl_error = tmp & 0x8;
+	tmp = tmp >> 4;
+	/* state->fsm_ctrl_state = (dma_ctrl_states_t)tmp; */
+	if (tmp == 0)
+		state->fsm_ctrl_state = DMA_CTRL_STATE_IDLE;
+	if (tmp == 1)
+		state->fsm_ctrl_state = DMA_CTRL_STATE_REQ_RCV;
+	if (tmp == 2)
+		state->fsm_ctrl_state = DMA_CTRL_STATE_RCV;
+	if (tmp == 3)
+		state->fsm_ctrl_state = DMA_CTRL_STATE_RCV_REQ;
+	if (tmp == 4)
+		state->fsm_ctrl_state = DMA_CTRL_STATE_INIT;
+	state->fsm_ctrl_source_dev = dma_reg_load(ID,
+				     DMA_CG_INFO_REG_IDX(
+					 _DMA_FSM_GROUP_FSM_CTRL_REQ_DEV_IDX,
+					 _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_source_addr = dma_reg_load(ID,
+				      DMA_CG_INFO_REG_IDX(
+					  _DMA_FSM_GROUP_FSM_CTRL_REQ_ADDR_IDX,
+					  _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_source_stride = dma_reg_load(ID,
+					DMA_CG_INFO_REG_IDX(
+					    _DMA_FSM_GROUP_FSM_CTRL_REQ_STRIDE_IDX,
+					    _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_source_width = dma_reg_load(ID,
+				       DMA_CG_INFO_REG_IDX(
+					   _DMA_FSM_GROUP_FSM_CTRL_REQ_XB_IDX,
+					   _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_source_height = dma_reg_load(ID,
+					DMA_CG_INFO_REG_IDX(
+					    _DMA_FSM_GROUP_FSM_CTRL_REQ_YB_IDX,
+					    _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_source_dev = dma_reg_load(ID,
+					  DMA_CG_INFO_REG_IDX(
+					      _DMA_FSM_GROUP_FSM_CTRL_PACK_REQ_DEV_IDX,
+					      _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_dest_dev = dma_reg_load(ID,
+					DMA_CG_INFO_REG_IDX(
+					    _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_DEV_IDX,
+					    _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_dest_addr = dma_reg_load(ID,
+				    DMA_CG_INFO_REG_IDX(
+					_DMA_FSM_GROUP_FSM_CTRL_WR_ADDR_IDX,
+					_DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_dest_stride = dma_reg_load(ID,
+				      DMA_CG_INFO_REG_IDX(
+					  _DMA_FSM_GROUP_FSM_CTRL_WR_STRIDE_IDX,
+					  _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_source_width = dma_reg_load(ID,
+					    DMA_CG_INFO_REG_IDX(
+						_DMA_FSM_GROUP_FSM_CTRL_PACK_REQ_XB_IDX,
+						_DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_dest_height = dma_reg_load(ID,
+					   DMA_CG_INFO_REG_IDX(
+					       _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_YB_IDX,
+					       _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_dest_width = dma_reg_load(ID,
+					  DMA_CG_INFO_REG_IDX(
+					      _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_XB_IDX,
+					      _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_source_elems = dma_reg_load(ID,
+					    DMA_CG_INFO_REG_IDX(
+						_DMA_FSM_GROUP_FSM_CTRL_PACK_ELEM_REQ_IDX,
+						_DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_dest_elems = dma_reg_load(ID,
+					  DMA_CG_INFO_REG_IDX(
+					      _DMA_FSM_GROUP_FSM_CTRL_PACK_ELEM_WR_IDX,
+					      _DMA_FSM_GROUP_FSM_CTRL_IDX));
+	state->fsm_ctrl_pack_extension = dma_reg_load(ID,
+					 DMA_CG_INFO_REG_IDX(
+					     _DMA_FSM_GROUP_FSM_CTRL_PACK_S_Z_IDX,
+					     _DMA_FSM_GROUP_FSM_CTRL_IDX));
+
+	tmp = dma_reg_load(ID,
+			   DMA_CG_INFO_REG_IDX(
+			       _DMA_FSM_GROUP_FSM_PACK_STATE_IDX,
+			       _DMA_FSM_GROUP_FSM_PACK_IDX));
+	state->pack_idle     = tmp & 0x1;
+	state->pack_run      = tmp & 0x2;
+	state->pack_stalling = tmp & 0x4;
+	state->pack_error    = tmp & 0x8;
+	state->pack_cnt_height = dma_reg_load(ID,
+					      DMA_CG_INFO_REG_IDX(
+						      _DMA_FSM_GROUP_FSM_PACK_CNT_YB_IDX,
+						      _DMA_FSM_GROUP_FSM_PACK_IDX));
+	state->pack_src_cnt_width = dma_reg_load(ID,
+				    DMA_CG_INFO_REG_IDX(
+					_DMA_FSM_GROUP_FSM_PACK_CNT_XB_REQ_IDX,
+					_DMA_FSM_GROUP_FSM_PACK_IDX));
+	state->pack_dest_cnt_width = dma_reg_load(ID,
+				     DMA_CG_INFO_REG_IDX(
+					 _DMA_FSM_GROUP_FSM_PACK_CNT_XB_WR_IDX,
+					 _DMA_FSM_GROUP_FSM_PACK_IDX));
+
+	tmp = dma_reg_load(ID,
+			   DMA_CG_INFO_REG_IDX(
+			       _DMA_FSM_GROUP_FSM_REQ_STATE_IDX,
+			       _DMA_FSM_GROUP_FSM_REQ_IDX));
+	/* state->read_state = (dma_rw_states_t)tmp; */
+	if (tmp == 0)
+		state->read_state = DMA_RW_STATE_IDLE;
+	if (tmp == 1)
+		state->read_state = DMA_RW_STATE_REQ;
+	if (tmp == 2)
+		state->read_state = DMA_RW_STATE_NEXT_LINE;
+	if (tmp == 3)
+		state->read_state = DMA_RW_STATE_UNLOCK_CHANNEL;
+	state->read_cnt_height = dma_reg_load(ID,
+					      DMA_CG_INFO_REG_IDX(
+						      _DMA_FSM_GROUP_FSM_REQ_CNT_YB_IDX,
+						      _DMA_FSM_GROUP_FSM_REQ_IDX));
+	state->read_cnt_width = dma_reg_load(ID,
+					     DMA_CG_INFO_REG_IDX(
+						     _DMA_FSM_GROUP_FSM_REQ_CNT_XB_IDX,
+						     _DMA_FSM_GROUP_FSM_REQ_IDX));
+
+	tmp = dma_reg_load(ID,
+			   DMA_CG_INFO_REG_IDX(
+			       _DMA_FSM_GROUP_FSM_WR_STATE_IDX,
+			       _DMA_FSM_GROUP_FSM_WR_IDX));
+	/* state->write_state = (dma_rw_states_t)tmp; */
+	if (tmp == 0)
+		state->write_state = DMA_RW_STATE_IDLE;
+	if (tmp == 1)
+		state->write_state = DMA_RW_STATE_REQ;
+	if (tmp == 2)
+		state->write_state = DMA_RW_STATE_NEXT_LINE;
+	if (tmp == 3)
+		state->write_state = DMA_RW_STATE_UNLOCK_CHANNEL;
+	state->write_height = dma_reg_load(ID,
+					   DMA_CG_INFO_REG_IDX(
+					       _DMA_FSM_GROUP_FSM_WR_CNT_YB_IDX,
+					       _DMA_FSM_GROUP_FSM_WR_IDX));
+	state->write_width = dma_reg_load(ID,
+					  DMA_CG_INFO_REG_IDX(
+					      _DMA_FSM_GROUP_FSM_WR_CNT_XB_IDX,
+					      _DMA_FSM_GROUP_FSM_WR_IDX));
+
+	for (i = 0; i < HIVE_ISP_NUM_DMA_CONNS; i++) {
+		dma_port_state_t *port = &state->port_states[i];
+
+		tmp = dma_reg_load(ID, DMA_DEV_INFO_REG_IDX(0, i));
+		port->req_cs   = ((tmp & 0x1) != 0);
+		port->req_we_n = ((tmp & 0x2) != 0);
+		port->req_run  = ((tmp & 0x4) != 0);
+		port->req_ack  = ((tmp & 0x8) != 0);
+
+		tmp = dma_reg_load(ID, DMA_DEV_INFO_REG_IDX(1, i));
+		port->send_cs   = ((tmp & 0x1) != 0);
+		port->send_we_n = ((tmp & 0x2) != 0);
+		port->send_run  = ((tmp & 0x4) != 0);
+		port->send_ack  = ((tmp & 0x8) != 0);
+
+		tmp = dma_reg_load(ID, DMA_DEV_INFO_REG_IDX(2, i));
+		if (tmp & 0x1)
+			port->fifo_state = DMA_FIFO_STATE_WILL_BE_FULL;
+		if (tmp & 0x2)
+			port->fifo_state = DMA_FIFO_STATE_FULL;
+		if (tmp & 0x4)
+			port->fifo_state = DMA_FIFO_STATE_EMPTY;
+		port->fifo_counter = tmp >> 3;
+	}
+
+	for (i = 0; i < HIVE_DMA_NUM_CHANNELS; i++) {
+		dma_channel_state_t *ch = &state->channel_states[i];
+
+		ch->connection = DMA_GET_CONNECTION(dma_reg_load(ID,
+						    DMA_CHANNEL_PARAM_REG_IDX(i,
+							    _DMA_PACKING_SETUP_PARAM)));
+		ch->sign_extend = DMA_GET_EXTENSION(dma_reg_load(ID,
+						    DMA_CHANNEL_PARAM_REG_IDX(i,
+							    _DMA_PACKING_SETUP_PARAM)));
+		ch->height = dma_reg_load(ID,
+					  DMA_CHANNEL_PARAM_REG_IDX(i,
+						  _DMA_HEIGHT_PARAM));
+		ch->stride_a = dma_reg_load(ID,
+					    DMA_CHANNEL_PARAM_REG_IDX(i,
+						    _DMA_STRIDE_A_PARAM));
+		ch->elems_a = DMA_GET_ELEMENTS(dma_reg_load(ID,
+					       DMA_CHANNEL_PARAM_REG_IDX(i,
+						       _DMA_ELEM_CROPPING_A_PARAM)));
+		ch->cropping_a = DMA_GET_CROPPING(dma_reg_load(ID,
+						  DMA_CHANNEL_PARAM_REG_IDX(i,
+							  _DMA_ELEM_CROPPING_A_PARAM)));
+		ch->width_a = dma_reg_load(ID,
+					   DMA_CHANNEL_PARAM_REG_IDX(i,
+						   _DMA_WIDTH_A_PARAM));
+		ch->stride_b = dma_reg_load(ID,
+					    DMA_CHANNEL_PARAM_REG_IDX(i,
+						    _DMA_STRIDE_B_PARAM));
+		ch->elems_b = DMA_GET_ELEMENTS(dma_reg_load(ID,
+					       DMA_CHANNEL_PARAM_REG_IDX(i,
+						       _DMA_ELEM_CROPPING_B_PARAM)));
+		ch->cropping_b = DMA_GET_CROPPING(dma_reg_load(ID,
+						  DMA_CHANNEL_PARAM_REG_IDX(i,
+							  _DMA_ELEM_CROPPING_B_PARAM)));
+		ch->width_b = dma_reg_load(ID,
+					   DMA_CHANNEL_PARAM_REG_IDX(i,
+						   _DMA_WIDTH_B_PARAM));
+	}
+}
+
+void
+dma_set_max_burst_size(const dma_ID_t ID, dma_connection conn,
+		       uint32_t max_burst_size)
+{
+	assert(ID < N_DMA_ID);
+	assert(max_burst_size > 0);
+	dma_reg_store(ID, DMA_DEV_INFO_REG_IDX(_DMA_DEV_INTERF_MAX_BURST_IDX, conn),
+		      max_burst_size - 1);
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_local.h
new file mode 100644
index 000000000..7e4cc7573
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_local.h
@@ -0,0 +1,208 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DMA_LOCAL_H_INCLUDED__
+#define __DMA_LOCAL_H_INCLUDED__
+
+#include <type_support.h>
+#include "dma_global.h"
+
+#include <defs.h>				/* HRTCAT() */
+#include <bits.h>				/* _hrt_get_bits() */
+#include <hive_isp_css_defs.h>		/* HIVE_DMA_NUM_CHANNELS */
+#include <dma_v2_defs.h>
+
+#define _DMA_FSM_GROUP_CMD_IDX						_DMA_V2_FSM_GROUP_CMD_IDX
+#define _DMA_FSM_GROUP_ADDR_A_IDX					_DMA_V2_FSM_GROUP_ADDR_SRC_IDX
+#define _DMA_FSM_GROUP_ADDR_B_IDX					_DMA_V2_FSM_GROUP_ADDR_DEST_IDX
+
+#define _DMA_FSM_GROUP_CMD_CTRL_IDX					_DMA_V2_FSM_GROUP_CMD_CTRL_IDX
+
+#define _DMA_FSM_GROUP_FSM_CTRL_IDX					_DMA_V2_FSM_GROUP_FSM_CTRL_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_STATE_IDX			_DMA_V2_FSM_GROUP_FSM_CTRL_STATE_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_REQ_DEV_IDX			_DMA_V2_FSM_GROUP_FSM_CTRL_REQ_DEV_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_REQ_ADDR_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_REQ_ADDR_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_REQ_STRIDE_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_REQ_STRIDE_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_REQ_XB_IDX			_DMA_V2_FSM_GROUP_FSM_CTRL_REQ_XB_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_REQ_YB_IDX			_DMA_V2_FSM_GROUP_FSM_CTRL_REQ_YB_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_REQ_DEV_IDX	_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_REQ_DEV_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_DEV_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_DEV_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_WR_ADDR_IDX			_DMA_V2_FSM_GROUP_FSM_CTRL_WR_ADDR_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_WR_STRIDE_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_WR_STRIDE_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_REQ_XB_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_REQ_XB_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_YB_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_YB_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_WR_XB_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_WR_XB_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_ELEM_REQ_IDX	_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_ELEM_REQ_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_ELEM_WR_IDX	_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_ELEM_WR_IDX
+#define _DMA_FSM_GROUP_FSM_CTRL_PACK_S_Z_IDX		_DMA_V2_FSM_GROUP_FSM_CTRL_PACK_S_Z_IDX
+
+#define _DMA_FSM_GROUP_FSM_PACK_IDX					_DMA_V2_FSM_GROUP_FSM_PACK_IDX
+#define _DMA_FSM_GROUP_FSM_PACK_STATE_IDX			_DMA_V2_FSM_GROUP_FSM_PACK_STATE_IDX
+#define _DMA_FSM_GROUP_FSM_PACK_CNT_YB_IDX			_DMA_V2_FSM_GROUP_FSM_PACK_CNT_YB_IDX
+#define _DMA_FSM_GROUP_FSM_PACK_CNT_XB_REQ_IDX		_DMA_V2_FSM_GROUP_FSM_PACK_CNT_XB_REQ_IDX
+#define _DMA_FSM_GROUP_FSM_PACK_CNT_XB_WR_IDX		_DMA_V2_FSM_GROUP_FSM_PACK_CNT_XB_WR_IDX
+
+#define _DMA_FSM_GROUP_FSM_REQ_IDX					_DMA_V2_FSM_GROUP_FSM_REQ_IDX
+#define _DMA_FSM_GROUP_FSM_REQ_STATE_IDX			_DMA_V2_FSM_GROUP_FSM_REQ_STATE_IDX
+#define _DMA_FSM_GROUP_FSM_REQ_CNT_YB_IDX			_DMA_V2_FSM_GROUP_FSM_REQ_CNT_YB_IDX
+#define _DMA_FSM_GROUP_FSM_REQ_CNT_XB_IDX			_DMA_V2_FSM_GROUP_FSM_REQ_CNT_XB_IDX
+
+#define _DMA_FSM_GROUP_FSM_WR_IDX					_DMA_V2_FSM_GROUP_FSM_WR_IDX
+#define _DMA_FSM_GROUP_FSM_WR_STATE_IDX				_DMA_V2_FSM_GROUP_FSM_WR_STATE_IDX
+#define _DMA_FSM_GROUP_FSM_WR_CNT_YB_IDX			_DMA_V2_FSM_GROUP_FSM_WR_CNT_YB_IDX
+#define _DMA_FSM_GROUP_FSM_WR_CNT_XB_IDX			_DMA_V2_FSM_GROUP_FSM_WR_CNT_XB_IDX
+
+#define _DMA_DEV_INTERF_MAX_BURST_IDX			_DMA_V2_DEV_INTERF_MAX_BURST_IDX
+
+/*
+ * Macro's to compute the DMA parameter register indices
+ */
+#define DMA_SEL_COMP(comp)     (((comp)  & _hrt_ones(_DMA_V2_ADDR_SEL_COMP_BITS))            << _DMA_V2_ADDR_SEL_COMP_IDX)
+#define DMA_SEL_CH(ch)         (((ch)    & _hrt_ones(_DMA_V2_ADDR_SEL_CH_REG_BITS))          << _DMA_V2_ADDR_SEL_CH_REG_IDX)
+#define DMA_SEL_PARAM(param)   (((param) & _hrt_ones(_DMA_V2_ADDR_SEL_PARAM_BITS))           << _DMA_V2_ADDR_SEL_PARAM_IDX)
+/* CG = Connection Group */
+#define DMA_SEL_CG_INFO(info)  (((info)  & _hrt_ones(_DMA_V2_ADDR_SEL_GROUP_COMP_INFO_BITS)) << _DMA_V2_ADDR_SEL_GROUP_COMP_INFO_IDX)
+#define DMA_SEL_CG_COMP(comp)  (((comp)  & _hrt_ones(_DMA_V2_ADDR_SEL_GROUP_COMP_BITS))      << _DMA_V2_ADDR_SEL_GROUP_COMP_IDX)
+#define DMA_SEL_DEV_INFO(info) (((info)  & _hrt_ones(_DMA_V2_ADDR_SEL_DEV_INTERF_INFO_BITS)) << _DMA_V2_ADDR_SEL_DEV_INTERF_INFO_IDX)
+#define DMA_SEL_DEV_ID(dev)    (((dev)   & _hrt_ones(_DMA_V2_ADDR_SEL_DEV_INTERF_IDX_BITS))  << _DMA_V2_ADDR_SEL_DEV_INTERF_IDX_IDX)
+
+#define DMA_COMMAND_FSM_REG_IDX					(DMA_SEL_COMP(_DMA_V2_SEL_FSM_CMD) >> 2)
+#define DMA_CHANNEL_PARAM_REG_IDX(ch, param)	((DMA_SEL_COMP(_DMA_V2_SEL_CH_REG) | DMA_SEL_CH(ch) | DMA_SEL_PARAM(param)) >> 2)
+#define DMA_CG_INFO_REG_IDX(info_id, comp_id)	((DMA_SEL_COMP(_DMA_V2_SEL_CONN_GROUP) | DMA_SEL_CG_INFO(info_id) | DMA_SEL_CG_COMP(comp_id)) >> 2)
+#define DMA_DEV_INFO_REG_IDX(info_id, dev_id)	((DMA_SEL_COMP(_DMA_V2_SEL_DEV_INTERF) | DMA_SEL_DEV_INFO(info_id) | DMA_SEL_DEV_ID(dev_id)) >> 2)
+#define DMA_RST_REG_IDX							(DMA_SEL_COMP(_DMA_V2_SEL_RESET) >> 2)
+
+#define DMA_GET_CONNECTION(val)    _hrt_get_bits(val, _DMA_V2_CONNECTION_IDX,    _DMA_V2_CONNECTION_BITS)
+#define DMA_GET_EXTENSION(val)     _hrt_get_bits(val, _DMA_V2_EXTENSION_IDX,     _DMA_V2_EXTENSION_BITS)
+#define DMA_GET_ELEMENTS(val)      _hrt_get_bits(val, _DMA_V2_ELEMENTS_IDX,      _DMA_V2_ELEMENTS_BITS)
+#define DMA_GET_CROPPING(val)      _hrt_get_bits(val, _DMA_V2_LEFT_CROPPING_IDX, _DMA_V2_LEFT_CROPPING_BITS)
+
+typedef enum {
+	DMA_CTRL_STATE_IDLE,
+	DMA_CTRL_STATE_REQ_RCV,
+	DMA_CTRL_STATE_RCV,
+	DMA_CTRL_STATE_RCV_REQ,
+	DMA_CTRL_STATE_INIT,
+	N_DMA_CTRL_STATES
+} dma_ctrl_states_t;
+
+typedef enum {
+	DMA_COMMAND_READ,
+	DMA_COMMAND_WRITE,
+	DMA_COMMAND_SET_CHANNEL,
+	DMA_COMMAND_SET_PARAM,
+	DMA_COMMAND_READ_SPECIFIC,
+	DMA_COMMAND_WRITE_SPECIFIC,
+	DMA_COMMAND_INIT,
+	DMA_COMMAND_INIT_SPECIFIC,
+	DMA_COMMAND_RST,
+	N_DMA_COMMANDS
+} dma_commands_t;
+
+typedef enum {
+	DMA_RW_STATE_IDLE,
+	DMA_RW_STATE_REQ,
+	DMA_RW_STATE_NEXT_LINE,
+	DMA_RW_STATE_UNLOCK_CHANNEL,
+	N_DMA_RW_STATES
+} dma_rw_states_t;
+
+typedef enum {
+	DMA_FIFO_STATE_WILL_BE_FULL,
+	DMA_FIFO_STATE_FULL,
+	DMA_FIFO_STATE_EMPTY,
+	N_DMA_FIFO_STATES
+} dma_fifo_states_t;
+
+/* typedef struct dma_state_s			dma_state_t; */
+typedef struct dma_channel_state_s	dma_channel_state_t;
+typedef struct dma_port_state_s		dma_port_state_t;
+
+struct dma_port_state_s {
+	bool                       req_cs;
+	bool                       req_we_n;
+	bool                       req_run;
+	bool                       req_ack;
+	bool                       send_cs;
+	bool                       send_we_n;
+	bool                       send_run;
+	bool                       send_ack;
+	dma_fifo_states_t          fifo_state;
+	int                        fifo_counter;
+};
+
+struct dma_channel_state_s {
+	int                        connection;
+	bool                       sign_extend;
+	int                        height;
+	int                        stride_a;
+	int                        elems_a;
+	int                        cropping_a;
+	int                        width_a;
+	int                        stride_b;
+	int                        elems_b;
+	int                        cropping_b;
+	int                        width_b;
+};
+
+struct dma_state_s {
+	bool                       fsm_command_idle;
+	bool                       fsm_command_run;
+	bool                       fsm_command_stalling;
+	bool                       fsm_command_error;
+	dma_commands_t             last_command;
+	int                        last_command_channel;
+	int                        last_command_param;
+	dma_commands_t             current_command;
+	int                        current_addr_a;
+	int                        current_addr_b;
+	bool                       fsm_ctrl_idle;
+	bool                       fsm_ctrl_run;
+	bool                       fsm_ctrl_stalling;
+	bool                       fsm_ctrl_error;
+	dma_ctrl_states_t          fsm_ctrl_state;
+	int                        fsm_ctrl_source_dev;
+	int                        fsm_ctrl_source_addr;
+	int                        fsm_ctrl_source_stride;
+	int                        fsm_ctrl_source_width;
+	int                        fsm_ctrl_source_height;
+	int                        fsm_ctrl_pack_source_dev;
+	int                        fsm_ctrl_pack_dest_dev;
+	int                        fsm_ctrl_dest_addr;
+	int                        fsm_ctrl_dest_stride;
+	int                        fsm_ctrl_pack_source_width;
+	int                        fsm_ctrl_pack_dest_height;
+	int                        fsm_ctrl_pack_dest_width;
+	int                        fsm_ctrl_pack_source_elems;
+	int                        fsm_ctrl_pack_dest_elems;
+	int                        fsm_ctrl_pack_extension;
+	int						   pack_idle;
+	int			       pack_run;
+	int				   pack_stalling;
+	int				   pack_error;
+	int                        pack_cnt_height;
+	int                        pack_src_cnt_width;
+	int                        pack_dest_cnt_width;
+	dma_rw_states_t            read_state;
+	int                        read_cnt_height;
+	int                        read_cnt_width;
+	dma_rw_states_t            write_state;
+	int                        write_height;
+	int                        write_width;
+	dma_port_state_t           port_states[HIVE_ISP_NUM_DMA_CONNS];
+	dma_channel_state_t        channel_states[HIVE_DMA_NUM_CHANNELS];
+};
+
+#endif /* __DMA_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_private.h
new file mode 100644
index 000000000..1f62bc2f1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/dma_private.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DMA_PRIVATE_H_INCLUDED__
+#define __DMA_PRIVATE_H_INCLUDED__
+
+#include "dma_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_DMA_C void dma_reg_store(const dma_ID_t ID,
+				       const unsigned int reg,
+				       const hrt_data value)
+{
+	assert(ID < N_DMA_ID);
+	assert(DMA_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(DMA_BASE[ID] + reg * sizeof(hrt_data), value);
+}
+
+STORAGE_CLASS_DMA_C hrt_data dma_reg_load(const dma_ID_t ID,
+	const unsigned int reg)
+{
+	assert(ID < N_DMA_ID);
+	assert(DMA_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(DMA_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+#endif /* __DMA_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo.c
new file mode 100644
index 000000000..62d4809e3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo.c
@@ -0,0 +1,20 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "event_fifo.h"
+
+#ifndef __INLINE_EVENT__
+#include "event_fifo_private.h"
+#endif /* __INLINE_EVENT__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_local.h
new file mode 100644
index 000000000..25d382302
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_local.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _EVENT_FIFO_LOCAL_H
+#define _EVENT_FIFO_LOCAL_H
+
+/*
+ * All events come from connections mapped on the system
+ * bus but do not use a global IRQ
+ */
+#include "event_fifo_global.h"
+
+typedef enum {
+	SP0_EVENT_ID,
+	ISP0_EVENT_ID,
+	STR2MIPI_EVENT_ID,
+	N_EVENT_ID
+} event_ID_t;
+
+#define	EVENT_QUERY_BIT		0
+
+/* Events are read from FIFO */
+static const hrt_address event_source_addr[N_EVENT_ID] = {
+	0x0000000000380000ULL,
+	0x0000000000380004ULL,
+	0xffffffffffffffffULL
+};
+
+/* Read from FIFO are blocking, query data availability */
+static const hrt_address event_source_query_addr[N_EVENT_ID] = {
+	0x0000000000380010ULL,
+	0x0000000000380014ULL,
+	0xffffffffffffffffULL
+};
+
+/* Events are written to FIFO */
+static const hrt_address event_sink_addr[N_EVENT_ID] = {
+	0x0000000000380008ULL,
+	0x000000000038000CULL,
+	0x0000000000090104ULL
+};
+
+/* Writes to FIFO are blocking, query data space */
+static const hrt_address event_sink_query_addr[N_EVENT_ID] = {
+	0x0000000000380018ULL,
+	0x000000000038001CULL,
+	0x000000000009010CULL
+};
+
+#endif /* _EVENT_FIFO_LOCAL_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_private.h
new file mode 100644
index 000000000..f59d45cc7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/event_fifo_private.h
@@ -0,0 +1,78 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __EVENT_FIFO_PRIVATE_H
+#define __EVENT_FIFO_PRIVATE_H
+
+#include "event_fifo_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+#include <bits.h>			/* _hrt_get_bits() */
+
+STORAGE_CLASS_EVENT_C void event_wait_for(const event_ID_t ID)
+{
+	assert(ID < N_EVENT_ID);
+	assert(event_source_addr[ID] != ((hrt_address) - 1));
+	(void)ia_css_device_load_uint32(event_source_addr[ID]);
+	return;
+}
+
+STORAGE_CLASS_EVENT_C void cnd_event_wait_for(const event_ID_t ID,
+	const bool cnd)
+{
+	if (cnd) {
+		event_wait_for(ID);
+	}
+}
+
+STORAGE_CLASS_EVENT_C hrt_data event_receive_token(const event_ID_t ID)
+{
+	assert(ID < N_EVENT_ID);
+	assert(event_source_addr[ID] != ((hrt_address) - 1));
+	return ia_css_device_load_uint32(event_source_addr[ID]);
+}
+
+STORAGE_CLASS_EVENT_C void event_send_token(const event_ID_t ID,
+	const hrt_data token)
+{
+	assert(ID < N_EVENT_ID);
+	assert(event_sink_addr[ID] != ((hrt_address) - 1));
+	ia_css_device_store_uint32(event_sink_addr[ID], token);
+}
+
+STORAGE_CLASS_EVENT_C bool is_event_pending(const event_ID_t ID)
+{
+	hrt_data	value;
+
+	assert(ID < N_EVENT_ID);
+	assert(event_source_query_addr[ID] != ((hrt_address) - 1));
+	value = ia_css_device_load_uint32(event_source_query_addr[ID]);
+	return !_hrt_get_bit(value, EVENT_QUERY_BIT);
+}
+
+STORAGE_CLASS_EVENT_C bool can_event_send_token(const event_ID_t ID)
+{
+	hrt_data	value;
+
+	assert(ID < N_EVENT_ID);
+	assert(event_sink_query_addr[ID] != ((hrt_address) - 1));
+	value = ia_css_device_load_uint32(event_sink_query_addr[ID]);
+	return !_hrt_get_bit(value, EVENT_QUERY_BIT);
+}
+
+#endif /* __EVENT_FIFO_PRIVATE_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor.c
new file mode 100644
index 000000000..01698064b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor.c
@@ -0,0 +1,570 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "fifo_monitor.h"
+
+#include <type_support.h>
+#include "device_access.h"
+
+#include <bits.h>
+
+#include "gp_device.h"
+
+#include "assert_support.h"
+
+#ifndef __INLINE_FIFO_MONITOR__
+#define STORAGE_CLASS_FIFO_MONITOR_DATA static const
+#else
+#define STORAGE_CLASS_FIFO_MONITOR_DATA const
+#endif /* __INLINE_FIFO_MONITOR__ */
+
+STORAGE_CLASS_FIFO_MONITOR_DATA unsigned int FIFO_SWITCH_ADDR[N_FIFO_SWITCH] = {
+	_REG_GP_SWITCH_IF_ADDR,
+	_REG_GP_SWITCH_GDC1_ADDR,
+	_REG_GP_SWITCH_GDC2_ADDR
+};
+
+#ifndef __INLINE_FIFO_MONITOR__
+#include "fifo_monitor_private.h"
+#endif /* __INLINE_FIFO_MONITOR__ */
+
+static inline bool fifo_monitor_status_valid(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const unsigned int			port_id);
+
+static inline bool fifo_monitor_status_accept(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const unsigned int			port_id);
+
+void fifo_channel_get_state(
+    const fifo_monitor_ID_t		ID,
+    const fifo_channel_t		channel_id,
+    fifo_channel_state_t		*state)
+{
+	assert(channel_id < N_FIFO_CHANNEL);
+	assert(state);
+
+	switch (channel_id) {
+	case FIFO_CHANNEL_ISP0_TO_SP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_SP); /* ISP_STR_MON_PORT_ISP2SP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_SP);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_ISP); /* ISP_STR_MON_PORT_SP2ISP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_ISP);
+		break;
+	case FIFO_CHANNEL_SP0_TO_ISP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_ISP); /* ISP_STR_MON_PORT_SP2ISP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_ISP);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_SP); /* ISP_STR_MON_PORT_ISP2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_SP);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_IF0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_PIF_A); /* ISP_STR_MON_PORT_ISP2PIFA */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_PIF_A);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_A); /* MOD_STR_MON_PORT_CELLS2PIFA */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_A);
+		break;
+	case FIFO_CHANNEL_IF0_TO_ISP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_A); /* MOD_STR_MON_PORT_PIFA2CELLS */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_A);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_A); /* ISP_STR_MON_PORT_PIFA2ISP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_A);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_IF1:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_PIF_B); /* ISP_STR_MON_PORT_ISP2PIFA */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_PIF_B);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_B); /* MOD_STR_MON_PORT_CELLS2PIFB */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_B);
+		break;
+	case FIFO_CHANNEL_IF1_TO_ISP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_B); /* MOD_STR_MON_PORT_PIFB2CELLS */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_B);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_B); /* ISP_STR_MON_PORT_PIFB2ISP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_B);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_DMA0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_DMA); /* ISP_STR_MON_PORT_ISP2DMA */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_DMA);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_DMA_FR_ISP); /* MOD_STR_MON_PORT_ISP2DMA */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_DMA_FR_ISP);
+		break;
+	case FIFO_CHANNEL_DMA0_TO_ISP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_DMA2ISP); /* MOD_STR_MON_PORT_DMA2ISP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_DMA2ISP);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_DMA); /* ISP_STR_MON_PORT_DMA2ISP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_DMA);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_GDC0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_GDC); /* ISP_STR_MON_PORT_ISP2GDC1 */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_GDC);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_GDC); /* MOD_STR_MON_PORT_CELLS2GDC1 */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_GDC);
+		break;
+	case FIFO_CHANNEL_GDC0_TO_ISP0:
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_GDC); /* MOD_STR_MON_PORT_GDC12CELLS */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_GDC);
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_GDC); /* ISP_STR_MON_PORT_GDC12ISP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_GDC);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_GDC1:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_ISP2GDC2);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_ISP2GDC2);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC2);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC2);
+		break;
+	case FIFO_CHANNEL_GDC1_TO_ISP0:
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC22CELLS);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC22CELLS);
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_GDC22ISP);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_GDC22ISP);
+		break;
+	case FIFO_CHANNEL_ISP0_TO_HOST0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_GPD); /* ISP_STR_MON_PORT_ISP2GPD */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_SND_GPD);
+		{
+			hrt_data	value = ia_css_device_load_uint32(0x0000000000380014ULL);
+
+			state->fifo_valid  = !_hrt_get_bit(value, 0);
+			state->sink_accept = false; /* no monitor connected */
+		}
+		break;
+	case FIFO_CHANNEL_HOST0_TO_ISP0: {
+		hrt_data	value = ia_css_device_load_uint32(0x000000000038001CULL);
+
+		state->fifo_valid  = false; /* no monitor connected */
+		state->sink_accept = !_hrt_get_bit(value, 0);
+	}
+	state->src_valid   = fifo_monitor_status_valid(ID,
+			     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_GPD); /* ISP_STR_MON_PORT_FA2ISP */
+	state->fifo_accept = fifo_monitor_status_accept(ID,
+			     HIVE_GP_REGS_ISP_STREAM_STAT_IDX,
+			     ISP_STR_MON_PORT_RCV_GPD);
+	break;
+	case FIFO_CHANNEL_SP0_TO_IF0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_PIF_A); /* SP_STR_MON_PORT_SP2PIFA */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_PIF_A);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_A); /* MOD_STR_MON_PORT_CELLS2PIFA */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_A);
+		break;
+	case FIFO_CHANNEL_IF0_TO_SP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_A); /* MOD_STR_MON_PORT_PIFA2CELLS */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_A);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_PIF_A); /* SP_STR_MON_PORT_PIFA2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_PIF_A);
+		break;
+	case FIFO_CHANNEL_SP0_TO_IF1:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_PIF_B); /* SP_STR_MON_PORT_SP2PIFB */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_PIF_B);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_B); /* MOD_STR_MON_PORT_CELLS2PIFB */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_PIF_B);
+		break;
+	case FIFO_CHANNEL_IF1_TO_SP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_B); /* MOD_STR_MON_PORT_PIFB2CELLS */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_PIF_B);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_B); /* SP_STR_MON_PORT_PIFB2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     ISP_STR_MON_PORT_RCV_PIF_B);
+		break;
+	case FIFO_CHANNEL_SP0_TO_IF2:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_SIF); /* SP_STR_MON_PORT_SP2SIF */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_SIF);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_SIF); /* MOD_STR_MON_PORT_SP2SIF */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_SIF);
+		break;
+	case FIFO_CHANNEL_IF2_TO_SP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_SIF); /* MOD_STR_MON_PORT_SIF2SP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_SIF);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_SIF); /* SP_STR_MON_PORT_SIF2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_SIF);
+		break;
+	case FIFO_CHANNEL_SP0_TO_DMA0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_DMA); /* SP_STR_MON_PORT_SP2DMA */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_DMA);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_DMA_FR_SP); /* MOD_STR_MON_PORT_SP2DMA */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_DMA_FR_SP);
+		break;
+	case FIFO_CHANNEL_DMA0_TO_SP0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_DMA2SP); /* MOD_STR_MON_PORT_DMA2SP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_DMA2SP);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_DMA); /* SP_STR_MON_PORT_DMA2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_DMA);
+		break;
+	case FIFO_CHANNEL_SP0_TO_GDC0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_SP2GDC1);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_SP2GDC1);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC1);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC1);
+		break;
+	case FIFO_CHANNEL_GDC0_TO_SP0:
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC12CELLS);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC12CELLS);
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_GDC12SP);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_GDC12SP);
+		break;
+	case FIFO_CHANNEL_SP0_TO_GDC1:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_SP2GDC2);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_SP2GDC2);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC2);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_CELLS2GDC2);
+		break;
+	case FIFO_CHANNEL_GDC1_TO_SP0:
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC22CELLS);
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_GDC22CELLS);
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_GDC22SP);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_B_IDX,
+				     SP_STR_MON_PORT_B_GDC22SP);
+		break;
+	case FIFO_CHANNEL_SP0_TO_HOST0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_GPD); /* SP_STR_MON_PORT_SP2GPD */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_GPD);
+		{
+			hrt_data	value = ia_css_device_load_uint32(0x0000000000380010ULL);
+
+			state->fifo_valid  = !_hrt_get_bit(value, 0);
+			state->sink_accept = false; /* no monitor connected */
+		}
+		break;
+	case FIFO_CHANNEL_HOST0_TO_SP0: {
+		hrt_data	value = ia_css_device_load_uint32(0x0000000000380018ULL);
+
+		state->fifo_valid  = false; /* no monitor connected */
+		state->sink_accept = !_hrt_get_bit(value, 0);
+	}
+	state->src_valid   = fifo_monitor_status_valid(ID,
+			     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_GPD); /* SP_STR_MON_PORT_FA2SP */
+	state->fifo_accept = fifo_monitor_status_accept(ID,
+			     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+			     SP_STR_MON_PORT_RCV_GPD);
+	break;
+	case FIFO_CHANNEL_SP0_TO_STREAM2MEM0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_MC); /* SP_STR_MON_PORT_SP2MC */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SND_MC);
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_MC); /* MOD_STR_MON_PORT_SP2MC */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_RCV_MC);
+		break;
+	case FIFO_CHANNEL_STREAM2MEM0_TO_SP0:
+		state->fifo_valid  = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_MC); /* SP_STR_MON_PORT_MC2SP */
+		state->sink_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_MOD_STREAM_STAT_IDX,
+				     MOD_STR_MON_PORT_SND_MC);
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_MC); /* MOD_STR_MON_PORT_MC2SP */
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_RCV_MC);
+		break;
+	case FIFO_CHANNEL_SP0_TO_INPUT_SYSTEM0:
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SP2ISYS);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_SP2ISYS);
+		state->fifo_valid  = false;
+		state->sink_accept = false;
+		break;
+	case FIFO_CHANNEL_INPUT_SYSTEM0_TO_SP0:
+		state->fifo_valid  = false;
+		state->sink_accept = false;
+		state->src_valid   = fifo_monitor_status_valid(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_ISYS2SP);
+		state->fifo_accept = fifo_monitor_status_accept(ID,
+				     HIVE_GP_REGS_SP_STREAM_STAT_IDX,
+				     SP_STR_MON_PORT_ISYS2SP);
+		break;
+	default:
+		assert(0);
+		break;
+	}
+
+	return;
+}
+
+void fifo_switch_get_state(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id,
+    fifo_switch_state_t			*state)
+{
+	hrt_data		data = (hrt_data)-1;
+
+	assert(ID == FIFO_MONITOR0_ID);
+	assert(switch_id < N_FIFO_SWITCH);
+	assert(state);
+
+	(void)ID;
+
+	data = gp_device_reg_load(GP_DEVICE0_ID, FIFO_SWITCH_ADDR[switch_id]);
+
+	state->is_none = (data == HIVE_ISP_CSS_STREAM_SWITCH_NONE);
+	state->is_sp = (data == HIVE_ISP_CSS_STREAM_SWITCH_SP);
+	state->is_isp = (data == HIVE_ISP_CSS_STREAM_SWITCH_ISP);
+
+	return;
+}
+
+void fifo_monitor_get_state(
+    const fifo_monitor_ID_t		ID,
+    fifo_monitor_state_t		*state)
+{
+	fifo_channel_t	ch_id;
+	fifo_switch_t	sw_id;
+
+	assert(ID < N_FIFO_MONITOR_ID);
+	assert(state);
+
+	for (ch_id = 0; ch_id < N_FIFO_CHANNEL; ch_id++) {
+		fifo_channel_get_state(ID, ch_id,
+				       &state->fifo_channels[ch_id]);
+	}
+
+	for (sw_id = 0; sw_id < N_FIFO_SWITCH; sw_id++) {
+		fifo_switch_get_state(ID, sw_id,
+				      &state->fifo_switches[sw_id]);
+	}
+	return;
+}
+
+static inline bool fifo_monitor_status_valid(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const unsigned int			port_id)
+{
+	hrt_data	data = fifo_monitor_reg_load(ID, reg);
+
+	return (data >> (((port_id * 2) + _hive_str_mon_valid_offset))) & 0x1;
+}
+
+static inline bool fifo_monitor_status_accept(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const unsigned int			port_id)
+{
+	hrt_data	data = fifo_monitor_reg_load(ID, reg);
+
+	return (data >> (((port_id * 2) + _hive_str_mon_accept_offset))) & 0x1;
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_local.h
new file mode 100644
index 000000000..dfdca944a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_local.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __FIFO_MONITOR_LOCAL_H_INCLUDED__
+#define __FIFO_MONITOR_LOCAL_H_INCLUDED__
+
+#include <type_support.h>
+#include "fifo_monitor_global.h"
+
+#include "hive_isp_css_defs.h"	/* ISP_STR_MON_PORT_SND_SP, ... */
+
+#define _hive_str_mon_valid_offset   0
+#define _hive_str_mon_accept_offset  1
+
+#define	FIFO_CHANNEL_SP_VALID_MASK		0x55555555
+#define	FIFO_CHANNEL_SP_VALID_B_MASK	0x00000055
+#define	FIFO_CHANNEL_ISP_VALID_MASK		0x15555555
+#define	FIFO_CHANNEL_MOD_VALID_MASK		0x55555555
+
+typedef enum fifo_switch {
+	FIFO_SWITCH_IF,
+	FIFO_SWITCH_GDC0,
+	FIFO_SWITCH_GDC1,
+	N_FIFO_SWITCH
+} fifo_switch_t;
+
+typedef enum fifo_channel {
+	FIFO_CHANNEL_ISP0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_IF0,
+	FIFO_CHANNEL_IF0_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_IF1,
+	FIFO_CHANNEL_IF1_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_DMA0,
+	FIFO_CHANNEL_DMA0_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_GDC0,
+	FIFO_CHANNEL_GDC0_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_GDC1,
+	FIFO_CHANNEL_GDC1_TO_ISP0,
+	FIFO_CHANNEL_ISP0_TO_HOST0,
+	FIFO_CHANNEL_HOST0_TO_ISP0,
+	FIFO_CHANNEL_SP0_TO_IF0,
+	FIFO_CHANNEL_IF0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_IF1,
+	FIFO_CHANNEL_IF1_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_IF2,
+	FIFO_CHANNEL_IF2_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_DMA0,
+	FIFO_CHANNEL_DMA0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_GDC0,
+	FIFO_CHANNEL_GDC0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_GDC1,
+	FIFO_CHANNEL_GDC1_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_HOST0,
+	FIFO_CHANNEL_HOST0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_STREAM2MEM0,
+	FIFO_CHANNEL_STREAM2MEM0_TO_SP0,
+	FIFO_CHANNEL_SP0_TO_INPUT_SYSTEM0,
+	FIFO_CHANNEL_INPUT_SYSTEM0_TO_SP0,
+	/*
+	 * No clue what this is
+	 *
+		FIFO_CHANNEL_SP0_TO_IRQ0,
+		FIFO_CHANNEL_IRQ0_TO_SP0,
+	 */
+	N_FIFO_CHANNEL
+} fifo_channel_t;
+
+struct fifo_channel_state_s {
+	bool	src_valid;
+	bool	fifo_accept;
+	bool	fifo_valid;
+	bool	sink_accept;
+};
+
+/* The switch is tri-state */
+struct fifo_switch_state_s {
+	bool	is_none;
+	bool	is_isp;
+	bool	is_sp;
+};
+
+struct fifo_monitor_state_s {
+	struct fifo_channel_state_s	fifo_channels[N_FIFO_CHANNEL];
+	struct fifo_switch_state_s	fifo_switches[N_FIFO_SWITCH];
+};
+
+#endif /* __FIFO_MONITOR_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_private.h
new file mode 100644
index 000000000..10d9c076c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/fifo_monitor_private.h
@@ -0,0 +1,81 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __FIFO_MONITOR_PRIVATE_H_INCLUDED__
+#define __FIFO_MONITOR_PRIVATE_H_INCLUDED__
+
+#include "fifo_monitor_public.h"
+
+#define __INLINE_GP_DEVICE__
+#include "gp_device.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+#ifdef __INLINE_FIFO_MONITOR__
+extern const unsigned int FIFO_SWITCH_ADDR[N_FIFO_SWITCH];
+#endif
+
+STORAGE_CLASS_FIFO_MONITOR_C void fifo_switch_set(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id,
+    const hrt_data				sel)
+{
+	assert(ID == FIFO_MONITOR0_ID);
+	assert(FIFO_MONITOR_BASE[ID] != (hrt_address) - 1);
+	assert(switch_id < N_FIFO_SWITCH);
+	(void)ID;
+
+	gp_device_reg_store(GP_DEVICE0_ID, FIFO_SWITCH_ADDR[switch_id], sel);
+
+	return;
+}
+
+STORAGE_CLASS_FIFO_MONITOR_C hrt_data fifo_switch_get(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id)
+{
+	assert(ID == FIFO_MONITOR0_ID);
+	assert(FIFO_MONITOR_BASE[ID] != (hrt_address) - 1);
+	assert(switch_id < N_FIFO_SWITCH);
+	(void)ID;
+
+	return gp_device_reg_load(GP_DEVICE0_ID, FIFO_SWITCH_ADDR[switch_id]);
+}
+
+STORAGE_CLASS_FIFO_MONITOR_C void fifo_monitor_reg_store(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const hrt_data				value)
+{
+	assert(ID < N_FIFO_MONITOR_ID);
+	assert(FIFO_MONITOR_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(FIFO_MONITOR_BASE[ID] + reg * sizeof(hrt_data),
+				   value);
+	return;
+}
+
+STORAGE_CLASS_FIFO_MONITOR_C hrt_data fifo_monitor_reg_load(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg)
+{
+	assert(ID < N_FIFO_MONITOR_ID);
+	assert(FIFO_MONITOR_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(FIFO_MONITOR_BASE[ID] + reg * sizeof(
+					     hrt_data));
+}
+
+#endif /* __FIFO_MONITOR_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc.c
new file mode 100644
index 000000000..25e082d6a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc.c
@@ -0,0 +1,115 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* The name "gdc.h is already taken" */
+#include "gdc_device.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+/*
+ * Local function declarations
+ */
+static inline void gdc_reg_store(
+    const gdc_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value);
+
+#ifndef __INLINE_GDC__
+#include "gdc_private.h"
+#endif /* __INLINE_GDC__ */
+
+/*
+ * Exported function implementations
+ */
+void gdc_lut_store(
+    const gdc_ID_t		ID,
+    const int			data[4][HRT_GDC_N])
+{
+	unsigned int i, lut_offset = HRT_GDC_LUT_IDX;
+
+	assert(ID < N_GDC_ID);
+	assert(HRT_GDC_LUT_COEFF_OFFSET <= (4 * sizeof(hrt_data)));
+
+	for (i = 0; i < HRT_GDC_N; i++) {
+		hrt_data	entry_0 = data[0][i] & HRT_GDC_BCI_COEF_MASK;
+		hrt_data	entry_1 = data[1][i] & HRT_GDC_BCI_COEF_MASK;
+		hrt_data	entry_2 = data[2][i] & HRT_GDC_BCI_COEF_MASK;
+		hrt_data	entry_3 = data[3][i] & HRT_GDC_BCI_COEF_MASK;
+
+		hrt_data	word_0 = entry_0 |
+				     (entry_1 << HRT_GDC_LUT_COEFF_OFFSET);
+		hrt_data	word_1 = entry_2 |
+				     (entry_3 << HRT_GDC_LUT_COEFF_OFFSET);
+
+		gdc_reg_store(ID, lut_offset++, word_0);
+		gdc_reg_store(ID, lut_offset++, word_1);
+	}
+	return;
+}
+
+/*
+ * Input LUT format:
+ * c0[0-1023], c1[0-1023], c2[0-1023] c3[0-1023]
+ *
+ * Output LUT format (interleaved):
+ * c0[0], c1[0], c2[0], c3[0], c0[1], c1[1], c2[1], c3[1], ....
+ * c0[1023], c1[1023], c2[1023], c3[1023]
+ *
+ * The first format needs c0[0], c1[0] (which are 1024 words apart)
+ * to program gdc LUT registers. This makes it difficult to do piecemeal
+ * reads in SP side gdc_lut_store
+ *
+ * Interleaved format allows use of contiguous bytes to store into
+ * gdc LUT registers.
+ *
+ * See gdc_lut_store() definition in host/gdc.c vs sp/gdc_private.h
+ *
+ */
+void gdc_lut_convert_to_isp_format(const int in_lut[4][HRT_GDC_N],
+				   int out_lut[4][HRT_GDC_N])
+{
+	unsigned int i;
+	int *out = (int *)out_lut;
+
+	for (i = 0; i < HRT_GDC_N; i++) {
+		out[0] = in_lut[0][i];
+		out[1] = in_lut[1][i];
+		out[2] = in_lut[2][i];
+		out[3] = in_lut[3][i];
+		out += 4;
+	}
+}
+
+int gdc_get_unity(
+    const gdc_ID_t		ID)
+{
+	assert(ID < N_GDC_ID);
+	(void)ID;
+	return (int)(1UL << HRT_GDC_FRAC_BITS);
+}
+
+/*
+ * Local function implementations
+ */
+static inline void gdc_reg_store(
+    const gdc_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value)
+{
+	ia_css_device_store_uint32(GDC_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_local.h
new file mode 100644
index 000000000..4b2b3282c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GDC_LOCAL_H_INCLUDED__
+#define __GDC_LOCAL_H_INCLUDED__
+
+#include "gdc_global.h"
+
+#endif /* __GDC_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_private.h
new file mode 100644
index 000000000..73051112f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gdc_private.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GDC_PRIVATE_H_INCLUDED__
+#define __GDC_PRIVATE_H_INCLUDED__
+
+#include "gdc_public.h"
+
+#endif /* __GDC_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device.c
new file mode 100644
index 000000000..a80e547d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device.c
@@ -0,0 +1,109 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "assert_support.h"
+#include "gp_device.h"
+
+#ifndef __INLINE_GP_DEVICE__
+#include "gp_device_private.h"
+#endif /* __INLINE_GP_DEVICE__ */
+
+void gp_device_get_state(
+    const gp_device_ID_t		ID,
+    gp_device_state_t			*state)
+{
+	assert(ID < N_GP_DEVICE_ID);
+	assert(state);
+
+	state->syncgen_enable = gp_device_reg_load(ID,
+				_REG_GP_SYNCGEN_ENABLE_ADDR);
+	state->syncgen_free_running = gp_device_reg_load(ID,
+				      _REG_GP_SYNCGEN_FREE_RUNNING_ADDR);
+	state->syncgen_pause = gp_device_reg_load(ID,
+			       _REG_GP_SYNCGEN_PAUSE_ADDR);
+	state->nr_frames = gp_device_reg_load(ID,
+					      _REG_GP_NR_FRAMES_ADDR);
+	state->syngen_nr_pix = gp_device_reg_load(ID,
+			       _REG_GP_SYNGEN_NR_PIX_ADDR);
+	state->syngen_nr_pix = gp_device_reg_load(ID,
+			       _REG_GP_SYNGEN_NR_PIX_ADDR);
+	state->syngen_nr_lines = gp_device_reg_load(ID,
+				 _REG_GP_SYNGEN_NR_LINES_ADDR);
+	state->syngen_hblank_cycles = gp_device_reg_load(ID,
+				      _REG_GP_SYNGEN_HBLANK_CYCLES_ADDR);
+	state->syngen_vblank_cycles = gp_device_reg_load(ID,
+				      _REG_GP_SYNGEN_VBLANK_CYCLES_ADDR);
+	state->isel_sof = gp_device_reg_load(ID,
+					     _REG_GP_ISEL_SOF_ADDR);
+	state->isel_eof = gp_device_reg_load(ID,
+					     _REG_GP_ISEL_EOF_ADDR);
+	state->isel_sol = gp_device_reg_load(ID,
+					     _REG_GP_ISEL_SOL_ADDR);
+	state->isel_eol = gp_device_reg_load(ID,
+					     _REG_GP_ISEL_EOL_ADDR);
+	state->isel_lfsr_enable = gp_device_reg_load(ID,
+				  _REG_GP_ISEL_LFSR_ENABLE_ADDR);
+	state->isel_lfsr_enable_b = gp_device_reg_load(ID,
+				    _REG_GP_ISEL_LFSR_ENABLE_B_ADDR);
+	state->isel_lfsr_reset_value = gp_device_reg_load(ID,
+				       _REG_GP_ISEL_LFSR_RESET_VALUE_ADDR);
+	state->isel_tpg_enable = gp_device_reg_load(ID,
+				 _REG_GP_ISEL_TPG_ENABLE_ADDR);
+	state->isel_tpg_enable_b = gp_device_reg_load(ID,
+				   _REG_GP_ISEL_TPG_ENABLE_B_ADDR);
+	state->isel_hor_cnt_mask = gp_device_reg_load(ID,
+				   _REG_GP_ISEL_HOR_CNT_MASK_ADDR);
+	state->isel_ver_cnt_mask = gp_device_reg_load(ID,
+				   _REG_GP_ISEL_VER_CNT_MASK_ADDR);
+	state->isel_xy_cnt_mask = gp_device_reg_load(ID,
+				  _REG_GP_ISEL_XY_CNT_MASK_ADDR);
+	state->isel_hor_cnt_delta = gp_device_reg_load(ID,
+				    _REG_GP_ISEL_HOR_CNT_DELTA_ADDR);
+	state->isel_ver_cnt_delta = gp_device_reg_load(ID,
+				    _REG_GP_ISEL_VER_CNT_DELTA_ADDR);
+	state->isel_tpg_mode = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_TPG_MODE_ADDR);
+	state->isel_tpg_red1 = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_TPG_RED1_ADDR);
+	state->isel_tpg_green1 = gp_device_reg_load(ID,
+				 _REG_GP_ISEL_TPG_GREEN1_ADDR);
+	state->isel_tpg_blue1 = gp_device_reg_load(ID,
+				_REG_GP_ISEL_TPG_BLUE1_ADDR);
+	state->isel_tpg_red2 = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_TPG_RED2_ADDR);
+	state->isel_tpg_green2 = gp_device_reg_load(ID,
+				 _REG_GP_ISEL_TPG_GREEN2_ADDR);
+	state->isel_tpg_blue2 = gp_device_reg_load(ID,
+				_REG_GP_ISEL_TPG_BLUE2_ADDR);
+	state->isel_ch_id = gp_device_reg_load(ID,
+					       _REG_GP_ISEL_CH_ID_ADDR);
+	state->isel_fmt_type = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_FMT_TYPE_ADDR);
+	state->isel_data_sel = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_DATA_SEL_ADDR);
+	state->isel_sband_sel = gp_device_reg_load(ID,
+				_REG_GP_ISEL_SBAND_SEL_ADDR);
+	state->isel_sync_sel = gp_device_reg_load(ID,
+			       _REG_GP_ISEL_SYNC_SEL_ADDR);
+	state->syncgen_hor_cnt = gp_device_reg_load(ID,
+				 _REG_GP_SYNCGEN_HOR_CNT_ADDR);
+	state->syncgen_ver_cnt = gp_device_reg_load(ID,
+				 _REG_GP_SYNCGEN_VER_CNT_ADDR);
+	state->syncgen_frame_cnt = gp_device_reg_load(ID,
+				   _REG_GP_SYNCGEN_FRAME_CNT_ADDR);
+	state->soft_reset = gp_device_reg_load(ID,
+					       _REG_GP_SOFT_RESET_ADDR);
+	return;
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_local.h
new file mode 100644
index 000000000..320ed3526
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_local.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_DEVICE_LOCAL_H_INCLUDED__
+#define __GP_DEVICE_LOCAL_H_INCLUDED__
+
+#include "gp_device_global.h"
+
+/* @ GP_REGS_BASE -> GP_DEVICE_BASE */
+#define _REG_GP_SDRAM_WAKEUP_ADDR					0x00
+#define _REG_GP_IDLE_ADDR							0x04
+/* #define _REG_GP_IRQ_REQ0_ADDR					0x08 */
+/* #define _REG_GP_IRQ_REQ1_ADDR					0x0C */
+#define _REG_GP_SP_STREAM_STAT_ADDR					0x10
+#define _REG_GP_SP_STREAM_STAT_B_ADDR				0x14
+#define _REG_GP_ISP_STREAM_STAT_ADDR				0x18
+#define _REG_GP_MOD_STREAM_STAT_ADDR				0x1C
+#define _REG_GP_SP_STREAM_STAT_IRQ_COND_ADDR		0x20
+#define _REG_GP_SP_STREAM_STAT_B_IRQ_COND_ADDR		0x24
+#define _REG_GP_ISP_STREAM_STAT_IRQ_COND_ADDR		0x28
+#define _REG_GP_MOD_STREAM_STAT_IRQ_COND_ADDR		0x2C
+#define _REG_GP_SP_STREAM_STAT_IRQ_ENABLE_ADDR		0x30
+#define _REG_GP_SP_STREAM_STAT_B_IRQ_ENABLE_ADDR	0x34
+#define _REG_GP_ISP_STREAM_STAT_IRQ_ENABLE_ADDR		0x38
+#define _REG_GP_MOD_STREAM_STAT_IRQ_ENABLE_ADDR		0x3C
+/*
+#define _REG_GP_SWITCH_IF_ADDR						0x40
+#define _REG_GP_SWITCH_GDC1_ADDR					0x44
+#define _REG_GP_SWITCH_GDC2_ADDR					0x48
+*/
+#define _REG_GP_SLV_REG_RST_ADDR					0x50
+#define _REG_GP_SWITCH_ISYS2401_ADDR				0x54
+
+/* @ INPUT_FORMATTER_BASE -> GP_DEVICE_BASE */
+/*
+#define _REG_GP_IFMT_input_switch_lut_reg0			0x00030800
+#define _REG_GP_IFMT_input_switch_lut_reg1			0x00030804
+#define _REG_GP_IFMT_input_switch_lut_reg2			0x00030808
+#define _REG_GP_IFMT_input_switch_lut_reg3			0x0003080C
+#define _REG_GP_IFMT_input_switch_lut_reg4			0x00030810
+#define _REG_GP_IFMT_input_switch_lut_reg5			0x00030814
+#define _REG_GP_IFMT_input_switch_lut_reg6			0x00030818
+#define _REG_GP_IFMT_input_switch_lut_reg7			0x0003081C
+#define _REG_GP_IFMT_input_switch_fsync_lut			0x00030820
+#define _REG_GP_IFMT_srst							0x00030824
+#define _REG_GP_IFMT_slv_reg_srst					0x00030828
+#define _REG_GP_IFMT_input_switch_ch_id_fmt_type	0x0003082C
+*/
+/* @ GP_DEVICE_BASE */
+/*
+#define _REG_GP_SYNCGEN_ENABLE_ADDR					0x00090000
+#define _REG_GP_SYNCGEN_FREE_RUNNING_ADDR			0x00090004
+#define _REG_GP_SYNCGEN_PAUSE_ADDR					0x00090008
+#define _REG_GP_NR_FRAMES_ADDR						0x0009000C
+#define _REG_GP_SYNGEN_NR_PIX_ADDR					0x00090010
+#define _REG_GP_SYNGEN_NR_LINES_ADDR				0x00090014
+#define _REG_GP_SYNGEN_HBLANK_CYCLES_ADDR			0x00090018
+#define _REG_GP_SYNGEN_VBLANK_CYCLES_ADDR			0x0009001C
+#define _REG_GP_ISEL_SOF_ADDR						0x00090020
+#define _REG_GP_ISEL_EOF_ADDR						0x00090024
+#define _REG_GP_ISEL_SOL_ADDR						0x00090028
+#define _REG_GP_ISEL_EOL_ADDR						0x0009002C
+#define _REG_GP_ISEL_LFSR_ENABLE_ADDR				0x00090030
+#define _REG_GP_ISEL_LFSR_ENABLE_B_ADDR				0x00090034
+#define _REG_GP_ISEL_LFSR_RESET_VALUE_ADDR			0x00090038
+#define _REG_GP_ISEL_TPG_ENABLE_ADDR				0x0009003C
+#define _REG_GP_ISEL_TPG_ENABLE_B_ADDR				0x00090040
+#define _REG_GP_ISEL_HOR_CNT_MASK_ADDR				0x00090044
+#define _REG_GP_ISEL_VER_CNT_MASK_ADDR				0x00090048
+#define _REG_GP_ISEL_XY_CNT_MASK_ADDR				0x0009004C
+#define _REG_GP_ISEL_HOR_CNT_DELTA_ADDR				0x00090050
+#define _REG_GP_ISEL_VER_CNT_DELTA_ADDR				0x00090054
+#define _REG_GP_ISEL_TPG_MODE_ADDR					0x00090058
+#define _REG_GP_ISEL_TPG_RED1_ADDR					0x0009005C
+#define _REG_GP_ISEL_TPG_GREEN1_ADDR				0x00090060
+#define _REG_GP_ISEL_TPG_BLUE1_ADDR					0x00090064
+#define _REG_GP_ISEL_TPG_RED2_ADDR					0x00090068
+#define _REG_GP_ISEL_TPG_GREEN2_ADDR				0x0009006C
+#define _REG_GP_ISEL_TPG_BLUE2_ADDR					0x00090070
+#define _REG_GP_ISEL_CH_ID_ADDR						0x00090074
+#define _REG_GP_ISEL_FMT_TYPE_ADDR					0x00090078
+#define _REG_GP_ISEL_DATA_SEL_ADDR					0x0009007C
+#define _REG_GP_ISEL_SBAND_SEL_ADDR					0x00090080
+#define _REG_GP_ISEL_SYNC_SEL_ADDR					0x00090084
+#define _REG_GP_SYNCGEN_HOR_CNT_ADDR				0x00090088
+#define _REG_GP_SYNCGEN_VER_CNT_ADDR				0x0009008C
+#define _REG_GP_SYNCGEN_FRAME_CNT_ADDR				0x00090090
+#define _REG_GP_SOFT_RESET_ADDR						0x00090094
+*/
+
+struct gp_device_state_s {
+	int syncgen_enable;
+	int syncgen_free_running;
+	int syncgen_pause;
+	int nr_frames;
+	int syngen_nr_pix;
+	int syngen_nr_lines;
+	int syngen_hblank_cycles;
+	int syngen_vblank_cycles;
+	int isel_sof;
+	int isel_eof;
+	int isel_sol;
+	int isel_eol;
+	int isel_lfsr_enable;
+	int isel_lfsr_enable_b;
+	int isel_lfsr_reset_value;
+	int isel_tpg_enable;
+	int isel_tpg_enable_b;
+	int isel_hor_cnt_mask;
+	int isel_ver_cnt_mask;
+	int isel_xy_cnt_mask;
+	int isel_hor_cnt_delta;
+	int isel_ver_cnt_delta;
+	int isel_tpg_mode;
+	int isel_tpg_red1;
+	int isel_tpg_green1;
+	int isel_tpg_blue1;
+	int isel_tpg_red2;
+	int isel_tpg_green2;
+	int isel_tpg_blue2;
+	int isel_ch_id;
+	int isel_fmt_type;
+	int isel_data_sel;
+	int isel_sband_sel;
+	int isel_sync_sel;
+	int syncgen_hor_cnt;
+	int syncgen_ver_cnt;
+	int syncgen_frame_cnt;
+	int soft_reset;
+};
+
+#endif /* __GP_DEVICE_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_private.h
new file mode 100644
index 000000000..f11a19f21
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_device_private.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_DEVICE_PRIVATE_H_INCLUDED__
+#define __GP_DEVICE_PRIVATE_H_INCLUDED__
+
+#include "gp_device_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_GP_DEVICE_C void gp_device_reg_store(
+    const gp_device_ID_t	ID,
+    const unsigned int		reg_addr,
+    const hrt_data			value)
+{
+	assert(ID < N_GP_DEVICE_ID);
+	assert(GP_DEVICE_BASE[ID] != (hrt_address) - 1);
+	assert((reg_addr % sizeof(hrt_data)) == 0);
+	ia_css_device_store_uint32(GP_DEVICE_BASE[ID] + reg_addr, value);
+	return;
+}
+
+STORAGE_CLASS_GP_DEVICE_C hrt_data gp_device_reg_load(
+    const gp_device_ID_t	ID,
+    const hrt_address	reg_addr)
+{
+	assert(ID < N_GP_DEVICE_ID);
+	assert(GP_DEVICE_BASE[ID] != (hrt_address)-1);
+	assert((reg_addr % sizeof(hrt_data)) == 0);
+	return ia_css_device_load_uint32(GP_DEVICE_BASE[ID] + reg_addr);
+}
+
+#endif /* __GP_DEVICE_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer.c
new file mode 100644
index 000000000..2a58dba3c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer.c
@@ -0,0 +1,71 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <type_support.h> /*uint32_t */
+#include "gp_timer.h"   /*system_local.h,
+			  gp_timer_public.h*/
+
+#ifndef __INLINE_GP_TIMER__
+#include "gp_timer_private.h"  /*device_access.h*/
+#endif /* __INLINE_GP_TIMER__ */
+#include "system_local.h"
+
+/* FIXME: not sure if reg_load(), reg_store() should be API.
+ */
+static uint32_t
+gp_timer_reg_load(uint32_t reg);
+
+static void
+gp_timer_reg_store(u32 reg, uint32_t value);
+
+static uint32_t
+gp_timer_reg_load(uint32_t reg)
+{
+	return ia_css_device_load_uint32(
+		   GP_TIMER_BASE +
+		   (reg * sizeof(uint32_t)));
+}
+
+static void
+gp_timer_reg_store(u32 reg, uint32_t value)
+{
+	ia_css_device_store_uint32((GP_TIMER_BASE +
+				    (reg * sizeof(uint32_t))),
+				   value);
+}
+
+void gp_timer_init(gp_timer_ID_t ID)
+{
+	/* set_overall_enable*/
+	gp_timer_reg_store(_REG_GP_TIMER_OVERALL_ENABLE, 1);
+
+	/*set enable*/
+	gp_timer_reg_store(_REG_GP_TIMER_ENABLE_ID(ID), 1);
+
+	/* set signal select */
+	gp_timer_reg_store(_REG_GP_TIMER_SIGNAL_SELECT_ID(ID), GP_TIMER_SIGNAL_SELECT);
+
+	/*set count type */
+	gp_timer_reg_store(_REG_GP_TIMER_COUNT_TYPE_ID(ID), GP_TIMER_COUNT_TYPE_LOW);
+
+	/*reset gp timer */
+	gp_timer_reg_store(_REG_GP_TIMER_RESET_REG, 0xFF);
+}
+
+uint32_t
+gp_timer_read(gp_timer_ID_t ID)
+{
+	return	gp_timer_reg_load(_REG_GP_TIMER_VALUE_ID(ID));
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_local.h
new file mode 100644
index 000000000..efede2558
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_local.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_TIMER_LOCAL_H_INCLUDED__
+#define  __GP_TIMER_LOCAL_H_INCLUDED__
+
+#include "gp_timer_global.h" /*GP_TIMER_SEL
+				GP_TIMER_SIGNAL_SELECT*/
+
+#include "gp_timer_defs.h"    /*HIVE_GP_TIMER_xxx registers*/
+#include "hive_isp_css_defs.h" /*HIVE_GP_TIMER_NUM_COUNTERS
+				 HIVE_GP_TIMER_NUM_IRQS*/
+
+#define _REG_GP_TIMER_RESET_REG HIVE_GP_TIMER_RESET_REG_IDX
+#define _REG_GP_TIMER_OVERALL_ENABLE HIVE_GP_TIMER_OVERALL_ENABLE_REG_IDX
+
+/*Register offsets for timers [1,7] can be obtained
+ * by adding (GP_TIMERx_ID * sizeof(uint32_t))*/
+#define _REG_GP_TIMER_ENABLE_ID(timer_id)        HIVE_GP_TIMER_ENABLE_REG_IDX(timer_id)
+#define _REG_GP_TIMER_VALUE_ID(timer_id)	 HIVE_GP_TIMER_VALUE_REG_IDX(timer_id, HIVE_GP_TIMER_NUM_COUNTERS)
+#define _REG_GP_TIMER_COUNT_TYPE_ID(timer_id)    HIVE_GP_TIMER_COUNT_TYPE_REG_IDX(timer_id, HIVE_GP_TIMER_NUM_COUNTERS)
+#define _REG_GP_TIMER_SIGNAL_SELECT_ID(timer_id) HIVE_GP_TIMER_SIGNAL_SELECT_REG_IDX(timer_id, HIVE_GP_TIMER_NUM_COUNTERS)
+
+#define _REG_GP_TIMER_IRQ_TRIGGER_VALUE_ID(irq_id) HIVE_GP_TIMER_IRQ_TRIGGER_VALUE_REG_IDX(irq_id, HIVE_GP_TIMER_NUM_COUNTERS)
+
+#define _REG_GP_TIMER_IRQ_TIMER_SELECT_ID(irq_id)   \
+	HIVE_GP_TIMER_IRQ_TIMER_SELECT_REG_IDX(irq_id, HIVE_GP_TIMER_NUM_COUNTERS, HIVE_GP_TIMER_NUM_IRQS)
+
+#define _REG_GP_TIMER_IRQ_ENABLE_ID(irq_id) \
+	HIVE_GP_TIMER_IRQ_ENABLE_REG_IDX(irq_id, HIVE_GP_TIMER_NUM_COUNTERS, HIVE_GP_TIMER_NUM_IRQS)
+
+#endif  /*__GP_TIMER_LOCAL_H_INCLUDED__*/
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_private.h
new file mode 100644
index 000000000..3e1b36105
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gp_timer_private.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_TIMER_PRIVATE_H_INCLUDED__
+#define __GP_TIMER_PRIVATE_H_INCLUDED__
+
+#include "gp_timer_public.h"
+#include "device_access.h"
+#include "assert_support.h"
+
+#endif /* __GP_TIMER_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_local.h
new file mode 100644
index 000000000..14013733f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GPIO_LOCAL_H_INCLUDED__
+#define __GPIO_LOCAL_H_INCLUDED__
+
+#include "gpio_global.h"
+
+#endif /* __GPIO_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_private.h
new file mode 100644
index 000000000..cc60bed71
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/gpio_private.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GPIO_PRIVATE_H_INCLUDED__
+#define __GPIO_PRIVATE_H_INCLUDED__
+
+#include "gpio_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_GPIO_C void gpio_reg_store(
+    const gpio_ID_t	ID,
+    const unsigned int		reg,
+    const hrt_data			value)
+{
+	OP___assert(ID < N_GPIO_ID);
+	OP___assert(GPIO_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(GPIO_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_GPIO_C hrt_data gpio_reg_load(
+    const gpio_ID_t	ID,
+    const unsigned int		reg)
+{
+	OP___assert(ID < N_GPIO_ID);
+	OP___assert(GPIO_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(GPIO_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+#endif /* __GPIO_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem.c
new file mode 100644
index 000000000..be102d5ce
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem.c
@@ -0,0 +1,20 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmem.h"
+
+#ifndef __INLINE_HMEM__
+#include "hmem_private.h"
+#endif /* __INLINE_HMEM__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_local.h
new file mode 100644
index 000000000..a3ee274bd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __HMEM_LOCAL_H_INCLUDED__
+#define __HMEM_LOCAL_H_INCLUDED__
+
+#include "hmem_global.h"
+
+#endif /* __HMEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_private.h
new file mode 100644
index 000000000..80d81983b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/hmem_private.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __HMEM_PRIVATE_H_INCLUDED__
+#define __HMEM_PRIVATE_H_INCLUDED__
+
+#include "hmem_public.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_HMEM_C size_t sizeof_hmem(
+    const hmem_ID_t		ID)
+{
+	assert(ID < N_HMEM_ID);
+	(void)ID;
+	return HMEM_SIZE * sizeof(hmem_data_t);
+}
+
+#endif /* __HMEM_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter.c
new file mode 100644
index 000000000..5cd6136f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter.c
@@ -0,0 +1,246 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#ifndef ISP2401
+
+#include "input_formatter.h"
+#include <type_support.h>
+#include "gp_device.h"
+
+#include "assert_support.h"
+
+#ifndef __INLINE_INPUT_FORMATTER__
+#include "input_formatter_private.h"
+#endif /* __INLINE_INPUT_FORMATTER__ */
+
+static const unsigned int input_formatter_alignment[N_INPUT_FORMATTER_ID] = {
+	ISP_VEC_ALIGN, ISP_VEC_ALIGN, HIVE_ISP_CTRL_DATA_BYTES
+};
+
+const hrt_address HIVE_IF_SRST_ADDRESS[N_INPUT_FORMATTER_ID] = {
+	INPUT_FORMATTER0_SRST_OFFSET,
+	INPUT_FORMATTER1_SRST_OFFSET,
+	INPUT_FORMATTER2_SRST_OFFSET,
+	INPUT_FORMATTER3_SRST_OFFSET
+};
+
+const hrt_data HIVE_IF_SRST_MASK[N_INPUT_FORMATTER_ID] = {
+	INPUT_FORMATTER0_SRST_MASK,
+	INPUT_FORMATTER1_SRST_MASK,
+	INPUT_FORMATTER2_SRST_MASK,
+	INPUT_FORMATTER3_SRST_MASK
+};
+
+const u8 HIVE_IF_SWITCH_CODE[N_INPUT_FORMATTER_ID] = {
+	HIVE_INPUT_SWITCH_SELECT_IF_PRIM,
+	HIVE_INPUT_SWITCH_SELECT_IF_PRIM,
+	HIVE_INPUT_SWITCH_SELECT_IF_SEC,
+	HIVE_INPUT_SWITCH_SELECT_STR_TO_MEM
+};
+
+/* MW Should be part of system_global.h, where we have the main enumeration */
+static const bool HIVE_IF_BIN_COPY[N_INPUT_FORMATTER_ID] = {
+	false, false, false, true
+};
+
+void input_formatter_rst(
+    const input_formatter_ID_t		ID)
+{
+	hrt_address	addr;
+	hrt_data	rst;
+
+	assert(ID < N_INPUT_FORMATTER_ID);
+
+	addr = HIVE_IF_SRST_ADDRESS[ID];
+	rst = HIVE_IF_SRST_MASK[ID];
+
+	/* TEMPORARY HACK: THIS RESET BREAKS THE METADATA FEATURE
+	 * WICH USES THE STREAM2MEMRY BLOCK.
+	 * MUST BE FIXED PROPERLY
+	 */
+	if (!HIVE_IF_BIN_COPY[ID]) {
+		input_formatter_reg_store(ID, addr, rst);
+	}
+
+	return;
+}
+
+unsigned int input_formatter_get_alignment(
+    const input_formatter_ID_t		ID)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+
+	return input_formatter_alignment[ID];
+}
+
+void input_formatter_set_fifo_blocking_mode(
+    const input_formatter_ID_t		ID,
+    const bool						enable)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+
+	/* cnd_input_formatter_reg_store() */
+	if (!HIVE_IF_BIN_COPY[ID]) {
+		input_formatter_reg_store(ID,
+					  HIVE_IF_BLOCK_FIFO_NO_REQ_ADDRESS, enable);
+	}
+	return;
+}
+
+void input_formatter_get_switch_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_switch_state_t	*state)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+	assert(state);
+
+	/* We'll change this into an intelligent function to get switch info per IF */
+	(void)ID;
+
+	state->if_input_switch_lut_reg[0] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg0);
+	state->if_input_switch_lut_reg[1] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg1);
+	state->if_input_switch_lut_reg[2] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg2);
+	state->if_input_switch_lut_reg[3] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg3);
+	state->if_input_switch_lut_reg[4] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg4);
+	state->if_input_switch_lut_reg[5] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg5);
+	state->if_input_switch_lut_reg[6] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg6);
+	state->if_input_switch_lut_reg[7] = gp_device_reg_load(GP_DEVICE0_ID,
+					    _REG_GP_IFMT_input_switch_lut_reg7);
+	state->if_input_switch_fsync_lut = gp_device_reg_load(GP_DEVICE0_ID,
+					   _REG_GP_IFMT_input_switch_fsync_lut);
+	state->if_input_switch_ch_id_fmt_type = gp_device_reg_load(GP_DEVICE0_ID,
+						_REG_GP_IFMT_input_switch_ch_id_fmt_type);
+
+	return;
+}
+
+void input_formatter_get_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_state_t			*state)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+	assert(state);
+	/*
+		state->reset = input_formatter_reg_load(ID,
+			HIVE_IF_RESET_ADDRESS);
+	 */
+	state->start_line = input_formatter_reg_load(ID,
+			    HIVE_IF_START_LINE_ADDRESS);
+	state->start_column = input_formatter_reg_load(ID,
+			      HIVE_IF_START_COLUMN_ADDRESS);
+	state->cropped_height = input_formatter_reg_load(ID,
+				HIVE_IF_CROPPED_HEIGHT_ADDRESS);
+	state->cropped_width = input_formatter_reg_load(ID,
+			       HIVE_IF_CROPPED_WIDTH_ADDRESS);
+	state->ver_decimation = input_formatter_reg_load(ID,
+				HIVE_IF_VERTICAL_DECIMATION_ADDRESS);
+	state->hor_decimation = input_formatter_reg_load(ID,
+				HIVE_IF_HORIZONTAL_DECIMATION_ADDRESS);
+	state->hor_deinterleaving = input_formatter_reg_load(ID,
+				    HIVE_IF_H_DEINTERLEAVING_ADDRESS);
+	state->left_padding = input_formatter_reg_load(ID,
+			      HIVE_IF_LEFTPADDING_WIDTH_ADDRESS);
+	state->eol_offset = input_formatter_reg_load(ID,
+			    HIVE_IF_END_OF_LINE_OFFSET_ADDRESS);
+	state->vmem_start_address = input_formatter_reg_load(ID,
+				    HIVE_IF_VMEM_START_ADDRESS_ADDRESS);
+	state->vmem_end_address = input_formatter_reg_load(ID,
+				  HIVE_IF_VMEM_END_ADDRESS_ADDRESS);
+	state->vmem_increment = input_formatter_reg_load(ID,
+				HIVE_IF_VMEM_INCREMENT_ADDRESS);
+	state->is_yuv420 = input_formatter_reg_load(ID,
+			   HIVE_IF_YUV_420_FORMAT_ADDRESS);
+	state->vsync_active_low = input_formatter_reg_load(ID,
+				  HIVE_IF_VSYNCK_ACTIVE_LOW_ADDRESS);
+	state->hsync_active_low = input_formatter_reg_load(ID,
+				  HIVE_IF_HSYNCK_ACTIVE_LOW_ADDRESS);
+	state->allow_fifo_overflow = input_formatter_reg_load(ID,
+				     HIVE_IF_ALLOW_FIFO_OVERFLOW_ADDRESS);
+	state->block_fifo_when_no_req = input_formatter_reg_load(ID,
+					HIVE_IF_BLOCK_FIFO_NO_REQ_ADDRESS);
+	state->ver_deinterleaving = input_formatter_reg_load(ID,
+				    HIVE_IF_V_DEINTERLEAVING_ADDRESS);
+	/* FSM */
+	state->fsm_sync_status = input_formatter_reg_load(ID,
+				 HIVE_IF_FSM_SYNC_STATUS);
+	state->fsm_sync_counter = input_formatter_reg_load(ID,
+				  HIVE_IF_FSM_SYNC_COUNTER);
+	state->fsm_crop_status = input_formatter_reg_load(ID,
+				 HIVE_IF_FSM_CROP_STATUS);
+	state->fsm_crop_line_counter = input_formatter_reg_load(ID,
+				       HIVE_IF_FSM_CROP_LINE_COUNTER);
+	state->fsm_crop_pixel_counter = input_formatter_reg_load(ID,
+					HIVE_IF_FSM_CROP_PIXEL_COUNTER);
+	state->fsm_deinterleaving_index = input_formatter_reg_load(ID,
+					  HIVE_IF_FSM_DEINTERLEAVING_IDX);
+	state->fsm_dec_h_counter = input_formatter_reg_load(ID,
+				   HIVE_IF_FSM_DECIMATION_H_COUNTER);
+	state->fsm_dec_v_counter = input_formatter_reg_load(ID,
+				   HIVE_IF_FSM_DECIMATION_V_COUNTER);
+	state->fsm_dec_block_v_counter = input_formatter_reg_load(ID,
+					 HIVE_IF_FSM_DECIMATION_BLOCK_V_COUNTER);
+	state->fsm_padding_status = input_formatter_reg_load(ID,
+				    HIVE_IF_FSM_PADDING_STATUS);
+	state->fsm_padding_elem_counter = input_formatter_reg_load(ID,
+					  HIVE_IF_FSM_PADDING_ELEMENT_COUNTER);
+	state->fsm_vector_support_error = input_formatter_reg_load(ID,
+					  HIVE_IF_FSM_VECTOR_SUPPORT_ERROR);
+	state->fsm_vector_buffer_full = input_formatter_reg_load(ID,
+					HIVE_IF_FSM_VECTOR_SUPPORT_BUFF_FULL);
+	state->vector_support = input_formatter_reg_load(ID,
+				HIVE_IF_FSM_VECTOR_SUPPORT);
+	state->sensor_data_lost = input_formatter_reg_load(ID,
+				  HIVE_IF_FIFO_SENSOR_STATUS);
+
+	return;
+}
+
+void input_formatter_bin_get_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_bin_state_t		*state)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+	assert(state);
+
+	state->reset = input_formatter_reg_load(ID,
+						HIVE_STR2MEM_SOFT_RESET_REG_ADDRESS);
+	state->input_endianness = input_formatter_reg_load(ID,
+				  HIVE_STR2MEM_INPUT_ENDIANNESS_REG_ADDRESS);
+	state->output_endianness = input_formatter_reg_load(ID,
+				   HIVE_STR2MEM_OUTPUT_ENDIANNESS_REG_ADDRESS);
+	state->bitswap = input_formatter_reg_load(ID,
+			 HIVE_STR2MEM_BIT_SWAPPING_REG_ADDRESS);
+	state->block_synch = input_formatter_reg_load(ID,
+			     HIVE_STR2MEM_BLOCK_SYNC_LEVEL_REG_ADDRESS);
+	state->packet_synch = input_formatter_reg_load(ID,
+			      HIVE_STR2MEM_PACKET_SYNC_LEVEL_REG_ADDRESS);
+	state->readpostwrite_synch = input_formatter_reg_load(ID,
+				     HIVE_STR2MEM_READ_POST_WRITE_SYNC_ENABLE_REG_ADDRESS);
+	state->is_2ppc = input_formatter_reg_load(ID,
+			 HIVE_STR2MEM_DUAL_BYTE_INPUTS_ENABLED_REG_ADDRESS);
+	state->en_status_update = input_formatter_reg_load(ID,
+				  HIVE_STR2MEM_EN_STAT_UPDATE_ADDRESS);
+	return;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_local.h
new file mode 100644
index 000000000..dfb593c10
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_local.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_FORMATTER_LOCAL_H_INCLUDED__
+#define __INPUT_FORMATTER_LOCAL_H_INCLUDED__
+
+#include "input_formatter_global.h"
+
+#include "isp.h"		/* ISP_VEC_ALIGN */
+
+typedef struct input_formatter_switch_state_s	input_formatter_switch_state_t;
+typedef struct input_formatter_state_s			input_formatter_state_t;
+typedef struct input_formatter_bin_state_s		input_formatter_bin_state_t;
+
+#define HIVE_IF_FSM_SYNC_STATUS                 0x100
+#define HIVE_IF_FSM_SYNC_COUNTER                0x104
+#define HIVE_IF_FSM_DEINTERLEAVING_IDX          0x114
+#define HIVE_IF_FSM_DECIMATION_H_COUNTER        0x118
+#define HIVE_IF_FSM_DECIMATION_V_COUNTER        0x11C
+#define HIVE_IF_FSM_DECIMATION_BLOCK_V_COUNTER  0x120
+#define HIVE_IF_FSM_PADDING_STATUS              0x124
+#define HIVE_IF_FSM_PADDING_ELEMENT_COUNTER     0x128
+#define HIVE_IF_FSM_VECTOR_SUPPORT_ERROR        0x12C
+#define HIVE_IF_FSM_VECTOR_SUPPORT_BUFF_FULL    0x130
+#define HIVE_IF_FSM_VECTOR_SUPPORT              0x134
+#define HIVE_IF_FIFO_SENSOR_STATUS              0x138
+
+/*
+ * The switch LUT's coding defines a sink for each
+ * single channel ID + channel format type. Conversely
+ * the sink (i.e. an input formatter) can be reached
+ * from multiple channel & format type combinations
+ *
+ * LUT[0,1] channel=0, format type {0,1,...31}
+ * LUT[2,3] channel=1, format type {0,1,...31}
+ * LUT[4,5] channel=2, format type {0,1,...31}
+ * LUT[6,7] channel=3, format type {0,1,...31}
+ *
+ * Each register hold 16 2-bit fields encoding the sink
+ * {0,1,2,3}, "0" means unconnected.
+ *
+ * The single FSYNCH register uses four 3-bit fields of 1-hot
+ * encoded sink information, "0" means unconnected.
+ *
+ * The encoding is redundant. The FSYNCH setting will connect
+ * a channel to a sink. At that point the LUT's belonging to
+ * that channel can be directed to another sink. Thus the data
+ * goes to another place than the synch
+ */
+struct input_formatter_switch_state_s {
+	int	if_input_switch_lut_reg[8];
+	int	if_input_switch_fsync_lut;
+	int	if_input_switch_ch_id_fmt_type;
+	bool if_input_switch_map[HIVE_SWITCH_N_CHANNELS][HIVE_SWITCH_N_FORMATTYPES];
+};
+
+struct input_formatter_state_s {
+	/*	int	reset; */
+	int	start_line;
+	int	start_column;
+	int	cropped_height;
+	int	cropped_width;
+	int	ver_decimation;
+	int	hor_decimation;
+	int	ver_deinterleaving;
+	int	hor_deinterleaving;
+	int	left_padding;
+	int	eol_offset;
+	int	vmem_start_address;
+	int	vmem_end_address;
+	int	vmem_increment;
+	int	is_yuv420;
+	int	vsync_active_low;
+	int	hsync_active_low;
+	int	allow_fifo_overflow;
+	int block_fifo_when_no_req;
+	int	fsm_sync_status;
+	int	fsm_sync_counter;
+	int	fsm_crop_status;
+	int	fsm_crop_line_counter;
+	int	fsm_crop_pixel_counter;
+	int	fsm_deinterleaving_index;
+	int	fsm_dec_h_counter;
+	int	fsm_dec_v_counter;
+	int	fsm_dec_block_v_counter;
+	int	fsm_padding_status;
+	int	fsm_padding_elem_counter;
+	int	fsm_vector_support_error;
+	int	fsm_vector_buffer_full;
+	int	vector_support;
+	int	sensor_data_lost;
+};
+
+struct input_formatter_bin_state_s {
+	u32	reset;
+	u32	input_endianness;
+	u32	output_endianness;
+	u32	bitswap;
+	u32	block_synch;
+	u32	packet_synch;
+	u32	readpostwrite_synch;
+	u32	is_2ppc;
+	u32	en_status_update;
+};
+
+#endif /* __INPUT_FORMATTER_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_private.h
new file mode 100644
index 000000000..e2bc952e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_formatter_private.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_FORMATTER_PRIVATE_H_INCLUDED__
+#define __INPUT_FORMATTER_PRIVATE_H_INCLUDED__
+
+#include "input_formatter_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_INPUT_FORMATTER_C void input_formatter_reg_store(
+    const input_formatter_ID_t		ID,
+    const hrt_address			reg_addr,
+    const hrt_data				value)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+	assert(INPUT_FORMATTER_BASE[ID] != (hrt_address)-1);
+	assert((reg_addr % sizeof(hrt_data)) == 0);
+	ia_css_device_store_uint32(INPUT_FORMATTER_BASE[ID] + reg_addr, value);
+	return;
+}
+
+STORAGE_CLASS_INPUT_FORMATTER_C hrt_data input_formatter_reg_load(
+    const input_formatter_ID_t	ID,
+    const unsigned int			reg_addr)
+{
+	assert(ID < N_INPUT_FORMATTER_ID);
+	assert(INPUT_FORMATTER_BASE[ID] != (hrt_address)-1);
+	assert((reg_addr % sizeof(hrt_data)) == 0);
+	return ia_css_device_load_uint32(INPUT_FORMATTER_BASE[ID] + reg_addr);
+}
+
+#endif /* __INPUT_FORMATTER_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_system.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_system.c
new file mode 100644
index 000000000..712e01c37
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/input_system.c
@@ -0,0 +1,1790 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#ifndef ISP2401
+
+#include "input_system.h"
+#include <type_support.h>
+#include "gp_device.h"
+
+#include "assert_support.h"
+
+#ifndef __INLINE_INPUT_SYSTEM__
+#include "input_system_private.h"
+#endif /* __INLINE_INPUT_SYSTEM__ */
+
+#define ZERO (0x0)
+#define ONE  (1U)
+
+static const isp2400_ib_buffer_t   IB_BUFFER_NULL = {0, 0, 0 };
+
+static input_system_err_t input_system_configure_channel(
+    const channel_cfg_t		channel);
+
+static input_system_err_t input_system_configure_channel_sensor(
+    const channel_cfg_t		channel);
+
+static input_system_err_t input_buffer_configuration(void);
+
+static input_system_err_t configuration_to_registers(void);
+
+static void receiver_rst(const rx_ID_t ID);
+static void input_system_network_rst(const input_system_ID_t ID);
+
+static void capture_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const isp2400_ib_buffer_t *const cfg);
+
+static void acquisition_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const isp2400_ib_buffer_t *const cfg);
+
+static void ctrl_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const ctrl_unit_cfg_t *const cfg);
+
+static void input_system_network_configure(
+    const input_system_ID_t			ID,
+    const input_system_network_cfg_t *const cfg);
+
+// MW: CSI is previously named as "rx" short for "receiver"
+static input_system_err_t set_csi_cfg(
+    csi_cfg_t *const lhs,
+    const csi_cfg_t *const rhs,
+    input_system_config_flags_t *const flags);
+
+static input_system_err_t set_source_type(
+    input_system_source_t *const lhs,
+    const input_system_source_t				rhs,
+    input_system_config_flags_t *const flags);
+
+static input_system_err_t input_system_multiplexer_cfg(
+    input_system_multiplex_t *const lhs,
+    const input_system_multiplex_t			rhs,
+    input_system_config_flags_t *const flags);
+
+static inline void capture_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    capture_unit_state_t			*state);
+
+static inline void acquisition_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    acquisition_unit_state_t		*state);
+
+static inline void ctrl_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    ctrl_unit_state_t				*state);
+
+static inline void mipi_port_get_state(
+    const rx_ID_t					ID,
+    const enum mipi_port_id			port_ID,
+    mipi_port_state_t				*state);
+
+static inline void rx_channel_get_state(
+    const rx_ID_t					ID,
+    const unsigned int				ch_id,
+    rx_channel_state_t				*state);
+
+static void gp_device_rst(const gp_device_ID_t		ID);
+
+static void input_selector_cfg_for_sensor(const gp_device_ID_t	ID);
+
+static void input_switch_rst(const gp_device_ID_t	ID);
+
+static void input_switch_cfg(
+    const gp_device_ID_t				ID,
+    const input_switch_cfg_t *const cfg
+);
+
+void input_system_get_state(
+    const input_system_ID_t			ID,
+    input_system_state_t			*state)
+{
+	sub_system_ID_t	sub_id;
+
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(state);
+
+	state->str_multicastA_sel = input_system_sub_system_reg_load(ID,
+				    GPREGS_UNIT0_ID,
+				    HIVE_ISYS_GPREG_MULTICAST_A_IDX);
+	state->str_multicastB_sel = input_system_sub_system_reg_load(ID,
+				    GPREGS_UNIT0_ID,
+				    HIVE_ISYS_GPREG_MULTICAST_B_IDX);
+	state->str_multicastC_sel = input_system_sub_system_reg_load(ID,
+				    GPREGS_UNIT0_ID,
+				    HIVE_ISYS_GPREG_MULTICAST_C_IDX);
+	state->str_mux_sel = input_system_sub_system_reg_load(ID,
+			     GPREGS_UNIT0_ID,
+			     HIVE_ISYS_GPREG_MUX_IDX);
+	state->str_mon_status = input_system_sub_system_reg_load(ID,
+				GPREGS_UNIT0_ID,
+				HIVE_ISYS_GPREG_STRMON_STAT_IDX);
+	state->str_mon_irq_cond = input_system_sub_system_reg_load(ID,
+				  GPREGS_UNIT0_ID,
+				  HIVE_ISYS_GPREG_STRMON_COND_IDX);
+	state->str_mon_irq_en = input_system_sub_system_reg_load(ID,
+				GPREGS_UNIT0_ID,
+				HIVE_ISYS_GPREG_STRMON_IRQ_EN_IDX);
+	state->isys_srst = input_system_sub_system_reg_load(ID,
+			   GPREGS_UNIT0_ID,
+			   HIVE_ISYS_GPREG_SRST_IDX);
+	state->isys_slv_reg_srst = input_system_sub_system_reg_load(ID,
+				   GPREGS_UNIT0_ID,
+				   HIVE_ISYS_GPREG_SLV_REG_SRST_IDX);
+	state->str_deint_portA_cnt = input_system_sub_system_reg_load(ID,
+				     GPREGS_UNIT0_ID,
+				     HIVE_ISYS_GPREG_REG_PORT_A_IDX);
+	state->str_deint_portB_cnt = input_system_sub_system_reg_load(ID,
+				     GPREGS_UNIT0_ID,
+				     HIVE_ISYS_GPREG_REG_PORT_B_IDX);
+
+	for (sub_id = CAPTURE_UNIT0_ID; sub_id < CAPTURE_UNIT0_ID + N_CAPTURE_UNIT_ID;
+	     sub_id++) {
+		capture_unit_get_state(ID, sub_id,
+				       &state->capture_unit[sub_id - CAPTURE_UNIT0_ID]);
+	}
+	for (sub_id = ACQUISITION_UNIT0_ID;
+	     sub_id < ACQUISITION_UNIT0_ID + N_ACQUISITION_UNIT_ID; sub_id++) {
+		acquisition_unit_get_state(ID, sub_id,
+					   &state->acquisition_unit[sub_id - ACQUISITION_UNIT0_ID]);
+	}
+	for (sub_id = CTRL_UNIT0_ID; sub_id < CTRL_UNIT0_ID + N_CTRL_UNIT_ID;
+	     sub_id++) {
+		ctrl_unit_get_state(ID, sub_id,
+				    &state->ctrl_unit_state[sub_id - CTRL_UNIT0_ID]);
+	}
+}
+
+void receiver_get_state(
+    const rx_ID_t				ID,
+    receiver_state_t			*state)
+{
+	enum mipi_port_id	port_id;
+	unsigned int	ch_id;
+
+	assert(ID < N_RX_ID);
+	assert(state);
+
+	state->fs_to_ls_delay = (uint8_t)receiver_reg_load(ID,
+				_HRT_CSS_RECEIVER_FS_TO_LS_DELAY_REG_IDX);
+	state->ls_to_data_delay = (uint8_t)receiver_reg_load(ID,
+				  _HRT_CSS_RECEIVER_LS_TO_DATA_DELAY_REG_IDX);
+	state->data_to_le_delay = (uint8_t)receiver_reg_load(ID,
+				  _HRT_CSS_RECEIVER_DATA_TO_LE_DELAY_REG_IDX);
+	state->le_to_fe_delay = (uint8_t)receiver_reg_load(ID,
+				_HRT_CSS_RECEIVER_LE_TO_FE_DELAY_REG_IDX);
+	state->fe_to_fs_delay = (uint8_t)receiver_reg_load(ID,
+				_HRT_CSS_RECEIVER_FE_TO_FS_DELAY_REG_IDX);
+	state->le_to_fs_delay = (uint8_t)receiver_reg_load(ID,
+				_HRT_CSS_RECEIVER_LE_TO_LS_DELAY_REG_IDX);
+	state->is_two_ppc = (bool)receiver_reg_load(ID,
+			    _HRT_CSS_RECEIVER_TWO_PIXEL_EN_REG_IDX);
+	state->backend_rst = receiver_reg_load(ID,
+					       _HRT_CSS_RECEIVER_BACKEND_RST_REG_IDX);
+	state->raw18 = (uint16_t)receiver_reg_load(ID,
+		       _HRT_CSS_RECEIVER_RAW18_REG_IDX);
+	state->force_raw8 = (bool)receiver_reg_load(ID,
+			    _HRT_CSS_RECEIVER_FORCE_RAW8_REG_IDX);
+	state->raw16 = (uint16_t)receiver_reg_load(ID,
+		       _HRT_CSS_RECEIVER_RAW16_REG_IDX);
+
+	for (port_id = (enum mipi_port_id)0; port_id < N_MIPI_PORT_ID; port_id++) {
+		mipi_port_get_state(ID, port_id,
+				    &state->mipi_port_state[port_id]);
+	}
+	for (ch_id = 0U; ch_id < N_RX_CHANNEL_ID; ch_id++) {
+		rx_channel_get_state(ID, ch_id,
+				     &state->rx_channel_state[ch_id]);
+	}
+
+	state->be_gsp_acc_ovl = receiver_reg_load(ID,
+				_HRT_CSS_RECEIVER_BE_GSP_ACC_OVL_REG_IDX);
+	state->be_srst = receiver_reg_load(ID,
+					   _HRT_CSS_RECEIVER_BE_SRST_REG_IDX);
+	state->be_is_two_ppc = receiver_reg_load(ID,
+			       _HRT_CSS_RECEIVER_BE_TWO_PPC_REG_IDX);
+	state->be_comp_format0 = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG0_IDX);
+	state->be_comp_format1 = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG1_IDX);
+	state->be_comp_format2 = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG2_IDX);
+	state->be_comp_format3 = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG3_IDX);
+	state->be_sel = receiver_reg_load(ID,
+					  _HRT_CSS_RECEIVER_BE_SEL_REG_IDX);
+	state->be_raw16_config = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_RAW16_CONFIG_REG_IDX);
+	state->be_raw18_config = receiver_reg_load(ID,
+				 _HRT_CSS_RECEIVER_BE_RAW18_CONFIG_REG_IDX);
+	state->be_force_raw8 = receiver_reg_load(ID,
+			       _HRT_CSS_RECEIVER_BE_FORCE_RAW8_REG_IDX);
+	state->be_irq_status = receiver_reg_load(ID,
+			       _HRT_CSS_RECEIVER_BE_IRQ_STATUS_REG_IDX);
+	state->be_irq_clear = receiver_reg_load(ID,
+						_HRT_CSS_RECEIVER_BE_IRQ_CLEAR_REG_IDX);
+}
+
+bool is_mipi_format_yuv420(
+    const mipi_format_t			mipi_format)
+{
+	bool	is_yuv420 = (
+				(mipi_format == MIPI_FORMAT_YUV420_8) ||
+				(mipi_format == MIPI_FORMAT_YUV420_10) ||
+				(mipi_format == MIPI_FORMAT_YUV420_8_SHIFT) ||
+				(mipi_format == MIPI_FORMAT_YUV420_10_SHIFT));
+	/* MIPI_FORMAT_YUV420_8_LEGACY is not YUV420 */
+
+	return is_yuv420;
+}
+
+void receiver_set_compression(
+    const rx_ID_t			ID,
+    const unsigned int		cfg_ID,
+    const mipi_compressor_t		comp,
+    const mipi_predictor_t		pred)
+{
+	const unsigned int		field_id = cfg_ID % N_MIPI_FORMAT_CUSTOM;
+	const unsigned int		ch_id = cfg_ID / N_MIPI_FORMAT_CUSTOM;
+	hrt_data			val;
+	hrt_address			addr = 0;
+	hrt_data			reg;
+
+	assert(ID < N_RX_ID);
+	assert(cfg_ID < N_MIPI_COMPRESSOR_CONTEXT);
+	assert(field_id < N_MIPI_FORMAT_CUSTOM);
+	assert(ch_id < N_RX_CHANNEL_ID);
+	assert(comp < N_MIPI_COMPRESSOR_METHODS);
+	assert(pred < N_MIPI_PREDICTOR_TYPES);
+
+	val = (((uint8_t)pred) << 3) | comp;
+
+	switch (ch_id) {
+	case 0:
+		addr = ((field_id < 6) ? _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG0_IDX :
+			_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG1_IDX);
+		break;
+	case 1:
+		addr = ((field_id < 6) ? _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG0_IDX :
+			_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG1_IDX);
+		break;
+	case 2:
+		addr = ((field_id < 6) ? _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG0_IDX :
+			_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG1_IDX);
+		break;
+	case 3:
+		addr = ((field_id < 6) ? _HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG0_IDX :
+			_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG1_IDX);
+		break;
+	default:
+		/* should not happen */
+		assert(false);
+		return;
+	}
+
+	reg = ((field_id < 6) ? (val << (field_id * 5)) : (val << ((
+		    field_id - 6) * 5)));
+	receiver_reg_store(ID, addr, reg);
+}
+
+void receiver_port_enable(
+    const rx_ID_t			ID,
+    const enum mipi_port_id		port_ID,
+    const bool			cnd)
+{
+	hrt_data	reg = receiver_port_reg_load(ID, port_ID,
+			  _HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX);
+
+	if (cnd) {
+		reg |= 0x01;
+	} else {
+		reg &= ~0x01;
+	}
+
+	receiver_port_reg_store(ID, port_ID,
+				_HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX, reg);
+}
+
+bool is_receiver_port_enabled(
+    const rx_ID_t			ID,
+    const enum mipi_port_id		port_ID)
+{
+	hrt_data	reg = receiver_port_reg_load(ID, port_ID,
+			  _HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX);
+	return ((reg & 0x01) != 0);
+}
+
+void receiver_irq_enable(
+    const rx_ID_t			ID,
+    const enum mipi_port_id		port_ID,
+    const rx_irq_info_t		irq_info)
+{
+	receiver_port_reg_store(ID,
+				port_ID, _HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX, irq_info);
+}
+
+rx_irq_info_t receiver_get_irq_info(
+    const rx_ID_t			ID,
+    const enum mipi_port_id		port_ID)
+{
+	return receiver_port_reg_load(ID,
+				      port_ID, _HRT_CSS_RECEIVER_IRQ_STATUS_REG_IDX);
+}
+
+void receiver_irq_clear(
+    const rx_ID_t			ID,
+    const enum mipi_port_id		port_ID,
+    const rx_irq_info_t		irq_info)
+{
+	receiver_port_reg_store(ID,
+				port_ID, _HRT_CSS_RECEIVER_IRQ_STATUS_REG_IDX, irq_info);
+}
+
+static inline void capture_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    capture_unit_state_t			*state)
+{
+	assert(/*(sub_id >= CAPTURE_UNIT0_ID) &&*/ (sub_id <= CAPTURE_UNIT2_ID));
+	assert(state);
+
+	state->StartMode = input_system_sub_system_reg_load(ID,
+			   sub_id,
+			   CAPT_START_MODE_REG_ID);
+	state->Start_Addr = input_system_sub_system_reg_load(ID,
+			    sub_id,
+			    CAPT_START_ADDR_REG_ID);
+	state->Mem_Region_Size = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 CAPT_MEM_REGION_SIZE_REG_ID);
+	state->Num_Mem_Regions = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 CAPT_NUM_MEM_REGIONS_REG_ID);
+//	AM: Illegal read from following registers.
+	/*	state->Init = input_system_sub_system_reg_load(ID,
+			sub_id,
+			CAPT_INIT_REG_ID);
+		state->Start = input_system_sub_system_reg_load(ID,
+			sub_id,
+			CAPT_START_REG_ID);
+		state->Stop = input_system_sub_system_reg_load(ID,
+			sub_id,
+			CAPT_STOP_REG_ID);
+	*/
+	state->Packet_Length = input_system_sub_system_reg_load(ID,
+			       sub_id,
+			       CAPT_PACKET_LENGTH_REG_ID);
+	state->Received_Length = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 CAPT_RECEIVED_LENGTH_REG_ID);
+	state->Received_Short_Packets = input_system_sub_system_reg_load(ID,
+					sub_id,
+					CAPT_RECEIVED_SHORT_PACKETS_REG_ID);
+	state->Received_Long_Packets = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       CAPT_RECEIVED_LONG_PACKETS_REG_ID);
+	state->Last_Command = input_system_sub_system_reg_load(ID,
+			      sub_id,
+			      CAPT_LAST_COMMAND_REG_ID);
+	state->Next_Command = input_system_sub_system_reg_load(ID,
+			      sub_id,
+			      CAPT_NEXT_COMMAND_REG_ID);
+	state->Last_Acknowledge = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  CAPT_LAST_ACKNOWLEDGE_REG_ID);
+	state->Next_Acknowledge = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  CAPT_NEXT_ACKNOWLEDGE_REG_ID);
+	state->FSM_State_Info = input_system_sub_system_reg_load(ID,
+				sub_id,
+				CAPT_FSM_STATE_INFO_REG_ID);
+}
+
+static inline void acquisition_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    acquisition_unit_state_t		*state)
+{
+	assert(sub_id == ACQUISITION_UNIT0_ID);
+	assert(state);
+
+	state->Start_Addr = input_system_sub_system_reg_load(ID,
+			    sub_id,
+			    ACQ_START_ADDR_REG_ID);
+	state->Mem_Region_Size = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 ACQ_MEM_REGION_SIZE_REG_ID);
+	state->Num_Mem_Regions = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 ACQ_NUM_MEM_REGIONS_REG_ID);
+//	AM: Illegal read from following registers.
+	/*	state->Init = input_system_sub_system_reg_load(ID,
+			sub_id,
+			ACQ_INIT_REG_ID);
+	*/
+	state->Received_Short_Packets = input_system_sub_system_reg_load(ID,
+					sub_id,
+					ACQ_RECEIVED_SHORT_PACKETS_REG_ID);
+	state->Received_Long_Packets = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ACQ_RECEIVED_LONG_PACKETS_REG_ID);
+	state->Last_Command = input_system_sub_system_reg_load(ID,
+			      sub_id,
+			      ACQ_LAST_COMMAND_REG_ID);
+	state->Next_Command = input_system_sub_system_reg_load(ID,
+			      sub_id,
+			      ACQ_NEXT_COMMAND_REG_ID);
+	state->Last_Acknowledge = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  ACQ_LAST_ACKNOWLEDGE_REG_ID);
+	state->Next_Acknowledge = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  ACQ_NEXT_ACKNOWLEDGE_REG_ID);
+	state->FSM_State_Info = input_system_sub_system_reg_load(ID,
+				sub_id,
+				ACQ_FSM_STATE_INFO_REG_ID);
+	state->Int_Cntr_Info = input_system_sub_system_reg_load(ID,
+			       sub_id,
+			       ACQ_INT_CNTR_INFO_REG_ID);
+}
+
+static inline void ctrl_unit_get_state(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    ctrl_unit_state_t			*state)
+{
+	assert(sub_id == CTRL_UNIT0_ID);
+	assert(state);
+
+	state->captA_start_addr = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  ISYS_CTRL_CAPT_START_ADDR_A_REG_ID);
+	state->captB_start_addr = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  ISYS_CTRL_CAPT_START_ADDR_B_REG_ID);
+	state->captC_start_addr = input_system_sub_system_reg_load(ID,
+				  sub_id,
+				  ISYS_CTRL_CAPT_START_ADDR_C_REG_ID);
+	state->captA_mem_region_size = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_MEM_REGION_SIZE_A_REG_ID);
+	state->captB_mem_region_size = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_MEM_REGION_SIZE_B_REG_ID);
+	state->captC_mem_region_size = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_MEM_REGION_SIZE_C_REG_ID);
+	state->captA_num_mem_regions = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_NUM_MEM_REGIONS_A_REG_ID);
+	state->captB_num_mem_regions = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_NUM_MEM_REGIONS_B_REG_ID);
+	state->captC_num_mem_regions = input_system_sub_system_reg_load(ID,
+				       sub_id,
+				       ISYS_CTRL_CAPT_NUM_MEM_REGIONS_C_REG_ID);
+	state->acq_start_addr = input_system_sub_system_reg_load(ID,
+				sub_id,
+				ISYS_CTRL_ACQ_START_ADDR_REG_ID);
+	state->acq_mem_region_size = input_system_sub_system_reg_load(ID,
+				     sub_id,
+				     ISYS_CTRL_ACQ_MEM_REGION_SIZE_REG_ID);
+	state->acq_num_mem_regions = input_system_sub_system_reg_load(ID,
+				     sub_id,
+				     ISYS_CTRL_ACQ_NUM_MEM_REGIONS_REG_ID);
+//	AM: Illegal read from following registers.
+	/*	state->ctrl_init = input_system_sub_system_reg_load(ID,
+			sub_id,
+			ISYS_CTRL_INIT_REG_ID);
+	*/
+	state->last_cmd = input_system_sub_system_reg_load(ID,
+			  sub_id,
+			  ISYS_CTRL_LAST_COMMAND_REG_ID);
+	state->next_cmd = input_system_sub_system_reg_load(ID,
+			  sub_id,
+			  ISYS_CTRL_NEXT_COMMAND_REG_ID);
+	state->last_ack = input_system_sub_system_reg_load(ID,
+			  sub_id,
+			  ISYS_CTRL_LAST_ACKNOWLEDGE_REG_ID);
+	state->next_ack = input_system_sub_system_reg_load(ID,
+			  sub_id,
+			  ISYS_CTRL_NEXT_ACKNOWLEDGE_REG_ID);
+	state->top_fsm_state = input_system_sub_system_reg_load(ID,
+			       sub_id,
+			       ISYS_CTRL_FSM_STATE_INFO_REG_ID);
+	state->captA_fsm_state = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 ISYS_CTRL_CAPT_A_FSM_STATE_INFO_REG_ID);
+	state->captB_fsm_state = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 ISYS_CTRL_CAPT_B_FSM_STATE_INFO_REG_ID);
+	state->captC_fsm_state = input_system_sub_system_reg_load(ID,
+				 sub_id,
+				 ISYS_CTRL_CAPT_C_FSM_STATE_INFO_REG_ID);
+	state->acq_fsm_state = input_system_sub_system_reg_load(ID,
+			       sub_id,
+			       ISYS_CTRL_ACQ_FSM_STATE_INFO_REG_ID);
+	state->capt_reserve_one_mem_region = input_system_sub_system_reg_load(ID,
+					     sub_id,
+					     ISYS_CTRL_CAPT_RESERVE_ONE_MEM_REGION_REG_ID);
+}
+
+static inline void mipi_port_get_state(
+    const rx_ID_t				ID,
+    const enum mipi_port_id			port_ID,
+    mipi_port_state_t			*state)
+{
+	int	i;
+
+	assert(ID < N_RX_ID);
+	assert(port_ID < N_MIPI_PORT_ID);
+	assert(state);
+
+	state->device_ready = receiver_port_reg_load(ID,
+			      port_ID, _HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX);
+	state->irq_status = receiver_port_reg_load(ID,
+			    port_ID, _HRT_CSS_RECEIVER_IRQ_STATUS_REG_IDX);
+	state->irq_enable = receiver_port_reg_load(ID,
+			    port_ID, _HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX);
+	state->timeout_count = receiver_port_reg_load(ID,
+			       port_ID, _HRT_CSS_RECEIVER_TIMEOUT_COUNT_REG_IDX);
+	state->init_count = (uint16_t)receiver_port_reg_load(ID,
+			    port_ID, _HRT_CSS_RECEIVER_INIT_COUNT_REG_IDX);
+	state->raw16_18 = (uint16_t)receiver_port_reg_load(ID,
+			  port_ID, _HRT_CSS_RECEIVER_RAW16_18_DATAID_REG_IDX);
+	state->sync_count = receiver_port_reg_load(ID,
+			    port_ID, _HRT_CSS_RECEIVER_SYNC_COUNT_REG_IDX);
+	state->rx_count = receiver_port_reg_load(ID,
+			  port_ID, _HRT_CSS_RECEIVER_RX_COUNT_REG_IDX);
+
+	for (i = 0; i < MIPI_4LANE_CFG ; i++) {
+		state->lane_sync_count[i] = (uint8_t)((state->sync_count) >> (i * 8));
+		state->lane_rx_count[i] = (uint8_t)((state->rx_count) >> (i * 8));
+	}
+}
+
+static inline void rx_channel_get_state(
+    const rx_ID_t					ID,
+    const unsigned int				ch_id,
+    rx_channel_state_t				*state)
+{
+	int	i;
+
+	assert(ID < N_RX_ID);
+	assert(ch_id < N_RX_CHANNEL_ID);
+	assert(state);
+
+	switch (ch_id) {
+	case 0:
+		state->comp_scheme0 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG0_IDX);
+		state->comp_scheme1 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG1_IDX);
+		break;
+	case 1:
+		state->comp_scheme0 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG0_IDX);
+		state->comp_scheme1 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG1_IDX);
+		break;
+	case 2:
+		state->comp_scheme0 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG0_IDX);
+		state->comp_scheme1 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG1_IDX);
+		break;
+	case 3:
+		state->comp_scheme0 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG0_IDX);
+		state->comp_scheme1 = receiver_reg_load(ID,
+							_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG1_IDX);
+		break;
+	}
+
+	/* See Table 7.1.17,..., 7.1.24 */
+	for (i = 0; i < 6; i++) {
+		u8	val = (uint8_t)((state->comp_scheme0) >> (i * 5)) & 0x1f;
+
+		state->comp[i] = (mipi_compressor_t)(val & 0x07);
+		state->pred[i] = (mipi_predictor_t)((val & 0x18) >> 3);
+	}
+	for (i = 6; i < N_MIPI_FORMAT_CUSTOM; i++) {
+		u8	val = (uint8_t)((state->comp_scheme0) >> ((i - 6) * 5)) & 0x1f;
+
+		state->comp[i] = (mipi_compressor_t)(val & 0x07);
+		state->pred[i] = (mipi_predictor_t)((val & 0x18) >> 3);
+	}
+}
+
+// MW: "2400" in the name is not good, but this is to avoid a naming conflict
+static input_system_cfg2400_t config;
+
+static void receiver_rst(
+    const rx_ID_t				ID)
+{
+	enum mipi_port_id		port_id;
+
+	assert(ID < N_RX_ID);
+
+// Disable all ports.
+	for (port_id = MIPI_PORT0_ID; port_id < N_MIPI_PORT_ID; port_id++) {
+		receiver_port_enable(ID, port_id, false);
+	}
+
+	// AM: Additional actions for stopping receiver?
+}
+
+//Single function to reset all the devices mapped via GP_DEVICE.
+static void gp_device_rst(const gp_device_ID_t		ID)
+{
+	assert(ID < N_GP_DEVICE_ID);
+
+	gp_device_reg_store(ID, _REG_GP_SYNCGEN_ENABLE_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNCGEN_FREE_RUNNING_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNCGEN_PAUSE_ADDR, ONE);
+	// gp_device_reg_store(ID, _REG_GP_NR_FRAMES_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNGEN_NR_PIX_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNGEN_NR_PIX_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNGEN_NR_LINES_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNGEN_HBLANK_CYCLES_ADDR, ZERO);
+	// gp_device_reg_store(ID, _REG_GP_SYNGEN_VBLANK_CYCLES_ADDR, ZERO);
+// AM: Following calls cause strange warnings. Probably they should not be initialized.
+//	gp_device_reg_store(ID, _REG_GP_ISEL_SOF_ADDR, ZERO);
+//	gp_device_reg_store(ID, _REG_GP_ISEL_EOF_ADDR, ZERO);
+//	gp_device_reg_store(ID, _REG_GP_ISEL_SOL_ADDR, ZERO);
+//	gp_device_reg_store(ID, _REG_GP_ISEL_EOL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_LFSR_ENABLE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_LFSR_ENABLE_B_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_LFSR_RESET_VALUE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_ENABLE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_ENABLE_B_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_HOR_CNT_MASK_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_VER_CNT_MASK_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_XY_CNT_MASK_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_HOR_CNT_DELTA_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_VER_CNT_DELTA_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_MODE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_RED1_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_GREEN1_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_BLUE1_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_RED2_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_GREEN2_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_TPG_BLUE2_ADDR, ZERO);
+	//gp_device_reg_store(ID, _REG_GP_ISEL_CH_ID_ADDR, ZERO);
+	//gp_device_reg_store(ID, _REG_GP_ISEL_FMT_TYPE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_DATA_SEL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_SBAND_SEL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_SYNC_SEL_ADDR, ZERO);
+	//	gp_device_reg_store(ID, _REG_GP_SYNCGEN_HOR_CNT_ADDR, ZERO);
+	//	gp_device_reg_store(ID, _REG_GP_SYNCGEN_VER_CNT_ADDR, ZERO);
+	//	gp_device_reg_store(ID, _REG_GP_SYNCGEN_FRAME_CNT_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_SOFT_RESET_ADDR,
+			    ZERO); // AM: Maybe this soft reset is not safe.
+}
+
+static void input_selector_cfg_for_sensor(const gp_device_ID_t ID)
+{
+	assert(ID < N_GP_DEVICE_ID);
+
+	gp_device_reg_store(ID, _REG_GP_ISEL_SOF_ADDR, ONE);
+	gp_device_reg_store(ID, _REG_GP_ISEL_EOF_ADDR, ONE);
+	gp_device_reg_store(ID, _REG_GP_ISEL_SOL_ADDR, ONE);
+	gp_device_reg_store(ID, _REG_GP_ISEL_EOL_ADDR, ONE);
+	gp_device_reg_store(ID, _REG_GP_ISEL_CH_ID_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_FMT_TYPE_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_DATA_SEL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_SBAND_SEL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_ISEL_SYNC_SEL_ADDR, ZERO);
+	gp_device_reg_store(ID, _REG_GP_SOFT_RESET_ADDR, ZERO);
+}
+
+static void input_switch_rst(const gp_device_ID_t ID)
+{
+	int addr;
+
+	assert(ID < N_GP_DEVICE_ID);
+
+	// Initialize the data&hsync LUT.
+	for (addr = _REG_GP_IFMT_input_switch_lut_reg0;
+	     addr <= _REG_GP_IFMT_input_switch_lut_reg7; addr += SIZEOF_HRT_REG) {
+		gp_device_reg_store(ID, addr, ZERO);
+	}
+
+	// Initialize the vsync LUT.
+	gp_device_reg_store(ID,
+			    _REG_GP_IFMT_input_switch_fsync_lut,
+			    ZERO);
+}
+
+static void input_switch_cfg(
+    const gp_device_ID_t			ID,
+    const input_switch_cfg_t *const cfg)
+{
+	int addr_offset;
+
+	assert(ID < N_GP_DEVICE_ID);
+	assert(cfg);
+
+	// Initialize the data&hsync LUT.
+	for (addr_offset = 0; addr_offset < N_RX_CHANNEL_ID * 2; addr_offset++) {
+		assert(addr_offset * SIZEOF_HRT_REG + _REG_GP_IFMT_input_switch_lut_reg0 <=
+		       _REG_GP_IFMT_input_switch_lut_reg7);
+		gp_device_reg_store(ID,
+				    _REG_GP_IFMT_input_switch_lut_reg0 + addr_offset * SIZEOF_HRT_REG,
+				    cfg->hsync_data_reg[addr_offset]);
+	}
+
+	// Initialize the vsync LUT.
+	gp_device_reg_store(ID,
+			    _REG_GP_IFMT_input_switch_fsync_lut,
+			    cfg->vsync_data_reg);
+}
+
+static void input_system_network_rst(const input_system_ID_t ID)
+{
+	unsigned int sub_id;
+
+	// Reset all 3 multicasts.
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_A_IDX,
+					  INPUT_SYSTEM_DISCARD_ALL);
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_B_IDX,
+					  INPUT_SYSTEM_DISCARD_ALL);
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_C_IDX,
+					  INPUT_SYSTEM_DISCARD_ALL);
+
+	// Reset stream mux.
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MUX_IDX,
+					  N_INPUT_SYSTEM_MULTIPLEX);
+
+	// Reset 3 capture units.
+	for (sub_id = CAPTURE_UNIT0_ID; sub_id < CAPTURE_UNIT0_ID + N_CAPTURE_UNIT_ID;
+	     sub_id++) {
+		input_system_sub_system_reg_store(ID,
+						  sub_id,
+						  CAPT_INIT_REG_ID,
+						  1U << CAPT_INIT_RST_REG_BIT);
+	}
+
+	// Reset acquisition unit.
+	for (sub_id = ACQUISITION_UNIT0_ID;
+	     sub_id < ACQUISITION_UNIT0_ID + N_ACQUISITION_UNIT_ID; sub_id++) {
+		input_system_sub_system_reg_store(ID,
+						  sub_id,
+						  ACQ_INIT_REG_ID,
+						  1U << ACQ_INIT_RST_REG_BIT);
+	}
+
+	// DMA unit reset is not needed.
+
+	// Reset controller units.
+	// NB: In future we need to keep part of ctrl_state for split capture and
+	for (sub_id = CTRL_UNIT0_ID; sub_id < CTRL_UNIT0_ID + N_CTRL_UNIT_ID;
+	     sub_id++) {
+		input_system_sub_system_reg_store(ID,
+						  sub_id,
+						  ISYS_CTRL_INIT_REG_ID,
+						  1U); //AM: Is there any named constant?
+	}
+}
+
+// Function that resets current configuration.
+input_system_err_t input_system_configuration_reset(void)
+{
+	unsigned int i;
+
+	receiver_rst(RX0_ID);
+
+	input_system_network_rst(INPUT_SYSTEM0_ID);
+
+	gp_device_rst(GP_DEVICE0_ID);
+
+	input_switch_rst(GP_DEVICE0_ID);
+
+	//target_rst();
+
+	// Reset IRQ_CTRLs.
+
+	// Reset configuration data structures.
+	for (i = 0; i < N_CHANNELS; i++) {
+		config.ch_flags[i] = INPUT_SYSTEM_CFG_FLAG_RESET;
+		config.target_isp_flags[i] = INPUT_SYSTEM_CFG_FLAG_RESET;
+		config.target_sp_flags[i] = INPUT_SYSTEM_CFG_FLAG_RESET;
+		config.target_strm2mem_flags[i] = INPUT_SYSTEM_CFG_FLAG_RESET;
+	}
+
+	for (i = 0; i < N_CSI_PORTS; i++) {
+		config.csi_buffer_flags[i]	 = INPUT_SYSTEM_CFG_FLAG_RESET;
+		config.multicast[i]		 = INPUT_SYSTEM_DISCARD_ALL;
+	}
+
+	config.source_type_flags				 = INPUT_SYSTEM_CFG_FLAG_RESET;
+	config.acquisition_buffer_unique_flags	 = INPUT_SYSTEM_CFG_FLAG_RESET;
+	config.unallocated_ib_mem_words			 = IB_CAPACITY_IN_WORDS;
+	//config.acq_allocated_ib_mem_words		 = 0;
+
+	// Set the start of the session cofiguration.
+	config.session_flags = INPUT_SYSTEM_CFG_FLAG_REQUIRED;
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// MW: Comments are good, but doxygen is required, place it at the declaration
+// Function that appends the channel to current configuration.
+static input_system_err_t input_system_configure_channel(
+    const channel_cfg_t		channel)
+{
+	input_system_err_t error = INPUT_SYSTEM_ERR_NO_ERROR;
+	// Check if channel is not already configured.
+	if (config.ch_flags[channel.ch_id] & INPUT_SYSTEM_CFG_FLAG_SET) {
+		return INPUT_SYSTEM_ERR_CHANNEL_ALREADY_SET;
+	} else {
+		switch (channel.source_type) {
+		case INPUT_SYSTEM_SOURCE_SENSOR:
+			error = input_system_configure_channel_sensor(channel);
+			break;
+		case INPUT_SYSTEM_SOURCE_TPG:
+		case INPUT_SYSTEM_SOURCE_PRBS:
+		case INPUT_SYSTEM_SOURCE_FIFO:
+		default:
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+		}
+
+		if (error != INPUT_SYSTEM_ERR_NO_ERROR) return error;
+		// Input switch channel configurations must be combined in united config.
+		config.input_switch_cfg.hsync_data_reg[channel.source_cfg.csi_cfg.csi_port * 2]
+		    =
+			channel.target_cfg.input_switch_channel_cfg.hsync_data_reg[0];
+		config.input_switch_cfg.hsync_data_reg[channel.source_cfg.csi_cfg.csi_port * 2 +
+											   1] =
+							       channel.target_cfg.input_switch_channel_cfg.hsync_data_reg[1];
+		config.input_switch_cfg.vsync_data_reg |=
+		    (channel.target_cfg.input_switch_channel_cfg.vsync_data_reg & 0x7) <<
+		    (channel.source_cfg.csi_cfg.csi_port * 3);
+
+		// Other targets are just copied and marked as set.
+		config.target_isp[channel.source_cfg.csi_cfg.csi_port] =
+		    channel.target_cfg.target_isp_cfg;
+		config.target_sp[channel.source_cfg.csi_cfg.csi_port] =
+		    channel.target_cfg.target_sp_cfg;
+		config.target_strm2mem[channel.source_cfg.csi_cfg.csi_port] =
+		    channel.target_cfg.target_strm2mem_cfg;
+		config.target_isp_flags[channel.source_cfg.csi_cfg.csi_port] |=
+		    INPUT_SYSTEM_CFG_FLAG_SET;
+		config.target_sp_flags[channel.source_cfg.csi_cfg.csi_port] |=
+		    INPUT_SYSTEM_CFG_FLAG_SET;
+		config.target_strm2mem_flags[channel.source_cfg.csi_cfg.csi_port] |=
+		    INPUT_SYSTEM_CFG_FLAG_SET;
+
+		config.ch_flags[channel.ch_id] = INPUT_SYSTEM_CFG_FLAG_SET;
+	}
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Function that partitions input buffer space with determining addresses.
+static input_system_err_t input_buffer_configuration(void)
+{
+	u32 current_address    = 0;
+	u32 unallocated_memory = IB_CAPACITY_IN_WORDS;
+
+	isp2400_ib_buffer_t	candidate_buffer_acq  = IB_BUFFER_NULL;
+	u32 size_requested;
+	input_system_config_flags_t	acq_already_specified = INPUT_SYSTEM_CFG_FLAG_RESET;
+	input_system_csi_port_t port;
+
+	for (port = INPUT_SYSTEM_PORT_A; port < N_INPUT_SYSTEM_PORTS; port++) {
+		csi_cfg_t source = config.csi_value[port];//.csi_cfg;
+
+		if (config.csi_flags[port] & INPUT_SYSTEM_CFG_FLAG_SET) {
+			// Check and set csi buffer in input buffer.
+			switch (source.buffering_mode) {
+			case INPUT_SYSTEM_FIFO_CAPTURE:
+			case INPUT_SYSTEM_XMEM_ACQUIRE:
+				config.csi_buffer_flags[port] =
+				    INPUT_SYSTEM_CFG_FLAG_BLOCKED; // Well, not used.
+				break;
+
+			case INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING:
+			case INPUT_SYSTEM_SRAM_BUFFERING:
+			case INPUT_SYSTEM_XMEM_BUFFERING:
+			case INPUT_SYSTEM_XMEM_CAPTURE:
+				size_requested = source.csi_buffer.mem_reg_size *
+						 source.csi_buffer.nof_mem_regs;
+				if (source.csi_buffer.mem_reg_size > 0
+				    && source.csi_buffer.nof_mem_regs > 0
+				    && size_requested <= unallocated_memory
+				   ) {
+					config.csi_buffer[port].mem_reg_addr = current_address;
+					config.csi_buffer[port].mem_reg_size = source.csi_buffer.mem_reg_size;
+					config.csi_buffer[port].nof_mem_regs = source.csi_buffer.nof_mem_regs;
+					current_address		+= size_requested;
+					unallocated_memory	-= size_requested;
+					config.csi_buffer_flags[port] = INPUT_SYSTEM_CFG_FLAG_SET;
+				} else {
+					config.csi_buffer_flags[port] |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+					return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+				}
+				break;
+
+			default:
+				config.csi_buffer_flags[port] |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			}
+
+			// Check acquisition buffer specified but set it later since it has to be unique.
+			switch (source.buffering_mode) {
+			case INPUT_SYSTEM_FIFO_CAPTURE:
+			case INPUT_SYSTEM_SRAM_BUFFERING:
+			case INPUT_SYSTEM_XMEM_CAPTURE:
+				// Nothing to do.
+				break;
+
+			case INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING:
+			case INPUT_SYSTEM_XMEM_BUFFERING:
+			case INPUT_SYSTEM_XMEM_ACQUIRE:
+				if (acq_already_specified == INPUT_SYSTEM_CFG_FLAG_RESET) {
+					size_requested = source.acquisition_buffer.mem_reg_size
+							 * source.acquisition_buffer.nof_mem_regs;
+					if (source.acquisition_buffer.mem_reg_size > 0
+					    && source.acquisition_buffer.nof_mem_regs > 0
+					    && size_requested <= unallocated_memory
+					   ) {
+						candidate_buffer_acq = source.acquisition_buffer;
+						acq_already_specified = INPUT_SYSTEM_CFG_FLAG_SET;
+					}
+				} else {
+					// Check if specified acquisition buffer is the same as specified before.
+					if (source.acquisition_buffer.mem_reg_size != candidate_buffer_acq.mem_reg_size
+					    || source.acquisition_buffer.nof_mem_regs !=  candidate_buffer_acq.nof_mem_regs
+					   ) {
+						config.acquisition_buffer_unique_flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+						return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+					}
+				}
+				break;
+
+			default:
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			}
+		} else {
+			config.csi_buffer_flags[port] = INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		}
+	} // end of for ( port )
+
+	// Set the acquisition buffer at the end.
+	size_requested = candidate_buffer_acq.mem_reg_size *
+			 candidate_buffer_acq.nof_mem_regs;
+	if (acq_already_specified == INPUT_SYSTEM_CFG_FLAG_SET
+	    && size_requested <= unallocated_memory) {
+		config.acquisition_buffer_unique.mem_reg_addr = current_address;
+		config.acquisition_buffer_unique.mem_reg_size =
+		    candidate_buffer_acq.mem_reg_size;
+		config.acquisition_buffer_unique.nof_mem_regs =
+		    candidate_buffer_acq.nof_mem_regs;
+		current_address		+= size_requested;
+		unallocated_memory	-= size_requested;
+		config.acquisition_buffer_unique_flags = INPUT_SYSTEM_CFG_FLAG_SET;
+
+		assert(current_address <= IB_CAPACITY_IN_WORDS);
+	}
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+static void capture_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const isp2400_ib_buffer_t *const cfg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(/*(sub_id >= CAPTURE_UNIT0_ID) &&*/ (sub_id <=
+		CAPTURE_UNIT2_ID)); // Commented part is always true.
+	assert(cfg);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  CAPT_START_ADDR_REG_ID,
+					  cfg->mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  CAPT_MEM_REGION_SIZE_REG_ID,
+					  cfg->mem_reg_size);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  CAPT_NUM_MEM_REGIONS_REG_ID,
+					  cfg->nof_mem_regs);
+}
+
+static void acquisition_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const isp2400_ib_buffer_t *const cfg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(sub_id == ACQUISITION_UNIT0_ID);
+	assert(cfg);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ACQ_START_ADDR_REG_ID,
+					  cfg->mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ACQ_NUM_MEM_REGIONS_REG_ID,
+					  cfg->nof_mem_regs);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ACQ_MEM_REGION_SIZE_REG_ID,
+					  cfg->mem_reg_size);
+}
+
+static void ctrl_unit_configure(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_id,
+    const ctrl_unit_cfg_t *const cfg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(sub_id == CTRL_UNIT0_ID);
+	assert(cfg);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_START_ADDR_A_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT0_ID].mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_MEM_REGION_SIZE_A_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT0_ID].mem_reg_size);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_NUM_MEM_REGIONS_A_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT0_ID].nof_mem_regs);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_START_ADDR_B_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT1_ID].mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_MEM_REGION_SIZE_B_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT1_ID].mem_reg_size);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_NUM_MEM_REGIONS_B_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT1_ID].nof_mem_regs);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_START_ADDR_C_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT2_ID].mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_MEM_REGION_SIZE_C_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT2_ID].mem_reg_size);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_NUM_MEM_REGIONS_C_REG_ID,
+					  cfg->buffer_mipi[CAPTURE_UNIT2_ID].nof_mem_regs);
+
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_ACQ_START_ADDR_REG_ID,
+					  cfg->buffer_acquire[ACQUISITION_UNIT0_ID - ACQUISITION_UNIT0_ID].mem_reg_addr);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_ACQ_MEM_REGION_SIZE_REG_ID,
+					  cfg->buffer_acquire[ACQUISITION_UNIT0_ID - ACQUISITION_UNIT0_ID].mem_reg_size);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_ACQ_NUM_MEM_REGIONS_REG_ID,
+					  cfg->buffer_acquire[ACQUISITION_UNIT0_ID - ACQUISITION_UNIT0_ID].nof_mem_regs);
+	input_system_sub_system_reg_store(ID,
+					  sub_id,
+					  ISYS_CTRL_CAPT_RESERVE_ONE_MEM_REGION_REG_ID,
+					  0);
+}
+
+static void input_system_network_configure(
+    const input_system_ID_t				ID,
+    const input_system_network_cfg_t *const cfg)
+{
+	u32 sub_id;
+
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(cfg);
+
+	// Set all 3 multicasts.
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_A_IDX,
+					  cfg->multicast_cfg[CAPTURE_UNIT0_ID]);
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_B_IDX,
+					  cfg->multicast_cfg[CAPTURE_UNIT1_ID]);
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MULTICAST_C_IDX,
+					  cfg->multicast_cfg[CAPTURE_UNIT2_ID]);
+
+	// Set stream mux.
+	input_system_sub_system_reg_store(ID,
+					  GPREGS_UNIT0_ID,
+					  HIVE_ISYS_GPREG_MUX_IDX,
+					  cfg->mux_cfg);
+
+	// Set capture units.
+	for (sub_id = CAPTURE_UNIT0_ID; sub_id < CAPTURE_UNIT0_ID + N_CAPTURE_UNIT_ID;
+	     sub_id++) {
+		capture_unit_configure(ID,
+				       sub_id,
+				       &cfg->ctrl_unit_cfg[ID].buffer_mipi[sub_id - CAPTURE_UNIT0_ID]);
+	}
+
+	// Set acquisition units.
+	for (sub_id = ACQUISITION_UNIT0_ID;
+	     sub_id < ACQUISITION_UNIT0_ID + N_ACQUISITION_UNIT_ID; sub_id++) {
+		acquisition_unit_configure(ID,
+					   sub_id,
+					   &cfg->ctrl_unit_cfg[sub_id - ACQUISITION_UNIT0_ID].buffer_acquire[sub_id -
+						   ACQUISITION_UNIT0_ID]);
+	}
+
+	// No DMA configuration needed. Ctrl_unit will fully control it.
+
+	// Set controller units.
+	for (sub_id = CTRL_UNIT0_ID; sub_id < CTRL_UNIT0_ID + N_CTRL_UNIT_ID;
+	     sub_id++) {
+		ctrl_unit_configure(ID,
+				    sub_id,
+				    &cfg->ctrl_unit_cfg[sub_id - CTRL_UNIT0_ID]);
+	}
+}
+
+static input_system_err_t configuration_to_registers(void)
+{
+	input_system_network_cfg_t input_system_network_cfg;
+	int i;
+
+	assert(config.source_type_flags & INPUT_SYSTEM_CFG_FLAG_SET);
+
+	switch (config.source_type) {
+	case INPUT_SYSTEM_SOURCE_SENSOR:
+
+		// Determine stream multicasts setting based on the mode of csi_cfg_t.
+		// AM: This should be moved towards earlier function call, e.g. in
+		// the commit function.
+		for (i = MIPI_PORT0_ID; i < N_MIPI_PORT_ID; i++) {
+			if (config.csi_flags[i] & INPUT_SYSTEM_CFG_FLAG_SET) {
+				switch (config.csi_value[i].buffering_mode) {
+				case INPUT_SYSTEM_FIFO_CAPTURE:
+					config.multicast[i] = INPUT_SYSTEM_CSI_BACKEND;
+					break;
+
+				case INPUT_SYSTEM_XMEM_CAPTURE:
+				case INPUT_SYSTEM_SRAM_BUFFERING:
+				case INPUT_SYSTEM_XMEM_BUFFERING:
+					config.multicast[i] = INPUT_SYSTEM_INPUT_BUFFER;
+					break;
+
+				case INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING:
+					config.multicast[i] = INPUT_SYSTEM_MULTICAST;
+					break;
+
+				case INPUT_SYSTEM_XMEM_ACQUIRE:
+					config.multicast[i] = INPUT_SYSTEM_DISCARD_ALL;
+					break;
+
+				default:
+					config.multicast[i] = INPUT_SYSTEM_DISCARD_ALL;
+					return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+					//break;
+				}
+			} else {
+				config.multicast[i] = INPUT_SYSTEM_DISCARD_ALL;
+			}
+
+			input_system_network_cfg.multicast_cfg[i] = config.multicast[i];
+
+		} // for
+
+		input_system_network_cfg.mux_cfg = config.multiplexer;
+
+		input_system_network_cfg.ctrl_unit_cfg[CTRL_UNIT0_ID -
+						       CTRL_UNIT0_ID].buffer_mipi[CAPTURE_UNIT0_ID] =
+							       config.csi_buffer[MIPI_PORT0_ID];
+		input_system_network_cfg.ctrl_unit_cfg[CTRL_UNIT0_ID -
+						       CTRL_UNIT0_ID].buffer_mipi[CAPTURE_UNIT1_ID] =
+							       config.csi_buffer[MIPI_PORT1_ID];
+		input_system_network_cfg.ctrl_unit_cfg[CTRL_UNIT0_ID -
+						       CTRL_UNIT0_ID].buffer_mipi[CAPTURE_UNIT2_ID] =
+							       config.csi_buffer[MIPI_PORT2_ID];
+		input_system_network_cfg.ctrl_unit_cfg[CTRL_UNIT0_ID -
+						       CTRL_UNIT0_ID].buffer_acquire[ACQUISITION_UNIT0_ID -
+							       ACQUISITION_UNIT0_ID] =
+								       config.acquisition_buffer_unique;
+
+		// First set input network around CSI receiver.
+		input_system_network_configure(INPUT_SYSTEM0_ID, &input_system_network_cfg);
+
+		// Set the CSI receiver.
+		//...
+		break;
+
+	case INPUT_SYSTEM_SOURCE_TPG:
+	case INPUT_SYSTEM_SOURCE_PRBS:
+	case INPUT_SYSTEM_SOURCE_FIFO:
+		break;
+
+	default:
+		return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+
+	} // end of switch (source_type)
+
+	// Set input selector.
+	input_selector_cfg_for_sensor(GP_DEVICE0_ID);
+
+	// Set input switch.
+	input_switch_cfg(GP_DEVICE0_ID, &config.input_switch_cfg);
+
+	// Set input formatters.
+	// AM: IF are set dynamically.
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Function that applies the whole configuration.
+input_system_err_t input_system_configuration_commit(void)
+{
+	// The last configuration step is to configure the input buffer.
+	input_system_err_t error = input_buffer_configuration();
+
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR) {
+		return error;
+	}
+
+	// Translate the whole configuration into registers.
+	error = configuration_to_registers();
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR) {
+		return error;
+	}
+
+	// Translate the whole configuration into ctrl commands etc.
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// FIFO
+
+input_system_err_t	input_system_csi_fifo_channel_cfg(
+    u32		ch_id,
+    input_system_csi_port_t	port,
+    backend_channel_cfg_t	backend_ch,
+    target_cfg2400_t	target
+)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	channel.backend_ch	= backend_ch;
+	channel.source_type = INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	channel.source_cfg.csi_cfg.buffering_mode	= INPUT_SYSTEM_FIFO_CAPTURE;
+	channel.source_cfg.csi_cfg.csi_buffer			= IB_BUFFER_NULL;
+	channel.source_cfg.csi_cfg.acquisition_buffer	= IB_BUFFER_NULL;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= 0;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+input_system_err_t	input_system_csi_fifo_channel_with_counting_cfg(
+    u32				ch_id,
+    u32				nof_frames,
+    input_system_csi_port_t			port,
+    backend_channel_cfg_t			backend_ch,
+    u32				csi_mem_reg_size,
+    u32				csi_nof_mem_regs,
+    target_cfg2400_t			target
+)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	channel.backend_ch	= backend_ch;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	channel.source_cfg.csi_cfg.buffering_mode	=
+	    INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_size		= csi_mem_reg_size;
+	channel.source_cfg.csi_cfg.csi_buffer.nof_mem_regs		= csi_nof_mem_regs;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_addr		= 0;
+	channel.source_cfg.csi_cfg.acquisition_buffer			= IB_BUFFER_NULL;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= nof_frames;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+// SRAM
+
+input_system_err_t	input_system_csi_sram_channel_cfg(
+    u32				ch_id,
+    input_system_csi_port_t			port,
+    backend_channel_cfg_t			backend_ch,
+    u32				csi_mem_reg_size,
+    u32				csi_nof_mem_regs,
+    //	uint32_t				acq_mem_reg_size,
+    //	uint32_t				acq_nof_mem_regs,
+    target_cfg2400_t			target
+)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	channel.backend_ch	= backend_ch;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	channel.source_cfg.csi_cfg.buffering_mode	= INPUT_SYSTEM_SRAM_BUFFERING;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_size		= csi_mem_reg_size;
+	channel.source_cfg.csi_cfg.csi_buffer.nof_mem_regs		= csi_nof_mem_regs;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_addr		= 0;
+	channel.source_cfg.csi_cfg.acquisition_buffer			= IB_BUFFER_NULL;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= 0;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+//XMEM
+
+// Collects all parameters and puts them in channel_cfg_t.
+input_system_err_t	input_system_csi_xmem_channel_cfg(
+    u32				ch_id,
+    input_system_csi_port_t			port,
+    backend_channel_cfg_t			backend_ch,
+    u32				csi_mem_reg_size,
+    u32				csi_nof_mem_regs,
+    u32				acq_mem_reg_size,
+    u32				acq_nof_mem_regs,
+    target_cfg2400_t			target,
+    uint32_t				nof_xmem_buffers
+)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	channel.backend_ch	= backend_ch;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	channel.source_cfg.csi_cfg.buffering_mode	= INPUT_SYSTEM_XMEM_BUFFERING;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_size		= csi_mem_reg_size;
+	channel.source_cfg.csi_cfg.csi_buffer.nof_mem_regs		= csi_nof_mem_regs;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_addr		= 0;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_size	= acq_mem_reg_size;
+	channel.source_cfg.csi_cfg.acquisition_buffer.nof_mem_regs	= acq_nof_mem_regs;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_addr	= 0;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= nof_xmem_buffers;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+input_system_err_t	input_system_csi_xmem_acquire_only_channel_cfg(
+    u32				ch_id,
+    u32				nof_frames,
+    input_system_csi_port_t			port,
+    backend_channel_cfg_t			backend_ch,
+    u32				acq_mem_reg_size,
+    u32				acq_nof_mem_regs,
+    target_cfg2400_t			target)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	channel.backend_ch	= backend_ch;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	channel.source_cfg.csi_cfg.buffering_mode	= INPUT_SYSTEM_XMEM_ACQUIRE;
+	channel.source_cfg.csi_cfg.csi_buffer		= IB_BUFFER_NULL;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_size	= acq_mem_reg_size;
+	channel.source_cfg.csi_cfg.acquisition_buffer.nof_mem_regs	= acq_nof_mem_regs;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_addr	= 0;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= nof_frames;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+input_system_err_t	input_system_csi_xmem_capture_only_channel_cfg(
+    u32				ch_id,
+    u32				nof_frames,
+    input_system_csi_port_t			port,
+    u32				csi_mem_reg_size,
+    u32				csi_nof_mem_regs,
+    u32				acq_mem_reg_size,
+    u32				acq_nof_mem_regs,
+    target_cfg2400_t			target)
+{
+	channel_cfg_t channel;
+
+	channel.ch_id	= ch_id;
+	//channel.backend_ch	= backend_ch;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_SENSOR;
+	//channel.source
+	channel.source_cfg.csi_cfg.csi_port			= port;
+	//channel.source_cfg.csi_cfg.backend_ch		= backend_ch;
+	channel.source_cfg.csi_cfg.buffering_mode	= INPUT_SYSTEM_XMEM_CAPTURE;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_size		= csi_mem_reg_size;
+	channel.source_cfg.csi_cfg.csi_buffer.nof_mem_regs		= csi_nof_mem_regs;
+	channel.source_cfg.csi_cfg.csi_buffer.mem_reg_addr		= 0;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_size	= acq_mem_reg_size;
+	channel.source_cfg.csi_cfg.acquisition_buffer.nof_mem_regs	= acq_nof_mem_regs;
+	channel.source_cfg.csi_cfg.acquisition_buffer.mem_reg_addr	= 0;
+	channel.source_cfg.csi_cfg.nof_xmem_buffers	= nof_frames;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+// Non - CSI
+
+input_system_err_t	input_system_prbs_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames,//not used yet
+    u32		seed,
+    u32		sync_gen_width,
+    u32		sync_gen_height,
+    u32		sync_gen_hblank_cycles,
+    u32		sync_gen_vblank_cycles,
+    target_cfg2400_t	target
+)
+{
+	channel_cfg_t channel;
+
+	(void)nof_frames;
+
+	channel.ch_id	= ch_id;
+	channel.source_type = INPUT_SYSTEM_SOURCE_PRBS;
+
+	channel.source_cfg.prbs_cfg.seed = seed;
+	channel.source_cfg.prbs_cfg.sync_gen_cfg.width		= sync_gen_width;
+	channel.source_cfg.prbs_cfg.sync_gen_cfg.height		= sync_gen_height;
+	channel.source_cfg.prbs_cfg.sync_gen_cfg.hblank_cycles	= sync_gen_hblank_cycles;
+	channel.source_cfg.prbs_cfg.sync_gen_cfg.vblank_cycles	= sync_gen_vblank_cycles;
+
+	channel.target_cfg	= target;
+
+	return input_system_configure_channel(channel);
+}
+
+input_system_err_t	input_system_tpg_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames,//not used yet
+    u32		x_mask,
+    u32		y_mask,
+    u32		x_delta,
+    u32		y_delta,
+    u32		xy_mask,
+    u32		sync_gen_width,
+    u32		sync_gen_height,
+    u32		sync_gen_hblank_cycles,
+    u32		sync_gen_vblank_cycles,
+    target_cfg2400_t	target
+)
+{
+	channel_cfg_t channel;
+
+	(void)nof_frames;
+
+	channel.ch_id	= ch_id;
+	channel.source_type		= INPUT_SYSTEM_SOURCE_TPG;
+
+	channel.source_cfg.tpg_cfg.x_mask	= x_mask;
+	channel.source_cfg.tpg_cfg.y_mask	= y_mask;
+	channel.source_cfg.tpg_cfg.x_delta	= x_delta;
+	channel.source_cfg.tpg_cfg.y_delta	= y_delta;
+	channel.source_cfg.tpg_cfg.xy_mask	= xy_mask;
+	channel.source_cfg.tpg_cfg.sync_gen_cfg.width		= sync_gen_width;
+	channel.source_cfg.tpg_cfg.sync_gen_cfg.height		= sync_gen_height;
+	channel.source_cfg.tpg_cfg.sync_gen_cfg.hblank_cycles	= sync_gen_hblank_cycles;
+	channel.source_cfg.tpg_cfg.sync_gen_cfg.vblank_cycles	= sync_gen_vblank_cycles;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+// MW: Don't use system specific names, (even in system specific files) "cfg2400" -> cfg
+input_system_err_t	input_system_gpfifo_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames, //not used yet
+
+    target_cfg2400_t	target)
+{
+	channel_cfg_t channel;
+
+	(void)nof_frames;
+
+	channel.ch_id	= ch_id;
+	channel.source_type	= INPUT_SYSTEM_SOURCE_FIFO;
+
+	channel.target_cfg	= target;
+	return input_system_configure_channel(channel);
+}
+
+///////////////////////////////////////////////////////////////////////////
+//
+// Private specialized functions for channel setting.
+//
+///////////////////////////////////////////////////////////////////////////
+
+// Fills the parameters to config.csi_value[port]
+static input_system_err_t input_system_configure_channel_sensor(
+    const channel_cfg_t channel)
+{
+	const u32 port = channel.source_cfg.csi_cfg.csi_port;
+	input_system_err_t status = INPUT_SYSTEM_ERR_NO_ERROR;
+
+	input_system_multiplex_t mux;
+
+	if (port >= N_INPUT_SYSTEM_PORTS)
+		return INPUT_SYSTEM_ERR_GENERIC;
+
+	//check if port > N_INPUT_SYSTEM_MULTIPLEX
+
+	status = set_source_type(&config.source_type, channel.source_type,
+				 &config.source_type_flags);
+	if (status != INPUT_SYSTEM_ERR_NO_ERROR) return status;
+
+	// Check for conflicts on source (implicitly on multicast, capture unit and input buffer).
+
+	status = set_csi_cfg(&config.csi_value[port], &channel.source_cfg.csi_cfg,
+			     &config.csi_flags[port]);
+	if (status != INPUT_SYSTEM_ERR_NO_ERROR) return status;
+
+	switch (channel.source_cfg.csi_cfg.buffering_mode) {
+	case INPUT_SYSTEM_FIFO_CAPTURE:
+
+		// Check for conflicts on mux.
+		mux = INPUT_SYSTEM_MIPI_PORT0 + port;
+		status = input_system_multiplexer_cfg(&config.multiplexer, mux,
+						      &config.multiplexer_flags);
+		if (status != INPUT_SYSTEM_ERR_NO_ERROR) return status;
+		config.multicast[port] = INPUT_SYSTEM_CSI_BACKEND;
+
+		// Shared resource, so it should be blocked.
+		//config.mux_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.csi_buffer_flags[port] |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.acquisition_buffer_unique_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+
+		break;
+	case INPUT_SYSTEM_SRAM_BUFFERING:
+
+		// Check for conflicts on mux.
+		mux = INPUT_SYSTEM_ACQUISITION_UNIT;
+		status = input_system_multiplexer_cfg(&config.multiplexer, mux,
+						      &config.multiplexer_flags);
+		if (status != INPUT_SYSTEM_ERR_NO_ERROR) return status;
+		config.multicast[port] = INPUT_SYSTEM_INPUT_BUFFER;
+
+		// Shared resource, so it should be blocked.
+		//config.mux_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.csi_buffer_flags[port] |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.acquisition_buffer_unique_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+
+		break;
+	case INPUT_SYSTEM_XMEM_BUFFERING:
+
+		// Check for conflicts on mux.
+		mux = INPUT_SYSTEM_ACQUISITION_UNIT;
+		status = input_system_multiplexer_cfg(&config.multiplexer, mux,
+						      &config.multiplexer_flags);
+		if (status != INPUT_SYSTEM_ERR_NO_ERROR) return status;
+		config.multicast[port] = INPUT_SYSTEM_INPUT_BUFFER;
+
+		// Shared resource, so it should be blocked.
+		//config.mux_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.csi_buffer_flags[port] |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+		//config.acquisition_buffer_unique_flags |= INPUT_SYSTEM_CFG_FLAG_BLOCKED;
+
+		break;
+	case INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING:
+	case INPUT_SYSTEM_XMEM_CAPTURE:
+	case INPUT_SYSTEM_XMEM_ACQUIRE:
+	default:
+		return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+	}
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Test flags and set structure.
+static input_system_err_t set_source_type(
+    input_system_source_t *const lhs,
+    const input_system_source_t			rhs,
+    input_system_config_flags_t *const flags)
+{
+	// MW: Not enough asserts
+	assert(lhs);
+	assert(flags);
+
+	if ((*flags) & INPUT_SYSTEM_CFG_FLAG_BLOCKED) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+	}
+
+	if ((*flags) & INPUT_SYSTEM_CFG_FLAG_SET) {
+		// Check for consistency with already set value.
+		if ((*lhs) == (rhs)) {
+			return INPUT_SYSTEM_ERR_NO_ERROR;
+		} else {
+			*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+			return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+		}
+	}
+	// Check the value (individually).
+	if (rhs >= N_INPUT_SYSTEM_SOURCE) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+	}
+	// Set the value.
+	*lhs = rhs;
+
+	*flags |= INPUT_SYSTEM_CFG_FLAG_SET;
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Test flags and set structure.
+static input_system_err_t set_csi_cfg(
+    csi_cfg_t *const lhs,
+    const csi_cfg_t *const rhs,
+    input_system_config_flags_t *const flags)
+{
+	u32 memory_required;
+	u32 acq_memory_required;
+
+	assert(lhs);
+	assert(flags);
+
+	if ((*flags) & INPUT_SYSTEM_CFG_FLAG_BLOCKED) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+	}
+
+	if (*flags & INPUT_SYSTEM_CFG_FLAG_SET) {
+		// check for consistency with already set value.
+		if (/*lhs->backend_ch == rhs.backend_ch
+			&&*/ lhs->buffering_mode == rhs->buffering_mode
+		    && lhs->csi_buffer.mem_reg_size == rhs->csi_buffer.mem_reg_size
+		    && lhs->csi_buffer.nof_mem_regs  == rhs->csi_buffer.nof_mem_regs
+		    && lhs->acquisition_buffer.mem_reg_size == rhs->acquisition_buffer.mem_reg_size
+		    && lhs->acquisition_buffer.nof_mem_regs  == rhs->acquisition_buffer.nof_mem_regs
+		    && lhs->nof_xmem_buffers  == rhs->nof_xmem_buffers
+		) {
+			return INPUT_SYSTEM_ERR_NO_ERROR;
+		} else {
+			*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+			return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+		}
+	}
+	// Check the value (individually).
+	// no check for backend_ch
+	// no check for nof_xmem_buffers
+	memory_required = rhs->csi_buffer.mem_reg_size * rhs->csi_buffer.nof_mem_regs;
+	acq_memory_required = rhs->acquisition_buffer.mem_reg_size *
+			      rhs->acquisition_buffer.nof_mem_regs;
+	if (rhs->buffering_mode >= N_INPUT_SYSTEM_BUFFERING_MODE
+	    ||
+	    // Check if required memory is available in input buffer (SRAM).
+	    (memory_required + acq_memory_required) > config.unallocated_ib_mem_words
+
+	   ) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+	}
+	// Set the value.
+	//lhs[port]->backend_ch		= rhs.backend_ch;
+	lhs->buffering_mode	= rhs->buffering_mode;
+	lhs->nof_xmem_buffers = rhs->nof_xmem_buffers;
+
+	lhs->csi_buffer.mem_reg_size = rhs->csi_buffer.mem_reg_size;
+	lhs->csi_buffer.nof_mem_regs  = rhs->csi_buffer.nof_mem_regs;
+	lhs->acquisition_buffer.mem_reg_size = rhs->acquisition_buffer.mem_reg_size;
+	lhs->acquisition_buffer.nof_mem_regs  = rhs->acquisition_buffer.nof_mem_regs;
+	// ALX: NB: Here we just set buffer parameters, but still not allocate it
+	// (no addresses determined). That will be done during commit.
+
+	//  FIXIT:	acq_memory_required is not deducted, since it can be allocated multiple times.
+	config.unallocated_ib_mem_words -= memory_required;
+//assert(config.unallocated_ib_mem_words >=0);
+	*flags |= INPUT_SYSTEM_CFG_FLAG_SET;
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Test flags and set structure.
+static input_system_err_t input_system_multiplexer_cfg(
+    input_system_multiplex_t *const lhs,
+    const input_system_multiplex_t		rhs,
+    input_system_config_flags_t *const flags)
+{
+	assert(lhs);
+	assert(flags);
+
+	if ((*flags) & INPUT_SYSTEM_CFG_FLAG_BLOCKED) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+	}
+
+	if ((*flags) & INPUT_SYSTEM_CFG_FLAG_SET) {
+		// Check for consistency with already set value.
+		if ((*lhs) == (rhs)) {
+			return INPUT_SYSTEM_ERR_NO_ERROR;
+		} else {
+			*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+			return INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE;
+		}
+	}
+	// Check the value (individually).
+	if (rhs >= N_INPUT_SYSTEM_MULTIPLEX) {
+		*flags |= INPUT_SYSTEM_CFG_FLAG_CONFLICT;
+		return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+	}
+	// Set the value.
+	*lhs = rhs;
+
+	*flags |= INPUT_SYSTEM_CFG_FLAG_SET;
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq.c
new file mode 100644
index 000000000..80b5fd0dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq.c
@@ -0,0 +1,447 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "assert_support.h"
+#include "irq.h"
+
+#ifndef __INLINE_GP_DEVICE__
+#define __INLINE_GP_DEVICE__
+#endif
+#include "gp_device.h"	/* _REG_GP_IRQ_REQUEST_ADDR */
+
+static inline void irq_wait_for_write_complete(
+    const irq_ID_t		ID);
+
+static inline bool any_irq_channel_enabled(
+    const irq_ID_t				ID);
+
+static inline irq_ID_t virq_get_irq_id(const enum virq_id irq_ID,
+				       unsigned int *channel_ID);
+
+#ifndef __INLINE_IRQ__
+#include "irq_private.h"
+#endif /* __INLINE_IRQ__ */
+
+static unsigned short IRQ_N_CHANNEL[N_IRQ_ID] = {
+	IRQ0_ID_N_CHANNEL,
+	IRQ1_ID_N_CHANNEL,
+	IRQ2_ID_N_CHANNEL,
+	IRQ3_ID_N_CHANNEL
+};
+
+static unsigned short IRQ_N_ID_OFFSET[N_IRQ_ID + 1] = {
+	IRQ0_ID_OFFSET,
+	IRQ1_ID_OFFSET,
+	IRQ2_ID_OFFSET,
+	IRQ3_ID_OFFSET,
+	IRQ_END_OFFSET
+};
+
+static enum virq_id IRQ_NESTING_ID[N_IRQ_ID] = {
+	N_virq_id,
+	virq_ifmt,
+	virq_isys,
+	virq_isel
+};
+
+void irq_clear_all(
+    const irq_ID_t				ID)
+{
+	hrt_data	mask = 0xFFFFFFFF;
+
+	assert(ID < N_IRQ_ID);
+	assert(IRQ_N_CHANNEL[ID] <= HRT_DATA_WIDTH);
+
+	if (IRQ_N_CHANNEL[ID] < HRT_DATA_WIDTH) {
+		mask = ~((~(hrt_data)0) >> IRQ_N_CHANNEL[ID]);
+	}
+
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, mask);
+	return;
+}
+
+/*
+ * Do we want the user to be able to set the signalling method ?
+ */
+void irq_enable_channel(
+    const irq_ID_t				ID,
+    const unsigned int			irq_id)
+{
+	unsigned int mask = irq_reg_load(ID,
+					 _HRT_IRQ_CONTROLLER_MASK_REG_IDX);
+	unsigned int enable = irq_reg_load(ID,
+					   _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX);
+	unsigned int edge_in = irq_reg_load(ID,
+					    _HRT_IRQ_CONTROLLER_EDGE_REG_IDX);
+	unsigned int me = 1U << irq_id;
+
+	assert(ID < N_IRQ_ID);
+	assert(irq_id < IRQ_N_CHANNEL[ID]);
+
+	mask |= me;
+	enable |= me;
+	edge_in |= me;	/* rising edge */
+
+	/* to avoid mishaps configuration must follow the following order */
+
+	/* mask this interrupt */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_MASK_REG_IDX, mask & ~me);
+	/* rising edge at input */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_EDGE_REG_IDX, edge_in);
+	/* enable interrupt to output */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX, enable);
+	/* clear current irq only */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, me);
+	/* unmask interrupt from input */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_MASK_REG_IDX, mask);
+
+	irq_wait_for_write_complete(ID);
+
+	return;
+}
+
+void irq_enable_pulse(
+    const irq_ID_t	ID,
+    bool			pulse)
+{
+	unsigned int edge_out = 0x0;
+
+	if (pulse) {
+		edge_out = 0xffffffff;
+	}
+	/* output is given as edge, not pulse */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_EDGE_NOT_PULSE_REG_IDX, edge_out);
+	return;
+}
+
+void irq_disable_channel(
+    const irq_ID_t				ID,
+    const unsigned int			irq_id)
+{
+	unsigned int mask = irq_reg_load(ID,
+					 _HRT_IRQ_CONTROLLER_MASK_REG_IDX);
+	unsigned int enable = irq_reg_load(ID,
+					   _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX);
+	unsigned int me = 1U << irq_id;
+
+	assert(ID < N_IRQ_ID);
+	assert(irq_id < IRQ_N_CHANNEL[ID]);
+
+	mask &= ~me;
+	enable &= ~me;
+
+	/* enable interrupt to output */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX, enable);
+	/* unmask interrupt from input */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_MASK_REG_IDX, mask);
+	/* clear current irq only */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, me);
+
+	irq_wait_for_write_complete(ID);
+
+	return;
+}
+
+enum hrt_isp_css_irq_status irq_get_channel_id(
+    const irq_ID_t				ID,
+    unsigned int				*irq_id)
+{
+	unsigned int irq_status = irq_reg_load(ID,
+					       _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+	unsigned int idx;
+	enum hrt_isp_css_irq_status status = hrt_isp_css_irq_status_success;
+
+	assert(ID < N_IRQ_ID);
+	assert(irq_id);
+
+	/* find the first irq bit */
+	for (idx = 0; idx < IRQ_N_CHANNEL[ID]; idx++) {
+		if (irq_status & (1U << idx))
+			break;
+	}
+	if (idx == IRQ_N_CHANNEL[ID])
+		return hrt_isp_css_irq_status_error;
+
+	/* now check whether there are more bits set */
+	if (irq_status != (1U << idx))
+		status = hrt_isp_css_irq_status_more_irqs;
+
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, 1U << idx);
+
+	irq_wait_for_write_complete(ID);
+
+	if (irq_id)
+		*irq_id = (unsigned int)idx;
+
+	return status;
+}
+
+static const hrt_address IRQ_REQUEST_ADDR[N_IRQ_SW_CHANNEL_ID] = {
+	_REG_GP_IRQ_REQUEST0_ADDR,
+	_REG_GP_IRQ_REQUEST1_ADDR
+};
+
+void irq_raise(
+    const irq_ID_t				ID,
+    const irq_sw_channel_id_t	irq_id)
+{
+	hrt_address		addr;
+
+	OP___assert(ID == IRQ0_ID);
+	OP___assert(IRQ_BASE[ID] != (hrt_address)-1);
+	OP___assert(irq_id < N_IRQ_SW_CHANNEL_ID);
+
+	(void)ID;
+
+	addr = IRQ_REQUEST_ADDR[irq_id];
+	/* The SW IRQ pins are remapped to offset zero */
+	gp_device_reg_store(GP_DEVICE0_ID,
+			    (unsigned int)addr, 1);
+	gp_device_reg_store(GP_DEVICE0_ID,
+			    (unsigned int)addr, 0);
+	return;
+}
+
+void irq_controller_get_state(const irq_ID_t ID,
+			      struct irq_controller_state *state)
+{
+	assert(ID < N_IRQ_ID);
+	assert(state);
+
+	state->irq_edge = irq_reg_load(ID,
+				       _HRT_IRQ_CONTROLLER_EDGE_REG_IDX);
+	state->irq_mask = irq_reg_load(ID,
+				       _HRT_IRQ_CONTROLLER_MASK_REG_IDX);
+	state->irq_status = irq_reg_load(ID,
+					 _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+	state->irq_enable = irq_reg_load(ID,
+					 _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX);
+	state->irq_level_not_pulse = irq_reg_load(ID,
+				     _HRT_IRQ_CONTROLLER_EDGE_NOT_PULSE_REG_IDX);
+	return;
+}
+
+bool any_virq_signal(void)
+{
+	unsigned int irq_status = irq_reg_load(IRQ0_ID,
+					       _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+
+	return (irq_status != 0);
+}
+
+void cnd_virq_enable_channel(
+    const enum virq_id				irq_ID,
+    const bool					en)
+{
+	irq_ID_t		i;
+	unsigned int	channel_ID;
+	irq_ID_t		ID = virq_get_irq_id(irq_ID, &channel_ID);
+
+	assert(ID < N_IRQ_ID);
+
+	for (i = IRQ1_ID; i < N_IRQ_ID; i++) {
+		/* It is not allowed to enable the pin of a nested IRQ directly */
+		assert(irq_ID != IRQ_NESTING_ID[i]);
+	}
+
+	if (en) {
+		irq_enable_channel(ID, channel_ID);
+		if (IRQ_NESTING_ID[ID] != N_virq_id) {
+			/* Single level nesting, otherwise we'd need to recurse */
+			irq_enable_channel(IRQ0_ID, IRQ_NESTING_ID[ID]);
+		}
+	} else {
+		irq_disable_channel(ID, channel_ID);
+		if ((IRQ_NESTING_ID[ID] != N_virq_id) && !any_irq_channel_enabled(ID)) {
+			/* Only disable the top if the nested ones are empty */
+			irq_disable_channel(IRQ0_ID, IRQ_NESTING_ID[ID]);
+		}
+	}
+	return;
+}
+
+void virq_clear_all(void)
+{
+	irq_ID_t	irq_id;
+
+	for (irq_id = (irq_ID_t)0; irq_id < N_IRQ_ID; irq_id++) {
+		irq_clear_all(irq_id);
+	}
+	return;
+}
+
+enum hrt_isp_css_irq_status
+virq_get_channel_signals(struct virq_info *irq_info)
+{
+	enum hrt_isp_css_irq_status irq_status = hrt_isp_css_irq_status_error;
+	irq_ID_t ID;
+
+	assert(irq_info);
+
+	for (ID = (irq_ID_t)0 ; ID < N_IRQ_ID; ID++) {
+		if (any_irq_channel_enabled(ID)) {
+			hrt_data	irq_data = irq_reg_load(ID,
+							    _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+
+			if (irq_data != 0) {
+				/* The error condition is an IRQ pulse received with no IRQ status written */
+				irq_status = hrt_isp_css_irq_status_success;
+			}
+
+			irq_info->irq_status_reg[ID] |= irq_data;
+
+			irq_reg_store(ID,
+				      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, irq_data);
+
+			irq_wait_for_write_complete(ID);
+		}
+	}
+
+	return irq_status;
+}
+
+void virq_clear_info(struct virq_info *irq_info)
+{
+	irq_ID_t ID;
+
+	assert(irq_info);
+
+	for (ID = (irq_ID_t)0 ; ID < N_IRQ_ID; ID++) {
+		irq_info->irq_status_reg[ID] = 0;
+	}
+	return;
+}
+
+enum hrt_isp_css_irq_status virq_get_channel_id(
+    enum virq_id					*irq_id)
+{
+	unsigned int irq_status = irq_reg_load(IRQ0_ID,
+					       _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+	unsigned int idx;
+	enum hrt_isp_css_irq_status status = hrt_isp_css_irq_status_success;
+	irq_ID_t ID;
+
+	assert(irq_id);
+
+	/* find the first irq bit on device 0 */
+	for (idx = 0; idx < IRQ_N_CHANNEL[IRQ0_ID]; idx++) {
+		if (irq_status & (1U << idx))
+			break;
+	}
+
+	if (idx == IRQ_N_CHANNEL[IRQ0_ID]) {
+		return hrt_isp_css_irq_status_error;
+	}
+
+	/* Check whether there are more bits set on device 0 */
+	if (irq_status != (1U << idx)) {
+		status = hrt_isp_css_irq_status_more_irqs;
+	}
+
+	/* Check whether we have an IRQ on one of the nested devices */
+	for (ID = N_IRQ_ID - 1 ; ID > (irq_ID_t)0; ID--) {
+		if (IRQ_NESTING_ID[ID] == (enum virq_id)idx) {
+			break;
+		}
+	}
+
+	/* If we have a nested IRQ, load that state, discard the device 0 state */
+	if (ID != IRQ0_ID) {
+		irq_status = irq_reg_load(ID,
+					  _HRT_IRQ_CONTROLLER_STATUS_REG_IDX);
+		/* find the first irq bit on device "id" */
+		for (idx = 0; idx < IRQ_N_CHANNEL[ID]; idx++) {
+			if (irq_status & (1U << idx))
+				break;
+		}
+
+		if (idx == IRQ_N_CHANNEL[ID]) {
+			return hrt_isp_css_irq_status_error;
+		}
+
+		/* Alternatively check whether there are more bits set on this device */
+		if (irq_status != (1U << idx)) {
+			status = hrt_isp_css_irq_status_more_irqs;
+		} else {
+			/* If this device is empty, clear the state on device 0 */
+			irq_reg_store(IRQ0_ID,
+				      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, 1U << IRQ_NESTING_ID[ID]);
+		}
+	} /* if (ID != IRQ0_ID) */
+
+	/* Here we proceed to clear the IRQ on detected device, if no nested IRQ, this is device 0 */
+	irq_reg_store(ID,
+		      _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX, 1U << idx);
+
+	irq_wait_for_write_complete(ID);
+
+	idx += IRQ_N_ID_OFFSET[ID];
+	if (irq_id)
+		*irq_id = (enum virq_id)idx;
+
+	return status;
+}
+
+static inline void irq_wait_for_write_complete(
+    const irq_ID_t		ID)
+{
+	assert(ID < N_IRQ_ID);
+	assert(IRQ_BASE[ID] != (hrt_address)-1);
+	(void)ia_css_device_load_uint32(IRQ_BASE[ID] +
+					_HRT_IRQ_CONTROLLER_ENABLE_REG_IDX * sizeof(hrt_data));
+}
+
+static inline bool any_irq_channel_enabled(
+    const irq_ID_t				ID)
+{
+	hrt_data	en_reg;
+
+	assert(ID < N_IRQ_ID);
+
+	en_reg = irq_reg_load(ID,
+			      _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX);
+
+	return (en_reg != 0);
+}
+
+static inline irq_ID_t virq_get_irq_id(
+    const enum virq_id		irq_ID,
+    unsigned int		*channel_ID)
+{
+	irq_ID_t ID;
+
+	assert(channel_ID);
+
+	for (ID = (irq_ID_t)0 ; ID < N_IRQ_ID; ID++) {
+		if (irq_ID < IRQ_N_ID_OFFSET[ID + 1]) {
+			break;
+		}
+	}
+
+	*channel_ID = (unsigned int)irq_ID - IRQ_N_ID_OFFSET[ID];
+
+	return ID;
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_local.h
new file mode 100644
index 000000000..6a25345ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_local.h
@@ -0,0 +1,126 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IRQ_LOCAL_H_INCLUDED__
+#define __IRQ_LOCAL_H_INCLUDED__
+
+#include "irq_global.h"
+
+#include <irq_controller_defs.h>
+
+/* IRQ0_ID */
+#include "hive_isp_css_defs.h"
+#define HIVE_GP_DEV_IRQ_NUM_IRQS	32
+/* IRQ1_ID */
+#include "input_formatter_subsystem_defs.h"
+#define HIVE_IFMT_IRQ_NUM_IRQS		5
+/* IRQ2_ID */
+#include "input_system_defs.h"
+/* IRQ3_ID */
+#include "input_selector_defs.h"
+
+#define	IRQ_ID_OFFSET	32
+#define	IRQ0_ID_OFFSET	0
+#define	IRQ1_ID_OFFSET	IRQ_ID_OFFSET
+#define	IRQ2_ID_OFFSET	(2 * IRQ_ID_OFFSET)
+#define	IRQ3_ID_OFFSET	(3 * IRQ_ID_OFFSET)
+#define	IRQ_END_OFFSET	(4 * IRQ_ID_OFFSET)
+
+#define	IRQ0_ID_N_CHANNEL	HIVE_GP_DEV_IRQ_NUM_IRQS
+#define	IRQ1_ID_N_CHANNEL	HIVE_IFMT_IRQ_NUM_IRQS
+#define	IRQ2_ID_N_CHANNEL	HIVE_ISYS_IRQ_NUM_BITS
+#define	IRQ3_ID_N_CHANNEL	HIVE_ISEL_IRQ_NUM_IRQS
+
+enum virq_id {
+	virq_gpio_pin_0            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_0_BIT_ID,
+	virq_gpio_pin_1            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_1_BIT_ID,
+	virq_gpio_pin_2            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_2_BIT_ID,
+	virq_gpio_pin_3            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_3_BIT_ID,
+	virq_gpio_pin_4            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_4_BIT_ID,
+	virq_gpio_pin_5            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_5_BIT_ID,
+	virq_gpio_pin_6            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_6_BIT_ID,
+	virq_gpio_pin_7            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_7_BIT_ID,
+	virq_gpio_pin_8            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_8_BIT_ID,
+	virq_gpio_pin_9            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_9_BIT_ID,
+	virq_gpio_pin_10           = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_10_BIT_ID,
+	virq_gpio_pin_11           = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GPIO_PIN_11_BIT_ID,
+	virq_sp                    = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SP_BIT_ID,
+	virq_isp                   = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_BIT_ID,
+	virq_isys                  = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISYS_BIT_ID,
+	virq_isel                  = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISEL_BIT_ID,
+	virq_ifmt                  = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_IFMT_BIT_ID,
+	virq_sp_stream_mon         = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SP_STREAM_MON_BIT_ID,
+	virq_isp_stream_mon        = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_STREAM_MON_BIT_ID,
+	virq_mod_stream_mon        = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_MOD_STREAM_MON_BIT_ID,
+	virq_isp_pmem_error        = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_PMEM_ERROR_BIT_ID,
+	virq_isp_bamem_error       = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_BAMEM_ERROR_BIT_ID,
+	virq_isp_dmem_error        = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_DMEM_ERROR_BIT_ID,
+	virq_sp_icache_mem_error   = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SP_ICACHE_MEM_ERROR_BIT_ID,
+	virq_sp_dmem_error         = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SP_DMEM_ERROR_BIT_ID,
+	virq_mmu_cache_mem_error   = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_MMU_CACHE_MEM_ERROR_BIT_ID,
+	virq_gp_timer_0            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GP_TIMER_0_BIT_ID,
+	virq_gp_timer_1            = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_GP_TIMER_1_BIT_ID,
+	virq_sw_pin_0              = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SW_PIN_0_BIT_ID,
+	virq_sw_pin_1              = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SW_PIN_1_BIT_ID,
+	virq_dma                   = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_DMA_BIT_ID,
+	virq_sp_stream_mon_b       = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_SP_STREAM_MON_B_BIT_ID,
+
+	virq_ifmt0_id              = IRQ1_ID_OFFSET + HIVE_IFMT_IRQ_IFT_PRIM_BIT_ID,
+	virq_ifmt1_id              = IRQ1_ID_OFFSET + HIVE_IFMT_IRQ_IFT_PRIM_B_BIT_ID,
+	virq_ifmt2_id              = IRQ1_ID_OFFSET + HIVE_IFMT_IRQ_IFT_SEC_BIT_ID,
+	virq_ifmt3_id              = IRQ1_ID_OFFSET + HIVE_IFMT_IRQ_MEM_CPY_BIT_ID,
+	virq_ifmt_sideband_changed = IRQ1_ID_OFFSET + HIVE_IFMT_IRQ_SIDEBAND_CHANGED_BIT_ID,
+
+	virq_isys_sof              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_SOF_BIT_ID,
+	virq_isys_eof              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_EOF_BIT_ID,
+	virq_isys_sol              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_SOL_BIT_ID,
+	virq_isys_eol              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_EOL_BIT_ID,
+	virq_isys_csi              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_RECEIVER_BIT_ID,
+	virq_isys_csi_be           = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CSI_RECEIVER_BE_BIT_ID,
+	virq_isys_capt0_id_no_sop  = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_A_NO_SOP,
+	virq_isys_capt0_id_late_sop = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_A_LATE_SOP,
+	virq_isys_capt1_id_no_sop  = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_B_NO_SOP,
+	virq_isys_capt1_id_late_sop = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_B_LATE_SOP,
+	virq_isys_capt2_id_no_sop  = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_C_NO_SOP,
+	virq_isys_capt2_id_late_sop = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CAP_UNIT_C_LATE_SOP,
+	virq_isys_acq_sop_mismatch = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_ACQ_UNIT_SOP_MISMATCH,
+	virq_isys_ctrl_capt0       = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_INP_CTRL_CAPA,
+	virq_isys_ctrl_capt1       = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_INP_CTRL_CAPB,
+	virq_isys_ctrl_capt2       = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_INP_CTRL_CAPC,
+	virq_isys_cio_to_ahb       = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_CIO2AHB,
+	virq_isys_dma              = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_DMA_BIT_ID,
+	virq_isys_fifo_monitor     = IRQ2_ID_OFFSET + HIVE_ISYS_IRQ_STREAM_MON_BIT_ID,
+
+	virq_isel_sof              = IRQ3_ID_OFFSET + HIVE_ISEL_IRQ_SYNC_GEN_SOF_BIT_ID,
+	virq_isel_eof              = IRQ3_ID_OFFSET + HIVE_ISEL_IRQ_SYNC_GEN_EOF_BIT_ID,
+	virq_isel_sol              = IRQ3_ID_OFFSET + HIVE_ISEL_IRQ_SYNC_GEN_SOL_BIT_ID,
+	virq_isel_eol              = IRQ3_ID_OFFSET + HIVE_ISEL_IRQ_SYNC_GEN_EOL_BIT_ID,
+
+	N_virq_id                  = IRQ_END_OFFSET
+};
+
+struct virq_info {
+	hrt_data		irq_status_reg[N_IRQ_ID];
+};
+
+struct irq_controller_state {
+	unsigned int	irq_edge;
+	unsigned int	irq_mask;
+	unsigned int	irq_status;
+	unsigned int	irq_enable;
+	unsigned int	irq_level_not_pulse;
+};
+
+#endif /* __IRQ_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_private.h
new file mode 100644
index 000000000..e98663ef0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/irq_private.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IRQ_PRIVATE_H_INCLUDED__
+#define __IRQ_PRIVATE_H_INCLUDED__
+
+#include "irq_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_IRQ_C void irq_reg_store(
+    const irq_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value)
+{
+	assert(ID < N_IRQ_ID);
+	assert(IRQ_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(IRQ_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_IRQ_C hrt_data irq_reg_load(
+    const irq_ID_t		ID,
+    const unsigned int	reg)
+{
+	assert(ID < N_IRQ_ID);
+	assert(IRQ_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(IRQ_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+#endif /* __IRQ_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp.c
new file mode 100644
index 000000000..4ad5e2db8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp.c
@@ -0,0 +1,130 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/delay.h>
+
+#include <system_global.h>
+#include "isp.h"
+
+#ifndef __INLINE_ISP__
+#include "isp_private.h"
+#endif /* __INLINE_ISP__ */
+
+#include "assert_support.h"
+
+void cnd_isp_irq_enable(
+    const isp_ID_t		ID,
+    const bool		cnd)
+{
+	if (cnd) {
+		isp_ctrl_setbit(ID, ISP_IRQ_READY_REG, ISP_IRQ_READY_BIT);
+		/* Enabling the IRQ immediately triggers an interrupt, clear it */
+		isp_ctrl_setbit(ID, ISP_IRQ_CLEAR_REG, ISP_IRQ_CLEAR_BIT);
+	} else {
+		isp_ctrl_clearbit(ID, ISP_IRQ_READY_REG,
+				  ISP_IRQ_READY_BIT);
+	}
+	return;
+}
+
+void isp_get_state(
+    const isp_ID_t		ID,
+    isp_state_t			*state,
+    isp_stall_t			*stall)
+{
+	hrt_data sc = isp_ctrl_load(ID, ISP_SC_REG);
+
+	assert(state);
+	assert(stall);
+
+#if defined(_hrt_sysmem_ident_address)
+	/* Patch to avoid compiler unused symbol warning in C_RUN build */
+	(void)__hrt_sysmem_ident_address;
+	(void)_hrt_sysmem_map_var;
+#endif
+
+	state->pc = isp_ctrl_load(ID, ISP_PC_REG);
+	state->status_register = sc;
+	state->is_broken = isp_ctrl_getbit(ID, ISP_SC_REG, ISP_BROKEN_BIT);
+	state->is_idle = isp_ctrl_getbit(ID, ISP_SC_REG, ISP_IDLE_BIT);
+	state->is_sleeping = isp_ctrl_getbit(ID, ISP_SC_REG, ISP_SLEEPING_BIT);
+	state->is_stalling = isp_ctrl_getbit(ID, ISP_SC_REG, ISP_STALLING_BIT);
+	stall->stat_ctrl =
+	    !isp_ctrl_getbit(ID, ISP_CTRL_SINK_REG, ISP_CTRL_SINK_BIT);
+	stall->pmem =
+	    !isp_ctrl_getbit(ID, ISP_PMEM_SINK_REG, ISP_PMEM_SINK_BIT);
+	stall->dmem =
+	    !isp_ctrl_getbit(ID, ISP_DMEM_SINK_REG, ISP_DMEM_SINK_BIT);
+	stall->vmem =
+	    !isp_ctrl_getbit(ID, ISP_VMEM_SINK_REG, ISP_VMEM_SINK_BIT);
+	stall->fifo0 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO0_SINK_REG, ISP_FIFO0_SINK_BIT);
+	stall->fifo1 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO1_SINK_REG, ISP_FIFO1_SINK_BIT);
+	stall->fifo2 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO2_SINK_REG, ISP_FIFO2_SINK_BIT);
+	stall->fifo3 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO3_SINK_REG, ISP_FIFO3_SINK_BIT);
+	stall->fifo4 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO4_SINK_REG, ISP_FIFO4_SINK_BIT);
+	stall->fifo5 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO5_SINK_REG, ISP_FIFO5_SINK_BIT);
+	stall->fifo6 =
+	    !isp_ctrl_getbit(ID, ISP_FIFO6_SINK_REG, ISP_FIFO6_SINK_BIT);
+	stall->vamem1 =
+	    !isp_ctrl_getbit(ID, ISP_VAMEM1_SINK_REG, ISP_VAMEM1_SINK_BIT);
+	stall->vamem2 =
+	    !isp_ctrl_getbit(ID, ISP_VAMEM2_SINK_REG, ISP_VAMEM2_SINK_BIT);
+	stall->vamem3 =
+	    !isp_ctrl_getbit(ID, ISP_VAMEM3_SINK_REG, ISP_VAMEM3_SINK_BIT);
+	stall->hmem =
+	    !isp_ctrl_getbit(ID, ISP_HMEM_SINK_REG, ISP_HMEM_SINK_BIT);
+	/*
+		stall->icache_master =
+			!isp_ctrl_getbit(ID, ISP_ICACHE_MT_SINK_REG,
+				ISP_ICACHE_MT_SINK_BIT);
+	 */
+	return;
+}
+
+/* ISP functions to control the ISP state from the host, even in crun. */
+
+/* Inspect readiness of an ISP indexed by ID */
+unsigned int isp_is_ready(isp_ID_t ID)
+{
+	assert(ID < N_ISP_ID);
+	return isp_ctrl_getbit(ID, ISP_SC_REG, ISP_IDLE_BIT);
+}
+
+/* Inspect sleeping of an ISP indexed by ID */
+unsigned int isp_is_sleeping(isp_ID_t ID)
+{
+	assert(ID < N_ISP_ID);
+	return isp_ctrl_getbit(ID, ISP_SC_REG, ISP_SLEEPING_BIT);
+}
+
+/* To be called by the host immediately before starting ISP ID. */
+void isp_start(isp_ID_t ID)
+{
+	assert(ID < N_ISP_ID);
+}
+
+/* Wake up ISP ID. */
+void isp_wake(isp_ID_t ID)
+{
+	assert(ID < N_ISP_ID);
+	isp_ctrl_setbit(ID, ISP_SC_REG, ISP_START_BIT);
+	udelay(1);
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_local.h
new file mode 100644
index 000000000..4dbec4063
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_local.h
@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISP_LOCAL_H_INCLUDED__
+#define __ISP_LOCAL_H_INCLUDED__
+
+#include "isp_global.h"
+
+#include <isp2400_support.h>
+
+#define HIVE_ISP_VMEM_MASK	((1U << ISP_VMEM_ELEMBITS) - 1)
+
+typedef struct isp_state_s		isp_state_t;
+typedef struct isp_stall_s		isp_stall_t;
+
+struct isp_state_s {
+	int	pc;
+	int	status_register;
+	bool	is_broken;
+	bool	is_idle;
+	bool	is_sleeping;
+	bool	is_stalling;
+};
+
+struct isp_stall_s {
+	bool	fifo0;
+	bool	fifo1;
+	bool	fifo2;
+	bool	fifo3;
+	bool	fifo4;
+	bool	fifo5;
+	bool	fifo6;
+	bool	stat_ctrl;
+	bool	dmem;
+	bool	vmem;
+	bool	vamem1;
+	bool	vamem2;
+	bool	vamem3;
+	bool	hmem;
+	bool	pmem;
+	bool	icache_master;
+};
+
+#endif /* __ISP_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_private.h
new file mode 100644
index 000000000..2f9aeb3bd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/isp_private.h
@@ -0,0 +1,161 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISP_PRIVATE_H_INCLUDED__
+#define __ISP_PRIVATE_H_INCLUDED__
+
+#ifdef HRT_MEMORY_ACCESS
+#include <hrt/api.h>
+#endif
+
+#include "isp_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+#include "type_support.h"
+
+STORAGE_CLASS_ISP_C void isp_ctrl_store(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_CTRL_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_store_uint32(ISP_CTRL_BASE[ID] + reg * sizeof(hrt_data), value);
+#else
+	hrt_master_port_store_32(ISP_CTRL_BASE[ID] + reg * sizeof(hrt_data), value);
+#endif
+	return;
+}
+
+STORAGE_CLASS_ISP_C hrt_data isp_ctrl_load(
+    const isp_ID_t		ID,
+    const unsigned int	reg)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_CTRL_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	return ia_css_device_load_uint32(ISP_CTRL_BASE[ID] + reg * sizeof(hrt_data));
+#else
+	return hrt_master_port_uload_32(ISP_CTRL_BASE[ID] + reg * sizeof(hrt_data));
+#endif
+}
+
+STORAGE_CLASS_ISP_C bool isp_ctrl_getbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit)
+{
+	hrt_data val = isp_ctrl_load(ID, reg);
+
+	return (val & (1UL << bit)) != 0;
+}
+
+STORAGE_CLASS_ISP_C void isp_ctrl_setbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit)
+{
+	hrt_data	data = isp_ctrl_load(ID, reg);
+
+	isp_ctrl_store(ID, reg, (data | (1UL << bit)));
+	return;
+}
+
+STORAGE_CLASS_ISP_C void isp_ctrl_clearbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit)
+{
+	hrt_data	data = isp_ctrl_load(ID, reg);
+
+	isp_ctrl_store(ID, reg, (data & ~(1UL << bit)));
+	return;
+}
+
+STORAGE_CLASS_ISP_C void isp_dmem_store(
+    const isp_ID_t		ID,
+    unsigned int		addr,
+    const void		*data,
+    const size_t		size)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_DMEM_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_store(ISP_DMEM_BASE[ID] + addr, data, size);
+#else
+	hrt_master_port_store(ISP_DMEM_BASE[ID] + addr, data, size);
+#endif
+	return;
+}
+
+STORAGE_CLASS_ISP_C void isp_dmem_load(
+    const isp_ID_t		ID,
+    const unsigned int	addr,
+    void			*data,
+    const size_t		size)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_DMEM_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_load(ISP_DMEM_BASE[ID] + addr, data, size);
+#else
+	hrt_master_port_load(ISP_DMEM_BASE[ID] + addr, data, size);
+#endif
+	return;
+}
+
+STORAGE_CLASS_ISP_C void isp_dmem_store_uint32(
+    const isp_ID_t		ID,
+    unsigned int		addr,
+    const uint32_t		data)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_DMEM_BASE[ID] != (hrt_address) - 1);
+	(void)ID;
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_store_uint32(ISP_DMEM_BASE[ID] + addr, data);
+#else
+	hrt_master_port_store_32(ISP_DMEM_BASE[ID] + addr, data);
+#endif
+	return;
+}
+
+STORAGE_CLASS_ISP_C uint32_t isp_dmem_load_uint32(
+    const isp_ID_t		ID,
+    const unsigned int	addr)
+{
+	assert(ID < N_ISP_ID);
+	assert(ISP_DMEM_BASE[ID] != (hrt_address) - 1);
+	(void)ID;
+#if !defined(HRT_MEMORY_ACCESS)
+	return ia_css_device_load_uint32(ISP_DMEM_BASE[ID] + addr);
+#else
+	return hrt_master_port_uload_32(ISP_DMEM_BASE[ID] + addr);
+#endif
+}
+
+STORAGE_CLASS_ISP_C uint32_t isp_2w_cat_1w(
+    const u16		x0,
+    const uint16_t		x1)
+{
+	u32 out = ((uint32_t)(x1 & HIVE_ISP_VMEM_MASK) << ISP_VMEM_ELEMBITS)
+		  | (x0 & HIVE_ISP_VMEM_MASK);
+	return out;
+}
+
+#endif /* __ISP_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu.c
new file mode 100644
index 000000000..eb02835aa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* The name "mmu.h is already taken" */
+#include "mmu_device.h"
+
+void mmu_set_page_table_base_index(
+    const mmu_ID_t		ID,
+    const hrt_data		base_index)
+{
+	mmu_reg_store(ID, _HRT_MMU_PAGE_TABLE_BASE_ADDRESS_REG_IDX, base_index);
+	return;
+}
+
+hrt_data mmu_get_page_table_base_index(
+    const mmu_ID_t		ID)
+{
+	return mmu_reg_load(ID, _HRT_MMU_PAGE_TABLE_BASE_ADDRESS_REG_IDX);
+}
+
+void mmu_invalidate_cache(
+    const mmu_ID_t		ID)
+{
+	mmu_reg_store(ID, _HRT_MMU_INVALIDATE_TLB_REG_IDX, 1);
+	return;
+}
+
+void mmu_invalidate_cache_all(void)
+{
+	mmu_ID_t	mmu_id;
+
+	for (mmu_id = (mmu_ID_t)0; mmu_id < N_MMU_ID; mmu_id++) {
+		mmu_invalidate_cache(mmu_id);
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu_local.h
new file mode 100644
index 000000000..913150504
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/mmu_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MMU_LOCAL_H_INCLUDED__
+#define __MMU_LOCAL_H_INCLUDED__
+
+#include "mmu_global.h"
+
+#endif /* __MMU_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp.c
new file mode 100644
index 000000000..aae18465b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "sp.h"
+
+#ifndef __INLINE_SP__
+#include "sp_private.h"
+#endif /* __INLINE_SP__ */
+
+#include "assert_support.h"
+
+void cnd_sp_irq_enable(
+    const sp_ID_t		ID,
+    const bool		cnd)
+{
+	if (cnd) {
+		sp_ctrl_setbit(ID, SP_IRQ_READY_REG, SP_IRQ_READY_BIT);
+		/* Enabling the IRQ immediately triggers an interrupt, clear it */
+		sp_ctrl_setbit(ID, SP_IRQ_CLEAR_REG, SP_IRQ_CLEAR_BIT);
+	} else {
+		sp_ctrl_clearbit(ID, SP_IRQ_READY_REG, SP_IRQ_READY_BIT);
+	}
+}
+
+void sp_get_state(
+    const sp_ID_t			ID,
+    sp_state_t				*state,
+    sp_stall_t				*stall)
+{
+	hrt_data sc = sp_ctrl_load(ID, SP_SC_REG);
+
+	assert(state);
+	assert(stall);
+
+	state->pc = sp_ctrl_load(ID, SP_PC_REG);
+	state->status_register = sc;
+	state->is_broken   = (sc & (1U << SP_BROKEN_BIT)) != 0;
+	state->is_idle     = (sc & (1U << SP_IDLE_BIT)) != 0;
+	state->is_sleeping = (sc & (1U << SP_SLEEPING_BIT)) != 0;
+	state->is_stalling = (sc & (1U << SP_STALLING_BIT)) != 0;
+	stall->fifo0 =
+	    !sp_ctrl_getbit(ID, SP_FIFO0_SINK_REG, SP_FIFO0_SINK_BIT);
+	stall->fifo1 =
+	    !sp_ctrl_getbit(ID, SP_FIFO1_SINK_REG, SP_FIFO1_SINK_BIT);
+	stall->fifo2 =
+	    !sp_ctrl_getbit(ID, SP_FIFO2_SINK_REG, SP_FIFO2_SINK_BIT);
+	stall->fifo3 =
+	    !sp_ctrl_getbit(ID, SP_FIFO3_SINK_REG, SP_FIFO3_SINK_BIT);
+	stall->fifo4 =
+	    !sp_ctrl_getbit(ID, SP_FIFO4_SINK_REG, SP_FIFO4_SINK_BIT);
+	stall->fifo5 =
+	    !sp_ctrl_getbit(ID, SP_FIFO5_SINK_REG, SP_FIFO5_SINK_BIT);
+	stall->fifo6 =
+	    !sp_ctrl_getbit(ID, SP_FIFO6_SINK_REG, SP_FIFO6_SINK_BIT);
+	stall->fifo7 =
+	    !sp_ctrl_getbit(ID, SP_FIFO7_SINK_REG, SP_FIFO7_SINK_BIT);
+	stall->fifo8 =
+	    !sp_ctrl_getbit(ID, SP_FIFO8_SINK_REG, SP_FIFO8_SINK_BIT);
+	stall->fifo9 =
+	    !sp_ctrl_getbit(ID, SP_FIFO9_SINK_REG, SP_FIFO9_SINK_BIT);
+	stall->fifoa =
+	    !sp_ctrl_getbit(ID, SP_FIFOA_SINK_REG, SP_FIFOA_SINK_BIT);
+	stall->dmem =
+	    !sp_ctrl_getbit(ID, SP_DMEM_SINK_REG, SP_DMEM_SINK_BIT);
+	stall->control_master =
+	    !sp_ctrl_getbit(ID, SP_CTRL_MT_SINK_REG, SP_CTRL_MT_SINK_BIT);
+	stall->icache_master =
+	    !sp_ctrl_getbit(ID, SP_ICACHE_MT_SINK_REG,
+			    SP_ICACHE_MT_SINK_BIT);
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_local.h
new file mode 100644
index 000000000..2956c7023
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_local.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SP_LOCAL_H_INCLUDED__
+#define __SP_LOCAL_H_INCLUDED__
+
+#include <type_support.h>
+#include "sp_global.h"
+
+struct sp_state_s {
+	int		pc;
+	int		status_register;
+	bool	is_broken;
+	bool	is_idle;
+	bool	is_sleeping;
+	bool	is_stalling;
+};
+
+struct sp_stall_s {
+	bool	fifo0;
+	bool	fifo1;
+	bool	fifo2;
+	bool	fifo3;
+	bool	fifo4;
+	bool	fifo5;
+	bool	fifo6;
+	bool	fifo7;
+	bool	fifo8;
+	bool	fifo9;
+	bool	fifoa;
+	bool	dmem;
+	bool	control_master;
+	bool	icache_master;
+};
+
+#define sp_address_of(var)	(HIVE_ADDR_ ## var)
+
+/*
+ * deprecated
+ */
+#define store_sp_int(var, value) \
+	sp_dmem_store_uint32(SP0_ID, (unsigned int)sp_address_of(var), \
+		(uint32_t)(value))
+
+#define store_sp_ptr(var, value) \
+	sp_dmem_store_uint32(SP0_ID, (unsigned int)sp_address_of(var), \
+		(uint32_t)(value))
+
+#define load_sp_uint(var) \
+	sp_dmem_load_uint32(SP0_ID, (unsigned int)sp_address_of(var))
+
+#define load_sp_array_uint8(array_name, index) \
+	sp_dmem_load_uint8(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint8_t))
+
+#define load_sp_array_uint16(array_name, index) \
+	sp_dmem_load_uint16(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint16_t))
+
+#define load_sp_array_uint(array_name, index) \
+	sp_dmem_load_uint32(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint32_t))
+
+#define store_sp_var(var, data, bytes) \
+	sp_dmem_store(SP0_ID, (unsigned int)sp_address_of(var), data, bytes)
+
+#define store_sp_array_uint8(array_name, index, value) \
+	sp_dmem_store_uint8(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint8_t), value)
+
+#define store_sp_array_uint16(array_name, index, value) \
+	sp_dmem_store_uint16(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint16_t), value)
+
+#define store_sp_array_uint(array_name, index, value) \
+	sp_dmem_store_uint32(SP0_ID, (unsigned int)sp_address_of(array_name) + \
+		(index) * sizeof(uint32_t), value)
+
+#define store_sp_var_with_offset(var, offset, data, bytes) \
+	sp_dmem_store(SP0_ID, (unsigned int)sp_address_of(var) + \
+		offset, data, bytes)
+
+#define load_sp_var(var, data, bytes) \
+	sp_dmem_load(SP0_ID, (unsigned int)sp_address_of(var), data, bytes)
+
+#define load_sp_var_with_offset(var, offset, data, bytes) \
+	sp_dmem_load(SP0_ID, (unsigned int)sp_address_of(var) + offset, \
+		data, bytes)
+
+#endif /* __SP_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_private.h
new file mode 100644
index 000000000..05e6b438d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/sp_private.h
@@ -0,0 +1,167 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SP_PRIVATE_H_INCLUDED__
+#define __SP_PRIVATE_H_INCLUDED__
+
+#include "sp_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_SP_C void sp_ctrl_store(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const hrt_data		value)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_CTRL_BASE[ID] != (hrt_address)-1);
+	ia_css_device_store_uint32(SP_CTRL_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_SP_C hrt_data sp_ctrl_load(
+    const sp_ID_t		ID,
+    const hrt_address	reg)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_CTRL_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(SP_CTRL_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+STORAGE_CLASS_SP_C bool sp_ctrl_getbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit)
+{
+	hrt_data val = sp_ctrl_load(ID, reg);
+
+	return (val & (1UL << bit)) != 0;
+}
+
+STORAGE_CLASS_SP_C void sp_ctrl_setbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit)
+{
+	hrt_data	data = sp_ctrl_load(ID, reg);
+
+	sp_ctrl_store(ID, reg, (data | (1UL << bit)));
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_ctrl_clearbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit)
+{
+	hrt_data	data = sp_ctrl_load(ID, reg);
+
+	sp_ctrl_store(ID, reg, (data & ~(1UL << bit)));
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_dmem_store(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const void			*data,
+    const size_t		size)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	ia_css_device_store(SP_DMEM_BASE[ID] + addr, data, size);
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_dmem_load(
+    const sp_ID_t		ID,
+    const hrt_address	addr,
+    void				*data,
+    const size_t		size)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	ia_css_device_load(SP_DMEM_BASE[ID] + addr, data, size);
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_dmem_store_uint8(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint8_t		data)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	ia_css_device_store_uint8(SP_DMEM_BASE[SP0_ID] + addr, data);
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_dmem_store_uint16(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint16_t		data)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	ia_css_device_store_uint16(SP_DMEM_BASE[SP0_ID] + addr, data);
+	return;
+}
+
+STORAGE_CLASS_SP_C void sp_dmem_store_uint32(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint32_t		data)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	ia_css_device_store_uint32(SP_DMEM_BASE[SP0_ID] + addr, data);
+	return;
+}
+
+STORAGE_CLASS_SP_C uint8_t sp_dmem_load_uint8(
+    const sp_ID_t		ID,
+    const hrt_address	addr)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	return ia_css_device_load_uint8(SP_DMEM_BASE[SP0_ID] + addr);
+}
+
+STORAGE_CLASS_SP_C uint16_t sp_dmem_load_uint16(
+    const sp_ID_t		ID,
+    const hrt_address	addr)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	return ia_css_device_load_uint16(SP_DMEM_BASE[SP0_ID] + addr);
+}
+
+STORAGE_CLASS_SP_C uint32_t sp_dmem_load_uint32(
+    const sp_ID_t		ID,
+    const hrt_address	addr)
+{
+	assert(ID < N_SP_ID);
+	assert(SP_DMEM_BASE[ID] != (hrt_address)-1);
+	(void)ID;
+	return ia_css_device_load_uint32(SP_DMEM_BASE[SP0_ID] + addr);
+}
+
+#endif /* __SP_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl.c
new file mode 100644
index 000000000..bc9e7f10f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl.c
@@ -0,0 +1,75 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "timed_ctrl.h"
+
+#ifndef __INLINE_TIMED_CTRL__
+#include "timed_ctrl_private.h"
+#endif /* __INLINE_TIMED_CTRL__ */
+
+#include "assert_support.h"
+
+void timed_ctrl_snd_commnd(
+    const timed_ctrl_ID_t			ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    hrt_address				addr,
+    hrt_data				value)
+{
+	OP___assert(ID == TIMED_CTRL0_ID);
+	OP___assert(TIMED_CTRL_BASE[ID] != (hrt_address)-1);
+
+	timed_ctrl_reg_store(ID, _HRT_TIMED_CONTROLLER_CMD_REG_IDX, mask);
+	timed_ctrl_reg_store(ID, _HRT_TIMED_CONTROLLER_CMD_REG_IDX, condition);
+	timed_ctrl_reg_store(ID, _HRT_TIMED_CONTROLLER_CMD_REG_IDX, counter);
+	timed_ctrl_reg_store(ID, _HRT_TIMED_CONTROLLER_CMD_REG_IDX, (hrt_data)addr);
+	timed_ctrl_reg_store(ID, _HRT_TIMED_CONTROLLER_CMD_REG_IDX, value);
+}
+
+/* pqiao TODO: make sure the following commands get
+	correct BASE address both for csim and android */
+
+void timed_ctrl_snd_sp_commnd(
+    const timed_ctrl_ID_t			ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    const sp_ID_t				SP_ID,
+    hrt_address				offset,
+    hrt_data				value)
+{
+	OP___assert(SP_ID < N_SP_ID);
+	OP___assert(SP_DMEM_BASE[SP_ID] != (hrt_address)-1);
+
+	timed_ctrl_snd_commnd(ID, mask, condition, counter,
+			      SP_DMEM_BASE[SP_ID] + offset, value);
+}
+
+void timed_ctrl_snd_gpio_commnd(
+    const timed_ctrl_ID_t			ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    const gpio_ID_t				GPIO_ID,
+    hrt_address				offset,
+    hrt_data				value)
+{
+	OP___assert(GPIO_ID < N_GPIO_ID);
+	OP___assert(GPIO_BASE[GPIO_ID] != (hrt_address)-1);
+
+	timed_ctrl_snd_commnd(ID, mask, condition, counter,
+			      GPIO_BASE[GPIO_ID] + offset, value);
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_local.h
new file mode 100644
index 000000000..f58ee6afc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TIMED_CTRL_LOCAL_H_INCLUDED__
+#define __TIMED_CTRL_LOCAL_H_INCLUDED__
+
+#include "timed_ctrl_global.h"
+
+#endif /* __TIMED_CTRL_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_private.h
new file mode 100644
index 000000000..c19eeafed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/timed_ctrl_private.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TIMED_CTRL_PRIVATE_H_INCLUDED__
+#define __TIMED_CTRL_PRIVATE_H_INCLUDED__
+
+#include "timed_ctrl_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_TIMED_CTRL_C void timed_ctrl_reg_store(
+    const timed_ctrl_ID_t	ID,
+    const unsigned int		reg,
+    const hrt_data			value)
+{
+	OP___assert(ID < N_TIMED_CTRL_ID);
+	OP___assert(TIMED_CTRL_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(TIMED_CTRL_BASE[ID] + reg * sizeof(hrt_data), value);
+}
+
+#endif /* __GP_DEVICE_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vamem_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vamem_local.h
new file mode 100644
index 000000000..c68ed984c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vamem_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VAMEM_LOCAL_H_INCLUDED__
+#define __VAMEM_LOCAL_H_INCLUDED__
+
+#include "vamem_global.h"
+
+#endif /* __VAMEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem.c b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem.c
new file mode 100644
index 000000000..6620f0914
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem.c
@@ -0,0 +1,276 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2016, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "isp.h"
+#include "vmem.h"
+#include "vmem_local.h"
+
+#if !defined(HRT_MEMORY_ACCESS)
+#include "ia_css_device_access.h"
+#endif
+#include "assert_support.h"
+
+typedef unsigned long long hive_uedge;
+typedef hive_uedge *hive_wide;
+
+/* Copied from SDK: sim_semantics.c */
+
+/* subword bits move like this:         MSB[____xxxx____]LSB -> MSB[00000000xxxx]LSB */
+#define SUBWORD(w, start, end)     (((w) & (((1ULL << ((end) - 1)) - 1) << 1 | 1)) >> (start))
+
+/* inverse subword bits move like this: MSB[xxxx____xxxx]LSB -> MSB[xxxx0000xxxx]LSB */
+#define INV_SUBWORD(w, start, end) ((w) & (~(((1ULL << ((end) - 1)) - 1) << 1 | 1) | ((1ULL << (start)) - 1)))
+
+#define uedge_bits (8 * sizeof(hive_uedge))
+#define move_lower_bits(target, target_bit, src, src_bit) move_subword(target, target_bit, src, 0, src_bit)
+#define move_upper_bits(target, target_bit, src, src_bit) move_subword(target, target_bit, src, src_bit, uedge_bits)
+#define move_word(target, target_bit, src) move_subword(target, target_bit, src, 0, uedge_bits)
+
+static void
+move_subword(
+    hive_uedge *target,
+    unsigned int target_bit,
+    hive_uedge src,
+    unsigned int src_start,
+    unsigned int src_end)
+{
+	unsigned int start_elem = target_bit / uedge_bits;
+	unsigned int start_bit  = target_bit % uedge_bits;
+	unsigned int subword_width = src_end - src_start;
+
+	hive_uedge src_subword = SUBWORD(src, src_start, src_end);
+
+	if (subword_width + start_bit > uedge_bits) { /* overlap */
+		hive_uedge old_val1;
+		hive_uedge old_val0 = INV_SUBWORD(target[start_elem], start_bit, uedge_bits);
+
+		target[start_elem] = old_val0 | (src_subword << start_bit);
+		old_val1 = INV_SUBWORD(target[start_elem + 1], 0,
+				       subword_width + start_bit - uedge_bits);
+		target[start_elem + 1] = old_val1 | (src_subword >> (uedge_bits - start_bit));
+	} else {
+		hive_uedge old_val = INV_SUBWORD(target[start_elem], start_bit,
+						 start_bit + subword_width);
+
+		target[start_elem] = old_val | (src_subword << start_bit);
+	}
+}
+
+static void
+hive_sim_wide_unpack(
+    hive_wide vector,
+    hive_wide elem,
+    hive_uint elem_bits,
+    hive_uint index)
+{
+	/* pointers into wide_type: */
+	unsigned int start_elem = (elem_bits * index) / uedge_bits;
+	unsigned int start_bit  = (elem_bits * index) % uedge_bits;
+	unsigned int end_elem   = (elem_bits * (index + 1) - 1) / uedge_bits;
+	unsigned int end_bit    = ((elem_bits * (index + 1) - 1) % uedge_bits) + 1;
+
+	if (elem_bits == uedge_bits) {
+		/* easy case for speedup: */
+		elem[0] = vector[index];
+	} else if (start_elem == end_elem) {
+		/* only one (<=64 bits) element needs to be (partly) copied: */
+		move_subword(elem, 0, vector[start_elem], start_bit, end_bit);
+	} else {
+		/* general case: handles edge spanning cases (includes >64bit elements) */
+		unsigned int bits_written = 0;
+		unsigned int i;
+
+		move_upper_bits(elem, bits_written, vector[start_elem], start_bit);
+		bits_written += (64 - start_bit);
+		for (i = start_elem + 1; i < end_elem; i++) {
+			move_word(elem, bits_written, vector[i]);
+			bits_written += uedge_bits;
+		}
+		move_lower_bits(elem, bits_written, vector[end_elem], end_bit);
+	}
+}
+
+static void
+hive_sim_wide_pack(
+    hive_wide vector,
+    hive_wide elem,
+    hive_uint elem_bits,
+    hive_uint index)
+{
+	/* pointers into wide_type: */
+	unsigned int start_elem = (elem_bits * index) / uedge_bits;
+
+	/* easy case for speedup: */
+	if (elem_bits == uedge_bits) {
+		vector[start_elem] = elem[0];
+	} else if (elem_bits > uedge_bits) {
+		unsigned int bits_to_write = elem_bits;
+		unsigned int start_bit = elem_bits * index;
+		unsigned int i = 0;
+
+		for (; bits_to_write > uedge_bits;
+		     bits_to_write -= uedge_bits, i++, start_bit += uedge_bits) {
+			move_word(vector, start_bit, elem[i]);
+		}
+		move_lower_bits(vector, start_bit, elem[i], bits_to_write);
+	} else {
+		/* only one element needs to be (partly) copied: */
+		move_lower_bits(vector, elem_bits * index, elem[0], elem_bits);
+	}
+}
+
+static void load_vector(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from)
+{
+	unsigned int i;
+	hive_uedge *data;
+	unsigned int size = sizeof(short) * ISP_NWAY;
+
+	VMEM_ARRAY(v, 2 * ISP_NWAY); /* Need 2 vectors to work around vmem hss bug */
+	assert(ISP_BAMEM_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_load(ISP_BAMEM_BASE[ID] + (unsigned long)from, &v[0][0], size);
+#else
+	hrt_master_port_load(ISP_BAMEM_BASE[ID] + (unsigned long)from, &v[0][0], size);
+#endif
+	data = (hive_uedge *)v;
+	for (i = 0; i < ISP_NWAY; i++) {
+		hive_uedge elem = 0;
+
+		hive_sim_wide_unpack(data, &elem, ISP_VEC_ELEMBITS, i);
+		to[i] = elem;
+	}
+	udelay(1); /* Spend at least 1 cycles per vector */
+}
+
+static void store_vector(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from)
+{
+	unsigned int i;
+	unsigned int size = sizeof(short) * ISP_NWAY;
+
+	VMEM_ARRAY(v, 2 * ISP_NWAY); /* Need 2 vectors to work around vmem hss bug */
+	//load_vector (&v[1][0], &to[ISP_NWAY]); /* Fetch the next vector, since it will be overwritten. */
+	hive_uedge *data = (hive_uedge *)v;
+
+	for (i = 0; i < ISP_NWAY; i++) {
+		hive_sim_wide_pack(data, (hive_wide)&from[i], ISP_VEC_ELEMBITS, i);
+	}
+	assert(ISP_BAMEM_BASE[ID] != (hrt_address) - 1);
+#if !defined(HRT_MEMORY_ACCESS)
+	ia_css_device_store(ISP_BAMEM_BASE[ID] + (unsigned long)to, &v, size);
+#else
+	//hrt_mem_store (ISP, VMEM, (unsigned)to, &v, siz); /* This will overwrite the next vector as well */
+	hrt_master_port_store(ISP_BAMEM_BASE[ID] + (unsigned long)to, &v, size);
+#endif
+	udelay(1); /* Spend at least 1 cycles per vector */
+}
+
+void isp_vmem_load(
+    const isp_ID_t		ID,
+    const t_vmem_elem	*from,
+    t_vmem_elem		*to,
+    unsigned int elems) /* In t_vmem_elem */
+{
+	unsigned int c;
+	const t_vmem_elem *vp = from;
+
+	assert(ID < N_ISP_ID);
+	assert((unsigned long)from % ISP_VEC_ALIGN == 0);
+	assert(elems % ISP_NWAY == 0);
+	for (c = 0; c < elems; c += ISP_NWAY) {
+		load_vector(ID, &to[c], vp);
+		vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
+	}
+}
+
+void isp_vmem_store(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from,
+    unsigned int elems) /* In t_vmem_elem */
+{
+	unsigned int c;
+	t_vmem_elem *vp = to;
+
+	assert(ID < N_ISP_ID);
+	assert((unsigned long)to % ISP_VEC_ALIGN == 0);
+	assert(elems % ISP_NWAY == 0);
+	for (c = 0; c < elems; c += ISP_NWAY) {
+		store_vector(ID, vp, &from[c]);
+		vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
+	}
+}
+
+void isp_vmem_2d_load(
+    const isp_ID_t		ID,
+    const t_vmem_elem	*from,
+    t_vmem_elem		*to,
+    unsigned int height,
+    unsigned int width,
+    unsigned int stride_to,  /* In t_vmem_elem */
+
+    unsigned stride_from /* In t_vmem_elem */)
+{
+	unsigned int h;
+
+	assert(ID < N_ISP_ID);
+	assert((unsigned long)from % ISP_VEC_ALIGN == 0);
+	assert(width % ISP_NWAY == 0);
+	assert(stride_from % ISP_NWAY == 0);
+	for (h = 0; h < height; h++) {
+		unsigned int c;
+		const t_vmem_elem *vp = from;
+
+		for (c = 0; c < width; c += ISP_NWAY) {
+			load_vector(ID, &to[stride_to * h + c], vp);
+			vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
+		}
+		from = (const t_vmem_elem *)((const char *)from + stride_from / ISP_NWAY *
+					     ISP_VEC_ALIGN);
+	}
+}
+
+void isp_vmem_2d_store(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from,
+    unsigned int height,
+    unsigned int width,
+    unsigned int stride_to,  /* In t_vmem_elem */
+
+    unsigned stride_from /* In t_vmem_elem */)
+{
+	unsigned int h;
+
+	assert(ID < N_ISP_ID);
+	assert((unsigned long)to % ISP_VEC_ALIGN == 0);
+	assert(width % ISP_NWAY == 0);
+	assert(stride_to % ISP_NWAY == 0);
+	for (h = 0; h < height; h++) {
+		unsigned int c;
+		t_vmem_elem *vp = to;
+
+		for (c = 0; c < width; c += ISP_NWAY) {
+			store_vector(ID, vp, &from[stride_from * h + c]);
+			vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
+		}
+		to = (t_vmem_elem *)((char *)to + stride_to / ISP_NWAY * ISP_VEC_ALIGN);
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_local.h
new file mode 100644
index 000000000..d0ba59ced
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_local.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VMEM_LOCAL_H_INCLUDED__
+#define __VMEM_LOCAL_H_INCLUDED__
+
+#include "type_support.h"
+#include "vmem_global.h"
+
+typedef u16 t_vmem_elem;
+
+#define VMEM_ARRAY(x, s)    t_vmem_elem x[s / ISP_NWAY][ISP_NWAY]
+
+void isp_vmem_load(
+    const isp_ID_t		ID,
+    const t_vmem_elem	*from,
+    t_vmem_elem		*to,
+    unsigned int elems); /* In t_vmem_elem */
+
+void isp_vmem_store(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from,
+    unsigned int elems); /* In t_vmem_elem */
+
+void isp_vmem_2d_load(
+    const isp_ID_t		ID,
+    const t_vmem_elem	*from,
+    t_vmem_elem		*to,
+    unsigned int height,
+    unsigned int width,
+    unsigned int stride_to,  /* In t_vmem_elem */
+
+    unsigned		stride_from /* In t_vmem_elem */);
+
+void isp_vmem_2d_store(
+    const isp_ID_t		ID,
+    t_vmem_elem		*to,
+    const t_vmem_elem	*from,
+    unsigned int height,
+    unsigned int width,
+    unsigned int stride_to,  /* In t_vmem_elem */
+
+    unsigned		stride_from /* In t_vmem_elem */);
+
+#endif /* __VMEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_private.h
new file mode 100644
index 000000000..39cf1316b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/host/vmem_private.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VMEM_PRIVATE_H_INCLUDED__
+#define __VMEM_PRIVATE_H_INCLUDED__
+
+#include "vmem_public.h"
+
+#endif /* __VMEM_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/input_formatter_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/input_formatter_global.h
new file mode 100644
index 000000000..605cf02e5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/input_formatter_global.h
@@ -0,0 +1,115 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_FORMATTER_GLOBAL_H_INCLUDED__
+#define __INPUT_FORMATTER_GLOBAL_H_INCLUDED__
+
+#define IS_INPUT_FORMATTER_VERSION2
+#define IS_INPUT_SWITCH_VERSION2
+
+#include <type_support.h>
+#include <system_local.h>
+#include "if_defs.h"
+#include "str2mem_defs.h"
+#include "input_switch_2400_defs.h"
+
+#define _HIVE_INPUT_SWITCH_GET_FSYNC_REG_LSB(ch_id)        ((ch_id) * 3)
+
+#define HIVE_SWITCH_N_CHANNELS				4
+#define HIVE_SWITCH_N_FORMATTYPES			32
+#define HIVE_SWITCH_N_SWITCH_CODE			4
+#define HIVE_SWITCH_M_CHANNELS				0x00000003
+#define HIVE_SWITCH_M_FORMATTYPES			0x0000001f
+#define HIVE_SWITCH_M_SWITCH_CODE			0x00000003
+#define HIVE_SWITCH_M_FSYNC					0x00000007
+
+#define HIVE_SWITCH_ENCODE_FSYNC(x) \
+	(1U << (((x) - 1) & HIVE_SWITCH_M_CHANNELS))
+
+#define _HIVE_INPUT_SWITCH_GET_LUT_FIELD(reg, bit_index) \
+	(((reg) >> (bit_index)) & HIVE_SWITCH_M_SWITCH_CODE)
+#define _HIVE_INPUT_SWITCH_SET_LUT_FIELD(reg, bit_index, val) \
+	(((reg) & ~(HIVE_SWITCH_M_SWITCH_CODE << (bit_index))) | (((hrt_data)(val) & HIVE_SWITCH_M_SWITCH_CODE) << (bit_index)))
+#define _HIVE_INPUT_SWITCH_GET_FSYNC_FIELD(reg, bit_index) \
+	(((reg) >> (bit_index)) & HIVE_SWITCH_M_FSYNC)
+#define _HIVE_INPUT_SWITCH_SET_FSYNC_FIELD(reg, bit_index, val) \
+	(((reg) & ~(HIVE_SWITCH_M_FSYNC << (bit_index))) | (((hrt_data)(val) & HIVE_SWITCH_M_FSYNC) << (bit_index)))
+
+typedef struct input_formatter_cfg_s	input_formatter_cfg_t;
+
+/* Hardware registers */
+/*#define HIVE_IF_RESET_ADDRESS                   0x000*/ /* deprecated */
+#define HIVE_IF_START_LINE_ADDRESS              0x004
+#define HIVE_IF_START_COLUMN_ADDRESS            0x008
+#define HIVE_IF_CROPPED_HEIGHT_ADDRESS          0x00C
+#define HIVE_IF_CROPPED_WIDTH_ADDRESS           0x010
+#define HIVE_IF_VERTICAL_DECIMATION_ADDRESS     0x014
+#define HIVE_IF_HORIZONTAL_DECIMATION_ADDRESS   0x018
+#define HIVE_IF_H_DEINTERLEAVING_ADDRESS        0x01C
+#define HIVE_IF_LEFTPADDING_WIDTH_ADDRESS       0x020
+#define HIVE_IF_END_OF_LINE_OFFSET_ADDRESS      0x024
+#define HIVE_IF_VMEM_START_ADDRESS_ADDRESS      0x028
+#define HIVE_IF_VMEM_END_ADDRESS_ADDRESS        0x02C
+#define HIVE_IF_VMEM_INCREMENT_ADDRESS          0x030
+#define HIVE_IF_YUV_420_FORMAT_ADDRESS          0x034
+#define HIVE_IF_VSYNCK_ACTIVE_LOW_ADDRESS       0x038
+#define HIVE_IF_HSYNCK_ACTIVE_LOW_ADDRESS       0x03C
+#define HIVE_IF_ALLOW_FIFO_OVERFLOW_ADDRESS     0x040
+#define HIVE_IF_BLOCK_FIFO_NO_REQ_ADDRESS       0x044
+#define HIVE_IF_V_DEINTERLEAVING_ADDRESS        0x048
+#define HIVE_IF_FSM_CROP_PIXEL_COUNTER          0x110
+#define HIVE_IF_FSM_CROP_LINE_COUNTER           0x10C
+#define HIVE_IF_FSM_CROP_STATUS                 0x108
+
+/* Registers only for simulation */
+#define HIVE_IF_CRUN_MODE_ADDRESS               0x04C
+#define HIVE_IF_DUMP_OUTPUT_ADDRESS             0x050
+
+/* Follow the DMA syntax, "cmd" last */
+#define IF_PACK(val, cmd)             ((val & 0x0fff) | (cmd /*& 0xf000*/))
+
+#define HIVE_STR2MEM_SOFT_RESET_REG_ADDRESS                   (_STR2MEM_SOFT_RESET_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_INPUT_ENDIANNESS_REG_ADDRESS             (_STR2MEM_INPUT_ENDIANNESS_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_OUTPUT_ENDIANNESS_REG_ADDRESS            (_STR2MEM_OUTPUT_ENDIANNESS_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_BIT_SWAPPING_REG_ADDRESS                 (_STR2MEM_BIT_SWAPPING_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_BLOCK_SYNC_LEVEL_REG_ADDRESS             (_STR2MEM_BLOCK_SYNC_LEVEL_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_PACKET_SYNC_LEVEL_REG_ADDRESS            (_STR2MEM_PACKET_SYNC_LEVEL_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_READ_POST_WRITE_SYNC_ENABLE_REG_ADDRESS  (_STR2MEM_READ_POST_WRITE_SYNC_ENABLE_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_DUAL_BYTE_INPUTS_ENABLED_REG_ADDRESS     (_STR2MEM_DUAL_BYTE_INPUTS_ENABLED_REG_ID * _STR2MEM_REG_ALIGN)
+#define HIVE_STR2MEM_EN_STAT_UPDATE_ADDRESS                   (_STR2MEM_EN_STAT_UPDATE_ID * _STR2MEM_REG_ALIGN)
+
+/*
+ * This data structure is shared between host and SP
+ */
+struct input_formatter_cfg_s {
+	u32	start_line;
+	u32	start_column;
+	u32	left_padding;
+	u32	cropped_height;
+	u32	cropped_width;
+	u32	deinterleaving;
+	u32	buf_vecs;
+	u32	buf_start_index;
+	u32	buf_increment;
+	u32	buf_eol_offset;
+	u32	is_yuv420_format;
+	u32	block_no_reqs;
+};
+
+extern const hrt_address HIVE_IF_SRST_ADDRESS[N_INPUT_FORMATTER_ID];
+extern const hrt_data HIVE_IF_SRST_MASK[N_INPUT_FORMATTER_ID];
+extern const u8 HIVE_IF_SWITCH_CODE[N_INPUT_FORMATTER_ID];
+
+#endif /* __INPUT_FORMATTER_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/irq_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/irq_global.h
new file mode 100644
index 000000000..4a1dea6df
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/irq_global.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IRQ_GLOBAL_H_INCLUDED__
+#define __IRQ_GLOBAL_H_INCLUDED__
+
+#include <system_local.h>
+
+#define IS_IRQ_VERSION_2
+#define IS_IRQ_MAP_VERSION_2
+
+/* We cannot include the (hrt host ID) file defining the "CSS_RECEIVER" property without side effects */
+#ifndef HAS_NO_RX
+#include "irq_types_hrt.h"
+#endif
+
+/* The IRQ is not mapped uniformly on its related interfaces */
+#define	IRQ_SW_CHANNEL_OFFSET	hrt_isp_css_irq_sw_pin_0
+
+typedef enum {
+	IRQ_SW_CHANNEL0_ID = hrt_isp_css_irq_sw_pin_0 - IRQ_SW_CHANNEL_OFFSET,
+	IRQ_SW_CHANNEL1_ID = hrt_isp_css_irq_sw_pin_1 - IRQ_SW_CHANNEL_OFFSET,
+	N_IRQ_SW_CHANNEL_ID
+} irq_sw_channel_id_t;
+
+#endif /* __IRQ_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/isp_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/isp_global.h
new file mode 100644
index 000000000..5c6891c9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/isp_global.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISP_GLOBAL_H_INCLUDED__
+#define __ISP_GLOBAL_H_INCLUDED__
+
+#include <system_local.h>
+
+#include "mamoiada_params.h"
+
+#define ISP_PMEM_WIDTH_LOG2		ISP_LOG2_PMEM_WIDTH
+#define ISP_PMEM_SIZE			ISP_PMEM_DEPTH
+
+#define ISP_NWAY_LOG2			6
+#define ISP_VEC_NELEMS_LOG2		ISP_NWAY_LOG2
+
+#ifdef PIPE_GENERATION
+#define PIPEMEM(x) MEM(x)
+#define ISP_NWAY   BIT(ISP_NWAY_LOG2)
+#else
+#define PIPEMEM(x)
+#endif
+
+/* The number of data bytes in a vector disregarding the reduced precision */
+#define ISP_VEC_BYTES			(ISP_VEC_NELEMS * sizeof(uint16_t))
+
+/* ISP SC Registers */
+#define ISP_SC_REG			0x00
+#define ISP_PC_REG			0x07
+#define ISP_IRQ_READY_REG		0x00
+#define ISP_IRQ_CLEAR_REG		0x00
+
+/* ISP SC Register bits */
+#define ISP_RST_BIT			0x00
+#define ISP_START_BIT			0x01
+#define ISP_BREAK_BIT			0x02
+#define ISP_RUN_BIT			0x03
+#define ISP_BROKEN_BIT			0x04
+#define ISP_IDLE_BIT			0x05     /* READY */
+#define ISP_SLEEPING_BIT		0x06
+#define ISP_STALLING_BIT		0x07
+#define ISP_IRQ_CLEAR_BIT		0x08
+#define ISP_IRQ_READY_BIT		0x0A
+#define ISP_IRQ_SLEEPING_BIT		0x0B
+
+/* ISP Register bits */
+#define ISP_CTRL_SINK_BIT		0x00
+#define ISP_PMEM_SINK_BIT		0x01
+#define ISP_DMEM_SINK_BIT		0x02
+#define ISP_FIFO0_SINK_BIT		0x03
+#define ISP_FIFO1_SINK_BIT		0x04
+#define ISP_FIFO2_SINK_BIT		0x05
+#define ISP_FIFO3_SINK_BIT		0x06
+#define ISP_FIFO4_SINK_BIT		0x07
+#define ISP_FIFO5_SINK_BIT		0x08
+#define ISP_FIFO6_SINK_BIT		0x09
+#define ISP_VMEM_SINK_BIT		0x0A
+#define ISP_VAMEM1_SINK_BIT		0x0B
+#define ISP_VAMEM2_SINK_BIT		0x0C
+#define ISP_VAMEM3_SINK_BIT		0x0D
+#define ISP_HMEM_SINK_BIT		0x0E
+
+#define ISP_CTRL_SINK_REG		0x08
+#define ISP_PMEM_SINK_REG		0x08
+#define ISP_DMEM_SINK_REG		0x08
+#define ISP_FIFO0_SINK_REG		0x08
+#define ISP_FIFO1_SINK_REG		0x08
+#define ISP_FIFO2_SINK_REG		0x08
+#define ISP_FIFO3_SINK_REG		0x08
+#define ISP_FIFO4_SINK_REG		0x08
+#define ISP_FIFO5_SINK_REG		0x08
+#define ISP_FIFO6_SINK_REG		0x08
+#define ISP_VMEM_SINK_REG		0x08
+#define ISP_VAMEM1_SINK_REG		0x08
+#define ISP_VAMEM2_SINK_REG		0x08
+#define ISP_VAMEM3_SINK_REG		0x08
+#define ISP_HMEM_SINK_REG		0x08
+
+/* ISP2401 */
+#define BAMEM VMEM
+#define XNR3_DOWN_BAMEM_BASE_ADDRESS    (0x16880)
+#define XNR3_UP_BAMEM_BASE_ADDRESS      (0x12880)
+#define bmem_ldrow(fu, pid, offset, data) bmem_ldrow_s(fu, pid, offset, data)
+#define bmem_strow(fu, pid, offset, data) bmem_strow_s(fu, pid, offset, data)
+#define bmem_ldblk(fu, pid, offset, data) bmem_ldblk_s(fu, pid, offset, data)
+#define bmem_stblk(fu, pid, offset, data) bmem_stblk_s(fu, pid, offset, data)
+
+#endif /* __ISP_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/mmu_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/mmu_global.h
new file mode 100644
index 000000000..8738fed6a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/mmu_global.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MMU_GLOBAL_H_INCLUDED__
+#define __MMU_GLOBAL_H_INCLUDED__
+
+#define IS_MMU_VERSION_2
+
+#include <mmu_defs.h>
+
+#endif /* __MMU_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/sp_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/sp_global.h
new file mode 100644
index 000000000..b8338f9b5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/sp_global.h
@@ -0,0 +1,84 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SP_GLOBAL_H_INCLUDED__
+#define __SP_GLOBAL_H_INCLUDED__
+
+#include <system_local.h>
+
+#include <scalar_processor_2400_params.h>
+
+#define SP_PMEM_WIDTH_LOG2		SP_PMEM_LOG_WIDTH_BITS
+#define SP_PMEM_SIZE			SP_PMEM_DEPTH
+
+#define SP_DMEM_SIZE			0x4000
+
+/* SP Registers */
+#define SP_PC_REG				0x09
+#define SP_SC_REG				0x00
+#define SP_START_ADDR_REG		0x01
+#define SP_ICACHE_ADDR_REG		0x05
+#define SP_IRQ_READY_REG		0x00
+#define SP_IRQ_CLEAR_REG		0x00
+#define SP_ICACHE_INV_REG		0x00
+#define SP_CTRL_SINK_REG		0x0A
+
+/* SP Register bits */
+#define SP_RST_BIT			0x00
+#define SP_START_BIT			0x01
+#define SP_BREAK_BIT			0x02
+#define SP_RUN_BIT			0x03
+#define SP_BROKEN_BIT			0x04
+#define SP_IDLE_BIT			0x05     /* READY */
+#define SP_SLEEPING_BIT			0x06
+#define SP_STALLING_BIT			0x07
+#define SP_IRQ_CLEAR_BIT		0x08
+#define SP_IRQ_READY_BIT		0x0A
+#define SP_IRQ_SLEEPING_BIT		0x0B
+
+#define SP_ICACHE_INV_BIT		0x0C
+#define SP_IPREFETCH_EN_BIT		0x0D
+
+#define SP_FIFO0_SINK_BIT		0x00
+#define SP_FIFO1_SINK_BIT		0x01
+#define SP_FIFO2_SINK_BIT		0x02
+#define SP_FIFO3_SINK_BIT		0x03
+#define SP_FIFO4_SINK_BIT		0x04
+#define SP_FIFO5_SINK_BIT		0x05
+#define SP_FIFO6_SINK_BIT		0x06
+#define SP_FIFO7_SINK_BIT		0x07
+#define SP_FIFO8_SINK_BIT		0x08
+#define SP_FIFO9_SINK_BIT		0x09
+#define SP_FIFOA_SINK_BIT		0x0A
+#define SP_DMEM_SINK_BIT		0x0B
+#define SP_CTRL_MT_SINK_BIT		0x0C
+#define SP_ICACHE_MT_SINK_BIT	0x0D
+
+#define SP_FIFO0_SINK_REG		0x0A
+#define SP_FIFO1_SINK_REG		0x0A
+#define SP_FIFO2_SINK_REG		0x0A
+#define SP_FIFO3_SINK_REG		0x0A
+#define SP_FIFO4_SINK_REG		0x0A
+#define SP_FIFO5_SINK_REG		0x0A
+#define SP_FIFO6_SINK_REG		0x0A
+#define SP_FIFO7_SINK_REG		0x0A
+#define SP_FIFO8_SINK_REG		0x0A
+#define SP_FIFO9_SINK_REG		0x0A
+#define SP_FIFOA_SINK_REG		0x0A
+#define SP_DMEM_SINK_REG		0x0A
+#define SP_CTRL_MT_SINK_REG		0x0A
+#define SP_ICACHE_MT_SINK_REG	0x0A
+
+#endif /* __SP_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/timed_ctrl_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/timed_ctrl_global.h
new file mode 100644
index 000000000..3f2915a78
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/timed_ctrl_global.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TIMED_CTRL_GLOBAL_H_INCLUDED__
+#define __TIMED_CTRL_GLOBAL_H_INCLUDED__
+
+#define IS_TIMED_CTRL_VERSION_1
+
+#include "timed_controller_defs.h"
+
+/**
+ * Order of the input bits for the timed controller taken from
+ * ISP_CSS_2401 System Architecture Description valid for
+ * 2400, 2401.
+ *
+ * Check for other systems.
+ */
+#define HIVE_TIMED_CTRL_GPIO_PIN_0_BIT_ID                       0
+#define HIVE_TIMED_CTRL_GPIO_PIN_1_BIT_ID                       1
+#define HIVE_TIMED_CTRL_GPIO_PIN_2_BIT_ID                       2
+#define HIVE_TIMED_CTRL_GPIO_PIN_3_BIT_ID                       3
+#define HIVE_TIMED_CTRL_GPIO_PIN_4_BIT_ID                       4
+#define HIVE_TIMED_CTRL_GPIO_PIN_5_BIT_ID                       5
+#define HIVE_TIMED_CTRL_GPIO_PIN_6_BIT_ID                       6
+#define HIVE_TIMED_CTRL_GPIO_PIN_7_BIT_ID                       7
+#define HIVE_TIMED_CTRL_GPIO_PIN_8_BIT_ID                       8
+#define HIVE_TIMED_CTRL_GPIO_PIN_9_BIT_ID                       9
+#define HIVE_TIMED_CTRL_GPIO_PIN_10_BIT_ID                      10
+#define HIVE_TIMED_CTRL_GPIO_PIN_11_BIT_ID                      11
+#define HIVE_TIMED_CTRL_IRQ_SP_BIT_ID                           12
+#define HIVE_TIMED_CTRL_IRQ_ISP_BIT_ID                          13
+#define HIVE_TIMED_CTRL_IRQ_INPUT_SYSTEM_BIT_ID                 14
+#define HIVE_TIMED_CTRL_IRQ_INPUT_SELECTOR_BIT_ID               15
+#define HIVE_TIMED_CTRL_IRQ_IF_BLOCK_BIT_ID                     16
+#define HIVE_TIMED_CTRL_IRQ_GP_TIMER_0_BIT_ID                   17
+#define HIVE_TIMED_CTRL_IRQ_GP_TIMER_1_BIT_ID                   18
+#define HIVE_TIMED_CTRL_CSI_SOL_BIT_ID                          19
+#define HIVE_TIMED_CTRL_CSI_EOL_BIT_ID                          20
+#define HIVE_TIMED_CTRL_CSI_SOF_BIT_ID                          21
+#define HIVE_TIMED_CTRL_CSI_EOF_BIT_ID                          22
+#define HIVE_TIMED_CTRL_IRQ_IS_STREAMING_MONITOR_BIT_ID         23
+
+#endif /* __TIMED_CTRL_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/vamem_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/vamem_global.h
new file mode 100644
index 000000000..0d290e815
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/vamem_global.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VAMEM_GLOBAL_H_INCLUDED__
+#define __VAMEM_GLOBAL_H_INCLUDED__
+
+#include <type_support.h>
+
+#define IS_VAMEM_VERSION_2
+
+/* (log) stepsize of linear interpolation */
+#define VAMEM_INTERP_STEP_LOG2	4
+#define VAMEM_INTERP_STEP		BIT(VAMEM_INTERP_STEP_LOG2)
+/* (physical) size of the tables */
+#define VAMEM_TABLE_UNIT_SIZE	((1 << (ISP_VAMEM_ADDRESS_BITS - VAMEM_INTERP_STEP_LOG2)) + 1)
+/* (logical) size of the tables */
+#define VAMEM_TABLE_UNIT_STEP	((VAMEM_TABLE_UNIT_SIZE - 1) << 1)
+/* Number of tables */
+#define VAMEM_TABLE_UNIT_COUNT	(ISP_VAMEM_DEPTH / VAMEM_TABLE_UNIT_STEP)
+
+typedef u16				vamem_data_t;
+
+#endif /* __VAMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_common/vmem_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_common/vmem_global.h
new file mode 100644
index 000000000..537b07421
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_common/vmem_global.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VMEM_GLOBAL_H_INCLUDED__
+#define __VMEM_GLOBAL_H_INCLUDED__
+
+#include "isp.h"
+
+#define VMEM_SIZE	ISP_VMEM_DEPTH
+#define VMEM_ELEMBITS	ISP_VMEM_ELEMBITS
+#define VMEM_ALIGN	ISP_VMEM_ALIGN
+
+#ifndef PIPE_GENERATION
+typedef tvector *pvector;
+#endif
+
+#endif /* __VMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_defs.h b/drivers/staging/media/atomisp/pci/hive_isp_css_defs.h
new file mode 100644
index 000000000..e9cf27438
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_defs.h
@@ -0,0 +1,412 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _hive_isp_css_defs_h__
+#define _hive_isp_css_defs_h__
+
+#define HIVE_ISP_CTRL_DATA_WIDTH     32
+#define HIVE_ISP_CTRL_ADDRESS_WIDTH  32
+#define HIVE_ISP_CTRL_MAX_BURST_SIZE  1
+#define HIVE_ISP_DDR_ADDRESS_WIDTH   36
+
+#define HIVE_ISP_HOST_MAX_BURST_SIZE  8 /* host supports bursts in order to prevent repeating DDRAM accesses */
+#define HIVE_ISP_NUM_GPIO_PINS       12
+
+/* This list of vector num_elems/elem_bits pairs is valid both in C as initializer
+   and in the DMA parameter list */
+#define HIVE_ISP_DDR_DMA_SPECS {{32,  8}, {16, 16}, {18, 14}, {25, 10}, {21, 12}}
+#define HIVE_ISP_DDR_WORD_BITS 256
+#define HIVE_ISP_DDR_WORD_BYTES  (HIVE_ISP_DDR_WORD_BITS / 8)
+#define HIVE_ISP_DDR_BYTES       (512 * 1024 * 1024) /* hss only */
+#define HIVE_ISP_DDR_BYTES_RTL   (127 * 1024 * 1024) /* RTL only */
+#define HIVE_ISP_DDR_SMALL_BYTES (128 * 256 / 8)
+#define HIVE_ISP_PAGE_SHIFT    12
+#define HIVE_ISP_PAGE_SIZE     BIT(HIVE_ISP_PAGE_SHIFT)
+
+#define CSS_DDR_WORD_BITS        HIVE_ISP_DDR_WORD_BITS
+#define CSS_DDR_WORD_BYTES       HIVE_ISP_DDR_WORD_BYTES
+
+/* If HIVE_ISP_DDR_BASE_OFFSET is set to a non-zero value, the wide bus just before the DDRAM gets an extra dummy port where         */
+/* address range 0 .. HIVE_ISP_DDR_BASE_OFFSET-1 maps onto. This effectively creates an offset for the DDRAM from system perspective */
+#define HIVE_ISP_DDR_BASE_OFFSET 0x120000000 /* 0x200000 */
+
+#define HIVE_DMA_ISP_BUS_CONN 0
+#define HIVE_DMA_ISP_DDR_CONN 1
+#define HIVE_DMA_BUS_DDR_CONN 2
+#define HIVE_DMA_ISP_MASTER master_port0
+#define HIVE_DMA_BUS_MASTER master_port1
+#define HIVE_DMA_DDR_MASTER master_port2
+
+#define HIVE_DMA_NUM_CHANNELS       32 /* old value was  8 */
+#define HIVE_DMA_CMD_FIFO_DEPTH     24 /* old value was 12 */
+
+#define HIVE_IF_PIXEL_WIDTH 12
+
+#define HIVE_MMU_TLB_SETS           8
+#define HIVE_MMU_TLB_SET_BLOCKS     8
+#define HIVE_MMU_TLB_BLOCK_ELEMENTS 8
+#define HIVE_MMU_PAGE_TABLE_LEVELS  2
+#define HIVE_MMU_PAGE_BYTES         HIVE_ISP_PAGE_SIZE
+
+#define HIVE_ISP_CH_ID_BITS    2
+#define HIVE_ISP_FMT_TYPE_BITS 5
+#define HIVE_ISP_ISEL_SEL_BITS 2
+
+#define HIVE_GP_REGS_SDRAM_WAKEUP_IDX                           0
+#define HIVE_GP_REGS_IDLE_IDX                                   1
+#define HIVE_GP_REGS_IRQ_0_IDX                                  2
+#define HIVE_GP_REGS_IRQ_1_IDX                                  3
+#define HIVE_GP_REGS_SP_STREAM_STAT_IDX                         4
+#define HIVE_GP_REGS_SP_STREAM_STAT_B_IDX                       5
+#define HIVE_GP_REGS_ISP_STREAM_STAT_IDX                        6
+#define HIVE_GP_REGS_MOD_STREAM_STAT_IDX                        7
+#define HIVE_GP_REGS_SP_STREAM_STAT_IRQ_COND_IDX                8
+#define HIVE_GP_REGS_SP_STREAM_STAT_B_IRQ_COND_IDX              9
+#define HIVE_GP_REGS_ISP_STREAM_STAT_IRQ_COND_IDX              10
+#define HIVE_GP_REGS_MOD_STREAM_STAT_IRQ_COND_IDX              11
+#define HIVE_GP_REGS_SP_STREAM_STAT_IRQ_ENABLE_IDX             12
+#define HIVE_GP_REGS_SP_STREAM_STAT_B_IRQ_ENABLE_IDX           13
+#define HIVE_GP_REGS_ISP_STREAM_STAT_IRQ_ENABLE_IDX            14
+#define HIVE_GP_REGS_MOD_STREAM_STAT_IRQ_ENABLE_IDX            15
+#define HIVE_GP_REGS_SWITCH_PRIM_IF_IDX                        16
+#define HIVE_GP_REGS_SWITCH_GDC1_IDX                           17
+#define HIVE_GP_REGS_SWITCH_GDC2_IDX                           18
+#define HIVE_GP_REGS_SRST_IDX                                  19
+#define HIVE_GP_REGS_SLV_REG_SRST_IDX                          20
+
+/* Bit numbers of the soft reset register */
+#define HIVE_GP_REGS_SRST_ISYS_CBUS                             0
+#define HIVE_GP_REGS_SRST_ISEL_CBUS                             1
+#define HIVE_GP_REGS_SRST_IFMT_CBUS                             2
+#define HIVE_GP_REGS_SRST_GPDEV_CBUS                            3
+#define HIVE_GP_REGS_SRST_GPIO                                  4
+#define HIVE_GP_REGS_SRST_TC                                    5
+#define HIVE_GP_REGS_SRST_GPTIMER                               6
+#define HIVE_GP_REGS_SRST_FACELLFIFOS                           7
+#define HIVE_GP_REGS_SRST_D_OSYS                                8
+#define HIVE_GP_REGS_SRST_IFT_SEC_PIPE                          9
+#define HIVE_GP_REGS_SRST_GDC1                                 10
+#define HIVE_GP_REGS_SRST_GDC2                                 11
+#define HIVE_GP_REGS_SRST_VEC_BUS                              12
+#define HIVE_GP_REGS_SRST_ISP                                  13
+#define HIVE_GP_REGS_SRST_SLV_GRP_BUS                          14
+#define HIVE_GP_REGS_SRST_DMA                                  15
+#define HIVE_GP_REGS_SRST_SF_ISP_SP                            16
+#define HIVE_GP_REGS_SRST_SF_PIF_CELLS                         17
+#define HIVE_GP_REGS_SRST_SF_SIF_SP                            18
+#define HIVE_GP_REGS_SRST_SF_MC_SP                             19
+#define HIVE_GP_REGS_SRST_SF_ISYS_SP                           20
+#define HIVE_GP_REGS_SRST_SF_DMA_CELLS                         21
+#define HIVE_GP_REGS_SRST_SF_GDC1_CELLS                        22
+#define HIVE_GP_REGS_SRST_SF_GDC2_CELLS                        23
+#define HIVE_GP_REGS_SRST_SP                                   24
+#define HIVE_GP_REGS_SRST_OCP2CIO                              25
+#define HIVE_GP_REGS_SRST_NBUS                                 26
+#define HIVE_GP_REGS_SRST_HOST12BUS                            27
+#define HIVE_GP_REGS_SRST_WBUS                                 28
+#define HIVE_GP_REGS_SRST_IC_OSYS                              29
+#define HIVE_GP_REGS_SRST_WBUS_IC                              30
+
+/* Bit numbers of the slave register soft reset register */
+#define HIVE_GP_REGS_SLV_REG_SRST_DMA                           0
+#define HIVE_GP_REGS_SLV_REG_SRST_GDC1                          1
+#define HIVE_GP_REGS_SLV_REG_SRST_GDC2                          2
+
+/* order of the input bits for the irq controller */
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_0_BIT_ID                       0
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_1_BIT_ID                       1
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_2_BIT_ID                       2
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_3_BIT_ID                       3
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_4_BIT_ID                       4
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_5_BIT_ID                       5
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_6_BIT_ID                       6
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_7_BIT_ID                       7
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_8_BIT_ID                       8
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_9_BIT_ID                       9
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_10_BIT_ID                     10
+#define HIVE_GP_DEV_IRQ_GPIO_PIN_11_BIT_ID                     11
+#define HIVE_GP_DEV_IRQ_SP_BIT_ID                              12
+#define HIVE_GP_DEV_IRQ_ISP_BIT_ID                             13
+#define HIVE_GP_DEV_IRQ_ISYS_BIT_ID                            14
+#define HIVE_GP_DEV_IRQ_ISEL_BIT_ID                            15
+#define HIVE_GP_DEV_IRQ_IFMT_BIT_ID                            16
+#define HIVE_GP_DEV_IRQ_SP_STREAM_MON_BIT_ID                   17
+#define HIVE_GP_DEV_IRQ_ISP_STREAM_MON_BIT_ID                  18
+#define HIVE_GP_DEV_IRQ_MOD_STREAM_MON_BIT_ID                  19
+#define HIVE_GP_DEV_IRQ_ISP_PMEM_ERROR_BIT_ID                  20
+#define HIVE_GP_DEV_IRQ_ISP_BAMEM_ERROR_BIT_ID                 21
+#define HIVE_GP_DEV_IRQ_ISP_DMEM_ERROR_BIT_ID                  22
+#define HIVE_GP_DEV_IRQ_SP_ICACHE_MEM_ERROR_BIT_ID             23
+#define HIVE_GP_DEV_IRQ_SP_DMEM_ERROR_BIT_ID                   24
+#define HIVE_GP_DEV_IRQ_MMU_CACHE_MEM_ERROR_BIT_ID             25
+#define HIVE_GP_DEV_IRQ_GP_TIMER_0_BIT_ID                      26
+#define HIVE_GP_DEV_IRQ_GP_TIMER_1_BIT_ID                      27
+#define HIVE_GP_DEV_IRQ_SW_PIN_0_BIT_ID                        28
+#define HIVE_GP_DEV_IRQ_SW_PIN_1_BIT_ID                        29
+#define HIVE_GP_DEV_IRQ_DMA_BIT_ID                             30
+#define HIVE_GP_DEV_IRQ_SP_STREAM_MON_B_BIT_ID                 31
+
+#define HIVE_GP_REGS_NUM_SW_IRQ_REGS                            2
+
+/* order of the input bits for the timed controller */
+#define HIVE_GP_DEV_TC_GPIO_PIN_0_BIT_ID                       0
+#define HIVE_GP_DEV_TC_GPIO_PIN_1_BIT_ID                       1
+#define HIVE_GP_DEV_TC_GPIO_PIN_2_BIT_ID                       2
+#define HIVE_GP_DEV_TC_GPIO_PIN_3_BIT_ID                       3
+#define HIVE_GP_DEV_TC_GPIO_PIN_4_BIT_ID                       4
+#define HIVE_GP_DEV_TC_GPIO_PIN_5_BIT_ID                       5
+#define HIVE_GP_DEV_TC_GPIO_PIN_6_BIT_ID                       6
+#define HIVE_GP_DEV_TC_GPIO_PIN_7_BIT_ID                       7
+#define HIVE_GP_DEV_TC_GPIO_PIN_8_BIT_ID                       8
+#define HIVE_GP_DEV_TC_GPIO_PIN_9_BIT_ID                       9
+#define HIVE_GP_DEV_TC_GPIO_PIN_10_BIT_ID                     10
+#define HIVE_GP_DEV_TC_GPIO_PIN_11_BIT_ID                     11
+#define HIVE_GP_DEV_TC_SP_BIT_ID                              12
+#define HIVE_GP_DEV_TC_ISP_BIT_ID                             13
+#define HIVE_GP_DEV_TC_ISYS_BIT_ID                            14
+#define HIVE_GP_DEV_TC_ISEL_BIT_ID                            15
+#define HIVE_GP_DEV_TC_IFMT_BIT_ID                            16
+#define HIVE_GP_DEV_TC_GP_TIMER_0_BIT_ID                      17
+#define HIVE_GP_DEV_TC_GP_TIMER_1_BIT_ID                      18
+#define HIVE_GP_DEV_TC_MIPI_SOL_BIT_ID                        19
+#define HIVE_GP_DEV_TC_MIPI_EOL_BIT_ID                        20
+#define HIVE_GP_DEV_TC_MIPI_SOF_BIT_ID                        21
+#define HIVE_GP_DEV_TC_MIPI_EOF_BIT_ID                        22
+#define HIVE_GP_DEV_TC_INPSYS_SM                              23
+
+/* definitions for the gp_timer block */
+#define HIVE_GP_TIMER_0                                         0
+#define HIVE_GP_TIMER_1                                         1
+#define HIVE_GP_TIMER_2                                         2
+#define HIVE_GP_TIMER_3                                         3
+#define HIVE_GP_TIMER_4                                         4
+#define HIVE_GP_TIMER_5                                         5
+#define HIVE_GP_TIMER_6                                         6
+#define HIVE_GP_TIMER_7                                         7
+#define HIVE_GP_TIMER_NUM_COUNTERS                              8
+
+#define HIVE_GP_TIMER_IRQ_0                                     0
+#define HIVE_GP_TIMER_IRQ_1                                     1
+#define HIVE_GP_TIMER_NUM_IRQS                                  2
+
+#define HIVE_GP_TIMER_GPIO_0_BIT_ID                             0
+#define HIVE_GP_TIMER_GPIO_1_BIT_ID                             1
+#define HIVE_GP_TIMER_GPIO_2_BIT_ID                             2
+#define HIVE_GP_TIMER_GPIO_3_BIT_ID                             3
+#define HIVE_GP_TIMER_GPIO_4_BIT_ID                             4
+#define HIVE_GP_TIMER_GPIO_5_BIT_ID                             5
+#define HIVE_GP_TIMER_GPIO_6_BIT_ID                             6
+#define HIVE_GP_TIMER_GPIO_7_BIT_ID                             7
+#define HIVE_GP_TIMER_GPIO_8_BIT_ID                             8
+#define HIVE_GP_TIMER_GPIO_9_BIT_ID                             9
+#define HIVE_GP_TIMER_GPIO_10_BIT_ID                           10
+#define HIVE_GP_TIMER_GPIO_11_BIT_ID                           11
+#define HIVE_GP_TIMER_INP_SYS_IRQ                              12
+#define HIVE_GP_TIMER_ISEL_IRQ                                 13
+#define HIVE_GP_TIMER_IFMT_IRQ                                 14
+#define HIVE_GP_TIMER_SP_STRMON_IRQ                            15
+#define HIVE_GP_TIMER_SP_B_STRMON_IRQ                          16
+#define HIVE_GP_TIMER_ISP_STRMON_IRQ                           17
+#define HIVE_GP_TIMER_MOD_STRMON_IRQ                           18
+#define HIVE_GP_TIMER_ISP_BAMEM_ERROR_IRQ                      20
+#define HIVE_GP_TIMER_ISP_DMEM_ERROR_IRQ                       21
+#define HIVE_GP_TIMER_SP_ICACHE_MEM_ERROR_IRQ                  22
+#define HIVE_GP_TIMER_SP_DMEM_ERROR_IRQ                        23
+#define HIVE_GP_TIMER_SP_OUT_RUN_DP                            24
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I0         25
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I1         26
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I2         27
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I3         28
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I4         29
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I5         30
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I6         31
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I7         32
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I8         33
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I9         34
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I0_I10        35
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I1_I0         36
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I2_I0         37
+#define HIVE_GP_TIMER_SP_WIRE_DEBUG_LM_MSINK_RUN_I3_I0         38
+#define HIVE_GP_TIMER_ISP_OUT_RUN_DP                           39
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I0_I0        40
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I0_I1        41
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I1_I0        42
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I0        43
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I1        44
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I2        45
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I3        46
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I4        47
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I5        48
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I2_I6        49
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I3_I0        50
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I4_I0        51
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I5_I0        52
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I6_I0        53
+#define HIVE_GP_TIMER_ISP_WIRE_DEBUG_LM_MSINK_RUN_I7_I0        54
+#define HIVE_GP_TIMER_MIPI_SOL_BIT_ID                          55
+#define HIVE_GP_TIMER_MIPI_EOL_BIT_ID                          56
+#define HIVE_GP_TIMER_MIPI_SOF_BIT_ID                          57
+#define HIVE_GP_TIMER_MIPI_EOF_BIT_ID                          58
+#define HIVE_GP_TIMER_INPSYS_SM                                59
+
+/* port definitions for the streaming monitors */
+/* port definititions SP streaming monitor, monitors the status of streaming ports at the SP side of the streaming FIFO's */
+#define SP_STR_MON_PORT_SP2SIF            0
+#define SP_STR_MON_PORT_SIF2SP            1
+#define SP_STR_MON_PORT_SP2MC             2
+#define SP_STR_MON_PORT_MC2SP             3
+#define SP_STR_MON_PORT_SP2DMA            4
+#define SP_STR_MON_PORT_DMA2SP            5
+#define SP_STR_MON_PORT_SP2ISP            6
+#define SP_STR_MON_PORT_ISP2SP            7
+#define SP_STR_MON_PORT_SP2GPD            8
+#define SP_STR_MON_PORT_FA2SP             9
+#define SP_STR_MON_PORT_SP2ISYS          10
+#define SP_STR_MON_PORT_ISYS2SP          11
+#define SP_STR_MON_PORT_SP2PIFA          12
+#define SP_STR_MON_PORT_PIFA2SP          13
+#define SP_STR_MON_PORT_SP2PIFB          14
+#define SP_STR_MON_PORT_PIFB2SP          15
+
+#define SP_STR_MON_PORT_B_SP2GDC1         0
+#define SP_STR_MON_PORT_B_GDC12SP         1
+#define SP_STR_MON_PORT_B_SP2GDC2         2
+#define SP_STR_MON_PORT_B_GDC22SP         3
+
+/* previously used SP streaming monitor port identifiers, kept for backward compatibility */
+#define SP_STR_MON_PORT_SND_SIF           SP_STR_MON_PORT_SP2SIF
+#define SP_STR_MON_PORT_RCV_SIF           SP_STR_MON_PORT_SIF2SP
+#define SP_STR_MON_PORT_SND_MC            SP_STR_MON_PORT_SP2MC
+#define SP_STR_MON_PORT_RCV_MC            SP_STR_MON_PORT_MC2SP
+#define SP_STR_MON_PORT_SND_DMA           SP_STR_MON_PORT_SP2DMA
+#define SP_STR_MON_PORT_RCV_DMA           SP_STR_MON_PORT_DMA2SP
+#define SP_STR_MON_PORT_SND_ISP           SP_STR_MON_PORT_SP2ISP
+#define SP_STR_MON_PORT_RCV_ISP           SP_STR_MON_PORT_ISP2SP
+#define SP_STR_MON_PORT_SND_GPD           SP_STR_MON_PORT_SP2GPD
+#define SP_STR_MON_PORT_RCV_GPD           SP_STR_MON_PORT_FA2SP
+/* Deprecated */
+#define SP_STR_MON_PORT_SND_PIF           SP_STR_MON_PORT_SP2PIFA
+#define SP_STR_MON_PORT_RCV_PIF           SP_STR_MON_PORT_PIFA2SP
+#define SP_STR_MON_PORT_SND_PIFB          SP_STR_MON_PORT_SP2PIFB
+#define SP_STR_MON_PORT_RCV_PIFB          SP_STR_MON_PORT_PIFB2SP
+
+#define SP_STR_MON_PORT_SND_PIF_A         SP_STR_MON_PORT_SP2PIFA
+#define SP_STR_MON_PORT_RCV_PIF_A         SP_STR_MON_PORT_PIFA2SP
+#define SP_STR_MON_PORT_SND_PIF_B         SP_STR_MON_PORT_SP2PIFB
+#define SP_STR_MON_PORT_RCV_PIF_B         SP_STR_MON_PORT_PIFB2SP
+
+/* port definititions ISP streaming monitor, monitors the status of streaming ports at the ISP side of the streaming FIFO's */
+#define ISP_STR_MON_PORT_ISP2PIFA         0
+#define ISP_STR_MON_PORT_PIFA2ISP         1
+#define ISP_STR_MON_PORT_ISP2PIFB         2
+#define ISP_STR_MON_PORT_PIFB2ISP         3
+#define ISP_STR_MON_PORT_ISP2DMA          4
+#define ISP_STR_MON_PORT_DMA2ISP          5
+#define ISP_STR_MON_PORT_ISP2GDC1         6
+#define ISP_STR_MON_PORT_GDC12ISP         7
+#define ISP_STR_MON_PORT_ISP2GDC2         8
+#define ISP_STR_MON_PORT_GDC22ISP         9
+#define ISP_STR_MON_PORT_ISP2GPD         10
+#define ISP_STR_MON_PORT_FA2ISP          11
+#define ISP_STR_MON_PORT_ISP2SP          12
+#define ISP_STR_MON_PORT_SP2ISP          13
+
+/* previously used ISP streaming monitor port identifiers, kept for backward compatibility */
+#define ISP_STR_MON_PORT_SND_PIF_A       ISP_STR_MON_PORT_ISP2PIFA
+#define ISP_STR_MON_PORT_RCV_PIF_A       ISP_STR_MON_PORT_PIFA2ISP
+#define ISP_STR_MON_PORT_SND_PIF_B       ISP_STR_MON_PORT_ISP2PIFB
+#define ISP_STR_MON_PORT_RCV_PIF_B       ISP_STR_MON_PORT_PIFB2ISP
+#define ISP_STR_MON_PORT_SND_DMA         ISP_STR_MON_PORT_ISP2DMA
+#define ISP_STR_MON_PORT_RCV_DMA         ISP_STR_MON_PORT_DMA2ISP
+#define ISP_STR_MON_PORT_SND_GDC         ISP_STR_MON_PORT_ISP2GDC1
+#define ISP_STR_MON_PORT_RCV_GDC         ISP_STR_MON_PORT_GDC12ISP
+#define ISP_STR_MON_PORT_SND_GPD         ISP_STR_MON_PORT_ISP2GPD
+#define ISP_STR_MON_PORT_RCV_GPD         ISP_STR_MON_PORT_FA2ISP
+#define ISP_STR_MON_PORT_SND_SP          ISP_STR_MON_PORT_ISP2SP
+#define ISP_STR_MON_PORT_RCV_SP          ISP_STR_MON_PORT_SP2ISP
+
+/* port definititions MOD streaming monitor, monitors the status of streaming ports at the module side of the streaming FIFO's */
+
+#define MOD_STR_MON_PORT_PIFA2CELLS       0
+#define MOD_STR_MON_PORT_CELLS2PIFA       1
+#define MOD_STR_MON_PORT_PIFB2CELLS       2
+#define MOD_STR_MON_PORT_CELLS2PIFB       3
+#define MOD_STR_MON_PORT_SIF2SP           4
+#define MOD_STR_MON_PORT_SP2SIF           5
+#define MOD_STR_MON_PORT_MC2SP            6
+#define MOD_STR_MON_PORT_SP2MC            7
+#define MOD_STR_MON_PORT_DMA2ISP          8
+#define MOD_STR_MON_PORT_ISP2DMA          9
+#define MOD_STR_MON_PORT_DMA2SP          10
+#define MOD_STR_MON_PORT_SP2DMA          11
+#define MOD_STR_MON_PORT_GDC12CELLS      12
+#define MOD_STR_MON_PORT_CELLS2GDC1      13
+#define MOD_STR_MON_PORT_GDC22CELLS      14
+#define MOD_STR_MON_PORT_CELLS2GDC2      15
+
+#define MOD_STR_MON_PORT_SND_PIF_A        0
+#define MOD_STR_MON_PORT_RCV_PIF_A        1
+#define MOD_STR_MON_PORT_SND_PIF_B        2
+#define MOD_STR_MON_PORT_RCV_PIF_B        3
+#define MOD_STR_MON_PORT_SND_SIF          4
+#define MOD_STR_MON_PORT_RCV_SIF          5
+#define MOD_STR_MON_PORT_SND_MC           6
+#define MOD_STR_MON_PORT_RCV_MC           7
+#define MOD_STR_MON_PORT_SND_DMA2ISP      8
+#define MOD_STR_MON_PORT_RCV_DMA_FR_ISP   9
+#define MOD_STR_MON_PORT_SND_DMA2SP      10
+#define MOD_STR_MON_PORT_RCV_DMA_FR_SP   11
+#define MOD_STR_MON_PORT_SND_GDC         12
+#define MOD_STR_MON_PORT_RCV_GDC         13
+
+/* testbench signals:       */
+
+/* testbench GP adapter register ids  */
+#define HIVE_TESTBENCH_GPIO_DATA_OUT_REG_IDX                    0
+#define HIVE_TESTBENCH_GPIO_DIR_OUT_REG_IDX                     1
+#define HIVE_TESTBENCH_IRQ_REG_IDX                              2
+#define HIVE_TESTBENCH_SDRAM_WAKEUP_REG_IDX                     3
+#define HIVE_TESTBENCH_IDLE_REG_IDX                             4
+#define HIVE_TESTBENCH_GPIO_DATA_IN_REG_IDX                     5
+#define HIVE_TESTBENCH_MIPI_BFM_EN_REG_IDX                      6
+#define HIVE_TESTBENCH_CSI_CONFIG_REG_IDX                       7
+#define HIVE_TESTBENCH_DDR_STALL_EN_REG_IDX                     8
+
+#define HIVE_TESTBENCH_ISP_PMEM_ERROR_IRQ_REG_IDX               9
+#define HIVE_TESTBENCH_ISP_BAMEM_ERROR_IRQ_REG_IDX             10
+#define HIVE_TESTBENCH_ISP_DMEM_ERROR_IRQ_REG_IDX              11
+#define HIVE_TESTBENCH_SP_ICACHE_MEM_ERROR_IRQ_REG_IDX         12
+#define HIVE_TESTBENCH_SP_DMEM_ERROR_IRQ_REG_IDX               13
+
+/* Signal monitor input bit ids */
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_O_BIT_ID                0
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_1_BIT_ID                1
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_2_BIT_ID                2
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_3_BIT_ID                3
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_4_BIT_ID                4
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_5_BIT_ID                5
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_6_BIT_ID                6
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_7_BIT_ID                7
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_8_BIT_ID                8
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_9_BIT_ID                9
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_10_BIT_ID              10
+#define HIVE_TESTBENCH_SIG_MON_GPIO_PIN_11_BIT_ID              11
+#define HIVE_TESTBENCH_SIG_MON_IRQ_PIN_BIT_ID                  12
+#define HIVE_TESTBENCH_SIG_MON_SDRAM_WAKEUP_PIN_BIT_ID         13
+#define HIVE_TESTBENCH_SIG_MON_IDLE_PIN_BIT_ID                 14
+
+#define ISP2400_DEBUG_NETWORK    1
+
+#endif /* _hive_isp_css_defs_h__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/assert_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/assert_support.h
new file mode 100644
index 000000000..7382c0bbf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/assert_support.h
@@ -0,0 +1,74 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ASSERT_SUPPORT_H_INCLUDED__
+#define __ASSERT_SUPPORT_H_INCLUDED__
+
+/**
+ * The following macro can help to test the size of a struct at compile
+ * time rather than at run-time. It does not work for all compilers; see
+ * below.
+ *
+ * Depending on the value of 'condition', the following macro is expanded to:
+ * - condition==true:
+ *     an expression containing an array declaration with negative size,
+ *     usually resulting in a compilation error
+ * - condition==false:
+ *     (void) 1; // C statement with no effect
+ *
+ * example:
+ *  COMPILATION_ERROR_IF( sizeof(struct host_sp_queues) != SIZE_OF_HOST_SP_QUEUES_STRUCT);
+ *
+ * verify that the macro indeed triggers a compilation error with your compiler:
+ *  COMPILATION_ERROR_IF( sizeof(struct host_sp_queues) != (sizeof(struct host_sp_queues)+1) );
+ *
+ * Not all compilers will trigger an error with this macro; use a search engine to search for
+ * BUILD_BUG_ON to find other methods.
+ */
+#define COMPILATION_ERROR_IF(condition) ((void)sizeof(char[1 - 2 * !!(condition)]))
+
+/* Compile time assertion */
+#ifndef CT_ASSERT
+#define CT_ASSERT(cnd) ((void)sizeof(char[(cnd) ? 1 :  -1]))
+#endif /* CT_ASSERT */
+
+#include <linux/bug.h>
+
+/* TODO: it would be cleaner to use this:
+ * #define assert(cnd) BUG_ON(cnd)
+ * but that causes many compiler warnings (==errors) under Android
+ * because it seems that the BUG_ON() macro is not seen as a check by
+ * gcc like the BUG() macro is. */
+#define assert(cnd) \
+	do { \
+		if (!(cnd)) \
+			BUG(); \
+	} while (0)
+
+#ifndef PIPE_GENERATION
+/* Deprecated OP___assert, this is still used in ~1000 places
+ * in the code. This will be removed over time.
+ * The implementation for the pipe generation tool is in see support.isp.h */
+#define OP___assert(cnd) assert(cnd)
+
+static inline void compile_time_assert(unsigned int cond)
+{
+	/* Call undefined function if cond is false */
+	void _compile_time_assert(void);
+	if (!cond) _compile_time_assert();
+}
+#endif /* PIPE_GENERATION */
+
+#endif /* __ASSERT_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/bitop_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/bitop_support.h
new file mode 100644
index 000000000..29f14e900
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/bitop_support.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __BITOP_SUPPORT_H_INCLUDED__
+#define __BITOP_SUPPORT_H_INCLUDED__
+
+#define bitop_setbit(a, b) ((a) |= (1UL << (b)))
+
+#define bitop_getbit(a, b) (((a) & (1UL << (b))) != 0)
+
+#define bitop_clearbit(a, b) ((a) &= ~(1UL << (b)))
+
+#endif /* __BITOP_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/csi_rx.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/csi_rx.h
new file mode 100644
index 000000000..4602885d5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/csi_rx.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_H_INCLUDED__
+#define __CSI_RX_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ * - system and cell agnostic interfaces, constants and identifiers
+ * - public:  system agnostic, cell specific interfaces
+ * - private: system dependent, cell specific interfaces &
+ *   inline implementations
+ * - global:  system specific constants and identifiers
+ * - local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "csi_rx_local.h"
+
+#ifndef __INLINE_CSI_RX__
+#include "csi_rx_public.h"
+#else  /* __INLINE_CSI_RX__ */
+#include "csi_rx_private.h"
+#endif /* __INLINE_CSI_RX__ */
+
+#endif /* __CSI_RX_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/debug.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/debug.h
new file mode 100644
index 000000000..0f8195ba8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/debug.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DEBUG_H_INCLUDED__
+#define __DEBUG_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the DMA device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ *
+ */
+
+#include "system_local.h"
+#include "debug_local.h"
+
+#ifndef __INLINE_DEBUG__
+#define STORAGE_CLASS_DEBUG_H extern
+#define STORAGE_CLASS_DEBUG_C
+#include "debug_public.h"
+#else  /* __INLINE_DEBUG__ */
+#define STORAGE_CLASS_DEBUG_H static inline
+#define STORAGE_CLASS_DEBUG_C static inline
+#include "debug_private.h"
+#endif /* __INLINE_DEBUG__ */
+
+#endif /* __DEBUG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/device_access/device_access.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/device_access/device_access.h
new file mode 100644
index 000000000..492f9e26c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/device_access/device_access.h
@@ -0,0 +1,178 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __DEVICE_ACCESS_H_INCLUDED__
+#define __DEVICE_ACCESS_H_INCLUDED__
+
+/*!
+ * \brief
+ * Define the public interface for physical system
+ * access functions to SRAM and registers. Access
+ * types are limited to those defined in <stdint.h>
+ * All accesses are aligned
+ *
+ * The address representation is private to the system
+ * and represented as/stored in "hrt_address".
+ *
+ * The system global address can differ by an offset;
+ * The device base address. This offset must be added
+ * by the implementation of the access function
+ *
+ * "store" is a transfer to the device
+ * "load" is a transfer from the device
+ */
+
+#include <type_support.h>
+
+/*
+ * User provided file that defines the system address types:
+ *	- hrt_address	a type that can hold the (sub)system address range
+ */
+#include "system_local.h"
+/*
+ * We cannot assume that the global system address size is the size of
+ * a pointer because a (say) 64-bit host can be simulated in a 32-bit
+ * environment. Only if the host environment is modelled as on the target
+ * we could use a pointer. Even then, prototyping may need to be done
+ * before the target environment is available. AS we cannot wait for that
+ * we are stuck with integer addresses
+ */
+
+/*typedef	char *sys_address;*/
+typedef	hrt_address		sys_address;
+
+/*! Set the (sub)system base address
+
+ \param	base_addr[in]		The offset on which the (sub)system is located
+							in the global address map
+
+ \return none,
+ */
+void device_set_base_address(
+    const sys_address		base_addr);
+
+/*! Get the (sub)system base address
+
+ \return base_address,
+ */
+sys_address device_get_base_address(void);
+
+/*! Read an 8-bit value from a device register or memory in the device
+
+ \param	addr[in]			Local address
+
+ \return device[addr]
+ */
+uint8_t ia_css_device_load_uint8(
+    const hrt_address		addr);
+
+/*! Read a 16-bit value from a device register or memory in the device
+
+ \param	addr[in]			Local address
+
+ \return device[addr]
+ */
+uint16_t ia_css_device_load_uint16(
+    const hrt_address		addr);
+
+/*! Read a 32-bit value from a device register or memory in the device
+
+ \param	addr[in]			Local address
+
+ \return device[addr]
+ */
+uint32_t ia_css_device_load_uint32(
+    const hrt_address		addr);
+
+/*! Read a 64-bit value from a device register or memory in the device
+
+ \param	addr[in]			Local address
+
+ \return device[addr]
+ */
+uint64_t ia_css_device_load_uint64(
+    const hrt_address		addr);
+
+/*! Write an 8-bit value to a device register or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[in]			value
+
+ \return none, device[addr] = value
+ */
+void ia_css_device_store_uint8(
+    const hrt_address		addr,
+    const uint8_t			data);
+
+/*! Write a 16-bit value to a device register or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[in]			value
+
+ \return none, device[addr] = value
+ */
+void ia_css_device_store_uint16(
+    const hrt_address		addr,
+    const uint16_t			data);
+
+/*! Write a 32-bit value to a device register or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[in]			value
+
+ \return none, device[addr] = value
+ */
+void ia_css_device_store_uint32(
+    const hrt_address		addr,
+    const uint32_t			data);
+
+/*! Write a 64-bit value to a device register or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[in]			value
+
+ \return none, device[addr] = value
+ */
+void ia_css_device_store_uint64(
+    const hrt_address		addr,
+    const uint64_t			data);
+
+/*! Read an array of bytes from device registers or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[out]			pointer to the destination array
+ \param	size[in]			number of bytes to read
+
+ \return none
+ */
+void ia_css_device_load(
+    const hrt_address		addr,
+    void					*data,
+    const size_t			size);
+
+/*! Write an array of bytes to device registers or memory in the device
+
+ \param	addr[in]			Local address
+ \param	data[in]			pointer to the source array
+ \param	size[in]			number of bytes to write
+
+ \return none
+ */
+void ia_css_device_store(
+    const hrt_address		addr,
+    const void				*data,
+    const size_t			size);
+
+#endif /* __DEVICE_ACCESS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/dma.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/dma.h
new file mode 100644
index 000000000..2f5ebfcd7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/dma.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DMA_H_INCLUDED__
+#define __DMA_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the DMA device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ *
+ */
+
+#include "system_local.h"
+#include "dma_local.h"
+
+#ifndef __INLINE_DMA__
+#define STORAGE_CLASS_DMA_H extern
+#define STORAGE_CLASS_DMA_C
+#include "dma_public.h"
+#else  /* __INLINE_DMA__ */
+#define STORAGE_CLASS_DMA_H static inline
+#define STORAGE_CLASS_DMA_C static inline
+#include "dma_private.h"
+#endif /* __INLINE_DMA__ */
+
+#endif /* __DMA_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/event_fifo.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/event_fifo.h
new file mode 100644
index 000000000..0a085abd3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/event_fifo.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __EVENT_FIFO_H
+#define __EVENT_FIFO_H
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the IRQ device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "event_fifo_local.h"
+
+#ifndef __INLINE_EVENT__
+#define STORAGE_CLASS_EVENT_H extern
+#define STORAGE_CLASS_EVENT_C
+#include "event_fifo_public.h"
+#else  /* __INLINE_EVENT__ */
+#define STORAGE_CLASS_EVENT_H static inline
+#define STORAGE_CLASS_EVENT_C static inline
+#include "event_fifo_private.h"
+#endif /* __INLINE_EVENT__ */
+
+#endif /* __EVENT_FIFO_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/fifo_monitor.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/fifo_monitor.h
new file mode 100644
index 000000000..19a1bdd91
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/fifo_monitor.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __FIFO_MONITOR_H_INCLUDED__
+#define __FIFO_MONITOR_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "fifo_monitor_local.h"
+
+#ifndef __INLINE_FIFO_MONITOR__
+#define STORAGE_CLASS_FIFO_MONITOR_H extern
+#define STORAGE_CLASS_FIFO_MONITOR_C
+#include "fifo_monitor_public.h"
+#else  /* __INLINE_FIFO_MONITOR__ */
+#define STORAGE_CLASS_FIFO_MONITOR_H static inline
+#define STORAGE_CLASS_FIFO_MONITOR_C static inline
+#include "fifo_monitor_private.h"
+#endif /* __INLINE_FIFO_MONITOR__ */
+
+#endif /* __FIFO_MONITOR_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/gdc_device.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gdc_device.h
new file mode 100644
index 000000000..4ed57fb45
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gdc_device.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GDC_DEVICE_H_INCLUDED__
+#define __GDC_DEVICE_H_INCLUDED__
+
+/* The file gdc.h already exists */
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the GDC device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "gdc_local.h"
+
+#ifndef __INLINE_GDC__
+#define STORAGE_CLASS_GDC_H extern
+#define STORAGE_CLASS_GDC_C
+#include "gdc_public.h"
+#else  /* __INLINE_GDC__ */
+#define STORAGE_CLASS_GDC_H static inline
+#define STORAGE_CLASS_GDC_C static inline
+#include "gdc_private.h"
+#endif /* __INLINE_GDC__ */
+
+#endif /* __GDC_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_device.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_device.h
new file mode 100644
index 000000000..d122bdeae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_device.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_DEVICE_H_INCLUDED__
+#define __GP_DEVICE_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "gp_device_local.h"
+
+#ifndef __INLINE_GP_DEVICE__
+#define STORAGE_CLASS_GP_DEVICE_H extern
+#define STORAGE_CLASS_GP_DEVICE_C
+#include "gp_device_public.h"
+#else  /* __INLINE_GP_DEVICE__ */
+#define STORAGE_CLASS_GP_DEVICE_H static inline
+#define STORAGE_CLASS_GP_DEVICE_C static inline
+#include "gp_device_private.h"
+#endif /* __INLINE_GP_DEVICE__ */
+
+#endif /* __GP_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_timer.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_timer.h
new file mode 100644
index 000000000..0b7e92b96
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gp_timer.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_TIMER_H_INCLUDED__
+#define __GP_TIMER_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"    /*GP_TIMER_BASE address */
+#include "gp_timer_local.h"  /*GP_TIMER register offsets */
+
+#ifndef __INLINE_GP_TIMER__
+#define STORAGE_CLASS_GP_TIMER_H extern
+#define STORAGE_CLASS_GP_TIMER_C
+#include "gp_timer_public.h"   /* functions*/
+#else  /* __INLINE_GP_TIMER__ */
+#define STORAGE_CLASS_GP_TIMER_H static inline
+#define STORAGE_CLASS_GP_TIMER_C static inline
+#include "gp_timer_private.h"  /* inline functions*/
+#endif /* __INLINE_GP_TIMER__ */
+
+#endif /* __GP_TIMER_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/gpio.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gpio.h
new file mode 100644
index 000000000..6f16ca77c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/gpio.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GPIO_H_INCLUDED__
+#define __GPIO_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "gpio_local.h"
+
+#ifndef __INLINE_GPIO__
+#define STORAGE_CLASS_GPIO_H extern
+#define STORAGE_CLASS_GPIO_C
+#include "gpio_public.h"
+#else  /* __INLINE_GPIO__ */
+#define STORAGE_CLASS_GPIO_H static inline
+#define STORAGE_CLASS_GPIO_C static inline
+#include "gpio_private.h"
+#endif /* __INLINE_GPIO__ */
+
+#endif /* __GPIO_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/hmem.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/hmem.h
new file mode 100644
index 000000000..898facd7b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/hmem.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __HMEM_H_INCLUDED__
+#define __HMEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the HMEM device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "hmem_local.h"
+
+#ifndef __INLINE_HMEM__
+#define STORAGE_CLASS_HMEM_H extern
+#define STORAGE_CLASS_HMEM_C
+#include "hmem_public.h"
+#else  /* __INLINE_HMEM__ */
+#define STORAGE_CLASS_HMEM_H static inline
+#define STORAGE_CLASS_HMEM_C static inline
+#include "hmem_private.h"
+#endif /* __INLINE_HMEM__ */
+
+#endif /* __HMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/csi_rx_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/csi_rx_public.h
new file mode 100644
index 000000000..3d6621f2f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/csi_rx_public.h
@@ -0,0 +1,136 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_PUBLIC_H_INCLUDED__
+#define __CSI_RX_PUBLIC_H_INCLUDED__
+
+#ifdef ISP2401
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the csi rx frontend state.
+ * Get the state of the csi rx frontend regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the csi rx fe controller.
+ * @param[out]	state	Point to the register-state.
+ */
+void csi_rx_fe_ctrl_get_state(
+    const csi_rx_frontend_ID_t ID,
+    csi_rx_fe_ctrl_state_t *state);
+/**
+ * @brief Dump the csi rx frontend state.
+ * Dump the state of the csi rx frontend regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the csi rx fe controller.
+ * @param[in]	state	Point to the register-state.
+ */
+void csi_rx_fe_ctrl_dump_state(
+    const csi_rx_frontend_ID_t ID,
+    csi_rx_fe_ctrl_state_t *state);
+/**
+ * @brief Get the state of the csi rx fe dlane.
+ * Get the state of the register set per dlane process.
+ *
+ * @param[in]	id			The global unique ID of the input-buffer controller.
+ * @param[in]	lane		The lane ID.
+ * @param[out]	state		Point to the dlane state.
+ */
+void csi_rx_fe_ctrl_get_dlane_state(
+    const csi_rx_frontend_ID_t ID,
+    const u32 lane,
+    csi_rx_fe_ctrl_lane_t *dlane_state);
+/**
+ * @brief Get the csi rx backend state.
+ * Get the state of the csi rx backend regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the csi rx be controller.
+ * @param[out]	state	Point to the register-state.
+ */
+void csi_rx_be_ctrl_get_state(
+    const csi_rx_backend_ID_t ID,
+    csi_rx_be_ctrl_state_t *state);
+/**
+ * @brief Dump the csi rx backend state.
+ * Dump the state of the csi rx backend regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the csi rx be controller.
+ * @param[in]	state	Point to the register-state.
+ */
+void csi_rx_be_ctrl_dump_state(
+    const csi_rx_backend_ID_t ID,
+    csi_rx_be_ctrl_state_t *state);
+/* end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Load the value of the register of the csi rx fe.
+ *
+ * @param[in]	ID	The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg	The offset address of the register.
+ *
+ * @return the value of the register.
+ */
+hrt_data csi_rx_fe_ctrl_reg_load(
+    const csi_rx_frontend_ID_t ID,
+    const hrt_address reg);
+/**
+ * @brief Store a value to the register.
+ * Store a value to the registe of the csi rx fe.
+ *
+ * @param[in]	ID		The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg		The offset address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+void csi_rx_fe_ctrl_reg_store(
+    const csi_rx_frontend_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value);
+/**
+ * @brief Load the register value.
+ * Load the value of the register of the csirx be.
+ *
+ * @param[in]	ID	The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg	The offset address of the register.
+ *
+ * @return the value of the register.
+ */
+hrt_data csi_rx_be_ctrl_reg_load(
+    const csi_rx_backend_ID_t ID,
+    const hrt_address reg);
+/**
+ * @brief Store a value to the register.
+ * Store a value to the registe of the csi rx be.
+ *
+ * @param[in]	ID		The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg		The offset address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+void csi_rx_be_ctrl_reg_store(
+    const csi_rx_backend_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value);
+/* end of DLI */
+#endif /* ISP2401 */
+#endif /* __CSI_RX_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/debug_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/debug_public.h
new file mode 100644
index 000000000..5660bd422
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/debug_public.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DEBUG_PUBLIC_H_INCLUDED__
+#define __DEBUG_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#include <ia_css_types.h>
+#include "system_local.h"
+
+/*! brief
+ *
+ * Simple queuing trace buffer for debug data
+ * instantiatable in SP DMEM
+ *
+ * The buffer has a remote and a local store
+ * which contain duplicate data (when in sync).
+ * The buffers are automatically synched when the
+ * user dequeues, or manualy using the synch function
+ *
+ * An alternative (storage efficient) implementation
+ * could manage the buffers to contain unique data
+ *
+ * The buffer empty status is computed from local
+ * state which does not reflect the presence of data
+ * in the remote buffer (unless the alternative
+ * implementation is followed)
+ */
+
+typedef struct debug_data_s		debug_data_t;
+typedef struct debug_data_ddr_s	debug_data_ddr_t;
+
+extern debug_data_t				*debug_data_ptr;
+extern hrt_address				debug_buffer_address;
+extern ia_css_ptr				debug_buffer_ddr_address;
+
+/*! Check the empty state of the local debug data buffer
+
+ \return isEmpty(buffer)
+ */
+STORAGE_CLASS_DEBUG_H bool is_debug_buffer_empty(void);
+
+/*! Dequeue a token from the debug data buffer
+
+ \return isEmpty(buffer)?0:buffer[head]
+ */
+STORAGE_CLASS_DEBUG_H hrt_data debug_dequeue(void);
+
+/*! Synchronise the remote buffer to the local buffer
+
+ \return none
+ */
+STORAGE_CLASS_DEBUG_H void debug_synch_queue(void);
+
+/*! Synchronise the remote buffer to the local buffer
+
+ \return none
+ */
+STORAGE_CLASS_DEBUG_H void debug_synch_queue_isp(void);
+
+/*! Synchronise the remote buffer to the local buffer
+
+ \return none
+ */
+STORAGE_CLASS_DEBUG_H void debug_synch_queue_ddr(void);
+
+/*! Set the offset/address of the (remote) debug buffer
+
+ \return none
+ */
+void debug_buffer_init(
+    const hrt_address		addr);
+
+/*! Set the offset/address of the (remote) debug buffer
+
+ \return none
+ */
+void debug_buffer_ddr_init(
+    const ia_css_ptr		addr);
+
+/*! Set the (remote) operating mode of the debug buffer
+
+ \return none
+ */
+void debug_buffer_setmode(
+    const debug_buf_mode_t	mode);
+
+#endif /* __DEBUG_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/dma_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/dma_public.h
new file mode 100644
index 000000000..a23cbc9a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/dma_public.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __DMA_PUBLIC_H_INCLUDED__
+#define __DMA_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+typedef struct dma_state_s		dma_state_t;
+
+/*! Read the control registers of DMA[ID]
+
+ \param	ID[in]				DMA identifier
+ \param	state[out]			input formatter state structure
+
+ \return none, state = DMA[ID].state
+ */
+void dma_get_state(
+    const dma_ID_t		ID,
+    dma_state_t			*state);
+
+/*! Write to a control register of DMA[ID]
+
+ \param	ID[in]				DMA identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, DMA[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_DMA_H void dma_reg_store(
+    const dma_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value);
+
+/*! Read from a control register of DMA[ID]
+
+ \param	ID[in]				DMA identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return DMA[ID].ctrl[reg]
+ */
+STORAGE_CLASS_DMA_H hrt_data dma_reg_load(
+    const dma_ID_t		ID,
+    const unsigned int	reg);
+
+/*! Set maximum burst size of DMA[ID]
+
+ \param ID[in]				DMA identifier
+ \param conn[in]			Connection to set max burst size for
+ \param max_burst_size[in]		Maximum burst size in words
+
+ \return none
+*/
+void
+dma_set_max_burst_size(
+    dma_ID_t		ID,
+    dma_connection		conn,
+    uint32_t		max_burst_size);
+
+#endif /* __DMA_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/event_fifo_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/event_fifo_public.h
new file mode 100644
index 000000000..22f1875f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/event_fifo_public.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __EVENT_FIFO_PUBLIC_H
+#define __EVENT_FIFO_PUBLIC_H
+
+#include <type_support.h>
+#include "system_local.h"
+
+/*! Blocking read from an event source EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+
+ \return none, dequeue(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H void event_wait_for(
+    const event_ID_t		ID);
+
+/*! Conditional blocking wait for an event source EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+ \param	cnd[in]				predicate
+
+ \return none, if(cnd) dequeue(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H void cnd_event_wait_for(
+    const event_ID_t		ID,
+    const bool				cnd);
+
+/*! Blocking read from an event source EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+
+ \return dequeue(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H hrt_data event_receive_token(
+    const event_ID_t		ID);
+
+/*! Blocking write to an event sink EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+ \param	token[in]			token to be written on the event
+
+ \return none, enqueue(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H void event_send_token(
+    const event_ID_t		ID,
+    const hrt_data			token);
+
+/*! Query an event source EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+
+ \return !isempty(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H bool is_event_pending(
+    const event_ID_t		ID);
+
+/*! Query an event sink EVENT[ID]
+
+ \param	ID[in]				EVENT identifier
+
+ \return !isfull(event_queue[ID])
+ */
+STORAGE_CLASS_EVENT_H bool can_event_send_token(
+    const event_ID_t		ID);
+
+#endif /* __EVENT_FIFO_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/fifo_monitor_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/fifo_monitor_public.h
new file mode 100644
index 000000000..7c1c3d2f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/fifo_monitor_public.h
@@ -0,0 +1,111 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __FIFO_MONITOR_PUBLIC_H_INCLUDED__
+#define __FIFO_MONITOR_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+typedef struct fifo_channel_state_s		fifo_channel_state_t;
+typedef struct fifo_switch_state_s		fifo_switch_state_t;
+typedef struct fifo_monitor_state_s		fifo_monitor_state_t;
+
+/*! Set a fifo switch multiplex
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	switch_id[in]		fifo switch identifier
+ \param	sel[in]				fifo switch selector
+
+ \return none, fifo_switch[switch_id].sel = sel
+ */
+STORAGE_CLASS_FIFO_MONITOR_H void fifo_switch_set(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id,
+    const hrt_data				sel);
+
+/*! Get a fifo switch multiplex
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	switch_id[in]		fifo switch identifier
+
+ \return fifo_switch[switch_id].sel
+ */
+STORAGE_CLASS_FIFO_MONITOR_H hrt_data fifo_switch_get(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id);
+
+/*! Read the state of FIFO_MONITOR[ID]
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	state[out]			fifo monitor state structure
+
+ \return none, state = FIFO_MONITOR[ID].state
+ */
+void fifo_monitor_get_state(
+    const fifo_monitor_ID_t		ID,
+    fifo_monitor_state_t		*state);
+
+/*! Read the state of a fifo channel
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	channel_id[in]		fifo channel identifier
+ \param	state[out]			fifo channel state structure
+
+ \return none, state = fifo_channel[channel_id].state
+ */
+void fifo_channel_get_state(
+    const fifo_monitor_ID_t		ID,
+    const fifo_channel_t		channel_id,
+    fifo_channel_state_t		*state);
+
+/*! Read the state of a fifo switch
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	switch_id[in]		fifo switch identifier
+ \param	state[out]			fifo switch state structure
+
+ \return none, state = fifo_switch[switch_id].state
+ */
+void fifo_switch_get_state(
+    const fifo_monitor_ID_t		ID,
+    const fifo_switch_t			switch_id,
+    fifo_switch_state_t			*state);
+
+/*! Write to a control register of FIFO_MONITOR[ID]
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, FIFO_MONITOR[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_FIFO_MONITOR_H void fifo_monitor_reg_store(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg,
+    const hrt_data				value);
+
+/*! Read from a control register of FIFO_MONITOR[ID]
+
+ \param	ID[in]				FIFO_MONITOR identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return FIFO_MONITOR[ID].ctrl[reg]
+ */
+STORAGE_CLASS_FIFO_MONITOR_H hrt_data fifo_monitor_reg_load(
+    const fifo_monitor_ID_t		ID,
+    const unsigned int			reg);
+
+#endif /* __FIFO_MONITOR_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gdc_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gdc_public.h
new file mode 100644
index 000000000..385b79254
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gdc_public.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GDC_PUBLIC_H_INCLUDED__
+#define __GDC_PUBLIC_H_INCLUDED__
+
+/*! Write the bicubic interpolation table of GDC[ID]
+
+ \param	ID[in]				GDC identifier
+ \param data[in]			The data matrix to be written
+
+ \pre
+	- data must point to a matrix[4][HRT_GDC_N]
+
+ \implementation dependent
+	- The value of "HRT_GDC_N" is device specific
+	- The LUT should not be partially written
+	- The LUT format is a quadri-phase interpolation
+	  table. The layout is device specific
+	- The range of the values data[n][m] is device
+	  specific
+
+ \return none, GDC[ID].lut[0...3][0...HRT_GDC_N-1] = data
+ */
+void gdc_lut_store(
+    const gdc_ID_t		ID,
+    const int			data[4][HRT_GDC_N]);
+
+/*! Convert the bicubic interpolation table of GDC[ID] to the ISP-specific format
+
+ \param	ID[in]				GDC identifier
+ \param in_lut[in]			The data matrix to be converted
+ \param out_lut[out]			The data matrix as the output of conversion
+ */
+void gdc_lut_convert_to_isp_format(
+    const int in_lut[4][HRT_GDC_N],
+    int out_lut[4][HRT_GDC_N]);
+
+/*! Return the integer representation of 1.0 of GDC[ID]
+
+ \param	ID[in]				GDC identifier
+
+ \return unity
+ */
+int gdc_get_unity(
+    const gdc_ID_t		ID);
+
+#endif /* __GDC_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_device_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_device_public.h
new file mode 100644
index 000000000..f017742d9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_device_public.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_DEVICE_PUBLIC_H_INCLUDED__
+#define __GP_DEVICE_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+typedef struct gp_device_state_s		gp_device_state_t;
+
+/*! Read the state of GP_DEVICE[ID]
+
+ \param	ID[in]				GP_DEVICE identifier
+ \param	state[out]			gp device state structure
+
+ \return none, state = GP_DEVICE[ID].state
+ */
+void gp_device_get_state(
+    const gp_device_ID_t		ID,
+    gp_device_state_t			*state);
+
+/*! Write to a control register of GP_DEVICE[ID]
+
+ \param	ID[in]				GP_DEVICE identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return none, GP_DEVICE[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_GP_DEVICE_H void gp_device_reg_store(
+    const gp_device_ID_t	ID,
+    const unsigned int		reg_addr,
+    const hrt_data			value);
+
+/*! Read from a control register of GP_DEVICE[ID]
+
+ \param	ID[in]				GP_DEVICE identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return GP_DEVICE[ID].ctrl[reg]
+ */
+STORAGE_CLASS_GP_DEVICE_H hrt_data gp_device_reg_load(
+    const gp_device_ID_t	ID,
+    const hrt_address	reg_addr);
+
+#endif /* __GP_DEVICE_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_timer_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_timer_public.h
new file mode 100644
index 000000000..13baf7236
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gp_timer_public.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GP_TIMER_PUBLIC_H_INCLUDED__
+#define __GP_TIMER_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+/*! initialize mentioned timer
+param ID		timer_id
+*/
+extern void
+gp_timer_init(gp_timer_ID_t ID);
+
+/*! read timer value for (platform selected)selected timer.
+param ID		timer_id
+ \return uint32_t	32 bit timer value
+*/
+extern uint32_t
+gp_timer_read(gp_timer_ID_t ID);
+
+#endif /* __GP_TIMER_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gpio_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gpio_public.h
new file mode 100644
index 000000000..13df9b57a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/gpio_public.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __GPIO_PUBLIC_H_INCLUDED__
+#define __GPIO_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+/*! Write to a control register of GPIO[ID]
+
+ \param	ID[in]				GPIO identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return none, GPIO[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_GPIO_H void gpio_reg_store(
+    const gpio_ID_t	ID,
+    const unsigned int		reg_addr,
+    const hrt_data			value);
+
+/*! Read from a control register of GPIO[ID]
+
+ \param	ID[in]				GPIO identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return GPIO[ID].ctrl[reg]
+ */
+STORAGE_CLASS_GPIO_H hrt_data gpio_reg_load(
+    const gpio_ID_t	ID,
+    const unsigned int		reg_addr);
+
+#endif /* __GPIO_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/hmem_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/hmem_public.h
new file mode 100644
index 000000000..8d271fb84
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/hmem_public.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __HMEM_PUBLIC_H_INCLUDED__
+#define __HMEM_PUBLIC_H_INCLUDED__
+
+#include <linux/types.h>		/* size_t */
+
+/*! Return the size of HMEM[ID]
+
+ \param	ID[in]				HMEM identifier
+
+ \Note: The size is the byte size of the area it occupies
+		in the address map. I.e. disregarding internal structure
+
+ \return sizeof(HMEM[ID])
+ */
+STORAGE_CLASS_HMEM_H size_t sizeof_hmem(
+    const hmem_ID_t		ID);
+
+#endif /* __HMEM_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/input_formatter_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/input_formatter_public.h
new file mode 100644
index 000000000..81dc58640
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/input_formatter_public.h
@@ -0,0 +1,116 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_FORMATTER_PUBLIC_H_INCLUDED__
+#define __INPUT_FORMATTER_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#include "system_local.h"
+
+/*! Reset INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+
+ \return none, reset(INPUT_FORMATTER[ID])
+ */
+void input_formatter_rst(
+    const input_formatter_ID_t		ID);
+
+/*! Set the blocking mode of INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	enable[in]			blocking enable flag
+
+ \use
+	- In HW, the capture unit will deliver an infinite stream of frames,
+	  the input formatter will synchronise on the first SOF. In simulation
+	  there are only a fixed number of frames, presented only once. By
+	  enabling blocking the inputformatter will wait on the first presented
+	  frame, thus avoiding race in the simulation setup.
+
+ \return none, INPUT_FORMATTER[ID].blocking_mode = enable
+ */
+void input_formatter_set_fifo_blocking_mode(
+    const input_formatter_ID_t		ID,
+    const bool						enable);
+
+/*! Return the data alignment of INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+
+ \return alignment(INPUT_FORMATTER[ID].data)
+ */
+unsigned int input_formatter_get_alignment(
+    const input_formatter_ID_t		ID);
+
+/*! Read the source switch state into INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	state[out]			input formatter switch state structure
+
+ \return none, state = INPUT_FORMATTER[ID].switch_state
+ */
+void input_formatter_get_switch_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_switch_state_t	*state);
+
+/*! Read the control registers of INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	state[out]			input formatter state structure
+
+ \return none, state = INPUT_FORMATTER[ID].state
+ */
+void input_formatter_get_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_state_t			*state);
+
+/*! Read the control registers of bin copy INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	state[out]			input formatter state structure
+
+ \return none, state = INPUT_FORMATTER[ID].state
+ */
+void input_formatter_bin_get_state(
+    const input_formatter_ID_t		ID,
+    input_formatter_bin_state_t		*state);
+
+/*! Write to a control register of INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return none, INPUT_FORMATTER[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_INPUT_FORMATTER_H void input_formatter_reg_store(
+    const input_formatter_ID_t	ID,
+    const hrt_address		reg_addr,
+    const hrt_data				value);
+
+/*! Read from a control register of INPUT_FORMATTER[ID]
+
+ \param	ID[in]				INPUT_FORMATTER identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return INPUT_FORMATTER[ID].ctrl[reg]
+ */
+STORAGE_CLASS_INPUT_FORMATTER_H hrt_data input_formatter_reg_load(
+    const input_formatter_ID_t	ID,
+    const unsigned int			reg_addr);
+
+#endif /* __INPUT_FORMATTER_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/irq_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/irq_public.h
new file mode 100644
index 000000000..d335e7b0a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/irq_public.h
@@ -0,0 +1,183 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IRQ_PUBLIC_H_INCLUDED__
+#define __IRQ_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#include "system_local.h"
+
+/*! Read the control registers of IRQ[ID]
+
+ \param	ID[in]				IRQ identifier
+ \param	state[out]			irq controller state structure
+
+ \return none, state = IRQ[ID].state
+ */
+void irq_controller_get_state(const irq_ID_t ID,
+			      struct irq_controller_state *state);
+
+/*! Write to a control register of IRQ[ID]
+
+ \param	ID[in]				IRQ identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, IRQ[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_IRQ_H void irq_reg_store(
+    const irq_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value);
+
+/*! Read from a control register of IRQ[ID]
+
+ \param	ID[in]				IRQ identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return IRQ[ID].ctrl[reg]
+ */
+STORAGE_CLASS_IRQ_H hrt_data irq_reg_load(
+    const irq_ID_t		ID,
+    const unsigned int	reg);
+
+/*! Enable an IRQ channel of IRQ[ID] with a mode
+
+ \param	ID[in]				IRQ (device) identifier
+ \param	irq[in]				IRQ (channel) identifier
+
+ \return none, enable(IRQ[ID].channel[irq_ID])
+ */
+void irq_enable_channel(
+    const irq_ID_t				ID,
+    const unsigned int			irq_ID);
+
+/*! Enable pulse interrupts for IRQ[ID] with a mode
+
+ \param	ID[in]				IRQ (device) identifier
+ \param	enable				enable/disable pulse interrupts
+
+ \return none
+ */
+void irq_enable_pulse(
+    const irq_ID_t	ID,
+    bool			pulse);
+
+/*! Disable an IRQ channel of IRQ[ID]
+
+ \param	ID[in]				IRQ (device) identifier
+ \param	irq[in]				IRQ (channel) identifier
+
+ \return none, disable(IRQ[ID].channel[irq_ID])
+ */
+void irq_disable_channel(
+    const irq_ID_t				ID,
+    const unsigned int			irq);
+
+/*! Clear the state of all IRQ channels of IRQ[ID]
+
+ \param	ID[in]				IRQ (device) identifier
+
+ \return none, clear(IRQ[ID].channel[])
+ */
+void irq_clear_all(
+    const irq_ID_t				ID);
+
+/*! Return the ID of a signalling IRQ channel of IRQ[ID]
+
+ \param	ID[in]				IRQ (device) identifier
+ \param irq_id[out]			active IRQ (channel) identifier
+
+ \Note: This function operates as strtok(), based on the return
+  state the user is informed if there are additional signalling
+  channels
+
+ \return state(IRQ[ID])
+ */
+enum hrt_isp_css_irq_status irq_get_channel_id(
+    const irq_ID_t				ID,
+    unsigned int				*irq_id);
+
+/*! Raise an interrupt on channel irq_id of device IRQ[ID]
+
+ \param	ID[in]				IRQ (device) identifier
+ \param	irq_id[in]			IRQ (channel) identifier
+
+ \return none, signal(IRQ[ID].channel[irq_id])
+ */
+void irq_raise(
+    const irq_ID_t				ID,
+    const irq_sw_channel_id_t	irq_id);
+
+/*! Test if any IRQ channel of the virtual super IRQ has raised a signal
+
+ \return any(VIRQ.channel[irq_ID] != 0)
+ */
+bool any_virq_signal(void);
+
+/*! Enable an IRQ channel of the virtual super IRQ
+
+ \param	irq[in]				IRQ (channel) identifier
+ \param	en[in]				predicate channel enable
+
+ \return none, VIRQ.channel[irq_ID].enable = en
+ */
+void cnd_virq_enable_channel(
+    const enum virq_id				irq_ID,
+    const bool					en);
+
+/*! Clear the state of all IRQ channels of the virtual super IRQ
+
+ \return none, clear(VIRQ.channel[])
+ */
+void virq_clear_all(void);
+
+/*! Clear the IRQ info state of the virtual super IRQ
+
+ \param irq_info[in/out]	The IRQ (channel) state
+
+ \return none
+ */
+void virq_clear_info(struct virq_info *irq_info);
+
+/*! Return the ID of a signalling IRQ channel of the virtual super IRQ
+
+ \param irq_id[out]			active IRQ (channel) identifier
+
+ \Note: This function operates as strtok(), based on the return
+  state the user is informed if there are additional signalling
+  channels
+
+ \return state(IRQ[...])
+ */
+enum hrt_isp_css_irq_status virq_get_channel_id(
+    enum virq_id					*irq_id);
+
+/*! Return the IDs of all signaling IRQ channels of the virtual super IRQ
+
+ \param irq_info[out]		all active IRQ (channel) identifiers
+
+ \Note: Unlike "irq_get_channel_id()" this function returns all
+  channel signaling info. The new info is OR'd with the current
+  info state. N.B. this is the same as repeatedly calling the function
+  "irq_get_channel_id()" in a (non-blocked) handler routine
+
+ \return (error(state(IRQ[...]))
+ */
+enum hrt_isp_css_irq_status
+virq_get_channel_signals(struct virq_info *irq_info);
+
+#endif /* __IRQ_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isp_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isp_public.h
new file mode 100644
index 000000000..a8ff75c63
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isp_public.h
@@ -0,0 +1,186 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISP_PUBLIC_H_INCLUDED__
+#define __ISP_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#include "system_local.h"
+
+/*! Enable or disable the program complete irq signal of ISP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	cnd[in]				predicate
+
+ \return none, if(cnd) enable(ISP[ID].irq) else disable(ISP[ID].irq)
+ */
+void cnd_isp_irq_enable(
+    const isp_ID_t		ID,
+    const bool			cnd);
+
+/*! Read the state of cell ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	state[out]			isp state structure
+ \param	stall[out]			isp stall conditions
+
+ \return none, state = ISP[ID].state, stall = ISP[ID].stall
+ */
+void isp_get_state(
+    const isp_ID_t		ID,
+    isp_state_t			*state,
+    isp_stall_t			*stall);
+
+/*! Write to the status and control register of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, ISP[ID].sc[reg] = value
+ */
+STORAGE_CLASS_ISP_H void isp_ctrl_store(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value);
+
+/*! Read from the status and control register of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return ISP[ID].sc[reg]
+ */
+STORAGE_CLASS_ISP_H hrt_data isp_ctrl_load(
+    const isp_ID_t		ID,
+    const unsigned int	reg);
+
+/*! Get the status of a bitfield in the control register of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be checked
+
+ \return  (ISP[ID].sc[reg] & (1<<bit)) != 0
+ */
+STORAGE_CLASS_ISP_H bool isp_ctrl_getbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit);
+
+/*! Set a bitfield in the control register of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be set
+
+ \return none, ISP[ID].sc[reg] |= (1<<bit)
+ */
+STORAGE_CLASS_ISP_H void isp_ctrl_setbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit);
+
+/*! Clear a bitfield in the control register of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be set
+
+ \return none, ISP[ID].sc[reg] &= ~(1<<bit)
+ */
+STORAGE_CLASS_ISP_H void isp_ctrl_clearbit(
+    const isp_ID_t		ID,
+    const unsigned int	reg,
+    const unsigned int	bit);
+
+/*! Write to the DMEM of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, ISP[ID].dmem[addr...addr+size-1] = data
+ */
+STORAGE_CLASS_ISP_H void isp_dmem_store(
+    const isp_ID_t		ID,
+    unsigned int		addr,
+    const void			*data,
+    const size_t		size);
+
+/*! Read from the DMEM of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = ISP[ID].dmem[addr...addr+size-1]
+ */
+STORAGE_CLASS_ISP_H void isp_dmem_load(
+    const isp_ID_t		ID,
+    const unsigned int	addr,
+    void				*data,
+    const size_t		size);
+
+/*! Write a 32-bit datum to the DMEM of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, ISP[ID].dmem[addr] = data
+ */
+STORAGE_CLASS_ISP_H void isp_dmem_store_uint32(
+    const isp_ID_t		ID,
+    unsigned int		addr,
+    const uint32_t		data);
+
+/*! Load a 32-bit datum from the DMEM of ISP[ID]
+
+ \param	ID[in]				ISP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = ISP[ID].dmem[addr]
+ */
+STORAGE_CLASS_ISP_H uint32_t isp_dmem_load_uint32(
+    const isp_ID_t		ID,
+    const unsigned int	addr);
+
+/*! Concatenate the LSW and MSW into a double precision word
+
+ \param	x0[in]				Integer containing the LSW
+ \param	x1[in]				Integer containing the MSW
+
+ \return x0 | (x1 << bits_per_vector_element)
+ */
+STORAGE_CLASS_ISP_H uint32_t isp_2w_cat_1w(
+    const u16		x0,
+    const uint16_t		x1);
+
+unsigned int isp_is_ready(isp_ID_t ID);
+
+unsigned int isp_is_sleeping(isp_ID_t ID);
+
+void isp_start(isp_ID_t ID);
+
+void isp_wake(isp_ID_t ID);
+
+#endif /* __ISP_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_dma_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_dma_public.h
new file mode 100644
index 000000000..d9b6af898
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_dma_public.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_DMA_PUBLIC_H_INCLUDED__
+#define __ISYS_DMA_PUBLIC_H_INCLUDED__
+
+#ifdef ISP2401
+
+#include "system_local.h"
+#include "type_support.h"
+
+extern void isys2401_dma_reg_store(
+    const isys2401_dma_ID_t dma_id,
+    const unsigned int	reg,
+    const hrt_data		value);
+
+extern hrt_data isys2401_dma_reg_load(
+    const isys2401_dma_ID_t dma_id,
+    const unsigned int	reg);
+
+void isys2401_dma_set_max_burst_size(
+    const isys2401_dma_ID_t dma_id,
+    uint32_t		max_burst_size);
+
+#endif /* ISP2401 */
+
+#endif /* __ISYS_DMA_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_irq_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_irq_public.h
new file mode 100644
index 000000000..736cbc4e3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_irq_public.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_IRQ_PUBLIC_H__
+#define __ISYS_IRQ_PUBLIC_H__
+
+#include "isys_irq_global.h"
+#include "isys_irq_local.h"
+
+#if defined(ISP2401)
+
+void isys_irqc_state_get(const isys_irq_ID_t	isys_irqc_id,
+			 isys_irqc_state_t	*state);
+
+void isys_irqc_state_dump(const isys_irq_ID_t	isys_irqc_id,
+			  const isys_irqc_state_t *state);
+
+void isys_irqc_reg_store(const isys_irq_ID_t	isys_irqc_id,
+			 const unsigned int	reg_idx,
+			 const hrt_data		value);
+
+hrt_data isys_irqc_reg_load(const isys_irq_ID_t	isys_irqc_id,
+			    const unsigned int	reg_idx);
+
+void isys_irqc_status_enable(const isys_irq_ID_t isys_irqc_id);
+
+#endif /* defined(ISP2401) */
+
+#endif	/* __ISYS_IRQ_PUBLIC_H__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_public.h
new file mode 100644
index 000000000..dac53e324
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_public.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_PUBLIC_H_INCLUDED__
+#define __ISYS_PUBLIC_H_INCLUDED__
+
+#ifdef ISP2401
+/*! Read the state of INPUT_SYSTEM[ID]
+ \param ID[in]		INPUT_SYSTEM identifier
+ \param state[out]	pointer to input system state structure
+ \return none, state = INPUT_SYSTEM[ID].state
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H input_system_err_t input_system_get_state(
+    const input_system_ID_t	ID,
+    input_system_state_t *state);
+/*! Dump the state of INPUT_SYSTEM[ID]
+ \param ID[in]		INPUT_SYSTEM identifier
+ \param state[in]	pointer to input system state structure
+ \return none
+ \depends on host supplied print function as part of ia_css_init()
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H void input_system_dump_state(
+    const input_system_ID_t	ID,
+    input_system_state_t *state);
+#endif /* ISP2401 */
+#endif /* __ISYS_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_stream2mmio_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_stream2mmio_public.h
new file mode 100644
index 000000000..73bcc424e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/isys_stream2mmio_public.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_PUBLIC_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_PUBLIC_H_INCLUDED__
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the stream2mmio-controller state.
+ * Get the state of the stream2mmio-controller regiester-set.
+ *
+ * @param[in]	id		The global unique ID of the steeam2mmio controller.
+ * @param[out]	state	Point to the register-state.
+ */
+STORAGE_CLASS_STREAM2MMIO_H void stream2mmio_get_state(
+    const stream2mmio_ID_t ID,
+    stream2mmio_state_t *state);
+
+/**
+ * @brief Get the state of the stream2mmio-controller sidess.
+ * Get the state of the register set per buf-controller sidess.
+ *
+ * @param[in]	id		The global unique ID of the steeam2mmio controller.
+ * @param[in]	sid_id		The sid ID.
+ * @param[out]	state		Point to the sid state.
+ */
+STORAGE_CLASS_STREAM2MMIO_H void stream2mmio_get_sid_state(
+    const stream2mmio_ID_t ID,
+    const stream2mmio_sid_ID_t sid_id,
+    stream2mmio_sid_state_t *state);
+/* end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Load the value of the register of the stream2mmio-controller.
+ *
+ * @param[in]	ID	The global unique ID for the stream2mmio-controller instance.
+ * @param[in]	sid_id	The SID in question.
+ * @param[in]	reg_idx	The offset address of the register.
+ *
+ * @return the value of the register.
+ */
+STORAGE_CLASS_STREAM2MMIO_H hrt_data stream2mmio_reg_load(
+    const stream2mmio_ID_t ID,
+    const stream2mmio_sid_ID_t sid_id,
+    const uint32_t reg_idx);
+
+/**
+ * @brief Dump the SID processor state.
+ * Dump the state of the sid regiester-set.
+ *
+ * @param[in]	state		Pointer to the register-state.
+ */
+STORAGE_CLASS_STREAM2MMIO_H void stream2mmio_print_sid_state(
+    stream2mmio_sid_state_t	*state);
+/**
+ * @brief Dump the stream2mmio state.
+ * Dump the state of the ibuf-controller regiester-set.
+ *
+ * @param[in]	id		The global unique ID of the st2mmio
+ * @param[in]	state		Pointer to the register-state.
+ */
+STORAGE_CLASS_STREAM2MMIO_H void stream2mmio_dump_state(
+    const stream2mmio_ID_t ID,
+    stream2mmio_state_t *state);
+/**
+ * @brief Store a value to the register.
+ * Store a value to the registe of the stream2mmio-controller.
+ *
+ * @param[in]	ID		The global unique ID for the stream2mmio-controller instance.
+ * @param[in]	reg		The offset address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+STORAGE_CLASS_STREAM2MMIO_H void stream2mmio_reg_store(
+    const stream2mmio_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value);
+/* end of DLI */
+
+#endif /* __ISYS_STREAM2MMIO_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/mmu_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/mmu_public.h
new file mode 100644
index 000000000..b8c7bbb71
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/mmu_public.h
@@ -0,0 +1,95 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MMU_PUBLIC_H_INCLUDED__
+#define __MMU_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+#include "device_access.h"
+#include "assert_support.h"
+
+/*! Set the page table base index of MMU[ID]
+
+ \param	ID[in]				MMU identifier
+ \param	base_index[in]		page table base index
+
+ \return none, MMU[ID].page_table_base_index = base_index
+ */
+void mmu_set_page_table_base_index(
+    const mmu_ID_t		ID,
+    const hrt_data		base_index);
+
+/*! Get the page table base index of MMU[ID]
+
+ \param	ID[in]				MMU identifier
+ \param	base_index[in]		page table base index
+
+ \return MMU[ID].page_table_base_index
+ */
+hrt_data mmu_get_page_table_base_index(
+    const mmu_ID_t		ID);
+
+/*! Invalidate the page table cache of MMU[ID]
+
+ \param	ID[in]				MMU identifier
+
+ \return none
+ */
+void mmu_invalidate_cache(
+    const mmu_ID_t		ID);
+
+/*! Invalidate the page table cache of all MMUs
+
+ \return none
+ */
+void mmu_invalidate_cache_all(void);
+
+/*! Write to a control register of MMU[ID]
+
+ \param	ID[in]				MMU identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, MMU[ID].ctrl[reg] = value
+ */
+static inline void mmu_reg_store(
+    const mmu_ID_t		ID,
+    const unsigned int	reg,
+    const hrt_data		value)
+{
+	assert(ID < N_MMU_ID);
+	assert(MMU_BASE[ID] != (hrt_address) - 1);
+	ia_css_device_store_uint32(MMU_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
+
+/*! Read from a control register of MMU[ID]
+
+ \param	ID[in]				MMU identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return MMU[ID].ctrl[reg]
+ */
+static inline hrt_data mmu_reg_load(
+    const mmu_ID_t		ID,
+    const unsigned int	reg)
+{
+	assert(ID < N_MMU_ID);
+	assert(MMU_BASE[ID] != (hrt_address) - 1);
+	return ia_css_device_load_uint32(MMU_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+#endif /* __MMU_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/pixelgen_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/pixelgen_public.h
new file mode 100644
index 000000000..40a9fb6d7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/pixelgen_public.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PIXELGEN_PUBLIC_H_INCLUDED__
+#define __PIXELGEN_PUBLIC_H_INCLUDED__
+
+#ifdef ISP2401
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the pixelgen state.
+ * Get the state of the pixelgen regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the pixelgen controller.
+ * @param[out]	state	Point to the register-state.
+ */
+STORAGE_CLASS_PIXELGEN_H void pixelgen_ctrl_get_state(
+    const pixelgen_ID_t ID,
+    pixelgen_ctrl_state_t *state);
+/**
+ * @brief Dump the pixelgen state.
+ * Dump the state of the pixelgen regiester-set.
+ *
+ * @param[in]	id	The global unique ID of the pixelgen controller.
+ * @param[in]	state	Point to the register-state.
+ */
+STORAGE_CLASS_PIXELGEN_H void pixelgen_ctrl_dump_state(
+    const pixelgen_ID_t ID,
+    pixelgen_ctrl_state_t *state);
+/* end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Load the value of the register of the pixelgen
+ *
+ * @param[in]	ID	The global unique ID for the pixelgen instance.
+ * @param[in]	reg	The offset address of the register.
+ *
+ * @return the value of the register.
+ */
+STORAGE_CLASS_PIXELGEN_H hrt_data pixelgen_ctrl_reg_load(
+    const pixelgen_ID_t ID,
+    const hrt_address reg);
+/**
+ * @brief Store a value to the register.
+ * Store a value to the registe of the pixelgen
+ *
+ * @param[in]	ID		The global unique ID for the pixelgen.
+ * @param[in]	reg		The offset address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+STORAGE_CLASS_PIXELGEN_H void pixelgen_ctrl_reg_store(
+    const pixelgen_ID_t ID,
+    const hrt_address reg,
+    const hrt_data value);
+/* end of DLI */
+
+#endif /* ISP2401 */
+#endif /* __PIXELGEN_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/sp_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/sp_public.h
new file mode 100644
index 000000000..b0b7f2e27
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/sp_public.h
@@ -0,0 +1,224 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SP_PUBLIC_H_INCLUDED__
+#define __SP_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#include "system_local.h"
+
+typedef struct sp_state_s		sp_state_t;
+typedef struct sp_stall_s		sp_stall_t;
+
+/*! Enable or disable the program complete irq signal of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	cnd[in]				predicate
+
+ \return none, if(cnd) enable(SP[ID].irq) else disable(SP[ID].irq)
+ */
+void cnd_sp_irq_enable(
+    const sp_ID_t		ID,
+    const bool			cnd);
+
+/*! Read the state of cell SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	state[out]			sp state structure
+ \param	stall[out]			isp stall conditions
+
+ \return none, state = SP[ID].state, stall = SP[ID].stall
+ */
+void sp_get_state(
+    const sp_ID_t		ID,
+    sp_state_t			*state,
+    sp_stall_t			*stall);
+
+/*! Write to the status and control register of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, SP[ID].sc[reg] = value
+ */
+STORAGE_CLASS_SP_H void sp_ctrl_store(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const hrt_data		value);
+
+/*! Read from the status and control register of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return SP[ID].sc[reg]
+ */
+STORAGE_CLASS_SP_H hrt_data sp_ctrl_load(
+    const sp_ID_t		ID,
+    const hrt_address	reg);
+
+/*! Get the status of a bitfield in the control register of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be checked
+
+ \return  (SP[ID].sc[reg] & (1<<bit)) != 0
+ */
+STORAGE_CLASS_SP_H bool sp_ctrl_getbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit);
+
+/*! Set a bitfield in the control register of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be set
+
+ \return none, SP[ID].sc[reg] |= (1<<bit)
+ */
+STORAGE_CLASS_SP_H void sp_ctrl_setbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit);
+
+/*! Clear a bitfield in the control register of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	reg[in]				register index
+ \param bit[in]				The bit index to be set
+
+ \return none, SP[ID].sc[reg] &= ~(1<<bit)
+ */
+STORAGE_CLASS_SP_H void sp_ctrl_clearbit(
+    const sp_ID_t		ID,
+    const hrt_address	reg,
+    const unsigned int	bit);
+
+/*! Write to the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, SP[ID].dmem[addr...addr+size-1] = data
+ */
+STORAGE_CLASS_SP_H void sp_dmem_store(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const void			*data,
+    const size_t		size);
+
+/*! Read from the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = SP[ID].dmem[addr...addr+size-1]
+ */
+STORAGE_CLASS_SP_H void sp_dmem_load(
+    const sp_ID_t		ID,
+    const hrt_address	addr,
+    void			*data,
+    const size_t		size);
+
+/*! Write a 8-bit datum to the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, SP[ID].dmem[addr...addr+size-1] = data
+ */
+STORAGE_CLASS_SP_H void sp_dmem_store_uint8(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint8_t		data);
+
+/*! Write a 16-bit datum to the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, SP[ID].dmem[addr...addr+size-1] = data
+ */
+STORAGE_CLASS_SP_H void sp_dmem_store_uint16(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint16_t		data);
+
+/*! Write a 32-bit datum to the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be written
+ \param size[in]			The size(in bytes) of the data to be written
+
+ \return none, SP[ID].dmem[addr...addr+size-1] = data
+ */
+STORAGE_CLASS_SP_H void sp_dmem_store_uint32(
+    const sp_ID_t		ID,
+    hrt_address		addr,
+    const uint32_t		data);
+
+/*! Load a 8-bit datum from the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = SP[ID].dmem[addr...addr+size-1]
+ */
+STORAGE_CLASS_SP_H uint8_t sp_dmem_load_uint8(
+    const sp_ID_t		ID,
+    const hrt_address	addr);
+
+/*! Load a 16-bit datum from the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = SP[ID].dmem[addr...addr+size-1]
+ */
+STORAGE_CLASS_SP_H uint16_t sp_dmem_load_uint16(
+    const sp_ID_t		ID,
+    const hrt_address	addr);
+
+/*! Load a 32-bit datum from the DMEM of SP[ID]
+
+ \param	ID[in]				SP identifier
+ \param	addr[in]			the address in DMEM
+ \param data[in]			The data to be read
+ \param size[in]			The size(in bytes) of the data to be read
+
+ \return none, data = SP[ID].dmem[addr...addr+size-1]
+ */
+STORAGE_CLASS_SP_H uint32_t sp_dmem_load_uint32(
+    const sp_ID_t		ID,
+    const hrt_address	addr);
+
+#endif /* __SP_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/tag_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/tag_public.h
new file mode 100644
index 000000000..b18b4a4e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/tag_public.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TAG_PUBLIC_H_INCLUDED__
+#define __TAG_PUBLIC_H_INCLUDED__
+
+/**
+ * @brief	Creates the tag description from the given parameters.
+ * @param[in]	num_captures
+ * @param[in]	skip
+ * @param[in]	offset
+ * @param[out]	tag_descr
+ */
+void
+sh_css_create_tag_descr(int num_captures,
+			unsigned int skip,
+			int offset,
+			unsigned int exp_id,
+			struct sh_css_tag_descr *tag_descr);
+
+/**
+ * @brief	Encodes the members of tag description into a 32-bit value.
+ * @param[in]	tag		Pointer to the tag description
+ * @return	(unsigned int)	Encoded 32-bit tag-info
+ */
+unsigned int
+sh_css_encode_tag_descr(struct sh_css_tag_descr *tag);
+
+#endif /* __TAG_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/timed_ctrl_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/timed_ctrl_public.h
new file mode 100644
index 000000000..563a2833d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/timed_ctrl_public.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TIMED_CTRL_PUBLIC_H_INCLUDED__
+#define __TIMED_CTRL_PUBLIC_H_INCLUDED__
+
+#include "system_local.h"
+
+/*! Write to a control register of TIMED_CTRL[ID]
+
+ \param	ID[in]				TIMED_CTRL identifier
+ \param	reg_addr[in]		register byte address
+ \param value[in]			The data to be written
+
+ \return none, TIMED_CTRL[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_TIMED_CTRL_H void timed_ctrl_reg_store(
+    const timed_ctrl_ID_t	ID,
+    const unsigned int		reg_addr,
+    const hrt_data			value);
+
+void timed_ctrl_snd_commnd(
+    const timed_ctrl_ID_t				ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    hrt_address				addr,
+    hrt_data				value);
+
+void timed_ctrl_snd_sp_commnd(
+    const timed_ctrl_ID_t				ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    const sp_ID_t			SP_ID,
+    hrt_address				offset,
+    hrt_data				value);
+
+void timed_ctrl_snd_gpio_commnd(
+    const timed_ctrl_ID_t				ID,
+    hrt_data				mask,
+    hrt_data				condition,
+    hrt_data				counter,
+    const gpio_ID_t			GPIO_ID,
+    hrt_address				offset,
+    hrt_data				value);
+
+#endif /* __TIMED_CTRL_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vamem_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vamem_public.h
new file mode 100644
index 000000000..823e3857e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vamem_public.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VAMEM_PUBLIC_H_INCLUDED__
+#define __VAMEM_PUBLIC_H_INCLUDED__
+
+#endif /* __VAMEM_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vmem_public.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vmem_public.h
new file mode 100644
index 000000000..c510d6a08
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/host/vmem_public.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VMEM_PUBLIC_H_INCLUDED__
+#define __VMEM_PUBLIC_H_INCLUDED__
+
+#include "isp.h" /* tmemvectoru */
+
+#endif /* __VMEM_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_formatter.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_formatter.h
new file mode 100644
index 000000000..daeb919b5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_formatter.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_FORMATTER_H_INCLUDED__
+#define __INPUT_FORMATTER_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "input_formatter_local.h"
+
+#ifndef __INLINE_INPUT_FORMATTER__
+#define STORAGE_CLASS_INPUT_FORMATTER_H extern
+#define STORAGE_CLASS_INPUT_FORMATTER_C
+#include "input_formatter_public.h"
+#else  /* __INLINE_INPUT_FORMATTER__ */
+#define STORAGE_CLASS_INPUT_FORMATTER_H static inline
+#define STORAGE_CLASS_INPUT_FORMATTER_C static inline
+#include "input_formatter_private.h"
+#endif /* __INLINE_INPUT_FORMATTER__ */
+
+#endif /* __INPUT_FORMATTER_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_system.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_system.h
new file mode 100644
index 000000000..0d951fbf4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/input_system.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_H_INCLUDED__
+#define __INPUT_SYSTEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "input_system_local.h"
+
+#ifndef __INLINE_INPUT_SYSTEM__
+#define STORAGE_CLASS_INPUT_SYSTEM_H extern
+#define STORAGE_CLASS_INPUT_SYSTEM_C
+#include "input_system_public.h"
+#else  /* __INLINE_INPUT_SYSTEM__ */
+#define STORAGE_CLASS_INPUT_SYSTEM_H static inline
+#define STORAGE_CLASS_INPUT_SYSTEM_C static inline
+#include "input_system_private.h"
+#endif /* __INLINE_INPUT_SYSTEM__ */
+
+#endif /* __INPUT_SYSTEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/irq.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/irq.h
new file mode 100644
index 000000000..3a83a8511
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/irq.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IRQ_H_INCLUDED__
+#define __IRQ_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the IRQ device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "irq_local.h"
+
+#ifndef __INLINE_IRQ__
+#define STORAGE_CLASS_IRQ_H extern
+#define STORAGE_CLASS_IRQ_C
+#include "irq_public.h"
+#else  /* __INLINE_IRQ__ */
+#define STORAGE_CLASS_IRQ_H static inline
+#define STORAGE_CLASS_IRQ_C static inline
+#include "irq_private.h"
+#endif /* __INLINE_IRQ__ */
+
+#endif /* __IRQ_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/isp.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isp.h
new file mode 100644
index 000000000..cb64e62c5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isp.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISP_H_INCLUDED__
+#define __ISP_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the ISP cell. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "isp_local.h"
+
+#ifndef __INLINE_ISP__
+#define STORAGE_CLASS_ISP_H extern
+#define STORAGE_CLASS_ISP_C
+#include "isp_public.h"
+#else  /* __INLINE_iSP__ */
+#define STORAGE_CLASS_ISP_H static inline
+#define STORAGE_CLASS_ISP_C static inline
+#include "isp_private.h"
+#endif /* __INLINE_ISP__ */
+
+#endif /* __ISP_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_irq.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_irq.h
new file mode 100644
index 000000000..001c55ea9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_irq.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ISYS_IRQ_H__
+#define __IA_CSS_ISYS_IRQ_H__
+
+#include <type_support.h>
+#include <system_local.h>
+
+#if defined(ISP2401)
+
+#include "isys_irq_public.h"
+
+#endif /* defined(ISP2401) */
+
+#endif	/* __IA_CSS_ISYS_IRQ_H__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_stream2mmio.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_stream2mmio.h
new file mode 100644
index 000000000..b0f09ffb4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/isys_stream2mmio.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ * - system and cell agnostic interfaces, constants and identifiers
+ * - public:  system agnostic, cell specific interfaces
+ * - private: system dependent, cell specific interfaces &
+ *   inline implementations
+ * - global:  system specific constants and identifiers
+ * - local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "isys_stream2mmio_local.h"
+
+#ifndef __INLINE_STREAM2MMIO__
+#define STORAGE_CLASS_STREAM2MMIO_H extern
+#define STORAGE_CLASS_STREAM2MMIO_C
+#include "isys_stream2mmio_public.h"
+#else  /* __INLINE_STREAM2MMIO__ */
+#define STORAGE_CLASS_STREAM2MMIO_H static inline
+#define STORAGE_CLASS_STREAM2MMIO_C static inline
+#include "isys_stream2mmio_private.h"
+#endif /* __INLINE_STREAM2MMIO__ */
+
+#endif /* __ISYS_STREAM2MMIO_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/math_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/math_support.h
new file mode 100644
index 000000000..a444ec14f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/math_support.h
@@ -0,0 +1,154 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MATH_SUPPORT_H
+#define __MATH_SUPPORT_H
+
+#include <linux/kernel.h> /* Override the definition of max/min from linux kernel*/
+
+#define IS_ODD(a)            ((a) & 0x1)
+#define IS_EVEN(a)           (!IS_ODD(a))
+
+/* force a value to a lower even value */
+#define EVEN_FLOOR(x)        ((x) & ~1)
+
+/* ISP2401 */
+/* If the number is odd, find the next even number */
+#define EVEN_CEIL(x)         ((IS_ODD(x)) ? ((x) + 1) : (x))
+
+/* A => B */
+#define IMPLIES(a, b)        (!(a) || (b))
+
+/* for preprocessor and array sizing use MIN and MAX
+   otherwise use min and max */
+#define MAX(a, b)            (((a) > (b)) ? (a) : (b))
+#define MIN(a, b)            (((a) < (b)) ? (a) : (b))
+
+#define ROUND_DIV(a, b)      (((b) != 0) ? ((a) + ((b) >> 1)) / (b) : 0)
+#define CEIL_DIV(a, b)       (((b) != 0) ? ((a) + (b) - 1) / (b) : 0)
+#define CEIL_MUL(a, b)       (CEIL_DIV(a, b) * (b))
+#define CEIL_MUL2(a, b)      (((a) + (b) - 1) & ~((b) - 1))
+#define CEIL_SHIFT(a, b)     (((a) + (1 << (b)) - 1) >> (b))
+#define CEIL_SHIFT_MUL(a, b) (CEIL_SHIFT(a, b) << (b))
+#define ROUND_HALF_DOWN_DIV(a, b)	(((b) != 0) ? ((a) + (b / 2) - 1) / (b) : 0)
+#define ROUND_HALF_DOWN_MUL(a, b)	(ROUND_HALF_DOWN_DIV(a, b) * (b))
+
+/*To Find next power of 2 number from x */
+#define bit2(x)            ((x)      | ((x) >> 1))
+#define bit4(x)            (bit2(x)  | (bit2(x) >> 2))
+#define bit8(x)            (bit4(x)  | (bit4(x) >> 4))
+#define bit16(x)           (bit8(x)  | (bit8(x) >> 8))
+#define bit32(x)           (bit16(x) | (bit16(x) >> 16))
+#define NEXT_POWER_OF_2(x) (bit32(x - 1) + 1)
+
+/* min and max should not be macros as they will evaluate their arguments twice.
+   if you really need a macro (e.g. for CPP or for initializing an array)
+   use MIN() and MAX(), otherwise use min() and max().
+
+*/
+
+#if !defined(PIPE_GENERATION)
+
+/*
+This macro versions are added back as we are mixing types in usage of inline.
+This causes corner cases of calculations to be incorrect due to conversions
+between signed and unsigned variables or overflows.
+Before the addition of the inline functions, max, min and ceil_div were macros
+and therefore adding them back.
+
+Leaving out the other math utility functions as they are newly added
+*/
+
+#define ceil_div(a, b)		(CEIL_DIV(a, b))
+
+static inline unsigned int ceil_mul(unsigned int a, unsigned int b)
+{
+	return CEIL_MUL(a, b);
+}
+
+static inline unsigned int ceil_mul2(unsigned int a, unsigned int b)
+{
+	return CEIL_MUL2(a, b);
+}
+
+static inline unsigned int ceil_shift(unsigned int a, unsigned int b)
+{
+	return CEIL_SHIFT(a, b);
+}
+
+static inline unsigned int ceil_shift_mul(unsigned int a, unsigned int b)
+{
+	return CEIL_SHIFT_MUL(a, b);
+}
+
+/* ISP2401 */
+static inline unsigned int round_half_down_div(unsigned int a, unsigned int b)
+{
+	return ROUND_HALF_DOWN_DIV(a, b);
+}
+
+/* ISP2401 */
+static inline unsigned int round_half_down_mul(unsigned int a, unsigned int b)
+{
+	return ROUND_HALF_DOWN_MUL(a, b);
+}
+
+/* @brief Next Power of Two
+ *
+ *  @param[in] unsigned number
+ *
+ *  @return next power of two
+ *
+ * This function rounds input to the nearest power of 2 (2^x)
+ * towards infinity
+ *
+ * Input Range: 0 .. 2^(8*sizeof(int)-1)
+ *
+ * IF input is a power of 2
+ *     out = in
+ * OTHERWISE
+ *     out = 2^(ceil(log2(in))
+ *
+ */
+
+static inline unsigned int ceil_pow2(unsigned int a)
+{
+	if (a == 0) {
+		return 1;
+	}
+	/* IF input is already a power of two*/
+	else if ((!((a) & ((a) - 1)))) {
+		return a;
+	} else {
+		unsigned int v = a;
+
+		v |= v >> 1;
+		v |= v >> 2;
+		v |= v >> 4;
+		v |= v >> 8;
+		v |= v >> 16;
+		return (v + 1);
+	}
+}
+
+#endif /* !defined(PIPE_GENERATION) */
+
+/*
+ * For SP and ISP, SDK provides the definition of OP_std_modadd.
+ * We need it only for host
+ */
+#define OP_std_modadd(base, offset, size) ((base + offset) % (size))
+
+#endif /* __MATH_SUPPORT_H */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/misc_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/misc_support.h
new file mode 100644
index 000000000..393452d7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/misc_support.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MISC_SUPPORT_H_INCLUDED__
+#define __MISC_SUPPORT_H_INCLUDED__
+
+/* suppress compiler warnings on unused variables */
+#ifndef NOT_USED
+#define NOT_USED(a) ((void)(a))
+#endif
+
+/* Calculate the  total bytes for pow(2) byte alignment */
+#define tot_bytes_for_pow2_align(pow2, cur_bytes)	((cur_bytes + (pow2 - 1)) & ~(pow2 - 1))
+
+#endif /* __MISC_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/mmu_device.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/mmu_device.h
new file mode 100644
index 000000000..b6f6eda4c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/mmu_device.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __MMU_DEVICE_H_INCLUDED__
+#define __MMU_DEVICE_H_INCLUDED__
+
+/* The file mmu.h already exists */
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the MMU device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "mmu_local.h"
+
+#include "mmu_public.h"
+
+#endif /* __MMU_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/pixelgen.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/pixelgen.h
new file mode 100644
index 000000000..e34cd3c58
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/pixelgen.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PIXELGEN_H_INCLUDED__
+#define __PIXELGEN_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ * - system and cell agnostic interfaces, constants and identifiers
+ * - public:  system agnostic, cell specific interfaces
+ * - private: system dependent, cell specific interfaces &
+ *   inline implementations
+ * - global:  system specific constants and identifiers
+ * - local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "pixelgen_local.h"
+
+#ifndef __INLINE_PIXELGEN__
+#define STORAGE_CLASS_PIXELGEN_H extern
+#define STORAGE_CLASS_PIXELGEN_C
+#include "pixelgen_public.h"
+#else  /* __INLINE_PIXELGEN__ */
+#define STORAGE_CLASS_PIXELGEN_H static inline
+#define STORAGE_CLASS_PIXELGEN_C static inline
+#include "pixelgen_private.h"
+#endif /* __INLINE_PIXELGEN__ */
+
+#endif /* __PIXELGEN_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/platform_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/platform_support.h
new file mode 100644
index 000000000..0cdef4a5e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/platform_support.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PLATFORM_SUPPORT_H_INCLUDED__
+#define __PLATFORM_SUPPORT_H_INCLUDED__
+
+/**
+* @file
+* Platform specific includes and functionality.
+*/
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+#define UINT16_MAX USHRT_MAX
+#define UINT32_MAX UINT_MAX
+#define UCHAR_MAX  (255)
+
+#define CSS_ALIGN(d, a) d __attribute__((aligned(a)))
+
+#endif /* __PLATFORM_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/print_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/print_support.h
new file mode 100644
index 000000000..a3c7f3de6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/print_support.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __PRINT_SUPPORT_H_INCLUDED__
+#define __PRINT_SUPPORT_H_INCLUDED__
+
+#include <linux/stdarg.h>
+
+extern int (*sh_css_printf)(const char *fmt, va_list args);
+/* depends on host supplied print function in ia_css_init() */
+static inline  __printf(1, 2) void ia_css_print(const char *fmt, ...)
+{
+	va_list ap;
+
+	if (sh_css_printf) {
+		va_start(ap, fmt);
+		sh_css_printf(fmt, ap);
+		va_end(ap);
+	}
+}
+
+/* Start adding support for bxt tracing functions for poc. From
+ * bxt_sandbox/support/print_support.h. */
+/* TODO: support these macros in userspace. */
+#define PWARN(format, ...) ia_css_print("warning: ", ##__VA_ARGS__)
+#define PRINT(format, ...) ia_css_print(format, ##__VA_ARGS__)
+#define PERROR(format, ...) ia_css_print("error: " format, ##__VA_ARGS__)
+#define PDEBUG(format, ...) ia_css_print("debug: " format, ##__VA_ARGS__)
+
+#endif /* __PRINT_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/queue.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/queue.h
new file mode 100644
index 000000000..e6978750a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/queue.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __QUEUE_H_INCLUDED__
+#define __QUEUE_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and is system agnostic
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - system and cell agnostic interfaces, constants and identifiers
+ *	- public:  cell specific interfaces
+ *	- private: cell specific inline implementations
+ *	- global:  inter cell constants and identifiers
+ *	- local:   cell specific constants and identifiers
+ *
+ */
+
+#include "queue_local.h"
+
+#ifndef __INLINE_QUEUE__
+#define STORAGE_CLASS_QUEUE_H extern
+#define STORAGE_CLASS_QUEUE_C
+/* #include "queue_public.h" */
+#include "ia_css_queue.h"
+#else  /* __INLINE_QUEUE__ */
+#define STORAGE_CLASS_QUEUE_H static inline
+#define STORAGE_CLASS_QUEUE_C static inline
+#include "queue_private.h"
+#endif /* __INLINE_QUEUE__ */
+
+#endif /* __QUEUE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/resource.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/resource.h
new file mode 100644
index 000000000..9be45b679
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/resource.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __RESOURCE_H_INCLUDED__
+#define __RESOURCE_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses a RESOURCE manager. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ *
+ */
+
+#include "system_local.h"
+#include "resource_local.h"
+
+#ifndef __INLINE_RESOURCE__
+#define STORAGE_CLASS_RESOURCE_H extern
+#define STORAGE_CLASS_RESOURCE_C
+#include "resource_public.h"
+#else  /* __INLINE_RESOURCE__ */
+#define STORAGE_CLASS_RESOURCE_H static inline
+#define STORAGE_CLASS_RESOURCE_C static inline
+#include "resource_private.h"
+#endif /* __INLINE_RESOURCE__ */
+
+#endif /* __RESOURCE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/sp.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/sp.h
new file mode 100644
index 000000000..a7d00c7bb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/sp.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SP_H_INCLUDED__
+#define __SP_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the SP cell. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "sp_local.h"
+
+#ifndef __INLINE_SP__
+#define STORAGE_CLASS_SP_H extern
+#define STORAGE_CLASS_SP_C
+#include "sp_public.h"
+#else  /* __INLINE_SP__ */
+#define STORAGE_CLASS_SP_H static inline
+#define STORAGE_CLASS_SP_C static inline
+#include "sp_private.h"
+#endif /* __INLINE_SP__ */
+
+#endif /* __SP_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/tag.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/tag.h
new file mode 100644
index 000000000..98d7e922a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/tag.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TAG_H_INCLUDED__
+#define __TAG_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and is system agnostic
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  cell specific interfaces
+ *	- private: cell specific inline implementations
+ *	- global:  inter cell constants and identifiers
+ *	- local:   cell specific constants and identifiers
+ *
+ */
+
+#include "tag_local.h"
+
+#ifndef __INLINE_TAG__
+#define STORAGE_CLASS_TAG_H extern
+#define STORAGE_CLASS_TAG_C
+#include "tag_public.h"
+#else  /* __INLINE_TAG__ */
+#define STORAGE_CLASS_TAG_H static inline
+#define STORAGE_CLASS_TAG_C static inline
+#include "tag_private.h"
+#endif /* __INLINE_TAG__ */
+
+#endif /* __TAG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/timed_ctrl.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/timed_ctrl.h
new file mode 100644
index 000000000..65b2871fb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/timed_ctrl.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TIMED_CTRL_H_INCLUDED__
+#define __TIMED_CTRL_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the input system device(s). It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "timed_ctrl_local.h"
+
+#ifndef __INLINE_TIMED_CTRL__
+#define STORAGE_CLASS_TIMED_CTRL_H extern
+#define STORAGE_CLASS_TIMED_CTRL_C
+#include "timed_ctrl_public.h"
+#else  /* __INLINE_TIMED_CTRL__ */
+#define STORAGE_CLASS_TIMED_CTRL_H static inline
+#define STORAGE_CLASS_TIMED_CTRL_C static inline
+#include "timed_ctrl_private.h"
+#endif /* __INLINE_TIMED_CTRL__ */
+
+#endif /* __TIMED_CTRL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/type_support.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/type_support.h
new file mode 100644
index 000000000..b996ee54d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/type_support.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TYPE_SUPPORT_H_INCLUDED__
+#define __TYPE_SUPPORT_H_INCLUDED__
+
+/**
+* @file
+* Platform specific types.
+*
+* Per the DLI spec, types are in "type_support.h" and
+* "platform_support.h" is for unclassified/to be refactored
+* platform specific definitions.
+*/
+
+#define IA_CSS_UINT8_T_BITS						8
+#define IA_CSS_UINT16_T_BITS					16
+#define IA_CSS_UINT32_T_BITS					32
+#define IA_CSS_INT32_T_BITS						32
+#define IA_CSS_UINT64_T_BITS					64
+
+#define CHAR_BIT (8)
+
+#include <linux/types.h>
+#include <linux/limits.h>
+#include <linux/errno.h>
+#define HOST_ADDRESS(x) (unsigned long)(x)
+
+#endif /* __TYPE_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/vamem.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/vamem.h
new file mode 100644
index 000000000..3ea6758aa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/vamem.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VAMEM_H_INCLUDED__
+#define __VAMEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the VAMEM device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "vamem_local.h"
+#include "vamem_public.h"
+
+#endif /* __VAMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_include/vmem.h b/drivers/staging/media/atomisp/pci/hive_isp_css_include/vmem.h
new file mode 100644
index 000000000..da479b370
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_include/vmem.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VMEM_H_INCLUDED__
+#define __VMEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the VMEM device. It defines the API to DLI bridge
+ *
+ * System and cell specific interfaces and inline code are included
+ * conditionally through Makefile path settings.
+ *
+ *  - .        system and cell agnostic interfaces, constants and identifiers
+ *	- public:  system agnostic, cell specific interfaces
+ *	- private: system dependent, cell specific interfaces & inline implementations
+ *	- global:  system specific constants and identifiers
+ *	- local:   system and cell specific constants and identifiers
+ */
+
+#include "system_local.h"
+#include "vmem_local.h"
+
+#ifndef __INLINE_VMEM__
+#define STORAGE_CLASS_VMEM_H extern
+#define STORAGE_CLASS_VMEM_C
+#include "vmem_public.h"
+#else  /* __INLINE_VMEM__ */
+#define STORAGE_CLASS_VMEM_H static inline
+#define STORAGE_CLASS_VMEM_C static inline
+#include "vmem_private.h"
+#endif /* __INLINE_VMEM__ */
+
+#endif /* __VMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_local.h
new file mode 100644
index 000000000..31121a22d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_local.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __QUEUE_LOCAL_H_INCLUDED__
+#define __QUEUE_LOCAL_H_INCLUDED__
+
+#include "queue_global.h"
+
+#endif /* __QUEUE_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_private.h
new file mode 100644
index 000000000..be6162dfb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/queue_private.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __QUEUE_PRIVATE_H_INCLUDED__
+#define __QUEUE_PRIVATE_H_INCLUDED__
+
+#endif /* __QUEUE_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag.c b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag.c
new file mode 100644
index 000000000..8931539a4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag.c
@@ -0,0 +1,92 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "tag.h"
+#include <platform_support.h>	/* NULL */
+#include <assert_support.h>
+#include "tag_local.h"
+
+/*
+ * @brief	Creates the tag description from the given parameters.
+ * @param[in]	num_captures
+ * @param[in]	skip
+ * @param[in]	offset
+ * @param[out]	tag_descr
+ */
+void
+sh_css_create_tag_descr(int num_captures,
+			unsigned int skip,
+			int offset,
+			unsigned int exp_id,
+			struct sh_css_tag_descr *tag_descr)
+{
+	assert(tag_descr);
+
+	tag_descr->num_captures = num_captures;
+	tag_descr->skip		= skip;
+	tag_descr->offset	= offset;
+	tag_descr->exp_id	= exp_id;
+}
+
+/*
+ * @brief	Encodes the members of tag description into a 32-bit value.
+ * @param[in]	tag		Pointer to the tag description
+ * @return	(unsigned int)	Encoded 32-bit tag-info
+ */
+unsigned int
+sh_css_encode_tag_descr(struct sh_css_tag_descr *tag)
+{
+	int num_captures;
+	unsigned int num_captures_sign;
+	unsigned int skip;
+	int offset;
+	unsigned int offset_sign;
+	unsigned int exp_id;
+	unsigned int encoded_tag;
+
+	assert(tag);
+
+	if (tag->num_captures < 0) {
+		num_captures = -tag->num_captures;
+		num_captures_sign = 1;
+	} else {
+		num_captures = tag->num_captures;
+		num_captures_sign = 0;
+	}
+	skip = tag->skip;
+	if (tag->offset < 0) {
+		offset = -tag->offset;
+		offset_sign = 1;
+	} else {
+		offset = tag->offset;
+		offset_sign = 0;
+	}
+	exp_id = tag->exp_id;
+
+	if (exp_id != 0) {
+		/* we encode either an exp_id or capture data */
+		assert((num_captures == 0) && (skip == 0) && (offset == 0));
+
+		encoded_tag = TAG_EXP | (exp_id & 0xFF) << TAG_EXP_ID_SHIFT;
+	} else {
+		encoded_tag = TAG_CAP
+			      | ((num_captures_sign & 0x00000001) << TAG_NUM_CAPTURES_SIGN_SHIFT)
+			      | ((offset_sign       & 0x00000001) << TAG_OFFSET_SIGN_SHIFT)
+			      | ((num_captures      & 0x000000FF) << TAG_NUM_CAPTURES_SHIFT)
+			      | ((skip              & 0x000000FF) << TAG_OFFSET_SHIFT)
+			      | ((offset            & 0x000000FF) << TAG_SKIP_SHIFT);
+	}
+	return encoded_tag;
+}
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_local.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_local.h
new file mode 100644
index 000000000..921e50a45
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_local.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TAG_LOCAL_H_INCLUDED__
+#define __TAG_LOCAL_H_INCLUDED__
+
+#include "tag_global.h"
+
+#define SH_CSS_MINIMUM_TAG_ID (-1)
+
+#endif /* __TAG_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_private.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_private.h
new file mode 100644
index 000000000..b14f09ade
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/host/tag_private.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TAG_PRIVATE_H_INCLUDED__
+#define __TAG_PRIVATE_H_INCLUDED__
+
+#endif /* __TAG_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/queue_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/queue_global.h
new file mode 100644
index 000000000..6ae453782
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/queue_global.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __QUEUE_GLOBAL_H_INCLUDED__
+#define __QUEUE_GLOBAL_H_INCLUDED__
+
+#endif /* __QUEUE_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/sw_event_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/sw_event_global.h
new file mode 100644
index 000000000..b256ea19c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/sw_event_global.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SW_EVENT_GLOBAL_H_INCLUDED__
+#define __SW_EVENT_GLOBAL_H_INCLUDED__
+
+#define MAX_NR_OF_PAYLOADS_PER_SW_EVENT 4
+
+enum ia_css_psys_sw_event {
+	IA_CSS_PSYS_SW_EVENT_BUFFER_ENQUEUED, /* from host to SP */
+	IA_CSS_PSYS_SW_EVENT_BUFFER_DEQUEUED, /* from SP to host */
+	IA_CSS_PSYS_SW_EVENT_EVENT_DEQUEUED, /* from SP to host, one way only */
+	IA_CSS_PSYS_SW_EVENT_START_STREAM,
+	IA_CSS_PSYS_SW_EVENT_STOP_STREAM,
+	IA_CSS_PSYS_SW_EVENT_MIPI_BUFFERS_READY,
+	IA_CSS_PSYS_SW_EVENT_UNLOCK_RAW_BUFFER,
+	IA_CSS_PSYS_SW_EVENT_STAGE_ENABLE_DISABLE /* for extension state change enable/disable */
+};
+
+enum ia_css_isys_sw_event {
+	IA_CSS_ISYS_SW_EVENT_EVENT_DEQUEUED
+};
+
+#endif /* __SW_EVENT_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_shared/tag_global.h b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/tag_global.h
new file mode 100644
index 000000000..af5a47ace
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_shared/tag_global.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __TAG_GLOBAL_H_INCLUDED__
+#define __TAG_GLOBAL_H_INCLUDED__
+
+/* offsets for encoding/decoding the tag into an uint32_t */
+
+#define TAG_CAP	1
+#define TAG_EXP	2
+
+#define TAG_NUM_CAPTURES_SIGN_SHIFT	 6
+#define TAG_OFFSET_SIGN_SHIFT		 7
+#define TAG_NUM_CAPTURES_SHIFT		 8
+#define TAG_OFFSET_SHIFT		16
+#define TAG_SKIP_SHIFT			24
+
+#define TAG_EXP_ID_SHIFT		 8
+
+/* Data structure containing the tagging information which is used in
+ * continuous mode to specify which frames should be captured.
+ * num_captures		The number of RAW frames to be processed to
+ *                      YUV. Setting this to -1 will make continuous
+ *                      capture run until it is stopped.
+ * skip			Skip N frames in between captures. This can be
+ *                      used to select a slower capture frame rate than
+ *                      the sensor output frame rate.
+ * offset		Start the RAW-to-YUV processing at RAW buffer
+ *                      with this offset. This allows the user to
+ *                      process RAW frames that were captured in the
+ *                      past or future.
+ * exp_id		Exposure id of the RAW frame to tag.
+ *
+ * NOTE: Either exp_id = 0 or all other fields are 0
+ *	 (so yeah, this could be a union)
+ */
+
+struct sh_css_tag_descr {
+	int num_captures;
+	unsigned int skip;
+	int offset;
+	unsigned int exp_id;
+};
+
+#endif /* __TAG_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/hive_isp_css_streaming_to_mipi_types_hrt.h b/drivers/staging/media/atomisp/pci/hive_isp_css_streaming_to_mipi_types_hrt.h
new file mode 100644
index 000000000..301dd9239
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_isp_css_streaming_to_mipi_types_hrt.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _hive_isp_css_streaming_to_mipi_types_hrt_h_
+#define _hive_isp_css_streaming_to_mipi_types_hrt_h_
+
+#include <streaming_to_mipi_defs.h>
+
+#define _HIVE_ISP_CH_ID_MASK    ((1U << HIVE_ISP_CH_ID_BITS) - 1)
+#define _HIVE_ISP_FMT_TYPE_MASK ((1U << HIVE_ISP_FMT_TYPE_BITS) - 1)
+
+#define _HIVE_STR_TO_MIPI_FMT_TYPE_LSB (HIVE_STR_TO_MIPI_CH_ID_LSB + HIVE_ISP_CH_ID_BITS)
+#define _HIVE_STR_TO_MIPI_DATA_B_LSB   (HIVE_STR_TO_MIPI_DATA_A_LSB + HIVE_IF_PIXEL_WIDTH)
+
+#endif /* _hive_isp_css_streaming_to_mipi_types_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/hive_types.h b/drivers/staging/media/atomisp/pci/hive_types.h
new file mode 100644
index 000000000..4b8a679fb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hive_types.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _HRT_HIVE_TYPES_H
+#define _HRT_HIVE_TYPES_H
+
+#include "version.h"
+#include "defs.h"
+
+#ifndef HRTCAT3
+#define _HRTCAT3(m, n, o)     m##n##o
+#define HRTCAT3(m, n, o)      _HRTCAT3(m, n, o)
+#endif
+
+#ifndef HRTCAT4
+#define _HRTCAT4(m, n, o, p)     m##n##o##p
+#define HRTCAT4(m, n, o, p)      _HRTCAT4(m, n, o, p)
+#endif
+
+#ifndef HRTMIN
+#define HRTMIN(a, b) (((a) < (b)) ? (a) : (b))
+#endif
+
+#ifndef HRTMAX
+#define HRTMAX(a, b) (((a) > (b)) ? (a) : (b))
+#endif
+
+/* boolean data type */
+typedef unsigned int hive_bool;
+#define hive_false 0
+#define hive_true  1
+
+typedef char                 hive_int8;
+typedef short                hive_int16;
+typedef int                  hive_int32;
+typedef long long            hive_int64;
+
+typedef unsigned char        hive_uint8;
+typedef unsigned short       hive_uint16;
+typedef unsigned int         hive_uint32;
+typedef unsigned long long   hive_uint64;
+
+#define HRT_DATA_WIDTH	  32
+#define HRT_ADDRESS_WIDTH 64
+#define HRT_DATA_BYTES    (HRT_DATA_WIDTH / 8)
+#define HRT_ADDRESS_BYTES (HRT_ADDRESS_WIDTH / 8)
+#define SIZEOF_HRT_REG    (HRT_DATA_WIDTH >> 3)
+
+typedef hive_uint32 hrt_data;
+typedef hive_uint64 hrt_address;
+
+/* use 64 bit addresses in simulation, where possible */
+typedef hive_uint64  hive_sim_address;
+
+/* below is for csim, not for hrt, rename and move this elsewhere */
+
+typedef unsigned int hive_uint;
+typedef hive_uint32  hive_address;
+typedef hive_address hive_slave_address;
+typedef hive_address hive_mem_address;
+
+/* MMIO devices */
+typedef hive_uint    hive_mmio_id;
+typedef hive_mmio_id hive_slave_id;
+typedef hive_mmio_id hive_port_id;
+typedef hive_mmio_id hive_master_id;
+typedef hive_mmio_id hive_mem_id;
+typedef hive_mmio_id hive_dev_id;
+typedef hive_mmio_id hive_fifo_id;
+
+typedef hive_uint      hive_hier_id;
+typedef hive_hier_id   hive_device_id;
+typedef hive_device_id hive_proc_id;
+typedef hive_device_id hive_cell_id;
+typedef hive_device_id hive_host_id;
+typedef hive_device_id hive_bus_id;
+typedef hive_device_id hive_bridge_id;
+typedef hive_device_id hive_fifo_adapter_id;
+typedef hive_device_id hive_custom_device_id;
+
+typedef hive_uint hive_slot_id;
+typedef hive_uint hive_fu_id;
+typedef hive_uint hive_reg_file_id;
+typedef hive_uint hive_reg_id;
+
+/* Streaming devices */
+typedef hive_uint hive_outport_id;
+typedef hive_uint hive_inport_id;
+
+typedef hive_uint hive_msink_id;
+
+/* HRT specific */
+typedef char *hive_program;
+typedef char *hive_function;
+
+#endif /* _HRT_HIVE_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/hmm/hmm.c b/drivers/staging/media/atomisp/pci/hmm/hmm.c
new file mode 100644
index 000000000..fc6cfe9f7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hmm/hmm.c
@@ -0,0 +1,608 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010-2017 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+/*
+ * This file contains entry functions for memory management of ISP driver
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <linux/highmem.h>	/* for kmap */
+#include <linux/io.h>		/* for page_to_phys */
+#include <linux/sysfs.h>
+
+#include "hmm/hmm.h"
+#include "hmm/hmm_bo.h"
+
+#include "atomisp_internal.h"
+#include "asm/cacheflush.h"
+#include "mmu/isp_mmu.h"
+#include "mmu/sh_mmu_mrfld.h"
+
+struct hmm_bo_device bo_device;
+static ia_css_ptr dummy_ptr = mmgr_EXCEPTION;
+static bool hmm_initialized;
+
+/*
+ * p: private
+ * s: shared
+ * u: user
+ * i: ion
+ */
+static const char hmm_bo_type_string[] = "psui";
+
+static ssize_t bo_show(struct device *dev, struct device_attribute *attr,
+		       char *buf, struct list_head *bo_list, bool active)
+{
+	ssize_t ret = 0;
+	struct hmm_buffer_object *bo;
+	unsigned long flags;
+	int i;
+	long total[HMM_BO_LAST] = { 0 };
+	long count[HMM_BO_LAST] = { 0 };
+	int index1 = 0;
+	int index2 = 0;
+
+	ret = scnprintf(buf, PAGE_SIZE, "type pgnr\n");
+	if (ret <= 0)
+		return 0;
+
+	index1 += ret;
+
+	spin_lock_irqsave(&bo_device.list_lock, flags);
+	list_for_each_entry(bo, bo_list, list) {
+		if ((active && (bo->status & HMM_BO_ALLOCED)) ||
+		    (!active && !(bo->status & HMM_BO_ALLOCED))) {
+			ret = scnprintf(buf + index1, PAGE_SIZE - index1,
+					"%c %d\n",
+					hmm_bo_type_string[bo->type], bo->pgnr);
+
+			total[bo->type] += bo->pgnr;
+			count[bo->type]++;
+			if (ret > 0)
+				index1 += ret;
+		}
+	}
+	spin_unlock_irqrestore(&bo_device.list_lock, flags);
+
+	for (i = 0; i < HMM_BO_LAST; i++) {
+		if (count[i]) {
+			ret = scnprintf(buf + index1 + index2,
+					PAGE_SIZE - index1 - index2,
+					"%ld %c buffer objects: %ld KB\n",
+					count[i], hmm_bo_type_string[i],
+					total[i] * 4);
+			if (ret > 0)
+				index2 += ret;
+		}
+	}
+
+	/* Add trailing zero, not included by scnprintf */
+	return index1 + index2 + 1;
+}
+
+static ssize_t active_bo_show(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	return bo_show(dev, attr, buf, &bo_device.entire_bo_list, true);
+}
+
+static ssize_t free_bo_show(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	return bo_show(dev, attr, buf, &bo_device.entire_bo_list, false);
+}
+
+
+static DEVICE_ATTR_RO(active_bo);
+static DEVICE_ATTR_RO(free_bo);
+
+static struct attribute *sysfs_attrs_ctrl[] = {
+	&dev_attr_active_bo.attr,
+	&dev_attr_free_bo.attr,
+	NULL
+};
+
+static struct attribute_group atomisp_attribute_group[] = {
+	{.attrs = sysfs_attrs_ctrl },
+};
+
+int hmm_init(void)
+{
+	int ret;
+
+	ret = hmm_bo_device_init(&bo_device, &sh_mmu_mrfld,
+				 ISP_VM_START, ISP_VM_SIZE);
+	if (ret)
+		dev_err(atomisp_dev, "hmm_bo_device_init failed.\n");
+
+	hmm_initialized = true;
+
+	/*
+	 * As hmm use NULL to indicate invalid ISP virtual address,
+	 * and ISP_VM_START is defined to 0 too, so we allocate
+	 * one piece of dummy memory, which should return value 0,
+	 * at the beginning, to avoid hmm_alloc return 0 in the
+	 * further allocation.
+	 */
+	dummy_ptr = hmm_alloc(1);
+
+	if (!ret) {
+		ret = sysfs_create_group(&atomisp_dev->kobj,
+					 atomisp_attribute_group);
+		if (ret)
+			dev_err(atomisp_dev,
+				"%s Failed to create sysfs\n", __func__);
+	}
+
+	return ret;
+}
+
+void hmm_cleanup(void)
+{
+	if (dummy_ptr == mmgr_EXCEPTION)
+		return;
+	sysfs_remove_group(&atomisp_dev->kobj, atomisp_attribute_group);
+
+	/* free dummy memory first */
+	hmm_free(dummy_ptr);
+	dummy_ptr = 0;
+
+	hmm_bo_device_exit(&bo_device);
+	hmm_initialized = false;
+}
+
+static ia_css_ptr __hmm_alloc(size_t bytes, enum hmm_bo_type type, const void __user *userptr)
+{
+	unsigned int pgnr;
+	struct hmm_buffer_object *bo;
+	int ret;
+
+	/*
+	 * Check if we are initialized. In the ideal world we wouldn't need
+	 * this but we can tackle it once the driver is a lot cleaner
+	 */
+
+	if (!hmm_initialized)
+		hmm_init();
+	/* Get page number from size */
+	pgnr = size_to_pgnr_ceil(bytes);
+
+	/* Buffer object structure init */
+	bo = hmm_bo_alloc(&bo_device, pgnr);
+	if (!bo) {
+		dev_err(atomisp_dev, "hmm_bo_create failed.\n");
+		goto create_bo_err;
+	}
+
+	/* Allocate pages for memory */
+	ret = hmm_bo_alloc_pages(bo, type, userptr);
+	if (ret) {
+		dev_err(atomisp_dev, "hmm_bo_alloc_pages failed.\n");
+		goto alloc_page_err;
+	}
+
+	/* Combine the virtual address and pages together */
+	ret = hmm_bo_bind(bo);
+	if (ret) {
+		dev_err(atomisp_dev, "hmm_bo_bind failed.\n");
+		goto bind_err;
+	}
+
+	dev_dbg(atomisp_dev,
+		"%s: pages: 0x%08x (%zu bytes), type: %d, user ptr %p\n",
+		__func__, bo->start, bytes, type, userptr);
+
+	return bo->start;
+
+bind_err:
+	hmm_bo_free_pages(bo);
+alloc_page_err:
+	hmm_bo_unref(bo);
+create_bo_err:
+	return 0;
+}
+
+ia_css_ptr hmm_alloc(size_t bytes)
+{
+	return __hmm_alloc(bytes, HMM_BO_PRIVATE, NULL);
+}
+
+ia_css_ptr hmm_create_from_userdata(size_t bytes, const void __user *userptr)
+{
+	return __hmm_alloc(bytes, HMM_BO_USER, userptr);
+}
+
+void hmm_free(ia_css_ptr virt)
+{
+	struct hmm_buffer_object *bo;
+
+	dev_dbg(atomisp_dev, "%s: free 0x%08x\n", __func__, virt);
+
+	if (WARN_ON(virt == mmgr_EXCEPTION))
+		return;
+
+	bo = hmm_bo_device_search_start(&bo_device, (unsigned int)virt);
+
+	if (!bo) {
+		dev_err(atomisp_dev,
+			"can not find buffer object start with address 0x%x\n",
+			(unsigned int)virt);
+		return;
+	}
+
+	hmm_bo_unbind(bo);
+	hmm_bo_free_pages(bo);
+	hmm_bo_unref(bo);
+}
+
+static inline int hmm_check_bo(struct hmm_buffer_object *bo, unsigned int ptr)
+{
+	if (!bo) {
+		dev_err(atomisp_dev,
+			"can not find buffer object contains address 0x%x\n",
+			ptr);
+		return -EINVAL;
+	}
+
+	if (!hmm_bo_page_allocated(bo)) {
+		dev_err(atomisp_dev,
+			"buffer object has no page allocated.\n");
+		return -EINVAL;
+	}
+
+	if (!hmm_bo_allocated(bo)) {
+		dev_err(atomisp_dev,
+			"buffer object has no virtual address space allocated.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Read function in ISP memory management */
+static int load_and_flush_by_kmap(ia_css_ptr virt, void *data,
+				  unsigned int bytes)
+{
+	struct hmm_buffer_object *bo;
+	unsigned int idx, offset, len;
+	char *src, *des;
+	int ret;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	ret = hmm_check_bo(bo, virt);
+	if (ret)
+		return ret;
+
+	des = (char *)data;
+	while (bytes) {
+		idx = (virt - bo->start) >> PAGE_SHIFT;
+		offset = (virt - bo->start) - (idx << PAGE_SHIFT);
+
+		src = (char *)kmap_local_page(bo->pages[idx]) + offset;
+
+		if ((bytes + offset) >= PAGE_SIZE) {
+			len = PAGE_SIZE - offset;
+			bytes -= len;
+		} else {
+			len = bytes;
+			bytes = 0;
+		}
+
+		virt += len;	/* update virt for next loop */
+
+		if (des) {
+			memcpy(des, src, len);
+			des += len;
+		}
+
+		clflush_cache_range(src, len);
+
+		kunmap_local(src);
+	}
+
+	return 0;
+}
+
+/* Read function in ISP memory management */
+static int load_and_flush(ia_css_ptr virt, void *data, unsigned int bytes)
+{
+	struct hmm_buffer_object *bo;
+	int ret;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	ret = hmm_check_bo(bo, virt);
+	if (ret)
+		return ret;
+
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		void *src = bo->vmap_addr;
+
+		src += (virt - bo->start);
+		memcpy(data, src, bytes);
+		if (bo->status & HMM_BO_VMAPED_CACHED)
+			clflush_cache_range(src, bytes);
+	} else {
+		void *vptr;
+
+		vptr = hmm_bo_vmap(bo, true);
+		if (!vptr)
+			return load_and_flush_by_kmap(virt, data, bytes);
+		else
+			vptr = vptr + (virt - bo->start);
+
+		memcpy(data, vptr, bytes);
+		clflush_cache_range(vptr, bytes);
+		hmm_bo_vunmap(bo);
+	}
+
+	return 0;
+}
+
+/* Read function in ISP memory management */
+int hmm_load(ia_css_ptr virt, void *data, unsigned int bytes)
+{
+	if (!virt) {
+		dev_warn(atomisp_dev,
+			"hmm_store: address is NULL\n");
+		return -EINVAL;
+	}
+	if (!data) {
+		dev_err(atomisp_dev,
+			"hmm_store: data is a NULL argument\n");
+		return -EINVAL;
+	}
+	return load_and_flush(virt, data, bytes);
+}
+
+/* Flush hmm data from the data cache */
+int hmm_flush(ia_css_ptr virt, unsigned int bytes)
+{
+	return load_and_flush(virt, NULL, bytes);
+}
+
+/* Write function in ISP memory management */
+int hmm_store(ia_css_ptr virt, const void *data, unsigned int bytes)
+{
+	struct hmm_buffer_object *bo;
+	unsigned int idx, offset, len;
+	char *src, *des;
+	int ret;
+
+	if (!virt) {
+		dev_warn(atomisp_dev,
+			"hmm_store: address is NULL\n");
+		return -EINVAL;
+	}
+	if (!data) {
+		dev_err(atomisp_dev,
+			"hmm_store: data is a NULL argument\n");
+		return -EINVAL;
+	}
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	ret = hmm_check_bo(bo, virt);
+	if (ret)
+		return ret;
+
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		void *dst = bo->vmap_addr;
+
+		dst += (virt - bo->start);
+		memcpy(dst, data, bytes);
+		if (bo->status & HMM_BO_VMAPED_CACHED)
+			clflush_cache_range(dst, bytes);
+	} else {
+		void *vptr;
+
+		vptr = hmm_bo_vmap(bo, true);
+		if (vptr) {
+			vptr = vptr + (virt - bo->start);
+
+			memcpy(vptr, data, bytes);
+			clflush_cache_range(vptr, bytes);
+			hmm_bo_vunmap(bo);
+			return 0;
+		}
+	}
+
+	src = (char *)data;
+	while (bytes) {
+		idx = (virt - bo->start) >> PAGE_SHIFT;
+		offset = (virt - bo->start) - (idx << PAGE_SHIFT);
+
+		des = (char *)kmap_local_page(bo->pages[idx]);
+
+		if (!des) {
+			dev_err(atomisp_dev,
+				"kmap buffer object page failed: pg_idx = %d\n",
+				idx);
+			return -EINVAL;
+		}
+
+		des += offset;
+
+		if ((bytes + offset) >= PAGE_SIZE) {
+			len = PAGE_SIZE - offset;
+			bytes -= len;
+		} else {
+			len = bytes;
+			bytes = 0;
+		}
+
+		virt += len;
+
+		memcpy(des, src, len);
+
+		src += len;
+
+		clflush_cache_range(des, len);
+
+		kunmap_local(des);
+	}
+
+	return 0;
+}
+
+/* memset function in ISP memory management */
+int hmm_set(ia_css_ptr virt, int c, unsigned int bytes)
+{
+	struct hmm_buffer_object *bo;
+	unsigned int idx, offset, len;
+	char *des;
+	int ret;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	ret = hmm_check_bo(bo, virt);
+	if (ret)
+		return ret;
+
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		void *dst = bo->vmap_addr;
+
+		dst += (virt - bo->start);
+		memset(dst, c, bytes);
+
+		if (bo->status & HMM_BO_VMAPED_CACHED)
+			clflush_cache_range(dst, bytes);
+	} else {
+		void *vptr;
+
+		vptr = hmm_bo_vmap(bo, true);
+		if (vptr) {
+			vptr = vptr + (virt - bo->start);
+			memset(vptr, c, bytes);
+			clflush_cache_range(vptr, bytes);
+			hmm_bo_vunmap(bo);
+			return 0;
+		}
+	}
+
+	while (bytes) {
+		idx = (virt - bo->start) >> PAGE_SHIFT;
+		offset = (virt - bo->start) - (idx << PAGE_SHIFT);
+
+		des = (char *)kmap_local_page(bo->pages[idx]) + offset;
+
+		if ((bytes + offset) >= PAGE_SIZE) {
+			len = PAGE_SIZE - offset;
+			bytes -= len;
+		} else {
+			len = bytes;
+			bytes = 0;
+		}
+
+		virt += len;
+
+		memset(des, c, len);
+
+		clflush_cache_range(des, len);
+
+		kunmap_local(des);
+	}
+
+	return 0;
+}
+
+/* Virtual address to physical address convert */
+phys_addr_t hmm_virt_to_phys(ia_css_ptr virt)
+{
+	unsigned int idx, offset;
+	struct hmm_buffer_object *bo;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	if (!bo) {
+		dev_err(atomisp_dev,
+			"can not find buffer object contains address 0x%x\n",
+			virt);
+		return -1;
+	}
+
+	idx = (virt - bo->start) >> PAGE_SHIFT;
+	offset = (virt - bo->start) - (idx << PAGE_SHIFT);
+
+	return page_to_phys(bo->pages[idx]) + offset;
+}
+
+int hmm_mmap(struct vm_area_struct *vma, ia_css_ptr virt)
+{
+	struct hmm_buffer_object *bo;
+
+	bo = hmm_bo_device_search_start(&bo_device, virt);
+	if (!bo) {
+		dev_err(atomisp_dev,
+			"can not find buffer object start with address 0x%x\n",
+			virt);
+		return -EINVAL;
+	}
+
+	return hmm_bo_mmap(vma, bo);
+}
+
+/* Map ISP virtual address into IA virtual address */
+void *hmm_vmap(ia_css_ptr virt, bool cached)
+{
+	struct hmm_buffer_object *bo;
+	void *ptr;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	if (!bo) {
+		dev_err(atomisp_dev,
+			"can not find buffer object contains address 0x%x\n",
+			virt);
+		return NULL;
+	}
+
+	ptr = hmm_bo_vmap(bo, cached);
+	if (ptr)
+		return ptr + (virt - bo->start);
+	else
+		return NULL;
+}
+
+/* Flush the memory which is mapped as cached memory through hmm_vmap */
+void hmm_flush_vmap(ia_css_ptr virt)
+{
+	struct hmm_buffer_object *bo;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	if (!bo) {
+		dev_warn(atomisp_dev,
+			 "can not find buffer object contains address 0x%x\n",
+			 virt);
+		return;
+	}
+
+	hmm_bo_flush_vmap(bo);
+}
+
+void hmm_vunmap(ia_css_ptr virt)
+{
+	struct hmm_buffer_object *bo;
+
+	bo = hmm_bo_device_search_in_range(&bo_device, virt);
+	if (!bo) {
+		dev_warn(atomisp_dev,
+			 "can not find buffer object contains address 0x%x\n",
+			 virt);
+		return;
+	}
+
+	hmm_bo_vunmap(bo);
+}
diff --git a/drivers/staging/media/atomisp/pci/hmm/hmm_bo.c b/drivers/staging/media/atomisp/pci/hmm/hmm_bo.c
new file mode 100644
index 000000000..a5fd6d38d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/hmm/hmm_bo.c
@@ -0,0 +1,1117 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+/*
+ * This file contains functions for buffer object structure management
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/gfp.h>		/* for GFP_ATOMIC */
+#include <linux/mm.h>
+#include <linux/mm_types.h>
+#include <linux/hugetlb.h>
+#include <linux/highmem.h>
+#include <linux/slab.h>		/* for kmalloc */
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/list.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <asm/current.h>
+#include <linux/sched/signal.h>
+#include <linux/file.h>
+
+#include <asm/set_memory.h>
+
+#include "atomisp_internal.h"
+#include "hmm/hmm_common.h"
+#include "hmm/hmm_bo.h"
+
+static int __bo_init(struct hmm_bo_device *bdev, struct hmm_buffer_object *bo,
+		     unsigned int pgnr)
+{
+	check_bodev_null_return(bdev, -EINVAL);
+	var_equal_return(hmm_bo_device_inited(bdev), 0, -EINVAL,
+			 "hmm_bo_device not inited yet.\n");
+	/* prevent zero size buffer object */
+	if (pgnr == 0) {
+		dev_err(atomisp_dev, "0 size buffer is not allowed.\n");
+		return -EINVAL;
+	}
+
+	memset(bo, 0, sizeof(*bo));
+	mutex_init(&bo->mutex);
+
+	/* init the bo->list HEAD as an element of entire_bo_list */
+	INIT_LIST_HEAD(&bo->list);
+
+	bo->bdev = bdev;
+	bo->vmap_addr = NULL;
+	bo->status = HMM_BO_FREE;
+	bo->start = bdev->start;
+	bo->pgnr = pgnr;
+	bo->end = bo->start + pgnr_to_size(pgnr);
+	bo->prev = NULL;
+	bo->next = NULL;
+
+	return 0;
+}
+
+static struct hmm_buffer_object *__bo_search_and_remove_from_free_rbtree(
+    struct rb_node *node, unsigned int pgnr)
+{
+	struct hmm_buffer_object *this, *ret_bo, *temp_bo;
+
+	this = rb_entry(node, struct hmm_buffer_object, node);
+	if (this->pgnr == pgnr ||
+	    (this->pgnr > pgnr && !this->node.rb_left)) {
+		goto remove_bo_and_return;
+	} else {
+		if (this->pgnr < pgnr) {
+			if (!this->node.rb_right)
+				return NULL;
+			ret_bo = __bo_search_and_remove_from_free_rbtree(
+				     this->node.rb_right, pgnr);
+		} else {
+			ret_bo = __bo_search_and_remove_from_free_rbtree(
+				     this->node.rb_left, pgnr);
+		}
+		if (!ret_bo) {
+			if (this->pgnr > pgnr)
+				goto remove_bo_and_return;
+			else
+				return NULL;
+		}
+		return ret_bo;
+	}
+
+remove_bo_and_return:
+	/* NOTE: All nodes on free rbtree have a 'prev' that points to NULL.
+	 * 1. check if 'this->next' is NULL:
+	 *	yes: erase 'this' node and rebalance rbtree, return 'this'.
+	 */
+	if (!this->next) {
+		rb_erase(&this->node, &this->bdev->free_rbtree);
+		return this;
+	}
+	/* NOTE: if 'this->next' is not NULL, always return 'this->next' bo.
+	 * 2. check if 'this->next->next' is NULL:
+	 *	yes: change the related 'next/prev' pointer,
+	 *		return 'this->next' but the rbtree stays unchanged.
+	 */
+	temp_bo = this->next;
+	this->next = temp_bo->next;
+	if (temp_bo->next)
+		temp_bo->next->prev = this;
+	temp_bo->next = NULL;
+	temp_bo->prev = NULL;
+	return temp_bo;
+}
+
+static struct hmm_buffer_object *__bo_search_by_addr(struct rb_root *root,
+	ia_css_ptr start)
+{
+	struct rb_node *n = root->rb_node;
+	struct hmm_buffer_object *bo;
+
+	do {
+		bo = rb_entry(n, struct hmm_buffer_object, node);
+
+		if (bo->start > start) {
+			if (!n->rb_left)
+				return NULL;
+			n = n->rb_left;
+		} else if (bo->start < start) {
+			if (!n->rb_right)
+				return NULL;
+			n = n->rb_right;
+		} else {
+			return bo;
+		}
+	} while (n);
+
+	return NULL;
+}
+
+static struct hmm_buffer_object *__bo_search_by_addr_in_range(
+    struct rb_root *root, unsigned int start)
+{
+	struct rb_node *n = root->rb_node;
+	struct hmm_buffer_object *bo;
+
+	do {
+		bo = rb_entry(n, struct hmm_buffer_object, node);
+
+		if (bo->start > start) {
+			if (!n->rb_left)
+				return NULL;
+			n = n->rb_left;
+		} else {
+			if (bo->end > start)
+				return bo;
+			if (!n->rb_right)
+				return NULL;
+			n = n->rb_right;
+		}
+	} while (n);
+
+	return NULL;
+}
+
+static void __bo_insert_to_free_rbtree(struct rb_root *root,
+				       struct hmm_buffer_object *bo)
+{
+	struct rb_node **new = &root->rb_node;
+	struct rb_node *parent = NULL;
+	struct hmm_buffer_object *this;
+	unsigned int pgnr = bo->pgnr;
+
+	while (*new) {
+		parent = *new;
+		this = container_of(*new, struct hmm_buffer_object, node);
+
+		if (pgnr < this->pgnr) {
+			new = &((*new)->rb_left);
+		} else if (pgnr > this->pgnr) {
+			new = &((*new)->rb_right);
+		} else {
+			bo->prev = this;
+			bo->next = this->next;
+			if (this->next)
+				this->next->prev = bo;
+			this->next = bo;
+			bo->status = (bo->status & ~HMM_BO_MASK) | HMM_BO_FREE;
+			return;
+		}
+	}
+
+	bo->status = (bo->status & ~HMM_BO_MASK) | HMM_BO_FREE;
+
+	rb_link_node(&bo->node, parent, new);
+	rb_insert_color(&bo->node, root);
+}
+
+static void __bo_insert_to_alloc_rbtree(struct rb_root *root,
+					struct hmm_buffer_object *bo)
+{
+	struct rb_node **new = &root->rb_node;
+	struct rb_node *parent = NULL;
+	struct hmm_buffer_object *this;
+	unsigned int start = bo->start;
+
+	while (*new) {
+		parent = *new;
+		this = container_of(*new, struct hmm_buffer_object, node);
+
+		if (start < this->start)
+			new = &((*new)->rb_left);
+		else
+			new = &((*new)->rb_right);
+	}
+
+	kref_init(&bo->kref);
+	bo->status = (bo->status & ~HMM_BO_MASK) | HMM_BO_ALLOCED;
+
+	rb_link_node(&bo->node, parent, new);
+	rb_insert_color(&bo->node, root);
+}
+
+static struct hmm_buffer_object *__bo_break_up(struct hmm_bo_device *bdev,
+	struct hmm_buffer_object *bo,
+	unsigned int pgnr)
+{
+	struct hmm_buffer_object *new_bo;
+	unsigned long flags;
+	int ret;
+
+	new_bo = kmem_cache_alloc(bdev->bo_cache, GFP_KERNEL);
+	if (!new_bo) {
+		dev_err(atomisp_dev, "%s: __bo_alloc failed!\n", __func__);
+		return NULL;
+	}
+	ret = __bo_init(bdev, new_bo, pgnr);
+	if (ret) {
+		dev_err(atomisp_dev, "%s: __bo_init failed!\n", __func__);
+		kmem_cache_free(bdev->bo_cache, new_bo);
+		return NULL;
+	}
+
+	new_bo->start = bo->start;
+	new_bo->end = new_bo->start + pgnr_to_size(pgnr);
+	bo->start = new_bo->end;
+	bo->pgnr = bo->pgnr - pgnr;
+
+	spin_lock_irqsave(&bdev->list_lock, flags);
+	list_add_tail(&new_bo->list, &bo->list);
+	spin_unlock_irqrestore(&bdev->list_lock, flags);
+
+	return new_bo;
+}
+
+static void __bo_take_off_handling(struct hmm_buffer_object *bo)
+{
+	struct hmm_bo_device *bdev = bo->bdev;
+	/* There are 4 situations when we take off a known bo from free rbtree:
+	 * 1. if bo->next && bo->prev == NULL, bo is a rbtree node
+	 *	and does not have a linked list after bo, to take off this bo,
+	 *	we just need erase bo directly and rebalance the free rbtree
+	 */
+	if (!bo->prev && !bo->next) {
+		rb_erase(&bo->node, &bdev->free_rbtree);
+		/* 2. when bo->next != NULL && bo->prev == NULL, bo is a rbtree node,
+		 *	and has a linked list,to take off this bo we need erase bo
+		 *	first, then, insert bo->next into free rbtree and rebalance
+		 *	the free rbtree
+		 */
+	} else if (!bo->prev && bo->next) {
+		bo->next->prev = NULL;
+		rb_erase(&bo->node, &bdev->free_rbtree);
+		__bo_insert_to_free_rbtree(&bdev->free_rbtree, bo->next);
+		bo->next = NULL;
+		/* 3. when bo->prev != NULL && bo->next == NULL, bo is not a rbtree
+		 *	node, bo is the last element of the linked list after rbtree
+		 *	node, to take off this bo, we just need set the "prev/next"
+		 *	pointers to NULL, the free rbtree stays unchaged
+		 */
+	} else if (bo->prev && !bo->next) {
+		bo->prev->next = NULL;
+		bo->prev = NULL;
+		/* 4. when bo->prev != NULL && bo->next != NULL ,bo is not a rbtree
+		 *	node, bo is in the middle of the linked list after rbtree node,
+		 *	to take off this bo, we just set take the "prev/next" pointers
+		 *	to NULL, the free rbtree stays unchaged
+		 */
+	} else if (bo->prev && bo->next) {
+		bo->next->prev = bo->prev;
+		bo->prev->next = bo->next;
+		bo->next = NULL;
+		bo->prev = NULL;
+	}
+}
+
+static struct hmm_buffer_object *__bo_merge(struct hmm_buffer_object *bo,
+	struct hmm_buffer_object *next_bo)
+{
+	struct hmm_bo_device *bdev;
+	unsigned long flags;
+
+	bdev = bo->bdev;
+	next_bo->start = bo->start;
+	next_bo->pgnr = next_bo->pgnr + bo->pgnr;
+
+	spin_lock_irqsave(&bdev->list_lock, flags);
+	list_del(&bo->list);
+	spin_unlock_irqrestore(&bdev->list_lock, flags);
+
+	kmem_cache_free(bo->bdev->bo_cache, bo);
+
+	return next_bo;
+}
+
+/*
+ * hmm_bo_device functions.
+ */
+int hmm_bo_device_init(struct hmm_bo_device *bdev,
+		       struct isp_mmu_client *mmu_driver,
+		       unsigned int vaddr_start,
+		       unsigned int size)
+{
+	struct hmm_buffer_object *bo;
+	unsigned long flags;
+	int ret;
+
+	check_bodev_null_return(bdev, -EINVAL);
+
+	ret = isp_mmu_init(&bdev->mmu, mmu_driver);
+	if (ret) {
+		dev_err(atomisp_dev, "isp_mmu_init failed.\n");
+		return ret;
+	}
+
+	bdev->start = vaddr_start;
+	bdev->pgnr = size_to_pgnr_ceil(size);
+	bdev->size = pgnr_to_size(bdev->pgnr);
+
+	spin_lock_init(&bdev->list_lock);
+	mutex_init(&bdev->rbtree_mutex);
+
+	bdev->flag = HMM_BO_DEVICE_INITED;
+
+	INIT_LIST_HEAD(&bdev->entire_bo_list);
+	bdev->allocated_rbtree = RB_ROOT;
+	bdev->free_rbtree = RB_ROOT;
+
+	bdev->bo_cache = kmem_cache_create("bo_cache",
+					   sizeof(struct hmm_buffer_object), 0, 0, NULL);
+	if (!bdev->bo_cache) {
+		dev_err(atomisp_dev, "%s: create cache failed!\n", __func__);
+		isp_mmu_exit(&bdev->mmu);
+		return -ENOMEM;
+	}
+
+	bo = kmem_cache_alloc(bdev->bo_cache, GFP_KERNEL);
+	if (!bo) {
+		dev_err(atomisp_dev, "%s: __bo_alloc failed!\n", __func__);
+		isp_mmu_exit(&bdev->mmu);
+		return -ENOMEM;
+	}
+
+	ret = __bo_init(bdev, bo, bdev->pgnr);
+	if (ret) {
+		dev_err(atomisp_dev, "%s: __bo_init failed!\n", __func__);
+		kmem_cache_free(bdev->bo_cache, bo);
+		isp_mmu_exit(&bdev->mmu);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&bdev->list_lock, flags);
+	list_add_tail(&bo->list, &bdev->entire_bo_list);
+	spin_unlock_irqrestore(&bdev->list_lock, flags);
+
+	__bo_insert_to_free_rbtree(&bdev->free_rbtree, bo);
+
+	return 0;
+}
+
+struct hmm_buffer_object *hmm_bo_alloc(struct hmm_bo_device *bdev,
+				       unsigned int pgnr)
+{
+	struct hmm_buffer_object *bo, *new_bo;
+	struct rb_root *root = &bdev->free_rbtree;
+
+	check_bodev_null_return(bdev, NULL);
+	var_equal_return(hmm_bo_device_inited(bdev), 0, NULL,
+			 "hmm_bo_device not inited yet.\n");
+
+	if (pgnr == 0) {
+		dev_err(atomisp_dev, "0 size buffer is not allowed.\n");
+		return NULL;
+	}
+
+	mutex_lock(&bdev->rbtree_mutex);
+	bo = __bo_search_and_remove_from_free_rbtree(root->rb_node, pgnr);
+	if (!bo) {
+		mutex_unlock(&bdev->rbtree_mutex);
+		dev_err(atomisp_dev, "%s: Out of Memory! hmm_bo_alloc failed",
+			__func__);
+		return NULL;
+	}
+
+	if (bo->pgnr > pgnr) {
+		new_bo = __bo_break_up(bdev, bo, pgnr);
+		if (!new_bo) {
+			mutex_unlock(&bdev->rbtree_mutex);
+			dev_err(atomisp_dev, "%s: __bo_break_up failed!\n",
+				__func__);
+			return NULL;
+		}
+
+		__bo_insert_to_alloc_rbtree(&bdev->allocated_rbtree, new_bo);
+		__bo_insert_to_free_rbtree(&bdev->free_rbtree, bo);
+
+		mutex_unlock(&bdev->rbtree_mutex);
+		return new_bo;
+	}
+
+	__bo_insert_to_alloc_rbtree(&bdev->allocated_rbtree, bo);
+
+	mutex_unlock(&bdev->rbtree_mutex);
+	return bo;
+}
+
+void hmm_bo_release(struct hmm_buffer_object *bo)
+{
+	struct hmm_bo_device *bdev = bo->bdev;
+	struct hmm_buffer_object *next_bo, *prev_bo;
+
+	mutex_lock(&bdev->rbtree_mutex);
+
+	/*
+	 * FIX ME:
+	 *
+	 * how to destroy the bo when it is stilled MMAPED?
+	 *
+	 * ideally, this will not happened as hmm_bo_release
+	 * will only be called when kref reaches 0, and in mmap
+	 * operation the hmm_bo_ref will eventually be called.
+	 * so, if this happened, something goes wrong.
+	 */
+	if (bo->status & HMM_BO_MMAPED) {
+		mutex_unlock(&bdev->rbtree_mutex);
+		dev_dbg(atomisp_dev, "destroy bo which is MMAPED, do nothing\n");
+		return;
+	}
+
+	if (bo->status & HMM_BO_BINDED) {
+		dev_warn(atomisp_dev, "the bo is still binded, unbind it first...\n");
+		hmm_bo_unbind(bo);
+	}
+
+	if (bo->status & HMM_BO_PAGE_ALLOCED) {
+		dev_warn(atomisp_dev, "the pages is not freed, free pages first\n");
+		hmm_bo_free_pages(bo);
+	}
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		dev_warn(atomisp_dev, "the vunmap is not done, do it...\n");
+		hmm_bo_vunmap(bo);
+	}
+
+	rb_erase(&bo->node, &bdev->allocated_rbtree);
+
+	prev_bo = list_entry(bo->list.prev, struct hmm_buffer_object, list);
+	next_bo = list_entry(bo->list.next, struct hmm_buffer_object, list);
+
+	if (bo->list.prev != &bdev->entire_bo_list &&
+	    prev_bo->end == bo->start &&
+	    (prev_bo->status & HMM_BO_MASK) == HMM_BO_FREE) {
+		__bo_take_off_handling(prev_bo);
+		bo = __bo_merge(prev_bo, bo);
+	}
+
+	if (bo->list.next != &bdev->entire_bo_list &&
+	    next_bo->start == bo->end &&
+	    (next_bo->status & HMM_BO_MASK) == HMM_BO_FREE) {
+		__bo_take_off_handling(next_bo);
+		bo = __bo_merge(bo, next_bo);
+	}
+
+	__bo_insert_to_free_rbtree(&bdev->free_rbtree, bo);
+
+	mutex_unlock(&bdev->rbtree_mutex);
+	return;
+}
+
+void hmm_bo_device_exit(struct hmm_bo_device *bdev)
+{
+	struct hmm_buffer_object *bo;
+	unsigned long flags;
+
+	dev_dbg(atomisp_dev, "%s: entering!\n", __func__);
+
+	check_bodev_null_return_void(bdev);
+
+	/*
+	 * release all allocated bos even they a in use
+	 * and all bos will be merged into a big bo
+	 */
+	while (!RB_EMPTY_ROOT(&bdev->allocated_rbtree))
+		hmm_bo_release(
+		    rbtree_node_to_hmm_bo(bdev->allocated_rbtree.rb_node));
+
+	dev_dbg(atomisp_dev, "%s: finished releasing all allocated bos!\n",
+		__func__);
+
+	/* free all bos to release all ISP virtual memory */
+	while (!list_empty(&bdev->entire_bo_list)) {
+		bo = list_to_hmm_bo(bdev->entire_bo_list.next);
+
+		spin_lock_irqsave(&bdev->list_lock, flags);
+		list_del(&bo->list);
+		spin_unlock_irqrestore(&bdev->list_lock, flags);
+
+		kmem_cache_free(bdev->bo_cache, bo);
+	}
+
+	dev_dbg(atomisp_dev, "%s: finished to free all bos!\n", __func__);
+
+	kmem_cache_destroy(bdev->bo_cache);
+
+	isp_mmu_exit(&bdev->mmu);
+}
+
+int hmm_bo_device_inited(struct hmm_bo_device *bdev)
+{
+	check_bodev_null_return(bdev, -EINVAL);
+
+	return bdev->flag == HMM_BO_DEVICE_INITED;
+}
+
+int hmm_bo_allocated(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return(bo, 0);
+
+	return bo->status & HMM_BO_ALLOCED;
+}
+
+struct hmm_buffer_object *hmm_bo_device_search_start(
+    struct hmm_bo_device *bdev, ia_css_ptr vaddr)
+{
+	struct hmm_buffer_object *bo;
+
+	check_bodev_null_return(bdev, NULL);
+
+	mutex_lock(&bdev->rbtree_mutex);
+	bo = __bo_search_by_addr(&bdev->allocated_rbtree, vaddr);
+	if (!bo) {
+		mutex_unlock(&bdev->rbtree_mutex);
+		dev_err(atomisp_dev, "%s can not find bo with addr: 0x%x\n",
+			__func__, vaddr);
+		return NULL;
+	}
+	mutex_unlock(&bdev->rbtree_mutex);
+
+	return bo;
+}
+
+struct hmm_buffer_object *hmm_bo_device_search_in_range(
+    struct hmm_bo_device *bdev, unsigned int vaddr)
+{
+	struct hmm_buffer_object *bo;
+
+	check_bodev_null_return(bdev, NULL);
+
+	mutex_lock(&bdev->rbtree_mutex);
+	bo = __bo_search_by_addr_in_range(&bdev->allocated_rbtree, vaddr);
+	if (!bo) {
+		mutex_unlock(&bdev->rbtree_mutex);
+		dev_err(atomisp_dev, "%s can not find bo contain addr: 0x%x\n",
+			__func__, vaddr);
+		return NULL;
+	}
+	mutex_unlock(&bdev->rbtree_mutex);
+
+	return bo;
+}
+
+struct hmm_buffer_object *hmm_bo_device_search_vmap_start(
+    struct hmm_bo_device *bdev, const void *vaddr)
+{
+	struct list_head *pos;
+	struct hmm_buffer_object *bo;
+	unsigned long flags;
+
+	check_bodev_null_return(bdev, NULL);
+
+	spin_lock_irqsave(&bdev->list_lock, flags);
+	list_for_each(pos, &bdev->entire_bo_list) {
+		bo = list_to_hmm_bo(pos);
+		/* pass bo which has no vm_node allocated */
+		if ((bo->status & HMM_BO_MASK) == HMM_BO_FREE)
+			continue;
+		if (bo->vmap_addr == vaddr)
+			goto found;
+	}
+	spin_unlock_irqrestore(&bdev->list_lock, flags);
+	return NULL;
+found:
+	spin_unlock_irqrestore(&bdev->list_lock, flags);
+	return bo;
+}
+
+static void free_pages_bulk_array(unsigned long nr_pages, struct page **page_array)
+{
+	unsigned long i;
+
+	for (i = 0; i < nr_pages; i++)
+		__free_pages(page_array[i], 0);
+}
+
+static void free_private_bo_pages(struct hmm_buffer_object *bo)
+{
+	set_pages_array_wb(bo->pages, bo->pgnr);
+	free_pages_bulk_array(bo->pgnr, bo->pages);
+}
+
+/*Allocate pages which will be used only by ISP*/
+static int alloc_private_pages(struct hmm_buffer_object *bo)
+{
+	const gfp_t gfp = __GFP_NOWARN | __GFP_RECLAIM | __GFP_FS;
+	int ret;
+
+	ret = alloc_pages_bulk_array(gfp, bo->pgnr, bo->pages);
+	if (ret != bo->pgnr) {
+		free_pages_bulk_array(ret, bo->pages);
+		return -ENOMEM;
+	}
+
+	ret = set_pages_array_uc(bo->pages, bo->pgnr);
+	if (ret) {
+		dev_err(atomisp_dev, "set pages uncacheable failed.\n");
+		free_pages_bulk_array(bo->pgnr, bo->pages);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void free_user_pages(struct hmm_buffer_object *bo,
+			    unsigned int page_nr)
+{
+	int i;
+
+	for (i = 0; i < page_nr; i++)
+		put_page(bo->pages[i]);
+}
+
+/*
+ * Convert user space virtual address into pages list
+ */
+static int alloc_user_pages(struct hmm_buffer_object *bo,
+			    const void __user *userptr)
+{
+	int page_nr;
+
+	userptr = untagged_addr(userptr);
+
+	/* Handle frame buffer allocated in user space */
+	mutex_unlock(&bo->mutex);
+	page_nr = get_user_pages_fast((unsigned long)userptr, bo->pgnr, 1, bo->pages);
+	mutex_lock(&bo->mutex);
+
+	/* can be written by caller, not forced */
+	if (page_nr != bo->pgnr) {
+		dev_err(atomisp_dev,
+			"get_user_pages err: bo->pgnr = %d, pgnr actually pinned = %d.\n",
+			bo->pgnr, page_nr);
+		if (page_nr < 0)
+			page_nr = 0;
+		goto out_of_mem;
+	}
+
+	return 0;
+
+out_of_mem:
+
+	free_user_pages(bo, page_nr);
+
+	return -ENOMEM;
+}
+
+/*
+ * allocate/free physical pages for the bo.
+ *
+ * type indicate where are the pages from. currently we have 3 types
+ * of memory: HMM_BO_PRIVATE, HMM_BO_USER.
+ *
+ * userptr is only valid when type is HMM_BO_USER, it indicates
+ * the start address from user space task.
+ */
+int hmm_bo_alloc_pages(struct hmm_buffer_object *bo,
+		       enum hmm_bo_type type,
+		       const void __user *userptr)
+{
+	int ret = -EINVAL;
+
+	check_bo_null_return(bo, -EINVAL);
+
+	mutex_lock(&bo->mutex);
+	check_bo_status_no_goto(bo, HMM_BO_PAGE_ALLOCED, status_err);
+
+	bo->pages = kcalloc(bo->pgnr, sizeof(struct page *), GFP_KERNEL);
+	if (unlikely(!bo->pages)) {
+		ret = -ENOMEM;
+		goto alloc_err;
+	}
+
+	/*
+	 * TO DO:
+	 * add HMM_BO_USER type
+	 */
+	if (type == HMM_BO_PRIVATE) {
+		ret = alloc_private_pages(bo);
+	} else if (type == HMM_BO_USER) {
+		ret = alloc_user_pages(bo, userptr);
+	} else {
+		dev_err(atomisp_dev, "invalid buffer type.\n");
+		ret = -EINVAL;
+	}
+	if (ret)
+		goto alloc_err;
+
+	bo->type = type;
+
+	bo->status |= HMM_BO_PAGE_ALLOCED;
+
+	mutex_unlock(&bo->mutex);
+
+	return 0;
+
+alloc_err:
+	kfree(bo->pages);
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev, "alloc pages err...\n");
+	return ret;
+status_err:
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev,
+		"buffer object has already page allocated.\n");
+	return -EINVAL;
+}
+
+/*
+ * free physical pages of the bo.
+ */
+void hmm_bo_free_pages(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return_void(bo);
+
+	mutex_lock(&bo->mutex);
+
+	check_bo_status_yes_goto(bo, HMM_BO_PAGE_ALLOCED, status_err2);
+
+	/* clear the flag anyway. */
+	bo->status &= (~HMM_BO_PAGE_ALLOCED);
+
+	if (bo->type == HMM_BO_PRIVATE)
+		free_private_bo_pages(bo);
+	else if (bo->type == HMM_BO_USER)
+		free_user_pages(bo, bo->pgnr);
+	else
+		dev_err(atomisp_dev, "invalid buffer type.\n");
+
+	kfree(bo->pages);
+	mutex_unlock(&bo->mutex);
+
+	return;
+
+status_err2:
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev,
+		"buffer object not page allocated yet.\n");
+}
+
+int hmm_bo_page_allocated(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return(bo, 0);
+
+	return bo->status & HMM_BO_PAGE_ALLOCED;
+}
+
+/*
+ * bind the physical pages to a virtual address space.
+ */
+int hmm_bo_bind(struct hmm_buffer_object *bo)
+{
+	int ret;
+	unsigned int virt;
+	struct hmm_bo_device *bdev;
+	unsigned int i;
+
+	check_bo_null_return(bo, -EINVAL);
+
+	mutex_lock(&bo->mutex);
+
+	check_bo_status_yes_goto(bo,
+				 HMM_BO_PAGE_ALLOCED | HMM_BO_ALLOCED,
+				 status_err1);
+
+	check_bo_status_no_goto(bo, HMM_BO_BINDED, status_err2);
+
+	bdev = bo->bdev;
+
+	virt = bo->start;
+
+	for (i = 0; i < bo->pgnr; i++) {
+		ret =
+		    isp_mmu_map(&bdev->mmu, virt,
+				page_to_phys(bo->pages[i]), 1);
+		if (ret)
+			goto map_err;
+		virt += (1 << PAGE_SHIFT);
+	}
+
+	/*
+	 * flush TBL here.
+	 *
+	 * theoretically, we donot need to flush TLB as we didnot change
+	 * any existed address mappings, but for Silicon Hive's MMU, its
+	 * really a bug here. I guess when fetching PTEs (page table entity)
+	 * to TLB, its MMU will fetch additional INVALID PTEs automatically
+	 * for performance issue. EX, we only set up 1 page address mapping,
+	 * meaning updating 1 PTE, but the MMU fetches 4 PTE at one time,
+	 * so the additional 3 PTEs are invalid.
+	 */
+	if (bo->start != 0x0)
+		isp_mmu_flush_tlb_range(&bdev->mmu, bo->start,
+					(bo->pgnr << PAGE_SHIFT));
+
+	bo->status |= HMM_BO_BINDED;
+
+	mutex_unlock(&bo->mutex);
+
+	return 0;
+
+map_err:
+	/* unbind the physical pages with related virtual address space */
+	virt = bo->start;
+	for ( ; i > 0; i--) {
+		isp_mmu_unmap(&bdev->mmu, virt, 1);
+		virt += pgnr_to_size(1);
+	}
+
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev,
+		"setup MMU address mapping failed.\n");
+	return ret;
+
+status_err2:
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev, "buffer object already binded.\n");
+	return -EINVAL;
+status_err1:
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev,
+		"buffer object vm_node or page not allocated.\n");
+	return -EINVAL;
+}
+
+/*
+ * unbind the physical pages with related virtual address space.
+ */
+void hmm_bo_unbind(struct hmm_buffer_object *bo)
+{
+	unsigned int virt;
+	struct hmm_bo_device *bdev;
+	unsigned int i;
+
+	check_bo_null_return_void(bo);
+
+	mutex_lock(&bo->mutex);
+
+	check_bo_status_yes_goto(bo,
+				 HMM_BO_PAGE_ALLOCED |
+				 HMM_BO_ALLOCED |
+				 HMM_BO_BINDED, status_err);
+
+	bdev = bo->bdev;
+
+	virt = bo->start;
+
+	for (i = 0; i < bo->pgnr; i++) {
+		isp_mmu_unmap(&bdev->mmu, virt, 1);
+		virt += pgnr_to_size(1);
+	}
+
+	/*
+	 * flush TLB as the address mapping has been removed and
+	 * related TLBs should be invalidated.
+	 */
+	isp_mmu_flush_tlb_range(&bdev->mmu, bo->start,
+				(bo->pgnr << PAGE_SHIFT));
+
+	bo->status &= (~HMM_BO_BINDED);
+
+	mutex_unlock(&bo->mutex);
+
+	return;
+
+status_err:
+	mutex_unlock(&bo->mutex);
+	dev_err(atomisp_dev,
+		"buffer vm or page not allocated or not binded yet.\n");
+}
+
+int hmm_bo_binded(struct hmm_buffer_object *bo)
+{
+	int ret;
+
+	check_bo_null_return(bo, 0);
+
+	mutex_lock(&bo->mutex);
+
+	ret = bo->status & HMM_BO_BINDED;
+
+	mutex_unlock(&bo->mutex);
+
+	return ret;
+}
+
+void *hmm_bo_vmap(struct hmm_buffer_object *bo, bool cached)
+{
+	check_bo_null_return(bo, NULL);
+
+	mutex_lock(&bo->mutex);
+	if (((bo->status & HMM_BO_VMAPED) && !cached) ||
+	    ((bo->status & HMM_BO_VMAPED_CACHED) && cached)) {
+		mutex_unlock(&bo->mutex);
+		return bo->vmap_addr;
+	}
+
+	/* cached status need to be changed, so vunmap first */
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		vunmap(bo->vmap_addr);
+		bo->vmap_addr = NULL;
+		bo->status &= ~(HMM_BO_VMAPED | HMM_BO_VMAPED_CACHED);
+	}
+
+	bo->vmap_addr = vmap(bo->pages, bo->pgnr, VM_MAP,
+			     cached ? PAGE_KERNEL : PAGE_KERNEL_NOCACHE);
+	if (unlikely(!bo->vmap_addr)) {
+		mutex_unlock(&bo->mutex);
+		dev_err(atomisp_dev, "vmap failed...\n");
+		return NULL;
+	}
+	bo->status |= (cached ? HMM_BO_VMAPED_CACHED : HMM_BO_VMAPED);
+
+	mutex_unlock(&bo->mutex);
+	return bo->vmap_addr;
+}
+
+void hmm_bo_flush_vmap(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return_void(bo);
+
+	mutex_lock(&bo->mutex);
+	if (!(bo->status & HMM_BO_VMAPED_CACHED) || !bo->vmap_addr) {
+		mutex_unlock(&bo->mutex);
+		return;
+	}
+
+	clflush_cache_range(bo->vmap_addr, bo->pgnr * PAGE_SIZE);
+	mutex_unlock(&bo->mutex);
+}
+
+void hmm_bo_vunmap(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return_void(bo);
+
+	mutex_lock(&bo->mutex);
+	if (bo->status & HMM_BO_VMAPED || bo->status & HMM_BO_VMAPED_CACHED) {
+		vunmap(bo->vmap_addr);
+		bo->vmap_addr = NULL;
+		bo->status &= ~(HMM_BO_VMAPED | HMM_BO_VMAPED_CACHED);
+	}
+
+	mutex_unlock(&bo->mutex);
+	return;
+}
+
+void hmm_bo_ref(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return_void(bo);
+
+	kref_get(&bo->kref);
+}
+
+static void kref_hmm_bo_release(struct kref *kref)
+{
+	if (!kref)
+		return;
+
+	hmm_bo_release(kref_to_hmm_bo(kref));
+}
+
+void hmm_bo_unref(struct hmm_buffer_object *bo)
+{
+	check_bo_null_return_void(bo);
+
+	kref_put(&bo->kref, kref_hmm_bo_release);
+}
+
+static void hmm_bo_vm_open(struct vm_area_struct *vma)
+{
+	struct hmm_buffer_object *bo =
+	    (struct hmm_buffer_object *)vma->vm_private_data;
+
+	check_bo_null_return_void(bo);
+
+	hmm_bo_ref(bo);
+
+	mutex_lock(&bo->mutex);
+
+	bo->status |= HMM_BO_MMAPED;
+
+	bo->mmap_count++;
+
+	mutex_unlock(&bo->mutex);
+}
+
+static void hmm_bo_vm_close(struct vm_area_struct *vma)
+{
+	struct hmm_buffer_object *bo =
+	    (struct hmm_buffer_object *)vma->vm_private_data;
+
+	check_bo_null_return_void(bo);
+
+	hmm_bo_unref(bo);
+
+	mutex_lock(&bo->mutex);
+
+	bo->mmap_count--;
+
+	if (!bo->mmap_count) {
+		bo->status &= (~HMM_BO_MMAPED);
+		vma->vm_private_data = NULL;
+	}
+
+	mutex_unlock(&bo->mutex);
+}
+
+static const struct vm_operations_struct hmm_bo_vm_ops = {
+	.open = hmm_bo_vm_open,
+	.close = hmm_bo_vm_close,
+};
+
+/*
+ * mmap the bo to user space.
+ */
+int hmm_bo_mmap(struct vm_area_struct *vma, struct hmm_buffer_object *bo)
+{
+	unsigned int start, end;
+	unsigned int virt;
+	unsigned int pgnr, i;
+	unsigned int pfn;
+
+	check_bo_null_return(bo, -EINVAL);
+
+	check_bo_status_yes_goto(bo, HMM_BO_PAGE_ALLOCED, status_err);
+
+	pgnr = bo->pgnr;
+	start = vma->vm_start;
+	end = vma->vm_end;
+
+	/*
+	 * check vma's virtual address space size and buffer object's size.
+	 * must be the same.
+	 */
+	if ((start + pgnr_to_size(pgnr)) != end) {
+		dev_warn(atomisp_dev,
+			 "vma's address space size not equal to buffer object's size");
+		return -EINVAL;
+	}
+
+	virt = vma->vm_start;
+	for (i = 0; i < pgnr; i++) {
+		pfn = page_to_pfn(bo->pages[i]);
+		if (remap_pfn_range(vma, virt, pfn, PAGE_SIZE, PAGE_SHARED)) {
+			dev_warn(atomisp_dev,
+				 "remap_pfn_range failed: virt = 0x%x, pfn = 0x%x, mapped_pgnr = %d\n",
+				 virt, pfn, 1);
+			return -EINVAL;
+		}
+		virt += PAGE_SIZE;
+	}
+
+	vma->vm_private_data = bo;
+
+	vma->vm_ops = &hmm_bo_vm_ops;
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
+
+	/*
+	 * call hmm_bo_vm_open explicitly.
+	 */
+	hmm_bo_vm_open(vma);
+
+	return 0;
+
+status_err:
+	dev_err(atomisp_dev, "buffer page not allocated yet.\n");
+	return -EINVAL;
+}
diff --git a/drivers/staging/media/atomisp/pci/ia_css.h b/drivers/staging/media/atomisp/pci/ia_css.h
new file mode 100644
index 000000000..d83e1ae5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_H_
+#define _IA_CSS_H_
+
+/* @file
+ * This file is the starting point of the CSS-API. It includes all CSS-API
+ * header files.
+ */
+
+#include "ia_css_3a.h"
+#include "ia_css_acc_types.h"
+#include "ia_css_buffer.h"
+#include "ia_css_control.h"
+#include "ia_css_device_access.h"
+#include "ia_css_dvs.h"
+#include "ia_css_env.h"
+#include "ia_css_err.h"
+#include "ia_css_event_public.h"
+#include "ia_css_firmware.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_input_port.h"
+#include "ia_css_irq.h"
+#include "ia_css_metadata.h"
+#include "ia_css_mipi.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_prbs.h"
+#include "ia_css_properties.h"
+#include "ia_css_stream_format.h"
+#include "ia_css_stream_public.h"
+#include "ia_css_tpg.h"
+#include "ia_css_version.h"
+#include "ia_css_mmu.h"
+#include "ia_css_morph.h"
+#include "ia_css_shading.h"
+#include "ia_css_timer.h"
+
+/*
+   Please do not add code to this file. Public functionality is to be
+   exposed in a function/data type specific header file.
+   Please add to the appropriate header file or create a new one.
+ */
+
+#endif /* _IA_CSS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_3a.h b/drivers/staging/media/atomisp/pci/ia_css_3a.h
new file mode 100644
index 000000000..70cfc915c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_3a.h
@@ -0,0 +1,190 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_3A_H
+#define __IA_CSS_3A_H
+
+/* @file
+ * This file contains types used for 3A statistics
+ */
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_err.h"
+#include "system_global.h"
+
+enum ia_css_3a_tables {
+	IA_CSS_S3A_TBL_HI,
+	IA_CSS_S3A_TBL_LO,
+	IA_CSS_RGBY_TBL,
+	IA_CSS_NUM_3A_TABLES
+};
+
+/* Structure that holds 3A statistics in the ISP internal
+ * format. Use ia_css_get_3a_statistics() to translate
+ * this to the format used on the host (3A library).
+ * */
+struct ia_css_isp_3a_statistics {
+	union {
+		struct {
+			ia_css_ptr s3a_tbl;
+		} dmem;
+		struct {
+			ia_css_ptr s3a_tbl_hi;
+			ia_css_ptr s3a_tbl_lo;
+		} vmem;
+	} data;
+	struct {
+		ia_css_ptr rgby_tbl;
+	} data_hmem;
+	u32 exp_id;     /** exposure id, to match statistics to a frame,
+				  see ia_css_event_public.h for more detail. */
+	u32 isp_config_id;/** Unique ID to track which config was actually applied to a particular frame */
+	ia_css_ptr data_ptr; /** pointer to base of all data */
+	u32   size;     /** total size of all data */
+	u32   dmem_size;
+	u32   vmem_size; /** both lo and hi have this size */
+	u32   hmem_size;
+};
+
+#define SIZE_OF_DMEM_STRUCT						\
+	(SIZE_OF_IA_CSS_PTR)
+
+#define SIZE_OF_VMEM_STRUCT						\
+	(2 * SIZE_OF_IA_CSS_PTR)
+
+#define SIZE_OF_DATA_UNION						\
+	(MAX(SIZE_OF_DMEM_STRUCT, SIZE_OF_VMEM_STRUCT))
+
+#define SIZE_OF_DATA_HMEM_STRUCT					\
+	(SIZE_OF_IA_CSS_PTR)
+
+#define SIZE_OF_IA_CSS_ISP_3A_STATISTICS_STRUCT				\
+	(SIZE_OF_DATA_UNION +						\
+	 SIZE_OF_DATA_HMEM_STRUCT +					\
+	 sizeof(uint32_t) +						\
+	 sizeof(uint32_t) +						\
+	 SIZE_OF_IA_CSS_PTR +						\
+	 4 * sizeof(uint32_t))
+
+/* Map with host-side pointers to ISP-format statistics.
+ * These pointers can either be copies of ISP data or memory mapped
+ * ISP pointers.
+ * All of the data behind these pointers is allocated contiguously, the
+ * allocated pointer is stored in the data_ptr field. The other fields
+ * point into this one block of data.
+ */
+struct ia_css_isp_3a_statistics_map {
+	void                    *data_ptr; /** Pointer to start of memory */
+	struct ia_css_3a_output *dmem_stats;
+	u16                *vmem_stats_hi;
+	u16                *vmem_stats_lo;
+	struct ia_css_bh_table  *hmem_stats;
+	u32                 size; /** total size in bytes of data_ptr */
+	u32                 data_allocated; /** indicate whether data_ptr
+						    was allocated or not. */
+};
+
+/* @brief Copy and translate 3A statistics from an ISP buffer to a host buffer
+ * @param[out]	host_stats Host buffer.
+ * @param[in]	isp_stats ISP buffer.
+ * @return	error value if temporary memory cannot be allocated
+ *
+ * This copies 3a statistics from an ISP pointer to a host pointer and then
+ * translates some of the statistics, details depend on which ISP binary is
+ * used.
+ * Always use this function, never copy the buffer directly.
+ */
+int
+ia_css_get_3a_statistics(struct ia_css_3a_statistics           *host_stats,
+			 const struct ia_css_isp_3a_statistics *isp_stats);
+
+/* @brief Translate 3A statistics from ISP format to host format.
+ * @param[out]	host_stats host-format statistics
+ * @param[in]	isp_stats  ISP-format statistics
+ * @return	None
+ *
+ * This function translates statistics from the internal ISP-format to
+ * the host-format. This function does not include an additional copy
+ * step.
+ * */
+void
+ia_css_translate_3a_statistics(
+    struct ia_css_3a_statistics               *host_stats,
+    const struct ia_css_isp_3a_statistics_map *isp_stats);
+
+/* Convenience functions for alloc/free of certain datatypes */
+
+/* @brief Allocate memory for the 3a statistics on the ISP
+ * @param[in]	grid The grid.
+ * @return		Pointer to the allocated 3a statistics buffer on the ISP
+*/
+struct ia_css_isp_3a_statistics *
+ia_css_isp_3a_statistics_allocate(const struct ia_css_3a_grid_info *grid);
+
+/* @brief Free the 3a statistics memory on the isp
+ * @param[in]	me Pointer to the 3a statistics buffer on the ISP.
+ * @return		None
+*/
+void
+ia_css_isp_3a_statistics_free(struct ia_css_isp_3a_statistics *me);
+
+/* @brief Allocate memory for the 3a statistics on the host
+ * @param[in]	grid The grid.
+ * @return		Pointer to the allocated 3a statistics buffer on the host
+*/
+struct ia_css_3a_statistics *
+ia_css_3a_statistics_allocate(const struct ia_css_3a_grid_info *grid);
+
+/* @brief Free the 3a statistics memory on the host
+ * @param[in]	me Pointer to the 3a statistics buffer on the host.
+ * @return		None
+ */
+void
+ia_css_3a_statistics_free(struct ia_css_3a_statistics *me);
+
+/* @brief Allocate a 3a statistics map structure
+ * @param[in]	isp_stats pointer to ISP 3a statistis struct
+ * @param[in]	data_ptr  host-side pointer to ISP 3a statistics.
+ * @return		Pointer to the allocated 3a statistics map
+ *
+ * This function allocates the ISP 3a statistics map structure
+ * and uses the data_ptr as base pointer to set the appropriate
+ * pointers to all relevant subsets of the 3a statistics (dmem,
+ * vmem, hmem).
+ * If the data_ptr is NULL, this function will allocate the host-side
+ * memory. This information is stored in the struct and used in the
+ * ia_css_isp_3a_statistics_map_free() function to determine whether
+ * the memory should be freed or not.
+ * Note that this function does not allocate or map any ISP
+ * memory.
+*/
+struct ia_css_isp_3a_statistics_map *
+ia_css_isp_3a_statistics_map_allocate(
+    const struct ia_css_isp_3a_statistics *isp_stats,
+    void *data_ptr);
+
+/* @brief Free the 3a statistics map
+ * @param[in]	me Pointer to the 3a statistics map
+ * @return		None
+ *
+ * This function frees the map struct. If the data_ptr inside it
+ * was allocated inside ia_css_isp_3a_statistics_map_allocate(), it
+ * will be freed in this function. Otherwise it will not be freed.
+ */
+void
+ia_css_isp_3a_statistics_map_free(struct ia_css_isp_3a_statistics_map *me);
+
+#endif /* __IA_CSS_3A_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_acc_types.h b/drivers/staging/media/atomisp/pci/ia_css_acc_types.h
new file mode 100644
index 000000000..a20879aed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_acc_types.h
@@ -0,0 +1,473 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_ACC_TYPES_H
+#define _IA_CSS_ACC_TYPES_H
+
+/* @file
+ * This file contains types used for acceleration
+ */
+
+#include <system_local.h>	/* HAS_IRQ_MAP_VERSION_# */
+#include <type_support.h>
+#include <platform_support.h>
+#include <debug_global.h>
+#include <linux/bits.h>
+
+#include "ia_css_types.h"
+#include "ia_css_frame_format.h"
+
+/* Should be included without the path.
+   However, that requires adding the path to numerous makefiles
+   that have nothing to do with isp parameters.
+ */
+#include "runtime/isp_param/interface/ia_css_isp_param_types.h"
+
+/* Types for the acceleration API.
+ * These should be moved to sh_css_internal.h once the old acceleration
+ * argument handling has been completed.
+ * After that, interpretation of these structures is no longer needed
+ * in the kernel and HAL.
+*/
+
+/* Type of acceleration.
+ */
+enum ia_css_acc_type {
+	IA_CSS_ACC_NONE,	/** Normal binary */
+	IA_CSS_ACC_OUTPUT,	/** Accelerator stage on output frame */
+	IA_CSS_ACC_VIEWFINDER,	/** Accelerator stage on viewfinder frame */
+	IA_CSS_ACC_STANDALONE,	/** Stand-alone acceleration */
+};
+
+/* Cells types
+ */
+enum ia_css_cell_type {
+	IA_CSS_SP0 = 0,
+	IA_CSS_SP1,
+	IA_CSS_ISP,
+	MAX_NUM_OF_CELLS
+};
+
+/* Firmware types.
+ */
+enum ia_css_fw_type {
+	ia_css_sp_firmware,		/** Firmware for the SP */
+	ia_css_isp_firmware,		/** Firmware for the ISP */
+	ia_css_bootloader_firmware,	/** Firmware for the BootLoader */
+	ia_css_acc_firmware		/** Firmware for accelrations */
+};
+
+struct ia_css_blob_descr;
+
+/* Blob descriptor.
+ * This structure describes an SP or ISP blob.
+ * It describes the test, data and bss sections as well as position in a
+ * firmware file.
+ * For convenience, it contains dynamic data after loading.
+ */
+struct ia_css_blob_info {
+	/** Static blob data */
+	u32 offset;		/** Blob offset in fw file */
+	struct ia_css_isp_param_memory_offsets
+		memory_offsets;  /** offset wrt hdr in bytes */
+	u32 prog_name_offset;  /** offset wrt hdr in bytes */
+	u32 size;			/** Size of blob */
+	u32 padding_size;	/** total cummulative of bytes added due to section alignment */
+	u32 icache_source;	/** Position of icache in blob */
+	u32 icache_size;	/** Size of icache section */
+	u32 icache_padding;/** bytes added due to icache section alignment */
+	u32 text_source;	/** Position of text in blob */
+	u32 text_size;		/** Size of text section */
+	u32 text_padding;	/** bytes added due to text section alignment */
+	u32 data_source;	/** Position of data in blob */
+	u32 data_target;	/** Start of data in SP dmem */
+	u32 data_size;		/** Size of text section */
+	u32 data_padding;	/** bytes added due to data section alignment */
+	u32 bss_target;	/** Start position of bss in SP dmem */
+	u32 bss_size;		/** Size of bss section */
+	/** Dynamic data filled by loader */
+	CSS_ALIGN(const void  *code,
+		  8);		/** Code section absolute pointer within fw, code = icache + text */
+	CSS_ALIGN(const void  *data,
+		  8);		/** Data section absolute pointer within fw, data = data + bss */
+};
+
+struct ia_css_binary_input_info {
+	u32		min_width;
+	u32		min_height;
+	u32		max_width;
+	u32		max_height;
+	u32		source; /* memory, sensor, variable */
+};
+
+struct ia_css_binary_output_info {
+	u32		min_width;
+	u32		min_height;
+	u32		max_width;
+	u32		max_height;
+	u32		num_chunks;
+	u32		variable_format;
+};
+
+struct ia_css_binary_internal_info {
+	u32		max_width;
+	u32		max_height;
+};
+
+struct ia_css_binary_bds_info {
+	u32		supported_bds_factors;
+};
+
+struct ia_css_binary_dvs_info {
+	u32		max_envelope_width;
+	u32		max_envelope_height;
+};
+
+struct ia_css_binary_vf_dec_info {
+	u32		is_variable;
+	u32		max_log_downscale;
+};
+
+struct ia_css_binary_s3a_info {
+	u32		s3atbl_use_dmem;
+	u32		fixed_s3a_deci_log;
+};
+
+/* DPC related binary info */
+struct ia_css_binary_dpc_info {
+	u32		bnr_lite; /** bnr lite enable flag */
+};
+
+struct ia_css_binary_iterator_info {
+	u32		num_stripes;
+	u32		row_stripes_height;
+	u32		row_stripes_overlap_lines;
+};
+
+struct ia_css_binary_address_info {
+	u32		isp_addresses;	/* Address in ISP dmem */
+	u32		main_entry;	/* Address of entry fct */
+	u32		in_frame;	/* Address in ISP dmem */
+	u32		out_frame;	/* Address in ISP dmem */
+	u32		in_data;	/* Address in ISP dmem */
+	u32		out_data;	/* Address in ISP dmem */
+	u32		sh_dma_cmd_ptr;     /* In ISP dmem */
+};
+
+struct ia_css_binary_uds_info {
+	u16	bpp;
+	u16	use_bci;
+	u16	use_str;
+	u16	woix;
+	u16	woiy;
+	u16	extra_out_vecs;
+	u16	vectors_per_line_in;
+	u16	vectors_per_line_out;
+	u16	vectors_c_per_line_in;
+	u16	vectors_c_per_line_out;
+	u16	vmem_gdc_in_block_height_y;
+	u16	vmem_gdc_in_block_height_c;
+	/* uint16_t padding; */
+};
+
+struct ia_css_binary_pipeline_info {
+	u32	mode;
+	u32	isp_pipe_version;
+	u32	pipelining;
+	u32	c_subsampling;
+	u32	top_cropping;
+	u32	left_cropping;
+	u32	variable_resolution;
+};
+
+struct ia_css_binary_block_info {
+	u32	block_width;
+	u32	block_height;
+	u32	output_block_height;
+};
+
+/* Structure describing an ISP binary.
+ * It describes the capabilities of a binary, like the maximum resolution,
+ * support features, dma channels, uds features, etc.
+ * This part is to be used by the SP.
+ * Future refactoring should move binary properties to ia_css_binary_xinfo,
+ * thereby making the SP code more binary independent.
+ */
+struct ia_css_binary_info {
+	CSS_ALIGN(u32			id, 8); /* IA_CSS_BINARY_ID_* */
+	struct ia_css_binary_pipeline_info	pipeline;
+	struct ia_css_binary_input_info		input;
+	struct ia_css_binary_output_info	output;
+	struct ia_css_binary_internal_info	internal;
+	struct ia_css_binary_bds_info		bds;
+	struct ia_css_binary_dvs_info		dvs;
+	struct ia_css_binary_vf_dec_info	vf_dec;
+	struct ia_css_binary_s3a_info		s3a;
+	struct ia_css_binary_dpc_info		dpc_bnr; /** DPC related binary info */
+	struct ia_css_binary_iterator_info	iterator;
+	struct ia_css_binary_address_info	addresses;
+	struct ia_css_binary_uds_info		uds;
+	struct ia_css_binary_block_info		block;
+	struct ia_css_isp_param_isp_segments	mem_initializers;
+	/* MW: Packing (related) bools in an integer ?? */
+	struct {
+		u8	reduced_pipe;
+		u8	vf_veceven;
+		u8	dis;
+		u8	dvs_envelope;
+		u8	uds;
+		u8	dvs_6axis;
+		u8	block_output;
+		u8	streaming_dma;
+		u8	ds;
+		u8	bayer_fir_6db;
+		u8	raw_binning;
+		u8	continuous;
+		u8	s3a;
+		u8	fpnr;
+		u8	sc;
+		u8	macc;
+		u8	output;
+		u8	ref_frame;
+		u8	tnr;
+		u8	xnr;
+		u8	params;
+		u8	ca_gdc;
+		u8	isp_addresses;
+		u8	in_frame;
+		u8	out_frame;
+		u8	high_speed;
+		u8	dpc;
+		u8 padding[2];
+	} enable;
+	struct {
+		/* DMA channel ID: [0,...,HIVE_ISP_NUM_DMA_CHANNELS> */
+		u8	ref_y_channel;
+		u8	ref_c_channel;
+		u8	tnr_channel;
+		u8	tnr_out_channel;
+		u8	dvs_coords_channel;
+		u8	output_channel;
+		u8	c_channel;
+		u8	vfout_channel;
+		u8	vfout_c_channel;
+		u8	vfdec_bits_per_pixel;
+		u8	claimed_by_isp;
+		u8 padding[2];
+	} dma;
+};
+
+/* Structure describing an ISP binary.
+ * It describes the capabilities of a binary, like the maximum resolution,
+ * support features, dma channels, uds features, etc.
+ */
+struct ia_css_binary_xinfo {
+	/* Part that is of interest to the SP. */
+	struct ia_css_binary_info    sp;
+
+	/* Rest of the binary info, only interesting to the host. */
+	enum ia_css_acc_type	     type;
+
+	CSS_ALIGN(s32	     num_output_formats, 8);
+	enum ia_css_frame_format     output_formats[IA_CSS_FRAME_FORMAT_NUM];
+
+	CSS_ALIGN(s32	     num_vf_formats, 8); /** number of supported vf formats */
+	enum ia_css_frame_format
+	vf_formats[IA_CSS_FRAME_FORMAT_NUM]; /** types of supported vf formats */
+	u8			     num_output_pins;
+	ia_css_ptr		     xmem_addr;
+
+	CSS_ALIGN(const struct ia_css_blob_descr *blob, 8);
+	CSS_ALIGN(u32 blob_index, 8);
+	CSS_ALIGN(union ia_css_all_memory_offsets mem_offsets, 8);
+	CSS_ALIGN(struct ia_css_binary_xinfo *next, 8);
+};
+
+/* Structure describing the Bootloader (an ISP binary).
+ * It contains several address, either in ddr, isp_dmem or
+ * the entry function in icache.
+ */
+struct ia_css_bl_info {
+	u32 num_dma_cmds;	/** Number of cmds sent by CSS */
+	u32 dma_cmd_list;	/** Dma command list sent by CSS */
+	u32 sw_state;	/** Polled from css */
+	/* Entry functions */
+	u32 bl_entry;	/** The SP entry function */
+};
+
+/* Structure describing the SP binary.
+ * It contains several address, either in ddr, sp_dmem or
+ * the entry function in pmem.
+ */
+struct ia_css_sp_info {
+	u32 init_dmem_data; /** data sect config, stored to dmem */
+	u32 per_frame_data; /** Per frame data, stored to dmem */
+	u32 group;		/** Per pipeline data, loaded by dma */
+	u32 output;		/** SP output data, loaded by dmem */
+	u32 host_sp_queue;	/** Host <-> SP queues */
+	u32 host_sp_com;/** Host <-> SP commands */
+	u32 isp_started;	/** Polled from sensor thread, csim only */
+	u32 sw_state;	/** Polled from css */
+	u32 host_sp_queues_initialized; /** Polled from the SP */
+	u32 sleep_mode;  /** different mode to halt SP */
+	u32 invalidate_tlb;		/** inform SP to invalidate mmu TLB */
+
+	/* ISP2400 */
+	u32 stop_copy_preview;       /** suspend copy and preview pipe when capture */
+
+	u32 debug_buffer_ddr_address;	/** inform SP the address
+	of DDR debug queue */
+	u32 perf_counter_input_system_error; /** input system perf
+	counter array */
+#ifdef HAS_WATCHDOG_SP_THREAD_DEBUG
+	u32 debug_wait; /** thread/pipe post mortem debug */
+	u32 debug_stage; /** thread/pipe post mortem debug */
+	u32 debug_stripe; /** thread/pipe post mortem debug */
+#endif
+	u32 threads_stack; /** sp thread's stack pointers */
+	u32 threads_stack_size; /** sp thread's stack sizes */
+	u32 curr_binary_id;        /** current binary id */
+	u32 raw_copy_line_count;   /** raw copy line counter */
+	u32 ddr_parameter_address; /** acc param ddrptr, sp dmem */
+	u32 ddr_parameter_size;    /** acc param size, sp dmem */
+	/* Entry functions */
+	u32 sp_entry;	/** The SP entry function */
+	u32 tagger_frames_addr;   /** Base address of tagger state */
+};
+
+/* The following #if is there because this header file is also included
+   by SP and ISP code but they do not need this data and HIVECC has alignment
+   issue with the firmware struct/union's.
+   More permanent solution will be to refactor this include.
+*/
+
+/* Accelerator firmware information.
+ */
+struct ia_css_acc_info {
+	u32 per_frame_data; /** Dummy for now */
+};
+
+/* Firmware information.
+ */
+union ia_css_fw_union {
+	struct ia_css_binary_xinfo	isp; /** ISP info */
+	struct ia_css_sp_info		sp;  /** SP info */
+	struct ia_css_bl_info           bl;  /** Bootloader info */
+	struct ia_css_acc_info		acc; /** Accelerator info */
+};
+
+/* Firmware information.
+ */
+struct ia_css_fw_info {
+	size_t			 header_size; /** size of fw header */
+
+	CSS_ALIGN(u32 type, 8);
+	union ia_css_fw_union	 info; /** Binary info */
+	struct ia_css_blob_info  blob; /** Blob info */
+	/* Dynamic part */
+	struct ia_css_fw_info   *next;
+
+	CSS_ALIGN(u32       loaded, 8);	/** Firmware has been loaded */
+	CSS_ALIGN(const u8 *isp_code, 8);  /** ISP pointer to code */
+	/** Firmware handle between user space and kernel */
+	CSS_ALIGN(u32	handle, 8);
+	/** Sections to copy from/to ISP */
+	struct ia_css_isp_param_css_segments mem_initializers;
+	/** Initializer for local ISP memories */
+};
+
+struct ia_css_blob_descr {
+	const unsigned char  *blob;
+	struct ia_css_fw_info header;
+	const char	     *name;
+	union ia_css_all_memory_offsets mem_offsets;
+};
+
+struct ia_css_acc_fw;
+
+/* Structure describing the SP binary of a stand-alone accelerator.
+ */
+struct ia_css_acc_sp {
+	void (*init)(struct ia_css_acc_fw *);	/** init for crun */
+	u32 sp_prog_name_offset;		/** program name offset wrt hdr in bytes */
+	u32 sp_blob_offset;		/** blob offset wrt hdr in bytes */
+	void	 *entry;			/** Address of sp entry point */
+	u32 *css_abort;			/** SP dmem abort flag */
+	void	 *isp_code;			/** SP dmem address holding xmem
+						     address of isp code */
+	struct ia_css_fw_info fw;		/** SP fw descriptor */
+	const u8 *code;			/** ISP pointer of allocated SP code */
+};
+
+/* Acceleration firmware descriptor.
+  * This descriptor descibes either SP code (stand-alone), or
+  * ISP code (a separate pipeline stage).
+  */
+struct ia_css_acc_fw_hdr {
+	enum ia_css_acc_type type;	/** Type of accelerator */
+	u32	isp_prog_name_offset; /** program name offset wrt
+						   header in bytes */
+	u32	isp_blob_offset;      /** blob offset wrt header
+						   in bytes */
+	u32	isp_size;	      /** Size of isp blob */
+	const u8  *isp_code;	      /** ISP pointer to code */
+	struct ia_css_acc_sp  sp;  /** Standalone sp code */
+	/** Firmware handle between user space and kernel */
+	u32	handle;
+	struct ia_css_data parameters; /** Current SP parameters */
+};
+
+/* Firmware structure.
+  * This contains the header and actual blobs.
+  * For standalone, it contains SP and ISP blob.
+  * For a pipeline stage accelerator, it contains ISP code only.
+  * Since its members are variable size, their offsets are described in the
+  * header and computed using the access macros below.
+  */
+struct ia_css_acc_fw {
+	struct ia_css_acc_fw_hdr header; /** firmware header */
+	/*
+	int8_t   isp_progname[];	  **< ISP program name
+	int8_t   sp_progname[];	  **< SP program name, stand-alone only
+	uint8_t sp_code[];  **< SP blob, stand-alone only
+	uint8_t isp_code[]; **< ISP blob
+	*/
+};
+
+/* Access macros for firmware */
+#define IA_CSS_ACC_OFFSET(t, f, n) ((t)((uint8_t *)(f) + (f->header.n)))
+#define IA_CSS_ACC_SP_PROG_NAME(f) IA_CSS_ACC_OFFSET(const char *, f, \
+						 sp.sp_prog_name_offset)
+#define IA_CSS_ACC_ISP_PROG_NAME(f) IA_CSS_ACC_OFFSET(const char *, f, \
+						 isp_prog_name_offset)
+#define IA_CSS_ACC_SP_CODE(f)      IA_CSS_ACC_OFFSET(uint8_t *, f, \
+						 sp.sp_blob_offset)
+#define IA_CSS_ACC_SP_DATA(f)      (IA_CSS_ACC_SP_CODE(f) + \
+					(f)->header.sp.fw.blob.data_source)
+#define IA_CSS_ACC_ISP_CODE(f)     IA_CSS_ACC_OFFSET(uint8_t*, f,\
+						 isp_blob_offset)
+#define IA_CSS_ACC_ISP_SIZE(f)     ((f)->header.isp_size)
+
+/* Binary name follows header immediately */
+#define IA_CSS_EXT_ISP_PROG_NAME(f)   ((const char *)(f) + (f)->blob.prog_name_offset)
+#define IA_CSS_EXT_ISP_MEM_OFFSETS(f) \
+	((const struct ia_css_memory_offsets *)((const char *)(f) + (f)->blob.mem_offsets))
+
+enum ia_css_sp_sleep_mode {
+	SP_DISABLE_SLEEP_MODE = 0,
+	SP_SLEEP_AFTER_FRAME  = BIT(0),
+	SP_SLEEP_AFTER_IRQ    = BIT(1),
+};
+#endif /* _IA_CSS_ACC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_buffer.h b/drivers/staging/media/atomisp/pci/ia_css_buffer.h
new file mode 100644
index 000000000..b1e8357b9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_buffer.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BUFFER_H
+#define __IA_CSS_BUFFER_H
+
+/* @file
+ * This file contains datastructures and types for buffers used in CSS
+ */
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_timer.h"
+
+/* Enumeration of buffer types. Buffers can be queued and de-queued
+ *  to hand them over between IA and ISP.
+ */
+enum ia_css_buffer_type {
+	IA_CSS_BUFFER_TYPE_INVALID = -1,
+	IA_CSS_BUFFER_TYPE_3A_STATISTICS = 0,
+	IA_CSS_BUFFER_TYPE_DIS_STATISTICS,
+	IA_CSS_BUFFER_TYPE_LACE_STATISTICS,
+	IA_CSS_BUFFER_TYPE_INPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_OUTPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME,
+	IA_CSS_BUFFER_TYPE_CUSTOM_INPUT,
+	IA_CSS_BUFFER_TYPE_CUSTOM_OUTPUT,
+	IA_CSS_BUFFER_TYPE_METADATA,
+	IA_CSS_BUFFER_TYPE_PARAMETER_SET,
+	IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET,
+	IA_CSS_NUM_DYNAMIC_BUFFER_TYPE,
+	IA_CSS_NUM_BUFFER_TYPE
+};
+
+/* Driver API is not SP/ISP visible, 64 bit types not supported on hivecc */
+
+/* Buffer structure. This is a container structure that enables content
+ *  independent buffer queues and access functions.
+ */
+struct ia_css_buffer {
+	enum ia_css_buffer_type type; /** Buffer type. */
+	unsigned int exp_id;
+	/** exposure id for this buffer; 0 = not available
+	     see ia_css_event_public.h for more detail. */
+	union {
+		struct ia_css_isp_3a_statistics
+			*stats_3a;    /** 3A statistics & optionally RGBY statistics. */
+		struct ia_css_isp_dvs_statistics *stats_dvs;   /** DVS statistics. */
+		struct ia_css_isp_skc_dvs_statistics *stats_skc_dvs;  /** SKC DVS statistics. */
+		struct ia_css_frame              *frame;       /** Frame buffer. */
+		struct ia_css_acc_param          *custom_data; /** Custom buffer. */
+		struct ia_css_metadata           *metadata;    /** Sensor metadata. */
+	} data; /** Buffer data pointer. */
+	u64 driver_cookie; /** cookie for the driver */
+	struct ia_css_time_meas
+		timing_data; /** timing data (readings from the timer) */
+	struct ia_css_clock_tick
+		isys_eof_clock_tick; /** ISYS's end of frame timer tick*/
+};
+
+/* @brief Dequeue param buffers from sp2host_queue
+ *
+ * @return                                       None
+ *
+ * This function must be called at every driver interrupt handler to prevent
+ * overflow of sp2host_queue.
+ */
+void
+ia_css_dequeue_param_buffers(void);
+
+#endif /* __IA_CSS_BUFFER_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_control.h b/drivers/staging/media/atomisp/pci/ia_css_control.h
new file mode 100644
index 000000000..88f031a63
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_control.h
@@ -0,0 +1,132 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CONTROL_H
+#define __IA_CSS_CONTROL_H
+
+/* @file
+ * This file contains functionality for starting and controlling CSS
+ */
+
+#include <type_support.h>
+#include <ia_css_env.h>
+#include <ia_css_firmware.h>
+#include <ia_css_irq.h>
+
+/* @brief Initialize the CSS API.
+ * @param[in]	env		Environment, provides functions to access the
+ *				environment in which the CSS code runs. This is
+ *				used for host side memory access and message
+ *				printing. May not be NULL.
+ * @param[in]	fw		Firmware package containing the firmware for all
+ *				predefined ISP binaries.
+ *				if fw is NULL the firmware must be loaded before
+ *				through a call of ia_css_load_firmware
+ * @param[in]	l1_base         Base index (isp2400)
+ *                              of the L1 page table. This is a physical
+ *                              address or index.
+ * @param[in]	irq_type	The type of interrupt to be used (edge or level)
+ * @return				Returns -EINVAL in case of any
+ *				errors and 0 otherwise.
+ *
+ * This function initializes the API which includes allocating and initializing
+ * internal data structures. This also interprets the firmware package. All
+ * contents of this firmware package are copied into local data structures, so
+ * the fw pointer could be freed after this function completes.
+ */
+int ia_css_init(struct device           *dev,
+			    const struct ia_css_env *env,
+			    const struct ia_css_fw  *fw,
+			    u32                     l1_base,
+			    enum ia_css_irq_type    irq_type);
+
+/* @brief Un-initialize the CSS API.
+ * @return	None
+ *
+ * This function deallocates all memory that has been allocated by the CSS API
+ * Exception: if you explicitly loaded firmware through ia_css_load_firmware
+ * you need to call ia_css_unload_firmware to deallocate the memory reserved
+ * for the firmware.
+ * After this function is called, no other CSS functions should be called
+ * with the exception of ia_css_init which will re-initialize the CSS code,
+ * ia_css_unload_firmware to unload the firmware or ia_css_load_firmware
+ * to load new firmware
+ */
+void
+ia_css_uninit(void);
+
+/* @brief Enable use of a separate queue for ISYS events.
+ *
+ * @param[in]	enable: enable or disable use of separate ISYS event queues.
+ * @return		error if called when SP is running.
+ *
+ * @deprecated{This is a temporary function that allows drivers to migrate to
+ * the use of the separate ISYS event queue. Once all drivers supports this, it
+ * will be made the default and this function will be removed.
+ * This function should only be called when the SP is not running, calling it
+ * when the SP is running will result in an error value being returned. }
+ */
+int
+ia_css_enable_isys_event_queue(bool enable);
+
+/* @brief Test whether the ISP has started.
+ *
+ * @return	Boolean flag true if the ISP has started or false otherwise.
+ *
+ * Temporary function to poll whether the ISP has been started. Once it has,
+ * the sensor can also be started. */
+bool
+ia_css_isp_has_started(void);
+
+/* @brief Test whether the SP has initialized.
+ *
+ * @return	Boolean flag true if the SP has initialized or false otherwise.
+ *
+ * Temporary function to poll whether the SP has been initialized. Once it has,
+ * we can enqueue buffers. */
+bool
+ia_css_sp_has_initialized(void);
+
+/* @brief Test whether the SP has terminated.
+ *
+ * @return	Boolean flag true if the SP has terminated or false otherwise.
+ *
+ * Temporary function to poll whether the SP has been terminated. Once it has,
+ * we can switch mode. */
+bool
+ia_css_sp_has_terminated(void);
+
+/* @brief start SP hardware
+ *
+ * @return			0 or error code upon error.
+ *
+ * It will boot the SP hardware and start multi-threading infrastructure.
+ * All threads will be started and blocked by semaphore. This function should
+ * be called before any ia_css_stream_start().
+ */
+int
+ia_css_start_sp(void);
+
+/* @brief stop SP hardware
+ *
+ * @return			0 or error code upon error.
+ *
+ * This function will terminate all threads and shut down SP. It should be
+ * called after all ia_css_stream_stop().
+ */
+int
+ia_css_stop_sp(void);
+
+#endif /* __IA_CSS_CONTROL_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_device_access.c b/drivers/staging/media/atomisp/pci/ia_css_device_access.c
new file mode 100644
index 000000000..9cd2d3caa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_device_access.c
@@ -0,0 +1,96 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_device_access.h"
+#include <type_support.h>   /* for uint*, size_t */
+#include <system_local.h>   /* for hrt_address */
+#include <ia_css_env.h>     /* for ia_css_hw_access_env */
+#include <assert_support.h> /* for assert */
+
+static struct ia_css_hw_access_env my_env;
+
+void
+ia_css_device_access_init(const struct ia_css_hw_access_env *env)
+{
+	assert(env);
+
+	my_env = *env;
+}
+
+uint8_t
+ia_css_device_load_uint8(const hrt_address addr)
+{
+	return my_env.load_8(addr);
+}
+
+uint16_t
+ia_css_device_load_uint16(const hrt_address addr)
+{
+	return my_env.load_16(addr);
+}
+
+uint32_t
+ia_css_device_load_uint32(const hrt_address addr)
+{
+	return my_env.load_32(addr);
+}
+
+uint64_t
+ia_css_device_load_uint64(const hrt_address addr)
+{
+	assert(0);
+
+	(void)addr;
+	return 0;
+}
+
+void
+ia_css_device_store_uint8(const hrt_address addr, const uint8_t data)
+{
+	my_env.store_8(addr, data);
+}
+
+void
+ia_css_device_store_uint16(const hrt_address addr, const uint16_t data)
+{
+	my_env.store_16(addr, data);
+}
+
+void
+ia_css_device_store_uint32(const hrt_address addr, const uint32_t data)
+{
+	my_env.store_32(addr, data);
+}
+
+void
+ia_css_device_store_uint64(const hrt_address addr, const uint64_t data)
+{
+	assert(0);
+
+	(void)addr;
+	(void)data;
+}
+
+void
+ia_css_device_load(const hrt_address addr, void *data, const size_t size)
+{
+	my_env.load(addr, data, (uint32_t)size);
+}
+
+void
+ia_css_device_store(const hrt_address addr, const void *data, const size_t size)
+{
+	my_env.store(addr, data, (uint32_t)size);
+}
diff --git a/drivers/staging/media/atomisp/pci/ia_css_device_access.h b/drivers/staging/media/atomisp/pci/ia_css_device_access.h
new file mode 100644
index 000000000..07d611fdd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_device_access.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_DEVICE_ACCESS_H
+#define _IA_CSS_DEVICE_ACCESS_H
+
+/* @file
+ * File containing internal functions for the CSS-API to access the CSS device.
+ */
+
+#include <type_support.h> /* for uint*, size_t */
+#include <system_local.h> /* for hrt_address */
+#include <ia_css_env.h>   /* for ia_css_hw_access_env */
+
+void
+ia_css_device_access_init(const struct ia_css_hw_access_env *env);
+
+uint8_t
+ia_css_device_load_uint8(const hrt_address addr);
+
+uint16_t
+ia_css_device_load_uint16(const hrt_address addr);
+
+uint32_t
+ia_css_device_load_uint32(const hrt_address addr);
+
+uint64_t
+ia_css_device_load_uint64(const hrt_address addr);
+
+void
+ia_css_device_store_uint8(const hrt_address addr, const uint8_t data);
+
+void
+ia_css_device_store_uint16(const hrt_address addr, const uint16_t data);
+
+void
+ia_css_device_store_uint32(const hrt_address addr, const uint32_t data);
+
+void
+ia_css_device_store_uint64(const hrt_address addr, const uint64_t data);
+
+void
+ia_css_device_load(const hrt_address addr, void *data, const size_t size);
+
+void
+ia_css_device_store(const hrt_address addr, const void *data,
+		    const size_t size);
+
+#endif /* _IA_CSS_DEVICE_ACCESS_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_dvs.h b/drivers/staging/media/atomisp/pci/ia_css_dvs.h
new file mode 100644
index 000000000..3367dfd64
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_dvs.h
@@ -0,0 +1,298 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DVS_H
+#define __IA_CSS_DVS_H
+
+/* @file
+ * This file contains types for DVS statistics
+ */
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_err.h"
+#include "ia_css_stream_public.h"
+
+enum dvs_statistics_type {
+	DVS_STATISTICS,
+	DVS2_STATISTICS,
+	SKC_DVS_STATISTICS
+};
+
+/* Structure that holds DVS statistics in the ISP internal
+ * format. Use ia_css_get_dvs_statistics() to translate
+ * this to the format used on the host (DVS engine).
+ * */
+struct ia_css_isp_dvs_statistics {
+	ia_css_ptr hor_proj;
+	ia_css_ptr ver_proj;
+	u32   hor_size;
+	u32   ver_size;
+	u32   exp_id;   /** see ia_css_event_public.h for more detail */
+	ia_css_ptr data_ptr; /* base pointer containing all memory */
+	u32   size;     /* size of allocated memory in data_ptr */
+};
+
+/* Structure that holds SKC DVS statistics in the ISP internal
+ * format. Use ia_css_dvs_statistics_get() to translate this to
+ * the format used on the host.
+ * */
+struct ia_css_isp_skc_dvs_statistics;
+
+#define SIZE_OF_IA_CSS_ISP_DVS_STATISTICS_STRUCT			\
+	((3 * SIZE_OF_IA_CSS_PTR) +					\
+	 (4 * sizeof(uint32_t)))
+
+/* Map with host-side pointers to ISP-format statistics.
+ * These pointers can either be copies of ISP data or memory mapped
+ * ISP pointers.
+ * All of the data behind these pointers is allocatd contiguously, the
+ * allocated pointer is stored in the data_ptr field. The other fields
+ * point into this one block of data.
+ */
+struct ia_css_isp_dvs_statistics_map {
+	void    *data_ptr;
+	s32 *hor_proj;
+	s32 *ver_proj;
+	u32 size;		 /* total size in bytes */
+	u32 data_allocated; /* indicate whether data was allocated */
+};
+
+union ia_css_dvs_statistics_isp {
+	struct ia_css_isp_dvs_statistics *p_dvs_statistics_isp;
+	struct ia_css_isp_skc_dvs_statistics *p_skc_dvs_statistics_isp;
+};
+
+union ia_css_dvs_statistics_host {
+	struct ia_css_dvs_statistics *p_dvs_statistics_host;
+	struct ia_css_dvs2_statistics *p_dvs2_statistics_host;
+	struct ia_css_skc_dvs_statistics *p_skc_dvs_statistics_host;
+};
+
+/* @brief Copy DVS statistics from an ISP buffer to a host buffer.
+ * @param[in]	host_stats Host buffer
+ * @param[in]	isp_stats ISP buffer
+ * @return	error value if temporary memory cannot be allocated
+ *
+ * This may include a translation step as well depending
+ * on the ISP version.
+ * Always use this function, never copy the buffer directly.
+ * Note that this function uses the mem_load function from the CSS
+ * environment struct.
+ * In certain environments this may be slow. In those cases it is
+ * advised to map the ISP memory into a host-side pointer and use
+ * the ia_css_translate_dvs_statistics() function instead.
+ */
+int
+ia_css_get_dvs_statistics(struct ia_css_dvs_statistics *host_stats,
+			  const struct ia_css_isp_dvs_statistics *isp_stats);
+
+/* @brief Translate DVS statistics from ISP format to host format
+ * @param[in]	host_stats Host buffer
+ * @param[in]	isp_stats ISP buffer
+ * @return	None
+ *
+ * This function translates the dvs statistics from the ISP-internal
+ * format to the format used by the DVS library on the CPU.
+ * This function takes a host-side pointer as input. This can either
+ * point to a copy of the data or be a memory mapped pointer to the
+ * ISP memory pages.
+ */
+void
+ia_css_translate_dvs_statistics(
+    struct ia_css_dvs_statistics *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats);
+
+/* @brief Copy DVS 2.0 statistics from an ISP buffer to a host buffer.
+ * @param[in]	host_stats Host buffer
+ * @param[in]	isp_stats ISP buffer
+ * @return	error value if temporary memory cannot be allocated
+ *
+ * This may include a translation step as well depending
+ * on the ISP version.
+ * Always use this function, never copy the buffer directly.
+ * Note that this function uses the mem_load function from the CSS
+ * environment struct.
+ * In certain environments this may be slow. In those cases it is
+ * advised to map the ISP memory into a host-side pointer and use
+ * the ia_css_translate_dvs2_statistics() function instead.
+ */
+int
+ia_css_get_dvs2_statistics(struct ia_css_dvs2_statistics *host_stats,
+			   const struct ia_css_isp_dvs_statistics *isp_stats);
+
+/* @brief Translate DVS2 statistics from ISP format to host format
+ * @param[in]	host_stats Host buffer
+ * @param[in]	isp_stats ISP buffer
+ * @return		None
+ *
+ * This function translates the dvs2 statistics from the ISP-internal
+ * format to the format used by the DVS2 library on the CPU.
+ * This function takes a host-side pointer as input. This can either
+ * point to a copy of the data or be a memory mapped pointer to the
+ * ISP memory pages.
+ */
+void
+ia_css_translate_dvs2_statistics(
+    struct ia_css_dvs2_statistics	   *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats);
+
+/* @brief Copy DVS statistics from an ISP buffer to a host buffer.
+ * @param[in] type - DVS statistics type
+ * @param[in] host_stats Host buffer
+ * @param[in] isp_stats ISP buffer
+ * @return None
+ */
+void
+ia_css_dvs_statistics_get(enum dvs_statistics_type type,
+			  union ia_css_dvs_statistics_host  *host_stats,
+			  const union ia_css_dvs_statistics_isp *isp_stats);
+
+/* @brief Allocate the DVS statistics memory on the ISP
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS statistics buffer on the ISP
+*/
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs_statistics_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS statistics memory on the ISP
+ * @param[in]	me Pointer to the DVS statistics buffer on the ISP.
+ * @return	None
+*/
+void
+ia_css_isp_dvs_statistics_free(struct ia_css_isp_dvs_statistics *me);
+
+/* @brief Allocate the DVS 2.0 statistics memory
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS statistics buffer on the ISP
+*/
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs2_statistics_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS 2.0 statistics memory
+ * @param[in]	me Pointer to the DVS statistics buffer on the ISP.
+ * @return	None
+*/
+void
+ia_css_isp_dvs2_statistics_free(struct ia_css_isp_dvs_statistics *me);
+
+/* @brief Allocate the DVS statistics memory on the host
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS statistics buffer on the host
+*/
+struct ia_css_dvs_statistics *
+ia_css_dvs_statistics_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS statistics memory on the host
+ * @param[in]	me Pointer to the DVS statistics buffer on the host.
+ * @return	None
+*/
+void
+ia_css_dvs_statistics_free(struct ia_css_dvs_statistics *me);
+
+/* @brief Allocate the DVS coefficients memory
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS coefficients buffer
+*/
+struct ia_css_dvs_coefficients *
+ia_css_dvs_coefficients_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS coefficients memory
+ * @param[in]	me Pointer to the DVS coefficients buffer.
+ * @return	None
+ */
+void
+ia_css_dvs_coefficients_free(struct ia_css_dvs_coefficients *me);
+
+/* @brief Allocate the DVS 2.0 statistics memory on the host
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS 2.0 statistics buffer on the host
+ */
+struct ia_css_dvs2_statistics *
+ia_css_dvs2_statistics_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS 2.0 statistics memory
+ * @param[in]	me Pointer to the DVS 2.0 statistics buffer on the host.
+ * @return	None
+*/
+void
+ia_css_dvs2_statistics_free(struct ia_css_dvs2_statistics *me);
+
+/* @brief Allocate the DVS 2.0 coefficients memory
+ * @param[in]	grid The grid.
+ * @return	Pointer to the allocated DVS 2.0 coefficients buffer
+*/
+struct ia_css_dvs2_coefficients *
+ia_css_dvs2_coefficients_allocate(const struct ia_css_dvs_grid_info *grid);
+
+/* @brief Free the DVS 2.0 coefficients memory
+ * @param[in]	me Pointer to the DVS 2.0 coefficients buffer.
+ * @return	None
+*/
+void
+ia_css_dvs2_coefficients_free(struct ia_css_dvs2_coefficients *me);
+
+/* @brief Allocate the DVS 2.0 6-axis config memory
+ * @param[in]	stream The stream.
+ * @return	Pointer to the allocated DVS 6axis configuration buffer
+*/
+struct ia_css_dvs_6axis_config *
+ia_css_dvs2_6axis_config_allocate(const struct ia_css_stream *stream);
+
+/* @brief Free the DVS 2.0 6-axis config memory
+ * @param[in]	dvs_6axis_config Pointer to the DVS 6axis configuration buffer
+ * @return	None
+ */
+void
+ia_css_dvs2_6axis_config_free(struct ia_css_dvs_6axis_config *dvs_6axis_config);
+
+/* @brief Allocate a dvs statistics map structure
+ * @param[in]	isp_stats pointer to ISP dvs statistis struct
+ * @param[in]	data_ptr  host-side pointer to ISP dvs statistics.
+ * @return	Pointer to the allocated dvs statistics map
+ *
+ * This function allocates the ISP dvs statistics map structure
+ * and uses the data_ptr as base pointer to set the appropriate
+ * pointers to all relevant subsets of the dvs statistics (dmem,
+ * vmem, hmem).
+ * If the data_ptr is NULL, this function will allocate the host-side
+ * memory. This information is stored in the struct and used in the
+ * ia_css_isp_dvs_statistics_map_free() function to determine whether
+ * the memory should be freed or not.
+ * Note that this function does not allocate or map any ISP
+ * memory.
+*/
+struct ia_css_isp_dvs_statistics_map *
+ia_css_isp_dvs_statistics_map_allocate(
+    const struct ia_css_isp_dvs_statistics *isp_stats,
+    void *data_ptr);
+
+/* @brief Free the dvs statistics map
+ * @param[in]	me Pointer to the dvs statistics map
+ * @return	None
+ *
+ * This function frees the map struct. If the data_ptr inside it
+ * was allocated inside ia_css_isp_dvs_statistics_map_allocate(), it
+ * will be freed in this function. Otherwise it will not be freed.
+ */
+void
+ia_css_isp_dvs_statistics_map_free(struct ia_css_isp_dvs_statistics_map *me);
+
+/* @brief Allocate memory for the SKC DVS statistics on the ISP
+ * @return		Pointer to the allocated ACC DVS statistics buffer on the ISP
+*/
+struct ia_css_isp_skc_dvs_statistics *ia_css_skc_dvs_statistics_allocate(void);
+
+#endif /*  __IA_CSS_DVS_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_env.h b/drivers/staging/media/atomisp/pci/ia_css_env.h
new file mode 100644
index 000000000..42bb1ec1c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_env.h
@@ -0,0 +1,96 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ENV_H
+#define __IA_CSS_ENV_H
+
+#include <type_support.h>
+#include <linux/stdarg.h> /* va_list */
+#include <linux/bits.h>
+#include "ia_css_types.h"
+#include "ia_css_acc_types.h"
+
+/* @file
+ * This file contains prototypes for functions that need to be provided to the
+ * CSS-API host-code by the environment in which the CSS-API code runs.
+ */
+
+/* Memory allocation attributes, for use in ia_css_css_mem_env. */
+enum ia_css_mem_attr {
+	IA_CSS_MEM_ATTR_CACHED     = BIT(0),
+	IA_CSS_MEM_ATTR_ZEROED     = BIT(1),
+	IA_CSS_MEM_ATTR_PAGEALIGN  = BIT(2),
+	IA_CSS_MEM_ATTR_CONTIGUOUS = BIT(3),
+};
+
+/* Environment with function pointers for local IA memory allocation.
+ *  This provides the CSS code with environment specific functionality
+ *  for memory allocation of small local buffers such as local data structures.
+ *  This is never expected to allocate more than one page of memory (4K bytes).
+ */
+struct ia_css_cpu_mem_env {
+	void (*flush)(struct ia_css_acc_fw *fw);
+	/** Flush function to flush the cache for given accelerator. */
+};
+
+/* Environment with function pointers to access the CSS hardware. This includes
+ *  registers and local memories.
+ */
+struct ia_css_hw_access_env {
+	void (*store_8)(hrt_address addr, uint8_t data);
+	/** Store an 8 bit value into an address in the CSS HW address space.
+	     The address must be an 8 bit aligned address. */
+	void (*store_16)(hrt_address addr, uint16_t data);
+	/** Store a 16 bit value into an address in the CSS HW address space.
+	     The address must be a 16 bit aligned address. */
+	void (*store_32)(hrt_address addr, uint32_t data);
+	/** Store a 32 bit value into an address in the CSS HW address space.
+	     The address must be a 32 bit aligned address. */
+	uint8_t (*load_8)(hrt_address addr);
+	/** Load an 8 bit value from an address in the CSS HW address
+	     space. The address must be an 8 bit aligned address. */
+	uint16_t (*load_16)(hrt_address addr);
+	/** Load a 16 bit value from an address in the CSS HW address
+	     space. The address must be a 16 bit aligned address. */
+	uint32_t (*load_32)(hrt_address addr);
+	/** Load a 32 bit value from an address in the CSS HW address
+	     space. The address must be a 32 bit aligned address. */
+	void (*store)(hrt_address addr, const void *data, uint32_t bytes);
+	/** Store a number of bytes into a byte-aligned address in the CSS HW address space. */
+	void (*load)(hrt_address addr, void *data, uint32_t bytes);
+	/** Load a number of bytes from a byte-aligned address in the CSS HW address space. */
+};
+
+/* Environment with function pointers to print error and debug messages.
+ */
+struct ia_css_print_env {
+	int  __printf(1, 0) (*debug_print)(const char *fmt, va_list args);
+	/** Print a debug message. */
+	int  __printf(1, 0) (*error_print)(const char *fmt, va_list args);
+	/** Print an error message.*/
+};
+
+/* Environment structure. This includes function pointers to access several
+ *  features provided by the environment in which the CSS API is used.
+ *  This is used to run the camera IP in multiple platforms such as Linux,
+ *  Windows and several simulation environments.
+ */
+struct ia_css_env {
+	struct ia_css_cpu_mem_env   cpu_mem_env;   /** local flush. */
+	struct ia_css_hw_access_env hw_access_env; /** CSS HW access functions */
+	struct ia_css_print_env     print_env;     /** Message printing env. */
+};
+
+#endif /* __IA_CSS_ENV_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_err.h b/drivers/staging/media/atomisp/pci/ia_css_err.h
new file mode 100644
index 000000000..98401a4a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_err.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ERR_H
+#define __IA_CSS_ERR_H
+
+/* @file
+ * This file contains possible return values for most
+ * functions in the CSS-API.
+ */
+
+/* FW warnings. This enum contains a value for each warning that
+ * the SP FW could indicate potential performance issue
+ */
+enum ia_css_fw_warning {
+	IA_CSS_FW_WARNING_NONE,
+	IA_CSS_FW_WARNING_ISYS_QUEUE_FULL, /* < CSS system delayed because of insufficient space in the ISys queue.
+		This warning can be avoided by de-queuing ISYS buffers more timely. */
+	IA_CSS_FW_WARNING_PSYS_QUEUE_FULL, /* < CSS system delayed because of insufficient space in the PSys queue.
+		This warning can be avoided by de-queuing PSYS buffers more timely. */
+	IA_CSS_FW_WARNING_CIRCBUF_ALL_LOCKED, /* < CSS system delayed because of insufficient available buffers.
+		This warning can be avoided by unlocking locked frame-buffers more timely. */
+	IA_CSS_FW_WARNING_EXP_ID_LOCKED, /* < Exposure ID skipped because the frame associated to it was still locked.
+		This warning can be avoided by unlocking locked frame-buffers more timely. */
+	IA_CSS_FW_WARNING_TAG_EXP_ID_FAILED, /* < Exposure ID cannot be found on the circular buffer.
+		This warning can be avoided by unlocking locked frame-buffers more timely. */
+	IA_CSS_FW_WARNING_FRAME_PARAM_MISMATCH, /* < Frame and param pair mismatched in tagger.
+		This warning can be avoided by providing a param set for each frame. */
+};
+
+#endif /* __IA_CSS_ERR_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_event_public.h b/drivers/staging/media/atomisp/pci/ia_css_event_public.h
new file mode 100644
index 000000000..b052648d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_event_public.h
@@ -0,0 +1,183 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_EVENT_PUBLIC_H
+#define __IA_CSS_EVENT_PUBLIC_H
+
+/* @file
+ * This file contains CSS-API events functionality
+ */
+
+#include <type_support.h>	/* uint8_t */
+#include <ia_css_err.h>		/* ia_css_err */
+#include <ia_css_types.h>	/* ia_css_pipe */
+#include <ia_css_timer.h>	/* ia_css_timer */
+#include <linux/bits.h>
+
+/* The event type, distinguishes the kind of events that
+ * can are generated by the CSS system.
+ *
+ * !!!IMPORTANT!!! KEEP THE FOLLOWING IN SYNC:
+ * 1) "enum ia_css_event_type"					(ia_css_event_public.h)
+ * 2) "enum sh_css_sp_event_type"				(sh_css_internal.h)
+ * 3) "enum ia_css_event_type event_id_2_event_mask"		(event_handler.sp.c)
+ * 4) "enum ia_css_event_type convert_event_sp_to_host_domain"	(sh_css.c)
+ */
+enum ia_css_event_type {
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE		= BIT(0),
+	/** Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_OUTPUT_FRAME_DONE	= BIT(1),
+	/** Second output frame ready. */
+	IA_CSS_EVENT_TYPE_VF_OUTPUT_FRAME_DONE		= BIT(2),
+	/** Viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_VF_OUTPUT_FRAME_DONE	= BIT(3),
+	/** Second viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE		= BIT(4),
+	/** Indication that 3A statistics are available. */
+	IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE		= BIT(5),
+	/** Indication that DIS statistics are available. */
+	IA_CSS_EVENT_TYPE_PIPELINE_DONE			= BIT(6),
+	/** Pipeline Done event, sent after last pipeline stage. */
+	IA_CSS_EVENT_TYPE_FRAME_TAGGED			= BIT(7),
+	/** Frame tagged. */
+	IA_CSS_EVENT_TYPE_INPUT_FRAME_DONE		= BIT(8),
+	/** Input frame ready. */
+	IA_CSS_EVENT_TYPE_METADATA_DONE			= BIT(9),
+	/** Metadata ready. */
+	IA_CSS_EVENT_TYPE_LACE_STATISTICS_DONE		= BIT(10),
+	/** Indication that LACE statistics are available. */
+	IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE		= BIT(11),
+	/** Extension stage complete. */
+	IA_CSS_EVENT_TYPE_TIMER				= BIT(12),
+	/** Timer event for measuring the SP side latencies. It contains the
+	     32-bit timer value from the SP */
+	IA_CSS_EVENT_TYPE_PORT_EOF			= BIT(13),
+	/** End Of Frame event, sent when in buffered sensor mode. */
+	IA_CSS_EVENT_TYPE_FW_WARNING			= BIT(14),
+	/** Performance warning encounter by FW */
+	IA_CSS_EVENT_TYPE_FW_ASSERT			= BIT(15),
+	/** Assertion hit by FW */
+};
+
+#define IA_CSS_EVENT_TYPE_NONE 0
+
+/* IA_CSS_EVENT_TYPE_ALL is a mask for all pipe related events.
+ * The other events (such as PORT_EOF) cannot be enabled/disabled
+ * and are hence excluded from this macro.
+ */
+#define IA_CSS_EVENT_TYPE_ALL \
+	(IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE		| \
+	 IA_CSS_EVENT_TYPE_SECOND_OUTPUT_FRAME_DONE	| \
+	 IA_CSS_EVENT_TYPE_VF_OUTPUT_FRAME_DONE		| \
+	 IA_CSS_EVENT_TYPE_SECOND_VF_OUTPUT_FRAME_DONE	| \
+	 IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE		| \
+	 IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE		| \
+	 IA_CSS_EVENT_TYPE_PIPELINE_DONE		| \
+	 IA_CSS_EVENT_TYPE_FRAME_TAGGED			| \
+	 IA_CSS_EVENT_TYPE_INPUT_FRAME_DONE		| \
+	 IA_CSS_EVENT_TYPE_METADATA_DONE		| \
+	 IA_CSS_EVENT_TYPE_LACE_STATISTICS_DONE		| \
+	 IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE)
+
+/* The event struct, container for the event type and its related values.
+ * Depending on the event type, either pipe or port will be filled.
+ * Pipeline related events (like buffer/frame events) will return a valid and filled pipe handle.
+ * For non pipeline related events (but i.e. stream specific, like EOF event), the port will be
+ * filled.
+ */
+struct ia_css_event {
+	struct ia_css_pipe    *pipe;
+	/** Pipe handle on which event happened, NULL for non pipe related
+	     events. */
+	enum ia_css_event_type type;
+	/** Type of Event, always valid/filled. */
+	u8                port;
+	/** Port number for EOF event (not valid for other events). */
+	u8                exp_id;
+	/** Exposure id for EOF/FRAME_TAGGED/FW_WARNING event (not valid for other events)
+	     The exposure ID is unique only within a logical stream and it is
+	     only generated on systems that have an input system (such as 2400
+	     and 2401).
+	     Most outputs produced by the CSS are tagged with an exposure ID.
+	     This allows users of the CSS API to keep track of which buffer
+	     was generated from which sensor output frame. This includes:
+	     EOF event, output frames, 3A statistics, DVS statistics and
+	     sensor metadata.
+	     Exposure IDs start at IA_CSS_MIN_EXPOSURE_ID, increment by one
+	     until IA_CSS_MAX_EXPOSURE_ID is reached, after that they wrap
+	     around to IA_CSS_MIN_EXPOSURE_ID again.
+	     Note that in case frames are dropped, this will not be reflected
+	     in the exposure IDs. Therefor applications should not use this
+	     to detect frame drops. */
+	u32               fw_handle;
+	/** Firmware Handle for ACC_STAGE_COMPLETE event (not valid for other
+	     events). */
+	enum ia_css_fw_warning fw_warning;
+	/** Firmware warning code, only for WARNING events. */
+	u8                fw_assert_module_id;
+	/** Firmware module id, only for ASSERT events, should be logged by driver. */
+	u16               fw_assert_line_no;
+	/** Firmware line number, only for ASSERT events, should be logged by driver. */
+	clock_value_t	       timer_data;
+	/** For storing the full 32-bit of the timer value. Valid only for TIMER
+	     event */
+	u8                timer_code;
+	/** For storing the code of the TIMER event. Valid only for
+	     TIMER event */
+	u8                timer_subcode;
+	/** For storing the subcode of the TIMER event. Valid only
+	     for TIMER event */
+};
+
+/* @brief Dequeue a PSYS event from the CSS system.
+ *
+ * @param[out]	event   Pointer to the event struct which will be filled by
+ *                      this function if an event is available.
+ * @return		-ENODATA if no events are
+ *			available or
+ *			0 otherwise.
+ *
+ * This function dequeues an event from the PSYS event queue. The queue is
+ * between the Host CPU and the CSS system. This function can be
+ * called after an interrupt has been generated that signalled that a new event
+ * was available and can be used in a polling-like situation where the NO_EVENT
+ * return value is used to determine whether an event was available or not.
+ */
+int
+ia_css_dequeue_psys_event(struct ia_css_event *event);
+
+/* @brief Dequeue an ISYS event from the CSS system.
+ *
+ * @param[out]	event   Pointer to the event struct which will be filled by
+ *                      this function if an event is available.
+ * @return		-ENODATA if no events are
+ *			available or
+ *			0 otherwise.
+ *
+ * This function dequeues an event from the ISYS event queue. The queue is
+ * between host and the CSS system.
+ * Unlike the ia_css_dequeue_psys_event() function, this function can be called
+ * directly from an interrupt service routine (ISR) and it is safe to call
+ * this function in parallel with other CSS API functions (but only one
+ * call to this function should be in flight at any point in time).
+ *
+ * The reason for having the ISYS events separate is to prevent them from
+ * incurring additional latency due to locks being held by other CSS API
+ * functions.
+ */
+int
+ia_css_dequeue_isys_event(struct ia_css_event *event);
+
+#endif /* __IA_CSS_EVENT_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_firmware.h b/drivers/staging/media/atomisp/pci/ia_css_firmware.h
new file mode 100644
index 000000000..01d2faf55
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_firmware.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FIRMWARE_H
+#define __IA_CSS_FIRMWARE_H
+
+/* @file
+ * This file contains firmware loading/unloading support functionality
+ */
+
+#include <linux/device.h>
+#include "ia_css_err.h"
+#include "ia_css_env.h"
+
+/* CSS firmware package structure.
+ */
+struct ia_css_fw {
+	void	    *data;  /** pointer to the firmware data */
+	unsigned int bytes; /** length in bytes of firmware data */
+};
+
+struct device;
+
+/* @brief Loads the firmware
+ * @param[in]	env		Environment, provides functions to access the
+ *				environment in which the CSS code runs. This is
+ *				used for host side memory access and message
+ *				printing.
+ * @param[in]	fw		Firmware package containing the firmware for all
+ *				predefined ISP binaries.
+ * @return			Returns -EINVAL in case of any
+ *				errors and 0 otherwise.
+ *
+ * This function interprets the firmware package. All
+ * contents of this firmware package are copied into local data structures, so
+ * the fw pointer could be freed after this function completes.
+ *
+ * Rationale for this function is that it can be called before ia_css_init, and thus
+ * speeds up ia_css_init (ia_css_init is called each time a stream is created but the
+ * firmware only needs to be loaded once).
+ */
+int
+ia_css_load_firmware(struct device *dev, const struct ia_css_env *env,
+		     const struct ia_css_fw  *fw);
+
+/* @brief Unloads the firmware
+ * @return	None
+ *
+ * This function unloads the firmware loaded by ia_css_load_firmware.
+ * It is pointless to call this function if no firmware is loaded,
+ * but it won't harm. Use this to deallocate all memory associated with the firmware.
+ */
+void
+ia_css_unload_firmware(void);
+
+#endif /* __IA_CSS_FIRMWARE_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_frac.h b/drivers/staging/media/atomisp/pci/ia_css_frac.h
new file mode 100644
index 000000000..661af9225
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_frac.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_FRAC_H
+#define _IA_CSS_FRAC_H
+
+/* @file
+ * This file contains typedefs used for fractional numbers
+ */
+
+#include <type_support.h>
+
+/* Fixed point types.
+ * NOTE: the 16 bit fixed point types actually occupy 32 bits
+ * to save on extension operations in the ISP code.
+ */
+/* Unsigned fixed point value, 0 integer bits, 16 fractional bits */
+typedef u32 ia_css_u0_16;
+/* Unsigned fixed point value, 5 integer bits, 11 fractional bits */
+typedef u32 ia_css_u5_11;
+/* Unsigned fixed point value, 8 integer bits, 8 fractional bits */
+typedef u32 ia_css_u8_8;
+/* Signed fixed point value, 0 integer bits, 15 fractional bits */
+typedef s32 ia_css_s0_15;
+
+#endif /* _IA_CSS_FRAC_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_frame_format.h b/drivers/staging/media/atomisp/pci/ia_css_frame_format.h
new file mode 100644
index 000000000..093e23a9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_frame_format.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FRAME_FORMAT_H
+#define __IA_CSS_FRAME_FORMAT_H
+
+/* @file
+ * This file contains information about formats supported in the ISP
+ */
+
+/* Frame formats, some of these come from fourcc.org, others are
+   better explained by video4linux2. The NV11 seems to be described only
+   on MSDN pages, but even those seem to be gone now.
+   Frames can come in many forms, the main categories are RAW, RGB and YUV
+   (or YCbCr). The YUV frames come in 4 flavors, determined by how the U and V
+   values are subsampled:
+   1. YUV420: hor = 2, ver = 2
+   2. YUV411: hor = 4, ver = 1
+   3. YUV422: hor = 2, ver = 1
+   4. YUV444: hor = 1, ver = 1
+
+  Warning: not all frame formats are supported as input or output to/from ISP.
+    Some of these formats are therefore not defined in the output table module.
+    Modifications in below frame format enum can require modifications in the
+    output table module.
+
+  Warning2: Throughout the CSS code assumptions are made on the order
+	of formats in this enumeration type, or some sort of copy is maintained.
+	The following files are identified:
+	- FileSupport.h
+	- css/isp/kernels/fc/fc_1.0/formats.isp.c
+	- css/isp/kernels/output/output_1.0/output_table.isp.c
+	- css/isp/kernels/output/sc_output_1.0/formats.hive.c
+	- css/isp/modes/interface/isp_formats.isp.h
+	- css/bxt_sandbox/psyspoc/interface/ia_css_pg_info.h
+	- css/bxt_sandbox/psysapi/data/interface/ia_css_program_group_data.h
+	- css/bxt_sandbox/isysapi/interface/ia_css_isysapi_fw_types.h
+*/
+enum ia_css_frame_format {
+	IA_CSS_FRAME_FORMAT_NV11 = 0,   /** 12 bit YUV 411, Y, UV plane */
+	IA_CSS_FRAME_FORMAT_NV12,       /** 12 bit YUV 420, Y, UV plane */
+	IA_CSS_FRAME_FORMAT_NV12_16,    /** 16 bit YUV 420, Y, UV plane */
+	IA_CSS_FRAME_FORMAT_NV12_TILEY, /** 12 bit YUV 420, Intel proprietary tiled format, TileY */
+	IA_CSS_FRAME_FORMAT_NV16,       /** 16 bit YUV 422, Y, UV plane */
+	IA_CSS_FRAME_FORMAT_NV21,       /** 12 bit YUV 420, Y, VU plane */
+	IA_CSS_FRAME_FORMAT_NV61,       /** 16 bit YUV 422, Y, VU plane */
+	IA_CSS_FRAME_FORMAT_YV12,       /** 12 bit YUV 420, Y, V, U plane */
+	IA_CSS_FRAME_FORMAT_YV16,       /** 16 bit YUV 422, Y, V, U plane */
+	IA_CSS_FRAME_FORMAT_YUV420,     /** 12 bit YUV 420, Y, U, V plane */
+	IA_CSS_FRAME_FORMAT_YUV420_16,  /** yuv420, 16 bits per subpixel */
+	IA_CSS_FRAME_FORMAT_YUV422,     /** 16 bit YUV 422, Y, U, V plane */
+	IA_CSS_FRAME_FORMAT_YUV422_16,  /** yuv422, 16 bits per subpixel */
+	IA_CSS_FRAME_FORMAT_UYVY,       /** 16 bit YUV 422, UYVY interleaved */
+	IA_CSS_FRAME_FORMAT_YUYV,       /** 16 bit YUV 422, YUYV interleaved */
+	IA_CSS_FRAME_FORMAT_YUV444,     /** 24 bit YUV 444, Y, U, V plane */
+	IA_CSS_FRAME_FORMAT_YUV_LINE,   /** Internal format, 2 y lines followed
+					     by a uvinterleaved line */
+	IA_CSS_FRAME_FORMAT_RAW,	/** RAW, 1 plane */
+	IA_CSS_FRAME_FORMAT_RGB565,     /** 16 bit RGB, 1 plane. Each 3 sub
+					     pixels are packed into one 16 bit
+					     value, 5 bits for R, 6 bits for G
+					     and 5 bits for B. */
+	IA_CSS_FRAME_FORMAT_PLANAR_RGB888, /** 24 bit RGB, 3 planes */
+	IA_CSS_FRAME_FORMAT_RGBA888,	/** 32 bit RGBA, 1 plane, A=Alpha
+					     (alpha is unused) */
+	IA_CSS_FRAME_FORMAT_QPLANE6, /** Internal, for advanced ISP */
+	IA_CSS_FRAME_FORMAT_BINARY_8,	/** byte stream, used for jpeg. For
+					     frames of this type, we set the
+					     height to 1 and the width to the
+					     number of allocated bytes. */
+	IA_CSS_FRAME_FORMAT_MIPI,	/** MIPI frame, 1 plane */
+	IA_CSS_FRAME_FORMAT_RAW_PACKED, /** RAW, 1 plane, packed */
+	IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8,	      /** 8 bit per Y/U/V.
+							   Y odd line; UYVY
+							   interleaved even line */
+	IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8, /** Legacy YUV420. UY odd
+							   line; VY even line */
+	IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_10       /** 10 bit per Y/U/V. Y odd
+							   line; UYVY interleaved
+							   even line */
+};
+
+/* NOTE: IA_CSS_FRAME_FORMAT_NUM was purposely defined outside of enum type ia_css_frame_format, */
+/*       because of issues this would cause with the Clockwork code checking tool.               */
+#define IA_CSS_FRAME_FORMAT_NUM (IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_10 + 1)
+
+/* Number of valid output frame formats for ISP **/
+#define IA_CSS_FRAME_OUT_FORMAT_NUM	(IA_CSS_FRAME_FORMAT_RGBA888 + 1)
+
+#endif /* __IA_CSS_FRAME_FORMAT_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_frame_public.h b/drivers/staging/media/atomisp/pci/ia_css_frame_public.h
new file mode 100644
index 000000000..514d933f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_frame_public.h
@@ -0,0 +1,312 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FRAME_PUBLIC_H
+#define __IA_CSS_FRAME_PUBLIC_H
+
+/* @file
+ * This file contains structs to describe various frame-formats supported by the ISP.
+ */
+
+#include <type_support.h>
+#include "ia_css_err.h"
+#include "ia_css_types.h"
+#include "ia_css_frame_format.h"
+#include "ia_css_buffer.h"
+
+/* For RAW input, the bayer order needs to be specified separately. There
+ *  are 4 possible orders. The name is constructed by taking the first two
+ *  colors on the first line and the first two colors from the second line.
+ */
+enum ia_css_bayer_order {
+	IA_CSS_BAYER_ORDER_GRBG, /** GRGRGRGRGR .. BGBGBGBGBG */
+	IA_CSS_BAYER_ORDER_RGGB, /** RGRGRGRGRG .. GBGBGBGBGB */
+	IA_CSS_BAYER_ORDER_BGGR, /** BGBGBGBGBG .. GRGRGRGRGR */
+	IA_CSS_BAYER_ORDER_GBRG, /** GBGBGBGBGB .. RGRGRGRGRG */
+};
+
+#define IA_CSS_BAYER_ORDER_NUM (IA_CSS_BAYER_ORDER_GBRG + 1)
+
+/* Frame plane structure. This describes one plane in an image
+ *  frame buffer.
+ */
+struct ia_css_frame_plane {
+	unsigned int height; /** height of a plane in lines */
+	unsigned int width;  /** width of a line, in DMA elements, note that
+				  for RGB565 the three subpixels are stored in
+				  one element. For all other formats this is
+				  the number of subpixels per line. */
+	unsigned int stride; /** stride of a line in bytes */
+	unsigned int offset; /** offset in bytes to start of frame data.
+				  offset is wrt data field in ia_css_frame */
+};
+
+/* Binary "plane". This is used to story binary streams such as jpeg
+ *  images. This is not actually a real plane.
+ */
+struct ia_css_frame_binary_plane {
+	unsigned int		  size; /** number of bytes in the stream */
+	struct ia_css_frame_plane data; /** plane */
+};
+
+/* Container for planar YUV frames. This contains 3 planes.
+ */
+struct ia_css_frame_yuv_planes {
+	struct ia_css_frame_plane y; /** Y plane */
+	struct ia_css_frame_plane u; /** U plane */
+	struct ia_css_frame_plane v; /** V plane */
+};
+
+/* Container for semi-planar YUV frames.
+  */
+struct ia_css_frame_nv_planes {
+	struct ia_css_frame_plane y;  /** Y plane */
+	struct ia_css_frame_plane uv; /** UV plane */
+};
+
+/* Container for planar RGB frames. Each color has its own plane.
+ */
+struct ia_css_frame_rgb_planes {
+	struct ia_css_frame_plane r; /** Red plane */
+	struct ia_css_frame_plane g; /** Green plane */
+	struct ia_css_frame_plane b; /** Blue plane */
+};
+
+/* Container for 6-plane frames. These frames are used internally
+ *  in the advanced ISP only.
+ */
+struct ia_css_frame_plane6_planes {
+	struct ia_css_frame_plane r;	  /** Red plane */
+	struct ia_css_frame_plane r_at_b; /** Red at blue plane */
+	struct ia_css_frame_plane gr;	  /** Red-green plane */
+	struct ia_css_frame_plane gb;	  /** Blue-green plane */
+	struct ia_css_frame_plane b;	  /** Blue plane */
+	struct ia_css_frame_plane b_at_r; /** Blue at red plane */
+};
+
+/* Crop info struct - stores the lines to be cropped in isp */
+struct ia_css_crop_info {
+	/* the final start column and start line
+	 * sum of lines to be cropped + bayer offset
+	 */
+	unsigned int start_column;
+	unsigned int start_line;
+};
+
+/* Frame info struct. This describes the contents of an image frame buffer.
+  */
+struct ia_css_frame_info {
+	struct ia_css_resolution res; /** Frame resolution (valid data) */
+	unsigned int padded_width; /** stride of line in memory (in pixels) */
+	enum ia_css_frame_format format; /** format of the frame data */
+	unsigned int raw_bit_depth; /** number of valid bits per pixel,
+					 only valid for RAW bayer frames */
+	enum ia_css_bayer_order raw_bayer_order; /** bayer order, only valid
+						      for RAW bayer frames */
+	/* the params below are computed based on bayer_order
+	 * we can remove the raw_bayer_order if it is redundant
+	 * keeping it for now as bxt and fpn code seem to use it
+	 */
+	struct ia_css_crop_info crop_info;
+};
+
+#define IA_CSS_BINARY_DEFAULT_FRAME_INFO { \
+	.format			= IA_CSS_FRAME_FORMAT_NUM,  \
+	.raw_bayer_order	= IA_CSS_BAYER_ORDER_NUM, \
+}
+
+/**
+ *  Specifies the DVS loop delay in "frame periods"
+ */
+enum ia_css_frame_delay {
+	IA_CSS_FRAME_DELAY_0, /** Frame delay = 0 */
+	IA_CSS_FRAME_DELAY_1, /** Frame delay = 1 */
+	IA_CSS_FRAME_DELAY_2  /** Frame delay = 2 */
+};
+
+enum ia_css_frame_flash_state {
+	IA_CSS_FRAME_FLASH_STATE_NONE,
+	IA_CSS_FRAME_FLASH_STATE_PARTIAL,
+	IA_CSS_FRAME_FLASH_STATE_FULL
+};
+
+/* Frame structure. This structure describes an image buffer or frame.
+ *  This is the main structure used for all input and output images.
+ */
+struct ia_css_frame {
+	struct ia_css_frame_info info; /** info struct describing the frame */
+	ia_css_ptr   data;	       /** pointer to start of image data */
+	unsigned int data_bytes;       /** size of image data in bytes */
+	/* LA: move this to ia_css_buffer */
+	/*
+	 * -1 if data address is static during life time of pipeline
+	 * >=0 if data address can change per pipeline/frame iteration
+	 *     index to dynamic data: ia_css_frame_in, ia_css_frame_out
+	 *                            ia_css_frame_out_vf
+	 *     index to host-sp queue id: queue_0, queue_1 etc.
+	 */
+	int dynamic_queue_id;
+	/*
+	 * if it is dynamic frame, buf_type indicates which buffer type it
+	 * should use for event generation. we have this because in vf_pp
+	 * binary, we use output port, but we expect VF_OUTPUT_DONE event
+	 */
+	enum ia_css_buffer_type buf_type;
+	enum ia_css_frame_flash_state flash_state;
+	unsigned int exp_id;
+	/** exposure id, see ia_css_event_public.h for more detail */
+	u32 isp_config_id; /** Unique ID to track which config was actually applied to a particular frame */
+	bool valid; /** First video output frame is not valid */
+	union {
+		unsigned int	_initialisation_dummy;
+		struct ia_css_frame_plane raw;
+		struct ia_css_frame_plane rgb;
+		struct ia_css_frame_rgb_planes planar_rgb;
+		struct ia_css_frame_plane yuyv;
+		struct ia_css_frame_yuv_planes yuv;
+		struct ia_css_frame_nv_planes nv;
+		struct ia_css_frame_plane6_planes plane6;
+		struct ia_css_frame_binary_plane binary;
+	} planes; /** frame planes, select the right one based on
+		       info.format */
+};
+
+#define DEFAULT_FRAME { \
+	.info			= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.dynamic_queue_id	= SH_CSS_INVALID_QUEUE_ID, \
+	.buf_type		= IA_CSS_BUFFER_TYPE_INVALID, \
+	.flash_state		= IA_CSS_FRAME_FLASH_STATE_NONE, \
+}
+
+/* @brief Fill a frame with zeros
+ *
+ * @param	frame		The frame.
+ * @return	None
+ *
+ * Fill a frame with pixel values of zero
+ */
+void ia_css_frame_zero(struct ia_css_frame *frame);
+
+/* @brief Allocate a CSS frame structure
+ *
+ * @param	frame		The allocated frame.
+ * @param	width		The width (in pixels) of the frame.
+ * @param	height		The height (in lines) of the frame.
+ * @param	format		The frame format.
+ * @param	stride		The padded stride, in pixels.
+ * @param	raw_bit_depth	The raw bit depth, in bits.
+ * @return			The error code.
+ *
+ * Allocate a CSS frame structure. The memory for the frame data will be
+ * allocated in the CSS address space.
+ */
+int
+ia_css_frame_allocate(struct ia_css_frame **frame,
+		      unsigned int width,
+		      unsigned int height,
+		      enum ia_css_frame_format format,
+		      unsigned int stride,
+		      unsigned int raw_bit_depth);
+
+/* @brief Allocate a CSS frame structure using a frame info structure.
+ *
+ * @param	frame	The allocated frame.
+ * @param[in]	info	The frame info structure.
+ * @return		The error code.
+ *
+ * Allocate a frame using the resolution and format from a frame info struct.
+ * This is a convenience function, implemented on top of
+ * ia_css_frame_allocate().
+ */
+int
+ia_css_frame_allocate_from_info(struct ia_css_frame **frame,
+				const struct ia_css_frame_info *info);
+/* @brief Free a CSS frame structure.
+ *
+ * @param[in]	frame	Pointer to the frame.
+ * @return	None
+ *
+ * Free a CSS frame structure. This will free both the frame structure
+ * and the pixel data pointer contained within the frame structure.
+ */
+void
+ia_css_frame_free(struct ia_css_frame *frame);
+
+/* @brief Allocate a CSS frame structure using a frame info structure.
+ *
+ * @param	frame	The allocated frame.
+ * @param[in]	info	The frame info structure.
+ * @return		The error code.
+ *
+ * Allocate an empty CSS frame with no data buffer using the parameters
+ * in the frame info.
+ */
+int
+ia_css_frame_create_from_info(struct ia_css_frame **frame,
+			      const struct ia_css_frame_info *info);
+
+/* @brief Set a mapped data buffer to a CSS frame
+ *
+ * @param[in]	frame       Valid CSS frame pointer
+ * @param[in]	mapped_data  Mapped data buffer to be assigned to the CSS frame
+ * @param[in]	data_size_bytes  Size of the mapped_data in bytes
+ * @return      The error code.
+ *
+ * Sets a mapped data buffer to this frame. This function can be called multiple
+ * times with different buffers or NULL to reset the data pointer. This API
+ * would not try free the mapped_data and its the callers responsiblity to
+ * free the mapped_data buffer. However if ia_css_frame_free() is called and
+ * the frame had a valid data buffer, it would be freed along with the frame.
+ */
+int
+ia_css_frame_set_data(struct ia_css_frame *frame,
+		      const ia_css_ptr   mapped_data,
+		      size_t data_size_bytes);
+
+/* @brief Map an existing frame data pointer to a CSS frame.
+ *
+ * @param	frame		Pointer to the frame to be initialized
+ * @param[in]	info		The frame info.
+ * @param[in]	data		Pointer to the allocated frame data.
+ * @param[in]	attribute	Attributes to be passed to mmgr_mmap.
+ * @param[in]	context		Pointer to the a context to be passed to mmgr_mmap.
+ * @return			The allocated frame structure.
+ *
+ * This function maps a pre-allocated pointer into a CSS frame. This can be
+ * used when an upper software layer is responsible for allocating the frame
+ * data and it wants to share that frame pointer with the CSS code.
+ * This function will fill the CSS frame structure just like
+ * ia_css_frame_allocate() does, but instead of allocating the memory, it will
+ * map the pre-allocated memory into the CSS address space.
+ */
+int
+ia_css_frame_map(struct ia_css_frame **frame,
+		 const struct ia_css_frame_info *info,
+		 const void __user *data,
+		 unsigned int pgnr);
+
+/* @brief Unmap a CSS frame structure.
+ *
+ * @param[in]	frame	Pointer to the CSS frame.
+ * @return	None
+ *
+ * This function unmaps the frame data pointer within a CSS frame and
+ * then frees the CSS frame structure. Use this for frame pointers created
+ * using ia_css_frame_map().
+ */
+void
+ia_css_frame_unmap(struct ia_css_frame *frame);
+
+#endif /* __IA_CSS_FRAME_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_host_data.h b/drivers/staging/media/atomisp/pci/ia_css_host_data.h
new file mode 100644
index 000000000..f54cc504f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_host_data.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_HOST_DATA_H
+#define __SH_CSS_HOST_DATA_H
+
+#include <ia_css_types.h>	/* ia_css_pipe */
+
+/**
+ * @brief Allocate structure ia_css_host_data.
+ *
+ * @param[in]	size		Size of the requested host data
+ *
+ * @return
+ *	- NULL, can't allocate requested size
+ *	- pointer to structure, field address points to host data with size bytes
+ */
+struct ia_css_host_data *
+ia_css_host_data_allocate(size_t size);
+
+/**
+ * @brief Free structure ia_css_host_data.
+ *
+ * @param[in]	me	Pointer to structure, if a NULL is passed functions
+ *			returns without error. Otherwise a valid pointer to
+ *			structure must be passed and a related memory
+ *			is freed.
+ *
+ * @return
+ */
+void ia_css_host_data_free(struct ia_css_host_data *me);
+
+#endif /* __SH_CSS_HOST_DATA_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_input_port.h b/drivers/staging/media/atomisp/pci/ia_css_input_port.h
new file mode 100644
index 000000000..9772b6928
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_input_port.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* For MIPI_PORT0_ID to MIPI_PORT2_ID */
+#include "system_global.h"
+
+#ifndef __IA_CSS_INPUT_PORT_H
+#define __IA_CSS_INPUT_PORT_H
+
+/* @file
+ * This file contains information about the possible input ports for CSS
+ */
+
+/* Backward compatible for CSS API 2.0 only
+ *  TO BE REMOVED when all drivers move to CSS	API 2.1
+ */
+#define	IA_CSS_CSI2_PORT_4LANE MIPI_PORT0_ID
+#define	IA_CSS_CSI2_PORT_1LANE MIPI_PORT1_ID
+#define	IA_CSS_CSI2_PORT_2LANE MIPI_PORT2_ID
+
+/* The CSI2 interface supports 2 types of compression or can
+ *  be run without compression.
+ */
+enum ia_css_csi2_compression_type {
+	IA_CSS_CSI2_COMPRESSION_TYPE_NONE, /** No compression */
+	IA_CSS_CSI2_COMPRESSION_TYPE_1,    /** Compression scheme 1 */
+	IA_CSS_CSI2_COMPRESSION_TYPE_2     /** Compression scheme 2 */
+};
+
+struct ia_css_csi2_compression {
+	enum ia_css_csi2_compression_type type;
+	/** Compression used */
+	unsigned int                      compressed_bits_per_pixel;
+	/** Compressed bits per pixel (only when compression is enabled) */
+	unsigned int                      uncompressed_bits_per_pixel;
+	/** Uncompressed bits per pixel (only when compression is enabled) */
+};
+
+/* Input port structure.
+ */
+struct ia_css_input_port {
+	enum mipi_port_id port; /** Physical CSI-2 port */
+	unsigned int num_lanes; /** Number of lanes used (4-lane port only) */
+	unsigned int timeout;   /** Timeout value */
+	unsigned int rxcount;   /** Register value, should include all lanes */
+	struct ia_css_csi2_compression compression; /** Compression used */
+};
+
+#endif /* __IA_CSS_INPUT_PORT_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_irq.h b/drivers/staging/media/atomisp/pci/ia_css_irq.h
new file mode 100644
index 000000000..26b1b3c8b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_irq.h
@@ -0,0 +1,234 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_IRQ_H
+#define __IA_CSS_IRQ_H
+
+/* @file
+ * This file contains information for Interrupts/IRQs from CSS
+ */
+
+#include "ia_css_err.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_input_port.h"
+#include <linux/bits.h>
+
+/* Interrupt types, these enumerate all supported interrupt types.
+ */
+enum ia_css_irq_type {
+	IA_CSS_IRQ_TYPE_EDGE,  /** Edge (level) sensitive interrupt */
+	IA_CSS_IRQ_TYPE_PULSE  /** Pulse-shaped interrupt */
+};
+
+/* Interrupt request type.
+ *  When the CSS hardware generates an interrupt, a function in this API
+ *  needs to be called to retrieve information about the interrupt.
+ *  This interrupt type is part of this information and indicates what
+ *  type of information the interrupt signals.
+ *
+ *  Note that one interrupt can carry multiple interrupt types. For
+ *  example: the online video ISP will generate only 2 interrupts, one to
+ *  signal that the statistics (3a and DIS) are ready and one to signal
+ *  that all output frames are done (output and viewfinder).
+ *
+ * DEPRECATED, this interface is not portable it should only define user
+ * (SW) interrupts
+ */
+enum ia_css_irq_info {
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_ERROR            = BIT(0),
+	/** the css receiver has encountered an error */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_FIFO_OVERFLOW    = BIT(1),
+	/** the FIFO in the csi receiver has overflown */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF              = BIT(2),
+	/** the css receiver received the start of frame */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_EOF              = BIT(3),
+	/** the css receiver received the end of frame */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SOL              = BIT(4),
+	/** the css receiver received the start of line */
+	IA_CSS_IRQ_INFO_EVENTS_READY                  = BIT(5),
+	/** One or more events are available in the PSYS event queue */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_EOL              = BIT(6),
+	/** the css receiver received the end of line */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SIDEBAND_CHANGED = BIT(7),
+	/** the css receiver received a change in side band signals */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_GEN_SHORT_0      = BIT(8),
+	/** generic short packets (0) */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_GEN_SHORT_1      = BIT(9),
+	/** generic short packets (1) */
+	IA_CSS_IRQ_INFO_IF_PRIM_ERROR                 = BIT(10),
+	/** the primary input formatter (A) has encountered an error */
+	IA_CSS_IRQ_INFO_IF_PRIM_B_ERROR               = BIT(11),
+	/** the primary input formatter (B) has encountered an error */
+	IA_CSS_IRQ_INFO_IF_SEC_ERROR                  = BIT(12),
+	/** the secondary input formatter has encountered an error */
+	IA_CSS_IRQ_INFO_STREAM_TO_MEM_ERROR           = BIT(13),
+	/** the stream-to-memory device has encountered an error */
+	IA_CSS_IRQ_INFO_SW_0                          = BIT(14),
+	/** software interrupt 0 */
+	IA_CSS_IRQ_INFO_SW_1                          = BIT(15),
+	/** software interrupt 1 */
+	IA_CSS_IRQ_INFO_SW_2                          = BIT(16),
+	/** software interrupt 2 */
+	IA_CSS_IRQ_INFO_ISP_BINARY_STATISTICS_READY   = BIT(17),
+	/** ISP binary statistics are ready */
+	IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR            = BIT(18),
+	/** the input system in in error */
+	IA_CSS_IRQ_INFO_IF_ERROR                      = BIT(19),
+	/** the input formatter in in error */
+	IA_CSS_IRQ_INFO_DMA_ERROR                     = BIT(20),
+	/** the dma in in error */
+	IA_CSS_IRQ_INFO_ISYS_EVENTS_READY             = BIT(21),
+	/** end-of-frame events are ready in the isys_event queue */
+};
+
+/* CSS receiver error types. Whenever the CSS receiver has encountered
+ *  an error, this enumeration is used to indicate which errors have occurred.
+ *
+ *  Note that multiple error flags can be enabled at once and that this is in
+ *  fact common (whenever an error occurs, it usually results in multiple
+ *  errors).
+ *
+ * DEPRECATED: This interface is not portable, different systems have
+ * different receiver types, or possibly none in case of tests systems.
+ */
+enum ia_css_rx_irq_info {
+	IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN   = BIT(0),  /** buffer overrun */
+	IA_CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE = BIT(1),  /** entering sleep mode */
+	IA_CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE  = BIT(2),  /** exited sleep mode */
+	IA_CSS_RX_IRQ_INFO_ECC_CORRECTED    = BIT(3),  /** ECC corrected */
+	IA_CSS_RX_IRQ_INFO_ERR_SOT          = BIT(4),
+	/** Start of transmission */
+	IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC     = BIT(5),  /** SOT sync (??) */
+	IA_CSS_RX_IRQ_INFO_ERR_CONTROL      = BIT(6),  /** Control (??) */
+	IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE   = BIT(7),  /** Double ECC */
+	IA_CSS_RX_IRQ_INFO_ERR_CRC          = BIT(8),  /** CRC error */
+	IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID   = BIT(9),  /** Unknown ID */
+	IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC   = BIT(10), /** Frame sync error */
+	IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA   = BIT(11), /** Frame data error */
+	IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT = BIT(12), /** Timeout occurred */
+	IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC  = BIT(13), /** Unknown escape seq. */
+	IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC    = BIT(14), /** Line Sync error */
+	IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT     = BIT(15),
+};
+
+/* Interrupt info structure. This structure contains information about an
+ *  interrupt. This needs to be used after an interrupt is received on the IA
+ *  to perform the correct action.
+ */
+struct ia_css_irq {
+	enum ia_css_irq_info type; /** Interrupt type. */
+	unsigned int sw_irq_0_val; /** In case of SW interrupt 0, value. */
+	unsigned int sw_irq_1_val; /** In case of SW interrupt 1, value. */
+	unsigned int sw_irq_2_val; /** In case of SW interrupt 2, value. */
+	struct ia_css_pipe *pipe;
+	/** The image pipe that generated the interrupt. */
+};
+
+/* @brief Obtain interrupt information.
+ *
+ * @param[out] info	Pointer to the interrupt info. The interrupt
+ *			information wil be written to this info.
+ * @return		If an error is encountered during the interrupt info
+ *			and no interrupt could be translated successfully, this
+ *			will return IA_CSS_INTERNAL_ERROR. Otherwise
+ *			0.
+ *
+ * This function is expected to be executed after an interrupt has been sent
+ * to the IA from the CSS. This function returns information about the interrupt
+ * which is needed by the IA code to properly handle the interrupt. This
+ * information includes the image pipe, buffer type etc.
+ */
+int
+ia_css_irq_translate(unsigned int *info);
+
+/* @brief Get CSI receiver error info.
+ *
+ * @param[out] irq_bits	Pointer to the interrupt bits. The interrupt
+ *			bits will be written this info.
+ *			This will be the error bits that are enabled in the CSI
+ *			receiver error register.
+ * @return	None
+ *
+ * This function should be used whenever a CSI receiver error interrupt is
+ * generated. It provides the detailed information (bits) on the exact error
+ * that occurred.
+ *
+ *@deprecated {this function is DEPRECATED since it only works on CSI port 1.
+ * Use the function below instead and specify the appropriate port.}
+ */
+void
+ia_css_rx_get_irq_info(unsigned int *irq_bits);
+
+/* @brief Get CSI receiver error info.
+ *
+ * @param[in]  port     Input port identifier.
+ * @param[out] irq_bits	Pointer to the interrupt bits. The interrupt
+ *			bits will be written this info.
+ *			This will be the error bits that are enabled in the CSI
+ *			receiver error register.
+ * @return	None
+ *
+ * This function should be used whenever a CSI receiver error interrupt is
+ * generated. It provides the detailed information (bits) on the exact error
+ * that occurred.
+ */
+void
+ia_css_rx_port_get_irq_info(enum mipi_port_id port, unsigned int *irq_bits);
+
+/* @brief Clear CSI receiver error info.
+ *
+ * @param[in] irq_bits	The bits that should be cleared from the CSI receiver
+ *			interrupt bits register.
+ * @return	None
+ *
+ * This function should be called after ia_css_rx_get_irq_info has been called
+ * and the error bits have been interpreted. It is advised to use the return
+ * value of that function as the argument to this function to make sure no new
+ * error bits get overwritten.
+ *
+ * @deprecated{this function is DEPRECATED since it only works on CSI port 1.
+ * Use the function below instead and specify the appropriate port.}
+ */
+void
+ia_css_rx_clear_irq_info(unsigned int irq_bits);
+
+/* @brief Clear CSI receiver error info.
+ *
+ * @param[in] port      Input port identifier.
+ * @param[in] irq_bits	The bits that should be cleared from the CSI receiver
+ *			interrupt bits register.
+ * @return	None
+ *
+ * This function should be called after ia_css_rx_get_irq_info has been called
+ * and the error bits have been interpreted. It is advised to use the return
+ * value of that function as the argument to this function to make sure no new
+ * error bits get overwritten.
+ */
+void
+ia_css_rx_port_clear_irq_info(enum mipi_port_id port, unsigned int irq_bits);
+
+/* @brief Enable or disable specific interrupts.
+ *
+ * @param[in] type	The interrupt type that will be enabled/disabled.
+ * @param[in] enable	enable or disable.
+ * @return		Returns IA_CSS_INTERNAL_ERROR if this interrupt
+ *			type cannot be enabled/disabled which is true for
+ *			CSS internal interrupts. Otherwise returns
+ *			0.
+ */
+int
+ia_css_irq_enable(enum ia_css_irq_info type, bool enable);
+
+#endif /* __IA_CSS_IRQ_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_configs.c b/drivers/staging/media/atomisp/pci/ia_css_isp_configs.c
new file mode 100644
index 000000000..d28a76a68
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_configs.c
@@ -0,0 +1,321 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* Generated code: do not edit or commmit. */
+
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_configs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+int ia_css_configure_iterator(const struct ia_css_binary *binary,
+			      const struct ia_css_iterator_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.iterator.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.iterator.offset;
+
+	ia_css_iterator_config((struct sh_css_isp_iterator_isp_config *)
+			       &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			       config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_copy_output(const struct ia_css_binary *binary,
+				 const struct ia_css_copy_output_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.copy_output.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.copy_output.offset;
+
+	ia_css_copy_output_config((struct sh_css_isp_copy_output_isp_config *)
+				  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+				  config_dmem, size);
+	return 0;
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+int ia_css_configure_crop(const struct ia_css_binary *binary,
+			  const struct ia_css_crop_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.crop.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.crop.offset;
+
+	ia_css_crop_config((struct sh_css_isp_crop_isp_config *)
+			   &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			   config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_fpn(const struct ia_css_binary *binary,
+			 const struct ia_css_fpn_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size   = binary->info->mem_offsets.offsets.config->dmem.fpn.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.fpn.offset;
+	ia_css_fpn_config((struct sh_css_isp_fpn_isp_config *)
+			  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			  config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_dvs(const struct ia_css_binary *binary,
+			 const struct ia_css_dvs_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.dvs.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.dvs.offset;
+	ia_css_dvs_config((struct sh_css_isp_dvs_isp_config *)
+			  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			  config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_qplane(const struct ia_css_binary *binary,
+			    const struct ia_css_qplane_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.qplane.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.qplane.offset;
+	ia_css_qplane_config((struct sh_css_isp_qplane_isp_config *)
+			     &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			     config_dmem, size);
+
+	return 0;
+}
+
+int ia_css_configure_output0(const struct ia_css_binary *binary,
+			     const struct ia_css_output0_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.output0.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.output0.offset;
+
+	ia_css_output0_config((struct sh_css_isp_output_isp_config *)
+			      &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			      config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_output1(const struct ia_css_binary *binary,
+			     const struct ia_css_output1_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.output1.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.output1.offset;
+
+	ia_css_output1_config((struct sh_css_isp_output_isp_config *)
+			      &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			      config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_output(const struct ia_css_binary *binary,
+			    const struct ia_css_output_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.output.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.output.offset;
+
+	ia_css_output_config((struct sh_css_isp_output_isp_config *)
+			     &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+					     config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_raw(const struct ia_css_binary *binary,
+			 const struct ia_css_raw_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.raw.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.raw.offset;
+
+	ia_css_raw_config((struct sh_css_isp_raw_isp_config *)
+			  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			  config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_tnr(const struct ia_css_binary *binary,
+			 const struct ia_css_tnr_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.tnr.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.tnr.offset;
+
+	ia_css_tnr_config((struct sh_css_isp_tnr_isp_config *)
+			  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			  config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_ref(const struct ia_css_binary *binary,
+			 const struct ia_css_ref_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.ref.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.ref.offset;
+
+	ia_css_ref_config((struct sh_css_isp_ref_isp_config *)
+			  &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			  config_dmem, size);
+	return 0;
+}
+
+int ia_css_configure_vf(const struct ia_css_binary *binary,
+			const struct ia_css_vf_configuration *config_dmem)
+{
+	unsigned int offset = 0;
+	unsigned int size   = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "%s:\n", __func__);
+
+	if (!binary->info->mem_offsets.offsets.config)
+		return 0;
+
+	size = binary->info->mem_offsets.offsets.config->dmem.vf.size;
+	if (!size)
+		return 0;
+
+	offset = binary->info->mem_offsets.offsets.config->dmem.vf.offset;
+
+	ia_css_vf_config((struct sh_css_isp_vf_isp_config *)
+			 &binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+			 config_dmem, size);
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_configs.h b/drivers/staging/media/atomisp/pci/ia_css_isp_configs.h
new file mode 100644
index 000000000..fffcfc871
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_configs.h
@@ -0,0 +1,119 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifdef IA_CSS_INCLUDE_CONFIGURATIONS
+#include "isp/kernels/crop/crop_1.0/ia_css_crop.host.h"
+#include "isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h"
+#include "isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h"
+#include "isp/kernels/ob/ob_1.0/ia_css_ob.host.h"
+#include "isp/kernels/output/output_1.0/ia_css_output.host.h"
+#include "isp/kernels/qplane/qplane_2/ia_css_qplane.host.h"
+#include "isp/kernels/raw/raw_1.0/ia_css_raw.host.h"
+#include "isp/kernels/ref/ref_1.0/ia_css_ref.host.h"
+#include "isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h"
+#include "isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h"
+#include "isp/kernels/vf/vf_1.0/ia_css_vf.host.h"
+#include "isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h"
+#include "isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h"
+#endif
+
+#ifndef _IA_CSS_ISP_CONFIG_H
+#define _IA_CSS_ISP_CONFIG_H
+
+enum ia_css_configuration_ids {
+	IA_CSS_ITERATOR_CONFIG_ID,
+	IA_CSS_COPY_OUTPUT_CONFIG_ID,
+	IA_CSS_CROP_CONFIG_ID,
+	IA_CSS_FPN_CONFIG_ID,
+	IA_CSS_DVS_CONFIG_ID,
+	IA_CSS_QPLANE_CONFIG_ID,
+	IA_CSS_OUTPUT0_CONFIG_ID,
+	IA_CSS_OUTPUT1_CONFIG_ID,
+	IA_CSS_OUTPUT_CONFIG_ID,
+	IA_CSS_RAW_CONFIG_ID,
+	IA_CSS_TNR_CONFIG_ID,
+	IA_CSS_REF_CONFIG_ID,
+	IA_CSS_VF_CONFIG_ID,
+
+	/* ISP 2401 */
+	IA_CSS_SC_CONFIG_ID,
+
+	IA_CSS_NUM_CONFIGURATION_IDS
+};
+
+struct ia_css_config_memory_offsets {
+	struct {
+		struct ia_css_isp_parameter iterator;
+		struct ia_css_isp_parameter copy_output;
+		struct ia_css_isp_parameter crop;
+		struct ia_css_isp_parameter fpn;
+		struct ia_css_isp_parameter dvs;
+		struct ia_css_isp_parameter qplane;
+		struct ia_css_isp_parameter output0;
+		struct ia_css_isp_parameter output1;
+		struct ia_css_isp_parameter output;
+		struct ia_css_isp_parameter raw;
+		struct ia_css_isp_parameter tnr;
+		struct ia_css_isp_parameter ref;
+		struct ia_css_isp_parameter vf;
+	} dmem;
+};
+
+#if defined(IA_CSS_INCLUDE_CONFIGURATIONS)
+
+#include "ia_css_stream.h"   /* struct ia_css_stream */
+#include "ia_css_binary.h"   /* struct ia_css_binary */
+
+int ia_css_configure_iterator(const struct ia_css_binary *binary,
+			      const struct ia_css_iterator_configuration *config_dmem);
+
+int ia_css_configure_copy_output(const struct ia_css_binary *binary,
+				 const struct ia_css_copy_output_configuration *config_dmem);
+
+int ia_css_configure_crop(const struct ia_css_binary *binary,
+			  const struct ia_css_crop_configuration *config_dmem);
+
+int ia_css_configure_fpn(const struct ia_css_binary *binary,
+			 const struct ia_css_fpn_configuration *config_dmem);
+
+int ia_css_configure_dvs(const struct ia_css_binary *binary,
+			 const struct ia_css_dvs_configuration *config_dmem);
+
+int ia_css_configure_qplane(const struct ia_css_binary *binary,
+			    const struct ia_css_qplane_configuration *config_dmem);
+int ia_css_configure_output0(const struct ia_css_binary *binary,
+			     const struct ia_css_output0_configuration *config_dmem);
+
+int ia_css_configure_output1(const struct ia_css_binary *binary,
+			     const struct ia_css_output1_configuration *config_dmem);
+
+int ia_css_configure_output(const struct ia_css_binary *binary,
+			    const struct ia_css_output_configuration *config_dmem);
+
+int ia_css_configure_raw(const struct ia_css_binary *binary,
+			 const struct ia_css_raw_configuration *config_dmem);
+
+int ia_css_configure_tnr(const struct ia_css_binary *binary,
+			 const struct ia_css_tnr_configuration *config_dmem);
+
+int ia_css_configure_ref(const struct ia_css_binary *binary,
+			 const struct ia_css_ref_configuration *config_dmem);
+
+int ia_css_configure_vf(const struct ia_css_binary *binary,
+			const struct ia_css_vf_configuration *config_dmem);
+
+#endif /* IA_CSS_INCLUDE_CONFIGURATION */
+
+#endif /* _IA_CSS_ISP_CONFIG_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_params.c b/drivers/staging/media/atomisp/pci/ia_css_isp_params.c
new file mode 100644
index 000000000..503ac65da
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_params.c
@@ -0,0 +1,3344 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#define IA_CSS_INCLUDE_PARAMETERS
+#include "sh_css_params.h"
+#include "isp/kernels/aa/aa_2/ia_css_aa2.host.h"
+#include "isp/kernels/anr/anr_1.0/ia_css_anr.host.h"
+#include "isp/kernels/anr/anr_2/ia_css_anr2.host.h"
+#include "isp/kernels/bh/bh_2/ia_css_bh.host.h"
+#include "isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h"
+#include "isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h"
+#include "isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h"
+#include "isp/kernels/crop/crop_1.0/ia_css_crop.host.h"
+#include "isp/kernels/csc/csc_1.0/ia_css_csc.host.h"
+#include "isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h"
+#include "isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h"
+#include "isp/kernels/ctc/ctc2/ia_css_ctc2.host.h"
+#include "isp/kernels/de/de_1.0/ia_css_de.host.h"
+#include "isp/kernels/de/de_2/ia_css_de2.host.h"
+#include "isp/kernels/dp/dp_1.0/ia_css_dp.host.h"
+#include "isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h"
+#include "isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h"
+#include "isp/kernels/gc/gc_1.0/ia_css_gc.host.h"
+#include "isp/kernels/gc/gc_2/ia_css_gc2.host.h"
+#include "isp/kernels/macc/macc_1.0/ia_css_macc.host.h"
+#include "isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h"
+#include "isp/kernels/ob/ob_1.0/ia_css_ob.host.h"
+#include "isp/kernels/ob/ob2/ia_css_ob2.host.h"
+#include "isp/kernels/output/output_1.0/ia_css_output.host.h"
+#include "isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h"
+#include "isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h"
+#include "isp/kernels/sc/sc_1.0/ia_css_sc.host.h"
+#include "isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h"
+#include "isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h"
+#include "isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h"
+#include "isp/kernels/uds/uds_1.0/ia_css_uds_param.h"
+#include "isp/kernels/wb/wb_1.0/ia_css_wb.host.h"
+#include "isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h"
+#include "isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h"
+#include "isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h"
+#include "isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h"
+#include "isp/kernels/fc/fc_1.0/ia_css_formats.host.h"
+#include "isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h"
+#include "isp/kernels/dpc2/ia_css_dpc2.host.h"
+#include "isp/kernels/eed1_8/ia_css_eed1_8.host.h"
+#include "isp/kernels/bnlm/ia_css_bnlm.host.h"
+#include "isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h"
+/* Generated code: do not edit or commmit. */
+
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_params.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_aa(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	unsigned int size   =
+	    stage->binary->info->mem_offsets.offsets.param->dmem.aa.size;
+	unsigned int offset =
+	    stage->binary->info->mem_offsets.offsets.param->dmem.aa.offset;
+
+	if (size) {
+		struct sh_css_isp_aa_params *t =  (struct sh_css_isp_aa_params *)
+						  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+		t->strength = params->aa_config.strength;
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_anr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.anr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.anr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_anr() enter:\n");
+
+			ia_css_anr_encode((struct sh_css_isp_anr_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->anr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_anr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_anr2(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.anr2.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.anr2.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_anr2() enter:\n");
+
+			ia_css_anr2_vmem_encode((struct ia_css_isp_anr2_params *)
+						&stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+						&params->anr_thres,
+						size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_anr2() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_bh(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bh.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bh.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_bh() enter:\n");
+
+			ia_css_bh_encode((struct sh_css_isp_bh_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->s3a_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_bh() leave:\n");
+		}
+	}
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->hmem0.bh.size;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_bh() enter:\n");
+
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_HMEM0] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_bh() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_cnr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.cnr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.cnr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_cnr() enter:\n");
+
+			ia_css_cnr_encode((struct sh_css_isp_cnr_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->cnr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_cnr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_crop(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.crop.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.crop.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_crop() enter:\n");
+
+			ia_css_crop_encode((struct sh_css_isp_crop_isp_params *)
+					   &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					   &params->crop_config,
+					   size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_crop() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_csc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.csc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.csc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_csc() enter:\n");
+
+			ia_css_csc_encode((struct sh_css_isp_csc_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->cc_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_csc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_dp(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.dp.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.dp.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_dp() enter:\n");
+
+			ia_css_dp_encode((struct sh_css_isp_dp_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->dp_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_dp() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_bnr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bnr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bnr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_bnr() enter:\n");
+
+			ia_css_bnr_encode((struct sh_css_isp_bnr_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->nr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_bnr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_de(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.de.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.de.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_de() enter:\n");
+
+			ia_css_de_encode((struct sh_css_isp_de_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->de_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_de() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_ecd(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ecd.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ecd.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ecd() enter:\n");
+
+			ia_css_ecd_encode((struct sh_css_isp_ecd_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->ecd_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ecd() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_formats(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.formats.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.formats.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_formats() enter:\n");
+
+			ia_css_formats_encode((struct sh_css_isp_formats_params *)
+					      &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					      &params->formats_config,
+					      size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_formats() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_fpn(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.fpn.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.fpn.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_fpn() enter:\n");
+
+			ia_css_fpn_encode((struct sh_css_isp_fpn_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->fpn_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_fpn() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_gc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.gc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.gc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_gc() enter:\n");
+
+			ia_css_gc_encode((struct sh_css_isp_gc_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->gc_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_gc() leave:\n");
+		}
+	}
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.gc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.gc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_gc() enter:\n");
+
+			ia_css_gc_vamem_encode((struct sh_css_isp_gc_vamem_params *)
+					       &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM1].address[offset],
+					       &params->gc_table,
+					       size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM1] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_gc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_ce(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ce.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ce.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ce() enter:\n");
+
+			ia_css_ce_encode((struct sh_css_isp_ce_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->ce_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ce() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_yuv2rgb(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.yuv2rgb.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.yuv2rgb.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_yuv2rgb() enter:\n");
+
+			ia_css_yuv2rgb_encode((struct sh_css_isp_csc_params *)
+					      &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					      &params->yuv2rgb_cc_config,
+					      size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_yuv2rgb() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_rgb2yuv(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.rgb2yuv.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.rgb2yuv.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_rgb2yuv() enter:\n");
+
+			ia_css_rgb2yuv_encode((struct sh_css_isp_csc_params *)
+					      &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					      &params->rgb2yuv_cc_config,
+					      size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_rgb2yuv() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_r_gamma(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem0.r_gamma.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem0.r_gamma.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_r_gamma() enter:\n");
+
+			ia_css_r_gamma_vamem_encode((struct sh_css_isp_rgb_gamma_vamem_params *)
+						    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM0].address[offset],
+						    &params->r_gamma_table,
+						    size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM0] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_r_gamma() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_g_gamma(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.g_gamma.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.g_gamma.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_g_gamma() enter:\n");
+
+			ia_css_g_gamma_vamem_encode((struct sh_css_isp_rgb_gamma_vamem_params *)
+						    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM1].address[offset],
+						    &params->g_gamma_table,
+						    size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM1] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_g_gamma() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_b_gamma(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem2.b_gamma.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem2.b_gamma.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_b_gamma() enter:\n");
+
+			ia_css_b_gamma_vamem_encode((struct sh_css_isp_rgb_gamma_vamem_params *)
+						    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM2].address[offset],
+						    &params->b_gamma_table,
+						    size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM2] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_b_gamma() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_uds(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.uds.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.uds.offset;
+
+		if (size) {
+			struct sh_css_sp_uds_params *p;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_uds() enter:\n");
+
+			p = (struct sh_css_sp_uds_params *)
+			    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+			p->crop_pos = params->uds_config.crop_pos;
+			p->uds = params->uds_config.uds;
+
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_uds() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_raa(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.raa.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.raa.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_raa() enter:\n");
+
+			ia_css_raa_encode((struct sh_css_isp_aa_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->raa_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_raa() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_s3a(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.s3a.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.s3a.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_s3a() enter:\n");
+
+			ia_css_s3a_encode((struct sh_css_isp_s3a_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->s3a_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_s3a() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_ob(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ob.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ob.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ob() enter:\n");
+
+			ia_css_ob_encode((struct sh_css_isp_ob_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->ob_config,
+					 &params->stream_configs.ob, size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ob() leave:\n");
+		}
+	}
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.ob.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.ob.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ob() enter:\n");
+
+			ia_css_ob_vmem_encode((struct sh_css_isp_ob_vmem_params *)
+					      &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+					      &params->ob_config,
+					      &params->stream_configs.ob, size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_ob() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_output(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.output.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.output.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_output() enter:\n");
+
+			ia_css_output_encode((struct sh_css_isp_output_params *)
+					     &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					     &params->output_config,
+					     size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_output() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_sc() enter:\n");
+
+			ia_css_sc_encode((struct sh_css_isp_sc_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->sc_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_sc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_bds(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bds.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.bds.offset;
+
+		if (size) {
+			struct sh_css_isp_bds_params *p;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_bds() enter:\n");
+
+			p = (struct sh_css_isp_bds_params *)
+			    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+			p->baf_strength = params->bds_config.strength;
+
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_bds() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_tnr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.tnr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.tnr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_tnr() enter:\n");
+
+			ia_css_tnr_encode((struct sh_css_isp_tnr_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->tnr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_tnr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_macc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.macc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.macc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_macc() enter:\n");
+
+			ia_css_macc_encode((struct sh_css_isp_macc_params *)
+					   &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					   &params->macc_config,
+					   size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_macc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis_horicoef(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis_horicoef.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis_horicoef.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_horicoef() enter:\n");
+
+			ia_css_sdis_horicoef_vmem_encode((struct sh_css_isp_sdis_hori_coef_tbl *)
+							 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+							 &params->dvs_coefs,
+							 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_horicoef() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis_vertcoef(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis_vertcoef.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis_vertcoef.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_vertcoef() enter:\n");
+
+			ia_css_sdis_vertcoef_vmem_encode((struct sh_css_isp_sdis_vert_coef_tbl *)
+							 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+							 &params->dvs_coefs,
+							 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_vertcoef() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis_horiproj(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis_horiproj.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis_horiproj.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_horiproj() enter:\n");
+
+			ia_css_sdis_horiproj_encode((struct sh_css_isp_sdis_hori_proj_tbl *)
+						    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+						    &params->dvs_coefs,
+						    size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_horiproj() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis_vertproj(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis_vertproj.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis_vertproj.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_vertproj() enter:\n");
+
+			ia_css_sdis_vertproj_encode((struct sh_css_isp_sdis_vert_proj_tbl *)
+						    &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+						    &params->dvs_coefs,
+						    size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis_vertproj() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis2_horicoef(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_horicoef.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_horicoef.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_horicoef() enter:\n");
+
+			ia_css_sdis2_horicoef_vmem_encode((struct sh_css_isp_sdis_hori_coef_tbl *)
+							  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+							  &params->dvs2_coefs,
+							  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_horicoef() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis2_vertcoef(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_vertcoef.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_vertcoef.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_vertcoef() enter:\n");
+
+			ia_css_sdis2_vertcoef_vmem_encode((struct sh_css_isp_sdis_vert_coef_tbl *)
+							  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+							  &params->dvs2_coefs,
+							  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_vertcoef() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis2_horiproj(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_horiproj.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_horiproj.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_horiproj() enter:\n");
+
+			ia_css_sdis2_horiproj_encode((struct sh_css_isp_sdis_hori_proj_tbl *)
+						     &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+						     &params->dvs2_coefs,
+						     size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_horiproj() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_sdis2_vertproj(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_vertproj.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_vertproj.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_vertproj() enter:\n");
+
+			ia_css_sdis2_vertproj_encode((struct sh_css_isp_sdis_vert_proj_tbl *)
+						     &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+						     &params->dvs2_coefs,
+						     size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_sdis2_vertproj() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_wb(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.wb.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.wb.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_wb() enter:\n");
+
+			ia_css_wb_encode((struct sh_css_isp_wb_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->wb_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_wb() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_nr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.nr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.nr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_nr() enter:\n");
+
+			ia_css_nr_encode((struct sh_css_isp_ynr_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->nr_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_nr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_yee(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.yee.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.yee.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_yee() enter:\n");
+
+			ia_css_yee_encode((struct sh_css_isp_yee_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->yee_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_yee() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_ynr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ynr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ynr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ynr() enter:\n");
+
+			ia_css_ynr_encode((struct sh_css_isp_yee2_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->ynr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ynr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_fc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.fc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.fc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_fc() enter:\n");
+
+			ia_css_fc_encode((struct sh_css_isp_fc_params *)
+					 &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					 &params->fc_config,
+					 size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_fc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_ctc(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ctc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.ctc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ctc() enter:\n");
+
+			ia_css_ctc_encode((struct sh_css_isp_ctc_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->ctc_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ctc() leave:\n");
+		}
+	}
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem0.ctc.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem0.ctc.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ctc() enter:\n");
+
+			ia_css_ctc_vamem_encode((struct sh_css_isp_ctc_vamem_params *)
+						&stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM0].address[offset],
+						&params->ctc_table,
+						size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM0] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_ctc() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_xnr_table(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.xnr_table.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->vamem1.xnr_table.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr_table() enter:\n");
+
+			ia_css_xnr_table_vamem_encode((struct sh_css_isp_xnr_vamem_params *)
+						      &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VAMEM1].address[offset],
+						      &params->xnr_table,
+						      size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_VAMEM1] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr_table() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_xnr(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.xnr.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.xnr.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr() enter:\n");
+
+			ia_css_xnr_encode((struct sh_css_isp_xnr_params *)
+					  &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					  &params->xnr_config,
+					  size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_xnr3(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	{
+		unsigned int size   =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.xnr3.size;
+
+		unsigned int offset =
+		    stage->binary->info->mem_offsets.offsets.param->dmem.xnr3.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr3() enter:\n");
+
+			ia_css_xnr3_encode((struct sh_css_isp_xnr3_params *)
+					   &stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+					   &params->xnr3_config,
+					   size);
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] =
+			    true;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					    "ia_css_process_xnr3() leave:\n");
+		}
+	}
+}
+
+/* Code generated by genparam/gencode.c:gen_param_process_table() */
+
+void (*ia_css_kernel_process_param[IA_CSS_NUM_PARAMETER_IDS])(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params) = {
+	ia_css_process_aa,
+	ia_css_process_anr,
+	ia_css_process_anr2,
+	ia_css_process_bh,
+	ia_css_process_cnr,
+	ia_css_process_crop,
+	ia_css_process_csc,
+	ia_css_process_dp,
+	ia_css_process_bnr,
+	ia_css_process_de,
+	ia_css_process_ecd,
+	ia_css_process_formats,
+	ia_css_process_fpn,
+	ia_css_process_gc,
+	ia_css_process_ce,
+	ia_css_process_yuv2rgb,
+	ia_css_process_rgb2yuv,
+	ia_css_process_r_gamma,
+	ia_css_process_g_gamma,
+	ia_css_process_b_gamma,
+	ia_css_process_uds,
+	ia_css_process_raa,
+	ia_css_process_s3a,
+	ia_css_process_ob,
+	ia_css_process_output,
+	ia_css_process_sc,
+	ia_css_process_bds,
+	ia_css_process_tnr,
+	ia_css_process_macc,
+	ia_css_process_sdis_horicoef,
+	ia_css_process_sdis_vertcoef,
+	ia_css_process_sdis_horiproj,
+	ia_css_process_sdis_vertproj,
+	ia_css_process_sdis2_horicoef,
+	ia_css_process_sdis2_vertcoef,
+	ia_css_process_sdis2_horiproj,
+	ia_css_process_sdis2_vertproj,
+	ia_css_process_wb,
+	ia_css_process_nr,
+	ia_css_process_yee,
+	ia_css_process_ynr,
+	ia_css_process_fc,
+	ia_css_process_ctc,
+	ia_css_process_xnr_table,
+	ia_css_process_xnr,
+	ia_css_process_xnr3,
+};
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_dp_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_dp_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_dp_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dp_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_dp_config() leave\n");
+	ia_css_dp_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_dp_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_dp_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_dp_config() enter:\n");
+	ia_css_dp_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dp_config = *config;
+	params->config_changed[IA_CSS_DP_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_dp_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_wb_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_wb_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_wb_config() enter: config=%p\n",
+			    config);
+
+	*config = params->wb_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_wb_config() leave\n");
+	ia_css_wb_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_wb_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_wb_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_wb_config() enter:\n");
+	ia_css_wb_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->wb_config = *config;
+	params->config_changed[IA_CSS_WB_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_wb_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_tnr_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_tnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_tnr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->tnr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_tnr_config() leave\n");
+	ia_css_tnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_tnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_tnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_tnr_config() enter:\n");
+	ia_css_tnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->tnr_config = *config;
+	params->config_changed[IA_CSS_TNR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_tnr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_ob_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_ob_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ob_config() enter: config=%p\n",
+			    config);
+
+	*config = params->ob_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ob_config() leave\n");
+	ia_css_ob_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ob_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ob_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_ob_config() enter:\n");
+	ia_css_ob_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->ob_config = *config;
+	params->config_changed[IA_CSS_OB_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_ob_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_de_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_de_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_de_config() enter: config=%p\n",
+			    config);
+
+	*config = params->de_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_de_config() leave\n");
+	ia_css_de_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_de_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_de_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_de_config() enter:\n");
+	ia_css_de_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->de_config = *config;
+	params->config_changed[IA_CSS_DE_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_de_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_anr_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_anr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_anr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->anr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_anr_config() leave\n");
+	ia_css_anr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_anr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_anr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_anr_config() enter:\n");
+	ia_css_anr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->anr_config = *config;
+	params->config_changed[IA_CSS_ANR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_anr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_anr2_config(const struct ia_css_isp_parameters *params,
+		       struct ia_css_anr_thres *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_anr2_config() enter: config=%p\n",
+			    config);
+
+	*config = params->anr_thres;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_anr2_config() leave\n");
+	ia_css_anr2_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_anr2_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_anr_thres *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_anr2_config() enter:\n");
+	ia_css_anr2_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->anr_thres = *config;
+	params->config_changed[IA_CSS_ANR2_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_anr2_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_ce_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_ce_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ce_config() enter: config=%p\n",
+			    config);
+
+	*config = params->ce_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ce_config() leave\n");
+	ia_css_ce_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ce_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ce_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_ce_config() enter:\n");
+	ia_css_ce_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->ce_config = *config;
+	params->config_changed[IA_CSS_CE_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_ce_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_ecd_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_ecd_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ecd_config() enter: config=%p\n",
+			    config);
+
+	*config = params->ecd_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ecd_config() leave\n");
+	ia_css_ecd_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ecd_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ecd_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_ecd_config() enter:\n");
+	ia_css_ecd_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->ecd_config = *config;
+	params->config_changed[IA_CSS_ECD_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_ecd_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_ynr_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_ynr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ynr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->ynr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ynr_config() leave\n");
+	ia_css_ynr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ynr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ynr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_ynr_config() enter:\n");
+	ia_css_ynr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->ynr_config = *config;
+	params->config_changed[IA_CSS_YNR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_ynr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_fc_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_fc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_fc_config() enter: config=%p\n",
+			    config);
+
+	*config = params->fc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_fc_config() leave\n");
+	ia_css_fc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_fc_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_fc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_fc_config() enter:\n");
+	ia_css_fc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->fc_config = *config;
+	params->config_changed[IA_CSS_FC_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_fc_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_cnr_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_cnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_cnr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->cnr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_cnr_config() leave\n");
+	ia_css_cnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_cnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_cnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_cnr_config() enter:\n");
+	ia_css_cnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->cnr_config = *config;
+	params->config_changed[IA_CSS_CNR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_cnr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_macc_config(const struct ia_css_isp_parameters *params,
+		       struct ia_css_macc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_macc_config() enter: config=%p\n",
+			    config);
+
+	*config = params->macc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_macc_config() leave\n");
+	ia_css_macc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_macc_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_macc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_macc_config() enter:\n");
+	ia_css_macc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->macc_config = *config;
+	params->config_changed[IA_CSS_MACC_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_macc_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_ctc_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_ctc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ctc_config() enter: config=%p\n",
+			    config);
+
+	*config = params->ctc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_ctc_config() leave\n");
+	ia_css_ctc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ctc_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ctc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_ctc_config() enter:\n");
+	ia_css_ctc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->ctc_config = *config;
+	params->config_changed[IA_CSS_CTC_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_ctc_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_aa_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_aa_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_aa_config() enter: config=%p\n",
+			    config);
+
+	*config = params->aa_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_aa_config() leave\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_aa_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_aa_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_aa_config() enter:\n");
+	params->aa_config = *config;
+	params->config_changed[IA_CSS_AA_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_aa_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_yuv2rgb_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_yuv2rgb_config() enter: config=%p\n",
+			    config);
+
+	*config = params->yuv2rgb_cc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_yuv2rgb_config() leave\n");
+	ia_css_yuv2rgb_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_yuv2rgb_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_yuv2rgb_config() enter:\n");
+	ia_css_yuv2rgb_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->yuv2rgb_cc_config = *config;
+	params->config_changed[IA_CSS_YUV2RGB_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_yuv2rgb_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_rgb2yuv_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_rgb2yuv_config() enter: config=%p\n",
+			    config);
+
+	*config = params->rgb2yuv_cc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_rgb2yuv_config() leave\n");
+	ia_css_rgb2yuv_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_rgb2yuv_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_rgb2yuv_config() enter:\n");
+	ia_css_rgb2yuv_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->rgb2yuv_cc_config = *config;
+	params->config_changed[IA_CSS_RGB2YUV_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_rgb2yuv_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_csc_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_csc_config() enter: config=%p\n",
+			    config);
+
+	*config = params->cc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_csc_config() leave\n");
+	ia_css_csc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_csc_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_cc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_csc_config() enter:\n");
+	ia_css_csc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->cc_config = *config;
+	params->config_changed[IA_CSS_CSC_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_csc_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_nr_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_nr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_nr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->nr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_nr_config() leave\n");
+	ia_css_nr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_nr_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_nr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_nr_config() enter:\n");
+	ia_css_nr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->nr_config = *config;
+	params->config_changed[IA_CSS_BNR_ID] = true;
+	params->config_changed[IA_CSS_NR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_nr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_gc_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_gc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_gc_config() enter: config=%p\n",
+			    config);
+
+	*config = params->gc_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_gc_config() leave\n");
+	ia_css_gc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_gc_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_gc_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_gc_config() enter:\n");
+	ia_css_gc_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->gc_config = *config;
+	params->config_changed[IA_CSS_GC_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_gc_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis_horicoef_config(const struct ia_css_isp_parameters *params,
+				struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_horicoef_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_horicoef_config() leave\n");
+	ia_css_sdis_horicoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_horicoef_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis_horicoef_config() enter:\n");
+	ia_css_sdis_horicoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs_coefs = *config;
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis_horicoef_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis_vertcoef_config(const struct ia_css_isp_parameters *params,
+				struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_vertcoef_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_vertcoef_config() leave\n");
+	ia_css_sdis_vertcoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_vertcoef_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis_vertcoef_config() enter:\n");
+	ia_css_sdis_vertcoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs_coefs = *config;
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis_vertcoef_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis_horiproj_config(const struct ia_css_isp_parameters *params,
+				struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_horiproj_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_horiproj_config() leave\n");
+	ia_css_sdis_horiproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_horiproj_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis_horiproj_config() enter:\n");
+	ia_css_sdis_horiproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs_coefs = *config;
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis_horiproj_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis_vertproj_config(const struct ia_css_isp_parameters *params,
+				struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_vertproj_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis_vertproj_config() leave\n");
+	ia_css_sdis_vertproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_vertproj_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis_vertproj_config() enter:\n");
+	ia_css_sdis_vertproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs_coefs = *config;
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis_vertproj_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis2_horicoef_config(const struct ia_css_isp_parameters *params,
+				 struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_horicoef_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs2_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_horicoef_config() leave\n");
+	ia_css_sdis2_horicoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_horicoef_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis2_horicoef_config() enter:\n");
+	ia_css_sdis2_horicoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs2_coefs = *config;
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis2_horicoef_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis2_vertcoef_config(const struct ia_css_isp_parameters *params,
+				 struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_vertcoef_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs2_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_vertcoef_config() leave\n");
+	ia_css_sdis2_vertcoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_vertcoef_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis2_vertcoef_config() enter:\n");
+	ia_css_sdis2_vertcoef_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs2_coefs = *config;
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis2_vertcoef_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis2_horiproj_config(const struct ia_css_isp_parameters *params,
+				 struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_horiproj_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs2_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_horiproj_config() leave\n");
+	ia_css_sdis2_horiproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_horiproj_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis2_horiproj_config() enter:\n");
+	ia_css_sdis2_horiproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs2_coefs = *config;
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis2_horiproj_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_sdis2_vertproj_config(const struct ia_css_isp_parameters *params,
+				 struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_vertproj_config() enter: config=%p\n",
+			    config);
+
+	*config = params->dvs2_coefs;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_sdis2_vertproj_config() leave\n");
+	ia_css_sdis2_vertproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_vertproj_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_sdis2_vertproj_config() enter:\n");
+	ia_css_sdis2_vertproj_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->dvs2_coefs = *config;
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_sdis2_vertproj_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_r_gamma_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_r_gamma_config() enter: config=%p\n",
+			    config);
+
+	*config = params->r_gamma_table;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_r_gamma_config() leave\n");
+	ia_css_r_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_r_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_r_gamma_config() enter:\n");
+	ia_css_r_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->r_gamma_table = *config;
+	params->config_changed[IA_CSS_R_GAMMA_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_r_gamma_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_g_gamma_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_g_gamma_config() enter: config=%p\n",
+			    config);
+
+	*config = params->g_gamma_table;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_g_gamma_config() leave\n");
+	ia_css_g_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_g_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_g_gamma_config() enter:\n");
+	ia_css_g_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->g_gamma_table = *config;
+	params->config_changed[IA_CSS_G_GAMMA_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_g_gamma_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_b_gamma_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_b_gamma_config() enter: config=%p\n",
+			    config);
+
+	*config = params->b_gamma_table;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_b_gamma_config() leave\n");
+	ia_css_b_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_b_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_b_gamma_config() enter:\n");
+	ia_css_b_gamma_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->b_gamma_table = *config;
+	params->config_changed[IA_CSS_B_GAMMA_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_b_gamma_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_xnr_table_config(const struct ia_css_isp_parameters *params,
+			    struct ia_css_xnr_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr_table_config() enter: config=%p\n",
+			    config);
+
+	*config = params->xnr_table;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr_table_config() leave\n");
+	ia_css_xnr_table_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr_table_config(struct ia_css_isp_parameters *params,
+			    const struct ia_css_xnr_table *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_xnr_table_config() enter:\n");
+	ia_css_xnr_table_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->xnr_table = *config;
+	params->config_changed[IA_CSS_XNR_TABLE_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_xnr_table_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_formats_config(const struct ia_css_isp_parameters *params,
+			  struct ia_css_formats_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_formats_config() enter: config=%p\n",
+			    config);
+
+	*config = params->formats_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_formats_config() leave\n");
+	ia_css_formats_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_formats_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_formats_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_formats_config() enter:\n");
+	ia_css_formats_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->formats_config = *config;
+	params->config_changed[IA_CSS_FORMATS_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_formats_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_xnr_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_xnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr_config() enter: config=%p\n",
+			    config);
+
+	*config = params->xnr_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr_config() leave\n");
+	ia_css_xnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_xnr_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_xnr_config() enter:\n");
+	ia_css_xnr_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->xnr_config = *config;
+	params->config_changed[IA_CSS_XNR_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_xnr_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_xnr3_config(const struct ia_css_isp_parameters *params,
+		       struct ia_css_xnr3_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr3_config() enter: config=%p\n",
+			    config);
+
+	*config = params->xnr3_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_xnr3_config() leave\n");
+	ia_css_xnr3_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr3_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_xnr3_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_xnr3_config() enter:\n");
+	ia_css_xnr3_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->xnr3_config = *config;
+	params->config_changed[IA_CSS_XNR3_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_xnr3_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_s3a_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_3a_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_s3a_config() enter: config=%p\n",
+			    config);
+
+	*config = params->s3a_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_s3a_config() leave\n");
+	ia_css_s3a_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_s3a_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_3a_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_s3a_config() enter:\n");
+	ia_css_s3a_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->s3a_config = *config;
+	params->config_changed[IA_CSS_BH_ID] = true;
+	params->config_changed[IA_CSS_S3A_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_s3a_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_get_function() */
+
+static void
+ia_css_get_output_config(const struct ia_css_isp_parameters *params,
+			 struct ia_css_output_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_output_config() enter: config=%p\n",
+			    config);
+
+	*config = params->output_config;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_get_output_config() leave\n");
+	ia_css_output_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+}
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_output_config(struct ia_css_isp_parameters *params,
+			 const struct ia_css_output_config *config)
+{
+	if (!config)
+		return;
+
+	assert(params);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_set_output_config() enter:\n");
+	ia_css_output_debug_dtrace(config, IA_CSS_DEBUG_TRACE);
+	params->output_config = *config;
+	params->config_changed[IA_CSS_OUTPUT_ID] = true;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_set_output_config() leave: return_void\n");
+}
+
+/* Code generated by genparam/gencode.c:gen_global_access_function() */
+
+void
+ia_css_get_configs(struct ia_css_isp_parameters *params,
+		   const struct ia_css_isp_config *config)
+{
+	ia_css_get_dp_config(params, config->dp_config);
+	ia_css_get_wb_config(params, config->wb_config);
+	ia_css_get_tnr_config(params, config->tnr_config);
+	ia_css_get_ob_config(params, config->ob_config);
+	ia_css_get_de_config(params, config->de_config);
+	ia_css_get_anr_config(params, config->anr_config);
+	ia_css_get_anr2_config(params, config->anr_thres);
+	ia_css_get_ce_config(params, config->ce_config);
+	ia_css_get_ecd_config(params, config->ecd_config);
+	ia_css_get_ynr_config(params, config->ynr_config);
+	ia_css_get_fc_config(params, config->fc_config);
+	ia_css_get_cnr_config(params, config->cnr_config);
+	ia_css_get_macc_config(params, config->macc_config);
+	ia_css_get_ctc_config(params, config->ctc_config);
+	ia_css_get_aa_config(params, config->aa_config);
+	ia_css_get_yuv2rgb_config(params, config->yuv2rgb_cc_config);
+	ia_css_get_rgb2yuv_config(params, config->rgb2yuv_cc_config);
+	ia_css_get_csc_config(params, config->cc_config);
+	ia_css_get_nr_config(params, config->nr_config);
+	ia_css_get_gc_config(params, config->gc_config);
+	ia_css_get_sdis_horicoef_config(params, config->dvs_coefs);
+	ia_css_get_sdis_vertcoef_config(params, config->dvs_coefs);
+	ia_css_get_sdis_horiproj_config(params, config->dvs_coefs);
+	ia_css_get_sdis_vertproj_config(params, config->dvs_coefs);
+	ia_css_get_sdis2_horicoef_config(params, config->dvs2_coefs);
+	ia_css_get_sdis2_vertcoef_config(params, config->dvs2_coefs);
+	ia_css_get_sdis2_horiproj_config(params, config->dvs2_coefs);
+	ia_css_get_sdis2_vertproj_config(params, config->dvs2_coefs);
+	ia_css_get_r_gamma_config(params, config->r_gamma_table);
+	ia_css_get_g_gamma_config(params, config->g_gamma_table);
+	ia_css_get_b_gamma_config(params, config->b_gamma_table);
+	ia_css_get_xnr_table_config(params, config->xnr_table);
+	ia_css_get_formats_config(params, config->formats_config);
+	ia_css_get_xnr_config(params, config->xnr_config);
+	ia_css_get_xnr3_config(params, config->xnr3_config);
+	ia_css_get_s3a_config(params, config->s3a_config);
+	ia_css_get_output_config(params, config->output_config);
+}
+
+/* Code generated by genparam/gencode.c:gen_global_access_function() */
+
+void
+ia_css_set_configs(struct ia_css_isp_parameters *params,
+		   const struct ia_css_isp_config *config)
+{
+	ia_css_set_dp_config(params, config->dp_config);
+	ia_css_set_wb_config(params, config->wb_config);
+	ia_css_set_tnr_config(params, config->tnr_config);
+	ia_css_set_ob_config(params, config->ob_config);
+	ia_css_set_de_config(params, config->de_config);
+	ia_css_set_anr_config(params, config->anr_config);
+	ia_css_set_anr2_config(params, config->anr_thres);
+	ia_css_set_ce_config(params, config->ce_config);
+	ia_css_set_ecd_config(params, config->ecd_config);
+	ia_css_set_ynr_config(params, config->ynr_config);
+	ia_css_set_fc_config(params, config->fc_config);
+	ia_css_set_cnr_config(params, config->cnr_config);
+	ia_css_set_macc_config(params, config->macc_config);
+	ia_css_set_ctc_config(params, config->ctc_config);
+	ia_css_set_aa_config(params, config->aa_config);
+	ia_css_set_yuv2rgb_config(params, config->yuv2rgb_cc_config);
+	ia_css_set_rgb2yuv_config(params, config->rgb2yuv_cc_config);
+	ia_css_set_csc_config(params, config->cc_config);
+	ia_css_set_nr_config(params, config->nr_config);
+	ia_css_set_gc_config(params, config->gc_config);
+	ia_css_set_sdis_horicoef_config(params, config->dvs_coefs);
+	ia_css_set_sdis_vertcoef_config(params, config->dvs_coefs);
+	ia_css_set_sdis_horiproj_config(params, config->dvs_coefs);
+	ia_css_set_sdis_vertproj_config(params, config->dvs_coefs);
+	ia_css_set_sdis2_horicoef_config(params, config->dvs2_coefs);
+	ia_css_set_sdis2_vertcoef_config(params, config->dvs2_coefs);
+	ia_css_set_sdis2_horiproj_config(params, config->dvs2_coefs);
+	ia_css_set_sdis2_vertproj_config(params, config->dvs2_coefs);
+	ia_css_set_r_gamma_config(params, config->r_gamma_table);
+	ia_css_set_g_gamma_config(params, config->g_gamma_table);
+	ia_css_set_b_gamma_config(params, config->b_gamma_table);
+	ia_css_set_xnr_table_config(params, config->xnr_table);
+	ia_css_set_formats_config(params, config->formats_config);
+	ia_css_set_xnr_config(params, config->xnr_config);
+	ia_css_set_xnr3_config(params, config->xnr3_config);
+	ia_css_set_s3a_config(params, config->s3a_config);
+	ia_css_set_output_config(params, config->output_config);
+}
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_params.h b/drivers/staging/media/atomisp/pci/ia_css_isp_params.h
new file mode 100644
index 000000000..c2de68987
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_params.h
@@ -0,0 +1,392 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* Generated code: do not edit or commmit. */
+
+#ifndef _IA_CSS_ISP_PARAM_H
+#define _IA_CSS_ISP_PARAM_H
+
+/* Code generated by genparam/gencode.c:gen_param_enum() */
+
+enum ia_css_parameter_ids {
+	IA_CSS_AA_ID,
+	IA_CSS_ANR_ID,
+	IA_CSS_ANR2_ID,
+	IA_CSS_BH_ID,
+	IA_CSS_CNR_ID,
+	IA_CSS_CROP_ID,
+	IA_CSS_CSC_ID,
+	IA_CSS_DP_ID,
+	IA_CSS_BNR_ID,
+	IA_CSS_DE_ID,
+	IA_CSS_ECD_ID,
+	IA_CSS_FORMATS_ID,
+	IA_CSS_FPN_ID,
+	IA_CSS_GC_ID,
+	IA_CSS_CE_ID,
+	IA_CSS_YUV2RGB_ID,
+	IA_CSS_RGB2YUV_ID,
+	IA_CSS_R_GAMMA_ID,
+	IA_CSS_G_GAMMA_ID,
+	IA_CSS_B_GAMMA_ID,
+	IA_CSS_UDS_ID,
+	IA_CSS_RAA_ID,
+	IA_CSS_S3A_ID,
+	IA_CSS_OB_ID,
+	IA_CSS_OUTPUT_ID,
+	IA_CSS_SC_ID,
+	IA_CSS_BDS_ID,
+	IA_CSS_TNR_ID,
+	IA_CSS_MACC_ID,
+	IA_CSS_SDIS_HORICOEF_ID,
+	IA_CSS_SDIS_VERTCOEF_ID,
+	IA_CSS_SDIS_HORIPROJ_ID,
+	IA_CSS_SDIS_VERTPROJ_ID,
+	IA_CSS_SDIS2_HORICOEF_ID,
+	IA_CSS_SDIS2_VERTCOEF_ID,
+	IA_CSS_SDIS2_HORIPROJ_ID,
+	IA_CSS_SDIS2_VERTPROJ_ID,
+	IA_CSS_WB_ID,
+	IA_CSS_NR_ID,
+	IA_CSS_YEE_ID,
+	IA_CSS_YNR_ID,
+	IA_CSS_FC_ID,
+	IA_CSS_CTC_ID,
+	IA_CSS_XNR_TABLE_ID,
+	IA_CSS_XNR_ID,
+	IA_CSS_XNR3_ID,
+	IA_CSS_NUM_PARAMETER_IDS
+};
+
+/* Code generated by genparam/gencode.c:gen_param_offsets() */
+
+struct ia_css_memory_offsets {
+	struct {
+		struct ia_css_isp_parameter aa;
+		struct ia_css_isp_parameter anr;
+		struct ia_css_isp_parameter bh;
+		struct ia_css_isp_parameter cnr;
+		struct ia_css_isp_parameter crop;
+		struct ia_css_isp_parameter csc;
+		struct ia_css_isp_parameter dp;
+		struct ia_css_isp_parameter bnr;
+		struct ia_css_isp_parameter de;
+		struct ia_css_isp_parameter ecd;
+		struct ia_css_isp_parameter formats;
+		struct ia_css_isp_parameter fpn;
+		struct ia_css_isp_parameter gc;
+		struct ia_css_isp_parameter ce;
+		struct ia_css_isp_parameter yuv2rgb;
+		struct ia_css_isp_parameter rgb2yuv;
+		struct ia_css_isp_parameter uds;
+		struct ia_css_isp_parameter raa;
+		struct ia_css_isp_parameter s3a;
+		struct ia_css_isp_parameter ob;
+		struct ia_css_isp_parameter output;
+		struct ia_css_isp_parameter sc;
+		struct ia_css_isp_parameter bds;
+		struct ia_css_isp_parameter tnr;
+		struct ia_css_isp_parameter macc;
+		struct ia_css_isp_parameter sdis_horiproj;
+		struct ia_css_isp_parameter sdis_vertproj;
+		struct ia_css_isp_parameter sdis2_horiproj;
+		struct ia_css_isp_parameter sdis2_vertproj;
+		struct ia_css_isp_parameter wb;
+		struct ia_css_isp_parameter nr;
+		struct ia_css_isp_parameter yee;
+		struct ia_css_isp_parameter ynr;
+		struct ia_css_isp_parameter fc;
+		struct ia_css_isp_parameter ctc;
+		struct ia_css_isp_parameter xnr;
+		struct ia_css_isp_parameter xnr3;
+		struct ia_css_isp_parameter get;
+		struct ia_css_isp_parameter put;
+	} dmem;
+	struct {
+		struct ia_css_isp_parameter anr2;
+		struct ia_css_isp_parameter ob;
+		struct ia_css_isp_parameter sdis_horicoef;
+		struct ia_css_isp_parameter sdis_vertcoef;
+		struct ia_css_isp_parameter sdis2_horicoef;
+		struct ia_css_isp_parameter sdis2_vertcoef;
+	} vmem;
+	struct {
+		struct ia_css_isp_parameter bh;
+	} hmem0;
+	struct {
+		struct ia_css_isp_parameter gc;
+		struct ia_css_isp_parameter g_gamma;
+		struct ia_css_isp_parameter xnr_table;
+	} vamem1;
+	struct {
+		struct ia_css_isp_parameter r_gamma;
+		struct ia_css_isp_parameter ctc;
+	} vamem0;
+	struct {
+		struct ia_css_isp_parameter b_gamma;
+	} vamem2;
+};
+
+#if defined(IA_CSS_INCLUDE_PARAMETERS)
+
+#include "ia_css_stream.h"   /* struct ia_css_stream */
+#include "ia_css_binary.h"   /* struct ia_css_binary */
+/* Code generated by genparam/gencode.c:gen_param_process_table() */
+
+struct ia_css_pipeline_stage; /* forward declaration */
+
+extern void (*ia_css_kernel_process_param[IA_CSS_NUM_PARAMETER_IDS])(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_dp_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_dp_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_wb_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_wb_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_tnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_tnr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ob_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ob_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_de_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_de_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_anr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_anr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_anr2_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_anr_thres *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ce_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ce_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ecd_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ecd_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ynr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ynr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_fc_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_fc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_cnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_cnr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_macc_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_macc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_ctc_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_ctc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_aa_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_aa_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_yuv2rgb_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_cc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_rgb2yuv_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_cc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_csc_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_cc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_nr_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_nr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_gc_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_gc_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_horicoef_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_vertcoef_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_horiproj_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis_vertproj_config(struct ia_css_isp_parameters *params,
+				const struct ia_css_dvs_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_horicoef_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_vertcoef_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_horiproj_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_sdis2_vertproj_config(struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs2_coefficients *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_r_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_g_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_b_gamma_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_rgb_gamma_table *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr_table_config(struct ia_css_isp_parameters *params,
+			    const struct ia_css_xnr_table *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_formats_config(struct ia_css_isp_parameters *params,
+			  const struct ia_css_formats_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_xnr_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_xnr3_config(struct ia_css_isp_parameters *params,
+		       const struct ia_css_xnr3_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_s3a_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_3a_config *config);
+
+/* Code generated by genparam/gencode.c:gen_set_function() */
+
+void
+ia_css_set_output_config(struct ia_css_isp_parameters *params,
+			 const struct ia_css_output_config *config);
+
+/* Code generated by genparam/gencode.c:gen_global_access_function() */
+
+void
+ia_css_get_configs(struct ia_css_isp_parameters *params,
+		   const struct ia_css_isp_config *config)
+;
+
+/* Code generated by genparam/gencode.c:gen_global_access_function() */
+
+void
+ia_css_set_configs(struct ia_css_isp_parameters *params,
+		   const struct ia_css_isp_config *config)
+;
+
+#endif /* IA_CSS_INCLUDE_PARAMETER */
+#endif /* _IA_CSS_ISP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_states.c b/drivers/staging/media/atomisp/pci/ia_css_isp_states.c
new file mode 100644
index 000000000..a6bc2e9ed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_states.c
@@ -0,0 +1,224 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+/* Generated code: do not edit or commmit. */
+
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_states.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_aa_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_aa_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.aa.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.aa.offset;
+
+		if (size)
+			memset(&binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			       0, size);
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_aa_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_cnr_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_cnr_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.cnr.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.cnr.offset;
+
+		if (size) {
+			ia_css_init_cnr_state(
+			    &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			    size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_cnr_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_cnr2_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_cnr2_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.cnr2.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.cnr2.offset;
+
+		if (size) {
+			ia_css_init_cnr2_state(
+			    &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			    size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_cnr2_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_dp_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_dp_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.dp.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.dp.offset;
+
+		if (size) {
+			ia_css_init_dp_state(
+			    &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			    size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_dp_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_de_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_de_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.de.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.de.offset;
+
+		if (size) {
+			ia_css_init_de_state(
+			    &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			    size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_de_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_tnr_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_tnr_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->dmem.tnr.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->dmem.tnr.offset;
+
+		if (size) {
+			ia_css_init_tnr_state((struct sh_css_isp_tnr_dmem_state *)
+					      &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_DMEM].address[offset],
+					      size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_tnr_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_ref_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_ref_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->dmem.ref.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->dmem.ref.offset;
+
+		if (size) {
+			ia_css_init_ref_state((struct sh_css_isp_ref_dmem_state *)
+					      &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_DMEM].address[offset],
+					      size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_ref_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_init_function() */
+
+static void
+ia_css_initialize_ynr_state(
+    const struct ia_css_binary *binary)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_ynr_state() enter:\n");
+
+	{
+		unsigned int size   = binary->info->mem_offsets.offsets.state->vmem.ynr.size;
+
+		unsigned int offset = binary->info->mem_offsets.offsets.state->vmem.ynr.offset;
+
+		if (size) {
+			ia_css_init_ynr_state(
+			    &binary->mem_params.params[IA_CSS_PARAM_CLASS_STATE][IA_CSS_ISP_VMEM].address[offset],
+			    size);
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_initialize_ynr_state() leave:\n");
+}
+
+/* Code generated by genparam/genstate.c:gen_state_init_table() */
+
+void (*ia_css_kernel_init_state[IA_CSS_NUM_STATE_IDS])(
+    const struct ia_css_binary *binary) = {
+	ia_css_initialize_aa_state,
+	ia_css_initialize_cnr_state,
+	ia_css_initialize_cnr2_state,
+	ia_css_initialize_dp_state,
+	ia_css_initialize_de_state,
+	ia_css_initialize_tnr_state,
+	ia_css_initialize_ref_state,
+	ia_css_initialize_ynr_state,
+};
diff --git a/drivers/staging/media/atomisp/pci/ia_css_isp_states.h b/drivers/staging/media/atomisp/pci/ia_css_isp_states.h
new file mode 100644
index 000000000..9f6c342a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_isp_states.h
@@ -0,0 +1,74 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#define IA_CSS_INCLUDE_STATES
+#include "isp/kernels/aa/aa_2/ia_css_aa2.host.h"
+#include "isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h"
+#include "isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h"
+#include "isp/kernels/de/de_1.0/ia_css_de.host.h"
+#include "isp/kernels/dp/dp_1.0/ia_css_dp.host.h"
+#include "isp/kernels/ref/ref_1.0/ia_css_ref.host.h"
+#include "isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h"
+#include "isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h"
+#include "isp/kernels/dpc2/ia_css_dpc2.host.h"
+#include "isp/kernels/eed1_8/ia_css_eed1_8.host.h"
+/* Generated code: do not edit or commmit. */
+
+#ifndef _IA_CSS_ISP_STATE_H
+#define _IA_CSS_ISP_STATE_H
+
+/* Code generated by genparam/gencode.c:gen_param_enum() */
+
+enum ia_css_state_ids {
+	IA_CSS_AA_STATE_ID,
+	IA_CSS_CNR_STATE_ID,
+	IA_CSS_CNR2_STATE_ID,
+	IA_CSS_DP_STATE_ID,
+	IA_CSS_DE_STATE_ID,
+	IA_CSS_TNR_STATE_ID,
+	IA_CSS_REF_STATE_ID,
+	IA_CSS_YNR_STATE_ID,
+	IA_CSS_NUM_STATE_IDS
+};
+
+/* Code generated by genparam/gencode.c:gen_param_offsets() */
+
+struct ia_css_state_memory_offsets {
+	struct {
+		struct ia_css_isp_parameter aa;
+		struct ia_css_isp_parameter cnr;
+		struct ia_css_isp_parameter cnr2;
+		struct ia_css_isp_parameter dp;
+		struct ia_css_isp_parameter de;
+		struct ia_css_isp_parameter ynr;
+	} vmem;
+	struct {
+		struct ia_css_isp_parameter tnr;
+		struct ia_css_isp_parameter ref;
+	} dmem;
+};
+
+#if defined(IA_CSS_INCLUDE_STATES)
+
+#include "ia_css_stream.h"   /* struct ia_css_stream */
+#include "ia_css_binary.h"   /* struct ia_css_binary */
+/* Code generated by genparam/genstate.c:gen_state_init_table() */
+
+extern void (*ia_css_kernel_init_state[IA_CSS_NUM_STATE_IDS])(
+    const struct ia_css_binary *binary);
+
+#endif /* IA_CSS_INCLUDE_STATE */
+
+#endif /* _IA_CSS_ISP_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_metadata.h b/drivers/staging/media/atomisp/pci/ia_css_metadata.h
new file mode 100644
index 000000000..9eb1b76a3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_metadata.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_METADATA_H
+#define __IA_CSS_METADATA_H
+
+/* @file
+ * This file contains structure for processing sensor metadata.
+ */
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_stream_format.h"
+
+/* Metadata configuration. This data structure contains necessary info
+ *  to process sensor metadata.
+ */
+struct ia_css_metadata_config {
+	enum atomisp_input_format data_type; /** Data type of CSI-2 embedded
+			data. The default value is ATOMISP_INPUT_FORMAT_EMBEDDED. For
+			certain sensors, user can choose non-default data type for embedded
+			data. */
+	struct ia_css_resolution  resolution; /** Resolution */
+};
+
+struct ia_css_metadata_info {
+	struct ia_css_resolution resolution; /** Resolution */
+	u32                 stride;     /** Stride in bytes */
+	u32                 size;       /** Total size in bytes */
+};
+
+struct ia_css_metadata {
+	struct ia_css_metadata_info info;    /** Layout info */
+	ia_css_ptr		    address; /** CSS virtual address */
+	u32		    exp_id;
+	/** Exposure ID, see ia_css_event_public.h for more detail */
+};
+
+#define SIZE_OF_IA_CSS_METADATA_STRUCT sizeof(struct ia_css_metadata)
+
+/* @brief Allocate a metadata buffer.
+ * @param[in]   metadata_info Metadata info struct, contains details on metadata buffers.
+ * @return      Pointer of metadata buffer or NULL (if error)
+ *
+ * This function allocates a metadata buffer according to the properties
+ * specified in the metadata_info struct.
+ */
+struct ia_css_metadata *
+ia_css_metadata_allocate(const struct ia_css_metadata_info *metadata_info);
+
+/* @brief Free a metadata buffer.
+ *
+ * @param[in]	metadata	Pointer of metadata buffer.
+ * @return	None
+ *
+ * This function frees a metadata buffer.
+ */
+void
+ia_css_metadata_free(struct ia_css_metadata *metadata);
+
+#endif /* __IA_CSS_METADATA_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_mipi.h b/drivers/staging/media/atomisp/pci/ia_css_mipi.h
new file mode 100644
index 000000000..9e50e1c61
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_mipi.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MIPI_H
+#define __IA_CSS_MIPI_H
+
+/* @file
+ * This file contains MIPI support functionality
+ */
+
+#include <type_support.h>
+#include "ia_css_err.h"
+#include "ia_css_stream_format.h"
+#include "ia_css_input_port.h"
+
+/* @brief Register size of a CSS MIPI frame for check during capturing.
+ *
+ * @param[in]	port	CSI-2 port this check is registered.
+ * @param[in]	size_mem_words	The frame size in memory words (32B).
+ * @return		Return the error in case of failure. E.g. MAX_NOF_ENTRIES REACHED
+ *
+ * Register size of a CSS MIPI frame to check during capturing. Up to
+ *		IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES entries per port allowed. Entries are reset
+ *		when stream is stopped.
+ *
+ *
+ */
+int
+ia_css_mipi_frame_enable_check_on_size(const enum mipi_port_id port,
+				       const unsigned int	size_mem_words);
+
+/* @brief Calculate the size of a mipi frame.
+ *
+ * @param[in]	width		The width (in pixels) of the frame.
+ * @param[in]	height		The height (in lines) of the frame.
+ * @param[in]	format		The frame (MIPI) format.
+ * @param[in]	hasSOLandEOL	Whether frame (MIPI) contains (optional) SOL and EOF packets.
+ * @param[in]	embedded_data_size_words		Embedded data size in memory words.
+ * @param		size_mem_words					The mipi frame size in memory words (32B).
+ * @return		The error code.
+ *
+ * Calculate the size of a mipi frame, based on the resolution and format.
+ */
+int
+ia_css_mipi_frame_calculate_size(const unsigned int width,
+				 const unsigned int height,
+				 const enum atomisp_input_format format,
+				 const bool hasSOLandEOL,
+				 const unsigned int embedded_data_size_words,
+				 unsigned int *size_mem_words);
+
+#endif /* __IA_CSS_MIPI_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_mmu.h b/drivers/staging/media/atomisp/pci/ia_css_mmu.h
new file mode 100644
index 000000000..8f04d196c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_mmu.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MMU_H
+#define __IA_CSS_MMU_H
+
+/* @file
+ * This file contains one support function for invalidating the CSS MMU cache
+ */
+
+/* @brief Invalidate the MMU internal cache.
+ * @return	None
+ *
+ * This function triggers an invalidation of the translate-look-aside
+ * buffer (TLB) that's inside the CSS MMU. This function should be called
+ * every time the page tables used by the MMU change.
+ */
+void
+ia_css_mmu_invalidate_cache(void);
+
+#endif /* __IA_CSS_MMU_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_mmu_private.h b/drivers/staging/media/atomisp/pci/ia_css_mmu_private.h
new file mode 100644
index 000000000..dc6542aa6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_mmu_private.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MMU_PRIVATE_H
+#define __IA_CSS_MMU_PRIVATE_H
+
+#include "system_local.h"
+
+/*
+ * This function sets the L1 pagetable address.
+ * After power-up of the ISP the L1 pagetable can be set.
+ * Once being set the L1 pagetable is protected against
+ * further modifications.
+ */
+void
+sh_css_mmu_set_page_table_base_index(hrt_data base_index);
+
+#endif /* __IA_CSS_MMU_PRIVATE_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_morph.h b/drivers/staging/media/atomisp/pci/ia_css_morph.h
new file mode 100644
index 000000000..9c4b41b94
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_morph.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MORPH_H
+#define __IA_CSS_MORPH_H
+
+/* @file
+ * This file contains supporting for morphing table
+ */
+
+#include <ia_css_types.h>
+
+/* @brief Morphing table
+ * @param[in]	width Width of the morphing table.
+ * @param[in]	height Height of the morphing table.
+ * @return		Pointer to the morphing table
+*/
+struct ia_css_morph_table *
+ia_css_morph_table_allocate(unsigned int width, unsigned int height);
+
+/* @brief Free the morph table
+ * @param[in]	me Pointer to the morph table.
+ * @return		None
+*/
+void
+ia_css_morph_table_free(struct ia_css_morph_table *me);
+
+#endif /* __IA_CSS_MORPH_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_pipe.h b/drivers/staging/media/atomisp/pci/ia_css_pipe.h
new file mode 100644
index 000000000..fb58535bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_pipe.h
@@ -0,0 +1,185 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPE_H__
+#define __IA_CSS_PIPE_H__
+
+#include <type_support.h>
+#include "ia_css_stream.h"
+#include "ia_css_frame.h"
+#include "ia_css_pipeline.h"
+#include "ia_css_binary.h"
+#include "sh_css_legacy.h"
+
+#define PIPE_ENTRY_EMPTY_TOKEN                (~0U)
+#define PIPE_ENTRY_RESERVED_TOKEN             (0x1)
+
+struct ia_css_preview_settings {
+	struct ia_css_binary copy_binary;
+	struct ia_css_binary preview_binary;
+	struct ia_css_binary vf_pp_binary;
+
+	/* 2401 only for these two - do we in fact use them for anything real */
+	struct ia_css_frame *delay_frames[MAX_NUM_VIDEO_DELAY_FRAMES];
+	struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];
+
+	struct ia_css_pipe *copy_pipe;
+	struct ia_css_pipe *capture_pipe;
+	struct ia_css_pipe *acc_pipe;
+};
+
+#define IA_CSS_DEFAULT_PREVIEW_SETTINGS { \
+	.copy_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.preview_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.vf_pp_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+}
+
+struct ia_css_capture_settings {
+	struct ia_css_binary copy_binary;
+	/* we extend primary binary to multiple stages because in ISP2.6.1
+	 * the computation load is too high to fit in one single binary. */
+	struct ia_css_binary primary_binary[MAX_NUM_PRIMARY_STAGES];
+	unsigned int num_primary_stage;
+	struct ia_css_binary pre_isp_binary;
+	struct ia_css_binary anr_gdc_binary;
+	struct ia_css_binary post_isp_binary;
+	struct ia_css_binary capture_pp_binary;
+	struct ia_css_binary vf_pp_binary;
+	struct ia_css_binary capture_ldc_binary;
+	struct ia_css_binary *yuv_scaler_binary;
+	struct ia_css_frame *delay_frames[MAX_NUM_VIDEO_DELAY_FRAMES];
+	bool *is_output_stage;
+	unsigned int num_yuv_scaler;
+};
+
+#define IA_CSS_DEFAULT_CAPTURE_SETTINGS { \
+	.copy_binary		= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.primary_binary		= {IA_CSS_BINARY_DEFAULT_SETTINGS}, \
+	.pre_isp_binary		= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.anr_gdc_binary		= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.post_isp_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.capture_pp_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.vf_pp_binary		= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.capture_ldc_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+}
+
+struct ia_css_video_settings {
+	struct ia_css_binary copy_binary;
+	struct ia_css_binary video_binary;
+	struct ia_css_binary vf_pp_binary;
+	struct ia_css_binary *yuv_scaler_binary;
+	struct ia_css_frame *delay_frames[MAX_NUM_VIDEO_DELAY_FRAMES];
+	struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];
+	struct ia_css_frame *vf_pp_in_frame;
+	struct ia_css_pipe *copy_pipe;
+	struct ia_css_pipe *capture_pipe;
+	bool *is_output_stage;
+	unsigned int num_yuv_scaler;
+};
+
+#define IA_CSS_DEFAULT_VIDEO_SETTINGS { \
+	.copy_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.video_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+	.vf_pp_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+}
+
+struct ia_css_yuvpp_settings {
+	struct ia_css_binary copy_binary;
+	struct ia_css_binary *yuv_scaler_binary;
+	struct ia_css_binary *vf_pp_binary;
+	bool *is_output_stage;
+	unsigned int num_yuv_scaler;
+	unsigned int num_vf_pp;
+	unsigned int num_output;
+};
+
+#define IA_CSS_DEFAULT_YUVPP_SETTINGS { \
+	.copy_binary	= IA_CSS_BINARY_DEFAULT_SETTINGS, \
+}
+
+struct osys_object;
+
+struct ia_css_pipe {
+	/* TODO: Remove stop_requested and use stop_requested in the pipeline */
+	bool                            stop_requested;
+	struct ia_css_pipe_config       config;
+	struct ia_css_pipe_extra_config extra_config;
+	struct ia_css_pipe_info         info;
+	enum ia_css_pipe_id		mode;
+	struct ia_css_shading_table	*shading_table;
+	struct ia_css_pipeline		pipeline;
+	struct ia_css_frame_info	output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame_info	bds_output_info;
+	struct ia_css_frame_info	vf_output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame_info	out_yuv_ds_input_info;
+	struct ia_css_frame_info	vf_yuv_ds_input_info;
+	struct ia_css_fw_info		*output_stage;	/* extra output stage */
+	struct ia_css_fw_info		*vf_stage;	/* extra vf_stage */
+	unsigned int			required_bds_factor;
+	unsigned int			dvs_frame_delay;
+	int				num_invalid_frames;
+	bool				enable_viewfinder[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_stream		*stream;
+	struct ia_css_frame		in_frame_struct;
+	struct ia_css_frame		out_frame_struct;
+	struct ia_css_frame		vf_frame_struct;
+	struct ia_css_frame		*continuous_frames[NUM_CONTINUOUS_FRAMES];
+	struct ia_css_metadata	*cont_md_buffers[NUM_CONTINUOUS_FRAMES];
+	union {
+		struct ia_css_preview_settings preview;
+		struct ia_css_video_settings   video;
+		struct ia_css_capture_settings capture;
+		struct ia_css_yuvpp_settings yuvpp;
+	} pipe_settings;
+	ia_css_ptr scaler_pp_lut;
+	struct osys_object *osys_obj;
+
+	/* This number is unique per pipe each instance of css. This number is
+	 * reused as pipeline number also. There is a 1-1 mapping between pipe_num
+	 * and sp thread id. Current logic limits pipe_num to
+	 * SH_CSS_MAX_SP_THREADS */
+	unsigned int pipe_num;
+};
+
+#define IA_CSS_DEFAULT_PIPE { \
+	.config			= DEFAULT_PIPE_CONFIG, \
+	.info			= DEFAULT_PIPE_INFO, \
+	.mode			= IA_CSS_PIPE_ID_ACC, /* (pipe_id) */ \
+	.pipeline		= DEFAULT_PIPELINE, \
+	.output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.bds_output_info	= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.vf_output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.out_yuv_ds_input_info	= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.vf_yuv_ds_input_info	= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.required_bds_factor	= SH_CSS_BDS_FACTOR_1_00, \
+	.dvs_frame_delay	= 1, \
+	.enable_viewfinder	= {true}, \
+	.in_frame_struct	= DEFAULT_FRAME, \
+	.out_frame_struct	= DEFAULT_FRAME, \
+	.vf_frame_struct	= DEFAULT_FRAME, \
+	.pipe_settings		= { \
+		.preview = IA_CSS_DEFAULT_PREVIEW_SETTINGS \
+	}, \
+	.pipe_num		= PIPE_ENTRY_EMPTY_TOKEN, \
+}
+
+void ia_css_pipe_map_queue(struct ia_css_pipe *pipe, bool map);
+
+int
+sh_css_param_update_isp_params(struct ia_css_pipe *curr_pipe,
+			       struct ia_css_isp_parameters *params,
+			       bool commit, struct ia_css_pipe *pipe);
+
+#endif /* __IA_CSS_PIPE_H__ */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_pipe_public.h b/drivers/staging/media/atomisp/pci/ia_css_pipe_public.h
new file mode 100644
index 000000000..7352cbf77
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_pipe_public.h
@@ -0,0 +1,542 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPE_PUBLIC_H
+#define __IA_CSS_PIPE_PUBLIC_H
+
+/* @file
+ * This file contains the public interface for CSS pipes.
+ */
+
+#include <type_support.h>
+#include <ia_css_err.h>
+#include <ia_css_types.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_buffer.h>
+/* ISP2401 */
+#include <ia_css_acc_types.h>
+
+enum {
+	IA_CSS_PIPE_OUTPUT_STAGE_0 = 0,
+	IA_CSS_PIPE_OUTPUT_STAGE_1,
+	IA_CSS_PIPE_MAX_OUTPUT_STAGE,
+};
+
+/* Enumeration of pipe modes. This mode can be used to create
+ *  an image pipe for this mode. These pipes can be combined
+ *  to configure and run streams on the ISP.
+ *
+ *  For example, one can create a preview and capture pipe to
+ *  create a continuous capture stream.
+ */
+enum ia_css_pipe_mode {
+	IA_CSS_PIPE_MODE_PREVIEW,	/** Preview pipe */
+	IA_CSS_PIPE_MODE_VIDEO,		/** Video pipe */
+	IA_CSS_PIPE_MODE_CAPTURE,	/** Still capture pipe */
+	IA_CSS_PIPE_MODE_ACC,		/** Accelerated pipe */
+	IA_CSS_PIPE_MODE_COPY,		/** Copy pipe, only used for embedded/image data copying */
+	IA_CSS_PIPE_MODE_YUVPP,		/** YUV post processing pipe, used for all use cases with YUV input,
+									for SoC sensor and external ISP */
+};
+
+/* Temporary define  */
+#define IA_CSS_PIPE_MODE_NUM (IA_CSS_PIPE_MODE_YUVPP + 1)
+
+/**
+ * Enumeration of pipe versions.
+ * the order should match with definition in sh_css_defs.h
+ */
+enum ia_css_pipe_version {
+	IA_CSS_PIPE_VERSION_1 = 1,		/** ISP1.0 pipe */
+	IA_CSS_PIPE_VERSION_2_2 = 2,		/** ISP2.2 pipe */
+	IA_CSS_PIPE_VERSION_2_6_1 = 3,		/** ISP2.6.1 pipe */
+	IA_CSS_PIPE_VERSION_2_7 = 4		/** ISP2.7 pipe */
+};
+
+/**
+ * Pipe configuration structure.
+ * Resolution properties are filled by Driver, kernel configurations are
+ * set by AIC
+ */
+struct ia_css_pipe_config {
+	enum ia_css_pipe_mode mode;
+	/** mode, indicates which mode the pipe should use. */
+	enum ia_css_pipe_version isp_pipe_version;
+	/** pipe version, indicates which imaging pipeline the pipe should use. */
+	struct ia_css_resolution input_effective_res;
+	/** input effective resolution */
+	struct ia_css_resolution bayer_ds_out_res;
+	/** bayer down scaling */
+	struct ia_css_resolution capt_pp_in_res;
+	/** capture post processing input resolution */
+	struct ia_css_resolution vf_pp_in_res;
+
+	/** ISP2401: view finder post processing input resolution */
+	struct ia_css_resolution output_system_in_res;
+	/** For IPU3 only: use output_system_in_res to specify what input resolution
+	     will OSYS receive, this resolution is equal to the output resolution of GDC
+	     if not determined CSS will set output_system_in_res with main osys output pin resolution
+	     All other IPUs may ignore this property */
+	struct ia_css_resolution dvs_crop_out_res;
+	/** dvs crop, video only, not in use yet. Use dvs_envelope below. */
+	struct ia_css_frame_info output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	/** output of YUV scaling */
+	struct ia_css_frame_info vf_output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	/** output of VF YUV scaling */
+	struct ia_css_fw_info *acc_extension;
+	/** Pipeline extension accelerator */
+	struct ia_css_fw_info **acc_stages;
+	/** Standalone accelerator stages */
+	u32 num_acc_stages;
+	/** Number of standalone accelerator stages */
+	struct ia_css_capture_config default_capture_config;
+	/** Default capture config for initial capture pipe configuration. */
+	struct ia_css_resolution dvs_envelope; /** temporary */
+	enum ia_css_frame_delay dvs_frame_delay;
+	/** indicates the DVS loop delay in frame periods */
+	int acc_num_execs;
+	/** For acceleration pipes only: determine how many times the pipe
+	     should be run. Setting this to -1 means it will run until
+	     stopped. */
+	bool enable_dz;
+	/** Disabling digital zoom for a pipeline, if this is set to false,
+	     then setting a zoom factor will have no effect.
+	     In some use cases this provides better performance. */
+	bool enable_dpc;
+	/** Disabling "Defect Pixel Correction" for a pipeline, if this is set
+	     to false. In some use cases this provides better performance. */
+	bool enable_vfpp_bci;
+	/** Enabling BCI mode will cause yuv_scale binary to be picked up
+	     instead of vf_pp. This only applies to viewfinder post
+	     processing stages. */
+
+/* ISP2401 */
+	bool enable_tnr;
+	/** Enabling of TNR (temporal noise reduction). This is only applicable to video
+	     pipes. Non video-pipes should always set this parameter to false. */
+
+	struct ia_css_isp_config *p_isp_config;
+	/** Pointer to ISP configuration */
+	struct ia_css_resolution gdc_in_buffer_res;
+	/** GDC in buffer resolution. */
+	struct ia_css_point gdc_in_buffer_offset;
+	/** GDC in buffer offset - indicates the pixel coordinates of the first valid pixel inside the buffer */
+
+/* ISP2401 */
+	struct ia_css_coordinate internal_frame_origin_bqs_on_sctbl;
+	/** Origin of internal frame positioned on shading table at shading correction in ISP.
+	     NOTE: Shading table is larger than or equal to internal frame.
+		   Shading table has shading gains and internal frame has bayer data.
+		   The origin of internal frame is used in shading correction in ISP
+		   to retrieve shading gains which correspond to bayer data. */
+};
+
+/**
+ * Default settings for newly created pipe configurations.
+ */
+#define DEFAULT_PIPE_CONFIG { \
+	.mode			= IA_CSS_PIPE_MODE_PREVIEW, \
+	.isp_pipe_version	= 1, \
+	.output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.vf_output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.default_capture_config	= DEFAULT_CAPTURE_CONFIG, \
+	.dvs_frame_delay	= IA_CSS_FRAME_DELAY_1, \
+	.acc_num_execs		= -1, \
+}
+
+/* Pipe info, this struct describes properties of a pipe after it's stream has
+ * been created.
+ * ~~~** DO NOT ADD NEW FIELD **~~~ This structure will be deprecated.
+ *           - On the Behalf of CSS-API Committee.
+ */
+struct ia_css_pipe_info {
+	struct ia_css_frame_info output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	/** Info about output resolution. This contains the stride which
+	     should be used for memory allocation. */
+	struct ia_css_frame_info vf_output_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	/** Info about viewfinder output resolution (optional). This contains
+	     the stride that should be used for memory allocation. */
+	struct ia_css_frame_info raw_output_info;
+	/** Raw output resolution. This indicates the resolution of the
+	     RAW bayer output for pipes that support this. Currently, only the
+	     still capture pipes support this feature. When this resolution is
+	     smaller than the input resolution, cropping will be performed by
+	     the ISP. The first cropping that will be performed is on the upper
+	     left corner where we crop 8 lines and 8 columns to remove the
+	     pixels normally used to initialize the ISP filters.
+	     This is why the raw output resolution should normally be set to
+	     the input resolution - 8x8. */
+	/* ISP2401 */
+	struct ia_css_resolution output_system_in_res_info;
+	/** For IPU3 only. Info about output system in resolution which is considered
+	     as gdc out resolution. */
+	struct ia_css_shading_info shading_info;
+	/** After an image pipe is created, this field will contain the info
+	     for the shading correction. */
+	struct ia_css_grid_info  grid_info;
+	/** After an image pipe is created, this field will contain the grid
+	     info for 3A and DVS. */
+	int num_invalid_frames;
+	/** The very first frames in a started stream do not contain valid data.
+	     In this field, the CSS-firmware communicates to the host-driver how
+	     many initial frames will contain invalid data; this allows the
+	     host-driver to discard those initial invalid frames and start it's
+	     output at the first valid frame. */
+};
+
+/**
+ * Defaults for ia_css_pipe_info structs.
+ */
+#define DEFAULT_PIPE_INFO {\
+	.output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.vf_output_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.raw_output_info	= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.shading_info		= DEFAULT_SHADING_INFO, \
+	.grid_info		= DEFAULT_GRID_INFO, \
+}
+
+/* @brief Load default pipe configuration
+ * @param[out]	pipe_config The pipe configuration.
+ * @return	None
+ *
+ * This function will load the default pipe configuration:
+@code
+	struct ia_css_pipe_config def_config = {
+		IA_CSS_PIPE_MODE_PREVIEW,  // mode
+		1,      // isp_pipe_version
+		{0, 0}, // bayer_ds_out_res
+		{0, 0}, // capt_pp_in_res
+		{0, 0}, // vf_pp_in_res
+		{0, 0}, // dvs_crop_out_res
+		{{0, 0}, 0, 0, 0, 0}, // output_info
+		{{0, 0}, 0, 0, 0, 0}, // second_output_info
+		{{0, 0}, 0, 0, 0, 0}, // vf_output_info
+		{{0, 0}, 0, 0, 0, 0}, // second_vf_output_info
+		NULL,   // acc_extension
+		NULL,   // acc_stages
+		0,      // num_acc_stages
+		{
+			IA_CSS_CAPTURE_MODE_RAW, // mode
+			false, // enable_xnr
+			false  // enable_raw_output
+		},      // default_capture_config
+		{0, 0}, // dvs_envelope
+		1,      // dvs_frame_delay
+		-1,     // acc_num_execs
+		true,   // enable_dz
+		NULL,   // p_isp_config
+	};
+@endcode
+ */
+void ia_css_pipe_config_defaults(struct ia_css_pipe_config *pipe_config);
+
+/* @brief Create a pipe
+ * @param[in]	config The pipe configuration.
+ * @param[out]	pipe The pipe.
+ * @return	0 or the error code.
+ *
+ * This function will create a pipe with the given
+ * configuration.
+ */
+int
+ia_css_pipe_create(const struct ia_css_pipe_config *config,
+		   struct ia_css_pipe **pipe);
+
+/* @brief Destroy a pipe
+ * @param[in]	pipe The pipe.
+ * @return	0 or the error code.
+ *
+ * This function will destroy a given pipe.
+ */
+int
+ia_css_pipe_destroy(struct ia_css_pipe *pipe);
+
+/* @brief Provides information about a pipe
+ * @param[in]	pipe The pipe.
+ * @param[out]	pipe_info The pipe information.
+ * @return	0 or -EINVAL.
+ *
+ * This function will provide information about a given pipe.
+ */
+int
+ia_css_pipe_get_info(const struct ia_css_pipe *pipe,
+		     struct ia_css_pipe_info *pipe_info);
+
+/* @brief Configure a pipe with filter coefficients.
+ * @param[in]	pipe	The pipe.
+ * @param[in]	config	The pointer to ISP configuration.
+ * @return		0 or error code upon error.
+ *
+ * This function configures the filter coefficients for an image
+ * pipe.
+ */
+int
+ia_css_pipe_set_isp_config(struct ia_css_pipe *pipe,
+			   struct ia_css_isp_config *config);
+
+/* @brief Controls when the Event generator raises an IRQ to the Host.
+ *
+ * @param[in]	pipe	The pipe.
+ * @param[in]	or_mask	Binary or of enum ia_css_event_irq_mask_type. Each pipe
+			related event that is part of this mask will directly
+			raise an IRQ to	the Host when the event occurs in the
+			CSS.
+ * @param[in]	and_mask Binary or of enum ia_css_event_irq_mask_type. An event
+			IRQ for the Host is only raised after all pipe related
+			events have occurred at least once for all the active
+			pipes. Events are remembered and don't need to occurred
+			at the same moment in time. There is no control over
+			the order of these events. Once an IRQ has been raised
+			all remembered events are reset.
+ * @return		0.
+ *
+ Controls when the Event generator in the CSS raises an IRQ to the Host.
+ The main purpose of this function is to reduce the amount of interrupts
+ between the CSS and the Host. This will help saving power as it wakes up the
+ Host less often. In case both or_mask and and_mask are
+ IA_CSS_EVENT_TYPE_NONE for all pipes, no event IRQ's will be raised. An
+ exception holds for IA_CSS_EVENT_TYPE_PORT_EOF, for this event an IRQ is always
+ raised.
+ Note that events are still queued and the Host can poll for them. The
+ or_mask and and_mask may be active at the same time\n
+ \n
+ Default values, for all pipe id's, after ia_css_init:\n
+ or_mask = IA_CSS_EVENT_TYPE_ALL\n
+ and_mask = IA_CSS_EVENT_TYPE_NONE\n
+ \n
+ Examples\n
+ \code
+ ia_css_pipe_set_irq_mask(h_pipe,
+ IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE |
+ IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE ,
+ IA_CSS_EVENT_TYPE_NONE);
+ \endcode
+ The event generator will only raise an interrupt to the Host when there are
+ 3A or DIS statistics available from the preview pipe. It will not generate
+ an interrupt for any other event of the preview pipe e.g when there is an
+ output frame available.
+
+ \code
+ ia_css_pipe_set_irq_mask(h_pipe_preview,
+	IA_CSS_EVENT_TYPE_NONE,
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE |
+	IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE );
+
+ ia_css_pipe_set_irq_mask(h_pipe_capture,
+	IA_CSS_EVENT_TYPE_NONE,
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE );
+ \endcode
+ The event generator will only raise an interrupt to the Host when there is
+ both a frame done and 3A event available from the preview pipe AND when there
+ is a frame done available from the capture pipe. Note that these events
+ may occur at different moments in time. Also the order of the events is not
+ relevant.
+
+ \code
+ ia_css_pipe_set_irq_mask(h_pipe_preview,
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE,
+	IA_CSS_EVENT_TYPE_ALL );
+
+ ia_css_pipe_set_irq_mask(h_pipe_capture,
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE,
+	IA_CSS_EVENT_TYPE_ALL );
+ \endcode
+ The event generator will only raise an interrupt to the Host when there is an
+ output frame from the preview pipe OR an output frame from the capture pipe.
+ All other events (3A, VF output, pipeline done) will not raise an interrupt
+ to the Host. These events are not lost but always stored in the event queue.
+ */
+int
+ia_css_pipe_set_irq_mask(struct ia_css_pipe *pipe,
+			 unsigned int or_mask,
+			 unsigned int and_mask);
+
+/* @brief Reads the current event IRQ mask from the CSS.
+ *
+ * @param[in]	pipe The pipe.
+ * @param[out]	or_mask	Current or_mask. The bits in this mask are a binary or
+		of enum ia_css_event_irq_mask_type. Pointer may be NULL.
+ * @param[out]	and_mask Current and_mask.The bits in this mask are a binary or
+		of enum ia_css_event_irq_mask_type. Pointer may be NULL.
+ * @return	0.
+ *
+ Reads the current event IRQ mask from the CSS. Reading returns the actual
+ values as used by the SP and not any mirrored values stored at the Host.\n
+\n
+Precondition:\n
+SP must be running.\n
+
+*/
+int
+ia_css_event_get_irq_mask(const struct ia_css_pipe *pipe,
+			  unsigned int *or_mask,
+			  unsigned int *and_mask);
+
+/* @brief Queue a buffer for an image pipe.
+ *
+ * @param[in] pipe	The pipe that will own the buffer.
+ * @param[in] buffer	Pointer to the buffer.
+ *			Note that the caller remains owner of the buffer
+ *			structure. Only the data pointer within it will
+ *			be passed into the internal queues.
+ * @return		IA_CSS_INTERNAL_ERROR in case of unexpected errors,
+ *			0 otherwise.
+ *
+ * This function adds a buffer (which has a certain buffer type) to the queue
+ * for this type. This queue is owned by the image pipe. After this function
+ * completes successfully, the buffer is now owned by the image pipe and should
+ * no longer be accessed by any other code until it gets dequeued. The image
+ * pipe will dequeue buffers from this queue, use them and return them to the
+ * host code via an interrupt. Buffers will be consumed in the same order they
+ * get queued, but may be returned to the host out of order.
+ */
+int
+ia_css_pipe_enqueue_buffer(struct ia_css_pipe *pipe,
+			   const struct ia_css_buffer *buffer);
+
+/* @brief Dequeue a buffer from an image pipe.
+ *
+ * @param[in]    pipe	 The pipeline that the buffer queue belongs to.
+ * @param[in,out] buffer The buffer is used to lookup the type which determines
+ *			 which internal queue to use.
+ *			 The resulting buffer pointer is written into the dta
+ *			 field.
+ * @return		 IA_CSS_ERR_NO_BUFFER if the queue is empty or
+ *			 0 otherwise.
+ *
+ * This function dequeues a buffer from a buffer queue. The queue is indicated
+ * by the buffer type argument. This function can be called after an interrupt
+ * has been generated that signalled that a new buffer was available and can
+ * be used in a polling-like situation where the NO_BUFFER return value is used
+ * to determine whether a buffer was available or not.
+ */
+int
+ia_css_pipe_dequeue_buffer(struct ia_css_pipe *pipe,
+			   struct ia_css_buffer *buffer);
+
+/* @brief  Set the state (Enable or Disable) of the Extension stage in the
+ *          given pipe.
+ * @param[in] pipe         Pipe handle.
+ * @param[in] fw_handle    Extension firmware Handle (ia_css_fw_info.handle)
+ * @param[in] enable       Enable Flag (1 to enable ; 0 to disable)
+ *
+ * @return
+ * 0			: Success
+ * -EINVAL		: Invalid Parameters
+ * -EBUSY	: Inactive QOS Pipe
+ *					(No active stream with this pipe)
+ *
+ * This function will request state change (enable or disable) for the Extension
+ * stage (firmware handle) in the given pipe.
+ *
+ * Note:
+ *	1. Extension can be enabled/disabled only on QOS Extensions
+ *	2. Extension can be enabled/disabled only with an active QOS Pipe
+ *	3. Initial(Default) state of QOS Extensions is Disabled
+ *	4. State change cannot be guaranteed immediately OR on frame boundary
+ *
+ */
+int
+ia_css_pipe_set_qos_ext_state(struct ia_css_pipe *pipe,
+			      u32 fw_handle,
+			      bool  enable);
+
+/* @brief  Get the state (Enable or Disable) of the Extension stage in the
+ *          given pipe.
+ * @param[in]  pipe        Pipe handle.
+ * @param[in]  fw_handle   Extension firmware Handle (ia_css_fw_info.handle)
+ * @param[out] *enable     Enable Flag
+ *
+ * @return
+ * 0			: Success
+ * -EINVAL		: Invalid Parameters
+ * -EBUSY	: Inactive QOS Pipe
+ *					(No active stream with this pipe)
+ *
+ * This function will query the state of the Extension stage (firmware handle)
+ * in the given Pipe.
+ *
+ * Note:
+ *	1. Extension state can be queried only on QOS Extensions
+ *	2. Extension can be enabled/disabled only with an active QOS Pipe
+ *	3. Initial(Default) state of QOS Extensions is Disabled.
+ *
+ */
+int
+ia_css_pipe_get_qos_ext_state(struct ia_css_pipe *pipe,
+			      u32 fw_handle,
+			      bool *enable);
+
+/* @brief Get selected configuration settings
+ * @param[in]	pipe	The pipe.
+ * @param[out]	config	Configuration settings.
+ * @return		None
+ */
+void
+ia_css_pipe_get_isp_config(struct ia_css_pipe *pipe,
+			   struct ia_css_isp_config *config);
+
+/* @brief Set the scaler lut on this pipe. A copy of lut is made in the inuit
+ *         address space. So the LUT can be freed by caller.
+ * @param[in]  pipe        Pipe handle.
+ * @param[in]  lut         Look up tabel
+ *
+ * @return
+ * 0			: Success
+ * -EINVAL		: Invalid Parameters
+ *
+ * Note:
+ * 1) Note that both GDC's are programmed with the same table.
+ * 2) Current implementation ignores the pipe and overrides the
+ *    global lut. This will be fixed in the future
+ * 3) This function must be called before stream start
+ *
+ */
+int
+ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
+			       const void *lut);
+/* @brief Checking of DVS statistics ability
+ * @param[in]	pipe_info	The pipe info.
+ * @return		true - has DVS statistics ability
+ *			false - otherwise
+ */
+bool ia_css_pipe_has_dvs_stats(struct ia_css_pipe_info *pipe_info);
+
+/* ISP2401 */
+/* @brief Override the frameformat set on the output pins.
+ * @param[in]  pipe        Pipe handle.
+ * @param[in]  output_pin  Pin index to set the format on
+ *                         0 - main output pin
+ *                         1 - display output pin
+ * @param[in]  format      Format to set
+ *
+ * @return
+ * 0		: Success
+ * -EINVAL	: Invalid Parameters
+ * -EINVAL	: Pipe misses binary info
+ *
+ * Note:
+ * 1) This is an optional function to override the formats set in the pipe.
+ * 2) Only overriding with IA_CSS_FRAME_FORMAT_NV12_TILEY is currently allowed.
+ * 3) This function is only to be used on pipes that use the output system.
+ * 4) If this function is used, it MUST be called after ia_css_pipe_create.
+ * 5) If this function is used, this function MUST be called before ia_css_stream_start.
+ */
+int
+ia_css_pipe_override_frame_format(struct ia_css_pipe *pipe,
+				  int output_pin,
+				  enum ia_css_frame_format format);
+
+#endif /* __IA_CSS_PIPE_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_prbs.h b/drivers/staging/media/atomisp/pci/ia_css_prbs.h
new file mode 100644
index 000000000..53bbf1dce
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_prbs.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PRBS_H
+#define __IA_CSS_PRBS_H
+
+/* @file
+ * This file contains support for Pseudo Random Bit Sequence (PRBS) inputs
+ */
+
+/* Enumerate the PRBS IDs.
+ */
+enum ia_css_prbs_id {
+	IA_CSS_PRBS_ID0,
+	IA_CSS_PRBS_ID1,
+	IA_CSS_PRBS_ID2
+};
+
+/**
+ * Maximum number of PRBS IDs.
+ *
+ * Make sure the value of this define gets changed to reflect the correct
+ * number of ia_css_prbs_id enum if you add/delete an item in the enum.
+ */
+#define N_CSS_PRBS_IDS (IA_CSS_PRBS_ID2 + 1)
+
+/**
+ * PRBS configuration structure.
+ *
+ * Seed the for the Pseudo Random Bit Sequence.
+ *
+ * @deprecated{This interface is deprecated, it is not portable -> move to input system API}
+ */
+struct ia_css_prbs_config {
+	enum ia_css_prbs_id	id;
+	unsigned int		h_blank;	/** horizontal blank */
+	unsigned int		v_blank;	/** vertical blank */
+	int			seed;	/** random seed for the 1st 2-pixel-components/clock */
+	int			seed1;	/** random seed for the 2nd 2-pixel-components/clock */
+};
+
+#endif /* __IA_CSS_PRBS_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_properties.h b/drivers/staging/media/atomisp/pci/ia_css_properties.h
new file mode 100644
index 000000000..2cd014f7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_properties.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PROPERTIES_H
+#define __IA_CSS_PROPERTIES_H
+
+/* @file
+ * This file contains support for retrieving properties of some hardware the CSS system
+ */
+
+#include <type_support.h> /* bool */
+#include <ia_css_types.h> /* ia_css_vamem_type */
+
+struct ia_css_properties {
+	int  gdc_coord_one;
+	bool l1_base_is_index; /** Indicate whether the L1 page base
+				    is a page index or a byte address. */
+	enum ia_css_vamem_type vamem_type;
+};
+
+/* @brief Get hardware properties
+ * @param[in,out]	properties The hardware properties
+ * @return	None
+ *
+ * This function returns a number of hardware properties.
+ */
+void
+ia_css_get_properties(struct ia_css_properties *properties);
+
+#endif /* __IA_CSS_PROPERTIES_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_shading.h b/drivers/staging/media/atomisp/pci/ia_css_shading.h
new file mode 100644
index 000000000..de7ae5cab
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_shading.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SHADING_H
+#define __IA_CSS_SHADING_H
+
+/* @file
+ * This file contains support for setting the shading table for CSS
+ */
+
+#include <ia_css_types.h>
+
+/* @brief Shading table
+ * @param[in]	width Width of the shading table.
+ * @param[in]	height Height of the shading table.
+ * @return		Pointer to the shading table
+*/
+struct ia_css_shading_table *
+ia_css_shading_table_alloc(unsigned int width,
+			   unsigned int height);
+
+/* @brief Free shading table
+ * @param[in]	table Pointer to the shading table.
+ * @return		None
+*/
+void
+ia_css_shading_table_free(struct ia_css_shading_table *table);
+
+#endif /* __IA_CSS_SHADING_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_stream.h b/drivers/staging/media/atomisp/pci/ia_css_stream.h
new file mode 100644
index 000000000..cf847586d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_stream.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_STREAM_H_
+#define _IA_CSS_STREAM_H_
+
+#include <type_support.h>
+#include <system_local.h>
+#include <input_system.h>
+#include "ia_css_types.h"
+#include "ia_css_stream_public.h"
+
+/**
+ * structure to hold all internal stream related information
+ */
+struct ia_css_stream {
+	struct ia_css_stream_config    config;
+	struct ia_css_stream_info      info;
+	rx_cfg_t                       csi_rx_config;
+	bool                           reconfigure_css_rx;
+	struct ia_css_pipe            *last_pipe;
+	int                            num_pipes;
+	struct ia_css_pipe           **pipes;
+	struct ia_css_pipe            *continuous_pipe;
+	struct ia_css_isp_parameters  *isp_params_configs;
+	struct ia_css_isp_parameters  *per_frame_isp_params_configs;
+
+	bool                           cont_capt;
+	bool                           disable_cont_vf;
+
+	/* ISP2401 */
+	bool                           stop_copy_preview;
+	bool                           started;
+};
+
+/* @brief Get a binary in the stream, which binary has the shading correction.
+ *
+ * @param[in] stream: The stream.
+ * @return	The binary which has the shading correction.
+ *
+ */
+struct ia_css_binary *
+ia_css_stream_get_shading_correction_binary(const struct ia_css_stream *stream);
+
+struct ia_css_binary *
+ia_css_stream_get_dvs_binary(const struct ia_css_stream *stream);
+
+struct ia_css_binary *
+ia_css_stream_get_3a_binary(const struct ia_css_stream *stream);
+
+unsigned int
+ia_css_stream_input_format_bits_per_pixel(struct ia_css_stream *stream);
+
+bool
+sh_css_params_set_binning_factor(struct ia_css_stream *stream,
+				 unsigned int sensor_binning);
+
+void
+sh_css_invalidate_params(struct ia_css_stream *stream);
+
+/* The following functions are used for testing purposes only */
+const struct ia_css_fpn_table *
+ia_css_get_fpn_table(struct ia_css_stream *stream);
+
+/* @brief Get a pointer to the shading table.
+ *
+ * @param[in] stream: The stream.
+ * @return	The pointer to the shading table.
+ *
+ */
+struct ia_css_shading_table *
+ia_css_get_shading_table(struct ia_css_stream *stream);
+
+void
+ia_css_get_isp_dis_coefficients(struct ia_css_stream *stream,
+				short *horizontal_coefficients,
+				short *vertical_coefficients);
+
+void
+ia_css_get_isp_dvs2_coefficients(struct ia_css_stream *stream,
+				 short *hor_coefs_odd_real,
+				 short *hor_coefs_odd_imag,
+				 short *hor_coefs_even_real,
+				 short *hor_coefs_even_imag,
+				 short *ver_coefs_odd_real,
+				 short *ver_coefs_odd_imag,
+				 short *ver_coefs_even_real,
+				 short *ver_coefs_even_imag);
+
+int
+ia_css_stream_isp_parameters_init(struct ia_css_stream *stream);
+
+void
+ia_css_stream_isp_parameters_uninit(struct ia_css_stream *stream);
+
+#endif /*_IA_CSS_STREAM_H_*/
diff --git a/drivers/staging/media/atomisp/pci/ia_css_stream_format.h b/drivers/staging/media/atomisp/pci/ia_css_stream_format.h
new file mode 100644
index 000000000..aac22d858
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_stream_format.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_STREAM_FORMAT_H
+#define __IA_CSS_STREAM_FORMAT_H
+
+/* @file
+ * This file contains formats usable for ISP streaming input
+ */
+
+#include <type_support.h> /* bool */
+#include "../../../include/linux/atomisp_platform.h"
+
+unsigned int ia_css_util_input_format_bpp(
+    enum atomisp_input_format format,
+    bool two_ppc);
+
+#endif /* __ATOMISP_INPUT_FORMAT_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_stream_public.h b/drivers/staging/media/atomisp/pci/ia_css_stream_public.h
new file mode 100644
index 000000000..47846ece8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_stream_public.h
@@ -0,0 +1,575 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_STREAM_PUBLIC_H
+#define __IA_CSS_STREAM_PUBLIC_H
+
+/* @file
+ * This file contains support for configuring and controlling streams
+ */
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_metadata.h"
+#include "ia_css_tpg.h"
+#include "ia_css_prbs.h"
+#include "ia_css_input_port.h"
+
+/* Input modes, these enumerate all supported input modes.
+ *  Note that not all ISP modes support all input modes.
+ */
+enum ia_css_input_mode {
+	IA_CSS_INPUT_MODE_SENSOR, /** data from sensor */
+	IA_CSS_INPUT_MODE_FIFO,   /** data from input-fifo */
+	IA_CSS_INPUT_MODE_TPG,    /** data from test-pattern generator */
+	IA_CSS_INPUT_MODE_PRBS,   /** data from pseudo-random bit stream */
+	IA_CSS_INPUT_MODE_MEMORY, /** data from a frame in memory */
+	IA_CSS_INPUT_MODE_BUFFERED_SENSOR /** data is sent through mipi buffer */
+};
+
+/* Structure of the MIPI buffer configuration
+ */
+struct ia_css_mipi_buffer_config {
+	unsigned int size_mem_words; /** The frame size in the system memory
+					  words (32B) */
+	bool contiguous;	     /** Allocated memory physically
+					  contiguously or not. \deprecated{Will be false always.}*/
+	unsigned int nof_mipi_buffers; /** The number of MIPI buffers required for this
+					stream */
+};
+
+enum {
+	IA_CSS_STREAM_ISYS_STREAM_0 = 0,
+	IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX = IA_CSS_STREAM_ISYS_STREAM_0,
+	IA_CSS_STREAM_ISYS_STREAM_1,
+	IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH
+};
+
+/* This is input data configuration for one MIPI data type. We can have
+ *  multiple of this in one virtual channel.
+ */
+struct ia_css_stream_isys_stream_config {
+	struct ia_css_resolution  input_res; /** Resolution of input data */
+	enum atomisp_input_format format; /** Format of input stream. This data
+					       format will be mapped to MIPI data
+					       type internally. */
+	int linked_isys_stream_id; /** default value is -1, other value means
+							current isys_stream shares the same buffer with
+							indicated isys_stream*/
+	bool valid; /** indicate whether other fields have valid value */
+};
+
+struct ia_css_stream_input_config {
+	struct ia_css_resolution  input_res; /** Resolution of input data */
+	struct ia_css_resolution  effective_res; /** Resolution of input data.
+							Used for CSS 2400/1 System and deprecated for other
+							systems (replaced by input_effective_res in
+							ia_css_pipe_config) */
+	enum atomisp_input_format format; /** Format of input stream. This data
+					       format will be mapped to MIPI data
+					       type internally. */
+	enum ia_css_bayer_order bayer_order; /** Bayer order for RAW streams */
+};
+
+/* Input stream description. This describes how input will flow into the
+ *  CSS. This is used to program the CSS hardware.
+ */
+struct ia_css_stream_config {
+	enum ia_css_input_mode    mode; /** Input mode */
+	union {
+		struct ia_css_input_port  port; /** Port, for sensor only. */
+		struct ia_css_tpg_config  tpg;  /** TPG configuration */
+		struct ia_css_prbs_config prbs; /** PRBS configuration */
+	} source; /** Source of input data */
+	unsigned int	      channel_id; /** Channel on which input data
+						   will arrive. Use this field
+						   to specify virtual channel id.
+						   Valid values are: 0, 1, 2, 3 */
+	struct ia_css_stream_isys_stream_config
+		isys_config[IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH];
+	struct ia_css_stream_input_config input_config;
+
+	/*
+	 * Currently, Linux and Windows platforms interpret the binning_factor
+	 * parameter differently. In Linux, the binning factor is expressed
+	 * in the form 2^N * 2^N
+	 */
+	/* ISP2401 */
+	unsigned int sensor_binning_factor; /** Binning factor used by sensor
+						 to produce image data. This is
+						 used for shading correction. */
+	unsigned int pixels_per_clock; /** Number of pixels per clock, which can be
+					    1, 2 or 4. */
+	bool online; /** offline will activate RAW copy on SP, use this for
+			  continuous capture. */
+	/* ISYS2401 usage: ISP receives data directly from sensor, no copy. */
+	unsigned int init_num_cont_raw_buf; /** initial number of raw buffers to
+					     allocate */
+	unsigned int target_num_cont_raw_buf; /** total number of raw buffers to
+					     allocate */
+	bool pack_raw_pixels; /** Pack pixels in the raw buffers */
+	bool continuous; /** Use SP copy feature to continuously capture frames
+			      to system memory and run pipes in offline mode */
+	bool disable_cont_viewfinder; /** disable continuous viewfinder for ZSL use case */
+	s32 flash_gpio_pin; /** pin on which the flash is connected, -1 for no flash */
+	int left_padding; /** The number of input-formatter left-paddings, -1 for default from binary.*/
+	struct ia_css_mipi_buffer_config
+		mipi_buffer_config; /** mipi buffer configuration */
+	struct ia_css_metadata_config
+		metadata_config;     /** Metadata configuration. */
+	bool ia_css_enable_raw_buffer_locking; /** Enable Raw Buffer Locking for HALv3 Support */
+	bool lock_all;
+	/** Lock all RAW buffers (true) or lock only buffers processed by
+	     video or preview pipe (false).
+	     This setting needs to be enabled to allow raw buffer locking
+	     without continuous viewfinder. */
+};
+
+struct ia_css_stream;
+
+/* Stream info, this struct describes properties of a stream after it has been
+ *  created.
+ */
+struct ia_css_stream_info {
+	struct ia_css_metadata_info metadata_info;
+	/** Info about the metadata layout, this contains the stride. */
+};
+
+/* @brief Load default stream configuration
+ * @param[in,out]	stream_config The stream configuration.
+ * @return	None
+ *
+ * This function will reset the stream configuration to the default state:
+@code
+	memset(stream_config, 0, sizeof(*stream_config));
+	stream_config->online = true;
+	stream_config->left_padding = -1;
+@endcode
+ */
+void ia_css_stream_config_defaults(struct ia_css_stream_config *stream_config);
+
+/*
+ * create the internal structures and fill in the configuration data and pipes
+ */
+
+/* @brief Creates a stream
+* @param[in]	stream_config The stream configuration.
+* @param[in]	num_pipes The number of pipes to incorporate in the stream.
+* @param[in]	pipes The pipes.
+* @param[out]	stream The stream.
+* @return	0 or the error code.
+*
+* This function will create a stream with a given configuration and given pipes.
+*/
+int
+ia_css_stream_create(const struct ia_css_stream_config *stream_config,
+		     int num_pipes,
+		     struct ia_css_pipe *pipes[],
+		     struct ia_css_stream **stream);
+
+/* @brief Destroys a stream
+ * @param[in]	stream The stream.
+ * @return	0 or the error code.
+ *
+ * This function will destroy a given stream.
+ */
+int
+ia_css_stream_destroy(struct ia_css_stream *stream);
+
+/* @brief Provides information about a stream
+ * @param[in]	stream The stream.
+ * @param[out]	stream_info The information about the stream.
+ * @return	0 or the error code.
+ *
+ * This function will destroy a given stream.
+ */
+int
+ia_css_stream_get_info(const struct ia_css_stream *stream,
+		       struct ia_css_stream_info *stream_info);
+
+
+/* @brief Starts the stream.
+ * @param[in]	stream The stream.
+ * @return 0 or the error code.
+ *
+ * The dynamic data in
+ * the buffers are not used and need to be queued with a separate call
+ * to ia_css_pipe_enqueue_buffer.
+ * NOTE: this function will only send start event to corresponding
+ * thread and will not start SP any more.
+ */
+int
+ia_css_stream_start(struct ia_css_stream *stream);
+
+/* @brief Stop the stream.
+ * @param[in]	stream The stream.
+ * @return	0 or the error code.
+ *
+ * NOTE: this function will send stop event to pipes belong to this
+ * stream but will not terminate threads.
+ */
+int
+ia_css_stream_stop(struct ia_css_stream *stream);
+
+/* @brief Check if a stream has stopped
+ * @param[in]	stream The stream.
+ * @return	boolean flag
+ *
+ * This function will check if the stream has stopped and return the correspondent boolean flag.
+ */
+bool
+ia_css_stream_has_stopped(struct ia_css_stream *stream);
+
+/* @brief	destroy a stream according to the stream seed previosly saved in the seed array.
+ * @param[in]	stream The stream.
+ * @return	0 (no other errors are generated now)
+ *
+ * Destroy the stream and all the pipes related to it.
+ */
+int
+ia_css_stream_unload(struct ia_css_stream *stream);
+
+/* @brief Returns stream format
+ * @param[in]	stream The stream.
+ * @return	format of the string
+ *
+ * This function will return the stream format.
+ */
+enum atomisp_input_format
+ia_css_stream_get_format(const struct ia_css_stream *stream);
+
+/* @brief Check if the stream is configured for 2 pixels per clock
+ * @param[in]	stream The stream.
+ * @return	boolean flag
+ *
+ * This function will check if the stream is configured for 2 pixels per clock and
+ * return the correspondent boolean flag.
+ */
+bool
+ia_css_stream_get_two_pixels_per_clock(const struct ia_css_stream *stream);
+
+/* @brief Sets the output frame stride (at the last pipe)
+ * @param[in]	stream The stream
+ * @param[in]	output_padded_width - the output buffer stride.
+ * @return	ia_css_err
+ *
+ * This function will Set the output frame stride (at the last pipe)
+ */
+int
+ia_css_stream_set_output_padded_width(struct ia_css_stream *stream,
+				      unsigned int output_padded_width);
+
+/* @brief Return max number of continuous RAW frames.
+ * @param[in]	stream The stream.
+ * @param[out]	buffer_depth The maximum number of continuous RAW frames.
+ * @return	0 or -EINVAL
+ *
+ * This function will return the maximum number of continuous RAW frames
+ * the system can support.
+ */
+int
+ia_css_stream_get_max_buffer_depth(struct ia_css_stream *stream,
+				   int *buffer_depth);
+
+/* @brief Set nr of continuous RAW frames to use.
+ *
+ * @param[in]	stream The stream.
+ * @param[in]	buffer_depth	Number of frames to set.
+ * @return	0 or error code upon error.
+ *
+ * Set the number of continuous frames to use during continuous modes.
+ */
+int
+ia_css_stream_set_buffer_depth(struct ia_css_stream *stream, int buffer_depth);
+
+/* @brief Get number of continuous RAW frames to use.
+ * @param[in]	stream The stream.
+ * @param[out]	buffer_depth The number of frames to use
+ * @return	0 or -EINVAL
+ *
+ * Get the currently set number of continuous frames
+ * to use during continuous modes.
+ */
+int
+ia_css_stream_get_buffer_depth(struct ia_css_stream *stream, int *buffer_depth);
+
+/* ===== CAPTURE ===== */
+
+/* @brief Configure the continuous capture
+ *
+ * @param[in]	stream		The stream.
+ * @param[in]	num_captures	The number of RAW frames to be processed to
+ *				YUV. Setting this to -1 will make continuous
+ *				capture run until it is stopped.
+ *				This number will also be used to allocate RAW
+ *				buffers. To allow the viewfinder to also
+ *				keep operating, 2 extra buffers will always be
+ *				allocated.
+ *				If the offset is negative and the skip setting
+ *				is greater than 0, additional buffers may be
+ *				needed.
+ * @param[in]	skip		Skip N frames in between captures. This can be
+ *				used to select a slower capture frame rate than
+ *				the sensor output frame rate.
+ * @param[in]	offset		Start the RAW-to-YUV processing at RAW buffer
+ *				with this offset. This allows the user to
+ *				process RAW frames that were captured in the
+ *				past or future.
+ * @return			0 or error code upon error.
+ *
+ *  For example, to capture the current frame plus the 2 previous
+ *  frames and 2 subsequent frames, you would call
+ *  ia_css_stream_capture(5, 0, -2).
+ */
+int
+ia_css_stream_capture(struct ia_css_stream *stream,
+		      int num_captures,
+		      unsigned int skip,
+		      int offset);
+
+/* @brief Specify which raw frame to tag based on exp_id found in frame info
+ *
+ * @param[in]	stream The stream.
+ * @param[in]	exp_id	The exposure id of the raw frame to tag.
+ *
+ * @return			0 or error code upon error.
+ *
+ * This function allows the user to tag a raw frame based on the exposure id
+ * found in the viewfinder frames' frame info.
+ */
+int
+ia_css_stream_capture_frame(struct ia_css_stream *stream,
+			    unsigned int exp_id);
+
+/* ===== VIDEO ===== */
+
+/* @brief Send streaming data into the css input FIFO
+ *
+ * @param[in]	stream	The stream.
+ * @param[in]	data	Pointer to the pixels to be send.
+ * @param[in]	width	Width of the input frame.
+ * @param[in]	height	Height of the input frame.
+ * @return	None
+ *
+ * Send streaming data into the css input FIFO. This is for testing purposes
+ * only. This uses the channel ID and input format as set by the user with
+ * the regular functions for this.
+ * This function blocks until the entire frame has been written into the
+ * input FIFO.
+ *
+ * Note:
+ * For higher flexibility the ia_css_stream_send_input_frame is replaced by
+ * three separate functions:
+ * 1) ia_css_stream_start_input_frame
+ * 2) ia_css_stream_send_input_line
+ * 3) ia_css_stream_end_input_frame
+ * In this way it is possible to stream multiple frames on different
+ * channel ID's on a line basis. It will be possible to simulate
+ * line-interleaved Stereo 3D muxed on 1 mipi port.
+ * These 3 functions are for testing purpose only and can be used in
+ * conjunction with ia_css_stream_send_input_frame
+ */
+void
+ia_css_stream_send_input_frame(const struct ia_css_stream *stream,
+			       const unsigned short *data,
+			       unsigned int width,
+			       unsigned int height);
+
+/* @brief Start an input frame on the CSS input FIFO.
+ *
+ * @param[in]	stream The stream.
+ * @return	None
+ *
+ * Starts the streaming to mipi frame by sending SoF for channel channel_id.
+ * It will use the input_format and two_pixels_per_clock as provided by
+ * the user.
+ * For the "correct" use-case, input_format and two_pixels_per_clock must match
+ * with the values as set by the user with the regular functions.
+ * To simulate an error, the user can provide "incorrect" values for
+ * input_format and/or two_pixels_per_clock.
+ */
+void
+ia_css_stream_start_input_frame(const struct ia_css_stream *stream);
+
+/* @brief Send a line of input data into the CSS input FIFO.
+ *
+ * @param[in]	stream		The stream.
+ * @param[in]	data	Array of the first line of image data.
+ * @param	width	The width (in pixels) of the first line.
+ * @param[in]	data2	Array of the second line of image data.
+ * @param	width2	The width (in pixels) of the second line.
+ * @return	None
+ *
+ * Sends 1 frame line. Start with SoL followed by width bytes of data, followed
+ * by width2 bytes of data2 and followed by and EoL
+ * It will use the input_format and two_pixels_per_clock settings as provided
+ * with the ia_css_stream_start_input_frame function call.
+ *
+ * This function blocks until the entire line has been written into the
+ * input FIFO.
+ */
+void
+ia_css_stream_send_input_line(const struct ia_css_stream *stream,
+			      const unsigned short *data,
+			      unsigned int width,
+			      const unsigned short *data2,
+			      unsigned int width2);
+
+/* @brief Send a line of input embedded data into the CSS input FIFO.
+ *
+ * @param[in]	stream     Pointer of the stream.
+ * @param[in]	format     Format of the embedded data.
+ * @param[in]	data       Pointer of the embedded data line.
+ * @param[in]	width      The width (in pixels) of the line.
+ * @return		None
+ *
+ * Sends one embedded data line to input fifo. Start with SoL followed by
+ * width bytes of data, and followed by and EoL.
+ * It will use the two_pixels_per_clock settings as provided with the
+ * ia_css_stream_start_input_frame function call.
+ *
+ * This function blocks until the entire line has been written into the
+ * input FIFO.
+ */
+void
+ia_css_stream_send_input_embedded_line(const struct ia_css_stream *stream,
+				       enum atomisp_input_format format,
+				       const unsigned short *data,
+				       unsigned int width);
+
+/* @brief End an input frame on the CSS input FIFO.
+ *
+ * @param[in]	stream	The stream.
+ * @return	None
+ *
+ * Send the end-of-frame signal into the CSS input FIFO.
+ */
+void
+ia_css_stream_end_input_frame(const struct ia_css_stream *stream);
+
+/* @brief send a request flash command to SP
+ *
+ * @param[in]	stream The stream.
+ * @return	None
+ *
+ * Driver needs to call this function to send a flash request command
+ * to SP, SP will be responsible for switching on/off the flash at proper
+ * time. Due to the SP multi-threading environment, this request may have
+ * one-frame delay, the driver needs to check the flashed flag in frame info
+ * to determine which frame is being flashed.
+ */
+void
+ia_css_stream_request_flash(struct ia_css_stream *stream);
+
+/* @brief Configure a stream with filter coefficients.
+ *	   @deprecated {Replaced by
+ *				   ia_css_pipe_set_isp_config_on_pipe()}
+ *
+ * @param[in]	stream The stream.
+ * @param[in]	config	The set of filter coefficients.
+ * @param[in]   pipe Pipe to be updated when set isp config, NULL means to
+ *		   update all pipes in the stream.
+ * @return		0 or error code upon error.
+ *
+ * This function configures the filter coefficients for an image
+ * stream. For image pipes that do not execute any ISP filters, this
+ * function will have no effect.
+ * It is safe to call this function while the image stream is running,
+ * in fact this is the expected behavior most of the time. Proper
+ * resource locking and double buffering is in place to allow for this.
+ */
+int
+ia_css_stream_set_isp_config_on_pipe(struct ia_css_stream *stream,
+				     const struct ia_css_isp_config *config,
+				     struct ia_css_pipe *pipe);
+
+/* @brief Configure a stream with filter coefficients.
+ *	   @deprecated {Replaced by
+ *				   ia_css_pipe_set_isp_config()}
+ * @param[in]	stream	The stream.
+ * @param[in]	config	The set of filter coefficients.
+ * @return		0 or error code upon error.
+ *
+ * This function configures the filter coefficients for an image
+ * stream. For image pipes that do not execute any ISP filters, this
+ * function will have no effect. All pipes of a stream will be updated.
+ * See ::ia_css_stream_set_isp_config_on_pipe() for the per-pipe alternative.
+ * It is safe to call this function while the image stream is running,
+ * in fact this is the expected behaviour most of the time. Proper
+ * resource locking and double buffering is in place to allow for this.
+ */
+int
+ia_css_stream_set_isp_config(
+    struct ia_css_stream *stream,
+    const struct ia_css_isp_config *config);
+
+/* @brief Get selected configuration settings
+ * @param[in]	stream	The stream.
+ * @param[out]	config	Configuration settings.
+ * @return		None
+ */
+void
+ia_css_stream_get_isp_config(const struct ia_css_stream *stream,
+			     struct ia_css_isp_config *config);
+
+/* @brief allocate continuous raw frames for continuous capture
+ * @param[in]	stream The stream.
+ * @return 0 or error code.
+ *
+ *  because this allocation takes a long time (around 120ms per frame),
+ *  we separate the allocation part and update part to let driver call
+ *  this function without locking. This function is the allocation part
+ *  and next one is update part
+ */
+int
+ia_css_alloc_continuous_frame_remain(struct ia_css_stream *stream);
+
+/* @brief allocate continuous raw frames for continuous capture
+ * @param[in]	stream The stream.
+ * @return	0 or error code.
+ *
+ *  because this allocation takes a long time (around 120ms per frame),
+ *  we separate the allocation part and update part to let driver call
+ *  this function without locking. This function is the update part
+ */
+int
+ia_css_update_continuous_frames(struct ia_css_stream *stream);
+
+/* @brief ia_css_unlock_raw_frame . unlock a raw frame (HALv3 Support)
+ * @param[in]	stream The stream.
+ * @param[in]   exp_id exposure id that uniquely identifies the locked Raw Frame Buffer
+ * @return      ia_css_err 0 or error code
+ *
+ * As part of HALv3 Feature requirement, SP locks raw buffer until the Application
+ * releases its reference to a raw buffer (which are managed by SP), this function allows
+ * application to explicitly unlock that buffer in SP.
+ */
+int
+ia_css_unlock_raw_frame(struct ia_css_stream *stream, uint32_t exp_id);
+
+/* @brief ia_css_en_dz_capt_pipe . Enable/Disable digital zoom for capture pipe
+ * @param[in]   stream The stream.
+ * @param[in]   enable - true, disable - false
+ * @return      None
+ *
+ * Enables or disables digital zoom for capture pipe in provided stream, if capture pipe
+ * exists. This function sets enable_zoom flag in CAPTURE_PP stage of the capture pipe.
+ * In process_zoom_and_motion(), decision to enable or disable zoom for every stage depends
+ * on this flag.
+ */
+void
+ia_css_en_dz_capt_pipe(struct ia_css_stream *stream, bool enable);
+#endif /* __IA_CSS_STREAM_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_timer.h b/drivers/staging/media/atomisp/pci/ia_css_timer.h
new file mode 100644
index 000000000..c78fbda90
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_timer.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __IA_CSS_TIMER_H
+#define __IA_CSS_TIMER_H
+
+/* @file
+ * Timer interface definitions
+ */
+#include <type_support.h>		/* for uint32_t */
+#include "ia_css_err.h"
+
+/* @brief timer reading definition */
+typedef u32 clock_value_t;
+
+/* @brief 32 bit clock tick,(timestamp based on timer-value of CSS-internal timer)*/
+struct ia_css_clock_tick {
+	clock_value_t ticks; /** measured time in ticks.*/
+};
+
+/* @brief TIMER event codes */
+enum ia_css_tm_event {
+	IA_CSS_TM_EVENT_AFTER_INIT,
+	/** Timer Event after Initialization */
+	IA_CSS_TM_EVENT_MAIN_END,
+	/** Timer Event after end of Main */
+	IA_CSS_TM_EVENT_THREAD_START,
+	/** Timer Event after thread start */
+	IA_CSS_TM_EVENT_FRAME_PROC_START,
+	/** Timer Event after Frame Process Start */
+	IA_CSS_TM_EVENT_FRAME_PROC_END
+	/** Timer Event after Frame Process End */
+};
+
+/* @brief code measurement common struct */
+struct ia_css_time_meas {
+	clock_value_t	start_timer_value;	/** measured time in ticks */
+	clock_value_t	end_timer_value;	/** measured time in ticks */
+};
+
+/**@brief SIZE_OF_IA_CSS_CLOCK_TICK_STRUCT checks to ensure correct alignment for struct ia_css_clock_tick. */
+#define SIZE_OF_IA_CSS_CLOCK_TICK_STRUCT sizeof(clock_value_t)
+/* @brief checks to ensure correct alignment for ia_css_time_meas. */
+#define SIZE_OF_IA_CSS_TIME_MEAS_STRUCT (sizeof(clock_value_t) \
+					+ sizeof(clock_value_t))
+
+/* @brief API to fetch timer count directly
+*
+* @param curr_ts [out] measured count value
+* @return 0 if success
+*
+*/
+int
+ia_css_timer_get_current_tick(
+    struct ia_css_clock_tick *curr_ts);
+
+#endif  /* __IA_CSS_TIMER_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_tpg.h b/drivers/staging/media/atomisp/pci/ia_css_tpg.h
new file mode 100644
index 000000000..8c744bedb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_tpg.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TPG_H
+#define __IA_CSS_TPG_H
+
+/* @file
+ * This file contains support for the test pattern generator (TPG)
+ */
+
+/* Enumerate the TPG IDs.
+ */
+enum ia_css_tpg_id {
+	IA_CSS_TPG_ID0,
+	IA_CSS_TPG_ID1,
+	IA_CSS_TPG_ID2
+};
+
+/**
+ * Maximum number of TPG IDs.
+ *
+ * Make sure the value of this define gets changed to reflect the correct
+ * number of ia_css_tpg_id enum if you add/delete an item in the enum.
+ */
+#define N_CSS_TPG_IDS (IA_CSS_TPG_ID2 + 1)
+
+/* Enumerate the TPG modes.
+ */
+enum ia_css_tpg_mode {
+	IA_CSS_TPG_MODE_RAMP,
+	IA_CSS_TPG_MODE_CHECKERBOARD,
+	IA_CSS_TPG_MODE_FRAME_BASED_COLOR,
+	IA_CSS_TPG_MODE_MONO
+};
+
+/* @brief Configure the test pattern generator.
+ *
+ * Configure the Test Pattern Generator, the way these values are used to
+ * generate the pattern can be seen in the HRT extension for the test pattern
+ * generator:
+ * devices/test_pat_gen/hrt/include/test_pat_gen.h: hrt_calc_tpg_data().
+ *
+ * This interface is deprecated, it is not portable -> move to input system API
+ *
+@code
+unsigned int test_pattern_value(unsigned int x, unsigned int y)
+{
+ unsigned int x_val, y_val;
+ if (x_delta > 0) (x_val = (x << x_delta) & x_mask;
+ else (x_val = (x >> -x_delta) & x_mask;
+ if (y_delta > 0) (y_val = (y << y_delta) & y_mask;
+ else (y_val = (y >> -y_delta) & x_mask;
+ return (x_val + y_val) & xy_mask;
+}
+@endcode
+ */
+struct ia_css_tpg_config {
+	enum ia_css_tpg_id   id;
+	enum ia_css_tpg_mode mode;
+	unsigned int         x_mask;
+	int                  x_delta;
+	unsigned int         y_mask;
+	int                  y_delta;
+	unsigned int         xy_mask;
+};
+
+#endif /* __IA_CSS_TPG_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_types.h b/drivers/staging/media/atomisp/pci/ia_css_types.h
new file mode 100644
index 000000000..6e34d401f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_types.h
@@ -0,0 +1,603 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_TYPES_H
+#define _IA_CSS_TYPES_H
+
+/* @file
+ * This file contains types used for the ia_css parameters.
+ * These types are in a separate file because they are expected
+ * to be used in software layers that do not access the CSS API
+ * directly but still need to forward parameters for it.
+ */
+
+#include <type_support.h>
+
+#include "ia_css_frac.h"
+
+#include "isp/kernels/aa/aa_2/ia_css_aa2_types.h"
+#include "isp/kernels/anr/anr_1.0/ia_css_anr_types.h"
+#include "isp/kernels/anr/anr_2/ia_css_anr2_types.h"
+#include "isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h"
+#include "isp/kernels/csc/csc_1.0/ia_css_csc_types.h"
+#include "isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h"
+#include "isp/kernels/dp/dp_1.0/ia_css_dp_types.h"
+#include "isp/kernels/de/de_1.0/ia_css_de_types.h"
+#include "isp/kernels/de/de_2/ia_css_de2_types.h"
+#include "isp/kernels/fc/fc_1.0/ia_css_formats_types.h"
+#include "isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h"
+#include "isp/kernels/gc/gc_1.0/ia_css_gc_types.h"
+#include "isp/kernels/gc/gc_2/ia_css_gc2_types.h"
+#include "isp/kernels/macc/macc_1.0/ia_css_macc_types.h"
+#include "isp/kernels/ob/ob_1.0/ia_css_ob_types.h"
+#include "isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h"
+#include "isp/kernels/sc/sc_1.0/ia_css_sc_types.h"
+#include "isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h"
+#include "isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h"
+#include "isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h"
+#include "isp/kernels/wb/wb_1.0/ia_css_wb_types.h"
+#include "isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h"
+#include "isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h"
+
+/* ISP2401 */
+#include "isp/kernels/tnr/tnr3/ia_css_tnr3_types.h"
+
+#include "isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h"
+#include "isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h"
+#include "isp/kernels/output/output_1.0/ia_css_output_types.h"
+
+#define IA_CSS_DVS_STAT_GRID_INFO_SUPPORTED
+/** Should be removed after Driver adaptation will be done */
+
+#define IA_CSS_VERSION_MAJOR    2
+#define IA_CSS_VERSION_MINOR    0
+#define IA_CSS_VERSION_REVISION 2
+
+#define IA_CSS_MORPH_TABLE_NUM_PLANES  6
+
+/* Min and max exposure IDs. These macros are here to allow
+ * the drivers to get this information. Changing these macros
+ * constitutes a CSS API change. */
+#define IA_CSS_ISYS_MIN_EXPOSURE_ID 1   /** Minimum exposure ID */
+#define IA_CSS_ISYS_MAX_EXPOSURE_ID 250 /** Maximum exposure ID */
+
+/* opaque types */
+struct ia_css_isp_parameters;
+struct ia_css_pipe;
+struct ia_css_memory_offsets;
+struct ia_css_config_memory_offsets;
+struct ia_css_state_memory_offsets;
+
+/* Virtual address within the CSS address space. */
+typedef u32 ia_css_ptr;
+
+/* Generic resolution structure.
+ */
+struct ia_css_resolution {
+	u32 width;  /** Width */
+	u32 height; /** Height */
+};
+
+/* Generic coordinate structure.
+ */
+struct ia_css_coordinate {
+	s32 x;	/** Value of a coordinate on the horizontal axis */
+	s32 y;	/** Value of a coordinate on the vertical axis */
+};
+
+/* Vector with signed values. This is used to indicate motion for
+ * Digital Image Stabilization.
+ */
+struct ia_css_vector {
+	s32 x; /** horizontal motion (in pixels) */
+	s32 y; /** vertical motion (in pixels) */
+};
+
+/* Short hands */
+#define IA_CSS_ISP_DMEM IA_CSS_ISP_DMEM0
+#define IA_CSS_ISP_VMEM IA_CSS_ISP_VMEM0
+
+/* CSS data descriptor */
+struct ia_css_data {
+	ia_css_ptr address; /** CSS virtual address */
+	u32   size;    /** Disabled if 0 */
+};
+
+/* Host data descriptor */
+struct ia_css_host_data {
+	char      *address; /** Host address */
+	u32   size;    /** Disabled if 0 */
+};
+
+/* ISP data descriptor */
+struct ia_css_isp_data {
+	u32   address; /** ISP address */
+	u32   size;    /** Disabled if 0 */
+};
+
+/* Shading Correction types. */
+enum ia_css_shading_correction_type {
+	IA_CSS_SHADING_CORRECTION_NONE,	 /** Shading Correction is not processed in the pipe. */
+	IA_CSS_SHADING_CORRECTION_TYPE_1 /** Shading Correction 1.0 (pipe 1.0 on ISP2300, pipe 2.2 on ISP2400/2401) */
+
+	/** More shading correction types can be added in the future. */
+};
+
+/* Shading Correction information. */
+struct ia_css_shading_info {
+	enum ia_css_shading_correction_type type; /** Shading Correction type. */
+
+	union {	/* Shading Correction information of each Shading Correction types. */
+
+		/* Shading Correction information of IA_CSS_SHADING_CORRECTION_TYPE_1.
+		 *
+		 *  This structure contains the information necessary to generate
+		 *  the shading table required in the isp.
+		 *  This structure is filled in the css,
+		 *  and the driver needs to get it to generate the shading table.
+		 *
+		 *  Before the shading correction is applied, NxN-filter and/or scaling
+		 *  are applied in the isp, depending on the isp binaries.
+		 *  Then, these should be considered in generating the shading table.
+		 *    - Bad pixels on left/top sides generated by NxN-filter
+		 *      (Bad pixels are NOT considered currently,
+		 *      because they are subtle.)
+		 *    - Down-scaling/Up-scaling factor
+		 *
+		 *  Shading correction is applied to the area
+		 *  which has real sensor data and margin.
+		 *  Then, the shading table should cover the area including margin.
+		 *  This structure has this information.
+		 *    - Origin coordinate of bayer (real sensor data)
+		 *      on the shading table
+		 *
+		 * ------------------------ISP 2401-----------------------
+		 *
+		 *  the shading table directly required from ISP.
+		 *  This structure is filled in CSS, and the driver needs to get it to generate the shading table.
+		 *
+		 *  The shading correction is applied to the bayer area which contains sensor data and padding data.
+		 *  The shading table should cover this bayer area.
+		 *
+		 *  The shading table size directly required from ISP is expressed by these parameters.
+		 *    1. uint32_t num_hor_grids;
+		 *    2. uint32_t num_ver_grids;
+		 *    3. uint32_t bqs_per_grid_cell;
+		 *
+		 *  In some isp binaries, the bayer scaling is applied before the shading correction is applied.
+		 *  Then, this scaling factor should be considered in generating the shading table.
+		 *  The scaling factor is expressed by these parameters.
+		 *    4. uint32_t bayer_scale_hor_ratio_in;
+		 *    5. uint32_t bayer_scale_hor_ratio_out;
+		 *    6. uint32_t bayer_scale_ver_ratio_in;
+		 *    7. uint32_t bayer_scale_ver_ratio_out;
+		 *
+		 *  The sensor data size inputted to ISP is expressed by this parameter.
+		 *  This is the size BEFORE the bayer scaling is applied.
+		 *    8. struct ia_css_resolution isp_input_sensor_data_res_bqs;
+		 *
+		 *  The origin of the sensor data area positioned on the shading table at the shading correction
+		 *  is expressed by this parameter.
+		 *  The size of this area assumes the size AFTER the bayer scaling is applied
+		 *  to the isp_input_sensor_data_resolution_bqs.
+		 *    9. struct ia_css_coordinate sensor_data_origin_bqs_on_sctbl;
+		 *
+		 *  ****** Definitions of the shading table and the sensor data at the shading correction ******
+		 *
+		 * (0,0)--------------------- TW -------------------------------
+		 *   |                                            shading table |
+		 *   |      (ox,oy)---------- W --------------------------      |
+		 *   |        |                               sensor data |     |
+		 *   |        |                                           |     |
+		 *  TH        H             sensor data center            |     |
+		 *   |        |                  (cx,cy)                  |     |
+		 *   |        |                                           |     |
+		 *   |        |                                           |     |
+		 *   |        |                                           |     |
+		 *   |         -------------------------------------------      |
+		 *   |                                                          |
+		 *    ----------------------------------------------------------
+		 *
+		 *    Example of still mode for output 1080p:
+		 *
+		 *    num_hor_grids = 66
+		 *    num_ver_grids = 37
+		 *    bqs_per_grid_cell = 16
+		 *    bayer_scale_hor_ratio_in = 1
+		 *    bayer_scale_hor_ratio_out = 1
+		 *    bayer_scale_ver_ratio_in = 1
+		 *    bayer_scale_ver_ratio_out = 1
+		 *    isp_input_sensor_data_resolution_bqs = {966, 546}
+		 *    sensor_data_origin_bqs_on_sctbl = {61, 15}
+		 *
+		 *    TW, TH [bqs]: width and height of shading table
+		 *        TW = (num_hor_grids - 1) * bqs_per_grid_cell = (66 - 1) * 16 = 1040
+		 *        TH = (num_ver_grids - 1) * bqs_per_grid_cell = (37 - 1) * 16 = 576
+		 *
+		 *    W, H [bqs]: width and height of sensor data at shading correction
+		 *        W = sensor_data_res_bqs.width
+		 *          = isp_input_sensor_data_res_bqs.width
+		 *              * bayer_scale_hor_ratio_out / bayer_scale_hor_ratio_in + 0.5 = 966
+		 *        H = sensor_data_res_bqs.height
+		 *          = isp_input_sensor_data_res_bqs.height
+		 *               * bayer_scale_ver_ratio_out / bayer_scale_ver_ratio_in + 0.5 = 546
+		 *
+		 *    (ox, oy) [bqs]: origin of sensor data positioned on shading table at shading correction
+		 *        ox = sensor_data_origin_bqs_on_sctbl.x = 61
+		 *        oy = sensor_data_origin_bqs_on_sctbl.y = 15
+		 *
+		 *    (cx, cy) [bqs]: center of sensor data positioned on shading table at shading correction
+		 *        cx = ox + W/2 = 61 + 966/2 = 544
+		 *        cy = oy + H/2 = 15 + 546/2 = 288
+		 *
+		 *  ****** Relation between the shading table and the sensor data ******
+		 *
+		 *    The origin of the sensor data should be on the shading table.
+		 *        0 <= ox < TW,  0 <= oy < TH
+		 *
+		 *  ****** How to center the shading table on the sensor data ******
+		 *
+		 *    To center the shading table on the sensor data,
+		 *    CSS decides the shading table size so that a certain grid point is positioned
+		 *    on the center of the sensor data at the shading correction.
+		 *    CSS expects the shading center is set on this grid point
+		 *    when the shading table data is calculated in AIC.
+		 *
+		 *    W, H [bqs]: width and height of sensor data at shading correction
+		 *	W = sensor_data_res_bqs.width
+		 *	H = sensor_data_res_bqs.height
+		 *
+		 *    (cx, cy) [bqs]: center of sensor data positioned on shading table at shading correction
+		 *	cx = sensor_data_origin_bqs_on_sctbl.x + W/2
+		 *	cy = sensor_data_origin_bqs_on_sctbl.y + H/2
+		 *
+		 *    CSS decides the shading table size and the sensor data position
+		 *    so that the (cx, cy) satisfies this condition.
+		 *	mod(cx, bqs_per_grid_cell) = 0
+		 *	mod(cy, bqs_per_grid_cell) = 0
+		 *
+		 *  ****** How to change the sensor data size by processes in the driver and ISP ******
+		 *
+		 *    1. sensor data size: Physical sensor size
+		 *			   (The struct ia_css_shading_info does not have this information.)
+		 *    2. process:          Driver applies the sensor cropping/binning/scaling to physical sensor size.
+		 *    3. sensor data size: ISP input size (== shading_info.isp_input_sensor_data_res_bqs)
+		 *			   (ISP assumes the ISP input sensor data is centered on the physical sensor.)
+		 *    4. process:          ISP applies the bayer scaling by the factor of shading_info.bayer_scale_*.
+		 *    5. sensor data size: Scaling factor * ISP input size (== shading_info.sensor_data_res_bqs)
+		 *    6. process:          ISP applies the shading correction.
+		 *
+		 *  ISP block: SC1
+		 *  ISP1: SC1 is used.
+		 *  ISP2: SC1 is used.
+		 */
+		struct {
+			/* ISP2400 */
+			u32 enable;	/** Shading correction enabled.
+						     0:disabled, 1:enabled */
+
+			/* ISP2401 */
+			u32 num_hor_grids;	/** Number of data points per line per color on shading table. */
+			u32 num_ver_grids;	/** Number of lines of data points per color on shading table. */
+			u32 bqs_per_grid_cell; /** Grid cell size in BQ unit.
+							 NOTE: bqs = size in BQ(Bayer Quad) unit.
+							       1BQ means {Gr,R,B,Gb} (2x2 pixels).
+							       Horizontal 1 bqs corresponds to horizontal 2 pixels.
+							       Vertical 1 bqs corresponds to vertical 2 pixels. */
+			u32 bayer_scale_hor_ratio_in;
+			u32 bayer_scale_hor_ratio_out;
+
+			/** Horizontal ratio of bayer scaling between input width and output width,
+			     for the scaling which should be done before shading correction.
+				output_width = input_width * bayer_scale_hor_ratio_out
+								/ bayer_scale_hor_ratio_in + 0.5 */
+			u32 bayer_scale_ver_ratio_in;
+			u32 bayer_scale_ver_ratio_out;
+
+			/** Vertical ratio of bayer scaling
+			between input height and output height, for the scaling
+			which should be done before shading correction.
+			  output_height = input_height * bayer_scale_ver_ratio_out
+						/ bayer_scale_ver_ratio_in */
+			/* ISP2400 */
+			u32 sc_bayer_origin_x_bqs_on_shading_table;
+			/** X coordinate (in bqs) of bayer origin on shading table.
+			This indicates the left-most pixel of bayer
+			(not include margin) inputted to the shading correction.
+			This corresponds to the left-most pixel of bayer
+			inputted to isp from sensor. */
+			/* ISP2400 */
+			u32 sc_bayer_origin_y_bqs_on_shading_table;
+			/** Y coordinate (in bqs) of bayer origin on shading table.
+			This indicates the top pixel of bayer
+			(not include margin) inputted to the shading correction.
+			This corresponds to the top pixel of bayer
+			inputted to isp from sensor. */
+
+			/** Vertical ratio of bayer scaling between input height and output height,
+			     for the scaling which should be done before shading correction.
+				output_height = input_height * bayer_scale_ver_ratio_out
+								/ bayer_scale_ver_ratio_in + 0.5 */
+			/* ISP2401 */
+			struct ia_css_resolution isp_input_sensor_data_res_bqs;
+			/** Sensor data size (in bqs) inputted to ISP. This is the size BEFORE bayer scaling.
+			     NOTE: This is NOT the size of the physical sensor size.
+				   CSS requests the driver that ISP inputs sensor data
+				   by the size of isp_input_sensor_data_res_bqs.
+				   The driver sends the sensor data to ISP,
+				   after the adequate cropping/binning/scaling
+				   are applied to the physical sensor data area.
+				   ISP assumes the area of isp_input_sensor_data_res_bqs
+				   is centered on the physical sensor. */
+			/* ISP2401 */
+			struct ia_css_resolution sensor_data_res_bqs;
+			/** Sensor data size (in bqs) at shading correction.
+			     This is the size AFTER bayer scaling. */
+			/* ISP2401 */
+			struct ia_css_coordinate sensor_data_origin_bqs_on_sctbl;
+			/** Origin of sensor data area positioned on shading table at shading correction.
+			     The coordinate x,y should be positive values. */
+		} type_1;
+
+		/** More structures can be added here when more shading correction types will be added
+		     in the future. */
+	} info;
+};
+
+/* Default Shading Correction information of Shading Correction Type 1. */
+#define DEFAULT_SHADING_INFO_TYPE_1 \
+(struct ia_css_shading_info) { \
+	.type = IA_CSS_SHADING_CORRECTION_TYPE_1, \
+	.info = { \
+		.type_1 = { \
+			.bayer_scale_hor_ratio_in	= 1, \
+			.bayer_scale_hor_ratio_out	= 1, \
+			.bayer_scale_ver_ratio_in	= 1, \
+			.bayer_scale_ver_ratio_out	= 1, \
+		} \
+	} \
+}
+
+/* Default Shading Correction information. */
+#define DEFAULT_SHADING_INFO	DEFAULT_SHADING_INFO_TYPE_1
+
+/* structure that describes the 3A and DIS grids */
+struct ia_css_grid_info {
+	/* \name ISP input size
+	  * that is visible for user
+	  * @{
+	  */
+	u32 isp_in_width;
+	u32 isp_in_height;
+	/* @}*/
+
+	struct ia_css_3a_grid_info  s3a_grid; /** 3A grid info */
+	union ia_css_dvs_grid_u dvs_grid;
+	/** All types of DVS statistics grid info union */
+
+	enum ia_css_vamem_type vamem_type;
+};
+
+/* defaults for ia_css_grid_info structs */
+#define DEFAULT_GRID_INFO { \
+	.dvs_grid	= DEFAULT_DVS_GRID_INFO, \
+	.vamem_type	= IA_CSS_VAMEM_TYPE_1 \
+}
+
+/* Morphing table, used for geometric distortion and chromatic abberration
+ *  correction (GDCAC, also called GDC).
+ *  This table describes the imperfections introduced by the lens, the
+ *  advanced ISP can correct for these imperfections using this table.
+ */
+struct ia_css_morph_table {
+	u32 enable; /** To disable GDC, set this field to false. The
+			  coordinates fields can be set to NULL in this case. */
+	u32 height; /** Table height */
+	u32 width;  /** Table width */
+	u16 *coordinates_x[IA_CSS_MORPH_TABLE_NUM_PLANES];
+	/** X coordinates that describe the sensor imperfection */
+	u16 *coordinates_y[IA_CSS_MORPH_TABLE_NUM_PLANES];
+	/** Y coordinates that describe the sensor imperfection */
+};
+
+struct ia_css_dvs_6axis_config {
+	unsigned int exp_id;
+	/** Exposure ID, see ia_css_event_public.h for more detail */
+	u32 width_y;
+	u32 height_y;
+	u32 width_uv;
+	u32 height_uv;
+	u32 *xcoords_y;
+	u32 *ycoords_y;
+	u32 *xcoords_uv;
+	u32 *ycoords_uv;
+};
+
+/**
+ * This specifies the coordinates (x,y)
+ */
+struct ia_css_point {
+	s32 x; /** x coordinate */
+	s32 y; /** y coordinate */
+};
+
+/**
+ * This specifies the region
+ */
+struct ia_css_region {
+	struct ia_css_point origin; /** Starting point coordinates for the region */
+	struct ia_css_resolution resolution; /** Region resolution */
+};
+
+/**
+ * Digital zoom:
+ * This feature is currently available only for video, but will become
+ * available for preview and capture as well.
+ * Set the digital zoom factor, this is a logarithmic scale. The actual zoom
+ * factor will be 64/x.
+ * Setting dx or dy to 0 disables digital zoom for that direction.
+ * New API change for Digital zoom:(added struct ia_css_region zoom_region)
+ * zoom_region specifies the origin of the zoom region and width and
+ * height of that region.
+ * origin : This is the coordinate (x,y) within the effective input resolution
+ * of the stream. where, x >= 0 and y >= 0. (0,0) maps to the upper left of the
+ * effective input resolution.
+ * resolution : This is resolution of zoom region.
+ * where, x + width <= effective input width
+ * y + height <= effective input height
+ */
+struct ia_css_dz_config {
+	u32 dx; /** Horizontal zoom factor */
+	u32 dy; /** Vertical zoom factor */
+	struct ia_css_region zoom_region; /** region for zoom */
+};
+
+/* The still capture mode, this can be RAW (simply copy sensor input to DDR),
+ *  Primary ISP, the Advanced ISP (GDC) or the low-light ISP (ANR).
+ */
+enum ia_css_capture_mode {
+	IA_CSS_CAPTURE_MODE_RAW,      /** no processing, copy data only */
+	IA_CSS_CAPTURE_MODE_BAYER,    /** bayer processing, up to demosaic */
+	IA_CSS_CAPTURE_MODE_PRIMARY,  /** primary ISP */
+	IA_CSS_CAPTURE_MODE_ADVANCED, /** advanced ISP (GDC) */
+	IA_CSS_CAPTURE_MODE_LOW_LIGHT /** low light ISP (ANR) */
+};
+
+struct ia_css_capture_config {
+	enum ia_css_capture_mode mode; /** Still capture mode */
+	u32 enable_xnr;	       /** Enable/disable XNR */
+	u32 enable_raw_output;
+	bool enable_capture_pp_bli;    /** Enable capture_pp_bli mode */
+};
+
+/* default settings for ia_css_capture_config structs */
+#define DEFAULT_CAPTURE_CONFIG { \
+	.mode	= IA_CSS_CAPTURE_MODE_PRIMARY, \
+}
+
+/* ISP filter configuration. This is a collection of configurations
+ *  for each of the ISP filters (modules).
+ *
+ *  NOTE! The contents of all pointers is copied when get or set with the
+ *  exception of the shading and morph tables. For these we only copy the
+ *  pointer, so the caller must make sure the memory contents of these pointers
+ *  remain valid as long as they are used by the CSS. This will be fixed in the
+ *  future by copying the contents instead of just the pointer.
+ *
+ *  Comment:
+ *    ["ISP block", 1&2]   : ISP block is used both for ISP1 and ISP2.
+ *    ["ISP block", 1only] : ISP block is used only for ISP1.
+ *    ["ISP block", 2only] : ISP block is used only for ISP2.
+ */
+struct ia_css_isp_config {
+	struct ia_css_wb_config   *wb_config;	/** White Balance
+							[WB1, 1&2] */
+	struct ia_css_cc_config   *cc_config;	/** Color Correction
+							[CSC1, 1only] */
+	struct ia_css_tnr_config  *tnr_config;	/** Temporal Noise Reduction
+							[TNR1, 1&2] */
+	struct ia_css_ecd_config  *ecd_config;	/** Eigen Color Demosaicing
+							[DE2, 2only] */
+	struct ia_css_ynr_config  *ynr_config;	/** Y(Luma) Noise Reduction
+							[YNR2&YEE2, 2only] */
+	struct ia_css_fc_config   *fc_config;	/** Fringe Control
+							[FC2, 2only] */
+	struct ia_css_formats_config
+		*formats_config;	/** Formats Control for main output
+							[FORMATS, 1&2] */
+	struct ia_css_cnr_config  *cnr_config;	/** Chroma Noise Reduction
+							[CNR2, 2only] */
+	struct ia_css_macc_config *macc_config;	/** MACC
+							[MACC2, 2only] */
+	struct ia_css_ctc_config  *ctc_config;	/** Chroma Tone Control
+							[CTC2, 2only] */
+	struct ia_css_aa_config   *aa_config;	/** YUV Anti-Aliasing
+							[AA2, 2only]
+							(not used currently) */
+	struct ia_css_aa_config   *baa_config;	/** Bayer Anti-Aliasing
+							[BAA2, 1&2] */
+	struct ia_css_ce_config   *ce_config;	/** Chroma Enhancement
+							[CE1, 1only] */
+	struct ia_css_dvs_6axis_config *dvs_6axis_config;
+	struct ia_css_ob_config   *ob_config;  /** Objective Black
+							[OB1, 1&2] */
+	struct ia_css_dp_config   *dp_config;  /** Defect Pixel Correction
+							[DPC1/DPC2, 1&2] */
+	struct ia_css_nr_config   *nr_config;  /** Noise Reduction
+							[BNR1&YNR1&CNR1, 1&2]*/
+	struct ia_css_ee_config   *ee_config;  /** Edge Enhancement
+							[YEE1, 1&2] */
+	struct ia_css_de_config   *de_config;  /** Demosaic
+							[DE1, 1only] */
+	struct ia_css_gc_config   *gc_config;  /** Gamma Correction (for YUV)
+							[GC1, 1only] */
+	struct ia_css_anr_config  *anr_config; /** Advanced Noise Reduction */
+	struct ia_css_3a_config   *s3a_config; /** 3A Statistics config */
+	struct ia_css_xnr_config  *xnr_config; /** eXtra Noise Reduction */
+	struct ia_css_dz_config   *dz_config;  /** Digital Zoom */
+	struct ia_css_cc_config *yuv2rgb_cc_config; /** Color Correction
+							[CCM2, 2only] */
+	struct ia_css_cc_config *rgb2yuv_cc_config; /** Color Correction
+							[CSC2, 2only] */
+	struct ia_css_macc_table  *macc_table;	/** MACC
+							[MACC1/MACC2, 1&2]*/
+	struct ia_css_gamma_table *gamma_table;	/** Gamma Correction (for YUV)
+							[GC1, 1only] */
+	struct ia_css_ctc_table   *ctc_table;	/** Chroma Tone Control
+							[CTC1, 1only] */
+
+	/* \deprecated */
+	struct ia_css_xnr_table   *xnr_table;	/** eXtra Noise Reduction
+							[XNR1, 1&2] */
+	struct ia_css_rgb_gamma_table *r_gamma_table;/** sRGB Gamma Correction
+							[GC2, 2only] */
+	struct ia_css_rgb_gamma_table *g_gamma_table;/** sRGB Gamma Correction
+							[GC2, 2only] */
+	struct ia_css_rgb_gamma_table *b_gamma_table;/** sRGB Gamma Correction
+							[GC2, 2only] */
+	struct ia_css_vector      *motion_vector; /** For 2-axis DVS */
+	struct ia_css_shading_table *shading_table;
+	struct ia_css_morph_table   *morph_table;
+	struct ia_css_dvs_coefficients *dvs_coefs; /** DVS 1.0 coefficients */
+	struct ia_css_dvs2_coefficients *dvs2_coefs; /** DVS 2.0 coefficients */
+	struct ia_css_capture_config   *capture_config;
+	struct ia_css_anr_thres   *anr_thres;
+	/* @deprecated{Old shading settings, see bugzilla bz675 for details} */
+	struct ia_css_shading_settings *shading_settings;
+	struct ia_css_xnr3_config *xnr3_config; /** eXtreme Noise Reduction v3 */
+	/* comment from Lasse: Be aware how this feature will affect coordinate
+	 *  normalization in different parts of the system. (e.g. face detection,
+	 *  touch focus, 3A statistics and windows of interest, shading correction,
+	 *  DVS, GDC) from IQ tool level and application level down-to ISP FW level.
+	 *  the risk for regression is not in the individual blocks, but how they
+	 *  integrate together. */
+	struct ia_css_output_config
+		*output_config;	/** Main Output Mirroring, flipping */
+
+	struct ia_css_scaler_config
+		*scaler_config;         /** Skylake: scaler config (optional) */
+	struct ia_css_formats_config
+		*formats_config_display;/** Formats control for viewfinder/display output (optional)
+										[OSYS, n/a] */
+	struct ia_css_output_config
+		*output_config_display; /** Viewfinder/display output mirroring, flipping (optional) */
+
+	struct ia_css_frame
+		*output_frame;          /** Output frame the config is to be applied to (optional) */
+	u32			isp_config_id;	/** Unique ID to track which config was actually applied to a particular frame */
+};
+
+#endif /* _IA_CSS_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_version.h b/drivers/staging/media/atomisp/pci/ia_css_version.h
new file mode 100644
index 000000000..cf1d010ba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_version.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_VERSION_H
+#define __IA_CSS_VERSION_H
+
+/* @file
+ * This file contains functions to retrieve CSS-API version information
+ */
+
+#include <ia_css_err.h>
+
+/* a common size for the version arrays */
+#define MAX_VERSION_SIZE	500
+
+/* @brief Retrieves the current CSS version
+ * @param[out]	version		A pointer to a buffer where to put the generated
+ *				version string. NULL is ignored.
+ * @param[in]	max_size	Size of the version buffer. If version string
+ *				would be larger than max_size, an error is
+ *				returned by this function.
+ *
+ * This function generates and returns the version string. If FW is loaded, it
+ * attaches the FW version.
+ */
+int
+ia_css_get_version(char *version, int max_size);
+
+#endif /* __IA_CSS_VERSION_H */
diff --git a/drivers/staging/media/atomisp/pci/ia_css_version_data.h b/drivers/staging/media/atomisp/pci/ia_css_version_data.h
new file mode 100644
index 000000000..428d78e16
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/ia_css_version_data.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+//
+// This file contains the version data for the CSS
+//
+// === Do not change - automatically generated ===
+//
+
+#ifndef __IA_CSS_VERSION_DATA_H
+#define __IA_CSS_VERSION_DATA_H
+
+#define ISP2400_CSS_VERSION_STRING "REL:20150521_21.4_0539; API:2.1.15.3; GIT:irci_candrpv_0415_20150504_35b345#35b345be52ac575f8934abb3a88fea26a94e7343; SDK:/nfs/iir/disks/iir_hivepackages_003/iir_hivepkgs_disk017/Css_Mizuchi/packages/Css_Mizuchi/int_css_mizuchi_20140829_1053; USER:viedifw; "
+#define ISP2401_CSS_VERSION_STRING "REL:20150911_37.5_1652; API:2.1.20.9; GIT:irci___#ebf437d53a8951bb7ff6d13fdb7270dab393a92a; SDK:; USER:viedifw; "
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/if_defs.h b/drivers/staging/media/atomisp/pci/if_defs.h
new file mode 100644
index 000000000..e21efa749
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/if_defs.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IF_DEFS_H
+#define _IF_DEFS_H
+
+#define HIVE_IF_FRAME_REQUEST        0xA000
+#define HIVE_IF_LINES_REQUEST        0xB000
+#define HIVE_IF_VECTORS_REQUEST      0xC000
+
+#endif /* _IF_DEFS_H */
diff --git a/drivers/staging/media/atomisp/pci/input_formatter_subsystem_defs.h b/drivers/staging/media/atomisp/pci/input_formatter_subsystem_defs.h
new file mode 100644
index 000000000..594fc36a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_formatter_subsystem_defs.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _if_subsystem_defs_h__
+#define _if_subsystem_defs_h__
+
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_0            0
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_1            1
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_2            2
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_3            3
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_4            4
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_5            5
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_6            6
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_7            7
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_FSYNC_LUT_REG        8
+#define HIVE_IFMT_GP_REGS_SRST_IDX                          9
+#define HIVE_IFMT_GP_REGS_SLV_REG_SRST_IDX                 10
+
+#define HIVE_IFMT_GP_REGS_CH_ID_FMT_TYPE_IDX               11
+
+#define HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_BASE         HIVE_IFMT_GP_REGS_INPUT_SWITCH_LUT_REG_0
+
+/* order of the input bits for the ifmt irq controller */
+#define HIVE_IFMT_IRQ_IFT_PRIM_BIT_ID                       0
+#define HIVE_IFMT_IRQ_IFT_PRIM_B_BIT_ID                     1
+#define HIVE_IFMT_IRQ_IFT_SEC_BIT_ID                        2
+#define HIVE_IFMT_IRQ_MEM_CPY_BIT_ID                        3
+#define HIVE_IFMT_IRQ_SIDEBAND_CHANGED_BIT_ID               4
+
+/* order of the input bits for the ifmt Soft reset register */
+#define HIVE_IFMT_GP_REGS_SRST_IFT_PRIM_BIT_IDX             0
+#define HIVE_IFMT_GP_REGS_SRST_IFT_PRIM_B_BIT_IDX           1
+#define HIVE_IFMT_GP_REGS_SRST_IFT_SEC_BIT_IDX              2
+#define HIVE_IFMT_GP_REGS_SRST_MEM_CPY_BIT_IDX              3
+
+/* order of the input bits for the ifmt Soft reset register */
+#define HIVE_IFMT_GP_REGS_SLV_REG_SRST_IFT_PRIM_BIT_IDX     0
+#define HIVE_IFMT_GP_REGS_SLV_REG_SRST_IFT_PRIM_B_BIT_IDX   1
+#define HIVE_IFMT_GP_REGS_SLV_REG_SRST_IFT_SEC_BIT_IDX      2
+#define HIVE_IFMT_GP_REGS_SLV_REG_SRST_MEM_CPY_BIT_IDX      3
+
+#endif /* _if_subsystem_defs_h__ */
diff --git a/drivers/staging/media/atomisp/pci/input_selector_defs.h b/drivers/staging/media/atomisp/pci/input_selector_defs.h
new file mode 100644
index 000000000..61882e4cb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_selector_defs.h
@@ -0,0 +1,89 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _input_selector_defs_h
+#define _input_selector_defs_h
+
+#ifndef HIVE_ISP_ISEL_SEL_BITS
+#define HIVE_ISP_ISEL_SEL_BITS                                  2
+#endif
+
+#ifndef HIVE_ISP_CH_ID_BITS
+#define HIVE_ISP_CH_ID_BITS                                     2
+#endif
+
+#ifndef HIVE_ISP_FMT_TYPE_BITS
+#define HIVE_ISP_FMT_TYPE_BITS                                  5
+#endif
+
+/* gp_register register id's -- Outputs */
+#define HIVE_ISEL_GP_REGS_SYNCGEN_ENABLE_IDX                    0
+#define HIVE_ISEL_GP_REGS_SYNCGEN_FREE_RUNNING_IDX              1
+#define HIVE_ISEL_GP_REGS_SYNCGEN_PAUSE_IDX                     2
+#define HIVE_ISEL_GP_REGS_SYNCGEN_NR_FRAMES_IDX                 3
+#define HIVE_ISEL_GP_REGS_SYNCGEN_NR_PIX_IDX                    4
+#define HIVE_ISEL_GP_REGS_SYNCGEN_NR_LINES_IDX                  5
+#define HIVE_ISEL_GP_REGS_SYNCGEN_HBLANK_CYCLES_IDX             6
+#define HIVE_ISEL_GP_REGS_SYNCGEN_VBLANK_CYCLES_IDX             7
+
+#define HIVE_ISEL_GP_REGS_SOF_IDX                               8
+#define HIVE_ISEL_GP_REGS_EOF_IDX                               9
+#define HIVE_ISEL_GP_REGS_SOL_IDX                              10
+#define HIVE_ISEL_GP_REGS_EOL_IDX                              11
+
+#define HIVE_ISEL_GP_REGS_PRBS_ENABLE                          12
+#define HIVE_ISEL_GP_REGS_PRBS_ENABLE_PORT_B                   13
+#define HIVE_ISEL_GP_REGS_PRBS_LFSR_RESET_VALUE                14
+
+#define HIVE_ISEL_GP_REGS_TPG_ENABLE                           15
+#define HIVE_ISEL_GP_REGS_TPG_ENABLE_PORT_B                    16
+#define HIVE_ISEL_GP_REGS_TPG_HOR_CNT_MASK_IDX                 17
+#define HIVE_ISEL_GP_REGS_TPG_VER_CNT_MASK_IDX                 18
+#define HIVE_ISEL_GP_REGS_TPG_XY_CNT_MASK_IDX                  19
+#define HIVE_ISEL_GP_REGS_TPG_HOR_CNT_DELTA_IDX                20
+#define HIVE_ISEL_GP_REGS_TPG_VER_CNT_DELTA_IDX                21
+#define HIVE_ISEL_GP_REGS_TPG_MODE_IDX                         22
+#define HIVE_ISEL_GP_REGS_TPG_R1_IDX                           23
+#define HIVE_ISEL_GP_REGS_TPG_G1_IDX                           24
+#define HIVE_ISEL_GP_REGS_TPG_B1_IDX                           25
+#define HIVE_ISEL_GP_REGS_TPG_R2_IDX                           26
+#define HIVE_ISEL_GP_REGS_TPG_G2_IDX                           27
+#define HIVE_ISEL_GP_REGS_TPG_B2_IDX                           28
+
+#define HIVE_ISEL_GP_REGS_CH_ID_IDX                            29
+#define HIVE_ISEL_GP_REGS_FMT_TYPE_IDX                         30
+#define HIVE_ISEL_GP_REGS_DATA_SEL_IDX                         31
+#define HIVE_ISEL_GP_REGS_SBAND_SEL_IDX                        32
+#define HIVE_ISEL_GP_REGS_SYNC_SEL_IDX                         33
+#define HIVE_ISEL_GP_REGS_SRST_IDX                             37
+
+#define HIVE_ISEL_GP_REGS_SRST_SYNCGEN_BIT                      0
+#define HIVE_ISEL_GP_REGS_SRST_PRBS_BIT                         1
+#define HIVE_ISEL_GP_REGS_SRST_TPG_BIT                          2
+#define HIVE_ISEL_GP_REGS_SRST_FIFO_BIT                         3
+
+/* gp_register register id's -- Inputs   */
+#define HIVE_ISEL_GP_REGS_SYNCGEN_HOR_CNT_IDX                  34
+#define HIVE_ISEL_GP_REGS_SYNCGEN_VER_CNT_IDX                  35
+#define HIVE_ISEL_GP_REGS_SYNCGEN_FRAMES_CNT_IDX               36
+
+/* irq sources isel irq controller */
+#define HIVE_ISEL_IRQ_SYNC_GEN_SOF_BIT_ID                       0
+#define HIVE_ISEL_IRQ_SYNC_GEN_EOF_BIT_ID                       1
+#define HIVE_ISEL_IRQ_SYNC_GEN_SOL_BIT_ID                       2
+#define HIVE_ISEL_IRQ_SYNC_GEN_EOL_BIT_ID                       3
+#define HIVE_ISEL_IRQ_NUM_IRQS                                  4
+
+#endif /* _input_selector_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/input_switch_2400_defs.h b/drivers/staging/media/atomisp/pci/input_switch_2400_defs.h
new file mode 100644
index 000000000..8ea1d7991
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_switch_2400_defs.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _input_switch_2400_defs_h
+#define _input_switch_2400_defs_h
+
+#define _HIVE_INPUT_SWITCH_GET_LUT_REG_ID(ch_id, fmt_type) (((ch_id) * 2) + ((fmt_type) >= 16))
+#define _HIVE_INPUT_SWITCH_GET_LUT_REG_LSB(fmt_type)        (((fmt_type) % 16) * 2)
+
+#define HIVE_INPUT_SWITCH_SELECT_NO_OUTPUT   0
+#define HIVE_INPUT_SWITCH_SELECT_IF_PRIM     1
+#define HIVE_INPUT_SWITCH_SELECT_IF_SEC      2
+#define HIVE_INPUT_SWITCH_SELECT_STR_TO_MEM  3
+#define HIVE_INPUT_SWITCH_VSELECT_NO_OUTPUT  0
+#define HIVE_INPUT_SWITCH_VSELECT_IF_PRIM    1
+#define HIVE_INPUT_SWITCH_VSELECT_IF_SEC     2
+#define HIVE_INPUT_SWITCH_VSELECT_STR_TO_MEM 4
+
+#endif /* _input_switch_2400_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/input_system_ctrl_defs.h b/drivers/staging/media/atomisp/pci/input_system_ctrl_defs.h
new file mode 100644
index 000000000..b2076a969
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_ctrl_defs.h
@@ -0,0 +1,244 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _input_system_ctrl_defs_h
+#define _input_system_ctrl_defs_h
+
+#define _INPUT_SYSTEM_CTRL_REG_ALIGN                    4  /* assuming 32 bit control bus width */
+
+/* --------------------------------------------------*/
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+// Number of registers
+#define ISYS_CTRL_NOF_REGS                              23
+
+// Register id's of MMIO slave accessible registers
+#define ISYS_CTRL_CAPT_START_ADDR_A_REG_ID              0
+#define ISYS_CTRL_CAPT_START_ADDR_B_REG_ID              1
+#define ISYS_CTRL_CAPT_START_ADDR_C_REG_ID              2
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_A_REG_ID         3
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_B_REG_ID         4
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_C_REG_ID         5
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_A_REG_ID         6
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_B_REG_ID         7
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_C_REG_ID         8
+#define ISYS_CTRL_ACQ_START_ADDR_REG_ID                 9
+#define ISYS_CTRL_ACQ_MEM_REGION_SIZE_REG_ID            10
+#define ISYS_CTRL_ACQ_NUM_MEM_REGIONS_REG_ID            11
+#define ISYS_CTRL_INIT_REG_ID                           12
+#define ISYS_CTRL_LAST_COMMAND_REG_ID                   13
+#define ISYS_CTRL_NEXT_COMMAND_REG_ID                   14
+#define ISYS_CTRL_LAST_ACKNOWLEDGE_REG_ID               15
+#define ISYS_CTRL_NEXT_ACKNOWLEDGE_REG_ID               16
+#define ISYS_CTRL_FSM_STATE_INFO_REG_ID                 17
+#define ISYS_CTRL_CAPT_A_FSM_STATE_INFO_REG_ID          18
+#define ISYS_CTRL_CAPT_B_FSM_STATE_INFO_REG_ID          19
+#define ISYS_CTRL_CAPT_C_FSM_STATE_INFO_REG_ID          20
+#define ISYS_CTRL_ACQ_FSM_STATE_INFO_REG_ID             21
+#define ISYS_CTRL_CAPT_RESERVE_ONE_MEM_REGION_REG_ID    22
+
+/* register reset value */
+#define ISYS_CTRL_CAPT_START_ADDR_A_REG_RSTVAL           0
+#define ISYS_CTRL_CAPT_START_ADDR_B_REG_RSTVAL           0
+#define ISYS_CTRL_CAPT_START_ADDR_C_REG_RSTVAL           0
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_A_REG_RSTVAL      128
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_B_REG_RSTVAL      128
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_C_REG_RSTVAL      128
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_A_REG_RSTVAL      3
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_B_REG_RSTVAL      3
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_C_REG_RSTVAL      3
+#define ISYS_CTRL_ACQ_START_ADDR_REG_RSTVAL              0
+#define ISYS_CTRL_ACQ_MEM_REGION_SIZE_REG_RSTVAL         128
+#define ISYS_CTRL_ACQ_NUM_MEM_REGIONS_REG_RSTVAL         3
+#define ISYS_CTRL_INIT_REG_RSTVAL                        0
+#define ISYS_CTRL_LAST_COMMAND_REG_RSTVAL                15    //0x0000_000F (to signal non-valid cmd/ack after reset/soft-reset)
+#define ISYS_CTRL_NEXT_COMMAND_REG_RSTVAL                15    //0x0000_000F (to signal non-valid cmd/ack after reset/soft-reset)
+#define ISYS_CTRL_LAST_ACKNOWLEDGE_REG_RSTVAL            15    //0x0000_000F (to signal non-valid cmd/ack after reset/soft-reset)
+#define ISYS_CTRL_NEXT_ACKNOWLEDGE_REG_RSTVAL            15    //0x0000_000F (to signal non-valid cmd/ack after reset/soft-reset)
+#define ISYS_CTRL_FSM_STATE_INFO_REG_RSTVAL              0
+#define ISYS_CTRL_CAPT_A_FSM_STATE_INFO_REG_RSTVAL       0
+#define ISYS_CTRL_CAPT_B_FSM_STATE_INFO_REG_RSTVAL       0
+#define ISYS_CTRL_CAPT_C_FSM_STATE_INFO_REG_RSTVAL       0
+#define ISYS_CTRL_ACQ_FSM_STATE_INFO_REG_RSTVAL          0
+#define ISYS_CTRL_CAPT_RESERVE_ONE_MEM_REGION_REG_RSTVAL 0
+
+/* register width value */
+#define ISYS_CTRL_CAPT_START_ADDR_A_REG_WIDTH            9
+#define ISYS_CTRL_CAPT_START_ADDR_B_REG_WIDTH            9
+#define ISYS_CTRL_CAPT_START_ADDR_C_REG_WIDTH            9
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_A_REG_WIDTH       9
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_B_REG_WIDTH       9
+#define ISYS_CTRL_CAPT_MEM_REGION_SIZE_C_REG_WIDTH       9
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_A_REG_WIDTH       9
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_B_REG_WIDTH       9
+#define ISYS_CTRL_CAPT_NUM_MEM_REGIONS_C_REG_WIDTH       9
+#define ISYS_CTRL_ACQ_START_ADDR_REG_WIDTH               9
+#define ISYS_CTRL_ACQ_MEM_REGION_SIZE_REG_WIDTH          9
+#define ISYS_CTRL_ACQ_NUM_MEM_REGIONS_REG_WIDTH          9
+#define ISYS_CTRL_INIT_REG_WIDTH                         3
+#define ISYS_CTRL_LAST_COMMAND_REG_WIDTH                 32    /* slave data width */
+#define ISYS_CTRL_NEXT_COMMAND_REG_WIDTH                 32
+#define ISYS_CTRL_LAST_ACKNOWLEDGE_REG_WIDTH             32
+#define ISYS_CTRL_NEXT_ACKNOWLEDGE_REG_WIDTH             32
+#define ISYS_CTRL_FSM_STATE_INFO_REG_WIDTH               32
+#define ISYS_CTRL_CAPT_A_FSM_STATE_INFO_REG_WIDTH        32
+#define ISYS_CTRL_CAPT_B_FSM_STATE_INFO_REG_WIDTH        32
+#define ISYS_CTRL_CAPT_C_FSM_STATE_INFO_REG_WIDTH        32
+#define ISYS_CTRL_ACQ_FSM_STATE_INFO_REG_WIDTH           32
+#define ISYS_CTRL_CAPT_RESERVE_ONE_MEM_REGION_REG_WIDTH  1
+
+/* bit definitions */
+
+/* --------------------------------------------------*/
+/* TOKEN INFO */
+/* --------------------------------------------------*/
+
+/*
+InpSysCaptFramesAcq  1/0  [3:0] - 'b0000
+[7:4] - CaptPortId,
+	   CaptA-'b0000
+	   CaptB-'b0001
+	   CaptC-'b0010
+[31:16] - NOF_frames
+InpSysCaptFrameExt  2/0  [3:0] - 'b0001'
+[7:4] - CaptPortId,
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+
+  2/1  [31:0] - external capture address
+InpSysAcqFrame  2/0  [3:0] - 'b0010,
+[31:4] - NOF_ext_mem_words
+  2/1  [31:0] - external memory read start address
+InpSysOverruleON  1/0  [3:0] - 'b0011,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+InpSysOverruleOFF  1/0  [3:0] - 'b0100,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+InpSysOverruleCmd  2/0  [3:0] - 'b0101,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+  2/1  [31:0] - command token value for port opid
+
+acknowledge tokens:
+
+InpSysAckCFA  1/0   [3:0] - 'b0000
+ [7:4] - CaptPortId,
+	   CaptA-'b0000
+	   CaptB- 'b0001
+	   CaptC-'b0010
+ [31:16] - NOF_frames
+InpSysAckCFE  1/0  [3:0] - 'b0001'
+[7:4] - CaptPortId,
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+
+InpSysAckAF  1/0  [3:0] - 'b0010
+InpSysAckOverruleON  1/0  [3:0] - 'b0011,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+InpSysAckOverruleOFF  1/0  [3:0] - 'b0100,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+InpSysAckOverrule  2/0  [3:0] - 'b0101,
+[7:4] - overrule port id (opid)
+	   'b0000 - CaptA
+	   'b0001 - CaptB
+	   'b0010 - CaptC
+	   'b0011 - Acq
+	   'b0100 - DMA
+
+  2/1  [31:0] - acknowledge token value from port opid
+
+*/
+
+/* Command and acknowledge tokens IDs */
+#define ISYS_CTRL_CAPT_FRAMES_ACQ_TOKEN_ID        0 /* 0000b */
+#define ISYS_CTRL_CAPT_FRAME_EXT_TOKEN_ID         1 /* 0001b */
+#define ISYS_CTRL_ACQ_FRAME_TOKEN_ID              2 /* 0010b */
+#define ISYS_CTRL_OVERRULE_ON_TOKEN_ID            3 /* 0011b */
+#define ISYS_CTRL_OVERRULE_OFF_TOKEN_ID           4 /* 0100b */
+#define ISYS_CTRL_OVERRULE_TOKEN_ID               5 /* 0101b */
+
+#define ISYS_CTRL_ACK_CFA_TOKEN_ID                0
+#define ISYS_CTRL_ACK_CFE_TOKEN_ID                1
+#define ISYS_CTRL_ACK_AF_TOKEN_ID                 2
+#define ISYS_CTRL_ACK_OVERRULE_ON_TOKEN_ID        3
+#define ISYS_CTRL_ACK_OVERRULE_OFF_TOKEN_ID       4
+#define ISYS_CTRL_ACK_OVERRULE_TOKEN_ID           5
+#define ISYS_CTRL_ACK_DEVICE_ERROR_TOKEN_ID       6
+
+#define ISYS_CTRL_TOKEN_ID_MSB                    3
+#define ISYS_CTRL_TOKEN_ID_LSB                    0
+#define ISYS_CTRL_PORT_ID_TOKEN_MSB               7
+#define ISYS_CTRL_PORT_ID_TOKEN_LSB               4
+#define ISYS_CTRL_NOF_CAPT_TOKEN_MSB              31
+#define ISYS_CTRL_NOF_CAPT_TOKEN_LSB              16
+#define ISYS_CTRL_NOF_EXT_TOKEN_MSB               31
+#define ISYS_CTRL_NOF_EXT_TOKEN_LSB               8
+
+#define ISYS_CTRL_TOKEN_ID_IDX                    0
+#define ISYS_CTRL_TOKEN_ID_BITS                   (ISYS_CTRL_TOKEN_ID_MSB - ISYS_CTRL_TOKEN_ID_LSB + 1)
+#define ISYS_CTRL_PORT_ID_IDX                     (ISYS_CTRL_TOKEN_ID_IDX + ISYS_CTRL_TOKEN_ID_BITS)
+#define ISYS_CTRL_PORT_ID_BITS                    (ISYS_CTRL_PORT_ID_TOKEN_MSB - ISYS_CTRL_PORT_ID_TOKEN_LSB + 1)
+#define ISYS_CTRL_NOF_CAPT_IDX                    ISYS_CTRL_NOF_CAPT_TOKEN_LSB
+#define ISYS_CTRL_NOF_CAPT_BITS                   (ISYS_CTRL_NOF_CAPT_TOKEN_MSB - ISYS_CTRL_NOF_CAPT_TOKEN_LSB + 1)
+#define ISYS_CTRL_NOF_EXT_IDX                     ISYS_CTRL_NOF_EXT_TOKEN_LSB
+#define ISYS_CTRL_NOF_EXT_BITS                    (ISYS_CTRL_NOF_EXT_TOKEN_MSB - ISYS_CTRL_NOF_EXT_TOKEN_LSB + 1)
+
+#define ISYS_CTRL_PORT_ID_CAPT_A                  0 /* device ID for capture unit A      */
+#define ISYS_CTRL_PORT_ID_CAPT_B                  1 /* device ID for capture unit B      */
+#define ISYS_CTRL_PORT_ID_CAPT_C                  2 /* device ID for capture unit C      */
+#define ISYS_CTRL_PORT_ID_ACQUISITION             3 /* device ID for acquistion unit     */
+#define ISYS_CTRL_PORT_ID_DMA_CAPT_A              4 /* device ID for dma unit            */
+#define ISYS_CTRL_PORT_ID_DMA_CAPT_B              5 /* device ID for dma unit            */
+#define ISYS_CTRL_PORT_ID_DMA_CAPT_C              6 /* device ID for dma unit            */
+#define ISYS_CTRL_PORT_ID_DMA_ACQ                 7 /* device ID for dma unit            */
+
+#define ISYS_CTRL_NO_ACQ_ACK                      16 /* no ack from acquisition unit */
+#define ISYS_CTRL_NO_DMA_ACK                      0
+#define ISYS_CTRL_NO_CAPT_ACK                     16
+
+#endif /* _input_system_ctrl_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/input_system_defs.h b/drivers/staging/media/atomisp/pci/input_system_defs.h
new file mode 100644
index 000000000..0c6a74b18
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_defs.h
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _input_system_defs_h
+#define _input_system_defs_h
+
+/* csi controller modes */
+#define HIVE_CSI_CONFIG_MAIN                   0
+#define HIVE_CSI_CONFIG_STEREO1                4
+#define HIVE_CSI_CONFIG_STEREO2                8
+
+/* general purpose register IDs */
+
+/* Stream Multicast select modes */
+#define HIVE_ISYS_GPREG_MULTICAST_A_IDX           0
+#define HIVE_ISYS_GPREG_MULTICAST_B_IDX           1
+#define HIVE_ISYS_GPREG_MULTICAST_C_IDX           2
+
+/* Stream Mux select modes */
+#define HIVE_ISYS_GPREG_MUX_IDX                   3
+
+/* streaming monitor status and control */
+#define HIVE_ISYS_GPREG_STRMON_STAT_IDX           4
+#define HIVE_ISYS_GPREG_STRMON_COND_IDX           5
+#define HIVE_ISYS_GPREG_STRMON_IRQ_EN_IDX         6
+#define HIVE_ISYS_GPREG_SRST_IDX                  7
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_IDX          8
+#define HIVE_ISYS_GPREG_REG_PORT_A_IDX            9
+#define HIVE_ISYS_GPREG_REG_PORT_B_IDX            10
+
+/* Bit numbers of the soft reset register */
+#define HIVE_ISYS_GPREG_SRST_CAPT_FIFO_A_BIT      0
+#define HIVE_ISYS_GPREG_SRST_CAPT_FIFO_B_BIT      1
+#define HIVE_ISYS_GPREG_SRST_CAPT_FIFO_C_BIT      2
+#define HIVE_ISYS_GPREG_SRST_MULTICAST_A_BIT      3
+#define HIVE_ISYS_GPREG_SRST_MULTICAST_B_BIT      4
+#define HIVE_ISYS_GPREG_SRST_MULTICAST_C_BIT      5
+#define HIVE_ISYS_GPREG_SRST_CAPT_A_BIT           6
+#define HIVE_ISYS_GPREG_SRST_CAPT_B_BIT           7
+#define HIVE_ISYS_GPREG_SRST_CAPT_C_BIT           8
+#define HIVE_ISYS_GPREG_SRST_ACQ_BIT              9
+/* For ISYS_CTRL 5bits are defined to allow soft-reset per sub-controller and top-ctrl */
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_BIT        10  /*LSB for 5bit vector */
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_CAPT_A_BIT 10
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_CAPT_B_BIT 11
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_CAPT_C_BIT 12
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_ACQ_BIT    13
+#define HIVE_ISYS_GPREG_SRST_ISYS_CTRL_TOP_BIT    14
+/* -- */
+#define HIVE_ISYS_GPREG_SRST_STR_MUX_BIT          15
+#define HIVE_ISYS_GPREG_SRST_CIO2AHB_BIT          16
+#define HIVE_ISYS_GPREG_SRST_GEN_SHORT_FIFO_BIT   17
+#define HIVE_ISYS_GPREG_SRST_WIDE_BUS_BIT         18 // includes CIO conv
+#define HIVE_ISYS_GPREG_SRST_DMA_BIT              19
+#define HIVE_ISYS_GPREG_SRST_SF_CTRL_CAPT_A_BIT   20
+#define HIVE_ISYS_GPREG_SRST_SF_CTRL_CAPT_B_BIT   21
+#define HIVE_ISYS_GPREG_SRST_SF_CTRL_CAPT_C_BIT   22
+#define HIVE_ISYS_GPREG_SRST_SF_CTRL_ACQ_BIT      23
+#define HIVE_ISYS_GPREG_SRST_CSI_BE_OUT_BIT       24
+
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_CAPT_A_BIT    0
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_CAPT_B_BIT    1
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_CAPT_C_BIT    2
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_ACQ_BIT       3
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_DMA_BIT        4
+#define HIVE_ISYS_GPREG_SLV_REG_SRST_ISYS_CTRL_BIT  5
+
+/* streaming monitor port id's */
+#define HIVE_ISYS_STR_MON_PORT_CAPA            0
+#define HIVE_ISYS_STR_MON_PORT_CAPB            1
+#define HIVE_ISYS_STR_MON_PORT_CAPC            2
+#define HIVE_ISYS_STR_MON_PORT_ACQ             3
+#define HIVE_ISYS_STR_MON_PORT_CSS_GENSH       4
+#define HIVE_ISYS_STR_MON_PORT_SF_GENSH        5
+#define HIVE_ISYS_STR_MON_PORT_SP2ISYS         6
+#define HIVE_ISYS_STR_MON_PORT_ISYS2SP         7
+#define HIVE_ISYS_STR_MON_PORT_PIXA            8
+#define HIVE_ISYS_STR_MON_PORT_PIXB            9
+
+/* interrupt bit ID's        */
+#define HIVE_ISYS_IRQ_CSI_SOF_BIT_ID           0
+#define HIVE_ISYS_IRQ_CSI_EOF_BIT_ID           1
+#define HIVE_ISYS_IRQ_CSI_SOL_BIT_ID           2
+#define HIVE_ISYS_IRQ_CSI_EOL_BIT_ID           3
+#define HIVE_ISYS_IRQ_CSI_RECEIVER_BIT_ID      4
+#define HIVE_ISYS_IRQ_CSI_RECEIVER_BE_BIT_ID   5
+#define HIVE_ISYS_IRQ_CAP_UNIT_A_NO_SOP        6
+#define HIVE_ISYS_IRQ_CAP_UNIT_A_LATE_SOP      7
+/*#define HIVE_ISYS_IRQ_CAP_UNIT_A_UNDEF_PH      7*/
+#define HIVE_ISYS_IRQ_CAP_UNIT_B_NO_SOP        8
+#define HIVE_ISYS_IRQ_CAP_UNIT_B_LATE_SOP      9
+/*#define HIVE_ISYS_IRQ_CAP_UNIT_B_UNDEF_PH     10*/
+#define HIVE_ISYS_IRQ_CAP_UNIT_C_NO_SOP       10
+#define HIVE_ISYS_IRQ_CAP_UNIT_C_LATE_SOP     11
+/*#define HIVE_ISYS_IRQ_CAP_UNIT_C_UNDEF_PH     13*/
+#define HIVE_ISYS_IRQ_ACQ_UNIT_SOP_MISMATCH   12
+/*#define HIVE_ISYS_IRQ_ACQ_UNIT_UNDEF_PH       15*/
+#define HIVE_ISYS_IRQ_INP_CTRL_CAPA           13
+#define HIVE_ISYS_IRQ_INP_CTRL_CAPB           14
+#define HIVE_ISYS_IRQ_INP_CTRL_CAPC           15
+#define HIVE_ISYS_IRQ_CIO2AHB                 16
+#define HIVE_ISYS_IRQ_DMA_BIT_ID              17
+#define HIVE_ISYS_IRQ_STREAM_MON_BIT_ID       18
+#define HIVE_ISYS_IRQ_NUM_BITS                19
+
+/* DMA */
+#define HIVE_ISYS_DMA_CHANNEL                  0
+#define HIVE_ISYS_DMA_IBUF_DDR_CONN            0
+#define HIVE_ISYS_DMA_HEIGHT                   1
+#define HIVE_ISYS_DMA_ELEMS                    1 /* both master buses of same width */
+#define HIVE_ISYS_DMA_STRIDE                   0 /* no stride required as height is fixed to 1 */
+#define HIVE_ISYS_DMA_CROP                     0 /* no cropping */
+#define HIVE_ISYS_DMA_EXTENSION                0 /* no extension as elem width is same on both side */
+
+#endif /* _input_system_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/input_system_global.h b/drivers/staging/media/atomisp/pci/input_system_global.h
new file mode 100644
index 000000000..145096444
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_global.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *    (c) 2020 Mauro Carvalho Chehab <mchehab+huawei@kernel.org>
+ */
+
+
+#ifndef __INPUT_SYSTEM_GLOBAL_H_INCLUDED__
+#define __INPUT_SYSTEM_GLOBAL_H_INCLUDED__
+typedef enum {
+	INPUT_SYSTEM_ERR_NO_ERROR = 0,
+	/* ISP2401 */
+	INPUT_SYSTEM_ERR_CREATE_CHANNEL_FAIL,
+	INPUT_SYSTEM_ERR_CONFIGURE_CHANNEL_FAIL,
+	INPUT_SYSTEM_ERR_OPEN_CHANNEL_FAIL,
+	INPUT_SYSTEM_ERR_TRANSFER_FAIL,
+	INPUT_SYSTEM_ERR_CREATE_INPUT_PORT_FAIL,
+	INPUT_SYSTEM_ERR_CONFIGURE_INPUT_PORT_FAIL,
+	INPUT_SYSTEM_ERR_OPEN_INPUT_PORT_FAIL,
+	/* ISP2400 */
+	INPUT_SYSTEM_ERR_GENERIC,
+	INPUT_SYSTEM_ERR_CHANNEL_ALREADY_SET,
+	INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE,
+	INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED,
+} input_system_err_t;
+
+#include "isp2401_input_system_global.h"
+#include "isp2400_input_system_global.h"
+
+#endif /* __INPUT_SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/input_system_local.h b/drivers/staging/media/atomisp/pci/input_system_local.h
new file mode 100644
index 000000000..357987d62
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_local.h
@@ -0,0 +1,145 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *    (c) 2020 Mauro Carvalho Chehab <mchehab+huawei@kernel.org>
+ */
+
+#include "type_support.h"
+#include "input_system_global.h"
+
+typedef enum {
+	INPUT_SYSTEM_PORT_A = 0,
+	INPUT_SYSTEM_PORT_B,
+	INPUT_SYSTEM_PORT_C,
+	N_INPUT_SYSTEM_PORTS
+} input_system_csi_port_t;
+
+typedef struct ctrl_unit_cfg_s			ctrl_unit_cfg_t;
+typedef struct input_system_network_cfg_s	input_system_network_cfg_t;
+typedef struct target_cfg2400_s		target_cfg2400_t;
+typedef struct channel_cfg_s			channel_cfg_t;
+typedef struct backend_channel_cfg_s		backend_channel_cfg_t;
+typedef struct input_system_cfg2400_s		input_system_cfg2400_t;
+typedef struct mipi_port_state_s		mipi_port_state_t;
+typedef struct rx_channel_state_s		rx_channel_state_t;
+typedef struct input_switch_cfg_channel_s	input_switch_cfg_channel_t;
+typedef struct input_switch_cfg_s		input_switch_cfg_t;
+
+struct ctrl_unit_cfg_s {
+	isp2400_ib_buffer_t		buffer_mipi[N_CAPTURE_UNIT_ID];
+	isp2400_ib_buffer_t		buffer_acquire[N_ACQUISITION_UNIT_ID];
+};
+
+struct input_system_network_cfg_s {
+	input_system_connection_t	multicast_cfg[N_CAPTURE_UNIT_ID];
+	input_system_multiplex_t	mux_cfg;
+	ctrl_unit_cfg_t				ctrl_unit_cfg[N_CTRL_UNIT_ID];
+};
+
+typedef struct {
+// TBD.
+	u32	dummy_parameter;
+} target_isp_cfg_t;
+
+typedef struct {
+// TBD.
+	u32	dummy_parameter;
+} target_sp_cfg_t;
+
+typedef struct {
+// TBD.
+	u32	dummy_parameter;
+} target_strm2mem_cfg_t;
+
+struct input_switch_cfg_channel_s {
+	u32 hsync_data_reg[2];
+	u32 vsync_data_reg;
+};
+
+struct backend_channel_cfg_s {
+	u32	fmt_control_word_1; // Format config.
+	u32	fmt_control_word_2;
+	u32	no_side_band;
+};
+
+typedef union  {
+	csi_cfg_t	csi_cfg;
+	tpg_cfg_t	tpg_cfg;
+	prbs_cfg_t	prbs_cfg;
+	gpfifo_cfg_t	gpfifo_cfg;
+} source_cfg_t;
+
+struct input_switch_cfg_s {
+	u32 hsync_data_reg[N_RX_CHANNEL_ID * 2];
+	u32 vsync_data_reg;
+};
+
+/*
+ * In 2300 ports can be configured independently and stream
+ * formats need to be specified. In 2400, there are only 8
+ * supported configurations but the HW is fused to support
+ * only a single one.
+ *
+ * In 2300 the compressed format types are programmed by the
+ * user. In 2400 all stream formats are encoded on the stream.
+ *
+ * Use the enum to check validity of a user configuration
+ */
+typedef enum {
+	MONO_4L_1L_0L = 0,
+	MONO_3L_1L_0L,
+	MONO_2L_1L_0L,
+	MONO_1L_1L_0L,
+	STEREO_2L_1L_2L,
+	STEREO_3L_1L_1L,
+	STEREO_2L_1L_1L,
+	STEREO_1L_1L_1L,
+	N_RX_MODE
+} rx_mode_t;
+
+#define UNCOMPRESSED_BITS_PER_PIXEL_10	10
+#define UNCOMPRESSED_BITS_PER_PIXEL_12	12
+#define COMPRESSED_BITS_PER_PIXEL_6	6
+#define COMPRESSED_BITS_PER_PIXEL_7	7
+#define COMPRESSED_BITS_PER_PIXEL_8	8
+enum mipi_compressor {
+	MIPI_COMPRESSOR_NONE = 0,
+	MIPI_COMPRESSOR_10_6_10,
+	MIPI_COMPRESSOR_10_7_10,
+	MIPI_COMPRESSOR_10_8_10,
+	MIPI_COMPRESSOR_12_6_12,
+	MIPI_COMPRESSOR_12_7_12,
+	MIPI_COMPRESSOR_12_8_12,
+	N_MIPI_COMPRESSOR_METHODS
+};
+
+typedef enum mipi_compressor mipi_compressor_t;
+
+typedef enum {
+	MIPI_PREDICTOR_NONE = 0,
+	MIPI_PREDICTOR_TYPE1,
+	MIPI_PREDICTOR_TYPE2,
+	N_MIPI_PREDICTOR_TYPES
+} mipi_predictor_t;
+
+typedef struct rx_cfg_s		rx_cfg_t;
+
+/*
+ * Applied per port
+ */
+struct rx_cfg_s {
+	rx_mode_t			mode;	/* The HW config */
+	enum mipi_port_id		port;	/* The port ID to apply the control on */
+	unsigned int		timeout;
+	unsigned int		initcount;
+	unsigned int		synccount;
+	unsigned int		rxcount;
+	mipi_predictor_t	comp;	/* Just for backward compatibility */
+	bool                is_two_ppc;
+};
+
+#ifdef ISP2401
+#  include "isp2401_input_system_local.h"
+#else
+#  include "isp2400_input_system_local.h"
+#endif
diff --git a/drivers/staging/media/atomisp/pci/input_system_private.h b/drivers/staging/media/atomisp/pci/input_system_private.h
new file mode 100644
index 000000000..889f204e7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_private.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *    (c) 2020 Mauro Carvalho Chehab <mchehab+huawei@kernel.org>
+ */
+
+#ifdef ISP2401
+#  include "isp2401_input_system_private.h"
+#else
+#  include "isp2400_input_system_private.h"
+#endif
diff --git a/drivers/staging/media/atomisp/pci/input_system_public.h b/drivers/staging/media/atomisp/pci/input_system_public.h
new file mode 100644
index 000000000..3f5167fd6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/input_system_public.h
@@ -0,0 +1,9 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *    (c) 2020 Mauro Carvalho Chehab <mchehab+huawei@kernel.org>
+ */
+
+#ifndef ISP2401
+#  include "isp2400_input_system_public.h"
+#endif
diff --git a/drivers/staging/media/atomisp/pci/irq_controller_defs.h b/drivers/staging/media/atomisp/pci/irq_controller_defs.h
new file mode 100644
index 000000000..e49e61e17
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/irq_controller_defs.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _irq_controller_defs_h
+#define _irq_controller_defs_h
+
+#define _HRT_IRQ_CONTROLLER_EDGE_REG_IDX           0
+#define _HRT_IRQ_CONTROLLER_MASK_REG_IDX           1
+#define _HRT_IRQ_CONTROLLER_STATUS_REG_IDX         2
+#define _HRT_IRQ_CONTROLLER_CLEAR_REG_IDX          3
+#define _HRT_IRQ_CONTROLLER_ENABLE_REG_IDX         4
+#define _HRT_IRQ_CONTROLLER_EDGE_NOT_PULSE_REG_IDX 5
+#define _HRT_IRQ_CONTROLLER_STR_OUT_ENABLE_REG_IDX 6
+
+#define _HRT_IRQ_CONTROLLER_REG_ALIGN 4
+
+#endif /* _irq_controller_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/irq_types_hrt.h b/drivers/staging/media/atomisp/pci/irq_types_hrt.h
new file mode 100644
index 000000000..4212bb01c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/irq_types_hrt.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _HIVE_ISP_CSS_IRQ_TYPES_HRT_H_
+#define _HIVE_ISP_CSS_IRQ_TYPES_HRT_H_
+
+/*
+ * These are the indices of each interrupt in the interrupt
+ * controller's registers. these can be used as the irq_id
+ * argument to the hrt functions irq_controller.h.
+ *
+ * The definitions are taken from <system>_defs.h
+ */
+typedef enum hrt_isp_css_irq {
+	hrt_isp_css_irq_gpio_pin_0           = HIVE_GP_DEV_IRQ_GPIO_PIN_0_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_1           = HIVE_GP_DEV_IRQ_GPIO_PIN_1_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_2           = HIVE_GP_DEV_IRQ_GPIO_PIN_2_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_3           = HIVE_GP_DEV_IRQ_GPIO_PIN_3_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_4           = HIVE_GP_DEV_IRQ_GPIO_PIN_4_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_5           = HIVE_GP_DEV_IRQ_GPIO_PIN_5_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_6           = HIVE_GP_DEV_IRQ_GPIO_PIN_6_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_7           = HIVE_GP_DEV_IRQ_GPIO_PIN_7_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_8           = HIVE_GP_DEV_IRQ_GPIO_PIN_8_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_9           = HIVE_GP_DEV_IRQ_GPIO_PIN_9_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_10          = HIVE_GP_DEV_IRQ_GPIO_PIN_10_BIT_ID,
+	hrt_isp_css_irq_gpio_pin_11          = HIVE_GP_DEV_IRQ_GPIO_PIN_11_BIT_ID,
+	hrt_isp_css_irq_sp                   = HIVE_GP_DEV_IRQ_SP_BIT_ID,
+	hrt_isp_css_irq_isp                  = HIVE_GP_DEV_IRQ_ISP_BIT_ID,
+	hrt_isp_css_irq_isys                 = HIVE_GP_DEV_IRQ_ISYS_BIT_ID,
+	hrt_isp_css_irq_isel                 = HIVE_GP_DEV_IRQ_ISEL_BIT_ID,
+	hrt_isp_css_irq_ifmt                 = HIVE_GP_DEV_IRQ_IFMT_BIT_ID,
+	hrt_isp_css_irq_sp_stream_mon        = HIVE_GP_DEV_IRQ_SP_STREAM_MON_BIT_ID,
+	hrt_isp_css_irq_isp_stream_mon       = HIVE_GP_DEV_IRQ_ISP_STREAM_MON_BIT_ID,
+	hrt_isp_css_irq_mod_stream_mon       = HIVE_GP_DEV_IRQ_MOD_STREAM_MON_BIT_ID,
+	hrt_isp_css_irq_isp_pmem_error       = HIVE_GP_DEV_IRQ_ISP_PMEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_isp_bamem_error      = HIVE_GP_DEV_IRQ_ISP_BAMEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_isp_dmem_error       = HIVE_GP_DEV_IRQ_ISP_DMEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_sp_icache_mem_error  = HIVE_GP_DEV_IRQ_SP_ICACHE_MEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_sp_dmem_error        = HIVE_GP_DEV_IRQ_SP_DMEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_mmu_cache_mem_error  = HIVE_GP_DEV_IRQ_MMU_CACHE_MEM_ERROR_BIT_ID,
+	hrt_isp_css_irq_gp_timer_0           = HIVE_GP_DEV_IRQ_GP_TIMER_0_BIT_ID,
+	hrt_isp_css_irq_gp_timer_1           = HIVE_GP_DEV_IRQ_GP_TIMER_1_BIT_ID,
+	hrt_isp_css_irq_sw_pin_0             = HIVE_GP_DEV_IRQ_SW_PIN_0_BIT_ID,
+	hrt_isp_css_irq_sw_pin_1             = HIVE_GP_DEV_IRQ_SW_PIN_1_BIT_ID,
+	hrt_isp_css_irq_dma                  = HIVE_GP_DEV_IRQ_DMA_BIT_ID,
+	hrt_isp_css_irq_sp_stream_mon_b      = HIVE_GP_DEV_IRQ_SP_STREAM_MON_B_BIT_ID,
+	/* this must (obviously) be the last on in the enum */
+	hrt_isp_css_irq_num_irqs
+} hrt_isp_css_irq_t;
+
+typedef enum hrt_isp_css_irq_status {
+	hrt_isp_css_irq_status_error,
+	hrt_isp_css_irq_status_more_irqs,
+	hrt_isp_css_irq_status_success
+} hrt_isp_css_irq_status_t;
+
+#endif /* _HIVE_ISP_CSS_IRQ_TYPES_HRT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.c
new file mode 100644
index 000000000..daf2f25c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.c
@@ -0,0 +1,32 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+
+#include "ia_css_aa2.host.h"
+
+/* YUV Anti-Aliasing configuration. */
+const struct ia_css_aa_config default_aa_config = {
+	8191 /* default should be 0 */
+};
+
+/* Bayer Anti-Aliasing configuration. */
+const struct ia_css_aa_config default_baa_config = {
+	8191 /* default should be 0 */
+};
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.h
new file mode 100644
index 000000000..3abc125de
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2.host.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_AA_HOST_H
+#define __IA_CSS_AA_HOST_H
+
+#include "ia_css_aa2_types.h"
+#include "ia_css_aa2_param.h"
+
+/* YUV Anti-Aliasing configuration. */
+extern const struct ia_css_aa_config default_aa_config;
+
+/* Bayer Anti-Aliasing configuration. */
+extern const struct ia_css_aa_config default_baa_config;
+
+#endif /* __IA_CSS_AA_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_param.h
new file mode 100644
index 000000000..4f8bb4de4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_param.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_AA_PARAM_H
+#define __IA_CSS_AA_PARAM_H
+
+#include "type_support.h"
+
+struct sh_css_isp_aa_params {
+	s32 strength;
+};
+
+#endif /* __IA_CSS_AA_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_types.h
new file mode 100644
index 000000000..900ba8f5e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/aa/aa_2/ia_css_aa2_types.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_AA2_TYPES_H
+#define __IA_CSS_AA2_TYPES_H
+
+/* @file
+* CSS-API header file for Anti-Aliasing parameters.
+*/
+
+/* Anti-Aliasing configuration.
+ *
+ *  This structure is used both for YUV AA and Bayer AA.
+ *
+ *  1. YUV Anti-Aliasing
+ *     struct ia_css_aa_config   *aa_config
+ *
+ *     ISP block: AA2
+ *    (ISP1: AA2 is not used.)
+ *     ISP2: AA2 should be used. But, AA2 is not used currently.
+ *
+ *  2. Bayer Anti-Aliasing
+ *     struct ia_css_aa_config   *baa_config
+ *
+ *     ISP block: BAA2
+ *     ISP1: BAA2 is used.
+ *     ISP2: BAA2 is used.
+ */
+struct ia_css_aa_config {
+	u16 strength;	/** Strength of the filter.
+					u0.13, [0,8191],
+					default/ineffective 0 */
+};
+
+#endif /* __IA_CSS_AA2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.c
new file mode 100644
index 000000000..3f079c954
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.c
@@ -0,0 +1,62 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_anr.host.h"
+
+const struct ia_css_anr_config default_anr_config = {
+	10,
+	{
+		0, 3, 1, 2, 3, 6, 4, 5, 1, 4, 2, 3, 2, 5, 3, 4,
+		0, 3, 1, 2, 3, 6, 4, 5, 1, 4, 2, 3, 2, 5, 3, 4,
+		0, 3, 1, 2, 3, 6, 4, 5, 1, 4, 2, 3, 2, 5, 3, 4,
+		0, 3, 1, 2, 3, 6, 4, 5, 1, 4, 2, 3, 2, 5, 3, 4
+	},
+	{10, 20, 30}
+};
+
+void
+ia_css_anr_encode(
+    struct sh_css_isp_anr_params *to,
+    const struct ia_css_anr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->threshold = from->threshold;
+}
+
+void
+ia_css_anr_dump(
+    const struct sh_css_isp_anr_params *anr,
+    unsigned int level)
+{
+	if (!anr) return;
+	ia_css_debug_dtrace(level, "Advance Noise Reduction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "anr_threshold", anr->threshold);
+}
+
+void
+ia_css_anr_debug_dtrace(
+    const struct ia_css_anr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.threshold=%d\n",
+			    config->threshold);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.h
new file mode 100644
index 000000000..a4720c4a9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr.host.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR_HOST_H
+#define __IA_CSS_ANR_HOST_H
+
+#include "ia_css_anr_types.h"
+#include "ia_css_anr_param.h"
+
+extern const struct ia_css_anr_config default_anr_config;
+
+void
+ia_css_anr_encode(
+    struct sh_css_isp_anr_params *to,
+    const struct ia_css_anr_config *from,
+    unsigned int size);
+
+void
+ia_css_anr_dump(
+    const struct sh_css_isp_anr_params *anr,
+    unsigned int level);
+
+void
+ia_css_anr_debug_dtrace(
+    const struct ia_css_anr_config *config, unsigned int level)
+;
+
+#endif /* __IA_CSS_ANR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_param.h
new file mode 100644
index 000000000..37dcb013b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_param.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR_PARAM_H
+#define __IA_CSS_ANR_PARAM_H
+
+#include "type_support.h"
+
+/* ANR (Advanced Noise Reduction) */
+struct sh_css_isp_anr_params {
+	s32 threshold;
+};
+
+#endif /* __IA_CSS_ANR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_types.h
new file mode 100644
index 000000000..9b22f2da4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_1.0/ia_css_anr_types.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR_TYPES_H
+#define __IA_CSS_ANR_TYPES_H
+
+/* @file
+ * CSS-API header file for Advanced Noise Reduction kernel v1
+ */
+
+/* Application specific DMA settings  */
+#define ANR_BPP                 10
+#define ANR_ELEMENT_BITS        ((CEIL_DIV(ANR_BPP, 8)) * 8)
+
+/* Advanced Noise Reduction configuration.
+ *  This is also known as Low-Light.
+ */
+struct ia_css_anr_config {
+	s32 threshold; /** Threshold */
+	s32 thresholds[4 * 4 * 4];
+	s32 factors[3];
+};
+
+#endif /* __IA_CSS_ANR_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.c
new file mode 100644
index 000000000..9cdefedc6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_anr2.host.h"
+
+void
+ia_css_anr2_vmem_encode(
+    struct ia_css_isp_anr2_params *to,
+    const struct ia_css_anr_thres *from,
+    size_t size)
+{
+	unsigned int i;
+
+	(void)size;
+	for (i = 0; i < ANR_PARAM_SIZE; i++) {
+		unsigned int j;
+
+		for (j = 0; j < ISP_VEC_NELEMS; j++) {
+			to->data[i][j] = from->data[i * ISP_VEC_NELEMS + j];
+		}
+	}
+}
+
+void
+ia_css_anr2_debug_dtrace(
+    const struct ia_css_anr_thres *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.h
new file mode 100644
index 000000000..2b3ab01c2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2.host.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR2_HOST_H
+#define __IA_CSS_ANR2_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_anr2_types.h"
+#include "ia_css_anr2_param.h"
+#include "ia_css_anr2_table.host.h"
+
+void
+ia_css_anr2_vmem_encode(
+    struct ia_css_isp_anr2_params *to,
+    const struct ia_css_anr_thres *from,
+    size_t size);
+
+void
+ia_css_anr2_debug_dtrace(
+    const struct ia_css_anr_thres *config, unsigned int level)
+;
+
+#endif /* __IA_CSS_ANR2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_param.h
new file mode 100644
index 000000000..4b83b8100
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_param.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR2_PARAM_H
+#define __IA_CSS_ANR2_PARAM_H
+
+#include "vmem.h"
+#include "ia_css_anr2_types.h"
+
+/* Advanced Noise Reduction (ANR) thresholds */
+
+struct ia_css_isp_anr2_params {
+	VMEM_ARRAY(data, ANR_PARAM_SIZE * ISP_VEC_NELEMS);
+};
+
+#endif /* __IA_CSS_ANR2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c
new file mode 100644
index 000000000..649283bd4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c
@@ -0,0 +1,56 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+#include "ia_css_types.h"
+#include "ia_css_anr2_table.host.h"
+
+#if 1
+const struct ia_css_anr_thres default_anr_thres = {
+	{
+		128, 384, 640, 896, 896, 640, 384, 128, 384, 1152, 1920, 2688, 2688, 1920, 1152, 384, 640, 1920, 3200, 4480, 4480, 3200, 1920, 640, 896, 2688, 4480, 6272, 6272, 4480, 2688, 896, 896, 2688, 4480, 6272, 6272, 4480, 2688, 896, 640, 1920, 3200, 4480, 4480, 3200, 1920, 640, 384, 1152, 1920, 2688, 2688, 1920, 1152, 384, 128, 384, 640, 896, 896, 640, 384, 128,
+		0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20, 0, 0, 30, 30, 10, 10, 20, 20,
+		0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40, 0, 0, 60, 60, 20, 20, 40, 40,
+		0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60, 0, 0, 90, 90, 30, 30, 60, 60,
+		30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50, 30, 30, 60, 60, 40, 40, 50, 50,
+		60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100, 60, 60, 120, 120, 80, 80, 100, 100,
+		90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150, 90, 90, 180, 180, 120, 120, 150, 150,
+		10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30, 10, 10, 40, 40, 20, 20, 30, 30,
+		20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60, 20, 20, 80, 80, 40, 40, 60, 60,
+		30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90, 30, 30, 120, 120, 60, 60, 90, 90,
+		20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40, 20, 20, 50, 50, 30, 30, 40, 40,
+		40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80, 40, 40, 100, 100, 60, 60, 80, 80,
+		60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120, 60, 60, 150, 150, 90, 90, 120, 120
+	}
+};
+#else
+const struct ia_css_anr_thres default_anr_thres = {
+	{
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	}
+};
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h
new file mode 100644
index 000000000..9e383e030
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR2_TABLE_HOST_H
+#define __IA_CSS_ANR2_TABLE_HOST_H
+
+#include "ia_css_anr2_types.h"
+
+extern const struct ia_css_anr_thres default_anr_thres;
+
+#endif /* __IA_CSS_ANR2_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_types.h
new file mode 100644
index 000000000..e12aae819
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/anr/anr_2/ia_css_anr2_types.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ANR2_TYPES_H
+#define __IA_CSS_ANR2_TYPES_H
+
+/* @file
+* CSS-API header file for Advanced Noise Reduction kernel v2
+*/
+
+#include "type_support.h"
+
+#define ANR_PARAM_SIZE          13
+
+/* Advanced Noise Reduction (ANR) thresholds */
+struct ia_css_anr_thres {
+	s16 data[13 * 64];
+};
+
+#endif /* __IA_CSS_ANR2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.c
new file mode 100644
index 000000000..2091f0015
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.c
@@ -0,0 +1,64 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_internal.h"
+#include "assert_support.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_bh.host.h"
+
+void
+ia_css_bh_hmem_decode(
+    struct ia_css_3a_rgby_output *out_ptr,
+    const struct ia_css_bh_table *hmem_buf)
+{
+	int i;
+
+	/*
+	 * No weighted histogram, hence no grid definition
+	 */
+	if (!hmem_buf)
+		return;
+	assert(sizeof_hmem(HMEM0_ID) == sizeof(*hmem_buf));
+
+	/* Deinterleave */
+	for (i = 0; i < HMEM_UNIT_SIZE; i++) {
+		out_ptr[i].r = hmem_buf->hmem[BH_COLOR_R][i];
+		out_ptr[i].g = hmem_buf->hmem[BH_COLOR_G][i];
+		out_ptr[i].b = hmem_buf->hmem[BH_COLOR_B][i];
+		out_ptr[i].y = hmem_buf->hmem[BH_COLOR_Y][i];
+		/* sh_css_print ("hmem[%d] = %d, %d, %d, %d\n",
+			i, out_ptr[i].r, out_ptr[i].g, out_ptr[i].b, out_ptr[i].y); */
+	}
+}
+
+void
+ia_css_bh_encode(
+    struct sh_css_isp_bh_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size)
+{
+	(void)size;
+	/* coefficients to calculate Y */
+	to->y_coef_r =
+	    uDIGIT_FITTING(from->ae_y_coef_r, 16, SH_CSS_AE_YCOEF_SHIFT);
+	to->y_coef_g =
+	    uDIGIT_FITTING(from->ae_y_coef_g, 16, SH_CSS_AE_YCOEF_SHIFT);
+	to->y_coef_b =
+	    uDIGIT_FITTING(from->ae_y_coef_b, 16, SH_CSS_AE_YCOEF_SHIFT);
+}
+
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.h
new file mode 100644
index 000000000..736b6e3f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh.host.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BH_HOST_H
+#define __IA_CSS_BH_HOST_H
+
+#include "ia_css_bh_param.h"
+#include "s3a/s3a_1.0/ia_css_s3a_types.h"
+
+void
+ia_css_bh_hmem_decode(
+    struct ia_css_3a_rgby_output *out_ptr,
+    const struct ia_css_bh_table *hmem_buf);
+
+void
+ia_css_bh_encode(
+    struct sh_css_isp_bh_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size);
+
+#endif /* __IA_CSS_BH_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_param.h
new file mode 100644
index 000000000..05d5c43e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_param.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_HB_PARAM_H
+#define __IA_CSS_HB_PARAM_H
+
+#include "type_support.h"
+
+#ifndef PIPE_GENERATION
+#define __INLINE_HMEM__
+#include "hmem.h"
+#endif
+
+#include "ia_css_bh_types.h"
+
+/* AE (3A Support) */
+struct sh_css_isp_bh_params {
+	/* coefficients to calculate Y */
+	s32 y_coef_r;
+	s32 y_coef_g;
+	s32 y_coef_b;
+};
+
+/* This should be hmem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_bh_hmem_params {
+	u32 bh[ISP_HIST_COMPONENTS][IA_CSS_HMEM_BH_UNIT_SIZE];
+};
+
+#endif /* __IA_CSS_HB_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_types.h
new file mode 100644
index 000000000..4c0e92f13
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bh/bh_2/ia_css_bh_types.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BH_TYPES_H
+#define __IA_CSS_BH_TYPES_H
+
+/* Number of elements in the BH table.
+  * Should be consistent with hmem.h
+  */
+#define IA_CSS_HMEM_BH_TABLE_SIZE	ISP_HIST_DEPTH
+#define IA_CSS_HMEM_BH_UNIT_SIZE	(ISP_HIST_DEPTH / ISP_HIST_COMPONENTS)
+
+#define BH_COLOR_R	(0)
+#define BH_COLOR_G	(1)
+#define BH_COLOR_B	(2)
+#define BH_COLOR_Y	(3)
+#define BH_COLOR_NUM	(4)
+
+/* BH table */
+struct ia_css_bh_table {
+	u32 hmem[ISP_HIST_COMPONENTS][IA_CSS_HMEM_BH_UNIT_SIZE];
+};
+
+#endif /* __IA_CSS_BH_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.c
new file mode 100644
index 000000000..45e37dc4f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.c
@@ -0,0 +1,197 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "type_support.h"
+#include "ia_css_bnlm.host.h"
+
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h" /* ia_css_debug_dtrace() */
+#endif
+#include <assert_support.h>
+
+#define BNLM_DIV_LUT_SIZE	(12)
+static const s32 div_lut_nearests[BNLM_DIV_LUT_SIZE] = {
+	0, 454, 948, 1484, 2070, 2710, 3412, 4184, 5035, 5978, 7025, 8191
+};
+
+static const s32 div_lut_slopes[BNLM_DIV_LUT_SIZE] = {
+	-7760, -6960, -6216, -5536, -4912, -4344, -3832, -3360, -2936, -2552, -2208, -2208
+    };
+
+static const s32 div_lut_intercepts[BNLM_DIV_LUT_SIZE] = {
+	8184, 7752, 7336, 6928, 6536, 6152, 5776, 5416, 5064, 4728, 4408, 4408
+};
+
+/* Encodes a look-up table from BNLM public parameters to vmem parameters.
+ * Input:
+ *	lut	:	bnlm_lut struct containing encoded vmem parameters look-up table
+ *	lut_thr	:	array containing threshold values for lut
+ *	lut_val	:	array containing output values related to lut_thr
+ *	lut_size:	Size of lut_val array
+ */
+static inline void
+bnlm_lut_encode(struct bnlm_lut *lut, const int32_t *lut_thr,
+		const s32 *lut_val, const uint32_t lut_size)
+{
+	u32 blk, i;
+	const u32 block_size = 16;
+	const u32 total_blocks = ISP_VEC_NELEMS / block_size;
+
+	/* Create VMEM LUTs from the threshold and value arrays.
+	 *
+	 * Min size of the LUT is 2 entries.
+	 *
+	 * Max size of the LUT is 16 entries, so that the LUT can fit into a
+	 * single group of 16 elements inside a vector.
+	 * Then these elements are copied into other groups inside the same
+	 * vector. If the LUT size is less than 16, then remaining elements are
+	 * set to 0.
+	 */
+	assert((lut_size >= 2) && (lut_size <= block_size));
+	/* array lut_thr has (lut_size-1) entries */
+	for (i = 0; i < lut_size - 2; i++) {
+		/* Check if the lut_thr is monotonically increasing */
+		assert(lut_thr[i] <= lut_thr[i + 1]);
+	}
+
+	/* Initialize */
+	for (i = 0; i < total_blocks * block_size; i++) {
+		lut->thr[0][i] = 0;
+		lut->val[0][i] = 0;
+	}
+
+	/* Copy all data */
+	for (i = 0; i < lut_size - 1; i++) {
+		lut->thr[0][i] = lut_thr[i];
+		lut->val[0][i] = lut_val[i];
+	}
+	lut->val[0][i] = lut_val[i]; /* val has one more element than thr */
+
+	/* Copy data from first block to all blocks */
+	for (blk = 1; blk < total_blocks; blk++) {
+		u32 blk_offset = blk * block_size;
+
+		for (i = 1; i < lut_size; i++) {
+			lut->thr[0][blk_offset + i] = lut->thr[0][i];
+			lut->val[0][blk_offset + i] = lut->val[0][i];
+		}
+	}
+}
+
+/*
+ * - Encodes BNLM public parameters into VMEM parameters
+ * - Generates VMEM parameters which will needed internally ISP
+ */
+void
+ia_css_bnlm_vmem_encode(
+    struct bnlm_vmem_params *to,
+    const struct ia_css_bnlm_config *from,
+    size_t size)
+{
+	int i;
+	(void)size;
+
+	/* Initialize LUTs in VMEM parameters */
+	bnlm_lut_encode(&to->mu_root_lut, from->mu_root_lut_thr, from->mu_root_lut_val,
+			16);
+	bnlm_lut_encode(&to->sad_norm_lut, from->sad_norm_lut_thr,
+			from->sad_norm_lut_val, 16);
+	bnlm_lut_encode(&to->sig_detail_lut, from->sig_detail_lut_thr,
+			from->sig_detail_lut_val, 16);
+	bnlm_lut_encode(&to->sig_rad_lut, from->sig_rad_lut_thr, from->sig_rad_lut_val,
+			16);
+	bnlm_lut_encode(&to->rad_pow_lut, from->rad_pow_lut_thr, from->rad_pow_lut_val,
+			16);
+	bnlm_lut_encode(&to->nl_0_lut, from->nl_0_lut_thr, from->nl_0_lut_val, 16);
+	bnlm_lut_encode(&to->nl_1_lut, from->nl_1_lut_thr, from->nl_1_lut_val, 16);
+	bnlm_lut_encode(&to->nl_2_lut, from->nl_2_lut_thr, from->nl_2_lut_val, 16);
+	bnlm_lut_encode(&to->nl_3_lut, from->nl_3_lut_thr, from->nl_3_lut_val, 16);
+
+	/* Initialize arrays in VMEM parameters */
+	memset(to->nl_th, 0, sizeof(to->nl_th));
+	to->nl_th[0][0] = from->nl_th[0];
+	to->nl_th[0][1] = from->nl_th[1];
+	to->nl_th[0][2] = from->nl_th[2];
+
+	memset(to->match_quality_max_idx, 0, sizeof(to->match_quality_max_idx));
+	to->match_quality_max_idx[0][0] = from->match_quality_max_idx[0];
+	to->match_quality_max_idx[0][1] = from->match_quality_max_idx[1];
+	to->match_quality_max_idx[0][2] = from->match_quality_max_idx[2];
+	to->match_quality_max_idx[0][3] = from->match_quality_max_idx[3];
+
+	bnlm_lut_encode(&to->div_lut, div_lut_nearests, div_lut_slopes,
+			BNLM_DIV_LUT_SIZE);
+	memset(to->div_lut_intercepts, 0, sizeof(to->div_lut_intercepts));
+	for (i = 0; i < BNLM_DIV_LUT_SIZE; i++) {
+		to->div_lut_intercepts[0][i] = div_lut_intercepts[i];
+	}
+
+	memset(to->power_of_2, 0, sizeof(to->power_of_2));
+	for (i = 0; i < (ISP_VEC_ELEMBITS - 1); i++) {
+		to->power_of_2[0][i] = 1 << i;
+	}
+}
+
+/* - Encodes BNLM public parameters into DMEM parameters */
+void
+ia_css_bnlm_encode(
+    struct bnlm_dmem_params *to,
+    const struct ia_css_bnlm_config *from,
+    size_t size)
+{
+	(void)size;
+	to->rad_enable = from->rad_enable;
+	to->rad_x_origin = from->rad_x_origin;
+	to->rad_y_origin = from->rad_y_origin;
+	to->avg_min_th = from->avg_min_th;
+	to->max_min_th = from->max_min_th;
+
+	to->exp_coeff_a = from->exp_coeff_a;
+	to->exp_coeff_b = from->exp_coeff_b;
+	to->exp_coeff_c = from->exp_coeff_c;
+	to->exp_exponent = from->exp_exponent;
+}
+
+/* Prints debug traces for BNLM public parameters */
+void
+ia_css_bnlm_debug_trace(
+    const struct ia_css_bnlm_config *config,
+    unsigned int level)
+{
+	if (!config)
+		return;
+
+#ifndef IA_CSS_NO_DEBUG
+	ia_css_debug_dtrace(level, "BNLM:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "rad_enable", config->rad_enable);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "rad_x_origin",
+			    config->rad_x_origin);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "rad_y_origin",
+			    config->rad_y_origin);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "avg_min_th", config->avg_min_th);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "max_min_th", config->max_min_th);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "exp_coeff_a",
+			    config->exp_coeff_a);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "exp_coeff_b",
+			    config->exp_coeff_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "exp_coeff_c",
+			    config->exp_coeff_c);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "exp_exponent",
+			    config->exp_exponent);
+
+	/* ToDo: print traces for LUTs */
+#endif /* IA_CSS_NO_DEBUG */
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.h
new file mode 100644
index 000000000..3632bf27c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm.host.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNLM_HOST_H
+#define __IA_CSS_BNLM_HOST_H
+
+#include "ia_css_bnlm_types.h"
+#include "ia_css_bnlm_param.h"
+
+void
+ia_css_bnlm_vmem_encode(
+    struct bnlm_vmem_params *to,
+    const struct ia_css_bnlm_config *from,
+    size_t size);
+
+void
+ia_css_bnlm_encode(
+    struct bnlm_dmem_params *to,
+    const struct ia_css_bnlm_config *from,
+    size_t size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_bnlm_debug_trace(
+    const struct ia_css_bnlm_config *config,
+    unsigned int level);
+#endif
+
+#endif /* __IA_CSS_BNLM_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_param.h
new file mode 100644
index 000000000..30672db26
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_param.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNLM_PARAM_H
+#define __IA_CSS_BNLM_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h" /* needed for VMEM_ARRAY */
+
+struct bnlm_lut {
+	VMEM_ARRAY(thr, ISP_VEC_NELEMS); /* thresholds */
+	VMEM_ARRAY(val, ISP_VEC_NELEMS); /* values */
+};
+
+struct bnlm_vmem_params {
+	VMEM_ARRAY(nl_th, ISP_VEC_NELEMS);
+	VMEM_ARRAY(match_quality_max_idx, ISP_VEC_NELEMS);
+	struct bnlm_lut mu_root_lut;
+	struct bnlm_lut sad_norm_lut;
+	struct bnlm_lut sig_detail_lut;
+	struct bnlm_lut sig_rad_lut;
+	struct bnlm_lut rad_pow_lut;
+	struct bnlm_lut nl_0_lut;
+	struct bnlm_lut nl_1_lut;
+	struct bnlm_lut nl_2_lut;
+	struct bnlm_lut nl_3_lut;
+
+	/* LUTs used for division approximiation */
+	struct bnlm_lut div_lut;
+
+	VMEM_ARRAY(div_lut_intercepts, ISP_VEC_NELEMS);
+
+	/* 240x does not have an ISP instruction to left shift each element of a
+	 * vector by different shift value. Hence it will be simulated by multiplying
+	 * the elements by required 2^shift. */
+	VMEM_ARRAY(power_of_2, ISP_VEC_NELEMS);
+};
+
+/* BNLM ISP parameters */
+struct bnlm_dmem_params {
+	bool rad_enable;
+	s32 rad_x_origin;
+	s32 rad_y_origin;
+	s32 avg_min_th;
+	s32 max_min_th;
+
+	s32 exp_coeff_a;
+	u32 exp_coeff_b;
+	s32 exp_coeff_c;
+	u32 exp_exponent;
+};
+
+#endif /* __IA_CSS_BNLM_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_types.h
new file mode 100644
index 000000000..407b5a3b0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnlm/ia_css_bnlm_types.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNLM_TYPES_H
+#define __IA_CSS_BNLM_TYPES_H
+
+/* @file
+* CSS-API header file for Bayer Non-Linear Mean parameters.
+*/
+
+#include "type_support.h" /* int32_t */
+
+/* Bayer Non-Linear Mean configuration
+ *
+ * \brief BNLM public parameters.
+ * \details Struct with all parameters for the BNLM kernel that can be set
+ * from the CSS API.
+ *
+ * ISP2.6.1: BNLM is used.
+ */
+struct ia_css_bnlm_config {
+	bool		rad_enable;	/** Enable a radial dependency in a weight calculation */
+	s32		rad_x_origin;	/** Initial x coordinate for a radius calculation */
+	s32		rad_y_origin;	/** Initial x coordinate for a radius calculation */
+	/* a threshold for average of weights if this < Th, do not denoise pixel */
+	s32		avg_min_th;
+	/* minimum weight for denoising if max < th, do not denoise pixel */
+	s32		max_min_th;
+
+	/**@{*/
+	/* Coefficient for approximation, in the form of (1 + x / N)^N,
+	 * that fits the first-order exp() to default exp_lut in BNLM sheet
+	 * */
+	s32		exp_coeff_a;
+	u32	exp_coeff_b;
+	s32		exp_coeff_c;
+	u32	exp_exponent;
+	/**@}*/
+
+	s32 nl_th[3];	/** Detail thresholds */
+
+	/* Index for n-th maximum candidate weight for each detail group */
+	s32 match_quality_max_idx[4];
+
+	/**@{*/
+	/* A lookup table for 1/sqrt(1+mu) approximation */
+	s32 mu_root_lut_thr[15];
+	s32 mu_root_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* A lookup table for SAD normalization */
+	s32 sad_norm_lut_thr[15];
+	s32 sad_norm_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* A lookup table that models a weight's dependency on textures */
+	s32 sig_detail_lut_thr[15];
+	s32 sig_detail_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* A lookup table that models a weight's dependency on a pixel's radial distance */
+	s32 sig_rad_lut_thr[15];
+	s32 sig_rad_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* A lookup table to control denoise power depending on a pixel's radial distance */
+	s32 rad_pow_lut_thr[15];
+	s32 rad_pow_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* Non linear transfer functions to calculate the blending coefficient depending on detail group */
+	/* detail group 0 */
+	/**@{*/
+	s32 nl_0_lut_thr[15];
+	s32 nl_0_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* detail group 1 */
+	s32 nl_1_lut_thr[15];
+	s32 nl_1_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* detail group 2 */
+	s32 nl_2_lut_thr[15];
+	s32 nl_2_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/* detail group 3 */
+	s32 nl_3_lut_thr[15];
+	s32 nl_3_lut_val[16];
+	/**@}*/
+	/**@}*/
+};
+
+#endif /* __IA_CSS_BNLM_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c
new file mode 100644
index 000000000..c42fcb1d9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c
@@ -0,0 +1,132 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "type_support.h"
+#include "ia_css_bnr2_2.host.h"
+
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h" /* ia_css_debug_dtrace() */
+#endif
+
+/* Default kernel parameters. */
+const struct ia_css_bnr2_2_config default_bnr2_2_config = {
+	200,
+	200,
+	200,
+	0,
+	0,
+	0,
+	200,
+	200,
+	200,
+	0,
+	0,
+	0,
+	0,
+	4096,
+	8191,
+	128,
+	1,
+	0,
+	0,
+	0,
+	8191,
+	0,
+	8191
+};
+
+void
+ia_css_bnr2_2_encode(
+    struct sh_css_isp_bnr2_2_params *to,
+    const struct ia_css_bnr2_2_config *from,
+    size_t size)
+{
+	(void)size;
+	to->d_var_gain_r = from->d_var_gain_r;
+	to->d_var_gain_g = from->d_var_gain_g;
+	to->d_var_gain_b = from->d_var_gain_b;
+	to->d_var_gain_slope_r = from->d_var_gain_slope_r;
+	to->d_var_gain_slope_g = from->d_var_gain_slope_g;
+	to->d_var_gain_slope_b = from->d_var_gain_slope_b;
+
+	to->n_var_gain_r = from->n_var_gain_r;
+	to->n_var_gain_g = from->n_var_gain_g;
+	to->n_var_gain_b = from->n_var_gain_b;
+	to->n_var_gain_slope_r = from->n_var_gain_slope_r;
+	to->n_var_gain_slope_g = from->n_var_gain_slope_g;
+	to->n_var_gain_slope_b = from->n_var_gain_slope_b;
+
+	to->dir_thres = from->dir_thres;
+	to->dir_thres_w = from->dir_thres_w;
+	to->var_offset_coef = from->var_offset_coef;
+
+	to->dir_gain = from->dir_gain;
+	to->detail_gain	= from->detail_gain;
+	to->detail_gain_divisor = from->detail_gain_divisor;
+	to->detail_level_offset = from->detail_level_offset;
+
+	to->d_var_th_min = from->d_var_th_min;
+	to->d_var_th_max = from->d_var_th_max;
+	to->n_var_th_min = from->n_var_th_min;
+	to->n_var_th_max = from->n_var_th_max;
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_bnr2_2_debug_dtrace(
+    const struct ia_css_bnr2_2_config *bnr,
+    unsigned int level)
+{
+	if (!bnr)
+		return;
+
+	ia_css_debug_dtrace(level, "Bayer Noise Reduction 2.2:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_r", bnr->d_var_gain_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_g", bnr->d_var_gain_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_b", bnr->d_var_gain_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_slope_r",
+			    bnr->d_var_gain_slope_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_slope_g",
+			    bnr->d_var_gain_slope_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_gain_slope_b",
+			    bnr->d_var_gain_slope_b);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_r", bnr->n_var_gain_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_g", bnr->n_var_gain_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_b", bnr->n_var_gain_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_slope_r",
+			    bnr->n_var_gain_slope_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_slope_g",
+			    bnr->n_var_gain_slope_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_gain_slope_b",
+			    bnr->n_var_gain_slope_b);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "dir_thres", bnr->dir_thres);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "dir_thres_w", bnr->dir_thres_w);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "var_offset_coef",
+			    bnr->var_offset_coef);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "dir_gain", bnr->dir_gain);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "detail_gain", bnr->detail_gain);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "detail_gain_divisor",
+			    bnr->detail_gain_divisor);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "detail_level_offset",
+			    bnr->detail_level_offset);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_th_min", bnr->d_var_th_min);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "d_var_th_max", bnr->d_var_th_max);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_th_min", bnr->n_var_th_min);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "n_var_th_max", bnr->n_var_th_max);
+}
+#endif /* IA_CSS_NO_DEBUG */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h
new file mode 100644
index 000000000..f6ab5d2bb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+#ifndef __IA_CSS_BNR2_2_HOST_H
+#define __IA_CSS_BNR2_2_HOST_H
+
+#include "ia_css_bnr2_2_types.h"
+#include "ia_css_bnr2_2_param.h"
+
+extern const struct ia_css_bnr2_2_config default_bnr2_2_config;
+
+void
+ia_css_bnr2_2_encode(
+    struct sh_css_isp_bnr2_2_params *to,
+    const struct ia_css_bnr2_2_config *from,
+    size_t size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_bnr2_2_debug_dtrace(
+    const struct ia_css_bnr2_2_config *config,
+    unsigned int level);
+#endif
+
+#endif /* __IA_CSS_BNR2_2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h
new file mode 100644
index 000000000..087723795
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNR2_2_PARAM_H
+#define __IA_CSS_BNR2_2_PARAM_H
+
+#include "type_support.h"
+
+/* BNR (Bayer Noise Reduction) ISP parameters */
+struct sh_css_isp_bnr2_2_params {
+	s32 d_var_gain_r;
+	s32 d_var_gain_g;
+	s32 d_var_gain_b;
+	s32 d_var_gain_slope_r;
+	s32 d_var_gain_slope_g;
+	s32 d_var_gain_slope_b;
+	s32 n_var_gain_r;
+	s32 n_var_gain_g;
+	s32 n_var_gain_b;
+	s32 n_var_gain_slope_r;
+	s32 n_var_gain_slope_g;
+	s32 n_var_gain_slope_b;
+	s32 dir_thres;
+	s32 dir_thres_w;
+	s32 var_offset_coef;
+	s32 dir_gain;
+	s32 detail_gain;
+	s32 detail_gain_divisor;
+	s32 detail_level_offset;
+	s32 d_var_th_min;
+	s32 d_var_th_max;
+	s32 n_var_th_min;
+	s32 n_var_th_max;
+};
+
+#endif /* __IA_CSS_BNR2_2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h
new file mode 100644
index 000000000..5f3dfa59f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNR2_2_TYPES_H
+#define __IA_CSS_BNR2_2_TYPES_H
+
+/* @file
+* CSS-API header file for Bayer Noise Reduction parameters.
+*/
+
+#include "type_support.h" /* int32_t */
+
+/* Bayer Noise Reduction 2.2 configuration
+ *
+ * \brief BNR2_2 public parameters.
+ * \details Struct with all parameters for the BNR2.2 kernel that can be set
+ * from the CSS API.
+ *
+ * ISP2.6.1: BNR2.2 is used.
+ */
+struct ia_css_bnr2_2_config {
+	/**@{*/
+	/* Directional variance gain for R/G/B components in dark region */
+	s32 d_var_gain_r;
+	s32 d_var_gain_g;
+	s32 d_var_gain_b;
+	/**@}*/
+	/**@{*/
+	/* Slope of Directional variance gain between dark and bright region */
+	s32 d_var_gain_slope_r;
+	s32 d_var_gain_slope_g;
+	s32 d_var_gain_slope_b;
+	/**@}*/
+	/**@{*/
+	/* Non-Directional variance gain for R/G/B components in dark region */
+	s32 n_var_gain_r;
+	s32 n_var_gain_g;
+	s32 n_var_gain_b;
+	/**@}*/
+	/**@{*/
+	/* Slope of Non-Directional variance gain between dark and bright region */
+	s32 n_var_gain_slope_r;
+	s32 n_var_gain_slope_g;
+	s32 n_var_gain_slope_b;
+	/**@}*/
+
+	s32 dir_thres;		/** Threshold for directional filtering */
+	s32 dir_thres_w;		/** Threshold width for directional filtering */
+	s32 var_offset_coef;	/** Variance offset coefficient */
+	s32 dir_gain;		/** Gain for directional coefficient */
+	s32 detail_gain;		/** Gain for low contrast texture control */
+	s32 detail_gain_divisor;	/** Gain divisor for low contrast texture control */
+	s32 detail_level_offset;	/** Bias value for low contrast texture control */
+	s32 d_var_th_min;		/** Minimum clipping value for directional variance*/
+	s32 d_var_th_max;		/** Maximum clipping value for diretional variance*/
+	s32 n_var_th_min;		/** Minimum clipping value for non-directional variance*/
+	s32 n_var_th_max;		/** Maximum clipping value for non-directional variance*/
+};
+
+#endif /* __IA_CSS_BNR2_2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c
new file mode 100644
index 000000000..457a004e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c
@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_bnr.host.h"
+
+void
+ia_css_bnr_encode(
+    struct sh_css_isp_bnr_params *to,
+    const struct ia_css_nr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	/* BNR (Bayer Noise Reduction) */
+	to->threshold_low =
+	    uDIGIT_FITTING(from->direction, 16, SH_CSS_BAYER_BITS);
+	to->threshold_width_log2 = uFRACTION_BITS_FITTING(8);
+	to->threshold_width =
+	    1 << to->threshold_width_log2;
+	to->gain_all =
+	    uDIGIT_FITTING(from->bnr_gain, 16, SH_CSS_BNR_GAIN_SHIFT);
+	to->gain_dir =
+	    uDIGIT_FITTING(from->bnr_gain, 16, SH_CSS_BNR_GAIN_SHIFT);
+	to->clip = uDIGIT_FITTING(16384U, 16, SH_CSS_BAYER_BITS);
+}
+
+void
+ia_css_bnr_dump(
+    const struct sh_css_isp_bnr_params *bnr,
+    unsigned int level)
+{
+	if (!bnr) return;
+	ia_css_debug_dtrace(level, "Bayer Noise Reduction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_gain_all", bnr->gain_all);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_gain_dir", bnr->gain_dir);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_threshold_low",
+			    bnr->threshold_low);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_threshold_width_log2",
+			    bnr->threshold_width_log2);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_threshold_width",
+			    bnr->threshold_width);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "bnr_clip", bnr->clip);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h
new file mode 100644
index 000000000..7fc2a728a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNR_HOST_H
+#define __IA_CSS_BNR_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ynr/ynr_1.0/ia_css_ynr_types.h"
+#include "ia_css_bnr_param.h"
+
+void
+ia_css_bnr_encode(
+    struct sh_css_isp_bnr_params *to,
+    const struct ia_css_nr_config *from,
+    unsigned int size);
+
+void
+ia_css_bnr_dump(
+    const struct sh_css_isp_bnr_params *bnr,
+    unsigned int level);
+
+#endif /* __IA_CSS_DP_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h
new file mode 100644
index 000000000..4f6469315
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BNR_PARAM_H
+#define __IA_CSS_BNR_PARAM_H
+
+#include "type_support.h"
+
+/* BNR (Bayer Noise Reduction) */
+struct sh_css_isp_bnr_params {
+	s32 gain_all;
+	s32 gain_dir;
+	s32 threshold_low;
+	s32 threshold_width_log2;
+	s32 threshold_width;
+	s32 clip;
+};
+
+#endif /* __IA_CSS_BNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c
new file mode 100644
index 000000000..0eb40517e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c
@@ -0,0 +1,29 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_cnr.host.h"
+
+/* keep the interface here, it is not enabled yet because host doesn't know the size of individual state */
+void
+ia_css_init_cnr_state(
+    void/*struct sh_css_isp_cnr_vmem_state*/ * state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h
new file mode 100644
index 000000000..4d046b730
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CNR_HOST_H
+#define __IA_CSS_CNR_HOST_H
+
+#include "ia_css_cnr_param.h"
+
+void
+ia_css_init_cnr_state(
+    void/*struct sh_css_isp_cnr_vmem_state*/ * state,
+    size_t size);
+
+#endif /* __IA_CSS_CNR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h
new file mode 100644
index 000000000..971ab87af
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CNR_PARAM_H
+#define __IA_CSS_CNR_PARAM_H
+
+#include "type_support.h"
+
+/* CNR (Chroma Noise Reduction) */
+/* Reuse YNR1 param structure */
+#include  "../../ynr/ynr_1.0/ia_css_ynr_param.h"
+
+#endif /* __IA_CSS_CNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c
new file mode 100644
index 000000000..495dc1f33
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c
@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_cnr2.host.h"
+
+const struct ia_css_cnr_config default_cnr_config = {
+	0,
+	0,
+	100,
+	100,
+	100,
+	50,
+	50,
+	50
+};
+
+void
+ia_css_cnr_encode(
+    struct sh_css_isp_cnr_params *to,
+    const struct ia_css_cnr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->coring_u = from->coring_u;
+	to->coring_v = from->coring_v;
+	to->sense_gain_vy = from->sense_gain_vy;
+	to->sense_gain_vu = from->sense_gain_vu;
+	to->sense_gain_vv = from->sense_gain_vv;
+	to->sense_gain_hy = from->sense_gain_hy;
+	to->sense_gain_hu = from->sense_gain_hu;
+	to->sense_gain_hv = from->sense_gain_hv;
+}
+
+void
+ia_css_cnr_dump(
+    const struct sh_css_isp_cnr_params *cnr,
+    unsigned int level);
+
+void
+ia_css_cnr_debug_dtrace(
+    const struct ia_css_cnr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.coring_u=%d, config.coring_v=%d, config.sense_gain_vy=%d, config.sense_gain_hy=%d, config.sense_gain_vu=%d, config.sense_gain_hu=%d, config.sense_gain_vv=%d, config.sense_gain_hv=%d\n",
+			    config->coring_u, config->coring_v,
+			    config->sense_gain_vy, config->sense_gain_hy,
+			    config->sense_gain_vu, config->sense_gain_hu,
+			    config->sense_gain_vv, config->sense_gain_hv);
+}
+
+void
+ia_css_init_cnr2_state(
+    void/*struct sh_css_isp_cnr_vmem_state*/ * state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h
new file mode 100644
index 000000000..38f848137
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CNR2_HOST_H
+#define __IA_CSS_CNR2_HOST_H
+
+#include "ia_css_cnr2_types.h"
+#include "ia_css_cnr2_param.h"
+
+extern const struct ia_css_cnr_config default_cnr_config;
+
+void
+ia_css_cnr_encode(
+    struct sh_css_isp_cnr_params *to,
+    const struct ia_css_cnr_config *from,
+    unsigned int size);
+
+void
+ia_css_cnr_dump(
+    const struct sh_css_isp_cnr_params *cnr,
+    unsigned int level);
+
+void
+ia_css_cnr_debug_dtrace(
+    const struct ia_css_cnr_config *config,
+    unsigned int level);
+
+void
+ia_css_init_cnr2_state(
+    void/*struct sh_css_isp_cnr_vmem_state*/ * state,
+    size_t size);
+#endif /* __IA_CSS_CNR2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h
new file mode 100644
index 000000000..3709aa4d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CNR2_PARAM_H
+#define __IA_CSS_CNR2_PARAM_H
+
+#include "type_support.h"
+
+/* CNR (Chroma Noise Reduction) */
+struct sh_css_isp_cnr_params {
+	s32 coring_u;
+	s32 coring_v;
+	s32 sense_gain_vy;
+	s32 sense_gain_vu;
+	s32 sense_gain_vv;
+	s32 sense_gain_hy;
+	s32 sense_gain_hu;
+	s32 sense_gain_hv;
+};
+
+#endif /* __IA_CSS_CNR2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h
new file mode 100644
index 000000000..d0a256167
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CNR2_TYPES_H
+#define __IA_CSS_CNR2_TYPES_H
+
+/* @file
+* CSS-API header file for Chroma Noise Reduction (CNR) parameters
+*/
+
+/* Chroma Noise Reduction configuration.
+ *
+ *  Small sensitivity of edge means strong smoothness and NR performance.
+ *  If you see blurred color on vertical edges,
+ *  set higher values on sense_gain_h*.
+ *  If you see blurred color on horizontal edges,
+ *  set higher values on sense_gain_v*.
+ *
+ *  ISP block: CNR2
+ * (ISP1: CNR1 is used.)
+ * (ISP2: CNR1 is used for Preview/Video.)
+ *  ISP2: CNR2 is used for Still.
+ */
+struct ia_css_cnr_config {
+	u16 coring_u;	/** Coring level of U.
+				u0.13, [0,8191], default/ineffective 0 */
+	u16 coring_v;	/** Coring level of V.
+				u0.13, [0,8191], default/ineffective 0 */
+	u16 sense_gain_vy;	/** Sensitivity of horizontal edge of Y.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	u16 sense_gain_vu;	/** Sensitivity of horizontal edge of U.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	u16 sense_gain_vv;	/** Sensitivity of horizontal edge of V.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	u16 sense_gain_hy;	/** Sensitivity of vertical edge of Y.
+				u13.0, [0,8191], default 50, ineffective 8191 */
+	u16 sense_gain_hu;	/** Sensitivity of vertical edge of U.
+				u13.0, [0,8191], default 50, ineffective 8191 */
+	u16 sense_gain_hv;	/** Sensitivity of vertical edge of V.
+				u13.0, [0,8191], default 50, ineffective 8191 */
+};
+
+#endif /* __IA_CSS_CNR2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c
new file mode 100644
index 000000000..ff452e2cc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c
@@ -0,0 +1,37 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "ia_css_conversion.host.h"
+
+const struct ia_css_conversion_config default_conversion_config = {
+	0,
+	0,
+	0,
+	0,
+};
+
+void
+ia_css_conversion_encode(
+    struct sh_css_isp_conversion_params *to,
+    const struct ia_css_conversion_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->en     = from->en;
+	to->dummy0 = from->dummy0;
+	to->dummy1 = from->dummy1;
+	to->dummy2 = from->dummy2;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h
new file mode 100644
index 000000000..520623e27
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CONVERSION_HOST_H
+#define __IA_CSS_CONVERSION_HOST_H
+
+#include "ia_css_conversion_types.h"
+#include "ia_css_conversion_param.h"
+
+extern const struct ia_css_conversion_config default_conversion_config;
+
+void
+ia_css_conversion_encode(
+    struct sh_css_isp_conversion_params *to,
+    const struct ia_css_conversion_config *from,
+    unsigned int size);
+
+#endif /* __IA_CSS_CONVERSION_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h
new file mode 100644
index 000000000..fcbec189e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CONVERSION_PARAM_H
+#define __IA_CSS_CONVERSION_PARAM_H
+
+#include "type_support.h"
+
+/* CONVERSION */
+struct sh_css_isp_conversion_params {
+	u32 en;
+	u32 dummy0;
+	u32 dummy1;
+	u32 dummy2;
+};
+
+#endif /* __IA_CSS_CONVERSION_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h
new file mode 100644
index 000000000..34152d6d0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CONVERSION_TYPES_H
+#define __IA_CSS_CONVERSION_TYPES_H
+
+/**
+ *  Conversion Kernel parameters.
+ *  Deinterleave bayer quad into isys format
+ *
+ *  ISP block: CONVERSION
+ *
+ */
+struct ia_css_conversion_config {
+	u32 en;     /** en parameter */
+	u32 dummy0; /** dummy0 dummy parameter 0 */
+	u32 dummy1; /** dummy1 dummy parameter 1 */
+	u32 dummy2; /** dummy2 dummy parameter 2 */
+};
+
+#endif /* __IA_CSS_CONVERSION_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c
new file mode 100644
index 000000000..cc415c72a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c
@@ -0,0 +1,45 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_copy_output.host.h"
+#include "ia_css_binary.h"
+#include "type_support.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+
+static const struct ia_css_copy_output_configuration default_config = {
+	.enable = false,
+};
+
+void
+ia_css_copy_output_config(
+    struct sh_css_isp_copy_output_isp_config      *to,
+    const struct ia_css_copy_output_configuration *from,
+    unsigned int size)
+{
+	(void)size;
+	to->enable = from->enable;
+}
+
+int ia_css_copy_output_configure(const struct ia_css_binary *binary,
+				 bool enable)
+{
+	struct ia_css_copy_output_configuration config = default_config;
+
+	config.enable = enable;
+
+	return ia_css_configure_copy_output(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h
new file mode 100644
index 000000000..44e3e45b0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_COPY_OUTPUT_HOST_H
+#define __IA_CSS_COPY_OUTPUT_HOST_H
+
+#include "type_support.h"
+#include "ia_css_binary.h"
+
+#include "ia_css_copy_output_param.h"
+
+void
+ia_css_copy_output_config(
+    struct sh_css_isp_copy_output_isp_config      *to,
+    const struct ia_css_copy_output_configuration *from,
+    unsigned int size);
+
+int ia_css_copy_output_configure(const struct ia_css_binary     *binary,
+				 bool enable);
+
+#endif /* __IA_CSS_COPY_OUTPUT_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h
new file mode 100644
index 000000000..56daa1d96
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_COPY_PARAM_H
+#define __IA_CSS_COPY_PARAM_H
+
+struct ia_css_copy_output_configuration {
+	bool enable;
+};
+
+struct sh_css_isp_copy_output_isp_config {
+	u32 enable;
+};
+
+#endif /* __IA_CSS_COPY_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.c
new file mode 100644
index 000000000..8c1d50f7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.c
@@ -0,0 +1,67 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <assert_support.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_frame.h>
+#include <ia_css_binary.h>
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+#include "ia_css_crop.host.h"
+
+static const struct ia_css_crop_configuration default_config = {
+	.info = (struct ia_css_frame_info *)NULL,
+};
+
+void
+ia_css_crop_encode(
+    struct sh_css_isp_crop_isp_params *to,
+    const struct ia_css_crop_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->crop_pos = from->crop_pos;
+}
+
+int ia_css_crop_config(struct sh_css_isp_crop_isp_config *to,
+		       const struct ia_css_crop_configuration *from,
+		       unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	int ret;
+
+	ret = ia_css_dma_configure_from_info(&to->port_b, from->info);
+	if (ret)
+		return ret;
+
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int ia_css_crop_configure(const struct ia_css_binary     *binary,
+			  const struct ia_css_frame_info *info)
+{
+	struct ia_css_crop_configuration config = default_config;
+
+	config.info = info;
+
+	return ia_css_configure_crop(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.h
new file mode 100644
index 000000000..e700149c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop.host.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CROP_HOST_H
+#define __IA_CSS_CROP_HOST_H
+
+#include <ia_css_frame_public.h>
+#include <ia_css_binary.h>
+
+#include "ia_css_crop_types.h"
+#include "ia_css_crop_param.h"
+
+void
+ia_css_crop_encode(
+    struct sh_css_isp_crop_isp_params *to,
+    const struct ia_css_crop_config *from,
+    unsigned int size);
+
+int ia_css_crop_config(struct sh_css_isp_crop_isp_config      *to,
+		       const struct ia_css_crop_configuration *from,
+		       unsigned int size);
+
+int ia_css_crop_configure(const struct ia_css_binary     *binary,
+			  const struct ia_css_frame_info *from);
+
+#endif /* __IA_CSS_CROP_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_param.h
new file mode 100644
index 000000000..7416e74dd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_param.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CROP_PARAM_H
+#define __IA_CSS_CROP_PARAM_H
+
+#include <type_support.h>
+#include "dma.h"
+#include "sh_css_internal.h" /* sh_css_crop_pos */
+
+/* Crop frame */
+struct sh_css_isp_crop_isp_config {
+	u32 width_a_over_b;
+	struct dma_port_config port_b;
+};
+
+struct sh_css_isp_crop_isp_params {
+	struct sh_css_crop_pos crop_pos;
+};
+
+#endif /* __IA_CSS_CROP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_types.h
new file mode 100644
index 000000000..aaaae5e2a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/crop/crop_1.0/ia_css_crop_types.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CROP_TYPES_H
+#define __IA_CSS_CROP_TYPES_H
+
+/* Crop frame
+ *
+ *  ISP block: crop frame
+ */
+
+#include <ia_css_frame_public.h>
+#include "sh_css_uds.h" /* sh_css_crop_pos */
+
+struct ia_css_crop_config {
+	struct sh_css_crop_pos crop_pos;
+};
+
+struct ia_css_crop_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+#endif /* __IA_CSS_CROP_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.c
new file mode 100644
index 000000000..284c17970
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.c
@@ -0,0 +1,128 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+#include "ia_css_debug.h"
+#endif
+
+#include "ia_css_csc.host.h"
+
+const struct ia_css_cc_config default_cc_config = {
+	8,
+	{255, 29, 120, 0, -374, -342, 0, -672, 301},
+};
+
+void
+ia_css_encode_cc(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size)
+{
+	(void)size;
+#ifndef IA_CSS_NO_DEBUG
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_encode_cc() enter:\n");
+#endif
+
+	to->m_shift    = (int16_t)from->fraction_bits;
+	to->m00 = (int16_t)from->matrix[0];
+	to->m01 = (int16_t)from->matrix[1];
+	to->m02 = (int16_t)from->matrix[2];
+	to->m10 = (int16_t)from->matrix[3];
+	to->m11 = (int16_t)from->matrix[4];
+	to->m12 = (int16_t)from->matrix[5];
+	to->m20 = (int16_t)from->matrix[6];
+	to->m21 = (int16_t)from->matrix[7];
+	to->m22 = (int16_t)from->matrix[8];
+
+#ifndef IA_CSS_NO_DEBUG
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_encode_cc() leave:\n");
+#endif
+}
+
+void
+ia_css_csc_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size)
+{
+	ia_css_encode_cc(to, from, size);
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_cc_dump(
+    const struct sh_css_isp_csc_params *csc,
+    unsigned int level,
+    const char *name)
+{
+	if (!csc) return;
+	ia_css_debug_dtrace(level, "%s\n", name);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m_shift",
+			    csc->m_shift);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m00",
+			    csc->m00);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m01",
+			    csc->m01);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m02",
+			    csc->m02);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m10",
+			    csc->m10);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m11",
+			    csc->m11);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m12",
+			    csc->m12);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m20",
+			    csc->m20);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m21",
+			    csc->m21);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "m22",
+			    csc->m22);
+}
+
+void
+ia_css_csc_dump(
+    const struct sh_css_isp_csc_params *csc,
+    unsigned int level)
+{
+	ia_css_cc_dump(csc, level, "Color Space Conversion");
+}
+
+void
+ia_css_cc_config_debug_dtrace(
+    const struct ia_css_cc_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.m[0]=%d, config.m[1]=%d, config.m[2]=%d, config.m[3]=%d, config.m[4]=%d, config.m[5]=%d, config.m[6]=%d, config.m[7]=%d, config.m[8]=%d\n",
+			    config->matrix[0],
+			    config->matrix[1], config->matrix[2],
+			    config->matrix[3], config->matrix[4],
+			    config->matrix[5], config->matrix[6],
+			    config->matrix[7], config->matrix[8]);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.h
new file mode 100644
index 000000000..6b0256a73
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc.host.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CSC_HOST_H
+#define __IA_CSS_CSC_HOST_H
+
+#include "ia_css_csc_types.h"
+#include "ia_css_csc_param.h"
+
+extern const struct ia_css_cc_config default_cc_config;
+
+void
+ia_css_encode_cc(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size);
+
+void
+ia_css_csc_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_cc_dump(
+    const struct sh_css_isp_csc_params *csc, unsigned int level,
+    const char *name);
+
+void
+ia_css_csc_dump(
+    const struct sh_css_isp_csc_params *csc,
+    unsigned int level);
+
+void
+ia_css_cc_config_debug_dtrace(
+    const struct ia_css_cc_config *config,
+    unsigned int level);
+
+#define ia_css_csc_debug_dtrace ia_css_cc_config_debug_dtrace
+#endif
+
+#endif /* __IA_CSS_CSC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_param.h
new file mode 100644
index 000000000..3809ef73e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_param.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CSC_PARAM_H
+#define __IA_CSS_CSC_PARAM_H
+
+#include "type_support.h"
+/* CSC (Color Space Conversion) */
+struct sh_css_isp_csc_params {
+	u16	m_shift;
+	s16		m00;
+	s16		m01;
+	s16		m02;
+	s16		m10;
+	s16		m11;
+	s16		m12;
+	s16		m20;
+	s16		m21;
+	s16		m22;
+};
+
+#endif /* __IA_CSS_CSC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_types.h
new file mode 100644
index 000000000..160f19bdf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/csc/csc_1.0/ia_css_csc_types.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CSC_TYPES_H
+#define __IA_CSS_CSC_TYPES_H
+
+/* @file
+* CSS-API header file for Color Space Conversion parameters.
+*/
+
+/* Color Correction configuration.
+ *
+ *  This structure is used for 3 cases.
+ *  ("YCgCo" is the output format of Demosaic.)
+ *
+ *  1. Color Space Conversion (YCgCo to YUV) for ISP1.
+ *     ISP block: CSC1 (Color Space Conversion)
+ *     struct ia_css_cc_config   *cc_config
+ *
+ *  2. Color Correction Matrix (YCgCo to RGB) for ISP2.
+ *     ISP block: CCM2 (Color Correction Matrix)
+ *     struct ia_css_cc_config   *yuv2rgb_cc_config
+ *
+ *  3. Color Space Conversion (RGB to YUV) for ISP2.
+ *     ISP block: CSC2 (Color Space Conversion)
+ *     struct ia_css_cc_config   *rgb2yuv_cc_config
+ *
+ *  default/ineffective:
+ *  1. YCgCo -> YUV
+ *	1	0.174		0.185
+ *	0	-0.66252	-0.66874
+ *	0	-0.83738	0.58131
+ *
+ *	fraction_bits = 12
+ *	4096	713	758
+ *	0	-2714	-2739
+ *	0	-3430	2381
+ *
+ *  2. YCgCo -> RGB
+ *	1	-1	1
+ *	1	1	0
+ *	1	-1	-1
+ *
+ *	fraction_bits = 12
+ *	4096	-4096	4096
+ *	4096	4096	0
+ *	4096	-4096	-4096
+ *
+ *  3. RGB -> YUV
+ *	0.299	   0.587	0.114
+ *	-0.16874   -0.33126	0.5
+ *	0.5	   -0.41869	-0.08131
+ *
+ *	fraction_bits = 13
+ *	2449	4809	934
+ *	-1382	-2714	4096
+ *	4096	-3430	-666
+ */
+struct ia_css_cc_config {
+	u32 fraction_bits;/** Fractional bits of matrix.
+					u8.0, [0,13] */
+	s32 matrix[3 * 3]; /** Conversion matrix.
+					s[13-fraction_bits].[fraction_bits],
+					[-8192,8191] */
+};
+
+#endif /* __IA_CSS_CSC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c
new file mode 100644
index 000000000..149adbc57
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+#include "ctc/ctc_1.0/ia_css_ctc.host.h"
+#include "ia_css_ctc1_5.host.h"
+
+static void ctc_gradient(
+    int *dydx, int *shift,
+    int y1, int y0, int x1, int x0)
+{
+	int frc_bits = max(IA_CSS_CTC_COEF_SHIFT, 16);
+	int dy = y1 - y0;
+	int dx = x1 - x0;
+	int dydx_int;
+	int dydx_frc;
+	int sft;
+	/* max_dydx = the maxinum gradient = the maximum y (gain) */
+	int max_dydx = (1 << IA_CSS_CTC_COEF_SHIFT) - 1;
+
+	if (dx == 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ctc_gradient() error, illegal division operation\n");
+		return;
+	} else {
+		dydx_int = dy / dx;
+		dydx_frc = ((dy - dydx_int * dx) << frc_bits) / dx;
+	}
+
+	assert(y0 >= 0 && y0 <= max_dydx);
+	assert(y1 >= 0 && y1 <= max_dydx);
+	assert(x0 < x1);
+	assert(dydx);
+	assert(shift);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ctc_gradient() enter:\n");
+
+	/* search "sft" which meets this condition:
+		   (1 << (IA_CSS_CTC_COEF_SHIFT - 1))
+		<= (((float)dy / (float)dx) * (1 << sft))
+		<= ((1 << IA_CSS_CTC_COEF_SHIFT) - 1) */
+	for (sft = 0; sft <= IA_CSS_CTC_COEF_SHIFT; sft++) {
+		int tmp_dydx = (dydx_int << sft)
+			       + (dydx_frc >> (frc_bits - sft));
+		if (tmp_dydx <= max_dydx) {
+			*dydx = tmp_dydx;
+			*shift = sft;
+		}
+		if (tmp_dydx >= max_dydx)
+			break;
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ctc_gradient() leave:\n");
+}
+
+void
+ia_css_ctc_encode(
+    struct sh_css_isp_ctc_params *to,
+    const struct ia_css_ctc_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->y0 = from->y0;
+	to->y1 = from->y1;
+	to->y2 = from->y2;
+	to->y3 = from->y3;
+	to->y4 = from->y4;
+	to->y5 = from->y5;
+
+	to->ce_gain_exp = from->ce_gain_exp;
+
+	to->x1 = from->x1;
+	to->x2 = from->x2;
+	to->x3 = from->x3;
+	to->x4 = from->x4;
+
+	ctc_gradient(&to->dydx0,
+		     &to->dydx0_shift,
+		     from->y1, from->y0,
+		     from->x1, 0);
+
+	ctc_gradient(&to->dydx1,
+		     &to->dydx1_shift,
+		     from->y2, from->y1,
+		     from->x2, from->x1);
+
+	ctc_gradient(&to->dydx2,
+		     &to->dydx2_shift,
+		     from->y3, from->y2,
+		     from->x3, from->x2);
+
+	ctc_gradient(&to->dydx3,
+		     &to->dydx3_shift,
+		     from->y4, from->y3,
+		     from->x4, from->x3);
+
+	ctc_gradient(&to->dydx4,
+		     &to->dydx4_shift,
+		     from->y5, from->y4,
+		     SH_CSS_BAYER_MAXVAL, from->x4);
+}
+
+void
+ia_css_ctc_dump(
+    const struct sh_css_isp_ctc_params *ctc,
+    unsigned int level);
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h
new file mode 100644
index 000000000..8c17e7b92
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC1_5_HOST_H
+#define __IA_CSS_CTC1_5_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_ctc1_5_param.h"
+
+void
+ia_css_ctc_encode(
+    struct sh_css_isp_ctc_params *to,
+    const struct ia_css_ctc_config *from,
+    unsigned int size);
+
+void
+ia_css_ctc_dump(
+    const struct sh_css_isp_ctc_params *ctc,
+    unsigned int level);
+
+#endif /* __IA_CSS_CTC1_5_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h
new file mode 100644
index 000000000..c18cfc930
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC1_5_PARAM_H
+#define __IA_CSS_CTC1_5_PARAM_H
+
+#include "type_support.h"
+#include "ctc/ctc_1.0/ia_css_ctc_param.h" /* vamem params */
+
+/* CTC (Color Tone Control) */
+struct sh_css_isp_ctc_params {
+	s32 y0;
+	s32 y1;
+	s32 y2;
+	s32 y3;
+	s32 y4;
+	s32 y5;
+	s32 ce_gain_exp;
+	s32 x1;
+	s32 x2;
+	s32 x3;
+	s32 x4;
+	s32 dydx0;
+	s32 dydx0_shift;
+	s32 dydx1;
+	s32 dydx1_shift;
+	s32 dydx2;
+	s32 dydx2_shift;
+	s32 dydx3;
+	s32 dydx3_shift;
+	s32 dydx4;
+	s32 dydx4_shift;
+};
+
+#endif /* __IA_CSS_CTC1_5_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c
new file mode 100644
index 000000000..e3d3f1253
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "assert_support.h"
+
+#include "ia_css_ctc2.host.h"
+
+#define INEFFECTIVE_VAL 4096
+#define BASIC_VAL 819
+
+/*Default configuration of parameters for Ctc2*/
+const struct ia_css_ctc2_config default_ctc2_config = {
+	INEFFECTIVE_VAL, INEFFECTIVE_VAL, INEFFECTIVE_VAL,
+	INEFFECTIVE_VAL, INEFFECTIVE_VAL, INEFFECTIVE_VAL,
+	BASIC_VAL * 2, BASIC_VAL * 4, BASIC_VAL * 6,
+	BASIC_VAL * 8, INEFFECTIVE_VAL, INEFFECTIVE_VAL,
+	BASIC_VAL >> 1, BASIC_VAL
+};
+
+/* (dydx) = ctc2_slope(y1, y0, x1, x0)
+ * -----------------------------------------------
+ * Calculation of the Slope of a Line = ((y1 - y0) >> 8)/(x1 - x0)
+ *
+ * Note: y1, y0 , x1 & x0 must lie within the range 0 <-> 8191
+ */
+static int ctc2_slope(int y1, int y0, int x1, int x0)
+{
+	const int shift_val = 8;
+	const int max_slope = (1 << IA_CSS_CTC_COEF_SHIFT) - 1;
+	int dy = y1 - y0;
+	int dx = x1 - x0;
+	int rounding = (dx + 1) >> 1;
+	int dy_shift = dy << shift_val;
+	int slope, dydx;
+
+	/*Protection for parameter values, & avoiding zero divisions*/
+	assert(y0 >= 0 && y0 <= max_slope);
+	assert(y1 >= 0 && y1 <= max_slope);
+	assert(x0 >= 0 && x0 <= max_slope);
+	assert(x1 > 0 && x1 <= max_slope);
+	assert(dx > 0);
+
+	if (dy < 0)
+		rounding = -rounding;
+	slope = (int)(dy_shift + rounding) / dx;
+
+	/*the slope must lie within the range
+	  (-max_slope-1) >= (dydx) >= (max_slope)
+	*/
+	if (slope <= -max_slope - 1) {
+		dydx = -max_slope - 1;
+	} else if (slope >= max_slope) {
+		dydx = max_slope;
+	} else {
+		dydx = slope;
+	}
+
+	return dydx;
+}
+
+/* (void) = ia_css_ctc2_vmem_encode(*to, *from)
+ * -----------------------------------------------
+ * VMEM Encode Function to translate Y parameters from userspace into ISP space
+ */
+void ia_css_ctc2_vmem_encode(struct ia_css_isp_ctc2_vmem_params *to,
+			     const struct ia_css_ctc2_config *from,
+			     size_t size)
+{
+	unsigned int i, j;
+	const unsigned int shffl_blck = 4;
+	const unsigned int length_zeros = 11;
+	short dydx0, dydx1, dydx2, dydx3, dydx4;
+
+	(void)size;
+	/*
+	*  Calculation of slopes of lines interconnecting
+	*  0.0 -> y_x1 -> y_x2 -> y _x3 -> y_x4 -> 1.0
+	*/
+	dydx0 = ctc2_slope(from->y_y1, from->y_y0,
+			   from->y_x1, 0);
+	dydx1 = ctc2_slope(from->y_y2, from->y_y1,
+			   from->y_x2, from->y_x1);
+	dydx2 = ctc2_slope(from->y_y3, from->y_y2,
+			   from->y_x3, from->y_x2);
+	dydx3 = ctc2_slope(from->y_y4, from->y_y3,
+			   from->y_x4, from->y_x3);
+	dydx4 = ctc2_slope(from->y_y5, from->y_y4,
+			   SH_CSS_BAYER_MAXVAL, from->y_x4);
+
+	/*Fill 3 arrays with:
+	 * - Luma input gain values y_y0, y_y1, y_y2, y_3, y_y4
+	 * - Luma kneepoints 0, y_x1, y_x2, y_x3, y_x4
+	 * - Calculated slopes dydx0, dyxd1, dydx2, dydx3, dydx4
+	 *
+	 * - Each 64-element array is divided in blocks of 16 elements:
+	 *   the 5 parameters + zeros in the remaining 11 positions
+	 * - All blocks of the same array will contain the same data
+	 */
+	for (i = 0; i < shffl_blck; i++) {
+		to->y_x[0][(i << shffl_blck)]     = 0;
+		to->y_x[0][(i << shffl_blck) + 1] = from->y_x1;
+		to->y_x[0][(i << shffl_blck) + 2] = from->y_x2;
+		to->y_x[0][(i << shffl_blck) + 3] = from->y_x3;
+		to->y_x[0][(i << shffl_blck) + 4] = from->y_x4;
+
+		to->y_y[0][(i << shffl_blck)]     = from->y_y0;
+		to->y_y[0][(i << shffl_blck) + 1] = from->y_y1;
+		to->y_y[0][(i << shffl_blck) + 2] = from->y_y2;
+		to->y_y[0][(i << shffl_blck) + 3] = from->y_y3;
+		to->y_y[0][(i << shffl_blck) + 4] = from->y_y4;
+
+		to->e_y_slope[0][(i << shffl_blck)]    = dydx0;
+		to->e_y_slope[0][(i << shffl_blck) + 1] = dydx1;
+		to->e_y_slope[0][(i << shffl_blck) + 2] = dydx2;
+		to->e_y_slope[0][(i << shffl_blck) + 3] = dydx3;
+		to->e_y_slope[0][(i << shffl_blck) + 4] = dydx4;
+
+		for (j = 0; j < length_zeros; j++) {
+			to->y_x[0][(i << shffl_blck) + 5 + j] = 0;
+			to->y_y[0][(i << shffl_blck) + 5 + j] = 0;
+			to->e_y_slope[0][(i << shffl_blck) + 5 + j] = 0;
+		}
+	}
+}
+
+/* (void) = ia_css_ctc2_encode(*to, *from)
+ * -----------------------------------------------
+ * DMEM Encode Function to translate UV parameters from userspace into ISP space
+ */
+void ia_css_ctc2_encode(struct ia_css_isp_ctc2_dmem_params *to,
+			struct ia_css_ctc2_config *from,
+			size_t size)
+{
+	(void)size;
+
+	to->uv_y0 = from->uv_y0;
+	to->uv_y1 = from->uv_y1;
+	to->uv_x0 = from->uv_x0;
+	to->uv_x1 = from->uv_x1;
+
+	/*Slope Calculation*/
+	to->uv_dydx = ctc2_slope(from->uv_y1, from->uv_y0,
+				 from->uv_x1, from->uv_x0);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h
new file mode 100644
index 000000000..eb10c3884
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC2_HOST_H
+#define __IA_CSS_CTC2_HOST_H
+
+#include "ia_css_ctc2_param.h"
+#include "ia_css_ctc2_types.h"
+
+extern const struct ia_css_ctc2_config default_ctc2_config;
+
+/*Encode Functions to translate parameters from userspace into ISP space*/
+
+void ia_css_ctc2_vmem_encode(struct ia_css_isp_ctc2_vmem_params *to,
+			     const struct ia_css_ctc2_config *from,
+			     size_t size);
+
+void ia_css_ctc2_encode(struct ia_css_isp_ctc2_dmem_params *to,
+			struct ia_css_ctc2_config *from,
+			size_t size);
+
+#endif /* __IA_CSS_CTC2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h
new file mode 100644
index 000000000..94844da66
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC2_PARAM_H
+#define __IA_CSS_CTC2_PARAM_H
+
+#define IA_CSS_CTC_COEF_SHIFT          13
+#include "vmem.h" /* needed for VMEM_ARRAY */
+
+/* CTC (Chroma Tone Control)ISP Parameters */
+
+/*VMEM Luma params*/
+struct ia_css_isp_ctc2_vmem_params {
+	/** Gains by Y(Luma) at Y = 0.0,Y_X1, Y_X2, Y_X3, Y_X4*/
+	VMEM_ARRAY(y_x, ISP_VEC_NELEMS);
+	/* kneepoints by Y(Luma) 0.0, y_x1, y_x2, y _x3, y_x4*/
+	VMEM_ARRAY(y_y, ISP_VEC_NELEMS);
+	/* Slopes of lines interconnecting
+	 *  0.0 -> y_x1 -> y_x2 -> y _x3 -> y_x4 -> 1.0*/
+	VMEM_ARRAY(e_y_slope, ISP_VEC_NELEMS);
+};
+
+/*DMEM Chroma params*/
+struct ia_css_isp_ctc2_dmem_params {
+	/* Gains by UV(Chroma) under kneepoints uv_x0 and uv_x1*/
+	s32 uv_y0;
+	s32 uv_y1;
+
+	/* Kneepoints by UV(Chroma)- uv_x0 and uv_x1*/
+	s32 uv_x0;
+	s32 uv_x1;
+
+	/* Slope of line interconnecting uv_x0 -> uv_x1*/
+	s32 uv_dydx;
+
+};
+#endif /* __IA_CSS_CTC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h
new file mode 100644
index 000000000..f9f329a58
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC2_TYPES_H
+#define __IA_CSS_CTC2_TYPES_H
+
+/* Chroma Tone Control configuration.
+*
+*  ISP block: CTC2 (CTC by polygonal approximation)
+* (ISP1: CTC1 (CTC by look-up table) is used.)
+*  ISP2: CTC2 is used.
+*  ISP261: CTC2 (CTC by Fast Approximate Distance)
+*/
+struct ia_css_ctc2_config {
+	/** Gains by Y(Luma) at Y =0.0,Y_X1, Y_X2, Y_X3, Y_X4 and Y_X5
+	*   --default/ineffective value: 4096(0.5f)
+	*/
+	s32 y_y0;
+	s32 y_y1;
+	s32 y_y2;
+	s32 y_y3;
+	s32 y_y4;
+	s32 y_y5;
+	/* 1st-4th  kneepoints by Y(Luma) --default/ineffective value:n/a
+	*   requirement: 0.0 < y_x1 < y_x2 <y _x3 < y_x4 < 1.0
+	*/
+	s32 y_x1;
+	s32 y_x2;
+	s32 y_x3;
+	s32 y_x4;
+	/* Gains by UV(Chroma) under threholds uv_x0 and uv_x1
+	*   --default/ineffective value: 4096(0.5f)
+	*/
+	s32 uv_y0;
+	s32 uv_y1;
+	/* Minimum and Maximum Thresholds by UV(Chroma)- uv_x0 and uv_x1
+	*   --default/ineffective value: n/a
+	*/
+	s32 uv_x0;
+	s32 uv_x1;
+};
+
+#endif /* __IA_CSS_CTC2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c
new file mode 100644
index 000000000..82f2adbbf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c
@@ -0,0 +1,59 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+#include "ia_css_ctc.host.h"
+
+const struct ia_css_ctc_config default_ctc_config = {
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	((1 << IA_CSS_CTC_COEF_SHIFT) + 1) / 2,		/* 0.5 */
+	1,
+	SH_CSS_BAYER_MAXVAL / 5,	/* To be implemented */
+	SH_CSS_BAYER_MAXVAL * 2 / 5,	/* To be implemented */
+	SH_CSS_BAYER_MAXVAL * 3 / 5,	/* To be implemented */
+	SH_CSS_BAYER_MAXVAL * 4 / 5,	/* To be implemented */
+};
+
+void
+ia_css_ctc_vamem_encode(
+    struct sh_css_isp_ctc_vamem_params *to,
+    const struct ia_css_ctc_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->ctc,  &from->data, sizeof(to->ctc));
+}
+
+void
+ia_css_ctc_debug_dtrace(
+    const struct ia_css_ctc_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.ce_gain_exp=%d, config.y0=%d, config.x1=%d, config.y1=%d, config.x2=%d, config.y2=%d, config.x3=%d, config.y3=%d, config.x4=%d, config.y4=%d\n",
+			    config->ce_gain_exp, config->y0,
+			    config->x1, config->y1,
+			    config->x2, config->y2,
+			    config->x3, config->y3,
+			    config->x4, config->y4);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h
new file mode 100644
index 000000000..57d1d08e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC_HOST_H
+#define __IA_CSS_CTC_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_ctc_param.h"
+#include "ia_css_ctc_table.host.h"
+
+extern const struct ia_css_ctc_config default_ctc_config;
+
+void
+ia_css_ctc_vamem_encode(
+    struct sh_css_isp_ctc_vamem_params *to,
+    const struct ia_css_ctc_table *from,
+    unsigned int size);
+
+void
+ia_css_ctc_debug_dtrace(
+    const struct ia_css_ctc_config *config, unsigned int level)
+;
+
+#endif /* __IA_CSS_CTC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h
new file mode 100644
index 000000000..eaad708c6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC_PARAM_H
+#define __IA_CSS_CTC_PARAM_H
+
+#include "type_support.h"
+#include <system_global.h>
+
+#include "ia_css_ctc_types.h"
+
+#ifndef PIPE_GENERATION
+#define SH_CSS_ISP_CTC_TABLE_SIZE_LOG2       IA_CSS_VAMEM_2_CTC_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_CTC_TABLE_SIZE            IA_CSS_VAMEM_2_CTC_TABLE_SIZE
+
+#else
+/* For pipe generation, the size is not relevant */
+#define SH_CSS_ISP_CTC_TABLE_SIZE 0
+#endif
+
+/* This should be vamem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_ctc_vamem_params {
+	u16 ctc[SH_CSS_ISP_CTC_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_CTC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c
new file mode 100644
index 000000000..6a7925c84
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c
@@ -0,0 +1,71 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+
+#include <type_support.h>
+#include "system_global.h"
+#include "vamem.h"
+#include "ia_css_types.h"
+#include "ia_css_ctc_table.host.h"
+
+struct ia_css_ctc_table       default_ctc_table;
+
+
+static const uint16_t
+default_ctc_table_data[IA_CSS_VAMEM_2_CTC_TABLE_SIZE] = {
+	0,  384,  837,  957, 1011, 1062, 1083, 1080,
+	1078, 1077, 1053, 1039, 1012,  992,  969,  951,
+	929,  906,  886,  866,  845,  823,  809,  790,
+	772,  758,  741,  726,  711,  701,  688,  675,
+	666,  656,  648,  639,  633,  626,  618,  612,
+	603,  594,  582,  572,  557,  545,  529,  516,
+	504,  491,  480,  467,  459,  447,  438,  429,
+	419,  412,  404,  397,  389,  382,  376,  368,
+	363,  357,  351,  345,  340,  336,  330,  326,
+	321,  318,  312,  308,  304,  300,  297,  294,
+	291,  286,  284,  281,  278,  275,  271,  268,
+	261,  257,  251,  245,  240,  235,  232,  225,
+	223,  218,  213,  209,  206,  204,  199,  197,
+	193,  189,  186,  185,  183,  179,  177,  175,
+	172,  170,  169,  167,  164,  164,  162,  160,
+	158,  157,  156,  154,  154,  152,  151,  150,
+	149,  148,  146,  147,  146,  144,  143,  143,
+	142,  141,  140,  141,  139,  138,  138,  138,
+	137,  136,  136,  135,  134,  134,  134,  133,
+	132,  132,  131,  130,  131,  130,  129,  128,
+	129,  127,  127,  127,  127,  125,  125,  125,
+	123,  123,  122,  120,  118,  115,  114,  111,
+	110,  108,  106,  105,  103,  102,  100,   99,
+	97,   97,   96,   95,   94,   93,   93,   91,
+	91,   91,   90,   90,   89,   89,   88,   88,
+	89,   88,   88,   87,   87,   87,   87,   86,
+	87,   87,   86,   87,   86,   86,   84,   84,
+	82,   80,   78,   76,   74,   72,   70,   68,
+	67,   65,   62,   60,   58,   56,   55,   54,
+	53,   51,   49,   49,   47,   45,   45,   45,
+	41,   40,   39,   39,   34,   33,   34,   32,
+	25,   23,   24,   20,   13,    9,   12,    0,
+	0
+};
+
+
+void
+ia_css_config_ctc_table(void)
+{
+	memcpy(default_ctc_table.data.vamem_2, default_ctc_table_data,
+	       sizeof(default_ctc_table_data));
+	default_ctc_table.vamem_type     = IA_CSS_VAMEM_TYPE_2;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h
new file mode 100644
index 000000000..33e8a0545
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC_TABLE_HOST_H
+#define __IA_CSS_CTC_TABLE_HOST_H
+
+#include "ia_css_ctc_types.h"
+
+extern struct ia_css_ctc_table default_ctc_table;
+
+void ia_css_config_ctc_table(void);
+
+#endif /* __IA_CSS_CTC_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h
new file mode 100644
index 000000000..b2d42f3c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h
@@ -0,0 +1,111 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_CTC_TYPES_H
+#define __IA_CSS_CTC_TYPES_H
+
+#include <linux/bitops.h>
+
+/* @file
+* CSS-API header file for Chroma Tone Control parameters.
+*/
+
+/* Fractional bits for CTC gain (used only for ISP1).
+ *
+ *  IA_CSS_CTC_COEF_SHIFT(=13) includes not only the fractional bits
+ *  of gain(=8), but also the bits(=5) to convert chroma
+ *  from 13bit precision to 8bit precision.
+ *
+ *    Gain (struct ia_css_ctc_table) : u5.8
+ *    Input(Chorma) : s0.12 (13bit precision)
+ *    Output(Chorma): s0.7  (8bit precision)
+ *    Output = (Input * Gain) >> IA_CSS_CTC_COEF_SHIFT
+ */
+#define IA_CSS_CTC_COEF_SHIFT          13
+
+/* Number of elements in the CTC table. */
+#define IA_CSS_VAMEM_1_CTC_TABLE_SIZE_LOG2      10
+/* Number of elements in the CTC table. */
+#define IA_CSS_VAMEM_1_CTC_TABLE_SIZE           BIT(IA_CSS_VAMEM_1_CTC_TABLE_SIZE_LOG2)
+
+/* Number of elements in the CTC table. */
+#define IA_CSS_VAMEM_2_CTC_TABLE_SIZE_LOG2      8
+/* Number of elements in the CTC table. */
+#define IA_CSS_VAMEM_2_CTC_TABLE_SIZE           ((1U << IA_CSS_VAMEM_2_CTC_TABLE_SIZE_LOG2) + 1)
+
+enum ia_css_vamem_type {
+	IA_CSS_VAMEM_TYPE_1,
+	IA_CSS_VAMEM_TYPE_2
+};
+
+/* Chroma Tone Control configuration.
+ *
+ *  ISP block: CTC2 (CTC by polygonal line approximation)
+ * (ISP1: CTC1 (CTC by look-up table) is used.)
+ *  ISP2: CTC2 is used.
+ */
+struct ia_css_ctc_config {
+	u16 y0;	/** 1st kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 y1;	/** 2nd kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 y2;	/** 3rd kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 y3;	/** 4th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 y4;	/** 5th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 y5;	/** 6th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	u16 ce_gain_exp;	/** Common exponent of y-axis gain.
+				u8.0, [0,13],
+				default/ineffective 1 */
+	u16 x1;	/** 2nd kneepoint luma.
+				u0.13, [0,8191], constraints: 0<x1<x2,
+				default/ineffective 1024 */
+	u16 x2;	/** 3rd kneepoint luma.
+				u0.13, [0,8191], constraints: x1<x2<x3,
+				default/ineffective 2048 */
+	u16 x3;	/** 4th kneepoint luma.
+				u0.13, [0,8191], constraints: x2<x3<x4,
+				default/ineffective 6144 */
+	u16 x4;	/** 5tn kneepoint luma.
+				u0.13, [0,8191], constraints: x3<x4<8191,
+				default/ineffective 7168 */
+};
+
+union ia_css_ctc_data {
+	u16 vamem_1[IA_CSS_VAMEM_1_CTC_TABLE_SIZE];
+	u16 vamem_2[IA_CSS_VAMEM_2_CTC_TABLE_SIZE];
+};
+
+/* CTC table, used for Chroma Tone Control.
+ *
+ *  ISP block: CTC1 (CTC by look-up table)
+ *  ISP1: CTC1 is used.
+ * (ISP2: CTC2 (CTC by polygonal line approximation) is used.)
+ */
+struct ia_css_ctc_table {
+	enum ia_css_vamem_type vamem_type;
+	union ia_css_ctc_data data;
+};
+
+#endif /* __IA_CSS_CTC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.c
new file mode 100644
index 000000000..25e3f0822
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.c
@@ -0,0 +1,79 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+#include "ia_css_de.host.h"
+
+const struct ia_css_de_config default_de_config = {
+	0,
+	0,
+	0
+};
+
+void
+ia_css_de_encode(
+    struct sh_css_isp_de_params *to,
+    const struct ia_css_de_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->pixelnoise =
+	    uDIGIT_FITTING(from->pixelnoise, 16, SH_CSS_BAYER_BITS);
+	to->c1_coring_threshold =
+	    uDIGIT_FITTING(from->c1_coring_threshold, 16,
+			   SH_CSS_BAYER_BITS);
+	to->c2_coring_threshold =
+	    uDIGIT_FITTING(from->c2_coring_threshold, 16,
+			   SH_CSS_BAYER_BITS);
+}
+
+void
+ia_css_de_dump(
+    const struct sh_css_isp_de_params *de,
+    unsigned int level)
+{
+	if (!de) return;
+	ia_css_debug_dtrace(level, "Demosaic:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "de_pixelnoise", de->pixelnoise);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "de_c1_coring_threshold",
+			    de->c1_coring_threshold);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "de_c2_coring_threshold",
+			    de->c2_coring_threshold);
+}
+
+void
+ia_css_de_debug_dtrace(
+    const struct ia_css_de_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.pixelnoise=%d, config.c1_coring_threshold=%d, config.c2_coring_threshold=%d\n",
+			    config->pixelnoise,
+			    config->c1_coring_threshold, config->c2_coring_threshold);
+}
+
+void
+ia_css_init_de_state(
+    void/*struct sh_css_isp_de_vmem_state*/ * state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.h
new file mode 100644
index 000000000..cb9106202
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de.host.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE_HOST_H
+#define __IA_CSS_DE_HOST_H
+
+#include "ia_css_de_types.h"
+#include "ia_css_de_param.h"
+
+extern const struct ia_css_de_config default_de_config;
+
+void
+ia_css_de_encode(
+    struct sh_css_isp_de_params *to,
+    const struct ia_css_de_config *from,
+    unsigned int size);
+
+void
+ia_css_de_dump(
+    const struct sh_css_isp_de_params *de,
+    unsigned int level);
+
+void
+ia_css_de_debug_dtrace(
+    const struct ia_css_de_config *config,
+    unsigned int level);
+
+void
+ia_css_init_de_state(
+    void/*struct sh_css_isp_de_vmem_state*/ * state,
+    size_t size);
+
+#endif /* __IA_CSS_DE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_param.h
new file mode 100644
index 000000000..2070ce040
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_param.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE_PARAM_H
+#define __IA_CSS_DE_PARAM_H
+
+#include "type_support.h"
+
+/* DE (Demosaic) */
+struct sh_css_isp_de_params {
+	s32 pixelnoise;
+	s32 c1_coring_threshold;
+	s32 c2_coring_threshold;
+};
+
+#endif /* __IA_CSS_DE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_types.h
new file mode 100644
index 000000000..daac1275c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_1.0/ia_css_de_types.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE_TYPES_H
+#define __IA_CSS_DE_TYPES_H
+
+/* @file
+* CSS-API header file for Demosaic (bayer-to-YCgCo) parameters.
+*/
+
+/* Demosaic (bayer-to-YCgCo) configuration.
+ *
+ *  ISP block: DE1
+ *  ISP1: DE1 is used.
+ * (ISP2: DE2 is used.)
+ */
+struct ia_css_de_config {
+	ia_css_u0_16 pixelnoise; /** Pixel noise used in moire elimination.
+				u0.16, [0,65535],
+				default 0, ineffective 0 */
+	ia_css_u0_16 c1_coring_threshold; /** Coring threshold for C1.
+				This is the same as nr_config.threshold_cb.
+				u0.16, [0,65535],
+				default 128(0.001953125), ineffective 0 */
+	ia_css_u0_16 c2_coring_threshold; /** Coring threshold for C2.
+				This is the same as nr_config.threshold_cr.
+				u0.16, [0,65535],
+				default 128(0.001953125), ineffective 0 */
+};
+
+#endif /* __IA_CSS_DE_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.c
new file mode 100644
index 000000000..f90da3929
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.c
@@ -0,0 +1,54 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_de2.host.h"
+
+const struct ia_css_ecd_config default_ecd_config = {
+	(1 << (ISP_VEC_ELEMBITS - 1)) * 2 / 3,	/* 2/3 */
+	(1 << (ISP_VEC_ELEMBITS - 1)) - 1,	/* 1.0 */
+	0,					/* 0.0 */
+};
+
+void
+ia_css_ecd_encode(
+    struct sh_css_isp_ecd_params *to,
+    const struct ia_css_ecd_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->zip_strength = from->zip_strength;
+	to->fc_strength  = from->fc_strength;
+	to->fc_debias    = from->fc_debias;
+}
+
+void
+ia_css_ecd_dump(
+    const struct sh_css_isp_ecd_params *ecd,
+    unsigned int level);
+
+void
+ia_css_ecd_debug_dtrace(
+    const struct ia_css_ecd_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.zip_strength=%d, config.fc_strength=%d, config.fc_debias=%d\n",
+			    config->zip_strength,
+			    config->fc_strength, config->fc_debias);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.h
new file mode 100644
index 000000000..294f619a3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2.host.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE2_HOST_H
+#define __IA_CSS_DE2_HOST_H
+
+#include "ia_css_de2_types.h"
+#include "ia_css_de2_param.h"
+
+extern const struct ia_css_ecd_config default_ecd_config;
+
+void
+ia_css_ecd_encode(
+    struct sh_css_isp_ecd_params *to,
+    const struct ia_css_ecd_config *from,
+    unsigned int size);
+
+void
+ia_css_ecd_dump(
+    const struct sh_css_isp_ecd_params *ecd,
+    unsigned int level);
+
+void
+ia_css_ecd_debug_dtrace(
+    const struct ia_css_ecd_config *config, unsigned int level);
+
+#endif /* __IA_CSS_DE2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_param.h
new file mode 100644
index 000000000..4c9d5c630
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_param.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE2_PARAM_H
+#define __IA_CSS_DE2_PARAM_H
+
+#include "type_support.h"
+
+/* Reuse DE1 params and extend them */
+#include "../de_1.0/ia_css_de_param.h"
+
+/* DE (Demosaic) */
+struct sh_css_isp_ecd_params {
+	s32 zip_strength;
+	s32 fc_strength;
+	s32 fc_debias;
+};
+
+#endif /* __IA_CSS_DE2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_types.h
new file mode 100644
index 000000000..372cd9d2b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/de/de_2/ia_css_de2_types.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DE2_TYPES_H
+#define __IA_CSS_DE2_TYPES_H
+
+/* @file
+* CSS-API header file for Demosaicing parameters.
+*/
+
+/* Eigen Color Demosaicing configuration.
+ *
+ *  ISP block: DE2
+ * (ISP1: DE1 is used.)
+ *  ISP2: DE2 is used.
+ */
+struct ia_css_ecd_config {
+	u16 zip_strength;	/** Strength of zipper reduction.
+				u0.13, [0,8191],
+				default 5489(0.67), ineffective 0 */
+	u16 fc_strength;	/** Strength of false color reduction.
+				u0.13, [0,8191],
+				default 8191(almost 1.0), ineffective 0 */
+	u16 fc_debias;	/** Prevent color change
+				     on noise or Gr/Gb imbalance.
+				u0.13, [0,8191],
+				default 0, ineffective 0 */
+};
+
+#endif /* __IA_CSS_DE2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.c
new file mode 100644
index 000000000..eff428c67
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.c
@@ -0,0 +1,132 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_dp.host.h"
+
+/* We use a different set of DPC configuration parameters when
+ * DPC is used before OBC and NORM. Currently these parameters
+ * are used in usecases which selects both BDS and DPC.
+ **/
+const struct ia_css_dp_config default_dp_10bpp_config = {
+	1024,
+	2048,
+	32768,
+	32768,
+	32768,
+	32768
+};
+
+const struct ia_css_dp_config default_dp_config = {
+	8192,
+	2048,
+	32768,
+	32768,
+	32768,
+	32768
+};
+
+void
+ia_css_dp_encode(
+    struct sh_css_isp_dp_params *to,
+    const struct ia_css_dp_config *from,
+    unsigned int size)
+{
+	int gain = from->gain;
+	int gr   = from->gr;
+	int r    = from->r;
+	int b    = from->b;
+	int gb   = from->gb;
+
+	(void)size;
+	to->threshold_single =
+	    SH_CSS_BAYER_MAXVAL;
+	to->threshold_2adjacent =
+	    uDIGIT_FITTING(from->threshold, 16, SH_CSS_BAYER_BITS);
+	to->gain =
+	    uDIGIT_FITTING(from->gain, 8, SH_CSS_DP_GAIN_SHIFT);
+
+	to->coef_rr_gr =
+	    uDIGIT_FITTING(gain * gr / r, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_rr_gb =
+	    uDIGIT_FITTING(gain * gb / r, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_bb_gb =
+	    uDIGIT_FITTING(gain * gb / b, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_bb_gr =
+	    uDIGIT_FITTING(gain * gr / b, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_gr_rr =
+	    uDIGIT_FITTING(gain * r / gr, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_gr_bb =
+	    uDIGIT_FITTING(gain * b / gr, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_gb_bb =
+	    uDIGIT_FITTING(gain * b / gb, 8, SH_CSS_DP_GAIN_SHIFT);
+	to->coef_gb_rr =
+	    uDIGIT_FITTING(gain * r / gb, 8, SH_CSS_DP_GAIN_SHIFT);
+}
+
+void
+ia_css_dp_dump(
+    const struct sh_css_isp_dp_params *dp,
+    unsigned int level)
+{
+	if (!dp) return;
+	ia_css_debug_dtrace(level, "Defect Pixel Correction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dp_threshold_single_w_2adj_on",
+			    dp->threshold_single);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dp_threshold_2adj_w_2adj_on",
+			    dp->threshold_2adjacent);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dp_gain", dp->gain);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_rr_gr", dp->coef_rr_gr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_rr_gb", dp->coef_rr_gb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_bb_gb", dp->coef_bb_gb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_bb_gr", dp->coef_bb_gr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_gr_rr", dp->coef_gr_rr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_gr_bb", dp->coef_gr_bb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_gb_bb", dp->coef_gb_bb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "dpc_coef_gb_rr", dp->coef_gb_rr);
+}
+
+void
+ia_css_dp_debug_dtrace(
+    const struct ia_css_dp_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.threshold=%d, config.gain=%d\n",
+			    config->threshold, config->gain);
+}
+
+void
+ia_css_init_dp_state(
+    void/*struct sh_css_isp_dp_vmem_state*/ * state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.h
new file mode 100644
index 000000000..e5f5a2716
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp.host.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DP_HOST_H
+#define __IA_CSS_DP_HOST_H
+
+#include "ia_css_dp_types.h"
+#include "ia_css_dp_param.h"
+
+extern const struct ia_css_dp_config default_dp_config;
+
+/* ISP2401 */
+extern const struct ia_css_dp_config default_dp_10bpp_config;
+
+void
+ia_css_dp_encode(
+    struct sh_css_isp_dp_params *to,
+    const struct ia_css_dp_config *from,
+    unsigned int size);
+
+void
+ia_css_dp_dump(
+    const struct sh_css_isp_dp_params *dp,
+    unsigned int level);
+
+void
+ia_css_dp_debug_dtrace(
+    const struct ia_css_dp_config *config,
+    unsigned int level);
+
+void
+ia_css_init_dp_state(
+    void/*struct sh_css_isp_dp_vmem_state*/ * state,
+    size_t size);
+
+#endif /* __IA_CSS_DP_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_param.h
new file mode 100644
index 000000000..e0e7f2d48
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_param.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DP_PARAM_H
+#define __IA_CSS_DP_PARAM_H
+
+#include "type_support.h"
+#include "bnr/bnr_1.0/ia_css_bnr_param.h"
+
+/* DP (Defect Pixel Correction) */
+struct sh_css_isp_dp_params {
+	s32 threshold_single;
+	s32 threshold_2adjacent;
+	s32 gain;
+	s32 coef_rr_gr;
+	s32 coef_rr_gb;
+	s32 coef_bb_gb;
+	s32 coef_bb_gr;
+	s32 coef_gr_rr;
+	s32 coef_gr_bb;
+	s32 coef_gb_bb;
+	s32 coef_gb_rr;
+};
+
+#endif /* __IA_CSS_DP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_types.h
new file mode 100644
index 000000000..c1666ebf1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dp/dp_1.0/ia_css_dp_types.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DP_TYPES_H
+#define __IA_CSS_DP_TYPES_H
+
+/* @file
+* CSS-API header file for Defect Pixel Correction (DPC) parameters.
+*/
+
+/* Defect Pixel Correction configuration.
+ *
+ *  ISP block: DPC1 (DPC after WB)
+ *             DPC2 (DPC before WB)
+ *  ISP1: DPC1 is used.
+ *  ISP2: DPC2 is used.
+ */
+struct ia_css_dp_config {
+	ia_css_u0_16 threshold; /** The threshold of defect pixel correction,
+			      representing the permissible difference of
+			      intensity between one pixel and its
+			      surrounding pixels. Smaller values result
+				in more frequent pixel corrections.
+				u0.16, [0,65535],
+				default 8192, ineffective 65535 */
+	ia_css_u8_8 gain;	 /** The sensitivity of mis-correction. ISP will
+			      miss a lot of defects if the value is set
+				too large.
+				u8.8, [0,65535],
+				default 4096, ineffective 65535 */
+	u32 gr;	/* unsigned <integer_bits>.<16-integer_bits> */
+	u32 r;	/* unsigned <integer_bits>.<16-integer_bits> */
+	u32 b;	/* unsigned <integer_bits>.<16-integer_bits> */
+	u32 gb;	/* unsigned <integer_bits>.<16-integer_bits> */
+};
+
+#endif /* __IA_CSS_DP_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.c
new file mode 100644
index 000000000..f6fe064bd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.c
@@ -0,0 +1,66 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_dpc2.host.h"
+#include "assert_support.h"
+
+void
+ia_css_dpc2_encode(
+    struct ia_css_isp_dpc2_params *to,
+    const struct ia_css_dpc2_config *from,
+    size_t size)
+{
+	(void)size;
+
+	assert((from->metric1 >= 0) && (from->metric1 <= METRIC1_ONE_FP));
+	assert((from->metric3 >= 0) && (from->metric3 <= METRIC3_ONE_FP));
+	assert((from->metric2 >= METRIC2_ONE_FP) &&
+	       (from->metric2 < 256 * METRIC2_ONE_FP));
+	assert((from->wb_gain_gr > 0) && (from->wb_gain_gr < 16 * WBGAIN_ONE_FP));
+	assert((from->wb_gain_r  > 0) && (from->wb_gain_r  < 16 * WBGAIN_ONE_FP));
+	assert((from->wb_gain_b  > 0) && (from->wb_gain_b  < 16 * WBGAIN_ONE_FP));
+	assert((from->wb_gain_gb > 0) && (from->wb_gain_gb < 16 * WBGAIN_ONE_FP));
+
+	to->metric1 = from->metric1;
+	to->metric2 = from->metric2;
+	to->metric3 = from->metric3;
+
+	to->wb_gain_gr = from->wb_gain_gr;
+	to->wb_gain_r  = from->wb_gain_r;
+	to->wb_gain_b  = from->wb_gain_b;
+	to->wb_gain_gb = from->wb_gain_gb;
+}
+
+/* TODO: AM: This needs a proper implementation. */
+void
+ia_css_init_dpc2_state(
+    void *state,
+    size_t size)
+{
+	(void)state;
+	(void)size;
+}
+
+#ifndef IA_CSS_NO_DEBUG
+/* TODO: AM: This needs a proper implementation. */
+void
+ia_css_dpc2_debug_dtrace(
+    const struct ia_css_dpc2_config *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.h
new file mode 100644
index 000000000..f6e019a65
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2.host.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DPC2_HOST_H
+#define __IA_CSS_DPC2_HOST_H
+
+#include "ia_css_dpc2_types.h"
+#include "ia_css_dpc2_param.h"
+
+void
+ia_css_dpc2_encode(
+    struct ia_css_isp_dpc2_params *to,
+    const struct ia_css_dpc2_config *from,
+    size_t size);
+
+void
+ia_css_init_dpc2_state(
+    void *state,
+    size_t size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_dpc2_debug_dtrace(
+    const struct ia_css_dpc2_config *config,
+    unsigned int level);
+#endif
+
+#endif /* __IA_CSS_DPC2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_param.h
new file mode 100644
index 000000000..1ccceadbb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_param.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DPC2_PARAM_H
+#define __IA_CSS_DPC2_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h" /* for VMEM_ARRAY*/
+
+/* 4 planes : GR, R, B, GB */
+#define NUM_PLANES		4
+
+/* ToDo: Move this to testsetup */
+#define MAX_FRAME_SIMDWIDTH	30
+
+/* 3 lines state per color plane input_line_state */
+#define DPC2_STATE_INPUT_BUFFER_HEIGHT	(3 * NUM_PLANES)
+/* Each plane has width equal to half frame line */
+#define DPC2_STATE_INPUT_BUFFER_WIDTH	CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line state per color plane for local deviation state*/
+#define DPC2_STATE_LOCAL_DEVIATION_BUFFER_HEIGHT	(1 * NUM_PLANES)
+/* Each plane has width equal to half frame line */
+#define DPC2_STATE_LOCAL_DEVIATION_BUFFER_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* MINMAX state buffer stores 1 full input line (GR-R color line) */
+#define DPC2_STATE_SECOND_MINMAX_BUFFER_HEIGHT	1
+#define DPC2_STATE_SECOND_MINMAX_BUFFER_WIDTH	MAX_FRAME_SIMDWIDTH
+
+struct ia_css_isp_dpc2_params {
+	s32 metric1;
+	s32 metric2;
+	s32 metric3;
+	s32 wb_gain_gr;
+	s32 wb_gain_r;
+	s32 wb_gain_b;
+	s32 wb_gain_gb;
+};
+
+#endif /* __IA_CSS_DPC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_types.h
new file mode 100644
index 000000000..f742a8dc1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dpc2/ia_css_dpc2_types.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DPC2_TYPES_H
+#define __IA_CSS_DPC2_TYPES_H
+
+/* @file
+* CSS-API header file for Defect Pixel Correction 2 (DPC2) parameters.
+*/
+
+#include "type_support.h"
+
+/**@{*/
+/* Floating point constants for different metrics. */
+#define METRIC1_ONE_FP	BIT(12)
+#define METRIC2_ONE_FP	BIT(5)
+#define METRIC3_ONE_FP	BIT(12)
+#define WBGAIN_ONE_FP	BIT(9)
+/**@}*/
+
+/**@{*/
+/* Defect Pixel Correction 2 configuration.
+ *
+ * \brief DPC2 public parameters.
+ * \details Struct with all parameters for the Defect Pixel Correction 2
+ * kernel that can be set from the CSS API.
+ *
+ * ISP block: DPC1 (DPC after WB)
+ *            DPC2 (DPC before WB)
+ * ISP1: DPC1 is used.
+ * ISP2: DPC2 is used.
+ *
+ */
+struct ia_css_dpc2_config {
+	/**@{*/
+	s32 metric1;
+	s32 metric2;
+	s32 metric3;
+	s32 wb_gain_gr;
+	s32 wb_gain_r;
+	s32 wb_gain_b;
+	s32 wb_gain_gb;
+	/**@}*/
+};
+
+/**@}*/
+
+#endif /* __IA_CSS_DPC2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c
new file mode 100644
index 000000000..07ce5b4f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c
@@ -0,0 +1,298 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_frame_public.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+
+#include "ia_css_types.h"
+#include "ia_css_host_data.h"
+#include "sh_css_param_dvs.h"
+#include "sh_css_params.h"
+#include "ia_css_binary.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+#include "ia_css_dvs.host.h"
+
+static const struct ia_css_dvs_configuration default_config = {
+	.info = (struct ia_css_frame_info *)NULL,
+};
+
+void
+ia_css_dvs_config(
+    struct sh_css_isp_dvs_isp_config *to,
+    const struct ia_css_dvs_configuration  *from,
+    unsigned int size)
+{
+	(void)size;
+	to->num_horizontal_blocks =
+	    DVS_NUM_BLOCKS_X(from->info->res.width);
+	to->num_vertical_blocks =
+	    DVS_NUM_BLOCKS_Y(from->info->res.height);
+}
+
+int ia_css_dvs_configure(const struct ia_css_binary     *binary,
+			 const struct ia_css_frame_info *info)
+{
+	struct ia_css_dvs_configuration config = default_config;
+
+	config.info = info;
+
+	return ia_css_configure_dvs(binary, &config);
+}
+
+static void
+convert_coords_to_ispparams(
+    struct ia_css_host_data *gdc_warp_table,
+    const struct ia_css_dvs_6axis_config *config,
+    unsigned int i_stride,
+    unsigned int o_width,
+    unsigned int o_height,
+    unsigned int uv_flag)
+{
+	unsigned int i, j;
+	gdc_warp_param_mem_t s = { 0 };
+	unsigned int x00, x01, x10, x11,
+		 y00, y01, y10, y11;
+
+	unsigned int xmin, ymin, xmax, ymax;
+	unsigned int topleft_x, topleft_y, bottom_x, bottom_y,
+		 topleft_x_frac, topleft_y_frac;
+	unsigned int dvs_interp_envelope = (DVS_GDC_INTERP_METHOD == HRT_GDC_BLI_MODE ?
+					    DVS_GDC_BLI_INTERP_ENVELOPE : DVS_GDC_BCI_INTERP_ENVELOPE);
+
+	/* number of blocks per height and width */
+	unsigned int num_blocks_y =  (uv_flag ? DVS_NUM_BLOCKS_Y_CHROMA(
+					  o_height) : DVS_NUM_BLOCKS_Y(o_height));
+	unsigned int num_blocks_x =  (uv_flag ? DVS_NUM_BLOCKS_X_CHROMA(
+					  o_width)  : DVS_NUM_BLOCKS_X(
+					  o_width)); // round num_x up to blockdim_x, if it concerns the Y0Y1 block (uv_flag==0) round up to even
+
+	unsigned int in_stride = i_stride * DVS_INPUT_BYTES_PER_PIXEL;
+	unsigned int width, height;
+	unsigned int *xbuff = NULL;
+	unsigned int *ybuff = NULL;
+	struct gdc_warp_param_mem_s *ptr;
+
+	assert(config);
+	assert(gdc_warp_table);
+	assert(gdc_warp_table->address);
+
+	ptr = (struct gdc_warp_param_mem_s *)gdc_warp_table->address;
+
+	ptr += (2 * uv_flag); /* format is Y0 Y1 UV, so UV starts at 3rd position */
+
+	if (uv_flag == 0) {
+		xbuff = config->xcoords_y;
+		ybuff = config->ycoords_y;
+		width = config->width_y;
+		height = config->height_y;
+	} else {
+		xbuff = config->xcoords_uv;
+		ybuff = config->ycoords_uv;
+		width = config->width_uv;
+		height = config->height_uv;
+	}
+
+	IA_CSS_LOG("blockdim_x %d blockdim_y %d",
+		   DVS_BLOCKDIM_X, DVS_BLOCKDIM_Y_LUMA >> uv_flag);
+	IA_CSS_LOG("num_blocks_x %d num_blocks_y %d", num_blocks_x, num_blocks_y);
+	IA_CSS_LOG("width %d height %d", width, height);
+
+	assert(width == num_blocks_x +
+	       1); // the width and height of the provided morphing table should be 1 more than the number of blocks
+	assert(height == num_blocks_y + 1);
+
+	for (j = 0; j < num_blocks_y; j++) {
+		for (i = 0; i < num_blocks_x; i++) {
+			x00 = xbuff[j * width + i];
+			x01 = xbuff[j * width + (i + 1)];
+			x10 = xbuff[(j + 1) * width + i];
+			x11 = xbuff[(j + 1) * width + (i + 1)];
+
+			y00 = ybuff[j * width + i];
+			y01 = ybuff[j * width + (i + 1)];
+			y10 = ybuff[(j + 1) * width + i];
+			y11 = ybuff[(j + 1) * width + (i + 1)];
+
+			xmin = min(x00, x10);
+			xmax = max(x01, x11);
+			ymin = min(y00, y01);
+			ymax = max(y10, y11);
+
+			/* Assert that right column's X is greater */
+			assert(x01 >= xmin);
+			assert(x11 >= xmin);
+			/* Assert that bottom row's Y is greater */
+			assert(y10 >= ymin);
+			assert(y11 >= ymin);
+
+			topleft_y = ymin >> DVS_COORD_FRAC_BITS;
+			topleft_x = ((xmin >> DVS_COORD_FRAC_BITS)
+				     >> XMEM_ALIGN_LOG2)
+				    << (XMEM_ALIGN_LOG2);
+			s.in_addr_offset = topleft_y * in_stride + topleft_x;
+
+			/* similar to topleft_y calculation, but round up if ymax
+			 * has any fraction bits */
+			bottom_y = CEIL_DIV(ymax, 1 << DVS_COORD_FRAC_BITS);
+			s.in_block_height = bottom_y - topleft_y + dvs_interp_envelope;
+
+			bottom_x = CEIL_DIV(xmax, 1 << DVS_COORD_FRAC_BITS);
+			s.in_block_width = bottom_x - topleft_x + dvs_interp_envelope;
+
+			topleft_x_frac = topleft_x << (DVS_COORD_FRAC_BITS);
+			topleft_y_frac = topleft_y << (DVS_COORD_FRAC_BITS);
+
+			s.p0_x = x00 - topleft_x_frac;
+			s.p1_x = x01 - topleft_x_frac;
+			s.p2_x = x10 - topleft_x_frac;
+			s.p3_x = x11 - topleft_x_frac;
+
+			s.p0_y = y00 - topleft_y_frac;
+			s.p1_y = y01 - topleft_y_frac;
+			s.p2_y = y10 - topleft_y_frac;
+			s.p3_y = y11 - topleft_y_frac;
+
+			// block should fit within the boundingbox.
+			assert(s.p0_x < (s.in_block_width << DVS_COORD_FRAC_BITS));
+			assert(s.p1_x < (s.in_block_width << DVS_COORD_FRAC_BITS));
+			assert(s.p2_x < (s.in_block_width << DVS_COORD_FRAC_BITS));
+			assert(s.p3_x < (s.in_block_width << DVS_COORD_FRAC_BITS));
+			assert(s.p0_y < (s.in_block_height << DVS_COORD_FRAC_BITS));
+			assert(s.p1_y < (s.in_block_height << DVS_COORD_FRAC_BITS));
+			assert(s.p2_y < (s.in_block_height << DVS_COORD_FRAC_BITS));
+			assert(s.p3_y < (s.in_block_height << DVS_COORD_FRAC_BITS));
+
+			// block size should be greater than zero.
+			assert(s.p0_x < s.p1_x);
+			assert(s.p2_x < s.p3_x);
+			assert(s.p0_y < s.p2_y);
+			assert(s.p1_y < s.p3_y);
+
+#if 0
+			printf("j: %d\ti:%d\n", j, i);
+			printf("offset: %d\n", s.in_addr_offset);
+			printf("p0_x: %d\n", s.p0_x);
+			printf("p0_y: %d\n", s.p0_y);
+			printf("p1_x: %d\n", s.p1_x);
+			printf("p1_y: %d\n", s.p1_y);
+			printf("p2_x: %d\n", s.p2_x);
+			printf("p2_y: %d\n", s.p2_y);
+			printf("p3_x: %d\n", s.p3_x);
+			printf("p3_y: %d\n", s.p3_y);
+
+			printf("p0_x_nofrac[0]: %d\n", s.p0_x >> DVS_COORD_FRAC_BITS);
+			printf("p0_y_nofrac[1]: %d\n", s.p0_y >> DVS_COORD_FRAC_BITS);
+			printf("p1_x_nofrac[2]: %d\n", s.p1_x >> DVS_COORD_FRAC_BITS);
+			printf("p1_y_nofrac[3]: %d\n", s.p1_y >> DVS_COORD_FRAC_BITS);
+			printf("p2_x_nofrac[0]: %d\n", s.p2_x >> DVS_COORD_FRAC_BITS);
+			printf("p2_y_nofrac[1]: %d\n", s.p2_y >> DVS_COORD_FRAC_BITS);
+			printf("p3_x_nofrac[2]: %d\n", s.p3_x >> DVS_COORD_FRAC_BITS);
+			printf("p3_y_nofrac[3]: %d\n", s.p3_y >> DVS_COORD_FRAC_BITS);
+			printf("\n");
+#endif
+
+			*ptr = s;
+
+			// storage format:
+			// Y0 Y1 UV0 Y2 Y3 UV1
+			/* if uv_flag equals true increment with 2 incase x is odd, this to
+			skip the uv position. */
+			if (uv_flag)
+				ptr += 3;
+			else
+				ptr += (1 + (i & 1));
+		}
+	}
+}
+
+struct ia_css_host_data *
+convert_allocate_dvs_6axis_config(
+    const struct ia_css_dvs_6axis_config *dvs_6axis_config,
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *dvs_in_frame_info)
+{
+	unsigned int i_stride;
+	unsigned int o_width;
+	unsigned int o_height;
+	struct ia_css_host_data *me;
+
+	assert(binary);
+	assert(dvs_6axis_config);
+	assert(dvs_in_frame_info);
+
+	me = ia_css_host_data_allocate((size_t)((DVS_6AXIS_BYTES(binary) / 2) * 3));
+
+	if (!me)
+		return NULL;
+
+	/*DVS only supports input frame of YUV420 or NV12. Fail for all other cases*/
+	assert((dvs_in_frame_info->format == IA_CSS_FRAME_FORMAT_NV12)
+	       || (dvs_in_frame_info->format == IA_CSS_FRAME_FORMAT_YUV420));
+
+	i_stride  = dvs_in_frame_info->padded_width;
+
+	o_width  = binary->out_frame_info[0].res.width;
+	o_height = binary->out_frame_info[0].res.height;
+
+	/* Y plane */
+	convert_coords_to_ispparams(me, dvs_6axis_config,
+				    i_stride, o_width, o_height, 0);
+
+	if (dvs_in_frame_info->format == IA_CSS_FRAME_FORMAT_YUV420) {
+		/*YUV420 has half the stride for U/V plane*/
+		i_stride /= 2;
+	}
+
+	/* UV plane (packed inside the y plane) */
+	convert_coords_to_ispparams(me, dvs_6axis_config,
+				    i_stride, o_width / 2, o_height / 2, 1);
+
+	return me;
+}
+
+int
+store_dvs_6axis_config(
+    const struct ia_css_dvs_6axis_config *dvs_6axis_config,
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *dvs_in_frame_info,
+    ia_css_ptr ddr_addr_y) {
+	struct ia_css_host_data *me;
+
+	assert(dvs_6axis_config);
+	assert(ddr_addr_y != mmgr_NULL);
+	assert(dvs_in_frame_info);
+
+	me = convert_allocate_dvs_6axis_config(dvs_6axis_config,
+					       binary,
+					       dvs_in_frame_info);
+
+	if (!me)
+	{
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return -ENOMEM;
+	}
+
+	ia_css_params_store_ia_css_host_data(
+	    ddr_addr_y,
+	    me);
+	ia_css_host_data_free(me);
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h
new file mode 100644
index 000000000..332aa5496
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DVS_HOST_H
+#define __IA_CSS_DVS_HOST_H
+
+#include "ia_css_frame_public.h"
+#include "ia_css_binary.h"
+#include "sh_css_params.h"
+
+#include "ia_css_types.h"
+#include "ia_css_dvs_types.h"
+#include "ia_css_dvs_param.h"
+
+/* For bilinear interpolation, we need to add +1 to input block height calculation.
+ * For bicubic interpolation, we will need to add +3 instaed */
+#define DVS_GDC_BLI_INTERP_ENVELOPE 1
+#define DVS_GDC_BCI_INTERP_ENVELOPE 3
+
+void
+ia_css_dvs_config(
+    struct sh_css_isp_dvs_isp_config      *to,
+    const struct ia_css_dvs_configuration *from,
+    unsigned int size);
+
+int ia_css_dvs_configure(const struct ia_css_binary     *binary,
+			 const struct ia_css_frame_info *from);
+
+void
+convert_dvs_6axis_config(
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_binary *binary);
+
+struct ia_css_host_data *
+convert_allocate_dvs_6axis_config(
+    const struct ia_css_dvs_6axis_config *dvs_6axis_config,
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *dvs_in_frame_info);
+
+int
+store_dvs_6axis_config(
+    const struct ia_css_dvs_6axis_config *dvs_6axis_config,
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *dvs_in_frame_info,
+    ia_css_ptr ddr_addr_y);
+
+#endif /* __IA_CSS_DVS_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h
new file mode 100644
index 000000000..2963bb10b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DVS_PARAM_H
+#define __IA_CSS_DVS_PARAM_H
+
+#include <type_support.h>
+
+#if !defined(ENABLE_TPROXY) && !defined(ENABLE_CRUN_FOR_TD) && !defined(PARAMBIN_GENERATION)
+#include "dma.h"
+#endif /* !defined(ENABLE_TPROXY) && !defined(ENABLE_CRUN_FOR_TD) */
+
+#include "uds/uds_1.0/ia_css_uds_param.h"
+
+/* dvserence frame */
+struct sh_css_isp_dvs_isp_config {
+	u32 num_horizontal_blocks;
+	u32 num_vertical_blocks;
+};
+
+#endif /* __IA_CSS_DVS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h
new file mode 100644
index 000000000..e99ff0ce8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_DVS_TYPES_H
+#define __IA_CSS_DVS_TYPES_H
+
+/* DVS frame
+ *
+ *  ISP block: dvs frame
+ */
+
+#include "ia_css_frame_public.h"
+
+struct ia_css_dvs_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+#endif /* __IA_CSS_DVS_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.c
new file mode 100644
index 000000000..bfea78171
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.c
@@ -0,0 +1,339 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+
+#include "type_support.h"
+#include "assert_support.h"
+#include "math_support.h" /* for min and max */
+
+#include "ia_css_eed1_8.host.h"
+
+/* WARNING1: Number of inv points should be less or equal to 16,
+ * due to implementation limitation. See kernel design document
+ * for more details.
+ * WARNING2: Do not modify the number of inv points without correcting
+ * the EED1_8 kernel implementation assumptions.
+ */
+#define NUMBER_OF_CHGRINV_POINTS 15
+#define NUMBER_OF_TCINV_POINTS 9
+#define NUMBER_OF_FCINV_POINTS 9
+
+static const s16 chgrinv_x[NUMBER_OF_CHGRINV_POINTS] = {
+	0, 16, 64, 144, 272, 448, 672, 976,
+	1376, 1888, 2528, 3312, 4256, 5376, 6688
+};
+
+static const s16 chgrinv_a[NUMBER_OF_CHGRINV_POINTS] = {
+	-7171, -256, -29, -3456, -1071, -475, -189, -102,
+	    -48, -38, -10, -9, -7, -6, 0
+    };
+
+static const s16 chgrinv_b[NUMBER_OF_CHGRINV_POINTS] = {
+	8191, 1021, 256, 114, 60, 37, 24, 17,
+	12, 9, 6, 5, 4, 3, 2
+};
+
+static const s16 chgrinv_c[NUMBER_OF_CHGRINV_POINTS] = {
+	1, 1, 1, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0
+};
+
+static const s16 tcinv_x[NUMBER_OF_TCINV_POINTS] = {
+	0, 4, 11, 23, 42, 68, 102, 148, 205
+};
+
+static const s16 tcinv_a[NUMBER_OF_TCINV_POINTS] = {
+	-6364, -631, -126, -34, -13, -6, -4452, -2156, 0
+    };
+
+static const s16 tcinv_b[NUMBER_OF_TCINV_POINTS] = {
+	8191, 1828, 726, 352, 197, 121, 80, 55, 40
+};
+
+static const s16 tcinv_c[NUMBER_OF_TCINV_POINTS] = {
+	1, 1, 1, 1, 1, 1, 0, 0, 0
+};
+
+static const s16 fcinv_x[NUMBER_OF_FCINV_POINTS] = {
+	0, 80, 216, 456, 824, 1344, 2040, 2952, 4096
+};
+
+static const s16 fcinv_a[NUMBER_OF_FCINV_POINTS] = {
+	-5244, -486, -86, -2849, -961, -400, -180, -86, 0
+    };
+
+static const s16 fcinv_b[NUMBER_OF_FCINV_POINTS] = {
+	8191, 1637, 607, 287, 159, 98, 64, 44, 32
+};
+
+static const s16 fcinv_c[NUMBER_OF_FCINV_POINTS] = {
+	1, 1, 1, 0, 0, 0, 0, 0, 0
+};
+
+void
+ia_css_eed1_8_vmem_encode(
+    struct eed1_8_vmem_params *to,
+    const struct ia_css_eed1_8_config *from,
+    size_t size)
+{
+	unsigned int i, j, base;
+	const unsigned int total_blocks = 4;
+	const unsigned int shuffle_block = 16;
+
+	(void)size;
+
+	/* Init */
+	for (i = 0; i < ISP_VEC_NELEMS; i++) {
+		to->e_dew_enh_x[0][i] = 0;
+		to->e_dew_enh_y[0][i] = 0;
+		to->e_dew_enh_a[0][i] = 0;
+		to->e_dew_enh_f[0][i] = 0;
+		to->chgrinv_x[0][i] = 0;
+		to->chgrinv_a[0][i] = 0;
+		to->chgrinv_b[0][i] = 0;
+		to->chgrinv_c[0][i] = 0;
+		to->tcinv_x[0][i] = 0;
+		to->tcinv_a[0][i] = 0;
+		to->tcinv_b[0][i] = 0;
+		to->tcinv_c[0][i] = 0;
+		to->fcinv_x[0][i] = 0;
+		to->fcinv_a[0][i] = 0;
+		to->fcinv_b[0][i] = 0;
+		to->fcinv_c[0][i] = 0;
+	}
+
+	/* Constraints on dew_enhance_seg_x and dew_enhance_seg_y:
+	 * - values should be greater or equal to 0.
+	 * - values should be ascending.
+	 * - value of index zero is equal to 0.
+	 */
+
+	/* Checking constraints: */
+	/* TODO: investigate if an assert is the right way to report that
+	 * the constraints are violated.
+	 */
+	for (j = 0; j < IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS; j++) {
+		assert(from->dew_enhance_seg_x[j] > -1);
+		assert(from->dew_enhance_seg_y[j] > -1);
+	}
+
+	for (j = 1; j < IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS; j++) {
+		assert(from->dew_enhance_seg_x[j] > from->dew_enhance_seg_x[j - 1]);
+		assert(from->dew_enhance_seg_y[j] > from->dew_enhance_seg_y[j - 1]);
+	}
+
+	assert(from->dew_enhance_seg_x[0] == 0);
+	assert(from->dew_enhance_seg_y[0] == 0);
+
+	/* Constraints on chgrinv_x, tcinv_x and fcinv_x:
+	 * - values should be greater or equal to 0.
+	 * - values should be ascending.
+	 * - value of index zero is equal to 0.
+	 */
+	assert(chgrinv_x[0] == 0);
+	assert(tcinv_x[0] == 0);
+	assert(fcinv_x[0] == 0);
+
+	for (j = 1; j < NUMBER_OF_CHGRINV_POINTS; j++) {
+		assert(chgrinv_x[j] > chgrinv_x[j - 1]);
+	}
+
+	for (j = 1; j < NUMBER_OF_TCINV_POINTS; j++) {
+		assert(tcinv_x[j] > tcinv_x[j - 1]);
+	}
+
+	for (j = 1; j < NUMBER_OF_FCINV_POINTS; j++) {
+		assert(fcinv_x[j] > fcinv_x[j - 1]);
+	}
+
+	/* The implementation of the calulating 1/x is based on the availability
+	 * of the OP_vec_shuffle16 operation.
+	 * A 64 element vector is split up in 4 blocks of 16 element. Each array is copied to
+	 * a vector 4 times, (starting at 0, 16, 32 and 48). All array elements are copied or
+	 * initialised as described in the KFS. The remaining elements of a vector are set to 0.
+	 */
+	/* TODO: guard this code with above assumptions */
+	for (i = 0; i < total_blocks; i++) {
+		base = shuffle_block * i;
+
+		for (j = 0; j < IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS; j++) {
+			to->e_dew_enh_x[0][base + j] = min_t(int, max_t(int,
+							     from->dew_enhance_seg_x[j], 0),
+							     8191);
+			to->e_dew_enh_y[0][base + j] = min_t(int, max_t(int,
+							     from->dew_enhance_seg_y[j], -8192),
+							     8191);
+		}
+
+		for (j = 0; j < (IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1); j++) {
+			to->e_dew_enh_a[0][base + j] = min_t(int, max_t(int,
+							     from->dew_enhance_seg_slope[j],
+							     -8192), 8191);
+			/* Convert dew_enhance_seg_exp to flag:
+			 * 0 -> 0
+			 * 1...13 -> 1
+			 */
+			to->e_dew_enh_f[0][base + j] = (min_t(int, max_t(int,
+							      from->dew_enhance_seg_exp[j],
+							      0), 13) > 0);
+		}
+
+		/* Hard-coded to 0, in order to be able to handle out of
+		 * range input in the same way as the other segments.
+		 * See KFS for more details.
+		 */
+		to->e_dew_enh_a[0][base + (IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1)] = 0;
+		to->e_dew_enh_f[0][base + (IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1)] = 0;
+
+		for (j = 0; j < NUMBER_OF_CHGRINV_POINTS; j++) {
+			to->chgrinv_x[0][base + j] = chgrinv_x[j];
+			to->chgrinv_a[0][base + j] = chgrinv_a[j];
+			to->chgrinv_b[0][base + j] = chgrinv_b[j];
+			to->chgrinv_c[0][base + j] = chgrinv_c[j];
+		}
+
+		for (j = 0; j < NUMBER_OF_TCINV_POINTS; j++) {
+			to->tcinv_x[0][base + j] = tcinv_x[j];
+			to->tcinv_a[0][base + j] = tcinv_a[j];
+			to->tcinv_b[0][base + j] = tcinv_b[j];
+			to->tcinv_c[0][base + j] = tcinv_c[j];
+		}
+
+		for (j = 0; j < NUMBER_OF_FCINV_POINTS; j++) {
+			to->fcinv_x[0][base + j] = fcinv_x[j];
+			to->fcinv_a[0][base + j] = fcinv_a[j];
+			to->fcinv_b[0][base + j] = fcinv_b[j];
+			to->fcinv_c[0][base + j] = fcinv_c[j];
+		}
+	}
+}
+
+void
+ia_css_eed1_8_encode(
+    struct eed1_8_dmem_params *to,
+    const struct ia_css_eed1_8_config *from,
+    size_t size)
+{
+	int i;
+	int min_exp = 0;
+
+	(void)size;
+
+	to->rbzp_strength = from->rbzp_strength;
+
+	to->fcstrength = from->fcstrength;
+	to->fcthres_0 = from->fcthres_0;
+	to->fc_sat_coef = from->fc_sat_coef;
+	to->fc_coring_prm = from->fc_coring_prm;
+	to->fc_slope = from->fcthres_1 - from->fcthres_0;
+
+	to->aerel_thres0 = from->aerel_thres0;
+	to->aerel_gain0 = from->aerel_gain0;
+	to->aerel_thres_diff = from->aerel_thres1 - from->aerel_thres0;
+	to->aerel_gain_diff = from->aerel_gain1 - from->aerel_gain0;
+
+	to->derel_thres0 = from->derel_thres0;
+	to->derel_gain0 = from->derel_gain0;
+	to->derel_thres_diff = (from->derel_thres1 - from->derel_thres0);
+	to->derel_gain_diff = (from->derel_gain1 - from->derel_gain0);
+
+	to->coring_pos0 = from->coring_pos0;
+	to->coring_pos_diff = (from->coring_pos1 - from->coring_pos0);
+	to->coring_neg0 = from->coring_neg0;
+	to->coring_neg_diff = (from->coring_neg1 - from->coring_neg0);
+
+	/* Note: (ISP_VEC_ELEMBITS -1)
+	 * TODO: currently the testbench does not support to use
+	 * ISP_VEC_ELEMBITS. Investigate how to fix this
+	 */
+	to->gain_exp = (13 - from->gain_exp);
+	to->gain_pos0 = from->gain_pos0;
+	to->gain_pos_diff = (from->gain_pos1 - from->gain_pos0);
+	to->gain_neg0 = from->gain_neg0;
+	to->gain_neg_diff = (from->gain_neg1 - from->gain_neg0);
+
+	to->margin_pos0 = from->pos_margin0;
+	to->margin_pos_diff = (from->pos_margin1 - from->pos_margin0);
+	to->margin_neg0 = from->neg_margin0;
+	to->margin_neg_diff = (from->neg_margin1 - from->neg_margin0);
+
+	/* Encode DEWEnhance exp (e_dew_enh_asr) */
+	for (i = 0; i < (IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1); i++) {
+		min_exp = max(min_exp, from->dew_enhance_seg_exp[i]);
+	}
+	to->e_dew_enh_asr = 13 - min(max(min_exp, 0), 13);
+
+	to->dedgew_max = from->dedgew_max;
+}
+
+void
+ia_css_init_eed1_8_state(
+    void *state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_eed1_8_debug_dtrace(
+    const struct ia_css_eed1_8_config *eed,
+    unsigned int level)
+{
+	if (!eed)
+		return;
+
+	ia_css_debug_dtrace(level, "Edge Enhancing Demosaic 1.8:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "rbzp_strength",
+			    eed->rbzp_strength);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "fcstrength", eed->fcstrength);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "fcthres_0", eed->fcthres_0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "fcthres_1", eed->fcthres_1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "fc_sat_coef", eed->fc_sat_coef);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "fc_coring_prm",
+			    eed->fc_coring_prm);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "aerel_thres0", eed->aerel_thres0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "aerel_gain0", eed->aerel_gain0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "aerel_thres1", eed->aerel_thres1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "aerel_gain1", eed->aerel_gain1);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "derel_thres0", eed->derel_thres0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "derel_gain0", eed->derel_gain0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "derel_thres1", eed->derel_thres1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "derel_gain1", eed->derel_gain1);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "coring_pos0", eed->coring_pos0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "coring_pos1", eed->coring_pos1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "coring_neg0", eed->coring_neg0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "coring_neg1", eed->coring_neg1);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "gain_exp", eed->gain_exp);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "gain_pos0", eed->gain_pos0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "gain_pos1", eed->gain_pos1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "gain_neg0", eed->gain_neg0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "gain_neg1", eed->gain_neg1);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "pos_margin0", eed->pos_margin0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "pos_margin1", eed->pos_margin1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "neg_margin0", eed->neg_margin0);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "neg_margin1", eed->neg_margin1);
+
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n", "dedgew_max", eed->dedgew_max);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.h
new file mode 100644
index 000000000..f1ad07e78
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8.host.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_EED1_8_HOST_H
+#define __IA_CSS_EED1_8_HOST_H
+
+#include "ia_css_eed1_8_types.h"
+#include "ia_css_eed1_8_param.h"
+
+void
+ia_css_eed1_8_vmem_encode(
+    struct eed1_8_vmem_params *to,
+    const struct ia_css_eed1_8_config *from,
+    size_t size);
+
+void
+ia_css_eed1_8_encode(
+    struct eed1_8_dmem_params *to,
+    const struct ia_css_eed1_8_config *from,
+    size_t size);
+
+void
+ia_css_init_eed1_8_state(
+    void *state,
+    size_t size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_eed1_8_debug_dtrace(
+    const struct ia_css_eed1_8_config *config,
+    unsigned int level);
+#endif
+
+#endif /* __IA_CSS_EED1_8_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_param.h
new file mode 100644
index 000000000..6fb3b38f4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_param.h
@@ -0,0 +1,154 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_EED1_8_PARAM_H
+#define __IA_CSS_EED1_8_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h" /* needed for VMEM_ARRAY */
+
+#include "ia_css_eed1_8_types.h" /* IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS */
+
+/* Configuration parameters: */
+
+/* Enable median for false color correction
+ * 0: Do not use median
+ * 1: Use median
+ * Default: 1
+ */
+#define EED1_8_FC_ENABLE_MEDIAN		1
+
+/* Coring Threshold minima
+ * Used in Tint color suppression.
+ * Default: 1
+ */
+#define EED1_8_CORINGTHMIN	1
+
+/* Define size of the state..... TODO: check if this is the correct place */
+/* 4 planes : GR, R, B, GB */
+#define NUM_PLANES	4
+
+/* 5 lines state per color plane input_line_state */
+#define EED1_8_STATE_INPUT_BUFFER_HEIGHT	(5 * NUM_PLANES)
+
+/* Each plane has width equal to half frame line */
+#define EED1_8_STATE_INPUT_BUFFER_WIDTH	CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line state per color plane LD_H state */
+#define EED1_8_STATE_LD_H_HEIGHT	(1 * NUM_PLANES)
+#define EED1_8_STATE_LD_H_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line state per color plane LD_V state */
+#define EED1_8_STATE_LD_V_HEIGHT	(1 * NUM_PLANES)
+#define EED1_8_STATE_LD_V_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line (single plane) state for D_Hr state */
+#define EED1_8_STATE_D_HR_HEIGHT	1
+#define EED1_8_STATE_D_HR_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line (single plane) state for D_Hb state */
+#define EED1_8_STATE_D_HB_HEIGHT	1
+#define EED1_8_STATE_D_HB_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 2 lines (single plane) state for D_Vr state */
+#define EED1_8_STATE_D_VR_HEIGHT	2
+#define EED1_8_STATE_D_VR_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 2 line (single plane) state for D_Vb state */
+#define EED1_8_STATE_D_VB_HEIGHT	2
+#define EED1_8_STATE_D_VB_WIDTH		CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 2 lines state for R and B (= 2 planes) rb_zipped_state */
+#define EED1_8_STATE_RB_ZIPPED_HEIGHT	(2 * 2)
+#define EED1_8_STATE_RB_ZIPPED_WIDTH	CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+#if EED1_8_FC_ENABLE_MEDIAN
+/* 1 full input line (GR-R color line) for Yc state */
+#define EED1_8_STATE_YC_HEIGHT	1
+#define EED1_8_STATE_YC_WIDTH	MAX_FRAME_SIMDWIDTH
+
+/* 1 line state per color plane Cg_state */
+#define EED1_8_STATE_CG_HEIGHT	(1 * NUM_PLANES)
+#define EED1_8_STATE_CG_WIDTH	CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 line state per color plane Co_state */
+#define EED1_8_STATE_CO_HEIGHT	(1 * NUM_PLANES)
+#define EED1_8_STATE_CO_WIDTH	CEIL_DIV(MAX_FRAME_SIMDWIDTH, 2)
+
+/* 1 full input line (GR-R color line) for AbsK state */
+#define EED1_8_STATE_ABSK_HEIGHT	1
+#define EED1_8_STATE_ABSK_WIDTH		MAX_FRAME_SIMDWIDTH
+#endif
+
+struct eed1_8_vmem_params {
+	VMEM_ARRAY(e_dew_enh_x, ISP_VEC_NELEMS);
+	VMEM_ARRAY(e_dew_enh_y, ISP_VEC_NELEMS);
+	VMEM_ARRAY(e_dew_enh_a, ISP_VEC_NELEMS);
+	VMEM_ARRAY(e_dew_enh_f, ISP_VEC_NELEMS);
+	VMEM_ARRAY(chgrinv_x, ISP_VEC_NELEMS);
+	VMEM_ARRAY(chgrinv_a, ISP_VEC_NELEMS);
+	VMEM_ARRAY(chgrinv_b, ISP_VEC_NELEMS);
+	VMEM_ARRAY(chgrinv_c, ISP_VEC_NELEMS);
+	VMEM_ARRAY(fcinv_x, ISP_VEC_NELEMS);
+	VMEM_ARRAY(fcinv_a, ISP_VEC_NELEMS);
+	VMEM_ARRAY(fcinv_b, ISP_VEC_NELEMS);
+	VMEM_ARRAY(fcinv_c, ISP_VEC_NELEMS);
+	VMEM_ARRAY(tcinv_x, ISP_VEC_NELEMS);
+	VMEM_ARRAY(tcinv_a, ISP_VEC_NELEMS);
+	VMEM_ARRAY(tcinv_b, ISP_VEC_NELEMS);
+	VMEM_ARRAY(tcinv_c, ISP_VEC_NELEMS);
+};
+
+/* EED (Edge Enhancing Demosaic) ISP parameters */
+struct eed1_8_dmem_params {
+	s32 rbzp_strength;
+
+	s32 fcstrength;
+	s32 fcthres_0;
+	s32 fc_sat_coef;
+	s32 fc_coring_prm;
+	s32 fc_slope;
+
+	s32 aerel_thres0;
+	s32 aerel_gain0;
+	s32 aerel_thres_diff;
+	s32 aerel_gain_diff;
+
+	s32 derel_thres0;
+	s32 derel_gain0;
+	s32 derel_thres_diff;
+	s32 derel_gain_diff;
+
+	s32 coring_pos0;
+	s32 coring_pos_diff;
+	s32 coring_neg0;
+	s32 coring_neg_diff;
+
+	s32 gain_exp;
+	s32 gain_pos0;
+	s32 gain_pos_diff;
+	s32 gain_neg0;
+	s32 gain_neg_diff;
+
+	s32 margin_pos0;
+	s32 margin_pos_diff;
+	s32 margin_neg0;
+	s32 margin_neg_diff;
+
+	s32 e_dew_enh_asr;
+	s32 dedgew_max;
+};
+
+#endif /* __IA_CSS_EED1_8_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_types.h
new file mode 100644
index 000000000..836e348c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/eed1_8/ia_css_eed1_8_types.h
@@ -0,0 +1,88 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_EED1_8_TYPES_H
+#define __IA_CSS_EED1_8_TYPES_H
+
+/* @file
+* CSS-API header file for Edge Enhanced Demosaic parameters.
+*/
+
+#include "type_support.h"
+
+/**
+ * \brief EED1_8 public parameters.
+ * \details Struct with all parameters for the EED1.8 kernel that can be set
+ * from the CSS API.
+ */
+
+/* parameter list is based on ISP261 CSS API public parameter list_all.xlsx from 28-01-2015 */
+
+/* Number of segments + 1 segment used in edge reliability enhancement
+ * Ineffective: N/A
+ * Default:	9
+ */
+#define IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS	9
+
+/* Edge Enhanced Demosaic configuration
+ *
+ * ISP2.6.1: EED1_8 is used.
+ */
+
+struct ia_css_eed1_8_config {
+	s32 rbzp_strength;	/** Strength of zipper reduction. */
+
+	s32 fcstrength;	/** Strength of false color reduction. */
+	s32 fcthres_0;	/** Threshold to prevent chroma coring due to noise or green disparity in dark region. */
+	s32 fcthres_1;	/** Threshold to prevent chroma coring due to noise or green disparity in bright region. */
+	s32 fc_sat_coef;	/** How much color saturation to maintain in high color saturation region. */
+	s32 fc_coring_prm;	/** Chroma coring coefficient for tint color suppression. */
+
+	s32 aerel_thres0;	/** Threshold for Non-Directional Reliability at dark region. */
+	s32 aerel_gain0;	/** Gain for Non-Directional Reliability at dark region. */
+	s32 aerel_thres1;	/** Threshold for Non-Directional Reliability at bright region. */
+	s32 aerel_gain1;	/** Gain for Non-Directional Reliability at bright region. */
+
+	s32 derel_thres0;	/** Threshold for Directional Reliability at dark region. */
+	s32 derel_gain0;	/** Gain for Directional Reliability at dark region. */
+	s32 derel_thres1;	/** Threshold for Directional Reliability at bright region. */
+	s32 derel_gain1;	/** Gain for Directional Reliability at bright region. */
+
+	s32 coring_pos0;	/** Positive Edge Coring Threshold in dark region. */
+	s32 coring_pos1;	/** Positive Edge Coring Threshold in bright region. */
+	s32 coring_neg0;	/** Negative Edge Coring Threshold in dark region. */
+	s32 coring_neg1;	/** Negative Edge Coring Threshold in bright region. */
+
+	s32 gain_exp;	/** Common Exponent of Gain. */
+	s32 gain_pos0;	/** Gain for Positive Edge in dark region. */
+	s32 gain_pos1;	/** Gain for Positive Edge in bright region. */
+	s32 gain_neg0;	/** Gain for Negative Edge in dark region. */
+	s32 gain_neg1;	/** Gain for Negative Edge in bright region. */
+
+	s32 pos_margin0;	/** Margin for Positive Edge in dark region. */
+	s32 pos_margin1;	/** Margin for Positive Edge in bright region. */
+	s32 neg_margin0;	/** Margin for Negative Edge in dark region. */
+	s32 neg_margin1;	/** Margin for Negative Edge in bright region. */
+
+	s32 dew_enhance_seg_x[IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS];		/** Segment data for directional edge weight: X. */
+	s32 dew_enhance_seg_y[IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS];		/** Segment data for directional edge weight: Y. */
+	s32 dew_enhance_seg_slope[(IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS -
+				   1)];	/** Segment data for directional edge weight: Slope. */
+	s32 dew_enhance_seg_exp[(IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS -
+				 1)];	/** Segment data for directional edge weight: Exponent. */
+	s32 dedgew_max;	/** Max Weight for Directional Edge. */
+};
+
+#endif /* __IA_CSS_EED1_8_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.c
new file mode 100644
index 000000000..bae1ca2cd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.c
@@ -0,0 +1,64 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_formats.host.h"
+#include "ia_css_types.h"
+#include "sh_css_defs.h"
+
+/*#include "sh_css_frac.h"*/
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+#include "ia_css_debug.h"
+#endif
+
+const struct ia_css_formats_config default_formats_config = {
+	1
+};
+
+void
+ia_css_formats_encode(
+    struct sh_css_isp_formats_params *to,
+    const struct ia_css_formats_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->video_full_range_flag = from->video_full_range_flag;
+}
+
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+void
+ia_css_formats_dump(
+    const struct sh_css_isp_formats_params *formats,
+    unsigned int level)
+{
+	if (!formats) return;
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "video_full_range_flag", formats->video_full_range_flag);
+}
+#endif
+
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+void
+ia_css_formats_debug_dtrace(
+    const struct ia_css_formats_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.video_full_range_flag=%d\n",
+			    config->video_full_range_flag);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.h
new file mode 100644
index 000000000..540423d85
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats.host.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FORMATS_HOST_H
+#define __IA_CSS_FORMATS_HOST_H
+
+#include "ia_css_formats_types.h"
+#include "ia_css_formats_param.h"
+
+extern const struct ia_css_formats_config default_formats_config;
+
+void
+ia_css_formats_encode(
+    struct sh_css_isp_formats_params *to,
+    const struct ia_css_formats_config *from,
+    unsigned int size);
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+void
+ia_css_formats_dump(
+    const struct sh_css_isp_formats_params *formats,
+    unsigned int level);
+#endif
+
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+void
+ia_css_formats_debug_dtrace(
+    const struct ia_css_formats_config *formats,
+    unsigned int level);
+#endif /*IA_CSS_NO_DEBUG*/
+
+#endif /* __IA_CSS_FORMATS_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_param.h
new file mode 100644
index 000000000..5275a1dad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_param.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FORMATS_PARAM_H
+#define __IA_CSS_FORMATS_PARAM_H
+
+#include "type_support.h"
+
+/* FORMATS (Format conversion) */
+struct sh_css_isp_formats_params {
+	s32 video_full_range_flag;
+};
+
+#endif /* __IA_CSS_FORMATS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_types.h
new file mode 100644
index 000000000..16b6a3ddc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fc/fc_1.0/ia_css_formats_types.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FORMATS_TYPES_H
+#define __IA_CSS_FORMATS_TYPES_H
+
+/* @file
+* CSS-API header file for output format parameters.
+*/
+
+#include "type_support.h"
+
+/* Formats configuration.
+ *
+ *  ISP block: FORMATS
+ *  ISP1: FORMATS is used.
+ *  ISP2: FORMATS is used.
+ */
+struct ia_css_formats_config {
+	u32 video_full_range_flag; /** selects the range of YUV output.
+				u8.0, [0,1],
+				default 1, ineffective n/a\n
+				1 - full range, luma 0-255, chroma 0-255\n
+				0 - reduced range, luma 16-235, chroma 16-240 */
+};
+
+#endif /* __IA_CSS_FORMATS_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h
new file mode 100644
index 000000000..6cd635f3e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FIXEDBDS_PARAM_H
+#define __IA_CSS_FIXEDBDS_PARAM_H
+
+#include "type_support.h"
+
+/* ISP2401 */
+#define BDS_UNIT 8
+#define FRAC_LOG 3
+#define FRAC_ACC BIT(FRAC_LOG)
+#if FRAC_ACC != BDS_UNIT
+#error "FRAC_ACC and BDS_UNIT need to be merged into one define"
+#endif
+
+struct sh_css_isp_bds_params {
+	int baf_strength;
+};
+
+#endif /* __IA_CSS_FIXEDBDS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h
new file mode 100644
index 000000000..3a55d4c69
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FIXEDBDS_TYPES_H
+#define __IA_CSS_FIXEDBDS_TYPES_H
+
+struct sh_css_bds_factor {
+	unsigned int numerator;
+	unsigned int denominator;
+	unsigned int bds_factor;
+};
+
+#endif	/*__IA_CSS_FIXEDBDS_TYPES_H*/
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c
new file mode 100644
index 000000000..57b5e11e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <assert_support.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_frame.h>
+#include <ia_css_binary.h>
+#include <ia_css_types.h>
+#include <sh_css_defs.h>
+#include <ia_css_debug.h>
+
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+
+#include "ia_css_fpn.host.h"
+
+void
+ia_css_fpn_encode(
+    struct sh_css_isp_fpn_params *to,
+    const struct ia_css_fpn_table *from,
+    unsigned int size)
+{
+	(void)size;
+	to->shift = from->shift;
+	to->enabled = from->data != NULL;
+}
+
+void
+ia_css_fpn_dump(
+    const struct sh_css_isp_fpn_params *fpn,
+    unsigned int level)
+{
+	if (!fpn) return;
+	ia_css_debug_dtrace(level, "Fixed Pattern Noise Reduction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "fpn_shift", fpn->shift);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "fpn_enabled", fpn->enabled);
+}
+
+int ia_css_fpn_config(struct sh_css_isp_fpn_isp_config *to,
+		      const struct ia_css_fpn_configuration *from,
+		      unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	int ret;
+
+	ret = ia_css_dma_configure_from_info(&to->port_b, from->info);
+	if (ret)
+		return ret;
+
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int ia_css_fpn_configure(const struct ia_css_binary     *binary,
+			 const struct ia_css_frame_info *info)
+{
+	struct ia_css_frame_info my_info = IA_CSS_BINARY_DEFAULT_FRAME_INFO;
+	const struct ia_css_fpn_configuration config = {
+		&my_info
+	};
+
+	my_info.res.width       = CEIL_DIV(info->res.width, 2);		/* Packed by 2x */
+	my_info.res.height      = info->res.height;
+	my_info.padded_width    = CEIL_DIV(info->padded_width, 2);	/* Packed by 2x */
+	my_info.format          = info->format;
+	my_info.raw_bit_depth   = FPN_BITS_PER_PIXEL;
+	my_info.raw_bayer_order = info->raw_bayer_order;
+	my_info.crop_info       = info->crop_info;
+
+	return ia_css_configure_fpn(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h
new file mode 100644
index 000000000..bd341fa28
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FPN_HOST_H
+#define __IA_CSS_FPN_HOST_H
+
+#include "ia_css_binary.h"
+#include "ia_css_fpn_types.h"
+#include "ia_css_fpn_param.h"
+
+void
+ia_css_fpn_encode(
+    struct sh_css_isp_fpn_params *to,
+    const struct ia_css_fpn_table *from,
+    unsigned int size);
+
+void
+ia_css_fpn_dump(
+    const struct sh_css_isp_fpn_params *fpn,
+    unsigned int level);
+
+int ia_css_fpn_config(struct sh_css_isp_fpn_isp_config      *to,
+		      const struct ia_css_fpn_configuration *from,
+		      unsigned int size);
+
+int ia_css_fpn_configure(const struct ia_css_binary     *binary,
+			 const struct ia_css_frame_info *from);
+
+#endif /* __IA_CSS_FPN_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h
new file mode 100644
index 000000000..b21415743
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FPN_PARAM_H
+#define __IA_CSS_FPN_PARAM_H
+
+#include "type_support.h"
+
+#include "dma.h"
+
+#define FPN_BITS_PER_PIXEL	16
+
+/* FPNR (Fixed Pattern Noise Reduction) */
+struct sh_css_isp_fpn_params {
+	s32 shift;
+	s32 enabled;
+};
+
+struct sh_css_isp_fpn_isp_config {
+	u32 width_a_over_b;
+	struct dma_port_config port_b;
+};
+
+#endif /* __IA_CSS_FPN_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h
new file mode 100644
index 000000000..14dc5e183
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FPN_TYPES_H
+#define __IA_CSS_FPN_TYPES_H
+
+/* @file
+* CSS-API header file for Fixed Pattern Noise parameters.
+*/
+
+/* Fixed Pattern Noise table.
+ *
+ *  This contains the fixed patterns noise values
+ *  obtained from a black frame capture.
+ *
+ *  "shift" should be set as the smallest value
+ *  which satisfies the requirement the maximum data is less than 64.
+ *
+ *  ISP block: FPN1
+ *  ISP1: FPN1 is used.
+ *  ISP2: FPN1 is used.
+ */
+
+struct ia_css_fpn_table {
+	s16 *data;		/** Table content (fixed patterns noise).
+					u0.[13-shift], [0,63] */
+	u32 width;		/** Table width (in pixels).
+					This is the input frame width. */
+	u32 height;	/** Table height (in pixels).
+					This is the input frame height. */
+	u32 shift;		/** Common exponent of table content.
+					u8.0, [0,13] */
+	u32 enabled;	/** Fpn is enabled.
+					bool */
+};
+
+struct ia_css_fpn_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+#endif /* __IA_CSS_FPN_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.c
new file mode 100644
index 000000000..7f3f87920
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.c
@@ -0,0 +1,118 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+#include "ia_css_debug.h"
+#endif
+#include "sh_css_frac.h"
+#include "vamem.h"
+
+#include "ia_css_gc.host.h"
+
+const struct ia_css_gc_config default_gc_config = {
+	0,
+	0
+};
+
+const struct ia_css_ce_config default_ce_config = {
+	0,
+	255
+};
+
+void
+ia_css_gc_encode(
+    struct sh_css_isp_gc_params *to,
+    const struct ia_css_gc_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->gain_k1 =
+	    uDIGIT_FITTING((int)from->gain_k1, 16,
+			   IA_CSS_GAMMA_GAIN_K_SHIFT);
+	to->gain_k2 =
+	    uDIGIT_FITTING((int)from->gain_k2, 16,
+			   IA_CSS_GAMMA_GAIN_K_SHIFT);
+}
+
+void
+ia_css_ce_encode(
+    struct sh_css_isp_ce_params *to,
+    const struct ia_css_ce_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->uv_level_min = from->uv_level_min;
+	to->uv_level_max = from->uv_level_max;
+}
+
+void
+ia_css_gc_vamem_encode(
+    struct sh_css_isp_gc_vamem_params *to,
+    const struct ia_css_gamma_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->gc,  &from->data, sizeof(to->gc));
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_gc_dump(
+    const struct sh_css_isp_gc_params *gc,
+    unsigned int level)
+{
+	if (!gc) return;
+	ia_css_debug_dtrace(level, "Gamma Correction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "gamma_gain_k1", gc->gain_k1);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "gamma_gain_k2", gc->gain_k2);
+}
+
+void
+ia_css_ce_dump(
+    const struct sh_css_isp_ce_params *ce,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level, "Chroma Enhancement:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ce_uv_level_min", ce->uv_level_min);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ce_uv_level_max", ce->uv_level_max);
+}
+
+void
+ia_css_gc_debug_dtrace(
+    const struct ia_css_gc_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.gain_k1=%d, config.gain_k2=%d\n",
+			    config->gain_k1, config->gain_k2);
+}
+
+void
+ia_css_ce_debug_dtrace(
+    const struct ia_css_ce_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.uv_level_min=%d, config.uv_level_max=%d\n",
+			    config->uv_level_min, config->uv_level_max);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.h
new file mode 100644
index 000000000..c2dc1574a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc.host.h
@@ -0,0 +1,66 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC_HOST_H
+#define __IA_CSS_GC_HOST_H
+
+#include "ia_css_gc_param.h"
+#include "ia_css_gc_table.host.h"
+
+extern const struct ia_css_gc_config default_gc_config;
+extern const struct ia_css_ce_config default_ce_config;
+
+void
+ia_css_gc_encode(
+    struct sh_css_isp_gc_params *to,
+    const struct ia_css_gc_config *from,
+    unsigned int size);
+
+void
+ia_css_gc_vamem_encode(
+    struct sh_css_isp_gc_vamem_params *to,
+    const struct ia_css_gamma_table *from,
+    unsigned int size);
+
+void
+ia_css_ce_encode(
+    struct sh_css_isp_ce_params *to,
+    const struct ia_css_ce_config *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_gc_dump(
+    const struct sh_css_isp_gc_params *gc,
+    unsigned int level);
+
+void
+ia_css_ce_dump(
+    const struct sh_css_isp_ce_params *ce,
+    unsigned int level);
+
+void
+ia_css_gc_debug_dtrace(
+    const struct ia_css_gc_config *config,
+    unsigned int level);
+
+void
+ia_css_ce_debug_dtrace(
+    const struct ia_css_ce_config *config,
+    unsigned int level);
+
+#endif
+
+#endif /* __IA_CSS_GC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_param.h
new file mode 100644
index 000000000..a81233add
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_param.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC_PARAM_H
+#define __IA_CSS_GC_PARAM_H
+
+#include "type_support.h"
+#ifndef PIPE_GENERATION
+#ifdef __ISP
+#define __INLINE_VAMEM__
+#endif
+#include "vamem.h"
+#include "ia_css_gc_types.h"
+
+#if defined(IS_VAMEM_VERSION_1)
+#define SH_CSS_ISP_GAMMA_TABLE_SIZE_LOG2 IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_GC_TABLE_SIZE	 IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE
+#elif defined(IS_VAMEM_VERSION_2)
+#define SH_CSS_ISP_GAMMA_TABLE_SIZE_LOG2 IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_GC_TABLE_SIZE	 IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE
+#else
+#error "Undefined vamem version"
+#endif
+
+#else
+/* For pipe generation, the size is not relevant */
+#define SH_CSS_ISP_GC_TABLE_SIZE 0
+#endif
+
+#define GAMMA_OUTPUT_BITS		8
+#define GAMMA_OUTPUT_MAX_VAL		((1 << GAMMA_OUTPUT_BITS) - 1)
+
+/* GC (Gamma Correction) */
+struct sh_css_isp_gc_params {
+	s32 gain_k1;
+	s32 gain_k2;
+};
+
+/* CE (Chroma Enhancement) */
+struct sh_css_isp_ce_params {
+	s32 uv_level_min;
+	s32 uv_level_max;
+};
+
+/* This should be vamem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_gc_vamem_params {
+	u16 gc[SH_CSS_ISP_GC_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_GC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c
new file mode 100644
index 000000000..7dbe2dc05
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c
@@ -0,0 +1,71 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+
+#include <type_support.h>
+#include "system_global.h"
+#include "vamem.h"
+#include "ia_css_types.h"
+#include "ia_css_gc_table.host.h"
+
+
+struct ia_css_gamma_table default_gamma_table;
+
+static const uint16_t
+default_gamma_table_data[IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE] = {
+	0,   4,   8,  12,  17,  21,  27,  32,
+	38,  44,  49,  55,  61,  66,  71,  76,
+	80,  84,  88,  92,  95,  98, 102, 105,
+	108, 110, 113, 116, 118, 121, 123, 126,
+	128, 130, 132, 135, 137, 139, 141, 143,
+	145, 146, 148, 150, 152, 153, 155, 156,
+	158, 160, 161, 162, 164, 165, 166, 168,
+	169, 170, 171, 172, 174, 175, 176, 177,
+	178, 179, 180, 181, 182, 183, 184, 184,
+	185, 186, 187, 188, 189, 189, 190, 191,
+	192, 192, 193, 194, 195, 195, 196, 197,
+	197, 198, 198, 199, 200, 200, 201, 201,
+	202, 203, 203, 204, 204, 205, 205, 206,
+	206, 207, 207, 208, 208, 209, 209, 210,
+	210, 210, 211, 211, 212, 212, 213, 213,
+	214, 214, 214, 215, 215, 216, 216, 216,
+	217, 217, 218, 218, 218, 219, 219, 220,
+	220, 220, 221, 221, 222, 222, 222, 223,
+	223, 223, 224, 224, 225, 225, 225, 226,
+	226, 226, 227, 227, 227, 228, 228, 228,
+	229, 229, 229, 230, 230, 230, 231, 231,
+	231, 232, 232, 232, 233, 233, 233, 234,
+	234, 234, 234, 235, 235, 235, 236, 236,
+	236, 237, 237, 237, 237, 238, 238, 238,
+	239, 239, 239, 239, 240, 240, 240, 241,
+	241, 241, 241, 242, 242, 242, 242, 243,
+	243, 243, 243, 244, 244, 244, 245, 245,
+	245, 245, 246, 246, 246, 246, 247, 247,
+	247, 247, 248, 248, 248, 248, 249, 249,
+	249, 249, 250, 250, 250, 250, 251, 251,
+	251, 251, 252, 252, 252, 252, 253, 253,
+	253, 253, 254, 254, 254, 254, 255, 255,
+	255
+};
+
+
+void
+ia_css_config_gamma_table(void)
+{
+	memcpy(default_gamma_table.data.vamem_2, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	default_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_2;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h
new file mode 100644
index 000000000..ee6fa07b3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC_TABLE_HOST_H
+#define __IA_CSS_GC_TABLE_HOST_H
+
+#include "ia_css_gc_types.h"
+
+extern struct ia_css_gamma_table default_gamma_table;
+
+void ia_css_config_gamma_table(void);
+
+#endif /* __IA_CSS_GC_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_types.h
new file mode 100644
index 000000000..ccd3d91a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_1.0/ia_css_gc_types.h
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC_TYPES_H
+#define __IA_CSS_GC_TYPES_H
+
+/* @file
+* CSS-API header file for Gamma Correction parameters.
+*/
+
+#include "isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h"  /* FIXME: Needed for ia_css_vamem_type */
+
+/* Fractional bits for GAMMA gain */
+#define IA_CSS_GAMMA_GAIN_K_SHIFT      13
+
+/* Number of elements in the gamma table. */
+#define IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE_LOG2    10
+#define IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE         BIT(IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE_LOG2)
+
+/* Number of elements in the gamma table. */
+#define IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE_LOG2    8
+#define IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE         ((1U << IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE_LOG2) + 1)
+
+/* Gamma table, used for Y(Luma) Gamma Correction.
+ *
+ *  ISP block: GC1 (YUV Gamma Correction)
+ *  ISP1: GC1 is used.
+ * (ISP2: GC2(sRGB Gamma Correction) is used.)
+ */
+/** IA_CSS_VAMEM_TYPE_1(ISP2300) or
+     IA_CSS_VAMEM_TYPE_2(ISP2400) */
+union ia_css_gc_data {
+	u16 vamem_1[IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE];
+	/** Y(Luma) Gamma table on vamem type 1. u0.8, [0,255] */
+	u16 vamem_2[IA_CSS_VAMEM_2_GAMMA_TABLE_SIZE];
+	/** Y(Luma) Gamma table on vamem type 2. u0.8, [0,255] */
+};
+
+struct ia_css_gamma_table {
+	enum ia_css_vamem_type vamem_type;
+	union ia_css_gc_data data;
+};
+
+/* Gamma Correction configuration (used only for YUV Gamma Correction).
+ *
+ *  ISP block: GC1 (YUV Gamma Correction)
+ *  ISP1: GC1 is used.
+ * (ISP2: GC2 (sRGB Gamma Correction) is used.)
+  */
+struct ia_css_gc_config {
+	u16 gain_k1; /** Gain to adjust U after YUV Gamma Correction.
+				u0.16, [0,65535],
+				default/ineffective 19000(0.29) */
+	u16 gain_k2; /** Gain to adjust V after YUV Gamma Correction.
+				u0.16, [0,65535],
+				default/ineffective 19000(0.29) */
+};
+
+/* Chroma Enhancement configuration.
+ *
+ *  This parameter specifies range of chroma output level.
+ *  The standard range is [0,255] or [16,240].
+ *
+ *  ISP block: CE1
+ *  ISP1: CE1 is used.
+ * (ISP2: CE1 is not used.)
+ */
+struct ia_css_ce_config {
+	u8 uv_level_min; /** Minimum of chroma output level.
+				u0.8, [0,255], default/ineffective 0 */
+	u8 uv_level_max; /** Maximum of chroma output level.
+				u0.8, [0,255], default/ineffective 255 */
+};
+
+/* Multi-Axes Color Correction (MACC) configuration.
+ *
+ *  ISP block: MACC2 (MACC by matrix and exponent(ia_css_macc_config))
+ * (ISP1: MACC1 (MACC by only matrix) is used.)
+ *  ISP2: MACC2 is used.
+ */
+struct ia_css_macc_config {
+	u8 exp;	/** Common exponent of ia_css_macc_table.
+				u8.0, [0,13], default 1, ineffective 1 */
+};
+
+#endif /* __IA_CSS_GC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.c
new file mode 100644
index 000000000..76209b7c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.c
@@ -0,0 +1,110 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+#include "ia_css_debug.h"
+#endif
+#include "csc/csc_1.0/ia_css_csc.host.h"
+#include "vamem.h"
+
+#include "ia_css_gc2.host.h"
+
+const struct ia_css_cc_config default_yuv2rgb_cc_config = {
+	12,
+	{4096, -4096, 4096, 4096, 4096, 0, 4096, -4096, -4096}
+};
+
+const struct ia_css_cc_config default_rgb2yuv_cc_config = {
+	13,
+	{2449, 4809, 934, -1382, -2714, 4096, 4096, -3430, -666}
+};
+
+void
+ia_css_yuv2rgb_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size)
+{
+	ia_css_encode_cc(to, from, size);
+}
+
+void
+ia_css_rgb2yuv_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size)
+{
+	ia_css_encode_cc(to, from, size);
+}
+
+void
+ia_css_r_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->gc,  &from->data, sizeof(to->gc));
+}
+
+void
+ia_css_g_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->gc,  &from->data, sizeof(to->gc));
+}
+
+void
+ia_css_b_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->gc,  &from->data, sizeof(to->gc));
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_yuv2rgb_dump(
+    const struct sh_css_isp_csc_params *yuv2rgb,
+    unsigned int level)
+{
+	ia_css_cc_dump(yuv2rgb, level, "YUV to RGB Conversion");
+}
+
+void
+ia_css_rgb2yuv_dump(
+    const struct sh_css_isp_csc_params *rgb2yuv,
+    unsigned int level)
+{
+	ia_css_cc_dump(rgb2yuv, level, "RGB to YUV Conversion");
+}
+
+void
+ia_css_rgb_gamma_table_debug_dtrace(
+    const struct ia_css_rgb_gamma_table *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.h
new file mode 100644
index 000000000..eabf78737
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2.host.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC2_HOST_H
+#define __IA_CSS_GC2_HOST_H
+
+#include "ia_css_gc2_types.h"
+#include "ia_css_gc2_param.h"
+#include "ia_css_gc2_table.host.h"
+
+extern const struct ia_css_cc_config default_yuv2rgb_cc_config;
+extern const struct ia_css_cc_config default_rgb2yuv_cc_config;
+
+void
+ia_css_yuv2rgb_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size);
+
+void
+ia_css_rgb2yuv_encode(
+    struct sh_css_isp_csc_params *to,
+    const struct ia_css_cc_config *from,
+    unsigned int size);
+
+void
+ia_css_r_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size);
+
+void
+ia_css_g_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size);
+
+void
+ia_css_b_gamma_vamem_encode(
+    struct sh_css_isp_rgb_gamma_vamem_params *to,
+    const struct ia_css_rgb_gamma_table *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_yuv2rgb_dump(
+    const struct sh_css_isp_csc_params *yuv2rgb,
+    unsigned int level);
+
+void
+ia_css_rgb2yuv_dump(
+    const struct sh_css_isp_csc_params *rgb2yuv,
+    unsigned int level);
+
+void
+ia_css_rgb_gamma_table_debug_dtrace(
+    const struct ia_css_rgb_gamma_table *config,
+    unsigned int level);
+
+#define ia_css_yuv2rgb_debug_dtrace ia_css_cc_config_debug_dtrace
+#define ia_css_rgb2yuv_debug_dtrace ia_css_cc_config_debug_dtrace
+#define ia_css_r_gamma_debug_dtrace ia_css_rgb_gamma_table_debug_dtrace
+#define ia_css_g_gamma_debug_dtrace ia_css_rgb_gamma_table_debug_dtrace
+#define ia_css_b_gamma_debug_dtrace ia_css_rgb_gamma_table_debug_dtrace
+
+#endif
+
+#endif /* __IA_CSS_GC2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_param.h
new file mode 100644
index 000000000..af456e75e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_param.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC2_PARAM_H
+#define __IA_CSS_GC2_PARAM_H
+
+#include "type_support.h"
+/* Extend GC1 */
+#include "ia_css_gc2_types.h"
+#include "gc/gc_1.0/ia_css_gc_param.h"
+#include "csc/csc_1.0/ia_css_csc_param.h"
+
+#ifndef PIPE_GENERATION
+#if defined(IS_VAMEM_VERSION_1)
+#define SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE IA_CSS_VAMEM_1_RGB_GAMMA_TABLE_SIZE
+#elif defined(IS_VAMEM_VERSION_2)
+#define SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE
+#else
+#error "Undefined vamem version"
+#endif
+
+#else
+/* For pipe generation, the size is not relevant */
+#define SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE 0
+#endif
+
+/* This should be vamem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_rgb_gamma_vamem_params {
+	u16 gc[SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_GC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c
new file mode 100644
index 000000000..347950119
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c
@@ -0,0 +1,80 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+
+#include <type_support.h>
+#include "system_global.h"
+#include "vamem.h"
+#include "ia_css_types.h"
+#include "ia_css_gc2_table.host.h"
+
+struct ia_css_rgb_gamma_table default_r_gamma_table;
+struct ia_css_rgb_gamma_table default_g_gamma_table;
+struct ia_css_rgb_gamma_table default_b_gamma_table;
+
+/* Identical default gamma table for R, G, and B. */
+
+
+static const uint16_t
+default_gamma_table_data[IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE] = {
+	0,   72,  144,  216,  288,  360,  426,  486,
+	541,  592,  641,  687,  730,  772,  812,  850,
+	887,  923,  958,  991, 1024, 1055, 1086, 1117,
+	1146, 1175, 1203, 1230, 1257, 1284, 1310, 1335,
+	1360, 1385, 1409, 1433, 1457, 1480, 1502, 1525,
+	1547, 1569, 1590, 1612, 1632, 1653, 1674, 1694,
+	1714, 1734, 1753, 1772, 1792, 1811, 1829, 1848,
+	1866, 1884, 1902, 1920, 1938, 1955, 1973, 1990,
+	2007, 2024, 2040, 2057, 2074, 2090, 2106, 2122,
+	2138, 2154, 2170, 2185, 2201, 2216, 2231, 2247,
+	2262, 2277, 2291, 2306, 2321, 2335, 2350, 2364,
+	2378, 2393, 2407, 2421, 2435, 2449, 2462, 2476,
+	2490, 2503, 2517, 2530, 2543, 2557, 2570, 2583,
+	2596, 2609, 2622, 2634, 2647, 2660, 2673, 2685,
+	2698, 2710, 2722, 2735, 2747, 2759, 2771, 2783,
+	2795, 2807, 2819, 2831, 2843, 2855, 2867, 2878,
+	2890, 2901, 2913, 2924, 2936, 2947, 2958, 2970,
+	2981, 2992, 3003, 3014, 3025, 3036, 3047, 3058,
+	3069, 3080, 3091, 3102, 3112, 3123, 3134, 3144,
+	3155, 3165, 3176, 3186, 3197, 3207, 3217, 3228,
+	3238, 3248, 3258, 3268, 3279, 3289, 3299, 3309,
+	3319, 3329, 3339, 3349, 3358, 3368, 3378, 3388,
+	3398, 3407, 3417, 3427, 3436, 3446, 3455, 3465,
+	3474, 3484, 3493, 3503, 3512, 3521, 3531, 3540,
+	3549, 3559, 3568, 3577, 3586, 3595, 3605, 3614,
+	3623, 3632, 3641, 3650, 3659, 3668, 3677, 3686,
+	3694, 3703, 3712, 3721, 3730, 3739, 3747, 3756,
+	3765, 3773, 3782, 3791, 3799, 3808, 3816, 3825,
+	3833, 3842, 3850, 3859, 3867, 3876, 3884, 3893,
+	3901, 3909, 3918, 3926, 3934, 3942, 3951, 3959,
+	3967, 3975, 3984, 3992, 4000, 4008, 4016, 4024,
+	4032, 4040, 4048, 4056, 4064, 4072, 4080, 4088,
+	4095
+};
+
+void
+ia_css_config_rgb_gamma_tables(void)
+{
+	default_r_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_2;
+	default_g_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_2;
+	default_b_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_2;
+	memcpy(default_r_gamma_table.data.vamem_2, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	memcpy(default_g_gamma_table.data.vamem_2, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	memcpy(default_b_gamma_table.data.vamem_2, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h
new file mode 100644
index 000000000..13049fbfa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC2_TABLE_HOST_H
+#define __IA_CSS_GC2_TABLE_HOST_H
+
+#include "ia_css_gc2_types.h"
+
+extern struct ia_css_rgb_gamma_table default_r_gamma_table;
+extern struct ia_css_rgb_gamma_table default_g_gamma_table;
+extern struct ia_css_rgb_gamma_table default_b_gamma_table;
+
+void ia_css_config_rgb_gamma_tables(void);
+
+#endif /* __IA_CSS_GC2_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_types.h
new file mode 100644
index 000000000..ae16409d8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/gc/gc_2/ia_css_gc2_types.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_GC2_TYPES_H
+#define __IA_CSS_GC2_TYPES_H
+
+#include "isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h"  /* FIXME: needed for ia_css_vamem_type */
+
+/* @file
+* CSS-API header file for Gamma Correction parameters.
+*/
+
+/* sRGB Gamma table, used for sRGB Gamma Correction.
+ *
+ *  ISP block: GC2 (sRGB Gamma Correction)
+ * (ISP1: GC1(YUV Gamma Correction) is used.)
+ *  ISP2: GC2 is used.
+ */
+
+/* Number of elements in the sRGB gamma table. */
+#define IA_CSS_VAMEM_1_RGB_GAMMA_TABLE_SIZE_LOG2 8
+#define IA_CSS_VAMEM_1_RGB_GAMMA_TABLE_SIZE      BIT(IA_CSS_VAMEM_1_RGB_GAMMA_TABLE_SIZE_LOG2)
+
+/* Number of elements in the sRGB gamma table. */
+#define IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE_LOG2    8
+#define IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE     ((1U << IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE_LOG2) + 1)
+
+/** IA_CSS_VAMEM_TYPE_1(ISP2300) or
+     IA_CSS_VAMEM_TYPE_2(ISP2400) */
+union ia_css_rgb_gamma_data {
+	u16 vamem_1[IA_CSS_VAMEM_1_RGB_GAMMA_TABLE_SIZE];
+	/** RGB Gamma table on vamem type1. This table is not used,
+		because sRGB Gamma Correction is not implemented for ISP2300. */
+	u16 vamem_2[IA_CSS_VAMEM_2_RGB_GAMMA_TABLE_SIZE];
+	/** RGB Gamma table on vamem type2. u0.12, [0,4095] */
+};
+
+struct ia_css_rgb_gamma_table {
+	enum ia_css_vamem_type vamem_type;
+	union ia_css_rgb_gamma_data data;
+};
+
+#endif /* __IA_CSS_GC2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.c
new file mode 100644
index 000000000..85a02b6ad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.c
@@ -0,0 +1,41 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_hdr.host.h"
+
+void
+ia_css_hdr_init_config(
+    struct sh_css_isp_hdr_params *to,
+    const struct ia_css_hdr_config *from,
+    unsigned int size)
+{
+	int i;
+	(void)size;
+
+	for (i = 0; i < HDR_NUM_INPUT_FRAMES - 1; i++) {
+		to->irradiance.match_shift[i] = from->irradiance.match_shift[i];
+		to->irradiance.match_mul[i]   = from->irradiance.match_mul[i];
+		to->irradiance.thr_low[i]     = from->irradiance.thr_low[i];
+		to->irradiance.thr_high[i]    = from->irradiance.thr_high[i];
+		to->irradiance.thr_coeff[i]   = from->irradiance.thr_coeff[i];
+		to->irradiance.thr_shift[i]   = from->irradiance.thr_shift[i];
+	}
+	to->irradiance.test_irr    = from->irradiance.test_irr;
+	to->irradiance.weight_bpp  = from->irradiance.weight_bpp;
+
+	to->deghost.test_deg    = from->deghost.test_deg;
+	to->exclusion.test_excl = from->exclusion.test_excl;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.h
new file mode 100644
index 000000000..83277b683
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr.host.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_HDR_HOST_H
+#define __IA_CSS_HDR_HOST_H
+
+#include "ia_css_hdr_param.h"
+#include "ia_css_hdr_types.h"
+
+extern const struct ia_css_hdr_config default_hdr_config;
+
+void
+ia_css_hdr_init_config(
+    struct sh_css_isp_hdr_params *to,
+    const struct ia_css_hdr_config *from,
+    unsigned int size);
+
+#endif /* __IA_CSS_HDR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_param.h
new file mode 100644
index 000000000..998c6d801
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_param.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_HDR_PARAMS_H
+#define __IA_CSS_HDR_PARAMS_H
+
+#include "type_support.h"
+
+#define HDR_NUM_INPUT_FRAMES         (3)
+
+/* HDR irradiance map parameters on ISP. */
+struct sh_css_hdr_irradiance_params {
+	s32 test_irr;
+	s32 match_shift[HDR_NUM_INPUT_FRAMES -
+					     1];  /* Histogram matching shift parameter */
+	s32 match_mul[HDR_NUM_INPUT_FRAMES -
+					   1];    /* Histogram matching multiplication parameter */
+	s32 thr_low[HDR_NUM_INPUT_FRAMES -
+					 1];      /* Weight map soft threshold low bound parameter */
+	s32 thr_high[HDR_NUM_INPUT_FRAMES -
+					  1];     /* Weight map soft threshold high bound parameter */
+	s32 thr_coeff[HDR_NUM_INPUT_FRAMES -
+					   1];    /* Soft threshold linear function coefficient */
+	s32 thr_shift[HDR_NUM_INPUT_FRAMES -
+					   1];    /* Soft threshold precision shift parameter */
+	s32 weight_bpp;                             /* Weight map bits per pixel */
+};
+
+/* HDR deghosting parameters on ISP */
+struct sh_css_hdr_deghost_params {
+	s32 test_deg;
+};
+
+/* HDR exclusion parameters on ISP */
+struct sh_css_hdr_exclusion_params {
+	s32 test_excl;
+};
+
+/* HDR ISP parameters */
+struct sh_css_isp_hdr_params {
+	struct sh_css_hdr_irradiance_params irradiance;
+	struct sh_css_hdr_deghost_params    deghost;
+	struct sh_css_hdr_exclusion_params  exclusion;
+};
+
+#endif /* __IA_CSS_HDR_PARAMS_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_types.h
new file mode 100644
index 000000000..175c301ee
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/hdr/ia_css_hdr_types.h
@@ -0,0 +1,70 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_HDR_TYPES_H
+#define __IA_CSS_HDR_TYPES_H
+
+#define IA_CSS_HDR_MAX_NUM_INPUT_FRAMES         (3)
+
+/**
+ * \brief HDR Irradiance Parameters
+ * \detail Currently HDR parameters are used only for testing purposes
+ */
+struct ia_css_hdr_irradiance_params {
+	int test_irr;                                          /** Test parameter */
+	int match_shift[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+							1];  /** Histogram matching shift parameter */
+	int match_mul[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+						      1];    /** Histogram matching multiplication parameter */
+	int thr_low[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+						    1];      /** Weight map soft threshold low bound parameter */
+	int thr_high[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+						     1];     /** Weight map soft threshold high bound parameter */
+	int thr_coeff[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+						      1];    /** Soft threshold linear function coefficien */
+	int thr_shift[IA_CSS_HDR_MAX_NUM_INPUT_FRAMES -
+						      1];    /** Soft threshold precision shift parameter */
+	int weight_bpp;                                        /** Weight map bits per pixel */
+};
+
+/**
+ * \brief HDR Deghosting Parameters
+ * \detail Currently HDR parameters are used only for testing purposes
+ */
+struct ia_css_hdr_deghost_params {
+	int test_deg; /** Test parameter */
+};
+
+/**
+ * \brief HDR Exclusion Parameters
+ * \detail Currently HDR parameters are used only for testing purposes
+ */
+struct ia_css_hdr_exclusion_params {
+	int test_excl; /** Test parameter */
+};
+
+/**
+ * \brief HDR public paramterers.
+ * \details Struct with all parameters for HDR that can be seet from
+ * the CSS API. Currenly, only test parameters are defined.
+ */
+struct ia_css_hdr_config {
+	struct ia_css_hdr_irradiance_params irradiance; /** HDR irradiance parameters */
+	struct ia_css_hdr_deghost_params    deghost;    /** HDR deghosting parameters */
+	struct ia_css_hdr_exclusion_params  exclusion; /** HDR exclusion parameters */
+};
+
+#endif /* __IA_CSS_HDR_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c
new file mode 100644
index 000000000..c7d88552d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c
@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_bayer_io.host.h"
+#include "dma.h"
+#include "math_support.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+#include "ia_css_isp_params.h"
+#include "ia_css_frame.h"
+
+int ia_css_bayer_io_config(const struct ia_css_binary      *binary,
+			   const struct sh_css_binary_args *args)
+{
+	const struct ia_css_frame *in_frame = args->in_frame;
+	const struct ia_css_frame **out_frames = (const struct ia_css_frame **)
+		&args->out_frame;
+	const struct ia_css_frame_info *in_frame_info = (in_frame) ? &in_frame->info :
+		&binary->in_frame_info;
+
+	const unsigned int ddr_bits_per_element = sizeof(short) * 8;
+	const unsigned int ddr_elems_per_word = ceil_div(HIVE_ISP_DDR_WORD_BITS,
+						ddr_bits_per_element);
+	unsigned int size_get = 0, size_put = 0;
+	unsigned int offset = 0;
+	int ret;
+
+	if (binary->info->mem_offsets.offsets.param) {
+		size_get = binary->info->mem_offsets.offsets.param->dmem.get.size;
+		offset = binary->info->mem_offsets.offsets.param->dmem.get.offset;
+	}
+
+	if (size_get) {
+		struct ia_css_common_io_config *to = (struct ia_css_common_io_config *)
+						     &binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+		struct dma_port_config config;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_bayer_io_config() get part enter:\n");
+#endif
+
+		ret = ia_css_dma_configure_from_info(&config, in_frame_info);
+		if (ret)
+			return ret;
+		// The base_address of the input frame will be set in the ISP
+		to->width = in_frame_info->res.width;
+		to->height = in_frame_info->res.height;
+		to->stride = config.stride;
+		to->ddr_elems_per_word = ddr_elems_per_word;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_bayer_io_config() get part leave:\n");
+#endif
+	}
+
+	if (binary->info->mem_offsets.offsets.param) {
+		size_put = binary->info->mem_offsets.offsets.param->dmem.put.size;
+		offset = binary->info->mem_offsets.offsets.param->dmem.put.offset;
+	}
+
+	if (size_put) {
+		struct ia_css_common_io_config *to = (struct ia_css_common_io_config *)
+						     &binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+		struct dma_port_config config;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_bayer_io_config() put part enter:\n");
+#endif
+
+		ret = ia_css_dma_configure_from_info(&config, &out_frames[0]->info);
+		if (ret)
+			return ret;
+		to->base_address = out_frames[0]->data;
+		to->width = out_frames[0]->info.res.width;
+		to->height = out_frames[0]->info.res.height;
+		to->stride = config.stride;
+		to->ddr_elems_per_word = ddr_elems_per_word;
+
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_bayer_io_config() put part leave:\n");
+#endif
+	}
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h
new file mode 100644
index 000000000..9c7e5a1ad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __BAYER_IO_HOST_H
+#define __BAYER_IO_HOST_H
+
+#include "ia_css_bayer_io_param.h"
+#include "ia_css_bayer_io_types.h"
+#include "ia_css_binary.h"
+#include "sh_css_internal.h"
+
+int ia_css_bayer_io_config(const struct ia_css_binary     *binary,
+			   const struct sh_css_binary_args *args);
+
+#endif /*__BAYER_IO_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h
new file mode 100644
index 000000000..283ace838
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BAYER_IO_PARAM
+#define __IA_CSS_BAYER_IO_PARAM
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif /* __IA_CSS_BAYER_IO_PARAM */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h
new file mode 100644
index 000000000..d06d25c9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_BAYER_IO_TYPES_H
+#define __IA_CSS_BAYER_IO_TYPES_H
+
+#include "../common/ia_css_common_io_types.h"
+
+#endif /* __IA_CSS_BAYER_IO_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h
new file mode 100644
index 000000000..5e0e4cd5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __IA_CSS_COMMON_IO_PARAM
+#define __IA_CSS_COMMON_IO_PARAM
+
+#include "../common/ia_css_common_io_types.h"
+
+#endif /* __IA_CSS_COMMON_IO_PARAM */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h
new file mode 100644
index 000000000..0801481c4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __IA_CSS_COMMON_IO_TYPES
+#define __IA_CSS_COMMON_IO_TYPES
+
+#define MAX_IO_DMA_CHANNELS 3
+
+struct ia_css_common_io_config {
+	unsigned int base_address;
+	unsigned int width;
+	unsigned int height;
+	unsigned int stride;
+	unsigned int ddr_elems_per_word;
+	unsigned int dma_channel[MAX_IO_DMA_CHANNELS];
+};
+
+#endif /* __IA_CSS_COMMON_IO_TYPES */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c
new file mode 100644
index 000000000..7d2ef6e26
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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 "ia_css_yuv444_io.host.h"
+#include "dma.h"
+#include "math_support.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+#include "ia_css_isp_params.h"
+#include "ia_css_frame.h"
+
+int ia_css_yuv444_io_config(const struct ia_css_binary      *binary,
+			    const struct sh_css_binary_args *args)
+{
+	const struct ia_css_frame *in_frame = args->in_frame;
+	const struct ia_css_frame **out_frames = (const struct ia_css_frame **)
+		&args->out_frame;
+	const struct ia_css_frame_info *in_frame_info = (in_frame) ? &in_frame->info :
+		&binary->in_frame_info;
+
+	const unsigned int ddr_bits_per_element = sizeof(short) * 8;
+	const unsigned int ddr_elems_per_word = ceil_div(HIVE_ISP_DDR_WORD_BITS,
+						ddr_bits_per_element);
+	unsigned int size_get = 0, size_put = 0;
+	unsigned int offset = 0;
+	int ret;
+
+	if (binary->info->mem_offsets.offsets.param) {
+		size_get = binary->info->mem_offsets.offsets.param->dmem.get.size;
+		offset = binary->info->mem_offsets.offsets.param->dmem.get.offset;
+	}
+
+	if (size_get) {
+		struct ia_css_common_io_config *to = (struct ia_css_common_io_config *)
+						     &binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+		struct dma_port_config config;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_yuv444_io_config() get part enter:\n");
+#endif
+
+		ret = ia_css_dma_configure_from_info(&config, in_frame_info);
+		if (ret)
+			return ret;
+
+		// The base_address of the input frame will be set in the ISP
+		to->width = in_frame_info->res.width;
+		to->height = in_frame_info->res.height;
+		to->stride = config.stride;
+		to->ddr_elems_per_word = ddr_elems_per_word;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_yuv444_io_config() get part leave:\n");
+#endif
+	}
+
+	if (binary->info->mem_offsets.offsets.param) {
+		size_put = binary->info->mem_offsets.offsets.param->dmem.put.size;
+		offset = binary->info->mem_offsets.offsets.param->dmem.put.offset;
+	}
+
+	if (size_put) {
+		struct ia_css_common_io_config *to = (struct ia_css_common_io_config *)
+						     &binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset];
+		struct dma_port_config config;
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_yuv444_io_config() put part enter:\n");
+#endif
+
+		ret = ia_css_dma_configure_from_info(&config, &out_frames[0]->info);
+		if (ret)
+			return ret;
+
+		to->base_address = out_frames[0]->data;
+		to->width = out_frames[0]->info.res.width;
+		to->height = out_frames[0]->info.res.height;
+		to->stride = config.stride;
+		to->ddr_elems_per_word = ddr_elems_per_word;
+
+#ifndef IA_CSS_NO_DEBUG
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "ia_css_yuv444_io_config() put part leave:\n");
+#endif
+	}
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h
new file mode 100644
index 000000000..13e50590f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __YUV444_IO_HOST_H
+#define __YUV444_IO_HOST_H
+
+#include "ia_css_yuv444_io_param.h"
+#include "ia_css_yuv444_io_types.h"
+#include "ia_css_binary.h"
+#include "sh_css_internal.h"
+
+int ia_css_yuv444_io_config(const struct ia_css_binary     *binary,
+			    const struct sh_css_binary_args *args);
+
+#endif /*__YUV44_IO_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
new file mode 100644
index 000000000..9b7537d50
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __IA_CSS_YUV444_IO_PARAM
+#define __IA_CSS_YUV444_IO_PARAM
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
new file mode 100644
index 000000000..137a2a05c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __IA_CSS_YUV444_IO_TYPES
+#define __IA_CSS_YUV444_IO_TYPES
+
+#include "../common/ia_css_common_io_types.h"
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c
new file mode 100644
index 000000000..5f186fb03
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c
@@ -0,0 +1,78 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_iterator.host.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_binary.h"
+#include "ia_css_err.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+
+static const struct ia_css_iterator_configuration default_config = {
+	.input_info = (struct ia_css_frame_info *)NULL,
+};
+
+void
+ia_css_iterator_config(
+    struct sh_css_isp_iterator_isp_config *to,
+    const struct ia_css_iterator_configuration *from,
+    unsigned int size)
+{
+	(void)size;
+	ia_css_frame_info_to_frame_sp_info(&to->input_info,    from->input_info);
+	ia_css_frame_info_to_frame_sp_info(&to->internal_info, from->internal_info);
+	ia_css_frame_info_to_frame_sp_info(&to->output_info,   from->output_info);
+	ia_css_frame_info_to_frame_sp_info(&to->vf_info,       from->vf_info);
+	ia_css_resolution_to_sp_resolution(&to->dvs_envelope,  from->dvs_envelope);
+}
+
+int ia_css_iterator_configure(const struct ia_css_binary *binary,
+			      const struct ia_css_frame_info *in_info)
+{
+	struct ia_css_frame_info my_info = IA_CSS_BINARY_DEFAULT_FRAME_INFO;
+	struct ia_css_iterator_configuration config = default_config;
+
+	config.input_info    = &binary->in_frame_info;
+	config.internal_info = &binary->internal_frame_info;
+	config.output_info   = &binary->out_frame_info[0];
+	config.vf_info       = &binary->vf_frame_info;
+	config.dvs_envelope  = &binary->dvs_envelope;
+
+	/* Use in_info iso binary->in_frame_info.
+	 * They can differ in padded width in case of scaling, e.g. for capture_pp.
+	 * Find out why.
+	*/
+	if (in_info)
+		config.input_info = in_info;
+	if (binary->out_frame_info[0].res.width == 0)
+		config.output_info = &binary->out_frame_info[1];
+	my_info = *config.output_info;
+	config.output_info = &my_info;
+	/* we do this only for preview pipe because in fill_binary_info function
+	 * we assign vf_out res to out res, but for ISP internal processing, we need
+	 * the original out res. for video pipe, it has two output pins --- out and
+	 * vf_out, so it can keep these two resolutions already. */
+	if (binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_PREVIEW &&
+	    binary->vf_downscale_log2 > 0)
+	{
+		/* TODO: Remove this after preview output decimation is fixed
+		 * by configuring out&vf info files properly */
+		my_info.padded_width <<= binary->vf_downscale_log2;
+		my_info.res.width    <<= binary->vf_downscale_log2;
+		my_info.res.height   <<= binary->vf_downscale_log2;
+	}
+
+	return ia_css_configure_iterator(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h
new file mode 100644
index 000000000..1419fa9a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ITERATOR_HOST_H
+#define __IA_CSS_ITERATOR_HOST_H
+
+#include "ia_css_frame_public.h"
+#include "ia_css_binary.h"
+#include "ia_css_err.h"
+#include "ia_css_iterator_param.h"
+
+void
+ia_css_iterator_config(
+    struct sh_css_isp_iterator_isp_config *to,
+    const struct ia_css_iterator_configuration *from,
+    unsigned int size);
+
+int
+ia_css_iterator_configure(
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *in_info);
+
+#endif /* __IA_CSS_ITERATOR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h
new file mode 100644
index 000000000..e062f8d06
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ITERATOR_PARAM_H
+#define __IA_CSS_ITERATOR_PARAM_H
+
+#include "ia_css_types.h" /* ia_css_resolution */
+#include "ia_css_frame_public.h" /* ia_css_frame_info */
+#include "ia_css_frame_comm.h" /* ia_css_frame_sp_info */
+
+struct ia_css_iterator_configuration {
+	const struct ia_css_frame_info *input_info;
+	const struct ia_css_frame_info *internal_info;
+	const struct ia_css_frame_info *output_info;
+	const struct ia_css_frame_info *vf_info;
+	const struct ia_css_resolution *dvs_envelope;
+};
+
+struct sh_css_isp_iterator_isp_config {
+	struct ia_css_frame_sp_info input_info;
+	struct ia_css_frame_sp_info internal_info;
+	struct ia_css_frame_sp_info output_info;
+	struct ia_css_frame_sp_info vf_info;
+	struct ia_css_sp_resolution dvs_envelope;
+};
+
+#endif /* __IA_CSS_ITERATOR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c
new file mode 100644
index 000000000..a70bce117
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c
@@ -0,0 +1,75 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+#include "ia_css_debug.h"
+#endif
+
+#include "ia_css_macc1_5.host.h"
+
+const struct ia_css_macc1_5_config default_macc1_5_config = {
+	1
+};
+
+void
+ia_css_macc1_5_encode(
+    struct sh_css_isp_macc1_5_params *to,
+    const struct ia_css_macc1_5_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->exp = from->exp;
+}
+
+void
+ia_css_macc1_5_vmem_encode(
+    struct sh_css_isp_macc1_5_vmem_params *params,
+    const struct ia_css_macc1_5_table *from,
+    unsigned int size)
+{
+	unsigned int i, j, k, idx;
+	static const unsigned int idx_map[] = {
+		0, 1, 3, 2, 6, 7, 5, 4, 12, 13, 15, 14, 10, 11, 9, 8
+	};
+
+	(void)size;
+
+	for (k = 0; k < 4; k++)
+		for (i = 0; i < IA_CSS_MACC_NUM_AXES; i++) {
+			idx = idx_map[i] + (k * IA_CSS_MACC_NUM_AXES);
+			j   = 4 * i;
+
+			params->data[0][(idx)] = from->data[j];
+			params->data[1][(idx)] = from->data[j + 1];
+			params->data[2][(idx)] = from->data[j + 2];
+			params->data[3][(idx)] = from->data[j + 3];
+		}
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_macc1_5_debug_dtrace(
+    const struct ia_css_macc1_5_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.exp=%d\n",
+			    config->exp);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h
new file mode 100644
index 000000000..6c1189e1d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC1_5_HOST_H
+#define __IA_CSS_MACC1_5_HOST_H
+
+#include "ia_css_macc1_5_param.h"
+#include "ia_css_macc1_5_table.host.h"
+
+extern const struct ia_css_macc1_5_config default_macc1_5_config;
+
+void
+ia_css_macc1_5_encode(
+    struct sh_css_isp_macc1_5_params *to,
+    const struct ia_css_macc1_5_config *from,
+    unsigned int size);
+
+void
+ia_css_macc1_5_vmem_encode(
+    struct sh_css_isp_macc1_5_vmem_params *params,
+    const struct ia_css_macc1_5_table *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_macc1_5_debug_dtrace(
+    const struct ia_css_macc1_5_config *config,
+    unsigned int level);
+#endif
+#endif /* __IA_CSS_MACC1_5_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h
new file mode 100644
index 000000000..66b8fb259
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC1_5_PARAM_H
+#define __IA_CSS_MACC1_5_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h"
+#include "ia_css_macc1_5_types.h"
+
+/* MACC */
+struct sh_css_isp_macc1_5_params {
+	s32 exp;
+};
+
+struct sh_css_isp_macc1_5_vmem_params {
+	VMEM_ARRAY(data, IA_CSS_MACC_NUM_COEFS * ISP_NWAY);
+};
+
+#endif /* __IA_CSS_MACC1_5_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c
new file mode 100644
index 000000000..d205d64e0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c
@@ -0,0 +1,35 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+#include "ia_css_types.h"
+#include "ia_css_macc1_5_table.host.h"
+
+/* Multi-Axes Color Correction table for ISP2.
+ *	64values = 2x2matrix for 16area, [s1.12]
+ *	ineffective: 16 of "identity 2x2 matix" {4096,0,0,4096}
+ */
+const struct ia_css_macc1_5_table default_macc1_5_table = {
+	{
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096
+	}
+};
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h
new file mode 100644
index 000000000..d451efbaa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC1_5_TABLE_HOST_H
+#define __IA_CSS_MACC1_5_TABLE_HOST_H
+
+#include "macc/macc1_5/ia_css_macc1_5_types.h"
+
+extern const struct ia_css_macc1_5_table default_macc1_5_table;
+
+#endif /* __IA_CSS_MACC1_5_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h
new file mode 100644
index 000000000..5492af0df
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h
@@ -0,0 +1,74 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC1_5_TYPES_H
+#define __IA_CSS_MACC1_5_TYPES_H
+
+/* @file
+* CSS-API header file for Multi-Axis Color Conversion algorithm parameters.
+*/
+
+/* Multi-Axis Color Conversion configuration
+ *
+ * ISP2.6.1: MACC1_5 is used.
+ */
+
+/* Number of axes in the MACC table. */
+#define IA_CSS_MACC_NUM_AXES           16
+/* Number of coefficients per MACC axes. */
+#define IA_CSS_MACC_NUM_COEFS          4
+
+/* Multi-Axes Color Correction (MACC) table.
+ *
+ *  ISP block: MACC (MACC by only matrix)
+ *             MACC1_5 (MACC by matrix and exponent(ia_css_macc_config))
+ *  ISP1: MACC is used.
+ *  ISP2: MACC1_5 is used.
+ *
+ *  [MACC]
+ *   OutU = (data00 * InU + data01 * InV) >> 13
+ *   OutV = (data10 * InU + data11 * InV) >> 13
+ *
+ *   default/ineffective:
+ *   OutU = (8192 * InU +    0 * InV) >> 13
+ *   OutV = (   0 * InU + 8192 * InV) >> 13
+ *
+ *  [MACC1_5]
+ *   OutU = (data00 * InU + data01 * InV) >> (13 - exp)
+ *   OutV = (data10 * InU + data11 * InV) >> (13 - exp)
+ *
+ *   default/ineffective: (exp=1)
+ *   OutU = (4096 * InU +    0 * InV) >> (13 - 1)
+ *   OutV = (   0 * InU + 4096 * InV) >> (13 - 1)
+ */
+struct ia_css_macc1_5_table {
+	s16 data[IA_CSS_MACC_NUM_COEFS * IA_CSS_MACC_NUM_AXES];
+	/** 16 of 2x2 matix
+	  MACC1_5: s[macc_config.exp].[13-macc_config.exp], [-8192,8191]
+	    default/ineffective: (s1.12)
+		16 of "identity 2x2 matix" {4096,0,0,4096} */
+};
+
+/* Multi-Axes Color Correction (MACC) configuration.
+ *
+ *  ISP block: MACC1_5 (MACC by matrix and exponent(ia_css_macc_config))
+ *  ISP2: MACC1_5 is used.
+ */
+struct ia_css_macc1_5_config {
+	u8 exp;	/** Common exponent of ia_css_macc_table.
+				u8.0, [0,13], default 1, ineffective 1 */
+};
+
+#endif /* __IA_CSS_MACC1_5_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.c
new file mode 100644
index 000000000..f2d3832a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.c
@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_macc.host.h"
+
+const struct ia_css_macc_config default_macc_config = {
+	1,
+};
+
+void
+ia_css_macc_encode(
+    struct sh_css_isp_macc_params *to,
+    const struct ia_css_macc_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->exp = from->exp;
+}
+
+void
+ia_css_macc_dump(
+    const struct sh_css_isp_macc_params *macc,
+    unsigned int level);
+
+void
+ia_css_macc_debug_dtrace(
+    const struct ia_css_macc_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.exp=%d\n",
+			    config->exp);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.h
new file mode 100644
index 000000000..912db9254
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc.host.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC_HOST_H
+#define __IA_CSS_MACC_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_macc_param.h"
+#include "ia_css_macc_table.host.h"
+
+extern const struct ia_css_macc_config default_macc_config;
+
+void
+ia_css_macc_encode(
+    struct sh_css_isp_macc_params *to,
+    const struct ia_css_macc_config *from,
+    unsigned int size);
+
+void
+ia_css_macc_dump(
+    const struct sh_css_isp_macc_params *macc,
+    unsigned int level);
+
+void
+ia_css_macc_debug_dtrace(
+    const struct ia_css_macc_config *config,
+    unsigned int level);
+
+#endif /* __IA_CSS_MACC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_param.h
new file mode 100644
index 000000000..71665204e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_param.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC_PARAM_H
+#define __IA_CSS_MACC_PARAM_H
+
+#include "type_support.h"
+
+/* MACC */
+struct sh_css_isp_macc_params {
+	s32 exp;
+};
+
+#endif /* __IA_CSS_MACC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c
new file mode 100644
index 000000000..946b074e8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c
@@ -0,0 +1,52 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+#include "ia_css_types.h"
+#include "ia_css_macc_table.host.h"
+
+/* Multi-Axes Color Correction table for ISP1.
+ *	64values = 2x2matrix for 16area, [s2.13]
+ *	ineffective: 16 of "identity 2x2 matix" {8192,0,0,8192}
+ */
+const struct ia_css_macc_table default_macc_table = {
+	{
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192,
+		8192, 0, 0, 8192, 8192, 0, 0, 8192
+	}
+};
+
+/* Multi-Axes Color Correction table for ISP2.
+ *	64values = 2x2matrix for 16area, [s1.12]
+ *	ineffective: 16 of "identity 2x2 matix" {4096,0,0,4096}
+ */
+const struct ia_css_macc_table default_macc2_table = {
+	{
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096,
+		4096, 0, 0, 4096, 4096, 0, 0, 4096
+	}
+};
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h
new file mode 100644
index 000000000..35099cb79
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC_TABLE_HOST_H
+#define __IA_CSS_MACC_TABLE_HOST_H
+
+#include "ia_css_macc_types.h"
+
+extern const struct ia_css_macc_table default_macc_table;
+extern const struct ia_css_macc_table default_macc2_table;
+
+#endif /* __IA_CSS_MACC_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_types.h
new file mode 100644
index 000000000..172a518cb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/macc/macc_1.0/ia_css_macc_types.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_MACC_TYPES_H
+#define __IA_CSS_MACC_TYPES_H
+
+/* @file
+* CSS-API header file for Multi-Axis Color Correction (MACC) parameters.
+*/
+
+/* Number of axes in the MACC table. */
+#define IA_CSS_MACC_NUM_AXES           16
+/* Number of coefficients per MACC axes. */
+#define IA_CSS_MACC_NUM_COEFS          4
+/* The number of planes in the morphing table. */
+
+/* Multi-Axis Color Correction (MACC) table.
+ *
+ *  ISP block: MACC1 (MACC by only matrix)
+ *             MACC2 (MACC by matrix and exponent(ia_css_macc_config))
+ *  ISP1: MACC1 is used.
+ *  ISP2: MACC2 is used.
+ *
+ *  [MACC1]
+ *   OutU = (data00 * InU + data01 * InV) >> 13
+ *   OutV = (data10 * InU + data11 * InV) >> 13
+ *
+ *   default/ineffective:
+ *   OutU = (8192 * InU +    0 * InV) >> 13
+ *   OutV = (   0 * InU + 8192 * InV) >> 13
+ *
+ *  [MACC2]
+ *   OutU = (data00 * InU + data01 * InV) >> (13 - exp)
+ *   OutV = (data10 * InU + data11 * InV) >> (13 - exp)
+ *
+ *   default/ineffective: (exp=1)
+ *   OutU = (4096 * InU +    0 * InV) >> (13 - 1)
+ *   OutV = (   0 * InU + 4096 * InV) >> (13 - 1)
+ */
+
+struct ia_css_macc_table {
+	s16 data[IA_CSS_MACC_NUM_COEFS * IA_CSS_MACC_NUM_AXES];
+	/** 16 of 2x2 matix
+	  MACC1: s2.13, [-65536,65535]
+	    default/ineffective:
+		16 of "identity 2x2 matix" {8192,0,0,8192}
+	  MACC2: s[macc_config.exp].[13-macc_config.exp], [-8192,8191]
+	    default/ineffective: (s1.12)
+		16 of "identity 2x2 matix" {4096,0,0,4096} */
+};
+
+#endif /* __IA_CSS_MACC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.c
new file mode 100644
index 000000000..69283b631
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.c
@@ -0,0 +1,16 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_norm.host.h"
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.h
new file mode 100644
index 000000000..3987abcae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm.host.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_NORM_HOST_H
+#define __IA_CSS_NORM_HOST_H
+
+#include "ia_css_norm_param.h"
+
+#endif /* __IA_CSS_NORM_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm_param.h
new file mode 100644
index 000000000..06c39fdfc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/norm/norm_1.0/ia_css_norm_param.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_NORM_PARAM_H
+#define __IA_CSS_NORM_PARAM_H
+
+#endif /* __IA_CSS_NORM_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.c
new file mode 100644
index 000000000..c4ffff630
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "sh_css_frac.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+#include "isp.h"
+#include "ia_css_ob2.host.h"
+
+const struct ia_css_ob2_config default_ob2_config = {
+	0,
+	0,
+	0,
+	0
+};
+
+void
+ia_css_ob2_encode(
+    struct sh_css_isp_ob2_params *to,
+    const struct ia_css_ob2_config *from,
+    unsigned int size)
+{
+	(void)size;
+
+	/* Blacklevels types are u0_16 */
+	to->blacklevel_gr = uDIGIT_FITTING(from->level_gr, 16, SH_CSS_BAYER_BITS);
+	to->blacklevel_r  = uDIGIT_FITTING(from->level_r,  16, SH_CSS_BAYER_BITS);
+	to->blacklevel_b  = uDIGIT_FITTING(from->level_b,  16, SH_CSS_BAYER_BITS);
+	to->blacklevel_gb = uDIGIT_FITTING(from->level_gb, 16, SH_CSS_BAYER_BITS);
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ob2_dump(
+    const struct sh_css_isp_ob2_params *ob2,
+    unsigned int level)
+{
+	if (!ob2)
+		return;
+
+	ia_css_debug_dtrace(level, "Optical Black 2:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob2_blacklevel_gr", ob2->blacklevel_gr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob2_blacklevel_r", ob2->blacklevel_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob2_blacklevel_b", ob2->blacklevel_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob2_blacklevel_gb", ob2->blacklevel_gb);
+}
+
+void
+ia_css_ob2_debug_dtrace(
+    const struct ia_css_ob2_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.level_gr=%d, config.level_r=%d, config.level_b=%d,  config.level_gb=%d, ",
+			    config->level_gr, config->level_r,
+			    config->level_b, config->level_gb);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.h
new file mode 100644
index 000000000..26c2e4320
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2.host.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB2_HOST_H
+#define __IA_CSS_OB2_HOST_H
+
+#include "ia_css_ob2_types.h"
+#include "ia_css_ob2_param.h"
+
+extern const struct ia_css_ob2_config default_ob2_config;
+
+void
+ia_css_ob2_encode(
+    struct sh_css_isp_ob2_params *to,
+    const struct ia_css_ob2_config *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ob2_dump(
+    const struct sh_css_isp_ob2_params *ob2,
+    unsigned int level);
+
+void
+ia_css_ob2_debug_dtrace(
+    const struct ia_css_ob2_config *config, unsigned int level);
+#endif
+
+#endif /* __IA_CSS_OB2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_param.h
new file mode 100644
index 000000000..c3c9fc3f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_param.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB2_PARAM_H
+#define __IA_CSS_OB2_PARAM_H
+
+#include "type_support.h"
+
+/* OB2 (Optical Black) */
+struct sh_css_isp_ob2_params {
+	s32 blacklevel_gr;
+	s32 blacklevel_r;
+	s32 blacklevel_b;
+	s32 blacklevel_gb;
+};
+
+#endif /* __IA_CSS_OB2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_types.h
new file mode 100644
index 000000000..51e4c35cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob2/ia_css_ob2_types.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB2_TYPES_H
+#define __IA_CSS_OB2_TYPES_H
+
+/* @file
+* CSS-API header file for Optical Black algorithm parameters.
+*/
+
+/* Optical Black configuration
+ *
+ * ISP2.6.1: OB2 is used.
+ */
+
+#include "ia_css_frac.h"
+
+struct ia_css_ob2_config {
+	ia_css_u0_16 level_gr;    /** Black level for GR pixels.
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16  level_r;     /** Black level for R pixels.
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16  level_b;     /** Black level for B pixels.
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16  level_gb;    /** Black level for GB pixels.
+					u0.16, [0,65535],
+					default/ineffective 0 */
+};
+
+#endif /* __IA_CSS_OB2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.c
new file mode 100644
index 000000000..12191cd36
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.c
@@ -0,0 +1,155 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "isp.h"
+
+#include "ia_css_ob.host.h"
+
+const struct ia_css_ob_config default_ob_config = {
+	IA_CSS_OB_MODE_NONE,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0
+};
+
+/* TODO: include ob.isp.h to get isp knowledge and
+   add assert on platform restrictions */
+
+void
+ia_css_ob_configure(
+    struct sh_css_isp_ob_stream_config *config,
+    unsigned int isp_pipe_version,
+    unsigned int raw_bit_depth)
+{
+	config->isp_pipe_version = isp_pipe_version;
+	config->raw_bit_depth    = raw_bit_depth;
+}
+
+void
+ia_css_ob_encode(
+    struct sh_css_isp_ob_params *to,
+    const struct ia_css_ob_config *from,
+    const struct sh_css_isp_ob_stream_config *config,
+    unsigned int size)
+{
+	unsigned int ob_bit_depth
+	    = config->isp_pipe_version == 2 ? SH_CSS_BAYER_BITS : config->raw_bit_depth;
+	unsigned int scale = 16 - ob_bit_depth;
+
+	(void)size;
+	switch (from->mode) {
+	case IA_CSS_OB_MODE_FIXED:
+		to->blacklevel_gr = from->level_gr >> scale;
+		to->blacklevel_r  = from->level_r  >> scale;
+		to->blacklevel_b  = from->level_b  >> scale;
+		to->blacklevel_gb = from->level_gb >> scale;
+		to->area_start_bq = 0;
+		to->area_length_bq = 0;
+		to->area_length_bq_inverse = 0;
+		break;
+	case IA_CSS_OB_MODE_RASTER:
+		to->blacklevel_gr = 0;
+		to->blacklevel_r = 0;
+		to->blacklevel_b = 0;
+		to->blacklevel_gb = 0;
+		to->area_start_bq = from->start_position;
+		to->area_length_bq =
+		    (from->end_position - from->start_position) + 1;
+		to->area_length_bq_inverse = AREA_LENGTH_UNIT / to->area_length_bq;
+		break;
+	default:
+		to->blacklevel_gr = 0;
+		to->blacklevel_r = 0;
+		to->blacklevel_b = 0;
+		to->blacklevel_gb = 0;
+		to->area_start_bq = 0;
+		to->area_length_bq = 0;
+		to->area_length_bq_inverse = 0;
+		break;
+	}
+}
+
+void
+ia_css_ob_vmem_encode(
+    struct sh_css_isp_ob_vmem_params *to,
+    const struct ia_css_ob_config *from,
+    const struct sh_css_isp_ob_stream_config *config,
+    unsigned int size)
+{
+	struct sh_css_isp_ob_params tmp;
+	struct sh_css_isp_ob_params *ob = &tmp;
+
+	(void)size;
+	ia_css_ob_encode(&tmp, from, config, sizeof(tmp));
+
+	{
+		unsigned int i;
+		unsigned int sp_obarea_start_bq  = ob->area_start_bq;
+		unsigned int sp_obarea_length_bq = ob->area_length_bq;
+		unsigned int low = sp_obarea_start_bq;
+		unsigned int high = low + sp_obarea_length_bq;
+		u16 all_ones = ~0;
+
+		for (i = 0; i < OBAREA_MASK_SIZE; i++) {
+			if (i >= low && i < high)
+				to->vmask[i / ISP_VEC_NELEMS][i % ISP_VEC_NELEMS] = all_ones;
+			else
+				to->vmask[i / ISP_VEC_NELEMS][i % ISP_VEC_NELEMS] = 0;
+		}
+	}
+}
+
+void
+ia_css_ob_dump(
+    const struct sh_css_isp_ob_params *ob,
+    unsigned int level)
+{
+	if (!ob) return;
+	ia_css_debug_dtrace(level, "Optical Black:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob_blacklevel_gr", ob->blacklevel_gr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob_blacklevel_r", ob->blacklevel_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob_blacklevel_b", ob->blacklevel_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ob_blacklevel_gb", ob->blacklevel_gb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "obarea_start_bq", ob->area_start_bq);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "obarea_length_bq", ob->area_length_bq);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "obarea_length_bq_inverse",
+			    ob->area_length_bq_inverse);
+}
+
+void
+ia_css_ob_debug_dtrace(
+    const struct ia_css_ob_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.mode=%d, config.level_gr=%d, config.level_r=%d, config.level_b=%d,  config.level_gb=%d, config.start_position=%d, config.end_position=%d\n",
+			    config->mode,
+			    config->level_gr, config->level_r,
+			    config->level_b, config->level_gb,
+			    config->start_position, config->end_position);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.h
new file mode 100644
index 000000000..dfcac0c64
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob.host.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB_HOST_H
+#define __IA_CSS_OB_HOST_H
+
+#include "ia_css_ob_types.h"
+#include "ia_css_ob_param.h"
+
+extern const struct ia_css_ob_config default_ob_config;
+
+void
+ia_css_ob_configure(
+    struct sh_css_isp_ob_stream_config *config,
+    unsigned int isp_pipe_version,
+    unsigned int raw_bit_depth);
+
+void
+ia_css_ob_encode(
+    struct sh_css_isp_ob_params *to,
+    const struct ia_css_ob_config *from,
+    const struct sh_css_isp_ob_stream_config *config,
+    unsigned int size);
+
+void
+ia_css_ob_vmem_encode(
+    struct sh_css_isp_ob_vmem_params *to,
+    const struct ia_css_ob_config *from,
+    const struct sh_css_isp_ob_stream_config *config,
+    unsigned int size);
+
+void
+ia_css_ob_dump(
+    const struct sh_css_isp_ob_params *ob,
+    unsigned int level);
+
+void
+ia_css_ob_debug_dtrace(
+    const struct ia_css_ob_config *config, unsigned int level)
+;
+
+#endif /* __IA_CSS_OB_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_param.h
new file mode 100644
index 000000000..991aa3c40
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_param.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB_PARAM_H
+#define __IA_CSS_OB_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h"
+
+#define OBAREA_MASK_SIZE 64
+#define OBAREA_LENGTHBQ_INVERSE_SHIFT     12
+
+/* AREA_LENGTH_UNIT is dependent on NWAY, requires rewrite */
+#define AREA_LENGTH_UNIT BIT(12)
+
+/* OB (Optical Black) */
+struct sh_css_isp_ob_stream_config {
+	unsigned int isp_pipe_version;
+	unsigned int raw_bit_depth;
+};
+
+struct sh_css_isp_ob_params {
+	s32 blacklevel_gr;
+	s32 blacklevel_r;
+	s32 blacklevel_b;
+	s32 blacklevel_gb;
+	s32 area_start_bq;
+	s32 area_length_bq;
+	s32 area_length_bq_inverse;
+};
+
+struct sh_css_isp_ob_vmem_params {
+	VMEM_ARRAY(vmask, OBAREA_MASK_SIZE);
+};
+
+#endif /* __IA_CSS_OB_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_types.h
new file mode 100644
index 000000000..b74296517
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ob/ob_1.0/ia_css_ob_types.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OB_TYPES_H
+#define __IA_CSS_OB_TYPES_H
+
+/* @file
+* CSS-API header file for Optical Black level parameters.
+*/
+
+#include "ia_css_frac.h"
+
+/* Optical black mode.
+ */
+enum ia_css_ob_mode {
+	IA_CSS_OB_MODE_NONE,	/** OB has no effect. */
+	IA_CSS_OB_MODE_FIXED,	/** Fixed OB */
+	IA_CSS_OB_MODE_RASTER	/** Raster OB */
+};
+
+/* Optical Black level configuration.
+ *
+ *  ISP block: OB1
+ *  ISP1: OB1 is used.
+ *  ISP2: OB1 is used.
+ */
+struct ia_css_ob_config {
+	enum ia_css_ob_mode mode; /** Mode (None / Fixed / Raster).
+					enum, [0,2],
+					default 1, ineffective 0 */
+	ia_css_u0_16 level_gr;    /** Black level for GR pixels
+					(used for Fixed Mode only).
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16 level_r;     /** Black level for R pixels
+					(used for Fixed Mode only).
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16 level_b;     /** Black level for B pixels
+					(used for Fixed Mode only).
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	ia_css_u0_16 level_gb;    /** Black level for GB pixels
+					(used for Fixed Mode only).
+					u0.16, [0,65535],
+					default/ineffective 0 */
+	u16 start_position; /** Start position of OB area
+					(used for Raster Mode only).
+					u16.0, [0,63],
+					default/ineffective 0 */
+	u16 end_position;  /** End position of OB area
+					(used for Raster Mode only).
+					u16.0, [0,63],
+					default/ineffective 0 */
+};
+
+#endif /* __IA_CSS_OB_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.c
new file mode 100644
index 000000000..be9e4ef29
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_frame.h"
+#include "ia_css_debug.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "ia_css_output.host.h"
+#include "isp.h"
+
+#include "assert_support.h"
+
+const struct ia_css_output_config default_output_config = {
+	0,
+	0
+};
+
+static const struct ia_css_output_configuration default_output_configuration = {
+	.info = (struct ia_css_frame_info *)NULL,
+};
+
+static const struct ia_css_output0_configuration default_output0_configuration
+	= {
+	.info = (struct ia_css_frame_info *)NULL,
+};
+
+static const struct ia_css_output1_configuration default_output1_configuration
+	= {
+	.info = (struct ia_css_frame_info *)NULL,
+};
+
+void
+ia_css_output_encode(
+    struct sh_css_isp_output_params *to,
+    const struct ia_css_output_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->enable_hflip = from->enable_hflip;
+	to->enable_vflip = from->enable_vflip;
+}
+
+int ia_css_output_config(struct sh_css_isp_output_isp_config *to,
+			 const struct ia_css_output_configuration  *from,
+			 unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	int ret;
+
+	ret = ia_css_dma_configure_from_info(&to->port_b, from->info);
+	if (ret)
+		return ret;
+
+	to->width_a_over_b = elems_a / to->port_b.elems;
+	to->height = from->info ? from->info->res.height : 0;
+	to->enable = from->info != NULL;
+	ia_css_frame_info_to_frame_sp_info(&to->info, from->info);
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int ia_css_output0_config(struct sh_css_isp_output_isp_config       *to,
+			  const struct ia_css_output0_configuration *from,
+			  unsigned int size)
+{
+	return ia_css_output_config(to, (const struct ia_css_output_configuration *)from, size);
+}
+
+int ia_css_output1_config(struct sh_css_isp_output_isp_config       *to,
+		          const struct ia_css_output1_configuration *from,
+			  unsigned int size)
+{
+	return ia_css_output_config(to, (const struct ia_css_output_configuration *)from, size);
+}
+
+int ia_css_output_configure(const struct ia_css_binary     *binary,
+			    const struct ia_css_frame_info *info)
+{
+	if (info) {
+		struct ia_css_output_configuration config =
+			    default_output_configuration;
+
+		config.info = info;
+
+		return ia_css_configure_output(binary, &config);
+	}
+	return 0;
+}
+
+int ia_css_output0_configure(const struct ia_css_binary    *binary,
+			    const struct ia_css_frame_info *info)
+{
+	if (info) {
+		struct ia_css_output0_configuration config =
+			    default_output0_configuration;
+
+		config.info = info;
+
+		return ia_css_configure_output0(binary, &config);
+	}
+	return 0;
+}
+
+int ia_css_output1_configure(const struct ia_css_binary     *binary,
+			     const struct ia_css_frame_info *info)
+{
+	if (info) {
+		struct ia_css_output1_configuration config =
+			    default_output1_configuration;
+
+		config.info = info;
+
+		return ia_css_configure_output1(binary, &config);
+	}
+	return 0;
+}
+
+void
+ia_css_output_dump(
+    const struct sh_css_isp_output_params *output,
+    unsigned int level)
+{
+	if (!output) return;
+	ia_css_debug_dtrace(level, "Horizontal Output Flip:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "enable", output->enable_hflip);
+	ia_css_debug_dtrace(level, "Vertical Output Flip:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "enable", output->enable_vflip);
+}
+
+void
+ia_css_output_debug_dtrace(
+    const struct ia_css_output_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.enable_hflip=%d",
+			    config->enable_hflip);
+	ia_css_debug_dtrace(level,
+			    "config.enable_vflip=%d",
+			    config->enable_vflip);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.h
new file mode 100644
index 000000000..c8523e95a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output.host.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OUTPUT_HOST_H
+#define __IA_CSS_OUTPUT_HOST_H
+
+#include "ia_css_frame_public.h"
+#include "ia_css_binary.h"
+
+#include "ia_css_output_types.h"
+#include "ia_css_output_param.h"
+
+extern const struct ia_css_output_config default_output_config;
+
+void
+ia_css_output_encode(
+    struct sh_css_isp_output_params *to,
+    const struct ia_css_output_config *from,
+    unsigned int size);
+
+int ia_css_output_config(struct sh_css_isp_output_isp_config      *to,
+			 const struct ia_css_output_configuration *from,
+			 unsigned int size);
+
+int ia_css_output0_config(struct sh_css_isp_output_isp_config       *to,
+			  const struct ia_css_output0_configuration *from,
+			  unsigned int size);
+
+int ia_css_output1_config(struct sh_css_isp_output_isp_config       *to,
+			  const struct ia_css_output1_configuration *from,
+			  unsigned int size);
+
+int ia_css_output_configure(const struct ia_css_binary     *binary,
+			    const struct ia_css_frame_info *from);
+
+int ia_css_output0_configure(const struct ia_css_binary     *binary,
+			     const struct ia_css_frame_info *from);
+
+int ia_css_output1_configure(const struct ia_css_binary     *binary,
+			     const struct ia_css_frame_info *from);
+
+void
+ia_css_output_dump(
+    const struct sh_css_isp_output_params *output,
+    unsigned int level);
+
+void
+ia_css_output_debug_dtrace(
+    const struct ia_css_output_config *config,
+    unsigned int level);
+
+#endif /* __IA_CSS_OUTPUT_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_param.h
new file mode 100644
index 000000000..df125674b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_param.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OUTPUT_PARAM_H
+#define __IA_CSS_OUTPUT_PARAM_H
+
+#include <type_support.h>
+#include "dma.h"
+#include "ia_css_frame_comm.h" /* ia_css_frame_sp_info */
+
+/* output frame */
+struct sh_css_isp_output_isp_config {
+	u32 width_a_over_b;
+	u32 height;
+	u32 enable;
+	struct ia_css_frame_sp_info info;
+	struct dma_port_config port_b;
+};
+
+struct sh_css_isp_output_params {
+	u8 enable_hflip;
+	u8 enable_vflip;
+};
+
+#endif /* __IA_CSS_OUTPUT_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_types.h
new file mode 100644
index 000000000..e5f9c05d2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/output/output_1.0/ia_css_output_types.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_OUTPUT_TYPES_H
+#define __IA_CSS_OUTPUT_TYPES_H
+
+/* @file
+* CSS-API header file for parameters of output frames.
+*/
+
+/* Output frame
+ *
+ *  ISP block: output frame
+ */
+
+//#include "ia_css_frame_public.h"
+struct ia_css_frame_info;
+
+struct ia_css_output_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+struct ia_css_output0_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+struct ia_css_output1_configuration {
+	const struct ia_css_frame_info *info;
+};
+
+struct ia_css_output_config {
+	u8 enable_hflip;  /** enable horizontal output mirroring */
+	u8 enable_vflip;  /** enable vertical output mirroring */
+};
+
+#endif /* __IA_CSS_OUTPUT_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c
new file mode 100644
index 000000000..9fd4435e9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c
@@ -0,0 +1,64 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_frame.h"
+#include "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+
+#include "ia_css_qplane.host.h"
+
+static const struct ia_css_qplane_configuration default_config = {
+	.pipe = (struct sh_css_sp_pipeline *)NULL,
+};
+
+int ia_css_qplane_config(struct sh_css_isp_qplane_isp_config *to,
+			 const struct ia_css_qplane_configuration  *from,
+			 unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	int ret;
+
+	ret = ia_css_dma_configure_from_info(&to->port_b, from->info);
+	if (ret)
+		return ret;
+
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	to->inout_port_config = from->pipe->inout_port_config;
+	to->format = from->info->format;
+
+	return 0;
+}
+
+int ia_css_qplane_configure(const struct sh_css_sp_pipeline *pipe,
+			    const struct ia_css_binary      *binary,
+			    const struct ia_css_frame_info  *info)
+{
+	struct ia_css_qplane_configuration config = default_config;
+
+	config.pipe = pipe;
+	config.info = info;
+
+	return ia_css_configure_qplane(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h
new file mode 100644
index 000000000..b3f8fa30c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_QPLANE_HOST_H
+#define __IA_CSS_QPLANE_HOST_H
+
+#include <ia_css_frame_public.h>
+#include <ia_css_binary.h>
+
+#if 0
+/* Cannot be included, since sh_css_internal.h is too generic
+ * e.g. for FW generation.
+*/
+#include "sh_css_internal.h"	/* sh_css_sp_pipeline */
+#endif
+
+#include "ia_css_qplane_types.h"
+#include "ia_css_qplane_param.h"
+
+int ia_css_qplane_config(struct sh_css_isp_qplane_isp_config      *to,
+			 const struct ia_css_qplane_configuration *from,
+			 unsigned int size);
+
+int ia_css_qplane_configure(const struct sh_css_sp_pipeline *pipe,
+			    const struct ia_css_binary      *binary,
+			    const struct ia_css_frame_info  *from);
+
+#endif /* __IA_CSS_QPLANE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h
new file mode 100644
index 000000000..9ae290450
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_QPLANE_PARAM_H
+#define __IA_CSS_QPLANE_PARAM_H
+
+#include <type_support.h>
+#include "dma.h"
+
+/* qplane channel */
+struct sh_css_isp_qplane_isp_config {
+	u32 width_a_over_b;
+	struct dma_port_config port_b;
+	u32 inout_port_config;
+	u32 input_needs_raw_binning;
+	u32 format; /* enum ia_css_frame_format */
+};
+
+#endif /* __IA_CSS_QPLANE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h
new file mode 100644
index 000000000..549f1a36b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_QPLANE_TYPES_H
+#define __IA_CSS_QPLANE_TYPES_H
+
+#include <ia_css_frame_public.h>
+#include "sh_css_internal.h"
+
+/* qplane frame
+ *
+ *  ISP block: qplane frame
+ */
+
+struct ia_css_qplane_configuration {
+	const struct sh_css_sp_pipeline *pipe;
+	const struct ia_css_frame_info  *info;
+};
+
+#endif /* __IA_CSS_QPLANE_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.c
new file mode 100644
index 000000000..646d6e39c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.c
@@ -0,0 +1,130 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_frame.h"
+#include "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+#include "isp/modes/interface/isp_types.h"
+
+#include "ia_css_raw.host.h"
+
+static const struct ia_css_raw_configuration default_config = {
+	.pipe = (struct sh_css_sp_pipeline *)NULL,
+};
+
+/* MW: These areMIPI / ISYS properties, not camera function properties */
+static enum sh_stream_format
+css2isp_stream_format(enum atomisp_input_format from) {
+	switch (from)
+	{
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+				return sh_stream_format_yuv420_legacy;
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+		return sh_stream_format_yuv420;
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		return sh_stream_format_yuv422;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		return sh_stream_format_rgb;
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		return sh_stream_format_raw;
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+	default:
+		return sh_stream_format_raw;
+	}
+}
+
+int ia_css_raw_config(struct sh_css_isp_raw_isp_config *to,
+		      const struct ia_css_raw_configuration  *from,
+		      unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	const struct ia_css_frame_info *in_info = from->in_info;
+	const struct ia_css_frame_info *internal_info = from->internal_info;
+	int ret;
+
+#if !defined(ISP2401)
+	/* 2401 input system uses input width width */
+	in_info = internal_info;
+#else
+	/*in some cases, in_info is NULL*/
+	if (in_info)
+		(void)internal_info;
+	else
+		in_info = internal_info;
+
+#endif
+	ret = ia_css_dma_configure_from_info(&to->port_b, in_info);
+	if (ret)
+		return ret;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	assert((in_info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED) ||
+	       (elems_a % to->port_b.elems == 0));
+
+	to->width_a_over_b      = elems_a / to->port_b.elems;
+	to->inout_port_config   = from->pipe->inout_port_config;
+	to->format              = in_info->format;
+	to->required_bds_factor = from->pipe->required_bds_factor;
+	to->two_ppc             = from->two_ppc;
+	to->stream_format       = css2isp_stream_format(from->stream_format);
+	to->deinterleaved       = from->deinterleaved;
+#if defined(ISP2401)
+	to->start_column        = in_info->crop_info.start_column;
+	to->start_line          = in_info->crop_info.start_line;
+	to->enable_left_padding = from->enable_left_padding;
+#endif
+
+	return 0;
+}
+
+int ia_css_raw_configure(const struct sh_css_sp_pipeline *pipe,
+			 const struct ia_css_binary      *binary,
+			 const struct ia_css_frame_info  *in_info,
+			 const struct ia_css_frame_info  *internal_info,
+			 bool two_ppc,
+			 bool deinterleaved)
+{
+	u8 enable_left_padding = (uint8_t)((binary->left_padding) ? 1 : 0);
+	struct ia_css_raw_configuration config = default_config;
+
+	config.pipe                = pipe;
+	config.in_info             = in_info;
+	config.internal_info       = internal_info;
+	config.two_ppc             = two_ppc;
+	config.stream_format       = binary->input_format;
+	config.deinterleaved       = deinterleaved;
+	config.enable_left_padding = enable_left_padding;
+
+	return ia_css_configure_raw(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.h
new file mode 100644
index 000000000..23da51aab
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw.host.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_RAW_HOST_H
+#define __IA_CSS_RAW_HOST_H
+
+#include "ia_css_binary.h"
+
+#include "ia_css_raw_types.h"
+#include "ia_css_raw_param.h"
+
+int ia_css_raw_config(struct sh_css_isp_raw_isp_config      *to,
+		      const struct ia_css_raw_configuration *from,
+		      unsigned int size);
+
+int ia_css_raw_configure(const struct sh_css_sp_pipeline *pipe,
+			 const struct ia_css_binary     *binary,
+			 const struct ia_css_frame_info *in_info,
+			 const struct ia_css_frame_info *internal_info,
+			 bool two_ppc,
+			 bool deinterleaved);
+
+#endif /* __IA_CSS_RAW_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_param.h
new file mode 100644
index 000000000..c4b5f719a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_param.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_RAW_PARAM_H
+#define __IA_CSS_RAW_PARAM_H
+
+#include "type_support.h"
+
+#include "dma.h"
+
+/* Raw channel */
+struct sh_css_isp_raw_isp_config {
+	u32 width_a_over_b;
+	struct dma_port_config port_b;
+	u32 inout_port_config;
+	u32 input_needs_raw_binning;
+	u32 format; /* enum ia_css_frame_format */
+	u32 required_bds_factor;
+	u32 two_ppc;
+	u32 stream_format; /* enum sh_stream_format */
+	u32 deinterleaved;
+	u32 start_column; /*left crop offset*/
+	u32 start_line; /*top crop offset*/
+	u8 enable_left_padding; /*need this for multiple binary case*/
+};
+
+#endif /* __IA_CSS_RAW_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_types.h
new file mode 100644
index 000000000..1ccaa3c48
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw/raw_1.0/ia_css_raw_types.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_RAW_TYPES_H
+#define __IA_CSS_RAW_TYPES_H
+
+#include <ia_css_frame_public.h>
+#include "sh_css_internal.h"
+
+/* Raw frame
+ *
+ *  ISP block: Raw frame
+ */
+
+struct ia_css_raw_configuration {
+	const struct sh_css_sp_pipeline *pipe;
+	const struct ia_css_frame_info  *in_info;
+	const struct ia_css_frame_info  *internal_info;
+	bool two_ppc;
+	enum atomisp_input_format stream_format;
+	bool deinterleaved;
+	u8 enable_left_padding;
+};
+
+#endif /* __IA_CSS_RAW_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c
new file mode 100644
index 000000000..9b756dadd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c
@@ -0,0 +1,33 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_internal.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_raa.host.h"
+
+void
+ia_css_raa_encode(
+    struct sh_css_isp_aa_params *to,
+    const struct ia_css_aa_config *from,
+    unsigned int size)
+{
+	(void)size;
+	(void)to;
+	(void)from;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h
new file mode 100644
index 000000000..4c2b3de72
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_RAA_HOST_H
+#define __IA_CSS_RAA_HOST_H
+
+#include "aa/aa_2/ia_css_aa2_types.h"
+#include "aa/aa_2/ia_css_aa2_param.h"
+
+void
+ia_css_raa_encode(
+    struct sh_css_isp_aa_params *to,
+    const struct ia_css_aa_config *from,
+    unsigned int size);
+
+#endif /* __IA_CSS_RAA_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.c
new file mode 100644
index 000000000..08ed916a7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.c
@@ -0,0 +1,86 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <assert_support.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_frame.h>
+#include <ia_css_binary.h>
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+#include "ia_css_ref.host.h"
+
+int ia_css_ref_config(struct sh_css_isp_ref_isp_config *to,
+		      const struct ia_css_ref_configuration  *from,
+		      unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS, i;
+	int ret;
+
+	if (from->ref_frames[0]) {
+		ret = ia_css_dma_configure_from_info(&to->port_b, &from->ref_frames[0]->info);
+		if (ret)
+			return ret;
+		to->width_a_over_b = elems_a / to->port_b.elems;
+		to->dvs_frame_delay = from->dvs_frame_delay;
+	} else {
+		to->width_a_over_b = 1;
+		to->dvs_frame_delay = 0;
+		to->port_b.elems = elems_a;
+	}
+	for (i = 0; i < MAX_NUM_VIDEO_DELAY_FRAMES; i++) {
+		if (from->ref_frames[i]) {
+			to->ref_frame_addr_y[i] = from->ref_frames[i]->data +
+						  from->ref_frames[i]->planes.yuv.y.offset;
+			to->ref_frame_addr_c[i] = from->ref_frames[i]->data +
+						  from->ref_frames[i]->planes.yuv.u.offset;
+		} else {
+			to->ref_frame_addr_y[i] = 0;
+			to->ref_frame_addr_c[i] = 0;
+		}
+	}
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int ia_css_ref_configure(const struct ia_css_binary        *binary,
+			 const struct ia_css_frame * const *ref_frames,
+			 const uint32_t dvs_frame_delay)
+{
+	struct ia_css_ref_configuration config;
+	unsigned int i;
+
+	for (i = 0; i < MAX_NUM_VIDEO_DELAY_FRAMES; i++)
+		config.ref_frames[i] = ref_frames[i];
+
+	config.dvs_frame_delay = dvs_frame_delay;
+
+	return ia_css_configure_ref(binary, &config);
+}
+
+void
+ia_css_init_ref_state(
+    struct sh_css_isp_ref_dmem_state *state,
+    unsigned int size)
+{
+	(void)size;
+	assert(MAX_NUM_VIDEO_DELAY_FRAMES >= 2);
+	state->ref_in_buf_idx = 0;
+	state->ref_out_buf_idx = 1;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.h
new file mode 100644
index 000000000..388cd4c36
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref.host.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_REF_HOST_H
+#define __IA_CSS_REF_HOST_H
+
+#include <ia_css_frame_public.h>
+#include <ia_css_binary.h>
+
+#include "ia_css_ref_types.h"
+#include "ia_css_ref_param.h"
+#include "ia_css_ref_state.h"
+
+int ia_css_ref_config(struct sh_css_isp_ref_isp_config      *to,
+		      const struct ia_css_ref_configuration *from,
+		      unsigned int size);
+
+int ia_css_ref_configure(const struct ia_css_binary        *binary,
+			 const struct ia_css_frame * const *ref_frames,
+			 const uint32_t                    dvs_frame_delay);
+
+void
+ia_css_init_ref_state(
+    struct sh_css_isp_ref_dmem_state *state,
+    unsigned int size);
+#endif /* __IA_CSS_REF_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_param.h
new file mode 100644
index 000000000..c727e27a8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_param.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_REF_PARAM_H
+#define __IA_CSS_REF_PARAM_H
+
+#include <type_support.h>
+#include "sh_css_defs.h"
+#include "dma.h"
+
+/* Reference frame */
+struct ia_css_ref_configuration {
+	const struct ia_css_frame *ref_frames[MAX_NUM_VIDEO_DELAY_FRAMES];
+	u32 dvs_frame_delay;
+};
+
+struct sh_css_isp_ref_isp_config {
+	u32 width_a_over_b;
+	struct dma_port_config port_b;
+	ia_css_ptr ref_frame_addr_y[MAX_NUM_VIDEO_DELAY_FRAMES];
+	ia_css_ptr ref_frame_addr_c[MAX_NUM_VIDEO_DELAY_FRAMES];
+	u32 dvs_frame_delay;
+};
+
+#endif /* __IA_CSS_REF_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_state.h b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_state.h
new file mode 100644
index 000000000..d4f7a6676
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_state.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_REF_STATE_H
+#define __IA_CSS_REF_STATE_H
+
+#include "type_support.h"
+
+/* REF (temporal noise reduction) */
+struct sh_css_isp_ref_dmem_state {
+	s32 ref_in_buf_idx;
+	s32 ref_out_buf_idx;
+};
+
+#endif /* __IA_CSS_REF_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_types.h
new file mode 100644
index 000000000..07d040bcf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ref/ref_1.0/ia_css_ref_types.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_REF_TYPES_H
+#define __IA_CSS_REF_TYPES_H
+
+/* Reference frame
+ *
+ *  ISP block: reference frame
+ */
+
+#include <ia_css_frame_public.h>
+
+#endif /* __IA_CSS_REF_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c
new file mode 100644
index 000000000..bd7b89d94
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c
@@ -0,0 +1,382 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+#include "sh_css_frac.h"
+#include "assert_support.h"
+
+#include "bh/bh_2/ia_css_bh.host.h"
+#include "ia_css_s3a.host.h"
+
+const struct ia_css_3a_config default_3a_config = {
+	25559,
+	32768,
+	7209,
+	65535,
+	0,
+	65535,
+	{-3344, -6104, -19143, 19143, 6104, 3344, 0},
+	{1027, 0, -9219, 16384, -9219, 1027, 0}
+};
+
+static unsigned int s3a_raw_bit_depth;
+
+void
+ia_css_s3a_configure(unsigned int raw_bit_depth)
+{
+	s3a_raw_bit_depth = raw_bit_depth;
+}
+
+static void
+ia_css_ae_encode(
+    struct sh_css_isp_ae_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size)
+{
+	(void)size;
+	/* coefficients to calculate Y */
+	to->y_coef_r =
+	    uDIGIT_FITTING(from->ae_y_coef_r, 16, SH_CSS_AE_YCOEF_SHIFT);
+	to->y_coef_g =
+	    uDIGIT_FITTING(from->ae_y_coef_g, 16, SH_CSS_AE_YCOEF_SHIFT);
+	to->y_coef_b =
+	    uDIGIT_FITTING(from->ae_y_coef_b, 16, SH_CSS_AE_YCOEF_SHIFT);
+}
+
+static void
+ia_css_awb_encode(
+    struct sh_css_isp_awb_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size)
+{
+	(void)size;
+	/* AWB level gate */
+	to->lg_high_raw =
+	    uDIGIT_FITTING(from->awb_lg_high_raw, 16, s3a_raw_bit_depth);
+	to->lg_low =
+	    uDIGIT_FITTING(from->awb_lg_low, 16, SH_CSS_BAYER_BITS);
+	to->lg_high =
+	    uDIGIT_FITTING(from->awb_lg_high, 16, SH_CSS_BAYER_BITS);
+}
+
+static void
+ia_css_af_encode(
+    struct sh_css_isp_af_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size)
+{
+	unsigned int i;
+	(void)size;
+
+	/* af fir coefficients */
+	for (i = 0; i < 7; ++i) {
+		to->fir1[i] =
+		    sDIGIT_FITTING(from->af_fir1_coef[i], 15,
+				   SH_CSS_AF_FIR_SHIFT);
+		to->fir2[i] =
+		    sDIGIT_FITTING(from->af_fir2_coef[i], 15,
+				   SH_CSS_AF_FIR_SHIFT);
+	}
+}
+
+void
+ia_css_s3a_encode(
+    struct sh_css_isp_s3a_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size)
+{
+	(void)size;
+
+	ia_css_ae_encode(&to->ae,   from, sizeof(to->ae));
+	ia_css_awb_encode(&to->awb, from, sizeof(to->awb));
+	ia_css_af_encode(&to->af,   from, sizeof(to->af));
+}
+
+#if 0
+void
+ia_css_process_s3a(
+    unsigned int pipe_id,
+    const struct ia_css_pipeline_stage *stage,
+    struct ia_css_isp_parameters *params)
+{
+	short dmem_offset = stage->binary->info->mem_offsets->dmem.s3a;
+
+	assert(params);
+
+	if (dmem_offset >= 0) {
+		ia_css_s3a_encode((struct sh_css_isp_s3a_params *)
+				  &stage->isp_mem_params[IA_CSS_ISP_DMEM0].address[dmem_offset],
+				  &params->s3a_config);
+		ia_css_bh_encode((struct sh_css_isp_bh_params *)
+				 &stage->isp_mem_params[IA_CSS_ISP_DMEM0].address[dmem_offset],
+				 &params->s3a_config);
+		params->isp_params_changed = true;
+		params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM0] =
+		    true;
+	}
+
+	params->isp_params_changed = true;
+}
+#endif
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ae_dump(
+    const struct sh_css_isp_ae_params *ae,
+    unsigned int level)
+{
+	if (!ae) return;
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ae_y_coef_r", ae->y_coef_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ae_y_coef_g", ae->y_coef_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ae_y_coef_b", ae->y_coef_b);
+}
+
+void
+ia_css_awb_dump(
+    const struct sh_css_isp_awb_params *awb,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "awb_lg_high_raw", awb->lg_high_raw);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "awb_lg_low", awb->lg_low);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "awb_lg_high", awb->lg_high);
+}
+
+void
+ia_css_af_dump(
+    const struct sh_css_isp_af_params *af,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[0]", af->fir1[0]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[1]", af->fir1[1]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[2]", af->fir1[2]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[3]", af->fir1[3]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[4]", af->fir1[4]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[5]", af->fir1[5]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir1[6]", af->fir1[6]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[0]", af->fir2[0]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[1]", af->fir2[1]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[2]", af->fir2[2]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[3]", af->fir2[3]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[4]", af->fir2[4]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[5]", af->fir2[5]);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "af_fir2[6]", af->fir2[6]);
+}
+
+void
+ia_css_s3a_dump(
+    const struct sh_css_isp_s3a_params *s3a,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level, "S3A Support:\n");
+	ia_css_ae_dump(&s3a->ae, level);
+	ia_css_awb_dump(&s3a->awb, level);
+	ia_css_af_dump(&s3a->af, level);
+}
+
+void
+ia_css_s3a_debug_dtrace(
+    const struct ia_css_3a_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.ae_y_coef_r=%d, config.ae_y_coef_g=%d, config.ae_y_coef_b=%d, config.awb_lg_high_raw=%d, config.awb_lg_low=%d, config.awb_lg_high=%d\n",
+			    config->ae_y_coef_r, config->ae_y_coef_g,
+			    config->ae_y_coef_b, config->awb_lg_high_raw,
+			    config->awb_lg_low, config->awb_lg_high);
+}
+#endif
+
+void
+ia_css_s3a_hmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const struct ia_css_bh_table *hmem_buf)
+{
+	struct ia_css_3a_rgby_output	*out_ptr;
+	int			i;
+
+	/* pixel counts(BQ) for 3A area */
+	int count_for_3a;
+	int sum_r, diff;
+
+	assert(host_stats);
+	assert(host_stats->rgby_data);
+	assert(hmem_buf);
+
+	count_for_3a = host_stats->grid.width * host_stats->grid.height
+		       * host_stats->grid.bqs_per_grid_cell
+		       * host_stats->grid.bqs_per_grid_cell;
+
+	out_ptr = host_stats->rgby_data;
+
+	ia_css_bh_hmem_decode(out_ptr, hmem_buf);
+
+	/* Calculate sum of histogram of R,
+	   which should not be less than count_for_3a */
+	sum_r = 0;
+	for (i = 0; i < HMEM_UNIT_SIZE; i++) {
+		sum_r += out_ptr[i].r;
+	}
+	if (sum_r < count_for_3a) {
+		/* histogram is invalid */
+		return;
+	}
+
+	/* Verify for sum of histogram of R/G/B/Y */
+#if 0
+	{
+		int sum_g = 0;
+		int sum_b = 0;
+		int sum_y = 0;
+
+		for (i = 0; i < HMEM_UNIT_SIZE; i++) {
+			sum_g += out_ptr[i].g;
+			sum_b += out_ptr[i].b;
+			sum_y += out_ptr[i].y;
+		}
+		if (sum_g != sum_r || sum_b != sum_r || sum_y != sum_r) {
+			/* histogram is invalid */
+			return;
+		}
+	}
+#endif
+
+	/*
+	 * Limit the histogram area only to 3A area.
+	 * In DSP, the histogram of 0 is incremented for pixels
+	 * which are outside of 3A area. That amount should be subtracted here.
+	 *   hist[0] = hist[0] - ((sum of all hist[]) - (pixel count for 3A area))
+	 */
+	diff = sum_r - count_for_3a;
+	out_ptr[0].r -= diff;
+	out_ptr[0].g -= diff;
+	out_ptr[0].b -= diff;
+	out_ptr[0].y -= diff;
+}
+
+void
+ia_css_s3a_dmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const struct ia_css_3a_output *isp_stats)
+{
+	int isp_width, host_width, height, i;
+	struct ia_css_3a_output *host_ptr;
+
+	assert(host_stats);
+	assert(host_stats->data);
+	assert(isp_stats);
+
+	isp_width  = host_stats->grid.aligned_width;
+	host_width = host_stats->grid.width;
+	height     = host_stats->grid.height;
+	host_ptr   = host_stats->data;
+
+	/* Getting 3A statistics from DMEM does not involve any
+	 * transformation (like the VMEM version), we just copy the data
+	 * using a different output width. */
+	for (i = 0; i < height; i++) {
+		memcpy(host_ptr, isp_stats, host_width * sizeof(*host_ptr));
+		isp_stats += isp_width;
+		host_ptr += host_width;
+	}
+}
+
+/* MW: this is an ISP function */
+static inline int
+merge_hi_lo_14(unsigned short hi, unsigned short lo)
+{
+	int val = (int)((((unsigned int)hi << 14) & 0xfffc000) |
+			((unsigned int)lo & 0x3fff));
+	return val;
+}
+
+void
+ia_css_s3a_vmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const u16 *isp_stats_hi,
+    const uint16_t *isp_stats_lo)
+{
+	int out_width, out_height, chunk, rest, kmax, y, x, k, elm_start, elm, ofs;
+	const u16 *hi, *lo;
+	struct ia_css_3a_output *output;
+
+	assert(host_stats);
+	assert(host_stats->data);
+	assert(isp_stats_hi);
+	assert(isp_stats_lo);
+
+	output = host_stats->data;
+	out_width  = host_stats->grid.width;
+	out_height = host_stats->grid.height;
+	hi = isp_stats_hi;
+	lo = isp_stats_lo;
+
+	chunk = ISP_VEC_NELEMS >> host_stats->grid.deci_factor_log2;
+	chunk = max(chunk, 1);
+
+	for (y = 0; y < out_height; y++) {
+		elm_start = y * ISP_S3ATBL_HI_LO_STRIDE;
+		rest = out_width;
+		x = 0;
+		while (x < out_width) {
+			kmax = (rest > chunk) ? chunk : rest;
+			ofs = y * out_width + x;
+			elm = elm_start + x * sizeof(*output) / sizeof(int32_t);
+			for (k = 0; k < kmax; k++, elm++) {
+				output[ofs + k].ae_y    = merge_hi_lo_14(
+							      hi[elm + chunk * 0], lo[elm + chunk * 0]);
+				output[ofs + k].awb_cnt = merge_hi_lo_14(
+							      hi[elm + chunk * 1], lo[elm + chunk * 1]);
+				output[ofs + k].awb_gr  = merge_hi_lo_14(
+							      hi[elm + chunk * 2], lo[elm + chunk * 2]);
+				output[ofs + k].awb_r   = merge_hi_lo_14(
+							      hi[elm + chunk * 3], lo[elm + chunk * 3]);
+				output[ofs + k].awb_b   = merge_hi_lo_14(
+							      hi[elm + chunk * 4], lo[elm + chunk * 4]);
+				output[ofs + k].awb_gb  = merge_hi_lo_14(
+							      hi[elm + chunk * 5], lo[elm + chunk * 5]);
+				output[ofs + k].af_hpf1 = merge_hi_lo_14(
+							      hi[elm + chunk * 6], lo[elm + chunk * 6]);
+				output[ofs + k].af_hpf2 = merge_hi_lo_14(
+							      hi[elm + chunk * 7], lo[elm + chunk * 7]);
+			}
+			x += chunk;
+			rest -= chunk;
+		}
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h
new file mode 100644
index 000000000..f9926e297
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h
@@ -0,0 +1,78 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_S3A_HOST_H
+#define __IA_CSS_S3A_HOST_H
+
+#include "ia_css_s3a_types.h"
+#include "ia_css_s3a_param.h"
+#include "bh/bh_2/ia_css_bh.host.h"
+
+extern const struct ia_css_3a_config default_3a_config;
+
+void
+ia_css_s3a_configure(
+    unsigned int raw_bit_depth);
+
+void
+ia_css_s3a_encode(
+    struct sh_css_isp_s3a_params *to,
+    const struct ia_css_3a_config *from,
+    unsigned int size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ae_dump(
+    const struct sh_css_isp_ae_params *ae,
+    unsigned int level);
+
+void
+ia_css_awb_dump(
+    const struct sh_css_isp_awb_params *awb,
+    unsigned int level);
+
+void
+ia_css_af_dump(
+    const struct sh_css_isp_af_params *af,
+    unsigned int level);
+
+void
+ia_css_s3a_dump(
+    const struct sh_css_isp_s3a_params *s3a,
+    unsigned int level);
+
+void
+ia_css_s3a_debug_dtrace(
+    const struct ia_css_3a_config *config,
+    unsigned int level);
+#endif
+
+void
+ia_css_s3a_hmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const struct ia_css_bh_table *hmem_buf);
+
+void
+ia_css_s3a_dmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const struct ia_css_3a_output *isp_stats);
+
+void
+ia_css_s3a_vmem_decode(
+    struct ia_css_3a_statistics *host_stats,
+    const u16 *isp_stats_hi,
+    const uint16_t *isp_stats_lo);
+
+#endif /* __IA_CSS_S3A_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h
new file mode 100644
index 000000000..9cb75b220
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_S3A_PARAM_H
+#define __IA_CSS_S3A_PARAM_H
+
+#include "type_support.h"
+
+/* AE (3A Support) */
+struct sh_css_isp_ae_params {
+	/* coefficients to calculate Y */
+	s32 y_coef_r;
+	s32 y_coef_g;
+	s32 y_coef_b;
+};
+
+/* AWB (3A Support) */
+struct sh_css_isp_awb_params {
+	s32 lg_high_raw;
+	s32 lg_low;
+	s32 lg_high;
+};
+
+/* AF (3A Support) */
+struct sh_css_isp_af_params {
+	s32 fir1[7];
+	s32 fir2[7];
+};
+
+/* S3A (3A Support) */
+struct sh_css_isp_s3a_params {
+	/* coefficients to calculate Y */
+	struct sh_css_isp_ae_params ae;
+
+	/* AWB level gate */
+	struct sh_css_isp_awb_params awb;
+
+	/* af fir coefficients */
+	struct sh_css_isp_af_params af;
+};
+
+#endif /* __IA_CSS_S3A_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h
new file mode 100644
index 000000000..f369e9b95
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h
@@ -0,0 +1,222 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_S3A_TYPES_H
+#define __IA_CSS_S3A_TYPES_H
+
+/* @file
+* CSS-API header file for 3A statistics parameters.
+*/
+
+#include <ia_css_frac.h>
+
+#if (defined(SYSTEM_css_skycam_c0_system)) && (!defined(PIPE_GENERATION))
+#include "../../../../components/stats_3a/src/stats_3a_public.h"
+#endif
+
+/* 3A configuration. This configures the 3A statistics collection
+ *  module.
+ */
+
+/* 3A statistics grid
+ *
+ *  ISP block: S3A1 (3A Support for 3A ver.1 (Histogram is not used for AE))
+ *             S3A2 (3A Support for 3A ver.2 (Histogram is used for AE))
+ *  ISP1: S3A1 is used.
+ *  ISP2: S3A2 is used.
+ */
+struct ia_css_3a_grid_info {
+#if defined(SYSTEM_css_skycam_c0_system)
+	u32 ae_enable;					/** ae enabled in binary,
+								   0:disabled, 1:enabled */
+	struct ae_public_config_grid_config
+		ae_grd_info;	/** see description in ae_public.h*/
+
+	u32 awb_enable;					/** awb enabled in binary,
+								   0:disabled, 1:enabled */
+	struct awb_public_config_grid_config
+		awb_grd_info;	/** see description in awb_public.h*/
+
+	u32 af_enable;					/** af enabled in binary,
+								   0:disabled, 1:enabled */
+	struct af_public_grid_config		af_grd_info;	/** see description in af_public.h*/
+
+	u32 awb_fr_enable;					/** awb_fr enabled in binary,
+								   0:disabled, 1:enabled */
+	struct awb_fr_public_grid_config
+		awb_fr_grd_info;/** see description in awb_fr_public.h*/
+
+	u32 elem_bit_depth;    /** TODO:Taken from BYT  - need input from AIQ
+					if needed for SKC
+					Bit depth of element used
+					to calculate 3A statistics.
+					This is 13, which is the normalized
+					bayer bit depth in DSP. */
+
+#else
+	u32 enable;            /** 3A statistics enabled.
+					0:disabled, 1:enabled */
+	u32 use_dmem;          /** DMEM or VMEM determines layout.
+					0:3A statistics are stored to VMEM,
+					1:3A statistics are stored to DMEM */
+	u32 has_histogram;     /** Statistics include histogram.
+					0:no histogram, 1:has histogram */
+	u32 width;		    /** Width of 3A grid table.
+					(= Horizontal number of grid cells
+					in table, which cells have effective
+					statistics.) */
+	u32 height;	    /** Height of 3A grid table.
+					(= Vertical number of grid cells
+					in table, which cells have effective
+					statistics.) */
+	u32 aligned_width;     /** Horizontal stride (for alloc).
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	u32 aligned_height;    /** Vertical stride (for alloc).
+					(= Vertical number of grid cells
+					in table, which means
+					the allocated height.) */
+	u32 bqs_per_grid_cell; /** Grid cell size in BQ(Bayer Quad) unit.
+					(1BQ means {Gr,R,B,Gb}(2x2 pixels).)
+					Valid values are 8,16,32,64. */
+	u32 deci_factor_log2;  /** log2 of bqs_per_grid_cell. */
+	u32 elem_bit_depth;    /** Bit depth of element used
+					to calculate 3A statistics.
+					This is 13, which is the normalized
+					bayer bit depth in DSP. */
+#endif
+};
+
+/* This struct should be split into 3, for AE, AWB and AF.
+ * However, that will require driver/ 3A lib modifications.
+ */
+
+/* 3A configuration. This configures the 3A statistics collection
+ *  module.
+ *
+ *  ae_y_*: Coefficients to calculate luminance from bayer.
+ *  awb_lg_*: Thresholds to check the saturated bayer pixels for AWB.
+ *    Condition of effective pixel for AWB level gate check:
+ *      bayer(sensor) <= awb_lg_high_raw &&
+ *      bayer(when AWB statisitcs is calculated) >= awb_lg_low &&
+ *      bayer(when AWB statisitcs is calculated) <= awb_lg_high
+ *  af_fir*: Coefficients of high pass filter to calculate AF statistics.
+ *
+ *  ISP block: S3A1(ae_y_* for AE/AF, awb_lg_* for AWB)
+ *             S3A2(ae_y_* for AF, awb_lg_* for AWB)
+ *             SDVS1(ae_y_*)
+ *             SDVS2(ae_y_*)
+ *  ISP1: S3A1 and SDVS1 are used.
+ *  ISP2: S3A2 and SDVS2 are used.
+ */
+struct ia_css_3a_config {
+	ia_css_u0_16 ae_y_coef_r;	/** Weight of R for Y.
+						u0.16, [0,65535],
+						default/ineffective 25559 */
+	ia_css_u0_16 ae_y_coef_g;	/** Weight of G for Y.
+						u0.16, [0,65535],
+						default/ineffective 32768 */
+	ia_css_u0_16 ae_y_coef_b;	/** Weight of B for Y.
+						u0.16, [0,65535],
+						default/ineffective 7209 */
+	ia_css_u0_16 awb_lg_high_raw;	/** AWB level gate high for raw.
+						u0.16, [0,65535],
+						default 65472(=1023*64),
+						ineffective 65535 */
+	ia_css_u0_16 awb_lg_low;	/** AWB level gate low.
+						u0.16, [0,65535],
+						default 64(=1*64),
+						ineffective 0 */
+	ia_css_u0_16 awb_lg_high;	/** AWB level gate high.
+						u0.16, [0,65535],
+						default 65535,
+						ineffective 65535 */
+	ia_css_s0_15 af_fir1_coef[7];	/** AF FIR coefficients of fir1.
+						s0.15, [-32768,32767],
+				default/ineffective
+				-6689,-12207,-32768,32767,12207,6689,0 */
+	ia_css_s0_15 af_fir2_coef[7];	/** AF FIR coefficients of fir2.
+						s0.15, [-32768,32767],
+				default/ineffective
+				2053,0,-18437,32767,-18437,2053,0 */
+};
+
+/* 3A statistics. This structure describes the data stored
+ *  in each 3A grid point.
+ *
+ *  ISP block: S3A1 (3A Support for 3A ver.1) (Histogram is not used for AE)
+ *             S3A2 (3A Support for 3A ver.2) (Histogram is used for AE)
+ *             - ae_y is used only for S3A1.
+ *             - awb_* and af_* are used both for S3A1 and S3A2.
+ *  ISP1: S3A1 is used.
+ *  ISP2: S3A2 is used.
+ */
+struct ia_css_3a_output {
+	s32 ae_y;    /** Sum of Y in a statistics window, for AE.
+				(u19.13) */
+	s32 awb_cnt; /** Number of effective pixels
+				in a statistics window.
+				Pixels passed by the AWB level gate check are
+				judged as "effective". (u32) */
+	s32 awb_gr;  /** Sum of Gr in a statistics window, for AWB.
+				All Gr pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	s32 awb_r;   /** Sum of R in a statistics window, for AWB.
+				All R pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	s32 awb_b;   /** Sum of B in a statistics window, for AWB.
+				All B pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	s32 awb_gb;  /** Sum of Gb in a statistics window, for AWB.
+				All Gb pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	s32 af_hpf1; /** Sum of |Y| following high pass filter af_fir1
+				within a statistics window, for AF. (u19.13) */
+	s32 af_hpf2; /** Sum of |Y| following high pass filter af_fir2
+				within a statistics window, for AF. (u19.13) */
+};
+
+/* 3A Statistics. This structure describes the statistics that are generated
+ *  using the provided configuration (ia_css_3a_config).
+ */
+struct ia_css_3a_statistics {
+	struct ia_css_3a_grid_info
+		grid;	/** grid info contains the dimensions of the 3A grid */
+	struct ia_css_3a_output
+		*data;	/** the pointer to 3a_output[grid.width * grid.height]
+						     containing the 3A statistics */
+	struct ia_css_3a_rgby_output *rgby_data;/** the pointer to 3a_rgby_output[256]
+						     containing the histogram */
+};
+
+/* Histogram (Statistics for AE).
+ *
+ *  4 histograms(r,g,b,y),
+ *  256 bins for each histogram, unsigned 24bit value for each bin.
+ *    struct ia_css_3a_rgby_output data[256];
+
+ *  ISP block: HIST2
+ * (ISP1: HIST2 is not used.)
+ *  ISP2: HIST2 is used.
+ */
+struct ia_css_3a_rgby_output {
+	u32 r;	/** Number of R of one bin of the histogram R. (u24) */
+	u32 g;	/** Number of G of one bin of the histogram G. (u24) */
+	u32 b;	/** Number of B of one bin of the histogram B. (u24) */
+	u32 y;	/** Number of Y of one bin of the histogram Y. (u24) */
+};
+
+#endif /* __IA_CSS_S3A_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.c
new file mode 100644
index 000000000..6974b3424
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+
+#include "ia_css_sc.host.h"
+
+void
+ia_css_sc_encode(
+    struct sh_css_isp_sc_params *to,
+    struct ia_css_shading_table **from,
+    unsigned int size)
+{
+	(void)size;
+	to->gain_shift = (*from)->fraction_bits;
+}
+
+void
+ia_css_sc_dump(
+    const struct sh_css_isp_sc_params *sc,
+    unsigned int level)
+{
+	if (!sc) return;
+	ia_css_debug_dtrace(level, "Shading Correction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "sc_gain_shift", sc->gain_shift);
+}
+
+/* ------ deprecated(bz675) : from ------ */
+/* It looks like @parameter{} (in *.pipe) is used to generate the process/get/set functions,
+   for parameters which should be used in the isp kernels.
+   However, the ia_css_shading_settings structure has a parameter which is used only in the css,
+   and does not have a parameter which is used in the isp kernels.
+   Then, I did not use @parameter{} to generate the get/set function
+   for the ia_css_shading_settings structure. (michie) */
+void
+sh_css_get_shading_settings(const struct ia_css_isp_parameters *params,
+			    struct ia_css_shading_settings *settings)
+{
+	if (!settings)
+		return;
+	assert(params);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_get_shading_settings() enter: settings=%p\n", settings);
+
+	*settings = params->shading_settings;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_get_shading_settings() leave: settings.enable_shading_table_conversion=%d\n",
+			    settings->enable_shading_table_conversion);
+}
+
+void
+sh_css_set_shading_settings(struct ia_css_isp_parameters *params,
+			    const struct ia_css_shading_settings *settings)
+{
+	if (!settings)
+		return;
+	assert(params);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_shading_settings() enter: settings.enable_shading_table_conversion=%d\n",
+			    settings->enable_shading_table_conversion);
+
+	params->shading_settings = *settings;
+	params->shading_settings_changed = true;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_set_shading_settings() leave: return_void\n");
+}
+
+/* ------ deprecated(bz675) : to ------ */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.h
new file mode 100644
index 000000000..d103103c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc.host.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SC_HOST_H
+#define __IA_CSS_SC_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_sc_types.h"
+#include "ia_css_sc_param.h"
+
+void
+ia_css_sc_encode(
+    struct sh_css_isp_sc_params *to,
+    struct ia_css_shading_table **from,
+    unsigned int size);
+
+void
+ia_css_sc_dump(
+    const struct sh_css_isp_sc_params *sc,
+    unsigned int level);
+
+/* ------ deprecated(bz675) : from ------ */
+void
+sh_css_get_shading_settings(const struct ia_css_isp_parameters *params,
+			    struct ia_css_shading_settings *settings);
+
+void
+sh_css_set_shading_settings(struct ia_css_isp_parameters *params,
+			    const struct ia_css_shading_settings *settings);
+/* ------ deprecated(bz675) : to ------ */
+
+#endif /* __IA_CSS_SC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_param.h
new file mode 100644
index 000000000..fab11d335
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_param.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SC_PARAM_H
+#define __IA_CSS_SC_PARAM_H
+
+#include "type_support.h"
+
+/* SC (Shading Corrction) */
+struct sh_css_isp_sc_params {
+	s32 gain_shift;
+};
+
+/* Number of horizontal slice times for interpolated gain:
+ *
+ * The start position of the internal frame does not match the start position of the shading table.
+ * To get a vector of shading gains (interpolated horizontally and vertically)
+ * which matches a vector on the internal frame,
+ * vec_slice is used for 2 adjacent vectors of shading gains.
+ * The number of shift times by vec_slice is 8.
+ *     Max grid cell bqs to support the shading table centerting: N = 32
+ *     CEIL_DIV(N-1, ISP_SLICE_NELEMS) = CEIL_DIV(31, 4) = 8
+ */
+#define SH_CSS_SC_INTERPED_GAIN_HOR_SLICE_TIMES   8
+
+struct sh_css_isp_sc_isp_config {
+	u32 interped_gain_hor_slice_bqs[SH_CSS_SC_INTERPED_GAIN_HOR_SLICE_TIMES];
+	u32 internal_frame_origin_y_bqs_on_sctbl;
+};
+
+#endif /* __IA_CSS_SC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_types.h
new file mode 100644
index 000000000..1d70f6b9a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sc/sc_1.0/ia_css_sc_types.h
@@ -0,0 +1,121 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SC_TYPES_H
+#define __IA_CSS_SC_TYPES_H
+
+/* @file
+* CSS-API header file for Lens Shading Correction (SC) parameters.
+*/
+
+/* Number of color planes in the shading table. */
+#define IA_CSS_SC_NUM_COLORS           4
+
+/* The 4 colors that a shading table consists of.
+ *  For each color we store a grid of values.
+ */
+enum ia_css_sc_color {
+	IA_CSS_SC_COLOR_GR, /** Green on a green-red line */
+	IA_CSS_SC_COLOR_R,  /** Red */
+	IA_CSS_SC_COLOR_B,  /** Blue */
+	IA_CSS_SC_COLOR_GB  /** Green on a green-blue line */
+};
+
+/* Lens Shading Correction table.
+ *
+ *  This describes the color shading artefacts
+ *  introduced by lens imperfections. To correct artefacts,
+ *  bayer values should be multiplied by gains in this table.
+ *
+ *------------ deprecated(bz675) : from ---------------------------
+ *  When shading_settings.enable_shading_table_conversion is set as 0,
+ *  this shading table is directly sent to the isp. This table should contain
+ *  the data based on the ia_css_shading_info information filled in the css.
+ *  So, the driver needs to get the ia_css_shading_info information
+ *  from the css, prior to generating the shading table.
+ *
+ *  When shading_settings.enable_shading_table_conversion is set as 1,
+ *  this shading table is converted in the legacy way in the css
+ *  before it is sent to the isp.
+ *  The driver does not need to get the ia_css_shading_info information.
+ *
+ *  NOTE:
+ *  The shading table conversion will be removed from the css in the near future,
+ *  because it does not support the bayer scaling by sensor.
+ *  Also, we had better generate the shading table only in one place(AIC).
+ *  At the moment, to support the old driver which assumes the conversion is done in the css,
+ *  shading_settings.enable_shading_table_conversion is set as 1 by default.
+ *------------ deprecated(bz675) : to ---------------------------
+ *
+ *  ISP block: SC1
+ *  ISP1: SC1 is used.
+ *  ISP2: SC1 is used.
+ */
+struct ia_css_shading_table {
+	u32 enable; /** Set to false for no shading correction.
+			  The data field can be NULL when enable == true */
+	/* ------ deprecated(bz675) : from ------ */
+	u32 sensor_width;  /** Native sensor width in pixels. */
+	u32 sensor_height; /** Native sensor height in lines.
+		When shading_settings.enable_shading_table_conversion is set
+		as 0, sensor_width and sensor_height are NOT used.
+		These are used only in the legacy shading table conversion
+		in the css, when shading_settings.
+		enable_shading_table_conversion is set as 1. */
+	/* ------ deprecated(bz675) : to ------ */
+	u32 width;  /** Number of data points per line per color.
+				u8.0, [0,81] */
+	u32 height; /** Number of lines of data points per color.
+				u8.0, [0,61] */
+	u32 fraction_bits; /** Bits of fractional part in the data
+				points.
+				u8.0, [0,13] */
+	u16 *data[IA_CSS_SC_NUM_COLORS];
+	/** Table data, one array for each color.
+	     Use ia_css_sc_color to index this array.
+	     u[13-fraction_bits].[fraction_bits], [0,8191] */
+};
+
+/* ------ deprecated(bz675) : from ------ */
+/* Shading Correction settings.
+ *
+ *  NOTE:
+ *  This structure should be removed when the shading table conversion is
+ *  removed from the css.
+ */
+struct ia_css_shading_settings {
+	u32 enable_shading_table_conversion; /** Set to 0,
+		if the conversion of the shading table should be disabled
+		in the css. (default 1)
+		  0: The shading table is directly sent to the isp.
+		     The shading table should contain the data based on the
+		     ia_css_shading_info information filled in the css.
+		  1: The shading table is converted in the css, to be fitted
+		     to the shading table definition required in the isp.
+		NOTE:
+		Previously, the shading table was always converted in the css
+		before it was sent to the isp, and this config was not defined.
+		Currently, the driver is supposed to pass the shading table
+		which should be directly sent to the isp.
+		However, some drivers may still pass the shading table which
+		needs the conversion without setting this config as 1.
+		To support such an unexpected case for the time being,
+		enable_shading_table_conversion is set as 1 by default
+		in the css. */
+};
+
+/* ------ deprecated(bz675) : to ------ */
+
+#endif /* __IA_CSS_SC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common.host.h
new file mode 100644
index 000000000..7b661e49b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common.host.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_SDIS_COMMON_HOST_H
+#define _IA_CSS_SDIS_COMMON_HOST_H
+
+#define ISP_MAX_SDIS_HOR_PROJ_NUM_ISP \
+	__ISP_SDIS_HOR_PROJ_NUM_ISP(ISP_MAX_INTERNAL_WIDTH, ISP_MAX_INTERNAL_HEIGHT, \
+		SH_CSS_DIS_DECI_FACTOR_LOG2, ISP_PIPE_VERSION)
+#define ISP_MAX_SDIS_VER_PROJ_NUM_ISP \
+	__ISP_SDIS_VER_PROJ_NUM_ISP(ISP_MAX_INTERNAL_WIDTH, \
+		SH_CSS_DIS_DECI_FACTOR_LOG2)
+
+#define _ISP_SDIS_HOR_COEF_NUM_VECS \
+	__ISP_SDIS_HOR_COEF_NUM_VECS(ISP_INTERNAL_WIDTH)
+#define ISP_MAX_SDIS_HOR_COEF_NUM_VECS \
+	__ISP_SDIS_HOR_COEF_NUM_VECS(ISP_MAX_INTERNAL_WIDTH)
+#define ISP_MAX_SDIS_VER_COEF_NUM_VECS \
+	__ISP_SDIS_VER_COEF_NUM_VECS(ISP_MAX_INTERNAL_HEIGHT)
+
+/* SDIS Coefficients: */
+/* The ISP uses vectors to store the coefficients, so we round
+   the number of coefficients up to vectors. */
+#define __ISP_SDIS_HOR_COEF_NUM_VECS(in_width)  _ISP_VECS(_ISP_BQS(in_width))
+#define __ISP_SDIS_VER_COEF_NUM_VECS(in_height) _ISP_VECS(_ISP_BQS(in_height))
+
+/* SDIS Projections:
+ * SDIS1: Horizontal projections are calculated for each line.
+ * Vertical projections are calculated for each column.
+ * SDIS2: Projections are calculated for each grid cell.
+ * Grid cells that do not fall completely within the image are not
+ * valid. The host needs to use the bigger one for the stride but
+ * should only return the valid ones to the 3A. */
+#define __ISP_SDIS_HOR_PROJ_NUM_ISP(in_width, in_height, deci_factor_log2, \
+	isp_pipe_version) \
+	((isp_pipe_version == 1) ? \
+		CEIL_SHIFT(_ISP_BQS(in_height), deci_factor_log2) : \
+		CEIL_SHIFT(_ISP_BQS(in_width), deci_factor_log2))
+
+#define __ISP_SDIS_VER_PROJ_NUM_ISP(in_width, deci_factor_log2) \
+	CEIL_SHIFT(_ISP_BQS(in_width), deci_factor_log2)
+
+#define SH_CSS_DIS_VER_NUM_COEF_TYPES(b) \
+  (((b)->info->sp.pipeline.isp_pipe_version == 2) ? \
+	IA_CSS_DVS2_NUM_COEF_TYPES : \
+	IA_CSS_DVS_NUM_COEF_TYPES)
+
+#ifndef PIPE_GENERATION
+#if defined(__ISP) || defined(MK_FIRMWARE)
+
+/* Array cannot be 2-dimensional, since driver ddr allocation does not know stride */
+struct sh_css_isp_sdis_hori_proj_tbl {
+	s32 tbl[ISP_DVS_NUM_COEF_TYPES * ISP_MAX_SDIS_HOR_PROJ_NUM_ISP];
+#if DVS2_PROJ_MARGIN > 0
+	s32 margin[DVS2_PROJ_MARGIN];
+#endif
+};
+
+struct sh_css_isp_sdis_vert_proj_tbl {
+	s32 tbl[ISP_DVS_NUM_COEF_TYPES * ISP_MAX_SDIS_VER_PROJ_NUM_ISP];
+#if DVS2_PROJ_MARGIN > 0
+	s32 margin[DVS2_PROJ_MARGIN];
+#endif
+};
+
+struct sh_css_isp_sdis_hori_coef_tbl {
+	VMEM_ARRAY(tbl[ISP_DVS_NUM_COEF_TYPES],
+		   ISP_MAX_SDIS_HOR_COEF_NUM_VECS * ISP_NWAY);
+};
+
+struct sh_css_isp_sdis_vert_coef_tbl {
+	VMEM_ARRAY(tbl[ISP_DVS_NUM_COEF_TYPES],
+		   ISP_MAX_SDIS_VER_COEF_NUM_VECS * ISP_NWAY);
+};
+
+#endif /* defined(__ISP) || defined (MK_FIRMWARE) */
+#endif /* PIPE_GENERATION */
+
+#ifndef PIPE_GENERATION
+struct s_sdis_config {
+	unsigned int horicoef_vectors;
+	unsigned int vertcoef_vectors;
+	unsigned int horiproj_num;
+	unsigned int vertproj_num;
+};
+
+extern struct s_sdis_config sdis_config;
+#endif
+
+#endif /* _IA_CSS_SDIS_COMMON_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common_types.h
new file mode 100644
index 000000000..c2ec30b4a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/common/ia_css_sdis_common_types.h
@@ -0,0 +1,220 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SDIS_COMMON_TYPES_H
+#define __IA_CSS_SDIS_COMMON_TYPES_H
+
+/* @file
+* CSS-API header file for DVS statistics parameters.
+*/
+
+#include <type_support.h>
+
+/* DVS statistics grid dimensions in number of cells.
+ */
+
+struct ia_css_dvs_grid_dim {
+	u32 width;		/** Width of DVS grid table in cells */
+	u32 height;	/** Height of DVS grid table in cells */
+};
+
+/* DVS statistics dimensions in number of cells for
+ * grid, coeffieicient and projection.
+ */
+
+struct ia_css_sdis_info {
+	struct {
+		struct ia_css_dvs_grid_dim dim; /* Dimensions */
+		struct ia_css_dvs_grid_dim pad; /* Padded dimensions */
+	} grid, coef, proj;
+	u32 deci_factor_log2;
+};
+
+/* DVS statistics grid
+ *
+ *  ISP block: SDVS1 (DIS/DVS Support for DIS/DVS ver.1 (2-axes))
+ *             SDVS2 (DVS Support for DVS ver.2 (6-axes))
+ *  ISP1: SDVS1 is used.
+ *  ISP2: SDVS2 is used.
+ */
+struct ia_css_dvs_grid_res {
+	u32 width;		/** Width of DVS grid table.
+					(= Horizontal number of grid cells
+					in table, which cells have effective
+					statistics.)
+					For DVS1, this is equal to
+					 the number of vertical statistics. */
+	u32 aligned_width; /** Stride of each grid line.
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	u32 height;	/** Height of DVS grid table.
+					(= Vertical number of grid cells
+					in table, which cells have effective
+					statistics.)
+					For DVS1, This is equal to
+					the number of horizontal statistics. */
+	u32 aligned_height;/** Stride of each grid column.
+					(= Vertical number of grid cells
+					in table, which means
+					the allocated height.) */
+};
+
+/* TODO: use ia_css_dvs_grid_res in here.
+ * However, that implies driver I/F changes
+ */
+struct ia_css_dvs_grid_info {
+	u32 enable;        /** DVS statistics enabled.
+					0:disabled, 1:enabled */
+	u32 width;		/** Width of DVS grid table.
+					(= Horizontal number of grid cells
+					in table, which cells have effective
+					statistics.)
+					For DVS1, this is equal to
+					 the number of vertical statistics. */
+	u32 aligned_width; /** Stride of each grid line.
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	u32 height;	/** Height of DVS grid table.
+					(= Vertical number of grid cells
+					in table, which cells have effective
+					statistics.)
+					For DVS1, This is equal to
+					the number of horizontal statistics. */
+	u32 aligned_height;/** Stride of each grid column.
+					(= Vertical number of grid cells
+					in table, which means
+					the allocated height.) */
+	u32 bqs_per_grid_cell; /** Grid cell size in BQ(Bayer Quad) unit.
+					(1BQ means {Gr,R,B,Gb}(2x2 pixels).)
+					For DVS1, valid value is 64.
+					For DVS2, valid value is only 64,
+					currently. */
+	u32 num_hor_coefs;	/** Number of horizontal coefficients. */
+	u32 num_ver_coefs;	/** Number of vertical coefficients. */
+};
+
+/* Number of DVS statistics levels
+ */
+#define IA_CSS_DVS_STAT_NUM_OF_LEVELS	3
+
+/* DVS statistics generated by accelerator global configuration
+ */
+struct dvs_stat_public_dvs_global_cfg {
+	unsigned char kappa;
+	/** DVS statistics global configuration - kappa */
+	unsigned char match_shift;
+	/** DVS statistics global configuration - match_shift */
+	unsigned char ybin_mode;
+	/** DVS statistics global configuration - y binning mode */
+};
+
+/* DVS statistics generated by accelerator level grid
+ *  configuration
+ */
+struct dvs_stat_public_dvs_level_grid_cfg {
+	unsigned char grid_width;
+	/** DVS statistics grid width */
+	unsigned char grid_height;
+	/** DVS statistics grid height */
+	unsigned char block_width;
+	/** DVS statistics block width */
+	unsigned char block_height;
+	/** DVS statistics block  height */
+};
+
+/* DVS statistics generated by accelerator level grid start
+ *  configuration
+ */
+struct dvs_stat_public_dvs_level_grid_start {
+	unsigned short x_start;
+	/** DVS statistics level x start */
+	unsigned short y_start;
+	/** DVS statistics level y start */
+	unsigned char enable;
+	/** DVS statistics level enable */
+};
+
+/* DVS statistics generated by accelerator level grid end
+ *  configuration
+ */
+struct dvs_stat_public_dvs_level_grid_end {
+	unsigned short x_end;
+	/** DVS statistics level x end */
+	unsigned short y_end;
+	/** DVS statistics level y end */
+};
+
+/* DVS statistics generated by accelerator Feature Extraction
+ *  Region Of Interest (FE-ROI) configuration
+ */
+struct dvs_stat_public_dvs_level_fe_roi_cfg {
+	unsigned char x_start;
+	/** DVS statistics fe-roi level x start */
+	unsigned char y_start;
+	/** DVS statistics fe-roi level y start */
+	unsigned char x_end;
+	/** DVS statistics fe-roi level x end */
+	unsigned char y_end;
+	/** DVS statistics fe-roi level y end */
+};
+
+/* DVS statistics generated by accelerator public configuration
+ */
+struct dvs_stat_public_dvs_grd_cfg {
+	struct dvs_stat_public_dvs_level_grid_cfg    grd_cfg;
+	/** DVS statistics level grid configuration */
+	struct dvs_stat_public_dvs_level_grid_start  grd_start;
+	/** DVS statistics level grid start configuration */
+	struct dvs_stat_public_dvs_level_grid_end    grd_end;
+	/** DVS statistics level grid end configuration */
+};
+
+/* DVS statistics grid generated by accelerator
+ */
+struct ia_css_dvs_stat_grid_info {
+	struct dvs_stat_public_dvs_global_cfg       dvs_gbl_cfg;
+	/** DVS statistics global configuration (kappa, match, binning) */
+	struct dvs_stat_public_dvs_grd_cfg       grd_cfg[IA_CSS_DVS_STAT_NUM_OF_LEVELS];
+	/** DVS statistics grid configuration (blocks and grids) */
+	struct dvs_stat_public_dvs_level_fe_roi_cfg
+		fe_roi_cfg[IA_CSS_DVS_STAT_NUM_OF_LEVELS];
+	/** DVS statistics FE ROI (region of interest) configuration */
+};
+
+/* DVS statistics generated by accelerator default grid info
+ */
+#define DEFAULT_DVS_GRID_INFO { \
+	.dvs_stat_grid_info = { \
+		.fe_roi_cfg = { \
+			[1] = { \
+			    .x_start = 4 \
+			} \
+		} \
+	} \
+}
+
+/* Union that holds all types of DVS statistics grid info in
+ *  CSS format
+ * */
+union ia_css_dvs_grid_u {
+	struct ia_css_dvs_stat_grid_info dvs_stat_grid_info;
+	/** DVS statistics produced by accelerator grid info */
+	struct ia_css_dvs_grid_info dvs_grid_info;
+	/** DVS (DVS1/DVS2) grid info */
+};
+
+#endif /* __IA_CSS_SDIS_COMMON_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
new file mode 100644
index 000000000..bf0a768f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
@@ -0,0 +1,438 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "assert_support.h"
+#include "ia_css_debug.h"
+#include "ia_css_sdis_types.h"
+#include "sdis/common/ia_css_sdis_common.host.h"
+#include "ia_css_sdis.host.h"
+
+const struct ia_css_dvs_coefficients default_sdis_config = {
+	.grid = { 0, 0, 0, 0, 0, 0, 0, 0 },
+	.hor_coefs = NULL,
+	.ver_coefs = NULL
+};
+
+static void
+fill_row(short *private, const short *public, unsigned int width,
+	 unsigned int padding)
+{
+	assert((int)width >= 0);
+	assert((int)padding >= 0);
+	memcpy(private, public, width * sizeof(short));
+	memset(&private[width], 0, padding * sizeof(short));
+}
+
+void ia_css_sdis_horicoef_vmem_encode(
+    struct sh_css_isp_sdis_hori_coef_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size)
+{
+	unsigned int aligned_width = from->grid.aligned_width *
+				     from->grid.bqs_per_grid_cell;
+	unsigned int width         = from->grid.num_hor_coefs;
+	int      padding       = aligned_width - width;
+	unsigned int stride        = size / IA_CSS_DVS_NUM_COEF_TYPES / sizeof(short);
+	unsigned int total_bytes   = aligned_width * IA_CSS_DVS_NUM_COEF_TYPES * sizeof(
+					 short);
+	short   *public        = from->hor_coefs;
+	short   *private       = (short *)to;
+	unsigned int type;
+
+	/* Copy the table, add padding */
+	assert(padding >= 0);
+	assert(total_bytes <= size);
+	assert(size % (IA_CSS_DVS_NUM_COEF_TYPES * ISP_VEC_NELEMS * sizeof(
+			   short)) == 0);
+
+	for (type = 0; type < IA_CSS_DVS_NUM_COEF_TYPES; type++) {
+		fill_row(&private[type * stride], &public[type * width], width, padding);
+	}
+}
+
+void ia_css_sdis_vertcoef_vmem_encode(
+    struct sh_css_isp_sdis_vert_coef_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size)
+{
+	unsigned int aligned_height = from->grid.aligned_height *
+				      from->grid.bqs_per_grid_cell;
+	unsigned int height         = from->grid.num_ver_coefs;
+	int      padding        = aligned_height - height;
+	unsigned int stride         = size / IA_CSS_DVS_NUM_COEF_TYPES / sizeof(short);
+	unsigned int total_bytes    = aligned_height * IA_CSS_DVS_NUM_COEF_TYPES *
+				      sizeof(short);
+	short   *public         = from->ver_coefs;
+	short   *private        = (short *)to;
+	unsigned int type;
+
+	/* Copy the table, add padding */
+	assert(padding >= 0);
+	assert(total_bytes <= size);
+	assert(size % (IA_CSS_DVS_NUM_COEF_TYPES * ISP_VEC_NELEMS * sizeof(
+			   short)) == 0);
+
+	for (type = 0; type < IA_CSS_DVS_NUM_COEF_TYPES; type++) {
+		fill_row(&private[type * stride], &public[type * height], height, padding);
+	}
+}
+
+void ia_css_sdis_horiproj_encode(
+    struct sh_css_isp_sdis_hori_proj_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size)
+{
+	(void)to;
+	(void)from;
+	(void)size;
+}
+
+void ia_css_sdis_vertproj_encode(
+    struct sh_css_isp_sdis_vert_proj_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size)
+{
+	(void)to;
+	(void)from;
+	(void)size;
+}
+
+void ia_css_get_isp_dis_coefficients(
+    struct ia_css_stream *stream,
+    short *horizontal_coefficients,
+    short *vertical_coefficients)
+{
+	struct ia_css_isp_parameters *params;
+	unsigned int hor_num_isp, ver_num_isp;
+	unsigned int hor_num_3a,  ver_num_3a;
+	int i;
+	struct ia_css_binary *dvs_binary;
+
+	IA_CSS_ENTER("void");
+
+	assert(horizontal_coefficients);
+	assert(vertical_coefficients);
+
+	params = stream->isp_params_configs;
+
+	/* Only video pipe supports DVS */
+	dvs_binary = ia_css_stream_get_dvs_binary(stream);
+	if (!dvs_binary)
+		return;
+
+	hor_num_isp = dvs_binary->dis.coef.pad.width;
+	ver_num_isp = dvs_binary->dis.coef.pad.height;
+	hor_num_3a  = dvs_binary->dis.coef.dim.width;
+	ver_num_3a  = dvs_binary->dis.coef.dim.height;
+
+	for (i = 0; i < IA_CSS_DVS_NUM_COEF_TYPES; i++) {
+		fill_row(&horizontal_coefficients[i * hor_num_isp],
+			 &params->dvs_coefs.hor_coefs[i * hor_num_3a], hor_num_3a,
+			 hor_num_isp - hor_num_3a);
+	}
+	for (i = 0; i < SH_CSS_DIS_VER_NUM_COEF_TYPES(dvs_binary); i++) {
+		fill_row(&vertical_coefficients[i * ver_num_isp],
+			 &params->dvs_coefs.ver_coefs[i * ver_num_3a], ver_num_3a,
+			 ver_num_isp - ver_num_3a);
+	}
+
+	IA_CSS_LEAVE("void");
+}
+
+size_t
+ia_css_sdis_hor_coef_tbl_bytes(
+    const struct ia_css_binary *binary)
+{
+	if (binary->info->sp.pipeline.isp_pipe_version == 1)
+		return sizeof(short) * IA_CSS_DVS_NUM_COEF_TYPES  * binary->dis.coef.pad.width;
+	else
+		return sizeof(short) * IA_CSS_DVS2_NUM_COEF_TYPES * binary->dis.coef.pad.width;
+}
+
+size_t
+ia_css_sdis_ver_coef_tbl_bytes(
+    const struct ia_css_binary *binary)
+{
+	return sizeof(short) * SH_CSS_DIS_VER_NUM_COEF_TYPES(binary) *
+	       binary->dis.coef.pad.height;
+}
+
+void
+ia_css_sdis_init_info(
+    struct ia_css_sdis_info *dis,
+    unsigned int sc_3a_dis_width,
+    unsigned int sc_3a_dis_padded_width,
+    unsigned int sc_3a_dis_height,
+    unsigned int isp_pipe_version,
+    unsigned int enabled)
+{
+	if (!enabled) {
+		*dis = (struct ia_css_sdis_info) { };
+		return;
+	}
+
+	dis->deci_factor_log2 = SH_CSS_DIS_DECI_FACTOR_LOG2;
+
+	dis->grid.dim.width  =
+	    _ISP_BQS(sc_3a_dis_width) >> SH_CSS_DIS_DECI_FACTOR_LOG2;
+	dis->grid.dim.height =
+	    _ISP_BQS(sc_3a_dis_height) >> SH_CSS_DIS_DECI_FACTOR_LOG2;
+	dis->grid.pad.width  =
+	    CEIL_SHIFT(_ISP_BQS(sc_3a_dis_padded_width), SH_CSS_DIS_DECI_FACTOR_LOG2);
+	dis->grid.pad.height =
+	    CEIL_SHIFT(_ISP_BQS(sc_3a_dis_height), SH_CSS_DIS_DECI_FACTOR_LOG2);
+
+	dis->coef.dim.width  =
+	    (_ISP_BQS(sc_3a_dis_width)  >> SH_CSS_DIS_DECI_FACTOR_LOG2) <<
+	    SH_CSS_DIS_DECI_FACTOR_LOG2;
+	dis->coef.dim.height =
+	    (_ISP_BQS(sc_3a_dis_height) >> SH_CSS_DIS_DECI_FACTOR_LOG2) <<
+	    SH_CSS_DIS_DECI_FACTOR_LOG2;
+	dis->coef.pad.width  =
+	    __ISP_SDIS_HOR_COEF_NUM_VECS(sc_3a_dis_padded_width) * ISP_VEC_NELEMS;
+	dis->coef.pad.height =
+	    __ISP_SDIS_VER_COEF_NUM_VECS(sc_3a_dis_height) * ISP_VEC_NELEMS;
+	if (isp_pipe_version == 1) {
+		dis->proj.dim.width  =
+		    _ISP_BQS(sc_3a_dis_height) >> SH_CSS_DIS_DECI_FACTOR_LOG2;
+		dis->proj.dim.height =
+		    _ISP_BQS(sc_3a_dis_width)  >> SH_CSS_DIS_DECI_FACTOR_LOG2;
+	} else {
+		dis->proj.dim.width  =
+		    (_ISP_BQS(sc_3a_dis_width)  >> SH_CSS_DIS_DECI_FACTOR_LOG2) *
+		    (_ISP_BQS(sc_3a_dis_height) >> SH_CSS_DIS_DECI_FACTOR_LOG2);
+		dis->proj.dim.height =
+		    (_ISP_BQS(sc_3a_dis_width)  >> SH_CSS_DIS_DECI_FACTOR_LOG2) *
+		    (_ISP_BQS(sc_3a_dis_height) >> SH_CSS_DIS_DECI_FACTOR_LOG2);
+	}
+	dis->proj.pad.width  =
+	    __ISP_SDIS_HOR_PROJ_NUM_ISP(sc_3a_dis_padded_width,
+					sc_3a_dis_height,
+					SH_CSS_DIS_DECI_FACTOR_LOG2,
+					isp_pipe_version);
+	dis->proj.pad.height =
+	    __ISP_SDIS_VER_PROJ_NUM_ISP(sc_3a_dis_padded_width,
+					SH_CSS_DIS_DECI_FACTOR_LOG2);
+}
+
+void ia_css_sdis_clear_coefficients(
+    struct ia_css_dvs_coefficients *dvs_coefs)
+{
+	dvs_coefs->hor_coefs = NULL;
+	dvs_coefs->ver_coefs = NULL;
+}
+
+int
+ia_css_get_dvs_statistics(
+    struct ia_css_dvs_statistics	       *host_stats,
+    const struct ia_css_isp_dvs_statistics *isp_stats) {
+	struct ia_css_isp_dvs_statistics_map *map;
+	int ret = 0;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats);
+	assert(isp_stats);
+
+	map = ia_css_isp_dvs_statistics_map_allocate(isp_stats, NULL);
+	if (map)
+	{
+		hmm_load(isp_stats->data_ptr, map->data_ptr, isp_stats->size);
+		ia_css_translate_dvs_statistics(host_stats, map);
+		ia_css_isp_dvs_statistics_map_free(map);
+	} else
+	{
+		IA_CSS_ERROR("out of memory");
+		ret = -ENOMEM;
+	}
+
+	IA_CSS_LEAVE_ERR(ret);
+	return ret;
+}
+
+void
+ia_css_translate_dvs_statistics(
+    struct ia_css_dvs_statistics               *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats)
+{
+	unsigned int hor_num_isp, ver_num_isp, hor_num_dvs, ver_num_dvs, i;
+	s32 *hor_ptr_dvs, *ver_ptr_dvs, *hor_ptr_isp, *ver_ptr_isp;
+
+	assert(host_stats);
+	assert(host_stats->hor_proj);
+	assert(host_stats->ver_proj);
+	assert(isp_stats);
+	assert(isp_stats->hor_proj);
+	assert(isp_stats->ver_proj);
+
+	IA_CSS_ENTER("hproj=%p, vproj=%p, haddr=%p, vaddr=%p",
+		     host_stats->hor_proj, host_stats->ver_proj,
+		     isp_stats->hor_proj, isp_stats->ver_proj);
+
+	hor_num_isp = host_stats->grid.aligned_height;
+	ver_num_isp = host_stats->grid.aligned_width;
+	hor_ptr_isp = isp_stats->hor_proj;
+	ver_ptr_isp = isp_stats->ver_proj;
+	hor_num_dvs = host_stats->grid.height;
+	ver_num_dvs = host_stats->grid.width;
+	hor_ptr_dvs = host_stats->hor_proj;
+	ver_ptr_dvs = host_stats->ver_proj;
+
+	for (i = 0; i < IA_CSS_DVS_NUM_COEF_TYPES; i++) {
+		memcpy(hor_ptr_dvs, hor_ptr_isp, hor_num_dvs * sizeof(int32_t));
+		hor_ptr_isp += hor_num_isp;
+		hor_ptr_dvs += hor_num_dvs;
+
+		memcpy(ver_ptr_dvs, ver_ptr_isp, ver_num_dvs * sizeof(int32_t));
+		ver_ptr_isp += ver_num_isp;
+		ver_ptr_dvs += ver_num_dvs;
+	}
+
+	IA_CSS_LEAVE("void");
+}
+
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs_statistics_allocate(
+    const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_isp_dvs_statistics *me;
+	int hor_size, ver_size;
+
+	assert(grid);
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	if (!grid->enable)
+		return NULL;
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	hor_size = CEIL_MUL(sizeof(int) * IA_CSS_DVS_NUM_COEF_TYPES *
+			    grid->aligned_height,
+			    HIVE_ISP_DDR_WORD_BYTES);
+	ver_size = CEIL_MUL(sizeof(int) * IA_CSS_DVS_NUM_COEF_TYPES *
+			    grid->aligned_width,
+			    HIVE_ISP_DDR_WORD_BYTES);
+
+	me->size = hor_size + ver_size;
+	me->data_ptr = hmm_alloc(me->size);
+	if (me->data_ptr == mmgr_NULL)
+		goto err;
+	me->hor_size = hor_size;
+	me->hor_proj = me->data_ptr;
+	me->ver_size = ver_size;
+	me->ver_proj = me->data_ptr + hor_size;
+
+	IA_CSS_LEAVE("return=%p", me);
+
+	return me;
+err:
+	ia_css_isp_dvs_statistics_free(me);
+
+	IA_CSS_LEAVE("return=%p", NULL);
+
+	return NULL;
+}
+
+struct ia_css_isp_dvs_statistics_map *
+ia_css_isp_dvs_statistics_map_allocate(
+    const struct ia_css_isp_dvs_statistics *isp_stats,
+    void *data_ptr)
+{
+	struct ia_css_isp_dvs_statistics_map *me;
+	/* Windows compiler does not like adding sizes to a void *
+	 * so we use a local char * instead. */
+	char *base_ptr;
+
+	me = kvmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me) {
+		IA_CSS_LOG("cannot allocate memory");
+		goto err;
+	}
+
+	me->data_ptr = data_ptr;
+	me->data_allocated = !data_ptr;
+
+	if (!me->data_ptr) {
+		me->data_ptr = kvmalloc(isp_stats->size, GFP_KERNEL);
+		if (!me->data_ptr) {
+			IA_CSS_LOG("cannot allocate memory");
+			goto err;
+		}
+	}
+	base_ptr = me->data_ptr;
+
+	me->size = isp_stats->size;
+	/* GCC complains when we assign a char * to a void *, so these
+	 * casts are necessary unfortunately. */
+	me->hor_proj = (void *)base_ptr;
+	me->ver_proj = (void *)(base_ptr + isp_stats->hor_size);
+
+	return me;
+err:
+	kvfree(me);
+	return NULL;
+}
+
+void
+ia_css_isp_dvs_statistics_map_free(struct ia_css_isp_dvs_statistics_map *me)
+{
+	if (me) {
+		if (me->data_allocated)
+			kvfree(me->data_ptr);
+		kvfree(me);
+	}
+}
+
+void
+ia_css_isp_dvs_statistics_free(struct ia_css_isp_dvs_statistics *me)
+{
+	if (me) {
+		hmm_free(me->data_ptr);
+		kvfree(me);
+	}
+}
+
+void ia_css_sdis_horicoef_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_vertcoef_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_horiproj_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_vertproj_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h
new file mode 100644
index 000000000..0d0ed96e0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SDIS_HOST_H
+#define __IA_CSS_SDIS_HOST_H
+
+#include "ia_css_sdis_types.h"
+#include "ia_css_binary.h"
+#include "ia_css_stream.h"
+#include "sh_css_params.h"
+
+extern const struct ia_css_dvs_coefficients default_sdis_config;
+
+/* Opaque here, since size is binary dependent. */
+struct sh_css_isp_sdis_hori_coef_tbl;
+struct sh_css_isp_sdis_vert_coef_tbl;
+struct sh_css_isp_sdis_hori_proj_tbl;
+struct sh_css_isp_sdis_vert_proj_tbl;
+
+void ia_css_sdis_horicoef_vmem_encode(
+    struct sh_css_isp_sdis_hori_coef_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis_vertcoef_vmem_encode(
+    struct sh_css_isp_sdis_vert_coef_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis_horiproj_encode(
+    struct sh_css_isp_sdis_hori_proj_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis_vertproj_encode(
+    struct sh_css_isp_sdis_vert_proj_tbl *to,
+    const struct ia_css_dvs_coefficients *from,
+    unsigned int size);
+
+void ia_css_get_isp_dis_coefficients(
+    struct ia_css_stream *stream,
+    short *horizontal_coefficients,
+    short *vertical_coefficients);
+
+int
+ia_css_get_dvs_statistics(
+    struct ia_css_dvs_statistics	       *host_stats,
+    const struct ia_css_isp_dvs_statistics *isp_stats);
+
+void
+ia_css_translate_dvs_statistics(
+    struct ia_css_dvs_statistics               *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats);
+
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs_statistics_allocate(
+    const struct ia_css_dvs_grid_info *grid);
+
+void
+ia_css_isp_dvs_statistics_free(
+    struct ia_css_isp_dvs_statistics *me);
+
+size_t ia_css_sdis_hor_coef_tbl_bytes(const struct ia_css_binary *binary);
+size_t ia_css_sdis_ver_coef_tbl_bytes(const struct ia_css_binary *binary);
+
+void
+ia_css_sdis_init_info(
+    struct ia_css_sdis_info *dis,
+    unsigned int sc_3a_dis_width,
+    unsigned int sc_3a_dis_padded_width,
+    unsigned int sc_3a_dis_height,
+    unsigned int isp_pipe_version,
+    unsigned int enabled);
+
+void ia_css_sdis_clear_coefficients(
+    struct ia_css_dvs_coefficients *dvs_coefs);
+
+void ia_css_sdis_horicoef_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level);
+
+void ia_css_sdis_vertcoef_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level);
+
+void ia_css_sdis_horiproj_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level);
+
+void ia_css_sdis_vertproj_debug_dtrace(
+    const struct ia_css_dvs_coefficients *config, unsigned int level);
+
+#endif /* __IA_CSS_SDIS_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h
new file mode 100644
index 000000000..a8f2b8afc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h
@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SDIS_TYPES_H
+#define __IA_CSS_SDIS_TYPES_H
+
+/* @file
+* CSS-API header file for DVS statistics parameters.
+*/
+
+/* Number of DVS coefficient types */
+#define IA_CSS_DVS_NUM_COEF_TYPES      6
+
+#ifndef PIPE_GENERATION
+#include "isp/kernels/sdis/common/ia_css_sdis_common_types.h"
+#endif
+
+/* DVS 1.0 Coefficients.
+ *  This structure describes the coefficients that are needed for the dvs statistics.
+ */
+
+struct ia_css_dvs_coefficients {
+	struct ia_css_dvs_grid_info
+		grid;/** grid info contains the dimensions of the dvs grid */
+	s16 *hor_coefs;	/** the pointer to int16_t[grid.num_hor_coefs * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the horizontal coefficients */
+	s16 *ver_coefs;	/** the pointer to int16_t[grid.num_ver_coefs * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the vertical coefficients */
+};
+
+/* DVS 1.0 Statistics.
+ *  This structure describes the statistics that are generated using the provided coefficients.
+ */
+
+struct ia_css_dvs_statistics {
+	struct ia_css_dvs_grid_info
+		grid;/** grid info contains the dimensions of the dvs grid */
+	s32 *hor_proj;	/** the pointer to int16_t[grid.height * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the horizontal projections */
+	s32 *ver_proj;	/** the pointer to int16_t[grid.width * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the vertical projections */
+};
+
+#endif /* __IA_CSS_SDIS_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
new file mode 100644
index 000000000..c13de289a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
@@ -0,0 +1,349 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include <assert_support.h>
+#include "ia_css_debug.h"
+#include "ia_css_sdis2.host.h"
+
+const struct ia_css_dvs2_coefficients default_sdis2_config = {
+	.grid = { 0, 0, 0, 0, 0, 0, 0, 0 },
+	.hor_coefs = { NULL, NULL, NULL, NULL },
+	.ver_coefs = { NULL, NULL, NULL, NULL },
+};
+
+static void
+fill_row(short *private, const short *public, unsigned int width,
+	 unsigned int padding)
+{
+	memcpy(private, public, width * sizeof(short));
+	memset(&private[width], 0, padding * sizeof(short));
+}
+
+void ia_css_sdis2_horicoef_vmem_encode(
+    struct sh_css_isp_sdis_hori_coef_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size)
+{
+	unsigned int aligned_width = from->grid.aligned_width *
+				     from->grid.bqs_per_grid_cell;
+	unsigned int width         = from->grid.num_hor_coefs;
+	int      padding       = aligned_width - width;
+	unsigned int stride        = size / IA_CSS_DVS2_NUM_COEF_TYPES / sizeof(short);
+	unsigned int total_bytes   = aligned_width * IA_CSS_DVS2_NUM_COEF_TYPES *
+				     sizeof(short);
+	short   *private       = (short *)to;
+
+	/* Copy the table, add padding */
+	assert(padding >= 0);
+	assert(total_bytes <= size);
+	assert(size % (IA_CSS_DVS2_NUM_COEF_TYPES * ISP_VEC_NELEMS * sizeof(
+			   short)) == 0);
+	fill_row(&private[0 * stride], from->hor_coefs.odd_real,  width, padding);
+	fill_row(&private[1 * stride], from->hor_coefs.odd_imag,  width, padding);
+	fill_row(&private[2 * stride], from->hor_coefs.even_real, width, padding);
+	fill_row(&private[3 * stride], from->hor_coefs.even_imag, width, padding);
+}
+
+void ia_css_sdis2_vertcoef_vmem_encode(
+    struct sh_css_isp_sdis_vert_coef_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size)
+{
+	unsigned int aligned_height = from->grid.aligned_height *
+				      from->grid.bqs_per_grid_cell;
+	unsigned int height         = from->grid.num_ver_coefs;
+	int      padding        = aligned_height - height;
+	unsigned int stride         = size / IA_CSS_DVS2_NUM_COEF_TYPES / sizeof(short);
+	unsigned int total_bytes    = aligned_height * IA_CSS_DVS2_NUM_COEF_TYPES *
+				      sizeof(short);
+	short   *private        = (short *)to;
+
+	/* Copy the table, add padding */
+	assert(padding >= 0);
+	assert(total_bytes <= size);
+	assert(size % (IA_CSS_DVS2_NUM_COEF_TYPES * ISP_VEC_NELEMS * sizeof(
+			   short)) == 0);
+	fill_row(&private[0 * stride], from->ver_coefs.odd_real,  height, padding);
+	fill_row(&private[1 * stride], from->ver_coefs.odd_imag,  height, padding);
+	fill_row(&private[2 * stride], from->ver_coefs.even_real, height, padding);
+	fill_row(&private[3 * stride], from->ver_coefs.even_imag, height, padding);
+}
+
+void ia_css_sdis2_horiproj_encode(
+    struct sh_css_isp_sdis_hori_proj_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size)
+{
+	(void)to;
+	(void)from;
+	(void)size;
+}
+
+void ia_css_sdis2_vertproj_encode(
+    struct sh_css_isp_sdis_vert_proj_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size)
+{
+	(void)to;
+	(void)from;
+	(void)size;
+}
+
+void ia_css_get_isp_dvs2_coefficients(
+    struct ia_css_stream *stream,
+    short *hor_coefs_odd_real,
+    short *hor_coefs_odd_imag,
+    short *hor_coefs_even_real,
+    short *hor_coefs_even_imag,
+    short *ver_coefs_odd_real,
+    short *ver_coefs_odd_imag,
+    short *ver_coefs_even_real,
+    short *ver_coefs_even_imag)
+{
+	struct ia_css_isp_parameters *params;
+	unsigned int hor_num_3a, ver_num_3a;
+	struct ia_css_binary *dvs_binary;
+
+	IA_CSS_ENTER("void");
+
+	assert(stream);
+	assert(hor_coefs_odd_real);
+	assert(hor_coefs_odd_imag);
+	assert(hor_coefs_even_real);
+	assert(hor_coefs_even_imag);
+	assert(ver_coefs_odd_real);
+	assert(ver_coefs_odd_imag);
+	assert(ver_coefs_even_real);
+	assert(ver_coefs_even_imag);
+
+	params = stream->isp_params_configs;
+
+	/* Only video pipe supports DVS */
+	dvs_binary = ia_css_stream_get_dvs_binary(stream);
+	if (!dvs_binary)
+		return;
+
+	hor_num_3a  = dvs_binary->dis.coef.dim.width;
+	ver_num_3a  = dvs_binary->dis.coef.dim.height;
+
+	memcpy(hor_coefs_odd_real,  params->dvs2_coefs.hor_coefs.odd_real,
+	       hor_num_3a * sizeof(short));
+	memcpy(hor_coefs_odd_imag,  params->dvs2_coefs.hor_coefs.odd_imag,
+	       hor_num_3a * sizeof(short));
+	memcpy(hor_coefs_even_real, params->dvs2_coefs.hor_coefs.even_real,
+	       hor_num_3a * sizeof(short));
+	memcpy(hor_coefs_even_imag, params->dvs2_coefs.hor_coefs.even_imag,
+	       hor_num_3a * sizeof(short));
+	memcpy(ver_coefs_odd_real,  params->dvs2_coefs.ver_coefs.odd_real,
+	       ver_num_3a * sizeof(short));
+	memcpy(ver_coefs_odd_imag,  params->dvs2_coefs.ver_coefs.odd_imag,
+	       ver_num_3a * sizeof(short));
+	memcpy(ver_coefs_even_real, params->dvs2_coefs.ver_coefs.even_real,
+	       ver_num_3a * sizeof(short));
+	memcpy(ver_coefs_even_imag, params->dvs2_coefs.ver_coefs.even_imag,
+	       ver_num_3a * sizeof(short));
+
+	IA_CSS_LEAVE("void");
+}
+
+void ia_css_sdis2_clear_coefficients(
+    struct ia_css_dvs2_coefficients *dvs2_coefs)
+{
+	dvs2_coefs->hor_coefs.odd_real  = NULL;
+	dvs2_coefs->hor_coefs.odd_imag  = NULL;
+	dvs2_coefs->hor_coefs.even_real = NULL;
+	dvs2_coefs->hor_coefs.even_imag = NULL;
+	dvs2_coefs->ver_coefs.odd_real  = NULL;
+	dvs2_coefs->ver_coefs.odd_imag  = NULL;
+	dvs2_coefs->ver_coefs.even_real = NULL;
+	dvs2_coefs->ver_coefs.even_imag = NULL;
+}
+
+int
+ia_css_get_dvs2_statistics(
+    struct ia_css_dvs2_statistics          *host_stats,
+    const struct ia_css_isp_dvs_statistics *isp_stats) {
+	struct ia_css_isp_dvs_statistics_map *map;
+	int ret = 0;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats);
+	assert(isp_stats);
+
+	map = ia_css_isp_dvs_statistics_map_allocate(isp_stats, NULL);
+	if (map)
+	{
+		hmm_load(isp_stats->data_ptr, map->data_ptr, isp_stats->size);
+		ia_css_translate_dvs2_statistics(host_stats, map);
+		ia_css_isp_dvs_statistics_map_free(map);
+	} else
+	{
+		IA_CSS_ERROR("out of memory");
+		ret = -ENOMEM;
+	}
+
+	IA_CSS_LEAVE_ERR(ret);
+	return ret;
+}
+
+void
+ia_css_translate_dvs2_statistics(
+    struct ia_css_dvs2_statistics		   *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats)
+{
+	unsigned int size_bytes, table_width, table_size, height;
+	unsigned int src_offset = 0, dst_offset = 0;
+	s32 *htemp_ptr, *vtemp_ptr;
+
+	assert(host_stats);
+	assert(host_stats->hor_prod.odd_real);
+	assert(host_stats->hor_prod.odd_imag);
+	assert(host_stats->hor_prod.even_real);
+	assert(host_stats->hor_prod.even_imag);
+	assert(host_stats->ver_prod.odd_real);
+	assert(host_stats->ver_prod.odd_imag);
+	assert(host_stats->ver_prod.even_real);
+	assert(host_stats->ver_prod.even_imag);
+	assert(isp_stats);
+	assert(isp_stats->hor_proj);
+	assert(isp_stats->ver_proj);
+
+	IA_CSS_ENTER("hor_coefs.odd_real=%p, hor_coefs.odd_imag=%p, hor_coefs.even_real=%p, hor_coefs.even_imag=%p, ver_coefs.odd_real=%p, ver_coefs.odd_imag=%p, ver_coefs.even_real=%p, ver_coefs.even_imag=%p, haddr=%p, vaddr=%p",
+		     host_stats->hor_prod.odd_real, host_stats->hor_prod.odd_imag,
+		     host_stats->hor_prod.even_real, host_stats->hor_prod.even_imag,
+		     host_stats->ver_prod.odd_real, host_stats->ver_prod.odd_imag,
+		     host_stats->ver_prod.even_real, host_stats->ver_prod.even_imag,
+		     isp_stats->hor_proj, isp_stats->ver_proj);
+
+	/* Host side: reflecting the true width in bytes */
+	size_bytes = host_stats->grid.aligned_width * sizeof(*htemp_ptr);
+
+	/* DDR side: need to be aligned to the system bus width */
+	/* statistics table width in terms of 32-bit words*/
+	table_width = CEIL_MUL(size_bytes,
+			       HIVE_ISP_DDR_WORD_BYTES) / sizeof(*htemp_ptr);
+	table_size = table_width * host_stats->grid.aligned_height;
+
+	htemp_ptr = isp_stats->hor_proj; /* horizontal stats */
+	vtemp_ptr = isp_stats->ver_proj; /* vertical stats */
+	for (height = 0; height < host_stats->grid.aligned_height; height++) {
+		/* hor stats */
+		memcpy(host_stats->hor_prod.odd_real + dst_offset,
+		       &htemp_ptr[0 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->hor_prod.odd_imag + dst_offset,
+		       &htemp_ptr[1 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->hor_prod.even_real + dst_offset,
+		       &htemp_ptr[2 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->hor_prod.even_imag + dst_offset,
+		       &htemp_ptr[3 * table_size + src_offset], size_bytes);
+
+		/* ver stats */
+		memcpy(host_stats->ver_prod.odd_real + dst_offset,
+		       &vtemp_ptr[0 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->ver_prod.odd_imag + dst_offset,
+		       &vtemp_ptr[1 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->ver_prod.even_real + dst_offset,
+		       &vtemp_ptr[2 * table_size + src_offset], size_bytes);
+		memcpy(host_stats->ver_prod.even_imag + dst_offset,
+		       &vtemp_ptr[3 * table_size + src_offset], size_bytes);
+
+		src_offset += table_width; /* aligned table width */
+		dst_offset += host_stats->grid.aligned_width;
+	}
+
+	IA_CSS_LEAVE("void");
+}
+
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs2_statistics_allocate(
+    const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_isp_dvs_statistics *me;
+	int size;
+
+	assert(grid);
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	if (!grid->enable)
+		return NULL;
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	/* on ISP 2 SDIS DMA model, every row of projection table width must be
+	   aligned to HIVE_ISP_DDR_WORD_BYTES
+	*/
+	size = CEIL_MUL(sizeof(int) * grid->aligned_width, HIVE_ISP_DDR_WORD_BYTES)
+	       * grid->aligned_height * IA_CSS_DVS2_NUM_COEF_TYPES;
+
+	me->size = 2 * size;
+	me->data_ptr = hmm_alloc(me->size);
+	if (me->data_ptr == mmgr_NULL)
+		goto err;
+	me->hor_proj = me->data_ptr;
+	me->hor_size = size;
+	me->ver_proj = me->data_ptr + size;
+	me->ver_size = size;
+
+	IA_CSS_LEAVE("return=%p", me);
+	return me;
+err:
+	ia_css_isp_dvs2_statistics_free(me);
+	IA_CSS_LEAVE("return=%p", NULL);
+
+	return NULL;
+}
+
+void
+ia_css_isp_dvs2_statistics_free(struct ia_css_isp_dvs_statistics *me)
+{
+	if (me) {
+		hmm_free(me->data_ptr);
+		kvfree(me);
+	}
+}
+
+void ia_css_sdis2_horicoef_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_vertcoef_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_horiproj_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_vertproj_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h
new file mode 100644
index 000000000..e0e6b9c33
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h
@@ -0,0 +1,96 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SDIS2_HOST_H
+#define __IA_CSS_SDIS2_HOST_H
+
+#include "ia_css_sdis2_types.h"
+#include "ia_css_binary.h"
+#include "ia_css_stream.h"
+#include "sh_css_params.h"
+
+extern const struct ia_css_dvs2_coefficients default_sdis2_config;
+
+/* Opaque here, since size is binary dependent. */
+struct sh_css_isp_sdis_hori_coef_tbl;
+struct sh_css_isp_sdis_vert_coef_tbl;
+struct sh_css_isp_sdis_hori_proj_tbl;
+struct sh_css_isp_sdis_vert_proj_tbl;
+
+void ia_css_sdis2_horicoef_vmem_encode(
+    struct sh_css_isp_sdis_hori_coef_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis2_vertcoef_vmem_encode(
+    struct sh_css_isp_sdis_vert_coef_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis2_horiproj_encode(
+    struct sh_css_isp_sdis_hori_proj_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size);
+
+void ia_css_sdis2_vertproj_encode(
+    struct sh_css_isp_sdis_vert_proj_tbl *to,
+    const struct ia_css_dvs2_coefficients *from,
+    unsigned int size);
+
+void ia_css_get_isp_dvs2_coefficients(
+    struct ia_css_stream *stream,
+    short *hor_coefs_odd_real,
+    short *hor_coefs_odd_imag,
+    short *hor_coefs_even_real,
+    short *hor_coefs_even_imag,
+    short *ver_coefs_odd_real,
+    short *ver_coefs_odd_imag,
+    short *ver_coefs_even_real,
+    short *ver_coefs_even_imag);
+
+void ia_css_sdis2_clear_coefficients(
+    struct ia_css_dvs2_coefficients *dvs2_coefs);
+
+int
+ia_css_get_dvs2_statistics(
+    struct ia_css_dvs2_statistics	       *host_stats,
+    const struct ia_css_isp_dvs_statistics *isp_stats);
+
+void
+ia_css_translate_dvs2_statistics(
+    struct ia_css_dvs2_statistics              *host_stats,
+    const struct ia_css_isp_dvs_statistics_map *isp_stats);
+
+struct ia_css_isp_dvs_statistics *
+ia_css_isp_dvs2_statistics_allocate(
+    const struct ia_css_dvs_grid_info *grid);
+
+void
+ia_css_isp_dvs2_statistics_free(
+    struct ia_css_isp_dvs_statistics *me);
+
+void ia_css_sdis2_horicoef_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level);
+
+void ia_css_sdis2_vertcoef_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level);
+
+void ia_css_sdis2_horiproj_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level);
+
+void ia_css_sdis2_vertproj_debug_dtrace(
+    const struct ia_css_dvs2_coefficients *config, unsigned int level);
+
+#endif /* __IA_CSS_SDIS2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h
new file mode 100644
index 000000000..d75b72e95
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h
@@ -0,0 +1,76 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SDIS2_TYPES_H
+#define __IA_CSS_SDIS2_TYPES_H
+
+/* @file
+* CSS-API header file for DVS statistics parameters.
+*/
+
+/* Number of DVS coefficient types */
+#define IA_CSS_DVS2_NUM_COEF_TYPES     4
+
+#ifndef PIPE_GENERATION
+#include "isp/kernels/sdis/common/ia_css_sdis_common_types.h"
+#endif
+
+/* DVS 2.0 Coefficient types. This structure contains 4 pointers to
+ *  arrays that contain the coeffients for each type.
+ */
+struct ia_css_dvs2_coef_types {
+	s16 *odd_real; /** real part of the odd coefficients*/
+	s16 *odd_imag; /** imaginary part of the odd coefficients*/
+	s16 *even_real;/** real part of the even coefficients*/
+	s16 *even_imag;/** imaginary part of the even coefficients*/
+};
+
+/* DVS 2.0 Coefficients. This structure describes the coefficients that are needed for the dvs statistics.
+ *  e.g. hor_coefs.odd_real is the pointer to int16_t[grid.num_hor_coefs] containing the horizontal odd real
+ *  coefficients.
+ */
+struct ia_css_dvs2_coefficients {
+	struct ia_css_dvs_grid_info
+		grid;        /** grid info contains the dimensions of the dvs grid */
+	struct ia_css_dvs2_coef_types
+		hor_coefs; /** struct with pointers that contain the horizontal coefficients */
+	struct ia_css_dvs2_coef_types
+		ver_coefs; /** struct with pointers that contain the vertical coefficients */
+};
+
+/* DVS 2.0 Statistic types. This structure contains 4 pointers to
+ *  arrays that contain the statistics for each type.
+ */
+struct ia_css_dvs2_stat_types {
+	s32 *odd_real; /** real part of the odd statistics*/
+	s32 *odd_imag; /** imaginary part of the odd statistics*/
+	s32 *even_real;/** real part of the even statistics*/
+	s32 *even_imag;/** imaginary part of the even statistics*/
+};
+
+/* DVS 2.0 Statistics. This structure describes the statistics that are generated using the provided coefficients.
+ *  e.g. hor_prod.odd_real is the pointer to int16_t[grid.aligned_height][grid.aligned_width] containing
+ *  the horizontal odd real statistics. Valid statistics data area is int16_t[0..grid.height-1][0..grid.width-1]
+ */
+struct ia_css_dvs2_statistics {
+	struct ia_css_dvs_grid_info
+		grid;       /** grid info contains the dimensions of the dvs grid */
+	struct ia_css_dvs2_stat_types
+		hor_prod; /** struct with pointers that contain the horizontal statistics */
+	struct ia_css_dvs2_stat_types
+		ver_prod; /** struct with pointers that contain the vertical statistics */
+};
+
+#endif /* __IA_CSS_SDIS2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c
new file mode 100644
index 000000000..fef8c5457
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c
@@ -0,0 +1,75 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_debug.h"
+#include "ia_css_tdf.host.h"
+
+static const s16 g_pyramid[8][8] = {
+	{128, 384, 640, 896, 896, 640, 384, 128},
+	{384, 1152, 1920, 2688, 2688, 1920, 1152, 384},
+	{640, 1920, 3200, 4480, 4480, 3200, 1920, 640},
+	{896, 2688, 4480, 6272, 6272, 4480, 2688, 896},
+	{896, 2688, 4480, 6272, 6272, 4480, 2688, 896},
+	{640, 1920, 3200, 4480, 4480, 3200, 1920, 640},
+	{384, 1152, 1920, 2688, 2688, 1920, 1152, 384},
+	{128, 384, 640, 896, 896, 640, 384, 128}
+};
+
+void
+ia_css_tdf_vmem_encode(
+    struct ia_css_isp_tdf_vmem_params *to,
+    const struct ia_css_tdf_config *from,
+    size_t size)
+{
+	unsigned int i;
+	(void)size;
+
+	for (i = 0; i < ISP_VEC_NELEMS; i++) {
+		to->pyramid[0][i]          = g_pyramid[i / 8][i % 8];
+		to->threshold_flat[0][i]   = from->thres_flat_table[i];
+		to->threshold_detail[0][i] = from->thres_detail_table[i];
+	}
+}
+
+void
+ia_css_tdf_encode(
+    struct ia_css_isp_tdf_dmem_params *to,
+    const struct ia_css_tdf_config *from,
+    size_t size)
+{
+	(void)size;
+	to->Epsilon_0        = from->epsilon_0;
+	to->Epsilon_1        = from->epsilon_1;
+	to->EpsScaleText     = from->eps_scale_text;
+	to->EpsScaleEdge     = from->eps_scale_edge;
+	to->Sepa_flat	     = from->sepa_flat;
+	to->Sepa_Edge	     = from->sepa_edge;
+	to->Blend_Flat	     = from->blend_flat;
+	to->Blend_Text	     = from->blend_text;
+	to->Blend_Edge	     = from->blend_edge;
+	to->Shading_Gain     = from->shading_gain;
+	to->Shading_baseGain = from->shading_base_gain;
+	to->LocalY_Gain      = from->local_y_gain;
+	to->LocalY_baseGain  = from->local_y_base_gain;
+}
+
+void
+ia_css_tdf_debug_dtrace(
+    const struct ia_css_tdf_config *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h
new file mode 100644
index 000000000..7e44d78c5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TDF_HOST_H
+#define __IA_CSS_TDF_HOST_H
+
+#include "ia_css_tdf_types.h"
+#include "ia_css_tdf_param.h"
+
+void
+ia_css_tdf_vmem_encode(
+    struct ia_css_isp_tdf_vmem_params *to,
+    const struct ia_css_tdf_config *from,
+    size_t size);
+
+void
+ia_css_tdf_encode(
+    struct ia_css_isp_tdf_dmem_params *to,
+    const struct ia_css_tdf_config *from,
+    size_t size);
+
+void
+ia_css_tdf_debug_dtrace(
+    const struct ia_css_tdf_config *config, unsigned int level)
+;
+
+#endif /* __IA_CSS_TDF_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h
new file mode 100644
index 000000000..e904f7122
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TDF_PARAM_H
+#define __IA_CSS_TDF_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h" /* needed for VMEM_ARRAY */
+
+struct ia_css_isp_tdf_vmem_params {
+	VMEM_ARRAY(pyramid, ISP_VEC_NELEMS);
+	VMEM_ARRAY(threshold_flat, ISP_VEC_NELEMS);
+	VMEM_ARRAY(threshold_detail, ISP_VEC_NELEMS);
+};
+
+struct ia_css_isp_tdf_dmem_params {
+	s32 Epsilon_0;
+	s32 Epsilon_1;
+	s32 EpsScaleText;
+	s32 EpsScaleEdge;
+	s32 Sepa_flat;
+	s32 Sepa_Edge;
+	s32 Blend_Flat;
+	s32 Blend_Text;
+	s32 Blend_Edge;
+	s32 Shading_Gain;
+	s32 Shading_baseGain;
+	s32 LocalY_Gain;
+	s32 LocalY_baseGain;
+};
+
+#endif /* __IA_CSS_TDF_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h
new file mode 100644
index 000000000..0f69f9128
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TDF_TYPES_H
+#define __IA_CSS_TDF_TYPES_H
+
+/* @file
+* CSS-API header file for Transform Domain Filter parameters.
+*/
+
+#include "type_support.h"
+
+/* Transform Domain Filter configuration
+ *
+ * \brief TDF public parameters.
+ * \details Struct with all parameters for the TDF kernel that can be set
+ * from the CSS API.
+ *
+ * ISP2.6.1: TDF is used.
+ */
+struct ia_css_tdf_config {
+	s32 thres_flat_table[64];	/** Final optimized strength table of NR for flat region. */
+	s32 thres_detail_table[64];	/** Final optimized strength table of NR for detail region. */
+	s32 epsilon_0;		/** Coefficient to control variance for dark area (for flat region). */
+	s32 epsilon_1;		/** Coefficient to control variance for bright area (for flat region). */
+	s32 eps_scale_text;		/** Epsilon scaling coefficient for texture region. */
+	s32 eps_scale_edge;		/** Epsilon scaling coefficient for edge region. */
+	s32 sepa_flat;		/** Threshold to judge flat (edge < m_Flat_thre). */
+	s32 sepa_edge;		/** Threshold to judge edge (edge > m_Edge_thre). */
+	s32 blend_flat;		/** Blending ratio at flat region. */
+	s32 blend_text;		/** Blending ratio at texture region. */
+	s32 blend_edge;		/** Blending ratio at edge region. */
+	s32 shading_gain;		/** Gain of Shading control. */
+	s32 shading_base_gain;	/** Base Gain of Shading control. */
+	s32 local_y_gain;		/** Gain of local luminance control. */
+	s32 local_y_base_gain;	/** Base gain of local luminance control. */
+	s32 rad_x_origin;		/** Initial x coord. for radius computation. */
+	s32 rad_y_origin;		/** Initial y coord. for radius computation. */
+};
+
+#endif /* __IA_CSS_TDF_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h
new file mode 100644
index 000000000..4b53fddfc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef _IA_CSS_TNR3_TYPES_H
+#define _IA_CSS_TNR3_TYPES_H
+
+/* @file
+* CSS-API header file for Temporal Noise Reduction v3 (TNR3) kernel
+*/
+
+/**
+ * \brief Number of piecewise linear segments.
+ * \details The parameters to TNR3 are specified as a piecewise linear segment.
+ * The number of such segments is fixed at 3.
+ */
+#define TNR3_NUM_SEGMENTS    3
+
+/* Temporal Noise Reduction v3 (TNR3) configuration.
+ * The parameter to this kernel is fourfold
+ * 1. Three piecewise linear graphs (one for each plane) with three segments
+ * each. Each line graph has Luma values on the x axis and sigma values for
+ * each plane on the y axis. The three linear segments may have a different
+ * slope and the point of Luma value which where the slope may change is called
+ * a "Knee" point. As there are three such segments, four points need to be
+ * specified each on the Luma axis and the per plane Sigma axis. On the Luma
+ * axis two points are fixed (namely 0 and maximum luma value - depending on
+ * ISP bit depth). The other two points are the points where the slope may
+ * change its value. These two points are called knee points. The four points on
+ * the per plane sigma axis are also specified at the interface.
+ * 2. One rounding adjustment parameter for each plane
+ * 3. One maximum feedback threshold value for each plane
+ * 4. Selection of the reference frame buffer to be used for noise reduction.
+ */
+struct ia_css_tnr3_kernel_config {
+	unsigned int maxfb_y;                        /** Maximum Feedback Gain for Y */
+	unsigned int maxfb_u;                        /** Maximum Feedback Gain for U */
+	unsigned int maxfb_v;                        /** Maximum Feedback Gain for V */
+	unsigned int round_adj_y;                    /** Rounding Adjust for Y */
+	unsigned int round_adj_u;                    /** Rounding Adjust for U */
+	unsigned int round_adj_v;                    /** Rounding Adjust for V */
+	unsigned int knee_y[TNR3_NUM_SEGMENTS - 1];  /** Knee points */
+	unsigned int sigma_y[TNR3_NUM_SEGMENTS +
+					       1]; /** Standard deviation for Y at points Y0, Y1, Y2, Y3 */
+	unsigned int sigma_u[TNR3_NUM_SEGMENTS +
+					       1]; /** Standard deviation for U at points U0, U1, U2, U3 */
+	unsigned int sigma_v[TNR3_NUM_SEGMENTS +
+					       1]; /** Standard deviation for V at points V0, V1, V2, V3 */
+	unsigned int
+	ref_buf_select;                 /** Selection of the reference buffer */
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
new file mode 100644
index 000000000..53050c0c4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "ia_css_frame.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+#include "assert_support.h"
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#include "isp.h"
+
+#include "ia_css_tnr.host.h"
+const struct ia_css_tnr_config default_tnr_config = {
+	32768,
+	32,
+	32,
+};
+
+void
+ia_css_tnr_encode(
+    struct sh_css_isp_tnr_params *to,
+    const struct ia_css_tnr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->coef =
+	    uDIGIT_FITTING(from->gain, 16, SH_CSS_TNR_COEF_SHIFT);
+	to->threshold_Y =
+	    uDIGIT_FITTING(from->threshold_y, 16, SH_CSS_ISP_YUV_BITS);
+	to->threshold_C =
+	    uDIGIT_FITTING(from->threshold_uv, 16, SH_CSS_ISP_YUV_BITS);
+}
+
+void
+ia_css_tnr_dump(
+    const struct sh_css_isp_tnr_params *tnr,
+    unsigned int level)
+{
+	if (!tnr) return;
+	ia_css_debug_dtrace(level, "Temporal Noise Reduction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "tnr_coef", tnr->coef);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "tnr_threshold_Y", tnr->threshold_Y);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "tnr_threshold_C", tnr->threshold_C);
+}
+
+void
+ia_css_tnr_debug_dtrace(
+    const struct ia_css_tnr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.gain=%d, config.threshold_y=%d, config.threshold_uv=%d\n",
+			    config->gain,
+			    config->threshold_y, config->threshold_uv);
+}
+
+int ia_css_tnr_config(struct sh_css_isp_tnr_isp_config *to,
+		      const struct ia_css_tnr_configuration *from,
+		      unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	unsigned int i;
+	int ret;
+
+	ret = ia_css_dma_configure_from_info(&to->port_b, &from->tnr_frames[0]->info);
+	if (ret)
+		return ret;
+	to->width_a_over_b = elems_a / to->port_b.elems;
+	to->frame_height = from->tnr_frames[0]->info.res.height;
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+		to->tnr_frame_addr[i] = from->tnr_frames[i]->data +
+					from->tnr_frames[i]->planes.yuyv.offset;
+	}
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	if (elems_a % to->port_b.elems != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int ia_css_tnr_configure(const struct ia_css_binary     *binary,
+			 const struct ia_css_frame * const *frames)
+{
+	struct ia_css_tnr_configuration config;
+	unsigned int i;
+
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++)
+		config.tnr_frames[i] = frames[i];
+
+	return ia_css_configure_tnr(binary, &config);
+}
+
+void
+ia_css_init_tnr_state(
+    struct sh_css_isp_tnr_dmem_state *state,
+    size_t size)
+{
+	(void)size;
+
+	assert(NUM_VIDEO_TNR_FRAMES >= 2);
+	assert(sizeof(*state) == size);
+	state->tnr_in_buf_idx = 0;
+	state->tnr_out_buf_idx = 1;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h
new file mode 100644
index 000000000..acf92052b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TNR_HOST_H
+#define __IA_CSS_TNR_HOST_H
+
+#include "ia_css_binary.h"
+#include "ia_css_tnr_state.h"
+#include "ia_css_tnr_types.h"
+#include "ia_css_tnr_param.h"
+
+extern const struct ia_css_tnr_config default_tnr_config;
+
+void
+ia_css_tnr_encode(
+    struct sh_css_isp_tnr_params *to,
+    const struct ia_css_tnr_config *from,
+    unsigned int size);
+
+void
+ia_css_tnr_dump(
+    const struct sh_css_isp_tnr_params *tnr,
+    unsigned int level);
+
+void
+ia_css_tnr_debug_dtrace(
+    const struct ia_css_tnr_config *config,
+    unsigned int level);
+
+int ia_css_tnr_config(struct sh_css_isp_tnr_isp_config      *to,
+		      const struct ia_css_tnr_configuration *from,
+		      unsigned int size);
+
+int ia_css_tnr_configure(const struct ia_css_binary        *binary,
+			 const struct ia_css_frame * const *frames);
+
+void
+ia_css_init_tnr_state(
+    struct sh_css_isp_tnr_dmem_state *state,
+    size_t size);
+#endif /* __IA_CSS_TNR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h
new file mode 100644
index 000000000..551dd5cfa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TNR_PARAM_H
+#define __IA_CSS_TNR_PARAM_H
+
+#include "type_support.h"
+#include "sh_css_defs.h"
+#include "dma.h"
+
+/* TNR (Temporal Noise Reduction) */
+struct sh_css_isp_tnr_params {
+	s32 coef;
+	s32 threshold_Y;
+	s32 threshold_C;
+};
+
+struct ia_css_tnr_configuration {
+	const struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];
+};
+
+struct sh_css_isp_tnr_isp_config {
+	u32 width_a_over_b;
+	u32 frame_height;
+	struct dma_port_config port_b;
+	ia_css_ptr tnr_frame_addr[NUM_VIDEO_TNR_FRAMES];
+};
+
+#endif /* __IA_CSS_TNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h
new file mode 100644
index 000000000..d57238423
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TNR_STATE_H
+#define __IA_CSS_TNR_STATE_H
+
+#include "type_support.h"
+
+/* TNR (temporal noise reduction) */
+struct sh_css_isp_tnr_dmem_state {
+	u32 tnr_in_buf_idx;
+	u32 tnr_out_buf_idx;
+};
+
+#endif /* __IA_CSS_TNR_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h
new file mode 100644
index 000000000..92dbe1389
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TNR_TYPES_H
+#define __IA_CSS_TNR_TYPES_H
+
+/* @file
+* CSS-API header file for Temporal Noise Reduction (TNR) parameters.
+*/
+
+/* Temporal Noise Reduction (TNR) configuration.
+ *
+ *  When difference between current frame and previous frame is less than or
+ *  equal to threshold, TNR works and current frame is mixed
+ *  with previous frame.
+ *  When difference between current frame and previous frame is greater
+ *  than threshold, we judge motion is detected. Then, TNR does not work and
+ *  current frame is outputted as it is.
+ *  Therefore, when threshold_y and threshold_uv are set as 0, TNR can be disabled.
+ *
+ *  ISP block: TNR1
+ *  ISP1: TNR1 is used.
+ *  ISP2: TNR1 is used.
+ */
+
+struct ia_css_tnr_config {
+	ia_css_u0_16 gain; /** Interpolation ratio of current frame
+				and previous frame.
+				gain=0.0 -> previous frame is outputted.
+				gain=1.0 -> current frame is outputted.
+				u0.16, [0,65535],
+			default 32768(0.5), ineffective 65535(almost 1.0) */
+	ia_css_u0_16 threshold_y; /** Threshold to enable interpolation of Y.
+				If difference between current frame and
+				previous frame is greater than threshold_y,
+				TNR for Y is disabled.
+				u0.16, [0,65535], default/ineffective 0 */
+	ia_css_u0_16 threshold_uv; /** Threshold to enable interpolation of
+				U/V.
+				If difference between current frame and
+				previous frame is greater than threshold_uv,
+				TNR for UV is disabled.
+				u0.16, [0,65535], default/ineffective 0 */
+};
+
+#endif /* __IA_CSS_TNR_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/uds/uds_1.0/ia_css_uds_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/uds/uds_1.0/ia_css_uds_param.h
new file mode 100644
index 000000000..784b5c4fa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/uds/uds_1.0/ia_css_uds_param.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_UDS_PARAM_H
+#define __IA_CSS_UDS_PARAM_H
+
+#include "sh_css_uds.h"
+
+/* uds (Up and Down scaling) */
+struct ia_css_uds_config {
+	struct sh_css_crop_pos crop_pos;
+	struct sh_css_uds_info uds;
+};
+
+struct sh_css_sp_uds_params {
+	struct sh_css_crop_pos crop_pos;
+	struct sh_css_uds_info uds;
+};
+
+#endif /* __IA_CSS_UDS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.c
new file mode 100644
index 000000000..aecdcbe04
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.c
@@ -0,0 +1,144 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "atomisp_internal.h"
+
+#include "ia_css_vf.host.h"
+#include <assert_support.h>
+#include <ia_css_err.h>
+#include <ia_css_frame.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_pipeline.h>
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+
+#include "isp.h"
+
+int ia_css_vf_config(struct sh_css_isp_vf_isp_config      *to,
+		    const struct ia_css_vf_configuration *from,
+		    unsigned int size)
+{
+	unsigned int elems_a = ISP_VEC_NELEMS;
+	int ret;
+
+	to->vf_downscale_bits = from->vf_downscale_bits;
+	to->enable = from->info != NULL;
+
+	if (from->info) {
+		ia_css_frame_info_to_frame_sp_info(&to->info, from->info);
+		ret = ia_css_dma_configure_from_info(&to->dma.port_b, from->info);
+		if (ret)
+			return ret;
+		to->dma.width_a_over_b = elems_a / to->dma.port_b.elems;
+
+		/* Assume divisiblity here, may need to generalize to fixed point. */
+		if (elems_a % to->dma.port_b.elems != 0)
+			return -EINVAL;
+	}
+	return 0;
+}
+
+/* compute the log2 of the downscale factor needed to get closest
+ * to the requested viewfinder resolution on the upper side. The output cannot
+ * be smaller than the requested viewfinder resolution.
+ */
+int
+sh_css_vf_downscale_log2(
+    const struct ia_css_frame_info *out_info,
+    const struct ia_css_frame_info *vf_info,
+    unsigned int *downscale_log2) {
+	unsigned int ds_log2 = 0;
+	unsigned int out_width;
+
+	if ((!out_info) || (!vf_info))
+		return -EINVAL;
+
+	out_width = out_info->res.width;
+
+	if (out_width == 0)
+		return -EINVAL;
+
+	/* downscale until width smaller than the viewfinder width. We don't
+	* test for the height since the vmem buffers only put restrictions on
+	* the width of a line, not on the number of lines in a frame.
+	*/
+	while (out_width >= vf_info->res.width)
+	{
+		ds_log2++;
+		out_width /= 2;
+	}
+	/* now width is smaller, so we go up one step */
+	if ((ds_log2 > 0) && (out_width < ia_css_binary_max_vf_width()))
+		ds_log2--;
+	/* TODO: use actual max input resolution of vf_pp binary */
+	if ((out_info->res.width >> ds_log2) >= 2 * ia_css_binary_max_vf_width())
+		return -EINVAL;
+	*downscale_log2 = ds_log2;
+	return 0;
+}
+
+static int
+configure_kernel(
+    const struct ia_css_binary_info *info,
+    const struct ia_css_frame_info *out_info,
+    const struct ia_css_frame_info *vf_info,
+    unsigned int *downscale_log2,
+    struct ia_css_vf_configuration *config) {
+	int err;
+	unsigned int vf_log_ds = 0;
+
+	/* First compute value */
+	if (vf_info)
+	{
+		err = sh_css_vf_downscale_log2(out_info, vf_info, &vf_log_ds);
+		if (err)
+			return err;
+	}
+	vf_log_ds = min(vf_log_ds, info->vf_dec.max_log_downscale);
+	*downscale_log2 = vf_log_ds;
+
+	/* Then store it in isp config section */
+	config->vf_downscale_bits = vf_log_ds;
+	return 0;
+}
+
+static void
+configure_dma(
+    struct ia_css_vf_configuration *config,
+    const struct ia_css_frame_info *vf_info)
+{
+	config->info = vf_info;
+}
+
+int ia_css_vf_configure(const struct ia_css_binary *binary,
+		        const struct ia_css_frame_info *out_info,
+			struct ia_css_frame_info *vf_info,
+			unsigned int *downscale_log2)
+{
+	int err;
+	struct ia_css_vf_configuration config;
+	const struct ia_css_binary_info *info = &binary->info->sp;
+
+	err = configure_kernel(info, out_info, vf_info, downscale_log2, &config);
+	if (err)
+		dev_warn(atomisp_dev, "Couldn't setup downscale\n");
+
+	configure_dma(&config, vf_info);
+
+	if (vf_info)
+		vf_info->raw_bit_depth = info->dma.vfdec_bits_per_pixel;
+
+	return ia_css_configure_vf(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.h
new file mode 100644
index 000000000..d6b45d375
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf.host.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_VF_HOST_H
+#define __IA_CSS_VF_HOST_H
+
+#include "ia_css_frame_public.h"
+#include "ia_css_binary.h"
+
+#include "ia_css_vf_types.h"
+#include "ia_css_vf_param.h"
+
+/* compute the log2 of the downscale factor needed to get closest
+ * to the requested viewfinder resolution on the upper side. The output cannot
+ * be smaller than the requested viewfinder resolution.
+ */
+int
+sh_css_vf_downscale_log2(
+    const struct ia_css_frame_info *out_info,
+    const struct ia_css_frame_info *vf_info,
+    unsigned int *downscale_log2);
+
+int ia_css_vf_config(struct sh_css_isp_vf_isp_config *to,
+		     const struct ia_css_vf_configuration *from,
+		     unsigned int size);
+
+int
+ia_css_vf_configure(
+    const struct ia_css_binary *binary,
+    const struct ia_css_frame_info *out_info,
+    struct ia_css_frame_info *vf_info,
+    unsigned int *downscale_log2);
+
+#endif /* __IA_CSS_VF_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_param.h
new file mode 100644
index 000000000..487ddf163
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_param.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_VF_PARAM_H
+#define __IA_CSS_VF_PARAM_H
+
+#include "type_support.h"
+#include "dma.h"
+#include "gc/gc_1.0/ia_css_gc_param.h" /* GAMMA_OUTPUT_BITS */
+#include "ia_css_frame_comm.h" /* ia_css_frame_sp_info */
+#include "ia_css_vf_types.h"
+
+#define VFDEC_BITS_PER_PIXEL	GAMMA_OUTPUT_BITS
+
+/* Viewfinder decimation */
+struct sh_css_isp_vf_isp_config {
+	u32 vf_downscale_bits; /** Log VF downscale value */
+	u32 enable;
+	struct ia_css_frame_sp_info info;
+	struct {
+		u32 width_a_over_b;
+		struct dma_port_config port_b;
+	} dma;
+};
+
+#endif /* __IA_CSS_VF_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_types.h
new file mode 100644
index 000000000..24fbb61d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/vf/vf_1.0/ia_css_vf_types.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_VF_TYPES_H
+#define __IA_CSS_VF_TYPES_H
+
+/* Viewfinder decimation
+ *
+ *  ISP block: vfeven_horizontal_downscale
+ */
+
+#include <ia_css_frame_public.h>
+#include <type_support.h>
+
+struct ia_css_vf_configuration {
+	u32 vf_downscale_bits; /** Log VF downscale value */
+	const struct ia_css_frame_info *info;
+};
+
+#endif /* __IA_CSS_VF_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.c
new file mode 100644
index 000000000..01d1a2d36
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.c
@@ -0,0 +1,87 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#ifndef IA_CSS_NO_DEBUG
+#include "ia_css_debug.h"
+#endif
+#include "sh_css_frac.h"
+
+#include "ia_css_wb.host.h"
+
+const struct ia_css_wb_config default_wb_config = {
+	1,
+	32768,
+	32768,
+	32768,
+	32768
+};
+
+void
+ia_css_wb_encode(
+    struct sh_css_isp_wb_params *to,
+    const struct ia_css_wb_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->gain_shift =
+	    uISP_REG_BIT - from->integer_bits;
+	to->gain_gr =
+	    uDIGIT_FITTING(from->gr, 16 - from->integer_bits,
+			   to->gain_shift);
+	to->gain_r =
+	    uDIGIT_FITTING(from->r, 16 - from->integer_bits,
+			   to->gain_shift);
+	to->gain_b =
+	    uDIGIT_FITTING(from->b, 16 - from->integer_bits,
+			   to->gain_shift);
+	to->gain_gb =
+	    uDIGIT_FITTING(from->gb, 16 - from->integer_bits,
+			   to->gain_shift);
+}
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_wb_dump(
+    const struct sh_css_isp_wb_params *wb,
+    unsigned int level)
+{
+	if (!wb) return;
+	ia_css_debug_dtrace(level, "White Balance:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "wb_gain_shift", wb->gain_shift);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "wb_gain_gr", wb->gain_gr);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "wb_gain_r", wb->gain_r);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "wb_gain_b", wb->gain_b);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "wb_gain_gb", wb->gain_gb);
+}
+
+void
+ia_css_wb_debug_dtrace(
+    const struct ia_css_wb_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.integer_bits=%d, config.gr=%d, config.r=%d, config.b=%d, config.gb=%d\n",
+			    config->integer_bits,
+			    config->gr, config->r,
+			    config->b, config->gb);
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.h
new file mode 100644
index 000000000..ffd75c8a6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb.host.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_WB_HOST_H
+#define __IA_CSS_WB_HOST_H
+
+#include "ia_css_wb_types.h"
+#include "ia_css_wb_param.h"
+
+extern const struct ia_css_wb_config default_wb_config;
+
+void
+ia_css_wb_encode(
+    struct sh_css_isp_wb_params *to,
+    const struct ia_css_wb_config *from,
+    unsigned int size);
+
+void
+ia_css_wb_dump(
+    const struct sh_css_isp_wb_params *wb,
+    unsigned int level);
+
+void
+ia_css_wb_debug_dtrace(
+    const struct ia_css_wb_config *wb,
+    unsigned int level);
+
+#endif /* __IA_CSS_WB_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_param.h
new file mode 100644
index 000000000..51b2ba8ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_param.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_WB_PARAM_H
+#define __IA_CSS_WB_PARAM_H
+
+#include "type_support.h"
+
+/* WB (White Balance) */
+struct sh_css_isp_wb_params {
+	s32 gain_shift;
+	s32 gain_gr;
+	s32 gain_r;
+	s32 gain_b;
+	s32 gain_gb;
+};
+
+#endif /* __IA_CSS_WB_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_types.h
new file mode 100644
index 000000000..20ae73c0e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/wb/wb_1.0/ia_css_wb_types.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_WB_TYPES_H
+#define __IA_CSS_WB_TYPES_H
+
+/* @file
+* CSS-API header file for White Balance parameters.
+*/
+
+/* White Balance configuration (Gain Adjust).
+ *
+ *  ISP block: WB1
+ *  ISP1: WB1 is used.
+ *  ISP2: WB1 is used.
+ */
+struct ia_css_wb_config {
+	u32 integer_bits; /** Common exponent of gains.
+				u8.0, [0,3],
+				default 1, ineffective 1 */
+	u32 gr;	/** Significand of Gr gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	u32 r;	/** Significand of R gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	u32 b;	/** Significand of B gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	u32 gb;	/** Significand of Gb gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+};
+
+#endif /* __IA_CSS_WB_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c
new file mode 100644
index 000000000..1cd596608
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c
@@ -0,0 +1,66 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+
+#include "ia_css_xnr.host.h"
+
+const struct ia_css_xnr_config default_xnr_config = {
+	/* default threshold 6400 translates to 25 on ISP. */
+	6400
+};
+
+void
+ia_css_xnr_table_vamem_encode(
+    struct sh_css_isp_xnr_vamem_params *to,
+    const struct ia_css_xnr_table *from,
+    unsigned int size)
+{
+	(void)size;
+	memcpy(&to->xnr,  &from->data, sizeof(to->xnr));
+}
+
+void
+ia_css_xnr_encode(
+    struct sh_css_isp_xnr_params *to,
+    const struct ia_css_xnr_config *from,
+    unsigned int size)
+{
+	(void)size;
+
+	to->threshold =
+	    (uint16_t)uDIGIT_FITTING(from->threshold, 16, SH_CSS_ISP_YUV_BITS);
+}
+
+void
+ia_css_xnr_table_debug_dtrace(
+    const struct ia_css_xnr_table *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
+
+void
+ia_css_xnr_debug_dtrace(
+    const struct ia_css_xnr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.threshold=%d\n", config->threshold);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h
new file mode 100644
index 000000000..686101c0b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR_HOST_H
+#define __IA_CSS_XNR_HOST_H
+
+#include "sh_css_params.h"
+
+#include "ia_css_xnr_param.h"
+#include "ia_css_xnr_table.host.h"
+
+extern const struct ia_css_xnr_config default_xnr_config;
+
+void
+ia_css_xnr_table_vamem_encode(
+    struct sh_css_isp_xnr_vamem_params *to,
+    const struct ia_css_xnr_table *from,
+    unsigned int size);
+
+void
+ia_css_xnr_encode(
+    struct sh_css_isp_xnr_params *to,
+    const struct ia_css_xnr_config *from,
+    unsigned int size);
+
+void
+ia_css_xnr_table_debug_dtrace(
+    const struct ia_css_xnr_table *s3a,
+    unsigned int level);
+
+void
+ia_css_xnr_debug_dtrace(
+    const struct ia_css_xnr_config *config,
+    unsigned int level);
+
+#endif /* __IA_CSS_XNR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h
new file mode 100644
index 000000000..93754f7c7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR_PARAM_H
+#define __IA_CSS_XNR_PARAM_H
+
+#include "type_support.h"
+#include <system_global.h>
+
+#ifndef PIPE_GENERATION
+#define SH_CSS_ISP_XNR_TABLE_SIZE_LOG2       IA_CSS_VAMEM_2_XNR_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_XNR_TABLE_SIZE            IA_CSS_VAMEM_2_XNR_TABLE_SIZE
+
+#else
+/* For pipe generation, the size is not relevant */
+#define SH_CSS_ISP_XNR_TABLE_SIZE 0
+#endif
+
+/* This should be vamem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_xnr_vamem_params {
+	u16 xnr[SH_CSS_ISP_XNR_TABLE_SIZE];
+};
+
+struct sh_css_isp_xnr_params {
+	/* XNR threshold.
+	 * type:u0.16 but actual valid range is:[0,255]
+	 * valid range is dependent on SH_CSS_ISP_YUV_BITS (currently 8bits)
+	 * default: 25 */
+	u16 threshold;
+};
+
+#endif /* __IA_CSS_XNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c
new file mode 100644
index 000000000..e5c153086
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c
@@ -0,0 +1,52 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+
+#include <type_support.h>
+#include "system_global.h"
+#include "vamem.h"
+#include "ia_css_types.h"
+#include "ia_css_xnr_table.host.h"
+
+struct ia_css_xnr_table default_xnr_table;
+
+
+static const uint16_t
+default_xnr_table_data[IA_CSS_VAMEM_2_XNR_TABLE_SIZE] = {
+	/* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 */
+	8191 >> 1, 4096 >> 1, 2730 >> 1, 2048 >> 1, 1638 >> 1, 1365 >> 1, 1170 >> 1, 1024 >> 1, 910 >> 1, 819 >> 1, 744 >> 1, 682 >> 1, 630 >> 1, 585 >> 1,
+	     546 >> 1, 512 >> 1,
+
+	     /* 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 */
+	     481 >> 1, 455 >> 1, 431 >> 1, 409 >> 1, 390 >> 1, 372 >> 1, 356 >> 1, 341 >> 1, 327 >> 1, 315 >> 1, 303 >> 1, 292 >> 1, 282 >> 1, 273 >> 1, 264 >> 1,
+	     256 >> 1,
+
+	     /* 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 */
+	     248 >> 1, 240 >> 1, 234 >> 1, 227 >> 1, 221 >> 1, 215 >> 1, 210 >> 1, 204 >> 1, 199 >> 1, 195 >> 1, 190 >> 1, 186 >> 1, 182 >> 1, 178 >> 1, 174 >> 1,
+	     170 >> 1,
+
+	     /* 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 */
+	     167 >> 1, 163 >> 1, 160 >> 1, 157 >> 1, 154 >> 1, 151 >> 1, 148 >> 1, 146 >> 1, 143 >> 1, 141 >> 1, 138 >> 1, 136 >> 1, 134 >> 1, 132 >> 1, 130 >> 1, 128 >> 1
+};
+
+
+void
+ia_css_config_xnr_table(void)
+{
+	memcpy(default_xnr_table.data.vamem_2, default_xnr_table_data,
+	       sizeof(default_xnr_table_data));
+	default_xnr_table.vamem_type     = IA_CSS_VAMEM_TYPE_2;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h
new file mode 100644
index 000000000..2f4ab8ad4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR_TABLE_HOST_H
+#define __IA_CSS_XNR_TABLE_HOST_H
+
+extern struct ia_css_xnr_table default_xnr_table;
+
+void ia_css_config_xnr_table(void);
+
+#endif /* __IA_CSS_XNR_TABLE_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h
new file mode 100644
index 000000000..9a4d2e470
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR_TYPES_H
+#define __IA_CSS_XNR_TYPES_H
+
+/* @file
+* CSS-API header file for Extra Noise Reduction (XNR) parameters.
+*/
+
+/* XNR table.
+ *
+ *  NOTE: The driver does not need to set this table,
+ *        because the default values are set inside the css.
+ *
+ *  This table contains coefficients used for division in XNR.
+ *
+ *	u0.12, [0,4095],
+ *      {4095, 2048, 1365, .........., 65, 64}
+ *      ({1/1, 1/2, 1/3, ............., 1/63, 1/64})
+ *
+ *  ISP block: XNR1
+ *  ISP1: XNR1 is used.
+ *  ISP2: XNR1 is used.
+ *
+ */
+
+/* Number of elements in the xnr table. */
+#define IA_CSS_VAMEM_1_XNR_TABLE_SIZE_LOG2      6
+/* Number of elements in the xnr table. */
+#define IA_CSS_VAMEM_1_XNR_TABLE_SIZE           BIT(IA_CSS_VAMEM_1_XNR_TABLE_SIZE_LOG2)
+
+/* Number of elements in the xnr table. */
+#define IA_CSS_VAMEM_2_XNR_TABLE_SIZE_LOG2      6
+/* Number of elements in the xnr table. */
+#define IA_CSS_VAMEM_2_XNR_TABLE_SIZE		BIT(IA_CSS_VAMEM_2_XNR_TABLE_SIZE_LOG2)
+
+/** IA_CSS_VAMEM_TYPE_1(ISP2300) or
+     IA_CSS_VAMEM_TYPE_2(ISP2400) */
+union ia_css_xnr_data {
+	u16 vamem_1[IA_CSS_VAMEM_1_XNR_TABLE_SIZE];
+	/** Coefficients table on vamem type1. u0.12, [0,4095] */
+	u16 vamem_2[IA_CSS_VAMEM_2_XNR_TABLE_SIZE];
+	/** Coefficients table on vamem type2. u0.12, [0,4095] */
+};
+
+struct ia_css_xnr_table {
+	enum ia_css_vamem_type vamem_type;
+	union ia_css_xnr_data data;
+};
+
+struct ia_css_xnr_config {
+	/* XNR threshold.
+	 * type:u0.16 valid range:[0,65535]
+	 * default: 6400 */
+	u16 threshold;
+};
+
+#endif /* __IA_CSS_XNR_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c
new file mode 100644
index 000000000..9c9d9b9a4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c
@@ -0,0 +1,249 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "type_support.h"
+#include "math_support.h"
+#include "sh_css_defs.h"
+#include "ia_css_types.h"
+#include "assert_support.h"
+#include "ia_css_xnr3.host.h"
+
+/* Maximum value for alpha on ISP interface */
+#define XNR_MAX_ALPHA  ((1 << (ISP_VEC_ELEMBITS - 1)) - 1)
+
+/* Minimum value for sigma on host interface. Lower values translate to
+ * max_alpha.
+ */
+#define XNR_MIN_SIGMA  (IA_CSS_XNR3_SIGMA_SCALE / 100)
+
+/*
+ * division look-up table
+ * Refers to XNR3.0.5
+ */
+#define XNR3_LOOK_UP_TABLE_POINTS 16
+
+static const s16 x[XNR3_LOOK_UP_TABLE_POINTS] = {
+	1024, 1164, 1320, 1492, 1680, 1884, 2108, 2352,
+	2616, 2900, 3208, 3540, 3896, 4276, 4684, 5120
+};
+
+static const s16 a[XNR3_LOOK_UP_TABLE_POINTS] = {
+	-7213, -5580, -4371, -3421, -2722, -2159, -6950, -5585,
+	    -4529, -3697, -3010, -2485, -2070, -1727, -1428, 0
+    };
+
+static const s16 b[XNR3_LOOK_UP_TABLE_POINTS] = {
+	4096, 3603, 3178, 2811, 2497, 2226, 1990, 1783,
+	1603, 1446, 1307, 1185, 1077, 981, 895, 819
+};
+
+static const s16 c[XNR3_LOOK_UP_TABLE_POINTS] = {
+	1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/*
+ * Default kernel parameters. In general, default is bypass mode or as close
+ * to the ineffective values as possible. Due to the chroma down+upsampling,
+ * perfect bypass mode is not possible for xnr3 filter itself. Instead, the
+ * 'blending' parameter is used to create a bypass.
+ */
+const struct ia_css_xnr3_config default_xnr3_config = {
+	/* sigma */
+	{ 0, 0, 0, 0, 0, 0 },
+	/* coring */
+	{ 0, 0, 0, 0 },
+	/* blending */
+	{ 0 }
+};
+
+/*
+ * Compute an alpha value for the ISP kernel from sigma value on the host
+ * parameter interface as: alpha_scale * 1/(sigma/sigma_scale)
+ */
+static int32_t
+compute_alpha(int sigma)
+{
+	s32 alpha;
+	int offset = sigma / 2;
+
+	if (sigma < XNR_MIN_SIGMA) {
+		alpha = XNR_MAX_ALPHA;
+	} else {
+		alpha = ((IA_CSS_XNR3_SIGMA_SCALE * XNR_ALPHA_SCALE_FACTOR) + offset) / sigma;
+
+		if (alpha > XNR_MAX_ALPHA)
+			alpha = XNR_MAX_ALPHA;
+	}
+
+	return alpha;
+}
+
+/*
+ * Compute the scaled coring value for the ISP kernel from the value on the
+ * host parameter interface.
+ */
+static int32_t
+compute_coring(int coring)
+{
+	s32 isp_coring;
+	s32 isp_scale = XNR_CORING_SCALE_FACTOR;
+	s32 host_scale = IA_CSS_XNR3_CORING_SCALE;
+	s32 offset = host_scale / 2; /* fixed-point 0.5 */
+
+	/* Convert from public host-side scale factor to isp-side scale
+	 * factor. Clip to [0, isp_scale-1).
+	 */
+	isp_coring = ((coring * isp_scale) + offset) / host_scale;
+	return min(max(isp_coring, 0), isp_scale - 1);
+}
+
+/*
+ * Compute the scaled blending strength for the ISP kernel from the value on
+ * the host parameter interface.
+ */
+static int32_t
+compute_blending(int strength)
+{
+	s32 isp_strength;
+	s32 isp_scale = XNR_BLENDING_SCALE_FACTOR;
+	s32 host_scale = IA_CSS_XNR3_BLENDING_SCALE;
+	s32 offset = host_scale / 2; /* fixed-point 0.5 */
+
+	/* Convert from public host-side scale factor to isp-side scale
+	 * factor. The blending factor is positive on the host side, but
+	 * negative on the ISP side because +1.0 cannot be represented
+	 * exactly as s0.11 fixed point, but -1.0 can.
+	 */
+	isp_strength = -(((strength * isp_scale) + offset) / host_scale);
+	return MAX(MIN(isp_strength, 0), -isp_scale);
+}
+
+void
+ia_css_xnr3_encode(
+    struct sh_css_isp_xnr3_params *to,
+    const struct ia_css_xnr3_config *from,
+    unsigned int size)
+{
+	int kernel_size = XNR_FILTER_SIZE;
+	/* The adjust factor is the next power of 2
+	   w.r.t. the kernel size*/
+	int adjust_factor = ceil_pow2(kernel_size);
+	s32 max_diff = (1 << (ISP_VEC_ELEMBITS - 1)) - 1;
+	s32 min_diff = -(1 << (ISP_VEC_ELEMBITS - 1));
+
+	s32 alpha_y0 = compute_alpha(from->sigma.y0);
+	s32 alpha_y1 = compute_alpha(from->sigma.y1);
+	s32 alpha_u0 = compute_alpha(from->sigma.u0);
+	s32 alpha_u1 = compute_alpha(from->sigma.u1);
+	s32 alpha_v0 = compute_alpha(from->sigma.v0);
+	s32 alpha_v1 = compute_alpha(from->sigma.v1);
+	s32 alpha_ydiff = (alpha_y1 - alpha_y0) * adjust_factor / kernel_size;
+	s32 alpha_udiff = (alpha_u1 - alpha_u0) * adjust_factor / kernel_size;
+	s32 alpha_vdiff = (alpha_v1 - alpha_v0) * adjust_factor / kernel_size;
+
+	s32 coring_u0 = compute_coring(from->coring.u0);
+	s32 coring_u1 = compute_coring(from->coring.u1);
+	s32 coring_v0 = compute_coring(from->coring.v0);
+	s32 coring_v1 = compute_coring(from->coring.v1);
+	s32 coring_udiff = (coring_u1 - coring_u0) * adjust_factor / kernel_size;
+	s32 coring_vdiff = (coring_v1 - coring_v0) * adjust_factor / kernel_size;
+
+	s32 blending = compute_blending(from->blending.strength);
+
+	(void)size;
+
+	/* alpha's are represented in qN.5 format */
+	to->alpha.y0 = alpha_y0;
+	to->alpha.u0 = alpha_u0;
+	to->alpha.v0 = alpha_v0;
+	to->alpha.ydiff = min(max(alpha_ydiff, min_diff), max_diff);
+	to->alpha.udiff = min(max(alpha_udiff, min_diff), max_diff);
+	to->alpha.vdiff = min(max(alpha_vdiff, min_diff), max_diff);
+
+	/* coring parameters are expressed in q1.NN format */
+	to->coring.u0 = coring_u0;
+	to->coring.v0 = coring_v0;
+	to->coring.udiff = min(max(coring_udiff, min_diff), max_diff);
+	to->coring.vdiff = min(max(coring_vdiff, min_diff), max_diff);
+
+	/* blending strength is expressed in q1.NN format */
+	to->blending.strength = blending;
+}
+
+/* ISP2401 */
+/* (void) = ia_css_xnr3_vmem_encode(*to, *from)
+ * -----------------------------------------------
+ * VMEM Encode Function to translate UV parameters from userspace into ISP space
+*/
+void
+ia_css_xnr3_vmem_encode(
+    struct sh_css_isp_xnr3_vmem_params *to,
+    const struct ia_css_xnr3_config *from,
+    unsigned int size)
+{
+	unsigned int i, j, base;
+	const unsigned int total_blocks = 4;
+	const unsigned int shuffle_block = 16;
+
+	(void)from;
+	(void)size;
+
+	/* Init */
+	for (i = 0; i < ISP_VEC_NELEMS; i++) {
+		to->x[0][i] = 0;
+		to->a[0][i] = 0;
+		to->b[0][i] = 0;
+		to->c[0][i] = 0;
+	}
+
+	/* Constraints on "x":
+	 * - values should be greater or equal to 0.
+	 * - values should be ascending.
+	 */
+	assert(x[0] >= 0);
+
+	for (j = 1; j < XNR3_LOOK_UP_TABLE_POINTS; j++) {
+		assert(x[j] >= 0);
+		assert(x[j] > x[j - 1]);
+	}
+
+	/* The implementation of the calulating 1/x is based on the availability
+	 * of the OP_vec_shuffle16 operation.
+	 * A 64 element vector is split up in 4 blocks of 16 element. Each array is copied to
+	 * a vector 4 times, (starting at 0, 16, 32 and 48). All array elements are copied or
+	 * initialised as described in the KFS. The remaining elements of a vector are set to 0.
+	 */
+	/* TODO: guard this code with above assumptions */
+	for (i = 0; i < total_blocks; i++) {
+		base = shuffle_block * i;
+
+		for (j = 0; j < XNR3_LOOK_UP_TABLE_POINTS; j++) {
+			to->x[0][base + j] = x[j];
+			to->a[0][base + j] = a[j];
+			to->b[0][base + j] = b[j];
+			to->c[0][base + j] = c[j];
+		}
+	}
+}
+
+/* Dummy Function added as the tool expects it*/
+void
+ia_css_xnr3_debug_dtrace(
+    const struct ia_css_xnr3_config *config,
+    unsigned int level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h
new file mode 100644
index 000000000..6b57990b7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR3_HOST_H
+#define __IA_CSS_XNR3_HOST_H
+
+#include "ia_css_xnr3_param.h"
+#include "ia_css_xnr3_types.h"
+
+extern const struct ia_css_xnr3_config default_xnr3_config;
+
+void
+ia_css_xnr3_encode(
+    struct sh_css_isp_xnr3_params *to,
+    const struct ia_css_xnr3_config *from,
+    unsigned int size);
+
+/* ISP2401 */
+void
+ia_css_xnr3_vmem_encode(
+    struct sh_css_isp_xnr3_vmem_params *to,
+    const struct ia_css_xnr3_config *from,
+    unsigned int size);
+
+void
+ia_css_xnr3_debug_dtrace(
+    const struct ia_css_xnr3_config *config,
+    unsigned int level);
+
+#endif /* __IA_CSS_XNR3_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h
new file mode 100644
index 000000000..c728db7ce
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h
@@ -0,0 +1,84 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR3_PARAM_H
+#define __IA_CSS_XNR3_PARAM_H
+
+#include "type_support.h"
+#include "vmem.h" /* ISP2401: needed for VMEM_ARRAY */
+
+/* Scaling factor of the alpha values: which fixed-point value represents 1.0?
+ * It must be chosen such that 1/min_sigma still fits in an ISP vector
+ * element. */
+#define XNR_ALPHA_SCALE_LOG2        5
+#define XNR_ALPHA_SCALE_FACTOR      BIT(XNR_ALPHA_SCALE_LOG2)
+
+/* Scaling factor of the coring values on the ISP. */
+#define XNR_CORING_SCALE_LOG2       (ISP_VEC_ELEMBITS - 1)
+#define XNR_CORING_SCALE_FACTOR     BIT(XNR_CORING_SCALE_LOG2)
+
+/* Scaling factor of the blending strength on the ISP. */
+#define XNR_BLENDING_SCALE_LOG2     (ISP_VEC_ELEMBITS - 1)
+#define XNR_BLENDING_SCALE_FACTOR   BIT(XNR_BLENDING_SCALE_LOG2)
+
+/* XNR3 filter size. Must be 11x11, 9x9 or 5x5. */
+#define XNR_FILTER_SIZE             5
+
+/* XNR3 alpha (1/sigma) parameters on the ISP, expressed as a base (0) value
+ * for dark areas, and a scaled diff towards the value for bright areas. */
+struct sh_css_xnr3_alpha_params {
+	s32 y0;
+	s32 u0;
+	s32 v0;
+	s32 ydiff;
+	s32 udiff;
+	s32 vdiff;
+};
+
+/* XNR3 coring parameters on the ISP, expressed as a base (0) value
+ * for dark areas, and a scaled diff towards the value for bright areas. */
+struct sh_css_xnr3_coring_params {
+	s32 u0;
+	s32 v0;
+	s32 udiff;
+	s32 vdiff;
+};
+
+/* XNR3 blending strength on the ISP. */
+struct sh_css_xnr3_blending_params {
+	s32 strength;
+};
+
+/* XNR3 ISP parameters */
+struct sh_css_isp_xnr3_params {
+	struct sh_css_xnr3_alpha_params    alpha;
+	struct sh_css_xnr3_coring_params   coring;
+	struct sh_css_xnr3_blending_params blending;
+};
+
+/* ISP2401 */
+/*
+ * STRUCT sh_css_isp_xnr3_vmem_params
+ * -----------------------------------------------
+ * ISP VMEM parameters
+ */
+struct sh_css_isp_xnr3_vmem_params {
+	VMEM_ARRAY(x, ISP_VEC_NELEMS);
+	VMEM_ARRAY(a, ISP_VEC_NELEMS);
+	VMEM_ARRAY(b, ISP_VEC_NELEMS);
+	VMEM_ARRAY(c, ISP_VEC_NELEMS);
+};
+
+#endif  /*__IA_CSS_XNR3_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h
new file mode 100644
index 000000000..4447ba31a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_XNR3_TYPES_H
+#define __IA_CSS_XNR3_TYPES_H
+
+/* @file
+* CSS-API header file for Extra Noise Reduction (XNR) parameters.
+*/
+
+/**
+ * \brief Scale of the XNR sigma parameters.
+ * \details The define specifies which fixed-point value represents 1.0.
+ */
+#define IA_CSS_XNR3_SIGMA_SCALE  BIT(10)
+
+/**
+ * \brief Scale of the XNR coring parameters.
+ * \details The define specifies which fixed-point value represents 1.0.
+ */
+#define IA_CSS_XNR3_CORING_SCALE BIT(15)
+
+/**
+ * \brief Scale of the XNR blending parameter.
+ * \details The define specifies which fixed-point value represents 1.0.
+ */
+#define IA_CSS_XNR3_BLENDING_SCALE BIT(11)
+
+/**
+ * \brief XNR3 Sigma Parameters.
+ * \details Sigma parameters define the strength of the XNR filter.
+ * A higher number means stronger filtering. There are two values for each of
+ * the three YUV planes: one for dark areas and one for bright areas. All
+ * sigma parameters are fixed-point values between 0.0 and 1.0, scaled with
+ * IA_CSS_XNR3_SIGMA_SCALE.
+ */
+struct ia_css_xnr3_sigma_params {
+	int y0;     /** Sigma for Y range similarity in dark area */
+	int y1;     /** Sigma for Y range similarity in bright area */
+	int u0;     /** Sigma for U range similarity in dark area */
+	int u1;     /** Sigma for U range similarity in bright area */
+	int v0;     /** Sigma for V range similarity in dark area */
+	int v1;     /** Sigma for V range similarity in bright area */
+};
+
+/**
+ * \brief XNR3 Coring Parameters
+ * \details Coring parameters define the "coring" strength, which is a soft
+ * thresholding technique to avoid false coloring. There are two values for
+ * each of the two chroma planes: one for dark areas and one for bright areas.
+ * All coring parameters are fixed-point values between 0.0 and 1.0, scaled
+ * with IA_CSS_XNR3_CORING_SCALE. The ineffective value is 0.
+ */
+struct ia_css_xnr3_coring_params {
+	int u0;     /** Coring threshold of U channel in dark area */
+	int u1;     /** Coring threshold of U channel in bright area */
+	int v0;     /** Coring threshold of V channel in dark area */
+	int v1;     /** Coring threshold of V channel in bright area */
+};
+
+/**
+ * \brief XNR3 Blending Parameters
+ * \details Blending parameters define the blending strength of filtered
+ * output pixels with the original chroma pixels from before xnr3. The
+ * blending strength is a fixed-point value between 0.0 and 1.0 (inclusive),
+ * scaled with IA_CSS_XNR3_BLENDING_SCALE.
+ * A higher number applies xnr filtering more strongly. A value of 1.0
+ * disables the blending and returns the xnr3 filtered output, while a
+ * value of 0.0 bypasses the entire xnr3 filter.
+ */
+struct ia_css_xnr3_blending_params {
+	int strength;   /** Blending strength */
+};
+
+/**
+ * \brief XNR3 public parameters.
+ * \details Struct with all parameters for the XNR3 kernel that can be set
+ * from the CSS API.
+ */
+struct ia_css_xnr3_config {
+	struct ia_css_xnr3_sigma_params    sigma;    /** XNR3 sigma parameters */
+	struct ia_css_xnr3_coring_params   coring;   /** XNR3 coring parameters */
+	struct ia_css_xnr3_blending_params blending; /** XNR3 blending parameters */
+};
+
+#endif /* __IA_CSS_XNR3_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c
new file mode 100644
index 000000000..048ffbc90
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c
@@ -0,0 +1,218 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_frac.h"
+
+#include "bnr/bnr_1.0/ia_css_bnr.host.h"
+#include "ia_css_ynr.host.h"
+
+const struct ia_css_nr_config default_nr_config = {
+	16384,
+	8192,
+	1280,
+	0,
+	0
+};
+
+const struct ia_css_ee_config default_ee_config = {
+	8192,
+	128,
+	2048
+};
+
+void
+ia_css_nr_encode(
+    struct sh_css_isp_ynr_params *to,
+    const struct ia_css_nr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	/* YNR (Y Noise Reduction) */
+	to->threshold =
+	    uDIGIT_FITTING(8192U, 16, SH_CSS_BAYER_BITS);
+	to->gain_all =
+	    uDIGIT_FITTING(from->ynr_gain, 16, SH_CSS_YNR_GAIN_SHIFT);
+	to->gain_dir =
+	    uDIGIT_FITTING(from->ynr_gain, 16, SH_CSS_YNR_GAIN_SHIFT);
+	to->threshold_cb =
+	    uDIGIT_FITTING(from->threshold_cb, 16, SH_CSS_BAYER_BITS);
+	to->threshold_cr =
+	    uDIGIT_FITTING(from->threshold_cr, 16, SH_CSS_BAYER_BITS);
+}
+
+void
+ia_css_yee_encode(
+    struct sh_css_isp_yee_params *to,
+    const struct ia_css_yee_config *from,
+    unsigned int size)
+{
+	int asiWk1 = (int)from->ee.gain;
+	int asiWk2 = asiWk1 / 8;
+	int asiWk3 = asiWk1 / 4;
+
+	(void)size;
+	/* YEE (Y Edge Enhancement) */
+	to->dirthreshold_s =
+	    min((uDIGIT_FITTING(from->nr.direction, 16, SH_CSS_BAYER_BITS)
+		 << 1),
+		SH_CSS_BAYER_MAXVAL);
+	to->dirthreshold_g =
+	    min((uDIGIT_FITTING(from->nr.direction, 16, SH_CSS_BAYER_BITS)
+		 << 4),
+		SH_CSS_BAYER_MAXVAL);
+	to->dirthreshold_width_log2 =
+	    uFRACTION_BITS_FITTING(8);
+	to->dirthreshold_width =
+	    1 << to->dirthreshold_width_log2;
+	to->detailgain =
+	    uDIGIT_FITTING(from->ee.detail_gain, 11,
+			   SH_CSS_YEE_DETAIL_GAIN_SHIFT);
+	to->coring_s =
+	    (uDIGIT_FITTING(56U, 16, SH_CSS_BAYER_BITS) *
+	     from->ee.threshold) >> 8;
+	to->coring_g =
+	    (uDIGIT_FITTING(224U, 16, SH_CSS_BAYER_BITS) *
+	     from->ee.threshold) >> 8;
+	/* 8; // *1.125 ->[s4.8] */
+	to->scale_plus_s =
+	    (asiWk1 + asiWk2) >> (11 - SH_CSS_YEE_SCALE_SHIFT);
+	/* 8; // ( * -.25)->[s4.8] */
+	to->scale_plus_g =
+	    (0 - asiWk3) >> (11 - SH_CSS_YEE_SCALE_SHIFT);
+	/* 8; // *0.875 ->[s4.8] */
+	to->scale_minus_s =
+	    (asiWk1 - asiWk2) >> (11 - SH_CSS_YEE_SCALE_SHIFT);
+	/* 8; // ( *.25 ) ->[s4.8] */
+	to->scale_minus_g =
+	    (asiWk3) >> (11 - SH_CSS_YEE_SCALE_SHIFT);
+	to->clip_plus_s =
+	    uDIGIT_FITTING(32760U, 16, SH_CSS_BAYER_BITS);
+	to->clip_plus_g = 0;
+	to->clip_minus_s =
+	    uDIGIT_FITTING(504U, 16, SH_CSS_BAYER_BITS);
+	to->clip_minus_g =
+	    uDIGIT_FITTING(32256U, 16, SH_CSS_BAYER_BITS);
+	to->Yclip = SH_CSS_BAYER_MAXVAL;
+}
+
+void
+ia_css_nr_dump(
+    const struct sh_css_isp_ynr_params *ynr,
+    unsigned int level)
+{
+	if (!ynr) return;
+	ia_css_debug_dtrace(level,
+			    "Y Noise Reduction:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynr_threshold", ynr->threshold);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynr_gain_all", ynr->gain_all);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynr_gain_dir", ynr->gain_dir);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynr_threshold_cb", ynr->threshold_cb);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynr_threshold_cr", ynr->threshold_cr);
+}
+
+void
+ia_css_yee_dump(
+    const struct sh_css_isp_yee_params *yee,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "Y Edge Enhancement:\n");
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynryee_dirthreshold_s",
+			    yee->dirthreshold_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynryee_dirthreshold_g",
+			    yee->dirthreshold_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynryee_dirthreshold_width_log2",
+			    yee->dirthreshold_width_log2);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynryee_dirthreshold_width",
+			    yee->dirthreshold_width);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_detailgain",
+			    yee->detailgain);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_coring_s",
+			    yee->coring_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_coring_g",
+			    yee->coring_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_scale_plus_s",
+			    yee->scale_plus_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_scale_plus_g",
+			    yee->scale_plus_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_scale_minus_s",
+			    yee->scale_minus_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_scale_minus_g",
+			    yee->scale_minus_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_clip_plus_s",
+			    yee->clip_plus_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_clip_plus_g",
+			    yee->clip_plus_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_clip_minus_s",
+			    yee->clip_minus_s);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "yee_clip_minus_g",
+			    yee->clip_minus_g);
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
+			    "ynryee_Yclip",
+			    yee->Yclip);
+}
+
+void
+ia_css_nr_debug_dtrace(
+    const struct ia_css_nr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.direction=%d, config.bnr_gain=%d, config.ynr_gain=%d, config.threshold_cb=%d, config.threshold_cr=%d\n",
+			    config->direction,
+			    config->bnr_gain, config->ynr_gain,
+			    config->threshold_cb, config->threshold_cr);
+}
+
+void
+ia_css_ee_debug_dtrace(
+    const struct ia_css_ee_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.threshold=%d, config.gain=%d, config.detail_gain=%d\n",
+			    config->threshold, config->gain, config->detail_gain);
+}
+
+void
+ia_css_init_ynr_state(
+    void/*struct sh_css_isp_ynr_vmem_state*/ * state,
+    size_t size)
+{
+	memset(state, 0, size);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h
new file mode 100644
index 000000000..049706e1f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR_HOST_H
+#define __IA_CSS_YNR_HOST_H
+
+#include "ia_css_ynr_types.h"
+#include "ia_css_ynr_param.h"
+
+extern const struct ia_css_nr_config default_nr_config;
+extern const struct ia_css_ee_config default_ee_config;
+
+void
+ia_css_nr_encode(
+    struct sh_css_isp_ynr_params *to,
+    const struct ia_css_nr_config *from,
+    unsigned int size);
+
+void
+ia_css_yee_encode(
+    struct sh_css_isp_yee_params *to,
+    const struct ia_css_yee_config *from,
+    unsigned int size);
+
+void
+ia_css_nr_dump(
+    const struct sh_css_isp_ynr_params *ynr,
+    unsigned int level);
+
+void
+ia_css_yee_dump(
+    const struct sh_css_isp_yee_params *yee,
+    unsigned int level);
+
+void
+ia_css_nr_debug_dtrace(
+    const struct ia_css_nr_config *config,
+    unsigned int level);
+
+void
+ia_css_ee_debug_dtrace(
+    const struct ia_css_ee_config *config,
+    unsigned int level);
+
+void
+ia_css_init_ynr_state(
+    void/*struct sh_css_isp_ynr_vmem_state*/ * state,
+    size_t size);
+#endif /* __IA_CSS_YNR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h
new file mode 100644
index 000000000..8d9069ec2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h
@@ -0,0 +1,50 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR_PARAM_H
+#define __IA_CSS_YNR_PARAM_H
+
+#include "type_support.h"
+
+/* YNR (Y Noise Reduction) */
+struct sh_css_isp_ynr_params {
+	s32 threshold;
+	s32 gain_all;
+	s32 gain_dir;
+	s32 threshold_cb;
+	s32 threshold_cr;
+};
+
+/* YEE (Y Edge Enhancement) */
+struct sh_css_isp_yee_params {
+	s32 dirthreshold_s;
+	s32 dirthreshold_g;
+	s32 dirthreshold_width_log2;
+	s32 dirthreshold_width;
+	s32 detailgain;
+	s32 coring_s;
+	s32 coring_g;
+	s32 scale_plus_s;
+	s32 scale_plus_g;
+	s32 scale_minus_s;
+	s32 scale_minus_g;
+	s32 clip_plus_s;
+	s32 clip_plus_g;
+	s32 clip_minus_s;
+	s32 clip_minus_g;
+	s32 Yclip;
+};
+
+#endif /* __IA_CSS_YNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h
new file mode 100644
index 000000000..da1ba21a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h
@@ -0,0 +1,81 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR_TYPES_H
+#define __IA_CSS_YNR_TYPES_H
+
+/* @file
+* CSS-API header file for Noise Reduction (BNR) and YCC Noise Reduction (YNR,CNR).
+*/
+
+/* Configuration used by Bayer Noise Reduction (BNR) and
+ *  YCC Noise Reduction (YNR,CNR).
+ *
+ *  ISP block: BNR1, YNR1, CNR1
+ *  ISP1: BNR1,YNR1,CNR1 are used.
+ *  ISP2: BNR1,YNR1,CNR1 are used for Preview/Video.
+ *        BNR1,YNR2,CNR2 are used for Still.
+ */
+struct ia_css_nr_config {
+	ia_css_u0_16 bnr_gain;	   /** Strength of noise reduction (BNR).
+				u0.16, [0,65535],
+				default 14336(0.21875), ineffective 0 */
+	ia_css_u0_16 ynr_gain;	   /** Strength of noise reduction (YNR).
+				u0.16, [0,65535],
+				default 14336(0.21875), ineffective 0 */
+	ia_css_u0_16 direction;    /** Sensitivity of edge (BNR).
+				u0.16, [0,65535],
+				default 512(0.0078125), ineffective 0 */
+	ia_css_u0_16 threshold_cb; /** Coring threshold for Cb (CNR).
+				This is the same as
+				de_config.c1_coring_threshold.
+				u0.16, [0,65535],
+				default 0(0), ineffective 0 */
+	ia_css_u0_16 threshold_cr; /** Coring threshold for Cr (CNR).
+				This is the same as
+				de_config.c2_coring_threshold.
+				u0.16, [0,65535],
+				default 0(0), ineffective 0 */
+};
+
+/* Edge Enhancement (sharpen) configuration.
+ *
+ *  ISP block: YEE1
+ *  ISP1: YEE1 is used.
+ *  ISP2: YEE1 is used for Preview/Video.
+ *       (YEE2 is used for Still.)
+ */
+struct ia_css_ee_config {
+	ia_css_u5_11 gain;	  /** The strength of sharpness.
+					u5.11, [0,65535],
+					default 8192(4.0), ineffective 0 */
+	ia_css_u8_8 threshold;    /** The threshold that divides noises from
+					edge.
+					u8.8, [0,65535],
+					default 256(1.0), ineffective 65535 */
+	ia_css_u5_11 detail_gain; /** The strength of sharpness in pell-mell
+					area.
+					u5.11, [0,65535],
+					default 2048(1.0), ineffective 0 */
+};
+
+/* YNR and YEE (sharpen) configuration.
+ */
+struct ia_css_yee_config {
+	struct ia_css_nr_config nr; /** The NR configuration. */
+	struct ia_css_ee_config ee; /** The EE configuration. */
+};
+
+#endif /* __IA_CSS_YNR_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c
new file mode 100644
index 000000000..08e9d72c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "assert_support.h"
+
+#include "ia_css_ynr2.host.h"
+
+const struct ia_css_ynr_config default_ynr_config = {
+	0,
+	0,
+	0,
+	0,
+};
+
+const struct ia_css_fc_config default_fc_config = {
+	1,
+	0,		/* 0 -> ineffective */
+	0,		/* 0 -> ineffective */
+	0,		/* 0 -> ineffective */
+	0,		/* 0 -> ineffective */
+	(1 << (ISP_VEC_ELEMBITS - 2)),		/* 0.5 */
+	(1 << (ISP_VEC_ELEMBITS - 2)),		/* 0.5 */
+	(1 << (ISP_VEC_ELEMBITS - 2)),		/* 0.5 */
+	(1 << (ISP_VEC_ELEMBITS - 2)),		/* 0.5 */
+	(1 << (ISP_VEC_ELEMBITS - 1)) - 1,	/* 1 */
+	(1 << (ISP_VEC_ELEMBITS - 1)) - 1,	/* 1 */
+	(int16_t)-(1 << (ISP_VEC_ELEMBITS - 1)),	/* -1 */
+	(int16_t)-(1 << (ISP_VEC_ELEMBITS - 1)),	/* -1 */
+};
+
+void
+ia_css_ynr_encode(
+    struct sh_css_isp_yee2_params *to,
+    const struct ia_css_ynr_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->edge_sense_gain_0   = from->edge_sense_gain_0;
+	to->edge_sense_gain_1   = from->edge_sense_gain_1;
+	to->corner_sense_gain_0 = from->corner_sense_gain_0;
+	to->corner_sense_gain_1 = from->corner_sense_gain_1;
+}
+
+void
+ia_css_fc_encode(
+    struct sh_css_isp_fc_params *to,
+    const struct ia_css_fc_config *from,
+    unsigned int size)
+{
+	(void)size;
+	to->gain_exp   = from->gain_exp;
+
+	to->coring_pos_0 = from->coring_pos_0;
+	to->coring_pos_1 = from->coring_pos_1;
+	to->coring_neg_0 = from->coring_neg_0;
+	to->coring_neg_1 = from->coring_neg_1;
+
+	to->gain_pos_0 = from->gain_pos_0;
+	to->gain_pos_1 = from->gain_pos_1;
+	to->gain_neg_0 = from->gain_neg_0;
+	to->gain_neg_1 = from->gain_neg_1;
+
+	to->crop_pos_0 = from->crop_pos_0;
+	to->crop_pos_1 = from->crop_pos_1;
+	to->crop_neg_0 = from->crop_neg_0;
+	to->crop_neg_1 = from->crop_neg_1;
+}
+
+void
+ia_css_ynr_dump(
+    const struct sh_css_isp_yee2_params *yee2,
+    unsigned int level);
+
+void
+ia_css_fc_dump(
+    const struct sh_css_isp_fc_params *fc,
+    unsigned int level);
+
+void
+ia_css_fc_debug_dtrace(
+    const struct ia_css_fc_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.gain_exp=%d, config.coring_pos_0=%d, config.coring_pos_1=%d, config.coring_neg_0=%d, config.coring_neg_1=%d, config.gain_pos_0=%d, config.gain_pos_1=%d, config.gain_neg_0=%d, config.gain_neg_1=%d, config.crop_pos_0=%d, config.crop_pos_1=%d, config.crop_neg_0=%d, config.crop_neg_1=%d\n",
+			    config->gain_exp,
+			    config->coring_pos_0, config->coring_pos_1,
+			    config->coring_neg_0, config->coring_neg_1,
+			    config->gain_pos_0, config->gain_pos_1,
+			    config->gain_neg_0, config->gain_neg_1,
+			    config->crop_pos_0, config->crop_pos_1,
+			    config->crop_neg_0, config->crop_neg_1);
+}
+
+void
+ia_css_ynr_debug_dtrace(
+    const struct ia_css_ynr_config *config,
+    unsigned int level)
+{
+	ia_css_debug_dtrace(level,
+			    "config.edge_sense_gain_0=%d, config.edge_sense_gain_1=%d, config.corner_sense_gain_0=%d, config.corner_sense_gain_1=%d\n",
+			    config->edge_sense_gain_0, config->edge_sense_gain_1,
+			    config->corner_sense_gain_0, config->corner_sense_gain_1);
+}
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h
new file mode 100644
index 000000000..eaf253d59
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR2_HOST_H
+#define __IA_CSS_YNR2_HOST_H
+
+#include "ia_css_ynr2_types.h"
+#include "ia_css_ynr2_param.h"
+
+extern const struct ia_css_ynr_config default_ynr_config;
+extern const struct ia_css_fc_config  default_fc_config;
+
+void
+ia_css_ynr_encode(
+    struct sh_css_isp_yee2_params *to,
+    const struct ia_css_ynr_config *from,
+    unsigned int size);
+
+void
+ia_css_fc_encode(
+    struct sh_css_isp_fc_params *to,
+    const struct ia_css_fc_config *from,
+    unsigned int size);
+
+void
+ia_css_ynr_dump(
+    const struct sh_css_isp_yee2_params *yee2,
+    unsigned int level);
+
+void
+ia_css_fc_dump(
+    const struct sh_css_isp_fc_params *fc,
+    unsigned int level);
+
+void
+ia_css_fc_debug_dtrace(
+    const struct ia_css_fc_config *config,
+    unsigned int level);
+
+void
+ia_css_ynr_debug_dtrace(
+    const struct ia_css_ynr_config *config,
+    unsigned int level);
+
+#endif /* __IA_CSS_YNR2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h
new file mode 100644
index 000000000..96c80b3af
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR2_PARAM_H
+#define __IA_CSS_YNR2_PARAM_H
+
+#include "type_support.h"
+
+/* YNR (Y Noise Reduction), YEE (Y Edge Enhancement) */
+struct sh_css_isp_yee2_params {
+	s32 edge_sense_gain_0;
+	s32 edge_sense_gain_1;
+	s32 corner_sense_gain_0;
+	s32 corner_sense_gain_1;
+};
+
+/* Fringe Control */
+struct sh_css_isp_fc_params {
+	s32 gain_exp;
+	u16 coring_pos_0;
+	u16 coring_pos_1;
+	u16 coring_neg_0;
+	u16 coring_neg_1;
+	s32 gain_pos_0;
+	s32 gain_pos_1;
+	s32 gain_neg_0;
+	s32 gain_neg_1;
+	s32 crop_pos_0;
+	s32 crop_pos_1;
+	s32 crop_neg_0;
+	s32 crop_neg_1;
+};
+
+#endif /* __IA_CSS_YNR2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h
new file mode 100644
index 000000000..ab77f4e85
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h
@@ -0,0 +1,94 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_YNR2_TYPES_H
+#define __IA_CSS_YNR2_TYPES_H
+
+/* @file
+* CSS-API header file for Y(Luma) Noise Reduction.
+*/
+
+/* Y(Luma) Noise Reduction configuration.
+ *
+ *  ISP block: YNR2 & YEE2
+ * (ISP1: YNR1 and YEE1 are used.)
+ * (ISP2: YNR1 and YEE1 are used for Preview/Video.)
+ *  ISP2: YNR2 and YEE2 are used for Still.
+ */
+struct ia_css_ynr_config {
+	u16 edge_sense_gain_0;   /** Sensitivity of edge in dark area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	u16 edge_sense_gain_1;   /** Sensitivity of edge in bright area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	u16 corner_sense_gain_0; /** Sensitivity of corner in dark area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	u16 corner_sense_gain_1; /** Sensitivity of corner in bright area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+};
+
+/* Fringe Control configuration.
+ *
+ *  ISP block: FC2 (FC2 is used with YNR2/YEE2.)
+ * (ISP1: FC2 is not used.)
+ * (ISP2: FC2 is not for Preview/Video.)
+ *  ISP2: FC2 is used for Still.
+ */
+struct ia_css_fc_config {
+	u8  gain_exp;   /** Common exponent of gains.
+				u8.0, [0,13],
+				default 1, ineffective 0 */
+	u16 coring_pos_0; /** Coring threshold for positive edge in dark area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	u16 coring_pos_1; /** Coring threshold for positive edge in bright area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	u16 coring_neg_0; /** Coring threshold for negative edge in dark area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	u16 coring_neg_1; /** Coring threshold for negative edge in bright area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	u16 gain_pos_0; /** Gain for positive edge in dark area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	u16 gain_pos_1; /** Gain for positive edge in bright area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	u16 gain_neg_0; /** Gain for negative edge in dark area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	u16 gain_neg_1; /** Gain for negative edge in bright area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	u16 crop_pos_0; /** Limit for positive edge in dark area.
+				u0.13, [0,8191],
+				default/ineffective 8191(almost 1.0) */
+	u16 crop_pos_1; /** Limit for positive edge in bright area.
+				u0.13, [0,8191],
+				default/ineffective 8191(almost 1.0) */
+	s16  crop_neg_0; /** Limit for negative edge in dark area.
+				s0.13, [-8192,0],
+				default/ineffective -8192(-1.0) */
+	s16  crop_neg_1; /** Limit for negative edge in bright area.
+				s0.13, [-8192,0],
+				default/ineffective -8192(-1.0) */
+};
+
+#endif /* __IA_CSS_YNR2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/isp/modes/interface/input_buf.isp.h b/drivers/staging/media/atomisp/pci/isp/modes/interface/input_buf.isp.h
new file mode 100644
index 000000000..f86cf9bf1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/modes/interface/input_buf.isp.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef _INPUT_BUF_ISP_H_
+#define _INPUT_BUF_ISP_H_
+
+/* Temporary include, since IA_CSS_BINARY_MODE_COPY is still needed */
+#include "sh_css_defs.h"
+#include "isp_const.h" /* MAX_VECTORS_PER_INPUT_LINE */
+
+#define INPUT_BUF_HEIGHT	2 /* double buffer */
+#define INPUT_BUF_LINES		2
+
+#ifndef ENABLE_CONTINUOUS
+#define ENABLE_CONTINUOUS 0
+#endif
+
+/* In continuous mode, the input buffer must be a fixed size for all binaries
+ * and at a fixed address since it will be used by the SP. */
+#define EXTRA_INPUT_VECTORS	2 /* For left padding */
+#define MAX_VECTORS_PER_INPUT_LINE_CONT (CEIL_DIV(SH_CSS_MAX_SENSOR_WIDTH, ISP_NWAY) + EXTRA_INPUT_VECTORS)
+
+/* The input buffer should be on a fixed address in vmem, for continuous capture */
+#define INPUT_BUF_ADDR 0x0
+
+#endif /* _INPUT_BUF_ISP_H_ */
diff --git a/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_const.h b/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_const.h
new file mode 100644
index 000000000..73432dc35
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_const.h
@@ -0,0 +1,165 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef _COMMON_ISP_CONST_H_
+#define _COMMON_ISP_CONST_H_
+
+/*#include "isp.h"*/	/* ISP_VEC_NELEMS */
+
+/* Binary independent constants */
+
+#ifndef NO_HOIST
+#  define		NO_HOIST	HIVE_ATTRIBUTE((no_hoist))
+#endif
+
+#define NO_HOIST_CSE HIVE_ATTRIBUTE((no_hoist, no_cse))
+
+#define UNION struct /* Union constructors not allowed in C++ */
+
+#define XMEM_WIDTH_BITS              HIVE_ISP_DDR_WORD_BITS
+#define XMEM_SHORTS_PER_WORD         (HIVE_ISP_DDR_WORD_BITS / 16)
+#define XMEM_INTS_PER_WORD           (HIVE_ISP_DDR_WORD_BITS / 32)
+#define XMEM_POW2_BYTES_PER_WORD      HIVE_ISP_DDR_WORD_BYTES
+
+#define BITS8_ELEMENTS_PER_XMEM_ADDR    CEIL_DIV(XMEM_WIDTH_BITS, 8)
+#define BITS16_ELEMENTS_PER_XMEM_ADDR    CEIL_DIV(XMEM_WIDTH_BITS, 16)
+
+#define ISP_NWAY_LOG2  6
+
+/* *****************************
+ * ISP input/output buffer sizes
+ * ****************************/
+/* input image */
+#define INPUT_BUF_DMA_HEIGHT          2
+#define INPUT_BUF_HEIGHT              2 /* double buffer */
+#define OUTPUT_BUF_DMA_HEIGHT         2
+#define OUTPUT_BUF_HEIGHT             2 /* double buffer */
+#define OUTPUT_NUM_TRANSFERS	      4
+
+/* GDC accelerator: Up/Down Scaling */
+/* These should be moved to the gdc_defs.h in the device */
+#define UDS_SCALING_N                 HRT_GDC_N
+/* AB: This should cover the zooming up to 16MP */
+#define UDS_MAX_OXDIM                 5000
+/* We support maximally 2 planes with different parameters
+       - luma and chroma (YUV420) */
+#define UDS_MAX_PLANES                2
+#define UDS_BLI_BLOCK_HEIGHT          2
+#define UDS_BCI_BLOCK_HEIGHT          4
+#define UDS_BLI_INTERP_ENVELOPE       1
+#define UDS_BCI_INTERP_ENVELOPE       3
+#define UDS_MAX_ZOOM_FAC              64
+/* Make it always one FPGA vector.
+   Four FPGA vectors are required and
+   four of them fit in one ASIC vector.*/
+#define UDS_MAX_CHUNKS                16
+
+#define ISP_LEFT_PADDING	_ISP_LEFT_CROP_EXTRA(ISP_LEFT_CROPPING)
+#define ISP_LEFT_PADDING_VECS	CEIL_DIV(ISP_LEFT_PADDING, ISP_VEC_NELEMS)
+/* in case of continuous the croppong of the current binary doesn't matter for the buffer calculation, but the cropping of the sp copy should be used */
+#define ISP_LEFT_PADDING_CONT	_ISP_LEFT_CROP_EXTRA(SH_CSS_MAX_LEFT_CROPPING)
+#define ISP_LEFT_PADDING_VECS_CONT	CEIL_DIV(ISP_LEFT_PADDING_CONT, ISP_VEC_NELEMS)
+
+#define CEIL_ROUND_DIV_STRIPE(width, stripe, padding) \
+	CEIL_MUL(padding + CEIL_DIV(width - padding, stripe), ((ENABLE_RAW_BINNING || ENABLE_FIXED_BAYER_DS) ? 4 : 2))
+
+/* output (Y,U,V) image, 4:2:0 */
+#define MAX_VECTORS_PER_LINE \
+	CEIL_ROUND_DIV_STRIPE(CEIL_DIV(ISP_MAX_INTERNAL_WIDTH, ISP_VEC_NELEMS), \
+			      ISP_NUM_STRIPES, \
+			      ISP_LEFT_PADDING_VECS)
+
+/*
+ * ITERATOR_VECTOR_INCREMENT' explanation:
+ * when striping an even number of iterations, one of the stripes is
+ * one iteration wider than the other to account for overlap
+ * so the calc for the output buffer vmem size is:
+ * ((width[vectors]/num_of_stripes) + 2[vectors])
+ */
+#define MAX_VECTORS_PER_OUTPUT_LINE \
+	CEIL_DIV(CEIL_DIV(ISP_MAX_OUTPUT_WIDTH, ISP_NUM_STRIPES) + ISP_LEFT_PADDING, ISP_VEC_NELEMS)
+
+/* Must be even due to interlaced bayer input */
+#define MAX_VECTORS_PER_INPUT_LINE	CEIL_MUL((CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS) + ISP_LEFT_PADDING_VECS), 2)
+#define MAX_VECTORS_PER_INPUT_STRIPE	CEIL_ROUND_DIV_STRIPE(MAX_VECTORS_PER_INPUT_LINE, \
+							      ISP_NUM_STRIPES, \
+							      ISP_LEFT_PADDING_VECS)
+
+/* Add 2 for left croppping */
+#define MAX_SP_RAW_COPY_VECTORS_PER_INPUT_LINE	(CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS) + 2)
+
+#define MAX_VECTORS_PER_BUF_LINE \
+	(MAX_VECTORS_PER_LINE + DUMMY_BUF_VECTORS)
+#define MAX_VECTORS_PER_BUF_INPUT_LINE \
+	(MAX_VECTORS_PER_INPUT_STRIPE + DUMMY_BUF_VECTORS)
+#define MAX_OUTPUT_Y_FRAME_WIDTH \
+	(MAX_VECTORS_PER_LINE * ISP_VEC_NELEMS)
+#define MAX_OUTPUT_Y_FRAME_SIMDWIDTH \
+	MAX_VECTORS_PER_LINE
+#define MAX_OUTPUT_C_FRAME_WIDTH \
+	(MAX_OUTPUT_Y_FRAME_WIDTH / 2)
+#define MAX_OUTPUT_C_FRAME_SIMDWIDTH \
+	CEIL_DIV(MAX_OUTPUT_C_FRAME_WIDTH, ISP_VEC_NELEMS)
+
+/* should be even */
+#define NO_CHUNKING (OUTPUT_NUM_CHUNKS == 1)
+
+#define MAX_VECTORS_PER_CHUNK \
+	(NO_CHUNKING ? MAX_VECTORS_PER_LINE \
+				: 2 * CEIL_DIV(MAX_VECTORS_PER_LINE, \
+					     2 * OUTPUT_NUM_CHUNKS))
+
+#define MAX_C_VECTORS_PER_CHUNK \
+	(MAX_VECTORS_PER_CHUNK / 2)
+
+/* should be even */
+#define MAX_VECTORS_PER_OUTPUT_CHUNK \
+	(NO_CHUNKING ? MAX_VECTORS_PER_OUTPUT_LINE \
+				: 2 * CEIL_DIV(MAX_VECTORS_PER_OUTPUT_LINE, \
+					     2 * OUTPUT_NUM_CHUNKS))
+
+#define MAX_C_VECTORS_PER_OUTPUT_CHUNK \
+	(MAX_VECTORS_PER_OUTPUT_CHUNK / 2)
+
+/* should be even */
+#define MAX_VECTORS_PER_INPUT_CHUNK \
+	(INPUT_NUM_CHUNKS == 1 ? MAX_VECTORS_PER_INPUT_STRIPE \
+			       : 2 * CEIL_DIV(MAX_VECTORS_PER_INPUT_STRIPE, \
+					    2 * OUTPUT_NUM_CHUNKS))
+
+#define DEFAULT_C_SUBSAMPLING      2
+
+/****** DMA buffer properties */
+
+#define RAW_BUF_LINES ((ENABLE_RAW_BINNING || ENABLE_FIXED_BAYER_DS) ? 4 : 2)
+
+/* [isp vmem] table size[vectors] per line per color (GR,R,B,GB),
+   multiples of NWAY */
+#define ISP2400_SCTBL_VECTORS_PER_LINE_PER_COLOR \
+	CEIL_DIV(SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR, ISP_VEC_NELEMS)
+#define ISP2401_SCTBL_VECTORS_PER_LINE_PER_COLOR \
+	CEIL_DIV(SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR, ISP_VEC_NELEMS)
+/* [isp vmem] table size[vectors] per line for 4colors (GR,R,B,GB),
+   multiples of NWAY */
+#define SCTBL_VECTORS_PER_LINE \
+	(SCTBL_VECTORS_PER_LINE_PER_COLOR * IA_CSS_SC_NUM_COLORS)
+
+/*************/
+
+/* Format for fixed primaries */
+
+#define ISP_FIXED_PRIMARY_FORMAT IA_CSS_FRAME_FORMAT_NV12
+
+#endif /* _COMMON_ISP_CONST_H_ */
diff --git a/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_types.h b/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_types.h
new file mode 100644
index 000000000..d1c42c77f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp/modes/interface/isp_types.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _ISP_TYPES_H_
+#define _ISP_TYPES_H_
+
+/*
+ * Workaround: hivecc complains about "tag "sh_css_3a_output" already declared"
+ * without this extra decl.
+ */
+struct ia_css_3a_output;
+
+/*
+ * Input stream formats, these correspond to the MIPI formats and the way
+ * the CSS receiver sends these to the input formatter.
+ * The bit depth of each pixel element is stored in the global variable
+ * isp_bits_per_pixel.
+ * NOTE: for rgb565, we set isp_bits_per_pixel to 565, for all other rgb
+ * formats it's the actual depth (4, for 444, 8 for 888 etc).
+ */
+enum sh_stream_format {
+	sh_stream_format_yuv420_legacy,
+	sh_stream_format_yuv420,
+	sh_stream_format_yuv422,
+	sh_stream_format_rgb,
+	sh_stream_format_raw,
+	sh_stream_format_binary,	/* bytestream such as jpeg */
+};
+
+struct s_isp_frames {
+	/*
+	 * Global variables that are written to by either the SP or the host,
+	 * every ISP binary needs these.
+	 */
+	/* output frame */
+	char *xmem_base_addr_y;
+	char *xmem_base_addr_uv;
+	char *xmem_base_addr_u;
+	char *xmem_base_addr_v;
+	/* 2nd output frame */
+	char *xmem_base_addr_second_out_y;
+	char *xmem_base_addr_second_out_u;
+	char *xmem_base_addr_second_out_v;
+	/* input yuv frame */
+	char *xmem_base_addr_y_in;
+	char *xmem_base_addr_u_in;
+	char *xmem_base_addr_v_in;
+	/* input raw frame */
+	char *xmem_base_addr_raw;
+	/* output raw frame */
+	char *xmem_base_addr_raw_out;
+	/* viewfinder output (vf_veceven) */
+	char *xmem_base_addr_vfout_y;
+	char *xmem_base_addr_vfout_u;
+	char *xmem_base_addr_vfout_v;
+	/* overlay frame (for vf_pp) */
+	char *xmem_base_addr_overlay_y;
+	char *xmem_base_addr_overlay_u;
+	char *xmem_base_addr_overlay_v;
+	/* pre-gdc output frame (gdc input) */
+	char *xmem_base_addr_qplane_r;
+	char *xmem_base_addr_qplane_ratb;
+	char *xmem_base_addr_qplane_gr;
+	char *xmem_base_addr_qplane_gb;
+	char *xmem_base_addr_qplane_b;
+	char *xmem_base_addr_qplane_batr;
+	/* YUV as input, used by postisp binary */
+	char *xmem_base_addr_yuv_16_y;
+	char *xmem_base_addr_yuv_16_u;
+	char *xmem_base_addr_yuv_16_v;
+};
+
+#endif /* _ISP_TYPES_H_ */
diff --git a/drivers/staging/media/atomisp/pci/isp2400_input_system_global.h b/drivers/staging/media/atomisp/pci/isp2400_input_system_global.h
new file mode 100644
index 000000000..61f23814e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2400_input_system_global.h
@@ -0,0 +1,149 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <type_support.h>
+
+//CSI reveiver has 3 ports.
+#define		N_CSI_PORTS (3)
+//AM: Use previous define for this.
+
+//MIPI allows upto 4 channels.
+#define		N_CHANNELS  (4)
+// 12KB = 256bit x 384 words
+#define		IB_CAPACITY_IN_WORDS (384)
+
+typedef enum {
+	MIPI_0LANE_CFG = 0,
+	MIPI_1LANE_CFG = 1,
+	MIPI_2LANE_CFG = 2,
+	MIPI_3LANE_CFG = 3,
+	MIPI_4LANE_CFG = 4
+} mipi_lane_cfg_t;
+
+typedef enum {
+	INPUT_SYSTEM_SOURCE_SENSOR = 0,
+	INPUT_SYSTEM_SOURCE_FIFO,
+	INPUT_SYSTEM_SOURCE_TPG,
+	INPUT_SYSTEM_SOURCE_PRBS,
+	INPUT_SYSTEM_SOURCE_MEMORY,
+	N_INPUT_SYSTEM_SOURCE
+} input_system_source_t;
+
+/* internal routing configuration */
+typedef enum {
+	INPUT_SYSTEM_DISCARD_ALL = 0,
+	INPUT_SYSTEM_CSI_BACKEND = 1,
+	INPUT_SYSTEM_INPUT_BUFFER = 2,
+	INPUT_SYSTEM_MULTICAST = 3,
+	N_INPUT_SYSTEM_CONNECTION
+} input_system_connection_t;
+
+typedef enum {
+	INPUT_SYSTEM_MIPI_PORT0,
+	INPUT_SYSTEM_MIPI_PORT1,
+	INPUT_SYSTEM_MIPI_PORT2,
+	INPUT_SYSTEM_ACQUISITION_UNIT,
+	N_INPUT_SYSTEM_MULTIPLEX
+} input_system_multiplex_t;
+
+typedef enum {
+	INPUT_SYSTEM_SINK_MEMORY = 0,
+	INPUT_SYSTEM_SINK_ISP,
+	INPUT_SYSTEM_SINK_SP,
+	N_INPUT_SYSTEM_SINK
+} input_system_sink_t;
+
+typedef enum {
+	INPUT_SYSTEM_FIFO_CAPTURE = 0,
+	INPUT_SYSTEM_FIFO_CAPTURE_WITH_COUNTING,
+	INPUT_SYSTEM_SRAM_BUFFERING,
+	INPUT_SYSTEM_XMEM_BUFFERING,
+	INPUT_SYSTEM_XMEM_CAPTURE,
+	INPUT_SYSTEM_XMEM_ACQUIRE,
+	N_INPUT_SYSTEM_BUFFERING_MODE
+} buffering_mode_t;
+
+typedef struct isp2400_input_system_cfg_s	input_system_cfg_t;
+typedef struct sync_generator_cfg_s	sync_generator_cfg_t;
+typedef struct tpg_cfg_s			tpg_cfg_t;
+typedef struct prbs_cfg_s			prbs_cfg_t;
+
+/* MW: uint16_t should be sufficient */
+struct isp2400_input_system_cfg_s {
+	u32	no_side_band;
+	u32	fmt_type;
+	u32	ch_id;
+	u32	input_mode;
+};
+
+struct sync_generator_cfg_s {
+	u32	width;
+	u32	height;
+	u32	hblank_cycles;
+	u32	vblank_cycles;
+};
+
+/* MW: tpg & prbs are exclusive */
+struct tpg_cfg_s {
+	u32	x_mask;
+	u32	y_mask;
+	u32	x_delta;
+	u32	y_delta;
+	u32	xy_mask;
+	sync_generator_cfg_t sync_gen_cfg;
+};
+
+struct prbs_cfg_s {
+	u32	seed;
+	sync_generator_cfg_t sync_gen_cfg;
+};
+
+struct gpfifo_cfg_s {
+// TBD.
+	sync_generator_cfg_t sync_gen_cfg;
+};
+
+typedef struct gpfifo_cfg_s		gpfifo_cfg_t;
+
+//ALX:Commented out to pass the compilation.
+//typedef struct isp2400_input_system_cfg_s input_system_cfg_t;
+
+struct ib_buffer_s {
+	u32	mem_reg_size;
+	u32	nof_mem_regs;
+	u32	mem_reg_addr;
+};
+
+typedef struct ib_buffer_s	isp2400_ib_buffer_t;
+
+struct csi_cfg_s {
+	u32			csi_port;
+	buffering_mode_t	buffering_mode;
+	isp2400_ib_buffer_t	csi_buffer;
+	isp2400_ib_buffer_t	acquisition_buffer;
+	u32			nof_xmem_buffers;
+};
+
+typedef struct csi_cfg_s	 csi_cfg_t;
+
+typedef enum {
+	INPUT_SYSTEM_CFG_FLAG_RESET	= 0,
+	INPUT_SYSTEM_CFG_FLAG_SET		= 1U << 0,
+	INPUT_SYSTEM_CFG_FLAG_BLOCKED	= 1U << 1,
+	INPUT_SYSTEM_CFG_FLAG_REQUIRED	= 1U << 2,
+	INPUT_SYSTEM_CFG_FLAG_CONFLICT	= 1U << 3	// To mark a conflicting configuration.
+} isp2400_input_system_cfg_flag_t;
+
+typedef u32 input_system_config_flags_t;
diff --git a/drivers/staging/media/atomisp/pci/isp2400_input_system_local.h b/drivers/staging/media/atomisp/pci/isp2400_input_system_local.h
new file mode 100644
index 000000000..c3ae5014a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2400_input_system_local.h
@@ -0,0 +1,375 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_LOCAL_H_INCLUDED__
+#define __INPUT_SYSTEM_LOCAL_H_INCLUDED__
+
+#include "input_system_defs.h"		/* HIVE_ISYS_GPREG_MULTICAST_A_IDX,... */
+
+/*
+ * _HRT_CSS_RECEIVER_2400_TWO_PIXEL_EN_REG_IDX,
+ * _HRT_CSS_RECEIVER_2400_CSI2_FUNC_PROG_REG_IDX,...
+ */
+#include "css_receiver_2400_defs.h"
+
+#include "isp_capture_defs.h"
+
+#include "isp_acquisition_defs.h"
+#include "input_system_ctrl_defs.h"
+
+struct target_cfg2400_s {
+	input_switch_cfg_channel_t		input_switch_channel_cfg;
+	target_isp_cfg_t	target_isp_cfg;
+	target_sp_cfg_t		target_sp_cfg;
+	target_strm2mem_cfg_t	target_strm2mem_cfg;
+};
+
+// Configuration of a channel.
+struct channel_cfg_s {
+	u32		ch_id;
+	backend_channel_cfg_t	backend_ch;
+	input_system_source_t	source_type;
+	source_cfg_t		source_cfg;
+	target_cfg2400_t	target_cfg;
+};
+
+// Complete configuration for input system.
+struct input_system_cfg2400_s {
+	input_system_source_t source_type;
+	input_system_config_flags_t	source_type_flags;
+	//channel_cfg_t		channel[N_CHANNELS];
+	input_system_config_flags_t	ch_flags[N_CHANNELS];
+	//  This is the place where the buffers' settings are collected, as given.
+	csi_cfg_t			csi_value[N_CSI_PORTS];
+	input_system_config_flags_t	csi_flags[N_CSI_PORTS];
+
+	// Possible another struct for ib.
+	// This buffers set at the end, based on the all configurations.
+	isp2400_ib_buffer_t			csi_buffer[N_CSI_PORTS];
+	input_system_config_flags_t	csi_buffer_flags[N_CSI_PORTS];
+	isp2400_ib_buffer_t			acquisition_buffer_unique;
+	input_system_config_flags_t	acquisition_buffer_unique_flags;
+	u32			unallocated_ib_mem_words; // Used for check.DEFAULT = IB_CAPACITY_IN_WORDS.
+	//uint32_t			acq_allocated_ib_mem_words;
+
+	input_system_connection_t		multicast[N_CSI_PORTS];
+	input_system_multiplex_t		multiplexer;
+	input_system_config_flags_t		multiplexer_flags;
+
+	tpg_cfg_t			tpg_value;
+	input_system_config_flags_t	tpg_flags;
+	prbs_cfg_t			prbs_value;
+	input_system_config_flags_t	prbs_flags;
+	gpfifo_cfg_t		gpfifo_value;
+	input_system_config_flags_t	gpfifo_flags;
+
+	input_switch_cfg_t		input_switch_cfg;
+
+	target_isp_cfg_t		target_isp[N_CHANNELS];
+	input_system_config_flags_t	target_isp_flags[N_CHANNELS];
+	target_sp_cfg_t			target_sp[N_CHANNELS];
+	input_system_config_flags_t	target_sp_flags[N_CHANNELS];
+	target_strm2mem_cfg_t	target_strm2mem[N_CHANNELS];
+	input_system_config_flags_t	target_strm2mem_flags[N_CHANNELS];
+
+	input_system_config_flags_t		session_flags;
+
+};
+
+/*
+ * For each MIPI port
+ */
+#define _HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX			_HRT_CSS_RECEIVER_2400_DEVICE_READY_REG_IDX
+#define _HRT_CSS_RECEIVER_IRQ_STATUS_REG_IDX			_HRT_CSS_RECEIVER_2400_IRQ_STATUS_REG_IDX
+#define _HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX			_HRT_CSS_RECEIVER_2400_IRQ_ENABLE_REG_IDX
+#define _HRT_CSS_RECEIVER_TIMEOUT_COUNT_REG_IDX		    _HRT_CSS_RECEIVER_2400_CSI2_FUNC_PROG_REG_IDX
+#define _HRT_CSS_RECEIVER_INIT_COUNT_REG_IDX			_HRT_CSS_RECEIVER_2400_INIT_COUNT_REG_IDX
+/* new regs for each MIPI port w.r.t. 2300 */
+#define _HRT_CSS_RECEIVER_RAW16_18_DATAID_REG_IDX       _HRT_CSS_RECEIVER_2400_RAW16_18_DATAID_REG_IDX
+#define _HRT_CSS_RECEIVER_SYNC_COUNT_REG_IDX            _HRT_CSS_RECEIVER_2400_SYNC_COUNT_REG_IDX
+#define _HRT_CSS_RECEIVER_RX_COUNT_REG_IDX              _HRT_CSS_RECEIVER_2400_RX_COUNT_REG_IDX
+
+/* _HRT_CSS_RECEIVER_2400_COMP_FORMAT_REG_IDX is not defined per MIPI port but per channel */
+/* _HRT_CSS_RECEIVER_2400_COMP_PREDICT_REG_IDX is not defined per MIPI port but per channel */
+#define _HRT_CSS_RECEIVER_FS_TO_LS_DELAY_REG_IDX        _HRT_CSS_RECEIVER_2400_FS_TO_LS_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_LS_TO_DATA_DELAY_REG_IDX      _HRT_CSS_RECEIVER_2400_LS_TO_DATA_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_DATA_TO_LE_DELAY_REG_IDX      _HRT_CSS_RECEIVER_2400_DATA_TO_LE_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_LE_TO_FE_DELAY_REG_IDX        _HRT_CSS_RECEIVER_2400_LE_TO_FE_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_FE_TO_FS_DELAY_REG_IDX        _HRT_CSS_RECEIVER_2400_FE_TO_FS_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_LE_TO_LS_DELAY_REG_IDX        _HRT_CSS_RECEIVER_2400_LE_TO_LS_DELAY_REG_IDX
+#define _HRT_CSS_RECEIVER_TWO_PIXEL_EN_REG_IDX			_HRT_CSS_RECEIVER_2400_TWO_PIXEL_EN_REG_IDX
+#define _HRT_CSS_RECEIVER_BACKEND_RST_REG_IDX           _HRT_CSS_RECEIVER_2400_BACKEND_RST_REG_IDX
+#define _HRT_CSS_RECEIVER_RAW18_REG_IDX                 _HRT_CSS_RECEIVER_2400_RAW18_REG_IDX
+#define _HRT_CSS_RECEIVER_FORCE_RAW8_REG_IDX            _HRT_CSS_RECEIVER_2400_FORCE_RAW8_REG_IDX
+#define _HRT_CSS_RECEIVER_RAW16_REG_IDX                 _HRT_CSS_RECEIVER_2400_RAW16_REG_IDX
+
+/* Previously MIPI port regs, now 2x2 logical channel regs */
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC0_REG0_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG0_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC0_REG1_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC0_REG1_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC1_REG0_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG0_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC1_REG1_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC1_REG1_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC2_REG0_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG0_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC2_REG1_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC2_REG1_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC3_REG0_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG0_IDX
+#define _HRT_CSS_RECEIVER_COMP_SCHEME_VC3_REG1_IDX		_HRT_CSS_RECEIVER_2400_COMP_SCHEME_VC3_REG1_IDX
+
+/* Second backend is at offset 0x0700 w.r.t. the first port at offset 0x0100 */
+#define _HRT_CSS_BE_OFFSET                              448
+#define _HRT_CSS_RECEIVER_BE_GSP_ACC_OVL_REG_IDX        (_HRT_CSS_RECEIVER_2400_BE_GSP_ACC_OVL_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_SRST_REG_IDX               (_HRT_CSS_RECEIVER_2400_BE_SRST_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_TWO_PPC_REG_IDX            (_HRT_CSS_RECEIVER_2400_BE_TWO_PPC_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG0_IDX       (_HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG0_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG1_IDX       (_HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG1_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG2_IDX       (_HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG2_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_COMP_FORMAT_REG3_IDX       (_HRT_CSS_RECEIVER_2400_BE_COMP_FORMAT_REG3_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_SEL_REG_IDX                (_HRT_CSS_RECEIVER_2400_BE_SEL_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_RAW16_CONFIG_REG_IDX       (_HRT_CSS_RECEIVER_2400_BE_RAW16_CONFIG_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_RAW18_CONFIG_REG_IDX       (_HRT_CSS_RECEIVER_2400_BE_RAW18_CONFIG_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_FORCE_RAW8_REG_IDX         (_HRT_CSS_RECEIVER_2400_BE_FORCE_RAW8_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_IRQ_STATUS_REG_IDX         (_HRT_CSS_RECEIVER_2400_BE_IRQ_STATUS_REG_IDX + _HRT_CSS_BE_OFFSET)
+#define _HRT_CSS_RECEIVER_BE_IRQ_CLEAR_REG_IDX          (_HRT_CSS_RECEIVER_2400_BE_IRQ_CLEAR_REG_IDX + _HRT_CSS_BE_OFFSET)
+
+#define _HRT_CSS_RECEIVER_IRQ_OVERRUN_BIT		_HRT_CSS_RECEIVER_2400_IRQ_OVERRUN_BIT
+#define _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT		_HRT_CSS_RECEIVER_2400_IRQ_RESERVED_BIT
+#define _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_ENTRY_BIT	_HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_ENTRY_BIT
+#define _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_EXIT_BIT	_HRT_CSS_RECEIVER_2400_IRQ_SLEEP_MODE_EXIT_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_SOT_HS_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_HS_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_SOT_SYNC_HS_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_SOT_SYNC_HS_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_CONTROL_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_CONTROL_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_ECC_DOUBLE_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_DOUBLE_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_ECC_CORRECTED_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_CORRECTED_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_ECC_NO_CORRECTION_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_ECC_NO_CORRECTION_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_CRC_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_CRC_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_ID_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_ID_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_SYNC_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_SYNC_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_DATA_BIT	_HRT_CSS_RECEIVER_2400_IRQ_ERR_FRAME_DATA_BIT
+#define _HRT_CSS_RECEIVER_IRQ_DATA_TIMEOUT_BIT		_HRT_CSS_RECEIVER_2400_IRQ_DATA_TIMEOUT_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_ESCAPE_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_ESCAPE_BIT
+#define _HRT_CSS_RECEIVER_IRQ_ERR_LINE_SYNC_BIT		_HRT_CSS_RECEIVER_2400_IRQ_ERR_LINE_SYNC_BIT
+
+#define _HRT_CSS_RECEIVER_FUNC_PROG_REG_IDX		_HRT_CSS_RECEIVER_2400_CSI2_FUNC_PROG_REG_IDX
+#define	_HRT_CSS_RECEIVER_DATA_TIMEOUT_IDX		_HRT_CSS_RECEIVER_2400_CSI2_DATA_TIMEOUT_IDX
+#define	_HRT_CSS_RECEIVER_DATA_TIMEOUT_BITS		_HRT_CSS_RECEIVER_2400_CSI2_DATA_TIMEOUT_BITS
+
+typedef struct capture_unit_state_s	capture_unit_state_t;
+typedef struct acquisition_unit_state_s	acquisition_unit_state_t;
+typedef struct ctrl_unit_state_s	ctrl_unit_state_t;
+
+typedef enum {
+	MIPI_FORMAT_RGB888 = 0,
+	MIPI_FORMAT_RGB555,
+	MIPI_FORMAT_RGB444,
+	MIPI_FORMAT_RGB565,
+	MIPI_FORMAT_RGB666,
+	MIPI_FORMAT_RAW8,		/* 5 */
+	MIPI_FORMAT_RAW10,
+	MIPI_FORMAT_RAW6,
+	MIPI_FORMAT_RAW7,
+	MIPI_FORMAT_RAW12,
+	MIPI_FORMAT_RAW14,		/* 10 */
+	MIPI_FORMAT_YUV420_8,
+	MIPI_FORMAT_YUV420_10,
+	MIPI_FORMAT_YUV422_8,
+	MIPI_FORMAT_YUV422_10,
+	MIPI_FORMAT_CUSTOM0,	/* 15 */
+	MIPI_FORMAT_YUV420_8_LEGACY,
+	MIPI_FORMAT_EMBEDDED,
+	MIPI_FORMAT_CUSTOM1,
+	MIPI_FORMAT_CUSTOM2,
+	MIPI_FORMAT_CUSTOM3,	/* 20 */
+	MIPI_FORMAT_CUSTOM4,
+	MIPI_FORMAT_CUSTOM5,
+	MIPI_FORMAT_CUSTOM6,
+	MIPI_FORMAT_CUSTOM7,
+	MIPI_FORMAT_YUV420_8_SHIFT,	/* 25 */
+	MIPI_FORMAT_YUV420_10_SHIFT,
+	MIPI_FORMAT_RAW16,
+	MIPI_FORMAT_RAW18,
+	N_MIPI_FORMAT,
+} mipi_format_t;
+
+#define MIPI_FORMAT_JPEG		MIPI_FORMAT_CUSTOM0
+#define MIPI_FORMAT_BINARY_8	MIPI_FORMAT_CUSTOM0
+#define N_MIPI_FORMAT_CUSTOM	8
+
+/* The number of stores for compressed format types */
+#define	N_MIPI_COMPRESSOR_CONTEXT	(N_RX_CHANNEL_ID * N_MIPI_FORMAT_CUSTOM)
+
+typedef enum {
+	RX_IRQ_INFO_BUFFER_OVERRUN   = 1UL << _HRT_CSS_RECEIVER_IRQ_OVERRUN_BIT,
+	RX_IRQ_INFO_INIT_TIMEOUT     = 1UL << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT,
+	RX_IRQ_INFO_ENTER_SLEEP_MODE = 1UL << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_ENTRY_BIT,
+	RX_IRQ_INFO_EXIT_SLEEP_MODE  = 1UL << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_EXIT_BIT,
+	RX_IRQ_INFO_ECC_CORRECTED    = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_CORRECTED_BIT,
+	RX_IRQ_INFO_ERR_SOT          = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_HS_BIT,
+	RX_IRQ_INFO_ERR_SOT_SYNC     = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_SYNC_HS_BIT,
+	RX_IRQ_INFO_ERR_CONTROL      = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_CONTROL_BIT,
+	RX_IRQ_INFO_ERR_ECC_DOUBLE   = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_DOUBLE_BIT,
+	/*	RX_IRQ_INFO_NO_ERR           = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_NO_CORRECTION_BIT, */
+	RX_IRQ_INFO_ERR_CRC          = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_CRC_BIT,
+	RX_IRQ_INFO_ERR_UNKNOWN_ID   = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_ID_BIT,
+	RX_IRQ_INFO_ERR_FRAME_SYNC   = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_SYNC_BIT,
+	RX_IRQ_INFO_ERR_FRAME_DATA   = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_DATA_BIT,
+	RX_IRQ_INFO_ERR_DATA_TIMEOUT = 1UL << _HRT_CSS_RECEIVER_IRQ_DATA_TIMEOUT_BIT,
+	RX_IRQ_INFO_ERR_UNKNOWN_ESC  = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_ESCAPE_BIT,
+	RX_IRQ_INFO_ERR_LINE_SYNC    = 1UL << _HRT_CSS_RECEIVER_IRQ_ERR_LINE_SYNC_BIT,
+}  rx_irq_info_t;
+
+/* NOTE: The base has already an offset of 0x0100 */
+static const hrt_address __maybe_unused MIPI_PORT_OFFSET[N_MIPI_PORT_ID] = {
+	0x00000000UL,
+	0x00000100UL,
+	0x00000200UL
+};
+
+static const hrt_address __maybe_unused SUB_SYSTEM_OFFSET[N_SUB_SYSTEM_ID] = {
+	0x00001000UL,
+	0x00002000UL,
+	0x00003000UL,
+	0x00004000UL,
+	0x00005000UL,
+	0x00009000UL,
+	0x0000A000UL,
+	0x0000B000UL,
+	0x0000C000UL
+};
+
+struct capture_unit_state_s {
+	int	Packet_Length;
+	int	Received_Length;
+	int	Received_Short_Packets;
+	int	Received_Long_Packets;
+	int	Last_Command;
+	int	Next_Command;
+	int	Last_Acknowledge;
+	int	Next_Acknowledge;
+	int	FSM_State_Info;
+	int	StartMode;
+	int	Start_Addr;
+	int	Mem_Region_Size;
+	int	Num_Mem_Regions;
+	/*	int	Init;   write-only registers
+		int	Start;
+		int	Stop;      */
+};
+
+struct acquisition_unit_state_s {
+	/*	int	Init;   write-only register */
+	int	Received_Short_Packets;
+	int	Received_Long_Packets;
+	int	Last_Command;
+	int	Next_Command;
+	int	Last_Acknowledge;
+	int	Next_Acknowledge;
+	int	FSM_State_Info;
+	int	Int_Cntr_Info;
+	int	Start_Addr;
+	int	Mem_Region_Size;
+	int	Num_Mem_Regions;
+};
+
+struct ctrl_unit_state_s {
+	int	last_cmd;
+	int	next_cmd;
+	int	last_ack;
+	int	next_ack;
+	int	top_fsm_state;
+	int	captA_fsm_state;
+	int	captB_fsm_state;
+	int	captC_fsm_state;
+	int	acq_fsm_state;
+	int	captA_start_addr;
+	int	captB_start_addr;
+	int	captC_start_addr;
+	int	captA_mem_region_size;
+	int	captB_mem_region_size;
+	int	captC_mem_region_size;
+	int	captA_num_mem_regions;
+	int	captB_num_mem_regions;
+	int	captC_num_mem_regions;
+	int	acq_start_addr;
+	int	acq_mem_region_size;
+	int	acq_num_mem_regions;
+	/*	int	ctrl_init;  write only register */
+	int	capt_reserve_one_mem_region;
+};
+
+struct input_system_state_s {
+	int	str_multicastA_sel;
+	int	str_multicastB_sel;
+	int	str_multicastC_sel;
+	int	str_mux_sel;
+	int	str_mon_status;
+	int	str_mon_irq_cond;
+	int	str_mon_irq_en;
+	int	isys_srst;
+	int	isys_slv_reg_srst;
+	int	str_deint_portA_cnt;
+	int	str_deint_portB_cnt;
+	struct capture_unit_state_s		capture_unit[N_CAPTURE_UNIT_ID];
+	struct acquisition_unit_state_s	acquisition_unit[N_ACQUISITION_UNIT_ID];
+	struct ctrl_unit_state_s		ctrl_unit_state[N_CTRL_UNIT_ID];
+};
+
+struct mipi_port_state_s {
+	int	device_ready;
+	int	irq_status;
+	int	irq_enable;
+	u32	timeout_count;
+	u16	init_count;
+	u16	raw16_18;
+	u32	sync_count;		/*4 x uint8_t */
+	u32	rx_count;		/*4 x uint8_t */
+	u8		lane_sync_count[MIPI_4LANE_CFG];
+	u8		lane_rx_count[MIPI_4LANE_CFG];
+};
+
+struct rx_channel_state_s {
+	u32	comp_scheme0;
+	u32	comp_scheme1;
+	mipi_predictor_t		pred[N_MIPI_FORMAT_CUSTOM];
+	mipi_compressor_t		comp[N_MIPI_FORMAT_CUSTOM];
+};
+
+struct receiver_state_s {
+	u8	fs_to_ls_delay;
+	u8	ls_to_data_delay;
+	u8	data_to_le_delay;
+	u8	le_to_fe_delay;
+	u8	fe_to_fs_delay;
+	u8	le_to_fs_delay;
+	bool	is_two_ppc;
+	int	backend_rst;
+	u16	raw18;
+	bool		force_raw8;
+	u16	raw16;
+	struct mipi_port_state_s	mipi_port_state[N_MIPI_PORT_ID];
+	struct rx_channel_state_s	rx_channel_state[N_RX_CHANNEL_ID];
+	int	be_gsp_acc_ovl;
+	int	be_srst;
+	int	be_is_two_ppc;
+	int	be_comp_format0;
+	int	be_comp_format1;
+	int	be_comp_format2;
+	int	be_comp_format3;
+	int	be_sel;
+	int	be_raw16_config;
+	int	be_raw18_config;
+	int	be_force_raw8;
+	int	be_irq_status;
+	int	be_irq_clear;
+};
+
+#endif /* __INPUT_SYSTEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/isp2400_input_system_private.h b/drivers/staging/media/atomisp/pci/isp2400_input_system_private.h
new file mode 100644
index 000000000..9c39ca2da
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2400_input_system_private.h
@@ -0,0 +1,123 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_PRIVATE_H_INCLUDED__
+#define __INPUT_SYSTEM_PRIVATE_H_INCLUDED__
+
+#include "input_system_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_INPUT_SYSTEM_C void input_system_reg_store(
+    const input_system_ID_t			ID,
+    const hrt_address			reg,
+    const hrt_data				value)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(INPUT_SYSTEM_BASE[ID] != (hrt_address)-1);
+	ia_css_device_store_uint32(INPUT_SYSTEM_BASE[ID] + reg * sizeof(hrt_data),
+				   value);
+	return;
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C hrt_data input_system_reg_load(
+    const input_system_ID_t			ID,
+    const hrt_address			reg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(INPUT_SYSTEM_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(INPUT_SYSTEM_BASE[ID] + reg * sizeof(
+					     hrt_data));
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C void receiver_reg_store(
+    const rx_ID_t				ID,
+    const hrt_address			reg,
+    const hrt_data				value)
+{
+	assert(ID < N_RX_ID);
+	assert(RX_BASE[ID] != (hrt_address)-1);
+	ia_css_device_store_uint32(RX_BASE[ID] + reg * sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C hrt_data receiver_reg_load(
+    const rx_ID_t				ID,
+    const hrt_address			reg)
+{
+	assert(ID < N_RX_ID);
+	assert(RX_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(RX_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C void receiver_port_reg_store(
+    const rx_ID_t				ID,
+    const enum mipi_port_id			port_ID,
+    const hrt_address			reg,
+    const hrt_data				value)
+{
+	assert(ID < N_RX_ID);
+	assert(port_ID < N_MIPI_PORT_ID);
+	assert(RX_BASE[ID] != (hrt_address)-1);
+	assert(MIPI_PORT_OFFSET[port_ID] != (hrt_address)-1);
+	ia_css_device_store_uint32(RX_BASE[ID] + MIPI_PORT_OFFSET[port_ID] + reg *
+				   sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C hrt_data receiver_port_reg_load(
+    const rx_ID_t				ID,
+    const enum mipi_port_id			port_ID,
+    const hrt_address			reg)
+{
+	assert(ID < N_RX_ID);
+	assert(port_ID < N_MIPI_PORT_ID);
+	assert(RX_BASE[ID] != (hrt_address)-1);
+	assert(MIPI_PORT_OFFSET[port_ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(RX_BASE[ID] + MIPI_PORT_OFFSET[port_ID] + reg *
+					 sizeof(hrt_data));
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C void input_system_sub_system_reg_store(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_ID,
+    const hrt_address			reg,
+    const hrt_data				value)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(sub_ID < N_SUB_SYSTEM_ID);
+	assert(INPUT_SYSTEM_BASE[ID] != (hrt_address)-1);
+	assert(SUB_SYSTEM_OFFSET[sub_ID] != (hrt_address)-1);
+	ia_css_device_store_uint32(INPUT_SYSTEM_BASE[ID] + SUB_SYSTEM_OFFSET[sub_ID] +
+				   reg * sizeof(hrt_data), value);
+	return;
+}
+
+STORAGE_CLASS_INPUT_SYSTEM_C hrt_data input_system_sub_system_reg_load(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_ID,
+    const hrt_address			reg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(sub_ID < N_SUB_SYSTEM_ID);
+	assert(INPUT_SYSTEM_BASE[ID] != (hrt_address)-1);
+	assert(SUB_SYSTEM_OFFSET[sub_ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(INPUT_SYSTEM_BASE[ID] +
+					 SUB_SYSTEM_OFFSET[sub_ID] + reg * sizeof(hrt_data));
+}
+
+#endif /* __INPUT_SYSTEM_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/isp2400_input_system_public.h b/drivers/staging/media/atomisp/pci/isp2400_input_system_public.h
new file mode 100644
index 000000000..85cb61e34
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2400_input_system_public.h
@@ -0,0 +1,370 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_PUBLIC_H_INCLUDED__
+#define __INPUT_SYSTEM_PUBLIC_H_INCLUDED__
+
+#include <type_support.h>
+#ifdef ISP2401
+#include "isys_public.h"
+#else
+
+typedef struct input_system_state_s		input_system_state_t;
+typedef struct receiver_state_s			receiver_state_t;
+
+/*! Read the state of INPUT_SYSTEM[ID]
+
+ \param	ID[in]				INPUT_SYSTEM identifier
+ \param	state[out]			input system state structure
+
+ \return none, state = INPUT_SYSTEM[ID].state
+ */
+void input_system_get_state(
+    const input_system_ID_t		ID,
+    input_system_state_t		*state);
+
+/*! Read the state of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	state[out]			receiver state structure
+
+ \return none, state = RECEIVER[ID].state
+ */
+void receiver_get_state(
+    const rx_ID_t				ID,
+    receiver_state_t			*state);
+
+/*! Flag whether a MIPI format is YUV420
+
+ \param	mipi_format[in]		MIPI format
+
+ \return mipi_format == YUV420
+ */
+bool is_mipi_format_yuv420(
+    const mipi_format_t			mipi_format);
+
+/*! Set compression parameters for cfg[cfg_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	cfg_ID[in]			Configuration identifier
+ \param	comp[in]			Compression method
+ \param	pred[in]			Predictor method
+
+ \NOTE: the storage of compression configuration is
+	implementation specific. The config can be
+	carried either on MIPI ports or on MIPI channels
+
+ \return none, RECEIVER[ID].cfg[cfg_ID] = {comp, pred}
+ */
+void receiver_set_compression(
+    const rx_ID_t				ID,
+    const unsigned int			cfg_ID,
+    const mipi_compressor_t		comp,
+    const mipi_predictor_t		pred);
+
+/*! Enable PORT[port_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+ \param	cnd[in]				irq predicate
+
+ \return None, enable(RECEIVER[ID].PORT[port_ID])
+ */
+void receiver_port_enable(
+    const rx_ID_t				ID,
+    const enum mipi_port_id		port_ID,
+    const bool					cnd);
+
+/*! Flag if PORT[port_ID] of RECEIVER[ID] is enabled
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+
+ \return enable(RECEIVER[ID].PORT[port_ID]) == true
+ */
+bool is_receiver_port_enabled(
+    const rx_ID_t				ID,
+    const enum mipi_port_id		port_ID);
+
+/*! Enable the IRQ channels of PORT[port_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+ \param	irq_info[in]		irq channels
+
+ \return None, enable(RECEIVER[ID].PORT[port_ID].irq_info)
+ */
+void receiver_irq_enable(
+    const rx_ID_t				ID,
+    const enum mipi_port_id		port_ID,
+    const rx_irq_info_t			irq_info);
+
+/*! Return the IRQ status of PORT[port_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+
+ \return RECEIVER[ID].PORT[port_ID].irq_info
+ */
+rx_irq_info_t receiver_get_irq_info(
+    const rx_ID_t				ID,
+    const enum mipi_port_id		port_ID);
+
+/*! Clear the IRQ status of PORT[port_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+ \param	irq_info[in]		irq status
+
+ \return None, clear(RECEIVER[ID].PORT[port_ID].irq_info)
+ */
+void receiver_irq_clear(
+    const rx_ID_t				ID,
+    const enum mipi_port_id			port_ID,
+    const rx_irq_info_t			irq_info);
+
+/*! Write to a control register of INPUT_SYSTEM[ID]
+
+ \param	ID[in]				INPUT_SYSTEM identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, INPUT_SYSTEM[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H void input_system_reg_store(
+    const input_system_ID_t			ID,
+    const hrt_address			reg,
+    const hrt_data				value);
+
+/*! Read from a control register of INPUT_SYSTEM[ID]
+
+ \param	ID[in]				INPUT_SYSTEM identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return INPUT_SYSTEM[ID].ctrl[reg]
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H hrt_data input_system_reg_load(
+    const input_system_ID_t			ID,
+    const hrt_address			reg);
+
+/*! Write to a control register of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, RECEIVER[ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H void receiver_reg_store(
+    const rx_ID_t				ID,
+    const hrt_address			reg,
+    const hrt_data				value);
+
+/*! Read from a control register of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return RECEIVER[ID].ctrl[reg]
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H hrt_data receiver_reg_load(
+    const rx_ID_t				ID,
+    const hrt_address			reg);
+
+/*! Write to a control register of PORT[port_ID] of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, RECEIVER[ID].PORT[port_ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H void receiver_port_reg_store(
+    const rx_ID_t				ID,
+    const enum mipi_port_id			port_ID,
+    const hrt_address			reg,
+    const hrt_data				value);
+
+/*! Read from a control register PORT[port_ID] of of RECEIVER[ID]
+
+ \param	ID[in]				RECEIVER identifier
+ \param	port_ID[in]			mipi PORT identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return RECEIVER[ID].PORT[port_ID].ctrl[reg]
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H hrt_data receiver_port_reg_load(
+    const rx_ID_t				ID,
+    const enum mipi_port_id		port_ID,
+    const hrt_address			reg);
+
+/*! Write to a control register of SUB_SYSTEM[sub_ID] of INPUT_SYSTEM[ID]
+
+ \param	ID[in]				INPUT_SYSTEM identifier
+ \param	port_ID[in]			sub system identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return none, INPUT_SYSTEM[ID].SUB_SYSTEM[sub_ID].ctrl[reg] = value
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H void input_system_sub_system_reg_store(
+    const input_system_ID_t			ID,
+    const sub_system_ID_t			sub_ID,
+    const hrt_address			reg,
+    const hrt_data				value);
+
+/*! Read from a control register SUB_SYSTEM[sub_ID] of INPUT_SYSTEM[ID]
+
+ \param	ID[in]				INPUT_SYSTEM identifier
+ \param	port_ID[in]			sub system identifier
+ \param	reg[in]				register index
+ \param value[in]			The data to be written
+
+ \return INPUT_SYSTEM[ID].SUB_SYSTEM[sub_ID].ctrl[reg]
+ */
+STORAGE_CLASS_INPUT_SYSTEM_H hrt_data input_system_sub_system_reg_load(
+    const input_system_ID_t		ID,
+    const sub_system_ID_t		sub_ID,
+    const hrt_address			reg);
+
+///////////////////////////////////////////////////////////////////////////
+//
+//    Functions for configuration phase on input system.
+//
+///////////////////////////////////////////////////////////////////////////
+
+// Function that resets current configuration.
+// remove the argument since it should be private.
+input_system_err_t input_system_configuration_reset(void);
+
+// Function that commits current configuration.
+// remove the argument since it should be private.
+input_system_err_t input_system_configuration_commit(void);
+
+///////////////////////////////////////////////////////////////////////////
+//
+// User functions:
+//		(encoded generic function)
+//    - no checking
+//    - decoding name and agruments into the generic (channel) configuration
+//    function.
+//
+///////////////////////////////////////////////////////////////////////////
+
+// FIFO channel config function user
+
+input_system_err_t	input_system_csi_fifo_channel_cfg(
+    u32				ch_id,
+    input_system_csi_port_t	port,
+    backend_channel_cfg_t	backend_ch,
+    target_cfg2400_t			target
+);
+
+input_system_err_t	input_system_csi_fifo_channel_with_counting_cfg(
+    u32				ch_id,
+    u32				nof_frame,
+    input_system_csi_port_t	port,
+    backend_channel_cfg_t	backend_ch,
+    u32				mem_region_size,
+    u32				nof_mem_regions,
+    target_cfg2400_t			target
+);
+
+// SRAM channel config function user
+
+input_system_err_t	input_system_csi_sram_channel_cfg(
+    u32				ch_id,
+    input_system_csi_port_t	port,
+    backend_channel_cfg_t	backend_ch,
+    u32				csi_mem_region_size,
+    u32				csi_nof_mem_regions,
+    target_cfg2400_t			target
+);
+
+//XMEM channel config function user
+
+input_system_err_t	input_system_csi_xmem_channel_cfg(
+    u32				ch_id,
+    input_system_csi_port_t port,
+    backend_channel_cfg_t	backend_ch,
+    u32				mem_region_size,
+    u32				nof_mem_regions,
+    u32				acq_mem_region_size,
+    u32				acq_nof_mem_regions,
+    target_cfg2400_t			target,
+    uint32_t				nof_xmem_buffers
+);
+
+input_system_err_t	input_system_csi_xmem_capture_only_channel_cfg(
+    u32				ch_id,
+    u32				nof_frames,
+    input_system_csi_port_t port,
+    u32				csi_mem_region_size,
+    u32				csi_nof_mem_regions,
+    u32				acq_mem_region_size,
+    u32				acq_nof_mem_regions,
+    target_cfg2400_t			target
+);
+
+input_system_err_t	input_system_csi_xmem_acquire_only_channel_cfg(
+    u32				ch_id,
+    u32				nof_frames,
+    input_system_csi_port_t port,
+    backend_channel_cfg_t	backend_ch,
+    u32				acq_mem_region_size,
+    u32				acq_nof_mem_regions,
+    target_cfg2400_t			target
+);
+
+// Non - CSI channel config function user
+
+input_system_err_t	input_system_prbs_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames,
+    u32		seed,
+    u32		sync_gen_width,
+    u32		sync_gen_height,
+    u32		sync_gen_hblank_cycles,
+    u32		sync_gen_vblank_cycles,
+    target_cfg2400_t	target
+);
+
+input_system_err_t	input_system_tpg_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames,//not used yet
+    u32		x_mask,
+    u32		y_mask,
+    u32		x_delta,
+    u32		y_delta,
+    u32		xy_mask,
+    u32		sync_gen_width,
+    u32		sync_gen_height,
+    u32		sync_gen_hblank_cycles,
+    u32		sync_gen_vblank_cycles,
+    target_cfg2400_t	target
+);
+
+input_system_err_t	input_system_gpfifo_channel_cfg(
+    u32		ch_id,
+    u32		nof_frames,
+    target_cfg2400_t	target
+);
+#endif /* #ifdef ISP2401 */
+
+#endif /* __INPUT_SYSTEM_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/isp2400_support.h b/drivers/staging/media/atomisp/pci/isp2400_support.h
new file mode 100644
index 000000000..06d04853d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2400_support.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _isp2400_support_h
+#define _isp2400_support_h
+
+#ifndef ISP2400_VECTOR_TYPES
+/* This typedef is to be able to include hive header files
+   in the host code which is useful in crun */
+typedef char *tmemvectors, *tmemvectoru, *tvector;
+#endif
+
+#define hrt_isp_vamem1_store_16(cell, addr, val) hrt_mem_store_16(cell, HRT_PROC_TYPE_PROP(cell, _simd_vamem1), addr, val)
+#define hrt_isp_vamem2_store_16(cell, addr, val) hrt_mem_store_16(cell, HRT_PROC_TYPE_PROP(cell, _simd_vamem2), addr, val)
+
+#define hrt_isp_dmem(cell) HRT_PROC_TYPE_PROP(cell, _base_dmem)
+#define hrt_isp_vmem(cell) HRT_PROC_TYPE_PROP(cell, _simd_vmem)
+
+#define hrt_isp_dmem_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_isp_dmem(cell))
+#define hrt_isp_vmem_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_isp_vmem(cell))
+
+#if ISP_HAS_HIST
+#define hrt_isp_hist(cell) HRT_PROC_TYPE_PROP(cell, _simd_histogram)
+#define hrt_isp_hist_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_isp_hist(cell))
+#endif
+
+#endif /* _isp2400_support_h */
diff --git a/drivers/staging/media/atomisp/pci/isp2401_input_system_global.h b/drivers/staging/media/atomisp/pci/isp2401_input_system_global.h
new file mode 100644
index 000000000..e3c86069b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2401_input_system_global.h
@@ -0,0 +1,176 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* CSI reveiver has 3 ports. */
+#define		N_CSI_PORTS (3)
+
+#include "system_local.h"
+#include "isys_dma_global.h"	/*	isys2401_dma_channel,
+				 *	isys2401_dma_cfg_t
+				 */
+
+#include "ibuf_ctrl_local.h"	/*	ibuf_cfg_t,
+				 *	ibuf_ctrl_cfg_t
+				 */
+
+#include "isys_stream2mmio.h"	/*	stream2mmio_cfg_t */
+
+#include "csi_rx.h"		/*	csi_rx_frontend_cfg_t,
+				 *	csi_rx_backend_cfg_t,
+				 *	csi_rx_backend_lut_entry_t
+				 */
+#include "pixelgen.h"
+
+#define INPUT_SYSTEM_N_STREAM_ID  6	/* maximum number of simultaneous
+					virtual channels supported*/
+
+typedef enum {
+	INPUT_SYSTEM_SOURCE_TYPE_UNDEFINED = 0,
+	INPUT_SYSTEM_SOURCE_TYPE_SENSOR,
+	INPUT_SYSTEM_SOURCE_TYPE_TPG,
+	INPUT_SYSTEM_SOURCE_TYPE_PRBS,
+	N_INPUT_SYSTEM_SOURCE_TYPE
+} input_system_source_type_t;
+
+typedef struct input_system_channel_s input_system_channel_t;
+struct input_system_channel_s {
+	stream2mmio_ID_t	stream2mmio_id;
+	stream2mmio_sid_ID_t	stream2mmio_sid_id;
+
+	ibuf_ctrl_ID_t		ibuf_ctrl_id;
+	isp2401_ib_buffer_t	ib_buffer;
+
+	isys2401_dma_ID_t	dma_id;
+	isys2401_dma_channel	dma_channel;
+};
+
+typedef struct input_system_channel_cfg_s input_system_channel_cfg_t;
+struct input_system_channel_cfg_s {
+	stream2mmio_cfg_t	stream2mmio_cfg;
+	ibuf_ctrl_cfg_t		ibuf_ctrl_cfg;
+	isys2401_dma_cfg_t	dma_cfg;
+	isys2401_dma_port_cfg_t	dma_src_port_cfg;
+	isys2401_dma_port_cfg_t	dma_dest_port_cfg;
+};
+
+typedef struct input_system_input_port_s input_system_input_port_t;
+struct input_system_input_port_s {
+	input_system_source_type_t	source_type;
+
+	struct {
+		csi_rx_frontend_ID_t		frontend_id;
+		csi_rx_backend_ID_t		backend_id;
+		csi_mipi_packet_type_t		packet_type;
+		csi_rx_backend_lut_entry_t	backend_lut_entry;
+	} csi_rx;
+
+	struct {
+		csi_mipi_packet_type_t		packet_type;
+		csi_rx_backend_lut_entry_t	backend_lut_entry;
+	} metadata;
+
+	struct {
+		pixelgen_ID_t			pixelgen_id;
+	} pixelgen;
+};
+
+typedef struct input_system_input_port_cfg_s input_system_input_port_cfg_t;
+struct input_system_input_port_cfg_s {
+	struct {
+		csi_rx_frontend_cfg_t	frontend_cfg;
+		csi_rx_backend_cfg_t	backend_cfg;
+		csi_rx_backend_cfg_t	md_backend_cfg;
+	} csi_rx_cfg;
+
+	struct {
+		pixelgen_tpg_cfg_t	tpg_cfg;
+		pixelgen_prbs_cfg_t	prbs_cfg;
+	} pixelgen_cfg;
+};
+
+typedef struct isp2401_input_system_cfg_s isp2401_input_system_cfg_t;
+struct isp2401_input_system_cfg_s {
+	input_system_input_port_ID_t	input_port_id;
+
+	input_system_source_type_t	mode;
+
+	bool online;
+	bool raw_packed;
+	s8 linked_isys_stream_id;
+
+	struct {
+		bool	comp_enable;
+		s32	active_lanes;
+		s32	fmt_type;
+		s32	ch_id;
+		s32 comp_predictor;
+		s32 comp_scheme;
+	} csi_port_attr;
+
+	pixelgen_tpg_cfg_t	tpg_port_attr;
+
+	pixelgen_prbs_cfg_t prbs_port_attr;
+
+	struct {
+		s32 align_req_in_bytes;
+		s32 bits_per_pixel;
+		s32 pixels_per_line;
+		s32 lines_per_frame;
+	} input_port_resolution;
+
+	struct {
+		s32 left_padding;
+		s32 max_isp_input_width;
+	} output_port_attr;
+
+	struct {
+		bool    enable;
+		s32 fmt_type;
+		s32 align_req_in_bytes;
+		s32 bits_per_pixel;
+		s32 pixels_per_line;
+		s32 lines_per_frame;
+	} metadata;
+};
+
+typedef struct virtual_input_system_stream_s virtual_input_system_stream_t;
+struct virtual_input_system_stream_s {
+	u32 id;				/*Used when multiple MIPI data types and/or virtual channels are used.
+								Must be unique within one CSI RX
+								and lower than SH_CSS_MAX_ISYS_CHANNEL_NODES */
+	u8 enable_metadata;
+	input_system_input_port_t	input_port;
+	input_system_channel_t		channel;
+	input_system_channel_t		md_channel; /* metadata channel */
+	u8 online;
+	s8 linked_isys_stream_id;
+	u8 valid;
+};
+
+typedef struct virtual_input_system_stream_cfg_s
+	virtual_input_system_stream_cfg_t;
+struct virtual_input_system_stream_cfg_s {
+	u8 enable_metadata;
+	input_system_input_port_cfg_t	input_port_cfg;
+	input_system_channel_cfg_t	channel_cfg;
+	input_system_channel_cfg_t	md_channel_cfg;
+	u8 valid;
+};
+
+#define ISP_INPUT_BUF_START_ADDR	0
+#define NUM_OF_INPUT_BUF		2
+#define NUM_OF_LINES_PER_BUF		2
+#define LINES_OF_ISP_INPUT_BUF		(NUM_OF_INPUT_BUF * NUM_OF_LINES_PER_BUF)
+#define ISP_INPUT_BUF_STRIDE		SH_CSS_MAX_SENSOR_WIDTH
diff --git a/drivers/staging/media/atomisp/pci/isp2401_input_system_local.h b/drivers/staging/media/atomisp/pci/isp2401_input_system_local.h
new file mode 100644
index 000000000..74bfa10e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2401_input_system_local.h
@@ -0,0 +1,78 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_LOCAL_H_INCLUDED__
+#define __INPUT_SYSTEM_LOCAL_H_INCLUDED__
+
+#include "csi_rx.h"
+#include "pixelgen.h"
+#include "isys_stream2mmio.h"
+#include "isys_irq.h"
+
+typedef enum {
+	MIPI_FORMAT_SHORT1 = 0x08,
+	MIPI_FORMAT_SHORT2,
+	MIPI_FORMAT_SHORT3,
+	MIPI_FORMAT_SHORT4,
+	MIPI_FORMAT_SHORT5,
+	MIPI_FORMAT_SHORT6,
+	MIPI_FORMAT_SHORT7,
+	MIPI_FORMAT_SHORT8,
+	MIPI_FORMAT_EMBEDDED = 0x12,
+	MIPI_FORMAT_YUV420_8 = 0x18,
+	MIPI_FORMAT_YUV420_10,
+	MIPI_FORMAT_YUV420_8_LEGACY,
+	MIPI_FORMAT_YUV420_8_SHIFT = 0x1C,
+	MIPI_FORMAT_YUV420_10_SHIFT,
+	MIPI_FORMAT_YUV422_8 = 0x1E,
+	MIPI_FORMAT_YUV422_10,
+	MIPI_FORMAT_RGB444 = 0x20,
+	MIPI_FORMAT_RGB555,
+	MIPI_FORMAT_RGB565,
+	MIPI_FORMAT_RGB666,
+	MIPI_FORMAT_RGB888,
+	MIPI_FORMAT_RAW6 = 0x28,
+	MIPI_FORMAT_RAW7,
+	MIPI_FORMAT_RAW8,
+	MIPI_FORMAT_RAW10,
+	MIPI_FORMAT_RAW12,
+	MIPI_FORMAT_RAW14,
+	MIPI_FORMAT_CUSTOM0 = 0x30,
+	MIPI_FORMAT_CUSTOM1,
+	MIPI_FORMAT_CUSTOM2,
+	MIPI_FORMAT_CUSTOM3,
+	MIPI_FORMAT_CUSTOM4,
+	MIPI_FORMAT_CUSTOM5,
+	MIPI_FORMAT_CUSTOM6,
+	MIPI_FORMAT_CUSTOM7,
+	//MIPI_FORMAT_RAW16, /*not supported by 2401*/
+	//MIPI_FORMAT_RAW18,
+	N_MIPI_FORMAT
+} mipi_format_t;
+
+#define N_MIPI_FORMAT_CUSTOM	8
+
+/* The number of stores for compressed format types */
+#define	N_MIPI_COMPRESSOR_CONTEXT	(N_RX_CHANNEL_ID * N_MIPI_FORMAT_CUSTOM)
+typedef struct input_system_state_s	input_system_state_t;
+struct input_system_state_s {
+	ibuf_ctrl_state_t	ibuf_ctrl_state[N_IBUF_CTRL_ID];
+	csi_rx_fe_ctrl_state_t	csi_rx_fe_ctrl_state[N_CSI_RX_FRONTEND_ID];
+	csi_rx_be_ctrl_state_t	csi_rx_be_ctrl_state[N_CSI_RX_BACKEND_ID];
+	pixelgen_ctrl_state_t	pixelgen_ctrl_state[N_PIXELGEN_ID];
+	stream2mmio_state_t	stream2mmio_state[N_STREAM2MMIO_ID];
+	isys_irqc_state_t	isys_irqc_state[N_ISYS_IRQ_ID];
+};
+#endif /* __INPUT_SYSTEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/isp2401_input_system_private.h b/drivers/staging/media/atomisp/pci/isp2401_input_system_private.h
new file mode 100644
index 000000000..e4c76428f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp2401_input_system_private.h
@@ -0,0 +1,342 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __INPUT_SYSTEM_PRIVATE_H_INCLUDED__
+#define __INPUT_SYSTEM_PRIVATE_H_INCLUDED__
+
+#include "input_system_public.h"
+
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+
+#include "assert_support.h" /* assert */
+#include "print_support.h" /* print */
+
+/* Load the register value */
+static inline hrt_data ibuf_ctrl_reg_load(const ibuf_ctrl_ID_t ID,
+					  const hrt_address reg)
+{
+	assert(ID < N_IBUF_CTRL_ID);
+	assert(IBUF_CTRL_BASE[ID] != (hrt_address)-1);
+	return ia_css_device_load_uint32(IBUF_CTRL_BASE[ID] + reg * sizeof(hrt_data));
+}
+
+/* Store a value to the register */
+static inline void ibuf_ctrl_reg_store(const ibuf_ctrl_ID_t ID,
+				       const hrt_address reg,
+				       const hrt_data value)
+{
+	assert(ID < N_IBUF_CTRL_ID);
+	assert(IBUF_CTRL_BASE[ID] != (hrt_address)-1);
+
+	ia_css_device_store_uint32(IBUF_CTRL_BASE[ID] + reg * sizeof(hrt_data), value);
+}
+
+/* Get the state of the ibuf-controller process */
+static inline void ibuf_ctrl_get_proc_state(const ibuf_ctrl_ID_t ID,
+					    const u32 proc_id,
+					    ibuf_ctrl_proc_state_t *state)
+{
+	hrt_address reg_bank_offset;
+
+	reg_bank_offset =
+	    _IBUF_CNTRL_PROC_REG_ALIGN * (1 + proc_id);
+
+	state->num_items =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_NUM_ITEMS_PER_STORE);
+
+	state->num_stores =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_NUM_STORES_PER_FRAME);
+
+	state->dma_channel =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DMA_CHANNEL);
+
+	state->dma_command =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DMA_CMD);
+
+	state->ibuf_st_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_BUFFER_START_ADDRESS);
+
+	state->ibuf_stride =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_BUFFER_STRIDE);
+
+	state->ibuf_end_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_BUFFER_END_ADDRESS);
+
+	state->dest_st_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DEST_START_ADDRESS);
+
+	state->dest_stride =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DEST_STRIDE);
+
+	state->dest_end_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DEST_END_ADDRESS);
+
+	state->sync_frame =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_SYNC_FRAME);
+
+	state->sync_command =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_STR2MMIO_SYNC_CMD);
+
+	state->store_command =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_STR2MMIO_STORE_CMD);
+
+	state->shift_returned_items =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_SHIFT_ITEMS);
+
+	state->elems_ibuf =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_ELEMS_P_WORD_IBUF);
+
+	state->elems_dest =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_ELEMS_P_WORD_DEST);
+
+	state->cur_stores =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_STORES);
+
+	state->cur_acks =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_ACKS);
+
+	state->cur_s2m_ibuf_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_S2M_IBUF_ADDR);
+
+	state->cur_dma_ibuf_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_DMA_IBUF_ADDR);
+
+	state->cur_dma_dest_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_DMA_DEST_ADDR);
+
+	state->cur_isp_dest_addr =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_ISP_DEST_ADDR);
+
+	state->dma_cmds_send =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_CUR_NR_DMA_CMDS_SEND);
+
+	state->main_cntrl_state =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_MAIN_CNTRL_STATE);
+
+	state->dma_sync_state =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_DMA_SYNC_STATE);
+
+	state->isp_sync_state =
+	    ibuf_ctrl_reg_load(ID, reg_bank_offset + _IBUF_CNTRL_ISP_SYNC_STATE);
+}
+
+/* Get the ibuf-controller state. */
+static inline void ibuf_ctrl_get_state(const ibuf_ctrl_ID_t ID,
+				       ibuf_ctrl_state_t *state)
+{
+	u32 i;
+
+	state->recalc_words =
+	    ibuf_ctrl_reg_load(ID, _IBUF_CNTRL_RECALC_WORDS_STATUS);
+	state->arbiters =
+	    ibuf_ctrl_reg_load(ID, _IBUF_CNTRL_ARBITERS_STATUS);
+
+	/*
+	 * Get the values of the register-set per
+	 * ibuf-controller process.
+	 */
+	for (i = 0; i < N_IBUF_CTRL_PROCS[ID]; i++) {
+		ibuf_ctrl_get_proc_state(
+		    ID,
+		    i,
+		    &state->proc_state[i]);
+	}
+}
+
+/* Dump the ibuf-controller state */
+static inline void ibuf_ctrl_dump_state(const ibuf_ctrl_ID_t ID,
+					ibuf_ctrl_state_t *state)
+{
+	u32 i;
+
+	ia_css_print("IBUF controller ID %d recalculate words 0x%x\n", ID,
+		     state->recalc_words);
+	ia_css_print("IBUF controller ID %d arbiters 0x%x\n", ID, state->arbiters);
+
+	/*
+	 * Dump the values of the register-set per
+	 * ibuf-controller process.
+	 */
+	for (i = 0; i < N_IBUF_CTRL_PROCS[ID]; i++) {
+		ia_css_print("IBUF controller ID %d Process ID %d num_items 0x%x\n", ID, i,
+			     state->proc_state[i].num_items);
+		ia_css_print("IBUF controller ID %d Process ID %d num_stores 0x%x\n", ID, i,
+			     state->proc_state[i].num_stores);
+		ia_css_print("IBUF controller ID %d Process ID %d dma_channel 0x%x\n", ID, i,
+			     state->proc_state[i].dma_channel);
+		ia_css_print("IBUF controller ID %d Process ID %d dma_command 0x%x\n", ID, i,
+			     state->proc_state[i].dma_command);
+		ia_css_print("IBUF controller ID %d Process ID %d ibuf_st_addr 0x%x\n", ID, i,
+			     state->proc_state[i].ibuf_st_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d ibuf_stride 0x%x\n", ID, i,
+			     state->proc_state[i].ibuf_stride);
+		ia_css_print("IBUF controller ID %d Process ID %d ibuf_end_addr 0x%x\n", ID, i,
+			     state->proc_state[i].ibuf_end_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d dest_st_addr 0x%x\n", ID, i,
+			     state->proc_state[i].dest_st_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d dest_stride 0x%x\n", ID, i,
+			     state->proc_state[i].dest_stride);
+		ia_css_print("IBUF controller ID %d Process ID %d dest_end_addr 0x%x\n", ID, i,
+			     state->proc_state[i].dest_end_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d sync_frame 0x%x\n", ID, i,
+			     state->proc_state[i].sync_frame);
+		ia_css_print("IBUF controller ID %d Process ID %d sync_command 0x%x\n", ID, i,
+			     state->proc_state[i].sync_command);
+		ia_css_print("IBUF controller ID %d Process ID %d store_command 0x%x\n", ID, i,
+			     state->proc_state[i].store_command);
+		ia_css_print("IBUF controller ID %d Process ID %d shift_returned_items 0x%x\n",
+			     ID, i,
+			     state->proc_state[i].shift_returned_items);
+		ia_css_print("IBUF controller ID %d Process ID %d elems_ibuf 0x%x\n", ID, i,
+			     state->proc_state[i].elems_ibuf);
+		ia_css_print("IBUF controller ID %d Process ID %d elems_dest 0x%x\n", ID, i,
+			     state->proc_state[i].elems_dest);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_stores 0x%x\n", ID, i,
+			     state->proc_state[i].cur_stores);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_acks 0x%x\n", ID, i,
+			     state->proc_state[i].cur_acks);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_s2m_ibuf_addr 0x%x\n", ID,
+			     i,
+			     state->proc_state[i].cur_s2m_ibuf_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_dma_ibuf_addr 0x%x\n", ID,
+			     i,
+			     state->proc_state[i].cur_dma_ibuf_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_dma_dest_addr 0x%x\n", ID,
+			     i,
+			     state->proc_state[i].cur_dma_dest_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d cur_isp_dest_addr 0x%x\n", ID,
+			     i,
+			     state->proc_state[i].cur_isp_dest_addr);
+		ia_css_print("IBUF controller ID %d Process ID %d dma_cmds_send 0x%x\n", ID, i,
+			     state->proc_state[i].dma_cmds_send);
+		ia_css_print("IBUF controller ID %d Process ID %d main_cntrl_state 0x%x\n", ID,
+			     i,
+			     state->proc_state[i].main_cntrl_state);
+		ia_css_print("IBUF controller ID %d Process ID %d dma_sync_state 0x%x\n", ID, i,
+			     state->proc_state[i].dma_sync_state);
+		ia_css_print("IBUF controller ID %d Process ID %d isp_sync_state 0x%x\n", ID, i,
+			     state->proc_state[i].isp_sync_state);
+	}
+}
+
+static inline input_system_err_t
+input_system_get_state(const input_system_ID_t ID,
+		       input_system_state_t *state)
+{
+	u32 i;
+
+	(void)(ID);
+
+	/*  get the states of all CSI RX frontend devices */
+	for (i = 0; i < N_CSI_RX_FRONTEND_ID; i++) {
+		csi_rx_fe_ctrl_get_state(
+		    (csi_rx_frontend_ID_t)i,
+		    &state->csi_rx_fe_ctrl_state[i]);
+	}
+
+	/*  get the states of all CIS RX backend devices */
+	for (i = 0; i < N_CSI_RX_BACKEND_ID; i++) {
+		csi_rx_be_ctrl_get_state(
+		    (csi_rx_backend_ID_t)i,
+		    &state->csi_rx_be_ctrl_state[i]);
+	}
+
+	/* get the states of all pixelgen devices */
+	for (i = 0; i < N_PIXELGEN_ID; i++) {
+		pixelgen_ctrl_get_state(
+		    (pixelgen_ID_t)i,
+		    &state->pixelgen_ctrl_state[i]);
+	}
+
+	/* get the states of all stream2mmio devices */
+	for (i = 0; i < N_STREAM2MMIO_ID; i++) {
+		stream2mmio_get_state(
+		    (stream2mmio_ID_t)i,
+		    &state->stream2mmio_state[i]);
+	}
+
+	/* get the states of all ibuf-controller devices */
+	for (i = 0; i < N_IBUF_CTRL_ID; i++) {
+		ibuf_ctrl_get_state(
+		    (ibuf_ctrl_ID_t)i,
+		    &state->ibuf_ctrl_state[i]);
+	}
+
+	/* get the states of all isys irq controllers */
+	for (i = 0; i < N_ISYS_IRQ_ID; i++) {
+		isys_irqc_state_get((isys_irq_ID_t)i, &state->isys_irqc_state[i]);
+	}
+
+	/* TODO: get the states of all ISYS2401 DMA devices  */
+	for (i = 0; i < N_ISYS2401_DMA_ID; i++) {
+	}
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+static inline void input_system_dump_state(const input_system_ID_t ID,
+					   input_system_state_t *state)
+{
+	u32 i;
+
+	(void)(ID);
+
+	/*  dump the states of all CSI RX frontend devices */
+	for (i = 0; i < N_CSI_RX_FRONTEND_ID; i++) {
+		csi_rx_fe_ctrl_dump_state(
+		    (csi_rx_frontend_ID_t)i,
+		    &state->csi_rx_fe_ctrl_state[i]);
+	}
+
+	/*  dump the states of all CIS RX backend devices */
+	for (i = 0; i < N_CSI_RX_BACKEND_ID; i++) {
+		csi_rx_be_ctrl_dump_state(
+		    (csi_rx_backend_ID_t)i,
+		    &state->csi_rx_be_ctrl_state[i]);
+	}
+
+	/* dump the states of all pixelgen devices */
+	for (i = 0; i < N_PIXELGEN_ID; i++) {
+		pixelgen_ctrl_dump_state(
+		    (pixelgen_ID_t)i,
+		    &state->pixelgen_ctrl_state[i]);
+	}
+
+	/* dump the states of all st2mmio devices */
+	for (i = 0; i < N_STREAM2MMIO_ID; i++) {
+		stream2mmio_dump_state(
+		    (stream2mmio_ID_t)i,
+		    &state->stream2mmio_state[i]);
+	}
+
+	/* dump the states of all ibuf-controller devices */
+	for (i = 0; i < N_IBUF_CTRL_ID; i++) {
+		ibuf_ctrl_dump_state(
+		    (ibuf_ctrl_ID_t)i,
+		    &state->ibuf_ctrl_state[i]);
+	}
+
+	/* dump the states of all isys irq controllers */
+	for (i = 0; i < N_ISYS_IRQ_ID; i++) {
+		isys_irqc_state_dump((isys_irq_ID_t)i, &state->isys_irqc_state[i]);
+	}
+
+	/* TODO: dump the states of all ISYS2401 DMA devices  */
+	for (i = 0; i < N_ISYS2401_DMA_ID; i++) {
+	}
+
+	return;
+}
+#endif /* __INPUT_SYSTEM_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/isp_acquisition_defs.h b/drivers/staging/media/atomisp/pci/isp_acquisition_defs.h
new file mode 100644
index 000000000..7e8f6f217
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp_acquisition_defs.h
@@ -0,0 +1,230 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _isp_acquisition_defs_h
+#define _isp_acquisition_defs_h
+
+#define _ISP_ACQUISITION_REG_ALIGN                4  /* assuming 32 bit control bus width */
+#define _ISP_ACQUISITION_BYTES_PER_ELEM           4
+
+/* --------------------------------------------------*/
+
+#define NOF_ACQ_IRQS                              1
+
+/* --------------------------------------------------*/
+/* FSM */
+/* --------------------------------------------------*/
+#define MEM2STREAM_FSM_STATE_BITS                 2
+#define ACQ_SYNCHRONIZER_FSM_STATE_BITS           2
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+#define NOF_ACQ_REGS                              12
+
+// Register id's of MMIO slave accessible registers
+#define ACQ_START_ADDR_REG_ID                     0
+#define ACQ_MEM_REGION_SIZE_REG_ID                1
+#define ACQ_NUM_MEM_REGIONS_REG_ID                2
+#define ACQ_INIT_REG_ID                           3
+#define ACQ_RECEIVED_SHORT_PACKETS_REG_ID         4
+#define ACQ_RECEIVED_LONG_PACKETS_REG_ID          5
+#define ACQ_LAST_COMMAND_REG_ID                   6
+#define ACQ_NEXT_COMMAND_REG_ID                   7
+#define ACQ_LAST_ACKNOWLEDGE_REG_ID               8
+#define ACQ_NEXT_ACKNOWLEDGE_REG_ID               9
+#define ACQ_FSM_STATE_INFO_REG_ID                 10
+#define ACQ_INT_CNTR_INFO_REG_ID                  11
+
+// Register width
+#define ACQ_START_ADDR_REG_WIDTH                  9
+#define ACQ_MEM_REGION_SIZE_REG_WIDTH             9
+#define ACQ_NUM_MEM_REGIONS_REG_WIDTH             9
+#define ACQ_INIT_REG_WIDTH                        3
+#define ACQ_RECEIVED_SHORT_PACKETS_REG_WIDTH      32
+#define ACQ_RECEIVED_LONG_PACKETS_REG_WIDTH       32
+#define ACQ_LAST_COMMAND_REG_WIDTH                32
+#define ACQ_NEXT_COMMAND_REG_WIDTH                32
+#define ACQ_LAST_ACKNOWLEDGE_REG_WIDTH            32
+#define ACQ_NEXT_ACKNOWLEDGE_REG_WIDTH            32
+#define ACQ_FSM_STATE_INFO_REG_WIDTH              ((MEM2STREAM_FSM_STATE_BITS * 3) + (ACQ_SYNCHRONIZER_FSM_STATE_BITS * 3))
+#define ACQ_INT_CNTR_INFO_REG_WIDTH               32
+
+/* register reset value */
+#define ACQ_START_ADDR_REG_RSTVAL                 0
+#define ACQ_MEM_REGION_SIZE_REG_RSTVAL            128
+#define ACQ_NUM_MEM_REGIONS_REG_RSTVAL            3
+#define ACQ_INIT_REG_RSTVAL                       0
+#define ACQ_RECEIVED_SHORT_PACKETS_REG_RSTVAL     0
+#define ACQ_RECEIVED_LONG_PACKETS_REG_RSTVAL      0
+#define ACQ_LAST_COMMAND_REG_RSTVAL               0
+#define ACQ_NEXT_COMMAND_REG_RSTVAL               0
+#define ACQ_LAST_ACKNOWLEDGE_REG_RSTVAL           0
+#define ACQ_NEXT_ACKNOWLEDGE_REG_RSTVAL           0
+#define ACQ_FSM_STATE_INFO_REG_RSTVAL             0
+#define ACQ_INT_CNTR_INFO_REG_RSTVAL              0
+
+/* bit definitions */
+#define ACQ_INIT_RST_REG_BIT                      0
+#define ACQ_INIT_RESYNC_BIT                       2
+#define ACQ_INIT_RST_IDX                          ACQ_INIT_RST_REG_BIT
+#define ACQ_INIT_RST_BITS                         1
+#define ACQ_INIT_RESYNC_IDX                       ACQ_INIT_RESYNC_BIT
+#define ACQ_INIT_RESYNC_BITS                      1
+
+/* --------------------------------------------------*/
+/* TOKEN INFO */
+/* --------------------------------------------------*/
+#define ACQ_TOKEN_ID_LSB                          0
+#define ACQ_TOKEN_ID_MSB                          3
+#define ACQ_TOKEN_WIDTH                           (ACQ_TOKEN_ID_MSB - ACQ_TOKEN_ID_LSB  + 1) // 4
+#define ACQ_TOKEN_ID_IDX                          0
+#define ACQ_TOKEN_ID_BITS                         ACQ_TOKEN_WIDTH
+#define ACQ_INIT_CMD_INIT_IDX                     4
+#define ACQ_INIT_CMD_INIT_BITS                    3
+#define ACQ_CMD_START_ADDR_IDX                    4
+#define ACQ_CMD_START_ADDR_BITS                   9
+#define ACQ_CMD_NOFWORDS_IDX                      13
+#define ACQ_CMD_NOFWORDS_BITS                     9
+#define ACQ_MEM_REGION_ID_IDX                     22
+#define ACQ_MEM_REGION_ID_BITS                    9
+#define ACQ_PACKET_LENGTH_TOKEN_MSB               21
+#define ACQ_PACKET_LENGTH_TOKEN_LSB               13
+#define ACQ_PACKET_DATA_FORMAT_ID_TOKEN_MSB       9
+#define ACQ_PACKET_DATA_FORMAT_ID_TOKEN_LSB       4
+#define ACQ_PACKET_CH_ID_TOKEN_MSB                11
+#define ACQ_PACKET_CH_ID_TOKEN_LSB                10
+#define ACQ_PACKET_MEM_REGION_ID_TOKEN_MSB        12		/* only for capt_end_of_packet_written */
+#define ACQ_PACKET_MEM_REGION_ID_TOKEN_LSB        4		/* only for capt_end_of_packet_written */
+
+/* Command tokens IDs */
+#define ACQ_READ_REGION_AUTO_INCR_TOKEN_ID        0 //0000b
+#define ACQ_READ_REGION_TOKEN_ID                  1 //0001b
+#define ACQ_READ_REGION_SOP_TOKEN_ID              2 //0010b
+#define ACQ_INIT_TOKEN_ID                         8 //1000b
+
+/* Acknowledge token IDs */
+#define ACQ_READ_REGION_ACK_TOKEN_ID              0 //0000b
+#define ACQ_END_OF_PACKET_TOKEN_ID                4 //0100b
+#define ACQ_END_OF_REGION_TOKEN_ID                5 //0101b
+#define ACQ_SOP_MISMATCH_TOKEN_ID                 6 //0110b
+#define ACQ_UNDEF_PH_TOKEN_ID                     7 //0111b
+
+#define ACQ_TOKEN_MEMREGIONID_MSB                 30
+#define ACQ_TOKEN_MEMREGIONID_LSB                 22
+#define ACQ_TOKEN_NOFWORDS_MSB                    21
+#define ACQ_TOKEN_NOFWORDS_LSB                    13
+#define ACQ_TOKEN_STARTADDR_MSB                   12
+#define ACQ_TOKEN_STARTADDR_LSB                   4
+
+/* --------------------------------------------------*/
+/* MIPI */
+/* --------------------------------------------------*/
+
+#define WORD_COUNT_WIDTH                          16
+#define PKT_CODE_WIDTH                            6
+#define CHN_NO_WIDTH                              2
+#define ERROR_INFO_WIDTH                          8
+
+#define LONG_PKTCODE_MAX                          63
+#define LONG_PKTCODE_MIN                          16
+#define SHORT_PKTCODE_MAX                         15
+
+#define EOF_CODE                                  1
+
+/* --------------------------------------------------*/
+/* Packet Info */
+/* --------------------------------------------------*/
+#define ACQ_START_OF_FRAME                        0
+#define ACQ_END_OF_FRAME                          1
+#define ACQ_START_OF_LINE                         2
+#define ACQ_END_OF_LINE                           3
+#define ACQ_LINE_PAYLOAD                          4
+#define ACQ_GEN_SH_PKT                            5
+
+/* bit definition */
+#define ACQ_PKT_TYPE_IDX                          16
+#define ACQ_PKT_TYPE_BITS                         6
+#define ACQ_PKT_SOP_IDX                           32
+#define ACQ_WORD_CNT_IDX                          0
+#define ACQ_WORD_CNT_BITS                         16
+#define ACQ_PKT_INFO_IDX                          16
+#define ACQ_PKT_INFO_BITS                         8
+#define ACQ_HEADER_DATA_IDX                       0
+#define ACQ_HEADER_DATA_BITS                      16
+#define ACQ_ACK_TOKEN_ID_IDX                      ACQ_TOKEN_ID_IDX
+#define ACQ_ACK_TOKEN_ID_BITS                     ACQ_TOKEN_ID_BITS
+#define ACQ_ACK_NOFWORDS_IDX                      13
+#define ACQ_ACK_NOFWORDS_BITS                     9
+#define ACQ_ACK_PKT_LEN_IDX                       4
+#define ACQ_ACK_PKT_LEN_BITS                      16
+
+/* --------------------------------------------------*/
+/* Packet Data Type */
+/* --------------------------------------------------*/
+
+#define ACQ_YUV420_8_DATA                       24   /* 01 1000 YUV420 8-bit                                        */
+#define ACQ_YUV420_10_DATA                      25   /* 01 1001  YUV420 10-bit                                      */
+#define ACQ_YUV420_8L_DATA                      26   /* 01 1010   YUV420 8-bit legacy                               */
+#define ACQ_YUV422_8_DATA                       30   /* 01 1110   YUV422 8-bit                                      */
+#define ACQ_YUV422_10_DATA                      31   /* 01 1111   YUV422 10-bit                                     */
+#define ACQ_RGB444_DATA                         32   /* 10 0000   RGB444                                            */
+#define ACQ_RGB555_DATA						 33   /* 10 0001   RGB555                                            */
+#define ACQ_RGB565_DATA						 34   /* 10 0010   RGB565                                            */
+#define ACQ_RGB666_DATA						 35   /* 10 0011   RGB666                                            */
+#define ACQ_RGB888_DATA						 36   /* 10 0100   RGB888                                            */
+#define ACQ_RAW6_DATA							 40   /* 10 1000   RAW6                                              */
+#define ACQ_RAW7_DATA							 41   /* 10 1001   RAW7                                              */
+#define ACQ_RAW8_DATA							 42   /* 10 1010   RAW8                                              */
+#define ACQ_RAW10_DATA						 43   /* 10 1011   RAW10                                             */
+#define ACQ_RAW12_DATA						 44   /* 10 1100   RAW12                                             */
+#define ACQ_RAW14_DATA						 45   /* 10 1101   RAW14                                             */
+#define ACQ_USR_DEF_1_DATA						 48   /* 11 0000    JPEG [User Defined 8-bit Data Type 1]            */
+#define ACQ_USR_DEF_2_DATA						 49   /* 11 0001    User Defined 8-bit Data Type 2                   */
+#define ACQ_USR_DEF_3_DATA						 50   /* 11 0010    User Defined 8-bit Data Type 3                   */
+#define ACQ_USR_DEF_4_DATA						 51   /* 11 0011    User Defined 8-bit Data Type 4                   */
+#define ACQ_USR_DEF_5_DATA						 52   /* 11 0100    User Defined 8-bit Data Type 5                   */
+#define ACQ_USR_DEF_6_DATA						 53   /* 11 0101    User Defined 8-bit Data Type 6                   */
+#define ACQ_USR_DEF_7_DATA						 54   /* 11 0110    User Defined 8-bit Data Type 7                   */
+#define ACQ_USR_DEF_8_DATA						 55   /* 11 0111    User Defined 8-bit Data Type 8                   */
+#define ACQ_Emb_DATA							 18   /* 01 0010    embedded eight bit non image data                */
+#define ACQ_SOF_DATA							 0   /* 00 0000    frame start                                      */
+#define ACQ_EOF_DATA							 1   /* 00 0001    frame end                                        */
+#define ACQ_SOL_DATA							 2   /* 00 0010    line start                                       */
+#define ACQ_EOL_DATA							 3   /* 00 0011    line end                                         */
+#define ACQ_GEN_SH1_DATA						 8   /* 00 1000  Generic Short Packet Code 1                        */
+#define ACQ_GEN_SH2_DATA						 9   /* 00 1001    Generic Short Packet Code 2                      */
+#define ACQ_GEN_SH3_DATA						 10   /* 00 1010    Generic Short Packet Code 3                      */
+#define ACQ_GEN_SH4_DATA						 11   /* 00 1011    Generic Short Packet Code 4                      */
+#define ACQ_GEN_SH5_DATA						 12   /* 00 1100    Generic Short Packet Code 5                      */
+#define ACQ_GEN_SH6_DATA						 13   /* 00 1101    Generic Short Packet Code 6                      */
+#define ACQ_GEN_SH7_DATA						 14   /* 00 1110    Generic Short Packet Code 7                      */
+#define ACQ_GEN_SH8_DATA						 15   /* 00 1111    Generic Short Packet Code 8                      */
+#define ACQ_YUV420_8_CSPS_DATA					 28   /* 01 1100   YUV420 8-bit (Chroma Shifted Pixel Sampling)      */
+#define ACQ_YUV420_10_CSPS_DATA					 29   /* 01 1101   YUV420 10-bit (Chroma Shifted Pixel Sampling)     */
+#define ACQ_RESERVED_DATA_TYPE_MIN              56
+#define ACQ_RESERVED_DATA_TYPE_MAX              63
+#define ACQ_GEN_LONG_RESERVED_DATA_TYPE_MIN     19
+#define ACQ_GEN_LONG_RESERVED_DATA_TYPE_MAX     23
+#define ACQ_YUV_RESERVED_DATA_TYPE              27
+#define ACQ_RGB_RESERVED_DATA_TYPE_MIN          37
+#define ACQ_RGB_RESERVED_DATA_TYPE_MAX          39
+#define ACQ_RAW_RESERVED_DATA_TYPE_MIN          46
+#define ACQ_RAW_RESERVED_DATA_TYPE_MAX          47
+
+/* --------------------------------------------------*/
+
+#endif /* _isp_acquisition_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/isp_capture_defs.h b/drivers/staging/media/atomisp/pci/isp_capture_defs.h
new file mode 100644
index 000000000..b9e5ed932
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/isp_capture_defs.h
@@ -0,0 +1,279 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _isp_capture_defs_h
+#define _isp_capture_defs_h
+
+#define _ISP_CAPTURE_REG_ALIGN                    4  /* assuming 32 bit control bus width */
+#define _ISP_CAPTURE_BITS_PER_ELEM                32  /* only for data, not SOP */
+#define _ISP_CAPTURE_BYTES_PER_ELEM               (_ISP_CAPTURE_BITS_PER_ELEM / 8)
+#define _ISP_CAPTURE_BYTES_PER_WORD               32		/* 256/8 */
+#define _ISP_CAPTURE_ELEM_PER_WORD                _ISP_CAPTURE_BYTES_PER_WORD / _ISP_CAPTURE_BYTES_PER_ELEM
+
+/* --------------------------------------------------*/
+
+#define NOF_IRQS                                  2
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+// Number of registers
+#define CAPT_NOF_REGS                             16
+
+// Register id's of MMIO slave accessible registers
+#define CAPT_START_MODE_REG_ID                    0
+#define CAPT_START_ADDR_REG_ID                    1
+#define CAPT_MEM_REGION_SIZE_REG_ID               2
+#define CAPT_NUM_MEM_REGIONS_REG_ID               3
+#define CAPT_INIT_REG_ID                          4
+#define CAPT_START_REG_ID                         5
+#define CAPT_STOP_REG_ID                          6
+
+#define CAPT_PACKET_LENGTH_REG_ID                 7
+#define CAPT_RECEIVED_LENGTH_REG_ID               8
+#define CAPT_RECEIVED_SHORT_PACKETS_REG_ID        9
+#define CAPT_RECEIVED_LONG_PACKETS_REG_ID         10
+#define CAPT_LAST_COMMAND_REG_ID                  11
+#define CAPT_NEXT_COMMAND_REG_ID                  12
+#define CAPT_LAST_ACKNOWLEDGE_REG_ID              13
+#define CAPT_NEXT_ACKNOWLEDGE_REG_ID              14
+#define CAPT_FSM_STATE_INFO_REG_ID                15
+
+// Register width
+#define CAPT_START_MODE_REG_WIDTH                 1
+
+#define CAPT_START_REG_WIDTH                      1
+#define CAPT_STOP_REG_WIDTH                       1
+
+/* --------------------------------------------------*/
+/* FSM */
+/* --------------------------------------------------*/
+#define CAPT_WRITE2MEM_FSM_STATE_BITS             2
+#define CAPT_SYNCHRONIZER_FSM_STATE_BITS          3
+
+#define CAPT_PACKET_LENGTH_REG_WIDTH              17
+#define CAPT_RECEIVED_LENGTH_REG_WIDTH            17
+#define CAPT_RECEIVED_SHORT_PACKETS_REG_WIDTH     32
+#define CAPT_RECEIVED_LONG_PACKETS_REG_WIDTH      32
+#define CAPT_LAST_COMMAND_REG_WIDTH               32
+#define CAPT_LAST_ACKNOWLEDGE_REG_WIDTH           32
+#define CAPT_NEXT_ACKNOWLEDGE_REG_WIDTH           32
+#define CAPT_FSM_STATE_INFO_REG_WIDTH             ((CAPT_WRITE2MEM_FSM_STATE_BITS * 3) + (CAPT_SYNCHRONIZER_FSM_STATE_BITS * 3))
+
+/* register reset value */
+#define CAPT_START_MODE_REG_RSTVAL                0
+#define CAPT_START_ADDR_REG_RSTVAL                0
+#define CAPT_MEM_REGION_SIZE_REG_RSTVAL           128
+#define CAPT_NUM_MEM_REGIONS_REG_RSTVAL           3
+#define CAPT_INIT_REG_RSTVAL                      0
+
+#define CAPT_START_REG_RSTVAL                     0
+#define CAPT_STOP_REG_RSTVAL                      0
+
+#define CAPT_PACKET_LENGTH_REG_RSTVAL             0
+#define CAPT_RECEIVED_LENGTH_REG_RSTVAL           0
+#define CAPT_RECEIVED_SHORT_PACKETS_REG_RSTVAL    0
+#define CAPT_RECEIVED_LONG_PACKETS_REG_RSTVAL     0
+#define CAPT_LAST_COMMAND_REG_RSTVAL              0
+#define CAPT_NEXT_COMMAND_REG_RSTVAL              0
+#define CAPT_LAST_ACKNOWLEDGE_REG_RSTVAL          0
+#define CAPT_NEXT_ACKNOWLEDGE_REG_RSTVAL          0
+#define CAPT_FSM_STATE_INFO_REG_RSTVAL            0
+
+/* bit definitions */
+#define CAPT_INIT_RST_REG_BIT                     0
+#define CAPT_INIT_FLUSH_BIT                       1
+#define CAPT_INIT_RESYNC_BIT                      2
+#define CAPT_INIT_RESTART_BIT                     3
+#define CAPT_INIT_RESTART_MEM_ADDR_LSB            4
+
+#define CAPT_INIT_RST_REG_IDX                     CAPT_INIT_RST_REG_BIT
+#define CAPT_INIT_RST_REG_BITS                    1
+#define CAPT_INIT_FLUSH_IDX                       CAPT_INIT_FLUSH_BIT
+#define CAPT_INIT_FLUSH_BITS                      1
+#define CAPT_INIT_RESYNC_IDX                      CAPT_INIT_RESYNC_BIT
+#define CAPT_INIT_RESYNC_BITS                     1
+#define CAPT_INIT_RESTART_IDX                     CAPT_INIT_RESTART_BIT
+#define CAPT_INIT_RESTART_BITS									1
+#define CAPT_INIT_RESTART_MEM_ADDR_IDX            CAPT_INIT_RESTART_MEM_ADDR_LSB
+
+/* --------------------------------------------------*/
+/* TOKEN INFO */
+/* --------------------------------------------------*/
+#define CAPT_TOKEN_ID_LSB                         0
+#define CAPT_TOKEN_ID_MSB                         3
+#define CAPT_TOKEN_WIDTH                         (CAPT_TOKEN_ID_MSB - CAPT_TOKEN_ID_LSB  + 1) /* 4 */
+
+/* Command tokens IDs */
+#define CAPT_START_TOKEN_ID                       0 /* 0000b */
+#define CAPT_STOP_TOKEN_ID                        1 /* 0001b */
+#define CAPT_FREEZE_TOKEN_ID                      2 /* 0010b */
+#define CAPT_RESUME_TOKEN_ID                      3 /* 0011b */
+#define CAPT_INIT_TOKEN_ID                        8 /* 1000b */
+
+#define CAPT_START_TOKEN_BIT                      0
+#define CAPT_STOP_TOKEN_BIT                       0
+#define CAPT_FREEZE_TOKEN_BIT                     0
+#define CAPT_RESUME_TOKEN_BIT                     0
+#define CAPT_INIT_TOKEN_BIT                       0
+
+/* Acknowledge token IDs */
+#define CAPT_END_OF_PACKET_RECEIVED_TOKEN_ID      0 /* 0000b */
+#define CAPT_END_OF_PACKET_WRITTEN_TOKEN_ID       1 /* 0001b */
+#define CAPT_END_OF_REGION_WRITTEN_TOKEN_ID       2 /* 0010b */
+#define CAPT_FLUSH_DONE_TOKEN_ID                  3 /* 0011b */
+#define CAPT_PREMATURE_SOP_TOKEN_ID               4 /* 0100b */
+#define CAPT_MISSING_SOP_TOKEN_ID                 5 /* 0101b */
+#define CAPT_UNDEF_PH_TOKEN_ID                    6 /* 0110b */
+#define CAPT_STOP_ACK_TOKEN_ID                    7 /* 0111b */
+
+#define CAPT_PACKET_LENGTH_TOKEN_MSB             19
+#define CAPT_PACKET_LENGTH_TOKEN_LSB              4
+#define CAPT_SUPER_PACKET_LENGTH_TOKEN_MSB       20
+#define CAPT_SUPER_PACKET_LENGTH_TOKEN_LSB        4
+#define CAPT_PACKET_DATA_FORMAT_ID_TOKEN_MSB     25
+#define CAPT_PACKET_DATA_FORMAT_ID_TOKEN_LSB     20
+#define CAPT_PACKET_CH_ID_TOKEN_MSB              27
+#define CAPT_PACKET_CH_ID_TOKEN_LSB              26
+#define CAPT_PACKET_MEM_REGION_ID_TOKEN_MSB      29
+#define CAPT_PACKET_MEM_REGION_ID_TOKEN_LSB      21
+
+/*  bit definition */
+#define CAPT_CMD_IDX                              CAPT_TOKEN_ID_LSB
+#define	CAPT_CMD_BITS                             (CAPT_TOKEN_ID_MSB - CAPT_TOKEN_ID_LSB + 1)
+#define CAPT_SOP_IDX                              32
+#define CAPT_SOP_BITS                             1
+#define CAPT_PKT_INFO_IDX                         16
+#define CAPT_PKT_INFO_BITS                        8
+#define CAPT_PKT_TYPE_IDX                         0
+#define CAPT_PKT_TYPE_BITS                        6
+#define CAPT_HEADER_DATA_IDX                      0
+#define CAPT_HEADER_DATA_BITS                     16
+#define CAPT_PKT_DATA_IDX                         0
+#define CAPT_PKT_DATA_BITS                        32
+#define CAPT_WORD_CNT_IDX                         0
+#define CAPT_WORD_CNT_BITS                        16
+#define CAPT_ACK_TOKEN_ID_IDX                     0
+#define CAPT_ACK_TOKEN_ID_BITS                    4
+//#define CAPT_ACK_PKT_LEN_IDX                      CAPT_PACKET_LENGTH_TOKEN_LSB
+//#define CAPT_ACK_PKT_LEN_BITS                     (CAPT_PACKET_LENGTH_TOKEN_MSB - CAPT_PACKET_LENGTH_TOKEN_LSB + 1)
+//#define CAPT_ACK_PKT_INFO_IDX                     20
+//#define CAPT_ACK_PKT_INFO_BITS                    8
+//#define CAPT_ACK_MEM_REG_ID1_IDX                  20			/* for capt_end_of_packet_written */
+//#define CAPT_ACK_MEM_REG_ID2_IDX                  4       /* for capt_end_of_region_written */
+#define CAPT_ACK_PKT_LEN_IDX                      CAPT_PACKET_LENGTH_TOKEN_LSB
+#define CAPT_ACK_PKT_LEN_BITS                     (CAPT_PACKET_LENGTH_TOKEN_MSB - CAPT_PACKET_LENGTH_TOKEN_LSB + 1)
+#define CAPT_ACK_SUPER_PKT_LEN_IDX                CAPT_SUPER_PACKET_LENGTH_TOKEN_LSB
+#define CAPT_ACK_SUPER_PKT_LEN_BITS               (CAPT_SUPER_PACKET_LENGTH_TOKEN_MSB - CAPT_SUPER_PACKET_LENGTH_TOKEN_LSB + 1)
+#define CAPT_ACK_PKT_INFO_IDX                     CAPT_PACKET_DATA_FORMAT_ID_TOKEN_LSB
+#define CAPT_ACK_PKT_INFO_BITS                    (CAPT_PACKET_CH_ID_TOKEN_MSB - CAPT_PACKET_DATA_FORMAT_ID_TOKEN_LSB + 1)
+#define CAPT_ACK_MEM_REGION_ID_IDX                CAPT_PACKET_MEM_REGION_ID_TOKEN_LSB
+#define CAPT_ACK_MEM_REGION_ID_BITS               (CAPT_PACKET_MEM_REGION_ID_TOKEN_MSB - CAPT_PACKET_MEM_REGION_ID_TOKEN_LSB + 1)
+#define CAPT_ACK_PKT_TYPE_IDX                     CAPT_PACKET_DATA_FORMAT_ID_TOKEN_LSB
+#define CAPT_ACK_PKT_TYPE_BITS                    (CAPT_PACKET_DATA_FORMAT_ID_TOKEN_MSB - CAPT_PACKET_DATA_FORMAT_ID_TOKEN_LSB + 1)
+#define CAPT_INIT_TOKEN_INIT_IDX                  4
+#define CAPT_INIT_TOKEN_INIT_BITS                 22
+
+/* --------------------------------------------------*/
+/* MIPI */
+/* --------------------------------------------------*/
+
+#define CAPT_WORD_COUNT_WIDTH                     16
+#define CAPT_PKT_CODE_WIDTH                       6
+#define CAPT_CHN_NO_WIDTH                         2
+#define CAPT_ERROR_INFO_WIDTH                     8
+
+#define LONG_PKTCODE_MAX                          63
+#define LONG_PKTCODE_MIN                          16
+#define SHORT_PKTCODE_MAX                         15
+
+/* --------------------------------------------------*/
+/* Packet Info */
+/* --------------------------------------------------*/
+#define CAPT_START_OF_FRAME                       0
+#define CAPT_END_OF_FRAME                         1
+#define CAPT_START_OF_LINE                        2
+#define CAPT_END_OF_LINE                          3
+#define CAPT_LINE_PAYLOAD                         4
+#define CAPT_GEN_SH_PKT                           5
+
+/* --------------------------------------------------*/
+/* Packet Data Type */
+/* --------------------------------------------------*/
+
+#define CAPT_YUV420_8_DATA                       24   /* 01 1000 YUV420 8-bit                                        */
+#define CAPT_YUV420_10_DATA                      25   /* 01 1001  YUV420 10-bit                                      */
+#define CAPT_YUV420_8L_DATA                      26   /* 01 1010   YUV420 8-bit legacy                               */
+#define CAPT_YUV422_8_DATA                       30   /* 01 1110   YUV422 8-bit                                      */
+#define CAPT_YUV422_10_DATA                      31   /* 01 1111   YUV422 10-bit                                     */
+#define CAPT_RGB444_DATA                         32   /* 10 0000   RGB444                                            */
+#define CAPT_RGB555_DATA						 33   /* 10 0001   RGB555                                            */
+#define CAPT_RGB565_DATA						 34   /* 10 0010   RGB565                                            */
+#define CAPT_RGB666_DATA						 35   /* 10 0011   RGB666                                            */
+#define CAPT_RGB888_DATA						 36   /* 10 0100   RGB888                                            */
+#define CAPT_RAW6_DATA							 40   /* 10 1000   RAW6                                              */
+#define CAPT_RAW7_DATA							 41   /* 10 1001   RAW7                                              */
+#define CAPT_RAW8_DATA							 42   /* 10 1010   RAW8                                              */
+#define CAPT_RAW10_DATA						 43   /* 10 1011   RAW10                                             */
+#define CAPT_RAW12_DATA						 44   /* 10 1100   RAW12                                             */
+#define CAPT_RAW14_DATA						 45   /* 10 1101   RAW14                                             */
+#define CAPT_USR_DEF_1_DATA						 48   /* 11 0000    JPEG [User Defined 8-bit Data Type 1]            */
+#define CAPT_USR_DEF_2_DATA						 49   /* 11 0001    User Defined 8-bit Data Type 2                   */
+#define CAPT_USR_DEF_3_DATA						 50   /* 11 0010    User Defined 8-bit Data Type 3                   */
+#define CAPT_USR_DEF_4_DATA						 51   /* 11 0011    User Defined 8-bit Data Type 4                   */
+#define CAPT_USR_DEF_5_DATA						 52   /* 11 0100    User Defined 8-bit Data Type 5                   */
+#define CAPT_USR_DEF_6_DATA						 53   /* 11 0101    User Defined 8-bit Data Type 6                   */
+#define CAPT_USR_DEF_7_DATA						 54   /* 11 0110    User Defined 8-bit Data Type 7                   */
+#define CAPT_USR_DEF_8_DATA						 55   /* 11 0111    User Defined 8-bit Data Type 8                   */
+#define CAPT_Emb_DATA							 18   /* 01 0010    embedded eight bit non image data                */
+#define CAPT_SOF_DATA							 0   /* 00 0000    frame start                                      */
+#define CAPT_EOF_DATA							 1   /* 00 0001    frame end                                        */
+#define CAPT_SOL_DATA							 2   /* 00 0010    line start                                       */
+#define CAPT_EOL_DATA							 3   /* 00 0011    line end                                         */
+#define CAPT_GEN_SH1_DATA						 8   /* 00 1000  Generic Short Packet Code 1                        */
+#define CAPT_GEN_SH2_DATA						 9   /* 00 1001    Generic Short Packet Code 2                      */
+#define CAPT_GEN_SH3_DATA						 10   /* 00 1010    Generic Short Packet Code 3                      */
+#define CAPT_GEN_SH4_DATA						 11   /* 00 1011    Generic Short Packet Code 4                      */
+#define CAPT_GEN_SH5_DATA						 12   /* 00 1100    Generic Short Packet Code 5                      */
+#define CAPT_GEN_SH6_DATA						 13   /* 00 1101    Generic Short Packet Code 6                      */
+#define CAPT_GEN_SH7_DATA						 14   /* 00 1110    Generic Short Packet Code 7                      */
+#define CAPT_GEN_SH8_DATA						 15   /* 00 1111    Generic Short Packet Code 8                      */
+#define CAPT_YUV420_8_CSPS_DATA					 28   /* 01 1100   YUV420 8-bit (Chroma Shifted Pixel Sampling)      */
+#define CAPT_YUV420_10_CSPS_DATA					 29   /* 01 1101   YUV420 10-bit (Chroma Shifted Pixel Sampling)     */
+#define CAPT_RESERVED_DATA_TYPE_MIN              56
+#define CAPT_RESERVED_DATA_TYPE_MAX              63
+#define CAPT_GEN_LONG_RESERVED_DATA_TYPE_MIN     19
+#define CAPT_GEN_LONG_RESERVED_DATA_TYPE_MAX     23
+#define CAPT_YUV_RESERVED_DATA_TYPE              27
+#define CAPT_RGB_RESERVED_DATA_TYPE_MIN          37
+#define CAPT_RGB_RESERVED_DATA_TYPE_MAX          39
+#define CAPT_RAW_RESERVED_DATA_TYPE_MIN          46
+#define CAPT_RAW_RESERVED_DATA_TYPE_MAX          47
+
+/* --------------------------------------------------*/
+/* Capture Unit State */
+/* --------------------------------------------------*/
+#define CAPT_FREE_RUN                             0
+#define CAPT_NO_SYNC                              1
+#define CAPT_SYNC_SWP                             2
+#define CAPT_SYNC_MWP                             3
+#define CAPT_SYNC_WAIT                            4
+#define CAPT_FREEZE                               5
+#define CAPT_RUN                                  6
+
+/* --------------------------------------------------*/
+
+#endif /* _isp_capture_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/mamoiada_params.h b/drivers/staging/media/atomisp/pci/mamoiada_params.h
new file mode 100644
index 000000000..e18e5f357
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/mamoiada_params.h
@@ -0,0 +1,211 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* Version */
+#define RTL_VERSION
+
+/* instruction pipeline depth */
+#define ISP_BRANCHDELAY                        5
+
+/* bus */
+#define ISP_BUS_WIDTH                          32
+#define ISP_BUS_ADDR_WIDTH                     32
+#define ISP_BUS_BURST_SIZE                     1
+
+/* data-path */
+#define ISP_SCALAR_WIDTH                       32
+#define ISP_SLICE_NELEMS                       4
+#define ISP_VEC_NELEMS                         64
+#define ISP_VEC_ELEMBITS                       14
+#define ISP_VEC_ELEM8BITS                      16
+#define ISP_CLONE_DATAPATH_IS_16               1
+
+/* memories */
+#define ISP_DMEM_DEPTH                         4096
+#define ISP_DMEM_BSEL_DOWNSAMPLE               8
+#define ISP_VMEM_DEPTH                         3072
+#define ISP_VMEM_BSEL_DOWNSAMPLE               8
+#define ISP_VMEM_ELEMBITS                      14
+#define ISP_VMEM_ELEM_PRECISION                14
+#define ISP_PMEM_DEPTH                         2048
+#define ISP_PMEM_WIDTH                         640
+#define ISP_VAMEM_ADDRESS_BITS                 12
+#define ISP_VAMEM_ELEMBITS                     12
+#define ISP_VAMEM_DEPTH                        2048
+#define ISP_VAMEM_ALIGNMENT                    2
+#define ISP_VA_ADDRESS_WIDTH                   896
+#define ISP_VEC_VALSU_LATENCY                  ISP_VEC_NELEMS
+#define ISP_HIST_ADDRESS_BITS                  12
+#define ISP_HIST_ALIGNMENT                     4
+#define ISP_HIST_COMP_IN_PREC                  12
+#define ISP_HIST_DEPTH                         1024
+#define ISP_HIST_WIDTH                         24
+#define ISP_HIST_COMPONENTS                    4
+
+/* program counter */
+#define ISP_PC_WIDTH                           13
+
+/* Template switches */
+#define ISP_SHIELD_INPUT_DMEM                  0
+#define ISP_SHIELD_OUTPUT_DMEM                 1
+#define ISP_SHIELD_INPUT_VMEM                  0
+#define ISP_SHIELD_OUTPUT_VMEM                 0
+#define ISP_SHIELD_INPUT_PMEM                  1
+#define ISP_SHIELD_OUTPUT_PMEM                 1
+#define ISP_SHIELD_INPUT_HIST                  1
+#define ISP_SHIELD_OUTPUT_HIST                 1
+/* When LUT is select the shielding is always on */
+#define ISP_SHIELD_INPUT_VAMEM                 1
+#define ISP_SHIELD_OUTPUT_VAMEM                1
+
+#define ISP_HAS_IRQ                            1
+#define ISP_HAS_SOFT_RESET                     1
+#define ISP_HAS_VEC_DIV                        0
+#define ISP_HAS_VFU_W_2O                       1
+#define ISP_HAS_DEINT3                         1
+#define ISP_HAS_LUT                            1
+#define ISP_HAS_HIST                           1
+#define ISP_HAS_VALSU                          1
+#define ISP_HAS_3rdVALSU                       1
+#define ISP_VRF1_HAS_2P                        1
+
+#define ISP_SRU_GUARDING                       1
+#define ISP_VLSU_GUARDING                      1
+
+#define ISP_VRF_RAM			     1
+#define ISP_SRF_RAM			     1
+
+#define ISP_SPLIT_VMUL_VADD_IS                 0
+#define ISP_RFSPLIT_FPGA                       0
+
+/* RSN or Bus pipelining */
+#define ISP_RSN_PIPE                           1
+#define ISP_VSF_BUS_PIPE                       0
+
+/* extra slave port to vmem */
+#define ISP_IF_VMEM                            0
+#define ISP_GDC_VMEM                           0
+
+/* Streaming ports */
+#define ISP_IF                                 1
+#define ISP_IF_B                               1
+#define ISP_GDC                                1
+#define ISP_SCL                                1
+#define ISP_GPFIFO                             1
+#define ISP_SP                                 1
+
+/* Removing Issue Slot(s) */
+#define ISP_HAS_NOT_SIMD_IS2                   0
+#define ISP_HAS_NOT_SIMD_IS3                   0
+#define ISP_HAS_NOT_SIMD_IS4                   0
+#define ISP_HAS_NOT_SIMD_IS4_VADD              0
+#define ISP_HAS_NOT_SIMD_IS5                   0
+#define ISP_HAS_NOT_SIMD_IS6                   0
+#define ISP_HAS_NOT_SIMD_IS7                   0
+#define ISP_HAS_NOT_SIMD_IS8                   0
+
+/* ICache  */
+#define ISP_ICACHE                             1
+#define ISP_ICACHE_ONLY                        0
+#define ISP_ICACHE_PREFETCH                    1
+#define ISP_ICACHE_INDEX_BITS                  8
+#define ISP_ICACHE_SET_BITS                    5
+#define ISP_ICACHE_BLOCKS_PER_SET_BITS         1
+
+/* Experimental Flags */
+#define ISP_EXP_1                              0
+#define ISP_EXP_2                              0
+#define ISP_EXP_3                              0
+#define ISP_EXP_4                              0
+#define ISP_EXP_5                              0
+#define ISP_EXP_6                              0
+
+/* Derived values */
+#define ISP_LOG2_PMEM_WIDTH                    10
+#define ISP_VEC_WIDTH                          896
+#define ISP_SLICE_WIDTH                        56
+#define ISP_VMEM_WIDTH                         896
+#define ISP_VMEM_ALIGN                         128
+#define ISP_SIMDLSU                            1
+#define ISP_LSU_IMM_BITS                       12
+
+/* convenient shortcuts for software*/
+#define ISP_NWAY                               ISP_VEC_NELEMS
+#define NBITS                                  ISP_VEC_ELEMBITS
+
+#define _isp_ceil_div(a, b)                     (((a) + (b) - 1) / (b))
+
+#define ISP_VEC_ALIGN                          ISP_VMEM_ALIGN
+
+/* register file sizes */
+#define ISP_RF0_SIZE        64
+#define ISP_RF1_SIZE        16
+#define ISP_RF2_SIZE        64
+#define ISP_RF3_SIZE        4
+#define ISP_RF4_SIZE        64
+#define ISP_RF5_SIZE        16
+#define ISP_RF6_SIZE        16
+#define ISP_RF7_SIZE        16
+#define ISP_RF8_SIZE        16
+#define ISP_RF9_SIZE        16
+#define ISP_RF10_SIZE       16
+#define ISP_RF11_SIZE       16
+
+#define ISP_SRF1_SIZE       4
+#define ISP_SRF2_SIZE       64
+#define ISP_SRF3_SIZE       64
+#define ISP_SRF4_SIZE       32
+#define ISP_SRF5_SIZE       64
+#define ISP_FRF0_SIZE       16
+#define ISP_FRF1_SIZE       4
+#define ISP_FRF2_SIZE       16
+#define ISP_FRF3_SIZE       4
+#define ISP_FRF4_SIZE       4
+#define ISP_FRF5_SIZE       8
+#define ISP_FRF6_SIZE       4
+/* register file read latency */
+#define ISP_VRF1_READ_LAT       1
+#define ISP_VRF2_READ_LAT       1
+#define ISP_VRF3_READ_LAT       1
+#define ISP_VRF4_READ_LAT       1
+#define ISP_VRF5_READ_LAT       1
+#define ISP_VRF6_READ_LAT       1
+#define ISP_VRF7_READ_LAT       1
+#define ISP_VRF8_READ_LAT       1
+#define ISP_SRF1_READ_LAT       1
+#define ISP_SRF2_READ_LAT       1
+#define ISP_SRF3_READ_LAT       1
+#define ISP_SRF4_READ_LAT       1
+#define ISP_SRF5_READ_LAT       1
+#define ISP_SRF5_READ_LAT       1
+/* immediate sizes */
+#define ISP_IS1_IMM_BITS        14
+#define ISP_IS2_IMM_BITS        13
+#define ISP_IS3_IMM_BITS        14
+#define ISP_IS4_IMM_BITS        14
+#define ISP_IS5_IMM_BITS        9
+#define ISP_IS6_IMM_BITS        16
+#define ISP_IS7_IMM_BITS        9
+#define ISP_IS8_IMM_BITS        16
+#define ISP_IS9_IMM_BITS        11
+/* fifo depths */
+#define ISP_IF_FIFO_DEPTH         0
+#define ISP_IF_B_FIFO_DEPTH       0
+#define ISP_DMA_FIFO_DEPTH        0
+#define ISP_OF_FIFO_DEPTH         0
+#define ISP_GDC_FIFO_DEPTH        0
+#define ISP_SCL_FIFO_DEPTH        0
+#define ISP_GPFIFO_FIFO_DEPTH     0
+#define ISP_SP_FIFO_DEPTH         0
diff --git a/drivers/staging/media/atomisp/pci/mmu/isp_mmu.c b/drivers/staging/media/atomisp/pci/mmu/isp_mmu.c
new file mode 100644
index 000000000..72287de75
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/mmu/isp_mmu.c
@@ -0,0 +1,567 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Medifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ *
+ */
+/*
+ * ISP MMU management wrap code
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/gfp.h>
+#include <linux/mm.h>		/* for GFP_ATOMIC */
+#include <linux/slab.h>		/* for kmalloc */
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/sizes.h>
+
+#ifdef CONFIG_X86
+#include <asm/set_memory.h>
+#endif
+
+#include "atomisp_internal.h"
+#include "mmu/isp_mmu.h"
+
+/*
+ * 64-bit x86 processor physical address layout:
+ * 0		- 0x7fffffff		DDR RAM	(2GB)
+ * 0x80000000	- 0xffffffff		MMIO	(2GB)
+ * 0x100000000	- 0x3fffffffffff	DDR RAM	(64TB)
+ * So if the system has more than 2GB DDR memory, the lower 2GB occupies the
+ * physical address 0 - 0x7fffffff and the rest will start from 0x100000000.
+ * We have to make sure memory is allocated from the lower 2GB for devices
+ * that are only 32-bit capable(e.g. the ISP MMU).
+ *
+ * For any confusion, contact bin.gao@intel.com.
+ */
+#define NR_PAGES_2GB	(SZ_2G / PAGE_SIZE)
+
+static void free_mmu_map(struct isp_mmu *mmu, unsigned int start_isp_virt,
+			 unsigned int end_isp_virt);
+
+static unsigned int atomisp_get_pte(phys_addr_t pt, unsigned int idx)
+{
+	unsigned int *pt_virt = phys_to_virt(pt);
+
+	return *(pt_virt + idx);
+}
+
+static void atomisp_set_pte(phys_addr_t pt,
+			    unsigned int idx, unsigned int pte)
+{
+	unsigned int *pt_virt = phys_to_virt(pt);
+	*(pt_virt + idx) = pte;
+}
+
+static void *isp_pt_phys_to_virt(phys_addr_t phys)
+{
+	return phys_to_virt(phys);
+}
+
+static phys_addr_t isp_pte_to_pgaddr(struct isp_mmu *mmu,
+				     unsigned int pte)
+{
+	return mmu->driver->pte_to_phys(mmu, pte);
+}
+
+static unsigned int isp_pgaddr_to_pte_valid(struct isp_mmu *mmu,
+	phys_addr_t phys)
+{
+	unsigned int pte = mmu->driver->phys_to_pte(mmu, phys);
+
+	return (unsigned int)(pte | ISP_PTE_VALID_MASK(mmu));
+}
+
+/*
+ * allocate a uncacheable page table.
+ * return physical address.
+ */
+static phys_addr_t alloc_page_table(struct isp_mmu *mmu)
+{
+	int i;
+	phys_addr_t page;
+	void *virt;
+
+	virt = (void *)__get_free_page(GFP_KERNEL | GFP_DMA32);
+
+	if (!virt)
+		return (phys_addr_t)NULL_PAGE;
+
+	/*
+	 * we need a uncacheable page table.
+	 */
+#ifdef	CONFIG_X86
+	set_memory_uc((unsigned long)virt, 1);
+#endif
+
+	page = virt_to_phys(virt);
+
+	for (i = 0; i < 1024; i++) {
+		/* NEED CHECK */
+		atomisp_set_pte(page, i, mmu->driver->null_pte);
+	}
+
+	return page;
+}
+
+static void free_page_table(struct isp_mmu *mmu, phys_addr_t page)
+{
+	void *virt;
+
+	page &= ISP_PAGE_MASK;
+	/*
+	 * reset the page to write back before free
+	 */
+	virt = phys_to_virt(page);
+
+#ifdef	CONFIG_X86
+	set_memory_wb((unsigned long)virt, 1);
+#endif
+
+	free_page((unsigned long)virt);
+}
+
+static void mmu_remap_error(struct isp_mmu *mmu,
+			    phys_addr_t l1_pt, unsigned int l1_idx,
+			    phys_addr_t l2_pt, unsigned int l2_idx,
+			    unsigned int isp_virt, phys_addr_t old_phys,
+			    phys_addr_t new_phys)
+{
+	dev_err(atomisp_dev, "address remap:\n\n"
+		"\tL1 PT: virt = %p, phys = 0x%llx, idx = %d\n"
+		"\tL2 PT: virt = %p, phys = 0x%llx, idx = %d\n"
+		"\told: isp_virt = 0x%x, phys = 0x%llx\n"
+		"\tnew: isp_virt = 0x%x, phys = 0x%llx\n",
+		isp_pt_phys_to_virt(l1_pt),
+		(u64)l1_pt, l1_idx,
+		isp_pt_phys_to_virt(l2_pt),
+		(u64)l2_pt, l2_idx, isp_virt,
+		(u64)old_phys, isp_virt,
+		(u64)new_phys);
+}
+
+static void mmu_unmap_l2_pte_error(struct isp_mmu *mmu,
+				   phys_addr_t l1_pt, unsigned int l1_idx,
+				   phys_addr_t l2_pt, unsigned int l2_idx,
+				   unsigned int isp_virt, unsigned int pte)
+{
+	dev_err(atomisp_dev, "unmap invalid L2 pte:\n\n"
+		"\tL1 PT: virt = %p, phys = 0x%llx, idx = %d\n"
+		"\tL2 PT: virt = %p, phys = 0x%llx, idx = %d\n"
+		"\tisp_virt = 0x%x, pte(page phys) = 0x%x\n",
+		isp_pt_phys_to_virt(l1_pt),
+		(u64)l1_pt, l1_idx,
+		isp_pt_phys_to_virt(l2_pt),
+		(u64)l2_pt, l2_idx, isp_virt,
+		pte);
+}
+
+static void mmu_unmap_l1_pte_error(struct isp_mmu *mmu,
+				   phys_addr_t l1_pt, unsigned int l1_idx,
+				   unsigned int isp_virt, unsigned int pte)
+{
+	dev_err(atomisp_dev, "unmap invalid L1 pte (L2 PT):\n\n"
+		"\tL1 PT: virt = %p, phys = 0x%llx, idx = %d\n"
+		"\tisp_virt = 0x%x, l1_pte(L2 PT) = 0x%x\n",
+		isp_pt_phys_to_virt(l1_pt),
+		(u64)l1_pt, l1_idx, (unsigned int)isp_virt,
+		pte);
+}
+
+static void mmu_unmap_l1_pt_error(struct isp_mmu *mmu, unsigned int pte)
+{
+	dev_err(atomisp_dev, "unmap invalid L1PT:\n\n"
+		"L1PT = 0x%x\n", (unsigned int)pte);
+}
+
+/*
+ * Update L2 page table according to isp virtual address and page physical
+ * address
+ */
+static int mmu_l2_map(struct isp_mmu *mmu, phys_addr_t l1_pt,
+		      unsigned int l1_idx, phys_addr_t l2_pt,
+		      unsigned int start, unsigned int end, phys_addr_t phys)
+{
+	unsigned int ptr;
+	unsigned int idx;
+	unsigned int pte;
+
+	l2_pt &= ISP_PAGE_MASK;
+
+	start = start & ISP_PAGE_MASK;
+	end = ISP_PAGE_ALIGN(end);
+	phys &= ISP_PAGE_MASK;
+
+	ptr = start;
+	do {
+		idx = ISP_PTR_TO_L2_IDX(ptr);
+
+		pte = atomisp_get_pte(l2_pt, idx);
+
+		if (ISP_PTE_VALID(mmu, pte)) {
+			mmu_remap_error(mmu, l1_pt, l1_idx,
+					l2_pt, idx, ptr, pte, phys);
+
+			/* free all mapped pages */
+			free_mmu_map(mmu, start, ptr);
+
+			return -EINVAL;
+		}
+
+		pte = isp_pgaddr_to_pte_valid(mmu, phys);
+
+		atomisp_set_pte(l2_pt, idx, pte);
+		mmu->l2_pgt_refcount[l1_idx]++;
+		ptr += (1U << ISP_L2PT_OFFSET);
+		phys += (1U << ISP_L2PT_OFFSET);
+	} while (ptr < end && idx < ISP_L2PT_PTES - 1);
+
+	return 0;
+}
+
+/*
+ * Update L1 page table according to isp virtual address and page physical
+ * address
+ */
+static int mmu_l1_map(struct isp_mmu *mmu, phys_addr_t l1_pt,
+		      unsigned int start, unsigned int end,
+		      phys_addr_t phys)
+{
+	phys_addr_t l2_pt;
+	unsigned int ptr, l1_aligned;
+	unsigned int idx;
+	unsigned int l2_pte;
+	int ret;
+
+	l1_pt &= ISP_PAGE_MASK;
+
+	start = start & ISP_PAGE_MASK;
+	end = ISP_PAGE_ALIGN(end);
+	phys &= ISP_PAGE_MASK;
+
+	ptr = start;
+	do {
+		idx = ISP_PTR_TO_L1_IDX(ptr);
+
+		l2_pte = atomisp_get_pte(l1_pt, idx);
+
+		if (!ISP_PTE_VALID(mmu, l2_pte)) {
+			l2_pt = alloc_page_table(mmu);
+			if (l2_pt == NULL_PAGE) {
+				dev_err(atomisp_dev,
+					"alloc page table fail.\n");
+
+				/* free all mapped pages */
+				free_mmu_map(mmu, start, ptr);
+
+				return -ENOMEM;
+			}
+
+			l2_pte = isp_pgaddr_to_pte_valid(mmu, l2_pt);
+
+			atomisp_set_pte(l1_pt, idx, l2_pte);
+			mmu->l2_pgt_refcount[idx] = 0;
+		}
+
+		l2_pt = isp_pte_to_pgaddr(mmu, l2_pte);
+
+		l1_aligned = (ptr & ISP_PAGE_MASK) + (1U << ISP_L1PT_OFFSET);
+
+		if (l1_aligned < end) {
+			ret = mmu_l2_map(mmu, l1_pt, idx,
+					 l2_pt, ptr, l1_aligned, phys);
+			phys += (l1_aligned - ptr);
+			ptr = l1_aligned;
+		} else {
+			ret = mmu_l2_map(mmu, l1_pt, idx,
+					 l2_pt, ptr, end, phys);
+			phys += (end - ptr);
+			ptr = end;
+		}
+
+		if (ret) {
+			dev_err(atomisp_dev, "setup mapping in L2PT fail.\n");
+
+			/* free all mapped pages */
+			free_mmu_map(mmu, start, ptr);
+
+			return -EINVAL;
+		}
+	} while (ptr < end && idx < ISP_L1PT_PTES);
+
+	return 0;
+}
+
+/*
+ * Update page table according to isp virtual address and page physical
+ * address
+ */
+static int mmu_map(struct isp_mmu *mmu, unsigned int isp_virt,
+		   phys_addr_t phys, unsigned int pgnr)
+{
+	unsigned int start, end;
+	phys_addr_t l1_pt;
+	int ret;
+
+	mutex_lock(&mmu->pt_mutex);
+	if (!ISP_PTE_VALID(mmu, mmu->l1_pte)) {
+		/*
+		 * allocate 1 new page for L1 page table
+		 */
+		l1_pt = alloc_page_table(mmu);
+		if (l1_pt == NULL_PAGE) {
+			dev_err(atomisp_dev, "alloc page table fail.\n");
+			mutex_unlock(&mmu->pt_mutex);
+			return -ENOMEM;
+		}
+
+		/*
+		 * setup L1 page table physical addr to MMU
+		 */
+		mmu->base_address = l1_pt;
+		mmu->l1_pte = isp_pgaddr_to_pte_valid(mmu, l1_pt);
+		memset(mmu->l2_pgt_refcount, 0, sizeof(int) * ISP_L1PT_PTES);
+	}
+
+	l1_pt = isp_pte_to_pgaddr(mmu, mmu->l1_pte);
+
+	start = (isp_virt) & ISP_PAGE_MASK;
+	end = start + (pgnr << ISP_PAGE_OFFSET);
+	phys &= ISP_PAGE_MASK;
+
+	ret = mmu_l1_map(mmu, l1_pt, start, end, phys);
+
+	if (ret)
+		dev_err(atomisp_dev, "setup mapping in L1PT fail.\n");
+
+	mutex_unlock(&mmu->pt_mutex);
+	return ret;
+}
+
+/*
+ * Free L2 page table according to isp virtual address and page physical
+ * address
+ */
+static void mmu_l2_unmap(struct isp_mmu *mmu, phys_addr_t l1_pt,
+			 unsigned int l1_idx, phys_addr_t l2_pt,
+			 unsigned int start, unsigned int end)
+{
+	unsigned int ptr;
+	unsigned int idx;
+	unsigned int pte;
+
+	l2_pt &= ISP_PAGE_MASK;
+
+	start = start & ISP_PAGE_MASK;
+	end = ISP_PAGE_ALIGN(end);
+
+	ptr = start;
+	do {
+		idx = ISP_PTR_TO_L2_IDX(ptr);
+
+		pte = atomisp_get_pte(l2_pt, idx);
+
+		if (!ISP_PTE_VALID(mmu, pte))
+			mmu_unmap_l2_pte_error(mmu, l1_pt, l1_idx,
+					       l2_pt, idx, ptr, pte);
+
+		atomisp_set_pte(l2_pt, idx, mmu->driver->null_pte);
+		mmu->l2_pgt_refcount[l1_idx]--;
+		ptr += (1U << ISP_L2PT_OFFSET);
+	} while (ptr < end && idx < ISP_L2PT_PTES - 1);
+
+	if (mmu->l2_pgt_refcount[l1_idx] == 0) {
+		free_page_table(mmu, l2_pt);
+		atomisp_set_pte(l1_pt, l1_idx, mmu->driver->null_pte);
+	}
+}
+
+/*
+ * Free L1 page table according to isp virtual address and page physical
+ * address
+ */
+static void mmu_l1_unmap(struct isp_mmu *mmu, phys_addr_t l1_pt,
+			 unsigned int start, unsigned int end)
+{
+	phys_addr_t l2_pt;
+	unsigned int ptr, l1_aligned;
+	unsigned int idx;
+	unsigned int l2_pte;
+
+	l1_pt &= ISP_PAGE_MASK;
+
+	start = start & ISP_PAGE_MASK;
+	end = ISP_PAGE_ALIGN(end);
+
+	ptr = start;
+	do {
+		idx = ISP_PTR_TO_L1_IDX(ptr);
+
+		l2_pte = atomisp_get_pte(l1_pt, idx);
+
+		if (!ISP_PTE_VALID(mmu, l2_pte)) {
+			mmu_unmap_l1_pte_error(mmu, l1_pt, idx, ptr, l2_pte);
+			continue;
+		}
+
+		l2_pt = isp_pte_to_pgaddr(mmu, l2_pte);
+
+		l1_aligned = (ptr & ISP_PAGE_MASK) + (1U << ISP_L1PT_OFFSET);
+
+		if (l1_aligned < end) {
+			mmu_l2_unmap(mmu, l1_pt, idx, l2_pt, ptr, l1_aligned);
+			ptr = l1_aligned;
+		} else {
+			mmu_l2_unmap(mmu, l1_pt, idx, l2_pt, ptr, end);
+			ptr = end;
+		}
+		/*
+		 * use the same L2 page next time, so we don't
+		 * need to invalidate and free this PT.
+		 */
+		/*      atomisp_set_pte(l1_pt, idx, NULL_PTE); */
+	} while (ptr < end && idx < ISP_L1PT_PTES);
+}
+
+/*
+ * Free page table according to isp virtual address and page physical
+ * address
+ */
+static void mmu_unmap(struct isp_mmu *mmu, unsigned int isp_virt,
+		      unsigned int pgnr)
+{
+	unsigned int start, end;
+	phys_addr_t l1_pt;
+
+	mutex_lock(&mmu->pt_mutex);
+	if (!ISP_PTE_VALID(mmu, mmu->l1_pte)) {
+		mmu_unmap_l1_pt_error(mmu, mmu->l1_pte);
+		mutex_unlock(&mmu->pt_mutex);
+		return;
+	}
+
+	l1_pt = isp_pte_to_pgaddr(mmu, mmu->l1_pte);
+
+	start = (isp_virt) & ISP_PAGE_MASK;
+	end = start + (pgnr << ISP_PAGE_OFFSET);
+
+	mmu_l1_unmap(mmu, l1_pt, start, end);
+	mutex_unlock(&mmu->pt_mutex);
+}
+
+/*
+ * Free page tables according to isp start virtual address and end virtual
+ * address.
+ */
+static void free_mmu_map(struct isp_mmu *mmu, unsigned int start_isp_virt,
+			 unsigned int end_isp_virt)
+{
+	unsigned int pgnr;
+	unsigned int start, end;
+
+	start = (start_isp_virt) & ISP_PAGE_MASK;
+	end = (end_isp_virt) & ISP_PAGE_MASK;
+	pgnr = (end - start) >> ISP_PAGE_OFFSET;
+	mmu_unmap(mmu, start, pgnr);
+}
+
+int isp_mmu_map(struct isp_mmu *mmu, unsigned int isp_virt,
+		phys_addr_t phys, unsigned int pgnr)
+{
+	return mmu_map(mmu, isp_virt, phys, pgnr);
+}
+
+void isp_mmu_unmap(struct isp_mmu *mmu, unsigned int isp_virt,
+		   unsigned int pgnr)
+{
+	mmu_unmap(mmu, isp_virt, pgnr);
+}
+
+static void isp_mmu_flush_tlb_range_default(struct isp_mmu *mmu,
+	unsigned int start,
+	unsigned int size)
+{
+	isp_mmu_flush_tlb(mmu);
+}
+
+/*MMU init for internal structure*/
+int isp_mmu_init(struct isp_mmu *mmu, struct isp_mmu_client *driver)
+{
+	if (!mmu)		/* error */
+		return -EINVAL;
+	if (!driver)		/* error */
+		return -EINVAL;
+
+	if (!driver->name)
+		dev_warn(atomisp_dev, "NULL name for MMU driver...\n");
+
+	mmu->driver = driver;
+
+	if (!driver->tlb_flush_all) {
+		dev_err(atomisp_dev, "tlb_flush_all operation not provided.\n");
+		return -EINVAL;
+	}
+
+	if (!driver->tlb_flush_range)
+		driver->tlb_flush_range = isp_mmu_flush_tlb_range_default;
+
+	if (!driver->pte_valid_mask) {
+		dev_err(atomisp_dev, "PTE_MASK is missing from mmu driver\n");
+		return -EINVAL;
+	}
+
+	mmu->l1_pte = driver->null_pte;
+
+	mutex_init(&mmu->pt_mutex);
+
+	return 0;
+}
+
+/*Free L1 and L2 page table*/
+void isp_mmu_exit(struct isp_mmu *mmu)
+{
+	unsigned int idx;
+	unsigned int pte;
+	phys_addr_t l1_pt, l2_pt;
+
+	if (!mmu)
+		return;
+
+	if (!ISP_PTE_VALID(mmu, mmu->l1_pte)) {
+		dev_warn(atomisp_dev, "invalid L1PT: pte = 0x%x\n",
+			 (unsigned int)mmu->l1_pte);
+		return;
+	}
+
+	l1_pt = isp_pte_to_pgaddr(mmu, mmu->l1_pte);
+
+	for (idx = 0; idx < ISP_L1PT_PTES; idx++) {
+		pte = atomisp_get_pte(l1_pt, idx);
+
+		if (ISP_PTE_VALID(mmu, pte)) {
+			l2_pt = isp_pte_to_pgaddr(mmu, pte);
+
+			free_page_table(mmu, l2_pt);
+		}
+	}
+
+	free_page_table(mmu, l1_pt);
+}
diff --git a/drivers/staging/media/atomisp/pci/mmu/sh_mmu_mrfld.c b/drivers/staging/media/atomisp/pci/mmu/sh_mmu_mrfld.c
new file mode 100644
index 000000000..0fbb361f5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/mmu/sh_mmu_mrfld.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Merrifield PNW Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2012 Intel Corporation. All Rights Reserved.
+ *
+ * Copyright (c) 2012 Silicon Hive www.siliconhive.com.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * 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 "type_support.h"
+#include "mmu/isp_mmu.h"
+#include "mmu/sh_mmu_mrfld.h"
+#include "atomisp_compat.h"
+
+#define MERR_VALID_PTE_MASK	0x80000000
+
+/*
+ * include SH header file here
+ */
+
+static unsigned int sh_phys_to_pte(struct isp_mmu *mmu,
+				   phys_addr_t phys)
+{
+	return phys >> ISP_PAGE_OFFSET;
+}
+
+static phys_addr_t sh_pte_to_phys(struct isp_mmu *mmu,
+				  unsigned int pte)
+{
+	unsigned int mask = mmu->driver->pte_valid_mask;
+
+	return (phys_addr_t)((pte & ~mask) << ISP_PAGE_OFFSET);
+}
+
+static unsigned int sh_get_pd_base(struct isp_mmu *mmu,
+				   phys_addr_t phys)
+{
+	unsigned int pte = sh_phys_to_pte(mmu, phys);
+
+	return HOST_ADDRESS(pte);
+}
+
+/*
+ * callback to flush tlb.
+ *
+ * tlb_flush_range will at least flush TLBs containing
+ * address mapping from addr to addr + size.
+ *
+ * tlb_flush_all will flush all TLBs.
+ *
+ * tlb_flush_all is must be provided. if tlb_flush_range is
+ * not valid, it will set to tlb_flush_all by default.
+ */
+static void sh_tlb_flush(struct isp_mmu *mmu)
+{
+	ia_css_mmu_invalidate_cache();
+}
+
+struct isp_mmu_client sh_mmu_mrfld = {
+	.name = "Silicon Hive ISP3000 MMU",
+	.pte_valid_mask = MERR_VALID_PTE_MASK,
+	.null_pte = ~MERR_VALID_PTE_MASK,
+	.get_pd_base = sh_get_pd_base,
+	.tlb_flush_all = sh_tlb_flush,
+	.phys_to_pte = sh_phys_to_pte,
+	.pte_to_phys = sh_pte_to_phys,
+};
diff --git a/drivers/staging/media/atomisp/pci/mmu_defs.h b/drivers/staging/media/atomisp/pci/mmu_defs.h
new file mode 100644
index 000000000..03cfb5833
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/mmu_defs.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _mmu_defs_h
+#define _mmu_defs_h
+
+#define _HRT_MMU_INVALIDATE_TLB_REG_IDX          0
+#define _HRT_MMU_PAGE_TABLE_BASE_ADDRESS_REG_IDX 1
+
+#define _HRT_MMU_REG_ALIGN 4
+
+#endif /* _mmu_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/runtime/binary/interface/ia_css_binary.h b/drivers/staging/media/atomisp/pci/runtime/binary/interface/ia_css_binary.h
new file mode 100644
index 000000000..9935ac860
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/binary/interface/ia_css_binary.h
@@ -0,0 +1,225 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef _IA_CSS_BINARY_H_
+#define _IA_CSS_BINARY_H_
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_err.h"
+#include "ia_css_stream_format.h"
+#include "ia_css_stream_public.h"
+#include "ia_css_frame_public.h"
+#include "sh_css_metrics.h"
+#include "isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h"
+
+/* The binary mode is used in pre-processor expressions so we cannot
+ * use an enum here. */
+#define IA_CSS_BINARY_MODE_COPY       0
+#define IA_CSS_BINARY_MODE_PREVIEW    1
+#define IA_CSS_BINARY_MODE_PRIMARY    2
+#define IA_CSS_BINARY_MODE_VIDEO      3
+#define IA_CSS_BINARY_MODE_PRE_ISP    4
+#define IA_CSS_BINARY_MODE_GDC        5
+#define IA_CSS_BINARY_MODE_POST_ISP   6
+#define IA_CSS_BINARY_MODE_ANR        7
+#define IA_CSS_BINARY_MODE_CAPTURE_PP 8
+#define IA_CSS_BINARY_MODE_VF_PP      9
+#define IA_CSS_BINARY_MODE_PRE_DE     10
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE0    11
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE1    12
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE2    13
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE3    14
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE4    15
+#define IA_CSS_BINARY_MODE_PRIMARY_HQ_STAGE5    16
+#define IA_CSS_BINARY_NUM_MODES       17
+
+#define MAX_NUM_PRIMARY_STAGES 6
+#define NUM_PRIMARY_HQ_STAGES  6  /* number of primary stages for ISP2.6.1 high quality pipe */
+#define NUM_PRIMARY_STAGES     1  /* number of primary satges for ISP1/ISP2.2 pipe */
+
+/* Indicate where binaries can read input from */
+#define IA_CSS_BINARY_INPUT_SENSOR   0
+#define IA_CSS_BINARY_INPUT_MEMORY   1
+#define IA_CSS_BINARY_INPUT_VARIABLE 2
+
+/* Should be included without the path.
+   However, that requires adding the path to numerous makefiles
+   that have nothing to do with isp parameters.
+ */
+#include "runtime/isp_param/interface/ia_css_isp_param_types.h"
+
+/* now these ports only include output ports but not vf output ports */
+enum {
+	IA_CSS_BINARY_OUTPUT_PORT_0 = 0,
+	IA_CSS_BINARY_OUTPUT_PORT_1 = 1,
+	IA_CSS_BINARY_MAX_OUTPUT_PORTS = 2
+};
+
+struct ia_css_cas_binary_descr {
+	unsigned int num_stage;
+	unsigned int num_output_stage;
+	struct ia_css_frame_info *in_info;
+	struct ia_css_frame_info *internal_out_info;
+	struct ia_css_frame_info *out_info;
+	struct ia_css_frame_info *vf_info;
+	bool *is_output_stage;
+};
+
+struct ia_css_binary_descr {
+	int mode;
+	bool online;
+	bool continuous;
+	bool striped;
+	bool two_ppc;
+	bool enable_yuv_ds;
+	bool enable_high_speed;
+	bool enable_dvs_6axis;
+	bool enable_reduced_pipe;
+	bool enable_dz;
+	bool enable_xnr;
+	bool enable_fractional_ds;
+	bool enable_dpc;
+
+	/* ISP2401 */
+	bool enable_tnr;
+
+	bool enable_capture_pp_bli;
+	struct ia_css_resolution dvs_env;
+	enum atomisp_input_format stream_format;
+	struct ia_css_frame_info *in_info;		/* the info of the input-frame with the
+							   ISP required resolution. */
+	struct ia_css_frame_info *bds_out_info;
+	struct ia_css_frame_info *out_info[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame_info *vf_info;
+	unsigned int isp_pipe_version;
+	unsigned int required_bds_factor;
+	int stream_config_left_padding;
+};
+
+struct ia_css_binary {
+	const struct ia_css_binary_xinfo *info;
+	enum atomisp_input_format input_format;
+	struct ia_css_frame_info in_frame_info;
+	struct ia_css_frame_info internal_frame_info;
+	struct ia_css_frame_info out_frame_info[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_resolution effective_in_frame_res;
+	struct ia_css_frame_info vf_frame_info;
+	int                      input_buf_vectors;
+	int                      deci_factor_log2;
+	int                      vf_downscale_log2;
+	int                      s3atbl_width;
+	int                      s3atbl_height;
+	int                      s3atbl_isp_width;
+	int                      s3atbl_isp_height;
+	unsigned int             morph_tbl_width;
+	unsigned int             morph_tbl_aligned_width;
+	unsigned int             morph_tbl_height;
+	int                      sctbl_width_per_color;
+	int                      sctbl_aligned_width_per_color;
+	int                      sctbl_height;
+	struct ia_css_sdis_info	 dis;
+	struct ia_css_resolution dvs_envelope;
+	bool                     online;
+	unsigned int             uds_xc;
+	unsigned int             uds_yc;
+	unsigned int             left_padding;
+	struct sh_css_binary_metrics metrics;
+	struct ia_css_isp_param_host_segments mem_params;
+	struct ia_css_isp_param_css_segments  css_params;
+};
+
+#define IA_CSS_BINARY_DEFAULT_SETTINGS { \
+	.input_format		= ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY, \
+	.in_frame_info		= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.internal_frame_info	= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	.out_frame_info		= {IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	.vf_frame_info		= IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+}
+
+int
+ia_css_binary_init_infos(void);
+
+int
+ia_css_binary_uninit(void);
+
+int
+ia_css_binary_fill_info(const struct ia_css_binary_xinfo *xinfo,
+			bool online,
+			bool two_ppc,
+			enum atomisp_input_format stream_format,
+			const struct ia_css_frame_info *in_info,
+			const struct ia_css_frame_info *bds_out_info,
+			const struct ia_css_frame_info *out_info[],
+			const struct ia_css_frame_info *vf_info,
+			struct ia_css_binary *binary,
+			struct ia_css_resolution *dvs_env,
+			int stream_config_left_padding,
+			bool accelerator);
+
+int
+ia_css_binary_find(struct ia_css_binary_descr *descr,
+		   struct ia_css_binary *binary);
+
+/* @brief Get the shading information of the specified shading correction type.
+ *
+ * @param[in] binary: The isp binary which has the shading correction.
+ * @param[in] type: The shading correction type.
+ * @param[in] required_bds_factor: The bayer downscaling factor required in the pipe.
+ * @param[in] stream_config: The stream configuration.
+ * @param[out] shading_info: The shading information.
+ *		The shading information necessary as API is stored in the shading_info.
+ *		The driver needs to get this information to generate
+ *		the shading table directly required from ISP.
+ * @param[out] pipe_config: The pipe configuration.
+ *		The shading information related to ISP (but, not necessary as API) is stored in the pipe_config.
+ * @return	0 or error code upon error.
+ *
+ */
+int
+ia_css_binary_get_shading_info(const struct ia_css_binary *binary,
+			       enum ia_css_shading_correction_type type,
+			       unsigned int required_bds_factor,
+			       const struct ia_css_stream_config *stream_config,
+			       struct ia_css_shading_info *shading_info,
+			       struct ia_css_pipe_config *pipe_config);
+
+int
+ia_css_binary_3a_grid_info(const struct ia_css_binary *binary,
+			   struct ia_css_grid_info *info,
+			   struct ia_css_pipe *pipe);
+
+void
+ia_css_binary_dvs_grid_info(const struct ia_css_binary *binary,
+			    struct ia_css_grid_info *info,
+			    struct ia_css_pipe *pipe);
+
+void
+ia_css_binary_dvs_stat_grid_info(
+    const struct ia_css_binary *binary,
+    struct ia_css_grid_info *info,
+    struct ia_css_pipe *pipe);
+
+unsigned
+ia_css_binary_max_vf_width(void);
+
+void
+ia_css_binary_destroy_isp_parameters(struct ia_css_binary *binary);
+
+void
+ia_css_binary_get_isp_binaries(struct ia_css_binary_xinfo **binaries,
+			       uint32_t *num_isp_binaries);
+
+#endif /* _IA_CSS_BINARY_H_ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/binary/src/binary.c b/drivers/staging/media/atomisp/pci/runtime/binary/src/binary.c
new file mode 100644
index 000000000..406ed5fb4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/binary/src/binary.c
@@ -0,0 +1,1375 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <math_support.h>
+#include <gdc_device.h>	/* HR_GDC_N */
+
+#include "hmm.h"
+
+#include "isp.h"	/* ISP_VEC_NELEMS */
+
+#include "ia_css_binary.h"
+#include "ia_css_debug.h"
+#include "ia_css_util.h"
+#include "ia_css_isp_param.h"
+#include "sh_css_internal.h"
+#include "sh_css_sp.h"
+#include "sh_css_firmware.h"
+#include "sh_css_defs.h"
+#include "sh_css_legacy.h"
+
+#include "atomisp_internal.h"
+
+#include "vf/vf_1.0/ia_css_vf.host.h"
+#include "sc/sc_1.0/ia_css_sc.host.h"
+#include "sdis/sdis_1.0/ia_css_sdis.host.h"
+#include "fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h"	/* FRAC_ACC */
+
+#include "camera/pipe/interface/ia_css_pipe_binarydesc.h"
+
+#include "assert_support.h"
+
+#define IMPLIES(a, b)           (!(a) || (b))   /* A => B */
+
+static struct ia_css_binary_xinfo *all_binaries; /* ISP binaries only (no SP) */
+static struct ia_css_binary_xinfo
+	*binary_infos[IA_CSS_BINARY_NUM_MODES] = { NULL, };
+
+static void
+ia_css_binary_dvs_env(const struct ia_css_binary_info *info,
+		      const struct ia_css_resolution *dvs_env,
+		      struct ia_css_resolution *binary_dvs_env)
+{
+	if (info->enable.dvs_envelope) {
+		assert(dvs_env);
+		binary_dvs_env->width  = max(dvs_env->width, SH_CSS_MIN_DVS_ENVELOPE);
+		binary_dvs_env->height = max(dvs_env->height, SH_CSS_MIN_DVS_ENVELOPE);
+	}
+}
+
+static void
+ia_css_binary_internal_res(const struct ia_css_frame_info *in_info,
+			   const struct ia_css_frame_info *bds_out_info,
+			   const struct ia_css_frame_info *out_info,
+			   const struct ia_css_resolution *dvs_env,
+			   const struct ia_css_binary_info *info,
+			   struct ia_css_resolution *internal_res)
+{
+	unsigned int isp_tmp_internal_width = 0,
+		     isp_tmp_internal_height = 0;
+	bool binary_supports_yuv_ds = info->enable.ds & 2;
+	struct ia_css_resolution binary_dvs_env;
+
+	binary_dvs_env.width = 0;
+	binary_dvs_env.height = 0;
+	ia_css_binary_dvs_env(info, dvs_env, &binary_dvs_env);
+
+	if (binary_supports_yuv_ds) {
+		if (in_info) {
+			isp_tmp_internal_width = in_info->res.width
+						 + info->pipeline.left_cropping + binary_dvs_env.width;
+			isp_tmp_internal_height = in_info->res.height
+						  + info->pipeline.top_cropping + binary_dvs_env.height;
+		}
+	} else if ((bds_out_info) && (out_info) &&
+		   /* TODO: hack to make video_us case work. this should be reverted after
+		   a nice solution in ISP */
+		   (bds_out_info->res.width >= out_info->res.width)) {
+		isp_tmp_internal_width = bds_out_info->padded_width;
+		isp_tmp_internal_height = bds_out_info->res.height;
+	} else {
+		if (out_info) {
+			isp_tmp_internal_width = out_info->padded_width;
+			isp_tmp_internal_height = out_info->res.height;
+		}
+	}
+
+	/* We first calculate the resolutions used by the ISP. After that,
+	 * we use those resolutions to compute sizes for tables etc. */
+	internal_res->width = __ISP_INTERNAL_WIDTH(isp_tmp_internal_width,
+			      (int)binary_dvs_env.width,
+			      info->pipeline.left_cropping, info->pipeline.mode,
+			      info->pipeline.c_subsampling,
+			      info->output.num_chunks, info->pipeline.pipelining);
+	internal_res->height = __ISP_INTERNAL_HEIGHT(isp_tmp_internal_height,
+			       info->pipeline.top_cropping,
+			       binary_dvs_env.height);
+}
+
+/* Computation results of the origin coordinate of bayer on the shading table. */
+struct sh_css_shading_table_bayer_origin_compute_results {
+	u32 bayer_scale_hor_ratio_in;	/* Horizontal ratio (in) of bayer scaling. */
+	u32 bayer_scale_hor_ratio_out;	/* Horizontal ratio (out) of bayer scaling. */
+	u32 bayer_scale_ver_ratio_in;	/* Vertical ratio (in) of bayer scaling. */
+	u32 bayer_scale_ver_ratio_out;	/* Vertical ratio (out) of bayer scaling. */
+	u32 sc_bayer_origin_x_bqs_on_shading_table; /* X coordinate (in bqs) of bayer origin on shading table. */
+	u32 sc_bayer_origin_y_bqs_on_shading_table; /* Y coordinate (in bqs) of bayer origin on shading table. */
+};
+
+/* Get the requirements for the shading correction. */
+static int
+ia_css_binary_compute_shading_table_bayer_origin(
+    const struct ia_css_binary *binary,				/* [in] */
+    unsigned int required_bds_factor,				/* [in] */
+    const struct ia_css_stream_config *stream_config,		/* [in] */
+    struct sh_css_shading_table_bayer_origin_compute_results *res)	/* [out] */
+{
+	int err;
+
+	/* Numerator and denominator of the fixed bayer downscaling factor.
+	(numerator >= denominator) */
+	unsigned int bds_num, bds_den;
+
+	/* Horizontal/Vertical ratio of bayer scaling
+	between input area and output area. */
+	unsigned int bs_hor_ratio_in;
+	unsigned int bs_hor_ratio_out;
+	unsigned int bs_ver_ratio_in;
+	unsigned int bs_ver_ratio_out;
+
+	/* Left padding set by InputFormatter. */
+	unsigned int left_padding_bqs;			/* in bqs */
+
+	/* Flag for the NEED_BDS_FACTOR_2_00 macro defined in isp kernels. */
+	unsigned int need_bds_factor_2_00;
+
+	/* Left padding adjusted inside the isp. */
+	unsigned int left_padding_adjusted_bqs;		/* in bqs */
+
+	/* Bad pixels caused by filters.
+	NxN-filter (before/after bayer scaling) moves the image position
+	to right/bottom directions by a few pixels.
+	It causes bad pixels at left/top sides,
+	and effective bayer size decreases. */
+	unsigned int bad_bqs_on_left_before_bs;	/* in bqs */
+	unsigned int bad_bqs_on_left_after_bs;	/* in bqs */
+	unsigned int bad_bqs_on_top_before_bs;	/* in bqs */
+	unsigned int bad_bqs_on_top_after_bs;	/* in bqs */
+
+	/* Get the numerator and denominator of bayer downscaling factor. */
+	err = sh_css_bds_factor_get_numerator_denominator
+	(required_bds_factor, &bds_num, &bds_den);
+	if (err)
+		return err;
+
+	/* Set the horizontal/vertical ratio of bayer scaling
+	between input area and output area. */
+	bs_hor_ratio_in  = bds_num;
+	bs_hor_ratio_out = bds_den;
+	bs_ver_ratio_in  = bds_num;
+	bs_ver_ratio_out = bds_den;
+
+	/* Set the left padding set by InputFormatter. (ifmtr.c) */
+	if (stream_config->left_padding == -1)
+		left_padding_bqs = _ISP_BQS(binary->left_padding);
+	else
+		left_padding_bqs = (unsigned int)((int)ISP_VEC_NELEMS
+				   - _ISP_BQS(stream_config->left_padding));
+
+	/* Set the left padding adjusted inside the isp.
+	When bds_factor 2.00 is needed, some padding is added to left_padding
+	inside the isp, before bayer downscaling. (raw.isp.c)
+	(Hopefully, left_crop/left_padding/top_crop should be defined in css
+	appropriately, depending on bds_factor.)
+	*/
+	need_bds_factor_2_00 = ((binary->info->sp.bds.supported_bds_factors &
+				(PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_2_00) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_2_50) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_3_00) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_4_00) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_4_50) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_5_00) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_6_00) |
+				 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_8_00))) != 0);
+
+	if (need_bds_factor_2_00 && binary->info->sp.pipeline.left_cropping > 0)
+		left_padding_adjusted_bqs = left_padding_bqs + ISP_VEC_NELEMS;
+	else
+		left_padding_adjusted_bqs = left_padding_bqs;
+
+	/* Currently, the bad pixel caused by filters before bayer scaling
+	is NOT considered, because the bad pixel is subtle.
+	When some large filter is used in the future,
+	we need to consider the bad pixel.
+
+	Currently, when bds_factor isn't 1.00, 3x3 anti-alias filter is applied
+	to each color plane(Gr/R/B/Gb) before bayer downscaling.
+	This filter moves each color plane to right/bottom directions
+	by 1 pixel at the most, depending on downscaling factor.
+	*/
+	bad_bqs_on_left_before_bs = 0;
+	bad_bqs_on_top_before_bs = 0;
+
+	/* Currently, the bad pixel caused by filters after bayer scaling
+	is NOT considered, because the bad pixel is subtle.
+	When some large filter is used in the future,
+	we need to consider the bad pixel.
+
+	Currently, when DPC&BNR is processed between bayer scaling and
+	shading correction, DPC&BNR moves each color plane to
+	right/bottom directions by 1 pixel.
+	*/
+	bad_bqs_on_left_after_bs = 0;
+	bad_bqs_on_top_after_bs = 0;
+
+	/* Calculate the origin of bayer (real sensor data area)
+	located on the shading table during the shading correction. */
+	res->sc_bayer_origin_x_bqs_on_shading_table =
+		((left_padding_adjusted_bqs + bad_bqs_on_left_before_bs)
+		* bs_hor_ratio_out + bs_hor_ratio_in / 2) / bs_hor_ratio_in
+		+ bad_bqs_on_left_after_bs;
+	/* "+ bs_hor_ratio_in/2": rounding for division by bs_hor_ratio_in */
+	res->sc_bayer_origin_y_bqs_on_shading_table =
+		(bad_bqs_on_top_before_bs * bs_ver_ratio_out + bs_ver_ratio_in / 2) / bs_ver_ratio_in
+		+ bad_bqs_on_top_after_bs;
+	/* "+ bs_ver_ratio_in/2": rounding for division by bs_ver_ratio_in */
+
+	res->bayer_scale_hor_ratio_in  = (uint32_t)bs_hor_ratio_in;
+	res->bayer_scale_hor_ratio_out = (uint32_t)bs_hor_ratio_out;
+	res->bayer_scale_ver_ratio_in  = (uint32_t)bs_ver_ratio_in;
+	res->bayer_scale_ver_ratio_out = (uint32_t)bs_ver_ratio_out;
+
+	return err;
+}
+
+/* Get the shading information of Shading Correction Type 1. */
+static int
+binary_get_shading_info_type_1(const struct ia_css_binary *binary,	/* [in] */
+			       unsigned int required_bds_factor,			/* [in] */
+			       const struct ia_css_stream_config *stream_config,	/* [in] */
+			       struct ia_css_shading_info *info)			/* [out] */
+{
+	int err;
+	struct sh_css_shading_table_bayer_origin_compute_results res;
+
+	assert(binary);
+	assert(info);
+
+	info->type = IA_CSS_SHADING_CORRECTION_TYPE_1;
+
+	info->info.type_1.enable	    = binary->info->sp.enable.sc;
+	info->info.type_1.num_hor_grids	    = binary->sctbl_width_per_color;
+	info->info.type_1.num_ver_grids	    = binary->sctbl_height;
+	info->info.type_1.bqs_per_grid_cell = (1 << binary->deci_factor_log2);
+
+	/* Initialize by default values. */
+	info->info.type_1.bayer_scale_hor_ratio_in	= 1;
+	info->info.type_1.bayer_scale_hor_ratio_out	= 1;
+	info->info.type_1.bayer_scale_ver_ratio_in	= 1;
+	info->info.type_1.bayer_scale_ver_ratio_out	= 1;
+	info->info.type_1.sc_bayer_origin_x_bqs_on_shading_table = 0;
+	info->info.type_1.sc_bayer_origin_y_bqs_on_shading_table = 0;
+
+	err = ia_css_binary_compute_shading_table_bayer_origin(
+	    binary,
+	    required_bds_factor,
+	    stream_config,
+	    &res);
+	if (err)
+		return err;
+
+	info->info.type_1.bayer_scale_hor_ratio_in	= res.bayer_scale_hor_ratio_in;
+	info->info.type_1.bayer_scale_hor_ratio_out	= res.bayer_scale_hor_ratio_out;
+	info->info.type_1.bayer_scale_ver_ratio_in	= res.bayer_scale_ver_ratio_in;
+	info->info.type_1.bayer_scale_ver_ratio_out	= res.bayer_scale_ver_ratio_out;
+	info->info.type_1.sc_bayer_origin_x_bqs_on_shading_table = res.sc_bayer_origin_x_bqs_on_shading_table;
+	info->info.type_1.sc_bayer_origin_y_bqs_on_shading_table = res.sc_bayer_origin_y_bqs_on_shading_table;
+
+	return err;
+}
+
+
+int
+ia_css_binary_get_shading_info(const struct ia_css_binary *binary,			/* [in] */
+			       enum ia_css_shading_correction_type type,		/* [in] */
+			       unsigned int required_bds_factor,			/* [in] */
+			       const struct ia_css_stream_config *stream_config,	/* [in] */
+			       struct ia_css_shading_info *shading_info,		/* [out] */
+			       struct ia_css_pipe_config *pipe_config)			/* [out] */
+{
+	int err;
+
+	assert(binary);
+	assert(shading_info);
+
+	IA_CSS_ENTER_PRIVATE("binary=%p, type=%d, required_bds_factor=%d, stream_config=%p",
+			     binary, type, required_bds_factor, stream_config);
+
+	if (type == IA_CSS_SHADING_CORRECTION_TYPE_1)
+		err = binary_get_shading_info_type_1(binary,
+						     required_bds_factor,
+						     stream_config,
+						     shading_info);
+	else
+		err = -ENOTSUPP;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static void sh_css_binary_common_grid_info(const struct ia_css_binary *binary,
+	struct ia_css_grid_info *info)
+{
+	assert(binary);
+	assert(info);
+
+	info->isp_in_width = binary->internal_frame_info.res.width;
+	info->isp_in_height = binary->internal_frame_info.res.height;
+
+	info->vamem_type = IA_CSS_VAMEM_TYPE_2;
+}
+
+void
+ia_css_binary_dvs_grid_info(const struct ia_css_binary *binary,
+			    struct ia_css_grid_info *info,
+			    struct ia_css_pipe *pipe)
+{
+	struct ia_css_dvs_grid_info *dvs_info;
+
+	(void)pipe;
+	assert(binary);
+	assert(info);
+
+	dvs_info = &info->dvs_grid.dvs_grid_info;
+
+	/* for DIS, we use a division instead of a ceil_div. If this is smaller
+	 * than the 3a grid size, it indicates that the outer values are not
+	 * valid for DIS.
+	 */
+	dvs_info->enable            = binary->info->sp.enable.dis;
+	dvs_info->width             = binary->dis.grid.dim.width;
+	dvs_info->height            = binary->dis.grid.dim.height;
+	dvs_info->aligned_width     = binary->dis.grid.pad.width;
+	dvs_info->aligned_height    = binary->dis.grid.pad.height;
+	dvs_info->bqs_per_grid_cell = 1 << binary->dis.deci_factor_log2;
+	dvs_info->num_hor_coefs     = binary->dis.coef.dim.width;
+	dvs_info->num_ver_coefs     = binary->dis.coef.dim.height;
+
+	sh_css_binary_common_grid_info(binary, info);
+}
+
+void
+ia_css_binary_dvs_stat_grid_info(
+    const struct ia_css_binary *binary,
+    struct ia_css_grid_info *info,
+    struct ia_css_pipe *pipe)
+{
+	(void)pipe;
+	sh_css_binary_common_grid_info(binary, info);
+	return;
+}
+
+int
+ia_css_binary_3a_grid_info(const struct ia_css_binary *binary,
+			   struct ia_css_grid_info *info,
+			   struct ia_css_pipe *pipe) {
+	struct ia_css_3a_grid_info *s3a_info;
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("binary=%p, info=%p, pipe=%p",
+			     binary, info, pipe);
+
+	assert(binary);
+	assert(info);
+	s3a_info = &info->s3a_grid;
+
+	/* 3A statistics grid */
+	s3a_info->enable            = binary->info->sp.enable.s3a;
+	s3a_info->width             = binary->s3atbl_width;
+	s3a_info->height            = binary->s3atbl_height;
+	s3a_info->aligned_width     = binary->s3atbl_isp_width;
+	s3a_info->aligned_height    = binary->s3atbl_isp_height;
+	s3a_info->bqs_per_grid_cell = (1 << binary->deci_factor_log2);
+	s3a_info->deci_factor_log2  = binary->deci_factor_log2;
+	s3a_info->elem_bit_depth    = SH_CSS_BAYER_BITS;
+	s3a_info->use_dmem          = binary->info->sp.s3a.s3atbl_use_dmem;
+	s3a_info->has_histogram     = 0;
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static void
+binary_init_pc_histogram(struct sh_css_pc_histogram *histo)
+{
+	assert(histo);
+
+	histo->length = 0;
+	histo->run = NULL;
+	histo->stall = NULL;
+}
+
+static void
+binary_init_metrics(struct sh_css_binary_metrics *metrics,
+		    const struct ia_css_binary_info *info)
+{
+	assert(metrics);
+	assert(info);
+
+	metrics->mode = info->pipeline.mode;
+	metrics->id   = info->id;
+	metrics->next = NULL;
+	binary_init_pc_histogram(&metrics->isp_histogram);
+	binary_init_pc_histogram(&metrics->sp_histogram);
+}
+
+/* move to host part of output module */
+static bool
+binary_supports_output_format(const struct ia_css_binary_xinfo *info,
+			      enum ia_css_frame_format format)
+{
+	int i;
+
+	assert(info);
+
+	for (i = 0; i < info->num_output_formats; i++) {
+		if (info->output_formats[i] == format)
+			return true;
+	}
+	return false;
+}
+
+static bool
+binary_supports_vf_format(const struct ia_css_binary_xinfo *info,
+			  enum ia_css_frame_format format)
+{
+	int i;
+
+	assert(info);
+
+	for (i = 0; i < info->num_vf_formats; i++) {
+		if (info->vf_formats[i] == format)
+			return true;
+	}
+	return false;
+}
+
+/* move to host part of bds module */
+static bool
+supports_bds_factor(u32 supported_factors,
+		    uint32_t bds_factor)
+{
+	return ((supported_factors & PACK_BDS_FACTOR(bds_factor)) != 0);
+}
+
+static int
+binary_init_info(struct ia_css_binary_xinfo *info, unsigned int i,
+		 bool *binary_found) {
+	const unsigned char *blob = sh_css_blob_info[i].blob;
+	unsigned int size = sh_css_blob_info[i].header.blob.size;
+
+	if ((!info) || (!binary_found))
+		return -EINVAL;
+
+	*info = sh_css_blob_info[i].header.info.isp;
+	*binary_found = blob;
+	info->blob_index = i;
+	/* we don't have this binary, skip it */
+	if (!size)
+		return 0;
+
+	info->xmem_addr = sh_css_load_blob(blob, size);
+	if (!info->xmem_addr)
+		return -ENOMEM;
+	return 0;
+}
+
+/* When binaries are put at the beginning, they will only
+ * be selected if no other primary matches.
+ */
+int
+ia_css_binary_init_infos(void) {
+	unsigned int i;
+	unsigned int num_of_isp_binaries = sh_css_num_binaries - NUM_OF_SPS - NUM_OF_BLS;
+
+	if (num_of_isp_binaries == 0)
+		return 0;
+
+	all_binaries = kvmalloc(num_of_isp_binaries * sizeof(*all_binaries),
+				GFP_KERNEL);
+	if (!all_binaries)
+		return -ENOMEM;
+
+	for (i = 0; i < num_of_isp_binaries; i++)
+	{
+		int ret;
+		struct ia_css_binary_xinfo *binary = &all_binaries[i];
+		bool binary_found;
+
+		ret = binary_init_info(binary, i, &binary_found);
+		if (ret)
+			return ret;
+		if (!binary_found)
+			continue;
+		/* Prepend new binary information */
+		binary->next = binary_infos[binary->sp.pipeline.mode];
+		binary_infos[binary->sp.pipeline.mode] = binary;
+		binary->blob = &sh_css_blob_info[i];
+		binary->mem_offsets = sh_css_blob_info[i].mem_offsets;
+	}
+	return 0;
+}
+
+int
+ia_css_binary_uninit(void) {
+	unsigned int i;
+	struct ia_css_binary_xinfo *b;
+
+	for (i = 0; i < IA_CSS_BINARY_NUM_MODES; i++)
+	{
+		for (b = binary_infos[i]; b; b = b->next) {
+			if (b->xmem_addr)
+				hmm_free(b->xmem_addr);
+			b->xmem_addr = mmgr_NULL;
+		}
+		binary_infos[i] = NULL;
+	}
+	kvfree(all_binaries);
+	return 0;
+}
+
+/* @brief Compute decimation factor for 3A statistics and shading correction.
+ *
+ * @param[in]	width	Frame width in pixels.
+ * @param[in]	height	Frame height in pixels.
+ * @return	Log2 of decimation factor (= grid cell size) in bayer quads.
+ */
+static int
+binary_grid_deci_factor_log2(int width, int height)
+{
+	/* 3A/Shading decimation factor spcification (at August 2008)
+	 * ------------------------------------------------------------------
+	 * [Image Width (BQ)] [Decimation Factor (BQ)] [Resulting grid cells]
+	 * 1280 ?c             32                       40 ?c
+	 *  640 ?c 1279        16                       40 ?c 80
+	 *      ?c  639         8                          ?c 80
+	 * ------------------------------------------------------------------
+	 */
+	/* Maximum and minimum decimation factor by the specification */
+#define MAX_SPEC_DECI_FACT_LOG2		5
+#define MIN_SPEC_DECI_FACT_LOG2		3
+	/* the smallest frame width in bayer quads when decimation factor (log2) is 5 or 4, by the specification */
+#define DECI_FACT_LOG2_5_SMALLEST_FRAME_WIDTH_BQ	1280
+#define DECI_FACT_LOG2_4_SMALLEST_FRAME_WIDTH_BQ	640
+
+	int smallest_factor; /* the smallest factor (log2) where the number of cells does not exceed the limitation */
+	int spec_factor;     /* the factor (log2) which satisfies the specification */
+
+	/* Currently supported maximum width and height are 5120(=80*64) and 3840(=60*64). */
+	assert(ISP_BQ_GRID_WIDTH(width,
+				 MAX_SPEC_DECI_FACT_LOG2) <= SH_CSS_MAX_BQ_GRID_WIDTH);
+	assert(ISP_BQ_GRID_HEIGHT(height,
+				  MAX_SPEC_DECI_FACT_LOG2) <= SH_CSS_MAX_BQ_GRID_HEIGHT);
+
+	/* Compute the smallest factor. */
+	smallest_factor = MAX_SPEC_DECI_FACT_LOG2;
+	while (ISP_BQ_GRID_WIDTH(width,
+				 smallest_factor - 1) <= SH_CSS_MAX_BQ_GRID_WIDTH &&
+	       ISP_BQ_GRID_HEIGHT(height, smallest_factor - 1) <= SH_CSS_MAX_BQ_GRID_HEIGHT
+	       && smallest_factor > MIN_SPEC_DECI_FACT_LOG2)
+		smallest_factor--;
+
+	/* Get the factor by the specification. */
+	if (_ISP_BQS(width) >= DECI_FACT_LOG2_5_SMALLEST_FRAME_WIDTH_BQ)
+		spec_factor = 5;
+	else if (_ISP_BQS(width) >= DECI_FACT_LOG2_4_SMALLEST_FRAME_WIDTH_BQ)
+		spec_factor = 4;
+	else
+		spec_factor = 3;
+
+	/* If smallest_factor is smaller than or equal to spec_factor, choose spec_factor to follow the specification.
+	   If smallest_factor is larger than spec_factor, choose smallest_factor.
+
+		ex. width=2560, height=1920
+			smallest_factor=4, spec_factor=5
+			smallest_factor < spec_factor   ->   return spec_factor
+
+		ex. width=300, height=3000
+			smallest_factor=5, spec_factor=3
+			smallest_factor > spec_factor   ->   return smallest_factor
+	*/
+	return max(smallest_factor, spec_factor);
+
+#undef MAX_SPEC_DECI_FACT_LOG2
+#undef MIN_SPEC_DECI_FACT_LOG2
+#undef DECI_FACT_LOG2_5_SMALLEST_FRAME_WIDTH_BQ
+#undef DECI_FACT_LOG2_4_SMALLEST_FRAME_WIDTH_BQ
+}
+
+static int
+binary_in_frame_padded_width(int in_frame_width,
+			     int isp_internal_width,
+			     int dvs_env_width,
+			     int stream_config_left_padding,
+			     int left_cropping,
+			     bool need_scaling)
+{
+	int rval;
+	int nr_of_left_paddings;	/* number of paddings pixels on the left of an image line */
+
+#if defined(ISP2401)
+	/* the output image line of Input System 2401 does not have the left paddings  */
+	nr_of_left_paddings = 0;
+#else
+	/* in other cases, the left padding pixels are always 128 */
+	nr_of_left_paddings = 2 * ISP_VEC_NELEMS;
+#endif
+	if (need_scaling) {
+		/* In SDV use-case, we need to match left-padding of
+		 * primary and the video binary. */
+		if (stream_config_left_padding != -1) {
+			/* Different than before, we do left&right padding. */
+			rval =
+			    CEIL_MUL(in_frame_width + nr_of_left_paddings,
+				     2 * ISP_VEC_NELEMS);
+		} else {
+			/* Different than before, we do left&right padding. */
+			in_frame_width += dvs_env_width;
+			rval =
+			    CEIL_MUL(in_frame_width +
+				     (left_cropping ? nr_of_left_paddings : 0),
+				     2 * ISP_VEC_NELEMS);
+		}
+	} else {
+		rval = isp_internal_width;
+	}
+
+	return rval;
+}
+
+int
+ia_css_binary_fill_info(const struct ia_css_binary_xinfo *xinfo,
+			bool online,
+			bool two_ppc,
+			enum atomisp_input_format stream_format,
+			const struct ia_css_frame_info *in_info, /* can be NULL */
+			const struct ia_css_frame_info *bds_out_info, /* can be NULL */
+			const struct ia_css_frame_info *out_info[], /* can be NULL */
+			const struct ia_css_frame_info *vf_info, /* can be NULL */
+			struct ia_css_binary *binary,
+			struct ia_css_resolution *dvs_env,
+			int stream_config_left_padding,
+			bool accelerator) {
+	const struct ia_css_binary_info *info = &xinfo->sp;
+	unsigned int dvs_env_width = 0,
+	dvs_env_height = 0,
+	vf_log_ds = 0,
+	s3a_log_deci = 0,
+	bits_per_pixel = 0,
+	/* Resolution at SC/3A/DIS kernel. */
+	sc_3a_dis_width = 0,
+	/* Resolution at SC/3A/DIS kernel. */
+	sc_3a_dis_padded_width = 0,
+	/* Resolution at SC/3A/DIS kernel. */
+	sc_3a_dis_height = 0,
+	isp_internal_width = 0,
+	isp_internal_height = 0,
+	s3a_isp_width = 0;
+
+	bool need_scaling = false;
+	struct ia_css_resolution binary_dvs_env, internal_res;
+	int err;
+	unsigned int i;
+	const struct ia_css_frame_info *bin_out_info = NULL;
+
+	assert(info);
+	assert(binary);
+
+	binary->info = xinfo;
+	if (!accelerator)
+	{
+		/* binary->css_params has been filled by accelerator itself. */
+		err = ia_css_isp_param_allocate_isp_parameters(
+		    &binary->mem_params, &binary->css_params,
+		    &info->mem_initializers);
+		if (err) {
+			return err;
+		}
+	}
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+	{
+		if (out_info[i] && (out_info[i]->res.width != 0)) {
+			bin_out_info = out_info[i];
+			break;
+		}
+	}
+	if (in_info && bin_out_info)
+	{
+		need_scaling = (in_info->res.width != bin_out_info->res.width) ||
+			       (in_info->res.height != bin_out_info->res.height);
+	}
+
+	/* binary_dvs_env has to be equal or larger than SH_CSS_MIN_DVS_ENVELOPE */
+	binary_dvs_env.width = 0;
+	binary_dvs_env.height = 0;
+	ia_css_binary_dvs_env(info, dvs_env, &binary_dvs_env);
+	dvs_env_width = binary_dvs_env.width;
+	dvs_env_height = binary_dvs_env.height;
+	binary->dvs_envelope.width  = dvs_env_width;
+	binary->dvs_envelope.height = dvs_env_height;
+
+	/* internal resolution calculation */
+	internal_res.width = 0;
+	internal_res.height = 0;
+	ia_css_binary_internal_res(in_info, bds_out_info, bin_out_info, dvs_env,
+				   info, &internal_res);
+	isp_internal_width = internal_res.width;
+	isp_internal_height = internal_res.height;
+
+	/* internal frame info */
+	if (bin_out_info) /* { */
+		binary->internal_frame_info.format = bin_out_info->format;
+	/* } */
+	binary->internal_frame_info.res.width       = isp_internal_width;
+	binary->internal_frame_info.padded_width    = CEIL_MUL(isp_internal_width, 2 * ISP_VEC_NELEMS);
+	binary->internal_frame_info.res.height      = isp_internal_height;
+	binary->internal_frame_info.raw_bit_depth   = bits_per_pixel;
+
+	if (in_info)
+	{
+		binary->effective_in_frame_res.width = in_info->res.width;
+		binary->effective_in_frame_res.height = in_info->res.height;
+
+		bits_per_pixel = in_info->raw_bit_depth;
+
+		/* input info */
+		binary->in_frame_info.res.width = in_info->res.width +
+						  info->pipeline.left_cropping;
+		binary->in_frame_info.res.height = in_info->res.height +
+						   info->pipeline.top_cropping;
+
+		binary->in_frame_info.res.width += dvs_env_width;
+		binary->in_frame_info.res.height += dvs_env_height;
+
+		binary->in_frame_info.padded_width =
+		    binary_in_frame_padded_width(in_info->res.width,
+						 isp_internal_width,
+						 dvs_env_width,
+						 stream_config_left_padding,
+						 info->pipeline.left_cropping,
+						 need_scaling);
+
+		binary->in_frame_info.format = in_info->format;
+		binary->in_frame_info.raw_bayer_order = in_info->raw_bayer_order;
+		binary->in_frame_info.crop_info = in_info->crop_info;
+	}
+
+	if (online)
+	{
+		bits_per_pixel = ia_css_util_input_format_bpp(
+				     stream_format, two_ppc);
+	}
+	binary->in_frame_info.raw_bit_depth = bits_per_pixel;
+
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+	{
+		if (out_info[i]) {
+			binary->out_frame_info[i].res.width     = out_info[i]->res.width;
+			binary->out_frame_info[i].res.height    = out_info[i]->res.height;
+			binary->out_frame_info[i].padded_width  = out_info[i]->padded_width;
+			if (info->pipeline.mode == IA_CSS_BINARY_MODE_COPY) {
+				binary->out_frame_info[i].raw_bit_depth = bits_per_pixel;
+			} else {
+				/* Only relevant for RAW format.
+				 * At the moment, all outputs are raw, 16 bit per pixel, except for copy.
+				 * To do this cleanly, the binary should specify in its info
+				 * the bit depth per output channel.
+				 */
+				binary->out_frame_info[i].raw_bit_depth = 16;
+			}
+			binary->out_frame_info[i].format        = out_info[i]->format;
+		}
+	}
+
+	if (vf_info && (vf_info->res.width != 0))
+	{
+		err = ia_css_vf_configure(binary, bin_out_info,
+					  (struct ia_css_frame_info *)vf_info, &vf_log_ds);
+		if (err) {
+			if (!accelerator) {
+				ia_css_isp_param_destroy_isp_parameters(
+				    &binary->mem_params,
+				    &binary->css_params);
+			}
+			return err;
+		}
+	}
+	binary->vf_downscale_log2 = vf_log_ds;
+
+	binary->online            = online;
+	binary->input_format      = stream_format;
+
+	/* viewfinder output info */
+	if ((vf_info) && (vf_info->res.width != 0))
+	{
+		unsigned int vf_out_vecs, vf_out_width, vf_out_height;
+
+		binary->vf_frame_info.format = vf_info->format;
+		if (!bin_out_info)
+			return -EINVAL;
+		vf_out_vecs = __ISP_VF_OUTPUT_WIDTH_VECS(bin_out_info->padded_width,
+			      vf_log_ds);
+		vf_out_width = _ISP_VF_OUTPUT_WIDTH(vf_out_vecs);
+		vf_out_height = _ISP_VF_OUTPUT_HEIGHT(bin_out_info->res.height,
+						      vf_log_ds);
+
+		/* For preview mode, output pin is used instead of vf. */
+		if (info->pipeline.mode == IA_CSS_BINARY_MODE_PREVIEW) {
+			binary->out_frame_info[0].res.width =
+			    (bin_out_info->res.width >> vf_log_ds);
+			binary->out_frame_info[0].padded_width = vf_out_width;
+			binary->out_frame_info[0].res.height   = vf_out_height;
+
+			binary->vf_frame_info.res.width    = 0;
+			binary->vf_frame_info.padded_width = 0;
+			binary->vf_frame_info.res.height   = 0;
+		} else {
+			/* we also store the raw downscaled width. This is
+			 * used for digital zoom in preview to zoom only on
+			 * the width that we actually want to keep, not on
+			 * the aligned width. */
+			binary->vf_frame_info.res.width =
+			    (bin_out_info->res.width >> vf_log_ds);
+			binary->vf_frame_info.padded_width = vf_out_width;
+			binary->vf_frame_info.res.height   = vf_out_height;
+		}
+	} else
+	{
+		binary->vf_frame_info.res.width    = 0;
+		binary->vf_frame_info.padded_width = 0;
+		binary->vf_frame_info.res.height   = 0;
+	}
+
+	if (info->enable.ca_gdc)
+	{
+		binary->morph_tbl_width =
+		    _ISP_MORPH_TABLE_WIDTH(isp_internal_width);
+		binary->morph_tbl_aligned_width  =
+		    _ISP_MORPH_TABLE_ALIGNED_WIDTH(isp_internal_width);
+		binary->morph_tbl_height =
+		    _ISP_MORPH_TABLE_HEIGHT(isp_internal_height);
+	} else
+	{
+		binary->morph_tbl_width  = 0;
+		binary->morph_tbl_aligned_width  = 0;
+		binary->morph_tbl_height = 0;
+	}
+
+	sc_3a_dis_width = binary->in_frame_info.res.width;
+	sc_3a_dis_padded_width = binary->in_frame_info.padded_width;
+	sc_3a_dis_height = binary->in_frame_info.res.height;
+	if (bds_out_info && in_info &&
+	    bds_out_info->res.width != in_info->res.width)
+	{
+		/* TODO: Next, "internal_frame_info" should be derived from
+		 * bds_out. So this part will change once it is in place! */
+		sc_3a_dis_width = bds_out_info->res.width + info->pipeline.left_cropping;
+		sc_3a_dis_padded_width = isp_internal_width;
+		sc_3a_dis_height = isp_internal_height;
+	}
+
+	s3a_isp_width = _ISP_S3A_ELEMS_ISP_WIDTH(sc_3a_dis_padded_width,
+			info->pipeline.left_cropping);
+	if (info->s3a.fixed_s3a_deci_log)
+	{
+		s3a_log_deci = info->s3a.fixed_s3a_deci_log;
+	} else
+	{
+		s3a_log_deci = binary_grid_deci_factor_log2(s3a_isp_width,
+			       sc_3a_dis_height);
+	}
+	binary->deci_factor_log2  = s3a_log_deci;
+
+	if (info->enable.s3a)
+	{
+		binary->s3atbl_width  =
+		    _ISP_S3ATBL_WIDTH(sc_3a_dis_width,
+				      s3a_log_deci);
+		binary->s3atbl_height =
+		    _ISP_S3ATBL_HEIGHT(sc_3a_dis_height,
+				       s3a_log_deci);
+		binary->s3atbl_isp_width =
+		    _ISP_S3ATBL_ISP_WIDTH(s3a_isp_width,
+					  s3a_log_deci);
+		binary->s3atbl_isp_height =
+		    _ISP_S3ATBL_ISP_HEIGHT(sc_3a_dis_height,
+					   s3a_log_deci);
+	} else
+	{
+		binary->s3atbl_width  = 0;
+		binary->s3atbl_height = 0;
+		binary->s3atbl_isp_width  = 0;
+		binary->s3atbl_isp_height = 0;
+	}
+
+	if (info->enable.sc)
+	{
+		binary->sctbl_width_per_color = _ISP_SCTBL_WIDTH_PER_COLOR(sc_3a_dis_padded_width, s3a_log_deci);
+		binary->sctbl_aligned_width_per_color = SH_CSS_MAX_SCTBL_ALIGNED_WIDTH_PER_COLOR;
+		binary->sctbl_height = _ISP_SCTBL_HEIGHT(sc_3a_dis_height, s3a_log_deci);
+	} else
+	{
+		binary->sctbl_width_per_color         = 0;
+		binary->sctbl_aligned_width_per_color = 0;
+		binary->sctbl_height                  = 0;
+	}
+	ia_css_sdis_init_info(&binary->dis,
+			      sc_3a_dis_width,
+			      sc_3a_dis_padded_width,
+			      sc_3a_dis_height,
+			      info->pipeline.isp_pipe_version,
+			      info->enable.dis);
+	if (info->pipeline.left_cropping)
+		binary->left_padding = 2 * ISP_VEC_NELEMS - info->pipeline.left_cropping;
+	else
+		binary->left_padding = 0;
+
+	return 0;
+}
+
+static int __ia_css_binary_find(struct ia_css_binary_descr *descr,
+				struct ia_css_binary *binary) {
+	int mode;
+	bool online;
+	bool two_ppc;
+	enum atomisp_input_format stream_format;
+	const struct ia_css_frame_info *req_in_info,
+		*req_bds_out_info,
+		*req_out_info[IA_CSS_BINARY_MAX_OUTPUT_PORTS],
+		*req_bin_out_info = NULL,
+		*req_vf_info;
+
+	struct ia_css_binary_xinfo *xcandidate;
+	bool need_ds, need_dz, need_dvs, need_xnr, need_dpc;
+	bool striped;
+	bool enable_yuv_ds;
+	bool enable_high_speed;
+	bool enable_dvs_6axis;
+	bool enable_reduced_pipe;
+	bool enable_capture_pp_bli;
+	int err = -EINVAL;
+	bool continuous;
+	unsigned int isp_pipe_version;
+	struct ia_css_resolution dvs_env, internal_res;
+	unsigned int i;
+
+	assert(descr);
+	/* MW: used after an error check, may accept NULL, but doubtfull */
+	assert(binary);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_binary_find() enter: descr=%p, (mode=%d), binary=%p\n",
+			    descr, descr->mode,
+			    binary);
+
+	mode = descr->mode;
+	online = descr->online;
+	two_ppc = descr->two_ppc;
+	stream_format = descr->stream_format;
+	req_in_info = descr->in_info;
+	req_bds_out_info = descr->bds_out_info;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		req_out_info[i] = descr->out_info[i];
+		if (req_out_info[i] && (req_out_info[i]->res.width != 0))
+			req_bin_out_info = req_out_info[i];
+	}
+	if (!req_bin_out_info)
+		return -EINVAL;
+	req_vf_info = descr->vf_info;
+
+	need_xnr = descr->enable_xnr;
+	need_ds = descr->enable_fractional_ds;
+	need_dz = false;
+	need_dvs = false;
+	need_dpc = descr->enable_dpc;
+
+	enable_yuv_ds = descr->enable_yuv_ds;
+	enable_high_speed = descr->enable_high_speed;
+	enable_dvs_6axis  = descr->enable_dvs_6axis;
+	enable_reduced_pipe = descr->enable_reduced_pipe;
+	enable_capture_pp_bli = descr->enable_capture_pp_bli;
+	continuous = descr->continuous;
+	striped = descr->striped;
+	isp_pipe_version = descr->isp_pipe_version;
+
+	dvs_env.width = 0;
+	dvs_env.height = 0;
+	internal_res.width = 0;
+	internal_res.height = 0;
+
+	if (mode == IA_CSS_BINARY_MODE_VIDEO) {
+		dvs_env = descr->dvs_env;
+		need_dz = descr->enable_dz;
+		/* Video is the only mode that has a nodz variant. */
+		need_dvs = dvs_env.width || dvs_env.height;
+	}
+
+	/* print a map of the binary file */
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"BINARY INFO:\n");
+	for (i = 0; i < IA_CSS_BINARY_NUM_MODES; i++) {
+		xcandidate = binary_infos[i];
+		if (xcandidate) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"%d:\n", i);
+			while (xcandidate) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, " Name:%s Type:%d Cont:%d\n",
+						    xcandidate->blob->name, xcandidate->type,
+						    xcandidate->sp.enable.continuous);
+				xcandidate = xcandidate->next;
+			}
+		}
+	}
+
+	/* printf("sh_css_binary_find: pipe version %d\n", isp_pipe_version); */
+	for (xcandidate = binary_infos[mode]; xcandidate;
+	     xcandidate = xcandidate->next) {
+		struct ia_css_binary_info *candidate = &xcandidate->sp;
+		/* printf("sh_css_binary_find: evaluating candidate:
+		 * %d\n",candidate->id); */
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_binary_find() candidate = %p, mode = %d ID = %d\n",
+				    candidate, candidate->pipeline.mode, candidate->id);
+
+		/*
+		 * MW: Only a limited set of jointly configured binaries can
+		 * be used in a continuous preview/video mode unless it is
+		 * the copy mode and runs on SP.
+		*/
+		if (!candidate->enable.continuous &&
+		    continuous && (mode != IA_CSS_BINARY_MODE_COPY)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d && (%d != %d)\n",
+					    __LINE__, candidate->enable.continuous,
+					    continuous, mode,
+					    IA_CSS_BINARY_MODE_COPY);
+			continue;
+		}
+		if (striped && candidate->iterator.num_stripes == 1) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: binary is not striped\n",
+					    __LINE__);
+			continue;
+		}
+
+		if (candidate->pipeline.isp_pipe_version != isp_pipe_version &&
+		    (mode != IA_CSS_BINARY_MODE_COPY) &&
+		    (mode != IA_CSS_BINARY_MODE_CAPTURE_PP) &&
+		    (mode != IA_CSS_BINARY_MODE_VF_PP)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d != %d)\n",
+					    __LINE__,
+					    candidate->pipeline.isp_pipe_version, isp_pipe_version);
+			continue;
+		}
+		if (!candidate->enable.reduced_pipe && enable_reduced_pipe) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d\n",
+					    __LINE__,
+					    candidate->enable.reduced_pipe,
+					    enable_reduced_pipe);
+			continue;
+		}
+		if (!candidate->enable.dvs_6axis && enable_dvs_6axis) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d\n",
+					    __LINE__,
+					    candidate->enable.dvs_6axis,
+					    enable_dvs_6axis);
+			continue;
+		}
+		if (candidate->enable.high_speed && !enable_high_speed) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: %d && !%d\n",
+					    __LINE__,
+					    candidate->enable.high_speed,
+					    enable_high_speed);
+			continue;
+		}
+		if (!candidate->enable.xnr && need_xnr) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: %d && !%d\n",
+					    __LINE__,
+					    candidate->enable.xnr,
+					    need_xnr);
+			continue;
+		}
+		if (!(candidate->enable.ds & 2) && enable_yuv_ds) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d\n",
+					    __LINE__,
+					    ((candidate->enable.ds & 2) != 0),
+					    enable_yuv_ds);
+			continue;
+		}
+		if ((candidate->enable.ds & 2) && !enable_yuv_ds) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: %d && !%d\n",
+					    __LINE__,
+					    ((candidate->enable.ds & 2) != 0),
+					    enable_yuv_ds);
+			continue;
+		}
+
+		if (mode == IA_CSS_BINARY_MODE_VIDEO &&
+		    candidate->enable.ds && need_ds)
+			need_dz = false;
+
+		/* when we require vf output, we need to have vf_veceven */
+		if ((req_vf_info) && !(candidate->enable.vf_veceven ||
+				       /* or variable vf vec even */
+				       candidate->vf_dec.is_variable ||
+				       /* or more than one output pin. */
+				       xcandidate->num_output_pins > 1)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%p != NULL) && !(%d || %d || (%d >%d))\n",
+					    __LINE__, req_vf_info,
+					    candidate->enable.vf_veceven,
+					    candidate->vf_dec.is_variable,
+					    xcandidate->num_output_pins, 1);
+			continue;
+		}
+		if (!candidate->enable.dvs_envelope && need_dvs) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d\n",
+					    __LINE__,
+					    candidate->enable.dvs_envelope, (int)need_dvs);
+			continue;
+		}
+		/* internal_res check considers input, output, and dvs envelope sizes */
+		ia_css_binary_internal_res(req_in_info, req_bds_out_info,
+					   req_bin_out_info, &dvs_env, candidate, &internal_res);
+		if (internal_res.width > candidate->internal.max_width) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d > %d)\n",
+					    __LINE__, internal_res.width,
+					    candidate->internal.max_width);
+			continue;
+		}
+		if (internal_res.height > candidate->internal.max_height) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d > %d)\n",
+					    __LINE__, internal_res.height,
+					    candidate->internal.max_height);
+			continue;
+		}
+		if (!candidate->enable.ds && need_ds && !(xcandidate->num_output_pins > 1)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && %d\n",
+					    __LINE__, candidate->enable.ds, (int)need_ds);
+			continue;
+		}
+		if (!candidate->enable.uds && !candidate->enable.dvs_6axis && need_dz) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && !%d && %d\n",
+					    __LINE__, candidate->enable.uds,
+					    candidate->enable.dvs_6axis, (int)need_dz);
+			continue;
+		}
+		if (online && candidate->input.source == IA_CSS_BINARY_INPUT_MEMORY) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: %d && (%d == %d)\n",
+					    __LINE__, online, candidate->input.source,
+					    IA_CSS_BINARY_INPUT_MEMORY);
+			continue;
+		}
+		if (!online && candidate->input.source == IA_CSS_BINARY_INPUT_SENSOR) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d && (%d == %d)\n",
+					    __LINE__, online, candidate->input.source,
+					    IA_CSS_BINARY_INPUT_SENSOR);
+			continue;
+		}
+		if (req_bin_out_info->res.width < candidate->output.min_width ||
+		    req_bin_out_info->res.width > candidate->output.max_width) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d > %d) || (%d < %d)\n",
+					    __LINE__,
+					    req_bin_out_info->padded_width,
+					    candidate->output.min_width,
+					    req_bin_out_info->padded_width,
+					    candidate->output.max_width);
+			continue;
+		}
+		if (xcandidate->num_output_pins > 1 &&
+		    /* in case we have a second output pin, */
+		    req_vf_info) { /* and we need vf output. */
+			if (req_vf_info->res.width > candidate->output.max_width) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_binary_find() [%d] continue: (%d < %d)\n",
+						    __LINE__,
+						    req_vf_info->res.width,
+						    candidate->output.max_width);
+				continue;
+			}
+		}
+		if (req_in_info->padded_width > candidate->input.max_width) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d > %d)\n",
+					    __LINE__, req_in_info->padded_width,
+					    candidate->input.max_width);
+			continue;
+		}
+		if (!binary_supports_output_format(xcandidate, req_bin_out_info->format)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d\n",
+					    __LINE__,
+					    binary_supports_output_format(xcandidate, req_bin_out_info->format));
+			continue;
+		}
+		if (xcandidate->num_output_pins > 1 &&
+		    /* in case we have a second output pin, */
+		    req_vf_info                   && /* and we need vf output. */
+		    /* check if the required vf format
+		    is supported. */
+		    !binary_supports_output_format(xcandidate, req_vf_info->format)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d > %d) && (%p != NULL) && !%d\n",
+					    __LINE__, xcandidate->num_output_pins, 1,
+					    req_vf_info,
+					    binary_supports_output_format(xcandidate, req_vf_info->format));
+			continue;
+		}
+
+		/* Check if vf_veceven supports the requested vf format */
+		if (xcandidate->num_output_pins == 1 &&
+		    req_vf_info && candidate->enable.vf_veceven &&
+		    !binary_supports_vf_format(xcandidate, req_vf_info->format)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: (%d == %d) && (%p != NULL) && %d && !%d\n",
+					    __LINE__, xcandidate->num_output_pins, 1,
+					    req_vf_info, candidate->enable.vf_veceven,
+					    binary_supports_vf_format(xcandidate, req_vf_info->format));
+			continue;
+		}
+
+		/* Check if vf_veceven supports the requested vf width */
+		if (xcandidate->num_output_pins == 1 &&
+		    req_vf_info && candidate->enable.vf_veceven) { /* and we need vf output. */
+			if (req_vf_info->res.width > candidate->output.max_width) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_binary_find() [%d] continue: (%d < %d)\n",
+						    __LINE__,
+						    req_vf_info->res.width,
+						    candidate->output.max_width);
+				continue;
+			}
+		}
+
+		if (!supports_bds_factor(candidate->bds.supported_bds_factors,
+					 descr->required_bds_factor)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: 0x%x & 0x%x)\n",
+					    __LINE__, candidate->bds.supported_bds_factors,
+					    descr->required_bds_factor);
+			continue;
+		}
+
+		if (!candidate->enable.dpc && need_dpc) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: 0x%x & 0x%x)\n",
+					    __LINE__, candidate->enable.dpc,
+					    descr->enable_dpc);
+			continue;
+		}
+
+		if (candidate->uds.use_bci && enable_capture_pp_bli) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: 0x%x & 0x%x)\n",
+					    __LINE__, candidate->uds.use_bci,
+					    descr->enable_capture_pp_bli);
+			continue;
+		}
+
+		/* reconfigure any variable properties of the binary */
+		err = ia_css_binary_fill_info(xcandidate, online, two_ppc,
+					      stream_format, req_in_info,
+					      req_bds_out_info,
+					      req_out_info, req_vf_info,
+					      binary, &dvs_env,
+					      descr->stream_config_left_padding,
+					      false);
+
+		if (err)
+			break;
+		binary_init_metrics(&binary->metrics, &binary->info->sp);
+		break;
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_binary_find() selected = %p, mode = %d ID = %d\n",
+			    xcandidate, xcandidate ? xcandidate->sp.pipeline.mode : 0, xcandidate ? xcandidate->sp.id : 0);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_binary_find() leave: return_err=%d\n", err);
+
+	if (!err && xcandidate)
+		dev_dbg(atomisp_dev,
+			"Using binary %s (id %d), type %d, mode %d, continuous %s\n",
+			xcandidate->blob->name,
+			xcandidate->sp.id,
+			xcandidate->type,
+			xcandidate->sp.pipeline.mode,
+			xcandidate->sp.enable.continuous ? "true" : "false");
+
+
+	return err;
+}
+
+int ia_css_binary_find(struct ia_css_binary_descr *descr,
+		       struct ia_css_binary *binary)
+{
+	int ret = __ia_css_binary_find(descr, binary);
+
+	if (unlikely(ret)) {
+		dev_dbg(atomisp_dev, "Seeking for binary failed at:");
+		dump_stack();
+	}
+
+	return ret;
+}
+
+unsigned
+ia_css_binary_max_vf_width(void)
+{
+	/* This is (should be) true for IPU1 and IPU2 */
+	/* For IPU3 (SkyCam) this pointer is guaranteed to be NULL simply because such a binary does not exist  */
+	if (binary_infos[IA_CSS_BINARY_MODE_VF_PP])
+		return binary_infos[IA_CSS_BINARY_MODE_VF_PP]->sp.output.max_width;
+	return 0;
+}
+
+void
+ia_css_binary_destroy_isp_parameters(struct ia_css_binary *binary)
+{
+	if (binary) {
+		ia_css_isp_param_destroy_isp_parameters(&binary->mem_params,
+							&binary->css_params);
+	}
+}
+
+void
+ia_css_binary_get_isp_binaries(struct ia_css_binary_xinfo **binaries,
+			       uint32_t *num_isp_binaries)
+{
+	assert(binaries);
+
+	if (num_isp_binaries)
+		*num_isp_binaries = 0;
+
+	*binaries = all_binaries;
+	if (all_binaries && num_isp_binaries) {
+		/* -1 to account for sp binary which is not stored in all_binaries */
+		if (sh_css_num_binaries > 0)
+			*num_isp_binaries = sh_css_num_binaries - 1;
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq.h b/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq.h
new file mode 100644
index 000000000..a461b0ed0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq.h
@@ -0,0 +1,178 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_BUFQ_H
+#define _IA_CSS_BUFQ_H
+
+#include <type_support.h>
+#include "ia_css_bufq_comm.h"
+#include "ia_css_buffer.h"
+#include "ia_css_err.h"
+#define BUFQ_EVENT_SIZE 4
+
+/**
+ * @brief Query the internal frame ID.
+ *
+ * @param[in]	key	The query key.
+ * @param[out]	val	The query value.
+ *
+ * @return
+ *	true, if the query succeeds;
+ *	false, if the query fails.
+ */
+bool ia_css_query_internal_queue_id(
+    enum ia_css_buffer_type buf_type,
+    unsigned int thread_id,
+    enum sh_css_queue_id *val
+);
+
+/**
+ * @brief  Map buffer type to a internal queue id.
+ *
+ * @param[in] thread id		Thread in which the buffer type has to be mapped or unmapped
+ * @param[in] buf_type		buffer type.
+ * @param[in] map		boolean flag to specify map or unmap
+ * @return none
+ */
+void ia_css_queue_map(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type,
+    bool map
+);
+
+/**
+ * @brief  Initialize buffer type to a queue id mapping
+ * @return none
+ */
+void ia_css_queue_map_init(void);
+
+/**
+ * @brief initializes bufq module
+ * It create instances of
+ * -host to SP buffer queue  which is a list with predefined size,
+ *	MxN queues where M is the number threads and N is the number queues per thread
+ *-SP to host buffer queue , is a list with N queues
+ *-host to SP event communication queue
+ * -SP to host event communication queue
+ * -queue for tagger commands
+ * @return none
+ */
+void ia_css_bufq_init(void);
+
+/**
+* @brief Enqueues an item into host to SP buffer queue
+ *
+ * @param thread_index[in]	Thread in which the item to be enqueued
+ *
+ * @param queue_id[in]		Index of the queue in the specified thread
+ * @param item[in]		Object to enqueue.
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_enqueue_buffer(
+    int thread_index,
+    int queue_id,
+    uint32_t item);
+
+/**
+* @brief Dequeues an item from SP to host buffer queue.
+ *
+ * @param queue_id[in]		Specifies  the index of the queue in the list where
+ *				the item has to be read.
+ * @paramitem [out]		Object to be dequeued into this item.
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_dequeue_buffer(
+    int queue_id,
+    uint32_t *item);
+
+/**
+* @brief  Enqueue an event item into host to SP communication event queue.
+ *
+ * @param[in]	evt_id		      The event ID.
+ * @param[in]	evt_payload_0	The event payload.
+ * @param[in]	evt_payload_1	The event payload.
+ * @param[in]	evt_payload_2	The event payload.
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_enqueue_psys_event(
+    u8 evt_id,
+    u8 evt_payload_0,
+    u8 evt_payload_1,
+    uint8_t evt_payload_2
+);
+
+/**
+ * @brief   Dequeue an item from  SP to host communication event queue.
+ *
+ * @param item	Object to be dequeued into this item.
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_dequeue_psys_event(
+    u8 item[BUFQ_EVENT_SIZE]
+
+);
+
+/**
+ * @brief  Enqueue an event item into host to SP EOF event queue.
+ *
+ * @param[in]	evt_id		      The event ID.
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_bufq_enqueue_isys_event(
+    uint8_t evt_id);
+
+/**
+* @brief   Dequeue an item from  SP to host communication EOF event queue.
+
+ *
+ * @param item	Object to be dequeued into this item.
+ * @return	0 or error code upon error.
+ *
+ */
+int ia_css_bufq_dequeue_isys_event(
+    u8 item[BUFQ_EVENT_SIZE]);
+
+/**
+* @brief   Enqueue a tagger command item into tagger command queue..
+ *
+ * @param item	Object to be enqueue.
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_enqueue_tag_cmd(
+    uint32_t item);
+
+/**
+* @brief  Uninitializes bufq module.
+ *
+ * @return	0 or error code upon error.
+ *
+*/
+int ia_css_bufq_deinit(void);
+
+/**
+* @brief  Dump queue states
+ *
+ * @return	None
+ *
+*/
+void ia_css_bufq_dump_queue_info(void);
+
+#endif	/* _IA_CSS_BUFQ_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq_comm.h b/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq_comm.h
new file mode 100644
index 000000000..567d94d91
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/bufq/interface/ia_css_bufq_comm.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_BUFQ_COMM_H
+#define _IA_CSS_BUFQ_COMM_H
+
+#include "system_global.h"
+
+enum sh_css_queue_id {
+	SH_CSS_INVALID_QUEUE_ID     = -1,
+	SH_CSS_QUEUE_A_ID = 0,
+	SH_CSS_QUEUE_B_ID,
+	SH_CSS_QUEUE_C_ID,
+	SH_CSS_QUEUE_D_ID,
+	SH_CSS_QUEUE_E_ID,
+	SH_CSS_QUEUE_F_ID,
+	SH_CSS_QUEUE_G_ID,
+	SH_CSS_QUEUE_H_ID, /* for metadata */
+
+#define SH_CSS_MAX_NUM_QUEUES (SH_CSS_QUEUE_H_ID + 1)
+
+};
+
+#define SH_CSS_MAX_DYNAMIC_BUFFERS_PER_THREAD SH_CSS_MAX_NUM_QUEUES
+/* for now we staticaly assign queue 0 & 1 to parameter sets */
+#define IA_CSS_PARAMETER_SET_QUEUE_ID SH_CSS_QUEUE_A_ID
+#define IA_CSS_PER_FRAME_PARAMETER_SET_QUEUE_ID SH_CSS_QUEUE_B_ID
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/bufq/src/bufq.c b/drivers/staging/media/atomisp/pci/runtime/bufq/src/bufq.c
new file mode 100644
index 000000000..6a75cba48
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/bufq/src/bufq.c
@@ -0,0 +1,532 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "assert_support.h"		/* assert */
+#include "ia_css_buffer.h"
+#include "sp.h"
+#include "ia_css_bufq.h"		/* Bufq API's */
+#include "ia_css_queue.h"		/* ia_css_queue_t */
+#include "sw_event_global.h"		/* Event IDs.*/
+#include "ia_css_eventq.h"		/* ia_css_eventq_recv()*/
+#include "ia_css_debug.h"		/* ia_css_debug_dtrace*/
+#include "sh_css_internal.h"		/* sh_css_queue_type */
+#include "sp_local.h"			/* sp_address_of */
+#include "sh_css_firmware.h"		/* sh_css_sp_fw*/
+
+#define BUFQ_DUMP_FILE_NAME_PREFIX_SIZE 256
+
+static char prefix[BUFQ_DUMP_FILE_NAME_PREFIX_SIZE] = {0};
+
+/*********************************************************/
+/* Global Queue objects used by CSS                      */
+/*********************************************************/
+
+struct sh_css_queues {
+	/* Host2SP buffer queue */
+	ia_css_queue_t host2sp_buffer_queue_handles
+	[SH_CSS_MAX_SP_THREADS][SH_CSS_MAX_NUM_QUEUES];
+	/* SP2Host buffer queue */
+	ia_css_queue_t sp2host_buffer_queue_handles
+	[SH_CSS_MAX_NUM_QUEUES];
+
+	/* Host2SP event queue */
+	ia_css_queue_t host2sp_psys_event_queue_handle;
+
+	/* SP2Host event queue */
+	ia_css_queue_t sp2host_psys_event_queue_handle;
+
+	/* Host2SP ISYS event queue */
+	ia_css_queue_t host2sp_isys_event_queue_handle;
+
+	/* SP2Host ISYS event queue */
+	ia_css_queue_t sp2host_isys_event_queue_handle;
+	/* Tagger command queue */
+	ia_css_queue_t host2sp_tag_cmd_queue_handle;
+};
+
+/*******************************************************
+*** Static variables
+********************************************************/
+static struct sh_css_queues css_queues;
+
+static int
+buffer_type_to_queue_id_map[SH_CSS_MAX_SP_THREADS][IA_CSS_NUM_DYNAMIC_BUFFER_TYPE];
+static bool queue_availability[SH_CSS_MAX_SP_THREADS][SH_CSS_MAX_NUM_QUEUES];
+
+/*******************************************************
+*** Static functions
+********************************************************/
+static void map_buffer_type_to_queue_id(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type
+);
+static void unmap_buffer_type_to_queue_id(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type
+);
+
+static ia_css_queue_t *bufq_get_qhandle(
+    enum sh_css_queue_type type,
+    enum sh_css_queue_id id,
+    int thread
+);
+
+/*******************************************************
+*** Public functions
+********************************************************/
+void ia_css_queue_map_init(void)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++) {
+		for (j = 0; j < SH_CSS_MAX_NUM_QUEUES; j++)
+			queue_availability[i][j] = true;
+	}
+
+	for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++) {
+		for (j = 0; j < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE; j++)
+			buffer_type_to_queue_id_map[i][j] = SH_CSS_INVALID_QUEUE_ID;
+	}
+}
+
+void ia_css_queue_map(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type,
+    bool map)
+{
+	assert(buf_type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE);
+	assert(thread_id < SH_CSS_MAX_SP_THREADS);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_queue_map() enter: buf_type=%d, thread_id=%d\n", buf_type, thread_id);
+
+	if (map)
+		map_buffer_type_to_queue_id(thread_id, buf_type);
+	else
+		unmap_buffer_type_to_queue_id(thread_id, buf_type);
+}
+
+/*
+ * @brief Query the internal queue ID.
+ */
+bool ia_css_query_internal_queue_id(
+    enum ia_css_buffer_type buf_type,
+    unsigned int thread_id,
+    enum sh_css_queue_id *val)
+{
+	IA_CSS_ENTER("buf_type=%d, thread_id=%d, val = %p", buf_type, thread_id, val);
+
+	if ((!val) || (thread_id >= SH_CSS_MAX_SP_THREADS) ||
+	    (buf_type >= IA_CSS_NUM_DYNAMIC_BUFFER_TYPE)) {
+		IA_CSS_LEAVE("return_val = false");
+		return false;
+	}
+
+	*val = buffer_type_to_queue_id_map[thread_id][buf_type];
+	if ((*val == SH_CSS_INVALID_QUEUE_ID) || (*val >= SH_CSS_MAX_NUM_QUEUES)) {
+		IA_CSS_LOG("INVALID queue ID MAP = %d\n", *val);
+		IA_CSS_LEAVE("return_val = false");
+		return false;
+	}
+	IA_CSS_LEAVE("return_val = true");
+	return true;
+}
+
+/*******************************************************
+*** Static functions
+********************************************************/
+static void map_buffer_type_to_queue_id(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type)
+{
+	unsigned int i;
+
+	assert(thread_id < SH_CSS_MAX_SP_THREADS);
+	assert(buf_type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE);
+	assert(buffer_type_to_queue_id_map[thread_id][buf_type] ==
+	       SH_CSS_INVALID_QUEUE_ID);
+
+	/* queue 0 is reserved for parameters because it doesn't depend on events */
+	if (buf_type == IA_CSS_BUFFER_TYPE_PARAMETER_SET) {
+		assert(queue_availability[thread_id][IA_CSS_PARAMETER_SET_QUEUE_ID]);
+		queue_availability[thread_id][IA_CSS_PARAMETER_SET_QUEUE_ID] = false;
+		buffer_type_to_queue_id_map[thread_id][buf_type] =
+		    IA_CSS_PARAMETER_SET_QUEUE_ID;
+		return;
+	}
+
+	/* queue 1 is reserved for per frame parameters because it doesn't depend on events */
+	if (buf_type == IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET) {
+		assert(queue_availability[thread_id][IA_CSS_PER_FRAME_PARAMETER_SET_QUEUE_ID]);
+		queue_availability[thread_id][IA_CSS_PER_FRAME_PARAMETER_SET_QUEUE_ID] = false;
+		buffer_type_to_queue_id_map[thread_id][buf_type] =
+		    IA_CSS_PER_FRAME_PARAMETER_SET_QUEUE_ID;
+		return;
+	}
+
+	for (i = SH_CSS_QUEUE_C_ID; i < SH_CSS_MAX_NUM_QUEUES; i++) {
+		if (queue_availability[thread_id][i]) {
+			queue_availability[thread_id][i] = false;
+			buffer_type_to_queue_id_map[thread_id][buf_type] = i;
+			break;
+		}
+	}
+
+	assert(i != SH_CSS_MAX_NUM_QUEUES);
+	return;
+}
+
+static void unmap_buffer_type_to_queue_id(
+    unsigned int thread_id,
+    enum ia_css_buffer_type buf_type)
+{
+	int queue_id;
+
+	assert(thread_id < SH_CSS_MAX_SP_THREADS);
+	assert(buf_type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE);
+	assert(buffer_type_to_queue_id_map[thread_id][buf_type] !=
+	       SH_CSS_INVALID_QUEUE_ID);
+
+	queue_id = buffer_type_to_queue_id_map[thread_id][buf_type];
+	buffer_type_to_queue_id_map[thread_id][buf_type] = SH_CSS_INVALID_QUEUE_ID;
+	queue_availability[thread_id][queue_id] = true;
+}
+
+static ia_css_queue_t *bufq_get_qhandle(
+    enum sh_css_queue_type type,
+    enum sh_css_queue_id id,
+    int thread)
+{
+	ia_css_queue_t *q = NULL;
+
+	switch (type) {
+	case sh_css_host2sp_buffer_queue:
+		if ((thread >= SH_CSS_MAX_SP_THREADS) || (thread < 0) ||
+		    (id == SH_CSS_INVALID_QUEUE_ID))
+			break;
+		q = &css_queues.host2sp_buffer_queue_handles[thread][id];
+		break;
+	case sh_css_sp2host_buffer_queue:
+		if (id == SH_CSS_INVALID_QUEUE_ID)
+			break;
+		q = &css_queues.sp2host_buffer_queue_handles[id];
+		break;
+	case sh_css_host2sp_psys_event_queue:
+		q = &css_queues.host2sp_psys_event_queue_handle;
+		break;
+	case sh_css_sp2host_psys_event_queue:
+		q = &css_queues.sp2host_psys_event_queue_handle;
+		break;
+	case sh_css_host2sp_isys_event_queue:
+		q = &css_queues.host2sp_isys_event_queue_handle;
+		break;
+	case sh_css_sp2host_isys_event_queue:
+		q = &css_queues.sp2host_isys_event_queue_handle;
+		break;
+	case sh_css_host2sp_tag_cmd_queue:
+		q = &css_queues.host2sp_tag_cmd_queue_handle;
+		break;
+	default:
+		break;
+	}
+
+	return q;
+}
+
+/* Local function to initialize a buffer queue. This reduces
+ * the chances of copy-paste errors or typos.
+ */
+static inline void
+init_bufq(unsigned int desc_offset,
+	  unsigned int elems_offset,
+	  ia_css_queue_t *handle)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int q_base_addr;
+	ia_css_queue_remote_t remoteq;
+
+	fw = &sh_css_sp_fw;
+	q_base_addr = fw->info.sp.host_sp_queue;
+
+	/* Setup queue location as SP and proc id as SP0_ID */
+	remoteq.location = IA_CSS_QUEUE_LOC_SP;
+	remoteq.proc_id = SP0_ID;
+	remoteq.cb_desc_addr = q_base_addr + desc_offset;
+	remoteq.cb_elems_addr = q_base_addr + elems_offset;
+	/* Initialize the queue instance and obtain handle */
+	ia_css_queue_remote_init(handle, &remoteq);
+}
+
+void ia_css_bufq_init(void)
+{
+	int i, j;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	/* Setup all the local queue descriptors for Host2SP Buffer Queues */
+	for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++)
+		for (j = 0; j < SH_CSS_MAX_NUM_QUEUES; j++) {
+			init_bufq((uint32_t)offsetof(struct host_sp_queues,
+						     host2sp_buffer_queues_desc[i][j]),
+				  (uint32_t)offsetof(struct host_sp_queues, host2sp_buffer_queues_elems[i][j]),
+				  &css_queues.host2sp_buffer_queue_handles[i][j]);
+		}
+
+	/* Setup all the local queue descriptors for SP2Host Buffer Queues */
+	for (i = 0; i < SH_CSS_MAX_NUM_QUEUES; i++) {
+		init_bufq(offsetof(struct host_sp_queues, sp2host_buffer_queues_desc[i]),
+			  offsetof(struct host_sp_queues, sp2host_buffer_queues_elems[i]),
+			  &css_queues.sp2host_buffer_queue_handles[i]);
+	}
+
+	/* Host2SP event queue*/
+	init_bufq((uint32_t)offsetof(struct host_sp_queues,
+				     host2sp_psys_event_queue_desc),
+		  (uint32_t)offsetof(struct host_sp_queues, host2sp_psys_event_queue_elems),
+		  &css_queues.host2sp_psys_event_queue_handle);
+
+	/* SP2Host event queue */
+	init_bufq((uint32_t)offsetof(struct host_sp_queues,
+				     sp2host_psys_event_queue_desc),
+		  (uint32_t)offsetof(struct host_sp_queues, sp2host_psys_event_queue_elems),
+		  &css_queues.sp2host_psys_event_queue_handle);
+
+	/* Host2SP ISYS event queue */
+	init_bufq((uint32_t)offsetof(struct host_sp_queues,
+				     host2sp_isys_event_queue_desc),
+		  (uint32_t)offsetof(struct host_sp_queues, host2sp_isys_event_queue_elems),
+		  &css_queues.host2sp_isys_event_queue_handle);
+
+	/* SP2Host ISYS event queue*/
+	init_bufq((uint32_t)offsetof(struct host_sp_queues,
+				     sp2host_isys_event_queue_desc),
+		  (uint32_t)offsetof(struct host_sp_queues, sp2host_isys_event_queue_elems),
+		  &css_queues.sp2host_isys_event_queue_handle);
+
+	/* Host2SP tagger command queue */
+	init_bufq((uint32_t)offsetof(struct host_sp_queues, host2sp_tag_cmd_queue_desc),
+		  (uint32_t)offsetof(struct host_sp_queues, host2sp_tag_cmd_queue_elems),
+		  &css_queues.host2sp_tag_cmd_queue_handle);
+
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+int ia_css_bufq_enqueue_buffer(
+    int thread_index,
+    int queue_id,
+    uint32_t item)
+{
+	ia_css_queue_t *q;
+	int error;
+
+	IA_CSS_ENTER_PRIVATE("queue_id=%d", queue_id);
+	if ((thread_index >= SH_CSS_MAX_SP_THREADS) || (thread_index < 0) ||
+	    (queue_id == SH_CSS_INVALID_QUEUE_ID))
+		return -EINVAL;
+
+	/* Get the queue for communication */
+	q = bufq_get_qhandle(sh_css_host2sp_buffer_queue,
+			     queue_id,
+			     thread_index);
+	if (q) {
+		error = ia_css_queue_enqueue(q, item);
+	} else {
+		IA_CSS_ERROR("queue is not initialized");
+		error = -EBUSY;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(error);
+	return error;
+}
+
+int ia_css_bufq_dequeue_buffer(
+    int queue_id,
+    uint32_t *item)
+{
+	int error;
+	ia_css_queue_t *q;
+
+	IA_CSS_ENTER_PRIVATE("queue_id=%d", queue_id);
+	if ((!item) ||
+	    (queue_id <= SH_CSS_INVALID_QUEUE_ID) ||
+	    (queue_id >= SH_CSS_MAX_NUM_QUEUES)
+	   )
+		return -EINVAL;
+
+	q = bufq_get_qhandle(sh_css_sp2host_buffer_queue,
+			     queue_id,
+			     -1);
+	if (q) {
+		error = ia_css_queue_dequeue(q, item);
+	} else {
+		IA_CSS_ERROR("queue is not initialized");
+		error = -EBUSY;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(error);
+	return error;
+}
+
+int ia_css_bufq_enqueue_psys_event(
+    u8 evt_id,
+    u8 evt_payload_0,
+    u8 evt_payload_1,
+    uint8_t evt_payload_2)
+{
+	int error = 0;
+	ia_css_queue_t *q;
+
+	IA_CSS_ENTER_PRIVATE("evt_id=%d", evt_id);
+	q = bufq_get_qhandle(sh_css_host2sp_psys_event_queue, -1, -1);
+	if (!q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return -EBUSY;
+	}
+
+	error = ia_css_eventq_send(q,
+				   evt_id, evt_payload_0, evt_payload_1, evt_payload_2);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(error);
+	return error;
+}
+
+int ia_css_bufq_dequeue_psys_event(
+    u8 item[BUFQ_EVENT_SIZE])
+{
+	int error = 0;
+	ia_css_queue_t *q;
+
+	/* No ENTER/LEAVE in this function since this is polled
+	 * by some test apps. Enablign logging here floods the log
+	 * files which may cause timeouts. */
+	if (!item)
+		return -EINVAL;
+
+	q = bufq_get_qhandle(sh_css_sp2host_psys_event_queue, -1, -1);
+	if (!q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return -EBUSY;
+	}
+	error = ia_css_eventq_recv(q, item);
+
+	return error;
+}
+
+int ia_css_bufq_dequeue_isys_event(
+    u8 item[BUFQ_EVENT_SIZE])
+{
+	int error = 0;
+	ia_css_queue_t *q;
+
+	/* No ENTER/LEAVE in this function since this is polled
+	 * by some test apps. Enablign logging here floods the log
+	 * files which may cause timeouts. */
+	if (!item)
+		return -EINVAL;
+
+	q = bufq_get_qhandle(sh_css_sp2host_isys_event_queue, -1, -1);
+	if (!q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return -EBUSY;
+	}
+	error = ia_css_eventq_recv(q, item);
+	return error;
+}
+
+int ia_css_bufq_enqueue_isys_event(uint8_t evt_id)
+{
+	int error = 0;
+	ia_css_queue_t *q;
+
+	IA_CSS_ENTER_PRIVATE("event_id=%d", evt_id);
+	q = bufq_get_qhandle(sh_css_host2sp_isys_event_queue, -1, -1);
+	if (!q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return -EBUSY;
+	}
+
+	error = ia_css_eventq_send(q, evt_id, 0, 0, 0);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(error);
+	return error;
+}
+
+int ia_css_bufq_enqueue_tag_cmd(
+    uint32_t item)
+{
+	int error;
+	ia_css_queue_t *q;
+
+	IA_CSS_ENTER_PRIVATE("item=%d", item);
+	q = bufq_get_qhandle(sh_css_host2sp_tag_cmd_queue, -1, -1);
+	if (!q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return -EBUSY;
+	}
+	error = ia_css_queue_enqueue(q, item);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(error);
+	return error;
+}
+
+int ia_css_bufq_deinit(void)
+{
+	return 0;
+}
+
+static void bufq_dump_queue_info(const char *prefix, ia_css_queue_t *qhandle)
+{
+	u32 free = 0, used = 0;
+
+	assert(prefix && qhandle);
+	ia_css_queue_get_used_space(qhandle, &used);
+	ia_css_queue_get_free_space(qhandle, &free);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s: used=%u free=%u\n",
+			    prefix, used, free);
+}
+
+void ia_css_bufq_dump_queue_info(void)
+{
+	int i, j;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "Queue Information:\n");
+
+	for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++) {
+		for (j = 0; j < SH_CSS_MAX_NUM_QUEUES; j++) {
+			snprintf(prefix, BUFQ_DUMP_FILE_NAME_PREFIX_SIZE,
+				 "host2sp_buffer_queue[%u][%u]", i, j);
+			bufq_dump_queue_info(prefix,
+					     &css_queues.host2sp_buffer_queue_handles[i][j]);
+		}
+	}
+
+	for (i = 0; i < SH_CSS_MAX_NUM_QUEUES; i++) {
+		snprintf(prefix, BUFQ_DUMP_FILE_NAME_PREFIX_SIZE,
+			 "sp2host_buffer_queue[%u]", i);
+		bufq_dump_queue_info(prefix,
+				     &css_queues.sp2host_buffer_queue_handles[i]);
+	}
+	bufq_dump_queue_info("host2sp_psys_event",
+			     &css_queues.host2sp_psys_event_queue_handle);
+	bufq_dump_queue_info("sp2host_psys_event",
+			     &css_queues.sp2host_psys_event_queue_handle);
+
+	bufq_dump_queue_info("host2sp_isys_event",
+			     &css_queues.host2sp_isys_event_queue_handle);
+	bufq_dump_queue_info("sp2host_isys_event",
+			     &css_queues.sp2host_isys_event_queue_handle);
+	bufq_dump_queue_info("host2sp_tag_cmd",
+			     &css_queues.host2sp_tag_cmd_queue_handle);
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug.h b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug.h
new file mode 100644
index 000000000..fff89e9b4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug.h
@@ -0,0 +1,500 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_DEBUG_H_
+#define _IA_CSS_DEBUG_H_
+
+/*! \file */
+
+#include <type_support.h>
+#include <linux/stdarg.h>
+#include <linux/bits.h>
+#include "ia_css_types.h"
+#include "ia_css_binary.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_stream_public.h"
+#include "ia_css_metadata.h"
+#include "sh_css_internal.h"
+/* ISP2500 */
+#include "ia_css_pipe.h"
+
+/* available levels */
+/*! Level for tracing errors */
+#define IA_CSS_DEBUG_ERROR   1
+/*! Level for tracing warnings */
+#define IA_CSS_DEBUG_WARNING 3
+/*! Level for tracing debug messages */
+#define IA_CSS_DEBUG_VERBOSE   5
+/*! Level for tracing trace messages a.o. ia_css public function calls */
+#define IA_CSS_DEBUG_TRACE   6
+/*! Level for tracing trace messages a.o. ia_css private function calls */
+#define IA_CSS_DEBUG_TRACE_PRIVATE   7
+/*! Level for tracing parameter messages e.g. in and out params of functions */
+#define IA_CSS_DEBUG_PARAM   8
+/*! Level for tracing info messages */
+#define IA_CSS_DEBUG_INFO    9
+
+/* Global variable which controls the verbosity levels of the debug tracing */
+extern int dbg_level;
+
+/*! @brief Enum defining the different isp parameters to dump.
+ *  Values can be combined to dump a combination of sets.
+ */
+enum ia_css_debug_enable_param_dump {
+	IA_CSS_DEBUG_DUMP_FPN = BIT(0),  /** FPN table */
+	IA_CSS_DEBUG_DUMP_OB  = BIT(1),  /** OB table */
+	IA_CSS_DEBUG_DUMP_SC  = BIT(2),  /** Shading table */
+	IA_CSS_DEBUG_DUMP_WB  = BIT(3),  /** White balance */
+	IA_CSS_DEBUG_DUMP_DP  = BIT(4),  /** Defect Pixel */
+	IA_CSS_DEBUG_DUMP_BNR = BIT(5),  /** Bayer Noise Reductions */
+	IA_CSS_DEBUG_DUMP_S3A = BIT(6),  /** 3A Statistics */
+	IA_CSS_DEBUG_DUMP_DE  = BIT(7),  /** De Mosaicing */
+	IA_CSS_DEBUG_DUMP_YNR = BIT(8),  /** Luma Noise Reduction */
+	IA_CSS_DEBUG_DUMP_CSC = BIT(9),  /** Color Space Conversion */
+	IA_CSS_DEBUG_DUMP_GC  = BIT(10), /** Gamma Correction */
+	IA_CSS_DEBUG_DUMP_TNR = BIT(11), /** Temporal Noise Reduction */
+	IA_CSS_DEBUG_DUMP_ANR = BIT(12), /** Advanced Noise Reduction */
+	IA_CSS_DEBUG_DUMP_CE  = BIT(13), /** Chroma Enhancement */
+	IA_CSS_DEBUG_DUMP_ALL = BIT(14), /** Dump all device parameters */
+};
+
+#define IA_CSS_ERROR(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR, \
+		"%s() %d: error: " fmt "\n", __func__, __LINE__, ##__VA_ARGS__)
+
+#define IA_CSS_WARNING(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_WARNING, \
+		"%s() %d: warning: " fmt "\n", __func__, __LINE__, ##__VA_ARGS__)
+
+/* Logging macros for public functions (API functions) */
+#define IA_CSS_ENTER(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, \
+		"%s(): enter: " fmt "\n", __func__, ##__VA_ARGS__)
+
+/* Use this macro for small functions that do not call other functions. */
+#define IA_CSS_ENTER_LEAVE(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, \
+		"%s(): enter: leave: " fmt "\n", __func__, ##__VA_ARGS__)
+
+#define IA_CSS_LEAVE(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, \
+		"%s(): leave: " fmt "\n", __func__, ##__VA_ARGS__)
+
+/* Shorthand for returning an int return value */
+#define IA_CSS_LEAVE_ERR(__err) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, \
+		"%s() %d: leave: return_err=%d\n", __func__, __LINE__, __err)
+
+/* Use this macro for logging other than enter/leave.
+ * Note that this macro always uses the PRIVATE logging level.
+ */
+#define IA_CSS_LOG(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, \
+		"%s(): " fmt "\n", __func__, ##__VA_ARGS__)
+
+/* Logging macros for non-API functions. These have a lower trace level */
+#define IA_CSS_ENTER_PRIVATE(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, \
+		"%s(): enter: " fmt "\n", __func__, ##__VA_ARGS__)
+
+#define IA_CSS_LEAVE_PRIVATE(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, \
+		"%s(): leave: " fmt "\n", __func__, ##__VA_ARGS__)
+
+/* Shorthand for returning an int return value */
+#define IA_CSS_LEAVE_ERR_PRIVATE(__err) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, \
+		"%s() %d: leave: return_err=%d\n", __func__, __LINE__, __err)
+
+/* Use this macro for small functions that do not call other functions. */
+#define IA_CSS_ENTER_LEAVE_PRIVATE(fmt, ...) \
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, \
+		"%s(): enter: leave: " fmt "\n", __func__, ##__VA_ARGS__)
+
+/*! @brief Function for tracing to the provided printf function in the
+ *	environment.
+ * @param[in]	level		Level of the message.
+ * @param[in]	fmt		printf like format string
+ * @param[in]	args		arguments for the format string
+ */
+static inline void __printf(2, 0) ia_css_debug_vdtrace(unsigned int level,
+						       const char *fmt,
+						       va_list args)
+{
+	if (dbg_level >= level)
+		sh_css_vprint(fmt, args);
+}
+
+__printf(2, 3) void ia_css_debug_dtrace(unsigned int level,
+					const char *fmt, ...);
+
+/*! @brief Dump sp thread's stack contents
+ * SP thread's stack contents are set to 0xcafecafe. This function dumps the
+ * stack to inspect if the stack's boundaries are compromised.
+ * @return	None
+ */
+void ia_css_debug_dump_sp_stack_info(void);
+
+/*! @brief Function to set the global dtrace verbosity level.
+ * @param[in]	trace_level	Maximum level of the messages to be traced.
+ * @return	None
+ */
+void ia_css_debug_set_dtrace_level(
+    const unsigned int	trace_level);
+
+/*! @brief Function to get the global dtrace verbosity level.
+ * @return	global dtrace verbosity level
+ */
+unsigned int ia_css_debug_get_dtrace_level(void);
+
+/*! @brief Dump isp hardware state.
+ * Dumps the isp hardware state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_isp_state(void);
+
+/*! @brief Dump sp hardware state.
+ * Dumps the sp hardware state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_sp_state(void);
+
+/* ISP2401 */
+/*! @brief Dump GAC hardware state.
+ * Dumps the GAC ACB hardware registers. may be useful for
+ * detecting a GAC which got hang.
+ * @return	None
+ */
+void ia_css_debug_dump_gac_state(void);
+
+/*! @brief Dump dma controller state.
+ * Dumps the dma controller state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_dma_state(void);
+
+/*! @brief Dump internal sp software state.
+ * Dumps the sp software state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_sp_sw_debug_info(void);
+
+/*! @brief Dump all related hardware state to the trace output
+ * @param[in]  context	String to identify context in output.
+ * @return	None
+ */
+void ia_css_debug_dump_debug_info(
+    const char	*context);
+
+#if SP_DEBUG != SP_DEBUG_NONE
+void ia_css_debug_print_sp_debug_state(
+    const struct sh_css_sp_debug_state *state);
+#endif
+
+/*! @brief Dump all related binary info data
+ * @param[in]  bi	Binary info struct.
+ * @return	None
+ */
+void ia_css_debug_binary_print(
+    const struct ia_css_binary *bi);
+
+void ia_css_debug_sp_dump_mipi_fifo_high_water(void);
+
+/*! @brief Dump isp gdc fifo state to the trace output
+ * Dumps the isp gdc fifo state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_isp_gdc_fifo_state(void);
+
+/*! @brief Dump dma isp fifo state
+ * Dumps the dma isp fifo state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_dma_isp_fifo_state(void);
+
+/*! @brief Dump dma sp fifo state
+ * Dumps the dma sp fifo state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_dma_sp_fifo_state(void);
+
+/*! \brief Dump pif A isp fifo state
+ * Dumps the primary input formatter state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_pif_a_isp_fifo_state(void);
+
+/*! \brief Dump pif B isp fifo state
+ * Dumps the primary input formatter state to tracing output.
+ * \return	None
+ */
+void ia_css_debug_dump_pif_b_isp_fifo_state(void);
+
+/*! @brief Dump stream-to-memory sp fifo state
+ * Dumps the stream-to-memory block state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_str2mem_sp_fifo_state(void);
+
+/*! @brief Dump isp sp fifo state
+ * Dumps the isp sp fifo state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_isp_sp_fifo_state(void);
+
+/*! @brief Dump all fifo state info to the output
+ * Dumps all fifo state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_all_fifo_state(void);
+
+/*! @brief Dump the rx state to the output
+ * Dumps the rx state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_rx_state(void);
+
+/*! @brief Dump the input system state to the output
+ * Dumps the input system state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_isys_state(void);
+
+/*! @brief Dump the frame info to the trace output
+ * Dumps the frame info to tracing output.
+ * @param[in]	frame		pointer to struct ia_css_frame
+ * @param[in]	descr		description output along with the frame info
+ * @return	None
+ */
+void ia_css_debug_frame_print(
+    const struct ia_css_frame	*frame,
+    const char	*descr);
+
+/*! @brief Function to enable sp sleep mode.
+ * Function that enables sp sleep mode
+ * @param[in]	mode		indicates when to put sp to sleep
+ * @return	None
+ */
+void ia_css_debug_enable_sp_sleep_mode(enum ia_css_sp_sleep_mode mode);
+
+/*! @brief Function to wake up sp when in sleep mode.
+ * After sp has been put to sleep, use this function to let it continue
+ * to run again.
+ * @return	None
+ */
+void ia_css_debug_wake_up_sp(void);
+
+/*! @brief Function to dump isp parameters.
+ * Dump isp parameters to tracing output
+ * @param[in]	stream		pointer to ia_css_stream struct
+ * @param[in]	enable		flag indicating which parameters to dump.
+ * @return	None
+ */
+void ia_css_debug_dump_isp_params(struct ia_css_stream *stream,
+				  unsigned int enable);
+
+/*! @brief Function to dump some sp performance counters.
+ * Dump sp performance counters, currently input system errors.
+ * @return	None
+ */
+void ia_css_debug_dump_perf_counters(void);
+
+#ifdef HAS_WATCHDOG_SP_THREAD_DEBUG
+void sh_css_dump_thread_wait_info(void);
+void sh_css_dump_pipe_stage_info(void);
+void sh_css_dump_pipe_stripe_info(void);
+#endif
+
+void ia_css_debug_dump_isp_binary(void);
+
+void sh_css_dump_sp_raw_copy_linecount(bool reduced);
+
+/*! @brief Dump the resolution info to the trace output
+ * Dumps the resolution info to the trace output.
+ * @param[in]	res	pointer to struct ia_css_resolution
+ * @param[in]	label	description of resolution output
+ * @return	None
+ */
+void ia_css_debug_dump_resolution(
+    const struct ia_css_resolution *res,
+    const char *label);
+
+/*! @brief Dump the frame info to the trace output
+ * Dumps the frame info to the trace output.
+ * @param[in]	info	pointer to struct ia_css_frame_info
+ * @param[in]	label	description of frame_info output
+ * @return	None
+ */
+void ia_css_debug_dump_frame_info(
+    const struct ia_css_frame_info *info,
+    const char *label);
+
+/*! @brief Dump the capture config info to the trace output
+ * Dumps the capture config info to the trace output.
+ * @param[in]	config	pointer to struct ia_css_capture_config
+ * @return	None
+ */
+void ia_css_debug_dump_capture_config(
+    const struct ia_css_capture_config *config);
+
+/*! @brief Dump the pipe extra config info to the trace output
+ * Dumps the pipe extra config info to the trace output.
+ * @param[in]	extra_config	pointer to struct ia_css_pipe_extra_config
+ * @return	None
+ */
+void ia_css_debug_dump_pipe_extra_config(
+    const struct ia_css_pipe_extra_config *extra_config);
+
+/*! @brief Dump the pipe config info to the trace output
+ * Dumps the pipe config info to the trace output.
+ * @param[in]	config	pointer to struct ia_css_pipe_config
+ * @return	None
+ */
+void ia_css_debug_dump_pipe_config(
+    const struct ia_css_pipe_config *config);
+
+/*! @brief Dump the stream config source info to the trace output
+ * Dumps the stream config source info to the trace output.
+ * @param[in]	config	pointer to struct ia_css_stream_config
+ * @return	None
+ */
+void ia_css_debug_dump_stream_config_source(
+    const struct ia_css_stream_config *config);
+
+/*! @brief Dump the mipi buffer config info to the trace output
+ * Dumps the mipi buffer config info to the trace output.
+ * @param[in]	config	pointer to struct ia_css_mipi_buffer_config
+ * @return	None
+ */
+void ia_css_debug_dump_mipi_buffer_config(
+    const struct ia_css_mipi_buffer_config *config);
+
+/*! @brief Dump the metadata config info to the trace output
+ * Dumps the metadata config info to the trace output.
+ * @param[in]	config	pointer to struct ia_css_metadata_config
+ * @return	None
+ */
+void ia_css_debug_dump_metadata_config(
+    const struct ia_css_metadata_config *config);
+
+/*! @brief Dump the stream config info to the trace output
+ * Dumps the stream config info to the trace output.
+ * @param[in]	config		pointer to struct ia_css_stream_config
+ * @param[in]	num_pipes	number of pipes for the stream
+ * @return	None
+ */
+void ia_css_debug_dump_stream_config(
+    const struct ia_css_stream_config *config,
+    int num_pipes);
+
+/*! @brief Dump the state of the SP tagger
+ * Dumps the internal state of the SP tagger
+ * @return	None
+ */
+void ia_css_debug_tagger_state(void);
+
+/**
+ * @brief Initialize the debug mode.
+ *
+ * WARNING:
+ * This API should be called ONLY once in the debug mode.
+ *
+ * @return
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool ia_css_debug_mode_init(void);
+
+/**
+ * @brief Disable the DMA channel.
+ *
+ * @param[in]	dma_ID		The ID of the target DMA.
+ * @param[in]	channel_id	The ID of the target DMA channel.
+ * @param[in]	request_type	The type of the DMA request.
+ *				For example:
+ *				- "0" indicates the writing request.
+ *				- "1" indicates the reading request.
+ *
+ * This is part of the DMA API -> dma.h
+ *
+ * @return
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool ia_css_debug_mode_disable_dma_channel(
+    int dma_ID,
+    int channel_id,
+    int request_type);
+/**
+ * @brief Enable the DMA channel.
+ *
+ * @param[in]	dma_ID		The ID of the target DMA.
+ * @param[in]	channel_id	The ID of the target DMA channel.
+ * @param[in]	request_type	The type of the DMA request.
+ *				For example:
+ *				- "0" indicates the writing request.
+ *				- "1" indicates the reading request.
+ *
+ * @return
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool ia_css_debug_mode_enable_dma_channel(
+    int dma_ID,
+    int channel_id,
+    int request_type);
+
+/**
+ * @brief Dump tracer data.
+ * [Currently support is only for SKC]
+ *
+ * @return
+ *	- none.
+ */
+void ia_css_debug_dump_trace(void);
+
+/* ISP2401 */
+/**
+ * @brief Program counter dumping (in loop)
+ *
+ * @param[in]	id		The ID of the SP
+ * @param[in]	num_of_dumps	The number of dumps
+ *
+ * @return
+ *	- none
+ */
+void ia_css_debug_pc_dump(sp_ID_t id, unsigned int num_of_dumps);
+
+/* ISP2500 */
+/*! @brief Dump all states for ISP hang case.
+ * Dumps the ISP previous and current configurations
+ * GACs status, SP0/1 statuses.
+ *
+ * @param[in]	pipe	The current pipe
+ *
+ * @return	None
+ */
+void ia_css_debug_dump_hang_status(
+    struct ia_css_pipe *pipe);
+
+/*! @brief External command handler
+ * External command handler
+ *
+ * @return	None
+ */
+void ia_css_debug_ext_command_handler(void);
+
+#endif /* _IA_CSS_DEBUG_H_ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_internal.h b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_internal.h
new file mode 100644
index 000000000..8ec487ad4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_internal.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/* TO DO: Move debug related code from ia_css_internal.h in */
diff --git a/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_pipe.h b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_pipe.h
new file mode 100644
index 000000000..538918cfb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/debug/interface/ia_css_debug_pipe.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_DEBUG_PIPE_H_
+#define _IA_CSS_DEBUG_PIPE_H_
+
+/*! \file */
+
+#include <ia_css_frame_public.h>
+#include <ia_css_stream_public.h>
+#include "ia_css_pipeline.h"
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ *
+ * @return	None
+ */
+void ia_css_debug_pipe_graph_dump_prologue(void);
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ *
+ * @return	None
+ */
+void ia_css_debug_pipe_graph_dump_epilogue(void);
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ * @param[in]	stage		Pipeline stage.
+ * @param[in]	id		Pipe id.
+ *
+ * @return	None
+ */
+void ia_css_debug_pipe_graph_dump_stage(
+    struct ia_css_pipeline_stage *stage,
+    enum ia_css_pipe_id id);
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ * @param[in]	out_frame	Output frame of SP raw copy.
+ *
+ * @return	None
+ */
+void ia_css_debug_pipe_graph_dump_sp_raw_copy(
+    struct ia_css_frame *out_frame);
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ * @param[in]	stream_config	info about sensor and input formatter.
+ *
+ * @return	None
+ */
+void ia_css_debug_pipe_graph_dump_stream_config(
+    const struct ia_css_stream_config *stream_config);
+
+#endif /* _IA_CSS_DEBUG_PIPE_H_ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/debug/src/ia_css_debug.c b/drivers/staging/media/atomisp/pci/runtime/debug/src/ia_css_debug.c
new file mode 100644
index 000000000..3d269bd23
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/debug/src/ia_css_debug.c
@@ -0,0 +1,3410 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "debug.h"
+
+#ifndef __INLINE_INPUT_SYSTEM__
+#define __INLINE_INPUT_SYSTEM__
+#endif
+#ifndef __INLINE_IBUF_CTRL__
+#define __INLINE_IBUF_CTRL__
+#endif
+#ifndef __INLINE_CSI_RX__
+#define __INLINE_CSI_RX__
+#endif
+#ifndef __INLINE_PIXELGEN__
+#define __INLINE_PIXELGEN__
+#endif
+#ifndef __INLINE_STREAM2MMIO__
+#define __INLINE_STREAM2MMIO__
+#endif
+
+#include <linux/string.h> /* for strscpy() */
+
+#include "ia_css_debug.h"
+#include "ia_css_debug_pipe.h"
+#include "ia_css_irq.h"
+#include "ia_css_stream.h"
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_param.h"
+#include "sh_css_params.h"
+#include "ia_css_bufq.h"
+/* ISP2401 */
+#include "ia_css_queue.h"
+
+#include "ia_css_isp_params.h"
+
+#include "system_local.h"
+#include "assert_support.h"
+#include "print_support.h"
+
+#include "fifo_monitor.h"
+
+#include "input_formatter.h"
+#include "dma.h"
+#include "irq.h"
+#include "gp_device.h"
+#include "sp.h"
+#include "isp.h"
+#include "type_support.h"
+#include "math_support.h" /* CEIL_DIV */
+#include "input_system.h"	/* input_formatter_reg_load */
+#include "ia_css_tagger_common.h"
+
+#include "sh_css_internal.h"
+#include "ia_css_isys.h"
+#include "sh_css_sp.h"		/* sh_css_sp_get_debug_state() */
+
+#include "css_trace.h"      /* tracer */
+
+#include "device_access.h"	/* for ia_css_device_load_uint32 */
+
+/* Include all kernel host interfaces for ISP1 */
+#include "anr/anr_1.0/ia_css_anr.host.h"
+#include "cnr/cnr_1.0/ia_css_cnr.host.h"
+#include "csc/csc_1.0/ia_css_csc.host.h"
+#include "de/de_1.0/ia_css_de.host.h"
+#include "dp/dp_1.0/ia_css_dp.host.h"
+#include "bnr/bnr_1.0/ia_css_bnr.host.h"
+#include "fpn/fpn_1.0/ia_css_fpn.host.h"
+#include "gc/gc_1.0/ia_css_gc.host.h"
+#include "ob/ob_1.0/ia_css_ob.host.h"
+#include "s3a/s3a_1.0/ia_css_s3a.host.h"
+#include "sc/sc_1.0/ia_css_sc.host.h"
+#include "tnr/tnr_1.0/ia_css_tnr.host.h"
+#include "uds/uds_1.0/ia_css_uds_param.h"
+#include "wb/wb_1.0/ia_css_wb.host.h"
+#include "ynr/ynr_1.0/ia_css_ynr.host.h"
+
+/* Include additional kernel host interfaces for ISP2 */
+#include "aa/aa_2/ia_css_aa2.host.h"
+#include "anr/anr_2/ia_css_anr2.host.h"
+#include "cnr/cnr_2/ia_css_cnr2.host.h"
+#include "de/de_2/ia_css_de2.host.h"
+#include "gc/gc_2/ia_css_gc2.host.h"
+#include "ynr/ynr_2/ia_css_ynr2.host.h"
+
+#define DPG_START "ia_css_debug_pipe_graph_dump_start "
+#define DPG_END   " ia_css_debug_pipe_graph_dump_end\n"
+
+#define ENABLE_LINE_MAX_LENGTH (25)
+
+/*
+ * TODO:SH_CSS_MAX_SP_THREADS is not the max number of sp threads
+ * future rework should fix this and remove the define MAX_THREAD_NUM
+ */
+#define MAX_THREAD_NUM (SH_CSS_MAX_SP_THREADS + SH_CSS_MAX_SP_INTERNAL_THREADS)
+
+static struct pipe_graph_class {
+	bool do_init;
+	int height;
+	int width;
+	int eff_height;
+	int eff_width;
+	enum atomisp_input_format stream_format;
+} pg_inst = {true, 0, 0, 0, 0, N_ATOMISP_INPUT_FORMAT};
+
+static const char *const queue_id_to_str[] = {
+	/* [SH_CSS_QUEUE_A_ID]     =*/ "queue_A",
+	/* [SH_CSS_QUEUE_B_ID]     =*/ "queue_B",
+	/* [SH_CSS_QUEUE_C_ID]     =*/ "queue_C",
+	/* [SH_CSS_QUEUE_D_ID]     =*/ "queue_D",
+	/* [SH_CSS_QUEUE_E_ID]     =*/ "queue_E",
+	/* [SH_CSS_QUEUE_F_ID]     =*/ "queue_F",
+	/* [SH_CSS_QUEUE_G_ID]     =*/ "queue_G",
+	/* [SH_CSS_QUEUE_H_ID]     =*/ "queue_H"
+};
+
+static const char *const pipe_id_to_str[] = {
+	/* [IA_CSS_PIPE_ID_PREVIEW]   =*/ "preview",
+	/* [IA_CSS_PIPE_ID_COPY]      =*/ "copy",
+	/* [IA_CSS_PIPE_ID_VIDEO]     =*/ "video",
+	/* [IA_CSS_PIPE_ID_CAPTURE]   =*/ "capture",
+	/* [IA_CSS_PIPE_ID_YUVPP]     =*/ "yuvpp",
+	/* [IA_CSS_PIPE_ID_ACC]       =*/ "accelerator"
+};
+
+static char dot_id_input_bin[SH_CSS_MAX_BINARY_NAME + 10];
+static char ring_buffer[200];
+
+void ia_css_debug_dtrace(unsigned int level, const char *fmt, ...)
+{
+	va_list ap;
+
+	va_start(ap, fmt);
+	ia_css_debug_vdtrace(level, fmt, ap);
+	va_end(ap);
+}
+
+static void debug_dump_long_array_formatted(
+    const sp_ID_t sp_id,
+    hrt_address stack_sp_addr,
+    unsigned int stack_size)
+{
+	unsigned int i;
+	u32 val;
+	u32 addr = (uint32_t)stack_sp_addr;
+	u32 stack_size_words = CEIL_DIV(stack_size, sizeof(uint32_t));
+
+	/* When size is not multiple of four, last word is only relevant for
+	 * remaining bytes */
+	for (i = 0; i < stack_size_words; i++) {
+		val = sp_dmem_load_uint32(sp_id, (hrt_address)addr);
+		if ((i % 8) == 0)
+			ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "\n");
+
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "0x%08x ", val);
+		addr += sizeof(uint32_t);
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "\n");
+}
+
+static void debug_dump_sp_stack_info(
+    const sp_ID_t sp_id)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_sp_threads_stack;
+	unsigned int HIVE_ADDR_sp_threads_stack_size;
+	u32 stack_sizes[MAX_THREAD_NUM];
+	u32 stack_sp_addr[MAX_THREAD_NUM];
+	unsigned int i;
+
+	fw = &sh_css_sp_fw;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "sp_id(%u) stack info\n", sp_id);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "from objects stack_addr_offset:0x%x stack_size_offset:0x%x\n",
+			    fw->info.sp.threads_stack,
+			    fw->info.sp.threads_stack_size);
+
+	HIVE_ADDR_sp_threads_stack = fw->info.sp.threads_stack;
+	HIVE_ADDR_sp_threads_stack_size = fw->info.sp.threads_stack_size;
+
+	if (fw->info.sp.threads_stack == 0 ||
+	    fw->info.sp.threads_stack_size == 0)
+		return;
+
+	(void)HIVE_ADDR_sp_threads_stack;
+	(void)HIVE_ADDR_sp_threads_stack_size;
+
+	sp_dmem_load(sp_id,
+		     (unsigned int)sp_address_of(sp_threads_stack),
+		     &stack_sp_addr, sizeof(stack_sp_addr));
+	sp_dmem_load(sp_id,
+		     (unsigned int)sp_address_of(sp_threads_stack_size),
+		     &stack_sizes, sizeof(stack_sizes));
+
+	for (i = 0 ; i < MAX_THREAD_NUM; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "thread: %u stack_addr: 0x%08x stack_size: %u\n",
+				    i, stack_sp_addr[i], stack_sizes[i]);
+		debug_dump_long_array_formatted(sp_id, (hrt_address)stack_sp_addr[i],
+						stack_sizes[i]);
+	}
+}
+
+void ia_css_debug_dump_sp_stack_info(void)
+{
+	debug_dump_sp_stack_info(SP0_ID);
+}
+
+void ia_css_debug_set_dtrace_level(const unsigned int trace_level)
+{
+	dbg_level = trace_level;
+	return;
+}
+
+unsigned int ia_css_debug_get_dtrace_level(void)
+{
+	return dbg_level;
+}
+
+static const char *debug_stream_format2str(const enum atomisp_input_format
+	stream_format)
+{
+	switch (stream_format) {
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+		return "yuv420-8-legacy";
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+		return "yuv420-8";
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+		return "yuv420-10";
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+		return "yuv420-16";
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+		return "yuv422-8";
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+		return "yuv422-10";
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		return "yuv422-16";
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+		return "rgb444";
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+		return "rgb555";
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+		return "rgb565";
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+		return "rgb666";
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		return "rgb888";
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+		return "raw6";
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+		return "raw7";
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+		return "raw8";
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+		return "raw10";
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+		return "raw12";
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+		return "raw14";
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		return "raw16";
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+		return "binary8";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT1:
+		return "generic-short1";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT2:
+		return "generic-short2";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT3:
+		return "generic-short3";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT4:
+		return "generic-short4";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT5:
+		return "generic-short5";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT6:
+		return "generic-short6";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT7:
+		return "generic-short7";
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT8:
+		return "generic-short8";
+	case ATOMISP_INPUT_FORMAT_YUV420_8_SHIFT:
+		return "yuv420-8-shift";
+	case ATOMISP_INPUT_FORMAT_YUV420_10_SHIFT:
+		return "yuv420-10-shift";
+	case ATOMISP_INPUT_FORMAT_EMBEDDED:
+		return "embedded-8";
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+		return "user-def-8-type-1";
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+		return "user-def-8-type-2";
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+		return "user-def-8-type-3";
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+		return "user-def-8-type-4";
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+		return "user-def-8-type-5";
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+		return "user-def-8-type-6";
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+		return "user-def-8-type-7";
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		return "user-def-8-type-8";
+
+	default:
+		assert(!"Unknown stream format");
+		return "unknown-stream-format";
+	}
+};
+
+static const char *debug_frame_format2str(const enum ia_css_frame_format
+	frame_format)
+{
+	switch (frame_format) {
+	case IA_CSS_FRAME_FORMAT_NV11:
+		return "NV11";
+	case IA_CSS_FRAME_FORMAT_NV12:
+		return "NV12";
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+		return "NV12_16";
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+		return "NV12_TILEY";
+	case IA_CSS_FRAME_FORMAT_NV16:
+		return "NV16";
+	case IA_CSS_FRAME_FORMAT_NV21:
+		return "NV21";
+	case IA_CSS_FRAME_FORMAT_NV61:
+		return "NV61";
+	case IA_CSS_FRAME_FORMAT_YV12:
+		return "YV12";
+	case IA_CSS_FRAME_FORMAT_YV16:
+		return "YV16";
+	case IA_CSS_FRAME_FORMAT_YUV420:
+		return "YUV420";
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+		return "YUV420_16";
+	case IA_CSS_FRAME_FORMAT_YUV422:
+		return "YUV422";
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+		return "YUV422_16";
+	case IA_CSS_FRAME_FORMAT_UYVY:
+		return "UYVY";
+	case IA_CSS_FRAME_FORMAT_YUYV:
+		return "YUYV";
+	case IA_CSS_FRAME_FORMAT_YUV444:
+		return "YUV444";
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+		return "YUV_LINE";
+	case IA_CSS_FRAME_FORMAT_RAW:
+		return "RAW";
+	case IA_CSS_FRAME_FORMAT_RGB565:
+		return "RGB565";
+	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
+		return "PLANAR_RGB888";
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+		return "RGBA888";
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+		return "QPLANE6";
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+		return "BINARY_8";
+	case IA_CSS_FRAME_FORMAT_MIPI:
+		return "MIPI";
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+		return "RAW_PACKED";
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+		return "CSI_MIPI_YUV420_8";
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+		return "CSI_MIPI_LEGACY_YUV420_8";
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_10:
+		return "CSI_MIPI_YUV420_10";
+
+	default:
+		assert(!"Unknown frame format");
+		return "unknown-frame-format";
+	}
+}
+
+static void debug_print_sp_state(const sp_state_t *state, const char *cell)
+{
+	assert(cell);
+	assert(state);
+
+	ia_css_debug_dtrace(2, "%s state:\n", cell);
+	ia_css_debug_dtrace(2, "\t%-32s: 0x%X\n", "PC", state->pc);
+	ia_css_debug_dtrace(2, "\t%-32s: 0x%X\n", "Status register",
+			    state->status_register);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is broken", state->is_broken);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is idle", state->is_idle);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is sleeping",
+			    state->is_sleeping);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is stalling",
+			    state->is_stalling);
+	return;
+}
+
+static void debug_print_isp_state(const isp_state_t *state, const char *cell)
+{
+	assert(state);
+	assert(cell);
+
+	ia_css_debug_dtrace(2, "%s state:\n", cell);
+	ia_css_debug_dtrace(2, "\t%-32s: 0x%X\n", "PC", state->pc);
+	ia_css_debug_dtrace(2, "\t%-32s: 0x%X\n", "Status register",
+			    state->status_register);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is broken", state->is_broken);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is idle", state->is_idle);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is sleeping",
+			    state->is_sleeping);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "Is stalling",
+			    state->is_stalling);
+	return;
+}
+
+void ia_css_debug_dump_isp_state(void)
+{
+	isp_state_t state;
+	isp_stall_t stall;
+
+	isp_get_state(ISP0_ID, &state, &stall);
+
+	debug_print_isp_state(&state, "ISP");
+
+	if (state.is_stalling) {
+		if (!IS_ISP2401) {
+			ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+					    "[0] if_prim_a_FIFO stalled", stall.fifo0);
+			ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+					    "[1] if_prim_b_FIFO stalled", stall.fifo1);
+		}
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[2] dma_FIFO stalled",
+				    stall.fifo2);
+
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[3] gdc0_FIFO stalled",
+				    stall.fifo3);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[4] gdc1_FIFO stalled",
+				    stall.fifo4);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[5] gpio_FIFO stalled",
+				    stall.fifo5);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[6] sp_FIFO stalled",
+				    stall.fifo6);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "status & control stalled",
+				    stall.stat_ctrl);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "dmem stalled",
+				    stall.dmem);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "vmem stalled",
+				    stall.vmem);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "vamem1 stalled",
+				    stall.vamem1);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "vamem2 stalled",
+				    stall.vamem2);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "vamem3 stalled",
+				    stall.vamem3);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "hmem stalled",
+				    stall.hmem);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "pmem stalled",
+				    stall.pmem);
+	}
+	return;
+}
+
+void ia_css_debug_dump_sp_state(void)
+{
+	sp_state_t state;
+	sp_stall_t stall;
+
+	sp_get_state(SP0_ID, &state, &stall);
+	debug_print_sp_state(&state, "SP");
+	if (state.is_stalling) {
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "isys_FIFO stalled",
+				    stall.fifo0);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "if_sec_FIFO stalled",
+				    stall.fifo1);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "str_to_mem_FIFO stalled", stall.fifo2);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "dma_FIFO stalled",
+				    stall.fifo3);
+		if (!IS_ISP2401)
+			ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+					    "if_prim_a_FIFO stalled", stall.fifo4);
+
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "isp_FIFO stalled",
+				    stall.fifo5);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "gp_FIFO stalled",
+				    stall.fifo6);
+		if (!IS_ISP2401)
+			ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+					    "if_prim_b_FIFO stalled", stall.fifo7);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "gdc0_FIFO stalled",
+				    stall.fifo8);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "gdc1_FIFO stalled",
+				    stall.fifo9);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "irq FIFO stalled",
+				    stall.fifoa);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "dmem stalled",
+				    stall.dmem);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "control master stalled",
+				    stall.control_master);
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "i-cache master stalled",
+				    stall.icache_master);
+	}
+	ia_css_debug_dump_trace();
+	return;
+}
+
+static void debug_print_fifo_channel_state(const fifo_channel_state_t *state,
+	const char *descr)
+{
+	assert(state);
+	assert(descr);
+
+	ia_css_debug_dtrace(2, "FIFO channel: %s\n", descr);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "source valid",
+			    state->src_valid);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "fifo accept",
+			    state->fifo_accept);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "fifo valid",
+			    state->fifo_valid);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "sink accept",
+			    state->sink_accept);
+	return;
+}
+
+void ia_css_debug_dump_pif_a_isp_fifo_state(void)
+{
+	fifo_channel_state_t pif_to_isp, isp_to_pif;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_IF0_TO_ISP0, &pif_to_isp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_ISP0_TO_IF0, &isp_to_pif);
+	debug_print_fifo_channel_state(&pif_to_isp, "Primary IF A to ISP");
+	debug_print_fifo_channel_state(&isp_to_pif, "ISP to Primary IF A");
+}
+
+void ia_css_debug_dump_pif_b_isp_fifo_state(void)
+{
+	fifo_channel_state_t pif_to_isp, isp_to_pif;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_IF1_TO_ISP0, &pif_to_isp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_ISP0_TO_IF1, &isp_to_pif);
+	debug_print_fifo_channel_state(&pif_to_isp, "Primary IF B to ISP");
+	debug_print_fifo_channel_state(&isp_to_pif, "ISP to Primary IF B");
+}
+
+void ia_css_debug_dump_str2mem_sp_fifo_state(void)
+{
+	fifo_channel_state_t s2m_to_sp, sp_to_s2m;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_STREAM2MEM0_TO_SP0, &s2m_to_sp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_SP0_TO_STREAM2MEM0, &sp_to_s2m);
+	debug_print_fifo_channel_state(&s2m_to_sp, "Stream-to-memory to SP");
+	debug_print_fifo_channel_state(&sp_to_s2m, "SP to stream-to-memory");
+}
+
+#ifndef ISP2401
+static void debug_print_if_state(input_formatter_state_t *state, const char *id)
+{
+	unsigned int val;
+
+	const char *st_vsync_active_low =
+	    (state->vsync_active_low ? "low" : "high");
+	const char *st_hsync_active_low =
+	    (state->hsync_active_low ? "low" : "high");
+
+	const char *fsm_sync_status_str = "unknown";
+	const char *fsm_crop_status_str = "unknown";
+	const char *fsm_padding_status_str = "unknown";
+
+	int st_stline = state->start_line;
+	int st_stcol = state->start_column;
+	int st_crpht = state->cropped_height;
+	int st_crpwd = state->cropped_width;
+	int st_verdcm = state->ver_decimation;
+	int st_hordcm = state->hor_decimation;
+	int st_ver_deinterleaving = state->ver_deinterleaving;
+	int st_hor_deinterleaving = state->hor_deinterleaving;
+	int st_leftpd = state->left_padding;
+	int st_eoloff = state->eol_offset;
+	int st_vmstartaddr = state->vmem_start_address;
+	int st_vmendaddr = state->vmem_end_address;
+	int st_vmincr = state->vmem_increment;
+	int st_yuv420 = state->is_yuv420;
+	int st_allow_fifo_overflow = state->allow_fifo_overflow;
+	int st_block_fifo_when_no_req = state->block_fifo_when_no_req;
+
+	assert(state);
+	ia_css_debug_dtrace(2, "InputFormatter State (%s):\n", id);
+
+	ia_css_debug_dtrace(2, "\tConfiguration:\n");
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Start line", st_stline);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Start column", st_stcol);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Cropped height", st_crpht);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Cropped width", st_crpwd);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Ver decimation", st_verdcm);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Hor decimation", st_hordcm);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Ver deinterleaving", st_ver_deinterleaving);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Hor deinterleaving", st_hor_deinterleaving);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Left padding", st_leftpd);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "EOL offset (bytes)", st_eoloff);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%06X\n",
+			    "VMEM start address", st_vmstartaddr);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%06X\n",
+			    "VMEM end address", st_vmendaddr);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%06X\n",
+			    "VMEM increment", st_vmincr);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "YUV 420 format", st_yuv420);
+	ia_css_debug_dtrace(2, "\t\t%-32s: Active %s\n",
+			    "Vsync", st_vsync_active_low);
+	ia_css_debug_dtrace(2, "\t\t%-32s: Active %s\n",
+			    "Hsync", st_hsync_active_low);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Allow FIFO overflow", st_allow_fifo_overflow);
+	/* Flag that tells whether the IF gives backpressure on frames */
+	/*
+	 * FYI, this is only on the frame request (indicate), when the IF has
+	 * synch'd on a frame it will always give back pressure
+	 */
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Block when no request", st_block_fifo_when_no_req);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "IF_BLOCKED_FIFO_NO_REQ_ADDRESS",
+			    input_formatter_reg_load(INPUT_FORMATTER0_ID,
+				    HIVE_IF_BLOCK_FIFO_NO_REQ_ADDRESS)
+			   );
+
+	ia_css_debug_dtrace(2, "\t%-32s:\n", "InputSwitch State");
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg0",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg0));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg1",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg1));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg2",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg2));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg3",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg3));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg4",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg4));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg5",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg5));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg6",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg6));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_lut_reg7",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_lut_reg7));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_input_switch_fsync_lut",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_input_switch_fsync_lut));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_srst",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_srst));
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "_REG_GP_IFMT_slv_reg_srst",
+			    gp_device_reg_load(GP_DEVICE0_ID,
+					       _REG_GP_IFMT_slv_reg_srst));
+
+	ia_css_debug_dtrace(2, "\tFSM Status:\n");
+
+	val = state->fsm_sync_status;
+
+	if (val > 7)
+		fsm_sync_status_str = "ERROR";
+
+	switch (val & 0x7) {
+	case 0:
+		fsm_sync_status_str = "idle";
+		break;
+	case 1:
+		fsm_sync_status_str = "request frame";
+		break;
+	case 2:
+		fsm_sync_status_str = "request lines";
+		break;
+	case 3:
+		fsm_sync_status_str = "request vectors";
+		break;
+	case 4:
+		fsm_sync_status_str = "send acknowledge";
+		break;
+	default:
+		fsm_sync_status_str = "unknown";
+		break;
+	}
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: (0x%X: %s)\n",
+			    "FSM Synchronization Status", val,
+			    fsm_sync_status_str);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM Synchronization Counter",
+			    state->fsm_sync_counter);
+
+	val = state->fsm_crop_status;
+
+	if (val > 7)
+		fsm_crop_status_str = "ERROR";
+
+	switch (val & 0x7) {
+	case 0:
+		fsm_crop_status_str = "idle";
+		break;
+	case 1:
+		fsm_crop_status_str = "wait line";
+		break;
+	case 2:
+		fsm_crop_status_str = "crop line";
+		break;
+	case 3:
+		fsm_crop_status_str = "crop pixel";
+		break;
+	case 4:
+		fsm_crop_status_str = "pass pixel";
+		break;
+	case 5:
+		fsm_crop_status_str = "pass line";
+		break;
+	case 6:
+		fsm_crop_status_str = "lost line";
+		break;
+	default:
+		fsm_crop_status_str = "unknown";
+		break;
+	}
+	ia_css_debug_dtrace(2, "\t\t%-32s: (0x%X: %s)\n",
+			    "FSM Crop Status", val, fsm_crop_status_str);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM Crop Line Counter",
+			    state->fsm_crop_line_counter);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM Crop Pixel Counter",
+			    state->fsm_crop_pixel_counter);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM Deinterleaving idx buffer",
+			    state->fsm_deinterleaving_index);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM H decimation counter",
+			    state->fsm_dec_h_counter);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM V decimation counter",
+			    state->fsm_dec_v_counter);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM block V decimation counter",
+			    state->fsm_dec_block_v_counter);
+
+	val = state->fsm_padding_status;
+
+	if (val > 7)
+		fsm_padding_status_str = "ERROR";
+
+	switch (val & 0x7) {
+	case 0:
+		fsm_padding_status_str = "idle";
+		break;
+	case 1:
+		fsm_padding_status_str = "left pad";
+		break;
+	case 2:
+		fsm_padding_status_str = "write";
+		break;
+	case 3:
+		fsm_padding_status_str = "right pad";
+		break;
+	case 4:
+		fsm_padding_status_str = "send end of line";
+		break;
+	default:
+		fsm_padding_status_str = "unknown";
+		break;
+	}
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: (0x%X: %s)\n", "FSM Padding Status",
+			    val, fsm_padding_status_str);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM Padding element idx counter",
+			    state->fsm_padding_elem_counter);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Vector support error",
+			    state->fsm_vector_support_error);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Vector support buf full",
+			    state->fsm_vector_buffer_full);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Vector support",
+			    state->vector_support);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Fifo sensor data lost",
+			    state->sensor_data_lost);
+}
+
+static void debug_print_if_bin_state(input_formatter_bin_state_t *state)
+{
+	ia_css_debug_dtrace(2, "Stream-to-memory state:\n");
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "reset", state->reset);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "input endianness",
+			    state->input_endianness);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "output endianness",
+			    state->output_endianness);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "bitswap", state->bitswap);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "block_synch",
+			    state->block_synch);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "packet_synch",
+			    state->packet_synch);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "readpostwrite_sync",
+			    state->readpostwrite_synch);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "is_2ppc", state->is_2ppc);
+	ia_css_debug_dtrace(2, "\t%-32s: %d\n", "en_status_update",
+			    state->en_status_update);
+}
+
+static void ia_css_debug_dump_if_state(void)
+{
+	input_formatter_state_t if_state;
+	input_formatter_bin_state_t if_bin_state;
+
+	input_formatter_get_state(INPUT_FORMATTER0_ID, &if_state);
+	debug_print_if_state(&if_state, "Primary IF A");
+	ia_css_debug_dump_pif_a_isp_fifo_state();
+
+	input_formatter_get_state(INPUT_FORMATTER1_ID, &if_state);
+	debug_print_if_state(&if_state, "Primary IF B");
+	ia_css_debug_dump_pif_b_isp_fifo_state();
+
+	input_formatter_bin_get_state(INPUT_FORMATTER3_ID, &if_bin_state);
+	debug_print_if_bin_state(&if_bin_state);
+	ia_css_debug_dump_str2mem_sp_fifo_state();
+}
+#endif
+
+void ia_css_debug_dump_dma_state(void)
+{
+	/* note: the var below is made static as it is quite large;
+	   if it is not static it ends up on the stack which could
+	   cause issues for drivers
+	*/
+	static dma_state_t state;
+	int i, ch_id;
+
+	const char *fsm_cmd_st_lbl = "FSM Command flag state";
+	const char *fsm_ctl_st_lbl = "FSM Control flag state";
+	const char *fsm_ctl_state = NULL;
+	const char *fsm_ctl_flag = NULL;
+	const char *fsm_pack_st = NULL;
+	const char *fsm_read_st = NULL;
+	const char *fsm_write_st = NULL;
+	char last_cmd_str[64];
+
+	dma_get_state(DMA0_ID, &state);
+	/* Print header for DMA dump status */
+	ia_css_debug_dtrace(2, "DMA dump status:\n");
+
+	/* Print FSM command flag state */
+	if (state.fsm_command_idle)
+		ia_css_debug_dtrace(2, "\t%-32s: %s\n", fsm_cmd_st_lbl, "IDLE");
+	if (state.fsm_command_run)
+		ia_css_debug_dtrace(2, "\t%-32s: %s\n", fsm_cmd_st_lbl, "RUN");
+	if (state.fsm_command_stalling)
+		ia_css_debug_dtrace(2, "\t%-32s: %s\n", fsm_cmd_st_lbl,
+				    "STALL");
+	if (state.fsm_command_error)
+		ia_css_debug_dtrace(2, "\t%-32s: %s\n", fsm_cmd_st_lbl,
+				    "ERROR");
+
+	/* Print last command along with the channel */
+	ch_id = state.last_command_channel;
+
+	switch (state.last_command) {
+	case DMA_COMMAND_READ:
+		snprintf(last_cmd_str, 64,
+			 "Read 2D Block [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_WRITE:
+		snprintf(last_cmd_str, 64,
+			 "Write 2D Block [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_SET_CHANNEL:
+		snprintf(last_cmd_str, 64, "Set Channel [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_SET_PARAM:
+		snprintf(last_cmd_str, 64,
+			 "Set Param: %d [Channel: %d]",
+			 state.last_command_param, ch_id);
+		break;
+	case DMA_COMMAND_READ_SPECIFIC:
+		snprintf(last_cmd_str, 64,
+			 "Read Specific 2D Block [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_WRITE_SPECIFIC:
+		snprintf(last_cmd_str, 64,
+			 "Write Specific 2D Block [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_INIT:
+		snprintf(last_cmd_str, 64,
+			 "Init 2D Block on Device A [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_INIT_SPECIFIC:
+		snprintf(last_cmd_str, 64,
+			 "Init Specific 2D Block [Channel: %d]", ch_id);
+		break;
+	case DMA_COMMAND_RST:
+		snprintf(last_cmd_str, 64, "DMA SW Reset");
+		break;
+	case N_DMA_COMMANDS:
+		snprintf(last_cmd_str, 64, "UNKNOWN");
+		break;
+	default:
+		snprintf(last_cmd_str, 64,
+			 "unknown [Channel: %d]", ch_id);
+		break;
+	}
+	ia_css_debug_dtrace(2, "\t%-32s: (0x%X : %s)\n",
+			    "last command received", state.last_command,
+			    last_cmd_str);
+
+	/* Print DMA registers */
+	ia_css_debug_dtrace(2, "\t%-32s\n",
+			    "DMA registers, connection group 0");
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "Cmd Fifo Command",
+			    state.current_command);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "Cmd Fifo Address A",
+			    state.current_addr_a);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "Cmd Fifo Address B",
+			    state.current_addr_b);
+
+	if (state.fsm_ctrl_idle)
+		fsm_ctl_flag = "IDLE";
+	else if (state.fsm_ctrl_run)
+		fsm_ctl_flag = "RUN";
+	else if (state.fsm_ctrl_stalling)
+		fsm_ctl_flag = "STAL";
+	else if (state.fsm_ctrl_error)
+		fsm_ctl_flag = "ERROR";
+	else
+		fsm_ctl_flag = "UNKNOWN";
+
+	switch (state.fsm_ctrl_state) {
+	case DMA_CTRL_STATE_IDLE:
+		fsm_ctl_state = "Idle state";
+		break;
+	case DMA_CTRL_STATE_REQ_RCV:
+		fsm_ctl_state = "Req Rcv state";
+		break;
+	case DMA_CTRL_STATE_RCV:
+		fsm_ctl_state = "Rcv state";
+		break;
+	case DMA_CTRL_STATE_RCV_REQ:
+		fsm_ctl_state = "Rcv Req state";
+		break;
+	case DMA_CTRL_STATE_INIT:
+		fsm_ctl_state = "Init state";
+		break;
+	case N_DMA_CTRL_STATES:
+		fsm_ctl_state = "Unknown";
+		break;
+	}
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %s -> %s\n", fsm_ctl_st_lbl,
+			    fsm_ctl_flag, fsm_ctl_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl source dev",
+			    state.fsm_ctrl_source_dev);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "FSM Ctrl source addr",
+			    state.fsm_ctrl_source_addr);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "FSM Ctrl source stride",
+			    state.fsm_ctrl_source_stride);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl source width",
+			    state.fsm_ctrl_source_width);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl source height",
+			    state.fsm_ctrl_source_height);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack source dev",
+			    state.fsm_ctrl_pack_source_dev);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack dest dev",
+			    state.fsm_ctrl_pack_dest_dev);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "FSM Ctrl dest addr",
+			    state.fsm_ctrl_dest_addr);
+	ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "FSM Ctrl dest stride",
+			    state.fsm_ctrl_dest_stride);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack source width",
+			    state.fsm_ctrl_pack_source_width);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack dest height",
+			    state.fsm_ctrl_pack_dest_height);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack dest width",
+			    state.fsm_ctrl_pack_dest_width);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack source elems",
+			    state.fsm_ctrl_pack_source_elems);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack dest elems",
+			    state.fsm_ctrl_pack_dest_elems);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Ctrl pack extension",
+			    state.fsm_ctrl_pack_extension);
+
+	if (state.pack_idle)
+		fsm_pack_st = "IDLE";
+	if (state.pack_run)
+		fsm_pack_st = "RUN";
+	if (state.pack_stalling)
+		fsm_pack_st = "STALL";
+	if (state.pack_error)
+		fsm_pack_st = "ERROR";
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %s\n", "FSM Pack flag state",
+			    fsm_pack_st);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Pack cnt height",
+			    state.pack_cnt_height);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Pack src cnt width",
+			    state.pack_src_cnt_width);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Pack dest cnt width",
+			    state.pack_dest_cnt_width);
+
+	if (state.read_state == DMA_RW_STATE_IDLE)
+		fsm_read_st = "Idle state";
+	if (state.read_state == DMA_RW_STATE_REQ)
+		fsm_read_st = "Req state";
+	if (state.read_state == DMA_RW_STATE_NEXT_LINE)
+		fsm_read_st = "Next line";
+	if (state.read_state == DMA_RW_STATE_UNLOCK_CHANNEL)
+		fsm_read_st = "Unlock channel";
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %s\n", "FSM Read state",
+			    fsm_read_st);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Read cnt height",
+			    state.read_cnt_height);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Read cnt width",
+			    state.read_cnt_width);
+
+	if (state.write_state == DMA_RW_STATE_IDLE)
+		fsm_write_st = "Idle state";
+	if (state.write_state == DMA_RW_STATE_REQ)
+		fsm_write_st = "Req state";
+	if (state.write_state == DMA_RW_STATE_NEXT_LINE)
+		fsm_write_st = "Next line";
+	if (state.write_state == DMA_RW_STATE_UNLOCK_CHANNEL)
+		fsm_write_st = "Unlock channel";
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %s\n", "FSM Write state",
+			    fsm_write_st);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Write height",
+			    state.write_height);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "FSM Write width",
+			    state.write_width);
+
+	for (i = 0; i < HIVE_ISP_NUM_DMA_CONNS; i++) {
+		dma_port_state_t *port = &state.port_states[i];
+
+		ia_css_debug_dtrace(2, "\tDMA device interface %d\n", i);
+		ia_css_debug_dtrace(2, "\t\tDMA internal side state\n");
+		ia_css_debug_dtrace(2,
+				    "\t\t\tCS:%d - We_n:%d - Run:%d - Ack:%d\n",
+				    port->req_cs, port->req_we_n, port->req_run,
+				    port->req_ack);
+		ia_css_debug_dtrace(2, "\t\tMaster Output side state\n");
+		ia_css_debug_dtrace(2,
+				    "\t\t\tCS:%d - We_n:%d - Run:%d - Ack:%d\n",
+				    port->send_cs, port->send_we_n,
+				    port->send_run, port->send_ack);
+		ia_css_debug_dtrace(2, "\t\tFifo state\n");
+		if (port->fifo_state == DMA_FIFO_STATE_WILL_BE_FULL)
+			ia_css_debug_dtrace(2, "\t\t\tFiFo will be full\n");
+		else if (port->fifo_state == DMA_FIFO_STATE_FULL)
+			ia_css_debug_dtrace(2, "\t\t\tFifo Full\n");
+		else if (port->fifo_state == DMA_FIFO_STATE_EMPTY)
+			ia_css_debug_dtrace(2, "\t\t\tFifo Empty\n");
+		else
+			ia_css_debug_dtrace(2, "\t\t\tFifo state unknown\n");
+
+		ia_css_debug_dtrace(2, "\t\tFifo counter %d\n\n",
+				    port->fifo_counter);
+	}
+
+	for (i = 0; i < HIVE_DMA_NUM_CHANNELS; i++) {
+		dma_channel_state_t *ch = &state.channel_states[i];
+
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "DMA channel register",
+				    i);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Connection",
+				    ch->connection);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Sign extend",
+				    ch->sign_extend);
+		ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "Stride Dev A",
+				    ch->stride_a);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Elems Dev A",
+				    ch->elems_a);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Cropping Dev A",
+				    ch->cropping_a);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Width Dev A",
+				    ch->width_a);
+		ia_css_debug_dtrace(2, "\t\t%-32s: 0x%X\n", "Stride Dev B",
+				    ch->stride_b);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Elems Dev B",
+				    ch->elems_b);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Cropping Dev B",
+				    ch->cropping_b);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Width Dev B",
+				    ch->width_b);
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "Height", ch->height);
+	}
+	ia_css_debug_dtrace(2, "\n");
+	return;
+}
+
+void ia_css_debug_dump_dma_sp_fifo_state(void)
+{
+	fifo_channel_state_t dma_to_sp, sp_to_dma;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_DMA0_TO_SP0, &dma_to_sp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_SP0_TO_DMA0, &sp_to_dma);
+	debug_print_fifo_channel_state(&dma_to_sp, "DMA to SP");
+	debug_print_fifo_channel_state(&sp_to_dma, "SP to DMA");
+	return;
+}
+
+void ia_css_debug_dump_dma_isp_fifo_state(void)
+{
+	fifo_channel_state_t dma_to_isp, isp_to_dma;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_DMA0_TO_ISP0, &dma_to_isp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_ISP0_TO_DMA0, &isp_to_dma);
+	debug_print_fifo_channel_state(&dma_to_isp, "DMA to ISP");
+	debug_print_fifo_channel_state(&isp_to_dma, "ISP to DMA");
+	return;
+}
+
+void ia_css_debug_dump_isp_sp_fifo_state(void)
+{
+	fifo_channel_state_t sp_to_isp, isp_to_sp;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_SP0_TO_ISP0, &sp_to_isp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_ISP0_TO_SP0, &isp_to_sp);
+	debug_print_fifo_channel_state(&sp_to_isp, "SP to ISP");
+	debug_print_fifo_channel_state(&isp_to_sp, "ISP to SP");
+	return;
+}
+
+void ia_css_debug_dump_isp_gdc_fifo_state(void)
+{
+	fifo_channel_state_t gdc_to_isp, isp_to_gdc;
+
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_GDC0_TO_ISP0, &gdc_to_isp);
+	fifo_channel_get_state(FIFO_MONITOR0_ID,
+			       FIFO_CHANNEL_ISP0_TO_GDC0, &isp_to_gdc);
+	debug_print_fifo_channel_state(&gdc_to_isp, "GDC to ISP");
+	debug_print_fifo_channel_state(&isp_to_gdc, "ISP to GDC");
+	return;
+}
+
+void ia_css_debug_dump_all_fifo_state(void)
+{
+	int i;
+	fifo_monitor_state_t state;
+
+	fifo_monitor_get_state(FIFO_MONITOR0_ID, &state);
+
+	for (i = 0; i < N_FIFO_CHANNEL; i++)
+		debug_print_fifo_channel_state(&state.fifo_channels[i],
+					       "squepfstqkt");
+	return;
+}
+
+static void debug_binary_info_print(const struct ia_css_binary_xinfo *info)
+{
+	assert(info);
+	ia_css_debug_dtrace(2, "id = %d\n", info->sp.id);
+	ia_css_debug_dtrace(2, "mode = %d\n", info->sp.pipeline.mode);
+	ia_css_debug_dtrace(2, "max_input_width = %d\n", info->sp.input.max_width);
+	ia_css_debug_dtrace(2, "min_output_width = %d\n",
+			    info->sp.output.min_width);
+	ia_css_debug_dtrace(2, "max_output_width = %d\n",
+			    info->sp.output.max_width);
+	ia_css_debug_dtrace(2, "top_cropping = %d\n", info->sp.pipeline.top_cropping);
+	ia_css_debug_dtrace(2, "left_cropping = %d\n", info->sp.pipeline.left_cropping);
+	ia_css_debug_dtrace(2, "xmem_addr = %d\n", info->xmem_addr);
+	ia_css_debug_dtrace(2, "enable_vf_veceven = %d\n",
+			    info->sp.enable.vf_veceven);
+	ia_css_debug_dtrace(2, "enable_dis = %d\n", info->sp.enable.dis);
+	ia_css_debug_dtrace(2, "enable_uds = %d\n", info->sp.enable.uds);
+	ia_css_debug_dtrace(2, "enable ds = %d\n", info->sp.enable.ds);
+	ia_css_debug_dtrace(2, "s3atbl_use_dmem = %d\n", info->sp.s3a.s3atbl_use_dmem);
+	return;
+}
+
+void ia_css_debug_binary_print(const struct ia_css_binary *bi)
+{
+	unsigned int i;
+
+	debug_binary_info_print(bi->info);
+	ia_css_debug_dtrace(2,
+			    "input:  %dx%d, format = %d, padded width = %d\n",
+			    bi->in_frame_info.res.width,
+			    bi->in_frame_info.res.height,
+			    bi->in_frame_info.format,
+			    bi->in_frame_info.padded_width);
+	ia_css_debug_dtrace(2,
+			    "internal :%dx%d, format = %d, padded width = %d\n",
+			    bi->internal_frame_info.res.width,
+			    bi->internal_frame_info.res.height,
+			    bi->internal_frame_info.format,
+			    bi->internal_frame_info.padded_width);
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		if (bi->out_frame_info[i].res.width != 0) {
+			ia_css_debug_dtrace(2,
+					    "out%d:    %dx%d, format = %d, padded width = %d\n",
+					    i,
+					    bi->out_frame_info[i].res.width,
+					    bi->out_frame_info[i].res.height,
+					    bi->out_frame_info[i].format,
+					    bi->out_frame_info[i].padded_width);
+		}
+	}
+	ia_css_debug_dtrace(2,
+			    "vf out: %dx%d, format = %d, padded width = %d\n",
+			    bi->vf_frame_info.res.width,
+			    bi->vf_frame_info.res.height,
+			    bi->vf_frame_info.format,
+			    bi->vf_frame_info.padded_width);
+	ia_css_debug_dtrace(2, "online = %d\n", bi->online);
+	ia_css_debug_dtrace(2, "input_buf_vectors = %d\n",
+			    bi->input_buf_vectors);
+	ia_css_debug_dtrace(2, "deci_factor_log2 = %d\n", bi->deci_factor_log2);
+	ia_css_debug_dtrace(2, "vf_downscale_log2 = %d\n",
+			    bi->vf_downscale_log2);
+	ia_css_debug_dtrace(2, "dis_deci_factor_log2 = %d\n",
+			    bi->dis.deci_factor_log2);
+	ia_css_debug_dtrace(2, "dis hor coef num = %d\n",
+			    bi->dis.coef.pad.width);
+	ia_css_debug_dtrace(2, "dis ver coef num = %d\n",
+			    bi->dis.coef.pad.height);
+	ia_css_debug_dtrace(2, "dis hor proj num = %d\n",
+			    bi->dis.proj.pad.height);
+	ia_css_debug_dtrace(2, "sctbl_width_per_color = %d\n",
+			    bi->sctbl_width_per_color);
+	ia_css_debug_dtrace(2, "s3atbl_width = %d\n", bi->s3atbl_width);
+	ia_css_debug_dtrace(2, "s3atbl_height = %d\n", bi->s3atbl_height);
+	return;
+}
+
+void ia_css_debug_frame_print(const struct ia_css_frame *frame,
+			      const char *descr)
+{
+	char *data = NULL;
+
+	assert(frame);
+	assert(descr);
+
+	data = (char *)HOST_ADDRESS(frame->data);
+	ia_css_debug_dtrace(2, "frame %s (%p):\n", descr, frame);
+	ia_css_debug_dtrace(2, "  resolution    = %dx%d\n",
+			    frame->info.res.width, frame->info.res.height);
+	ia_css_debug_dtrace(2, "  padded width  = %d\n",
+			    frame->info.padded_width);
+	ia_css_debug_dtrace(2, "  format        = %d\n", frame->info.format);
+	switch (frame->info.format) {
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV16:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_NV61:
+		ia_css_debug_dtrace(2, "  Y = %p\n",
+				    data + frame->planes.nv.y.offset);
+		ia_css_debug_dtrace(2, "  UV = %p\n",
+				    data + frame->planes.nv.uv.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+		ia_css_debug_dtrace(2, "  YUYV = %p\n",
+				    data + frame->planes.yuyv.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YUV422:
+	case IA_CSS_FRAME_FORMAT_YUV444:
+	case IA_CSS_FRAME_FORMAT_YV12:
+	case IA_CSS_FRAME_FORMAT_YV16:
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+		ia_css_debug_dtrace(2, "  Y = %p\n",
+				    data + frame->planes.yuv.y.offset);
+		ia_css_debug_dtrace(2, "  U = %p\n",
+				    data + frame->planes.yuv.u.offset);
+		ia_css_debug_dtrace(2, "  V = %p\n",
+				    data + frame->planes.yuv.v.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+		ia_css_debug_dtrace(2, "  RAW PACKED = %p\n",
+				    data + frame->planes.raw.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_RAW:
+		ia_css_debug_dtrace(2, "  RAW = %p\n",
+				    data + frame->planes.raw.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+	case IA_CSS_FRAME_FORMAT_RGB565:
+		ia_css_debug_dtrace(2, "  RGB = %p\n",
+				    data + frame->planes.rgb.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+		ia_css_debug_dtrace(2, "  R    = %p\n",
+				    data + frame->planes.plane6.r.offset);
+		ia_css_debug_dtrace(2, "  RatB = %p\n",
+				    data + frame->planes.plane6.r_at_b.offset);
+		ia_css_debug_dtrace(2, "  Gr   = %p\n",
+				    data + frame->planes.plane6.gr.offset);
+		ia_css_debug_dtrace(2, "  Gb   = %p\n",
+				    data + frame->planes.plane6.gb.offset);
+		ia_css_debug_dtrace(2, "  B    = %p\n",
+				    data + frame->planes.plane6.b.offset);
+		ia_css_debug_dtrace(2, "  BatR = %p\n",
+				    data + frame->planes.plane6.b_at_r.offset);
+		break;
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+		ia_css_debug_dtrace(2, "  Binary data = %p\n",
+				    data + frame->planes.binary.data.offset);
+		break;
+	default:
+		ia_css_debug_dtrace(2, "  unknown frame type\n");
+		break;
+	}
+	return;
+}
+
+#if SP_DEBUG != SP_DEBUG_NONE
+
+void ia_css_debug_print_sp_debug_state(const struct sh_css_sp_debug_state
+				       *state)
+{
+#endif
+
+#if SP_DEBUG == SP_DEBUG_DUMP
+
+	assert(state);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "current SP software counter: %d\n",
+			    state->debug[0]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty output buffer queue head: 0x%x\n",
+			    state->debug[1]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty output buffer queue tail: 0x%x\n",
+			    state->debug[2]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a buffer queue head: 0x%x\n",
+			    state->debug[3]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a buffer queue tail: 0x%x\n",
+			    state->debug[4]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "full output buffer queue head: 0x%x\n",
+			    state->debug[5]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "full output buffer queue tail: 0x%x\n",
+			    state->debug[6]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "full s3a buffer queue head: 0x%x\n",
+			    state->debug[7]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "full s3a buffer queue tail: 0x%x\n",
+			    state->debug[8]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "event queue head: 0x%x\n",
+			    state->debug[9]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "event queue tail: 0x%x\n",
+			    state->debug[10]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "num of stages of current pipeline: 0x%x\n",
+			    state->debug[11]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "DDR address of stage 1: 0x%x\n",
+			    state->debug[12]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "DDR address of stage 2: 0x%x\n",
+			    state->debug[13]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "current stage out_vf buffer idx: 0x%x\n",
+			    state->debug[14]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "current stage output buffer idx: 0x%x\n",
+			    state->debug[15]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "current stage s3a buffer idx: 0x%x\n",
+			    state->debug[16]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first char of current stage name: 0x%x\n",
+			    state->debug[17]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "current SP thread id: 0x%x\n",
+			    state->debug[18]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty output buffer address 1: 0x%x\n",
+			    state->debug[19]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty output buffer address 2: 0x%x\n",
+			    state->debug[20]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty out_vf buffer address 1: 0x%x\n",
+			    state->debug[21]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty out_vf buffer address 2: 0x%x\n",
+			    state->debug[22]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a_hi buffer address 1: 0x%x\n",
+			    state->debug[23]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a_hi buffer address 2: 0x%x\n",
+			    state->debug[24]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a_lo buffer address 1: 0x%x\n",
+			    state->debug[25]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty s3a_lo buffer address 2: 0x%x\n",
+			    state->debug[26]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty dis_hor buffer address 1: 0x%x\n",
+			    state->debug[27]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty dis_hor buffer address 2: 0x%x\n",
+			    state->debug[28]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty dis_ver buffer address 1: 0x%x\n",
+			    state->debug[29]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty dis_ver buffer address 2: 0x%x\n",
+			    state->debug[30]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "empty param buffer address: 0x%x\n",
+			    state->debug[31]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect frame address: 0x%x\n",
+			    state->debug[32]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect frame container address: 0x%x\n",
+			    state->debug[33]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect frame container payload: 0x%x\n",
+			    state->debug[34]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_hi address: 0x%x\n",
+			    state->debug[35]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_hi container address: 0x%x\n",
+			    state->debug[36]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_hi container payload: 0x%x\n",
+			    state->debug[37]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_lo address: 0x%x\n",
+			    state->debug[38]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_lo container address: 0x%x\n",
+			    state->debug[39]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "first incorrect s3a_lo container payload: 0x%x\n",
+			    state->debug[40]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "number of calling flash start function: 0x%x\n",
+			    state->debug[41]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "number of calling flash close function: 0x%x\n",
+			    state->debug[42]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "number of flashed frame: 0x%x\n",
+			    state->debug[43]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "flash in use flag: 0x%x\n",
+			    state->debug[44]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "number of update frame flashed flag: 0x%x\n",
+			    state->debug[46]);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			    "number of active threads: 0x%x\n",
+			    state->debug[45]);
+
+#elif SP_DEBUG == SP_DEBUG_COPY
+
+	/* Remember last_index because we only want to print new entries */
+	static int last_index;
+	int sp_index = state->index;
+	int n;
+
+	assert(state);
+	if (sp_index < last_index) {
+		/* SP has been reset */
+		last_index = 0;
+	}
+
+	if (last_index == 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "copy-trace init: sp_dbg_if_start_line=%d, sp_dbg_if_start_column=%d, sp_dbg_if_cropped_height=%d, sp_debg_if_cropped_width=%d\n",
+				    state->if_start_line,
+				    state->if_start_column,
+				    state->if_cropped_height,
+				    state->if_cropped_width);
+	}
+
+	if ((last_index + SH_CSS_SP_DBG_TRACE_DEPTH) < sp_index) {
+		/* last index can be multiple rounds behind */
+		/* while trace size is only SH_CSS_SP_DBG_TRACE_DEPTH */
+		last_index = sp_index - SH_CSS_SP_DBG_TRACE_DEPTH;
+	}
+
+	for (n = last_index; n < sp_index; n++) {
+		int i = n % SH_CSS_SP_DBG_TRACE_DEPTH;
+
+		if (state->trace[i].frame != 0) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+					    "copy-trace: frame=%d, line=%d, pixel_distance=%d, mipi_used_dword=%d, sp_index=%d\n",
+					    state->trace[i].frame,
+					    state->trace[i].line,
+					    state->trace[i].pixel_distance,
+					    state->trace[i].mipi_used_dword,
+					    state->trace[i].sp_index);
+		}
+	}
+
+	last_index = sp_index;
+
+#elif SP_DEBUG == SP_DEBUG_TRACE
+
+	/*
+	 * This is just an example how TRACE_FILE_ID (see ia_css_debug.sp.h) will
+	 * me mapped on the file name string.
+	 *
+	 * Adjust this to your trace case!
+	 */
+	static char const *const id2filename[8] = {
+		"param_buffer.sp.c | tagger.sp.c | pipe_data.sp.c",
+		"isp_init.sp.c",
+		"sp_raw_copy.hive.c",
+		"dma_configure.sp.c",
+		"sp.hive.c",
+		"event_proxy_sp.hive.c",
+		"circular_buffer.sp.c",
+		"frame_buffer.sp.c"
+	};
+
+	/* Example SH_CSS_SP_DBG_NR_OF_TRACES==1 */
+	/* Adjust this to your trace case */
+	static char const *trace_name[SH_CSS_SP_DBG_NR_OF_TRACES] = {
+		"default"
+	};
+
+	/* Remember host_index_last because we only want to print new entries */
+	static int host_index_last[SH_CSS_SP_DBG_NR_OF_TRACES] = { 0 };
+	int t, n;
+
+	assert(state);
+
+	for (t = 0; t < SH_CSS_SP_DBG_NR_OF_TRACES; t++) {
+		int sp_index_last = state->index_last[t];
+
+		if (sp_index_last < host_index_last[t]) {
+			/* SP has been reset */
+			host_index_last[t] = 0;
+		}
+
+		if ((host_index_last[t] + SH_CSS_SP_DBG_TRACE_DEPTH) <
+		    sp_index_last) {
+			/* last index can be multiple rounds behind */
+			/* while trace size is only SH_CSS_SP_DBG_TRACE_DEPTH */
+			ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+					    "Warning: trace %s has gap of %d traces\n",
+					    trace_name[t],
+					    (sp_index_last -
+					     (host_index_last[t] +
+					      SH_CSS_SP_DBG_TRACE_DEPTH)));
+
+			host_index_last[t] =
+			    sp_index_last - SH_CSS_SP_DBG_TRACE_DEPTH;
+		}
+
+		for (n = host_index_last[t]; n < sp_index_last; n++) {
+			int i = n % SH_CSS_SP_DBG_TRACE_DEPTH;
+			int l = state->trace[t][i].location &
+				((1 << SH_CSS_SP_DBG_TRACE_FILE_ID_BIT_POS) - 1);
+			int fid = state->trace[t][i].location >>
+				  SH_CSS_SP_DBG_TRACE_FILE_ID_BIT_POS;
+			int ts = state->trace[t][i].time_stamp;
+
+			if (ts) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+						    "%05d trace=%s, file=%s:%d, data=0x%08x\n",
+						    ts,
+						    trace_name[t],
+						    id2filename[fid], l,
+						    state->trace[t][i].data);
+			}
+		}
+		host_index_last[t] = sp_index_last;
+	}
+
+#elif SP_DEBUG == SP_DEBUG_MINIMAL
+	int i;
+	int base = 0;
+	int limit = SH_CSS_NUM_SP_DEBUG;
+	int step = 1;
+
+	assert(state);
+
+	for (i = base; i < limit; i += step) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "sp_dbg_trace[%d] = %d\n",
+				    i, state->debug[i]);
+	}
+#endif
+
+#if SP_DEBUG != SP_DEBUG_NONE
+
+	return;
+}
+#endif
+
+#if !defined(ISP2401)
+static void debug_print_rx_mipi_port_state(mipi_port_state_t *state)
+{
+	int i;
+	unsigned int bits, infos;
+
+	assert(state);
+
+	bits = state->irq_status;
+	infos = ia_css_isys_rx_translate_irq_infos(bits);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: (irq reg = 0x%X)\n",
+			    "receiver errors", bits);
+
+	if (infos & IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
+		ia_css_debug_dtrace(2, "\t\t\tbuffer overrun\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT)
+		ia_css_debug_dtrace(2, "\t\t\tstart-of-transmission error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
+		ia_css_debug_dtrace(2, "\t\t\tstart-of-transmission sync error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CONTROL)
+		ia_css_debug_dtrace(2, "\t\t\tcontrol error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
+		ia_css_debug_dtrace(2, "\t\t\t2 or more ECC errors\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CRC)
+		ia_css_debug_dtrace(2, "\t\t\tCRC mismatch\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
+		ia_css_debug_dtrace(2, "\t\t\tunknown error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
+		ia_css_debug_dtrace(2, "\t\t\tframe sync error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
+		ia_css_debug_dtrace(2, "\t\t\tframe data error\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
+		ia_css_debug_dtrace(2, "\t\t\tdata timeout\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
+		ia_css_debug_dtrace(2, "\t\t\tunknown escape command entry\n");
+	if (infos & IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
+		ia_css_debug_dtrace(2, "\t\t\tline sync error\n");
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "device_ready", state->device_ready);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "irq_status", state->irq_status);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "irq_enable", state->irq_enable);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "timeout_count", state->timeout_count);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "init_count", state->init_count);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "raw16_18", state->raw16_18);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "sync_count", state->sync_count);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "rx_count", state->rx_count);
+
+	for (i = 0; i < MIPI_4LANE_CFG; i++) {
+		ia_css_debug_dtrace(2, "\t\t%-32s%d%-32s: %d\n",
+				    "lane_sync_count[", i, "]",
+				    state->lane_sync_count[i]);
+	}
+
+	for (i = 0; i < MIPI_4LANE_CFG; i++) {
+		ia_css_debug_dtrace(2, "\t\t%-32s%d%-32s: %d\n",
+				    "lane_rx_count[", i, "]",
+				    state->lane_rx_count[i]);
+	}
+
+	return;
+}
+
+static void debug_print_rx_channel_state(rx_channel_state_t *state)
+{
+	int i;
+
+	assert(state);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "compression_scheme0", state->comp_scheme0);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "compression_scheme1", state->comp_scheme1);
+
+	for (i = 0; i < N_MIPI_FORMAT_CUSTOM; i++) {
+		ia_css_debug_dtrace(2, "\t\t%-32s%d: %d\n",
+				    "MIPI Predictor ", i, state->pred[i]);
+	}
+
+	for (i = 0; i < N_MIPI_FORMAT_CUSTOM; i++) {
+		ia_css_debug_dtrace(2, "\t\t%-32s%d: %d\n",
+				    "MIPI Compressor ", i, state->comp[i]);
+	}
+
+	return;
+}
+
+static void debug_print_rx_state(receiver_state_t *state)
+{
+	int i;
+
+	assert(state);
+	ia_css_debug_dtrace(2, "CSI Receiver State:\n");
+
+	ia_css_debug_dtrace(2, "\tConfiguration:\n");
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "fs_to_ls_delay", state->fs_to_ls_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "ls_to_data_delay", state->ls_to_data_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "data_to_le_delay", state->data_to_le_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "le_to_fe_delay", state->le_to_fe_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "fe_to_fs_delay", state->fe_to_fs_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "le_to_fs_delay", state->le_to_fs_delay);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "is_two_ppc", state->is_two_ppc);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "backend_rst", state->backend_rst);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "raw18", state->raw18);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "force_raw8", state->force_raw8);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "raw16", state->raw16);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_gsp_acc_ovl", state->be_gsp_acc_ovl);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "be_srst", state->be_srst);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_is_two_ppc", state->be_is_two_ppc);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_comp_format0", state->be_comp_format0);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_comp_format1", state->be_comp_format1);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_comp_format2", state->be_comp_format2);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_comp_format3", state->be_comp_format3);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "be_sel", state->be_sel);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_raw16_config", state->be_raw16_config);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_raw18_config", state->be_raw18_config);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_force_raw8", state->be_force_raw8);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_irq_status", state->be_irq_status);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "be_irq_clear", state->be_irq_clear);
+
+	/* mipi port state */
+	for (i = 0; i < N_MIPI_PORT_ID; i++) {
+		ia_css_debug_dtrace(2, "\tMIPI Port %d State:\n", i);
+
+		debug_print_rx_mipi_port_state(&state->mipi_port_state[i]);
+	}
+	/* end of mipi port state */
+
+	/* rx channel state */
+	for (i = 0; i < N_RX_CHANNEL_ID; i++) {
+		ia_css_debug_dtrace(2, "\tRX Channel %d State:\n", i);
+
+		debug_print_rx_channel_state(&state->rx_channel_state[i]);
+	}
+	/* end of rx channel state */
+
+	return;
+}
+#endif
+
+void ia_css_debug_dump_rx_state(void)
+{
+#if !defined(ISP2401)
+	receiver_state_t state;
+
+	receiver_get_state(RX0_ID, &state);
+	debug_print_rx_state(&state);
+#endif
+}
+
+void ia_css_debug_dump_sp_sw_debug_info(void)
+{
+#if SP_DEBUG != SP_DEBUG_NONE
+	struct sh_css_sp_debug_state state;
+
+	sh_css_sp_get_debug_state(&state);
+	ia_css_debug_print_sp_debug_state(&state);
+#endif
+	ia_css_bufq_dump_queue_info();
+	ia_css_pipeline_dump_thread_map_info();
+	return;
+}
+
+#if !defined(ISP2401)
+static void debug_print_isys_capture_unit_state(capture_unit_state_t *state)
+{
+	assert(state);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Packet_Length", state->Packet_Length);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Received_Length", state->Received_Length);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Received_Short_Packets",
+			    state->Received_Short_Packets);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Received_Long_Packets",
+			    state->Received_Long_Packets);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Last_Command", state->Last_Command);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Next_Command", state->Next_Command);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Last_Acknowledge", state->Last_Acknowledge);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Next_Acknowledge", state->Next_Acknowledge);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM_State_Info", state->FSM_State_Info);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "StartMode", state->StartMode);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Start_Addr", state->Start_Addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Mem_Region_Size", state->Mem_Region_Size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Num_Mem_Regions", state->Num_Mem_Regions);
+	return;
+}
+
+static void debug_print_isys_acquisition_unit_state(
+    acquisition_unit_state_t *state)
+{
+	assert(state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Received_Short_Packets",
+			    state->Received_Short_Packets);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Received_Long_Packets",
+			    state->Received_Long_Packets);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Last_Command", state->Last_Command);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Next_Command", state->Next_Command);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Last_Acknowledge", state->Last_Acknowledge);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Next_Acknowledge", state->Next_Acknowledge);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "FSM_State_Info", state->FSM_State_Info);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Int_Cntr_Info", state->Int_Cntr_Info);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Start_Addr", state->Start_Addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Mem_Region_Size", state->Mem_Region_Size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "Num_Mem_Regions", state->Num_Mem_Regions);
+}
+
+static void debug_print_isys_ctrl_unit_state(ctrl_unit_state_t *state)
+{
+	assert(state);
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "last_cmd", state->last_cmd);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "next_cmd", state->next_cmd);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "last_ack", state->last_ack);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n", "next_ack", state->next_ack);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "top_fsm_state", state->top_fsm_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captA_fsm_state", state->captA_fsm_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captB_fsm_state", state->captB_fsm_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captC_fsm_state", state->captC_fsm_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "acq_fsm_state", state->acq_fsm_state);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captA_start_addr", state->captA_start_addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captB_start_addr", state->captB_start_addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captC_start_addr", state->captC_start_addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captA_mem_region_size",
+			    state->captA_mem_region_size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captB_mem_region_size",
+			    state->captB_mem_region_size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captC_mem_region_size",
+			    state->captC_mem_region_size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captA_num_mem_regions",
+			    state->captA_num_mem_regions);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captB_num_mem_regions",
+			    state->captB_num_mem_regions);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "captC_num_mem_regions",
+			    state->captC_num_mem_regions);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "acq_start_addr", state->acq_start_addr);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "acq_mem_region_size", state->acq_mem_region_size);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "acq_num_mem_regions", state->acq_num_mem_regions);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "capt_reserve_one_mem_region",
+			    state->capt_reserve_one_mem_region);
+
+	return;
+}
+
+static void debug_print_isys_state(input_system_state_t *state)
+{
+	int i;
+
+	assert(state);
+	ia_css_debug_dtrace(2, "InputSystem State:\n");
+
+	/* configuration */
+	ia_css_debug_dtrace(2, "\tConfiguration:\n");
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_multiCastA_sel", state->str_multicastA_sel);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_multicastB_sel", state->str_multicastB_sel);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_multicastC_sel", state->str_multicastC_sel);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_mux_sel", state->str_mux_sel);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_mon_status", state->str_mon_status);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_mon_irq_cond", state->str_mon_irq_cond);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_mon_irq_en", state->str_mon_irq_en);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "isys_srst", state->isys_srst);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "isys_slv_reg_srst", state->isys_slv_reg_srst);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_deint_portA_cnt", state->str_deint_portA_cnt);
+
+	ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+			    "str_deint_portB_cnd", state->str_deint_portB_cnt);
+	/* end of configuration */
+
+	/* capture unit state */
+	for (i = 0; i < N_CAPTURE_UNIT_ID; i++) {
+		capture_unit_state_t *capture_unit_state;
+
+		ia_css_debug_dtrace(2, "\tCaptureUnit %d State:\n", i);
+
+		capture_unit_state = &state->capture_unit[i];
+		debug_print_isys_capture_unit_state(capture_unit_state);
+	}
+	/* end of capture unit state */
+
+	/* acquisition unit state */
+	for (i = 0; i < N_ACQUISITION_UNIT_ID; i++) {
+		acquisition_unit_state_t *acquisition_unit_state;
+
+		ia_css_debug_dtrace(2, "\tAcquisitionUnit %d State:\n", i);
+
+		acquisition_unit_state = &state->acquisition_unit[i];
+		debug_print_isys_acquisition_unit_state(acquisition_unit_state);
+	}
+	/* end of acquisition unit state */
+
+	/* control unit state */
+	for (i = 0; i < N_CTRL_UNIT_ID; i++) {
+		ia_css_debug_dtrace(2, "\tControlUnit %d State:\n", i);
+
+		debug_print_isys_ctrl_unit_state(&state->ctrl_unit_state[i]);
+	}
+	/* end of control unit state */
+}
+#endif
+
+void ia_css_debug_dump_isys_state(void)
+{
+	static input_system_state_t state;
+
+	input_system_get_state(INPUT_SYSTEM0_ID, &state);
+
+#ifndef ISP2401
+	debug_print_isys_state(&state);
+#else
+	input_system_dump_state(INPUT_SYSTEM0_ID, &state);
+#endif
+}
+
+void ia_css_debug_dump_debug_info(const char *context)
+{
+	if (!context)
+		context = "No Context provided";
+
+	ia_css_debug_dtrace(2, "CSS Debug Info dump [Context = %s]\n", context);
+	if (!IS_ISP2401)
+		ia_css_debug_dump_rx_state();
+
+#ifndef ISP2401
+	ia_css_debug_dump_if_state();
+#endif
+	ia_css_debug_dump_isp_state();
+	ia_css_debug_dump_isp_sp_fifo_state();
+	ia_css_debug_dump_isp_gdc_fifo_state();
+	ia_css_debug_dump_sp_state();
+	ia_css_debug_dump_perf_counters();
+
+#ifdef HAS_WATCHDOG_SP_THREAD_DEBUG
+	sh_css_dump_thread_wait_info();
+	sh_css_dump_pipe_stage_info();
+	sh_css_dump_pipe_stripe_info();
+#endif
+	ia_css_debug_dump_dma_isp_fifo_state();
+	ia_css_debug_dump_dma_sp_fifo_state();
+	ia_css_debug_dump_dma_state();
+
+	if (!IS_ISP2401) {
+		struct irq_controller_state state;
+
+		ia_css_debug_dump_isys_state();
+
+		irq_controller_get_state(IRQ2_ID, &state);
+
+		ia_css_debug_dtrace(2, "\t%-32s:\n",
+				    "Input System IRQ Controller State");
+
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+				    "irq_edge", state.irq_edge);
+
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+				    "irq_mask", state.irq_mask);
+
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+				    "irq_status", state.irq_status);
+
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+				    "irq_enable", state.irq_enable);
+
+		ia_css_debug_dtrace(2, "\t\t%-32s: %d\n",
+				    "irq_level_not_pulse",
+				    state.irq_level_not_pulse);
+	} else {
+		ia_css_debug_dump_isys_state();
+	}
+
+	ia_css_debug_tagger_state();
+
+	return;
+}
+
+/* this function is for debug use, it can make SP go to sleep
+  state after each frame, then user can dump the stable SP dmem.
+  this function can be called after ia_css_start_sp()
+  and before sh_css_init_buffer_queues()
+*/
+void ia_css_debug_enable_sp_sleep_mode(enum ia_css_sp_sleep_mode mode)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_sp_sleep_mode;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_sleep_mode = fw->info.sp.sleep_mode;
+
+	(void)HIVE_ADDR_sp_sleep_mode;	/* Suppres warnings in CRUN */
+
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(sp_sleep_mode),
+			     (uint32_t)mode);
+}
+
+void ia_css_debug_wake_up_sp(void)
+{
+	/*hrt_ctl_start(SP); */
+	sp_ctrl_setbit(SP0_ID, SP_SC_REG, SP_START_BIT);
+}
+
+#define FIND_DMEM_PARAMS_TYPE(stream, kernel, type) \
+	(struct HRTCAT(HRTCAT(sh_css_isp_, type), _params) *) \
+	findf_dmem_params(stream, offsetof(struct ia_css_memory_offsets, dmem.kernel))
+
+#define FIND_DMEM_PARAMS(stream, kernel) FIND_DMEM_PARAMS_TYPE(stream, kernel, kernel)
+
+/* Find a stage that support the kernel and return the parameters for that kernel */
+static char *
+findf_dmem_params(struct ia_css_stream *stream, short idx)
+{
+	int i;
+
+	for (i = 0; i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+		struct ia_css_pipeline *pipeline = ia_css_pipe_get_pipeline(pipe);
+		struct ia_css_pipeline_stage *stage;
+
+		for (stage = pipeline->stages; stage; stage = stage->next) {
+			struct ia_css_binary *binary = stage->binary;
+			short *offsets = (short *)&binary->info->mem_offsets.offsets.param->dmem;
+			short dmem_offset = offsets[idx];
+			const struct ia_css_host_data *isp_data =
+			    ia_css_isp_param_get_mem_init(&binary->mem_params,
+							  IA_CSS_PARAM_CLASS_PARAM, IA_CSS_ISP_DMEM0);
+			if (dmem_offset < 0)
+				continue;
+			return &isp_data->address[dmem_offset];
+		}
+	}
+	return NULL;
+}
+
+void ia_css_debug_dump_isp_params(struct ia_css_stream *stream,
+				  unsigned int enable)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "ISP PARAMETERS:\n");
+
+	assert(stream);
+	if ((enable & IA_CSS_DEBUG_DUMP_FPN)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_fpn_dump(FIND_DMEM_PARAMS(stream, fpn), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_OB)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_ob_dump(FIND_DMEM_PARAMS(stream, ob), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_SC)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_sc_dump(FIND_DMEM_PARAMS(stream, sc), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_WB)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_wb_dump(FIND_DMEM_PARAMS(stream, wb), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_DP)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_dp_dump(FIND_DMEM_PARAMS(stream, dp), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_BNR)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_bnr_dump(FIND_DMEM_PARAMS(stream, bnr), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_S3A)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_s3a_dump(FIND_DMEM_PARAMS(stream, s3a), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_DE)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_de_dump(FIND_DMEM_PARAMS(stream, de), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_YNR)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_nr_dump(FIND_DMEM_PARAMS_TYPE(stream, nr, ynr),  IA_CSS_DEBUG_VERBOSE);
+		ia_css_yee_dump(FIND_DMEM_PARAMS(stream, yee), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_CSC)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_csc_dump(FIND_DMEM_PARAMS(stream, csc), IA_CSS_DEBUG_VERBOSE);
+		ia_css_yuv2rgb_dump(FIND_DMEM_PARAMS_TYPE(stream, yuv2rgb, csc),
+				    IA_CSS_DEBUG_VERBOSE);
+		ia_css_rgb2yuv_dump(FIND_DMEM_PARAMS_TYPE(stream, rgb2yuv, csc),
+				    IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_GC)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_gc_dump(FIND_DMEM_PARAMS(stream, gc), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_TNR)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_tnr_dump(FIND_DMEM_PARAMS(stream, tnr), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_ANR)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_anr_dump(FIND_DMEM_PARAMS(stream, anr), IA_CSS_DEBUG_VERBOSE);
+	}
+	if ((enable & IA_CSS_DEBUG_DUMP_CE)
+	    || (enable & IA_CSS_DEBUG_DUMP_ALL)) {
+		ia_css_ce_dump(FIND_DMEM_PARAMS(stream, ce), IA_CSS_DEBUG_VERBOSE);
+	}
+}
+
+void sh_css_dump_sp_raw_copy_linecount(bool reduced)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_raw_copy_line_count;
+	s32 raw_copy_line_count;
+	static s32 prev_raw_copy_line_count = -1;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_raw_copy_line_count =
+	    fw->info.sp.raw_copy_line_count;
+
+	(void)HIVE_ADDR_raw_copy_line_count;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(raw_copy_line_count),
+		     &raw_copy_line_count,
+		     sizeof(raw_copy_line_count));
+
+	/* only indicate if copy loop is active */
+	if (reduced)
+		raw_copy_line_count = (raw_copy_line_count < 0) ? raw_copy_line_count : 1;
+	/* do the handling */
+	if (prev_raw_copy_line_count != raw_copy_line_count) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "sh_css_dump_sp_raw_copy_linecount() line_count=%d\n",
+				    raw_copy_line_count);
+		prev_raw_copy_line_count = raw_copy_line_count;
+	}
+}
+
+void ia_css_debug_dump_isp_binary(void)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_pipeline_sp_curr_binary_id;
+	u32 curr_binary_id;
+	static u32 prev_binary_id = 0xFFFFFFFF;
+	static u32 sample_count;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_pipeline_sp_curr_binary_id = fw->info.sp.curr_binary_id;
+
+	(void)HIVE_ADDR_pipeline_sp_curr_binary_id;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(pipeline_sp_curr_binary_id),
+		     &curr_binary_id,
+		     sizeof(curr_binary_id));
+
+	/* do the handling */
+	sample_count++;
+	if (prev_binary_id != curr_binary_id) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "sh_css_dump_isp_binary() pipe_id=%d, binary_id=%d, sample_count=%d\n",
+				    (curr_binary_id >> 16),
+				    (curr_binary_id & 0x0ffff),
+				    sample_count);
+		sample_count = 0;
+		prev_binary_id = curr_binary_id;
+	}
+}
+
+void ia_css_debug_dump_perf_counters(void)
+{
+	const struct ia_css_fw_info *fw;
+	int i;
+	unsigned int HIVE_ADDR_ia_css_isys_sp_error_cnt;
+	/* N_MIPI_PORT_ID + 1: 3 Capture Units and 1 Acquire Unit. */
+	s32 ia_css_sp_input_system_error_cnt[N_MIPI_PORT_ID + 1];
+
+	if (IS_ISP2401)
+		return;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "Input System Error Counters:\n");
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_ia_css_isys_sp_error_cnt =
+	    fw->info.sp.perf_counter_input_system_error;
+
+	(void)HIVE_ADDR_ia_css_isys_sp_error_cnt;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(ia_css_isys_sp_error_cnt),
+		     &ia_css_sp_input_system_error_cnt,
+		     sizeof(ia_css_sp_input_system_error_cnt));
+
+	for (i = 0; i < N_MIPI_PORT_ID + 1; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "\tport[%d] = %d\n",
+				    i, ia_css_sp_input_system_error_cnt[i]);
+	}
+}
+
+/*
+ * @brief Initialize the debug mode.
+ * Refer to "ia_css_debug.h" for more details.
+ */
+bool ia_css_debug_mode_init(void)
+{
+	bool rc;
+
+	rc = sh_css_sp_init_dma_sw_reg(0);
+	return rc;
+}
+
+/*
+ * @brief Disable the DMA channel.
+ * Refer to "ia_css_debug.h" for more details.
+ */
+bool
+ia_css_debug_mode_disable_dma_channel(int dma_id,
+				      int channel_id, int request_type)
+{
+	bool rc;
+
+	rc = sh_css_sp_set_dma_sw_reg(dma_id, channel_id, request_type, false);
+
+	return rc;
+}
+
+/*
+ * @brief Enable the DMA channel.
+ * Refer to "ia_css_debug.h" for more details.
+ */
+bool
+ia_css_debug_mode_enable_dma_channel(int dma_id,
+				     int channel_id, int request_type)
+{
+	bool rc;
+
+	rc = sh_css_sp_set_dma_sw_reg(dma_id, channel_id, request_type, true);
+
+	return rc;
+}
+
+static void __printf(1, 2) dtrace_dot(const char *fmt, ...)
+{
+	va_list ap;
+
+	assert(fmt);
+	va_start(ap, fmt);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_INFO, "%s", DPG_START);
+	ia_css_debug_vdtrace(IA_CSS_DEBUG_INFO, fmt, ap);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_INFO, "%s", DPG_END);
+	va_end(ap);
+}
+
+#ifdef HAS_WATCHDOG_SP_THREAD_DEBUG
+void sh_css_dump_thread_wait_info(void)
+{
+	const struct ia_css_fw_info *fw;
+	int i;
+	unsigned int HIVE_ADDR_sp_thread_wait;
+	s32 sp_thread_wait[MAX_THREAD_NUM];
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "SEM WAITS:\n");
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_thread_wait =
+	    fw->info.sp.debug_wait;
+
+	(void)HIVE_ADDR_sp_thread_wait;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(sp_thread_wait),
+		     &sp_thread_wait,
+		     sizeof(sp_thread_wait));
+	for (i = 0; i < MAX_THREAD_NUM; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "\twait[%d] = 0x%X\n",
+				    i, sp_thread_wait[i]);
+	}
+}
+
+void sh_css_dump_pipe_stage_info(void)
+{
+	const struct ia_css_fw_info *fw;
+	int i;
+	unsigned int HIVE_ADDR_sp_pipe_stage;
+	s32 sp_pipe_stage[MAX_THREAD_NUM];
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "PIPE STAGE:\n");
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_pipe_stage =
+	    fw->info.sp.debug_stage;
+
+	(void)HIVE_ADDR_sp_pipe_stage;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(sp_pipe_stage),
+		     &sp_pipe_stage,
+		     sizeof(sp_pipe_stage));
+	for (i = 0; i < MAX_THREAD_NUM; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "\tstage[%d] = %d\n",
+				    i, sp_pipe_stage[i]);
+	}
+}
+
+void sh_css_dump_pipe_stripe_info(void)
+{
+	const struct ia_css_fw_info *fw;
+	int i;
+	unsigned int HIVE_ADDR_sp_pipe_stripe;
+	s32 sp_pipe_stripe[MAX_THREAD_NUM];
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE, "PIPE STRIPE:\n");
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_pipe_stripe =
+	    fw->info.sp.debug_stripe;
+
+	(void)HIVE_ADDR_sp_pipe_stripe;
+
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(sp_pipe_stripe),
+		     &sp_pipe_stripe,
+		     sizeof(sp_pipe_stripe));
+	for (i = 0; i < MAX_THREAD_NUM; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+				    "\tstripe[%d] = %d\n",
+				    i, sp_pipe_stripe[i]);
+	}
+}
+#endif
+
+static void
+ia_css_debug_pipe_graph_dump_frame(
+    const struct ia_css_frame *frame,
+    enum ia_css_pipe_id id,
+    char const *blob_name,
+    char const *frame_name,
+    bool in_frame)
+{
+	char bufinfo[100];
+
+	if (frame->dynamic_queue_id == SH_CSS_INVALID_QUEUE_ID) {
+		snprintf(bufinfo, sizeof(bufinfo), "Internal");
+	} else {
+		snprintf(bufinfo, sizeof(bufinfo), "Queue: %s %s",
+			 pipe_id_to_str[id],
+			 queue_id_to_str[frame->dynamic_queue_id]);
+	}
+	dtrace_dot(
+	    "node [shape = box, fixedsize=true, width=2, height=0.7]; \"%p\" [label = \"%s\\n%d(%d) x %d, %dbpp\\n%s\"];",
+	    frame,
+	    debug_frame_format2str(frame->info.format),
+	    frame->info.res.width,
+	    frame->info.padded_width,
+	    frame->info.res.height,
+	    frame->info.raw_bit_depth,
+	    bufinfo);
+
+	if (in_frame) {
+		dtrace_dot(
+		    "\"%p\"->\"%s(pipe%d)\" [label = %s_frame];",
+		    frame,
+		    blob_name, id, frame_name);
+	} else {
+		dtrace_dot(
+		    "\"%s(pipe%d)\"->\"%p\" [label = %s_frame];",
+		    blob_name, id,
+		    frame,
+		    frame_name);
+	}
+}
+
+void
+ia_css_debug_pipe_graph_dump_prologue(void)
+{
+	dtrace_dot("digraph sh_css_pipe_graph {");
+	dtrace_dot("rankdir=LR;");
+
+	dtrace_dot("fontsize=9;");
+	dtrace_dot("label = \"\\nEnable options: rp=reduced pipe, vfve=vf_veceven, dvse=dvs_envelope, dvs6=dvs_6axis, bo=block_out, fbds=fixed_bayer_ds, bf6=bayer_fir_6db, rawb=raw_binning, cont=continuous, disc=dis_crop\\n"
+		   "dp2a=dp_2adjacent, outp=output, outt=out_table, reff=ref_frame, par=params, gam=gamma, cagdc=ca_gdc, ispa=isp_addresses, inf=in_frame, outf=out_frame, hs=high_speed, inpc=input_chunking\"");
+}
+
+void ia_css_debug_pipe_graph_dump_epilogue(void)
+{
+	if (strlen(ring_buffer) > 0) {
+		dtrace_dot(ring_buffer);
+	}
+
+	if (pg_inst.stream_format != N_ATOMISP_INPUT_FORMAT) {
+		/* An input stream format has been set so assume we have
+		 * an input system and sensor
+		 */
+
+		dtrace_dot(
+		    "node [shape = doublecircle, fixedsize=true, width=2.5]; \"input_system\" [label = \"Input system\"];");
+
+		dtrace_dot(
+		    "\"input_system\"->\"%s\" [label = \"%s\"];",
+		    dot_id_input_bin, debug_stream_format2str(pg_inst.stream_format));
+
+		dtrace_dot(
+		    "node [shape = doublecircle, fixedsize=true, width=2.5]; \"sensor\" [label = \"Sensor\"];");
+
+		dtrace_dot(
+		    "\"sensor\"->\"input_system\" [label = \"%s\\n%d x %d\\n(%d x %d)\"];",
+		    debug_stream_format2str(pg_inst.stream_format),
+		    pg_inst.width, pg_inst.height,
+		    pg_inst.eff_width, pg_inst.eff_height);
+	}
+
+	dtrace_dot("}");
+
+	/* Reset temp strings */
+	memset(dot_id_input_bin, 0, sizeof(dot_id_input_bin));
+	memset(ring_buffer, 0, sizeof(ring_buffer));
+
+	pg_inst.do_init = true;
+	pg_inst.width = 0;
+	pg_inst.height = 0;
+	pg_inst.eff_width = 0;
+	pg_inst.eff_height = 0;
+	pg_inst.stream_format = N_ATOMISP_INPUT_FORMAT;
+}
+
+void
+ia_css_debug_pipe_graph_dump_stage(
+    struct ia_css_pipeline_stage *stage,
+    enum ia_css_pipe_id id)
+{
+	char blob_name[SH_CSS_MAX_BINARY_NAME + 10] = "<unknown type>";
+	char const *bin_type = "<unknown type>";
+	int i;
+
+	assert(stage);
+	if (stage->sp_func != IA_CSS_PIPELINE_NO_FUNC)
+		return;
+
+	if (pg_inst.do_init) {
+		ia_css_debug_pipe_graph_dump_prologue();
+		pg_inst.do_init = false;
+	}
+
+	if (stage->binary) {
+		bin_type = "binary";
+		if (stage->binary->info->blob)
+			snprintf(blob_name, sizeof(blob_name), "%s_stage%d",
+				 stage->binary->info->blob->name, stage->stage_num);
+	} else if (stage->firmware) {
+		bin_type = "firmware";
+
+		strscpy(blob_name, IA_CSS_EXT_ISP_PROG_NAME(stage->firmware),
+			sizeof(blob_name));
+	}
+
+	/* Guard in case of binaries that don't have any binary_info */
+	if (stage->binary_info) {
+		char enable_info1[100];
+		char enable_info2[100];
+		char enable_info3[100];
+		char enable_info[200];
+		struct ia_css_binary_info *bi = stage->binary_info;
+
+		/* Split it in 2 function-calls to keep the amount of
+		 * parameters per call "reasonable"
+		 */
+		snprintf(enable_info1, sizeof(enable_info1),
+			 "%s%s%s%s%s%s%s%s%s%s%s%s%s%s",
+			 bi->enable.reduced_pipe ?	"rp," : "",
+			 bi->enable.vf_veceven ?		"vfve," : "",
+			 bi->enable.dis ?		"dis," : "",
+			 bi->enable.dvs_envelope ?	"dvse," : "",
+			 bi->enable.uds ?		"uds," : "",
+			 bi->enable.dvs_6axis ?		"dvs6," : "",
+			 bi->enable.block_output ?	"bo," : "",
+			 bi->enable.ds ?			"ds," : "",
+			 bi->enable.bayer_fir_6db ?	"bf6," : "",
+			 bi->enable.raw_binning ?	"rawb," : "",
+			 bi->enable.continuous ?		"cont," : "",
+			 bi->enable.s3a ?		"s3a," : "",
+			 bi->enable.fpnr ?		"fpnr," : "",
+			 bi->enable.sc ?			"sc," : ""
+			);
+
+		snprintf(enable_info2, sizeof(enable_info2),
+			 "%s%s%s%s%s%s%s%s%s%s%s",
+			 bi->enable.macc ?		"macc," : "",
+			 bi->enable.output ?		"outp," : "",
+			 bi->enable.ref_frame ?		"reff," : "",
+			 bi->enable.tnr ?		"tnr," : "",
+			 bi->enable.xnr ?		"xnr," : "",
+			 bi->enable.params ?		"par," : "",
+			 bi->enable.ca_gdc ?		"cagdc," : "",
+			 bi->enable.isp_addresses ?	"ispa," : "",
+			 bi->enable.in_frame ?		"inf," : "",
+			 bi->enable.out_frame ?		"outf," : "",
+			 bi->enable.high_speed ?		"hs," : ""
+			);
+
+		/* And merge them into one string */
+		snprintf(enable_info, sizeof(enable_info), "%s%s",
+			 enable_info1, enable_info2);
+		{
+			int l, p;
+			char *ei = enable_info;
+
+			l = strlen(ei);
+
+			/* Replace last ',' with \0 if present */
+			if (l && enable_info[l - 1] == ',')
+				enable_info[--l] = '\0';
+
+			if (l > ENABLE_LINE_MAX_LENGTH) {
+				/* Too big for one line, find last comma */
+				p = ENABLE_LINE_MAX_LENGTH;
+				while (ei[p] != ',')
+					p--;
+				/* Last comma found, copy till that comma */
+				strscpy(enable_info1, ei,
+                                        p > sizeof(enable_info1) ? sizeof(enable_info1) : p);
+
+				ei += p + 1;
+				l = strlen(ei);
+
+				if (l <= ENABLE_LINE_MAX_LENGTH) {
+					/* The 2nd line fits */
+					/* we cannot use ei as argument because
+					 * it is not guaranteed dword aligned
+					 */
+
+					strscpy(enable_info2, ei,
+						l > sizeof(enable_info2) ? sizeof(enable_info2) : l);
+
+					snprintf(enable_info, sizeof(enable_info), "%s\\n%s",
+						 enable_info1, enable_info2);
+
+				} else {
+					/* 2nd line is still too long */
+					p = ENABLE_LINE_MAX_LENGTH;
+					while (ei[p] != ',')
+						p--;
+
+					strscpy(enable_info2, ei,
+						p > sizeof(enable_info2) ? sizeof(enable_info2) : p);
+
+					ei += p + 1;
+					l = strlen(ei);
+
+					if (l <= ENABLE_LINE_MAX_LENGTH) {
+						/* The 3rd line fits */
+						/* we cannot use ei as argument because
+						* it is not guaranteed dword aligned
+						*/
+						strscpy(enable_info3, ei,
+							sizeof(enable_info3));
+						snprintf(enable_info, sizeof(enable_info),
+							 "%s\\n%s\\n%s",
+							 enable_info1, enable_info2,
+							 enable_info3);
+					} else {
+						/* 3rd line is still too long */
+						p = ENABLE_LINE_MAX_LENGTH;
+						while (ei[p] != ',')
+							p--;
+						strscpy(enable_info3, ei,
+							p > sizeof(enable_info3) ? sizeof(enable_info3) : p);
+						ei += p + 1;
+						strscpy(enable_info3, ei,
+							sizeof(enable_info3));
+						snprintf(enable_info, sizeof(enable_info),
+							 "%s\\n%s\\n%s",
+							 enable_info1, enable_info2,
+							 enable_info3);
+					}
+				}
+			}
+		}
+
+		dtrace_dot("node [shape = circle, fixedsize=true, width=2.5, label=\"%s\\n%s\\n\\n%s\"]; \"%s(pipe%d)\"",
+			   bin_type, blob_name, enable_info, blob_name, id);
+	} else {
+		dtrace_dot("node [shape = circle, fixedsize=true, width=2.5, label=\"%s\\n%s\\n\"]; \"%s(pipe%d)\"",
+			   bin_type, blob_name, blob_name, id);
+	}
+
+	if (stage->stage_num == 0) {
+		/*
+		 * There are some implicite assumptions about which bin is the
+		 * input binary e.g. which one is connected to the input system
+		 * Priority:
+		 * 1) sp_raw_copy bin has highest priority
+		 * 2) First stage==0 binary of preview, video or capture
+		 */
+		if (strlen(dot_id_input_bin) == 0) {
+			snprintf(dot_id_input_bin, sizeof(dot_id_input_bin),
+				 "%s(pipe%d)", blob_name, id);
+		}
+	}
+
+	if (stage->args.in_frame) {
+		ia_css_debug_pipe_graph_dump_frame(
+		    stage->args.in_frame, id, blob_name,
+		    "in", true);
+	}
+
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+		if (stage->args.tnr_frames[i]) {
+			ia_css_debug_pipe_graph_dump_frame(
+			    stage->args.tnr_frames[i], id,
+			    blob_name, "tnr_frame", true);
+		}
+	}
+
+	for (i = 0; i < MAX_NUM_VIDEO_DELAY_FRAMES; i++) {
+		if (stage->args.delay_frames[i]) {
+			ia_css_debug_pipe_graph_dump_frame(
+			    stage->args.delay_frames[i], id,
+			    blob_name, "delay_frame", true);
+		}
+	}
+
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		if (stage->args.out_frame[i]) {
+			ia_css_debug_pipe_graph_dump_frame(
+			    stage->args.out_frame[i], id, blob_name,
+			    "out", false);
+		}
+	}
+
+	if (stage->args.out_vf_frame) {
+		ia_css_debug_pipe_graph_dump_frame(
+		    stage->args.out_vf_frame, id, blob_name,
+		    "out_vf", false);
+	}
+}
+
+void
+ia_css_debug_pipe_graph_dump_sp_raw_copy(
+    struct ia_css_frame *out_frame)
+{
+	assert(out_frame);
+	if (pg_inst.do_init) {
+		ia_css_debug_pipe_graph_dump_prologue();
+		pg_inst.do_init = false;
+	}
+
+	dtrace_dot("node [shape = circle, fixedsize=true, width=2.5, label=\"%s\\n%s\"]; \"%s(pipe%d)\"",
+		   "sp-binary", "sp_raw_copy", "sp_raw_copy", 1);
+
+	snprintf(ring_buffer, sizeof(ring_buffer),
+		 "node [shape = box, fixedsize=true, width=2, height=0.7]; \"%p\" [label = \"%s\\n%d(%d) x %d\\nRingbuffer\"];",
+		 out_frame,
+		 debug_frame_format2str(out_frame->info.format),
+		 out_frame->info.res.width,
+		 out_frame->info.padded_width,
+		 out_frame->info.res.height);
+
+	dtrace_dot(ring_buffer);
+
+	dtrace_dot(
+	    "\"%s(pipe%d)\"->\"%p\" [label = out_frame];",
+	    "sp_raw_copy", 1, out_frame);
+
+	snprintf(dot_id_input_bin, sizeof(dot_id_input_bin), "%s(pipe%d)",
+		 "sp_raw_copy", 1);
+}
+
+void
+ia_css_debug_pipe_graph_dump_stream_config(
+    const struct ia_css_stream_config *stream_config)
+{
+	pg_inst.width = stream_config->input_config.input_res.width;
+	pg_inst.height = stream_config->input_config.input_res.height;
+	pg_inst.eff_width = stream_config->input_config.effective_res.width;
+	pg_inst.eff_height = stream_config->input_config.effective_res.height;
+	pg_inst.stream_format = stream_config->input_config.format;
+}
+
+void
+ia_css_debug_dump_resolution(
+    const struct ia_css_resolution *res,
+    const char *label)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s: =%d x =%d\n",
+			    label, res->width, res->height);
+}
+
+void
+ia_css_debug_dump_frame_info(
+    const struct ia_css_frame_info *info,
+    const char *label)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s\n", label);
+	ia_css_debug_dump_resolution(&info->res, "res");
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "padded_width: %d\n",
+			    info->padded_width);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "format: %d\n", info->format);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "raw_bit_depth: %d\n",
+			    info->raw_bit_depth);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "raw_bayer_order: %d\n",
+			    info->raw_bayer_order);
+}
+
+void
+ia_css_debug_dump_capture_config(
+    const struct ia_css_capture_config *config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s\n", __func__);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "mode: %d\n", config->mode);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "enable_xnr:  %d\n",
+			    config->enable_xnr);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "enable_raw_output: %d\n",
+			    config->enable_raw_output);
+}
+
+void
+ia_css_debug_dump_pipe_extra_config(
+    const struct ia_css_pipe_extra_config *extra_config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s\n", __func__);
+	if (extra_config) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "enable_raw_binning: %d\n",
+				    extra_config->enable_raw_binning);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "enable_yuv_ds: %d\n",
+				    extra_config->enable_yuv_ds);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "enable_high_speed:  %d\n",
+				    extra_config->enable_high_speed);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "enable_dvs_6axis: %d\n",
+				    extra_config->enable_dvs_6axis);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "enable_reduced_pipe: %d\n",
+				    extra_config->enable_reduced_pipe);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "enable_fractional_ds: %d\n",
+				    extra_config->enable_fractional_ds);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "disable_vf_pp: %d\n",
+				    extra_config->disable_vf_pp);
+	}
+}
+
+void
+ia_css_debug_dump_pipe_config(
+    const struct ia_css_pipe_config *config)
+{
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("config = %p", config);
+	if (!config) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "mode: %d\n", config->mode);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "isp_pipe_version: %d\n",
+			    config->isp_pipe_version);
+	ia_css_debug_dump_resolution(&config->bayer_ds_out_res,
+				     "bayer_ds_out_res");
+	ia_css_debug_dump_resolution(&config->capt_pp_in_res,
+				     "capt_pp_in_res");
+	ia_css_debug_dump_resolution(&config->vf_pp_in_res, "vf_pp_in_res");
+
+	if (IS_ISP2401) {
+		ia_css_debug_dump_resolution(&config->output_system_in_res,
+					    "output_system_in_res");
+	}
+	ia_css_debug_dump_resolution(&config->dvs_crop_out_res,
+				     "dvs_crop_out_res");
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		ia_css_debug_dump_frame_info(&config->output_info[i], "output_info");
+		ia_css_debug_dump_frame_info(&config->vf_output_info[i],
+					     "vf_output_info");
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "acc_extension: %p\n",
+			    config->acc_extension);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "num_acc_stages: %d\n",
+			    config->num_acc_stages);
+	ia_css_debug_dump_capture_config(&config->default_capture_config);
+	ia_css_debug_dump_resolution(&config->dvs_envelope, "dvs_envelope");
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "dvs_frame_delay: %d\n",
+			    config->dvs_frame_delay);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "acc_num_execs: %d\n",
+			    config->acc_num_execs);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "enable_dz: %d\n",
+			    config->enable_dz);
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void
+ia_css_debug_dump_stream_config_source(
+    const struct ia_css_stream_config *config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	switch (config->mode) {
+	case IA_CSS_INPUT_MODE_SENSOR:
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "source.port\n");
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "port: %d\n",
+				    config->source.port.port);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "num_lanes: %d\n",
+				    config->source.port.num_lanes);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "timeout: %d\n",
+				    config->source.port.timeout);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "compression: %d\n",
+				    config->source.port.compression.type);
+		break;
+	case IA_CSS_INPUT_MODE_TPG:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "source.tpg\n");
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "id: %d\n",
+				    config->source.tpg.id);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "mode: %d\n",
+				    config->source.tpg.mode);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "x_mask: 0x%x\n",
+				    config->source.tpg.x_mask);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "x_delta: %d\n",
+				    config->source.tpg.x_delta);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "y_mask: 0x%x\n",
+				    config->source.tpg.y_mask);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "y_delta: %d\n",
+				    config->source.tpg.y_delta);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "xy_mask: 0x%x\n",
+				    config->source.tpg.xy_mask);
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "source.prbs\n");
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "id: %d\n",
+				    config->source.prbs.id);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "h_blank: %d\n",
+				    config->source.prbs.h_blank);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "v_blank: %d\n",
+				    config->source.prbs.v_blank);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "seed: 0x%x\n",
+				    config->source.prbs.seed);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "seed1: 0x%x\n",
+				    config->source.prbs.seed1);
+		break;
+	default:
+	case IA_CSS_INPUT_MODE_FIFO:
+	case IA_CSS_INPUT_MODE_MEMORY:
+		break;
+	}
+}
+
+void
+ia_css_debug_dump_mipi_buffer_config(
+    const struct ia_css_mipi_buffer_config *config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "size_mem_words: %d\n",
+			    config->size_mem_words);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "nof_mipi_buffers: %d\n",
+			    config->nof_mipi_buffers);
+}
+
+void
+ia_css_debug_dump_metadata_config(
+    const struct ia_css_metadata_config *config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "data_type: %d\n",
+			    config->data_type);
+	ia_css_debug_dump_resolution(&config->resolution, "resolution");
+}
+
+void
+ia_css_debug_dump_stream_config(
+    const struct ia_css_stream_config *config,
+    int num_pipes)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "num_pipes: %d\n", num_pipes);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "mode: %d\n", config->mode);
+	ia_css_debug_dump_stream_config_source(config);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "channel_id: %d\n",
+			    config->channel_id);
+	ia_css_debug_dump_resolution(&config->input_config.input_res, "input_res");
+	ia_css_debug_dump_resolution(&config->input_config.effective_res,
+				     "effective_res");
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "format: %d\n",
+			    config->input_config.format);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "bayer_order: %d\n",
+			    config->input_config.bayer_order);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sensor_binning_factor: %d\n",
+			    config->sensor_binning_factor);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "pixels_per_clock: %d\n",
+			    config->pixels_per_clock);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "online: %d\n",
+			    config->online);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "init_num_cont_raw_buf: %d\n",
+			    config->init_num_cont_raw_buf);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "target_num_cont_raw_buf: %d\n",
+			    config->target_num_cont_raw_buf);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "pack_raw_pixels: %d\n",
+			    config->pack_raw_pixels);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "continuous: %d\n",
+			    config->continuous);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "flash_gpio_pin: %d\n",
+			    config->flash_gpio_pin);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "left_padding: %d\n",
+			    config->left_padding);
+	ia_css_debug_dump_mipi_buffer_config(&config->mipi_buffer_config);
+	ia_css_debug_dump_metadata_config(&config->metadata_config);
+}
+
+/*
+    Trace support.
+
+    This tracer is using a buffer to trace the flow of the FW and dump misc values (see below for details).
+    Currently, support is only for SKC.
+    To enable support for other platforms:
+     - Allocate a buffer for tracing in DMEM. The longer the better.
+     - Use the DBG_init routine in sp.hive.c to initiatilize the tracer with the address and size selected.
+     - Add trace points in the SP code wherever needed.
+     - Enable the dump below with the required address and required adjustments.
+	   Dump is called at the end of ia_css_debug_dump_sp_state().
+*/
+
+/*
+ dump_trace() : dump the trace points from DMEM2.
+ for every trace point, the following are printed: index, major:minor and the 16-bit attached value.
+ The routine looks for the first 0, and then prints from it cyclically.
+ Data forma in DMEM2:
+  first 4 DWORDS: header
+   DWORD 0: data description
+    byte 0: version
+    byte 1: number of threads (for future use)
+    byte 2+3: number ot TPs
+   DWORD 1: command byte + data (for future use)
+    byte 0: command
+    byte 1-3: command signature
+   DWORD 2-3: additional data (for future use)
+  Following data is 4-byte oriented:
+    byte 0:   major
+	byte 1:   minor
+	byte 2-3: data
+*/
+#if TRACE_ENABLE_SP0 || TRACE_ENABLE_SP1 || TRACE_ENABLE_ISP
+static void debug_dump_one_trace(enum TRACE_CORE_ID proc_id)
+{
+#if defined(HAS_TRACER_V2)
+	u32 start_addr;
+	u32 start_addr_data;
+	u32 item_size;
+	u32 tmp;
+	u8 tid_val;
+	enum TRACE_DUMP_FORMAT dump_format;
+
+	int i, j, max_trace_points, point_num, limit = -1;
+	/* using a static buffer here as the driver has issues allocating memory */
+	static u32 trace_read_buf[TRACE_BUFF_SIZE] = {0};
+	static struct trace_header_t header;
+	u8 *header_arr;
+
+	/* read the header and parse it */
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "~~~ Tracer ");
+	switch (proc_id) {
+	case TRACE_SP0_ID:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "SP0");
+		start_addr = TRACE_SP0_ADDR;
+		start_addr_data = TRACE_SP0_DATA_ADDR;
+		item_size = TRACE_SP0_ITEM_SIZE;
+		max_trace_points = TRACE_SP0_MAX_POINTS;
+		break;
+	case TRACE_SP1_ID:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "SP1");
+		start_addr = TRACE_SP1_ADDR;
+		start_addr_data = TRACE_SP1_DATA_ADDR;
+		item_size = TRACE_SP1_ITEM_SIZE;
+		max_trace_points = TRACE_SP1_MAX_POINTS;
+		break;
+	case TRACE_ISP_ID:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ISP");
+		start_addr = TRACE_ISP_ADDR;
+		start_addr_data = TRACE_ISP_DATA_ADDR;
+		item_size = TRACE_ISP_ITEM_SIZE;
+		max_trace_points = TRACE_ISP_MAX_POINTS;
+		break;
+	default:
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "\t\ttraces are not supported for this processor ID - exiting\n");
+		return;
+	}
+
+	if (!IS_ISP2401) {
+		tmp = ia_css_device_load_uint32(start_addr);
+		point_num = (tmp >> 16) & 0xFFFF;
+
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, " ver %d %d points\n", tmp & 0xFF,
+				    point_num);
+	} else {
+		/* Loading byte-by-byte as using the master routine had issues */
+		header_arr = (uint8_t *)&header;
+		for (i = 0; i < (int)sizeof(struct trace_header_t); i++)
+			header_arr[i] = ia_css_device_load_uint8(start_addr + (i));
+
+		point_num = header.max_tracer_points;
+
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, " ver %d %d points\n", header.version,
+				    point_num);
+
+		tmp = header.version;
+	}
+	if ((tmp & 0xFF) != TRACER_VER) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "\t\tUnknown version - exiting\n");
+		return;
+	}
+	if (point_num > max_trace_points) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "\t\tToo many points - exiting\n");
+		return;
+	}
+	/* copy the TPs and find the first 0 */
+	for (i = 0; i < point_num; i++) {
+		trace_read_buf[i] = ia_css_device_load_uint32(start_addr_data +
+				    (i * item_size));
+		if ((limit == (-1)) && (trace_read_buf[i] == 0))
+			limit = i;
+	}
+	if (IS_ISP2401) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "Status:\n");
+		for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++)
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "\tT%d: %3d (%02x)  %6d (%04x)  %10d (%08x)\n", i,
+					    header.thr_status_byte[i], header.thr_status_byte[i],
+					    header.thr_status_word[i], header.thr_status_word[i],
+					    header.thr_status_dword[i], header.thr_status_dword[i]);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "Scratch:\n");
+		for (i = 0; i < MAX_SCRATCH_DATA; i++)
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%10d (%08x)  ",
+					    header.scratch_debug[i], header.scratch_debug[i]);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "\n");
+	}
+	/* two 0s in the beginning: empty buffer */
+	if ((trace_read_buf[0] == 0) && (trace_read_buf[1] == 0)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "\t\tEmpty tracer - exiting\n");
+		return;
+	}
+	/* no overrun: start from 0 */
+	if ((limit == point_num - 1) ||
+	    /* first 0 is at the end - border case */
+	    (trace_read_buf[limit + 1] ==
+	     0))   /* did not make a full cycle after the memset */
+		limit = 0;
+	/* overrun: limit is the first non-zero after the first zero */
+	else
+		limit++;
+
+	/* print the TPs */
+	for (i = 0; i < point_num; i++) {
+		j = (limit + i) % point_num;
+		if (trace_read_buf[j]) {
+			if (!IS_ISP2401) {
+				TRACE_DUMP_FORMAT dump_format = FIELD_FORMAT_UNPACK(trace_read_buf[j]);
+			} else {
+				tid_val = FIELD_TID_UNPACK(trace_read_buf[j]);
+				dump_format = TRACE_DUMP_FORMAT_POINT;
+
+				/*
+				* When tid value is 111b, the data will be interpreted differently:
+				* tid val is ignored, major field contains 2 bits (msb) for format type
+				*/
+				if (tid_val == FIELD_TID_SEL_FORMAT_PAT) {
+					dump_format = FIELD_FORMAT_UNPACK(trace_read_buf[j]);
+				}
+			}
+			switch (dump_format) {
+			case TRACE_DUMP_FORMAT_POINT:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d %d:%d value - %d\n",
+				    j, FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_MINOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_UNPACK(trace_read_buf[j]));
+				break;
+			/* ISP2400 */
+			case TRACE_DUMP_FORMAT_VALUE24_HEX:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %x H\n",
+				    j,
+				    FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			/* ISP2400 */
+			case TRACE_DUMP_FORMAT_VALUE24_DEC:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %d D\n",
+				    j,
+				    FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			/* ISP2401 */
+			case TRACE_DUMP_FORMAT_POINT_NO_TID:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d %d:%d value - %x (%d)\n",
+				    j,
+				    FIELD_MAJOR_W_FMT_UNPACK(trace_read_buf[j]),
+				    FIELD_MINOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_UNPACK(trace_read_buf[j]));
+				break;
+			/* ISP2401 */
+			case TRACE_DUMP_FORMAT_VALUE24:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %x (%d)\n",
+				    j,
+				    FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_MAJOR_W_FMT_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			case TRACE_DUMP_FORMAT_VALUE24_TIMING:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, timing %x\n",
+				    j,
+				    FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			case TRACE_DUMP_FORMAT_VALUE24_TIMING_DELTA:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, timing delta %x\n",
+				    j,
+				    FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+				    FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			default:
+				ia_css_debug_dtrace(
+				    IA_CSS_DEBUG_TRACE,
+				    "no such trace dump format %d",
+				    dump_format);
+				break;
+			}
+		}
+	}
+#else
+	(void)proc_id;
+#endif /* HAS_TRACER_V2 */
+}
+#endif /* TRACE_ENABLE_SP0 || TRACE_ENABLE_SP1 || TRACE_ENABLE_ISP */
+
+void ia_css_debug_dump_trace(void)
+{
+#if TRACE_ENABLE_SP0
+	debug_dump_one_trace(TRACE_SP0_ID);
+#endif
+#if TRACE_ENABLE_SP1
+	debug_dump_one_trace(TRACE_SP1_ID);
+#endif
+#if TRACE_ENABLE_ISP
+	debug_dump_one_trace(TRACE_ISP_ID);
+#endif
+}
+
+/* Tagger state dump function. The tagger is only available when the CSS
+ * contains an input system (2400 or 2401). */
+void ia_css_debug_tagger_state(void)
+{
+	unsigned int i;
+	unsigned int HIVE_ADDR_tagger_frames;
+	ia_css_tagger_buf_sp_elem_t tbuf_frames[MAX_CB_ELEMS_FOR_TAGGER];
+
+	HIVE_ADDR_tagger_frames = sh_css_sp_fw.info.sp.tagger_frames_addr;
+
+	/* This variable is not used in crun */
+	(void)HIVE_ADDR_tagger_frames;
+
+	/* 2400 and 2401 only have 1 SP, so the tagger lives on SP0 */
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(tagger_frames),
+		     tbuf_frames,
+		     sizeof(tbuf_frames));
+
+	ia_css_debug_dtrace(2, "Tagger Info:\n");
+	for (i = 0; i < MAX_CB_ELEMS_FOR_TAGGER; i++) {
+		ia_css_debug_dtrace(2, "\t tagger frame[%d]: exp_id=%d, marked=%d, locked=%d\n",
+				    i, tbuf_frames[i].exp_id, tbuf_frames[i].mark, tbuf_frames[i].lock);
+	}
+}
+
+/* ISP2401 */
+void ia_css_debug_pc_dump(sp_ID_t id, unsigned int num_of_dumps)
+{
+	unsigned int pc;
+	unsigned int i;
+	hrt_data sc = sp_ctrl_load(id, SP_SC_REG);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "SP%-1d Status reg: 0x%X\n", id, sc);
+	sc = sp_ctrl_load(id, SP_CTRL_SINK_REG);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "SP%-1d Stall reg: 0x%X\n", id, sc);
+	for (i = 0; i < num_of_dumps; i++) {
+		pc = sp_ctrl_load(id, SP_PC_REG);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "SP%-1d PC: 0x%X\n", id, pc);
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/event/interface/ia_css_event.h b/drivers/staging/media/atomisp/pci/runtime/event/interface/ia_css_event.h
new file mode 100644
index 000000000..ebbd90b14
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/event/interface/ia_css_event.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_EVENT_H
+#define _IA_CSS_EVENT_H
+
+#include <type_support.h>
+#include "sw_event_global.h"    /*event macros.TODO : Change File Name..???*/
+
+bool ia_css_event_encode(
+    u8	*in,
+    u8	nr,
+    uint32_t	*out);
+
+void ia_css_event_decode(
+    u32 event,
+    uint8_t *payload);
+
+#endif /*_IA_CSS_EVENT_H*/
diff --git a/drivers/staging/media/atomisp/pci/runtime/event/src/event.c b/drivers/staging/media/atomisp/pci/runtime/event/src/event.c
new file mode 100644
index 000000000..e702297b0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/event/src/event.c
@@ -0,0 +1,110 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "sh_css_sp.h"
+
+#include "dma.h"	/* N_DMA_CHANNEL_ID */
+
+#include <type_support.h>
+#include "ia_css_binary.h"
+#include "sh_css_hrt.h"
+#include "sh_css_defs.h"
+#include "sh_css_internal.h"
+#include "ia_css_debug.h"
+#include "ia_css_debug_internal.h"
+#include "sh_css_legacy.h"
+
+#include "gdc_device.h"				/* HRT_GDC_N */
+
+/*#include "sp.h"*/	/* host2sp_enqueue_frame_data() */
+
+#include "assert_support.h"
+
+#include "ia_css_queue.h"	/* host_sp_enqueue_XXX */
+#include "ia_css_event.h"	/* ia_css_event_encode */
+/*
+ * @brief Encode the information into the software-event.
+ * Refer to "sw_event_public.h" for details.
+ */
+bool ia_css_event_encode(
+    u8	*in,
+    u8	nr,
+    uint32_t	*out)
+{
+	bool ret;
+	u32 nr_of_bits;
+	u32 i;
+
+	assert(in);
+	assert(out);
+	OP___assert(nr > 0 && nr <= MAX_NR_OF_PAYLOADS_PER_SW_EVENT);
+
+	/* initialize the output */
+	*out = 0;
+
+	/* get the number of bits per information */
+	nr_of_bits = sizeof(uint32_t) * 8 / nr;
+
+	/* compress the all inputs into a signle output */
+	for (i = 0; i < nr; i++) {
+		*out <<= nr_of_bits;
+		*out |= in[i];
+	}
+
+	/* get the return value */
+	ret = (nr > 0 && nr <= MAX_NR_OF_PAYLOADS_PER_SW_EVENT);
+
+	return ret;
+}
+
+void ia_css_event_decode(
+    u32 event,
+    uint8_t *payload)
+{
+	assert(payload[1] == 0);
+	assert(payload[2] == 0);
+	assert(payload[3] == 0);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_event_decode() enter:\n");
+
+	/* First decode according to the common case
+	 * In case of a PORT_EOF event we overwrite with
+	 * the specific values
+	 * This is somewhat ugly but probably somewhat efficient
+	 * (and it avoids some code duplication)
+	 */
+	payload[0] = event & 0xff;  /*event_code */
+	payload[1] = (event >> 8) & 0xff;
+	payload[2] = (event >> 16) & 0xff;
+	payload[3] = 0;
+
+	switch (payload[0]) {
+	case SH_CSS_SP_EVENT_PORT_EOF:
+		payload[2] = 0;
+		payload[3] = (event >> 24) & 0xff;
+		break;
+
+	case SH_CSS_SP_EVENT_ACC_STAGE_COMPLETE:
+	case SH_CSS_SP_EVENT_TIMER:
+	case SH_CSS_SP_EVENT_FRAME_TAGGED:
+	case SH_CSS_SP_EVENT_FW_WARNING:
+	case SH_CSS_SP_EVENT_FW_ASSERT:
+		payload[3] = (event >> 24) & 0xff;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/eventq/interface/ia_css_eventq.h b/drivers/staging/media/atomisp/pci/runtime/eventq/interface/ia_css_eventq.h
new file mode 100644
index 000000000..fd001ae35
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/eventq/interface/ia_css_eventq.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_EVENTQ_H
+#define _IA_CSS_EVENTQ_H
+
+#include "ia_css_queue.h"	/* queue APIs */
+
+/**
+ * @brief HOST receives event from SP.
+ *
+ * @param[in]	eventq_handle	eventq_handle.
+ * @param[in]	payload		The event payload.
+ * @return	0		- Successfully dequeue.
+ * @return	-EINVAL		- Invalid argument.
+ * @return	-ENODATA		- Queue is empty.
+ */
+int ia_css_eventq_recv(
+    ia_css_queue_t *eventq_handle,
+    uint8_t *payload);
+
+/**
+ * @brief The Host sends the event to SP.
+ * The caller of this API will be blocked until the event
+ * is sent.
+ *
+ * @param[in]	eventq_handle   eventq_handle.
+ * @param[in]	evt_id		The event ID.
+ * @param[in]	evt_payload_0	The event payload.
+ * @param[in]	evt_payload_1	The event payload.
+ * @param[in]	evt_payload_2	The event payload.
+ * @return	0		- Successfully enqueue.
+ * @return	-EINVAL		- Invalid argument.
+ * @return	-ENOBUFS		- Queue is full.
+ */
+int ia_css_eventq_send(
+    ia_css_queue_t *eventq_handle,
+    u8 evt_id,
+    u8 evt_payload_0,
+    u8 evt_payload_1,
+    uint8_t evt_payload_2);
+#endif /* _IA_CSS_EVENTQ_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/eventq/src/eventq.c b/drivers/staging/media/atomisp/pci/runtime/eventq/src/eventq.c
new file mode 100644
index 000000000..df75cef46
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/eventq/src/eventq.c
@@ -0,0 +1,76 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_types.h"
+#include "assert_support.h"
+#include "ia_css_queue.h" /* sp2host_dequeue_irq_event() */
+#include "ia_css_eventq.h"
+#include "ia_css_event.h"	/* ia_css_event_encode()
+				ia_css_event_decode()
+				*/
+int ia_css_eventq_recv(
+    ia_css_queue_t *eventq_handle,
+    uint8_t *payload)
+{
+	u32 sp_event;
+	int error;
+
+	/* dequeue the IRQ event */
+	error = ia_css_queue_dequeue(eventq_handle, &sp_event);
+
+	/* check whether the IRQ event is available or not */
+	if (!error)
+		ia_css_event_decode(sp_event, payload);
+	return error;
+}
+
+/*
+ * @brief The Host sends the event to the SP.
+ * Refer to "sh_css_sp.h" for details.
+ */
+int ia_css_eventq_send(
+    ia_css_queue_t *eventq_handle,
+    u8 evt_id,
+    u8 evt_payload_0,
+    u8 evt_payload_1,
+    uint8_t evt_payload_2)
+{
+	u8 tmp[4];
+	u32 sw_event;
+	int error = -ENOSYS;
+
+	/*
+	 * Encode the queue type, the thread ID and
+	 * the queue ID into the event.
+	 */
+	tmp[0] = evt_id;
+	tmp[1] = evt_payload_0;
+	tmp[2] = evt_payload_1;
+	tmp[3] = evt_payload_2;
+	ia_css_event_encode(tmp, 4, &sw_event);
+
+	/* queue the software event (busy-waiting) */
+	for ( ; ; ) {
+		error = ia_css_queue_enqueue(eventq_handle, sw_event);
+		if (error != -ENOBUFS) {
+			/* We were able to successfully send the event
+			   or had a real failure. return the status*/
+			break;
+		}
+		/* Wait for the queue to be not full and try again*/
+		udelay(1);
+	}
+	return error;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame.h b/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame.h
new file mode 100644
index 000000000..700070c58
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame.h
@@ -0,0 +1,141 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FRAME_H__
+#define __IA_CSS_FRAME_H__
+
+/* ISP2401 */
+#include <ia_css_types.h>
+
+#include <ia_css_frame_format.h>
+#include <ia_css_frame_public.h>
+#include "dma.h"
+
+/*********************************************************************
+****	Frame INFO APIs
+**********************************************************************/
+/* @brief Sets the given width and alignment to the frame info
+ *
+ * @param
+ * @param[in]	info        The info to which parameters would set
+ * @param[in]	width       The width to be set to info
+ * @param[in]	aligned     The aligned to be set to info
+ * @return
+ */
+void ia_css_frame_info_set_width(struct ia_css_frame_info *info,
+				 unsigned int width,
+				 unsigned int min_padded_width);
+
+/* @brief Sets the given format to the frame info
+ *
+ * @param
+ * @param[in]	info        The info to which parameters would set
+ * @param[in]	format      The format to be set to info
+ * @return
+ */
+void ia_css_frame_info_set_format(struct ia_css_frame_info *info,
+				  enum ia_css_frame_format format);
+
+/* @brief Sets the frame info with the given parameters
+ *
+ * @param
+ * @param[in]	info        The info to which parameters would set
+ * @param[in]	width       The width to be set to info
+ * @param[in]	height      The height to be set to info
+ * @param[in]	format      The format to be set to info
+ * @param[in]	aligned     The aligned to be set to info
+ * @return
+ */
+void ia_css_frame_info_init(struct ia_css_frame_info *info,
+			    unsigned int width,
+			    unsigned int height,
+			    enum ia_css_frame_format format,
+			    unsigned int aligned);
+
+/* @brief Checks whether 2 frame infos has the same resolution
+ *
+ * @param
+ * @param[in]	frame_a         The first frame to be compared
+ * @param[in]	frame_b         The second frame to be compared
+ * @return      Returns true if the frames are equal
+ */
+bool ia_css_frame_info_is_same_resolution(
+    const struct ia_css_frame_info *info_a,
+    const struct ia_css_frame_info *info_b);
+
+/* @brief Check the frame info is valid
+ *
+ * @param
+ * @param[in]	info       The frame attributes to be initialized
+ * @return	The error code.
+ */
+int ia_css_frame_check_info(const struct ia_css_frame_info *info);
+
+/*********************************************************************
+****	Frame APIs
+**********************************************************************/
+
+/* @brief Initialize the plane depending on the frame type
+ *
+ * @param
+ * @param[in]	frame           The frame attributes to be initialized
+ * @return	The error code.
+ */
+int ia_css_frame_init_planes(struct ia_css_frame *frame);
+
+/* @brief Free an array of frames
+ *
+ * @param
+ * @param[in]	num_frames      The number of frames to be freed in the array
+ * @param[in]   **frames_array  The array of frames to be removed
+ * @return
+ */
+void ia_css_frame_free_multiple(unsigned int num_frames,
+				struct ia_css_frame **frames_array);
+
+/* @brief Allocate a CSS frame structure of given size in bytes..
+ *
+ * @param	frame	The allocated frame.
+ * @param[in]	size_bytes	The frame size in bytes.
+ * @return	The error code.
+ *
+ * Allocate a frame using the given size in bytes.
+ * The frame structure is partially null initialized.
+ */
+int ia_css_frame_allocate_with_buffer_size(struct ia_css_frame **frame,
+					   const unsigned int size_bytes);
+
+/* @brief Check whether 2 frames are same type
+ *
+ * @param
+ * @param[in]	frame_a         The first frame to be compared
+ * @param[in]	frame_b         The second frame to be compared
+ * @return      Returns true if the frames are equal
+ */
+bool ia_css_frame_is_same_type(
+    const struct ia_css_frame *frame_a,
+    const struct ia_css_frame *frame_b);
+
+/* @brief Configure a dma port from frame info
+ *
+ * @param
+ * @param[in]	config         The DAM port configuration
+ * @param[in]	info           The frame info
+ * @return
+ */
+int ia_css_dma_configure_from_info(struct dma_port_config *config,
+				   const struct ia_css_frame_info *info);
+
+#endif /* __IA_CSS_FRAME_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame_comm.h b/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame_comm.h
new file mode 100644
index 000000000..ce6110efb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/frame/interface/ia_css_frame_comm.h
@@ -0,0 +1,116 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_FRAME_COMM_H__
+#define __IA_CSS_FRAME_COMM_H__
+
+#include "type_support.h"
+#include "platform_support.h"
+#include "runtime/bufq/interface/ia_css_bufq_comm.h"
+#include <system_local.h>	 /* ia_css_ptr */
+
+/*
+ * These structs are derived from structs defined in ia_css_types.h
+ * (just take out the "_sp" from the struct name to get the "original")
+ * All the fields that are not needed by the SP are removed.
+ */
+struct ia_css_frame_sp_plane {
+	unsigned int offset;	/* offset in bytes to start of frame data */
+	/* offset is wrt data in sh_css_sp_sp_frame */
+};
+
+struct ia_css_frame_sp_binary_plane {
+	unsigned int size;
+	struct ia_css_frame_sp_plane data;
+};
+
+struct ia_css_frame_sp_yuv_planes {
+	struct ia_css_frame_sp_plane y;
+	struct ia_css_frame_sp_plane u;
+	struct ia_css_frame_sp_plane v;
+};
+
+struct ia_css_frame_sp_nv_planes {
+	struct ia_css_frame_sp_plane y;
+	struct ia_css_frame_sp_plane uv;
+};
+
+struct ia_css_frame_sp_rgb_planes {
+	struct ia_css_frame_sp_plane r;
+	struct ia_css_frame_sp_plane g;
+	struct ia_css_frame_sp_plane b;
+};
+
+struct ia_css_frame_sp_plane6 {
+	struct ia_css_frame_sp_plane r;
+	struct ia_css_frame_sp_plane r_at_b;
+	struct ia_css_frame_sp_plane gr;
+	struct ia_css_frame_sp_plane gb;
+	struct ia_css_frame_sp_plane b;
+	struct ia_css_frame_sp_plane b_at_r;
+};
+
+struct ia_css_sp_resolution {
+	u16 width;		/* width of valid data in pixels */
+	u16 height;	/* Height of valid data in lines */
+};
+
+/*
+ * Frame info struct. This describes the contents of an image frame buffer.
+ */
+struct ia_css_frame_sp_info {
+	struct ia_css_sp_resolution res;
+	u16 padded_width;		/* stride of line in memory
+					(in pixels) */
+	unsigned char format;		/* format of the frame data */
+	unsigned char raw_bit_depth;	/* number of valid bits per pixel,
+					only valid for RAW bayer frames */
+	unsigned char raw_bayer_order;	/* bayer order, only valid
+					for RAW bayer frames */
+	unsigned char padding[3];	/* Extend to 32 bit multiple */
+};
+
+struct ia_css_buffer_sp {
+	union {
+		ia_css_ptr xmem_addr;
+		enum sh_css_queue_id queue_id;
+	} buf_src;
+	enum ia_css_buffer_type buf_type;
+};
+
+struct ia_css_frame_sp {
+	struct ia_css_frame_sp_info info;
+	struct ia_css_buffer_sp buf_attr;
+	union {
+		struct ia_css_frame_sp_plane raw;
+		struct ia_css_frame_sp_plane rgb;
+		struct ia_css_frame_sp_rgb_planes planar_rgb;
+		struct ia_css_frame_sp_plane yuyv;
+		struct ia_css_frame_sp_yuv_planes yuv;
+		struct ia_css_frame_sp_nv_planes nv;
+		struct ia_css_frame_sp_plane6 plane6;
+		struct ia_css_frame_sp_binary_plane binary;
+	} planes;
+};
+
+void ia_css_frame_info_to_frame_sp_info(
+    struct ia_css_frame_sp_info *sp_info,
+    const struct ia_css_frame_info *info);
+
+void ia_css_resolution_to_sp_resolution(
+    struct ia_css_sp_resolution *sp_info,
+    const struct ia_css_resolution *info);
+
+#endif /*__IA_CSS_FRAME_COMM_H__*/
diff --git a/drivers/staging/media/atomisp/pci/runtime/frame/src/frame.c b/drivers/staging/media/atomisp/pci/runtime/frame/src/frame.c
new file mode 100644
index 000000000..5a7058320
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/frame/src/frame.c
@@ -0,0 +1,849 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_frame.h"
+#include <math_support.h>
+#include "assert_support.h"
+#include "ia_css_debug.h"
+#include "isp.h"
+#include "sh_css_internal.h"
+#include "atomisp_internal.h"
+
+#define NV12_TILEY_TILE_WIDTH  128
+#define NV12_TILEY_TILE_HEIGHT  32
+
+/**************************************************************************
+**	Static functions declarations
+**************************************************************************/
+static void frame_init_plane(struct ia_css_frame_plane *plane,
+			     unsigned int width,
+			     unsigned int stride,
+			     unsigned int height,
+			     unsigned int offset);
+
+static void frame_init_single_plane(struct ia_css_frame *frame,
+				    struct ia_css_frame_plane *plane,
+				    unsigned int height,
+				    unsigned int subpixels_per_line,
+				    unsigned int bytes_per_pixel);
+
+static void frame_init_raw_single_plane(
+    struct ia_css_frame *frame,
+    struct ia_css_frame_plane *plane,
+    unsigned int height,
+    unsigned int subpixels_per_line,
+    unsigned int bits_per_pixel);
+
+static void frame_init_nv_planes(struct ia_css_frame *frame,
+				 unsigned int horizontal_decimation,
+				 unsigned int vertical_decimation,
+				 unsigned int bytes_per_element);
+
+static void frame_init_yuv_planes(struct ia_css_frame *frame,
+				  unsigned int horizontal_decimation,
+				  unsigned int vertical_decimation,
+				  bool swap_uv,
+				  unsigned int bytes_per_element);
+
+static void frame_init_rgb_planes(struct ia_css_frame *frame,
+				  unsigned int bytes_per_element);
+
+static void frame_init_qplane6_planes(struct ia_css_frame *frame);
+
+static int frame_allocate_buffer_data(struct ia_css_frame *frame);
+
+static int frame_allocate_with_data(struct ia_css_frame **frame,
+	unsigned int width,
+	unsigned int height,
+	enum ia_css_frame_format format,
+	unsigned int padded_width,
+	unsigned int raw_bit_depth);
+
+static struct ia_css_frame *frame_create(unsigned int width,
+	unsigned int height,
+	enum ia_css_frame_format format,
+	unsigned int padded_width,
+	unsigned int raw_bit_depth,
+	bool valid);
+
+static unsigned
+ia_css_elems_bytes_from_info(
+    const struct ia_css_frame_info *info);
+
+/**************************************************************************
+**	CSS API functions, exposed by ia_css.h
+**************************************************************************/
+
+void ia_css_frame_zero(struct ia_css_frame *frame)
+{
+	assert(frame);
+	hmm_set(frame->data, 0, frame->data_bytes);
+}
+
+int ia_css_frame_allocate_from_info(struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info)
+{
+	int err = 0;
+
+	if (!frame || !info)
+		return -EINVAL;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_allocate_from_info() enter:\n");
+	err =
+	    ia_css_frame_allocate(frame, info->res.width, info->res.height,
+				  info->format, info->padded_width,
+				  info->raw_bit_depth);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_allocate_from_info() leave:\n");
+	return err;
+}
+
+int ia_css_frame_allocate(struct ia_css_frame **frame,
+				      unsigned int width,
+				      unsigned int height,
+				      enum ia_css_frame_format format,
+				      unsigned int padded_width,
+				      unsigned int raw_bit_depth)
+{
+	int err = 0;
+
+	if (!frame || width == 0 || height == 0)
+		return -EINVAL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_allocate() enter: width=%d, height=%d, format=%d, padded_width=%d, raw_bit_depth=%d\n",
+			    width, height, format, padded_width, raw_bit_depth);
+
+	err = frame_allocate_with_data(frame, width, height, format,
+				       padded_width, raw_bit_depth);
+
+	if ((*frame) && err == 0)
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_allocate() leave: frame=%p, data(DDR address)=0x%x\n", *frame,
+				    (*frame)->data);
+	else
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_allocate() leave: frame=%p, data(DDR address)=0x%x\n",
+				    (void *)-1, (unsigned int)-1);
+
+	return err;
+}
+
+int ia_css_frame_map(struct ia_css_frame **frame,
+				 const struct ia_css_frame_info *info,
+				 const void __user *data,
+				 unsigned int pgnr)
+{
+	int err = 0;
+	struct ia_css_frame *me;
+
+	assert(frame);
+
+	/* Create the frame structure */
+	err = ia_css_frame_create_from_info(&me, info);
+
+	if (err)
+		return err;
+
+	if (pgnr < ((PAGE_ALIGN(me->data_bytes)) >> PAGE_SHIFT)) {
+		dev_err(atomisp_dev,
+			"user space memory size is less than the expected size..\n");
+		err = -ENOMEM;
+		goto error;
+	} else if (pgnr > ((PAGE_ALIGN(me->data_bytes)) >> PAGE_SHIFT)) {
+		dev_err(atomisp_dev,
+			"user space memory size is large than the expected size..\n");
+		err = -ENOMEM;
+		goto error;
+	}
+
+	me->data = hmm_create_from_userdata(me->data_bytes, data);
+	if (me->data == mmgr_NULL)
+		err = -EINVAL;
+
+error:
+	if (err) {
+		kvfree(me);
+		me = NULL;
+	}
+
+	*frame = me;
+
+	return err;
+}
+
+int ia_css_frame_create_from_info(struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info)
+{
+	int err = 0;
+	struct ia_css_frame *me;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_create_from_info() enter:\n");
+	if (!frame || !info) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_create_from_info() leave: invalid arguments\n");
+		return -EINVAL;
+	}
+
+	me = frame_create(info->res.width,
+			  info->res.height,
+			  info->format,
+			  info->padded_width,
+			  info->raw_bit_depth,
+			  false);
+	if (!me) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_create_from_info() leave: frame create failed\n");
+		return -ENOMEM;
+	}
+
+	err = ia_css_frame_init_planes(me);
+
+	if (err) {
+		kvfree(me);
+		me = NULL;
+	}
+
+	*frame = me;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_create_from_info() leave:\n");
+
+	return err;
+}
+
+int ia_css_frame_set_data(struct ia_css_frame *frame,
+				      const ia_css_ptr mapped_data,
+				      size_t data_bytes)
+{
+	int err = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_set_data() enter:\n");
+	if (!frame) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_set_data() leave: NULL frame\n");
+		return -EINVAL;
+	}
+
+	/* If we are setting a valid data.
+	 * Make sure that there is enough
+	 * room for the expected frame format
+	 */
+	if ((mapped_data != mmgr_NULL) && (frame->data_bytes > data_bytes)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_frame_set_data() leave: invalid arguments\n");
+		return -EINVAL;
+	}
+
+	frame->data = mapped_data;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_frame_set_data() leave:\n");
+
+	return err;
+}
+
+void ia_css_frame_free(struct ia_css_frame *frame)
+{
+	IA_CSS_ENTER_PRIVATE("frame = %p", frame);
+
+	if (frame) {
+		hmm_free(frame->data);
+		kvfree(frame);
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/**************************************************************************
+**	Module public functions
+**************************************************************************/
+
+int ia_css_frame_check_info(const struct ia_css_frame_info *info)
+{
+	assert(info);
+	if (info->res.width == 0 || info->res.height == 0)
+		return -EINVAL;
+	return 0;
+}
+
+int ia_css_frame_init_planes(struct ia_css_frame *frame)
+{
+	assert(frame);
+
+	switch (frame->info.format) {
+	case IA_CSS_FRAME_FORMAT_MIPI:
+		dev_err(atomisp_dev,
+			"%s: unexpected use of IA_CSS_FRAME_FORMAT_MIPI\n", __func__);
+		return -EINVAL;
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+		frame_init_raw_single_plane(frame, &frame->planes.raw,
+					    frame->info.res.height,
+					    frame->info.padded_width,
+					    frame->info.raw_bit_depth);
+		break;
+	case IA_CSS_FRAME_FORMAT_RAW:
+		frame_init_single_plane(frame, &frame->planes.raw,
+					frame->info.res.height,
+					frame->info.padded_width,
+					frame->info.raw_bit_depth <= 8 ? 1 : 2);
+		break;
+	case IA_CSS_FRAME_FORMAT_RGB565:
+		frame_init_single_plane(frame, &frame->planes.rgb,
+					frame->info.res.height,
+					frame->info.padded_width, 2);
+		break;
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+		frame_init_single_plane(frame, &frame->planes.rgb,
+					frame->info.res.height,
+					frame->info.padded_width * 4, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
+		frame_init_rgb_planes(frame, 1);
+		break;
+	/* yuyv and uyvu have the same frame layout, only the data
+	 * positioning differs.
+	 */
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+		frame_init_single_plane(frame, &frame->planes.yuyv,
+					frame->info.res.height,
+					frame->info.padded_width * 2, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+		/* Needs 3 extra lines to allow vf_pp prefetching */
+		frame_init_single_plane(frame, &frame->planes.yuyv,
+					frame->info.res.height * 3 / 2 + 3,
+					frame->info.padded_width, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_NV11:
+		frame_init_nv_planes(frame, 4, 1, 1);
+		break;
+	/* nv12 and nv21 have the same frame layout, only the data
+	 * positioning differs.
+	 */
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+		frame_init_nv_planes(frame, 2, 2, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+		frame_init_nv_planes(frame, 2, 2, 2);
+		break;
+	/* nv16 and nv61 have the same frame layout, only the data
+	 * positioning differs.
+	 */
+	case IA_CSS_FRAME_FORMAT_NV16:
+	case IA_CSS_FRAME_FORMAT_NV61:
+		frame_init_nv_planes(frame, 2, 1, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV420:
+		frame_init_yuv_planes(frame, 2, 2, false, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV422:
+		frame_init_yuv_planes(frame, 2, 1, false, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV444:
+		frame_init_yuv_planes(frame, 1, 1, false, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+		frame_init_yuv_planes(frame, 2, 2, false, 2);
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+		frame_init_yuv_planes(frame, 2, 1, false, 2);
+		break;
+	case IA_CSS_FRAME_FORMAT_YV12:
+		frame_init_yuv_planes(frame, 2, 2, true, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_YV16:
+		frame_init_yuv_planes(frame, 2, 1, true, 1);
+		break;
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+		frame_init_qplane6_planes(frame);
+		break;
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+		frame_init_single_plane(frame, &frame->planes.binary.data,
+					frame->info.res.height,
+					frame->info.padded_width, 1);
+		frame->planes.binary.size = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+void ia_css_frame_info_set_width(struct ia_css_frame_info *info,
+				 unsigned int width,
+				 unsigned int min_padded_width)
+{
+	unsigned int align;
+
+	IA_CSS_ENTER_PRIVATE("info = %p,width = %d, minimum padded width = %d",
+			     info, width, min_padded_width);
+	if (!info) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	align = max(min_padded_width, width);
+
+	info->res.width = width;
+	/* frames with a U and V plane of 8 bits per pixel need to have
+	   all planes aligned, this means double the alignment for the
+	   Y plane if the horizontal decimation is 2. */
+	if (info->format == IA_CSS_FRAME_FORMAT_YUV420 ||
+	    info->format == IA_CSS_FRAME_FORMAT_YV12 ||
+	    info->format == IA_CSS_FRAME_FORMAT_NV12 ||
+	    info->format == IA_CSS_FRAME_FORMAT_NV21 ||
+	    info->format == IA_CSS_FRAME_FORMAT_BINARY_8 ||
+	    info->format == IA_CSS_FRAME_FORMAT_YUV_LINE)
+		info->padded_width =
+		    CEIL_MUL(align, 2 * HIVE_ISP_DDR_WORD_BYTES);
+	else if (info->format == IA_CSS_FRAME_FORMAT_NV12_TILEY)
+		info->padded_width = CEIL_MUL(align, NV12_TILEY_TILE_WIDTH);
+	else if (info->format == IA_CSS_FRAME_FORMAT_RAW ||
+		 info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED)
+		info->padded_width = CEIL_MUL(align, 2 * ISP_VEC_NELEMS);
+	else {
+		info->padded_width = CEIL_MUL(align, HIVE_ISP_DDR_WORD_BYTES);
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_frame_info_set_format(struct ia_css_frame_info *info,
+				  enum ia_css_frame_format format)
+{
+	assert(info);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_info_set_format() enter:\n");
+	info->format = format;
+}
+
+void ia_css_frame_info_init(struct ia_css_frame_info *info,
+			    unsigned int width,
+			    unsigned int height,
+			    enum ia_css_frame_format format,
+			    unsigned int aligned)
+{
+	IA_CSS_ENTER_PRIVATE("info = %p, width = %d, height = %d, format = %d, aligned = %d",
+			     info, width, height, format, aligned);
+	if (!info) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	info->res.height = height;
+	info->format     = format;
+	ia_css_frame_info_set_width(info, width, aligned);
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+void ia_css_frame_free_multiple(unsigned int num_frames,
+				struct ia_css_frame **frames_array)
+{
+	unsigned int i;
+
+	for (i = 0; i < num_frames; i++) {
+		if (frames_array[i]) {
+			ia_css_frame_free(frames_array[i]);
+			frames_array[i] = NULL;
+		}
+	}
+}
+
+int ia_css_frame_allocate_with_buffer_size(struct ia_css_frame **frame,
+					   const unsigned int buffer_size_bytes)
+{
+	/* AM: Body coppied from frame_allocate_with_data(). */
+	int err;
+	struct ia_css_frame *me = frame_create(0, 0,
+					       IA_CSS_FRAME_FORMAT_NUM,/* Not valid format yet */
+					       0, 0, false);
+
+	if (!me)
+		return -ENOMEM;
+
+	/* Get the data size */
+	me->data_bytes = buffer_size_bytes;
+
+	err = frame_allocate_buffer_data(me);
+
+	if (err) {
+		kvfree(me);
+		me = NULL;
+	}
+
+	*frame = me;
+
+	return err;
+}
+
+bool ia_css_frame_info_is_same_resolution(
+    const struct ia_css_frame_info *info_a,
+    const struct ia_css_frame_info *info_b)
+{
+	if (!info_a || !info_b)
+		return false;
+	return (info_a->res.width == info_b->res.width) &&
+	       (info_a->res.height == info_b->res.height);
+}
+
+bool ia_css_frame_is_same_type(const struct ia_css_frame *frame_a,
+			       const struct ia_css_frame *frame_b)
+{
+	bool is_equal = false;
+	const struct ia_css_frame_info *info_a = &frame_a->info,
+						*info_b = &frame_b->info;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_is_same_type() enter:\n");
+
+	if (!info_a || !info_b)
+		return false;
+	if (info_a->format != info_b->format)
+		return false;
+	if (info_a->padded_width != info_b->padded_width)
+		return false;
+	is_equal = ia_css_frame_info_is_same_resolution(info_a, info_b);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_frame_is_same_type() leave:\n");
+
+	return is_equal;
+}
+
+int ia_css_dma_configure_from_info(struct dma_port_config *config,
+				   const struct ia_css_frame_info *info)
+{
+	unsigned int is_raw_packed = info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED;
+	unsigned int bits_per_pixel = is_raw_packed ? info->raw_bit_depth :
+				      ia_css_elems_bytes_from_info(info) * 8;
+	unsigned int pix_per_ddrword = HIVE_ISP_DDR_WORD_BITS / bits_per_pixel;
+	unsigned int words_per_line = CEIL_DIV(info->padded_width, pix_per_ddrword);
+	unsigned int elems_b = pix_per_ddrword;
+
+	config->stride = HIVE_ISP_DDR_WORD_BYTES * words_per_line;
+	config->elems  = (uint8_t)elems_b;
+	config->width  = (uint16_t)info->res.width;
+	config->crop   = 0;
+
+	if (config->width > info->padded_width) {
+		dev_err(atomisp_dev, "internal error: padded_width is too small!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**************************************************************************
+**	Static functions
+**************************************************************************/
+
+static void frame_init_plane(struct ia_css_frame_plane *plane,
+			     unsigned int width,
+			     unsigned int stride,
+			     unsigned int height,
+			     unsigned int offset)
+{
+	plane->height = height;
+	plane->width = width;
+	plane->stride = stride;
+	plane->offset = offset;
+}
+
+static void frame_init_single_plane(struct ia_css_frame *frame,
+				    struct ia_css_frame_plane *plane,
+				    unsigned int height,
+				    unsigned int subpixels_per_line,
+				    unsigned int bytes_per_pixel)
+{
+	unsigned int stride;
+
+	stride = subpixels_per_line * bytes_per_pixel;
+	/* Frame height needs to be even number - needed by hw ISYS2401
+	   In case of odd number, round up to even.
+	   Images won't be impacted by this round up,
+	   only needed by jpeg/embedded data.
+	   As long as buffer allocation and release are using data_bytes,
+	   there won't be memory leak. */
+	frame->data_bytes = stride * CEIL_MUL2(height, 2);
+	frame_init_plane(plane, subpixels_per_line, stride, height, 0);
+	return;
+}
+
+static void frame_init_raw_single_plane(
+    struct ia_css_frame *frame,
+    struct ia_css_frame_plane *plane,
+    unsigned int height,
+    unsigned int subpixels_per_line,
+    unsigned int bits_per_pixel)
+{
+	unsigned int stride;
+
+	assert(frame);
+
+	stride = HIVE_ISP_DDR_WORD_BYTES *
+		 CEIL_DIV(subpixels_per_line,
+			  HIVE_ISP_DDR_WORD_BITS / bits_per_pixel);
+	frame->data_bytes = stride * height;
+	frame_init_plane(plane, subpixels_per_line, stride, height, 0);
+	return;
+}
+
+static void frame_init_nv_planes(struct ia_css_frame *frame,
+				 unsigned int horizontal_decimation,
+				 unsigned int vertical_decimation,
+				 unsigned int bytes_per_element)
+{
+	unsigned int y_width = frame->info.padded_width;
+	unsigned int y_height = frame->info.res.height;
+	unsigned int uv_width;
+	unsigned int uv_height;
+	unsigned int y_bytes;
+	unsigned int uv_bytes;
+	unsigned int y_stride;
+	unsigned int uv_stride;
+
+	assert(horizontal_decimation != 0 && vertical_decimation != 0);
+
+	uv_width = 2 * (y_width / horizontal_decimation);
+	uv_height = y_height / vertical_decimation;
+
+	if (frame->info.format == IA_CSS_FRAME_FORMAT_NV12_TILEY) {
+		y_width   = CEIL_MUL(y_width,   NV12_TILEY_TILE_WIDTH);
+		uv_width  = CEIL_MUL(uv_width,  NV12_TILEY_TILE_WIDTH);
+		y_height  = CEIL_MUL(y_height,  NV12_TILEY_TILE_HEIGHT);
+		uv_height = CEIL_MUL(uv_height, NV12_TILEY_TILE_HEIGHT);
+	}
+
+	y_stride = y_width * bytes_per_element;
+	uv_stride = uv_width * bytes_per_element;
+	y_bytes = y_stride * y_height;
+	uv_bytes = uv_stride * uv_height;
+
+	frame->data_bytes = y_bytes + uv_bytes;
+	frame_init_plane(&frame->planes.nv.y, y_width, y_stride, y_height, 0);
+	frame_init_plane(&frame->planes.nv.uv, uv_width,
+			 uv_stride, uv_height, y_bytes);
+	return;
+}
+
+static void frame_init_yuv_planes(struct ia_css_frame *frame,
+				  unsigned int horizontal_decimation,
+				  unsigned int vertical_decimation,
+				  bool swap_uv,
+				  unsigned int bytes_per_element)
+{
+	unsigned int y_width = frame->info.padded_width,
+		     y_height = frame->info.res.height,
+		     uv_width = y_width / horizontal_decimation,
+		     uv_height = y_height / vertical_decimation,
+		     y_stride, y_bytes, uv_bytes, uv_stride;
+
+	y_stride = y_width * bytes_per_element;
+	uv_stride = uv_width * bytes_per_element;
+	y_bytes = y_stride * y_height;
+	uv_bytes = uv_stride * uv_height;
+
+	frame->data_bytes = y_bytes + 2 * uv_bytes;
+	frame_init_plane(&frame->planes.yuv.y, y_width, y_stride, y_height, 0);
+	if (swap_uv) {
+		frame_init_plane(&frame->planes.yuv.v, uv_width, uv_stride,
+				 uv_height, y_bytes);
+		frame_init_plane(&frame->planes.yuv.u, uv_width, uv_stride,
+				 uv_height, y_bytes + uv_bytes);
+	} else {
+		frame_init_plane(&frame->planes.yuv.u, uv_width, uv_stride,
+				 uv_height, y_bytes);
+		frame_init_plane(&frame->planes.yuv.v, uv_width, uv_stride,
+				 uv_height, y_bytes + uv_bytes);
+	}
+	return;
+}
+
+static void frame_init_rgb_planes(struct ia_css_frame *frame,
+				  unsigned int bytes_per_element)
+{
+	unsigned int width = frame->info.res.width,
+		     height = frame->info.res.height, stride, bytes;
+
+	stride = width * bytes_per_element;
+	bytes = stride * height;
+	frame->data_bytes = 3 * bytes;
+	frame_init_plane(&frame->planes.planar_rgb.r, width, stride, height, 0);
+	frame_init_plane(&frame->planes.planar_rgb.g,
+			 width, stride, height, 1 * bytes);
+	frame_init_plane(&frame->planes.planar_rgb.b,
+			 width, stride, height, 2 * bytes);
+	return;
+}
+
+static void frame_init_qplane6_planes(struct ia_css_frame *frame)
+{
+	unsigned int width = frame->info.padded_width / 2,
+		     height = frame->info.res.height / 2, bytes, stride;
+
+	stride = width * 2;
+	bytes = stride * height;
+
+	frame->data_bytes = 6 * bytes;
+	frame_init_plane(&frame->planes.plane6.r,
+			 width, stride, height, 0 * bytes);
+	frame_init_plane(&frame->planes.plane6.r_at_b,
+			 width, stride, height, 1 * bytes);
+	frame_init_plane(&frame->planes.plane6.gr,
+			 width, stride, height, 2 * bytes);
+	frame_init_plane(&frame->planes.plane6.gb,
+			 width, stride, height, 3 * bytes);
+	frame_init_plane(&frame->planes.plane6.b,
+			 width, stride, height, 4 * bytes);
+	frame_init_plane(&frame->planes.plane6.b_at_r,
+			 width, stride, height, 5 * bytes);
+	return;
+}
+
+static int frame_allocate_buffer_data(struct ia_css_frame *frame)
+{
+#ifdef ISP2401
+	IA_CSS_ENTER_LEAVE_PRIVATE("frame->data_bytes=%d\n", frame->data_bytes);
+#endif
+	frame->data = hmm_alloc(frame->data_bytes);
+	if (frame->data == mmgr_NULL)
+		return -ENOMEM;
+	return 0;
+}
+
+static int frame_allocate_with_data(struct ia_css_frame **frame,
+	unsigned int width,
+	unsigned int height,
+	enum ia_css_frame_format format,
+	unsigned int padded_width,
+	unsigned int raw_bit_depth)
+{
+	int err;
+	struct ia_css_frame *me = frame_create(width,
+					       height,
+					       format,
+					       padded_width,
+					       raw_bit_depth,
+					       true);
+
+	if (!me)
+		return -ENOMEM;
+
+	err = ia_css_frame_init_planes(me);
+
+	if (!err)
+		err = frame_allocate_buffer_data(me);
+
+	if (err) {
+		kvfree(me);
+#ifndef ISP2401
+		return err;
+#else
+		me = NULL;
+#endif
+	}
+
+	*frame = me;
+
+	return err;
+}
+
+static struct ia_css_frame *frame_create(unsigned int width,
+	unsigned int height,
+	enum ia_css_frame_format format,
+	unsigned int padded_width,
+	unsigned int raw_bit_depth,
+	bool valid)
+{
+	struct ia_css_frame *me = kvmalloc(sizeof(*me), GFP_KERNEL);
+
+	if (!me)
+		return NULL;
+
+	memset(me, 0, sizeof(*me));
+	me->info.res.width = width;
+	me->info.res.height = height;
+	me->info.format = format;
+	me->info.padded_width = padded_width;
+	me->info.raw_bit_depth = raw_bit_depth;
+	me->valid = valid;
+	me->data_bytes = 0;
+	me->data = mmgr_NULL;
+	/* To indicate it is not valid frame. */
+	me->dynamic_queue_id = (int)SH_CSS_INVALID_QUEUE_ID;
+	me->buf_type = IA_CSS_BUFFER_TYPE_INVALID;
+
+	return me;
+}
+
+static unsigned
+ia_css_elems_bytes_from_info(const struct ia_css_frame_info *info)
+{
+	if (info->format == IA_CSS_FRAME_FORMAT_RGB565)
+		return 2; /* bytes per pixel */
+	if (info->format == IA_CSS_FRAME_FORMAT_YUV420_16)
+		return 2; /* bytes per pixel */
+	if (info->format == IA_CSS_FRAME_FORMAT_YUV422_16)
+		return 2; /* bytes per pixel */
+	/* Note: Essentially NV12_16 is a 2 bytes per pixel format, this return value is used
+	 * to configure DMA for the output buffer,
+	 * At least in SKC this data is overwritten by isp_output_init.sp.c except for elements(elems),
+	 * which is configured from this return value,
+	 * NV12_16 is implemented by a double buffer of 8 bit elements hence elems should be configured as 8 */
+	if (info->format == IA_CSS_FRAME_FORMAT_NV12_16)
+		return 1; /* bytes per pixel */
+
+	if (info->format == IA_CSS_FRAME_FORMAT_RAW
+	    || (info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED)) {
+		if (info->raw_bit_depth)
+			return CEIL_DIV(info->raw_bit_depth, 8);
+		else
+			return 2; /* bytes per pixel */
+	}
+	if (info->format == IA_CSS_FRAME_FORMAT_PLANAR_RGB888)
+		return 3; /* bytes per pixel */
+	if (info->format == IA_CSS_FRAME_FORMAT_RGBA888)
+		return 4; /* bytes per pixel */
+	if (info->format == IA_CSS_FRAME_FORMAT_QPLANE6)
+		return 2; /* bytes per pixel */
+	return 1; /* Default is 1 byte per pixel */
+}
+
+void ia_css_frame_info_to_frame_sp_info(
+    struct ia_css_frame_sp_info *to,
+    const struct ia_css_frame_info *from)
+{
+	ia_css_resolution_to_sp_resolution(&to->res, &from->res);
+	to->padded_width = (uint16_t)from->padded_width;
+	to->format = (uint8_t)from->format;
+	to->raw_bit_depth = (uint8_t)from->raw_bit_depth;
+	to->raw_bayer_order = from->raw_bayer_order;
+}
+
+void ia_css_resolution_to_sp_resolution(
+    struct ia_css_sp_resolution *to,
+    const struct ia_css_resolution *from)
+{
+	to->width  = (uint16_t)from->width;
+	to->height = (uint16_t)from->height;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/ifmtr/interface/ia_css_ifmtr.h b/drivers/staging/media/atomisp/pci/runtime/ifmtr/interface/ia_css_ifmtr.h
new file mode 100644
index 000000000..2c440feec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/ifmtr/interface/ia_css_ifmtr.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_IFMTR_H__
+#define __IA_CSS_IFMTR_H__
+
+#include <type_support.h>
+#include <ia_css_stream_public.h>
+#include <ia_css_binary.h>
+
+extern bool ifmtr_set_if_blocking_mode_reset;
+
+unsigned int ia_css_ifmtr_lines_needed_for_bayer_order(
+    const struct ia_css_stream_config *config);
+
+unsigned int ia_css_ifmtr_columns_needed_for_bayer_order(
+    const struct ia_css_stream_config *config);
+
+int ia_css_ifmtr_configure(struct ia_css_stream_config *config,
+				       struct ia_css_binary *binary);
+
+#endif /* __IA_CSS_IFMTR_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/ifmtr/src/ifmtr.c b/drivers/staging/media/atomisp/pci/runtime/ifmtr/src/ifmtr.c
new file mode 100644
index 000000000..6d9f47629
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/ifmtr/src/ifmtr.c
@@ -0,0 +1,553 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+#include <linux/kernel.h>
+
+#ifndef ISP2401
+
+#include "ia_css_ifmtr.h"
+#include <math_support.h>
+#include "sh_css_internal.h"
+#include "input_formatter.h"
+#include "assert_support.h"
+#include "sh_css_sp.h"
+#include "isp/modes/interface/input_buf.isp.h"
+
+/************************************************************
+ * Static functions declarations
+ ************************************************************/
+static int ifmtr_start_column(
+    const struct ia_css_stream_config *config,
+    unsigned int bin_in,
+    unsigned int *start_column);
+
+static int ifmtr_input_start_line(
+    const struct ia_css_stream_config *config,
+    unsigned int bin_in,
+    unsigned int *start_line);
+
+static void ifmtr_set_if_blocking_mode(
+    const input_formatter_cfg_t *const config_a,
+    const input_formatter_cfg_t *const config_b);
+
+/************************************************************
+ * Public functions
+ ************************************************************/
+
+/* ISP expects GRBG bayer order, we skip one line and/or one row
+ * to correct in case the input bayer order is different.
+ */
+unsigned int ia_css_ifmtr_lines_needed_for_bayer_order(
+    const struct ia_css_stream_config *config)
+{
+	assert(config);
+	if ((config->input_config.bayer_order == IA_CSS_BAYER_ORDER_BGGR)
+	    || (config->input_config.bayer_order == IA_CSS_BAYER_ORDER_GBRG))
+		return 1;
+
+	return 0;
+}
+
+unsigned int ia_css_ifmtr_columns_needed_for_bayer_order(
+    const struct ia_css_stream_config *config)
+{
+	assert(config);
+	if ((config->input_config.bayer_order == IA_CSS_BAYER_ORDER_RGGB)
+	    || (config->input_config.bayer_order == IA_CSS_BAYER_ORDER_GBRG))
+		return 1;
+
+	return 0;
+}
+
+int ia_css_ifmtr_configure(struct ia_css_stream_config *config,
+				       struct ia_css_binary *binary)
+{
+	unsigned int start_line, start_column = 0,
+				 cropped_height,
+				 cropped_width,
+				 num_vectors,
+				 buffer_height = 2,
+				 buffer_width,
+				 two_ppc,
+				 vmem_increment = 0,
+				 deinterleaving = 0,
+				 deinterleaving_b = 0,
+				 width_a = 0,
+				 width_b = 0,
+				 bits_per_pixel,
+				 vectors_per_buffer,
+				 vectors_per_line = 0,
+				 buffers_per_line = 0,
+				 buf_offset_a = 0,
+				 buf_offset_b = 0,
+				 line_width = 0,
+				 width_b_factor = 1, start_column_b,
+				 left_padding = 0;
+	input_formatter_cfg_t if_a_config, if_b_config;
+	enum atomisp_input_format input_format;
+	int err = 0;
+	u8 if_config_index;
+
+	/* Determine which input formatter config set is targeted. */
+	/* Index is equal to the CSI-2 port used. */
+	enum mipi_port_id port;
+
+	if (binary) {
+		cropped_height = binary->in_frame_info.res.height;
+		cropped_width = binary->in_frame_info.res.width;
+		/* This should correspond to the input buffer definition for
+		ISP binaries in input_buf.isp.h */
+		if (binary->info->sp.enable.continuous &&
+		    binary->info->sp.pipeline.mode != IA_CSS_BINARY_MODE_COPY)
+			buffer_width = MAX_VECTORS_PER_INPUT_LINE_CONT * ISP_VEC_NELEMS;
+		else
+			buffer_width = binary->info->sp.input.max_width;
+		input_format = binary->input_format;
+	} else {
+		/* sp raw copy pipe (IA_CSS_PIPE_MODE_COPY): binary is NULL */
+		cropped_height = config->input_config.input_res.height;
+		cropped_width = config->input_config.input_res.width;
+		buffer_width = MAX_VECTORS_PER_INPUT_LINE_CONT * ISP_VEC_NELEMS;
+		input_format = config->input_config.format;
+	}
+	two_ppc = config->pixels_per_clock == 2;
+	if (config->mode == IA_CSS_INPUT_MODE_SENSOR
+	    || config->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		port = config->source.port.port;
+		if_config_index = (uint8_t)(port - MIPI_PORT0_ID);
+	} else if (config->mode == IA_CSS_INPUT_MODE_MEMORY) {
+		if_config_index = SH_CSS_IF_CONFIG_NOT_NEEDED;
+	} else {
+		if_config_index = 0;
+	}
+
+	assert(if_config_index <= SH_CSS_MAX_IF_CONFIGS
+	       || if_config_index == SH_CSS_IF_CONFIG_NOT_NEEDED);
+
+	/* TODO: check to see if input is RAW and if current mode interprets
+	 * RAW data in any particular bayer order. copy binary with output
+	 * format other than raw should not result in dropping lines and/or
+	 * columns.
+	 */
+	err = ifmtr_input_start_line(config, cropped_height, &start_line);
+	if (err)
+		return err;
+	err = ifmtr_start_column(config, cropped_width, &start_column);
+	if (err)
+		return err;
+
+	if (config->left_padding == -1)
+		if (!binary)
+			/* sp raw copy pipe: set left_padding value */
+			left_padding = 0;
+		else
+			left_padding = binary->left_padding;
+	else
+		left_padding = 2 * ISP_VEC_NELEMS - config->left_padding;
+
+	if (left_padding) {
+		num_vectors = CEIL_DIV(cropped_width + left_padding,
+				       ISP_VEC_NELEMS);
+	} else {
+		num_vectors = CEIL_DIV(cropped_width, ISP_VEC_NELEMS);
+		num_vectors *= buffer_height;
+		/* todo: in case of left padding,
+		   num_vectors is vectors per line,
+		   otherwise vectors per line * buffer_height. */
+	}
+
+	start_column_b = start_column;
+
+	bits_per_pixel = input_formatter_get_alignment(INPUT_FORMATTER0_ID)
+			 * 8 / ISP_VEC_NELEMS;
+	switch (input_format) {
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+		if (two_ppc) {
+			vmem_increment = 1;
+			deinterleaving = 1;
+			deinterleaving_b = 1;
+			/* half lines */
+			width_a = cropped_width * deinterleaving / 2;
+			width_b_factor = 2;
+			/* full lines */
+			width_b = width_a * width_b_factor;
+			buffer_width *= deinterleaving * 2;
+			/* Patch from bayer to yuv */
+			num_vectors *= deinterleaving;
+			buf_offset_b = buffer_width / 2 / ISP_VEC_NELEMS;
+			vectors_per_line = num_vectors / buffer_height;
+			/* Even lines are half size */
+			line_width = vectors_per_line *
+				     input_formatter_get_alignment(INPUT_FORMATTER0_ID) /
+				     2;
+			start_column /= 2;
+		} else {
+			vmem_increment = 1;
+			deinterleaving = 3;
+			width_a = cropped_width * deinterleaving / 2;
+			buffer_width = buffer_width * deinterleaving / 2;
+			/* Patch from bayer to yuv */
+			num_vectors = num_vectors / 2 * deinterleaving;
+			start_column = start_column * deinterleaving / 2;
+		}
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+		if (two_ppc) {
+			vmem_increment = 1;
+			deinterleaving = 1;
+			width_a = width_b = cropped_width * deinterleaving / 2;
+			buffer_width *= deinterleaving * 2;
+			num_vectors *= deinterleaving;
+			buf_offset_b = buffer_width / 2 / ISP_VEC_NELEMS;
+			vectors_per_line = num_vectors / buffer_height;
+			/* Even lines are half size */
+			line_width = vectors_per_line *
+				     input_formatter_get_alignment(INPUT_FORMATTER0_ID) /
+				     2;
+			start_column *= deinterleaving;
+			start_column /= 2;
+			start_column_b = start_column;
+		} else {
+			vmem_increment = 1;
+			deinterleaving = 1;
+			width_a = cropped_width * deinterleaving;
+			buffer_width *= deinterleaving * 2;
+			num_vectors *= deinterleaving;
+			start_column *= deinterleaving;
+		}
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		if (two_ppc) {
+			vmem_increment = 1;
+			deinterleaving = 1;
+			width_a = width_b = cropped_width * deinterleaving;
+			buffer_width *= deinterleaving * 2;
+			num_vectors *= deinterleaving;
+			start_column *= deinterleaving;
+			buf_offset_b = buffer_width / 2 / ISP_VEC_NELEMS;
+			start_column_b = start_column;
+		} else {
+			vmem_increment = 1;
+			deinterleaving = 2;
+			width_a = cropped_width * deinterleaving;
+			buffer_width *= deinterleaving;
+			num_vectors *= deinterleaving;
+			start_column *= deinterleaving;
+		}
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		num_vectors *= 2;
+		if (two_ppc) {
+			deinterleaving = 2;	/* BR in if_a, G in if_b */
+			deinterleaving_b = 1;	/* BR in if_a, G in if_b */
+			buffers_per_line = 4;
+			start_column_b = start_column;
+			start_column *= deinterleaving;
+			start_column_b *= deinterleaving_b;
+		} else {
+			deinterleaving = 3;	/* BGR */
+			buffers_per_line = 3;
+			start_column *= deinterleaving;
+		}
+		vmem_increment = 1;
+		width_a = cropped_width * deinterleaving;
+		width_b = cropped_width * deinterleaving_b;
+		buffer_width *= buffers_per_line;
+		/* Patch from bayer to rgb */
+		num_vectors = num_vectors / 2 * deinterleaving;
+		buf_offset_b = buffer_width / 2 / ISP_VEC_NELEMS;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+		if (two_ppc) {
+			int crop_col = (start_column % 2) == 1;
+
+			vmem_increment = 2;
+			deinterleaving = 1;
+			width_a = width_b = cropped_width / 2;
+
+			/* When two_ppc is enabled AND we need to crop one extra
+			 * column, if_a crops by one extra and we swap the
+			 * output offsets to interleave the bayer pattern in
+			 * the correct order.
+			 */
+			buf_offset_a   = crop_col ? 1 : 0;
+			buf_offset_b   = crop_col ? 0 : 1;
+			start_column_b = start_column / 2;
+			start_column   = start_column / 2 + crop_col;
+		} else {
+			vmem_increment = 1;
+			deinterleaving = 2;
+			if ((!binary) || (config->continuous && binary
+					  && binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_COPY)) {
+				/* !binary -> sp raw copy pipe, no deinterleaving */
+				deinterleaving = 1;
+			}
+			width_a = cropped_width;
+			/* Must be multiple of deinterleaving */
+			num_vectors = CEIL_MUL(num_vectors, deinterleaving);
+		}
+		buffer_height *= 2;
+		if ((!binary) || config->continuous)
+			/* !binary -> sp raw copy pipe */
+			buffer_height *= 2;
+		vectors_per_line = CEIL_DIV(cropped_width, ISP_VEC_NELEMS);
+		vectors_per_line = CEIL_MUL(vectors_per_line, deinterleaving);
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		if (two_ppc) {
+			num_vectors *= 2;
+			vmem_increment = 1;
+			deinterleaving = 2;
+			width_a = width_b = cropped_width;
+			/* B buffer is one line further */
+			buf_offset_b = buffer_width / ISP_VEC_NELEMS;
+			bits_per_pixel *= 2;
+		} else {
+			vmem_increment = 1;
+			deinterleaving = 2;
+			width_a = cropped_width;
+			start_column /= deinterleaving;
+		}
+		buffer_height *= 2;
+		break;
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT1:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT2:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT3:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT4:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT5:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT6:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT7:
+	case ATOMISP_INPUT_FORMAT_GENERIC_SHORT8:
+	case ATOMISP_INPUT_FORMAT_YUV420_8_SHIFT:
+	case ATOMISP_INPUT_FORMAT_YUV420_10_SHIFT:
+	case ATOMISP_INPUT_FORMAT_EMBEDDED:
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		break;
+	}
+	if (width_a == 0)
+		return -EINVAL;
+
+	if (two_ppc)
+		left_padding /= 2;
+
+	/* Default values */
+	if (left_padding)
+		vectors_per_line = num_vectors;
+	if (!vectors_per_line) {
+		vectors_per_line = CEIL_MUL(num_vectors / buffer_height,
+					    deinterleaving);
+		line_width = 0;
+	}
+	if (!line_width)
+		line_width = vectors_per_line *
+			     input_formatter_get_alignment(INPUT_FORMATTER0_ID);
+	if (!buffers_per_line)
+		buffers_per_line = deinterleaving;
+	line_width = CEIL_MUL(line_width,
+			      input_formatter_get_alignment(INPUT_FORMATTER0_ID)
+			      * vmem_increment);
+
+	vectors_per_buffer = buffer_height * buffer_width / ISP_VEC_NELEMS;
+
+	if (config->mode == IA_CSS_INPUT_MODE_TPG &&
+	    ((binary && binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_VIDEO) ||
+	     (!binary))) {
+		/* !binary -> sp raw copy pipe */
+		/* workaround for TPG in video mode */
+		start_line = 0;
+		start_column = 0;
+		cropped_height -= start_line;
+		width_a -= start_column;
+	}
+
+	if_a_config.start_line = start_line;
+	if_a_config.start_column = start_column;
+	if_a_config.left_padding = left_padding / deinterleaving;
+	if_a_config.cropped_height = cropped_height;
+	if_a_config.cropped_width = width_a;
+	if_a_config.deinterleaving = deinterleaving;
+	if_a_config.buf_vecs = vectors_per_buffer;
+	if_a_config.buf_start_index = buf_offset_a;
+	if_a_config.buf_increment = vmem_increment;
+	if_a_config.buf_eol_offset =
+	    buffer_width * bits_per_pixel / 8 - line_width;
+	if_a_config.is_yuv420_format =
+	    (input_format == ATOMISP_INPUT_FORMAT_YUV420_8)
+	    || (input_format == ATOMISP_INPUT_FORMAT_YUV420_10)
+	    || (input_format == ATOMISP_INPUT_FORMAT_YUV420_16);
+	if_a_config.block_no_reqs = (config->mode != IA_CSS_INPUT_MODE_SENSOR);
+
+	if (two_ppc) {
+		if (deinterleaving_b) {
+			deinterleaving = deinterleaving_b;
+			width_b = cropped_width * deinterleaving;
+			buffer_width *= deinterleaving;
+			/* Patch from bayer to rgb */
+			num_vectors = num_vectors / 2 *
+				      deinterleaving * width_b_factor;
+			vectors_per_line = num_vectors / buffer_height;
+			line_width = vectors_per_line *
+				     input_formatter_get_alignment(INPUT_FORMATTER0_ID);
+		}
+		if_b_config.start_line = start_line;
+		if_b_config.start_column = start_column_b;
+		if_b_config.left_padding = left_padding / deinterleaving;
+		if_b_config.cropped_height = cropped_height;
+		if_b_config.cropped_width = width_b;
+		if_b_config.deinterleaving = deinterleaving;
+		if_b_config.buf_vecs = vectors_per_buffer;
+		if_b_config.buf_start_index = buf_offset_b;
+		if_b_config.buf_increment = vmem_increment;
+		if_b_config.buf_eol_offset =
+		    buffer_width * bits_per_pixel / 8 - line_width;
+		if_b_config.is_yuv420_format =
+		    input_format == ATOMISP_INPUT_FORMAT_YUV420_8
+		    || input_format == ATOMISP_INPUT_FORMAT_YUV420_10
+		    || input_format == ATOMISP_INPUT_FORMAT_YUV420_16;
+		if_b_config.block_no_reqs =
+		    (config->mode != IA_CSS_INPUT_MODE_SENSOR);
+
+		if (if_config_index != SH_CSS_IF_CONFIG_NOT_NEEDED) {
+			assert(if_config_index <= SH_CSS_MAX_IF_CONFIGS);
+
+			ifmtr_set_if_blocking_mode(&if_a_config, &if_b_config);
+			/* Set the ifconfigs to SP group */
+			sh_css_sp_set_if_configs(&if_a_config, &if_b_config,
+						 if_config_index);
+		}
+	} else {
+		if (if_config_index != SH_CSS_IF_CONFIG_NOT_NEEDED) {
+			assert(if_config_index <= SH_CSS_MAX_IF_CONFIGS);
+
+			ifmtr_set_if_blocking_mode(&if_a_config, NULL);
+			/* Set the ifconfigs to SP group */
+			sh_css_sp_set_if_configs(&if_a_config, NULL,
+						 if_config_index);
+		}
+	}
+
+	return 0;
+}
+
+bool ifmtr_set_if_blocking_mode_reset = true;
+
+/************************************************************
+ * Static functions
+ ************************************************************/
+static void ifmtr_set_if_blocking_mode(
+    const input_formatter_cfg_t *const config_a,
+    const input_formatter_cfg_t *const config_b)
+{
+	int i;
+	bool block[] = { false, false, false, false };
+
+	assert(N_INPUT_FORMATTER_ID <= (ARRAY_SIZE(block)));
+
+	block[INPUT_FORMATTER0_ID] = (bool)config_a->block_no_reqs;
+	if (config_b)
+		block[INPUT_FORMATTER1_ID] = (bool)config_b->block_no_reqs;
+
+	/* TODO: next could cause issues when streams are started after
+	 * eachother. */
+	/*IF should not be reconfigured/reset from host */
+	if (ifmtr_set_if_blocking_mode_reset) {
+		ifmtr_set_if_blocking_mode_reset = false;
+		for (i = 0; i < N_INPUT_FORMATTER_ID; i++) {
+			input_formatter_ID_t id = (input_formatter_ID_t)i;
+
+			input_formatter_rst(id);
+			input_formatter_set_fifo_blocking_mode(id, block[id]);
+		}
+	}
+
+	return;
+}
+
+static int ifmtr_start_column(
+    const struct ia_css_stream_config *config,
+    unsigned int bin_in,
+    unsigned int *start_column)
+{
+	unsigned int in = config->input_config.input_res.width, start,
+		     for_bayer = ia_css_ifmtr_columns_needed_for_bayer_order(config);
+
+	if (bin_in + 2 * for_bayer > in)
+		return -EINVAL;
+
+	/* On the hardware, we want to use the middle of the input, so we
+	 * divide the start column by 2. */
+	start = (in - bin_in) / 2;
+	/* in case the number of extra columns is 2 or odd, we round the start
+	 * column down */
+	start &= ~0x1;
+
+	/* now we add the one column (if needed) to correct for the bayer
+	 * order).
+	 */
+	start += for_bayer;
+	*start_column = start;
+	return 0;
+}
+
+static int ifmtr_input_start_line(
+    const struct ia_css_stream_config *config,
+    unsigned int bin_in,
+    unsigned int *start_line)
+{
+	unsigned int in = config->input_config.input_res.height, start,
+		     for_bayer = ia_css_ifmtr_lines_needed_for_bayer_order(config);
+
+	if (bin_in + 2 * for_bayer > in)
+		return -EINVAL;
+
+	/* On the hardware, we want to use the middle of the input, so we
+	 * divide the start line by 2. On the simulator, we cannot handle extra
+	 * lines at the end of the frame.
+	 */
+	start = (in - bin_in) / 2;
+	/* in case the number of extra lines is 2 or odd, we round the start
+	 * line down.
+	 */
+	start &= ~0x1;
+
+	/* now we add the one line (if needed) to correct for the bayer order */
+	start += for_bayer;
+	*start_line = start;
+	return 0;
+}
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/inputfifo/interface/ia_css_inputfifo.h b/drivers/staging/media/atomisp/pci/runtime/inputfifo/interface/ia_css_inputfifo.h
new file mode 100644
index 000000000..7950c5c36
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/inputfifo/interface/ia_css_inputfifo.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_INPUTFIFO_H
+#define _IA_CSS_INPUTFIFO_H
+
+#include <sp.h>
+#include <isp.h>
+
+#include "ia_css_stream_format.h"
+
+/* SP access */
+void ia_css_inputfifo_send_input_frame(
+    const unsigned short	*data,
+    unsigned int	width,
+    unsigned int	height,
+    unsigned int	ch_id,
+    enum atomisp_input_format	input_format,
+    bool			two_ppc);
+
+void ia_css_inputfifo_start_frame(
+    unsigned int	ch_id,
+    enum atomisp_input_format	input_format,
+    bool			two_ppc);
+
+void ia_css_inputfifo_send_line(
+    unsigned int	ch_id,
+    const unsigned short	*data,
+    unsigned int	width,
+    const unsigned short	*data2,
+    unsigned int	width2);
+
+void ia_css_inputfifo_send_embedded_line(
+    unsigned int	ch_id,
+    enum atomisp_input_format	data_type,
+    const unsigned short	*data,
+    unsigned int	width);
+
+void ia_css_inputfifo_end_frame(
+    unsigned int	ch_id);
+
+#endif /* _IA_CSS_INPUTFIFO_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/inputfifo/src/inputfifo.c b/drivers/staging/media/atomisp/pci/runtime/inputfifo/src/inputfifo.c
new file mode 100644
index 000000000..2d06e1240
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/inputfifo/src/inputfifo.c
@@ -0,0 +1,529 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "platform_support.h"
+
+#include "ia_css_inputfifo.h"
+
+#include "device_access.h"
+
+#define __INLINE_SP__
+#include "sp.h"
+#define __INLINE_ISP__
+#include "isp.h"
+#define __INLINE_IRQ__
+#include "irq.h"
+#define __INLINE_FIFO_MONITOR__
+#include "fifo_monitor.h"
+
+#define __INLINE_EVENT__
+#include "event_fifo.h"
+#define __INLINE_SP__
+
+#include "input_system.h"	/* MIPI_PREDICTOR_NONE,... */
+
+#include "assert_support.h"
+
+/* System independent */
+#include "sh_css_internal.h"
+#include "ia_css_isys.h"
+
+#define HBLANK_CYCLES (187)
+#define MARKER_CYCLES (6)
+
+#include <hive_isp_css_streaming_to_mipi_types_hrt.h>
+
+/* The data type is used to send special cases:
+ * yuv420: odd lines (1, 3 etc) are twice as wide as even
+ *         lines (0, 2, 4 etc).
+ * rgb: for two pixels per clock, the R and B values are sent
+ *      to output_0 while only G is sent to output_1. This means
+ *      that output_1 only gets half the number of values of output_0.
+ *      WARNING: This type should also be used for Legacy YUV420.
+ * regular: used for all other data types (RAW, YUV422, etc)
+ */
+enum inputfifo_mipi_data_type {
+	inputfifo_mipi_data_type_regular,
+	inputfifo_mipi_data_type_yuv420,
+	inputfifo_mipi_data_type_yuv420_legacy,
+	inputfifo_mipi_data_type_rgb,
+};
+
+static unsigned int inputfifo_curr_ch_id, inputfifo_curr_fmt_type;
+struct inputfifo_instance {
+	unsigned int				ch_id;
+	enum atomisp_input_format	input_format;
+	bool						two_ppc;
+	bool						streaming;
+	unsigned int				hblank_cycles;
+	unsigned int				marker_cycles;
+	unsigned int				fmt_type;
+	enum inputfifo_mipi_data_type	type;
+};
+
+/*
+ * Maintain a basic streaming to Mipi administration with ch_id as index
+ * ch_id maps on the "Mipi virtual channel ID" and can have value 0..3
+ */
+#define INPUTFIFO_NR_OF_S2M_CHANNELS	(4)
+static struct inputfifo_instance
+	inputfifo_inst_admin[INPUTFIFO_NR_OF_S2M_CHANNELS];
+
+/* Streaming to MIPI */
+static unsigned int inputfifo_wrap_marker(
+    /* static inline unsigned inputfifo_wrap_marker( */
+    unsigned int marker)
+{
+	return marker |
+	       (inputfifo_curr_ch_id << HIVE_STR_TO_MIPI_CH_ID_LSB) |
+	       (inputfifo_curr_fmt_type << _HIVE_STR_TO_MIPI_FMT_TYPE_LSB);
+}
+
+static inline void
+_sh_css_fifo_snd(unsigned int token)
+{
+	while (!can_event_send_token(STR2MIPI_EVENT_ID))
+		udelay(1);
+	event_send_token(STR2MIPI_EVENT_ID, token);
+	return;
+}
+
+static void inputfifo_send_data_a(
+    /* static inline void inputfifo_send_data_a( */
+    unsigned int data)
+{
+	unsigned int token = (1 << HIVE_STR_TO_MIPI_VALID_A_BIT) |
+			     (data << HIVE_STR_TO_MIPI_DATA_A_LSB);
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_data_b(
+    /* static inline void inputfifo_send_data_b( */
+    unsigned int data)
+{
+	unsigned int token = (1 << HIVE_STR_TO_MIPI_VALID_B_BIT) |
+			     (data << _HIVE_STR_TO_MIPI_DATA_B_LSB);
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_data(
+    /* static inline void inputfifo_send_data( */
+    unsigned int a,
+    unsigned int b)
+{
+	unsigned int token = ((1 << HIVE_STR_TO_MIPI_VALID_A_BIT) |
+			      (1 << HIVE_STR_TO_MIPI_VALID_B_BIT) |
+			      (a << HIVE_STR_TO_MIPI_DATA_A_LSB) |
+			      (b << _HIVE_STR_TO_MIPI_DATA_B_LSB));
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_sol(void)
+/* static inline void inputfifo_send_sol(void) */
+{
+	hrt_data	token = inputfifo_wrap_marker(
+				1 << HIVE_STR_TO_MIPI_SOL_BIT);
+
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_eol(void)
+/* static inline void inputfifo_send_eol(void) */
+{
+	hrt_data	token = inputfifo_wrap_marker(
+				1 << HIVE_STR_TO_MIPI_EOL_BIT);
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_sof(void)
+/* static inline void inputfifo_send_sof(void) */
+{
+	hrt_data	token = inputfifo_wrap_marker(
+				1 << HIVE_STR_TO_MIPI_SOF_BIT);
+
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_eof(void)
+/* static inline void inputfifo_send_eof(void) */
+{
+	hrt_data	token = inputfifo_wrap_marker(
+				1 << HIVE_STR_TO_MIPI_EOF_BIT);
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_ch_id_and_fmt_type(
+    /* static inline
+    void inputfifo_send_ch_id_and_fmt_type( */
+    unsigned int ch_id,
+    unsigned int fmt_type)
+{
+	hrt_data	token;
+
+	inputfifo_curr_ch_id = ch_id & _HIVE_ISP_CH_ID_MASK;
+	inputfifo_curr_fmt_type = fmt_type & _HIVE_ISP_FMT_TYPE_MASK;
+	/* we send an zero marker, this will wrap the ch_id and
+	 * fmt_type automatically.
+	 */
+	token = inputfifo_wrap_marker(0);
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_send_empty_token(void)
+/* static inline void inputfifo_send_empty_token(void) */
+{
+	hrt_data	token = inputfifo_wrap_marker(0);
+
+	_sh_css_fifo_snd(token);
+	return;
+}
+
+static void inputfifo_start_frame(
+    /* static inline void inputfifo_start_frame( */
+    unsigned int ch_id,
+    unsigned int fmt_type)
+{
+	inputfifo_send_ch_id_and_fmt_type(ch_id, fmt_type);
+	inputfifo_send_sof();
+	return;
+}
+
+static void inputfifo_end_frame(
+    unsigned int marker_cycles)
+{
+	unsigned int i;
+
+	for (i = 0; i < marker_cycles; i++)
+		inputfifo_send_empty_token();
+	inputfifo_send_eof();
+	return;
+}
+
+static void inputfifo_send_line2(
+    const unsigned short *data,
+    unsigned int width,
+    const unsigned short *data2,
+    unsigned int width2,
+    unsigned int hblank_cycles,
+    unsigned int marker_cycles,
+    unsigned int two_ppc,
+    enum inputfifo_mipi_data_type type)
+{
+	unsigned int i, is_rgb = 0, is_legacy = 0;
+
+	assert(data);
+	assert((data2) || (width2 == 0));
+	if (type == inputfifo_mipi_data_type_rgb)
+		is_rgb = 1;
+
+	if (type == inputfifo_mipi_data_type_yuv420_legacy)
+		is_legacy = 1;
+
+	for (i = 0; i < hblank_cycles; i++)
+		inputfifo_send_empty_token();
+	inputfifo_send_sol();
+	for (i = 0; i < marker_cycles; i++)
+		inputfifo_send_empty_token();
+	for (i = 0; i < width; i++, data++) {
+		/* for RGB in two_ppc, we only actually send 2 pixels per
+		 * clock in the even pixels (0, 2 etc). In the other cycles,
+		 * we only send 1 pixel, to data[0].
+		 */
+		unsigned int send_two_pixels = two_ppc;
+
+		if ((is_rgb || is_legacy) && (i % 3 == 2))
+			send_two_pixels = 0;
+		if (send_two_pixels) {
+			if (i + 1 == width) {
+				/* for jpg (binary) copy, this can occur
+				 * if the file contains an odd number of bytes.
+				 */
+				inputfifo_send_data(
+				    data[0], 0);
+			} else {
+				inputfifo_send_data(
+				    data[0], data[1]);
+			}
+			/* Additional increment because we send 2 pixels */
+			data++;
+			i++;
+		} else if (two_ppc && is_legacy) {
+			inputfifo_send_data_b(data[0]);
+		} else {
+			inputfifo_send_data_a(data[0]);
+		}
+	}
+
+	for (i = 0; i < width2; i++, data2++) {
+		/* for RGB in two_ppc, we only actually send 2 pixels per
+		 * clock in the even pixels (0, 2 etc). In the other cycles,
+		 * we only send 1 pixel, to data2[0].
+		 */
+		unsigned int send_two_pixels = two_ppc;
+
+		if ((is_rgb || is_legacy) && (i % 3 == 2))
+			send_two_pixels = 0;
+		if (send_two_pixels) {
+			if (i + 1 == width2) {
+				/* for jpg (binary) copy, this can occur
+				 * if the file contains an odd number of bytes.
+				 */
+				inputfifo_send_data(
+				    data2[0], 0);
+			} else {
+				inputfifo_send_data(
+				    data2[0], data2[1]);
+			}
+			/* Additional increment because we send 2 pixels */
+			data2++;
+			i++;
+		} else if (two_ppc && is_legacy) {
+			inputfifo_send_data_b(data2[0]);
+		} else {
+			inputfifo_send_data_a(data2[0]);
+		}
+	}
+	for (i = 0; i < hblank_cycles; i++)
+		inputfifo_send_empty_token();
+	inputfifo_send_eol();
+	return;
+}
+
+static void
+inputfifo_send_line(const unsigned short *data,
+		    unsigned int width,
+		    unsigned int hblank_cycles,
+		    unsigned int marker_cycles,
+		    unsigned int two_ppc,
+		    enum inputfifo_mipi_data_type type)
+{
+	assert(data);
+	inputfifo_send_line2(data, width, NULL, 0,
+			     hblank_cycles,
+			     marker_cycles,
+			     two_ppc,
+			     type);
+}
+
+/* Send a frame of data into the input network via the GP FIFO.
+ *  Parameters:
+ *   - data: array of 16 bit values that contains all data for the frame.
+ *   - width: width of a line in number of subpixels, for yuv420 it is the
+ *            number of Y components per line.
+ *   - height: height of the frame in number of lines.
+ *   - ch_id: channel ID.
+ *   - fmt_type: format type.
+ *   - hblank_cycles: length of horizontal blanking in cycles.
+ *   - marker_cycles: number of empty cycles after start-of-line and before
+ *                    end-of-frame.
+ *   - two_ppc: boolean, describes whether to send one or two pixels per clock
+ *              cycle. In this mode, we sent pixels N and N+1 in the same cycle,
+ *              to IF_PRIM_A and IF_PRIM_B respectively. The caller must make
+ *              sure the input data has been formatted correctly for this.
+ *              For example, for RGB formats this means that unused values
+ *              must be inserted.
+ *   - yuv420: boolean, describes whether (non-legacy) yuv420 data is used. In
+ *             this mode, the odd lines (1,3,5 etc) are half as long as the
+ *             even lines (2,4,6 etc).
+ *             Note that the first line is odd (1) and the second line is even
+ *             (2).
+ *
+ * This function does not do any reordering of pixels, the caller must make
+ * sure the data is in the righ format. Please refer to the CSS receiver
+ * documentation for details on the data formats.
+ */
+
+static void inputfifo_send_frame(
+    const unsigned short *data,
+    unsigned int width,
+    unsigned int height,
+    unsigned int ch_id,
+    unsigned int fmt_type,
+    unsigned int hblank_cycles,
+    unsigned int marker_cycles,
+    unsigned int two_ppc,
+    enum inputfifo_mipi_data_type type)
+{
+	unsigned int i;
+
+	assert(data);
+	inputfifo_start_frame(ch_id, fmt_type);
+
+	for (i = 0; i < height; i++) {
+		if ((type == inputfifo_mipi_data_type_yuv420) &&
+		    (i & 1) == 1) {
+			inputfifo_send_line(data, 2 * width,
+					    hblank_cycles,
+					    marker_cycles,
+					    two_ppc, type);
+			data += 2 * width;
+		} else {
+			inputfifo_send_line(data, width,
+					    hblank_cycles,
+					    marker_cycles,
+					    two_ppc, type);
+			data += width;
+		}
+	}
+	inputfifo_end_frame(marker_cycles);
+	return;
+}
+
+static enum inputfifo_mipi_data_type inputfifo_determine_type(
+    enum atomisp_input_format input_format)
+{
+	enum inputfifo_mipi_data_type type;
+
+	type = inputfifo_mipi_data_type_regular;
+	if (input_format == ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY) {
+		type =
+		    inputfifo_mipi_data_type_yuv420_legacy;
+	} else if (input_format == ATOMISP_INPUT_FORMAT_YUV420_8  ||
+		   input_format == ATOMISP_INPUT_FORMAT_YUV420_10 ||
+		   input_format == ATOMISP_INPUT_FORMAT_YUV420_16) {
+		type =
+		    inputfifo_mipi_data_type_yuv420;
+	} else if (input_format >= ATOMISP_INPUT_FORMAT_RGB_444 &&
+		   input_format <= ATOMISP_INPUT_FORMAT_RGB_888) {
+		type =
+		    inputfifo_mipi_data_type_rgb;
+	}
+	return type;
+}
+
+static struct inputfifo_instance *inputfifo_get_inst(
+    unsigned int ch_id)
+{
+	return &inputfifo_inst_admin[ch_id];
+}
+
+void ia_css_inputfifo_send_input_frame(
+    const unsigned short *data,
+    unsigned int width,
+    unsigned int height,
+    unsigned int ch_id,
+    enum atomisp_input_format input_format,
+    bool two_ppc)
+{
+	unsigned int fmt_type, hblank_cycles, marker_cycles;
+	enum inputfifo_mipi_data_type type;
+
+	assert(data);
+	hblank_cycles = HBLANK_CYCLES;
+	marker_cycles = MARKER_CYCLES;
+	ia_css_isys_convert_stream_format_to_mipi_format(input_format,
+		MIPI_PREDICTOR_NONE,
+		&fmt_type);
+
+	type = inputfifo_determine_type(input_format);
+
+	inputfifo_send_frame(data, width, height,
+			     ch_id, fmt_type, hblank_cycles, marker_cycles,
+			     two_ppc, type);
+}
+
+void ia_css_inputfifo_start_frame(
+    unsigned int ch_id,
+    enum atomisp_input_format input_format,
+    bool two_ppc)
+{
+	struct inputfifo_instance *s2mi;
+
+	s2mi = inputfifo_get_inst(ch_id);
+
+	s2mi->ch_id = ch_id;
+	ia_css_isys_convert_stream_format_to_mipi_format(input_format,
+		MIPI_PREDICTOR_NONE,
+		&s2mi->fmt_type);
+	s2mi->two_ppc = two_ppc;
+	s2mi->type = inputfifo_determine_type(input_format);
+	s2mi->hblank_cycles = HBLANK_CYCLES;
+	s2mi->marker_cycles = MARKER_CYCLES;
+	s2mi->streaming = true;
+
+	inputfifo_start_frame(ch_id, s2mi->fmt_type);
+	return;
+}
+
+void ia_css_inputfifo_send_line(
+    unsigned int ch_id,
+    const unsigned short *data,
+    unsigned int width,
+    const unsigned short *data2,
+    unsigned int width2)
+{
+	struct inputfifo_instance *s2mi;
+
+	assert(data);
+	assert((data2) || (width2 == 0));
+	s2mi = inputfifo_get_inst(ch_id);
+
+	/* Set global variables that indicate channel_id and format_type */
+	inputfifo_curr_ch_id = (s2mi->ch_id) & _HIVE_ISP_CH_ID_MASK;
+	inputfifo_curr_fmt_type = (s2mi->fmt_type) & _HIVE_ISP_FMT_TYPE_MASK;
+
+	inputfifo_send_line2(data, width, data2, width2,
+			     s2mi->hblank_cycles,
+			     s2mi->marker_cycles,
+			     s2mi->two_ppc,
+			     s2mi->type);
+}
+
+void ia_css_inputfifo_send_embedded_line(
+    unsigned int	ch_id,
+    enum atomisp_input_format	data_type,
+    const unsigned short	*data,
+    unsigned int	width)
+{
+	struct inputfifo_instance *s2mi;
+	unsigned int fmt_type;
+
+	assert(data);
+	s2mi = inputfifo_get_inst(ch_id);
+	ia_css_isys_convert_stream_format_to_mipi_format(data_type,
+		MIPI_PREDICTOR_NONE, &fmt_type);
+
+	/* Set format_type for metadata line. */
+	inputfifo_curr_fmt_type = fmt_type & _HIVE_ISP_FMT_TYPE_MASK;
+
+	inputfifo_send_line(data, width, s2mi->hblank_cycles, s2mi->marker_cycles,
+			    s2mi->two_ppc, inputfifo_mipi_data_type_regular);
+}
+
+void ia_css_inputfifo_end_frame(
+    unsigned int	ch_id)
+{
+	struct inputfifo_instance *s2mi;
+
+	s2mi = inputfifo_get_inst(ch_id);
+
+	/* Set global variables that indicate channel_id and format_type */
+	inputfifo_curr_ch_id = (s2mi->ch_id) & _HIVE_ISP_CH_ID_MASK;
+	inputfifo_curr_fmt_type = (s2mi->fmt_type) & _HIVE_ISP_FMT_TYPE_MASK;
+
+	/* Call existing HRT function */
+	inputfifo_end_frame(s2mi->marker_cycles);
+
+	s2mi->streaming = false;
+	return;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param.h b/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param.h
new file mode 100644
index 000000000..0ea5d6fdc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param.h
@@ -0,0 +1,103 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_ISP_PARAM_H_
+#define _IA_CSS_ISP_PARAM_H_
+
+#include <ia_css_err.h>
+#include "ia_css_isp_param_types.h"
+
+/* Set functions for parameter memory descriptors */
+void
+ia_css_isp_param_set_mem_init(
+    struct ia_css_isp_param_host_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    char *address, size_t size);
+
+void
+ia_css_isp_param_set_css_mem_init(
+    struct ia_css_isp_param_css_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    ia_css_ptr address, size_t size);
+
+void
+ia_css_isp_param_set_isp_mem_init(
+    struct ia_css_isp_param_isp_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    u32 address, size_t size);
+
+/* Get functions for parameter memory descriptors */
+const struct ia_css_host_data *
+ia_css_isp_param_get_mem_init(
+    const struct ia_css_isp_param_host_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem);
+
+const struct ia_css_data *
+ia_css_isp_param_get_css_mem_init(
+    const struct ia_css_isp_param_css_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem);
+
+const struct ia_css_isp_data *
+ia_css_isp_param_get_isp_mem_init(
+    const struct ia_css_isp_param_isp_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem);
+
+/* Initialize the memory interface sizes and addresses */
+void
+ia_css_init_memory_interface(
+    struct ia_css_isp_param_css_segments *isp_mem_if,
+    const struct ia_css_isp_param_host_segments *mem_params,
+    const struct ia_css_isp_param_css_segments *css_params);
+
+/* Allocate memory parameters */
+int
+ia_css_isp_param_allocate_isp_parameters(
+    struct ia_css_isp_param_host_segments *mem_params,
+    struct ia_css_isp_param_css_segments *css_params,
+    const struct ia_css_isp_param_isp_segments *mem_initializers);
+
+/* Destroy memory parameters */
+void
+ia_css_isp_param_destroy_isp_parameters(
+    struct ia_css_isp_param_host_segments *mem_params,
+    struct ia_css_isp_param_css_segments *css_params);
+
+/* Load fw parameters */
+void
+ia_css_isp_param_load_fw_params(
+    const char *fw,
+    union ia_css_all_memory_offsets *mem_offsets,
+    const struct ia_css_isp_param_memory_offsets *memory_offsets,
+    bool init);
+
+/* Copy host parameter images to ddr */
+int
+ia_css_isp_param_copy_isp_mem_if_to_ddr(
+    struct ia_css_isp_param_css_segments *ddr,
+    const struct ia_css_isp_param_host_segments *host,
+    enum ia_css_param_class pclass);
+
+/* Enable a pipeline by setting the control field in the isp dmem parameters */
+void
+ia_css_isp_param_enable_pipeline(
+    const struct ia_css_isp_param_host_segments *mem_params);
+
+#endif /* _IA_CSS_ISP_PARAM_H_ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param_types.h b/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param_types.h
new file mode 100644
index 000000000..8cdeae98b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isp_param/interface/ia_css_isp_param_types.h
@@ -0,0 +1,82 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef _IA_CSS_ISP_PARAM_TYPES_H_
+#define _IA_CSS_ISP_PARAM_TYPES_H_
+
+#include "ia_css_types.h"
+#include <platform_support.h>
+#include <system_global.h>
+
+/* Short hands */
+#define IA_CSS_ISP_DMEM IA_CSS_ISP_DMEM0
+#define IA_CSS_ISP_VMEM IA_CSS_ISP_VMEM0
+
+/* The driver depends on this, to be removed later. */
+#define IA_CSS_NUM_ISP_MEMORIES IA_CSS_NUM_MEMORIES
+
+/* Explicit member numbering to avoid fish type checker bug */
+enum ia_css_param_class {
+	IA_CSS_PARAM_CLASS_PARAM  = 0,	/* Late binding parameters, like 3A */
+	IA_CSS_PARAM_CLASS_CONFIG = 1,	/* Pipe config time parameters, like resolution */
+	IA_CSS_PARAM_CLASS_STATE  = 2,  /* State parameters, like tnr buffer index */
+#if 0 /* Not yet implemented */
+	IA_CSS_PARAM_CLASS_FRAME  = 3,  /* Frame time parameters, like output buffer */
+#endif
+};
+
+#define IA_CSS_NUM_PARAM_CLASSES (IA_CSS_PARAM_CLASS_STATE + 1)
+
+/* ISP parameter descriptor */
+struct ia_css_isp_parameter {
+	u32 offset; /* Offset in isp_<mem>)parameters, etc. */
+	u32 size;   /* Disabled if 0 */
+};
+
+/* Address/size of each parameter class in each isp memory, host memory pointers */
+struct ia_css_isp_param_host_segments {
+	struct ia_css_host_data params[IA_CSS_NUM_PARAM_CLASSES][IA_CSS_NUM_MEMORIES];
+};
+
+/* Address/size of each parameter class in each isp memory, css memory pointers */
+struct ia_css_isp_param_css_segments {
+	struct ia_css_data      params[IA_CSS_NUM_PARAM_CLASSES][IA_CSS_NUM_MEMORIES];
+};
+
+/* Address/size of each parameter class in each isp memory, isp memory pointers */
+struct ia_css_isp_param_isp_segments {
+	struct ia_css_isp_data  params[IA_CSS_NUM_PARAM_CLASSES][IA_CSS_NUM_MEMORIES];
+};
+
+/* Memory offsets in binary info */
+struct ia_css_isp_param_memory_offsets {
+	u32 offsets[IA_CSS_NUM_PARAM_CLASSES];  /** offset wrt hdr in bytes */
+};
+
+/* Offsets for ISP kernel parameters per isp memory.
+ * Only relevant for standard ISP binaries, not ACC or SP.
+ */
+union ia_css_all_memory_offsets {
+	struct {
+		CSS_ALIGN(struct ia_css_memory_offsets	      *param, 8);
+		CSS_ALIGN(struct ia_css_config_memory_offsets *config, 8);
+		CSS_ALIGN(struct ia_css_state_memory_offsets  *state, 8);
+	} offsets;
+	struct {
+		CSS_ALIGN(void *ptr, 8);
+	} array[IA_CSS_NUM_PARAM_CLASSES];
+};
+
+#endif /* _IA_CSS_ISP_PARAM_TYPES_H_ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isp_param/src/isp_param.c b/drivers/staging/media/atomisp/pci/runtime/isp_param/src/isp_param.c
new file mode 100644
index 000000000..99c2f3a53
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isp_param/src/isp_param.c
@@ -0,0 +1,219 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_param.h"
+
+/* Set functions for parameter memory descriptors */
+
+void
+ia_css_isp_param_set_mem_init(
+    struct ia_css_isp_param_host_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    char *address, size_t size)
+{
+	mem_init->params[pclass][mem].address = address;
+	mem_init->params[pclass][mem].size = (uint32_t)size;
+}
+
+void
+ia_css_isp_param_set_css_mem_init(
+    struct ia_css_isp_param_css_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    ia_css_ptr address, size_t size)
+{
+	mem_init->params[pclass][mem].address = address;
+	mem_init->params[pclass][mem].size = (uint32_t)size;
+}
+
+void
+ia_css_isp_param_set_isp_mem_init(
+    struct ia_css_isp_param_isp_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem,
+    u32 address, size_t size)
+{
+	mem_init->params[pclass][mem].address = address;
+	mem_init->params[pclass][mem].size = (uint32_t)size;
+}
+
+/* Get functions for parameter memory descriptors */
+const struct ia_css_host_data *
+ia_css_isp_param_get_mem_init(
+    const struct ia_css_isp_param_host_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem)
+{
+	return &mem_init->params[pclass][mem];
+}
+
+const struct ia_css_data *
+ia_css_isp_param_get_css_mem_init(
+    const struct ia_css_isp_param_css_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem)
+{
+	return &mem_init->params[pclass][mem];
+}
+
+const struct ia_css_isp_data *
+ia_css_isp_param_get_isp_mem_init(
+    const struct ia_css_isp_param_isp_segments *mem_init,
+    enum ia_css_param_class pclass,
+    enum ia_css_isp_memories mem)
+{
+	return &mem_init->params[pclass][mem];
+}
+
+void
+ia_css_init_memory_interface(
+    struct ia_css_isp_param_css_segments *isp_mem_if,
+    const struct ia_css_isp_param_host_segments *mem_params,
+    const struct ia_css_isp_param_css_segments *css_params)
+{
+	unsigned int pclass, mem;
+
+	for (pclass = 0; pclass < IA_CSS_NUM_PARAM_CLASSES; pclass++) {
+		memset(isp_mem_if->params[pclass], 0, sizeof(isp_mem_if->params[pclass]));
+		for (mem = 0; mem < IA_CSS_NUM_MEMORIES; mem++) {
+			if (!mem_params->params[pclass][mem].address)
+				continue;
+			isp_mem_if->params[pclass][mem].size = mem_params->params[pclass][mem].size;
+			if (pclass != IA_CSS_PARAM_CLASS_PARAM)
+				isp_mem_if->params[pclass][mem].address =
+				    css_params->params[pclass][mem].address;
+		}
+	}
+}
+
+int
+ia_css_isp_param_allocate_isp_parameters(
+    struct ia_css_isp_param_host_segments *mem_params,
+    struct ia_css_isp_param_css_segments *css_params,
+    const struct ia_css_isp_param_isp_segments *mem_initializers) {
+	int err = 0;
+	unsigned int mem, pclass;
+
+	pclass = IA_CSS_PARAM_CLASS_PARAM;
+	for (mem = 0; mem < IA_CSS_NUM_MEMORIES; mem++)
+	{
+		for (pclass = 0; pclass < IA_CSS_NUM_PARAM_CLASSES; pclass++) {
+			u32 size = 0;
+
+			if (mem_initializers)
+				size = mem_initializers->params[pclass][mem].size;
+			mem_params->params[pclass][mem].size = size;
+			mem_params->params[pclass][mem].address = NULL;
+			css_params->params[pclass][mem].size = size;
+			css_params->params[pclass][mem].address = 0x0;
+			if (size) {
+				mem_params->params[pclass][mem].address = kvcalloc(1,
+										   size,
+										   GFP_KERNEL);
+				if (!mem_params->params[pclass][mem].address) {
+					err = -ENOMEM;
+					goto cleanup;
+				}
+				if (pclass != IA_CSS_PARAM_CLASS_PARAM) {
+					css_params->params[pclass][mem].address = hmm_alloc(size);
+					if (!css_params->params[pclass][mem].address) {
+						err = -ENOMEM;
+						goto cleanup;
+					}
+				}
+			}
+		}
+	}
+	return err;
+cleanup:
+	ia_css_isp_param_destroy_isp_parameters(mem_params, css_params);
+	return err;
+}
+
+void
+ia_css_isp_param_destroy_isp_parameters(
+    struct ia_css_isp_param_host_segments *mem_params,
+    struct ia_css_isp_param_css_segments *css_params)
+{
+	unsigned int mem, pclass;
+
+	for (mem = 0; mem < IA_CSS_NUM_MEMORIES; mem++) {
+		for (pclass = 0; pclass < IA_CSS_NUM_PARAM_CLASSES; pclass++) {
+			kvfree(mem_params->params[pclass][mem].address);
+			if (css_params->params[pclass][mem].address)
+				hmm_free(css_params->params[pclass][mem].address);
+			mem_params->params[pclass][mem].address = NULL;
+			css_params->params[pclass][mem].address = 0x0;
+		}
+	}
+}
+
+void
+ia_css_isp_param_load_fw_params(
+    const char *fw,
+    union ia_css_all_memory_offsets *mem_offsets,
+    const struct ia_css_isp_param_memory_offsets *memory_offsets,
+    bool init)
+{
+	unsigned int pclass;
+
+	for (pclass = 0; pclass < IA_CSS_NUM_PARAM_CLASSES; pclass++) {
+		mem_offsets->array[pclass].ptr = NULL;
+		if (init)
+			mem_offsets->array[pclass].ptr = (void *)(fw + memory_offsets->offsets[pclass]);
+	}
+}
+
+int
+ia_css_isp_param_copy_isp_mem_if_to_ddr(
+    struct ia_css_isp_param_css_segments *ddr,
+    const struct ia_css_isp_param_host_segments *host,
+    enum ia_css_param_class pclass) {
+	unsigned int mem;
+
+	for (mem = 0; mem < N_IA_CSS_ISP_MEMORIES; mem++)
+	{
+		size_t       size	  = host->params[pclass][mem].size;
+		ia_css_ptr ddr_mem_ptr  = ddr->params[pclass][mem].address;
+		char	    *host_mem_ptr = host->params[pclass][mem].address;
+
+		if (size != ddr->params[pclass][mem].size)
+			return -EINVAL;
+		if (!size)
+			continue;
+		hmm_store(ddr_mem_ptr, host_mem_ptr, size);
+	}
+	return 0;
+}
+
+void
+ia_css_isp_param_enable_pipeline(
+    const struct ia_css_isp_param_host_segments *mem_params)
+{
+	/* By protocol b0 of the mandatory uint32_t first field of the
+	   input parameter is a disable bit*/
+	short dmem_offset = 0;
+
+	if (mem_params->params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM0].size == 0)
+		return;
+
+	*(uint32_t *)
+	&mem_params->params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM0].address[dmem_offset]
+	    = 0x0;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys.h b/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys.h
new file mode 100644
index 000000000..711a321e9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys.h
@@ -0,0 +1,183 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ISYS_H__
+#define __IA_CSS_ISYS_H__
+
+#include <type_support.h>
+#include <input_system.h>
+#include <ia_css_input_port.h>
+#include <ia_css_stream_format.h>
+#include <ia_css_stream_public.h>
+#include <system_global.h>
+#include "ia_css_isys_comm.h"
+
+#ifdef ISP2401
+/**
+ * Virtual Input System. (Input System 2401)
+ */
+typedef isp2401_input_system_cfg_t	ia_css_isys_descr_t;
+/* end of Virtual Input System */
+#endif
+
+input_system_err_t ia_css_isys_init(void);
+void ia_css_isys_uninit(void);
+enum mipi_port_id ia_css_isys_port_to_mipi_port(
+    enum mipi_port_id api_port);
+
+#if defined(ISP2401)
+
+/**
+ * @brief Register one (virtual) stream. This is used to track when all
+ * virtual streams are configured inside the input system. The CSI RX is
+ * only started when all registered streams are configured.
+ *
+ * @param[in]	port		CSI port
+ * @param[in]	isys_stream_id	Stream handle generated with ia_css_isys_generate_stream_id()
+ *				Must be lower than SH_CSS_MAX_ISYS_CHANNEL_NODES
+ * @return			0 if successful, -EINVAL if
+ *				there is already a stream registered with the same handle
+ */
+int ia_css_isys_csi_rx_register_stream(
+    enum mipi_port_id port,
+    uint32_t isys_stream_id);
+
+/**
+ * @brief Unregister one (virtual) stream. This is used to track when all
+ * virtual streams are configured inside the input system. The CSI RX is
+ * only started when all registered streams are configured.
+ *
+ * @param[in]	port		CSI port
+ * @param[in]	isys_stream_id	Stream handle generated with ia_css_isys_generate_stream_id()
+ *				Must be lower than SH_CSS_MAX_ISYS_CHANNEL_NODES
+ * @return			0 if successful, -EINVAL if
+ *				there is no stream registered with that handle
+ */
+int ia_css_isys_csi_rx_unregister_stream(
+    enum mipi_port_id port,
+    uint32_t isys_stream_id);
+
+int ia_css_isys_convert_compressed_format(
+    struct ia_css_csi2_compression *comp,
+    struct isp2401_input_system_cfg_s *cfg);
+unsigned int ia_css_csi2_calculate_input_system_alignment(
+    enum atomisp_input_format fmt_type);
+#endif
+
+#if !defined(ISP2401)
+/* CSS Receiver */
+void ia_css_isys_rx_configure(
+    const rx_cfg_t *config,
+    const enum ia_css_input_mode input_mode);
+
+void ia_css_isys_rx_disable(void);
+
+void ia_css_isys_rx_enable_all_interrupts(enum mipi_port_id port);
+
+unsigned int ia_css_isys_rx_get_interrupt_reg(enum mipi_port_id port);
+void ia_css_isys_rx_get_irq_info(enum mipi_port_id port,
+				 unsigned int *irq_infos);
+void ia_css_isys_rx_clear_irq_info(enum mipi_port_id port,
+				   unsigned int irq_infos);
+unsigned int ia_css_isys_rx_translate_irq_infos(unsigned int bits);
+
+#endif /* #if !defined(ISP2401) */
+
+/* @brief Translate format and compression to format type.
+ *
+ * @param[in]	input_format	The input format.
+ * @param[in]	compression	The compression scheme.
+ * @param[out]	fmt_type	Pointer to the resulting format type.
+ * @return			Error code.
+ *
+ * Translate an input format and mipi compression pair to the fmt_type.
+ * This is normally done by the sensor, but when using the input fifo, this
+ * format type must be sumitted correctly by the application.
+ */
+int ia_css_isys_convert_stream_format_to_mipi_format(
+    enum atomisp_input_format input_format,
+    mipi_predictor_t compression,
+    unsigned int *fmt_type);
+
+#ifdef ISP2401
+/**
+ * Virtual Input System. (Input System 2401)
+ */
+ia_css_isys_error_t ia_css_isys_stream_create(
+    ia_css_isys_descr_t	*isys_stream_descr,
+    ia_css_isys_stream_h	isys_stream,
+    uint32_t isys_stream_id);
+
+void ia_css_isys_stream_destroy(
+    ia_css_isys_stream_h	isys_stream);
+
+ia_css_isys_error_t ia_css_isys_stream_calculate_cfg(
+    ia_css_isys_stream_h		isys_stream,
+    ia_css_isys_descr_t		*isys_stream_descr,
+    ia_css_isys_stream_cfg_t	*isys_stream_cfg);
+
+void ia_css_isys_csi_rx_lut_rmgr_init(void);
+
+void ia_css_isys_csi_rx_lut_rmgr_uninit(void);
+
+bool ia_css_isys_csi_rx_lut_rmgr_acquire(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry);
+
+void ia_css_isys_csi_rx_lut_rmgr_release(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry);
+
+void ia_css_isys_ibuf_rmgr_init(void);
+
+void ia_css_isys_ibuf_rmgr_uninit(void);
+
+bool ia_css_isys_ibuf_rmgr_acquire(
+    u32	size,
+    uint32_t	*start_addr);
+
+void ia_css_isys_ibuf_rmgr_release(
+    uint32_t	*start_addr);
+
+void ia_css_isys_dma_channel_rmgr_init(void);
+
+void ia_css_isys_dma_channel_rmgr_uninit(void);
+
+bool ia_css_isys_dma_channel_rmgr_acquire(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel);
+
+void ia_css_isys_dma_channel_rmgr_release(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel);
+
+void ia_css_isys_stream2mmio_sid_rmgr_init(void);
+
+void ia_css_isys_stream2mmio_sid_rmgr_uninit(void);
+
+bool ia_css_isys_stream2mmio_sid_rmgr_acquire(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid);
+
+void ia_css_isys_stream2mmio_sid_rmgr_release(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid);
+
+/* end of Virtual Input System */
+#endif
+
+#endif				/* __IA_CSS_ISYS_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys_comm.h b/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys_comm.h
new file mode 100644
index 000000000..d80ef42c7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/interface/ia_css_isys_comm.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_ISYS_COMM_H
+#define __IA_CSS_ISYS_COMM_H
+
+#include <type_support.h>
+#include <input_system.h>
+
+#ifdef ISP2401
+#include <platform_support.h>		/* inline */
+#include <input_system_global.h>
+#include <ia_css_stream_public.h>	/* IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH */
+
+#define SH_CSS_NODES_PER_THREAD		2
+#define SH_CSS_MAX_ISYS_CHANNEL_NODES	(SH_CSS_MAX_SP_THREADS * SH_CSS_NODES_PER_THREAD)
+
+/*
+ * a) ia_css_isys_stream_h & ia_css_isys_stream_cfg_t come from host.
+ *
+ * b) Here it is better  to use actual structures for stream handle
+ * instead of opaque handles. Otherwise, we need to have another
+ * communication channel to interpret that opaque handle(this handle is
+ * maintained by host and needs to be populated to sp for every stream open)
+ * */
+typedef virtual_input_system_stream_t		*ia_css_isys_stream_h;
+typedef virtual_input_system_stream_cfg_t	ia_css_isys_stream_cfg_t;
+
+/*
+ * error check for ISYS APIs.
+ * */
+typedef bool ia_css_isys_error_t;
+
+static inline uint32_t ia_css_isys_generate_stream_id(
+    u32	sp_thread_id,
+    uint32_t	stream_id)
+{
+	return sp_thread_id * IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH + stream_id;
+}
+
+#endif  /* ISP2401*/
+#endif  /*_IA_CSS_ISYS_COMM_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.c
new file mode 100644
index 000000000..3fc9fed1e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.c
@@ -0,0 +1,168 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#ifdef ISP2401
+
+#include "assert_support.h"
+#include "platform_support.h"
+#include "ia_css_isys.h"
+#include "bitop_support.h"
+#include "ia_css_pipeline.h"	/* ia_css_pipeline_get_pipe_io_status() */
+#include "sh_css_internal.h"	/* sh_css_sp_pipeline_io_status
+				 * SH_CSS_MAX_SP_THREADS
+				 */
+#include "csi_rx_rmgr.h"
+
+static isys_csi_rx_rsrc_t  isys_csi_rx_rsrc[N_CSI_RX_BACKEND_ID];
+
+void ia_css_isys_csi_rx_lut_rmgr_init(void)
+{
+	memset(isys_csi_rx_rsrc, 0, sizeof(isys_csi_rx_rsrc));
+}
+
+void ia_css_isys_csi_rx_lut_rmgr_uninit(void)
+{
+	memset(isys_csi_rx_rsrc, 0, sizeof(isys_csi_rx_rsrc));
+}
+
+bool ia_css_isys_csi_rx_lut_rmgr_acquire(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry)
+{
+	bool retval = false;
+	u32 max_num_packets_of_type;
+	u32 num_active_of_type;
+	isys_csi_rx_rsrc_t *cur_rsrc = NULL;
+	u16 i;
+
+	assert(backend < N_CSI_RX_BACKEND_ID);
+	assert((packet_type == CSI_MIPI_PACKET_TYPE_LONG) ||
+	       (packet_type == CSI_MIPI_PACKET_TYPE_SHORT));
+	assert(entry);
+
+	if ((backend < N_CSI_RX_BACKEND_ID) && (entry)) {
+		cur_rsrc = &isys_csi_rx_rsrc[backend];
+		if (packet_type == CSI_MIPI_PACKET_TYPE_LONG) {
+			max_num_packets_of_type = N_LONG_PACKET_LUT_ENTRIES[backend];
+			num_active_of_type = cur_rsrc->num_long_packets;
+		} else {
+			max_num_packets_of_type = N_SHORT_PACKET_LUT_ENTRIES[backend];
+			num_active_of_type = cur_rsrc->num_short_packets;
+		}
+
+		if (num_active_of_type < max_num_packets_of_type) {
+			for (i = 0; i < max_num_packets_of_type; i++) {
+				if (bitop_getbit(cur_rsrc->active_table, i) == 0) {
+					bitop_setbit(cur_rsrc->active_table, i);
+
+					if (packet_type == CSI_MIPI_PACKET_TYPE_LONG) {
+						entry->long_packet_entry = i;
+						entry->short_packet_entry = 0;
+						cur_rsrc->num_long_packets++;
+					} else {
+						entry->long_packet_entry = 0;
+						entry->short_packet_entry = i;
+						cur_rsrc->num_short_packets++;
+					}
+					cur_rsrc->num_active++;
+					retval = true;
+					break;
+				}
+			}
+		}
+	}
+	return retval;
+}
+
+void ia_css_isys_csi_rx_lut_rmgr_release(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry)
+{
+	u32 max_num_packets;
+	isys_csi_rx_rsrc_t *cur_rsrc = NULL;
+	u32 packet_entry = 0;
+
+	assert(backend < N_CSI_RX_BACKEND_ID);
+	assert(entry);
+	assert((packet_type >= CSI_MIPI_PACKET_TYPE_LONG) ||
+	       (packet_type <= CSI_MIPI_PACKET_TYPE_SHORT));
+
+	if ((backend < N_CSI_RX_BACKEND_ID) && (entry)) {
+		if (packet_type == CSI_MIPI_PACKET_TYPE_LONG) {
+			max_num_packets = N_LONG_PACKET_LUT_ENTRIES[backend];
+			packet_entry = entry->long_packet_entry;
+		} else {
+			max_num_packets = N_SHORT_PACKET_LUT_ENTRIES[backend];
+			packet_entry = entry->short_packet_entry;
+		}
+
+		cur_rsrc = &isys_csi_rx_rsrc[backend];
+		if ((packet_entry < max_num_packets) && (cur_rsrc->num_active > 0)) {
+			if (bitop_getbit(cur_rsrc->active_table, packet_entry) == 1) {
+				bitop_clearbit(cur_rsrc->active_table, packet_entry);
+
+				if (packet_type == CSI_MIPI_PACKET_TYPE_LONG)
+					cur_rsrc->num_long_packets--;
+				else
+					cur_rsrc->num_short_packets--;
+				cur_rsrc->num_active--;
+			}
+		}
+	}
+}
+
+int ia_css_isys_csi_rx_register_stream(
+    enum mipi_port_id port,
+    uint32_t isys_stream_id)
+{
+	int retval = -EINVAL;
+
+	if ((port < N_INPUT_SYSTEM_CSI_PORT) &&
+	    (isys_stream_id < SH_CSS_MAX_ISYS_CHANNEL_NODES)) {
+		struct sh_css_sp_pipeline_io_status *pipe_io_status;
+
+		pipe_io_status = ia_css_pipeline_get_pipe_io_status();
+		if (bitop_getbit(pipe_io_status->active[port], isys_stream_id) == 0) {
+			bitop_setbit(pipe_io_status->active[port], isys_stream_id);
+			pipe_io_status->running[port] = 0;
+			retval = 0;
+		}
+	}
+	return retval;
+}
+
+int ia_css_isys_csi_rx_unregister_stream(
+    enum mipi_port_id port,
+    uint32_t isys_stream_id)
+{
+	int retval = -EINVAL;
+
+	if ((port < N_INPUT_SYSTEM_CSI_PORT) &&
+	    (isys_stream_id < SH_CSS_MAX_ISYS_CHANNEL_NODES)) {
+		struct sh_css_sp_pipeline_io_status *pipe_io_status;
+
+		pipe_io_status = ia_css_pipeline_get_pipe_io_status();
+		if (bitop_getbit(pipe_io_status->active[port], isys_stream_id) == 1) {
+			bitop_clearbit(pipe_io_status->active[port], isys_stream_id);
+			retval = 0;
+		}
+	}
+	return retval;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.h b/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.h
new file mode 100644
index 000000000..11f730dc1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/csi_rx_rmgr.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __CSI_RX_RMGR_H_INCLUDED__
+#define __CSI_RX_RMGR_H_INCLUDED__
+
+typedef struct isys_csi_rx_rsrc_s isys_csi_rx_rsrc_t;
+struct isys_csi_rx_rsrc_s {
+	u32	active_table;
+	u32        num_active;
+	u16	num_long_packets;
+	u16	num_short_packets;
+};
+
+#endif /* __CSI_RX_RMGR_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.c
new file mode 100644
index 000000000..9710493c4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.c
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#include "assert_support.h"
+#include "platform_support.h"
+#include "ia_css_isys.h"
+#include "ibuf_ctrl_rmgr.h"
+
+static ibuf_rsrc_t	ibuf_rsrc;
+
+static ibuf_handle_t *getHandle(uint16_t index)
+{
+	ibuf_handle_t *handle = NULL;
+
+	if (index < MAX_IBUF_HANDLES)
+		handle = &ibuf_rsrc.handles[index];
+	return handle;
+}
+
+void ia_css_isys_ibuf_rmgr_init(void)
+{
+	memset(&ibuf_rsrc, 0, sizeof(ibuf_rsrc));
+	ibuf_rsrc.free_size = MAX_INPUT_BUFFER_SIZE;
+}
+
+void ia_css_isys_ibuf_rmgr_uninit(void)
+{
+	memset(&ibuf_rsrc, 0, sizeof(ibuf_rsrc));
+	ibuf_rsrc.free_size = MAX_INPUT_BUFFER_SIZE;
+}
+
+bool ia_css_isys_ibuf_rmgr_acquire(
+    u32	size,
+    uint32_t	*start_addr)
+{
+	bool retval = false;
+	bool input_buffer_found = false;
+	u32 aligned_size;
+	ibuf_handle_t *handle = NULL;
+	u16 i;
+
+	assert(start_addr);
+	assert(size > 0);
+
+	aligned_size = (size + (IBUF_ALIGN - 1)) & ~(IBUF_ALIGN - 1);
+
+	/* Check if there is an available un-used handle with the size
+	 * that will fulfill the request.
+	 */
+	if (ibuf_rsrc.num_active < ibuf_rsrc.num_allocated) {
+		for (i = 0; i < ibuf_rsrc.num_allocated; i++) {
+			handle = getHandle(i);
+			if (!handle->active) {
+				if (handle->size >= aligned_size) {
+					handle->active = true;
+					input_buffer_found = true;
+					ibuf_rsrc.num_active++;
+					break;
+				}
+			}
+		}
+	}
+
+	if (!input_buffer_found) {
+		/* There were no available handles that fulfilled the
+		 * request. Allocate a new handle with the requested size.
+		 */
+		if ((ibuf_rsrc.num_allocated < MAX_IBUF_HANDLES) &&
+		    (ibuf_rsrc.free_size >= aligned_size)) {
+			handle = getHandle(ibuf_rsrc.num_allocated);
+			handle->start_addr	= ibuf_rsrc.free_start_addr;
+			handle->size		= aligned_size;
+			handle->active		= true;
+
+			ibuf_rsrc.free_start_addr += aligned_size;
+			ibuf_rsrc.free_size -= aligned_size;
+			ibuf_rsrc.num_active++;
+			ibuf_rsrc.num_allocated++;
+
+			input_buffer_found = true;
+		}
+	}
+
+	if (input_buffer_found && handle) {
+		*start_addr = handle->start_addr;
+		retval = true;
+	}
+
+	return retval;
+}
+
+void ia_css_isys_ibuf_rmgr_release(
+    uint32_t	*start_addr)
+{
+	u16 i;
+	ibuf_handle_t *handle = NULL;
+
+	assert(start_addr);
+
+	for (i = 0; i < ibuf_rsrc.num_allocated; i++) {
+		handle = getHandle(i);
+		if (handle->active && handle->start_addr == *start_addr) {
+			handle->active = false;
+			ibuf_rsrc.num_active--;
+			break;
+		}
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.h b/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.h
new file mode 100644
index 000000000..7c754ec72
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/ibuf_ctrl_rmgr.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IBUF_CTRL_RMGR_H_INCLUDED__
+#define __IBUF_CTRL_RMGR_H_INCLUDED__
+
+#define MAX_IBUF_HANDLES	24
+#define MAX_INPUT_BUFFER_SIZE	(64 * 1024)
+#define IBUF_ALIGN		8
+
+typedef struct ibuf_handle_s ibuf_handle_t;
+struct ibuf_handle_s {
+	u32	start_addr;
+	u32	size;
+	bool		active;
+};
+
+typedef struct ibuf_rsrc_s ibuf_rsrc_t;
+struct ibuf_rsrc_s {
+	u32	free_start_addr;
+	u32	free_size;
+	u16	num_active;
+	u16	num_allocated;
+	ibuf_handle_t	handles[MAX_IBUF_HANDLES];
+};
+
+#endif /* __IBUF_CTRL_RMGR_H_INCLUDED */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.c
new file mode 100644
index 000000000..261c6460e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.c
@@ -0,0 +1,88 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#ifdef ISP2401
+
+#include "assert_support.h"
+#include "platform_support.h"
+#include "ia_css_isys.h"
+#include "bitop_support.h"
+#include "isys_dma_rmgr.h"
+
+static isys_dma_rsrc_t isys_dma_rsrc[N_ISYS2401_DMA_ID];
+
+void ia_css_isys_dma_channel_rmgr_init(void)
+{
+	memset(&isys_dma_rsrc, 0, sizeof(isys_dma_rsrc_t));
+}
+
+void ia_css_isys_dma_channel_rmgr_uninit(void)
+{
+	memset(&isys_dma_rsrc, 0, sizeof(isys_dma_rsrc_t));
+}
+
+bool ia_css_isys_dma_channel_rmgr_acquire(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel)
+{
+	bool retval = false;
+	isys2401_dma_channel	i;
+	isys2401_dma_channel	max_dma_channel;
+	isys_dma_rsrc_t		*cur_rsrc = NULL;
+
+	assert(dma_id < N_ISYS2401_DMA_ID);
+	assert(channel);
+
+	max_dma_channel = N_ISYS2401_DMA_CHANNEL_PROCS[dma_id];
+	cur_rsrc = &isys_dma_rsrc[dma_id];
+
+	if (cur_rsrc->num_active < max_dma_channel) {
+		for (i = ISYS2401_DMA_CHANNEL_0; i < N_ISYS2401_DMA_CHANNEL; i++) {
+			if (bitop_getbit(cur_rsrc->active_table, i) == 0) {
+				bitop_setbit(cur_rsrc->active_table, i);
+				*channel = i;
+				cur_rsrc->num_active++;
+				retval = true;
+				break;
+			}
+		}
+	}
+
+	return retval;
+}
+
+void ia_css_isys_dma_channel_rmgr_release(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel)
+{
+	isys2401_dma_channel	max_dma_channel;
+	isys_dma_rsrc_t		*cur_rsrc = NULL;
+
+	assert(dma_id < N_ISYS2401_DMA_ID);
+	assert(channel);
+
+	max_dma_channel = N_ISYS2401_DMA_CHANNEL_PROCS[dma_id];
+	cur_rsrc = &isys_dma_rsrc[dma_id];
+
+	if ((*channel < max_dma_channel) && (cur_rsrc->num_active > 0)) {
+		if (bitop_getbit(cur_rsrc->active_table, *channel) == 1) {
+			bitop_clearbit(cur_rsrc->active_table, *channel);
+			cur_rsrc->num_active--;
+		}
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.h b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.h
new file mode 100644
index 000000000..88c3d5581
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_dma_rmgr.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_DMA_RMGR_H_INCLUDED__
+#define __ISYS_DMA_RMGR_H_INCLUDED__
+
+typedef struct isys_dma_rsrc_s	isys_dma_rsrc_t;
+struct isys_dma_rsrc_s {
+	u32 active_table;
+	u16 num_active;
+};
+
+#endif /* __ISYS_DMA_RMGR_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_init.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_init.c
new file mode 100644
index 000000000..d0a43c449
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_init.c
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "input_system.h"
+
+#include "ia_css_isys.h"
+#include "platform_support.h"
+
+#ifdef ISP2401
+#include "isys_dma_public.h"	/* isys2401_dma_set_max_burst_size() */
+#include "isys_irq.h"
+#endif
+
+#if !defined(ISP2401)
+input_system_err_t ia_css_isys_init(void)
+{
+	backend_channel_cfg_t backend_ch0;
+	backend_channel_cfg_t backend_ch1;
+	target_cfg2400_t targetB;
+	target_cfg2400_t targetC;
+	u32 acq_mem_region_size = 24;
+	u32 acq_nof_mem_regions = 2;
+	input_system_err_t error = INPUT_SYSTEM_ERR_NO_ERROR;
+
+	memset(&backend_ch0, 0, sizeof(backend_channel_cfg_t));
+	memset(&backend_ch1, 0, sizeof(backend_channel_cfg_t));
+	memset(&targetB, 0, sizeof(targetB));
+	memset(&targetC, 0, sizeof(targetC));
+
+	error = input_system_configuration_reset();
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR)
+		return error;
+
+	error = input_system_csi_xmem_channel_cfg(
+		    0,			/*ch_id                 */
+		    INPUT_SYSTEM_PORT_A,	/*port                  */
+		    backend_ch0,		/*backend_ch            */
+		    32,			/*mem_region_size       */
+		    6,			/*nof_mem_regions       */
+		    acq_mem_region_size,	/*acq_mem_region_size   */
+		    acq_nof_mem_regions,	/*acq_nof_mem_regions   */
+		    targetB,		/*target                */
+		    3);			/*nof_xmem_buffers      */
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR)
+		return error;
+
+	error = input_system_csi_xmem_channel_cfg(
+		    1,			/*ch_id                 */
+		    INPUT_SYSTEM_PORT_B,	/*port                  */
+		    backend_ch0,		/*backend_ch            */
+		    16,			/*mem_region_size       */
+		    3,			/*nof_mem_regions       */
+		    acq_mem_region_size,	/*acq_mem_region_size   */
+		    acq_nof_mem_regions,	/*acq_nof_mem_regions   */
+		    targetB,		/*target                */
+		    3);			/*nof_xmem_buffers      */
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR)
+		return error;
+
+	error = input_system_csi_xmem_channel_cfg(
+		    2,			/*ch_id                 */
+		    INPUT_SYSTEM_PORT_C,	/*port                  */
+		    backend_ch1,		/*backend_ch            */
+		    32,			/*mem_region_size       */
+		    3,			/*nof_mem_regions       */
+		    acq_mem_region_size,	/*acq_mem_region_size   */
+		    acq_nof_mem_regions,	/*acq_nof_mem_regions   */
+		    targetC,		/*target                */
+		    2);			/*nof_xmem_buffers      */
+	if (error != INPUT_SYSTEM_ERR_NO_ERROR)
+		return error;
+
+	error = input_system_configuration_commit();
+
+	return error;
+}
+#elif defined(ISP2401)
+input_system_err_t ia_css_isys_init(void)
+{
+	ia_css_isys_csi_rx_lut_rmgr_init();
+	ia_css_isys_ibuf_rmgr_init();
+	ia_css_isys_dma_channel_rmgr_init();
+	ia_css_isys_stream2mmio_sid_rmgr_init();
+
+	isys2401_dma_set_max_burst_size(ISYS2401_DMA0_ID,
+					1 /* Non Burst DMA transactions */);
+
+	/* Enable 2401 input system IRQ status for driver to retrieve */
+	isys_irqc_status_enable(ISYS_IRQ0_ID);
+	isys_irqc_status_enable(ISYS_IRQ1_ID);
+	isys_irqc_status_enable(ISYS_IRQ2_ID);
+
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+#endif
+
+#if !defined(ISP2401)
+void ia_css_isys_uninit(void)
+{
+}
+#elif defined(ISP2401)
+void ia_css_isys_uninit(void)
+{
+	ia_css_isys_csi_rx_lut_rmgr_uninit();
+	ia_css_isys_ibuf_rmgr_uninit();
+	ia_css_isys_dma_channel_rmgr_uninit();
+	ia_css_isys_stream2mmio_sid_rmgr_uninit();
+}
+#endif
+
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.c
new file mode 100644
index 000000000..fb0cb183f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.c
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_global.h"
+
+#ifdef ISP2401
+
+#include "assert_support.h"
+#include "platform_support.h"
+#include "ia_css_isys.h"
+#include "bitop_support.h"
+#include "isys_stream2mmio_rmgr.h"
+
+static isys_stream2mmio_rsrc_t	isys_stream2mmio_rsrc[N_STREAM2MMIO_ID];
+
+void ia_css_isys_stream2mmio_sid_rmgr_init(void)
+{
+	memset(isys_stream2mmio_rsrc, 0, sizeof(isys_stream2mmio_rsrc));
+}
+
+void ia_css_isys_stream2mmio_sid_rmgr_uninit(void)
+{
+	memset(isys_stream2mmio_rsrc, 0, sizeof(isys_stream2mmio_rsrc));
+}
+
+bool ia_css_isys_stream2mmio_sid_rmgr_acquire(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid)
+{
+	bool retval = false;
+	stream2mmio_sid_ID_t max_sid;
+	isys_stream2mmio_rsrc_t *cur_rsrc = NULL;
+	stream2mmio_sid_ID_t	i;
+
+	assert(stream2mmio < N_STREAM2MMIO_ID);
+	assert(sid);
+
+	if ((stream2mmio < N_STREAM2MMIO_ID) && (sid)) {
+		max_sid = N_STREAM2MMIO_SID_PROCS[stream2mmio];
+		cur_rsrc = &isys_stream2mmio_rsrc[stream2mmio];
+
+		if (cur_rsrc->num_active < max_sid) {
+			for (i = STREAM2MMIO_SID0_ID; i < max_sid; i++) {
+				if (bitop_getbit(cur_rsrc->active_table, i) == 0) {
+					bitop_setbit(cur_rsrc->active_table, i);
+					*sid = i;
+					cur_rsrc->num_active++;
+					retval = true;
+					break;
+				}
+			}
+		}
+	}
+	return retval;
+}
+
+void ia_css_isys_stream2mmio_sid_rmgr_release(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid)
+{
+	stream2mmio_sid_ID_t max_sid;
+	isys_stream2mmio_rsrc_t *cur_rsrc = NULL;
+
+	assert(stream2mmio < N_STREAM2MMIO_ID);
+	assert(sid);
+
+	if ((stream2mmio < N_STREAM2MMIO_ID) && (sid)) {
+		max_sid = N_STREAM2MMIO_SID_PROCS[stream2mmio];
+		cur_rsrc = &isys_stream2mmio_rsrc[stream2mmio];
+		if ((*sid < max_sid) && (cur_rsrc->num_active > 0)) {
+			if (bitop_getbit(cur_rsrc->active_table, *sid) == 1) {
+				bitop_clearbit(cur_rsrc->active_table, *sid);
+				cur_rsrc->num_active--;
+			}
+		}
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.h b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.h
new file mode 100644
index 000000000..78a4c867f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/isys_stream2mmio_rmgr.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __ISYS_STREAM2MMIO_RMGR_H_INCLUDED__
+#define __ISYS_STREAM2MMIO_RMGR_H_INCLUDED__
+
+typedef struct isys_stream2mmio_rsrc_s isys_stream2mmio_rsrc_t;
+struct isys_stream2mmio_rsrc_s {
+	u32	active_table;
+	u16	num_active;
+};
+
+#endif /* __ISYS_STREAM2MMIO_RMGR_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/rx.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/rx.c
new file mode 100644
index 000000000..af153c3fb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/rx.c
@@ -0,0 +1,600 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#define __INLINE_INPUT_SYSTEM__
+#include "input_system.h"
+#include "assert_support.h"
+#include "ia_css_isys.h"
+#include "ia_css_irq.h"
+#include "sh_css_internal.h"
+
+#if !defined(ISP2401)
+void ia_css_isys_rx_enable_all_interrupts(enum mipi_port_id port)
+{
+	hrt_data bits = receiver_port_reg_load(RX0_ID,
+					       port,
+					       _HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX);
+
+	bits |= (1U << _HRT_CSS_RECEIVER_IRQ_OVERRUN_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_ENTRY_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_EXIT_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_HS_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_SYNC_HS_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_CONTROL_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_DOUBLE_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_CORRECTED_BIT) |
+		/*(1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_NO_CORRECTION_BIT) | */
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_CRC_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_ID_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_SYNC_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_DATA_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_DATA_TIMEOUT_BIT) |
+		(1U << _HRT_CSS_RECEIVER_IRQ_ERR_ESCAPE_BIT);
+	/*(1U << _HRT_CSS_RECEIVER_IRQ_ERR_LINE_SYNC_BIT); */
+
+	receiver_port_reg_store(RX0_ID,
+				port,
+				_HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX, bits);
+
+	/*
+	 * The CSI is nested into the Iunit IRQ's
+	 */
+	ia_css_irq_enable(IA_CSS_IRQ_INFO_CSS_RECEIVER_ERROR, true);
+
+	return;
+}
+
+/* This function converts between the enum used on the CSS API and the
+ * internal DLI enum type.
+ * We do not use an array for this since we cannot use named array
+ * initializers in Windows. Without that there is no easy way to guarantee
+ * that the array values would be in the correct order.
+ * */
+enum mipi_port_id ia_css_isys_port_to_mipi_port(enum mipi_port_id api_port)
+{
+	/* In this module the validity of the inptu variable should
+	 * have been checked already, so we do not check for erroneous
+	 * values. */
+	enum mipi_port_id port = MIPI_PORT0_ID;
+
+	if (api_port == MIPI_PORT1_ID)
+		port = MIPI_PORT1_ID;
+	else if (api_port == MIPI_PORT2_ID)
+		port = MIPI_PORT2_ID;
+
+	return port;
+}
+
+unsigned int ia_css_isys_rx_get_interrupt_reg(enum mipi_port_id port)
+{
+	return receiver_port_reg_load(RX0_ID,
+				      port,
+				      _HRT_CSS_RECEIVER_IRQ_STATUS_REG_IDX);
+}
+
+void ia_css_rx_get_irq_info(unsigned int *irq_infos)
+{
+	ia_css_rx_port_get_irq_info(MIPI_PORT1_ID, irq_infos);
+}
+
+void ia_css_rx_port_get_irq_info(enum mipi_port_id api_port,
+				 unsigned int *irq_infos)
+{
+	enum mipi_port_id port = ia_css_isys_port_to_mipi_port(api_port);
+
+	ia_css_isys_rx_get_irq_info(port, irq_infos);
+}
+
+void ia_css_isys_rx_get_irq_info(enum mipi_port_id port,
+				 unsigned int *irq_infos)
+{
+	unsigned int bits;
+
+	assert(irq_infos);
+	bits = ia_css_isys_rx_get_interrupt_reg(port);
+	*irq_infos = ia_css_isys_rx_translate_irq_infos(bits);
+}
+
+/* Translate register bits to CSS API enum mask */
+unsigned int ia_css_isys_rx_translate_irq_infos(unsigned int bits)
+{
+	unsigned int infos = 0;
+
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_OVERRUN_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_ENTRY_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_EXIT_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_CORRECTED_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ECC_CORRECTED;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_HS_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_SOT;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_SYNC_HS_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_CONTROL_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_CONTROL;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_DOUBLE_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_CRC_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_CRC;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_ID_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_SYNC_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_DATA_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_DATA_TIMEOUT_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_ESCAPE_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC;
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_ERR_LINE_SYNC_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC;
+
+	return infos;
+}
+
+void ia_css_rx_clear_irq_info(unsigned int irq_infos)
+{
+	ia_css_rx_port_clear_irq_info(MIPI_PORT1_ID, irq_infos);
+}
+
+void ia_css_rx_port_clear_irq_info(enum mipi_port_id api_port,
+				   unsigned int irq_infos)
+{
+	enum mipi_port_id port = ia_css_isys_port_to_mipi_port(api_port);
+
+	ia_css_isys_rx_clear_irq_info(port, irq_infos);
+}
+
+void ia_css_isys_rx_clear_irq_info(enum mipi_port_id port,
+				   unsigned int irq_infos)
+{
+	hrt_data bits = receiver_port_reg_load(RX0_ID,
+					       port,
+					       _HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX);
+
+	/* MW: Why do we remap the receiver bitmap */
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_OVERRUN_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_ENTRY_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_SLEEP_MODE_EXIT_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ECC_CORRECTED)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_CORRECTED_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_SOT)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_HS_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_SOT_SYNC_HS_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_CONTROL)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_CONTROL_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_ECC_DOUBLE_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_CRC)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_CRC_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_ID_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_SYNC_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_FRAME_DATA_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_DATA_TIMEOUT_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_ESCAPE_BIT;
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_ERR_LINE_SYNC_BIT;
+
+	receiver_port_reg_store(RX0_ID,
+				port,
+				_HRT_CSS_RECEIVER_IRQ_ENABLE_REG_IDX, bits);
+
+	return;
+}
+#endif /* #if !defined(ISP2401) */
+
+int ia_css_isys_convert_stream_format_to_mipi_format(
+    enum atomisp_input_format input_format,
+    mipi_predictor_t compression,
+    unsigned int *fmt_type)
+{
+	assert(fmt_type);
+	/*
+	 * Custom (user defined) modes. Used for compressed
+	 * MIPI transfers
+	 *
+	 * Checkpatch thinks the indent before "if" is suspect
+	 * I think the only suspect part is the missing "else"
+	 * because of the return.
+	 */
+	if (compression != MIPI_PREDICTOR_NONE) {
+		switch (input_format) {
+		case ATOMISP_INPUT_FORMAT_RAW_6:
+			*fmt_type = 6;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_7:
+			*fmt_type = 7;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_8:
+			*fmt_type = 8;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_10:
+			*fmt_type = 10;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_12:
+			*fmt_type = 12;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_14:
+			*fmt_type = 14;
+			break;
+		case ATOMISP_INPUT_FORMAT_RAW_16:
+			*fmt_type = 16;
+			break;
+		default:
+			return -EINVAL;
+		}
+		return 0;
+	}
+	/*
+	 * This mapping comes from the Arasan CSS function spec
+	 * (CSS_func_spec1.08_ahb_sep29_08.pdf).
+	 *
+	 * MW: For some reason the mapping is not 1-to-1
+	 */
+	switch (input_format) {
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		*fmt_type = MIPI_FORMAT_RGB888;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+		*fmt_type = MIPI_FORMAT_RGB555;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+		*fmt_type = MIPI_FORMAT_RGB444;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+		*fmt_type = MIPI_FORMAT_RGB565;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+		*fmt_type = MIPI_FORMAT_RGB666;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+		*fmt_type = MIPI_FORMAT_RAW8;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+		*fmt_type = MIPI_FORMAT_RAW10;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+		*fmt_type = MIPI_FORMAT_RAW6;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+		*fmt_type = MIPI_FORMAT_RAW7;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+		*fmt_type = MIPI_FORMAT_RAW12;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+		*fmt_type = MIPI_FORMAT_RAW14;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+		*fmt_type = MIPI_FORMAT_YUV420_8;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+		*fmt_type = MIPI_FORMAT_YUV420_10;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+		*fmt_type = MIPI_FORMAT_YUV422_8;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+		*fmt_type = MIPI_FORMAT_YUV422_10;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+		*fmt_type = MIPI_FORMAT_YUV420_8_LEGACY;
+		break;
+	case ATOMISP_INPUT_FORMAT_EMBEDDED:
+		*fmt_type = MIPI_FORMAT_EMBEDDED;
+		break;
+#ifndef ISP2401
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		/* This is not specified by Arasan, so we use
+		 * 17 for now.
+		 */
+		*fmt_type = MIPI_FORMAT_RAW16;
+		break;
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+		*fmt_type = MIPI_FORMAT_BINARY_8;
+		break;
+#else
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+		*fmt_type = MIPI_FORMAT_CUSTOM0;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+		*fmt_type = MIPI_FORMAT_CUSTOM1;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+		*fmt_type = MIPI_FORMAT_CUSTOM2;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+		*fmt_type = MIPI_FORMAT_CUSTOM3;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+		*fmt_type = MIPI_FORMAT_CUSTOM4;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+		*fmt_type = MIPI_FORMAT_CUSTOM5;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+		*fmt_type = MIPI_FORMAT_CUSTOM6;
+		break;
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		*fmt_type = MIPI_FORMAT_CUSTOM7;
+		break;
+#endif
+
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#if defined(ISP2401)
+static mipi_predictor_t sh_css_csi2_compression_type_2_mipi_predictor(
+    enum ia_css_csi2_compression_type type)
+{
+	mipi_predictor_t predictor = MIPI_PREDICTOR_NONE;
+
+	switch (type) {
+	case IA_CSS_CSI2_COMPRESSION_TYPE_1:
+		predictor = MIPI_PREDICTOR_TYPE1 - 1;
+		break;
+	case IA_CSS_CSI2_COMPRESSION_TYPE_2:
+		predictor = MIPI_PREDICTOR_TYPE2 - 1;
+		break;
+	default:
+		break;
+	}
+	return predictor;
+}
+
+int ia_css_isys_convert_compressed_format(
+    struct ia_css_csi2_compression *comp,
+    struct isp2401_input_system_cfg_s *cfg)
+{
+	int err = 0;
+
+	assert(comp);
+	assert(cfg);
+
+	if (comp->type != IA_CSS_CSI2_COMPRESSION_TYPE_NONE) {
+		/* compression register bit slicing
+		4 bit for each user defined data type
+			3 bit indicate compression scheme
+				000 No compression
+				001 10-6-10
+				010 10-7-10
+				011 10-8-10
+				100 12-6-12
+				101 12-6-12
+				100 12-7-12
+				110 12-8-12
+			1 bit indicate predictor
+		*/
+		if (comp->uncompressed_bits_per_pixel == UNCOMPRESSED_BITS_PER_PIXEL_10) {
+			switch (comp->compressed_bits_per_pixel) {
+			case COMPRESSED_BITS_PER_PIXEL_6:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_10_6_10;
+				break;
+			case COMPRESSED_BITS_PER_PIXEL_7:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_10_7_10;
+				break;
+			case COMPRESSED_BITS_PER_PIXEL_8:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_10_8_10;
+				break;
+			default:
+				err = -EINVAL;
+			}
+		} else if (comp->uncompressed_bits_per_pixel ==
+			   UNCOMPRESSED_BITS_PER_PIXEL_12) {
+			switch (comp->compressed_bits_per_pixel) {
+			case COMPRESSED_BITS_PER_PIXEL_6:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_12_6_12;
+				break;
+			case COMPRESSED_BITS_PER_PIXEL_7:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_12_7_12;
+				break;
+			case COMPRESSED_BITS_PER_PIXEL_8:
+				cfg->csi_port_attr.comp_scheme = MIPI_COMPRESSOR_12_8_12;
+				break;
+			default:
+				err = -EINVAL;
+			}
+		} else
+			err = -EINVAL;
+		cfg->csi_port_attr.comp_predictor =
+		    sh_css_csi2_compression_type_2_mipi_predictor(comp->type);
+		cfg->csi_port_attr.comp_enable = true;
+	} else /* No compression */
+		cfg->csi_port_attr.comp_enable = false;
+	return err;
+}
+
+unsigned int ia_css_csi2_calculate_input_system_alignment(
+    enum atomisp_input_format fmt_type)
+{
+	unsigned int memory_alignment_in_bytes = HIVE_ISP_DDR_WORD_BYTES;
+
+	switch (fmt_type) {
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+		memory_alignment_in_bytes = 2 * ISP_VEC_NELEMS;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		/* Planar YUV formats need to have all planes aligned, this means
+		 * double the alignment for the Y plane if the horizontal decimation is 2. */
+		memory_alignment_in_bytes = 2 * HIVE_ISP_DDR_WORD_BYTES;
+		break;
+	case ATOMISP_INPUT_FORMAT_EMBEDDED:
+	default:
+		memory_alignment_in_bytes = HIVE_ISP_DDR_WORD_BYTES;
+		break;
+	}
+	return memory_alignment_in_bytes;
+}
+
+#endif
+
+#if !defined(ISP2401)
+static const mipi_lane_cfg_t MIPI_PORT_LANES[N_RX_MODE][N_MIPI_PORT_ID] = {
+	{MIPI_4LANE_CFG, MIPI_1LANE_CFG, MIPI_0LANE_CFG},
+	{MIPI_3LANE_CFG, MIPI_1LANE_CFG, MIPI_0LANE_CFG},
+	{MIPI_2LANE_CFG, MIPI_1LANE_CFG, MIPI_0LANE_CFG},
+	{MIPI_1LANE_CFG, MIPI_1LANE_CFG, MIPI_0LANE_CFG},
+	{MIPI_2LANE_CFG, MIPI_1LANE_CFG, MIPI_2LANE_CFG},
+	{MIPI_3LANE_CFG, MIPI_1LANE_CFG, MIPI_1LANE_CFG},
+	{MIPI_2LANE_CFG, MIPI_1LANE_CFG, MIPI_1LANE_CFG},
+	{MIPI_1LANE_CFG, MIPI_1LANE_CFG, MIPI_1LANE_CFG}
+};
+
+void ia_css_isys_rx_configure(const rx_cfg_t *config,
+			      const enum ia_css_input_mode input_mode)
+{
+	bool any_port_enabled = false;
+	enum mipi_port_id port;
+
+	if ((!config)
+	    || (config->mode >= N_RX_MODE)
+	    || (config->port >= N_MIPI_PORT_ID)) {
+		assert(0);
+		return;
+	}
+	for (port = (enum mipi_port_id)0; port < N_MIPI_PORT_ID; port++) {
+		if (is_receiver_port_enabled(RX0_ID, port))
+			any_port_enabled = true;
+	}
+	/* AM: Check whether this is a problem with multiple
+	 * streams. MS: This is the case. */
+
+	port = config->port;
+	receiver_port_enable(RX0_ID, port, false);
+
+	port = config->port;
+
+	/* AM: Check whether this is a problem with multiple streams. */
+	if (MIPI_PORT_LANES[config->mode][port] != MIPI_0LANE_CFG) {
+		receiver_port_reg_store(RX0_ID, port,
+					_HRT_CSS_RECEIVER_FUNC_PROG_REG_IDX,
+					config->timeout);
+		receiver_port_reg_store(RX0_ID, port,
+					_HRT_CSS_RECEIVER_2400_INIT_COUNT_REG_IDX,
+					config->initcount);
+		receiver_port_reg_store(RX0_ID, port,
+					_HRT_CSS_RECEIVER_2400_SYNC_COUNT_REG_IDX,
+					config->synccount);
+		receiver_port_reg_store(RX0_ID, port,
+					_HRT_CSS_RECEIVER_2400_RX_COUNT_REG_IDX,
+					config->rxcount);
+
+		if (input_mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+			/* MW: A bit of a hack, straight wiring of the capture
+			 * units,assuming they are linearly enumerated. */
+			input_system_sub_system_reg_store(INPUT_SYSTEM0_ID,
+							  GPREGS_UNIT0_ID,
+							  HIVE_ISYS_GPREG_MULTICAST_A_IDX
+							  + (unsigned int)port,
+							  INPUT_SYSTEM_CSI_BACKEND);
+			/* MW: Like the integration test example we overwite,
+			 * the GPREG_MUX register */
+			input_system_sub_system_reg_store(INPUT_SYSTEM0_ID,
+							  GPREGS_UNIT0_ID,
+							  HIVE_ISYS_GPREG_MUX_IDX,
+							  (input_system_multiplex_t)port);
+		} else {
+			/*
+			 * AM: A bit of a hack, wiring the input system.
+			 */
+			input_system_sub_system_reg_store(INPUT_SYSTEM0_ID,
+							  GPREGS_UNIT0_ID,
+							  HIVE_ISYS_GPREG_MULTICAST_A_IDX
+							  + (unsigned int)port,
+							  INPUT_SYSTEM_INPUT_BUFFER);
+			input_system_sub_system_reg_store(INPUT_SYSTEM0_ID,
+							  GPREGS_UNIT0_ID,
+							  HIVE_ISYS_GPREG_MUX_IDX,
+							  INPUT_SYSTEM_ACQUISITION_UNIT);
+		}
+	}
+	/*
+	 * The 2ppc is shared for all ports, so we cannot
+	 * disable->configure->enable individual ports
+	 */
+	/* AM: Check whether this is a problem with multiple streams. */
+	/* MS: 2ppc should be a property per binary and should be
+	 * enabled/disabled per binary.
+	 * Currently it is implemented as a system wide setting due
+	 * to effort and risks. */
+	if (!any_port_enabled) {
+		receiver_reg_store(RX0_ID,
+				   _HRT_CSS_RECEIVER_TWO_PIXEL_EN_REG_IDX,
+				   config->is_two_ppc);
+		receiver_reg_store(RX0_ID, _HRT_CSS_RECEIVER_BE_TWO_PPC_REG_IDX,
+				   config->is_two_ppc);
+	}
+	receiver_port_enable(RX0_ID, port, true);
+	/* TODO: JB: need to add the beneath used define to mizuchi */
+	/* sh_css_sw_hive_isp_css_2400_system_20121224_0125\css
+	 *                      \hrt\input_system_defs.h
+	 * #define INPUT_SYSTEM_CSI_RECEIVER_SELECT_BACKENG 0X207
+	 */
+	/* TODO: need better name for define
+	 * input_system_reg_store(INPUT_SYSTEM0_ID,
+	 *                INPUT_SYSTEM_CSI_RECEIVER_SELECT_BACKENG, 1);
+	 */
+	input_system_reg_store(INPUT_SYSTEM0_ID, 0x207, 1);
+
+	return;
+}
+
+void ia_css_isys_rx_disable(void)
+{
+	enum mipi_port_id port;
+
+	for (port = (enum mipi_port_id)0; port < N_MIPI_PORT_ID; port++) {
+		receiver_port_reg_store(RX0_ID, port,
+					_HRT_CSS_RECEIVER_DEVICE_READY_REG_IDX,
+					false);
+	}
+	return;
+}
+#endif /* if !defined(ISP2401) */
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.c b/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.c
new file mode 100644
index 000000000..8fc7746f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.c
@@ -0,0 +1,869 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+
+#include "system_global.h"
+
+#ifdef ISP2401
+
+#include "ia_css_isys.h"
+#include "ia_css_debug.h"
+#include "math_support.h"
+#include "virtual_isys.h"
+#include "isp.h"
+#include "sh_css_defs.h"
+
+/*************************************************
+ *
+ * Forwarded Declaration
+ *
+ *************************************************/
+
+static bool create_input_system_channel(
+    isp2401_input_system_cfg_t	*cfg,
+    bool			metadata,
+    input_system_channel_t	*channel);
+
+static void destroy_input_system_channel(
+    input_system_channel_t	*channel);
+
+static bool create_input_system_input_port(
+    isp2401_input_system_cfg_t		*cfg,
+    input_system_input_port_t	*input_port);
+
+static void destroy_input_system_input_port(
+    input_system_input_port_t	*input_port);
+
+static bool calculate_input_system_channel_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    input_system_channel_cfg_t	*channel_cfg,
+    bool metadata);
+
+static bool calculate_input_system_input_port_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    input_system_input_port_cfg_t	*input_port_cfg);
+
+static bool acquire_sid(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid);
+
+static void release_sid(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid);
+
+static bool acquire_ib_buffer(
+    s32 bits_per_pixel,
+    s32 pixels_per_line,
+    s32 lines_per_frame,
+    s32 align_in_bytes,
+    bool online,
+    isp2401_ib_buffer_t *buf);
+
+static void release_ib_buffer(
+    isp2401_ib_buffer_t *buf);
+
+static bool acquire_dma_channel(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel);
+
+static void release_dma_channel(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel);
+
+static bool acquire_be_lut_entry(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry);
+
+static void release_be_lut_entry(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry);
+
+static bool calculate_tpg_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    pixelgen_tpg_cfg_t		*cfg);
+
+static bool calculate_prbs_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    pixelgen_prbs_cfg_t		*cfg);
+
+static bool calculate_fe_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    csi_rx_frontend_cfg_t		*cfg);
+
+static bool calculate_be_cfg(
+    const input_system_input_port_t	*input_port,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				metadata,
+    csi_rx_backend_cfg_t		*cfg);
+
+static bool calculate_stream2mmio_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				metadata,
+    stream2mmio_cfg_t		*cfg);
+
+static bool calculate_ibuf_ctrl_cfg(
+    const input_system_channel_t	*channel,
+    const input_system_input_port_t	*input_port,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    ibuf_ctrl_cfg_t			*cfg);
+
+static bool calculate_isys2401_dma_cfg(
+    const input_system_channel_t	*channel,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    isys2401_dma_cfg_t		*cfg);
+
+static bool calculate_isys2401_dma_port_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				raw_packed,
+    bool				metadata,
+    isys2401_dma_port_cfg_t		*cfg);
+
+static csi_mipi_packet_type_t get_csi_mipi_packet_type(
+    int32_t data_type);
+
+static int32_t calculate_stride(
+    s32 bits_per_pixel,
+    s32 pixels_per_line,
+    bool	raw_packed,
+    int32_t	align_in_bytes);
+
+/* end of Forwarded Declaration */
+
+/**************************************************
+ *
+ * Public Methods
+ *
+ **************************************************/
+ia_css_isys_error_t ia_css_isys_stream_create(
+    ia_css_isys_descr_t	*isys_stream_descr,
+    ia_css_isys_stream_h	isys_stream,
+    uint32_t isys_stream_id)
+{
+	ia_css_isys_error_t rc;
+
+	if (!isys_stream_descr || !isys_stream ||
+	    isys_stream_id >= SH_CSS_MAX_ISYS_CHANNEL_NODES)
+		return	false;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_isys_stream_create() enter:\n");
+
+	/*Reset isys_stream to 0*/
+	memset(isys_stream, 0, sizeof(*isys_stream));
+	isys_stream->enable_metadata = isys_stream_descr->metadata.enable;
+	isys_stream->id = isys_stream_id;
+
+	isys_stream->linked_isys_stream_id = isys_stream_descr->linked_isys_stream_id;
+	rc = create_input_system_input_port(isys_stream_descr,
+					    &isys_stream->input_port);
+	if (!rc)
+		return false;
+
+	rc = create_input_system_channel(isys_stream_descr, false,
+					 &isys_stream->channel);
+	if (!rc) {
+		destroy_input_system_input_port(&isys_stream->input_port);
+		return false;
+	}
+
+	/* create metadata channel */
+	if (isys_stream_descr->metadata.enable) {
+		rc = create_input_system_channel(isys_stream_descr, true,
+						 &isys_stream->md_channel);
+		if (!rc) {
+			destroy_input_system_input_port(&isys_stream->input_port);
+			destroy_input_system_channel(&isys_stream->channel);
+			return false;
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_isys_stream_create() leave:\n");
+
+	return true;
+}
+
+void ia_css_isys_stream_destroy(
+    ia_css_isys_stream_h	isys_stream)
+{
+	destroy_input_system_input_port(&isys_stream->input_port);
+	destroy_input_system_channel(&isys_stream->channel);
+	if (isys_stream->enable_metadata) {
+		/* Destroy metadata channel only if its allocated*/
+		destroy_input_system_channel(&isys_stream->md_channel);
+	}
+}
+
+ia_css_isys_error_t ia_css_isys_stream_calculate_cfg(
+    ia_css_isys_stream_h		isys_stream,
+    ia_css_isys_descr_t		*isys_stream_descr,
+    ia_css_isys_stream_cfg_t	*isys_stream_cfg)
+{
+	ia_css_isys_error_t rc;
+
+	if (!isys_stream_cfg		||
+	    !isys_stream_descr	||
+	    !isys_stream)
+		return false;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_isys_stream_calculate_cfg() enter:\n");
+
+	rc  = calculate_input_system_channel_cfg(
+		  &isys_stream->channel,
+		  &isys_stream->input_port,
+		  isys_stream_descr,
+		  &isys_stream_cfg->channel_cfg,
+		  false);
+	if (!rc)
+		return false;
+
+	/* configure metadata channel */
+	if (isys_stream_descr->metadata.enable) {
+		isys_stream_cfg->enable_metadata = true;
+		rc  = calculate_input_system_channel_cfg(
+			  &isys_stream->md_channel,
+			  &isys_stream->input_port,
+			  isys_stream_descr,
+			  &isys_stream_cfg->md_channel_cfg,
+			  true);
+		if (!rc)
+			return false;
+	}
+
+	rc = calculate_input_system_input_port_cfg(
+		 &isys_stream->channel,
+		 &isys_stream->input_port,
+		 isys_stream_descr,
+		 &isys_stream_cfg->input_port_cfg);
+	if (!rc)
+		return false;
+
+	isys_stream->valid = 1;
+	isys_stream_cfg->valid = 1;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_isys_stream_calculate_cfg() leave:\n");
+	return rc;
+}
+
+/* end of Public Methods */
+
+/**************************************************
+ *
+ * Private Methods
+ *
+ **************************************************/
+static bool create_input_system_channel(
+    isp2401_input_system_cfg_t	*cfg,
+    bool			metadata,
+    input_system_channel_t	*me)
+{
+	bool rc = true;
+
+	me->dma_id = ISYS2401_DMA0_ID;
+
+	switch (cfg->input_port_id) {
+	case INPUT_SYSTEM_CSI_PORT0_ID:
+	case INPUT_SYSTEM_PIXELGEN_PORT0_ID:
+		me->stream2mmio_id = STREAM2MMIO0_ID;
+		me->ibuf_ctrl_id = IBUF_CTRL0_ID;
+		break;
+
+	case INPUT_SYSTEM_CSI_PORT1_ID:
+	case INPUT_SYSTEM_PIXELGEN_PORT1_ID:
+		me->stream2mmio_id = STREAM2MMIO1_ID;
+		me->ibuf_ctrl_id = IBUF_CTRL1_ID;
+		break;
+
+	case INPUT_SYSTEM_CSI_PORT2_ID:
+	case INPUT_SYSTEM_PIXELGEN_PORT2_ID:
+		me->stream2mmio_id = STREAM2MMIO2_ID;
+		me->ibuf_ctrl_id = IBUF_CTRL2_ID;
+		break;
+	default:
+		rc = false;
+		break;
+	}
+
+	if (!rc)
+		return false;
+
+	if (!acquire_sid(me->stream2mmio_id, &me->stream2mmio_sid_id)) {
+		return false;
+	}
+
+	if (!acquire_ib_buffer(
+		metadata ? cfg->metadata.bits_per_pixel :
+		cfg->input_port_resolution.bits_per_pixel,
+		metadata ? cfg->metadata.pixels_per_line :
+		cfg->input_port_resolution.pixels_per_line,
+		metadata ? cfg->metadata.lines_per_frame :
+		cfg->input_port_resolution.lines_per_frame,
+		metadata ? cfg->metadata.align_req_in_bytes :
+		cfg->input_port_resolution.align_req_in_bytes,
+		cfg->online,
+		&me->ib_buffer)) {
+		release_sid(me->stream2mmio_id, &me->stream2mmio_sid_id);
+		return false;
+	}
+
+	if (!acquire_dma_channel(me->dma_id, &me->dma_channel)) {
+		release_sid(me->stream2mmio_id, &me->stream2mmio_sid_id);
+		release_ib_buffer(&me->ib_buffer);
+		return false;
+	}
+
+	return true;
+}
+
+static void destroy_input_system_channel(
+    input_system_channel_t	*me)
+{
+	release_sid(me->stream2mmio_id,
+		    &me->stream2mmio_sid_id);
+
+	release_ib_buffer(&me->ib_buffer);
+
+	release_dma_channel(me->dma_id, &me->dma_channel);
+}
+
+static bool create_input_system_input_port(
+    isp2401_input_system_cfg_t		*cfg,
+    input_system_input_port_t	*me)
+{
+	csi_mipi_packet_type_t packet_type;
+	bool rc = true;
+
+	switch (cfg->input_port_id) {
+	case INPUT_SYSTEM_CSI_PORT0_ID:
+		me->csi_rx.frontend_id = CSI_RX_FRONTEND0_ID;
+		me->csi_rx.backend_id = CSI_RX_BACKEND0_ID;
+
+		packet_type = get_csi_mipi_packet_type(cfg->csi_port_attr.fmt_type);
+		me->csi_rx.packet_type = packet_type;
+
+		rc = acquire_be_lut_entry(
+			 me->csi_rx.backend_id,
+			 packet_type,
+			 &me->csi_rx.backend_lut_entry);
+		break;
+	case INPUT_SYSTEM_PIXELGEN_PORT0_ID:
+		me->pixelgen.pixelgen_id = PIXELGEN0_ID;
+		break;
+	case INPUT_SYSTEM_CSI_PORT1_ID:
+		me->csi_rx.frontend_id = CSI_RX_FRONTEND1_ID;
+		me->csi_rx.backend_id = CSI_RX_BACKEND1_ID;
+
+		packet_type = get_csi_mipi_packet_type(cfg->csi_port_attr.fmt_type);
+		me->csi_rx.packet_type = packet_type;
+
+		rc = acquire_be_lut_entry(
+			 me->csi_rx.backend_id,
+			 packet_type,
+			 &me->csi_rx.backend_lut_entry);
+		break;
+	case INPUT_SYSTEM_PIXELGEN_PORT1_ID:
+		me->pixelgen.pixelgen_id = PIXELGEN1_ID;
+
+		break;
+	case INPUT_SYSTEM_CSI_PORT2_ID:
+		me->csi_rx.frontend_id = CSI_RX_FRONTEND2_ID;
+		me->csi_rx.backend_id = CSI_RX_BACKEND2_ID;
+
+		packet_type = get_csi_mipi_packet_type(cfg->csi_port_attr.fmt_type);
+		me->csi_rx.packet_type = packet_type;
+
+		rc = acquire_be_lut_entry(
+			 me->csi_rx.backend_id,
+			 packet_type,
+			 &me->csi_rx.backend_lut_entry);
+		break;
+	case INPUT_SYSTEM_PIXELGEN_PORT2_ID:
+		me->pixelgen.pixelgen_id = PIXELGEN2_ID;
+		break;
+	default:
+		rc = false;
+		break;
+	}
+
+	me->source_type = cfg->mode;
+
+	/* for metadata */
+	me->metadata.packet_type = CSI_MIPI_PACKET_TYPE_UNDEFINED;
+	if (rc && cfg->metadata.enable) {
+		me->metadata.packet_type = get_csi_mipi_packet_type(
+					       cfg->metadata.fmt_type);
+		rc = acquire_be_lut_entry(
+			 me->csi_rx.backend_id,
+			 me->metadata.packet_type,
+			 &me->metadata.backend_lut_entry);
+	}
+
+	return rc;
+}
+
+static void destroy_input_system_input_port(
+    input_system_input_port_t	*me)
+{
+	if (me->source_type == INPUT_SYSTEM_SOURCE_TYPE_SENSOR) {
+		release_be_lut_entry(
+		    me->csi_rx.backend_id,
+		    me->csi_rx.packet_type,
+		    &me->csi_rx.backend_lut_entry);
+	}
+
+	if (me->metadata.packet_type != CSI_MIPI_PACKET_TYPE_UNDEFINED) {
+		/*Free the backend lut allocated for metadata*/
+		release_be_lut_entry(
+		    me->csi_rx.backend_id,
+		    me->metadata.packet_type,
+		    &me->metadata.backend_lut_entry);
+	}
+}
+
+static bool calculate_input_system_channel_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    input_system_channel_cfg_t	*channel_cfg,
+    bool metadata)
+{
+	bool rc;
+
+	rc = calculate_stream2mmio_cfg(isys_cfg, metadata,
+				       &channel_cfg->stream2mmio_cfg);
+	if (!rc)
+		return false;
+
+	rc = calculate_ibuf_ctrl_cfg(
+		 channel,
+		 input_port,
+		 isys_cfg,
+		 &channel_cfg->ibuf_ctrl_cfg);
+	if (!rc)
+		return false;
+	if (metadata)
+		channel_cfg->ibuf_ctrl_cfg.stores_per_frame =
+		    isys_cfg->metadata.lines_per_frame;
+
+	rc = calculate_isys2401_dma_cfg(
+		 channel,
+		 isys_cfg,
+		 &channel_cfg->dma_cfg);
+	if (!rc)
+		return false;
+
+	rc = calculate_isys2401_dma_port_cfg(
+		 isys_cfg,
+		 false,
+		 metadata,
+		 &channel_cfg->dma_src_port_cfg);
+	if (!rc)
+		return false;
+
+	rc = calculate_isys2401_dma_port_cfg(
+		 isys_cfg,
+		 isys_cfg->raw_packed,
+		 metadata,
+		 &channel_cfg->dma_dest_port_cfg);
+	if (!rc)
+		return false;
+
+	return true;
+}
+
+static bool calculate_input_system_input_port_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    input_system_input_port_cfg_t	*input_port_cfg)
+{
+	bool rc;
+
+	switch (input_port->source_type) {
+	case INPUT_SYSTEM_SOURCE_TYPE_SENSOR:
+		rc  = calculate_fe_cfg(
+			  isys_cfg,
+			  &input_port_cfg->csi_rx_cfg.frontend_cfg);
+
+		rc &= calculate_be_cfg(
+			  input_port,
+			  isys_cfg,
+			  false,
+			  &input_port_cfg->csi_rx_cfg.backend_cfg);
+
+		if (rc && isys_cfg->metadata.enable)
+			rc &= calculate_be_cfg(input_port, isys_cfg, true,
+					       &input_port_cfg->csi_rx_cfg.md_backend_cfg);
+		break;
+	case INPUT_SYSTEM_SOURCE_TYPE_TPG:
+		rc = calculate_tpg_cfg(
+			 channel,
+			 input_port,
+			 isys_cfg,
+			 &input_port_cfg->pixelgen_cfg.tpg_cfg);
+		break;
+	case INPUT_SYSTEM_SOURCE_TYPE_PRBS:
+		rc = calculate_prbs_cfg(
+			 channel,
+			 input_port,
+			 isys_cfg,
+			 &input_port_cfg->pixelgen_cfg.prbs_cfg);
+		break;
+	default:
+		rc = false;
+		break;
+	}
+
+	return rc;
+}
+
+static bool acquire_sid(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid)
+{
+	return ia_css_isys_stream2mmio_sid_rmgr_acquire(stream2mmio, sid);
+}
+
+static void release_sid(
+    stream2mmio_ID_t	stream2mmio,
+    stream2mmio_sid_ID_t	*sid)
+{
+	ia_css_isys_stream2mmio_sid_rmgr_release(stream2mmio, sid);
+}
+
+/* See also: ia_css_dma_configure_from_info() */
+static int32_t calculate_stride(
+    s32 bits_per_pixel,
+    s32 pixels_per_line,
+    bool	raw_packed,
+    int32_t align_in_bytes)
+{
+	s32 bytes_per_line;
+	s32 pixels_per_word;
+	s32 words_per_line;
+	s32 pixels_per_line_padded;
+
+	pixels_per_line_padded = CEIL_MUL(pixels_per_line, align_in_bytes);
+
+	if (!raw_packed)
+		bits_per_pixel = CEIL_MUL(bits_per_pixel, 8);
+
+	pixels_per_word = HIVE_ISP_DDR_WORD_BITS / bits_per_pixel;
+	words_per_line  = ceil_div(pixels_per_line_padded, pixels_per_word);
+	bytes_per_line  = HIVE_ISP_DDR_WORD_BYTES * words_per_line;
+
+	return bytes_per_line;
+}
+
+static bool acquire_ib_buffer(
+    s32 bits_per_pixel,
+    s32 pixels_per_line,
+    s32 lines_per_frame,
+    s32 align_in_bytes,
+    bool online,
+    isp2401_ib_buffer_t *buf)
+{
+	buf->stride = calculate_stride(bits_per_pixel, pixels_per_line, false,
+				       align_in_bytes);
+	if (online)
+		buf->lines = 4; /* use double buffering for online usecases */
+	else
+		buf->lines = 2;
+
+	(void)(lines_per_frame);
+	return ia_css_isys_ibuf_rmgr_acquire(buf->stride * buf->lines,
+					     &buf->start_addr);
+}
+
+static void release_ib_buffer(
+    isp2401_ib_buffer_t *buf)
+{
+	ia_css_isys_ibuf_rmgr_release(&buf->start_addr);
+}
+
+static bool acquire_dma_channel(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel)
+{
+	return ia_css_isys_dma_channel_rmgr_acquire(dma_id, channel);
+}
+
+static void release_dma_channel(
+    isys2401_dma_ID_t	dma_id,
+    isys2401_dma_channel	*channel)
+{
+	ia_css_isys_dma_channel_rmgr_release(dma_id, channel);
+}
+
+static bool acquire_be_lut_entry(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry)
+{
+	return ia_css_isys_csi_rx_lut_rmgr_acquire(backend, packet_type, entry);
+}
+
+static void release_be_lut_entry(
+    csi_rx_backend_ID_t		backend,
+    csi_mipi_packet_type_t		packet_type,
+    csi_rx_backend_lut_entry_t	*entry)
+{
+	ia_css_isys_csi_rx_lut_rmgr_release(backend, packet_type, entry);
+}
+
+static bool calculate_tpg_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    pixelgen_tpg_cfg_t		*cfg)
+{
+	memcpy(cfg, &isys_cfg->tpg_port_attr, sizeof(pixelgen_tpg_cfg_t));
+
+	return true;
+}
+
+static bool calculate_prbs_cfg(
+    input_system_channel_t		*channel,
+    input_system_input_port_t	*input_port,
+    isp2401_input_system_cfg_t		*isys_cfg,
+    pixelgen_prbs_cfg_t		*cfg)
+{
+	memcpy(cfg, &isys_cfg->prbs_port_attr, sizeof(pixelgen_prbs_cfg_t));
+
+	return true;
+}
+
+static bool calculate_fe_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    csi_rx_frontend_cfg_t		*cfg)
+{
+	cfg->active_lanes = isys_cfg->csi_port_attr.active_lanes;
+	return true;
+}
+
+static bool calculate_be_cfg(
+    const input_system_input_port_t	*input_port,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				metadata,
+    csi_rx_backend_cfg_t		*cfg)
+{
+	memcpy(&cfg->lut_entry,
+	      metadata ? &input_port->metadata.backend_lut_entry :
+			 &input_port->csi_rx.backend_lut_entry,
+	      sizeof(csi_rx_backend_lut_entry_t));
+
+	cfg->csi_mipi_cfg.virtual_channel = isys_cfg->csi_port_attr.ch_id;
+	if (metadata) {
+		cfg->csi_mipi_packet_type = get_csi_mipi_packet_type(
+						isys_cfg->metadata.fmt_type);
+		cfg->csi_mipi_cfg.comp_enable = false;
+		cfg->csi_mipi_cfg.data_type = isys_cfg->metadata.fmt_type;
+	} else {
+		cfg->csi_mipi_packet_type = get_csi_mipi_packet_type(
+						isys_cfg->csi_port_attr.fmt_type);
+		cfg->csi_mipi_cfg.data_type = isys_cfg->csi_port_attr.fmt_type;
+		cfg->csi_mipi_cfg.comp_enable = isys_cfg->csi_port_attr.comp_enable;
+		cfg->csi_mipi_cfg.comp_scheme = isys_cfg->csi_port_attr.comp_scheme;
+		cfg->csi_mipi_cfg.comp_predictor = isys_cfg->csi_port_attr.comp_predictor;
+		cfg->csi_mipi_cfg.comp_bit_idx = cfg->csi_mipi_cfg.data_type -
+						 MIPI_FORMAT_CUSTOM0;
+	}
+
+	return true;
+}
+
+static bool calculate_stream2mmio_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				metadata,
+    stream2mmio_cfg_t		*cfg
+)
+{
+	cfg->bits_per_pixel = metadata ? isys_cfg->metadata.bits_per_pixel :
+			      isys_cfg->input_port_resolution.bits_per_pixel;
+
+	cfg->enable_blocking =
+	    ((isys_cfg->mode == INPUT_SYSTEM_SOURCE_TYPE_TPG) ||
+	     (isys_cfg->mode == INPUT_SYSTEM_SOURCE_TYPE_PRBS));
+
+	return true;
+}
+
+static bool calculate_ibuf_ctrl_cfg(
+    const input_system_channel_t	*channel,
+    const input_system_input_port_t	*input_port,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    ibuf_ctrl_cfg_t			*cfg)
+{
+	const s32 bits_per_byte = 8;
+	s32 bits_per_pixel;
+	s32 bytes_per_pixel;
+	s32 left_padding;
+
+	(void)input_port;
+
+	bits_per_pixel = isys_cfg->input_port_resolution.bits_per_pixel;
+	bytes_per_pixel = ceil_div(bits_per_pixel, bits_per_byte);
+
+	left_padding = CEIL_MUL(isys_cfg->output_port_attr.left_padding, ISP_VEC_NELEMS)
+		       * bytes_per_pixel;
+
+	cfg->online	= isys_cfg->online;
+
+	cfg->dma_cfg.channel	= channel->dma_channel;
+	cfg->dma_cfg.cmd	= _DMA_V2_MOVE_A2B_NO_SYNC_CHK_COMMAND;
+
+	cfg->dma_cfg.shift_returned_items	= 0;
+	cfg->dma_cfg.elems_per_word_in_ibuf	= 0;
+	cfg->dma_cfg.elems_per_word_in_dest	= 0;
+
+	cfg->ib_buffer.start_addr		= channel->ib_buffer.start_addr;
+	cfg->ib_buffer.stride			= channel->ib_buffer.stride;
+	cfg->ib_buffer.lines			= channel->ib_buffer.lines;
+
+	/*
+	#ifndef ISP2401
+	 * zhengjie.lu@intel.com:
+	#endif
+	 * "dest_buf_cfg" should be part of the input system output
+	 * port configuration.
+	 *
+	 * TODO: move "dest_buf_cfg" to the input system output
+	 * port configuration.
+	 */
+
+	/* input_buf addr only available in sched mode;
+	   this buffer is allocated in isp, crun mode addr
+	   can be passed by after ISP allocation */
+	if (cfg->online) {
+		cfg->dest_buf_cfg.start_addr	= ISP_INPUT_BUF_START_ADDR + left_padding;
+		cfg->dest_buf_cfg.stride	= bytes_per_pixel
+					      * isys_cfg->output_port_attr.max_isp_input_width;
+		cfg->dest_buf_cfg.lines		= LINES_OF_ISP_INPUT_BUF;
+	} else if (isys_cfg->raw_packed) {
+		cfg->dest_buf_cfg.stride	= calculate_stride(bits_per_pixel,
+					      isys_cfg->input_port_resolution.pixels_per_line,
+					      isys_cfg->raw_packed,
+					      isys_cfg->input_port_resolution.align_req_in_bytes);
+	} else {
+		cfg->dest_buf_cfg.stride	= channel->ib_buffer.stride;
+	}
+
+	/*
+	#ifndef ISP2401
+	 * zhengjie.lu@intel.com:
+	#endif
+	 * "items_per_store" is hard coded as "1", which is ONLY valid
+	 * when the CSI-MIPI long packet is transferred.
+	 *
+	 * TODO: After the 1st stage of MERR+,  make the proper solution to
+	 * configure "items_per_store" so that it can also handle the CSI-MIPI
+	 * short packet.
+	 */
+	cfg->items_per_store		= 1;
+
+	cfg->stores_per_frame		= isys_cfg->input_port_resolution.lines_per_frame;
+
+	cfg->stream2mmio_cfg.sync_cmd	= _STREAM2MMIO_CMD_TOKEN_SYNC_FRAME;
+
+	/* TODO: Define conditions as when to use store words vs store packets */
+	cfg->stream2mmio_cfg.store_cmd	= _STREAM2MMIO_CMD_TOKEN_STORE_PACKETS;
+
+	return true;
+}
+
+static bool calculate_isys2401_dma_cfg(
+    const input_system_channel_t	*channel,
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    isys2401_dma_cfg_t		*cfg)
+{
+	cfg->channel	= channel->dma_channel;
+
+	/* only online/sensor mode goto vmem
+	   offline/buffered_sensor, tpg and prbs will go to ddr */
+	if (isys_cfg->online)
+		cfg->connection = isys2401_dma_ibuf_to_vmem_connection;
+	else
+		cfg->connection = isys2401_dma_ibuf_to_ddr_connection;
+
+	cfg->extension	= isys2401_dma_zero_extension;
+	cfg->height	= 1;
+
+	return true;
+}
+
+/* See also: ia_css_dma_configure_from_info() */
+static bool calculate_isys2401_dma_port_cfg(
+    const isp2401_input_system_cfg_t	*isys_cfg,
+    bool				raw_packed,
+    bool				metadata,
+    isys2401_dma_port_cfg_t		*cfg)
+{
+	s32 bits_per_pixel;
+	s32 pixels_per_line;
+	s32 align_req_in_bytes;
+
+	/* TODO: Move metadata away from isys_cfg to application layer */
+	if (metadata) {
+		bits_per_pixel = isys_cfg->metadata.bits_per_pixel;
+		pixels_per_line = isys_cfg->metadata.pixels_per_line;
+		align_req_in_bytes = isys_cfg->metadata.align_req_in_bytes;
+	} else {
+		bits_per_pixel = isys_cfg->input_port_resolution.bits_per_pixel;
+		pixels_per_line = isys_cfg->input_port_resolution.pixels_per_line;
+		align_req_in_bytes = isys_cfg->input_port_resolution.align_req_in_bytes;
+	}
+
+	cfg->stride	= calculate_stride(bits_per_pixel, pixels_per_line, raw_packed,
+				       align_req_in_bytes);
+
+	if (!raw_packed)
+		bits_per_pixel = CEIL_MUL(bits_per_pixel, 8);
+
+	cfg->elements	= HIVE_ISP_DDR_WORD_BITS / bits_per_pixel;
+	cfg->cropping	= 0;
+	cfg->width	= CEIL_DIV(cfg->stride, HIVE_ISP_DDR_WORD_BYTES);
+
+	return true;
+}
+
+static csi_mipi_packet_type_t get_csi_mipi_packet_type(
+    int32_t data_type)
+{
+	csi_mipi_packet_type_t packet_type;
+
+	packet_type = CSI_MIPI_PACKET_TYPE_RESERVED;
+
+	if (data_type >= 0 && data_type <= MIPI_FORMAT_SHORT8)
+		packet_type = CSI_MIPI_PACKET_TYPE_SHORT;
+
+	if (data_type > MIPI_FORMAT_SHORT8 && data_type <= N_MIPI_FORMAT)
+		packet_type = CSI_MIPI_PACKET_TYPE_LONG;
+
+	return packet_type;
+}
+
+/* end of Private Methods */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.h b/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.h
new file mode 100644
index 000000000..fbdbca0cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/isys/src/virtual_isys.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __VIRTUAL_ISYS_H_INCLUDED__
+#define __VIRTUAL_ISYS_H_INCLUDED__
+
+/* cmd for storing a number of packets indicated by reg _STREAM2MMIO_NUM_ITEMS*/
+#define _STREAM2MMIO_CMD_TOKEN_STORE_PACKETS	1
+
+/* command for waiting for a frame start */
+#define _STREAM2MMIO_CMD_TOKEN_SYNC_FRAME	2
+
+#endif /* __VIRTUAL_ISYS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline.h b/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline.h
new file mode 100644
index 000000000..de2c526a5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline.h
@@ -0,0 +1,286 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPELINE_H__
+#define __IA_CSS_PIPELINE_H__
+
+#include "sh_css_internal.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_pipeline_common.h"
+
+#define IA_CSS_PIPELINE_NUM_MAX		(20)
+
+/* Pipeline stage to be executed on SP/ISP */
+struct ia_css_pipeline_stage {
+	unsigned int stage_num;
+	struct ia_css_binary *binary;	/* built-in binary */
+	struct ia_css_binary_info *binary_info;
+	const struct ia_css_fw_info *firmware;	/* acceleration binary */
+	/* SP function for SP stage */
+	enum ia_css_pipeline_stage_sp_func sp_func;
+	unsigned int max_input_width;	/* For SP raw copy */
+	struct sh_css_binary_args args;
+	int mode;
+	bool out_frame_allocated[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	bool vf_frame_allocated;
+	struct ia_css_pipeline_stage *next;
+	bool enable_zoom;
+};
+
+/* Pipeline of n stages to be executed on SP/ISP per stage */
+struct ia_css_pipeline {
+	enum ia_css_pipe_id pipe_id;
+	u8 pipe_num;
+	bool stop_requested;
+	struct ia_css_pipeline_stage *stages;
+	struct ia_css_pipeline_stage *current_stage;
+	unsigned int num_stages;
+	struct ia_css_frame in_frame;
+	struct ia_css_frame out_frame[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame vf_frame[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	unsigned int dvs_frame_delay;
+	unsigned int inout_port_config;
+	int num_execs;
+	bool acquire_isp_each_stage;
+	u32 pipe_qos_config;
+};
+
+#define DEFAULT_PIPELINE { \
+	.pipe_id		= IA_CSS_PIPE_ID_PREVIEW, \
+	.in_frame		= DEFAULT_FRAME, \
+	.out_frame		= {DEFAULT_FRAME}, \
+	.vf_frame		= {DEFAULT_FRAME}, \
+	.dvs_frame_delay	= IA_CSS_FRAME_DELAY_1, \
+	.num_execs		= -1, \
+	.acquire_isp_each_stage	= true, \
+	.pipe_qos_config	= QOS_INVALID \
+}
+
+/* Stage descriptor used to create a new stage in the pipeline */
+struct ia_css_pipeline_stage_desc {
+	struct ia_css_binary *binary;
+	const struct ia_css_fw_info *firmware;
+	enum ia_css_pipeline_stage_sp_func sp_func;
+	unsigned int max_input_width;
+	unsigned int mode;
+	struct ia_css_frame *in_frame;
+	struct ia_css_frame *out_frame[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame *vf_frame;
+};
+
+/* @brief initialize the pipeline module
+ *
+ * @return    None
+ *
+ * Initializes the pipeline module. This API has to be called
+ * before any operation on the pipeline module is done
+ */
+void ia_css_pipeline_init(void);
+
+/* @brief initialize the pipeline structure with default values
+ *
+ * @param[out] pipeline  structure to be initialized with defaults
+ * @param[in] pipe_id
+ * @param[in] pipe_num Number that uniquely identifies a pipeline.
+ * @return                     0 or error code upon error.
+ *
+ * Initializes the pipeline structure with a set of default values.
+ * This API is expected to be used when a pipeline structure is allocated
+ * externally and needs sane defaults
+ */
+int ia_css_pipeline_create(
+    struct ia_css_pipeline *pipeline,
+    enum ia_css_pipe_id pipe_id,
+    unsigned int pipe_num,
+    unsigned int dvs_frame_delay);
+
+/* @brief destroy a pipeline
+ *
+ * @param[in] pipeline
+ * @return    None
+ *
+ */
+void ia_css_pipeline_destroy(struct ia_css_pipeline *pipeline);
+
+/* @brief Starts a pipeline
+ *
+ * @param[in] pipe_id
+ * @param[in] pipeline
+ * @return    None
+ *
+ */
+void ia_css_pipeline_start(enum ia_css_pipe_id pipe_id,
+			   struct ia_css_pipeline *pipeline);
+
+/* @brief Request to stop a pipeline
+ *
+ * @param[in] pipeline
+ * @return                     0 or error code upon error.
+ *
+ */
+int ia_css_pipeline_request_stop(struct ia_css_pipeline *pipeline);
+
+/* @brief Check whether pipeline has stopped
+ *
+ * @param[in] pipeline
+ * @return    true if the pipeline has stopped
+ *
+ */
+bool ia_css_pipeline_has_stopped(struct ia_css_pipeline *pipe);
+
+/* @brief clean all the stages pipeline and make it as new
+ *
+ * @param[in] pipeline
+ * @return    None
+ *
+ */
+void ia_css_pipeline_clean(struct ia_css_pipeline *pipeline);
+
+/* @brief Add a stage to pipeline.
+ *
+ * @param     pipeline               Pointer to the pipeline to be added to.
+ * @param[in] stage_desc       The description of the stage
+ * @param[out] stage            The successor of the stage.
+ * @return                     0 or error code upon error.
+ *
+ * Add a new stage to a non-NULL pipeline.
+ * The stage consists of an ISP binary or firmware and input and output
+ * arguments.
+*/
+int ia_css_pipeline_create_and_add_stage(
+    struct ia_css_pipeline *pipeline,
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_pipeline_stage **stage);
+
+/* @brief Finalize the stages in a pipeline
+ *
+ * @param     pipeline               Pointer to the pipeline to be added to.
+ * @return                     None
+ *
+ * This API is expected to be called after adding all stages
+*/
+void ia_css_pipeline_finalize_stages(struct ia_css_pipeline *pipeline,
+				     bool continuous);
+
+/* @brief gets a stage from the pipeline
+ *
+ * @param[in] pipeline
+ * @return                     0 or error code upon error.
+ *
+ */
+int ia_css_pipeline_get_stage(struct ia_css_pipeline *pipeline,
+	int mode,
+	struct ia_css_pipeline_stage **stage);
+
+/* @brief Gets a pipeline stage corresponding Firmware handle from the pipeline
+ *
+ * @param[in] pipeline
+ * @param[in] fw_handle
+ * @param[out] stage Pointer to Stage
+ *
+ * @return   0 or error code upon error.
+ *
+ */
+int ia_css_pipeline_get_stage_from_fw(struct ia_css_pipeline
+	*pipeline,
+	u32 fw_handle,
+	struct ia_css_pipeline_stage **stage);
+
+/* @brief Gets the Firmware handle corresponding the stage num from the pipeline
+ *
+ * @param[in] pipeline
+ * @param[in] stage_num
+ * @param[out] fw_handle
+ *
+ * @return   0 or error code upon error.
+ *
+ */
+int ia_css_pipeline_get_fw_from_stage(struct ia_css_pipeline
+	*pipeline,
+	u32 stage_num,
+	uint32_t *fw_handle);
+
+/* @brief gets the output stage from the pipeline
+ *
+ * @param[in] pipeline
+ * @return                     0 or error code upon error.
+ *
+ */
+int ia_css_pipeline_get_output_stage(
+    struct ia_css_pipeline *pipeline,
+    int mode,
+    struct ia_css_pipeline_stage **stage);
+
+/* @brief Checks whether the pipeline uses params
+ *
+ * @param[in] pipeline
+ * @return    true if the pipeline uses params
+ *
+ */
+bool ia_css_pipeline_uses_params(struct ia_css_pipeline *pipeline);
+
+/**
+ * @brief get the SP thread ID.
+ *
+ * @param[in]	key	The query key, typical use is pipe_num.
+ * @param[out]	val	The query value.
+ *
+ * @return
+ *	true, if the query succeeds;
+ *	false, if the query fails.
+ */
+bool ia_css_pipeline_get_sp_thread_id(unsigned int key, unsigned int *val);
+
+#if defined(ISP2401)
+/**
+ * @brief Get the pipeline io status
+ *
+ * @param[in] None
+ * @return
+ *	Pointer to pipe_io_status
+ */
+struct sh_css_sp_pipeline_io_status *ia_css_pipeline_get_pipe_io_status(void);
+#endif
+
+/**
+ * @brief Map an SP thread to this pipeline
+ *
+ * @param[in]	pipe_num
+ * @param[in]	map true for mapping and false for unmapping sp threads.
+ *
+ */
+void ia_css_pipeline_map(unsigned int pipe_num, bool map);
+
+/**
+ * @brief Checks whether the pipeline is mapped to SP threads
+ *
+ * @param[in]	Query key, typical use is pipe_num
+ *
+ * return
+ *	true, pipeline is mapped to SP threads
+ *	false, pipeline is not mapped to SP threads
+ */
+bool ia_css_pipeline_is_mapped(unsigned int key);
+
+/**
+ * @brief Print pipeline thread mapping
+ *
+ * @param[in]	none
+ *
+ * return none
+ */
+void ia_css_pipeline_dump_thread_map_info(void);
+
+#endif /*__IA_CSS_PIPELINE_H__*/
diff --git a/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline_common.h b/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline_common.h
new file mode 100644
index 000000000..cc44f03c3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/pipeline/interface/ia_css_pipeline_common.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_PIPELINE_COMMON_H__
+#define __IA_CSS_PIPELINE_COMMON_H__
+
+enum ia_css_pipeline_stage_sp_func {
+	IA_CSS_PIPELINE_RAW_COPY = 0,
+	IA_CSS_PIPELINE_BIN_COPY = 1,
+	IA_CSS_PIPELINE_ISYS_COPY = 2,
+	IA_CSS_PIPELINE_NO_FUNC = 3,
+};
+
+#define IA_CSS_PIPELINE_NUM_STAGE_FUNCS 3
+
+#endif /*__IA_CSS_PIPELINE_COMMON_H__*/
diff --git a/drivers/staging/media/atomisp/pci/runtime/pipeline/src/pipeline.c b/drivers/staging/media/atomisp/pci/runtime/pipeline/src/pipeline.c
new file mode 100644
index 000000000..dfc50247e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/pipeline/src/pipeline.c
@@ -0,0 +1,791 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_debug.h"
+#include "sw_event_global.h"		/* encode_sw_event */
+#include "sp.h"			/* cnd_sp_irq_enable() */
+#include "assert_support.h"
+#include "sh_css_sp.h"
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_param.h"
+#include "ia_css_bufq.h"
+
+#define PIPELINE_NUM_UNMAPPED                   (~0U)
+#define PIPELINE_SP_THREAD_EMPTY_TOKEN          (0x0)
+#define PIPELINE_SP_THREAD_RESERVED_TOKEN       (0x1)
+
+/*******************************************************
+*** Static variables
+********************************************************/
+static unsigned int pipeline_num_to_sp_thread_map[IA_CSS_PIPELINE_NUM_MAX];
+static unsigned int pipeline_sp_thread_list[SH_CSS_MAX_SP_THREADS];
+
+/*******************************************************
+*** Static functions
+********************************************************/
+static void pipeline_init_sp_thread_map(void);
+static void pipeline_map_num_to_sp_thread(unsigned int pipe_num);
+static void pipeline_unmap_num_to_sp_thread(unsigned int pipe_num);
+static void pipeline_init_defaults(
+    struct ia_css_pipeline *pipeline,
+    enum ia_css_pipe_id pipe_id,
+    unsigned int pipe_num,
+    unsigned int dvs_frame_delay);
+
+static void pipeline_stage_destroy(struct ia_css_pipeline_stage *stage);
+static int pipeline_stage_create(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_pipeline_stage **new_stage);
+static void ia_css_pipeline_set_zoom_stage(struct ia_css_pipeline *pipeline);
+static void ia_css_pipeline_configure_inout_port(struct ia_css_pipeline *me,
+	bool continuous);
+
+/*******************************************************
+*** Public functions
+********************************************************/
+void ia_css_pipeline_init(void)
+{
+	pipeline_init_sp_thread_map();
+}
+
+int ia_css_pipeline_create(
+    struct ia_css_pipeline *pipeline,
+    enum ia_css_pipe_id pipe_id,
+    unsigned int pipe_num,
+    unsigned int dvs_frame_delay)
+{
+	assert(pipeline);
+	IA_CSS_ENTER_PRIVATE("pipeline = %p, pipe_id = %d, pipe_num = %d, dvs_frame_delay = %d",
+			     pipeline, pipe_id, pipe_num, dvs_frame_delay);
+	if (!pipeline) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipeline_init_defaults(pipeline, pipe_id, pipe_num, dvs_frame_delay);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+void ia_css_pipeline_map(unsigned int pipe_num, bool map)
+{
+	assert(pipe_num < IA_CSS_PIPELINE_NUM_MAX);
+	IA_CSS_ENTER_PRIVATE("pipe_num = %d, map = %d", pipe_num, map);
+
+	if (pipe_num >= IA_CSS_PIPELINE_NUM_MAX) {
+		IA_CSS_ERROR("Invalid pipe number");
+		IA_CSS_LEAVE_PRIVATE("void");
+		return;
+	}
+	if (map)
+		pipeline_map_num_to_sp_thread(pipe_num);
+	else
+		pipeline_unmap_num_to_sp_thread(pipe_num);
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/* @brief destroy a pipeline
+ *
+ * @param[in] pipeline
+ * @return    None
+ *
+ */
+void ia_css_pipeline_destroy(struct ia_css_pipeline *pipeline)
+{
+	assert(pipeline);
+	IA_CSS_ENTER_PRIVATE("pipeline = %p", pipeline);
+
+	if (!pipeline) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("void");
+		return;
+	}
+
+	IA_CSS_LOG("pipe_num = %d", pipeline->pipe_num);
+
+	/* Free the pipeline number */
+	ia_css_pipeline_clean(pipeline);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/* Run a pipeline and wait till it completes. */
+void ia_css_pipeline_start(enum ia_css_pipe_id pipe_id,
+			   struct ia_css_pipeline *pipeline)
+{
+	u8 pipe_num = 0;
+	unsigned int thread_id;
+
+	assert(pipeline);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_start() enter: pipe_id=%d, pipeline=%p\n",
+			    pipe_id, pipeline);
+	pipeline->pipe_id = pipe_id;
+	sh_css_sp_init_pipeline(pipeline, pipe_id, pipe_num,
+				false, false, false, true, SH_CSS_BDS_FACTOR_1_00,
+				SH_CSS_PIPE_CONFIG_OVRD_NO_OVRD,
+				IA_CSS_INPUT_MODE_MEMORY, NULL, NULL,
+				(enum mipi_port_id)0);
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	if (!sh_css_sp_is_running()) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_pipeline_start() error,leaving\n");
+		/* queues are invalid*/
+		return;
+	}
+	ia_css_bufq_enqueue_psys_event(IA_CSS_PSYS_SW_EVENT_START_STREAM,
+				       (uint8_t)thread_id,
+				       0,
+				       0);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_start() leave: return_void\n");
+}
+
+/*
+ * @brief Query the SP thread ID.
+ * Refer to "sh_css_internal.h" for details.
+ */
+bool ia_css_pipeline_get_sp_thread_id(unsigned int key, unsigned int *val)
+{
+	IA_CSS_ENTER("key=%d, val=%p", key, val);
+
+	if ((!val) || (key >= IA_CSS_PIPELINE_NUM_MAX) || (key >= IA_CSS_PIPE_ID_NUM)) {
+		IA_CSS_LEAVE("return value = false");
+		return false;
+	}
+
+	*val = pipeline_num_to_sp_thread_map[key];
+
+	if (*val == (unsigned int)PIPELINE_NUM_UNMAPPED) {
+		IA_CSS_LOG("unmapped pipeline number");
+		IA_CSS_LEAVE("return value = false");
+		return false;
+	}
+	IA_CSS_LEAVE("return value = true");
+	return true;
+}
+
+void ia_css_pipeline_dump_thread_map_info(void)
+{
+	unsigned int i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "pipeline_num_to_sp_thread_map:\n");
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "pipe_num: %u, tid: 0x%x\n", i, pipeline_num_to_sp_thread_map[i]);
+	}
+}
+
+int ia_css_pipeline_request_stop(struct ia_css_pipeline *pipeline)
+{
+	int err = 0;
+	unsigned int thread_id;
+
+	assert(pipeline);
+
+	if (!pipeline)
+		return -EINVAL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_request_stop() enter: pipeline=%p\n",
+			    pipeline);
+	pipeline->stop_requested = true;
+
+	/* Send stop event to the sp*/
+	/* This needs improvement, stop on all the pipes available
+	 * in the stream*/
+	ia_css_pipeline_get_sp_thread_id(pipeline->pipe_num, &thread_id);
+	if (!sh_css_sp_is_running()) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_pipeline_request_stop() leaving\n");
+		/* queues are invalid */
+		return -EBUSY;
+	}
+	ia_css_bufq_enqueue_psys_event(IA_CSS_PSYS_SW_EVENT_STOP_STREAM,
+				       (uint8_t)thread_id,
+				       0,
+				       0);
+	sh_css_sp_uninit_pipeline(pipeline->pipe_num);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_request_stop() leave: return_err=%d\n",
+			    err);
+	return err;
+}
+
+void ia_css_pipeline_clean(struct ia_css_pipeline *pipeline)
+{
+	struct ia_css_pipeline_stage *s;
+
+	assert(pipeline);
+	IA_CSS_ENTER_PRIVATE("pipeline = %p", pipeline);
+
+	if (!pipeline) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("void");
+		return;
+	}
+	s = pipeline->stages;
+
+	while (s) {
+		struct ia_css_pipeline_stage *next = s->next;
+
+		pipeline_stage_destroy(s);
+		s = next;
+	}
+	pipeline_init_defaults(pipeline, pipeline->pipe_id, pipeline->pipe_num,
+			       pipeline->dvs_frame_delay);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/* @brief Add a stage to pipeline.
+ *
+ * @param       pipeline      Pointer to the pipeline to be added to.
+ * @param[in]   stage_desc    The description of the stage
+ * @param[out]	stage         The successor of the stage.
+ * @return      0 or error code upon error.
+ *
+ * Add a new stage to a non-NULL pipeline.
+ * The stage consists of an ISP binary or firmware and input and
+ * output arguments.
+*/
+int ia_css_pipeline_create_and_add_stage(
+    struct ia_css_pipeline *pipeline,
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_pipeline_stage **stage)
+{
+	struct ia_css_pipeline_stage *last, *new_stage = NULL;
+	int err;
+
+	/* other arguments can be NULL */
+	assert(pipeline);
+	assert(stage_desc);
+	last = pipeline->stages;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_create_and_add_stage() enter:\n");
+	if (!stage_desc->binary && !stage_desc->firmware
+	    && (stage_desc->sp_func == IA_CSS_PIPELINE_NO_FUNC)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_pipeline_create_and_add_stage() done: Invalid args\n");
+
+		return -EINVAL;
+	}
+
+	/* Find the last stage */
+	while (last && last->next)
+		last = last->next;
+
+	/* if in_frame is not set, we use the out_frame from the previous
+	 * stage, if no previous stage, it's an error.
+	 */
+	if ((stage_desc->sp_func == IA_CSS_PIPELINE_NO_FUNC)
+	    && (!stage_desc->in_frame)
+	    && (!stage_desc->firmware)
+	    && (!stage_desc->binary->online)) {
+		/* Do this only for ISP stages*/
+		if (last && last->args.out_frame[0])
+			stage_desc->in_frame = last->args.out_frame[0];
+
+		if (!stage_desc->in_frame)
+			return -EINVAL;
+	}
+
+	/* Create the new stage */
+	err = pipeline_stage_create(stage_desc, &new_stage);
+	if (err) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_pipeline_create_and_add_stage() done: stage_create_failed\n");
+		return err;
+	}
+
+	if (last)
+		last->next = new_stage;
+	else
+		pipeline->stages = new_stage;
+
+	/* Output the new stage */
+	if (stage)
+		*stage = new_stage;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_create_and_add_stage() done:\n");
+	return 0;
+}
+
+void ia_css_pipeline_finalize_stages(struct ia_css_pipeline *pipeline,
+				     bool continuous)
+{
+	unsigned int i = 0;
+	struct ia_css_pipeline_stage *stage;
+
+	assert(pipeline);
+	for (stage = pipeline->stages; stage; stage = stage->next) {
+		stage->stage_num = i;
+		i++;
+	}
+	pipeline->num_stages = i;
+
+	ia_css_pipeline_set_zoom_stage(pipeline);
+	ia_css_pipeline_configure_inout_port(pipeline, continuous);
+}
+
+int ia_css_pipeline_get_stage(struct ia_css_pipeline *pipeline,
+	int mode,
+	struct ia_css_pipeline_stage **stage)
+{
+	struct ia_css_pipeline_stage *s;
+
+	assert(pipeline);
+	assert(stage);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_get_stage() enter:\n");
+	for (s = pipeline->stages; s; s = s->next) {
+		if (s->mode == mode) {
+			*stage = s;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+int ia_css_pipeline_get_stage_from_fw(struct ia_css_pipeline
+	*pipeline,
+	u32 fw_handle,
+	struct ia_css_pipeline_stage **stage)
+{
+	struct ia_css_pipeline_stage *s;
+
+	assert(pipeline);
+	assert(stage);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	for (s = pipeline->stages; s; s = s->next) {
+		if ((s->firmware) && (s->firmware->handle == fw_handle)) {
+			*stage = s;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+int ia_css_pipeline_get_fw_from_stage(struct ia_css_pipeline
+	*pipeline,
+	u32 stage_num,
+	uint32_t *fw_handle)
+{
+	struct ia_css_pipeline_stage *s;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s()\n", __func__);
+	if ((!pipeline) || (!fw_handle))
+		return -EINVAL;
+
+	for (s = pipeline->stages; s; s = s->next) {
+		if ((s->stage_num == stage_num) && (s->firmware)) {
+			*fw_handle = s->firmware->handle;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+int ia_css_pipeline_get_output_stage(
+    struct ia_css_pipeline *pipeline,
+    int mode,
+    struct ia_css_pipeline_stage **stage)
+{
+	struct ia_css_pipeline_stage *s;
+
+	assert(pipeline);
+	assert(stage);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_get_output_stage() enter:\n");
+
+	*stage = NULL;
+	/* First find acceleration firmware at end of pipe */
+	for (s = pipeline->stages; s; s = s->next) {
+		if (s->firmware && s->mode == mode &&
+		    s->firmware->info.isp.sp.enable.output)
+			*stage = s;
+	}
+	if (*stage)
+		return 0;
+	/* If no firmware, find binary in pipe */
+	return ia_css_pipeline_get_stage(pipeline, mode, stage);
+}
+
+bool ia_css_pipeline_has_stopped(struct ia_css_pipeline *pipeline)
+{
+	/* Android compilation files if made an local variable
+	stack size on android is limited to 2k and this structure
+	is around 2.5K, in place of static malloc can be done but
+	if this call is made too often it will lead to fragment memory
+	versus a fixed allocation */
+	static struct sh_css_sp_group sp_group;
+	unsigned int thread_id;
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_sp_group;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_group = fw->info.sp.group;
+
+	ia_css_pipeline_get_sp_thread_id(pipeline->pipe_num, &thread_id);
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(sp_group),
+		     &sp_group, sizeof(struct sh_css_sp_group));
+	return sp_group.pipe[thread_id].num_stages == 0;
+}
+
+#if defined(ISP2401)
+struct sh_css_sp_pipeline_io_status *ia_css_pipeline_get_pipe_io_status(void)
+{
+	return(&sh_css_sp_group.pipe_io_status);
+}
+#endif
+
+bool ia_css_pipeline_is_mapped(unsigned int key)
+{
+	bool ret = false;
+
+	IA_CSS_ENTER_PRIVATE("key = %d", key);
+
+	if ((key >= IA_CSS_PIPELINE_NUM_MAX) || (key >= IA_CSS_PIPE_ID_NUM)) {
+		IA_CSS_ERROR("Invalid key!!");
+		IA_CSS_LEAVE_PRIVATE("return = %d", false);
+		return false;
+	}
+
+	ret = (bool)(pipeline_num_to_sp_thread_map[key] != (unsigned int)
+		     PIPELINE_NUM_UNMAPPED);
+
+	IA_CSS_LEAVE_PRIVATE("return = %d", ret);
+	return ret;
+}
+
+/*******************************************************
+*** Static functions
+********************************************************/
+
+/* Pipeline:
+ * To organize the several different binaries for each type of mode,
+ * we use a pipeline. A pipeline contains a number of stages, each with
+ * their own binary and frame pointers.
+ * When stages are added to a pipeline, output frames that are not passed
+ * from outside are automatically allocated.
+ * When input frames are not passed from outside, each stage will use the
+ * output frame of the previous stage as input (the full resolution output,
+ * not the viewfinder output).
+ * Pipelines must be cleaned and re-created when settings of the binaries
+ * change.
+ */
+static void pipeline_stage_destroy(struct ia_css_pipeline_stage *stage)
+{
+	unsigned int i;
+
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		if (stage->out_frame_allocated[i]) {
+			ia_css_frame_free(stage->args.out_frame[i]);
+			stage->args.out_frame[i] = NULL;
+		}
+	}
+	if (stage->vf_frame_allocated) {
+		ia_css_frame_free(stage->args.out_vf_frame);
+		stage->args.out_vf_frame = NULL;
+	}
+	kvfree(stage);
+}
+
+static void pipeline_init_sp_thread_map(void)
+{
+	unsigned int i;
+
+	for (i = 1; i < SH_CSS_MAX_SP_THREADS; i++)
+		pipeline_sp_thread_list[i] = PIPELINE_SP_THREAD_EMPTY_TOKEN;
+
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++)
+		pipeline_num_to_sp_thread_map[i] = PIPELINE_NUM_UNMAPPED;
+}
+
+static void pipeline_map_num_to_sp_thread(unsigned int pipe_num)
+{
+	unsigned int i;
+	bool found_sp_thread = false;
+
+	/* pipe is not mapped to any thread */
+	assert(pipeline_num_to_sp_thread_map[pipe_num]
+	       == (unsigned int)PIPELINE_NUM_UNMAPPED);
+
+	for (i = 0; i < SH_CSS_MAX_SP_THREADS; i++) {
+		if (pipeline_sp_thread_list[i] ==
+		    PIPELINE_SP_THREAD_EMPTY_TOKEN) {
+			pipeline_sp_thread_list[i] =
+			    PIPELINE_SP_THREAD_RESERVED_TOKEN;
+			pipeline_num_to_sp_thread_map[pipe_num] = i;
+			found_sp_thread = true;
+			break;
+		}
+	}
+
+	/* Make sure a mapping is found */
+	/* I could do:
+		assert(i < SH_CSS_MAX_SP_THREADS);
+
+		But the below is more descriptive.
+	*/
+	assert(found_sp_thread);
+}
+
+static void pipeline_unmap_num_to_sp_thread(unsigned int pipe_num)
+{
+	unsigned int thread_id;
+
+	assert(pipeline_num_to_sp_thread_map[pipe_num]
+	       != (unsigned int)PIPELINE_NUM_UNMAPPED);
+
+	thread_id = pipeline_num_to_sp_thread_map[pipe_num];
+	pipeline_num_to_sp_thread_map[pipe_num] = PIPELINE_NUM_UNMAPPED;
+	pipeline_sp_thread_list[thread_id] = PIPELINE_SP_THREAD_EMPTY_TOKEN;
+}
+
+static int pipeline_stage_create(
+    struct ia_css_pipeline_stage_desc *stage_desc,
+    struct ia_css_pipeline_stage **new_stage)
+{
+	int err = 0;
+	struct ia_css_pipeline_stage *stage = NULL;
+	struct ia_css_binary *binary;
+	struct ia_css_frame *vf_frame;
+	struct ia_css_frame *out_frame[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	const struct ia_css_fw_info *firmware;
+	unsigned int i;
+
+	/* Verify input parameters*/
+	if (!(stage_desc->in_frame) && !(stage_desc->firmware)
+	    && (stage_desc->binary) && !(stage_desc->binary->online)) {
+		err = -EINVAL;
+		goto ERR;
+	}
+
+	binary = stage_desc->binary;
+	firmware = stage_desc->firmware;
+	vf_frame = stage_desc->vf_frame;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		out_frame[i] = stage_desc->out_frame[i];
+	}
+
+	stage = kvzalloc(sizeof(*stage), GFP_KERNEL);
+	if (!stage) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+
+	if (firmware) {
+		stage->binary = NULL;
+		stage->binary_info =
+		    (struct ia_css_binary_info *)&firmware->info.isp;
+	} else {
+		stage->binary = binary;
+		if (binary)
+			stage->binary_info =
+			    (struct ia_css_binary_info *)binary->info;
+		else
+			stage->binary_info = NULL;
+	}
+
+	stage->firmware = firmware;
+	stage->sp_func = stage_desc->sp_func;
+	stage->max_input_width = stage_desc->max_input_width;
+	stage->mode = stage_desc->mode;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		stage->out_frame_allocated[i] = false;
+	stage->vf_frame_allocated = false;
+	stage->next = NULL;
+	sh_css_binary_args_reset(&stage->args);
+
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		if (!(out_frame[i]) && (binary)
+		    && (binary->out_frame_info[i].res.width)) {
+			err = ia_css_frame_allocate_from_info(&out_frame[i],
+							      &binary->out_frame_info[i]);
+			if (err)
+				goto ERR;
+			stage->out_frame_allocated[i] = true;
+		}
+	}
+	/* VF frame is not needed in case of need_pp
+	   However, the capture binary needs a vf frame to write to.
+	 */
+	if (!vf_frame) {
+		if ((binary && binary->vf_frame_info.res.width) ||
+		    (firmware && firmware->info.isp.sp.enable.vf_veceven)
+		   ) {
+			err = ia_css_frame_allocate_from_info(&vf_frame,
+							      &binary->vf_frame_info);
+			if (err)
+				goto ERR;
+			stage->vf_frame_allocated = true;
+		}
+	} else if (vf_frame && binary && binary->vf_frame_info.res.width
+		   && !firmware) {
+		/* only mark as allocated if buffer pointer available */
+		if (vf_frame->data != mmgr_NULL)
+			stage->vf_frame_allocated = true;
+	}
+
+	stage->args.in_frame = stage_desc->in_frame;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		stage->args.out_frame[i] = out_frame[i];
+	stage->args.out_vf_frame = vf_frame;
+	*new_stage = stage;
+	return err;
+ERR:
+	if (stage)
+		pipeline_stage_destroy(stage);
+	return err;
+}
+
+static const struct ia_css_frame ia_css_default_frame = DEFAULT_FRAME;
+
+static void pipeline_init_defaults(
+    struct ia_css_pipeline *pipeline,
+    enum ia_css_pipe_id pipe_id,
+    unsigned int pipe_num,
+    unsigned int dvs_frame_delay)
+{
+	unsigned int i;
+
+	pipeline->pipe_id = pipe_id;
+	pipeline->stages = NULL;
+	pipeline->stop_requested = false;
+	pipeline->current_stage = NULL;
+
+	memcpy(&pipeline->in_frame, &ia_css_default_frame,
+	       sizeof(ia_css_default_frame));
+
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		memcpy(&pipeline->out_frame[i], &ia_css_default_frame,
+		       sizeof(ia_css_default_frame));
+		memcpy(&pipeline->vf_frame[i], &ia_css_default_frame,
+		       sizeof(ia_css_default_frame));
+	}
+	pipeline->num_execs = -1;
+	pipeline->acquire_isp_each_stage = true;
+	pipeline->pipe_num = (uint8_t)pipe_num;
+	pipeline->dvs_frame_delay = dvs_frame_delay;
+}
+
+static void ia_css_pipeline_set_zoom_stage(struct ia_css_pipeline *pipeline)
+{
+	struct ia_css_pipeline_stage *stage = NULL;
+	int err = 0;
+
+	assert(pipeline);
+	if (pipeline->pipe_id == IA_CSS_PIPE_ID_PREVIEW) {
+		/* in preview pipeline, vf_pp stage should do zoom */
+		err = ia_css_pipeline_get_stage(pipeline, IA_CSS_BINARY_MODE_VF_PP, &stage);
+		if (!err)
+			stage->enable_zoom = true;
+	} else if (pipeline->pipe_id == IA_CSS_PIPE_ID_CAPTURE) {
+		/* in capture pipeline, capture_pp stage should do zoom */
+		err = ia_css_pipeline_get_stage(pipeline, IA_CSS_BINARY_MODE_CAPTURE_PP,
+						&stage);
+		if (!err)
+			stage->enable_zoom = true;
+	} else if (pipeline->pipe_id == IA_CSS_PIPE_ID_VIDEO) {
+		/* in video pipeline, video stage should do zoom */
+		err = ia_css_pipeline_get_stage(pipeline, IA_CSS_BINARY_MODE_VIDEO, &stage);
+		if (!err)
+			stage->enable_zoom = true;
+	} else if (pipeline->pipe_id == IA_CSS_PIPE_ID_YUVPP) {
+		/* in yuvpp pipeline, first yuv_scaler stage should do zoom */
+		err = ia_css_pipeline_get_stage(pipeline, IA_CSS_BINARY_MODE_CAPTURE_PP,
+						&stage);
+		if (!err)
+			stage->enable_zoom = true;
+	}
+}
+
+static void
+ia_css_pipeline_configure_inout_port(struct ia_css_pipeline *me,
+				     bool continuous)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipeline_configure_inout_port() enter: pipe_id(%d) continuous(%d)\n",
+			    me->pipe_id, continuous);
+	switch (me->pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+	case IA_CSS_PIPE_ID_VIDEO:
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)(continuous ? SH_CSS_COPYSINK_TYPE : SH_CSS_HOST_TYPE), 1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		break;
+	case IA_CSS_PIPE_ID_COPY: /*Copy pipe ports configured to "offline" mode*/
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		if (continuous) {
+			SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+						    (uint8_t)SH_CSS_PORT_OUTPUT,
+						    (uint8_t)SH_CSS_COPYSINK_TYPE, 1);
+			SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+						    (uint8_t)SH_CSS_PORT_OUTPUT,
+						    (uint8_t)SH_CSS_TAGGERSINK_TYPE, 1);
+		} else {
+			SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+						    (uint8_t)SH_CSS_PORT_OUTPUT,
+						    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		}
+		break;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)(continuous ? SH_CSS_TAGGERSINK_TYPE : SH_CSS_HOST_TYPE),
+					    1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		break;
+	case IA_CSS_PIPE_ID_YUVPP:
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)(SH_CSS_HOST_TYPE), 1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		break;
+	case IA_CSS_PIPE_ID_ACC:
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(me->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		break;
+	default:
+		break;
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipeline_configure_inout_port() leave: inout_port_config(%x)\n",
+			    me->inout_port_config);
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue.h b/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue.h
new file mode 100644
index 000000000..08112be46
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue.h
@@ -0,0 +1,176 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_QUEUE_H
+#define __IA_CSS_QUEUE_H
+
+#include <platform_support.h>
+#include <type_support.h>
+
+#include "ia_css_queue_comm.h"
+#include "../src/queue_access.h"
+
+/* Local Queue object descriptor */
+struct ia_css_queue_local {
+	ia_css_circbuf_desc_t *cb_desc; /*Circbuf desc for local queues*/
+	ia_css_circbuf_elem_t *cb_elems; /*Circbuf elements*/
+};
+
+typedef struct ia_css_queue_local ia_css_queue_local_t;
+
+/* Handle for queue object*/
+typedef struct ia_css_queue ia_css_queue_t;
+
+/*****************************************************************************
+ * Queue Public APIs
+ *****************************************************************************/
+/* @brief Initialize a local queue instance.
+ *
+ * @param[out] qhandle. Handle to queue instance for use with API
+ * @param[in]  desc.   Descriptor with queue properties filled-in
+ * @return     0      - Successful init of local queue instance.
+ * @return     -EINVAL - Invalid argument.
+ *
+ */
+int ia_css_queue_local_init(
+    ia_css_queue_t *qhandle,
+    ia_css_queue_local_t *desc);
+
+/* @brief Initialize a remote queue instance
+ *
+ * @param[out] qhandle. Handle to queue instance for use with API
+ * @param[in]  desc.   Descriptor with queue properties filled-in
+ * @return     0      - Successful init of remote queue instance.
+ * @return     -EINVAL - Invalid argument.
+ */
+int ia_css_queue_remote_init(
+    ia_css_queue_t *qhandle,
+    ia_css_queue_remote_t *desc);
+
+/* @brief Uninitialize a queue instance
+ *
+ * @param[in]  qhandle. Handle to queue instance
+ * @return     0 - Successful uninit.
+ *
+ */
+int ia_css_queue_uninit(
+    ia_css_queue_t *qhandle);
+
+/* @brief Enqueue an item in the queue instance
+ *
+ * @param[in]  qhandle. Handle to queue instance
+ * @param[in]  item.    Object to be enqueued.
+ * @return     0       - Successful enqueue.
+ * @return     -EINVAL  - Invalid argument.
+ * @return     -ENOBUFS - Queue is full.
+ *
+ */
+int ia_css_queue_enqueue(
+    ia_css_queue_t *qhandle,
+    uint32_t item);
+
+/* @brief Dequeue an item from the queue instance
+ *
+ * @param[in]  qhandle. Handle to queue instance
+ * @param[out] item.    Object to be dequeued into this item.
+
+ * @return     0       - Successful dequeue.
+ * @return     -EINVAL  - Invalid argument.
+ * @return     -ENODATA - Queue is empty.
+ *
+ */
+int ia_css_queue_dequeue(
+    ia_css_queue_t *qhandle,
+    uint32_t *item);
+
+/* @brief Check if the queue is empty
+ *
+ * @param[in]  qhandle.  Handle to queue instance
+ * @param[in]  is_empty  True if empty, False if not.
+ * @return     0       - Successful access state.
+ * @return     -EINVAL  - Invalid argument.
+ * @return     -ENOSYS  - Function not implemented.
+ *
+ */
+int ia_css_queue_is_empty(
+    ia_css_queue_t *qhandle,
+    bool *is_empty);
+
+/* @brief Check if the queue is full
+ *
+ * @param[in]  qhandle.  Handle to queue instance
+ * @param[in]  is_full   True if Full, False if not.
+ * @return     0       - Successfully access state.
+ * @return     -EINVAL  - Invalid argument.
+ * @return     -ENOSYS  - Function not implemented.
+ *
+ */
+int ia_css_queue_is_full(
+    ia_css_queue_t *qhandle,
+    bool *is_full);
+
+/* @brief Get used space in the queue
+ *
+ * @param[in]  qhandle.  Handle to queue instance
+ * @param[in]  size      Number of available elements in the queue
+ * @return     0       - Successfully access state.
+ * @return     -EINVAL  - Invalid argument.
+ *
+ */
+int ia_css_queue_get_used_space(
+    ia_css_queue_t *qhandle,
+    uint32_t *size);
+
+/* @brief Get free space in the queue
+ *
+ * @param[in]  qhandle.  Handle to queue instance
+ * @param[in]  size      Number of free elements in the queue
+ * @return     0       - Successfully access state.
+ * @return     -EINVAL  - Invalid argument.
+ *
+ */
+int ia_css_queue_get_free_space(
+    ia_css_queue_t *qhandle,
+    uint32_t *size);
+
+/* @brief Peek at an element in the queue
+ *
+ * @param[in]  qhandle.  Handle to queue instance
+ * @param[in]  offset   Offset of element to peek,
+ *			 starting from head of queue
+ * @param[in]  element   Value of element returned
+ * @return     0       - Successfully access state.
+ * @return     -EINVAL  - Invalid argument.
+ *
+ */
+int ia_css_queue_peek(
+    ia_css_queue_t *qhandle,
+    u32 offset,
+    uint32_t *element);
+
+/* @brief Get the usable size for the queue
+ *
+ * @param[in]  qhandle. Handle to queue instance
+ * @param[out] size     Size value to be returned here.
+ * @return     0       - Successful get size.
+ * @return     -EINVAL  - Invalid argument.
+ * @return     -ENOSYS  - Function not implemented.
+ *
+ */
+int ia_css_queue_get_size(
+    ia_css_queue_t *qhandle,
+    uint32_t *size);
+
+#endif /* __IA_CSS_QUEUE_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue_comm.h b/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue_comm.h
new file mode 100644
index 000000000..1379ae8f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/queue/interface/ia_css_queue_comm.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_QUEUE_COMM_H
+#define __IA_CSS_QUEUE_COMM_H
+
+#include "type_support.h"
+#include "ia_css_circbuf.h"
+/*****************************************************************************
+ * Queue Public Data Structures
+ *****************************************************************************/
+
+/* Queue location specifier */
+/* Avoiding enums to save space */
+#define IA_CSS_QUEUE_LOC_HOST 0
+#define IA_CSS_QUEUE_LOC_SP   1
+#define IA_CSS_QUEUE_LOC_ISP  2
+
+/* Queue type specifier */
+/* Avoiding enums to save space */
+#define IA_CSS_QUEUE_TYPE_LOCAL  0
+#define IA_CSS_QUEUE_TYPE_REMOTE 1
+
+/* for DDR Allocated queues,
+allocate minimum these many elements.
+DDR->SP' DMEM DMA transfer needs 32byte aligned address.
+Since each element size is 4 bytes, 8 elements need to be
+DMAed to access single element.*/
+#define IA_CSS_MIN_ELEM_COUNT    8
+#define IA_CSS_DMA_XFER_MASK (IA_CSS_MIN_ELEM_COUNT - 1)
+
+/* Remote Queue object descriptor */
+struct ia_css_queue_remote {
+	u32 cb_desc_addr; /*Circbuf desc address for remote queues*/
+	u32 cb_elems_addr; /*Circbuf elements addr for remote queue*/
+	u8 location;    /* Cell location for queue */
+	u8 proc_id;     /* Processor id for queue access */
+};
+
+typedef struct ia_css_queue_remote ia_css_queue_remote_t;
+
+#endif /* __IA_CSS_QUEUE_COMM_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/queue/src/queue.c b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue.c
new file mode 100644
index 000000000..2f1c2df59
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue.c
@@ -0,0 +1,401 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_queue.h"
+#include <math_support.h>
+#include <ia_css_circbuf.h>
+#include <ia_css_circbuf_desc.h>
+#include "queue_access.h"
+
+/*****************************************************************************
+ * Queue Public APIs
+ *****************************************************************************/
+int ia_css_queue_local_init(ia_css_queue_t *qhandle, ia_css_queue_local_t *desc)
+{
+	if (NULL == qhandle || NULL == desc
+	    || NULL == desc->cb_elems || NULL == desc->cb_desc) {
+		/* Invalid parameters, return error*/
+		return -EINVAL;
+	}
+
+	/* Mark the queue as Local */
+	qhandle->type = IA_CSS_QUEUE_TYPE_LOCAL;
+
+	/* Create a local circular buffer queue*/
+	ia_css_circbuf_create(&qhandle->desc.cb_local,
+			      desc->cb_elems,
+			      desc->cb_desc);
+
+	return 0;
+}
+
+int ia_css_queue_remote_init(ia_css_queue_t *qhandle, ia_css_queue_remote_t *desc)
+{
+	if (NULL == qhandle || NULL == desc) {
+		/* Invalid parameters, return error*/
+		return -EINVAL;
+	}
+
+	/* Mark the queue as remote*/
+	qhandle->type = IA_CSS_QUEUE_TYPE_REMOTE;
+
+	/* Copy over the local queue descriptor*/
+	qhandle->location = desc->location;
+	qhandle->proc_id = desc->proc_id;
+	qhandle->desc.remote.cb_desc_addr = desc->cb_desc_addr;
+	qhandle->desc.remote.cb_elems_addr = desc->cb_elems_addr;
+
+	/* If queue is remote, we let the local processor
+	 * do its init, before using it. This is just to get us
+	 * started, we can remove this restriction as we go ahead
+	 */
+
+	return 0;
+}
+
+int ia_css_queue_uninit(ia_css_queue_t *qhandle)
+{
+	if (!qhandle)
+		return -EINVAL;
+
+	/* Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Local queues are created. Destroy it*/
+		ia_css_circbuf_destroy(&qhandle->desc.cb_local);
+	}
+
+	return 0;
+}
+
+int ia_css_queue_enqueue(ia_css_queue_t *qhandle, uint32_t item)
+{
+	int error = 0;
+
+	if (!qhandle)
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		if (ia_css_circbuf_is_full(&qhandle->desc.cb_local)) {
+			/* Cannot push the element. Return*/
+			return -ENOBUFS;
+		}
+
+		/* Push the element*/
+		ia_css_circbuf_push(&qhandle->desc.cb_local, item);
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		ia_css_circbuf_desc_t cb_desc;
+		ia_css_circbuf_elem_t cb_elem;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		/* a. Load the queue cb_desc from remote */
+		QUEUE_CB_DESC_INIT(&cb_desc);
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		if (ia_css_circbuf_desc_is_full(&cb_desc))
+			return -ENOBUFS;
+
+		cb_elem.val = item;
+
+		error = ia_css_queue_item_store(qhandle, cb_desc.end, &cb_elem);
+		if (error != 0)
+			return error;
+
+		cb_desc.end = (cb_desc.end + 1) % cb_desc.size;
+
+		/* c. Store the queue object */
+		/* Set only fields requiring update with
+		 * valid value. Avoids uncessary calls
+		 * to load/store functions
+		 */
+		ignore_desc_flags = QUEUE_IGNORE_SIZE_START_STEP_FLAGS;
+
+		error = ia_css_queue_store(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_dequeue(ia_css_queue_t *qhandle, uint32_t *item)
+{
+	int error = 0;
+
+	if (!qhandle || NULL == item)
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		if (ia_css_circbuf_is_empty(&qhandle->desc.cb_local)) {
+			/* Nothing to pop. Return empty queue*/
+			return -ENODATA;
+		}
+
+		*item = ia_css_circbuf_pop(&qhandle->desc.cb_local);
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		ia_css_circbuf_elem_t cb_elem;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		if (ia_css_circbuf_desc_is_empty(&cb_desc))
+			return -ENODATA;
+
+		error = ia_css_queue_item_load(qhandle, cb_desc.start, &cb_elem);
+		if (error != 0)
+			return error;
+
+		*item = cb_elem.val;
+
+		cb_desc.start = OP_std_modadd(cb_desc.start, 1, cb_desc.size);
+
+		/* c. Store the queue object */
+		/* Set only fields requiring update with
+		 * valid value. Avoids uncessary calls
+		 * to load/store functions
+		 */
+		ignore_desc_flags = QUEUE_IGNORE_SIZE_END_STEP_FLAGS;
+		error = ia_css_queue_store(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+	}
+	return 0;
+}
+
+int ia_css_queue_is_full(ia_css_queue_t *qhandle, bool *is_full)
+{
+	int error = 0;
+
+	if ((!qhandle) || (!is_full))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		*is_full = ia_css_circbuf_is_full(&qhandle->desc.cb_local);
+		return 0;
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		*is_full = ia_css_circbuf_desc_is_full(&cb_desc);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int ia_css_queue_get_free_space(ia_css_queue_t *qhandle, uint32_t *size)
+{
+	int error = 0;
+
+	if ((!qhandle) || (!size))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		*size = ia_css_circbuf_get_free_elems(&qhandle->desc.cb_local);
+		return 0;
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		*size = ia_css_circbuf_desc_get_free_elems(&cb_desc);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int ia_css_queue_get_used_space(ia_css_queue_t *qhandle, uint32_t *size)
+{
+	int error = 0;
+
+	if ((!qhandle) || (!size))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		*size = ia_css_circbuf_get_num_elems(&qhandle->desc.cb_local);
+		return 0;
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		*size = ia_css_circbuf_desc_get_num_elems(&cb_desc);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int ia_css_queue_peek(ia_css_queue_t *qhandle, u32 offset, uint32_t *element)
+{
+	u32 num_elems = 0;
+	int error = 0;
+
+	if ((!qhandle) || (!element))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		/* Check if offset is valid */
+		num_elems = ia_css_circbuf_get_num_elems(&qhandle->desc.cb_local);
+		if (offset > num_elems)
+			return -EINVAL;
+
+		*element = ia_css_circbuf_peek_from_start(&qhandle->desc.cb_local, (int)offset);
+		return 0;
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		ia_css_circbuf_elem_t cb_elem;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+
+		error =  ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* Check if offset is valid */
+		num_elems = ia_css_circbuf_desc_get_num_elems(&cb_desc);
+		if (offset > num_elems)
+			return -EINVAL;
+
+		offset = OP_std_modadd(cb_desc.start, offset, cb_desc.size);
+		error = ia_css_queue_item_load(qhandle, (uint8_t)offset, &cb_elem);
+		if (error != 0)
+			return error;
+
+		*element = cb_elem.val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int ia_css_queue_is_empty(ia_css_queue_t *qhandle, bool *is_empty)
+{
+	int error = 0;
+
+	if ((!qhandle) || (!is_empty))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		*is_empty = ia_css_circbuf_is_empty(&qhandle->desc.cb_local);
+		return 0;
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		u32 ignore_desc_flags = QUEUE_IGNORE_STEP_FLAG;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* b. Operate on the queue */
+		*is_empty = ia_css_circbuf_desc_is_empty(&cb_desc);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int ia_css_queue_get_size(ia_css_queue_t *qhandle, uint32_t *size)
+{
+	int error = 0;
+
+	if ((!qhandle) || (!size))
+		return -EINVAL;
+
+	/* 1. Load the required queue object */
+	if (qhandle->type == IA_CSS_QUEUE_TYPE_LOCAL) {
+		/* Directly de-ref the object and
+		 * operate on the queue
+		 */
+		/* Return maximum usable capacity */
+		*size = ia_css_circbuf_get_size(&qhandle->desc.cb_local);
+	} else if (qhandle->type == IA_CSS_QUEUE_TYPE_REMOTE) {
+		/* a. Load the queue from remote */
+		ia_css_circbuf_desc_t cb_desc;
+		u32 ignore_desc_flags = QUEUE_IGNORE_START_END_STEP_FLAGS;
+
+		QUEUE_CB_DESC_INIT(&cb_desc);
+
+		error = ia_css_queue_load(qhandle, &cb_desc, ignore_desc_flags);
+		if (error != 0)
+			return error;
+
+		/* Return maximum usable capacity */
+		*size = cb_desc.size;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.c b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.c
new file mode 100644
index 000000000..424e7a15a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.c
@@ -0,0 +1,178 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "type_support.h"
+#include "queue_access.h"
+#include "ia_css_circbuf.h"
+#include "sp.h"
+#include "assert_support.h"
+
+int ia_css_queue_load(
+    struct ia_css_queue *rdesc,
+    ia_css_circbuf_desc_t *cb_desc,
+    uint32_t ignore_desc_flags)
+{
+	if (!rdesc || !cb_desc)
+		return -EINVAL;
+
+	if (rdesc->location == IA_CSS_QUEUE_LOC_SP) {
+		assert(ignore_desc_flags <= QUEUE_IGNORE_DESC_FLAGS_MAX);
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_SIZE_FLAG)) {
+			cb_desc->size = sp_dmem_load_uint8(rdesc->proc_id,
+							   rdesc->desc.remote.cb_desc_addr
+							   + offsetof(ia_css_circbuf_desc_t, size));
+
+			if (cb_desc->size == 0) {
+				/* Adding back the workaround which was removed
+				   while refactoring queues. When reading size
+				   through sp_dmem_load_*, sometimes we get back
+				   the value as zero. This causes division by 0
+				   exception as the size is used in a modular
+				   division operation. */
+				return -EDOM;
+			}
+		}
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_START_FLAG))
+			cb_desc->start = sp_dmem_load_uint8(rdesc->proc_id,
+							    rdesc->desc.remote.cb_desc_addr
+							    + offsetof(ia_css_circbuf_desc_t, start));
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_END_FLAG))
+			cb_desc->end = sp_dmem_load_uint8(rdesc->proc_id,
+							  rdesc->desc.remote.cb_desc_addr
+							  + offsetof(ia_css_circbuf_desc_t, end));
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_STEP_FLAG))
+			cb_desc->step = sp_dmem_load_uint8(rdesc->proc_id,
+							   rdesc->desc.remote.cb_desc_addr
+							   + offsetof(ia_css_circbuf_desc_t, step));
+
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_HOST) {
+		/* doing DMA transfer of entire structure */
+		hmm_load(rdesc->desc.remote.cb_desc_addr,
+			  (void *)cb_desc,
+			  sizeof(ia_css_circbuf_desc_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_ISP) {
+		/* Not supported yet */
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_store(
+    struct ia_css_queue *rdesc,
+    ia_css_circbuf_desc_t *cb_desc,
+    uint32_t ignore_desc_flags)
+{
+	if (!rdesc || !cb_desc)
+		return -EINVAL;
+
+	if (rdesc->location == IA_CSS_QUEUE_LOC_SP) {
+		assert(ignore_desc_flags <= QUEUE_IGNORE_DESC_FLAGS_MAX);
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_SIZE_FLAG))
+			sp_dmem_store_uint8(rdesc->proc_id,
+					    rdesc->desc.remote.cb_desc_addr
+					    + offsetof(ia_css_circbuf_desc_t, size),
+					    cb_desc->size);
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_START_FLAG))
+			sp_dmem_store_uint8(rdesc->proc_id,
+					    rdesc->desc.remote.cb_desc_addr
+					    + offsetof(ia_css_circbuf_desc_t, start),
+					    cb_desc->start);
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_END_FLAG))
+			sp_dmem_store_uint8(rdesc->proc_id,
+					    rdesc->desc.remote.cb_desc_addr
+					    + offsetof(ia_css_circbuf_desc_t, end),
+					    cb_desc->end);
+
+		if (0 == (ignore_desc_flags & QUEUE_IGNORE_STEP_FLAG))
+			sp_dmem_store_uint8(rdesc->proc_id,
+					    rdesc->desc.remote.cb_desc_addr
+					    + offsetof(ia_css_circbuf_desc_t, step),
+					    cb_desc->step);
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_HOST) {
+		/* doing DMA transfer of entire structure */
+		hmm_store(rdesc->desc.remote.cb_desc_addr,
+			   (void *)cb_desc,
+			   sizeof(ia_css_circbuf_desc_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_ISP) {
+		/* Not supported yet */
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_item_load(
+    struct ia_css_queue *rdesc,
+    u8 position,
+    ia_css_circbuf_elem_t *item)
+{
+	if (!rdesc || !item)
+		return -EINVAL;
+
+	if (rdesc->location == IA_CSS_QUEUE_LOC_SP) {
+		sp_dmem_load(rdesc->proc_id,
+			     rdesc->desc.remote.cb_elems_addr
+			     + position * sizeof(ia_css_circbuf_elem_t),
+			     item,
+			     sizeof(ia_css_circbuf_elem_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_HOST) {
+		hmm_load(rdesc->desc.remote.cb_elems_addr
+			  + position * sizeof(ia_css_circbuf_elem_t),
+			  (void *)item,
+			  sizeof(ia_css_circbuf_elem_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_ISP) {
+		/* Not supported yet */
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_item_store(
+    struct ia_css_queue *rdesc,
+    u8 position,
+    ia_css_circbuf_elem_t *item)
+{
+	if (!rdesc || !item)
+		return -EINVAL;
+
+	if (rdesc->location == IA_CSS_QUEUE_LOC_SP) {
+		sp_dmem_store(rdesc->proc_id,
+			      rdesc->desc.remote.cb_elems_addr
+			      + position * sizeof(ia_css_circbuf_elem_t),
+			      item,
+			      sizeof(ia_css_circbuf_elem_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_HOST) {
+		hmm_store(rdesc->desc.remote.cb_elems_addr
+			   + position * sizeof(ia_css_circbuf_elem_t),
+			   (void *)item,
+			   sizeof(ia_css_circbuf_elem_t));
+	} else if (rdesc->location == IA_CSS_QUEUE_LOC_ISP) {
+		/* Not supported yet */
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.h b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.h
new file mode 100644
index 000000000..d5107adcc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/queue/src/queue_access.h
@@ -0,0 +1,87 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __QUEUE_ACCESS_H
+#define __QUEUE_ACCESS_H
+
+#include <linux/errno.h>
+
+#include <type_support.h>
+#include <ia_css_queue_comm.h>
+#include <ia_css_circbuf.h>
+
+#define QUEUE_IGNORE_START_FLAG	0x0001
+#define QUEUE_IGNORE_END_FLAG	0x0002
+#define QUEUE_IGNORE_SIZE_FLAG	0x0004
+#define QUEUE_IGNORE_STEP_FLAG	0x0008
+#define QUEUE_IGNORE_DESC_FLAGS_MAX 0x000f
+
+#define QUEUE_IGNORE_SIZE_START_STEP_FLAGS \
+	(QUEUE_IGNORE_SIZE_FLAG | \
+	QUEUE_IGNORE_START_FLAG | \
+	QUEUE_IGNORE_STEP_FLAG)
+
+#define QUEUE_IGNORE_SIZE_END_STEP_FLAGS \
+	(QUEUE_IGNORE_SIZE_FLAG | \
+	QUEUE_IGNORE_END_FLAG   | \
+	QUEUE_IGNORE_STEP_FLAG)
+
+#define QUEUE_IGNORE_START_END_STEP_FLAGS \
+	(QUEUE_IGNORE_START_FLAG | \
+	QUEUE_IGNORE_END_FLAG	  | \
+	QUEUE_IGNORE_STEP_FLAG)
+
+#define QUEUE_CB_DESC_INIT(cb_desc)	\
+	do {				\
+		(cb_desc)->size  = 0;	\
+		(cb_desc)->step  = 0;	\
+		(cb_desc)->start = 0;	\
+		(cb_desc)->end   = 0;	\
+	} while (0)
+
+struct ia_css_queue {
+	u8 type;        /* Specify remote/local type of access */
+	u8 location;    /* Cell location for queue */
+	u8 proc_id;     /* Processor id for queue access */
+	union {
+		ia_css_circbuf_t cb_local;
+		struct {
+			u32 cb_desc_addr; /*Circbuf desc address for remote queues*/
+			u32 cb_elems_addr; /*Circbuf elements addr for remote queue*/
+		}	remote;
+	} desc;
+};
+
+int ia_css_queue_load(
+    struct ia_css_queue *rdesc,
+    ia_css_circbuf_desc_t *cb_desc,
+    uint32_t ignore_desc_flags);
+
+int ia_css_queue_store(
+    struct ia_css_queue *rdesc,
+    ia_css_circbuf_desc_t *cb_desc,
+    uint32_t ignore_desc_flags);
+
+int ia_css_queue_item_load(
+    struct ia_css_queue *rdesc,
+    u8 position,
+    ia_css_circbuf_elem_t *item);
+
+int ia_css_queue_item_store(
+    struct ia_css_queue *rdesc,
+    u8 position,
+    ia_css_circbuf_elem_t *item);
+
+#endif /* __QUEUE_ACCESS_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr.h b/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr.h
new file mode 100644
index 000000000..9cd3d92b3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_RMGR_H
+#define _IA_CSS_RMGR_H
+
+#include <ia_css_err.h>
+
+#ifndef __INLINE_RMGR__
+#define STORAGE_CLASS_RMGR_H extern
+#define STORAGE_CLASS_RMGR_C
+#else				/* __INLINE_RMGR__ */
+#define STORAGE_CLASS_RMGR_H static inline
+#define STORAGE_CLASS_RMGR_C static inline
+#endif				/* __INLINE_RMGR__ */
+
+/**
+ * @brief Initialize resource manager (host/common)
+ */
+int ia_css_rmgr_init(void);
+
+/**
+ * @brief Uninitialize resource manager (host/common)
+ */
+void ia_css_rmgr_uninit(void);
+
+/*****************************************************************
+ * Interface definition - resource type (host/common)
+ *****************************************************************
+ *
+ * struct ia_css_rmgr_<type>_pool;
+ * struct ia_css_rmgr_<type>_handle;
+ *
+ * STORAGE_CLASS_RMGR_H void ia_css_rmgr_init_<type>(
+ *	struct ia_css_rmgr_<type>_pool *pool);
+ *
+ * STORAGE_CLASS_RMGR_H void ia_css_rmgr_uninit_<type>(
+ *	struct ia_css_rmgr_<type>_pool *pool);
+ *
+ * STORAGE_CLASS_RMGR_H void ia_css_rmgr_acq_<type>(
+ *	struct ia_css_rmgr_<type>_pool *pool,
+ *	struct ia_css_rmgr_<type>_handle **handle);
+ *
+ * STORAGE_CLASS_RMGR_H void ia_css_rmgr_rel_<type>(
+ *	struct ia_css_rmgr_<type>_pool *pool,
+ *	struct ia_css_rmgr_<type>_handle **handle);
+ *
+ *****************************************************************
+ * Interface definition - refcounting (host/common)
+ *****************************************************************
+ *
+ * void ia_css_rmgr_refcount_retain_<type>(
+ *	struct ia_css_rmgr_<type>_handle **handle);
+ *
+ * void ia_css_rmgr_refcount_release_<type>(
+ *	struct ia_css_rmgr_<type>_handle **handle);
+ */
+
+#include "ia_css_rmgr_vbuf.h"
+
+#endif	/* _IA_CSS_RMGR_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr_vbuf.h b/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr_vbuf.h
new file mode 100644
index 000000000..ac969afc8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/rmgr/interface/ia_css_rmgr_vbuf.h
@@ -0,0 +1,101 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _IA_CSS_RMGR_VBUF_H
+#define _IA_CSS_RMGR_VBUF_H
+
+#include "ia_css_rmgr.h"
+#include <type_support.h>
+#include <ia_css_types.h>
+#include <system_local.h>
+
+/**
+ * @brief Data structure for the resource handle (host, vbuf)
+ */
+struct ia_css_rmgr_vbuf_handle {
+	ia_css_ptr vptr;
+	u8 count;
+	u32 size;
+};
+
+/**
+ * @brief Data structure for the resource pool (host, vbuf)
+ */
+struct ia_css_rmgr_vbuf_pool {
+	u8 copy_on_write;
+	u8 recycle;
+	u32 size;
+	u32 index;
+	struct ia_css_rmgr_vbuf_handle **handles;
+};
+
+/**
+ * @brief VBUF resource pools
+ */
+extern struct ia_css_rmgr_vbuf_pool *vbuf_ref;
+extern struct ia_css_rmgr_vbuf_pool *vbuf_write;
+extern struct ia_css_rmgr_vbuf_pool *hmm_buffer_pool;
+
+/**
+ * @brief Initialize the resource pool (host, vbuf)
+ *
+ * @param pool	The pointer to the pool
+ */
+STORAGE_CLASS_RMGR_H int ia_css_rmgr_init_vbuf(
+    struct ia_css_rmgr_vbuf_pool *pool);
+
+/**
+ * @brief Uninitialize the resource pool (host, vbuf)
+ *
+ * @param pool	The pointer to the pool
+ */
+STORAGE_CLASS_RMGR_H void ia_css_rmgr_uninit_vbuf(
+    struct ia_css_rmgr_vbuf_pool *pool);
+
+/**
+ * @brief Acquire a handle from the pool (host, vbuf)
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+STORAGE_CLASS_RMGR_H void ia_css_rmgr_acq_vbuf(
+    struct ia_css_rmgr_vbuf_pool *pool,
+    struct ia_css_rmgr_vbuf_handle **handle);
+
+/**
+ * @brief Release a handle to the pool (host, vbuf)
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+STORAGE_CLASS_RMGR_H void ia_css_rmgr_rel_vbuf(
+    struct ia_css_rmgr_vbuf_pool *pool,
+    struct ia_css_rmgr_vbuf_handle **handle);
+
+/**
+ * @brief Retain the reference count for a handle (host, vbuf)
+ *
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_refcount_retain_vbuf(struct ia_css_rmgr_vbuf_handle **handle);
+
+/**
+ * @brief Release the reference count for a handle (host, vbuf)
+ *
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_refcount_release_vbuf(struct ia_css_rmgr_vbuf_handle **handle);
+
+#endif	/* _IA_CSS_RMGR_VBUF_H */
diff --git a/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr.c b/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr.c
new file mode 100644
index 000000000..c94a428aa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr.c
@@ -0,0 +1,40 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_rmgr.h"
+
+int ia_css_rmgr_init(void)
+{
+	int err = 0;
+
+	err = ia_css_rmgr_init_vbuf(vbuf_ref);
+	if (!err)
+		err = ia_css_rmgr_init_vbuf(vbuf_write);
+	if (!err)
+		err = ia_css_rmgr_init_vbuf(hmm_buffer_pool);
+	if (err)
+		ia_css_rmgr_uninit();
+	return err;
+}
+
+/*
+ * @brief Uninitialize resource pool (host)
+ */
+void ia_css_rmgr_uninit(void)
+{
+	ia_css_rmgr_uninit_vbuf(hmm_buffer_pool);
+	ia_css_rmgr_uninit_vbuf(vbuf_write);
+	ia_css_rmgr_uninit_vbuf(vbuf_ref);
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr_vbuf.c b/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr_vbuf.c
new file mode 100644
index 000000000..2e07dab8b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/rmgr/src/rmgr_vbuf.c
@@ -0,0 +1,329 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010-2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+#include "ia_css_rmgr.h"
+
+#include <type_support.h>
+#include <assert_support.h>
+#include <platform_support.h> /* memset */
+#include <ia_css_debug.h>
+
+/*
+ * @brief VBUF resource handles
+ */
+#define NUM_HANDLES 1000
+static struct ia_css_rmgr_vbuf_handle handle_table[NUM_HANDLES];
+
+/*
+ * @brief VBUF resource pool - refpool
+ */
+static struct ia_css_rmgr_vbuf_pool refpool;
+
+/*
+ * @brief VBUF resource pool - writepool
+ */
+static struct ia_css_rmgr_vbuf_pool writepool = {
+	.copy_on_write	= true,
+};
+
+/*
+ * @brief VBUF resource pool - hmmbufferpool
+ */
+static struct ia_css_rmgr_vbuf_pool hmmbufferpool = {
+	.copy_on_write	= true,
+	.recycle	= true,
+	.size		= 32,
+};
+
+struct ia_css_rmgr_vbuf_pool *vbuf_ref = &refpool;
+struct ia_css_rmgr_vbuf_pool *vbuf_write = &writepool;
+struct ia_css_rmgr_vbuf_pool *hmm_buffer_pool = &hmmbufferpool;
+
+/*
+ * @brief Initialize the reference count (host, vbuf)
+ */
+static void rmgr_refcount_init_vbuf(void)
+{
+	/* initialize the refcount table */
+	memset(&handle_table, 0, sizeof(handle_table));
+}
+
+/*
+ * @brief Retain the reference count for a handle (host, vbuf)
+ *
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_refcount_retain_vbuf(struct ia_css_rmgr_vbuf_handle **handle)
+{
+	int i;
+	struct ia_css_rmgr_vbuf_handle *h;
+
+	if ((!handle) || (!*handle)) {
+		IA_CSS_LOG("Invalid inputs");
+		return;
+	}
+	/* new vbuf to count on */
+	if ((*handle)->count == 0) {
+		h = *handle;
+		*handle = NULL;
+		for (i = 0; i < NUM_HANDLES; i++) {
+			if (handle_table[i].count == 0) {
+				*handle = &handle_table[i];
+				break;
+			}
+		}
+		/* if the loop dus not break and *handle == NULL
+		 * this is an error handle and report it.
+		 */
+		if (!*handle) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+					    "ia_css_i_host_refcount_retain_vbuf() failed to find empty slot!\n");
+			return;
+		}
+		(*handle)->vptr = h->vptr;
+		(*handle)->size = h->size;
+	}
+	(*handle)->count++;
+}
+
+/*
+ * @brief Release the reference count for a handle (host, vbuf)
+ *
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_refcount_release_vbuf(struct ia_css_rmgr_vbuf_handle **handle)
+{
+	if ((!handle) || ((*handle) == NULL) || (((*handle)->count) == 0)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR, "%s invalid arguments!\n", __func__);
+		return;
+	}
+	/* decrease reference count */
+	(*handle)->count--;
+	/* remove from admin */
+	if ((*handle)->count == 0) {
+		(*handle)->vptr = 0x0;
+		(*handle)->size = 0;
+		*handle = NULL;
+	}
+}
+
+/*
+ * @brief Initialize the resource pool (host, vbuf)
+ *
+ * @param pool	The pointer to the pool
+ */
+int ia_css_rmgr_init_vbuf(struct ia_css_rmgr_vbuf_pool *pool)
+{
+	int err = 0;
+	size_t bytes_needed;
+
+	rmgr_refcount_init_vbuf();
+	assert(pool);
+	if (!pool)
+		return -EINVAL;
+	/* initialize the recycle pool if used */
+	if (pool->recycle && pool->size) {
+		/* allocate memory for storing the handles */
+		bytes_needed =
+		    sizeof(void *) *
+		    pool->size;
+		pool->handles = kvmalloc(bytes_needed, GFP_KERNEL);
+		if (pool->handles)
+			memset(pool->handles, 0, bytes_needed);
+		else
+			err = -ENOMEM;
+	} else {
+		/* just in case, set the size to 0 */
+		pool->size = 0;
+		pool->handles = NULL;
+	}
+	return err;
+}
+
+/*
+ * @brief Uninitialize the resource pool (host, vbuf)
+ *
+ * @param pool	The pointer to the pool
+ */
+void ia_css_rmgr_uninit_vbuf(struct ia_css_rmgr_vbuf_pool *pool)
+{
+	u32 i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "%s\n", __func__);
+	if (!pool) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR, "%s NULL argument\n", __func__);
+		return;
+	}
+	if (pool->handles) {
+		/* free the hmm buffers */
+		for (i = 0; i < pool->size; i++) {
+			if (pool->handles[i]) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "   freeing/releasing %x (count=%d)\n",
+						    pool->handles[i]->vptr,
+						    pool->handles[i]->count);
+				/* free memory */
+				hmm_free(pool->handles[i]->vptr);
+				/* remove from refcount admin */
+				ia_css_rmgr_refcount_release_vbuf(&pool->handles[i]);
+			}
+		}
+		/* now free the pool handles list */
+		kvfree(pool->handles);
+		pool->handles = NULL;
+	}
+}
+
+/*
+ * @brief Push a handle to the pool
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+static
+void rmgr_push_handle(struct ia_css_rmgr_vbuf_pool *pool,
+		      struct ia_css_rmgr_vbuf_handle **handle)
+{
+	u32 i;
+	bool succes = false;
+
+	assert(pool);
+	assert(pool->recycle);
+	assert(pool->handles);
+	assert(handle);
+	for (i = 0; i < pool->size; i++) {
+		if (!pool->handles[i]) {
+			ia_css_rmgr_refcount_retain_vbuf(handle);
+			pool->handles[i] = *handle;
+			succes = true;
+			break;
+		}
+	}
+	assert(succes);
+}
+
+/*
+ * @brief Pop a handle from the pool
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+static
+void rmgr_pop_handle(struct ia_css_rmgr_vbuf_pool *pool,
+		     struct ia_css_rmgr_vbuf_handle **handle)
+{
+	u32 i;
+
+	assert(pool);
+	assert(pool->recycle);
+	assert(pool->handles);
+	assert(handle);
+	assert(*handle);
+	for (i = 0; i < pool->size; i++) {
+		if ((pool->handles[i]) &&
+		    (pool->handles[i]->size == (*handle)->size)) {
+			*handle = pool->handles[i];
+			pool->handles[i] = NULL;
+			/* dont release, we are returning it...
+			 * ia_css_rmgr_refcount_release_vbuf(handle);
+			 */
+			return;
+		}
+	}
+}
+
+/*
+ * @brief Acquire a handle from the pool (host, vbuf)
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_acq_vbuf(struct ia_css_rmgr_vbuf_pool *pool,
+			  struct ia_css_rmgr_vbuf_handle **handle)
+{
+	if ((!pool) || (!handle) || (!*handle)) {
+		IA_CSS_LOG("Invalid inputs");
+		return;
+	}
+
+	if (pool->copy_on_write) {
+		struct ia_css_rmgr_vbuf_handle *new_handle;
+		struct ia_css_rmgr_vbuf_handle h = { 0 };
+
+		/* only one reference, reuse (no new retain) */
+		if ((*handle)->count == 1)
+			return;
+		/* more than one reference, release current buffer */
+		if ((*handle)->count > 1) {
+			/* store current values */
+			h.vptr = 0x0;
+			h.size = (*handle)->size;
+			/* release ref to current buffer */
+			ia_css_rmgr_refcount_release_vbuf(handle);
+			new_handle = &h;
+		} else {
+			new_handle = *handle;
+		}
+		/* get new buffer for needed size */
+		if (new_handle->vptr == 0x0) {
+			if (pool->recycle) {
+				/* try and pop from pool */
+				rmgr_pop_handle(pool, &new_handle);
+			}
+			if (new_handle->vptr == 0x0) {
+				/* we need to allocate */
+				new_handle->vptr = hmm_alloc(new_handle->size);
+			} else {
+				/* we popped a buffer */
+				*handle = new_handle;
+				return;
+			}
+		}
+		/* Note that new_handle will change to an internally maintained one */
+		ia_css_rmgr_refcount_retain_vbuf(&new_handle);
+		*handle = new_handle;
+		return;
+	}
+	/* Note that handle will change to an internally maintained one */
+	ia_css_rmgr_refcount_retain_vbuf(handle);
+}
+
+/*
+ * @brief Release a handle to the pool (host, vbuf)
+ *
+ * @param pool		The pointer to the pool
+ * @param handle	The pointer to the handle
+ */
+void ia_css_rmgr_rel_vbuf(struct ia_css_rmgr_vbuf_pool *pool,
+			  struct ia_css_rmgr_vbuf_handle **handle)
+{
+	if ((!pool) || (!handle) || (!*handle)) {
+		IA_CSS_LOG("Invalid inputs");
+		return;
+	}
+	/* release the handle */
+	if ((*handle)->count == 1) {
+		if (!pool->recycle) {
+			/* non recycling pool, free mem */
+			hmm_free((*handle)->vptr);
+		} else {
+			/* recycle to pool */
+			rmgr_push_handle(pool, handle);
+		}
+	}
+	ia_css_rmgr_refcount_release_vbuf(handle);
+	*handle = NULL;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl.h b/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl.h
new file mode 100644
index 000000000..efe6c4a82
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SPCTRL_H__
+#define __IA_CSS_SPCTRL_H__
+
+#include <system_global.h>
+#include <ia_css_err.h>
+#include "ia_css_spctrl_comm.h"
+
+typedef struct {
+	u32        ddr_data_offset;       /**  posistion of data in DDR */
+	u32        dmem_data_addr;        /** data segment address in dmem */
+	u32        dmem_bss_addr;         /** bss segment address in dmem  */
+	u32        data_size;             /** data segment size            */
+	u32        bss_size;              /** bss segment size             */
+	u32        spctrl_config_dmem_addr; /* <location of dmem_cfg  in SP dmem */
+	u32        spctrl_state_dmem_addr;  /* < location of state  in SP dmem */
+	unsigned int    sp_entry;                /* < entry function ptr on SP */
+	const void      *code;                   /** location of firmware */
+	u32         code_size;
+	char      *program_name;    /** not used on hardware, only for simulation */
+} ia_css_spctrl_cfg;
+
+/* Get the code addr in DDR of SP */
+ia_css_ptr get_sp_code_addr(sp_ID_t  sp_id);
+
+/* ! Load firmware on to specfied SP
+*/
+int ia_css_spctrl_load_fw(sp_ID_t sp_id,
+				      ia_css_spctrl_cfg *spctrl_cfg);
+
+/* ISP2401 */
+/*! Setup registers for reloading FW */
+void sh_css_spctrl_reload_fw(sp_ID_t sp_id);
+
+/*!  Unload/release any memory allocated to hold the firmware
+*/
+int ia_css_spctrl_unload_fw(sp_ID_t sp_id);
+
+/*! Intilaize dmem_cfg in SP dmem  and  start SP program
+*/
+int ia_css_spctrl_start(sp_ID_t sp_id);
+
+/*! stop spctrl
+*/
+int ia_css_spctrl_stop(sp_ID_t sp_id);
+
+/*! Query the state of SP
+*/
+ia_css_spctrl_sp_sw_state ia_css_spctrl_get_state(sp_ID_t sp_id);
+
+/*! Check if SP is idle/ready
+*/
+int ia_css_spctrl_is_idle(sp_ID_t sp_id);
+
+#endif /* __IA_CSS_SPCTRL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl_comm.h b/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl_comm.h
new file mode 100644
index 000000000..78e0f3096
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/spctrl/interface/ia_css_spctrl_comm.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_SPCTRL_COMM_H__
+#define __IA_CSS_SPCTRL_COMM_H__
+
+#include <type_support.h>
+
+/* state of SP */
+typedef enum {
+	IA_CSS_SP_SW_TERMINATED = 0,
+	IA_CSS_SP_SW_INITIALIZED,
+	IA_CSS_SP_SW_CONNECTED,
+	IA_CSS_SP_SW_RUNNING
+} ia_css_spctrl_sp_sw_state;
+
+/* Structure to encapsulate required arguments for
+ * initialization of SP DMEM using the SP itself
+ */
+struct ia_css_sp_init_dmem_cfg {
+	ia_css_ptr      ddr_data_addr;  /** data segment address in ddr  */
+	u32        dmem_data_addr; /** data segment address in dmem */
+	u32        dmem_bss_addr;  /** bss segment address in dmem  */
+	u32        data_size;      /** data segment size            */
+	u32        bss_size;       /** bss segment size             */
+	sp_ID_t         sp_id;          /* <sp Id */
+};
+
+#define SIZE_OF_IA_CSS_SP_INIT_DMEM_CFG_STRUCT	\
+	(1 * SIZE_OF_IA_CSS_PTR) +		\
+	(4 * sizeof(uint32_t)) +		\
+	(1 * sizeof(sp_ID_t))
+
+#endif /* __IA_CSS_SPCTRL_COMM_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/spctrl/src/spctrl.c b/drivers/staging/media/atomisp/pci/runtime/spctrl/src/spctrl.c
new file mode 100644
index 000000000..c34bfc5f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/spctrl/src/spctrl.c
@@ -0,0 +1,185 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "ia_css_types.h"
+#define __INLINE_SP__
+#include "sp.h"
+
+#include "assert_support.h"
+#include "ia_css_spctrl.h"
+#include "ia_css_debug.h"
+
+struct spctrl_context_info {
+	struct ia_css_sp_init_dmem_cfg dmem_config;
+	u32        spctrl_config_dmem_addr; /* location of dmem_cfg  in SP dmem */
+	u32        spctrl_state_dmem_addr;
+	unsigned int    sp_entry;           /* entry function ptr on SP */
+	ia_css_ptr    code_addr;          /* sp firmware location in host mem-DDR*/
+	u32        code_size;
+	char           *program_name;       /* used in case of PLATFORM_SIM */
+};
+
+static struct spctrl_context_info spctrl_cofig_info[N_SP_ID];
+static bool spctrl_loaded[N_SP_ID] = {0};
+
+/* Load firmware */
+int ia_css_spctrl_load_fw(sp_ID_t sp_id, ia_css_spctrl_cfg *spctrl_cfg)
+{
+	ia_css_ptr code_addr = mmgr_NULL;
+	struct ia_css_sp_init_dmem_cfg *init_dmem_cfg;
+
+	if ((sp_id >= N_SP_ID) || (!spctrl_cfg))
+		return -EINVAL;
+
+	spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;
+
+	init_dmem_cfg = &spctrl_cofig_info[sp_id].dmem_config;
+	init_dmem_cfg->dmem_data_addr = spctrl_cfg->dmem_data_addr;
+	init_dmem_cfg->dmem_bss_addr  = spctrl_cfg->dmem_bss_addr;
+	init_dmem_cfg->data_size      = spctrl_cfg->data_size;
+	init_dmem_cfg->bss_size       = spctrl_cfg->bss_size;
+	init_dmem_cfg->sp_id          = sp_id;
+
+	spctrl_cofig_info[sp_id].spctrl_config_dmem_addr =
+	    spctrl_cfg->spctrl_config_dmem_addr;
+	spctrl_cofig_info[sp_id].spctrl_state_dmem_addr =
+	    spctrl_cfg->spctrl_state_dmem_addr;
+
+	/* store code (text + icache) and data to DDR
+	 *
+	 * Data used to be stored separately, because of access alignment constraints,
+	 * fix the FW generation instead
+	 */
+	code_addr = hmm_alloc(spctrl_cfg->code_size);
+	if (code_addr == mmgr_NULL)
+		return -ENOMEM;
+	hmm_store(code_addr, spctrl_cfg->code, spctrl_cfg->code_size);
+
+	if (sizeof(ia_css_ptr) > sizeof(hrt_data)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "size of ia_css_ptr can not be greater than hrt_data\n");
+		hmm_free(code_addr);
+		code_addr = mmgr_NULL;
+		return -EINVAL;
+	}
+
+	init_dmem_cfg->ddr_data_addr  = code_addr + spctrl_cfg->ddr_data_offset;
+	if ((init_dmem_cfg->ddr_data_addr % HIVE_ISP_DDR_WORD_BYTES) != 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "DDR address pointer is not properly aligned for DMA transfer\n");
+		hmm_free(code_addr);
+		code_addr = mmgr_NULL;
+		return -EINVAL;
+	}
+
+	spctrl_cofig_info[sp_id].sp_entry = spctrl_cfg->sp_entry;
+	spctrl_cofig_info[sp_id].code_addr = code_addr;
+	spctrl_cofig_info[sp_id].program_name = spctrl_cfg->program_name;
+
+	/* now we program the base address into the icache and
+	 * invalidate the cache.
+	 */
+	sp_ctrl_store(sp_id, SP_ICACHE_ADDR_REG,
+		      (hrt_data)spctrl_cofig_info[sp_id].code_addr);
+	sp_ctrl_setbit(sp_id, SP_ICACHE_INV_REG, SP_ICACHE_INV_BIT);
+	spctrl_loaded[sp_id] = true;
+	return 0;
+}
+
+/* ISP2401 */
+/* reload pre-loaded FW */
+void sh_css_spctrl_reload_fw(sp_ID_t sp_id)
+{
+	/* now we program the base address into the icache and
+	 * invalidate the cache.
+	 */
+	sp_ctrl_store(sp_id, SP_ICACHE_ADDR_REG,
+		      (hrt_data)spctrl_cofig_info[sp_id].code_addr);
+	sp_ctrl_setbit(sp_id, SP_ICACHE_INV_REG, SP_ICACHE_INV_BIT);
+	spctrl_loaded[sp_id] = true;
+}
+
+ia_css_ptr get_sp_code_addr(sp_ID_t  sp_id)
+{
+	return spctrl_cofig_info[sp_id].code_addr;
+}
+
+int ia_css_spctrl_unload_fw(sp_ID_t sp_id)
+{
+	if ((sp_id >= N_SP_ID) || ((sp_id < N_SP_ID) && (!spctrl_loaded[sp_id])))
+		return -EINVAL;
+
+	/*  freeup the resource */
+	if (spctrl_cofig_info[sp_id].code_addr) {
+		hmm_free(spctrl_cofig_info[sp_id].code_addr);
+		spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;
+	}
+	spctrl_loaded[sp_id] = false;
+	return 0;
+}
+
+/* Initialize dmem_cfg in SP dmem  and  start SP program*/
+int ia_css_spctrl_start(sp_ID_t sp_id)
+{
+	if ((sp_id >= N_SP_ID) || ((sp_id < N_SP_ID) && (!spctrl_loaded[sp_id])))
+		return -EINVAL;
+
+	/* Set descr in the SP to initialize the SP DMEM */
+	/*
+	 * The FW stores user-space pointers to the FW, the ISP pointer
+	 * is only available here
+	 *
+	 */
+	assert(sizeof(unsigned int) <= sizeof(hrt_data));
+
+	sp_dmem_store(sp_id,
+		      spctrl_cofig_info[sp_id].spctrl_config_dmem_addr,
+		      &spctrl_cofig_info[sp_id].dmem_config,
+		      sizeof(spctrl_cofig_info[sp_id].dmem_config));
+	/* set the start address */
+	sp_ctrl_store(sp_id, SP_START_ADDR_REG,
+		      (hrt_data)spctrl_cofig_info[sp_id].sp_entry);
+	sp_ctrl_setbit(sp_id, SP_SC_REG, SP_RUN_BIT);
+	sp_ctrl_setbit(sp_id, SP_SC_REG, SP_START_BIT);
+	return 0;
+}
+
+/* Query the state of SP1 */
+ia_css_spctrl_sp_sw_state ia_css_spctrl_get_state(sp_ID_t sp_id)
+{
+	ia_css_spctrl_sp_sw_state state = 0;
+	unsigned int HIVE_ADDR_sp_sw_state;
+
+	if (sp_id >= N_SP_ID)
+		return IA_CSS_SP_SW_TERMINATED;
+
+	HIVE_ADDR_sp_sw_state = spctrl_cofig_info[sp_id].spctrl_state_dmem_addr;
+	(void)HIVE_ADDR_sp_sw_state; /* Suppres warnings in CRUN */
+	if (sp_id == SP0_ID)
+		state = sp_dmem_load_uint32(sp_id, (unsigned int)sp_address_of(sp_sw_state));
+	return state;
+}
+
+int ia_css_spctrl_is_idle(sp_ID_t sp_id)
+{
+	int state = 0;
+
+	assert(sp_id < N_SP_ID);
+
+	state = sp_ctrl_getbit(sp_id, SP_SC_REG, SP_IDLE_BIT);
+	return state;
+}
diff --git a/drivers/staging/media/atomisp/pci/runtime/tagger/interface/ia_css_tagger_common.h b/drivers/staging/media/atomisp/pci/runtime/tagger/interface/ia_css_tagger_common.h
new file mode 100644
index 000000000..49801fbc1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/tagger/interface/ia_css_tagger_common.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2010 - 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __IA_CSS_TAGGER_COMMON_H__
+#define __IA_CSS_TAGGER_COMMON_H__
+
+#include <system_local.h>
+#include <type_support.h>
+
+/**
+ * @brief The tagger's circular buffer.
+ *
+ * Should be one less than NUM_CONTINUOUS_FRAMES in sh_css_internal.h
+ */
+#define MAX_CB_ELEMS_FOR_TAGGER 14
+
+/**
+ * @brief Data structure for the tagger buffer element.
+ */
+typedef struct {
+	u32 frame;	/* the frame value stored in the element */
+	u32 param;	/* the param value stored in the element */
+	u8 mark;	/* the mark on the element */
+	u8 lock;	/* the lock on the element */
+	u8 exp_id; /* exp_id of frame, for debugging only */
+} ia_css_tagger_buf_sp_elem_t;
+
+#endif /* __IA_CSS_TAGGER_COMMON_H__ */
diff --git a/drivers/staging/media/atomisp/pci/runtime/timer/src/timer.c b/drivers/staging/media/atomisp/pci/runtime/timer/src/timer.c
new file mode 100644
index 000000000..08f5c3ea6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/runtime/timer/src/timer.c
@@ -0,0 +1,29 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <type_support.h>		/* for uint32_t */
+#include "ia_css_timer.h" /*struct ia_css_clock_tick */
+#include "sh_css_legacy.h" /* IA_CSS_PIPE_ID_NUM*/
+#include "gp_timer.h" /*gp_timer_read()*/
+#include "assert_support.h"
+
+int ia_css_timer_get_current_tick(struct ia_css_clock_tick *curr_ts)
+{
+	assert(curr_ts);
+	if (!curr_ts)
+		return -EINVAL;
+	curr_ts->ticks = (clock_value_t)gp_timer_read(GP_TIMER_SEL);
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/scalar_processor_2400_params.h b/drivers/staging/media/atomisp/pci/scalar_processor_2400_params.h
new file mode 100644
index 000000000..7e7188797
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/scalar_processor_2400_params.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _scalar_processor_2400_params_h
+#define _scalar_processor_2400_params_h
+
+#include "cell_params.h"
+
+#endif /* _scalar_processor_2400_params_h */
diff --git a/drivers/staging/media/atomisp/pci/sh_css.c b/drivers/staging/media/atomisp/pci/sh_css.c
new file mode 100644
index 000000000..da96aaffe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css.c
@@ -0,0 +1,9974 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+/*! \file */
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include "hmm.h"
+
+#include "atomisp_internal.h"
+
+#include "ia_css.h"
+#include "sh_css_hrt.h"		/* only for file 2 MIPI */
+#include "ia_css_buffer.h"
+#include "ia_css_binary.h"
+#include "sh_css_internal.h"
+#include "sh_css_mipi.h"
+#include "sh_css_sp.h"		/* sh_css_sp_group */
+#include "ia_css_isys.h"
+#include "ia_css_frame.h"
+#include "sh_css_defs.h"
+#include "sh_css_firmware.h"
+#include "sh_css_params.h"
+#include "sh_css_params_internal.h"
+#include "sh_css_param_shading.h"
+#include "ia_css_refcount.h"
+#include "ia_css_rmgr.h"
+#include "ia_css_debug.h"
+#include "ia_css_debug_pipe.h"
+#include "ia_css_device_access.h"
+#include "device_access.h"
+#include "sh_css_legacy.h"
+#include "ia_css_pipeline.h"
+#include "ia_css_stream.h"
+#include "sh_css_stream_format.h"
+#include "ia_css_pipe.h"
+#include "ia_css_util.h"
+#include "ia_css_pipe_util.h"
+#include "ia_css_pipe_binarydesc.h"
+#include "ia_css_pipe_stagedesc.h"
+
+#include "tag.h"
+#include "assert_support.h"
+#include "math_support.h"
+#include "sw_event_global.h"			/* Event IDs.*/
+#if !defined(ISP2401)
+#include "ia_css_ifmtr.h"
+#endif
+#include "input_system.h"
+#include "mmu_device.h"		/* mmu_set_page_table_base_index(), ... */
+#include "ia_css_mmu_private.h" /* sh_css_mmu_set_page_table_base_index() */
+#include "gdc_device.h"		/* HRT_GDC_N */
+#include "dma.h"		/* dma_set_max_burst_size() */
+#include "irq.h"		/* virq */
+#include "sp.h"			/* cnd_sp_irq_enable() */
+#include "isp.h"		/* cnd_isp_irq_enable, ISP_VEC_NELEMS */
+#include "gp_device.h"		/* gp_device_reg_store() */
+#define __INLINE_GPIO__
+#include "gpio.h"
+#include "timed_ctrl.h"
+#include "ia_css_inputfifo.h"
+#define WITH_PC_MONITORING  0
+
+#define SH_CSS_VIDEO_BUFFER_ALIGNMENT 0
+
+
+#include "ia_css_spctrl.h"
+#include "ia_css_version_data.h"
+#include "sh_css_struct.h"
+#include "ia_css_bufq.h"
+#include "ia_css_timer.h" /* clock_value_t */
+
+#include "isp/modes/interface/input_buf.isp.h"
+
+/* Name of the sp program: should not be built-in */
+#define SP_PROG_NAME "sp"
+/* Size of Refcount List */
+#define REFCOUNT_SIZE 1000
+
+/*
+ * for JPEG, we don't know the length of the image upfront,
+ * but since we support sensor up to 16MP, we take this as
+ * upper limit.
+ */
+#define JPEG_BYTES (16 * 1024 * 1024)
+
+#define STATS_ENABLED(stage) (stage && stage->binary && stage->binary->info && \
+	(stage->binary->info->sp.enable.s3a || stage->binary->info->sp.enable.dis))
+
+struct sh_css my_css;
+
+int  __printf(1, 0) (*sh_css_printf)(const char *fmt, va_list args) = NULL;
+
+/*
+ * modes of work: stream_create and stream_destroy will update the save/restore
+ * data only when in working mode, not suspend/resume
+ */
+enum ia_sh_css_modes {
+	sh_css_mode_none = 0,
+	sh_css_mode_working,
+	sh_css_mode_suspend,
+	sh_css_mode_resume
+};
+
+/**
+ * struct sh_css_stream_seed - a stream seed, to save and restore the
+ * stream data.
+ *
+ * @orig_stream:	pointer to restore the original handle
+ * @stream:		handle, used as ID too.
+ * @stream_config:	stream config struct
+ * @num_pipes:		number of pipes
+ * @pipes:		pipe handles
+ * @orig_pipes:		pointer to restore original handle
+ * @pipe_config:	pipe config structs
+ *
+ * the stream seed contains all the data required to "grow" the seed again
+ * after it was closed.
+*/
+struct sh_css_stream_seed {
+	struct ia_css_stream		**orig_stream;
+	struct ia_css_stream		*stream;
+	struct ia_css_stream_config	stream_config;
+	int				num_pipes;
+	struct ia_css_pipe		*pipes[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_pipe		**orig_pipes[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_pipe_config	pipe_config[IA_CSS_PIPE_ID_NUM];
+};
+
+#define MAX_ACTIVE_STREAMS	5
+/*
+ * A global struct for save/restore to hold all the data that should
+ * sustain power-down: MMU base, IRQ type, env for routines, binary loaded FW
+ * and the stream seeds.
+ */
+struct sh_css_save {
+	enum ia_sh_css_modes		mode;
+	u32		       mmu_base;		/* the last mmu_base */
+	enum ia_css_irq_type           irq_type;
+	struct sh_css_stream_seed      stream_seeds[MAX_ACTIVE_STREAMS];
+	struct ia_css_fw	       *loaded_fw;	/* fw struct previously loaded */
+	struct ia_css_env	       driver_env;	/* driver-supplied env copy */
+};
+
+static bool my_css_save_initialized;	/* if my_css_save was initialized */
+static struct sh_css_save my_css_save;
+
+/*
+ * pqiao NOTICE: this is for css internal buffer recycling when stopping
+ * pipeline,
+ * this array is temporary and will be replaced by resource manager
+ */
+
+/* Taking the biggest Size for number of Elements */
+#define MAX_HMM_BUFFER_NUM	\
+	(SH_CSS_MAX_NUM_QUEUES * (IA_CSS_NUM_ELEMS_SP2HOST_BUFFER_QUEUE + 2))
+
+struct sh_css_hmm_buffer_record {
+	bool in_use;
+	enum ia_css_buffer_type type;
+	struct ia_css_rmgr_vbuf_handle *h_vbuf;
+	hrt_address kernel_ptr;
+};
+
+static struct sh_css_hmm_buffer_record hmm_buffer_record[MAX_HMM_BUFFER_NUM];
+
+#define GPIO_FLASH_PIN_MASK BIT(HIVE_GPIO_STROBE_TRIGGER_PIN)
+
+static bool fw_explicitly_loaded;
+
+/*
+ * Local prototypes
+ */
+
+static int
+allocate_delay_frames(struct ia_css_pipe *pipe);
+
+static int
+sh_css_pipe_start(struct ia_css_stream *stream);
+
+/*
+ * @brief Check if all "ia_css_pipe" instances in the target
+ * "ia_css_stream" instance have stopped.
+ *
+ * @param[in] stream	Point to the target "ia_css_stream" instance.
+ *
+ * @return
+ * - true, if all "ia_css_pipe" instances in the target "ia_css_stream"
+ *   instance have ben stopped.
+ * - false, otherwise.
+ */
+
+/* ISP 2401 */
+static int
+ia_css_pipe_check_format(struct ia_css_pipe *pipe,
+			 enum ia_css_frame_format format);
+
+/* ISP 2401 */
+static int
+ia_css_pipe_load_extension(struct ia_css_pipe *pipe,
+			   struct ia_css_fw_info *firmware);
+
+static void
+ia_css_pipe_unload_extension(struct ia_css_pipe *pipe,
+			     struct ia_css_fw_info *firmware);
+static void
+ia_css_reset_defaults(struct sh_css *css);
+
+static void
+sh_css_init_host_sp_control_vars(void);
+
+static int
+set_num_primary_stages(unsigned int *num, enum ia_css_pipe_version version);
+
+static bool
+need_capture_pp(const struct ia_css_pipe *pipe);
+
+static bool
+need_yuv_scaler_stage(const struct ia_css_pipe *pipe);
+
+static int ia_css_pipe_create_cas_scaler_desc_single_output(
+    struct ia_css_frame_info *cas_scaler_in_info,
+    struct ia_css_frame_info *cas_scaler_out_info,
+    struct ia_css_frame_info *cas_scaler_vf_info,
+    struct ia_css_cas_binary_descr *descr);
+
+static void ia_css_pipe_destroy_cas_scaler_desc(struct ia_css_cas_binary_descr
+	*descr);
+
+static bool
+need_downscaling(const struct ia_css_resolution in_res,
+		 const struct ia_css_resolution out_res);
+
+static bool need_capt_ldc(const struct ia_css_pipe *pipe);
+
+static int
+sh_css_pipe_load_binaries(struct ia_css_pipe *pipe);
+
+static
+int sh_css_pipe_get_viewfinder_frame_info(
+    struct ia_css_pipe *pipe,
+    struct ia_css_frame_info *info,
+    unsigned int idx);
+
+static int
+sh_css_pipe_get_output_frame_info(struct ia_css_pipe *pipe,
+				  struct ia_css_frame_info *info,
+				  unsigned int idx);
+
+static int
+capture_start(struct ia_css_pipe *pipe);
+
+static int
+video_start(struct ia_css_pipe *pipe);
+
+static int
+preview_start(struct ia_css_pipe *pipe);
+
+static int
+yuvpp_start(struct ia_css_pipe *pipe);
+
+static bool copy_on_sp(struct ia_css_pipe *pipe);
+
+static int
+init_vf_frameinfo_defaults(struct ia_css_pipe *pipe,
+			   struct ia_css_frame *vf_frame, unsigned int idx);
+
+static int
+init_in_frameinfo_memory_defaults(struct ia_css_pipe *pipe,
+				  struct ia_css_frame *frame, enum ia_css_frame_format format);
+
+static int
+init_out_frameinfo_defaults(struct ia_css_pipe *pipe,
+			    struct ia_css_frame *out_frame, unsigned int idx);
+
+static int
+sh_css_pipeline_add_acc_stage(struct ia_css_pipeline *pipeline,
+			      const void *acc_fw);
+
+static int
+alloc_continuous_frames(struct ia_css_pipe *pipe, bool init_time);
+
+static void
+pipe_global_init(void);
+
+static int
+pipe_generate_pipe_num(const struct ia_css_pipe *pipe,
+		       unsigned int *pipe_number);
+
+static void
+pipe_release_pipe_num(unsigned int pipe_num);
+
+static int
+create_host_pipeline_structure(struct ia_css_stream *stream);
+
+static int
+create_host_pipeline(struct ia_css_stream *stream);
+
+static int
+create_host_preview_pipeline(struct ia_css_pipe *pipe);
+
+static int
+create_host_video_pipeline(struct ia_css_pipe *pipe);
+
+static int
+create_host_copy_pipeline(struct ia_css_pipe *pipe,
+			  unsigned int max_input_width,
+			  struct ia_css_frame *out_frame);
+
+static int
+create_host_isyscopy_capture_pipeline(struct ia_css_pipe *pipe);
+
+static int
+create_host_capture_pipeline(struct ia_css_pipe *pipe);
+
+static int
+create_host_yuvpp_pipeline(struct ia_css_pipe *pipe);
+
+static int
+create_host_acc_pipeline(struct ia_css_pipe *pipe);
+
+static unsigned int
+sh_css_get_sw_interrupt_value(unsigned int irq);
+
+static struct ia_css_binary *ia_css_pipe_get_shading_correction_binary(
+    const struct ia_css_pipe *pipe);
+
+static struct ia_css_binary *
+ia_css_pipe_get_s3a_binary(const struct ia_css_pipe *pipe);
+
+static struct ia_css_binary *
+ia_css_pipe_get_sdis_binary(const struct ia_css_pipe *pipe);
+
+static void
+sh_css_hmm_buffer_record_init(void);
+
+static void
+sh_css_hmm_buffer_record_uninit(void);
+
+static void
+sh_css_hmm_buffer_record_reset(struct sh_css_hmm_buffer_record *buffer_record);
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+				  enum ia_css_buffer_type type,
+				  hrt_address kernel_ptr);
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_validate(ia_css_ptr ddr_buffer_addr,
+				   enum ia_css_buffer_type type);
+
+void
+ia_css_get_acc_configs(
+    struct ia_css_pipe *pipe,
+    struct ia_css_isp_config *config);
+
+
+#ifdef ISP2401
+static unsigned int get_crop_lines_for_bayer_order(const struct
+	ia_css_stream_config *config);
+static unsigned int get_crop_columns_for_bayer_order(const struct
+	ia_css_stream_config *config);
+static void get_pipe_extra_pixel(struct ia_css_pipe *pipe,
+				 unsigned int *extra_row, unsigned int *extra_column);
+
+#endif
+
+static void
+sh_css_pipe_free_shading_table(struct ia_css_pipe *pipe)
+{
+	if (!pipe) {
+		IA_CSS_ERROR("NULL input parameter");
+		return;
+	}
+
+	if (pipe->shading_table)
+		ia_css_shading_table_free(pipe->shading_table);
+	pipe->shading_table = NULL;
+}
+
+static enum ia_css_frame_format yuv420_copy_formats[] = {
+	IA_CSS_FRAME_FORMAT_NV12,
+	IA_CSS_FRAME_FORMAT_NV21,
+	IA_CSS_FRAME_FORMAT_YV12,
+	IA_CSS_FRAME_FORMAT_YUV420,
+	IA_CSS_FRAME_FORMAT_YUV420_16,
+	IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8,
+	IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8
+};
+
+static enum ia_css_frame_format yuv422_copy_formats[] = {
+	IA_CSS_FRAME_FORMAT_NV12,
+	IA_CSS_FRAME_FORMAT_NV16,
+	IA_CSS_FRAME_FORMAT_NV21,
+	IA_CSS_FRAME_FORMAT_NV61,
+	IA_CSS_FRAME_FORMAT_YV12,
+	IA_CSS_FRAME_FORMAT_YV16,
+	IA_CSS_FRAME_FORMAT_YUV420,
+	IA_CSS_FRAME_FORMAT_YUV420_16,
+	IA_CSS_FRAME_FORMAT_YUV422,
+	IA_CSS_FRAME_FORMAT_YUV422_16,
+	IA_CSS_FRAME_FORMAT_UYVY,
+	IA_CSS_FRAME_FORMAT_YUYV
+};
+
+/*
+ * Verify whether the selected output format is can be produced
+ * by the copy binary given the stream format.
+ */
+static int
+verify_copy_out_frame_format(struct ia_css_pipe *pipe)
+{
+	enum ia_css_frame_format out_fmt = pipe->output_info[0].format;
+	unsigned int i, found = 0;
+
+	assert(pipe);
+	assert(pipe->stream);
+
+	switch (pipe->stream->config.input_config.format) {
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+		for (i = 0; i < ARRAY_SIZE(yuv420_copy_formats) && !found; i++)
+			found = (out_fmt == yuv420_copy_formats[i]);
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_YUV420_16);
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+		for (i = 0; i < ARRAY_SIZE(yuv422_copy_formats) && !found; i++)
+			found = (out_fmt == yuv422_copy_formats[i]);
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_YUV422_16 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_YUV420_16);
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RGBA888 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_RGB565);
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RGBA888 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_YUV420);
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RAW) ||
+		(out_fmt == IA_CSS_FRAME_FORMAT_RAW_PACKED);
+		break;
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_BINARY_8);
+		break;
+	default:
+		break;
+	}
+	if (!found)
+		return -EINVAL;
+	return 0;
+}
+
+unsigned int
+ia_css_stream_input_format_bits_per_pixel(struct ia_css_stream *stream)
+{
+	int bpp = 0;
+
+	if (stream)
+		bpp = ia_css_util_input_format_bpp(stream->config.input_config.format,
+						   stream->config.pixels_per_clock == 2);
+
+	return bpp;
+}
+
+/* TODO: move define to proper file in tools */
+#define GP_ISEL_TPG_MODE 0x90058
+
+#if !defined(ISP2401)
+static int
+sh_css_config_input_network(struct ia_css_stream *stream)
+{
+	unsigned int fmt_type;
+	struct ia_css_pipe *pipe = stream->last_pipe;
+	struct ia_css_binary *binary = NULL;
+	int err = 0;
+
+	assert(stream);
+	assert(pipe);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_config_input_network() enter:\n");
+
+	if (pipe->pipeline.stages)
+		binary = pipe->pipeline.stages->binary;
+
+	err = ia_css_isys_convert_stream_format_to_mipi_format(
+	    stream->config.input_config.format,
+	    stream->csi_rx_config.comp,
+	    &fmt_type);
+	if (err)
+		return err;
+	sh_css_sp_program_input_circuit(fmt_type,
+					stream->config.channel_id,
+					stream->config.mode);
+
+	if ((binary && (binary->online || stream->config.continuous)) ||
+	    pipe->config.mode == IA_CSS_PIPE_MODE_COPY) {
+		err = ia_css_ifmtr_configure(&stream->config,
+					     binary);
+		if (err)
+			return err;
+	}
+
+	if (stream->config.mode == IA_CSS_INPUT_MODE_TPG ||
+	    stream->config.mode == IA_CSS_INPUT_MODE_PRBS) {
+		unsigned int hblank_cycles = 100,
+		vblank_lines = 6,
+		width,
+		height,
+		vblank_cycles;
+		width  = (stream->config.input_config.input_res.width) / (1 +
+			(stream->config.pixels_per_clock == 2));
+		height = stream->config.input_config.input_res.height;
+		vblank_cycles = vblank_lines * (width + hblank_cycles);
+		sh_css_sp_configure_sync_gen(width, height, hblank_cycles,
+					     vblank_cycles);
+		if (pipe->stream->config.mode == IA_CSS_INPUT_MODE_TPG)
+			ia_css_device_store_uint32(GP_ISEL_TPG_MODE, 0);
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_config_input_network() leave:\n");
+	return 0;
+}
+#elif defined(ISP2401)
+static unsigned int csi2_protocol_calculate_max_subpixels_per_line(
+    enum atomisp_input_format	format,
+    unsigned int			pixels_per_line)
+{
+	unsigned int rval;
+
+	switch (format) {
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	UYY0 UYY0 ... UYY0
+		 *		Line	1:	VYY0 VYY0 ... VYY0
+		 *		Line	2:	UYY0 UYY0 ... UYY0
+		 *		Line	3:	VYY0 VYY0 ... VYY0
+		 *		...
+		 *		Line (n-2):	UYY0 UYY0 ... UYY0
+		 *		Line (n-1):	VYY0 VYY0 ... VYY0
+		 *
+		 *	In this frame format, the even-line is
+		 *	as wide as the odd-line.
+		 *	The 0 is introduced by the input system
+		 *	(mipi backend).
+		 */
+		rval = pixels_per_line * 2;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	YYYY YYYY ... YYYY
+		 *		Line	1:	UYVY UYVY ... UYVY UYVY
+		 *		Line	2:	YYYY YYYY ... YYYY
+		 *		Line	3:	UYVY UYVY ... UYVY UYVY
+		 *		...
+		 *		Line (n-2):	YYYY YYYY ... YYYY
+		 *		Line (n-1):	UYVY UYVY ... UYVY UYVY
+		 *
+		 * In this frame format, the odd-line is twice
+		 * wider than the even-line.
+		 */
+		rval = pixels_per_line * 2;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	UYVY UYVY ... UYVY
+		 *		Line	1:	UYVY UYVY ... UYVY
+		 *		Line	2:	UYVY UYVY ... UYVY
+		 *		Line	3:	UYVY UYVY ... UYVY
+		 *		...
+		 *		Line (n-2):	UYVY UYVY ... UYVY
+		 *		Line (n-1):	UYVY UYVY ... UYVY
+		 *
+		 * In this frame format, the even-line is
+		 * as wide as the odd-line.
+		 */
+		rval = pixels_per_line * 2;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	ABGR ABGR ... ABGR
+		 *		Line	1:	ABGR ABGR ... ABGR
+		 *		Line	2:	ABGR ABGR ... ABGR
+		 *		Line	3:	ABGR ABGR ... ABGR
+		 *		...
+		 *		Line (n-2):	ABGR ABGR ... ABGR
+		 *		Line (n-1):	ABGR ABGR ... ABGR
+		 *
+		 * In this frame format, the even-line is
+		 * as wide as the odd-line.
+		 */
+		rval = pixels_per_line * 4;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	Pixel ... Pixel
+		 *		Line	1:	Pixel ... Pixel
+		 *		Line	2:	Pixel ... Pixel
+		 *		Line	3:	Pixel ... Pixel
+		 *		...
+		 *		Line (n-2):	Pixel ... Pixel
+		 *		Line (n-1):	Pixel ... Pixel
+		 *
+		 * In this frame format, the even-line is
+		 * as wide as the odd-line.
+		 */
+		rval = pixels_per_line;
+		break;
+	default:
+		rval = 0;
+		break;
+	}
+
+	return rval;
+}
+
+static bool sh_css_translate_stream_cfg_to_input_system_input_port_id(
+    struct ia_css_stream_config *stream_cfg,
+    ia_css_isys_descr_t	*isys_stream_descr)
+{
+	bool rc;
+
+	rc = true;
+	switch (stream_cfg->mode) {
+	case IA_CSS_INPUT_MODE_TPG:
+
+		if (stream_cfg->source.tpg.id == IA_CSS_TPG_ID0)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT0_ID;
+		else if (stream_cfg->source.tpg.id == IA_CSS_TPG_ID1)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT1_ID;
+		else if (stream_cfg->source.tpg.id == IA_CSS_TPG_ID2)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT2_ID;
+
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+
+		if (stream_cfg->source.prbs.id == IA_CSS_PRBS_ID0)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT0_ID;
+		else if (stream_cfg->source.prbs.id == IA_CSS_PRBS_ID1)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT1_ID;
+		else if (stream_cfg->source.prbs.id == IA_CSS_PRBS_ID2)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_PIXELGEN_PORT2_ID;
+
+		break;
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR:
+
+		if (stream_cfg->source.port.port == MIPI_PORT0_ID)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_CSI_PORT0_ID;
+		else if (stream_cfg->source.port.port == MIPI_PORT1_ID)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_CSI_PORT1_ID;
+		else if (stream_cfg->source.port.port == MIPI_PORT2_ID)
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_CSI_PORT2_ID;
+
+		break;
+	default:
+		rc = false;
+		break;
+	}
+
+	return rc;
+}
+
+static bool sh_css_translate_stream_cfg_to_input_system_input_port_type(
+    struct ia_css_stream_config *stream_cfg,
+    ia_css_isys_descr_t	*isys_stream_descr)
+{
+	bool rc;
+
+	rc = true;
+	switch (stream_cfg->mode) {
+	case IA_CSS_INPUT_MODE_TPG:
+
+		isys_stream_descr->mode = INPUT_SYSTEM_SOURCE_TYPE_TPG;
+
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+
+		isys_stream_descr->mode = INPUT_SYSTEM_SOURCE_TYPE_PRBS;
+
+		break;
+	case IA_CSS_INPUT_MODE_SENSOR:
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR:
+
+		isys_stream_descr->mode = INPUT_SYSTEM_SOURCE_TYPE_SENSOR;
+		break;
+
+	default:
+		rc = false;
+		break;
+	}
+
+	return rc;
+}
+
+static bool sh_css_translate_stream_cfg_to_input_system_input_port_attr(
+    struct ia_css_stream_config *stream_cfg,
+    ia_css_isys_descr_t	*isys_stream_descr,
+    int isys_stream_idx)
+{
+	bool rc;
+
+	rc = true;
+	switch (stream_cfg->mode) {
+	case IA_CSS_INPUT_MODE_TPG:
+		if (stream_cfg->source.tpg.mode == IA_CSS_TPG_MODE_RAMP)
+			isys_stream_descr->tpg_port_attr.mode = PIXELGEN_TPG_MODE_RAMP;
+		else if (stream_cfg->source.tpg.mode == IA_CSS_TPG_MODE_CHECKERBOARD)
+			isys_stream_descr->tpg_port_attr.mode = PIXELGEN_TPG_MODE_CHBO;
+		else if (stream_cfg->source.tpg.mode == IA_CSS_TPG_MODE_MONO)
+			isys_stream_descr->tpg_port_attr.mode = PIXELGEN_TPG_MODE_MONO;
+		else
+			rc = false;
+
+		/*
+		 * TODO
+		 * - Make "color_cfg" as part of "ia_css_tpg_config".
+		 */
+		isys_stream_descr->tpg_port_attr.color_cfg.R1 = 51;
+		isys_stream_descr->tpg_port_attr.color_cfg.G1 = 102;
+		isys_stream_descr->tpg_port_attr.color_cfg.B1 = 255;
+		isys_stream_descr->tpg_port_attr.color_cfg.R2 = 0;
+		isys_stream_descr->tpg_port_attr.color_cfg.G2 = 100;
+		isys_stream_descr->tpg_port_attr.color_cfg.B2 = 160;
+
+		isys_stream_descr->tpg_port_attr.mask_cfg.h_mask =
+		    stream_cfg->source.tpg.x_mask;
+		isys_stream_descr->tpg_port_attr.mask_cfg.v_mask =
+		    stream_cfg->source.tpg.y_mask;
+		isys_stream_descr->tpg_port_attr.mask_cfg.hv_mask =
+		    stream_cfg->source.tpg.xy_mask;
+
+		isys_stream_descr->tpg_port_attr.delta_cfg.h_delta =
+		    stream_cfg->source.tpg.x_delta;
+		isys_stream_descr->tpg_port_attr.delta_cfg.v_delta =
+		    stream_cfg->source.tpg.y_delta;
+
+		/*
+		 * TODO
+		 * - Make "sync_gen_cfg" as part of "ia_css_tpg_config".
+		 */
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.hblank_cycles = 100;
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.vblank_cycles = 100;
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.pixels_per_clock =
+		    stream_cfg->pixels_per_clock;
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.nr_of_frames = (uint32_t)~(0x0);
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.pixels_per_line =
+		    stream_cfg->isys_config[IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX].input_res.width;
+		isys_stream_descr->tpg_port_attr.sync_gen_cfg.lines_per_frame =
+		    stream_cfg->isys_config[IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX].input_res.height;
+
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+
+		isys_stream_descr->prbs_port_attr.seed0 = stream_cfg->source.prbs.seed;
+		isys_stream_descr->prbs_port_attr.seed1 = stream_cfg->source.prbs.seed1;
+
+		/*
+		 * TODO
+		 * - Make "sync_gen_cfg" as part of "ia_css_prbs_config".
+		 */
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.hblank_cycles = 100;
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.vblank_cycles = 100;
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.pixels_per_clock =
+		    stream_cfg->pixels_per_clock;
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.nr_of_frames = (uint32_t)~(0x0);
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.pixels_per_line =
+		    stream_cfg->isys_config[IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX].input_res.width;
+		isys_stream_descr->prbs_port_attr.sync_gen_cfg.lines_per_frame =
+		    stream_cfg->isys_config[IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX].input_res.height;
+
+		break;
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR: {
+		int err;
+		unsigned int fmt_type;
+
+		err = ia_css_isys_convert_stream_format_to_mipi_format(
+			  stream_cfg->isys_config[isys_stream_idx].format,
+			  MIPI_PREDICTOR_NONE,
+			  &fmt_type);
+		if (err)
+			rc = false;
+
+		isys_stream_descr->csi_port_attr.active_lanes =
+		    stream_cfg->source.port.num_lanes;
+		isys_stream_descr->csi_port_attr.fmt_type = fmt_type;
+		isys_stream_descr->csi_port_attr.ch_id = stream_cfg->channel_id;
+#ifdef ISP2401
+		isys_stream_descr->online = stream_cfg->online;
+#endif
+		err |= ia_css_isys_convert_compressed_format(
+			   &stream_cfg->source.port.compression,
+			   isys_stream_descr);
+		if (err)
+			rc = false;
+
+		/* metadata */
+		isys_stream_descr->metadata.enable = false;
+		if (stream_cfg->metadata_config.resolution.height > 0) {
+			err = ia_css_isys_convert_stream_format_to_mipi_format(
+				  stream_cfg->metadata_config.data_type,
+				  MIPI_PREDICTOR_NONE,
+				  &fmt_type);
+			if (err)
+				rc = false;
+			isys_stream_descr->metadata.fmt_type = fmt_type;
+			isys_stream_descr->metadata.bits_per_pixel =
+			    ia_css_util_input_format_bpp(stream_cfg->metadata_config.data_type, true);
+			isys_stream_descr->metadata.pixels_per_line =
+			    stream_cfg->metadata_config.resolution.width;
+			isys_stream_descr->metadata.lines_per_frame =
+			    stream_cfg->metadata_config.resolution.height;
+#ifdef ISP2401
+			/*
+			 * For new input system, number of str2mmio requests must be even.
+			 * So we round up number of metadata lines to be even.
+			 */
+			if (isys_stream_descr->metadata.lines_per_frame > 0)
+				isys_stream_descr->metadata.lines_per_frame +=
+				    (isys_stream_descr->metadata.lines_per_frame & 1);
+#endif
+			isys_stream_descr->metadata.align_req_in_bytes =
+			    ia_css_csi2_calculate_input_system_alignment(
+				stream_cfg->metadata_config.data_type);
+			isys_stream_descr->metadata.enable = true;
+		}
+
+		break;
+	}
+	default:
+		rc = false;
+		break;
+	}
+
+	return rc;
+}
+
+static bool sh_css_translate_stream_cfg_to_input_system_input_port_resolution(
+    struct ia_css_stream_config *stream_cfg,
+    ia_css_isys_descr_t	*isys_stream_descr,
+    int isys_stream_idx)
+{
+	unsigned int bits_per_subpixel;
+	unsigned int max_subpixels_per_line;
+	unsigned int lines_per_frame;
+	unsigned int align_req_in_bytes;
+	enum atomisp_input_format fmt_type;
+
+	fmt_type = stream_cfg->isys_config[isys_stream_idx].format;
+	if ((stream_cfg->mode == IA_CSS_INPUT_MODE_SENSOR ||
+	     stream_cfg->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) &&
+	    stream_cfg->source.port.compression.type != IA_CSS_CSI2_COMPRESSION_TYPE_NONE) {
+		if (stream_cfg->source.port.compression.uncompressed_bits_per_pixel ==
+		    UNCOMPRESSED_BITS_PER_PIXEL_10)
+			fmt_type = ATOMISP_INPUT_FORMAT_RAW_10;
+		else if (stream_cfg->source.port.compression.uncompressed_bits_per_pixel ==
+			   UNCOMPRESSED_BITS_PER_PIXEL_12)
+			fmt_type = ATOMISP_INPUT_FORMAT_RAW_12;
+		else
+			return false;
+	}
+
+	bits_per_subpixel =
+	    sh_css_stream_format_2_bits_per_subpixel(fmt_type);
+	if (bits_per_subpixel == 0)
+		return false;
+
+	max_subpixels_per_line =
+	    csi2_protocol_calculate_max_subpixels_per_line(fmt_type,
+		    stream_cfg->isys_config[isys_stream_idx].input_res.width);
+	if (max_subpixels_per_line == 0)
+		return false;
+
+	lines_per_frame = stream_cfg->isys_config[isys_stream_idx].input_res.height;
+	if (lines_per_frame == 0)
+		return false;
+
+	align_req_in_bytes = ia_css_csi2_calculate_input_system_alignment(fmt_type);
+
+	/* HW needs subpixel info for their settings */
+	isys_stream_descr->input_port_resolution.bits_per_pixel = bits_per_subpixel;
+	isys_stream_descr->input_port_resolution.pixels_per_line =
+	    max_subpixels_per_line;
+	isys_stream_descr->input_port_resolution.lines_per_frame = lines_per_frame;
+	isys_stream_descr->input_port_resolution.align_req_in_bytes =
+	    align_req_in_bytes;
+
+	return true;
+}
+
+static bool sh_css_translate_stream_cfg_to_isys_stream_descr(
+    struct ia_css_stream_config *stream_cfg,
+    bool early_polling,
+    ia_css_isys_descr_t	*isys_stream_descr,
+    int isys_stream_idx)
+{
+	bool rc;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_translate_stream_cfg_to_isys_stream_descr() enter:\n");
+	rc  = sh_css_translate_stream_cfg_to_input_system_input_port_id(stream_cfg,
+		isys_stream_descr);
+	rc &= sh_css_translate_stream_cfg_to_input_system_input_port_type(stream_cfg,
+		isys_stream_descr);
+	rc &= sh_css_translate_stream_cfg_to_input_system_input_port_attr(stream_cfg,
+		isys_stream_descr, isys_stream_idx);
+	rc &= sh_css_translate_stream_cfg_to_input_system_input_port_resolution(
+		  stream_cfg, isys_stream_descr, isys_stream_idx);
+
+	isys_stream_descr->raw_packed = stream_cfg->pack_raw_pixels;
+	isys_stream_descr->linked_isys_stream_id = (int8_t)
+		stream_cfg->isys_config[isys_stream_idx].linked_isys_stream_id;
+
+	if (IS_ISP2401)
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "sh_css_translate_stream_cfg_to_isys_stream_descr() leave:\n");
+
+	return rc;
+}
+
+static bool sh_css_translate_binary_info_to_input_system_output_port_attr(
+    struct ia_css_binary *binary,
+    ia_css_isys_descr_t     *isys_stream_descr)
+{
+	if (!binary)
+		return false;
+
+	isys_stream_descr->output_port_attr.left_padding = binary->left_padding;
+	isys_stream_descr->output_port_attr.max_isp_input_width =
+	    binary->info->sp.input.max_width;
+
+	return true;
+}
+
+static int
+sh_css_config_input_network(struct ia_css_stream *stream)
+{
+	bool					rc;
+	ia_css_isys_descr_t			isys_stream_descr;
+	unsigned int				sp_thread_id;
+	struct sh_css_sp_pipeline_terminal	*sp_pipeline_input_terminal;
+	struct ia_css_pipe *pipe = NULL;
+	struct ia_css_binary *binary = NULL;
+	int i;
+	u32 isys_stream_id;
+	bool early_polling = false;
+
+	assert(stream);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_config_input_network() enter 0x%p:\n", stream);
+
+	if (stream->config.continuous) {
+		if (stream->last_pipe->config.mode == IA_CSS_PIPE_MODE_CAPTURE)
+			pipe = stream->last_pipe;
+		else if (stream->last_pipe->config.mode == IA_CSS_PIPE_MODE_YUVPP)
+			pipe = stream->last_pipe;
+		else if (stream->last_pipe->config.mode == IA_CSS_PIPE_MODE_PREVIEW)
+			pipe = stream->last_pipe->pipe_settings.preview.copy_pipe;
+		else if (stream->last_pipe->config.mode == IA_CSS_PIPE_MODE_VIDEO)
+			pipe = stream->last_pipe->pipe_settings.video.copy_pipe;
+	} else {
+		pipe = stream->last_pipe;
+	}
+
+	if (!pipe)
+		return -EINVAL;
+
+	if (pipe->pipeline.stages)
+		if (pipe->pipeline.stages->binary)
+			binary = pipe->pipeline.stages->binary;
+
+	if (binary) {
+		/*
+		 * this was being done in ifmtr in 2400.
+		 * online and cont bypass the init_in_frameinfo_memory_defaults
+		 * so need to do it here
+		 */
+		ia_css_get_crop_offsets(pipe, &binary->in_frame_info);
+	}
+
+	/* get the SP thread id */
+	rc = ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &sp_thread_id);
+	if (!rc)
+		return -EINVAL;
+	/* get the target input terminal */
+	sp_pipeline_input_terminal = &sh_css_sp_group.pipe_io[sp_thread_id].input;
+
+	for (i = 0; i < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH; i++) {
+		/* initialization */
+		memset((void *)(&isys_stream_descr), 0, sizeof(ia_css_isys_descr_t));
+		sp_pipeline_input_terminal->context.virtual_input_system_stream[i].valid = 0;
+		sp_pipeline_input_terminal->ctrl.virtual_input_system_stream_cfg[i].valid = 0;
+
+		if (!stream->config.isys_config[i].valid)
+			continue;
+
+		/* translate the stream configuration to the Input System (2401) configuration */
+		rc = sh_css_translate_stream_cfg_to_isys_stream_descr(
+			 &stream->config,
+			 early_polling,
+			 &(isys_stream_descr), i);
+
+		if (stream->config.online) {
+			rc &= sh_css_translate_binary_info_to_input_system_output_port_attr(
+				  binary,
+				  &(isys_stream_descr));
+		}
+
+		if (!rc)
+			return -EINVAL;
+
+		isys_stream_id = ia_css_isys_generate_stream_id(sp_thread_id, i);
+
+		/* create the virtual Input System (2401) */
+		rc =  ia_css_isys_stream_create(
+			  &(isys_stream_descr),
+			  &sp_pipeline_input_terminal->context.virtual_input_system_stream[i],
+			  isys_stream_id);
+		if (!rc)
+			return -EINVAL;
+
+		/* calculate the configuration of the virtual Input System (2401) */
+		rc = ia_css_isys_stream_calculate_cfg(
+			 &sp_pipeline_input_terminal->context.virtual_input_system_stream[i],
+			 &(isys_stream_descr),
+			 &sp_pipeline_input_terminal->ctrl.virtual_input_system_stream_cfg[i]);
+		if (!rc) {
+			ia_css_isys_stream_destroy(
+			    &sp_pipeline_input_terminal->context.virtual_input_system_stream[i]);
+			return -EINVAL;
+		}
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_config_input_network() leave:\n");
+
+	return 0;
+}
+
+static inline struct ia_css_pipe *stream_get_last_pipe(
+    struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *last_pipe = NULL;
+
+	if (stream)
+		last_pipe = stream->last_pipe;
+
+	return last_pipe;
+}
+
+static inline struct ia_css_pipe *stream_get_copy_pipe(
+    struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *copy_pipe = NULL;
+	struct ia_css_pipe *last_pipe = NULL;
+	enum ia_css_pipe_id pipe_id;
+
+	last_pipe = stream_get_last_pipe(stream);
+
+	if ((stream) &&
+	    (last_pipe) &&
+	    (stream->config.continuous)) {
+		pipe_id = last_pipe->mode;
+		switch (pipe_id) {
+		case IA_CSS_PIPE_ID_PREVIEW:
+			copy_pipe = last_pipe->pipe_settings.preview.copy_pipe;
+			break;
+		case IA_CSS_PIPE_ID_VIDEO:
+			copy_pipe = last_pipe->pipe_settings.video.copy_pipe;
+			break;
+		default:
+			copy_pipe = NULL;
+			break;
+		}
+	}
+
+	return copy_pipe;
+}
+
+static inline struct ia_css_pipe *stream_get_target_pipe(
+    struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *target_pipe;
+
+	/* get the pipe that consumes the stream */
+	if (stream->config.continuous)
+		target_pipe = stream_get_copy_pipe(stream);
+	else
+		target_pipe = stream_get_last_pipe(stream);
+
+	return target_pipe;
+}
+
+static int stream_csi_rx_helper(
+    struct ia_css_stream *stream,
+    int (*func)(enum mipi_port_id, uint32_t))
+{
+	int retval = -EINVAL;
+	u32 sp_thread_id, stream_id;
+	bool rc;
+	struct ia_css_pipe *target_pipe = NULL;
+
+	if ((!stream) || (stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR))
+		goto exit;
+
+	target_pipe = stream_get_target_pipe(stream);
+
+	if (!target_pipe)
+		goto exit;
+
+	rc = ia_css_pipeline_get_sp_thread_id(
+		 ia_css_pipe_get_pipe_num(target_pipe),
+		 &sp_thread_id);
+
+	if (!rc)
+		goto exit;
+
+	/* (un)register all valid "virtual isys streams" within the ia_css_stream */
+	stream_id = 0;
+	do {
+		if (stream->config.isys_config[stream_id].valid) {
+			u32 isys_stream_id = ia_css_isys_generate_stream_id(sp_thread_id, stream_id);
+
+			retval = func(stream->config.source.port.port, isys_stream_id);
+		}
+		stream_id++;
+	} while ((retval == 0) &&
+		 (stream_id < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH));
+
+exit:
+	return retval;
+}
+
+static inline int stream_register_with_csi_rx(
+    struct ia_css_stream *stream)
+{
+	return stream_csi_rx_helper(stream, ia_css_isys_csi_rx_register_stream);
+}
+
+static inline int stream_unregister_with_csi_rx(
+    struct ia_css_stream *stream)
+{
+	return stream_csi_rx_helper(stream, ia_css_isys_csi_rx_unregister_stream);
+}
+#endif
+
+
+static void
+start_binary(struct ia_css_pipe *pipe,
+	     struct ia_css_binary *binary)
+{
+	assert(pipe);
+	/* Acceleration uses firmware, the binary thus can be NULL */
+
+	if (binary)
+		sh_css_metrics_start_binary(&binary->metrics);
+
+
+#if !defined(ISP2401)
+	if (pipe->stream->reconfigure_css_rx) {
+		ia_css_isys_rx_configure(&pipe->stream->csi_rx_config,
+					 pipe->stream->config.mode);
+		pipe->stream->reconfigure_css_rx = false;
+	}
+#endif
+}
+
+/* start the copy function on the SP */
+static int
+start_copy_on_sp(struct ia_css_pipe *pipe,
+		 struct ia_css_frame *out_frame)
+{
+	(void)out_frame;
+
+	if ((!pipe) || (!pipe->stream))
+		return -EINVAL;
+
+#if !defined(ISP2401)
+	if (pipe->stream->reconfigure_css_rx)
+		ia_css_isys_rx_disable();
+#endif
+
+	if (pipe->stream->config.input_config.format != ATOMISP_INPUT_FORMAT_BINARY_8)
+		return -EINVAL;
+	sh_css_sp_start_binary_copy(ia_css_pipe_get_pipe_num(pipe), out_frame, pipe->stream->config.pixels_per_clock == 2);
+
+#if !defined(ISP2401)
+	if (pipe->stream->reconfigure_css_rx) {
+		ia_css_isys_rx_configure(&pipe->stream->csi_rx_config,
+					 pipe->stream->config.mode);
+		pipe->stream->reconfigure_css_rx = false;
+	}
+#endif
+
+	return 0;
+}
+
+void sh_css_binary_args_reset(struct sh_css_binary_args *args)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++)
+		args->tnr_frames[i] = NULL;
+	for (i = 0; i < MAX_NUM_VIDEO_DELAY_FRAMES; i++)
+		args->delay_frames[i] = NULL;
+	args->in_frame      = NULL;
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++)
+		args->out_frame[i] = NULL;
+	args->out_vf_frame  = NULL;
+	args->copy_vf       = false;
+	args->copy_output   = true;
+	args->vf_downscale_log2 = 0;
+}
+
+static void start_pipe(
+    struct ia_css_pipe *me,
+    enum sh_css_pipe_config_override copy_ovrd,
+    enum ia_css_input_mode input_mode)
+{
+	IA_CSS_ENTER_PRIVATE("me = %p, copy_ovrd = %d, input_mode = %d",
+			     me, copy_ovrd, input_mode);
+
+	assert(me); /* all callers are in this file and call with non null argument */
+
+	sh_css_sp_init_pipeline(&me->pipeline,
+				me->mode,
+				(uint8_t)ia_css_pipe_get_pipe_num(me),
+				me->config.default_capture_config.enable_xnr != 0,
+				me->stream->config.pixels_per_clock == 2,
+				me->stream->config.continuous,
+				false,
+				me->required_bds_factor,
+				copy_ovrd,
+				input_mode,
+				&me->stream->config.metadata_config,
+				&me->stream->info.metadata_info
+				, (input_mode == IA_CSS_INPUT_MODE_MEMORY) ?
+				(enum mipi_port_id)0 :
+				me->stream->config.source.port.port);
+
+	if (me->config.mode != IA_CSS_PIPE_MODE_COPY) {
+		struct ia_css_pipeline_stage *stage;
+
+		stage = me->pipeline.stages;
+		if (stage) {
+			me->pipeline.current_stage = stage;
+			start_binary(me, stage->binary);
+		}
+	}
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+sh_css_invalidate_shading_tables(struct ia_css_stream *stream)
+{
+	int i;
+
+	assert(stream);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_invalidate_shading_tables() enter:\n");
+
+	for (i = 0; i < stream->num_pipes; i++) {
+		assert(stream->pipes[i]);
+		sh_css_pipe_free_shading_table(stream->pipes[i]);
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_invalidate_shading_tables() leave: return_void\n");
+}
+
+static void
+enable_interrupts(enum ia_css_irq_type irq_type)
+{
+#ifndef ISP2401
+	enum mipi_port_id port;
+#endif
+	bool enable_pulse = irq_type != IA_CSS_IRQ_TYPE_EDGE;
+
+	IA_CSS_ENTER_PRIVATE("");
+	/* Enable IRQ on the SP which signals that SP goes to idle
+	 * (aka ready state) */
+	cnd_sp_irq_enable(SP0_ID, true);
+	/* Set the IRQ device 0 to either level or pulse */
+	irq_enable_pulse(IRQ0_ID, enable_pulse);
+
+	cnd_virq_enable_channel(virq_sp, true);
+
+	/* Enable SW interrupt 0, this is used to signal ISYS events */
+	cnd_virq_enable_channel(
+	    (enum virq_id)(IRQ_SW_CHANNEL0_ID + IRQ_SW_CHANNEL_OFFSET),
+	    true);
+	/* Enable SW interrupt 1, this is used to signal PSYS events */
+	cnd_virq_enable_channel(
+	    (enum virq_id)(IRQ_SW_CHANNEL1_ID + IRQ_SW_CHANNEL_OFFSET),
+	    true);
+
+#ifndef ISP2401
+	for (port = 0; port < N_MIPI_PORT_ID; port++)
+		ia_css_isys_rx_enable_all_interrupts(port);
+#endif
+
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+static bool sh_css_setup_spctrl_config(const struct ia_css_fw_info *fw,
+				       const char *program,
+				       ia_css_spctrl_cfg  *spctrl_cfg)
+{
+	if ((!fw) || (!spctrl_cfg))
+		return false;
+	spctrl_cfg->sp_entry = 0;
+	spctrl_cfg->program_name = (char *)(program);
+
+	spctrl_cfg->ddr_data_offset =  fw->blob.data_source;
+	spctrl_cfg->dmem_data_addr = fw->blob.data_target;
+	spctrl_cfg->dmem_bss_addr = fw->blob.bss_target;
+	spctrl_cfg->data_size = fw->blob.data_size;
+	spctrl_cfg->bss_size = fw->blob.bss_size;
+
+	spctrl_cfg->spctrl_config_dmem_addr = fw->info.sp.init_dmem_data;
+	spctrl_cfg->spctrl_state_dmem_addr = fw->info.sp.sw_state;
+
+	spctrl_cfg->code_size = fw->blob.size;
+	spctrl_cfg->code      = fw->blob.code;
+	spctrl_cfg->sp_entry  = fw->info.sp.sp_entry; /* entry function ptr on SP */
+
+	return true;
+}
+
+void
+ia_css_unload_firmware(void)
+{
+	if (sh_css_num_binaries) {
+		/* we have already loaded before so get rid of the old stuff */
+		ia_css_binary_uninit();
+		sh_css_unload_firmware();
+	}
+	fw_explicitly_loaded = false;
+}
+
+static void
+ia_css_reset_defaults(struct sh_css *css)
+{
+	struct sh_css default_css;
+
+	/* Reset everything to zero */
+	memset(&default_css, 0, sizeof(default_css));
+
+	/* Initialize the non zero values */
+	default_css.check_system_idle = true;
+	default_css.num_cont_raw_frames = NUM_CONTINUOUS_FRAMES;
+
+	/*
+	 * All should be 0: but memset does it already.
+	 * default_css.num_mipi_frames[N_CSI_PORTS] = 0;
+	 */
+
+	default_css.irq_type = IA_CSS_IRQ_TYPE_EDGE;
+
+	/* Set the defaults to the output */
+	*css = default_css;
+}
+
+int
+ia_css_load_firmware(struct device *dev, const struct ia_css_env *env,
+		     const struct ia_css_fw  *fw)
+{
+	int err;
+
+	if (!env)
+		return -EINVAL;
+	if (!fw)
+		return -EINVAL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_load_firmware() enter\n");
+
+	/* make sure we initialize my_css */
+	if (my_css.flush != env->cpu_mem_env.flush) {
+		ia_css_reset_defaults(&my_css);
+		my_css.flush = env->cpu_mem_env.flush;
+	}
+
+	ia_css_unload_firmware(); /* in case we are called twice */
+	err = sh_css_load_firmware(dev, fw->data, fw->bytes);
+	if (!err) {
+		err = ia_css_binary_init_infos();
+		if (!err)
+			fw_explicitly_loaded = true;
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_load_firmware() leave\n");
+	return err;
+}
+
+int
+ia_css_init(struct device *dev, const struct ia_css_env *env,
+	    const struct ia_css_fw  *fw,
+	    u32                 mmu_l1_base,
+	    enum ia_css_irq_type     irq_type)
+{
+	int err;
+	ia_css_spctrl_cfg spctrl_cfg;
+
+	void (*flush_func)(struct ia_css_acc_fw *fw);
+	hrt_data select, enable;
+
+	/*
+	 * The C99 standard does not specify the exact object representation of structs;
+	 * the representation is compiler dependent.
+	 *
+	 * The structs that are communicated between host and SP/ISP should have the
+	 * exact same object representation. The compiler that is used to compile the
+	 * firmware is hivecc.
+	 *
+	 * To check if a different compiler, used to compile a host application, uses
+	 * another object representation, macros are defined specifying the size of
+	 * the structs as expected by the firmware.
+	 *
+	 * A host application shall verify that a sizeof( ) of the struct is equal to
+	 * the SIZE_OF_XXX macro of the corresponding struct. If they are not
+	 * equal, functionality will break.
+	 */
+
+	/* Check struct sh_css_ddr_address_map */
+	COMPILATION_ERROR_IF(sizeof(struct sh_css_ddr_address_map)		!= SIZE_OF_SH_CSS_DDR_ADDRESS_MAP_STRUCT);
+	/* Check struct host_sp_queues */
+	COMPILATION_ERROR_IF(sizeof(struct host_sp_queues)			!= SIZE_OF_HOST_SP_QUEUES_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_circbuf_desc_s)		!= SIZE_OF_IA_CSS_CIRCBUF_DESC_S_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_circbuf_elem_s)		!= SIZE_OF_IA_CSS_CIRCBUF_ELEM_S_STRUCT);
+
+	/* Check struct host_sp_communication */
+	COMPILATION_ERROR_IF(sizeof(struct host_sp_communication)		!= SIZE_OF_HOST_SP_COMMUNICATION_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct sh_css_event_irq_mask)		!= SIZE_OF_SH_CSS_EVENT_IRQ_MASK_STRUCT);
+
+	/* Check struct sh_css_hmm_buffer */
+	COMPILATION_ERROR_IF(sizeof(struct sh_css_hmm_buffer)			!= SIZE_OF_SH_CSS_HMM_BUFFER_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_isp_3a_statistics)		!= SIZE_OF_IA_CSS_ISP_3A_STATISTICS_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_isp_dvs_statistics)		!= SIZE_OF_IA_CSS_ISP_DVS_STATISTICS_STRUCT);
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_metadata)			!= SIZE_OF_IA_CSS_METADATA_STRUCT);
+
+	/* Check struct ia_css_init_dmem_cfg */
+	COMPILATION_ERROR_IF(sizeof(struct ia_css_sp_init_dmem_cfg)		!= SIZE_OF_IA_CSS_SP_INIT_DMEM_CFG_STRUCT);
+
+	if (!fw && !fw_explicitly_loaded)
+		return -EINVAL;
+	if (!env)
+		return -EINVAL;
+
+	sh_css_printf = env->print_env.debug_print;
+
+	IA_CSS_ENTER("void");
+
+	flush_func     = env->cpu_mem_env.flush;
+
+	pipe_global_init();
+	ia_css_pipeline_init();
+	ia_css_queue_map_init();
+
+	ia_css_device_access_init(&env->hw_access_env);
+
+	select = gpio_reg_load(GPIO0_ID, _gpio_block_reg_do_select)
+	& (~GPIO_FLASH_PIN_MASK);
+	enable = gpio_reg_load(GPIO0_ID, _gpio_block_reg_do_e)
+	| GPIO_FLASH_PIN_MASK;
+	sh_css_mmu_set_page_table_base_index(mmu_l1_base);
+
+	my_css_save.mmu_base = mmu_l1_base;
+
+	ia_css_reset_defaults(&my_css);
+
+	my_css_save.driver_env = *env;
+	my_css.flush     = flush_func;
+
+	err = ia_css_rmgr_init();
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	IA_CSS_LOG("init: %d", my_css_save_initialized);
+
+	if (!my_css_save_initialized) {
+		my_css_save_initialized = true;
+		my_css_save.mode = sh_css_mode_working;
+		memset(my_css_save.stream_seeds, 0,
+		       sizeof(struct sh_css_stream_seed) * MAX_ACTIVE_STREAMS);
+		IA_CSS_LOG("init: %d mode=%d", my_css_save_initialized, my_css_save.mode);
+	}
+
+	mipi_init();
+
+#ifndef ISP2401
+	/*
+	 * In case this has been programmed already, update internal
+	 * data structure ...
+	 * DEPRECATED
+	 */
+	my_css.page_table_base_index = mmu_get_page_table_base_index(MMU0_ID);
+
+#endif
+	my_css.irq_type = irq_type;
+
+	my_css_save.irq_type = irq_type;
+
+	enable_interrupts(my_css.irq_type);
+
+	/* configure GPIO to output mode */
+	gpio_reg_store(GPIO0_ID, _gpio_block_reg_do_select, select);
+	gpio_reg_store(GPIO0_ID, _gpio_block_reg_do_e, enable);
+	gpio_reg_store(GPIO0_ID, _gpio_block_reg_do_0, 0);
+
+	err = ia_css_refcount_init(REFCOUNT_SIZE);
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	err = sh_css_params_init();
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	if (fw) {
+		ia_css_unload_firmware(); /* in case we already had firmware loaded */
+		err = sh_css_load_firmware(dev, fw->data, fw->bytes);
+		if (err) {
+			IA_CSS_LEAVE_ERR(err);
+			return err;
+		}
+		err = ia_css_binary_init_infos();
+		if (err) {
+			IA_CSS_LEAVE_ERR(err);
+			return err;
+		}
+		fw_explicitly_loaded = false;
+
+		my_css_save.loaded_fw = (struct ia_css_fw *)fw;
+	}
+	if (!sh_css_setup_spctrl_config(&sh_css_sp_fw, SP_PROG_NAME, &spctrl_cfg))
+		return -EINVAL;
+
+	err = ia_css_spctrl_load_fw(SP0_ID, &spctrl_cfg);
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	if (!sh_css_hrt_system_is_idle()) {
+		IA_CSS_LEAVE_ERR(-EBUSY);
+		return -EBUSY;
+	}
+	/*
+	 * can be called here, queuing works, but:
+	 * - when sp is started later, it will wipe queued items
+	 * so for now we leave it for later and make sure
+	 * updates are not called to frequently.
+	 * sh_css_init_buffer_queues();
+	 */
+
+#if defined(ISP2401)
+	gp_device_reg_store(GP_DEVICE0_ID, _REG_GP_SWITCH_ISYS2401_ADDR, 1);
+#endif
+
+
+	if (!IS_ISP2401)
+		dma_set_max_burst_size(DMA0_ID, HIVE_DMA_BUS_DDR_CONN,
+				       ISP2400_DMA_MAX_BURST_LENGTH);
+	else
+		dma_set_max_burst_size(DMA0_ID, HIVE_DMA_BUS_DDR_CONN,
+				       ISP2401_DMA_MAX_BURST_LENGTH);
+
+	if (ia_css_isys_init() != INPUT_SYSTEM_ERR_NO_ERROR)
+		err = -EINVAL;
+
+	sh_css_params_map_and_store_default_gdc_lut();
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+int
+ia_css_enable_isys_event_queue(bool enable)
+{
+	if (sh_css_sp_is_running())
+		return -EBUSY;
+	sh_css_sp_enable_isys_event_queue(enable);
+	return 0;
+}
+
+/* For Acceleration API: Flush FW (shared buffer pointer) arguments */
+void
+sh_css_flush(struct ia_css_acc_fw *fw)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_flush() enter:\n");
+	if ((fw) && (my_css.flush))
+		my_css.flush(fw);
+}
+
+/*
+ * Mapping sp threads. Currently, this is done when a stream is created and
+ * pipelines are ready to be converted to sp pipelines. Be careful if you are
+ * doing it from stream_create since we could run out of sp threads due to
+ * allocation on inactive pipelines.
+ */
+static int
+map_sp_threads(struct ia_css_stream *stream, bool map)
+{
+	struct ia_css_pipe *main_pipe = NULL;
+	struct ia_css_pipe *copy_pipe = NULL;
+	struct ia_css_pipe *capture_pipe = NULL;
+	struct ia_css_pipe *acc_pipe = NULL;
+	int err = 0;
+	enum ia_css_pipe_id pipe_id;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p, map = %s",
+			     stream, map ? "true" : "false");
+
+	if (!stream) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	main_pipe = stream->last_pipe;
+	pipe_id	= main_pipe->mode;
+
+	ia_css_pipeline_map(main_pipe->pipe_num, map);
+
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		copy_pipe    = main_pipe->pipe_settings.preview.copy_pipe;
+		capture_pipe = main_pipe->pipe_settings.preview.capture_pipe;
+		acc_pipe     = main_pipe->pipe_settings.preview.acc_pipe;
+		break;
+
+	case IA_CSS_PIPE_ID_VIDEO:
+		copy_pipe    = main_pipe->pipe_settings.video.copy_pipe;
+		capture_pipe = main_pipe->pipe_settings.video.capture_pipe;
+		break;
+
+	case IA_CSS_PIPE_ID_CAPTURE:
+	case IA_CSS_PIPE_ID_ACC:
+	default:
+		break;
+	}
+
+	if (acc_pipe)
+		ia_css_pipeline_map(acc_pipe->pipe_num, map);
+
+	if (capture_pipe)
+		ia_css_pipeline_map(capture_pipe->pipe_num, map);
+
+	/* Firmware expects copy pipe to be the last pipe mapped. (if needed) */
+	if (copy_pipe)
+		ia_css_pipeline_map(copy_pipe->pipe_num, map);
+
+	/* DH regular multi pipe - not continuous mode: map the next pipes too */
+	if (!stream->config.continuous) {
+		int i;
+
+		for (i = 1; i < stream->num_pipes; i++)
+			ia_css_pipeline_map(stream->pipes[i]->pipe_num, map);
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/*
+ * creates a host pipeline skeleton for all pipes in a stream. Called during
+ * stream_create.
+ */
+static int
+create_host_pipeline_structure(struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *copy_pipe = NULL, *capture_pipe = NULL;
+	struct ia_css_pipe *acc_pipe = NULL;
+	enum ia_css_pipe_id pipe_id;
+	struct ia_css_pipe *main_pipe = NULL;
+	int err = 0;
+	unsigned int copy_pipe_delay = 0,
+	capture_pipe_delay = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+
+	if (!stream) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	main_pipe	= stream->last_pipe;
+	if (!main_pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipe_id	= main_pipe->mode;
+
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		copy_pipe    = main_pipe->pipe_settings.preview.copy_pipe;
+		copy_pipe_delay = main_pipe->dvs_frame_delay;
+		capture_pipe = main_pipe->pipe_settings.preview.capture_pipe;
+		capture_pipe_delay = IA_CSS_FRAME_DELAY_0;
+		acc_pipe     = main_pipe->pipe_settings.preview.acc_pipe;
+		err = ia_css_pipeline_create(&main_pipe->pipeline, main_pipe->mode,
+					     main_pipe->pipe_num, main_pipe->dvs_frame_delay);
+		break;
+
+	case IA_CSS_PIPE_ID_VIDEO:
+		copy_pipe    = main_pipe->pipe_settings.video.copy_pipe;
+		copy_pipe_delay = main_pipe->dvs_frame_delay;
+		capture_pipe = main_pipe->pipe_settings.video.capture_pipe;
+		capture_pipe_delay = IA_CSS_FRAME_DELAY_0;
+		err = ia_css_pipeline_create(&main_pipe->pipeline, main_pipe->mode,
+					     main_pipe->pipe_num, main_pipe->dvs_frame_delay);
+		break;
+
+	case IA_CSS_PIPE_ID_CAPTURE:
+		capture_pipe = main_pipe;
+		capture_pipe_delay = main_pipe->dvs_frame_delay;
+		break;
+
+	case IA_CSS_PIPE_ID_YUVPP:
+		err = ia_css_pipeline_create(&main_pipe->pipeline, main_pipe->mode,
+					     main_pipe->pipe_num, main_pipe->dvs_frame_delay);
+		break;
+
+	case IA_CSS_PIPE_ID_ACC:
+		err = ia_css_pipeline_create(&main_pipe->pipeline, main_pipe->mode,
+					     main_pipe->pipe_num, main_pipe->dvs_frame_delay);
+		break;
+
+	default:
+		err = -EINVAL;
+	}
+
+	if (!(err) && copy_pipe)
+		err = ia_css_pipeline_create(&copy_pipe->pipeline,
+					     copy_pipe->mode,
+					     copy_pipe->pipe_num,
+					     copy_pipe_delay);
+
+	if (!(err) && capture_pipe)
+		err = ia_css_pipeline_create(&capture_pipe->pipeline,
+					     capture_pipe->mode,
+					     capture_pipe->pipe_num,
+					     capture_pipe_delay);
+
+	if (!(err) && acc_pipe)
+		err = ia_css_pipeline_create(&acc_pipe->pipeline, acc_pipe->mode,
+					     acc_pipe->pipe_num, main_pipe->dvs_frame_delay);
+
+	/* DH regular multi pipe - not continuous mode: create the next pipelines too */
+	if (!stream->config.continuous) {
+		int i;
+
+		for (i = 1; i < stream->num_pipes && 0 == err; i++) {
+			main_pipe = stream->pipes[i];
+			err = ia_css_pipeline_create(&main_pipe->pipeline,
+						     main_pipe->mode,
+						     main_pipe->pipe_num,
+						     main_pipe->dvs_frame_delay);
+		}
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/*
+ * creates a host pipeline for all pipes in a stream. Called during
+ * stream_start.
+ */
+static int
+create_host_pipeline(struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *copy_pipe = NULL, *capture_pipe = NULL;
+	struct ia_css_pipe *acc_pipe = NULL;
+	enum ia_css_pipe_id pipe_id;
+	struct ia_css_pipe *main_pipe = NULL;
+	int err = 0;
+	unsigned int max_input_width = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+	if (!stream) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	main_pipe	= stream->last_pipe;
+	pipe_id	= main_pipe->mode;
+
+	/*
+	 * No continuous frame allocation for capture pipe. It uses the
+	 * "main" pipe's frames.
+	 */
+	if ((pipe_id == IA_CSS_PIPE_ID_PREVIEW) ||
+	    (pipe_id == IA_CSS_PIPE_ID_VIDEO)) {
+		/*
+		 * About
+		 *    pipe_id == IA_CSS_PIPE_ID_PREVIEW &&
+		 *    stream->config.mode != IA_CSS_INPUT_MODE_MEMORY:
+		 *
+		 * The original condition pipe_id == IA_CSS_PIPE_ID_PREVIEW is
+		 * too strong. E.g. in SkyCam (with memory based input frames)
+		 * there is no continuous mode and thus no need for allocated
+		 * continuous frames.
+		 * This is not only for SkyCam but for all preview cases that
+		 * use DDR based input frames. For this reason the
+		 * stream->config.mode != IA_CSS_INPUT_MODE_MEMORY has beed
+		 * added.
+		 */
+		if (stream->config.continuous ||
+		    (pipe_id == IA_CSS_PIPE_ID_PREVIEW &&
+		     stream->config.mode != IA_CSS_INPUT_MODE_MEMORY)) {
+			err = alloc_continuous_frames(main_pipe, true);
+			if (err)
+				goto ERR;
+		}
+	}
+
+#if !defined(ISP2401)
+	/* old isys: need to allocate_mipi_frames() even in IA_CSS_PIPE_MODE_COPY */
+	if (pipe_id != IA_CSS_PIPE_ID_ACC) {
+		err = allocate_mipi_frames(main_pipe, &stream->info);
+		if (err)
+			goto ERR;
+	}
+#elif defined(ISP2401)
+	if ((pipe_id != IA_CSS_PIPE_ID_ACC) &&
+	    (main_pipe->config.mode != IA_CSS_PIPE_MODE_COPY)) {
+		err = allocate_mipi_frames(main_pipe, &stream->info);
+		if (err)
+			goto ERR;
+	}
+#endif
+
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		copy_pipe    = main_pipe->pipe_settings.preview.copy_pipe;
+		capture_pipe = main_pipe->pipe_settings.preview.capture_pipe;
+		acc_pipe     = main_pipe->pipe_settings.preview.acc_pipe;
+		max_input_width =
+		    main_pipe->pipe_settings.preview.preview_binary.info->sp.input.max_width;
+
+		err = create_host_preview_pipeline(main_pipe);
+		if (err)
+			goto ERR;
+
+		break;
+
+	case IA_CSS_PIPE_ID_VIDEO:
+		copy_pipe    = main_pipe->pipe_settings.video.copy_pipe;
+		capture_pipe = main_pipe->pipe_settings.video.capture_pipe;
+		max_input_width =
+		    main_pipe->pipe_settings.video.video_binary.info->sp.input.max_width;
+
+		err = create_host_video_pipeline(main_pipe);
+		if (err)
+			goto ERR;
+
+		break;
+
+	case IA_CSS_PIPE_ID_CAPTURE:
+		capture_pipe = main_pipe;
+
+		break;
+
+	case IA_CSS_PIPE_ID_YUVPP:
+		err = create_host_yuvpp_pipeline(main_pipe);
+		if (err)
+			goto ERR;
+
+		break;
+
+	case IA_CSS_PIPE_ID_ACC:
+		err = create_host_acc_pipeline(main_pipe);
+		if (err)
+			goto ERR;
+
+		break;
+	default:
+		err = -EINVAL;
+	}
+	if (err)
+		goto ERR;
+
+	if (copy_pipe) {
+		err = create_host_copy_pipeline(copy_pipe, max_input_width,
+						main_pipe->continuous_frames[0]);
+		if (err)
+			goto ERR;
+	}
+
+	if (capture_pipe) {
+		err = create_host_capture_pipeline(capture_pipe);
+		if (err)
+			goto ERR;
+	}
+
+	if (acc_pipe) {
+		err = create_host_acc_pipeline(acc_pipe);
+		if (err)
+			goto ERR;
+	}
+
+	/* DH regular multi pipe - not continuous mode: create the next pipelines too */
+	if (!stream->config.continuous) {
+		int i;
+
+		for (i = 1; i < stream->num_pipes && 0 == err; i++) {
+			switch (stream->pipes[i]->mode) {
+			case IA_CSS_PIPE_ID_PREVIEW:
+				err = create_host_preview_pipeline(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_VIDEO:
+				err = create_host_video_pipeline(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_CAPTURE:
+				err = create_host_capture_pipeline(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_YUVPP:
+				err = create_host_yuvpp_pipeline(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_ACC:
+				err = create_host_acc_pipeline(stream->pipes[i]);
+				break;
+			default:
+				err = -EINVAL;
+			}
+			if (err)
+				goto ERR;
+		}
+	}
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static const struct ia_css_pipe default_pipe = IA_CSS_DEFAULT_PIPE;
+static const struct ia_css_preview_settings preview = IA_CSS_DEFAULT_PREVIEW_SETTINGS;
+static const struct ia_css_capture_settings capture = IA_CSS_DEFAULT_CAPTURE_SETTINGS;
+static const struct ia_css_video_settings video = IA_CSS_DEFAULT_VIDEO_SETTINGS;
+static const struct ia_css_yuvpp_settings yuvpp = IA_CSS_DEFAULT_YUVPP_SETTINGS;
+
+static int
+init_pipe_defaults(enum ia_css_pipe_mode mode,
+		   struct ia_css_pipe *pipe,
+		   bool copy_pipe)
+{
+	if (!pipe) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return -EINVAL;
+	}
+
+	/* Initialize pipe to pre-defined defaults */
+	memcpy(pipe, &default_pipe, sizeof(default_pipe));
+
+	/* TODO: JB should not be needed, but temporary backward reference */
+	switch (mode) {
+	case IA_CSS_PIPE_MODE_PREVIEW:
+		pipe->mode = IA_CSS_PIPE_ID_PREVIEW;
+		memcpy(&pipe->pipe_settings.preview, &preview, sizeof(preview));
+		break;
+	case IA_CSS_PIPE_MODE_CAPTURE:
+		if (copy_pipe)
+			pipe->mode = IA_CSS_PIPE_ID_COPY;
+		else
+			pipe->mode = IA_CSS_PIPE_ID_CAPTURE;
+
+		memcpy(&pipe->pipe_settings.capture, &capture, sizeof(capture));
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		pipe->mode = IA_CSS_PIPE_ID_VIDEO;
+		memcpy(&pipe->pipe_settings.video, &video, sizeof(video));
+		break;
+	case IA_CSS_PIPE_MODE_ACC:
+		pipe->mode = IA_CSS_PIPE_ID_ACC;
+		break;
+	case IA_CSS_PIPE_MODE_COPY:
+		pipe->mode = IA_CSS_PIPE_ID_CAPTURE;
+		break;
+	case IA_CSS_PIPE_MODE_YUVPP:
+		pipe->mode = IA_CSS_PIPE_ID_YUVPP;
+		memcpy(&pipe->pipe_settings.yuvpp, &yuvpp, sizeof(yuvpp));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+pipe_global_init(void)
+{
+	u8 i;
+
+	my_css.pipe_counter = 0;
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++)
+		my_css.all_pipes[i] = NULL;
+}
+
+static int
+pipe_generate_pipe_num(const struct ia_css_pipe *pipe,
+		       unsigned int *pipe_number)
+{
+	const u8 INVALID_PIPE_NUM = (uint8_t)~(0);
+	u8 pipe_num = INVALID_PIPE_NUM;
+	u8 i;
+
+	if (!pipe) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return -EINVAL;
+	}
+
+	/* Assign a new pipe_num .... search for empty place */
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++) {
+		if (!my_css.all_pipes[i]) {
+			/* position is reserved */
+			my_css.all_pipes[i] = (struct ia_css_pipe *)pipe;
+			pipe_num = i;
+			break;
+		}
+	}
+	if (pipe_num == INVALID_PIPE_NUM) {
+		/* Max number of pipes already allocated */
+		IA_CSS_ERROR("Max number of pipes already created");
+		return -ENOSPC;
+	}
+
+	my_css.pipe_counter++;
+
+	IA_CSS_LOG("pipe_num (%d)", pipe_num);
+
+	*pipe_number = pipe_num;
+	return 0;
+}
+
+static void
+pipe_release_pipe_num(unsigned int pipe_num)
+{
+	my_css.all_pipes[pipe_num] = NULL;
+	my_css.pipe_counter--;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "pipe_release_pipe_num (%d)\n", pipe_num);
+}
+
+static int
+create_pipe(enum ia_css_pipe_mode mode,
+	    struct ia_css_pipe **pipe,
+	    bool copy_pipe)
+{
+	int err = 0;
+	struct ia_css_pipe *me;
+
+	if (!pipe) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return -EINVAL;
+	}
+
+	me = kmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me)
+		return -ENOMEM;
+
+	err = init_pipe_defaults(mode, me, copy_pipe);
+	if (err) {
+		kfree(me);
+		return err;
+	}
+
+	err = pipe_generate_pipe_num(me, &me->pipe_num);
+	if (err) {
+		kfree(me);
+		return err;
+	}
+
+	*pipe = me;
+	return 0;
+}
+
+struct ia_css_pipe *
+find_pipe_by_num(uint32_t pipe_num)
+{
+	unsigned int i;
+
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++) {
+		if (my_css.all_pipes[i] &&
+		    ia_css_pipe_get_pipe_num(my_css.all_pipes[i]) == pipe_num) {
+			return my_css.all_pipes[i];
+		}
+	}
+	return NULL;
+}
+
+static void sh_css_pipe_free_acc_binaries(
+    struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *pipeline;
+	struct ia_css_pipeline_stage *stage;
+
+	if (!pipe) {
+		IA_CSS_ERROR("NULL input pointer");
+		return;
+	}
+	pipeline = &pipe->pipeline;
+
+	/* loop through the stages and unload them */
+	for (stage = pipeline->stages; stage; stage = stage->next) {
+		struct ia_css_fw_info *firmware = (struct ia_css_fw_info *)
+						  stage->firmware;
+		if (firmware)
+			ia_css_pipe_unload_extension(pipe, firmware);
+	}
+}
+
+int
+ia_css_pipe_destroy(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	IA_CSS_ENTER("pipe = %p", pipe);
+
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (pipe->stream) {
+		IA_CSS_LOG("ia_css_stream_destroy not called!");
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	switch (pipe->config.mode) {
+	case IA_CSS_PIPE_MODE_PREVIEW:
+		/*
+		 * need to take into account that this function is also called
+		 * on the internal copy pipe
+		 */
+		if (pipe->mode == IA_CSS_PIPE_ID_PREVIEW) {
+			ia_css_frame_free_multiple(NUM_CONTINUOUS_FRAMES,
+						   pipe->continuous_frames);
+			ia_css_metadata_free_multiple(NUM_CONTINUOUS_FRAMES,
+						      pipe->cont_md_buffers);
+			if (pipe->pipe_settings.preview.copy_pipe) {
+				err = ia_css_pipe_destroy(pipe->pipe_settings.preview.copy_pipe);
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_pipe_destroy(): destroyed internal copy pipe err=%d\n",
+						    err);
+			}
+		}
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		if (pipe->mode == IA_CSS_PIPE_ID_VIDEO) {
+			ia_css_frame_free_multiple(NUM_CONTINUOUS_FRAMES,
+						   pipe->continuous_frames);
+			ia_css_metadata_free_multiple(NUM_CONTINUOUS_FRAMES,
+						      pipe->cont_md_buffers);
+			if (pipe->pipe_settings.video.copy_pipe) {
+				err = ia_css_pipe_destroy(pipe->pipe_settings.video.copy_pipe);
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_pipe_destroy(): destroyed internal copy pipe err=%d\n",
+						    err);
+			}
+		}
+#ifndef ISP2401
+		ia_css_frame_free_multiple(NUM_VIDEO_TNR_FRAMES,
+					   pipe->pipe_settings.video.tnr_frames);
+#else
+		ia_css_frame_free_multiple(NUM_VIDEO_TNR_FRAMES,
+					   pipe->pipe_settings.video.tnr_frames);
+#endif
+		ia_css_frame_free_multiple(MAX_NUM_VIDEO_DELAY_FRAMES,
+					   pipe->pipe_settings.video.delay_frames);
+		break;
+	case IA_CSS_PIPE_MODE_CAPTURE:
+		ia_css_frame_free_multiple(MAX_NUM_VIDEO_DELAY_FRAMES,
+					   pipe->pipe_settings.capture.delay_frames);
+		break;
+	case IA_CSS_PIPE_MODE_ACC:
+		sh_css_pipe_free_acc_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_MODE_COPY:
+		break;
+	case IA_CSS_PIPE_MODE_YUVPP:
+		break;
+	}
+
+	if (pipe->scaler_pp_lut != mmgr_NULL) {
+		hmm_free(pipe->scaler_pp_lut);
+		pipe->scaler_pp_lut = mmgr_NULL;
+	}
+
+	my_css.active_pipes[ia_css_pipe_get_pipe_num(pipe)] = NULL;
+	sh_css_pipe_free_shading_table(pipe);
+
+	ia_css_pipeline_destroy(&pipe->pipeline);
+	pipe_release_pipe_num(ia_css_pipe_get_pipe_num(pipe));
+
+	/* Temporarily, not every sh_css_pipe has an acc_extension. */
+	if (pipe->config.acc_extension)
+		ia_css_pipe_unload_extension(pipe, pipe->config.acc_extension);
+
+	kfree(pipe);
+	IA_CSS_LEAVE("err = %d", err);
+	return err;
+}
+
+void
+ia_css_uninit(void)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_uninit() enter: void\n");
+
+	sh_css_params_free_default_gdc_lut();
+
+	/* TODO: JB: implement decent check and handling of freeing mipi frames */
+	if (!mipi_is_free())
+		dev_warn(atomisp_dev, "mipi frames are not freed.\n");
+
+	/* cleanup generic data */
+	sh_css_params_uninit();
+	ia_css_refcount_uninit();
+
+	ia_css_rmgr_uninit();
+
+#if !defined(ISP2401)
+	/* needed for reprogramming the inputformatter after power cycle of css */
+	ifmtr_set_if_blocking_mode_reset = true;
+#endif
+
+	if (!fw_explicitly_loaded)
+		ia_css_unload_firmware();
+
+	ia_css_spctrl_unload_fw(SP0_ID);
+	sh_css_sp_set_sp_running(false);
+	/* check and free any remaining mipi frames */
+	free_mipi_frames(NULL);
+
+	sh_css_sp_reset_global_vars();
+
+	ia_css_isys_uninit();
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_uninit() leave: return_void\n");
+}
+
+int ia_css_irq_translate(
+    unsigned int *irq_infos)
+{
+	enum virq_id	irq;
+	enum hrt_isp_css_irq_status status = hrt_isp_css_irq_status_more_irqs;
+	unsigned int infos = 0;
+
+	/* irq_infos can be NULL, but that would make the function useless */
+	/* assert(irq_infos != NULL); */
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_irq_translate() enter: irq_infos=%p\n", irq_infos);
+
+	while (status == hrt_isp_css_irq_status_more_irqs) {
+		status = virq_get_channel_id(&irq);
+		if (status == hrt_isp_css_irq_status_error)
+			return -EINVAL;
+
+
+		switch (irq) {
+		case virq_sp:
+			/*
+			 * When SP goes to idle, info is available in the
+			 * event queue.
+			 */
+			infos |= IA_CSS_IRQ_INFO_EVENTS_READY;
+			break;
+		case virq_isp:
+			break;
+		case virq_isys_sof:
+			infos |= IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF;
+			break;
+		case virq_isys_eof:
+			infos |= IA_CSS_IRQ_INFO_CSS_RECEIVER_EOF;
+			break;
+		case virq_isys_csi:
+			infos |= IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR;
+			break;
+#if !defined(ISP2401)
+		case virq_ifmt0_id:
+			infos |= IA_CSS_IRQ_INFO_IF_ERROR;
+			break;
+#endif
+		case virq_dma:
+			infos |= IA_CSS_IRQ_INFO_DMA_ERROR;
+			break;
+		case virq_sw_pin_0:
+			infos |= sh_css_get_sw_interrupt_value(0);
+			break;
+		case virq_sw_pin_1:
+			infos |= sh_css_get_sw_interrupt_value(1);
+			/* pqiao TODO: also assumption here */
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (irq_infos)
+		*irq_infos = infos;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_irq_translate() leave: irq_infos=%u\n",
+			    infos);
+
+	return 0;
+}
+
+int ia_css_irq_enable(
+    enum ia_css_irq_info info,
+    bool enable)
+{
+	enum virq_id	irq = N_virq_id;
+
+	IA_CSS_ENTER("info=%d, enable=%d", info, enable);
+
+	switch (info) {
+#if !defined(ISP2401)
+	case IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF:
+		irq = virq_isys_sof;
+		break;
+	case IA_CSS_IRQ_INFO_CSS_RECEIVER_EOF:
+		irq = virq_isys_eof;
+		break;
+	case IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR:
+		irq = virq_isys_csi;
+		break;
+	case IA_CSS_IRQ_INFO_IF_ERROR:
+		irq = virq_ifmt0_id;
+		break;
+#else
+	case IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF:
+	case IA_CSS_IRQ_INFO_CSS_RECEIVER_EOF:
+	case IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR:
+	case IA_CSS_IRQ_INFO_IF_ERROR:
+		/* Just ignore those unused IRQs without printing errors */
+		return 0;
+#endif
+	case IA_CSS_IRQ_INFO_DMA_ERROR:
+		irq = virq_dma;
+		break;
+	case IA_CSS_IRQ_INFO_SW_0:
+		irq = virq_sw_pin_0;
+		break;
+	case IA_CSS_IRQ_INFO_SW_1:
+		irq = virq_sw_pin_1;
+		break;
+	default:
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	cnd_virq_enable_channel(irq, enable);
+
+	IA_CSS_LEAVE_ERR(0);
+	return 0;
+}
+
+
+static unsigned int
+sh_css_get_sw_interrupt_value(unsigned int irq)
+{
+	unsigned int irq_value;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_get_sw_interrupt_value() enter: irq=%d\n", irq);
+	irq_value = sh_css_sp_get_sw_interrupt_value(irq);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_get_sw_interrupt_value() leave: irq_value=%d\n", irq_value);
+	return irq_value;
+}
+
+/*
+ * configure and load the copy binary, the next binary is used to
+ * determine whether the copy binary needs to do left padding.
+ */
+static int load_copy_binary(
+    struct ia_css_pipe *pipe,
+    struct ia_css_binary *copy_binary,
+    struct ia_css_binary *next_binary)
+{
+	struct ia_css_frame_info copy_out_info, copy_in_info, copy_vf_info;
+	unsigned int left_padding;
+	int err;
+	struct ia_css_binary_descr copy_descr;
+
+	/* next_binary can be NULL */
+	assert(pipe);
+	assert(copy_binary);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "load_copy_binary() enter:\n");
+
+	if (next_binary) {
+		copy_out_info = next_binary->in_frame_info;
+		left_padding = next_binary->left_padding;
+	} else {
+		copy_out_info = pipe->output_info[0];
+		copy_vf_info = pipe->vf_output_info[0];
+		ia_css_frame_info_set_format(&copy_vf_info, IA_CSS_FRAME_FORMAT_YUV_LINE);
+		left_padding = 0;
+	}
+
+	ia_css_pipe_get_copy_binarydesc(pipe, &copy_descr,
+					&copy_in_info, &copy_out_info,
+					(next_binary) ? NULL : NULL/*TODO: &copy_vf_info*/);
+	err = ia_css_binary_find(&copy_descr, copy_binary);
+	if (err)
+		return err;
+	copy_binary->left_padding = left_padding;
+	return 0;
+}
+
+static int
+alloc_continuous_frames(struct ia_css_pipe *pipe, bool init_time)
+{
+	int err = 0;
+	struct ia_css_frame_info ref_info;
+	enum ia_css_pipe_id pipe_id;
+	bool continuous;
+	unsigned int i, idx;
+	unsigned int num_frames;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, init_time = %d", pipe, init_time);
+
+	if ((!pipe) || (!pipe->stream)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipe_id = pipe->mode;
+	continuous = pipe->stream->config.continuous;
+
+	if (continuous) {
+		if (init_time) {
+			num_frames = pipe->stream->config.init_num_cont_raw_buf;
+			pipe->stream->continuous_pipe = pipe;
+		} else {
+			num_frames = pipe->stream->config.target_num_cont_raw_buf;
+		}
+	} else {
+		num_frames = NUM_ONLINE_INIT_CONTINUOUS_FRAMES;
+	}
+
+	if (pipe_id == IA_CSS_PIPE_ID_PREVIEW) {
+		ref_info = pipe->pipe_settings.preview.preview_binary.in_frame_info;
+	} else if (pipe_id == IA_CSS_PIPE_ID_VIDEO) {
+		ref_info = pipe->pipe_settings.video.video_binary.in_frame_info;
+	} else {
+		/* should not happen */
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+#if defined(ISP2401)
+	/* For CSI2+, the continuous frame will hold the full input frame */
+	ref_info.res.width = pipe->stream->config.input_config.input_res.width;
+	ref_info.res.height = pipe->stream->config.input_config.input_res.height;
+
+	/* Ensure padded width is aligned for 2401 */
+	ref_info.padded_width = CEIL_MUL(ref_info.res.width, 2 * ISP_VEC_NELEMS);
+#endif
+
+	if (pipe->stream->config.pack_raw_pixels) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "alloc_continuous_frames() IA_CSS_FRAME_FORMAT_RAW_PACKED\n");
+		ref_info.format = IA_CSS_FRAME_FORMAT_RAW_PACKED;
+	} else
+	{
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "alloc_continuous_frames() IA_CSS_FRAME_FORMAT_RAW\n");
+		ref_info.format = IA_CSS_FRAME_FORMAT_RAW;
+	}
+
+	/* Write format back to binary */
+	if (pipe_id == IA_CSS_PIPE_ID_PREVIEW) {
+		pipe->pipe_settings.preview.preview_binary.in_frame_info.format =
+		    ref_info.format;
+	} else if (pipe_id == IA_CSS_PIPE_ID_VIDEO) {
+		pipe->pipe_settings.video.video_binary.in_frame_info.format = ref_info.format;
+	} else {
+		/* should not happen */
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (init_time)
+		idx = 0;
+	else
+		idx = pipe->stream->config.init_num_cont_raw_buf;
+
+	for (i = idx; i < NUM_CONTINUOUS_FRAMES; i++) {
+		/* free previous frame */
+		if (pipe->continuous_frames[i]) {
+			ia_css_frame_free(pipe->continuous_frames[i]);
+			pipe->continuous_frames[i] = NULL;
+		}
+		/* free previous metadata buffer */
+		ia_css_metadata_free(pipe->cont_md_buffers[i]);
+		pipe->cont_md_buffers[i] = NULL;
+
+		/* check if new frame needed */
+		if (i < num_frames) {
+			/* allocate new frame */
+			err = ia_css_frame_allocate_from_info(
+				  &pipe->continuous_frames[i],
+				  &ref_info);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			/* allocate metadata buffer */
+			pipe->cont_md_buffers[i] = ia_css_metadata_allocate(
+						       &pipe->stream->info.metadata_info);
+		}
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+int
+ia_css_alloc_continuous_frame_remain(struct ia_css_stream *stream)
+{
+	if (!stream)
+		return -EINVAL;
+	return alloc_continuous_frames(stream->continuous_pipe, false);
+}
+
+static int
+load_preview_binaries(struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info prev_in_info,
+		prev_bds_out_info,
+		prev_out_info,
+		prev_vf_info;
+	struct ia_css_binary_descr preview_descr;
+	bool online;
+	int err = 0;
+	bool need_vf_pp = false;
+	bool need_isp_copy_binary = false;
+#ifdef ISP2401
+	bool sensor = false;
+#else
+	bool continuous;
+#endif
+	/* preview only have 1 output pin now */
+	struct ia_css_frame_info *pipe_out_info = &pipe->output_info[0];
+	struct ia_css_preview_settings *mycs  = &pipe->pipe_settings.preview;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->stream);
+	assert(pipe->mode == IA_CSS_PIPE_ID_PREVIEW);
+
+	online = pipe->stream->config.online;
+#ifdef ISP2401
+	sensor = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
+#else
+	continuous = pipe->stream->config.continuous;
+#endif
+
+	if (mycs->preview_binary.info)
+		return 0;
+
+	err = ia_css_util_check_input(&pipe->stream->config, false, false);
+	if (err)
+		return err;
+	err = ia_css_frame_check_info(pipe_out_info);
+	if (err)
+		return err;
+
+	/*
+	 * Note: the current selection of vf_pp binary and
+	 * parameterization of the preview binary contains a few pieces
+	 * of hardcoded knowledge. This needs to be cleaned up such that
+	 * the binary selection becomes more generic.
+	 * The vf_pp binary is needed if one or more of the following features
+	 * are required:
+	 * 1. YUV downscaling.
+	 * 2. Digital zoom.
+	 * 3. An output format that is not supported by the preview binary.
+	 *    In practice this means something other than yuv_line or nv12.
+	 * The decision if the vf_pp binary is needed for YUV downscaling is
+	 * made after the preview binary selection, since some preview binaries
+	 * can perform the requested YUV downscaling.
+	 */
+	need_vf_pp = pipe->config.enable_dz;
+	need_vf_pp |= pipe_out_info->format != IA_CSS_FRAME_FORMAT_YUV_LINE &&
+	!(pipe_out_info->format == IA_CSS_FRAME_FORMAT_NV12 ||
+	  pipe_out_info->format == IA_CSS_FRAME_FORMAT_NV12_16 ||
+	  pipe_out_info->format == IA_CSS_FRAME_FORMAT_NV12_TILEY);
+
+	/* Preview step 1 */
+	if (pipe->vf_yuv_ds_input_info.res.width)
+		prev_vf_info = pipe->vf_yuv_ds_input_info;
+	else
+		prev_vf_info = *pipe_out_info;
+	/*
+	 * If vf_pp is needed, then preview must output yuv_line.
+	 * The exception is when vf_pp is manually disabled, that is only
+	 * used in combination with a pipeline extension that requires
+	 * yuv_line as input.
+	 */
+	if (need_vf_pp)
+		ia_css_frame_info_set_format(&prev_vf_info,
+					     IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+	err = ia_css_pipe_get_preview_binarydesc(
+	    pipe,
+	    &preview_descr,
+	    &prev_in_info,
+	    &prev_bds_out_info,
+	    &prev_out_info,
+	    &prev_vf_info);
+	if (err)
+		return err;
+	err = ia_css_binary_find(&preview_descr, &mycs->preview_binary);
+	if (err)
+		return err;
+
+	/* The vf_pp binary is needed when (further) YUV downscaling is required */
+	need_vf_pp |= mycs->preview_binary.out_frame_info[0].res.width != pipe_out_info->res.width;
+	need_vf_pp |= mycs->preview_binary.out_frame_info[0].res.height != pipe_out_info->res.height;
+
+	/*
+	 * When vf_pp is needed, then the output format of the selected
+	 * preview binary must be yuv_line. If this is not the case,
+	 * then the preview binary selection is done again.
+	 */
+	if (need_vf_pp &&
+	    (mycs->preview_binary.out_frame_info[0].format != IA_CSS_FRAME_FORMAT_YUV_LINE)) {
+		/* Preview step 2 */
+		if (pipe->vf_yuv_ds_input_info.res.width)
+			prev_vf_info = pipe->vf_yuv_ds_input_info;
+		else
+			prev_vf_info = *pipe_out_info;
+
+		ia_css_frame_info_set_format(&prev_vf_info,
+					     IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+		err = ia_css_pipe_get_preview_binarydesc(
+		    pipe,
+		    &preview_descr,
+		    &prev_in_info,
+		    &prev_bds_out_info,
+		    &prev_out_info,
+		    &prev_vf_info);
+		if (err)
+			return err;
+		err = ia_css_binary_find(&preview_descr,
+					 &mycs->preview_binary);
+		if (err)
+			return err;
+	}
+
+	if (need_vf_pp) {
+		struct ia_css_binary_descr vf_pp_descr;
+
+		/* Viewfinder post-processing */
+		ia_css_pipe_get_vfpp_binarydesc(pipe, &vf_pp_descr,
+						&mycs->preview_binary.out_frame_info[0],
+						pipe_out_info);
+		err = ia_css_binary_find(&vf_pp_descr,
+					 &mycs->vf_pp_binary);
+		if (err)
+			return err;
+	}
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, only the Direct Sensor Mode
+	 * Offline Preview uses the ISP copy binary.
+	 */
+	need_isp_copy_binary = !online && sensor;
+#else
+	/*
+	 * About pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY:
+	 * This is typical the case with SkyCam (which has no input system) but it also applies to all cases
+	 * where the driver chooses for memory based input frames. In these cases, a copy binary (which typical
+	 * copies sensor data to DDR) does not have much use.
+	 */
+	if (!IS_ISP2401)
+		need_isp_copy_binary = !online && !continuous;
+	else
+		need_isp_copy_binary = !online && !continuous && !(pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY);
+#endif
+
+	/* Copy */
+	if (need_isp_copy_binary) {
+		err = load_copy_binary(pipe,
+				       &mycs->copy_binary,
+				       &mycs->preview_binary);
+		if (err)
+			return err;
+	}
+
+	if (pipe->shading_table) {
+		ia_css_shading_table_free(pipe->shading_table);
+		pipe->shading_table = NULL;
+	}
+
+	return 0;
+}
+
+static void
+ia_css_binary_unload(struct ia_css_binary *binary)
+{
+	ia_css_binary_destroy_isp_parameters(binary);
+}
+
+static int
+unload_preview_binaries(struct ia_css_pipe *pipe)
+{
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((!pipe) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	ia_css_binary_unload(&pipe->pipe_settings.preview.copy_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.preview.preview_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.preview.vf_pp_binary);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static const struct ia_css_fw_info *last_output_firmware(
+    const struct ia_css_fw_info *fw)
+{
+	const struct ia_css_fw_info *last_fw = NULL;
+	/* fw can be NULL */
+	IA_CSS_ENTER_LEAVE_PRIVATE("");
+
+	for (; fw; fw = fw->next) {
+		const struct ia_css_fw_info *info = fw;
+
+		if (info->info.isp.sp.enable.output)
+			last_fw = fw;
+	}
+	return last_fw;
+}
+
+static int add_firmwares(
+    struct ia_css_pipeline *me,
+    struct ia_css_binary *binary,
+    const struct ia_css_fw_info *fw,
+    const struct ia_css_fw_info *last_fw,
+    unsigned int binary_mode,
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *out_frame,
+    struct ia_css_frame *vf_frame,
+    struct ia_css_pipeline_stage **my_stage,
+    struct ia_css_pipeline_stage **vf_stage)
+{
+	int err = 0;
+	struct ia_css_pipeline_stage *extra_stage = NULL;
+	struct ia_css_pipeline_stage_desc stage_desc;
+
+	/* all args can be NULL ??? */
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "add_firmwares() enter:\n");
+
+	for (; fw; fw = fw->next) {
+		struct ia_css_frame *out[IA_CSS_BINARY_MAX_OUTPUT_PORTS] = {NULL};
+		struct ia_css_frame *in = NULL;
+		struct ia_css_frame *vf = NULL;
+
+		if ((fw == last_fw) && (fw->info.isp.sp.enable.out_frame  != 0))
+			out[0] = out_frame;
+
+		if (fw->info.isp.sp.enable.in_frame != 0)
+			in = in_frame;
+
+		if (fw->info.isp.sp.enable.out_frame != 0)
+			vf = vf_frame;
+
+		ia_css_pipe_get_firmwares_stage_desc(&stage_desc, binary,
+						     out, in, vf, fw, binary_mode);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   &extra_stage);
+		if (err)
+			return err;
+		if (fw->info.isp.sp.enable.output != 0)
+			in_frame = extra_stage->args.out_frame[0];
+		if (my_stage && !*my_stage && extra_stage)
+			*my_stage = extra_stage;
+		if (vf_stage && !*vf_stage && extra_stage &&
+		    fw->info.isp.sp.enable.vf_veceven)
+			*vf_stage = extra_stage;
+	}
+	return err;
+}
+
+static int add_vf_pp_stage(
+    struct ia_css_pipe *pipe,
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *out_frame,
+    struct ia_css_binary *vf_pp_binary,
+    struct ia_css_pipeline_stage **vf_pp_stage)
+{
+	struct ia_css_pipeline *me = NULL;
+	const struct ia_css_fw_info *last_fw = NULL;
+	int err = 0;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_pipeline_stage_desc stage_desc;
+
+	/* out_frame can be NULL ??? */
+
+	if (!pipe)
+		return -EINVAL;
+	if (!in_frame)
+		return -EINVAL;
+	if (!vf_pp_binary)
+		return -EINVAL;
+	if (!vf_pp_stage)
+		return -EINVAL;
+
+	ia_css_pipe_util_create_output_frames(out_frames);
+	me = &pipe->pipeline;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "add_vf_pp_stage() enter:\n");
+
+	*vf_pp_stage = NULL;
+
+	last_fw = last_output_firmware(pipe->vf_stage);
+	if (!pipe->extra_config.disable_vf_pp) {
+		if (last_fw) {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc, vf_pp_binary,
+							   out_frames, in_frame, NULL);
+		} else {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc, vf_pp_binary,
+							   out_frames, in_frame, NULL);
+		}
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc, vf_pp_stage);
+		if (err)
+			return err;
+		in_frame = (*vf_pp_stage)->args.out_frame[0];
+	}
+	err = add_firmwares(me, vf_pp_binary, pipe->vf_stage, last_fw,
+			    IA_CSS_BINARY_MODE_VF_PP,
+			    in_frame, out_frame, NULL,
+			    vf_pp_stage, NULL);
+	return err;
+}
+
+static int add_yuv_scaler_stage(
+    struct ia_css_pipe *pipe,
+    struct ia_css_pipeline *me,
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *out_frame,
+    struct ia_css_frame *internal_out_frame,
+    struct ia_css_binary *yuv_scaler_binary,
+    struct ia_css_pipeline_stage **pre_vf_pp_stage)
+{
+	const struct ia_css_fw_info *last_fw;
+	int err = 0;
+	struct ia_css_frame *vf_frame = NULL;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_pipeline_stage_desc stage_desc;
+
+	/* out_frame can be NULL ??? */
+	assert(in_frame);
+	assert(pipe);
+	assert(me);
+	assert(yuv_scaler_binary);
+	assert(pre_vf_pp_stage);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "add_yuv_scaler_stage() enter:\n");
+
+	*pre_vf_pp_stage = NULL;
+	ia_css_pipe_util_create_output_frames(out_frames);
+
+	last_fw = last_output_firmware(pipe->output_stage);
+
+	if (last_fw) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc,
+						   yuv_scaler_binary, out_frames, in_frame, vf_frame);
+	} else {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+		ia_css_pipe_util_set_output_frames(out_frames, 1, internal_out_frame);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc,
+						   yuv_scaler_binary, out_frames, in_frame, vf_frame);
+	}
+	err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+						   pre_vf_pp_stage);
+	if (err)
+		return err;
+	in_frame = (*pre_vf_pp_stage)->args.out_frame[0];
+
+	err = add_firmwares(me, yuv_scaler_binary, pipe->output_stage, last_fw,
+			    IA_CSS_BINARY_MODE_CAPTURE_PP,
+			    in_frame, out_frame, vf_frame,
+			    NULL, pre_vf_pp_stage);
+	/* If a firmware produce vf_pp output, we set that as vf_pp input */
+	(*pre_vf_pp_stage)->args.vf_downscale_log2 =
+	    yuv_scaler_binary->vf_downscale_log2;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "add_yuv_scaler_stage() leave:\n");
+	return err;
+}
+
+static int add_capture_pp_stage(
+    struct ia_css_pipe *pipe,
+    struct ia_css_pipeline *me,
+    struct ia_css_frame *in_frame,
+    struct ia_css_frame *out_frame,
+    struct ia_css_binary *capture_pp_binary,
+    struct ia_css_pipeline_stage **capture_pp_stage)
+{
+	const struct ia_css_fw_info *last_fw = NULL;
+	int err = 0;
+	struct ia_css_frame *vf_frame = NULL;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_pipeline_stage_desc stage_desc;
+
+	/* out_frame can be NULL ??? */
+	assert(in_frame);
+	assert(pipe);
+	assert(me);
+	assert(capture_pp_binary);
+	assert(capture_pp_stage);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "add_capture_pp_stage() enter:\n");
+
+	*capture_pp_stage = NULL;
+	ia_css_pipe_util_create_output_frames(out_frames);
+
+	last_fw = last_output_firmware(pipe->output_stage);
+	err = ia_css_frame_allocate_from_info(&vf_frame,
+					      &capture_pp_binary->vf_frame_info);
+	if (err)
+		return err;
+	if (last_fw)	{
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc,
+						   capture_pp_binary, out_frames, NULL, vf_frame);
+	} else {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc,
+						   capture_pp_binary, out_frames, NULL, vf_frame);
+	}
+	err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+						   capture_pp_stage);
+	if (err)
+		return err;
+	err = add_firmwares(me, capture_pp_binary, pipe->output_stage, last_fw,
+			    IA_CSS_BINARY_MODE_CAPTURE_PP,
+			    in_frame, out_frame, vf_frame,
+			    NULL, capture_pp_stage);
+	/* If a firmware produce vf_pp output, we set that as vf_pp input */
+	if (*capture_pp_stage) {
+		(*capture_pp_stage)->args.vf_downscale_log2 =
+		    capture_pp_binary->vf_downscale_log2;
+	}
+	return err;
+}
+
+static void sh_css_setup_queues(void)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_host_sp_queues_initialized;
+
+	sh_css_hmm_buffer_record_init();
+
+	sh_css_event_init_irq_mask();
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_host_sp_queues_initialized =
+	    fw->info.sp.host_sp_queues_initialized;
+
+	ia_css_bufq_init();
+
+	/* set "host_sp_queues_initialized" to "true" */
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(host_sp_queues_initialized),
+			     (uint32_t)(1));
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_setup_queues() leave:\n");
+}
+
+static int
+init_vf_frameinfo_defaults(struct ia_css_pipe *pipe,
+			   struct ia_css_frame *vf_frame, unsigned int idx)
+{
+	int err = 0;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(vf_frame);
+
+	sh_css_pipe_get_viewfinder_frame_info(pipe, &vf_frame->info, idx);
+	vf_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME + idx, thread_id, &queue_id);
+	vf_frame->dynamic_queue_id = queue_id;
+	vf_frame->buf_type = IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME + idx;
+
+	err = ia_css_frame_init_planes(vf_frame);
+	return err;
+}
+
+#ifdef ISP2401
+static unsigned int
+get_crop_lines_for_bayer_order(const struct ia_css_stream_config *config)
+{
+	assert(config);
+	if ((config->input_config.bayer_order == IA_CSS_BAYER_ORDER_BGGR) ||
+	    (config->input_config.bayer_order == IA_CSS_BAYER_ORDER_GBRG))
+		return 1;
+
+	return 0;
+}
+
+static unsigned int
+get_crop_columns_for_bayer_order(const struct ia_css_stream_config *config)
+{
+	assert(config);
+	if ((config->input_config.bayer_order == IA_CSS_BAYER_ORDER_RGGB) ||
+	    (config->input_config.bayer_order == IA_CSS_BAYER_ORDER_GBRG))
+		return 1;
+
+	return 0;
+}
+
+/*
+ * This function is to get the sum of all extra pixels in addition to the effective
+ * input, it includes dvs envelop and filter run-in
+ */
+static void get_pipe_extra_pixel(struct ia_css_pipe *pipe,
+				 unsigned int *extra_row, unsigned int *extra_column)
+{
+	enum ia_css_pipe_id pipe_id = pipe->mode;
+	unsigned int left_cropping = 0, top_cropping = 0;
+	unsigned int i;
+	struct ia_css_resolution dvs_env = pipe->config.dvs_envelope;
+
+	/*
+	 * The dvs envelope info may not be correctly sent down via pipe config
+	 * The check is made and the correct value is populated in the binary info
+	 * Use this value when computing crop, else excess lines may get trimmed
+	 */
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		if (pipe->pipe_settings.preview.preview_binary.info) {
+			left_cropping =
+			    pipe->pipe_settings.preview.preview_binary.info->sp.pipeline.left_cropping;
+			top_cropping =
+			    pipe->pipe_settings.preview.preview_binary.info->sp.pipeline.top_cropping;
+		}
+		dvs_env = pipe->pipe_settings.preview.preview_binary.dvs_envelope;
+		break;
+	case IA_CSS_PIPE_ID_VIDEO:
+		if (pipe->pipe_settings.video.video_binary.info) {
+			left_cropping =
+			    pipe->pipe_settings.video.video_binary.info->sp.pipeline.left_cropping;
+			top_cropping =
+			    pipe->pipe_settings.video.video_binary.info->sp.pipeline.top_cropping;
+		}
+		dvs_env = pipe->pipe_settings.video.video_binary.dvs_envelope;
+		break;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		for (i = 0; i < pipe->pipe_settings.capture.num_primary_stage; i++) {
+			if (pipe->pipe_settings.capture.primary_binary[i].info) {
+				left_cropping +=
+				    pipe->pipe_settings.capture.primary_binary[i].info->sp.pipeline.left_cropping;
+				top_cropping +=
+				    pipe->pipe_settings.capture.primary_binary[i].info->sp.pipeline.top_cropping;
+			}
+			dvs_env.width +=
+			    pipe->pipe_settings.capture.primary_binary[i].dvs_envelope.width;
+			dvs_env.height +=
+			    pipe->pipe_settings.capture.primary_binary[i].dvs_envelope.height;
+		}
+		break;
+	default:
+		break;
+	}
+
+	*extra_row = top_cropping + dvs_env.height;
+	*extra_column = left_cropping + dvs_env.width;
+}
+
+void
+ia_css_get_crop_offsets(
+    struct ia_css_pipe *pipe,
+    struct ia_css_frame_info *in_frame)
+{
+	unsigned int row = 0;
+	unsigned int column = 0;
+	struct ia_css_resolution *input_res;
+	struct ia_css_resolution *effective_res;
+	unsigned int extra_row = 0, extra_col = 0;
+	unsigned int min_reqd_height, min_reqd_width;
+
+	assert(pipe);
+	assert(pipe->stream);
+	assert(in_frame);
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p effective_wd = %u effective_ht = %u",
+			     pipe, pipe->config.input_effective_res.width,
+			     pipe->config.input_effective_res.height);
+
+	input_res = &pipe->stream->config.input_config.input_res;
+#ifndef ISP2401
+	effective_res = &pipe->stream->config.input_config.effective_res;
+#else
+	effective_res = &pipe->config.input_effective_res;
+#endif
+
+	get_pipe_extra_pixel(pipe, &extra_row, &extra_col);
+
+	in_frame->raw_bayer_order = pipe->stream->config.input_config.bayer_order;
+
+	min_reqd_height = effective_res->height + extra_row;
+	min_reqd_width = effective_res->width + extra_col;
+
+	if (input_res->height > min_reqd_height) {
+		row = (input_res->height - min_reqd_height) / 2;
+		row &= ~0x1;
+	}
+	if (input_res->width > min_reqd_width) {
+		column = (input_res->width - min_reqd_width) / 2;
+		column &= ~0x1;
+	}
+
+	/*
+	 * TODO:
+	 * 1. Require the special support for RAW10 packed mode.
+	 * 2. Require the special support for the online use cases.
+	 */
+
+	/*
+	 * ISP expects GRBG bayer order, we skip one line and/or one row
+	 * to correct in case the input bayer order is different.
+	 */
+	column += get_crop_columns_for_bayer_order(&pipe->stream->config);
+	row += get_crop_lines_for_bayer_order(&pipe->stream->config);
+
+	in_frame->crop_info.start_column = column;
+	in_frame->crop_info.start_line = row;
+
+	IA_CSS_LEAVE_PRIVATE("void start_col: %u start_row: %u", column, row);
+
+	return;
+}
+#endif
+
+static int
+init_in_frameinfo_memory_defaults(struct ia_css_pipe *pipe,
+				  struct ia_css_frame *frame, enum ia_css_frame_format format)
+{
+	struct ia_css_frame *in_frame;
+	int err = 0;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(frame);
+	in_frame = frame;
+
+	in_frame->info.format = format;
+
+#ifdef ISP2401
+	if (format == IA_CSS_FRAME_FORMAT_RAW)
+		in_frame->info.format = (pipe->stream->config.pack_raw_pixels) ?
+		IA_CSS_FRAME_FORMAT_RAW_PACKED : IA_CSS_FRAME_FORMAT_RAW;
+#endif
+
+	in_frame->info.res.width = pipe->stream->config.input_config.input_res.width;
+	in_frame->info.res.height = pipe->stream->config.input_config.input_res.height;
+	in_frame->info.raw_bit_depth =
+	ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	ia_css_frame_info_set_width(&in_frame->info, pipe->stream->config.input_config.input_res.width, 0);
+	in_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_INPUT_FRAME, thread_id, &queue_id);
+	in_frame->dynamic_queue_id = queue_id;
+	in_frame->buf_type = IA_CSS_BUFFER_TYPE_INPUT_FRAME;
+#ifdef ISP2401
+	ia_css_get_crop_offsets(pipe, &in_frame->info);
+#endif
+	err = ia_css_frame_init_planes(in_frame);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "init_in_frameinfo_memory_defaults() bayer_order = %d:\n", in_frame->info.raw_bayer_order);
+
+	return err;
+}
+
+static int
+init_out_frameinfo_defaults(struct ia_css_pipe *pipe,
+			    struct ia_css_frame *out_frame, unsigned int idx)
+{
+	int err = 0;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(out_frame);
+
+	sh_css_pipe_get_output_frame_info(pipe, &out_frame->info, idx);
+	out_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_OUTPUT_FRAME + idx, thread_id, &queue_id);
+	out_frame->dynamic_queue_id = queue_id;
+	out_frame->buf_type = IA_CSS_BUFFER_TYPE_OUTPUT_FRAME + idx;
+	err = ia_css_frame_init_planes(out_frame);
+
+	return err;
+}
+
+/* Create stages for video pipe */
+static int create_host_video_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline_stage_desc stage_desc;
+	struct ia_css_binary *copy_binary, *video_binary,
+		       *yuv_scaler_binary, *vf_pp_binary;
+	struct ia_css_pipeline_stage *copy_stage  = NULL;
+	struct ia_css_pipeline_stage *video_stage = NULL;
+	struct ia_css_pipeline_stage *yuv_scaler_stage  = NULL;
+	struct ia_css_pipeline_stage *vf_pp_stage = NULL;
+	struct ia_css_pipeline *me;
+	struct ia_css_frame *in_frame = NULL;
+	struct ia_css_frame *out_frame;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame *vf_frame = NULL;
+	int err = 0;
+	bool need_copy   = false;
+	bool need_vf_pp  = false;
+	bool need_yuv_pp = false;
+	bool need_in_frameinfo_memory = false;
+
+	unsigned int i, num_yuv_scaler;
+	bool *is_output_stage = NULL;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (!pipe->stream) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	ia_css_pipe_util_create_output_frames(out_frames);
+	out_frame = &pipe->out_frame_struct;
+
+	/* pipeline already created as part of create_host_pipeline_structure */
+	me = &pipe->pipeline;
+	ia_css_pipeline_clean(me);
+
+	me->dvs_frame_delay = pipe->dvs_frame_delay;
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, always enable 'in_frameinfo_memory'
+	 * except for the following: online or continuous
+	 */
+	need_in_frameinfo_memory = !(pipe->stream->config.online ||
+				     pipe->stream->config.continuous);
+#else
+	/* Construct in_frame info (only in case we have dynamic input */
+	need_in_frameinfo_memory = pipe->stream->config.mode ==
+				   IA_CSS_INPUT_MODE_MEMORY;
+#endif
+
+	/* Construct in_frame info (only in case we have dynamic input */
+	if (need_in_frameinfo_memory) {
+		in_frame = &pipe->in_frame_struct;
+		err = init_in_frameinfo_memory_defaults(pipe, in_frame,
+							IA_CSS_FRAME_FORMAT_RAW);
+		if (err)
+			goto ERR;
+	}
+
+	out_frame->data = 0;
+	err = init_out_frameinfo_defaults(pipe, out_frame, 0);
+	if (err)
+		goto ERR;
+
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		vf_frame = &pipe->vf_frame_struct;
+		vf_frame->data = 0;
+		err = init_vf_frameinfo_defaults(pipe, vf_frame, 0);
+		if (err)
+			goto ERR;
+	}
+
+	copy_binary  = &pipe->pipe_settings.video.copy_binary;
+	video_binary = &pipe->pipe_settings.video.video_binary;
+	vf_pp_binary = &pipe->pipe_settings.video.vf_pp_binary;
+
+	yuv_scaler_binary = pipe->pipe_settings.video.yuv_scaler_binary;
+	num_yuv_scaler  = pipe->pipe_settings.video.num_yuv_scaler;
+	is_output_stage = pipe->pipe_settings.video.is_output_stage;
+
+	need_copy   = (copy_binary && copy_binary->info);
+	need_vf_pp  = (vf_pp_binary && vf_pp_binary->info);
+	need_yuv_pp = (yuv_scaler_binary && yuv_scaler_binary->info);
+
+	if (need_copy) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, copy_binary,
+						   out_frames, NULL, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   &copy_stage);
+		if (err)
+			goto ERR;
+		in_frame = me->stages->args.out_frame[0];
+	} else if (pipe->stream->config.continuous) {
+#ifdef ISP2401
+		/*
+		 * When continuous is enabled, configure in_frame with the
+		 * last pipe, which is the copy pipe.
+		 */
+		in_frame = pipe->stream->last_pipe->continuous_frames[0];
+#else
+		in_frame = pipe->continuous_frames[0];
+#endif
+	}
+
+	ia_css_pipe_util_set_output_frames(out_frames, 0,
+					   need_yuv_pp ? NULL : out_frame);
+
+	/*
+	 * when the video binary supports a second output pin,
+	 * it can directly produce the vf_frame.
+	 */
+	if (need_vf_pp) {
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, video_binary,
+						   out_frames, in_frame, NULL);
+	} else {
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, video_binary,
+						   out_frames, in_frame, vf_frame);
+	}
+	err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+						   &video_stage);
+	if (err)
+		goto ERR;
+
+	/* If we use copy iso video, the input must be yuv iso raw */
+	if (video_stage) {
+		video_stage->args.copy_vf =
+		    video_binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_COPY;
+		video_stage->args.copy_output = video_stage->args.copy_vf;
+	}
+
+	/* when the video binary supports only 1 output pin, vf_pp is needed to
+	produce the vf_frame.*/
+	if (need_vf_pp && video_stage) {
+		in_frame = video_stage->args.out_vf_frame;
+		err = add_vf_pp_stage(pipe, in_frame, vf_frame, vf_pp_binary,
+				      &vf_pp_stage);
+		if (err)
+			goto ERR;
+	}
+	if (video_stage) {
+		int frm;
+
+		for (frm = 0; frm < NUM_VIDEO_TNR_FRAMES; frm++) {
+			video_stage->args.tnr_frames[frm] =
+			    pipe->pipe_settings.video.tnr_frames[frm];
+		}
+		for (frm = 0; frm < MAX_NUM_VIDEO_DELAY_FRAMES; frm++) {
+			video_stage->args.delay_frames[frm] =
+			    pipe->pipe_settings.video.delay_frames[frm];
+		}
+	}
+
+	/* Append Extension on Video out, if enabled */
+	if (!need_vf_pp && video_stage && pipe->config.acc_extension &&
+	    (pipe->config.acc_extension->info.isp.type == IA_CSS_ACC_OUTPUT)) {
+		struct ia_css_frame *out = NULL;
+		struct ia_css_frame *in = NULL;
+
+		if ((pipe->config.acc_extension->info.isp.sp.enable.output) &&
+		    (pipe->config.acc_extension->info.isp.sp.enable.in_frame) &&
+		    (pipe->config.acc_extension->info.isp.sp.enable.out_frame)) {
+			/* In/Out Frame mapping to support output frame extension.*/
+			out = video_stage->args.out_frame[0];
+			err = ia_css_frame_allocate_from_info(&in, &pipe->output_info[0]);
+			if (err)
+				goto ERR;
+			video_stage->args.out_frame[0] = in;
+		}
+
+		err = add_firmwares(me, video_binary, pipe->output_stage,
+				    last_output_firmware(pipe->output_stage),
+				    IA_CSS_BINARY_MODE_VIDEO,
+				    in, out, NULL, &video_stage, NULL);
+		if (err)
+			goto ERR;
+	}
+
+	if (need_yuv_pp && video_stage) {
+		struct ia_css_frame *tmp_in_frame = video_stage->args.out_frame[0];
+		struct ia_css_frame *tmp_out_frame = NULL;
+
+		for (i = 0; i < num_yuv_scaler; i++) {
+			tmp_out_frame = is_output_stage[i] ? out_frame : NULL;
+
+			err = add_yuv_scaler_stage(pipe, me, tmp_in_frame,
+						   tmp_out_frame, NULL,
+						   &yuv_scaler_binary[i],
+						   &yuv_scaler_stage);
+
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			/* we use output port 1 as internal output port */
+			if (yuv_scaler_stage)
+				tmp_in_frame = yuv_scaler_stage->args.out_frame[1];
+		}
+	}
+
+	pipe->pipeline.acquire_isp_each_stage = false;
+	ia_css_pipeline_finalize_stages(&pipe->pipeline,
+					pipe->stream->config.continuous);
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+create_host_acc_pipeline(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	const struct ia_css_fw_info *fw;
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (!pipe->stream)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipe->pipeline.num_execs = pipe->config.acc_num_execs;
+	/* Reset pipe_qos_config to default disable all QOS extension stages */
+	if (pipe->config.acc_extension)
+		pipe->pipeline.pipe_qos_config = 0;
+
+	for (fw = pipe->vf_stage; fw; fw = fw->next) {
+		err = sh_css_pipeline_add_acc_stage(&pipe->pipeline, fw);
+		if (err)
+			goto ERR;
+	}
+
+	for (i = 0; i < pipe->config.num_acc_stages; i++) {
+		struct ia_css_fw_info *fw = pipe->config.acc_stages[i];
+
+		err = sh_css_pipeline_add_acc_stage(&pipe->pipeline, fw);
+		if (err)
+			goto ERR;
+	}
+
+	ia_css_pipeline_finalize_stages(&pipe->pipeline, pipe->stream->config.continuous);
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/* Create stages for preview */
+static int
+create_host_preview_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline_stage *copy_stage = NULL;
+	struct ia_css_pipeline_stage *preview_stage = NULL;
+	struct ia_css_pipeline_stage *vf_pp_stage = NULL;
+	struct ia_css_pipeline_stage_desc stage_desc;
+	struct ia_css_pipeline *me = NULL;
+	struct ia_css_binary *copy_binary, *preview_binary, *vf_pp_binary = NULL;
+	struct ia_css_frame *in_frame = NULL;
+	int err = 0;
+	struct ia_css_frame *out_frame;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	bool need_in_frameinfo_memory = false;
+#ifdef ISP2401
+	bool sensor = false;
+	bool buffered_sensor = false;
+	bool online = false;
+	bool continuous = false;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (!pipe->stream) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	ia_css_pipe_util_create_output_frames(out_frames);
+	/* pipeline already created as part of create_host_pipeline_structure */
+	me = &pipe->pipeline;
+	ia_css_pipeline_clean(me);
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, always enable 'in_frameinfo_memory'
+	 * except for the following:
+	 * - Direct Sensor Mode Online Preview
+	 * - Buffered Sensor Mode Online Preview
+	 * - Direct Sensor Mode Continuous Preview
+	 * - Buffered Sensor Mode Continuous Preview
+	 */
+	sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR);
+	buffered_sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
+	online = pipe->stream->config.online;
+	continuous = pipe->stream->config.continuous;
+	need_in_frameinfo_memory =
+	!((sensor && (online || continuous)) || (buffered_sensor && (online || continuous)));
+#else
+	/* Construct in_frame info (only in case we have dynamic input */
+	need_in_frameinfo_memory = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+#endif
+	if (need_in_frameinfo_memory) {
+		err = init_in_frameinfo_memory_defaults(pipe, &me->in_frame,
+							IA_CSS_FRAME_FORMAT_RAW);
+		if (err)
+			goto ERR;
+
+		in_frame = &me->in_frame;
+	} else {
+		in_frame = NULL;
+	}
+
+	err = init_out_frameinfo_defaults(pipe, &me->out_frame[0], 0);
+	if (err)
+		goto ERR;
+	out_frame = &me->out_frame[0];
+
+	copy_binary    = &pipe->pipe_settings.preview.copy_binary;
+	preview_binary = &pipe->pipe_settings.preview.preview_binary;
+	if (pipe->pipe_settings.preview.vf_pp_binary.info)
+		vf_pp_binary = &pipe->pipe_settings.preview.vf_pp_binary;
+
+	if (pipe->pipe_settings.preview.copy_binary.info) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, copy_binary,
+						   out_frames, NULL, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   &copy_stage);
+		if (err)
+			goto ERR;
+		in_frame = me->stages->args.out_frame[0];
+	} else if (pipe->stream->config.continuous) {
+#ifdef ISP2401
+		/*
+		 * When continuous is enabled, configure in_frame with the
+		 * last pipe, which is the copy pipe.
+		 */
+		if (continuous || !online)
+			in_frame = pipe->stream->last_pipe->continuous_frames[0];
+
+#else
+		in_frame = pipe->continuous_frames[0];
+#endif
+	}
+
+	if (vf_pp_binary) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, preview_binary,
+						   out_frames, in_frame, NULL);
+	} else {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, preview_binary,
+						   out_frames, in_frame, NULL);
+	}
+	err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+						   &preview_stage);
+	if (err)
+		goto ERR;
+	/* If we use copy iso preview, the input must be yuv iso raw */
+	preview_stage->args.copy_vf =
+	    preview_binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_COPY;
+	preview_stage->args.copy_output = !preview_stage->args.copy_vf;
+	if (preview_stage->args.copy_vf && !preview_stage->args.out_vf_frame) {
+		/* in case of copy, use the vf frame as output frame */
+		preview_stage->args.out_vf_frame =
+		    preview_stage->args.out_frame[0];
+	}
+	if (vf_pp_binary) {
+		if (preview_binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_COPY)
+			in_frame = preview_stage->args.out_vf_frame;
+		else
+			in_frame = preview_stage->args.out_frame[0];
+		err = add_vf_pp_stage(pipe, in_frame, out_frame, vf_pp_binary,
+				      &vf_pp_stage);
+		if (err)
+			goto ERR;
+	}
+
+	pipe->pipeline.acquire_isp_each_stage = false;
+	ia_css_pipeline_finalize_stages(&pipe->pipeline, pipe->stream->config.continuous);
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static void send_raw_frames(struct ia_css_pipe *pipe)
+{
+	if (pipe->stream->config.continuous) {
+		unsigned int i;
+
+		sh_css_update_host2sp_cont_num_raw_frames
+		(pipe->stream->config.init_num_cont_raw_buf, true);
+		sh_css_update_host2sp_cont_num_raw_frames
+		(pipe->stream->config.target_num_cont_raw_buf, false);
+
+		/* Hand-over all the SP-internal buffers */
+		for (i = 0; i < pipe->stream->config.init_num_cont_raw_buf; i++) {
+			sh_css_update_host2sp_offline_frame(i,
+							    pipe->continuous_frames[i], pipe->cont_md_buffers[i]);
+		}
+	}
+
+	return;
+}
+
+static int
+preview_start(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	struct ia_css_pipe *copy_pipe, *capture_pipe;
+	struct ia_css_pipe *acc_pipe;
+	enum sh_css_pipe_config_override copy_ovrd;
+	enum ia_css_input_mode preview_pipe_input_mode;
+	unsigned int thread_id;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (!pipe->stream) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	preview_pipe_input_mode = pipe->stream->config.mode;
+
+	copy_pipe    = pipe->pipe_settings.preview.copy_pipe;
+	capture_pipe = pipe->pipe_settings.preview.capture_pipe;
+	acc_pipe     = pipe->pipe_settings.preview.acc_pipe;
+
+	sh_css_metrics_start_frame();
+
+	/* multi stream video needs mipi buffers */
+	err = send_mipi_frames(pipe);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	send_raw_frames(pipe);
+
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	copy_ovrd = 1 << thread_id;
+
+	if (pipe->stream->cont_capt) {
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(capture_pipe),
+						 &thread_id);
+		copy_ovrd |= 1 << thread_id;
+	}
+
+	/* Construct and load the copy pipe */
+	if (pipe->stream->config.continuous) {
+		sh_css_sp_init_pipeline(&copy_pipe->pipeline,
+					IA_CSS_PIPE_ID_COPY,
+					(uint8_t)ia_css_pipe_get_pipe_num(copy_pipe),
+					false,
+					pipe->stream->config.pixels_per_clock == 2, false,
+					false, pipe->required_bds_factor,
+					copy_ovrd,
+					pipe->stream->config.mode,
+					&pipe->stream->config.metadata_config,
+					&pipe->stream->info.metadata_info,
+					pipe->stream->config.source.port.port);
+
+		/*
+		 * make the preview pipe start with mem mode input, copy handles
+		 * the actual mode
+		 */
+		preview_pipe_input_mode = IA_CSS_INPUT_MODE_MEMORY;
+	}
+
+	/* Construct and load the capture pipe */
+	if (pipe->stream->cont_capt) {
+		sh_css_sp_init_pipeline(&capture_pipe->pipeline,
+					IA_CSS_PIPE_ID_CAPTURE,
+					(uint8_t)ia_css_pipe_get_pipe_num(capture_pipe),
+					capture_pipe->config.default_capture_config.enable_xnr != 0,
+					capture_pipe->stream->config.pixels_per_clock == 2,
+					true, /* continuous */
+					false, /* offline */
+					capture_pipe->required_bds_factor,
+					0,
+					IA_CSS_INPUT_MODE_MEMORY,
+					&pipe->stream->config.metadata_config,
+					&pipe->stream->info.metadata_info,
+					(enum mipi_port_id)0);
+	}
+
+	if (acc_pipe) {
+		sh_css_sp_init_pipeline(&acc_pipe->pipeline,
+					IA_CSS_PIPE_ID_ACC,
+					(uint8_t)ia_css_pipe_get_pipe_num(acc_pipe),
+					false,
+					pipe->stream->config.pixels_per_clock == 2,
+					false, /* continuous */
+					false, /* offline */
+					pipe->required_bds_factor,
+					0,
+					IA_CSS_INPUT_MODE_MEMORY,
+					NULL,
+					NULL,
+					(enum mipi_port_id)0);
+	}
+
+	start_pipe(pipe, copy_ovrd, preview_pipe_input_mode);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+int
+ia_css_pipe_enqueue_buffer(struct ia_css_pipe *pipe,
+			   const struct ia_css_buffer *buffer)
+{
+	int return_err = 0;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+	struct ia_css_pipeline *pipeline;
+	struct ia_css_pipeline_stage *stage;
+	struct ia_css_rmgr_vbuf_handle p_vbuf;
+	struct ia_css_rmgr_vbuf_handle *h_vbuf;
+	struct sh_css_hmm_buffer ddr_buffer;
+	enum ia_css_buffer_type buf_type;
+	enum ia_css_pipe_id pipe_id;
+	bool ret_err;
+
+	IA_CSS_ENTER("pipe=%p, buffer=%p", pipe, buffer);
+
+	if ((!pipe) || (!buffer)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	buf_type = buffer->type;
+
+	pipe_id = pipe->mode;
+
+	IA_CSS_LOG("pipe_id=%d, buf_type=%d", pipe_id, buf_type);
+
+	assert(pipe_id < IA_CSS_PIPE_ID_NUM);
+	assert(buf_type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE);
+	if (buf_type == IA_CSS_BUFFER_TYPE_INVALID ||
+	    buf_type >= IA_CSS_NUM_DYNAMIC_BUFFER_TYPE ||
+	    pipe_id >= IA_CSS_PIPE_ID_NUM) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	ret_err = ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	ret_err = ia_css_query_internal_queue_id(buf_type, thread_id, &queue_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if ((queue_id <= SH_CSS_INVALID_QUEUE_ID) || (queue_id >= SH_CSS_MAX_NUM_QUEUES)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LOG("SP is not running!");
+		IA_CSS_LEAVE_ERR(-EBUSY);
+		/* SP is not running. The queues are not valid */
+		return -EBUSY;
+	}
+
+	pipeline = &pipe->pipeline;
+
+	assert(pipeline ||
+	       pipe_id == IA_CSS_PIPE_ID_COPY ||
+	       pipe_id == IA_CSS_PIPE_ID_ACC);
+
+	assert(sizeof(NULL) <= sizeof(ddr_buffer.kernel_ptr));
+	ddr_buffer.kernel_ptr = HOST_ADDRESS(NULL);
+	ddr_buffer.cookie_ptr = buffer->driver_cookie;
+	ddr_buffer.timing_data = buffer->timing_data;
+
+	if (buf_type == IA_CSS_BUFFER_TYPE_3A_STATISTICS) {
+		if (!buffer->data.stats_3a) {
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+		ddr_buffer.kernel_ptr = HOST_ADDRESS(buffer->data.stats_3a);
+		ddr_buffer.payload.s3a = *buffer->data.stats_3a;
+	} else if (buf_type == IA_CSS_BUFFER_TYPE_DIS_STATISTICS) {
+		if (!buffer->data.stats_dvs) {
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+		ddr_buffer.kernel_ptr = HOST_ADDRESS(buffer->data.stats_dvs);
+		ddr_buffer.payload.dis = *buffer->data.stats_dvs;
+	} else if (buf_type == IA_CSS_BUFFER_TYPE_METADATA) {
+		if (!buffer->data.metadata) {
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+		ddr_buffer.kernel_ptr = HOST_ADDRESS(buffer->data.metadata);
+		ddr_buffer.payload.metadata = *buffer->data.metadata;
+	} else if (buf_type == IA_CSS_BUFFER_TYPE_INPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME) {
+		if (!buffer->data.frame) {
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+		ddr_buffer.kernel_ptr = HOST_ADDRESS(buffer->data.frame);
+		ddr_buffer.payload.frame.frame_data = buffer->data.frame->data;
+		ddr_buffer.payload.frame.flashed = 0;
+
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_pipe_enqueue_buffer() buf_type=%d, data(DDR address)=0x%x\n",
+				    buf_type, buffer->data.frame->data);
+
+	}
+
+	/* start of test for using rmgr for acq/rel memory */
+	p_vbuf.vptr = 0;
+	p_vbuf.count = 0;
+	p_vbuf.size = sizeof(struct sh_css_hmm_buffer);
+	h_vbuf = &p_vbuf;
+	/* TODO: change next to correct pool for optimization */
+	ia_css_rmgr_acq_vbuf(hmm_buffer_pool, &h_vbuf);
+
+	if ((!h_vbuf) || (h_vbuf->vptr == 0x0)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	hmm_store(h_vbuf->vptr,
+		  (void *)(&ddr_buffer),
+		  sizeof(struct sh_css_hmm_buffer));
+	if (buf_type == IA_CSS_BUFFER_TYPE_3A_STATISTICS ||
+	    buf_type == IA_CSS_BUFFER_TYPE_DIS_STATISTICS ||
+	    buf_type == IA_CSS_BUFFER_TYPE_LACE_STATISTICS) {
+		if (!pipeline) {
+			ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &h_vbuf);
+			IA_CSS_LOG("pipeline is empty!");
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+
+		for (stage = pipeline->stages; stage; stage = stage->next) {
+			/*
+			 * The SP will read the params after it got
+			 * empty 3a and dis
+			 */
+			if (STATS_ENABLED(stage)) {
+				/* there is a stage that needs it */
+				return_err = ia_css_bufq_enqueue_buffer(thread_id,
+									queue_id,
+									(uint32_t)h_vbuf->vptr);
+			}
+		}
+	} else if (buf_type == IA_CSS_BUFFER_TYPE_INPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME ||
+		   buf_type == IA_CSS_BUFFER_TYPE_METADATA) {
+		return_err = ia_css_bufq_enqueue_buffer(thread_id,
+							queue_id,
+							(uint32_t)h_vbuf->vptr);
+		if (!return_err &&
+		    buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME) {
+			IA_CSS_LOG("pfp: enqueued OF %d to q %d thread %d",
+				   ddr_buffer.payload.frame.frame_data,
+				   queue_id, thread_id);
+		}
+	}
+
+	if (!return_err) {
+		if (sh_css_hmm_buffer_record_acquire(
+			h_vbuf, buf_type,
+			HOST_ADDRESS(ddr_buffer.kernel_ptr))) {
+			IA_CSS_LOG("send vbuf=%p", h_vbuf);
+		} else {
+			return_err = -EINVAL;
+			IA_CSS_ERROR("hmm_buffer_record[]: no available slots\n");
+		}
+	}
+
+	/*
+	 * Tell the SP which queues are not empty,
+	 * by sending the software event.
+	 */
+	if (!return_err) {
+		if (!sh_css_sp_is_running()) {
+			/* SP is not running. The queues are not valid */
+			IA_CSS_LOG("SP is not running!");
+			IA_CSS_LEAVE_ERR(-EBUSY);
+			return -EBUSY;
+		}
+		return_err = ia_css_bufq_enqueue_psys_event(
+				 IA_CSS_PSYS_SW_EVENT_BUFFER_ENQUEUED,
+				 (uint8_t)thread_id,
+				 queue_id,
+				 0);
+	} else {
+		ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &h_vbuf);
+		IA_CSS_ERROR("buffer not enqueued");
+	}
+
+	IA_CSS_LEAVE("return value = %d", return_err);
+
+	return return_err;
+}
+
+/*
+ * TODO: Free up the hmm memory space.
+ */
+int
+ia_css_pipe_dequeue_buffer(struct ia_css_pipe *pipe,
+			   struct ia_css_buffer *buffer)
+{
+	int return_err;
+	enum sh_css_queue_id queue_id;
+	ia_css_ptr ddr_buffer_addr = (ia_css_ptr)0;
+	struct sh_css_hmm_buffer ddr_buffer;
+	enum ia_css_buffer_type buf_type;
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	hrt_address kernel_ptr = 0;
+	bool ret_err;
+
+	IA_CSS_ENTER("pipe=%p, buffer=%p", pipe, buffer);
+
+	if ((!pipe) || (!buffer)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipe_id = pipe->mode;
+
+	buf_type = buffer->type;
+
+	IA_CSS_LOG("pipe_id=%d, buf_type=%d", pipe_id, buf_type);
+
+	ddr_buffer.kernel_ptr = 0;
+
+	ret_err = ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	ret_err = ia_css_query_internal_queue_id(buf_type, thread_id, &queue_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if ((queue_id <= SH_CSS_INVALID_QUEUE_ID) || (queue_id >= SH_CSS_MAX_NUM_QUEUES)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LOG("SP is not running!");
+		IA_CSS_LEAVE_ERR(-EBUSY);
+		/* SP is not running. The queues are not valid */
+		return -EBUSY;
+	}
+
+	return_err = ia_css_bufq_dequeue_buffer(queue_id,
+						(uint32_t *)&ddr_buffer_addr);
+
+	if (!return_err) {
+		struct ia_css_frame *frame;
+		struct sh_css_hmm_buffer_record *hmm_buffer_record = NULL;
+
+		IA_CSS_LOG("receive vbuf=%x", (int)ddr_buffer_addr);
+
+		/* Validate the ddr_buffer_addr and buf_type */
+		hmm_buffer_record = sh_css_hmm_buffer_record_validate(
+		    ddr_buffer_addr, buf_type);
+		if (hmm_buffer_record) {
+			/*
+			 * valid hmm_buffer_record found. Save the kernel_ptr
+			 * for validation after performing hmm_load.  The
+			 * vbuf handle and buffer_record can be released.
+			 */
+			kernel_ptr = hmm_buffer_record->kernel_ptr;
+			ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &hmm_buffer_record->h_vbuf);
+			sh_css_hmm_buffer_record_reset(hmm_buffer_record);
+		} else {
+			IA_CSS_ERROR("hmm_buffer_record not found (0x%x) buf_type(%d)",
+				     ddr_buffer_addr, buf_type);
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+
+		hmm_load(ddr_buffer_addr,
+			 &ddr_buffer,
+			 sizeof(struct sh_css_hmm_buffer));
+
+		/*
+		 * if the kernel_ptr is 0 or an invalid, return an error.
+		 * do not access the buffer via the kernal_ptr.
+		 */
+		if ((ddr_buffer.kernel_ptr == 0) ||
+		    (kernel_ptr != HOST_ADDRESS(ddr_buffer.kernel_ptr))) {
+			IA_CSS_ERROR("kernel_ptr invalid");
+			IA_CSS_ERROR("expected: (0x%llx)", (u64)kernel_ptr);
+			IA_CSS_ERROR("actual: (0x%llx)", (u64)HOST_ADDRESS(ddr_buffer.kernel_ptr));
+			IA_CSS_ERROR("buf_type: %d\n", buf_type);
+			IA_CSS_LEAVE_ERR(-EINVAL);
+			return -EINVAL;
+		}
+
+		if (ddr_buffer.kernel_ptr != 0) {
+			/*
+			 * buffer->exp_id : all instances to be removed later
+			 * once the driver change is completed. See patch #5758
+			 * for reference
+			 */
+			buffer->exp_id = 0;
+			buffer->driver_cookie = ddr_buffer.cookie_ptr;
+			buffer->timing_data = ddr_buffer.timing_data;
+
+			if (buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME ||
+			    buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
+				buffer->isys_eof_clock_tick.ticks = ddr_buffer.isys_eof_clock_tick;
+			}
+
+			switch (buf_type) {
+			case IA_CSS_BUFFER_TYPE_INPUT_FRAME:
+			case IA_CSS_BUFFER_TYPE_OUTPUT_FRAME:
+			case IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+				if (pipe && pipe->stop_requested) {
+#if !defined(ISP2401)
+					/*
+					 * free mipi frames only for old input
+					 * system for 2401 it is done in
+					 * ia_css_stream_destroy call
+					 */
+					return_err = free_mipi_frames(pipe);
+					if (return_err) {
+						IA_CSS_LOG("free_mipi_frames() failed");
+						IA_CSS_LEAVE_ERR(return_err);
+						return return_err;
+					}
+#endif
+					pipe->stop_requested = false;
+				}
+				fallthrough;
+			case IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME:
+			case IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
+				frame = (struct ia_css_frame *)HOST_ADDRESS(ddr_buffer.kernel_ptr);
+				buffer->data.frame = frame;
+				buffer->exp_id = ddr_buffer.payload.frame.exp_id;
+				frame->exp_id = ddr_buffer.payload.frame.exp_id;
+				frame->isp_config_id = ddr_buffer.payload.frame.isp_parameters_id;
+				if (ddr_buffer.payload.frame.flashed == 1)
+					frame->flash_state =
+					    IA_CSS_FRAME_FLASH_STATE_PARTIAL;
+				if (ddr_buffer.payload.frame.flashed == 2)
+					frame->flash_state =
+					    IA_CSS_FRAME_FLASH_STATE_FULL;
+				frame->valid = pipe->num_invalid_frames == 0;
+				if (!frame->valid)
+					pipe->num_invalid_frames--;
+
+				if (frame->info.format == IA_CSS_FRAME_FORMAT_BINARY_8) {
+#ifdef ISP2401
+					frame->planes.binary.size = frame->data_bytes;
+#else
+					frame->planes.binary.size =
+					    sh_css_sp_get_binary_copy_size();
+#endif
+				}
+				if (buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME) {
+					IA_CSS_LOG("pfp: dequeued OF %d with config id %d thread %d",
+						   frame->data, frame->isp_config_id, thread_id);
+				}
+
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_pipe_dequeue_buffer() buf_type=%d, data(DDR address)=0x%x\n",
+						    buf_type, buffer->data.frame->data);
+
+				break;
+			case IA_CSS_BUFFER_TYPE_3A_STATISTICS:
+				buffer->data.stats_3a =
+				    (struct ia_css_isp_3a_statistics *)HOST_ADDRESS(ddr_buffer.kernel_ptr);
+				buffer->exp_id = ddr_buffer.payload.s3a.exp_id;
+				buffer->data.stats_3a->exp_id = ddr_buffer.payload.s3a.exp_id;
+				buffer->data.stats_3a->isp_config_id = ddr_buffer.payload.s3a.isp_config_id;
+				break;
+			case IA_CSS_BUFFER_TYPE_DIS_STATISTICS:
+				buffer->data.stats_dvs =
+				    (struct ia_css_isp_dvs_statistics *)
+				    HOST_ADDRESS(ddr_buffer.kernel_ptr);
+				buffer->exp_id = ddr_buffer.payload.dis.exp_id;
+				buffer->data.stats_dvs->exp_id = ddr_buffer.payload.dis.exp_id;
+				break;
+			case IA_CSS_BUFFER_TYPE_LACE_STATISTICS:
+				break;
+			case IA_CSS_BUFFER_TYPE_METADATA:
+				buffer->data.metadata =
+				    (struct ia_css_metadata *)HOST_ADDRESS(ddr_buffer.kernel_ptr);
+				buffer->exp_id = ddr_buffer.payload.metadata.exp_id;
+				buffer->data.metadata->exp_id = ddr_buffer.payload.metadata.exp_id;
+				break;
+			default:
+				return_err = -EINVAL;
+				break;
+			}
+		}
+	}
+
+	/*
+	 * Tell the SP which queues are not full,
+	 * by sending the software event.
+	 */
+	if (!return_err) {
+		if (!sh_css_sp_is_running()) {
+			IA_CSS_LOG("SP is not running!");
+			IA_CSS_LEAVE_ERR(-EBUSY);
+			/* SP is not running. The queues are not valid */
+			return -EBUSY;
+		}
+		ia_css_bufq_enqueue_psys_event(
+		    IA_CSS_PSYS_SW_EVENT_BUFFER_DEQUEUED,
+		    0,
+		    queue_id,
+		    0);
+	}
+	IA_CSS_LEAVE("buffer=%p", buffer);
+
+	return return_err;
+}
+
+/*
+ * Cannot Move this to event module as it is of ia_css_event_type which is declared in ia_css.h
+ * TODO: modify and move it if possible.
+ *
+ * !!!IMPORTANT!!! KEEP THE FOLLOWING IN SYNC:
+ * 1) "enum ia_css_event_type"					(ia_css_event_public.h)
+ * 2) "enum sh_css_sp_event_type"				(sh_css_internal.h)
+ * 3) "enum ia_css_event_type event_id_2_event_mask"		(event_handler.sp.c)
+ * 4) "enum ia_css_event_type convert_event_sp_to_host_domain"	(sh_css.c)
+ */
+static enum ia_css_event_type convert_event_sp_to_host_domain[] = {
+	IA_CSS_EVENT_TYPE_OUTPUT_FRAME_DONE,	/* Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_OUTPUT_FRAME_DONE,	/* Second output frame ready. */
+	IA_CSS_EVENT_TYPE_VF_OUTPUT_FRAME_DONE,	/* Viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_VF_OUTPUT_FRAME_DONE,	/* Second viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE,	/* Indication that 3A statistics are available. */
+	IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE,	/* Indication that DIS statistics are available. */
+	IA_CSS_EVENT_TYPE_PIPELINE_DONE,	/* Pipeline Done event, sent after last pipeline stage. */
+	IA_CSS_EVENT_TYPE_FRAME_TAGGED,		/* Frame tagged. */
+	IA_CSS_EVENT_TYPE_INPUT_FRAME_DONE,	/* Input frame ready. */
+	IA_CSS_EVENT_TYPE_METADATA_DONE,	/* Metadata ready. */
+	IA_CSS_EVENT_TYPE_LACE_STATISTICS_DONE,	/* Indication that LACE statistics are available. */
+	IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE,	/* Extension stage executed. */
+	IA_CSS_EVENT_TYPE_TIMER,		/* Timing measurement data. */
+	IA_CSS_EVENT_TYPE_PORT_EOF,		/* End Of Frame event, sent when in buffered sensor mode. */
+	IA_CSS_EVENT_TYPE_FW_WARNING,		/* Performance warning encountered by FW */
+	IA_CSS_EVENT_TYPE_FW_ASSERT,		/* Assertion hit by FW */
+	0,					/* error if sp passes  SH_CSS_SP_EVENT_NR_OF_TYPES as a valid event. */
+};
+
+int
+ia_css_dequeue_psys_event(struct ia_css_event *event)
+{
+	enum ia_css_pipe_id pipe_id = 0;
+	u8 payload[4] = {0, 0, 0, 0};
+	int ret_err;
+
+	/*
+	 * TODO:
+	 * a) use generic decoding function , same as the one used by sp.
+	 * b) group decode and dequeue into eventQueue module
+	 *
+	 * We skip the IA_CSS_ENTER logging call
+	 * to avoid flooding the logs when the host application
+	 * uses polling.
+	 */
+	if (!event)
+		return -EINVAL;
+
+	/* SP is not running. The queues are not valid */
+	if (!sh_css_sp_is_running())
+		return -EBUSY;
+
+	/* dequeue the event (if any) from the psys event queue */
+	ret_err = ia_css_bufq_dequeue_psys_event(payload);
+	if (ret_err)
+		return ret_err;
+
+	IA_CSS_LOG("event dequeued from psys event queue");
+
+	/* Tell the SP that we dequeued an event from the event queue. */
+	ia_css_bufq_enqueue_psys_event(
+	    IA_CSS_PSYS_SW_EVENT_EVENT_DEQUEUED, 0, 0, 0);
+
+	/*
+	 * Events are decoded into 4 bytes of payload, the first byte
+	 * contains the sp event type. This is converted to a host enum.
+	 * TODO: can this enum conversion be eliminated
+	 */
+	event->type = convert_event_sp_to_host_domain[payload[0]];
+	/* Some sane default values since not all events use all fields. */
+	event->pipe = NULL;
+	event->port = MIPI_PORT0_ID;
+	event->exp_id = 0;
+	event->fw_warning = IA_CSS_FW_WARNING_NONE;
+	event->fw_handle = 0;
+	event->timer_data = 0;
+	event->timer_code = 0;
+	event->timer_subcode = 0;
+
+	if (event->type == IA_CSS_EVENT_TYPE_TIMER) {
+		/*
+		 * timer event ??? get the 2nd event and decode the data
+		 * into the event struct
+		 */
+		u32 tmp_data;
+		/* 1st event: LSB 16-bit timer data and code */
+		event->timer_data = ((payload[1] & 0xFF) | ((payload[3] & 0xFF) << 8));
+		event->timer_code = payload[2];
+		payload[0] = payload[1] = payload[2] = payload[3] = 0;
+		ret_err = ia_css_bufq_dequeue_psys_event(payload);
+		if (ret_err) {
+			/* no 2nd event ??? an error */
+			/*
+			 * Putting IA_CSS_ERROR is resulting in failures in
+			 * Merrifield smoke testing
+			 */
+			IA_CSS_WARNING("Timer: Error de-queuing the 2nd TIMER event!!!\n");
+			return ret_err;
+		}
+		ia_css_bufq_enqueue_psys_event(
+		    IA_CSS_PSYS_SW_EVENT_EVENT_DEQUEUED, 0, 0, 0);
+		event->type = convert_event_sp_to_host_domain[payload[0]];
+		/* It's a timer */
+		if (event->type == IA_CSS_EVENT_TYPE_TIMER) {
+			/* 2nd event data: MSB 16-bit timer and subcode */
+			tmp_data = ((payload[1] & 0xFF) | ((payload[3] & 0xFF) << 8));
+			event->timer_data |= (tmp_data << 16);
+			event->timer_subcode = payload[2];
+		} else {
+			/*
+			 * It's a non timer event. So clear first half of the
+			 * timer event data.
+			 * If the second part of the TIMER event is not
+			 * received, we discard the first half of the timer
+			 * data and process the non timer event without
+			 * affecting the flow. So the non timer event falls
+			 * through the code.
+			 */
+			event->timer_data = 0;
+			event->timer_code = 0;
+			event->timer_subcode = 0;
+			IA_CSS_ERROR("Missing 2nd timer event. Timer event discarded");
+		}
+	}
+	if (event->type == IA_CSS_EVENT_TYPE_PORT_EOF) {
+		event->port = (enum mipi_port_id)payload[1];
+		event->exp_id = payload[3];
+	} else if (event->type == IA_CSS_EVENT_TYPE_FW_WARNING) {
+		event->fw_warning = (enum ia_css_fw_warning)payload[1];
+		/* exp_id is only available in these warning types */
+		if (event->fw_warning == IA_CSS_FW_WARNING_EXP_ID_LOCKED ||
+		    event->fw_warning == IA_CSS_FW_WARNING_TAG_EXP_ID_FAILED)
+			event->exp_id = payload[3];
+	} else if (event->type == IA_CSS_EVENT_TYPE_FW_ASSERT) {
+		event->fw_assert_module_id = payload[1]; /* module */
+		event->fw_assert_line_no = (payload[2] << 8) + payload[3];
+		/* payload[2] is line_no>>8, payload[3] is line_no&0xff */
+	} else if (event->type != IA_CSS_EVENT_TYPE_TIMER) {
+		/*
+		 * pipe related events.
+		 * payload[1] contains the pipe_num,
+		 * payload[2] contains the pipe_id. These are different.
+		 */
+		event->pipe = find_pipe_by_num(payload[1]);
+		pipe_id = (enum ia_css_pipe_id)payload[2];
+		/* Check to see if pipe still exists */
+		if (!event->pipe)
+			return -EBUSY;
+
+		if (event->type == IA_CSS_EVENT_TYPE_FRAME_TAGGED) {
+			/* find the capture pipe that goes with this */
+			int i, n;
+
+			n = event->pipe->stream->num_pipes;
+			for (i = 0; i < n; i++) {
+				struct ia_css_pipe *p =
+					    event->pipe->stream->pipes[i];
+				if (p->config.mode == IA_CSS_PIPE_MODE_CAPTURE) {
+					event->pipe = p;
+					break;
+				}
+			}
+			event->exp_id = payload[3];
+		}
+		if (event->type == IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE) {
+			/* payload[3] contains the acc fw handle. */
+			u32 stage_num = (uint32_t)payload[3];
+
+			ret_err = ia_css_pipeline_get_fw_from_stage(
+				      &event->pipe->pipeline,
+				      stage_num,
+				      &event->fw_handle);
+			if (ret_err) {
+				IA_CSS_ERROR("Invalid stage num received for ACC event. stage_num:%u",
+					     stage_num);
+				return ret_err;
+			}
+		}
+	}
+
+	if (event->pipe)
+		IA_CSS_LEAVE("event_id=%d, pipe_id=%d", event->type, pipe_id);
+	else
+		IA_CSS_LEAVE("event_id=%d", event->type);
+
+	return 0;
+}
+
+int
+ia_css_dequeue_isys_event(struct ia_css_event *event)
+{
+	u8 payload[4] = {0, 0, 0, 0};
+	int err = 0;
+
+	/*
+	 * We skip the IA_CSS_ENTER logging call
+	 * to avoid flooding the logs when the host application
+	 * uses polling.
+	 */
+	if (!event)
+		return -EINVAL;
+
+	/* SP is not running. The queues are not valid */
+	if (!sh_css_sp_is_running())
+		return -EBUSY;
+
+	err = ia_css_bufq_dequeue_isys_event(payload);
+	if (err)
+		return err;
+
+	IA_CSS_LOG("event dequeued from isys event queue");
+
+	/* Update SP state to indicate that element was dequeued. */
+	ia_css_bufq_enqueue_isys_event(IA_CSS_ISYS_SW_EVENT_EVENT_DEQUEUED);
+
+	/* Fill return struct with appropriate info */
+	event->type = IA_CSS_EVENT_TYPE_PORT_EOF;
+	/* EOF events are associated with a CSI port, not with a pipe */
+	event->pipe = NULL;
+	event->port = payload[1];
+	event->exp_id = payload[3];
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+static void
+acc_start(struct ia_css_pipe *pipe)
+{
+	assert(pipe);
+	assert(pipe->stream);
+
+	start_pipe(pipe, SH_CSS_PIPE_CONFIG_OVRD_NO_OVRD,
+		   pipe->stream->config.mode);
+}
+
+static int
+sh_css_pipe_start(struct ia_css_stream *stream)
+{
+	int err = 0;
+
+	struct ia_css_pipe *pipe;
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+
+	if (!stream) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+	pipe = stream->last_pipe;
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	pipe_id = pipe->mode;
+
+	if (stream->started) {
+		IA_CSS_WARNING("Cannot start stream that is already started");
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	pipe->stop_requested = false;
+
+	switch (pipe_id) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		err = preview_start(pipe);
+		break;
+	case IA_CSS_PIPE_ID_VIDEO:
+		err = video_start(pipe);
+		break;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		err = capture_start(pipe);
+		break;
+	case IA_CSS_PIPE_ID_YUVPP:
+		err = yuvpp_start(pipe);
+		break;
+	case IA_CSS_PIPE_ID_ACC:
+		acc_start(pipe);
+		break;
+	default:
+		err = -EINVAL;
+	}
+	/* DH regular multi pipe - not continuous mode: start the next pipes too */
+	if (!stream->config.continuous) {
+		int i;
+
+		for (i = 1; i < stream->num_pipes && 0 == err ; i++) {
+			switch (stream->pipes[i]->mode) {
+			case IA_CSS_PIPE_ID_PREVIEW:
+				stream->pipes[i]->stop_requested = false;
+				err = preview_start(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_VIDEO:
+				stream->pipes[i]->stop_requested = false;
+				err = video_start(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_CAPTURE:
+				stream->pipes[i]->stop_requested = false;
+				err = capture_start(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_YUVPP:
+				stream->pipes[i]->stop_requested = false;
+				err = yuvpp_start(stream->pipes[i]);
+				break;
+			case IA_CSS_PIPE_ID_ACC:
+				stream->pipes[i]->stop_requested = false;
+				acc_start(stream->pipes[i]);
+				break;
+			default:
+				err = -EINVAL;
+			}
+		}
+	}
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	/*
+	 * Force ISP parameter calculation after a mode change
+	 * Acceleration API examples pass NULL for stream but they
+	 * don't use ISP parameters anyway. So this should be okay.
+	 * The SP binary (jpeg) copy does not use any parameters.
+	 */
+	if (!copy_on_sp(pipe)) {
+		sh_css_invalidate_params(stream);
+		err = sh_css_param_update_isp_params(pipe,
+						     stream->isp_params_configs, true, NULL);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	ia_css_debug_pipe_graph_dump_epilogue();
+
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EBUSY);
+		/* SP is not running. The queues are not valid */
+		return -EBUSY;
+	}
+	ia_css_bufq_enqueue_psys_event(IA_CSS_PSYS_SW_EVENT_START_STREAM,
+				       (uint8_t)thread_id, 0, 0);
+
+	/* DH regular multi pipe - not continuous mode: enqueue event to the next pipes too */
+	if (!stream->config.continuous) {
+		int i;
+
+		for (i = 1; i < stream->num_pipes; i++) {
+			ia_css_pipeline_get_sp_thread_id(
+			    ia_css_pipe_get_pipe_num(stream->pipes[i]),
+			    &thread_id);
+			ia_css_bufq_enqueue_psys_event(
+			    IA_CSS_PSYS_SW_EVENT_START_STREAM,
+			    (uint8_t)thread_id, 0, 0);
+		}
+	}
+
+	/* in case of continuous capture mode, we also start capture thread and copy thread*/
+	if (pipe->stream->config.continuous) {
+		struct ia_css_pipe *copy_pipe = NULL;
+
+		if (pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			copy_pipe = pipe->pipe_settings.preview.copy_pipe;
+		else if (pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			copy_pipe = pipe->pipe_settings.video.copy_pipe;
+
+		if (!copy_pipe) {
+			IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+			return -EINVAL;
+		}
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(copy_pipe),
+						 &thread_id);
+		/* by the time we reach here q is initialized and handle is available.*/
+		ia_css_bufq_enqueue_psys_event(
+		    IA_CSS_PSYS_SW_EVENT_START_STREAM,
+		    (uint8_t)thread_id, 0,  0);
+	}
+	if (pipe->stream->cont_capt) {
+		struct ia_css_pipe *capture_pipe = NULL;
+
+		if (pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			capture_pipe = pipe->pipe_settings.preview.capture_pipe;
+		else if (pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			capture_pipe = pipe->pipe_settings.video.capture_pipe;
+
+		if (!capture_pipe) {
+			IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+			return -EINVAL;
+		}
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(capture_pipe),
+						 &thread_id);
+		/* by the time we reach here q is initialized and handle is available.*/
+		ia_css_bufq_enqueue_psys_event(
+		    IA_CSS_PSYS_SW_EVENT_START_STREAM,
+		    (uint8_t)thread_id, 0,  0);
+	}
+
+	/* in case of PREVIEW mode, check whether QOS acc_pipe is available, then start the qos pipe */
+	if (pipe_id == IA_CSS_PIPE_ID_PREVIEW) {
+		struct ia_css_pipe *acc_pipe = NULL;
+
+		acc_pipe = pipe->pipe_settings.preview.acc_pipe;
+
+		if (acc_pipe) {
+			ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(acc_pipe),
+							 &thread_id);
+			/* by the time we reach here q is initialized and handle is available.*/
+			ia_css_bufq_enqueue_psys_event(
+			    IA_CSS_PSYS_SW_EVENT_START_STREAM,
+			    (uint8_t)thread_id, 0, 0);
+		}
+	}
+
+	stream->started = true;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/* ISP2400 */
+void
+sh_css_enable_cont_capt(bool enable, bool stop_copy_preview)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_enable_cont_capt() enter: enable=%d\n", enable);
+//my_css.cont_capt = enable;
+	my_css.stop_copy_preview = stop_copy_preview;
+}
+
+bool
+sh_css_continuous_is_enabled(uint8_t pipe_num)
+{
+	struct ia_css_pipe *pipe;
+	bool continuous;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_continuous_is_enabled() enter: pipe_num=%d\n", pipe_num);
+
+	pipe = find_pipe_by_num(pipe_num);
+	continuous = pipe && pipe->stream->config.continuous;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_continuous_is_enabled() leave: enable=%d\n",
+			    continuous);
+	return continuous;
+}
+
+/* ISP2400 */
+int
+ia_css_stream_get_max_buffer_depth(struct ia_css_stream *stream,
+				   int *buffer_depth)
+{
+	if (!buffer_depth)
+		return -EINVAL;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_get_max_buffer_depth() enter: void\n");
+	(void)stream;
+	*buffer_depth = NUM_CONTINUOUS_FRAMES;
+	return 0;
+}
+
+int
+ia_css_stream_set_buffer_depth(struct ia_css_stream *stream, int buffer_depth)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_set_buffer_depth() enter: num_frames=%d\n", buffer_depth);
+	(void)stream;
+	if (buffer_depth > NUM_CONTINUOUS_FRAMES || buffer_depth < 1)
+		return -EINVAL;
+	/* ok, value allowed */
+	stream->config.target_num_cont_raw_buf = buffer_depth;
+	/* TODO: check what to regarding initialization */
+	return 0;
+}
+
+/* ISP2401 */
+int
+ia_css_stream_get_buffer_depth(struct ia_css_stream *stream,
+			       int *buffer_depth)
+{
+	if (!buffer_depth)
+		return -EINVAL;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_get_buffer_depth() enter: void\n");
+	(void)stream;
+	*buffer_depth = stream->config.target_num_cont_raw_buf;
+	return 0;
+}
+
+#if !defined(ISP2401)
+unsigned int
+sh_css_get_mipi_sizes_for_check(const unsigned int port, const unsigned int idx)
+{
+	OP___assert(port < N_CSI_PORTS);
+	OP___assert(idx  < IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_get_mipi_sizes_for_check(port %d, idx %d): %d\n",
+			    port, idx, my_css.mipi_sizes_for_check[port][idx]);
+	return my_css.mipi_sizes_for_check[port][idx];
+}
+#endif
+
+static int sh_css_pipe_configure_output(
+    struct ia_css_pipe *pipe,
+    unsigned int width,
+    unsigned int height,
+    unsigned int padded_width,
+    enum ia_css_frame_format format,
+    unsigned int idx)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, width = %d, height = %d, padded width = %d, format = %d, idx = %d",
+			     pipe, width, height, padded_width, format, idx);
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	err = ia_css_util_check_res(width, height);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (pipe->output_info[idx].res.width != width ||
+	    pipe->output_info[idx].res.height != height ||
+	    pipe->output_info[idx].format != format) {
+		ia_css_frame_info_init(
+		    &pipe->output_info[idx],
+		    width,
+		    height,
+		    format,
+		    padded_width);
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static int
+sh_css_pipe_get_shading_info(struct ia_css_pipe *pipe,
+			     struct ia_css_shading_info *shading_info,
+			     struct ia_css_pipe_config *pipe_config)
+{
+	int err = 0;
+	struct ia_css_binary *binary = NULL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_pipe_get_shading_info() enter:\n");
+
+	binary = ia_css_pipe_get_shading_correction_binary(pipe);
+
+	if (binary) {
+		err = ia_css_binary_get_shading_info(binary,
+						     IA_CSS_SHADING_CORRECTION_TYPE_1,
+						     pipe->required_bds_factor,
+						     (const struct ia_css_stream_config *)&pipe->stream->config,
+						     shading_info, pipe_config);
+
+		/*
+		 * Other function calls can be added here when other shading
+		 * correction types will be added in the future.
+		 */
+	} else {
+		/*
+		 * When the pipe does not have a binary which has the shading
+		 * correction, this function does not need to fill the shading
+		 * information. It is not a error case, and then
+		 * this function should return 0.
+		 */
+		memset(shading_info, 0, sizeof(*shading_info));
+	}
+	return err;
+}
+
+static int
+sh_css_pipe_get_grid_info(struct ia_css_pipe *pipe,
+			  struct ia_css_grid_info *info)
+{
+	int err = 0;
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe);
+	assert(info);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	binary = ia_css_pipe_get_s3a_binary(pipe);
+
+	if (binary) {
+		err = ia_css_binary_3a_grid_info(binary, info, pipe);
+		if (err)
+			goto err;
+	} else {
+		memset(&info->s3a_grid, 0, sizeof(info->s3a_grid));
+	}
+
+	binary = ia_css_pipe_get_sdis_binary(pipe);
+
+	if (binary) {
+		ia_css_binary_dvs_grid_info(binary, info, pipe);
+		ia_css_binary_dvs_stat_grid_info(binary, info, pipe);
+	} else {
+		memset(&info->dvs_grid, 0, sizeof(info->dvs_grid));
+		memset(&info->dvs_grid.dvs_stat_grid_info, 0,
+			   sizeof(info->dvs_grid.dvs_stat_grid_info));
+	}
+
+	if (binary) {
+		/* copy pipe does not have ISP binary*/
+		info->isp_in_width = binary->internal_frame_info.res.width;
+		info->isp_in_height = binary->internal_frame_info.res.height;
+	}
+
+	info->vamem_type = IA_CSS_VAMEM_TYPE_2;
+
+err:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/* ISP2401 */
+/*
+ * @brief Check if a format is supported by the pipe.
+ *
+ */
+static int
+ia_css_pipe_check_format(struct ia_css_pipe *pipe,
+			 enum ia_css_frame_format format)
+{
+	const enum ia_css_frame_format *supported_formats;
+	int number_of_formats;
+	int found = 0;
+	int i;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (NULL == pipe || NULL == pipe->pipe_settings.video.video_binary.info) {
+		IA_CSS_ERROR("Pipe or binary info is not set");
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	supported_formats = pipe->pipe_settings.video.video_binary.info->output_formats;
+	number_of_formats = sizeof(pipe->pipe_settings.video.video_binary.info->output_formats) / sizeof(enum ia_css_frame_format);
+
+	for (i = 0; i < number_of_formats && !found; i++) {
+		if (supported_formats[i] == format) {
+			found = 1;
+			break;
+		}
+	}
+	if (!found) {
+		IA_CSS_ERROR("Requested format is not supported by binary");
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static int load_video_binaries(struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info video_in_info, tnr_info,
+		       *video_vf_info, video_bds_out_info, *pipe_out_info, *pipe_vf_out_info;
+	bool online;
+	int err = 0;
+	bool continuous = pipe->stream->config.continuous;
+	unsigned int i;
+	unsigned int num_output_pins;
+	struct ia_css_frame_info video_bin_out_info;
+	bool need_scaler = false;
+	bool vf_res_different_than_output = false;
+	bool need_vf_pp = false;
+	int vf_ds_log2;
+	struct ia_css_video_settings *mycs  = &pipe->pipe_settings.video;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_VIDEO);
+	/*
+	 * we only test the video_binary because offline video doesn't need a
+	 * vf_pp binary and online does not (always use) the copy_binary.
+	 * All are always reset at the same time anyway.
+	 */
+	if (mycs->video_binary.info)
+		return 0;
+
+	online = pipe->stream->config.online;
+	pipe_out_info = &pipe->output_info[0];
+	pipe_vf_out_info = &pipe->vf_output_info[0];
+
+	assert(pipe_out_info);
+
+	/*
+	 * There is no explicit input format requirement for raw or yuv
+	 * What matters is that there is a binary that supports the stream format.
+	 * This is checked in the binary_find(), so no need to check it here
+	 */
+	err = ia_css_util_check_input(&pipe->stream->config, false, false);
+	if (err)
+		return err;
+	/* cannot have online video and input_mode memory */
+	if (online && pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY)
+		return -EINVAL;
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		err = ia_css_util_check_vf_out_info(pipe_out_info,
+						    pipe_vf_out_info);
+		if (err)
+			return err;
+	} else {
+		err = ia_css_frame_check_info(pipe_out_info);
+		if (err)
+			return err;
+	}
+
+	if (pipe->out_yuv_ds_input_info.res.width)
+		video_bin_out_info = pipe->out_yuv_ds_input_info;
+	else
+		video_bin_out_info = *pipe_out_info;
+
+	/* Video */
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		video_vf_info = pipe_vf_out_info;
+		vf_res_different_than_output = (video_vf_info->res.width !=
+						video_bin_out_info.res.width) ||
+					       (video_vf_info->res.height != video_bin_out_info.res.height);
+	} else {
+		video_vf_info = NULL;
+	}
+
+	need_scaler = need_downscaling(video_bin_out_info.res, pipe_out_info->res);
+
+	/* we build up the pipeline starting at the end */
+	/* YUV post-processing if needed */
+	if (need_scaler) {
+		struct ia_css_cas_binary_descr cas_scaler_descr = { };
+
+		/* NV12 is the common format that is supported by both */
+		/* yuv_scaler and the video_xx_isp2_min binaries. */
+		video_bin_out_info.format = IA_CSS_FRAME_FORMAT_NV12;
+
+		err = ia_css_pipe_create_cas_scaler_desc_single_output(
+			  &video_bin_out_info,
+			  pipe_out_info,
+			  NULL,
+			  &cas_scaler_descr);
+		if (err)
+			return err;
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kcalloc(cas_scaler_descr.num_stage,
+						  sizeof(struct ia_css_binary),
+						  GFP_KERNEL);
+		if (!mycs->yuv_scaler_binary) {
+			err = -ENOMEM;
+			return err;
+		}
+		mycs->is_output_stage = kcalloc(cas_scaler_descr.num_stage,
+						sizeof(bool), GFP_KERNEL);
+		if (!mycs->is_output_stage) {
+			err = -ENOMEM;
+			return err;
+		}
+		for (i = 0; i < cas_scaler_descr.num_stage; i++) {
+			struct ia_css_binary_descr yuv_scaler_descr;
+
+			mycs->is_output_stage[i] = cas_scaler_descr.is_output_stage[i];
+			ia_css_pipe_get_yuvscaler_binarydesc(pipe,
+							     &yuv_scaler_descr, &cas_scaler_descr.in_info[i],
+							     &cas_scaler_descr.out_info[i],
+							     &cas_scaler_descr.internal_out_info[i],
+							     &cas_scaler_descr.vf_info[i]);
+			err = ia_css_binary_find(&yuv_scaler_descr,
+						 &mycs->yuv_scaler_binary[i]);
+			if (err) {
+				kfree(mycs->is_output_stage);
+				mycs->is_output_stage = NULL;
+				return err;
+			}
+		}
+		ia_css_pipe_destroy_cas_scaler_desc(&cas_scaler_descr);
+	}
+
+	{
+		struct ia_css_binary_descr video_descr;
+		enum ia_css_frame_format vf_info_format;
+
+		err = ia_css_pipe_get_video_binarydesc(pipe,
+						       &video_descr, &video_in_info, &video_bds_out_info, &video_bin_out_info,
+						       video_vf_info,
+						       pipe->stream->config.left_padding);
+		if (err)
+			return err;
+
+		/*
+		 * In the case where video_vf_info is not NULL, this allows
+		 * us to find a potential video library with desired vf format.
+		 * If success, no vf_pp binary is needed.
+		 * If failed, we will look up video binary with YUV_LINE vf format
+		 */
+		err = ia_css_binary_find(&video_descr,
+					 &mycs->video_binary);
+
+		if (err) {
+			/* This will do another video binary lookup later for YUV_LINE format*/
+			if (video_vf_info)
+				need_vf_pp = true;
+			else
+				return err;
+		} else if (video_vf_info) {
+			/*
+			 * The first video binary lookup is successful, but we
+			 * may still need vf_pp binary based on additional check
+			 */
+			num_output_pins = mycs->video_binary.info->num_output_pins;
+			vf_ds_log2 = mycs->video_binary.vf_downscale_log2;
+
+			/*
+			 * If the binary has dual output pins, we need vf_pp
+			 * if the resolution is different.
+			 */
+			need_vf_pp |= ((num_output_pins == 2) && vf_res_different_than_output);
+
+			/*
+			 * If the binary has single output pin, we need vf_pp
+			 * if additional scaling is needed for vf
+			 */
+			need_vf_pp |= ((num_output_pins == 1) &&
+				       ((video_vf_info->res.width << vf_ds_log2 != pipe_out_info->res.width) ||
+					(video_vf_info->res.height << vf_ds_log2 != pipe_out_info->res.height)));
+		}
+
+		if (need_vf_pp) {
+			/* save the current vf_info format for restoration later */
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "load_video_binaries() need_vf_pp; find video binary with YUV_LINE again\n");
+
+			vf_info_format = video_vf_info->format;
+
+			if (!pipe->config.enable_vfpp_bci)
+				ia_css_frame_info_set_format(video_vf_info,
+							     IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+			ia_css_binary_destroy_isp_parameters(&mycs->video_binary);
+
+			err = ia_css_binary_find(&video_descr,
+						 &mycs->video_binary);
+
+			/* restore original vf_info format */
+			ia_css_frame_info_set_format(video_vf_info,
+						     vf_info_format);
+			if (err)
+				return err;
+		}
+	}
+
+	/*
+	 * If a video binary does not use a ref_frame, we set the frame delay
+	 * to 0. This is the case for the 1-stage low-power video binary.
+	 */
+	if (!mycs->video_binary.info->sp.enable.ref_frame)
+		pipe->dvs_frame_delay = 0;
+
+	/*
+	 * The delay latency determines the number of invalid frames after
+	 * a stream is started.
+	 */
+	pipe->num_invalid_frames = pipe->dvs_frame_delay;
+	pipe->info.num_invalid_frames = pipe->num_invalid_frames;
+
+	/*
+	 * Viewfinder frames also decrement num_invalid_frames. If the pipe
+	 * outputs a viewfinder output, then we need double the number of
+	 * invalid frames
+	 */
+	if (video_vf_info)
+		pipe->num_invalid_frames *= 2;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "load_video_binaries() num_invalid_frames=%d dvs_frame_delay=%d\n",
+			    pipe->num_invalid_frames, pipe->dvs_frame_delay);
+
+	/* pqiao TODO: temp hack for PO, should be removed after offline YUVPP is enabled */
+#if !defined(ISP2401)
+	/* Copy */
+	if (!online && !continuous) {
+		/*
+		 * TODO: what exactly needs doing, prepend the copy binary to
+		 *	 video base this only on !online?
+		 */
+		err = load_copy_binary(pipe,
+				       &mycs->copy_binary,
+				       &mycs->video_binary);
+		if (err)
+			return err;
+	}
+#else
+	(void)continuous;
+#endif
+
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0] && need_vf_pp) {
+		struct ia_css_binary_descr vf_pp_descr;
+
+		if (mycs->video_binary.vf_frame_info.format
+		    == IA_CSS_FRAME_FORMAT_YUV_LINE) {
+			ia_css_pipe_get_vfpp_binarydesc(pipe, &vf_pp_descr,
+							&mycs->video_binary.vf_frame_info,
+							pipe_vf_out_info);
+		} else {
+			/*
+			 * output from main binary is not yuv line. currently
+			 * this is possible only when bci is enabled on vfpp
+			 * output
+			 */
+			assert(pipe->config.enable_vfpp_bci);
+			ia_css_pipe_get_yuvscaler_binarydesc(pipe, &vf_pp_descr,
+							     &mycs->video_binary.vf_frame_info,
+							     pipe_vf_out_info, NULL, NULL);
+		}
+
+		err = ia_css_binary_find(&vf_pp_descr,
+					 &mycs->vf_pp_binary);
+		if (err)
+			return err;
+	}
+
+	err = allocate_delay_frames(pipe);
+
+	if (err)
+		return err;
+
+	if (mycs->video_binary.info->sp.enable.block_output) {
+		tnr_info = mycs->video_binary.out_frame_info[0];
+
+		/* Make tnr reference buffers output block height align */
+		tnr_info.res.height = CEIL_MUL(tnr_info.res.height,
+					       mycs->video_binary.info->sp.block.output_block_height);
+	} else {
+		tnr_info = mycs->video_binary.internal_frame_info;
+	}
+	tnr_info.format = IA_CSS_FRAME_FORMAT_YUV_LINE;
+	tnr_info.raw_bit_depth = SH_CSS_TNR_BIT_DEPTH;
+
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+		if (mycs->tnr_frames[i]) {
+			ia_css_frame_free(mycs->tnr_frames[i]);
+			mycs->tnr_frames[i] = NULL;
+		}
+		err = ia_css_frame_allocate_from_info(
+			  &mycs->tnr_frames[i],
+			  &tnr_info);
+		if (err)
+			return err;
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+	return 0;
+}
+
+static int
+unload_video_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((!pipe) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	ia_css_binary_unload(&pipe->pipe_settings.video.copy_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.video.video_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.video.vf_pp_binary);
+
+	for (i = 0; i < pipe->pipe_settings.video.num_yuv_scaler; i++)
+		ia_css_binary_unload(&pipe->pipe_settings.video.yuv_scaler_binary[i]);
+
+	kfree(pipe->pipe_settings.video.is_output_stage);
+	pipe->pipe_settings.video.is_output_stage = NULL;
+	kfree(pipe->pipe_settings.video.yuv_scaler_binary);
+	pipe->pipe_settings.video.yuv_scaler_binary = NULL;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static int video_start(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	struct ia_css_pipe *copy_pipe, *capture_pipe;
+	enum sh_css_pipe_config_override copy_ovrd;
+	enum ia_css_input_mode video_pipe_input_mode;
+	unsigned int thread_id;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	video_pipe_input_mode = pipe->stream->config.mode;
+
+	copy_pipe    = pipe->pipe_settings.video.copy_pipe;
+	capture_pipe = pipe->pipe_settings.video.capture_pipe;
+
+	sh_css_metrics_start_frame();
+
+	/* multi stream video needs mipi buffers */
+
+	err = send_mipi_frames(pipe);
+	if (err)
+		return err;
+
+	send_raw_frames(pipe);
+
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	copy_ovrd = 1 << thread_id;
+
+	if (pipe->stream->cont_capt) {
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(capture_pipe),
+						    &thread_id);
+		copy_ovrd |= 1 << thread_id;
+	}
+
+	/* Construct and load the copy pipe */
+	if (pipe->stream->config.continuous) {
+		sh_css_sp_init_pipeline(&copy_pipe->pipeline,
+					IA_CSS_PIPE_ID_COPY,
+					(uint8_t)ia_css_pipe_get_pipe_num(copy_pipe),
+					false,
+					pipe->stream->config.pixels_per_clock == 2, false,
+					false, pipe->required_bds_factor,
+					copy_ovrd,
+					pipe->stream->config.mode,
+					&pipe->stream->config.metadata_config,
+					&pipe->stream->info.metadata_info,
+					pipe->stream->config.source.port.port);
+
+		/*
+		 * make the video pipe start with mem mode input, copy handles
+		 * the actual mode
+		 */
+		video_pipe_input_mode = IA_CSS_INPUT_MODE_MEMORY;
+	}
+
+	/* Construct and load the capture pipe */
+	if (pipe->stream->cont_capt) {
+		sh_css_sp_init_pipeline(&capture_pipe->pipeline,
+					IA_CSS_PIPE_ID_CAPTURE,
+					(uint8_t)ia_css_pipe_get_pipe_num(capture_pipe),
+					capture_pipe->config.default_capture_config.enable_xnr != 0,
+					capture_pipe->stream->config.pixels_per_clock == 2,
+					true, /* continuous */
+					false, /* offline */
+					capture_pipe->required_bds_factor,
+					0,
+					IA_CSS_INPUT_MODE_MEMORY,
+					&pipe->stream->config.metadata_config,
+					&pipe->stream->info.metadata_info,
+					(enum mipi_port_id)0);
+	}
+
+	start_pipe(pipe, copy_ovrd, video_pipe_input_mode);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static
+int sh_css_pipe_get_viewfinder_frame_info(
+    struct ia_css_pipe *pipe,
+    struct ia_css_frame_info *info,
+    unsigned int idx)
+{
+	assert(pipe);
+	assert(info);
+
+	/* We could print the pointer as input arg, and the values as output */
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_pipe_get_viewfinder_frame_info() enter: void\n");
+
+	if (pipe->mode == IA_CSS_PIPE_ID_CAPTURE &&
+	    (pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_RAW ||
+	     pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_BAYER))
+		return -EINVAL;
+	/* offline video does not generate viewfinder output */
+	*info = pipe->vf_output_info[idx];
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_pipe_get_viewfinder_frame_info() leave: \
+		info.res.width=%d, info.res.height=%d, \
+		info.padded_width=%d, info.format=%d, \
+		info.raw_bit_depth=%d, info.raw_bayer_order=%d\n",
+			    info->res.width, info->res.height,
+			    info->padded_width, info->format,
+			    info->raw_bit_depth, info->raw_bayer_order);
+
+	return 0;
+}
+
+static int
+sh_css_pipe_configure_viewfinder(struct ia_css_pipe *pipe, unsigned int width,
+				 unsigned int height, unsigned int min_width,
+				 enum ia_css_frame_format format,
+				 unsigned int idx)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, width = %d, height = %d, min_width = %d, format = %d, idx = %d\n",
+			     pipe, width, height, min_width, format, idx);
+
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	err = ia_css_util_check_res(width, height);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (pipe->vf_output_info[idx].res.width != width ||
+	    pipe->vf_output_info[idx].res.height != height ||
+	    pipe->vf_output_info[idx].format != format)
+		ia_css_frame_info_init(&pipe->vf_output_info[idx], width, height,
+				       format, min_width);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static int load_copy_binaries(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	assert(pipe);
+	IA_CSS_ENTER_PRIVATE("");
+
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+	if (pipe->pipe_settings.capture.copy_binary.info)
+		return 0;
+
+	err = ia_css_frame_check_info(&pipe->output_info[0]);
+	if (err)
+		goto ERR;
+
+	err = verify_copy_out_frame_format(pipe);
+	if (err)
+		goto ERR;
+
+	err = load_copy_binary(pipe,
+			       &pipe->pipe_settings.capture.copy_binary,
+			       NULL);
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static bool need_capture_pp(
+    const struct ia_css_pipe *pipe)
+{
+	const struct ia_css_frame_info *out_info = &pipe->output_info[0];
+
+	IA_CSS_ENTER_LEAVE_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE);
+
+	/* determine whether we need to use the capture_pp binary.
+	 * This is needed for:
+	 *   1. XNR or
+	 *   2. Digital Zoom or
+	 *   3. YUV downscaling
+	 */
+	if (pipe->out_yuv_ds_input_info.res.width &&
+	    ((pipe->out_yuv_ds_input_info.res.width != out_info->res.width) ||
+	     (pipe->out_yuv_ds_input_info.res.height != out_info->res.height)))
+		return true;
+
+	if (pipe->config.default_capture_config.enable_xnr != 0)
+		return true;
+
+	if ((pipe->stream->isp_params_configs->dz_config.dx < HRT_GDC_N) ||
+	    (pipe->stream->isp_params_configs->dz_config.dy < HRT_GDC_N) ||
+	    pipe->config.enable_dz)
+		return true;
+
+	return false;
+}
+
+static bool need_capt_ldc(
+    const struct ia_css_pipe *pipe)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE);
+	return (pipe->extra_config.enable_dvs_6axis) ? true : false;
+}
+
+static int set_num_primary_stages(unsigned int *num,
+				  enum ia_css_pipe_version version)
+{
+	int err = 0;
+
+	if (!num)
+		return -EINVAL;
+
+	switch (version) {
+	case IA_CSS_PIPE_VERSION_2_6_1:
+		*num = NUM_PRIMARY_HQ_STAGES;
+		break;
+	case IA_CSS_PIPE_VERSION_2_2:
+	case IA_CSS_PIPE_VERSION_1:
+		*num = NUM_PRIMARY_STAGES;
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+static int load_primary_binaries(
+    struct ia_css_pipe *pipe)
+{
+	bool online = false;
+	bool need_pp = false;
+	bool need_isp_copy_binary = false;
+	bool need_ldc = false;
+#ifdef ISP2401
+	bool sensor = false;
+#else
+	bool memory, continuous;
+#endif
+	struct ia_css_frame_info prim_in_info,
+		       prim_out_info,
+		       capt_pp_out_info, vf_info,
+		       *vf_pp_in_info, *pipe_out_info,
+		       *pipe_vf_out_info, *capt_pp_in_info,
+		       capt_ldc_out_info;
+	int err = 0;
+	struct ia_css_capture_settings *mycs;
+	unsigned int i;
+	bool need_extra_yuv_scaler = false;
+	struct ia_css_binary_descr prim_descr[MAX_NUM_PRIMARY_STAGES];
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->stream);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+
+	online = pipe->stream->config.online;
+#ifdef ISP2401
+	sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR);
+#else
+	memory = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+	continuous = pipe->stream->config.continuous;
+#endif
+
+	mycs = &pipe->pipe_settings.capture;
+	pipe_out_info = &pipe->output_info[0];
+	pipe_vf_out_info = &pipe->vf_output_info[0];
+
+	if (mycs->primary_binary[0].info)
+		return 0;
+
+	err = set_num_primary_stages(&mycs->num_primary_stage,
+				     pipe->config.isp_pipe_version);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		err = ia_css_util_check_vf_out_info(pipe_out_info, pipe_vf_out_info);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	} else {
+		err = ia_css_frame_check_info(pipe_out_info);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+	need_pp = need_capture_pp(pipe);
+
+	/*
+	 * we use the vf output info to get the primary/capture_pp binary
+	 * configured for vf_veceven. It will select the closest downscaling
+	 * factor.
+	 */
+	vf_info = *pipe_vf_out_info;
+
+	/*
+	 * WARNING: The #if def flag has been added below as a
+	 * temporary solution to solve the problem of enabling the
+	 * view finder in a single binary in a capture flow. The
+	 * vf-pp stage has been removed for Skycam in the solution
+	 * provided. The vf-pp stage should be re-introduced when
+	 * required. This should not be considered as a clean solution.
+	 * Proper investigation should be done to come up with the clean
+	 * solution.
+	 */
+	ia_css_frame_info_set_format(&vf_info, IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+	/*
+	 * TODO: All this yuv_scaler and capturepp calculation logic
+	 * can be shared later. Capture_pp is also a yuv_scale binary
+	 * with extra XNR funcionality. Therefore, it can be made as the
+	 * first step of the cascade.
+	 */
+	capt_pp_out_info = pipe->out_yuv_ds_input_info;
+	capt_pp_out_info.format = IA_CSS_FRAME_FORMAT_YUV420;
+	capt_pp_out_info.res.width  /= MAX_PREFERRED_YUV_DS_PER_STEP;
+	capt_pp_out_info.res.height /= MAX_PREFERRED_YUV_DS_PER_STEP;
+	ia_css_frame_info_set_width(&capt_pp_out_info, capt_pp_out_info.res.width, 0);
+
+	need_extra_yuv_scaler = need_downscaling(capt_pp_out_info.res,
+						 pipe_out_info->res);
+
+	if (need_extra_yuv_scaler) {
+		struct ia_css_cas_binary_descr cas_scaler_descr = { };
+
+		err = ia_css_pipe_create_cas_scaler_desc_single_output(
+			  &capt_pp_out_info,
+			  pipe_out_info,
+			  NULL,
+			  &cas_scaler_descr);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kcalloc(cas_scaler_descr.num_stage,
+						  sizeof(struct ia_css_binary),
+						  GFP_KERNEL);
+		if (!mycs->yuv_scaler_binary) {
+			err = -ENOMEM;
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		mycs->is_output_stage = kcalloc(cas_scaler_descr.num_stage,
+						sizeof(bool), GFP_KERNEL);
+		if (!mycs->is_output_stage) {
+			err = -ENOMEM;
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		for (i = 0; i < cas_scaler_descr.num_stage; i++) {
+			struct ia_css_binary_descr yuv_scaler_descr;
+
+			mycs->is_output_stage[i] = cas_scaler_descr.is_output_stage[i];
+			ia_css_pipe_get_yuvscaler_binarydesc(pipe,
+							     &yuv_scaler_descr, &cas_scaler_descr.in_info[i],
+							     &cas_scaler_descr.out_info[i],
+							     &cas_scaler_descr.internal_out_info[i],
+							     &cas_scaler_descr.vf_info[i]);
+			err = ia_css_binary_find(&yuv_scaler_descr,
+						 &mycs->yuv_scaler_binary[i]);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+		ia_css_pipe_destroy_cas_scaler_desc(&cas_scaler_descr);
+
+	} else {
+		capt_pp_out_info = pipe->output_info[0];
+	}
+
+	/* TODO Do we disable ldc for skycam */
+	need_ldc = need_capt_ldc(pipe);
+
+	/* we build up the pipeline starting at the end */
+	/* Capture post-processing */
+	if (need_pp) {
+		struct ia_css_binary_descr capture_pp_descr;
+
+		capt_pp_in_info = need_ldc ? &capt_ldc_out_info : &prim_out_info;
+
+		ia_css_pipe_get_capturepp_binarydesc(pipe,
+						     &capture_pp_descr,
+						     capt_pp_in_info,
+						     &capt_pp_out_info,
+						     &vf_info);
+
+		err = ia_css_binary_find(&capture_pp_descr,
+					 &mycs->capture_pp_binary);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		if (need_ldc) {
+			struct ia_css_binary_descr capt_ldc_descr;
+
+			ia_css_pipe_get_ldc_binarydesc(pipe,
+						       &capt_ldc_descr,
+						       &prim_out_info,
+						       &capt_ldc_out_info);
+
+			err = ia_css_binary_find(&capt_ldc_descr,
+						 &mycs->capture_ldc_binary);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+	} else {
+		prim_out_info = *pipe_out_info;
+	}
+
+	/* Primary */
+	for (i = 0; i < mycs->num_primary_stage; i++) {
+		struct ia_css_frame_info *local_vf_info = NULL;
+
+		if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0] &&
+		    (i == mycs->num_primary_stage - 1))
+			local_vf_info = &vf_info;
+		ia_css_pipe_get_primary_binarydesc(pipe, &prim_descr[i],
+						   &prim_in_info, &prim_out_info,
+						   local_vf_info, i);
+		err = ia_css_binary_find(&prim_descr[i], &mycs->primary_binary[i]);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	/* Viewfinder post-processing */
+	if (need_pp)
+		vf_pp_in_info = &mycs->capture_pp_binary.vf_frame_info;
+	else
+		vf_pp_in_info = &mycs->primary_binary[mycs->num_primary_stage - 1].vf_frame_info;
+
+	/*
+	 * WARNING: The #if def flag has been added below as a
+	 * temporary solution to solve the problem of enabling the
+	 * view finder in a single binary in a capture flow. The
+	 * vf-pp stage has been removed for Skycam in the solution
+	 * provided. The vf-pp stage should be re-introduced when
+	 * required. Thisshould not be considered as a clean solution.
+	 * Proper  * investigation should be done to come up with the clean
+	 * solution.
+	 */
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		struct ia_css_binary_descr vf_pp_descr;
+
+		ia_css_pipe_get_vfpp_binarydesc(pipe,
+						&vf_pp_descr, vf_pp_in_info, pipe_vf_out_info);
+		err = ia_css_binary_find(&vf_pp_descr, &mycs->vf_pp_binary);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+	err = allocate_delay_frames(pipe);
+
+	if (err)
+		return err;
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, only the Direct Sensor Mode
+	 * Offline Capture uses the ISP copy binary.
+	 */
+	need_isp_copy_binary = !online && sensor;
+#else
+	need_isp_copy_binary = !online && !continuous && !memory;
+#endif
+
+	/* ISP Copy */
+	if (need_isp_copy_binary) {
+		err = load_copy_binary(pipe,
+				       &mycs->copy_binary,
+				       &mycs->primary_binary[0]);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static int
+allocate_delay_frames(struct ia_css_pipe *pipe)
+{
+	unsigned int num_delay_frames = 0, i = 0;
+	unsigned int dvs_frame_delay = 0;
+	struct ia_css_frame_info ref_info;
+	int err = 0;
+	enum ia_css_pipe_id mode = IA_CSS_PIPE_ID_VIDEO;
+	struct ia_css_frame **delay_frames = NULL;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (!pipe) {
+		IA_CSS_ERROR("Invalid args - pipe %p", pipe);
+		return -EINVAL;
+	}
+
+	mode = pipe->mode;
+	dvs_frame_delay = pipe->dvs_frame_delay;
+
+	if (dvs_frame_delay > 0)
+		num_delay_frames = dvs_frame_delay + 1;
+
+	switch (mode) {
+	case IA_CSS_PIPE_ID_CAPTURE: {
+		struct ia_css_capture_settings *mycs_capture = &pipe->pipe_settings.capture;
+		(void)mycs_capture;
+		return err;
+	}
+	break;
+	case IA_CSS_PIPE_ID_VIDEO: {
+		struct ia_css_video_settings *mycs_video = &pipe->pipe_settings.video;
+
+		ref_info = mycs_video->video_binary.internal_frame_info;
+
+		/*
+		 * The ref frame expects
+		 * 1. Y plane
+		 * 2. UV plane with line interleaving, like below
+		 *	UUUUUU(width/2 times) VVVVVVVV..(width/2 times)
+		 *
+		 * This format is not YUV420(which has Y, U and V planes).
+		 * Its closer to NV12, except that the UV plane has UV
+		 * interleaving, like UVUVUVUVUVUVUVUVU...
+		 *
+		 * TODO: make this ref_frame format as a separate frame format
+		 */
+		ref_info.format        = IA_CSS_FRAME_FORMAT_NV12;
+		delay_frames = mycs_video->delay_frames;
+	}
+	break;
+	case IA_CSS_PIPE_ID_PREVIEW: {
+		struct ia_css_preview_settings *mycs_preview = &pipe->pipe_settings.preview;
+
+		ref_info = mycs_preview->preview_binary.internal_frame_info;
+
+		/*
+		 * The ref frame expects
+		 * 1. Y plane
+		 * 2. UV plane with line interleaving, like below
+		 *	UUUUUU(width/2 times) VVVVVVVV..(width/2 times)
+		 *
+		 * This format is not YUV420(which has Y, U and V planes).
+		 * Its closer to NV12, except that the UV plane has UV
+		 * interleaving, like UVUVUVUVUVUVUVUVU...
+		 *
+		 * TODO: make this ref_frame format as a separate frame format
+		 */
+		ref_info.format        = IA_CSS_FRAME_FORMAT_NV12;
+		delay_frames = mycs_preview->delay_frames;
+	}
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	ref_info.raw_bit_depth = SH_CSS_REF_BIT_DEPTH;
+
+	assert(num_delay_frames <= MAX_NUM_VIDEO_DELAY_FRAMES);
+	for (i = 0; i < num_delay_frames; i++) {
+		err = ia_css_frame_allocate_from_info(&delay_frames[i],	&ref_info);
+		if (err)
+			return err;
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+	return 0;
+}
+
+static int load_advanced_binaries(struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info pre_in_info, gdc_in_info,
+			post_in_info, post_out_info,
+			vf_info, *vf_pp_in_info, *pipe_out_info,
+			*pipe_vf_out_info;
+	bool need_pp;
+	bool need_isp_copy = true;
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+	if (pipe->pipe_settings.capture.pre_isp_binary.info)
+		return 0;
+	pipe_out_info = &pipe->output_info[0];
+	pipe_vf_out_info = &pipe->vf_output_info[0];
+
+	vf_info = *pipe_vf_out_info;
+	err = ia_css_util_check_vf_out_info(pipe_out_info, &vf_info);
+	if (err)
+		return err;
+	need_pp = need_capture_pp(pipe);
+
+	ia_css_frame_info_set_format(&vf_info,
+				     IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+	/* we build up the pipeline starting at the end */
+	/* Capture post-processing */
+	if (need_pp) {
+		struct ia_css_binary_descr capture_pp_descr;
+
+		ia_css_pipe_get_capturepp_binarydesc(pipe, &capture_pp_descr,
+						     &post_out_info,
+						     pipe_out_info, &vf_info);
+		err = ia_css_binary_find(&capture_pp_descr,
+					 &pipe->pipe_settings.capture.capture_pp_binary);
+		if (err)
+			return err;
+	} else {
+		post_out_info = *pipe_out_info;
+	}
+
+	/* Post-gdc */
+	{
+		struct ia_css_binary_descr post_gdc_descr;
+
+		ia_css_pipe_get_post_gdc_binarydesc(pipe, &post_gdc_descr,
+						    &post_in_info,
+						    &post_out_info, &vf_info);
+		err = ia_css_binary_find(&post_gdc_descr,
+					 &pipe->pipe_settings.capture.post_isp_binary);
+		if (err)
+			return err;
+	}
+
+	/* Gdc */
+	{
+		struct ia_css_binary_descr gdc_descr;
+
+		ia_css_pipe_get_gdc_binarydesc(pipe, &gdc_descr, &gdc_in_info,
+					       &pipe->pipe_settings.capture.post_isp_binary.in_frame_info);
+		err = ia_css_binary_find(&gdc_descr,
+					 &pipe->pipe_settings.capture.anr_gdc_binary);
+		if (err)
+			return err;
+	}
+	pipe->pipe_settings.capture.anr_gdc_binary.left_padding =
+	    pipe->pipe_settings.capture.post_isp_binary.left_padding;
+
+	/* Pre-gdc */
+	{
+		struct ia_css_binary_descr pre_gdc_descr;
+
+		ia_css_pipe_get_pre_gdc_binarydesc(pipe, &pre_gdc_descr, &pre_in_info,
+						   &pipe->pipe_settings.capture.anr_gdc_binary.in_frame_info);
+		err = ia_css_binary_find(&pre_gdc_descr,
+					 &pipe->pipe_settings.capture.pre_isp_binary);
+		if (err)
+			return err;
+	}
+	pipe->pipe_settings.capture.pre_isp_binary.left_padding =
+	    pipe->pipe_settings.capture.anr_gdc_binary.left_padding;
+
+	/* Viewfinder post-processing */
+	if (need_pp) {
+		vf_pp_in_info =
+		    &pipe->pipe_settings.capture.capture_pp_binary.vf_frame_info;
+	} else {
+		vf_pp_in_info =
+		    &pipe->pipe_settings.capture.post_isp_binary.vf_frame_info;
+	}
+
+	{
+		struct ia_css_binary_descr vf_pp_descr;
+
+		ia_css_pipe_get_vfpp_binarydesc(pipe,
+						&vf_pp_descr, vf_pp_in_info, pipe_vf_out_info);
+		err = ia_css_binary_find(&vf_pp_descr,
+					 &pipe->pipe_settings.capture.vf_pp_binary);
+		if (err)
+			return err;
+	}
+
+	/* Copy */
+#ifdef ISP2401
+	/* For CSI2+, only the direct sensor mode/online requires ISP copy */
+	need_isp_copy = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
+#endif
+	if (need_isp_copy)
+		load_copy_binary(pipe,
+				 &pipe->pipe_settings.capture.copy_binary,
+				 &pipe->pipe_settings.capture.pre_isp_binary);
+
+	return err;
+}
+
+static int load_bayer_isp_binaries(struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info pre_isp_in_info, *pipe_out_info;
+	int err = 0;
+	struct ia_css_binary_descr pre_de_descr;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+	pipe_out_info = &pipe->output_info[0];
+
+	if (pipe->pipe_settings.capture.pre_isp_binary.info)
+		return 0;
+
+	err = ia_css_frame_check_info(pipe_out_info);
+	if (err)
+		return err;
+
+	ia_css_pipe_get_pre_de_binarydesc(pipe, &pre_de_descr,
+					  &pre_isp_in_info,
+					  pipe_out_info);
+
+	err = ia_css_binary_find(&pre_de_descr,
+				 &pipe->pipe_settings.capture.pre_isp_binary);
+
+	return err;
+}
+
+static int load_low_light_binaries(struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info pre_in_info, anr_in_info,
+			post_in_info, post_out_info,
+			vf_info, *pipe_vf_out_info, *pipe_out_info,
+			*vf_pp_in_info;
+	bool need_pp;
+	bool need_isp_copy = true;
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+
+	if (pipe->pipe_settings.capture.pre_isp_binary.info)
+		return 0;
+	pipe_vf_out_info = &pipe->vf_output_info[0];
+	pipe_out_info = &pipe->output_info[0];
+
+	vf_info = *pipe_vf_out_info;
+	err = ia_css_util_check_vf_out_info(pipe_out_info,
+					    &vf_info);
+	if (err)
+		return err;
+	need_pp = need_capture_pp(pipe);
+
+	ia_css_frame_info_set_format(&vf_info,
+				     IA_CSS_FRAME_FORMAT_YUV_LINE);
+
+	/* we build up the pipeline starting at the end */
+	/* Capture post-processing */
+	if (need_pp) {
+		struct ia_css_binary_descr capture_pp_descr;
+
+		ia_css_pipe_get_capturepp_binarydesc(pipe, &capture_pp_descr,
+						     &post_out_info,
+						     pipe_out_info, &vf_info);
+		err = ia_css_binary_find(&capture_pp_descr,
+					 &pipe->pipe_settings.capture.capture_pp_binary);
+		if (err)
+			return err;
+	} else {
+		post_out_info = *pipe_out_info;
+	}
+
+	/* Post-anr */
+	{
+		struct ia_css_binary_descr post_anr_descr;
+
+		ia_css_pipe_get_post_anr_binarydesc(pipe,
+						    &post_anr_descr, &post_in_info, &post_out_info, &vf_info);
+		err = ia_css_binary_find(&post_anr_descr,
+					 &pipe->pipe_settings.capture.post_isp_binary);
+		if (err)
+			return err;
+	}
+
+	/* Anr */
+	{
+		struct ia_css_binary_descr anr_descr;
+
+		ia_css_pipe_get_anr_binarydesc(pipe, &anr_descr, &anr_in_info,
+					       &pipe->pipe_settings.capture.post_isp_binary.in_frame_info);
+		err = ia_css_binary_find(&anr_descr,
+					 &pipe->pipe_settings.capture.anr_gdc_binary);
+		if (err)
+			return err;
+	}
+	pipe->pipe_settings.capture.anr_gdc_binary.left_padding =
+	    pipe->pipe_settings.capture.post_isp_binary.left_padding;
+
+	/* Pre-anr */
+	{
+		struct ia_css_binary_descr pre_anr_descr;
+
+		ia_css_pipe_get_pre_anr_binarydesc(pipe, &pre_anr_descr, &pre_in_info,
+						   &pipe->pipe_settings.capture.anr_gdc_binary.in_frame_info);
+		err = ia_css_binary_find(&pre_anr_descr,
+					 &pipe->pipe_settings.capture.pre_isp_binary);
+		if (err)
+			return err;
+	}
+	pipe->pipe_settings.capture.pre_isp_binary.left_padding =
+	    pipe->pipe_settings.capture.anr_gdc_binary.left_padding;
+
+	/* Viewfinder post-processing */
+	if (need_pp) {
+		vf_pp_in_info =
+		    &pipe->pipe_settings.capture.capture_pp_binary.vf_frame_info;
+	} else {
+		vf_pp_in_info =
+		    &pipe->pipe_settings.capture.post_isp_binary.vf_frame_info;
+	}
+
+	{
+		struct ia_css_binary_descr vf_pp_descr;
+
+		ia_css_pipe_get_vfpp_binarydesc(pipe, &vf_pp_descr,
+						vf_pp_in_info, pipe_vf_out_info);
+		err = ia_css_binary_find(&vf_pp_descr,
+					 &pipe->pipe_settings.capture.vf_pp_binary);
+		if (err)
+			return err;
+	}
+
+	/* Copy */
+#ifdef ISP2401
+	/* For CSI2+, only the direct sensor mode/online requires ISP copy */
+	need_isp_copy = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
+#endif
+	if (need_isp_copy)
+		err = load_copy_binary(pipe,
+				       &pipe->pipe_settings.capture.copy_binary,
+				       &pipe->pipe_settings.capture.pre_isp_binary);
+
+	return err;
+}
+
+static bool copy_on_sp(struct ia_css_pipe *pipe)
+{
+	bool rval;
+
+	assert(pipe);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "copy_on_sp() enter:\n");
+
+	rval = true;
+
+	rval &=	(pipe->mode == IA_CSS_PIPE_ID_CAPTURE);
+
+	rval &= (pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_RAW);
+
+	rval &= ((pipe->stream->config.input_config.format ==
+		    ATOMISP_INPUT_FORMAT_BINARY_8) ||
+		    (pipe->config.mode == IA_CSS_PIPE_MODE_COPY));
+
+	return rval;
+}
+
+static int load_capture_binaries(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	bool must_be_raw;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+
+	if (pipe->pipe_settings.capture.primary_binary[0].info) {
+		IA_CSS_LEAVE_ERR_PRIVATE(0);
+		return 0;
+	}
+
+	/* in primary, advanced,low light or bayer,
+						the input format must be raw */
+	must_be_raw =
+	    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_ADVANCED ||
+	    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_BAYER ||
+	    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_LOW_LIGHT;
+	err = ia_css_util_check_input(&pipe->stream->config, must_be_raw, false);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (copy_on_sp(pipe) &&
+	    pipe->stream->config.input_config.format == ATOMISP_INPUT_FORMAT_BINARY_8) {
+		ia_css_frame_info_init(
+		    &pipe->output_info[0],
+		    JPEG_BYTES,
+		    1,
+		    IA_CSS_FRAME_FORMAT_BINARY_8,
+		    0);
+		IA_CSS_LEAVE_ERR_PRIVATE(0);
+		return 0;
+	}
+
+	switch (pipe->config.default_capture_config.mode) {
+	case IA_CSS_CAPTURE_MODE_RAW:
+		err = load_copy_binaries(pipe);
+#if defined(ISP2401)
+		if (!err)
+			pipe->pipe_settings.capture.copy_binary.online = pipe->stream->config.online;
+#endif
+		break;
+	case IA_CSS_CAPTURE_MODE_BAYER:
+		err = load_bayer_isp_binaries(pipe);
+		break;
+	case IA_CSS_CAPTURE_MODE_PRIMARY:
+		err = load_primary_binaries(pipe);
+		break;
+	case IA_CSS_CAPTURE_MODE_ADVANCED:
+		err = load_advanced_binaries(pipe);
+		break;
+	case IA_CSS_CAPTURE_MODE_LOW_LIGHT:
+		err = load_low_light_binaries(pipe);
+		break;
+	}
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+unload_capture_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if (!pipe || (pipe->mode != IA_CSS_PIPE_ID_CAPTURE &&
+		      pipe->mode != IA_CSS_PIPE_ID_COPY)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	ia_css_binary_unload(&pipe->pipe_settings.capture.copy_binary);
+	for (i = 0; i < MAX_NUM_PRIMARY_STAGES; i++)
+		ia_css_binary_unload(&pipe->pipe_settings.capture.primary_binary[i]);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.pre_isp_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.anr_gdc_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.post_isp_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.capture_pp_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.capture_ldc_binary);
+	ia_css_binary_unload(&pipe->pipe_settings.capture.vf_pp_binary);
+
+	for (i = 0; i < pipe->pipe_settings.capture.num_yuv_scaler; i++)
+		ia_css_binary_unload(&pipe->pipe_settings.capture.yuv_scaler_binary[i]);
+
+	kfree(pipe->pipe_settings.capture.is_output_stage);
+	pipe->pipe_settings.capture.is_output_stage = NULL;
+	kfree(pipe->pipe_settings.capture.yuv_scaler_binary);
+	pipe->pipe_settings.capture.yuv_scaler_binary = NULL;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static bool
+need_downscaling(const struct ia_css_resolution in_res,
+		 const struct ia_css_resolution out_res)
+{
+	if (in_res.width > out_res.width || in_res.height > out_res.height)
+		return true;
+
+	return false;
+}
+
+static bool
+need_yuv_scaler_stage(const struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+	struct ia_css_resolution in_res, out_res;
+
+	bool need_format_conversion = false;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->mode == IA_CSS_PIPE_ID_YUVPP);
+
+	/* TODO: make generic function */
+	need_format_conversion =
+	    ((pipe->stream->config.input_config.format ==
+		ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY) &&
+		(pipe->output_info[0].format != IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8));
+
+	in_res = pipe->config.input_effective_res;
+
+	if (pipe->config.enable_dz)
+		return true;
+
+	if ((pipe->output_info[0].res.width != 0) && need_format_conversion)
+		return true;
+
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		out_res = pipe->output_info[i].res;
+
+		/* A non-zero width means it is a valid output port */
+		if ((out_res.width != 0) && need_downscaling(in_res, out_res))
+			return true;
+	}
+
+	return false;
+}
+
+/*
+ * TODO: it is temporarily created from ia_css_pipe_create_cas_scaler_desc
+ * which has some hard-coded knowledge which prevents reuse of the function.
+ * Later, merge this with ia_css_pipe_create_cas_scaler_desc
+ */
+static int ia_css_pipe_create_cas_scaler_desc_single_output(
+	    struct ia_css_frame_info *cas_scaler_in_info,
+	    struct ia_css_frame_info *cas_scaler_out_info,
+	    struct ia_css_frame_info *cas_scaler_vf_info,
+	    struct ia_css_cas_binary_descr *descr)
+{
+	unsigned int i;
+	unsigned int hor_ds_factor = 0, ver_ds_factor = 0;
+	int err = 0;
+	struct ia_css_frame_info tmp_in_info;
+
+	unsigned int max_scale_factor_per_stage = MAX_PREFERRED_YUV_DS_PER_STEP;
+
+	assert(cas_scaler_in_info);
+	assert(cas_scaler_out_info);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_create_cas_scaler_desc() enter:\n");
+
+	/* We assume that this function is used only for single output port case. */
+	descr->num_output_stage = 1;
+
+	hor_ds_factor = CEIL_DIV(cas_scaler_in_info->res.width,
+				 cas_scaler_out_info->res.width);
+	ver_ds_factor = CEIL_DIV(cas_scaler_in_info->res.height,
+				 cas_scaler_out_info->res.height);
+	/* use the same horizontal and vertical downscaling factor for simplicity */
+	assert(hor_ds_factor == ver_ds_factor);
+
+	i = 1;
+	while (i < hor_ds_factor) {
+		descr->num_stage++;
+		i *= max_scale_factor_per_stage;
+	}
+
+	descr->in_info = kmalloc(descr->num_stage *
+				 sizeof(struct ia_css_frame_info),
+				 GFP_KERNEL);
+	if (!descr->in_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->internal_out_info = kmalloc(descr->num_stage *
+					   sizeof(struct ia_css_frame_info),
+					   GFP_KERNEL);
+	if (!descr->internal_out_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->out_info = kmalloc(descr->num_stage *
+				  sizeof(struct ia_css_frame_info),
+				  GFP_KERNEL);
+	if (!descr->out_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->vf_info = kmalloc(descr->num_stage *
+				 sizeof(struct ia_css_frame_info),
+				 GFP_KERNEL);
+	if (!descr->vf_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->is_output_stage = kmalloc(descr->num_stage * sizeof(bool),
+					 GFP_KERNEL);
+	if (!descr->is_output_stage) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+
+	tmp_in_info = *cas_scaler_in_info;
+	for (i = 0; i < descr->num_stage; i++) {
+		descr->in_info[i] = tmp_in_info;
+		if ((tmp_in_info.res.width / max_scale_factor_per_stage) <=
+		    cas_scaler_out_info->res.width) {
+			descr->is_output_stage[i] = true;
+			if ((descr->num_output_stage > 1) && (i != (descr->num_stage - 1))) {
+				descr->internal_out_info[i].res.width = cas_scaler_out_info->res.width;
+				descr->internal_out_info[i].res.height = cas_scaler_out_info->res.height;
+				descr->internal_out_info[i].padded_width = cas_scaler_out_info->padded_width;
+				descr->internal_out_info[i].format = IA_CSS_FRAME_FORMAT_YUV420;
+			} else {
+				assert(i == (descr->num_stage - 1));
+				descr->internal_out_info[i].res.width = 0;
+				descr->internal_out_info[i].res.height = 0;
+			}
+			descr->out_info[i].res.width = cas_scaler_out_info->res.width;
+			descr->out_info[i].res.height = cas_scaler_out_info->res.height;
+			descr->out_info[i].padded_width = cas_scaler_out_info->padded_width;
+			descr->out_info[i].format = cas_scaler_out_info->format;
+			if (cas_scaler_vf_info) {
+				descr->vf_info[i].res.width = cas_scaler_vf_info->res.width;
+				descr->vf_info[i].res.height = cas_scaler_vf_info->res.height;
+				descr->vf_info[i].padded_width = cas_scaler_vf_info->padded_width;
+				ia_css_frame_info_set_format(&descr->vf_info[i], IA_CSS_FRAME_FORMAT_YUV_LINE);
+			} else {
+				descr->vf_info[i].res.width = 0;
+				descr->vf_info[i].res.height = 0;
+				descr->vf_info[i].padded_width = 0;
+			}
+		} else {
+			descr->is_output_stage[i] = false;
+			descr->internal_out_info[i].res.width = tmp_in_info.res.width /
+								max_scale_factor_per_stage;
+			descr->internal_out_info[i].res.height = tmp_in_info.res.height /
+				max_scale_factor_per_stage;
+			descr->internal_out_info[i].format = IA_CSS_FRAME_FORMAT_YUV420;
+			ia_css_frame_info_init(&descr->internal_out_info[i],
+					       tmp_in_info.res.width / max_scale_factor_per_stage,
+					       tmp_in_info.res.height / max_scale_factor_per_stage,
+					       IA_CSS_FRAME_FORMAT_YUV420, 0);
+			descr->out_info[i].res.width = 0;
+			descr->out_info[i].res.height = 0;
+			descr->vf_info[i].res.width = 0;
+			descr->vf_info[i].res.height = 0;
+		}
+		tmp_in_info = descr->internal_out_info[i];
+	}
+ERR:
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_create_cas_scaler_desc() leave, err=%d\n",
+			    err);
+	return err;
+}
+
+/* FIXME: merge most of this and single output version */
+static int
+ia_css_pipe_create_cas_scaler_desc(struct ia_css_pipe *pipe,
+				   struct ia_css_cas_binary_descr *descr)
+{
+	struct ia_css_frame_info in_info = IA_CSS_BINARY_DEFAULT_FRAME_INFO;
+	struct ia_css_frame_info *out_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame_info *vf_out_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame_info tmp_in_info = IA_CSS_BINARY_DEFAULT_FRAME_INFO;
+	unsigned int i, j;
+	unsigned int hor_scale_factor[IA_CSS_PIPE_MAX_OUTPUT_STAGE],
+		    ver_scale_factor[IA_CSS_PIPE_MAX_OUTPUT_STAGE],
+		    scale_factor = 0;
+	unsigned int num_stages = 0;
+	int err = 0;
+
+	unsigned int max_scale_factor_per_stage = MAX_PREFERRED_YUV_DS_PER_STEP;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_create_cas_scaler_desc() enter:\n");
+
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		out_info[i] = NULL;
+		vf_out_info[i] = NULL;
+		hor_scale_factor[i] = 0;
+		ver_scale_factor[i] = 0;
+	}
+
+	in_info.res = pipe->config.input_effective_res;
+	in_info.padded_width = in_info.res.width;
+	descr->num_output_stage = 0;
+	/* Find out how much scaling we need for each output */
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		if (pipe->output_info[i].res.width != 0) {
+			out_info[i] = &pipe->output_info[i];
+			if (pipe->vf_output_info[i].res.width != 0)
+				vf_out_info[i] = &pipe->vf_output_info[i];
+			descr->num_output_stage += 1;
+		}
+
+		if (out_info[i]) {
+			hor_scale_factor[i] = CEIL_DIV(in_info.res.width, out_info[i]->res.width);
+			ver_scale_factor[i] = CEIL_DIV(in_info.res.height, out_info[i]->res.height);
+			/* use the same horizontal and vertical scaling factor for simplicity */
+			assert(hor_scale_factor[i] == ver_scale_factor[i]);
+			scale_factor = 1;
+			do {
+				num_stages++;
+				scale_factor *= max_scale_factor_per_stage;
+			} while (scale_factor < hor_scale_factor[i]);
+
+			in_info.res = out_info[i]->res;
+		}
+	}
+
+	if (need_yuv_scaler_stage(pipe) && (num_stages == 0))
+		num_stages = 1;
+
+	descr->num_stage = num_stages;
+
+	descr->in_info = kmalloc_array(descr->num_stage,
+				       sizeof(struct ia_css_frame_info),
+				       GFP_KERNEL);
+	if (!descr->in_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->internal_out_info = kmalloc(descr->num_stage *
+					   sizeof(struct ia_css_frame_info),
+					   GFP_KERNEL);
+	if (!descr->internal_out_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->out_info = kmalloc(descr->num_stage *
+				  sizeof(struct ia_css_frame_info),
+				  GFP_KERNEL);
+	if (!descr->out_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->vf_info = kmalloc(descr->num_stage *
+				 sizeof(struct ia_css_frame_info),
+				 GFP_KERNEL);
+	if (!descr->vf_info) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+	descr->is_output_stage = kmalloc(descr->num_stage * sizeof(bool),
+					 GFP_KERNEL);
+	if (!descr->is_output_stage) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		if (out_info[i]) {
+			if (i > 0) {
+				assert((out_info[i - 1]->res.width >= out_info[i]->res.width) &&
+				       (out_info[i - 1]->res.height >= out_info[i]->res.height));
+			}
+		}
+	}
+
+	tmp_in_info.res = pipe->config.input_effective_res;
+	tmp_in_info.format = IA_CSS_FRAME_FORMAT_YUV420;
+	for (i = 0, j = 0; i < descr->num_stage; i++) {
+		assert(j < 2);
+		assert(out_info[j]);
+
+		descr->in_info[i] = tmp_in_info;
+		if ((tmp_in_info.res.width / max_scale_factor_per_stage) <=
+		    out_info[j]->res.width) {
+			descr->is_output_stage[i] = true;
+			if ((descr->num_output_stage > 1) && (i != (descr->num_stage - 1))) {
+				descr->internal_out_info[i].res.width = out_info[j]->res.width;
+				descr->internal_out_info[i].res.height = out_info[j]->res.height;
+				descr->internal_out_info[i].padded_width = out_info[j]->padded_width;
+				descr->internal_out_info[i].format = IA_CSS_FRAME_FORMAT_YUV420;
+			} else {
+				assert(i == (descr->num_stage - 1));
+				descr->internal_out_info[i].res.width = 0;
+				descr->internal_out_info[i].res.height = 0;
+			}
+			descr->out_info[i].res.width = out_info[j]->res.width;
+			descr->out_info[i].res.height = out_info[j]->res.height;
+			descr->out_info[i].padded_width = out_info[j]->padded_width;
+			descr->out_info[i].format = out_info[j]->format;
+			if (vf_out_info[j]) {
+				descr->vf_info[i].res.width = vf_out_info[j]->res.width;
+				descr->vf_info[i].res.height = vf_out_info[j]->res.height;
+				descr->vf_info[i].padded_width = vf_out_info[j]->padded_width;
+				ia_css_frame_info_set_format(&descr->vf_info[i], IA_CSS_FRAME_FORMAT_YUV_LINE);
+			} else {
+				descr->vf_info[i].res.width = 0;
+				descr->vf_info[i].res.height = 0;
+				descr->vf_info[i].padded_width = 0;
+			}
+			j++;
+		} else {
+			descr->is_output_stage[i] = false;
+			descr->internal_out_info[i].res.width = tmp_in_info.res.width /
+								max_scale_factor_per_stage;
+			descr->internal_out_info[i].res.height = tmp_in_info.res.height /
+				max_scale_factor_per_stage;
+			descr->internal_out_info[i].format = IA_CSS_FRAME_FORMAT_YUV420;
+			ia_css_frame_info_init(&descr->internal_out_info[i],
+					       tmp_in_info.res.width / max_scale_factor_per_stage,
+					       tmp_in_info.res.height / max_scale_factor_per_stage,
+					       IA_CSS_FRAME_FORMAT_YUV420, 0);
+			descr->out_info[i].res.width = 0;
+			descr->out_info[i].res.height = 0;
+			descr->vf_info[i].res.width = 0;
+			descr->vf_info[i].res.height = 0;
+		}
+		tmp_in_info = descr->internal_out_info[i];
+	}
+ERR:
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_create_cas_scaler_desc() leave, err=%d\n",
+			    err);
+	return err;
+}
+
+static void ia_css_pipe_destroy_cas_scaler_desc(struct ia_css_cas_binary_descr
+	*descr)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_destroy_cas_scaler_desc() enter:\n");
+	kfree(descr->in_info);
+	descr->in_info = NULL;
+	kfree(descr->internal_out_info);
+	descr->internal_out_info = NULL;
+	kfree(descr->out_info);
+	descr->out_info = NULL;
+	kfree(descr->vf_info);
+	descr->vf_info = NULL;
+	kfree(descr->is_output_stage);
+	descr->is_output_stage = NULL;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "ia_css_pipe_destroy_cas_scaler_desc() leave\n");
+}
+
+static int
+load_yuvpp_binaries(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	bool need_scaler = false;
+	struct ia_css_frame_info *vf_pp_in_info[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_yuvpp_settings *mycs;
+	struct ia_css_binary *next_binary;
+	struct ia_css_cas_binary_descr cas_scaler_descr = { };
+	unsigned int i, j;
+	bool need_isp_copy_binary = false;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->stream);
+	assert(pipe->mode == IA_CSS_PIPE_ID_YUVPP);
+
+	if (pipe->pipe_settings.yuvpp.copy_binary.info)
+		goto ERR;
+
+	/* Set both must_be_raw and must_be_yuv to false then yuvpp can take rgb inputs */
+	err = ia_css_util_check_input(&pipe->stream->config, false, false);
+	if (err)
+		goto ERR;
+
+	mycs = &pipe->pipe_settings.yuvpp;
+
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		if (pipe->vf_output_info[i].res.width != 0) {
+			err = ia_css_util_check_vf_out_info(&pipe->output_info[i],
+							    &pipe->vf_output_info[i]);
+			if (err)
+				goto ERR;
+		}
+		vf_pp_in_info[i] = NULL;
+	}
+
+	need_scaler = need_yuv_scaler_stage(pipe);
+
+	/* we build up the pipeline starting at the end */
+	/* Capture post-processing */
+	if (need_scaler) {
+		struct ia_css_binary_descr yuv_scaler_descr;
+
+		err = ia_css_pipe_create_cas_scaler_desc(pipe,
+							 &cas_scaler_descr);
+		if (err)
+			goto ERR;
+		mycs->num_output = cas_scaler_descr.num_output_stage;
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kcalloc(cas_scaler_descr.num_stage,
+						  sizeof(struct ia_css_binary),
+						  GFP_KERNEL);
+		if (!mycs->yuv_scaler_binary) {
+			err = -ENOMEM;
+			goto ERR;
+		}
+		mycs->is_output_stage = kcalloc(cas_scaler_descr.num_stage,
+						sizeof(bool), GFP_KERNEL);
+		if (!mycs->is_output_stage) {
+			err = -ENOMEM;
+			goto ERR;
+		}
+		for (i = 0; i < cas_scaler_descr.num_stage; i++) {
+			mycs->is_output_stage[i] = cas_scaler_descr.is_output_stage[i];
+			ia_css_pipe_get_yuvscaler_binarydesc(pipe,
+							     &yuv_scaler_descr,
+							     &cas_scaler_descr.in_info[i],
+							     &cas_scaler_descr.out_info[i],
+							     &cas_scaler_descr.internal_out_info[i],
+							     &cas_scaler_descr.vf_info[i]);
+			err = ia_css_binary_find(&yuv_scaler_descr,
+						 &mycs->yuv_scaler_binary[i]);
+			if (err)
+				goto ERR;
+		}
+		ia_css_pipe_destroy_cas_scaler_desc(&cas_scaler_descr);
+	} else {
+		mycs->num_output = 1;
+	}
+
+	if (need_scaler)
+		next_binary = &mycs->yuv_scaler_binary[0];
+	else
+		next_binary = NULL;
+
+#if defined(ISP2401)
+	/*
+	 * NOTES
+	 * - Why does the "yuvpp" pipe needs "isp_copy_binary" (i.e. ISP Copy) when
+	 *   its input is "ATOMISP_INPUT_FORMAT_YUV422_8"?
+	 *
+	 *   In most use cases, the first stage in the "yuvpp" pipe is the "yuv_scale_
+	 *   binary". However, the "yuv_scale_binary" does NOT support the input-frame
+	 *   format as "IA_CSS_STREAM _FORMAT_YUV422_8".
+	 *
+	 *   Hence, the "isp_copy_binary" is required to be present in front of the "yuv
+	 *   _scale_binary". It would translate the input-frame to the frame formats that
+	 *   are supported by the "yuv_scale_binary".
+	 *
+	 *   Please refer to "FrameWork/css/isp/pipes/capture_pp/capture_pp_1.0/capture_
+	 *   pp_defs.h" for the list of input-frame formats that are supported by the
+	 *   "yuv_scale_binary".
+	 */
+	need_isp_copy_binary =
+	    (pipe->stream->config.input_config.format == ATOMISP_INPUT_FORMAT_YUV422_8);
+#else  /* !ISP2401 */
+	need_isp_copy_binary = true;
+#endif /*  ISP2401 */
+
+	if (need_isp_copy_binary) {
+		err = load_copy_binary(pipe,
+				       &mycs->copy_binary,
+				       next_binary);
+
+		if (err)
+			goto ERR;
+
+		/*
+		 * NOTES
+		 * - Why is "pipe->pipe_settings.capture.copy_binary.online" specified?
+		 *
+		 *   In some use cases, the first stage in the "yuvpp" pipe is the
+		 *   "isp_copy_binary". The "isp_copy_binary" is designed to process
+		 *   the input from either the system DDR or from the IPU internal VMEM.
+		 *   So it provides the flag "online" to specify where its input is from,
+		 *   i.e.:
+		 *
+		 *      (1) "online <= true", the input is from the IPU internal VMEM.
+		 *      (2) "online <= false", the input is from the system DDR.
+		 *
+		 *   In other use cases, the first stage in the "yuvpp" pipe is the
+		 *   "yuv_scale_binary". "The "yuv_scale_binary" is designed to process the
+		 *   input ONLY from the system DDR. So it does not provide the flag "online"
+		 *   to specify where its input is from.
+		 */
+		pipe->pipe_settings.capture.copy_binary.online = pipe->stream->config.online;
+	}
+
+	/* Viewfinder post-processing */
+	if (need_scaler) {
+		for (i = 0, j = 0; i < mycs->num_yuv_scaler; i++) {
+			if (mycs->is_output_stage[i]) {
+				assert(j < 2);
+				vf_pp_in_info[j] =
+				    &mycs->yuv_scaler_binary[i].vf_frame_info;
+				j++;
+			}
+		}
+		mycs->num_vf_pp = j;
+	} else {
+		vf_pp_in_info[0] =
+		    &mycs->copy_binary.vf_frame_info;
+		for (i = 1; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++)
+			vf_pp_in_info[i] = NULL;
+
+		mycs->num_vf_pp = 1;
+	}
+	mycs->vf_pp_binary = kcalloc(mycs->num_vf_pp,
+				     sizeof(struct ia_css_binary),
+				     GFP_KERNEL);
+	if (!mycs->vf_pp_binary) {
+		err = -ENOMEM;
+		goto ERR;
+	}
+
+	{
+		struct ia_css_binary_descr vf_pp_descr;
+
+		for (i = 0; i < mycs->num_vf_pp; i++) {
+			if (pipe->vf_output_info[i].res.width != 0) {
+				ia_css_pipe_get_vfpp_binarydesc(pipe,
+								&vf_pp_descr, vf_pp_in_info[i], &pipe->vf_output_info[i]);
+				err = ia_css_binary_find(&vf_pp_descr, &mycs->vf_pp_binary[i]);
+				if (err)
+					goto ERR;
+			}
+		}
+	}
+
+	if (err)
+		goto ERR;
+
+ERR:
+	if (need_scaler)
+		ia_css_pipe_destroy_cas_scaler_desc(&cas_scaler_descr);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "load_yuvpp_binaries() leave, err=%d\n",
+			    err);
+	return err;
+}
+
+static int
+unload_yuvpp_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((!pipe) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	ia_css_binary_unload(&pipe->pipe_settings.yuvpp.copy_binary);
+	for (i = 0; i < pipe->pipe_settings.yuvpp.num_yuv_scaler; i++)
+		ia_css_binary_unload(&pipe->pipe_settings.yuvpp.yuv_scaler_binary[i]);
+
+	for (i = 0; i < pipe->pipe_settings.yuvpp.num_vf_pp; i++)
+		ia_css_binary_unload(&pipe->pipe_settings.yuvpp.vf_pp_binary[i]);
+
+	kfree(pipe->pipe_settings.yuvpp.is_output_stage);
+	pipe->pipe_settings.yuvpp.is_output_stage = NULL;
+	kfree(pipe->pipe_settings.yuvpp.yuv_scaler_binary);
+	pipe->pipe_settings.yuvpp.yuv_scaler_binary = NULL;
+	kfree(pipe->pipe_settings.yuvpp.vf_pp_binary);
+	pipe->pipe_settings.yuvpp.vf_pp_binary = NULL;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static int yuvpp_start(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	enum sh_css_pipe_config_override copy_ovrd;
+	enum ia_css_input_mode yuvpp_pipe_input_mode;
+	unsigned int thread_id;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	yuvpp_pipe_input_mode = pipe->stream->config.mode;
+
+	sh_css_metrics_start_frame();
+
+	/* multi stream video needs mipi buffers */
+
+	err = send_mipi_frames(pipe);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	copy_ovrd = 1 << thread_id;
+
+	start_pipe(pipe, copy_ovrd, yuvpp_pipe_input_mode);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_pipe_unload_binaries(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	/* PIPE_MODE_COPY has no binaries, but has output frames to outside*/
+	if (pipe->config.mode == IA_CSS_PIPE_MODE_COPY) {
+		IA_CSS_LEAVE_ERR_PRIVATE(0);
+		return 0;
+	}
+
+	switch (pipe->mode) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		err = unload_preview_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_VIDEO:
+		err = unload_video_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		err = unload_capture_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_YUVPP:
+		err = unload_yuvpp_binaries(pipe);
+		break;
+	default:
+		break;
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_pipe_load_binaries(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	assert(pipe);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "sh_css_pipe_load_binaries() enter:\n");
+
+	/* PIPE_MODE_COPY has no binaries, but has output frames to outside*/
+	if (pipe->config.mode == IA_CSS_PIPE_MODE_COPY)
+		return err;
+
+	switch (pipe->mode) {
+	case IA_CSS_PIPE_ID_PREVIEW:
+		err = load_preview_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_VIDEO:
+		err = load_video_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_CAPTURE:
+		err = load_capture_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_YUVPP:
+		err = load_yuvpp_binaries(pipe);
+		break;
+	case IA_CSS_PIPE_ID_ACC:
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+	if (err) {
+		if (sh_css_pipe_unload_binaries(pipe)) {
+			/*
+			 * currently css does not support multiple error
+			 * returns in a single function, using -EINVAL in
+			 * this case
+			 */
+			err = -EINVAL;
+		}
+	}
+	return err;
+}
+
+static int
+create_host_yuvpp_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+	int err = 0;
+	struct ia_css_pipeline_stage *vf_pp_stage = NULL,
+		*copy_stage = NULL,
+		*yuv_scaler_stage = NULL;
+	struct ia_css_binary *copy_binary,
+		*vf_pp_binary,
+		*yuv_scaler_binary;
+	bool need_scaler = false;
+	unsigned int num_stage, num_output_stage;
+	unsigned int i, j;
+
+	struct ia_css_frame *in_frame = NULL;
+	struct ia_css_frame *out_frame[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_frame *bin_out_frame[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame *vf_frame[IA_CSS_PIPE_MAX_OUTPUT_STAGE];
+	struct ia_css_pipeline_stage_desc stage_desc;
+	bool need_in_frameinfo_memory = false;
+#ifdef ISP2401
+	bool sensor = false;
+	bool buffered_sensor = false;
+	bool online = false;
+	bool continuous = false;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((!pipe) || (!pipe->stream) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	me = &pipe->pipeline;
+	ia_css_pipeline_clean(me);
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		out_frame[i] = NULL;
+		vf_frame[i] = NULL;
+	}
+	ia_css_pipe_util_create_output_frames(bin_out_frame);
+	num_stage  = pipe->pipe_settings.yuvpp.num_yuv_scaler;
+	num_output_stage   = pipe->pipe_settings.yuvpp.num_output;
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, always enable 'in_frameinfo_memory'
+	 * except for the following:
+	 * - Direct Sensor Mode Online Capture
+	 * - Direct Sensor Mode Continuous Capture
+	 * - Buffered Sensor Mode Continuous Capture
+	 */
+	sensor = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
+	buffered_sensor = pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR;
+	online = pipe->stream->config.online;
+	continuous = pipe->stream->config.continuous;
+	need_in_frameinfo_memory =
+	!((sensor && (online || continuous)) || (buffered_sensor && continuous));
+#else
+	/* Construct in_frame info (only in case we have dynamic input */
+	need_in_frameinfo_memory = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+#endif
+	/*
+	 * the input frame can come from:
+	 *
+	 *  a) memory: connect yuvscaler to me->in_frame
+	 *  b) sensor, via copy binary: connect yuvscaler to copy binary later
+	 *     on
+	 */
+	if (need_in_frameinfo_memory) {
+		/* TODO: improve for different input formats. */
+
+		/*
+		 * "pipe->stream->config.input_config.format" represents the sensor output
+		 * frame format, e.g. YUV422 8-bit.
+		 *
+		 * "in_frame_format" represents the imaging pipe's input frame format, e.g.
+		 * Bayer-Quad RAW.
+		 */
+		int in_frame_format;
+
+		if (pipe->stream->config.input_config.format ==
+		    ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY) {
+			in_frame_format = IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8;
+		} else if (pipe->stream->config.input_config.format ==
+			    ATOMISP_INPUT_FORMAT_YUV422_8) {
+			/*
+			 * When the sensor output frame format is "ATOMISP_INPUT_FORMAT_YUV422_8",
+			 * the "isp_copy_var" binary is selected as the first stage in the yuvpp
+			 * pipe.
+			 *
+			 * For the "isp_copy_var" binary, it reads the YUV422-8 pixels from
+			 * the frame buffer (at DDR) to the frame-line buffer (at VMEM).
+			 *
+			 * By now, the "isp_copy_var" binary does NOT provide a separated
+			 * frame-line buffer to store the YUV422-8 pixels. Instead, it stores
+			 * the YUV422-8 pixels in the frame-line buffer which is designed to
+			 * store the Bayer-Quad RAW pixels.
+			 *
+			 * To direct the "isp_copy_var" binary reading from the RAW frame-line
+			 * buffer, its input frame format must be specified as "IA_CSS_FRAME_
+			 * FORMAT_RAW".
+			 */
+			in_frame_format = IA_CSS_FRAME_FORMAT_RAW;
+		} else {
+			in_frame_format = IA_CSS_FRAME_FORMAT_NV12;
+		}
+
+		err = init_in_frameinfo_memory_defaults(pipe,
+							&me->in_frame,
+							in_frame_format);
+
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		in_frame = &me->in_frame;
+	} else {
+		in_frame = NULL;
+	}
+
+	for (i = 0; i < num_output_stage; i++) {
+		assert(i < IA_CSS_PIPE_MAX_OUTPUT_STAGE);
+		if (pipe->output_info[i].res.width != 0) {
+			err = init_out_frameinfo_defaults(pipe, &me->out_frame[i], i);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			out_frame[i] = &me->out_frame[i];
+		}
+
+		/* Construct vf_frame info (only in case we have VF) */
+		if (pipe->vf_output_info[i].res.width != 0) {
+			err = init_vf_frameinfo_defaults(pipe, &me->vf_frame[i], i);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			vf_frame[i] = &me->vf_frame[i];
+		}
+	}
+
+	copy_binary       = &pipe->pipe_settings.yuvpp.copy_binary;
+	vf_pp_binary      = pipe->pipe_settings.yuvpp.vf_pp_binary;
+	yuv_scaler_binary = pipe->pipe_settings.yuvpp.yuv_scaler_binary;
+	need_scaler = need_yuv_scaler_stage(pipe);
+
+	if (pipe->pipe_settings.yuvpp.copy_binary.info) {
+		struct ia_css_frame *in_frame_local = NULL;
+
+#ifdef ISP2401
+		/* After isp copy is enabled in_frame needs to be passed. */
+		if (!online)
+			in_frame_local = in_frame;
+#endif
+
+		if (need_scaler) {
+			ia_css_pipe_util_set_output_frames(bin_out_frame,
+							   0, NULL);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   copy_binary,
+							   bin_out_frame,
+							   in_frame_local,
+							   NULL);
+		} else {
+			ia_css_pipe_util_set_output_frames(bin_out_frame,
+							   0, out_frame[0]);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   copy_binary,
+							   bin_out_frame,
+							   in_frame_local,
+							   NULL);
+		}
+
+		err = ia_css_pipeline_create_and_add_stage(me,
+							   &stage_desc,
+							   &copy_stage);
+
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		if (copy_stage) {
+			/* if we use yuv scaler binary, vf output should be from there */
+			copy_stage->args.copy_vf = !need_scaler;
+			/* for yuvpp pipe, it should always be enabled */
+			copy_stage->args.copy_output = true;
+			/* connect output of copy binary to input of yuv scaler */
+			in_frame = copy_stage->args.out_frame[0];
+		}
+	}
+
+	if (need_scaler) {
+		struct ia_css_frame *tmp_out_frame = NULL;
+		struct ia_css_frame *tmp_vf_frame = NULL;
+		struct ia_css_frame *tmp_in_frame = in_frame;
+
+		for (i = 0, j = 0; i < num_stage; i++) {
+			assert(j < num_output_stage);
+			if (pipe->pipe_settings.yuvpp.is_output_stage[i]) {
+				tmp_out_frame = out_frame[j];
+				tmp_vf_frame = vf_frame[j];
+			} else {
+				tmp_out_frame = NULL;
+				tmp_vf_frame = NULL;
+			}
+
+			err = add_yuv_scaler_stage(pipe, me, tmp_in_frame,
+						   tmp_out_frame,
+						   NULL,
+						   &yuv_scaler_binary[i],
+						   &yuv_scaler_stage);
+
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			/* we use output port 1 as internal output port */
+			tmp_in_frame = yuv_scaler_stage->args.out_frame[1];
+			if (pipe->pipe_settings.yuvpp.is_output_stage[i]) {
+				if (tmp_vf_frame && (tmp_vf_frame->info.res.width != 0)) {
+					in_frame = yuv_scaler_stage->args.out_vf_frame;
+					err = add_vf_pp_stage(pipe, in_frame,
+							      tmp_vf_frame,
+							      &vf_pp_binary[j],
+							      &vf_pp_stage);
+
+					if (err) {
+						IA_CSS_LEAVE_ERR_PRIVATE(err);
+						return err;
+					}
+				}
+				j++;
+			}
+		}
+	} else if (copy_stage) {
+		if (vf_frame[0] && vf_frame[0]->info.res.width != 0) {
+			in_frame = copy_stage->args.out_vf_frame;
+			err = add_vf_pp_stage(pipe, in_frame, vf_frame[0],
+					      &vf_pp_binary[0], &vf_pp_stage);
+		}
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	ia_css_pipeline_finalize_stages(&pipe->pipeline,
+					pipe->stream->config.continuous);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+
+	return 0;
+}
+
+static int
+create_host_copy_pipeline(struct ia_css_pipe *pipe,
+			  unsigned int max_input_width,
+			  struct ia_css_frame *out_frame)
+{
+	struct ia_css_pipeline *me;
+	int err = 0;
+	struct ia_css_pipeline_stage_desc stage_desc;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "create_host_copy_pipeline() enter:\n");
+
+	/* pipeline already created as part of create_host_pipeline_structure */
+	me = &pipe->pipeline;
+	ia_css_pipeline_clean(me);
+
+	/* Construct out_frame info */
+	out_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+
+	if (copy_on_sp(pipe) &&
+	    pipe->stream->config.input_config.format == ATOMISP_INPUT_FORMAT_BINARY_8) {
+		ia_css_frame_info_init(&out_frame->info, JPEG_BYTES, 1,
+				       IA_CSS_FRAME_FORMAT_BINARY_8, 0);
+	} else if (out_frame->info.format == IA_CSS_FRAME_FORMAT_RAW) {
+		out_frame->info.raw_bit_depth =
+		ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	}
+
+	me->num_stages = 1;
+	me->pipe_id = IA_CSS_PIPE_ID_COPY;
+	pipe->mode  = IA_CSS_PIPE_ID_COPY;
+
+	ia_css_pipe_get_sp_func_stage_desc(&stage_desc, out_frame,
+					   IA_CSS_PIPELINE_RAW_COPY,
+					   max_input_width);
+	err = ia_css_pipeline_create_and_add_stage(me, &stage_desc, NULL);
+
+	ia_css_pipeline_finalize_stages(&pipe->pipeline,
+					pipe->stream->config.continuous);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "create_host_copy_pipeline() leave:\n");
+
+	return err;
+}
+
+static int
+create_host_isyscopy_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me = &pipe->pipeline;
+	int err = 0;
+	struct ia_css_pipeline_stage_desc stage_desc;
+	struct ia_css_frame *out_frame = &me->out_frame[0];
+	struct ia_css_pipeline_stage *out_stage = NULL;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+	unsigned int max_input_width = MAX_VECTORS_PER_INPUT_LINE_CONT * ISP_VEC_NELEMS;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "create_host_isyscopy_capture_pipeline() enter:\n");
+	ia_css_pipeline_clean(me);
+
+	/* Construct out_frame info */
+	err = sh_css_pipe_get_output_frame_info(pipe, &out_frame->info, 0);
+	if (err)
+		return err;
+	out_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, thread_id, &queue_id);
+	out_frame->dynamic_queue_id = queue_id;
+	out_frame->buf_type = IA_CSS_BUFFER_TYPE_OUTPUT_FRAME;
+
+	me->num_stages = 1;
+	me->pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+	pipe->mode  = IA_CSS_PIPE_ID_CAPTURE;
+	ia_css_pipe_get_sp_func_stage_desc(&stage_desc, out_frame,
+					   IA_CSS_PIPELINE_ISYS_COPY,
+					   max_input_width);
+	err = ia_css_pipeline_create_and_add_stage(me,
+						   &stage_desc, &out_stage);
+	if (err)
+		return err;
+
+	ia_css_pipeline_finalize_stages(me, pipe->stream->config.continuous);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "create_host_isyscopy_capture_pipeline() leave:\n");
+
+	return err;
+}
+
+static int
+create_host_regular_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+	int err = 0;
+	enum ia_css_capture_mode mode;
+	struct ia_css_pipeline_stage *current_stage = NULL;
+	struct ia_css_pipeline_stage *yuv_scaler_stage = NULL;
+	struct ia_css_binary *copy_binary,
+		*primary_binary[MAX_NUM_PRIMARY_STAGES],
+		*vf_pp_binary,
+		*pre_isp_binary,
+		*anr_gdc_binary,
+		*post_isp_binary,
+		*yuv_scaler_binary,
+		*capture_pp_binary,
+		*capture_ldc_binary;
+	bool need_pp = false;
+	bool raw;
+
+	struct ia_css_frame *in_frame;
+	struct ia_css_frame *out_frame;
+	struct ia_css_frame *out_frames[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_frame *vf_frame;
+	struct ia_css_pipeline_stage_desc stage_desc;
+	bool need_in_frameinfo_memory = false;
+#ifdef ISP2401
+	bool sensor = false;
+	bool buffered_sensor = false;
+	bool online = false;
+	bool continuous = false;
+#endif
+	unsigned int i, num_yuv_scaler, num_primary_stage;
+	bool need_yuv_pp = false;
+	bool *is_output_stage = NULL;
+	bool need_ldc = false;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe);
+	assert(pipe->stream);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE ||
+	       pipe->mode == IA_CSS_PIPE_ID_COPY);
+
+	me = &pipe->pipeline;
+	mode = pipe->config.default_capture_config.mode;
+	raw = (mode == IA_CSS_CAPTURE_MODE_RAW);
+	ia_css_pipeline_clean(me);
+	ia_css_pipe_util_create_output_frames(out_frames);
+
+#ifdef ISP2401
+	/*
+	 * When the input system is 2401, always enable 'in_frameinfo_memory'
+	 * except for the following:
+	 * - Direct Sensor Mode Online Capture
+	 * - Direct Sensor Mode Online Capture
+	 * - Direct Sensor Mode Continuous Capture
+	 * - Buffered Sensor Mode Continuous Capture
+	 */
+	sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR);
+	buffered_sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
+	online = pipe->stream->config.online;
+	continuous = pipe->stream->config.continuous;
+	need_in_frameinfo_memory =
+	!((sensor && (online || continuous)) || (buffered_sensor && (online || continuous)));
+#else
+	/* Construct in_frame info (only in case we have dynamic input */
+	need_in_frameinfo_memory = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+#endif
+	if (need_in_frameinfo_memory) {
+		err = init_in_frameinfo_memory_defaults(pipe, &me->in_frame,
+							IA_CSS_FRAME_FORMAT_RAW);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		in_frame = &me->in_frame;
+	} else {
+		in_frame = NULL;
+	}
+
+	err = init_out_frameinfo_defaults(pipe, &me->out_frame[0], 0);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	out_frame = &me->out_frame[0];
+
+	/* Construct vf_frame info (only in case we have VF) */
+	if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0]) {
+		if (mode == IA_CSS_CAPTURE_MODE_RAW || mode == IA_CSS_CAPTURE_MODE_BAYER) {
+			/* These modes don't support viewfinder output */
+			vf_frame = NULL;
+		} else {
+			init_vf_frameinfo_defaults(pipe, &me->vf_frame[0], 0);
+			vf_frame = &me->vf_frame[0];
+		}
+	} else {
+		vf_frame = NULL;
+	}
+
+	copy_binary       = &pipe->pipe_settings.capture.copy_binary;
+	num_primary_stage = pipe->pipe_settings.capture.num_primary_stage;
+	if ((num_primary_stage == 0) && (mode == IA_CSS_CAPTURE_MODE_PRIMARY)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < num_primary_stage; i++)
+		primary_binary[i] = &pipe->pipe_settings.capture.primary_binary[i];
+
+	vf_pp_binary      = &pipe->pipe_settings.capture.vf_pp_binary;
+	pre_isp_binary    = &pipe->pipe_settings.capture.pre_isp_binary;
+	anr_gdc_binary    = &pipe->pipe_settings.capture.anr_gdc_binary;
+	post_isp_binary   = &pipe->pipe_settings.capture.post_isp_binary;
+	capture_pp_binary = &pipe->pipe_settings.capture.capture_pp_binary;
+	yuv_scaler_binary = pipe->pipe_settings.capture.yuv_scaler_binary;
+	num_yuv_scaler	  = pipe->pipe_settings.capture.num_yuv_scaler;
+	is_output_stage   = pipe->pipe_settings.capture.is_output_stage;
+	capture_ldc_binary = &pipe->pipe_settings.capture.capture_ldc_binary;
+
+	need_pp = (need_capture_pp(pipe) || pipe->output_stage) &&
+		    mode != IA_CSS_CAPTURE_MODE_RAW &&
+		    mode != IA_CSS_CAPTURE_MODE_BAYER;
+	need_yuv_pp = (yuv_scaler_binary && yuv_scaler_binary->info);
+	need_ldc = (capture_ldc_binary && capture_ldc_binary->info);
+
+	if (pipe->pipe_settings.capture.copy_binary.info) {
+		if (raw) {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+#if defined(ISP2401)
+			if (!continuous) {
+				ia_css_pipe_get_generic_stage_desc(&stage_desc,
+								   copy_binary,
+								   out_frames,
+								   in_frame,
+								   NULL);
+			} else {
+				in_frame = pipe->stream->last_pipe->continuous_frames[0];
+				ia_css_pipe_get_generic_stage_desc(&stage_desc,
+								   copy_binary,
+								   out_frames,
+								   in_frame,
+								   NULL);
+			}
+#else
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   copy_binary,
+							   out_frames,
+							   NULL, NULL);
+#endif
+		} else {
+			ia_css_pipe_util_set_output_frames(out_frames, 0,
+							   in_frame);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   copy_binary,
+							   out_frames,
+							   NULL, NULL);
+		}
+
+		err = ia_css_pipeline_create_and_add_stage(me,
+							   &stage_desc,
+							   &current_stage);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	} else if (pipe->stream->config.continuous) {
+		in_frame = pipe->stream->last_pipe->continuous_frames[0];
+	}
+
+	if (mode == IA_CSS_CAPTURE_MODE_PRIMARY) {
+		struct ia_css_frame *local_in_frame = NULL;
+		struct ia_css_frame *local_out_frame = NULL;
+
+		for (i = 0; i < num_primary_stage; i++) {
+			if (i == 0)
+				local_in_frame = in_frame;
+			else
+				local_in_frame = NULL;
+#ifndef ISP2401
+			if (!need_pp && (i == num_primary_stage - 1))
+#else
+			if (!need_pp && (i == num_primary_stage - 1) && !need_ldc)
+#endif
+				local_out_frame = out_frame;
+			else
+				local_out_frame = NULL;
+			ia_css_pipe_util_set_output_frames(out_frames, 0, local_out_frame);
+			/*
+			 * WARNING: The #if def flag has been added below as a
+			 * temporary solution to solve the problem of enabling the
+			 * view finder in a single binary in a capture flow. The
+			 * vf-pp stage has been removed from Skycam in the solution
+			 * provided. The vf-pp stage should be re-introduced when
+			 * required. This  * should not be considered as a clean solution.
+			 * Proper investigation should be done to come up with the clean
+			 * solution.
+			 */
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   primary_binary[i],
+							   out_frames,
+							   local_in_frame,
+							   NULL);
+			err = ia_css_pipeline_create_and_add_stage(me,
+								   &stage_desc,
+								   &current_stage);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+		/* If we use copy iso primary, the input must be yuv iso raw */
+		current_stage->args.copy_vf =
+		    primary_binary[0]->info->sp.pipeline.mode ==
+		    IA_CSS_BINARY_MODE_COPY;
+		current_stage->args.copy_output = current_stage->args.copy_vf;
+	} else if (mode == IA_CSS_CAPTURE_MODE_ADVANCED ||
+		    mode == IA_CSS_CAPTURE_MODE_LOW_LIGHT) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, pre_isp_binary,
+						   out_frames, in_frame, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   NULL);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, anr_gdc_binary,
+						   out_frames, NULL, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   NULL);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		if (need_pp) {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   post_isp_binary,
+							   out_frames,
+							   NULL, NULL);
+		} else {
+			ia_css_pipe_util_set_output_frames(out_frames, 0,
+							   out_frame);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   post_isp_binary,
+							   out_frames,
+							   NULL, NULL);
+		}
+
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   &current_stage);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	} else if (mode == IA_CSS_CAPTURE_MODE_BAYER) {
+		ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, pre_isp_binary,
+						   out_frames, in_frame, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me, &stage_desc,
+							   NULL);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	if (need_pp && current_stage) {
+		struct ia_css_frame *local_in_frame = NULL;
+
+		local_in_frame = current_stage->args.out_frame[0];
+
+		if (need_ldc) {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, NULL);
+			ia_css_pipe_get_generic_stage_desc(&stage_desc,
+							   capture_ldc_binary,
+							   out_frames,
+							   local_in_frame,
+							   NULL);
+			err = ia_css_pipeline_create_and_add_stage(me,
+								   &stage_desc,
+								   &current_stage);
+			local_in_frame = current_stage->args.out_frame[0];
+		}
+		err = add_capture_pp_stage(pipe, me, local_in_frame,
+					   need_yuv_pp ? NULL : out_frame,
+					   capture_pp_binary,
+					   &current_stage);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	if (need_yuv_pp && current_stage) {
+		struct ia_css_frame *tmp_in_frame = current_stage->args.out_frame[0];
+		struct ia_css_frame *tmp_out_frame = NULL;
+
+		for (i = 0; i < num_yuv_scaler; i++) {
+			if (is_output_stage[i])
+				tmp_out_frame = out_frame;
+			else
+				tmp_out_frame = NULL;
+
+			err = add_yuv_scaler_stage(pipe, me, tmp_in_frame,
+						   tmp_out_frame, NULL,
+						   &yuv_scaler_binary[i],
+						   &yuv_scaler_stage);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			/* we use output port 1 as internal output port */
+			tmp_in_frame = yuv_scaler_stage->args.out_frame[1];
+		}
+	}
+
+	/*
+	 * WARNING: The #if def flag has been added below as a
+	 * temporary solution to solve the problem of enabling the
+	 * view finder in a single binary in a capture flow. The vf-pp
+	 * stage has been removed from Skycam in the solution provided.
+	 * The vf-pp stage should be re-introduced when required. This
+	 * should not be considered as a clean solution. Proper
+	 * investigation should be done to come up with the clean solution.
+	 */
+	if (mode != IA_CSS_CAPTURE_MODE_RAW &&
+	    mode != IA_CSS_CAPTURE_MODE_BAYER &&
+	    current_stage && vf_frame) {
+		in_frame = current_stage->args.out_vf_frame;
+		err = add_vf_pp_stage(pipe, in_frame, vf_frame, vf_pp_binary,
+				      &current_stage);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+	ia_css_pipeline_finalize_stages(&pipe->pipeline, pipe->stream->config.continuous);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "create_host_regular_capture_pipeline() leave:\n");
+
+	return 0;
+}
+
+static int
+create_host_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if (pipe->config.mode == IA_CSS_PIPE_MODE_COPY)
+		err = create_host_isyscopy_capture_pipeline(pipe);
+	else
+		err = create_host_regular_capture_pipeline(pipe);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+
+	return err;
+}
+
+static int capture_start(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+	unsigned int thread_id;
+
+	int err = 0;
+	enum sh_css_pipe_config_override copy_ovrd;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if (!pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	me = &pipe->pipeline;
+
+	if ((pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_RAW   ||
+	     pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_BAYER) &&
+	    (pipe->config.mode != IA_CSS_PIPE_MODE_COPY)) {
+		if (copy_on_sp(pipe)) {
+			err = start_copy_on_sp(pipe, &me->out_frame[0]);
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+#if !defined(ISP2401)
+	/* old isys: need to send_mipi_frames() in all pipe modes */
+	err = send_mipi_frames(pipe);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+#else
+	if (pipe->config.mode != IA_CSS_PIPE_MODE_COPY) {
+		err = send_mipi_frames(pipe);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+#endif
+
+	ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+	copy_ovrd = 1 << thread_id;
+
+	start_pipe(pipe, copy_ovrd, pipe->stream->config.mode);
+
+#if !defined(ISP2401)
+	/*
+	 * old isys: for IA_CSS_PIPE_MODE_COPY pipe, isys rx has to be configured,
+	 * which is currently done in start_binary(); but COPY pipe contains no binary,
+	 * and does not call start_binary(); so we need to configure the rx here.
+	 */
+	if (pipe->config.mode == IA_CSS_PIPE_MODE_COPY &&
+	    pipe->stream->reconfigure_css_rx) {
+		ia_css_isys_rx_configure(&pipe->stream->csi_rx_config,
+					 pipe->stream->config.mode);
+		pipe->stream->reconfigure_css_rx = false;
+	}
+#endif
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_pipe_get_output_frame_info(struct ia_css_pipe *pipe,
+				  struct ia_css_frame_info *info,
+				  unsigned int idx)
+{
+	assert(pipe);
+	assert(info);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_pipe_get_output_frame_info() enter:\n");
+
+	*info = pipe->output_info[idx];
+	if (copy_on_sp(pipe) &&
+	    pipe->stream->config.input_config.format == ATOMISP_INPUT_FORMAT_BINARY_8) {
+		ia_css_frame_info_init(
+		    info,
+		    JPEG_BYTES,
+		    1,
+		    IA_CSS_FRAME_FORMAT_BINARY_8,
+		    0);
+	} else if (info->format == IA_CSS_FRAME_FORMAT_RAW ||
+		   info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED) {
+		info->raw_bit_depth =
+		ia_css_pipe_util_pipe_input_format_bpp(pipe);
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_pipe_get_output_frame_info() leave:\n");
+	return 0;
+}
+
+void
+ia_css_stream_send_input_frame(const struct ia_css_stream *stream,
+			       const unsigned short *data,
+			       unsigned int width,
+			       unsigned int height)
+{
+	assert(stream);
+
+	ia_css_inputfifo_send_input_frame(
+	    data, width, height,
+	    stream->config.channel_id,
+	    stream->config.input_config.format,
+	    stream->config.pixels_per_clock == 2);
+}
+
+void
+ia_css_stream_start_input_frame(const struct ia_css_stream *stream)
+{
+	assert(stream);
+
+	ia_css_inputfifo_start_frame(
+	    stream->config.channel_id,
+	    stream->config.input_config.format,
+	    stream->config.pixels_per_clock == 2);
+}
+
+void
+ia_css_stream_send_input_line(const struct ia_css_stream *stream,
+			      const unsigned short *data,
+			      unsigned int width,
+			      const unsigned short *data2,
+			      unsigned int width2)
+{
+	assert(stream);
+
+	ia_css_inputfifo_send_line(stream->config.channel_id,
+				   data, width, data2, width2);
+}
+
+void
+ia_css_stream_send_input_embedded_line(const struct ia_css_stream *stream,
+				       enum atomisp_input_format format,
+				       const unsigned short *data,
+				       unsigned int width)
+{
+	assert(stream);
+	if (!data || width == 0)
+		return;
+	ia_css_inputfifo_send_embedded_line(stream->config.channel_id,
+					    format, data, width);
+}
+
+void
+ia_css_stream_end_input_frame(const struct ia_css_stream *stream)
+{
+	assert(stream);
+
+	ia_css_inputfifo_end_frame(stream->config.channel_id);
+}
+
+static void
+append_firmware(struct ia_css_fw_info **l, struct ia_css_fw_info *firmware)
+{
+	IA_CSS_ENTER_PRIVATE("l = %p, firmware = %p", l, firmware);
+	if (!l) {
+		IA_CSS_ERROR("NULL fw_info");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	while (*l)
+		l = &(*l)->next;
+	*l = firmware;
+	/* when multiple acc extensions are loaded, 'next' can be not NULL */
+	/*firmware->next = NULL;*/
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+static void
+remove_firmware(struct ia_css_fw_info **l, struct ia_css_fw_info *firmware)
+{
+	assert(*l);
+	assert(firmware);
+	(void)l;
+	(void)firmware;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "remove_firmware() enter:\n");
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "remove_firmware() leave:\n");
+	return; /* removing single and multiple firmware is handled in acc_unload_extension() */
+}
+
+static int upload_isp_code(struct ia_css_fw_info *firmware)
+{
+	ia_css_ptr binary;
+
+	if (!firmware) {
+		IA_CSS_ERROR("NULL input parameter");
+		return -EINVAL;
+	}
+	binary = firmware->info.isp.xmem_addr;
+
+	if (!binary) {
+		unsigned int size = firmware->blob.size;
+		const unsigned char *blob;
+		const unsigned char *binary_name;
+
+		binary_name =
+		    (const unsigned char *)(IA_CSS_EXT_ISP_PROG_NAME(
+						firmware));
+		blob = binary_name +
+			strlen((const char *)binary_name) +
+			1;
+		binary = sh_css_load_blob(blob, size);
+		firmware->info.isp.xmem_addr = binary;
+	}
+
+	if (!binary)
+		return -ENOMEM;
+	return 0;
+}
+
+static int
+acc_load_extension(struct ia_css_fw_info *firmware)
+{
+	int err;
+	struct ia_css_fw_info *hd = firmware;
+
+	while (hd) {
+		err = upload_isp_code(hd);
+		if (err)
+			return err;
+		hd = hd->next;
+	}
+
+	if (!firmware)
+		return -EINVAL;
+	firmware->loaded = true;
+	return 0;
+}
+
+static void
+acc_unload_extension(struct ia_css_fw_info *firmware)
+{
+	struct ia_css_fw_info *hd = firmware;
+	struct ia_css_fw_info *hdn = NULL;
+
+	if (!firmware) /* should not happen */
+		return;
+	/* unload and remove multiple firmwares */
+	while (hd) {
+		hdn = (hd->next) ? &(*hd->next) : NULL;
+		if (hd->info.isp.xmem_addr) {
+			hmm_free(hd->info.isp.xmem_addr);
+			hd->info.isp.xmem_addr = mmgr_NULL;
+		}
+		hd->isp_code = NULL;
+		hd->next = NULL;
+		hd = hdn;
+	}
+
+	firmware->loaded = false;
+}
+
+/* Load firmware for extension */
+static int
+ia_css_pipe_load_extension(struct ia_css_pipe *pipe,
+			   struct ia_css_fw_info *firmware)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("fw = %p pipe = %p", firmware, pipe);
+
+	if ((!firmware) || (!pipe)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (firmware->info.isp.type == IA_CSS_ACC_OUTPUT)
+		append_firmware(&pipe->output_stage, firmware);
+	else if (firmware->info.isp.type == IA_CSS_ACC_VIEWFINDER)
+		append_firmware(&pipe->vf_stage, firmware);
+	err = acc_load_extension(firmware);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/* Unload firmware for extension */
+static void
+ia_css_pipe_unload_extension(struct ia_css_pipe *pipe,
+			     struct ia_css_fw_info *firmware)
+{
+	IA_CSS_ENTER_PRIVATE("fw = %p pipe = %p", firmware, pipe);
+
+	if ((!firmware) || (!pipe)) {
+		IA_CSS_ERROR("NULL input parameters");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+
+	if (firmware->info.isp.type == IA_CSS_ACC_OUTPUT)
+		remove_firmware(&pipe->output_stage, firmware);
+	else if (firmware->info.isp.type == IA_CSS_ACC_VIEWFINDER)
+		remove_firmware(&pipe->vf_stage, firmware);
+	acc_unload_extension(firmware);
+
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+bool
+ia_css_pipeline_uses_params(struct ia_css_pipeline *me)
+{
+	struct ia_css_pipeline_stage *stage;
+
+	assert(me);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_uses_params() enter: me=%p\n", me);
+
+	for (stage = me->stages; stage; stage = stage->next)
+		if (stage->binary_info && stage->binary_info->enable.params) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_pipeline_uses_params() leave: return_bool=true\n");
+			return true;
+		}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipeline_uses_params() leave: return_bool=false\n");
+	return false;
+}
+
+static int
+sh_css_pipeline_add_acc_stage(struct ia_css_pipeline *pipeline,
+			      const void *acc_fw)
+{
+	struct ia_css_fw_info *fw = (struct ia_css_fw_info *)acc_fw;
+	/* In QoS case, load_extension already called, so skipping */
+	int	err = 0;
+
+	if (!fw->loaded)
+		err = acc_load_extension(fw);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_pipeline_add_acc_stage() enter: pipeline=%p, acc_fw=%p\n",
+			    pipeline, acc_fw);
+
+	if (!err) {
+		struct ia_css_pipeline_stage_desc stage_desc;
+
+		ia_css_pipe_get_acc_stage_desc(&stage_desc, NULL, fw);
+		err = ia_css_pipeline_create_and_add_stage(pipeline,
+							   &stage_desc,
+							   NULL);
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "sh_css_pipeline_add_acc_stage() leave: return_err=%d\n", err);
+	return err;
+}
+
+/*
+ * @brief Tag a specific frame in continuous capture.
+ * Refer to "sh_css_internal.h" for details.
+ */
+int ia_css_stream_capture_frame(struct ia_css_stream *stream,
+				unsigned int exp_id)
+{
+	struct sh_css_tag_descr tag_descr;
+	u32 encoded_tag_descr;
+	int err;
+
+	assert(stream);
+	IA_CSS_ENTER("exp_id=%d", exp_id);
+
+	/* Only continuous streams have a tagger */
+	if (exp_id == 0 || !stream->config.continuous) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		IA_CSS_LEAVE_ERR(-EBUSY);
+		return -EBUSY;
+	}
+
+	/* Create the tag descriptor from the parameters */
+	sh_css_create_tag_descr(0, 0, 0, exp_id, &tag_descr);
+	/* Encode the tag descriptor into a 32-bit value */
+	encoded_tag_descr = sh_css_encode_tag_descr(&tag_descr);
+	/*
+	 * Enqueue the encoded tag to the host2sp queue.
+	 * Note: The pipe and stage IDs for tag_cmd queue are hard-coded to 0
+	 * on both host and the SP side.
+	 * It is mainly because it is enough to have only one tag_cmd queue
+	 */
+	err = ia_css_bufq_enqueue_tag_cmd(encoded_tag_descr);
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+/*
+ * @brief Configure the continuous capture.
+ * Refer to "sh_css_internal.h" for details.
+ */
+int ia_css_stream_capture(struct ia_css_stream *stream, int num_captures,
+			  unsigned int skip, int offset)
+{
+	struct sh_css_tag_descr tag_descr;
+	unsigned int encoded_tag_descr;
+	int return_err;
+
+	if (!stream)
+		return -EINVAL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_stream_capture() enter: num_captures=%d, skip=%d, offset=%d\n",
+			    num_captures, skip, offset);
+
+	/* Check if the tag descriptor is valid */
+	if (num_captures < SH_CSS_MINIMUM_TAG_ID) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_stream_capture() leave: return_err=%d\n",
+				    -EINVAL);
+		return -EINVAL;
+	}
+
+	/* Create the tag descriptor from the parameters */
+	sh_css_create_tag_descr(num_captures, skip, offset, 0, &tag_descr);
+
+	/* Encode the tag descriptor into a 32-bit value */
+	encoded_tag_descr = sh_css_encode_tag_descr(&tag_descr);
+
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_stream_capture() leaving:queues unavailable\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Enqueue the encoded tag to the host2sp queue.
+	 * Note: The pipe and stage IDs for tag_cmd queue are hard-coded to 0
+	 * on both host and the SP side.
+	 * It is mainly because it is enough to have only one tag_cmd queue
+	 */
+	return_err = ia_css_bufq_enqueue_tag_cmd((uint32_t)encoded_tag_descr);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_stream_capture() leave: return_err=%d\n",
+			    return_err);
+
+	return return_err;
+}
+
+void ia_css_stream_request_flash(struct ia_css_stream *stream)
+{
+	(void)stream;
+
+	assert(stream);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_stream_request_flash() enter: void\n");
+
+#ifndef ISP2401
+	sh_css_write_host2sp_command(host2sp_cmd_start_flash);
+#else
+	if (sh_css_sp_is_running()) {
+		if (!sh_css_write_host2sp_command(host2sp_cmd_start_flash)) {
+			IA_CSS_ERROR("Call to 'sh-css_write_host2sp_command()' failed");
+			ia_css_debug_dump_sp_sw_debug_info();
+			ia_css_debug_dump_debug_info(NULL);
+		}
+	} else {
+		IA_CSS_LOG("SP is not running!");
+	}
+
+#endif
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_stream_request_flash() leave: return_void\n");
+}
+
+static void
+sh_css_init_host_sp_control_vars(void)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_ia_css_ispctrl_sp_isp_started;
+
+	unsigned int HIVE_ADDR_host_sp_queues_initialized;
+	unsigned int HIVE_ADDR_sp_sleep_mode;
+	unsigned int HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb;
+	unsigned int HIVE_ADDR_sp_stop_copy_preview;
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int o = offsetof(struct host_sp_communication, host2sp_command)
+			    / sizeof(int);
+
+	unsigned int i;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_init_host_sp_control_vars() enter: void\n");
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_ia_css_ispctrl_sp_isp_started = fw->info.sp.isp_started;
+
+	HIVE_ADDR_host_sp_queues_initialized =
+	    fw->info.sp.host_sp_queues_initialized;
+	HIVE_ADDR_sp_sleep_mode = fw->info.sp.sleep_mode;
+	HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb = fw->info.sp.invalidate_tlb;
+	HIVE_ADDR_sp_stop_copy_preview = fw->info.sp.stop_copy_preview;
+	HIVE_ADDR_host_sp_com = fw->info.sp.host_sp_com;
+
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(ia_css_ispctrl_sp_isp_started),
+			     (uint32_t)(0));
+
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(host_sp_queues_initialized),
+			     (uint32_t)(0));
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(sp_sleep_mode),
+			     (uint32_t)(0));
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(ia_css_dmaproxy_sp_invalidate_tlb),
+			     (uint32_t)(false));
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(sp_stop_copy_preview),
+			     my_css.stop_copy_preview ? (uint32_t)(1) : (uint32_t)(0));
+	store_sp_array_uint(host_sp_com, o, host2sp_cmd_ready);
+
+	for (i = 0; i < N_CSI_PORTS; i++) {
+		sh_css_update_host2sp_num_mipi_frames
+		(my_css.num_mipi_frames[i]);
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_init_host_sp_control_vars() leave: return_void\n");
+}
+
+/*
+ * create the internal structures and fill in the configuration data
+ */
+
+static const struct
+ia_css_pipe_config ia_css_pipe_default_config = DEFAULT_PIPE_CONFIG;
+
+void ia_css_pipe_config_defaults(struct ia_css_pipe_config *pipe_config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_pipe_config_defaults()\n");
+	memcpy(pipe_config, &ia_css_pipe_default_config, sizeof(*pipe_config));
+}
+
+void
+ia_css_pipe_extra_config_defaults(struct ia_css_pipe_extra_config *extra_config)
+{
+	if (!extra_config) {
+		IA_CSS_ERROR("NULL input parameter");
+		return;
+	}
+
+	extra_config->enable_raw_binning = false;
+	extra_config->enable_yuv_ds = false;
+	extra_config->enable_high_speed = false;
+	extra_config->enable_dvs_6axis = false;
+	extra_config->enable_reduced_pipe = false;
+	extra_config->disable_vf_pp = false;
+	extra_config->enable_fractional_ds = false;
+}
+
+void ia_css_stream_config_defaults(struct ia_css_stream_config *stream_config)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_config_defaults()\n");
+	assert(stream_config);
+	memset(stream_config, 0, sizeof(*stream_config));
+	stream_config->online = true;
+	stream_config->left_padding = -1;
+	stream_config->pixels_per_clock = 1;
+	/*
+	 * temporary default value for backwards compatibility.
+	 * This field used to be hardcoded within CSS but this has now
+	 * been moved to the stream_config struct.
+	 */
+	stream_config->source.port.rxcount = 0x04040404;
+}
+
+static int
+ia_css_acc_pipe_create(struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	if (!pipe) {
+		IA_CSS_ERROR("NULL input parameter");
+		return -EINVAL;
+	}
+
+	/* There is not meaning for num_execs = 0 semantically. Run at least once. */
+	if (pipe->config.acc_num_execs == 0)
+		pipe->config.acc_num_execs = 1;
+
+	if (pipe->config.acc_extension)
+		err = ia_css_pipe_load_extension(pipe, pipe->config.acc_extension);
+
+	return err;
+}
+
+int ia_css_pipe_create(const struct ia_css_pipe_config *config,
+		       struct ia_css_pipe **pipe)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("config = %p, pipe = %p", config, pipe);
+
+	if (!config || !pipe) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	err = ia_css_pipe_create_extra(config, NULL, pipe);
+
+	if (err == 0)
+		IA_CSS_LOG("pipe created successfully = %p", *pipe);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+
+	return err;
+}
+
+int
+ia_css_pipe_create_extra(const struct ia_css_pipe_config *config,
+			 const struct ia_css_pipe_extra_config *extra_config,
+			 struct ia_css_pipe **pipe)
+{
+	int err = -EINVAL;
+	struct ia_css_pipe *internal_pipe = NULL;
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("config = %p, extra_config = %p and pipe = %p", config, extra_config, pipe);
+
+	/* do not allow to create more than the maximum limit */
+	if (my_css.pipe_counter >= IA_CSS_PIPELINE_NUM_MAX) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOSPC);
+		return -EINVAL;
+	}
+
+	if ((!pipe) || (!config)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	ia_css_debug_dump_pipe_config(config);
+	ia_css_debug_dump_pipe_extra_config(extra_config);
+
+	err = create_pipe(config->mode, &internal_pipe, false);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	/* now we have a pipe structure to fill */
+	internal_pipe->config = *config;
+	if (extra_config)
+		internal_pipe->extra_config = *extra_config;
+	else
+		ia_css_pipe_extra_config_defaults(&internal_pipe->extra_config);
+
+	if (config->mode == IA_CSS_PIPE_MODE_ACC) {
+		/*
+		 * Temporary hack to migrate acceleration to CSS 2.0.
+		 * In the future the code for all pipe types should be
+		 * unified.
+		 */
+		*pipe = internal_pipe;
+		if (!internal_pipe->config.acc_extension &&
+		    internal_pipe->config.num_acc_stages ==
+		    0) { /* if no acc binary and no standalone stage */
+			*pipe = NULL;
+			IA_CSS_LEAVE_ERR_PRIVATE(0);
+			return 0;
+		}
+		return ia_css_acc_pipe_create(internal_pipe);
+	}
+
+	/*
+	 * Use config value when dvs_frame_delay setting equal to 2,
+	 * otherwise always 1 by default
+	 */
+	if (internal_pipe->config.dvs_frame_delay == IA_CSS_FRAME_DELAY_2)
+		internal_pipe->dvs_frame_delay = 2;
+	else
+		internal_pipe->dvs_frame_delay = 1;
+
+	/*
+	 * we still keep enable_raw_binning for backward compatibility,
+	 * for any new fractional bayer downscaling, we should use
+	 * bayer_ds_out_res. if both are specified, bayer_ds_out_res will
+	 * take precedence.if none is specified, we set bayer_ds_out_res
+	 * equal to IF output resolution(IF may do cropping on sensor output)
+	 * or use default decimation factor 1.
+	 */
+
+	/* YUV downscaling */
+	if ((internal_pipe->config.vf_pp_in_res.width ||
+	     internal_pipe->config.capt_pp_in_res.width)) {
+		enum ia_css_frame_format format;
+
+		if (internal_pipe->config.vf_pp_in_res.width) {
+			format = IA_CSS_FRAME_FORMAT_YUV_LINE;
+			ia_css_frame_info_init(
+			    &internal_pipe->vf_yuv_ds_input_info,
+			    internal_pipe->config.vf_pp_in_res.width,
+			    internal_pipe->config.vf_pp_in_res.height,
+			    format, 0);
+		}
+		if (internal_pipe->config.capt_pp_in_res.width) {
+			format = IA_CSS_FRAME_FORMAT_YUV420;
+			ia_css_frame_info_init(
+			    &internal_pipe->out_yuv_ds_input_info,
+			    internal_pipe->config.capt_pp_in_res.width,
+			    internal_pipe->config.capt_pp_in_res.height,
+			    format, 0);
+		}
+	}
+	if (internal_pipe->config.vf_pp_in_res.width &&
+	    internal_pipe->config.mode == IA_CSS_PIPE_MODE_PREVIEW) {
+		ia_css_frame_info_init(
+		    &internal_pipe->vf_yuv_ds_input_info,
+		    internal_pipe->config.vf_pp_in_res.width,
+		    internal_pipe->config.vf_pp_in_res.height,
+		    IA_CSS_FRAME_FORMAT_YUV_LINE, 0);
+	}
+	/* handle bayer downscaling output info */
+	if (internal_pipe->config.bayer_ds_out_res.width) {
+		ia_css_frame_info_init(
+		    &internal_pipe->bds_output_info,
+		    internal_pipe->config.bayer_ds_out_res.width,
+		    internal_pipe->config.bayer_ds_out_res.height,
+		    IA_CSS_FRAME_FORMAT_RAW, 0);
+	}
+
+	/* handle output info, assume always needed */
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		if (internal_pipe->config.output_info[i].res.width) {
+			err = sh_css_pipe_configure_output(
+				    internal_pipe,
+				    internal_pipe->config.output_info[i].res.width,
+				    internal_pipe->config.output_info[i].res.height,
+				    internal_pipe->config.output_info[i].padded_width,
+				    internal_pipe->config.output_info[i].format,
+				    i);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				kvfree(internal_pipe);
+				internal_pipe = NULL;
+				return err;
+			}
+		}
+
+		/* handle vf output info, when configured */
+		internal_pipe->enable_viewfinder[i] =
+		    (internal_pipe->config.vf_output_info[i].res.width != 0);
+		if (internal_pipe->config.vf_output_info[i].res.width) {
+			err = sh_css_pipe_configure_viewfinder(
+				    internal_pipe,
+				    internal_pipe->config.vf_output_info[i].res.width,
+				    internal_pipe->config.vf_output_info[i].res.height,
+				    internal_pipe->config.vf_output_info[i].padded_width,
+				    internal_pipe->config.vf_output_info[i].format,
+				    i);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				kvfree(internal_pipe);
+				internal_pipe = NULL;
+				return err;
+			}
+		}
+	}
+	if (internal_pipe->config.acc_extension) {
+		err = ia_css_pipe_load_extension(internal_pipe,
+						 internal_pipe->config.acc_extension);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			kvfree(internal_pipe);
+			return err;
+		}
+	}
+	/* set all info to zeroes first */
+	memset(&internal_pipe->info, 0, sizeof(internal_pipe->info));
+
+	/* all went well, return the pipe */
+	*pipe = internal_pipe;
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+int
+ia_css_pipe_get_info(const struct ia_css_pipe *pipe,
+		     struct ia_css_pipe_info *pipe_info)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_pipe_get_info()\n");
+	if (!pipe_info) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "ia_css_pipe_get_info: pipe_info cannot be NULL\n");
+		return -EINVAL;
+	}
+	if (!pipe || !pipe->stream) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "ia_css_pipe_get_info: ia_css_stream_create needs to be called before ia_css_[stream/pipe]_get_info\n");
+		return -EINVAL;
+	}
+	/* we succeeded return the info */
+	*pipe_info = pipe->info;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_pipe_get_info() leave\n");
+	return 0;
+}
+
+bool ia_css_pipe_has_dvs_stats(struct ia_css_pipe_info *pipe_info)
+{
+	unsigned int i;
+
+	if (pipe_info) {
+		for (i = 0; i < IA_CSS_DVS_STAT_NUM_OF_LEVELS; i++) {
+			if (pipe_info->grid_info.dvs_grid.dvs_stat_grid_info.grd_cfg[i].grd_start.enable)
+				return true;
+		}
+	}
+
+	return false;
+}
+
+int
+ia_css_pipe_override_frame_format(struct ia_css_pipe *pipe,
+				  int pin_index,
+				  enum ia_css_frame_format new_format)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, pin_index = %d, new_formats = %d", pipe, pin_index, new_format);
+
+	if (!pipe) {
+		IA_CSS_ERROR("pipe is not set");
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (0 != pin_index && 1 != pin_index) {
+		IA_CSS_ERROR("pin index is not valid");
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (new_format != IA_CSS_FRAME_FORMAT_NV12_TILEY) {
+		IA_CSS_ERROR("new format is not valid");
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	} else {
+		err = ia_css_pipe_check_format(pipe, new_format);
+		if (!err) {
+			if (pin_index == 0)
+				pipe->output_info[0].format = new_format;
+			else
+				pipe->vf_output_info[0].format = new_format;
+		}
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+#if !defined(ISP2401)
+/* Configuration of INPUT_SYSTEM_VERSION_2401 is done on SP */
+static int
+ia_css_stream_configure_rx(struct ia_css_stream *stream)
+{
+	struct ia_css_input_port *config;
+
+	assert(stream);
+
+	config = &stream->config.source.port;
+	/* AM: this code is not reliable, especially for 2400 */
+	if (config->num_lanes == 1)
+		stream->csi_rx_config.mode = MONO_1L_1L_0L;
+	else if (config->num_lanes == 2)
+		stream->csi_rx_config.mode = MONO_2L_1L_0L;
+	else if (config->num_lanes == 3)
+		stream->csi_rx_config.mode = MONO_3L_1L_0L;
+	else if (config->num_lanes == 4)
+		stream->csi_rx_config.mode = MONO_4L_1L_0L;
+	else if (config->num_lanes != 0)
+		return -EINVAL;
+
+	if (config->port > MIPI_PORT2_ID)
+		return -EINVAL;
+	stream->csi_rx_config.port =
+	ia_css_isys_port_to_mipi_port(config->port);
+	stream->csi_rx_config.timeout    = config->timeout;
+	stream->csi_rx_config.initcount  = 0;
+	stream->csi_rx_config.synccount  = 0x28282828;
+	stream->csi_rx_config.rxcount    = config->rxcount;
+	if (config->compression.type == IA_CSS_CSI2_COMPRESSION_TYPE_NONE)
+		stream->csi_rx_config.comp = MIPI_PREDICTOR_NONE;
+	else
+		/*
+		 * not implemented yet, requires extension of the rx_cfg_t
+		 * struct
+		 */
+		return -EINVAL;
+
+	stream->csi_rx_config.is_two_ppc = (stream->config.pixels_per_clock == 2);
+	stream->reconfigure_css_rx = true;
+	return 0;
+}
+#endif
+
+static struct ia_css_pipe *
+find_pipe(struct ia_css_pipe *pipes[], unsigned int num_pipes,
+	  enum ia_css_pipe_mode mode, bool copy_pipe)
+{
+	unsigned int i;
+
+	assert(pipes);
+	for (i = 0; i < num_pipes; i++) {
+		assert(pipes[i]);
+		if (pipes[i]->config.mode != mode)
+			continue;
+		if (copy_pipe && pipes[i]->mode != IA_CSS_PIPE_ID_COPY)
+			continue;
+		return pipes[i];
+	}
+	return NULL;
+}
+
+static int
+ia_css_acc_stream_create(struct ia_css_stream *stream)
+{
+	int i;
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+
+	if (!stream) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+
+	for (i = 0;  i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+
+		if (!pipe) {
+			IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+			return -EINVAL;
+		}
+
+		pipe->stream = stream;
+	}
+
+	/* Map SP threads before doing anything. */
+	err = map_sp_threads(stream, true);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	for (i = 0;  i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+
+		assert(pipe);
+		ia_css_pipe_map_queue(pipe, true);
+	}
+
+	err = create_host_pipeline_structure(stream);
+	if (err) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	stream->started = false;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+
+	return 0;
+}
+
+static int
+metadata_info_init(const struct ia_css_metadata_config *mdc,
+		   struct ia_css_metadata_info *md)
+{
+	/* Either both width and height should be set or neither */
+	if ((mdc->resolution.height > 0) ^ (mdc->resolution.width > 0))
+		return -EINVAL;
+
+	md->resolution = mdc->resolution;
+	/*
+	 * We round up the stride to a multiple of the width
+	 * of the port going to DDR, this is a HW requirements (DMA).
+	 */
+	md->stride = CEIL_MUL(mdc->resolution.width, HIVE_ISP_DDR_WORD_BYTES);
+	md->size = mdc->resolution.height * md->stride;
+	return 0;
+}
+
+int
+ia_css_stream_create(const struct ia_css_stream_config *stream_config,
+		     int num_pipes,
+		     struct ia_css_pipe *pipes[],
+		     struct ia_css_stream **stream)
+{
+	struct ia_css_pipe *curr_pipe;
+	struct ia_css_stream *curr_stream = NULL;
+	bool spcopyonly;
+	bool sensor_binning_changed;
+	int i, j;
+	int err = -EINVAL;
+	struct ia_css_metadata_info md_info;
+	struct ia_css_resolution effective_res;
+
+	IA_CSS_ENTER("num_pipes=%d", num_pipes);
+	ia_css_debug_dump_stream_config(stream_config, num_pipes);
+
+	/* some checks */
+	if (num_pipes == 0 ||
+	    !stream ||
+	    !pipes) {
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#if !defined(ISP2401)
+	/* We don't support metadata for JPEG stream, since they both use str2mem */
+	if (stream_config->input_config.format == ATOMISP_INPUT_FORMAT_BINARY_8 &&
+	    stream_config->metadata_config.resolution.height > 0) {
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif
+
+#ifdef ISP2401
+	if (stream_config->online && stream_config->pack_raw_pixels) {
+		IA_CSS_LOG("online and pack raw is invalid on input system 2401");
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif
+
+	ia_css_debug_pipe_graph_dump_stream_config(stream_config);
+
+	/* check if mipi size specified */
+	if (stream_config->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR)
+#ifdef ISP2401
+		if (!stream_config->online)
+#endif
+		{
+			unsigned int port = (unsigned int)stream_config->source.port.port;
+
+			if (port >= N_MIPI_PORT_ID) {
+				err = -EINVAL;
+				IA_CSS_LEAVE_ERR(err);
+				return err;
+			}
+
+			if (my_css.size_mem_words != 0) {
+				my_css.mipi_frame_size[port] = my_css.size_mem_words;
+			} else if (stream_config->mipi_buffer_config.size_mem_words != 0) {
+				my_css.mipi_frame_size[port] = stream_config->mipi_buffer_config.size_mem_words;
+			} else {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_stream_create() exit: error, need to set mipi frame size.\n");
+				assert(stream_config->mipi_buffer_config.size_mem_words != 0);
+				err = -EINVAL;
+				IA_CSS_LEAVE_ERR(err);
+				return err;
+			}
+
+			if (my_css.size_mem_words != 0) {
+				my_css.num_mipi_frames[port] =
+				    2; /* Temp change: Default for backwards compatibility. */
+			} else if (stream_config->mipi_buffer_config.nof_mipi_buffers != 0) {
+				my_css.num_mipi_frames[port] =
+				    stream_config->mipi_buffer_config.nof_mipi_buffers;
+			} else {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+						    "ia_css_stream_create() exit: error, need to set number of mipi frames.\n");
+				assert(stream_config->mipi_buffer_config.nof_mipi_buffers != 0);
+				err = -EINVAL;
+				IA_CSS_LEAVE_ERR(err);
+				return err;
+			}
+		}
+
+	/* Currently we only supported metadata up to a certain size. */
+	err = metadata_info_init(&stream_config->metadata_config, &md_info);
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	/* allocate the stream instance */
+	curr_stream = kzalloc(sizeof(struct ia_css_stream), GFP_KERNEL);
+	if (!curr_stream) {
+		err = -ENOMEM;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	/* default all to 0 */
+	curr_stream->info.metadata_info = md_info;
+
+	/* allocate pipes */
+	curr_stream->num_pipes = num_pipes;
+	curr_stream->pipes = kcalloc(num_pipes, sizeof(struct ia_css_pipe *), GFP_KERNEL);
+	if (!curr_stream->pipes) {
+		curr_stream->num_pipes = 0;
+		kfree(curr_stream);
+		curr_stream = NULL;
+		err = -ENOMEM;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	/* store pipes */
+	spcopyonly = (num_pipes == 1) && (pipes[0]->config.mode == IA_CSS_PIPE_MODE_COPY);
+	for (i = 0; i < num_pipes; i++)
+		curr_stream->pipes[i] = pipes[i];
+	curr_stream->last_pipe = curr_stream->pipes[0];
+	/* take over stream config */
+	curr_stream->config = *stream_config;
+
+#if defined(ISP2401)
+	if (stream_config->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR &&
+	    stream_config->online)
+		curr_stream->config.online = false;
+#endif
+
+#ifdef ISP2401
+	if (curr_stream->config.online) {
+		curr_stream->config.source.port.num_lanes =
+		    stream_config->source.port.num_lanes;
+		curr_stream->config.mode =  IA_CSS_INPUT_MODE_BUFFERED_SENSOR;
+	}
+#endif
+	/* in case driver doesn't configure init number of raw buffers, configure it here */
+	if (curr_stream->config.target_num_cont_raw_buf == 0)
+		curr_stream->config.target_num_cont_raw_buf = NUM_CONTINUOUS_FRAMES;
+	if (curr_stream->config.init_num_cont_raw_buf == 0)
+		curr_stream->config.init_num_cont_raw_buf = curr_stream->config.target_num_cont_raw_buf;
+
+	/* Enable locking & unlocking of buffers in RAW buffer pool */
+	if (curr_stream->config.ia_css_enable_raw_buffer_locking)
+		sh_css_sp_configure_enable_raw_pool_locking(
+		    curr_stream->config.lock_all);
+
+	/* copy mode specific stuff */
+	switch (curr_stream->config.mode) {
+	case IA_CSS_INPUT_MODE_SENSOR:
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR:
+#if !defined(ISP2401)
+		ia_css_stream_configure_rx(curr_stream);
+#endif
+		break;
+	case IA_CSS_INPUT_MODE_TPG:
+#if !defined(ISP2401)
+		IA_CSS_LOG("tpg_configuration: x_mask=%d, y_mask=%d, x_delta=%d, y_delta=%d, xy_mask=%d",
+			   curr_stream->config.source.tpg.x_mask,
+			   curr_stream->config.source.tpg.y_mask,
+			   curr_stream->config.source.tpg.x_delta,
+			   curr_stream->config.source.tpg.y_delta,
+			   curr_stream->config.source.tpg.xy_mask);
+
+		sh_css_sp_configure_tpg(
+		    curr_stream->config.source.tpg.x_mask,
+		    curr_stream->config.source.tpg.y_mask,
+		    curr_stream->config.source.tpg.x_delta,
+		    curr_stream->config.source.tpg.y_delta,
+		    curr_stream->config.source.tpg.xy_mask);
+#endif
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+#if !defined(ISP2401)
+		IA_CSS_LOG("mode prbs");
+		sh_css_sp_configure_prbs(curr_stream->config.source.prbs.seed);
+#endif
+		break;
+	case IA_CSS_INPUT_MODE_MEMORY:
+		IA_CSS_LOG("mode memory");
+		curr_stream->reconfigure_css_rx = false;
+		break;
+	default:
+		IA_CSS_LOG("mode sensor/default");
+	}
+
+	for (i = 0; i < num_pipes; i++) {
+		struct ia_css_resolution effective_res;
+
+		curr_pipe = pipes[i];
+		/* set current stream */
+		curr_pipe->stream = curr_stream;
+		/* take over effective info */
+
+		effective_res = curr_pipe->config.input_effective_res;
+		if (effective_res.height == 0 || effective_res.width == 0) {
+			effective_res = curr_pipe->stream->config.input_config.effective_res;
+
+			curr_pipe->config.input_effective_res = effective_res;
+		}
+		IA_CSS_LOG("effective_res=%dx%d",
+			   effective_res.width,
+			   effective_res.height);
+	}
+
+	err = ia_css_stream_isp_parameters_init(curr_stream);
+	if (err)
+		goto ERR;
+	IA_CSS_LOG("isp_params_configs: %p", curr_stream->isp_params_configs);
+
+	if (num_pipes == 1 && pipes[0]->config.mode == IA_CSS_PIPE_MODE_ACC) {
+		*stream = curr_stream;
+		err = ia_css_acc_stream_create(curr_stream);
+		goto ERR;
+	}
+	/* sensor binning */
+	if (!spcopyonly) {
+		sensor_binning_changed =
+		    sh_css_params_set_binning_factor(curr_stream,
+						     curr_stream->config.sensor_binning_factor);
+	} else {
+		sensor_binning_changed = false;
+	}
+
+	IA_CSS_LOG("sensor_binning=%d, changed=%d",
+		   curr_stream->config.sensor_binning_factor, sensor_binning_changed);
+	/* loop over pipes */
+	IA_CSS_LOG("num_pipes=%d", num_pipes);
+	curr_stream->cont_capt = false;
+	/* Temporary hack: we give the preview pipe a reference to the capture
+	    * pipe in continuous capture mode. */
+	if (curr_stream->config.continuous) {
+		/* Search for the preview pipe and create the copy pipe */
+		struct ia_css_pipe *preview_pipe;
+		struct ia_css_pipe *video_pipe;
+		struct ia_css_pipe *acc_pipe;
+		struct ia_css_pipe *capture_pipe = NULL;
+		struct ia_css_pipe *copy_pipe = NULL;
+
+		if (num_pipes >= 2) {
+			curr_stream->cont_capt = true;
+			curr_stream->disable_cont_vf = curr_stream->config.disable_cont_viewfinder;
+			curr_stream->stop_copy_preview = my_css.stop_copy_preview;
+		}
+
+		/* Create copy pipe here, since it may not be exposed to the driver */
+		preview_pipe = find_pipe(pipes, num_pipes,
+					 IA_CSS_PIPE_MODE_PREVIEW, false);
+		video_pipe = find_pipe(pipes, num_pipes,
+				       IA_CSS_PIPE_MODE_VIDEO, false);
+		acc_pipe = find_pipe(pipes, num_pipes, IA_CSS_PIPE_MODE_ACC,
+				     false);
+		if (acc_pipe && num_pipes == 2 && curr_stream->cont_capt)
+			curr_stream->cont_capt =
+			    false; /* preview + QoS case will not need cont_capt switch */
+		if (curr_stream->cont_capt) {
+			capture_pipe = find_pipe(pipes, num_pipes,
+						 IA_CSS_PIPE_MODE_CAPTURE,
+						 false);
+			if (!capture_pipe) {
+				err = -EINVAL;
+				goto ERR;
+			}
+		}
+		/* We do not support preview and video pipe at the same time */
+		if (preview_pipe && video_pipe) {
+			err = -EINVAL;
+			goto ERR;
+		}
+
+		if (preview_pipe && !preview_pipe->pipe_settings.preview.copy_pipe) {
+			err = create_pipe(IA_CSS_PIPE_MODE_CAPTURE, &copy_pipe, true);
+			if (err)
+				goto ERR;
+			ia_css_pipe_config_defaults(&copy_pipe->config);
+			preview_pipe->pipe_settings.preview.copy_pipe = copy_pipe;
+			copy_pipe->stream = curr_stream;
+		}
+		if (preview_pipe && curr_stream->cont_capt)
+			preview_pipe->pipe_settings.preview.capture_pipe = capture_pipe;
+
+		if (video_pipe && !video_pipe->pipe_settings.video.copy_pipe) {
+			err = create_pipe(IA_CSS_PIPE_MODE_CAPTURE, &copy_pipe, true);
+			if (err)
+				goto ERR;
+			ia_css_pipe_config_defaults(&copy_pipe->config);
+			video_pipe->pipe_settings.video.copy_pipe = copy_pipe;
+			copy_pipe->stream = curr_stream;
+		}
+		if (video_pipe && curr_stream->cont_capt)
+			video_pipe->pipe_settings.video.capture_pipe = capture_pipe;
+
+		if (preview_pipe && acc_pipe)
+			preview_pipe->pipe_settings.preview.acc_pipe = acc_pipe;
+	}
+	for (i = 0; i < num_pipes; i++) {
+		curr_pipe = pipes[i];
+		/* set current stream */
+		curr_pipe->stream = curr_stream;
+
+		/* take over effective info */
+
+		effective_res = curr_pipe->config.input_effective_res;
+		err = ia_css_util_check_res(
+			effective_res.width,
+			effective_res.height);
+		if (err)
+			goto ERR;
+
+		/* sensor binning per pipe */
+		if (sensor_binning_changed)
+			sh_css_pipe_free_shading_table(curr_pipe);
+	}
+
+	/* now pipes have been configured, info should be available */
+	for (i = 0; i < num_pipes; i++) {
+		struct ia_css_pipe_info *pipe_info = NULL;
+
+		curr_pipe = pipes[i];
+
+		err = sh_css_pipe_load_binaries(curr_pipe);
+		if (err)
+			goto ERR;
+
+		/* handle each pipe */
+		pipe_info = &curr_pipe->info;
+		for (j = 0; j < IA_CSS_PIPE_MAX_OUTPUT_STAGE; j++) {
+			err = sh_css_pipe_get_output_frame_info(curr_pipe,
+								&pipe_info->output_info[j], j);
+			if (err)
+				goto ERR;
+		}
+
+		if (!spcopyonly) {
+			if (!IS_ISP2401)
+				err = sh_css_pipe_get_shading_info(curr_pipe,
+								   &pipe_info->shading_info,
+								   NULL);
+			else
+				err = sh_css_pipe_get_shading_info(curr_pipe,
+								   &pipe_info->shading_info,
+								   &curr_pipe->config);
+
+			if (err)
+				goto ERR;
+			err = sh_css_pipe_get_grid_info(curr_pipe,
+							&pipe_info->grid_info);
+			if (err)
+				goto ERR;
+			for (j = 0; j < IA_CSS_PIPE_MAX_OUTPUT_STAGE; j++) {
+				sh_css_pipe_get_viewfinder_frame_info(curr_pipe,
+								      &pipe_info->vf_output_info[j],
+								      j);
+				if (err)
+					goto ERR;
+			}
+		}
+
+		my_css.active_pipes[ia_css_pipe_get_pipe_num(curr_pipe)] = curr_pipe;
+	}
+
+	curr_stream->started = false;
+
+	/* Map SP threads before doing anything. */
+	err = map_sp_threads(curr_stream, true);
+	if (err) {
+		IA_CSS_LOG("map_sp_threads: return_err=%d", err);
+		goto ERR;
+	}
+
+	for (i = 0; i < num_pipes; i++) {
+		curr_pipe = pipes[i];
+		ia_css_pipe_map_queue(curr_pipe, true);
+	}
+
+	/* Create host side pipeline objects without stages */
+	err = create_host_pipeline_structure(curr_stream);
+	if (err) {
+		IA_CSS_LOG("create_host_pipeline_structure: return_err=%d", err);
+		goto ERR;
+	}
+
+	/* assign curr_stream */
+	*stream = curr_stream;
+
+ERR:
+	if (!err) {
+		/* working mode: enter into the seed list */
+		if (my_css_save.mode == sh_css_mode_working) {
+			for (i = 0; i < MAX_ACTIVE_STREAMS; i++) {
+				if (!my_css_save.stream_seeds[i].stream) {
+					IA_CSS_LOG("entered stream into loc=%d", i);
+					my_css_save.stream_seeds[i].orig_stream = stream;
+					my_css_save.stream_seeds[i].stream = curr_stream;
+					my_css_save.stream_seeds[i].num_pipes = num_pipes;
+					my_css_save.stream_seeds[i].stream_config = *stream_config;
+					for (j = 0; j < num_pipes; j++) {
+						my_css_save.stream_seeds[i].pipe_config[j] = pipes[j]->config;
+						my_css_save.stream_seeds[i].pipes[j] = pipes[j];
+						my_css_save.stream_seeds[i].orig_pipes[j] = &pipes[j];
+					}
+					break;
+				}
+			}
+		} else {
+			ia_css_stream_destroy(curr_stream);
+		}
+	} else {
+		ia_css_stream_destroy(curr_stream);
+	}
+	IA_CSS_LEAVE("return_err=%d mode=%d", err, my_css_save.mode);
+	return err;
+}
+
+int
+ia_css_stream_destroy(struct ia_css_stream *stream)
+{
+	int i;
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+	if (!stream) {
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	ia_css_stream_isp_parameters_uninit(stream);
+
+	if ((stream->last_pipe) &&
+	    ia_css_pipeline_is_mapped(stream->last_pipe->pipe_num)) {
+#if defined(ISP2401)
+		for (i = 0; i < stream->num_pipes; i++) {
+			struct ia_css_pipe *entry = stream->pipes[i];
+			unsigned int sp_thread_id;
+			struct sh_css_sp_pipeline_terminal *sp_pipeline_input_terminal;
+
+			assert(entry);
+			if (entry) {
+				/* get the SP thread id */
+				if (!ia_css_pipeline_get_sp_thread_id(
+					ia_css_pipe_get_pipe_num(entry), &sp_thread_id))
+					return -EINVAL;
+				/* get the target input terminal */
+				sp_pipeline_input_terminal =
+				&sh_css_sp_group.pipe_io[sp_thread_id].input;
+
+				for (i = 0; i < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH; i++) {
+					ia_css_isys_stream_h isys_stream =
+					&sp_pipeline_input_terminal->context.virtual_input_system_stream[i];
+					if (stream->config.isys_config[i].valid && isys_stream->valid)
+						ia_css_isys_stream_destroy(isys_stream);
+				}
+			}
+		}
+		if (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+			for (i = 0; i < stream->num_pipes; i++) {
+				struct ia_css_pipe *entry = stream->pipes[i];
+				/*
+				 * free any mipi frames that are remaining:
+				 * some test stream create-destroy cycles do
+				 * not generate output frames
+				 * and the mipi buffer is not freed in the
+				 * deque function
+				 */
+				if (entry)
+					free_mipi_frames(entry);
+			}
+		}
+		stream_unregister_with_csi_rx(stream);
+#endif
+
+		for (i = 0; i < stream->num_pipes; i++) {
+			struct ia_css_pipe *curr_pipe = stream->pipes[i];
+
+			assert(curr_pipe);
+			ia_css_pipe_map_queue(curr_pipe, false);
+		}
+
+		err = map_sp_threads(stream, false);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	/* remove references from pipes to stream */
+	for (i = 0; i < stream->num_pipes; i++) {
+		struct ia_css_pipe *entry = stream->pipes[i];
+
+		assert(entry);
+		if (entry) {
+			/* clear reference to stream */
+			entry->stream = NULL;
+			/* check internal copy pipe */
+			if (entry->mode == IA_CSS_PIPE_ID_PREVIEW &&
+			    entry->pipe_settings.preview.copy_pipe) {
+				IA_CSS_LOG("clearing stream on internal preview copy pipe");
+				entry->pipe_settings.preview.copy_pipe->stream = NULL;
+			}
+			if (entry->mode == IA_CSS_PIPE_ID_VIDEO &&
+			    entry->pipe_settings.video.copy_pipe) {
+				IA_CSS_LOG("clearing stream on internal video copy pipe");
+				entry->pipe_settings.video.copy_pipe->stream = NULL;
+			}
+			err = sh_css_pipe_unload_binaries(entry);
+		}
+	}
+	/* free associated memory of stream struct */
+	kfree(stream->pipes);
+	stream->pipes = NULL;
+	stream->num_pipes = 0;
+
+	/* working mode: take out of the seed list */
+	if (my_css_save.mode == sh_css_mode_working) {
+		for (i = 0; i < MAX_ACTIVE_STREAMS; i++) {
+			if (my_css_save.stream_seeds[i].stream == stream) {
+				IA_CSS_LOG("took out stream %d", i);
+				my_css_save.stream_seeds[i].stream = NULL;
+				break;
+			}
+		}
+	}
+
+	kfree(stream);
+	IA_CSS_LEAVE_ERR(err);
+
+	return err;
+}
+
+int
+ia_css_stream_get_info(const struct ia_css_stream *stream,
+		       struct ia_css_stream_info *stream_info)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_get_info: enter/exit\n");
+	assert(stream);
+	assert(stream_info);
+
+	*stream_info = stream->info;
+	return 0;
+}
+
+int
+ia_css_stream_start(struct ia_css_stream *stream)
+{
+	int err = 0;
+
+	IA_CSS_ENTER("stream = %p", stream);
+	if ((!stream) || (!stream->last_pipe)) {
+		IA_CSS_LEAVE_ERR(-EINVAL);
+		return -EINVAL;
+	}
+	IA_CSS_LOG("starting %d", stream->last_pipe->mode);
+
+	sh_css_sp_set_disable_continuous_viewfinder(stream->disable_cont_vf);
+
+	/* Create host side pipeline. */
+	err = create_host_pipeline(stream);
+	if (err) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#if defined(ISP2401)
+	if ((stream->config.mode == IA_CSS_INPUT_MODE_SENSOR) ||
+	    (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR))
+		stream_register_with_csi_rx(stream);
+#endif
+
+#if !defined(ISP2401)
+	/* Initialize mipi size checks */
+	if (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		unsigned int idx;
+		unsigned int port = (unsigned int)(stream->config.source.port.port);
+
+		for (idx = 0; idx < IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT; idx++) {
+			sh_css_sp_group.config.mipi_sizes_for_check[port][idx] =
+			sh_css_get_mipi_sizes_for_check(port, idx);
+		}
+	}
+#endif
+
+	if (stream->config.mode != IA_CSS_INPUT_MODE_MEMORY) {
+		err = sh_css_config_input_network(stream);
+		if (err)
+			return err;
+	}
+
+	err = sh_css_pipe_start(stream);
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+int
+ia_css_stream_stop(struct ia_css_stream *stream)
+{
+	int err = 0;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_stop() enter/exit\n");
+	assert(stream);
+	assert(stream->last_pipe);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_stop: stopping %d\n",
+			    stream->last_pipe->mode);
+
+#if !defined(ISP2401)
+	/* De-initialize mipi size checks */
+	if (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		unsigned int idx;
+		unsigned int port = (unsigned int)(stream->config.source.port.port);
+
+		for (idx = 0; idx < IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT; idx++)
+			sh_css_sp_group.config.mipi_sizes_for_check[port][idx] = 0;
+	}
+#endif
+
+	err = ia_css_pipeline_request_stop(&stream->last_pipe->pipeline);
+	if (err)
+		return err;
+
+	/*
+	 * Ideally, unmapping should happen after pipeline_stop, but current
+	 * semantics do not allow that.
+	 */
+	/* err = map_sp_threads(stream, false); */
+
+	return err;
+}
+
+bool
+ia_css_stream_has_stopped(struct ia_css_stream *stream)
+{
+	bool stopped;
+
+	assert(stream);
+
+	stopped = ia_css_pipeline_has_stopped(&stream->last_pipe->pipeline);
+
+	return stopped;
+}
+
+/* ISP2400 */
+/*
+ * Destroy the stream and all the pipes related to it.
+ * The stream handle is used to identify the correct entry in the css_save struct
+ */
+int
+ia_css_stream_unload(struct ia_css_stream *stream)
+{
+	int i;
+
+	assert(stream);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"ia_css_stream_unload() enter,\n");
+	/* some checks */
+	assert(stream);
+	for (i = 0; i < MAX_ACTIVE_STREAMS; i++)
+		if (my_css_save.stream_seeds[i].stream == stream) {
+			int j;
+
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_stream_unload(): unloading %d (%p)\n", i,
+					    my_css_save.stream_seeds[i].stream);
+			ia_css_stream_destroy(stream);
+			for (j = 0; j < my_css_save.stream_seeds[i].num_pipes; j++)
+				ia_css_pipe_destroy(my_css_save.stream_seeds[i].pipes[j]);
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_stream_unload(): after unloading %d (%p)\n", i,
+					    my_css_save.stream_seeds[i].stream);
+			break;
+		}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"ia_css_stream_unload() exit,\n");
+	return 0;
+}
+
+int
+ia_css_temp_pipe_to_pipe_id(const struct ia_css_pipe *pipe,
+			    enum ia_css_pipe_id *pipe_id)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_temp_pipe_to_pipe_id() enter/exit\n");
+	if (pipe)
+		*pipe_id = pipe->mode;
+	else
+		*pipe_id = IA_CSS_PIPE_ID_COPY;
+
+	return 0;
+}
+
+enum atomisp_input_format
+ia_css_stream_get_format(const struct ia_css_stream *stream)
+{
+	return stream->config.input_config.format;
+}
+
+bool
+ia_css_stream_get_two_pixels_per_clock(const struct ia_css_stream *stream)
+{
+	return (stream->config.pixels_per_clock == 2);
+}
+
+struct ia_css_binary *
+ia_css_stream_get_shading_correction_binary(const struct ia_css_stream
+	*stream)
+{
+	struct ia_css_pipe *pipe;
+
+	assert(stream);
+
+	pipe = stream->pipes[0];
+
+	if (stream->num_pipes == 2) {
+		assert(stream->pipes[1]);
+		if (stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_VIDEO ||
+		    stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_PREVIEW)
+			pipe = stream->pipes[1];
+	}
+
+	return ia_css_pipe_get_shading_correction_binary(pipe);
+}
+
+struct ia_css_binary *
+ia_css_stream_get_dvs_binary(const struct ia_css_stream *stream)
+{
+	int i;
+	struct ia_css_pipe *video_pipe = NULL;
+
+	/* First we find the video pipe */
+	for (i = 0; i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+
+		if (pipe->config.mode == IA_CSS_PIPE_MODE_VIDEO) {
+			video_pipe = pipe;
+			break;
+		}
+	}
+	if (video_pipe)
+		return &video_pipe->pipe_settings.video.video_binary;
+	return NULL;
+}
+
+struct ia_css_binary *
+ia_css_stream_get_3a_binary(const struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *pipe;
+	struct ia_css_binary *s3a_binary = NULL;
+
+	assert(stream);
+
+	pipe = stream->pipes[0];
+
+	if (stream->num_pipes == 2) {
+		assert(stream->pipes[1]);
+		if (stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_VIDEO ||
+		    stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_PREVIEW)
+			pipe = stream->pipes[1];
+	}
+
+	s3a_binary = ia_css_pipe_get_s3a_binary(pipe);
+
+	return s3a_binary;
+}
+
+int
+ia_css_stream_set_output_padded_width(struct ia_css_stream *stream,
+				      unsigned int output_padded_width)
+{
+	struct ia_css_pipe *pipe;
+
+	assert(stream);
+
+	pipe = stream->last_pipe;
+
+	assert(pipe);
+
+	/* set the config also just in case (redundant info? why do we save config in pipe?) */
+	pipe->config.output_info[IA_CSS_PIPE_OUTPUT_STAGE_0].padded_width = output_padded_width;
+	pipe->output_info[IA_CSS_PIPE_OUTPUT_STAGE_0].padded_width = output_padded_width;
+
+	return 0;
+}
+
+static struct ia_css_binary *
+ia_css_pipe_get_shading_correction_binary(const struct ia_css_pipe *pipe)
+{
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe);
+
+	switch (pipe->config.mode) {
+	case IA_CSS_PIPE_MODE_PREVIEW:
+		binary = (struct ia_css_binary *)&pipe->pipe_settings.preview.preview_binary;
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		binary = (struct ia_css_binary *)&pipe->pipe_settings.video.video_binary;
+		break;
+	case IA_CSS_PIPE_MODE_CAPTURE:
+		if (pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_PRIMARY) {
+			unsigned int i;
+
+			for (i = 0; i < pipe->pipe_settings.capture.num_primary_stage; i++) {
+				if (pipe->pipe_settings.capture.primary_binary[i].info->sp.enable.sc) {
+					binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.primary_binary[i];
+					break;
+				}
+			}
+		} else if (pipe->config.default_capture_config.mode ==
+			    IA_CSS_CAPTURE_MODE_BAYER)
+			binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.pre_isp_binary;
+		else if (pipe->config.default_capture_config.mode ==
+			    IA_CSS_CAPTURE_MODE_ADVANCED ||
+			    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_LOW_LIGHT) {
+			if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_1)
+				binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.pre_isp_binary;
+			else if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_2_2)
+				binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.post_isp_binary;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (binary && binary->info->sp.enable.sc)
+		return binary;
+
+	return NULL;
+}
+
+static struct ia_css_binary *
+ia_css_pipe_get_s3a_binary(const struct ia_css_pipe *pipe)
+{
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe);
+
+	switch (pipe->config.mode) {
+	case IA_CSS_PIPE_MODE_PREVIEW:
+		binary = (struct ia_css_binary *)&pipe->pipe_settings.preview.preview_binary;
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		binary = (struct ia_css_binary *)&pipe->pipe_settings.video.video_binary;
+		break;
+	case IA_CSS_PIPE_MODE_CAPTURE:
+		if (pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_PRIMARY) {
+			unsigned int i;
+
+			for (i = 0; i < pipe->pipe_settings.capture.num_primary_stage; i++) {
+				if (pipe->pipe_settings.capture.primary_binary[i].info->sp.enable.s3a) {
+					binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.primary_binary[i];
+					break;
+				}
+			}
+		} else if (pipe->config.default_capture_config.mode ==
+			    IA_CSS_CAPTURE_MODE_BAYER) {
+			binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.pre_isp_binary;
+		} else if (pipe->config.default_capture_config.mode ==
+			    IA_CSS_CAPTURE_MODE_ADVANCED ||
+			    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_LOW_LIGHT) {
+			if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_1)
+				binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.pre_isp_binary;
+			else if (pipe->config.isp_pipe_version == IA_CSS_PIPE_VERSION_2_2)
+				binary = (struct ia_css_binary *)&pipe->pipe_settings.capture.post_isp_binary;
+			else
+				assert(0);
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (binary && !binary->info->sp.enable.s3a)
+		binary = NULL;
+
+	return binary;
+}
+
+static struct ia_css_binary *
+ia_css_pipe_get_sdis_binary(const struct ia_css_pipe *pipe)
+{
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe);
+
+	switch (pipe->config.mode) {
+	case IA_CSS_PIPE_MODE_VIDEO:
+		binary = (struct ia_css_binary *)&pipe->pipe_settings.video.video_binary;
+		break;
+	default:
+		break;
+	}
+
+	if (binary && !binary->info->sp.enable.dis)
+		binary = NULL;
+
+	return binary;
+}
+
+struct ia_css_pipeline *
+ia_css_pipe_get_pipeline(const struct ia_css_pipe *pipe)
+{
+	assert(pipe);
+
+	return (struct ia_css_pipeline *)&pipe->pipeline;
+}
+
+unsigned int
+ia_css_pipe_get_pipe_num(const struct ia_css_pipe *pipe)
+{
+	assert(pipe);
+
+	/*
+	 * KW was not sure this function was not returning a value
+	 * that was out of range; so added an assert, and, for the
+	 * case when asserts are not enabled, clip to the largest
+	 * value; pipe_num is unsigned so the value cannot be too small
+	 */
+	assert(pipe->pipe_num < IA_CSS_PIPELINE_NUM_MAX);
+
+	if (pipe->pipe_num >= IA_CSS_PIPELINE_NUM_MAX)
+		return (IA_CSS_PIPELINE_NUM_MAX - 1);
+
+	return pipe->pipe_num;
+}
+
+unsigned int
+ia_css_pipe_get_isp_pipe_version(const struct ia_css_pipe *pipe)
+{
+	assert(pipe);
+
+	return (unsigned int)pipe->config.isp_pipe_version;
+}
+
+#define SP_START_TIMEOUT_US 30000000
+
+int
+ia_css_start_sp(void)
+{
+	unsigned long timeout;
+	int err = 0;
+
+	IA_CSS_ENTER("");
+	sh_css_sp_start_isp();
+
+	/* waiting for the SP is completely started */
+	timeout = SP_START_TIMEOUT_US;
+	while ((ia_css_spctrl_get_state(SP0_ID) != IA_CSS_SP_SW_INITIALIZED) && timeout) {
+		timeout--;
+		udelay(1);
+	}
+	if (timeout == 0) {
+		IA_CSS_ERROR("timeout during SP initialization");
+		return -EINVAL;
+	}
+
+	/* Workaround, in order to run two streams in parallel. See TASK 4271*/
+	/* TODO: Fix this. */
+
+	sh_css_init_host_sp_control_vars();
+
+	/* buffers should be initialized only when sp is started */
+	/* AM: At the moment it will be done only when there is no stream active. */
+
+	sh_css_setup_queues();
+	ia_css_bufq_dump_queue_info();
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+/*
+ * Time to wait SP for termincate. Only condition when this can happen
+ * is a fatal hw failure, but we must be able to detect this and emit
+ * a proper error trace.
+ */
+#define SP_SHUTDOWN_TIMEOUT_US 200000
+
+int
+ia_css_stop_sp(void)
+{
+	unsigned long timeout;
+	int err = 0;
+
+	IA_CSS_ENTER("void");
+
+	if (!sh_css_sp_is_running()) {
+		err = -EINVAL;
+		IA_CSS_LEAVE("SP already stopped : return_err=%d", err);
+
+		/* Return an error - stop SP should not have been called by driver */
+		return err;
+	}
+
+	/* For now, stop whole SP */
+	if (!sh_css_write_host2sp_command(host2sp_cmd_terminate)) {
+		IA_CSS_ERROR("Call to 'sh-css_write_host2sp_command()' failed");
+		ia_css_debug_dump_sp_sw_debug_info();
+		ia_css_debug_dump_debug_info(NULL);
+	}
+
+	sh_css_sp_set_sp_running(false);
+
+	timeout = SP_SHUTDOWN_TIMEOUT_US;
+	while (!ia_css_spctrl_is_idle(SP0_ID) && timeout) {
+		timeout--;
+		udelay(1);
+	}
+	if (ia_css_spctrl_get_state(SP0_ID) != IA_CSS_SP_SW_TERMINATED)
+		IA_CSS_WARNING("SP has not terminated (SW)");
+
+	if (timeout == 0) {
+		IA_CSS_WARNING("SP is not idle");
+		ia_css_debug_dump_sp_sw_debug_info();
+	}
+	timeout = SP_SHUTDOWN_TIMEOUT_US;
+	while (!isp_ctrl_getbit(ISP0_ID, ISP_SC_REG, ISP_IDLE_BIT) && timeout) {
+		timeout--;
+		udelay(1);
+	}
+	if (timeout == 0) {
+		IA_CSS_WARNING("ISP is not idle");
+		ia_css_debug_dump_sp_sw_debug_info();
+	}
+
+	sh_css_hmm_buffer_record_uninit();
+
+	/* clear pending param sets from refcount */
+	sh_css_param_clear_param_sets();
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+int
+ia_css_update_continuous_frames(struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *pipe;
+	unsigned int i;
+
+	ia_css_debug_dtrace(
+	    IA_CSS_DEBUG_TRACE,
+	    "sh_css_update_continuous_frames() enter:\n");
+
+	if (!stream) {
+		ia_css_debug_dtrace(
+		    IA_CSS_DEBUG_TRACE,
+		    "sh_css_update_continuous_frames() leave: invalid stream, return_void\n");
+		return -EINVAL;
+	}
+
+	pipe = stream->continuous_pipe;
+
+	for (i = stream->config.init_num_cont_raw_buf;
+		i < stream->config.target_num_cont_raw_buf; i++)
+		sh_css_update_host2sp_offline_frame(i,
+						    pipe->continuous_frames[i], pipe->cont_md_buffers[i]);
+
+	sh_css_update_host2sp_cont_num_raw_frames
+	(stream->config.target_num_cont_raw_buf, true);
+	ia_css_debug_dtrace(
+	    IA_CSS_DEBUG_TRACE,
+	    "sh_css_update_continuous_frames() leave: return_void\n");
+
+	return 0;
+}
+
+void ia_css_pipe_map_queue(struct ia_css_pipe *pipe, bool map)
+{
+	unsigned int thread_id;
+	unsigned int pipe_num;
+	bool need_input_queue;
+
+	IA_CSS_ENTER("");
+	assert(pipe);
+
+	pipe_num = pipe->pipe_num;
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+
+#if defined(ISP2401)
+	need_input_queue = true;
+#else
+	need_input_queue = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+#endif
+
+	/* map required buffer queues to resources */
+	/* TODO: to be improved */
+	if (pipe->mode == IA_CSS_PIPE_ID_PREVIEW) {
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+		if (pipe->pipe_settings.preview.preview_binary.info &&
+		    pipe->pipe_settings.preview.preview_binary.info->sp.enable.s3a)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_3A_STATISTICS, map);
+	} else if (pipe->mode == IA_CSS_PIPE_ID_CAPTURE) {
+		unsigned int i;
+
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+		if (pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_PRIMARY) {
+			for (i = 0; i < pipe->pipe_settings.capture.num_primary_stage; i++) {
+				if (pipe->pipe_settings.capture.primary_binary[i].info &&
+				    pipe->pipe_settings.capture.primary_binary[i].info->sp.enable.s3a) {
+					ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_3A_STATISTICS, map);
+					break;
+				}
+			}
+		} else if (pipe->config.default_capture_config.mode ==
+			    IA_CSS_CAPTURE_MODE_ADVANCED ||
+			    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_LOW_LIGHT ||
+			    pipe->config.default_capture_config.mode == IA_CSS_CAPTURE_MODE_BAYER) {
+			if (pipe->pipe_settings.capture.pre_isp_binary.info &&
+			    pipe->pipe_settings.capture.pre_isp_binary.info->sp.enable.s3a)
+				ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_3A_STATISTICS, map);
+		}
+	} else if (pipe->mode == IA_CSS_PIPE_ID_VIDEO) {
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, map);
+		if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0])
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+		if (pipe->pipe_settings.video.video_binary.info &&
+		    pipe->pipe_settings.video.video_binary.info->sp.enable.s3a)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_3A_STATISTICS, map);
+		if (pipe->pipe_settings.video.video_binary.info &&
+		    (pipe->pipe_settings.video.video_binary.info->sp.enable.dis
+		    ))
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_DIS_STATISTICS, map);
+	} else if (pipe->mode == IA_CSS_PIPE_ID_COPY) {
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		if (!pipe->stream->config.continuous)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+	} else if (pipe->mode == IA_CSS_PIPE_ID_ACC) {
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+	} else if (pipe->mode == IA_CSS_PIPE_ID_YUVPP) {
+		unsigned int idx;
+
+		for (idx = 0; idx < IA_CSS_PIPE_MAX_OUTPUT_STAGE; idx++) {
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_OUTPUT_FRAME + idx, map);
+			if (pipe->enable_viewfinder[idx])
+				ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME + idx, map);
+		}
+		if (need_input_queue)
+			ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_INPUT_FRAME, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_PARAMETER_SET, map);
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+	}
+	IA_CSS_LEAVE("");
+}
+
+
+int
+ia_css_unlock_raw_frame(struct ia_css_stream *stream, uint32_t exp_id)
+{
+	int ret;
+
+	IA_CSS_ENTER("");
+
+	/*
+	 * Only continuous streams have a tagger to which we can send the
+	 * unlock message.
+	 */
+	if (!stream || !stream->config.continuous) {
+		IA_CSS_ERROR("invalid stream pointer");
+		return -EINVAL;
+	}
+
+	if (exp_id > IA_CSS_ISYS_MAX_EXPOSURE_ID ||
+	    exp_id < IA_CSS_ISYS_MIN_EXPOSURE_ID) {
+		IA_CSS_ERROR("invalid exposure ID: %d\n", exp_id);
+		return -EINVAL;
+	}
+
+	/*
+	 * Send the event. Since we verified that the exp_id is valid,
+	 * we can safely assign it to an 8-bit argument here.
+	 */
+	ret = ia_css_bufq_enqueue_psys_event(
+	    IA_CSS_PSYS_SW_EVENT_UNLOCK_RAW_BUFFER, exp_id, 0, 0);
+
+	IA_CSS_LEAVE_ERR(ret);
+	return ret;
+}
+
+/*
+ * @brief	Set the state (Enable or Disable) of the Extension stage in the
+ *		given pipe.
+ */
+int
+ia_css_pipe_set_qos_ext_state(struct ia_css_pipe *pipe, uint32_t fw_handle,
+			      bool enable)
+{
+	unsigned int thread_id;
+	struct ia_css_pipeline_stage *stage;
+	int err = 0;
+
+	IA_CSS_ENTER("");
+
+	/* Parameter Check */
+	if (!pipe || !pipe->stream) {
+		IA_CSS_ERROR("Invalid Pipe.");
+		err = -EINVAL;
+	} else if (!(pipe->config.acc_extension)) {
+		IA_CSS_ERROR("Invalid Pipe(No Extension Firmware)");
+		err = -EINVAL;
+	} else if (!sh_css_sp_is_running()) {
+		IA_CSS_ERROR("Leaving: queue unavailable.");
+		err = -EBUSY;
+	} else {
+		/* Query the threadid and stage_num for the Extension firmware*/
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+		err = ia_css_pipeline_get_stage_from_fw(&pipe->pipeline, fw_handle, &stage);
+		if (!err) {
+			/* Set the Extension State;. TODO: Add check for stage firmware.type (QOS)*/
+			err = ia_css_bufq_enqueue_psys_event(
+			    (uint8_t)IA_CSS_PSYS_SW_EVENT_STAGE_ENABLE_DISABLE,
+			    (uint8_t)thread_id,
+			    (uint8_t)stage->stage_num,
+			    enable ? 1 : 0);
+			if (!err) {
+				if (enable)
+					SH_CSS_QOS_STAGE_ENABLE(&sh_css_sp_group.pipe[thread_id], stage->stage_num);
+				else
+					SH_CSS_QOS_STAGE_DISABLE(&sh_css_sp_group.pipe[thread_id], stage->stage_num);
+			}
+		}
+	}
+	IA_CSS_LEAVE("err:%d handle:%u enable:%d", err, fw_handle, enable);
+	return err;
+}
+
+/*
+ * @brief	Get the state (Enable or Disable) of the Extension stage in the
+ * given pipe.
+ */
+int
+ia_css_pipe_get_qos_ext_state(struct ia_css_pipe *pipe, uint32_t fw_handle,
+			      bool *enable)
+{
+	struct ia_css_pipeline_stage *stage;
+	unsigned int thread_id;
+	int err = 0;
+
+	IA_CSS_ENTER("");
+
+	/* Parameter Check */
+	if (!pipe || !pipe->stream) {
+		IA_CSS_ERROR("Invalid Pipe.");
+		err = -EINVAL;
+	} else if (!(pipe->config.acc_extension)) {
+		IA_CSS_ERROR("Invalid Pipe (No Extension Firmware).");
+		err = -EINVAL;
+	} else if (!sh_css_sp_is_running()) {
+		IA_CSS_ERROR("Leaving: queue unavailable.");
+		err = -EBUSY;
+	} else {
+		/* Query the threadid and stage_num corresponding to the Extension firmware*/
+		ia_css_pipeline_get_sp_thread_id(ia_css_pipe_get_pipe_num(pipe), &thread_id);
+		err = ia_css_pipeline_get_stage_from_fw(&pipe->pipeline, fw_handle, &stage);
+
+		if (!err) {
+			/* Get the Extension State */
+			*enable = (SH_CSS_QOS_STAGE_IS_ENABLED(&sh_css_sp_group.pipe[thread_id],
+								stage->stage_num)) ? true : false;
+		}
+	}
+	IA_CSS_LEAVE("err:%d handle:%u enable:%d", err, fw_handle, *enable);
+	return err;
+}
+
+static void
+sh_css_hmm_buffer_record_init(void)
+{
+	int i;
+
+	for (i = 0; i < MAX_HMM_BUFFER_NUM; i++)
+		sh_css_hmm_buffer_record_reset(&hmm_buffer_record[i]);
+}
+
+static void
+sh_css_hmm_buffer_record_uninit(void)
+{
+	int i;
+	struct sh_css_hmm_buffer_record *buffer_record = NULL;
+
+	buffer_record = &hmm_buffer_record[0];
+	for (i = 0; i < MAX_HMM_BUFFER_NUM; i++) {
+		if (buffer_record->in_use) {
+			if (buffer_record->h_vbuf)
+				ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &buffer_record->h_vbuf);
+			sh_css_hmm_buffer_record_reset(buffer_record);
+		}
+		buffer_record++;
+	}
+}
+
+static void
+sh_css_hmm_buffer_record_reset(struct sh_css_hmm_buffer_record *buffer_record)
+{
+	assert(buffer_record);
+	buffer_record->in_use = false;
+	buffer_record->type = IA_CSS_BUFFER_TYPE_INVALID;
+	buffer_record->h_vbuf = NULL;
+	buffer_record->kernel_ptr = 0;
+}
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+				    enum ia_css_buffer_type type,
+				    hrt_address kernel_ptr)
+{
+	int i;
+	struct sh_css_hmm_buffer_record *buffer_record = NULL;
+	struct sh_css_hmm_buffer_record *out_buffer_record = NULL;
+
+	assert(h_vbuf);
+	assert((type > IA_CSS_BUFFER_TYPE_INVALID) &&
+	       (type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE));
+	assert(kernel_ptr != 0);
+
+	buffer_record = &hmm_buffer_record[0];
+	for (i = 0; i < MAX_HMM_BUFFER_NUM; i++) {
+		if (!buffer_record->in_use) {
+			buffer_record->in_use = true;
+			buffer_record->type = type;
+			buffer_record->h_vbuf = h_vbuf;
+			buffer_record->kernel_ptr = kernel_ptr;
+			out_buffer_record = buffer_record;
+			break;
+		}
+		buffer_record++;
+	}
+
+	return out_buffer_record;
+}
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_validate(ia_css_ptr ddr_buffer_addr,
+				    enum ia_css_buffer_type type)
+{
+	int i;
+	struct sh_css_hmm_buffer_record *buffer_record = NULL;
+	bool found_record = false;
+
+	buffer_record = &hmm_buffer_record[0];
+	for (i = 0; i < MAX_HMM_BUFFER_NUM; i++) {
+		if ((buffer_record->in_use) &&
+		    (buffer_record->type == type) &&
+		    (buffer_record->h_vbuf) &&
+		    (buffer_record->h_vbuf->vptr == ddr_buffer_addr)) {
+			found_record = true;
+			break;
+		}
+		buffer_record++;
+	}
+
+	if (found_record)
+		return buffer_record;
+	else
+		return NULL;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_defs.h b/drivers/staging/media/atomisp/pci/sh_css_defs.h
new file mode 100644
index 000000000..7eb10b226
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_defs.h
@@ -0,0 +1,362 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_DEFS_H_
+#define _SH_CSS_DEFS_H_
+
+#include "isp.h"
+
+/*#include "vamem.h"*/ /* Cannot include for VAMEM properties this file is visible on ISP -> pipeline generator */
+
+#include "math_support.h"	/* max(), min, etc etc */
+
+/* ID's for refcount */
+#define IA_CSS_REFCOUNT_PARAM_SET_POOL  0xCAFE0001
+#define IA_CSS_REFCOUNT_PARAM_BUFFER    0xCAFE0002
+
+/* Digital Image Stabilization */
+#define SH_CSS_DIS_DECI_FACTOR_LOG2       6
+
+/* UV offset: 1:uv=-128...127, 0:uv=0...255 */
+#define SH_CSS_UV_OFFSET_IS_0             0
+
+/* Bits of bayer is adjusted as 13 in ISP */
+#define SH_CSS_BAYER_BITS                 13
+
+/* Max value of bayer data (unsigned 13bit in ISP) */
+#define SH_CSS_BAYER_MAXVAL               ((1U << SH_CSS_BAYER_BITS) - 1)
+
+/* Bits of yuv in ISP */
+#define SH_CSS_ISP_YUV_BITS               8
+
+#define SH_CSS_DP_GAIN_SHIFT              5
+#define SH_CSS_BNR_GAIN_SHIFT             13
+#define SH_CSS_YNR_GAIN_SHIFT             13
+#define SH_CSS_AE_YCOEF_SHIFT             13
+#define SH_CSS_AF_FIR_SHIFT               13
+#define SH_CSS_YEE_DETAIL_GAIN_SHIFT      8  /* [u5.8] */
+#define SH_CSS_YEE_SCALE_SHIFT            8
+#define SH_CSS_TNR_COEF_SHIFT             13
+#define SH_CSS_MACC_COEF_SHIFT            11 /* [s2.11] for ISP1 */
+#define SH_CSS_MACC2_COEF_SHIFT           13 /* [s[exp].[13-exp]] for ISP2 */
+#define SH_CSS_DIS_COEF_SHIFT             13
+
+/* enumeration of the bayer downscale factors. When a binary supports multiple
+ * factors, the OR of these defines is used to build the mask of supported
+ * factors. The BDS factor is used in pre-processor expressions so we cannot
+ * use an enum here. */
+#define SH_CSS_BDS_FACTOR_1_00	(0)
+#define SH_CSS_BDS_FACTOR_1_25	(1)
+#define SH_CSS_BDS_FACTOR_1_50	(2)
+#define SH_CSS_BDS_FACTOR_2_00	(3)
+#define SH_CSS_BDS_FACTOR_2_25	(4)
+#define SH_CSS_BDS_FACTOR_2_50	(5)
+#define SH_CSS_BDS_FACTOR_3_00	(6)
+#define SH_CSS_BDS_FACTOR_4_00	(7)
+#define SH_CSS_BDS_FACTOR_4_50	(8)
+#define SH_CSS_BDS_FACTOR_5_00	(9)
+#define SH_CSS_BDS_FACTOR_6_00	(10)
+#define SH_CSS_BDS_FACTOR_8_00	(11)
+#define NUM_BDS_FACTORS		(12)
+
+#define PACK_BDS_FACTOR(factor)	(1 << (factor))
+
+/* Following macros should match with the type enum ia_css_pipe_version in
+ * ia_css_pipe_public.h. The reason to add these macros is that enum type
+ * will be evaluted to 0 in preprocessing time. */
+#define SH_CSS_ISP_PIPE_VERSION_1	1
+#define SH_CSS_ISP_PIPE_VERSION_2_2	2
+#define SH_CSS_ISP_PIPE_VERSION_2_6_1	3
+#define SH_CSS_ISP_PIPE_VERSION_2_7	4
+
+/*--------------- sRGB Gamma -----------------
+CCM        : YCgCo[0,8191] -> RGB[0,4095]
+sRGB Gamma : RGB  [0,4095] -> RGB[0,8191]
+CSC        : RGB  [0,8191] -> YUV[0,8191]
+
+CCM:
+Y[0,8191],CgCo[-4096,4095],coef[-8192,8191] -> RGB[0,4095]
+
+sRGB Gamma:
+RGB[0,4095] -(interpolation step16)-> RGB[0,255] -(LUT 12bit)-> RGB[0,4095] -> RGB[0,8191]
+
+CSC:
+RGB[0,8191],coef[-8192,8191] -> RGB[0,8191]
+--------------------------------------------*/
+/* Bits of input/output of sRGB Gamma */
+#define SH_CSS_RGB_GAMMA_INPUT_BITS       12 /* [0,4095] */
+#define SH_CSS_RGB_GAMMA_OUTPUT_BITS      13 /* [0,8191] */
+
+/* Bits of fractional part of interpolation in vamem, [0,4095]->[0,255] */
+#define SH_CSS_RGB_GAMMA_FRAC_BITS        \
+	(SH_CSS_RGB_GAMMA_INPUT_BITS - SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE_LOG2)
+#define SH_CSS_RGB_GAMMA_ONE              BIT(SH_CSS_RGB_GAMMA_FRAC_BITS)
+
+/* Bits of input of CCM,  = 13, Y[0,8191],CgCo[-4096,4095] */
+#define SH_CSS_YUV2RGB_CCM_INPUT_BITS     SH_CSS_BAYER_BITS
+
+/* Bits of output of CCM,  = 12, RGB[0,4095] */
+#define SH_CSS_YUV2RGB_CCM_OUTPUT_BITS    SH_CSS_RGB_GAMMA_INPUT_BITS
+
+/* Maximum value of output of CCM */
+#define SH_CSS_YUV2RGB_CCM_MAX_OUTPUT     \
+	((1 << SH_CSS_YUV2RGB_CCM_OUTPUT_BITS) - 1)
+
+#define SH_CSS_NUM_INPUT_BUF_LINES        4
+
+/* Left cropping only applicable for sufficiently large nway */
+#define SH_CSS_MAX_LEFT_CROPPING          12
+#define SH_CSS_MAX_TOP_CROPPING           12
+
+#define	SH_CSS_SP_MAX_WIDTH               1280
+
+/* This is the maximum grid we can handle in the ISP binaries.
+ * The host code makes sure no bigger grid is ever selected. */
+#define SH_CSS_MAX_BQ_GRID_WIDTH          80
+#define SH_CSS_MAX_BQ_GRID_HEIGHT         60
+
+/* The minimum dvs envelope is 12x12(for IPU2) to make sure the
+ * invalid rows/columns that result from filter initialization are skipped. */
+#define SH_CSS_MIN_DVS_ENVELOPE           12U
+
+/* The FPGA system (vec_nelems == 16) only supports upto 5MP */
+#define SH_CSS_MAX_SENSOR_WIDTH           4608
+#define SH_CSS_MAX_SENSOR_HEIGHT          3450
+
+/* Limited to reduce vmem pressure */
+#if ISP_VMEM_DEPTH >= 3072
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH  SH_CSS_MAX_SENSOR_WIDTH
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT SH_CSS_MAX_SENSOR_HEIGHT
+#else
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH  3264
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT 2448
+#endif
+/* When using bayer decimation */
+/*
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH_DEC  4224
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT_DEC 3168
+*/
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH_DEC  SH_CSS_MAX_SENSOR_WIDTH
+#define SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT_DEC SH_CSS_MAX_SENSOR_HEIGHT
+
+#define SH_CSS_MIN_SENSOR_WIDTH           2
+#define SH_CSS_MIN_SENSOR_HEIGHT          2
+
+/*
+#define SH_CSS_MAX_VF_WIDTH_DEC               1920
+#define SH_CSS_MAX_VF_HEIGHT_DEC              1080
+*/
+#define SH_CSS_MAX_VF_WIDTH_DEC               SH_CSS_MAX_VF_WIDTH
+#define SH_CSS_MAX_VF_HEIGHT_DEC              SH_CSS_MAX_VF_HEIGHT
+
+/* We use 16 bits per coordinate component, including integer
+   and fractional bits */
+#define SH_CSS_MORPH_TABLE_GRID               ISP_VEC_NELEMS
+#define SH_CSS_MORPH_TABLE_ELEM_BYTES         2
+#define SH_CSS_MORPH_TABLE_ELEMS_PER_DDR_WORD \
+	(HIVE_ISP_DDR_WORD_BYTES / SH_CSS_MORPH_TABLE_ELEM_BYTES)
+
+#define SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR   (SH_CSS_MAX_BQ_GRID_WIDTH + 1)
+#define SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR   (SH_CSS_MAX_BQ_GRID_HEIGHT + 1)
+
+#define SH_CSS_MAX_SCTBL_ALIGNED_WIDTH_PER_COLOR \
+	CEIL_MUL(SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR, ISP_VEC_NELEMS)
+
+/* Each line of this table is aligned to the maximum line width. */
+#define SH_CSS_MAX_S3ATBL_WIDTH              SH_CSS_MAX_BQ_GRID_WIDTH
+
+/* Video mode specific DVS define */
+/* The video binary supports a delay of 1 or 2 frames */
+#define MAX_DVS_FRAME_DELAY		2
+/* +1 because DVS reads the previous and writes the current frame concurrently */
+#define MAX_NUM_VIDEO_DELAY_FRAMES	(MAX_DVS_FRAME_DELAY + 1)
+
+#define NUM_VIDEO_TNR_FRAMES		2
+
+/* Note that this is the define used to configure all data structures common for all modes */
+/* It should be equal or bigger to the max number of DVS frames for all possible modes */
+/* Rules: these implement logic shared between the host code and ISP firmware.
+   The ISP firmware needs these rules to be applied at pre-processor time,
+   that's why these are macros, not functions. */
+#define _ISP_BQS(num)  ((num) / 2)
+#define _ISP_VECS(width) CEIL_DIV(width, ISP_VEC_NELEMS)
+
+#define ISP_BQ_GRID_WIDTH(elements_per_line, deci_factor_log2) \
+	CEIL_SHIFT(elements_per_line / 2,  deci_factor_log2)
+#define ISP_BQ_GRID_HEIGHT(lines_per_frame, deci_factor_log2) \
+	CEIL_SHIFT(lines_per_frame / 2,  deci_factor_log2)
+#define ISP_C_VECTORS_PER_LINE(elements_per_line) \
+	_ISP_VECS(elements_per_line / 2)
+
+/* The morphing table is similar to the shading table in the sense that we
+   have 1 more value than we have cells in the grid. */
+#define _ISP_MORPH_TABLE_WIDTH(int_width) \
+	(CEIL_DIV(int_width, SH_CSS_MORPH_TABLE_GRID) + 1)
+#define _ISP_MORPH_TABLE_HEIGHT(int_height) \
+	(CEIL_DIV(int_height, SH_CSS_MORPH_TABLE_GRID) + 1)
+#define _ISP_MORPH_TABLE_ALIGNED_WIDTH(width) \
+	CEIL_MUL(_ISP_MORPH_TABLE_WIDTH(width), \
+		 SH_CSS_MORPH_TABLE_ELEMS_PER_DDR_WORD)
+
+#define _ISP_SCTBL_WIDTH_PER_COLOR(input_width, deci_factor_log2) \
+	(ISP_BQ_GRID_WIDTH(input_width, deci_factor_log2) + 1)
+#define _ISP_SCTBL_HEIGHT(input_height, deci_factor_log2) \
+	(ISP_BQ_GRID_HEIGHT(input_height, deci_factor_log2) + 1)
+#define _ISP_SCTBL_ALIGNED_WIDTH_PER_COLOR(input_width, deci_factor_log2) \
+	CEIL_MUL(_ISP_SCTBL_WIDTH_PER_COLOR(input_width, deci_factor_log2), \
+		 ISP_VEC_NELEMS)
+
+/* To position the shading center grid point on the center of output image,
+ * one more grid cell is needed as margin. */
+#define SH_CSS_SCTBL_CENTERING_MARGIN	1
+
+/* The shading table width and height are the number of grids, not cells. The last grid should be counted. */
+#define SH_CSS_SCTBL_LAST_GRID_COUNT	1
+
+/* Number of horizontal grids per color in the shading table. */
+#define _ISP2401_SCTBL_WIDTH_PER_COLOR(input_width, deci_factor_log2) \
+	(ISP_BQ_GRID_WIDTH(input_width, deci_factor_log2) + \
+	SH_CSS_SCTBL_CENTERING_MARGIN + SH_CSS_SCTBL_LAST_GRID_COUNT)
+
+/* Number of vertical grids per color in the shading table. */
+#define _ISP2401_SCTBL_HEIGHT(input_height, deci_factor_log2) \
+	(ISP_BQ_GRID_HEIGHT(input_height, deci_factor_log2) + \
+	SH_CSS_SCTBL_CENTERING_MARGIN + SH_CSS_SCTBL_LAST_GRID_COUNT)
+
+/* ISP2401: Legacy API: Number of horizontal grids per color in the shading table. */
+#define _ISP_SCTBL_LEGACY_WIDTH_PER_COLOR(input_width, deci_factor_log2) \
+	(ISP_BQ_GRID_WIDTH(input_width, deci_factor_log2) + SH_CSS_SCTBL_LAST_GRID_COUNT)
+
+/* ISP2401: Legacy API: Number of vertical grids per color in the shading table. */
+#define _ISP_SCTBL_LEGACY_HEIGHT(input_height, deci_factor_log2) \
+	(ISP_BQ_GRID_HEIGHT(input_height, deci_factor_log2) + SH_CSS_SCTBL_LAST_GRID_COUNT)
+
+/* *****************************************************************
+ * Statistics for 3A (Auto Focus, Auto White Balance, Auto Exposure)
+ * *****************************************************************/
+/* if left cropping is used, 3A statistics are also cropped by 2 vectors. */
+#define _ISP_S3ATBL_WIDTH(in_width, deci_factor_log2) \
+	(_ISP_BQS(in_width) >> deci_factor_log2)
+#define _ISP_S3ATBL_HEIGHT(in_height, deci_factor_log2) \
+	(_ISP_BQS(in_height) >> deci_factor_log2)
+#define _ISP_S3A_ELEMS_ISP_WIDTH(width, left_crop) \
+	(width - ((left_crop) ? 2 * ISP_VEC_NELEMS : 0))
+
+#define _ISP_S3ATBL_ISP_WIDTH(in_width, deci_factor_log2) \
+	CEIL_SHIFT(_ISP_BQS(in_width), deci_factor_log2)
+#define _ISP_S3ATBL_ISP_HEIGHT(in_height, deci_factor_log2) \
+	CEIL_SHIFT(_ISP_BQS(in_height), deci_factor_log2)
+#define ISP_S3ATBL_VECTORS \
+	_ISP_VECS(SH_CSS_MAX_S3ATBL_WIDTH * \
+		  (sizeof(struct ia_css_3a_output) / sizeof(int32_t)))
+#define ISP_S3ATBL_HI_LO_STRIDE \
+	(ISP_S3ATBL_VECTORS * ISP_VEC_NELEMS)
+#define ISP_S3ATBL_HI_LO_STRIDE_BYTES \
+	(sizeof(unsigned short) * ISP_S3ATBL_HI_LO_STRIDE)
+
+/* Viewfinder support */
+#define __ISP_MAX_VF_OUTPUT_WIDTH(width, left_crop) \
+	(width - 2 * ISP_VEC_NELEMS + ((left_crop) ? 2 * ISP_VEC_NELEMS : 0))
+
+#define __ISP_VF_OUTPUT_WIDTH_VECS(out_width, vf_log_downscale) \
+	(_ISP_VECS((out_width) >> (vf_log_downscale)))
+
+#define _ISP_VF_OUTPUT_WIDTH(vf_out_vecs) ((vf_out_vecs) * ISP_VEC_NELEMS)
+#define _ISP_VF_OUTPUT_HEIGHT(out_height, vf_log_ds) \
+	((out_height) >> (vf_log_ds))
+
+#define _ISP_LOG_VECTOR_STEP(mode) \
+	((mode) == IA_CSS_BINARY_MODE_CAPTURE_PP ? 2 : 1)
+
+/* It is preferred to have not more than 2x scaling at one step
+ * in GDC (assumption is for capture_pp and yuv_scale stages) */
+#define MAX_PREFERRED_YUV_DS_PER_STEP	2
+
+/* Rules for computing the internal width. This is extremely complicated
+ * and definitely needs to be commented and explained. */
+#define _ISP_LEFT_CROP_EXTRA(left_crop) ((left_crop) > 0 ? 2 * ISP_VEC_NELEMS : 0)
+
+#define __ISP_MIN_INTERNAL_WIDTH(num_chunks, pipelining, mode) \
+	((num_chunks) * (pipelining) * (1 << _ISP_LOG_VECTOR_STEP(mode)) * \
+	 ISP_VEC_NELEMS)
+
+#define __ISP_PADDED_OUTPUT_WIDTH(out_width, dvs_env_width, left_crop) \
+	((out_width) + MAX(dvs_env_width, _ISP_LEFT_CROP_EXTRA(left_crop)))
+
+#define __ISP_CHUNK_STRIDE_ISP(mode) \
+	((1 << _ISP_LOG_VECTOR_STEP(mode)) * ISP_VEC_NELEMS)
+
+#define __ISP_CHUNK_STRIDE_DDR(c_subsampling, num_chunks) \
+	((c_subsampling) * (num_chunks) * HIVE_ISP_DDR_WORD_BYTES)
+#define __ISP_INTERNAL_WIDTH(out_width, \
+			     dvs_env_width, \
+			     left_crop, \
+			     mode, \
+			     c_subsampling, \
+			     num_chunks, \
+			     pipelining) \
+	CEIL_MUL2(CEIL_MUL2(MAX(__ISP_PADDED_OUTPUT_WIDTH(out_width, \
+							    dvs_env_width, \
+							    left_crop), \
+				  __ISP_MIN_INTERNAL_WIDTH(num_chunks, \
+							   pipelining, \
+							   mode) \
+				 ), \
+			  __ISP_CHUNK_STRIDE_ISP(mode) \
+			 ), \
+		 __ISP_CHUNK_STRIDE_DDR(c_subsampling, num_chunks) \
+		)
+
+#define __ISP_INTERNAL_HEIGHT(out_height, dvs_env_height, top_crop) \
+	((out_height) + (dvs_env_height) + top_crop)
+
+/* @GC: Input can be up to sensor resolution when either bayer downscaling
+ *	or raw binning is enabled.
+ *	Also, during continuous mode, we need to align to 4*NWAY since input
+ *	should support binning */
+#define _ISP_MAX_INPUT_WIDTH(max_internal_width, enable_ds, enable_fixed_bayer_ds, enable_raw_bin, \
+				enable_continuous) \
+	((enable_ds) ? \
+	   SH_CSS_MAX_SENSOR_WIDTH :\
+	 (enable_fixed_bayer_ds) ? \
+	   CEIL_MUL(SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH_DEC, 4 * ISP_VEC_NELEMS) : \
+	 (enable_raw_bin) ? \
+	   CEIL_MUL(SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH, 4 * ISP_VEC_NELEMS) : \
+	 (enable_continuous) ? \
+	   SH_CSS_MAX_CONTINUOUS_SENSOR_WIDTH \
+	   : max_internal_width)
+
+#define _ISP_INPUT_WIDTH(internal_width, ds_input_width, enable_ds) \
+	((enable_ds) ? (ds_input_width) : (internal_width))
+
+#define _ISP_MAX_INPUT_HEIGHT(max_internal_height, enable_ds, enable_fixed_bayer_ds, enable_raw_bin, \
+				enable_continuous) \
+	((enable_ds) ? \
+	   SH_CSS_MAX_SENSOR_HEIGHT :\
+	 (enable_fixed_bayer_ds) ? \
+	   SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT_DEC : \
+	 (enable_raw_bin || enable_continuous) ? \
+	   SH_CSS_MAX_CONTINUOUS_SENSOR_HEIGHT \
+	   : max_internal_height)
+
+#define _ISP_INPUT_HEIGHT(internal_height, ds_input_height, enable_ds) \
+	((enable_ds) ? (ds_input_height) : (internal_height))
+
+#define SH_CSS_MAX_STAGES 8 /* primary_stage[1-6], capture_pp, vf_pp */
+
+/* For CSI2+ input system, it requires extra paddinga from vmem */
+#define _ISP_EXTRA_PADDING_VECS 0
+
+#endif /* _SH_CSS_DEFS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_dvs_info.h b/drivers/staging/media/atomisp/pci/sh_css_dvs_info.h
new file mode 100644
index 000000000..6f058f132
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_dvs_info.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/**
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope 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.
+*/
+
+#ifndef __SH_CSS_DVS_INFO_H__
+#define __SH_CSS_DVS_INFO_H__
+
+#include <math_support.h>
+
+/* horizontal 64x64 blocks round up to DVS_BLOCKDIM_X, make even */
+#define DVS_NUM_BLOCKS_X(X)		(CEIL_MUL(CEIL_DIV((X), DVS_BLOCKDIM_X), 2))
+
+/* vertical   64x64 blocks round up to DVS_BLOCKDIM_Y */
+#define DVS_NUM_BLOCKS_Y(X)		(CEIL_DIV((X), DVS_BLOCKDIM_Y_LUMA))
+
+/* Bilinear interpolation (HRT_GDC_BLI_MODE) is the supported method currently.
+ * Bicubic interpolation (HRT_GDC_BCI_MODE) is not supported yet */
+#define DVS_GDC_INTERP_METHOD HRT_GDC_BLI_MODE
+
+#define DVS_INPUT_BYTES_PER_PIXEL (1)
+
+#define DVS_NUM_BLOCKS_X_CHROMA(X)	(CEIL_DIV((X), DVS_BLOCKDIM_X))
+
+#define DVS_NUM_BLOCKS_Y_CHROMA(X)	(CEIL_DIV((X), DVS_BLOCKDIM_Y_CHROMA))
+
+#endif /* __SH_CSS_DVS_INFO_H__ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_firmware.c b/drivers/staging/media/atomisp/pci/sh_css_firmware.c
new file mode 100644
index 000000000..e7ef578db
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_firmware.c
@@ -0,0 +1,382 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/string.h> /* for memcpy() */
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include "hmm.h"
+
+#include <math_support.h>
+#include "platform_support.h"
+#include "sh_css_firmware.h"
+
+#include "sh_css_defs.h"
+#include "ia_css_debug.h"
+#include "sh_css_internal.h"
+#include "ia_css_isp_param.h"
+
+#include "assert_support.h"
+
+#include "isp.h"				/* PMEM_WIDTH_LOG2 */
+
+#include "ia_css_isp_params.h"
+#include "ia_css_isp_configs.h"
+#include "ia_css_isp_states.h"
+
+#define _STR(x) #x
+#define STR(x) _STR(x)
+
+struct firmware_header {
+	struct sh_css_fw_bi_file_h file_header;
+	struct ia_css_fw_info      binary_header;
+};
+
+struct fw_param {
+	const char *name;
+	const void *buffer;
+};
+
+static struct firmware_header *firmware_header;
+
+/*
+ * The string STR is a place holder
+ * which will be replaced with the actual RELEASE_VERSION
+ * during package generation. Please do not modify
+ */
+#ifdef ISP2401
+static const char *release_version = STR(irci_stable_candrpv_0415_20150521_0458);
+#else
+static const char *release_version = STR(irci_stable_candrpv_0415_20150423_1753);
+#endif
+
+#define MAX_FW_REL_VER_NAME	300
+static char FW_rel_ver_name[MAX_FW_REL_VER_NAME] = "---";
+
+struct ia_css_fw_info	  sh_css_sp_fw;
+struct ia_css_blob_descr *sh_css_blob_info; /* Only ISP blob info (no SP) */
+unsigned int sh_css_num_binaries; /* This includes 1 SP binary */
+
+static struct fw_param *fw_minibuffer;
+
+char *sh_css_get_fw_version(void)
+{
+	return FW_rel_ver_name;
+}
+
+/*
+ * Split the loaded firmware into blobs
+ */
+
+/* Setup sp/sp1 binary */
+static int
+setup_binary(struct ia_css_fw_info *fw, const char *fw_data,
+	     struct ia_css_fw_info *sh_css_fw, unsigned int binary_id)
+{
+	const char *blob_data;
+
+	if ((!fw) || (!fw_data))
+		return -EINVAL;
+
+	blob_data = fw_data + fw->blob.offset;
+
+	*sh_css_fw = *fw;
+
+	sh_css_fw->blob.code = vmalloc(fw->blob.size);
+	if (!sh_css_fw->blob.code)
+		return -ENOMEM;
+
+	memcpy((void *)sh_css_fw->blob.code, blob_data, fw->blob.size);
+	sh_css_fw->blob.data = (char *)sh_css_fw->blob.code + fw->blob.data_source;
+	fw_minibuffer[binary_id].buffer = sh_css_fw->blob.code;
+
+	return 0;
+}
+
+int
+sh_css_load_blob_info(const char *fw, const struct ia_css_fw_info *bi,
+		      struct ia_css_blob_descr *bd,
+		      unsigned int index)
+{
+	const char *name;
+	const unsigned char *blob;
+
+	if ((!fw) || (!bd))
+		return -EINVAL;
+
+	/* Special case: only one binary in fw */
+	if (!bi)
+		bi = (const struct ia_css_fw_info *)fw;
+
+	name = fw + bi->blob.prog_name_offset;
+	blob = (const unsigned char *)fw + bi->blob.offset;
+
+	/* sanity check */
+	if (bi->blob.size !=
+		bi->blob.text_size + bi->blob.icache_size +
+			bi->blob.data_size + bi->blob.padding_size) {
+		/* sanity check, note the padding bytes added for section to DDR alignment */
+		return -EINVAL;
+	}
+
+	if ((bi->blob.offset % (1UL << (ISP_PMEM_WIDTH_LOG2 - 3))) != 0)
+		return -EINVAL;
+
+	bd->blob = blob;
+	bd->header = *bi;
+
+	if (bi->type == ia_css_isp_firmware || bi->type == ia_css_sp_firmware) {
+		char *namebuffer;
+
+		namebuffer = kstrdup(name, GFP_KERNEL);
+		if (!namebuffer)
+			return -ENOMEM;
+		bd->name = fw_minibuffer[index].name = namebuffer;
+	} else {
+		bd->name = name;
+	}
+
+	if (bi->type == ia_css_isp_firmware) {
+		size_t paramstruct_size = sizeof(struct ia_css_memory_offsets);
+		size_t configstruct_size = sizeof(struct ia_css_config_memory_offsets);
+		size_t statestruct_size = sizeof(struct ia_css_state_memory_offsets);
+
+		char *parambuf = kmalloc(paramstruct_size + configstruct_size +
+					 statestruct_size,
+					 GFP_KERNEL);
+		if (!parambuf)
+			return -ENOMEM;
+
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_PARAM].ptr = NULL;
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_CONFIG].ptr = NULL;
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_STATE].ptr = NULL;
+
+		fw_minibuffer[index].buffer = parambuf;
+
+		/* copy ia_css_memory_offsets */
+		memcpy(parambuf, (void *)(fw +
+					  bi->blob.memory_offsets.offsets[IA_CSS_PARAM_CLASS_PARAM]),
+		       paramstruct_size);
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_PARAM].ptr = parambuf;
+
+		/* copy ia_css_config_memory_offsets */
+		memcpy(parambuf + paramstruct_size,
+		       (void *)(fw + bi->blob.memory_offsets.offsets[IA_CSS_PARAM_CLASS_CONFIG]),
+		       configstruct_size);
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_CONFIG].ptr = parambuf +
+		paramstruct_size;
+
+		/* copy ia_css_state_memory_offsets */
+		memcpy(parambuf + paramstruct_size + configstruct_size,
+		       (void *)(fw + bi->blob.memory_offsets.offsets[IA_CSS_PARAM_CLASS_STATE]),
+		       statestruct_size);
+		bd->mem_offsets.array[IA_CSS_PARAM_CLASS_STATE].ptr = parambuf +
+		paramstruct_size + configstruct_size;
+	}
+	return 0;
+}
+
+bool
+sh_css_check_firmware_version(struct device *dev, const char *fw_data)
+{
+	struct sh_css_fw_bi_file_h *file_header;
+
+	firmware_header = (struct firmware_header *)fw_data;
+	file_header = &firmware_header->file_header;
+
+	if (strcmp(file_header->version, release_version) != 0) {
+		dev_err(dev, "Firmware version may not be compatible with this driver\n");
+		dev_err(dev, "Expecting version '%s', but firmware is '%s'.\n",
+			release_version, file_header->version);
+	}
+
+	/* For now, let's just accept a wrong version, even if wrong */
+	return false;
+}
+
+static const char * const fw_type_name[] = {
+	[ia_css_sp_firmware]		= "SP",
+	[ia_css_isp_firmware]		= "ISP",
+	[ia_css_bootloader_firmware]	= "BootLoader",
+	[ia_css_acc_firmware]		= "accel",
+};
+
+static const char * const fw_acc_type_name[] = {
+	[IA_CSS_ACC_NONE] =		"Normal",
+	[IA_CSS_ACC_OUTPUT] =		"Accel for output",
+	[IA_CSS_ACC_VIEWFINDER] =	"Accel for viewfinder",
+	[IA_CSS_ACC_STANDALONE] =	"Stand-alone accel",
+};
+
+int
+sh_css_load_firmware(struct device *dev, const char *fw_data,
+		     unsigned int fw_size)
+{
+	unsigned int i;
+	struct ia_css_fw_info *binaries;
+	struct sh_css_fw_bi_file_h *file_header;
+	int ret;
+
+	firmware_header = (struct firmware_header *)fw_data;
+	file_header = &firmware_header->file_header;
+	binaries = &firmware_header->binary_header;
+	strscpy(FW_rel_ver_name, file_header->version,
+		min(sizeof(FW_rel_ver_name), sizeof(file_header->version)));
+	ret = sh_css_check_firmware_version(dev, fw_data);
+	if (ret) {
+		IA_CSS_ERROR("CSS code version (%s) and firmware version (%s) mismatch!",
+			     file_header->version, release_version);
+		return -EINVAL;
+	} else {
+		IA_CSS_LOG("successfully load firmware version %s", release_version);
+	}
+
+	/* some sanity checks */
+	if (!fw_data || fw_size < sizeof(struct sh_css_fw_bi_file_h))
+		return -EINVAL;
+
+	if (file_header->h_size != sizeof(struct sh_css_fw_bi_file_h))
+		return -EINVAL;
+
+	sh_css_num_binaries = file_header->binary_nr;
+	/* Only allocate memory for ISP blob info */
+	if (sh_css_num_binaries > NUM_OF_SPS) {
+		sh_css_blob_info = kmalloc(
+		    (sh_css_num_binaries - NUM_OF_SPS) *
+		    sizeof(*sh_css_blob_info), GFP_KERNEL);
+		if (!sh_css_blob_info)
+			return -ENOMEM;
+	} else {
+		sh_css_blob_info = NULL;
+	}
+
+	fw_minibuffer = kcalloc(sh_css_num_binaries, sizeof(struct fw_param),
+				GFP_KERNEL);
+	if (!fw_minibuffer)
+		return -ENOMEM;
+
+	for (i = 0; i < sh_css_num_binaries; i++) {
+		struct ia_css_fw_info *bi = &binaries[i];
+		/*
+		 * note: the var below is made static as it is quite large;
+		 * if it is not static it ends up on the stack which could
+		 * cause issues for drivers
+		 */
+		static struct ia_css_blob_descr bd;
+		int err;
+
+		err = sh_css_load_blob_info(fw_data, bi, &bd, i);
+
+		if (err)
+			return -EINVAL;
+
+		if (bi->blob.offset + bi->blob.size > fw_size)
+			return -EINVAL;
+
+		switch (bd.header.type) {
+		case ia_css_isp_firmware:
+			if (bd.header.info.isp.type > IA_CSS_ACC_STANDALONE) {
+				dev_err(dev, "binary #%2d: invalid SP type\n",
+					i);
+				return -EINVAL;
+			}
+
+			dev_dbg(dev,
+				"binary #%-2d type %s (%s), binary id is %2d: %s\n",
+				i,
+				fw_type_name[bd.header.type],
+				fw_acc_type_name[bd.header.info.isp.type],
+				bd.header.info.isp.sp.id,
+				bd.name);
+			break;
+		case ia_css_sp_firmware:
+		case ia_css_bootloader_firmware:
+		case ia_css_acc_firmware:
+			dev_dbg(dev,
+				"binary #%-2d type %s: %s\n",
+				i, fw_type_name[bd.header.type],
+				bd.name);
+			break;
+		default:
+			if (bd.header.info.isp.type > IA_CSS_ACC_STANDALONE) {
+				dev_err(dev,
+					"binary #%2d: invalid firmware type\n",
+					i);
+				return -EINVAL;
+			}
+			break;
+		}
+
+		if (bi->type == ia_css_sp_firmware) {
+			if (i != SP_FIRMWARE)
+				return -EINVAL;
+			err = setup_binary(bi, fw_data, &sh_css_sp_fw, i);
+			if (err)
+				return err;
+
+		} else {
+			/*
+			 * All subsequent binaries
+			 * (including bootloaders) (i>NUM_OF_SPS)
+			 * are ISP firmware
+			 */
+			if (i < NUM_OF_SPS)
+				return -EINVAL;
+
+			if (bi->type != ia_css_isp_firmware)
+				return -EINVAL;
+			if (!sh_css_blob_info) /* cannot happen but KW does not see this */
+				return -EINVAL;
+			sh_css_blob_info[i - NUM_OF_SPS] = bd;
+		}
+	}
+
+	return 0;
+}
+
+void sh_css_unload_firmware(void)
+{
+	/* release firmware minibuffer */
+	if (fw_minibuffer) {
+		unsigned int i = 0;
+
+		for (i = 0; i < sh_css_num_binaries; i++) {
+			kfree(fw_minibuffer[i].name);
+			kvfree(fw_minibuffer[i].buffer);
+		}
+		kfree(fw_minibuffer);
+		fw_minibuffer = NULL;
+	}
+
+	memset(&sh_css_sp_fw, 0, sizeof(sh_css_sp_fw));
+	kfree(sh_css_blob_info);
+	sh_css_blob_info = NULL;
+	sh_css_num_binaries = 0;
+}
+
+ia_css_ptr
+sh_css_load_blob(const unsigned char *blob, unsigned int size)
+{
+	ia_css_ptr target_addr = hmm_alloc(size);
+	/*
+	 * this will allocate memory aligned to a DDR word boundary which
+	 * is required for the CSS DMA to read the instructions.
+	 */
+
+	assert(blob);
+	if (target_addr)
+		hmm_store(target_addr, blob, size);
+	return target_addr;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_firmware.h b/drivers/staging/media/atomisp/pci/sh_css_firmware.h
new file mode 100644
index 000000000..a73ce703a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_firmware.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_FIRMWARE_H_
+#define _SH_CSS_FIRMWARE_H_
+
+#include <system_local.h>
+
+#include <ia_css_err.h>
+#include <ia_css_acc_types.h>
+
+/* This is for the firmware loaded from user space */
+struct  sh_css_fw_bi_file_h {
+	char version[64];		/* branch tag + week day + time */
+	int binary_nr;			/* Number of binaries */
+	unsigned int h_size;		/* sizeof(struct sh_css_fw_bi_file_h) */
+};
+
+extern struct ia_css_fw_info     sh_css_sp_fw;
+extern struct ia_css_blob_descr *sh_css_blob_info;
+extern unsigned int sh_css_num_binaries;
+
+char
+*sh_css_get_fw_version(void);
+
+struct device;
+bool
+sh_css_check_firmware_version(struct device *dev, const char *fw_data);
+
+int
+sh_css_load_firmware(struct device *dev, const char *fw_data,
+		     unsigned int fw_size);
+
+void sh_css_unload_firmware(void);
+
+ia_css_ptr sh_css_load_blob(const unsigned char *blob, unsigned int size);
+
+int
+sh_css_load_blob_info(const char *fw, const struct ia_css_fw_info *bi,
+		      struct ia_css_blob_descr *bd, unsigned int i);
+
+#endif /* _SH_CSS_FIRMWARE_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_frac.h b/drivers/staging/media/atomisp/pci/sh_css_frac.h
new file mode 100644
index 000000000..8f08df5c8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_frac.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_FRAC_H
+#define __SH_CSS_FRAC_H
+
+#include <math_support.h>
+
+#define sISP_REG_BIT		      ISP_VEC_ELEMBITS
+#define uISP_REG_BIT		      ((unsigned int)(sISP_REG_BIT - 1))
+#define sSHIFT				    (16 - sISP_REG_BIT)
+#define uSHIFT				    ((unsigned int)(16 - uISP_REG_BIT))
+#define sFRACTION_BITS_FITTING(a) (a - sSHIFT)
+#define uFRACTION_BITS_FITTING(a) ((unsigned int)(a - uSHIFT))
+#define sISP_VAL_MIN		      (-(1 << uISP_REG_BIT))
+#define sISP_VAL_MAX		      ((1 << uISP_REG_BIT) - 1)
+#define uISP_VAL_MIN		      (0U)
+#define uISP_VAL_MAX		      ((unsigned int)((1 << uISP_REG_BIT) - 1))
+
+/* a:fraction bits for 16bit precision, b:fraction bits for ISP precision */
+#define sDIGIT_FITTING(v, a, b) \
+	min_t(int, max_t(int, (((v) >> sSHIFT) >> max(sFRACTION_BITS_FITTING(a) - (b), 0)), \
+	  sISP_VAL_MIN), sISP_VAL_MAX)
+#define uDIGIT_FITTING(v, a, b) \
+	min((unsigned int)max((unsigned)(((v) >> uSHIFT) \
+	>> max((int)(uFRACTION_BITS_FITTING(a) - (b)), 0)), \
+	  uISP_VAL_MIN), uISP_VAL_MAX)
+
+#endif /* __SH_CSS_FRAC_H */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_host_data.c b/drivers/staging/media/atomisp/pci/sh_css_host_data.c
new file mode 100644
index 000000000..39a9b9812
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_host_data.c
@@ -0,0 +1,43 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/slab.h>
+#include <ia_css_host_data.h>
+#include <sh_css_internal.h>
+
+struct ia_css_host_data *ia_css_host_data_allocate(size_t size)
+{
+	struct ia_css_host_data *me;
+
+	me =  kmalloc(sizeof(struct ia_css_host_data), GFP_KERNEL);
+	if (!me)
+		return NULL;
+	me->size = (uint32_t)size;
+	me->address = kvmalloc(size, GFP_KERNEL);
+	if (!me->address) {
+		kfree(me);
+		return NULL;
+	}
+	return me;
+}
+
+void ia_css_host_data_free(struct ia_css_host_data *me)
+{
+	if (me) {
+		kvfree(me->address);
+		me->address = NULL;
+		kfree(me);
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_hrt.c b/drivers/staging/media/atomisp/pci/sh_css_hrt.c
new file mode 100644
index 000000000..879c85311
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_hrt.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "platform_support.h"
+
+#include "sh_css_hrt.h"
+#include "ia_css_debug.h"
+
+#include "device_access.h"
+
+#define __INLINE_EVENT__
+#include "event_fifo.h"
+#define __INLINE_SP__
+#include "sp.h"
+#define __INLINE_ISP__
+#include "isp.h"
+#define __INLINE_IRQ__
+#include "irq.h"
+#define __INLINE_FIFO_MONITOR__
+#include "fifo_monitor.h"
+
+/* System independent */
+#include "sh_css_internal.h"
+
+bool sh_css_hrt_system_is_idle(void)
+{
+	bool not_idle = false, idle;
+	fifo_channel_t ch;
+
+	idle = sp_ctrl_getbit(SP0_ID, SP_SC_REG, SP_IDLE_BIT);
+	not_idle |= !idle;
+	if (!idle)
+		IA_CSS_WARNING("SP not idle");
+
+	idle = isp_ctrl_getbit(ISP0_ID, ISP_SC_REG, ISP_IDLE_BIT);
+	not_idle |= !idle;
+	if (!idle)
+		IA_CSS_WARNING("ISP not idle");
+
+	for (ch = 0; ch < N_FIFO_CHANNEL; ch++) {
+		fifo_channel_state_t state;
+
+		fifo_channel_get_state(FIFO_MONITOR0_ID, ch, &state);
+		if (state.fifo_valid) {
+			IA_CSS_WARNING("FIFO channel %d is not empty", ch);
+			not_idle = true;
+		}
+	}
+
+	return !not_idle;
+}
+
+int sh_css_hrt_sp_wait(void)
+{
+	irq_sw_channel_id_t	irq_id = IRQ_SW_CHANNEL0_ID;
+	/*
+	 * Wait till SP is idle or till there is a SW2 interrupt
+	 * The SW2 interrupt will be used when frameloop runs on SP
+	 * and signals an event with similar meaning as SP idle
+	 * (e.g. frame_done)
+	 */
+	while (!sp_ctrl_getbit(SP0_ID, SP_SC_REG, SP_IDLE_BIT) &&
+	       ((irq_reg_load(IRQ0_ID,
+			      _HRT_IRQ_CONTROLLER_STATUS_REG_IDX) &
+		 (1U << (irq_id + IRQ_SW_CHANNEL_OFFSET))) == 0)) {
+		udelay(1);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_hrt.h b/drivers/staging/media/atomisp/pci/sh_css_hrt.h
new file mode 100644
index 000000000..168bbd579
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_hrt.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_HRT_H_
+#define _SH_CSS_HRT_H_
+
+#include <sp.h>
+#include <isp.h>
+
+#include <ia_css_err.h>
+
+/* SP access */
+void sh_css_hrt_sp_start_si(void);
+
+void sh_css_hrt_sp_start_copy_frame(void);
+
+void sh_css_hrt_sp_start_isp(void);
+
+int sh_css_hrt_sp_wait(void);
+
+bool sh_css_hrt_system_is_idle(void);
+
+#endif /* _SH_CSS_HRT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_internal.h b/drivers/staging/media/atomisp/pci/sh_css_internal.h
new file mode 100644
index 000000000..435b3cedd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_internal.h
@@ -0,0 +1,994 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_INTERNAL_H_
+#define _SH_CSS_INTERNAL_H_
+
+#include <system_global.h>
+#include <math_support.h>
+#include <type_support.h>
+#include <platform_support.h>
+#include <linux/stdarg.h>
+
+#if !defined(ISP2401)
+#include "input_formatter.h"
+#endif
+#include "input_system.h"
+
+#include "ia_css_types.h"
+#include "ia_css_acc_types.h"
+#include "ia_css_buffer.h"
+
+#include "ia_css_binary.h"
+#include "sh_css_firmware.h" /* not needed/desired on SP/ISP */
+#include "sh_css_legacy.h"
+#include "sh_css_defs.h"
+#include "sh_css_uds.h"
+#include "dma.h"	/* N_DMA_CHANNEL_ID */
+#include "ia_css_circbuf_comm.h" /* Circular buffer */
+#include "ia_css_frame_comm.h"
+#include "ia_css_3a.h"
+#include "ia_css_dvs.h"
+#include "ia_css_metadata.h"
+#include "runtime/bufq/interface/ia_css_bufq.h"
+#include "ia_css_timer.h"
+
+/* TODO: Move to a more suitable place when sp pipeline design is done. */
+#define IA_CSS_NUM_CB_SEM_READ_RESOURCE	2
+#define IA_CSS_NUM_CB_SEM_WRITE_RESOURCE	1
+#define IA_CSS_NUM_CBS						2
+#define IA_CSS_CB_MAX_ELEMS					2
+
+/* Use case specific. index limited to IA_CSS_NUM_CB_SEM_READ_RESOURCE or
+ * IA_CSS_NUM_CB_SEM_WRITE_RESOURCE for read and write respectively.
+ * TODO: Enforce the limitation above.
+*/
+#define IA_CSS_COPYSINK_SEM_INDEX	0
+#define IA_CSS_TAGGER_SEM_INDEX	1
+
+/* Force generation of output event. Used by acceleration pipe. */
+#define IA_CSS_POST_OUT_EVENT_FORCE		2
+
+#define SH_CSS_MAX_BINARY_NAME	64
+
+#define SP_DEBUG_NONE	(0)
+#define SP_DEBUG_DUMP	(1)
+#define SP_DEBUG_COPY	(2)
+#define SP_DEBUG_TRACE	(3)
+#define SP_DEBUG_MINIMAL (4)
+
+#define SP_DEBUG SP_DEBUG_NONE
+#define SP_DEBUG_MINIMAL_OVERWRITE 1
+
+#define SH_CSS_TNR_BIT_DEPTH 8
+#define SH_CSS_REF_BIT_DEPTH 8
+
+/* keep next up to date with the definition for MAX_CB_ELEMS_FOR_TAGGER in tagger.sp.c */
+#define NUM_CONTINUOUS_FRAMES	15
+#define NUM_MIPI_FRAMES_PER_STREAM		2
+
+#define NUM_ONLINE_INIT_CONTINUOUS_FRAMES      2
+
+#define NR_OF_PIPELINES			IA_CSS_PIPE_ID_NUM /* Must match with IA_CSS_PIPE_ID_NUM */
+
+#define SH_CSS_MAX_IF_CONFIGS	3 /* Must match with IA_CSS_NR_OF_CONFIGS (not defined yet).*/
+#define SH_CSS_IF_CONFIG_NOT_NEEDED	0xFF
+
+/*
+ * SH_CSS_MAX_SP_THREADS:
+ *	 sp threads visible to host with connected communication queues
+ *	 these threads are capable of running an image pipe
+ * SH_CSS_MAX_SP_INTERNAL_THREADS:
+ *	 internal sp service threads, no communication queues to host
+ *	 these threads can't be used as image pipe
+ */
+
+#if !defined(ISP2401)
+#define SH_CSS_SP_INTERNAL_METADATA_THREAD	1
+#else
+#define SH_CSS_SP_INTERNAL_METADATA_THREAD	0
+#endif
+
+#define SH_CSS_SP_INTERNAL_SERVICE_THREAD		1
+
+#define SH_CSS_MAX_SP_THREADS		5
+
+#define SH_CSS_MAX_SP_INTERNAL_THREADS	(\
+	 SH_CSS_SP_INTERNAL_SERVICE_THREAD +\
+	 SH_CSS_SP_INTERNAL_METADATA_THREAD)
+
+#define SH_CSS_MAX_PIPELINES	SH_CSS_MAX_SP_THREADS
+
+/**
+ * The C99 standard does not specify the exact object representation of structs;
+ * the representation is compiler dependent.
+ *
+ * The structs that are communicated between host and SP/ISP should have the
+ * exact same object representation. The compiler that is used to compile the
+ * firmware is hivecc.
+ *
+ * To check if a different compiler, used to compile a host application, uses
+ * another object representation, macros are defined specifying the size of
+ * the structs as expected by the firmware.
+ *
+ * A host application shall verify that a sizeof( ) of the struct is equal to
+ * the SIZE_OF_XXX macro of the corresponding struct. If they are not
+ * equal, functionality will break.
+ */
+#define CALC_ALIGNMENT_MEMBER(x, y)	(CEIL_MUL(x, y) - x)
+#define SIZE_OF_HRT_VADDRESS		sizeof(hive_uint32)
+#define SIZE_OF_IA_CSS_PTR		sizeof(uint32_t)
+
+/* Number of SP's */
+#define NUM_OF_SPS 1
+
+#define NUM_OF_BLS 0
+
+/* Enum for order of Binaries */
+enum sh_css_order_binaries {
+	SP_FIRMWARE = 0,
+	ISP_FIRMWARE
+};
+
+/*
+* JB: keep next enum in sync with thread id's
+* and pipe id's
+*/
+enum sh_css_pipe_config_override {
+	SH_CSS_PIPE_CONFIG_OVRD_NONE     = 0,
+	SH_CSS_PIPE_CONFIG_OVRD_NO_OVRD  = 0xffff
+};
+
+enum host2sp_commands {
+	host2sp_cmd_error = 0,
+	/*
+	 * The host2sp_cmd_ready command is the only command written by the SP
+	 * It acknowledges that is previous command has been received.
+	 * (this does not mean that the command has been executed)
+	 * It also indicates that a new command can be send (it is a queue
+	 * with depth 1).
+	 */
+	host2sp_cmd_ready = 1,
+	/* Command written by the Host */
+	host2sp_cmd_dummy,		/* No action, can be used as watchdog */
+	host2sp_cmd_start_flash,	/* Request SP to start the flash */
+	host2sp_cmd_terminate,		/* SP should terminate itself */
+	N_host2sp_cmd
+};
+
+/* Enumeration used to indicate the events that are produced by
+ *  the SP and consumed by the Host.
+ *
+ * !!!IMPORTANT!!! KEEP THE FOLLOWING IN SYNC:
+ * 1) "enum ia_css_event_type"					(ia_css_event_public.h)
+ * 2) "enum sh_css_sp_event_type"				(sh_css_internal.h)
+ * 3) "enum ia_css_event_type event_id_2_event_mask"		(event_handler.sp.c)
+ * 4) "enum ia_css_event_type convert_event_sp_to_host_domain"	(sh_css.c)
+ */
+enum sh_css_sp_event_type {
+	SH_CSS_SP_EVENT_OUTPUT_FRAME_DONE,
+	SH_CSS_SP_EVENT_SECOND_OUTPUT_FRAME_DONE,
+	SH_CSS_SP_EVENT_VF_OUTPUT_FRAME_DONE,
+	SH_CSS_SP_EVENT_SECOND_VF_OUTPUT_FRAME_DONE,
+	SH_CSS_SP_EVENT_3A_STATISTICS_DONE,
+	SH_CSS_SP_EVENT_DIS_STATISTICS_DONE,
+	SH_CSS_SP_EVENT_PIPELINE_DONE,
+	SH_CSS_SP_EVENT_FRAME_TAGGED,
+	SH_CSS_SP_EVENT_INPUT_FRAME_DONE,
+	SH_CSS_SP_EVENT_METADATA_DONE,
+	SH_CSS_SP_EVENT_LACE_STATISTICS_DONE,
+	SH_CSS_SP_EVENT_ACC_STAGE_COMPLETE,
+	SH_CSS_SP_EVENT_TIMER,
+	SH_CSS_SP_EVENT_PORT_EOF,
+	SH_CSS_SP_EVENT_FW_WARNING,
+	SH_CSS_SP_EVENT_FW_ASSERT,
+	SH_CSS_SP_EVENT_NR_OF_TYPES		/* must be last */
+};
+
+/* xmem address map allocation per pipeline, css pointers */
+/* Note that the struct below should only consist of ia_css_ptr-es
+   Otherwise this will cause a fail in the function ref_sh_css_ddr_address_map
+ */
+struct sh_css_ddr_address_map {
+	ia_css_ptr isp_param;
+	ia_css_ptr isp_mem_param[SH_CSS_MAX_STAGES][IA_CSS_NUM_MEMORIES];
+	ia_css_ptr macc_tbl;
+	ia_css_ptr fpn_tbl;
+	ia_css_ptr sc_tbl;
+	ia_css_ptr tetra_r_x;
+	ia_css_ptr tetra_r_y;
+	ia_css_ptr tetra_gr_x;
+	ia_css_ptr tetra_gr_y;
+	ia_css_ptr tetra_gb_x;
+	ia_css_ptr tetra_gb_y;
+	ia_css_ptr tetra_b_x;
+	ia_css_ptr tetra_b_y;
+	ia_css_ptr tetra_ratb_x;
+	ia_css_ptr tetra_ratb_y;
+	ia_css_ptr tetra_batr_x;
+	ia_css_ptr tetra_batr_y;
+	ia_css_ptr dvs_6axis_params_y;
+};
+
+#define SIZE_OF_SH_CSS_DDR_ADDRESS_MAP_STRUCT					\
+	(SIZE_OF_HRT_VADDRESS +							\
+	(SH_CSS_MAX_STAGES * IA_CSS_NUM_MEMORIES * SIZE_OF_HRT_VADDRESS) +	\
+	(16 * SIZE_OF_HRT_VADDRESS))
+
+/* xmem address map allocation per pipeline */
+struct sh_css_ddr_address_map_size {
+	size_t isp_param;
+	size_t isp_mem_param[SH_CSS_MAX_STAGES][IA_CSS_NUM_MEMORIES];
+	size_t macc_tbl;
+	size_t fpn_tbl;
+	size_t sc_tbl;
+	size_t tetra_r_x;
+	size_t tetra_r_y;
+	size_t tetra_gr_x;
+	size_t tetra_gr_y;
+	size_t tetra_gb_x;
+	size_t tetra_gb_y;
+	size_t tetra_b_x;
+	size_t tetra_b_y;
+	size_t tetra_ratb_x;
+	size_t tetra_ratb_y;
+	size_t tetra_batr_x;
+	size_t tetra_batr_y;
+	size_t dvs_6axis_params_y;
+};
+
+struct sh_css_ddr_address_map_compound {
+	struct sh_css_ddr_address_map		map;
+	struct sh_css_ddr_address_map_size	size;
+};
+
+struct ia_css_isp_parameter_set_info {
+	struct sh_css_ddr_address_map
+		mem_map;/** pointers to Parameters in ISP format IMPT:
+						    This should be first member of this struct */
+	u32
+	isp_parameters_id;/** Unique ID to track which config was actually applied to a particular frame */
+	ia_css_ptr
+	output_frame_ptr;/** Output frame to which this config has to be applied (optional) */
+};
+
+/* this struct contains all arguments that can be passed to
+   a binary. It depends on the binary which ones are used. */
+struct sh_css_binary_args {
+	struct ia_css_frame *in_frame;	     /* input frame */
+	const struct ia_css_frame
+		*delay_frames[MAX_NUM_VIDEO_DELAY_FRAMES];   /* reference input frame */
+	const struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];   /* tnr frames */
+	struct ia_css_frame
+		*out_frame[IA_CSS_BINARY_MAX_OUTPUT_PORTS];      /* output frame */
+	struct ia_css_frame *out_vf_frame;   /* viewfinder output frame */
+	bool                 copy_vf;
+	bool                 copy_output;
+	unsigned int vf_downscale_log2;
+};
+
+#if SP_DEBUG == SP_DEBUG_DUMP
+
+#define SH_CSS_NUM_SP_DEBUG 48
+
+struct sh_css_sp_debug_state {
+	unsigned int error;
+	unsigned int debug[SH_CSS_NUM_SP_DEBUG];
+};
+
+#elif SP_DEBUG == SP_DEBUG_COPY
+
+#define SH_CSS_SP_DBG_TRACE_DEPTH	(40)
+
+struct sh_css_sp_debug_trace {
+	u16 frame;
+	u16 line;
+	u16 pixel_distance;
+	u16 mipi_used_dword;
+	u16 sp_index;
+};
+
+struct sh_css_sp_debug_state {
+	u16 if_start_line;
+	u16 if_start_column;
+	u16 if_cropped_height;
+	u16 if_cropped_width;
+	unsigned int index;
+	struct sh_css_sp_debug_trace
+		trace[SH_CSS_SP_DBG_TRACE_DEPTH];
+};
+
+#elif SP_DEBUG == SP_DEBUG_TRACE
+
+/* Example of just one global trace */
+#define SH_CSS_SP_DBG_NR_OF_TRACES	(1)
+#define SH_CSS_SP_DBG_TRACE_DEPTH	(40)
+
+#define SH_CSS_SP_DBG_TRACE_FILE_ID_BIT_POS (13)
+
+struct sh_css_sp_debug_trace {
+	u16 time_stamp;
+	u16 location;	/* bit 15..13 = file_id, 12..0 = line nr. */
+	u32 data;
+};
+
+struct sh_css_sp_debug_state {
+	struct sh_css_sp_debug_trace
+		trace[SH_CSS_SP_DBG_NR_OF_TRACES][SH_CSS_SP_DBG_TRACE_DEPTH];
+	u16 index_last[SH_CSS_SP_DBG_NR_OF_TRACES];
+	u8 index[SH_CSS_SP_DBG_NR_OF_TRACES];
+};
+
+#elif SP_DEBUG == SP_DEBUG_MINIMAL
+
+#define SH_CSS_NUM_SP_DEBUG 128
+
+struct sh_css_sp_debug_state {
+	unsigned int error;
+	unsigned int debug[SH_CSS_NUM_SP_DEBUG];
+};
+
+#endif
+
+struct sh_css_sp_debug_command {
+	/*
+	 * The DMA software-mask,
+	 *	Bit 31...24: unused.
+	 *	Bit 23...16: unused.
+	 *	Bit 15...08: reading-request enabling bits for DMA channel 7..0
+	 *	Bit 07...00: writing-request enabling bits for DMA channel 7..0
+	 *
+	 * For example, "0...0 0...0 11111011 11111101" indicates that the
+	 * writing request through DMA Channel 1 and the reading request
+	 * through DMA channel 2 are both disabled. The others are enabled.
+	 */
+	u32 dma_sw_reg;
+};
+
+#if !defined(ISP2401)
+/* SP input formatter configuration.*/
+struct sh_css_sp_input_formatter_set {
+	u32				stream_format;
+	input_formatter_cfg_t	config_a;
+	input_formatter_cfg_t	config_b;
+};
+#endif
+
+#define IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT (3)
+
+/* SP configuration information */
+struct sh_css_sp_config {
+	u8			no_isp_sync; /* Signal host immediately after start */
+	u8			enable_raw_pool_locking; /** Enable Raw Buffer Locking for HALv3 Support */
+	u8			lock_all;
+	/** If raw buffer locking is enabled, this flag indicates whether raw
+	     frames are locked when their EOF event is successfully sent to the
+	     host (true) or when they are passed to the preview/video pipe
+	     (false). */
+#if !defined(ISP2401)
+	struct {
+		u8					a_changed;
+		u8					b_changed;
+		u8					isp_2ppc;
+		struct sh_css_sp_input_formatter_set
+			set[SH_CSS_MAX_IF_CONFIGS]; /* CSI-2 port is used as index. */
+	} input_formatter;
+#endif
+#if !defined(ISP2401)
+	sync_generator_cfg_t	sync_gen;
+	tpg_cfg_t		tpg;
+	prbs_cfg_t		prbs;
+	input_system_cfg_t	input_circuit;
+	u8			input_circuit_cfg_changed;
+	u32		mipi_sizes_for_check[N_CSI_PORTS][IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT];
+#endif
+	u8                 enable_isys_event_queue;
+	u8			disable_cont_vf;
+};
+
+enum sh_css_stage_type {
+	SH_CSS_SP_STAGE_TYPE  = 0,
+	SH_CSS_ISP_STAGE_TYPE = 1
+};
+
+#define SH_CSS_NUM_STAGE_TYPES 2
+
+#define SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS	BIT(0)
+#define SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS_MASK \
+	((SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS << SH_CSS_MAX_SP_THREADS) - 1)
+
+#if defined(ISP2401)
+struct sh_css_sp_pipeline_terminal {
+	union {
+		/* Input System 2401 */
+		virtual_input_system_stream_t
+		virtual_input_system_stream[IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH];
+	} context;
+	/*
+	 * TODO
+	 * - Remove "virtual_input_system_cfg" when the ISYS2401 DLI is ready.
+	 */
+	union {
+		/* Input System 2401 */
+		virtual_input_system_stream_cfg_t
+		virtual_input_system_stream_cfg[IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH];
+	} ctrl;
+};
+
+struct sh_css_sp_pipeline_io {
+	struct sh_css_sp_pipeline_terminal	input;
+	/* pqiao: comment out temporarily to save dmem */
+	/*struct sh_css_sp_pipeline_terminal	output;*/
+};
+
+/* This struct tracks how many streams are registered per CSI port.
+ * This is used to track which streams have already been configured.
+ * Only when all streams are configured, the CSI RX is started for that port.
+ */
+struct sh_css_sp_pipeline_io_status {
+	u32	active[N_INPUT_SYSTEM_CSI_PORT];	/** registered streams */
+	u32	running[N_INPUT_SYSTEM_CSI_PORT];	/** configured streams */
+};
+
+#endif
+enum sh_css_port_dir {
+	SH_CSS_PORT_INPUT  = 0,
+	SH_CSS_PORT_OUTPUT  = 1
+};
+
+enum sh_css_port_type {
+	SH_CSS_HOST_TYPE  = 0,
+	SH_CSS_COPYSINK_TYPE  = 1,
+	SH_CSS_TAGGERSINK_TYPE  = 2
+};
+
+/* Pipe inout settings: output port on 7-4bits, input port on 3-0bits */
+#define SH_CSS_PORT_FLD_WIDTH_IN_BITS (4)
+#define SH_CSS_PORT_TYPE_BIT_FLD(pt) (0x1 << (pt))
+#define SH_CSS_PORT_FLD(pd) ((pd) ? SH_CSS_PORT_FLD_WIDTH_IN_BITS : 0)
+#define SH_CSS_PIPE_PORT_CONFIG_ON(p, pd, pt) ((p) |= (SH_CSS_PORT_TYPE_BIT_FLD(pt) << SH_CSS_PORT_FLD(pd)))
+#define SH_CSS_PIPE_PORT_CONFIG_OFF(p, pd, pt) ((p) &= ~(SH_CSS_PORT_TYPE_BIT_FLD(pt) << SH_CSS_PORT_FLD(pd)))
+#define SH_CSS_PIPE_PORT_CONFIG_SET(p, pd, pt, val) ((val) ? \
+		SH_CSS_PIPE_PORT_CONFIG_ON(p, pd, pt) : SH_CSS_PIPE_PORT_CONFIG_OFF(p, pd, pt))
+#define SH_CSS_PIPE_PORT_CONFIG_GET(p, pd, pt) ((p) & (SH_CSS_PORT_TYPE_BIT_FLD(pt) << SH_CSS_PORT_FLD(pd)))
+#define SH_CSS_PIPE_PORT_CONFIG_IS_CONTINUOUS(p) \
+	(!(SH_CSS_PIPE_PORT_CONFIG_GET(p, SH_CSS_PORT_INPUT, SH_CSS_HOST_TYPE) && \
+	   SH_CSS_PIPE_PORT_CONFIG_GET(p, SH_CSS_PORT_OUTPUT, SH_CSS_HOST_TYPE)))
+
+#define IA_CSS_ACQUIRE_ISP_POS	31
+
+/* Flags for metadata processing */
+#define SH_CSS_METADATA_ENABLED        0x01
+#define SH_CSS_METADATA_PROCESSED      0x02
+#define SH_CSS_METADATA_OFFLINE_MODE   0x04
+#define SH_CSS_METADATA_WAIT_INPUT     0x08
+
+/* @brief Free an array of metadata buffers.
+ *
+ * @param[in]	num_bufs	Number of metadata buffers to be freed.
+ * @param[in]	bufs		Pointer of array of metadata buffers.
+ *
+ * This function frees an array of metadata buffers.
+ */
+void
+ia_css_metadata_free_multiple(unsigned int num_bufs,
+			      struct ia_css_metadata **bufs);
+
+/* Macro for handling pipe_qos_config */
+#define QOS_INVALID                  (~0U)
+#define QOS_ALL_STAGES_DISABLED      (0U)
+#define QOS_STAGE_MASK(num)          (0x00000001 << num)
+#define SH_CSS_IS_QOS_PIPE(pipe)               ((pipe)->pipe_qos_config != QOS_INVALID)
+#define SH_CSS_QOS_STAGE_ENABLE(pipe, num)     ((pipe)->pipe_qos_config |= QOS_STAGE_MASK(num))
+#define SH_CSS_QOS_STAGE_DISABLE(pipe, num)    ((pipe)->pipe_qos_config &= ~QOS_STAGE_MASK(num))
+#define SH_CSS_QOS_STAGE_IS_ENABLED(pipe, num) ((pipe)->pipe_qos_config & QOS_STAGE_MASK(num))
+#define SH_CSS_QOS_STAGE_IS_ALL_DISABLED(pipe) ((pipe)->pipe_qos_config == QOS_ALL_STAGES_DISABLED)
+#define SH_CSS_QOS_MODE_PIPE_ADD(mode, pipe)    ((mode) |= (0x1 << (pipe)->pipe_id))
+#define SH_CSS_QOS_MODE_PIPE_REMOVE(mode, pipe) ((mode) &= ~(0x1 << (pipe)->pipe_id))
+#define SH_CSS_IS_QOS_ONLY_MODE(mode)           ((mode) == (0x1 << IA_CSS_PIPE_ID_ACC))
+
+/* Information for a pipeline */
+struct sh_css_sp_pipeline {
+	u32	pipe_id;	/* the pipe ID */
+	u32	pipe_num;	/* the dynamic pipe number */
+	u32	thread_id;	/* the sp thread ID */
+	u32	pipe_config;	/* the pipe config */
+	u32	pipe_qos_config;	/* Bitmap of multiple QOS extension fw state.
+						(0xFFFFFFFF) indicates non QOS pipe.*/
+	u32	inout_port_config;
+	u32	required_bds_factor;
+	u32	dvs_frame_delay;
+	u32	input_system_mode;	/* enum ia_css_input_mode */
+	u32	port_id;	/* port_id for input system */
+	u32	num_stages;		/* the pipe config */
+	u32	running;	/* needed for pipe termination */
+	ia_css_ptr	sp_stage_addr[SH_CSS_MAX_STAGES];
+	ia_css_ptr	scaler_pp_lut; /* Early bound LUT */
+	u32	dummy; /* stage ptr is only used on sp but lives in
+				  this struct; needs cleanup */
+	s32 num_execs; /* number of times to run if this is
+			      an acceleration pipe. */
+	struct {
+		u32        format;   /* Metadata format in hrt format */
+		u32        width;    /* Width of a line */
+		u32        height;   /* Number of lines */
+		u32        stride;   /* Stride (in bytes) per line */
+		u32        size;     /* Total size (in bytes) */
+		ia_css_ptr    cont_buf; /* Address of continuous buffer */
+	} metadata;
+	u32	output_frame_queue_id;
+	union {
+		struct {
+			u32	bytes_available;
+		} bin;
+		struct {
+			u32	height;
+			u32	width;
+			u32	padded_width;
+			u32	max_input_width;
+			u32	raw_bit_depth;
+		} raw;
+	} copy;
+};
+
+/*
+ * The first frames (with comment Dynamic) can be dynamic or static
+ * The other frames (ref_in and below) can only be static
+ * Static means that the data address will not change during the life time
+ * of the associated pipe. Dynamic means that the data address can
+ * change with every (frame) iteration of the associated pipe
+ *
+ * s3a and dis are now also dynamic but (stil) handled separately
+ */
+#define SH_CSS_NUM_DYNAMIC_FRAME_IDS (3)
+
+struct ia_css_frames_sp {
+	struct ia_css_frame_sp	in;
+	struct ia_css_frame_sp	out[IA_CSS_BINARY_MAX_OUTPUT_PORTS];
+	struct ia_css_resolution effective_in_res;
+	struct ia_css_frame_sp	out_vf;
+	struct ia_css_frame_sp_info internal_frame_info;
+	struct ia_css_buffer_sp s3a_buf;
+	struct ia_css_buffer_sp dvs_buf;
+	struct ia_css_buffer_sp metadata_buf;
+};
+
+/* Information for a single pipeline stage for an ISP */
+struct sh_css_isp_stage {
+	/*
+	 * For compatibility and portabilty, only types
+	 * from "stdint.h" are allowed
+	 *
+	 * Use of "enum" and "bool" is prohibited
+	 * Multiple boolean flags can be stored in an
+	 * integer
+	 */
+	struct ia_css_blob_info	  blob_info;
+	struct ia_css_binary_info binary_info;
+	char			  binary_name[SH_CSS_MAX_BINARY_NAME];
+	struct ia_css_isp_param_css_segments mem_initializers;
+};
+
+/* Information for a single pipeline stage */
+struct sh_css_sp_stage {
+	/*
+	 * For compatibility and portabilty, only types
+	 * from "stdint.h" are allowed
+	 *
+	 * Use of "enum" and "bool" is prohibited
+	 * Multiple boolean flags can be stored in an
+	 * integer
+	 */
+	u8			num; /* Stage number */
+	u8			isp_online;
+	u8			isp_copy_vf;
+	u8			isp_copy_output;
+	u8			sp_enable_xnr;
+	u8			isp_deci_log_factor;
+	u8			isp_vf_downscale_bits;
+	u8			deinterleaved;
+	/*
+	 * NOTE: Programming the input circuit can only be done at the
+	 * start of a session. It is illegal to program it during execution
+	 * The input circuit defines the connectivity
+	 */
+	u8			program_input_circuit;
+	/* enum ia_css_pipeline_stage_sp_func	func; */
+	u8			func;
+	/* The type of the pipe-stage */
+	/* enum sh_css_stage_type	stage_type; */
+	u8			stage_type;
+	u8			num_stripes;
+	u8			isp_pipe_version;
+	struct {
+		u8		vf_output;
+		u8		s3a;
+		u8		sdis;
+		u8		dvs_stats;
+		u8		lace_stats;
+	} enable;
+	/* Add padding to come to a word boundary */
+	/* unsigned char			padding[0]; */
+
+	struct sh_css_crop_pos		sp_out_crop_pos;
+	struct ia_css_frames_sp		frames;
+	struct ia_css_resolution	dvs_envelope;
+	struct sh_css_uds_info		uds;
+	ia_css_ptr			isp_stage_addr;
+	ia_css_ptr			xmem_bin_addr;
+	ia_css_ptr			xmem_map_addr;
+
+	u16		top_cropping;
+	u16		row_stripes_height;
+	u16		row_stripes_overlap_lines;
+	u8			if_config_index; /* Which should be applied by this stage. */
+};
+
+/*
+ * Time: 2012-07-19, 17:40.
+ * Note: Add a new data memeber "debug" in "sh_css_sp_group". This
+ * data member is used to pass the debugging command from the
+ * Host to the SP.
+ *
+ * Time: Before 2012-07-19.
+ * Note:
+ * Group all host initialized SP variables into this struct.
+ * This is initialized every stage through dma.
+ * The stage part itself is transferred through sh_css_sp_stage.
+*/
+struct sh_css_sp_group {
+	struct sh_css_sp_config		config;
+	struct sh_css_sp_pipeline	pipe[SH_CSS_MAX_SP_THREADS];
+#if defined(ISP2401)
+	struct sh_css_sp_pipeline_io	pipe_io[SH_CSS_MAX_SP_THREADS];
+	struct sh_css_sp_pipeline_io_status	pipe_io_status;
+#endif
+	struct sh_css_sp_debug_command	debug;
+};
+
+/* Data in SP dmem that is set from the host every stage. */
+struct sh_css_sp_per_frame_data {
+	/* ddr address of sp_group and sp_stage */
+	ia_css_ptr			sp_group_addr;
+};
+
+#define SH_CSS_NUM_SDW_IRQS 3
+
+/* Output data from SP to css */
+struct sh_css_sp_output {
+	unsigned int			bin_copy_bytes_copied;
+#if SP_DEBUG != SP_DEBUG_NONE
+	struct sh_css_sp_debug_state	debug;
+#endif
+	unsigned int		sw_interrupt_value[SH_CSS_NUM_SDW_IRQS];
+};
+
+/**
+ * @brief Data structure for the circular buffer.
+ * The circular buffer is empty if "start == end". The
+ * circular buffer is full if "(end + 1) % size == start".
+ */
+/* Variable Sized Buffer Queue Elements */
+
+#define  IA_CSS_NUM_ELEMS_HOST2SP_BUFFER_QUEUE    6
+#define  IA_CSS_NUM_ELEMS_HOST2SP_PARAM_QUEUE    3
+#define  IA_CSS_NUM_ELEMS_HOST2SP_TAG_CMD_QUEUE  6
+
+/* sp-to-host queue is expected to be emptied in ISR since
+ * it is used instead of HW interrupts (due to HW design issue).
+ * We need one queue element per CSI port. */
+#define  IA_CSS_NUM_ELEMS_SP2HOST_ISYS_EVENT_QUEUE (2 * N_CSI_PORTS)
+/* The host-to-sp queue needs to allow for some delay
+ * in the emptying of this queue in the SP since there is no
+ * separate SP thread for this. */
+#define  IA_CSS_NUM_ELEMS_HOST2SP_ISYS_EVENT_QUEUE (2 * N_CSI_PORTS)
+
+#define  IA_CSS_NUM_ELEMS_HOST2SP_PSYS_EVENT_QUEUE    13
+#define  IA_CSS_NUM_ELEMS_SP2HOST_BUFFER_QUEUE        19
+#define  IA_CSS_NUM_ELEMS_SP2HOST_PSYS_EVENT_QUEUE    26 /* holds events for all type of buffers, hence deeper */
+
+struct sh_css_hmm_buffer {
+	union {
+		struct ia_css_isp_3a_statistics  s3a;
+		struct ia_css_isp_dvs_statistics dis;
+		ia_css_ptr skc_dvs_statistics;
+		ia_css_ptr lace_stat;
+		struct ia_css_metadata	metadata;
+		struct frame_data_wrapper {
+			ia_css_ptr	frame_data;
+			u32	flashed;
+			u32	exp_id;
+			u32	isp_parameters_id; /** Unique ID to track which config was
+								actually applied to a particular frame */
+		} frame;
+		ia_css_ptr ddr_ptrs;
+	} payload;
+	/*
+	 * kernel_ptr is present for host administration purposes only.
+	 * type is uint64_t in order to be 64-bit host compatible.
+	 * uint64_t does not exist on SP/ISP.
+	 * Size of the struct is checked by sp.hive.c.
+	 */
+	CSS_ALIGN(u64 cookie_ptr, 8); /* TODO: check if this alignment is needed */
+	u64 kernel_ptr;
+	struct ia_css_time_meas timing_data;
+	clock_value_t isys_eof_clock_tick;
+};
+
+#define SIZE_OF_FRAME_STRUCT						\
+	(SIZE_OF_HRT_VADDRESS +						\
+	(3 * sizeof(uint32_t)))
+
+#define SIZE_OF_PAYLOAD_UNION						\
+	(MAX(MAX(MAX(MAX(						\
+	SIZE_OF_IA_CSS_ISP_3A_STATISTICS_STRUCT,			\
+	SIZE_OF_IA_CSS_ISP_DVS_STATISTICS_STRUCT),			\
+	SIZE_OF_IA_CSS_METADATA_STRUCT),				\
+	SIZE_OF_FRAME_STRUCT),						\
+	SIZE_OF_HRT_VADDRESS))
+
+/* Do not use sizeof(uint64_t) since that does not exist of SP */
+#define SIZE_OF_SH_CSS_HMM_BUFFER_STRUCT				\
+	(SIZE_OF_PAYLOAD_UNION +					\
+	CALC_ALIGNMENT_MEMBER(SIZE_OF_PAYLOAD_UNION, 8) +		\
+	8 +						\
+	8 +						\
+	SIZE_OF_IA_CSS_TIME_MEAS_STRUCT +				\
+	SIZE_OF_IA_CSS_CLOCK_TICK_STRUCT +			\
+	CALC_ALIGNMENT_MEMBER(SIZE_OF_IA_CSS_CLOCK_TICK_STRUCT, 8))
+
+enum sh_css_queue_type {
+	sh_css_invalid_queue_type = -1,
+	sh_css_host2sp_buffer_queue,
+	sh_css_sp2host_buffer_queue,
+	sh_css_host2sp_psys_event_queue,
+	sh_css_sp2host_psys_event_queue,
+	sh_css_sp2host_isys_event_queue,
+	sh_css_host2sp_isys_event_queue,
+	sh_css_host2sp_tag_cmd_queue,
+};
+
+struct sh_css_event_irq_mask {
+	u16 or_mask;
+	u16 and_mask;
+};
+
+#define SIZE_OF_SH_CSS_EVENT_IRQ_MASK_STRUCT				\
+	(2 * sizeof(uint16_t))
+
+struct host_sp_communication {
+	/*
+	 * Don't use enum host2sp_commands, because the sizeof an enum is
+	 * compiler dependent and thus non-portable
+	 */
+	u32 host2sp_command;
+
+	/*
+	 * The frame buffers that are reused by the
+	 * copy pipe in the offline preview mode.
+	 *
+	 * host2sp_offline_frames[0]: the input frame of the preview pipe.
+	 * host2sp_offline_frames[1]: the output frame of the copy pipe.
+	 *
+	 * TODO:
+	 *   Remove it when the Host and the SP is decoupled.
+	 */
+	ia_css_ptr host2sp_offline_frames[NUM_CONTINUOUS_FRAMES];
+	ia_css_ptr host2sp_offline_metadata[NUM_CONTINUOUS_FRAMES];
+
+	ia_css_ptr host2sp_mipi_frames[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	ia_css_ptr host2sp_mipi_metadata[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	u32 host2sp_num_mipi_frames[N_CSI_PORTS];
+	u32 host2sp_cont_avail_num_raw_frames;
+	u32 host2sp_cont_extra_num_raw_frames;
+	u32 host2sp_cont_target_num_raw_frames;
+	struct sh_css_event_irq_mask host2sp_event_irq_mask[NR_OF_PIPELINES];
+
+};
+
+#define SIZE_OF_HOST_SP_COMMUNICATION_STRUCT				\
+	(sizeof(uint32_t) +						\
+	(NUM_CONTINUOUS_FRAMES * SIZE_OF_HRT_VADDRESS * 2) +		\
+	(N_CSI_PORTS * NUM_MIPI_FRAMES_PER_STREAM * SIZE_OF_HRT_VADDRESS * 2) +			\
+	((3 + N_CSI_PORTS) * sizeof(uint32_t)) +						\
+	(NR_OF_PIPELINES * SIZE_OF_SH_CSS_EVENT_IRQ_MASK_STRUCT))
+
+struct host_sp_queues {
+	/*
+	 * Queues for the dynamic frame information,
+	 * i.e. the "in_frame" buffer, the "out_frame"
+	 * buffer and the "vf_out_frame" buffer.
+	 */
+	ia_css_circbuf_desc_t host2sp_buffer_queues_desc
+	[SH_CSS_MAX_SP_THREADS][SH_CSS_MAX_NUM_QUEUES];
+	ia_css_circbuf_elem_t host2sp_buffer_queues_elems
+	[SH_CSS_MAX_SP_THREADS][SH_CSS_MAX_NUM_QUEUES]
+	[IA_CSS_NUM_ELEMS_HOST2SP_BUFFER_QUEUE];
+	ia_css_circbuf_desc_t sp2host_buffer_queues_desc
+	[SH_CSS_MAX_NUM_QUEUES];
+	ia_css_circbuf_elem_t sp2host_buffer_queues_elems
+	[SH_CSS_MAX_NUM_QUEUES][IA_CSS_NUM_ELEMS_SP2HOST_BUFFER_QUEUE];
+
+	/*
+	 * The queues for the events.
+	 */
+	ia_css_circbuf_desc_t host2sp_psys_event_queue_desc;
+
+	ia_css_circbuf_elem_t host2sp_psys_event_queue_elems
+	[IA_CSS_NUM_ELEMS_HOST2SP_PSYS_EVENT_QUEUE];
+	ia_css_circbuf_desc_t sp2host_psys_event_queue_desc;
+
+	ia_css_circbuf_elem_t sp2host_psys_event_queue_elems
+	[IA_CSS_NUM_ELEMS_SP2HOST_PSYS_EVENT_QUEUE];
+
+	/*
+	 * The queues for the ISYS events.
+	 */
+	ia_css_circbuf_desc_t host2sp_isys_event_queue_desc;
+
+	ia_css_circbuf_elem_t host2sp_isys_event_queue_elems
+	[IA_CSS_NUM_ELEMS_HOST2SP_ISYS_EVENT_QUEUE];
+	ia_css_circbuf_desc_t sp2host_isys_event_queue_desc;
+
+	ia_css_circbuf_elem_t sp2host_isys_event_queue_elems
+	[IA_CSS_NUM_ELEMS_SP2HOST_ISYS_EVENT_QUEUE];
+	/*
+	 * The queue for the tagger commands.
+	 * CHECK: are these last two present on the 2401 ?
+	 */
+	ia_css_circbuf_desc_t host2sp_tag_cmd_queue_desc;
+
+	ia_css_circbuf_elem_t host2sp_tag_cmd_queue_elems
+	[IA_CSS_NUM_ELEMS_HOST2SP_TAG_CMD_QUEUE];
+};
+
+#define SIZE_OF_QUEUES_ELEMS							\
+	(SIZE_OF_IA_CSS_CIRCBUF_ELEM_S_STRUCT *				\
+	((SH_CSS_MAX_SP_THREADS * SH_CSS_MAX_NUM_QUEUES * IA_CSS_NUM_ELEMS_HOST2SP_BUFFER_QUEUE) + \
+	(SH_CSS_MAX_NUM_QUEUES * IA_CSS_NUM_ELEMS_SP2HOST_BUFFER_QUEUE) +	\
+	(IA_CSS_NUM_ELEMS_HOST2SP_PSYS_EVENT_QUEUE) +				\
+	(IA_CSS_NUM_ELEMS_SP2HOST_PSYS_EVENT_QUEUE) +				\
+	(IA_CSS_NUM_ELEMS_HOST2SP_ISYS_EVENT_QUEUE) +				\
+	(IA_CSS_NUM_ELEMS_SP2HOST_ISYS_EVENT_QUEUE) +				\
+	(IA_CSS_NUM_ELEMS_HOST2SP_TAG_CMD_QUEUE)))
+
+#define IA_CSS_NUM_CIRCBUF_DESCS 5
+
+#define SIZE_OF_QUEUES_DESC \
+	((SH_CSS_MAX_SP_THREADS * SH_CSS_MAX_NUM_QUEUES * \
+	  SIZE_OF_IA_CSS_CIRCBUF_DESC_S_STRUCT) + \
+	 (SH_CSS_MAX_NUM_QUEUES * SIZE_OF_IA_CSS_CIRCBUF_DESC_S_STRUCT) + \
+	 (IA_CSS_NUM_CIRCBUF_DESCS * SIZE_OF_IA_CSS_CIRCBUF_DESC_S_STRUCT))
+
+#define SIZE_OF_HOST_SP_QUEUES_STRUCT		\
+	(SIZE_OF_QUEUES_ELEMS + SIZE_OF_QUEUES_DESC)
+
+extern int  __printf(1, 0) (*sh_css_printf)(const char *fmt, va_list args);
+
+static inline void  __printf(1, 2) sh_css_print(const char *fmt, ...)
+{
+	va_list ap;
+
+	if (sh_css_printf) {
+		va_start(ap, fmt);
+		sh_css_printf(fmt, ap);
+		va_end(ap);
+	}
+}
+
+static inline void  __printf(1, 0) sh_css_vprint(const char *fmt, va_list args)
+{
+	if (sh_css_printf)
+		sh_css_printf(fmt, args);
+}
+
+/* The following #if is there because this header file is also included
+   by SP and ISP code but they do not need this data and HIVECC has alignment
+   issue with the firmware struct/union's.
+   More permanent solution will be to refactor this include.
+*/
+ia_css_ptr sh_css_params_ddr_address_map(void);
+
+int
+sh_css_params_init(void);
+
+void
+sh_css_params_uninit(void);
+
+/* For Acceleration API: Flush FW (shared buffer pointer) arguments */
+void sh_css_flush(struct ia_css_acc_fw *fw);
+
+void
+sh_css_binary_args_reset(struct sh_css_binary_args *args);
+
+/* Check two frames for equality (format, resolution, bits per element) */
+bool
+sh_css_frame_equal_types(const struct ia_css_frame *frame_a,
+			 const struct ia_css_frame *frame_b);
+
+bool
+sh_css_frame_info_equal_resolution(const struct ia_css_frame_info *info_a,
+				   const struct ia_css_frame_info *info_b);
+
+void
+sh_css_capture_enable_bayer_downscaling(bool enable);
+
+void
+sh_css_binary_print(const struct ia_css_binary *binary);
+
+/* aligned argument of sh_css_frame_info_set_width can be used for an extra alignment requirement.
+  When 0, no extra alignment is done. */
+void
+sh_css_frame_info_set_width(struct ia_css_frame_info *info,
+			    unsigned int width,
+			    unsigned int aligned);
+
+#if !defined(ISP2401)
+
+unsigned int
+sh_css_get_mipi_sizes_for_check(const unsigned int port,
+				const unsigned int idx);
+
+#endif
+
+ia_css_ptr
+sh_css_store_sp_group_to_ddr(void);
+
+ia_css_ptr
+sh_css_store_sp_stage_to_ddr(unsigned int pipe, unsigned int stage);
+
+ia_css_ptr
+sh_css_store_isp_stage_to_ddr(unsigned int pipe, unsigned int stage);
+
+void
+sh_css_update_uds_and_crop_info(
+    const struct ia_css_binary_info *info,
+    const struct ia_css_frame_info *in_frame_info,
+    const struct ia_css_frame_info *out_frame_info,
+    const struct ia_css_resolution *dvs_env,
+    const struct ia_css_dz_config *zoom,
+    const struct ia_css_vector *motion_vector,
+    struct sh_css_uds_info *uds,		/* out */
+    struct sh_css_crop_pos *sp_out_crop_pos,	/* out */
+
+    bool enable_zoom
+);
+
+void
+sh_css_invalidate_shading_tables(struct ia_css_stream *stream);
+
+struct ia_css_pipeline *
+ia_css_pipe_get_pipeline(const struct ia_css_pipe *pipe);
+
+unsigned int
+ia_css_pipe_get_pipe_num(const struct ia_css_pipe *pipe);
+
+unsigned int
+ia_css_pipe_get_isp_pipe_version(const struct ia_css_pipe *pipe);
+
+bool
+sh_css_continuous_is_enabled(uint8_t pipe_num);
+
+struct ia_css_pipe *
+find_pipe_by_num(uint32_t pipe_num);
+
+#ifdef ISP2401
+void
+ia_css_get_crop_offsets(
+    struct ia_css_pipe *pipe,
+    struct ia_css_frame_info *in_frame);
+#endif
+
+#endif /* _SH_CSS_INTERNAL_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_legacy.h b/drivers/staging/media/atomisp/pci/sh_css_legacy.h
new file mode 100644
index 000000000..567c8d6dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_legacy.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_LEGACY_H_
+#define _SH_CSS_LEGACY_H_
+
+#include <type_support.h>
+#include <ia_css_err.h>
+#include <ia_css_types.h>
+#include <ia_css_frame_public.h>
+#include <ia_css_pipe_public.h>
+#include <ia_css_stream_public.h>
+
+/* The pipe id type, distinguishes the kind of pipes that
+ *  can be run in parallel.
+ */
+enum ia_css_pipe_id {
+	IA_CSS_PIPE_ID_PREVIEW,
+	IA_CSS_PIPE_ID_COPY,
+	IA_CSS_PIPE_ID_VIDEO,
+	IA_CSS_PIPE_ID_CAPTURE,
+	IA_CSS_PIPE_ID_YUVPP,
+	IA_CSS_PIPE_ID_ACC,
+	IA_CSS_PIPE_ID_NUM
+};
+
+struct ia_css_pipe_extra_config {
+	bool enable_raw_binning;
+	bool enable_yuv_ds;
+	bool enable_high_speed;
+	bool enable_dvs_6axis;
+	bool enable_reduced_pipe;
+	bool enable_fractional_ds;
+	bool disable_vf_pp;
+};
+
+int
+ia_css_pipe_create_extra(const struct ia_css_pipe_config *config,
+			 const struct ia_css_pipe_extra_config *extra_config,
+			 struct ia_css_pipe **pipe);
+
+void
+ia_css_pipe_extra_config_defaults(struct ia_css_pipe_extra_config
+				  *extra_config);
+
+int
+ia_css_temp_pipe_to_pipe_id(const struct ia_css_pipe *pipe,
+			    enum ia_css_pipe_id *pipe_id);
+
+/* DEPRECATED. FPN is not supported. */
+int
+sh_css_set_black_frame(struct ia_css_stream *stream,
+		       const struct ia_css_frame *raw_black_frame);
+
+/* ISP2400 */
+void
+sh_css_enable_cont_capt(bool enable, bool stop_copy_preview);
+
+#endif /* _SH_CSS_LEGACY_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_metrics.c b/drivers/staging/media/atomisp/pci/sh_css_metrics.c
new file mode 100644
index 000000000..8ded6cdd1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_metrics.c
@@ -0,0 +1,154 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "assert_support.h"
+#include "sh_css_metrics.h"
+
+#include "sp.h"
+#include "isp.h"
+
+#include "sh_css_internal.h"
+
+#define MULTIPLE_PCS 0
+#define SUSPEND      0
+#define NOF_PCS      1
+#define RESUME_MASK  0x8
+#define STOP_MASK    0x0
+
+static bool pc_histogram_enabled;
+static struct sh_css_pc_histogram *isp_histogram;
+static struct sh_css_pc_histogram *sp_histogram;
+
+struct sh_css_metrics sh_css_metrics;
+
+void
+sh_css_metrics_start_frame(void)
+{
+	sh_css_metrics.frame_metrics.num_frames++;
+}
+
+static void
+clear_histogram(struct sh_css_pc_histogram *histogram)
+{
+	unsigned int i;
+
+	assert(histogram);
+
+	for (i = 0; i < histogram->length; i++) {
+		histogram->run[i] = 0;
+		histogram->stall[i] = 0;
+		histogram->msink[i] = 0xFFFF;
+	}
+}
+
+void
+sh_css_metrics_enable_pc_histogram(bool enable)
+{
+	pc_histogram_enabled = enable;
+}
+
+static void
+make_histogram(struct sh_css_pc_histogram *histogram, unsigned int length)
+{
+	assert(histogram);
+
+	if (histogram->length)
+		return;
+	if (histogram->run)
+		return;
+	histogram->run = kvmalloc(length * sizeof(*histogram->run),
+				  GFP_KERNEL);
+	if (!histogram->run)
+		return;
+	histogram->stall = kvmalloc(length * sizeof(*histogram->stall),
+				    GFP_KERNEL);
+	if (!histogram->stall)
+		return;
+	histogram->msink = kvmalloc(length * sizeof(*histogram->msink),
+				    GFP_KERNEL);
+	if (!histogram->msink)
+		return;
+
+	histogram->length = length;
+	clear_histogram(histogram);
+}
+
+static void
+insert_binary_metrics(struct sh_css_binary_metrics **l,
+		      struct sh_css_binary_metrics *metrics)
+{
+	assert(l);
+	assert(*l);
+	assert(metrics);
+
+	for (; *l; l = &(*l)->next)
+		if (*l == metrics)
+			return;
+
+	*l = metrics;
+	metrics->next = NULL;
+}
+
+void
+sh_css_metrics_start_binary(struct sh_css_binary_metrics *metrics)
+{
+	assert(metrics);
+
+	if (!pc_histogram_enabled)
+		return;
+
+	isp_histogram = &metrics->isp_histogram;
+	sp_histogram = &metrics->sp_histogram;
+	make_histogram(isp_histogram, ISP_PMEM_DEPTH);
+	make_histogram(sp_histogram, SP_PMEM_DEPTH);
+	insert_binary_metrics(&sh_css_metrics.binary_metrics, metrics);
+}
+
+void
+sh_css_metrics_sample_pcs(void)
+{
+	bool stall;
+	unsigned int pc;
+	unsigned int msink;
+
+
+
+	if (!pc_histogram_enabled)
+		return;
+
+	if (isp_histogram) {
+		msink = isp_ctrl_load(ISP0_ID, ISP_CTRL_SINK_REG);
+		pc = isp_ctrl_load(ISP0_ID, ISP_PC_REG);
+
+		isp_histogram->msink[pc] &= msink;
+		stall = (msink != 0x7FF);
+
+		if (stall)
+			isp_histogram->stall[pc]++;
+		else
+			isp_histogram->run[pc]++;
+	}
+
+	if (sp_histogram && 0) {
+		msink = sp_ctrl_load(SP0_ID, SP_CTRL_SINK_REG);
+		pc = sp_ctrl_load(SP0_ID, SP_PC_REG);
+		sp_histogram->msink[pc] &= msink;
+		stall = (msink != 0x7FF);
+		if (stall)
+			sp_histogram->stall[pc]++;
+		else
+			sp_histogram->run[pc]++;
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_metrics.h b/drivers/staging/media/atomisp/pci/sh_css_metrics.h
new file mode 100644
index 000000000..f4bcd0838
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_metrics.h
@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_METRICS_H_
+#define _SH_CSS_METRICS_H_
+
+#include <type_support.h>
+
+struct sh_css_pc_histogram {
+	unsigned int length;
+	unsigned int *run;
+	unsigned int *stall;
+	unsigned int *msink;
+};
+
+struct sh_css_binary_metrics {
+	unsigned int mode;
+	unsigned int id;
+	struct sh_css_pc_histogram isp_histogram;
+	struct sh_css_pc_histogram sp_histogram;
+	struct sh_css_binary_metrics *next;
+};
+
+struct ia_css_frame_metrics {
+	unsigned int num_frames;
+};
+
+struct sh_css_metrics {
+	struct sh_css_binary_metrics *binary_metrics;
+	struct ia_css_frame_metrics   frame_metrics;
+};
+
+extern struct sh_css_metrics sh_css_metrics;
+
+/* includes ia_css_binary.h, which depends on sh_css_metrics.h */
+#include "ia_css_types.h"
+
+/* Sample ISP and SP pc and add to histogram */
+void sh_css_metrics_enable_pc_histogram(bool enable);
+void sh_css_metrics_start_frame(void);
+void sh_css_metrics_start_binary(struct sh_css_binary_metrics *metrics);
+void sh_css_metrics_sample_pcs(void);
+
+#endif /* _SH_CSS_METRICS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_mipi.c b/drivers/staging/media/atomisp/pci/sh_css_mipi.c
new file mode 100644
index 000000000..bc6e8598a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_mipi.c
@@ -0,0 +1,632 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_mipi.h"
+#include "sh_css_mipi.h"
+#include <type_support.h>
+#include "system_global.h"
+#include "ia_css_err.h"
+#include "ia_css_pipe.h"
+#include "ia_css_stream_format.h"
+#include "sh_css_stream_format.h"
+#include "ia_css_stream_public.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_input_port.h"
+#include "ia_css_debug.h"
+#include "sh_css_struct.h"
+#include "sh_css_defs.h"
+#include "sh_css_sp.h" /* sh_css_update_host2sp_mipi_frame sh_css_update_host2sp_num_mipi_frames ... */
+#include "sw_event_global.h" /* IA_CSS_PSYS_SW_EVENT_MIPI_BUFFERS_READY */
+
+static u32
+ref_count_mipi_allocation[N_CSI_PORTS]; /* Initialized in mipi_init */
+
+/* Assumptions:
+ *	- A line is multiple of 4 bytes = 1 word.
+ *	- Each frame has SOF and EOF (each 1 word).
+ *	- Each line has format header and optionally SOL and EOL (each 1 word).
+ *	- Odd and even lines of YUV420 format are different in bites per pixel size.
+ *	- Custom size of embedded data.
+ *  -- Interleaved frames are not taken into account.
+ *  -- Lines are multiples of 8B, and not necessary of (custom 3B, or 7B
+ *  etc.).
+ * Result is given in DDR mem words, 32B or 256 bits
+ */
+int
+ia_css_mipi_frame_calculate_size(const unsigned int width,
+				 const unsigned int height,
+				 const enum atomisp_input_format format,
+				 const bool hasSOLandEOL,
+				 const unsigned int embedded_data_size_words,
+				 unsigned int *size_mem_words)
+{
+	int err = 0;
+
+	unsigned int bits_per_pixel = 0;
+	unsigned int even_line_bytes = 0;
+	unsigned int odd_line_bytes = 0;
+	unsigned int words_per_odd_line = 0;
+	unsigned int words_for_first_line = 0;
+	unsigned int words_per_even_line = 0;
+	unsigned int mem_words_per_even_line = 0;
+	unsigned int mem_words_per_odd_line = 0;
+	unsigned int mem_words_for_first_line = 0;
+	unsigned int mem_words_for_EOF = 0;
+	unsigned int mem_words = 0;
+	unsigned int width_padded = width;
+
+#if defined(ISP2401)
+	/* The changes will be reverted as soon as RAW
+	 * Buffers are deployed by the 2401 Input System
+	 * in the non-continuous use scenario.
+	 */
+	width_padded += (2 * ISP_VEC_NELEMS);
+#endif
+
+	IA_CSS_ENTER("padded_width=%d, height=%d, format=%d, hasSOLandEOL=%d, embedded_data_size_words=%d\n",
+		     width_padded, height, format, hasSOLandEOL, embedded_data_size_words);
+
+	switch (format) {
+	case ATOMISP_INPUT_FORMAT_RAW_6:		/* 4p, 3B, 24bits */
+		bits_per_pixel = 6;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_7:		/* 8p, 7B, 56bits */
+		bits_per_pixel = 7;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_8:		/* 1p, 1B, 8bits */
+	case ATOMISP_INPUT_FORMAT_BINARY_8:		/*  8bits, TODO: check. */
+	case ATOMISP_INPUT_FORMAT_YUV420_8:		/* odd 2p, 2B, 16bits, even 2p, 4B, 32bits */
+		bits_per_pixel = 8;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_10:		/* odd 4p, 5B, 40bits, even 4p, 10B, 80bits */
+	case ATOMISP_INPUT_FORMAT_RAW_10:		/* 4p, 5B, 40bits */
+		/* The changes will be reverted as soon as RAW
+		 * Buffers are deployed by the 2401 Input System
+		 * in the non-continuous use scenario.
+		 */
+		bits_per_pixel = 10;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:	/* 2p, 3B, 24bits */
+	case ATOMISP_INPUT_FORMAT_RAW_12:		/* 2p, 3B, 24bits */
+		bits_per_pixel = 12;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_14:		/* 4p, 7B, 56bits */
+		bits_per_pixel = 14;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_444:		/* 1p, 2B, 16bits */
+	case ATOMISP_INPUT_FORMAT_RGB_555:		/* 1p, 2B, 16bits */
+	case ATOMISP_INPUT_FORMAT_RGB_565:		/* 1p, 2B, 16bits */
+	case ATOMISP_INPUT_FORMAT_YUV422_8:		/* 2p, 4B, 32bits */
+		bits_per_pixel = 16;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_666:		/* 4p, 9B, 72bits */
+		bits_per_pixel = 18;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV422_10:		/* 2p, 5B, 40bits */
+		bits_per_pixel = 20;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_888:		/* 1p, 3B, 24bits */
+		bits_per_pixel = 24;
+		break;
+
+	case ATOMISP_INPUT_FORMAT_YUV420_16:		/* Not supported */
+	case ATOMISP_INPUT_FORMAT_YUV422_16:		/* Not supported */
+	case ATOMISP_INPUT_FORMAT_RAW_16:		/* TODO: not specified in MIPI SPEC, check */
+	default:
+		return -EINVAL;
+	}
+
+	odd_line_bytes = (width_padded * bits_per_pixel + 7) >> 3; /* ceil ( bits per line / 8) */
+
+	/* Even lines for YUV420 formats are double in bits_per_pixel. */
+	if (format == ATOMISP_INPUT_FORMAT_YUV420_8
+	    || format == ATOMISP_INPUT_FORMAT_YUV420_10
+	    || format == ATOMISP_INPUT_FORMAT_YUV420_16) {
+		even_line_bytes = (width_padded * 2 * bits_per_pixel + 7) >>
+			3; /* ceil ( bits per line / 8) */
+	} else {
+		even_line_bytes = odd_line_bytes;
+	}
+
+	/*  a frame represented in memory:  ()- optional; data - payload words.
+	*  addr		0	1	2	3	4	5	6	7:
+	*  first	SOF	(SOL)	PACK_H	data	data	data	data	data
+	*		data	data	data	data	data	data	data	data
+	*		...
+	*		data	data	0	0	0	0	0	0
+	*  second	(EOL)	(SOL)	PACK_H	data	data	data	data	data
+	*		data	data	data	data	data	data	data	data
+	*		...
+	*		data	data	0	0	0	0	0	0
+	*  ...
+	*  last		(EOL)	EOF	0	0	0	0	0	0
+	*
+	*  Embedded lines are regular lines stored before the first and after
+	*  payload lines.
+	*/
+
+	words_per_odd_line = (odd_line_bytes + 3) >> 2;
+	/* ceil(odd_line_bytes/4); word = 4 bytes */
+	words_per_even_line  = (even_line_bytes  + 3) >> 2;
+	words_for_first_line = words_per_odd_line + 2 + (hasSOLandEOL ? 1 : 0);
+	/* + SOF +packet header + optionally (SOL), but (EOL) is not in the first line */
+	words_per_odd_line	+= (1 + (hasSOLandEOL ? 2 : 0));
+	/* each non-first line has format header, and optionally (SOL) and (EOL). */
+	words_per_even_line += (1 + (hasSOLandEOL ? 2 : 0));
+
+	mem_words_per_odd_line	 = (words_per_odd_line + 7) >> 3;
+	/* ceil(words_per_odd_line/8); mem_word = 32 bytes, 8 words */
+	mem_words_for_first_line = (words_for_first_line + 7) >> 3;
+	mem_words_per_even_line  = (words_per_even_line + 7) >> 3;
+	mem_words_for_EOF        = 1; /* last line consisit of the optional (EOL) and EOF */
+
+	mem_words = ((embedded_data_size_words + 7) >> 3) +
+	mem_words_for_first_line +
+	(((height + 1) >> 1) - 1) * mem_words_per_odd_line +
+	/* ceil (height/2) - 1 (first line is calculated separatelly) */
+	(height      >> 1) * mem_words_per_even_line + /* floor(height/2) */
+	mem_words_for_EOF;
+
+	*size_mem_words = mem_words; /* ceil(words/8); mem word is 32B = 8words. */
+	/* Check if the above is still needed. */
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+/*
+ * Check if a source port or TPG/PRBS ID is valid
+ */
+
+#if !defined(ISP2401)
+int
+ia_css_mipi_frame_enable_check_on_size(const enum mipi_port_id port,
+				       const unsigned int	size_mem_words)
+{
+	u32 idx;
+
+	int err = -EBUSY;
+
+	OP___assert(port < N_CSI_PORTS);
+	OP___assert(size_mem_words != 0);
+
+	for (idx = 0; idx < IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT &&
+	     my_css.mipi_sizes_for_check[port][idx] != 0;
+	     idx++) { /* do nothing */
+	}
+	if (idx < IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT) {
+		my_css.mipi_sizes_for_check[port][idx] = size_mem_words;
+		err = 0;
+	}
+
+	return err;
+}
+#endif
+
+void
+mipi_init(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < N_CSI_PORTS; i++)
+		ref_count_mipi_allocation[i] = 0;
+}
+
+bool mipi_is_free(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < N_CSI_PORTS; i++)
+		if (ref_count_mipi_allocation[i])
+			return false;
+
+	return true;
+}
+
+#if defined(ISP2401)
+/*
+ * @brief Calculate the required MIPI buffer sizes.
+ * Based on the stream configuration, calculate the
+ * required MIPI buffer sizes (in DDR words).
+ *
+ * @param[in]   stream_cfg              Point to the target stream configuration
+ * @param[out]  size_mem_words  MIPI buffer size in DDR words.
+ *
+ * @return
+ */
+static int calculate_mipi_buff_size(struct ia_css_stream_config *stream_cfg,
+				    unsigned int *size_mem_words)
+{
+	unsigned int width;
+	unsigned int height;
+	enum atomisp_input_format format;
+	bool pack_raw_pixels;
+
+	unsigned int width_padded;
+	unsigned int bits_per_pixel = 0;
+
+	unsigned int even_line_bytes = 0;
+	unsigned int odd_line_bytes = 0;
+
+	unsigned int words_per_odd_line = 0;
+	unsigned int words_per_even_line = 0;
+
+	unsigned int mem_words_per_even_line = 0;
+	unsigned int mem_words_per_odd_line = 0;
+
+	unsigned int mem_words_per_buff_line = 0;
+	unsigned int mem_words_per_buff = 0;
+	int err = 0;
+
+	/**
+	 * zhengjie.lu@intel.com
+	 *
+	 * NOTE
+	 * - In the struct "ia_css_stream_config", there
+	 *   are two members: "input_config" and "isys_config".
+	 *   Both of them provide the same information, e.g.
+	 *   input_res and format.
+	 *
+	 *   Question here is that: which one shall be used?
+	 */
+	width = stream_cfg->input_config.input_res.width;
+	height = stream_cfg->input_config.input_res.height;
+	format = stream_cfg->input_config.format;
+	pack_raw_pixels = stream_cfg->pack_raw_pixels;
+	/* end of NOTE */
+
+	/**
+	 * zhengjie.lu@intel.com
+	 *
+	 * NOTE
+	 * - The following code is derived from the
+	 *   existing code "ia_css_mipi_frame_calculate_size()".
+	 *
+	 *   Question here is: why adding "2 * ISP_VEC_NELEMS"
+	 *   to "width_padded", but not making "width_padded"
+	 *   aligned with "2 * ISP_VEC_NELEMS"?
+	 */
+	/* The changes will be reverted as soon as RAW
+	 * Buffers are deployed by the 2401 Input System
+	 * in the non-continuous use scenario.
+	 */
+	width_padded = width + (2 * ISP_VEC_NELEMS);
+	/* end of NOTE */
+
+	IA_CSS_ENTER("padded_width=%d, height=%d, format=%d\n",
+		     width_padded, height, format);
+
+	bits_per_pixel = sh_css_stream_format_2_bits_per_subpixel(format);
+	bits_per_pixel =
+	(format == ATOMISP_INPUT_FORMAT_RAW_10 && pack_raw_pixels) ? bits_per_pixel : 16;
+	if (bits_per_pixel == 0)
+		return -EINVAL;
+
+	odd_line_bytes = (width_padded * bits_per_pixel + 7) >> 3; /* ceil ( bits per line / 8) */
+
+	/* Even lines for YUV420 formats are double in bits_per_pixel. */
+	if (format == ATOMISP_INPUT_FORMAT_YUV420_8
+	    || format == ATOMISP_INPUT_FORMAT_YUV420_10) {
+		even_line_bytes = (width_padded * 2 * bits_per_pixel + 7) >>
+			3; /* ceil ( bits per line / 8) */
+	} else {
+		even_line_bytes = odd_line_bytes;
+	}
+
+	words_per_odd_line	 = (odd_line_bytes   + 3) >> 2;
+	/* ceil(odd_line_bytes/4); word = 4 bytes */
+	words_per_even_line  = (even_line_bytes  + 3) >> 2;
+
+	mem_words_per_odd_line	 = (words_per_odd_line + 7) >> 3;
+	/* ceil(words_per_odd_line/8); mem_word = 32 bytes, 8 words */
+	mem_words_per_even_line  = (words_per_even_line + 7) >> 3;
+
+	mem_words_per_buff_line =
+	(mem_words_per_odd_line > mem_words_per_even_line) ? mem_words_per_odd_line : mem_words_per_even_line;
+	mem_words_per_buff = mem_words_per_buff_line * height;
+
+	*size_mem_words = mem_words_per_buff;
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+#endif
+
+int
+allocate_mipi_frames(struct ia_css_pipe *pipe,
+		     struct ia_css_stream_info *info)
+{
+	int err = -EINVAL;
+	unsigned int port;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "allocate_mipi_frames(%p) enter:\n", pipe);
+
+	assert(pipe);
+	assert(pipe->stream);
+	if ((!pipe) || (!pipe->stream)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "allocate_mipi_frames(%p) exit: pipe or stream is null.\n",
+				    pipe);
+		return -EINVAL;
+	}
+
+#ifdef ISP2401
+	if (pipe->stream->config.online) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "allocate_mipi_frames(%p) exit: no buffers needed for 2401 pipe mode.\n",
+				    pipe);
+		return 0;
+	}
+
+#endif
+	if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "allocate_mipi_frames(%p) exit: no buffers needed for pipe mode.\n",
+				    pipe);
+		return 0; /* AM TODO: Check  */
+	}
+
+	port = (unsigned int)pipe->stream->config.source.port.port;
+	if (port >= N_CSI_PORTS) {
+		IA_CSS_ERROR("allocate_mipi_frames(%p) exit: port is not correct (port=%d).",
+			     pipe, port);
+		return -EINVAL;
+	}
+
+#ifdef ISP2401
+	err = calculate_mipi_buff_size(&pipe->stream->config,
+				       &my_css.mipi_frame_size[port]);
+	/*
+	 * 2401 system allows multiple streams to use same physical port. This is not
+	 * true for 2400 system. Currently 2401 uses MIPI buffers as a temporary solution.
+	 * TODO AM: Once that is changed (removed) this code should be removed as well.
+	 * In that case only 2400 related code should remain.
+	 */
+	if (ref_count_mipi_allocation[port] != 0) {
+		ref_count_mipi_allocation[port]++;
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "allocate_mipi_frames(%p) leave: nothing to do, already allocated for this port (port=%d).\n",
+				    pipe, port);
+		return 0;
+	}
+#else
+	if (ref_count_mipi_allocation[port] != 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				    "allocate_mipi_frames(%p) exit: already allocated for this port (port=%d).\n",
+				    pipe, port);
+		return 0;
+	}
+#endif
+
+	ref_count_mipi_allocation[port]++;
+
+	/* AM TODO: mipi frames number should come from stream struct. */
+	my_css.num_mipi_frames[port] = NUM_MIPI_FRAMES_PER_STREAM;
+
+	/* Incremental allocation (per stream), not for all streams at once. */
+	{ /* limit the scope of i,j */
+		unsigned int i, j;
+
+		for (i = 0; i < my_css.num_mipi_frames[port]; i++) {
+			/* free previous frame */
+			if (my_css.mipi_frames[port][i]) {
+				ia_css_frame_free(my_css.mipi_frames[port][i]);
+				my_css.mipi_frames[port][i] = NULL;
+			}
+			/* check if new frame is needed */
+			if (i < my_css.num_mipi_frames[port]) {
+				/* allocate new frame */
+				err = ia_css_frame_allocate_with_buffer_size(
+					  &my_css.mipi_frames[port][i],
+					  my_css.mipi_frame_size[port] * HIVE_ISP_DDR_WORD_BYTES);
+				if (err) {
+					for (j = 0; j < i; j++) {
+						if (my_css.mipi_frames[port][j]) {
+							ia_css_frame_free(my_css.mipi_frames[port][j]);
+							my_css.mipi_frames[port][j] = NULL;
+						}
+					}
+					IA_CSS_ERROR("allocate_mipi_frames(%p, %d) exit: allocation failed.",
+						     pipe, port);
+					return err;
+				}
+			}
+			if (info->metadata_info.size > 0) {
+				/* free previous metadata buffer */
+				if (my_css.mipi_metadata[port][i]) {
+					ia_css_metadata_free(my_css.mipi_metadata[port][i]);
+					my_css.mipi_metadata[port][i] = NULL;
+				}
+				/* check if need to allocate a new metadata buffer */
+				if (i < my_css.num_mipi_frames[port]) {
+					/* allocate new metadata buffer */
+					my_css.mipi_metadata[port][i] = ia_css_metadata_allocate(&info->metadata_info);
+					if (!my_css.mipi_metadata[port][i]) {
+						ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+								    "allocate_mipi_metadata(%p, %d) failed.\n",
+								    pipe, port);
+						return err;
+					}
+				}
+			}
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "allocate_mipi_frames(%p) exit:\n", pipe);
+
+	return err;
+}
+
+int
+free_mipi_frames(struct ia_css_pipe *pipe)
+{
+	int err = -EINVAL;
+	unsigned int port;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "free_mipi_frames(%p) enter:\n", pipe);
+
+	/* assert(pipe != NULL); TEMP: TODO: Should be assert only. */
+	if (pipe) {
+		assert(pipe->stream);
+		if ((!pipe) || (!pipe->stream)) {
+			IA_CSS_ERROR("free_mipi_frames(%p) exit: pipe or stream is null.",
+				     pipe);
+			return -EINVAL;
+		}
+
+		if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+			IA_CSS_ERROR("free_mipi_frames(%p) exit: wrong mode.",
+				     pipe);
+			return err;
+		}
+
+		port = (unsigned int)pipe->stream->config.source.port.port;
+
+		if (port >= N_CSI_PORTS) {
+			IA_CSS_ERROR("free_mipi_frames(%p, %d) exit: pipe port is not correct.",
+				     pipe, port);
+			return err;
+		}
+
+		if (ref_count_mipi_allocation[port] > 0) {
+#if !defined(ISP2401)
+			assert(ref_count_mipi_allocation[port] == 1);
+			if (ref_count_mipi_allocation[port] != 1) {
+				IA_CSS_ERROR("free_mipi_frames(%p) exit: wrong ref_count (ref_count=%d).",
+					     pipe, ref_count_mipi_allocation[port]);
+				return err;
+			}
+#endif
+
+			ref_count_mipi_allocation[port]--;
+
+			if (ref_count_mipi_allocation[port] == 0) {
+				/* no streams are using this buffer, so free it */
+				unsigned int i;
+
+				for (i = 0; i < my_css.num_mipi_frames[port]; i++) {
+					if (my_css.mipi_frames[port][i]) {
+						ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+								    "free_mipi_frames(port=%d, num=%d).\n", port, i);
+						ia_css_frame_free(my_css.mipi_frames[port][i]);
+						my_css.mipi_frames[port][i] = NULL;
+					}
+					if (my_css.mipi_metadata[port][i]) {
+						ia_css_metadata_free(my_css.mipi_metadata[port][i]);
+						my_css.mipi_metadata[port][i] = NULL;
+					}
+				}
+
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+						    "free_mipi_frames(%p) exit (deallocated).\n", pipe);
+			}
+#if defined(ISP2401)
+			else {
+				/* 2401 system allows multiple streams to use same physical port. This is not
+				 * true for 2400 system. Currently 2401 uses MIPI buffers as a temporary solution.
+				 * TODO AM: Once that is changed (removed) this code should be removed as well.
+				 * In that case only 2400 related code should remain.
+				 */
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+						    "free_mipi_frames(%p) leave: nothing to do, other streams still use this port (port=%d).\n",
+						    pipe, port);
+			}
+#endif
+		}
+	} else { /* pipe ==NULL */
+		/* AM TEMP: free-ing all mipi buffers just like a legacy code. */
+		for (port = CSI_PORT0_ID; port < N_CSI_PORTS; port++) {
+			unsigned int i;
+
+			for (i = 0; i < my_css.num_mipi_frames[port]; i++) {
+				if (my_css.mipi_frames[port][i]) {
+					ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+							    "free_mipi_frames(port=%d, num=%d).\n", port, i);
+					ia_css_frame_free(my_css.mipi_frames[port][i]);
+					my_css.mipi_frames[port][i] = NULL;
+				}
+				if (my_css.mipi_metadata[port][i]) {
+					ia_css_metadata_free(my_css.mipi_metadata[port][i]);
+					my_css.mipi_metadata[port][i] = NULL;
+				}
+			}
+			ref_count_mipi_allocation[port] = 0;
+		}
+	}
+	return 0;
+}
+
+int
+send_mipi_frames(struct ia_css_pipe *pipe)
+{
+	int err = -EINVAL;
+	unsigned int i;
+	unsigned int port;
+
+	IA_CSS_ENTER_PRIVATE("pipe=%p", pipe);
+
+	assert(pipe);
+	assert(pipe->stream);
+	if (!pipe || !pipe->stream) {
+		IA_CSS_ERROR("pipe or stream is null");
+		return -EINVAL;
+	}
+
+	/* multi stream video needs mipi buffers */
+	/* nothing to be done in other cases. */
+	if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		IA_CSS_LOG("nothing to be done for this mode");
+		return 0;
+		/* TODO: AM: maybe this should be returning an error. */
+	}
+
+	port = (unsigned int)pipe->stream->config.source.port.port;
+
+	if (port >= N_CSI_PORTS) {
+		IA_CSS_ERROR("send_mipi_frames(%p) exit: invalid port specified (port=%d).",
+			     pipe, port);
+		return err;
+	}
+
+	/* Hand-over the SP-internal mipi buffers */
+	for (i = 0; i < my_css.num_mipi_frames[port]; i++) {
+		/* Need to include the ofset for port. */
+		sh_css_update_host2sp_mipi_frame(port * NUM_MIPI_FRAMES_PER_STREAM + i,
+						 my_css.mipi_frames[port][i]);
+		sh_css_update_host2sp_mipi_metadata(port * NUM_MIPI_FRAMES_PER_STREAM + i,
+						    my_css.mipi_metadata[port][i]);
+	}
+	sh_css_update_host2sp_num_mipi_frames(my_css.num_mipi_frames[port]);
+
+	/**********************************
+	 * Send an event to inform the SP
+	 * that all MIPI frames are passed.
+	 **********************************/
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		IA_CSS_ERROR("sp is not running");
+		return err;
+	}
+
+	ia_css_bufq_enqueue_psys_event(
+	    IA_CSS_PSYS_SW_EVENT_MIPI_BUFFERS_READY,
+	    (uint8_t)port,
+	    (uint8_t)my_css.num_mipi_frames[port],
+	    0 /* not used */);
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_mipi.h b/drivers/staging/media/atomisp/pci/sh_css_mipi.h
new file mode 100644
index 000000000..e6c86d0ac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_mipi.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_MIPI_H
+#define __SH_CSS_MIPI_H
+
+#include <ia_css_err.h>		  /* ia_css_err */
+#include <ia_css_types.h>	  /* ia_css_pipe */
+#include <ia_css_stream_public.h> /* ia_css_stream_config */
+
+void
+mipi_init(void);
+
+bool mipi_is_free(void);
+
+int
+allocate_mipi_frames(struct ia_css_pipe *pipe, struct ia_css_stream_info *info);
+
+int
+free_mipi_frames(struct ia_css_pipe *pipe);
+
+int
+send_mipi_frames(struct ia_css_pipe *pipe);
+
+#endif /* __SH_CSS_MIPI_H */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_mmu.c b/drivers/staging/media/atomisp/pci/sh_css_mmu.c
new file mode 100644
index 000000000..1da7459ea
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_mmu.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_mmu.h"
+#include "ia_css_mmu_private.h"
+#include <ia_css_debug.h>
+#include "sh_css_sp.h"
+#include "sh_css_firmware.h"
+#include "sp.h"
+#include "mmu_device.h"
+
+void
+ia_css_mmu_invalidate_cache(void)
+{
+	const struct ia_css_fw_info *fw = &sh_css_sp_fw;
+	unsigned int HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_mmu_invalidate_cache() enter\n");
+
+	/* if the SP is not running we should not access its dmem */
+	if (sh_css_sp_is_running()) {
+		HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb = fw->info.sp.invalidate_tlb;
+
+		(void)HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb; /* Suppres warnings in CRUN */
+
+		sp_dmem_store_uint32(SP0_ID,
+				     (unsigned int)sp_address_of(ia_css_dmaproxy_sp_invalidate_tlb),
+				     true);
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_mmu_invalidate_cache() leave\n");
+}
+
+void
+sh_css_mmu_set_page_table_base_index(hrt_data base_index)
+{
+	int i;
+
+	IA_CSS_ENTER_PRIVATE("base_index=0x%08x\n", base_index);
+	for (i = 0; i < N_MMU_ID; i++) {
+		mmu_ID_t mmu_id = i;
+
+		mmu_set_page_table_base_index(mmu_id, base_index);
+		mmu_invalidate_cache(mmu_id);
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_param_dvs.c b/drivers/staging/media/atomisp/pci/sh_css_param_dvs.c
new file mode 100644
index 000000000..ff0082d02
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_param_dvs.c
@@ -0,0 +1,286 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "sh_css_param_dvs.h"
+#include <assert_support.h>
+#include <type_support.h>
+#include <ia_css_err.h>
+#include <ia_css_types.h>
+#include "ia_css_debug.h"
+
+static struct ia_css_dvs_6axis_config *
+alloc_dvs_6axis_table(const struct ia_css_resolution *frame_res,
+		      struct ia_css_dvs_6axis_config  *dvs_config_src)
+{
+	unsigned int width_y = 0;
+	unsigned int height_y = 0;
+	unsigned int width_uv = 0;
+	unsigned int height_uv = 0;
+	int err = 0;
+	struct ia_css_dvs_6axis_config  *dvs_config = NULL;
+
+	dvs_config = kvmalloc(sizeof(struct ia_css_dvs_6axis_config),
+			      GFP_KERNEL);
+	if (!dvs_config)	{
+		IA_CSS_ERROR("out of memory");
+		err = -ENOMEM;
+	} else {
+		/*Initialize new struct with latest config settings*/
+		if (dvs_config_src) {
+			dvs_config->width_y = width_y = dvs_config_src->width_y;
+			dvs_config->height_y = height_y = dvs_config_src->height_y;
+			dvs_config->width_uv = width_uv = dvs_config_src->width_uv;
+			dvs_config->height_uv = height_uv = dvs_config_src->height_uv;
+			IA_CSS_LOG("alloc_dvs_6axis_table Y: W %d H %d", width_y, height_y);
+		} else if (frame_res) {
+			dvs_config->width_y = width_y = DVS_TABLE_IN_BLOCKDIM_X_LUMA(frame_res->width);
+			dvs_config->height_y = height_y = DVS_TABLE_IN_BLOCKDIM_Y_LUMA(
+							      frame_res->height);
+			dvs_config->width_uv = width_uv = DVS_TABLE_IN_BLOCKDIM_X_CHROMA(
+							      frame_res->width /
+							      2); /* UV = Y/2, depens on colour format YUV 4.2.0*/
+			dvs_config->height_uv = height_uv = DVS_TABLE_IN_BLOCKDIM_Y_CHROMA(
+								frame_res->height /
+								2);/* UV = Y/2, depens on colour format YUV 4.2.0*/
+			IA_CSS_LOG("alloc_dvs_6axis_table Y: W %d H %d", width_y, height_y);
+		}
+
+		/* Generate Y buffers  */
+		dvs_config->xcoords_y = kvmalloc(width_y * height_y * sizeof(uint32_t),
+						 GFP_KERNEL);
+		if (!dvs_config->xcoords_y) {
+			IA_CSS_ERROR("out of memory");
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		dvs_config->ycoords_y = kvmalloc(width_y * height_y * sizeof(uint32_t),
+						 GFP_KERNEL);
+		if (!dvs_config->ycoords_y) {
+			IA_CSS_ERROR("out of memory");
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		/* Generate UV buffers  */
+		IA_CSS_LOG("UV W %d H %d", width_uv, height_uv);
+
+		dvs_config->xcoords_uv = kvmalloc(width_uv * height_uv * sizeof(uint32_t),
+						  GFP_KERNEL);
+		if (!dvs_config->xcoords_uv) {
+			IA_CSS_ERROR("out of memory");
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		dvs_config->ycoords_uv = kvmalloc(width_uv * height_uv * sizeof(uint32_t),
+						  GFP_KERNEL);
+		if (!dvs_config->ycoords_uv) {
+			IA_CSS_ERROR("out of memory");
+			err = -ENOMEM;
+		}
+exit:
+		if (err) {
+			free_dvs_6axis_table(
+			    &dvs_config); /* we might have allocated some memory, release this */
+			dvs_config = NULL;
+		}
+	}
+
+	IA_CSS_LEAVE("dvs_config=%p", dvs_config);
+	return dvs_config;
+}
+
+static void
+init_dvs_6axis_table_from_default(struct ia_css_dvs_6axis_config *dvs_config,
+				  const struct ia_css_resolution *dvs_offset)
+{
+	unsigned int x, y;
+	unsigned int width_y = dvs_config->width_y;
+	unsigned int height_y = dvs_config->height_y;
+	unsigned int width_uv = dvs_config->width_uv;
+	unsigned int height_uv = dvs_config->height_uv;
+
+	IA_CSS_LOG("Env_X=%d, Env_Y=%d, width_y=%d, height_y=%d",
+		   dvs_offset->width, dvs_offset->height, width_y, height_y);
+	for (y = 0; y < height_y; y++) {
+		for (x = 0; x < width_y; x++) {
+			dvs_config->xcoords_y[y * width_y + x] =  (dvs_offset->width + x *
+				DVS_BLOCKDIM_X) << DVS_COORD_FRAC_BITS;
+		}
+	}
+
+	for (y = 0; y < height_y; y++) {
+		for (x = 0; x < width_y; x++) {
+			dvs_config->ycoords_y[y * width_y + x] =  (dvs_offset->height + y *
+				DVS_BLOCKDIM_Y_LUMA) << DVS_COORD_FRAC_BITS;
+		}
+	}
+
+	for (y = 0; y < height_uv; y++) {
+		for (x = 0; x < width_uv;
+		     x++) { /* Envelope dimensions set in Ypixels hence offset UV = offset Y/2 */
+			dvs_config->xcoords_uv[y * width_uv + x] =  ((dvs_offset->width / 2) + x *
+				DVS_BLOCKDIM_X) << DVS_COORD_FRAC_BITS;
+		}
+	}
+
+	for (y = 0; y < height_uv; y++) {
+		for (x = 0; x < width_uv;
+		     x++) { /* Envelope dimensions set in Ypixels hence offset UV = offset Y/2 */
+			dvs_config->ycoords_uv[y * width_uv + x] =  ((dvs_offset->height / 2) + y *
+				DVS_BLOCKDIM_Y_CHROMA) <<
+				DVS_COORD_FRAC_BITS;
+		}
+	}
+}
+
+static void
+init_dvs_6axis_table_from_config(struct ia_css_dvs_6axis_config *dvs_config,
+				 struct ia_css_dvs_6axis_config  *dvs_config_src)
+{
+	unsigned int width_y = dvs_config->width_y;
+	unsigned int height_y = dvs_config->height_y;
+	unsigned int width_uv = dvs_config->width_uv;
+	unsigned int height_uv = dvs_config->height_uv;
+
+	memcpy(dvs_config->xcoords_y, dvs_config_src->xcoords_y,
+	       (width_y * height_y * sizeof(uint32_t)));
+	memcpy(dvs_config->ycoords_y, dvs_config_src->ycoords_y,
+	       (width_y * height_y * sizeof(uint32_t)));
+	memcpy(dvs_config->xcoords_uv, dvs_config_src->xcoords_uv,
+	       (width_uv * height_uv * sizeof(uint32_t)));
+	memcpy(dvs_config->ycoords_uv, dvs_config_src->ycoords_uv,
+	       (width_uv * height_uv * sizeof(uint32_t)));
+}
+
+struct ia_css_dvs_6axis_config *
+generate_dvs_6axis_table(const struct ia_css_resolution *frame_res,
+			 const struct ia_css_resolution *dvs_offset)
+{
+	struct ia_css_dvs_6axis_config *dvs_6axis_table;
+
+	assert(frame_res);
+	assert(dvs_offset);
+
+	dvs_6axis_table = alloc_dvs_6axis_table(frame_res, NULL);
+	if (dvs_6axis_table) {
+		init_dvs_6axis_table_from_default(dvs_6axis_table, dvs_offset);
+		return dvs_6axis_table;
+	}
+	return NULL;
+}
+
+struct ia_css_dvs_6axis_config *
+generate_dvs_6axis_table_from_config(struct ia_css_dvs_6axis_config
+				     *dvs_config_src)
+{
+	struct ia_css_dvs_6axis_config *dvs_6axis_table;
+
+	assert(dvs_config_src);
+
+	dvs_6axis_table = alloc_dvs_6axis_table(NULL, dvs_config_src);
+	if (dvs_6axis_table) {
+		init_dvs_6axis_table_from_config(dvs_6axis_table, dvs_config_src);
+		return dvs_6axis_table;
+	}
+	return NULL;
+}
+
+void
+free_dvs_6axis_table(struct ia_css_dvs_6axis_config  **dvs_6axis_config)
+{
+	assert(dvs_6axis_config);
+	assert(*dvs_6axis_config);
+
+	if ((dvs_6axis_config) && (*dvs_6axis_config)) {
+		IA_CSS_ENTER_PRIVATE("dvs_6axis_config %p", (*dvs_6axis_config));
+		if ((*dvs_6axis_config)->xcoords_y) {
+			kvfree((*dvs_6axis_config)->xcoords_y);
+			(*dvs_6axis_config)->xcoords_y = NULL;
+		}
+
+		if ((*dvs_6axis_config)->ycoords_y) {
+			kvfree((*dvs_6axis_config)->ycoords_y);
+			(*dvs_6axis_config)->ycoords_y = NULL;
+		}
+
+		/* Free up UV buffers */
+		if ((*dvs_6axis_config)->xcoords_uv) {
+			kvfree((*dvs_6axis_config)->xcoords_uv);
+			(*dvs_6axis_config)->xcoords_uv = NULL;
+		}
+
+		if ((*dvs_6axis_config)->ycoords_uv) {
+			kvfree((*dvs_6axis_config)->ycoords_uv);
+			(*dvs_6axis_config)->ycoords_uv = NULL;
+		}
+
+		IA_CSS_LEAVE_PRIVATE("dvs_6axis_config %p", (*dvs_6axis_config));
+		kvfree(*dvs_6axis_config);
+		*dvs_6axis_config = NULL;
+	}
+}
+
+void copy_dvs_6axis_table(struct ia_css_dvs_6axis_config *dvs_config_dst,
+			  const struct ia_css_dvs_6axis_config *dvs_config_src)
+{
+	unsigned int width_y;
+	unsigned int height_y;
+	unsigned int width_uv;
+	unsigned int height_uv;
+
+	assert(dvs_config_src);
+	assert(dvs_config_dst);
+	assert(dvs_config_src->xcoords_y);
+	assert(dvs_config_src->xcoords_uv);
+	assert(dvs_config_src->ycoords_y);
+	assert(dvs_config_src->ycoords_uv);
+	assert(dvs_config_src->width_y == dvs_config_dst->width_y);
+	assert(dvs_config_src->width_uv == dvs_config_dst->width_uv);
+	assert(dvs_config_src->height_y == dvs_config_dst->height_y);
+	assert(dvs_config_src->height_uv == dvs_config_dst->height_uv);
+
+	width_y = dvs_config_src->width_y;
+	height_y = dvs_config_src->height_y;
+	width_uv =
+	    dvs_config_src->width_uv; /* = Y/2, depens on colour format YUV 4.2.0*/
+	height_uv = dvs_config_src->height_uv;
+
+	memcpy(dvs_config_dst->xcoords_y, dvs_config_src->xcoords_y,
+	       (width_y * height_y * sizeof(uint32_t)));
+	memcpy(dvs_config_dst->ycoords_y, dvs_config_src->ycoords_y,
+	       (width_y * height_y * sizeof(uint32_t)));
+
+	memcpy(dvs_config_dst->xcoords_uv, dvs_config_src->xcoords_uv,
+	       (width_uv * height_uv * sizeof(uint32_t)));
+	memcpy(dvs_config_dst->ycoords_uv, dvs_config_src->ycoords_uv,
+	       (width_uv * height_uv * sizeof(uint32_t)));
+}
+
+void
+ia_css_dvs_statistics_get(enum dvs_statistics_type type,
+			  union ia_css_dvs_statistics_host  *host_stats,
+			  const union ia_css_dvs_statistics_isp *isp_stats)
+{
+	if (type == DVS_STATISTICS) {
+		ia_css_get_dvs_statistics(host_stats->p_dvs_statistics_host,
+					  isp_stats->p_dvs_statistics_isp);
+	} else if (type == DVS2_STATISTICS) {
+		ia_css_get_dvs2_statistics(host_stats->p_dvs2_statistics_host,
+					   isp_stats->p_dvs_statistics_isp);
+	}
+	return;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_param_dvs.h b/drivers/staging/media/atomisp/pci/sh_css_param_dvs.h
new file mode 100644
index 000000000..7782f76b9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_param_dvs.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_PARAMS_DVS_H_
+#define _SH_CSS_PARAMS_DVS_H_
+
+#include <math_support.h>
+#include <ia_css_types.h>
+#include <sh_css_dvs_info.h>
+#include "gdc_global.h" /* gdc_warp_param_mem_t */
+
+#define DVS_ENV_MIN_X (12)
+#define DVS_ENV_MIN_Y (12)
+
+#define DVS_BLOCKDIM_X (64)        /* X block height*/
+#define DVS_BLOCKDIM_Y_LUMA (64)   /* Y block height*/
+#define DVS_BLOCKDIM_Y_CHROMA (32) /* UV height block size is half the Y block height*/
+
+/* ISP2400 */
+/* horizontal 64x64 blocks round up to DVS_BLOCKDIM_X, make even */
+#define DVS_NUM_BLOCKS_X(X)		(CEIL_MUL(CEIL_DIV((X), DVS_BLOCKDIM_X), 2))
+
+/* ISP2400 */
+/* vertical   64x64 blocks round up to DVS_BLOCKDIM_Y */
+#define DVS_NUM_BLOCKS_Y(X)		(CEIL_DIV((X), DVS_BLOCKDIM_Y_LUMA))
+#define DVS_NUM_BLOCKS_X_CHROMA(X)	(CEIL_DIV((X), DVS_BLOCKDIM_X))
+#define DVS_NUM_BLOCKS_Y_CHROMA(X)	(CEIL_DIV((X), DVS_BLOCKDIM_Y_CHROMA))
+
+#define DVS_TABLE_IN_BLOCKDIM_X_LUMA(X)	(DVS_NUM_BLOCKS_X(X) + 1)  /* N blocks have N + 1 set of coords */
+#define DVS_TABLE_IN_BLOCKDIM_X_CHROMA(X)   (DVS_NUM_BLOCKS_X_CHROMA(X) + 1)
+#define DVS_TABLE_IN_BLOCKDIM_Y_LUMA(X)		(DVS_NUM_BLOCKS_Y(X) + 1)
+#define DVS_TABLE_IN_BLOCKDIM_Y_CHROMA(X)	(DVS_NUM_BLOCKS_Y_CHROMA(X) + 1)
+
+#define DVS_ENVELOPE_X(X) (((X) == 0) ? (DVS_ENV_MIN_X) : (X))
+#define DVS_ENVELOPE_Y(X) (((X) == 0) ? (DVS_ENV_MIN_Y) : (X))
+
+#define DVS_COORD_FRAC_BITS (10)
+
+/* ISP2400 */
+#define DVS_INPUT_BYTES_PER_PIXEL (1)
+
+#define XMEM_ALIGN_LOG2 (5)
+
+#define DVS_6AXIS_COORDS_ELEMS CEIL_MUL(sizeof(gdc_warp_param_mem_t) \
+					, HIVE_ISP_DDR_WORD_BYTES)
+
+/* currently we only support two output with the same resolution, output 0 is th default one. */
+#define DVS_6AXIS_BYTES(binary) \
+	(DVS_6AXIS_COORDS_ELEMS \
+	* DVS_NUM_BLOCKS_X((binary)->out_frame_info[0].res.width) \
+	* DVS_NUM_BLOCKS_Y((binary)->out_frame_info[0].res.height))
+
+/*
+ * ISP2400:
+ * Bilinear interpolation (HRT_GDC_BLI_MODE) is the supported method currently.
+ * Bicubic interpolation (HRT_GDC_BCI_MODE) is not supported yet */
+#define DVS_GDC_INTERP_METHOD HRT_GDC_BLI_MODE
+
+struct ia_css_dvs_6axis_config *
+generate_dvs_6axis_table(const struct ia_css_resolution	*frame_res,
+			 const struct ia_css_resolution *dvs_offset);
+
+struct ia_css_dvs_6axis_config *
+generate_dvs_6axis_table_from_config(struct ia_css_dvs_6axis_config
+				     *dvs_config_src);
+
+void
+free_dvs_6axis_table(struct ia_css_dvs_6axis_config  **dvs_6axis_config);
+
+void
+copy_dvs_6axis_table(struct ia_css_dvs_6axis_config *dvs_config_dst,
+		     const struct ia_css_dvs_6axis_config *dvs_config_src);
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/sh_css_param_shading.c b/drivers/staging/media/atomisp/pci/sh_css_param_shading.c
new file mode 100644
index 000000000..41a4c9162
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_param_shading.c
@@ -0,0 +1,393 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/slab.h>
+
+#include <math_support.h>
+#include "sh_css_param_shading.h"
+#include "ia_css_shading.h"
+#include "assert_support.h"
+#include "sh_css_defs.h"
+#include "sh_css_internal.h"
+#include "ia_css_debug.h"
+#include "ia_css_pipe_binarydesc.h"
+
+#include "sh_css_hrt.h"
+
+#include "platform_support.h"
+
+/* Bilinear interpolation on shading tables:
+ * For each target point T, we calculate the 4 surrounding source points:
+ * ul (upper left), ur (upper right), ll (lower left) and lr (lower right).
+ * We then calculate the distances from the T to the source points: x0, x1,
+ * y0 and y1.
+ * We then calculate the value of T:
+ *   dx0*dy0*Slr + dx0*dy1*Sur + dx1*dy0*Sll + dx1*dy1*Sul.
+ * We choose a grid size of 1x1 which means:
+ *   dx1 = 1-dx0
+ *   dy1 = 1-dy0
+ *
+ *   Sul dx0         dx1      Sur
+ *    .<----->|<------------->.
+ *    ^
+ * dy0|
+ *    v        T
+ *    -        .
+ *    ^
+ *    |
+ * dy1|
+ *    v
+ *    .                        .
+ *   Sll                      Slr
+ *
+ * Padding:
+ * The area that the ISP operates on can include padding both on the left
+ * and the right. We need to padd the shading table such that the shading
+ * values end up on the correct pixel values. This means we must padd the
+ * shading table to match the ISP padding.
+ * We can have 5 cases:
+ * 1. All 4 points fall in the left padding.
+ * 2. The left 2 points fall in the left padding.
+ * 3. All 4 points fall in the cropped (target) region.
+ * 4. The right 2 points fall in the right padding.
+ * 5. All 4 points fall in the right padding.
+ * Cases 1 and 5 are easy to handle: we simply use the
+ * value 1 in the shading table.
+ * Cases 2 and 4 require interpolation that takes into
+ * account how far into the padding area the pixels
+ * fall. We extrapolate the shading table into the
+ * padded area and then interpolate.
+ */
+static void
+crop_and_interpolate(unsigned int cropped_width,
+		     unsigned int cropped_height,
+		     unsigned int left_padding,
+		     int right_padding,
+		     int top_padding,
+		     const struct ia_css_shading_table *in_table,
+		     struct ia_css_shading_table *out_table,
+		     enum ia_css_sc_color color)
+{
+	unsigned int i, j,
+		 sensor_width,
+		 sensor_height,
+		 table_width,
+		 table_height,
+		 table_cell_h,
+		 out_cell_size,
+		 in_cell_size,
+		 out_start_row,
+		 padded_width;
+	int out_start_col, /* can be negative to indicate padded space */
+	    table_cell_w;
+	unsigned short *in_ptr,
+		 *out_ptr;
+
+	assert(in_table);
+	assert(out_table);
+
+	sensor_width  = in_table->sensor_width;
+	sensor_height = in_table->sensor_height;
+	table_width   = in_table->width;
+	table_height  = in_table->height;
+	in_ptr = in_table->data[color];
+	out_ptr = out_table->data[color];
+
+	padded_width = cropped_width + left_padding + right_padding;
+	out_cell_size = CEIL_DIV(padded_width, out_table->width - 1);
+	in_cell_size  = CEIL_DIV(sensor_width, table_width - 1);
+
+	out_start_col = ((int)sensor_width - (int)cropped_width) / 2 - left_padding;
+	out_start_row = ((int)sensor_height - (int)cropped_height) / 2 - top_padding;
+	table_cell_w = (int)((table_width - 1) * in_cell_size);
+	table_cell_h = (table_height - 1) * in_cell_size;
+
+	for (i = 0; i < out_table->height; i++) {
+		int ty, src_y0, src_y1;
+		unsigned int sy0, sy1, dy0, dy1, divy;
+
+		/*
+		 * calculate target point and make sure it falls within
+		 * the table
+		 */
+		ty = out_start_row + i * out_cell_size;
+
+		/* calculate closest source points in shading table and
+		   make sure they fall within the table */
+		src_y0 = ty / (int)in_cell_size;
+		if (in_cell_size < out_cell_size)
+			src_y1 = (ty + out_cell_size) / in_cell_size;
+		else
+			src_y1 = src_y0 + 1;
+		src_y0 = clamp(src_y0, 0, (int)table_height - 1);
+		src_y1 = clamp(src_y1, 0, (int)table_height - 1);
+		ty = min(clamp(ty, 0, (int)sensor_height - 1),
+			 (int)table_cell_h);
+
+		/* calculate closest source points for distance computation */
+		sy0 = min(src_y0 * in_cell_size, sensor_height - 1);
+		sy1 = min(src_y1 * in_cell_size, sensor_height - 1);
+		/* calculate distance between source and target pixels */
+		dy0 = ty - sy0;
+		dy1 = sy1 - ty;
+		divy = sy1 - sy0;
+		if (divy == 0) {
+			dy0 = 1;
+			divy = 1;
+		}
+
+		for (j = 0; j < out_table->width; j++, out_ptr++) {
+			int tx, src_x0, src_x1;
+			unsigned int sx0, sx1, dx0, dx1, divx;
+			unsigned short s_ul, s_ur, s_ll, s_lr;
+
+			/* calculate target point */
+			tx = out_start_col + j * out_cell_size;
+			/* calculate closest source points. */
+			src_x0 = tx / (int)in_cell_size;
+			if (in_cell_size < out_cell_size) {
+				src_x1 = (tx + out_cell_size) /
+					 (int)in_cell_size;
+			} else {
+				src_x1 = src_x0 + 1;
+			}
+			/* if src points fall in padding, select closest ones.*/
+			src_x0 = clamp(src_x0, 0, (int)table_width - 1);
+			src_x1 = clamp(src_x1, 0, (int)table_width - 1);
+			tx = min(clamp(tx, 0, (int)sensor_width - 1),
+				 (int)table_cell_w);
+			/*
+			 * calculate closest source points for distance
+			 * computation
+			 */
+			sx0 = min(src_x0 * in_cell_size, sensor_width - 1);
+			sx1 = min(src_x1 * in_cell_size, sensor_width - 1);
+			/*
+			 * calculate distances between source and target
+			 * pixels
+			 */
+			dx0 = tx - sx0;
+			dx1 = sx1 - tx;
+			divx = sx1 - sx0;
+			/* if we're at the edge, we just use the closest
+			 * point still in the grid. We make up for the divider
+			 * in this case by setting the distance to
+			 * out_cell_size, since it's actually 0.
+			 */
+			if (divx == 0) {
+				dx0 = 1;
+				divx = 1;
+			}
+
+			/* get source pixel values */
+			s_ul = in_ptr[(table_width * src_y0) + src_x0];
+			s_ur = in_ptr[(table_width * src_y0) + src_x1];
+			s_ll = in_ptr[(table_width * src_y1) + src_x0];
+			s_lr = in_ptr[(table_width * src_y1) + src_x1];
+
+			*out_ptr = (unsigned short)((dx0 * dy0 * s_lr + dx0 * dy1 * s_ur + dx1 * dy0 *
+						     s_ll + dx1 * dy1 * s_ul) /
+						    (divx * divy));
+		}
+	}
+}
+
+void
+sh_css_params_shading_id_table_generate(
+    struct ia_css_shading_table **target_table,
+    unsigned int table_width,
+    unsigned int table_height)
+{
+	/* initialize table with ones, shift becomes zero */
+	unsigned int i, j;
+	struct ia_css_shading_table *result;
+
+	assert(target_table);
+
+	result = ia_css_shading_table_alloc(table_width, table_height);
+	if (!result) {
+		*target_table = NULL;
+		return;
+	}
+
+	for (i = 0; i < IA_CSS_SC_NUM_COLORS; i++) {
+		for (j = 0; j < table_height * table_width; j++)
+			result->data[i][j] = 1;
+	}
+	result->fraction_bits = 0;
+	*target_table = result;
+}
+
+void
+prepare_shading_table(const struct ia_css_shading_table *in_table,
+		      unsigned int sensor_binning,
+		      struct ia_css_shading_table **target_table,
+		      const struct ia_css_binary *binary,
+		      unsigned int bds_factor)
+{
+	unsigned int input_width, input_height, table_width, table_height, i;
+	unsigned int left_padding, top_padding, left_cropping;
+	unsigned int bds_numerator, bds_denominator;
+	int right_padding;
+
+	struct ia_css_shading_table *result;
+
+	assert(target_table);
+	assert(binary);
+
+	if (!in_table) {
+		sh_css_params_shading_id_table_generate(target_table,
+							binary->sctbl_width_per_color,
+							binary->sctbl_height);
+		return;
+	}
+
+	/*
+	 * We use the ISP input resolution for the shading table because
+	 * shading correction is performed in the bayer domain (before bayer
+	 * down scaling).
+	 */
+	input_height  = binary->in_frame_info.res.height;
+	input_width   = binary->in_frame_info.res.width;
+	left_padding  = binary->left_padding;
+	left_cropping = (binary->info->sp.pipeline.left_cropping == 0) ?
+			binary->dvs_envelope.width : 2 * ISP_VEC_NELEMS;
+
+	sh_css_bds_factor_get_numerator_denominator
+	(bds_factor, &bds_numerator, &bds_denominator);
+
+	left_padding  = (left_padding + binary->info->sp.pipeline.left_cropping) *
+			bds_numerator / bds_denominator -
+			binary->info->sp.pipeline.left_cropping;
+	right_padding = (binary->internal_frame_info.res.width -
+			 binary->effective_in_frame_res.width * bds_denominator /
+			 bds_numerator - left_cropping) * bds_numerator / bds_denominator;
+	top_padding = binary->info->sp.pipeline.top_cropping * bds_numerator /
+		      bds_denominator -
+		      binary->info->sp.pipeline.top_cropping;
+
+	/*
+	 * We take into account the binning done by the sensor. We do this
+	 * by cropping the non-binned part of the shading table and then
+	 * increasing the size of a grid cell with this same binning factor.
+	 */
+	input_width  <<= sensor_binning;
+	input_height <<= sensor_binning;
+	/*
+	 * We also scale the padding by the same binning factor. This will
+	 * make it much easier later on to calculate the padding of the
+	 * shading table.
+	 */
+	left_padding  <<= sensor_binning;
+	right_padding <<= sensor_binning;
+	top_padding   <<= sensor_binning;
+
+	/*
+	 * during simulation, the used resolution can exceed the sensor
+	 * resolution, so we clip it.
+	 */
+	input_width  = min(input_width,  in_table->sensor_width);
+	input_height = min(input_height, in_table->sensor_height);
+
+	/* This prepare_shading_table() function is called only in legacy API (not in new API).
+	   Then, the legacy shading table width and height should be used. */
+	table_width  = binary->sctbl_width_per_color;
+	table_height = binary->sctbl_height;
+
+	result = ia_css_shading_table_alloc(table_width, table_height);
+	if (!result) {
+		*target_table = NULL;
+		return;
+	}
+	result->sensor_width  = in_table->sensor_width;
+	result->sensor_height = in_table->sensor_height;
+	result->fraction_bits = in_table->fraction_bits;
+
+	/*
+	 * now we crop the original shading table and then interpolate to the
+	 * requested resolution and decimation factor.
+	 */
+	for (i = 0; i < IA_CSS_SC_NUM_COLORS; i++) {
+		crop_and_interpolate(input_width, input_height,
+				     left_padding, right_padding, top_padding,
+				     in_table,
+				     result, i);
+	}
+	*target_table = result;
+}
+
+struct ia_css_shading_table *
+ia_css_shading_table_alloc(
+    unsigned int width,
+    unsigned int height)
+{
+	unsigned int i;
+	struct ia_css_shading_table *me;
+
+	IA_CSS_ENTER("");
+
+	me = kmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me)
+		return me;
+
+	me->width         = width;
+	me->height        = height;
+	me->sensor_width  = 0;
+	me->sensor_height = 0;
+	me->fraction_bits = 0;
+	for (i = 0; i < IA_CSS_SC_NUM_COLORS; i++) {
+		me->data[i] =
+		    kvmalloc(width * height * sizeof(*me->data[0]),
+			     GFP_KERNEL);
+		if (!me->data[i]) {
+			unsigned int j;
+
+			for (j = 0; j < i; j++) {
+				kvfree(me->data[j]);
+				me->data[j] = NULL;
+			}
+			kfree(me);
+			return NULL;
+		}
+	}
+
+	IA_CSS_LEAVE("");
+	return me;
+}
+
+void
+ia_css_shading_table_free(struct ia_css_shading_table *table)
+{
+	unsigned int i;
+
+	if (!table)
+		return;
+
+	/*
+	 * We only output logging when the table is not NULL, otherwise
+	 * logs will give the impression that a table was freed.
+	 */
+	IA_CSS_ENTER("");
+
+	for (i = 0; i < IA_CSS_SC_NUM_COLORS; i++) {
+		if (table->data[i]) {
+			kvfree(table->data[i]);
+			table->data[i] = NULL;
+		}
+	}
+	kfree(table);
+
+	IA_CSS_LEAVE("");
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_param_shading.h b/drivers/staging/media/atomisp/pci/sh_css_param_shading.h
new file mode 100644
index 000000000..7cdfaaec0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_param_shading.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_PARAMS_SHADING_H
+#define __SH_CSS_PARAMS_SHADING_H
+
+#include <ia_css_types.h>
+#include <ia_css_binary.h>
+
+void
+sh_css_params_shading_id_table_generate(
+    struct ia_css_shading_table **target_table,
+    unsigned int table_width,
+    unsigned int table_height);
+
+void
+prepare_shading_table(const struct ia_css_shading_table *in_table,
+		      unsigned int sensor_binning,
+		      struct ia_css_shading_table **target_table,
+		      const struct ia_css_binary *binary,
+		      unsigned int bds_factor);
+
+#endif /* __SH_CSS_PARAMS_SHADING_H */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_params.c b/drivers/staging/media/atomisp/pci/sh_css_params.c
new file mode 100644
index 000000000..67915d76a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_params.c
@@ -0,0 +1,4511 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "gdc_device.h"		/* gdc_lut_store(), ... */
+#include "isp.h"			/* ISP_VEC_ELEMBITS */
+#include "vamem.h"
+#ifndef __INLINE_HMEM__
+#define __INLINE_HMEM__
+#endif
+#include "hmem.h"
+#define IA_CSS_INCLUDE_PARAMETERS
+#define IA_CSS_INCLUDE_ACC_PARAMETERS
+
+#include "hmm.h"
+#include "sh_css_params.h"
+#include "ia_css_queue.h"
+#include "sw_event_global.h"		/* Event IDs */
+
+#include "platform_support.h"
+#include "assert_support.h"
+#include "misc_support.h"	/* NOT_USED */
+#include "math_support.h"	/* max(), min()  EVEN_FLOOR()*/
+
+#include "ia_css_stream.h"
+#include "sh_css_params_internal.h"
+#include "sh_css_param_shading.h"
+#include "sh_css_param_dvs.h"
+#include "ia_css_refcount.h"
+#include "sh_css_internal.h"
+#include "ia_css_control.h"
+#include "ia_css_shading.h"
+#include "sh_css_defs.h"
+#include "sh_css_sp.h"
+#include "ia_css_pipeline.h"
+#include "ia_css_debug.h"
+
+#include "ia_css_isp_param.h"
+#include "ia_css_isp_params.h"
+#include "ia_css_mipi.h"
+#include "ia_css_morph.h"
+#include "ia_css_host_data.h"
+#include "ia_css_pipe.h"
+#include "ia_css_pipe_binarydesc.h"
+
+/* Include all kernel host interfaces for ISP1 */
+
+#include "anr/anr_1.0/ia_css_anr.host.h"
+#include "cnr/cnr_1.0/ia_css_cnr.host.h"
+#include "csc/csc_1.0/ia_css_csc.host.h"
+#include "de/de_1.0/ia_css_de.host.h"
+#include "dp/dp_1.0/ia_css_dp.host.h"
+#include "bnr/bnr_1.0/ia_css_bnr.host.h"
+#include "dvs/dvs_1.0/ia_css_dvs.host.h"
+#include "fpn/fpn_1.0/ia_css_fpn.host.h"
+#include "gc/gc_1.0/ia_css_gc.host.h"
+#include "macc/macc_1.0/ia_css_macc.host.h"
+#include "ctc/ctc_1.0/ia_css_ctc.host.h"
+#include "ob/ob_1.0/ia_css_ob.host.h"
+#include "raw/raw_1.0/ia_css_raw.host.h"
+#include "fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h"
+#include "s3a/s3a_1.0/ia_css_s3a.host.h"
+#include "sc/sc_1.0/ia_css_sc.host.h"
+#include "sdis/sdis_1.0/ia_css_sdis.host.h"
+#include "tnr/tnr_1.0/ia_css_tnr.host.h"
+#include "uds/uds_1.0/ia_css_uds_param.h"
+#include "wb/wb_1.0/ia_css_wb.host.h"
+#include "ynr/ynr_1.0/ia_css_ynr.host.h"
+#include "xnr/xnr_1.0/ia_css_xnr.host.h"
+
+/* Include additional kernel host interfaces for ISP2 */
+
+#include "aa/aa_2/ia_css_aa2.host.h"
+#include "anr/anr_2/ia_css_anr2.host.h"
+#include "bh/bh_2/ia_css_bh.host.h"
+#include "cnr/cnr_2/ia_css_cnr2.host.h"
+#include "ctc/ctc1_5/ia_css_ctc1_5.host.h"
+#include "de/de_2/ia_css_de2.host.h"
+#include "gc/gc_2/ia_css_gc2.host.h"
+#include "sdis/sdis_2/ia_css_sdis2.host.h"
+#include "ynr/ynr_2/ia_css_ynr2.host.h"
+#include "fc/fc_1.0/ia_css_formats.host.h"
+
+#include "xnr/xnr_3.0/ia_css_xnr3.host.h"
+
+
+#include "sh_css_frac.h"
+#include "ia_css_bufq.h"
+
+#define FPNTBL_BYTES(binary) \
+	(sizeof(char) * (binary)->in_frame_info.res.height * \
+	 (binary)->in_frame_info.padded_width)
+
+#define SCTBL_BYTES(binary) \
+	(sizeof(unsigned short) * (binary)->sctbl_height * \
+	 (binary)->sctbl_aligned_width_per_color * IA_CSS_SC_NUM_COLORS)
+
+#define MORPH_PLANE_BYTES(binary) \
+	(SH_CSS_MORPH_TABLE_ELEM_BYTES * (binary)->morph_tbl_aligned_width * \
+	 (binary)->morph_tbl_height)
+
+/* We keep a second copy of the ptr struct for the SP to access.
+   Again, this would not be necessary on the chip. */
+static ia_css_ptr sp_ddr_ptrs;
+
+/* sp group address on DDR */
+static ia_css_ptr xmem_sp_group_ptrs;
+
+static ia_css_ptr xmem_sp_stage_ptrs[IA_CSS_PIPE_ID_NUM]
+[SH_CSS_MAX_STAGES];
+static ia_css_ptr xmem_isp_stage_ptrs[IA_CSS_PIPE_ID_NUM]
+[SH_CSS_MAX_STAGES];
+
+static ia_css_ptr default_gdc_lut;
+static int interleaved_lut_temp[4][HRT_GDC_N];
+
+/* END DO NOT MOVE INTO VIMALS_WORLD */
+
+/* Digital Zoom lookup table. See documentation for more details about the
+ * contents of this table.
+ */
+static const int zoom_table[4][HRT_GDC_N] = {
+	{
+		0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4,
+		  -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,  -7 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4
+	},
+	{
+		0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,
+		  2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,
+		  4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,
+		  4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,
+		  7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,
+		  7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,
+		  9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,
+		  9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,
+		  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,
+		  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,
+		  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,
+		  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,
+		  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,
+		  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,
+		  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,
+		  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,
+		  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,
+		  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,
+		  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,
+		  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,
+		  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,
+		  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,
+		  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,
+		  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,
+		  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,
+		  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,
+		  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,
+		  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,
+		  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,
+		  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,
+		  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,
+		  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,
+		  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,
+		  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,
+		  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,
+		  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,
+		  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,
+		  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,
+		  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,
+		  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,
+		  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,
+		  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,
+		  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,
+		  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,
+		  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,
+		  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,
+		  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,
+		  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,
+		  105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4,
+		  105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4,
+		  110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4,
+		  110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4,
+		  116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4,
+		  116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4,
+		  121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4,
+		  121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4,
+		  127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4,
+		  127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4,
+		  132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4,
+		  132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4,
+		  138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4,
+		  138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4,
+		  144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4,
+		  144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4,
+		  149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4,
+		  149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4,
+		  154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4,
+		  154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4,
+		  160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4,
+		  160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4,
+		  165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4,
+		  165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4,
+		  170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4,
+		  170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4,
+		  176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4,
+		  176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4,
+		  181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4,
+		  181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4,
+		  186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4,
+		  186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4,
+		  191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4,
+		  191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4,
+		  195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4,
+		  195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4,
+		  200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4,
+		  200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4,
+		  205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4,
+		  205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4,
+		  209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4,
+		  209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4,
+		  213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4,
+		  213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4,
+		  218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4,
+		  218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4,
+		  222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4,
+		  222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4,
+		  225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4,
+		  225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4,
+		  229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4,
+		  229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4,
+		  232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4,
+		  232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4,
+		  236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4,
+		  236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4,
+		  239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4,
+		  239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4,
+		  241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4,
+		  241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4,
+		  244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4,
+		  244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4,
+		  246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4,
+		  246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4,
+		  248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4,
+		  248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4,
+		  250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4,
+		  250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4,
+		  252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4,
+		  252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4,
+		  253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4,
+		  253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4,
+		  254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4,
+		  254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4,
+		  255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		  255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		  255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		  255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4
+	},
+	{
+		256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4,
+		    256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4, 256 << 4,
+		    255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		    255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		    255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		    255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4, 255 << 4,
+		    254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4,
+		    254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4, 254 << 4,
+		    253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4,
+		    253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4, 253 << 4,
+		    252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4,
+		    252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4, 252 << 4,
+		    250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4,
+		    250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4, 250 << 4,
+		    248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4,
+		    248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4, 248 << 4,
+		    246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4,
+		    246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4, 246 << 4,
+		    244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4,
+		    244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4, 244 << 4,
+		    241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4,
+		    241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4, 241 << 4,
+		    239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4,
+		    239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4, 239 << 4,
+		    236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4,
+		    236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4, 236 << 4,
+		    232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4,
+		    232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4, 232 << 4,
+		    229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4,
+		    229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4, 229 << 4,
+		    225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4,
+		    225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4, 225 << 4,
+		    222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4,
+		    222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4, 222 << 4,
+		    218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4,
+		    218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4, 218 << 4,
+		    213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4,
+		    213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4, 213 << 4,
+		    209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4,
+		    209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4, 209 << 4,
+		    205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4,
+		    205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4, 205 << 4,
+		    200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4,
+		    200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4, 200 << 4,
+		    195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4,
+		    195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4, 195 << 4,
+		    191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4,
+		    191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4, 191 << 4,
+		    186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4,
+		    186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4, 186 << 4,
+		    181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4,
+		    181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4, 181 << 4,
+		    176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4,
+		    176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4, 176 << 4,
+		    170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4,
+		    170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4, 170 << 4,
+		    165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4,
+		    165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4, 165 << 4,
+		    160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4,
+		    160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4, 160 << 4,
+		    154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4,
+		    154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4, 154 << 4,
+		    149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4,
+		    149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4, 149 << 4,
+		    144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4,
+		    144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4, 144 << 4,
+		    138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4,
+		    138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4, 138 << 4,
+		    132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4,
+		    132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4, 132 << 4,
+		    127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4,
+		    127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4, 127 << 4,
+		    121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4,
+		    121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4, 121 << 4,
+		    116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4,
+		    116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4, 116 << 4,
+		    110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4,
+		    110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4, 110 << 4,
+		    105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4,
+		    105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4, 105 << 4,
+		    99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,
+		    99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,  99 << 4,
+		    94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,
+		    94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,  94 << 4,
+		    88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,
+		    88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,  88 << 4,
+		    83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,
+		    83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,  83 << 4,
+		    78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,
+		    78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,  78 << 4,
+		    73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,
+		    73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,  73 << 4,
+		    67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,
+		    67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,  67 << 4,
+		    62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,
+		    62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,  62 << 4,
+		    58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,
+		    58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,  58 << 4,
+		    53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,
+		    53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,  53 << 4,
+		    48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,
+		    48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,  48 << 4,
+		    43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,
+		    43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,  43 << 4,
+		    39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,
+		    39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,  39 << 4,
+		    35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,
+		    35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,  35 << 4,
+		    31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,
+		    31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,  31 << 4,
+		    27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,
+		    27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,  27 << 4,
+		    23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,
+		    23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,  23 << 4,
+		    19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,
+		    19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,  19 << 4,
+		    16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,
+		    16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,  16 << 4,
+		    12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,
+		    12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,  12 << 4,
+		    9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,
+		    9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,   9 << 4,
+		    7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,
+		    7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,   7 << 4,
+		    4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,
+		    4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,   4 << 4,
+		    2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,
+		    2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4,   2 << 4
+	},
+	{
+		0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4,
+		  -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4, -10 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4, -19 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4, -18 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4, -17 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4, -16 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4, -15 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4, -14 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4, -13 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4, -12 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4, -11 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,  -9 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,  -8 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,  -6 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,  -5 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,  -4 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,  -3 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,  -2 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,  -1 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,
+		  1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,   1 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,
+		  0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4,   0 << 4
+	}
+};
+
+static const struct ia_css_dz_config default_dz_config = {
+	HRT_GDC_N,
+	HRT_GDC_N,
+	{
+		\
+		{0, 0}, \
+		{0, 0}, \
+	}
+};
+
+static const struct ia_css_vector default_motion_config = {
+	0,
+	0
+};
+
+/* ------ deprecated(bz675) : from ------ */
+static const struct ia_css_shading_settings default_shading_settings = {
+	1	/* enable shading table conversion in the css
+		(This matches the legacy way.) */
+};
+
+/* ------ deprecated(bz675) : to ------ */
+
+struct ia_css_isp_skc_dvs_statistics {
+	ia_css_ptr p_data;
+};
+
+static int
+ref_sh_css_ddr_address_map(
+    struct sh_css_ddr_address_map *map,
+    struct sh_css_ddr_address_map *out);
+
+static int
+write_ia_css_isp_parameter_set_info_to_ddr(
+    struct ia_css_isp_parameter_set_info *me,
+    ia_css_ptr *out);
+
+static int
+free_ia_css_isp_parameter_set_info(ia_css_ptr ptr);
+
+static int
+sh_css_params_write_to_ddr_internal(
+    struct ia_css_pipe *pipe,
+    unsigned int pipe_id,
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_pipeline_stage *stage,
+    struct sh_css_ddr_address_map *ddr_map,
+    struct sh_css_ddr_address_map_size *ddr_map_size);
+
+static int
+sh_css_create_isp_params(struct ia_css_stream *stream,
+			 struct ia_css_isp_parameters **isp_params_out);
+
+static bool
+sh_css_init_isp_params_from_global(struct ia_css_stream *stream,
+				   struct ia_css_isp_parameters *params,
+				   bool use_default_config,
+				   struct ia_css_pipe *pipe_in);
+
+static int
+sh_css_init_isp_params_from_config(struct ia_css_pipe *pipe,
+				   struct ia_css_isp_parameters *params,
+				   const struct ia_css_isp_config *config,
+				   struct ia_css_pipe *pipe_in);
+
+static int
+sh_css_set_global_isp_config_on_pipe(
+    struct ia_css_pipe *curr_pipe,
+    const struct ia_css_isp_config *config,
+    struct ia_css_pipe *pipe);
+
+static int
+sh_css_set_per_frame_isp_config_on_pipe(
+    struct ia_css_stream *stream,
+    const struct ia_css_isp_config *config,
+    struct ia_css_pipe *pipe);
+
+static int
+sh_css_update_uds_and_crop_info_based_on_zoom_region(
+    const struct ia_css_binary_info *info,
+    const struct ia_css_frame_info *in_frame_info,
+    const struct ia_css_frame_info *out_frame_info,
+    const struct ia_css_resolution *dvs_env,
+    const struct ia_css_dz_config *zoom,
+    const struct ia_css_vector *motion_vector,
+    struct sh_css_uds_info *uds,		/* out */
+    struct sh_css_crop_pos *sp_out_crop_pos,	/* out */
+    struct ia_css_resolution pipe_in_res,
+    bool enable_zoom);
+
+ia_css_ptr
+sh_css_params_ddr_address_map(void)
+{
+	return sp_ddr_ptrs;
+}
+
+/* ****************************************************
+ * Each coefficient is stored as 7bits to fit 2 of them into one
+ * ISP vector element, so we will store 4 coefficents on every
+ * memory word (32bits)
+ *
+ * 0: Coefficient 0 used bits
+ * 1: Coefficient 1 used bits
+ * 2: Coefficient 2 used bits
+ * 3: Coefficient 3 used bits
+ * x: not used
+ *
+ * xx33333332222222 | xx11111110000000
+ *
+ * ***************************************************
+ */
+static struct ia_css_host_data *
+convert_allocate_fpntbl(struct ia_css_isp_parameters *params)
+{
+	unsigned int i, j;
+	short *data_ptr;
+	struct ia_css_host_data *me;
+	unsigned int isp_format_data_size;
+	u32 *isp_format_data_ptr;
+
+	assert(params);
+
+	data_ptr = params->fpn_config.data;
+	isp_format_data_size = params->fpn_config.height * params->fpn_config.width *
+			       sizeof(uint32_t);
+
+	me = ia_css_host_data_allocate(isp_format_data_size);
+
+	if (!me)
+		return NULL;
+
+	isp_format_data_ptr = (uint32_t *)me->address;
+
+	for (i = 0; i < params->fpn_config.height; i++) {
+		for (j = 0;
+		     j < params->fpn_config.width;
+		     j += 4, data_ptr += 4, isp_format_data_ptr++) {
+			int data = data_ptr[0] << 0 |
+				   data_ptr[1] << 7 |
+				   data_ptr[2] << 16 |
+				   data_ptr[3] << 23;
+			*isp_format_data_ptr = data;
+		}
+	}
+	return me;
+}
+
+static int
+store_fpntbl(struct ia_css_isp_parameters *params, ia_css_ptr ptr)
+{
+	struct ia_css_host_data *isp_data;
+
+	assert(params);
+	assert(ptr != mmgr_NULL);
+
+	isp_data = convert_allocate_fpntbl(params);
+	if (!isp_data) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return -ENOMEM;
+	}
+	ia_css_params_store_ia_css_host_data(ptr, isp_data);
+
+	ia_css_host_data_free(isp_data);
+	return 0;
+}
+
+static void
+convert_raw_to_fpn(struct ia_css_isp_parameters *params)
+{
+	int maxval = 0;
+	unsigned int i;
+
+	assert(params);
+
+	/* Find the maximum value in the table */
+	for (i = 0; i < params->fpn_config.height * params->fpn_config.width; i++) {
+		int val = params->fpn_config.data[i];
+		/* Make sure FPN value can be represented in 13-bit unsigned
+		 * number (ISP precision - 1), but note that actual input range
+		 * depends on precision of input frame data.
+		 */
+		if (val < 0) {
+			/* Checkpatch patch */
+			val = 0;
+		} else if (val >= (1 << 13)) {
+			/* Checkpatch patch */
+			/* MW: BUG, is "13" a system or application property */
+			val = (1 << 13) - 1;
+		}
+		maxval = max(maxval, val);
+	}
+	/* Find the lowest shift value to remap the values in the range
+	 * 0..maxval to 0..2^shiftval*63.
+	 */
+	params->fpn_config.shift = 0;
+	while (maxval > 63) {
+		/* MW: BUG, is "63" a system or application property */
+		maxval >>= 1;
+		params->fpn_config.shift++;
+	}
+	/* Adjust the values in the table for the shift value */
+	for (i = 0; i < params->fpn_config.height * params->fpn_config.width; i++)
+		((unsigned short *)params->fpn_config.data)[i] >>= params->fpn_config.shift;
+}
+
+static void
+ia_css_process_kernel(struct ia_css_stream *stream,
+		      struct ia_css_isp_parameters *params,
+		      void (*process)(unsigned int pipe_id,
+				      const struct ia_css_pipeline_stage *stage,
+				      struct ia_css_isp_parameters *params))
+{
+	int i;
+
+	for (i = 0; i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+		struct ia_css_pipeline *pipeline = ia_css_pipe_get_pipeline(pipe);
+		struct ia_css_pipeline_stage *stage;
+
+		/* update the other buffers to the pipe specific copies */
+		for (stage = pipeline->stages; stage; stage = stage->next) {
+			if (!stage || !stage->binary) continue;
+			process(pipeline->pipe_id, stage, params);
+		}
+	}
+}
+
+static int
+sh_css_select_dp_10bpp_config(const struct ia_css_pipe *pipe,
+			      bool *is_dp_10bpp)
+{
+	int err = 0;
+	/* Currently we check if 10bpp DPC configuration is required based
+	 * on the use case,i.e. if BDS and DPC is both enabled. The more cleaner
+	 * design choice would be to expose the type of DPC (either 10bpp or 13bpp)
+	 * using the binary info, but the current control flow does not allow this
+	 * implementation. (This is because the configuration is set before a
+	 * binary is selected, and the binary info is not available)
+	 */
+	if ((!pipe) || (!is_dp_10bpp)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		err = -EINVAL;
+	} else {
+		*is_dp_10bpp = false;
+
+		/* check if DPC is enabled from the host */
+		if (pipe->config.enable_dpc) {
+			/*check if BDS is enabled*/
+			unsigned int required_bds_factor = SH_CSS_BDS_FACTOR_1_00;
+
+			if ((pipe->config.bayer_ds_out_res.width != 0) &&
+			    (pipe->config.bayer_ds_out_res.height != 0)) {
+				if (0 == binarydesc_calculate_bds_factor(
+					pipe->config.input_effective_res,
+					pipe->config.bayer_ds_out_res,
+					&required_bds_factor)) {
+					if (required_bds_factor != SH_CSS_BDS_FACTOR_1_00) {
+						/*we use 10bpp BDS configuration*/
+						*is_dp_10bpp = true;
+					}
+				}
+			}
+		}
+	}
+
+	return err;
+}
+
+int
+sh_css_set_black_frame(struct ia_css_stream *stream,
+		       const struct ia_css_frame *raw_black_frame)
+{
+	struct ia_css_isp_parameters *params;
+	/* this function desperately needs to be moved to the ISP or SP such
+	 * that it can use the DMA.
+	 */
+	unsigned int height, width, y, x, k, data;
+	ia_css_ptr ptr;
+
+	assert(stream);
+	assert(raw_black_frame);
+
+	params = stream->isp_params_configs;
+	height = raw_black_frame->info.res.height;
+	width = raw_black_frame->info.padded_width;
+
+	ptr = raw_black_frame->data
+	+ raw_black_frame->planes.raw.offset;
+
+	IA_CSS_ENTER_PRIVATE("black_frame=%p", raw_black_frame);
+
+	if (params->fpn_config.data &&
+	    (params->fpn_config.width != width || params->fpn_config.height != height)) {
+		kvfree(params->fpn_config.data);
+		params->fpn_config.data = NULL;
+	}
+	if (!params->fpn_config.data) {
+		params->fpn_config.data = kvmalloc(array3_size(height, width, sizeof(short)),
+						   GFP_KERNEL);
+		if (!params->fpn_config.data) {
+			IA_CSS_ERROR("out of memory");
+			IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+			return -ENOMEM;
+		}
+		params->fpn_config.width = width;
+		params->fpn_config.height = height;
+		params->fpn_config.shift = 0;
+	}
+
+	/* store raw to fpntbl */
+	for (y = 0; y < height; y++) {
+		for (x = 0; x < width; x += (ISP_VEC_NELEMS * 2)) {
+			int ofs = y * width + x;
+
+			for (k = 0; k < ISP_VEC_NELEMS; k += 2) {
+				hmm_load(ptr, (void *)(&data), sizeof(int));
+				params->fpn_config.data[ofs + 2 * k] =
+				    (short)(data & 0xFFFF);
+				params->fpn_config.data[ofs + 2 * k + 2] =
+				    (short)((data >> 16) & 0xFFFF);
+				ptr += sizeof(int);	/* byte system address */
+			}
+			for (k = 0; k < ISP_VEC_NELEMS; k += 2) {
+				hmm_load(ptr, (void *)(&data), sizeof(int));
+				params->fpn_config.data[ofs + 2 * k + 1] =
+				    (short)(data & 0xFFFF);
+				params->fpn_config.data[ofs + 2 * k + 3] =
+				    (short)((data >> 16) & 0xFFFF);
+				ptr += sizeof(int);	/* byte system address */
+			}
+		}
+	}
+
+	/* raw -> fpn */
+	convert_raw_to_fpn(params);
+
+	/* overwrite isp parameter */
+	ia_css_process_kernel(stream, params, ia_css_kernel_process_param[IA_CSS_FPN_ID]);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+
+	return 0;
+}
+
+bool
+sh_css_params_set_binning_factor(struct ia_css_stream *stream,
+				 unsigned int binning_fact)
+{
+	struct ia_css_isp_parameters *params;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(stream);
+
+	params = stream->isp_params_configs;
+
+	if (params->sensor_binning != binning_fact) {
+		params->sensor_binning = binning_fact;
+		params->sc_table_changed = true;
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+
+	return params->sc_table_changed;
+}
+
+static void
+sh_css_set_shading_table(struct ia_css_stream *stream,
+			 struct ia_css_isp_parameters *params,
+			 const struct ia_css_shading_table *table)
+{
+	IA_CSS_ENTER_PRIVATE("");
+	if (!table)
+		return;
+	assert(stream);
+
+	if (!table->enable)
+		table = NULL;
+
+	if (table != params->sc_table) {
+		params->sc_table = table;
+		params->sc_table_changed = true;
+		/* Not very clean, this goes to sh_css.c to invalidate the
+		 * shading table for all pipes. Should replaced by a loop
+		 * and a pipe-specific call.
+		 */
+		if (!params->output_frame)
+			sh_css_invalidate_shading_tables(stream);
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+ia_css_params_store_ia_css_host_data(
+    ia_css_ptr ddr_addr,
+    struct ia_css_host_data *data)
+{
+	assert(data);
+	assert(data->address);
+	assert(ddr_addr != mmgr_NULL);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	hmm_store(ddr_addr,
+		   (void *)(data->address),
+		   (size_t)data->size);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+struct ia_css_host_data *
+ia_css_params_alloc_convert_sctbl(
+    const struct ia_css_pipeline_stage *stage,
+    const struct ia_css_shading_table *shading_table)
+{
+	const struct ia_css_binary *binary = stage->binary;
+	struct ia_css_host_data    *sctbl;
+	unsigned int i, j, aligned_width;
+	unsigned int sctbl_size;
+	short int    *ptr;
+
+	assert(binary);
+	assert(shading_table);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (!shading_table) {
+		IA_CSS_LEAVE_PRIVATE("void");
+		return NULL;
+	}
+
+	aligned_width = binary->sctbl_aligned_width_per_color;
+	sctbl_size = shading_table->height * IA_CSS_SC_NUM_COLORS * aligned_width *
+		     sizeof(short);
+
+	sctbl = ia_css_host_data_allocate((size_t)sctbl_size);
+
+	if (!sctbl)
+		return NULL;
+	ptr = (short int *)sctbl->address;
+	memset(ptr,
+	       0,
+	       sctbl_size);
+
+	for (i = 0; i < shading_table->height; i++) {
+		for (j = 0; j < IA_CSS_SC_NUM_COLORS; j++) {
+			memcpy(ptr,
+			       &shading_table->data[j]
+			       [i * shading_table->width],
+			       shading_table->width * sizeof(short));
+			ptr += aligned_width;
+		}
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+	return sctbl;
+}
+
+int ia_css_params_store_sctbl(
+    const struct ia_css_pipeline_stage *stage,
+    ia_css_ptr sc_tbl,
+    const struct ia_css_shading_table  *sc_config)
+{
+	struct ia_css_host_data *isp_sc_tbl;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (!sc_config) {
+		IA_CSS_LEAVE_PRIVATE("void");
+		return 0;
+	}
+
+	isp_sc_tbl = ia_css_params_alloc_convert_sctbl(stage, sc_config);
+	if (!isp_sc_tbl) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return -ENOMEM;
+	}
+	/* store the shading table to ddr */
+	ia_css_params_store_ia_css_host_data(sc_tbl, isp_sc_tbl);
+	ia_css_host_data_free(isp_sc_tbl);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+
+	return 0;
+}
+
+static void
+sh_css_enable_pipeline(const struct ia_css_binary *binary)
+{
+	if (!binary)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	ia_css_isp_param_enable_pipeline(&binary->mem_params);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static int
+ia_css_process_zoom_and_motion(
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_pipeline_stage *first_stage)
+{
+	/* first_stage can be  NULL */
+	const struct ia_css_pipeline_stage *stage;
+	int err = 0;
+	struct ia_css_resolution pipe_in_res;
+
+	pipe_in_res.width = 0;
+	pipe_in_res.height = 0;
+
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	/* Go through all stages to udate uds and cropping */
+	for (stage = first_stage; stage; stage = stage->next) {
+		struct ia_css_binary *binary;
+		/* note: the var below is made static as it is quite large;
+		   if it is not static it ends up on the stack which could
+		   cause issues for drivers
+		*/
+		static struct ia_css_binary tmp_binary;
+
+		const struct ia_css_binary_xinfo *info = NULL;
+
+		binary = stage->binary;
+		if (binary) {
+			info = binary->info;
+		} else {
+			const struct sh_css_binary_args *args = &stage->args;
+			const struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS] = {NULL};
+
+			if (args->out_frame[0])
+				out_infos[0] = &args->out_frame[0]->info;
+			info = &stage->firmware->info.isp;
+			ia_css_binary_fill_info(info, false, false,
+						ATOMISP_INPUT_FORMAT_RAW_10,
+						args->in_frame  ? &args->in_frame->info  : NULL,
+						NULL,
+						out_infos,
+						args->out_vf_frame ? &args->out_vf_frame->info
+						: NULL,
+						&tmp_binary,
+						NULL,
+						-1, true);
+			binary = &tmp_binary;
+			binary->info = info;
+		}
+
+		if (stage == first_stage) {
+			/* we will use pipe_in_res to scale the zoom crop region if needed */
+			pipe_in_res = binary->effective_in_frame_res;
+		}
+
+		assert(stage->stage_num < SH_CSS_MAX_STAGES);
+		if (params->dz_config.zoom_region.resolution.width == 0 &&
+		    params->dz_config.zoom_region.resolution.height == 0) {
+			sh_css_update_uds_and_crop_info(
+			    &info->sp,
+			    &binary->in_frame_info,
+			    &binary->out_frame_info[0],
+			    &binary->dvs_envelope,
+			    &params->dz_config,
+			    &params->motion_config,
+			    &params->uds[stage->stage_num].uds,
+			    &params->uds[stage->stage_num].crop_pos,
+			    stage->enable_zoom);
+		} else {
+			err = sh_css_update_uds_and_crop_info_based_on_zoom_region(
+				  &info->sp,
+				  &binary->in_frame_info,
+				  &binary->out_frame_info[0],
+				  &binary->dvs_envelope,
+				  &params->dz_config,
+				  &params->motion_config,
+				  &params->uds[stage->stage_num].uds,
+				  &params->uds[stage->stage_num].crop_pos,
+				  pipe_in_res,
+				  stage->enable_zoom);
+			if (err)
+				return err;
+		}
+	}
+	params->isp_params_changed = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+	return err;
+}
+
+static void
+sh_css_set_gamma_table(struct ia_css_isp_parameters *params,
+		       const struct ia_css_gamma_table *table)
+{
+	if (!table)
+		return;
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	params->gc_table = *table;
+	params->config_changed[IA_CSS_GC_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_gamma_table(const struct ia_css_isp_parameters *params,
+		       struct ia_css_gamma_table *table)
+{
+	if (!table)
+		return;
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	*table = params->gc_table;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_ctc_table(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ctc_table *table)
+{
+	if (!table)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	params->ctc_table = *table;
+	params->config_changed[IA_CSS_CTC_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_ctc_table(const struct ia_css_isp_parameters *params,
+		     struct ia_css_ctc_table *table)
+{
+	if (!table)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	*table = params->ctc_table;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_macc_table(struct ia_css_isp_parameters *params,
+		      const struct ia_css_macc_table *table)
+{
+	if (!table)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	params->macc_table = *table;
+	params->config_changed[IA_CSS_MACC_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_macc_table(const struct ia_css_isp_parameters *params,
+		      struct ia_css_macc_table *table)
+{
+	if (!table)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	*table = params->macc_table;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void ia_css_morph_table_free(
+    struct ia_css_morph_table *me)
+{
+	unsigned int i;
+
+	if (!me)
+		return;
+
+	IA_CSS_ENTER("");
+
+	for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		if (me->coordinates_x[i]) {
+			kvfree(me->coordinates_x[i]);
+			me->coordinates_x[i] = NULL;
+		}
+		if (me->coordinates_y[i]) {
+			kvfree(me->coordinates_y[i]);
+			me->coordinates_y[i] = NULL;
+		}
+	}
+
+	kvfree(me);
+	IA_CSS_LEAVE("void");
+}
+
+struct ia_css_morph_table *ia_css_morph_table_allocate(
+    unsigned int width,
+    unsigned int height)
+{
+	unsigned int i;
+	struct ia_css_morph_table *me;
+
+	IA_CSS_ENTER("");
+
+	me = kvmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me) {
+		IA_CSS_ERROR("out of memory");
+		return me;
+	}
+
+	for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		me->coordinates_x[i] = NULL;
+		me->coordinates_y[i] = NULL;
+	}
+
+	for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		me->coordinates_x[i] = kvmalloc(height * width *
+						sizeof(*me->coordinates_x[i]),
+						GFP_KERNEL);
+		me->coordinates_y[i] = kvmalloc(height * width *
+						sizeof(*me->coordinates_y[i]),
+						GFP_KERNEL);
+
+		if ((!me->coordinates_x[i]) ||
+		    (!me->coordinates_y[i])) {
+			ia_css_morph_table_free(me);
+			me = NULL;
+			return me;
+		}
+	}
+	me->width = width;
+	me->height = height;
+	IA_CSS_LEAVE("");
+	return me;
+}
+
+static int sh_css_params_default_morph_table(
+    struct ia_css_morph_table **table,
+    const struct ia_css_binary *binary)
+{
+	/* MW 2400 advanced requires different scaling */
+	unsigned int i, j, k, step, width, height;
+	short start_x[IA_CSS_MORPH_TABLE_NUM_PLANES] = { -8, 0, -8, 0, 0, -8 },
+		start_y[IA_CSS_MORPH_TABLE_NUM_PLANES] = { 0, 0, -8, -8, -8, 0 };
+	struct ia_css_morph_table *tab;
+
+	assert(table);
+	assert(binary);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	step = (ISP_VEC_NELEMS / 16) * 128;
+	width = binary->morph_tbl_width;
+	height = binary->morph_tbl_height;
+
+	tab = ia_css_morph_table_allocate(width, height);
+	if (!tab)
+		return -ENOMEM;
+
+	for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		short val_y = start_y[i];
+
+		for (j = 0; j < height; j++) {
+			short val_x = start_x[i];
+			unsigned short *x_ptr, *y_ptr;
+
+			x_ptr = &tab->coordinates_x[i][j * width];
+			y_ptr = &tab->coordinates_y[i][j * width];
+			for (k = 0; k < width;
+			     k++, x_ptr++, y_ptr++, val_x += (short)step) {
+				if (k == 0)
+					*x_ptr = 0;
+				else if (k == width - 1)
+					*x_ptr = val_x + 2 * start_x[i];
+				else
+					*x_ptr = val_x;
+				if (j == 0)
+					*y_ptr = 0;
+				else
+					*y_ptr = val_y;
+			}
+			val_y += (short)step;
+		}
+	}
+	*table = tab;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+
+	return 0;
+}
+
+static void
+sh_css_set_morph_table(struct ia_css_isp_parameters *params,
+		       const struct ia_css_morph_table *table)
+{
+	if (!table)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params);
+	if (table->enable == false)
+		table = NULL;
+	params->morph_table = table;
+	params->morph_table_changed = true;
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+ia_css_translate_3a_statistics(
+    struct ia_css_3a_statistics               *host_stats,
+    const struct ia_css_isp_3a_statistics_map *isp_stats)
+{
+	IA_CSS_ENTER("");
+	if (host_stats->grid.use_dmem) {
+		IA_CSS_LOG("3A: DMEM");
+		ia_css_s3a_dmem_decode(host_stats, isp_stats->dmem_stats);
+	} else {
+		IA_CSS_LOG("3A: VMEM");
+		ia_css_s3a_vmem_decode(host_stats, isp_stats->vmem_stats_hi,
+				       isp_stats->vmem_stats_lo);
+	}
+	IA_CSS_LOG("3A: HMEM");
+	ia_css_s3a_hmem_decode(host_stats, isp_stats->hmem_stats);
+
+	IA_CSS_LEAVE("void");
+}
+
+void
+ia_css_isp_3a_statistics_map_free(struct ia_css_isp_3a_statistics_map *me)
+{
+	if (me) {
+		if (me->data_allocated) {
+			kvfree(me->data_ptr);
+			me->data_ptr = NULL;
+			me->data_allocated = false;
+		}
+		kvfree(me);
+	}
+}
+
+struct ia_css_isp_3a_statistics_map *
+ia_css_isp_3a_statistics_map_allocate(
+    const struct ia_css_isp_3a_statistics *isp_stats,
+    void *data_ptr)
+{
+	struct ia_css_isp_3a_statistics_map *me;
+	/* Windows compiler does not like adding sizes to a void *
+	 * so we use a local char * instead. */
+	char *base_ptr;
+
+	me = kvmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me) {
+		IA_CSS_LEAVE("cannot allocate memory");
+		goto err;
+	}
+
+	me->data_ptr = data_ptr;
+	me->data_allocated = !data_ptr;
+	if (!data_ptr) {
+		me->data_ptr = kvmalloc(isp_stats->size, GFP_KERNEL);
+		if (!me->data_ptr) {
+			IA_CSS_LEAVE("cannot allocate memory");
+			goto err;
+		}
+	}
+	base_ptr = me->data_ptr;
+
+	me->size = isp_stats->size;
+	/* GCC complains when we assign a char * to a void *, so these
+	 * casts are necessary unfortunately. */
+	me->dmem_stats    = (void *)base_ptr;
+	me->vmem_stats_hi = (void *)(base_ptr + isp_stats->dmem_size);
+	me->vmem_stats_lo = (void *)(base_ptr + isp_stats->dmem_size +
+				     isp_stats->vmem_size);
+	me->hmem_stats    = (void *)(base_ptr + isp_stats->dmem_size +
+				     2 * isp_stats->vmem_size);
+
+	IA_CSS_LEAVE("map=%p", me);
+	return me;
+
+err:
+	kvfree(me);
+	return NULL;
+}
+
+int
+ia_css_get_3a_statistics(struct ia_css_3a_statistics           *host_stats,
+			 const struct ia_css_isp_3a_statistics *isp_stats)
+{
+	struct ia_css_isp_3a_statistics_map *map;
+	int ret = 0;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats);
+	assert(isp_stats);
+
+	map = ia_css_isp_3a_statistics_map_allocate(isp_stats, NULL);
+	if (map) {
+		hmm_load(isp_stats->data_ptr, map->data_ptr, isp_stats->size);
+		ia_css_translate_3a_statistics(host_stats, map);
+		ia_css_isp_3a_statistics_map_free(map);
+	} else {
+		IA_CSS_ERROR("out of memory");
+		ret = -ENOMEM;
+	}
+
+	IA_CSS_LEAVE_ERR(ret);
+	return ret;
+}
+
+/* Parameter encoding is not yet orthogonal.
+   This function hnadles some of the exceptions.
+*/
+static void
+ia_css_set_param_exceptions(const struct ia_css_pipe *pipe,
+			    struct ia_css_isp_parameters *params)
+{
+	assert(params);
+
+	/* Copy also to DP. Should be done by the driver. */
+	params->dp_config.gr = params->wb_config.gr;
+	params->dp_config.r  = params->wb_config.r;
+	params->dp_config.b  = params->wb_config.b;
+	params->dp_config.gb = params->wb_config.gb;
+}
+
+static void
+sh_css_set_nr_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_nr_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	ia_css_nr_debug_dtrace(config, IA_CSS_DEBUG_TRACE_PRIVATE);
+	params->nr_config = *config;
+	params->yee_config.nr = *config;
+	params->config_changed[IA_CSS_NR_ID]  = true;
+	params->config_changed[IA_CSS_YEE_ID] = true;
+	params->config_changed[IA_CSS_BNR_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_ee_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_ee_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+	ia_css_ee_debug_dtrace(config, IA_CSS_DEBUG_TRACE_PRIVATE);
+
+	params->ee_config = *config;
+	params->yee_config.ee = *config;
+	params->config_changed[IA_CSS_YEE_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_ee_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_ee_config *config)
+{
+	if (!config)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	assert(params);
+	*config = params->ee_config;
+
+	ia_css_ee_debug_dtrace(config, IA_CSS_DEBUG_TRACE_PRIVATE);
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_pipe_dvs_6axis_config(const struct ia_css_pipe *pipe,
+				 struct ia_css_isp_parameters *params,
+				 const struct ia_css_dvs_6axis_config  *dvs_config)
+{
+	if (!dvs_config)
+		return;
+	assert(params);
+	assert(pipe);
+	assert(dvs_config->height_y == dvs_config->height_uv);
+	assert((dvs_config->width_y - 1) == 2 * (dvs_config->width_uv - 1));
+	assert(pipe->mode < IA_CSS_PIPE_ID_NUM);
+
+	IA_CSS_ENTER_PRIVATE("dvs_config=%p", dvs_config);
+
+	copy_dvs_6axis_table(params->pipe_dvs_6axis_config[pipe->mode], dvs_config);
+
+	params->pipe_dvs_6axis_config_changed[pipe->mode] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_pipe_dvs_6axis_config(const struct ia_css_pipe *pipe,
+				 const struct ia_css_isp_parameters *params,
+				 struct ia_css_dvs_6axis_config *dvs_config)
+{
+	if (!dvs_config)
+		return;
+	assert(params);
+	assert(pipe);
+	assert(dvs_config->height_y == dvs_config->height_uv);
+	assert((dvs_config->width_y - 1) == 2 * dvs_config->width_uv - 1);
+
+	IA_CSS_ENTER_PRIVATE("dvs_config=%p", dvs_config);
+
+	if ((pipe->mode < IA_CSS_PIPE_ID_NUM) &&
+	    (dvs_config->width_y == params->pipe_dvs_6axis_config[pipe->mode]->width_y) &&
+	    (dvs_config->height_y == params->pipe_dvs_6axis_config[pipe->mode]->height_y) &&
+	    (dvs_config->width_uv == params->pipe_dvs_6axis_config[pipe->mode]->width_uv) &&
+	    (dvs_config->height_uv == params->pipe_dvs_6axis_config[pipe->mode]->height_uv)
+	    &&
+	    dvs_config->xcoords_y &&
+	    dvs_config->ycoords_y &&
+	    dvs_config->xcoords_uv &&
+	    dvs_config->ycoords_uv) {
+		copy_dvs_6axis_table(dvs_config, params->pipe_dvs_6axis_config[pipe->mode]);
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_baa_config(struct ia_css_isp_parameters *params,
+		      const struct ia_css_aa_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	params->bds_config = *config;
+	params->config_changed[IA_CSS_BDS_ID] = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_baa_config(const struct ia_css_isp_parameters *params,
+		      struct ia_css_aa_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	*config = params->bds_config;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_dz_config(struct ia_css_isp_parameters *params,
+		     const struct ia_css_dz_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("dx=%d, dy=%d", config->dx, config->dy);
+
+	assert(config->dx <= HRT_GDC_N);
+	assert(config->dy <= HRT_GDC_N);
+
+	params->dz_config = *config;
+	params->dz_config_changed = true;
+	/* JK: Why isp params changed?? */
+	params->isp_params_changed = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_dz_config(const struct ia_css_isp_parameters *params,
+		     struct ia_css_dz_config *config)
+{
+	if (!config)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	*config = params->dz_config;
+
+	IA_CSS_LEAVE_PRIVATE("dx=%d, dy=%d", config->dx, config->dy);
+}
+
+static void
+sh_css_set_motion_vector(struct ia_css_isp_parameters *params,
+			 const struct ia_css_vector *motion)
+{
+	if (!motion)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("x=%d, y=%d", motion->x, motion->y);
+
+	params->motion_config = *motion;
+	/* JK: Why do isp params change? */
+	params->motion_config_changed = true;
+	params->isp_params_changed = true;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_get_motion_vector(const struct ia_css_isp_parameters *params,
+			 struct ia_css_vector *motion)
+{
+	if (!motion)
+		return;
+	assert(params);
+
+	IA_CSS_ENTER_PRIVATE("motion=%p", motion);
+
+	*motion = params->motion_config;
+
+	IA_CSS_LEAVE_PRIVATE("x=%d, y=%d", motion->x, motion->y);
+}
+
+struct ia_css_isp_config *
+sh_css_pipe_isp_config_get(struct ia_css_pipe *pipe)
+{
+	if (!pipe) {
+		IA_CSS_ERROR("pipe=%p", NULL);
+		return NULL;
+	}
+	return pipe->config.p_isp_config;
+}
+
+int
+ia_css_stream_set_isp_config(
+    struct ia_css_stream *stream,
+    const struct ia_css_isp_config *config)
+{
+	return ia_css_stream_set_isp_config_on_pipe(stream, config, NULL);
+}
+
+int
+ia_css_stream_set_isp_config_on_pipe(
+    struct ia_css_stream *stream,
+    const struct ia_css_isp_config *config,
+    struct ia_css_pipe *pipe)
+{
+	int err = 0;
+
+	if ((!stream) || (!config))
+		return -EINVAL;
+
+	IA_CSS_ENTER("stream=%p, config=%p, pipe=%p", stream, config, pipe);
+
+	if (config->output_frame)
+		err = sh_css_set_per_frame_isp_config_on_pipe(stream, config, pipe);
+	else
+		err = sh_css_set_global_isp_config_on_pipe(stream->pipes[0], config, pipe);
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+int
+ia_css_pipe_set_isp_config(struct ia_css_pipe *pipe,
+			   struct ia_css_isp_config *config)
+{
+	struct ia_css_pipe *pipe_in = pipe;
+	int err = 0;
+
+	IA_CSS_ENTER("pipe=%p", pipe);
+
+	if ((!pipe) || (!pipe->stream))
+		return -EINVAL;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "config=%p\n", config);
+
+	if (config->output_frame)
+		err = sh_css_set_per_frame_isp_config_on_pipe(pipe->stream, config, pipe);
+	else
+		err = sh_css_set_global_isp_config_on_pipe(pipe, config, pipe_in);
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+static int
+sh_css_set_global_isp_config_on_pipe(
+    struct ia_css_pipe *curr_pipe,
+    const struct ia_css_isp_config *config,
+    struct ia_css_pipe *pipe)
+{
+	int err = 0;
+	int err1 = 0;
+	int err2 = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream=%p, config=%p, pipe=%p", curr_pipe, config, pipe);
+
+	err1 = sh_css_init_isp_params_from_config(curr_pipe, curr_pipe->stream->isp_params_configs, config, pipe);
+
+	/* Now commit all changes to the SP */
+	err2 = sh_css_param_update_isp_params(curr_pipe, curr_pipe->stream->isp_params_configs, sh_css_sp_is_running(), pipe);
+
+	/* The following code is intentional. The sh_css_init_isp_params_from_config interface
+	 * throws an error when both DPC and BDS is enabled. The CSS API must pass this error
+	 * information to the caller, ie. the host. We do not return this error immediately,
+	 * but instead continue with updating the ISP params to enable testing of features
+	 * which are currently in TR phase. */
+
+	err = (err1 != 0) ? err1 : ((err2 != 0) ? err2 : err);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_set_per_frame_isp_config_on_pipe(
+    struct ia_css_stream *stream,
+    const struct ia_css_isp_config *config,
+    struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+	bool per_frame_config_created = false;
+	int err = 0;
+	int err1 = 0;
+	int err2 = 0;
+	int err3 = 0;
+
+	struct sh_css_ddr_address_map *ddr_ptrs;
+	struct sh_css_ddr_address_map_size *ddr_ptrs_size;
+	struct ia_css_isp_parameters *params;
+
+	IA_CSS_ENTER_PRIVATE("stream=%p, config=%p, pipe=%p", stream, config, pipe);
+
+	if (!pipe) {
+		err = -EINVAL;
+		goto exit;
+	}
+
+	/* create per-frame ISP params object with default values
+	 * from stream->isp_params_configs if one doesn't already exist
+	*/
+	if (!stream->per_frame_isp_params_configs) {
+		err = sh_css_create_isp_params(stream,
+					       &stream->per_frame_isp_params_configs);
+		if (err)
+			goto exit;
+		per_frame_config_created = true;
+	}
+
+	params = stream->per_frame_isp_params_configs;
+
+	/* update new ISP params object with the new config */
+	if (!sh_css_init_isp_params_from_global(stream, params, false, pipe)) {
+		err1 = -EINVAL;
+	}
+
+	err2 = sh_css_init_isp_params_from_config(stream->pipes[0], params, config, pipe);
+
+	if (per_frame_config_created) {
+		ddr_ptrs = &params->ddr_ptrs;
+		ddr_ptrs_size = &params->ddr_ptrs_size;
+		/* create per pipe reference to general ddr_ptrs */
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+			ref_sh_css_ddr_address_map(ddr_ptrs, &params->pipe_ddr_ptrs[i]);
+			params->pipe_ddr_ptrs_size[i] = *ddr_ptrs_size;
+		}
+	}
+
+	/* now commit to ddr */
+	err3 = sh_css_param_update_isp_params(stream->pipes[0], params, sh_css_sp_is_running(), pipe);
+
+	/* The following code is intentional. The sh_css_init_sp_params_from_config and
+	 * sh_css_init_isp_params_from_config throws an error when both DPC and BDS is enabled.
+	 * The CSS API must pass this error information to the caller, ie. the host.
+	 * We do not return this error immediately, but instead continue with updating the ISP params
+	 *  to enable testing of features which are currently in TR phase. */
+	err = (err1 != 0) ? err1 :
+	      (err2 != 0) ? err2 :
+	      (err3 != 0) ? err3 : err;
+exit:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_init_isp_params_from_config(struct ia_css_pipe *pipe,
+				   struct ia_css_isp_parameters *params,
+				   const struct ia_css_isp_config *config,
+				   struct ia_css_pipe *pipe_in)
+{
+	int err = 0;
+	bool is_dp_10bpp = true;
+
+	assert(pipe);
+
+	IA_CSS_ENTER_PRIVATE("pipe=%p, config=%p, params=%p", pipe, config, params);
+
+	ia_css_set_configs(params, config);
+
+	sh_css_set_nr_config(params, config->nr_config);
+	sh_css_set_ee_config(params, config->ee_config);
+	sh_css_set_baa_config(params, config->baa_config);
+	if ((pipe->mode < IA_CSS_PIPE_ID_NUM) &&
+	    (params->pipe_dvs_6axis_config[pipe->mode]))
+		sh_css_set_pipe_dvs_6axis_config(pipe, params, config->dvs_6axis_config);
+	sh_css_set_dz_config(params, config->dz_config);
+	sh_css_set_motion_vector(params, config->motion_vector);
+	sh_css_set_shading_table(pipe->stream, params, config->shading_table);
+	sh_css_set_morph_table(params, config->morph_table);
+	sh_css_set_macc_table(params, config->macc_table);
+	sh_css_set_gamma_table(params, config->gamma_table);
+	sh_css_set_ctc_table(params, config->ctc_table);
+	/* ------ deprecated(bz675) : from ------ */
+	sh_css_set_shading_settings(params, config->shading_settings);
+	/* ------ deprecated(bz675) : to ------ */
+
+	params->dis_coef_table_changed = (config->dvs_coefs);
+	params->dvs2_coef_table_changed = (config->dvs2_coefs);
+
+	params->output_frame = config->output_frame;
+	params->isp_parameters_id = config->isp_config_id;
+
+	if (0 ==
+	    sh_css_select_dp_10bpp_config(pipe, &is_dp_10bpp)) {
+		/* return an error when both DPC and BDS is enabled by the
+		 * user. */
+		/* we do not exit from this point immediately to allow internal
+		 * firmware feature testing. */
+		if (is_dp_10bpp) {
+			err = -EINVAL;
+		}
+	} else {
+		err = -EINVAL;
+		goto exit;
+	}
+
+	ia_css_set_param_exceptions(pipe, params);
+
+exit:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+void
+ia_css_stream_get_isp_config(
+    const struct ia_css_stream *stream,
+    struct ia_css_isp_config *config)
+{
+	IA_CSS_ENTER("void");
+	ia_css_pipe_get_isp_config(stream->pipes[0], config);
+	IA_CSS_LEAVE("void");
+}
+
+void
+ia_css_pipe_get_isp_config(struct ia_css_pipe *pipe,
+			   struct ia_css_isp_config *config)
+{
+	struct ia_css_isp_parameters *params = NULL;
+
+	assert(config);
+
+	IA_CSS_ENTER("config=%p", config);
+
+	params = pipe->stream->isp_params_configs;
+	assert(params);
+
+	ia_css_get_configs(params, config);
+
+	sh_css_get_ee_config(params, config->ee_config);
+	sh_css_get_baa_config(params, config->baa_config);
+	sh_css_get_pipe_dvs_6axis_config(pipe, params, config->dvs_6axis_config);
+	sh_css_get_macc_table(params, config->macc_table);
+	sh_css_get_gamma_table(params, config->gamma_table);
+	sh_css_get_ctc_table(params, config->ctc_table);
+	sh_css_get_dz_config(params, config->dz_config);
+	sh_css_get_motion_vector(params, config->motion_vector);
+	/* ------ deprecated(bz675) : from ------ */
+	sh_css_get_shading_settings(params, config->shading_settings);
+	/* ------ deprecated(bz675) : to ------ */
+
+	config->output_frame = params->output_frame;
+	config->isp_config_id = params->isp_parameters_id;
+
+	IA_CSS_LEAVE("void");
+}
+
+/*
+ * coding style says the return of "mmgr_NULL" is the error signal
+ *
+ * Deprecated: Implement mmgr_realloc()
+ */
+static bool realloc_isp_css_mm_buf(
+    ia_css_ptr *curr_buf,
+    size_t *curr_size,
+    size_t needed_size,
+    bool force,
+    int *err)
+{
+	s32 id;
+
+	*err = 0;
+	/* Possible optimization: add a function sh_css_isp_css_mm_realloc()
+	 * and implement on top of hmm. */
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (!force && *curr_size >= needed_size) {
+		IA_CSS_LEAVE_PRIVATE("false");
+		return false;
+	}
+	/* don't reallocate if single ref to buffer and same size */
+	if (*curr_size == needed_size && ia_css_refcount_is_single(*curr_buf)) {
+		IA_CSS_LEAVE_PRIVATE("false");
+		return false;
+	}
+
+	id = IA_CSS_REFCOUNT_PARAM_BUFFER;
+	ia_css_refcount_decrement(id, *curr_buf);
+	*curr_buf = ia_css_refcount_increment(id, hmm_alloc(needed_size));
+	if (!*curr_buf) {
+		*err = -ENOMEM;
+		*curr_size = 0;
+	} else {
+		*curr_size = needed_size;
+	}
+	IA_CSS_LEAVE_PRIVATE("true");
+	return true;
+}
+
+static bool reallocate_buffer(
+    ia_css_ptr *curr_buf,
+    size_t *curr_size,
+    size_t needed_size,
+    bool force,
+    int *err)
+{
+	bool ret;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	ret = realloc_isp_css_mm_buf(curr_buf,
+				     curr_size, needed_size, force, err);
+
+	IA_CSS_LEAVE_PRIVATE("ret=%d", ret);
+	return ret;
+}
+
+struct ia_css_isp_3a_statistics *
+ia_css_isp_3a_statistics_allocate(const struct ia_css_3a_grid_info *grid)
+{
+	struct ia_css_isp_3a_statistics *me;
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	assert(grid);
+
+	/* MW: Does "grid->enable" also control the histogram output ?? */
+	if (!grid->enable)
+		return NULL;
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	if (grid->use_dmem) {
+		me->dmem_size = sizeof(struct ia_css_3a_output) *
+				grid->aligned_width *
+				grid->aligned_height;
+	} else {
+		me->vmem_size = ISP_S3ATBL_HI_LO_STRIDE_BYTES *
+				grid->aligned_height;
+	}
+	me->hmem_size = sizeof_hmem(HMEM0_ID);
+
+	/* All subsections need to be aligned to the system bus width */
+	me->dmem_size = CEIL_MUL(me->dmem_size, HIVE_ISP_DDR_WORD_BYTES);
+	me->vmem_size = CEIL_MUL(me->vmem_size, HIVE_ISP_DDR_WORD_BYTES);
+	me->hmem_size = CEIL_MUL(me->hmem_size, HIVE_ISP_DDR_WORD_BYTES);
+
+	me->size = me->dmem_size + me->vmem_size * 2 + me->hmem_size;
+	me->data_ptr = hmm_alloc(me->size);
+	if (me->data_ptr == mmgr_NULL) {
+		kvfree(me);
+		me = NULL;
+		goto err;
+	}
+	if (me->dmem_size)
+		me->data.dmem.s3a_tbl = me->data_ptr;
+	if (me->vmem_size) {
+		me->data.vmem.s3a_tbl_hi = me->data_ptr + me->dmem_size;
+		me->data.vmem.s3a_tbl_lo = me->data_ptr + me->dmem_size + me->vmem_size;
+	}
+	if (me->hmem_size)
+		me->data_hmem.rgby_tbl = me->data_ptr + me->dmem_size + 2 * me->vmem_size;
+
+err:
+	IA_CSS_LEAVE("return=%p", me);
+	return me;
+}
+
+void
+ia_css_isp_3a_statistics_free(struct ia_css_isp_3a_statistics *me)
+{
+	if (me) {
+		hmm_free(me->data_ptr);
+		kvfree(me);
+	}
+}
+
+struct ia_css_isp_skc_dvs_statistics *ia_css_skc_dvs_statistics_allocate(void)
+{
+	return NULL;
+}
+
+struct ia_css_metadata *
+ia_css_metadata_allocate(const struct ia_css_metadata_info *metadata_info)
+{
+	struct ia_css_metadata *md = NULL;
+
+	IA_CSS_ENTER("");
+
+	if (metadata_info->size == 0)
+		return NULL;
+
+	md = kvmalloc(sizeof(*md), GFP_KERNEL);
+	if (!md)
+		goto error;
+
+	md->info = *metadata_info;
+	md->exp_id = 0;
+	md->address = hmm_alloc(metadata_info->size);
+	if (md->address == mmgr_NULL)
+		goto error;
+
+	IA_CSS_LEAVE("return=%p", md);
+	return md;
+
+error:
+	ia_css_metadata_free(md);
+	IA_CSS_LEAVE("return=%p", NULL);
+	return NULL;
+}
+
+void
+ia_css_metadata_free(struct ia_css_metadata *me)
+{
+	if (me) {
+		/* The enter and leave macros are placed inside
+		 * the condition to avoid false logging of metadata
+		 * free events when metadata is disabled.
+		 * We found this to be confusing during development
+		 * and debugging. */
+		IA_CSS_ENTER("me=%p", me);
+		hmm_free(me->address);
+		kvfree(me);
+		IA_CSS_LEAVE("void");
+	}
+}
+
+void
+ia_css_metadata_free_multiple(unsigned int num_bufs,
+			      struct ia_css_metadata **bufs)
+{
+	unsigned int i;
+
+	if (bufs) {
+		for (i = 0; i < num_bufs; i++)
+			ia_css_metadata_free(bufs[i]);
+	}
+}
+
+static unsigned int g_param_buffer_dequeue_count;
+static unsigned int g_param_buffer_enqueue_count;
+
+int
+ia_css_stream_isp_parameters_init(struct ia_css_stream *stream)
+{
+	int err = 0;
+	unsigned int i;
+	struct sh_css_ddr_address_map *ddr_ptrs;
+	struct sh_css_ddr_address_map_size *ddr_ptrs_size;
+	struct ia_css_isp_parameters *params;
+
+	assert(stream);
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (!stream) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-EINVAL);
+		return -EINVAL;
+	}
+	/* TMP: tracking of paramsets */
+	g_param_buffer_dequeue_count = 0;
+	g_param_buffer_enqueue_count = 0;
+
+	stream->per_frame_isp_params_configs = NULL;
+	err = sh_css_create_isp_params(stream,
+				       &stream->isp_params_configs);
+	if (err)
+		goto ERR;
+
+	params = stream->isp_params_configs;
+	if (!sh_css_init_isp_params_from_global(stream, params, true, NULL)) {
+		/* we do not return the error immediately to enable internal
+		 * firmware feature testing */
+		err = -EINVAL;
+	}
+
+	ddr_ptrs = &params->ddr_ptrs;
+	ddr_ptrs_size = &params->ddr_ptrs_size;
+
+	/* create per pipe reference to general ddr_ptrs */
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		ref_sh_css_ddr_address_map(ddr_ptrs, &params->pipe_ddr_ptrs[i]);
+		params->pipe_ddr_ptrs_size[i] = *ddr_ptrs_size;
+	}
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static void
+ia_css_set_sdis_config(
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_dvs_coefficients *dvs_coefs)
+{
+	ia_css_set_sdis_horicoef_config(params, dvs_coefs);
+	ia_css_set_sdis_vertcoef_config(params, dvs_coefs);
+	ia_css_set_sdis_horiproj_config(params, dvs_coefs);
+	ia_css_set_sdis_vertproj_config(params, dvs_coefs);
+}
+
+static void
+ia_css_set_sdis2_config(
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_dvs2_coefficients *dvs2_coefs)
+{
+	ia_css_set_sdis2_horicoef_config(params, dvs2_coefs);
+	ia_css_set_sdis2_vertcoef_config(params, dvs2_coefs);
+	ia_css_set_sdis2_horiproj_config(params, dvs2_coefs);
+	ia_css_set_sdis2_vertproj_config(params, dvs2_coefs);
+}
+
+static int
+sh_css_create_isp_params(struct ia_css_stream *stream,
+			 struct ia_css_isp_parameters **isp_params_out)
+{
+	bool succ = true;
+	unsigned int i;
+	struct sh_css_ddr_address_map *ddr_ptrs;
+	struct sh_css_ddr_address_map_size *ddr_ptrs_size;
+	int err;
+	size_t params_size;
+	struct ia_css_isp_parameters *params =
+	kvmalloc(sizeof(struct ia_css_isp_parameters), GFP_KERNEL);
+
+	if (!params) {
+		*isp_params_out = NULL;
+		err = -ENOMEM;
+		IA_CSS_ERROR("%s:%d error: cannot allocate memory", __FILE__, __LINE__);
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	} else {
+		memset(params, 0, sizeof(struct ia_css_isp_parameters));
+	}
+
+	ddr_ptrs = &params->ddr_ptrs;
+	ddr_ptrs_size = &params->ddr_ptrs_size;
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		memset(&params->pipe_ddr_ptrs[i], 0,
+		       sizeof(params->pipe_ddr_ptrs[i]));
+		memset(&params->pipe_ddr_ptrs_size[i], 0,
+		       sizeof(params->pipe_ddr_ptrs_size[i]));
+	}
+
+	memset(ddr_ptrs, 0, sizeof(*ddr_ptrs));
+	memset(ddr_ptrs_size, 0, sizeof(*ddr_ptrs_size));
+
+	params_size = sizeof(params->uds);
+	ddr_ptrs_size->isp_param = params_size;
+	ddr_ptrs->isp_param =
+	ia_css_refcount_increment(IA_CSS_REFCOUNT_PARAM_BUFFER,
+				  hmm_alloc(params_size));
+	succ &= (ddr_ptrs->isp_param != mmgr_NULL);
+
+	ddr_ptrs_size->macc_tbl = sizeof(struct ia_css_macc_table);
+	ddr_ptrs->macc_tbl =
+	ia_css_refcount_increment(IA_CSS_REFCOUNT_PARAM_BUFFER,
+				  hmm_alloc(sizeof(struct ia_css_macc_table)));
+	succ &= (ddr_ptrs->macc_tbl != mmgr_NULL);
+
+	*isp_params_out = params;
+
+	if (!succ)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static bool
+sh_css_init_isp_params_from_global(struct ia_css_stream *stream,
+				   struct ia_css_isp_parameters *params,
+				   bool use_default_config,
+				   struct ia_css_pipe *pipe_in)
+{
+	bool retval = true;
+	int i = 0;
+	bool is_dp_10bpp = true;
+	unsigned int isp_pipe_version = ia_css_pipe_get_isp_pipe_version(
+					    stream->pipes[0]);
+	struct ia_css_isp_parameters *stream_params = stream->isp_params_configs;
+
+	if (!use_default_config && !stream_params) {
+		retval = false;
+		goto exit;
+	}
+
+	params->output_frame = NULL;
+	params->isp_parameters_id = 0;
+
+	if (use_default_config) {
+		ia_css_set_xnr3_config(params, &default_xnr3_config);
+
+		sh_css_set_nr_config(params, &default_nr_config);
+		sh_css_set_ee_config(params, &default_ee_config);
+		if (isp_pipe_version == SH_CSS_ISP_PIPE_VERSION_1)
+			sh_css_set_macc_table(params, &default_macc_table);
+		else if (isp_pipe_version == SH_CSS_ISP_PIPE_VERSION_2_2)
+			sh_css_set_macc_table(params, &default_macc2_table);
+		sh_css_set_gamma_table(params, &default_gamma_table);
+		sh_css_set_ctc_table(params, &default_ctc_table);
+		sh_css_set_baa_config(params, &default_baa_config);
+		sh_css_set_dz_config(params, &default_dz_config);
+		/* ------ deprecated(bz675) : from ------ */
+		sh_css_set_shading_settings(params, &default_shading_settings);
+		/* ------ deprecated(bz675) : to ------ */
+
+		ia_css_set_s3a_config(params, &default_3a_config);
+		ia_css_set_wb_config(params, &default_wb_config);
+		ia_css_set_csc_config(params, &default_cc_config);
+		ia_css_set_tnr_config(params, &default_tnr_config);
+		ia_css_set_ob_config(params, &default_ob_config);
+		ia_css_set_dp_config(params, &default_dp_config);
+
+		ia_css_set_param_exceptions(pipe_in, params);
+
+		ia_css_set_de_config(params, &default_de_config);
+		ia_css_set_gc_config(params, &default_gc_config);
+		ia_css_set_anr_config(params, &default_anr_config);
+		ia_css_set_anr2_config(params, &default_anr_thres);
+		ia_css_set_ce_config(params, &default_ce_config);
+		ia_css_set_xnr_table_config(params, &default_xnr_table);
+		ia_css_set_ecd_config(params, &default_ecd_config);
+		ia_css_set_ynr_config(params, &default_ynr_config);
+		ia_css_set_fc_config(params, &default_fc_config);
+		ia_css_set_cnr_config(params, &default_cnr_config);
+		ia_css_set_macc_config(params, &default_macc_config);
+		ia_css_set_ctc_config(params, &default_ctc_config);
+		ia_css_set_aa_config(params, &default_aa_config);
+		ia_css_set_r_gamma_config(params, &default_r_gamma_table);
+		ia_css_set_g_gamma_config(params, &default_g_gamma_table);
+		ia_css_set_b_gamma_config(params, &default_b_gamma_table);
+		ia_css_set_yuv2rgb_config(params, &default_yuv2rgb_cc_config);
+		ia_css_set_rgb2yuv_config(params, &default_rgb2yuv_cc_config);
+		ia_css_set_xnr_config(params, &default_xnr_config);
+		ia_css_set_sdis_config(params, &default_sdis_config);
+		ia_css_set_sdis2_config(params, &default_sdis2_config);
+		ia_css_set_formats_config(params, &default_formats_config);
+
+		params->fpn_config.data = NULL;
+		params->config_changed[IA_CSS_FPN_ID] = true;
+		params->fpn_config.enabled = 0;
+
+		params->motion_config = default_motion_config;
+		params->motion_config_changed = true;
+
+		params->morph_table = NULL;
+		params->morph_table_changed = true;
+
+		params->sc_table = NULL;
+		params->sc_table_changed = true;
+
+		ia_css_sdis2_clear_coefficients(&params->dvs2_coefs);
+		params->dvs2_coef_table_changed = true;
+
+		ia_css_sdis_clear_coefficients(&params->dvs_coefs);
+		params->dis_coef_table_changed = true;
+	} else {
+		ia_css_set_xnr3_config(params, &stream_params->xnr3_config);
+
+		sh_css_set_nr_config(params, &stream_params->nr_config);
+		sh_css_set_ee_config(params, &stream_params->ee_config);
+		if (isp_pipe_version == SH_CSS_ISP_PIPE_VERSION_1)
+			sh_css_set_macc_table(params, &stream_params->macc_table);
+		else if (isp_pipe_version == SH_CSS_ISP_PIPE_VERSION_2_2)
+			sh_css_set_macc_table(params, &stream_params->macc_table);
+		sh_css_set_gamma_table(params, &stream_params->gc_table);
+		sh_css_set_ctc_table(params, &stream_params->ctc_table);
+		sh_css_set_baa_config(params, &stream_params->bds_config);
+		sh_css_set_dz_config(params, &stream_params->dz_config);
+		/* ------ deprecated(bz675) : from ------ */
+		sh_css_set_shading_settings(params, &stream_params->shading_settings);
+		/* ------ deprecated(bz675) : to ------ */
+
+		ia_css_set_s3a_config(params, &stream_params->s3a_config);
+		ia_css_set_wb_config(params, &stream_params->wb_config);
+		ia_css_set_csc_config(params, &stream_params->cc_config);
+		ia_css_set_tnr_config(params, &stream_params->tnr_config);
+		ia_css_set_ob_config(params, &stream_params->ob_config);
+		ia_css_set_dp_config(params, &stream_params->dp_config);
+		ia_css_set_de_config(params, &stream_params->de_config);
+		ia_css_set_gc_config(params, &stream_params->gc_config);
+		ia_css_set_anr_config(params, &stream_params->anr_config);
+		ia_css_set_anr2_config(params, &stream_params->anr_thres);
+		ia_css_set_ce_config(params, &stream_params->ce_config);
+		ia_css_set_xnr_table_config(params, &stream_params->xnr_table);
+		ia_css_set_ecd_config(params, &stream_params->ecd_config);
+		ia_css_set_ynr_config(params, &stream_params->ynr_config);
+		ia_css_set_fc_config(params, &stream_params->fc_config);
+		ia_css_set_cnr_config(params, &stream_params->cnr_config);
+		ia_css_set_macc_config(params, &stream_params->macc_config);
+		ia_css_set_ctc_config(params, &stream_params->ctc_config);
+		ia_css_set_aa_config(params, &stream_params->aa_config);
+		ia_css_set_r_gamma_config(params, &stream_params->r_gamma_table);
+		ia_css_set_g_gamma_config(params, &stream_params->g_gamma_table);
+		ia_css_set_b_gamma_config(params, &stream_params->b_gamma_table);
+		ia_css_set_yuv2rgb_config(params, &stream_params->yuv2rgb_cc_config);
+		ia_css_set_rgb2yuv_config(params, &stream_params->rgb2yuv_cc_config);
+		ia_css_set_xnr_config(params, &stream_params->xnr_config);
+		ia_css_set_formats_config(params, &stream_params->formats_config);
+
+		for (i = 0; i < stream->num_pipes; i++) {
+			if (0 ==
+			    sh_css_select_dp_10bpp_config(stream->pipes[i], &is_dp_10bpp)) {
+				/* set the return value as false if both DPC and
+				 * BDS is enabled by the user. But we do not return
+				 * the value immediately to enable internal firmware
+				 * feature testing. */
+				retval = !is_dp_10bpp;
+				/* FIXME: should it ignore this error? */
+			} else {
+				retval = false;
+				goto exit;
+			}
+		}
+
+		ia_css_set_param_exceptions(pipe_in, params);
+
+		params->fpn_config.data = stream_params->fpn_config.data;
+		params->config_changed[IA_CSS_FPN_ID] =
+		    stream_params->config_changed[IA_CSS_FPN_ID];
+		params->fpn_config.enabled = stream_params->fpn_config.enabled;
+
+		sh_css_set_motion_vector(params, &stream_params->motion_config);
+		sh_css_set_morph_table(params, stream_params->morph_table);
+
+		if (stream_params->sc_table) {
+			sh_css_set_shading_table(stream, params, stream_params->sc_table);
+		} else {
+			params->sc_table = NULL;
+			params->sc_table_changed = true;
+		}
+
+		/* Only IA_CSS_PIPE_ID_VIDEO & IA_CSS_PIPE_ID_CAPTURE will support dvs_6axis_config*/
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+			if (stream_params->pipe_dvs_6axis_config[i]) {
+				if (params->pipe_dvs_6axis_config[i]) {
+					copy_dvs_6axis_table(params->pipe_dvs_6axis_config[i],
+							     stream_params->pipe_dvs_6axis_config[i]);
+				} else {
+					params->pipe_dvs_6axis_config[i] =
+					    generate_dvs_6axis_table_from_config(stream_params->pipe_dvs_6axis_config[i]);
+				}
+			}
+		}
+		ia_css_set_sdis_config(params, &stream_params->dvs_coefs);
+		params->dis_coef_table_changed = stream_params->dis_coef_table_changed;
+
+		ia_css_set_sdis2_config(params, &stream_params->dvs2_coefs);
+		params->dvs2_coef_table_changed = stream_params->dvs2_coef_table_changed;
+		params->sensor_binning = stream_params->sensor_binning;
+	}
+
+exit:
+	return retval;
+}
+
+int
+sh_css_params_init(void)
+{
+	int i, p;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	/* TMP: tracking of paramsets */
+	g_param_buffer_dequeue_count = 0;
+	g_param_buffer_enqueue_count = 0;
+
+	for (p = 0; p < IA_CSS_PIPE_ID_NUM; p++) {
+		for (i = 0; i < SH_CSS_MAX_STAGES; i++) {
+			xmem_sp_stage_ptrs[p][i] =
+			ia_css_refcount_increment(-1,
+						  hmm_alloc(sizeof(struct sh_css_sp_stage)));
+			xmem_isp_stage_ptrs[p][i] =
+			ia_css_refcount_increment(-1,
+						  hmm_alloc(sizeof(struct sh_css_sp_stage)));
+
+			if ((xmem_sp_stage_ptrs[p][i] == mmgr_NULL) ||
+			    (xmem_isp_stage_ptrs[p][i] == mmgr_NULL)) {
+				sh_css_params_uninit();
+				IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+				return -ENOMEM;
+			}
+
+			hmm_set(xmem_sp_stage_ptrs[p][i], 0, sizeof(struct sh_css_sp_stage));
+			hmm_set(xmem_isp_stage_ptrs[p][i], 0, sizeof(struct sh_css_sp_stage));
+		}
+	}
+
+	ia_css_config_gamma_table();
+	ia_css_config_ctc_table();
+	ia_css_config_rgb_gamma_tables();
+	ia_css_config_xnr_table();
+
+	sp_ddr_ptrs = ia_css_refcount_increment(-1,
+						hmm_alloc(CEIL_MUL(sizeof(struct sh_css_ddr_address_map),
+								   HIVE_ISP_DDR_WORD_BYTES)));
+	xmem_sp_group_ptrs = ia_css_refcount_increment(-1,
+						       hmm_alloc(sizeof(struct sh_css_sp_group)));
+
+	if ((sp_ddr_ptrs == mmgr_NULL) ||
+	    (xmem_sp_group_ptrs == mmgr_NULL)) {
+		ia_css_uninit();
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return -ENOMEM;
+	}
+	hmm_set(sp_ddr_ptrs, 0, CEIL_MUL(sizeof(struct sh_css_ddr_address_map),
+					 HIVE_ISP_DDR_WORD_BYTES));
+	hmm_set(xmem_sp_group_ptrs, 0, sizeof(struct sh_css_sp_group));
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+static void host_lut_store(const void *lut)
+{
+	unsigned int i;
+
+	for (i = 0; i < N_GDC_ID; i++)
+		gdc_lut_store((gdc_ID_t)i, (const int (*)[HRT_GDC_N]) lut);
+}
+
+int ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
+	const void *lut)
+{
+	int err = 0;
+	bool stream_started = false;
+
+	IA_CSS_ENTER("pipe=%p lut=%p", pipe, lut);
+
+	if (!lut || !pipe) {
+		err = -EINVAL;
+		IA_CSS_LEAVE("err=%d", err);
+		return err;
+	}
+
+	/* If the pipe belongs to a stream and the stream has started, it is not
+	 * safe to store lut to gdc HW. If pipe->stream is NULL, then no stream is
+	 * created with this pipe, so it is safe to do this operation as long as
+	 * ia_css_init() has been called. */
+	if (pipe->stream && pipe->stream->started) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "unable to set scaler lut since stream has started\n");
+		stream_started = true;
+		err = -ENOTSUPP;
+	}
+
+	/* Free any existing tables. */
+	if (pipe->scaler_pp_lut != mmgr_NULL) {
+		hmm_free(pipe->scaler_pp_lut);
+		pipe->scaler_pp_lut = mmgr_NULL;
+	}
+
+	if (!stream_started) {
+		pipe->scaler_pp_lut = hmm_alloc(sizeof(zoom_table));
+
+		if (pipe->scaler_pp_lut == mmgr_NULL) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+					    "unable to allocate scaler_pp_lut\n");
+			err = -ENOMEM;
+		} else {
+			gdc_lut_convert_to_isp_format((const int(*)[HRT_GDC_N])lut,
+						      interleaved_lut_temp);
+			hmm_store(pipe->scaler_pp_lut,
+				   (int *)interleaved_lut_temp,
+				   sizeof(zoom_table));
+		}
+	}
+
+	IA_CSS_LEAVE("lut(%u) err=%d", pipe->scaler_pp_lut, err);
+	return err;
+}
+
+/* if pipe is NULL, returns default lut addr. */
+ia_css_ptr sh_css_pipe_get_pp_gdc_lut(const struct ia_css_pipe *pipe)
+{
+	assert(pipe);
+
+	if (pipe->scaler_pp_lut != mmgr_NULL)
+		return pipe->scaler_pp_lut;
+	else
+		return sh_css_params_get_default_gdc_lut();
+}
+
+int sh_css_params_map_and_store_default_gdc_lut(void)
+{
+	int err = 0;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	/* Is table already mapped? Nothing to do if it is mapped. */
+	if (default_gdc_lut != mmgr_NULL)
+		return err;
+
+	host_lut_store((void *)zoom_table);
+
+	default_gdc_lut = hmm_alloc(sizeof(zoom_table));
+
+	if (default_gdc_lut == mmgr_NULL)
+		return -ENOMEM;
+
+	gdc_lut_convert_to_isp_format((const int(*)[HRT_GDC_N])zoom_table,
+				      interleaved_lut_temp);
+	hmm_store(default_gdc_lut, (int *)interleaved_lut_temp,
+		   sizeof(zoom_table));
+
+	IA_CSS_LEAVE_PRIVATE("lut(%u) err=%d", default_gdc_lut, err);
+	return err;
+}
+
+void sh_css_params_free_default_gdc_lut(void)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (default_gdc_lut != mmgr_NULL) {
+		hmm_free(default_gdc_lut);
+		default_gdc_lut = mmgr_NULL;
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+ia_css_ptr sh_css_params_get_default_gdc_lut(void)
+{
+	return default_gdc_lut;
+}
+
+static void free_param_set_callback(
+    ia_css_ptr ptr)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	free_ia_css_isp_parameter_set_info(ptr);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void free_buffer_callback(
+    ia_css_ptr ptr)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	hmm_free(ptr);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+sh_css_param_clear_param_sets(void)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	ia_css_refcount_clear(IA_CSS_REFCOUNT_PARAM_SET_POOL, &free_param_set_callback);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/*
+ * MW: we can define hmm_free() to return a NULL
+ * then you can write ptr = hmm_free(ptr);
+ */
+#define safe_free(id, x)      \
+	do {                  \
+		ia_css_refcount_decrement(id, x);     \
+		(x) = mmgr_NULL;  \
+	} while (0)
+
+static void free_map(struct sh_css_ddr_address_map *map)
+{
+	unsigned int i;
+
+	ia_css_ptr *addrs = (ia_css_ptr *)map;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	/* free buffers */
+	for (i = 0; i < (sizeof(struct sh_css_ddr_address_map_size) /
+			 sizeof(size_t)); i++) {
+		if (addrs[i] == mmgr_NULL)
+			continue;
+		safe_free(IA_CSS_REFCOUNT_PARAM_BUFFER, addrs[i]);
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+ia_css_stream_isp_parameters_uninit(struct ia_css_stream *stream)
+{
+	int i;
+	struct ia_css_isp_parameters *params = stream->isp_params_configs;
+	struct ia_css_isp_parameters *per_frame_params =
+		    stream->per_frame_isp_params_configs;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	if (!params) {
+		IA_CSS_LEAVE_PRIVATE("isp_param_configs is NULL");
+		return;
+	}
+
+	/* free existing ddr_ptr maps */
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		free_map(&params->pipe_ddr_ptrs[i]);
+		if (per_frame_params)
+			free_map(&per_frame_params->pipe_ddr_ptrs[i]);
+		/* Free up theDVS table memory blocks before recomputing new table */
+		if (params->pipe_dvs_6axis_config[i])
+			free_dvs_6axis_table(&params->pipe_dvs_6axis_config[i]);
+		if (per_frame_params && per_frame_params->pipe_dvs_6axis_config[i])
+			free_dvs_6axis_table(&per_frame_params->pipe_dvs_6axis_config[i]);
+	}
+	free_map(&params->ddr_ptrs);
+	if (per_frame_params)
+		free_map(&per_frame_params->ddr_ptrs);
+
+	if (params->fpn_config.data) {
+		kvfree(params->fpn_config.data);
+		params->fpn_config.data = NULL;
+	}
+
+	/* Free up sc_config (temporal shading table) if it is allocated. */
+	if (params->sc_config) {
+		ia_css_shading_table_free(params->sc_config);
+		params->sc_config = NULL;
+	}
+	if (per_frame_params) {
+		if (per_frame_params->sc_config) {
+			ia_css_shading_table_free(per_frame_params->sc_config);
+			per_frame_params->sc_config = NULL;
+		}
+	}
+
+	kvfree(params);
+	kvfree(per_frame_params);
+	stream->isp_params_configs = NULL;
+	stream->per_frame_isp_params_configs = NULL;
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+sh_css_params_uninit(void)
+{
+	unsigned int p, i;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	ia_css_refcount_decrement(-1, sp_ddr_ptrs);
+	sp_ddr_ptrs = mmgr_NULL;
+	ia_css_refcount_decrement(-1, xmem_sp_group_ptrs);
+	xmem_sp_group_ptrs = mmgr_NULL;
+
+	for (p = 0; p < IA_CSS_PIPE_ID_NUM; p++)
+		for (i = 0; i < SH_CSS_MAX_STAGES; i++) {
+			ia_css_refcount_decrement(-1, xmem_sp_stage_ptrs[p][i]);
+			xmem_sp_stage_ptrs[p][i] = mmgr_NULL;
+			ia_css_refcount_decrement(-1, xmem_isp_stage_ptrs[p][i]);
+			xmem_isp_stage_ptrs[p][i] = mmgr_NULL;
+		}
+
+	/* go through the pools to clear references */
+	ia_css_refcount_clear(IA_CSS_REFCOUNT_PARAM_SET_POOL, &free_param_set_callback);
+	ia_css_refcount_clear(IA_CSS_REFCOUNT_PARAM_BUFFER, &free_buffer_callback);
+	ia_css_refcount_clear(-1, &free_buffer_callback);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static struct ia_css_host_data *
+convert_allocate_morph_plane(
+    unsigned short *data,
+    unsigned int width,
+    unsigned int height,
+    unsigned int aligned_width)
+{
+	unsigned int i, j, padding, w;
+	struct ia_css_host_data *me;
+	unsigned int isp_data_size;
+	u16 *isp_data_ptr;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	/* currently we don't have morph table interpolation yet,
+	 * so we allow a wider table to be used. This will be removed
+	 * in the future. */
+	if (width > aligned_width) {
+		padding = 0;
+		w = aligned_width;
+	} else {
+		padding = aligned_width - width;
+		w = width;
+	}
+	isp_data_size = height * (w + padding) * sizeof(uint16_t);
+
+	me = ia_css_host_data_allocate((size_t)isp_data_size);
+
+	if (!me) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return NULL;
+	}
+
+	isp_data_ptr = (uint16_t *)me->address;
+
+	memset(isp_data_ptr, 0, (size_t)isp_data_size);
+
+	for (i = 0; i < height; i++) {
+		for (j = 0; j < w; j++)
+			*isp_data_ptr++ = (uint16_t)data[j];
+		isp_data_ptr += padding;
+		data += width;
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+	return me;
+}
+
+static int
+store_morph_plane(
+    unsigned short *data,
+    unsigned int width,
+    unsigned int height,
+    ia_css_ptr dest,
+    unsigned int aligned_width)
+{
+	struct ia_css_host_data *isp_data;
+
+	assert(dest != mmgr_NULL);
+
+	isp_data = convert_allocate_morph_plane(data, width, height, aligned_width);
+	if (!isp_data) {
+		IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+		return -ENOMEM;
+	}
+	ia_css_params_store_ia_css_host_data(dest, isp_data);
+
+	ia_css_host_data_free(isp_data);
+	return 0;
+}
+
+static void sh_css_update_isp_params_to_ddr(
+    struct ia_css_isp_parameters *params,
+    ia_css_ptr ddr_ptr)
+{
+	size_t size = sizeof(params->uds);
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(params);
+
+	hmm_store(ddr_ptr, &params->uds, size);
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void sh_css_update_isp_mem_params_to_ddr(
+    const struct ia_css_binary *binary,
+    ia_css_ptr ddr_mem_ptr,
+    size_t size,
+    enum ia_css_isp_memories mem)
+{
+	const struct ia_css_host_data *params;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	params = ia_css_isp_param_get_mem_init(&binary->mem_params,
+					       IA_CSS_PARAM_CLASS_PARAM, mem);
+	hmm_store(ddr_mem_ptr, params->address, size);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void ia_css_dequeue_param_buffers(/*unsigned int pipe_num*/ void)
+{
+	unsigned int i;
+	ia_css_ptr cpy;
+	enum sh_css_queue_id param_queue_ids[3] = {	IA_CSS_PARAMETER_SET_QUEUE_ID,
+						    IA_CSS_PER_FRAME_PARAMETER_SET_QUEUE_ID,
+						    SH_CSS_INVALID_QUEUE_ID
+						  };
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LEAVE_PRIVATE("sp is not running");
+		/* SP is not running. The queues are not valid */
+		return;
+	}
+
+	for (i = 0; SH_CSS_INVALID_QUEUE_ID != param_queue_ids[i]; i++) {
+		cpy = (ia_css_ptr)0;
+		/* clean-up old copy */
+		while (ia_css_bufq_dequeue_buffer(param_queue_ids[i],
+						  (uint32_t *)&cpy) == 0) {
+			/* TMP: keep track of dequeued param set count
+			 */
+			g_param_buffer_dequeue_count++;
+			ia_css_bufq_enqueue_psys_event(
+			    IA_CSS_PSYS_SW_EVENT_BUFFER_DEQUEUED,
+			    0,
+			    param_queue_ids[i],
+			    0);
+
+			IA_CSS_LOG("dequeued param set %x from %d, release ref", cpy, 0);
+			free_ia_css_isp_parameter_set_info(cpy);
+			cpy = (ia_css_ptr)0;
+		}
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+process_kernel_parameters(unsigned int pipe_id,
+			  struct ia_css_pipeline_stage *stage,
+			  struct ia_css_isp_parameters *params,
+			  unsigned int isp_pipe_version,
+			  unsigned int raw_bit_depth)
+{
+	unsigned int param_id;
+
+	(void)isp_pipe_version;
+	(void)raw_bit_depth;
+
+	sh_css_enable_pipeline(stage->binary);
+
+	if (params->config_changed[IA_CSS_OB_ID]) {
+		ia_css_ob_configure(&params->stream_configs.ob,
+				    isp_pipe_version, raw_bit_depth);
+	}
+	if (params->config_changed[IA_CSS_S3A_ID]) {
+		ia_css_s3a_configure(raw_bit_depth);
+	}
+	/* Copy stage uds parameters to config, since they can differ per stage.
+	 */
+	params->crop_config.crop_pos = params->uds[stage->stage_num].crop_pos;
+	params->uds_config.crop_pos  = params->uds[stage->stage_num].crop_pos;
+	params->uds_config.uds       = params->uds[stage->stage_num].uds;
+	/* Call parameter process functions for all kernels */
+	/* Skip SC, since that is called on a temp sc table */
+	for (param_id = 0; param_id < IA_CSS_NUM_PARAMETER_IDS; param_id++) {
+		if (param_id == IA_CSS_SC_ID) continue;
+		if (params->config_changed[param_id])
+			ia_css_kernel_process_param[param_id](pipe_id, stage, params);
+	}
+}
+
+int
+sh_css_param_update_isp_params(struct ia_css_pipe *curr_pipe,
+			       struct ia_css_isp_parameters *params,
+			       bool commit,
+			       struct ia_css_pipe *pipe_in)
+{
+	int err = 0;
+	ia_css_ptr cpy;
+	int i;
+	unsigned int raw_bit_depth = 10;
+	unsigned int isp_pipe_version = SH_CSS_ISP_PIPE_VERSION_1;
+	bool acc_cluster_params_changed = false;
+	unsigned int thread_id, pipe_num;
+
+	(void)acc_cluster_params_changed;
+
+	assert(curr_pipe);
+
+	IA_CSS_ENTER_PRIVATE("pipe=%p, isp_parameters_id=%d", pipe_in, params->isp_parameters_id);
+	raw_bit_depth = ia_css_stream_input_format_bits_per_pixel(curr_pipe->stream);
+
+	/* now make the map available to the sp */
+	if (!commit) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	/* enqueue a copies of the mem_map to
+	   the designated pipelines */
+	for (i = 0; i < curr_pipe->stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe;
+		struct sh_css_ddr_address_map *cur_map;
+		struct sh_css_ddr_address_map_size *cur_map_size;
+		struct ia_css_isp_parameter_set_info isp_params_info;
+		struct ia_css_pipeline *pipeline;
+		struct ia_css_pipeline_stage *stage;
+
+		enum sh_css_queue_id queue_id;
+
+		pipe = curr_pipe->stream->pipes[i];
+		pipeline = ia_css_pipe_get_pipeline(pipe);
+		pipe_num = ia_css_pipe_get_pipe_num(pipe);
+		isp_pipe_version = ia_css_pipe_get_isp_pipe_version(pipe);
+		ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+
+		ia_css_query_internal_queue_id(params->output_frame
+					       ? IA_CSS_BUFFER_TYPE_PER_FRAME_PARAMETER_SET
+					       : IA_CSS_BUFFER_TYPE_PARAMETER_SET,
+					       thread_id, &queue_id);
+		if (!sh_css_sp_is_running()) {
+			/* SP is not running. The queues are not valid */
+			err = -EBUSY;
+			break;
+		}
+		cur_map = &params->pipe_ddr_ptrs[pipeline->pipe_id];
+		cur_map_size = &params->pipe_ddr_ptrs_size[pipeline->pipe_id];
+
+		/* TODO: Normally, zoom and motion parameters shouldn't
+		 * be part of "isp_params" as it is resolution/pipe dependent
+		 * Therefore, move the zoom config elsewhere (e.g. shading
+		 * table can be taken as an example! @GC
+		 * */
+		{
+			/* we have to do this per pipeline because */
+			/* the processing is a.o. resolution dependent */
+			err = ia_css_process_zoom_and_motion(params,
+							     pipeline->stages);
+			if (err)
+				return err;
+		}
+		/* check if to actually update the parameters for this pipe */
+		/* When API change is implemented making good distinction between
+		* stream config and pipe config this skipping code can be moved out of the #ifdef */
+		if (pipe_in && (pipe != pipe_in)) {
+			IA_CSS_LOG("skipping pipe %p", pipe);
+			continue;
+		}
+
+		/* BZ 125915, should be moved till after "update other buff" */
+		/* update the other buffers to the pipe specific copies */
+		for (stage = pipeline->stages; stage; stage = stage->next) {
+			unsigned int mem;
+
+			if (!stage || !stage->binary)
+				continue;
+
+			process_kernel_parameters(pipeline->pipe_id,
+						  stage, params,
+						  isp_pipe_version, raw_bit_depth);
+
+			err = sh_css_params_write_to_ddr_internal(
+				  pipe,
+				  pipeline->pipe_id,
+				  params,
+				  stage,
+				  cur_map,
+				  cur_map_size);
+
+			if (err)
+				break;
+			for (mem = 0; mem < IA_CSS_NUM_MEMORIES; mem++) {
+				params->isp_mem_params_changed
+				[pipeline->pipe_id][stage->stage_num][mem] = false;
+			}
+		} /* for */
+		if (err)
+			break;
+		/* update isp_params to pipe specific copies */
+		if (params->isp_params_changed) {
+			reallocate_buffer(&cur_map->isp_param,
+					  &cur_map_size->isp_param,
+					  cur_map_size->isp_param,
+					  true,
+					  &err);
+			if (err)
+				break;
+			sh_css_update_isp_params_to_ddr(params, cur_map->isp_param);
+		}
+
+		/* last make referenced copy */
+		err = ref_sh_css_ddr_address_map(
+			  cur_map,
+			  &isp_params_info.mem_map);
+		if (err)
+			break;
+
+		/* Update Parameters ID */
+		isp_params_info.isp_parameters_id = params->isp_parameters_id;
+
+		/* Update output frame pointer */
+		isp_params_info.output_frame_ptr =
+		    (params->output_frame) ? params->output_frame->data : mmgr_NULL;
+
+		/* now write the copy to ddr */
+		err = write_ia_css_isp_parameter_set_info_to_ddr(&isp_params_info, &cpy);
+		if (err)
+			break;
+
+		/* enqueue the set to sp */
+		IA_CSS_LOG("queue param set %x to %d", cpy, thread_id);
+
+		err = ia_css_bufq_enqueue_buffer(thread_id, queue_id, (uint32_t)cpy);
+		if (err) {
+			free_ia_css_isp_parameter_set_info(cpy);
+			IA_CSS_LOG("pfp: FAILED to add config id %d for OF %d to q %d on thread %d",
+				   isp_params_info.isp_parameters_id,
+				   isp_params_info.output_frame_ptr,
+				   queue_id, thread_id);
+			break;
+		} else {
+			/* TMP: check discrepancy between nr of enqueued
+			 * parameter sets and dequeued sets
+			 */
+			g_param_buffer_enqueue_count++;
+			assert(g_param_buffer_enqueue_count < g_param_buffer_dequeue_count + 50);
+			/*
+			 * Tell the SP which queues are not empty,
+			 * by sending the software event.
+			 */
+			if (!sh_css_sp_is_running()) {
+				/* SP is not running. The queues are not valid */
+				IA_CSS_LEAVE_ERR_PRIVATE(-EBUSY);
+				return -EBUSY;
+			}
+			ia_css_bufq_enqueue_psys_event(
+			    IA_CSS_PSYS_SW_EVENT_BUFFER_ENQUEUED,
+			    (uint8_t)thread_id,
+			    (uint8_t)queue_id,
+			    0);
+			IA_CSS_LOG("pfp: added config id %d for OF %d to q %d on thread %d",
+				   isp_params_info.isp_parameters_id,
+				   isp_params_info.output_frame_ptr,
+				   queue_id, thread_id);
+		}
+		/* clean-up old copy */
+		ia_css_dequeue_param_buffers(/*pipe_num*/);
+		params->pipe_dvs_6axis_config_changed[pipeline->pipe_id] = false;
+	} /* end for each 'active' pipeline */
+	/* clear the changed flags after all params
+	for all pipelines have been updated */
+	params->isp_params_changed = false;
+	params->sc_table_changed = false;
+	params->dis_coef_table_changed = false;
+	params->dvs2_coef_table_changed = false;
+	params->morph_table_changed = false;
+	params->dz_config_changed = false;
+	params->motion_config_changed = false;
+	/* ------ deprecated(bz675) : from ------ */
+	params->shading_settings_changed = false;
+	/* ------ deprecated(bz675) : to ------ */
+
+	memset(&params->config_changed[0], 0, sizeof(params->config_changed));
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+sh_css_params_write_to_ddr_internal(
+    struct ia_css_pipe *pipe,
+    unsigned int pipe_id,
+    struct ia_css_isp_parameters *params,
+    const struct ia_css_pipeline_stage *stage,
+    struct sh_css_ddr_address_map *ddr_map,
+    struct sh_css_ddr_address_map_size *ddr_map_size)
+{
+	int err;
+	const struct ia_css_binary *binary;
+
+	unsigned int stage_num;
+	unsigned int mem;
+	bool buff_realloced;
+
+	/* struct is > 128 bytes so it should not be on stack (see checkpatch) */
+	static struct ia_css_macc_table converted_macc_table;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(params);
+	assert(ddr_map);
+	assert(ddr_map_size);
+	assert(stage);
+
+	binary = stage->binary;
+	assert(binary);
+
+	stage_num = stage->stage_num;
+
+	if (binary->info->sp.enable.fpnr) {
+		buff_realloced = reallocate_buffer(&ddr_map->fpn_tbl,
+						   &ddr_map_size->fpn_tbl,
+						   (size_t)(FPNTBL_BYTES(binary)),
+						   params->config_changed[IA_CSS_FPN_ID],
+						   &err);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		if (params->config_changed[IA_CSS_FPN_ID] || buff_realloced) {
+			if (params->fpn_config.enabled) {
+				err = store_fpntbl(params, ddr_map->fpn_tbl);
+				if (err) {
+					IA_CSS_LEAVE_ERR_PRIVATE(err);
+					return err;
+				}
+			}
+		}
+	}
+
+	if (binary->info->sp.enable.sc) {
+		u32 enable_conv;
+
+		enable_conv = params->shading_settings.enable_shading_table_conversion;
+
+		buff_realloced = reallocate_buffer(&ddr_map->sc_tbl,
+						   &ddr_map_size->sc_tbl,
+						   SCTBL_BYTES(binary),
+						   params->sc_table_changed,
+						   &err);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		if (params->shading_settings_changed ||
+		    params->sc_table_changed || buff_realloced) {
+			if (enable_conv == 0) {
+				if (params->sc_table) {
+					/* store the shading table to ddr */
+					err = ia_css_params_store_sctbl(stage, ddr_map->sc_tbl, params->sc_table);
+					if (err) {
+						IA_CSS_LEAVE_ERR_PRIVATE(err);
+						return err;
+					}
+					/* set sc_config to isp */
+					params->sc_config = (struct ia_css_shading_table *)params->sc_table;
+					ia_css_kernel_process_param[IA_CSS_SC_ID](pipe_id, stage, params);
+					params->sc_config = NULL;
+				} else {
+					/* generate the identical shading table */
+					if (params->sc_config) {
+						ia_css_shading_table_free(params->sc_config);
+						params->sc_config = NULL;
+					}
+					sh_css_params_shading_id_table_generate(&params->sc_config,
+										binary->sctbl_width_per_color,
+										binary->sctbl_height);
+					if (!params->sc_config) {
+						IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+						return -ENOMEM;
+					}
+
+					/* store the shading table to ddr */
+					err = ia_css_params_store_sctbl(stage, ddr_map->sc_tbl, params->sc_config);
+					if (err) {
+						IA_CSS_LEAVE_ERR_PRIVATE(err);
+						return err;
+					}
+
+					/* set sc_config to isp */
+					ia_css_kernel_process_param[IA_CSS_SC_ID](pipe_id, stage, params);
+
+					/* free the shading table */
+					ia_css_shading_table_free(params->sc_config);
+					params->sc_config = NULL;
+				}
+			} else { /* legacy */
+				/* ------ deprecated(bz675) : from ------ */
+				/* shading table is full resolution, reduce */
+				if (params->sc_config) {
+					ia_css_shading_table_free(params->sc_config);
+					params->sc_config = NULL;
+				}
+				prepare_shading_table(
+				    (const struct ia_css_shading_table *)params->sc_table,
+				    params->sensor_binning,
+				    &params->sc_config,
+				    binary, pipe->required_bds_factor);
+				if (!params->sc_config) {
+					IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+					return -ENOMEM;
+				}
+
+				/* store the shading table to ddr */
+				err = ia_css_params_store_sctbl(stage, ddr_map->sc_tbl, params->sc_config);
+				if (err) {
+					IA_CSS_LEAVE_ERR_PRIVATE(err);
+					return err;
+				}
+
+				/* set sc_config to isp */
+				ia_css_kernel_process_param[IA_CSS_SC_ID](pipe_id, stage, params);
+
+				/* free the shading table */
+				ia_css_shading_table_free(params->sc_config);
+				params->sc_config = NULL;
+				/* ------ deprecated(bz675) : to ------ */
+			}
+		}
+	}
+
+	if (params->config_changed[IA_CSS_MACC_ID] && binary->info->sp.enable.macc) {
+		unsigned int i, j, idx;
+		static const unsigned int idx_map[] = {
+			0, 1, 3, 2, 6, 7, 5, 4, 12, 13, 15, 14, 10, 11, 9, 8
+		};
+
+		for (i = 0; i < IA_CSS_MACC_NUM_AXES; i++) {
+			idx = 4 * idx_map[i];
+			j   = 4 * i;
+
+			if (binary->info->sp.pipeline.isp_pipe_version == SH_CSS_ISP_PIPE_VERSION_1) {
+				converted_macc_table.data[idx] =
+				    (int16_t)sDIGIT_FITTING(params->macc_table.data[j],
+							    13, SH_CSS_MACC_COEF_SHIFT);
+				converted_macc_table.data[idx + 1] =
+				    (int16_t)sDIGIT_FITTING(params->macc_table.data[j + 1],
+							    13, SH_CSS_MACC_COEF_SHIFT);
+				converted_macc_table.data[idx + 2] =
+				    (int16_t)sDIGIT_FITTING(params->macc_table.data[j + 2],
+							    13, SH_CSS_MACC_COEF_SHIFT);
+				converted_macc_table.data[idx + 3] =
+				    (int16_t)sDIGIT_FITTING(params->macc_table.data[j + 3],
+							    13, SH_CSS_MACC_COEF_SHIFT);
+			} else if (binary->info->sp.pipeline.isp_pipe_version ==
+				   SH_CSS_ISP_PIPE_VERSION_2_2) {
+				converted_macc_table.data[idx] =
+				    params->macc_table.data[j];
+				converted_macc_table.data[idx + 1] =
+				    params->macc_table.data[j + 1];
+				converted_macc_table.data[idx + 2] =
+				    params->macc_table.data[j + 2];
+				converted_macc_table.data[idx + 3] =
+				    params->macc_table.data[j + 3];
+			}
+		}
+		reallocate_buffer(&ddr_map->macc_tbl,
+				  &ddr_map_size->macc_tbl,
+				  ddr_map_size->macc_tbl,
+				  true,
+				  &err);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		hmm_store(ddr_map->macc_tbl,
+			   converted_macc_table.data,
+			   sizeof(converted_macc_table.data));
+	}
+
+	if (binary->info->sp.enable.dvs_6axis) {
+		/* because UV is packed into the Y plane, calc total
+		 * YYU size = /2 gives size of UV-only,
+		 * total YYU size = UV-only * 3.
+		 */
+		buff_realloced = reallocate_buffer(
+				     &ddr_map->dvs_6axis_params_y,
+				     &ddr_map_size->dvs_6axis_params_y,
+				     (size_t)((DVS_6AXIS_BYTES(binary) / 2) * 3),
+				     params->pipe_dvs_6axis_config_changed[pipe_id],
+				     &err);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+
+		if (params->pipe_dvs_6axis_config_changed[pipe_id] || buff_realloced) {
+			const struct ia_css_frame_info *dvs_in_frame_info;
+
+			if (stage->args.delay_frames[0]) {
+				/*When delay frames are present(as in case of video),
+				they are used for dvs. Configure DVS using those params*/
+				dvs_in_frame_info = &stage->args.delay_frames[0]->info;
+			} else {
+				/*Otherwise, use input frame to configure DVS*/
+				dvs_in_frame_info = &stage->args.in_frame->info;
+			}
+
+			/* Generate default DVS unity table on start up*/
+			if (!params->pipe_dvs_6axis_config[pipe_id]) {
+				struct ia_css_resolution dvs_offset = {0};
+
+				dvs_offset.width = (PIX_SHIFT_FILTER_RUN_IN_X + binary->dvs_envelope.width) / 2;
+				dvs_offset.height = (PIX_SHIFT_FILTER_RUN_IN_Y + binary->dvs_envelope.height) / 2;
+
+				params->pipe_dvs_6axis_config[pipe_id] =
+				    generate_dvs_6axis_table(&binary->out_frame_info[0].res, &dvs_offset);
+				if (!params->pipe_dvs_6axis_config[pipe_id]) {
+					IA_CSS_LEAVE_ERR_PRIVATE(-ENOMEM);
+					return -ENOMEM;
+				}
+				params->pipe_dvs_6axis_config_changed[pipe_id] = true;
+
+				store_dvs_6axis_config(params->pipe_dvs_6axis_config[pipe_id],
+						    binary,
+						    dvs_in_frame_info,
+						    ddr_map->dvs_6axis_params_y);
+				params->isp_params_changed = true;
+			}
+		}
+	}
+
+	if (binary->info->sp.enable.ca_gdc) {
+		unsigned int i;
+		ia_css_ptr *virt_addr_tetra_x[
+
+		IA_CSS_MORPH_TABLE_NUM_PLANES];
+		size_t *virt_size_tetra_x[
+
+		IA_CSS_MORPH_TABLE_NUM_PLANES];
+		ia_css_ptr *virt_addr_tetra_y[
+
+		IA_CSS_MORPH_TABLE_NUM_PLANES];
+		size_t *virt_size_tetra_y[
+
+		IA_CSS_MORPH_TABLE_NUM_PLANES];
+
+		virt_addr_tetra_x[0] = &ddr_map->tetra_r_x;
+		virt_addr_tetra_x[1] = &ddr_map->tetra_gr_x;
+		virt_addr_tetra_x[2] = &ddr_map->tetra_gb_x;
+		virt_addr_tetra_x[3] = &ddr_map->tetra_b_x;
+		virt_addr_tetra_x[4] = &ddr_map->tetra_ratb_x;
+		virt_addr_tetra_x[5] = &ddr_map->tetra_batr_x;
+
+		virt_size_tetra_x[0] = &ddr_map_size->tetra_r_x;
+		virt_size_tetra_x[1] = &ddr_map_size->tetra_gr_x;
+		virt_size_tetra_x[2] = &ddr_map_size->tetra_gb_x;
+		virt_size_tetra_x[3] = &ddr_map_size->tetra_b_x;
+		virt_size_tetra_x[4] = &ddr_map_size->tetra_ratb_x;
+		virt_size_tetra_x[5] = &ddr_map_size->tetra_batr_x;
+
+		virt_addr_tetra_y[0] = &ddr_map->tetra_r_y;
+		virt_addr_tetra_y[1] = &ddr_map->tetra_gr_y;
+		virt_addr_tetra_y[2] = &ddr_map->tetra_gb_y;
+		virt_addr_tetra_y[3] = &ddr_map->tetra_b_y;
+		virt_addr_tetra_y[4] = &ddr_map->tetra_ratb_y;
+		virt_addr_tetra_y[5] = &ddr_map->tetra_batr_y;
+
+		virt_size_tetra_y[0] = &ddr_map_size->tetra_r_y;
+		virt_size_tetra_y[1] = &ddr_map_size->tetra_gr_y;
+		virt_size_tetra_y[2] = &ddr_map_size->tetra_gb_y;
+		virt_size_tetra_y[3] = &ddr_map_size->tetra_b_y;
+		virt_size_tetra_y[4] = &ddr_map_size->tetra_ratb_y;
+		virt_size_tetra_y[5] = &ddr_map_size->tetra_batr_y;
+
+		buff_realloced = false;
+		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+			buff_realloced |=
+			    reallocate_buffer(virt_addr_tetra_x[i],
+					    virt_size_tetra_x[i],
+					    (size_t)
+					    (MORPH_PLANE_BYTES(binary)),
+					    params->morph_table_changed,
+					    &err);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+			buff_realloced |=
+			    reallocate_buffer(virt_addr_tetra_y[i],
+					    virt_size_tetra_y[i],
+					    (size_t)
+					    (MORPH_PLANE_BYTES(binary)),
+					    params->morph_table_changed,
+					    &err);
+			if (err) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+		if (params->morph_table_changed || buff_realloced) {
+			const struct ia_css_morph_table *table = params->morph_table;
+			struct ia_css_morph_table *id_table = NULL;
+
+			if ((table) &&
+			    (table->width < binary->morph_tbl_width ||
+			    table->height < binary->morph_tbl_height)) {
+				table = NULL;
+			}
+			if (!table) {
+				err = sh_css_params_default_morph_table(&id_table,
+									binary);
+				if (err) {
+					IA_CSS_LEAVE_ERR_PRIVATE(err);
+					return err;
+				}
+				table = id_table;
+			}
+
+			for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+				store_morph_plane(table->coordinates_x[i],
+						table->width,
+						table->height,
+						*virt_addr_tetra_x[i],
+						binary->morph_tbl_aligned_width);
+				store_morph_plane(table->coordinates_y[i],
+						table->width,
+						table->height,
+						*virt_addr_tetra_y[i],
+						binary->morph_tbl_aligned_width);
+			}
+			if (id_table)
+				ia_css_morph_table_free(id_table);
+		}
+	}
+
+	/* After special cases like SC, FPN since they may change parameters */
+	for (mem = 0; mem < N_IA_CSS_MEMORIES; mem++) {
+		const struct ia_css_isp_data *isp_data =
+		    ia_css_isp_param_get_isp_mem_init(&binary->info->sp.mem_initializers,
+						    IA_CSS_PARAM_CLASS_PARAM, mem);
+		size_t size = isp_data->size;
+
+		if (!size) continue;
+		buff_realloced = reallocate_buffer(&ddr_map->isp_mem_param[stage_num][mem],
+						&ddr_map_size->isp_mem_param[stage_num][mem],
+						size,
+						params->isp_mem_params_changed[pipe_id][stage_num][mem],
+						&err);
+		if (err) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		if (params->isp_mem_params_changed[pipe_id][stage_num][mem] || buff_realloced) {
+			sh_css_update_isp_mem_params_to_ddr(binary,
+							    ddr_map->isp_mem_param[stage_num][mem],
+							    ddr_map_size->isp_mem_param[stage_num][mem], mem);
+		}
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(0);
+	return 0;
+}
+
+const struct ia_css_fpn_table *ia_css_get_fpn_table(struct ia_css_stream
+	*stream)
+{
+	struct ia_css_isp_parameters *params;
+
+	IA_CSS_ENTER_LEAVE("void");
+	assert(stream);
+
+	params = stream->isp_params_configs;
+
+	return &params->fpn_config;
+}
+
+struct ia_css_shading_table *ia_css_get_shading_table(struct ia_css_stream
+	*stream)
+{
+	struct ia_css_shading_table *table = NULL;
+	struct ia_css_isp_parameters *params;
+
+	IA_CSS_ENTER("void");
+
+	assert(stream);
+
+	params = stream->isp_params_configs;
+	if (!params)
+		return NULL;
+
+	if (params->shading_settings.enable_shading_table_conversion == 0) {
+		if (params->sc_table) {
+			table = (struct ia_css_shading_table *)params->sc_table;
+		} else {
+			const struct ia_css_binary *binary
+			    = ia_css_stream_get_shading_correction_binary(stream);
+			if (binary) {
+				/* generate the identical shading table */
+				if (params->sc_config) {
+					ia_css_shading_table_free(params->sc_config);
+					params->sc_config = NULL;
+				}
+				sh_css_params_shading_id_table_generate(&params->sc_config,
+									binary->sctbl_width_per_color,
+									binary->sctbl_height);
+				table = params->sc_config;
+				/* The sc_config will be freed in the
+				 * ia_css_stream_isp_parameters_uninit function. */
+			}
+		}
+	} else {
+		/* ------ deprecated(bz675) : from ------ */
+		const struct ia_css_binary *binary
+		    = ia_css_stream_get_shading_correction_binary(stream);
+		struct ia_css_pipe *pipe;
+
+		/**********************************************************************/
+		/* following code is copied from function ia_css_stream_get_shading_correction_binary()
+		 * to match with the binary */
+		pipe = stream->pipes[0];
+
+		if (stream->num_pipes == 2) {
+			assert(stream->pipes[1]);
+			if (stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_VIDEO ||
+			    stream->pipes[1]->config.mode == IA_CSS_PIPE_MODE_PREVIEW)
+				pipe = stream->pipes[1];
+		}
+		/**********************************************************************/
+		if (binary) {
+			if (params->sc_config) {
+				ia_css_shading_table_free(params->sc_config);
+				params->sc_config = NULL;
+			}
+			prepare_shading_table(
+			    (const struct ia_css_shading_table *)params->sc_table,
+			    params->sensor_binning,
+			    &params->sc_config,
+			    binary, pipe->required_bds_factor);
+
+			table = params->sc_config;
+			/* The sc_config will be freed in the
+			 * ia_css_stream_isp_parameters_uninit function. */
+		}
+		/* ------ deprecated(bz675) : to ------ */
+	}
+
+	IA_CSS_LEAVE("table=%p", table);
+
+	return table;
+}
+
+ia_css_ptr sh_css_store_sp_group_to_ddr(void)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	hmm_store(xmem_sp_group_ptrs,
+		   &sh_css_sp_group,
+		   sizeof(struct sh_css_sp_group));
+	return xmem_sp_group_ptrs;
+}
+
+ia_css_ptr sh_css_store_sp_stage_to_ddr(
+    unsigned int pipe,
+    unsigned int stage)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	hmm_store(xmem_sp_stage_ptrs[pipe][stage],
+		   &sh_css_sp_stage,
+		   sizeof(struct sh_css_sp_stage));
+	return xmem_sp_stage_ptrs[pipe][stage];
+}
+
+ia_css_ptr sh_css_store_isp_stage_to_ddr(
+    unsigned int pipe,
+    unsigned int stage)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	hmm_store(xmem_isp_stage_ptrs[pipe][stage],
+		   &sh_css_isp_stage,
+		   sizeof(struct sh_css_isp_stage));
+	return xmem_isp_stage_ptrs[pipe][stage];
+}
+
+static int ref_sh_css_ddr_address_map(
+    struct sh_css_ddr_address_map *map,
+    struct sh_css_ddr_address_map *out)
+{
+	int err = 0;
+	unsigned int i;
+
+	/* we will use a union to copy things; overlaying an array
+	   with the struct; that way adding fields in the struct
+	   will keep things working, and we will not get type errors.
+	*/
+	union {
+		struct sh_css_ddr_address_map *map;
+		ia_css_ptr *addrs;
+	} in_addrs, to_addrs;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(map);
+	assert(out);
+
+	in_addrs.map = map;
+	to_addrs.map = out;
+
+	assert(sizeof(struct sh_css_ddr_address_map_size) / sizeof(size_t) ==
+	       sizeof(struct sh_css_ddr_address_map) / sizeof(ia_css_ptr));
+
+	/* copy map using size info */
+	for (i = 0; i < (sizeof(struct sh_css_ddr_address_map_size) /
+			 sizeof(size_t)); i++) {
+		if (in_addrs.addrs[i] == mmgr_NULL)
+			to_addrs.addrs[i] = mmgr_NULL;
+		else
+			to_addrs.addrs[i] = ia_css_refcount_increment(IA_CSS_REFCOUNT_PARAM_BUFFER,
+					    in_addrs.addrs[i]);
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int write_ia_css_isp_parameter_set_info_to_ddr(
+    struct ia_css_isp_parameter_set_info *me,
+    ia_css_ptr *out)
+{
+	int err = 0;
+	bool succ;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(me);
+	assert(out);
+
+	*out = ia_css_refcount_increment(IA_CSS_REFCOUNT_PARAM_SET_POOL,
+					 hmm_alloc(sizeof(struct ia_css_isp_parameter_set_info)));
+	succ = (*out != mmgr_NULL);
+	if (succ)
+		hmm_store(*out,
+			   me, sizeof(struct ia_css_isp_parameter_set_info));
+	else
+		err = -ENOMEM;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static int
+free_ia_css_isp_parameter_set_info(
+    ia_css_ptr ptr)
+{
+	int err = 0;
+	struct ia_css_isp_parameter_set_info isp_params_info;
+	unsigned int i;
+	ia_css_ptr *addrs = (ia_css_ptr *)&isp_params_info.mem_map;
+
+	IA_CSS_ENTER_PRIVATE("ptr = %u", ptr);
+
+	/* sanity check - ptr must be valid */
+	if (!ia_css_refcount_is_valid(ptr)) {
+		IA_CSS_ERROR("%s: IA_CSS_REFCOUNT_PARAM_SET_POOL(0x%x) invalid arg", __func__,
+			     ptr);
+		err = -EINVAL;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	hmm_load(ptr, &isp_params_info.mem_map, sizeof(struct sh_css_ddr_address_map));
+	/* copy map using size info */
+	for (i = 0; i < (sizeof(struct sh_css_ddr_address_map_size) /
+			 sizeof(size_t)); i++) {
+		if (addrs[i] == mmgr_NULL)
+			continue;
+
+		/* sanity check - ptr must be valid */
+		if (!ia_css_refcount_is_valid(addrs[i])) {
+			IA_CSS_ERROR("%s: IA_CSS_REFCOUNT_PARAM_BUFFER(0x%x) invalid arg", __func__,
+				     ptr);
+			err = -EINVAL;
+			continue;
+		}
+
+		ia_css_refcount_decrement(IA_CSS_REFCOUNT_PARAM_BUFFER, addrs[i]);
+	}
+	ia_css_refcount_decrement(IA_CSS_REFCOUNT_PARAM_SET_POOL, ptr);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/* Mark all parameters as changed to force recomputing the derived ISP parameters */
+void
+sh_css_invalidate_params(struct ia_css_stream *stream)
+{
+	struct	ia_css_isp_parameters *params;
+	unsigned int i, j, mem;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(stream);
+
+	params = stream->isp_params_configs;
+	params->isp_params_changed = true;
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		for (j = 0; j < SH_CSS_MAX_STAGES; j++) {
+			for (mem = 0; mem < N_IA_CSS_MEMORIES; mem++) {
+				params->isp_mem_params_changed[i][j][mem] = true;
+			}
+		}
+	}
+
+	memset(&params->config_changed[0], 1, sizeof(params->config_changed));
+	params->dis_coef_table_changed = true;
+	params->dvs2_coef_table_changed = true;
+	params->morph_table_changed = true;
+	params->sc_table_changed = true;
+	params->dz_config_changed = true;
+	params->motion_config_changed = true;
+
+	/*Free up theDVS table memory blocks before recomputing new table  */
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		if (params->pipe_dvs_6axis_config[i]) {
+			free_dvs_6axis_table(&params->pipe_dvs_6axis_config[i]);
+			params->pipe_dvs_6axis_config_changed[i] = true;
+		}
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+void
+sh_css_update_uds_and_crop_info(
+    const struct ia_css_binary_info *info,
+    const struct ia_css_frame_info *in_frame_info,
+    const struct ia_css_frame_info *out_frame_info,
+    const struct ia_css_resolution *dvs_env,
+    const struct ia_css_dz_config *zoom,
+    const struct ia_css_vector *motion_vector,
+    struct sh_css_uds_info *uds,		/* out */
+    struct sh_css_crop_pos *sp_out_crop_pos,	/* out */
+
+    bool enable_zoom)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(info);
+	assert(in_frame_info);
+	assert(out_frame_info);
+	assert(dvs_env);
+	assert(zoom);
+	assert(motion_vector);
+	assert(uds);
+	assert(sp_out_crop_pos);
+
+	uds->curr_dx   = enable_zoom ? (uint16_t)zoom->dx : HRT_GDC_N;
+	uds->curr_dy   = enable_zoom ? (uint16_t)zoom->dy : HRT_GDC_N;
+
+	if (info->enable.dvs_envelope) {
+		unsigned int crop_x = 0,
+			     crop_y = 0,
+			     uds_xc = 0,
+			     uds_yc = 0,
+			     env_width, env_height;
+		int half_env_x, half_env_y;
+		int motion_x = motion_vector->x;
+		int motion_y = motion_vector->y;
+		bool upscale_x = in_frame_info->res.width < out_frame_info->res.width;
+		bool upscale_y = in_frame_info->res.height < out_frame_info->res.height;
+
+		if (info->enable.uds && !info->enable.ds) {
+			/**
+			 * we calculate with the envelope that we can actually
+			 * use, the min dvs envelope is for the filter
+			 * initialization.
+			 */
+			env_width  = dvs_env->width -
+				     SH_CSS_MIN_DVS_ENVELOPE;
+			env_height = dvs_env->height -
+				     SH_CSS_MIN_DVS_ENVELOPE;
+			half_env_x = env_width / 2;
+			half_env_y = env_height / 2;
+			/**
+			 * for digital zoom, we use the dvs envelope and make
+			 * sure that we don't include the 8 leftmost pixels or
+			 * 8 topmost rows.
+			 */
+			if (upscale_x) {
+				uds_xc = (in_frame_info->res.width
+					  + env_width
+					  + SH_CSS_MIN_DVS_ENVELOPE) / 2;
+			} else {
+				uds_xc = (out_frame_info->res.width
+					  + env_width) / 2
+					 + SH_CSS_MIN_DVS_ENVELOPE;
+			}
+			if (upscale_y) {
+				uds_yc = (in_frame_info->res.height
+					  + env_height
+					  + SH_CSS_MIN_DVS_ENVELOPE) / 2;
+			} else {
+				uds_yc = (out_frame_info->res.height
+					  + env_height) / 2
+					 + SH_CSS_MIN_DVS_ENVELOPE;
+			}
+			/* clip the motion vector to +/- half the envelope */
+			motion_x = clamp(motion_x, -half_env_x, half_env_x);
+			motion_y = clamp(motion_y, -half_env_y, half_env_y);
+			uds_xc += motion_x;
+			uds_yc += motion_y;
+			/* uds can be pipelined, remove top lines */
+			crop_y = 2;
+		} else if (info->enable.ds) {
+			env_width  = dvs_env->width;
+			env_height = dvs_env->height;
+			half_env_x = env_width / 2;
+			half_env_y = env_height / 2;
+			/* clip the motion vector to +/- half the envelope */
+			motion_x = clamp(motion_x, -half_env_x, half_env_x);
+			motion_y = clamp(motion_y, -half_env_y, half_env_y);
+			/* for video with downscaling, the envelope is included
+			    in the input resolution. */
+			uds_xc = in_frame_info->res.width / 2 + motion_x;
+			uds_yc = in_frame_info->res.height / 2 + motion_y;
+			crop_x = info->pipeline.left_cropping;
+			/* ds == 2 (yuv_ds) can be pipelined, remove top
+			   lines */
+			if (info->enable.ds & 1)
+				crop_y = info->pipeline.top_cropping;
+			else
+				crop_y = 2;
+		} else {
+			/* video nodz: here we can only crop. We make sure we
+			   crop at least the first 8x8 pixels away. */
+			env_width  = dvs_env->width -
+				     SH_CSS_MIN_DVS_ENVELOPE;
+			env_height = dvs_env->height -
+				     SH_CSS_MIN_DVS_ENVELOPE;
+			half_env_x = env_width / 2;
+			half_env_y = env_height / 2;
+			motion_x = clamp(motion_x, -half_env_x, half_env_x);
+			motion_y = clamp(motion_y, -half_env_y, half_env_y);
+			crop_x = SH_CSS_MIN_DVS_ENVELOPE
+				 + half_env_x + motion_x;
+			crop_y = SH_CSS_MIN_DVS_ENVELOPE
+				 + half_env_y + motion_y;
+		}
+
+		/* Must enforce that the crop position is even */
+		crop_x = EVEN_FLOOR(crop_x);
+		crop_y = EVEN_FLOOR(crop_y);
+		uds_xc = EVEN_FLOOR(uds_xc);
+		uds_yc = EVEN_FLOOR(uds_yc);
+
+		uds->xc = (uint16_t)uds_xc;
+		uds->yc = (uint16_t)uds_yc;
+		sp_out_crop_pos->x = (uint16_t)crop_x;
+		sp_out_crop_pos->y = (uint16_t)crop_y;
+	} else {
+		/* for down scaling, we always use the center of the image */
+		uds->xc = (uint16_t)in_frame_info->res.width / 2;
+		uds->yc = (uint16_t)in_frame_info->res.height / 2;
+		sp_out_crop_pos->x = (uint16_t)info->pipeline.left_cropping;
+		sp_out_crop_pos->y = (uint16_t)info->pipeline.top_cropping;
+	}
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static int
+sh_css_update_uds_and_crop_info_based_on_zoom_region(
+    const struct ia_css_binary_info *info,
+    const struct ia_css_frame_info *in_frame_info,
+    const struct ia_css_frame_info *out_frame_info,
+    const struct ia_css_resolution *dvs_env,
+    const struct ia_css_dz_config *zoom,
+    const struct ia_css_vector *motion_vector,
+    struct sh_css_uds_info *uds,		/* out */
+    struct sh_css_crop_pos *sp_out_crop_pos,	/* out */
+    struct ia_css_resolution pipe_in_res,
+    bool enable_zoom)
+{
+	unsigned int x0 = 0, y0 = 0, x1 = 0, y1 = 0;
+	int err = 0;
+	/* Note:
+	* Filter_Envelope = 0 for NND/LUT
+	* Filter_Envelope = 1 for BCI
+	* Filter_Envelope = 3 for BLI
+	* Currently, not considering this filter envelope because, In uds.sp.c is recalculating
+	* the dx/dy based on filter envelope and other information (ia_css_uds_sp_scale_params)
+	* Ideally, That should be done on host side not on sp side.
+	*/
+	unsigned int filter_envelope = 0;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(info);
+	assert(in_frame_info);
+	assert(out_frame_info);
+	assert(dvs_env);
+	assert(zoom);
+	assert(motion_vector);
+	assert(uds);
+	assert(sp_out_crop_pos);
+	x0 = zoom->zoom_region.origin.x;
+	y0 = zoom->zoom_region.origin.y;
+	x1 = zoom->zoom_region.resolution.width + x0;
+	y1 = zoom->zoom_region.resolution.height + y0;
+
+	if ((x0 > x1) || (y0 > y1) || (x1 > pipe_in_res.width) || (y1 > pipe_in_res.height))
+		return -EINVAL;
+
+	if (!enable_zoom) {
+		uds->curr_dx = HRT_GDC_N;
+		uds->curr_dy = HRT_GDC_N;
+	}
+
+	if (info->enable.dvs_envelope) {
+		/* Zoom region is only supported by the UDS module on ISP
+		 * 2 and higher. It is not supported in video mode on ISP 1 */
+		return -EINVAL;
+	} else {
+		if (enable_zoom) {
+			/* A. Calculate dx/dy based on crop region using in_frame_info
+			* Scale the crop region if in_frame_info to the stage is not same as
+			* actual effective input of the pipeline
+			*/
+			if (in_frame_info->res.width != pipe_in_res.width ||
+			    in_frame_info->res.height != pipe_in_res.height) {
+				x0 = (x0 * in_frame_info->res.width) / (pipe_in_res.width);
+				y0 = (y0 * in_frame_info->res.height) / (pipe_in_res.height);
+				x1 = (x1 * in_frame_info->res.width) / (pipe_in_res.width);
+				y1 = (y1 * in_frame_info->res.height) / (pipe_in_res.height);
+			}
+			uds->curr_dx =
+			    ((x1 - x0 - filter_envelope) * HRT_GDC_N) / in_frame_info->res.width;
+			uds->curr_dy =
+			    ((y1 - y0 - filter_envelope) * HRT_GDC_N) / in_frame_info->res.height;
+
+			/* B. Calculate xc/yc based on crop region */
+			uds->xc = (uint16_t)x0 + (((x1) - (x0)) / 2);
+			uds->yc = (uint16_t)y0 + (((y1) - (y0)) / 2);
+		} else {
+			uds->xc = (uint16_t)in_frame_info->res.width / 2;
+			uds->yc = (uint16_t)in_frame_info->res.height / 2;
+		}
+
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "uds->curr_dx=%d, uds->xc=%d, uds->yc=%d\n",
+				    uds->curr_dx, uds->xc, uds->yc);
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "x0=%d, y0=%d, x1=%d, y1=%d\n",
+				    x0, y0, x1, y1);
+		sp_out_crop_pos->x = (uint16_t)info->pipeline.left_cropping;
+		sp_out_crop_pos->y = (uint16_t)info->pipeline.top_cropping;
+	}
+	IA_CSS_LEAVE_PRIVATE("void");
+	return err;
+}
+
+struct ia_css_3a_statistics *
+ia_css_3a_statistics_allocate(const struct ia_css_3a_grid_info *grid)
+{
+	struct ia_css_3a_statistics *me;
+	int grid_size;
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	assert(grid);
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+	grid_size = grid->width * grid->height;
+	me->data = kvmalloc(grid_size * sizeof(*me->data), GFP_KERNEL);
+	if (!me->data)
+		goto err;
+	/* No weighted histogram, no structure, treat the histogram data as a byte dump in a byte array */
+	me->rgby_data = kvmalloc(sizeof_hmem(HMEM0_ID), GFP_KERNEL);
+
+	IA_CSS_LEAVE("return=%p", me);
+	return me;
+err:
+	ia_css_3a_statistics_free(me);
+
+	IA_CSS_LEAVE("return=%p", NULL);
+	return NULL;
+}
+
+void
+ia_css_3a_statistics_free(struct ia_css_3a_statistics *me)
+{
+	if (me) {
+		kvfree(me->rgby_data);
+		kvfree(me->data);
+		kvfree(me);
+	}
+}
+
+struct ia_css_dvs_statistics *
+ia_css_dvs_statistics_allocate(const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_dvs_statistics *me;
+
+	assert(grid);
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+	me->hor_proj = kvmalloc(grid->height * IA_CSS_DVS_NUM_COEF_TYPES *
+				sizeof(*me->hor_proj), GFP_KERNEL);
+	if (!me->hor_proj)
+		goto err;
+
+	me->ver_proj = kvmalloc(grid->width * IA_CSS_DVS_NUM_COEF_TYPES *
+				sizeof(*me->ver_proj), GFP_KERNEL);
+	if (!me->ver_proj)
+		goto err;
+
+	return me;
+err:
+	ia_css_dvs_statistics_free(me);
+	return NULL;
+}
+
+void
+ia_css_dvs_statistics_free(struct ia_css_dvs_statistics *me)
+{
+	if (me) {
+		kvfree(me->hor_proj);
+		kvfree(me->ver_proj);
+		kvfree(me);
+	}
+}
+
+struct ia_css_dvs_coefficients *
+ia_css_dvs_coefficients_allocate(const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_dvs_coefficients *me;
+
+	assert(grid);
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_coefs = kvmalloc(grid->num_hor_coefs *
+				 IA_CSS_DVS_NUM_COEF_TYPES *
+				 sizeof(*me->hor_coefs), GFP_KERNEL);
+	if (!me->hor_coefs)
+		goto err;
+
+	me->ver_coefs = kvmalloc(grid->num_ver_coefs *
+				 IA_CSS_DVS_NUM_COEF_TYPES *
+				 sizeof(*me->ver_coefs), GFP_KERNEL);
+	if (!me->ver_coefs)
+		goto err;
+
+	return me;
+err:
+	ia_css_dvs_coefficients_free(me);
+	return NULL;
+}
+
+void
+ia_css_dvs_coefficients_free(struct ia_css_dvs_coefficients *me)
+{
+	if (me) {
+		kvfree(me->hor_coefs);
+		kvfree(me->ver_coefs);
+		kvfree(me);
+	}
+}
+
+struct ia_css_dvs2_statistics *
+ia_css_dvs2_statistics_allocate(const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_dvs2_statistics *me;
+
+	assert(grid);
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_prod.odd_real = kvmalloc(grid->aligned_width *
+					 grid->aligned_height *
+					 sizeof(*me->hor_prod.odd_real),
+					 GFP_KERNEL);
+	if (!me->hor_prod.odd_real)
+		goto err;
+
+	me->hor_prod.odd_imag = kvmalloc(grid->aligned_width *
+					 grid->aligned_height *
+					 sizeof(*me->hor_prod.odd_imag),
+					 GFP_KERNEL);
+	if (!me->hor_prod.odd_imag)
+		goto err;
+
+	me->hor_prod.even_real = kvmalloc(grid->aligned_width *
+					  grid->aligned_height *
+					  sizeof(*me->hor_prod.even_real),
+					  GFP_KERNEL);
+	if (!me->hor_prod.even_real)
+		goto err;
+
+	me->hor_prod.even_imag = kvmalloc(grid->aligned_width *
+					  grid->aligned_height *
+					  sizeof(*me->hor_prod.even_imag),
+					  GFP_KERNEL);
+	if (!me->hor_prod.even_imag)
+		goto err;
+
+	me->ver_prod.odd_real = kvmalloc(grid->aligned_width *
+					 grid->aligned_height *
+					 sizeof(*me->ver_prod.odd_real),
+					 GFP_KERNEL);
+	if (!me->ver_prod.odd_real)
+		goto err;
+
+	me->ver_prod.odd_imag = kvmalloc(grid->aligned_width *
+					 grid->aligned_height *
+					 sizeof(*me->ver_prod.odd_imag),
+					 GFP_KERNEL);
+	if (!me->ver_prod.odd_imag)
+		goto err;
+
+	me->ver_prod.even_real = kvmalloc(grid->aligned_width *
+					  grid->aligned_height *
+					  sizeof(*me->ver_prod.even_real),
+					  GFP_KERNEL);
+	if (!me->ver_prod.even_real)
+		goto err;
+
+	me->ver_prod.even_imag = kvmalloc(grid->aligned_width *
+					  grid->aligned_height *
+					  sizeof(*me->ver_prod.even_imag),
+					  GFP_KERNEL);
+	if (!me->ver_prod.even_imag)
+		goto err;
+
+	return me;
+err:
+	ia_css_dvs2_statistics_free(me);
+	return NULL;
+}
+
+void
+ia_css_dvs2_statistics_free(struct ia_css_dvs2_statistics *me)
+{
+	if (me) {
+		kvfree(me->hor_prod.odd_real);
+		kvfree(me->hor_prod.odd_imag);
+		kvfree(me->hor_prod.even_real);
+		kvfree(me->hor_prod.even_imag);
+		kvfree(me->ver_prod.odd_real);
+		kvfree(me->ver_prod.odd_imag);
+		kvfree(me->ver_prod.even_real);
+		kvfree(me->ver_prod.even_imag);
+		kvfree(me);
+	}
+}
+
+struct ia_css_dvs2_coefficients *
+ia_css_dvs2_coefficients_allocate(const struct ia_css_dvs_grid_info *grid)
+{
+	struct ia_css_dvs2_coefficients *me;
+
+	assert(grid);
+
+	me = kvcalloc(1, sizeof(*me), GFP_KERNEL);
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_coefs.odd_real = kvmalloc(grid->num_hor_coefs *
+					  sizeof(*me->hor_coefs.odd_real),
+					  GFP_KERNEL);
+	if (!me->hor_coefs.odd_real)
+		goto err;
+
+	me->hor_coefs.odd_imag = kvmalloc(grid->num_hor_coefs *
+					  sizeof(*me->hor_coefs.odd_imag),
+					  GFP_KERNEL);
+	if (!me->hor_coefs.odd_imag)
+		goto err;
+
+	me->hor_coefs.even_real = kvmalloc(grid->num_hor_coefs *
+					   sizeof(*me->hor_coefs.even_real),
+					   GFP_KERNEL);
+	if (!me->hor_coefs.even_real)
+		goto err;
+
+	me->hor_coefs.even_imag = kvmalloc(grid->num_hor_coefs *
+					   sizeof(*me->hor_coefs.even_imag),
+					   GFP_KERNEL);
+	if (!me->hor_coefs.even_imag)
+		goto err;
+
+	me->ver_coefs.odd_real = kvmalloc(grid->num_ver_coefs *
+					  sizeof(*me->ver_coefs.odd_real),
+					  GFP_KERNEL);
+	if (!me->ver_coefs.odd_real)
+		goto err;
+
+	me->ver_coefs.odd_imag = kvmalloc(grid->num_ver_coefs *
+					  sizeof(*me->ver_coefs.odd_imag),
+					  GFP_KERNEL);
+	if (!me->ver_coefs.odd_imag)
+		goto err;
+
+	me->ver_coefs.even_real = kvmalloc(grid->num_ver_coefs *
+					   sizeof(*me->ver_coefs.even_real),
+					   GFP_KERNEL);
+	if (!me->ver_coefs.even_real)
+		goto err;
+
+	me->ver_coefs.even_imag = kvmalloc(grid->num_ver_coefs *
+					   sizeof(*me->ver_coefs.even_imag),
+					   GFP_KERNEL);
+	if (!me->ver_coefs.even_imag)
+		goto err;
+
+	return me;
+err:
+	ia_css_dvs2_coefficients_free(me);
+	return NULL;
+}
+
+void
+ia_css_dvs2_coefficients_free(struct ia_css_dvs2_coefficients *me)
+{
+	if (me) {
+		kvfree(me->hor_coefs.odd_real);
+		kvfree(me->hor_coefs.odd_imag);
+		kvfree(me->hor_coefs.even_real);
+		kvfree(me->hor_coefs.even_imag);
+		kvfree(me->ver_coefs.odd_real);
+		kvfree(me->ver_coefs.odd_imag);
+		kvfree(me->ver_coefs.even_real);
+		kvfree(me->ver_coefs.even_imag);
+		kvfree(me);
+	}
+}
+
+struct ia_css_dvs_6axis_config *
+ia_css_dvs2_6axis_config_allocate(const struct ia_css_stream *stream)
+{
+	struct ia_css_dvs_6axis_config *dvs_config = NULL;
+	struct ia_css_isp_parameters *params = NULL;
+	unsigned int width_y;
+	unsigned int height_y;
+	unsigned int width_uv;
+	unsigned int height_uv;
+
+	assert(stream);
+	params = stream->isp_params_configs;
+
+	/* Backward compatibility by default consider pipe as Video*/
+	if (!params || !params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO])
+		goto err;
+
+	dvs_config = kvcalloc(1, sizeof(struct ia_css_dvs_6axis_config),
+			      GFP_KERNEL);
+	if (!dvs_config)
+		goto err;
+
+	dvs_config->width_y = width_y =
+				  params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO]->width_y;
+	dvs_config->height_y = height_y =
+				   params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO]->height_y;
+	dvs_config->width_uv = width_uv =
+				   params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO]->width_uv;
+	dvs_config->height_uv = height_uv =
+				    params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO]->height_uv;
+	IA_CSS_LOG("table Y: W %d H %d", width_y, height_y);
+	IA_CSS_LOG("table UV: W %d H %d", width_uv, height_uv);
+	dvs_config->xcoords_y = kvmalloc(width_y * height_y * sizeof(uint32_t),
+					 GFP_KERNEL);
+	if (!dvs_config->xcoords_y)
+		goto err;
+
+	dvs_config->ycoords_y = kvmalloc(width_y * height_y * sizeof(uint32_t),
+					 GFP_KERNEL);
+	if (!dvs_config->ycoords_y)
+		goto err;
+
+	dvs_config->xcoords_uv = kvmalloc(width_uv * height_uv *
+					  sizeof(uint32_t),
+					  GFP_KERNEL);
+	if (!dvs_config->xcoords_uv)
+		goto err;
+
+	dvs_config->ycoords_uv = kvmalloc(width_uv * height_uv *
+					  sizeof(uint32_t),
+					  GFP_KERNEL);
+	if (!dvs_config->ycoords_uv)
+		goto err;
+
+	return dvs_config;
+err:
+	ia_css_dvs2_6axis_config_free(dvs_config);
+	return NULL;
+}
+
+void
+ia_css_dvs2_6axis_config_free(struct ia_css_dvs_6axis_config *dvs_6axis_config)
+{
+	if (dvs_6axis_config) {
+		kvfree(dvs_6axis_config->xcoords_y);
+		kvfree(dvs_6axis_config->ycoords_y);
+		kvfree(dvs_6axis_config->xcoords_uv);
+		kvfree(dvs_6axis_config->ycoords_uv);
+		kvfree(dvs_6axis_config);
+	}
+}
+
+void
+ia_css_en_dz_capt_pipe(struct ia_css_stream *stream, bool enable)
+{
+	struct ia_css_pipe *pipe;
+	struct ia_css_pipeline *pipeline;
+	struct ia_css_pipeline_stage *stage;
+	enum ia_css_pipe_id pipe_id;
+	int err;
+	int i;
+
+	if (!stream)
+		return;
+
+	for (i = 0; i < stream->num_pipes; i++) {
+		pipe = stream->pipes[i];
+		pipeline = ia_css_pipe_get_pipeline(pipe);
+		pipe_id = pipeline->pipe_id;
+
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE) {
+			err = ia_css_pipeline_get_stage(pipeline, IA_CSS_BINARY_MODE_CAPTURE_PP,
+							&stage);
+			if (!err)
+				stage->enable_zoom = enable;
+			break;
+		}
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_params.h b/drivers/staging/media/atomisp/pci/sh_css_params.h
new file mode 100644
index 000000000..bbca19d0e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_params.h
@@ -0,0 +1,181 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_PARAMS_H_
+#define _SH_CSS_PARAMS_H_
+
+/*! \file */
+
+/* Forward declaration to break mutual dependency */
+struct ia_css_isp_parameters;
+
+#include <type_support.h>
+#include "ia_css_types.h"
+#include "ia_css_binary.h"
+#include "sh_css_legacy.h"
+
+#include "sh_css_defs.h"	/* SH_CSS_MAX_STAGES */
+#include "ia_css_pipeline.h"
+#include "ia_css_isp_params.h"
+#include "uds/uds_1.0/ia_css_uds_param.h"
+#include "crop/crop_1.0/ia_css_crop_types.h"
+
+#define PIX_SHIFT_FILTER_RUN_IN_X 12
+#define PIX_SHIFT_FILTER_RUN_IN_Y 12
+
+#include "ob/ob_1.0/ia_css_ob_param.h"
+/* Isp configurations per stream */
+struct sh_css_isp_param_configs {
+	/* OB (Optical Black) */
+	struct sh_css_isp_ob_stream_config ob;
+};
+
+/* Isp parameters per stream */
+struct ia_css_isp_parameters {
+	/* UDS */
+	struct sh_css_sp_uds_params uds[SH_CSS_MAX_STAGES];
+	struct sh_css_isp_param_configs stream_configs;
+	struct ia_css_fpn_table     fpn_config;
+	struct ia_css_vector	    motion_config;
+	const struct ia_css_morph_table   *morph_table;
+	const struct ia_css_shading_table *sc_table;
+	struct ia_css_shading_table *sc_config;
+	struct ia_css_macc_table    macc_table;
+	struct ia_css_gamma_table   gc_table;
+	struct ia_css_ctc_table     ctc_table;
+	struct ia_css_xnr_table     xnr_table;
+
+	struct ia_css_dz_config     dz_config;
+	struct ia_css_3a_config     s3a_config;
+	struct ia_css_wb_config     wb_config;
+	struct ia_css_cc_config     cc_config;
+	struct ia_css_cc_config     yuv2rgb_cc_config;
+	struct ia_css_cc_config     rgb2yuv_cc_config;
+	struct ia_css_tnr_config    tnr_config;
+	struct ia_css_ob_config     ob_config;
+	/*----- DPC configuration -----*/
+	/* The default DPC configuration is retained and currently set
+	 * using the stream configuration. The code generated from genparams
+	 * uses this configuration to set the DPC parameters per stage but this
+	 * will be overwritten by the per pipe configuration */
+	struct ia_css_dp_config     dp_config;
+	/* ------ pipe specific DPC configuration ------ */
+	/* Please note that this implementation is a temporary solution and
+	 * should be replaced by CSS per pipe configuration when the support
+	 * is ready (HSD 1303967698)*/
+	struct ia_css_dp_config     pipe_dp_config[IA_CSS_PIPE_ID_NUM];
+	struct ia_css_nr_config     nr_config;
+	struct ia_css_ee_config     ee_config;
+	struct ia_css_de_config     de_config;
+	struct ia_css_gc_config     gc_config;
+	struct ia_css_anr_config    anr_config;
+	struct ia_css_ce_config     ce_config;
+	struct ia_css_formats_config     formats_config;
+	/* ---- deprecated: replaced with pipe_dvs_6axis_config---- */
+	struct ia_css_dvs_6axis_config  *dvs_6axis_config;
+	struct ia_css_ecd_config    ecd_config;
+	struct ia_css_ynr_config    ynr_config;
+	struct ia_css_yee_config    yee_config;
+	struct ia_css_fc_config     fc_config;
+	struct ia_css_cnr_config    cnr_config;
+	struct ia_css_macc_config   macc_config;
+	struct ia_css_ctc_config    ctc_config;
+	struct ia_css_aa_config     aa_config;
+	struct ia_css_aa_config     bds_config;
+	struct ia_css_aa_config     raa_config;
+	struct ia_css_rgb_gamma_table     r_gamma_table;
+	struct ia_css_rgb_gamma_table     g_gamma_table;
+	struct ia_css_rgb_gamma_table     b_gamma_table;
+	struct ia_css_anr_thres     anr_thres;
+	struct ia_css_xnr_config    xnr_config;
+	struct ia_css_xnr3_config   xnr3_config;
+	struct ia_css_uds_config    uds_config;
+	struct ia_css_crop_config   crop_config;
+	struct ia_css_output_config output_config;
+	struct ia_css_dvs_6axis_config  *pipe_dvs_6axis_config[IA_CSS_PIPE_ID_NUM];
+	/* ------ deprecated(bz675) : from ------ */
+	struct ia_css_shading_settings shading_settings;
+	/* ------ deprecated(bz675) : to ------ */
+	struct ia_css_dvs_coefficients  dvs_coefs;
+	struct ia_css_dvs2_coefficients dvs2_coefs;
+
+	bool isp_params_changed;
+
+	bool isp_mem_params_changed
+	[IA_CSS_PIPE_ID_NUM][SH_CSS_MAX_STAGES][IA_CSS_NUM_MEMORIES];
+	bool dz_config_changed;
+	bool motion_config_changed;
+	bool dis_coef_table_changed;
+	bool dvs2_coef_table_changed;
+	bool morph_table_changed;
+	bool sc_table_changed;
+	bool anr_thres_changed;
+	/* ---- deprecated: replaced with pipe_dvs_6axis_config_changed ---- */
+	bool dvs_6axis_config_changed;
+	/* ------ pipe specific DPC configuration ------ */
+	/* Please note that this implementation is a temporary solution and
+	 * should be replaced by CSS per pipe configuration when the support
+	 * is ready (HSD 1303967698) */
+	bool pipe_dpc_config_changed[IA_CSS_PIPE_ID_NUM];
+	/* ------ deprecated(bz675) : from ------ */
+	bool shading_settings_changed;
+	/* ------ deprecated(bz675) : to ------ */
+	bool pipe_dvs_6axis_config_changed[IA_CSS_PIPE_ID_NUM];
+
+	bool config_changed[IA_CSS_NUM_PARAMETER_IDS];
+
+	unsigned int sensor_binning;
+	/* local buffers, used to re-order the 3a statistics in vmem-format */
+	struct sh_css_ddr_address_map pipe_ddr_ptrs[IA_CSS_PIPE_ID_NUM];
+	struct sh_css_ddr_address_map_size pipe_ddr_ptrs_size[IA_CSS_PIPE_ID_NUM];
+	struct sh_css_ddr_address_map ddr_ptrs;
+	struct sh_css_ddr_address_map_size ddr_ptrs_size;
+	struct ia_css_frame
+		*output_frame; /** Output frame the config is to be applied to (optional) */
+	u32 isp_parameters_id; /** Unique ID to track which config was actually applied to a particular frame */
+};
+
+void
+ia_css_params_store_ia_css_host_data(
+    ia_css_ptr ddr_addr,
+    struct ia_css_host_data *data);
+
+int
+ia_css_params_store_sctbl(
+    const struct ia_css_pipeline_stage *stage,
+    ia_css_ptr ddr_addr,
+    const struct ia_css_shading_table *shading_table);
+
+struct ia_css_host_data *
+ia_css_params_alloc_convert_sctbl(
+    const struct ia_css_pipeline_stage *stage,
+    const struct ia_css_shading_table *shading_table);
+
+struct ia_css_isp_config *
+sh_css_pipe_isp_config_get(struct ia_css_pipe *pipe);
+
+int
+sh_css_params_map_and_store_default_gdc_lut(void);
+
+void
+sh_css_params_free_default_gdc_lut(void);
+
+ia_css_ptr
+sh_css_params_get_default_gdc_lut(void);
+
+ia_css_ptr
+sh_css_pipe_get_pp_gdc_lut(const struct ia_css_pipe *pipe);
+
+#endif /* _SH_CSS_PARAMS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_params_internal.h b/drivers/staging/media/atomisp/pci/sh_css_params_internal.h
new file mode 100644
index 000000000..8e5e6f273
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_params_internal.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_PARAMS_INTERNAL_H_
+#define _SH_CSS_PARAMS_INTERNAL_H_
+
+void
+sh_css_param_clear_param_sets(void);
+
+#endif /* _SH_CSS_PARAMS_INTERNAL_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_properties.c b/drivers/staging/media/atomisp/pci/sh_css_properties.c
new file mode 100644
index 000000000..8ecd93d65
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_properties.c
@@ -0,0 +1,34 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "ia_css_properties.h"
+#include <assert_support.h>
+#include "ia_css_types.h"
+#include "gdc_device.h"
+
+void
+ia_css_get_properties(struct ia_css_properties *properties)
+{
+	assert(properties);
+	/*
+	 * MW: We don't want to store the coordinates
+	 * full range in memory: Truncate
+	 */
+	properties->gdc_coord_one = gdc_get_unity(GDC0_ID) / HRT_GDC_COORD_SCALE;
+
+	properties->l1_base_is_index = true;
+
+	properties->vamem_type = IA_CSS_VAMEM_TYPE_2;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_sp.c b/drivers/staging/media/atomisp/pci/sh_css_sp.c
new file mode 100644
index 000000000..615500a7d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_sp.c
@@ -0,0 +1,1773 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "hmm.h"
+
+#include "sh_css_sp.h"
+
+#if !defined(ISP2401)
+#include "input_formatter.h"
+#endif
+
+#include "dma.h"	/* N_DMA_CHANNEL_ID */
+
+#include "ia_css_buffer.h"
+#include "ia_css_binary.h"
+#include "sh_css_hrt.h"
+#include "sh_css_defs.h"
+#include "sh_css_internal.h"
+#include "ia_css_control.h"
+#include "ia_css_debug.h"
+#include "ia_css_debug_pipe.h"
+#include "ia_css_event_public.h"
+#include "ia_css_mmu.h"
+#include "ia_css_stream.h"
+#include "ia_css_isp_param.h"
+#include "sh_css_params.h"
+#include "sh_css_legacy.h"
+#include "ia_css_frame_comm.h"
+#include "ia_css_isys.h"
+
+#include "gdc_device.h"				/* HRT_GDC_N */
+
+/*#include "sp.h"*/	/* host2sp_enqueue_frame_data() */
+
+
+#include "assert_support.h"
+
+#include "sw_event_global.h"			/* Event IDs.*/
+#include "ia_css_event.h"
+#include "mmu_device.h"
+#include "ia_css_spctrl.h"
+
+#ifndef offsetof
+#define offsetof(T, x) ((unsigned int)&(((T *)0)->x))
+#endif
+
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#define IA_CSS_INCLUDE_STATES
+#include "ia_css_isp_states.h"
+
+#include "isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h"
+
+struct sh_css_sp_group		sh_css_sp_group;
+struct sh_css_sp_stage		sh_css_sp_stage;
+struct sh_css_isp_stage		sh_css_isp_stage;
+static struct sh_css_sp_output		sh_css_sp_output;
+static struct sh_css_sp_per_frame_data per_frame_data;
+
+/* true if SP supports frame loop and host2sp_commands */
+/* For the moment there is only code that sets this bool to true */
+/* TODO: add code that sets this bool to false */
+static bool sp_running;
+
+static int
+set_output_frame_buffer(const struct ia_css_frame *frame,
+			unsigned int idx);
+
+static void
+sh_css_copy_buffer_attr_to_spbuffer(struct ia_css_buffer_sp *dest_buf,
+				    const enum sh_css_queue_id queue_id,
+				    const ia_css_ptr xmem_addr,
+				    const enum ia_css_buffer_type buf_type);
+
+static void
+initialize_frame_buffer_attribute(struct ia_css_buffer_sp *buf_attr);
+
+static void
+initialize_stage_frames(struct ia_css_frames_sp *frames);
+
+/* This data is stored every frame */
+void
+store_sp_group_data(void)
+{
+	per_frame_data.sp_group_addr = sh_css_store_sp_group_to_ddr();
+}
+
+static void
+copy_isp_stage_to_sp_stage(void)
+{
+	/* [WW07.5]type casting will cause potential issues */
+	sh_css_sp_stage.num_stripes = (uint8_t)
+				      sh_css_isp_stage.binary_info.iterator.num_stripes;
+	sh_css_sp_stage.row_stripes_height = (uint16_t)
+					     sh_css_isp_stage.binary_info.iterator.row_stripes_height;
+	sh_css_sp_stage.row_stripes_overlap_lines = (uint16_t)
+		sh_css_isp_stage.binary_info.iterator.row_stripes_overlap_lines;
+	sh_css_sp_stage.top_cropping = (uint16_t)
+				       sh_css_isp_stage.binary_info.pipeline.top_cropping;
+	/* moved to sh_css_sp_init_stage
+	   sh_css_sp_stage.enable.vf_output =
+	   sh_css_isp_stage.binary_info.enable.vf_veceven ||
+	   sh_css_isp_stage.binary_info.num_output_pins > 1;
+	*/
+	sh_css_sp_stage.enable.sdis = sh_css_isp_stage.binary_info.enable.dis;
+	sh_css_sp_stage.enable.s3a = sh_css_isp_stage.binary_info.enable.s3a;
+}
+
+void
+store_sp_stage_data(enum ia_css_pipe_id id, unsigned int pipe_num,
+		    unsigned int stage)
+{
+	unsigned int thread_id;
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	copy_isp_stage_to_sp_stage();
+	if (id != IA_CSS_PIPE_ID_COPY)
+		sh_css_sp_stage.isp_stage_addr =
+		    sh_css_store_isp_stage_to_ddr(pipe_num, stage);
+	sh_css_sp_group.pipe[thread_id].sp_stage_addr[stage] =
+	    sh_css_store_sp_stage_to_ddr(pipe_num, stage);
+
+	/* Clear for next frame */
+	sh_css_sp_stage.program_input_circuit = false;
+}
+
+static void
+store_sp_per_frame_data(const struct ia_css_fw_info *fw)
+{
+	unsigned int HIVE_ADDR_sp_per_frame_data = 0;
+
+	assert(fw);
+
+	switch (fw->type) {
+	case ia_css_sp_firmware:
+		HIVE_ADDR_sp_per_frame_data = fw->info.sp.per_frame_data;
+		break;
+	case ia_css_acc_firmware:
+		HIVE_ADDR_sp_per_frame_data = fw->info.acc.per_frame_data;
+		break;
+	case ia_css_isp_firmware:
+		return;
+	}
+
+	sp_dmem_store(SP0_ID,
+		      (unsigned int)sp_address_of(sp_per_frame_data),
+		      &per_frame_data,
+		      sizeof(per_frame_data));
+}
+
+static void
+sh_css_store_sp_per_frame_data(enum ia_css_pipe_id pipe_id,
+			       unsigned int pipe_num,
+			       const struct ia_css_fw_info *sp_fw)
+{
+	if (!sp_fw)
+		sp_fw = &sh_css_sp_fw;
+
+	store_sp_stage_data(pipe_id, pipe_num, 0);
+	store_sp_group_data();
+	store_sp_per_frame_data(sp_fw);
+}
+
+#if SP_DEBUG != SP_DEBUG_NONE
+
+void
+sh_css_sp_get_debug_state(struct sh_css_sp_debug_state *state)
+{
+	const struct ia_css_fw_info *fw = &sh_css_sp_fw;
+	unsigned int HIVE_ADDR_sp_output = fw->info.sp.output;
+	unsigned int i;
+	unsigned int offset = (unsigned int)offsetof(struct sh_css_sp_output,
+			      debug) / sizeof(int);
+
+	assert(state);
+
+	(void)HIVE_ADDR_sp_output; /* To get rid of warning in CRUN */
+	for (i = 0; i < sizeof(*state) / sizeof(int); i++)
+		((unsigned *)state)[i] = load_sp_array_uint(sp_output, i + offset);
+}
+
+#endif
+
+void
+sh_css_sp_start_binary_copy(unsigned int pipe_num,
+			    struct ia_css_frame *out_frame,
+			    unsigned int two_ppc)
+{
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	struct sh_css_sp_pipeline *pipe;
+	u8 stage_num = 0;
+
+	assert(out_frame);
+	pipe_id = IA_CSS_PIPE_ID_CAPTURE;
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	pipe = &sh_css_sp_group.pipe[thread_id];
+
+	pipe->copy.bin.bytes_available = out_frame->data_bytes;
+	pipe->num_stages = 1;
+	pipe->pipe_id = pipe_id;
+	pipe->pipe_num = pipe_num;
+	pipe->thread_id = thread_id;
+	pipe->pipe_config = 0x0; /* No parameters */
+	pipe->pipe_qos_config = QOS_INVALID;
+
+	if (pipe->inout_port_config == 0) {
+		SH_CSS_PIPE_PORT_CONFIG_SET(pipe->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(pipe->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+	}
+	IA_CSS_LOG("pipe_id %d port_config %08x",
+		   pipe->pipe_id, pipe->inout_port_config);
+
+#if !defined(ISP2401)
+	sh_css_sp_group.config.input_formatter.isp_2ppc = (uint8_t)two_ppc;
+#else
+	(void)two_ppc;
+#endif
+
+	sh_css_sp_stage.num = stage_num;
+	sh_css_sp_stage.stage_type = SH_CSS_SP_STAGE_TYPE;
+	sh_css_sp_stage.func =
+	    (unsigned int)IA_CSS_PIPELINE_BIN_COPY;
+
+	set_output_frame_buffer(out_frame, 0);
+
+	/* sp_bin_copy_init on the SP does not deal with dynamica/static yet */
+	/* For now always update the dynamic data from out frames. */
+	sh_css_store_sp_per_frame_data(pipe_id, pipe_num, &sh_css_sp_fw);
+}
+
+static void
+sh_css_sp_start_raw_copy(struct ia_css_frame *out_frame,
+			 unsigned int pipe_num,
+			 unsigned int two_ppc,
+			 unsigned int max_input_width,
+			 enum sh_css_pipe_config_override pipe_conf_override,
+			 unsigned int if_config_index)
+{
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	u8 stage_num = 0;
+	struct sh_css_sp_pipeline *pipe;
+
+	assert(out_frame);
+
+	{
+		/*
+		 * Clear sh_css_sp_stage for easy debugging.
+		 * program_input_circuit must be saved as it is set outside
+		 * this function.
+		 */
+		u8 program_input_circuit;
+
+		program_input_circuit = sh_css_sp_stage.program_input_circuit;
+		memset(&sh_css_sp_stage, 0, sizeof(sh_css_sp_stage));
+		sh_css_sp_stage.program_input_circuit = program_input_circuit;
+	}
+
+	pipe_id = IA_CSS_PIPE_ID_COPY;
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	pipe = &sh_css_sp_group.pipe[thread_id];
+
+	pipe->copy.raw.height	    = out_frame->info.res.height;
+	pipe->copy.raw.width	    = out_frame->info.res.width;
+	pipe->copy.raw.padded_width  = out_frame->info.padded_width;
+	pipe->copy.raw.raw_bit_depth = out_frame->info.raw_bit_depth;
+	pipe->copy.raw.max_input_width = max_input_width;
+	pipe->num_stages = 1;
+	pipe->pipe_id = pipe_id;
+	/* TODO: next indicates from which queues parameters need to be
+		 sampled, needs checking/improvement */
+	if (pipe_conf_override == SH_CSS_PIPE_CONFIG_OVRD_NO_OVRD)
+		pipe->pipe_config =
+		    (SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS << thread_id);
+	else
+		pipe->pipe_config = pipe_conf_override;
+
+	pipe->pipe_qos_config = QOS_INVALID;
+
+	if (pipe->inout_port_config == 0) {
+		SH_CSS_PIPE_PORT_CONFIG_SET(pipe->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_INPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+		SH_CSS_PIPE_PORT_CONFIG_SET(pipe->inout_port_config,
+					    (uint8_t)SH_CSS_PORT_OUTPUT,
+					    (uint8_t)SH_CSS_HOST_TYPE, 1);
+	}
+	IA_CSS_LOG("pipe_id %d port_config %08x",
+		   pipe->pipe_id, pipe->inout_port_config);
+
+#if !defined(ISP2401)
+	sh_css_sp_group.config.input_formatter.isp_2ppc = (uint8_t)two_ppc;
+#else
+	(void)two_ppc;
+#endif
+
+	sh_css_sp_stage.num = stage_num;
+	sh_css_sp_stage.xmem_bin_addr = 0x0;
+	sh_css_sp_stage.stage_type = SH_CSS_SP_STAGE_TYPE;
+	sh_css_sp_stage.func = (unsigned int)IA_CSS_PIPELINE_RAW_COPY;
+	sh_css_sp_stage.if_config_index = (uint8_t)if_config_index;
+	set_output_frame_buffer(out_frame, 0);
+
+	ia_css_debug_pipe_graph_dump_sp_raw_copy(out_frame);
+}
+
+static void
+sh_css_sp_start_isys_copy(struct ia_css_frame *out_frame,
+			  unsigned int pipe_num, unsigned int max_input_width,
+			  unsigned int if_config_index)
+{
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	u8 stage_num = 0;
+	struct sh_css_sp_pipeline *pipe;
+	enum sh_css_queue_id queue_id;
+
+	assert(out_frame);
+
+	{
+		/*
+		 * Clear sh_css_sp_stage for easy debugging.
+		 * program_input_circuit must be saved as it is set outside
+		 * this function.
+		 */
+		u8 program_input_circuit;
+
+		program_input_circuit = sh_css_sp_stage.program_input_circuit;
+		memset(&sh_css_sp_stage, 0, sizeof(sh_css_sp_stage));
+		sh_css_sp_stage.program_input_circuit = program_input_circuit;
+	}
+
+	pipe_id = IA_CSS_PIPE_ID_COPY;
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	pipe = &sh_css_sp_group.pipe[thread_id];
+
+	pipe->copy.raw.height		= out_frame->info.res.height;
+	pipe->copy.raw.width		= out_frame->info.res.width;
+	pipe->copy.raw.padded_width	= out_frame->info.padded_width;
+	pipe->copy.raw.raw_bit_depth	= out_frame->info.raw_bit_depth;
+	pipe->copy.raw.max_input_width	= max_input_width;
+	pipe->num_stages		= 1;
+	pipe->pipe_id			= pipe_id;
+	pipe->pipe_config		= 0x0;	/* No parameters */
+	pipe->pipe_qos_config		= QOS_INVALID;
+
+	initialize_stage_frames(&sh_css_sp_stage.frames);
+	sh_css_sp_stage.num = stage_num;
+	sh_css_sp_stage.xmem_bin_addr = 0x0;
+	sh_css_sp_stage.stage_type = SH_CSS_SP_STAGE_TYPE;
+	sh_css_sp_stage.func = (unsigned int)IA_CSS_PIPELINE_ISYS_COPY;
+	sh_css_sp_stage.if_config_index = (uint8_t)if_config_index;
+
+	set_output_frame_buffer(out_frame, 0);
+
+	if (pipe->metadata.height > 0) {
+		ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_METADATA, thread_id,
+					       &queue_id);
+		sh_css_copy_buffer_attr_to_spbuffer(&sh_css_sp_stage.frames.metadata_buf,
+						    queue_id, mmgr_EXCEPTION,
+						    IA_CSS_BUFFER_TYPE_METADATA);
+	}
+
+	ia_css_debug_pipe_graph_dump_sp_raw_copy(out_frame);
+}
+
+unsigned int
+sh_css_sp_get_binary_copy_size(void)
+{
+	const struct ia_css_fw_info *fw = &sh_css_sp_fw;
+	unsigned int HIVE_ADDR_sp_output = fw->info.sp.output;
+	unsigned int offset = (unsigned int)offsetof(struct sh_css_sp_output,
+			      bin_copy_bytes_copied) / sizeof(int);
+	(void)HIVE_ADDR_sp_output; /* To get rid of warning in CRUN */
+	return load_sp_array_uint(sp_output, offset);
+}
+
+unsigned int
+sh_css_sp_get_sw_interrupt_value(unsigned int irq)
+{
+	const struct ia_css_fw_info *fw = &sh_css_sp_fw;
+	unsigned int HIVE_ADDR_sp_output = fw->info.sp.output;
+	unsigned int offset = (unsigned int)offsetof(struct sh_css_sp_output,
+			      sw_interrupt_value)
+			      / sizeof(int);
+	(void)HIVE_ADDR_sp_output; /* To get rid of warning in CRUN */
+	return load_sp_array_uint(sp_output, offset + irq);
+}
+
+static void
+sh_css_copy_buffer_attr_to_spbuffer(struct ia_css_buffer_sp *dest_buf,
+				    const enum sh_css_queue_id queue_id,
+				    const ia_css_ptr xmem_addr,
+				    const enum ia_css_buffer_type buf_type)
+{
+	assert(buf_type < IA_CSS_NUM_BUFFER_TYPE);
+	if (queue_id > SH_CSS_INVALID_QUEUE_ID) {
+		/*
+		 * value >=0 indicates that function init_frame_pointers()
+		 * should use the dynamic data address
+		 */
+		assert(queue_id < SH_CSS_MAX_NUM_QUEUES);
+
+		/* Klocwork assumes assert can be disabled;
+		   Since we can get there with any type, and it does not
+		   know that frame_in->dynamic_data_index can only be set
+		   for one of the types in the assert) it has to assume we
+		   can get here for any type. however this could lead to an
+		   out of bounds reference when indexing buf_type about 10
+		   lines below. In order to satisfy KW an additional if
+		   has been added. This one will always yield true.
+		 */
+		if ((queue_id < SH_CSS_MAX_NUM_QUEUES)) {
+			dest_buf->buf_src.queue_id = queue_id;
+		}
+	} else {
+		assert(xmem_addr != mmgr_EXCEPTION);
+		dest_buf->buf_src.xmem_addr = xmem_addr;
+	}
+	dest_buf->buf_type = buf_type;
+}
+
+static void
+sh_css_copy_frame_to_spframe(struct ia_css_frame_sp *sp_frame_out,
+			     const struct ia_css_frame *frame_in)
+{
+	assert(frame_in);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			    "sh_css_copy_frame_to_spframe():\n");
+
+	sh_css_copy_buffer_attr_to_spbuffer(&sp_frame_out->buf_attr,
+					    frame_in->dynamic_queue_id,
+					    frame_in->data,
+					    frame_in->buf_type);
+
+	ia_css_frame_info_to_frame_sp_info(&sp_frame_out->info, &frame_in->info);
+
+	switch (frame_in->info.format) {
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+	case IA_CSS_FRAME_FORMAT_RAW:
+		sp_frame_out->planes.raw.offset = frame_in->planes.raw.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_RGB565:
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+		sp_frame_out->planes.rgb.offset = frame_in->planes.rgb.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
+		sp_frame_out->planes.planar_rgb.r.offset =
+		    frame_in->planes.planar_rgb.r.offset;
+		sp_frame_out->planes.planar_rgb.g.offset =
+		    frame_in->planes.planar_rgb.g.offset;
+		sp_frame_out->planes.planar_rgb.b.offset =
+		    frame_in->planes.planar_rgb.b.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+		sp_frame_out->planes.yuyv.offset = frame_in->planes.yuyv.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_NV11:
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_NV16:
+	case IA_CSS_FRAME_FORMAT_NV61:
+		sp_frame_out->planes.nv.y.offset =
+		    frame_in->planes.nv.y.offset;
+		sp_frame_out->planes.nv.uv.offset =
+		    frame_in->planes.nv.uv.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YUV422:
+	case IA_CSS_FRAME_FORMAT_YUV444:
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+	case IA_CSS_FRAME_FORMAT_YV12:
+	case IA_CSS_FRAME_FORMAT_YV16:
+		sp_frame_out->planes.yuv.y.offset =
+		    frame_in->planes.yuv.y.offset;
+		sp_frame_out->planes.yuv.u.offset =
+		    frame_in->planes.yuv.u.offset;
+		sp_frame_out->planes.yuv.v.offset =
+		    frame_in->planes.yuv.v.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+		sp_frame_out->planes.plane6.r.offset =
+		    frame_in->planes.plane6.r.offset;
+		sp_frame_out->planes.plane6.r_at_b.offset =
+		    frame_in->planes.plane6.r_at_b.offset;
+		sp_frame_out->planes.plane6.gr.offset =
+		    frame_in->planes.plane6.gr.offset;
+		sp_frame_out->planes.plane6.gb.offset =
+		    frame_in->planes.plane6.gb.offset;
+		sp_frame_out->planes.plane6.b.offset =
+		    frame_in->planes.plane6.b.offset;
+		sp_frame_out->planes.plane6.b_at_r.offset =
+		    frame_in->planes.plane6.b_at_r.offset;
+		break;
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+		sp_frame_out->planes.binary.data.offset =
+		    frame_in->planes.binary.data.offset;
+		break;
+	default:
+		/* This should not happen, but in case it does,
+		 * nullify the planes
+		 */
+		memset(&sp_frame_out->planes, 0, sizeof(sp_frame_out->planes));
+		break;
+	}
+}
+
+static int
+set_input_frame_buffer(const struct ia_css_frame *frame)
+{
+	if (!frame)
+		return -EINVAL;
+
+	switch (frame->info.format) {
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+	case IA_CSS_FRAME_FORMAT_RAW:
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_10:
+		break;
+	default:
+		return -EINVAL;
+	}
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.in, frame);
+
+	return 0;
+}
+
+static int
+set_output_frame_buffer(const struct ia_css_frame *frame,
+			unsigned int idx)
+{
+	if (!frame)
+		return -EINVAL;
+
+	switch (frame->info.format) {
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YUV422:
+	case IA_CSS_FRAME_FORMAT_YUV444:
+	case IA_CSS_FRAME_FORMAT_YV12:
+	case IA_CSS_FRAME_FORMAT_YV16:
+	case IA_CSS_FRAME_FORMAT_YUV420_16:
+	case IA_CSS_FRAME_FORMAT_YUV422_16:
+	case IA_CSS_FRAME_FORMAT_NV11:
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+	case IA_CSS_FRAME_FORMAT_NV16:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_NV61:
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+	case IA_CSS_FRAME_FORMAT_RGB565:
+	case IA_CSS_FRAME_FORMAT_RGBA888:
+	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
+	case IA_CSS_FRAME_FORMAT_RAW:
+	case IA_CSS_FRAME_FORMAT_RAW_PACKED:
+	case IA_CSS_FRAME_FORMAT_QPLANE6:
+	case IA_CSS_FRAME_FORMAT_BINARY_8:
+		break;
+	default:
+		return -EINVAL;
+	}
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.out[idx], frame);
+	return 0;
+}
+
+static int
+set_view_finder_buffer(const struct ia_css_frame *frame)
+{
+	if (!frame)
+		return -EINVAL;
+
+	switch (frame->info.format) {
+	/* the dual output pin */
+	case IA_CSS_FRAME_FORMAT_NV12:
+	case IA_CSS_FRAME_FORMAT_NV12_16:
+	case IA_CSS_FRAME_FORMAT_NV21:
+	case IA_CSS_FRAME_FORMAT_YUYV:
+	case IA_CSS_FRAME_FORMAT_UYVY:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8:
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_YV12:
+	case IA_CSS_FRAME_FORMAT_NV12_TILEY:
+
+	/* for vf_veceven */
+	case IA_CSS_FRAME_FORMAT_YUV_LINE:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.out_vf, frame);
+	return 0;
+}
+
+#if !defined(ISP2401)
+void sh_css_sp_set_if_configs(
+    const input_formatter_cfg_t	*config_a,
+    const input_formatter_cfg_t	*config_b,
+    const uint8_t		if_config_index
+)
+{
+	assert(if_config_index < SH_CSS_MAX_IF_CONFIGS);
+	assert(config_a);
+
+	sh_css_sp_group.config.input_formatter.set[if_config_index].config_a =
+	    *config_a;
+	sh_css_sp_group.config.input_formatter.a_changed = true;
+
+	if (config_b) {
+		sh_css_sp_group.config.input_formatter.set[if_config_index].config_b =
+		    *config_b;
+		sh_css_sp_group.config.input_formatter.b_changed = true;
+	}
+
+	return;
+}
+#endif
+
+#if !defined(ISP2401)
+void
+sh_css_sp_program_input_circuit(int fmt_type,
+				int ch_id,
+				enum ia_css_input_mode input_mode)
+{
+	sh_css_sp_group.config.input_circuit.no_side_band = false;
+	sh_css_sp_group.config.input_circuit.fmt_type     = fmt_type;
+	sh_css_sp_group.config.input_circuit.ch_id	      = ch_id;
+	sh_css_sp_group.config.input_circuit.input_mode   = input_mode;
+	/*
+	 * The SP group is only loaded at SP boot time and is read once
+	 * change flags as "input_circuit_cfg_changed" must be reset on the SP
+	 */
+	sh_css_sp_group.config.input_circuit_cfg_changed = true;
+	sh_css_sp_stage.program_input_circuit = true;
+}
+#endif
+
+#if !defined(ISP2401)
+void
+sh_css_sp_configure_sync_gen(int width, int height,
+			     int hblank_cycles,
+			     int vblank_cycles)
+{
+	sh_css_sp_group.config.sync_gen.width	       = width;
+	sh_css_sp_group.config.sync_gen.height	       = height;
+	sh_css_sp_group.config.sync_gen.hblank_cycles = hblank_cycles;
+	sh_css_sp_group.config.sync_gen.vblank_cycles = vblank_cycles;
+}
+
+void
+sh_css_sp_configure_tpg(int x_mask,
+			int y_mask,
+			int x_delta,
+			int y_delta,
+			int xy_mask)
+{
+	sh_css_sp_group.config.tpg.x_mask  = x_mask;
+	sh_css_sp_group.config.tpg.y_mask  = y_mask;
+	sh_css_sp_group.config.tpg.x_delta = x_delta;
+	sh_css_sp_group.config.tpg.y_delta = y_delta;
+	sh_css_sp_group.config.tpg.xy_mask = xy_mask;
+}
+
+void
+sh_css_sp_configure_prbs(int seed)
+{
+	sh_css_sp_group.config.prbs.seed = seed;
+}
+#endif
+
+void
+sh_css_sp_configure_enable_raw_pool_locking(bool lock_all)
+{
+	sh_css_sp_group.config.enable_raw_pool_locking = true;
+	sh_css_sp_group.config.lock_all = lock_all;
+}
+
+void
+sh_css_sp_enable_isys_event_queue(bool enable)
+{
+	sh_css_sp_group.config.enable_isys_event_queue = enable;
+}
+
+void
+sh_css_sp_set_disable_continuous_viewfinder(bool flag)
+{
+	sh_css_sp_group.config.disable_cont_vf = flag;
+}
+
+static int
+sh_css_sp_write_frame_pointers(const struct sh_css_binary_args *args)
+{
+	int err = 0;
+	int i;
+
+	assert(args);
+
+	if (args->in_frame)
+		err = set_input_frame_buffer(args->in_frame);
+	if (!err && args->out_vf_frame)
+		err = set_view_finder_buffer(args->out_vf_frame);
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		if (!err && args->out_frame[i])
+			err = set_output_frame_buffer(args->out_frame[i], i);
+	}
+
+	/* we don't pass this error back to the upper layer, so we add a assert here
+	   because we actually hit the error here but it still works by accident... */
+	if (err) assert(false);
+	return err;
+}
+
+static void
+sh_css_sp_init_group(bool two_ppc,
+		     enum atomisp_input_format input_format,
+		     bool no_isp_sync,
+		     uint8_t if_config_index)
+{
+#if !defined(ISP2401)
+	sh_css_sp_group.config.input_formatter.isp_2ppc = two_ppc;
+#else
+	(void)two_ppc;
+#endif
+
+	sh_css_sp_group.config.no_isp_sync = (uint8_t)no_isp_sync;
+	/* decide whether the frame is processed online or offline */
+	if (if_config_index == SH_CSS_IF_CONFIG_NOT_NEEDED) return;
+#if !defined(ISP2401)
+	assert(if_config_index < SH_CSS_MAX_IF_CONFIGS);
+	sh_css_sp_group.config.input_formatter.set[if_config_index].stream_format =
+	    input_format;
+#else
+	(void)input_format;
+#endif
+}
+
+void
+sh_css_stage_write_binary_info(struct ia_css_binary_info *info)
+{
+	assert(info);
+	sh_css_isp_stage.binary_info = *info;
+}
+
+static int
+copy_isp_mem_if_to_ddr(struct ia_css_binary *binary)
+{
+	int err;
+
+	err = ia_css_isp_param_copy_isp_mem_if_to_ddr(
+	    &binary->css_params,
+	    &binary->mem_params,
+	    IA_CSS_PARAM_CLASS_CONFIG);
+	if (err)
+		return err;
+	err = ia_css_isp_param_copy_isp_mem_if_to_ddr(
+	    &binary->css_params,
+	    &binary->mem_params,
+	    IA_CSS_PARAM_CLASS_STATE);
+	if (err)
+		return err;
+	return 0;
+}
+
+static bool
+is_sp_stage(struct ia_css_pipeline_stage *stage)
+{
+	assert(stage);
+	return stage->sp_func != IA_CSS_PIPELINE_NO_FUNC;
+}
+
+static int configure_isp_from_args(const struct sh_css_sp_pipeline *pipeline,
+				   const struct ia_css_binary      *binary,
+				   const struct sh_css_binary_args *args,
+				   bool				   two_ppc,
+				   bool				   deinterleaved)
+{
+	int ret;
+
+	ret = ia_css_fpn_configure(binary,  &binary->in_frame_info);
+	if (ret)
+		return ret;
+	ret = ia_css_crop_configure(binary, &args->delay_frames[0]->info);
+	if (ret)
+		return ret;
+	ret = ia_css_qplane_configure(pipeline, binary, &binary->in_frame_info);
+	if (ret)
+		return ret;
+	ret = ia_css_output0_configure(binary, &args->out_frame[0]->info);
+	if (ret)
+		return ret;
+	ret = ia_css_output1_configure(binary, &args->out_vf_frame->info);
+	if (ret)
+		return ret;
+	ret = ia_css_copy_output_configure(binary, args->copy_output);
+	if (ret)
+		return ret;
+	ret = ia_css_output0_configure(binary, &args->out_frame[0]->info);
+	if (ret)
+		return ret;
+	ret = ia_css_iterator_configure(binary, &args->in_frame->info);
+	if (ret)
+		return ret;
+	ret = ia_css_dvs_configure(binary, &args->out_frame[0]->info);
+	if (ret)
+		return ret;
+	ret = ia_css_output_configure(binary, &args->out_frame[0]->info);
+	if (ret)
+		return ret;
+	ret = ia_css_raw_configure(pipeline, binary, &args->in_frame->info, &binary->in_frame_info, two_ppc, deinterleaved);
+	if (ret)
+		return ret;
+
+	/*
+	 * FIXME: args->delay_frames can be NULL here
+	 *
+	 * Somehow, the driver at the Intel Atom Yocto tree doesn't seem to
+	 * suffer from the same issue.
+	 *
+	 * Anyway, the function below should now handle a NULL delay_frames
+	 * without crashing, but the pipeline should likely be built without
+	 * adding it at the first place (or there are a hidden bug somewhere)
+	 */
+	ret = ia_css_ref_configure(binary, args->delay_frames, pipeline->dvs_frame_delay);
+	if (ret)
+		return ret;
+	ret = ia_css_tnr_configure(binary, args->tnr_frames);
+	if (ret)
+		return ret;
+	return ia_css_bayer_io_config(binary, args);
+}
+
+static void
+initialize_isp_states(const struct ia_css_binary *binary)
+{
+	unsigned int i;
+
+	if (!binary->info->mem_offsets.offsets.state)
+		return;
+	for (i = 0; i < IA_CSS_NUM_STATE_IDS; i++) {
+		ia_css_kernel_init_state[i](binary);
+	}
+}
+
+static void
+initialize_frame_buffer_attribute(struct ia_css_buffer_sp *buf_attr)
+{
+	buf_attr->buf_src.queue_id = SH_CSS_INVALID_QUEUE_ID;
+	buf_attr->buf_type = IA_CSS_BUFFER_TYPE_INVALID;
+}
+
+static void
+initialize_stage_frames(struct ia_css_frames_sp *frames)
+{
+	unsigned int i;
+
+	initialize_frame_buffer_attribute(&frames->in.buf_attr);
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		initialize_frame_buffer_attribute(&frames->out[i].buf_attr);
+	}
+	initialize_frame_buffer_attribute(&frames->out_vf.buf_attr);
+	initialize_frame_buffer_attribute(&frames->s3a_buf);
+	initialize_frame_buffer_attribute(&frames->dvs_buf);
+	initialize_frame_buffer_attribute(&frames->metadata_buf);
+}
+
+static int
+sh_css_sp_init_stage(struct ia_css_binary *binary,
+		     const char *binary_name,
+		     const struct ia_css_blob_info *blob_info,
+		     const struct sh_css_binary_args *args,
+		     unsigned int pipe_num,
+		     unsigned int stage,
+		     bool xnr,
+		     const struct ia_css_isp_param_css_segments *isp_mem_if,
+		     unsigned int if_config_index,
+		     bool two_ppc)
+{
+	const struct ia_css_binary_xinfo *xinfo;
+	const struct ia_css_binary_info  *info;
+	int err = 0;
+	int i;
+	struct ia_css_pipe *pipe = NULL;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+	bool continuous = sh_css_continuous_is_enabled((uint8_t)pipe_num);
+
+	assert(binary);
+	assert(blob_info);
+	assert(args);
+	assert(isp_mem_if);
+
+	xinfo = binary->info;
+	info  = &xinfo->sp;
+	{
+		/*
+		 * Clear sh_css_sp_stage for easy debugging.
+		 * program_input_circuit must be saved as it is set outside
+		 * this function.
+		 */
+		u8 program_input_circuit;
+
+		program_input_circuit = sh_css_sp_stage.program_input_circuit;
+		memset(&sh_css_sp_stage, 0, sizeof(sh_css_sp_stage));
+		sh_css_sp_stage.program_input_circuit = (uint8_t)program_input_circuit;
+	}
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+
+	if (!info) {
+		sh_css_sp_group.pipe[thread_id].sp_stage_addr[stage] = mmgr_NULL;
+		return 0;
+	}
+
+#if defined(ISP2401)
+	(void)continuous;
+	sh_css_sp_stage.deinterleaved = 0;
+#else
+	sh_css_sp_stage.deinterleaved = ((stage == 0) && continuous);
+#endif
+
+	initialize_stage_frames(&sh_css_sp_stage.frames);
+	/*
+	 * TODO: Make the Host dynamically determine
+	 * the stage type.
+	 */
+	sh_css_sp_stage.stage_type = SH_CSS_ISP_STAGE_TYPE;
+	sh_css_sp_stage.num		= (uint8_t)stage;
+	sh_css_sp_stage.isp_online	= (uint8_t)binary->online;
+	sh_css_sp_stage.isp_copy_vf     = (uint8_t)args->copy_vf;
+	sh_css_sp_stage.isp_copy_output = (uint8_t)args->copy_output;
+	sh_css_sp_stage.enable.vf_output = (args->out_vf_frame != NULL);
+
+	/* Copy the frame infos first, to be overwritten by the frames,
+	   if these are present.
+	*/
+	sh_css_sp_stage.frames.effective_in_res.width = binary->effective_in_frame_res.width;
+	sh_css_sp_stage.frames.effective_in_res.height = binary->effective_in_frame_res.height;
+
+	ia_css_frame_info_to_frame_sp_info(&sh_css_sp_stage.frames.in.info,
+					   &binary->in_frame_info);
+	for (i = 0; i < IA_CSS_BINARY_MAX_OUTPUT_PORTS; i++) {
+		ia_css_frame_info_to_frame_sp_info(&sh_css_sp_stage.frames.out[i].info,
+						   &binary->out_frame_info[i]);
+	}
+	ia_css_frame_info_to_frame_sp_info(&sh_css_sp_stage.frames.internal_frame_info,
+					   &binary->internal_frame_info);
+	sh_css_sp_stage.dvs_envelope.width    = binary->dvs_envelope.width;
+	sh_css_sp_stage.dvs_envelope.height   = binary->dvs_envelope.height;
+	sh_css_sp_stage.isp_pipe_version      = (uint8_t)info->pipeline.isp_pipe_version;
+	sh_css_sp_stage.isp_deci_log_factor   = (uint8_t)binary->deci_factor_log2;
+	sh_css_sp_stage.isp_vf_downscale_bits = (uint8_t)binary->vf_downscale_log2;
+
+	sh_css_sp_stage.if_config_index = (uint8_t)if_config_index;
+
+	sh_css_sp_stage.sp_enable_xnr = (uint8_t)xnr;
+	sh_css_sp_stage.xmem_bin_addr = xinfo->xmem_addr;
+	sh_css_sp_stage.xmem_map_addr = sh_css_params_ddr_address_map();
+	sh_css_isp_stage.blob_info = *blob_info;
+	sh_css_stage_write_binary_info((struct ia_css_binary_info *)info);
+
+	/* Make sure binary name is smaller than allowed string size */
+	assert(strlen(binary_name) < SH_CSS_MAX_BINARY_NAME - 1);
+	strscpy(sh_css_isp_stage.binary_name, binary_name, SH_CSS_MAX_BINARY_NAME);
+	sh_css_isp_stage.mem_initializers = *isp_mem_if;
+
+	/*
+	 * Even when a stage does not need uds and does not params,
+	 * ia_css_uds_sp_scale_params() seems to be called (needs
+	 * further investigation). This function can not deal with
+	 * dx, dy = {0, 0}
+	 */
+
+	err = sh_css_sp_write_frame_pointers(args);
+	/* TODO: move it to a better place */
+	if (binary->info->sp.enable.s3a) {
+		ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_3A_STATISTICS, thread_id,
+					       &queue_id);
+		sh_css_copy_buffer_attr_to_spbuffer(&sh_css_sp_stage.frames.s3a_buf, queue_id,
+						    mmgr_EXCEPTION,
+						    IA_CSS_BUFFER_TYPE_3A_STATISTICS);
+	}
+	if (binary->info->sp.enable.dis) {
+		ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_DIS_STATISTICS, thread_id,
+					       &queue_id);
+		sh_css_copy_buffer_attr_to_spbuffer(&sh_css_sp_stage.frames.dvs_buf, queue_id,
+						    mmgr_EXCEPTION,
+						    IA_CSS_BUFFER_TYPE_DIS_STATISTICS);
+	}
+	ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_METADATA, thread_id, &queue_id);
+	sh_css_copy_buffer_attr_to_spbuffer(&sh_css_sp_stage.frames.metadata_buf, queue_id, mmgr_EXCEPTION, IA_CSS_BUFFER_TYPE_METADATA);
+	if (err)
+		return err;
+
+#ifdef ISP2401
+	pipe = find_pipe_by_num(sh_css_sp_group.pipe[thread_id].pipe_num);
+	if (!pipe)
+		return -EINVAL;
+
+	if (args->in_frame)
+		ia_css_get_crop_offsets(pipe, &args->in_frame->info);
+	else
+		ia_css_get_crop_offsets(pipe, &binary->in_frame_info);
+#else
+	(void)pipe; /*avoid build warning*/
+#endif
+
+	err = configure_isp_from_args(&sh_css_sp_group.pipe[thread_id],
+				      binary, args, two_ppc, sh_css_sp_stage.deinterleaved);
+	if (err)
+		return err;
+
+	initialize_isp_states(binary);
+
+	/* we do this only for preview pipe because in fill_binary_info function
+	 * we assign vf_out res to out res, but for ISP internal processing, we need
+	 * the original out res. for video pipe, it has two output pins --- out and
+	 * vf_out, so it can keep these two resolutions already. */
+	if (binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_PREVIEW &&
+	    (binary->vf_downscale_log2 > 0)) {
+		/* TODO: Remove this after preview output decimation is fixed
+		 * by configuring out&vf info fiels properly */
+		sh_css_sp_stage.frames.out[0].info.padded_width
+		<<= binary->vf_downscale_log2;
+		sh_css_sp_stage.frames.out[0].info.res.width
+		<<= binary->vf_downscale_log2;
+		sh_css_sp_stage.frames.out[0].info.res.height
+		<<= binary->vf_downscale_log2;
+	}
+	err = copy_isp_mem_if_to_ddr(binary);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int
+sp_init_stage(struct ia_css_pipeline_stage *stage,
+	      unsigned int pipe_num,
+	      bool xnr,
+	      unsigned int if_config_index,
+	      bool two_ppc)
+{
+	struct ia_css_binary *binary;
+	const struct ia_css_fw_info *firmware;
+	const struct sh_css_binary_args *args;
+	unsigned int stage_num;
+	/*
+	 * Initialiser required because of the "else" path below.
+	 * Is this a valid path ?
+	 */
+	const char *binary_name = "";
+	const struct ia_css_binary_xinfo *info = NULL;
+	/* note: the var below is made static as it is quite large;
+	   if it is not static it ends up on the stack which could
+	   cause issues for drivers
+	*/
+	static struct ia_css_binary tmp_binary;
+	const struct ia_css_blob_info *blob_info = NULL;
+	struct ia_css_isp_param_css_segments isp_mem_if;
+	/* LA: should be ia_css_data, should not contain host pointer.
+	   However, CSS/DDR pointer is not available yet.
+	   Hack is to store it in params->ddr_ptrs and then copy it late in the SP just before vmem init.
+	   TODO: Call this after CSS/DDR allocation and store that pointer.
+	   Best is to allocate it at stage creation time together with host pointer.
+	   Remove vmem from params.
+	*/
+	struct ia_css_isp_param_css_segments *mem_if = &isp_mem_if;
+
+	int err = 0;
+
+	assert(stage);
+
+	binary = stage->binary;
+	firmware = stage->firmware;
+	args = &stage->args;
+	stage_num = stage->stage_num;
+
+	if (binary) {
+		info = binary->info;
+		binary_name = (const char *)(info->blob->name);
+		blob_info = &info->blob->header.blob;
+		ia_css_init_memory_interface(mem_if, &binary->mem_params, &binary->css_params);
+	} else if (firmware) {
+		const struct ia_css_frame_info *out_infos[IA_CSS_BINARY_MAX_OUTPUT_PORTS] = {NULL};
+
+		if (args->out_frame[0])
+			out_infos[0] = &args->out_frame[0]->info;
+		info = &firmware->info.isp;
+		ia_css_binary_fill_info(info, false, false,
+					ATOMISP_INPUT_FORMAT_RAW_10,
+					args->in_frame  ? &args->in_frame->info  : NULL,
+					NULL,
+					out_infos,
+					args->out_vf_frame ? &args->out_vf_frame->info
+					: NULL,
+					&tmp_binary,
+					NULL,
+					-1, true);
+		binary = &tmp_binary;
+		binary->info = info;
+		binary_name = IA_CSS_EXT_ISP_PROG_NAME(firmware);
+		blob_info = &firmware->blob;
+		mem_if = (struct ia_css_isp_param_css_segments *)&firmware->mem_initializers;
+	} else {
+		/* SP stage */
+		assert(stage->sp_func != IA_CSS_PIPELINE_NO_FUNC);
+		/* binary and blob_info are now NULL.
+		   These will be passed to sh_css_sp_init_stage
+		   and dereferenced there, so passing a NULL
+		   pointer is no good. return an error */
+		return -EINVAL;
+	}
+
+	err = sh_css_sp_init_stage(binary,
+				   (const char *)binary_name,
+				   blob_info,
+				   args,
+				   pipe_num,
+				   stage_num,
+				   xnr,
+				   mem_if,
+				   if_config_index,
+				   two_ppc);
+	return err;
+}
+
+static void
+sp_init_sp_stage(struct ia_css_pipeline_stage *stage,
+		 unsigned int pipe_num,
+		 bool two_ppc,
+		 enum sh_css_pipe_config_override copy_ovrd,
+		 unsigned int if_config_index)
+{
+	const struct sh_css_binary_args *args = &stage->args;
+
+	assert(stage);
+	switch (stage->sp_func) {
+	case IA_CSS_PIPELINE_RAW_COPY:
+		sh_css_sp_start_raw_copy(args->out_frame[0],
+					 pipe_num, two_ppc,
+					 stage->max_input_width,
+					 copy_ovrd, if_config_index);
+		break;
+	case IA_CSS_PIPELINE_BIN_COPY:
+		assert(false); /* TBI */
+		break;
+	case IA_CSS_PIPELINE_ISYS_COPY:
+		sh_css_sp_start_isys_copy(args->out_frame[0],
+					  pipe_num, stage->max_input_width, if_config_index);
+		break;
+	case IA_CSS_PIPELINE_NO_FUNC:
+		assert(false);
+		break;
+	}
+}
+
+void
+sh_css_sp_init_pipeline(struct ia_css_pipeline *me,
+			enum ia_css_pipe_id id,
+			u8 pipe_num,
+			bool xnr,
+			bool two_ppc,
+			bool continuous,
+			bool offline,
+			unsigned int required_bds_factor,
+			enum sh_css_pipe_config_override copy_ovrd,
+			enum ia_css_input_mode input_mode,
+			const struct ia_css_metadata_config *md_config,
+			const struct ia_css_metadata_info *md_info,
+			const enum mipi_port_id port_id)
+{
+	/* Get first stage */
+	struct ia_css_pipeline_stage *stage        = NULL;
+	struct ia_css_binary	     *first_binary = NULL;
+	struct ia_css_pipe *pipe = NULL;
+	unsigned int num;
+
+	enum ia_css_pipe_id pipe_id = id;
+	unsigned int thread_id;
+	u8 if_config_index, tmp_if_config_index;
+
+	assert(me);
+
+	assert(me->stages);
+
+	first_binary = me->stages->binary;
+
+	if (input_mode == IA_CSS_INPUT_MODE_SENSOR ||
+	    input_mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+		assert(port_id < N_MIPI_PORT_ID);
+		if (port_id >= N_MIPI_PORT_ID) /* should not happen but KW does not know */
+			return; /* we should be able to return an error */
+		if_config_index  = (uint8_t)(port_id - MIPI_PORT0_ID);
+	} else if (input_mode == IA_CSS_INPUT_MODE_MEMORY) {
+		if_config_index = SH_CSS_IF_CONFIG_NOT_NEEDED;
+	} else {
+		if_config_index = 0x0;
+	}
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	memset(&sh_css_sp_group.pipe[thread_id], 0, sizeof(struct sh_css_sp_pipeline));
+
+	/* Count stages */
+	for (stage = me->stages, num = 0; stage; stage = stage->next, num++) {
+		stage->stage_num = num;
+		ia_css_debug_pipe_graph_dump_stage(stage, id);
+	}
+	me->num_stages = num;
+
+	if (first_binary) {
+		/* Init pipeline data */
+		sh_css_sp_init_group(two_ppc, first_binary->input_format,
+				     offline, if_config_index);
+	} /* if (first_binary != NULL) */
+
+	/* Signal the host immediately after start for SP_ISYS_COPY only */
+	if ((me->num_stages == 1) && me->stages &&
+	    (me->stages->sp_func == IA_CSS_PIPELINE_ISYS_COPY))
+		sh_css_sp_group.config.no_isp_sync = true;
+
+	/* Init stage data */
+	sh_css_init_host2sp_frame_data();
+
+	sh_css_sp_group.pipe[thread_id].num_stages = 0;
+	sh_css_sp_group.pipe[thread_id].pipe_id = pipe_id;
+	sh_css_sp_group.pipe[thread_id].thread_id = thread_id;
+	sh_css_sp_group.pipe[thread_id].pipe_num = pipe_num;
+	sh_css_sp_group.pipe[thread_id].num_execs = me->num_execs;
+	sh_css_sp_group.pipe[thread_id].pipe_qos_config = me->pipe_qos_config;
+	sh_css_sp_group.pipe[thread_id].required_bds_factor = required_bds_factor;
+	sh_css_sp_group.pipe[thread_id].input_system_mode
+	= (uint32_t)input_mode;
+	sh_css_sp_group.pipe[thread_id].port_id = port_id;
+	sh_css_sp_group.pipe[thread_id].dvs_frame_delay = (uint32_t)me->dvs_frame_delay;
+
+	/* TODO: next indicates from which queues parameters need to be
+		 sampled, needs checking/improvement */
+	if (ia_css_pipeline_uses_params(me)) {
+		sh_css_sp_group.pipe[thread_id].pipe_config =
+		SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS << thread_id;
+	}
+
+	/* For continuous use-cases, SP copy is responsible for sampling the
+	 * parameters */
+	if (continuous)
+		sh_css_sp_group.pipe[thread_id].pipe_config = 0;
+
+	sh_css_sp_group.pipe[thread_id].inout_port_config = me->inout_port_config;
+
+	pipe = find_pipe_by_num(pipe_num);
+	assert(pipe);
+	if (!pipe) {
+		return;
+	}
+	sh_css_sp_group.pipe[thread_id].scaler_pp_lut = sh_css_pipe_get_pp_gdc_lut(pipe);
+
+	if (md_info && md_info->size > 0) {
+		sh_css_sp_group.pipe[thread_id].metadata.width  = md_info->resolution.width;
+		sh_css_sp_group.pipe[thread_id].metadata.height = md_info->resolution.height;
+		sh_css_sp_group.pipe[thread_id].metadata.stride = md_info->stride;
+		sh_css_sp_group.pipe[thread_id].metadata.size   = md_info->size;
+		ia_css_isys_convert_stream_format_to_mipi_format(
+		    md_config->data_type, MIPI_PREDICTOR_NONE,
+		    &sh_css_sp_group.pipe[thread_id].metadata.format);
+	}
+
+	sh_css_sp_group.pipe[thread_id].output_frame_queue_id = (uint32_t)SH_CSS_INVALID_QUEUE_ID;
+	if (pipe_id != IA_CSS_PIPE_ID_COPY) {
+		ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_OUTPUT_FRAME, thread_id,
+					       (enum sh_css_queue_id *)(
+						   &sh_css_sp_group.pipe[thread_id].output_frame_queue_id));
+	}
+
+	IA_CSS_LOG("pipe_id %d port_config %08x",
+		   pipe_id, sh_css_sp_group.pipe[thread_id].inout_port_config);
+
+	for (stage = me->stages, num = 0; stage; stage = stage->next, num++) {
+		sh_css_sp_group.pipe[thread_id].num_stages++;
+		if (is_sp_stage(stage)) {
+			sp_init_sp_stage(stage, pipe_num, two_ppc,
+					 copy_ovrd, if_config_index);
+		} else {
+			if ((stage->stage_num != 0) ||
+			    SH_CSS_PIPE_PORT_CONFIG_IS_CONTINUOUS(me->inout_port_config))
+				tmp_if_config_index = SH_CSS_IF_CONFIG_NOT_NEEDED;
+			else
+				tmp_if_config_index = if_config_index;
+			sp_init_stage(stage, pipe_num,
+				      xnr, tmp_if_config_index, two_ppc);
+		}
+
+		store_sp_stage_data(pipe_id, pipe_num, num);
+	}
+	sh_css_sp_group.pipe[thread_id].pipe_config |= (uint32_t)
+		(me->acquire_isp_each_stage << IA_CSS_ACQUIRE_ISP_POS);
+	store_sp_group_data();
+}
+
+void
+sh_css_sp_uninit_pipeline(unsigned int pipe_num)
+{
+	unsigned int thread_id;
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+	/*memset(&sh_css_sp_group.pipe[thread_id], 0, sizeof(struct sh_css_sp_pipeline));*/
+	sh_css_sp_group.pipe[thread_id].num_stages = 0;
+}
+
+bool sh_css_write_host2sp_command(enum host2sp_commands host2sp_command)
+{
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	unsigned int offset = (unsigned int)offsetof(struct host_sp_communication,
+			      host2sp_command)
+			      / sizeof(int);
+	enum host2sp_commands last_cmd = host2sp_cmd_error;
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	/* Previous command must be handled by SP (by design) */
+	last_cmd = load_sp_array_uint(host_sp_com, offset);
+	if (last_cmd != host2sp_cmd_ready)
+		IA_CSS_ERROR("last host command not handled by SP(%d)", last_cmd);
+
+	store_sp_array_uint(host_sp_com, offset, host2sp_command);
+
+	return (last_cmd == host2sp_cmd_ready);
+}
+
+enum host2sp_commands
+sh_css_read_host2sp_command(void)
+{
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	unsigned int offset = (unsigned int)offsetof(struct host_sp_communication, host2sp_command)
+	/ sizeof(int);
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+	return (enum host2sp_commands)load_sp_array_uint(host_sp_com, offset);
+}
+
+/*
+ * Frame data is no longer part of the sp_stage structure but part of a
+ * separate structure. The aim is to make the sp_data struct static
+ * (it defines a pipeline) and that the dynamic (per frame) data is stored
+ * separetly.
+ *
+ * This function must be called first every where were you start constructing
+ * a new pipeline by defining one or more stages with use of variable
+ * sh_css_sp_stage. Even the special cases like accelerator and copy_frame
+ * These have a pipeline of just 1 stage.
+ */
+void
+sh_css_init_host2sp_frame_data(void)
+{
+	/* Clean table */
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+	/*
+	 * rvanimme: don't clean it to save static frame info line ref_in
+	 * ref_out, and tnr_frames. Once this static data is in a
+	 * separate data struct, this may be enable (but still, there is
+	 * no need for it)
+	 */
+}
+
+/*
+ * @brief Update the offline frame information in host_sp_communication.
+ * Refer to "sh_css_sp.h" for more details.
+ */
+void
+sh_css_update_host2sp_offline_frame(
+    unsigned int frame_num,
+    struct ia_css_frame *frame,
+    struct ia_css_metadata *metadata)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	assert(frame_num < NUM_CONTINUOUS_FRAMES);
+
+	/* Write new frame data into SP DMEM */
+	HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_offline_frames)
+		 / sizeof(int);
+	offset += frame_num;
+	store_sp_array_uint(host_sp_com, offset, frame ? frame->data : 0);
+
+	/* Write metadata buffer into SP DMEM */
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_offline_metadata)
+		 / sizeof(int);
+	offset += frame_num;
+	store_sp_array_uint(host_sp_com, offset, metadata ? metadata->address : 0);
+}
+
+/*
+ * @brief Update the mipi frame information in host_sp_communication.
+ * Refer to "sh_css_sp.h" for more details.
+ */
+void
+sh_css_update_host2sp_mipi_frame(
+    unsigned int frame_num,
+    struct ia_css_frame *frame)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	/* MIPI buffers are dedicated to port, so now there are more of them. */
+	assert(frame_num < (N_CSI_PORTS * NUM_MIPI_FRAMES_PER_STREAM));
+
+	/* Write new frame data into SP DMEM */
+	HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_mipi_frames)
+		 / sizeof(int);
+	offset += frame_num;
+
+	store_sp_array_uint(host_sp_com, offset,
+			    frame ? frame->data : 0);
+}
+
+/*
+ * @brief Update the mipi metadata information in host_sp_communication.
+ * Refer to "sh_css_sp.h" for more details.
+ */
+void
+sh_css_update_host2sp_mipi_metadata(
+    unsigned int frame_num,
+    struct ia_css_metadata *metadata)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int o;
+
+	/* MIPI buffers are dedicated to port, so now there are more of them. */
+	assert(frame_num < (N_CSI_PORTS * NUM_MIPI_FRAMES_PER_STREAM));
+
+	/* Write new frame data into SP DMEM */
+	HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	o = offsetof(struct host_sp_communication, host2sp_mipi_metadata)
+	    / sizeof(int);
+	o += frame_num;
+	store_sp_array_uint(host_sp_com, o,
+			    metadata ? metadata->address : 0);
+}
+
+void
+sh_css_update_host2sp_num_mipi_frames(unsigned int num_frames)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	/* Write new frame data into SP DMEM */
+	HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_num_mipi_frames)
+		 / sizeof(int);
+
+	store_sp_array_uint(host_sp_com, offset, num_frames);
+}
+
+void
+sh_css_update_host2sp_cont_num_raw_frames(unsigned int num_frames,
+	bool set_avail)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int extra_num_frames, avail_num_frames;
+	unsigned int offset, offset_extra;
+
+	/* Write new frame data into SP DMEM */
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_host_sp_com = fw->info.sp.host_sp_com;
+	if (set_avail) {
+		offset = (unsigned int)offsetof(struct host_sp_communication,
+						host2sp_cont_avail_num_raw_frames)
+			 / sizeof(int);
+		avail_num_frames = load_sp_array_uint(host_sp_com, offset);
+		extra_num_frames = num_frames - avail_num_frames;
+		offset_extra = (unsigned int)offsetof(struct host_sp_communication,
+						      host2sp_cont_extra_num_raw_frames)
+			       / sizeof(int);
+		store_sp_array_uint(host_sp_com, offset_extra, extra_num_frames);
+	} else
+		offset = (unsigned int)offsetof(struct host_sp_communication,
+						host2sp_cont_target_num_raw_frames)
+			 / sizeof(int);
+
+	store_sp_array_uint(host_sp_com, offset, num_frames);
+}
+
+void
+sh_css_event_init_irq_mask(void)
+{
+	int i;
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	unsigned int offset;
+	struct sh_css_event_irq_mask event_irq_mask_init;
+
+	event_irq_mask_init.or_mask  = IA_CSS_EVENT_TYPE_ALL;
+	event_irq_mask_init.and_mask = IA_CSS_EVENT_TYPE_NONE;
+	(void)HIVE_ADDR_host_sp_com; /* Suppress warnings in CRUN */
+
+	assert(sizeof(event_irq_mask_init) % HRT_BUS_BYTES == 0);
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++) {
+		offset = (unsigned int)offsetof(struct host_sp_communication,
+						host2sp_event_irq_mask[i]);
+		assert(offset % HRT_BUS_BYTES == 0);
+		sp_dmem_store(SP0_ID,
+			      (unsigned int)sp_address_of(host_sp_com) + offset,
+			      &event_irq_mask_init, sizeof(event_irq_mask_init));
+	}
+}
+
+int
+ia_css_pipe_set_irq_mask(struct ia_css_pipe *pipe,
+			 unsigned int or_mask,
+			 unsigned int and_mask)
+{
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	unsigned int offset;
+	struct sh_css_event_irq_mask event_irq_mask;
+	unsigned int pipe_num;
+
+	assert(pipe);
+
+	assert(IA_CSS_PIPE_ID_NUM == NR_OF_PIPELINES);
+	/* Linux kernel does not have UINT16_MAX
+	 * Therefore decided to comment out these 2 asserts for Linux
+	 * Alternatives that were not chosen:
+	 * - add a conditional #define for UINT16_MAX
+	 * - compare with (uint16_t)~0 or 0xffff
+	 * - different assert for Linux and Windows
+	 */
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	IA_CSS_LOG("or_mask=%x, and_mask=%x", or_mask, and_mask);
+	event_irq_mask.or_mask  = (uint16_t)or_mask;
+	event_irq_mask.and_mask = (uint16_t)and_mask;
+
+	pipe_num = ia_css_pipe_get_pipe_num(pipe);
+	if (pipe_num >= IA_CSS_PIPE_ID_NUM)
+		return -EINVAL;
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_event_irq_mask[pipe_num]);
+	assert(offset % HRT_BUS_BYTES == 0);
+	sp_dmem_store(SP0_ID,
+		      (unsigned int)sp_address_of(host_sp_com) + offset,
+		      &event_irq_mask, sizeof(event_irq_mask));
+
+	return 0;
+}
+
+int
+ia_css_event_get_irq_mask(const struct ia_css_pipe *pipe,
+			  unsigned int *or_mask,
+			  unsigned int *and_mask)
+{
+	unsigned int HIVE_ADDR_host_sp_com = sh_css_sp_fw.info.sp.host_sp_com;
+	unsigned int offset;
+	struct sh_css_event_irq_mask event_irq_mask;
+	unsigned int pipe_num;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	IA_CSS_ENTER_LEAVE("");
+
+	assert(pipe);
+	assert(IA_CSS_PIPE_ID_NUM == NR_OF_PIPELINES);
+
+	pipe_num = ia_css_pipe_get_pipe_num(pipe);
+	if (pipe_num >= IA_CSS_PIPE_ID_NUM)
+		return -EINVAL;
+	offset = (unsigned int)offsetof(struct host_sp_communication,
+					host2sp_event_irq_mask[pipe_num]);
+	assert(offset % HRT_BUS_BYTES == 0);
+	sp_dmem_load(SP0_ID,
+		     (unsigned int)sp_address_of(host_sp_com) + offset,
+		     &event_irq_mask, sizeof(event_irq_mask));
+
+	if (or_mask)
+		*or_mask = event_irq_mask.or_mask;
+
+	if (and_mask)
+		*and_mask = event_irq_mask.and_mask;
+
+	return 0;
+}
+
+void
+sh_css_sp_set_sp_running(bool flag)
+{
+	sp_running = flag;
+}
+
+bool
+sh_css_sp_is_running(void)
+{
+	return sp_running;
+}
+
+void
+sh_css_sp_start_isp(void)
+{
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_sp_sw_state;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_sp_sw_state = fw->info.sp.sw_state;
+
+	if (sp_running)
+		return;
+
+	(void)HIVE_ADDR_sp_sw_state; /* Suppres warnings in CRUN */
+
+	/* no longer here, sp started immediately */
+	/*ia_css_debug_pipe_graph_dump_epilogue();*/
+
+	store_sp_group_data();
+	store_sp_per_frame_data(fw);
+
+	sp_dmem_store_uint32(SP0_ID,
+			     (unsigned int)sp_address_of(sp_sw_state),
+			     (uint32_t)(IA_CSS_SP_SW_TERMINATED));
+
+	/* Note 1: The sp_start_isp function contains a wait till
+	 * the input network is configured by the SP.
+	 * Note 2: Not all SP binaries supports host2sp_commands.
+	 * In case a binary does support it, the host2sp_command
+	 * will have status cmd_ready after return of the function
+	 * sh_css_hrt_sp_start_isp. There is no race-condition here
+	 * because only after the process_frame command has been
+	 * received, the SP starts configuring the input network.
+	 */
+
+	/* we need to set sp_running before we call ia_css_mmu_invalidate_cache
+	 * as ia_css_mmu_invalidate_cache checks on sp_running to
+	 * avoid that it accesses dmem while the SP is not powered
+	 */
+	sp_running = true;
+	ia_css_mmu_invalidate_cache();
+	/* Invalidate all MMU caches */
+	mmu_invalidate_cache_all();
+
+	ia_css_spctrl_start(SP0_ID);
+}
+
+bool
+ia_css_isp_has_started(void)
+{
+	const struct ia_css_fw_info *fw = &sh_css_sp_fw;
+	unsigned int HIVE_ADDR_ia_css_ispctrl_sp_isp_started = fw->info.sp.isp_started;
+	(void)HIVE_ADDR_ia_css_ispctrl_sp_isp_started; /* Suppres warnings in CRUN */
+
+	return (bool)load_sp_uint(ia_css_ispctrl_sp_isp_started);
+}
+
+/*
+ * @brief Initialize the DMA software-mask in the debug mode.
+ * Refer to "sh_css_sp.h" for more details.
+ */
+bool
+sh_css_sp_init_dma_sw_reg(int dma_id)
+{
+	int i;
+
+	/* enable all the DMA channels */
+	for (i = 0; i < N_DMA_CHANNEL_ID; i++) {
+		/* enable the writing request */
+		sh_css_sp_set_dma_sw_reg(dma_id,
+					 i,
+					 0,
+					 true);
+		/* enable the reading request */
+		sh_css_sp_set_dma_sw_reg(dma_id,
+					 i,
+					 1,
+					 true);
+	}
+
+	return true;
+}
+
+/*
+ * @brief Set the DMA software-mask in the debug mode.
+ * Refer to "sh_css_sp.h" for more details.
+ */
+bool
+sh_css_sp_set_dma_sw_reg(int dma_id,
+			 int channel_id,
+			 int request_type,
+			 bool enable)
+{
+	u32 sw_reg;
+	u32 bit_val;
+	u32 bit_offset;
+	u32 bit_mask;
+
+	(void)dma_id;
+
+	assert(channel_id >= 0 && channel_id < N_DMA_CHANNEL_ID);
+	assert(request_type >= 0);
+
+	/* get the software-mask */
+	sw_reg =
+	    sh_css_sp_group.debug.dma_sw_reg;
+
+	/* get the offest of the target bit */
+	bit_offset = (8 * request_type) + channel_id;
+
+	/* clear the value of the target bit */
+	bit_mask = ~(1 << bit_offset);
+	sw_reg &= bit_mask;
+
+	/* set the value of the bit for the DMA channel */
+	bit_val = enable ? 1 : 0;
+	bit_val <<= bit_offset;
+	sw_reg |= bit_val;
+
+	/* update the software status of DMA channels */
+	sh_css_sp_group.debug.dma_sw_reg = sw_reg;
+
+	return true;
+}
+
+void
+sh_css_sp_reset_global_vars(void)
+{
+	memset(&sh_css_sp_group, 0, sizeof(struct sh_css_sp_group));
+	memset(&sh_css_sp_stage, 0, sizeof(struct sh_css_sp_stage));
+	memset(&sh_css_isp_stage, 0, sizeof(struct sh_css_isp_stage));
+	memset(&sh_css_sp_output, 0, sizeof(struct sh_css_sp_output));
+	memset(&per_frame_data, 0, sizeof(struct sh_css_sp_per_frame_data));
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_sp.h b/drivers/staging/media/atomisp/pci/sh_css_sp.h
new file mode 100644
index 000000000..f69a79b0b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_sp.h
@@ -0,0 +1,240 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_SP_H_
+#define _SH_CSS_SP_H_
+
+#include <system_global.h>
+#include <type_support.h>
+#if !defined(ISP2401)
+#include "input_formatter.h"
+#endif
+
+#include "ia_css_binary.h"
+#include "ia_css_types.h"
+#include "ia_css_pipeline.h"
+
+/* Function to initialize the data and bss section descr of the binary */
+void
+sh_css_sp_store_init_dmem(const struct ia_css_fw_info *fw);
+
+void
+store_sp_stage_data(enum ia_css_pipe_id id, unsigned int pipe_num,
+		    unsigned int stage);
+
+void
+sh_css_stage_write_binary_info(struct ia_css_binary_info *info);
+
+void
+store_sp_group_data(void);
+
+/* Start binary (jpeg) copy on the SP */
+void
+sh_css_sp_start_binary_copy(unsigned int pipe_num,
+			    struct ia_css_frame *out_frame,
+			    unsigned int two_ppc);
+
+unsigned int
+sh_css_sp_get_binary_copy_size(void);
+
+/* Return the value of a SW interrupt */
+unsigned int
+sh_css_sp_get_sw_interrupt_value(unsigned int irq);
+
+void
+sh_css_sp_init_pipeline(struct ia_css_pipeline *me,
+			enum ia_css_pipe_id id,
+			u8 pipe_num,
+			bool xnr,
+			bool two_ppc,
+			bool continuous,
+			bool offline,
+			unsigned int required_bds_factor,
+			enum sh_css_pipe_config_override copy_ovrd,
+			enum ia_css_input_mode input_mode,
+			const struct ia_css_metadata_config *md_config,
+			const struct ia_css_metadata_info *md_info,
+			const enum mipi_port_id port_id);
+
+void
+sh_css_sp_uninit_pipeline(unsigned int pipe_num);
+
+bool sh_css_write_host2sp_command(enum host2sp_commands host2sp_command);
+
+enum host2sp_commands
+sh_css_read_host2sp_command(void);
+
+void
+sh_css_init_host2sp_frame_data(void);
+
+/**
+ * @brief Update the offline frame information in host_sp_communication.
+ *
+ * @param[in] frame_num The offline frame number.
+ * @param[in] frame The pointer to the offline frame.
+ */
+void
+sh_css_update_host2sp_offline_frame(
+    unsigned int frame_num,
+    struct ia_css_frame *frame,
+    struct ia_css_metadata *metadata);
+
+/**
+ * @brief Update the mipi frame information in host_sp_communication.
+ *
+ * @param[in] frame_num The mipi frame number.
+ * @param[in] frame The pointer to the mipi frame.
+ */
+void
+sh_css_update_host2sp_mipi_frame(
+    unsigned int frame_num,
+    struct ia_css_frame *frame);
+
+/**
+ * @brief Update the mipi metadata information in host_sp_communication.
+ *
+ * @param[in] frame_num The mipi frame number.
+ * @param[in] metadata The pointer to the mipi metadata.
+ */
+void
+sh_css_update_host2sp_mipi_metadata(
+    unsigned int frame_num,
+    struct ia_css_metadata *metadata);
+
+/**
+ * @brief Update the nr of mipi frames to use in host_sp_communication.
+ *
+ * @param[in] num_frames The number of mipi frames to use.
+ */
+void
+sh_css_update_host2sp_num_mipi_frames(unsigned int num_frames);
+
+/**
+ * @brief Update the nr of offline frames to use in host_sp_communication.
+ *
+ * @param[in] num_frames The number of raw frames to use.
+ */
+void
+sh_css_update_host2sp_cont_num_raw_frames(unsigned int num_frames,
+	bool set_avail);
+
+void
+sh_css_event_init_irq_mask(void);
+
+void
+sh_css_sp_start_isp(void);
+
+void
+sh_css_sp_set_sp_running(bool flag);
+
+bool
+sh_css_sp_is_running(void);
+
+#if SP_DEBUG != SP_DEBUG_NONE
+
+void
+sh_css_sp_get_debug_state(struct sh_css_sp_debug_state *state);
+
+#endif
+
+#if !defined(ISP2401)
+void
+sh_css_sp_set_if_configs(
+    const input_formatter_cfg_t	*config_a,
+    const input_formatter_cfg_t	*config_b,
+    const uint8_t		if_config_index);
+#endif
+
+void
+sh_css_sp_program_input_circuit(int fmt_type,
+				int ch_id,
+				enum ia_css_input_mode input_mode);
+
+void
+sh_css_sp_configure_sync_gen(int width,
+			     int height,
+			     int hblank_cycles,
+			     int vblank_cycles);
+
+void
+sh_css_sp_configure_tpg(int x_mask,
+			int y_mask,
+			int x_delta,
+			int y_delta,
+			int xy_mask);
+
+void
+sh_css_sp_configure_prbs(int seed);
+
+void
+sh_css_sp_configure_enable_raw_pool_locking(bool lock_all);
+
+void
+sh_css_sp_enable_isys_event_queue(bool enable);
+
+void
+sh_css_sp_set_disable_continuous_viewfinder(bool flag);
+
+void
+sh_css_sp_reset_global_vars(void);
+
+/**
+ * @brief Initialize the DMA software-mask in the debug mode.
+ * This API should be ONLY called in the debugging mode.
+ * And it should be always called before the first call of
+ * "sh_css_set_dma_sw_reg(...)".
+ *
+ * @param[in]	dma_id		The ID of the target DMA.
+ *
+ * @return
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool
+sh_css_sp_init_dma_sw_reg(int dma_id);
+
+/**
+ * @brief Set the DMA software-mask in the debug mode.
+ * This API should be ONLYL called in the debugging mode. Must
+ * call "sh_css_set_dma_sw_reg(...)" before this
+ * API is called for the first time.
+ *
+ * @param[in]	dma_id		The ID of the target DMA.
+ * @param[in]	channel_id	The ID of the target DMA channel.
+ * @param[in]	request_type	The type of the DMA request.
+ *				For example:
+ *				- "0" indicates the writing request.
+ *				- "1" indicates the reading request.
+ *
+ * @param[in]	enable		If it is "true", the target DMA
+ *				channel is enabled in the software.
+ *				Otherwise, the target DMA channel
+ *				is disabled in the software.
+ *
+ * @return
+ *	- true, if it is successful.
+ *	- false, otherwise.
+ */
+bool
+sh_css_sp_set_dma_sw_reg(int dma_id,
+			 int channel_id,
+			 int request_type,
+			 bool enable);
+
+extern struct sh_css_sp_group sh_css_sp_group;
+extern struct sh_css_sp_stage sh_css_sp_stage;
+extern struct sh_css_isp_stage sh_css_isp_stage;
+
+#endif /* _SH_CSS_SP_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_stream_format.c b/drivers/staging/media/atomisp/pci/sh_css_stream_format.c
new file mode 100644
index 000000000..a798f0537
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_stream_format.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "sh_css_stream_format.h"
+#include <ia_css_stream_format.h>
+
+unsigned int sh_css_stream_format_2_bits_per_subpixel(
+    enum atomisp_input_format format)
+{
+	unsigned int rval;
+
+	switch (format) {
+	case ATOMISP_INPUT_FORMAT_RGB_444:
+		rval = 4;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_555:
+		rval = 5;
+		break;
+	case ATOMISP_INPUT_FORMAT_RGB_565:
+	case ATOMISP_INPUT_FORMAT_RGB_666:
+	case ATOMISP_INPUT_FORMAT_RAW_6:
+		rval = 6;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_7:
+		rval = 7;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY:
+	case ATOMISP_INPUT_FORMAT_YUV420_8:
+	case ATOMISP_INPUT_FORMAT_YUV422_8:
+	case ATOMISP_INPUT_FORMAT_RGB_888:
+	case ATOMISP_INPUT_FORMAT_RAW_8:
+	case ATOMISP_INPUT_FORMAT_BINARY_8:
+	case ATOMISP_INPUT_FORMAT_USER_DEF1:
+	case ATOMISP_INPUT_FORMAT_USER_DEF2:
+	case ATOMISP_INPUT_FORMAT_USER_DEF3:
+	case ATOMISP_INPUT_FORMAT_USER_DEF4:
+	case ATOMISP_INPUT_FORMAT_USER_DEF5:
+	case ATOMISP_INPUT_FORMAT_USER_DEF6:
+	case ATOMISP_INPUT_FORMAT_USER_DEF7:
+	case ATOMISP_INPUT_FORMAT_USER_DEF8:
+		rval = 8;
+		break;
+	case ATOMISP_INPUT_FORMAT_YUV420_10:
+	case ATOMISP_INPUT_FORMAT_YUV422_10:
+	case ATOMISP_INPUT_FORMAT_RAW_10:
+		rval = 10;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_12:
+		rval = 12;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_14:
+		rval = 14;
+		break;
+	case ATOMISP_INPUT_FORMAT_RAW_16:
+	case ATOMISP_INPUT_FORMAT_YUV420_16:
+	case ATOMISP_INPUT_FORMAT_YUV422_16:
+		rval = 16;
+		break;
+	default:
+		rval = 0;
+		break;
+	}
+
+	return rval;
+}
diff --git a/drivers/staging/media/atomisp/pci/sh_css_stream_format.h b/drivers/staging/media/atomisp/pci/sh_css_stream_format.h
new file mode 100644
index 000000000..84b794214
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_stream_format.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_STREAM_FORMAT_H
+#define __SH_CSS_STREAM_FORMAT_H
+
+#include <ia_css_stream_format.h>
+
+unsigned int sh_css_stream_format_2_bits_per_subpixel(
+    enum atomisp_input_format format);
+
+#endif /* __SH_CSS_STREAM_FORMAT_H */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_struct.h b/drivers/staging/media/atomisp/pci/sh_css_struct.h
new file mode 100644
index 000000000..eb8960eba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_struct.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SH_CSS_STRUCT_H
+#define __SH_CSS_STRUCT_H
+
+/* This header files contains the definition of the
+   sh_css struct and friends; locigally the file would
+   probably be called sh_css.h after the pattern
+   <type>.h but sh_css.h is the predecesssor of ia_css.h
+   so this could cause confusion; hence the _struct
+   in the filename
+*/
+
+#include <type_support.h>
+#include <system_local.h>
+#include "ia_css_pipeline.h"
+#include "ia_css_pipe_public.h"
+#include "ia_css_frame_public.h"
+#include "ia_css_queue.h"
+#include "ia_css_irq.h"
+
+struct sh_css {
+	struct ia_css_pipe            *active_pipes[IA_CSS_PIPELINE_NUM_MAX];
+	/* All of the pipes created at any point of time. At this moment there can
+	 * be no more than MAX_SP_THREADS of them because pipe_num is reused as SP
+	 * thread_id to which a pipe's pipeline is associated. At a later point, if
+	 * we support more pipe objects, we should add test code to test that
+	 * possibility. Also, active_pipes[] should be able to hold only
+	 * SH_CSS_MAX_SP_THREADS objects. Anything else is misleading. */
+	struct ia_css_pipe            *all_pipes[IA_CSS_PIPELINE_NUM_MAX];
+	void *(*malloc)(size_t bytes, bool zero_mem);
+	void (*free)(void *ptr);
+	void (*flush)(struct ia_css_acc_fw *fw);
+
+/* ISP2401 */
+	void *(*malloc_ex)(size_t bytes, bool zero_mem, const char *caller_func,
+			   int caller_line);
+	void (*free_ex)(void *ptr, const char *caller_func, int caller_line);
+
+/* ISP2400 */
+	bool stop_copy_preview;
+
+	bool                           check_system_idle;
+	unsigned int                   num_cont_raw_frames;
+	unsigned int                   num_mipi_frames[N_CSI_PORTS];
+	struct ia_css_frame
+		*mipi_frames[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	struct ia_css_metadata
+		*mipi_metadata[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	unsigned int
+	mipi_sizes_for_check[N_CSI_PORTS][IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT];
+	unsigned int                   mipi_frame_size[N_CSI_PORTS];
+	ia_css_ptr                   sp_bin_addr;
+	hrt_data                       page_table_base_index;
+
+	unsigned int
+	size_mem_words; /* \deprecated{Use ia_css_mipi_buffer_config instead.}*/
+	enum ia_css_irq_type           irq_type;
+	unsigned int                   pipe_counter;
+
+	unsigned int		type;	/* 2400 or 2401 for now */
+};
+
+#define IPU_2400		1
+#define IPU_2401		2
+
+#define IS_2400()		(my_css.type == IPU_2400)
+#define IS_2401()		(my_css.type == IPU_2401)
+
+extern struct sh_css my_css;
+
+#endif /* __SH_CSS_STRUCT_H */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_uds.h b/drivers/staging/media/atomisp/pci/sh_css_uds.h
new file mode 100644
index 000000000..8d9c5c92b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_uds.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _SH_CSS_UDS_H_
+#define _SH_CSS_UDS_H_
+
+#include <type_support.h>
+
+#define SIZE_OF_SH_CSS_UDS_INFO_IN_BITS (4 * 16)
+#define SIZE_OF_SH_CSS_CROP_POS_IN_BITS (2 * 16)
+
+/* Uds types, used in pipeline_global.h and sh_css_internal.h */
+
+struct sh_css_uds_info {
+	u16 curr_dx;
+	u16 curr_dy;
+	u16 xc;
+	u16 yc;
+};
+
+struct sh_css_crop_pos {
+	u16 x;
+	u16 y;
+};
+
+#endif /* _SH_CSS_UDS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/sh_css_version.c b/drivers/staging/media/atomisp/pci/sh_css_version.c
new file mode 100644
index 000000000..f5ff8ca66
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/sh_css_version.c
@@ -0,0 +1,40 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "../../include/linux/atomisp.h"
+#include "../../include/linux/atomisp_platform.h"
+#include "ia_css_version.h"
+#include "ia_css_version_data.h"
+#include "ia_css_err.h"
+#include "sh_css_firmware.h"
+
+int
+ia_css_get_version(char *version, int max_size)
+{
+	char *css_version;
+
+	if (!IS_ISP2401)
+		css_version = ISP2400_CSS_VERSION_STRING;
+	else
+		css_version = ISP2401_CSS_VERSION_STRING;
+
+	if (max_size <= (int)strlen(css_version) + (int)strlen(sh_css_get_fw_version()) + 5)
+		return -EINVAL;
+	strscpy(version, css_version, max_size);
+	strcat(version, "FW:");
+	strcat(version, sh_css_get_fw_version());
+	strcat(version, "; ");
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/str2mem_defs.h b/drivers/staging/media/atomisp/pci/str2mem_defs.h
new file mode 100644
index 000000000..e8cb456ac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/str2mem_defs.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _ST2MEM_DEFS_H
+#define _ST2MEM_DEFS_H
+
+#define _STR2MEM_CRUN_BIT               0x100000
+#define _STR2MEM_CMD_BITS               0x0F0000
+#define _STR2MEM_COUNT_BITS             0x00FFFF
+
+#define _STR2MEM_BLOCKS_CMD             0xA0000
+#define _STR2MEM_PACKETS_CMD            0xB0000
+#define _STR2MEM_BYTES_CMD              0xC0000
+#define _STR2MEM_BYTES_FROM_PACKET_CMD  0xD0000
+
+#define _STR2MEM_SOFT_RESET_REG_ID                   0
+#define _STR2MEM_INPUT_ENDIANNESS_REG_ID             1
+#define _STR2MEM_OUTPUT_ENDIANNESS_REG_ID            2
+#define _STR2MEM_BIT_SWAPPING_REG_ID                 3
+#define _STR2MEM_BLOCK_SYNC_LEVEL_REG_ID             4
+#define _STR2MEM_PACKET_SYNC_LEVEL_REG_ID            5
+#define _STR2MEM_READ_POST_WRITE_SYNC_ENABLE_REG_ID  6
+#define _STR2MEM_DUAL_BYTE_INPUTS_ENABLED_REG_ID     7
+#define _STR2MEM_EN_STAT_UPDATE_ID                   8
+
+#define _STR2MEM_REG_ALIGN      4
+
+#endif /* _ST2MEM_DEFS_H */
diff --git a/drivers/staging/media/atomisp/pci/streaming_to_mipi_defs.h b/drivers/staging/media/atomisp/pci/streaming_to_mipi_defs.h
new file mode 100644
index 000000000..e0e3a6a66
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/streaming_to_mipi_defs.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _streaming_to_mipi_defs_h
+#define _streaming_to_mipi_defs_h
+
+#define HIVE_STR_TO_MIPI_VALID_A_BIT 0
+#define HIVE_STR_TO_MIPI_VALID_B_BIT 1
+#define HIVE_STR_TO_MIPI_SOL_BIT     2
+#define HIVE_STR_TO_MIPI_EOL_BIT     3
+#define HIVE_STR_TO_MIPI_SOF_BIT     4
+#define HIVE_STR_TO_MIPI_EOF_BIT     5
+#define HIVE_STR_TO_MIPI_CH_ID_LSB   6
+
+#define HIVE_STR_TO_MIPI_DATA_A_LSB  (HIVE_STR_TO_MIPI_VALID_B_BIT + 1)
+
+#endif /* _streaming_to_mipi_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/system_global.h b/drivers/staging/media/atomisp/pci/system_global.h
new file mode 100644
index 000000000..060b92402
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/system_global.h
@@ -0,0 +1,380 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *    (c) 2020 Mauro Carvalho Chehab <mchehab+huawei@kernel.org>
+ */
+
+#ifndef __SYSTEM_GLOBAL_H_INCLUDED__
+#define __SYSTEM_GLOBAL_H_INCLUDED__
+
+/*
+ * Create a list of HAS and IS properties that defines the system
+ * Those are common for both ISP2400 and ISP2401
+ *
+ * The configuration assumes the following
+ * - The system is hetereogeneous; Multiple cells and devices classes
+ * - The cell and device instances are homogeneous, each device type
+ *   belongs to the same class
+ * - Device instances supporting a subset of the class capabilities are
+ *   allowed
+ *
+ * We could manage different device classes through the enumerated
+ * lists (C) or the use of classes (C++), but that is presently not
+ * fully supported
+ *
+ * N.B. the 3 input formatters are of 2 different classess
+ */
+
+#define DMA_DDR_TO_VAMEM_WORKAROUND
+#define DMA_DDR_TO_HMEM_WORKAROUND
+
+/*
+ * The longest allowed (uninteruptible) bus transfer, does not
+ * take stalling into account
+ */
+#define HIVE_ISP_MAX_BURST_LENGTH	1024
+
+/*
+ * Maximum allowed burst length in words for the ISP DMA
+ * This value is set to 2 to prevent the ISP DMA from blocking
+ * the bus for too long; as the input system can only buffer
+ * 2 lines on Moorefield and Cherrytrail, the input system buffers
+ * may overflow if blocked for too long (BZ 2726).
+ */
+#define ISP2400_DMA_MAX_BURST_LENGTH	128
+#define ISP2401_DMA_MAX_BURST_LENGTH	2
+
+#include <hive_isp_css_defs.h>
+#include <type_support.h>
+
+/* This interface is deprecated */
+#include "hive_types.h"
+
+/*
+ * Semi global. "HRT" is accessible from SP, but the HRT types do not fully apply
+ */
+#define HRT_VADDRESS_WIDTH	32
+
+#define SIZEOF_HRT_REG		(HRT_DATA_WIDTH >> 3)
+#define HIVE_ISP_CTRL_DATA_BYTES (HIVE_ISP_CTRL_DATA_WIDTH / 8)
+
+/* The main bus connecting all devices */
+#define HRT_BUS_WIDTH		HIVE_ISP_CTRL_DATA_WIDTH
+#define HRT_BUS_BYTES		HIVE_ISP_CTRL_DATA_BYTES
+
+typedef u32			hrt_bus_align_t;
+
+/*
+ * Enumerate the devices, device access through the API is by ID,
+ * through the DLI by address. The enumerator terminators are used
+ * to size the wiring arrays and as an exception value.
+ */
+typedef enum {
+	DDR0_ID = 0,
+	N_DDR_ID
+} ddr_ID_t;
+
+typedef enum {
+	ISP0_ID = 0,
+	N_ISP_ID
+} isp_ID_t;
+
+typedef enum {
+	SP0_ID = 0,
+	N_SP_ID
+} sp_ID_t;
+
+typedef enum {
+	MMU0_ID = 0,
+	MMU1_ID,
+	N_MMU_ID
+} mmu_ID_t;
+
+typedef enum {
+	DMA0_ID = 0,
+	N_DMA_ID
+} dma_ID_t;
+
+typedef enum {
+	GDC0_ID = 0,
+	GDC1_ID,
+	N_GDC_ID
+} gdc_ID_t;
+
+/* this extra define is needed because we want to use it also
+   in the preprocessor, and that doesn't work with enums.
+ */
+#define N_GDC_ID_CPP 2
+
+typedef enum {
+	VAMEM0_ID = 0,
+	VAMEM1_ID,
+	VAMEM2_ID,
+	N_VAMEM_ID
+} vamem_ID_t;
+
+typedef enum {
+	BAMEM0_ID = 0,
+	N_BAMEM_ID
+} bamem_ID_t;
+
+typedef enum {
+	HMEM0_ID = 0,
+	N_HMEM_ID
+} hmem_ID_t;
+
+typedef enum {
+	IRQ0_ID = 0,	/* GP IRQ block */
+	IRQ1_ID,	/* Input formatter */
+	IRQ2_ID,	/* input system */
+	IRQ3_ID,	/* input selector */
+	N_IRQ_ID
+} irq_ID_t;
+
+typedef enum {
+	FIFO_MONITOR0_ID = 0,
+	N_FIFO_MONITOR_ID
+} fifo_monitor_ID_t;
+
+typedef enum {
+	GP_DEVICE0_ID = 0,
+	N_GP_DEVICE_ID
+} gp_device_ID_t;
+
+typedef enum {
+	GP_TIMER0_ID = 0,
+	GP_TIMER1_ID,
+	GP_TIMER2_ID,
+	GP_TIMER3_ID,
+	GP_TIMER4_ID,
+	GP_TIMER5_ID,
+	GP_TIMER6_ID,
+	GP_TIMER7_ID,
+	N_GP_TIMER_ID
+} gp_timer_ID_t;
+
+typedef enum {
+	GPIO0_ID = 0,
+	N_GPIO_ID
+} gpio_ID_t;
+
+typedef enum {
+	TIMED_CTRL0_ID = 0,
+	N_TIMED_CTRL_ID
+} timed_ctrl_ID_t;
+
+typedef enum {
+	INPUT_FORMATTER0_ID = 0,
+	INPUT_FORMATTER1_ID,
+	INPUT_FORMATTER2_ID,
+	INPUT_FORMATTER3_ID,
+	N_INPUT_FORMATTER_ID
+} input_formatter_ID_t;
+
+/* The IF RST is outside the IF */
+#define INPUT_FORMATTER0_SRST_OFFSET	0x0824
+#define INPUT_FORMATTER1_SRST_OFFSET	0x0624
+#define INPUT_FORMATTER2_SRST_OFFSET	0x0424
+#define INPUT_FORMATTER3_SRST_OFFSET	0x0224
+
+#define INPUT_FORMATTER0_SRST_MASK		0x0001
+#define INPUT_FORMATTER1_SRST_MASK		0x0002
+#define INPUT_FORMATTER2_SRST_MASK		0x0004
+#define INPUT_FORMATTER3_SRST_MASK		0x0008
+
+typedef enum {
+	INPUT_SYSTEM0_ID = 0,
+	N_INPUT_SYSTEM_ID
+} input_system_ID_t;
+
+typedef enum {
+	RX0_ID = 0,
+	N_RX_ID
+} rx_ID_t;
+
+enum mipi_port_id {
+	MIPI_PORT0_ID = 0,
+	MIPI_PORT1_ID,
+	MIPI_PORT2_ID,
+	N_MIPI_PORT_ID
+};
+
+#define	N_RX_CHANNEL_ID		4
+
+/* Generic port enumeration with an internal port type ID */
+typedef enum {
+	CSI_PORT0_ID = 0,
+	CSI_PORT1_ID,
+	CSI_PORT2_ID,
+	TPG_PORT0_ID,
+	PRBS_PORT0_ID,
+	FIFO_PORT0_ID,
+	MEMORY_PORT0_ID,
+	N_INPUT_PORT_ID
+} input_port_ID_t;
+
+typedef enum {
+	CAPTURE_UNIT0_ID = 0,
+	CAPTURE_UNIT1_ID,
+	CAPTURE_UNIT2_ID,
+	ACQUISITION_UNIT0_ID,
+	DMA_UNIT0_ID,
+	CTRL_UNIT0_ID,
+	GPREGS_UNIT0_ID,
+	FIFO_UNIT0_ID,
+	IRQ_UNIT0_ID,
+	N_SUB_SYSTEM_ID
+} sub_system_ID_t;
+
+#define	N_CAPTURE_UNIT_ID		3
+#define	N_ACQUISITION_UNIT_ID		1
+#define	N_CTRL_UNIT_ID			1
+
+
+enum ia_css_isp_memories {
+	IA_CSS_ISP_PMEM0 = 0,
+	IA_CSS_ISP_DMEM0,
+	IA_CSS_ISP_VMEM0,
+	IA_CSS_ISP_VAMEM0,
+	IA_CSS_ISP_VAMEM1,
+	IA_CSS_ISP_VAMEM2,
+	IA_CSS_ISP_HMEM0,
+	IA_CSS_SP_DMEM0,
+	IA_CSS_DDR,
+	N_IA_CSS_MEMORIES
+};
+
+#define IA_CSS_NUM_MEMORIES 9
+/* For driver compatibility */
+#define N_IA_CSS_ISP_MEMORIES   IA_CSS_NUM_MEMORIES
+#define IA_CSS_NUM_ISP_MEMORIES IA_CSS_NUM_MEMORIES
+
+/*
+ * ISP2401 specific enums
+ */
+
+typedef enum {
+	ISYS_IRQ0_ID = 0,	/* port a */
+	ISYS_IRQ1_ID,	/* port b */
+	ISYS_IRQ2_ID,	/* port c */
+	N_ISYS_IRQ_ID
+} isys_irq_ID_t;
+
+
+/*
+ * Input-buffer Controller.
+ */
+typedef enum {
+	IBUF_CTRL0_ID = 0,	/* map to ISYS2401_IBUF_CNTRL_A */
+	IBUF_CTRL1_ID,		/* map to ISYS2401_IBUF_CNTRL_B */
+	IBUF_CTRL2_ID,		/* map ISYS2401_IBUF_CNTRL_C */
+	N_IBUF_CTRL_ID
+} ibuf_ctrl_ID_t;
+/* end of Input-buffer Controller */
+
+/*
+ * Stream2MMIO.
+ */
+typedef enum {
+	STREAM2MMIO0_ID = 0,	/* map to ISYS2401_S2M_A */
+	STREAM2MMIO1_ID,	/* map to ISYS2401_S2M_B */
+	STREAM2MMIO2_ID,	/* map to ISYS2401_S2M_C */
+	N_STREAM2MMIO_ID
+} stream2mmio_ID_t;
+
+typedef enum {
+	/*
+	 * Stream2MMIO 0 has 8 SIDs that are indexed by
+	 * [STREAM2MMIO_SID0_ID...STREAM2MMIO_SID7_ID].
+	 *
+	 * Stream2MMIO 1 has 4 SIDs that are indexed by
+	 * [STREAM2MMIO_SID0_ID...TREAM2MMIO_SID3_ID].
+	 *
+	 * Stream2MMIO 2 has 4 SIDs that are indexed by
+	 * [STREAM2MMIO_SID0_ID...STREAM2MMIO_SID3_ID].
+	 */
+	STREAM2MMIO_SID0_ID = 0,
+	STREAM2MMIO_SID1_ID,
+	STREAM2MMIO_SID2_ID,
+	STREAM2MMIO_SID3_ID,
+	STREAM2MMIO_SID4_ID,
+	STREAM2MMIO_SID5_ID,
+	STREAM2MMIO_SID6_ID,
+	STREAM2MMIO_SID7_ID,
+	N_STREAM2MMIO_SID_ID
+} stream2mmio_sid_ID_t;
+/* end of Stream2MMIO */
+
+/**
+ * Input System 2401: CSI-MIPI recevier.
+ */
+typedef enum {
+	CSI_RX_BACKEND0_ID = 0,	/* map to ISYS2401_MIPI_BE_A */
+	CSI_RX_BACKEND1_ID,		/* map to ISYS2401_MIPI_BE_B */
+	CSI_RX_BACKEND2_ID,		/* map to ISYS2401_MIPI_BE_C */
+	N_CSI_RX_BACKEND_ID
+} csi_rx_backend_ID_t;
+
+typedef enum {
+	CSI_RX_FRONTEND0_ID = 0,	/* map to ISYS2401_CSI_RX_A */
+	CSI_RX_FRONTEND1_ID,		/* map to ISYS2401_CSI_RX_B */
+	CSI_RX_FRONTEND2_ID,		/* map to ISYS2401_CSI_RX_C */
+#define N_CSI_RX_FRONTEND_ID (CSI_RX_FRONTEND2_ID + 1)
+} csi_rx_frontend_ID_t;
+
+typedef enum {
+	CSI_RX_DLANE0_ID = 0,		/* map to DLANE0 in CSI RX */
+	CSI_RX_DLANE1_ID,		/* map to DLANE1 in CSI RX */
+	CSI_RX_DLANE2_ID,		/* map to DLANE2 in CSI RX */
+	CSI_RX_DLANE3_ID,		/* map to DLANE3 in CSI RX */
+	N_CSI_RX_DLANE_ID
+} csi_rx_fe_dlane_ID_t;
+/* end of CSI-MIPI receiver */
+
+typedef enum {
+	ISYS2401_DMA0_ID = 0,
+	N_ISYS2401_DMA_ID
+} isys2401_dma_ID_t;
+
+/**
+ * Pixel-generator. ("system_global.h")
+ */
+typedef enum {
+	PIXELGEN0_ID = 0,
+	PIXELGEN1_ID,
+	PIXELGEN2_ID,
+	N_PIXELGEN_ID
+} pixelgen_ID_t;
+/* end of pixel-generator. ("system_global.h") */
+
+typedef enum {
+	INPUT_SYSTEM_CSI_PORT0_ID = 0,
+	INPUT_SYSTEM_CSI_PORT1_ID,
+	INPUT_SYSTEM_CSI_PORT2_ID,
+
+	INPUT_SYSTEM_PIXELGEN_PORT0_ID,
+	INPUT_SYSTEM_PIXELGEN_PORT1_ID,
+	INPUT_SYSTEM_PIXELGEN_PORT2_ID,
+
+	N_INPUT_SYSTEM_INPUT_PORT_ID
+} input_system_input_port_ID_t;
+
+#define N_INPUT_SYSTEM_CSI_PORT	3
+
+typedef enum {
+	ISYS2401_DMA_CHANNEL_0 = 0,
+	ISYS2401_DMA_CHANNEL_1,
+	ISYS2401_DMA_CHANNEL_2,
+	ISYS2401_DMA_CHANNEL_3,
+	ISYS2401_DMA_CHANNEL_4,
+	ISYS2401_DMA_CHANNEL_5,
+	ISYS2401_DMA_CHANNEL_6,
+	ISYS2401_DMA_CHANNEL_7,
+	ISYS2401_DMA_CHANNEL_8,
+	ISYS2401_DMA_CHANNEL_9,
+	ISYS2401_DMA_CHANNEL_10,
+	ISYS2401_DMA_CHANNEL_11,
+	N_ISYS2401_DMA_CHANNEL
+} isys2401_dma_channel;
+
+#endif /* __SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/system_local.c b/drivers/staging/media/atomisp/pci/system_local.c
new file mode 100644
index 000000000..4ca8569d7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/system_local.c
@@ -0,0 +1,179 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 "system_local.h"
+
+/* ISP */
+const hrt_address ISP_CTRL_BASE[N_ISP_ID] = {
+	0x0000000000020000ULL
+};
+
+const hrt_address ISP_DMEM_BASE[N_ISP_ID] = {
+	0x0000000000200000ULL
+};
+
+const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID] = {
+	0x0000000000100000ULL
+};
+
+/* SP */
+const hrt_address SP_CTRL_BASE[N_SP_ID] = {
+	0x0000000000010000ULL
+};
+
+const hrt_address SP_DMEM_BASE[N_SP_ID] = {
+	0x0000000000300000ULL
+};
+
+/* MMU */
+/*
+ * MMU0_ID: The data MMU
+ * MMU1_ID: The icache MMU
+ */
+const hrt_address MMU_BASE[N_MMU_ID] = {
+	0x0000000000070000ULL,
+	0x00000000000A0000ULL
+};
+
+/* DMA */
+const hrt_address DMA_BASE[N_DMA_ID] = {
+	0x0000000000040000ULL
+};
+
+const hrt_address ISYS2401_DMA_BASE[N_ISYS2401_DMA_ID] = {
+	0x00000000000CA000ULL
+};
+
+/* IRQ */
+const hrt_address IRQ_BASE[N_IRQ_ID] = {
+	0x0000000000000500ULL,
+	0x0000000000030A00ULL,
+	0x000000000008C000ULL,
+	0x0000000000090200ULL
+};
+
+/*
+	0x0000000000000500ULL};
+ */
+
+/* GDC */
+const hrt_address GDC_BASE[N_GDC_ID] = {
+	0x0000000000050000ULL,
+	0x0000000000060000ULL
+};
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID] = {
+	0x0000000000000000ULL
+};
+
+/*
+const hrt_address GP_REGS_BASE[N_GP_REGS_ID] = {
+	0x0000000000000000ULL};
+
+const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	0x0000000000090000ULL};
+*/
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	0x0000000000000000ULL
+};
+
+/*GP TIMER , all timer registers are inter-twined,
+ * so, having multiple base addresses for
+ * different timers does not help*/
+const hrt_address GP_TIMER_BASE =
+    (hrt_address)0x0000000000000600ULL;
+
+/* GPIO */
+const hrt_address GPIO_BASE[N_GPIO_ID] = {
+	0x0000000000000400ULL
+};
+
+/* TIMED_CTRL */
+const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID] = {
+	0x0000000000000100ULL
+};
+
+/* INPUT_FORMATTER */
+const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID] = {
+	0x0000000000030000ULL,
+	0x0000000000030200ULL,
+	0x0000000000030400ULL,
+	0x0000000000030600ULL
+}; /* memcpy() */
+
+/* INPUT_SYSTEM */
+const hrt_address INPUT_SYSTEM_BASE[N_INPUT_SYSTEM_ID] = {
+	0x0000000000080000ULL
+};
+
+/*	0x0000000000081000ULL, */ /* capture A */
+/*	0x0000000000082000ULL, */ /* capture B */
+/*	0x0000000000083000ULL, */ /* capture C */
+/*	0x0000000000084000ULL, */ /* Acquisition */
+/*	0x0000000000085000ULL, */ /* DMA */
+/*	0x0000000000089000ULL, */ /* ctrl */
+/*	0x000000000008A000ULL, */ /* GP regs */
+/*	0x000000000008B000ULL, */ /* FIFO */
+/*	0x000000000008C000ULL, */ /* IRQ */
+
+/* RX, the MIPI lane control regs start at offset 0 */
+const hrt_address RX_BASE[N_RX_ID] = {
+	0x0000000000080100ULL
+};
+
+/* IBUF_CTRL, part of the Input System 2401 */
+const hrt_address IBUF_CTRL_BASE[N_IBUF_CTRL_ID] = {
+	0x00000000000C1800ULL,	/* ibuf controller A */
+	0x00000000000C3800ULL,	/* ibuf controller B */
+	0x00000000000C5800ULL	/* ibuf controller C */
+};
+
+/* ISYS IRQ Controllers, part of the Input System 2401 */
+const hrt_address ISYS_IRQ_BASE[N_ISYS_IRQ_ID] = {
+	0x00000000000C1400ULL,	/* port a */
+	0x00000000000C3400ULL,	/* port b */
+	0x00000000000C5400ULL	/* port c */
+};
+
+/* CSI FE, part of the Input System 2401 */
+const hrt_address CSI_RX_FE_CTRL_BASE[N_CSI_RX_FRONTEND_ID] = {
+	0x00000000000C0400ULL,	/* csi fe controller A */
+	0x00000000000C2400ULL,	/* csi fe controller B */
+	0x00000000000C4400ULL	/* csi fe controller C */
+};
+
+/* CSI BE, part of the Input System 2401 */
+const hrt_address CSI_RX_BE_CTRL_BASE[N_CSI_RX_BACKEND_ID] = {
+	0x00000000000C0800ULL,	/* csi be controller A */
+	0x00000000000C2800ULL,	/* csi be controller B */
+	0x00000000000C4800ULL	/* csi be controller C */
+};
+
+/* PIXEL Generator, part of the Input System 2401 */
+const hrt_address PIXELGEN_CTRL_BASE[N_PIXELGEN_ID] = {
+	0x00000000000C1000ULL,	/* pixel gen controller A */
+	0x00000000000C3000ULL,	/* pixel gen controller B */
+	0x00000000000C5000ULL	/* pixel gen controller C */
+};
+
+/* Stream2MMIO, part of the Input System 2401 */
+const hrt_address STREAM2MMIO_CTRL_BASE[N_STREAM2MMIO_ID] = {
+	0x00000000000C0C00ULL,	/* stream2mmio controller A */
+	0x00000000000C2C00ULL,	/* stream2mmio controller B */
+	0x00000000000C4C00ULL	/* stream2mmio controller C */
+};
diff --git a/drivers/staging/media/atomisp/pci/system_local.h b/drivers/staging/media/atomisp/pci/system_local.h
new file mode 100644
index 000000000..a47258c2e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/system_local.h
@@ -0,0 +1,103 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef __SYSTEM_LOCAL_H_INCLUDED__
+#define __SYSTEM_LOCAL_H_INCLUDED__
+
+#ifdef HRT_ISP_CSS_CUSTOM_HOST
+#ifndef HRT_USE_VIR_ADDRS
+#define HRT_USE_VIR_ADDRS
+#endif
+#endif
+
+#include "system_global.h"
+
+/* This interface is deprecated */
+#include "hive_types.h"
+
+/*
+ * Cell specific address maps
+ */
+
+#define GP_FIFO_BASE   ((hrt_address)0x0000000000090104)		/* This is NOT a base address */
+
+/* ISP */
+extern const hrt_address ISP_CTRL_BASE[N_ISP_ID];
+extern const hrt_address ISP_DMEM_BASE[N_ISP_ID];
+extern const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID];
+
+/* SP */
+extern const hrt_address SP_CTRL_BASE[N_SP_ID];
+extern const hrt_address SP_DMEM_BASE[N_SP_ID];
+
+/* MMU */
+
+extern const hrt_address MMU_BASE[N_MMU_ID];
+
+/* DMA */
+extern const hrt_address DMA_BASE[N_DMA_ID];
+extern const hrt_address ISYS2401_DMA_BASE[N_ISYS2401_DMA_ID];
+
+/* IRQ */
+extern const hrt_address IRQ_BASE[N_IRQ_ID];
+
+/* GDC */
+extern const hrt_address GDC_BASE[N_GDC_ID];
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+extern const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID];
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+extern const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID];
+
+/*GP TIMER , all timer registers are inter-twined,
+ * so, having multiple base addresses for
+ * different timers does not help*/
+extern const hrt_address GP_TIMER_BASE;
+
+/* GPIO */
+extern const hrt_address GPIO_BASE[N_GPIO_ID];
+
+/* TIMED_CTRL */
+extern const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID];
+
+/* INPUT_FORMATTER */
+extern const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID];
+
+/* INPUT_SYSTEM */
+extern const hrt_address INPUT_SYSTEM_BASE[N_INPUT_SYSTEM_ID];
+
+/* RX, the MIPI lane control regs start at offset 0 */
+extern const hrt_address RX_BASE[N_RX_ID];
+
+/* IBUF_CTRL, part of the Input System 2401 */
+extern const hrt_address IBUF_CTRL_BASE[N_IBUF_CTRL_ID];
+
+/* ISYS IRQ Controllers, part of the Input System 2401 */
+extern const hrt_address ISYS_IRQ_BASE[N_ISYS_IRQ_ID];
+
+/* CSI FE, part of the Input System 2401 */
+extern const hrt_address CSI_RX_FE_CTRL_BASE[N_CSI_RX_FRONTEND_ID];
+
+/* CSI BE, part of the Input System 2401 */
+extern const hrt_address CSI_RX_BE_CTRL_BASE[N_CSI_RX_BACKEND_ID];
+
+/* PIXEL Generator, part of the Input System 2401 */
+extern const hrt_address PIXELGEN_CTRL_BASE[N_PIXELGEN_ID];
+
+/* Stream2MMIO, part of the Input System 2401 */
+extern const hrt_address STREAM2MMIO_CTRL_BASE[N_STREAM2MMIO_ID];
+
+#endif /* __SYSTEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/timed_controller_defs.h b/drivers/staging/media/atomisp/pci/timed_controller_defs.h
new file mode 100644
index 000000000..9ddad8770
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/timed_controller_defs.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef _timed_controller_defs_h
+#define _timed_controller_defs_h
+
+#define _HRT_TIMED_CONTROLLER_CMD_REG_IDX 0
+
+#define _HRT_TIMED_CONTROLLER_REG_ALIGN 4
+
+#endif /* _timed_controller_defs_h */
diff --git a/drivers/staging/media/atomisp/pci/version.h b/drivers/staging/media/atomisp/pci/version.h
new file mode 100644
index 000000000..a74a7bbfd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/version.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#ifndef HRT_VERSION_H
+#define HRT_VERSION_H
+#define HRT_VERSION_MAJOR 1
+#define HRT_VERSION_MINOR 4
+#define HRT_VERSION 1_4
+#endif
diff --git a/drivers/staging/media/deprecated/cpia2/Kconfig b/drivers/staging/media/deprecated/cpia2/Kconfig
new file mode 100644
index 000000000..ee3b25a75
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_CPIA2
+	tristate "CPiA2 Video For Linux (DEPRECATED)"
+	depends on USB && VIDEO_DEV
+	help
+	  This is the video4linux driver for cameras based on Vision's CPiA2
+	  (Colour Processor Interface ASIC), such as the Digital Blue QX5
+	  Microscope. If you have one of these cameras, say Y here
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  This driver is also available as a module (cpia2).
diff --git a/drivers/staging/media/deprecated/cpia2/Makefile b/drivers/staging/media/deprecated/cpia2/Makefile
new file mode 100644
index 000000000..05664141f
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0-only
+cpia2-objs	:= cpia2_v4l.o cpia2_usb.o cpia2_core.o
+
+obj-$(CONFIG_VIDEO_CPIA2) += cpia2.o
diff --git a/drivers/staging/media/deprecated/cpia2/TODO b/drivers/staging/media/deprecated/cpia2/TODO
new file mode 100644
index 000000000..92ac8718d
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/TODO
@@ -0,0 +1,6 @@
+The cpia2 driver does not use the vb2 framework for streaming
+video, instead it implements this in the driver.
+
+To prevent removal of this driver early 2023 it has to be
+converted to use vb2. Contact the linux-media@vger.kernel.org
+mailing list if you want to do this.
diff --git a/drivers/staging/media/deprecated/cpia2/cpia2.h b/drivers/staging/media/deprecated/cpia2/cpia2.h
new file mode 100644
index 000000000..57b7f1ea6
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/cpia2.h
@@ -0,0 +1,475 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/****************************************************************************
+ *
+ *  Filename: cpia2.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPiA2 based video cameras.
+ *
+ *     This driver is modelled on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ ****************************************************************************/
+
+#ifndef __CPIA2_H__
+#define __CPIA2_H__
+
+#include <linux/videodev2.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include "cpia2_registers.h"
+
+/* define for verbose debug output */
+//#define _CPIA2_DEBUG_
+
+/***
+ * Image defines
+ ***/
+
+/*  Misc constants */
+#define ALLOW_CORRUPT 0		/* Causes collater to discard checksum */
+
+/* USB Transfer mode */
+#define XFER_ISOC 0
+#define XFER_BULK 1
+
+/* USB Alternates */
+#define USBIF_CMDONLY 0
+#define USBIF_BULK 1
+#define USBIF_ISO_1 2	/*  128 bytes/ms */
+#define USBIF_ISO_2 3	/*  384 bytes/ms */
+#define USBIF_ISO_3 4	/*  640 bytes/ms */
+#define USBIF_ISO_4 5	/*  768 bytes/ms */
+#define USBIF_ISO_5 6	/*  896 bytes/ms */
+#define USBIF_ISO_6 7	/* 1023 bytes/ms */
+
+/* Flicker Modes */
+#define NEVER_FLICKER   0
+#define FLICKER_60      60
+#define FLICKER_50      50
+
+/* Debug flags */
+#define DEBUG_NONE          0
+#define DEBUG_REG           0x00000001
+#define DEBUG_DUMP_PATCH    0x00000002
+#define DEBUG_DUMP_REGS     0x00000004
+
+/***
+ * Video frame sizes
+ ***/
+enum {
+	VIDEOSIZE_VGA = 0,	/* 640x480 */
+	VIDEOSIZE_CIF,		/* 352x288 */
+	VIDEOSIZE_QVGA,		/* 320x240 */
+	VIDEOSIZE_QCIF,		/* 176x144 */
+	VIDEOSIZE_288_216,
+	VIDEOSIZE_256_192,
+	VIDEOSIZE_224_168,
+	VIDEOSIZE_192_144,
+};
+
+#define STV_IMAGE_CIF_ROWS    288
+#define STV_IMAGE_CIF_COLS    352
+
+#define STV_IMAGE_QCIF_ROWS   144
+#define STV_IMAGE_QCIF_COLS   176
+
+#define STV_IMAGE_VGA_ROWS    480
+#define STV_IMAGE_VGA_COLS    640
+
+#define STV_IMAGE_QVGA_ROWS   240
+#define STV_IMAGE_QVGA_COLS   320
+
+#define JPEG_MARKER_COM (1<<6)	/* Comment segment */
+
+/***
+ * Enums
+ ***/
+/* Sensor types available with cpia2 asics */
+enum sensors {
+	CPIA2_SENSOR_410,
+	CPIA2_SENSOR_500
+};
+
+/* Asic types available in the CPiA2 architecture */
+#define  CPIA2_ASIC_672 0x67
+
+/* Device types (stv672, stv676, etc) */
+#define  DEVICE_STV_672   0x0001
+#define  DEVICE_STV_676   0x0002
+
+enum frame_status {
+	FRAME_EMPTY,
+	FRAME_READING,		/* In the process of being grabbed into */
+	FRAME_READY,		/* Ready to be read */
+	FRAME_ERROR,
+};
+
+/***
+ * Register access (for USB request byte)
+ ***/
+enum {
+	CAMERAACCESS_SYSTEM = 0,
+	CAMERAACCESS_VC,
+	CAMERAACCESS_VP,
+	CAMERAACCESS_IDATA
+};
+
+#define CAMERAACCESS_TYPE_BLOCK    0x00
+#define CAMERAACCESS_TYPE_RANDOM   0x04
+#define CAMERAACCESS_TYPE_MASK     0x08
+#define CAMERAACCESS_TYPE_REPEAT   0x0C
+
+#define TRANSFER_READ 0
+#define TRANSFER_WRITE 1
+
+#define DEFAULT_ALT   USBIF_ISO_6
+#define DEFAULT_BRIGHTNESS 0x46
+#define DEFAULT_CONTRAST 0x93
+#define DEFAULT_SATURATION 0x7f
+
+/* Power state */
+#define HI_POWER_MODE CPIA2_SYSTEM_CONTROL_HIGH_POWER
+#define LO_POWER_MODE CPIA2_SYSTEM_CONTROL_LOW_POWER
+
+
+/********
+ * Commands
+ *******/
+enum {
+	CPIA2_CMD_NONE = 0,
+	CPIA2_CMD_GET_VERSION,
+	CPIA2_CMD_GET_PNP_ID,
+	CPIA2_CMD_GET_ASIC_TYPE,
+	CPIA2_CMD_GET_SENSOR,
+	CPIA2_CMD_GET_VP_DEVICE,
+	CPIA2_CMD_GET_VP_BRIGHTNESS,
+	CPIA2_CMD_SET_VP_BRIGHTNESS,
+	CPIA2_CMD_GET_CONTRAST,
+	CPIA2_CMD_SET_CONTRAST,
+	CPIA2_CMD_GET_VP_SATURATION,
+	CPIA2_CMD_SET_VP_SATURATION,
+	CPIA2_CMD_GET_VP_GPIO_DIRECTION,
+	CPIA2_CMD_SET_VP_GPIO_DIRECTION,
+	CPIA2_CMD_GET_VP_GPIO_DATA,
+	CPIA2_CMD_SET_VP_GPIO_DATA,
+	CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION,
+	CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+	CPIA2_CMD_GET_VC_MP_GPIO_DATA,
+	CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+	CPIA2_CMD_ENABLE_PACKET_CTRL,
+	CPIA2_CMD_GET_FLICKER_MODES,
+	CPIA2_CMD_SET_FLICKER_MODES,
+	CPIA2_CMD_RESET_FIFO,	/* clear fifo and enable stream block */
+	CPIA2_CMD_SET_HI_POWER,
+	CPIA2_CMD_SET_LOW_POWER,
+	CPIA2_CMD_CLEAR_V2W_ERR,
+	CPIA2_CMD_SET_USER_MODE,
+	CPIA2_CMD_GET_USER_MODE,
+	CPIA2_CMD_FRAMERATE_REQ,
+	CPIA2_CMD_SET_COMPRESSION_STATE,
+	CPIA2_CMD_GET_WAKEUP,
+	CPIA2_CMD_SET_WAKEUP,
+	CPIA2_CMD_GET_PW_CONTROL,
+	CPIA2_CMD_SET_PW_CONTROL,
+	CPIA2_CMD_GET_SYSTEM_CTRL,
+	CPIA2_CMD_SET_SYSTEM_CTRL,
+	CPIA2_CMD_GET_VP_SYSTEM_STATE,
+	CPIA2_CMD_GET_VP_SYSTEM_CTRL,
+	CPIA2_CMD_SET_VP_SYSTEM_CTRL,
+	CPIA2_CMD_GET_VP_EXP_MODES,
+	CPIA2_CMD_SET_VP_EXP_MODES,
+	CPIA2_CMD_GET_DEVICE_CONFIG,
+	CPIA2_CMD_SET_DEVICE_CONFIG,
+	CPIA2_CMD_SET_SERIAL_ADDR,
+	CPIA2_CMD_SET_SENSOR_CR1,
+	CPIA2_CMD_GET_VC_CONTROL,
+	CPIA2_CMD_SET_VC_CONTROL,
+	CPIA2_CMD_SET_TARGET_KB,
+	CPIA2_CMD_SET_DEF_JPEG_OPT,
+	CPIA2_CMD_REHASH_VP4,
+	CPIA2_CMD_GET_USER_EFFECTS,
+	CPIA2_CMD_SET_USER_EFFECTS
+};
+
+enum user_cmd {
+	COMMAND_NONE = 0x00000001,
+	COMMAND_SET_FPS = 0x00000002,
+	COMMAND_SET_COLOR_PARAMS = 0x00000004,
+	COMMAND_GET_COLOR_PARAMS = 0x00000008,
+	COMMAND_SET_FORMAT = 0x00000010,	/* size, etc */
+	COMMAND_SET_FLICKER = 0x00000020
+};
+
+/***
+ * Some defines specific to the 676 chip
+ ***/
+#define CAMACC_CIF      0x01
+#define CAMACC_VGA      0x02
+#define CAMACC_QCIF     0x04
+#define CAMACC_QVGA     0x08
+
+
+struct cpia2_register {
+	u8 index;
+	u8 value;
+};
+
+struct cpia2_reg_mask {
+	u8 index;
+	u8 and_mask;
+	u8 or_mask;
+	u8 fill;
+};
+
+struct cpia2_command {
+	u32 command;
+	u8 req_mode;		/* (Block or random) | registerBank */
+	u8 reg_count;
+	u8 direction;
+	u8 start;
+	union reg_types {
+		struct cpia2_register registers[32];
+		struct cpia2_reg_mask masks[16];
+		u8 block_data[64];
+		u8 *patch_data;	/* points to function defined block */
+	} buffer;
+};
+
+struct camera_params {
+	struct {
+		u8 firmware_revision_hi; /* For system register set (bank 0) */
+		u8 firmware_revision_lo;
+		u8 asic_id;	/* Video Compressor set (bank 1) */
+		u8 asic_rev;
+		u8 vp_device_hi;	/* Video Processor set (bank 2) */
+		u8 vp_device_lo;
+		u8 sensor_flags;
+		u8 sensor_rev;
+	} version;
+
+	struct {
+		u32 device_type;     /* enumerated from vendor/product ids.
+				      * Currently, either STV_672 or STV_676 */
+		u16 vendor;
+		u16 product;
+		u16 device_revision;
+	} pnp_id;
+
+	struct {
+		u8 brightness;	/* CPIA2_VP_EXPOSURE_TARGET */
+		u8 contrast;	/* Note: this is CPIA2_VP_YRANGE */
+		u8 saturation;	/*  CPIA2_VP_SATURATION */
+	} color_params;
+
+	struct {
+		u8 cam_register;
+		u8 flicker_mode_req;	/* 1 if flicker on, else never flicker */
+	} flicker_control;
+
+	struct {
+		u8 jpeg_options;
+		u8 creep_period;
+		u8 user_squeeze;
+		u8 inhibit_htables;
+	} compression;
+
+	struct {
+		u8 ohsize;	/* output image size */
+		u8 ovsize;
+		u8 hcrop;	/* cropping start_pos/4 */
+		u8 vcrop;
+		u8 hphase;	/* scaling registers */
+		u8 vphase;
+		u8 hispan;
+		u8 vispan;
+		u8 hicrop;
+		u8 vicrop;
+		u8 hifraction;
+		u8 vifraction;
+	} image_size;
+
+	struct {
+		int width;	/* actual window width */
+		int height;	/* actual window height */
+	} roi;
+
+	struct {
+		u8 video_mode;
+		u8 frame_rate;
+		u8 video_size;	/* Not a register, just a convenience for cropped sizes */
+		u8 gpio_direction;
+		u8 gpio_data;
+		u8 system_ctrl;
+		u8 system_state;
+		u8 lowlight_boost;	/* Bool: 0 = off, 1 = on */
+		u8 device_config;
+		u8 exposure_modes;
+		u8 user_effects;
+	} vp_params;
+
+	struct {
+		u8 pw_control;
+		u8 wakeup;
+		u8 vc_control;
+		u8 vc_mp_direction;
+		u8 vc_mp_data;
+		u8 quality;
+	} vc_params;
+
+	struct {
+		u8 power_mode;
+		u8 system_ctrl;
+		u8 stream_mode;	/* This is the current alternate for usb drivers */
+		u8 allow_corrupt;
+	} camera_state;
+};
+
+#define NUM_SBUF    2
+
+struct cpia2_sbuf {
+	char *data;
+	struct urb *urb;
+};
+
+struct framebuf {
+	u64 ts;
+	unsigned long seq;
+	int num;
+	int length;
+	int max_length;
+	volatile enum frame_status status;
+	u8 *data;
+	struct framebuf *next;
+};
+
+struct camera_data {
+	/* locks */
+	struct v4l2_device v4l2_dev;
+	struct mutex v4l2_lock;	/* serialize file operations */
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* Lights control cluster */
+		struct v4l2_ctrl *top_light;
+		struct v4l2_ctrl *bottom_light;
+	};
+	struct v4l2_ctrl *usb_alt;
+
+	/* camera status */
+	int first_image_seen;
+	enum sensors sensor_type;
+	u8 flush;
+	struct v4l2_fh *stream_fh;
+	u8 mmapped;
+	int streaming;		/* 0 = no, 1 = yes */
+	int xfer_mode;		/* XFER_BULK or XFER_ISOC */
+	struct camera_params params;	/* camera settings */
+
+	/* v4l */
+	int video_size;			/* VIDEO_SIZE_ */
+	struct video_device vdev;	/* v4l videodev */
+	u32 width;
+	u32 height;			/* Its size */
+	__u32 pixelformat;       /* Format fourcc      */
+
+	/* USB */
+	struct usb_device *dev;
+	unsigned char iface;
+	unsigned int cur_alt;
+	unsigned int old_alt;
+	struct cpia2_sbuf sbuf[NUM_SBUF];	/* Double buffering */
+
+	wait_queue_head_t wq_stream;
+
+	/* Buffering */
+	u32 frame_size;
+	int num_frames;
+	unsigned long frame_count;
+	u8 *frame_buffer;	/* frame buffer data */
+	struct framebuf *buffers;
+	struct framebuf * volatile curbuff;
+	struct framebuf *workbuff;
+
+	/* MJPEG Extension */
+	int APPn;		/* Number of APP segment to be written, must be 0..15 */
+	int APP_len;		/* Length of data in JPEG APPn segment */
+	char APP_data[60];	/* Data in the JPEG APPn segment. */
+
+	int COM_len;		/* Length of data in JPEG COM segment */
+	char COM_data[60];	/* Data in JPEG COM segment */
+};
+
+/* v4l */
+int cpia2_register_camera(struct camera_data *cam);
+void cpia2_unregister_camera(struct camera_data *cam);
+void cpia2_camera_release(struct v4l2_device *v4l2_dev);
+
+/* core */
+int cpia2_reset_camera(struct camera_data *cam);
+int cpia2_set_low_power(struct camera_data *cam);
+void cpia2_dbg_dump_registers(struct camera_data *cam);
+int cpia2_match_video_size(int width, int height);
+void cpia2_set_camera_state(struct camera_data *cam);
+void cpia2_save_camera_state(struct camera_data *cam);
+void cpia2_set_color_params(struct camera_data *cam);
+void cpia2_set_brightness(struct camera_data *cam, unsigned char value);
+void cpia2_set_contrast(struct camera_data *cam, unsigned char value);
+void cpia2_set_saturation(struct camera_data *cam, unsigned char value);
+int cpia2_set_flicker_mode(struct camera_data *cam, int mode);
+void cpia2_set_format(struct camera_data *cam);
+int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd);
+int cpia2_do_command(struct camera_data *cam,
+		     unsigned int command,
+		     unsigned char direction, unsigned char param);
+void cpia2_deinit_camera_struct(struct camera_data *cam, struct usb_interface *intf);
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf);
+int cpia2_init_camera(struct camera_data *cam);
+int cpia2_allocate_buffers(struct camera_data *cam);
+void cpia2_free_buffers(struct camera_data *cam);
+long cpia2_read(struct camera_data *cam,
+		char __user *buf, unsigned long count, int noblock);
+__poll_t cpia2_poll(struct camera_data *cam,
+			struct file *filp, poll_table *wait);
+int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma);
+void cpia2_set_property_flip(struct camera_data *cam, int prop_val);
+void cpia2_set_property_mirror(struct camera_data *cam, int prop_val);
+int cpia2_set_gpio(struct camera_data *cam, unsigned char setting);
+int cpia2_set_fps(struct camera_data *cam, int framerate);
+
+/* usb */
+int cpia2_usb_init(void);
+void cpia2_usb_cleanup(void);
+int cpia2_usb_transfer_cmd(struct camera_data *cam, void *registers,
+			   u8 request, u8 start, u8 count, u8 direction);
+int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate);
+int cpia2_usb_stream_stop(struct camera_data *cam);
+int cpia2_usb_stream_pause(struct camera_data *cam);
+int cpia2_usb_stream_resume(struct camera_data *cam);
+int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
+					 unsigned int alt);
+
+
+/* ----------------------- debug functions ---------------------- */
+#ifdef _CPIA2_DEBUG_
+#define ALOG(lev, fmt, args...) printk(lev "%s:%d %s(): " fmt, __FILE__, __LINE__, __func__, ## args)
+#define LOG(fmt, args...) ALOG(KERN_INFO, fmt, ## args)
+#define ERR(fmt, args...) ALOG(KERN_ERR, fmt, ## args)
+#define DBG(fmt, args...) ALOG(KERN_DEBUG, fmt, ## args)
+#else
+#define ALOG(fmt,args...) printk(fmt,##args)
+#define LOG(fmt,args...) ALOG(KERN_INFO "cpia2: "fmt,##args)
+#define ERR(fmt,args...) ALOG(KERN_ERR "cpia2: "fmt,##args)
+#define DBG(fmn,args...) do {} while(0)
+#endif
+/* No function or lineno, for shorter lines */
+#define KINFO(fmt, args...) printk(KERN_INFO fmt,##args)
+
+#endif
diff --git a/drivers/staging/media/deprecated/cpia2/cpia2_core.c b/drivers/staging/media/deprecated/cpia2/cpia2_core.c
new file mode 100644
index 000000000..b5a2d06fb
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/cpia2_core.c
@@ -0,0 +1,2434 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/****************************************************************************
+ *
+ *  Filename: cpia2_core.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@lxorguk.ukuu.org.uk>
+ *
+ ****************************************************************************/
+
+#include "cpia2.h"
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+#include <linux/sched/signal.h>
+
+#define FIRMWARE "cpia2/stv0672_vp4.bin"
+MODULE_FIRMWARE(FIRMWARE);
+
+/* #define _CPIA2_DEBUG_ */
+
+#ifdef _CPIA2_DEBUG_
+
+static const char *block_name[] = {
+	"System",
+	"VC",
+	"VP",
+	"IDATA"
+};
+#endif
+
+static unsigned int debugs_on;	/* default 0 - DEBUG_REG */
+
+
+/******************************************************************************
+ *
+ *  Forward Declarations
+ *
+ *****************************************************************************/
+static int apply_vp_patch(struct camera_data *cam);
+static int set_default_user_mode(struct camera_data *cam);
+static int set_vw_size(struct camera_data *cam, int size);
+static int configure_sensor(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int config_sensor_410(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int config_sensor_500(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int set_all_properties(struct camera_data *cam);
+static void wake_system(struct camera_data *cam);
+static void set_lowlight_boost(struct camera_data *cam);
+static void reset_camera_struct(struct camera_data *cam);
+static int cpia2_set_high_power(struct camera_data *cam);
+
+/* Here we want the physical address of the memory.
+ * This is used when initializing the contents of the
+ * area and marking the pages as reserved.
+ */
+static inline unsigned long kvirt_to_pa(unsigned long adr)
+{
+	unsigned long kva, ret;
+
+	kva = (unsigned long) page_address(vmalloc_to_page((void *)adr));
+	kva |= adr & (PAGE_SIZE-1); /* restore the offset */
+	ret = __pa(kva);
+	return ret;
+}
+
+static void *rvmalloc(unsigned long size)
+{
+	void *mem;
+	unsigned long adr;
+
+	/* Round it off to PAGE_SIZE */
+	size = PAGE_ALIGN(size);
+
+	mem = vmalloc_32(size);
+	if (!mem)
+		return NULL;
+
+	memset(mem, 0, size);	/* Clear the ram out, no junk to the user */
+	adr = (unsigned long) mem;
+
+	while ((long)size > 0) {
+		SetPageReserved(vmalloc_to_page((void *)adr));
+		adr += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	return mem;
+}
+
+static void rvfree(void *mem, unsigned long size)
+{
+	unsigned long adr;
+
+	if (!mem)
+		return;
+
+	size = PAGE_ALIGN(size);
+
+	adr = (unsigned long) mem;
+	while ((long)size > 0) {
+		ClearPageReserved(vmalloc_to_page((void *)adr));
+		adr += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	vfree(mem);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_do_command
+ *
+ *  Send an arbitrary command to the camera.  For commands that read from
+ *  the camera, copy the buffers into the proper param structures.
+ *****************************************************************************/
+int cpia2_do_command(struct camera_data *cam,
+		     u32 command, u8 direction, u8 param)
+{
+	int retval = 0;
+	struct cpia2_command cmd;
+	unsigned int device = cam->params.pnp_id.device_type;
+
+	cmd.command = command;
+	cmd.reg_count = 2;	/* default */
+	cmd.direction = direction;
+
+	/***
+	 * Set up the command.
+	 ***/
+	switch (command) {
+	case CPIA2_CMD_GET_VERSION:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.start = CPIA2_SYSTEM_DEVICE_HI;
+		break;
+	case CPIA2_CMD_GET_PNP_ID:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 8;
+		cmd.start = CPIA2_SYSTEM_DESCRIP_VID_HI;
+		break;
+	case CPIA2_CMD_GET_ASIC_TYPE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.start = CPIA2_VC_ASIC_ID;
+		break;
+	case CPIA2_CMD_GET_SENSOR:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.start = CPIA2_VP_SENSOR_FLAGS;
+		break;
+	case CPIA2_CMD_GET_VP_DEVICE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.start = CPIA2_VP_DEVICEH;
+		break;
+	case CPIA2_CMD_SET_VP_BRIGHTNESS:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_BRIGHTNESS:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_EXPOSURE_TARGET;
+		else
+			cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
+		break;
+	case CPIA2_CMD_SET_CONTRAST:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_CONTRAST:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_YRANGE;
+		break;
+	case CPIA2_CMD_SET_VP_SATURATION:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_SATURATION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP_SATURATION;
+		else
+			cmd.start = CPIA2_VP5_MCUVSATURATION;
+		break;
+	case CPIA2_CMD_SET_VP_GPIO_DATA:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_GPIO_DATA:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_GPIO_DATA;
+		break;
+	case CPIA2_CMD_SET_VP_GPIO_DIRECTION:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_GPIO_DIRECTION;
+		break;
+	case CPIA2_CMD_SET_VC_MP_GPIO_DATA:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_MP_DATA;
+		break;
+	case CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_MP_DIR;
+		break;
+	case CPIA2_CMD_ENABLE_PACKET_CTRL:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.start = CPIA2_SYSTEM_INT_PACKET_CTRL;
+		cmd.reg_count = 1;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_FLICKER_MODES:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_FLICKER_MODES:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_FLICKER_MODES;
+		break;
+	case CPIA2_CMD_RESET_FIFO:	/* clear fifo and enable stream block */
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 2;
+		cmd.start = 0;
+		cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
+		cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		    CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
+		cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
+		cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		    CPIA2_VC_ST_CTRL_DST_USB |
+		    CPIA2_VC_ST_CTRL_EOF_DETECT |
+		    CPIA2_VC_ST_CTRL_FIFO_ENABLE;
+		break;
+	case CPIA2_CMD_SET_HI_POWER:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 2;
+		cmd.buffer.registers[0].index =
+		    CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.registers[1].index =
+		    CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.registers[0].value = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
+		cmd.buffer.registers[1].value =
+		    CPIA2_SYSTEM_CONTROL_HIGH_POWER;
+		break;
+	case CPIA2_CMD_SET_LOW_POWER:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.block_data[0] = 0;
+		break;
+	case CPIA2_CMD_CLEAR_V2W_ERR:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.block_data[0] = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
+		break;
+	case CPIA2_CMD_SET_USER_MODE:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_USER_MODE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_USER_MODE;
+		else
+			cmd.start = CPIA2_VP5_USER_MODE;
+		break;
+	case CPIA2_CMD_FRAMERATE_REQ:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_FRAMERATE_REQUEST;
+		else
+			cmd.start = CPIA2_VP5_FRAMERATE_REQUEST;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_WAKEUP:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_WAKEUP:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_WAKEUP;
+		break;
+	case CPIA2_CMD_SET_PW_CONTROL:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_PW_CONTROL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_PW_CTRL;
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_SYSTEMSTATE;
+		break;
+	case CPIA2_CMD_SET_SYSTEM_CTRL:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_SYSTEM_CTRL:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		break;
+	case CPIA2_CMD_SET_VP_SYSTEM_CTRL:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_SYSTEMCTRL;
+		break;
+	case CPIA2_CMD_SET_VP_EXP_MODES:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VP_EXP_MODES:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_EXPOSURE_MODES;
+		break;
+	case CPIA2_CMD_SET_DEVICE_CONFIG:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_DEVICE_CONFIG:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_DEVICE_CONFIG;
+		break;
+	case CPIA2_CMD_SET_SERIAL_ADDR:
+		cmd.buffer.block_data[0] = param;
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_VP_SERIAL_ADDR;
+		break;
+	case CPIA2_CMD_SET_SENSOR_CR1:
+		cmd.buffer.block_data[0] = param;
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SENSOR_CR1;
+		break;
+	case CPIA2_CMD_SET_VC_CONTROL:
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_VC_CONTROL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_VC_CTRL;
+		break;
+	case CPIA2_CMD_SET_TARGET_KB:
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.buffer.registers[0].index = CPIA2_VC_VC_TARGET_KB;
+		cmd.buffer.registers[0].value = param;
+		break;
+	case CPIA2_CMD_SET_DEF_JPEG_OPT:
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 4;
+		cmd.buffer.registers[0].index = CPIA2_VC_VC_JPEG_OPT;
+		cmd.buffer.registers[0].value =
+		    CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE;
+		cmd.buffer.registers[1].index = CPIA2_VC_VC_USER_SQUEEZE;
+		cmd.buffer.registers[1].value = 20;
+		cmd.buffer.registers[2].index = CPIA2_VC_VC_CREEP_PERIOD;
+		cmd.buffer.registers[2].value = 2;
+		cmd.buffer.registers[3].index = CPIA2_VC_VC_JPEG_OPT;
+		cmd.buffer.registers[3].value = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
+		break;
+	case CPIA2_CMD_REHASH_VP4:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_REHASH_VALUES;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_USER_EFFECTS:  /* Note: Be careful with this as
+					     this register can also affect
+					     flicker modes */
+		cmd.buffer.block_data[0] = param;
+		fallthrough;
+	case CPIA2_CMD_GET_USER_EFFECTS:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_USER_EFFECTS;
+		else
+			cmd.start = CPIA2_VP5_USER_EFFECTS;
+		break;
+	default:
+		LOG("DoCommand received invalid command\n");
+		return -EINVAL;
+	}
+
+	retval = cpia2_send_command(cam, &cmd);
+	if (retval) {
+		return retval;
+	}
+
+	/***
+	 * Now copy any results from a read into the appropriate param struct.
+	 ***/
+	switch (command) {
+	case CPIA2_CMD_GET_VERSION:
+		cam->params.version.firmware_revision_hi =
+		    cmd.buffer.block_data[0];
+		cam->params.version.firmware_revision_lo =
+		    cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_PNP_ID:
+		cam->params.pnp_id.vendor = (cmd.buffer.block_data[0] << 8) |
+					    cmd.buffer.block_data[1];
+		cam->params.pnp_id.product = (cmd.buffer.block_data[2] << 8) |
+					     cmd.buffer.block_data[3];
+		cam->params.pnp_id.device_revision =
+			(cmd.buffer.block_data[4] << 8) |
+			cmd.buffer.block_data[5];
+		if (cam->params.pnp_id.vendor == 0x553) {
+			if (cam->params.pnp_id.product == 0x100) {
+				cam->params.pnp_id.device_type = DEVICE_STV_672;
+			} else if (cam->params.pnp_id.product == 0x140 ||
+				   cam->params.pnp_id.product == 0x151) {
+				cam->params.pnp_id.device_type = DEVICE_STV_676;
+			}
+		}
+		break;
+	case CPIA2_CMD_GET_ASIC_TYPE:
+		cam->params.version.asic_id = cmd.buffer.block_data[0];
+		cam->params.version.asic_rev = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_SENSOR:
+		cam->params.version.sensor_flags = cmd.buffer.block_data[0];
+		cam->params.version.sensor_rev = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_VP_DEVICE:
+		cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
+		cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_VP_GPIO_DATA:
+		cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
+		cam->params.vp_params.gpio_direction = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
+		cam->params.vc_params.vc_mp_direction =cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
+		cam->params.vc_params.vc_mp_data = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_FLICKER_MODES:
+		cam->params.flicker_control.cam_register =
+			cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_WAKEUP:
+		cam->params.vc_params.wakeup = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_PW_CONTROL:
+		cam->params.vc_params.pw_control = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_SYSTEM_CTRL:
+		cam->params.camera_state.system_ctrl = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
+		cam->params.vp_params.system_state = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
+		cam->params.vp_params.system_ctrl = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_EXP_MODES:
+		cam->params.vp_params.exposure_modes = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_DEVICE_CONFIG:
+		cam->params.vp_params.device_config = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_CONTROL:
+		cam->params.vc_params.vc_control = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_USER_MODE:
+		cam->params.vp_params.video_mode = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_USER_EFFECTS:
+		cam->params.vp_params.user_effects = cmd.buffer.block_data[0];
+		break;
+	default:
+		break;
+	}
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_send_command
+ *
+ *****************************************************************************/
+
+#define DIR(cmd) ((cmd->direction == TRANSFER_WRITE) ? "Write" : "Read")
+#define BINDEX(cmd) (cmd->req_mode & 0x03)
+
+int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd)
+{
+	u8 count;
+	u8 start;
+	u8 *buffer;
+	int retval;
+
+	switch (cmd->req_mode & 0x0c) {
+	case CAMERAACCESS_TYPE_RANDOM:
+		count = cmd->reg_count * sizeof(struct cpia2_register);
+		start = 0;
+		buffer = (u8 *) & cmd->buffer;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Random: Register block %s\n", DIR(cmd),
+			    block_name[BINDEX(cmd)]);
+		break;
+	case CAMERAACCESS_TYPE_BLOCK:
+		count = cmd->reg_count;
+		start = cmd->start;
+		buffer = cmd->buffer.block_data;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Block: Register block %s\n", DIR(cmd),
+			    block_name[BINDEX(cmd)]);
+		break;
+	case CAMERAACCESS_TYPE_MASK:
+		count = cmd->reg_count * sizeof(struct cpia2_reg_mask);
+		start = 0;
+		buffer = (u8 *) & cmd->buffer;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Mask: Register block %s\n", DIR(cmd),
+			    block_name[BINDEX(cmd)]);
+		break;
+	case CAMERAACCESS_TYPE_REPEAT:	/* For patch blocks only */
+		count = cmd->reg_count;
+		start = cmd->start;
+		buffer = cmd->buffer.block_data;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Repeat: Register block %s\n", DIR(cmd),
+			    block_name[BINDEX(cmd)]);
+		break;
+	default:
+		LOG("%s: invalid request mode\n",__func__);
+		return -EINVAL;
+	}
+
+	retval = cpia2_usb_transfer_cmd(cam,
+					buffer,
+					cmd->req_mode,
+					start, count, cmd->direction);
+#ifdef _CPIA2_DEBUG_
+	if (debugs_on & DEBUG_REG) {
+		int i;
+		for (i = 0; i < cmd->reg_count; i++) {
+			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_BLOCK)
+				KINFO("%s Block: [0x%02X] = 0x%02X\n",
+				    DIR(cmd), start + i, buffer[i]);
+			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_RANDOM)
+				KINFO("%s Random: [0x%02X] = 0x%02X\n",
+				    DIR(cmd), cmd->buffer.registers[i].index,
+				    cmd->buffer.registers[i].value);
+		}
+	}
+#endif
+
+	return retval;
+};
+
+/*************
+ * Functions to implement camera functionality
+ *************/
+/******************************************************************************
+ *
+ *  cpia2_get_version_info
+ *
+ *****************************************************************************/
+static void cpia2_get_version_info(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_GET_VERSION, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_PNP_ID, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_ASIC_TYPE, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_SENSOR, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_DEVICE, TRANSFER_READ, 0);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_reset_camera
+ *
+ *  Called at least during the open process, sets up initial params.
+ *****************************************************************************/
+int cpia2_reset_camera(struct camera_data *cam)
+{
+	u8 tmp_reg;
+	int retval = 0;
+	int target_kb;
+	int i;
+	struct cpia2_command cmd;
+
+	/***
+	 * VC setup
+	 ***/
+	retval = configure_sensor(cam,
+				  cam->params.roi.width,
+				  cam->params.roi.height);
+	if (retval < 0) {
+		ERR("Couldn't configure sensor, error=%d\n", retval);
+		return retval;
+	}
+
+	/* Clear FIFO and route/enable stream block */
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.reg_count = 2;
+	cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
+	cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
+	cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
+	cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		CPIA2_VC_ST_CTRL_DST_USB |
+		CPIA2_VC_ST_CTRL_EOF_DETECT | CPIA2_VC_ST_CTRL_FIFO_ENABLE;
+
+	cpia2_send_command(cam, &cmd);
+
+	cpia2_set_high_power(cam);
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		/* Enable button notification */
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
+		cmd.buffer.registers[0].index = CPIA2_SYSTEM_INT_PACKET_CTRL;
+		cmd.buffer.registers[0].value =
+			CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX;
+		cmd.reg_count = 1;
+		cpia2_send_command(cam, &cmd);
+	}
+
+	schedule_timeout_interruptible(msecs_to_jiffies(100));
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		retval = apply_vp_patch(cam);
+
+	/* wait for vp to go to sleep */
+	schedule_timeout_interruptible(msecs_to_jiffies(100));
+
+	/***
+	 * If this is a 676, apply VP5 fixes before we start streaming
+	 ***/
+	if (cam->params.pnp_id.device_type == DEVICE_STV_676) {
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+
+		/* The following writes improve the picture */
+		cmd.buffer.registers[0].index = CPIA2_VP5_MYBLACK_LEVEL;
+		cmd.buffer.registers[0].value = 0; /* reduce from the default
+						    * rec 601 pedestal of 16 */
+		cmd.buffer.registers[1].index = CPIA2_VP5_MCYRANGE;
+		cmd.buffer.registers[1].value = 0x92; /* increase from 100% to
+						       * (256/256 - 31) to fill
+						       * available range */
+		cmd.buffer.registers[2].index = CPIA2_VP5_MYCEILING;
+		cmd.buffer.registers[2].value = 0xFF; /* Increase from the
+						       * default rec 601 ceiling
+						       * of 240 */
+		cmd.buffer.registers[3].index = CPIA2_VP5_MCUVSATURATION;
+		cmd.buffer.registers[3].value = 0xFF; /* Increase from the rec
+						       * 601 100% level (128)
+						       * to 145-192 */
+		cmd.buffer.registers[4].index = CPIA2_VP5_ANTIFLKRSETUP;
+		cmd.buffer.registers[4].value = 0x80;  /* Inhibit the
+							* anti-flicker */
+
+		/* The following 4 writes are a fix to allow QVGA to work at 30 fps */
+		cmd.buffer.registers[5].index = CPIA2_VP_RAM_ADDR_H;
+		cmd.buffer.registers[5].value = 0x01;
+		cmd.buffer.registers[6].index = CPIA2_VP_RAM_ADDR_L;
+		cmd.buffer.registers[6].value = 0xE3;
+		cmd.buffer.registers[7].index = CPIA2_VP_RAM_DATA;
+		cmd.buffer.registers[7].value = 0x02;
+		cmd.buffer.registers[8].index = CPIA2_VP_RAM_DATA;
+		cmd.buffer.registers[8].value = 0xFC;
+
+		cmd.direction = TRANSFER_WRITE;
+		cmd.reg_count = 9;
+
+		cpia2_send_command(cam, &cmd);
+	}
+
+	/* Activate all settings and start the data stream */
+	/* Set user mode */
+	set_default_user_mode(cam);
+
+	/* Give VP time to wake up */
+	schedule_timeout_interruptible(msecs_to_jiffies(100));
+
+	set_all_properties(cam);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
+	DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
+	    cam->params.vp_params.video_mode);
+
+	/***
+	 * Set audio regulator off.  This and the code to set the compresison
+	 * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
+	 * intertwined.  This stuff came straight from the windows driver.
+	 ***/
+	/* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
+	tmp_reg = cam->params.vp_params.system_ctrl;
+	cmd.buffer.registers[0].value = tmp_reg &
+		(tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
+	cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
+					CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
+	cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
+	cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+	cmd.reg_count = 2;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+
+	/* Set the correct I2C address in the CPiA-2 system register */
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_SERIAL_ADDR,
+			 TRANSFER_WRITE,
+			 CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);
+
+	/* Now have sensor access - set bit to turn the audio regulator off */
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_SENSOR_CR1,
+			 TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);
+
+	/* Set the correct I2C address in the CPiA-2 system register */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_SERIAL_ADDR,
+				 TRANSFER_WRITE,
+				 CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
+	else
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_SERIAL_ADDR,
+				 TRANSFER_WRITE,
+				 CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a
+
+	/* increase signal drive strength */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_VP_EXP_MODES,
+				 TRANSFER_WRITE,
+				 CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);
+
+	/* Start autoexposure */
+	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
+	cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
+				  (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
+	cmd.buffer.registers[1].value =
+	    cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;
+
+	cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
+	cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+	cmd.reg_count = 2;
+	cmd.direction = TRANSFER_WRITE;
+
+	cpia2_send_command(cam, &cmd);
+
+	/* Set compression state */
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
+	if (cam->params.compression.inhibit_htables) {
+		tmp_reg = cam->params.vc_params.vc_control |
+			  CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
+	} else  {
+		tmp_reg = cam->params.vc_params.vc_control &
+			  ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
+	}
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	/* Set target size (kb) on vc
+	   This is a heuristic based on the quality parameter and the raw
+	   framesize in kB divided by 16 (the compression factor when the
+	   quality is 100%) */
+	target_kb = (cam->width * cam->height * 2 / 16384) *
+				cam->params.vc_params.quality / 100;
+	if (target_kb < 1)
+		target_kb = 1;
+	cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
+			 TRANSFER_WRITE, target_kb);
+
+	/* Wiggle VC Reset */
+	/***
+	 * First read and wait a bit.
+	 ***/
+	for (i = 0; i < 50; i++) {
+		cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
+				 TRANSFER_READ, 0);
+	}
+
+	tmp_reg = cam->params.vc_params.pw_control;
+	tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;
+
+	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
+	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
+	DBG("After VC RESET, user mode is 0x%0X\n",
+	    cam->params.vp_params.video_mode);
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_high_power
+ *
+ *****************************************************************************/
+static int cpia2_set_high_power(struct camera_data *cam)
+{
+	int i;
+	for (i = 0; i <= 50; i++) {
+		/* Read system status */
+		cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);
+
+		/* If there is an error, clear it */
+		if(cam->params.camera_state.system_ctrl &
+		   CPIA2_SYSTEM_CONTROL_V2W_ERR)
+			cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
+					 TRANSFER_WRITE, 0);
+
+		/* Try to set high power mode */
+		cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
+				 TRANSFER_WRITE, 1);
+
+		/* Try to read something in VP to check if everything is awake */
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
+				 TRANSFER_READ, 0);
+		if (cam->params.vp_params.system_state &
+		    CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
+			break;
+		} else if (i == 50) {
+			cam->params.camera_state.power_mode = LO_POWER_MODE;
+			ERR("Camera did not wake up\n");
+			return -EIO;
+		}
+	}
+
+	DBG("System now in high power state\n");
+	cam->params.camera_state.power_mode = HI_POWER_MODE;
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_low_power
+ *
+ *****************************************************************************/
+int cpia2_set_low_power(struct camera_data *cam)
+{
+	cam->params.camera_state.power_mode = LO_POWER_MODE;
+	cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  apply_vp_patch
+ *
+ *****************************************************************************/
+static int cpia2_send_onebyte_command(struct camera_data *cam,
+				      struct cpia2_command *cmd,
+				      u8 start, u8 datum)
+{
+	cmd->buffer.block_data[0] = datum;
+	cmd->start = start;
+	cmd->reg_count = 1;
+	return cpia2_send_command(cam, cmd);
+}
+
+static int apply_vp_patch(struct camera_data *cam)
+{
+	const struct firmware *fw;
+	const char fw_name[] = FIRMWARE;
+	int i, ret;
+	struct cpia2_command cmd;
+
+	ret = request_firmware(&fw, fw_name, &cam->dev->dev);
+	if (ret) {
+		printk(KERN_ERR "cpia2: failed to load VP patch \"%s\"\n",
+		       fw_name);
+		return ret;
+	}
+
+	cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
+	cmd.direction = TRANSFER_WRITE;
+
+	/* First send the start address... */
+	cpia2_send_onebyte_command(cam, &cmd, 0x0A, fw->data[0]); /* hi */
+	cpia2_send_onebyte_command(cam, &cmd, 0x0B, fw->data[1]); /* lo */
+
+	/* ... followed by the data payload */
+	for (i = 2; i < fw->size; i += 64) {
+		cmd.start = 0x0C; /* Data */
+		cmd.reg_count = min_t(uint, 64, fw->size - i);
+		memcpy(cmd.buffer.block_data, &fw->data[i], cmd.reg_count);
+		cpia2_send_command(cam, &cmd);
+	}
+
+	/* Next send the start address... */
+	cpia2_send_onebyte_command(cam, &cmd, 0x0A, fw->data[0]); /* hi */
+	cpia2_send_onebyte_command(cam, &cmd, 0x0B, fw->data[1]); /* lo */
+
+	/* ... followed by the 'goto' command */
+	cpia2_send_onebyte_command(cam, &cmd, 0x0D, 1);
+
+	release_firmware(fw);
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  set_default_user_mode
+ *
+ *****************************************************************************/
+static int set_default_user_mode(struct camera_data *cam)
+{
+	unsigned char user_mode;
+	unsigned char frame_rate;
+	int width = cam->params.roi.width;
+	int height = cam->params.roi.height;
+
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+	case CPIA2_VP_SENSOR_FLAGS_407:
+	case CPIA2_VP_SENSOR_FLAGS_409:
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		if ((width > STV_IMAGE_QCIF_COLS)
+		    || (height > STV_IMAGE_QCIF_ROWS)) {
+			user_mode = CPIA2_VP_USER_MODE_CIF;
+		} else {
+			user_mode = CPIA2_VP_USER_MODE_QCIFDS;
+		}
+		frame_rate = CPIA2_VP_FRAMERATE_30;
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		if ((width > STV_IMAGE_CIF_COLS)
+		    || (height > STV_IMAGE_CIF_ROWS)) {
+			user_mode = CPIA2_VP_USER_MODE_VGA;
+		} else {
+			user_mode = CPIA2_VP_USER_MODE_QVGADS;
+		}
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+			frame_rate = CPIA2_VP_FRAMERATE_15;
+		else
+			frame_rate = CPIA2_VP_FRAMERATE_30;
+		break;
+	default:
+		LOG("%s: Invalid sensor flag value 0x%0X\n",__func__,
+		    cam->params.version.sensor_flags);
+		return -EINVAL;
+	}
+
+	DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
+	    cam->params.version.sensor_flags, user_mode, frame_rate);
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
+			 user_mode);
+	if(cam->params.vp_params.frame_rate > 0 &&
+	   frame_rate > cam->params.vp_params.frame_rate)
+		frame_rate = cam->params.vp_params.frame_rate;
+
+	cpia2_set_fps(cam, frame_rate);
+
+//	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
+//		cpia2_do_command(cam,
+//				 CPIA2_CMD_SET_VP_SYSTEM_CTRL,
+//				 TRANSFER_WRITE,
+//				 CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
+//				 CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_match_video_size
+ *
+ *  return the best match, where 'best' is as always
+ *  the largest that is not bigger than what is requested.
+ *****************************************************************************/
+int cpia2_match_video_size(int width, int height)
+{
+	if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
+		return VIDEOSIZE_VGA;
+
+	if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
+		return VIDEOSIZE_CIF;
+
+	if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
+		return VIDEOSIZE_QVGA;
+
+	if (width >= 288 && height >= 216)
+		return VIDEOSIZE_288_216;
+
+	if (width >= 256 && height >= 192)
+		return VIDEOSIZE_256_192;
+
+	if (width >= 224 && height >= 168)
+		return VIDEOSIZE_224_168;
+
+	if (width >= 192 && height >= 144)
+		return VIDEOSIZE_192_144;
+
+	if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
+		return VIDEOSIZE_QCIF;
+
+	return -1;
+}
+
+/******************************************************************************
+ *
+ *  SetVideoSize
+ *
+ *****************************************************************************/
+static int set_vw_size(struct camera_data *cam, int size)
+{
+	int retval = 0;
+
+	cam->params.vp_params.video_size = size;
+
+	switch (size) {
+	case VIDEOSIZE_VGA:
+		DBG("Setting size to VGA\n");
+		cam->params.roi.width = STV_IMAGE_VGA_COLS;
+		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
+		cam->width = STV_IMAGE_VGA_COLS;
+		cam->height = STV_IMAGE_VGA_ROWS;
+		break;
+	case VIDEOSIZE_CIF:
+		DBG("Setting size to CIF\n");
+		cam->params.roi.width = STV_IMAGE_CIF_COLS;
+		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
+		cam->width = STV_IMAGE_CIF_COLS;
+		cam->height = STV_IMAGE_CIF_ROWS;
+		break;
+	case VIDEOSIZE_QVGA:
+		DBG("Setting size to QVGA\n");
+		cam->params.roi.width = STV_IMAGE_QVGA_COLS;
+		cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
+		cam->width = STV_IMAGE_QVGA_COLS;
+		cam->height = STV_IMAGE_QVGA_ROWS;
+		break;
+	case VIDEOSIZE_288_216:
+		cam->params.roi.width = 288;
+		cam->params.roi.height = 216;
+		cam->width = 288;
+		cam->height = 216;
+		break;
+	case VIDEOSIZE_256_192:
+		cam->width = 256;
+		cam->height = 192;
+		cam->params.roi.width = 256;
+		cam->params.roi.height = 192;
+		break;
+	case VIDEOSIZE_224_168:
+		cam->width = 224;
+		cam->height = 168;
+		cam->params.roi.width = 224;
+		cam->params.roi.height = 168;
+		break;
+	case VIDEOSIZE_192_144:
+		cam->width = 192;
+		cam->height = 144;
+		cam->params.roi.width = 192;
+		cam->params.roi.height = 144;
+		break;
+	case VIDEOSIZE_QCIF:
+		DBG("Setting size to QCIF\n");
+		cam->params.roi.width = STV_IMAGE_QCIF_COLS;
+		cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
+		cam->width = STV_IMAGE_QCIF_COLS;
+		cam->height = STV_IMAGE_QCIF_ROWS;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  configure_sensor
+ *
+ *****************************************************************************/
+static int configure_sensor(struct camera_data *cam,
+			    int req_width, int req_height)
+{
+	int retval;
+
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+	case CPIA2_VP_SENSOR_FLAGS_407:
+	case CPIA2_VP_SENSOR_FLAGS_409:
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		retval = config_sensor_410(cam, req_width, req_height);
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		retval = config_sensor_500(cam, req_width, req_height);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  config_sensor_410
+ *
+ *****************************************************************************/
+static int config_sensor_410(struct camera_data *cam,
+			    int req_width, int req_height)
+{
+	struct cpia2_command cmd;
+	int i = 0;
+	int image_size;
+	int image_type;
+	int width = req_width;
+	int height = req_height;
+
+	/***
+	 *  Make sure size doesn't exceed CIF.
+	 ***/
+	if (width > STV_IMAGE_CIF_COLS)
+		width = STV_IMAGE_CIF_COLS;
+	if (height > STV_IMAGE_CIF_ROWS)
+		height = STV_IMAGE_CIF_ROWS;
+
+	image_size = cpia2_match_video_size(width, height);
+
+	DBG("Config 410: width = %d, height = %d\n", width, height);
+	DBG("Image size returned is %d\n", image_size);
+	if (image_size >= 0) {
+		set_vw_size(cam, image_size);
+		width = cam->params.roi.width;
+		height = cam->params.roi.height;
+
+		DBG("After set_vw_size(), width = %d, height = %d\n",
+		    width, height);
+		if (width <= 176 && height <= 144) {
+			DBG("image type = VIDEOSIZE_QCIF\n");
+			image_type = VIDEOSIZE_QCIF;
+		}
+		else if (width <= 320 && height <= 240) {
+			DBG("image type = VIDEOSIZE_QVGA\n");
+			image_type = VIDEOSIZE_QVGA;
+		}
+		else {
+			DBG("image type = VIDEOSIZE_CIF\n");
+			image_type = VIDEOSIZE_CIF;
+		}
+	} else {
+		ERR("ConfigSensor410 failed\n");
+		return -EINVAL;
+	}
+
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+
+	/* VC Format */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
+	if (image_type == VIDEOSIZE_CIF) {
+		cmd.buffer.registers[i++].value =
+		    (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
+			  CPIA2_VC_VC_FORMAT_SHORTLINE);
+	} else {
+		cmd.buffer.registers[i++].value =
+		    (u8) CPIA2_VC_VC_FORMAT_UFIRST;
+	}
+
+	/* VC Clocks */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
+	if (image_type == VIDEOSIZE_QCIF) {
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+			cmd.buffer.registers[i++].value=
+				(u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
+				     CPIA2_VC_VC_672_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+			DBG("VC_Clocks (0xc4) should be B\n");
+		}
+		else {
+			cmd.buffer.registers[i++].value=
+				(u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+		}
+	} else {
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+			cmd.buffer.registers[i++].value =
+			   (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
+				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
+		}
+		else {
+			cmd.buffer.registers[i++].value =
+			   (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
+				 CPIA2_VC_VC_676_CLOCKS_SCALING |
+				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
+		}
+	}
+	DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);
+
+	/* Input reqWidth from VC */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (STV_IMAGE_QCIF_COLS / 4);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (STV_IMAGE_CIF_COLS / 4);
+
+	/* Timings */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 208;
+	else
+		cmd.buffer.registers[i++].value = (u8) 160;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 160;
+	else
+		cmd.buffer.registers[i++].value = (u8) 64;
+
+	/* Output Image Size */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
+	cmd.buffer.registers[i++].value = cam->params.roi.width / 4;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
+	cmd.buffer.registers[i++].value = cam->params.roi.height / 4;
+
+	/* Cropping */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
+
+	/* Scaling registers (defaults) */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
+	cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
+	cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
+	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
+	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
+
+	cmd.reg_count = i;
+
+	cpia2_send_command(cam, &cmd);
+
+	return i;
+}
+
+
+/******************************************************************************
+ *
+ *  config_sensor_500(cam)
+ *
+ *****************************************************************************/
+static int config_sensor_500(struct camera_data *cam,
+			     int req_width, int req_height)
+{
+	struct cpia2_command cmd;
+	int i = 0;
+	int image_size = VIDEOSIZE_CIF;
+	int image_type = VIDEOSIZE_VGA;
+	int width = req_width;
+	int height = req_height;
+	unsigned int device = cam->params.pnp_id.device_type;
+
+	image_size = cpia2_match_video_size(width, height);
+
+	if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
+		image_type = VIDEOSIZE_VGA;
+	else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
+		image_type = VIDEOSIZE_CIF;
+	else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
+		image_type = VIDEOSIZE_QVGA;
+	else
+		image_type = VIDEOSIZE_QCIF;
+
+	if (image_size >= 0) {
+		set_vw_size(cam, image_size);
+		width = cam->params.roi.width;
+		height = cam->params.roi.height;
+	} else {
+		ERR("ConfigSensor500 failed\n");
+		return -EINVAL;
+	}
+
+	DBG("image_size = %d, width = %d, height = %d, type = %d\n",
+	    image_size, width, height, image_type);
+
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+	i = 0;
+
+	/* VC Format */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
+	cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
+	i++;
+
+	/* VC Clocks */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
+	if (device == DEVICE_STV_672) {
+		if (image_type == VIDEOSIZE_VGA)
+			cmd.buffer.registers[i].value =
+				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
+		else
+			cmd.buffer.registers[i].value =
+				(u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV3);
+	} else {
+		if (image_type == VIDEOSIZE_VGA)
+			cmd.buffer.registers[i].value =
+				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
+		else
+			cmd.buffer.registers[i].value =
+				(u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+	}
+	i++;
+
+	DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);
+
+	/* Input width from VP */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i].value =
+		    (u8) (STV_IMAGE_VGA_COLS / 4);
+	else
+		cmd.buffer.registers[i].value =
+		    (u8) (STV_IMAGE_QVGA_COLS / 4);
+	i++;
+	DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);
+
+	/* Timings */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 2;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 250;
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value = (u8) 125;
+	else
+		cmd.buffer.registers[i++].value = (u8) 160;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 2;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 12;
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value = (u8) 64;
+	else
+		cmd.buffer.registers[i++].value = (u8) 6;
+
+	/* Output Image Size */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
+	else
+		cmd.buffer.registers[i++].value = width / 4;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
+	else
+		cmd.buffer.registers[i++].value = height / 4;
+
+	/* Cropping */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
+	else if (image_type == VIDEOSIZE_CIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
+	else /*if (image_type == VIDEOSIZE_QCIF)*/
+		cmd.buffer.registers[i++].value =
+			(u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
+	else if (image_type == VIDEOSIZE_CIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
+	else /*if (image_type == VIDEOSIZE_QCIF)*/
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
+
+	/* Scaling registers (defaults) */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 36;
+	else
+		cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 32;
+	else
+		cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 26;
+	else
+		cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 21;
+	else
+		cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0x2B;	/* 2/11 */
+	else
+		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0x13;	/* 1/3 */
+	else
+		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
+
+	cmd.reg_count = i;
+
+	cpia2_send_command(cam, &cmd);
+
+	return i;
+}
+
+
+/******************************************************************************
+ *
+ *  setallproperties
+ *
+ *  This sets all user changeable properties to the values in cam->params.
+ *****************************************************************************/
+static int set_all_properties(struct camera_data *cam)
+{
+	/**
+	 * Don't set target_kb here, it will be set later.
+	 * framerate and user_mode were already set (set_default_user_mode).
+	 **/
+
+	cpia2_usb_change_streaming_alternate(cam,
+					  cam->params.camera_state.stream_mode);
+
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			 TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
+			 cam->params.vp_params.gpio_data);
+
+	v4l2_ctrl_handler_setup(&cam->hdl);
+
+	wake_system(cam);
+
+	set_lowlight_boost(cam);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_save_camera_state
+ *
+ *****************************************************************************/
+void cpia2_save_camera_state(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
+			 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
+	/* Don't get framerate or target_kb. Trust the values we already have */
+}
+
+
+/******************************************************************************
+ *
+ *  cpia2_set_flicker_mode
+ *
+ *****************************************************************************/
+int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
+{
+	unsigned char cam_reg;
+	int err = 0;
+
+	if(cam->params.pnp_id.device_type != DEVICE_STV_672)
+		return -EINVAL;
+
+	/* Set the appropriate bits in FLICKER_MODES, preserving the rest */
+	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
+				   TRANSFER_READ, 0)))
+		return err;
+	cam_reg = cam->params.flicker_control.cam_register;
+
+	switch(mode) {
+	case NEVER_FLICKER:
+		cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	case FLICKER_60:
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	case FLICKER_50:
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
+				   TRANSFER_WRITE, cam_reg)))
+		return err;
+
+	/* Set the appropriate bits in EXP_MODES, preserving the rest */
+	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
+				   TRANSFER_READ, 0)))
+		return err;
+	cam_reg = cam->params.vp_params.exposure_modes;
+
+	if (mode == NEVER_FLICKER) {
+		cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
+	} else {
+		cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
+	}
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
+				   TRANSFER_WRITE, cam_reg)))
+		return err;
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
+				   TRANSFER_WRITE, 1)))
+		return err;
+
+	switch(mode) {
+	case NEVER_FLICKER:
+	case FLICKER_60:
+	case FLICKER_50:
+		cam->params.flicker_control.flicker_mode_req = mode;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_property_flip
+ *
+ *****************************************************************************/
+void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
+{
+	unsigned char cam_reg;
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cam_reg = cam->params.vp_params.user_effects;
+
+	if (prop_val)
+	{
+		cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
+	}
+	else
+	{
+		cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
+	}
+	cam->params.vp_params.user_effects = cam_reg;
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+			 cam_reg);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_property_mirror
+ *
+ *****************************************************************************/
+void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
+{
+	unsigned char cam_reg;
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cam_reg = cam->params.vp_params.user_effects;
+
+	if (prop_val)
+	{
+		cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
+	}
+	else
+	{
+		cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
+	}
+	cam->params.vp_params.user_effects = cam_reg;
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+			 cam_reg);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_gpio
+ *
+ *****************************************************************************/
+int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
+{
+	int ret;
+
+	/* Set the microport direction (register 0x90, should be defined
+	 * already) to 1 (user output), and set the microport data (0x91) to
+	 * the value in the ioctl argument.
+	 */
+
+	ret = cpia2_do_command(cam,
+			       CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			       CPIA2_VC_MP_DIR_OUTPUT,
+			       255);
+	if (ret < 0)
+		return ret;
+	cam->params.vp_params.gpio_direction = 255;
+
+	ret = cpia2_do_command(cam,
+			       CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+			       CPIA2_VC_MP_DIR_OUTPUT,
+			       setting);
+	if (ret < 0)
+		return ret;
+	cam->params.vp_params.gpio_data = setting;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_fps
+ *
+ *****************************************************************************/
+int cpia2_set_fps(struct camera_data *cam, int framerate)
+{
+	int retval;
+
+	switch(framerate) {
+		case CPIA2_VP_FRAMERATE_30:
+		case CPIA2_VP_FRAMERATE_25:
+			if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+			   cam->params.version.sensor_flags ==
+						    CPIA2_VP_SENSOR_FLAGS_500) {
+				return -EINVAL;
+			}
+			fallthrough;
+		case CPIA2_VP_FRAMERATE_15:
+		case CPIA2_VP_FRAMERATE_12_5:
+		case CPIA2_VP_FRAMERATE_7_5:
+		case CPIA2_VP_FRAMERATE_6_25:
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    framerate == CPIA2_VP_FRAMERATE_15)
+		framerate = 0; /* Work around bug in VP4 */
+
+	retval = cpia2_do_command(cam,
+				 CPIA2_CMD_FRAMERATE_REQ,
+				 TRANSFER_WRITE,
+				 framerate);
+
+	if(retval == 0)
+		cam->params.vp_params.frame_rate = framerate;
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_brightness
+ *
+ *****************************************************************************/
+void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
+{
+	/***
+	 * Don't let the register be set to zero - bug in VP4 - flash of full
+	 * brightness
+	 ***/
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
+		value++;
+	DBG("Setting brightness to %d (0x%0x)\n", value, value);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE, value);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_contrast
+ *
+ *****************************************************************************/
+void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
+{
+	DBG("Setting contrast to %d (0x%0x)\n", value, value);
+	cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_saturation
+ *
+ *****************************************************************************/
+void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
+{
+	DBG("Setting saturation to %d (0x%0x)\n", value, value);
+	cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
+}
+
+/******************************************************************************
+ *
+ *  wake_system
+ *
+ *****************************************************************************/
+static void wake_system(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
+}
+
+/******************************************************************************
+ *
+ *  set_lowlight_boost
+ *
+ *  Valid for STV500 sensor only
+ *****************************************************************************/
+static void set_lowlight_boost(struct camera_data *cam)
+{
+	struct cpia2_command cmd;
+
+	if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
+	    cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
+		return;
+
+	cmd.direction = TRANSFER_WRITE;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+	cmd.reg_count = 3;
+	cmd.start = CPIA2_VP_RAM_ADDR_H;
+
+	cmd.buffer.block_data[0] = 0;	/* High byte of address to write to */
+	cmd.buffer.block_data[1] = 0x59;	/* Low byte of address to write to */
+	cmd.buffer.block_data[2] = 0;	/* High byte of data to write */
+
+	cpia2_send_command(cam, &cmd);
+
+	if (cam->params.vp_params.lowlight_boost) {
+		cmd.buffer.block_data[0] = 0x02;	/* Low byte data to write */
+	} else {
+		cmd.buffer.block_data[0] = 0x06;
+	}
+	cmd.start = CPIA2_VP_RAM_DATA;
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+
+	/* Rehash the VP4 values */
+	cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_format
+ *
+ *  Assumes that new size is already set in param struct.
+ *****************************************************************************/
+void cpia2_set_format(struct camera_data *cam)
+{
+	cam->flush = true;
+
+	cpia2_usb_stream_pause(cam);
+
+	/* reset camera to new size */
+	cpia2_set_low_power(cam);
+	cpia2_reset_camera(cam);
+	cam->flush = false;
+
+	cpia2_dbg_dump_registers(cam);
+
+	cpia2_usb_stream_resume(cam);
+}
+
+/******************************************************************************
+ *
+ * cpia2_dbg_dump_registers
+ *
+ *****************************************************************************/
+void cpia2_dbg_dump_registers(struct camera_data *cam)
+{
+#ifdef _CPIA2_DEBUG_
+	struct cpia2_command cmd;
+
+	if (!(debugs_on & DEBUG_DUMP_REGS))
+		return;
+
+	cmd.direction = TRANSFER_READ;
+
+	/* Start with bank 0 (SYSTEM) */
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+	cmd.reg_count = 3;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "System_system control = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+
+	/* Bank 1 (VC) */
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.reg_count = 4;
+	cmd.start = 0x80;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+	printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
+	       cmd.buffer.block_data[3]);
+
+	cmd.start = 0xA0;	/* ST_CTRL */
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xA4;	/* Stream status */
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "Stream status = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xA8;	/* USB status */
+	cmd.reg_count = 3;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+
+	cmd.start = 0xAF;	/* USB settings */
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "USB settings  = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xC0;	/* VC stuff */
+	cmd.reg_count = 26;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "VC Control    = 0x%0X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "VC Format     = 0x%0X\n",
+	       cmd.buffer.block_data[3]);
+	printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
+	       cmd.buffer.block_data[4]);
+	printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
+	       cmd.buffer.block_data[5]);
+	printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
+	       cmd.buffer.block_data[6]);
+	printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
+	       cmd.buffer.block_data[7]);
+	printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
+	       cmd.buffer.block_data[8]);
+	printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
+	       cmd.buffer.block_data[9]);
+	printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
+	       cmd.buffer.block_data[10]);
+	printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
+	       cmd.buffer.block_data[11]);
+	printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
+	       cmd.buffer.block_data[12]);
+	printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
+	       cmd.buffer.block_data[13]);
+	printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
+	       cmd.buffer.block_data[14]);
+	printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
+	       cmd.buffer.block_data[15]);
+	printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
+	       cmd.buffer.block_data[16]);
+	printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
+	       cmd.buffer.block_data[17]);
+	printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
+	       cmd.buffer.block_data[18]);
+	printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
+	       cmd.buffer.block_data[19]);
+	printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
+	       cmd.buffer.block_data[20]);
+	printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
+	       cmd.buffer.block_data[21]);
+	printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
+	       cmd.buffer.block_data[22]);
+	printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
+	       cmd.buffer.block_data[23]);
+	printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
+	       cmd.buffer.block_data[24]);
+	printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
+	       cmd.buffer.block_data[25]);
+
+	/*** VP ***/
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+	cmd.reg_count = 14;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+
+	printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
+	       cmd.buffer.block_data[2]);
+	printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
+	       cmd.buffer.block_data[3]);
+	printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
+	       cmd.buffer.block_data[5]);
+	printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
+	       cmd.buffer.block_data[6]);
+	printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
+	       cmd.buffer.block_data[7]);
+	printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
+	       cmd.buffer.block_data[8]);
+	printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
+	       cmd.buffer.block_data[9]);
+	printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
+	       cmd.buffer.block_data[10]);
+	printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
+	       cmd.buffer.block_data[11]);
+	printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
+	       cmd.buffer.block_data[12]);
+	printk(KERN_DEBUG "Do Call       = 0x%0X\n",
+	       cmd.buffer.block_data[13]);
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		cmd.reg_count = 9;
+		cmd.start = 0x0E;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
+		       cmd.buffer.block_data[2]);
+		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
+		       cmd.buffer.block_data[3]);
+		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
+		       cmd.buffer.block_data[4]);
+		printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
+		       cmd.buffer.block_data[5]);
+		printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
+		       cmd.buffer.block_data[6]);
+		printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
+		       cmd.buffer.block_data[7]);
+		printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
+		       cmd.buffer.block_data[8]);
+
+		cmd.reg_count = 1;
+		cmd.start = 0x1B;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+	} else {
+		cmd.reg_count = 8 ;
+		cmd.start = 0x0E;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
+		       cmd.buffer.block_data[5]);
+		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
+		       cmd.buffer.block_data[6]);
+		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
+		       cmd.buffer.block_data[7]);
+
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+
+		cmd.reg_count = 4;
+		cmd.start = 0x3A;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
+		       cmd.buffer.block_data[2]);
+		printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
+		       cmd.buffer.block_data[3]);
+	}
+#endif
+}
+
+/******************************************************************************
+ *
+ *  reset_camera_struct
+ *
+ *  Sets all values to the defaults
+ *****************************************************************************/
+static void reset_camera_struct(struct camera_data *cam)
+{
+	/***
+	 * The following parameter values are the defaults from the register map.
+	 ***/
+	cam->params.vp_params.lowlight_boost = 0;
+
+	/* FlickerModes */
+	cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
+
+	/* jpeg params */
+	cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
+	cam->params.compression.creep_period = 2;
+	cam->params.compression.user_squeeze = 20;
+	cam->params.compression.inhibit_htables = false;
+
+	/* gpio params */
+	cam->params.vp_params.gpio_direction = 0;	/* write, the default safe mode */
+	cam->params.vp_params.gpio_data = 0;
+
+	/* Target kb params */
+	cam->params.vc_params.quality = 100;
+
+	/***
+	 * Set Sensor FPS as fast as possible.
+	 ***/
+	if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
+		else
+			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
+	} else {
+		cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
+	}
+
+	/***
+	 * Set default video mode as large as possible :
+	 * for vga sensor set to vga, for cif sensor set to CIF.
+	 ***/
+	if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
+		cam->sensor_type = CPIA2_SENSOR_500;
+		cam->video_size = VIDEOSIZE_VGA;
+		cam->params.roi.width = STV_IMAGE_VGA_COLS;
+		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
+	} else {
+		cam->sensor_type = CPIA2_SENSOR_410;
+		cam->video_size = VIDEOSIZE_CIF;
+		cam->params.roi.width = STV_IMAGE_CIF_COLS;
+		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
+	}
+
+	cam->width = cam->params.roi.width;
+	cam->height = cam->params.roi.height;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_init_camera_struct
+ *
+ *  Deinitialize camera struct
+ *****************************************************************************/
+void cpia2_deinit_camera_struct(struct camera_data *cam, struct usb_interface *intf)
+{
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_init_camera_struct
+ *
+ *  Initializes camera struct, does not call reset to fill in defaults.
+ *****************************************************************************/
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf)
+{
+	struct camera_data *cam;
+
+	cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+
+	if (!cam) {
+		ERR("couldn't kmalloc cpia2 struct\n");
+		return NULL;
+	}
+
+	cam->v4l2_dev.release = cpia2_camera_release;
+	if (v4l2_device_register(&intf->dev, &cam->v4l2_dev) < 0) {
+		v4l2_err(&cam->v4l2_dev, "couldn't register v4l2_device\n");
+		kfree(cam);
+		return NULL;
+	}
+
+	mutex_init(&cam->v4l2_lock);
+	init_waitqueue_head(&cam->wq_stream);
+
+	return cam;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_init_camera
+ *
+ *  Initializes camera.
+ *****************************************************************************/
+int cpia2_init_camera(struct camera_data *cam)
+{
+	DBG("Start\n");
+
+	cam->mmapped = false;
+
+	/* Get sensor and asic types before reset. */
+	cpia2_set_high_power(cam);
+	cpia2_get_version_info(cam);
+	if (cam->params.version.asic_id != CPIA2_ASIC_672) {
+		ERR("Device IO error (asicID has incorrect value of 0x%X\n",
+		    cam->params.version.asic_id);
+		return -ENODEV;
+	}
+
+	/* Set GPIO direction and data to a safe state. */
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			 TRANSFER_WRITE, 0);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+			 TRANSFER_WRITE, 0);
+
+	/* resetting struct requires version info for sensor and asic types */
+	reset_camera_struct(cam);
+
+	cpia2_set_low_power(cam);
+
+	DBG("End\n");
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_allocate_buffers
+ *
+ *****************************************************************************/
+int cpia2_allocate_buffers(struct camera_data *cam)
+{
+	int i;
+
+	if(!cam->buffers) {
+		u32 size = cam->num_frames*sizeof(struct framebuf);
+		cam->buffers = kmalloc(size, GFP_KERNEL);
+		if(!cam->buffers) {
+			ERR("couldn't kmalloc frame buffer structures\n");
+			return -ENOMEM;
+		}
+	}
+
+	if(!cam->frame_buffer) {
+		cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
+		if (!cam->frame_buffer) {
+			ERR("couldn't vmalloc frame buffer data area\n");
+			kfree(cam->buffers);
+			cam->buffers = NULL;
+			return -ENOMEM;
+		}
+	}
+
+	for(i=0; i<cam->num_frames-1; ++i) {
+		cam->buffers[i].next = &cam->buffers[i+1];
+		cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
+		cam->buffers[i].status = FRAME_EMPTY;
+		cam->buffers[i].length = 0;
+		cam->buffers[i].max_length = 0;
+		cam->buffers[i].num = i;
+	}
+	cam->buffers[i].next = cam->buffers;
+	cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
+	cam->buffers[i].status = FRAME_EMPTY;
+	cam->buffers[i].length = 0;
+	cam->buffers[i].max_length = 0;
+	cam->buffers[i].num = i;
+	cam->curbuff = cam->buffers;
+	cam->workbuff = cam->curbuff->next;
+	DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
+	    cam->workbuff);
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_free_buffers
+ *
+ *****************************************************************************/
+void cpia2_free_buffers(struct camera_data *cam)
+{
+	if(cam->buffers) {
+		kfree(cam->buffers);
+		cam->buffers = NULL;
+	}
+	if(cam->frame_buffer) {
+		rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
+		cam->frame_buffer = NULL;
+	}
+}
+
+/******************************************************************************
+ *
+ *  cpia2_read
+ *
+ *****************************************************************************/
+long cpia2_read(struct camera_data *cam,
+		char __user *buf, unsigned long count, int noblock)
+{
+	struct framebuf *frame;
+
+	if (!count)
+		return 0;
+
+	if (!buf) {
+		ERR("%s: buffer NULL\n",__func__);
+		return -EINVAL;
+	}
+
+	if (!cam) {
+		ERR("%s: Internal error, camera_data NULL!\n",__func__);
+		return -EINVAL;
+	}
+
+	if (!cam->streaming) {
+		/* Start streaming */
+		cpia2_usb_stream_start(cam,
+				       cam->params.camera_state.stream_mode);
+	}
+
+	/* Copy cam->curbuff in case it changes while we're processing */
+	frame = cam->curbuff;
+	if (noblock && frame->status != FRAME_READY) {
+		return -EAGAIN;
+	}
+
+	if (frame->status != FRAME_READY) {
+		mutex_unlock(&cam->v4l2_lock);
+		wait_event_interruptible(cam->wq_stream,
+			       !video_is_registered(&cam->vdev) ||
+			       (frame = cam->curbuff)->status == FRAME_READY);
+		mutex_lock(&cam->v4l2_lock);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		if (!video_is_registered(&cam->vdev))
+			return 0;
+	}
+
+	/* copy data to user space */
+	if (frame->length > count)
+		return -EFAULT;
+	if (copy_to_user(buf, frame->data, frame->length))
+		return -EFAULT;
+
+	count = frame->length;
+
+	frame->status = FRAME_EMPTY;
+
+	return count;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_poll
+ *
+ *****************************************************************************/
+__poll_t cpia2_poll(struct camera_data *cam, struct file *filp,
+			poll_table *wait)
+{
+	__poll_t status = v4l2_ctrl_poll(filp, wait);
+
+	if ((poll_requested_events(wait) & (EPOLLIN | EPOLLRDNORM)) &&
+			!cam->streaming) {
+		/* Start streaming */
+		cpia2_usb_stream_start(cam,
+				       cam->params.camera_state.stream_mode);
+	}
+
+	poll_wait(filp, &cam->wq_stream, wait);
+
+	if (cam->curbuff->status == FRAME_READY)
+		status |= EPOLLIN | EPOLLRDNORM;
+
+	return status;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_remap_buffer
+ *
+ *****************************************************************************/
+int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
+{
+	const char *adr = (const char *)vma->vm_start;
+	unsigned long size = vma->vm_end-vma->vm_start;
+	unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long start = (unsigned long) adr;
+	unsigned long page, pos;
+
+	DBG("mmap offset:%ld size:%ld\n", start_offset, size);
+
+	if (!video_is_registered(&cam->vdev))
+		return -ENODEV;
+
+	if (size > cam->frame_size*cam->num_frames  ||
+	    (start_offset % cam->frame_size) != 0 ||
+	    (start_offset+size > cam->frame_size*cam->num_frames))
+		return -EINVAL;
+
+	pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
+	while (size > 0) {
+		page = kvirt_to_pa(pos);
+		if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED))
+			return -EAGAIN;
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+		if (size > PAGE_SIZE)
+			size -= PAGE_SIZE;
+		else
+			size = 0;
+	}
+
+	cam->mmapped = true;
+	return 0;
+}
diff --git a/drivers/staging/media/deprecated/cpia2/cpia2_registers.h b/drivers/staging/media/deprecated/cpia2/cpia2_registers.h
new file mode 100644
index 000000000..8c73812a1
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/cpia2_registers.h
@@ -0,0 +1,463 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/****************************************************************************
+ *
+ *  Filename: cpia2registers.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Description:
+ *     Definitions for the CPia2 register set
+ *
+ ****************************************************************************/
+
+#ifndef CPIA2_REGISTER_HEADER
+#define CPIA2_REGISTER_HEADER
+
+/***
+ * System register set (Bank 0)
+ ***/
+#define CPIA2_SYSTEM_DEVICE_HI                     0x00
+#define CPIA2_SYSTEM_DEVICE_LO                     0x01
+
+#define CPIA2_SYSTEM_SYSTEM_CONTROL                0x02
+#define CPIA2_SYSTEM_CONTROL_LOW_POWER       0x00
+#define CPIA2_SYSTEM_CONTROL_HIGH_POWER      0x01
+#define CPIA2_SYSTEM_CONTROL_SUSPEND         0x02
+#define CPIA2_SYSTEM_CONTROL_V2W_ERR         0x10
+#define CPIA2_SYSTEM_CONTROL_RB_ERR          0x10
+#define CPIA2_SYSTEM_CONTROL_CLEAR_ERR       0x80
+
+#define CPIA2_SYSTEM_INT_PACKET_CTRL                0x04
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX 0x01
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_EOF   0x02
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_INT1  0x04
+
+#define CPIA2_SYSTEM_CACHE_CTRL                     0x05
+#define CPIA2_SYSTEM_CACHE_CTRL_CACHE_RESET      0x01
+#define CPIA2_SYSTEM_CACHE_CTRL_CACHE_FLUSH      0x02
+
+#define CPIA2_SYSTEM_SERIAL_CTRL                    0x06
+#define CPIA2_SYSTEM_SERIAL_CTRL_NULL_CMD        0x00
+#define CPIA2_SYSTEM_SERIAL_CTRL_START_CMD       0x01
+#define CPIA2_SYSTEM_SERIAL_CTRL_STOP_CMD        0x02
+#define CPIA2_SYSTEM_SERIAL_CTRL_WRITE_CMD       0x03
+#define CPIA2_SYSTEM_SERIAL_CTRL_READ_ACK_CMD    0x04
+#define CPIA2_SYSTEM_SERIAL_CTRL_READ_NACK_CMD   0x05
+
+#define CPIA2_SYSTEM_SERIAL_DATA                     0x07
+
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR                  0x08
+
+/***
+ * I2C addresses for various devices in CPiA2
+ ***/
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR           0x20
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_VP               0x88
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP           0x8A
+
+#define CPIA2_SYSTEM_SPARE_REG1                      0x09
+#define CPIA2_SYSTEM_SPARE_REG2                      0x0A
+#define CPIA2_SYSTEM_SPARE_REG3                      0x0B
+
+#define CPIA2_SYSTEM_MC_PORT_0                       0x0C
+#define CPIA2_SYSTEM_MC_PORT_1                       0x0D
+#define CPIA2_SYSTEM_MC_PORT_2                       0x0E
+#define CPIA2_SYSTEM_MC_PORT_3                       0x0F
+
+#define CPIA2_SYSTEM_STATUS_PKT                      0x20
+#define CPIA2_SYSTEM_STATUS_PKT_END                  0x27
+
+#define CPIA2_SYSTEM_DESCRIP_VID_HI                  0x30
+#define CPIA2_SYSTEM_DESCRIP_VID_LO                  0x31
+#define CPIA2_SYSTEM_DESCRIP_PID_HI                  0x32
+#define CPIA2_SYSTEM_DESCRIP_PID_LO                  0x33
+
+#define CPIA2_SYSTEM_FW_VERSION_HI                   0x34
+#define CPIA2_SYSTEM_FW_VERSION_LO                   0x35
+
+#define CPIA2_SYSTEM_CACHE_START_INDEX               0x80
+#define CPIA2_SYSTEM_CACHE_MAX_WRITES                0x10
+
+/***
+ * VC register set (Bank 1)
+ ***/
+#define CPIA2_VC_ASIC_ID                 0x80
+
+#define CPIA2_VC_ASIC_REV                0x81
+
+#define CPIA2_VC_PW_CTRL                 0x82
+#define CPIA2_VC_PW_CTRL_COLDSTART      0x01
+#define CPIA2_VC_PW_CTRL_CP_CLK_EN      0x02
+#define CPIA2_VC_PW_CTRL_VP_RESET_N     0x04
+#define CPIA2_VC_PW_CTRL_VC_CLK_EN      0x08
+#define CPIA2_VC_PW_CTRL_VC_RESET_N     0x10
+#define CPIA2_VC_PW_CTRL_GOTO_SUSPEND   0x20
+#define CPIA2_VC_PW_CTRL_UDC_SUSPEND    0x40
+#define CPIA2_VC_PW_CTRL_PWR_DOWN       0x80
+
+#define CPIA2_VC_WAKEUP                   0x83
+#define CPIA2_VC_WAKEUP_SW_ENABLE       0x01
+#define CPIA2_VC_WAKEUP_XX_ENABLE       0x02
+#define CPIA2_VC_WAKEUP_SW_ATWAKEUP     0x04
+#define CPIA2_VC_WAKEUP_XX_ATWAKEUP     0x08
+
+#define CPIA2_VC_CLOCK_CTRL               0x84
+#define CPIA2_VC_CLOCK_CTRL_TESTUP72    0x01
+
+#define CPIA2_VC_INT_ENABLE                0x88
+#define CPIA2_VC_INT_ENABLE_XX_IE       0x01
+#define CPIA2_VC_INT_ENABLE_SW_IE       0x02
+#define CPIA2_VC_INT_ENABLE_VC_IE       0x04
+#define CPIA2_VC_INT_ENABLE_USBDATA_IE  0x08
+#define CPIA2_VC_INT_ENABLE_USBSETUP_IE 0x10
+#define CPIA2_VC_INT_ENABLE_USBCFG_IE   0x20
+
+#define CPIA2_VC_INT_FLAG                  0x89
+#define CPIA2_VC_INT_ENABLE_XX_FLAG       0x01
+#define CPIA2_VC_INT_ENABLE_SW_FLAG       0x02
+#define CPIA2_VC_INT_ENABLE_VC_FLAG       0x04
+#define CPIA2_VC_INT_ENABLE_USBDATA_FLAG  0x08
+#define CPIA2_VC_INT_ENABLE_USBSETUP_FLAG 0x10
+#define CPIA2_VC_INT_ENABLE_USBCFG_FLAG   0x20
+#define CPIA2_VC_INT_ENABLE_SET_RESET_BIT 0x80
+
+#define CPIA2_VC_INT_STATE                 0x8A
+#define CPIA2_VC_INT_STATE_XX_STATE     0x01
+#define CPIA2_VC_INT_STATE_SW_STATE     0x02
+
+#define CPIA2_VC_MP_DIR                    0x90
+#define CPIA2_VC_MP_DIR_INPUT           0x00
+#define CPIA2_VC_MP_DIR_OUTPUT          0x01
+
+#define CPIA2_VC_MP_DATA                   0x91
+
+#define CPIA2_VC_DP_CTRL                   0x98
+#define CPIA2_VC_DP_CTRL_MODE_0         0x00
+#define CPIA2_VC_DP_CTRL_MODE_A         0x01
+#define CPIA2_VC_DP_CTRL_MODE_B         0x02
+#define CPIA2_VC_DP_CTRL_MODE_C         0x03
+#define CPIA2_VC_DP_CTRL_FAKE_FST       0x04
+
+#define CPIA2_VC_AD_CTRL                   0x99
+#define CPIA2_VC_AD_CTRL_SRC_0          0x00
+#define CPIA2_VC_AD_CTRL_SRC_DIGI_A     0x01
+#define CPIA2_VC_AD_CTRL_SRC_REG        0x02
+#define CPIA2_VC_AD_CTRL_DST_USB        0x00
+#define CPIA2_VC_AD_CTRL_DST_REG        0x04
+
+#define CPIA2_VC_AD_TEST_IN                0x9B
+
+#define CPIA2_VC_AD_TEST_OUT               0x9C
+
+#define CPIA2_VC_AD_STATUS                 0x9D
+#define CPIA2_VC_AD_STATUS_EMPTY        0x01
+#define CPIA2_VC_AD_STATUS_FULL         0x02
+
+#define CPIA2_VC_DP_DATA                   0x9E
+
+#define CPIA2_VC_ST_CTRL                   0xA0
+#define CPIA2_VC_ST_CTRL_SRC_VC         0x00
+#define CPIA2_VC_ST_CTRL_SRC_DP         0x01
+#define CPIA2_VC_ST_CTRL_SRC_REG        0x02
+
+#define CPIA2_VC_ST_CTRL_RAW_SELECT     0x04
+
+#define CPIA2_VC_ST_CTRL_DST_USB        0x00
+#define CPIA2_VC_ST_CTRL_DST_DP         0x08
+#define CPIA2_VC_ST_CTRL_DST_REG        0x10
+
+#define CPIA2_VC_ST_CTRL_FIFO_ENABLE    0x20
+#define CPIA2_VC_ST_CTRL_EOF_DETECT     0x40
+
+#define CPIA2_VC_ST_TEST                   0xA1
+#define CPIA2_VC_ST_TEST_MODE_MANUAL    0x00
+#define CPIA2_VC_ST_TEST_MODE_INCREMENT 0x02
+
+#define CPIA2_VC_ST_TEST_AUTO_FILL      0x08
+
+#define CPIA2_VC_ST_TEST_REPEAT_FIFO    0x10
+
+#define CPIA2_VC_ST_TEST_IN                0xA2
+
+#define CPIA2_VC_ST_TEST_OUT               0xA3
+
+#define CPIA2_VC_ST_STATUS                 0xA4
+#define CPIA2_VC_ST_STATUS_EMPTY        0x01
+#define CPIA2_VC_ST_STATUS_FULL         0x02
+
+#define CPIA2_VC_ST_FRAME_DETECT_1         0xA5
+
+#define CPIA2_VC_ST_FRAME_DETECT_2         0xA6
+
+#define CPIA2_VC_USB_CTRL                    0xA8
+#define CPIA2_VC_USB_CTRL_CMD_STALLED      0x01
+#define CPIA2_VC_USB_CTRL_CMD_READY        0x02
+#define CPIA2_VC_USB_CTRL_CMD_STATUS       0x04
+#define CPIA2_VC_USB_CTRL_CMD_STATUS_DIR   0x08
+#define CPIA2_VC_USB_CTRL_CMD_NO_CLASH     0x10
+#define CPIA2_VC_USB_CTRL_CMD_MICRO_ACCESS 0x80
+
+#define CPIA2_VC_USB_STRM                  0xA9
+#define CPIA2_VC_USB_STRM_ISO_ENABLE    0x01
+#define CPIA2_VC_USB_STRM_BLK_ENABLE    0x02
+#define CPIA2_VC_USB_STRM_INT_ENABLE    0x04
+#define CPIA2_VC_USB_STRM_AUD_ENABLE    0x08
+
+#define CPIA2_VC_USB_STATUS                   0xAA
+#define CPIA2_VC_USB_STATUS_CMD_IN_PROGRESS  0x01
+#define CPIA2_VC_USB_STATUS_CMD_STATUS_STALL 0x02
+#define CPIA2_VC_USB_STATUS_CMD_HANDSHAKE    0x04
+#define CPIA2_VC_USB_STATUS_CMD_OVERRIDE     0x08
+#define CPIA2_VC_USB_STATUS_CMD_FIFO_BUSY    0x10
+#define CPIA2_VC_USB_STATUS_BULK_REPEAT_TXN  0x20
+#define CPIA2_VC_USB_STATUS_CONFIG_DONE      0x40
+#define CPIA2_VC_USB_STATUS_USB_SUSPEND      0x80
+
+#define CPIA2_VC_USB_CMDW                   0xAB
+
+#define CPIA2_VC_USB_DATARW                 0xAC
+
+#define CPIA2_VC_USB_INFO                   0xAD
+
+#define CPIA2_VC_USB_CONFIG                 0xAE
+
+#define CPIA2_VC_USB_SETTINGS                  0xAF
+#define CPIA2_VC_USB_SETTINGS_CONFIG_MASK    0x03
+#define CPIA2_VC_USB_SETTINGS_INTERFACE_MASK 0x0C
+#define CPIA2_VC_USB_SETTINGS_ALTERNATE_MASK 0x70
+
+#define CPIA2_VC_USB_ISOLIM                  0xB0
+
+#define CPIA2_VC_USB_ISOFAILS                0xB1
+
+#define CPIA2_VC_USB_ISOMAXPKTHI             0xB2
+
+#define CPIA2_VC_USB_ISOMAXPKTLO             0xB3
+
+#define CPIA2_VC_V2W_CTRL                    0xB8
+#define CPIA2_VC_V2W_SELECT               0x01
+
+#define CPIA2_VC_V2W_SCL                     0xB9
+
+#define CPIA2_VC_V2W_SDA                     0xBA
+
+#define CPIA2_VC_VC_CTRL                     0xC0
+#define CPIA2_VC_VC_CTRL_RUN              0x01
+#define CPIA2_VC_VC_CTRL_SINGLESHOT       0x02
+#define CPIA2_VC_VC_CTRL_IDLING           0x04
+#define CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES 0x10
+#define CPIA2_VC_VC_CTRL_INHIBIT_Q_TABLES 0x20
+#define CPIA2_VC_VC_CTRL_INHIBIT_PRIVATE  0x40
+
+#define CPIA2_VC_VC_RESTART_IVAL_HI          0xC1
+
+#define CPIA2_VC_VC_RESTART_IVAL_LO          0xC2
+
+#define CPIA2_VC_VC_FORMAT                   0xC3
+#define CPIA2_VC_VC_FORMAT_UFIRST         0x01
+#define CPIA2_VC_VC_FORMAT_MONO           0x02
+#define CPIA2_VC_VC_FORMAT_DECIMATING     0x04
+#define CPIA2_VC_VC_FORMAT_SHORTLINE      0x08
+#define CPIA2_VC_VC_FORMAT_SELFTEST       0x10
+
+#define CPIA2_VC_VC_CLOCKS                         0xC4
+#define CPIA2_VC_VC_CLOCKS_CLKDIV_MASK        0x03
+#define CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3   0x04
+#define CPIA2_VC_VC_672_CLOCKS_SCALING        0x08
+#define CPIA2_VC_VC_CLOCKS_LOGDIV0        0x00
+#define CPIA2_VC_VC_CLOCKS_LOGDIV1        0x01
+#define CPIA2_VC_VC_CLOCKS_LOGDIV2        0x02
+#define CPIA2_VC_VC_CLOCKS_LOGDIV3        0x03
+#define CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3   0x08
+#define CPIA2_VC_VC_676_CLOCKS_SCALING	      0x10
+
+#define CPIA2_VC_VC_IHSIZE_LO                0xC5
+
+#define CPIA2_VC_VC_XLIM_HI                  0xC6
+
+#define CPIA2_VC_VC_XLIM_LO                  0xC7
+
+#define CPIA2_VC_VC_YLIM_HI                  0xC8
+
+#define CPIA2_VC_VC_YLIM_LO                  0xC9
+
+#define CPIA2_VC_VC_OHSIZE                   0xCA
+
+#define CPIA2_VC_VC_OVSIZE                   0xCB
+
+#define CPIA2_VC_VC_HCROP                    0xCC
+
+#define CPIA2_VC_VC_VCROP                    0xCD
+
+#define CPIA2_VC_VC_HPHASE                   0xCE
+
+#define CPIA2_VC_VC_VPHASE                   0xCF
+
+#define CPIA2_VC_VC_HISPAN                   0xD0
+
+#define CPIA2_VC_VC_VISPAN                   0xD1
+
+#define CPIA2_VC_VC_HICROP                   0xD2
+
+#define CPIA2_VC_VC_VICROP                   0xD3
+
+#define CPIA2_VC_VC_HFRACT                   0xD4
+#define CPIA2_VC_VC_HFRACT_DEN_MASK       0x0F
+#define CPIA2_VC_VC_HFRACT_NUM_MASK       0xF0
+
+#define CPIA2_VC_VC_VFRACT                   0xD5
+#define CPIA2_VC_VC_VFRACT_DEN_MASK       0x0F
+#define CPIA2_VC_VC_VFRACT_NUM_MASK       0xF0
+
+#define CPIA2_VC_VC_JPEG_OPT                      0xD6
+#define CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE     0x01
+#define CPIA2_VC_VC_JPEG_OPT_NO_DC_AUTO_SQUEEZE 0x02
+#define CPIA2_VC_VC_JPEG_OPT_AUTO_SQUEEZE       0x04
+#define CPIA2_VC_VC_JPEG_OPT_DEFAULT      (CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE|\
+					   CPIA2_VC_VC_JPEG_OPT_AUTO_SQUEEZE)
+
+
+#define CPIA2_VC_VC_CREEP_PERIOD             0xD7
+#define CPIA2_VC_VC_USER_SQUEEZE             0xD8
+#define CPIA2_VC_VC_TARGET_KB                0xD9
+
+#define CPIA2_VC_VC_AUTO_SQUEEZE             0xE6
+
+
+/***
+ * VP register set (Bank 2)
+ ***/
+#define CPIA2_VP_DEVICEH                             0
+#define CPIA2_VP_DEVICEL                             1
+
+#define CPIA2_VP_SYSTEMSTATE                         0x02
+#define CPIA2_VP_SYSTEMSTATE_HK_ALIVE             0x01
+
+#define CPIA2_VP_SYSTEMCTRL                          0x03
+#define CPIA2_VP_SYSTEMCTRL_REQ_CLEAR_ERROR       0x80
+#define CPIA2_VP_SYSTEMCTRL_POWER_DOWN_PLL        0x20
+#define CPIA2_VP_SYSTEMCTRL_REQ_SUSPEND_STATE     0x10
+#define CPIA2_VP_SYSTEMCTRL_REQ_SERIAL_WAKEUP     0x08
+#define CPIA2_VP_SYSTEMCTRL_REQ_AUTOLOAD          0x04
+#define CPIA2_VP_SYSTEMCTRL_HK_CONTROL            0x02
+#define CPIA2_VP_SYSTEMCTRL_POWER_CONTROL         0x01
+
+#define CPIA2_VP_SENSOR_FLAGS                        0x05
+#define CPIA2_VP_SENSOR_FLAGS_404                 0x01
+#define CPIA2_VP_SENSOR_FLAGS_407                 0x02
+#define CPIA2_VP_SENSOR_FLAGS_409                 0x04
+#define CPIA2_VP_SENSOR_FLAGS_410                 0x08
+#define CPIA2_VP_SENSOR_FLAGS_500                 0x10
+
+#define CPIA2_VP_SENSOR_REV                          0x06
+
+#define CPIA2_VP_DEVICE_CONFIG                       0x07
+#define CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE      0x01
+
+#define CPIA2_VP_GPIO_DIRECTION                      0x08
+#define CPIA2_VP_GPIO_READ                        0xFF
+#define CPIA2_VP_GPIO_WRITE                       0x00
+
+#define CPIA2_VP_GPIO_DATA                           0x09
+
+#define CPIA2_VP_RAM_ADDR_H                          0x0A
+#define CPIA2_VP_RAM_ADDR_L                          0x0B
+#define CPIA2_VP_RAM_DATA                            0x0C
+
+#define CPIA2_VP_PATCH_REV                           0x0F
+
+#define CPIA2_VP4_USER_MODE                           0x10
+#define CPIA2_VP5_USER_MODE                           0x13
+#define CPIA2_VP_USER_MODE_CIF                    0x01
+#define CPIA2_VP_USER_MODE_QCIFDS                 0x02
+#define CPIA2_VP_USER_MODE_QCIFPTC                0x04
+#define CPIA2_VP_USER_MODE_QVGADS                 0x08
+#define CPIA2_VP_USER_MODE_QVGAPTC                0x10
+#define CPIA2_VP_USER_MODE_VGA                    0x20
+
+#define CPIA2_VP4_FRAMERATE_REQUEST                    0x11
+#define CPIA2_VP5_FRAMERATE_REQUEST                    0x14
+#define CPIA2_VP_FRAMERATE_60                     0x80
+#define CPIA2_VP_FRAMERATE_50                     0x40
+#define CPIA2_VP_FRAMERATE_30                     0x20
+#define CPIA2_VP_FRAMERATE_25                     0x10
+#define CPIA2_VP_FRAMERATE_15                     0x08
+#define CPIA2_VP_FRAMERATE_12_5                   0x04
+#define CPIA2_VP_FRAMERATE_7_5                    0x02
+#define CPIA2_VP_FRAMERATE_6_25                   0x01
+
+#define CPIA2_VP4_USER_EFFECTS                         0x12
+#define CPIA2_VP5_USER_EFFECTS                         0x15
+#define CPIA2_VP_USER_EFFECTS_COLBARS             0x01
+#define CPIA2_VP_USER_EFFECTS_COLBARS_GRAD        0x02
+#define CPIA2_VP_USER_EFFECTS_MIRROR              0x04
+#define CPIA2_VP_USER_EFFECTS_FLIP                0x40  // VP5 only
+
+/* NOTE: CPIA2_VP_EXPOSURE_MODES shares the same register as VP5 User
+ * Effects */
+#define CPIA2_VP_EXPOSURE_MODES                       0x15
+#define CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER   0x20
+#define CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP       0x10
+
+#define CPIA2_VP4_EXPOSURE_TARGET                     0x16    // VP4
+#define CPIA2_VP5_EXPOSURE_TARGET		      0x20    // VP5
+
+#define CPIA2_VP_FLICKER_MODES                        0x1B
+#define CPIA2_VP_FLICKER_MODES_50HZ               0x80
+#define CPIA2_VP_FLICKER_MODES_CUSTOM_FLT_FFREQ   0x40
+#define CPIA2_VP_FLICKER_MODES_NEVER_FLICKER      0x20
+#define CPIA2_VP_FLICKER_MODES_INHIBIT_RUB        0x10
+#define CPIA2_VP_FLICKER_MODES_ADJUST_LINE_FREQ   0x08
+#define CPIA2_VP_FLICKER_MODES_CUSTOM_INT_FFREQ   0x04
+
+#define CPIA2_VP_UMISC                                0x1D
+#define CPIA2_VP_UMISC_FORCE_MONO                 0x80
+#define CPIA2_VP_UMISC_FORCE_ID_MASK              0x40
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_FGS           0x20
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_DIMS          0x08
+#define CPIA2_VP_UMISC_OPT_FOR_SENSOR_DS          0x04
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_MODE_INT      0x02
+
+#define CPIA2_VP5_ANTIFLKRSETUP                       0x22  //34
+
+#define CPIA2_VP_INTERPOLATION                        0x24
+#define CPIA2_VP_INTERPOLATION_EVEN_FIRST         0x40
+#define CPIA2_VP_INTERPOLATION_HJOG               0x20
+#define CPIA2_VP_INTERPOLATION_VJOG               0x10
+
+#define CPIA2_VP_GAMMA                                0x25
+#define CPIA2_VP_DEFAULT_GAMMA                    0x10
+
+#define CPIA2_VP_YRANGE                               0x26
+
+#define CPIA2_VP_SATURATION                           0x27
+
+#define CPIA2_VP5_MYBLACK_LEVEL                       0x3A   //58
+#define CPIA2_VP5_MCYRANGE                            0x3B   //59
+#define CPIA2_VP5_MYCEILING                           0x3C   //60
+#define CPIA2_VP5_MCUVSATURATION                      0x3D   //61
+
+
+#define CPIA2_VP_REHASH_VALUES                        0x60
+
+
+/***
+ * Common sensor registers
+ ***/
+#define CPIA2_SENSOR_DEVICE_H                         0x00
+#define CPIA2_SENSOR_DEVICE_L                         0x01
+
+#define CPIA2_SENSOR_DATA_FORMAT                      0x16
+#define CPIA2_SENSOR_DATA_FORMAT_HMIRROR      0x08
+#define CPIA2_SENSOR_DATA_FORMAT_VMIRROR      0x10
+
+#define CPIA2_SENSOR_CR1                              0x76
+#define CPIA2_SENSOR_CR1_STAND_BY             0x01
+#define CPIA2_SENSOR_CR1_DOWN_RAMP_GEN        0x02
+#define CPIA2_SENSOR_CR1_DOWN_COLUMN_ADC      0x04
+#define CPIA2_SENSOR_CR1_DOWN_CAB_REGULATOR   0x08
+#define CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR 0x10
+#define CPIA2_SENSOR_CR1_DOWN_VRT_AMP         0x20
+#define CPIA2_SENSOR_CR1_DOWN_BAND_GAP        0x40
+
+#endif
diff --git a/drivers/staging/media/deprecated/cpia2/cpia2_usb.c b/drivers/staging/media/deprecated/cpia2/cpia2_usb.c
new file mode 100644
index 000000000..cba03b286
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/cpia2_usb.c
@@ -0,0 +1,966 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/****************************************************************************
+ *
+ *  Filename: cpia2_usb.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@lxorguk.ukuu.org.uk>
+ ****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/module.h>
+
+#include "cpia2.h"
+
+static int frame_sizes[] = {
+	0,	// USBIF_CMDONLY
+	0,	// USBIF_BULK
+	128,	// USBIF_ISO_1
+	384,	// USBIF_ISO_2
+	640,	// USBIF_ISO_3
+	768,	// USBIF_ISO_4
+	896,	// USBIF_ISO_5
+	1023,	// USBIF_ISO_6
+};
+
+#define FRAMES_PER_DESC    10
+#define FRAME_SIZE_PER_DESC   frame_sizes[cam->cur_alt]
+
+static void process_frame(struct camera_data *cam);
+static void cpia2_usb_complete(struct urb *urb);
+static int cpia2_usb_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id);
+static void cpia2_usb_disconnect(struct usb_interface *intf);
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
+static int cpia2_usb_resume(struct usb_interface *intf);
+
+static void free_sbufs(struct camera_data *cam);
+static void add_APPn(struct camera_data *cam);
+static void add_COM(struct camera_data *cam);
+static int submit_urbs(struct camera_data *cam);
+static int set_alternate(struct camera_data *cam, unsigned int alt);
+static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
+
+static const struct usb_device_id cpia2_id_table[] = {
+	{USB_DEVICE(0x0553, 0x0100)},
+	{USB_DEVICE(0x0553, 0x0140)},
+	{USB_DEVICE(0x0553, 0x0151)},  /* STV0676 */
+	{}			/* Terminating entry */
+};
+MODULE_DEVICE_TABLE(usb, cpia2_id_table);
+
+static struct usb_driver cpia2_driver = {
+	.name		= "cpia2",
+	.probe		= cpia2_usb_probe,
+	.disconnect	= cpia2_usb_disconnect,
+	.suspend	= cpia2_usb_suspend,
+	.resume		= cpia2_usb_resume,
+	.reset_resume	= cpia2_usb_resume,
+	.id_table	= cpia2_id_table
+};
+
+
+/******************************************************************************
+ *
+ *  process_frame
+ *
+ *****************************************************************************/
+static void process_frame(struct camera_data *cam)
+{
+	static int frame_count;
+
+	unsigned char *inbuff = cam->workbuff->data;
+
+	DBG("Processing frame #%d, current:%d\n",
+	    cam->workbuff->num, cam->curbuff->num);
+
+	if(cam->workbuff->length > cam->workbuff->max_length)
+		cam->workbuff->max_length = cam->workbuff->length;
+
+	if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
+		frame_count++;
+	} else {
+		cam->workbuff->status = FRAME_ERROR;
+		DBG("Start of frame not found\n");
+		return;
+	}
+
+	/***
+	 * Now the output buffer should have a JPEG image in it.
+	 ***/
+	if(!cam->first_image_seen) {
+		/* Always skip the first image after streaming
+		 * starts. It is almost certainly corrupt. */
+		cam->first_image_seen = 1;
+		cam->workbuff->status = FRAME_EMPTY;
+		return;
+	}
+	if (cam->workbuff->length > 3) {
+		if(cam->mmapped &&
+		   cam->workbuff->length < cam->workbuff->max_length) {
+			/* No junk in the buffers */
+			memset(cam->workbuff->data+cam->workbuff->length,
+			       0, cam->workbuff->max_length-
+				  cam->workbuff->length);
+		}
+		cam->workbuff->max_length = cam->workbuff->length;
+		cam->workbuff->status = FRAME_READY;
+
+		if(!cam->mmapped && cam->num_frames > 2) {
+			/* During normal reading, the most recent
+			 * frame will be read.  If the current frame
+			 * hasn't started reading yet, it will never
+			 * be read, so mark it empty.  If the buffer is
+			 * mmapped, or we have few buffers, we need to
+			 * wait for the user to free the buffer.
+			 *
+			 * NOTE: This is not entirely foolproof with 3
+			 * buffers, but it would take an EXTREMELY
+			 * overloaded system to cause problems (possible
+			 * image data corruption).  Basically, it would
+			 * need to take more time to execute cpia2_read
+			 * than it would for the camera to send
+			 * cam->num_frames-2 frames before problems
+			 * could occur.
+			 */
+			cam->curbuff->status = FRAME_EMPTY;
+		}
+		cam->curbuff = cam->workbuff;
+		cam->workbuff = cam->workbuff->next;
+		DBG("Changed buffers, work:%d, current:%d\n",
+		    cam->workbuff->num, cam->curbuff->num);
+		return;
+	} else {
+		DBG("Not enough data for an image.\n");
+	}
+
+	cam->workbuff->status = FRAME_ERROR;
+	return;
+}
+
+/******************************************************************************
+ *
+ *  add_APPn
+ *
+ *  Adds a user specified APPn record
+ *****************************************************************************/
+static void add_APPn(struct camera_data *cam)
+{
+	if(cam->APP_len > 0) {
+		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
+		cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
+		cam->workbuff->data[cam->workbuff->length++] = 0;
+		cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
+		memcpy(cam->workbuff->data+cam->workbuff->length,
+		       cam->APP_data, cam->APP_len);
+		cam->workbuff->length += cam->APP_len;
+	}
+}
+
+/******************************************************************************
+ *
+ *  add_COM
+ *
+ *  Adds a user specified COM record
+ *****************************************************************************/
+static void add_COM(struct camera_data *cam)
+{
+	if(cam->COM_len > 0) {
+		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
+		cam->workbuff->data[cam->workbuff->length++] = 0xFE;
+		cam->workbuff->data[cam->workbuff->length++] = 0;
+		cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
+		memcpy(cam->workbuff->data+cam->workbuff->length,
+		       cam->COM_data, cam->COM_len);
+		cam->workbuff->length += cam->COM_len;
+	}
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_complete
+ *
+ *  callback when incoming packet is received
+ *****************************************************************************/
+static void cpia2_usb_complete(struct urb *urb)
+{
+	int i;
+	unsigned char *cdata;
+	static bool frame_ready = false;
+	struct camera_data *cam = (struct camera_data *) urb->context;
+
+	if (urb->status!=0) {
+		if (!(urb->status == -ENOENT ||
+		      urb->status == -ECONNRESET ||
+		      urb->status == -ESHUTDOWN))
+		{
+			DBG("urb->status = %d!\n", urb->status);
+		}
+		DBG("Stopping streaming\n");
+		return;
+	}
+
+	if (!cam->streaming || !video_is_registered(&cam->vdev)) {
+		LOG("Will now stop the streaming: streaming = %d, present=%d\n",
+		    cam->streaming, video_is_registered(&cam->vdev));
+		return;
+	}
+
+	/***
+	 * Packet collater
+	 ***/
+	//DBG("Collating %d packets\n", urb->number_of_packets);
+	for (i = 0; i < urb->number_of_packets; i++) {
+		u16 checksum, iso_checksum;
+		int j;
+		int n = urb->iso_frame_desc[i].actual_length;
+		int st = urb->iso_frame_desc[i].status;
+
+		if(cam->workbuff->status == FRAME_READY) {
+			struct framebuf *ptr;
+			/* Try to find an available buffer */
+			DBG("workbuff full, searching\n");
+			for (ptr = cam->workbuff->next;
+			     ptr != cam->workbuff;
+			     ptr = ptr->next)
+			{
+				if (ptr->status == FRAME_EMPTY) {
+					ptr->status = FRAME_READING;
+					ptr->length = 0;
+					break;
+				}
+			}
+			if (ptr == cam->workbuff)
+				break; /* No READING or EMPTY buffers left */
+
+			cam->workbuff = ptr;
+		}
+
+		if (cam->workbuff->status == FRAME_EMPTY ||
+		    cam->workbuff->status == FRAME_ERROR) {
+			cam->workbuff->status = FRAME_READING;
+			cam->workbuff->length = 0;
+		}
+
+		//DBG("   Packet %d length = %d, status = %d\n", i, n, st);
+		cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+
+		if (st) {
+			LOG("cpia2 data error: [%d] len=%d, status = %d\n",
+			    i, n, st);
+			if(!ALLOW_CORRUPT)
+				cam->workbuff->status = FRAME_ERROR;
+			continue;
+		}
+
+		if(n<=2)
+			continue;
+
+		checksum = 0;
+		for(j=0; j<n-2; ++j)
+			checksum += cdata[j];
+		iso_checksum = cdata[j] + cdata[j+1]*256;
+		if(checksum != iso_checksum) {
+			LOG("checksum mismatch: [%d] len=%d, calculated = %x, checksum = %x\n",
+			    i, n, (int)checksum, (int)iso_checksum);
+			if(!ALLOW_CORRUPT) {
+				cam->workbuff->status = FRAME_ERROR;
+				continue;
+			}
+		}
+		n -= 2;
+
+		if(cam->workbuff->status != FRAME_READING) {
+			if((0xFF == cdata[0] && 0xD8 == cdata[1]) ||
+			   (0xD8 == cdata[0] && 0xFF == cdata[1] &&
+			    0 != cdata[2])) {
+				/* frame is skipped, but increment total
+				 * frame count anyway */
+				cam->frame_count++;
+			}
+			DBG("workbuff not reading, status=%d\n",
+			    cam->workbuff->status);
+			continue;
+		}
+
+		if (cam->frame_size < cam->workbuff->length + n) {
+			ERR("buffer overflow! length: %d, n: %d\n",
+			    cam->workbuff->length, n);
+			cam->workbuff->status = FRAME_ERROR;
+			if(cam->workbuff->length > cam->workbuff->max_length)
+				cam->workbuff->max_length =
+					cam->workbuff->length;
+			continue;
+		}
+
+		if (cam->workbuff->length == 0) {
+			int data_offset;
+			if ((0xD8 == cdata[0]) && (0xFF == cdata[1])) {
+				data_offset = 1;
+			} else if((0xFF == cdata[0]) && (0xD8 == cdata[1])
+				  && (0xFF == cdata[2])) {
+				data_offset = 2;
+			} else {
+				DBG("Ignoring packet, not beginning!\n");
+				continue;
+			}
+			DBG("Start of frame pattern found\n");
+			cam->workbuff->ts = ktime_get_ns();
+			cam->workbuff->seq = cam->frame_count++;
+			cam->workbuff->data[0] = 0xFF;
+			cam->workbuff->data[1] = 0xD8;
+			cam->workbuff->length = 2;
+			add_APPn(cam);
+			add_COM(cam);
+			memcpy(cam->workbuff->data+cam->workbuff->length,
+			       cdata+data_offset, n-data_offset);
+			cam->workbuff->length += n-data_offset;
+		} else if (cam->workbuff->length > 0) {
+			memcpy(cam->workbuff->data + cam->workbuff->length,
+			       cdata, n);
+			cam->workbuff->length += n;
+		}
+
+		if ((cam->workbuff->length >= 3) &&
+		    (cam->workbuff->data[cam->workbuff->length - 3] == 0xFF) &&
+		    (cam->workbuff->data[cam->workbuff->length - 2] == 0xD9) &&
+		    (cam->workbuff->data[cam->workbuff->length - 1] == 0xFF)) {
+			frame_ready = true;
+			cam->workbuff->data[cam->workbuff->length - 1] = 0;
+			cam->workbuff->length -= 1;
+		} else if ((cam->workbuff->length >= 2) &&
+		   (cam->workbuff->data[cam->workbuff->length - 2] == 0xFF) &&
+		   (cam->workbuff->data[cam->workbuff->length - 1] == 0xD9)) {
+			frame_ready = true;
+		}
+
+		if (frame_ready) {
+			DBG("Workbuff image size = %d\n",cam->workbuff->length);
+			process_frame(cam);
+
+			frame_ready = false;
+
+			if (waitqueue_active(&cam->wq_stream))
+				wake_up_interruptible(&cam->wq_stream);
+		}
+	}
+
+	if(cam->streaming) {
+		/* resubmit */
+		urb->dev = cam->dev;
+		if ((i = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
+			ERR("%s: usb_submit_urb ret %d!\n", __func__, i);
+	}
+}
+
+/******************************************************************************
+ *
+ * configure_transfer_mode
+ *
+ *****************************************************************************/
+static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
+{
+	static unsigned char iso_regs[8][4] = {
+		{0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00},
+		{0xB9, 0x00, 0x00, 0x7E},
+		{0xB9, 0x00, 0x01, 0x7E},
+		{0xB9, 0x00, 0x02, 0x7E},
+		{0xB9, 0x00, 0x02, 0xFE},
+		{0xB9, 0x00, 0x03, 0x7E},
+		{0xB9, 0x00, 0x03, 0xFD}
+	};
+	struct cpia2_command cmd;
+	unsigned char reg;
+
+	if (!video_is_registered(&cam->vdev))
+		return -ENODEV;
+
+	/***
+	 * Write the isoc registers according to the alternate selected
+	 ***/
+	cmd.direction = TRANSFER_WRITE;
+	cmd.buffer.block_data[0] = iso_regs[alt][0];
+	cmd.buffer.block_data[1] = iso_regs[alt][1];
+	cmd.buffer.block_data[2] = iso_regs[alt][2];
+	cmd.buffer.block_data[3] = iso_regs[alt][3];
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.start = CPIA2_VC_USB_ISOLIM;
+	cmd.reg_count = 4;
+	cpia2_send_command(cam, &cmd);
+
+	/***
+	 * Enable relevant streams before starting polling.
+	 * First read USB Stream Config Register.
+	 ***/
+	cmd.direction = TRANSFER_READ;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.start = CPIA2_VC_USB_STRM;
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	reg = cmd.buffer.block_data[0];
+
+	/* Clear iso, bulk, and int */
+	reg &= ~(CPIA2_VC_USB_STRM_BLK_ENABLE |
+		 CPIA2_VC_USB_STRM_ISO_ENABLE |
+		 CPIA2_VC_USB_STRM_INT_ENABLE);
+
+	if (alt == USBIF_BULK) {
+		DBG("Enabling bulk xfer\n");
+		reg |= CPIA2_VC_USB_STRM_BLK_ENABLE;	/* Enable Bulk */
+		cam->xfer_mode = XFER_BULK;
+	} else if (alt >= USBIF_ISO_1) {
+		DBG("Enabling ISOC xfer\n");
+		reg |= CPIA2_VC_USB_STRM_ISO_ENABLE;
+		cam->xfer_mode = XFER_ISOC;
+	}
+
+	cmd.buffer.block_data[0] = reg;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.start = CPIA2_VC_USB_STRM;
+	cmd.reg_count = 1;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cpia2_send_command(cam, &cmd);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * cpia2_usb_change_streaming_alternate
+ *
+ *****************************************************************************/
+int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
+					 unsigned int alt)
+{
+	int ret = 0;
+
+	if(alt < USBIF_ISO_1 || alt > USBIF_ISO_6)
+		return -EINVAL;
+
+	if(alt == cam->params.camera_state.stream_mode)
+		return 0;
+
+	cpia2_usb_stream_pause(cam);
+
+	configure_transfer_mode(cam, alt);
+
+	cam->params.camera_state.stream_mode = alt;
+
+	/* Reset the camera to prevent image quality degradation */
+	cpia2_reset_camera(cam);
+
+	cpia2_usb_stream_resume(cam);
+
+	return ret;
+}
+
+/******************************************************************************
+ *
+ * set_alternate
+ *
+ *****************************************************************************/
+static int set_alternate(struct camera_data *cam, unsigned int alt)
+{
+	int ret = 0;
+
+	if(alt == cam->cur_alt)
+		return 0;
+
+	if (cam->cur_alt != USBIF_CMDONLY) {
+		DBG("Changing from alt %d to %d\n", cam->cur_alt, USBIF_CMDONLY);
+		ret = usb_set_interface(cam->dev, cam->iface, USBIF_CMDONLY);
+		if (ret != 0)
+			return ret;
+	}
+	if (alt != USBIF_CMDONLY) {
+		DBG("Changing from alt %d to %d\n", USBIF_CMDONLY, alt);
+		ret = usb_set_interface(cam->dev, cam->iface, alt);
+		if (ret != 0)
+			return ret;
+	}
+
+	cam->old_alt = cam->cur_alt;
+	cam->cur_alt = alt;
+
+	return ret;
+}
+
+/******************************************************************************
+ *
+ * free_sbufs
+ *
+ * Free all cam->sbuf[]. All non-NULL .data and .urb members that are non-NULL
+ * are assumed to be allocated. Non-NULL .urb members are also assumed to be
+ * submitted (and must therefore be killed before they are freed).
+ *****************************************************************************/
+static void free_sbufs(struct camera_data *cam)
+{
+	int i;
+
+	for (i = 0; i < NUM_SBUF; i++) {
+		if(cam->sbuf[i].urb) {
+			usb_kill_urb(cam->sbuf[i].urb);
+			usb_free_urb(cam->sbuf[i].urb);
+			cam->sbuf[i].urb = NULL;
+		}
+		if(cam->sbuf[i].data) {
+			kfree(cam->sbuf[i].data);
+			cam->sbuf[i].data = NULL;
+		}
+	}
+}
+
+/*******
+* Convenience functions
+*******/
+/****************************************************************************
+ *
+ *  write_packet
+ *
+ ***************************************************************************/
+static int write_packet(struct usb_device *udev,
+			u8 request, u8 * registers, u16 start, size_t size)
+{
+	unsigned char *buf;
+	int ret;
+
+	if (!registers || size <= 0)
+		return -EINVAL;
+
+	buf = kmemdup(registers, size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = usb_control_msg(udev,
+			       usb_sndctrlpipe(udev, 0),
+			       request,
+			       USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			       start,	/* value */
+			       0,	/* index */
+			       buf,	/* buffer */
+			       size,
+			       1000);
+
+	kfree(buf);
+	return ret;
+}
+
+/****************************************************************************
+ *
+ *  read_packet
+ *
+ ***************************************************************************/
+static int read_packet(struct usb_device *udev,
+		       u8 request, u8 * registers, u16 start, size_t size)
+{
+	unsigned char *buf;
+	int ret;
+
+	if (!registers || size <= 0)
+		return -EINVAL;
+
+	buf = kmalloc(size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = usb_control_msg(udev,
+			       usb_rcvctrlpipe(udev, 0),
+			       request,
+			       USB_DIR_IN|USB_TYPE_VENDOR|USB_RECIP_DEVICE,
+			       start,	/* value */
+			       0,	/* index */
+			       buf,	/* buffer */
+			       size,
+			       1000);
+
+	if (ret >= 0)
+		memcpy(registers, buf, size);
+
+	kfree(buf);
+
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_transfer_cmd
+ *
+ *****************************************************************************/
+int cpia2_usb_transfer_cmd(struct camera_data *cam,
+			   void *registers,
+			   u8 request, u8 start, u8 count, u8 direction)
+{
+	int err = 0;
+	struct usb_device *udev = cam->dev;
+
+	if (!udev) {
+		ERR("%s: Internal driver error: udev is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!registers) {
+		ERR("%s: Internal driver error: register array is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (direction == TRANSFER_READ) {
+		err = read_packet(udev, request, (u8 *)registers, start, count);
+		if (err > 0)
+			err = 0;
+	} else if (direction == TRANSFER_WRITE) {
+		err =write_packet(udev, request, (u8 *)registers, start, count);
+		if (err < 0) {
+			LOG("Control message failed, err val = %d\n", err);
+			LOG("Message: request = 0x%0X, start = 0x%0X\n",
+			    request, start);
+			LOG("Message: count = %d, register[0] = 0x%0X\n",
+			    count, ((unsigned char *) registers)[0]);
+		} else
+			err=0;
+	} else {
+		LOG("Unexpected first byte of direction: %d\n",
+		       direction);
+		return -EINVAL;
+	}
+
+	if(err != 0)
+		LOG("Unexpected error: %d\n", err);
+	return err;
+}
+
+
+/******************************************************************************
+ *
+ *  submit_urbs
+ *
+ *****************************************************************************/
+static int submit_urbs(struct camera_data *cam)
+{
+	struct urb *urb;
+	int fx, err, i, j;
+
+	for(i=0; i<NUM_SBUF; ++i) {
+		if (cam->sbuf[i].data)
+			continue;
+		cam->sbuf[i].data =
+		    kmalloc_array(FRAME_SIZE_PER_DESC, FRAMES_PER_DESC,
+				  GFP_KERNEL);
+		if (!cam->sbuf[i].data) {
+			while (--i >= 0) {
+				kfree(cam->sbuf[i].data);
+				cam->sbuf[i].data = NULL;
+			}
+			return -ENOMEM;
+		}
+	}
+
+	/* We double buffer the Isoc lists, and also know the polling
+	 * interval is every frame (1 == (1 << (bInterval -1))).
+	 */
+	for(i=0; i<NUM_SBUF; ++i) {
+		if(cam->sbuf[i].urb) {
+			continue;
+		}
+		urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
+		if (!urb) {
+			for (j = 0; j < i; j++)
+				usb_free_urb(cam->sbuf[j].urb);
+			for (j = 0; j < NUM_SBUF; j++) {
+				kfree(cam->sbuf[j].data);
+				cam->sbuf[j].data = NULL;
+			}
+			return -ENOMEM;
+		}
+
+		cam->sbuf[i].urb = urb;
+		urb->dev = cam->dev;
+		urb->context = cam;
+		urb->pipe = usb_rcvisocpipe(cam->dev, 1 /*ISOC endpoint*/);
+		urb->transfer_flags = URB_ISO_ASAP;
+		urb->transfer_buffer = cam->sbuf[i].data;
+		urb->complete = cpia2_usb_complete;
+		urb->number_of_packets = FRAMES_PER_DESC;
+		urb->interval = 1;
+		urb->transfer_buffer_length =
+			FRAME_SIZE_PER_DESC * FRAMES_PER_DESC;
+
+		for (fx = 0; fx < FRAMES_PER_DESC; fx++) {
+			urb->iso_frame_desc[fx].offset =
+				FRAME_SIZE_PER_DESC * fx;
+			urb->iso_frame_desc[fx].length = FRAME_SIZE_PER_DESC;
+		}
+	}
+
+
+	/* Queue the ISO urbs, and resubmit in the completion handler */
+	for(i=0; i<NUM_SBUF; ++i) {
+		err = usb_submit_urb(cam->sbuf[i].urb, GFP_KERNEL);
+		if (err) {
+			ERR("usb_submit_urb[%d]() = %d\n", i, err);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_start
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate)
+{
+	int ret;
+	int old_alt;
+
+	if(cam->streaming)
+		return 0;
+
+	if (cam->flush) {
+		int i;
+		DBG("Flushing buffers\n");
+		for(i=0; i<cam->num_frames; ++i) {
+			cam->buffers[i].status = FRAME_EMPTY;
+			cam->buffers[i].length = 0;
+		}
+		cam->curbuff = &cam->buffers[0];
+		cam->workbuff = cam->curbuff->next;
+		cam->flush = false;
+	}
+
+	old_alt = cam->params.camera_state.stream_mode;
+	cam->params.camera_state.stream_mode = 0;
+	ret = cpia2_usb_change_streaming_alternate(cam, alternate);
+	if (ret < 0) {
+		int ret2;
+		ERR("cpia2_usb_change_streaming_alternate() = %d!\n", ret);
+		cam->params.camera_state.stream_mode = old_alt;
+		ret2 = set_alternate(cam, USBIF_CMDONLY);
+		if (ret2 < 0) {
+			ERR("cpia2_usb_change_streaming_alternate(%d) =%d has already failed. Then tried to call set_alternate(USBIF_CMDONLY) = %d.\n",
+			    alternate, ret, ret2);
+		}
+	} else {
+		cam->frame_count = 0;
+		cam->streaming = 1;
+		ret = cpia2_usb_stream_resume(cam);
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_pause
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_pause(struct camera_data *cam)
+{
+	int ret = 0;
+	if(cam->streaming) {
+		free_sbufs(cam);
+		ret = set_alternate(cam, USBIF_CMDONLY);
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_resume
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_resume(struct camera_data *cam)
+{
+	int ret = 0;
+	if(cam->streaming) {
+		cam->first_image_seen = 0;
+		ret = set_alternate(cam, cam->params.camera_state.stream_mode);
+		if(ret == 0) {
+			/* for some reason the user effects need to be set
+			   again when starting streaming. */
+			cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+					cam->params.vp_params.user_effects);
+			ret = submit_urbs(cam);
+		}
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_stop
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_stop(struct camera_data *cam)
+{
+	int ret;
+
+	ret = cpia2_usb_stream_pause(cam);
+	cam->streaming = 0;
+	configure_transfer_mode(cam, 0);
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_probe
+ *
+ *  Probe and initialize.
+ *****************************************************************************/
+static int cpia2_usb_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_interface_descriptor *interface;
+	struct camera_data *cam;
+	int ret;
+
+	/* A multi-config CPiA2 camera? */
+	if (udev->descriptor.bNumConfigurations != 1)
+		return -ENODEV;
+	interface = &intf->cur_altsetting->desc;
+
+	/* If we get to this point, we found a CPiA2 camera */
+	LOG("CPiA2 USB camera found\n");
+
+	cam = cpia2_init_camera_struct(intf);
+	if (cam == NULL)
+		return -ENOMEM;
+
+	cam->dev = udev;
+	cam->iface = interface->bInterfaceNumber;
+
+	ret = set_alternate(cam, USBIF_CMDONLY);
+	if (ret < 0) {
+		ERR("%s: usb_set_interface error (ret = %d)\n", __func__, ret);
+		goto alt_err;
+	}
+
+
+	if((ret = cpia2_init_camera(cam)) < 0) {
+		ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
+		goto alt_err;
+	}
+	LOG("  CPiA Version: %d.%02d (%d.%d)\n",
+	       cam->params.version.firmware_revision_hi,
+	       cam->params.version.firmware_revision_lo,
+	       cam->params.version.asic_id,
+	       cam->params.version.asic_rev);
+	LOG("  CPiA PnP-ID: %04x:%04x:%04x\n",
+	       cam->params.pnp_id.vendor,
+	       cam->params.pnp_id.product,
+	       cam->params.pnp_id.device_revision);
+	LOG("  SensorID: %d.(version %d)\n",
+	       cam->params.version.sensor_flags,
+	       cam->params.version.sensor_rev);
+
+	usb_set_intfdata(intf, cam);
+
+	ret = cpia2_register_camera(cam);
+	if (ret < 0) {
+		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
+		goto alt_err;
+	}
+
+	return 0;
+
+alt_err:
+	cpia2_deinit_camera_struct(cam, intf);
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_disconnect
+ *
+ *****************************************************************************/
+static void cpia2_usb_disconnect(struct usb_interface *intf)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+
+	DBG("Stopping stream\n");
+	cpia2_usb_stream_stop(cam);
+
+	mutex_lock(&cam->v4l2_lock);
+	DBG("Unregistering camera\n");
+	cpia2_unregister_camera(cam);
+	v4l2_device_disconnect(&cam->v4l2_dev);
+	mutex_unlock(&cam->v4l2_lock);
+
+	if(cam->buffers) {
+		DBG("Wakeup waiting processes\n");
+		cam->curbuff->status = FRAME_READY;
+		cam->curbuff->length = 0;
+		wake_up_interruptible(&cam->wq_stream);
+	}
+
+	v4l2_device_put(&cam->v4l2_dev);
+
+	LOG("CPiA2 camera disconnected.\n");
+}
+
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	if (cam->streaming) {
+		cpia2_usb_stream_stop(cam);
+		cam->streaming = 1;
+	}
+	mutex_unlock(&cam->v4l2_lock);
+
+	dev_info(&intf->dev, "going into suspend..\n");
+	return 0;
+}
+
+/* Resume device - start device. */
+static int cpia2_usb_resume(struct usb_interface *intf)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	v4l2_ctrl_handler_setup(&cam->hdl);
+	if (cam->streaming) {
+		cam->streaming = 0;
+		cpia2_usb_stream_start(cam,
+				cam->params.camera_state.stream_mode);
+	}
+	mutex_unlock(&cam->v4l2_lock);
+
+	dev_info(&intf->dev, "coming out of suspend..\n");
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  usb_cpia2_init
+ *
+ *****************************************************************************/
+int cpia2_usb_init(void)
+{
+	return usb_register(&cpia2_driver);
+}
+
+/******************************************************************************
+ *
+ *  usb_cpia_cleanup
+ *
+ *****************************************************************************/
+void cpia2_usb_cleanup(void)
+{
+	schedule_timeout(2 * HZ);
+	usb_deregister(&cpia2_driver);
+}
diff --git a/drivers/staging/media/deprecated/cpia2/cpia2_v4l.c b/drivers/staging/media/deprecated/cpia2/cpia2_v4l.c
new file mode 100644
index 000000000..926ecfc9b
--- /dev/null
+++ b/drivers/staging/media/deprecated/cpia2/cpia2_v4l.c
@@ -0,0 +1,1226 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/****************************************************************************
+ *
+ *  Filename: cpia2_v4l.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *  Copyright 2001,2005, Scott J. Bertin <scottbertin@yahoo.com>
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@lxorguk.ukuu.org.uk>
+ ****************************************************************************/
+
+#define CPIA_VERSION "3.0.1"
+
+#include <linux/module.h>
+#include <linux/time.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/videodev2.h>
+#include <linux/stringify.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+
+#include "cpia2.h"
+
+static int video_nr = -1;
+module_param(video_nr, int, 0);
+MODULE_PARM_DESC(video_nr, "video device to register (0=/dev/video0, etc)");
+
+static int buffer_size = 68 * 1024;
+module_param(buffer_size, int, 0);
+MODULE_PARM_DESC(buffer_size, "Size for each frame buffer in bytes (default 68k)");
+
+static int num_buffers = 3;
+module_param(num_buffers, int, 0);
+MODULE_PARM_DESC(num_buffers, "Number of frame buffers (1-"
+		 __stringify(VIDEO_MAX_FRAME) ", default 3)");
+
+static int alternate = DEFAULT_ALT;
+module_param(alternate, int, 0);
+MODULE_PARM_DESC(alternate, "USB Alternate (" __stringify(USBIF_ISO_1) "-"
+		 __stringify(USBIF_ISO_6) ", default "
+		 __stringify(DEFAULT_ALT) ")");
+
+static int flicker_mode;
+module_param(flicker_mode, int, 0);
+MODULE_PARM_DESC(flicker_mode, "Flicker frequency (0 (disabled), " __stringify(50) " or "
+		 __stringify(60) ", default 0)");
+
+MODULE_AUTHOR("Steve Miller (STMicroelectronics) <steve.miller@st.com>");
+MODULE_DESCRIPTION("V4L-driver for STMicroelectronics CPiA2 based cameras");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(CPIA_VERSION);
+
+#define ABOUT "V4L-Driver for Vision CPiA2 based cameras"
+#define CPIA2_CID_USB_ALT (V4L2_CID_USER_BASE | 0xf000)
+
+/******************************************************************************
+ *
+ *  cpia2_open
+ *
+ *****************************************************************************/
+static int cpia2_open(struct file *file)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int retval;
+
+	if (mutex_lock_interruptible(&cam->v4l2_lock))
+		return -ERESTARTSYS;
+	retval = v4l2_fh_open(file);
+	if (retval)
+		goto open_unlock;
+
+	if (v4l2_fh_is_singular_file(file)) {
+		if (cpia2_allocate_buffers(cam)) {
+			v4l2_fh_release(file);
+			retval = -ENOMEM;
+			goto open_unlock;
+		}
+
+		/* reset the camera */
+		if (cpia2_reset_camera(cam) < 0) {
+			v4l2_fh_release(file);
+			retval = -EIO;
+			goto open_unlock;
+		}
+
+		cam->APP_len = 0;
+		cam->COM_len = 0;
+	}
+
+	cpia2_dbg_dump_registers(cam);
+open_unlock:
+	mutex_unlock(&cam->v4l2_lock);
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_close
+ *
+ *****************************************************************************/
+static int cpia2_close(struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+
+	mutex_lock(&cam->v4l2_lock);
+	if (video_is_registered(&cam->vdev) && v4l2_fh_is_singular_file(file)) {
+		cpia2_usb_stream_stop(cam);
+
+		/* save camera state for later open */
+		cpia2_save_camera_state(cam);
+
+		cpia2_set_low_power(cam);
+		cpia2_free_buffers(cam);
+	}
+
+	if (cam->stream_fh == file->private_data) {
+		cam->stream_fh = NULL;
+		cam->mmapped = 0;
+	}
+	mutex_unlock(&cam->v4l2_lock);
+	return v4l2_fh_release(file);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_v4l_read
+ *
+ *****************************************************************************/
+static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
+			      loff_t *off)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int noblock = file->f_flags & O_NONBLOCK;
+	ssize_t ret;
+
+	if (!cam)
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&cam->v4l2_lock))
+		return -ERESTARTSYS;
+	ret = cpia2_read(cam, buf, count, noblock);
+	mutex_unlock(&cam->v4l2_lock);
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_v4l_poll
+ *
+ *****************************************************************************/
+static __poll_t cpia2_v4l_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct camera_data *cam = video_drvdata(filp);
+	__poll_t res;
+
+	mutex_lock(&cam->v4l2_lock);
+	res = cpia2_poll(cam, filp, wait);
+	mutex_unlock(&cam->v4l2_lock);
+	return res;
+}
+
+static int sync(struct camera_data *cam, int frame_nr)
+{
+	struct framebuf *frame = &cam->buffers[frame_nr];
+
+	while (1) {
+		if (frame->status == FRAME_READY)
+			return 0;
+
+		if (!cam->streaming) {
+			frame->status = FRAME_READY;
+			frame->length = 0;
+			return 0;
+		}
+
+		mutex_unlock(&cam->v4l2_lock);
+		wait_event_interruptible(cam->wq_stream,
+					 !cam->streaming ||
+					 frame->status == FRAME_READY);
+		mutex_lock(&cam->v4l2_lock);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		if (!video_is_registered(&cam->vdev))
+			return -ENOTTY;
+	}
+}
+
+/******************************************************************************
+ *
+ *  ioctl_querycap
+ *
+ *  V4L2 device capabilities
+ *
+ *****************************************************************************/
+
+static int cpia2_querycap(struct file *file, void *fh, struct v4l2_capability *vc)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	strscpy(vc->driver, "cpia2", sizeof(vc->driver));
+
+	if (cam->params.pnp_id.product == 0x151)
+		strscpy(vc->card, "QX5 Microscope", sizeof(vc->card));
+	else
+		strscpy(vc->card, "CPiA2 Camera", sizeof(vc->card));
+	switch (cam->params.pnp_id.device_type) {
+	case DEVICE_STV_672:
+		strcat(vc->card, " (672/");
+		break;
+	case DEVICE_STV_676:
+		strcat(vc->card, " (676/");
+		break;
+	default:
+		strcat(vc->card, " (XXX/");
+		break;
+	}
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+		strcat(vc->card, "404)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_407:
+		strcat(vc->card, "407)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_409:
+		strcat(vc->card, "409)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		strcat(vc->card, "410)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		strcat(vc->card, "500)");
+		break;
+	default:
+		strcat(vc->card, "XXX)");
+		break;
+	}
+
+	if (usb_make_path(cam->dev, vc->bus_info, sizeof(vc->bus_info)) < 0)
+		memset(vc->bus_info, 0, sizeof(vc->bus_info));
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_input
+ *
+ *  V4L2 input get/set/enumerate
+ *
+ *****************************************************************************/
+
+static int cpia2_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	if (i->index)
+		return -EINVAL;
+	strscpy(i->name, "Camera", sizeof(i->name));
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	return 0;
+}
+
+static int cpia2_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int cpia2_s_input(struct file *file, void *fh, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_enum_fmt
+ *
+ *  V4L2 format enumerate
+ *
+ *****************************************************************************/
+
+static int cpia2_enum_fmt_vid_cap(struct file *file, void *fh,
+				  struct v4l2_fmtdesc *f)
+{
+	if (f->index > 1)
+		return -EINVAL;
+
+	if (f->index == 0)
+		f->pixelformat = V4L2_PIX_FMT_MJPEG;
+	else
+		f->pixelformat = V4L2_PIX_FMT_JPEG;
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_try_fmt
+ *
+ *  V4L2 format try
+ *
+ *****************************************************************************/
+
+static int cpia2_try_fmt_vid_cap(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG &&
+	    f->fmt.pix.pixelformat != V4L2_PIX_FMT_JPEG)
+		return -EINVAL;
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = cam->frame_size;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+
+	switch (cpia2_match_video_size(f->fmt.pix.width, f->fmt.pix.height)) {
+	case VIDEOSIZE_VGA:
+		f->fmt.pix.width = 640;
+		f->fmt.pix.height = 480;
+		break;
+	case VIDEOSIZE_CIF:
+		f->fmt.pix.width = 352;
+		f->fmt.pix.height = 288;
+		break;
+	case VIDEOSIZE_QVGA:
+		f->fmt.pix.width = 320;
+		f->fmt.pix.height = 240;
+		break;
+	case VIDEOSIZE_288_216:
+		f->fmt.pix.width = 288;
+		f->fmt.pix.height = 216;
+		break;
+	case VIDEOSIZE_256_192:
+		f->fmt.pix.width = 256;
+		f->fmt.pix.height = 192;
+		break;
+	case VIDEOSIZE_224_168:
+		f->fmt.pix.width = 224;
+		f->fmt.pix.height = 168;
+		break;
+	case VIDEOSIZE_192_144:
+		f->fmt.pix.width = 192;
+		f->fmt.pix.height = 144;
+		break;
+	case VIDEOSIZE_QCIF:
+	default:
+		f->fmt.pix.width = 176;
+		f->fmt.pix.height = 144;
+		break;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_set_fmt
+ *
+ *  V4L2 format set
+ *
+ *****************************************************************************/
+
+static int cpia2_s_fmt_vid_cap(struct file *file, void *_fh,
+			       struct v4l2_format *f)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int err, frame;
+
+	err = cpia2_try_fmt_vid_cap(file, _fh, f);
+	if (err != 0)
+		return err;
+
+	cam->pixelformat = f->fmt.pix.pixelformat;
+
+	/* NOTE: This should be set to 1 for MJPEG, but some apps don't handle
+	 * the missing Huffman table properly.
+	 */
+	cam->params.compression.inhibit_htables = 0;
+		/*f->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG;*/
+
+	/* we set the video window to something smaller or equal to what
+	 * is requested by the user???
+	 */
+	DBG("Requested width = %d, height = %d\n",
+	    f->fmt.pix.width, f->fmt.pix.height);
+	if (f->fmt.pix.width != cam->width ||
+	    f->fmt.pix.height != cam->height) {
+		cam->width = f->fmt.pix.width;
+		cam->height = f->fmt.pix.height;
+		cam->params.roi.width = f->fmt.pix.width;
+		cam->params.roi.height = f->fmt.pix.height;
+		cpia2_set_format(cam);
+	}
+
+	for (frame = 0; frame < cam->num_frames; ++frame) {
+		if (cam->buffers[frame].status == FRAME_READING)
+			if ((err = sync(cam, frame)) < 0)
+				return err;
+
+		cam->buffers[frame].status = FRAME_EMPTY;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_get_fmt
+ *
+ *  V4L2 format get
+ *
+ *****************************************************************************/
+
+static int cpia2_g_fmt_vid_cap(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	f->fmt.pix.width = cam->width;
+	f->fmt.pix.height = cam->height;
+	f->fmt.pix.pixelformat = cam->pixelformat;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = cam->frame_size;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_cropcap
+ *
+ *  V4L2 query cropping capabilities
+ *  NOTE: cropping is currently disabled
+ *
+ *****************************************************************************/
+
+static int cpia2_g_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = cam->width;
+		s->r.height = cam->height;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+struct framerate_info {
+	int value;
+	struct v4l2_fract period;
+};
+
+static const struct framerate_info framerate_controls[] = {
+	{ CPIA2_VP_FRAMERATE_6_25, { 4, 25 } },
+	{ CPIA2_VP_FRAMERATE_7_5,  { 2, 15 } },
+	{ CPIA2_VP_FRAMERATE_12_5, { 2, 25 } },
+	{ CPIA2_VP_FRAMERATE_15,   { 1, 15 } },
+	{ CPIA2_VP_FRAMERATE_25,   { 1, 25 } },
+	{ CPIA2_VP_FRAMERATE_30,   { 1, 30 } },
+};
+
+static int cpia2_g_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
+{
+	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
+	int i;
+
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	cap->capability = V4L2_CAP_TIMEPERFRAME;
+	cap->readbuffers = cam->num_frames;
+	for (i = 0; i < ARRAY_SIZE(framerate_controls); i++)
+		if (cam->params.vp_params.frame_rate == framerate_controls[i].value) {
+			cap->timeperframe = framerate_controls[i].period;
+			break;
+		}
+	return 0;
+}
+
+static int cpia2_s_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
+{
+	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
+	struct v4l2_fract tpf = cap->timeperframe;
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int ret;
+	int i;
+
+	ret = cpia2_g_parm(file, fh, p);
+	if (ret || !tpf.denominator || !tpf.numerator)
+		return ret;
+
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	for (i = 0; i <= max; i++) {
+		struct v4l2_fract f1 = tpf;
+		struct v4l2_fract f2 = framerate_controls[i].period;
+
+		f1.numerator *= f2.denominator;
+		f2.numerator *= f1.denominator;
+		if (f1.numerator >= f2.numerator)
+			break;
+	}
+	if (i > max)
+		i = max;
+	cap->timeperframe = framerate_controls[i].period;
+	return cpia2_set_fps(cam, framerate_controls[i].value);
+}
+
+static const struct {
+	u32 width;
+	u32 height;
+} cpia2_framesizes[] = {
+	{ 640, 480 },
+	{ 352, 288 },
+	{ 320, 240 },
+	{ 288, 216 },
+	{ 256, 192 },
+	{ 224, 168 },
+	{ 192, 144 },
+	{ 176, 144 },
+};
+
+static int cpia2_enum_framesizes(struct file *file, void *fh,
+				 struct v4l2_frmsizeenum *fsize)
+{
+	if (fsize->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fsize->pixel_format != V4L2_PIX_FMT_JPEG)
+		return -EINVAL;
+	if (fsize->index >= ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = cpia2_framesizes[fsize->index].width;
+	fsize->discrete.height = cpia2_framesizes[fsize->index].height;
+
+	return 0;
+}
+
+static int cpia2_enum_frameintervals(struct file *file, void *fh,
+				     struct v4l2_frmivalenum *fival)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int i;
+
+	if (fival->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fival->pixel_format != V4L2_PIX_FMT_JPEG)
+		return -EINVAL;
+
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	if (fival->index > max)
+		return -EINVAL;
+	for (i = 0; i < ARRAY_SIZE(cpia2_framesizes); i++)
+		if (fival->width == cpia2_framesizes[i].width &&
+		    fival->height == cpia2_framesizes[i].height)
+			break;
+	if (i == ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = framerate_controls[fival->index].period;
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_s_ctrl
+ *
+ *  V4L2 set the value of a control variable
+ *
+ *****************************************************************************/
+
+static int cpia2_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct camera_data *cam =
+		container_of(ctrl->handler, struct camera_data, hdl);
+	static const int flicker_table[] = {
+		NEVER_FLICKER,
+		FLICKER_50,
+		FLICKER_60,
+	};
+
+	DBG("Set control id:%d, value:%d\n", ctrl->id, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cpia2_set_brightness(cam, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		cpia2_set_contrast(cam, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		cpia2_set_saturation(cam, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		cpia2_set_property_mirror(cam, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		cpia2_set_property_flip(cam, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		return cpia2_set_flicker_mode(cam, flicker_table[ctrl->val]);
+	case V4L2_CID_ILLUMINATORS_1:
+		return cpia2_set_gpio(cam, (cam->top_light->val << 6) |
+					   (cam->bottom_light->val << 7));
+	case V4L2_CID_JPEG_ACTIVE_MARKER:
+		cam->params.compression.inhibit_htables =
+			!(ctrl->val & V4L2_JPEG_ACTIVE_MARKER_DHT);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		cam->params.vc_params.quality = ctrl->val;
+		break;
+	case CPIA2_CID_USB_ALT:
+		cam->params.camera_state.stream_mode = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_g_jpegcomp
+ *
+ *  V4L2 get the JPEG compression parameters
+ *
+ *****************************************************************************/
+
+static int cpia2_g_jpegcomp(struct file *file, void *fh, struct v4l2_jpegcompression *parms)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	memset(parms, 0, sizeof(*parms));
+
+	parms->quality = 80; // TODO: Can this be made meaningful?
+
+	parms->jpeg_markers = V4L2_JPEG_MARKER_DQT | V4L2_JPEG_MARKER_DRI;
+	if (!cam->params.compression.inhibit_htables)
+		parms->jpeg_markers |= V4L2_JPEG_MARKER_DHT;
+
+	parms->APPn = cam->APPn;
+	parms->APP_len = cam->APP_len;
+	if (cam->APP_len > 0) {
+		memcpy(parms->APP_data, cam->APP_data, cam->APP_len);
+		parms->jpeg_markers |= V4L2_JPEG_MARKER_APP;
+	}
+
+	parms->COM_len = cam->COM_len;
+	if (cam->COM_len > 0) {
+		memcpy(parms->COM_data, cam->COM_data, cam->COM_len);
+		parms->jpeg_markers |= JPEG_MARKER_COM;
+	}
+
+	DBG("G_JPEGCOMP APP_len:%d COM_len:%d\n",
+	    parms->APP_len, parms->COM_len);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_s_jpegcomp
+ *
+ *  V4L2 set the JPEG compression parameters
+ *  NOTE: quality and some jpeg_markers are ignored.
+ *
+ *****************************************************************************/
+
+static int cpia2_s_jpegcomp(struct file *file, void *fh,
+			    const struct v4l2_jpegcompression *parms)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	DBG("S_JPEGCOMP APP_len:%d COM_len:%d\n",
+	    parms->APP_len, parms->COM_len);
+
+	cam->params.compression.inhibit_htables =
+		!(parms->jpeg_markers & V4L2_JPEG_MARKER_DHT);
+
+	if (parms->APP_len != 0) {
+		if (parms->APP_len > 0 &&
+		    parms->APP_len <= sizeof(cam->APP_data) &&
+		   parms->APPn >= 0 && parms->APPn <= 15) {
+			cam->APPn = parms->APPn;
+			cam->APP_len = parms->APP_len;
+			memcpy(cam->APP_data, parms->APP_data, parms->APP_len);
+		} else {
+			LOG("Bad APPn Params n=%d len=%d\n",
+			    parms->APPn, parms->APP_len);
+			return -EINVAL;
+		}
+	} else {
+		cam->APP_len = 0;
+	}
+
+	if (parms->COM_len != 0) {
+		if (parms->COM_len > 0 &&
+		    parms->COM_len <= sizeof(cam->COM_data)) {
+			cam->COM_len = parms->COM_len;
+			memcpy(cam->COM_data, parms->COM_data, parms->COM_len);
+		} else {
+			LOG("Bad COM_len=%d\n", parms->COM_len);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_reqbufs
+ *
+ *  V4L2 Initiate memory mapping.
+ *  NOTE: The user's request is ignored. For now the buffers are fixed.
+ *
+ *****************************************************************************/
+
+static int cpia2_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *req)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    req->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	DBG("REQBUFS requested:%d returning:%d\n", req->count, cam->num_frames);
+	req->count = cam->num_frames;
+	memset(&req->reserved, 0, sizeof(req->reserved));
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_querybuf
+ *
+ *  V4L2 Query memory buffer status.
+ *
+ *****************************************************************************/
+
+static int cpia2_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    buf->index >= cam->num_frames)
+		return -EINVAL;
+
+	buf->m.offset = cam->buffers[buf->index].data - cam->frame_buffer;
+	buf->length = cam->frame_size;
+
+	buf->memory = V4L2_MEMORY_MMAP;
+
+	if (cam->mmapped)
+		buf->flags = V4L2_BUF_FLAG_MAPPED;
+	else
+		buf->flags = 0;
+
+	buf->flags |= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	switch (cam->buffers[buf->index].status) {
+	case FRAME_EMPTY:
+	case FRAME_ERROR:
+	case FRAME_READING:
+		buf->bytesused = 0;
+		buf->flags = V4L2_BUF_FLAG_QUEUED;
+		break;
+	case FRAME_READY:
+		buf->bytesused = cam->buffers[buf->index].length;
+		v4l2_buffer_set_timestamp(buf, cam->buffers[buf->index].ts);
+		buf->sequence = cam->buffers[buf->index].seq;
+		buf->flags = V4L2_BUF_FLAG_DONE;
+		break;
+	}
+
+	DBG("QUERYBUF index:%d offset:%d flags:%d seq:%d bytesused:%d\n",
+	    buf->index, buf->m.offset, buf->flags, buf->sequence,
+	    buf->bytesused);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_qbuf
+ *
+ *  V4L2 User is freeing buffer
+ *
+ *****************************************************************************/
+
+static int cpia2_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	struct camera_data *cam = video_drvdata(file);
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    buf->memory != V4L2_MEMORY_MMAP ||
+	   buf->index >= cam->num_frames)
+		return -EINVAL;
+
+	DBG("QBUF #%d\n", buf->index);
+
+	if (cam->buffers[buf->index].status == FRAME_READY)
+		cam->buffers[buf->index].status = FRAME_EMPTY;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  find_earliest_filled_buffer
+ *
+ *  Helper for ioctl_dqbuf. Find the next ready buffer.
+ *
+ *****************************************************************************/
+
+static int find_earliest_filled_buffer(struct camera_data *cam)
+{
+	int i;
+	int found = -1;
+
+	for (i = 0; i < cam->num_frames; i++) {
+		if (cam->buffers[i].status == FRAME_READY) {
+			if (found < 0) {
+				found = i;
+			} else {
+				/* find which buffer is earlier */
+				if (cam->buffers[i].ts < cam->buffers[found].ts)
+					found = i;
+			}
+		}
+	}
+	return found;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_dqbuf
+ *
+ *  V4L2 User is asking for a filled buffer.
+ *
+ *****************************************************************************/
+
+static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int frame;
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	frame = find_earliest_filled_buffer(cam);
+
+	if (frame < 0 && file->f_flags & O_NONBLOCK)
+		return -EAGAIN;
+
+	if (frame < 0) {
+		/* Wait for a frame to become available */
+		struct framebuf *cb = cam->curbuff;
+
+		mutex_unlock(&cam->v4l2_lock);
+		wait_event_interruptible(cam->wq_stream,
+					 !video_is_registered(&cam->vdev) ||
+					 (cb = cam->curbuff)->status == FRAME_READY);
+		mutex_lock(&cam->v4l2_lock);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		if (!video_is_registered(&cam->vdev))
+			return -ENOTTY;
+		frame = cb->num;
+	}
+
+	buf->index = frame;
+	buf->bytesused = cam->buffers[buf->index].length;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_DONE
+		| V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	buf->field = V4L2_FIELD_NONE;
+	v4l2_buffer_set_timestamp(buf, cam->buffers[buf->index].ts);
+	buf->sequence = cam->buffers[buf->index].seq;
+	buf->m.offset = cam->buffers[buf->index].data - cam->frame_buffer;
+	buf->length = cam->frame_size;
+	buf->reserved2 = 0;
+	buf->request_fd = 0;
+	memset(&buf->timecode, 0, sizeof(buf->timecode));
+
+	DBG("DQBUF #%d status:%d seq:%d length:%d\n", buf->index,
+	    cam->buffers[buf->index].status, buf->sequence, buf->bytesused);
+
+	return 0;
+}
+
+static int cpia2_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
+
+	DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
+	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (!cam->streaming) {
+		ret = cpia2_usb_stream_start(cam,
+					     cam->params.camera_state.stream_mode);
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, true);
+	}
+	return ret;
+}
+
+static int cpia2_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
+
+	DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
+	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (cam->streaming) {
+		ret = cpia2_usb_stream_stop(cam);
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, false);
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_mmap
+ *
+ *****************************************************************************/
+static int cpia2_mmap(struct file *file, struct vm_area_struct *area)
+{
+	struct camera_data *cam = video_drvdata(file);
+	int retval;
+
+	if (mutex_lock_interruptible(&cam->v4l2_lock))
+		return -ERESTARTSYS;
+	retval = cpia2_remap_buffer(cam, area);
+
+	if (!retval)
+		cam->stream_fh = file->private_data;
+	mutex_unlock(&cam->v4l2_lock);
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  reset_camera_struct_v4l
+ *
+ *  Sets all values to the defaults
+ *****************************************************************************/
+static void reset_camera_struct_v4l(struct camera_data *cam)
+{
+	cam->width = cam->params.roi.width;
+	cam->height = cam->params.roi.height;
+
+	cam->frame_size = buffer_size;
+	cam->num_frames = num_buffers;
+
+	/* Flicker modes */
+	cam->params.flicker_control.flicker_mode_req = flicker_mode;
+
+	/* stream modes */
+	cam->params.camera_state.stream_mode = alternate;
+
+	cam->pixelformat = V4L2_PIX_FMT_JPEG;
+}
+
+static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
+	.vidioc_querycap		    = cpia2_querycap,
+	.vidioc_enum_input		    = cpia2_enum_input,
+	.vidioc_g_input			    = cpia2_g_input,
+	.vidioc_s_input			    = cpia2_s_input,
+	.vidioc_enum_fmt_vid_cap	    = cpia2_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		    = cpia2_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		    = cpia2_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		    = cpia2_try_fmt_vid_cap,
+	.vidioc_g_jpegcomp		    = cpia2_g_jpegcomp,
+	.vidioc_s_jpegcomp		    = cpia2_s_jpegcomp,
+	.vidioc_g_selection		    = cpia2_g_selection,
+	.vidioc_reqbufs			    = cpia2_reqbufs,
+	.vidioc_querybuf		    = cpia2_querybuf,
+	.vidioc_qbuf			    = cpia2_qbuf,
+	.vidioc_dqbuf			    = cpia2_dqbuf,
+	.vidioc_streamon		    = cpia2_streamon,
+	.vidioc_streamoff		    = cpia2_streamoff,
+	.vidioc_s_parm			    = cpia2_s_parm,
+	.vidioc_g_parm			    = cpia2_g_parm,
+	.vidioc_enum_framesizes		    = cpia2_enum_framesizes,
+	.vidioc_enum_frameintervals	    = cpia2_enum_frameintervals,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+/***
+ * The v4l video device structure initialized for this device
+ ***/
+static const struct v4l2_file_operations cpia2_fops = {
+	.owner		= THIS_MODULE,
+	.open		= cpia2_open,
+	.release	= cpia2_close,
+	.read		= cpia2_v4l_read,
+	.poll		= cpia2_v4l_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= cpia2_mmap,
+};
+
+static const struct video_device cpia2_template = {
+	/* I could not find any place for the old .initialize initializer?? */
+	.name =		"CPiA2 Camera",
+	.fops =		&cpia2_fops,
+	.ioctl_ops =	&cpia2_ioctl_ops,
+	.release =	video_device_release_empty,
+};
+
+void cpia2_camera_release(struct v4l2_device *v4l2_dev)
+{
+	struct camera_data *cam =
+		container_of(v4l2_dev, struct camera_data, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&cam->hdl);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+}
+
+static const struct v4l2_ctrl_ops cpia2_ctrl_ops = {
+	.s_ctrl = cpia2_s_ctrl,
+};
+
+/******************************************************************************
+ *
+ *  cpia2_register_camera
+ *
+ *****************************************************************************/
+int cpia2_register_camera(struct camera_data *cam)
+{
+	struct v4l2_ctrl_handler *hdl = &cam->hdl;
+	struct v4l2_ctrl_config cpia2_usb_alt = {
+		.ops = &cpia2_ctrl_ops,
+		.id = CPIA2_CID_USB_ALT,
+		.name = "USB Alternate",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = USBIF_ISO_1,
+		.max = USBIF_ISO_6,
+		.step = 1,
+	};
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 12);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS,
+			  cam->params.pnp_id.device_type == DEVICE_STV_672 ? 1 : 0,
+			  255, 1, DEFAULT_BRIGHTNESS);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, DEFAULT_CONTRAST);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 255, 1, DEFAULT_SATURATION);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			  V4L2_JPEG_ACTIVE_MARKER_DHT, 0,
+			  V4L2_JPEG_ACTIVE_MARKER_DHT);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			  V4L2_CID_JPEG_COMPRESSION_QUALITY, 1,
+			  100, 1, 100);
+	cpia2_usb_alt.def = alternate;
+	cam->usb_alt = v4l2_ctrl_new_custom(hdl, &cpia2_usb_alt, NULL);
+	/* VP5 Only */
+	if (cam->params.pnp_id.device_type != DEVICE_STV_672)
+		v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+				  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	/* Flicker control only valid for 672 */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		v4l2_ctrl_new_std_menu(hdl, &cpia2_ctrl_ops,
+				       V4L2_CID_POWER_LINE_FREQUENCY,
+				       V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
+				       0, 0);
+	/* Light control only valid for the QX5 Microscope */
+	if (cam->params.pnp_id.product == 0x151) {
+		cam->top_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+						   V4L2_CID_ILLUMINATORS_1,
+						   0, 1, 1, 0);
+		cam->bottom_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+						      V4L2_CID_ILLUMINATORS_2,
+						      0, 1, 1, 0);
+		v4l2_ctrl_cluster(2, &cam->top_light);
+	}
+
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	cam->vdev = cpia2_template;
+	video_set_drvdata(&cam->vdev, cam);
+	cam->vdev.lock = &cam->v4l2_lock;
+	cam->vdev.ctrl_handler = hdl;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	cam->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+				V4L2_CAP_STREAMING;
+
+	reset_camera_struct_v4l(cam);
+
+	/* register v4l device */
+	if (video_register_device(&cam->vdev, VFL_TYPE_VIDEO, video_nr) < 0) {
+		ERR("video_register_device failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_unregister_camera
+ *
+ *****************************************************************************/
+void cpia2_unregister_camera(struct camera_data *cam)
+{
+	video_unregister_device(&cam->vdev);
+}
+
+/******************************************************************************
+ *
+ *  check_parameters
+ *
+ *  Make sure that all user-supplied parameters are sensible
+ *****************************************************************************/
+static void __init check_parameters(void)
+{
+	if (buffer_size < PAGE_SIZE) {
+		buffer_size = PAGE_SIZE;
+		LOG("buffer_size too small, setting to %d\n", buffer_size);
+	} else if (buffer_size > 1024 * 1024) {
+		/* arbitrary upper limiit */
+		buffer_size = 1024 * 1024;
+		LOG("buffer_size ridiculously large, setting to %d\n",
+		    buffer_size);
+	} else {
+		buffer_size += PAGE_SIZE - 1;
+		buffer_size &= ~(PAGE_SIZE - 1);
+	}
+
+	if (num_buffers < 1) {
+		num_buffers = 1;
+		LOG("num_buffers too small, setting to %d\n", num_buffers);
+	} else if (num_buffers > VIDEO_MAX_FRAME) {
+		num_buffers = VIDEO_MAX_FRAME;
+		LOG("num_buffers too large, setting to %d\n", num_buffers);
+	}
+
+	if (alternate < USBIF_ISO_1 || alternate > USBIF_ISO_6) {
+		alternate = DEFAULT_ALT;
+		LOG("alternate specified is invalid, using %d\n", alternate);
+	}
+
+	if (flicker_mode != 0 && flicker_mode != FLICKER_50 && flicker_mode != FLICKER_60) {
+		flicker_mode = 0;
+		LOG("Flicker mode specified is invalid, using %d\n",
+		    flicker_mode);
+	}
+
+	DBG("Using %d buffers, each %d bytes, alternate=%d\n",
+	    num_buffers, buffer_size, alternate);
+}
+
+/************   Module Stuff ***************/
+
+/******************************************************************************
+ *
+ * cpia2_init/module_init
+ *
+ *****************************************************************************/
+static int __init cpia2_init(void)
+{
+	LOG("%s v%s\n",
+	    ABOUT, CPIA_VERSION);
+	check_parameters();
+	return cpia2_usb_init();
+}
+
+/******************************************************************************
+ *
+ * cpia2_exit/module_exit
+ *
+ *****************************************************************************/
+static void __exit cpia2_exit(void)
+{
+	cpia2_usb_cleanup();
+	schedule_timeout(2 * HZ);
+}
+
+module_init(cpia2_init);
+module_exit(cpia2_exit);
diff --git a/drivers/staging/media/deprecated/fsl-viu/Kconfig b/drivers/staging/media/deprecated/fsl-viu/Kconfig
new file mode 100644
index 000000000..399892c69
--- /dev/null
+++ b/drivers/staging/media/deprecated/fsl-viu/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_VIU
+	tristate "NXP VIU Video Driver (DEPRECATED)"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV && (PPC_MPC512x || COMPILE_TEST) && I2C
+	select VIDEOBUF_DMA_CONTIG
+	help
+	  Support for Freescale VIU video driver. This device captures
+	  video data, or overlays video on DIU frame buffer.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y here if you want to enable VIU device on MPC5121e Rev2+.
+	  In doubt, say N.
diff --git a/drivers/staging/media/deprecated/fsl-viu/Makefile b/drivers/staging/media/deprecated/fsl-viu/Makefile
new file mode 100644
index 000000000..931ec56ad
--- /dev/null
+++ b/drivers/staging/media/deprecated/fsl-viu/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
diff --git a/drivers/staging/media/deprecated/fsl-viu/TODO b/drivers/staging/media/deprecated/fsl-viu/TODO
new file mode 100644
index 000000000..ecb30a429
--- /dev/null
+++ b/drivers/staging/media/deprecated/fsl-viu/TODO
@@ -0,0 +1,7 @@
+This is one of the few drivers still not using the vb2
+framework, so this driver is now deprecated with the intent of
+removing it altogether by the beginning of 2023.
+
+In order to keep this driver it has to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/fsl-viu/fsl-viu.c b/drivers/staging/media/deprecated/fsl-viu/fsl-viu.c
new file mode 100644
index 000000000..afc96f6db
--- /dev/null
+++ b/drivers/staging/media/deprecated/fsl-viu/fsl-viu.c
@@ -0,0 +1,1599 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ *  Freescale VIU video driver
+ *
+ *  Authors: Hongjun Chen <hong-jun.chen@freescale.com>
+ *	     Porting to 2.6.35 by DENX Software Engineering,
+ *	     Anatolij Gustschin <agust@denx.de>
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/slab.h>
+#include <media/v4l2-common.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/videobuf-dma-contig.h>
+
+#define DRV_NAME		"fsl_viu"
+#define VIU_VERSION		"0.5.1"
+
+#define BUFFER_TIMEOUT		msecs_to_jiffies(500)  /* 0.5 seconds */
+
+#define	VIU_VID_MEM_LIMIT	4	/* Video memory limit, in Mb */
+
+/* I2C address of video decoder chip is 0x4A */
+#define VIU_VIDEO_DECODER_ADDR	0x25
+
+static int info_level;
+
+#define dprintk(level, fmt, arg...)					\
+	do {								\
+		if (level <= info_level)				\
+			printk(KERN_DEBUG "viu: " fmt , ## arg);	\
+	} while (0)
+
+/*
+ * Basic structures
+ */
+struct viu_fmt {
+	u32   fourcc;		/* v4l2 format id */
+	u32   pixelformat;
+	int   depth;
+};
+
+static struct viu_fmt formats[] = {
+	{
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.pixelformat	= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.pixelformat	= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+	}
+};
+
+struct viu_dev;
+struct viu_buf;
+
+/* buffer for one video frame */
+struct viu_buf {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer vb;
+	struct viu_fmt *fmt;
+};
+
+struct viu_dmaqueue {
+	struct viu_dev		*dev;
+	struct list_head	active;
+	struct list_head	queued;
+	struct timer_list	timeout;
+};
+
+struct viu_status {
+	u32 field_irq;
+	u32 vsync_irq;
+	u32 hsync_irq;
+	u32 vstart_irq;
+	u32 dma_end_irq;
+	u32 error_irq;
+};
+
+struct viu_reg {
+	u32 status_cfg;
+	u32 luminance;
+	u32 chroma_r;
+	u32 chroma_g;
+	u32 chroma_b;
+	u32 field_base_addr;
+	u32 dma_inc;
+	u32 picture_count;
+	u32 req_alarm;
+	u32 alpha;
+} __attribute__ ((packed));
+
+struct viu_dev {
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
+	struct mutex		lock;
+	spinlock_t		slock;
+	int			users;
+
+	struct device		*dev;
+	/* various device info */
+	struct video_device	*vdev;
+	struct viu_dmaqueue	vidq;
+	enum v4l2_field		capfield;
+	int			field;
+	int			first;
+	int			dma_done;
+
+	/* Hardware register area */
+	struct viu_reg __iomem	*vr;
+
+	/* Interrupt vector */
+	int			irq;
+	struct viu_status	irqs;
+
+	/* video overlay */
+	struct v4l2_framebuffer	ovbuf;
+	struct viu_fmt		*ovfmt;
+	unsigned int		ovenable;
+	enum v4l2_field		ovfield;
+
+	/* crop */
+	struct v4l2_rect	crop_current;
+
+	/* clock pointer */
+	struct clk		*clk;
+
+	/* decoder */
+	struct v4l2_subdev	*decoder;
+
+	v4l2_std_id		std;
+};
+
+struct viu_fh {
+	/* must remain the first field of this struct */
+	struct v4l2_fh		fh;
+	struct viu_dev		*dev;
+
+	/* video capture */
+	struct videobuf_queue	vb_vidq;
+	spinlock_t		vbq_lock; /* spinlock for the videobuf queue */
+
+	/* video overlay */
+	struct v4l2_window	win;
+	struct v4l2_clip	clips[1];
+
+	/* video capture */
+	struct viu_fmt		*fmt;
+	int			width, height, sizeimage;
+	enum v4l2_buf_type	type;
+};
+
+static struct viu_reg reg_val;
+
+/*
+ * Macro definitions of VIU registers
+ */
+
+/* STATUS_CONFIG register */
+enum status_config {
+	SOFT_RST		= 1 << 0,
+
+	ERR_MASK		= 0x0f << 4,	/* Error code mask */
+	ERR_NO			= 0x00,		/* No error */
+	ERR_DMA_V		= 0x01 << 4,	/* DMA in vertical active */
+	ERR_DMA_VB		= 0x02 << 4,	/* DMA in vertical blanking */
+	ERR_LINE_TOO_LONG	= 0x04 << 4,	/* Line too long */
+	ERR_TOO_MANG_LINES	= 0x05 << 4,	/* Too many lines in field */
+	ERR_LINE_TOO_SHORT	= 0x06 << 4,	/* Line too short */
+	ERR_NOT_ENOUGH_LINE	= 0x07 << 4,	/* Not enough lines in field */
+	ERR_FIFO_OVERFLOW	= 0x08 << 4,	/* FIFO overflow */
+	ERR_FIFO_UNDERFLOW	= 0x09 << 4,	/* FIFO underflow */
+	ERR_1bit_ECC		= 0x0a << 4,	/* One bit ECC error */
+	ERR_MORE_ECC		= 0x0b << 4,	/* Two/more bits ECC error */
+
+	INT_FIELD_EN		= 0x01 << 8,	/* Enable field interrupt */
+	INT_VSYNC_EN		= 0x01 << 9,	/* Enable vsync interrupt */
+	INT_HSYNC_EN		= 0x01 << 10,	/* Enable hsync interrupt */
+	INT_VSTART_EN		= 0x01 << 11,	/* Enable vstart interrupt */
+	INT_DMA_END_EN		= 0x01 << 12,	/* Enable DMA end interrupt */
+	INT_ERROR_EN		= 0x01 << 13,	/* Enable error interrupt */
+	INT_ECC_EN		= 0x01 << 14,	/* Enable ECC interrupt */
+
+	INT_FIELD_STATUS	= 0x01 << 16,	/* field interrupt status */
+	INT_VSYNC_STATUS	= 0x01 << 17,	/* vsync interrupt status */
+	INT_HSYNC_STATUS	= 0x01 << 18,	/* hsync interrupt status */
+	INT_VSTART_STATUS	= 0x01 << 19,	/* vstart interrupt status */
+	INT_DMA_END_STATUS	= 0x01 << 20,	/* DMA end interrupt status */
+	INT_ERROR_STATUS	= 0x01 << 21,	/* error interrupt status */
+
+	DMA_ACT			= 0x01 << 27,	/* Enable DMA transfer */
+	FIELD_NO		= 0x01 << 28,	/* Field number */
+	DITHER_ON		= 0x01 << 29,	/* Dithering is on */
+	ROUND_ON		= 0x01 << 30,	/* Round is on */
+	MODE_32BIT		= 1UL << 31,	/* Data in RGBa888,
+						 * 0 in RGB565
+						 */
+};
+
+#define norm_maxw()	720
+#define norm_maxh()	576
+
+#define INT_ALL_STATUS	(INT_FIELD_STATUS | INT_VSYNC_STATUS | \
+			 INT_HSYNC_STATUS | INT_VSTART_STATUS | \
+			 INT_DMA_END_STATUS | INT_ERROR_STATUS)
+
+#define NUM_FORMATS	ARRAY_SIZE(formats)
+
+static irqreturn_t viu_intr(int irq, void *dev_id);
+
+static struct viu_fmt *format_by_fourcc(int fourcc)
+{
+	int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].pixelformat == fourcc)
+			return formats + i;
+	}
+
+	dprintk(0, "unknown pixelformat:'%4.4s'\n", (char *)&fourcc);
+	return NULL;
+}
+
+static void viu_start_dma(struct viu_dev *dev)
+{
+	struct viu_reg __iomem *vr = dev->vr;
+
+	dev->field = 0;
+
+	/* Enable DMA operation */
+	iowrite32be(SOFT_RST, &vr->status_cfg);
+	iowrite32be(INT_FIELD_EN, &vr->status_cfg);
+}
+
+static void viu_stop_dma(struct viu_dev *dev)
+{
+	struct viu_reg __iomem *vr = dev->vr;
+	int cnt = 100;
+	u32 status_cfg;
+
+	iowrite32be(0, &vr->status_cfg);
+
+	/* Clear pending interrupts */
+	status_cfg = ioread32be(&vr->status_cfg);
+	if (status_cfg & 0x3f0000)
+		iowrite32be(status_cfg & 0x3f0000, &vr->status_cfg);
+
+	if (status_cfg & DMA_ACT) {
+		do {
+			status_cfg = ioread32be(&vr->status_cfg);
+			if (status_cfg & INT_DMA_END_STATUS)
+				break;
+		} while (cnt--);
+
+		if (cnt < 0) {
+			/* timed out, issue soft reset */
+			iowrite32be(SOFT_RST, &vr->status_cfg);
+			iowrite32be(0, &vr->status_cfg);
+		} else {
+			/* clear DMA_END and other pending irqs */
+			iowrite32be(status_cfg & 0x3f0000, &vr->status_cfg);
+		}
+	}
+
+	dev->field = 0;
+}
+
+static int restart_video_queue(struct viu_dmaqueue *vidq)
+{
+	struct viu_buf *buf, *prev;
+
+	dprintk(1, "%s vidq=%p\n", __func__, vidq);
+	if (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf, vb.queue);
+		dprintk(2, "restart_queue [%p/%d]: restart dma\n",
+			buf, buf->vb.i);
+
+		viu_stop_dma(vidq->dev);
+
+		/* cancel all outstanding capture requests */
+		list_for_each_entry_safe(buf, prev, &vidq->active, vb.queue) {
+			list_del(&buf->vb.queue);
+			buf->vb.state = VIDEOBUF_ERROR;
+			wake_up(&buf->vb.done);
+		}
+		mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+		return 0;
+	}
+
+	prev = NULL;
+	for (;;) {
+		if (list_empty(&vidq->queued))
+			return 0;
+		buf = list_entry(vidq->queued.next, struct viu_buf, vb.queue);
+		if (prev == NULL) {
+			list_move_tail(&buf->vb.queue, &vidq->active);
+
+			dprintk(1, "Restarting video dma\n");
+			viu_stop_dma(vidq->dev);
+			viu_start_dma(vidq->dev);
+
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+			dprintk(2, "[%p/%d] restart_queue - first active\n",
+				buf, buf->vb.i);
+
+		} else if (prev->vb.width  == buf->vb.width  &&
+			   prev->vb.height == buf->vb.height &&
+			   prev->fmt       == buf->fmt) {
+			list_move_tail(&buf->vb.queue, &vidq->active);
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			dprintk(2, "[%p/%d] restart_queue - move to active\n",
+				buf, buf->vb.i);
+		} else {
+			return 0;
+		}
+		prev = buf;
+	}
+}
+
+static void viu_vid_timeout(struct timer_list *t)
+{
+	struct viu_dev *dev = from_timer(dev, t, vidq.timeout);
+	struct viu_buf *buf;
+	struct viu_dmaqueue *vidq = &dev->vidq;
+
+	while (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf, vb.queue);
+		list_del(&buf->vb.queue);
+		buf->vb.state = VIDEOBUF_ERROR;
+		wake_up(&buf->vb.done);
+		dprintk(1, "viu/0: [%p/%d] timeout\n", buf, buf->vb.i);
+	}
+
+	restart_video_queue(vidq);
+}
+
+/*
+ * Videobuf operations
+ */
+static int buffer_setup(struct videobuf_queue *vq, unsigned int *count,
+			unsigned int *size)
+{
+	struct viu_fh *fh = vq->priv_data;
+
+	*size = fh->width * fh->height * fh->fmt->depth >> 3;
+	if (*count == 0)
+		*count = 32;
+
+	while (*size * *count > VIU_VID_MEM_LIMIT * 1024 * 1024)
+		(*count)--;
+
+	dprintk(1, "%s, count=%d, size=%d\n", __func__, *count, *size);
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct viu_buf *buf)
+{
+	struct videobuf_buffer *vb = &buf->vb;
+	void *vaddr = NULL;
+
+	videobuf_waiton(vq, &buf->vb, 0, 0);
+
+	if (vq->int_ops && vq->int_ops->vaddr)
+		vaddr = vq->int_ops->vaddr(vb);
+
+	if (vaddr)
+		videobuf_dma_contig_free(vq, &buf->vb);
+
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+inline int buffer_activate(struct viu_dev *dev, struct viu_buf *buf)
+{
+	struct viu_reg __iomem *vr = dev->vr;
+	int bpp;
+
+	/* setup the DMA base address */
+	reg_val.field_base_addr = videobuf_to_dma_contig(&buf->vb);
+
+	dprintk(1, "buffer_activate [%p/%d]: dma addr 0x%lx\n",
+		buf, buf->vb.i, (unsigned long)reg_val.field_base_addr);
+
+	/* interlace is on by default, set horizontal DMA increment */
+	reg_val.status_cfg = 0;
+	bpp = buf->fmt->depth >> 3;
+	switch (bpp) {
+	case 2:
+		reg_val.status_cfg &= ~MODE_32BIT;
+		reg_val.dma_inc = buf->vb.width * 2;
+		break;
+	case 4:
+		reg_val.status_cfg |= MODE_32BIT;
+		reg_val.dma_inc = buf->vb.width * 4;
+		break;
+	default:
+		dprintk(0, "doesn't support color depth(%d)\n",
+			bpp * 8);
+		return -EINVAL;
+	}
+
+	/* setup picture_count register */
+	reg_val.picture_count = (buf->vb.height / 2) << 16 |
+				buf->vb.width;
+
+	reg_val.status_cfg |= DMA_ACT | INT_DMA_END_EN | INT_FIELD_EN;
+
+	buf->vb.state = VIDEOBUF_ACTIVE;
+	dev->capfield = buf->vb.field;
+
+	/* reset dma increment if needed */
+	if (!V4L2_FIELD_HAS_BOTH(buf->vb.field))
+		reg_val.dma_inc = 0;
+
+	iowrite32be(reg_val.dma_inc, &vr->dma_inc);
+	iowrite32be(reg_val.picture_count, &vr->picture_count);
+	iowrite32be(reg_val.field_base_addr, &vr->field_base_addr);
+	mod_timer(&dev->vidq.timeout, jiffies + BUFFER_TIMEOUT);
+	return 0;
+}
+
+static int buffer_prepare(struct videobuf_queue *vq,
+			  struct videobuf_buffer *vb,
+			  enum v4l2_field field)
+{
+	struct viu_fh  *fh  = vq->priv_data;
+	struct viu_buf *buf = container_of(vb, struct viu_buf, vb);
+	int rc;
+
+	BUG_ON(fh->fmt == NULL);
+
+	if (fh->width  < 48 || fh->width  > norm_maxw() ||
+	    fh->height < 32 || fh->height > norm_maxh())
+		return -EINVAL;
+	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+	if (buf->vb.baddr != 0 && buf->vb.bsize < buf->vb.size)
+		return -EINVAL;
+
+	if (buf->fmt       != fh->fmt	 ||
+	    buf->vb.width  != fh->width  ||
+	    buf->vb.height != fh->height ||
+	    buf->vb.field  != field) {
+		buf->fmt       = fh->fmt;
+		buf->vb.width  = fh->width;
+		buf->vb.height = fh->height;
+		buf->vb.field  = field;
+	}
+
+	if (buf->vb.state == VIDEOBUF_NEEDS_INIT) {
+		rc = videobuf_iolock(vq, &buf->vb, NULL);
+		if (rc != 0)
+			goto fail;
+
+		buf->vb.width  = fh->width;
+		buf->vb.height = fh->height;
+		buf->vb.field  = field;
+		buf->fmt       = fh->fmt;
+	}
+
+	buf->vb.state = VIDEOBUF_PREPARED;
+	return 0;
+
+fail:
+	free_buffer(vq, buf);
+	return rc;
+}
+
+static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct viu_buf       *buf     = container_of(vb, struct viu_buf, vb);
+	struct viu_fh        *fh      = vq->priv_data;
+	struct viu_dev       *dev     = fh->dev;
+	struct viu_dmaqueue  *vidq    = &dev->vidq;
+	struct viu_buf       *prev;
+
+	if (!list_empty(&vidq->queued)) {
+		dprintk(1, "adding vb queue=%p\n", &buf->vb.queue);
+		dprintk(1, "vidq pointer 0x%p, queued 0x%p\n",
+				vidq, &vidq->queued);
+		dprintk(1, "dev %p, queued: self %p, next %p, head %p\n",
+			dev, &vidq->queued, vidq->queued.next,
+			vidq->queued.prev);
+		list_add_tail(&buf->vb.queue, &vidq->queued);
+		buf->vb.state = VIDEOBUF_QUEUED;
+		dprintk(2, "[%p/%d] buffer_queue - append to queued\n",
+			buf, buf->vb.i);
+	} else if (list_empty(&vidq->active)) {
+		dprintk(1, "adding vb active=%p\n", &buf->vb.queue);
+		list_add_tail(&buf->vb.queue, &vidq->active);
+		buf->vb.state = VIDEOBUF_ACTIVE;
+		mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+		dprintk(2, "[%p/%d] buffer_queue - first active\n",
+			buf, buf->vb.i);
+
+		buffer_activate(dev, buf);
+	} else {
+		dprintk(1, "adding vb queue2=%p\n", &buf->vb.queue);
+		prev = list_entry(vidq->active.prev, struct viu_buf, vb.queue);
+		if (prev->vb.width  == buf->vb.width  &&
+		    prev->vb.height == buf->vb.height &&
+		    prev->fmt       == buf->fmt) {
+			list_add_tail(&buf->vb.queue, &vidq->active);
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			dprintk(2, "[%p/%d] buffer_queue - append to active\n",
+				buf, buf->vb.i);
+		} else {
+			list_add_tail(&buf->vb.queue, &vidq->queued);
+			buf->vb.state = VIDEOBUF_QUEUED;
+			dprintk(2, "[%p/%d] buffer_queue - first queued\n",
+				buf, buf->vb.i);
+		}
+	}
+}
+
+static void buffer_release(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb)
+{
+	struct viu_buf *buf  = container_of(vb, struct viu_buf, vb);
+	struct viu_fh  *fh   = vq->priv_data;
+	struct viu_dev *dev  = (struct viu_dev *)fh->dev;
+
+	viu_stop_dma(dev);
+	free_buffer(vq, buf);
+}
+
+static const struct videobuf_queue_ops viu_video_qops = {
+	.buf_setup      = buffer_setup,
+	.buf_prepare    = buffer_prepare,
+	.buf_queue      = buffer_queue,
+	.buf_release    = buffer_release,
+};
+
+/*
+ * IOCTL vidioc handling
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, "viu", sizeof(cap->driver));
+	strscpy(cap->card, "viu", sizeof(cap->card));
+	strscpy(cap->bus_info, "platform:viu", sizeof(cap->bus_info));
+	return 0;
+}
+
+static int vidioc_enum_fmt(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	int index = f->index;
+
+	if (f->index >= NUM_FORMATS)
+		return -EINVAL;
+
+	f->pixelformat = formats[index].fourcc;
+	return 0;
+}
+
+static int vidioc_g_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+
+	f->fmt.pix.width        = fh->width;
+	f->fmt.pix.height       = fh->height;
+	f->fmt.pix.field        = fh->vb_vidq.field;
+	f->fmt.pix.pixelformat  = fh->fmt->pixelformat;
+	f->fmt.pix.bytesperline =
+			(f->fmt.pix.width * fh->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage	= fh->sizeimage;
+	f->fmt.pix.colorspace	= V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int vidioc_try_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fmt *fmt;
+	unsigned int maxw, maxh;
+
+	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (!fmt) {
+		dprintk(1, "Fourcc format (0x%08x) invalid.",
+			f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	maxw  = norm_maxw();
+	maxh  = norm_maxh();
+
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	if (f->fmt.pix.height < 32)
+		f->fmt.pix.height = 32;
+	if (f->fmt.pix.height > maxh)
+		f->fmt.pix.height = maxh;
+	if (f->fmt.pix.width < 48)
+		f->fmt.pix.width = 48;
+	if (f->fmt.pix.width > maxw)
+		f->fmt.pix.width = maxw;
+	f->fmt.pix.width &= ~0x03;
+	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.colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+	int ret;
+
+	ret = vidioc_try_fmt_cap(file, fh, f);
+	if (ret < 0)
+		return ret;
+
+	fh->fmt           = format_by_fourcc(f->fmt.pix.pixelformat);
+	fh->width         = f->fmt.pix.width;
+	fh->height        = f->fmt.pix.height;
+	fh->sizeimage     = f->fmt.pix.sizeimage;
+	fh->vb_vidq.field = f->fmt.pix.field;
+	fh->type          = f->type;
+	return 0;
+}
+
+static int vidioc_g_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+
+	f->fmt.win = fh->win;
+	return 0;
+}
+
+static int verify_preview(struct viu_dev *dev, struct v4l2_window *win)
+{
+	enum v4l2_field field;
+	int maxw, maxh;
+
+	if (dev->ovbuf.base == NULL)
+		return -EINVAL;
+	if (dev->ovfmt == NULL)
+		return -EINVAL;
+	if (win->w.width < 48 || win->w.height < 32)
+		return -EINVAL;
+
+	field = win->field;
+	maxw  = dev->crop_current.width;
+	maxh  = dev->crop_current.height;
+
+	if (field == V4L2_FIELD_ANY) {
+		field = (win->w.height > maxh/2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_TOP;
+	}
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		maxh = maxh / 2;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	win->field = field;
+	if (win->w.width > maxw)
+		win->w.width = maxw;
+	if (win->w.height > maxh)
+		win->w.height = maxh;
+	return 0;
+}
+
+inline void viu_activate_overlay(struct viu_reg __iomem *vr)
+{
+	iowrite32be(reg_val.field_base_addr, &vr->field_base_addr);
+	iowrite32be(reg_val.dma_inc, &vr->dma_inc);
+	iowrite32be(reg_val.picture_count, &vr->picture_count);
+}
+
+static int viu_setup_preview(struct viu_dev *dev, struct viu_fh *fh)
+{
+	int bpp;
+
+	dprintk(1, "%s %dx%d\n", __func__,
+		fh->win.w.width, fh->win.w.height);
+
+	reg_val.status_cfg = 0;
+
+	/* setup window */
+	reg_val.picture_count = (fh->win.w.height / 2) << 16 |
+				fh->win.w.width;
+
+	/* setup color depth and dma increment */
+	bpp = dev->ovfmt->depth / 8;
+	switch (bpp) {
+	case 2:
+		reg_val.status_cfg &= ~MODE_32BIT;
+		reg_val.dma_inc = fh->win.w.width * 2;
+		break;
+	case 4:
+		reg_val.status_cfg |= MODE_32BIT;
+		reg_val.dma_inc = fh->win.w.width * 4;
+		break;
+	default:
+		dprintk(0, "device doesn't support color depth(%d)\n",
+			bpp * 8);
+		return -EINVAL;
+	}
+
+	dev->ovfield = fh->win.field;
+	if (!V4L2_FIELD_HAS_BOTH(dev->ovfield))
+		reg_val.dma_inc = 0;
+
+	reg_val.status_cfg |= DMA_ACT | INT_DMA_END_EN | INT_FIELD_EN;
+
+	/* setup the base address of the overlay buffer */
+	reg_val.field_base_addr = (u32)(long)dev->ovbuf.base;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh  *fh  = priv;
+	struct viu_dev *dev = (struct viu_dev *)fh->dev;
+	unsigned long  flags;
+	int err;
+
+	err = verify_preview(dev, &f->fmt.win);
+	if (err)
+		return err;
+
+	fh->win = f->fmt.win;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	viu_setup_preview(dev, fh);
+	spin_unlock_irqrestore(&dev->slock, flags);
+	return 0;
+}
+
+static int vidioc_try_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	return 0;
+}
+
+static int vidioc_overlay(struct file *file, void *priv, unsigned int on)
+{
+	struct viu_fh  *fh  = priv;
+	struct viu_dev *dev = (struct viu_dev *)fh->dev;
+	unsigned long  flags;
+
+	if (on) {
+		spin_lock_irqsave(&dev->slock, flags);
+		viu_activate_overlay(dev->vr);
+		dev->ovenable = 1;
+
+		/* start dma */
+		viu_start_dma(dev);
+		spin_unlock_irqrestore(&dev->slock, flags);
+	} else {
+		viu_stop_dma(dev);
+		dev->ovenable = 0;
+	}
+
+	return 0;
+}
+
+static int vidioc_g_fbuf(struct file *file, void *priv, struct v4l2_framebuffer *arg)
+{
+	struct viu_fh  *fh = priv;
+	struct viu_dev *dev = fh->dev;
+	struct v4l2_framebuffer *fb = arg;
+
+	*fb = dev->ovbuf;
+	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	return 0;
+}
+
+static int vidioc_s_fbuf(struct file *file, void *priv, const struct v4l2_framebuffer *arg)
+{
+	struct viu_fh  *fh = priv;
+	struct viu_dev *dev = fh->dev;
+	const struct v4l2_framebuffer *fb = arg;
+	struct viu_fmt *fmt;
+
+	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	/* check args */
+	fmt = format_by_fourcc(fb->fmt.pixelformat);
+	if (fmt == NULL)
+		return -EINVAL;
+
+	/* ok, accept it */
+	dev->ovbuf = *fb;
+	dev->ovfmt = fmt;
+	if (dev->ovbuf.fmt.bytesperline == 0) {
+		dev->ovbuf.fmt.bytesperline =
+			dev->ovbuf.fmt.width * fmt->depth / 8;
+	}
+	return 0;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+				struct v4l2_requestbuffers *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_reqbufs(&fh->vb_vidq, p);
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+					struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_querybuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_qbuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_dqbuf(&fh->vb_vidq, p,
+				file->f_flags & O_NONBLOCK);
+}
+
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct viu_fh *fh = priv;
+	struct viu_dev *dev = fh->dev;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (fh->type != i)
+		return -EINVAL;
+
+	if (dev->ovenable)
+		dev->ovenable = 0;
+
+	viu_start_dma(fh->dev);
+
+	return videobuf_streamon(&fh->vb_vidq);
+}
+
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct viu_fh  *fh = priv;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (fh->type != i)
+		return -EINVAL;
+
+	viu_stop_dma(fh->dev);
+
+	return videobuf_streamoff(&fh->vb_vidq);
+}
+
+#define decoder_call(viu, o, f, args...) \
+	v4l2_subdev_call(viu->decoder, o, f, ##args)
+
+static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct viu_fh *fh = priv;
+
+	decoder_call(fh->dev, video, querystd, std_id);
+	return 0;
+}
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct viu_fh *fh = priv;
+
+	fh->dev->std = id;
+	decoder_call(fh->dev, video, s_std, id);
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct viu_fh *fh = priv;
+
+	*std_id = fh->dev->std;
+	return 0;
+}
+
+/* only one input in this driver */
+static int vidioc_enum_input(struct file *file, void *priv,
+					struct v4l2_input *inp)
+{
+	struct viu_fh *fh = priv;
+
+	if (inp->index != 0)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = fh->dev->vdev->tvnorms;
+	strscpy(inp->name, "Camera", sizeof(inp->name));
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct viu_fh *fh = priv;
+
+	if (i)
+		return -EINVAL;
+
+	decoder_call(fh->dev, video, s_routing, i, 0, 0);
+	return 0;
+}
+
+inline void viu_activate_next_buf(struct viu_dev *dev,
+				struct viu_dmaqueue *viuq)
+{
+	struct viu_dmaqueue *vidq = viuq;
+	struct viu_buf *buf;
+
+	/* launch another DMA operation for an active/queued buffer */
+	if (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf,
+					vb.queue);
+		dprintk(1, "start another queued buffer: 0x%p\n", buf);
+		buffer_activate(dev, buf);
+	} else if (!list_empty(&vidq->queued)) {
+		buf = list_entry(vidq->queued.next, struct viu_buf,
+					vb.queue);
+		list_del(&buf->vb.queue);
+
+		dprintk(1, "start another queued buffer: 0x%p\n", buf);
+		list_add_tail(&buf->vb.queue, &vidq->active);
+		buf->vb.state = VIDEOBUF_ACTIVE;
+		buffer_activate(dev, buf);
+	}
+}
+
+inline void viu_default_settings(struct viu_reg __iomem *vr)
+{
+	iowrite32be(0x9512A254, &vr->luminance);
+	iowrite32be(0x03310000, &vr->chroma_r);
+	iowrite32be(0x06600F38, &vr->chroma_g);
+	iowrite32be(0x00000409, &vr->chroma_b);
+	iowrite32be(0x000000ff, &vr->alpha);
+	iowrite32be(0x00000090, &vr->req_alarm);
+	dprintk(1, "status reg: 0x%08x, field base: 0x%08x\n",
+		ioread32be(&vr->status_cfg), ioread32be(&vr->field_base_addr));
+}
+
+static void viu_overlay_intr(struct viu_dev *dev, u32 status)
+{
+	struct viu_reg __iomem *vr = dev->vr;
+
+	if (status & INT_DMA_END_STATUS)
+		dev->dma_done = 1;
+
+	if (status & INT_FIELD_STATUS) {
+		if (dev->dma_done) {
+			u32 addr = reg_val.field_base_addr;
+
+			dev->dma_done = 0;
+			if (status & FIELD_NO)
+				addr += reg_val.dma_inc;
+
+			iowrite32be(addr, &vr->field_base_addr);
+			iowrite32be(reg_val.dma_inc, &vr->dma_inc);
+			iowrite32be((status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg, &vr->status_cfg);
+		} else if (status & INT_VSYNC_STATUS) {
+			iowrite32be((status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg, &vr->status_cfg);
+		}
+	}
+}
+
+static void viu_capture_intr(struct viu_dev *dev, u32 status)
+{
+	struct viu_dmaqueue *vidq = &dev->vidq;
+	struct viu_reg __iomem *vr = dev->vr;
+	struct viu_buf *buf;
+	int field_num;
+	int need_two;
+	int dma_done = 0;
+
+	field_num = status & FIELD_NO;
+	need_two = V4L2_FIELD_HAS_BOTH(dev->capfield);
+
+	if (status & INT_DMA_END_STATUS) {
+		dma_done = 1;
+		if (((field_num == 0) && (dev->field == 0)) ||
+		    (field_num && (dev->field == 1)))
+			dev->field++;
+	}
+
+	if (status & INT_FIELD_STATUS) {
+		dprintk(1, "irq: field %d, done %d\n",
+			!!field_num, dma_done);
+		if (unlikely(dev->first)) {
+			if (field_num == 0) {
+				dev->first = 0;
+				dprintk(1, "activate first buf\n");
+				viu_activate_next_buf(dev, vidq);
+			} else
+				dprintk(1, "wait field 0\n");
+			return;
+		}
+
+		/* setup buffer address for next dma operation */
+		if (!list_empty(&vidq->active)) {
+			u32 addr = reg_val.field_base_addr;
+
+			if (field_num && need_two) {
+				addr += reg_val.dma_inc;
+				dprintk(1, "field 1, 0x%lx, dev field %d\n",
+					(unsigned long)addr, dev->field);
+			}
+			iowrite32be(addr, &vr->field_base_addr);
+			iowrite32be(reg_val.dma_inc, &vr->dma_inc);
+			iowrite32be((status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg, &vr->status_cfg);
+			return;
+		}
+	}
+
+	if (dma_done && field_num && (dev->field == 2)) {
+		dev->field = 0;
+		buf = list_entry(vidq->active.next,
+				 struct viu_buf, vb.queue);
+		dprintk(1, "viu/0: [%p/%d] 0x%lx/0x%lx: dma complete\n",
+			buf, buf->vb.i,
+			(unsigned long)videobuf_to_dma_contig(&buf->vb),
+			(unsigned long)ioread32be(&vr->field_base_addr));
+
+		if (waitqueue_active(&buf->vb.done)) {
+			list_del(&buf->vb.queue);
+			buf->vb.ts = ktime_get_ns();
+			buf->vb.state = VIDEOBUF_DONE;
+			buf->vb.field_count++;
+			wake_up(&buf->vb.done);
+		}
+		/* activate next dma buffer */
+		viu_activate_next_buf(dev, vidq);
+	}
+}
+
+static irqreturn_t viu_intr(int irq, void *dev_id)
+{
+	struct viu_dev *dev  = (struct viu_dev *)dev_id;
+	struct viu_reg __iomem *vr = dev->vr;
+	u32 status;
+	u32 error;
+
+	status = ioread32be(&vr->status_cfg);
+
+	if (status & INT_ERROR_STATUS) {
+		dev->irqs.error_irq++;
+		error = status & ERR_MASK;
+		if (error)
+			dprintk(1, "Err: error(%d), times:%d!\n",
+				error >> 4, dev->irqs.error_irq);
+		/* Clear interrupt error bit and error flags */
+		iowrite32be((status & 0xffc0ffff) | INT_ERROR_STATUS,
+			    &vr->status_cfg);
+	}
+
+	if (status & INT_DMA_END_STATUS) {
+		dev->irqs.dma_end_irq++;
+		dev->dma_done = 1;
+		dprintk(2, "VIU DMA end interrupt times: %d\n",
+					dev->irqs.dma_end_irq);
+	}
+
+	if (status & INT_HSYNC_STATUS)
+		dev->irqs.hsync_irq++;
+
+	if (status & INT_FIELD_STATUS) {
+		dev->irqs.field_irq++;
+		dprintk(2, "VIU field interrupt times: %d\n",
+					dev->irqs.field_irq);
+	}
+
+	if (status & INT_VSTART_STATUS)
+		dev->irqs.vstart_irq++;
+
+	if (status & INT_VSYNC_STATUS) {
+		dev->irqs.vsync_irq++;
+		dprintk(2, "VIU vsync interrupt times: %d\n",
+			dev->irqs.vsync_irq);
+	}
+
+	/* clear all pending irqs */
+	status = ioread32be(&vr->status_cfg);
+	iowrite32be((status & 0xffc0ffff) | (status & INT_ALL_STATUS),
+		    &vr->status_cfg);
+
+	if (dev->ovenable) {
+		viu_overlay_intr(dev, status);
+		return IRQ_HANDLED;
+	}
+
+	/* Capture mode */
+	viu_capture_intr(dev, status);
+	return IRQ_HANDLED;
+}
+
+/*
+ * File operations for the device
+ */
+static int viu_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct viu_dev *dev = video_get_drvdata(vdev);
+	struct viu_fh *fh;
+	struct viu_reg __iomem *vr;
+	int minor = vdev->minor;
+	u32 status_cfg;
+
+	dprintk(1, "viu: open (minor=%d)\n", minor);
+
+	dev->users++;
+	if (dev->users > 1) {
+		dev->users--;
+		return -EBUSY;
+	}
+
+	vr = dev->vr;
+
+	dprintk(1, "open minor=%d type=%s users=%d\n", minor,
+		v4l2_type_names[V4L2_BUF_TYPE_VIDEO_CAPTURE], dev->users);
+
+	if (mutex_lock_interruptible(&dev->lock)) {
+		dev->users--;
+		return -ERESTARTSYS;
+	}
+
+	/* allocate and initialize per filehandle data */
+	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+	if (!fh) {
+		dev->users--;
+		mutex_unlock(&dev->lock);
+		return -ENOMEM;
+	}
+
+	v4l2_fh_init(&fh->fh, vdev);
+	file->private_data = fh;
+	fh->dev = dev;
+
+	fh->type     = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_RGB32);
+	fh->width    = norm_maxw();
+	fh->height   = norm_maxh();
+	dev->crop_current.width  = fh->width;
+	dev->crop_current.height = fh->height;
+
+	dprintk(1, "Open: fh=%p, dev=%p, dev->vidq=%p\n", fh, dev, &dev->vidq);
+	dprintk(1, "Open: list_empty queued=%d\n",
+		list_empty(&dev->vidq.queued));
+	dprintk(1, "Open: list_empty active=%d\n",
+		list_empty(&dev->vidq.active));
+
+	viu_default_settings(vr);
+
+	status_cfg = ioread32be(&vr->status_cfg);
+	iowrite32be(status_cfg & ~(INT_VSYNC_EN | INT_HSYNC_EN |
+				INT_FIELD_EN | INT_VSTART_EN |
+				INT_DMA_END_EN | INT_ERROR_EN | INT_ECC_EN),
+		    &vr->status_cfg);
+
+	status_cfg = ioread32be(&vr->status_cfg);
+	iowrite32be(status_cfg | INT_ALL_STATUS, &vr->status_cfg);
+
+	spin_lock_init(&fh->vbq_lock);
+	videobuf_queue_dma_contig_init(&fh->vb_vidq, &viu_video_qops,
+				       dev->dev, &fh->vbq_lock,
+				       fh->type, V4L2_FIELD_INTERLACED,
+				       sizeof(struct viu_buf), fh,
+				       &fh->dev->lock);
+	v4l2_fh_add(&fh->fh);
+	mutex_unlock(&dev->lock);
+	return 0;
+}
+
+static ssize_t viu_read(struct file *file, char __user *data, size_t count,
+			loff_t *ppos)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int ret = 0;
+
+	dprintk(2, "%s\n", __func__);
+	if (dev->ovenable)
+		dev->ovenable = 0;
+
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		if (mutex_lock_interruptible(&dev->lock))
+			return -ERESTARTSYS;
+		viu_start_dma(dev);
+		ret = videobuf_read_stream(&fh->vb_vidq, data, count,
+				ppos, 0, file->f_flags & O_NONBLOCK);
+		mutex_unlock(&dev->lock);
+		return ret;
+	}
+	return 0;
+}
+
+static __poll_t viu_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct viu_fh *fh = file->private_data;
+	struct videobuf_queue *q = &fh->vb_vidq;
+	struct viu_dev *dev = fh->dev;
+	__poll_t req_events = poll_requested_events(wait);
+	__poll_t res = v4l2_ctrl_poll(file, wait);
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != fh->type)
+		return EPOLLERR;
+
+	if (!(req_events & (EPOLLIN | EPOLLRDNORM)))
+		return res;
+
+	mutex_lock(&dev->lock);
+	res |= videobuf_poll_stream(file, q, wait);
+	mutex_unlock(&dev->lock);
+	return res;
+}
+
+static int viu_release(struct file *file)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int minor = video_devdata(file)->minor;
+
+	mutex_lock(&dev->lock);
+	viu_stop_dma(dev);
+	videobuf_stop(&fh->vb_vidq);
+	videobuf_mmap_free(&fh->vb_vidq);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	mutex_unlock(&dev->lock);
+
+	kfree(fh);
+
+	dev->users--;
+	dprintk(1, "close (minor=%d, users=%d)\n",
+		minor, dev->users);
+	return 0;
+}
+
+static void viu_reset(struct viu_reg __iomem *reg)
+{
+	iowrite32be(0, &reg->status_cfg);
+	iowrite32be(0x9512a254, &reg->luminance);
+	iowrite32be(0x03310000, &reg->chroma_r);
+	iowrite32be(0x06600f38, &reg->chroma_g);
+	iowrite32be(0x00000409, &reg->chroma_b);
+	iowrite32be(0, &reg->field_base_addr);
+	iowrite32be(0, &reg->dma_inc);
+	iowrite32be(0x01e002d0, &reg->picture_count);
+	iowrite32be(0x00000090, &reg->req_alarm);
+	iowrite32be(0x000000ff, &reg->alpha);
+}
+
+static int viu_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int ret;
+
+	dprintk(1, "mmap called, vma=%p\n", vma);
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	ret = videobuf_mmap_mapper(&fh->vb_vidq, vma);
+	mutex_unlock(&dev->lock);
+
+	dprintk(1, "vma start=0x%08lx, size=%ld, ret=%d\n",
+		(unsigned long)vma->vm_start,
+		(unsigned long)vma->vm_end-(unsigned long)vma->vm_start,
+		ret);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations viu_fops = {
+	.owner		= THIS_MODULE,
+	.open		= viu_open,
+	.release	= viu_release,
+	.read		= viu_read,
+	.poll		= viu_poll,
+	.unlocked_ioctl	= video_ioctl2, /* V4L2 ioctl handler */
+	.mmap		= viu_mmap,
+};
+
+static const struct v4l2_ioctl_ops viu_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_cap,
+	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_cap,
+	.vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_overlay = vidioc_g_fmt_overlay,
+	.vidioc_try_fmt_vid_overlay = vidioc_try_fmt_overlay,
+	.vidioc_s_fmt_vid_overlay = vidioc_s_fmt_overlay,
+	.vidioc_overlay	      = vidioc_overlay,
+	.vidioc_g_fbuf	      = vidioc_g_fbuf,
+	.vidioc_s_fbuf	      = vidioc_s_fbuf,
+	.vidioc_reqbufs       = vidioc_reqbufs,
+	.vidioc_querybuf      = vidioc_querybuf,
+	.vidioc_qbuf          = vidioc_qbuf,
+	.vidioc_dqbuf         = vidioc_dqbuf,
+	.vidioc_g_std         = vidioc_g_std,
+	.vidioc_s_std         = vidioc_s_std,
+	.vidioc_querystd      = vidioc_querystd,
+	.vidioc_enum_input    = vidioc_enum_input,
+	.vidioc_g_input       = vidioc_g_input,
+	.vidioc_s_input       = vidioc_s_input,
+	.vidioc_streamon      = vidioc_streamon,
+	.vidioc_streamoff     = vidioc_streamoff,
+	.vidioc_log_status    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct video_device viu_template = {
+	.name		= "FSL viu",
+	.fops		= &viu_fops,
+	.minor		= -1,
+	.ioctl_ops	= &viu_ioctl_ops,
+	.release	= video_device_release,
+
+	.tvnorms        = V4L2_STD_NTSC_M | V4L2_STD_PAL,
+	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+			  V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_READWRITE,
+};
+
+static int viu_of_probe(struct platform_device *op)
+{
+	struct viu_dev *viu_dev;
+	struct video_device *vdev;
+	struct resource r;
+	struct viu_reg __iomem *viu_regs;
+	struct i2c_adapter *ad;
+	int ret, viu_irq;
+	struct clk *clk;
+
+	ret = of_address_to_resource(op->dev.of_node, 0, &r);
+	if (ret) {
+		dev_err(&op->dev, "Can't parse device node resource\n");
+		return -ENODEV;
+	}
+
+	viu_irq = irq_of_parse_and_map(op->dev.of_node, 0);
+	if (!viu_irq) {
+		dev_err(&op->dev, "Error while mapping the irq\n");
+		return -EINVAL;
+	}
+
+	/* request mem region */
+	if (!devm_request_mem_region(&op->dev, r.start,
+				     sizeof(struct viu_reg), DRV_NAME)) {
+		dev_err(&op->dev, "Error while requesting mem region\n");
+		ret = -EBUSY;
+		goto err_irq;
+	}
+
+	/* remap registers */
+	viu_regs = devm_ioremap(&op->dev, r.start, sizeof(struct viu_reg));
+	if (!viu_regs) {
+		dev_err(&op->dev, "Can't map register set\n");
+		ret = -ENOMEM;
+		goto err_irq;
+	}
+
+	/* Prepare our private structure */
+	viu_dev = devm_kzalloc(&op->dev, sizeof(struct viu_dev), GFP_KERNEL);
+	if (!viu_dev) {
+		dev_err(&op->dev, "Can't allocate private structure\n");
+		ret = -ENOMEM;
+		goto err_irq;
+	}
+
+	viu_dev->vr = viu_regs;
+	viu_dev->irq = viu_irq;
+	viu_dev->dev = &op->dev;
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&viu_dev->vidq.active);
+	INIT_LIST_HEAD(&viu_dev->vidq.queued);
+
+	snprintf(viu_dev->v4l2_dev.name,
+		 sizeof(viu_dev->v4l2_dev.name), "%s", "VIU");
+	ret = v4l2_device_register(viu_dev->dev, &viu_dev->v4l2_dev);
+	if (ret < 0) {
+		dev_err(&op->dev, "v4l2_device_register() failed: %d\n", ret);
+		goto err_irq;
+	}
+
+	ad = i2c_get_adapter(0);
+	if (!ad) {
+		ret = -EFAULT;
+		dev_err(&op->dev, "couldn't get i2c adapter\n");
+		goto err_v4l2;
+	}
+
+	v4l2_ctrl_handler_init(&viu_dev->hdl, 5);
+	if (viu_dev->hdl.error) {
+		ret = viu_dev->hdl.error;
+		dev_err(&op->dev, "couldn't register control\n");
+		goto err_i2c;
+	}
+	/* This control handler will inherit the control(s) from the
+	   sub-device(s). */
+	viu_dev->v4l2_dev.ctrl_handler = &viu_dev->hdl;
+	viu_dev->decoder = v4l2_i2c_new_subdev(&viu_dev->v4l2_dev, ad,
+			"saa7113", VIU_VIDEO_DECODER_ADDR, NULL);
+
+	timer_setup(&viu_dev->vidq.timeout, viu_vid_timeout, 0);
+	viu_dev->std = V4L2_STD_NTSC_M;
+	viu_dev->first = 1;
+
+	/* Allocate memory for video device */
+	vdev = video_device_alloc();
+	if (vdev == NULL) {
+		ret = -ENOMEM;
+		goto err_hdl;
+	}
+
+	*vdev = viu_template;
+
+	vdev->v4l2_dev = &viu_dev->v4l2_dev;
+
+	viu_dev->vdev = vdev;
+
+	/* initialize locks */
+	mutex_init(&viu_dev->lock);
+	viu_dev->vdev->lock = &viu_dev->lock;
+	spin_lock_init(&viu_dev->slock);
+
+	video_set_drvdata(viu_dev->vdev, viu_dev);
+
+	mutex_lock(&viu_dev->lock);
+
+	ret = video_register_device(viu_dev->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		video_device_release(viu_dev->vdev);
+		goto err_unlock;
+	}
+
+	/* enable VIU clock */
+	clk = devm_clk_get(&op->dev, "ipg");
+	if (IS_ERR(clk)) {
+		dev_err(&op->dev, "failed to lookup the clock!\n");
+		ret = PTR_ERR(clk);
+		goto err_vdev;
+	}
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(&op->dev, "failed to enable the clock!\n");
+		goto err_vdev;
+	}
+	viu_dev->clk = clk;
+
+	/* reset VIU module */
+	viu_reset(viu_dev->vr);
+
+	/* install interrupt handler */
+	if (request_irq(viu_dev->irq, viu_intr, 0, "viu", (void *)viu_dev)) {
+		dev_err(&op->dev, "Request VIU IRQ failed.\n");
+		ret = -ENODEV;
+		goto err_clk;
+	}
+
+	mutex_unlock(&viu_dev->lock);
+
+	dev_info(&op->dev, "Freescale VIU Video Capture Board\n");
+	return ret;
+
+err_clk:
+	clk_disable_unprepare(viu_dev->clk);
+err_vdev:
+	video_unregister_device(viu_dev->vdev);
+err_unlock:
+	mutex_unlock(&viu_dev->lock);
+err_hdl:
+	v4l2_ctrl_handler_free(&viu_dev->hdl);
+err_i2c:
+	i2c_put_adapter(ad);
+err_v4l2:
+	v4l2_device_unregister(&viu_dev->v4l2_dev);
+err_irq:
+	irq_dispose_mapping(viu_irq);
+	return ret;
+}
+
+static int viu_of_remove(struct platform_device *op)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(op);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+	struct v4l2_subdev *sdev = list_entry(v4l2_dev->subdevs.next,
+					      struct v4l2_subdev, list);
+	struct i2c_client *client = v4l2_get_subdevdata(sdev);
+
+	free_irq(dev->irq, (void *)dev);
+	irq_dispose_mapping(dev->irq);
+
+	clk_disable_unprepare(dev->clk);
+
+	v4l2_ctrl_handler_free(&dev->hdl);
+	video_unregister_device(dev->vdev);
+	i2c_put_adapter(client->adapter);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int viu_suspend(struct platform_device *op, pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(op);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+
+	clk_disable(dev->clk);
+	return 0;
+}
+
+static int viu_resume(struct platform_device *op)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(op);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+
+	clk_enable(dev->clk);
+	return 0;
+}
+#endif
+
+/*
+ * Initialization and module stuff
+ */
+static const struct of_device_id mpc512x_viu_of_match[] = {
+	{
+		.compatible = "fsl,mpc5121-viu",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mpc512x_viu_of_match);
+
+static struct platform_driver viu_of_platform_driver = {
+	.probe = viu_of_probe,
+	.remove = viu_of_remove,
+#ifdef CONFIG_PM
+	.suspend = viu_suspend,
+	.resume = viu_resume,
+#endif
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = mpc512x_viu_of_match,
+	},
+};
+
+module_platform_driver(viu_of_platform_driver);
+
+MODULE_DESCRIPTION("Freescale Video-In(VIU)");
+MODULE_AUTHOR("Hongjun Chen");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VIU_VERSION);
diff --git a/drivers/staging/media/deprecated/meye/Kconfig b/drivers/staging/media/deprecated/meye/Kconfig
new file mode 100644
index 000000000..f135f8568
--- /dev/null
+++ b/drivers/staging/media/deprecated/meye/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_MEYE
+	tristate "Sony Vaio Picturebook Motion Eye Video For Linux (DEPRECATED)"
+	depends on PCI && VIDEO_DEV
+	depends on SONY_LAPTOP
+	depends on X86 || COMPILE_TEST
+	help
+	  This is the video4linux driver for the Motion Eye camera found
+	  in the Vaio Picturebook laptops. Please read the material in
+	  <file:Documentation/admin-guide/media/meye.rst> for more information.
+
+	  If you say Y or M here, you need to say Y or M to "Sony Laptop
+	  Extras" in the misc device section.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called meye.
diff --git a/drivers/staging/media/deprecated/meye/Makefile b/drivers/staging/media/deprecated/meye/Makefile
new file mode 100644
index 000000000..36f1f86f0
--- /dev/null
+++ b/drivers/staging/media/deprecated/meye/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_VIDEO_MEYE) += meye.o
diff --git a/drivers/staging/media/deprecated/meye/TODO b/drivers/staging/media/deprecated/meye/TODO
new file mode 100644
index 000000000..6d1d1433d
--- /dev/null
+++ b/drivers/staging/media/deprecated/meye/TODO
@@ -0,0 +1,6 @@
+The meye driver does not use the vb2 framework for streaming
+video, instead it implements this in the driver.
+
+To prevent removal of this driver early 2023 it has to be
+converted to use vb2. Contact the linux-media@vger.kernel.org
+mailing list if you want to do this.
diff --git a/drivers/staging/media/deprecated/meye/meye.c b/drivers/staging/media/deprecated/meye/meye.c
new file mode 100644
index 000000000..5d87efd9b
--- /dev/null
+++ b/drivers/staging/media/deprecated/meye/meye.c
@@ -0,0 +1,1814 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Motion Eye video4linux driver for Sony Vaio PictureBook
+ *
+ * Copyright (C) 2001-2004 Stelian Pop <stelian@popies.net>
+ *
+ * Copyright (C) 2001-2002 Alcôve <www.alcove.com>
+ *
+ * Copyright (C) 2000 Andrew Tridgell <tridge@valinux.com>
+ *
+ * Earlier work by Werner Almesberger, Paul `Rusty' Russell and Paul Mackerras.
+ *
+ * Some parts borrowed from various video4linux drivers, especially
+ * bttv-driver.c and zoran.c, see original files for credits.
+ */
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/sched.h>
+#include <linux/init.h>
+#include <linux/gfp.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <linux/uaccess.h>
+#include <asm/io.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/vmalloc.h>
+#include <linux/dma-mapping.h>
+
+#include "meye.h"
+#include <linux/meye.h>
+
+MODULE_AUTHOR("Stelian Pop <stelian@popies.net>");
+MODULE_DESCRIPTION("v4l2 driver for the MotionEye camera");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(MEYE_DRIVER_VERSION);
+
+/* number of grab buffers */
+static unsigned int gbuffers = 2;
+module_param(gbuffers, int, 0444);
+MODULE_PARM_DESC(gbuffers, "number of capture buffers, default is 2 (32 max)");
+
+/* size of a grab buffer */
+static unsigned int gbufsize = MEYE_MAX_BUFSIZE;
+module_param(gbufsize, int, 0444);
+MODULE_PARM_DESC(gbufsize, "size of the capture buffers, default is 614400 (will be rounded up to a page multiple)");
+
+/* /dev/videoX registration number */
+static int video_nr = -1;
+module_param(video_nr, int, 0444);
+MODULE_PARM_DESC(video_nr, "video device to register (0=/dev/video0, etc)");
+
+/* driver structure - only one possible */
+static struct meye meye;
+
+/****************************************************************************/
+/* Memory allocation routines (stolen from bttv-driver.c)                   */
+/****************************************************************************/
+static void *rvmalloc(unsigned long size)
+{
+	void *mem;
+	unsigned long adr;
+
+	size = PAGE_ALIGN(size);
+	mem = vmalloc_32(size);
+	if (mem) {
+		memset(mem, 0, size);
+		adr = (unsigned long) mem;
+		while (size > 0) {
+			SetPageReserved(vmalloc_to_page((void *)adr));
+			adr += PAGE_SIZE;
+			size -= PAGE_SIZE;
+		}
+	}
+	return mem;
+}
+
+static void rvfree(void * mem, unsigned long size)
+{
+	unsigned long adr;
+
+	if (mem) {
+		adr = (unsigned long) mem;
+		while ((long) size > 0) {
+			ClearPageReserved(vmalloc_to_page((void *)adr));
+			adr += PAGE_SIZE;
+			size -= PAGE_SIZE;
+		}
+		vfree(mem);
+	}
+}
+
+/*
+ * return a page table pointing to N pages of locked memory
+ *
+ * NOTE: The meye device expects DMA addresses on 32 bits, we build
+ * a table of 1024 entries = 4 bytes * 1024 = 4096 bytes.
+ */
+static int ptable_alloc(void)
+{
+	u32 *pt;
+	int i;
+
+	memset(meye.mchip_ptable, 0, sizeof(meye.mchip_ptable));
+
+	/* give only 32 bit DMA addresses */
+	if (dma_set_mask(&meye.mchip_dev->dev, DMA_BIT_MASK(32)))
+		return -1;
+
+	meye.mchip_ptable_toc = dma_alloc_coherent(&meye.mchip_dev->dev,
+						   PAGE_SIZE,
+						   &meye.mchip_dmahandle,
+						   GFP_KERNEL);
+	if (!meye.mchip_ptable_toc) {
+		meye.mchip_dmahandle = 0;
+		return -1;
+	}
+
+	pt = meye.mchip_ptable_toc;
+	for (i = 0; i < MCHIP_NB_PAGES; i++) {
+		dma_addr_t dma;
+		meye.mchip_ptable[i] = dma_alloc_coherent(&meye.mchip_dev->dev,
+							  PAGE_SIZE,
+							  &dma,
+							  GFP_KERNEL);
+		if (!meye.mchip_ptable[i]) {
+			int j;
+			pt = meye.mchip_ptable_toc;
+			for (j = 0; j < i; ++j) {
+				dma = (dma_addr_t) *pt;
+				dma_free_coherent(&meye.mchip_dev->dev,
+						  PAGE_SIZE,
+						  meye.mchip_ptable[j], dma);
+				pt++;
+			}
+			dma_free_coherent(&meye.mchip_dev->dev,
+					  PAGE_SIZE,
+					  meye.mchip_ptable_toc,
+					  meye.mchip_dmahandle);
+			meye.mchip_ptable_toc = NULL;
+			meye.mchip_dmahandle = 0;
+			return -1;
+		}
+		*pt = (u32) dma;
+		pt++;
+	}
+	return 0;
+}
+
+static void ptable_free(void)
+{
+	u32 *pt;
+	int i;
+
+	pt = meye.mchip_ptable_toc;
+	for (i = 0; i < MCHIP_NB_PAGES; i++) {
+		dma_addr_t dma = (dma_addr_t) *pt;
+		if (meye.mchip_ptable[i])
+			dma_free_coherent(&meye.mchip_dev->dev,
+					  PAGE_SIZE,
+					  meye.mchip_ptable[i], dma);
+		pt++;
+	}
+
+	if (meye.mchip_ptable_toc)
+		dma_free_coherent(&meye.mchip_dev->dev,
+				  PAGE_SIZE,
+				  meye.mchip_ptable_toc,
+				  meye.mchip_dmahandle);
+
+	memset(meye.mchip_ptable, 0, sizeof(meye.mchip_ptable));
+	meye.mchip_ptable_toc = NULL;
+	meye.mchip_dmahandle = 0;
+}
+
+/* copy data from ptable into buf */
+static void ptable_copy(u8 *buf, int start, int size, int pt_pages)
+{
+	int i;
+
+	for (i = 0; i < (size / PAGE_SIZE) * PAGE_SIZE; i += PAGE_SIZE) {
+		memcpy(buf + i, meye.mchip_ptable[start++], PAGE_SIZE);
+		if (start >= pt_pages)
+			start = 0;
+	}
+	memcpy(buf + i, meye.mchip_ptable[start], size % PAGE_SIZE);
+}
+
+/****************************************************************************/
+/* JPEG tables at different qualities to load into the VRJ chip             */
+/****************************************************************************/
+
+/* return a set of quantisation tables based on a quality from 1 to 10 */
+static u16 *jpeg_quantisation_tables(int *length, int quality)
+{
+	static u16 jpeg_tables[][70] = { {
+		0xdbff, 0x4300, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff,
+		0xdbff, 0x4300, 0xff01, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff,
+	},
+	{
+		0xdbff, 0x4300, 0x5000, 0x3c37, 0x3c46, 0x5032, 0x4146, 0x5a46,
+		0x5055, 0x785f, 0x82c8, 0x6e78, 0x786e, 0xaff5, 0x91b9, 0xffc8,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff,
+		0xdbff, 0x4300, 0x5501, 0x5a5a, 0x6978, 0xeb78, 0x8282, 0xffeb,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
+		0xffff, 0xffff, 0xffff,
+	},
+	{
+		0xdbff, 0x4300, 0x2800, 0x1e1c, 0x1e23, 0x2819, 0x2123, 0x2d23,
+		0x282b, 0x3c30, 0x4164, 0x373c, 0x3c37, 0x587b, 0x495d, 0x9164,
+		0x9980, 0x8f96, 0x8c80, 0xa08a, 0xe6b4, 0xa0c3, 0xdaaa, 0x8aad,
+		0xc88c, 0xcbff, 0xeeda, 0xfff5, 0xffff, 0xc19b, 0xffff, 0xfaff,
+		0xe6ff, 0xfffd, 0xfff8,
+		0xdbff, 0x4300, 0x2b01, 0x2d2d, 0x353c, 0x763c, 0x4141, 0xf876,
+		0x8ca5, 0xf8a5, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8,
+		0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8,
+		0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8, 0xf8f8,
+		0xf8f8, 0xf8f8, 0xfff8,
+	},
+	{
+		0xdbff, 0x4300, 0x1b00, 0x1412, 0x1417, 0x1b11, 0x1617, 0x1e17,
+		0x1b1c, 0x2820, 0x2b42, 0x2528, 0x2825, 0x3a51, 0x303d, 0x6042,
+		0x6555, 0x5f64, 0x5d55, 0x6a5b, 0x9978, 0x6a81, 0x9071, 0x5b73,
+		0x855d, 0x86b5, 0x9e90, 0xaba3, 0xabad, 0x8067, 0xc9bc, 0xa6ba,
+		0x99c7, 0xaba8, 0xffa4,
+		0xdbff, 0x4300, 0x1c01, 0x1e1e, 0x2328, 0x4e28, 0x2b2b, 0xa44e,
+		0x5d6e, 0xa46e, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4,
+		0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4,
+		0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4, 0xa4a4,
+		0xa4a4, 0xa4a4, 0xffa4,
+	},
+	{
+		0xdbff, 0x4300, 0x1400, 0x0f0e, 0x0f12, 0x140d, 0x1012, 0x1712,
+		0x1415, 0x1e18, 0x2132, 0x1c1e, 0x1e1c, 0x2c3d, 0x242e, 0x4932,
+		0x4c40, 0x474b, 0x4640, 0x5045, 0x735a, 0x5062, 0x6d55, 0x4556,
+		0x6446, 0x6588, 0x776d, 0x817b, 0x8182, 0x604e, 0x978d, 0x7d8c,
+		0x7396, 0x817e, 0xff7c,
+		0xdbff, 0x4300, 0x1501, 0x1717, 0x1a1e, 0x3b1e, 0x2121, 0x7c3b,
+		0x4653, 0x7c53, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c,
+		0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c,
+		0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c, 0x7c7c,
+		0x7c7c, 0x7c7c, 0xff7c,
+	},
+	{
+		0xdbff, 0x4300, 0x1000, 0x0c0b, 0x0c0e, 0x100a, 0x0d0e, 0x120e,
+		0x1011, 0x1813, 0x1a28, 0x1618, 0x1816, 0x2331, 0x1d25, 0x3a28,
+		0x3d33, 0x393c, 0x3833, 0x4037, 0x5c48, 0x404e, 0x5744, 0x3745,
+		0x5038, 0x516d, 0x5f57, 0x6762, 0x6768, 0x4d3e, 0x7971, 0x6470,
+		0x5c78, 0x6765, 0xff63,
+		0xdbff, 0x4300, 0x1101, 0x1212, 0x1518, 0x2f18, 0x1a1a, 0x632f,
+		0x3842, 0x6342, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363,
+		0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363,
+		0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363, 0x6363,
+		0x6363, 0x6363, 0xff63,
+	},
+	{
+		0xdbff, 0x4300, 0x0d00, 0x0a09, 0x0a0b, 0x0d08, 0x0a0b, 0x0e0b,
+		0x0d0e, 0x130f, 0x1520, 0x1213, 0x1312, 0x1c27, 0x171e, 0x2e20,
+		0x3129, 0x2e30, 0x2d29, 0x332c, 0x4a3a, 0x333e, 0x4636, 0x2c37,
+		0x402d, 0x4157, 0x4c46, 0x524e, 0x5253, 0x3e32, 0x615a, 0x505a,
+		0x4a60, 0x5251, 0xff4f,
+		0xdbff, 0x4300, 0x0e01, 0x0e0e, 0x1113, 0x2613, 0x1515, 0x4f26,
+		0x2d35, 0x4f35, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f,
+		0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f,
+		0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f, 0x4f4f,
+		0x4f4f, 0x4f4f, 0xff4f,
+	},
+	{
+		0xdbff, 0x4300, 0x0a00, 0x0707, 0x0708, 0x0a06, 0x0808, 0x0b08,
+		0x0a0a, 0x0e0b, 0x1018, 0x0d0e, 0x0e0d, 0x151d, 0x1116, 0x2318,
+		0x251f, 0x2224, 0x221f, 0x2621, 0x372b, 0x262f, 0x3429, 0x2129,
+		0x3022, 0x3141, 0x3934, 0x3e3b, 0x3e3e, 0x2e25, 0x4944, 0x3c43,
+		0x3748, 0x3e3d, 0xff3b,
+		0xdbff, 0x4300, 0x0a01, 0x0b0b, 0x0d0e, 0x1c0e, 0x1010, 0x3b1c,
+		0x2228, 0x3b28, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b,
+		0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b,
+		0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b, 0x3b3b,
+		0x3b3b, 0x3b3b, 0xff3b,
+	},
+	{
+		0xdbff, 0x4300, 0x0600, 0x0504, 0x0506, 0x0604, 0x0506, 0x0706,
+		0x0607, 0x0a08, 0x0a10, 0x090a, 0x0a09, 0x0e14, 0x0c0f, 0x1710,
+		0x1814, 0x1718, 0x1614, 0x1a16, 0x251d, 0x1a1f, 0x231b, 0x161c,
+		0x2016, 0x202c, 0x2623, 0x2927, 0x292a, 0x1f19, 0x302d, 0x282d,
+		0x2530, 0x2928, 0xff28,
+		0xdbff, 0x4300, 0x0701, 0x0707, 0x080a, 0x130a, 0x0a0a, 0x2813,
+		0x161a, 0x281a, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828,
+		0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828,
+		0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828, 0x2828,
+		0x2828, 0x2828, 0xff28,
+	},
+	{
+		0xdbff, 0x4300, 0x0300, 0x0202, 0x0203, 0x0302, 0x0303, 0x0403,
+		0x0303, 0x0504, 0x0508, 0x0405, 0x0504, 0x070a, 0x0607, 0x0c08,
+		0x0c0a, 0x0b0c, 0x0b0a, 0x0d0b, 0x120e, 0x0d10, 0x110e, 0x0b0e,
+		0x100b, 0x1016, 0x1311, 0x1514, 0x1515, 0x0f0c, 0x1817, 0x1416,
+		0x1218, 0x1514, 0xff14,
+		0xdbff, 0x4300, 0x0301, 0x0404, 0x0405, 0x0905, 0x0505, 0x1409,
+		0x0b0d, 0x140d, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414,
+		0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414,
+		0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414, 0x1414,
+		0x1414, 0x1414, 0xff14,
+	},
+	{
+		0xdbff, 0x4300, 0x0100, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0xff01,
+		0xdbff, 0x4300, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0101, 0x0101, 0xff01,
+	} };
+
+	if (quality < 0 || quality > 10) {
+		printk(KERN_WARNING
+		       "meye: invalid quality level %d - using 8\n", quality);
+		quality = 8;
+	}
+
+	*length = ARRAY_SIZE(jpeg_tables[quality]);
+	return jpeg_tables[quality];
+}
+
+/* return a generic set of huffman tables */
+static u16 *jpeg_huffman_tables(int *length)
+{
+	static u16 tables[] = {
+		0xC4FF, 0xB500, 0x0010, 0x0102, 0x0303, 0x0402, 0x0503, 0x0405,
+		0x0004, 0x0100, 0x017D, 0x0302, 0x0400, 0x0511, 0x2112, 0x4131,
+		0x1306, 0x6151, 0x2207, 0x1471, 0x8132, 0xA191, 0x2308, 0xB142,
+		0x15C1, 0xD152, 0x24F0, 0x6233, 0x8272, 0x0A09, 0x1716, 0x1918,
+		0x251A, 0x2726, 0x2928, 0x342A, 0x3635, 0x3837, 0x3A39, 0x4443,
+		0x4645, 0x4847, 0x4A49, 0x5453, 0x5655, 0x5857, 0x5A59, 0x6463,
+		0x6665, 0x6867, 0x6A69, 0x7473, 0x7675, 0x7877, 0x7A79, 0x8483,
+		0x8685, 0x8887, 0x8A89, 0x9392, 0x9594, 0x9796, 0x9998, 0xA29A,
+		0xA4A3, 0xA6A5, 0xA8A7, 0xAAA9, 0xB3B2, 0xB5B4, 0xB7B6, 0xB9B8,
+		0xC2BA, 0xC4C3, 0xC6C5, 0xC8C7, 0xCAC9, 0xD3D2, 0xD5D4, 0xD7D6,
+		0xD9D8, 0xE1DA, 0xE3E2, 0xE5E4, 0xE7E6, 0xE9E8, 0xF1EA, 0xF3F2,
+		0xF5F4, 0xF7F6, 0xF9F8, 0xFFFA,
+		0xC4FF, 0xB500, 0x0011, 0x0102, 0x0402, 0x0304, 0x0704, 0x0405,
+		0x0004, 0x0201, 0x0077, 0x0201, 0x1103, 0x0504, 0x3121, 0x1206,
+		0x5141, 0x6107, 0x1371, 0x3222, 0x0881, 0x4214, 0xA191, 0xC1B1,
+		0x2309, 0x5233, 0x15F0, 0x7262, 0x0AD1, 0x2416, 0xE134, 0xF125,
+		0x1817, 0x1A19, 0x2726, 0x2928, 0x352A, 0x3736, 0x3938, 0x433A,
+		0x4544, 0x4746, 0x4948, 0x534A, 0x5554, 0x5756, 0x5958, 0x635A,
+		0x6564, 0x6766, 0x6968, 0x736A, 0x7574, 0x7776, 0x7978, 0x827A,
+		0x8483, 0x8685, 0x8887, 0x8A89, 0x9392, 0x9594, 0x9796, 0x9998,
+		0xA29A, 0xA4A3, 0xA6A5, 0xA8A7, 0xAAA9, 0xB3B2, 0xB5B4, 0xB7B6,
+		0xB9B8, 0xC2BA, 0xC4C3, 0xC6C5, 0xC8C7, 0xCAC9, 0xD3D2, 0xD5D4,
+		0xD7D6, 0xD9D8, 0xE2DA, 0xE4E3, 0xE6E5, 0xE8E7, 0xEAE9, 0xF3F2,
+		0xF5F4, 0xF7F6, 0xF9F8, 0xFFFA,
+		0xC4FF, 0x1F00, 0x0000, 0x0501, 0x0101, 0x0101, 0x0101, 0x0000,
+		0x0000, 0x0000, 0x0000, 0x0201, 0x0403, 0x0605, 0x0807, 0x0A09,
+		0xFF0B,
+		0xC4FF, 0x1F00, 0x0001, 0x0103, 0x0101, 0x0101, 0x0101, 0x0101,
+		0x0000, 0x0000, 0x0000, 0x0201, 0x0403, 0x0605, 0x0807, 0x0A09,
+		0xFF0B
+	};
+
+	*length = ARRAY_SIZE(tables);
+	return tables;
+}
+
+/****************************************************************************/
+/* MCHIP low-level functions                                                */
+/****************************************************************************/
+
+/* returns the horizontal capture size */
+static inline int mchip_hsize(void)
+{
+	return meye.params.subsample ? 320 : 640;
+}
+
+/* returns the vertical capture size */
+static inline int mchip_vsize(void)
+{
+	return meye.params.subsample ? 240 : 480;
+}
+
+/* waits for a register to be available */
+static void mchip_sync(int reg)
+{
+	u32 status;
+	int i;
+
+	if (reg == MCHIP_MM_FIFO_DATA) {
+		for (i = 0; i < MCHIP_REG_TIMEOUT; i++) {
+			status = readl(meye.mchip_mmregs +
+				       MCHIP_MM_FIFO_STATUS);
+			if (!(status & MCHIP_MM_FIFO_WAIT)) {
+				printk(KERN_WARNING "meye: fifo not ready\n");
+				return;
+			}
+			if (status & MCHIP_MM_FIFO_READY)
+				return;
+			udelay(1);
+		}
+	} else if (reg > 0x80) {
+		u32 mask = (reg < 0x100) ? MCHIP_HIC_STATUS_MCC_RDY
+					 : MCHIP_HIC_STATUS_VRJ_RDY;
+		for (i = 0; i < MCHIP_REG_TIMEOUT; i++) {
+			status = readl(meye.mchip_mmregs + MCHIP_HIC_STATUS);
+			if (status & mask)
+				return;
+			udelay(1);
+		}
+	} else
+		return;
+	printk(KERN_WARNING
+	       "meye: mchip_sync() timeout on reg 0x%x status=0x%x\n",
+	       reg, status);
+}
+
+/* sets a value into the register */
+static inline void mchip_set(int reg, u32 v)
+{
+	mchip_sync(reg);
+	writel(v, meye.mchip_mmregs + reg);
+}
+
+/* get the register value */
+static inline u32 mchip_read(int reg)
+{
+	mchip_sync(reg);
+	return readl(meye.mchip_mmregs + reg);
+}
+
+/* wait for a register to become a particular value */
+static inline int mchip_delay(u32 reg, u32 v)
+{
+	int n = 10;
+	while (--n && mchip_read(reg) != v)
+		udelay(1);
+	return n;
+}
+
+/* setup subsampling */
+static void mchip_subsample(void)
+{
+	mchip_set(MCHIP_MCC_R_SAMPLING, meye.params.subsample);
+	mchip_set(MCHIP_MCC_R_XRANGE, mchip_hsize());
+	mchip_set(MCHIP_MCC_R_YRANGE, mchip_vsize());
+	mchip_set(MCHIP_MCC_B_XRANGE, mchip_hsize());
+	mchip_set(MCHIP_MCC_B_YRANGE, mchip_vsize());
+	mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE);
+}
+
+/* set the framerate into the mchip */
+static void mchip_set_framerate(void)
+{
+	mchip_set(MCHIP_HIC_S_RATE, meye.params.framerate);
+}
+
+/* load some huffman and quantisation tables into the VRJ chip ready
+   for JPEG compression */
+static void mchip_load_tables(void)
+{
+	int i;
+	int length;
+	u16 *tables;
+
+	tables = jpeg_huffman_tables(&length);
+	for (i = 0; i < length; i++)
+		writel(tables[i], meye.mchip_mmregs + MCHIP_VRJ_TABLE_DATA);
+
+	tables = jpeg_quantisation_tables(&length, meye.params.quality);
+	for (i = 0; i < length; i++)
+		writel(tables[i], meye.mchip_mmregs + MCHIP_VRJ_TABLE_DATA);
+}
+
+/* setup the VRJ parameters in the chip */
+static void mchip_vrj_setup(u8 mode)
+{
+	mchip_set(MCHIP_VRJ_BUS_MODE, 5);
+	mchip_set(MCHIP_VRJ_SIGNAL_ACTIVE_LEVEL, 0x1f);
+	mchip_set(MCHIP_VRJ_PDAT_USE, 1);
+	mchip_set(MCHIP_VRJ_IRQ_FLAG, 0xa0);
+	mchip_set(MCHIP_VRJ_MODE_SPECIFY, mode);
+	mchip_set(MCHIP_VRJ_NUM_LINES, mchip_vsize());
+	mchip_set(MCHIP_VRJ_NUM_PIXELS, mchip_hsize());
+	mchip_set(MCHIP_VRJ_NUM_COMPONENTS, 0x1b);
+	mchip_set(MCHIP_VRJ_LIMIT_COMPRESSED_LO, 0xFFFF);
+	mchip_set(MCHIP_VRJ_LIMIT_COMPRESSED_HI, 0xFFFF);
+	mchip_set(MCHIP_VRJ_COMP_DATA_FORMAT, 0xC);
+	mchip_set(MCHIP_VRJ_RESTART_INTERVAL, 0);
+	mchip_set(MCHIP_VRJ_SOF1, 0x601);
+	mchip_set(MCHIP_VRJ_SOF2, 0x1502);
+	mchip_set(MCHIP_VRJ_SOF3, 0x1503);
+	mchip_set(MCHIP_VRJ_SOF4, 0x1596);
+	mchip_set(MCHIP_VRJ_SOS, 0x0ed0);
+
+	mchip_load_tables();
+}
+
+/* sets the DMA parameters into the chip */
+static void mchip_dma_setup(dma_addr_t dma_addr)
+{
+	int i;
+
+	mchip_set(MCHIP_MM_PT_ADDR, (u32)dma_addr);
+	for (i = 0; i < 4; i++)
+		mchip_set(MCHIP_MM_FIR(i), 0);
+	meye.mchip_fnum = 0;
+}
+
+/* setup for DMA transfers - also zeros the framebuffer */
+static int mchip_dma_alloc(void)
+{
+	if (!meye.mchip_dmahandle)
+		if (ptable_alloc())
+			return -1;
+	return 0;
+}
+
+/* frees the DMA buffer */
+static void mchip_dma_free(void)
+{
+	if (meye.mchip_dmahandle) {
+		mchip_dma_setup(0);
+		ptable_free();
+	}
+}
+
+/* stop any existing HIC action and wait for any dma to complete then
+   reset the dma engine */
+static void mchip_hic_stop(void)
+{
+	int i, j;
+
+	meye.mchip_mode = MCHIP_HIC_MODE_NOOP;
+	if (!(mchip_read(MCHIP_HIC_STATUS) & MCHIP_HIC_STATUS_BUSY))
+		return;
+	for (i = 0; i < 20; ++i) {
+		mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_STOP);
+		mchip_delay(MCHIP_HIC_CMD, 0);
+		for (j = 0; j < 100; ++j) {
+			if (mchip_delay(MCHIP_HIC_STATUS,
+					MCHIP_HIC_STATUS_IDLE))
+				return;
+			msleep(1);
+		}
+		printk(KERN_ERR "meye: need to reset HIC!\n");
+
+		mchip_set(MCHIP_HIC_CTL, MCHIP_HIC_CTL_SOFT_RESET);
+		msleep(250);
+	}
+	printk(KERN_ERR "meye: resetting HIC hanged!\n");
+}
+
+/****************************************************************************/
+/* MCHIP frame processing functions                                         */
+/****************************************************************************/
+
+/* get the next ready frame from the dma engine */
+static u32 mchip_get_frame(void)
+{
+	return mchip_read(MCHIP_MM_FIR(meye.mchip_fnum));
+}
+
+/* frees the current frame from the dma engine */
+static void mchip_free_frame(void)
+{
+	mchip_set(MCHIP_MM_FIR(meye.mchip_fnum), 0);
+	meye.mchip_fnum++;
+	meye.mchip_fnum %= 4;
+}
+
+/* read one frame from the framebuffer assuming it was captured using
+   a uncompressed transfer */
+static void mchip_cont_read_frame(u32 v, u8 *buf, int size)
+{
+	int pt_id;
+
+	pt_id = (v >> 17) & 0x3FF;
+
+	ptable_copy(buf, pt_id, size, MCHIP_NB_PAGES);
+}
+
+/* read a compressed frame from the framebuffer */
+static int mchip_comp_read_frame(u32 v, u8 *buf, int size)
+{
+	int pt_start, pt_end, trailer;
+	int fsize;
+	int i;
+
+	pt_start = (v >> 19) & 0xFF;
+	pt_end = (v >> 11) & 0xFF;
+	trailer = (v >> 1) & 0x3FF;
+
+	if (pt_end < pt_start)
+		fsize = (MCHIP_NB_PAGES_MJPEG - pt_start) * PAGE_SIZE +
+			pt_end * PAGE_SIZE + trailer * 4;
+	else
+		fsize = (pt_end - pt_start) * PAGE_SIZE + trailer * 4;
+
+	if (fsize > size) {
+		printk(KERN_WARNING "meye: oversized compressed frame %d\n",
+		       fsize);
+		return -1;
+	}
+
+	ptable_copy(buf, pt_start, fsize, MCHIP_NB_PAGES_MJPEG);
+
+#ifdef MEYE_JPEG_CORRECTION
+
+	/* Some mchip generated jpeg frames are incorrect. In most
+	 * (all ?) of those cases, the final EOI (0xff 0xd9) marker
+	 * is not present at the end of the frame.
+	 *
+	 * Since adding the final marker is not enough to restore
+	 * the jpeg integrity, we drop the frame.
+	 */
+
+	for (i = fsize - 1; i > 0 && buf[i] == 0xff; i--) ;
+
+	if (i < 2 || buf[i - 1] != 0xff || buf[i] != 0xd9)
+		return -1;
+
+#endif
+
+	return fsize;
+}
+
+/* take a picture into SDRAM */
+static void mchip_take_picture(void)
+{
+	int i;
+
+	mchip_hic_stop();
+	mchip_subsample();
+	mchip_dma_setup(meye.mchip_dmahandle);
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_STILL_CAP);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+
+	for (i = 0; i < 100; ++i) {
+		if (mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE))
+			break;
+		msleep(1);
+	}
+}
+
+/* dma a previously taken picture into a buffer */
+static void mchip_get_picture(u8 *buf, int bufsize)
+{
+	u32 v;
+	int i;
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_STILL_OUT);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+	for (i = 0; i < 100; ++i) {
+		if (mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE))
+			break;
+		msleep(1);
+	}
+	for (i = 0; i < 4; ++i) {
+		v = mchip_get_frame();
+		if (v & MCHIP_MM_FIR_RDY) {
+			mchip_cont_read_frame(v, buf, bufsize);
+			break;
+		}
+		mchip_free_frame();
+	}
+}
+
+/* start continuous dma capture */
+static void mchip_continuous_start(void)
+{
+	mchip_hic_stop();
+	mchip_subsample();
+	mchip_set_framerate();
+	mchip_dma_setup(meye.mchip_dmahandle);
+
+	meye.mchip_mode = MCHIP_HIC_MODE_CONT_OUT;
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_CONT_OUT);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+}
+
+/* compress one frame into a buffer */
+static int mchip_compress_frame(u8 *buf, int bufsize)
+{
+	u32 v;
+	int len = -1, i;
+
+	mchip_vrj_setup(0x3f);
+	udelay(50);
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_STILL_COMP);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+	for (i = 0; i < 100; ++i) {
+		if (mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE))
+			break;
+		msleep(1);
+	}
+
+	for (i = 0; i < 4; ++i) {
+		v = mchip_get_frame();
+		if (v & MCHIP_MM_FIR_RDY) {
+			len = mchip_comp_read_frame(v, buf, bufsize);
+			break;
+		}
+		mchip_free_frame();
+	}
+	return len;
+}
+
+#if 0
+/* uncompress one image into a buffer */
+static int mchip_uncompress_frame(u8 *img, int imgsize, u8 *buf, int bufsize)
+{
+	mchip_vrj_setup(0x3f);
+	udelay(50);
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_STILL_DECOMP);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+
+	return mchip_comp_read_frame(buf, bufsize);
+}
+#endif
+
+/* start continuous compressed capture */
+static void mchip_cont_compression_start(void)
+{
+	mchip_hic_stop();
+	mchip_vrj_setup(0x3f);
+	mchip_subsample();
+	mchip_set_framerate();
+	mchip_dma_setup(meye.mchip_dmahandle);
+
+	meye.mchip_mode = MCHIP_HIC_MODE_CONT_COMP;
+
+	mchip_set(MCHIP_HIC_MODE, MCHIP_HIC_MODE_CONT_COMP);
+	mchip_set(MCHIP_HIC_CMD, MCHIP_HIC_CMD_START);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+}
+
+/****************************************************************************/
+/* Interrupt handling                                                       */
+/****************************************************************************/
+
+static irqreturn_t meye_irq(int irq, void *dev_id)
+{
+	u32 v;
+	int reqnr;
+	static int sequence;
+
+	v = mchip_read(MCHIP_MM_INTA);
+
+	if (meye.mchip_mode != MCHIP_HIC_MODE_CONT_OUT &&
+	    meye.mchip_mode != MCHIP_HIC_MODE_CONT_COMP)
+		return IRQ_NONE;
+
+again:
+	v = mchip_get_frame();
+	if (!(v & MCHIP_MM_FIR_RDY))
+		return IRQ_HANDLED;
+
+	if (meye.mchip_mode == MCHIP_HIC_MODE_CONT_OUT) {
+		if (kfifo_out_locked(&meye.grabq, (unsigned char *)&reqnr,
+			      sizeof(int), &meye.grabq_lock) != sizeof(int)) {
+			mchip_free_frame();
+			return IRQ_HANDLED;
+		}
+		mchip_cont_read_frame(v, meye.grab_fbuffer + gbufsize * reqnr,
+				      mchip_hsize() * mchip_vsize() * 2);
+		meye.grab_buffer[reqnr].size = mchip_hsize() * mchip_vsize() * 2;
+		meye.grab_buffer[reqnr].state = MEYE_BUF_DONE;
+		meye.grab_buffer[reqnr].ts = ktime_get_ns();
+		meye.grab_buffer[reqnr].sequence = sequence++;
+		kfifo_in_locked(&meye.doneq, (unsigned char *)&reqnr,
+				sizeof(int), &meye.doneq_lock);
+		wake_up_interruptible(&meye.proc_list);
+	} else {
+		int size;
+		size = mchip_comp_read_frame(v, meye.grab_temp, gbufsize);
+		if (size == -1) {
+			mchip_free_frame();
+			goto again;
+		}
+		if (kfifo_out_locked(&meye.grabq, (unsigned char *)&reqnr,
+			      sizeof(int), &meye.grabq_lock) != sizeof(int)) {
+			mchip_free_frame();
+			goto again;
+		}
+		memcpy(meye.grab_fbuffer + gbufsize * reqnr, meye.grab_temp,
+		       size);
+		meye.grab_buffer[reqnr].size = size;
+		meye.grab_buffer[reqnr].state = MEYE_BUF_DONE;
+		meye.grab_buffer[reqnr].ts = ktime_get_ns();
+		meye.grab_buffer[reqnr].sequence = sequence++;
+		kfifo_in_locked(&meye.doneq, (unsigned char *)&reqnr,
+				sizeof(int), &meye.doneq_lock);
+		wake_up_interruptible(&meye.proc_list);
+	}
+	mchip_free_frame();
+	goto again;
+}
+
+/****************************************************************************/
+/* video4linux integration                                                  */
+/****************************************************************************/
+
+static int meye_open(struct file *file)
+{
+	int i;
+
+	if (test_and_set_bit(0, &meye.in_use))
+		return -EBUSY;
+
+	mchip_hic_stop();
+
+	if (mchip_dma_alloc()) {
+		printk(KERN_ERR "meye: mchip framebuffer allocation failed\n");
+		clear_bit(0, &meye.in_use);
+		return -ENOBUFS;
+	}
+
+	for (i = 0; i < MEYE_MAX_BUFNBRS; i++)
+		meye.grab_buffer[i].state = MEYE_BUF_UNUSED;
+	kfifo_reset(&meye.grabq);
+	kfifo_reset(&meye.doneq);
+	return v4l2_fh_open(file);
+}
+
+static int meye_release(struct file *file)
+{
+	mchip_hic_stop();
+	mchip_dma_free();
+	clear_bit(0, &meye.in_use);
+	return v4l2_fh_release(file);
+}
+
+static int meyeioc_g_params(struct meye_params *p)
+{
+	*p = meye.params;
+	return 0;
+}
+
+static int meyeioc_s_params(struct meye_params *jp)
+{
+	if (jp->subsample > 1)
+		return -EINVAL;
+
+	if (jp->quality > 10)
+		return -EINVAL;
+
+	if (jp->sharpness > 63 || jp->agc > 63 || jp->picture > 63)
+		return -EINVAL;
+
+	if (jp->framerate > 31)
+		return -EINVAL;
+
+	mutex_lock(&meye.lock);
+
+	if (meye.params.subsample != jp->subsample ||
+	    meye.params.quality != jp->quality)
+		mchip_hic_stop();	/* need restart */
+
+	meye.params = *jp;
+	sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERASHARPNESS,
+			      meye.params.sharpness);
+	sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERAAGC,
+			      meye.params.agc);
+	sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERAPICTURE,
+			      meye.params.picture);
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int meyeioc_qbuf_capt(int *nb)
+{
+	if (!meye.grab_fbuffer)
+		return -EINVAL;
+
+	if (*nb >= gbuffers)
+		return -EINVAL;
+
+	if (*nb < 0) {
+		/* stop capture */
+		mchip_hic_stop();
+		return 0;
+	}
+
+	if (meye.grab_buffer[*nb].state != MEYE_BUF_UNUSED)
+		return -EBUSY;
+
+	mutex_lock(&meye.lock);
+
+	if (meye.mchip_mode != MCHIP_HIC_MODE_CONT_COMP)
+		mchip_cont_compression_start();
+
+	meye.grab_buffer[*nb].state = MEYE_BUF_USING;
+	kfifo_in_locked(&meye.grabq, (unsigned char *)nb, sizeof(int),
+			 &meye.grabq_lock);
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int meyeioc_sync(struct file *file, void *fh, int *i)
+{
+	int unused;
+
+	if (*i < 0 || *i >= gbuffers)
+		return -EINVAL;
+
+	mutex_lock(&meye.lock);
+	switch (meye.grab_buffer[*i].state) {
+
+	case MEYE_BUF_UNUSED:
+		mutex_unlock(&meye.lock);
+		return -EINVAL;
+	case MEYE_BUF_USING:
+		if (file->f_flags & O_NONBLOCK) {
+			mutex_unlock(&meye.lock);
+			return -EAGAIN;
+		}
+		if (wait_event_interruptible(meye.proc_list,
+			(meye.grab_buffer[*i].state != MEYE_BUF_USING))) {
+			mutex_unlock(&meye.lock);
+			return -EINTR;
+		}
+		fallthrough;
+	case MEYE_BUF_DONE:
+		meye.grab_buffer[*i].state = MEYE_BUF_UNUSED;
+		if (kfifo_out_locked(&meye.doneq, (unsigned char *)&unused,
+				sizeof(int), &meye.doneq_lock) != sizeof(int))
+					break;
+	}
+	*i = meye.grab_buffer[*i].size;
+	mutex_unlock(&meye.lock);
+	return 0;
+}
+
+static int meyeioc_stillcapt(void)
+{
+	if (!meye.grab_fbuffer)
+		return -EINVAL;
+
+	if (meye.grab_buffer[0].state != MEYE_BUF_UNUSED)
+		return -EBUSY;
+
+	mutex_lock(&meye.lock);
+	meye.grab_buffer[0].state = MEYE_BUF_USING;
+	mchip_take_picture();
+
+	mchip_get_picture(meye.grab_fbuffer,
+			mchip_hsize() * mchip_vsize() * 2);
+
+	meye.grab_buffer[0].state = MEYE_BUF_DONE;
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int meyeioc_stilljcapt(int *len)
+{
+	if (!meye.grab_fbuffer)
+		return -EINVAL;
+
+	if (meye.grab_buffer[0].state != MEYE_BUF_UNUSED)
+		return -EBUSY;
+
+	mutex_lock(&meye.lock);
+	meye.grab_buffer[0].state = MEYE_BUF_USING;
+	*len = -1;
+
+	while (*len == -1) {
+		mchip_take_picture();
+		*len = mchip_compress_frame(meye.grab_fbuffer, gbufsize);
+	}
+
+	meye.grab_buffer[0].state = MEYE_BUF_DONE;
+	mutex_unlock(&meye.lock);
+	return 0;
+}
+
+static int vidioc_querycap(struct file *file, void *fh,
+				struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, "meye", sizeof(cap->driver));
+	strscpy(cap->card, "meye", sizeof(cap->card));
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	if (i->index != 0)
+		return -EINVAL;
+
+	strscpy(i->name, "Camera", sizeof(i->name));
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
+{
+	if (i != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int meye_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	mutex_lock(&meye.lock);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERABRIGHTNESS, ctrl->val);
+		meye.brightness = ctrl->val << 10;
+		break;
+	case V4L2_CID_HUE:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERAHUE, ctrl->val);
+		meye.hue = ctrl->val << 10;
+		break;
+	case V4L2_CID_CONTRAST:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERACONTRAST, ctrl->val);
+		meye.contrast = ctrl->val << 10;
+		break;
+	case V4L2_CID_SATURATION:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERACOLOR, ctrl->val);
+		meye.colour = ctrl->val << 10;
+		break;
+	case V4L2_CID_MEYE_AGC:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERAAGC, ctrl->val);
+		meye.params.agc = ctrl->val;
+		break;
+	case V4L2_CID_SHARPNESS:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERASHARPNESS, ctrl->val);
+		meye.params.sharpness = ctrl->val;
+		break;
+	case V4L2_CID_MEYE_PICTURE:
+		sony_pic_camera_command(
+			SONY_PIC_COMMAND_SETCAMERAPICTURE, ctrl->val);
+		meye.params.picture = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		meye.params.quality = ctrl->val;
+		break;
+	case V4L2_CID_MEYE_FRAMERATE:
+		meye.params.framerate = ctrl->val;
+		break;
+	default:
+		mutex_unlock(&meye.lock);
+		return -EINVAL;
+	}
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *fh,
+				struct v4l2_fmtdesc *f)
+{
+	if (f->index > 1)
+		return -EINVAL;
+
+	if (f->index == 0) {
+		/* standard YUV 422 capture */
+		f->flags = 0;
+		f->pixelformat = V4L2_PIX_FMT_YUYV;
+	} else {
+		/* compressed MJPEG capture */
+		f->pixelformat = V4L2_PIX_FMT_MJPEG;
+	}
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV &&
+	    f->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG)
+		return -EINVAL;
+
+	if (f->fmt.pix.field != V4L2_FIELD_ANY &&
+	    f->fmt.pix.field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	if (f->fmt.pix.width <= 320) {
+		f->fmt.pix.width = 320;
+		f->fmt.pix.height = 240;
+	} else {
+		f->fmt.pix.width = 640;
+		f->fmt.pix.height = 480;
+	}
+
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height *
+			       f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = 0;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	switch (meye.mchip_mode) {
+	case MCHIP_HIC_MODE_CONT_OUT:
+	default:
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	case MCHIP_HIC_MODE_CONT_COMP:
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+		break;
+	}
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.width = mchip_hsize();
+	f->fmt.pix.height = mchip_vsize();
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height *
+			       f->fmt.pix.bytesperline;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV &&
+	    f->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG)
+		return -EINVAL;
+
+	if (f->fmt.pix.field != V4L2_FIELD_ANY &&
+	    f->fmt.pix.field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	mutex_lock(&meye.lock);
+
+	if (f->fmt.pix.width <= 320) {
+		f->fmt.pix.width = 320;
+		f->fmt.pix.height = 240;
+		meye.params.subsample = 1;
+	} else {
+		f->fmt.pix.width = 640;
+		f->fmt.pix.height = 480;
+		meye.params.subsample = 0;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		meye.mchip_mode = MCHIP_HIC_MODE_CONT_OUT;
+		break;
+	case V4L2_PIX_FMT_MJPEG:
+		meye.mchip_mode = MCHIP_HIC_MODE_CONT_COMP;
+		break;
+	}
+
+	mutex_unlock(&meye.lock);
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height *
+			       f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = 0;
+
+	return 0;
+}
+
+static int vidioc_reqbufs(struct file *file, void *fh,
+				struct v4l2_requestbuffers *req)
+{
+	int i;
+
+	if (req->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	if (meye.grab_fbuffer && req->count == gbuffers) {
+		/* already allocated, no modifications */
+		return 0;
+	}
+
+	mutex_lock(&meye.lock);
+	if (meye.grab_fbuffer) {
+		for (i = 0; i < gbuffers; i++)
+			if (meye.vma_use_count[i]) {
+				mutex_unlock(&meye.lock);
+				return -EINVAL;
+			}
+		rvfree(meye.grab_fbuffer, gbuffers * gbufsize);
+		meye.grab_fbuffer = NULL;
+	}
+
+	gbuffers = max(2, min((int)req->count, MEYE_MAX_BUFNBRS));
+	req->count = gbuffers;
+	meye.grab_fbuffer = rvmalloc(gbuffers * gbufsize);
+
+	if (!meye.grab_fbuffer) {
+		printk(KERN_ERR "meye: v4l framebuffer allocation failed\n");
+		mutex_unlock(&meye.lock);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < gbuffers; i++)
+		meye.vma_use_count[i] = 0;
+
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int vidioc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	unsigned int index = buf->index;
+
+	if (index >= gbuffers)
+		return -EINVAL;
+
+	buf->bytesused = meye.grab_buffer[index].size;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	if (meye.grab_buffer[index].state == MEYE_BUF_USING)
+		buf->flags |= V4L2_BUF_FLAG_QUEUED;
+
+	if (meye.grab_buffer[index].state == MEYE_BUF_DONE)
+		buf->flags |= V4L2_BUF_FLAG_DONE;
+
+	buf->field = V4L2_FIELD_NONE;
+	v4l2_buffer_set_timestamp(buf, meye.grab_buffer[index].ts);
+	buf->sequence = meye.grab_buffer[index].sequence;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = index * gbufsize;
+	buf->length = gbufsize;
+
+	return 0;
+}
+
+static int vidioc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	if (buf->index >= gbuffers)
+		return -EINVAL;
+
+	if (meye.grab_buffer[buf->index].state != MEYE_BUF_UNUSED)
+		return -EINVAL;
+
+	mutex_lock(&meye.lock);
+	buf->flags |= V4L2_BUF_FLAG_QUEUED;
+	buf->flags &= ~V4L2_BUF_FLAG_DONE;
+	meye.grab_buffer[buf->index].state = MEYE_BUF_USING;
+	kfifo_in_locked(&meye.grabq, (unsigned char *)&buf->index,
+			sizeof(int), &meye.grabq_lock);
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int vidioc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	int reqnr;
+
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	mutex_lock(&meye.lock);
+
+	if (kfifo_len(&meye.doneq) == 0 && file->f_flags & O_NONBLOCK) {
+		mutex_unlock(&meye.lock);
+		return -EAGAIN;
+	}
+
+	if (wait_event_interruptible(meye.proc_list,
+				     kfifo_len(&meye.doneq) != 0) < 0) {
+		mutex_unlock(&meye.lock);
+		return -EINTR;
+	}
+
+	if (!kfifo_out_locked(&meye.doneq, (unsigned char *)&reqnr,
+		       sizeof(int), &meye.doneq_lock)) {
+		mutex_unlock(&meye.lock);
+		return -EBUSY;
+	}
+
+	if (meye.grab_buffer[reqnr].state != MEYE_BUF_DONE) {
+		mutex_unlock(&meye.lock);
+		return -EINVAL;
+	}
+
+	buf->index = reqnr;
+	buf->bytesused = meye.grab_buffer[reqnr].size;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	buf->field = V4L2_FIELD_NONE;
+	v4l2_buffer_set_timestamp(buf, meye.grab_buffer[reqnr].ts);
+	buf->sequence = meye.grab_buffer[reqnr].sequence;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = reqnr * gbufsize;
+	buf->length = gbufsize;
+	meye.grab_buffer[reqnr].state = MEYE_BUF_UNUSED;
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	mutex_lock(&meye.lock);
+
+	switch (meye.mchip_mode) {
+	case MCHIP_HIC_MODE_CONT_OUT:
+		mchip_continuous_start();
+		break;
+	case MCHIP_HIC_MODE_CONT_COMP:
+		mchip_cont_compression_start();
+		break;
+	default:
+		mutex_unlock(&meye.lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&meye.lock);
+
+	return 0;
+}
+
+static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	mutex_lock(&meye.lock);
+	mchip_hic_stop();
+	kfifo_reset(&meye.grabq);
+	kfifo_reset(&meye.doneq);
+
+	for (i = 0; i < MEYE_MAX_BUFNBRS; i++)
+		meye.grab_buffer[i].state = MEYE_BUF_UNUSED;
+
+	mutex_unlock(&meye.lock);
+	return 0;
+}
+
+static long vidioc_default(struct file *file, void *fh, bool valid_prio,
+			   unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case MEYEIOC_G_PARAMS:
+		return meyeioc_g_params((struct meye_params *) arg);
+
+	case MEYEIOC_S_PARAMS:
+		return meyeioc_s_params((struct meye_params *) arg);
+
+	case MEYEIOC_QBUF_CAPT:
+		return meyeioc_qbuf_capt((int *) arg);
+
+	case MEYEIOC_SYNC:
+		return meyeioc_sync(file, fh, (int *) arg);
+
+	case MEYEIOC_STILLCAPT:
+		return meyeioc_stillcapt();
+
+	case MEYEIOC_STILLJCAPT:
+		return meyeioc_stilljcapt((int *) arg);
+
+	default:
+		return -ENOTTY;
+	}
+
+}
+
+static __poll_t meye_poll(struct file *file, poll_table *wait)
+{
+	__poll_t res = v4l2_ctrl_poll(file, wait);
+
+	mutex_lock(&meye.lock);
+	poll_wait(file, &meye.proc_list, wait);
+	if (kfifo_len(&meye.doneq))
+		res |= EPOLLIN | EPOLLRDNORM;
+	mutex_unlock(&meye.lock);
+	return res;
+}
+
+static void meye_vm_open(struct vm_area_struct *vma)
+{
+	long idx = (long)vma->vm_private_data;
+	meye.vma_use_count[idx]++;
+}
+
+static void meye_vm_close(struct vm_area_struct *vma)
+{
+	long idx = (long)vma->vm_private_data;
+	meye.vma_use_count[idx]--;
+}
+
+static const struct vm_operations_struct meye_vm_ops = {
+	.open		= meye_vm_open,
+	.close		= meye_vm_close,
+};
+
+static int meye_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long start = vma->vm_start;
+	unsigned long size = vma->vm_end - vma->vm_start;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long page, pos;
+
+	mutex_lock(&meye.lock);
+	if (size > gbuffers * gbufsize || offset > gbuffers * gbufsize - size) {
+		mutex_unlock(&meye.lock);
+		return -EINVAL;
+	}
+	if (!meye.grab_fbuffer) {
+		int i;
+
+		/* lazy allocation */
+		meye.grab_fbuffer = rvmalloc(gbuffers*gbufsize);
+		if (!meye.grab_fbuffer) {
+			printk(KERN_ERR "meye: v4l framebuffer allocation failed\n");
+			mutex_unlock(&meye.lock);
+			return -ENOMEM;
+		}
+		for (i = 0; i < gbuffers; i++)
+			meye.vma_use_count[i] = 0;
+	}
+	pos = (unsigned long)meye.grab_fbuffer + offset;
+
+	while (size > 0) {
+		page = vmalloc_to_pfn((void *)pos);
+		if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED)) {
+			mutex_unlock(&meye.lock);
+			return -EAGAIN;
+		}
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+		if (size > PAGE_SIZE)
+			size -= PAGE_SIZE;
+		else
+			size = 0;
+	}
+
+	vma->vm_ops = &meye_vm_ops;
+	vma->vm_flags &= ~VM_IO;	/* not I/O memory */
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
+	vma->vm_private_data = (void *) (offset / gbufsize);
+	meye_vm_open(vma);
+
+	mutex_unlock(&meye.lock);
+	return 0;
+}
+
+static const struct v4l2_file_operations meye_fops = {
+	.owner		= THIS_MODULE,
+	.open		= meye_open,
+	.release	= meye_release,
+	.mmap		= meye_mmap,
+	.unlocked_ioctl	= video_ioctl2,
+	.poll		= meye_poll,
+};
+
+static const struct v4l2_ioctl_ops meye_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_enum_input	= vidioc_enum_input,
+	.vidioc_g_input		= vidioc_g_input,
+	.vidioc_s_input		= vidioc_s_input,
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
+	.vidioc_reqbufs		= vidioc_reqbufs,
+	.vidioc_querybuf	= vidioc_querybuf,
+	.vidioc_qbuf		= vidioc_qbuf,
+	.vidioc_dqbuf		= vidioc_dqbuf,
+	.vidioc_streamon	= vidioc_streamon,
+	.vidioc_streamoff	= vidioc_streamoff,
+	.vidioc_log_status	= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event	= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_default		= vidioc_default,
+};
+
+static const struct video_device meye_template = {
+	.name		= "meye",
+	.fops		= &meye_fops,
+	.ioctl_ops	= &meye_ioctl_ops,
+	.release	= video_device_release_empty,
+	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+};
+
+static const struct v4l2_ctrl_ops meye_ctrl_ops = {
+	.s_ctrl = meye_s_ctrl,
+};
+
+static int __maybe_unused meye_suspend(struct device *dev)
+{
+	meye.pm_mchip_mode = meye.mchip_mode;
+	mchip_hic_stop();
+	mchip_set(MCHIP_MM_INTA, 0x0);
+	return 0;
+}
+
+static int __maybe_unused meye_resume(struct device *dev)
+{
+	pci_write_config_word(meye.mchip_dev, MCHIP_PCI_SOFTRESET_SET, 1);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+	mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE);
+	msleep(1);
+	mchip_set(MCHIP_VRJ_SOFT_RESET, 1);
+	msleep(1);
+	mchip_set(MCHIP_MM_PCI_MODE, 5);
+	msleep(1);
+	mchip_set(MCHIP_MM_INTA, MCHIP_MM_INTA_HIC_1_MASK);
+
+	switch (meye.pm_mchip_mode) {
+	case MCHIP_HIC_MODE_CONT_OUT:
+		mchip_continuous_start();
+		break;
+	case MCHIP_HIC_MODE_CONT_COMP:
+		mchip_cont_compression_start();
+		break;
+	}
+	return 0;
+}
+
+static int meye_probe(struct pci_dev *pcidev, const struct pci_device_id *ent)
+{
+	static const struct v4l2_ctrl_config ctrl_agc = {
+		.id = V4L2_CID_MEYE_AGC,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.ops = &meye_ctrl_ops,
+		.name = "AGC",
+		.max = 63,
+		.step = 1,
+		.def = 48,
+		.flags = V4L2_CTRL_FLAG_SLIDER,
+	};
+	static const struct v4l2_ctrl_config ctrl_picture = {
+		.id = V4L2_CID_MEYE_PICTURE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.ops = &meye_ctrl_ops,
+		.name = "Picture",
+		.max = 63,
+		.step = 1,
+	};
+	static const struct v4l2_ctrl_config ctrl_framerate = {
+		.id = V4L2_CID_MEYE_FRAMERATE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.ops = &meye_ctrl_ops,
+		.name = "Framerate",
+		.max = 31,
+		.step = 1,
+	};
+	struct v4l2_device *v4l2_dev = &meye.v4l2_dev;
+	int ret = -EBUSY;
+	unsigned long mchip_adr;
+
+	if (meye.mchip_dev != NULL) {
+		printk(KERN_ERR "meye: only one device allowed!\n");
+		return ret;
+	}
+
+	ret = v4l2_device_register(&pcidev->dev, v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+		return ret;
+	}
+	ret = -ENOMEM;
+	meye.mchip_dev = pcidev;
+
+	meye.grab_temp = vmalloc(array_size(PAGE_SIZE, MCHIP_NB_PAGES_MJPEG));
+	if (!meye.grab_temp)
+		goto outvmalloc;
+
+	spin_lock_init(&meye.grabq_lock);
+	if (kfifo_alloc(&meye.grabq, sizeof(int) * MEYE_MAX_BUFNBRS,
+			GFP_KERNEL))
+		goto outkfifoalloc1;
+
+	spin_lock_init(&meye.doneq_lock);
+	if (kfifo_alloc(&meye.doneq, sizeof(int) * MEYE_MAX_BUFNBRS,
+			GFP_KERNEL))
+		goto outkfifoalloc2;
+
+	meye.vdev = meye_template;
+	meye.vdev.v4l2_dev = &meye.v4l2_dev;
+
+	ret = sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERA, 1);
+	if (ret) {
+		v4l2_err(v4l2_dev, "meye: unable to power on the camera\n");
+		v4l2_err(v4l2_dev, "meye: did you enable the camera in sonypi using the module options ?\n");
+		goto outsonypienable;
+	}
+
+	ret = pci_enable_device(meye.mchip_dev);
+	if (ret) {
+		v4l2_err(v4l2_dev, "meye: pci_enable_device failed\n");
+		goto outenabledev;
+	}
+
+	ret = -EIO;
+	mchip_adr = pci_resource_start(meye.mchip_dev,0);
+	if (!mchip_adr) {
+		v4l2_err(v4l2_dev, "meye: mchip has no device base address\n");
+		goto outregions;
+	}
+	if (!request_mem_region(pci_resource_start(meye.mchip_dev, 0),
+				pci_resource_len(meye.mchip_dev, 0),
+				"meye")) {
+		v4l2_err(v4l2_dev, "meye: request_mem_region failed\n");
+		goto outregions;
+	}
+	meye.mchip_mmregs = ioremap(mchip_adr, MCHIP_MM_REGS);
+	if (!meye.mchip_mmregs) {
+		v4l2_err(v4l2_dev, "meye: ioremap failed\n");
+		goto outremap;
+	}
+
+	meye.mchip_irq = pcidev->irq;
+	if (request_irq(meye.mchip_irq, meye_irq,
+			IRQF_SHARED, "meye", meye_irq)) {
+		v4l2_err(v4l2_dev, "request_irq failed\n");
+		goto outreqirq;
+	}
+
+	pci_write_config_byte(meye.mchip_dev, PCI_CACHE_LINE_SIZE, 8);
+	pci_write_config_byte(meye.mchip_dev, PCI_LATENCY_TIMER, 64);
+
+	pci_set_master(meye.mchip_dev);
+
+	/* Ask the camera to perform a soft reset. */
+	pci_write_config_word(meye.mchip_dev, MCHIP_PCI_SOFTRESET_SET, 1);
+
+	mchip_delay(MCHIP_HIC_CMD, 0);
+	mchip_delay(MCHIP_HIC_STATUS, MCHIP_HIC_STATUS_IDLE);
+
+	msleep(1);
+	mchip_set(MCHIP_VRJ_SOFT_RESET, 1);
+
+	msleep(1);
+	mchip_set(MCHIP_MM_PCI_MODE, 5);
+
+	msleep(1);
+	mchip_set(MCHIP_MM_INTA, MCHIP_MM_INTA_HIC_1_MASK);
+
+	mutex_init(&meye.lock);
+	init_waitqueue_head(&meye.proc_list);
+
+	v4l2_ctrl_handler_init(&meye.hdl, 3);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 63, 1, 32);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_HUE, 0, 63, 1, 32);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 63, 1, 32);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 63, 1, 32);
+	v4l2_ctrl_new_custom(&meye.hdl, &ctrl_agc, NULL);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_SHARPNESS, 0, 63, 1, 32);
+	v4l2_ctrl_new_custom(&meye.hdl, &ctrl_picture, NULL);
+	v4l2_ctrl_new_std(&meye.hdl, &meye_ctrl_ops,
+			  V4L2_CID_JPEG_COMPRESSION_QUALITY, 0, 10, 1, 8);
+	v4l2_ctrl_new_custom(&meye.hdl, &ctrl_framerate, NULL);
+	if (meye.hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		goto outvideoreg;
+	}
+
+	v4l2_ctrl_handler_setup(&meye.hdl);
+	meye.vdev.ctrl_handler = &meye.hdl;
+
+	if (video_register_device(&meye.vdev, VFL_TYPE_VIDEO,
+				  video_nr) < 0) {
+		v4l2_err(v4l2_dev, "video_register_device failed\n");
+		goto outvideoreg;
+	}
+
+	v4l2_info(v4l2_dev, "Motion Eye Camera Driver v%s.\n",
+	       MEYE_DRIVER_VERSION);
+	v4l2_info(v4l2_dev, "mchip KL5A72002 rev. %d, base %lx, irq %d\n",
+	       meye.mchip_dev->revision, mchip_adr, meye.mchip_irq);
+
+	return 0;
+
+outvideoreg:
+	v4l2_ctrl_handler_free(&meye.hdl);
+	free_irq(meye.mchip_irq, meye_irq);
+outreqirq:
+	iounmap(meye.mchip_mmregs);
+outremap:
+	release_mem_region(pci_resource_start(meye.mchip_dev, 0),
+			   pci_resource_len(meye.mchip_dev, 0));
+outregions:
+	pci_disable_device(meye.mchip_dev);
+outenabledev:
+	sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERA, 0);
+outsonypienable:
+	kfifo_free(&meye.doneq);
+outkfifoalloc2:
+	kfifo_free(&meye.grabq);
+outkfifoalloc1:
+	vfree(meye.grab_temp);
+outvmalloc:
+	return ret;
+}
+
+static void meye_remove(struct pci_dev *pcidev)
+{
+	video_unregister_device(&meye.vdev);
+
+	mchip_hic_stop();
+
+	mchip_dma_free();
+
+	/* disable interrupts */
+	mchip_set(MCHIP_MM_INTA, 0x0);
+
+	free_irq(meye.mchip_irq, meye_irq);
+
+	iounmap(meye.mchip_mmregs);
+
+	release_mem_region(pci_resource_start(meye.mchip_dev, 0),
+			   pci_resource_len(meye.mchip_dev, 0));
+
+	pci_disable_device(meye.mchip_dev);
+
+	sony_pic_camera_command(SONY_PIC_COMMAND_SETCAMERA, 0);
+
+	kfifo_free(&meye.doneq);
+	kfifo_free(&meye.grabq);
+
+	vfree(meye.grab_temp);
+
+	if (meye.grab_fbuffer) {
+		rvfree(meye.grab_fbuffer, gbuffers*gbufsize);
+		meye.grab_fbuffer = NULL;
+	}
+
+	printk(KERN_INFO "meye: removed\n");
+}
+
+static const struct pci_device_id meye_pci_tbl[] = {
+	{ PCI_VDEVICE(KAWASAKI, PCI_DEVICE_ID_MCHIP_KL5A72002), 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(pci, meye_pci_tbl);
+
+static SIMPLE_DEV_PM_OPS(meye_pm_ops, meye_suspend, meye_resume);
+
+static struct pci_driver meye_driver = {
+	.name		= "meye",
+	.id_table	= meye_pci_tbl,
+	.probe		= meye_probe,
+	.remove		= meye_remove,
+	.driver.pm	= &meye_pm_ops,
+};
+
+static int __init meye_init(void)
+{
+	gbuffers = max(2, min((int)gbuffers, MEYE_MAX_BUFNBRS));
+	if (gbufsize > MEYE_MAX_BUFSIZE)
+		gbufsize = MEYE_MAX_BUFSIZE;
+	gbufsize = PAGE_ALIGN(gbufsize);
+	printk(KERN_INFO "meye: using %d buffers with %dk (%dk total) for capture\n",
+			 gbuffers,
+			 gbufsize / 1024, gbuffers * gbufsize / 1024);
+	return pci_register_driver(&meye_driver);
+}
+
+static void __exit meye_exit(void)
+{
+	pci_unregister_driver(&meye_driver);
+}
+
+module_init(meye_init);
+module_exit(meye_exit);
diff --git a/drivers/staging/media/deprecated/meye/meye.h b/drivers/staging/media/deprecated/meye/meye.h
new file mode 100644
index 000000000..5fa6552cf
--- /dev/null
+++ b/drivers/staging/media/deprecated/meye/meye.h
@@ -0,0 +1,311 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Motion Eye video4linux driver for Sony Vaio PictureBook
+ *
+ * Copyright (C) 2001-2004 Stelian Pop <stelian@popies.net>
+ *
+ * Copyright (C) 2001-2002 Alcôve <www.alcove.com>
+ *
+ * Copyright (C) 2000 Andrew Tridgell <tridge@valinux.com>
+ *
+ * Earlier work by Werner Almesberger, Paul `Rusty' Russell and Paul Mackerras.
+ *
+ * Some parts borrowed from various video4linux drivers, especially
+ * bttv-driver.c and zoran.c, see original files for credits.
+ */
+
+#ifndef _MEYE_PRIV_H_
+#define _MEYE_PRIV_H_
+
+#define MEYE_DRIVER_MAJORVERSION	 1
+#define MEYE_DRIVER_MINORVERSION	14
+
+#define MEYE_DRIVER_VERSION __stringify(MEYE_DRIVER_MAJORVERSION) "." \
+			    __stringify(MEYE_DRIVER_MINORVERSION)
+
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/kfifo.h>
+#include <media/v4l2-ctrls.h>
+
+/****************************************************************************/
+/* Motion JPEG chip registers                                               */
+/****************************************************************************/
+
+/* Motion JPEG chip PCI configuration registers */
+#define MCHIP_PCI_POWER_CSR		0x54
+#define MCHIP_PCI_MCORE_STATUS		0x60		/* see HIC_STATUS   */
+#define MCHIP_PCI_HOSTUSEREQ_SET	0x64
+#define MCHIP_PCI_HOSTUSEREQ_CLR	0x68
+#define MCHIP_PCI_LOWPOWER_SET		0x6c
+#define MCHIP_PCI_LOWPOWER_CLR		0x70
+#define MCHIP_PCI_SOFTRESET_SET		0x74
+
+/* Motion JPEG chip memory mapped registers */
+#define MCHIP_MM_REGS			0x200		/* 512 bytes        */
+#define MCHIP_REG_TIMEOUT		1000		/* reg access, ~us  */
+#define MCHIP_MCC_VRJ_TIMEOUT		1000		/* MCC & VRJ access */
+
+#define MCHIP_MM_PCI_MODE		0x00		/* PCI access mode */
+#define MCHIP_MM_PCI_MODE_RETRY		0x00000001	/* retry mode */
+#define MCHIP_MM_PCI_MODE_MASTER	0x00000002	/* master access */
+#define MCHIP_MM_PCI_MODE_READ_LINE	0x00000004	/* read line */
+
+#define MCHIP_MM_INTA			0x04		/* Int status/mask */
+#define MCHIP_MM_INTA_MCC		0x00000001	/* MCC interrupt */
+#define MCHIP_MM_INTA_VRJ		0x00000002	/* VRJ interrupt */
+#define MCHIP_MM_INTA_HIC_1		0x00000004	/* one frame done */
+#define MCHIP_MM_INTA_HIC_1_MASK	0x00000400	/* 1: enable */
+#define MCHIP_MM_INTA_HIC_END		0x00000008	/* all frames done */
+#define MCHIP_MM_INTA_HIC_END_MASK	0x00000800
+#define MCHIP_MM_INTA_JPEG		0x00000010	/* decompress. error */
+#define MCHIP_MM_INTA_JPEG_MASK		0x00001000
+#define MCHIP_MM_INTA_CAPTURE		0x00000020	/* capture end */
+#define MCHIP_MM_INTA_PCI_ERR		0x00000040	/* PCI error */
+#define MCHIP_MM_INTA_PCI_ERR_MASK	0x00004000
+
+#define MCHIP_MM_PT_ADDR		0x08		/* page table address*/
+							/* n*4kB */
+#define MCHIP_NB_PAGES			1024		/* pages for display */
+#define MCHIP_NB_PAGES_MJPEG		256		/* pages for mjpeg */
+
+#define MCHIP_MM_FIR(n)			(0x0c+(n)*4)	/* Frame info 0-3 */
+#define MCHIP_MM_FIR_RDY		0x00000001	/* frame ready */
+#define MCHIP_MM_FIR_FAILFR_MASK	0xf8000000	/* # of failed frames */
+#define MCHIP_MM_FIR_FAILFR_SHIFT	27
+
+	/* continuous comp/decomp mode */
+#define MCHIP_MM_FIR_C_ENDL_MASK	0x000007fe	/* end DW [10] */
+#define MCHIP_MM_FIR_C_ENDL_SHIFT	1
+#define MCHIP_MM_FIR_C_ENDP_MASK	0x0007f800	/* end page [8] */
+#define MCHIP_MM_FIR_C_ENDP_SHIFT	11
+#define MCHIP_MM_FIR_C_STARTP_MASK	0x07f80000	/* start page [8] */
+#define MCHIP_MM_FIR_C_STARTP_SHIFT	19
+
+	/* continuous picture output mode */
+#define MCHIP_MM_FIR_O_STARTP_MASK	0x7ffe0000	/* start page [10] */
+#define MCHIP_MM_FIR_O_STARTP_SHIFT	17
+
+#define MCHIP_MM_FIFO_DATA		0x1c		/* PCI TGT FIFO data */
+#define MCHIP_MM_FIFO_STATUS		0x20		/* PCI TGT FIFO stat */
+#define MCHIP_MM_FIFO_MASK		0x00000003
+#define MCHIP_MM_FIFO_WAIT_OR_READY	0x00000002      /* Bits common to WAIT & READY*/
+#define MCHIP_MM_FIFO_IDLE		0x0		/* HIC idle */
+#define MCHIP_MM_FIFO_IDLE1		0x1		/* idem ??? */
+#define	MCHIP_MM_FIFO_WAIT		0x2		/* wait request */
+#define MCHIP_MM_FIFO_READY		0x3		/* data ready */
+
+#define MCHIP_HIC_HOST_USEREQ		0x40		/* host uses MCORE */
+
+#define MCHIP_HIC_TP_BUSY		0x44		/* taking picture */
+
+#define MCHIP_HIC_PIC_SAVED		0x48		/* pic in SDRAM */
+
+#define MCHIP_HIC_LOWPOWER		0x4c		/* clock stopped */
+
+#define MCHIP_HIC_CTL			0x50		/* HIC control */
+#define MCHIP_HIC_CTL_SOFT_RESET	0x00000001	/* MCORE reset */
+#define MCHIP_HIC_CTL_MCORE_RDY		0x00000002	/* MCORE ready */
+
+#define MCHIP_HIC_CMD			0x54		/* HIC command */
+#define MCHIP_HIC_CMD_BITS		0x00000003      /* cmd width=[1:0]*/
+#define MCHIP_HIC_CMD_NOOP		0x0
+#define MCHIP_HIC_CMD_START		0x1
+#define MCHIP_HIC_CMD_STOP		0x2
+
+#define MCHIP_HIC_MODE			0x58
+#define MCHIP_HIC_MODE_NOOP		0x0
+#define MCHIP_HIC_MODE_STILL_CAP	0x1		/* still pic capt */
+#define MCHIP_HIC_MODE_DISPLAY		0x2		/* display */
+#define MCHIP_HIC_MODE_STILL_COMP	0x3		/* still pic comp. */
+#define MCHIP_HIC_MODE_STILL_DECOMP	0x4		/* still pic decomp. */
+#define MCHIP_HIC_MODE_CONT_COMP	0x5		/* cont capt+comp */
+#define MCHIP_HIC_MODE_CONT_DECOMP	0x6		/* cont decomp+disp */
+#define MCHIP_HIC_MODE_STILL_OUT	0x7		/* still pic output */
+#define MCHIP_HIC_MODE_CONT_OUT		0x8		/* cont output */
+
+#define MCHIP_HIC_STATUS		0x5c
+#define MCHIP_HIC_STATUS_MCC_RDY	0x00000001	/* MCC reg acc ok */
+#define MCHIP_HIC_STATUS_VRJ_RDY	0x00000002	/* VRJ reg acc ok */
+#define MCHIP_HIC_STATUS_IDLE           0x00000003
+#define MCHIP_HIC_STATUS_CAPDIS		0x00000004	/* cap/disp in prog */
+#define MCHIP_HIC_STATUS_COMPDEC	0x00000008	/* (de)comp in prog */
+#define MCHIP_HIC_STATUS_BUSY		0x00000010	/* HIC busy */
+
+#define MCHIP_HIC_S_RATE		0x60		/* MJPEG # frames */
+
+#define MCHIP_HIC_PCI_VFMT		0x64		/* video format */
+#define MCHIP_HIC_PCI_VFMT_YVYU		0x00000001	/* 0: V Y' U Y */
+							/* 1: Y' V Y U */
+
+#define MCHIP_MCC_CMD			0x80		/* MCC commands */
+#define MCHIP_MCC_CMD_INITIAL		0x0		/* idle ? */
+#define MCHIP_MCC_CMD_IIC_START_SET	0x1
+#define MCHIP_MCC_CMD_IIC_END_SET	0x2
+#define MCHIP_MCC_CMD_FM_WRITE		0x3		/* frame memory */
+#define MCHIP_MCC_CMD_FM_READ		0x4
+#define MCHIP_MCC_CMD_FM_STOP		0x5
+#define MCHIP_MCC_CMD_CAPTURE		0x6
+#define MCHIP_MCC_CMD_DISPLAY		0x7
+#define MCHIP_MCC_CMD_END_DISP		0x8
+#define MCHIP_MCC_CMD_STILL_COMP	0x9
+#define MCHIP_MCC_CMD_STILL_DECOMP	0xa
+#define MCHIP_MCC_CMD_STILL_OUTPUT	0xb
+#define MCHIP_MCC_CMD_CONT_OUTPUT	0xc
+#define MCHIP_MCC_CMD_CONT_COMP		0xd
+#define MCHIP_MCC_CMD_CONT_DECOMP	0xe
+#define MCHIP_MCC_CMD_RESET		0xf		/* MCC reset */
+
+#define MCHIP_MCC_IIC_WR		0x84
+
+#define MCHIP_MCC_MCC_WR		0x88
+
+#define MCHIP_MCC_MCC_RD		0x8c
+
+#define MCHIP_MCC_STATUS		0x90
+#define MCHIP_MCC_STATUS_CAPT		0x00000001	/* capturing */
+#define MCHIP_MCC_STATUS_DISP		0x00000002	/* displaying */
+#define MCHIP_MCC_STATUS_COMP		0x00000004	/* compressing */
+#define MCHIP_MCC_STATUS_DECOMP		0x00000008	/* decompressing */
+#define MCHIP_MCC_STATUS_MCC_WR		0x00000010	/* register ready */
+#define MCHIP_MCC_STATUS_MCC_RD		0x00000020	/* register ready */
+#define MCHIP_MCC_STATUS_IIC_WR		0x00000040	/* register ready */
+#define MCHIP_MCC_STATUS_OUTPUT		0x00000080	/* output in prog */
+
+#define MCHIP_MCC_SIG_POLARITY		0x94
+#define MCHIP_MCC_SIG_POL_VS_H		0x00000001	/* VS active-high */
+#define MCHIP_MCC_SIG_POL_HS_H		0x00000002	/* HS active-high */
+#define MCHIP_MCC_SIG_POL_DOE_H		0x00000004	/* DOE active-high */
+
+#define MCHIP_MCC_IRQ			0x98
+#define MCHIP_MCC_IRQ_CAPDIS_STRT	0x00000001	/* cap/disp started */
+#define MCHIP_MCC_IRQ_CAPDIS_STRT_MASK	0x00000010
+#define MCHIP_MCC_IRQ_CAPDIS_END	0x00000002	/* cap/disp ended */
+#define MCHIP_MCC_IRQ_CAPDIS_END_MASK	0x00000020
+#define MCHIP_MCC_IRQ_COMPDEC_STRT	0x00000004	/* (de)comp started */
+#define MCHIP_MCC_IRQ_COMPDEC_STRT_MASK	0x00000040
+#define MCHIP_MCC_IRQ_COMPDEC_END	0x00000008	/* (de)comp ended */
+#define MCHIP_MCC_IRQ_COMPDEC_END_MASK	0x00000080
+
+#define MCHIP_MCC_HSTART		0x9c		/* video in */
+#define MCHIP_MCC_VSTART		0xa0
+#define MCHIP_MCC_HCOUNT		0xa4
+#define MCHIP_MCC_VCOUNT		0xa8
+#define MCHIP_MCC_R_XBASE		0xac		/* capt/disp */
+#define MCHIP_MCC_R_YBASE		0xb0
+#define MCHIP_MCC_R_XRANGE		0xb4
+#define MCHIP_MCC_R_YRANGE		0xb8
+#define MCHIP_MCC_B_XBASE		0xbc		/* comp/decomp */
+#define MCHIP_MCC_B_YBASE		0xc0
+#define MCHIP_MCC_B_XRANGE		0xc4
+#define MCHIP_MCC_B_YRANGE		0xc8
+
+#define MCHIP_MCC_R_SAMPLING		0xcc		/* 1: 1:4 */
+
+#define MCHIP_VRJ_CMD			0x100		/* VRJ commands */
+
+/* VRJ registers (see table 12.2.4) */
+#define MCHIP_VRJ_COMPRESSED_DATA	0x1b0
+#define MCHIP_VRJ_PIXEL_DATA		0x1b8
+
+#define MCHIP_VRJ_BUS_MODE		0x100
+#define MCHIP_VRJ_SIGNAL_ACTIVE_LEVEL	0x108
+#define MCHIP_VRJ_PDAT_USE		0x110
+#define MCHIP_VRJ_MODE_SPECIFY		0x118
+#define MCHIP_VRJ_LIMIT_COMPRESSED_LO	0x120
+#define MCHIP_VRJ_LIMIT_COMPRESSED_HI	0x124
+#define MCHIP_VRJ_COMP_DATA_FORMAT	0x128
+#define MCHIP_VRJ_TABLE_DATA		0x140
+#define MCHIP_VRJ_RESTART_INTERVAL	0x148
+#define MCHIP_VRJ_NUM_LINES		0x150
+#define MCHIP_VRJ_NUM_PIXELS		0x158
+#define MCHIP_VRJ_NUM_COMPONENTS	0x160
+#define MCHIP_VRJ_SOF1			0x168
+#define MCHIP_VRJ_SOF2			0x170
+#define MCHIP_VRJ_SOF3			0x178
+#define MCHIP_VRJ_SOF4			0x180
+#define MCHIP_VRJ_SOS			0x188
+#define MCHIP_VRJ_SOFT_RESET		0x190
+
+#define MCHIP_VRJ_STATUS		0x1c0
+#define MCHIP_VRJ_STATUS_BUSY		0x00001
+#define MCHIP_VRJ_STATUS_COMP_ACCESS	0x00002
+#define MCHIP_VRJ_STATUS_PIXEL_ACCESS	0x00004
+#define MCHIP_VRJ_STATUS_ERROR		0x00008
+
+#define MCHIP_VRJ_IRQ_FLAG		0x1c8
+#define MCHIP_VRJ_ERROR_REPORT		0x1d8
+
+#define MCHIP_VRJ_START_COMMAND		0x1a0
+
+/****************************************************************************/
+/* Driver definitions.                                                      */
+/****************************************************************************/
+
+/* Sony Programmable I/O Controller for accessing the camera commands */
+#include <linux/sony-laptop.h>
+
+/* private API definitions */
+#include <linux/meye.h>
+#include <linux/mutex.h>
+
+
+/* Enable jpg software correction */
+#define MEYE_JPEG_CORRECTION	1
+
+/* Maximum size of a buffer */
+#define MEYE_MAX_BUFSIZE	614400	/* 640 * 480 * 2 */
+
+/* Maximum number of buffers */
+#define MEYE_MAX_BUFNBRS	32
+
+/* State of a buffer */
+#define MEYE_BUF_UNUSED	0	/* not used */
+#define MEYE_BUF_USING	1	/* currently grabbing / playing */
+#define MEYE_BUF_DONE	2	/* done */
+
+/* grab buffer */
+struct meye_grab_buffer {
+	int state;			/* state of buffer */
+	unsigned long size;		/* size of jpg frame */
+	u64 ts;				/* timestamp */
+	unsigned long sequence;		/* sequence number */
+};
+
+/* size of kfifos containing buffer indices */
+#define MEYE_QUEUE_SIZE	MEYE_MAX_BUFNBRS
+
+/* Motion Eye device structure */
+struct meye {
+	struct v4l2_device v4l2_dev;	/* Main v4l2_device struct */
+	struct v4l2_ctrl_handler hdl;
+	struct pci_dev *mchip_dev;	/* pci device */
+	u8 mchip_irq;			/* irq */
+	u8 mchip_mode;			/* actual mchip mode: HIC_MODE... */
+	u8 mchip_fnum;			/* current mchip frame number */
+	unsigned char __iomem *mchip_mmregs;/* mchip: memory mapped registers */
+	u8 *mchip_ptable[MCHIP_NB_PAGES];/* mchip: ptable */
+	void *mchip_ptable_toc;		/* mchip: ptable toc */
+	dma_addr_t mchip_dmahandle;	/* mchip: dma handle to ptable toc */
+	unsigned char *grab_fbuffer;	/* capture framebuffer */
+	unsigned char *grab_temp;	/* temporary buffer */
+					/* list of buffers */
+	struct meye_grab_buffer grab_buffer[MEYE_MAX_BUFNBRS];
+	int vma_use_count[MEYE_MAX_BUFNBRS]; /* mmap count */
+	struct mutex lock;		/* mutex for open/mmap... */
+	struct kfifo grabq;		/* queue for buffers to be grabbed */
+	spinlock_t grabq_lock;		/* lock protecting the queue */
+	struct kfifo doneq;		/* queue for grabbed buffers */
+	spinlock_t doneq_lock;		/* lock protecting the queue */
+	wait_queue_head_t proc_list;	/* wait queue */
+	struct video_device vdev;	/* video device parameters */
+	u16 brightness;
+	u16 hue;
+	u16 contrast;
+	u16 colour;
+	struct meye_params params;	/* additional parameters */
+	unsigned long in_use;		/* set to 1 if the device is in use */
+	u8 pm_mchip_mode;		/* old mchip mode */
+};
+
+#endif
diff --git a/drivers/staging/media/deprecated/saa7146/Kconfig b/drivers/staging/media/deprecated/saa7146/Kconfig
new file mode 100644
index 000000000..54154da79
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/Kconfig
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0
+source "drivers/staging/media/deprecated/saa7146/common/Kconfig"
+source "drivers/staging/media/deprecated/saa7146/av7110/Kconfig"
+source "drivers/staging/media/deprecated/saa7146/saa7146/Kconfig"
+source "drivers/staging/media/deprecated/saa7146/ttpci/Kconfig"
diff --git a/drivers/staging/media/deprecated/saa7146/Makefile b/drivers/staging/media/deprecated/saa7146/Makefile
new file mode 100644
index 000000000..68e7aa10c
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/Makefile
@@ -0,0 +1,2 @@
+ # SPDX-License-Identifier: GPL-2.0-only
+obj-y += common/ av7110/ saa7146/ ttpci/
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/Kconfig b/drivers/staging/media/deprecated/saa7146/av7110/Kconfig
new file mode 100644
index 000000000..1571eab31
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/Kconfig
@@ -0,0 +1,106 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config DVB_AV7110_IR
+	bool
+	depends on RC_CORE=y || RC_CORE = DVB_AV7110
+	default DVB_AV7110
+
+config DVB_AV7110
+	tristate "AV7110 cards (DEPRECATED)"
+	depends on DVB_CORE && PCI && I2C
+	select TTPCI_EEPROM
+	select VIDEO_SAA7146_VV
+	depends on VIDEO_DEV	# dependencies of VIDEO_SAA7146_VV
+	select DVB_VES1820 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_VES1X93 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0299 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA8083 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_SP8870 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0297 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_L64781 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Support for SAA7146 and AV7110 based DVB cards as produced
+	  by Fujitsu-Siemens, Technotrend, Hauppauge and others.
+
+	  This driver only supports the fullfeatured cards with
+	  onboard MPEG2 decoder.
+
+	  This driver needs an external firmware. Please use the script
+	  "<kerneldir>/scripts/get_dvb_firmware av7110" to
+	  download/extract it, and then copy it to /usr/lib/hotplug/firmware
+	  or /lib/firmware (depending on configuration of firmware hotplug).
+
+	  Alternatively, you can download the file and use the kernel's
+	  EXTRA_FIRMWARE configuration option to build it into your
+	  kernel image by adding the filename to the EXTRA_FIRMWARE
+	  configuration option string.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a card and want to use it.
+
+config DVB_AV7110_OSD
+	bool "AV7110 OSD support (DEPRECATED)"
+	depends on DVB_AV7110
+	default y if DVB_AV7110=y || DVB_AV7110=m
+	help
+	  The AV7110 firmware provides some code to generate an OnScreenDisplay
+	  on the video output. This is kind of nonstandard and not guaranteed to
+	  be maintained.
+
+	  Anyway, some popular DVB software like VDR uses this OSD to render
+	  its menus, so say Y if you want to use this software.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  All other people say N.
+
+config DVB_BUDGET_PATCH
+	tristate "AV7110 cards with Budget Patch (DEPRECATED)"
+	depends on DVB_BUDGET_CORE && I2C
+	depends on DVB_AV7110
+	select DVB_STV0299 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_VES1X93 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA8083 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Support for Budget Patch (full TS) modification on
+	  SAA7146+AV7110 based cards (DVB-S cards). This
+	  driver doesn't use onboard MPEG2 decoder. The
+	  card is driven in Budget-only mode. Card is
+	  required to have loaded firmware to tune properly.
+	  Firmware can be loaded by insertion and removal of
+	  standard AV7110 driver prior to loading this
+	  driver.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a card and want to use it.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called budget-patch.
+
+if DVB_AV7110
+
+# Frontend driver that it is used only by AV7110 driver
+# While technically independent, it doesn't make sense to keep
+# it if we drop support for AV7110, as no other driver will use it.
+
+config DVB_SP8870
+	tristate "Spase sp8870 based (DEPRECATED)"
+	depends on DVB_CORE && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  A DVB-T tuner module. Say Y when you want to support this frontend.
+
+	  This driver needs external firmware. Please use the command
+	  "<kerneldir>/scripts/get_dvb_firmware sp8870" to
+	  download/extract it, and then copy it to /usr/lib/hotplug/firmware
+	  or /lib/firmware (depending on configuration of firmware hotplug).
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+endif
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/Makefile b/drivers/staging/media/deprecated/saa7146/av7110/Makefile
new file mode 100644
index 000000000..c04cd0a59
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/Makefile
@@ -0,0 +1,23 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the AV7110 DVB device driver
+#
+
+dvb-ttpci-objs := av7110_hw.o av7110_v4l.o av7110_av.o av7110_ca.o av7110.o \
+		  av7110_ipack.o dvb_filter.o
+
+ifdef CONFIG_DVB_AV7110_IR
+dvb-ttpci-objs += av7110_ir.o
+endif
+
+obj-$(CONFIG_DVB_BUDGET_PATCH) += budget-patch.o
+
+obj-$(CONFIG_DVB_AV7110) += dvb-ttpci.o
+
+obj-$(CONFIG_DVB_SP8870) += sp8870.o
+
+ccflags-y += -I $(srctree)/drivers/media/dvb-frontends
+ccflags-y += -I $(srctree)/drivers/media/tuners
+ccflags-y += -I $(srctree)/drivers/media/common
+ccflags-y += -I $(srctree)/drivers/staging/media/deprecated/saa7146/ttpci
+ccflags-y += -I $(srctree)/drivers/staging/media/deprecated/saa7146/common
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/TODO b/drivers/staging/media/deprecated/saa7146/av7110/TODO
new file mode 100644
index 000000000..38817e04b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/TODO
@@ -0,0 +1,9 @@
+- This driver is too old and relies on a different API.
+  Drop it from Kernel on a couple of versions.
+- Cleanup patches for the drivers here won't be accepted.
+
+These drivers are now deprecated with the intent of
+removing them altogether by the beginning of 2023.
+
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-bilingual-channel-select.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-bilingual-channel-select.rst
new file mode 100644
index 000000000..33b536331
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-bilingual-channel-select.rst
@@ -0,0 +1,58 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_BILINGUAL_CHANNEL_SELECT:
+
+==============================
+AUDIO_BILINGUAL_CHANNEL_SELECT
+==============================
+
+Name
+----
+
+AUDIO_BILINGUAL_CHANNEL_SELECT
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_BILINGUAL_CHANNEL_SELECT
+
+``int ioctl(int fd, AUDIO_BILINGUAL_CHANNEL_SELECT, struct audio_channel_select *select)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  audio_channel_select_t ch
+
+       -  Select the output format of the audio (mono left/right, stereo).
+
+Description
+-----------
+
+This ioctl is obsolete. Do not use in new drivers. It has been replaced
+by the V4L2 ``V4L2_CID_MPEG_AUDIO_DEC_MULTILINGUAL_PLAYBACK`` control
+for MPEG decoders controlled through V4L2.
+
+This ioctl call asks the Audio Device to select the requested channel
+for bilingual streams if possible.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-channel-select.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-channel-select.rst
new file mode 100644
index 000000000..74093df92
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-channel-select.rst
@@ -0,0 +1,57 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_CHANNEL_SELECT:
+
+====================
+AUDIO_CHANNEL_SELECT
+====================
+
+Name
+----
+
+AUDIO_CHANNEL_SELECT
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_CHANNEL_SELECT
+
+``int ioctl(int fd, AUDIO_CHANNEL_SELECT, struct audio_channel_select *select)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  audio_channel_select_t ch
+
+       -  Select the output format of the audio (mono left/right, stereo).
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 ``V4L2_CID_MPEG_AUDIO_DEC_PLAYBACK`` control instead.
+
+This ioctl call asks the Audio Device to select the requested channel if
+possible.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-clear-buffer.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-clear-buffer.rst
new file mode 100644
index 000000000..a0ebb0278
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-clear-buffer.rst
@@ -0,0 +1,48 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_CLEAR_BUFFER:
+
+==================
+AUDIO_CLEAR_BUFFER
+==================
+
+Name
+----
+
+AUDIO_CLEAR_BUFFER
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_CLEAR_BUFFER
+
+``int ioctl(int fd, AUDIO_CLEAR_BUFFER)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to clear all software and hardware
+buffers of the audio decoder device.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-continue.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-continue.rst
new file mode 100644
index 000000000..a2e9850f3
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-continue.rst
@@ -0,0 +1,48 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_CONTINUE:
+
+==============
+AUDIO_CONTINUE
+==============
+
+Name
+----
+
+AUDIO_CONTINUE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_CONTINUE
+
+``int ioctl(int fd, AUDIO_CONTINUE)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This ioctl restarts the decoding and playing process previously paused
+with AUDIO_PAUSE command.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-fclose.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-fclose.rst
new file mode 100644
index 000000000..77857d578
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-fclose.rst
@@ -0,0 +1,51 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _audio_fclose:
+
+========================
+Digital TV audio close()
+========================
+
+Name
+----
+
+Digital TV audio close()
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:function:: int close(int fd)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This system call closes a previously opened audio device.
+
+Return Value
+------------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EBADF``
+
+       -  fd is not a valid open file descriptor.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-fopen.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-fopen.rst
new file mode 100644
index 000000000..774daaab3
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-fopen.rst
@@ -0,0 +1,103 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _audio_fopen:
+
+=======================
+Digital TV audio open()
+=======================
+
+Name
+----
+
+Digital TV audio open()
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:function:: int open(const char *deviceName, int flags)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  const char \*deviceName
+
+       -  Name of specific audio device.
+
+    -  .. row 2
+
+       -  int flags
+
+       -  A bit-wise OR of the following flags:
+
+    -  .. row 3
+
+       -
+       -  O_RDONLY read-only access
+
+    -  .. row 4
+
+       -
+       -  O_RDWR read/write access
+
+    -  .. row 5
+
+       -
+       -  O_NONBLOCK open in non-blocking mode
+
+    -  .. row 6
+
+       -
+       -  (blocking mode is the default)
+
+Description
+-----------
+
+This system call opens a named audio device (e.g.
+/dev/dvb/adapter0/audio0) for subsequent use. When an open() call has
+succeeded, the device will be ready for use. The significance of
+blocking or non-blocking mode is described in the documentation for
+functions where there is a difference. It does not affect the semantics
+of the open() call itself. A device opened in blocking mode can later be
+put into non-blocking mode (and vice versa) using the F_SETFL command
+of the fcntl system call. This is a standard system call, documented in
+the Linux manual page for fcntl. Only one user can open the Audio Device
+in O_RDWR mode. All other attempts to open the device in this mode will
+fail, and an error code will be returned. If the Audio Device is opened
+in O_RDONLY mode, the only ioctl call that can be used is
+AUDIO_GET_STATUS. All other call will return with an error code.
+
+Return Value
+------------
+
+.. tabularcolumns:: |p{2.5cm}|p{15.0cm}|
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``ENODEV``
+
+       -  Device driver not loaded/available.
+
+    -  .. row 2
+
+       -  ``EBUSY``
+
+       -  Device or resource busy.
+
+    -  .. row 3
+
+       -  ``EINVAL``
+
+       -  Invalid argument.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-fwrite.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-fwrite.rst
new file mode 100644
index 000000000..7b096ac2b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-fwrite.rst
@@ -0,0 +1,79 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _audio_fwrite:
+
+=========================
+Digital TV audio write()
+=========================
+
+Name
+----
+
+Digital TV audio write()
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:function:: size_t write(int fd, const void *buf, size_t count)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  void \*buf
+
+       -  Pointer to the buffer containing the PES data.
+
+    -  .. row 3
+
+       -  size_t count
+
+       -  Size of buf.
+
+Description
+-----------
+
+This system call can only be used if AUDIO_SOURCE_MEMORY is selected
+in the ioctl call AUDIO_SELECT_SOURCE. The data provided shall be in
+PES format. If O_NONBLOCK is not specified the function will block
+until buffer space is available. The amount of data to be transferred is
+implied by count.
+
+Return Value
+------------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EPERM``
+
+       -  Mode AUDIO_SOURCE_MEMORY not selected.
+
+    -  .. row 2
+
+       -  ``ENOMEM``
+
+       -  Attempted to write more data than the internal buffer can hold.
+
+    -  .. row 3
+
+       -  ``EBADF``
+
+       -  fd is not a valid open file descriptor.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-get-capabilities.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-get-capabilities.rst
new file mode 100644
index 000000000..6d9eb71da
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-get-capabilities.rst
@@ -0,0 +1,54 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_GET_CAPABILITIES:
+
+======================
+AUDIO_GET_CAPABILITIES
+======================
+
+Name
+----
+
+AUDIO_GET_CAPABILITIES
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_GET_CAPABILITIES
+
+``int ioctl(int fd, AUDIO_GET_CAPABILITIES, unsigned int *cap)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  unsigned int \*cap
+
+       -  Returns a bit array of supported sound formats.
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to tell us about the decoding
+capabilities of the audio hardware.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-get-status.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-get-status.rst
new file mode 100644
index 000000000..7ae8db2e6
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-get-status.rst
@@ -0,0 +1,54 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_GET_STATUS:
+
+================
+AUDIO_GET_STATUS
+================
+
+Name
+----
+
+AUDIO_GET_STATUS
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_GET_STATUS
+
+``int ioctl(int fd, AUDIO_GET_STATUS, struct audio_status *status)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  struct audio_status \*status
+
+       -  Returns the current state of Audio Device.
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to return the current state of the
+Audio Device.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-pause.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-pause.rst
new file mode 100644
index 000000000..d37d1ddce
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-pause.rst
@@ -0,0 +1,49 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_PAUSE:
+
+===========
+AUDIO_PAUSE
+===========
+
+Name
+----
+
+AUDIO_PAUSE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_PAUSE
+
+``int ioctl(int fd, AUDIO_PAUSE)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This ioctl call suspends the audio stream being played. Decoding and
+playing are paused. It is then possible to restart again decoding and
+playing process of the audio stream using AUDIO_CONTINUE command.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-play.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-play.rst
new file mode 100644
index 000000000..e591930b6
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-play.rst
@@ -0,0 +1,48 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_PLAY:
+
+==========
+AUDIO_PLAY
+==========
+
+Name
+----
+
+AUDIO_PLAY
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_PLAY
+
+``int ioctl(int fd, AUDIO_PLAY)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to start playing an audio stream
+from the selected source.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-select-source.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-select-source.rst
new file mode 100644
index 000000000..6a0c0f365
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-select-source.rst
@@ -0,0 +1,56 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SELECT_SOURCE:
+
+===================
+AUDIO_SELECT_SOURCE
+===================
+
+Name
+----
+
+AUDIO_SELECT_SOURCE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SELECT_SOURCE
+
+``int ioctl(int fd, AUDIO_SELECT_SOURCE, struct audio_stream_source *source)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  audio_stream_source_t source
+
+       -  Indicates the source that shall be used for the Audio stream.
+
+Description
+-----------
+
+This ioctl call informs the audio device which source shall be used for
+the input data. The possible sources are demux or memory. If
+AUDIO_SOURCE_MEMORY is selected, the data is fed to the Audio Device
+through the write command.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-av-sync.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-av-sync.rst
new file mode 100644
index 000000000..85a8016bf
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-av-sync.rst
@@ -0,0 +1,58 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_AV_SYNC:
+
+=================
+AUDIO_SET_AV_SYNC
+=================
+
+Name
+----
+
+AUDIO_SET_AV_SYNC
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_AV_SYNC
+
+``int ioctl(int fd, AUDIO_SET_AV_SYNC, boolean state)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  boolean state
+
+       -  Tells the Digital TV subsystem if A/V synchronization shall be ON or OFF.
+
+          TRUE: AV-sync ON
+
+          FALSE: AV-sync OFF
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to turn ON or OFF A/V
+synchronization.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-bypass-mode.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-bypass-mode.rst
new file mode 100644
index 000000000..80d551a20
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-bypass-mode.rst
@@ -0,0 +1,62 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_BYPASS_MODE:
+
+=====================
+AUDIO_SET_BYPASS_MODE
+=====================
+
+Name
+----
+
+AUDIO_SET_BYPASS_MODE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_BYPASS_MODE
+
+``int ioctl(int fd, AUDIO_SET_BYPASS_MODE, boolean mode)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  boolean mode
+
+       -  Enables or disables the decoding of the current Audio stream in
+	  the Digital TV subsystem.
+
+          TRUE: Bypass is disabled
+
+          FALSE: Bypass is enabled
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to bypass the Audio decoder and
+forward the stream without decoding. This mode shall be used if streams
+that can't be handled by the Digital TV system shall be decoded. Dolby
+DigitalTM streams are automatically forwarded by the Digital TV subsystem if
+the hardware can handle it.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-id.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-id.rst
new file mode 100644
index 000000000..39ad846d4
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-id.rst
@@ -0,0 +1,59 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_ID:
+
+============
+AUDIO_SET_ID
+============
+
+Name
+----
+
+AUDIO_SET_ID
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_ID
+
+``int ioctl(int fd, AUDIO_SET_ID, int id)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  int id
+
+       -  audio sub-stream id
+
+Description
+-----------
+
+This ioctl selects which sub-stream is to be decoded if a program or
+system stream is sent to the video device. If no audio stream type is
+set the id has to be in [0xC0,0xDF] for MPEG sound, in [0x80,0x87] for
+AC3 and in [0xA0,0xA7] for LPCM. More specifications may follow for
+other stream types. If the stream type is set the id just specifies the
+substream id of the audio stream and only the first 5 bits are
+recognized.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mixer.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mixer.rst
new file mode 100644
index 000000000..45dbdf480
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mixer.rst
@@ -0,0 +1,53 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_MIXER:
+
+===============
+AUDIO_SET_MIXER
+===============
+
+Name
+----
+
+AUDIO_SET_MIXER
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_MIXER
+
+``int ioctl(int fd, AUDIO_SET_MIXER, struct audio_mixer *mix)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  audio_mixer_t \*mix
+
+       -  mixer settings.
+
+Description
+-----------
+
+This ioctl lets you adjust the mixer settings of the audio decoder.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mute.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mute.rst
new file mode 100644
index 000000000..987751f92
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-mute.rst
@@ -0,0 +1,62 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_MUTE:
+
+==============
+AUDIO_SET_MUTE
+==============
+
+Name
+----
+
+AUDIO_SET_MUTE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_MUTE
+
+``int ioctl(int fd, AUDIO_SET_MUTE, boolean state)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  boolean state
+
+       -  Indicates if audio device shall mute or not.
+
+          TRUE: Audio Mute
+
+          FALSE: Audio Un-mute
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 :ref:`VIDIOC_DECODER_CMD` with the
+``V4L2_DEC_CMD_START_MUTE_AUDIO`` flag instead.
+
+This ioctl call asks the audio device to mute the stream that is
+currently being played.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-set-streamtype.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-streamtype.rst
new file mode 100644
index 000000000..77d73c748
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-set-streamtype.rst
@@ -0,0 +1,66 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_SET_STREAMTYPE:
+
+====================
+AUDIO_SET_STREAMTYPE
+====================
+
+Name
+----
+
+AUDIO_SET_STREAMTYPE
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_SET_STREAMTYPE
+
+``int ioctl(fd, AUDIO_SET_STREAMTYPE, int type)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -
+
+       -  int type
+
+       -  stream type
+
+Description
+-----------
+
+This ioctl tells the driver which kind of audio stream to expect. This
+is useful if the stream offers several audio sub-streams like LPCM and
+AC3.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
+
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EINVAL``
+
+       -  type is not a valid or supported stream type.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio-stop.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio-stop.rst
new file mode 100644
index 000000000..d77f786fd
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio-stop.rst
@@ -0,0 +1,48 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.audio
+
+.. _AUDIO_STOP:
+
+==========
+AUDIO_STOP
+==========
+
+Name
+----
+
+AUDIO_STOP
+
+.. attention:: This ioctl is deprecated
+
+Synopsis
+--------
+
+.. c:macro:: AUDIO_STOP
+
+``int ioctl(int fd, AUDIO_STOP)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This ioctl call asks the Audio Device to stop playing the current
+stream.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio.rst
new file mode 100644
index 000000000..aa753336b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio.rst
@@ -0,0 +1,27 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _dvb_audio:
+
+#######################
+Digital TV Audio Device
+#######################
+
+The Digital TV audio device controls the MPEG2 audio decoder of the Digital
+TV hardware. It can be accessed through ``/dev/dvb/adapter?/audio?``. Data
+types and ioctl definitions can be accessed by including
+``linux/dvb/audio.h`` in your application.
+
+Please note that some Digital TV cards don't have their own MPEG decoder, which
+results in the omission of the audio and video device.
+
+These ioctls were also used by V4L2 to control MPEG decoders implemented
+in V4L2. The use of these ioctls for that purpose has been made obsolete
+and proper V4L2 ioctls or controls have been created to replace that
+functionality.
+
+
+.. toctree::
+    :maxdepth: 1
+
+    audio_data_types
+    audio_function_calls
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio_data_types.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio_data_types.rst
new file mode 100644
index 000000000..474452913
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio_data_types.rst
@@ -0,0 +1,116 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _audio_data_types:
+
+****************
+Audio Data Types
+****************
+
+This section describes the structures, data types and defines used when
+talking to the audio device.
+
+.. c:type:: audio_stream_source
+
+The audio stream source is set through the AUDIO_SELECT_SOURCE call
+and can take the following values, depending on whether we are replaying
+from an internal (demux) or external (user write) source.
+
+
+.. code-block:: c
+
+    typedef enum {
+	AUDIO_SOURCE_DEMUX,
+	AUDIO_SOURCE_MEMORY
+    } audio_stream_source_t;
+
+AUDIO_SOURCE_DEMUX selects the demultiplexer (fed either by the
+frontend or the DVR device) as the source of the video stream. If
+AUDIO_SOURCE_MEMORY is selected the stream comes from the application
+through the ``write()`` system call.
+
+
+.. c:type:: audio_play_state
+
+The following values can be returned by the AUDIO_GET_STATUS call
+representing the state of audio playback.
+
+
+.. code-block:: c
+
+    typedef enum {
+	AUDIO_STOPPED,
+	AUDIO_PLAYING,
+	AUDIO_PAUSED
+    } audio_play_state_t;
+
+
+.. c:type:: audio_channel_select
+
+The audio channel selected via AUDIO_CHANNEL_SELECT is determined by
+the following values.
+
+
+.. code-block:: c
+
+    typedef enum {
+	AUDIO_STEREO,
+	AUDIO_MONO_LEFT,
+	AUDIO_MONO_RIGHT,
+	AUDIO_MONO,
+	AUDIO_STEREO_SWAPPED
+    } audio_channel_select_t;
+
+
+.. c:type:: audio_status
+
+The AUDIO_GET_STATUS call returns the following structure informing
+about various states of the playback operation.
+
+
+.. code-block:: c
+
+    typedef struct audio_status {
+	boolean AV_sync_state;
+	boolean mute_state;
+	audio_play_state_t play_state;
+	audio_stream_source_t stream_source;
+	audio_channel_select_t channel_select;
+	boolean bypass_mode;
+	audio_mixer_t mixer_state;
+    } audio_status_t;
+
+
+.. c:type:: audio_mixer
+
+The following structure is used by the AUDIO_SET_MIXER call to set the
+audio volume.
+
+
+.. code-block:: c
+
+    typedef struct audio_mixer {
+	unsigned int volume_left;
+	unsigned int volume_right;
+    } audio_mixer_t;
+
+
+.. _audio_encodings:
+
+audio encodings
+===============
+
+A call to AUDIO_GET_CAPABILITIES returns an unsigned integer with the
+following bits set according to the hardwares capabilities.
+
+
+.. code-block:: c
+
+     #define AUDIO_CAP_DTS    1
+     #define AUDIO_CAP_LPCM   2
+     #define AUDIO_CAP_MP1    4
+     #define AUDIO_CAP_MP2    8
+     #define AUDIO_CAP_MP3   16
+     #define AUDIO_CAP_AAC   32
+     #define AUDIO_CAP_OGG   64
+     #define AUDIO_CAP_SDDS 128
+     #define AUDIO_CAP_AC3  256
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/audio_function_calls.rst b/drivers/staging/media/deprecated/saa7146/av7110/audio_function_calls.rst
new file mode 100644
index 000000000..fa5ba9539
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/audio_function_calls.rst
@@ -0,0 +1,30 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _audio_function_calls:
+
+********************
+Audio Function Calls
+********************
+
+.. toctree::
+    :maxdepth: 1
+
+    audio-fopen
+    audio-fclose
+    audio-fwrite
+    audio-stop
+    audio-play
+    audio-pause
+    audio-continue
+    audio-select-source
+    audio-set-mute
+    audio-set-av-sync
+    audio-set-bypass-mode
+    audio-channel-select
+    audio-bilingual-channel-select
+    audio-get-status
+    audio-get-capabilities
+    audio-clear-buffer
+    audio-set-id
+    audio-set-mixer
+    audio-set-streamtype
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110.c
new file mode 100644
index 000000000..df81a9b74
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110.c
@@ -0,0 +1,2919 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * driver for the SAA7146 based AV110 cards (like the Fujitsu-Siemens DVB)
+ * av7110.c: initialization and demux stuff
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * originally based on code by:
+ * Copyright (C) 1998,1999 Christian Theiss <mistert@rz.fh-augsburg.de>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+
+#include <linux/module.h>
+#include <linux/kmod.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/poll.h>
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/string.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+#include <linux/crc32.h>
+#include <linux/i2c.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <asm/unaligned.h>
+#include <asm/byteorder.h>
+
+
+#include <linux/dvb/frontend.h>
+
+#include <media/dvb_frontend.h>
+
+#include "ttpci-eeprom.h"
+#include "av7110.h"
+#include "av7110_hw.h"
+#include "av7110_av.h"
+#include "av7110_ca.h"
+#include "av7110_ipack.h"
+
+#include "bsbe1.h"
+#include "lnbp21.h"
+#include "bsru6.h"
+
+#define TS_WIDTH  376
+#define TS_HEIGHT 512
+#define TS_BUFLEN (TS_WIDTH*TS_HEIGHT)
+#define TS_MAX_PACKETS (TS_BUFLEN/TS_SIZE)
+
+
+int av7110_debug;
+
+static int vidmode = CVBS_RGB_OUT;
+static int pids_off;
+static int adac = DVB_ADAC_TI;
+static int hw_sections;
+static int rgb_on;
+static int volume = 255;
+static int budgetpatch;
+static int wss_cfg_4_3 = 0x4008;
+static int wss_cfg_16_9 = 0x0007;
+static int tv_standard;
+static int full_ts;
+
+module_param_named(debug, av7110_debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (bitmask, default 0)");
+module_param(vidmode, int, 0444);
+MODULE_PARM_DESC(vidmode,"analog video out: 0 off, 1 CVBS+RGB (default), 2 CVBS+YC, 3 YC");
+module_param(pids_off, int, 0444);
+MODULE_PARM_DESC(pids_off,"clear video/audio/PCR PID filters when demux is closed");
+module_param(adac, int, 0444);
+MODULE_PARM_DESC(adac,"audio DAC type: 0 TI, 1 CRYSTAL, 2 MSP (use if autodetection fails)");
+module_param(hw_sections, int, 0444);
+MODULE_PARM_DESC(hw_sections, "0 use software section filter, 1 use hardware");
+module_param(rgb_on, int, 0444);
+MODULE_PARM_DESC(rgb_on, "For Siemens DVB-C cards only: Enable RGB control signal on SCART pin 16 to switch SCART video mode from CVBS to RGB");
+module_param(volume, int, 0444);
+MODULE_PARM_DESC(volume, "initial volume: default 255 (range 0-255)");
+module_param(budgetpatch, int, 0444);
+MODULE_PARM_DESC(budgetpatch, "use budget-patch hardware modification: default 0 (0 no, 1 autodetect, 2 always)");
+module_param(full_ts, int, 0444);
+MODULE_PARM_DESC(full_ts, "enable code for full-ts hardware modification: 0 disable (default), 1 enable");
+module_param(wss_cfg_4_3, int, 0444);
+MODULE_PARM_DESC(wss_cfg_4_3, "WSS 4:3 - default 0x4008 - bit 15: disable, 14: burst mode, 13..0: wss data");
+module_param(wss_cfg_16_9, int, 0444);
+MODULE_PARM_DESC(wss_cfg_16_9, "WSS 16:9 - default 0x0007 - bit 15: disable, 14: burst mode, 13..0: wss data");
+module_param(tv_standard, int, 0444);
+MODULE_PARM_DESC(tv_standard, "TV standard: 0 PAL (default), 1 NTSC");
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static void restart_feeds(struct av7110 *av7110);
+static int budget_start_feed(struct dvb_demux_feed *feed);
+static int budget_stop_feed(struct dvb_demux_feed *feed);
+
+static int av7110_num;
+
+#define FE_FUNC_OVERRIDE(fe_func, av7110_copy, av7110_func) \
+{\
+	if (fe_func != NULL) { \
+		av7110_copy = fe_func; \
+		fe_func = av7110_func; \
+	} \
+}
+
+
+static void init_av7110_av(struct av7110 *av7110)
+{
+	int ret;
+	struct saa7146_dev *dev = av7110->dev;
+
+	/* set internal volume control to maximum */
+	av7110->adac_type = DVB_ADAC_TI;
+	ret = av7110_set_volume(av7110, av7110->mixer.volume_left, av7110->mixer.volume_right);
+	if (ret < 0)
+		printk("dvb-ttpci:cannot set internal volume to maximum:%d\n",ret);
+
+	ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetMonitorType,
+			    1, (u16) av7110->display_ar);
+	if (ret < 0)
+		printk("dvb-ttpci: unable to set aspect ratio\n");
+	ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetPanScanType,
+			    1, av7110->display_panscan);
+	if (ret < 0)
+		printk("dvb-ttpci: unable to set pan scan\n");
+
+	ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetWSSConfig, 2, 2, wss_cfg_4_3);
+	if (ret < 0)
+		printk("dvb-ttpci: unable to configure 4:3 wss\n");
+	ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetWSSConfig, 2, 3, wss_cfg_16_9);
+	if (ret < 0)
+		printk("dvb-ttpci: unable to configure 16:9 wss\n");
+
+	ret = av7710_set_video_mode(av7110, vidmode);
+	if (ret < 0)
+		printk("dvb-ttpci:cannot set video mode:%d\n",ret);
+
+	/* handle different card types */
+	/* remaining inits according to card and frontend type */
+	av7110->analog_tuner_flags = 0;
+	av7110->current_input = 0;
+	if (dev->pci->subsystem_vendor == 0x13c2 && dev->pci->subsystem_device == 0x000a)
+		av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, ADSwitch, 1, 0); // SPDIF on
+	if (i2c_writereg(av7110, 0x20, 0x00, 0x00) == 1) {
+		printk ("dvb-ttpci: Crystal audio DAC @ card %d detected\n",
+			av7110->dvb_adapter.num);
+		av7110->adac_type = DVB_ADAC_CRYSTAL;
+		i2c_writereg(av7110, 0x20, 0x01, 0xd2);
+		i2c_writereg(av7110, 0x20, 0x02, 0x49);
+		i2c_writereg(av7110, 0x20, 0x03, 0x00);
+		i2c_writereg(av7110, 0x20, 0x04, 0x00);
+
+		/**
+		 * some special handling for the Siemens DVB-C cards...
+		 */
+	} else if (0 == av7110_init_analog_module(av7110)) {
+		/* done. */
+	}
+	else if (dev->pci->subsystem_vendor == 0x110a) {
+		printk("dvb-ttpci: DVB-C w/o analog module @ card %d detected\n",
+			av7110->dvb_adapter.num);
+		av7110->adac_type = DVB_ADAC_NONE;
+	}
+	else {
+		av7110->adac_type = adac;
+		printk("dvb-ttpci: adac type set to %d @ card %d\n",
+			av7110->adac_type, av7110->dvb_adapter.num);
+	}
+
+	if (av7110->adac_type == DVB_ADAC_NONE || av7110->adac_type == DVB_ADAC_MSP34x0) {
+		// switch DVB SCART on
+		ret = av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, MainSwitch, 1, 0);
+		if (ret < 0)
+			printk("dvb-ttpci:cannot switch on SCART(Main):%d\n",ret);
+		ret = av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, ADSwitch, 1, 1);
+		if (ret < 0)
+			printk("dvb-ttpci:cannot switch on SCART(AD):%d\n",ret);
+		if (rgb_on &&
+		    ((av7110->dev->pci->subsystem_vendor == 0x110a) ||
+		     (av7110->dev->pci->subsystem_vendor == 0x13c2)) &&
+		     (av7110->dev->pci->subsystem_device == 0x0000)) {
+			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI); // RGB on, SCART pin 16
+			//saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO); // SCARTpin 8
+		}
+	}
+
+	if (dev->pci->subsystem_vendor == 0x13c2 && dev->pci->subsystem_device == 0x000e)
+		av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, SpdifSwitch, 1, 0); // SPDIF on
+
+	ret = av7110_set_volume(av7110, av7110->mixer.volume_left, av7110->mixer.volume_right);
+	if (ret < 0)
+		printk("dvb-ttpci:cannot set volume :%d\n",ret);
+}
+
+static void recover_arm(struct av7110 *av7110)
+{
+	dprintk(4, "%p\n",av7110);
+
+	av7110_bootarm(av7110);
+	msleep(100);
+
+	init_av7110_av(av7110);
+
+	/* card-specific recovery */
+	if (av7110->recover)
+		av7110->recover(av7110);
+
+	restart_feeds(av7110);
+
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
+	av7110_set_ir_config(av7110);
+#endif
+}
+
+static void av7110_arm_sync(struct av7110 *av7110)
+{
+	if (av7110->arm_thread)
+		kthread_stop(av7110->arm_thread);
+
+	av7110->arm_thread = NULL;
+}
+
+static int arm_thread(void *data)
+{
+	struct av7110 *av7110 = data;
+	u16 newloops = 0;
+	int timeout;
+
+	dprintk(4, "%p\n",av7110);
+
+	for (;;) {
+		timeout = wait_event_interruptible_timeout(av7110->arm_wait,
+			kthread_should_stop(), 5 * HZ);
+
+		if (-ERESTARTSYS == timeout || kthread_should_stop()) {
+			/* got signal or told to quit*/
+			break;
+		}
+
+		if (!av7110->arm_ready)
+			continue;
+
+		if (mutex_lock_interruptible(&av7110->dcomlock))
+			break;
+		newloops = rdebi(av7110, DEBINOSWAP, STATUS_LOOPS, 0, 2);
+		mutex_unlock(&av7110->dcomlock);
+
+		if (newloops == av7110->arm_loops || av7110->arm_errors > 3) {
+			printk(KERN_ERR "dvb-ttpci: ARM crashed @ card %d\n",
+			       av7110->dvb_adapter.num);
+
+			recover_arm(av7110);
+
+			if (mutex_lock_interruptible(&av7110->dcomlock))
+				break;
+			newloops = rdebi(av7110, DEBINOSWAP, STATUS_LOOPS, 0, 2) - 1;
+			mutex_unlock(&av7110->dcomlock);
+		}
+		av7110->arm_loops = newloops;
+		av7110->arm_errors = 0;
+	}
+
+	return 0;
+}
+
+
+/****************************************************************************
+ * IRQ handling
+ ****************************************************************************/
+
+static int DvbDmxFilterCallback(u8 *buffer1, size_t buffer1_len,
+				u8 *buffer2, size_t buffer2_len,
+				struct dvb_demux_filter *dvbdmxfilter,
+				struct av7110 *av7110)
+{
+	if (!dvbdmxfilter->feed->demux->dmx.frontend)
+		return 0;
+	if (dvbdmxfilter->feed->demux->dmx.frontend->source == DMX_MEMORY_FE)
+		return 0;
+
+	switch (dvbdmxfilter->type) {
+	case DMX_TYPE_SEC:
+		if ((((buffer1[1] << 8) | buffer1[2]) & 0xfff) + 3 != buffer1_len)
+			return 0;
+		if (dvbdmxfilter->doneq) {
+			struct dmx_section_filter *filter = &dvbdmxfilter->filter;
+			int i;
+			u8 xor, neq = 0;
+
+			for (i = 0; i < DVB_DEMUX_MASK_MAX; i++) {
+				xor = filter->filter_value[i] ^ buffer1[i];
+				neq |= dvbdmxfilter->maskandnotmode[i] & xor;
+			}
+			if (!neq)
+				return 0;
+		}
+		return dvbdmxfilter->feed->cb.sec(buffer1, buffer1_len,
+						  buffer2, buffer2_len,
+						  &dvbdmxfilter->filter, NULL);
+	case DMX_TYPE_TS:
+		if (!(dvbdmxfilter->feed->ts_type & TS_PACKET))
+			return 0;
+		if (dvbdmxfilter->feed->ts_type & TS_PAYLOAD_ONLY)
+			return dvbdmxfilter->feed->cb.ts(buffer1, buffer1_len,
+							 buffer2, buffer2_len,
+							 &dvbdmxfilter->feed->feed.ts,
+							 NULL);
+		else
+			av7110_p2t_write(buffer1, buffer1_len,
+					 dvbdmxfilter->feed->pid,
+					 &av7110->p2t_filter[dvbdmxfilter->index]);
+		return 0;
+	default:
+		return 0;
+	}
+}
+
+
+//#define DEBUG_TIMING
+static inline void print_time(char *s)
+{
+#ifdef DEBUG_TIMING
+	struct timespec64 ts;
+	ktime_get_real_ts64(&ts);
+	printk("%s: %lld.%09ld\n", s, (s64)ts.tv_sec, ts.tv_nsec);
+#endif
+}
+
+#define DEBI_READ 0
+#define DEBI_WRITE 1
+static inline void start_debi_dma(struct av7110 *av7110, int dir,
+				  unsigned long addr, unsigned int len)
+{
+	dprintk(8, "%c %08lx %u\n", dir == DEBI_READ ? 'R' : 'W', addr, len);
+	if (saa7146_wait_for_debi_done(av7110->dev, 0)) {
+		printk(KERN_ERR "%s: saa7146_wait_for_debi_done timed out\n", __func__);
+		return;
+	}
+
+	SAA7146_ISR_CLEAR(av7110->dev, MASK_19); /* for good measure */
+	SAA7146_IER_ENABLE(av7110->dev, MASK_19);
+	if (len < 5)
+		len = 5; /* we want a real DEBI DMA */
+	if (dir == DEBI_WRITE)
+		iwdebi(av7110, DEBISWAB, addr, 0, (len + 3) & ~3);
+	else
+		irdebi(av7110, DEBISWAB, addr, 0, len);
+}
+
+static void debiirq(struct tasklet_struct *t)
+{
+	struct av7110 *av7110 = from_tasklet(av7110, t, debi_tasklet);
+	int type = av7110->debitype;
+	int handle = (type >> 8) & 0x1f;
+	unsigned int xfer = 0;
+
+	print_time("debi");
+	dprintk(4, "type 0x%04x\n", type);
+
+	if (type == -1) {
+		printk("DEBI irq oops @ %ld, psr:0x%08x, ssr:0x%08x\n",
+		       jiffies, saa7146_read(av7110->dev, PSR),
+		       saa7146_read(av7110->dev, SSR));
+		goto debi_done;
+	}
+	av7110->debitype = -1;
+
+	switch (type & 0xff) {
+
+	case DATA_TS_RECORD:
+		dvb_dmx_swfilter_packets(&av7110->demux,
+					 (const u8 *) av7110->debi_virt,
+					 av7110->debilen / 188);
+		xfer = RX_BUFF;
+		break;
+
+	case DATA_PES_RECORD:
+		if (av7110->demux.recording)
+			av7110_record_cb(&av7110->p2t[handle],
+					 (u8 *) av7110->debi_virt,
+					 av7110->debilen);
+		xfer = RX_BUFF;
+		break;
+
+	case DATA_IPMPE:
+	case DATA_FSECTION:
+	case DATA_PIPING:
+		if (av7110->handle2filter[handle])
+			DvbDmxFilterCallback((u8 *)av7110->debi_virt,
+					     av7110->debilen, NULL, 0,
+					     av7110->handle2filter[handle],
+					     av7110);
+		xfer = RX_BUFF;
+		break;
+
+	case DATA_CI_GET:
+	{
+		u8 *data = av7110->debi_virt;
+		u8 data_0 = data[0];
+
+		if (data_0 < 2 && data[2] == 0xff) {
+			int flags = 0;
+			if (data[5] > 0)
+				flags |= CA_CI_MODULE_PRESENT;
+			if (data[5] > 5)
+				flags |= CA_CI_MODULE_READY;
+			av7110->ci_slot[data_0].flags = flags;
+		} else
+			ci_get_data(&av7110->ci_rbuffer,
+				    av7110->debi_virt,
+				    av7110->debilen);
+		xfer = RX_BUFF;
+		break;
+	}
+
+	case DATA_COMMON_INTERFACE:
+		CI_handle(av7110, (u8 *)av7110->debi_virt, av7110->debilen);
+		xfer = RX_BUFF;
+		break;
+
+	case DATA_DEBUG_MESSAGE:
+		((s8*)av7110->debi_virt)[Reserved_SIZE - 1] = 0;
+		printk("%s\n", (s8 *) av7110->debi_virt);
+		xfer = RX_BUFF;
+		break;
+
+	case DATA_CI_PUT:
+		dprintk(4, "debi DATA_CI_PUT\n");
+		xfer = TX_BUFF;
+		break;
+	case DATA_MPEG_PLAY:
+		dprintk(4, "debi DATA_MPEG_PLAY\n");
+		xfer = TX_BUFF;
+		break;
+	case DATA_BMP_LOAD:
+		dprintk(4, "debi DATA_BMP_LOAD\n");
+		xfer = TX_BUFF;
+		break;
+	default:
+		break;
+	}
+debi_done:
+	spin_lock(&av7110->debilock);
+	if (xfer)
+		iwdebi(av7110, DEBINOSWAP, xfer, 0, 2);
+	ARM_ClearMailBox(av7110);
+	spin_unlock(&av7110->debilock);
+}
+
+/* irq from av7110 firmware writing the mailbox register in the DPRAM */
+static void gpioirq(struct tasklet_struct *t)
+{
+	struct av7110 *av7110 = from_tasklet(av7110, t, gpio_tasklet);
+	u32 rxbuf, txbuf;
+	int len;
+
+	if (av7110->debitype != -1)
+		/* we shouldn't get any irq while a debi xfer is running */
+		printk("dvb-ttpci: GPIO0 irq oops @ %ld, psr:0x%08x, ssr:0x%08x\n",
+		       jiffies, saa7146_read(av7110->dev, PSR),
+		       saa7146_read(av7110->dev, SSR));
+
+	if (saa7146_wait_for_debi_done(av7110->dev, 0)) {
+		printk(KERN_ERR "%s: saa7146_wait_for_debi_done timed out\n", __func__);
+		BUG(); /* maybe we should try resetting the debi? */
+	}
+
+	spin_lock(&av7110->debilock);
+	ARM_ClearIrq(av7110);
+
+	/* see what the av7110 wants */
+	av7110->debitype = irdebi(av7110, DEBINOSWAP, IRQ_STATE, 0, 2);
+	av7110->debilen  = irdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+	rxbuf = irdebi(av7110, DEBINOSWAP, RX_BUFF, 0, 2);
+	txbuf = irdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+	len = (av7110->debilen + 3) & ~3;
+
+	print_time("gpio");
+	dprintk(8, "GPIO0 irq 0x%04x %d\n", av7110->debitype, av7110->debilen);
+
+	switch (av7110->debitype & 0xff) {
+
+	case DATA_TS_PLAY:
+	case DATA_PES_PLAY:
+		break;
+
+	case DATA_MPEG_VIDEO_EVENT:
+	{
+		u32 h_ar;
+		struct video_event event;
+
+		av7110->video_size.w = irdebi(av7110, DEBINOSWAP, STATUS_MPEG_WIDTH, 0, 2);
+		h_ar = irdebi(av7110, DEBINOSWAP, STATUS_MPEG_HEIGHT_AR, 0, 2);
+
+		iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+		iwdebi(av7110, DEBINOSWAP, RX_BUFF, 0, 2);
+
+		av7110->video_size.h = h_ar & 0xfff;
+
+		event.type = VIDEO_EVENT_SIZE_CHANGED;
+		event.u.size.w = av7110->video_size.w;
+		event.u.size.h = av7110->video_size.h;
+		switch ((h_ar >> 12) & 0xf)
+		{
+		case 3:
+			av7110->video_size.aspect_ratio = VIDEO_FORMAT_16_9;
+			event.u.size.aspect_ratio = VIDEO_FORMAT_16_9;
+			av7110->videostate.video_format = VIDEO_FORMAT_16_9;
+			break;
+		case 4:
+			av7110->video_size.aspect_ratio = VIDEO_FORMAT_221_1;
+			event.u.size.aspect_ratio = VIDEO_FORMAT_221_1;
+			av7110->videostate.video_format = VIDEO_FORMAT_221_1;
+			break;
+		default:
+			av7110->video_size.aspect_ratio = VIDEO_FORMAT_4_3;
+			event.u.size.aspect_ratio = VIDEO_FORMAT_4_3;
+			av7110->videostate.video_format = VIDEO_FORMAT_4_3;
+		}
+
+		dprintk(8, "GPIO0 irq: DATA_MPEG_VIDEO_EVENT: w/h/ar = %u/%u/%u\n",
+			av7110->video_size.w, av7110->video_size.h,
+			av7110->video_size.aspect_ratio);
+
+		dvb_video_add_event(av7110, &event);
+		break;
+	}
+
+	case DATA_CI_PUT:
+	{
+		int avail;
+		struct dvb_ringbuffer *cibuf = &av7110->ci_wbuffer;
+
+		avail = dvb_ringbuffer_avail(cibuf);
+		if (avail <= 2) {
+			iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_LEN, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+			break;
+		}
+		len = DVB_RINGBUFFER_PEEK(cibuf, 0) << 8;
+		len |= DVB_RINGBUFFER_PEEK(cibuf, 1);
+		if (avail < len + 2) {
+			iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_LEN, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+			break;
+		}
+		DVB_RINGBUFFER_SKIP(cibuf, 2);
+
+		dvb_ringbuffer_read(cibuf, av7110->debi_virt, len);
+
+		iwdebi(av7110, DEBINOSWAP, TX_LEN, len, 2);
+		iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, len, 2);
+		dprintk(8, "DMA: CI\n");
+		start_debi_dma(av7110, DEBI_WRITE, DPRAM_BASE + txbuf, len);
+		spin_unlock(&av7110->debilock);
+		wake_up(&cibuf->queue);
+		return;
+	}
+
+	case DATA_MPEG_PLAY:
+		if (!av7110->playing) {
+			iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_LEN, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+			break;
+		}
+		len = 0;
+		if (av7110->debitype & 0x100) {
+			spin_lock(&av7110->aout.lock);
+			len = av7110_pes_play(av7110->debi_virt, &av7110->aout, 2048);
+			spin_unlock(&av7110->aout.lock);
+		}
+		if (len <= 0 && (av7110->debitype & 0x200)
+		    &&av7110->videostate.play_state != VIDEO_FREEZED) {
+			spin_lock(&av7110->avout.lock);
+			len = av7110_pes_play(av7110->debi_virt, &av7110->avout, 2048);
+			spin_unlock(&av7110->avout.lock);
+		}
+		if (len <= 0) {
+			iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_LEN, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+			break;
+		}
+		dprintk(8, "GPIO0 PES_PLAY len=%04x\n", len);
+		iwdebi(av7110, DEBINOSWAP, TX_LEN, len, 2);
+		iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, len, 2);
+		dprintk(8, "DMA: MPEG_PLAY\n");
+		start_debi_dma(av7110, DEBI_WRITE, DPRAM_BASE + txbuf, len);
+		spin_unlock(&av7110->debilock);
+		return;
+
+	case DATA_BMP_LOAD:
+		len = av7110->debilen;
+		dprintk(8, "gpio DATA_BMP_LOAD len %d\n", len);
+		if (!len) {
+			av7110->bmp_state = BMP_LOADED;
+			iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_LEN, 0, 2);
+			iwdebi(av7110, DEBINOSWAP, TX_BUFF, 0, 2);
+			wake_up(&av7110->bmpq);
+			dprintk(8, "gpio DATA_BMP_LOAD done\n");
+			break;
+		}
+		if (len > av7110->bmplen)
+			len = av7110->bmplen;
+		if (len > 2 * 1024)
+			len = 2 * 1024;
+		iwdebi(av7110, DEBINOSWAP, TX_LEN, len, 2);
+		iwdebi(av7110, DEBINOSWAP, IRQ_STATE_EXT, len, 2);
+		memcpy(av7110->debi_virt, av7110->bmpbuf+av7110->bmpp, len);
+		av7110->bmpp += len;
+		av7110->bmplen -= len;
+		dprintk(8, "gpio DATA_BMP_LOAD DMA len %d\n", len);
+		start_debi_dma(av7110, DEBI_WRITE, DPRAM_BASE+txbuf, len);
+		spin_unlock(&av7110->debilock);
+		return;
+
+	case DATA_CI_GET:
+	case DATA_COMMON_INTERFACE:
+	case DATA_FSECTION:
+	case DATA_IPMPE:
+	case DATA_PIPING:
+		if (!len || len > 4 * 1024) {
+			iwdebi(av7110, DEBINOSWAP, RX_BUFF, 0, 2);
+			break;
+		}
+		fallthrough;
+
+	case DATA_TS_RECORD:
+	case DATA_PES_RECORD:
+		dprintk(8, "DMA: TS_REC etc.\n");
+		start_debi_dma(av7110, DEBI_READ, DPRAM_BASE+rxbuf, len);
+		spin_unlock(&av7110->debilock);
+		return;
+
+	case DATA_DEBUG_MESSAGE:
+		if (!len || len > 0xff) {
+			iwdebi(av7110, DEBINOSWAP, RX_BUFF, 0, 2);
+			break;
+		}
+		start_debi_dma(av7110, DEBI_READ, Reserved, len);
+		spin_unlock(&av7110->debilock);
+		return;
+
+	case DATA_IRCOMMAND:
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
+		av7110_ir_handler(av7110,
+				  swahw32(irdebi(av7110, DEBINOSWAP, Reserved,
+						 0, 4)));
+#endif
+		iwdebi(av7110, DEBINOSWAP, RX_BUFF, 0, 2);
+		break;
+
+	default:
+		printk("dvb-ttpci: gpioirq unknown type=%d len=%d\n",
+		       av7110->debitype, av7110->debilen);
+		break;
+	}
+	av7110->debitype = -1;
+	ARM_ClearMailBox(av7110);
+	spin_unlock(&av7110->debilock);
+}
+
+
+#ifdef CONFIG_DVB_AV7110_OSD
+static int dvb_osd_ioctl(struct file *file,
+			 unsigned int cmd, void *parg)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (cmd == OSD_SEND_CMD)
+		return av7110_osd_cmd(av7110, (osd_cmd_t *) parg);
+	if (cmd == OSD_GET_CAPABILITY)
+		return av7110_osd_capability(av7110, (osd_cap_t *) parg);
+
+	return -EINVAL;
+}
+
+
+static const struct file_operations dvb_osd_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= dvb_generic_ioctl,
+	.open		= dvb_generic_open,
+	.release	= dvb_generic_release,
+	.llseek		= noop_llseek,
+};
+
+static struct dvb_device dvbdev_osd = {
+	.priv		= NULL,
+	.users		= 1,
+	.writers	= 1,
+	.fops		= &dvb_osd_fops,
+	.kernel_ioctl	= dvb_osd_ioctl,
+};
+#endif /* CONFIG_DVB_AV7110_OSD */
+
+
+static inline int SetPIDs(struct av7110 *av7110, u16 vpid, u16 apid, u16 ttpid,
+			  u16 subpid, u16 pcrpid)
+{
+	u16 aflags = 0;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (vpid == 0x1fff || apid == 0x1fff ||
+	    ttpid == 0x1fff || subpid == 0x1fff || pcrpid == 0x1fff) {
+		vpid = apid = ttpid = subpid = pcrpid = 0;
+		av7110->pids[DMX_PES_VIDEO] = 0;
+		av7110->pids[DMX_PES_AUDIO] = 0;
+		av7110->pids[DMX_PES_TELETEXT] = 0;
+		av7110->pids[DMX_PES_PCR] = 0;
+	}
+
+	if (av7110->audiostate.bypass_mode)
+		aflags |= 0x8000;
+
+	return av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, MultiPID, 6,
+			     pcrpid, vpid, apid, ttpid, subpid, aflags);
+}
+
+int ChangePIDs(struct av7110 *av7110, u16 vpid, u16 apid, u16 ttpid,
+		u16 subpid, u16 pcrpid)
+{
+	int ret = 0;
+	dprintk(4, "%p\n", av7110);
+
+	if (mutex_lock_interruptible(&av7110->pid_mutex))
+		return -ERESTARTSYS;
+
+	if (!(vpid & 0x8000))
+		av7110->pids[DMX_PES_VIDEO] = vpid;
+	if (!(apid & 0x8000))
+		av7110->pids[DMX_PES_AUDIO] = apid;
+	if (!(ttpid & 0x8000))
+		av7110->pids[DMX_PES_TELETEXT] = ttpid;
+	if (!(pcrpid & 0x8000))
+		av7110->pids[DMX_PES_PCR] = pcrpid;
+
+	av7110->pids[DMX_PES_SUBTITLE] = 0;
+
+	if (av7110->fe_synced) {
+		pcrpid = av7110->pids[DMX_PES_PCR];
+		ret = SetPIDs(av7110, vpid, apid, ttpid, subpid, pcrpid);
+	}
+
+	mutex_unlock(&av7110->pid_mutex);
+	return ret;
+}
+
+
+/******************************************************************************
+ * hardware filter functions
+ ******************************************************************************/
+
+static int StartHWFilter(struct dvb_demux_filter *dvbdmxfilter)
+{
+	struct dvb_demux_feed *dvbdmxfeed = dvbdmxfilter->feed;
+	struct av7110 *av7110 = dvbdmxfeed->demux->priv;
+	u16 buf[20];
+	int ret, i;
+	u16 handle;
+//	u16 mode = 0x0320;
+	u16 mode = 0xb96a;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (av7110->full_ts)
+		return 0;
+
+	if (dvbdmxfilter->type == DMX_TYPE_SEC) {
+		if (hw_sections) {
+			buf[4] = (dvbdmxfilter->filter.filter_value[0] << 8) |
+				dvbdmxfilter->maskandmode[0];
+			for (i = 3; i < 18; i++)
+				buf[i + 4 - 2] =
+					(dvbdmxfilter->filter.filter_value[i] << 8) |
+					dvbdmxfilter->maskandmode[i];
+			mode = 4;
+		}
+	} else if ((dvbdmxfeed->ts_type & TS_PACKET) &&
+		   !(dvbdmxfeed->ts_type & TS_PAYLOAD_ONLY)) {
+		av7110_p2t_init(&av7110->p2t_filter[dvbdmxfilter->index], dvbdmxfeed);
+	}
+
+	buf[0] = (COMTYPE_PID_FILTER << 8) + AddPIDFilter;
+	buf[1] = 16;
+	buf[2] = dvbdmxfeed->pid;
+	buf[3] = mode;
+
+	ret = av7110_fw_request(av7110, buf, 20, &handle, 1);
+	if (ret != 0 || handle >= 32) {
+		printk(KERN_ERR "dvb-ttpci: %s error  buf %04x %04x %04x %04x  ret %d  handle %04x\n",
+				__func__, buf[0], buf[1], buf[2], buf[3],
+				ret, handle);
+		dvbdmxfilter->hw_handle = 0xffff;
+		if (!ret)
+			ret = -1;
+		return ret;
+	}
+
+	av7110->handle2filter[handle] = dvbdmxfilter;
+	dvbdmxfilter->hw_handle = handle;
+
+	return ret;
+}
+
+static int StopHWFilter(struct dvb_demux_filter *dvbdmxfilter)
+{
+	struct av7110 *av7110 = dvbdmxfilter->feed->demux->priv;
+	u16 buf[3];
+	u16 answ[2];
+	int ret;
+	u16 handle;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (av7110->full_ts)
+		return 0;
+
+	handle = dvbdmxfilter->hw_handle;
+	if (handle >= 32) {
+		printk("%s tried to stop invalid filter %04x, filter type = %x\n",
+				__func__, handle, dvbdmxfilter->type);
+		return -EINVAL;
+	}
+
+	av7110->handle2filter[handle] = NULL;
+
+	buf[0] = (COMTYPE_PID_FILTER << 8) + DelPIDFilter;
+	buf[1] = 1;
+	buf[2] = handle;
+	ret = av7110_fw_request(av7110, buf, 3, answ, 2);
+	if (ret != 0 || answ[1] != handle) {
+		printk(KERN_ERR "dvb-ttpci: %s error  cmd %04x %04x %04x  ret %x  resp %04x %04x  pid %d\n",
+				__func__, buf[0], buf[1], buf[2], ret,
+				answ[0], answ[1], dvbdmxfilter->feed->pid);
+		if (!ret)
+			ret = -1;
+	}
+	return ret;
+}
+
+
+static int dvb_feed_start_pid(struct dvb_demux_feed *dvbdmxfeed)
+{
+	struct dvb_demux *dvbdmx = dvbdmxfeed->demux;
+	struct av7110 *av7110 = dvbdmx->priv;
+	u16 *pid = dvbdmx->pids, npids[5];
+	int i;
+	int ret = 0;
+
+	dprintk(4, "%p\n", av7110);
+
+	npids[0] = npids[1] = npids[2] = npids[3] = npids[4] = 0xffff;
+	i = dvbdmxfeed->pes_type;
+	npids[i] = (pid[i]&0x8000) ? 0 : pid[i];
+	if ((i == 2) && npids[i] && (dvbdmxfeed->ts_type & TS_PACKET)) {
+		npids[i] = 0;
+		ret = ChangePIDs(av7110, npids[1], npids[0], npids[2], npids[3], npids[4]);
+		if (!ret)
+			ret = StartHWFilter(dvbdmxfeed->filter);
+		return ret;
+	}
+	if (dvbdmxfeed->pes_type <= 2 || dvbdmxfeed->pes_type == 4) {
+		ret = ChangePIDs(av7110, npids[1], npids[0], npids[2], npids[3], npids[4]);
+		if (ret)
+			return ret;
+	}
+
+	if (dvbdmxfeed->pes_type < 2 && npids[0])
+		if (av7110->fe_synced)
+		{
+			ret = av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, Scan, 0);
+			if (ret)
+				return ret;
+		}
+
+	if ((dvbdmxfeed->ts_type & TS_PACKET) && !av7110->full_ts) {
+		if (dvbdmxfeed->pes_type == 0 && !(dvbdmx->pids[0] & 0x8000))
+			ret = av7110_av_start_record(av7110, RP_AUDIO, dvbdmxfeed);
+		if (dvbdmxfeed->pes_type == 1 && !(dvbdmx->pids[1] & 0x8000))
+			ret = av7110_av_start_record(av7110, RP_VIDEO, dvbdmxfeed);
+	}
+	return ret;
+}
+
+static int dvb_feed_stop_pid(struct dvb_demux_feed *dvbdmxfeed)
+{
+	struct dvb_demux *dvbdmx = dvbdmxfeed->demux;
+	struct av7110 *av7110 = dvbdmx->priv;
+	u16 *pid = dvbdmx->pids, npids[5];
+	int i;
+
+	int ret = 0;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (dvbdmxfeed->pes_type <= 1) {
+		ret = av7110_av_stop(av7110, dvbdmxfeed->pes_type ?  RP_VIDEO : RP_AUDIO);
+		if (ret)
+			return ret;
+		if (!av7110->rec_mode)
+			dvbdmx->recording = 0;
+		if (!av7110->playing)
+			dvbdmx->playing = 0;
+	}
+	npids[0] = npids[1] = npids[2] = npids[3] = npids[4] = 0xffff;
+	i = dvbdmxfeed->pes_type;
+	switch (i) {
+	case 2: //teletext
+		if (dvbdmxfeed->ts_type & TS_PACKET)
+			ret = StopHWFilter(dvbdmxfeed->filter);
+		npids[2] = 0;
+		break;
+	case 0:
+	case 1:
+	case 4:
+		if (!pids_off)
+			return 0;
+		npids[i] = (pid[i]&0x8000) ? 0 : pid[i];
+		break;
+	}
+	if (!ret)
+		ret = ChangePIDs(av7110, npids[1], npids[0], npids[2], npids[3], npids[4]);
+	return ret;
+}
+
+static int av7110_start_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct av7110 *av7110 = demux->priv;
+	int ret = 0;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (!demux->dmx.frontend)
+		return -EINVAL;
+
+	if (!av7110->full_ts && feed->pid > 0x1fff)
+		return -EINVAL;
+
+	if (feed->type == DMX_TYPE_TS) {
+		if ((feed->ts_type & TS_DECODER) &&
+		    (feed->pes_type <= DMX_PES_PCR)) {
+			switch (demux->dmx.frontend->source) {
+			case DMX_MEMORY_FE:
+				if (feed->ts_type & TS_DECODER)
+				       if (feed->pes_type < 2 &&
+					   !(demux->pids[0] & 0x8000) &&
+					   !(demux->pids[1] & 0x8000)) {
+					       dvb_ringbuffer_flush_spinlock_wakeup(&av7110->avout);
+					       dvb_ringbuffer_flush_spinlock_wakeup(&av7110->aout);
+					       ret = av7110_av_start_play(av7110,RP_AV);
+					       if (!ret)
+						       demux->playing = 1;
+					}
+				break;
+			default:
+				ret = dvb_feed_start_pid(feed);
+				break;
+			}
+		} else if ((feed->ts_type & TS_PACKET) &&
+			   (demux->dmx.frontend->source != DMX_MEMORY_FE)) {
+			ret = StartHWFilter(feed->filter);
+		}
+	}
+
+	if (av7110->full_ts) {
+		budget_start_feed(feed);
+		return ret;
+	}
+
+	if (feed->type == DMX_TYPE_SEC) {
+		int i;
+
+		for (i = 0; i < demux->filternum; i++) {
+			if (demux->filter[i].state != DMX_STATE_READY)
+				continue;
+			if (demux->filter[i].type != DMX_TYPE_SEC)
+				continue;
+			if (demux->filter[i].filter.parent != &feed->feed.sec)
+				continue;
+			demux->filter[i].state = DMX_STATE_GO;
+			if (demux->dmx.frontend->source != DMX_MEMORY_FE) {
+				ret = StartHWFilter(&demux->filter[i]);
+				if (ret)
+					break;
+			}
+		}
+	}
+
+	return ret;
+}
+
+
+static int av7110_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct av7110 *av7110 = demux->priv;
+	int i, rc, ret = 0;
+	dprintk(4, "%p\n", av7110);
+
+	if (feed->type == DMX_TYPE_TS) {
+		if (feed->ts_type & TS_DECODER) {
+			if (feed->pes_type >= DMX_PES_OTHER ||
+			    !demux->pesfilter[feed->pes_type])
+				return -EINVAL;
+			demux->pids[feed->pes_type] |= 0x8000;
+			demux->pesfilter[feed->pes_type] = NULL;
+		}
+		if (feed->ts_type & TS_DECODER &&
+		    feed->pes_type < DMX_PES_OTHER) {
+			ret = dvb_feed_stop_pid(feed);
+		} else
+			if ((feed->ts_type & TS_PACKET) &&
+			    (demux->dmx.frontend->source != DMX_MEMORY_FE))
+				ret = StopHWFilter(feed->filter);
+	}
+
+	if (av7110->full_ts) {
+		budget_stop_feed(feed);
+		return ret;
+	}
+
+	if (feed->type == DMX_TYPE_SEC) {
+		for (i = 0; i<demux->filternum; i++) {
+			if (demux->filter[i].state == DMX_STATE_GO &&
+			    demux->filter[i].filter.parent == &feed->feed.sec) {
+				demux->filter[i].state = DMX_STATE_READY;
+				if (demux->dmx.frontend->source != DMX_MEMORY_FE) {
+					rc = StopHWFilter(&demux->filter[i]);
+					if (!ret)
+						ret = rc;
+					/* keep going, stop as many filters as possible */
+				}
+			}
+		}
+	}
+
+	return ret;
+}
+
+
+static void restart_feeds(struct av7110 *av7110)
+{
+	struct dvb_demux *dvbdmx = &av7110->demux;
+	struct dvb_demux_feed *feed;
+	int mode;
+	int feeding;
+	int i, j;
+
+	dprintk(4, "%p\n", av7110);
+
+	mode = av7110->playing;
+	av7110->playing = 0;
+	av7110->rec_mode = 0;
+
+	feeding = av7110->feeding1; /* full_ts mod */
+
+	for (i = 0; i < dvbdmx->feednum; i++) {
+		feed = &dvbdmx->feed[i];
+		if (feed->state == DMX_STATE_GO) {
+			if (feed->type == DMX_TYPE_SEC) {
+				for (j = 0; j < dvbdmx->filternum; j++) {
+					if (dvbdmx->filter[j].type != DMX_TYPE_SEC)
+						continue;
+					if (dvbdmx->filter[j].filter.parent != &feed->feed.sec)
+						continue;
+					if (dvbdmx->filter[j].state == DMX_STATE_GO)
+						dvbdmx->filter[j].state = DMX_STATE_READY;
+				}
+			}
+			av7110_start_feed(feed);
+		}
+	}
+
+	av7110->feeding1 = feeding; /* full_ts mod */
+
+	if (mode)
+		av7110_av_start_play(av7110, mode);
+}
+
+static int dvb_get_stc(struct dmx_demux *demux, unsigned int num,
+		       uint64_t *stc, unsigned int *base)
+{
+	int ret;
+	u16 fwstc[4];
+	u16 tag = ((COMTYPE_REQUEST << 8) + ReqSTC);
+	struct dvb_demux *dvbdemux;
+	struct av7110 *av7110;
+
+	/* pointer casting paranoia... */
+	BUG_ON(!demux);
+	dvbdemux = demux->priv;
+	BUG_ON(!dvbdemux);
+	av7110 = dvbdemux->priv;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (num != 0)
+		return -EINVAL;
+
+	ret = av7110_fw_request(av7110, &tag, 0, fwstc, 4);
+	if (ret) {
+		printk(KERN_ERR "%s: av7110_fw_request error\n", __func__);
+		return ret;
+	}
+	dprintk(2, "fwstc = %04hx %04hx %04hx %04hx\n",
+		fwstc[0], fwstc[1], fwstc[2], fwstc[3]);
+
+	*stc =	(((uint64_t) ((fwstc[3] & 0x8000) >> 15)) << 32) |
+		(((uint64_t)  fwstc[1]) << 16) | ((uint64_t) fwstc[0]);
+	*base = 1;
+
+	dprintk(4, "stc = %lu\n", (unsigned long)*stc);
+
+	return 0;
+}
+
+
+/******************************************************************************
+ * SEC device file operations
+ ******************************************************************************/
+
+
+static int av7110_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		return Set22K(av7110, 1);
+
+	case SEC_TONE_OFF:
+		return Set22K(av7110, 0);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int av7110_diseqc_send_master_cmd(struct dvb_frontend* fe,
+					 struct dvb_diseqc_master_cmd* cmd)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	return av7110_diseqc_send(av7110, cmd->msg_len, cmd->msg, -1);
+}
+
+static int av7110_diseqc_send_burst(struct dvb_frontend* fe,
+				    enum fe_sec_mini_cmd minicmd)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	return av7110_diseqc_send(av7110, 0, NULL, minicmd);
+}
+
+/* simplified code from budget-core.c */
+static int stop_ts_capture(struct av7110 *budget)
+{
+	dprintk(2, "budget: %p\n", budget);
+
+	if (--budget->feeding1)
+		return budget->feeding1;
+	saa7146_write(budget->dev, MC1, MASK_20);	/* DMA3 off */
+	SAA7146_IER_DISABLE(budget->dev, MASK_10);
+	SAA7146_ISR_CLEAR(budget->dev, MASK_10);
+	return 0;
+}
+
+static int start_ts_capture(struct av7110 *budget)
+{
+	unsigned y;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	if (budget->feeding1)
+		return ++budget->feeding1;
+	for (y = 0; y < TS_HEIGHT; y++)
+		memset(budget->grabbing + y * TS_WIDTH, 0x00, TS_WIDTH);
+	budget->ttbp = 0;
+	SAA7146_ISR_CLEAR(budget->dev, MASK_10);  /* VPE */
+	SAA7146_IER_ENABLE(budget->dev, MASK_10); /* VPE */
+	saa7146_write(budget->dev, MC1, (MASK_04 | MASK_20)); /* DMA3 on */
+	return ++budget->feeding1;
+}
+
+static int budget_start_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct av7110 *budget = demux->priv;
+	int status;
+
+	dprintk(2, "av7110: %p\n", budget);
+
+	spin_lock(&budget->feedlock1);
+	feed->pusi_seen = false; /* have a clean section start */
+	status = start_ts_capture(budget);
+	spin_unlock(&budget->feedlock1);
+	return status;
+}
+
+static int budget_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct av7110 *budget = demux->priv;
+	int status;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	spin_lock(&budget->feedlock1);
+	status = stop_ts_capture(budget);
+	spin_unlock(&budget->feedlock1);
+	return status;
+}
+
+static void vpeirq(struct tasklet_struct *t)
+{
+	struct av7110 *budget = from_tasklet(budget, t, vpe_tasklet);
+	u8 *mem = (u8 *) (budget->grabbing);
+	u32 olddma = budget->ttbp;
+	u32 newdma = saa7146_read(budget->dev, PCI_VDP3);
+	struct dvb_demux *demux = budget->full_ts ? &budget->demux : &budget->demux1;
+
+	/* nearest lower position divisible by 188 */
+	newdma -= newdma % 188;
+
+	if (newdma >= TS_BUFLEN)
+		return;
+
+	budget->ttbp = newdma;
+
+	if (!budget->feeding1 || (newdma == olddma))
+		return;
+
+	/* Ensure streamed PCI data is synced to CPU */
+	dma_sync_sg_for_cpu(&budget->dev->pci->dev, budget->pt.slist,
+			    budget->pt.nents, DMA_FROM_DEVICE);
+
+#if 0
+	/* track rps1 activity */
+	printk("vpeirq: %02x Event Counter 1 0x%04x\n",
+	       mem[olddma],
+	       saa7146_read(budget->dev, EC1R) & 0x3fff);
+#endif
+
+	if (newdma > olddma)
+		/* no wraparound, dump olddma..newdma */
+		dvb_dmx_swfilter_packets(demux, mem + olddma, (newdma - olddma) / 188);
+	else {
+		/* wraparound, dump olddma..buflen and 0..newdma */
+		dvb_dmx_swfilter_packets(demux, mem + olddma, (TS_BUFLEN - olddma) / 188);
+		dvb_dmx_swfilter_packets(demux, mem, newdma / 188);
+	}
+}
+
+static int av7110_register(struct av7110 *av7110)
+{
+	int ret, i;
+	struct dvb_demux *dvbdemux = &av7110->demux;
+	struct dvb_demux *dvbdemux1 = &av7110->demux1;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (av7110->registered)
+		return -1;
+
+	av7110->registered = 1;
+
+	dvbdemux->priv = (void *) av7110;
+
+	for (i = 0; i < 32; i++)
+		av7110->handle2filter[i] = NULL;
+
+	dvbdemux->filternum = (av7110->full_ts) ? 256 : 32;
+	dvbdemux->feednum = (av7110->full_ts) ? 256 : 32;
+	dvbdemux->start_feed = av7110_start_feed;
+	dvbdemux->stop_feed = av7110_stop_feed;
+	dvbdemux->write_to_decoder = av7110_write_to_decoder;
+	dvbdemux->dmx.capabilities = (DMX_TS_FILTERING | DMX_SECTION_FILTERING |
+				      DMX_MEMORY_BASED_FILTERING);
+
+	dvb_dmx_init(&av7110->demux);
+	av7110->demux.dmx.get_stc = dvb_get_stc;
+
+	av7110->dmxdev.filternum = (av7110->full_ts) ? 256 : 32;
+	av7110->dmxdev.demux = &dvbdemux->dmx;
+	av7110->dmxdev.capabilities = 0;
+
+	dvb_dmxdev_init(&av7110->dmxdev, &av7110->dvb_adapter);
+
+	av7110->hw_frontend.source = DMX_FRONTEND_0;
+
+	ret = dvbdemux->dmx.add_frontend(&dvbdemux->dmx, &av7110->hw_frontend);
+
+	if (ret < 0)
+		return ret;
+
+	av7110->mem_frontend.source = DMX_MEMORY_FE;
+
+	ret = dvbdemux->dmx.add_frontend(&dvbdemux->dmx, &av7110->mem_frontend);
+
+	if (ret < 0)
+		return ret;
+
+	ret = dvbdemux->dmx.connect_frontend(&dvbdemux->dmx,
+					     &av7110->hw_frontend);
+	if (ret < 0)
+		return ret;
+
+	av7110_av_register(av7110);
+	av7110_ca_register(av7110);
+
+#ifdef CONFIG_DVB_AV7110_OSD
+	dvb_register_device(&av7110->dvb_adapter, &av7110->osd_dev,
+			    &dvbdev_osd, av7110, DVB_DEVICE_OSD, 0);
+#endif
+
+	dvb_net_init(&av7110->dvb_adapter, &av7110->dvb_net, &dvbdemux->dmx);
+
+	if (budgetpatch) {
+		/* initialize software demux1 without its own frontend
+		 * demux1 hardware is connected to frontend0 of demux0
+		 */
+		dvbdemux1->priv = (void *) av7110;
+
+		dvbdemux1->filternum = 256;
+		dvbdemux1->feednum = 256;
+		dvbdemux1->start_feed = budget_start_feed;
+		dvbdemux1->stop_feed = budget_stop_feed;
+		dvbdemux1->write_to_decoder = NULL;
+
+		dvbdemux1->dmx.capabilities = (DMX_TS_FILTERING | DMX_SECTION_FILTERING |
+					       DMX_MEMORY_BASED_FILTERING);
+
+		dvb_dmx_init(&av7110->demux1);
+
+		av7110->dmxdev1.filternum = 256;
+		av7110->dmxdev1.demux = &dvbdemux1->dmx;
+		av7110->dmxdev1.capabilities = 0;
+
+		dvb_dmxdev_init(&av7110->dmxdev1, &av7110->dvb_adapter);
+
+		dvb_net_init(&av7110->dvb_adapter, &av7110->dvb_net1, &dvbdemux1->dmx);
+		printk("dvb-ttpci: additional demux1 for budget-patch registered\n");
+	}
+	return 0;
+}
+
+
+static void dvb_unregister(struct av7110 *av7110)
+{
+	struct dvb_demux *dvbdemux = &av7110->demux;
+	struct dvb_demux *dvbdemux1 = &av7110->demux1;
+
+	dprintk(4, "%p\n", av7110);
+
+	if (!av7110->registered)
+		return;
+
+	if (budgetpatch) {
+		dvb_net_release(&av7110->dvb_net1);
+		dvbdemux->dmx.close(&dvbdemux1->dmx);
+		dvb_dmxdev_release(&av7110->dmxdev1);
+		dvb_dmx_release(&av7110->demux1);
+	}
+
+	dvb_net_release(&av7110->dvb_net);
+
+	dvbdemux->dmx.close(&dvbdemux->dmx);
+	dvbdemux->dmx.remove_frontend(&dvbdemux->dmx, &av7110->hw_frontend);
+	dvbdemux->dmx.remove_frontend(&dvbdemux->dmx, &av7110->mem_frontend);
+
+	dvb_dmxdev_release(&av7110->dmxdev);
+	dvb_dmx_release(&av7110->demux);
+
+	if (av7110->fe != NULL) {
+		dvb_unregister_frontend(av7110->fe);
+		dvb_frontend_detach(av7110->fe);
+	}
+	dvb_unregister_device(av7110->osd_dev);
+	av7110_av_unregister(av7110);
+	av7110_ca_unregister(av7110);
+}
+
+
+/****************************************************************************
+ * I2C client commands
+ ****************************************************************************/
+
+int i2c_writereg(struct av7110 *av7110, u8 id, u8 reg, u8 val)
+{
+	u8 msg[2] = { reg, val };
+	struct i2c_msg msgs;
+
+	msgs.flags = 0;
+	msgs.addr = id / 2;
+	msgs.len = 2;
+	msgs.buf = msg;
+	return i2c_transfer(&av7110->i2c_adap, &msgs, 1);
+}
+
+u8 i2c_readreg(struct av7110 *av7110, u8 id, u8 reg)
+{
+	u8 mm1[] = {0x00};
+	u8 mm2[] = {0x00};
+	struct i2c_msg msgs[2];
+
+	msgs[0].flags = 0;
+	msgs[1].flags = I2C_M_RD;
+	msgs[0].addr = msgs[1].addr = id / 2;
+	mm1[0] = reg;
+	msgs[0].len = 1; msgs[1].len = 1;
+	msgs[0].buf = mm1; msgs[1].buf = mm2;
+	i2c_transfer(&av7110->i2c_adap, msgs, 2);
+
+	return mm2[0];
+}
+
+/****************************************************************************
+ * INITIALIZATION
+ ****************************************************************************/
+
+
+static int check_firmware(struct av7110* av7110)
+{
+	u32 crc = 0, len = 0;
+	unsigned char *ptr;
+
+	/* check for firmware magic */
+	ptr = av7110->bin_fw;
+	if (ptr[0] != 'A' || ptr[1] != 'V' ||
+	    ptr[2] != 'F' || ptr[3] != 'W') {
+		printk("dvb-ttpci: this is not an av7110 firmware\n");
+		return -EINVAL;
+	}
+	ptr += 4;
+
+	/* check dpram file */
+	crc = get_unaligned_be32(ptr);
+	ptr += 4;
+	len = get_unaligned_be32(ptr);
+	ptr += 4;
+	if (len >= 512) {
+		printk("dvb-ttpci: dpram file is way too big.\n");
+		return -EINVAL;
+	}
+	if (crc != crc32_le(0, ptr, len)) {
+		printk("dvb-ttpci: crc32 of dpram file does not match.\n");
+		return -EINVAL;
+	}
+	av7110->bin_dpram = ptr;
+	av7110->size_dpram = len;
+	ptr += len;
+
+	/* check root file */
+	crc = get_unaligned_be32(ptr);
+	ptr += 4;
+	len = get_unaligned_be32(ptr);
+	ptr += 4;
+
+	if (len <= 200000 || len >= 300000 ||
+	    len > ((av7110->bin_fw + av7110->size_fw) - ptr)) {
+		printk("dvb-ttpci: root file has strange size (%d). aborting.\n", len);
+		return -EINVAL;
+	}
+	if( crc != crc32_le(0, ptr, len)) {
+		printk("dvb-ttpci: crc32 of root file does not match.\n");
+		return -EINVAL;
+	}
+	av7110->bin_root = ptr;
+	av7110->size_root = len;
+	return 0;
+}
+
+static void put_firmware(struct av7110* av7110)
+{
+	vfree(av7110->bin_fw);
+}
+
+static int get_firmware(struct av7110* av7110)
+{
+	int ret;
+	const struct firmware *fw;
+
+	/* request the av7110 firmware, this will block until someone uploads it */
+	ret = request_firmware(&fw, "dvb-ttpci-01.fw", &av7110->dev->pci->dev);
+	if (ret) {
+		if (ret == -ENOENT) {
+			printk(KERN_ERR "dvb-ttpci: could not load firmware, file not found: dvb-ttpci-01.fw\n");
+			printk(KERN_ERR "dvb-ttpci: usually this should be in /usr/lib/hotplug/firmware or /lib/firmware\n");
+			printk(KERN_ERR "dvb-ttpci: and can be downloaded from https://linuxtv.org/download/dvb/firmware/\n");
+		} else
+			printk(KERN_ERR "dvb-ttpci: cannot request firmware (error %i)\n",
+			       ret);
+		return -EINVAL;
+	}
+
+	if (fw->size <= 200000) {
+		printk("dvb-ttpci: this firmware is way too small.\n");
+		release_firmware(fw);
+		return -EINVAL;
+	}
+
+	/* check if the firmware is available */
+	av7110->bin_fw = vmalloc(fw->size);
+	if (NULL == av7110->bin_fw) {
+		dprintk(1, "out of memory\n");
+		release_firmware(fw);
+		return -ENOMEM;
+	}
+
+	memcpy(av7110->bin_fw, fw->data, fw->size);
+	av7110->size_fw = fw->size;
+	if ((ret = check_firmware(av7110)))
+		vfree(av7110->bin_fw);
+
+	release_firmware(fw);
+	return ret;
+}
+
+static int alps_bsrv2_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u8 pwr = 0;
+	u8 buf[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = buf, .len = sizeof(buf) };
+	u32 div = (p->frequency + 479500) / 125;
+
+	if (p->frequency > 2000000)
+		pwr = 3;
+	else if (p->frequency > 1800000)
+		pwr = 2;
+	else if (p->frequency > 1600000)
+		pwr = 1;
+	else if (p->frequency > 1200000)
+		pwr = 0;
+	else if (p->frequency >= 1100000)
+		pwr = 1;
+	else
+		pwr = 2;
+
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = ((div & 0x18000) >> 10) | 0x95;
+	buf[3] = (pwr << 6) | 0x30;
+
+	// NOTE: since we're using a prescaler of 2, we set the
+	// divisor frequency to 62.5kHz and divide by 125 above
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&av7110->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct ves1x93_config alps_bsrv2_config = {
+	.demod_address = 0x08,
+	.xin = 90100000UL,
+	.invert_pwm = 0,
+};
+
+static int alps_tdbe2_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x62, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = (p->frequency + 35937500 + 31250) / 62500;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x85 | ((div >> 10) & 0x60);
+	data[3] = (p->frequency < 174000000 ? 0x88 : p->frequency < 470000000 ? 0x84 : 0x81);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&av7110->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct ves1820_config alps_tdbe2_config = {
+	.demod_address = 0x09,
+	.xin = 57840000UL,
+	.invert = 1,
+	.selagc = VES1820_SELAGC_SIGNAMPERR,
+};
+
+
+
+
+static int grundig_29504_451_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = p->frequency / 125;
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x8e;
+	data[3] = 0x00;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&av7110->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct tda8083_config grundig_29504_451_config = {
+	.demod_address = 0x68,
+};
+
+
+
+static int philips_cd1516_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div;
+	u32 f = p->frequency;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = (f + 36125000 + 31250) / 62500;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x8e;
+	data[3] = (f < 174000000 ? 0xa1 : f < 470000000 ? 0x92 : 0x34);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&av7110->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct ves1820_config philips_cd1516_config = {
+	.demod_address = 0x09,
+	.xin = 57840000UL,
+	.invert = 1,
+	.selagc = VES1820_SELAGC_SIGNAMPERR,
+};
+
+
+
+static int alps_tdlb7_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div, pwr;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x60, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = (p->frequency + 36200000) / 166666;
+
+	if (p->frequency <= 782000000)
+		pwr = 1;
+	else
+		pwr = 2;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x85;
+	data[3] = pwr << 6;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&av7110->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int alps_tdlb7_request_firmware(struct dvb_frontend* fe, const struct firmware **fw, char* name)
+{
+#if IS_ENABLED(CONFIG_DVB_SP8870)
+	struct av7110* av7110 = fe->dvb->priv;
+
+	return request_firmware(fw, name, &av7110->dev->pci->dev);
+#else
+	return -EINVAL;
+#endif
+}
+
+static const struct sp8870_config alps_tdlb7_config = {
+
+	.demod_address = 0x71,
+	.request_firmware = alps_tdlb7_request_firmware,
+};
+
+
+static u8 nexusca_stv0297_inittab[] = {
+	0x80, 0x01,
+	0x80, 0x00,
+	0x81, 0x01,
+	0x81, 0x00,
+	0x00, 0x09,
+	0x01, 0x69,
+	0x03, 0x00,
+	0x04, 0x00,
+	0x07, 0x00,
+	0x08, 0x00,
+	0x20, 0x00,
+	0x21, 0x40,
+	0x22, 0x00,
+	0x23, 0x00,
+	0x24, 0x40,
+	0x25, 0x88,
+	0x30, 0xff,
+	0x31, 0x00,
+	0x32, 0xff,
+	0x33, 0x00,
+	0x34, 0x50,
+	0x35, 0x7f,
+	0x36, 0x00,
+	0x37, 0x20,
+	0x38, 0x00,
+	0x40, 0x1c,
+	0x41, 0xff,
+	0x42, 0x29,
+	0x43, 0x00,
+	0x44, 0xff,
+	0x45, 0x00,
+	0x46, 0x00,
+	0x49, 0x04,
+	0x4a, 0x00,
+	0x4b, 0x7b,
+	0x52, 0x30,
+	0x55, 0xae,
+	0x56, 0x47,
+	0x57, 0xe1,
+	0x58, 0x3a,
+	0x5a, 0x1e,
+	0x5b, 0x34,
+	0x60, 0x00,
+	0x63, 0x00,
+	0x64, 0x00,
+	0x65, 0x00,
+	0x66, 0x00,
+	0x67, 0x00,
+	0x68, 0x00,
+	0x69, 0x00,
+	0x6a, 0x02,
+	0x6b, 0x00,
+	0x70, 0xff,
+	0x71, 0x00,
+	0x72, 0x00,
+	0x73, 0x00,
+	0x74, 0x0c,
+	0x80, 0x00,
+	0x81, 0x00,
+	0x82, 0x00,
+	0x83, 0x00,
+	0x84, 0x04,
+	0x85, 0x80,
+	0x86, 0x24,
+	0x87, 0x78,
+	0x88, 0x10,
+	0x89, 0x00,
+	0x90, 0x01,
+	0x91, 0x01,
+	0xa0, 0x04,
+	0xa1, 0x00,
+	0xa2, 0x00,
+	0xb0, 0x91,
+	0xb1, 0x0b,
+	0xc0, 0x53,
+	0xc1, 0x70,
+	0xc2, 0x12,
+	0xd0, 0x00,
+	0xd1, 0x00,
+	0xd2, 0x00,
+	0xd3, 0x00,
+	0xd4, 0x00,
+	0xd5, 0x00,
+	0xde, 0x00,
+	0xdf, 0x00,
+	0x61, 0x49,
+	0x62, 0x0b,
+	0x53, 0x08,
+	0x59, 0x08,
+	0xff, 0xff,
+};
+
+static int nexusca_stv0297_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x63, .flags = 0, .buf = data, .len = sizeof(data) };
+	struct i2c_msg readmsg = { .addr = 0x63, .flags = I2C_M_RD, .buf = data, .len = 1 };
+	int i;
+
+	div = (p->frequency + 36150000 + 31250) / 62500;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0xce;
+
+	if (p->frequency < 45000000)
+		return -EINVAL;
+	else if (p->frequency < 137000000)
+		data[3] = 0x01;
+	else if (p->frequency < 403000000)
+		data[3] = 0x02;
+	else if (p->frequency < 860000000)
+		data[3] = 0x04;
+	else
+		return -EINVAL;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&av7110->i2c_adap, &msg, 1) != 1) {
+		printk("nexusca: pll transfer failed!\n");
+		return -EIO;
+	}
+
+	// wait for PLL lock
+	for(i = 0; i < 20; i++) {
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 1);
+		if (i2c_transfer(&av7110->i2c_adap, &readmsg, 1) == 1)
+			if (data[0] & 0x40) break;
+		msleep(10);
+	}
+
+	return 0;
+}
+
+static struct stv0297_config nexusca_stv0297_config = {
+
+	.demod_address = 0x1C,
+	.inittab = nexusca_stv0297_inittab,
+	.invert = 1,
+	.stop_during_read = 1,
+};
+
+
+
+static int grundig_29504_401_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct av7110* av7110 = fe->dvb->priv;
+	u32 div;
+	u8 cfg, cpump, band_select;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = (36125000 + p->frequency) / 166666;
+
+	cfg = 0x88;
+
+	if (p->frequency < 175000000)
+		cpump = 2;
+	else if (p->frequency < 390000000)
+		cpump = 1;
+	else if (p->frequency < 470000000)
+		cpump = 2;
+	else if (p->frequency < 750000000)
+		cpump = 1;
+	else
+		cpump = 3;
+
+	if (p->frequency < 175000000)
+		band_select = 0x0e;
+	else if (p->frequency < 470000000)
+		band_select = 0x05;
+	else
+		band_select = 0x03;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = ((div >> 10) & 0x60) | cfg;
+	data[3] = (cpump << 6) | band_select;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&av7110->i2c_adap, &msg, 1) != 1) return -EIO;
+	return 0;
+}
+
+static struct l64781_config grundig_29504_401_config = {
+	.demod_address = 0x55,
+};
+
+
+
+static int av7110_fe_lock_fix(struct av7110 *av7110, enum fe_status status)
+{
+	int ret = 0;
+	int synced = (status & FE_HAS_LOCK) ? 1 : 0;
+
+	av7110->fe_status = status;
+
+	if (av7110->fe_synced == synced)
+		return 0;
+
+	if (av7110->playing) {
+		av7110->fe_synced = synced;
+		return 0;
+	}
+
+	if (mutex_lock_interruptible(&av7110->pid_mutex))
+		return -ERESTARTSYS;
+
+	if (synced) {
+		ret = SetPIDs(av7110, av7110->pids[DMX_PES_VIDEO],
+			av7110->pids[DMX_PES_AUDIO],
+			av7110->pids[DMX_PES_TELETEXT], 0,
+			av7110->pids[DMX_PES_PCR]);
+		if (!ret)
+			ret = av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, Scan, 0);
+	} else {
+		ret = SetPIDs(av7110, 0, 0, 0, 0, 0);
+		if (!ret) {
+			ret = av7110_fw_cmd(av7110, COMTYPE_PID_FILTER, FlushTSQueue, 0);
+			if (!ret)
+				ret = av7110_wait_msgstate(av7110, GPMQBusy);
+		}
+	}
+
+	if (!ret)
+		av7110->fe_synced = synced;
+
+	mutex_unlock(&av7110->pid_mutex);
+	return ret;
+}
+
+static int av7110_fe_set_frontend(struct dvb_frontend *fe)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret)
+		ret = av7110->fe_set_frontend(fe);
+
+	return ret;
+}
+
+static int av7110_fe_init(struct dvb_frontend* fe)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret)
+		ret = av7110->fe_init(fe);
+	return ret;
+}
+
+static int av7110_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	/* call the real implementation */
+	int ret = av7110->fe_read_status(fe, status);
+	if (!ret)
+		if (((*status ^ av7110->fe_status) & FE_HAS_LOCK) && (*status & FE_HAS_LOCK))
+			ret = av7110_fe_lock_fix(av7110, *status);
+	return ret;
+}
+
+static int av7110_fe_diseqc_reset_overload(struct dvb_frontend* fe)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret)
+		ret = av7110->fe_diseqc_reset_overload(fe);
+	return ret;
+}
+
+static int av7110_fe_diseqc_send_master_cmd(struct dvb_frontend* fe,
+					    struct dvb_diseqc_master_cmd* cmd)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret) {
+		av7110->saved_master_cmd = *cmd;
+		ret = av7110->fe_diseqc_send_master_cmd(fe, cmd);
+	}
+	return ret;
+}
+
+static int av7110_fe_diseqc_send_burst(struct dvb_frontend *fe,
+				       enum fe_sec_mini_cmd minicmd)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret) {
+		av7110->saved_minicmd = minicmd;
+		ret = av7110->fe_diseqc_send_burst(fe, minicmd);
+	}
+	return ret;
+}
+
+static int av7110_fe_set_tone(struct dvb_frontend *fe,
+			      enum fe_sec_tone_mode tone)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret) {
+		av7110->saved_tone = tone;
+		ret = av7110->fe_set_tone(fe, tone);
+	}
+	return ret;
+}
+
+static int av7110_fe_set_voltage(struct dvb_frontend *fe,
+				 enum fe_sec_voltage voltage)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret) {
+		av7110->saved_voltage = voltage;
+		ret = av7110->fe_set_voltage(fe, voltage);
+	}
+	return ret;
+}
+
+static int av7110_fe_dishnetwork_send_legacy_command(struct dvb_frontend* fe, unsigned long cmd)
+{
+	struct av7110* av7110 = fe->dvb->priv;
+
+	int ret = av7110_fe_lock_fix(av7110, 0);
+	if (!ret)
+		ret = av7110->fe_dishnetwork_send_legacy_command(fe, cmd);
+	return ret;
+}
+
+static void dvb_s_recover(struct av7110* av7110)
+{
+	av7110_fe_init(av7110->fe);
+
+	av7110_fe_set_voltage(av7110->fe, av7110->saved_voltage);
+	if (av7110->saved_master_cmd.msg_len) {
+		msleep(20);
+		av7110_fe_diseqc_send_master_cmd(av7110->fe, &av7110->saved_master_cmd);
+	}
+	msleep(20);
+	av7110_fe_diseqc_send_burst(av7110->fe, av7110->saved_minicmd);
+	msleep(20);
+	av7110_fe_set_tone(av7110->fe, av7110->saved_tone);
+
+	av7110_fe_set_frontend(av7110->fe);
+}
+
+static u8 read_pwm(struct av7110* av7110)
+{
+	u8 b = 0xff;
+	u8 pwm;
+	struct i2c_msg msg[] = { { .addr = 0x50,.flags = 0,.buf = &b,.len = 1 },
+				 { .addr = 0x50,.flags = I2C_M_RD,.buf = &pwm,.len = 1} };
+
+	if ((i2c_transfer(&av7110->i2c_adap, msg, 2) != 2) || (pwm == 0xff))
+		pwm = 0x48;
+
+	return pwm;
+}
+
+static int frontend_init(struct av7110 *av7110)
+{
+	int ret;
+
+	if (av7110->dev->pci->subsystem_vendor == 0x110a) {
+		switch(av7110->dev->pci->subsystem_device) {
+		case 0x0000: // Fujitsu/Siemens DVB-Cable (ves1820/Philips CD1516(??))
+			av7110->fe = dvb_attach(ves1820_attach, &philips_cd1516_config,
+						    &av7110->i2c_adap, read_pwm(av7110));
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = philips_cd1516_tuner_set_params;
+			}
+			break;
+		}
+
+	} else if (av7110->dev->pci->subsystem_vendor == 0x13c2) {
+		switch(av7110->dev->pci->subsystem_device) {
+		case 0x0000: // Hauppauge/TT WinTV DVB-S rev1.X
+		case 0x0003: // Hauppauge/TT WinTV Nexus-S Rev 2.X
+		case 0x1002: // Hauppauge/TT WinTV DVB-S rev1.3SE
+
+			// try the ALPS BSRV2 first of all
+			av7110->fe = dvb_attach(ves1x93_attach, &alps_bsrv2_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = alps_bsrv2_tuner_set_params;
+				av7110->fe->ops.diseqc_send_master_cmd = av7110_diseqc_send_master_cmd;
+				av7110->fe->ops.diseqc_send_burst = av7110_diseqc_send_burst;
+				av7110->fe->ops.set_tone = av7110_set_tone;
+				av7110->recover = dvb_s_recover;
+				break;
+			}
+
+			// try the ALPS BSRU6 now
+			av7110->fe = dvb_attach(stv0299_attach, &alps_bsru6_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+				av7110->fe->tuner_priv = &av7110->i2c_adap;
+
+				av7110->fe->ops.diseqc_send_master_cmd = av7110_diseqc_send_master_cmd;
+				av7110->fe->ops.diseqc_send_burst = av7110_diseqc_send_burst;
+				av7110->fe->ops.set_tone = av7110_set_tone;
+				av7110->recover = dvb_s_recover;
+				break;
+			}
+
+			// Try the grundig 29504-451
+			av7110->fe = dvb_attach(tda8083_attach, &grundig_29504_451_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = grundig_29504_451_tuner_set_params;
+				av7110->fe->ops.diseqc_send_master_cmd = av7110_diseqc_send_master_cmd;
+				av7110->fe->ops.diseqc_send_burst = av7110_diseqc_send_burst;
+				av7110->fe->ops.set_tone = av7110_set_tone;
+				av7110->recover = dvb_s_recover;
+				break;
+			}
+
+			/* Try DVB-C cards */
+			switch(av7110->dev->pci->subsystem_device) {
+			case 0x0000:
+				/* Siemens DVB-C (full-length card) VES1820/Philips CD1516 */
+				av7110->fe = dvb_attach(ves1820_attach, &philips_cd1516_config, &av7110->i2c_adap,
+							read_pwm(av7110));
+				if (av7110->fe) {
+					av7110->fe->ops.tuner_ops.set_params = philips_cd1516_tuner_set_params;
+				}
+				break;
+			case 0x0003:
+				/* Hauppauge DVB-C 2.1 VES1820/ALPS TDBE2 */
+				av7110->fe = dvb_attach(ves1820_attach, &alps_tdbe2_config, &av7110->i2c_adap,
+							read_pwm(av7110));
+				if (av7110->fe) {
+					av7110->fe->ops.tuner_ops.set_params = alps_tdbe2_tuner_set_params;
+				}
+				break;
+			}
+			break;
+
+		case 0x0001: // Hauppauge/TT Nexus-T premium rev1.X
+		{
+			struct dvb_frontend *fe;
+
+			// try ALPS TDLB7 first, then Grundig 29504-401
+			fe = dvb_attach(sp8870_attach, &alps_tdlb7_config, &av7110->i2c_adap);
+			if (fe) {
+				fe->ops.tuner_ops.set_params = alps_tdlb7_tuner_set_params;
+				av7110->fe = fe;
+				break;
+			}
+		}
+			fallthrough;
+
+		case 0x0008: // Hauppauge/TT DVB-T
+			// Grundig 29504-401
+			av7110->fe = dvb_attach(l64781_attach, &grundig_29504_401_config, &av7110->i2c_adap);
+			if (av7110->fe)
+				av7110->fe->ops.tuner_ops.set_params = grundig_29504_401_tuner_set_params;
+			break;
+
+		case 0x0002: // Hauppauge/TT DVB-C premium rev2.X
+
+			av7110->fe = dvb_attach(ves1820_attach, &alps_tdbe2_config, &av7110->i2c_adap, read_pwm(av7110));
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = alps_tdbe2_tuner_set_params;
+			}
+			break;
+
+		case 0x0004: // Galaxis DVB-S rev1.3
+			/* ALPS BSRV2 */
+			av7110->fe = dvb_attach(ves1x93_attach, &alps_bsrv2_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = alps_bsrv2_tuner_set_params;
+				av7110->fe->ops.diseqc_send_master_cmd = av7110_diseqc_send_master_cmd;
+				av7110->fe->ops.diseqc_send_burst = av7110_diseqc_send_burst;
+				av7110->fe->ops.set_tone = av7110_set_tone;
+				av7110->recover = dvb_s_recover;
+			}
+			break;
+
+		case 0x0006: /* Fujitsu-Siemens DVB-S rev 1.6 */
+			/* Grundig 29504-451 */
+			av7110->fe = dvb_attach(tda8083_attach, &grundig_29504_451_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = grundig_29504_451_tuner_set_params;
+				av7110->fe->ops.diseqc_send_master_cmd = av7110_diseqc_send_master_cmd;
+				av7110->fe->ops.diseqc_send_burst = av7110_diseqc_send_burst;
+				av7110->fe->ops.set_tone = av7110_set_tone;
+				av7110->recover = dvb_s_recover;
+			}
+			break;
+
+		case 0x000A: // Hauppauge/TT Nexus-CA rev1.X
+
+			av7110->fe = dvb_attach(stv0297_attach, &nexusca_stv0297_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = nexusca_stv0297_tuner_set_params;
+
+				/* set TDA9819 into DVB mode */
+				saa7146_setgpio(av7110->dev, 1, SAA7146_GPIO_OUTLO); // TDA9819 pin9(STD)
+				saa7146_setgpio(av7110->dev, 3, SAA7146_GPIO_OUTLO); // TDA9819 pin30(VIF)
+
+				/* tuner on this needs a slower i2c bus speed */
+				av7110->dev->i2c_bitrate = SAA7146_I2C_BUS_BIT_RATE_240;
+				break;
+			}
+			break;
+
+		case 0x000E: /* Hauppauge/TT Nexus-S rev 2.3 */
+			/* ALPS BSBE1 */
+			av7110->fe = dvb_attach(stv0299_attach, &alps_bsbe1_config, &av7110->i2c_adap);
+			if (av7110->fe) {
+				av7110->fe->ops.tuner_ops.set_params = alps_bsbe1_tuner_set_params;
+				av7110->fe->tuner_priv = &av7110->i2c_adap;
+
+				if (dvb_attach(lnbp21_attach, av7110->fe, &av7110->i2c_adap, 0, 0) == NULL) {
+					printk("dvb-ttpci: LNBP21 not found!\n");
+					if (av7110->fe->ops.release)
+						av7110->fe->ops.release(av7110->fe);
+					av7110->fe = NULL;
+				} else {
+					av7110->fe->ops.dishnetwork_send_legacy_command = NULL;
+					av7110->recover = dvb_s_recover;
+				}
+			}
+			break;
+		}
+	}
+
+	if (!av7110->fe) {
+		/* FIXME: propagate the failure code from the lower layers */
+		ret = -ENOMEM;
+		printk("dvb-ttpci: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		       av7110->dev->pci->vendor,
+		       av7110->dev->pci->device,
+		       av7110->dev->pci->subsystem_vendor,
+		       av7110->dev->pci->subsystem_device);
+	} else {
+		FE_FUNC_OVERRIDE(av7110->fe->ops.init, av7110->fe_init, av7110_fe_init);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.read_status, av7110->fe_read_status, av7110_fe_read_status);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.diseqc_reset_overload, av7110->fe_diseqc_reset_overload, av7110_fe_diseqc_reset_overload);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.diseqc_send_master_cmd, av7110->fe_diseqc_send_master_cmd, av7110_fe_diseqc_send_master_cmd);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.diseqc_send_burst, av7110->fe_diseqc_send_burst, av7110_fe_diseqc_send_burst);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.set_tone, av7110->fe_set_tone, av7110_fe_set_tone);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.set_voltage, av7110->fe_set_voltage, av7110_fe_set_voltage);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.dishnetwork_send_legacy_command, av7110->fe_dishnetwork_send_legacy_command, av7110_fe_dishnetwork_send_legacy_command);
+		FE_FUNC_OVERRIDE(av7110->fe->ops.set_frontend, av7110->fe_set_frontend, av7110_fe_set_frontend);
+
+		ret = dvb_register_frontend(&av7110->dvb_adapter, av7110->fe);
+		if (ret < 0) {
+			printk("av7110: Frontend registration failed!\n");
+			dvb_frontend_detach(av7110->fe);
+			av7110->fe = NULL;
+		}
+	}
+	return ret;
+}
+
+/* Budgetpatch note:
+ * Original hardware design by Roberto Deza:
+ * There is a DVB_Wiki at
+ * https://linuxtv.org
+ *
+ * New software triggering design by Emard that works on
+ * original Roberto Deza's hardware:
+ *
+ * rps1 code for budgetpatch will copy internal HS event to GPIO3 pin.
+ * GPIO3 is in budget-patch hardware connectd to port B VSYNC
+ * HS is an internal event of 7146, accessible with RPS
+ * and temporarily raised high every n lines
+ * (n in defined in the RPS_THRESH1 counter threshold)
+ * I think HS is raised high on the beginning of the n-th line
+ * and remains high until this n-th line that triggered
+ * it is completely received. When the reception of n-th line
+ * ends, HS is lowered.
+ *
+ * To transmit data over DMA, 7146 needs changing state at
+ * port B VSYNC pin. Any changing of port B VSYNC will
+ * cause some DMA data transfer, with more or less packets loss.
+ * It depends on the phase and frequency of VSYNC and
+ * the way of 7146 is instructed to trigger on port B (defined
+ * in DD1_INIT register, 3rd nibble from the right valid
+ * numbers are 0-7, see datasheet)
+ *
+ * The correct triggering can minimize packet loss,
+ * dvbtraffic should give this stable bandwidths:
+ *   22k transponder = 33814 kbit/s
+ * 27.5k transponder = 38045 kbit/s
+ * by experiment it is found that the best results
+ * (stable bandwidths and almost no packet loss)
+ * are obtained using DD1_INIT triggering number 2
+ * (Va at rising edge of VS Fa = HS x VS-failing forced toggle)
+ * and a VSYNC phase that occurs in the middle of DMA transfer
+ * (about byte 188*512=96256 in the DMA window).
+ *
+ * Phase of HS is still not clear to me how to control,
+ * It just happens to be so. It can be seen if one enables
+ * RPS_IRQ and print Event Counter 1 in vpeirq(). Every
+ * time RPS_INTERRUPT is called, the Event Counter 1 will
+ * increment. That's how the 7146 is programmed to do event
+ * counting in this budget-patch.c
+ * I *think* HPS setting has something to do with the phase
+ * of HS but I can't be 100% sure in that.
+ *
+ * hardware debug note: a working budget card (including budget patch)
+ * with vpeirq() interrupt setup in mode "0x90" (every 64K) will
+ * generate 3 interrupts per 25-Hz DMA frame of 2*188*512 bytes
+ * and that means 3*25=75 Hz of interrupt frequency, as seen by
+ * watch cat /proc/interrupts
+ *
+ * If this frequency is 3x lower (and data received in the DMA
+ * buffer don't start with 0x47, but in the middle of packets,
+ * whose lengths appear to be like 188 292 188 104 etc.
+ * this means VSYNC line is not connected in the hardware.
+ * (check soldering pcb and pins)
+ * The same behaviour of missing VSYNC can be duplicated on budget
+ * cards, by setting DD1_INIT trigger mode 7 in 3rd nibble.
+ */
+static int av7110_attach(struct saa7146_dev* dev,
+			 struct saa7146_pci_extension_data *pci_ext)
+{
+	const int length = TS_WIDTH * TS_HEIGHT;
+	struct pci_dev *pdev = dev->pci;
+	struct av7110 *av7110;
+	struct task_struct *thread;
+	int ret, count = 0;
+
+	dprintk(4, "dev: %p\n", dev);
+
+	/* Set RPS_IRQ to 1 to track rps1 activity.
+	 * Enabling this won't send any interrupt to PC CPU.
+	 */
+#define RPS_IRQ 0
+
+	if (budgetpatch == 1) {
+		budgetpatch = 0;
+		/* autodetect the presence of budget patch
+		 * this only works if saa7146 has been recently
+		 * reset with MASK_31 to MC1
+		 *
+		 * will wait for VBI_B event (vertical blank at port B)
+		 * and will reset GPIO3 after VBI_B is detected.
+		 * (GPIO3 should be raised high by CPU to
+		 * test if GPIO3 will generate vertical blank signal
+		 * in budget patch GPIO3 is connected to VSYNC_B
+		 */
+
+		/* RESET SAA7146 */
+		saa7146_write(dev, MC1, MASK_31);
+		/* autodetection success seems to be time-dependend after reset */
+
+		/* Fix VSYNC level */
+		saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+		/* set vsync_b triggering */
+		saa7146_write(dev, DD1_STREAM_B, 0);
+		/* port B VSYNC at rising edge */
+		saa7146_write(dev, DD1_INIT, 0x00000200);
+		saa7146_write(dev, BRS_CTRL, 0x00000000);  // VBI
+		saa7146_write(dev, MC2,
+			      1 * (MASK_08 | MASK_24)  |   // BRS control
+			      0 * (MASK_09 | MASK_25)  |   // a
+			      1 * (MASK_10 | MASK_26)  |   // b
+			      0 * (MASK_06 | MASK_22)  |   // HPS_CTRL1
+			      0 * (MASK_05 | MASK_21)  |   // HPS_CTRL2
+			      0 * (MASK_01 | MASK_15)      // DEBI
+		);
+
+		/* start writing RPS1 code from beginning */
+		count = 0;
+		/* Disable RPS1 */
+		saa7146_write(dev, MC1, MASK_29);
+		/* RPS1 timeout disable */
+		saa7146_write(dev, RPS_TOV1, 0);
+		WRITE_RPS1(CMD_PAUSE | EVT_VBI_B);
+		WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+		WRITE_RPS1(GPIO3_MSK);
+		WRITE_RPS1(SAA7146_GPIO_OUTLO<<24);
+#if RPS_IRQ
+		/* issue RPS1 interrupt to increment counter */
+		WRITE_RPS1(CMD_INTERRUPT);
+#endif
+		WRITE_RPS1(CMD_STOP);
+		/* Jump to begin of RPS program as safety measure               (p37) */
+		WRITE_RPS1(CMD_JUMP);
+		WRITE_RPS1(dev->d_rps1.dma_handle);
+
+#if RPS_IRQ
+		/* set event counter 1 source as RPS1 interrupt (0x03)          (rE4 p53)
+		 * use 0x03 to track RPS1 interrupts - increase by 1 every gpio3 is toggled
+		 * use 0x15 to track VPE  interrupts - increase by 1 every vpeirq() is called
+		 */
+		saa7146_write(dev, EC1SSR, (0x03<<2) | 3 );
+		/* set event counter 1 threshold to maximum allowed value        (rEC p55) */
+		saa7146_write(dev, ECT1R,  0x3fff );
+#endif
+		/* Set RPS1 Address register to point to RPS code               (r108 p42) */
+		saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+		/* Enable RPS1,                                                 (rFC p33) */
+		saa7146_write(dev, MC1, (MASK_13 | MASK_29 ));
+
+		mdelay(10);
+		/* now send VSYNC_B to rps1 by rising GPIO3 */
+		saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+		mdelay(10);
+		/* if rps1 responded by lowering the GPIO3,
+		 * then we have budgetpatch hardware
+		 */
+		if ((saa7146_read(dev, GPIO_CTRL) & 0x10000000) == 0) {
+			budgetpatch = 1;
+			printk("dvb-ttpci: BUDGET-PATCH DETECTED.\n");
+		}
+		/* Disable RPS1 */
+		saa7146_write(dev, MC1, ( MASK_29 ));
+#if RPS_IRQ
+		printk("dvb-ttpci: Event Counter 1 0x%04x\n", saa7146_read(dev, EC1R) & 0x3fff );
+#endif
+	}
+
+	/* prepare the av7110 device struct */
+	av7110 = kzalloc(sizeof(struct av7110), GFP_KERNEL);
+	if (!av7110) {
+		dprintk(1, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	av7110->card_name = (char*) pci_ext->ext_priv;
+	av7110->dev = dev;
+	dev->ext_priv = av7110;
+
+	ret = get_firmware(av7110);
+	if (ret < 0)
+		goto err_kfree_0;
+
+	ret = dvb_register_adapter(&av7110->dvb_adapter, av7110->card_name,
+				   THIS_MODULE, &dev->pci->dev, adapter_nr);
+	if (ret < 0)
+		goto err_put_firmware_1;
+
+	/* the Siemens DVB needs this if you want to have the i2c chips
+	   get recognized before the main driver is fully loaded */
+	saa7146_write(dev, GPIO_CTRL, 0x500000);
+
+	strscpy(av7110->i2c_adap.name, pci_ext->ext_priv,
+		sizeof(av7110->i2c_adap.name));
+
+	saa7146_i2c_adapter_prepare(dev, &av7110->i2c_adap, SAA7146_I2C_BUS_BIT_RATE_120); /* 275 kHz */
+
+	ret = i2c_add_adapter(&av7110->i2c_adap);
+	if (ret < 0)
+		goto err_dvb_unregister_adapter_2;
+
+	ttpci_eeprom_parse_mac(&av7110->i2c_adap,
+			       av7110->dvb_adapter.proposed_mac);
+	ret = -ENOMEM;
+
+	/* full-ts mod? */
+	if (full_ts)
+		av7110->full_ts = true;
+
+	/* check for full-ts flag in eeprom */
+	if (i2c_readreg(av7110, 0xaa, 0) == 0x4f && i2c_readreg(av7110, 0xaa, 1) == 0x45) {
+		u8 flags = i2c_readreg(av7110, 0xaa, 2);
+		if (flags != 0xff && (flags & 0x01))
+			av7110->full_ts = true;
+	}
+
+	if (av7110->full_ts) {
+		printk(KERN_INFO "dvb-ttpci: full-ts mode enabled for saa7146 port B\n");
+		spin_lock_init(&av7110->feedlock1);
+		av7110->grabbing = saa7146_vmalloc_build_pgtable(pdev, length,
+								 &av7110->pt);
+		if (!av7110->grabbing)
+			goto err_i2c_del_3;
+
+		saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+		saa7146_write(dev, MC2, (MASK_10 | MASK_26));
+
+		saa7146_write(dev, DD1_INIT, 0x00000600);
+		saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+		saa7146_write(dev, BRS_CTRL, 0x60000000);
+		saa7146_write(dev, MC2, MASK_08 | MASK_24);
+
+		/* dma3 */
+		saa7146_write(dev, PCI_BT_V1, 0x001c0000 | (saa7146_read(dev, PCI_BT_V1) & ~0x001f0000));
+		saa7146_write(dev, BASE_ODD3, 0);
+		saa7146_write(dev, BASE_EVEN3, 0);
+		saa7146_write(dev, PROT_ADDR3, TS_WIDTH * TS_HEIGHT);
+		saa7146_write(dev, PITCH3, TS_WIDTH);
+		saa7146_write(dev, BASE_PAGE3, av7110->pt.dma | ME1 | 0x90);
+		saa7146_write(dev, NUM_LINE_BYTE3, (TS_HEIGHT << 16) | TS_WIDTH);
+		saa7146_write(dev, MC2, MASK_04 | MASK_20);
+
+		tasklet_setup(&av7110->vpe_tasklet, vpeirq);
+
+	} else if (budgetpatch) {
+		spin_lock_init(&av7110->feedlock1);
+		av7110->grabbing = saa7146_vmalloc_build_pgtable(pdev, length,
+								 &av7110->pt);
+		if (!av7110->grabbing)
+			goto err_i2c_del_3;
+
+		saa7146_write(dev, PCI_BT_V1, 0x1c1f101f);
+		saa7146_write(dev, BCS_CTRL, 0x80400040);
+		/* set dd1 stream a & b */
+		saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+		saa7146_write(dev, DD1_INIT, 0x03000200);
+		saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+		saa7146_write(dev, BRS_CTRL, 0x60000000);
+		saa7146_write(dev, BASE_ODD3, 0);
+		saa7146_write(dev, BASE_EVEN3, 0);
+		saa7146_write(dev, PROT_ADDR3, TS_WIDTH * TS_HEIGHT);
+		saa7146_write(dev, BASE_PAGE3, av7110->pt.dma | ME1 | 0x90);
+
+		saa7146_write(dev, PITCH3, TS_WIDTH);
+		saa7146_write(dev, NUM_LINE_BYTE3, (TS_HEIGHT << 16) | TS_WIDTH);
+
+		/* upload all */
+		saa7146_write(dev, MC2, 0x077c077c);
+		saa7146_write(dev, GPIO_CTRL, 0x000000);
+#if RPS_IRQ
+		/* set event counter 1 source as RPS1 interrupt (0x03)          (rE4 p53)
+		 * use 0x03 to track RPS1 interrupts - increase by 1 every gpio3 is toggled
+		 * use 0x15 to track VPE  interrupts - increase by 1 every vpeirq() is called
+		 */
+		saa7146_write(dev, EC1SSR, (0x03<<2) | 3 );
+		/* set event counter 1 threshold to maximum allowed value        (rEC p55) */
+		saa7146_write(dev, ECT1R,  0x3fff );
+#endif
+		/* Setup BUDGETPATCH MAIN RPS1 "program" (p35) */
+		count = 0;
+
+		/* Wait Source Line Counter Threshold                           (p36) */
+		WRITE_RPS1(CMD_PAUSE | EVT_HS);
+		/* Set GPIO3=1                                                  (p42) */
+		WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+		WRITE_RPS1(GPIO3_MSK);
+		WRITE_RPS1(SAA7146_GPIO_OUTHI<<24);
+#if RPS_IRQ
+		/* issue RPS1 interrupt */
+		WRITE_RPS1(CMD_INTERRUPT);
+#endif
+		/* Wait reset Source Line Counter Threshold                     (p36) */
+		WRITE_RPS1(CMD_PAUSE | RPS_INV | EVT_HS);
+		/* Set GPIO3=0                                                  (p42) */
+		WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+		WRITE_RPS1(GPIO3_MSK);
+		WRITE_RPS1(SAA7146_GPIO_OUTLO<<24);
+#if RPS_IRQ
+		/* issue RPS1 interrupt */
+		WRITE_RPS1(CMD_INTERRUPT);
+#endif
+		/* Jump to begin of RPS program                                 (p37) */
+		WRITE_RPS1(CMD_JUMP);
+		WRITE_RPS1(dev->d_rps1.dma_handle);
+
+		/* Fix VSYNC level */
+		saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+		/* Set RPS1 Address register to point to RPS code               (r108 p42) */
+		saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+		/* Set Source Line Counter Threshold, using BRS                 (rCC p43)
+		 * It generates HS event every TS_HEIGHT lines
+		 * this is related to TS_WIDTH set in register
+		 * NUM_LINE_BYTE3. If NUM_LINE_BYTE low 16 bits
+		 * are set to TS_WIDTH bytes (TS_WIDTH=2*188),
+		 * then RPS_THRESH1 should be set to trigger
+		 * every TS_HEIGHT (512) lines.
+		 */
+		saa7146_write(dev, RPS_THRESH1, (TS_HEIGHT*1) | MASK_12 );
+
+		/* Enable RPS1                                                  (rFC p33) */
+		saa7146_write(dev, MC1, (MASK_13 | MASK_29));
+
+		/* end of budgetpatch register initialization */
+		tasklet_setup(&av7110->vpe_tasklet,  vpeirq);
+	} else {
+		saa7146_write(dev, PCI_BT_V1, 0x1c00101f);
+		saa7146_write(dev, BCS_CTRL, 0x80400040);
+
+		/* set dd1 stream a & b */
+		saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+		saa7146_write(dev, DD1_INIT, 0x03000000);
+		saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+		/* upload all */
+		saa7146_write(dev, MC2, 0x077c077c);
+		saa7146_write(dev, GPIO_CTRL, 0x000000);
+	}
+
+	tasklet_setup(&av7110->debi_tasklet, debiirq);
+	tasklet_setup(&av7110->gpio_tasklet, gpioirq);
+
+	mutex_init(&av7110->pid_mutex);
+
+	/* locks for data transfers from/to AV7110 */
+	spin_lock_init(&av7110->debilock);
+	mutex_init(&av7110->dcomlock);
+	av7110->debitype = -1;
+
+	/* default OSD window */
+	av7110->osdwin = 1;
+	mutex_init(&av7110->osd_mutex);
+
+	/* TV standard */
+	av7110->vidmode = tv_standard == 1 ? AV7110_VIDEO_MODE_NTSC
+					   : AV7110_VIDEO_MODE_PAL;
+
+	/* ARM "watchdog" */
+	init_waitqueue_head(&av7110->arm_wait);
+	av7110->arm_thread = NULL;
+
+	/* allocate and init buffers */
+	av7110->debi_virt = dma_alloc_coherent(&pdev->dev, 8192,
+					       &av7110->debi_bus, GFP_KERNEL);
+	if (!av7110->debi_virt)
+		goto err_saa71466_vfree_4;
+
+
+	av7110->iobuf = vmalloc(AVOUTLEN+AOUTLEN+BMPLEN+4*IPACKS);
+	if (!av7110->iobuf)
+		goto err_pci_free_5;
+
+	ret = av7110_av_init(av7110);
+	if (ret < 0)
+		goto err_iobuf_vfree_6;
+
+	/* init BMP buffer */
+	av7110->bmpbuf = av7110->iobuf+AVOUTLEN+AOUTLEN;
+	init_waitqueue_head(&av7110->bmpq);
+
+	ret = av7110_ca_init(av7110);
+	if (ret < 0)
+		goto err_av7110_av_exit_7;
+
+	/* load firmware into AV7110 cards */
+	ret = av7110_bootarm(av7110);
+	if (ret < 0)
+		goto err_av7110_ca_exit_8;
+
+	ret = av7110_firmversion(av7110);
+	if (ret < 0)
+		goto err_stop_arm_9;
+
+	if (FW_VERSION(av7110->arm_app)<0x2501)
+		printk(KERN_WARNING
+		       "dvb-ttpci: Warning, firmware version 0x%04x is too old. System might be unstable!\n",
+		       FW_VERSION(av7110->arm_app));
+
+	thread = kthread_run(arm_thread, (void *) av7110, "arm_mon");
+	if (IS_ERR(thread)) {
+		ret = PTR_ERR(thread);
+		goto err_stop_arm_9;
+	}
+	av7110->arm_thread = thread;
+
+	/* set initial volume in mixer struct */
+	av7110->mixer.volume_left  = volume;
+	av7110->mixer.volume_right = volume;
+
+	ret = av7110_register(av7110);
+	if (ret < 0)
+		goto err_arm_thread_stop_10;
+
+	init_av7110_av(av7110);
+
+	/* special case DVB-C: these cards have an analog tuner
+	   plus need some special handling, so we have separate
+	   saa7146_ext_vv data for these... */
+	ret = av7110_init_v4l(av7110);
+	if (ret < 0)
+		goto err_av7110_unregister_11;
+
+	av7110->dvb_adapter.priv = av7110;
+	ret = frontend_init(av7110);
+	if (ret < 0)
+		goto err_av7110_exit_v4l_12;
+
+	mutex_init(&av7110->ioctl_mutex);
+
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
+	av7110_ir_init(av7110);
+#endif
+	printk(KERN_INFO "dvb-ttpci: found av7110-%d.\n", av7110_num);
+	av7110_num++;
+out:
+	return ret;
+
+err_av7110_exit_v4l_12:
+	av7110_exit_v4l(av7110);
+err_av7110_unregister_11:
+	dvb_unregister(av7110);
+err_arm_thread_stop_10:
+	av7110_arm_sync(av7110);
+err_stop_arm_9:
+	/* Nothing to do. Rejoice. */
+err_av7110_ca_exit_8:
+	av7110_ca_exit(av7110);
+err_av7110_av_exit_7:
+	av7110_av_exit(av7110);
+err_iobuf_vfree_6:
+	vfree(av7110->iobuf);
+err_pci_free_5:
+	dma_free_coherent(&pdev->dev, 8192, av7110->debi_virt,
+			  av7110->debi_bus);
+err_saa71466_vfree_4:
+	if (av7110->grabbing)
+		saa7146_vfree_destroy_pgtable(pdev, av7110->grabbing, &av7110->pt);
+err_i2c_del_3:
+	i2c_del_adapter(&av7110->i2c_adap);
+err_dvb_unregister_adapter_2:
+	dvb_unregister_adapter(&av7110->dvb_adapter);
+err_put_firmware_1:
+	put_firmware(av7110);
+err_kfree_0:
+	kfree(av7110);
+	goto out;
+}
+
+static int av7110_detach(struct saa7146_dev* saa)
+{
+	struct av7110 *av7110 = saa->ext_priv;
+	dprintk(4, "%p\n", av7110);
+
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
+	av7110_ir_exit(av7110);
+#endif
+	if (budgetpatch || av7110->full_ts) {
+		if (budgetpatch) {
+			/* Disable RPS1 */
+			saa7146_write(saa, MC1, MASK_29);
+			/* VSYNC LOW (inactive) */
+			saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTLO);
+		}
+		saa7146_write(saa, MC1, MASK_20);	/* DMA3 off */
+		SAA7146_IER_DISABLE(saa, MASK_10);
+		SAA7146_ISR_CLEAR(saa, MASK_10);
+		msleep(50);
+		tasklet_kill(&av7110->vpe_tasklet);
+		saa7146_vfree_destroy_pgtable(saa->pci, av7110->grabbing, &av7110->pt);
+	}
+	av7110_exit_v4l(av7110);
+
+	av7110_arm_sync(av7110);
+
+	tasklet_kill(&av7110->debi_tasklet);
+	tasklet_kill(&av7110->gpio_tasklet);
+
+	dvb_unregister(av7110);
+
+	SAA7146_IER_DISABLE(saa, MASK_19 | MASK_03);
+	SAA7146_ISR_CLEAR(saa, MASK_19 | MASK_03);
+
+	av7110_ca_exit(av7110);
+	av7110_av_exit(av7110);
+
+	vfree(av7110->iobuf);
+	dma_free_coherent(&saa->pci->dev, 8192, av7110->debi_virt,
+			  av7110->debi_bus);
+
+	i2c_del_adapter(&av7110->i2c_adap);
+
+	dvb_unregister_adapter (&av7110->dvb_adapter);
+
+	av7110_num--;
+
+	put_firmware(av7110);
+
+	kfree(av7110);
+
+	saa->ext_priv = NULL;
+
+	return 0;
+}
+
+
+static void av7110_irq(struct saa7146_dev* dev, u32 *isr)
+{
+	struct av7110 *av7110 = dev->ext_priv;
+
+	//print_time("av7110_irq");
+
+	/* Note: Don't try to handle the DEBI error irq (MASK_18), in
+	 * intel mode the timeout is asserted all the time...
+	 */
+
+	if (*isr & MASK_19) {
+		//printk("av7110_irq: DEBI\n");
+		/* Note 1: The DEBI irq is level triggered: We must enable it
+		 * only after we started a DMA xfer, and disable it here
+		 * immediately, or it will be signalled all the time while
+		 * DEBI is idle.
+		 * Note 2: You would think that an irq which is masked is
+		 * not signalled by the hardware. Not so for the SAA7146:
+		 * An irq is signalled as long as the corresponding bit
+		 * in the ISR is set, and disabling irqs just prevents the
+		 * hardware from setting the ISR bit. This means a) that we
+		 * must clear the ISR *after* disabling the irq (which is why
+		 * we must do it here even though saa7146_core did it already),
+		 * and b) that if we were to disable an edge triggered irq
+		 * (like the gpio irqs sadly are) temporarily we would likely
+		 * loose some. This sucks :-(
+		 */
+		SAA7146_IER_DISABLE(av7110->dev, MASK_19);
+		SAA7146_ISR_CLEAR(av7110->dev, MASK_19);
+		tasklet_schedule(&av7110->debi_tasklet);
+	}
+
+	if (*isr & MASK_03) {
+		//printk("av7110_irq: GPIO\n");
+		tasklet_schedule(&av7110->gpio_tasklet);
+	}
+
+	if (*isr & MASK_10)
+		tasklet_schedule(&av7110->vpe_tasklet);
+}
+
+
+static struct saa7146_extension av7110_extension_driver;
+
+#define MAKE_AV7110_INFO(x_var,x_name) \
+static struct saa7146_pci_extension_data x_var = { \
+	.ext_priv = x_name, \
+	.ext = &av7110_extension_driver }
+
+MAKE_AV7110_INFO(tts_1_X_fsc,"Technotrend/Hauppauge WinTV DVB-S rev1.X or Fujitsu Siemens DVB-C");
+MAKE_AV7110_INFO(ttt_1_X,    "Technotrend/Hauppauge WinTV DVB-T rev1.X");
+MAKE_AV7110_INFO(ttc_1_X,    "Technotrend/Hauppauge WinTV Nexus-CA rev1.X");
+MAKE_AV7110_INFO(ttc_2_X,    "Technotrend/Hauppauge WinTV DVB-C rev2.X");
+MAKE_AV7110_INFO(tts_2_X,    "Technotrend/Hauppauge WinTV Nexus-S rev2.X");
+MAKE_AV7110_INFO(tts_2_3,    "Technotrend/Hauppauge WinTV Nexus-S rev2.3");
+MAKE_AV7110_INFO(tts_1_3se,  "Technotrend/Hauppauge WinTV DVB-S rev1.3 SE");
+MAKE_AV7110_INFO(ttt,        "Technotrend/Hauppauge DVB-T");
+MAKE_AV7110_INFO(fsc,        "Fujitsu Siemens DVB-C");
+MAKE_AV7110_INFO(fss,        "Fujitsu Siemens DVB-S rev1.6");
+MAKE_AV7110_INFO(gxs_1_3,    "Galaxis DVB-S rev1.3");
+
+static const struct pci_device_id pci_tbl[] = {
+	MAKE_EXTENSION_PCI(fsc,         0x110a, 0x0000),
+	MAKE_EXTENSION_PCI(tts_1_X_fsc, 0x13c2, 0x0000),
+	MAKE_EXTENSION_PCI(ttt_1_X,     0x13c2, 0x0001),
+	MAKE_EXTENSION_PCI(ttc_2_X,     0x13c2, 0x0002),
+	MAKE_EXTENSION_PCI(tts_2_X,     0x13c2, 0x0003),
+	MAKE_EXTENSION_PCI(gxs_1_3,     0x13c2, 0x0004),
+	MAKE_EXTENSION_PCI(fss,         0x13c2, 0x0006),
+	MAKE_EXTENSION_PCI(ttt,         0x13c2, 0x0008),
+	MAKE_EXTENSION_PCI(ttc_1_X,     0x13c2, 0x000a),
+	MAKE_EXTENSION_PCI(tts_2_3,     0x13c2, 0x000e),
+	MAKE_EXTENSION_PCI(tts_1_3se,   0x13c2, 0x1002),
+
+/*	MAKE_EXTENSION_PCI(???, 0x13c2, 0x0005), UNDEFINED CARD */ // Technisat SkyStar1
+/*	MAKE_EXTENSION_PCI(???, 0x13c2, 0x0009), UNDEFINED CARD */ // TT/Hauppauge WinTV Nexus-CA v????
+
+	{
+		.vendor    = 0,
+	}
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+
+static struct saa7146_extension av7110_extension_driver = {
+	.name		= "av7110",
+	.flags		= SAA7146_USE_I2C_IRQ,
+
+	.module		= THIS_MODULE,
+	.pci_tbl	= &pci_tbl[0],
+	.attach		= av7110_attach,
+	.detach		= av7110_detach,
+
+	.irq_mask	= MASK_19 | MASK_03 | MASK_10,
+	.irq_func	= av7110_irq,
+};
+
+
+static int __init av7110_init(void)
+{
+	return saa7146_register_extension(&av7110_extension_driver);
+}
+
+
+static void __exit av7110_exit(void)
+{
+	saa7146_unregister_extension(&av7110_extension_driver);
+}
+
+module_init(av7110_init);
+module_exit(av7110_exit);
+
+MODULE_DESCRIPTION("driver for the SAA7146 based AV110 PCI DVB cards by Siemens, Technotrend, Hauppauge");
+MODULE_AUTHOR("Ralph Metzler, Marcus Metzler, others");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110.h b/drivers/staging/media/deprecated/saa7146/av7110/av7110.h
new file mode 100644
index 000000000..9fde69b38
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110.h
@@ -0,0 +1,315 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _AV7110_H_
+#define _AV7110_H_
+
+#include <linux/interrupt.h>
+#include <linux/socket.h>
+#include <linux/netdevice.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/time.h>
+
+#include <linux/dvb/video.h>
+#include <linux/dvb/audio.h>
+#include <linux/dvb/dmx.h>
+#include <linux/dvb/ca.h>
+#include <linux/dvb/osd.h>
+#include <linux/dvb/net.h>
+#include <linux/mutex.h>
+
+#include <media/dvbdev.h>
+#include <media/demux.h>
+#include <media/dvb_demux.h>
+#include <media/dmxdev.h>
+#include "dvb_filter.h"
+#include <media/dvb_net.h>
+#include <media/dvb_ringbuffer.h>
+#include <media/dvb_frontend.h>
+#include "ves1820.h"
+#include "ves1x93.h"
+#include "stv0299.h"
+#include "tda8083.h"
+#include "sp8870.h"
+#include "stv0297.h"
+#include "l64781.h"
+
+#include "saa7146_vv.h"
+
+
+#define ANALOG_TUNER_VES1820 1
+#define ANALOG_TUNER_STV0297 2
+
+extern int av7110_debug;
+
+#define dprintk(level, fmt, arg...) do {				\
+	if (level & av7110_debug)					\
+		printk(KERN_DEBUG KBUILD_MODNAME ": %s(): " fmt,	\
+		       __func__, ##arg);				\
+} while (0)
+
+#define MAXFILT 32
+
+enum {AV_PES_STREAM, PS_STREAM, TS_STREAM, PES_STREAM};
+
+enum av7110_video_mode {
+	AV7110_VIDEO_MODE_PAL	= 0,
+	AV7110_VIDEO_MODE_NTSC	= 1
+};
+
+struct av7110_p2t {
+	u8		  pes[TS_SIZE];
+	u8		  counter;
+	long int	  pos;
+	int		  frags;
+	struct dvb_demux_feed *feed;
+};
+
+/* video MPEG decoder events: */
+/* (code copied from dvb_frontend.c, should maybe be factored out...) */
+#define MAX_VIDEO_EVENT 8
+struct dvb_video_events {
+	struct video_event	  events[MAX_VIDEO_EVENT];
+	int			  eventw;
+	int			  eventr;
+	int			  overflow;
+	wait_queue_head_t	  wait_queue;
+	spinlock_t		  lock;
+};
+
+
+struct av7110;
+
+/* infrared remote control */
+struct infrared {
+	struct rc_dev		*rcdev;
+	char			input_phys[32];
+	u32			ir_config;
+};
+
+/* place to store all the necessary device information */
+struct av7110 {
+
+	/* devices */
+
+	struct dvb_device	dvb_dev;
+	struct dvb_net		dvb_net;
+
+	struct video_device	v4l_dev;
+	struct video_device	vbi_dev;
+
+	struct saa7146_dev	*dev;
+
+	struct i2c_adapter	i2c_adap;
+
+	char			*card_name;
+
+	/* support for analog module of dvb-c */
+	int			analog_tuner_flags;
+	int			current_input;
+	u32			current_freq;
+
+	struct tasklet_struct	debi_tasklet;
+	struct tasklet_struct	gpio_tasklet;
+
+	int adac_type;	       /* audio DAC type */
+#define DVB_ADAC_TI	  0
+#define DVB_ADAC_CRYSTAL  1
+#define DVB_ADAC_MSP34x0  2
+#define DVB_ADAC_MSP34x5  3
+#define DVB_ADAC_NONE	 -1
+
+
+	/* buffers */
+
+	void		       *iobuf;	 /* memory for all buffers */
+	struct dvb_ringbuffer	avout;   /* buffer for video or A/V mux */
+#define AVOUTLEN (128*1024)
+	struct dvb_ringbuffer	aout;    /* buffer for audio */
+#define AOUTLEN (64*1024)
+	void		       *bmpbuf;
+#define BMPLEN (8*32768+1024)
+
+	/* bitmap buffers and states */
+
+	int			bmpp;
+	int			bmplen;
+	volatile int		bmp_state;
+#define BMP_NONE     0
+#define BMP_LOADING  1
+#define BMP_LOADED   2
+	wait_queue_head_t	bmpq;
+
+
+	/* DEBI and polled command interface */
+
+	spinlock_t		debilock;
+	struct mutex		dcomlock;
+	volatile int		debitype;
+	volatile int		debilen;
+
+
+	/* Recording and playback flags */
+
+	int			rec_mode;
+	int			playing;
+#define RP_NONE  0
+#define RP_VIDEO 1
+#define RP_AUDIO 2
+#define RP_AV	 3
+
+
+	/* OSD */
+
+	int			osdwin;      /* currently active window */
+	u16			osdbpp[8];
+	struct mutex		osd_mutex;
+
+	/* CA */
+
+	struct ca_slot_info	ci_slot[2];
+
+	enum av7110_video_mode	vidmode;
+	struct dmxdev		dmxdev;
+	struct dvb_demux	demux;
+
+	struct dmx_frontend	hw_frontend;
+	struct dmx_frontend	mem_frontend;
+
+	/* for budget mode demux1 */
+	struct dmxdev		dmxdev1;
+	struct dvb_demux	demux1;
+	struct dvb_net		dvb_net1;
+	spinlock_t		feedlock1;
+	int			feeding1;
+	u32			ttbp;
+	unsigned char           *grabbing;
+	struct saa7146_pgtable  pt;
+	struct tasklet_struct   vpe_tasklet;
+	bool			full_ts;
+
+	int			fe_synced;
+	struct mutex		pid_mutex;
+
+	int			video_blank;
+	struct video_status	videostate;
+	u16			display_panscan;
+	int			display_ar;
+	int			trickmode;
+#define TRICK_NONE   0
+#define TRICK_FAST   1
+#define TRICK_SLOW   2
+#define TRICK_FREEZE 3
+	struct audio_status	audiostate;
+
+	struct dvb_demux_filter *handle2filter[32];
+	struct av7110_p2t	 p2t_filter[MAXFILT];
+	struct dvb_filter_pes2ts p2t[2];
+	struct ipack		 ipack[2];
+	u8			*kbuf[2];
+
+	int sinfo;
+	int feeding;
+
+	int arm_errors;
+	int registered;
+
+
+	/* AV711X */
+
+	u32		    arm_fw;
+	u32		    arm_rtsl;
+	u32		    arm_vid;
+	u32		    arm_app;
+	u32		    avtype;
+	int		    arm_ready;
+	struct task_struct *arm_thread;
+	wait_queue_head_t   arm_wait;
+	u16		    arm_loops;
+
+	void		   *debi_virt;
+	dma_addr_t	    debi_bus;
+
+	u16		    pids[DMX_PES_OTHER];
+
+	struct dvb_ringbuffer	 ci_rbuffer;
+	struct dvb_ringbuffer	 ci_wbuffer;
+
+	struct audio_mixer	mixer;
+
+	struct dvb_adapter	 dvb_adapter;
+	struct dvb_device	 *video_dev;
+	struct dvb_device	 *audio_dev;
+	struct dvb_device	 *ca_dev;
+	struct dvb_device	 *osd_dev;
+
+	struct dvb_video_events  video_events;
+	video_size_t		 video_size;
+
+	u16			wssMode;
+	u16			wssData;
+
+	struct infrared		ir;
+
+	/* firmware stuff */
+	unsigned char *bin_fw;
+	unsigned long size_fw;
+
+	unsigned char *bin_dpram;
+	unsigned long size_dpram;
+
+	unsigned char *bin_root;
+	unsigned long size_root;
+
+	struct dvb_frontend* fe;
+	enum fe_status fe_status;
+
+	struct mutex ioctl_mutex;
+
+	/* crash recovery */
+	void				(*recover)(struct av7110* av7110);
+	enum fe_sec_voltage		saved_voltage;
+	enum fe_sec_tone_mode		saved_tone;
+	struct dvb_diseqc_master_cmd	saved_master_cmd;
+	enum fe_sec_mini_cmd		saved_minicmd;
+
+	int (*fe_init)(struct dvb_frontend* fe);
+	int (*fe_read_status)(struct dvb_frontend *fe, enum fe_status *status);
+	int (*fe_diseqc_reset_overload)(struct dvb_frontend *fe);
+	int (*fe_diseqc_send_master_cmd)(struct dvb_frontend *fe,
+					 struct dvb_diseqc_master_cmd *cmd);
+	int (*fe_diseqc_send_burst)(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd);
+	int (*fe_set_tone)(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone);
+	int (*fe_set_voltage)(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage);
+	int (*fe_dishnetwork_send_legacy_command)(struct dvb_frontend *fe,
+						  unsigned long cmd);
+	int (*fe_set_frontend)(struct dvb_frontend *fe);
+};
+
+
+extern int ChangePIDs(struct av7110 *av7110, u16 vpid, u16 apid, u16 ttpid,
+		       u16 subpid, u16 pcrpid);
+
+void av7110_ir_handler(struct av7110 *av7110, u32 ircom);
+int av7110_set_ir_config(struct av7110 *av7110);
+int av7110_ir_init(struct av7110 *av7110);
+void av7110_ir_exit(struct av7110 *av7110);
+
+/* msp3400 i2c subaddresses */
+#define MSP_WR_DEM 0x10
+#define MSP_RD_DEM 0x11
+#define MSP_WR_DSP 0x12
+#define MSP_RD_DSP 0x13
+
+extern int i2c_writereg(struct av7110 *av7110, u8 id, u8 reg, u8 val);
+extern u8 i2c_readreg(struct av7110 *av7110, u8 id, u8 reg);
+extern int msp_writereg(struct av7110 *av7110, u8 dev, u16 reg, u16 val);
+
+
+extern int av7110_init_analog_module(struct av7110 *av7110);
+extern int av7110_init_v4l(struct av7110 *av7110);
+extern int av7110_exit_v4l(struct av7110 *av7110);
+
+#endif /* _AV7110_H_ */
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.c
new file mode 100644
index 000000000..a5c5bebad
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.c
@@ -0,0 +1,1681 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * av7110_av.c: audio and video MPEG decoder stuff
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * originally based on code by:
+ * Copyright (C) 1998,1999 Christian Theiss <mistert@rz.fh-augsburg.de>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#include <linux/ethtool.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+
+#include "av7110.h"
+#include "av7110_hw.h"
+#include "av7110_av.h"
+#include "av7110_ipack.h"
+
+/* MPEG-2 (ISO 13818 / H.222.0) stream types */
+#define PROG_STREAM_MAP  0xBC
+#define PRIVATE_STREAM1  0xBD
+#define PADDING_STREAM	 0xBE
+#define PRIVATE_STREAM2  0xBF
+#define AUDIO_STREAM_S	 0xC0
+#define AUDIO_STREAM_E	 0xDF
+#define VIDEO_STREAM_S	 0xE0
+#define VIDEO_STREAM_E	 0xEF
+#define ECM_STREAM	 0xF0
+#define EMM_STREAM	 0xF1
+#define DSM_CC_STREAM	 0xF2
+#define ISO13522_STREAM  0xF3
+#define PROG_STREAM_DIR  0xFF
+
+#define PTS_DTS_FLAGS	 0xC0
+
+//pts_dts flags
+#define PTS_ONLY	 0x80
+#define PTS_DTS		 0xC0
+#define TS_SIZE		 188
+#define TRANS_ERROR	 0x80
+#define PAY_START	 0x40
+#define TRANS_PRIO	 0x20
+#define PID_MASK_HI	 0x1F
+//flags
+#define TRANS_SCRMBL1	 0x80
+#define TRANS_SCRMBL2	 0x40
+#define ADAPT_FIELD	 0x20
+#define PAYLOAD		 0x10
+#define COUNT_MASK	 0x0F
+
+// adaptation flags
+#define DISCON_IND	 0x80
+#define RAND_ACC_IND	 0x40
+#define ES_PRI_IND	 0x20
+#define PCR_FLAG	 0x10
+#define OPCR_FLAG	 0x08
+#define SPLICE_FLAG	 0x04
+#define TRANS_PRIV	 0x02
+#define ADAP_EXT_FLAG	 0x01
+
+// adaptation extension flags
+#define LTW_FLAG	 0x80
+#define PIECE_RATE	 0x40
+#define SEAM_SPLICE	 0x20
+
+
+static void p_to_t(u8 const *buf, long int length, u16 pid,
+		   u8 *counter, struct dvb_demux_feed *feed);
+static int write_ts_to_decoder(struct av7110 *av7110, int type, const u8 *buf, size_t len);
+
+
+int av7110_record_cb(struct dvb_filter_pes2ts *p2t, u8 *buf, size_t len)
+{
+	struct dvb_demux_feed *dvbdmxfeed = (struct dvb_demux_feed *) p2t->priv;
+
+	if (!(dvbdmxfeed->ts_type & TS_PACKET))
+		return 0;
+	if (buf[3] == 0xe0)	 // video PES do not have a length in TS
+		buf[4] = buf[5] = 0;
+	if (dvbdmxfeed->ts_type & TS_PAYLOAD_ONLY)
+		return dvbdmxfeed->cb.ts(buf, len, NULL, 0,
+					 &dvbdmxfeed->feed.ts, NULL);
+	else
+		return dvb_filter_pes2ts(p2t, buf, len, 1);
+}
+
+static int dvb_filter_pes2ts_cb(void *priv, unsigned char *data)
+{
+	struct dvb_demux_feed *dvbdmxfeed = (struct dvb_demux_feed *) priv;
+
+	dvbdmxfeed->cb.ts(data, 188, NULL, 0,
+			  &dvbdmxfeed->feed.ts, NULL);
+	return 0;
+}
+
+int av7110_av_start_record(struct av7110 *av7110, int av,
+			   struct dvb_demux_feed *dvbdmxfeed)
+{
+	int ret = 0;
+	struct dvb_demux *dvbdmx = dvbdmxfeed->demux;
+
+	dprintk(2, "av7110:%p, dvb_demux_feed:%p\n", av7110, dvbdmxfeed);
+
+	if (av7110->playing || (av7110->rec_mode & av))
+		return -EBUSY;
+	av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Stop, 0);
+	dvbdmx->recording = 1;
+	av7110->rec_mode |= av;
+
+	switch (av7110->rec_mode) {
+	case RP_AUDIO:
+		dvb_filter_pes2ts_init(&av7110->p2t[0],
+				       dvbdmx->pesfilter[0]->pid,
+				       dvb_filter_pes2ts_cb,
+				       (void *) dvbdmx->pesfilter[0]);
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Record, 2, AudioPES, 0);
+		break;
+
+	case RP_VIDEO:
+		dvb_filter_pes2ts_init(&av7110->p2t[1],
+				       dvbdmx->pesfilter[1]->pid,
+				       dvb_filter_pes2ts_cb,
+				       (void *) dvbdmx->pesfilter[1]);
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Record, 2, VideoPES, 0);
+		break;
+
+	case RP_AV:
+		dvb_filter_pes2ts_init(&av7110->p2t[0],
+				       dvbdmx->pesfilter[0]->pid,
+				       dvb_filter_pes2ts_cb,
+				       (void *) dvbdmx->pesfilter[0]);
+		dvb_filter_pes2ts_init(&av7110->p2t[1],
+				       dvbdmx->pesfilter[1]->pid,
+				       dvb_filter_pes2ts_cb,
+				       (void *) dvbdmx->pesfilter[1]);
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Record, 2, AV_PES, 0);
+		break;
+	}
+	return ret;
+}
+
+int av7110_av_start_play(struct av7110 *av7110, int av)
+{
+	int ret = 0;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (av7110->rec_mode)
+		return -EBUSY;
+	if (av7110->playing & av)
+		return -EBUSY;
+
+	av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Stop, 0);
+
+	if (av7110->playing == RP_NONE) {
+		av7110_ipack_reset(&av7110->ipack[0]);
+		av7110_ipack_reset(&av7110->ipack[1]);
+	}
+
+	av7110->playing |= av;
+	switch (av7110->playing) {
+	case RP_AUDIO:
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Play, 2, AudioPES, 0);
+		break;
+	case RP_VIDEO:
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Play, 2, VideoPES, 0);
+		av7110->sinfo = 0;
+		break;
+	case RP_AV:
+		av7110->sinfo = 0;
+		ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Play, 2, AV_PES, 0);
+		break;
+	}
+	return ret;
+}
+
+int av7110_av_stop(struct av7110 *av7110, int av)
+{
+	int ret = 0;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (!(av7110->playing & av) && !(av7110->rec_mode & av))
+		return 0;
+	av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Stop, 0);
+	if (av7110->playing) {
+		av7110->playing &= ~av;
+		switch (av7110->playing) {
+		case RP_AUDIO:
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Play, 2, AudioPES, 0);
+			break;
+		case RP_VIDEO:
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Play, 2, VideoPES, 0);
+			break;
+		case RP_NONE:
+			ret = av7110_set_vidmode(av7110, av7110->vidmode);
+			break;
+		}
+	} else {
+		av7110->rec_mode &= ~av;
+		switch (av7110->rec_mode) {
+		case RP_AUDIO:
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Record, 2, AudioPES, 0);
+			break;
+		case RP_VIDEO:
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Record, 2, VideoPES, 0);
+			break;
+		case RP_NONE:
+			break;
+		}
+	}
+	return ret;
+}
+
+
+int av7110_pes_play(void *dest, struct dvb_ringbuffer *buf, int dlen)
+{
+	int len;
+	u32 sync;
+	u16 blen;
+
+	if (!dlen) {
+		wake_up(&buf->queue);
+		return -1;
+	}
+	while (1) {
+		len = dvb_ringbuffer_avail(buf);
+		if (len < 6) {
+			wake_up(&buf->queue);
+			return -1;
+		}
+		sync =  DVB_RINGBUFFER_PEEK(buf, 0) << 24;
+		sync |= DVB_RINGBUFFER_PEEK(buf, 1) << 16;
+		sync |= DVB_RINGBUFFER_PEEK(buf, 2) << 8;
+		sync |= DVB_RINGBUFFER_PEEK(buf, 3);
+
+		if (((sync &~ 0x0f) == 0x000001e0) ||
+		    ((sync &~ 0x1f) == 0x000001c0) ||
+		    (sync == 0x000001bd))
+			break;
+		printk("resync\n");
+		DVB_RINGBUFFER_SKIP(buf, 1);
+	}
+	blen =  DVB_RINGBUFFER_PEEK(buf, 4) << 8;
+	blen |= DVB_RINGBUFFER_PEEK(buf, 5);
+	blen += 6;
+	if (len < blen || blen > dlen) {
+		//printk("buffer empty - avail %d blen %u dlen %d\n", len, blen, dlen);
+		wake_up(&buf->queue);
+		return -1;
+	}
+
+	dvb_ringbuffer_read(buf, dest, (size_t) blen);
+
+	dprintk(2, "pread=0x%08lx, pwrite=0x%08lx\n",
+	       (unsigned long) buf->pread, (unsigned long) buf->pwrite);
+	wake_up(&buf->queue);
+	return blen;
+}
+
+
+int av7110_set_volume(struct av7110 *av7110, unsigned int volleft,
+		      unsigned int volright)
+{
+	unsigned int vol, val, balance = 0;
+	int err;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	av7110->mixer.volume_left = volleft;
+	av7110->mixer.volume_right = volright;
+
+	switch (av7110->adac_type) {
+	case DVB_ADAC_TI:
+		volleft = (volleft * 256) / 1036;
+		volright = (volright * 256) / 1036;
+		if (volleft > 0x3f)
+			volleft = 0x3f;
+		if (volright > 0x3f)
+			volright = 0x3f;
+		if ((err = SendDAC(av7110, 3, 0x80 + volleft)))
+			return err;
+		return SendDAC(av7110, 4, volright);
+
+	case DVB_ADAC_CRYSTAL:
+		volleft = 127 - volleft / 2;
+		volright = 127 - volright / 2;
+		i2c_writereg(av7110, 0x20, 0x03, volleft);
+		i2c_writereg(av7110, 0x20, 0x04, volright);
+		return 0;
+
+	case DVB_ADAC_MSP34x0:
+		vol  = (volleft > volright) ? volleft : volright;
+		val	= (vol * 0x73 / 255) << 8;
+		if (vol > 0)
+		       balance = ((volright - volleft) * 127) / vol;
+		msp_writereg(av7110, MSP_WR_DSP, 0x0001, balance << 8);
+		msp_writereg(av7110, MSP_WR_DSP, 0x0000, val); /* loudspeaker */
+		msp_writereg(av7110, MSP_WR_DSP, 0x0006, val); /* headphonesr */
+		return 0;
+
+	case DVB_ADAC_MSP34x5:
+		vol = (volleft > volright) ? volleft : volright;
+		val = (vol * 0x73 / 255) << 8;
+		if (vol > 0)
+			balance = ((volright - volleft) * 127) / vol;
+		msp_writereg(av7110, MSP_WR_DSP, 0x0001, balance << 8);
+		msp_writereg(av7110, MSP_WR_DSP, 0x0000, val); /* loudspeaker */
+		return 0;
+	}
+
+	return 0;
+}
+
+int av7110_set_vidmode(struct av7110 *av7110, enum av7110_video_mode mode)
+{
+	int ret;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, LoadVidCode, 1, mode);
+
+	if (!ret && !av7110->playing) {
+		ret = ChangePIDs(av7110, av7110->pids[DMX_PES_VIDEO],
+			   av7110->pids[DMX_PES_AUDIO],
+			   av7110->pids[DMX_PES_TELETEXT],
+			   0, av7110->pids[DMX_PES_PCR]);
+		if (!ret)
+			ret = av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, Scan, 0);
+	}
+	return ret;
+}
+
+
+static enum av7110_video_mode sw2mode[16] = {
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_NTSC,
+	AV7110_VIDEO_MODE_NTSC, AV7110_VIDEO_MODE_PAL,
+	AV7110_VIDEO_MODE_NTSC, AV7110_VIDEO_MODE_NTSC,
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_NTSC,
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_PAL,
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_PAL,
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_PAL,
+	AV7110_VIDEO_MODE_PAL, AV7110_VIDEO_MODE_PAL,
+};
+
+static int get_video_format(struct av7110 *av7110, u8 *buf, int count)
+{
+	int i;
+	int hsize, vsize;
+	int sw;
+	u8 *p;
+	int ret = 0;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (av7110->sinfo)
+		return 0;
+	for (i = 7; i < count - 10; i++) {
+		p = buf + i;
+		if (p[0] || p[1] || p[2] != 0x01 || p[3] != 0xb3)
+			continue;
+		p += 4;
+		hsize = ((p[1] &0xF0) >> 4) | (p[0] << 4);
+		vsize = ((p[1] &0x0F) << 8) | (p[2]);
+		sw = (p[3] & 0x0F);
+		ret = av7110_set_vidmode(av7110, sw2mode[sw]);
+		if (!ret) {
+			dprintk(2, "playback %dx%d fr=%d\n", hsize, vsize, sw);
+			av7110->sinfo = 1;
+		}
+		break;
+	}
+	return ret;
+}
+
+
+/****************************************************************************
+ * I/O buffer management and control
+ ****************************************************************************/
+
+static inline long aux_ring_buffer_write(struct dvb_ringbuffer *rbuf,
+					 const u8 *buf, unsigned long count)
+{
+	unsigned long todo = count;
+	int free;
+
+	while (todo > 0) {
+		if (dvb_ringbuffer_free(rbuf) < 2048) {
+			if (wait_event_interruptible(rbuf->queue,
+						     (dvb_ringbuffer_free(rbuf) >= 2048)))
+				return count - todo;
+		}
+		free = dvb_ringbuffer_free(rbuf);
+		if (free > todo)
+			free = todo;
+		dvb_ringbuffer_write(rbuf, buf, free);
+		todo -= free;
+		buf += free;
+	}
+
+	return count - todo;
+}
+
+static void play_video_cb(u8 *buf, int count, void *priv)
+{
+	struct av7110 *av7110 = (struct av7110 *) priv;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if ((buf[3] & 0xe0) == 0xe0) {
+		get_video_format(av7110, buf, count);
+		aux_ring_buffer_write(&av7110->avout, buf, count);
+	} else
+		aux_ring_buffer_write(&av7110->aout, buf, count);
+}
+
+static void play_audio_cb(u8 *buf, int count, void *priv)
+{
+	struct av7110 *av7110 = (struct av7110 *) priv;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	aux_ring_buffer_write(&av7110->aout, buf, count);
+}
+
+
+#define FREE_COND_TS (dvb_ringbuffer_free(rb) >= 4096)
+
+static ssize_t ts_play(struct av7110 *av7110, const char __user *buf,
+		       unsigned long count, int nonblock, int type)
+{
+	struct dvb_ringbuffer *rb;
+	u8 *kb;
+	unsigned long todo = count;
+
+	dprintk(2, "%s: type %d cnt %lu\n", __func__, type, count);
+
+	rb = (type) ? &av7110->avout : &av7110->aout;
+	kb = av7110->kbuf[type];
+
+	if (!kb)
+		return -ENOBUFS;
+
+	if (nonblock && !FREE_COND_TS)
+		return -EWOULDBLOCK;
+
+	while (todo >= TS_SIZE) {
+		if (!FREE_COND_TS) {
+			if (nonblock)
+				return count - todo;
+			if (wait_event_interruptible(rb->queue, FREE_COND_TS))
+				return count - todo;
+		}
+		if (copy_from_user(kb, buf, TS_SIZE))
+			return -EFAULT;
+		write_ts_to_decoder(av7110, type, kb, TS_SIZE);
+		todo -= TS_SIZE;
+		buf += TS_SIZE;
+	}
+
+	return count - todo;
+}
+
+
+#define FREE_COND (dvb_ringbuffer_free(&av7110->avout) >= 20 * 1024 && \
+		   dvb_ringbuffer_free(&av7110->aout) >= 20 * 1024)
+
+static ssize_t dvb_play(struct av7110 *av7110, const char __user *buf,
+			unsigned long count, int nonblock, int type)
+{
+	unsigned long todo = count, n;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (!av7110->kbuf[type])
+		return -ENOBUFS;
+
+	if (nonblock && !FREE_COND)
+		return -EWOULDBLOCK;
+
+	while (todo > 0) {
+		if (!FREE_COND) {
+			if (nonblock)
+				return count - todo;
+			if (wait_event_interruptible(av7110->avout.queue,
+						     FREE_COND))
+				return count - todo;
+		}
+		n = todo;
+		if (n > IPACKS * 2)
+			n = IPACKS * 2;
+		if (copy_from_user(av7110->kbuf[type], buf, n))
+			return -EFAULT;
+		av7110_ipack_instant_repack(av7110->kbuf[type], n,
+					    &av7110->ipack[type]);
+		todo -= n;
+		buf += n;
+	}
+	return count - todo;
+}
+
+static ssize_t dvb_play_kernel(struct av7110 *av7110, const u8 *buf,
+			unsigned long count, int nonblock, int type)
+{
+	unsigned long todo = count, n;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (!av7110->kbuf[type])
+		return -ENOBUFS;
+
+	if (nonblock && !FREE_COND)
+		return -EWOULDBLOCK;
+
+	while (todo > 0) {
+		if (!FREE_COND) {
+			if (nonblock)
+				return count - todo;
+			if (wait_event_interruptible(av7110->avout.queue,
+						     FREE_COND))
+				return count - todo;
+		}
+		n = todo;
+		if (n > IPACKS * 2)
+			n = IPACKS * 2;
+		av7110_ipack_instant_repack(buf, n, &av7110->ipack[type]);
+		todo -= n;
+		buf += n;
+	}
+	return count - todo;
+}
+
+static ssize_t dvb_aplay(struct av7110 *av7110, const char __user *buf,
+			 unsigned long count, int nonblock, int type)
+{
+	unsigned long todo = count, n;
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (!av7110->kbuf[type])
+		return -ENOBUFS;
+	if (nonblock && dvb_ringbuffer_free(&av7110->aout) < 20 * 1024)
+		return -EWOULDBLOCK;
+
+	while (todo > 0) {
+		if (dvb_ringbuffer_free(&av7110->aout) < 20 * 1024) {
+			if (nonblock)
+				return count - todo;
+			if (wait_event_interruptible(av7110->aout.queue,
+					(dvb_ringbuffer_free(&av7110->aout) >= 20 * 1024)))
+				return count-todo;
+		}
+		n = todo;
+		if (n > IPACKS * 2)
+			n = IPACKS * 2;
+		if (copy_from_user(av7110->kbuf[type], buf, n))
+			return -EFAULT;
+		av7110_ipack_instant_repack(av7110->kbuf[type], n,
+					    &av7110->ipack[type]);
+		todo -= n;
+		buf += n;
+	}
+	return count - todo;
+}
+
+void av7110_p2t_init(struct av7110_p2t *p, struct dvb_demux_feed *feed)
+{
+	memset(p->pes, 0, TS_SIZE);
+	p->counter = 0;
+	p->pos = 0;
+	p->frags = 0;
+	if (feed)
+		p->feed = feed;
+}
+
+static void clear_p2t(struct av7110_p2t *p)
+{
+	memset(p->pes, 0, TS_SIZE);
+//	p->counter = 0;
+	p->pos = 0;
+	p->frags = 0;
+}
+
+
+static int find_pes_header(u8 const *buf, long int length, int *frags)
+{
+	int c = 0;
+	int found = 0;
+
+	*frags = 0;
+
+	while (c < length - 3 && !found) {
+		if (buf[c] == 0x00 && buf[c + 1] == 0x00 &&
+		    buf[c + 2] == 0x01) {
+			switch ( buf[c + 3] ) {
+			case PROG_STREAM_MAP:
+			case PRIVATE_STREAM2:
+			case PROG_STREAM_DIR:
+			case ECM_STREAM:
+			case EMM_STREAM:
+			case PADDING_STREAM:
+			case DSM_CC_STREAM:
+			case ISO13522_STREAM:
+			case PRIVATE_STREAM1:
+			case AUDIO_STREAM_S ... AUDIO_STREAM_E:
+			case VIDEO_STREAM_S ... VIDEO_STREAM_E:
+				found = 1;
+				break;
+
+			default:
+				c++;
+				break;
+			}
+		} else
+			c++;
+	}
+	if (c == length - 3 && !found) {
+		if (buf[length - 1] == 0x00)
+			*frags = 1;
+		if (buf[length - 2] == 0x00 &&
+		    buf[length - 1] == 0x00)
+			*frags = 2;
+		if (buf[length - 3] == 0x00 &&
+		    buf[length - 2] == 0x00 &&
+		    buf[length - 1] == 0x01)
+			*frags = 3;
+		return -1;
+	}
+
+	return c;
+}
+
+void av7110_p2t_write(u8 const *buf, long int length, u16 pid, struct av7110_p2t *p)
+{
+	int c, c2, l, add;
+	int check, rest;
+
+	c = 0;
+	c2 = 0;
+	if (p->frags){
+		check = 0;
+		switch(p->frags) {
+		case 1:
+			if (buf[c] == 0x00 && buf[c + 1] == 0x01) {
+				check = 1;
+				c += 2;
+			}
+			break;
+		case 2:
+			if (buf[c] == 0x01) {
+				check = 1;
+				c++;
+			}
+			break;
+		case 3:
+			check = 1;
+		}
+		if (check) {
+			switch (buf[c]) {
+			case PROG_STREAM_MAP:
+			case PRIVATE_STREAM2:
+			case PROG_STREAM_DIR:
+			case ECM_STREAM:
+			case EMM_STREAM:
+			case PADDING_STREAM:
+			case DSM_CC_STREAM:
+			case ISO13522_STREAM:
+			case PRIVATE_STREAM1:
+			case AUDIO_STREAM_S ... AUDIO_STREAM_E:
+			case VIDEO_STREAM_S ... VIDEO_STREAM_E:
+				p->pes[0] = 0x00;
+				p->pes[1] = 0x00;
+				p->pes[2] = 0x01;
+				p->pes[3] = buf[c];
+				p->pos = 4;
+				memcpy(p->pes + p->pos, buf + c, (TS_SIZE - 4) - p->pos);
+				c += (TS_SIZE - 4) - p->pos;
+				p_to_t(p->pes, (TS_SIZE - 4), pid, &p->counter, p->feed);
+				clear_p2t(p);
+				break;
+
+			default:
+				c = 0;
+				break;
+			}
+		}
+		p->frags = 0;
+	}
+
+	if (p->pos) {
+		c2 = find_pes_header(buf + c, length - c, &p->frags);
+		if (c2 >= 0 && c2 < (TS_SIZE - 4) - p->pos)
+			l = c2+c;
+		else
+			l = (TS_SIZE - 4) - p->pos;
+		memcpy(p->pes + p->pos, buf, l);
+		c += l;
+		p->pos += l;
+		p_to_t(p->pes, p->pos, pid, &p->counter, p->feed);
+		clear_p2t(p);
+	}
+
+	add = 0;
+	while (c < length) {
+		c2 = find_pes_header(buf + c + add, length - c - add, &p->frags);
+		if (c2 >= 0) {
+			c2 += c + add;
+			if (c2 > c){
+				p_to_t(buf + c, c2 - c, pid, &p->counter, p->feed);
+				c = c2;
+				clear_p2t(p);
+				add = 0;
+			} else
+				add = 1;
+		} else {
+			l = length - c;
+			rest = l % (TS_SIZE - 4);
+			l -= rest;
+			p_to_t(buf + c, l, pid, &p->counter, p->feed);
+			memcpy(p->pes, buf + c + l, rest);
+			p->pos = rest;
+			c = length;
+		}
+	}
+}
+
+
+static int write_ts_header2(u16 pid, u8 *counter, int pes_start, u8 *buf, u8 length)
+{
+	int i;
+	int c = 0;
+	int fill;
+	u8 tshead[4] = { 0x47, 0x00, 0x00, 0x10 };
+
+	fill = (TS_SIZE - 4) - length;
+	if (pes_start)
+		tshead[1] = 0x40;
+	if (fill)
+		tshead[3] = 0x30;
+	tshead[1] |= (u8)((pid & 0x1F00) >> 8);
+	tshead[2] |= (u8)(pid & 0x00FF);
+	tshead[3] |= ((*counter)++ & 0x0F);
+	memcpy(buf, tshead, 4);
+	c += 4;
+
+	if (fill) {
+		buf[4] = fill - 1;
+		c++;
+		if (fill > 1) {
+			buf[5] = 0x00;
+			c++;
+		}
+		for (i = 6; i < fill + 4; i++) {
+			buf[i] = 0xFF;
+			c++;
+		}
+	}
+
+	return c;
+}
+
+
+static void p_to_t(u8 const *buf, long int length, u16 pid, u8 *counter,
+		   struct dvb_demux_feed *feed)
+{
+	int l, pes_start;
+	u8 obuf[TS_SIZE];
+	long c = 0;
+
+	pes_start = 0;
+	if (length > 3 &&
+	     buf[0] == 0x00 && buf[1] == 0x00 && buf[2] == 0x01)
+		switch (buf[3]) {
+		case PROG_STREAM_MAP:
+		case PRIVATE_STREAM2:
+		case PROG_STREAM_DIR:
+		case ECM_STREAM:
+		case EMM_STREAM:
+		case PADDING_STREAM:
+		case DSM_CC_STREAM:
+		case ISO13522_STREAM:
+		case PRIVATE_STREAM1:
+		case AUDIO_STREAM_S ... AUDIO_STREAM_E:
+		case VIDEO_STREAM_S ... VIDEO_STREAM_E:
+			pes_start = 1;
+			break;
+
+		default:
+			break;
+		}
+
+	while (c < length) {
+		memset(obuf, 0, TS_SIZE);
+		if (length - c >= (TS_SIZE - 4)){
+			l = write_ts_header2(pid, counter, pes_start,
+					     obuf, (TS_SIZE - 4));
+			memcpy(obuf + l, buf + c, TS_SIZE - l);
+			c += TS_SIZE - l;
+		} else {
+			l = write_ts_header2(pid, counter, pes_start,
+					     obuf, length - c);
+			memcpy(obuf + l, buf + c, TS_SIZE - l);
+			c = length;
+		}
+		feed->cb.ts(obuf, 188, NULL, 0, &feed->feed.ts, NULL);
+		pes_start = 0;
+	}
+}
+
+
+static int write_ts_to_decoder(struct av7110 *av7110, int type, const u8 *buf, size_t len)
+{
+	struct ipack *ipack = &av7110->ipack[type];
+
+	if (buf[1] & TRANS_ERROR) {
+		av7110_ipack_reset(ipack);
+		return -1;
+	}
+
+	if (!(buf[3] & PAYLOAD))
+		return -1;
+
+	if (buf[1] & PAY_START)
+		av7110_ipack_flush(ipack);
+
+	if (buf[3] & ADAPT_FIELD) {
+		if (buf[4] > len - 1 - 4)
+			return 0;
+		len -= buf[4] + 1;
+		buf += buf[4] + 1;
+	}
+
+	av7110_ipack_instant_repack(buf + 4, len - 4, ipack);
+	return 0;
+}
+
+
+int av7110_write_to_decoder(struct dvb_demux_feed *feed, const u8 *buf, size_t len)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct av7110 *av7110 = (struct av7110 *) demux->priv;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (av7110->full_ts && demux->dmx.frontend->source != DMX_MEMORY_FE)
+		return 0;
+
+	switch (feed->pes_type) {
+	case 0:
+		if (av7110->audiostate.stream_source == AUDIO_SOURCE_MEMORY)
+			return -EINVAL;
+		break;
+	case 1:
+		if (av7110->videostate.stream_source == VIDEO_SOURCE_MEMORY)
+			return -EINVAL;
+		break;
+	default:
+		return -1;
+	}
+
+	return write_ts_to_decoder(av7110, feed->pes_type, buf, len);
+}
+
+
+
+/******************************************************************************
+ * Video MPEG decoder events
+ ******************************************************************************/
+void dvb_video_add_event(struct av7110 *av7110, struct video_event *event)
+{
+	struct dvb_video_events *events = &av7110->video_events;
+	int wp;
+
+	spin_lock_bh(&events->lock);
+
+	wp = (events->eventw + 1) % MAX_VIDEO_EVENT;
+	if (wp == events->eventr) {
+		events->overflow = 1;
+		events->eventr = (events->eventr + 1) % MAX_VIDEO_EVENT;
+	}
+
+	//FIXME: timestamp?
+	memcpy(&events->events[events->eventw], event, sizeof(struct video_event));
+	events->eventw = wp;
+
+	spin_unlock_bh(&events->lock);
+
+	wake_up_interruptible(&events->wait_queue);
+}
+
+
+static int dvb_video_get_event (struct av7110 *av7110, struct video_event *event, int flags)
+{
+	struct dvb_video_events *events = &av7110->video_events;
+
+	if (events->overflow) {
+		events->overflow = 0;
+		return -EOVERFLOW;
+	}
+	if (events->eventw == events->eventr) {
+		int ret;
+
+		if (flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+
+		ret = wait_event_interruptible(events->wait_queue,
+					       events->eventw != events->eventr);
+		if (ret < 0)
+			return ret;
+	}
+
+	spin_lock_bh(&events->lock);
+
+	memcpy(event, &events->events[events->eventr],
+	       sizeof(struct video_event));
+	events->eventr = (events->eventr + 1) % MAX_VIDEO_EVENT;
+
+	spin_unlock_bh(&events->lock);
+
+	return 0;
+}
+
+/******************************************************************************
+ * DVB device file operations
+ ******************************************************************************/
+
+static __poll_t dvb_video_poll(struct file *file, poll_table *wait)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	__poll_t mask = 0;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if ((file->f_flags & O_ACCMODE) != O_RDONLY)
+		poll_wait(file, &av7110->avout.queue, wait);
+
+	poll_wait(file, &av7110->video_events.wait_queue, wait);
+
+	if (av7110->video_events.eventw != av7110->video_events.eventr)
+		mask = EPOLLPRI;
+
+	if ((file->f_flags & O_ACCMODE) != O_RDONLY) {
+		if (av7110->playing) {
+			if (FREE_COND)
+				mask |= (EPOLLOUT | EPOLLWRNORM);
+		} else {
+			/* if not playing: may play if asked for */
+			mask |= (EPOLLOUT | EPOLLWRNORM);
+		}
+	}
+
+	return mask;
+}
+
+static ssize_t dvb_video_write(struct file *file, const char __user *buf,
+			       size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	unsigned char c;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if ((file->f_flags & O_ACCMODE) == O_RDONLY)
+		return -EPERM;
+
+	if (av7110->videostate.stream_source != VIDEO_SOURCE_MEMORY)
+		return -EPERM;
+
+	if (get_user(c, buf))
+		return -EFAULT;
+	if (c == 0x47 && count % TS_SIZE == 0)
+		return ts_play(av7110, buf, count, file->f_flags & O_NONBLOCK, 1);
+	else
+		return dvb_play(av7110, buf, count, file->f_flags & O_NONBLOCK, 1);
+}
+
+static __poll_t dvb_audio_poll(struct file *file, poll_table *wait)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	__poll_t mask = 0;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	poll_wait(file, &av7110->aout.queue, wait);
+
+	if (av7110->playing) {
+		if (dvb_ringbuffer_free(&av7110->aout) >= 20 * 1024)
+			mask |= (EPOLLOUT | EPOLLWRNORM);
+	} else /* if not playing: may play if asked for */
+		mask = (EPOLLOUT | EPOLLWRNORM);
+
+	return mask;
+}
+
+static ssize_t dvb_audio_write(struct file *file, const char __user *buf,
+			       size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	unsigned char c;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (av7110->audiostate.stream_source != AUDIO_SOURCE_MEMORY) {
+		printk(KERN_ERR "not audio source memory\n");
+		return -EPERM;
+	}
+
+	if (get_user(c, buf))
+		return -EFAULT;
+	if (c == 0x47 && count % TS_SIZE == 0)
+		return ts_play(av7110, buf, count, file->f_flags & O_NONBLOCK, 0);
+	else
+		return dvb_aplay(av7110, buf, count, file->f_flags & O_NONBLOCK, 0);
+}
+
+static u8 iframe_header[] = { 0x00, 0x00, 0x01, 0xe0, 0x00, 0x00, 0x80, 0x00, 0x00 };
+
+#define MIN_IFRAME 400000
+
+static int play_iframe(struct av7110 *av7110, char __user *buf, unsigned int len, int nonblock)
+{
+	unsigned i, n;
+	int progressive = 0;
+	int match = 0;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (len == 0)
+		return 0;
+
+	if (!(av7110->playing & RP_VIDEO)) {
+		if (av7110_av_start_play(av7110, RP_VIDEO) < 0)
+			return -EBUSY;
+	}
+
+	/* search in buf for instances of 00 00 01 b5 1? */
+	for (i = 0; i < len; i++) {
+		unsigned char c;
+		if (get_user(c, buf + i))
+			return -EFAULT;
+		if (match == 5) {
+			progressive = c & 0x08;
+			match = 0;
+		}
+		if (c == 0x00) {
+			match = (match == 1 || match == 2) ? 2 : 1;
+			continue;
+		}
+		switch (match++) {
+		case 2: if (c == 0x01)
+				continue;
+			break;
+		case 3: if (c == 0xb5)
+				continue;
+			break;
+		case 4: if ((c & 0xf0) == 0x10)
+				continue;
+			break;
+		}
+		match = 0;
+	}
+
+	/* setting n always > 1, fixes problems when playing stillframes
+	   consisting of I- and P-Frames */
+	n = MIN_IFRAME / len + 1;
+
+	/* FIXME: nonblock? */
+	dvb_play_kernel(av7110, iframe_header, sizeof(iframe_header), 0, 1);
+
+	for (i = 0; i < n; i++)
+		dvb_play(av7110, buf, len, 0, 1);
+
+	av7110_ipack_flush(&av7110->ipack[1]);
+
+	if (progressive)
+		return vidcom(av7110, AV_VIDEO_CMD_FREEZE, 1);
+	else
+		return 0;
+}
+
+#ifdef CONFIG_COMPAT
+struct compat_video_still_picture {
+	compat_uptr_t iFrame;
+	int32_t size;
+};
+#define VIDEO_STILLPICTURE32 _IOW('o', 30, struct compat_video_still_picture)
+
+struct compat_video_event {
+	__s32 type;
+	/* unused, make sure to use atomic time for y2038 if it ever gets used */
+	compat_long_t timestamp;
+	union {
+		video_size_t size;
+		unsigned int frame_rate;        /* in frames per 1000sec */
+		unsigned char vsync_field;      /* unknown/odd/even/progressive */
+	} u;
+};
+#define VIDEO_GET_EVENT32 _IOR('o', 28, struct compat_video_event)
+
+static int dvb_compat_video_get_event(struct av7110 *av7110,
+				      struct compat_video_event *event, int flags)
+{
+	struct video_event ev;
+	int ret;
+
+	ret = dvb_video_get_event(av7110, &ev, flags);
+
+	*event = (struct compat_video_event) {
+		.type = ev.type,
+		.timestamp = ev.timestamp,
+		.u.size = ev.u.size,
+	};
+
+	return ret;
+}
+#endif
+
+static int dvb_video_ioctl(struct file *file,
+			   unsigned int cmd, void *parg)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	unsigned long arg = (unsigned long) parg;
+	int ret = 0;
+
+	dprintk(1, "av7110:%p, cmd=%04x\n", av7110,cmd);
+
+	if ((file->f_flags & O_ACCMODE) == O_RDONLY) {
+		if ( cmd != VIDEO_GET_STATUS && cmd != VIDEO_GET_EVENT &&
+		     cmd != VIDEO_GET_SIZE ) {
+			return -EPERM;
+		}
+	}
+
+	if (mutex_lock_interruptible(&av7110->ioctl_mutex))
+		return -ERESTARTSYS;
+
+	switch (cmd) {
+	case VIDEO_STOP:
+		av7110->videostate.play_state = VIDEO_STOPPED;
+		if (av7110->videostate.stream_source == VIDEO_SOURCE_MEMORY)
+			ret = av7110_av_stop(av7110, RP_VIDEO);
+		else
+			ret = vidcom(av7110, AV_VIDEO_CMD_STOP,
+			       av7110->videostate.video_blank ? 0 : 1);
+		if (!ret)
+			av7110->trickmode = TRICK_NONE;
+		break;
+
+	case VIDEO_PLAY:
+		av7110->trickmode = TRICK_NONE;
+		if (av7110->videostate.play_state == VIDEO_FREEZED) {
+			av7110->videostate.play_state = VIDEO_PLAYING;
+			ret = vidcom(av7110, AV_VIDEO_CMD_PLAY, 0);
+			if (ret)
+				break;
+		}
+		if (av7110->videostate.stream_source == VIDEO_SOURCE_MEMORY) {
+			if (av7110->playing == RP_AV) {
+				ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Stop, 0);
+				if (ret)
+					break;
+				av7110->playing &= ~RP_VIDEO;
+			}
+			ret = av7110_av_start_play(av7110, RP_VIDEO);
+		}
+		if (!ret)
+			ret = vidcom(av7110, AV_VIDEO_CMD_PLAY, 0);
+		if (!ret)
+			av7110->videostate.play_state = VIDEO_PLAYING;
+		break;
+
+	case VIDEO_FREEZE:
+		av7110->videostate.play_state = VIDEO_FREEZED;
+		if (av7110->playing & RP_VIDEO)
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Pause, 0);
+		else
+			ret = vidcom(av7110, AV_VIDEO_CMD_FREEZE, 1);
+		if (!ret)
+			av7110->trickmode = TRICK_FREEZE;
+		break;
+
+	case VIDEO_CONTINUE:
+		if (av7110->playing & RP_VIDEO)
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Continue, 0);
+		if (!ret)
+			ret = vidcom(av7110, AV_VIDEO_CMD_PLAY, 0);
+		if (!ret) {
+			av7110->videostate.play_state = VIDEO_PLAYING;
+			av7110->trickmode = TRICK_NONE;
+		}
+		break;
+
+	case VIDEO_SELECT_SOURCE:
+		av7110->videostate.stream_source = (video_stream_source_t) arg;
+		break;
+
+	case VIDEO_SET_BLANK:
+		av7110->videostate.video_blank = (int) arg;
+		break;
+
+	case VIDEO_GET_STATUS:
+		memcpy(parg, &av7110->videostate, sizeof(struct video_status));
+		break;
+
+#ifdef CONFIG_COMPAT
+	case VIDEO_GET_EVENT32:
+		ret = dvb_compat_video_get_event(av7110, parg, file->f_flags);
+		break;
+#endif
+
+	case VIDEO_GET_EVENT:
+		ret = dvb_video_get_event(av7110, parg, file->f_flags);
+		break;
+
+	case VIDEO_GET_SIZE:
+		memcpy(parg, &av7110->video_size, sizeof(video_size_t));
+		break;
+
+	case VIDEO_SET_DISPLAY_FORMAT:
+	{
+		video_displayformat_t format = (video_displayformat_t) arg;
+		switch (format) {
+		case VIDEO_PAN_SCAN:
+			av7110->display_panscan = VID_PAN_SCAN_PREF;
+			break;
+		case VIDEO_LETTER_BOX:
+			av7110->display_panscan = VID_VC_AND_PS_PREF;
+			break;
+		case VIDEO_CENTER_CUT_OUT:
+			av7110->display_panscan = VID_CENTRE_CUT_PREF;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+		if (ret < 0)
+			break;
+		av7110->videostate.display_format = format;
+		ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetPanScanType,
+				    1, av7110->display_panscan);
+		break;
+	}
+
+	case VIDEO_SET_FORMAT:
+		if (arg > 1) {
+			ret = -EINVAL;
+			break;
+		}
+		av7110->display_ar = arg;
+		ret = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetMonitorType,
+				    1, (u16) arg);
+		break;
+
+#ifdef CONFIG_COMPAT
+	case VIDEO_STILLPICTURE32:
+	{
+		struct compat_video_still_picture *pic =
+			(struct compat_video_still_picture *) parg;
+		av7110->videostate.stream_source = VIDEO_SOURCE_MEMORY;
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->avout);
+		ret = play_iframe(av7110, compat_ptr(pic->iFrame),
+				  pic->size, file->f_flags & O_NONBLOCK);
+		break;
+	}
+#endif
+
+	case VIDEO_STILLPICTURE:
+	{
+		struct video_still_picture *pic =
+			(struct video_still_picture *) parg;
+		av7110->videostate.stream_source = VIDEO_SOURCE_MEMORY;
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->avout);
+		ret = play_iframe(av7110, pic->iFrame, pic->size,
+				  file->f_flags & O_NONBLOCK);
+		break;
+	}
+
+	case VIDEO_FAST_FORWARD:
+		//note: arg is ignored by firmware
+		if (av7110->playing & RP_VIDEO)
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY,
+					    __Scan_I, 2, AV_PES, 0);
+		else
+			ret = vidcom(av7110, AV_VIDEO_CMD_FFWD, arg);
+		if (!ret) {
+			av7110->trickmode = TRICK_FAST;
+			av7110->videostate.play_state = VIDEO_PLAYING;
+		}
+		break;
+
+	case VIDEO_SLOWMOTION:
+		if (av7110->playing&RP_VIDEO) {
+			if (av7110->trickmode != TRICK_SLOW)
+				ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY, __Slow, 2, 0, 0);
+			if (!ret)
+				ret = vidcom(av7110, AV_VIDEO_CMD_SLOW, arg);
+		} else {
+			ret = vidcom(av7110, AV_VIDEO_CMD_PLAY, 0);
+			if (!ret)
+				ret = vidcom(av7110, AV_VIDEO_CMD_STOP, 0);
+			if (!ret)
+				ret = vidcom(av7110, AV_VIDEO_CMD_SLOW, arg);
+		}
+		if (!ret) {
+			av7110->trickmode = TRICK_SLOW;
+			av7110->videostate.play_state = VIDEO_PLAYING;
+		}
+		break;
+
+	case VIDEO_GET_CAPABILITIES:
+		*(int *)parg = VIDEO_CAP_MPEG1 | VIDEO_CAP_MPEG2 |
+			VIDEO_CAP_SYS | VIDEO_CAP_PROG;
+		break;
+
+	case VIDEO_CLEAR_BUFFER:
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->avout);
+		av7110_ipack_reset(&av7110->ipack[1]);
+		if (av7110->playing == RP_AV) {
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY,
+					    __Play, 2, AV_PES, 0);
+			if (ret)
+				break;
+			if (av7110->trickmode == TRICK_FAST)
+				ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY,
+						    __Scan_I, 2, AV_PES, 0);
+			if (av7110->trickmode == TRICK_SLOW) {
+				ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY,
+						    __Slow, 2, 0, 0);
+				if (!ret)
+					ret = vidcom(av7110, AV_VIDEO_CMD_SLOW, arg);
+			}
+			if (av7110->trickmode == TRICK_FREEZE)
+				ret = vidcom(av7110, AV_VIDEO_CMD_STOP, 1);
+		}
+		break;
+
+	case VIDEO_SET_STREAMTYPE:
+		break;
+
+	default:
+		ret = -ENOIOCTLCMD;
+		break;
+	}
+
+	mutex_unlock(&av7110->ioctl_mutex);
+	return ret;
+}
+
+static int dvb_audio_ioctl(struct file *file,
+			   unsigned int cmd, void *parg)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	unsigned long arg = (unsigned long) parg;
+	int ret = 0;
+
+	dprintk(1, "av7110:%p, cmd=%04x\n", av7110,cmd);
+
+	if (((file->f_flags & O_ACCMODE) == O_RDONLY) &&
+	    (cmd != AUDIO_GET_STATUS))
+		return -EPERM;
+
+	if (mutex_lock_interruptible(&av7110->ioctl_mutex))
+		return -ERESTARTSYS;
+
+	switch (cmd) {
+	case AUDIO_STOP:
+		if (av7110->audiostate.stream_source == AUDIO_SOURCE_MEMORY)
+			ret = av7110_av_stop(av7110, RP_AUDIO);
+		else
+			ret = audcom(av7110, AUDIO_CMD_MUTE);
+		if (!ret)
+			av7110->audiostate.play_state = AUDIO_STOPPED;
+		break;
+
+	case AUDIO_PLAY:
+		if (av7110->audiostate.stream_source == AUDIO_SOURCE_MEMORY)
+			ret = av7110_av_start_play(av7110, RP_AUDIO);
+		if (!ret)
+			ret = audcom(av7110, AUDIO_CMD_UNMUTE);
+		if (!ret)
+			av7110->audiostate.play_state = AUDIO_PLAYING;
+		break;
+
+	case AUDIO_PAUSE:
+		ret = audcom(av7110, AUDIO_CMD_MUTE);
+		if (!ret)
+			av7110->audiostate.play_state = AUDIO_PAUSED;
+		break;
+
+	case AUDIO_CONTINUE:
+		if (av7110->audiostate.play_state == AUDIO_PAUSED) {
+			av7110->audiostate.play_state = AUDIO_PLAYING;
+			ret = audcom(av7110, AUDIO_CMD_UNMUTE | AUDIO_CMD_PCM16);
+		}
+		break;
+
+	case AUDIO_SELECT_SOURCE:
+		av7110->audiostate.stream_source = (audio_stream_source_t) arg;
+		break;
+
+	case AUDIO_SET_MUTE:
+	{
+		ret = audcom(av7110, arg ? AUDIO_CMD_MUTE : AUDIO_CMD_UNMUTE);
+		if (!ret)
+			av7110->audiostate.mute_state = (int) arg;
+		break;
+	}
+
+	case AUDIO_SET_AV_SYNC:
+		av7110->audiostate.AV_sync_state = (int) arg;
+		ret = audcom(av7110, arg ? AUDIO_CMD_SYNC_ON : AUDIO_CMD_SYNC_OFF);
+		break;
+
+	case AUDIO_SET_BYPASS_MODE:
+		if (FW_VERSION(av7110->arm_app) < 0x2621)
+			ret = -EINVAL;
+		av7110->audiostate.bypass_mode = (int)arg;
+		break;
+
+	case AUDIO_CHANNEL_SELECT:
+		av7110->audiostate.channel_select = (audio_channel_select_t) arg;
+		switch(av7110->audiostate.channel_select) {
+		case AUDIO_STEREO:
+			ret = audcom(av7110, AUDIO_CMD_STEREO);
+			if (!ret) {
+				if (av7110->adac_type == DVB_ADAC_CRYSTAL)
+					i2c_writereg(av7110, 0x20, 0x02, 0x49);
+				else if (av7110->adac_type == DVB_ADAC_MSP34x5)
+					msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0220);
+			}
+			break;
+		case AUDIO_MONO_LEFT:
+			ret = audcom(av7110, AUDIO_CMD_MONO_L);
+			if (!ret) {
+				if (av7110->adac_type == DVB_ADAC_CRYSTAL)
+					i2c_writereg(av7110, 0x20, 0x02, 0x4a);
+				else if (av7110->adac_type == DVB_ADAC_MSP34x5)
+					msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0200);
+			}
+			break;
+		case AUDIO_MONO_RIGHT:
+			ret = audcom(av7110, AUDIO_CMD_MONO_R);
+			if (!ret) {
+				if (av7110->adac_type == DVB_ADAC_CRYSTAL)
+					i2c_writereg(av7110, 0x20, 0x02, 0x45);
+				else if (av7110->adac_type == DVB_ADAC_MSP34x5)
+					msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0210);
+			}
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+
+	case AUDIO_GET_STATUS:
+		memcpy(parg, &av7110->audiostate, sizeof(struct audio_status));
+		break;
+
+	case AUDIO_GET_CAPABILITIES:
+		if (FW_VERSION(av7110->arm_app) < 0x2621)
+			*(unsigned int *)parg = AUDIO_CAP_LPCM | AUDIO_CAP_MP1 | AUDIO_CAP_MP2;
+		else
+			*(unsigned int *)parg = AUDIO_CAP_LPCM | AUDIO_CAP_DTS | AUDIO_CAP_AC3 |
+						AUDIO_CAP_MP1 | AUDIO_CAP_MP2;
+		break;
+
+	case AUDIO_CLEAR_BUFFER:
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->aout);
+		av7110_ipack_reset(&av7110->ipack[0]);
+		if (av7110->playing == RP_AV)
+			ret = av7110_fw_cmd(av7110, COMTYPE_REC_PLAY,
+					    __Play, 2, AV_PES, 0);
+		break;
+
+	case AUDIO_SET_ID:
+		break;
+
+	case AUDIO_SET_MIXER:
+	{
+		struct audio_mixer *amix = (struct audio_mixer *)parg;
+		ret = av7110_set_volume(av7110, amix->volume_left, amix->volume_right);
+		break;
+	}
+
+	case AUDIO_SET_STREAMTYPE:
+		break;
+
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	mutex_unlock(&av7110->ioctl_mutex);
+	return ret;
+}
+
+
+static int dvb_video_open(struct inode *inode, struct file *file)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	int err;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if ((err = dvb_generic_open(inode, file)) < 0)
+		return err;
+
+	if ((file->f_flags & O_ACCMODE) != O_RDONLY) {
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->aout);
+		dvb_ringbuffer_flush_spinlock_wakeup(&av7110->avout);
+		av7110->video_blank = 1;
+		av7110->audiostate.AV_sync_state = 1;
+		av7110->videostate.stream_source = VIDEO_SOURCE_DEMUX;
+
+		/*  empty event queue */
+		av7110->video_events.eventr = av7110->video_events.eventw = 0;
+	}
+
+	return 0;
+}
+
+static int dvb_video_release(struct inode *inode, struct file *file)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if ((file->f_flags & O_ACCMODE) != O_RDONLY) {
+		av7110_av_stop(av7110, RP_VIDEO);
+	}
+
+	return dvb_generic_release(inode, file);
+}
+
+static int dvb_audio_open(struct inode *inode, struct file *file)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	int err = dvb_generic_open(inode, file);
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	if (err < 0)
+		return err;
+	dvb_ringbuffer_flush_spinlock_wakeup(&av7110->aout);
+	av7110->audiostate.stream_source = AUDIO_SOURCE_DEMUX;
+	return 0;
+}
+
+static int dvb_audio_release(struct inode *inode, struct file *file)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+
+	dprintk(2, "av7110:%p, \n", av7110);
+
+	av7110_av_stop(av7110, RP_AUDIO);
+	return dvb_generic_release(inode, file);
+}
+
+
+
+/******************************************************************************
+ * driver registration
+ ******************************************************************************/
+
+static const struct file_operations dvb_video_fops = {
+	.owner		= THIS_MODULE,
+	.write		= dvb_video_write,
+	.unlocked_ioctl	= dvb_generic_ioctl,
+	.compat_ioctl	= dvb_generic_ioctl,
+	.open		= dvb_video_open,
+	.release	= dvb_video_release,
+	.poll		= dvb_video_poll,
+	.llseek		= noop_llseek,
+};
+
+static struct dvb_device dvbdev_video = {
+	.priv		= NULL,
+	.users		= 6,
+	.readers	= 5,	/* arbitrary */
+	.writers	= 1,
+	.fops		= &dvb_video_fops,
+	.kernel_ioctl	= dvb_video_ioctl,
+};
+
+static const struct file_operations dvb_audio_fops = {
+	.owner		= THIS_MODULE,
+	.write		= dvb_audio_write,
+	.unlocked_ioctl	= dvb_generic_ioctl,
+	.compat_ioctl	= dvb_generic_ioctl,
+	.open		= dvb_audio_open,
+	.release	= dvb_audio_release,
+	.poll		= dvb_audio_poll,
+	.llseek		= noop_llseek,
+};
+
+static struct dvb_device dvbdev_audio = {
+	.priv		= NULL,
+	.users		= 1,
+	.writers	= 1,
+	.fops		= &dvb_audio_fops,
+	.kernel_ioctl	= dvb_audio_ioctl,
+};
+
+
+int av7110_av_register(struct av7110 *av7110)
+{
+	av7110->audiostate.AV_sync_state = 0;
+	av7110->audiostate.mute_state = 0;
+	av7110->audiostate.play_state = AUDIO_STOPPED;
+	av7110->audiostate.stream_source = AUDIO_SOURCE_DEMUX;
+	av7110->audiostate.channel_select = AUDIO_STEREO;
+	av7110->audiostate.bypass_mode = 0;
+
+	av7110->videostate.video_blank = 0;
+	av7110->videostate.play_state = VIDEO_STOPPED;
+	av7110->videostate.stream_source = VIDEO_SOURCE_DEMUX;
+	av7110->videostate.video_format = VIDEO_FORMAT_4_3;
+	av7110->videostate.display_format = VIDEO_LETTER_BOX;
+	av7110->display_ar = VIDEO_FORMAT_4_3;
+	av7110->display_panscan = VID_VC_AND_PS_PREF;
+
+	init_waitqueue_head(&av7110->video_events.wait_queue);
+	spin_lock_init(&av7110->video_events.lock);
+	av7110->video_events.eventw = av7110->video_events.eventr = 0;
+	av7110->video_events.overflow = 0;
+	memset(&av7110->video_size, 0, sizeof (video_size_t));
+
+	dvb_register_device(&av7110->dvb_adapter, &av7110->video_dev,
+			    &dvbdev_video, av7110, DVB_DEVICE_VIDEO, 0);
+
+	dvb_register_device(&av7110->dvb_adapter, &av7110->audio_dev,
+			    &dvbdev_audio, av7110, DVB_DEVICE_AUDIO, 0);
+
+	return 0;
+}
+
+void av7110_av_unregister(struct av7110 *av7110)
+{
+	dvb_unregister_device(av7110->audio_dev);
+	dvb_unregister_device(av7110->video_dev);
+}
+
+int av7110_av_init(struct av7110 *av7110)
+{
+	void (*play[])(u8 *, int, void *) = { play_audio_cb, play_video_cb };
+	int i, ret;
+
+	for (i = 0; i < 2; i++) {
+		struct ipack *ipack = av7110->ipack + i;
+
+		ret = av7110_ipack_init(ipack, IPACKS, play[i]);
+		if (ret < 0) {
+			if (i)
+				av7110_ipack_free(--ipack);
+			goto out;
+		}
+		ipack->data = av7110;
+	}
+
+	dvb_ringbuffer_init(&av7110->avout, av7110->iobuf, AVOUTLEN);
+	dvb_ringbuffer_init(&av7110->aout, av7110->iobuf + AVOUTLEN, AOUTLEN);
+
+	av7110->kbuf[0] = (u8 *)(av7110->iobuf + AVOUTLEN + AOUTLEN + BMPLEN);
+	av7110->kbuf[1] = av7110->kbuf[0] + 2 * IPACKS;
+out:
+	return ret;
+}
+
+void av7110_av_exit(struct av7110 *av7110)
+{
+	av7110_ipack_free(&av7110->ipack[0]);
+	av7110_ipack_free(&av7110->ipack[1]);
+}
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.h b/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.h
new file mode 100644
index 000000000..71bbd4391
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_av.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _AV7110_AV_H_
+#define _AV7110_AV_H_
+
+struct av7110;
+
+extern int av7110_set_vidmode(struct av7110 *av7110,
+			      enum av7110_video_mode mode);
+
+extern int av7110_record_cb(struct dvb_filter_pes2ts *p2t, u8 *buf, size_t len);
+extern int av7110_pes_play(void *dest, struct dvb_ringbuffer *buf, int dlen);
+extern int av7110_write_to_decoder(struct dvb_demux_feed *feed, const u8 *buf, size_t len);
+
+extern int av7110_set_volume(struct av7110 *av7110, unsigned int volleft,
+			     unsigned int volright);
+extern int av7110_av_stop(struct av7110 *av7110, int av);
+extern int av7110_av_start_record(struct av7110 *av7110, int av,
+			  struct dvb_demux_feed *dvbdmxfeed);
+extern int av7110_av_start_play(struct av7110 *av7110, int av);
+
+extern void dvb_video_add_event(struct av7110 *av7110, struct video_event *event);
+
+extern void av7110_p2t_init(struct av7110_p2t *p, struct dvb_demux_feed *feed);
+extern void av7110_p2t_write(u8 const *buf, long int length, u16 pid, struct av7110_p2t *p);
+
+extern int av7110_av_register(struct av7110 *av7110);
+extern void av7110_av_unregister(struct av7110 *av7110);
+extern int av7110_av_init(struct av7110 *av7110);
+extern void av7110_av_exit(struct av7110 *av7110);
+
+
+#endif /* _AV7110_AV_H_ */
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.c
new file mode 100644
index 000000000..c1338e074
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.c
@@ -0,0 +1,380 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * av7110_ca.c: CA and CI stuff
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * originally based on code by:
+ * Copyright (C) 1998,1999 Christian Theiss <mistert@rz.fh-augsburg.de>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/poll.h>
+#include <linux/gfp.h>
+
+#include "av7110.h"
+#include "av7110_hw.h"
+#include "av7110_ca.h"
+
+
+void CI_handle(struct av7110 *av7110, u8 *data, u16 len)
+{
+	dprintk(8, "av7110:%p\n",av7110);
+
+	if (len < 3)
+		return;
+	switch (data[0]) {
+	case CI_MSG_CI_INFO:
+		if (data[2] != 1 && data[2] != 2)
+			break;
+		switch (data[1]) {
+		case 0:
+			av7110->ci_slot[data[2] - 1].flags = 0;
+			break;
+		case 1:
+			av7110->ci_slot[data[2] - 1].flags |= CA_CI_MODULE_PRESENT;
+			break;
+		case 2:
+			av7110->ci_slot[data[2] - 1].flags |= CA_CI_MODULE_READY;
+			break;
+		}
+		break;
+	case CI_SWITCH_PRG_REPLY:
+		//av7110->ci_stat=data[1];
+		break;
+	default:
+		break;
+	}
+}
+
+
+void ci_get_data(struct dvb_ringbuffer *cibuf, u8 *data, int len)
+{
+	if (dvb_ringbuffer_free(cibuf) < len + 2)
+		return;
+
+	DVB_RINGBUFFER_WRITE_BYTE(cibuf, len >> 8);
+	DVB_RINGBUFFER_WRITE_BYTE(cibuf, len & 0xff);
+	dvb_ringbuffer_write(cibuf, data, len);
+	wake_up_interruptible(&cibuf->queue);
+}
+
+
+/******************************************************************************
+ * CI link layer file ops
+ ******************************************************************************/
+
+static int ci_ll_init(struct dvb_ringbuffer *cirbuf, struct dvb_ringbuffer *ciwbuf, int size)
+{
+	struct dvb_ringbuffer *tab[] = { cirbuf, ciwbuf, NULL }, **p;
+	void *data;
+
+	for (p = tab; *p; p++) {
+		data = vmalloc(size);
+		if (!data) {
+			while (p-- != tab) {
+				vfree(p[0]->data);
+				p[0]->data = NULL;
+			}
+			return -ENOMEM;
+		}
+		dvb_ringbuffer_init(*p, data, size);
+	}
+	return 0;
+}
+
+static void ci_ll_flush(struct dvb_ringbuffer *cirbuf, struct dvb_ringbuffer *ciwbuf)
+{
+	dvb_ringbuffer_flush_spinlock_wakeup(cirbuf);
+	dvb_ringbuffer_flush_spinlock_wakeup(ciwbuf);
+}
+
+static void ci_ll_release(struct dvb_ringbuffer *cirbuf, struct dvb_ringbuffer *ciwbuf)
+{
+	vfree(cirbuf->data);
+	cirbuf->data = NULL;
+	vfree(ciwbuf->data);
+	ciwbuf->data = NULL;
+}
+
+static int ci_ll_reset(struct dvb_ringbuffer *cibuf, struct file *file,
+		       int slots, struct ca_slot_info *slot)
+{
+	int i;
+	int len = 0;
+	u8 msg[8] = { 0x00, 0x06, 0x00, 0x00, 0xff, 0x02, 0x00, 0x00 };
+
+	for (i = 0; i < 2; i++) {
+		if (slots & (1 << i))
+			len += 8;
+	}
+
+	if (dvb_ringbuffer_free(cibuf) < len)
+		return -EBUSY;
+
+	for (i = 0; i < 2; i++) {
+		if (slots & (1 << i)) {
+			msg[2] = i;
+			dvb_ringbuffer_write(cibuf, msg, 8);
+			slot[i].flags = 0;
+		}
+	}
+
+	return 0;
+}
+
+static ssize_t ci_ll_write(struct dvb_ringbuffer *cibuf, struct file *file,
+			   const char __user *buf, size_t count, loff_t *ppos)
+{
+	int free;
+	int non_blocking = file->f_flags & O_NONBLOCK;
+	u8 *page = (u8 *)__get_free_page(GFP_USER);
+	int res;
+
+	if (!page)
+		return -ENOMEM;
+
+	res = -EINVAL;
+	if (count > 2048)
+		goto out;
+
+	res = -EFAULT;
+	if (copy_from_user(page, buf, count))
+		goto out;
+
+	free = dvb_ringbuffer_free(cibuf);
+	if (count + 2 > free) {
+		res = -EWOULDBLOCK;
+		if (non_blocking)
+			goto out;
+		res = -ERESTARTSYS;
+		if (wait_event_interruptible(cibuf->queue,
+					     (dvb_ringbuffer_free(cibuf) >= count + 2)))
+			goto out;
+	}
+
+	DVB_RINGBUFFER_WRITE_BYTE(cibuf, count >> 8);
+	DVB_RINGBUFFER_WRITE_BYTE(cibuf, count & 0xff);
+
+	res = dvb_ringbuffer_write(cibuf, page, count);
+out:
+	free_page((unsigned long)page);
+	return res;
+}
+
+static ssize_t ci_ll_read(struct dvb_ringbuffer *cibuf, struct file *file,
+			  char __user *buf, size_t count, loff_t *ppos)
+{
+	int avail;
+	int non_blocking = file->f_flags & O_NONBLOCK;
+	ssize_t len;
+
+	if (!cibuf->data || !count)
+		return 0;
+	if (non_blocking && (dvb_ringbuffer_empty(cibuf)))
+		return -EWOULDBLOCK;
+	if (wait_event_interruptible(cibuf->queue,
+				     !dvb_ringbuffer_empty(cibuf)))
+		return -ERESTARTSYS;
+	avail = dvb_ringbuffer_avail(cibuf);
+	if (avail < 4)
+		return 0;
+	len = DVB_RINGBUFFER_PEEK(cibuf, 0) << 8;
+	len |= DVB_RINGBUFFER_PEEK(cibuf, 1);
+	if (avail < len + 2 || count < len)
+		return -EINVAL;
+	DVB_RINGBUFFER_SKIP(cibuf, 2);
+
+	return dvb_ringbuffer_read_user(cibuf, buf, len);
+}
+
+static int dvb_ca_open(struct inode *inode, struct file *file)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	int err = dvb_generic_open(inode, file);
+
+	dprintk(8, "av7110:%p\n",av7110);
+
+	if (err < 0)
+		return err;
+	ci_ll_flush(&av7110->ci_rbuffer, &av7110->ci_wbuffer);
+	return 0;
+}
+
+static __poll_t dvb_ca_poll (struct file *file, poll_table *wait)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	struct dvb_ringbuffer *rbuf = &av7110->ci_rbuffer;
+	struct dvb_ringbuffer *wbuf = &av7110->ci_wbuffer;
+	__poll_t mask = 0;
+
+	dprintk(8, "av7110:%p\n",av7110);
+
+	poll_wait(file, &rbuf->queue, wait);
+	poll_wait(file, &wbuf->queue, wait);
+
+	if (!dvb_ringbuffer_empty(rbuf))
+		mask |= (EPOLLIN | EPOLLRDNORM);
+
+	if (dvb_ringbuffer_free(wbuf) > 1024)
+		mask |= (EPOLLOUT | EPOLLWRNORM);
+
+	return mask;
+}
+
+static int dvb_ca_ioctl(struct file *file, unsigned int cmd, void *parg)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+	unsigned long arg = (unsigned long) parg;
+	int ret = 0;
+
+	dprintk(8, "av7110:%p\n",av7110);
+
+	if (mutex_lock_interruptible(&av7110->ioctl_mutex))
+		return -ERESTARTSYS;
+
+	switch (cmd) {
+	case CA_RESET:
+		ret = ci_ll_reset(&av7110->ci_wbuffer, file, arg,
+				  &av7110->ci_slot[0]);
+		break;
+	case CA_GET_CAP:
+	{
+		struct ca_caps cap;
+
+		cap.slot_num = 2;
+		cap.slot_type = (FW_CI_LL_SUPPORT(av7110->arm_app) ?
+				 CA_CI_LINK : CA_CI) | CA_DESCR;
+		cap.descr_num = 16;
+		cap.descr_type = CA_ECD;
+		memcpy(parg, &cap, sizeof(cap));
+		break;
+	}
+
+	case CA_GET_SLOT_INFO:
+	{
+		struct ca_slot_info *info=(struct ca_slot_info *)parg;
+
+		if (info->num < 0 || info->num > 1) {
+			mutex_unlock(&av7110->ioctl_mutex);
+			return -EINVAL;
+		}
+		av7110->ci_slot[info->num].num = info->num;
+		av7110->ci_slot[info->num].type = FW_CI_LL_SUPPORT(av7110->arm_app) ?
+							CA_CI_LINK : CA_CI;
+		memcpy(info, &av7110->ci_slot[info->num], sizeof(struct ca_slot_info));
+		break;
+	}
+
+	case CA_GET_MSG:
+		break;
+
+	case CA_SEND_MSG:
+		break;
+
+	case CA_GET_DESCR_INFO:
+	{
+		struct ca_descr_info info;
+
+		info.num = 16;
+		info.type = CA_ECD;
+		memcpy(parg, &info, sizeof (info));
+		break;
+	}
+
+	case CA_SET_DESCR:
+	{
+		struct ca_descr *descr = (struct ca_descr*) parg;
+
+		if (descr->index >= 16 || descr->parity > 1) {
+			mutex_unlock(&av7110->ioctl_mutex);
+			return -EINVAL;
+		}
+		av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, SetDescr, 5,
+			      (descr->index<<8)|descr->parity,
+			      (descr->cw[0]<<8)|descr->cw[1],
+			      (descr->cw[2]<<8)|descr->cw[3],
+			      (descr->cw[4]<<8)|descr->cw[5],
+			      (descr->cw[6]<<8)|descr->cw[7]);
+		break;
+	}
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&av7110->ioctl_mutex);
+	return ret;
+}
+
+static ssize_t dvb_ca_write(struct file *file, const char __user *buf,
+			    size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+
+	dprintk(8, "av7110:%p\n",av7110);
+	return ci_ll_write(&av7110->ci_wbuffer, file, buf, count, ppos);
+}
+
+static ssize_t dvb_ca_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct av7110 *av7110 = dvbdev->priv;
+
+	dprintk(8, "av7110:%p\n",av7110);
+	return ci_ll_read(&av7110->ci_rbuffer, file, buf, count, ppos);
+}
+
+static const struct file_operations dvb_ca_fops = {
+	.owner		= THIS_MODULE,
+	.read		= dvb_ca_read,
+	.write		= dvb_ca_write,
+	.unlocked_ioctl	= dvb_generic_ioctl,
+	.open		= dvb_ca_open,
+	.release	= dvb_generic_release,
+	.poll		= dvb_ca_poll,
+	.llseek		= default_llseek,
+};
+
+static struct dvb_device dvbdev_ca = {
+	.priv		= NULL,
+	.users		= 1,
+	.writers	= 1,
+	.fops		= &dvb_ca_fops,
+	.kernel_ioctl	= dvb_ca_ioctl,
+};
+
+
+int av7110_ca_register(struct av7110 *av7110)
+{
+	return dvb_register_device(&av7110->dvb_adapter, &av7110->ca_dev,
+				   &dvbdev_ca, av7110, DVB_DEVICE_CA, 0);
+}
+
+void av7110_ca_unregister(struct av7110 *av7110)
+{
+	dvb_unregister_device(av7110->ca_dev);
+}
+
+int av7110_ca_init(struct av7110* av7110)
+{
+	return ci_ll_init(&av7110->ci_rbuffer, &av7110->ci_wbuffer, 8192);
+}
+
+void av7110_ca_exit(struct av7110* av7110)
+{
+	ci_ll_release(&av7110->ci_rbuffer, &av7110->ci_wbuffer);
+}
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.h b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.h
new file mode 100644
index 000000000..a6e3f2955
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ca.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _AV7110_CA_H_
+#define _AV7110_CA_H_
+
+struct av7110;
+
+extern void CI_handle(struct av7110 *av7110, u8 *data, u16 len);
+extern void ci_get_data(struct dvb_ringbuffer *cibuf, u8 *data, int len);
+
+extern int av7110_ca_register(struct av7110 *av7110);
+extern void av7110_ca_unregister(struct av7110 *av7110);
+extern int av7110_ca_init(struct av7110* av7110);
+extern void av7110_ca_exit(struct av7110* av7110);
+
+#endif /* _AV7110_CA_H_ */
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.c
new file mode 100644
index 000000000..93ca31e38
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.c
@@ -0,0 +1,1204 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * av7110_hw.c: av7110 low level hardware access and firmware interface
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * originally based on code by:
+ * Copyright (C) 1998,1999 Christian Theiss <mistert@rz.fh-augsburg.de>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+/* for debugging ARM communication: */
+//#define COM_DEBUG
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+
+#include "av7110.h"
+#include "av7110_hw.h"
+
+#define _NOHANDSHAKE
+
+/*
+ * Max transfer size done by av7110_fw_cmd()
+ *
+ * The maximum size passed to this function is 6 bytes. The buffer also
+ * uses two additional ones for type and size. So, 8 bytes is enough.
+ */
+#define MAX_XFER_SIZE  8
+
+/****************************************************************************
+ * DEBI functions
+ ****************************************************************************/
+
+/* This DEBI code is based on the Stradis driver
+   by Nathan Laredo <laredo@gnu.org> */
+
+int av7110_debiwrite(struct av7110 *av7110, u32 config,
+		     int addr, u32 val, unsigned int count)
+{
+	struct saa7146_dev *dev = av7110->dev;
+
+	if (count > 32764) {
+		printk("%s: invalid count %d\n", __func__, count);
+		return -1;
+	}
+	if (saa7146_wait_for_debi_done(av7110->dev, 0) < 0) {
+		printk("%s: wait_for_debi_done failed\n", __func__);
+		return -1;
+	}
+	saa7146_write(dev, DEBI_CONFIG, config);
+	if (count <= 4)		/* immediate transfer */
+		saa7146_write(dev, DEBI_AD, val);
+	else			/* block transfer */
+		saa7146_write(dev, DEBI_AD, av7110->debi_bus);
+	saa7146_write(dev, DEBI_COMMAND, (count << 17) | (addr & 0xffff));
+	saa7146_write(dev, MC2, (2 << 16) | 2);
+	return 0;
+}
+
+u32 av7110_debiread(struct av7110 *av7110, u32 config, int addr, unsigned int count)
+{
+	struct saa7146_dev *dev = av7110->dev;
+	u32 result = 0;
+
+	if (count > 32764) {
+		printk("%s: invalid count %d\n", __func__, count);
+		return 0;
+	}
+	if (saa7146_wait_for_debi_done(av7110->dev, 0) < 0) {
+		printk("%s: wait_for_debi_done #1 failed\n", __func__);
+		return 0;
+	}
+	saa7146_write(dev, DEBI_AD, av7110->debi_bus);
+	saa7146_write(dev, DEBI_COMMAND, (count << 17) | 0x10000 | (addr & 0xffff));
+
+	saa7146_write(dev, DEBI_CONFIG, config);
+	saa7146_write(dev, MC2, (2 << 16) | 2);
+	if (count > 4)
+		return count;
+	if (saa7146_wait_for_debi_done(av7110->dev, 0) < 0) {
+		printk("%s: wait_for_debi_done #2 failed\n", __func__);
+		return 0;
+	}
+
+	result = saa7146_read(dev, DEBI_AD);
+	result &= (0xffffffffUL >> ((4 - count) * 8));
+	return result;
+}
+
+
+
+/* av7110 ARM core boot stuff */
+#if 0
+void av7110_reset_arm(struct av7110 *av7110)
+{
+	saa7146_setgpio(av7110->dev, RESET_LINE, SAA7146_GPIO_OUTLO);
+
+	/* Disable DEBI and GPIO irq */
+	SAA7146_IER_DISABLE(av7110->dev, MASK_19 | MASK_03);
+	SAA7146_ISR_CLEAR(av7110->dev, MASK_19 | MASK_03);
+
+	saa7146_setgpio(av7110->dev, RESET_LINE, SAA7146_GPIO_OUTHI);
+	msleep(30);	/* the firmware needs some time to initialize */
+
+	ARM_ResetMailBox(av7110);
+
+	SAA7146_ISR_CLEAR(av7110->dev, MASK_19 | MASK_03);
+	SAA7146_IER_ENABLE(av7110->dev, MASK_03);
+
+	av7110->arm_ready = 1;
+	dprintk(1, "reset ARM\n");
+}
+#endif  /*  0  */
+
+static int waitdebi(struct av7110 *av7110, int adr, int state)
+{
+	int k;
+
+	dprintk(4, "%p\n", av7110);
+
+	for (k = 0; k < 100; k++) {
+		if (irdebi(av7110, DEBINOSWAP, adr, 0, 2) == state)
+			return 0;
+		udelay(5);
+	}
+	return -ETIMEDOUT;
+}
+
+static int load_dram(struct av7110 *av7110, u32 *data, int len)
+{
+	int i;
+	int blocks, rest;
+	u32 base, bootblock = AV7110_BOOT_BLOCK;
+
+	dprintk(4, "%p\n", av7110);
+
+	blocks = len / AV7110_BOOT_MAX_SIZE;
+	rest = len % AV7110_BOOT_MAX_SIZE;
+	base = DRAM_START_CODE;
+
+	for (i = 0; i < blocks; i++) {
+		if (waitdebi(av7110, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_EMPTY) < 0) {
+			printk(KERN_ERR "dvb-ttpci: load_dram(): timeout at block %d\n", i);
+			return -ETIMEDOUT;
+		}
+		dprintk(4, "writing DRAM block %d\n", i);
+		mwdebi(av7110, DEBISWAB, bootblock,
+		       ((u8 *)data) + i * AV7110_BOOT_MAX_SIZE, AV7110_BOOT_MAX_SIZE);
+		bootblock ^= 0x1400;
+		iwdebi(av7110, DEBISWAB, AV7110_BOOT_BASE, swab32(base), 4);
+		iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_SIZE, AV7110_BOOT_MAX_SIZE, 2);
+		iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_FULL, 2);
+		base += AV7110_BOOT_MAX_SIZE;
+	}
+
+	if (rest > 0) {
+		if (waitdebi(av7110, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_EMPTY) < 0) {
+			printk(KERN_ERR "dvb-ttpci: load_dram(): timeout at last block\n");
+			return -ETIMEDOUT;
+		}
+		if (rest > 4)
+			mwdebi(av7110, DEBISWAB, bootblock,
+			       ((u8 *)data) + i * AV7110_BOOT_MAX_SIZE, rest);
+		else
+			mwdebi(av7110, DEBISWAB, bootblock,
+			       ((u8 *)data) + i * AV7110_BOOT_MAX_SIZE - 4, rest + 4);
+
+		iwdebi(av7110, DEBISWAB, AV7110_BOOT_BASE, swab32(base), 4);
+		iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_SIZE, rest, 2);
+		iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_FULL, 2);
+	}
+	if (waitdebi(av7110, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_EMPTY) < 0) {
+		printk(KERN_ERR "dvb-ttpci: load_dram(): timeout after last block\n");
+		return -ETIMEDOUT;
+	}
+	iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_SIZE, 0, 2);
+	iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_FULL, 2);
+	if (waitdebi(av7110, AV7110_BOOT_STATE, BOOTSTATE_AV7110_BOOT_COMPLETE) < 0) {
+		printk(KERN_ERR "dvb-ttpci: load_dram(): final handshake timeout\n");
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+
+/* we cannot write av7110 DRAM directly, so load a bootloader into
+ * the DPRAM which implements a simple boot protocol */
+int av7110_bootarm(struct av7110 *av7110)
+{
+	const struct firmware *fw;
+	const char *fw_name = "av7110/bootcode.bin";
+	struct saa7146_dev *dev = av7110->dev;
+	u32 ret;
+	int i;
+
+	dprintk(4, "%p\n", av7110);
+
+	av7110->arm_ready = 0;
+
+	saa7146_setgpio(dev, RESET_LINE, SAA7146_GPIO_OUTLO);
+
+	/* Disable DEBI and GPIO irq */
+	SAA7146_IER_DISABLE(av7110->dev, MASK_03 | MASK_19);
+	SAA7146_ISR_CLEAR(av7110->dev, MASK_19 | MASK_03);
+
+	/* enable DEBI */
+	saa7146_write(av7110->dev, MC1, 0x08800880);
+	saa7146_write(av7110->dev, DD1_STREAM_B, 0x00000000);
+	saa7146_write(av7110->dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	/* test DEBI */
+	iwdebi(av7110, DEBISWAP, DPRAM_BASE, 0x76543210, 4);
+	/* FIXME: Why does Nexus CA require 2x iwdebi for first init? */
+	iwdebi(av7110, DEBISWAP, DPRAM_BASE, 0x76543210, 4);
+
+	if ((ret=irdebi(av7110, DEBINOSWAP, DPRAM_BASE, 0, 4)) != 0x10325476) {
+		printk(KERN_ERR "dvb-ttpci: debi test in av7110_bootarm() failed: %08x != %08x (check your BIOS 'Plug&Play OS' settings)\n",
+		       ret, 0x10325476);
+		return -1;
+	}
+	for (i = 0; i < 8192; i += 4)
+		iwdebi(av7110, DEBISWAP, DPRAM_BASE + i, 0x00, 4);
+	dprintk(2, "debi test OK\n");
+
+	/* boot */
+	dprintk(1, "load boot code\n");
+	saa7146_setgpio(dev, ARM_IRQ_LINE, SAA7146_GPIO_IRQLO);
+	//saa7146_setgpio(dev, DEBI_DONE_LINE, SAA7146_GPIO_INPUT);
+	//saa7146_setgpio(dev, 3, SAA7146_GPIO_INPUT);
+
+	ret = request_firmware(&fw, fw_name, &dev->pci->dev);
+	if (ret) {
+		printk(KERN_ERR "dvb-ttpci: Failed to load firmware \"%s\"\n",
+			fw_name);
+		return ret;
+	}
+
+	mwdebi(av7110, DEBISWAB, DPRAM_BASE, fw->data, fw->size);
+	release_firmware(fw);
+	iwdebi(av7110, DEBINOSWAP, AV7110_BOOT_STATE, BOOTSTATE_BUFFER_FULL, 2);
+
+	if (saa7146_wait_for_debi_done(av7110->dev, 1)) {
+		printk(KERN_ERR "dvb-ttpci: av7110_bootarm(): saa7146_wait_for_debi_done() timed out\n");
+		return -ETIMEDOUT;
+	}
+	saa7146_setgpio(dev, RESET_LINE, SAA7146_GPIO_OUTHI);
+	mdelay(1);
+
+	dprintk(1, "load dram code\n");
+	if (load_dram(av7110, (u32 *)av7110->bin_root, av7110->size_root) < 0) {
+		printk(KERN_ERR "dvb-ttpci: av7110_bootarm(): load_dram() failed\n");
+		return -1;
+	}
+
+	saa7146_setgpio(dev, RESET_LINE, SAA7146_GPIO_OUTLO);
+	mdelay(1);
+
+	dprintk(1, "load dpram code\n");
+	mwdebi(av7110, DEBISWAB, DPRAM_BASE, av7110->bin_dpram, av7110->size_dpram);
+
+	if (saa7146_wait_for_debi_done(av7110->dev, 1)) {
+		printk(KERN_ERR "dvb-ttpci: av7110_bootarm(): saa7146_wait_for_debi_done() timed out after loading DRAM\n");
+		return -ETIMEDOUT;
+	}
+	saa7146_setgpio(dev, RESET_LINE, SAA7146_GPIO_OUTHI);
+	msleep(30);	/* the firmware needs some time to initialize */
+
+	//ARM_ClearIrq(av7110);
+	ARM_ResetMailBox(av7110);
+	SAA7146_ISR_CLEAR(av7110->dev, MASK_19 | MASK_03);
+	SAA7146_IER_ENABLE(av7110->dev, MASK_03);
+
+	av7110->arm_errors = 0;
+	av7110->arm_ready = 1;
+	return 0;
+}
+MODULE_FIRMWARE("av7110/bootcode.bin");
+
+/****************************************************************************
+ * DEBI command polling
+ ****************************************************************************/
+
+int av7110_wait_msgstate(struct av7110 *av7110, u16 flags)
+{
+	unsigned long start;
+	u32 stat;
+	int err;
+
+	if (FW_VERSION(av7110->arm_app) <= 0x261c) {
+		/* not supported by old firmware */
+		msleep(50);
+		return 0;
+	}
+
+	/* new firmware */
+	start = jiffies;
+	for (;;) {
+		err = time_after(jiffies, start + ARM_WAIT_FREE);
+		if (mutex_lock_interruptible(&av7110->dcomlock))
+			return -ERESTARTSYS;
+		stat = rdebi(av7110, DEBINOSWAP, MSGSTATE, 0, 2);
+		mutex_unlock(&av7110->dcomlock);
+		if ((stat & flags) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "%s: timeout waiting for MSGSTATE %04x\n",
+				__func__, stat & flags);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+	return 0;
+}
+
+static int __av7110_send_fw_cmd(struct av7110 *av7110, u16* buf, int length)
+{
+	int i;
+	unsigned long start;
+	char *type = NULL;
+	u16 flags[2] = {0, 0};
+	u32 stat;
+	int err;
+
+//	dprintk(4, "%p\n", av7110);
+
+	if (!av7110->arm_ready) {
+		dprintk(1, "arm not ready.\n");
+		return -ENXIO;
+	}
+
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_FREE);
+		if (rdebi(av7110, DEBINOSWAP, COMMAND, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "dvb-ttpci: %s(): timeout waiting for COMMAND idle\n", __func__);
+			av7110->arm_errors++;
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+
+	if (FW_VERSION(av7110->arm_app) <= 0x261f)
+		wdebi(av7110, DEBINOSWAP, COM_IF_LOCK, 0xffff, 2);
+
+#ifndef _NOHANDSHAKE
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_SHAKE);
+		if (rdebi(av7110, DEBINOSWAP, HANDSHAKE_REG, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "dvb-ttpci: %s(): timeout waiting for HANDSHAKE_REG\n", __func__);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+#endif
+
+	switch ((buf[0] >> 8) & 0xff) {
+	case COMTYPE_PIDFILTER:
+	case COMTYPE_ENCODER:
+	case COMTYPE_REC_PLAY:
+	case COMTYPE_MPEGDECODER:
+		type = "MSG";
+		flags[0] = GPMQOver;
+		flags[1] = GPMQFull;
+		break;
+	case COMTYPE_OSD:
+		type = "OSD";
+		flags[0] = OSDQOver;
+		flags[1] = OSDQFull;
+		break;
+	case COMTYPE_MISC:
+		if (FW_VERSION(av7110->arm_app) >= 0x261d) {
+			type = "MSG";
+			flags[0] = GPMQOver;
+			flags[1] = GPMQBusy;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (type != NULL) {
+		/* non-immediate COMMAND type */
+		start = jiffies;
+		for (;;) {
+			err = time_after(jiffies, start + ARM_WAIT_FREE);
+			stat = rdebi(av7110, DEBINOSWAP, MSGSTATE, 0, 2);
+			if (stat & flags[0]) {
+				printk(KERN_ERR "%s: %s QUEUE overflow\n",
+					__func__, type);
+				return -1;
+			}
+			if ((stat & flags[1]) == 0)
+				break;
+			if (err) {
+				printk(KERN_ERR "%s: timeout waiting on busy %s QUEUE\n",
+					__func__, type);
+				av7110->arm_errors++;
+				return -ETIMEDOUT;
+			}
+			msleep(1);
+		}
+	}
+
+	for (i = 2; i < length; i++)
+		wdebi(av7110, DEBINOSWAP, COMMAND + 2 * i, (u32) buf[i], 2);
+
+	if (length)
+		wdebi(av7110, DEBINOSWAP, COMMAND + 2, (u32) buf[1], 2);
+	else
+		wdebi(av7110, DEBINOSWAP, COMMAND + 2, 0, 2);
+
+	wdebi(av7110, DEBINOSWAP, COMMAND, (u32) buf[0], 2);
+
+	if (FW_VERSION(av7110->arm_app) <= 0x261f)
+		wdebi(av7110, DEBINOSWAP, COM_IF_LOCK, 0x0000, 2);
+
+#ifdef COM_DEBUG
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_FREE);
+		if (rdebi(av7110, DEBINOSWAP, COMMAND, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "dvb-ttpci: %s(): timeout waiting for COMMAND %d to complete\n",
+			       __func__, (buf[0] >> 8) & 0xff);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+
+	stat = rdebi(av7110, DEBINOSWAP, MSGSTATE, 0, 2);
+	if (stat & GPMQOver) {
+		printk(KERN_ERR "dvb-ttpci: %s(): GPMQOver\n", __func__);
+		return -ENOSPC;
+	}
+	else if (stat & OSDQOver) {
+		printk(KERN_ERR "dvb-ttpci: %s(): OSDQOver\n", __func__);
+		return -ENOSPC;
+	}
+#endif
+
+	return 0;
+}
+
+static int av7110_send_fw_cmd(struct av7110 *av7110, u16* buf, int length)
+{
+	int ret;
+
+//	dprintk(4, "%p\n", av7110);
+
+	if (!av7110->arm_ready) {
+		dprintk(1, "arm not ready.\n");
+		return -1;
+	}
+	if (mutex_lock_interruptible(&av7110->dcomlock))
+		return -ERESTARTSYS;
+
+	ret = __av7110_send_fw_cmd(av7110, buf, length);
+	mutex_unlock(&av7110->dcomlock);
+	if (ret && ret!=-ERESTARTSYS)
+		printk(KERN_ERR "dvb-ttpci: %s(): av7110_send_fw_cmd error %d\n",
+		       __func__, ret);
+	return ret;
+}
+
+int av7110_fw_cmd(struct av7110 *av7110, int type, int com, int num, ...)
+{
+	va_list args;
+	u16 buf[MAX_XFER_SIZE];
+	int i, ret;
+
+//	dprintk(4, "%p\n", av7110);
+
+	if (2 + num > ARRAY_SIZE(buf)) {
+		printk(KERN_WARNING
+		       "%s: %s len=%d is too big!\n",
+		       KBUILD_MODNAME, __func__, num);
+		return -EINVAL;
+	}
+
+	buf[0] = ((type << 8) | com);
+	buf[1] = num;
+
+	if (num) {
+		va_start(args, num);
+		for (i = 0; i < num; i++)
+			buf[i + 2] = va_arg(args, u32);
+		va_end(args);
+	}
+
+	ret = av7110_send_fw_cmd(av7110, buf, num + 2);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_ERR "dvb-ttpci: av7110_fw_cmd error %d\n", ret);
+	return ret;
+}
+
+#if 0
+int av7110_send_ci_cmd(struct av7110 *av7110, u8 subcom, u8 *buf, u8 len)
+{
+	int i, ret;
+	u16 cmd[18] = { ((COMTYPE_COMMON_IF << 8) + subcom),
+		16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	dprintk(4, "%p\n", av7110);
+
+	for(i = 0; i < len && i < 32; i++)
+	{
+		if(i % 2 == 0)
+			cmd[(i / 2) + 2] = (u16)(buf[i]) << 8;
+		else
+			cmd[(i / 2) + 2] |= buf[i];
+	}
+
+	ret = av7110_send_fw_cmd(av7110, cmd, 18);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_ERR "dvb-ttpci: av7110_send_ci_cmd error %d\n", ret);
+	return ret;
+}
+#endif  /*  0  */
+
+int av7110_fw_request(struct av7110 *av7110, u16 *request_buf,
+		      int request_buf_len, u16 *reply_buf, int reply_buf_len)
+{
+	int err;
+	s16 i;
+	unsigned long start;
+#ifdef COM_DEBUG
+	u32 stat;
+#endif
+
+	dprintk(4, "%p\n", av7110);
+
+	if (!av7110->arm_ready) {
+		dprintk(1, "arm not ready.\n");
+		return -1;
+	}
+
+	if (mutex_lock_interruptible(&av7110->dcomlock))
+		return -ERESTARTSYS;
+
+	if ((err = __av7110_send_fw_cmd(av7110, request_buf, request_buf_len)) < 0) {
+		mutex_unlock(&av7110->dcomlock);
+		printk(KERN_ERR "dvb-ttpci: av7110_fw_request error %d\n", err);
+		return err;
+	}
+
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_FREE);
+		if (rdebi(av7110, DEBINOSWAP, COMMAND, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "%s: timeout waiting for COMMAND to complete\n", __func__);
+			mutex_unlock(&av7110->dcomlock);
+			return -ETIMEDOUT;
+		}
+#ifdef _NOHANDSHAKE
+		msleep(1);
+#endif
+	}
+
+#ifndef _NOHANDSHAKE
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_SHAKE);
+		if (rdebi(av7110, DEBINOSWAP, HANDSHAKE_REG, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "%s: timeout waiting for HANDSHAKE_REG\n", __func__);
+			mutex_unlock(&av7110->dcomlock);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+#endif
+
+#ifdef COM_DEBUG
+	stat = rdebi(av7110, DEBINOSWAP, MSGSTATE, 0, 2);
+	if (stat & GPMQOver) {
+		printk(KERN_ERR "%s: GPMQOver\n", __func__);
+		mutex_unlock(&av7110->dcomlock);
+		return -1;
+	}
+	else if (stat & OSDQOver) {
+		printk(KERN_ERR "%s: OSDQOver\n", __func__);
+		mutex_unlock(&av7110->dcomlock);
+		return -1;
+	}
+#endif
+
+	for (i = 0; i < reply_buf_len; i++)
+		reply_buf[i] = rdebi(av7110, DEBINOSWAP, COM_BUFF + 2 * i, 0, 2);
+
+	mutex_unlock(&av7110->dcomlock);
+	return 0;
+}
+
+static int av7110_fw_query(struct av7110 *av7110, u16 tag, u16* buf, s16 length)
+{
+	int ret;
+	ret = av7110_fw_request(av7110, &tag, 0, buf, length);
+	if (ret)
+		printk(KERN_ERR "dvb-ttpci: av7110_fw_query error %d\n", ret);
+	return ret;
+}
+
+
+/****************************************************************************
+ * Firmware commands
+ ****************************************************************************/
+
+/* get version of the firmware ROM, RTSL, video ucode and ARM application  */
+int av7110_firmversion(struct av7110 *av7110)
+{
+	u16 buf[20];
+	u16 tag = ((COMTYPE_REQUEST << 8) + ReqVersion);
+
+	dprintk(4, "%p\n", av7110);
+
+	if (av7110_fw_query(av7110, tag, buf, 16)) {
+		printk("dvb-ttpci: failed to boot firmware @ card %d\n",
+		       av7110->dvb_adapter.num);
+		return -EIO;
+	}
+
+	av7110->arm_fw = (buf[0] << 16) + buf[1];
+	av7110->arm_rtsl = (buf[2] << 16) + buf[3];
+	av7110->arm_vid = (buf[4] << 16) + buf[5];
+	av7110->arm_app = (buf[6] << 16) + buf[7];
+	av7110->avtype = (buf[8] << 16) + buf[9];
+
+	printk("dvb-ttpci: info @ card %d: firm %08x, rtsl %08x, vid %08x, app %08x\n",
+	       av7110->dvb_adapter.num, av7110->arm_fw,
+	       av7110->arm_rtsl, av7110->arm_vid, av7110->arm_app);
+
+	/* print firmware capabilities */
+	if (FW_CI_LL_SUPPORT(av7110->arm_app))
+		printk("dvb-ttpci: firmware @ card %d supports CI link layer interface\n",
+		       av7110->dvb_adapter.num);
+	else
+		printk("dvb-ttpci: no firmware support for CI link layer interface @ card %d\n",
+		       av7110->dvb_adapter.num);
+
+	return 0;
+}
+
+
+int av7110_diseqc_send(struct av7110 *av7110, int len, u8 *msg, unsigned long burst)
+{
+	int i, ret;
+	u16 buf[18] = { ((COMTYPE_AUDIODAC << 8) + SendDiSEqC),
+			16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	dprintk(4, "%p\n", av7110);
+
+	if (len > 10)
+		len = 10;
+
+	buf[1] = len + 2;
+	buf[2] = len;
+
+	if (burst != -1)
+		buf[3] = burst ? 0x01 : 0x00;
+	else
+		buf[3] = 0xffff;
+
+	for (i = 0; i < len; i++)
+		buf[i + 4] = msg[i];
+
+	ret = av7110_send_fw_cmd(av7110, buf, 18);
+	if (ret && ret!=-ERESTARTSYS)
+		printk(KERN_ERR "dvb-ttpci: av7110_diseqc_send error %d\n", ret);
+	return ret;
+}
+
+
+#ifdef CONFIG_DVB_AV7110_OSD
+
+static inline int SetColorBlend(struct av7110 *av7110, u8 windownr)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, SetCBlend, 1, windownr);
+}
+
+static inline int SetBlend_(struct av7110 *av7110, u8 windownr,
+		     enum av7110_osd_palette_type colordepth, u16 index, u8 blending)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, SetBlend, 4,
+			     windownr, colordepth, index, blending);
+}
+
+static inline int SetColor_(struct av7110 *av7110, u8 windownr,
+		     enum av7110_osd_palette_type colordepth, u16 index, u16 colorhi, u16 colorlo)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, SetColor, 5,
+			     windownr, colordepth, index, colorhi, colorlo);
+}
+
+static inline int SetFont(struct av7110 *av7110, u8 windownr, u8 fontsize,
+			  u16 colorfg, u16 colorbg)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, Set_Font, 4,
+			     windownr, fontsize, colorfg, colorbg);
+}
+
+static int FlushText(struct av7110 *av7110)
+{
+	unsigned long start;
+	int err;
+
+	if (mutex_lock_interruptible(&av7110->dcomlock))
+		return -ERESTARTSYS;
+	start = jiffies;
+	while (1) {
+		err = time_after(jiffies, start + ARM_WAIT_OSD);
+		if (rdebi(av7110, DEBINOSWAP, BUFF1_BASE, 0, 2) == 0)
+			break;
+		if (err) {
+			printk(KERN_ERR "dvb-ttpci: %s(): timeout waiting for BUFF1_BASE == 0\n",
+			       __func__);
+			mutex_unlock(&av7110->dcomlock);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+	mutex_unlock(&av7110->dcomlock);
+	return 0;
+}
+
+static int WriteText(struct av7110 *av7110, u8 win, u16 x, u16 y, char *buf)
+{
+	int i, ret;
+	unsigned long start;
+	int length = strlen(buf) + 1;
+	u16 cbuf[5] = { (COMTYPE_OSD << 8) + DText, 3, win, x, y };
+
+	if (mutex_lock_interruptible(&av7110->dcomlock))
+		return -ERESTARTSYS;
+
+	start = jiffies;
+	while (1) {
+		ret = time_after(jiffies, start + ARM_WAIT_OSD);
+		if (rdebi(av7110, DEBINOSWAP, BUFF1_BASE, 0, 2) == 0)
+			break;
+		if (ret) {
+			printk(KERN_ERR "dvb-ttpci: %s: timeout waiting for BUFF1_BASE == 0\n",
+			       __func__);
+			mutex_unlock(&av7110->dcomlock);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+#ifndef _NOHANDSHAKE
+	start = jiffies;
+	while (1) {
+		ret = time_after(jiffies, start + ARM_WAIT_SHAKE);
+		if (rdebi(av7110, DEBINOSWAP, HANDSHAKE_REG, 0, 2) == 0)
+			break;
+		if (ret) {
+			printk(KERN_ERR "dvb-ttpci: %s: timeout waiting for HANDSHAKE_REG\n",
+			       __func__);
+			mutex_unlock(&av7110->dcomlock);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+#endif
+	for (i = 0; i < length / 2; i++)
+		wdebi(av7110, DEBINOSWAP, BUFF1_BASE + i * 2,
+		      swab16(*(u16 *)(buf + 2 * i)), 2);
+	if (length & 1)
+		wdebi(av7110, DEBINOSWAP, BUFF1_BASE + i * 2, 0, 2);
+	ret = __av7110_send_fw_cmd(av7110, cbuf, 5);
+	mutex_unlock(&av7110->dcomlock);
+	if (ret && ret!=-ERESTARTSYS)
+		printk(KERN_ERR "dvb-ttpci: WriteText error %d\n", ret);
+	return ret;
+}
+
+static inline int DrawLine(struct av7110 *av7110, u8 windownr,
+			   u16 x, u16 y, u16 dx, u16 dy, u16 color)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, DLine, 6,
+			     windownr, x, y, dx, dy, color);
+}
+
+static inline int DrawBlock(struct av7110 *av7110, u8 windownr,
+			    u16 x, u16 y, u16 dx, u16 dy, u16 color)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, DBox, 6,
+			     windownr, x, y, dx, dy, color);
+}
+
+static inline int HideWindow(struct av7110 *av7110, u8 windownr)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, WHide, 1, windownr);
+}
+
+static inline int MoveWindowRel(struct av7110 *av7110, u8 windownr, u16 x, u16 y)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, WMoveD, 3, windownr, x, y);
+}
+
+static inline int MoveWindowAbs(struct av7110 *av7110, u8 windownr, u16 x, u16 y)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, WMoveA, 3, windownr, x, y);
+}
+
+static inline int DestroyOSDWindow(struct av7110 *av7110, u8 windownr)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, WDestroy, 1, windownr);
+}
+
+static inline int CreateOSDWindow(struct av7110 *av7110, u8 windownr,
+				  osd_raw_window_t disptype,
+				  u16 width, u16 height)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, WCreate, 4,
+			     windownr, disptype, width, height);
+}
+
+
+static enum av7110_osd_palette_type bpp2pal[8] = {
+	Pal1Bit, Pal2Bit, 0, Pal4Bit, 0, 0, 0, Pal8Bit
+};
+static osd_raw_window_t bpp2bit[8] = {
+	OSD_BITMAP1, OSD_BITMAP2, 0, OSD_BITMAP4, 0, 0, 0, OSD_BITMAP8
+};
+
+static inline int WaitUntilBmpLoaded(struct av7110 *av7110)
+{
+	int ret = wait_event_timeout(av7110->bmpq,
+				av7110->bmp_state != BMP_LOADING, 10*HZ);
+	if (ret == 0) {
+		printk("dvb-ttpci: warning: timeout waiting in LoadBitmap: %d, %d\n",
+		       ret, av7110->bmp_state);
+		av7110->bmp_state = BMP_NONE;
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static inline int LoadBitmap(struct av7110 *av7110,
+			     u16 dx, u16 dy, int inc, u8 __user * data)
+{
+	u16 format;
+	int bpp;
+	int i;
+	int d, delta;
+	u8 c;
+	int ret;
+
+	dprintk(4, "%p\n", av7110);
+
+	format = bpp2bit[av7110->osdbpp[av7110->osdwin]];
+
+	av7110->bmp_state = BMP_LOADING;
+	if	(format == OSD_BITMAP8) {
+		bpp=8; delta = 1;
+	} else if (format == OSD_BITMAP4) {
+		bpp=4; delta = 2;
+	} else if (format == OSD_BITMAP2) {
+		bpp=2; delta = 4;
+	} else if (format == OSD_BITMAP1) {
+		bpp=1; delta = 8;
+	} else {
+		av7110->bmp_state = BMP_NONE;
+		return -EINVAL;
+	}
+	av7110->bmplen = ((dx * dy * bpp + 7) & ~7) / 8;
+	av7110->bmpp = 0;
+	if (av7110->bmplen > 32768) {
+		av7110->bmp_state = BMP_NONE;
+		return -EINVAL;
+	}
+	for (i = 0; i < dy; i++) {
+		if (copy_from_user(av7110->bmpbuf + 1024 + i * dx, data + i * inc, dx)) {
+			av7110->bmp_state = BMP_NONE;
+			return -EINVAL;
+		}
+	}
+	if (format != OSD_BITMAP8) {
+		for (i = 0; i < dx * dy / delta; i++) {
+			c = ((u8 *)av7110->bmpbuf)[1024 + i * delta + delta - 1];
+			for (d = delta - 2; d >= 0; d--) {
+				c |= (((u8 *)av7110->bmpbuf)[1024 + i * delta + d]
+				      << ((delta - d - 1) * bpp));
+				((u8 *)av7110->bmpbuf)[1024 + i] = c;
+			}
+		}
+	}
+	av7110->bmplen += 1024;
+	dprintk(4, "av7110_fw_cmd: LoadBmp size %d\n", av7110->bmplen);
+	ret = av7110_fw_cmd(av7110, COMTYPE_OSD, LoadBmp, 3, format, dx, dy);
+	if (!ret)
+		ret = WaitUntilBmpLoaded(av7110);
+	return ret;
+}
+
+static int BlitBitmap(struct av7110 *av7110, u16 x, u16 y)
+{
+	dprintk(4, "%p\n", av7110);
+
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, BlitBmp, 4, av7110->osdwin, x, y, 0);
+}
+
+static inline int ReleaseBitmap(struct av7110 *av7110)
+{
+	dprintk(4, "%p\n", av7110);
+
+	if (av7110->bmp_state != BMP_LOADED && FW_VERSION(av7110->arm_app) < 0x261e)
+		return -1;
+	if (av7110->bmp_state == BMP_LOADING)
+		dprintk(1,"ReleaseBitmap called while BMP_LOADING\n");
+	av7110->bmp_state = BMP_NONE;
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, ReleaseBmp, 0);
+}
+
+static u32 RGB2YUV(u16 R, u16 G, u16 B)
+{
+	u16 y, u, v;
+	u16 Y, Cr, Cb;
+
+	y = R * 77 + G * 150 + B * 29;	/* Luma=0.299R+0.587G+0.114B 0..65535 */
+	u = 2048 + B * 8 -(y >> 5);	/* Cr 0..4095 */
+	v = 2048 + R * 8 -(y >> 5);	/* Cb 0..4095 */
+
+	Y = y / 256;
+	Cb = u / 16;
+	Cr = v / 16;
+
+	return Cr | (Cb << 16) | (Y << 8);
+}
+
+static int OSDSetColor(struct av7110 *av7110, u8 color, u8 r, u8 g, u8 b, u8 blend)
+{
+	int ret;
+
+	u16 ch, cl;
+	u32 yuv;
+
+	yuv = blend ? RGB2YUV(r,g,b) : 0;
+	cl = (yuv & 0xffff);
+	ch = ((yuv >> 16) & 0xffff);
+	ret = SetColor_(av7110, av7110->osdwin, bpp2pal[av7110->osdbpp[av7110->osdwin]],
+			color, ch, cl);
+	if (!ret)
+		ret = SetBlend_(av7110, av7110->osdwin, bpp2pal[av7110->osdbpp[av7110->osdwin]],
+				color, ((blend >> 4) & 0x0f));
+	return ret;
+}
+
+static int OSDSetPalette(struct av7110 *av7110, u32 __user * colors, u8 first, u8 last)
+{
+	int i;
+	int length = last - first + 1;
+
+	if (length * 4 > DATA_BUFF3_SIZE)
+		return -EINVAL;
+
+	for (i = 0; i < length; i++) {
+		u32 color, blend, yuv;
+
+		if (get_user(color, colors + i))
+			return -EFAULT;
+		blend = (color & 0xF0000000) >> 4;
+		yuv = blend ? RGB2YUV(color & 0xFF, (color >> 8) & 0xFF,
+				     (color >> 16) & 0xFF) | blend : 0;
+		yuv = ((yuv & 0xFFFF0000) >> 16) | ((yuv & 0x0000FFFF) << 16);
+		wdebi(av7110, DEBINOSWAP, DATA_BUFF3_BASE + i * 4, yuv, 4);
+	}
+	return av7110_fw_cmd(av7110, COMTYPE_OSD, Set_Palette, 4,
+			    av7110->osdwin,
+			    bpp2pal[av7110->osdbpp[av7110->osdwin]],
+			    first, last);
+}
+
+static int OSDSetBlock(struct av7110 *av7110, int x0, int y0,
+		       int x1, int y1, int inc, u8 __user * data)
+{
+	uint w, h, bpp, bpl, size, lpb, bnum, brest;
+	int i;
+	int rc,release_rc;
+
+	w = x1 - x0 + 1;
+	h = y1 - y0 + 1;
+	if (inc <= 0)
+		inc = w;
+	if (w <= 0 || w > 720 || h <= 0 || h > 576)
+		return -EINVAL;
+	bpp = av7110->osdbpp[av7110->osdwin] + 1;
+	bpl = ((w * bpp + 7) & ~7) / 8;
+	size = h * bpl;
+	lpb = (32 * 1024) / bpl;
+	bnum = size / (lpb * bpl);
+	brest = size - bnum * lpb * bpl;
+
+	if (av7110->bmp_state == BMP_LOADING) {
+		/* possible if syscall is repeated by -ERESTARTSYS and if firmware cannot abort */
+		BUG_ON (FW_VERSION(av7110->arm_app) >= 0x261e);
+		rc = WaitUntilBmpLoaded(av7110);
+		if (rc)
+			return rc;
+		/* just continue. This should work for all fw versions
+		 * if bnum==1 && !brest && LoadBitmap was successful
+		 */
+	}
+
+	rc = 0;
+	for (i = 0; i < bnum; i++) {
+		rc = LoadBitmap(av7110, w, lpb, inc, data);
+		if (rc)
+			break;
+		rc = BlitBitmap(av7110, x0, y0 + i * lpb);
+		if (rc)
+			break;
+		data += lpb * inc;
+	}
+	if (!rc && brest) {
+		rc = LoadBitmap(av7110, w, brest / bpl, inc, data);
+		if (!rc)
+			rc = BlitBitmap(av7110, x0, y0 + bnum * lpb);
+	}
+	release_rc = ReleaseBitmap(av7110);
+	if (!rc)
+		rc = release_rc;
+	if (rc)
+		dprintk(1,"returns %d\n",rc);
+	return rc;
+}
+
+int av7110_osd_cmd(struct av7110 *av7110, osd_cmd_t *dc)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&av7110->osd_mutex))
+		return -ERESTARTSYS;
+
+	switch (dc->cmd) {
+	case OSD_Close:
+		ret = DestroyOSDWindow(av7110, av7110->osdwin);
+		break;
+	case OSD_Open:
+		av7110->osdbpp[av7110->osdwin] = (dc->color - 1) & 7;
+		ret = CreateOSDWindow(av7110, av7110->osdwin,
+				bpp2bit[av7110->osdbpp[av7110->osdwin]],
+				dc->x1 - dc->x0 + 1, dc->y1 - dc->y0 + 1);
+		if (ret)
+			break;
+		if (!dc->data) {
+			ret = MoveWindowAbs(av7110, av7110->osdwin, dc->x0, dc->y0);
+			if (ret)
+				break;
+			ret = SetColorBlend(av7110, av7110->osdwin);
+		}
+		break;
+	case OSD_Show:
+		ret = MoveWindowRel(av7110, av7110->osdwin, 0, 0);
+		break;
+	case OSD_Hide:
+		ret = HideWindow(av7110, av7110->osdwin);
+		break;
+	case OSD_Clear:
+		ret = DrawBlock(av7110, av7110->osdwin, 0, 0, 720, 576, 0);
+		break;
+	case OSD_Fill:
+		ret = DrawBlock(av7110, av7110->osdwin, 0, 0, 720, 576, dc->color);
+		break;
+	case OSD_SetColor:
+		ret = OSDSetColor(av7110, dc->color, dc->x0, dc->y0, dc->x1, dc->y1);
+		break;
+	case OSD_SetPalette:
+		if (FW_VERSION(av7110->arm_app) >= 0x2618)
+			ret = OSDSetPalette(av7110, dc->data, dc->color, dc->x0);
+		else {
+			int i, len = dc->x0-dc->color+1;
+			u8 __user *colors = (u8 __user *)dc->data;
+			u8 r, g = 0, b = 0, blend = 0;
+			ret = 0;
+			for (i = 0; i<len; i++) {
+				if (get_user(r, colors + i * 4) ||
+				    get_user(g, colors + i * 4 + 1) ||
+				    get_user(b, colors + i * 4 + 2) ||
+				    get_user(blend, colors + i * 4 + 3)) {
+					ret = -EFAULT;
+					break;
+				    }
+				ret = OSDSetColor(av7110, dc->color + i, r, g, b, blend);
+				if (ret)
+					break;
+			}
+		}
+		break;
+	case OSD_SetPixel:
+		ret = DrawLine(av7110, av7110->osdwin,
+			 dc->x0, dc->y0, 0, 0, dc->color);
+		break;
+	case OSD_SetRow:
+		dc->y1 = dc->y0;
+		fallthrough;
+	case OSD_SetBlock:
+		ret = OSDSetBlock(av7110, dc->x0, dc->y0, dc->x1, dc->y1, dc->color, dc->data);
+		break;
+	case OSD_FillRow:
+		ret = DrawBlock(av7110, av7110->osdwin, dc->x0, dc->y0,
+			  dc->x1-dc->x0+1, dc->y1, dc->color);
+		break;
+	case OSD_FillBlock:
+		ret = DrawBlock(av7110, av7110->osdwin, dc->x0, dc->y0,
+			  dc->x1 - dc->x0 + 1, dc->y1 - dc->y0 + 1, dc->color);
+		break;
+	case OSD_Line:
+		ret = DrawLine(av7110, av7110->osdwin,
+			 dc->x0, dc->y0, dc->x1 - dc->x0, dc->y1 - dc->y0, dc->color);
+		break;
+	case OSD_Text:
+	{
+		char textbuf[240];
+
+		if (strncpy_from_user(textbuf, dc->data, 240) < 0) {
+			ret = -EFAULT;
+			break;
+		}
+		textbuf[239] = 0;
+		if (dc->x1 > 3)
+			dc->x1 = 3;
+		ret = SetFont(av7110, av7110->osdwin, dc->x1,
+			(u16) (dc->color & 0xffff), (u16) (dc->color >> 16));
+		if (!ret)
+			ret = FlushText(av7110);
+		if (!ret)
+			ret = WriteText(av7110, av7110->osdwin, dc->x0, dc->y0, textbuf);
+		break;
+	}
+	case OSD_SetWindow:
+		if (dc->x0 < 1 || dc->x0 > 7)
+			ret = -EINVAL;
+		else {
+			av7110->osdwin = dc->x0;
+			ret = 0;
+		}
+		break;
+	case OSD_MoveWindow:
+		ret = MoveWindowAbs(av7110, av7110->osdwin, dc->x0, dc->y0);
+		if (!ret)
+			ret = SetColorBlend(av7110, av7110->osdwin);
+		break;
+	case OSD_OpenRaw:
+		if (dc->color < OSD_BITMAP1 || dc->color > OSD_CURSOR) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dc->color >= OSD_BITMAP1 && dc->color <= OSD_BITMAP8HR)
+			av7110->osdbpp[av7110->osdwin] = (1 << (dc->color & 3)) - 1;
+		else
+			av7110->osdbpp[av7110->osdwin] = 0;
+		ret = CreateOSDWindow(av7110, av7110->osdwin, (osd_raw_window_t)dc->color,
+				dc->x1 - dc->x0 + 1, dc->y1 - dc->y0 + 1);
+		if (ret)
+			break;
+		if (!dc->data) {
+			ret = MoveWindowAbs(av7110, av7110->osdwin, dc->x0, dc->y0);
+			if (!ret)
+				ret = SetColorBlend(av7110, av7110->osdwin);
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&av7110->osd_mutex);
+	if (ret==-ERESTARTSYS)
+		dprintk(1, "av7110_osd_cmd(%d) returns with -ERESTARTSYS\n",dc->cmd);
+	else if (ret)
+		dprintk(1, "av7110_osd_cmd(%d) returns with %d\n",dc->cmd,ret);
+
+	return ret;
+}
+
+int av7110_osd_capability(struct av7110 *av7110, osd_cap_t *cap)
+{
+	switch (cap->cmd) {
+	case OSD_CAP_MEMSIZE:
+		if (FW_4M_SDRAM(av7110->arm_app))
+			cap->val = 1000000;
+		else
+			cap->val = 92000;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+#endif /* CONFIG_DVB_AV7110_OSD */
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.h b/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.h
new file mode 100644
index 000000000..6380d8950
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_hw.h
@@ -0,0 +1,496 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _AV7110_HW_H_
+#define _AV7110_HW_H_
+
+#include "av7110.h"
+
+/* DEBI transfer mode defs */
+
+#define DEBINOSWAP 0x000e0000
+#define DEBISWAB   0x001e0000
+#define DEBISWAP   0x002e0000
+
+#define ARM_WAIT_FREE  (HZ)
+#define ARM_WAIT_SHAKE (HZ/5)
+#define ARM_WAIT_OSD (HZ)
+
+
+enum av7110_bootstate
+{
+	BOOTSTATE_BUFFER_EMPTY	= 0,
+	BOOTSTATE_BUFFER_FULL	= 1,
+	BOOTSTATE_AV7110_BOOT_COMPLETE	= 2
+};
+
+enum av7110_type_rec_play_format
+{	RP_None,
+	AudioPES,
+	AudioMp2,
+	AudioPCM,
+	VideoPES,
+	AV_PES
+};
+
+enum av7110_osd_palette_type
+{
+	NoPalet =  0,	   /* No palette */
+	Pal1Bit =  2,	   /* 2 colors for 1 Bit Palette    */
+	Pal2Bit =  4,	   /* 4 colors for 2 bit palette    */
+	Pal4Bit =  16,	   /* 16 colors for 4 bit palette   */
+	Pal8Bit =  256	   /* 256 colors for 16 bit palette */
+};
+
+/* switch defines */
+#define SB_GPIO 3
+#define SB_OFF	SAA7146_GPIO_OUTLO  /* SlowBlank off (TV-Mode) */
+#define SB_ON	SAA7146_GPIO_INPUT  /* SlowBlank on  (AV-Mode) */
+#define SB_WIDE SAA7146_GPIO_OUTHI  /* SlowBlank 6V  (16/9-Mode) (not implemented) */
+
+#define FB_GPIO 1
+#define FB_OFF	SAA7146_GPIO_LO     /* FastBlank off (CVBS-Mode) */
+#define FB_ON	SAA7146_GPIO_OUTHI  /* FastBlank on  (RGB-Mode) */
+#define FB_LOOP	SAA7146_GPIO_INPUT  /* FastBlank loop-through (PC graphics ???) */
+
+enum av7110_video_output_mode
+{
+	NO_OUT	     = 0,		/* disable analog output */
+	CVBS_RGB_OUT = 1,
+	CVBS_YC_OUT  = 2,
+	YC_OUT	     = 3
+};
+
+/* firmware internal msg q status: */
+#define GPMQFull	0x0001		/* Main Message Queue Full */
+#define GPMQOver	0x0002		/* Main Message Queue Overflow */
+#define HPQFull		0x0004		/* High Priority Msg Queue Full */
+#define HPQOver		0x0008
+#define OSDQFull	0x0010		/* OSD Queue Full */
+#define OSDQOver	0x0020
+#define GPMQBusy	0x0040		/* Queue not empty, FW >= 261d */
+#define HPQBusy		0x0080
+#define OSDQBusy	0x0100
+
+/* hw section filter flags */
+#define	SECTION_EIT		0x01
+#define	SECTION_SINGLE		0x00
+#define	SECTION_CYCLE		0x02
+#define	SECTION_CONTINUOS	0x04
+#define	SECTION_MODE		0x06
+#define SECTION_IPMPE		0x0C	/* size up to 4k */
+#define SECTION_HIGH_SPEED	0x1C	/* larger buffer */
+#define DATA_PIPING_FLAG	0x20	/* for Data Piping Filter */
+
+#define	PBUFSIZE_NONE 0x0000
+#define	PBUFSIZE_1P   0x0100
+#define	PBUFSIZE_2P   0x0200
+#define	PBUFSIZE_1K   0x0300
+#define	PBUFSIZE_2K   0x0400
+#define	PBUFSIZE_4K   0x0500
+#define	PBUFSIZE_8K   0x0600
+#define PBUFSIZE_16K  0x0700
+#define PBUFSIZE_32K  0x0800
+
+
+/* firmware command codes */
+enum av7110_osd_command {
+	WCreate,
+	WDestroy,
+	WMoveD,
+	WMoveA,
+	WHide,
+	WTop,
+	DBox,
+	DLine,
+	DText,
+	Set_Font,
+	SetColor,
+	SetBlend,
+	SetWBlend,
+	SetCBlend,
+	SetNonBlend,
+	LoadBmp,
+	BlitBmp,
+	ReleaseBmp,
+	SetWTrans,
+	SetWNoTrans,
+	Set_Palette
+};
+
+enum av7110_pid_command {
+	MultiPID,
+	VideoPID,
+	AudioPID,
+	InitFilt,
+	FiltError,
+	NewVersion,
+	CacheError,
+	AddPIDFilter,
+	DelPIDFilter,
+	Scan,
+	SetDescr,
+	SetIR,
+	FlushTSQueue
+};
+
+enum av7110_mpeg_command {
+	SelAudChannels
+};
+
+enum av7110_audio_command {
+	AudioDAC,
+	CabADAC,
+	ON22K,
+	OFF22K,
+	MainSwitch,
+	ADSwitch,
+	SendDiSEqC,
+	SetRegister,
+	SpdifSwitch
+};
+
+enum av7110_request_command {
+	AudioState,
+	AudioBuffState,
+	VideoState1,
+	VideoState2,
+	VideoState3,
+	CrashCounter,
+	ReqVersion,
+	ReqVCXO,
+	ReqRegister,
+	ReqSecFilterError,
+	ReqSTC
+};
+
+enum av7110_encoder_command {
+	SetVidMode,
+	SetTestMode,
+	LoadVidCode,
+	SetMonitorType,
+	SetPanScanType,
+	SetFreezeMode,
+	SetWSSConfig
+};
+
+enum av7110_rec_play_state {
+	__Record,
+	__Stop,
+	__Play,
+	__Pause,
+	__Slow,
+	__FF_IP,
+	__Scan_I,
+	__Continue
+};
+
+enum av7110_fw_cmd_misc {
+	AV7110_FW_VIDEO_ZOOM = 1,
+	AV7110_FW_VIDEO_COMMAND,
+	AV7110_FW_AUDIO_COMMAND
+};
+
+enum av7110_command_type {
+	COMTYPE_NOCOM,
+	COMTYPE_PIDFILTER,
+	COMTYPE_MPEGDECODER,
+	COMTYPE_OSD,
+	COMTYPE_BMP,
+	COMTYPE_ENCODER,
+	COMTYPE_AUDIODAC,
+	COMTYPE_REQUEST,
+	COMTYPE_SYSTEM,
+	COMTYPE_REC_PLAY,
+	COMTYPE_COMMON_IF,
+	COMTYPE_PID_FILTER,
+	COMTYPE_PES,
+	COMTYPE_TS,
+	COMTYPE_VIDEO,
+	COMTYPE_AUDIO,
+	COMTYPE_CI_LL,
+	COMTYPE_MISC = 0x80
+};
+
+#define VID_NONE_PREF		0x00	/* No aspect ration processing preferred */
+#define VID_PAN_SCAN_PREF	0x01	/* Pan and Scan Display preferred */
+#define VID_VERT_COMP_PREF	0x02	/* Vertical compression display preferred */
+#define VID_VC_AND_PS_PREF	0x03	/* PanScan and vertical Compression if allowed */
+#define VID_CENTRE_CUT_PREF	0x05	/* PanScan with zero vector */
+
+/* MPEG video decoder commands */
+#define AV_VIDEO_CMD_STOP	0x000e
+#define AV_VIDEO_CMD_PLAY	0x000d
+#define AV_VIDEO_CMD_FREEZE	0x0102
+#define AV_VIDEO_CMD_FFWD	0x0016
+#define AV_VIDEO_CMD_SLOW	0x0022
+
+/* MPEG audio decoder commands */
+#define AUDIO_CMD_MUTE		0x0001
+#define AUDIO_CMD_UNMUTE	0x0002
+#define AUDIO_CMD_PCM16		0x0010
+#define AUDIO_CMD_STEREO	0x0080
+#define AUDIO_CMD_MONO_L	0x0100
+#define AUDIO_CMD_MONO_R	0x0200
+#define AUDIO_CMD_SYNC_OFF	0x000e
+#define AUDIO_CMD_SYNC_ON	0x000f
+
+/* firmware data interface codes */
+#define DATA_NONE		 0x00
+#define DATA_FSECTION		 0x01
+#define DATA_IPMPE		 0x02
+#define DATA_MPEG_RECORD	 0x03
+#define DATA_DEBUG_MESSAGE	 0x04
+#define DATA_COMMON_INTERFACE	 0x05
+#define DATA_MPEG_PLAY		 0x06
+#define DATA_BMP_LOAD		 0x07
+#define DATA_IRCOMMAND		 0x08
+#define DATA_PIPING		 0x09
+#define DATA_STREAMING		 0x0a
+#define DATA_CI_GET		 0x0b
+#define DATA_CI_PUT		 0x0c
+#define DATA_MPEG_VIDEO_EVENT	 0x0d
+
+#define DATA_PES_RECORD		 0x10
+#define DATA_PES_PLAY		 0x11
+#define DATA_TS_RECORD		 0x12
+#define DATA_TS_PLAY		 0x13
+
+/* ancient CI command codes, only two are actually still used
+ * by the link level CI firmware */
+#define CI_CMD_ERROR		 0x00
+#define CI_CMD_ACK		 0x01
+#define CI_CMD_SYSTEM_READY	 0x02
+#define CI_CMD_KEYPRESS		 0x03
+#define CI_CMD_ON_TUNED		 0x04
+#define CI_CMD_ON_SWITCH_PROGRAM 0x05
+#define CI_CMD_SECTION_ARRIVED	 0x06
+#define CI_CMD_SECTION_TIMEOUT	 0x07
+#define CI_CMD_TIME		 0x08
+#define CI_CMD_ENTER_MENU	 0x09
+#define CI_CMD_FAST_PSI		 0x0a
+#define CI_CMD_GET_SLOT_INFO	 0x0b
+
+#define CI_MSG_NONE		 0x00
+#define CI_MSG_CI_INFO		 0x01
+#define CI_MSG_MENU		 0x02
+#define CI_MSG_LIST		 0x03
+#define CI_MSG_TEXT		 0x04
+#define CI_MSG_REQUEST_INPUT	 0x05
+#define CI_MSG_INPUT_COMPLETE	 0x06
+#define CI_MSG_LIST_MORE	 0x07
+#define CI_MSG_MENU_MORE	 0x08
+#define CI_MSG_CLOSE_MMI_IMM	 0x09
+#define CI_MSG_SECTION_REQUEST	 0x0a
+#define CI_MSG_CLOSE_FILTER	 0x0b
+#define CI_PSI_COMPLETE		 0x0c
+#define CI_MODULE_READY		 0x0d
+#define CI_SWITCH_PRG_REPLY	 0x0e
+#define CI_MSG_TEXT_MORE	 0x0f
+
+#define CI_MSG_CA_PMT		 0xe0
+#define CI_MSG_ERROR		 0xf0
+
+
+/* base address of the dual ported RAM which serves as communication
+ * area between PCI bus and av7110,
+ * as seen by the DEBI bus of the saa7146 */
+#define	DPRAM_BASE 0x4000
+
+/* boot protocol area */
+#define AV7110_BOOT_STATE	(DPRAM_BASE + 0x3F8)
+#define AV7110_BOOT_SIZE	(DPRAM_BASE + 0x3FA)
+#define AV7110_BOOT_BASE	(DPRAM_BASE + 0x3FC)
+#define AV7110_BOOT_BLOCK	(DPRAM_BASE + 0x400)
+#define AV7110_BOOT_MAX_SIZE	0xc00
+
+/* firmware command protocol area */
+#define IRQ_STATE	(DPRAM_BASE + 0x0F4)
+#define IRQ_STATE_EXT	(DPRAM_BASE + 0x0F6)
+#define MSGSTATE	(DPRAM_BASE + 0x0F8)
+#define COMMAND		(DPRAM_BASE + 0x0FC)
+#define COM_BUFF	(DPRAM_BASE + 0x100)
+#define COM_BUFF_SIZE	0x20
+
+/* various data buffers */
+#define BUFF1_BASE	(DPRAM_BASE + 0x120)
+#define BUFF1_SIZE	0xE0
+
+#define DATA_BUFF0_BASE	(DPRAM_BASE + 0x200)
+#define DATA_BUFF0_SIZE	0x0800
+
+#define DATA_BUFF1_BASE	(DATA_BUFF0_BASE+DATA_BUFF0_SIZE)
+#define DATA_BUFF1_SIZE	0x0800
+
+#define DATA_BUFF2_BASE	(DATA_BUFF1_BASE+DATA_BUFF1_SIZE)
+#define DATA_BUFF2_SIZE	0x0800
+
+#define DATA_BUFF3_BASE (DATA_BUFF2_BASE+DATA_BUFF2_SIZE)
+#define DATA_BUFF3_SIZE 0x0400
+
+#define Reserved	(DPRAM_BASE + 0x1E00)
+#define Reserved_SIZE	0x1C0
+
+
+/* firmware status area */
+#define STATUS_BASE	(DPRAM_BASE + 0x1FC0)
+#define STATUS_LOOPS	(STATUS_BASE + 0x08)
+
+#define STATUS_MPEG_WIDTH     (STATUS_BASE + 0x0C)
+/* ((aspect_ratio & 0xf) << 12) | (height & 0xfff) */
+#define STATUS_MPEG_HEIGHT_AR (STATUS_BASE + 0x0E)
+
+/* firmware data protocol area */
+#define RX_TYPE		(DPRAM_BASE + 0x1FE8)
+#define RX_LEN		(DPRAM_BASE + 0x1FEA)
+#define TX_TYPE		(DPRAM_BASE + 0x1FEC)
+#define TX_LEN		(DPRAM_BASE + 0x1FEE)
+
+#define RX_BUFF		(DPRAM_BASE + 0x1FF4)
+#define TX_BUFF		(DPRAM_BASE + 0x1FF6)
+
+#define HANDSHAKE_REG	(DPRAM_BASE + 0x1FF8)
+#define COM_IF_LOCK	(DPRAM_BASE + 0x1FFA)
+
+#define IRQ_RX		(DPRAM_BASE + 0x1FFC)
+#define IRQ_TX		(DPRAM_BASE + 0x1FFE)
+
+/* used by boot protocol to load firmware into av7110 DRAM */
+#define DRAM_START_CODE		0x2e000404
+#define DRAM_MAX_CODE_SIZE	0x00100000
+
+/* saa7146 gpio lines */
+#define RESET_LINE		2
+#define DEBI_DONE_LINE		1
+#define ARM_IRQ_LINE		0
+
+
+
+extern int av7110_bootarm(struct av7110 *av7110);
+extern int av7110_firmversion(struct av7110 *av7110);
+#define FW_CI_LL_SUPPORT(arm_app) ((arm_app) & 0x80000000)
+#define FW_4M_SDRAM(arm_app)      ((arm_app) & 0x40000000)
+#define FW_VERSION(arm_app)	  ((arm_app) & 0x0000FFFF)
+
+extern int av7110_wait_msgstate(struct av7110 *av7110, u16 flags);
+extern int av7110_fw_cmd(struct av7110 *av7110, int type, int com, int num, ...);
+extern int av7110_fw_request(struct av7110 *av7110, u16 *request_buf,
+			     int request_buf_len, u16 *reply_buf, int reply_buf_len);
+
+
+/* DEBI (saa7146 data extension bus interface) access */
+extern int av7110_debiwrite(struct av7110 *av7110, u32 config,
+			    int addr, u32 val, unsigned int count);
+extern u32 av7110_debiread(struct av7110 *av7110, u32 config,
+			   int addr, unsigned int count);
+
+
+/* DEBI during interrupt */
+/* single word writes */
+static inline void iwdebi(struct av7110 *av7110, u32 config, int addr, u32 val, unsigned int count)
+{
+	av7110_debiwrite(av7110, config, addr, val, count);
+}
+
+/* buffer writes */
+static inline void mwdebi(struct av7110 *av7110, u32 config, int addr,
+			  const u8 *val, int count)
+{
+	memcpy(av7110->debi_virt, val, count);
+	av7110_debiwrite(av7110, config, addr, 0, count);
+}
+
+static inline u32 irdebi(struct av7110 *av7110, u32 config, int addr, u32 val, unsigned int count)
+{
+	u32 res;
+
+	res=av7110_debiread(av7110, config, addr, count);
+	if (count<=4)
+		memcpy(av7110->debi_virt, (char *) &res, count);
+	return res;
+}
+
+/* DEBI outside interrupts, only for count <= 4! */
+static inline void wdebi(struct av7110 *av7110, u32 config, int addr, u32 val, unsigned int count)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&av7110->debilock, flags);
+	av7110_debiwrite(av7110, config, addr, val, count);
+	spin_unlock_irqrestore(&av7110->debilock, flags);
+}
+
+static inline u32 rdebi(struct av7110 *av7110, u32 config, int addr, u32 val, unsigned int count)
+{
+	unsigned long flags;
+	u32 res;
+
+	spin_lock_irqsave(&av7110->debilock, flags);
+	res=av7110_debiread(av7110, config, addr, count);
+	spin_unlock_irqrestore(&av7110->debilock, flags);
+	return res;
+}
+
+/* handle mailbox registers of the dual ported RAM */
+static inline void ARM_ResetMailBox(struct av7110 *av7110)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&av7110->debilock, flags);
+	av7110_debiread(av7110, DEBINOSWAP, IRQ_RX, 2);
+	av7110_debiwrite(av7110, DEBINOSWAP, IRQ_RX, 0, 2);
+	spin_unlock_irqrestore(&av7110->debilock, flags);
+}
+
+static inline void ARM_ClearMailBox(struct av7110 *av7110)
+{
+	iwdebi(av7110, DEBINOSWAP, IRQ_RX, 0, 2);
+}
+
+static inline void ARM_ClearIrq(struct av7110 *av7110)
+{
+	irdebi(av7110, DEBINOSWAP, IRQ_RX, 0, 2);
+}
+
+/****************************************************************************
+ * Firmware commands
+ ****************************************************************************/
+
+static inline int SendDAC(struct av7110 *av7110, u8 addr, u8 data)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, AudioDAC, 2, addr, data);
+}
+
+static inline int av7710_set_video_mode(struct av7110 *av7110, int mode)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetVidMode, 1, mode);
+}
+
+static inline int vidcom(struct av7110 *av7110, u32 com, u32 arg)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_MISC, AV7110_FW_VIDEO_COMMAND, 4,
+			     (com>>16), (com&0xffff),
+			     (arg>>16), (arg&0xffff));
+}
+
+static inline int audcom(struct av7110 *av7110, u32 com)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_MISC, AV7110_FW_AUDIO_COMMAND, 2,
+			     (com>>16), (com&0xffff));
+}
+
+static inline int Set22K(struct av7110 *av7110, int state)
+{
+	return av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, (state ? ON22K : OFF22K), 0);
+}
+
+
+extern int av7110_diseqc_send(struct av7110 *av7110, int len, u8 *msg, unsigned long burst);
+
+
+#ifdef CONFIG_DVB_AV7110_OSD
+extern int av7110_osd_cmd(struct av7110 *av7110, osd_cmd_t *dc);
+extern int av7110_osd_capability(struct av7110 *av7110, osd_cap_t *cap);
+#endif /* CONFIG_DVB_AV7110_OSD */
+
+
+
+#endif /* _AV7110_HW_H_ */
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.c
new file mode 100644
index 000000000..30330ed01
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.c
@@ -0,0 +1,404 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "dvb_filter.h"
+#include "av7110_ipack.h"
+#include <linux/string.h>	/* for memcpy() */
+#include <linux/vmalloc.h>
+
+
+void av7110_ipack_reset(struct ipack *p)
+{
+	p->found = 0;
+	p->cid = 0;
+	p->plength = 0;
+	p->flag1 = 0;
+	p->flag2 = 0;
+	p->hlength = 0;
+	p->mpeg = 0;
+	p->check = 0;
+	p->which = 0;
+	p->done = 0;
+	p->count = 0;
+}
+
+
+int av7110_ipack_init(struct ipack *p, int size,
+		      void (*func)(u8 *buf, int size, void *priv))
+{
+	if (!(p->buf = vmalloc(size))) {
+		printk(KERN_WARNING "Couldn't allocate memory for ipack\n");
+		return -ENOMEM;
+	}
+	p->size = size;
+	p->func = func;
+	p->repack_subids = 0;
+	av7110_ipack_reset(p);
+	return 0;
+}
+
+
+void av7110_ipack_free(struct ipack *p)
+{
+	vfree(p->buf);
+}
+
+
+static void send_ipack(struct ipack *p)
+{
+	int off;
+	struct dvb_audio_info ai;
+	int ac3_off = 0;
+	int streamid = 0;
+	int nframes = 0;
+	int f = 0;
+
+	switch (p->mpeg) {
+	case 2:
+		if (p->count < 10)
+			return;
+		p->buf[3] = p->cid;
+		p->buf[4] = (u8)(((p->count - 6) & 0xff00) >> 8);
+		p->buf[5] = (u8)((p->count - 6) & 0x00ff);
+		if (p->repack_subids && p->cid == PRIVATE_STREAM1) {
+			off = 9 + p->buf[8];
+			streamid = p->buf[off];
+			if ((streamid & 0xf8) == 0x80) {
+				ai.off = 0;
+				ac3_off = ((p->buf[off + 2] << 8)|
+					   p->buf[off + 3]);
+				if (ac3_off < p->count)
+					f = dvb_filter_get_ac3info(p->buf + off + 3 + ac3_off,
+								   p->count - ac3_off, &ai, 0);
+				if (!f) {
+					nframes = (p->count - off - 3 - ac3_off) /
+						ai.framesize + 1;
+					p->buf[off + 2] = (ac3_off >> 8) & 0xff;
+					p->buf[off + 3] = (ac3_off) & 0xff;
+					p->buf[off + 1] = nframes;
+					ac3_off +=  nframes * ai.framesize - p->count;
+				}
+			}
+		}
+		p->func(p->buf, p->count, p->data);
+
+		p->buf[6] = 0x80;
+		p->buf[7] = 0x00;
+		p->buf[8] = 0x00;
+		p->count = 9;
+		if (p->repack_subids && p->cid == PRIVATE_STREAM1
+		    && (streamid & 0xf8) == 0x80) {
+			p->count += 4;
+			p->buf[9] = streamid;
+			p->buf[10] = (ac3_off >> 8) & 0xff;
+			p->buf[11] = (ac3_off) & 0xff;
+			p->buf[12] = 0;
+		}
+		break;
+
+	case 1:
+		if (p->count < 8)
+			return;
+		p->buf[3] = p->cid;
+		p->buf[4] = (u8)(((p->count - 6) & 0xff00) >> 8);
+		p->buf[5] = (u8)((p->count - 6) & 0x00ff);
+		p->func(p->buf, p->count, p->data);
+
+		p->buf[6] = 0x0f;
+		p->count = 7;
+		break;
+	}
+}
+
+
+void av7110_ipack_flush(struct ipack *p)
+{
+	if (p->plength != MMAX_PLENGTH - 6 || p->found <= 6)
+		return;
+	p->plength = p->found - 6;
+	p->found = 0;
+	send_ipack(p);
+	av7110_ipack_reset(p);
+}
+
+
+static void write_ipack(struct ipack *p, const u8 *data, int count)
+{
+	u8 headr[3] = { 0x00, 0x00, 0x01 };
+
+	if (p->count < 6) {
+		memcpy(p->buf, headr, 3);
+		p->count = 6;
+	}
+
+	if (p->count + count < p->size){
+		memcpy(p->buf+p->count, data, count);
+		p->count += count;
+	} else {
+		int rest = p->size - p->count;
+		memcpy(p->buf+p->count, data, rest);
+		p->count += rest;
+		send_ipack(p);
+		if (count - rest > 0)
+			write_ipack(p, data + rest, count - rest);
+	}
+}
+
+
+int av7110_ipack_instant_repack (const u8 *buf, int count, struct ipack *p)
+{
+	int l;
+	int c = 0;
+
+	while (c < count && (p->mpeg == 0 ||
+			     (p->mpeg == 1 && p->found < 7) ||
+			     (p->mpeg == 2 && p->found < 9))
+	       &&  (p->found < 5 || !p->done)) {
+		switch (p->found) {
+		case 0:
+		case 1:
+			if (buf[c] == 0x00)
+				p->found++;
+			else
+				p->found = 0;
+			c++;
+			break;
+		case 2:
+			if (buf[c] == 0x01)
+				p->found++;
+			else if (buf[c] == 0)
+				p->found = 2;
+			else
+				p->found = 0;
+			c++;
+			break;
+		case 3:
+			p->cid = 0;
+			switch (buf[c]) {
+			case PROG_STREAM_MAP:
+			case PRIVATE_STREAM2:
+			case PROG_STREAM_DIR:
+			case ECM_STREAM     :
+			case EMM_STREAM     :
+			case PADDING_STREAM :
+			case DSM_CC_STREAM  :
+			case ISO13522_STREAM:
+				p->done = 1;
+				fallthrough;
+			case PRIVATE_STREAM1:
+			case VIDEO_STREAM_S ... VIDEO_STREAM_E:
+			case AUDIO_STREAM_S ... AUDIO_STREAM_E:
+				p->found++;
+				p->cid = buf[c];
+				c++;
+				break;
+			default:
+				p->found = 0;
+				break;
+			}
+			break;
+
+		case 4:
+			if (count-c > 1) {
+				p->plen[0] = buf[c];
+				c++;
+				p->plen[1] = buf[c];
+				c++;
+				p->found += 2;
+				p->plength = (p->plen[0] << 8) | p->plen[1];
+			} else {
+				p->plen[0] = buf[c];
+				p->found++;
+				return count;
+			}
+			break;
+		case 5:
+			p->plen[1] = buf[c];
+			c++;
+			p->found++;
+			p->plength = (p->plen[0] << 8) | p->plen[1];
+			break;
+		case 6:
+			if (!p->done) {
+				p->flag1 = buf[c];
+				c++;
+				p->found++;
+				if ((p->flag1 & 0xc0) == 0x80)
+					p->mpeg = 2;
+				else {
+					p->hlength = 0;
+					p->which = 0;
+					p->mpeg = 1;
+					p->flag2 = 0;
+				}
+			}
+			break;
+
+		case 7:
+			if (!p->done && p->mpeg == 2) {
+				p->flag2 = buf[c];
+				c++;
+				p->found++;
+			}
+			break;
+
+		case 8:
+			if (!p->done && p->mpeg == 2) {
+				p->hlength = buf[c];
+				c++;
+				p->found++;
+			}
+			break;
+		}
+	}
+
+	if (c == count)
+		return count;
+
+	if (!p->plength)
+		p->plength = MMAX_PLENGTH - 6;
+
+	if (p->done || ((p->mpeg == 2 && p->found >= 9) ||
+			(p->mpeg == 1 && p->found >= 7))) {
+		switch (p->cid) {
+		case AUDIO_STREAM_S ... AUDIO_STREAM_E:
+		case VIDEO_STREAM_S ... VIDEO_STREAM_E:
+		case PRIVATE_STREAM1:
+			if (p->mpeg == 2 && p->found == 9) {
+				write_ipack(p, &p->flag1, 1);
+				write_ipack(p, &p->flag2, 1);
+				write_ipack(p, &p->hlength, 1);
+			}
+
+			if (p->mpeg == 1 && p->found == 7)
+				write_ipack(p, &p->flag1, 1);
+
+			if (p->mpeg == 2 && (p->flag2 & PTS_ONLY) &&
+			    p->found < 14) {
+				while (c < count && p->found < 14) {
+					p->pts[p->found - 9] = buf[c];
+					write_ipack(p, buf + c, 1);
+					c++;
+					p->found++;
+				}
+				if (c == count)
+					return count;
+			}
+
+			if (p->mpeg == 1 && p->which < 2000) {
+
+				if (p->found == 7) {
+					p->check = p->flag1;
+					p->hlength = 1;
+				}
+
+				while (!p->which && c < count &&
+				       p->check == 0xff){
+					p->check = buf[c];
+					write_ipack(p, buf + c, 1);
+					c++;
+					p->found++;
+					p->hlength++;
+				}
+
+				if (c == count)
+					return count;
+
+				if ((p->check & 0xc0) == 0x40 && !p->which) {
+					p->check = buf[c];
+					write_ipack(p, buf + c, 1);
+					c++;
+					p->found++;
+					p->hlength++;
+
+					p->which = 1;
+					if (c == count)
+						return count;
+					p->check = buf[c];
+					write_ipack(p, buf + c, 1);
+					c++;
+					p->found++;
+					p->hlength++;
+					p->which = 2;
+					if (c == count)
+						return count;
+				}
+
+				if (p->which == 1) {
+					p->check = buf[c];
+					write_ipack(p, buf + c, 1);
+					c++;
+					p->found++;
+					p->hlength++;
+					p->which = 2;
+					if (c == count)
+						return count;
+				}
+
+				if ((p->check & 0x30) && p->check != 0xff) {
+					p->flag2 = (p->check & 0xf0) << 2;
+					p->pts[0] = p->check;
+					p->which = 3;
+				}
+
+				if (c == count)
+					return count;
+				if (p->which > 2){
+					if ((p->flag2 & PTS_DTS_FLAGS) == PTS_ONLY) {
+						while (c < count && p->which < 7) {
+							p->pts[p->which - 2] = buf[c];
+							write_ipack(p, buf + c, 1);
+							c++;
+							p->found++;
+							p->which++;
+							p->hlength++;
+						}
+						if (c == count)
+							return count;
+					} else if ((p->flag2 & PTS_DTS_FLAGS) == PTS_DTS) {
+						while (c < count && p->which < 12) {
+							if (p->which < 7)
+								p->pts[p->which - 2] = buf[c];
+							write_ipack(p, buf + c, 1);
+							c++;
+							p->found++;
+							p->which++;
+							p->hlength++;
+						}
+						if (c == count)
+							return count;
+					}
+					p->which = 2000;
+				}
+
+			}
+
+			while (c < count && p->found < p->plength + 6) {
+				l = count - c;
+				if (l + p->found > p->plength + 6)
+					l = p->plength + 6 - p->found;
+				write_ipack(p, buf + c, l);
+				p->found += l;
+				c += l;
+			}
+			break;
+		}
+
+
+		if (p->done) {
+			if (p->found + count - c < p->plength + 6) {
+				p->found += count - c;
+				c = count;
+			} else {
+				c += p->plength + 6 - p->found;
+				p->found = p->plength + 6;
+			}
+		}
+
+		if (p->plength && p->found == p->plength + 6) {
+			send_ipack(p);
+			av7110_ipack_reset(p);
+			if (c < count)
+				av7110_ipack_instant_repack(buf + c, count - c, p);
+		}
+	}
+	return count;
+}
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.h b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.h
new file mode 100644
index 000000000..943ec899b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ipack.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _AV7110_IPACK_H_
+#define _AV7110_IPACK_H_
+
+extern int av7110_ipack_init(struct ipack *p, int size,
+			     void (*func)(u8 *buf,  int size, void *priv));
+
+extern void av7110_ipack_reset(struct ipack *p);
+extern int  av7110_ipack_instant_repack(const u8 *buf, int count, struct ipack *p);
+extern void av7110_ipack_free(struct ipack * p);
+extern void av7110_ipack_flush(struct ipack *p);
+
+#endif
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_ir.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ir.c
new file mode 100644
index 000000000..a851ba328
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_ir.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Driver for the remote control of SAA7146 based AV7110 cards
+ *
+ * Copyright (C) 1999-2003 Holger Waechtler <holger@convergence.de>
+ * Copyright (C) 2003-2007 Oliver Endriss <o.endriss@gmx.de>
+ * Copyright (C) 2019 Sean Young <sean@mess.org>
+ */
+
+#include <linux/kernel.h>
+#include <media/rc-core.h>
+
+#include "av7110.h"
+#include "av7110_hw.h"
+
+#define IR_RC5		0
+#define IR_RCMM		1
+#define IR_RC5_EXT	2 /* internal only */
+
+/* interrupt handler */
+void av7110_ir_handler(struct av7110 *av7110, u32 ircom)
+{
+	struct rc_dev *rcdev = av7110->ir.rcdev;
+	enum rc_proto proto;
+	u32 command, addr, scancode;
+	u32 toggle;
+
+	dprintk(4, "ir command = %08x\n", ircom);
+
+	if (rcdev) {
+		switch (av7110->ir.ir_config) {
+		case IR_RC5: /* RC5: 5 bits device address, 6 bits command */
+			command = ircom & 0x3f;
+			addr = (ircom >> 6) & 0x1f;
+			scancode = RC_SCANCODE_RC5(addr, command);
+			toggle = ircom & 0x0800;
+			proto = RC_PROTO_RC5;
+			break;
+
+		case IR_RCMM: /* RCMM: 32 bits scancode */
+			scancode = ircom & ~0x8000;
+			toggle = ircom & 0x8000;
+			proto = RC_PROTO_RCMM32;
+			break;
+
+		case IR_RC5_EXT:
+			/*
+			 * extended RC5: 5 bits device address, 7 bits command
+			 *
+			 * Extended RC5 uses only one start bit. The second
+			 * start bit is re-assigned bit 6 of the command bit.
+			 */
+			command = ircom & 0x3f;
+			addr = (ircom >> 6) & 0x1f;
+			if (!(ircom & 0x1000))
+				command |= 0x40;
+			scancode = RC_SCANCODE_RC5(addr, command);
+			toggle = ircom & 0x0800;
+			proto = RC_PROTO_RC5;
+			break;
+		default:
+			dprintk(2, "unknown ir config %d\n",
+				av7110->ir.ir_config);
+			return;
+		}
+
+		rc_keydown(rcdev, proto, scancode, toggle != 0);
+	}
+}
+
+int av7110_set_ir_config(struct av7110 *av7110)
+{
+	dprintk(4, "ir config = %08x\n", av7110->ir.ir_config);
+
+	return av7110_fw_cmd(av7110, COMTYPE_PIDFILTER, SetIR, 1,
+			     av7110->ir.ir_config);
+}
+
+static int change_protocol(struct rc_dev *rcdev, u64 *rc_type)
+{
+	struct av7110 *av7110 = rcdev->priv;
+	u32 ir_config;
+
+	if (*rc_type & RC_PROTO_BIT_RCMM32) {
+		ir_config = IR_RCMM;
+		*rc_type = RC_PROTO_BIT_RCMM32;
+	} else if (*rc_type & RC_PROTO_BIT_RC5) {
+		if (FW_VERSION(av7110->arm_app) >= 0x2620)
+			ir_config = IR_RC5_EXT;
+		else
+			ir_config = IR_RC5;
+		*rc_type = RC_PROTO_BIT_RC5;
+	} else {
+		return -EINVAL;
+	}
+
+	if (ir_config == av7110->ir.ir_config)
+		return 0;
+
+	av7110->ir.ir_config = ir_config;
+
+	return av7110_set_ir_config(av7110);
+}
+
+int av7110_ir_init(struct av7110 *av7110)
+{
+	struct rc_dev *rcdev;
+	struct pci_dev *pci;
+	int ret;
+
+	rcdev = rc_allocate_device(RC_DRIVER_SCANCODE);
+	if (!rcdev)
+		return -ENOMEM;
+
+	pci = av7110->dev->pci;
+
+	snprintf(av7110->ir.input_phys, sizeof(av7110->ir.input_phys),
+		 "pci-%s/ir0", pci_name(pci));
+
+	rcdev->device_name = av7110->card_name;
+	rcdev->driver_name = KBUILD_MODNAME;
+	rcdev->input_phys = av7110->ir.input_phys;
+	rcdev->input_id.bustype = BUS_PCI;
+	rcdev->input_id.version = 2;
+	if (pci->subsystem_vendor) {
+		rcdev->input_id.vendor	= pci->subsystem_vendor;
+		rcdev->input_id.product = pci->subsystem_device;
+	} else {
+		rcdev->input_id.vendor	= pci->vendor;
+		rcdev->input_id.product = pci->device;
+	}
+
+	rcdev->dev.parent = &pci->dev;
+	rcdev->allowed_protocols = RC_PROTO_BIT_RC5 | RC_PROTO_BIT_RCMM32;
+	rcdev->change_protocol = change_protocol;
+	rcdev->map_name = RC_MAP_HAUPPAUGE;
+	rcdev->priv = av7110;
+
+	av7110->ir.rcdev = rcdev;
+	av7110->ir.ir_config = IR_RC5;
+	av7110_set_ir_config(av7110);
+
+	ret = rc_register_device(rcdev);
+	if (ret) {
+		av7110->ir.rcdev = NULL;
+		rc_free_device(rcdev);
+	}
+
+	return ret;
+}
+
+void av7110_ir_exit(struct av7110 *av7110)
+{
+	rc_unregister_device(av7110->ir.rcdev);
+}
+
+//MODULE_AUTHOR("Holger Waechtler <holger@convergence.de>, Oliver Endriss <o.endriss@gmx.de>");
+//MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/av7110_v4l.c b/drivers/staging/media/deprecated/saa7146/av7110/av7110_v4l.c
new file mode 100644
index 000000000..c89f536f6
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/av7110_v4l.c
@@ -0,0 +1,952 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * av7110_v4l.c: av7110 video4linux interface for DVB and Siemens DVB-C analog module
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * originally based on code by:
+ * Copyright (C) 1998,1999 Christian Theiss <mistert@rz.fh-augsburg.de>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/poll.h>
+
+#include "av7110.h"
+#include "av7110_hw.h"
+#include "av7110_av.h"
+
+int msp_writereg(struct av7110 *av7110, u8 dev, u16 reg, u16 val)
+{
+	u8 msg[5] = { dev, reg >> 8, reg & 0xff, val >> 8 , val & 0xff };
+	struct i2c_msg msgs = { .flags = 0, .len = 5, .buf = msg };
+
+	switch (av7110->adac_type) {
+	case DVB_ADAC_MSP34x0:
+		msgs.addr = 0x40;
+		break;
+	case DVB_ADAC_MSP34x5:
+		msgs.addr = 0x42;
+		break;
+	default:
+		return 0;
+	}
+
+	if (i2c_transfer(&av7110->i2c_adap, &msgs, 1) != 1) {
+		dprintk(1, "dvb-ttpci: failed @ card %d, %u = %u\n",
+		       av7110->dvb_adapter.num, reg, val);
+		return -EIO;
+	}
+	return 0;
+}
+
+static int msp_readreg(struct av7110 *av7110, u8 dev, u16 reg, u16 *val)
+{
+	u8 msg1[3] = { dev, reg >> 8, reg & 0xff };
+	u8 msg2[2];
+	struct i2c_msg msgs[2] = {
+		{ .flags = 0	   , .len = 3, .buf = msg1 },
+		{ .flags = I2C_M_RD, .len = 2, .buf = msg2 }
+	};
+
+	switch (av7110->adac_type) {
+	case DVB_ADAC_MSP34x0:
+		msgs[0].addr = 0x40;
+		msgs[1].addr = 0x40;
+		break;
+	case DVB_ADAC_MSP34x5:
+		msgs[0].addr = 0x42;
+		msgs[1].addr = 0x42;
+		break;
+	default:
+		return 0;
+	}
+
+	if (i2c_transfer(&av7110->i2c_adap, &msgs[0], 2) != 2) {
+		dprintk(1, "dvb-ttpci: failed @ card %d, %u\n",
+		       av7110->dvb_adapter.num, reg);
+		return -EIO;
+	}
+	*val = (msg2[0] << 8) | msg2[1];
+	return 0;
+}
+
+static struct v4l2_input inputs[4] = {
+	{
+		.index		= 0,
+		.name		= "DVB",
+		.type		= V4L2_INPUT_TYPE_CAMERA,
+		.audioset	= 1,
+		.tuner		= 0, /* ignored */
+		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
+		.status		= 0,
+		.capabilities	= V4L2_IN_CAP_STD,
+	}, {
+		.index		= 1,
+		.name		= "Television",
+		.type		= V4L2_INPUT_TYPE_TUNER,
+		.audioset	= 1,
+		.tuner		= 0,
+		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
+		.status		= 0,
+		.capabilities	= V4L2_IN_CAP_STD,
+	}, {
+		.index		= 2,
+		.name		= "Video",
+		.type		= V4L2_INPUT_TYPE_CAMERA,
+		.audioset	= 0,
+		.tuner		= 0,
+		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
+		.status		= 0,
+		.capabilities	= V4L2_IN_CAP_STD,
+	}, {
+		.index		= 3,
+		.name		= "Y/C",
+		.type		= V4L2_INPUT_TYPE_CAMERA,
+		.audioset	= 0,
+		.tuner		= 0,
+		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
+		.status		= 0,
+		.capabilities	= V4L2_IN_CAP_STD,
+	}
+};
+
+static int ves1820_writereg(struct saa7146_dev *dev, u8 addr, u8 reg, u8 data)
+{
+	struct av7110 *av7110 = dev->ext_priv;
+	u8 buf[] = { 0x00, reg, data };
+	struct i2c_msg msg = { .addr = addr, .flags = 0, .buf = buf, .len = 3 };
+
+	dprintk(4, "dev: %p\n", dev);
+
+	if (1 != i2c_transfer(&av7110->i2c_adap, &msg, 1))
+		return -1;
+	return 0;
+}
+
+static int tuner_write(struct saa7146_dev *dev, u8 addr, u8 data [4])
+{
+	struct av7110 *av7110 = dev->ext_priv;
+	struct i2c_msg msg = { .addr = addr, .flags = 0, .buf = data, .len = 4 };
+
+	dprintk(4, "dev: %p\n", dev);
+
+	if (1 != i2c_transfer(&av7110->i2c_adap, &msg, 1))
+		return -1;
+	return 0;
+}
+
+static int ves1820_set_tv_freq(struct saa7146_dev *dev, u32 freq)
+{
+	u32 div;
+	u8 config;
+	u8 buf[4];
+
+	dprintk(4, "freq: 0x%08x\n", freq);
+
+	/* magic number: 614. tuning with the frequency given by v4l2
+	   is always off by 614*62.5 = 38375 kHz...*/
+	div = freq + 614;
+
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = 0x8e;
+
+	if (freq < 16U * 16825 / 100)
+		config = 0xa0;
+	else if (freq < 16U * 44725 / 100)
+		config = 0x90;
+	else
+		config = 0x30;
+	config &= ~0x02;
+
+	buf[3] = config;
+
+	return tuner_write(dev, 0x61, buf);
+}
+
+static int stv0297_set_tv_freq(struct saa7146_dev *dev, u32 freq)
+{
+	struct av7110 *av7110 = (struct av7110*)dev->ext_priv;
+	u32 div;
+	u8 data[4];
+
+	div = (freq + 38900000 + 31250) / 62500;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0xce;
+
+	if (freq < 45000000)
+		return -EINVAL;
+	else if (freq < 137000000)
+		data[3] = 0x01;
+	else if (freq < 403000000)
+		data[3] = 0x02;
+	else if (freq < 860000000)
+		data[3] = 0x04;
+	else
+		return -EINVAL;
+
+	if (av7110->fe->ops.i2c_gate_ctrl)
+		av7110->fe->ops.i2c_gate_ctrl(av7110->fe, 1);
+	return tuner_write(dev, 0x63, data);
+}
+
+
+
+static struct saa7146_standard analog_standard[];
+static struct saa7146_standard dvb_standard[];
+static struct saa7146_standard standard[];
+
+static const struct v4l2_audio msp3400_v4l2_audio = {
+	.index = 0,
+	.name = "Television",
+	.capability = V4L2_AUDCAP_STEREO
+};
+
+static int av7110_dvb_c_switch(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct av7110 *av7110 = (struct av7110*)dev->ext_priv;
+	u16 adswitch;
+	int source, sync, err;
+
+	dprintk(4, "%p\n", av7110);
+
+	if ((vv->video_status & STATUS_OVERLAY) != 0) {
+		vv->ov_suspend = vv->video_fh;
+		err = saa7146_stop_preview(vv->video_fh); /* side effect: video_status is now 0, video_fh is NULL */
+		if (err != 0) {
+			dprintk(2, "suspending video failed\n");
+			vv->ov_suspend = NULL;
+		}
+	}
+
+	if (0 != av7110->current_input) {
+		dprintk(1, "switching to analog TV:\n");
+		adswitch = 1;
+		source = SAA7146_HPS_SOURCE_PORT_B;
+		sync = SAA7146_HPS_SYNC_PORT_B;
+		memcpy(standard, analog_standard, sizeof(struct saa7146_standard) * 2);
+
+		switch (av7110->current_input) {
+		case 1:
+			dprintk(1, "switching SAA7113 to Analog Tuner Input\n");
+			msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0000); // loudspeaker source
+			msp_writereg(av7110, MSP_WR_DSP, 0x0009, 0x0000); // headphone source
+			msp_writereg(av7110, MSP_WR_DSP, 0x000a, 0x0000); // SCART 1 source
+			msp_writereg(av7110, MSP_WR_DSP, 0x000e, 0x3000); // FM matrix, mono
+			msp_writereg(av7110, MSP_WR_DSP, 0x0000, 0x4f00); // loudspeaker + headphone
+			msp_writereg(av7110, MSP_WR_DSP, 0x0007, 0x4f00); // SCART 1 volume
+
+			if (av7110->analog_tuner_flags & ANALOG_TUNER_VES1820) {
+				if (ves1820_writereg(dev, 0x09, 0x0f, 0x60))
+					dprintk(1, "setting band in demodulator failed\n");
+			} else if (av7110->analog_tuner_flags & ANALOG_TUNER_STV0297) {
+				saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI); // TDA9819 pin9(STD)
+				saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI); // TDA9819 pin30(VIF)
+			}
+			if (i2c_writereg(av7110, 0x48, 0x02, 0xd0) != 1)
+				dprintk(1, "saa7113 write failed @ card %d", av7110->dvb_adapter.num);
+			break;
+		case 2:
+			dprintk(1, "switching SAA7113 to Video AV CVBS Input\n");
+			if (i2c_writereg(av7110, 0x48, 0x02, 0xd2) != 1)
+				dprintk(1, "saa7113 write failed @ card %d", av7110->dvb_adapter.num);
+			break;
+		case 3:
+			dprintk(1, "switching SAA7113 to Video AV Y/C Input\n");
+			if (i2c_writereg(av7110, 0x48, 0x02, 0xd9) != 1)
+				dprintk(1, "saa7113 write failed @ card %d", av7110->dvb_adapter.num);
+			break;
+		default:
+			dprintk(1, "switching SAA7113 to Input: AV7110: SAA7113: invalid input\n");
+		}
+	} else {
+		adswitch = 0;
+		source = SAA7146_HPS_SOURCE_PORT_A;
+		sync = SAA7146_HPS_SYNC_PORT_A;
+		memcpy(standard, dvb_standard, sizeof(struct saa7146_standard) * 2);
+		dprintk(1, "switching DVB mode\n");
+		msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0220); // loudspeaker source
+		msp_writereg(av7110, MSP_WR_DSP, 0x0009, 0x0220); // headphone source
+		msp_writereg(av7110, MSP_WR_DSP, 0x000a, 0x0220); // SCART 1 source
+		msp_writereg(av7110, MSP_WR_DSP, 0x000e, 0x3000); // FM matrix, mono
+		msp_writereg(av7110, MSP_WR_DSP, 0x0000, 0x7f00); // loudspeaker + headphone
+		msp_writereg(av7110, MSP_WR_DSP, 0x0007, 0x7f00); // SCART 1 volume
+
+		if (av7110->analog_tuner_flags & ANALOG_TUNER_VES1820) {
+			if (ves1820_writereg(dev, 0x09, 0x0f, 0x20))
+				dprintk(1, "setting band in demodulator failed\n");
+		} else if (av7110->analog_tuner_flags & ANALOG_TUNER_STV0297) {
+			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTLO); // TDA9819 pin9(STD)
+			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO); // TDA9819 pin30(VIF)
+		}
+	}
+
+	/* hmm, this does not do anything!? */
+	if (av7110_fw_cmd(av7110, COMTYPE_AUDIODAC, ADSwitch, 1, adswitch))
+		dprintk(1, "ADSwitch error\n");
+
+	saa7146_set_hps_source_and_sync(dev, source, sync);
+
+	if (vv->ov_suspend != NULL) {
+		saa7146_start_preview(vv->ov_suspend);
+		vv->ov_suspend = NULL;
+	}
+
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+	u16 stereo_det;
+	s8 stereo;
+
+	dprintk(2, "VIDIOC_G_TUNER: %d\n", t->index);
+
+	if (!av7110->analog_tuner_flags || t->index != 0)
+		return -EINVAL;
+
+	memset(t, 0, sizeof(*t));
+	strscpy((char *)t->name, "Television", sizeof(t->name));
+
+	t->type = V4L2_TUNER_ANALOG_TV;
+	t->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2 | V4L2_TUNER_CAP_SAP;
+	t->rangelow = 772;	/* 48.25 MHZ / 62.5 kHz = 772, see fi1216mk2-specs, page 2 */
+	t->rangehigh = 13684;	/* 855.25 MHz / 62.5 kHz = 13684 */
+	/* FIXME: add the real signal strength here */
+	t->signal = 0xffff;
+	t->afc = 0;
+
+	/* FIXME: standard / stereo detection is still broken */
+	msp_readreg(av7110, MSP_RD_DEM, 0x007e, &stereo_det);
+	dprintk(1, "VIDIOC_G_TUNER: msp3400 TV standard detection: 0x%04x\n", stereo_det);
+	msp_readreg(av7110, MSP_RD_DSP, 0x0018, &stereo_det);
+	dprintk(1, "VIDIOC_G_TUNER: msp3400 stereo detection: 0x%04x\n", stereo_det);
+	stereo = (s8)(stereo_det >> 8);
+	if (stereo > 0x10) {
+		/* stereo */
+		t->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_MONO;
+		t->audmode = V4L2_TUNER_MODE_STEREO;
+	} else if (stereo < -0x10) {
+		/* bilingual */
+		t->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+	} else /* mono */
+		t->rxsubchans = V4L2_TUNER_SUB_MONO;
+
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+	u16 fm_matrix, src;
+	dprintk(2, "VIDIOC_S_TUNER: %d\n", t->index);
+
+	if (!av7110->analog_tuner_flags || av7110->current_input != 1)
+		return -EINVAL;
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_STEREO:
+		dprintk(2, "VIDIOC_S_TUNER: V4L2_TUNER_MODE_STEREO\n");
+		fm_matrix = 0x3001; /* stereo */
+		src = 0x0020;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		dprintk(2, "VIDIOC_S_TUNER: V4L2_TUNER_MODE_LANG1_LANG2\n");
+		fm_matrix = 0x3000; /* bilingual */
+		src = 0x0020;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		dprintk(2, "VIDIOC_S_TUNER: V4L2_TUNER_MODE_LANG1\n");
+		fm_matrix = 0x3000; /* mono */
+		src = 0x0000;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		dprintk(2, "VIDIOC_S_TUNER: V4L2_TUNER_MODE_LANG2\n");
+		fm_matrix = 0x3000; /* mono */
+		src = 0x0010;
+		break;
+	default: /* case V4L2_TUNER_MODE_MONO: */
+		dprintk(2, "VIDIOC_S_TUNER: TDA9840_SET_MONO\n");
+		fm_matrix = 0x3000; /* mono */
+		src = 0x0030;
+		break;
+	}
+	msp_writereg(av7110, MSP_WR_DSP, 0x000e, fm_matrix);
+	msp_writereg(av7110, MSP_WR_DSP, 0x0008, src);
+	msp_writereg(av7110, MSP_WR_DSP, 0x0009, src);
+	msp_writereg(av7110, MSP_WR_DSP, 0x000a, src);
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_G_FREQ: freq:0x%08x\n", f->frequency);
+
+	if (!av7110->analog_tuner_flags || av7110->current_input != 1)
+		return -EINVAL;
+
+	memset(f, 0, sizeof(*f));
+	f->type = V4L2_TUNER_ANALOG_TV;
+	f->frequency =	av7110->current_freq;
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_S_FREQUENCY: freq:0x%08x\n", f->frequency);
+
+	if (!av7110->analog_tuner_flags || av7110->current_input != 1)
+		return -EINVAL;
+
+	if (V4L2_TUNER_ANALOG_TV != f->type)
+		return -EINVAL;
+
+	msp_writereg(av7110, MSP_WR_DSP, 0x0000, 0xffe0); /* fast mute */
+	msp_writereg(av7110, MSP_WR_DSP, 0x0007, 0xffe0);
+
+	/* tune in desired frequency */
+	if (av7110->analog_tuner_flags & ANALOG_TUNER_VES1820)
+		ves1820_set_tv_freq(dev, f->frequency);
+	else if (av7110->analog_tuner_flags & ANALOG_TUNER_STV0297)
+		stv0297_set_tv_freq(dev, f->frequency);
+	av7110->current_freq = f->frequency;
+
+	msp_writereg(av7110, MSP_WR_DSP, 0x0015, 0x003f); /* start stereo detection */
+	msp_writereg(av7110, MSP_WR_DSP, 0x0015, 0x0000);
+	msp_writereg(av7110, MSP_WR_DSP, 0x0000, 0x4f00); /* loudspeaker + headphone */
+	msp_writereg(av7110, MSP_WR_DSP, 0x0007, 0x4f00); /* SCART 1 volume */
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_ENUMINPUT: %d\n", i->index);
+
+	if (av7110->analog_tuner_flags) {
+		if (i->index >= 4)
+			return -EINVAL;
+	} else {
+		if (i->index != 0)
+			return -EINVAL;
+	}
+
+	memcpy(i, &inputs[i->index], sizeof(struct v4l2_input));
+
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	*input = av7110->current_input;
+	dprintk(2, "VIDIOC_G_INPUT: %d\n", *input);
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_S_INPUT: %d\n", input);
+
+	if (!av7110->analog_tuner_flags)
+		return input ? -EINVAL : 0;
+
+	if (input >= 4)
+		return -EINVAL;
+
+	av7110->current_input = input;
+	return av7110_dvb_c_switch(fh);
+}
+
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
+	if (a->index != 0)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
+	return 0;
+}
+
+static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
+	if (a->index != 0)
+		return -EINVAL;
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
+	return 0;
+}
+
+static int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_S_AUDIO: %d\n", a->index);
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	return a->index ? -EINVAL : 0;
+}
+
+static int vidioc_g_sliced_vbi_cap(struct file *file, void *fh,
+					struct v4l2_sliced_vbi_cap *cap)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_G_SLICED_VBI_CAP\n");
+	if (cap->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT)
+		return -EINVAL;
+	if (FW_VERSION(av7110->arm_app) >= 0x2623) {
+		cap->service_set = V4L2_SLICED_WSS_625;
+		cap->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	}
+	return 0;
+}
+
+static int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_G_FMT:\n");
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		return -EINVAL;
+	memset(&f->fmt.sliced, 0, sizeof f->fmt.sliced);
+	if (av7110->wssMode) {
+		f->fmt.sliced.service_set = V4L2_SLICED_WSS_625;
+		f->fmt.sliced.service_lines[0][23] = V4L2_SLICED_WSS_625;
+		f->fmt.sliced.io_size = sizeof(struct v4l2_sliced_vbi_data);
+	}
+	return 0;
+}
+
+static int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
+	dprintk(2, "VIDIOC_S_FMT\n");
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		return -EINVAL;
+	if (f->fmt.sliced.service_set != V4L2_SLICED_WSS_625 &&
+	    f->fmt.sliced.service_lines[0][23] != V4L2_SLICED_WSS_625) {
+		memset(&f->fmt.sliced, 0, sizeof(f->fmt.sliced));
+		/* WSS controlled by firmware */
+		av7110->wssMode = 0;
+		av7110->wssData = 0;
+		return av7110_fw_cmd(av7110, COMTYPE_ENCODER,
+				     SetWSSConfig, 1, 0);
+	} else {
+		memset(&f->fmt.sliced, 0, sizeof(f->fmt.sliced));
+		f->fmt.sliced.service_set = V4L2_SLICED_WSS_625;
+		f->fmt.sliced.service_lines[0][23] = V4L2_SLICED_WSS_625;
+		f->fmt.sliced.io_size = sizeof(struct v4l2_sliced_vbi_data);
+		/* WSS controlled by userspace */
+		av7110->wssMode = 1;
+		av7110->wssData = 0;
+	}
+	return 0;
+}
+
+static int av7110_vbi_reset(struct file *file)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct av7110 *av7110 = (struct av7110*) dev->ext_priv;
+
+	dprintk(2, "%s\n", __func__);
+	av7110->wssMode = 0;
+	av7110->wssData = 0;
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		return 0;
+	else
+		return av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetWSSConfig, 1, 0);
+}
+
+static ssize_t av7110_vbi_write(struct file *file, const char __user *data, size_t count, loff_t *ppos)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct av7110 *av7110 = (struct av7110*) dev->ext_priv;
+	struct v4l2_sliced_vbi_data d;
+	int rc;
+
+	dprintk(2, "%s\n", __func__);
+	if (FW_VERSION(av7110->arm_app) < 0x2623 || !av7110->wssMode || count != sizeof d)
+		return -EINVAL;
+	if (copy_from_user(&d, data, count))
+		return -EFAULT;
+	if ((d.id != 0 && d.id != V4L2_SLICED_WSS_625) || d.field != 0 || d.line != 23)
+		return -EINVAL;
+	if (d.id)
+		av7110->wssData = ((d.data[1] << 8) & 0x3f00) | d.data[0];
+	else
+		av7110->wssData = 0x8000;
+	rc = av7110_fw_cmd(av7110, COMTYPE_ENCODER, SetWSSConfig, 2, 1, av7110->wssData);
+	return (rc < 0) ? rc : count;
+}
+
+/****************************************************************************
+ * INITIALIZATION
+ ****************************************************************************/
+
+static u8 saa7113_init_regs[] = {
+	0x02, 0xd0,
+	0x03, 0x23,
+	0x04, 0x00,
+	0x05, 0x00,
+	0x06, 0xe9,
+	0x07, 0x0d,
+	0x08, 0x98,
+	0x09, 0x02,
+	0x0a, 0x80,
+	0x0b, 0x40,
+	0x0c, 0x40,
+	0x0d, 0x00,
+	0x0e, 0x01,
+	0x0f, 0x7c,
+	0x10, 0x48,
+	0x11, 0x0c,
+	0x12, 0x8b,
+	0x13, 0x1a,
+	0x14, 0x00,
+	0x15, 0x00,
+	0x16, 0x00,
+	0x17, 0x00,
+	0x18, 0x00,
+	0x19, 0x00,
+	0x1a, 0x00,
+	0x1b, 0x00,
+	0x1c, 0x00,
+	0x1d, 0x00,
+	0x1e, 0x00,
+
+	0x41, 0x77,
+	0x42, 0x77,
+	0x43, 0x77,
+	0x44, 0x77,
+	0x45, 0x77,
+	0x46, 0x77,
+	0x47, 0x77,
+	0x48, 0x77,
+	0x49, 0x77,
+	0x4a, 0x77,
+	0x4b, 0x77,
+	0x4c, 0x77,
+	0x4d, 0x77,
+	0x4e, 0x77,
+	0x4f, 0x77,
+	0x50, 0x77,
+	0x51, 0x77,
+	0x52, 0x77,
+	0x53, 0x77,
+	0x54, 0x77,
+	0x55, 0x77,
+	0x56, 0x77,
+	0x57, 0xff,
+
+	0xff
+};
+
+
+static struct saa7146_ext_vv av7110_vv_data_st;
+static struct saa7146_ext_vv av7110_vv_data_c;
+
+int av7110_init_analog_module(struct av7110 *av7110)
+{
+	u16 version1, version2;
+
+	if (i2c_writereg(av7110, 0x80, 0x0, 0x80) == 1 &&
+	    i2c_writereg(av7110, 0x80, 0x0, 0) == 1) {
+		pr_info("DVB-C analog module @ card %d detected, initializing MSP3400\n",
+			av7110->dvb_adapter.num);
+		av7110->adac_type = DVB_ADAC_MSP34x0;
+	} else if (i2c_writereg(av7110, 0x84, 0x0, 0x80) == 1 &&
+		   i2c_writereg(av7110, 0x84, 0x0, 0) == 1) {
+		pr_info("DVB-C analog module @ card %d detected, initializing MSP3415\n",
+			av7110->dvb_adapter.num);
+		av7110->adac_type = DVB_ADAC_MSP34x5;
+	} else
+		return -ENODEV;
+
+	msleep(100); // the probing above resets the msp...
+	msp_readreg(av7110, MSP_RD_DSP, 0x001e, &version1);
+	msp_readreg(av7110, MSP_RD_DSP, 0x001f, &version2);
+	dprintk(1, "dvb-ttpci: @ card %d MSP34xx version 0x%04x 0x%04x\n",
+		av7110->dvb_adapter.num, version1, version2);
+	msp_writereg(av7110, MSP_WR_DSP, 0x0013, 0x0c00);
+	msp_writereg(av7110, MSP_WR_DSP, 0x0000, 0x7f00); // loudspeaker + headphone
+	msp_writereg(av7110, MSP_WR_DSP, 0x0008, 0x0220); // loudspeaker source
+	msp_writereg(av7110, MSP_WR_DSP, 0x0009, 0x0220); // headphone source
+	msp_writereg(av7110, MSP_WR_DSP, 0x0004, 0x7f00); // loudspeaker volume
+	msp_writereg(av7110, MSP_WR_DSP, 0x000a, 0x0220); // SCART 1 source
+	msp_writereg(av7110, MSP_WR_DSP, 0x0007, 0x7f00); // SCART 1 volume
+	msp_writereg(av7110, MSP_WR_DSP, 0x000d, 0x1900); // prescale SCART
+
+	if (i2c_writereg(av7110, 0x48, 0x01, 0x00)!=1) {
+		pr_info("saa7113 not accessible\n");
+	} else {
+		u8 *i = saa7113_init_regs;
+
+		if ((av7110->dev->pci->subsystem_vendor == 0x110a) && (av7110->dev->pci->subsystem_device == 0x0000)) {
+			/* Fujitsu/Siemens DVB-Cable */
+			av7110->analog_tuner_flags |= ANALOG_TUNER_VES1820;
+		} else if ((av7110->dev->pci->subsystem_vendor == 0x13c2) && (av7110->dev->pci->subsystem_device == 0x0002)) {
+			/* Hauppauge/TT DVB-C premium */
+			av7110->analog_tuner_flags |= ANALOG_TUNER_VES1820;
+		} else if ((av7110->dev->pci->subsystem_vendor == 0x13c2) && (av7110->dev->pci->subsystem_device == 0x000A)) {
+			/* Hauppauge/TT DVB-C premium */
+			av7110->analog_tuner_flags |= ANALOG_TUNER_STV0297;
+		}
+
+		/* setup for DVB by default */
+		if (av7110->analog_tuner_flags & ANALOG_TUNER_VES1820) {
+			if (ves1820_writereg(av7110->dev, 0x09, 0x0f, 0x20))
+				dprintk(1, "setting band in demodulator failed\n");
+		} else if (av7110->analog_tuner_flags & ANALOG_TUNER_STV0297) {
+			saa7146_setgpio(av7110->dev, 1, SAA7146_GPIO_OUTLO); // TDA9819 pin9(STD)
+			saa7146_setgpio(av7110->dev, 3, SAA7146_GPIO_OUTLO); // TDA9819 pin30(VIF)
+		}
+
+		/* init the saa7113 */
+		while (*i != 0xff) {
+			if (i2c_writereg(av7110, 0x48, i[0], i[1]) != 1) {
+				dprintk(1, "saa7113 initialization failed @ card %d", av7110->dvb_adapter.num);
+				break;
+			}
+			i += 2;
+		}
+		/* setup msp for analog sound: B/G Dual-FM */
+		msp_writereg(av7110, MSP_WR_DEM, 0x00bb, 0x02d0); // AD_CV
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001,  3); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001, 18); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001, 27); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001, 48); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001, 66); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0001, 72); // FIR1
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005,  4); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 64); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005,  0); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005,  3); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 18); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 27); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 48); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 66); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0005, 72); // FIR2
+		msp_writereg(av7110, MSP_WR_DEM, 0x0083, 0xa000); // MODE_REG
+		msp_writereg(av7110, MSP_WR_DEM, 0x0093, 0x00aa); // DCO1_LO 5.74MHz
+		msp_writereg(av7110, MSP_WR_DEM, 0x009b, 0x04fc); // DCO1_HI
+		msp_writereg(av7110, MSP_WR_DEM, 0x00a3, 0x038e); // DCO2_LO 5.5MHz
+		msp_writereg(av7110, MSP_WR_DEM, 0x00ab, 0x04c6); // DCO2_HI
+		msp_writereg(av7110, MSP_WR_DEM, 0x0056, 0); // LOAD_REG 1/2
+	}
+
+	memcpy(standard, dvb_standard, sizeof(struct saa7146_standard) * 2);
+	/* set dd1 stream a & b */
+	saa7146_write(av7110->dev, DD1_STREAM_B, 0x00000000);
+	saa7146_write(av7110->dev, DD1_INIT, 0x03000700);
+	saa7146_write(av7110->dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	return 0;
+}
+
+int av7110_init_v4l(struct av7110 *av7110)
+{
+	struct saa7146_dev* dev = av7110->dev;
+	struct saa7146_ext_vv *vv_data;
+	int ret;
+
+	/* special case DVB-C: these cards have an analog tuner
+	   plus need some special handling, so we have separate
+	   saa7146_ext_vv data for these... */
+	if (av7110->analog_tuner_flags)
+		vv_data = &av7110_vv_data_c;
+	else
+		vv_data = &av7110_vv_data_st;
+	ret = saa7146_vv_init(dev, vv_data);
+
+	if (ret) {
+		ERR("cannot init capture device. skipping\n");
+		return -ENODEV;
+	}
+	vv_data->vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data->vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data->vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data->vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data->vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data->vid_ops.vidioc_s_audio = vidioc_s_audio;
+	vv_data->vid_ops.vidioc_g_fmt_vbi_cap = NULL;
+
+	vv_data->vbi_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vbi_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vbi_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vbi_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vbi_ops.vidioc_g_fmt_vbi_cap = NULL;
+	vv_data->vbi_ops.vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap;
+	vv_data->vbi_ops.vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out;
+	vv_data->vbi_ops.vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out;
+
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		vv_data->capabilities &= ~V4L2_CAP_SLICED_VBI_OUTPUT;
+
+	if (saa7146_register_device(&av7110->v4l_dev, dev, "av7110", VFL_TYPE_VIDEO)) {
+		ERR("cannot register capture device. skipping\n");
+		saa7146_vv_release(dev);
+		return -ENODEV;
+	}
+	if (FW_VERSION(av7110->arm_app) >= 0x2623) {
+		if (saa7146_register_device(&av7110->vbi_dev, dev, "av7110", VFL_TYPE_VBI))
+			ERR("cannot register vbi v4l2 device. skipping\n");
+	}
+	return 0;
+}
+
+int av7110_exit_v4l(struct av7110 *av7110)
+{
+	struct saa7146_dev* dev = av7110->dev;
+
+	saa7146_unregister_device(&av7110->v4l_dev, av7110->dev);
+	saa7146_unregister_device(&av7110->vbi_dev, av7110->dev);
+
+	saa7146_vv_release(dev);
+
+	return 0;
+}
+
+
+
+/* FIXME: these values are experimental values that look better than the
+   values from the latest "official" driver -- at least for me... (MiHu) */
+static struct saa7146_standard standard[] = {
+	{
+		.name	= "PAL",	.id		= V4L2_STD_PAL_BG,
+		.v_offset	= 0x15,	.v_field	= 288,
+		.h_offset	= 0x48,	.h_pixels	= 708,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id		= V4L2_STD_NTSC,
+		.v_offset	= 0x10,	.v_field	= 244,
+		.h_offset	= 0x40,	.h_pixels	= 708,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}
+};
+
+static struct saa7146_standard analog_standard[] = {
+	{
+		.name	= "PAL",	.id		= V4L2_STD_PAL_BG,
+		.v_offset	= 0x1b,	.v_field	= 288,
+		.h_offset	= 0x08,	.h_pixels	= 708,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id		= V4L2_STD_NTSC,
+		.v_offset	= 0x10,	.v_field	= 244,
+		.h_offset	= 0x40,	.h_pixels	= 708,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}
+};
+
+static struct saa7146_standard dvb_standard[] = {
+	{
+		.name	= "PAL",	.id		= V4L2_STD_PAL_BG,
+		.v_offset	= 0x14,	.v_field	= 288,
+		.h_offset	= 0x48,	.h_pixels	= 708,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id		= V4L2_STD_NTSC,
+		.v_offset	= 0x10,	.v_field	= 244,
+		.h_offset	= 0x40,	.h_pixels	= 708,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}
+};
+
+static int std_callback(struct saa7146_dev* dev, struct saa7146_standard *std)
+{
+	struct av7110 *av7110 = (struct av7110*) dev->ext_priv;
+
+	if (std->id & V4L2_STD_PAL) {
+		av7110->vidmode = AV7110_VIDEO_MODE_PAL;
+		av7110_set_vidmode(av7110, av7110->vidmode);
+	}
+	else if (std->id & V4L2_STD_NTSC) {
+		av7110->vidmode = AV7110_VIDEO_MODE_NTSC;
+		av7110_set_vidmode(av7110, av7110->vidmode);
+	}
+	else
+		return -1;
+
+	return 0;
+}
+
+
+static struct saa7146_ext_vv av7110_vv_data_st = {
+	.inputs		= 1,
+	.audios		= 1,
+	.capabilities	= V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
+	.flags		= 0,
+
+	.stds		= &standard[0],
+	.num_stds	= ARRAY_SIZE(standard),
+	.std_callback	= &std_callback,
+
+	.vbi_fops.open	= av7110_vbi_reset,
+	.vbi_fops.release = av7110_vbi_reset,
+	.vbi_fops.write	= av7110_vbi_write,
+};
+
+static struct saa7146_ext_vv av7110_vv_data_c = {
+	.inputs		= 1,
+	.audios		= 1,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
+	.flags		= SAA7146_USE_PORT_B_FOR_VBI,
+
+	.stds		= &standard[0],
+	.num_stds	= ARRAY_SIZE(standard),
+	.std_callback	= &std_callback,
+
+	.vbi_fops.open	= av7110_vbi_reset,
+	.vbi_fops.release = av7110_vbi_reset,
+	.vbi_fops.write	= av7110_vbi_write,
+};
+
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/budget-patch.c b/drivers/staging/media/deprecated/saa7146/av7110/budget-patch.c
new file mode 100644
index 000000000..d173c8ade
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/budget-patch.c
@@ -0,0 +1,665 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * budget-patch.c: driver for Budget Patch,
+ * hardware modification of DVB-S cards enabling full TS
+ *
+ * Written by Emard <emard@softhome.net>
+ *
+ * Original idea by Roberto Deza <rdeza@unav.es>
+ *
+ * Special thanks to Holger Waechtler, Michael Hunold, Marian Durkovic
+ * and Metzlerbros
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#include "av7110.h"
+#include "av7110_hw.h"
+#include "budget.h"
+#include "stv0299.h"
+#include "ves1x93.h"
+#include "tda8083.h"
+
+#include "bsru6.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+#define budget_patch budget
+
+static struct saa7146_extension budget_extension;
+
+MAKE_BUDGET_INFO(ttbp, "TT-Budget/Patch DVB-S 1.x PCI", BUDGET_PATCH);
+//MAKE_BUDGET_INFO(satel,"TT-Budget/Patch SATELCO PCI", BUDGET_TT_HW_DISEQC);
+
+static const struct pci_device_id pci_tbl[] = {
+	MAKE_EXTENSION_PCI(ttbp,0x13c2, 0x0000),
+//        MAKE_EXTENSION_PCI(satel, 0x13c2, 0x1013),
+	{
+		.vendor    = 0,
+	}
+};
+
+/* those lines are for budget-patch to be tried
+** on a true budget card and observe the
+** behaviour of VSYNC generated by rps1.
+** this code was shamelessly copy/pasted from budget.c
+*/
+static void gpio_Set22K (struct budget *budget, int state)
+{
+	struct saa7146_dev *dev=budget->dev;
+	dprintk(2, "budget: %p\n", budget);
+	saa7146_setgpio(dev, 3, (state ? SAA7146_GPIO_OUTHI : SAA7146_GPIO_OUTLO));
+}
+
+/* Diseqc functions only for TT Budget card */
+/* taken from the Skyvision DVB driver by
+   Ralph Metzler <rjkm@metzlerbros.de> */
+
+static void DiseqcSendBit (struct budget *budget, int data)
+{
+	struct saa7146_dev *dev=budget->dev;
+	dprintk(2, "budget: %p\n", budget);
+
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+	udelay(data ? 500 : 1000);
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	udelay(data ? 1000 : 500);
+}
+
+static void DiseqcSendByte (struct budget *budget, int data)
+{
+	int i, par=1, d;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	for (i=7; i>=0; i--) {
+		d = (data>>i)&1;
+		par ^= d;
+		DiseqcSendBit(budget, d);
+	}
+
+	DiseqcSendBit(budget, par);
+}
+
+static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long burst)
+{
+	struct saa7146_dev *dev=budget->dev;
+	int i;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	mdelay(16);
+
+	for (i=0; i<len; i++)
+		DiseqcSendByte(budget, msg[i]);
+
+	mdelay(16);
+
+	if (burst!=-1) {
+		if (burst)
+			DiseqcSendByte(budget, 0xff);
+		else {
+			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+			mdelay(12);
+			udelay(500);
+			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+		}
+		msleep(20);
+	}
+
+	return 0;
+}
+
+/* shamelessly copy/pasted from budget.c */
+static int budget_set_tone(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		gpio_Set22K (budget, 1);
+		break;
+
+	case SEC_TONE_OFF:
+		gpio_Set22K (budget, 0);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int budget_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	SendDiSEqCMsg (budget, cmd->msg_len, cmd->msg, 0);
+
+	return 0;
+}
+
+static int budget_diseqc_send_burst(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	SendDiSEqCMsg (budget, 0, NULL, minicmd);
+
+	return 0;
+}
+
+static int budget_av7110_send_fw_cmd(struct budget_patch *budget, u16* buf, int length)
+{
+	int i;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	for (i = 2; i < length; i++)
+	{
+		  ttpci_budget_debiwrite(budget, DEBINOSWAP, COMMAND + 2*i, 2, (u32) buf[i], 0,0);
+		  msleep(5);
+	}
+	if (length)
+		  ttpci_budget_debiwrite(budget, DEBINOSWAP, COMMAND + 2, 2, (u32) buf[1], 0,0);
+	else
+		  ttpci_budget_debiwrite(budget, DEBINOSWAP, COMMAND + 2, 2, 0, 0,0);
+	msleep(5);
+	ttpci_budget_debiwrite(budget, DEBINOSWAP, COMMAND, 2, (u32) buf[0], 0,0);
+	msleep(5);
+	return 0;
+}
+
+static void av7110_set22k(struct budget_patch *budget, int state)
+{
+	u16 buf[2] = {( COMTYPE_AUDIODAC << 8) | (state ? ON22K : OFF22K), 0};
+
+	dprintk(2, "budget: %p\n", budget);
+	budget_av7110_send_fw_cmd(budget, buf, 2);
+}
+
+static int av7110_send_diseqc_msg(struct budget_patch *budget, int len, u8 *msg, int burst)
+{
+	int i;
+	u16 buf[18] = { ((COMTYPE_AUDIODAC << 8) | SendDiSEqC),
+		16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	dprintk(2, "budget: %p\n", budget);
+
+	if (len>10)
+		len=10;
+
+	buf[1] = len+2;
+	buf[2] = len;
+
+	if (burst != -1)
+		buf[3]=burst ? 0x01 : 0x00;
+	else
+		buf[3]=0xffff;
+
+	for (i=0; i<len; i++)
+		buf[i+4]=msg[i];
+
+	budget_av7110_send_fw_cmd(budget, buf, 18);
+	return 0;
+}
+
+static int budget_patch_set_tone(struct dvb_frontend *fe,
+				 enum fe_sec_tone_mode tone)
+{
+	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		av7110_set22k (budget, 1);
+		break;
+
+	case SEC_TONE_OFF:
+		av7110_set22k (budget, 0);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int budget_patch_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd)
+{
+	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
+
+	av7110_send_diseqc_msg (budget, cmd->msg_len, cmd->msg, 0);
+
+	return 0;
+}
+
+static int budget_patch_diseqc_send_burst(struct dvb_frontend *fe,
+					  enum fe_sec_mini_cmd minicmd)
+{
+	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
+
+	av7110_send_diseqc_msg (budget, 0, NULL, minicmd);
+
+	return 0;
+}
+
+static int alps_bsrv2_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
+	u8 pwr = 0;
+	u8 buf[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = buf, .len = sizeof(buf) };
+	u32 div = (p->frequency + 479500) / 125;
+
+	if (p->frequency > 2000000)
+		pwr = 3;
+	else if (p->frequency > 1800000)
+		pwr = 2;
+	else if (p->frequency > 1600000)
+		pwr = 1;
+	else if (p->frequency > 1200000)
+		pwr = 0;
+	else if (p->frequency >= 1100000)
+		pwr = 1;
+	else pwr = 2;
+
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = ((div & 0x18000) >> 10) | 0x95;
+	buf[3] = (pwr << 6) | 0x30;
+
+	// NOTE: since we're using a prescaler of 2, we set the
+	// divisor frequency to 62.5kHz and divide by 125 above
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct ves1x93_config alps_bsrv2_config = {
+	.demod_address = 0x08,
+	.xin = 90100000UL,
+	.invert_pwm = 0,
+};
+
+static int grundig_29504_451_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = p->frequency / 125;
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x8e;
+	data[3] = 0x00;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static struct tda8083_config grundig_29504_451_config = {
+	.demod_address = 0x68,
+};
+
+static void frontend_init(struct budget_patch* budget)
+{
+	switch(budget->dev->pci->subsystem_device) {
+	case 0x0000: // Hauppauge/TT WinTV DVB-S rev1.X
+	case 0x1013: // SATELCO Multimedia PCI
+
+		// try the ALPS BSRV2 first of all
+		budget->dvb_frontend = dvb_attach(ves1x93_attach, &alps_bsrv2_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsrv2_tuner_set_params;
+			budget->dvb_frontend->ops.diseqc_send_master_cmd = budget_patch_diseqc_send_master_cmd;
+			budget->dvb_frontend->ops.diseqc_send_burst = budget_patch_diseqc_send_burst;
+			budget->dvb_frontend->ops.set_tone = budget_patch_set_tone;
+			break;
+		}
+
+		// try the ALPS BSRU6 now
+		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+
+			budget->dvb_frontend->ops.diseqc_send_master_cmd = budget_diseqc_send_master_cmd;
+			budget->dvb_frontend->ops.diseqc_send_burst = budget_diseqc_send_burst;
+			budget->dvb_frontend->ops.set_tone = budget_set_tone;
+			break;
+		}
+
+		// Try the grundig 29504-451
+		budget->dvb_frontend = dvb_attach(tda8083_attach, &grundig_29504_451_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = grundig_29504_451_tuner_set_params;
+			budget->dvb_frontend->ops.diseqc_send_master_cmd = budget_diseqc_send_master_cmd;
+			budget->dvb_frontend->ops.diseqc_send_burst = budget_diseqc_send_burst;
+			budget->dvb_frontend->ops.set_tone = budget_set_tone;
+			break;
+		}
+		break;
+	}
+
+	if (budget->dvb_frontend == NULL) {
+		printk("dvb-ttpci: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		       budget->dev->pci->vendor,
+		       budget->dev->pci->device,
+		       budget->dev->pci->subsystem_vendor,
+		       budget->dev->pci->subsystem_device);
+	} else {
+		if (dvb_register_frontend(&budget->dvb_adapter, budget->dvb_frontend)) {
+			printk("budget-av: Frontend registration failed!\n");
+			dvb_frontend_detach(budget->dvb_frontend);
+			budget->dvb_frontend = NULL;
+		}
+	}
+}
+
+/* written by Emard */
+static int budget_patch_attach (struct saa7146_dev* dev, struct saa7146_pci_extension_data *info)
+{
+	struct budget_patch *budget;
+	int err;
+	int count = 0;
+	int detected = 0;
+
+#define PATCH_RESET 0
+#define RPS_IRQ 0
+#define HPS_SETUP 0
+#if PATCH_RESET
+	saa7146_write(dev, MC1, MASK_31);
+	msleep(40);
+#endif
+#if HPS_SETUP
+	// initialize registers. Better to have it like this
+	// than leaving something unconfigured
+	saa7146_write(dev, DD1_STREAM_B, 0);
+	// port B VSYNC at rising edge
+	saa7146_write(dev, DD1_INIT, 0x00000200);  // have this in budget-core too!
+	saa7146_write(dev, BRS_CTRL, 0x00000000);  // VBI
+
+	// debi config
+	// saa7146_write(dev, DEBI_CONFIG, MASK_30|MASK_28|MASK_18);
+
+	// zero all HPS registers
+	saa7146_write(dev, HPS_H_PRESCALE, 0);                  // r68
+	saa7146_write(dev, HPS_H_SCALE, 0);                     // r6c
+	saa7146_write(dev, BCS_CTRL, 0);                        // r70
+	saa7146_write(dev, HPS_V_SCALE, 0);                     // r60
+	saa7146_write(dev, HPS_V_GAIN, 0);                      // r64
+	saa7146_write(dev, CHROMA_KEY_RANGE, 0);                // r74
+	saa7146_write(dev, CLIP_FORMAT_CTRL, 0);                // r78
+	// Set HPS prescaler for port B input
+	saa7146_write(dev, HPS_CTRL, (1<<30) | (0<<29) | (1<<28) | (0<<12) );
+	saa7146_write(dev, MC2,
+	  0 * (MASK_08 | MASK_24)  |   // BRS control
+	  0 * (MASK_09 | MASK_25)  |   // a
+	  0 * (MASK_10 | MASK_26)  |   // b
+	  1 * (MASK_06 | MASK_22)  |   // HPS_CTRL1
+	  1 * (MASK_05 | MASK_21)  |   // HPS_CTRL2
+	  0 * (MASK_01 | MASK_15)      // DEBI
+	   );
+#endif
+	// Disable RPS1 and RPS0
+	saa7146_write(dev, MC1, ( MASK_29 | MASK_28));
+	// RPS1 timeout disable
+	saa7146_write(dev, RPS_TOV1, 0);
+
+	// code for autodetection
+	// will wait for VBI_B event (vertical blank at port B)
+	// and will reset GPIO3 after VBI_B is detected.
+	// (GPIO3 should be raised high by CPU to
+	// test if GPIO3 will generate vertical blank signal
+	// in budget patch GPIO3 is connected to VSYNC_B
+	count = 0;
+#if 0
+	WRITE_RPS1(CMD_UPLOAD |
+	  MASK_10 | MASK_09 | MASK_08 | MASK_06 | MASK_05 | MASK_04 | MASK_03 | MASK_02 );
+#endif
+	WRITE_RPS1(CMD_PAUSE | EVT_VBI_B);
+	WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+	WRITE_RPS1(GPIO3_MSK);
+	WRITE_RPS1(SAA7146_GPIO_OUTLO<<24);
+#if RPS_IRQ
+	// issue RPS1 interrupt to increment counter
+	WRITE_RPS1(CMD_INTERRUPT);
+	// at least a NOP is neede between two interrupts
+	WRITE_RPS1(CMD_NOP);
+	// interrupt again
+	WRITE_RPS1(CMD_INTERRUPT);
+#endif
+	WRITE_RPS1(CMD_STOP);
+
+#if RPS_IRQ
+	// set event counter 1 source as RPS1 interrupt (0x03)          (rE4 p53)
+	// use 0x03 to track RPS1 interrupts - increase by 1 every gpio3 is toggled
+	// use 0x15 to track VPE  interrupts - increase by 1 every vpeirq() is called
+	saa7146_write(dev, EC1SSR, (0x03<<2) | 3 );
+	// set event counter 1 threshold to maximum allowed value        (rEC p55)
+	saa7146_write(dev, ECT1R,  0x3fff );
+#endif
+	// Fix VSYNC level
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	// Set RPS1 Address register to point to RPS code               (r108 p42)
+	saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+	// Enable RPS1,                                                 (rFC p33)
+	saa7146_write(dev, MC1, (MASK_13 | MASK_29 ));
+
+
+	mdelay(50);
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+	mdelay(150);
+
+
+	if( (saa7146_read(dev, GPIO_CTRL) & 0x10000000) == 0)
+		detected = 1;
+
+#if RPS_IRQ
+	printk("Event Counter 1 0x%04x\n", saa7146_read(dev, EC1R) & 0x3fff );
+#endif
+	// Disable RPS1
+	saa7146_write(dev, MC1, ( MASK_29 ));
+
+	if(detected == 0)
+		printk("budget-patch not detected or saa7146 in non-default state.\n"
+		       "try enabling resetting of 7146 with MASK_31 in MC1 register\n");
+
+	else
+		printk("BUDGET-PATCH DETECTED.\n");
+
+
+/*      OLD (Original design by Roberto Deza):
+**      This code will setup the SAA7146_RPS1 to generate a square
+**      wave on GPIO3, changing when a field (TS_HEIGHT/2 "lines" of
+**      TS_WIDTH packets) has been acquired on SAA7146_D1B video port;
+**      then, this GPIO3 output which is connected to the D1B_VSYNC
+**      input, will trigger the acquisition of the alternate field
+**      and so on.
+**      Currently, the TT_budget / WinTV_Nova cards have two ICs
+**      (74HCT4040, LVC74) for the generation of this VSYNC signal,
+**      which seems that can be done perfectly without this :-)).
+*/
+
+/*      New design (By Emard)
+**      this rps1 code will copy internal HS event to GPIO3 pin.
+**      GPIO3 is in budget-patch hardware connected to port B VSYNC
+
+**      HS is an internal event of 7146, accessible with RPS
+**      and temporarily raised high every n lines
+**      (n in defined in the RPS_THRESH1 counter threshold)
+**      I think HS is raised high on the beginning of the n-th line
+**      and remains high until this n-th line that triggered
+**      it is completely received. When the reception of n-th line
+**      ends, HS is lowered.
+
+**      To transmit data over DMA, 7146 needs changing state at
+**      port B VSYNC pin. Any changing of port B VSYNC will
+**      cause some DMA data transfer, with more or less packets loss.
+**      It depends on the phase and frequency of VSYNC and
+**      the way of 7146 is instructed to trigger on port B (defined
+**      in DD1_INIT register, 3rd nibble from the right valid
+**      numbers are 0-7, see datasheet)
+**
+**      The correct triggering can minimize packet loss,
+**      dvbtraffic should give this stable bandwidths:
+**        22k transponder = 33814 kbit/s
+**      27.5k transponder = 38045 kbit/s
+**      by experiment it is found that the best results
+**      (stable bandwidths and almost no packet loss)
+**      are obtained using DD1_INIT triggering number 2
+**      (Va at rising edge of VS Fa = HS x VS-failing forced toggle)
+**      and a VSYNC phase that occurs in the middle of DMA transfer
+**      (about byte 188*512=96256 in the DMA window).
+**
+**      Phase of HS is still not clear to me how to control,
+**      It just happens to be so. It can be seen if one enables
+**      RPS_IRQ and print Event Counter 1 in vpeirq(). Every
+**      time RPS_INTERRUPT is called, the Event Counter 1 will
+**      increment. That's how the 7146 is programmed to do event
+**      counting in this budget-patch.c
+**      I *think* HPS setting has something to do with the phase
+**      of HS but I can't be 100% sure in that.
+
+**      hardware debug note: a working budget card (including budget patch)
+**      with vpeirq() interrupt setup in mode "0x90" (every 64K) will
+**      generate 3 interrupts per 25-Hz DMA frame of 2*188*512 bytes
+**      and that means 3*25=75 Hz of interrupt frequency, as seen by
+**      watch cat /proc/interrupts
+**
+**      If this frequency is 3x lower (and data received in the DMA
+**      buffer don't start with 0x47, but in the middle of packets,
+**      whose lengths appear to be like 188 292 188 104 etc.
+**      this means VSYNC line is not connected in the hardware.
+**      (check soldering pcb and pins)
+**      The same behaviour of missing VSYNC can be duplicated on budget
+**      cards, by setting DD1_INIT trigger mode 7 in 3rd nibble.
+*/
+
+	// Setup RPS1 "program" (p35)
+	count = 0;
+
+
+	// Wait Source Line Counter Threshold                           (p36)
+	WRITE_RPS1(CMD_PAUSE | EVT_HS);
+	// Set GPIO3=1                                                  (p42)
+	WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+	WRITE_RPS1(GPIO3_MSK);
+	WRITE_RPS1(SAA7146_GPIO_OUTHI<<24);
+#if RPS_IRQ
+	// issue RPS1 interrupt
+	WRITE_RPS1(CMD_INTERRUPT);
+#endif
+	// Wait reset Source Line Counter Threshold                     (p36)
+	WRITE_RPS1(CMD_PAUSE | RPS_INV | EVT_HS);
+	// Set GPIO3=0                                                  (p42)
+	WRITE_RPS1(CMD_WR_REG_MASK | (GPIO_CTRL>>2));
+	WRITE_RPS1(GPIO3_MSK);
+	WRITE_RPS1(SAA7146_GPIO_OUTLO<<24);
+#if RPS_IRQ
+	// issue RPS1 interrupt
+	WRITE_RPS1(CMD_INTERRUPT);
+#endif
+	// Jump to begin of RPS program                                 (p37)
+	WRITE_RPS1(CMD_JUMP);
+	WRITE_RPS1(dev->d_rps1.dma_handle);
+
+	// Fix VSYNC level
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	// Set RPS1 Address register to point to RPS code               (r108 p42)
+	saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+
+	if (!(budget = kmalloc (sizeof(struct budget_patch), GFP_KERNEL)))
+		return -ENOMEM;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	err = ttpci_budget_init(budget, dev, info, THIS_MODULE, adapter_nr);
+	if (err) {
+		kfree(budget);
+		return err;
+	}
+
+	// Set Source Line Counter Threshold, using BRS                 (rCC p43)
+	// It generates HS event every TS_HEIGHT lines
+	// this is related to TS_WIDTH set in register
+	// NUM_LINE_BYTE3 in budget-core.c. If NUM_LINE_BYTE
+	// low 16 bits are set to TS_WIDTH bytes (TS_WIDTH=2*188
+	//,then RPS_THRESH1
+	// should be set to trigger every TS_HEIGHT (512) lines.
+	//
+	saa7146_write(dev, RPS_THRESH1, budget->buffer_height | MASK_12 );
+
+	// saa7146_write(dev, RPS_THRESH0, ((TS_HEIGHT/2)<<16) |MASK_28| (TS_HEIGHT/2) |MASK_12 );
+	// Enable RPS1                                                  (rFC p33)
+	saa7146_write(dev, MC1, (MASK_13 | MASK_29));
+
+
+	dev->ext_priv = budget;
+
+	budget->dvb_adapter.priv = budget;
+	frontend_init(budget);
+
+	ttpci_budget_init_hooks(budget);
+
+	return 0;
+}
+
+static int budget_patch_detach (struct saa7146_dev* dev)
+{
+	struct budget_patch *budget = (struct budget_patch*) dev->ext_priv;
+	int err;
+
+	if (budget->dvb_frontend) {
+		dvb_unregister_frontend(budget->dvb_frontend);
+		dvb_frontend_detach(budget->dvb_frontend);
+	}
+	err = ttpci_budget_deinit (budget);
+
+	kfree (budget);
+
+	return err;
+}
+
+static int __init budget_patch_init(void)
+{
+	return saa7146_register_extension(&budget_extension);
+}
+
+static void __exit budget_patch_exit(void)
+{
+	saa7146_unregister_extension(&budget_extension);
+}
+
+static struct saa7146_extension budget_extension = {
+	.name           = "budget_patch dvb",
+	.flags          = 0,
+
+	.module         = THIS_MODULE,
+	.pci_tbl        = pci_tbl,
+	.attach         = budget_patch_attach,
+	.detach         = budget_patch_detach,
+
+	.irq_mask       = MASK_10,
+	.irq_func       = ttpci_budget_irq10_handler,
+};
+
+module_init(budget_patch_init);
+module_exit(budget_patch_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Emard, Roberto Deza, Holger Waechtler, Michael Hunold, others");
+MODULE_DESCRIPTION("Driver for full TS modified DVB-S SAA7146+AV7110 based so-called Budget Patch cards");
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.c b/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.c
new file mode 100644
index 000000000..8c2eca5dc
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.c
@@ -0,0 +1,115 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include "dvb_filter.h"
+
+static u32 freq[4] = {480, 441, 320, 0};
+
+static unsigned int ac3_bitrates[32] =
+    {32,40,48,56,64,80,96,112,128,160,192,224,256,320,384,448,512,576,640,
+     0,0,0,0,0,0,0,0,0,0,0,0,0};
+
+static u32 ac3_frames[3][32] =
+    {{64,80,96,112,128,160,192,224,256,320,384,448,512,640,768,896,1024,
+      1152,1280,0,0,0,0,0,0,0,0,0,0,0,0,0},
+     {69,87,104,121,139,174,208,243,278,348,417,487,557,696,835,975,1114,
+      1253,1393,0,0,0,0,0,0,0,0,0,0,0,0,0},
+     {96,120,144,168,192,240,288,336,384,480,576,672,768,960,1152,1344,
+      1536,1728,1920,0,0,0,0,0,0,0,0,0,0,0,0,0}};
+
+int dvb_filter_get_ac3info(u8 *mbuf, int count, struct dvb_audio_info *ai, int pr)
+{
+	u8 *headr;
+	int found = 0;
+	int c = 0;
+	u8 frame = 0;
+	int fr = 0;
+
+	while ( !found  && c < count){
+		u8 *b = mbuf+c;
+
+		if ( b[0] == 0x0b &&  b[1] == 0x77 )
+			found = 1;
+		else {
+			c++;
+		}
+	}
+
+	if (!found) return -1;
+	if (pr)
+		printk(KERN_DEBUG "Audiostream: AC3");
+
+	ai->off = c;
+	if (c+5 >= count) return -1;
+
+	ai->layer = 0;  // 0 for AC3
+	headr = mbuf+c+2;
+
+	frame = (headr[2]&0x3f);
+	ai->bit_rate = ac3_bitrates[frame >> 1]*1000;
+
+	if (pr)
+		printk(KERN_CONT "  BRate: %d kb/s", (int) ai->bit_rate/1000);
+
+	ai->frequency = (headr[2] & 0xc0 ) >> 6;
+	fr = (headr[2] & 0xc0 ) >> 6;
+	ai->frequency = freq[fr]*100;
+	if (pr)
+		printk(KERN_CONT "  Freq: %d Hz\n", (int) ai->frequency);
+
+	ai->framesize = ac3_frames[fr][frame >> 1];
+	if ((frame & 1) &&  (fr == 1)) ai->framesize++;
+	ai->framesize = ai->framesize << 1;
+	if (pr)
+		printk(KERN_DEBUG "  Framesize %d\n", (int) ai->framesize);
+
+	return 0;
+}
+
+void dvb_filter_pes2ts_init(struct dvb_filter_pes2ts *p2ts, unsigned short pid,
+			    dvb_filter_pes2ts_cb_t *cb, void *priv)
+{
+	unsigned char *buf=p2ts->buf;
+
+	buf[0]=0x47;
+	buf[1]=(pid>>8);
+	buf[2]=pid&0xff;
+	p2ts->cc=0;
+	p2ts->cb=cb;
+	p2ts->priv=priv;
+}
+
+int dvb_filter_pes2ts(struct dvb_filter_pes2ts *p2ts, unsigned char *pes,
+		      int len, int payload_start)
+{
+	unsigned char *buf=p2ts->buf;
+	int ret=0, rest;
+
+	//len=6+((pes[4]<<8)|pes[5]);
+
+	if (payload_start)
+		buf[1]|=0x40;
+	else
+		buf[1]&=~0x40;
+	while (len>=184) {
+		buf[3]=0x10|((p2ts->cc++)&0x0f);
+		memcpy(buf+4, pes, 184);
+		if ((ret=p2ts->cb(p2ts->priv, buf)))
+			return ret;
+		len-=184; pes+=184;
+		buf[1]&=~0x40;
+	}
+	if (!len)
+		return 0;
+	buf[3]=0x30|((p2ts->cc++)&0x0f);
+	rest=183-len;
+	if (rest) {
+		buf[5]=0x00;
+		if (rest-1)
+			memset(buf+6, 0xff, rest-1);
+	}
+	buf[4]=rest;
+	memcpy(buf+5+rest, pes, len);
+	return p2ts->cb(p2ts->priv, buf);
+}
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.h b/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.h
new file mode 100644
index 000000000..67a3c6333
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/dvb_filter.h
@@ -0,0 +1,242 @@
+/*
+ * dvb_filter.h
+ *
+ * Copyright (C) 2003 Convergence GmbH
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation; either version 2.1
+ * 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.
+ */
+
+#ifndef _DVB_FILTER_H_
+#define _DVB_FILTER_H_
+
+#include <linux/slab.h>
+
+#include <media/demux.h>
+
+typedef int (dvb_filter_pes2ts_cb_t) (void *, unsigned char *);
+
+struct dvb_filter_pes2ts {
+	unsigned char buf[188];
+	unsigned char cc;
+	dvb_filter_pes2ts_cb_t *cb;
+	void *priv;
+};
+
+void dvb_filter_pes2ts_init(struct dvb_filter_pes2ts *p2ts, unsigned short pid,
+			    dvb_filter_pes2ts_cb_t *cb, void *priv);
+
+int dvb_filter_pes2ts(struct dvb_filter_pes2ts *p2ts, unsigned char *pes,
+		      int len, int payload_start);
+
+
+#define PROG_STREAM_MAP  0xBC
+#define PRIVATE_STREAM1  0xBD
+#define PADDING_STREAM   0xBE
+#define PRIVATE_STREAM2  0xBF
+#define AUDIO_STREAM_S   0xC0
+#define AUDIO_STREAM_E   0xDF
+#define VIDEO_STREAM_S   0xE0
+#define VIDEO_STREAM_E   0xEF
+#define ECM_STREAM       0xF0
+#define EMM_STREAM       0xF1
+#define DSM_CC_STREAM    0xF2
+#define ISO13522_STREAM  0xF3
+#define PROG_STREAM_DIR  0xFF
+
+#define DVB_PICTURE_START    0x00
+#define DVB_USER_START       0xb2
+#define DVB_SEQUENCE_HEADER  0xb3
+#define DVB_SEQUENCE_ERROR   0xb4
+#define DVB_EXTENSION_START  0xb5
+#define DVB_SEQUENCE_END     0xb7
+#define DVB_GOP_START        0xb8
+#define DVB_EXCEPT_SLICE     0xb0
+
+#define SEQUENCE_EXTENSION           0x01
+#define SEQUENCE_DISPLAY_EXTENSION   0x02
+#define PICTURE_CODING_EXTENSION     0x08
+#define QUANT_MATRIX_EXTENSION       0x03
+#define PICTURE_DISPLAY_EXTENSION    0x07
+
+#define I_FRAME 0x01
+#define B_FRAME 0x02
+#define P_FRAME 0x03
+
+/* Initialize sequence_data */
+#define INIT_HORIZONTAL_SIZE        720
+#define INIT_VERTICAL_SIZE          576
+#define INIT_ASPECT_RATIO          0x02
+#define INIT_FRAME_RATE            0x03
+#define INIT_DISP_HORIZONTAL_SIZE   540
+#define INIT_DISP_VERTICAL_SIZE     576
+
+
+//flags2
+#define PTS_DTS_FLAGS    0xC0
+#define ESCR_FLAG        0x20
+#define ES_RATE_FLAG     0x10
+#define DSM_TRICK_FLAG   0x08
+#define ADD_CPY_FLAG     0x04
+#define PES_CRC_FLAG     0x02
+#define PES_EXT_FLAG     0x01
+
+//pts_dts flags
+#define PTS_ONLY         0x80
+#define PTS_DTS          0xC0
+
+#define TS_SIZE        188
+#define TRANS_ERROR    0x80
+#define PAY_START      0x40
+#define TRANS_PRIO     0x20
+#define PID_MASK_HI    0x1F
+//flags
+#define TRANS_SCRMBL1  0x80
+#define TRANS_SCRMBL2  0x40
+#define ADAPT_FIELD    0x20
+#define PAYLOAD        0x10
+#define COUNT_MASK     0x0F
+
+// adaptation flags
+#define DISCON_IND     0x80
+#define RAND_ACC_IND   0x40
+#define ES_PRI_IND     0x20
+#define PCR_FLAG       0x10
+#define OPCR_FLAG      0x08
+#define SPLICE_FLAG    0x04
+#define TRANS_PRIV     0x02
+#define ADAP_EXT_FLAG  0x01
+
+// adaptation extension flags
+#define LTW_FLAG       0x80
+#define PIECE_RATE     0x40
+#define SEAM_SPLICE    0x20
+
+
+#define MAX_PLENGTH 0xFFFF
+#define MMAX_PLENGTH (256*MAX_PLENGTH)
+
+#ifndef IPACKS
+#define IPACKS 2048
+#endif
+
+struct ipack {
+	int size;
+	int found;
+	u8 *buf;
+	u8 cid;
+	u32 plength;
+	u8 plen[2];
+	u8 flag1;
+	u8 flag2;
+	u8 hlength;
+	u8 pts[5];
+	u16 *pid;
+	int mpeg;
+	u8 check;
+	int which;
+	int done;
+	void *data;
+	void (*func)(u8 *buf,  int size, void *priv);
+	int count;
+	int repack_subids;
+};
+
+struct dvb_video_info {
+	u32 horizontal_size;
+	u32 vertical_size;
+	u32 aspect_ratio;
+	u32 framerate;
+	u32 video_format;
+	u32 bit_rate;
+	u32 comp_bit_rate;
+	u32 vbv_buffer_size;
+	s16 vbv_delay;
+	u32 CSPF;
+	u32 off;
+};
+
+#define OFF_SIZE 4
+#define FIRST_FIELD 0
+#define SECOND_FIELD 1
+#define VIDEO_FRAME_PICTURE 0x03
+
+struct mpg_picture {
+	int       channel;
+	struct dvb_video_info vinfo;
+	u32      *sequence_gop_header;
+	u32      *picture_header;
+	s32       time_code;
+	int       low_delay;
+	int       closed_gop;
+	int       broken_link;
+	int       sequence_header_flag;
+	int       gop_flag;
+	int       sequence_end_flag;
+
+	u8        profile_and_level;
+	s32       picture_coding_parameter;
+	u32       matrix[32];
+	s8        matrix_change_flag;
+
+	u8        picture_header_parameter;
+  /* bit 0 - 2: bwd f code
+     bit 3    : fpb vector
+     bit 4 - 6: fwd f code
+     bit 7    : fpf vector */
+
+	int       mpeg1_flag;
+	int       progressive_sequence;
+	int       sequence_display_extension_flag;
+	u32       sequence_header_data;
+	s16       last_frame_centre_horizontal_offset;
+	s16       last_frame_centre_vertical_offset;
+
+	u32       pts[2]; /* [0] 1st field, [1] 2nd field */
+	int       top_field_first;
+	int       repeat_first_field;
+	int       progressive_frame;
+	int       bank;
+	int       forward_bank;
+	int       backward_bank;
+	int       compress;
+	s16       frame_centre_horizontal_offset[OFF_SIZE];
+		  /* [0-2] 1st field, [3] 2nd field */
+	s16       frame_centre_vertical_offset[OFF_SIZE];
+		  /* [0-2] 1st field, [3] 2nd field */
+	s16       temporal_reference[2];
+		  /* [0] 1st field, [1] 2nd field */
+
+	s8        picture_coding_type[2];
+		  /* [0] 1st field, [1] 2nd field */
+	s8        picture_structure[2];
+		  /* [0] 1st field, [1] 2nd field */
+	s8        picture_display_extension_flag[2];
+		  /* [0] 1st field, [1] 2nd field */
+		  /* picture_display_extenion() 0:no 1:exit*/
+	s8        pts_flag[2];
+		  /* [0] 1st field, [1] 2nd field */
+};
+
+struct dvb_audio_info {
+	int layer;
+	u32 bit_rate;
+	u32 frequency;
+	u32 mode;
+	u32 mode_extension ;
+	u32 emphasis;
+	u32 framesize;
+	u32 off;
+};
+
+int dvb_filter_get_ac3info(u8 *mbuf, int count, struct dvb_audio_info *ai, int pr);
+
+
+#endif
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/sp8870.c b/drivers/staging/media/deprecated/saa7146/av7110/sp8870.c
new file mode 100644
index 000000000..9767159ae
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/sp8870.c
@@ -0,0 +1,609 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    Driver for Spase SP8870 demodulator
+
+    Copyright (C) 1999 Juergen Peitz
+
+
+*/
+/*
+ * This driver needs external firmware. Please use the command
+ * "<kerneldir>/scripts/get_dvb_firmware alps_tdlb7" to
+ * download/extract it, and then copy it to /usr/lib/hotplug/firmware
+ * or /lib/firmware (depending on configuration of firmware hotplug).
+ */
+#define SP8870_DEFAULT_FIRMWARE "dvb-fe-sp8870.fw"
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+
+#include <media/dvb_frontend.h>
+#include "sp8870.h"
+
+
+struct sp8870_state {
+
+	struct i2c_adapter* i2c;
+
+	const struct sp8870_config* config;
+
+	struct dvb_frontend frontend;
+
+	/* demodulator private data */
+	u8 initialised:1;
+};
+
+static int debug;
+#define dprintk(args...) \
+	do { \
+		if (debug) printk(KERN_DEBUG "sp8870: " args); \
+	} while (0)
+
+/* firmware size for sp8870 */
+#define SP8870_FIRMWARE_SIZE 16382
+
+/* starting point for firmware in file 'Sc_main.mc' */
+#define SP8870_FIRMWARE_OFFSET 0x0A
+
+static int sp8870_writereg (struct sp8870_state* state, u16 reg, u16 data)
+{
+	u8 buf [] = { reg >> 8, reg & 0xff, data >> 8, data & 0xff };
+	struct i2c_msg msg = { .addr = state->config->demod_address, .flags = 0, .buf = buf, .len = 4 };
+	int err;
+
+	if ((err = i2c_transfer (state->i2c, &msg, 1)) != 1) {
+		dprintk ("%s: writereg error (err == %i, reg == 0x%02x, data == 0x%02x)\n", __func__, err, reg, data);
+		return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+static int sp8870_readreg (struct sp8870_state* state, u16 reg)
+{
+	int ret;
+	u8 b0 [] = { reg >> 8 , reg & 0xff };
+	u8 b1 [] = { 0, 0 };
+	struct i2c_msg msg [] = { { .addr = state->config->demod_address, .flags = 0, .buf = b0, .len = 2 },
+			   { .addr = state->config->demod_address, .flags = I2C_M_RD, .buf = b1, .len = 2 } };
+
+	ret = i2c_transfer (state->i2c, msg, 2);
+
+	if (ret != 2) {
+		dprintk("%s: readreg error (ret == %i)\n", __func__, ret);
+		return -1;
+	}
+
+	return (b1[0] << 8 | b1[1]);
+}
+
+static int sp8870_firmware_upload (struct sp8870_state* state, const struct firmware *fw)
+{
+	struct i2c_msg msg;
+	const char *fw_buf = fw->data;
+	int fw_pos;
+	u8 tx_buf[255];
+	int tx_len;
+	int err = 0;
+
+	dprintk ("%s: ...\n", __func__);
+
+	if (fw->size < SP8870_FIRMWARE_SIZE + SP8870_FIRMWARE_OFFSET)
+		return -EINVAL;
+
+	// system controller stop
+	sp8870_writereg(state, 0x0F00, 0x0000);
+
+	// instruction RAM register hiword
+	sp8870_writereg(state, 0x8F08, ((SP8870_FIRMWARE_SIZE / 2) & 0xFFFF));
+
+	// instruction RAM MWR
+	sp8870_writereg(state, 0x8F0A, ((SP8870_FIRMWARE_SIZE / 2) >> 16));
+
+	// do firmware upload
+	fw_pos = SP8870_FIRMWARE_OFFSET;
+	while (fw_pos < SP8870_FIRMWARE_SIZE + SP8870_FIRMWARE_OFFSET){
+		tx_len = (fw_pos <= SP8870_FIRMWARE_SIZE + SP8870_FIRMWARE_OFFSET - 252) ? 252 : SP8870_FIRMWARE_SIZE + SP8870_FIRMWARE_OFFSET - fw_pos;
+		// write register 0xCF0A
+		tx_buf[0] = 0xCF;
+		tx_buf[1] = 0x0A;
+		memcpy(&tx_buf[2], fw_buf + fw_pos, tx_len);
+		msg.addr = state->config->demod_address;
+		msg.flags = 0;
+		msg.buf = tx_buf;
+		msg.len = tx_len + 2;
+		if ((err = i2c_transfer (state->i2c, &msg, 1)) != 1) {
+			printk("%s: firmware upload failed!\n", __func__);
+			printk ("%s: i2c error (err == %i)\n", __func__, err);
+			return err;
+		}
+		fw_pos += tx_len;
+	}
+
+	dprintk ("%s: done!\n", __func__);
+	return 0;
+};
+
+static void sp8870_microcontroller_stop (struct sp8870_state* state)
+{
+	sp8870_writereg(state, 0x0F08, 0x000);
+	sp8870_writereg(state, 0x0F09, 0x000);
+
+	// microcontroller STOP
+	sp8870_writereg(state, 0x0F00, 0x000);
+}
+
+static void sp8870_microcontroller_start (struct sp8870_state* state)
+{
+	sp8870_writereg(state, 0x0F08, 0x000);
+	sp8870_writereg(state, 0x0F09, 0x000);
+
+	// microcontroller START
+	sp8870_writereg(state, 0x0F00, 0x001);
+	// not documented but if we don't read 0x0D01 out here
+	// we don't get a correct data valid signal
+	sp8870_readreg(state, 0x0D01);
+}
+
+static int sp8870_read_data_valid_signal(struct sp8870_state* state)
+{
+	return (sp8870_readreg(state, 0x0D02) > 0);
+}
+
+static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
+{
+	int known_parameters = 1;
+
+	*reg0xc05 = 0x000;
+
+	switch (p->modulation) {
+	case QPSK:
+		break;
+	case QAM_16:
+		*reg0xc05 |= (1 << 10);
+		break;
+	case QAM_64:
+		*reg0xc05 |= (2 << 10);
+		break;
+	case QAM_AUTO:
+		known_parameters = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (p->hierarchy) {
+	case HIERARCHY_NONE:
+		break;
+	case HIERARCHY_1:
+		*reg0xc05 |= (1 << 7);
+		break;
+	case HIERARCHY_2:
+		*reg0xc05 |= (2 << 7);
+		break;
+	case HIERARCHY_4:
+		*reg0xc05 |= (3 << 7);
+		break;
+	case HIERARCHY_AUTO:
+		known_parameters = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (p->code_rate_HP) {
+	case FEC_1_2:
+		break;
+	case FEC_2_3:
+		*reg0xc05 |= (1 << 3);
+		break;
+	case FEC_3_4:
+		*reg0xc05 |= (2 << 3);
+		break;
+	case FEC_5_6:
+		*reg0xc05 |= (3 << 3);
+		break;
+	case FEC_7_8:
+		*reg0xc05 |= (4 << 3);
+		break;
+	case FEC_AUTO:
+		known_parameters = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (known_parameters)
+		*reg0xc05 |= (2 << 1);	/* use specified parameters */
+	else
+		*reg0xc05 |= (1 << 1);	/* enable autoprobing */
+
+	return 0;
+}
+
+static int sp8870_wake_up(struct sp8870_state* state)
+{
+	// enable TS output and interface pins
+	return sp8870_writereg(state, 0xC18, 0x00D);
+}
+
+static int sp8870_set_frontend_parameters(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct sp8870_state* state = fe->demodulator_priv;
+	int  err;
+	u16 reg0xc05;
+
+	if ((err = configure_reg0xc05(p, &reg0xc05)))
+		return err;
+
+	// system controller stop
+	sp8870_microcontroller_stop(state);
+
+	// set tuner parameters
+	if (fe->ops.tuner_ops.set_params) {
+		fe->ops.tuner_ops.set_params(fe);
+		if (fe->ops.i2c_gate_ctrl) fe->ops.i2c_gate_ctrl(fe, 0);
+	}
+
+	// sample rate correction bit [23..17]
+	sp8870_writereg(state, 0x0319, 0x000A);
+
+	// sample rate correction bit [16..0]
+	sp8870_writereg(state, 0x031A, 0x0AAB);
+
+	// integer carrier offset
+	sp8870_writereg(state, 0x0309, 0x0400);
+
+	// fractional carrier offset
+	sp8870_writereg(state, 0x030A, 0x0000);
+
+	// filter for 6/7/8 Mhz channel
+	if (p->bandwidth_hz == 6000000)
+		sp8870_writereg(state, 0x0311, 0x0002);
+	else if (p->bandwidth_hz == 7000000)
+		sp8870_writereg(state, 0x0311, 0x0001);
+	else
+		sp8870_writereg(state, 0x0311, 0x0000);
+
+	// scan order: 2k first = 0x0000, 8k first = 0x0001
+	if (p->transmission_mode == TRANSMISSION_MODE_2K)
+		sp8870_writereg(state, 0x0338, 0x0000);
+	else
+		sp8870_writereg(state, 0x0338, 0x0001);
+
+	sp8870_writereg(state, 0xc05, reg0xc05);
+
+	// read status reg in order to clear pending irqs
+	err = sp8870_readreg(state, 0x200);
+	if (err < 0)
+		return err;
+
+	// system controller start
+	sp8870_microcontroller_start(state);
+
+	return 0;
+}
+
+static int sp8870_init (struct dvb_frontend* fe)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	const struct firmware *fw = NULL;
+
+	sp8870_wake_up(state);
+	if (state->initialised) return 0;
+	state->initialised = 1;
+
+	dprintk ("%s\n", __func__);
+
+
+	/* request the firmware, this will block until someone uploads it */
+	printk("sp8870: waiting for firmware upload (%s)...\n", SP8870_DEFAULT_FIRMWARE);
+	if (state->config->request_firmware(fe, &fw, SP8870_DEFAULT_FIRMWARE)) {
+		printk("sp8870: no firmware upload (timeout or file not found?)\n");
+		return -EIO;
+	}
+
+	if (sp8870_firmware_upload(state, fw)) {
+		printk("sp8870: writing firmware to device failed\n");
+		release_firmware(fw);
+		return -EIO;
+	}
+	release_firmware(fw);
+	printk("sp8870: firmware upload complete\n");
+
+	/* enable TS output and interface pins */
+	sp8870_writereg(state, 0xc18, 0x00d);
+
+	// system controller stop
+	sp8870_microcontroller_stop(state);
+
+	// ADC mode
+	sp8870_writereg(state, 0x0301, 0x0003);
+
+	// Reed Solomon parity bytes passed to output
+	sp8870_writereg(state, 0x0C13, 0x0001);
+
+	// MPEG clock is suppressed if no valid data
+	sp8870_writereg(state, 0x0C14, 0x0001);
+
+	/* bit 0x010: enable data valid signal */
+	sp8870_writereg(state, 0x0D00, 0x010);
+	sp8870_writereg(state, 0x0D01, 0x000);
+
+	return 0;
+}
+
+static int sp8870_read_status(struct dvb_frontend *fe,
+			      enum fe_status *fe_status)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	int status;
+	int signal;
+
+	*fe_status = 0;
+
+	status = sp8870_readreg (state, 0x0200);
+	if (status < 0)
+		return -EIO;
+
+	signal = sp8870_readreg (state, 0x0303);
+	if (signal < 0)
+		return -EIO;
+
+	if (signal > 0x0F)
+		*fe_status |= FE_HAS_SIGNAL;
+	if (status & 0x08)
+		*fe_status |= FE_HAS_SYNC;
+	if (status & 0x04)
+		*fe_status |= FE_HAS_LOCK | FE_HAS_CARRIER | FE_HAS_VITERBI;
+
+	return 0;
+}
+
+static int sp8870_read_ber (struct dvb_frontend* fe, u32 * ber)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	int ret;
+	u32 tmp;
+
+	*ber = 0;
+
+	ret = sp8870_readreg(state, 0xC08);
+	if (ret < 0)
+		return -EIO;
+
+	tmp = ret & 0x3F;
+
+	ret = sp8870_readreg(state, 0xC07);
+	if (ret < 0)
+		return -EIO;
+
+	tmp = ret << 6;
+	if (tmp >= 0x3FFF0)
+		tmp = ~0;
+
+	*ber = tmp;
+
+	return 0;
+}
+
+static int sp8870_read_signal_strength(struct dvb_frontend* fe,  u16 * signal)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	int ret;
+	u16 tmp;
+
+	*signal = 0;
+
+	ret = sp8870_readreg (state, 0x306);
+	if (ret < 0)
+		return -EIO;
+
+	tmp = ret << 8;
+
+	ret = sp8870_readreg (state, 0x303);
+	if (ret < 0)
+		return -EIO;
+
+	tmp |= ret;
+
+	if (tmp)
+		*signal = 0xFFFF - tmp;
+
+	return 0;
+}
+
+static int sp8870_read_uncorrected_blocks (struct dvb_frontend* fe, u32* ublocks)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	int ret;
+
+	*ublocks = 0;
+
+	ret = sp8870_readreg(state, 0xC0C);
+	if (ret < 0)
+		return -EIO;
+
+	if (ret == 0xFFFF)
+		ret = ~0;
+
+	*ublocks = ret;
+
+	return 0;
+}
+
+/* number of trials to recover from lockup */
+#define MAXTRIALS 5
+/* maximum checks for data valid signal */
+#define MAXCHECKS 100
+
+/* only for debugging: counter for detected lockups */
+static int lockups;
+/* only for debugging: counter for channel switches */
+static int switches;
+
+static int sp8870_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct sp8870_state* state = fe->demodulator_priv;
+
+	/*
+	    The firmware of the sp8870 sometimes locks up after setting frontend parameters.
+	    We try to detect this by checking the data valid signal.
+	    If it is not set after MAXCHECKS we try to recover the lockup by setting
+	    the frontend parameters again.
+	*/
+
+	int err = 0;
+	int valid = 0;
+	int trials = 0;
+	int check_count = 0;
+
+	dprintk("%s: frequency = %i\n", __func__, p->frequency);
+
+	for (trials = 1; trials <= MAXTRIALS; trials++) {
+
+		err = sp8870_set_frontend_parameters(fe);
+		if (err)
+			return err;
+
+		for (check_count = 0; check_count < MAXCHECKS; check_count++) {
+//			valid = ((sp8870_readreg(i2c, 0x0200) & 4) == 0);
+			valid = sp8870_read_data_valid_signal(state);
+			if (valid) {
+				dprintk("%s: delay = %i usec\n",
+					__func__, check_count * 10);
+				break;
+			}
+			udelay(10);
+		}
+		if (valid)
+			break;
+	}
+
+	if (!valid) {
+		printk("%s: firmware crash!!!!!!\n", __func__);
+		return -EIO;
+	}
+
+	if (debug) {
+		if (valid) {
+			if (trials > 1) {
+				printk("%s: firmware lockup!!!\n", __func__);
+				printk("%s: recovered after %i trial(s))\n",  __func__, trials - 1);
+				lockups++;
+			}
+		}
+		switches++;
+		printk("%s: switches = %i lockups = %i\n", __func__, switches, lockups);
+	}
+
+	return 0;
+}
+
+static int sp8870_sleep(struct dvb_frontend* fe)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+
+	// tristate TS output and disable interface pins
+	return sp8870_writereg(state, 0xC18, 0x000);
+}
+
+static int sp8870_get_tune_settings(struct dvb_frontend* fe, struct dvb_frontend_tune_settings* fesettings)
+{
+	fesettings->min_delay_ms = 350;
+	fesettings->step_size = 0;
+	fesettings->max_drift = 0;
+	return 0;
+}
+
+static int sp8870_i2c_gate_ctrl(struct dvb_frontend* fe, int enable)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+
+	if (enable) {
+		return sp8870_writereg(state, 0x206, 0x001);
+	} else {
+		return sp8870_writereg(state, 0x206, 0x000);
+	}
+}
+
+static void sp8870_release(struct dvb_frontend* fe)
+{
+	struct sp8870_state* state = fe->demodulator_priv;
+	kfree(state);
+}
+
+static const struct dvb_frontend_ops sp8870_ops;
+
+struct dvb_frontend* sp8870_attach(const struct sp8870_config* config,
+				   struct i2c_adapter* i2c)
+{
+	struct sp8870_state* state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct sp8870_state), GFP_KERNEL);
+	if (state == NULL) goto error;
+
+	/* setup the state */
+	state->config = config;
+	state->i2c = i2c;
+	state->initialised = 0;
+
+	/* check if the demod is there */
+	if (sp8870_readreg(state, 0x0200) < 0) goto error;
+
+	/* create dvb_frontend */
+	memcpy(&state->frontend.ops, &sp8870_ops, sizeof(struct dvb_frontend_ops));
+	state->frontend.demodulator_priv = state;
+	return &state->frontend;
+
+error:
+	kfree(state);
+	return NULL;
+}
+
+static const struct dvb_frontend_ops sp8870_ops = {
+	.delsys = { SYS_DVBT },
+	.info = {
+		.name			= "Spase SP8870 DVB-T",
+		.frequency_min_hz	= 470 * MHz,
+		.frequency_max_hz	= 860 * MHz,
+		.frequency_stepsize_hz	= 166666,
+		.caps			= FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 |
+					  FE_CAN_FEC_3_4 | FE_CAN_FEC_5_6 |
+					  FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+					  FE_CAN_QPSK | FE_CAN_QAM_16 |
+					  FE_CAN_QAM_64 | FE_CAN_QAM_AUTO |
+					  FE_CAN_HIERARCHY_AUTO |  FE_CAN_RECOVER
+	},
+
+	.release = sp8870_release,
+
+	.init = sp8870_init,
+	.sleep = sp8870_sleep,
+	.i2c_gate_ctrl = sp8870_i2c_gate_ctrl,
+
+	.set_frontend = sp8870_set_frontend,
+	.get_tune_settings = sp8870_get_tune_settings,
+
+	.read_status = sp8870_read_status,
+	.read_ber = sp8870_read_ber,
+	.read_signal_strength = sp8870_read_signal_strength,
+	.read_ucblocks = sp8870_read_uncorrected_blocks,
+};
+
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
+
+MODULE_DESCRIPTION("Spase SP8870 DVB-T Demodulator driver");
+MODULE_AUTHOR("Juergen Peitz");
+MODULE_LICENSE("GPL");
+
+EXPORT_SYMBOL(sp8870_attach);
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/sp8870.h b/drivers/staging/media/deprecated/saa7146/av7110/sp8870.h
new file mode 100644
index 000000000..5eacf39f4
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/sp8870.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+    Driver for Spase SP8870 demodulator
+
+    Copyright (C) 1999 Juergen Peitz
+
+
+*/
+
+#ifndef SP8870_H
+#define SP8870_H
+
+#include <linux/dvb/frontend.h>
+#include <linux/firmware.h>
+
+struct sp8870_config
+{
+	/* the demodulator's i2c address */
+	u8 demod_address;
+
+	/* request firmware for device */
+	int (*request_firmware)(struct dvb_frontend* fe, const struct firmware **fw, char* name);
+};
+
+#if IS_REACHABLE(CONFIG_DVB_SP8870)
+extern struct dvb_frontend* sp8870_attach(const struct sp8870_config* config,
+					  struct i2c_adapter* i2c);
+#else
+static inline struct dvb_frontend* sp8870_attach(const struct sp8870_config* config,
+					  struct i2c_adapter* i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif // CONFIG_DVB_SP8870
+
+#endif // SP8870_H
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-clear-buffer.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-clear-buffer.rst
new file mode 100644
index 000000000..a7730559b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-clear-buffer.rst
@@ -0,0 +1,54 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_CLEAR_BUFFER:
+
+==================
+VIDEO_CLEAR_BUFFER
+==================
+
+Name
+----
+
+VIDEO_CLEAR_BUFFER
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_CLEAR_BUFFER
+
+``int ioctl(fd, VIDEO_CLEAR_BUFFER)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_CLEAR_BUFFER for this command.
+
+Description
+-----------
+
+This ioctl call clears all video buffers in the driver and in the
+decoder hardware.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-command.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-command.rst
new file mode 100644
index 000000000..cae9445eb
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-command.rst
@@ -0,0 +1,96 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_COMMAND:
+
+=============
+VIDEO_COMMAND
+=============
+
+Name
+----
+
+VIDEO_COMMAND
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_COMMAND
+
+``int ioctl(int fd, VIDEO_COMMAND, struct video_command *cmd)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_COMMAND for this command.
+
+    -  .. row 3
+
+       -  struct video_command \*cmd
+
+       -  Commands the decoder.
+
+Description
+-----------
+
+This ioctl is obsolete. Do not use in new drivers. For V4L2 decoders
+this ioctl has been replaced by the
+:ref:`VIDIOC_DECODER_CMD` ioctl.
+
+This ioctl commands the decoder. The ``video_command`` struct is a
+subset of the ``v4l2_decoder_cmd`` struct, so refer to the
+:ref:`VIDIOC_DECODER_CMD` documentation for
+more information.
+
+.. c:type:: video_command
+
+.. code-block:: c
+
+	/* The structure must be zeroed before use by the application
+	This ensures it can be extended safely in the future. */
+	struct video_command {
+		__u32 cmd;
+		__u32 flags;
+		union {
+			struct {
+				__u64 pts;
+			} stop;
+
+			struct {
+				/* 0 or 1000 specifies normal speed,
+				1 specifies forward single stepping,
+				-1 specifies backward single stepping,
+				>1: playback at speed/1000 of the normal speed,
+				<-1: reverse playback at (-speed/1000) of the normal speed. */
+				__s32 speed;
+				__u32 format;
+			} play;
+
+			struct {
+				__u32 data[16];
+			} raw;
+		};
+	};
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-continue.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-continue.rst
new file mode 100644
index 000000000..bc34bf398
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-continue.rst
@@ -0,0 +1,57 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_CONTINUE:
+
+==============
+VIDEO_CONTINUE
+==============
+
+Name
+----
+
+VIDEO_CONTINUE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_CONTINUE
+
+``int ioctl(fd, VIDEO_CONTINUE)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_CONTINUE for this command.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 :ref:`VIDIOC_DECODER_CMD` instead.
+
+This ioctl call restarts decoding and playing processes of the video
+stream which was played before a call to VIDEO_FREEZE was made.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-fast-forward.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-fast-forward.rst
new file mode 100644
index 000000000..e71fa8d69
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-fast-forward.rst
@@ -0,0 +1,72 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_FAST_FORWARD:
+
+==================
+VIDEO_FAST_FORWARD
+==================
+
+Name
+----
+
+VIDEO_FAST_FORWARD
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_FAST_FORWARD
+
+``int ioctl(fd, VIDEO_FAST_FORWARD, int nFrames)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_FAST_FORWARD for this command.
+
+    -  .. row 3
+
+       -  int nFrames
+
+       -  The number of frames to skip.
+
+Description
+-----------
+
+This ioctl call asks the Video Device to skip decoding of N number of
+I-frames. This call can only be used if VIDEO_SOURCE_MEMORY is
+selected.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
+
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EPERM``
+
+       -  Mode VIDEO_SOURCE_MEMORY not selected.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-fclose.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-fclose.rst
new file mode 100644
index 000000000..01d24d548
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-fclose.rst
@@ -0,0 +1,51 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _video_fclose:
+
+=================
+dvb video close()
+=================
+
+Name
+----
+
+dvb video close()
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:function:: int close(int fd)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+Description
+-----------
+
+This system call closes a previously opened video device.
+
+Return Value
+------------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EBADF``
+
+       -  fd is not a valid open file descriptor.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-fopen.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-fopen.rst
new file mode 100644
index 000000000..1371b083e
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-fopen.rst
@@ -0,0 +1,111 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _video_fopen:
+
+================
+dvb video open()
+================
+
+Name
+----
+
+dvb video open()
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:function:: int open(const char *deviceName, int flags)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  const char \*deviceName
+
+       -  Name of specific video device.
+
+    -  .. row 2
+
+       -  int flags
+
+       -  A bit-wise OR of the following flags:
+
+    -  .. row 3
+
+       -
+       -  O_RDONLY read-only access
+
+    -  .. row 4
+
+       -
+       -  O_RDWR read/write access
+
+    -  .. row 5
+
+       -
+       -  O_NONBLOCK open in non-blocking mode
+
+    -  .. row 6
+
+       -
+       -  (blocking mode is the default)
+
+Description
+-----------
+
+This system call opens a named video device (e.g.
+/dev/dvb/adapter0/video0) for subsequent use.
+
+When an open() call has succeeded, the device will be ready for use. The
+significance of blocking or non-blocking mode is described in the
+documentation for functions where there is a difference. It does not
+affect the semantics of the open() call itself. A device opened in
+blocking mode can later be put into non-blocking mode (and vice versa)
+using the F_SETFL command of the fcntl system call. This is a standard
+system call, documented in the Linux manual page for fcntl. Only one
+user can open the Video Device in O_RDWR mode. All other attempts to
+open the device in this mode will fail, and an error-code will be
+returned. If the Video Device is opened in O_RDONLY mode, the only
+ioctl call that can be used is VIDEO_GET_STATUS. All other call will
+return an error code.
+
+Return Value
+------------
+
+.. tabularcolumns:: |p{2.5cm}|p{15.0cm}|
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``ENODEV``
+
+       -  Device driver not loaded/available.
+
+    -  .. row 2
+
+       -  ``EINTERNAL``
+
+       -  Internal error.
+
+    -  .. row 3
+
+       -  ``EBUSY``
+
+       -  Device or resource busy.
+
+    -  .. row 4
+
+       -  ``EINVAL``
+
+       -  Invalid argument.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-freeze.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-freeze.rst
new file mode 100644
index 000000000..4321f257c
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-freeze.rst
@@ -0,0 +1,61 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_FREEZE:
+
+============
+VIDEO_FREEZE
+============
+
+Name
+----
+
+VIDEO_FREEZE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_FREEZE
+
+``int ioctl(fd, VIDEO_FREEZE)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_FREEZE for this command.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 :ref:`VIDIOC_DECODER_CMD` instead.
+
+This ioctl call suspends the live video stream being played. Decoding
+and playing are frozen. It is then possible to restart the decoding and
+playing process of the video stream using the VIDEO_CONTINUE command.
+If VIDEO_SOURCE_MEMORY is selected in the ioctl call
+VIDEO_SELECT_SOURCE, the Digital TV subsystem will not decode any more data
+until the ioctl call VIDEO_CONTINUE or VIDEO_PLAY is performed.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-fwrite.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-fwrite.rst
new file mode 100644
index 000000000..a07fd7d7a
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-fwrite.rst
@@ -0,0 +1,79 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _video_fwrite:
+
+=================
+dvb video write()
+=================
+
+Name
+----
+
+dvb video write()
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:function:: size_t write(int fd, const void *buf, size_t count)
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  void \*buf
+
+       -  Pointer to the buffer containing the PES data.
+
+    -  .. row 3
+
+       -  size_t count
+
+       -  Size of buf.
+
+Description
+-----------
+
+This system call can only be used if VIDEO_SOURCE_MEMORY is selected
+in the ioctl call VIDEO_SELECT_SOURCE. The data provided shall be in
+PES format, unless the capability allows other formats. If O_NONBLOCK
+is not specified the function will block until buffer space is
+available. The amount of data to be transferred is implied by count.
+
+Return Value
+------------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EPERM``
+
+       -  Mode VIDEO_SOURCE_MEMORY not selected.
+
+    -  .. row 2
+
+       -  ``ENOMEM``
+
+       -  Attempted to write more data than the internal buffer can hold.
+
+    -  .. row 3
+
+       -  ``EBADF``
+
+       -  fd is not a valid open file descriptor.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-capabilities.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-capabilities.rst
new file mode 100644
index 000000000..01e09f566
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-capabilities.rst
@@ -0,0 +1,61 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_CAPABILITIES:
+
+======================
+VIDEO_GET_CAPABILITIES
+======================
+
+Name
+----
+
+VIDEO_GET_CAPABILITIES
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_CAPABILITIES
+
+``int ioctl(fd, VIDEO_GET_CAPABILITIES, unsigned int *cap)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_CAPABILITIES for this command.
+
+    -  .. row 3
+
+       -  unsigned int \*cap
+
+       -  Pointer to a location where to store the capability information.
+
+Description
+-----------
+
+This ioctl call asks the video device about its decoding capabilities.
+On success it returns and integer which has bits set according to the
+defines in section ??.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-event.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-event.rst
new file mode 100644
index 000000000..90382bc36
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-event.rst
@@ -0,0 +1,105 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_EVENT:
+
+===============
+VIDEO_GET_EVENT
+===============
+
+Name
+----
+
+VIDEO_GET_EVENT
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_EVENT
+
+``int ioctl(fd, VIDEO_GET_EVENT, struct video_event *ev)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_EVENT for this command.
+
+    -  .. row 3
+
+       -  struct video_event \*ev
+
+       -  Points to the location where the event, if any, is to be stored.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To get events from a V4L2 decoder
+use the V4L2 :ref:`VIDIOC_DQEVENT` ioctl instead.
+
+This ioctl call returns an event of type video_event if available. If
+an event is not available, the behavior depends on whether the device is
+in blocking or non-blocking mode. In the latter case, the call fails
+immediately with errno set to ``EWOULDBLOCK``. In the former case, the call
+blocks until an event becomes available. The standard Linux poll()
+and/or select() system calls can be used with the device file descriptor
+to watch for new events. For select(), the file descriptor should be
+included in the exceptfds argument, and for poll(), POLLPRI should be
+specified as the wake-up condition. Read-only permissions are sufficient
+for this ioctl call.
+
+.. c:type:: video_event
+
+.. code-block:: c
+
+	struct video_event {
+		__s32 type;
+	#define VIDEO_EVENT_SIZE_CHANGED	1
+	#define VIDEO_EVENT_FRAME_RATE_CHANGED	2
+	#define VIDEO_EVENT_DECODER_STOPPED 	3
+	#define VIDEO_EVENT_VSYNC 		4
+		long timestamp;
+		union {
+			video_size_t size;
+			unsigned int frame_rate;	/* in frames per 1000sec */
+			unsigned char vsync_field;	/* unknown/odd/even/progressive */
+		} u;
+	};
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EWOULDBLOCK``
+
+       -  There is no event pending, and the device is in non-blocking mode.
+
+    -  .. row 2
+
+       -  ``EOVERFLOW``
+
+       -  Overflow in event queue - one or more events were lost.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-frame-count.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-frame-count.rst
new file mode 100644
index 000000000..b48ac8c58
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-frame-count.rst
@@ -0,0 +1,65 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_FRAME_COUNT:
+
+=====================
+VIDEO_GET_FRAME_COUNT
+=====================
+
+Name
+----
+
+VIDEO_GET_FRAME_COUNT
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_FRAME_COUNT
+
+``int ioctl(int fd, VIDEO_GET_FRAME_COUNT, __u64 *pts)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_FRAME_COUNT for this command.
+
+    -  .. row 3
+
+       -  __u64 \*pts
+
+       -  Returns the number of frames displayed since the decoder was
+	  started.
+
+Description
+-----------
+
+This ioctl is obsolete. Do not use in new drivers. For V4L2 decoders
+this ioctl has been replaced by the ``V4L2_CID_MPEG_VIDEO_DEC_FRAME``
+control.
+
+This ioctl call asks the Video Device to return the number of displayed
+frames since the decoder was started.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-pts.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-pts.rst
new file mode 100644
index 000000000..fedaff41b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-pts.rst
@@ -0,0 +1,69 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_PTS:
+
+=============
+VIDEO_GET_PTS
+=============
+
+Name
+----
+
+VIDEO_GET_PTS
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_PTS
+
+``int ioctl(int fd, VIDEO_GET_PTS, __u64 *pts)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_PTS for this command.
+
+    -  .. row 3
+
+       -  __u64 \*pts
+
+       -  Returns the 33-bit timestamp as defined in ITU T-REC-H.222.0 /
+	  ISO/IEC 13818-1.
+
+	  The PTS should belong to the currently played frame if possible,
+	  but may also be a value close to it like the PTS of the last
+	  decoded frame or the last PTS extracted by the PES parser.
+
+Description
+-----------
+
+This ioctl is obsolete. Do not use in new drivers. For V4L2 decoders
+this ioctl has been replaced by the ``V4L2_CID_MPEG_VIDEO_DEC_PTS``
+control.
+
+This ioctl call asks the Video Device to return the current PTS
+timestamp.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-size.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-size.rst
new file mode 100644
index 000000000..de34331c5
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-size.rst
@@ -0,0 +1,69 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_SIZE:
+
+==============
+VIDEO_GET_SIZE
+==============
+
+Name
+----
+
+VIDEO_GET_SIZE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_SIZE
+
+``int ioctl(int fd, VIDEO_GET_SIZE, video_size_t *size)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_SIZE for this command.
+
+    -  .. row 3
+
+       -  video_size_t \*size
+
+       -  Returns the size and aspect ratio.
+
+Description
+-----------
+
+This ioctl returns the size and aspect ratio.
+
+.. c:type:: video_size_t
+
+.. code-block::c
+
+	typedef struct {
+		int w;
+		int h;
+		video_format_t aspect_ratio;
+	} video_size_t;
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-get-status.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-get-status.rst
new file mode 100644
index 000000000..9b86fbf41
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-get-status.rst
@@ -0,0 +1,72 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_GET_STATUS:
+
+================
+VIDEO_GET_STATUS
+================
+
+Name
+----
+
+VIDEO_GET_STATUS
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_GET_STATUS
+
+``int ioctl(fd, VIDEO_GET_STATUS, struct video_status *status)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_GET_STATUS for this command.
+
+    -  .. row 3
+
+       -  struct video_status \*status
+
+       -  Returns the current status of the Video Device.
+
+Description
+-----------
+
+This ioctl call asks the Video Device to return the current status of
+the device.
+
+.. c:type:: video_status
+
+.. code-block:: c
+
+	struct video_status {
+		int                   video_blank;   /* blank video on freeze? */
+		video_play_state_t    play_state;    /* current state of playback */
+		video_stream_source_t stream_source; /* current source (demux/memory) */
+		video_format_t        video_format;  /* current aspect ratio of stream*/
+		video_displayformat_t display_format;/* selected cropping mode */
+	};
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-play.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-play.rst
new file mode 100644
index 000000000..35ac8b98f
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-play.rst
@@ -0,0 +1,57 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_PLAY:
+
+==========
+VIDEO_PLAY
+==========
+
+Name
+----
+
+VIDEO_PLAY
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_PLAY
+
+``int ioctl(fd, VIDEO_PLAY)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_PLAY for this command.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 :ref:`VIDIOC_DECODER_CMD` instead.
+
+This ioctl call asks the Video Device to start playing a video stream
+from the selected source.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-select-source.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-select-source.rst
new file mode 100644
index 000000000..929a20985
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-select-source.rst
@@ -0,0 +1,76 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SELECT_SOURCE:
+
+===================
+VIDEO_SELECT_SOURCE
+===================
+
+Name
+----
+
+VIDEO_SELECT_SOURCE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SELECT_SOURCE
+
+``int ioctl(fd, VIDEO_SELECT_SOURCE, video_stream_source_t source)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SELECT_SOURCE for this command.
+
+    -  .. row 3
+
+       -  video_stream_source_t source
+
+       -  Indicates which source shall be used for the Video stream.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. This ioctl was also supported by the
+V4L2 ivtv driver, but that has been replaced by the ivtv-specific
+``IVTV_IOC_PASSTHROUGH_MODE`` ioctl.
+
+This ioctl call informs the video device which source shall be used for
+the input data. The possible sources are demux or memory. If memory is
+selected, the data is fed to the video device through the write command.
+
+.. c:type:: video_stream_source_t
+
+.. code-block:: c
+
+	typedef enum {
+		VIDEO_SOURCE_DEMUX, /* Select the demux as the main source */
+		VIDEO_SOURCE_MEMORY /* If this source is selected, the stream
+				comes from the user through the write
+				system call */
+	} video_stream_source_t;
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-set-blank.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-set-blank.rst
new file mode 100644
index 000000000..70249a6ba
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-set-blank.rst
@@ -0,0 +1,64 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SET_BLANK:
+
+===============
+VIDEO_SET_BLANK
+===============
+
+Name
+----
+
+VIDEO_SET_BLANK
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SET_BLANK
+
+``int ioctl(fd, VIDEO_SET_BLANK, boolean mode)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SET_BLANK for this command.
+
+    -  .. row 3
+
+       -  boolean mode
+
+       -  TRUE: Blank screen when stop.
+
+    -  .. row 4
+
+       -
+       -  FALSE: Show last decoded frame.
+
+Description
+-----------
+
+This ioctl call asks the Video Device to blank out the picture.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-set-display-format.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-set-display-format.rst
new file mode 100644
index 000000000..1de4f40ae
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-set-display-format.rst
@@ -0,0 +1,60 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SET_DISPLAY_FORMAT:
+
+========================
+VIDEO_SET_DISPLAY_FORMAT
+========================
+
+Name
+----
+
+VIDEO_SET_DISPLAY_FORMAT
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SET_DISPLAY_FORMAT
+
+``int ioctl(fd, VIDEO_SET_DISPLAY_FORMAT)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SET_DISPLAY_FORMAT for this command.
+
+    -  .. row 3
+
+       -  video_display_format_t format
+
+       -  Selects the video format to be used.
+
+Description
+-----------
+
+This ioctl call asks the Video Device to select the video format to be
+applied by the MPEG chip on the video.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-set-format.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-set-format.rst
new file mode 100644
index 000000000..bb64e37ae
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-set-format.rst
@@ -0,0 +1,82 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SET_FORMAT:
+
+================
+VIDEO_SET_FORMAT
+================
+
+Name
+----
+
+VIDEO_SET_FORMAT
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SET_FORMAT
+
+``int ioctl(fd, VIDEO_SET_FORMAT, video_format_t format)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SET_FORMAT for this command.
+
+    -  .. row 3
+
+       -  video_format_t format
+
+       -  video format of TV as defined in section ??.
+
+Description
+-----------
+
+This ioctl sets the screen format (aspect ratio) of the connected output
+device (TV) so that the output of the decoder can be adjusted
+accordingly.
+
+.. c:type:: video_format_t
+
+.. code-block:: c
+
+	typedef enum {
+		VIDEO_FORMAT_4_3,     /* Select 4:3 format */
+		VIDEO_FORMAT_16_9,    /* Select 16:9 format. */
+		VIDEO_FORMAT_221_1    /* 2.21:1 */
+	} video_format_t;
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
+
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EINVAL``
+
+       -  format is not a valid video format.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-set-streamtype.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-set-streamtype.rst
new file mode 100644
index 000000000..1f31c048b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-set-streamtype.rst
@@ -0,0 +1,61 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SET_STREAMTYPE:
+
+====================
+VIDEO_SET_STREAMTYPE
+====================
+
+Name
+----
+
+VIDEO_SET_STREAMTYPE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SET_STREAMTYPE
+
+``int ioctl(fd, VIDEO_SET_STREAMTYPE, int type)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SET_STREAMTYPE for this command.
+
+    -  .. row 3
+
+       -  int type
+
+       -  stream type
+
+Description
+-----------
+
+This ioctl tells the driver which kind of stream to expect being written
+to it. If this call is not used the default of video PES is used. Some
+drivers might not support this call and always expect PES.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-slowmotion.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-slowmotion.rst
new file mode 100644
index 000000000..1478fcc30
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-slowmotion.rst
@@ -0,0 +1,72 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_SLOWMOTION:
+
+================
+VIDEO_SLOWMOTION
+================
+
+Name
+----
+
+VIDEO_SLOWMOTION
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_SLOWMOTION
+
+``int ioctl(fd, VIDEO_SLOWMOTION, int nFrames)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_SLOWMOTION for this command.
+
+    -  .. row 3
+
+       -  int nFrames
+
+       -  The number of times to repeat each frame.
+
+Description
+-----------
+
+This ioctl call asks the video device to repeat decoding frames N number
+of times. This call can only be used if VIDEO_SOURCE_MEMORY is
+selected.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
+
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  ``EPERM``
+
+       -  Mode VIDEO_SOURCE_MEMORY not selected.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-stillpicture.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-stillpicture.rst
new file mode 100644
index 000000000..d25384222
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-stillpicture.rst
@@ -0,0 +1,61 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_STILLPICTURE:
+
+==================
+VIDEO_STILLPICTURE
+==================
+
+Name
+----
+
+VIDEO_STILLPICTURE
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_STILLPICTURE
+
+``int ioctl(fd, VIDEO_STILLPICTURE, struct video_still_picture *sp)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_STILLPICTURE for this command.
+
+    -  .. row 3
+
+       -  struct video_still_picture \*sp
+
+       -  Pointer to a location where an I-frame and size is stored.
+
+Description
+-----------
+
+This ioctl call asks the Video Device to display a still picture
+(I-frame). The input data shall contain an I-frame. If the pointer is
+NULL, then the current displayed still picture is blanked.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-stop.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-stop.rst
new file mode 100644
index 000000000..96f61c5b4
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-stop.rst
@@ -0,0 +1,74 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_STOP:
+
+==========
+VIDEO_STOP
+==========
+
+Name
+----
+
+VIDEO_STOP
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_STOP
+
+``int ioctl(fd, VIDEO_STOP, boolean mode)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_STOP for this command.
+
+    -  .. row 3
+
+       -  Boolean mode
+
+       -  Indicates how the screen shall be handled.
+
+    -  .. row 4
+
+       -
+       -  TRUE: Blank screen when stop.
+
+    -  .. row 5
+
+       -
+       -  FALSE: Show last decoded frame.
+
+Description
+-----------
+
+This ioctl is for Digital TV devices only. To control a V4L2 decoder use the
+V4L2 :ref:`VIDIOC_DECODER_CMD` instead.
+
+This ioctl call asks the Video Device to stop playing the current
+stream. Depending on the input parameter, the screen can be blanked out
+or displaying the last decoded frame.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video-try-command.rst b/drivers/staging/media/deprecated/saa7146/av7110/video-try-command.rst
new file mode 100644
index 000000000..79bf3dfb8
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video-try-command.rst
@@ -0,0 +1,66 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+.. c:namespace:: DTV.video
+
+.. _VIDEO_TRY_COMMAND:
+
+=================
+VIDEO_TRY_COMMAND
+=================
+
+Name
+----
+
+VIDEO_TRY_COMMAND
+
+.. attention:: This ioctl is deprecated.
+
+Synopsis
+--------
+
+.. c:macro:: VIDEO_TRY_COMMAND
+
+``int ioctl(int fd, VIDEO_TRY_COMMAND, struct video_command *cmd)``
+
+Arguments
+---------
+
+.. flat-table::
+    :header-rows:  0
+    :stub-columns: 0
+
+    -  .. row 1
+
+       -  int fd
+
+       -  File descriptor returned by a previous call to open().
+
+    -  .. row 2
+
+       -  int request
+
+       -  Equals VIDEO_TRY_COMMAND for this command.
+
+    -  .. row 3
+
+       -  struct video_command \*cmd
+
+       -  Try a decoder command.
+
+Description
+-----------
+
+This ioctl is obsolete. Do not use in new drivers. For V4L2 decoders
+this ioctl has been replaced by the
+:ref:`VIDIOC_TRY_DECODER_CMD <VIDIOC_DECODER_CMD>` ioctl.
+
+This ioctl tries a decoder command. The ``video_command`` struct is a
+subset of the ``v4l2_decoder_cmd`` struct, so refer to the
+:ref:`VIDIOC_TRY_DECODER_CMD <VIDIOC_DECODER_CMD>` documentation
+for more information.
+
+Return Value
+------------
+
+On success 0 is returned, on error -1 and the ``errno`` variable is set
+appropriately. The generic error codes are described at the
+:ref:`Generic Error Codes <gen-errors>` chapter.
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video.rst b/drivers/staging/media/deprecated/saa7146/av7110/video.rst
new file mode 100644
index 000000000..808705b76
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video.rst
@@ -0,0 +1,36 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _dvb_video:
+
+#######################
+Digital TV Video Device
+#######################
+
+The Digital TV video device controls the MPEG2 video decoder of the Digital
+TV hardware. It can be accessed through **/dev/dvb/adapter0/video0**. Data
+types and ioctl definitions can be accessed by including
+**linux/dvb/video.h** in your application.
+
+Note that the Digital TV video device only controls decoding of the MPEG video
+stream, not its presentation on the TV or computer screen. On PCs this
+is typically handled by an associated video4linux device, e.g.
+**/dev/video**, which allows scaling and defining output windows.
+
+Some Digital TV cards don't have their own MPEG decoder, which results in the
+omission of the audio and video device as well as the video4linux
+device.
+
+The ioctls that deal with SPUs (sub picture units) and navigation
+packets are only supported on some MPEG decoders made for DVD playback.
+
+These ioctls were also used by V4L2 to control MPEG decoders implemented
+in V4L2. The use of these ioctls for that purpose has been made obsolete
+and proper V4L2 ioctls or controls have been created to replace that
+functionality.
+
+
+.. toctree::
+    :maxdepth: 1
+
+    video_types
+    video_function_calls
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video_function_calls.rst b/drivers/staging/media/deprecated/saa7146/av7110/video_function_calls.rst
new file mode 100644
index 000000000..20a897be5
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video_function_calls.rst
@@ -0,0 +1,35 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _video_function_calls:
+
+********************
+Video Function Calls
+********************
+
+.. toctree::
+    :maxdepth: 1
+
+    video-fopen
+    video-fclose
+    video-fwrite
+    video-stop
+    video-play
+    video-freeze
+    video-continue
+    video-select-source
+    video-set-blank
+    video-get-status
+    video-get-frame-count
+    video-get-pts
+    video-get-event
+    video-command
+    video-try-command
+    video-get-size
+    video-set-display-format
+    video-stillpicture
+    video-fast-forward
+    video-slowmotion
+    video-get-capabilities
+    video-clear-buffer
+    video-set-streamtype
+    video-set-format
diff --git a/drivers/staging/media/deprecated/saa7146/av7110/video_types.rst b/drivers/staging/media/deprecated/saa7146/av7110/video_types.rst
new file mode 100644
index 000000000..c4557d328
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/av7110/video_types.rst
@@ -0,0 +1,248 @@
+.. SPDX-License-Identifier: GFDL-1.1-no-invariants-or-later
+
+.. _video_types:
+
+****************
+Video Data Types
+****************
+
+
+.. _video-format-t:
+
+video_format_t
+==============
+
+The ``video_format_t`` data type defined by
+
+
+.. code-block:: c
+
+    typedef enum {
+	VIDEO_FORMAT_4_3,     /* Select 4:3 format */
+	VIDEO_FORMAT_16_9,    /* Select 16:9 format. */
+	VIDEO_FORMAT_221_1    /* 2.21:1 */
+    } video_format_t;
+
+is used in the VIDEO_SET_FORMAT function (??) to tell the driver which
+aspect ratio the output hardware (e.g. TV) has. It is also used in the
+data structures video_status (??) returned by VIDEO_GET_STATUS (??)
+and video_event (??) returned by VIDEO_GET_EVENT (??) which report
+about the display format of the current video stream.
+
+
+.. _video-displayformat-t:
+
+video_displayformat_t
+=====================
+
+In case the display format of the video stream and of the display
+hardware differ the application has to specify how to handle the
+cropping of the picture. This can be done using the
+VIDEO_SET_DISPLAY_FORMAT call (??) which accepts
+
+
+.. code-block:: c
+
+    typedef enum {
+	VIDEO_PAN_SCAN,       /* use pan and scan format */
+	VIDEO_LETTER_BOX,     /* use letterbox format */
+	VIDEO_CENTER_CUT_OUT  /* use center cut out format */
+    } video_displayformat_t;
+
+as argument.
+
+
+.. _video-stream-source-t:
+
+video_stream_source_t
+=====================
+
+The video stream source is set through the VIDEO_SELECT_SOURCE call
+and can take the following values, depending on whether we are replaying
+from an internal (demuxer) or external (user write) source.
+
+
+.. code-block:: c
+
+    typedef enum {
+	VIDEO_SOURCE_DEMUX, /* Select the demux as the main source */
+	VIDEO_SOURCE_MEMORY /* If this source is selected, the stream
+		       comes from the user through the write
+		       system call */
+    } video_stream_source_t;
+
+VIDEO_SOURCE_DEMUX selects the demultiplexer (fed either by the
+frontend or the DVR device) as the source of the video stream. If
+VIDEO_SOURCE_MEMORY is selected the stream comes from the application
+through the **write()** system call.
+
+
+.. _video-play-state-t:
+
+video_play_state_t
+==================
+
+The following values can be returned by the VIDEO_GET_STATUS call
+representing the state of video playback.
+
+
+.. code-block:: c
+
+    typedef enum {
+	VIDEO_STOPPED, /* Video is stopped */
+	VIDEO_PLAYING, /* Video is currently playing */
+	VIDEO_FREEZED  /* Video is freezed */
+    } video_play_state_t;
+
+
+.. c:type:: video_command
+
+struct video_command
+====================
+
+The structure must be zeroed before use by the application This ensures
+it can be extended safely in the future.
+
+
+.. code-block:: c
+
+    struct video_command {
+	__u32 cmd;
+	__u32 flags;
+	union {
+	    struct {
+		__u64 pts;
+	    } stop;
+
+	    struct {
+		/* 0 or 1000 specifies normal speed,
+		   1 specifies forward single stepping,
+		   -1 specifies backward single stepping,
+		   >>1: playback at speed/1000 of the normal speed,
+		   <-1: reverse playback at (-speed/1000) of the normal speed. */
+		__s32 speed;
+		__u32 format;
+	    } play;
+
+	    struct {
+		__u32 data[16];
+	    } raw;
+	};
+    };
+
+
+.. _video-size-t:
+
+video_size_t
+============
+
+
+.. code-block:: c
+
+    typedef struct {
+	int w;
+	int h;
+	video_format_t aspect_ratio;
+    } video_size_t;
+
+
+.. c:type:: video_event
+
+struct video_event
+==================
+
+The following is the structure of a video event as it is returned by the
+VIDEO_GET_EVENT call.
+
+
+.. code-block:: c
+
+    struct video_event {
+	__s32 type;
+    #define VIDEO_EVENT_SIZE_CHANGED    1
+    #define VIDEO_EVENT_FRAME_RATE_CHANGED  2
+    #define VIDEO_EVENT_DECODER_STOPPED     3
+    #define VIDEO_EVENT_VSYNC       4
+	long timestamp;
+	union {
+	    video_size_t size;
+	    unsigned int frame_rate;    /* in frames per 1000sec */
+	    unsigned char vsync_field;  /* unknown/odd/even/progressive */
+	} u;
+    };
+
+
+.. c:type:: video_status
+
+struct video_status
+===================
+
+The VIDEO_GET_STATUS call returns the following structure informing
+about various states of the playback operation.
+
+
+.. code-block:: c
+
+    struct video_status {
+	int                   video_blank;   /* blank video on freeze? */
+	video_play_state_t    play_state;    /* current state of playback */
+	video_stream_source_t stream_source; /* current source (demux/memory) */
+	video_format_t        video_format;  /* current aspect ratio of stream */
+	video_displayformat_t display_format;/* selected cropping mode */
+    };
+
+If video_blank is set video will be blanked out if the channel is
+changed or if playback is stopped. Otherwise, the last picture will be
+displayed. play_state indicates if the video is currently frozen,
+stopped, or being played back. The stream_source corresponds to the
+selected source for the video stream. It can come either from the
+demultiplexer or from memory. The video_format indicates the aspect
+ratio (one of 4:3 or 16:9) of the currently played video stream.
+Finally, display_format corresponds to the selected cropping mode in
+case the source video format is not the same as the format of the output
+device.
+
+
+.. c:type:: video_still_picture
+
+struct video_still_picture
+==========================
+
+An I-frame displayed via the VIDEO_STILLPICTURE call is passed on
+within the following structure.
+
+
+.. code-block:: c
+
+    /* pointer to and size of a single iframe in memory */
+    struct video_still_picture {
+	char *iFrame;        /* pointer to a single iframe in memory */
+	int32_t size;
+    };
+
+
+.. _video_caps:
+
+video capabilities
+==================
+
+A call to VIDEO_GET_CAPABILITIES returns an unsigned integer with the
+following bits set according to the hardwares capabilities.
+
+
+.. code-block:: c
+
+     /* bit definitions for capabilities: */
+     /* can the hardware decode MPEG1 and/or MPEG2? */
+     #define VIDEO_CAP_MPEG1   1
+     #define VIDEO_CAP_MPEG2   2
+     /* can you send a system and/or program stream to video device?
+	(you still have to open the video and the audio device but only
+	 send the stream to the video device) */
+     #define VIDEO_CAP_SYS     4
+     #define VIDEO_CAP_PROG    8
+     /* can the driver also handle SPU, NAVI and CSS encoded data?
+	(CSS API is not present yet) */
+     #define VIDEO_CAP_SPU    16
+     #define VIDEO_CAP_NAVI   32
+     #define VIDEO_CAP_CSS    64
diff --git a/drivers/staging/media/deprecated/saa7146/common/Kconfig b/drivers/staging/media/deprecated/saa7146/common/Kconfig
new file mode 100644
index 000000000..a0aa155e5
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/Kconfig
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_SAA7146
+	tristate
+	depends on I2C && PCI
+
+config VIDEO_SAA7146_VV
+	tristate
+	depends on VIDEO_DEV
+	select VIDEOBUF_DMA_SG
+	select VIDEO_SAA7146
diff --git a/drivers/staging/media/deprecated/saa7146/common/Makefile b/drivers/staging/media/deprecated/saa7146/common/Makefile
new file mode 100644
index 000000000..2a6337fea
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+saa7146-objs    := saa7146_i2c.o saa7146_core.o
+saa7146_vv-objs := saa7146_fops.o saa7146_video.o saa7146_hlp.o saa7146_vbi.o
+
+obj-$(CONFIG_VIDEO_SAA7146) += saa7146.o
+obj-$(CONFIG_VIDEO_SAA7146_VV) += saa7146_vv.o
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146.h b/drivers/staging/media/deprecated/saa7146/common/saa7146.h
new file mode 100644
index 000000000..71ce63c99
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146.h
@@ -0,0 +1,472 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __SAA7146__
+#define __SAA7146__
+
+#include <linux/delay.h>	/* for delay-stuff */
+#include <linux/slab.h>		/* for kmalloc/kfree */
+#include <linux/pci.h>		/* for pci-config-stuff, vendor ids etc. */
+#include <linux/init.h>		/* for "__init" */
+#include <linux/interrupt.h>	/* for IMMEDIATE_BH */
+#include <linux/kmod.h>		/* for kernel module loader */
+#include <linux/i2c.h>		/* for i2c subsystem */
+#include <asm/io.h>		/* for accessing devices */
+#include <linux/stringify.h>
+#include <linux/mutex.h>
+#include <linux/scatterlist.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include <linux/vmalloc.h>	/* for vmalloc() */
+#include <linux/mm.h>		/* for vmalloc_to_page() */
+
+#define saa7146_write(sxy,adr,dat)    writel((dat),(sxy->mem+(adr)))
+#define saa7146_read(sxy,adr)         readl(sxy->mem+(adr))
+
+extern unsigned int saa7146_debug;
+
+#ifndef DEBUG_VARIABLE
+	#define DEBUG_VARIABLE saa7146_debug
+#endif
+
+#define ERR(fmt, ...)	pr_err("%s: " fmt, __func__, ##__VA_ARGS__)
+
+#define _DBG(mask, fmt, ...)						\
+do {									\
+	if (DEBUG_VARIABLE & mask)					\
+		pr_debug("%s(): " fmt, __func__, ##__VA_ARGS__);	\
+} while (0)
+
+/* simple debug messages */
+#define DEB_S(fmt, ...)		_DBG(0x01, fmt, ##__VA_ARGS__)
+/* more detailed debug messages */
+#define DEB_D(fmt, ...)		_DBG(0x02, fmt, ##__VA_ARGS__)
+/* print enter and exit of functions */
+#define DEB_EE(fmt, ...)	_DBG(0x04, fmt, ##__VA_ARGS__)
+/* i2c debug messages */
+#define DEB_I2C(fmt, ...)	_DBG(0x08, fmt, ##__VA_ARGS__)
+/* vbi debug messages */
+#define DEB_VBI(fmt, ...)	_DBG(0x10, fmt, ##__VA_ARGS__)
+/* interrupt debug messages */
+#define DEB_INT(fmt, ...)	_DBG(0x20, fmt, ##__VA_ARGS__)
+/* capture debug messages */
+#define DEB_CAP(fmt, ...)	_DBG(0x40, fmt, ##__VA_ARGS__)
+
+#define SAA7146_ISR_CLEAR(x,y) \
+	saa7146_write(x, ISR, (y));
+
+struct module;
+
+struct saa7146_dev;
+struct saa7146_extension;
+struct saa7146_vv;
+
+/* saa7146 page table */
+struct saa7146_pgtable {
+	unsigned int	size;
+	__le32		*cpu;
+	dma_addr_t	dma;
+	/* used for offsets for u,v planes for planar capture modes */
+	unsigned long	offset;
+	/* used for custom pagetables (used for example by budget dvb cards) */
+	struct scatterlist *slist;
+	int		nents;
+};
+
+struct saa7146_pci_extension_data {
+	struct saa7146_extension *ext;
+	void *ext_priv;			/* most likely a name string */
+};
+
+#define MAKE_EXTENSION_PCI(x_var, x_vendor, x_device)		\
+	{							\
+		.vendor    = PCI_VENDOR_ID_PHILIPS,		\
+		.device	   = PCI_DEVICE_ID_PHILIPS_SAA7146,	\
+		.subvendor = x_vendor,				\
+		.subdevice = x_device,				\
+		.driver_data = (unsigned long)& x_var,		\
+	}
+
+struct saa7146_extension
+{
+	char	name[32];		/* name of the device */
+#define SAA7146_USE_I2C_IRQ	0x1
+#define SAA7146_I2C_SHORT_DELAY	0x2
+	int	flags;
+
+	/* pairs of subvendor and subdevice ids for
+	   supported devices, last entry 0xffff, 0xfff */
+	struct module *module;
+	struct pci_driver driver;
+	const struct pci_device_id *pci_tbl;
+
+	/* extension functions */
+	int (*probe)(struct saa7146_dev *);
+	int (*attach)(struct saa7146_dev *, struct saa7146_pci_extension_data *);
+	int (*detach)(struct saa7146_dev*);
+
+	u32	irq_mask;	/* mask to indicate, which irq-events are handled by the extension */
+	void	(*irq_func)(struct saa7146_dev*, u32* irq_mask);
+};
+
+struct saa7146_dma
+{
+	dma_addr_t	dma_handle;
+	__le32		*cpu_addr;
+};
+
+struct saa7146_dev
+{
+	struct module			*module;
+
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_ctrl_handler	ctrl_handler;
+
+	/* different device locks */
+	spinlock_t			slock;
+	struct mutex			v4l2_lock;
+
+	unsigned char			__iomem *mem;		/* pointer to mapped IO memory */
+	u32				revision;	/* chip revision; needed for bug-workarounds*/
+
+	/* pci-device & irq stuff*/
+	char				name[32];
+	struct pci_dev			*pci;
+	u32				int_todo;
+	spinlock_t			int_slock;
+
+	/* extension handling */
+	struct saa7146_extension	*ext;		/* indicates if handled by extension */
+	void				*ext_priv;	/* pointer for extension private use (most likely some private data) */
+	struct saa7146_ext_vv		*ext_vv_data;
+
+	/* per device video/vbi information (if available) */
+	struct saa7146_vv	*vv_data;
+	void (*vv_callback)(struct saa7146_dev *dev, unsigned long status);
+
+	/* i2c-stuff */
+	struct mutex			i2c_lock;
+
+	u32				i2c_bitrate;
+	struct saa7146_dma		d_i2c;	/* pointer to i2c memory */
+	wait_queue_head_t		i2c_wq;
+	int				i2c_op;
+
+	/* memories */
+	struct saa7146_dma		d_rps0;
+	struct saa7146_dma		d_rps1;
+};
+
+static inline struct saa7146_dev *to_saa7146_dev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct saa7146_dev, v4l2_dev);
+}
+
+/* from saa7146_i2c.c */
+int saa7146_i2c_adapter_prepare(struct saa7146_dev *dev, struct i2c_adapter *i2c_adapter, u32 bitrate);
+
+/* from saa7146_core.c */
+int saa7146_register_extension(struct saa7146_extension*);
+int saa7146_unregister_extension(struct saa7146_extension*);
+struct saa7146_format* saa7146_format_by_fourcc(struct saa7146_dev *dev, int fourcc);
+int saa7146_pgtable_alloc(struct pci_dev *pci, struct saa7146_pgtable *pt);
+void saa7146_pgtable_free(struct pci_dev *pci, struct saa7146_pgtable *pt);
+int saa7146_pgtable_build_single(struct pci_dev *pci, struct saa7146_pgtable *pt, struct scatterlist *list, int length );
+void *saa7146_vmalloc_build_pgtable(struct pci_dev *pci, long length, struct saa7146_pgtable *pt);
+void saa7146_vfree_destroy_pgtable(struct pci_dev *pci, void *mem, struct saa7146_pgtable *pt);
+void saa7146_setgpio(struct saa7146_dev *dev, int port, u32 data);
+int saa7146_wait_for_debi_done(struct saa7146_dev *dev, int nobusyloop);
+
+/* some memory sizes */
+#define SAA7146_I2C_MEM		( 1*PAGE_SIZE)
+#define SAA7146_RPS_MEM		( 1*PAGE_SIZE)
+
+/* some i2c constants */
+#define SAA7146_I2C_TIMEOUT	100	/* i2c-timeout-value in ms */
+#define SAA7146_I2C_RETRIES	3	/* how many times shall we retry an i2c-operation? */
+#define SAA7146_I2C_DELAY	5	/* time we wait after certain i2c-operations */
+
+/* unsorted defines */
+#define ME1    0x0000000800
+#define PV1    0x0000000008
+
+/* gpio defines */
+#define SAA7146_GPIO_INPUT 0x00
+#define SAA7146_GPIO_IRQHI 0x10
+#define SAA7146_GPIO_IRQLO 0x20
+#define SAA7146_GPIO_IRQHL 0x30
+#define SAA7146_GPIO_OUTLO 0x40
+#define SAA7146_GPIO_OUTHI 0x50
+
+/* debi defines */
+#define DEBINOSWAP 0x000e0000
+
+/* define for the register programming sequencer (rps) */
+#define CMD_NOP		0x00000000  /* No operation */
+#define CMD_CLR_EVENT	0x00000000  /* Clear event */
+#define CMD_SET_EVENT	0x10000000  /* Set signal event */
+#define CMD_PAUSE	0x20000000  /* Pause */
+#define CMD_CHECK_LATE	0x30000000  /* Check late */
+#define CMD_UPLOAD	0x40000000  /* Upload */
+#define CMD_STOP	0x50000000  /* Stop */
+#define CMD_INTERRUPT	0x60000000  /* Interrupt */
+#define CMD_JUMP	0x80000000  /* Jump */
+#define CMD_WR_REG	0x90000000  /* Write (load) register */
+#define CMD_RD_REG	0xa0000000  /* Read (store) register */
+#define CMD_WR_REG_MASK	0xc0000000  /* Write register with mask */
+
+#define CMD_OAN		MASK_27
+#define CMD_INV		MASK_26
+#define CMD_SIG4	MASK_25
+#define CMD_SIG3	MASK_24
+#define CMD_SIG2	MASK_23
+#define CMD_SIG1	MASK_22
+#define CMD_SIG0	MASK_21
+#define CMD_O_FID_B	MASK_14
+#define CMD_E_FID_B	MASK_13
+#define CMD_O_FID_A	MASK_12
+#define CMD_E_FID_A	MASK_11
+
+/* some events and command modifiers for rps1 squarewave generator */
+#define EVT_HS          (1<<15)     // Source Line Threshold reached
+#define EVT_VBI_B       (1<<9)      // VSYNC Event
+#define RPS_OAN         (1<<27)     // 1: OR events, 0: AND events
+#define RPS_INV         (1<<26)     // Invert (compound) event
+#define GPIO3_MSK       0xFF000000  // GPIO #3 control bits
+
+/* Bit mask constants */
+#define MASK_00   0x00000001    /* Mask value for bit 0 */
+#define MASK_01   0x00000002    /* Mask value for bit 1 */
+#define MASK_02   0x00000004    /* Mask value for bit 2 */
+#define MASK_03   0x00000008    /* Mask value for bit 3 */
+#define MASK_04   0x00000010    /* Mask value for bit 4 */
+#define MASK_05   0x00000020    /* Mask value for bit 5 */
+#define MASK_06   0x00000040    /* Mask value for bit 6 */
+#define MASK_07   0x00000080    /* Mask value for bit 7 */
+#define MASK_08   0x00000100    /* Mask value for bit 8 */
+#define MASK_09   0x00000200    /* Mask value for bit 9 */
+#define MASK_10   0x00000400    /* Mask value for bit 10 */
+#define MASK_11   0x00000800    /* Mask value for bit 11 */
+#define MASK_12   0x00001000    /* Mask value for bit 12 */
+#define MASK_13   0x00002000    /* Mask value for bit 13 */
+#define MASK_14   0x00004000    /* Mask value for bit 14 */
+#define MASK_15   0x00008000    /* Mask value for bit 15 */
+#define MASK_16   0x00010000    /* Mask value for bit 16 */
+#define MASK_17   0x00020000    /* Mask value for bit 17 */
+#define MASK_18   0x00040000    /* Mask value for bit 18 */
+#define MASK_19   0x00080000    /* Mask value for bit 19 */
+#define MASK_20   0x00100000    /* Mask value for bit 20 */
+#define MASK_21   0x00200000    /* Mask value for bit 21 */
+#define MASK_22   0x00400000    /* Mask value for bit 22 */
+#define MASK_23   0x00800000    /* Mask value for bit 23 */
+#define MASK_24   0x01000000    /* Mask value for bit 24 */
+#define MASK_25   0x02000000    /* Mask value for bit 25 */
+#define MASK_26   0x04000000    /* Mask value for bit 26 */
+#define MASK_27   0x08000000    /* Mask value for bit 27 */
+#define MASK_28   0x10000000    /* Mask value for bit 28 */
+#define MASK_29   0x20000000    /* Mask value for bit 29 */
+#define MASK_30   0x40000000    /* Mask value for bit 30 */
+#define MASK_31   0x80000000    /* Mask value for bit 31 */
+
+#define MASK_B0   0x000000ff    /* Mask value for byte 0 */
+#define MASK_B1   0x0000ff00    /* Mask value for byte 1 */
+#define MASK_B2   0x00ff0000    /* Mask value for byte 2 */
+#define MASK_B3   0xff000000    /* Mask value for byte 3 */
+
+#define MASK_W0   0x0000ffff    /* Mask value for word 0 */
+#define MASK_W1   0xffff0000    /* Mask value for word 1 */
+
+#define MASK_PA   0xfffffffc    /* Mask value for physical address */
+#define MASK_PR   0xfffffffe	/* Mask value for protection register */
+#define MASK_ER   0xffffffff    /* Mask value for the entire register */
+
+#define MASK_NONE 0x00000000    /* No mask */
+
+/* register aliases */
+#define BASE_ODD1         0x00  /* Video DMA 1 registers  */
+#define BASE_EVEN1        0x04
+#define PROT_ADDR1        0x08
+#define PITCH1            0x0C
+#define BASE_PAGE1        0x10  /* Video DMA 1 base page */
+#define NUM_LINE_BYTE1    0x14
+
+#define BASE_ODD2         0x18  /* Video DMA 2 registers */
+#define BASE_EVEN2        0x1C
+#define PROT_ADDR2        0x20
+#define PITCH2            0x24
+#define BASE_PAGE2        0x28  /* Video DMA 2 base page */
+#define NUM_LINE_BYTE2    0x2C
+
+#define BASE_ODD3         0x30  /* Video DMA 3 registers */
+#define BASE_EVEN3        0x34
+#define PROT_ADDR3        0x38
+#define PITCH3            0x3C
+#define BASE_PAGE3        0x40  /* Video DMA 3 base page */
+#define NUM_LINE_BYTE3    0x44
+
+#define PCI_BT_V1         0x48  /* Video/FIFO 1 */
+#define PCI_BT_V2         0x49  /* Video/FIFO 2 */
+#define PCI_BT_V3         0x4A  /* Video/FIFO 3 */
+#define PCI_BT_DEBI       0x4B  /* DEBI */
+#define PCI_BT_A          0x4C  /* Audio */
+
+#define DD1_INIT          0x50  /* Init setting of DD1 interface */
+
+#define DD1_STREAM_B      0x54  /* DD1 B video data stream handling */
+#define DD1_STREAM_A      0x56  /* DD1 A video data stream handling */
+
+#define BRS_CTRL          0x58  /* BRS control register */
+#define HPS_CTRL          0x5C  /* HPS control register */
+#define HPS_V_SCALE       0x60  /* HPS vertical scale */
+#define HPS_V_GAIN        0x64  /* HPS vertical ACL and gain */
+#define HPS_H_PRESCALE    0x68  /* HPS horizontal prescale   */
+#define HPS_H_SCALE       0x6C  /* HPS horizontal scale */
+#define BCS_CTRL          0x70  /* BCS control */
+#define CHROMA_KEY_RANGE  0x74
+#define CLIP_FORMAT_CTRL  0x78  /* HPS outputs formats & clipping */
+
+#define DEBI_CONFIG       0x7C
+#define DEBI_COMMAND      0x80
+#define DEBI_PAGE         0x84
+#define DEBI_AD           0x88
+
+#define I2C_TRANSFER      0x8C
+#define I2C_STATUS        0x90
+
+#define BASE_A1_IN        0x94	/* Audio 1 input DMA */
+#define PROT_A1_IN        0x98
+#define PAGE_A1_IN        0x9C
+
+#define BASE_A1_OUT       0xA0  /* Audio 1 output DMA */
+#define PROT_A1_OUT       0xA4
+#define PAGE_A1_OUT       0xA8
+
+#define BASE_A2_IN        0xAC  /* Audio 2 input DMA */
+#define PROT_A2_IN        0xB0
+#define PAGE_A2_IN        0xB4
+
+#define BASE_A2_OUT       0xB8  /* Audio 2 output DMA */
+#define PROT_A2_OUT       0xBC
+#define PAGE_A2_OUT       0xC0
+
+#define RPS_PAGE0         0xC4  /* RPS task 0 page register */
+#define RPS_PAGE1         0xC8  /* RPS task 1 page register */
+
+#define RPS_THRESH0       0xCC  /* HBI threshold for task 0 */
+#define RPS_THRESH1       0xD0  /* HBI threshold for task 1 */
+
+#define RPS_TOV0          0xD4  /* RPS timeout for task 0 */
+#define RPS_TOV1          0xD8  /* RPS timeout for task 1 */
+
+#define IER               0xDC  /* Interrupt enable register */
+
+#define GPIO_CTRL         0xE0  /* GPIO 0-3 register */
+
+#define EC1SSR            0xE4  /* Event cnt set 1 source select */
+#define EC2SSR            0xE8  /* Event cnt set 2 source select */
+#define ECT1R             0xEC  /* Event cnt set 1 thresholds */
+#define ECT2R             0xF0  /* Event cnt set 2 thresholds */
+
+#define ACON1             0xF4
+#define ACON2             0xF8
+
+#define MC1               0xFC   /* Main control register 1 */
+#define MC2               0x100  /* Main control register 2  */
+
+#define RPS_ADDR0         0x104  /* RPS task 0 address register */
+#define RPS_ADDR1         0x108  /* RPS task 1 address register */
+
+#define ISR               0x10C  /* Interrupt status register */
+#define PSR               0x110  /* Primary status register */
+#define SSR               0x114  /* Secondary status register */
+
+#define EC1R              0x118  /* Event counter set 1 register */
+#define EC2R              0x11C  /* Event counter set 2 register */
+
+#define PCI_VDP1          0x120  /* Video DMA pointer of FIFO 1 */
+#define PCI_VDP2          0x124  /* Video DMA pointer of FIFO 2 */
+#define PCI_VDP3          0x128  /* Video DMA pointer of FIFO 3 */
+#define PCI_ADP1          0x12C  /* Audio DMA pointer of audio out 1 */
+#define PCI_ADP2          0x130  /* Audio DMA pointer of audio in 1 */
+#define PCI_ADP3          0x134  /* Audio DMA pointer of audio out 2 */
+#define PCI_ADP4          0x138  /* Audio DMA pointer of audio in 2 */
+#define PCI_DMA_DDP       0x13C  /* DEBI DMA pointer */
+
+#define LEVEL_REP         0x140,
+#define A_TIME_SLOT1      0x180,  /* from 180 - 1BC */
+#define A_TIME_SLOT2      0x1C0,  /* from 1C0 - 1FC */
+
+/* isr masks */
+#define SPCI_PPEF       0x80000000  /* PCI parity error */
+#define SPCI_PABO       0x40000000  /* PCI access error (target or master abort) */
+#define SPCI_PPED       0x20000000  /* PCI parity error on 'real time data' */
+#define SPCI_RPS_I1     0x10000000  /* Interrupt issued by RPS1 */
+#define SPCI_RPS_I0     0x08000000  /* Interrupt issued by RPS0 */
+#define SPCI_RPS_LATE1  0x04000000  /* RPS task 1 is late */
+#define SPCI_RPS_LATE0  0x02000000  /* RPS task 0 is late */
+#define SPCI_RPS_E1     0x01000000  /* RPS error from task 1 */
+#define SPCI_RPS_E0     0x00800000  /* RPS error from task 0 */
+#define SPCI_RPS_TO1    0x00400000  /* RPS timeout task 1 */
+#define SPCI_RPS_TO0    0x00200000  /* RPS timeout task 0 */
+#define SPCI_UPLD       0x00100000  /* RPS in upload */
+#define SPCI_DEBI_S     0x00080000  /* DEBI status */
+#define SPCI_DEBI_E     0x00040000  /* DEBI error */
+#define SPCI_IIC_S      0x00020000  /* I2C status */
+#define SPCI_IIC_E      0x00010000  /* I2C error */
+#define SPCI_A2_IN      0x00008000  /* Audio 2 input DMA protection / limit */
+#define SPCI_A2_OUT     0x00004000  /* Audio 2 output DMA protection / limit */
+#define SPCI_A1_IN      0x00002000  /* Audio 1 input DMA protection / limit */
+#define SPCI_A1_OUT     0x00001000  /* Audio 1 output DMA protection / limit */
+#define SPCI_AFOU       0x00000800  /* Audio FIFO over- / underflow */
+#define SPCI_V_PE       0x00000400  /* Video protection address */
+#define SPCI_VFOU       0x00000200  /* Video FIFO over- / underflow */
+#define SPCI_FIDA       0x00000100  /* Field ID video port A */
+#define SPCI_FIDB       0x00000080  /* Field ID video port B */
+#define SPCI_PIN3       0x00000040  /* GPIO pin 3 */
+#define SPCI_PIN2       0x00000020  /* GPIO pin 2 */
+#define SPCI_PIN1       0x00000010  /* GPIO pin 1 */
+#define SPCI_PIN0       0x00000008  /* GPIO pin 0 */
+#define SPCI_ECS        0x00000004  /* Event counter 1, 2, 4, 5 */
+#define SPCI_EC3S       0x00000002  /* Event counter 3 */
+#define SPCI_EC0S       0x00000001  /* Event counter 0 */
+
+/* i2c */
+#define	SAA7146_I2C_ABORT	(1<<7)
+#define	SAA7146_I2C_SPERR	(1<<6)
+#define	SAA7146_I2C_APERR	(1<<5)
+#define	SAA7146_I2C_DTERR	(1<<4)
+#define	SAA7146_I2C_DRERR	(1<<3)
+#define	SAA7146_I2C_AL		(1<<2)
+#define	SAA7146_I2C_ERR		(1<<1)
+#define	SAA7146_I2C_BUSY	(1<<0)
+
+#define	SAA7146_I2C_START	(0x3)
+#define	SAA7146_I2C_CONT	(0x2)
+#define	SAA7146_I2C_STOP	(0x1)
+#define	SAA7146_I2C_NOP		(0x0)
+
+#define SAA7146_I2C_BUS_BIT_RATE_6400	(0x500)
+#define SAA7146_I2C_BUS_BIT_RATE_3200	(0x100)
+#define SAA7146_I2C_BUS_BIT_RATE_480	(0x400)
+#define SAA7146_I2C_BUS_BIT_RATE_320	(0x600)
+#define SAA7146_I2C_BUS_BIT_RATE_240	(0x700)
+#define SAA7146_I2C_BUS_BIT_RATE_120	(0x000)
+#define SAA7146_I2C_BUS_BIT_RATE_80	(0x200)
+#define SAA7146_I2C_BUS_BIT_RATE_60	(0x300)
+
+static inline void SAA7146_IER_DISABLE(struct saa7146_dev *x, unsigned y)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&x->int_slock, flags);
+	saa7146_write(x, IER, saa7146_read(x, IER) & ~y);
+	spin_unlock_irqrestore(&x->int_slock, flags);
+}
+
+static inline void SAA7146_IER_ENABLE(struct saa7146_dev *x, unsigned y)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&x->int_slock, flags);
+	saa7146_write(x, IER, saa7146_read(x, IER) | y);
+	spin_unlock_irqrestore(&x->int_slock, flags);
+}
+
+#endif
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_core.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_core.c
new file mode 100644
index 000000000..da21d346b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_core.c
@@ -0,0 +1,578 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    saa7146.o - driver for generic saa7146-based hardware
+
+    Copyright (C) 1998-2003 Michael Hunold <michael@mihu.de>
+
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include "saa7146.h"
+
+static int saa7146_num;
+
+unsigned int saa7146_debug;
+
+module_param(saa7146_debug, uint, 0644);
+MODULE_PARM_DESC(saa7146_debug, "debug level (default: 0)");
+
+#if 0
+static void dump_registers(struct saa7146_dev* dev)
+{
+	int i = 0;
+
+	pr_info(" @ %li jiffies:\n", jiffies);
+	for (i = 0; i <= 0x148; i += 4)
+		pr_info("0x%03x: 0x%08x\n", i, saa7146_read(dev, i));
+}
+#endif
+
+/****************************************************************************
+ * gpio and debi helper functions
+ ****************************************************************************/
+
+void saa7146_setgpio(struct saa7146_dev *dev, int port, u32 data)
+{
+	u32 value = 0;
+
+	BUG_ON(port > 3);
+
+	value = saa7146_read(dev, GPIO_CTRL);
+	value &= ~(0xff << (8*port));
+	value |= (data << (8*port));
+	saa7146_write(dev, GPIO_CTRL, value);
+}
+
+/* This DEBI code is based on the saa7146 Stradis driver by Nathan Laredo */
+static inline int saa7146_wait_for_debi_done_sleep(struct saa7146_dev *dev,
+				unsigned long us1, unsigned long us2)
+{
+	unsigned long timeout;
+	int err;
+
+	/* wait for registers to be programmed */
+	timeout = jiffies + usecs_to_jiffies(us1);
+	while (1) {
+		err = time_after(jiffies, timeout);
+		if (saa7146_read(dev, MC2) & 2)
+			break;
+		if (err) {
+			pr_debug("%s: %s timed out while waiting for registers getting programmed\n",
+			       dev->name, __func__);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+
+	/* wait for transfer to complete */
+	timeout = jiffies + usecs_to_jiffies(us2);
+	while (1) {
+		err = time_after(jiffies, timeout);
+		if (!(saa7146_read(dev, PSR) & SPCI_DEBI_S))
+			break;
+		saa7146_read(dev, MC2);
+		if (err) {
+			DEB_S("%s: %s timed out while waiting for transfer completion\n",
+			      dev->name, __func__);
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+
+	return 0;
+}
+
+static inline int saa7146_wait_for_debi_done_busyloop(struct saa7146_dev *dev,
+				unsigned long us1, unsigned long us2)
+{
+	unsigned long loops;
+
+	/* wait for registers to be programmed */
+	loops = us1;
+	while (1) {
+		if (saa7146_read(dev, MC2) & 2)
+			break;
+		if (!loops--) {
+			pr_err("%s: %s timed out while waiting for registers getting programmed\n",
+			       dev->name, __func__);
+			return -ETIMEDOUT;
+		}
+		udelay(1);
+	}
+
+	/* wait for transfer to complete */
+	loops = us2 / 5;
+	while (1) {
+		if (!(saa7146_read(dev, PSR) & SPCI_DEBI_S))
+			break;
+		saa7146_read(dev, MC2);
+		if (!loops--) {
+			DEB_S("%s: %s timed out while waiting for transfer completion\n",
+			      dev->name, __func__);
+			return -ETIMEDOUT;
+		}
+		udelay(5);
+	}
+
+	return 0;
+}
+
+int saa7146_wait_for_debi_done(struct saa7146_dev *dev, int nobusyloop)
+{
+	if (nobusyloop)
+		return saa7146_wait_for_debi_done_sleep(dev, 50000, 250000);
+	else
+		return saa7146_wait_for_debi_done_busyloop(dev, 50000, 250000);
+}
+
+/****************************************************************************
+ * general helper functions
+ ****************************************************************************/
+
+/* this is videobuf_vmalloc_to_sg() from videobuf-dma-sg.c
+   make sure virt has been allocated with vmalloc_32(), otherwise the BUG()
+   may be triggered on highmem machines */
+static struct scatterlist* vmalloc_to_sg(unsigned char *virt, int nr_pages)
+{
+	struct scatterlist *sglist;
+	struct page *pg;
+	int i;
+
+	sglist = kmalloc_array(nr_pages, sizeof(struct scatterlist), GFP_KERNEL);
+	if (NULL == sglist)
+		return NULL;
+	sg_init_table(sglist, nr_pages);
+	for (i = 0; i < nr_pages; i++, virt += PAGE_SIZE) {
+		pg = vmalloc_to_page(virt);
+		if (NULL == pg)
+			goto err;
+		BUG_ON(PageHighMem(pg));
+		sg_set_page(&sglist[i], pg, PAGE_SIZE, 0);
+	}
+	return sglist;
+
+ err:
+	kfree(sglist);
+	return NULL;
+}
+
+/********************************************************************************/
+/* common page table functions */
+
+void *saa7146_vmalloc_build_pgtable(struct pci_dev *pci, long length, struct saa7146_pgtable *pt)
+{
+	int pages = (length+PAGE_SIZE-1)/PAGE_SIZE;
+	void *mem = vmalloc_32(length);
+	int slen = 0;
+
+	if (NULL == mem)
+		goto err_null;
+
+	if (!(pt->slist = vmalloc_to_sg(mem, pages)))
+		goto err_free_mem;
+
+	if (saa7146_pgtable_alloc(pci, pt))
+		goto err_free_slist;
+
+	pt->nents = pages;
+	slen = dma_map_sg(&pci->dev, pt->slist, pt->nents, DMA_FROM_DEVICE);
+	if (0 == slen)
+		goto err_free_pgtable;
+
+	if (0 != saa7146_pgtable_build_single(pci, pt, pt->slist, slen))
+		goto err_unmap_sg;
+
+	return mem;
+
+err_unmap_sg:
+	dma_unmap_sg(&pci->dev, pt->slist, pt->nents, DMA_FROM_DEVICE);
+err_free_pgtable:
+	saa7146_pgtable_free(pci, pt);
+err_free_slist:
+	kfree(pt->slist);
+	pt->slist = NULL;
+err_free_mem:
+	vfree(mem);
+err_null:
+	return NULL;
+}
+
+void saa7146_vfree_destroy_pgtable(struct pci_dev *pci, void *mem, struct saa7146_pgtable *pt)
+{
+	dma_unmap_sg(&pci->dev, pt->slist, pt->nents, DMA_FROM_DEVICE);
+	saa7146_pgtable_free(pci, pt);
+	kfree(pt->slist);
+	pt->slist = NULL;
+	vfree(mem);
+}
+
+void saa7146_pgtable_free(struct pci_dev *pci, struct saa7146_pgtable *pt)
+{
+	if (NULL == pt->cpu)
+		return;
+	dma_free_coherent(&pci->dev, pt->size, pt->cpu, pt->dma);
+	pt->cpu = NULL;
+}
+
+int saa7146_pgtable_alloc(struct pci_dev *pci, struct saa7146_pgtable *pt)
+{
+	__le32       *cpu;
+	dma_addr_t   dma_addr = 0;
+
+	cpu = dma_alloc_coherent(&pci->dev, PAGE_SIZE, &dma_addr, GFP_KERNEL);
+	if (NULL == cpu) {
+		return -ENOMEM;
+	}
+	pt->size = PAGE_SIZE;
+	pt->cpu  = cpu;
+	pt->dma  = dma_addr;
+
+	return 0;
+}
+
+int saa7146_pgtable_build_single(struct pci_dev *pci, struct saa7146_pgtable *pt,
+	struct scatterlist *list, int sglen  )
+{
+	__le32 *ptr, fill;
+	int nr_pages = 0;
+	int i,p;
+
+	BUG_ON(0 == sglen);
+	BUG_ON(list->offset > PAGE_SIZE);
+
+	/* if we have a user buffer, the first page may not be
+	   aligned to a page boundary. */
+	pt->offset = list->offset;
+
+	ptr = pt->cpu;
+	for (i = 0; i < sglen; i++, list++) {
+/*
+		pr_debug("i:%d, adr:0x%08x, len:%d, offset:%d\n",
+			 i, sg_dma_address(list), sg_dma_len(list),
+			 list->offset);
+*/
+		for (p = 0; p * 4096 < sg_dma_len(list); p++, ptr++) {
+			*ptr = cpu_to_le32(sg_dma_address(list) + p * 4096);
+			nr_pages++;
+		}
+	}
+
+
+	/* safety; fill the page table up with the last valid page */
+	fill = *(ptr-1);
+	for(i=nr_pages;i<1024;i++) {
+		*ptr++ = fill;
+	}
+
+/*
+	ptr = pt->cpu;
+	pr_debug("offset: %d\n", pt->offset);
+	for(i=0;i<5;i++) {
+		pr_debug("ptr1 %d: 0x%08x\n", i, ptr[i]);
+	}
+*/
+	return 0;
+}
+
+/********************************************************************************/
+/* interrupt handler */
+static irqreturn_t interrupt_hw(int irq, void *dev_id)
+{
+	struct saa7146_dev *dev = dev_id;
+	u32 isr;
+	u32 ack_isr;
+
+	/* read out the interrupt status register */
+	ack_isr = isr = saa7146_read(dev, ISR);
+
+	/* is this our interrupt? */
+	if ( 0 == isr ) {
+		/* nope, some other device */
+		return IRQ_NONE;
+	}
+
+	if (dev->ext) {
+		if (dev->ext->irq_mask & isr) {
+			if (dev->ext->irq_func)
+				dev->ext->irq_func(dev, &isr);
+			isr &= ~dev->ext->irq_mask;
+		}
+	}
+	if (0 != (isr & (MASK_27))) {
+		DEB_INT("irq: RPS0 (0x%08x)\n", isr);
+		if (dev->vv_data && dev->vv_callback)
+			dev->vv_callback(dev,isr);
+		isr &= ~MASK_27;
+	}
+	if (0 != (isr & (MASK_28))) {
+		if (dev->vv_data && dev->vv_callback)
+			dev->vv_callback(dev,isr);
+		isr &= ~MASK_28;
+	}
+	if (0 != (isr & (MASK_16|MASK_17))) {
+		SAA7146_IER_DISABLE(dev, MASK_16|MASK_17);
+		/* only wake up if we expect something */
+		if (0 != dev->i2c_op) {
+			dev->i2c_op = 0;
+			wake_up(&dev->i2c_wq);
+		} else {
+			u32 psr = saa7146_read(dev, PSR);
+			u32 ssr = saa7146_read(dev, SSR);
+			pr_warn("%s: unexpected i2c irq: isr %08x psr %08x ssr %08x\n",
+				dev->name, isr, psr, ssr);
+		}
+		isr &= ~(MASK_16|MASK_17);
+	}
+	if( 0 != isr ) {
+		ERR("warning: interrupt enabled, but not handled properly.(0x%08x)\n",
+		    isr);
+		ERR("disabling interrupt source(s)!\n");
+		SAA7146_IER_DISABLE(dev,isr);
+	}
+	saa7146_write(dev, ISR, ack_isr);
+	return IRQ_HANDLED;
+}
+
+/*********************************************************************************/
+/* configuration-functions                                                       */
+
+static int saa7146_init_one(struct pci_dev *pci, const struct pci_device_id *ent)
+{
+	struct saa7146_pci_extension_data *pci_ext = (struct saa7146_pci_extension_data *)ent->driver_data;
+	struct saa7146_extension *ext = pci_ext->ext;
+	struct saa7146_dev *dev;
+	int err = -ENOMEM;
+
+	/* clear out mem for sure */
+	dev = kzalloc(sizeof(struct saa7146_dev), GFP_KERNEL);
+	if (!dev) {
+		ERR("out of memory\n");
+		goto out;
+	}
+
+	/* create a nice device name */
+	sprintf(dev->name, "saa7146 (%d)", saa7146_num);
+
+	DEB_EE("pci:%p\n", pci);
+
+	err = pci_enable_device(pci);
+	if (err < 0) {
+		ERR("pci_enable_device() failed\n");
+		goto err_free;
+	}
+
+	/* enable bus-mastering */
+	pci_set_master(pci);
+
+	dev->pci = pci;
+
+	/* get chip-revision; this is needed to enable bug-fixes */
+	dev->revision = pci->revision;
+
+	/* remap the memory from virtual to physical address */
+
+	err = pci_request_region(pci, 0, "saa7146");
+	if (err < 0)
+		goto err_disable;
+
+	dev->mem = ioremap(pci_resource_start(pci, 0),
+			   pci_resource_len(pci, 0));
+	if (!dev->mem) {
+		ERR("ioremap() failed\n");
+		err = -ENODEV;
+		goto err_release;
+	}
+
+	/* we don't do a master reset here anymore, it screws up
+	   some boards that don't have an i2c-eeprom for configuration
+	   values */
+/*
+	saa7146_write(dev, MC1, MASK_31);
+*/
+
+	/* disable all irqs */
+	saa7146_write(dev, IER, 0);
+
+	/* shut down all dma transfers and rps tasks */
+	saa7146_write(dev, MC1, 0x30ff0000);
+
+	/* clear out any rps-signals pending */
+	saa7146_write(dev, MC2, 0xf8000000);
+
+	/* request an interrupt for the saa7146 */
+	err = request_irq(pci->irq, interrupt_hw, IRQF_SHARED,
+			  dev->name, dev);
+	if (err < 0) {
+		ERR("request_irq() failed\n");
+		goto err_unmap;
+	}
+
+	err = -ENOMEM;
+
+	/* get memory for various stuff */
+	dev->d_rps0.cpu_addr = dma_alloc_coherent(&pci->dev, SAA7146_RPS_MEM,
+						  &dev->d_rps0.dma_handle,
+						  GFP_KERNEL);
+	if (!dev->d_rps0.cpu_addr)
+		goto err_free_irq;
+
+	dev->d_rps1.cpu_addr = dma_alloc_coherent(&pci->dev, SAA7146_RPS_MEM,
+						  &dev->d_rps1.dma_handle,
+						  GFP_KERNEL);
+	if (!dev->d_rps1.cpu_addr)
+		goto err_free_rps0;
+
+	dev->d_i2c.cpu_addr = dma_alloc_coherent(&pci->dev, SAA7146_RPS_MEM,
+						 &dev->d_i2c.dma_handle, GFP_KERNEL);
+	if (!dev->d_i2c.cpu_addr)
+		goto err_free_rps1;
+
+	/* the rest + print status message */
+
+	pr_info("found saa7146 @ mem %p (revision %d, irq %d) (0x%04x,0x%04x)\n",
+		dev->mem, dev->revision, pci->irq,
+		pci->subsystem_vendor, pci->subsystem_device);
+	dev->ext = ext;
+
+	mutex_init(&dev->v4l2_lock);
+	spin_lock_init(&dev->int_slock);
+	spin_lock_init(&dev->slock);
+
+	mutex_init(&dev->i2c_lock);
+
+	dev->module = THIS_MODULE;
+	init_waitqueue_head(&dev->i2c_wq);
+
+	/* set some sane pci arbitrition values */
+	saa7146_write(dev, PCI_BT_V1, 0x1c00101f);
+
+	/* TODO: use the status code of the callback */
+
+	err = -ENODEV;
+
+	if (ext->probe && ext->probe(dev)) {
+		DEB_D("ext->probe() failed for %p. skipping device.\n", dev);
+		goto err_free_i2c;
+	}
+
+	if (ext->attach(dev, pci_ext)) {
+		DEB_D("ext->attach() failed for %p. skipping device.\n", dev);
+		goto err_free_i2c;
+	}
+	/* V4L extensions will set the pci drvdata to the v4l2_device in the
+	   attach() above. So for those cards that do not use V4L we have to
+	   set it explicitly. */
+	pci_set_drvdata(pci, &dev->v4l2_dev);
+
+	saa7146_num++;
+
+	err = 0;
+out:
+	return err;
+
+err_free_i2c:
+	dma_free_coherent(&pci->dev, SAA7146_RPS_MEM, dev->d_i2c.cpu_addr,
+			  dev->d_i2c.dma_handle);
+err_free_rps1:
+	dma_free_coherent(&pci->dev, SAA7146_RPS_MEM, dev->d_rps1.cpu_addr,
+			  dev->d_rps1.dma_handle);
+err_free_rps0:
+	dma_free_coherent(&pci->dev, SAA7146_RPS_MEM, dev->d_rps0.cpu_addr,
+			  dev->d_rps0.dma_handle);
+err_free_irq:
+	free_irq(pci->irq, (void *)dev);
+err_unmap:
+	iounmap(dev->mem);
+err_release:
+	pci_release_region(pci, 0);
+err_disable:
+	pci_disable_device(pci);
+err_free:
+	kfree(dev);
+	goto out;
+}
+
+static void saa7146_remove_one(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct saa7146_dev *dev = to_saa7146_dev(v4l2_dev);
+	struct {
+		void *addr;
+		dma_addr_t dma;
+	} dev_map[] = {
+		{ dev->d_i2c.cpu_addr, dev->d_i2c.dma_handle },
+		{ dev->d_rps1.cpu_addr, dev->d_rps1.dma_handle },
+		{ dev->d_rps0.cpu_addr, dev->d_rps0.dma_handle },
+		{ NULL, 0 }
+	}, *p;
+
+	DEB_EE("dev:%p\n", dev);
+
+	dev->ext->detach(dev);
+
+	/* shut down all video dma transfers */
+	saa7146_write(dev, MC1, 0x00ff0000);
+
+	/* disable all irqs, release irq-routine */
+	saa7146_write(dev, IER, 0);
+
+	free_irq(pdev->irq, dev);
+
+	for (p = dev_map; p->addr; p++)
+		dma_free_coherent(&pdev->dev, SAA7146_RPS_MEM, p->addr,
+				  p->dma);
+
+	iounmap(dev->mem);
+	pci_release_region(pdev, 0);
+	pci_disable_device(pdev);
+	kfree(dev);
+
+	saa7146_num--;
+}
+
+/*********************************************************************************/
+/* extension handling functions                                                  */
+
+int saa7146_register_extension(struct saa7146_extension* ext)
+{
+	DEB_EE("ext:%p\n", ext);
+
+	ext->driver.name = ext->name;
+	ext->driver.id_table = ext->pci_tbl;
+	ext->driver.probe = saa7146_init_one;
+	ext->driver.remove = saa7146_remove_one;
+
+	pr_info("register extension '%s'\n", ext->name);
+	return pci_register_driver(&ext->driver);
+}
+
+int saa7146_unregister_extension(struct saa7146_extension* ext)
+{
+	DEB_EE("ext:%p\n", ext);
+	pr_info("unregister extension '%s'\n", ext->name);
+	pci_unregister_driver(&ext->driver);
+	return 0;
+}
+
+EXPORT_SYMBOL_GPL(saa7146_register_extension);
+EXPORT_SYMBOL_GPL(saa7146_unregister_extension);
+
+/* misc functions used by extension modules */
+EXPORT_SYMBOL_GPL(saa7146_pgtable_alloc);
+EXPORT_SYMBOL_GPL(saa7146_pgtable_free);
+EXPORT_SYMBOL_GPL(saa7146_pgtable_build_single);
+EXPORT_SYMBOL_GPL(saa7146_vmalloc_build_pgtable);
+EXPORT_SYMBOL_GPL(saa7146_vfree_destroy_pgtable);
+EXPORT_SYMBOL_GPL(saa7146_wait_for_debi_done);
+
+EXPORT_SYMBOL_GPL(saa7146_setgpio);
+
+EXPORT_SYMBOL_GPL(saa7146_i2c_adapter_prepare);
+
+EXPORT_SYMBOL_GPL(saa7146_debug);
+
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_DESCRIPTION("driver for generic saa7146-based hardware");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_fops.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_fops.c
new file mode 100644
index 000000000..aa14698a9
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_fops.c
@@ -0,0 +1,658 @@
+// SPDX-License-Identifier: GPL-2.0-only
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include "saa7146_vv.h"
+
+/****************************************************************************/
+/* resource management functions, shamelessly stolen from saa7134 driver */
+
+int saa7146_res_get(struct saa7146_fh *fh, unsigned int bit)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	if (fh->resources & bit) {
+		DEB_D("already allocated! want: 0x%02x, cur:0x%02x\n",
+		      bit, vv->resources);
+		/* have it already allocated */
+		return 1;
+	}
+
+	/* is it free? */
+	if (vv->resources & bit) {
+		DEB_D("locked! vv->resources:0x%02x, we want:0x%02x\n",
+		      vv->resources, bit);
+		/* no, someone else uses it */
+		return 0;
+	}
+	/* it's free, grab it */
+	fh->resources |= bit;
+	vv->resources |= bit;
+	DEB_D("res: get 0x%02x, cur:0x%02x\n", bit, vv->resources);
+	return 1;
+}
+
+void saa7146_res_free(struct saa7146_fh *fh, unsigned int bits)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	BUG_ON((fh->resources & bits) != bits);
+
+	fh->resources &= ~bits;
+	vv->resources &= ~bits;
+	DEB_D("res: put 0x%02x, cur:0x%02x\n", bits, vv->resources);
+}
+
+
+/********************************************************************************/
+/* common dma functions */
+
+void saa7146_dma_free(struct saa7146_dev *dev,struct videobuf_queue *q,
+						struct saa7146_buf *buf)
+{
+	struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
+	DEB_EE("dev:%p, buf:%p\n", dev, buf);
+
+	videobuf_waiton(q, &buf->vb, 0, 0);
+	videobuf_dma_unmap(q->dev, dma);
+	videobuf_dma_free(dma);
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+
+/********************************************************************************/
+/* common buffer functions */
+
+int saa7146_buffer_queue(struct saa7146_dev *dev,
+			 struct saa7146_dmaqueue *q,
+			 struct saa7146_buf *buf)
+{
+	assert_spin_locked(&dev->slock);
+	DEB_EE("dev:%p, dmaq:%p, buf:%p\n", dev, q, buf);
+
+	BUG_ON(!q);
+
+	if (NULL == q->curr) {
+		q->curr = buf;
+		DEB_D("immediately activating buffer %p\n", buf);
+		buf->activate(dev,buf,NULL);
+	} else {
+		list_add_tail(&buf->vb.queue,&q->queue);
+		buf->vb.state = VIDEOBUF_QUEUED;
+		DEB_D("adding buffer %p to queue. (active buffer present)\n",
+		      buf);
+	}
+	return 0;
+}
+
+void saa7146_buffer_finish(struct saa7146_dev *dev,
+			   struct saa7146_dmaqueue *q,
+			   int state)
+{
+	assert_spin_locked(&dev->slock);
+	DEB_EE("dev:%p, dmaq:%p, state:%d\n", dev, q, state);
+	DEB_EE("q->curr:%p\n", q->curr);
+
+	/* finish current buffer */
+	if (NULL == q->curr) {
+		DEB_D("aiii. no current buffer\n");
+		return;
+	}
+
+	q->curr->vb.state = state;
+	q->curr->vb.ts = ktime_get_ns();
+	wake_up(&q->curr->vb.done);
+
+	q->curr = NULL;
+}
+
+void saa7146_buffer_next(struct saa7146_dev *dev,
+			 struct saa7146_dmaqueue *q, int vbi)
+{
+	struct saa7146_buf *buf,*next = NULL;
+
+	BUG_ON(!q);
+
+	DEB_INT("dev:%p, dmaq:%p, vbi:%d\n", dev, q, vbi);
+
+	assert_spin_locked(&dev->slock);
+	if (!list_empty(&q->queue)) {
+		/* activate next one from queue */
+		buf = list_entry(q->queue.next,struct saa7146_buf,vb.queue);
+		list_del(&buf->vb.queue);
+		if (!list_empty(&q->queue))
+			next = list_entry(q->queue.next,struct saa7146_buf, vb.queue);
+		q->curr = buf;
+		DEB_INT("next buffer: buf:%p, prev:%p, next:%p\n",
+			buf, q->queue.prev, q->queue.next);
+		buf->activate(dev,buf,next);
+	} else {
+		DEB_INT("no next buffer. stopping.\n");
+		if( 0 != vbi ) {
+			/* turn off video-dma3 */
+			saa7146_write(dev,MC1, MASK_20);
+		} else {
+			/* nothing to do -- just prevent next video-dma1 transfer
+			   by lowering the protection address */
+
+			// fixme: fix this for vflip != 0
+
+			saa7146_write(dev, PROT_ADDR1, 0);
+			saa7146_write(dev, MC2, (MASK_02|MASK_18));
+
+			/* write the address of the rps-program */
+			saa7146_write(dev, RPS_ADDR0, dev->d_rps0.dma_handle);
+			/* turn on rps */
+			saa7146_write(dev, MC1, (MASK_12 | MASK_28));
+
+/*
+			printk("vdma%d.base_even:     0x%08x\n", 1,saa7146_read(dev,BASE_EVEN1));
+			printk("vdma%d.base_odd:      0x%08x\n", 1,saa7146_read(dev,BASE_ODD1));
+			printk("vdma%d.prot_addr:     0x%08x\n", 1,saa7146_read(dev,PROT_ADDR1));
+			printk("vdma%d.base_page:     0x%08x\n", 1,saa7146_read(dev,BASE_PAGE1));
+			printk("vdma%d.pitch:         0x%08x\n", 1,saa7146_read(dev,PITCH1));
+			printk("vdma%d.num_line_byte: 0x%08x\n", 1,saa7146_read(dev,NUM_LINE_BYTE1));
+*/
+		}
+		del_timer(&q->timeout);
+	}
+}
+
+void saa7146_buffer_timeout(struct timer_list *t)
+{
+	struct saa7146_dmaqueue *q = from_timer(q, t, timeout);
+	struct saa7146_dev *dev = q->dev;
+	unsigned long flags;
+
+	DEB_EE("dev:%p, dmaq:%p\n", dev, q);
+
+	spin_lock_irqsave(&dev->slock,flags);
+	if (q->curr) {
+		DEB_D("timeout on %p\n", q->curr);
+		saa7146_buffer_finish(dev,q,VIDEOBUF_ERROR);
+	}
+
+	/* we don't restart the transfer here like other drivers do. when
+	   a streaming capture is disabled, the timeout function will be
+	   called for the current buffer. if we activate the next buffer now,
+	   we mess up our capture logic. if a timeout occurs on another buffer,
+	   then something is seriously broken before, so no need to buffer the
+	   next capture IMHO... */
+/*
+	saa7146_buffer_next(dev,q);
+*/
+	spin_unlock_irqrestore(&dev->slock,flags);
+}
+
+/********************************************************************************/
+/* file operations */
+
+static int fops_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_dev *dev = video_drvdata(file);
+	struct saa7146_fh *fh = NULL;
+	int result = 0;
+
+	DEB_EE("file:%p, dev:%s\n", file, video_device_node_name(vdev));
+
+	if (mutex_lock_interruptible(vdev->lock))
+		return -ERESTARTSYS;
+
+	DEB_D("using: %p\n", dev);
+
+	/* check if an extension is registered */
+	if( NULL == dev->ext ) {
+		DEB_S("no extension registered for this device\n");
+		result = -ENODEV;
+		goto out;
+	}
+
+	/* allocate per open data */
+	fh = kzalloc(sizeof(*fh),GFP_KERNEL);
+	if (NULL == fh) {
+		DEB_S("cannot allocate memory for per open data\n");
+		result = -ENOMEM;
+		goto out;
+	}
+
+	v4l2_fh_init(&fh->fh, vdev);
+
+	file->private_data = &fh->fh;
+	fh->dev = dev;
+
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
+		DEB_S("initializing vbi...\n");
+		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
+			result = saa7146_vbi_uops.open(dev,file);
+		if (dev->ext_vv_data->vbi_fops.open)
+			dev->ext_vv_data->vbi_fops.open(file);
+	} else {
+		DEB_S("initializing video...\n");
+		result = saa7146_video_uops.open(dev,file);
+	}
+
+	if (0 != result) {
+		goto out;
+	}
+
+	if( 0 == try_module_get(dev->ext->module)) {
+		result = -EINVAL;
+		goto out;
+	}
+
+	result = 0;
+	v4l2_fh_add(&fh->fh);
+out:
+	if (fh && result != 0) {
+		kfree(fh);
+		file->private_data = NULL;
+	}
+	mutex_unlock(vdev->lock);
+	return result;
+}
+
+static int fops_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_fh  *fh  = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+
+	DEB_EE("file:%p\n", file);
+
+	mutex_lock(vdev->lock);
+
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
+		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
+			saa7146_vbi_uops.release(dev,file);
+		if (dev->ext_vv_data->vbi_fops.release)
+			dev->ext_vv_data->vbi_fops.release(file);
+	} else {
+		saa7146_video_uops.release(dev,file);
+	}
+
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	module_put(dev->ext->module);
+	file->private_data = NULL;
+	kfree(fh);
+
+	mutex_unlock(vdev->lock);
+
+	return 0;
+}
+
+static int fops_mmap(struct file *file, struct vm_area_struct * vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_fh *fh = file->private_data;
+	struct videobuf_queue *q;
+	int res;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_VIDEO: {
+		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, vma:%p\n",
+		       file, vma);
+		q = &fh->video_q;
+		break;
+		}
+	case VFL_TYPE_VBI: {
+		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, vma:%p\n",
+		       file, vma);
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return -ENODEV;
+		q = &fh->vbi_q;
+		break;
+		}
+	default:
+		BUG();
+	}
+
+	if (mutex_lock_interruptible(vdev->lock))
+		return -ERESTARTSYS;
+	res = videobuf_mmap_mapper(q, vma);
+	mutex_unlock(vdev->lock);
+	return res;
+}
+
+static __poll_t __fops_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_fh *fh = file->private_data;
+	struct videobuf_buffer *buf = NULL;
+	struct videobuf_queue *q;
+	__poll_t res = v4l2_ctrl_poll(file, wait);
+
+	DEB_EE("file:%p, poll:%p\n", file, wait);
+
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return res | EPOLLOUT | EPOLLWRNORM;
+		if( 0 == fh->vbi_q.streaming )
+			return res | videobuf_poll_stream(file, &fh->vbi_q, wait);
+		q = &fh->vbi_q;
+	} else {
+		DEB_D("using video queue\n");
+		q = &fh->video_q;
+	}
+
+	if (!list_empty(&q->stream))
+		buf = list_entry(q->stream.next, struct videobuf_buffer, stream);
+
+	if (!buf) {
+		DEB_D("buf == NULL!\n");
+		return res | EPOLLERR;
+	}
+
+	poll_wait(file, &buf->done, wait);
+	if (buf->state == VIDEOBUF_DONE || buf->state == VIDEOBUF_ERROR) {
+		DEB_D("poll succeeded!\n");
+		return res | EPOLLIN | EPOLLRDNORM;
+	}
+
+	DEB_D("nothing to poll for, buf->state:%d\n", buf->state);
+	return res;
+}
+
+static __poll_t fops_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+	__poll_t res;
+
+	mutex_lock(vdev->lock);
+	res = __fops_poll(file, wait);
+	mutex_unlock(vdev->lock);
+	return res;
+}
+
+static ssize_t fops_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_fh *fh = file->private_data;
+	int ret;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_VIDEO:
+/*
+		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, data:%p, count:%lun",
+		       file, data, (unsigned long)count);
+*/
+		return saa7146_video_uops.read(file,data,count,ppos);
+	case VFL_TYPE_VBI:
+/*
+		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, data:%p, count:%lu\n",
+		       file, data, (unsigned long)count);
+*/
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE) {
+			if (mutex_lock_interruptible(vdev->lock))
+				return -ERESTARTSYS;
+			ret = saa7146_vbi_uops.read(file, data, count, ppos);
+			mutex_unlock(vdev->lock);
+			return ret;
+		}
+		return -EINVAL;
+	default:
+		BUG();
+	}
+}
+
+static ssize_t fops_write(struct file *file, const char __user *data, size_t count, loff_t *ppos)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct saa7146_fh *fh = file->private_data;
+	int ret;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_VIDEO:
+		return -EINVAL;
+	case VFL_TYPE_VBI:
+		if (fh->dev->ext_vv_data->vbi_fops.write) {
+			if (mutex_lock_interruptible(vdev->lock))
+				return -ERESTARTSYS;
+			ret = fh->dev->ext_vv_data->vbi_fops.write(file, data, count, ppos);
+			mutex_unlock(vdev->lock);
+			return ret;
+		}
+		return -EINVAL;
+	default:
+		BUG();
+	}
+}
+
+static const struct v4l2_file_operations video_fops =
+{
+	.owner		= THIS_MODULE,
+	.open		= fops_open,
+	.release	= fops_release,
+	.read		= fops_read,
+	.write		= fops_write,
+	.poll		= fops_poll,
+	.mmap		= fops_mmap,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static void vv_callback(struct saa7146_dev *dev, unsigned long status)
+{
+	u32 isr = status;
+
+	DEB_INT("dev:%p, isr:0x%08x\n", dev, (u32)status);
+
+	if (0 != (isr & (MASK_27))) {
+		DEB_INT("irq: RPS0 (0x%08x)\n", isr);
+		saa7146_video_uops.irq_done(dev,isr);
+	}
+
+	if (0 != (isr & (MASK_28))) {
+		u32 mc2 = saa7146_read(dev, MC2);
+		if( 0 != (mc2 & MASK_15)) {
+			DEB_INT("irq: RPS1 vbi workaround (0x%08x)\n", isr);
+			wake_up(&dev->vv_data->vbi_wq);
+			saa7146_write(dev,MC2, MASK_31);
+			return;
+		}
+		DEB_INT("irq: RPS1 (0x%08x)\n", isr);
+		saa7146_vbi_uops.irq_done(dev,isr);
+	}
+}
+
+static const struct v4l2_ctrl_ops saa7146_ctrl_ops = {
+	.s_ctrl = saa7146_s_ctrl,
+};
+
+int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
+{
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct v4l2_pix_format *fmt;
+	struct v4l2_vbi_format *vbi;
+	struct saa7146_vv *vv;
+	int err;
+
+	err = v4l2_device_register(&dev->pci->dev, &dev->v4l2_dev);
+	if (err)
+		return err;
+
+	v4l2_ctrl_handler_init(hdl, 6);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (hdl->error) {
+		err = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		v4l2_device_unregister(&dev->v4l2_dev);
+		return err;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+
+	vv = kzalloc(sizeof(struct saa7146_vv), GFP_KERNEL);
+	if (vv == NULL) {
+		ERR("out of memory. aborting.\n");
+		v4l2_ctrl_handler_free(hdl);
+		v4l2_device_unregister(&dev->v4l2_dev);
+		return -ENOMEM;
+	}
+	ext_vv->vid_ops = saa7146_video_ioctl_ops;
+	ext_vv->vbi_ops = saa7146_vbi_ioctl_ops;
+	ext_vv->core_ops = &saa7146_video_ioctl_ops;
+
+	DEB_EE("dev:%p\n", dev);
+
+	/* set default values for video parts of the saa7146 */
+	saa7146_write(dev, BCS_CTRL, 0x80400040);
+
+	/* enable video-port pins */
+	saa7146_write(dev, MC1, (MASK_10 | MASK_26));
+
+	/* save per-device extension data (one extension can
+	   handle different devices that might need different
+	   configuration data) */
+	dev->ext_vv_data = ext_vv;
+
+	vv->d_clipping.cpu_addr =
+		dma_alloc_coherent(&dev->pci->dev, SAA7146_CLIPPING_MEM,
+				   &vv->d_clipping.dma_handle, GFP_KERNEL);
+	if( NULL == vv->d_clipping.cpu_addr ) {
+		ERR("out of memory. aborting.\n");
+		kfree(vv);
+		v4l2_ctrl_handler_free(hdl);
+		v4l2_device_unregister(&dev->v4l2_dev);
+		return -ENOMEM;
+	}
+
+	saa7146_video_uops.init(dev,vv);
+	if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
+		saa7146_vbi_uops.init(dev,vv);
+
+	vv->ov_fb.fmt.width = vv->standard->h_max_out;
+	vv->ov_fb.fmt.height = vv->standard->v_max_out;
+	vv->ov_fb.fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+	vv->ov_fb.fmt.bytesperline = 2 * vv->ov_fb.fmt.width;
+	vv->ov_fb.fmt.sizeimage = vv->ov_fb.fmt.bytesperline * vv->ov_fb.fmt.height;
+	vv->ov_fb.fmt.colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt = &vv->video_fmt;
+	fmt->width = 384;
+	fmt->height = 288;
+	fmt->pixelformat = V4L2_PIX_FMT_BGR24;
+	fmt->field = V4L2_FIELD_ANY;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	fmt->bytesperline = 3 * fmt->width;
+	fmt->sizeimage = fmt->bytesperline * fmt->height;
+
+	vbi = &vv->vbi_fmt;
+	vbi->sampling_rate	= 27000000;
+	vbi->offset		= 248; /* todo */
+	vbi->samples_per_line	= 720 * 2;
+	vbi->sample_format	= V4L2_PIX_FMT_GREY;
+
+	/* fixme: this only works for PAL */
+	vbi->start[0] = 5;
+	vbi->count[0] = 16;
+	vbi->start[1] = 312;
+	vbi->count[1] = 16;
+
+	timer_setup(&vv->vbi_read_timeout, NULL, 0);
+
+	vv->ov_fb.capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	vv->ov_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
+	dev->vv_data = vv;
+	dev->vv_callback = &vv_callback;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_vv_init);
+
+int saa7146_vv_release(struct saa7146_dev* dev)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+
+	DEB_EE("dev:%p\n", dev);
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+	dma_free_coherent(&dev->pci->dev, SAA7146_CLIPPING_MEM,
+			  vv->d_clipping.cpu_addr, vv->d_clipping.dma_handle);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(vv);
+	dev->vv_data = NULL;
+	dev->vv_callback = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_vv_release);
+
+int saa7146_register_device(struct video_device *vfd, struct saa7146_dev *dev,
+			    char *name, int type)
+{
+	int err;
+	int i;
+
+	DEB_EE("dev:%p, name:'%s', type:%d\n", dev, name, type);
+
+	vfd->fops = &video_fops;
+	if (type == VFL_TYPE_VIDEO)
+		vfd->ioctl_ops = &dev->ext_vv_data->vid_ops;
+	else
+		vfd->ioctl_ops = &dev->ext_vv_data->vbi_ops;
+	vfd->release = video_device_release_empty;
+	vfd->lock = &dev->v4l2_lock;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->tvnorms = 0;
+	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
+		vfd->tvnorms |= dev->ext_vv_data->stds[i].id;
+	strscpy(vfd->name, name, sizeof(vfd->name));
+	vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
+			   V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	vfd->device_caps |= dev->ext_vv_data->capabilities;
+	if (type == VFL_TYPE_VIDEO)
+		vfd->device_caps &=
+			~(V4L2_CAP_VBI_CAPTURE | V4L2_CAP_SLICED_VBI_OUTPUT);
+	else
+		vfd->device_caps &=
+			~(V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_AUDIO);
+	video_set_drvdata(vfd, dev);
+
+	err = video_register_device(vfd, type, -1);
+	if (err < 0) {
+		ERR("cannot register v4l2 device. skipping.\n");
+		return err;
+	}
+
+	pr_info("%s: registered device %s [v4l2]\n",
+		dev->name, video_device_node_name(vfd));
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_register_device);
+
+int saa7146_unregister_device(struct video_device *vfd, struct saa7146_dev *dev)
+{
+	DEB_EE("dev:%p\n", dev);
+
+	video_unregister_device(vfd);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_unregister_device);
+
+static int __init saa7146_vv_init_module(void)
+{
+	return 0;
+}
+
+
+static void __exit saa7146_vv_cleanup_module(void)
+{
+}
+
+module_init(saa7146_vv_init_module);
+module_exit(saa7146_vv_cleanup_module);
+
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_DESCRIPTION("video4linux driver for saa7146-based hardware");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_hlp.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_hlp.c
new file mode 100644
index 000000000..b1222a4cf
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_hlp.c
@@ -0,0 +1,1046 @@
+// SPDX-License-Identifier: GPL-2.0-only
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/export.h>
+#include "saa7146_vv.h"
+
+static void calculate_output_format_register(struct saa7146_dev* saa, u32 palette, u32* clip_format)
+{
+	/* clear out the necessary bits */
+	*clip_format &= 0x0000ffff;
+	/* set these bits new */
+	*clip_format |=  (( ((palette&0xf00)>>8) << 30) | ((palette&0x00f) << 24) | (((palette&0x0f0)>>4) << 16));
+}
+
+static void calculate_hps_source_and_sync(struct saa7146_dev *dev, int source, int sync, u32* hps_ctrl)
+{
+	*hps_ctrl &= ~(MASK_30 | MASK_31 | MASK_28);
+	*hps_ctrl |= (source << 30) | (sync << 28);
+}
+
+static void calculate_hxo_and_hyo(struct saa7146_vv *vv, u32* hps_h_scale, u32* hps_ctrl)
+{
+	int hyo = 0, hxo = 0;
+
+	hyo = vv->standard->v_offset;
+	hxo = vv->standard->h_offset;
+
+	*hps_h_scale	&= ~(MASK_B0 | 0xf00);
+	*hps_h_scale	|= (hxo <<  0);
+
+	*hps_ctrl	&= ~(MASK_W0 | MASK_B2);
+	*hps_ctrl	|= (hyo << 12);
+}
+
+/* helper functions for the calculation of the horizontal- and vertical
+   scaling registers, clip-format-register etc ...
+   these functions take pointers to the (most-likely read-out
+   original-values) and manipulate them according to the requested
+   changes.
+*/
+
+/* hps_coeff used for CXY and CXUV; scale 1/1 -> scale 1/64 */
+static struct {
+	u16 hps_coeff;
+	u16 weight_sum;
+} hps_h_coeff_tab [] = {
+	{0x00,   2}, {0x02,   4}, {0x00,   4}, {0x06,   8}, {0x02,   8},
+	{0x08,   8}, {0x00,   8}, {0x1E,  16}, {0x0E,   8}, {0x26,   8},
+	{0x06,   8}, {0x42,   8}, {0x02,   8}, {0x80,   8}, {0x00,   8},
+	{0xFE,  16}, {0xFE,   8}, {0x7E,   8}, {0x7E,   8}, {0x3E,   8},
+	{0x3E,   8}, {0x1E,   8}, {0x1E,   8}, {0x0E,   8}, {0x0E,   8},
+	{0x06,   8}, {0x06,   8}, {0x02,   8}, {0x02,   8}, {0x00,   8},
+	{0x00,   8}, {0xFE,  16}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8},
+	{0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8},
+	{0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8},
+	{0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0xFE,   8}, {0x7E,   8},
+	{0x7E,   8}, {0x3E,   8}, {0x3E,   8}, {0x1E,   8}, {0x1E,   8},
+	{0x0E,   8}, {0x0E,   8}, {0x06,   8}, {0x06,   8}, {0x02,   8},
+	{0x02,   8}, {0x00,   8}, {0x00,   8}, {0xFE,  16}
+};
+
+/* table of attenuation values for horizontal scaling */
+static u8 h_attenuation[] = { 1, 2, 4, 8, 2, 4, 8, 16, 0};
+
+/* calculate horizontal scale registers */
+static int calculate_h_scale_registers(struct saa7146_dev *dev,
+	int in_x, int out_x, int flip_lr,
+	u32* hps_ctrl, u32* hps_v_gain, u32* hps_h_prescale, u32* hps_h_scale)
+{
+	/* horizontal prescaler */
+	u32 dcgx = 0, xpsc = 0, xacm = 0, cxy = 0, cxuv = 0;
+	/* horizontal scaler */
+	u32 xim = 0, xp = 0, xsci =0;
+	/* vertical scale & gain */
+	u32 pfuv = 0;
+
+	/* helper variables */
+	u32 h_atten = 0, i = 0;
+
+	if ( 0 == out_x ) {
+		return -EINVAL;
+	}
+
+	/* mask out vanity-bit */
+	*hps_ctrl &= ~MASK_29;
+
+	/* calculate prescale-(xspc)-value:	[n   .. 1/2) : 1
+						[1/2 .. 1/3) : 2
+						[1/3 .. 1/4) : 3
+						...		*/
+	if (in_x > out_x) {
+		xpsc = in_x / out_x;
+	}
+	else {
+		/* zooming */
+		xpsc = 1;
+	}
+
+	/* if flip_lr-bit is set, number of pixels after
+	   horizontal prescaling must be < 384 */
+	if ( 0 != flip_lr ) {
+
+		/* set vanity bit */
+		*hps_ctrl |= MASK_29;
+
+		while (in_x / xpsc >= 384 )
+			xpsc++;
+	}
+	/* if zooming is wanted, number of pixels after
+	   horizontal prescaling must be < 768 */
+	else {
+		while ( in_x / xpsc >= 768 )
+			xpsc++;
+	}
+
+	/* maximum prescale is 64 (p.69) */
+	if ( xpsc > 64 )
+		xpsc = 64;
+
+	/* keep xacm clear*/
+	xacm = 0;
+
+	/* set horizontal filter parameters (CXY = CXUV) */
+	cxy = hps_h_coeff_tab[( (xpsc - 1) < 63 ? (xpsc - 1) : 63 )].hps_coeff;
+	cxuv = cxy;
+
+	/* calculate and set horizontal fine scale (xsci) */
+
+	/* bypass the horizontal scaler ? */
+	if ( (in_x == out_x) && ( 1 == xpsc ) )
+		xsci = 0x400;
+	else
+		xsci = ( (1024 * in_x) / (out_x * xpsc) ) + xpsc;
+
+	/* set start phase for horizontal fine scale (xp) to 0 */
+	xp = 0;
+
+	/* set xim, if we bypass the horizontal scaler */
+	if ( 0x400 == xsci )
+		xim = 1;
+	else
+		xim = 0;
+
+	/* if the prescaler is bypassed, enable horizontal
+	   accumulation mode (xacm) and clear dcgx */
+	if( 1 == xpsc ) {
+		xacm = 1;
+		dcgx = 0;
+	} else {
+		xacm = 0;
+		/* get best match in the table of attenuations
+		   for horizontal scaling */
+		h_atten = hps_h_coeff_tab[( (xpsc - 1) < 63 ? (xpsc - 1) : 63 )].weight_sum;
+
+		for (i = 0; h_attenuation[i] != 0; i++) {
+			if (h_attenuation[i] >= h_atten)
+				break;
+		}
+
+		dcgx = i;
+	}
+
+	/* the horizontal scaling increment controls the UV filter
+	   to reduce the bandwidth to improve the display quality,
+	   so set it ... */
+	if ( xsci == 0x400)
+		pfuv = 0x00;
+	else if ( xsci < 0x600)
+		pfuv = 0x01;
+	else if ( xsci < 0x680)
+		pfuv = 0x11;
+	else if ( xsci < 0x700)
+		pfuv = 0x22;
+	else
+		pfuv = 0x33;
+
+
+	*hps_v_gain  &= MASK_W0|MASK_B2;
+	*hps_v_gain  |= (pfuv << 24);
+
+	*hps_h_scale	&= ~(MASK_W1 | 0xf000);
+	*hps_h_scale	|= (xim << 31) | (xp << 24) | (xsci << 12);
+
+	*hps_h_prescale	|= (dcgx << 27) | ((xpsc-1) << 18) | (xacm << 17) | (cxy << 8) | (cxuv << 0);
+
+	return 0;
+}
+
+static struct {
+	u16 hps_coeff;
+	u16 weight_sum;
+} hps_v_coeff_tab [] = {
+ {0x0100,   2},  {0x0102,   4},  {0x0300,   4},  {0x0106,   8},  {0x0502,   8},
+ {0x0708,   8},  {0x0F00,   8},  {0x011E,  16},  {0x110E,  16},  {0x1926,  16},
+ {0x3906,  16},  {0x3D42,  16},  {0x7D02,  16},  {0x7F80,  16},  {0xFF00,  16},
+ {0x01FE,  32},  {0x01FE,  32},  {0x817E,  32},  {0x817E,  32},  {0xC13E,  32},
+ {0xC13E,  32},  {0xE11E,  32},  {0xE11E,  32},  {0xF10E,  32},  {0xF10E,  32},
+ {0xF906,  32},  {0xF906,  32},  {0xFD02,  32},  {0xFD02,  32},  {0xFF00,  32},
+ {0xFF00,  32},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},
+ {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},
+ {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},
+ {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x01FE,  64},  {0x817E,  64},
+ {0x817E,  64},  {0xC13E,  64},  {0xC13E,  64},  {0xE11E,  64},  {0xE11E,  64},
+ {0xF10E,  64},  {0xF10E,  64},  {0xF906,  64},  {0xF906,  64},  {0xFD02,  64},
+ {0xFD02,  64},  {0xFF00,  64},  {0xFF00,  64},  {0x01FE, 128}
+};
+
+/* table of attenuation values for vertical scaling */
+static u16 v_attenuation[] = { 2, 4, 8, 16, 32, 64, 128, 256, 0};
+
+/* calculate vertical scale registers */
+static int calculate_v_scale_registers(struct saa7146_dev *dev, enum v4l2_field field,
+	int in_y, int out_y, u32* hps_v_scale, u32* hps_v_gain)
+{
+	int lpi = 0;
+
+	/* vertical scaling */
+	u32 yacm = 0, ysci = 0, yacl = 0, ypo = 0, ype = 0;
+	/* vertical scale & gain */
+	u32 dcgy = 0, cya_cyb = 0;
+
+	/* helper variables */
+	u32 v_atten = 0, i = 0;
+
+	/* error, if vertical zooming */
+	if ( in_y < out_y ) {
+		return -EINVAL;
+	}
+
+	/* linear phase interpolation may be used
+	   if scaling is between 1 and 1/2 (both fields used)
+	   or scaling is between 1/2 and 1/4 (if only one field is used) */
+
+	if (V4L2_FIELD_HAS_BOTH(field)) {
+		if( 2*out_y >= in_y) {
+			lpi = 1;
+		}
+	} else if (field == V4L2_FIELD_TOP
+		|| field == V4L2_FIELD_ALTERNATE
+		|| field == V4L2_FIELD_BOTTOM) {
+		if( 4*out_y >= in_y ) {
+			lpi = 1;
+		}
+		out_y *= 2;
+	}
+	if( 0 != lpi ) {
+
+		yacm = 0;
+		yacl = 0;
+		cya_cyb = 0x00ff;
+
+		/* calculate scaling increment */
+		if ( in_y > out_y )
+			ysci = ((1024 * in_y) / (out_y + 1)) - 1024;
+		else
+			ysci = 0;
+
+		dcgy = 0;
+
+		/* calculate ype and ypo */
+		ype = ysci / 16;
+		ypo = ype + (ysci / 64);
+
+	} else {
+		yacm = 1;
+
+		/* calculate scaling increment */
+		ysci = (((10 * 1024 * (in_y - out_y - 1)) / in_y) + 9) / 10;
+
+		/* calculate ype and ypo */
+		ypo = ype = ((ysci + 15) / 16);
+
+		/* the sequence length interval (yacl) has to be set according
+		   to the prescale value, e.g.	[n   .. 1/2) : 0
+						[1/2 .. 1/3) : 1
+						[1/3 .. 1/4) : 2
+						... */
+		if ( ysci < 512) {
+			yacl = 0;
+		} else {
+			yacl = ( ysci / (1024 - ysci) );
+		}
+
+		/* get filter coefficients for cya, cyb from table hps_v_coeff_tab */
+		cya_cyb = hps_v_coeff_tab[ (yacl < 63 ? yacl : 63 ) ].hps_coeff;
+
+		/* get best match in the table of attenuations for vertical scaling */
+		v_atten = hps_v_coeff_tab[ (yacl < 63 ? yacl : 63 ) ].weight_sum;
+
+		for (i = 0; v_attenuation[i] != 0; i++) {
+			if (v_attenuation[i] >= v_atten)
+				break;
+		}
+
+		dcgy = i;
+	}
+
+	/* ypo and ype swapped in spec ? */
+	*hps_v_scale	|= (yacm << 31) | (ysci << 21) | (yacl << 15) | (ypo << 8 ) | (ype << 1);
+
+	*hps_v_gain	&= ~(MASK_W0|MASK_B2);
+	*hps_v_gain	|= (dcgy << 16) | (cya_cyb << 0);
+
+	return 0;
+}
+
+/* simple bubble-sort algorithm with duplicate elimination */
+static int sort_and_eliminate(u32* values, int* count)
+{
+	int low = 0, high = 0, top = 0;
+	int cur = 0, next = 0;
+
+	/* sanity checks */
+	if( (0 > *count) || (NULL == values) ) {
+		return -EINVAL;
+	}
+
+	/* bubble sort the first @count items of the array @values */
+	for( top = *count; top > 0; top--) {
+		for( low = 0, high = 1; high < top; low++, high++) {
+			if( values[low] > values[high] )
+				swap(values[low], values[high]);
+		}
+	}
+
+	/* remove duplicate items */
+	for( cur = 0, next = 1; next < *count; next++) {
+		if( values[cur] != values[next])
+			values[++cur] = values[next];
+	}
+
+	*count = cur + 1;
+
+	return 0;
+}
+
+static void calculate_clipping_registers_rect(struct saa7146_dev *dev, struct saa7146_fh *fh,
+	struct saa7146_video_dma *vdma2, u32* clip_format, u32* arbtr_ctrl, enum v4l2_field field)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	__le32 *clipping = vv->d_clipping.cpu_addr;
+
+	int width = vv->ov.win.w.width;
+	int height =  vv->ov.win.w.height;
+	int clipcount = vv->ov.nclips;
+
+	u32 line_list[32];
+	u32 pixel_list[32];
+	int numdwords = 0;
+
+	int i = 0, j = 0;
+	int cnt_line = 0, cnt_pixel = 0;
+
+	int x[32], y[32], w[32], h[32];
+
+	/* clear out memory */
+	memset(&line_list[0],  0x00, sizeof(u32)*32);
+	memset(&pixel_list[0], 0x00, sizeof(u32)*32);
+	memset(clipping,  0x00, SAA7146_CLIPPING_MEM);
+
+	/* fill the line and pixel-lists */
+	for(i = 0; i < clipcount; i++) {
+		int l = 0, r = 0, t = 0, b = 0;
+
+		x[i] = vv->ov.clips[i].c.left;
+		y[i] = vv->ov.clips[i].c.top;
+		w[i] = vv->ov.clips[i].c.width;
+		h[i] = vv->ov.clips[i].c.height;
+
+		if( w[i] < 0) {
+			x[i] += w[i]; w[i] = -w[i];
+		}
+		if( h[i] < 0) {
+			y[i] += h[i]; h[i] = -h[i];
+		}
+		if( x[i] < 0) {
+			w[i] += x[i]; x[i] = 0;
+		}
+		if( y[i] < 0) {
+			h[i] += y[i]; y[i] = 0;
+		}
+		if( 0 != vv->vflip ) {
+			y[i] = height - y[i] - h[i];
+		}
+
+		l = x[i];
+		r = x[i]+w[i];
+		t = y[i];
+		b = y[i]+h[i];
+
+		/* insert left/right coordinates */
+		pixel_list[ 2*i   ] = min_t(int, l, width);
+		pixel_list[(2*i)+1] = min_t(int, r, width);
+		/* insert top/bottom coordinates */
+		line_list[ 2*i   ] = min_t(int, t, height);
+		line_list[(2*i)+1] = min_t(int, b, height);
+	}
+
+	/* sort and eliminate lists */
+	cnt_line = cnt_pixel = 2*clipcount;
+	sort_and_eliminate( &pixel_list[0], &cnt_pixel );
+	sort_and_eliminate( &line_list[0], &cnt_line );
+
+	/* calculate the number of used u32s */
+	numdwords = max_t(int, (cnt_line+1), (cnt_pixel+1))*2;
+	numdwords = max_t(int, 4, numdwords);
+	numdwords = min_t(int, 64, numdwords);
+
+	/* fill up cliptable */
+	for(i = 0; i < cnt_pixel; i++) {
+		clipping[2*i] |= cpu_to_le32(pixel_list[i] << 16);
+	}
+	for(i = 0; i < cnt_line; i++) {
+		clipping[(2*i)+1] |= cpu_to_le32(line_list[i] << 16);
+	}
+
+	/* fill up cliptable with the display infos */
+	for(j = 0; j < clipcount; j++) {
+
+		for(i = 0; i < cnt_pixel; i++) {
+
+			if( x[j] < 0)
+				x[j] = 0;
+
+			if( pixel_list[i] < (x[j] + w[j])) {
+
+				if ( pixel_list[i] >= x[j] ) {
+					clipping[2*i] |= cpu_to_le32(1 << j);
+				}
+			}
+		}
+		for(i = 0; i < cnt_line; i++) {
+
+			if( y[j] < 0)
+				y[j] = 0;
+
+			if( line_list[i] < (y[j] + h[j]) ) {
+
+				if( line_list[i] >= y[j] ) {
+					clipping[(2*i)+1] |= cpu_to_le32(1 << j);
+				}
+			}
+		}
+	}
+
+	/* adjust arbitration control register */
+	*arbtr_ctrl &= 0xffff00ff;
+	*arbtr_ctrl |= 0x00001c00;
+
+	vdma2->base_even	= vv->d_clipping.dma_handle;
+	vdma2->base_odd		= vv->d_clipping.dma_handle;
+	vdma2->prot_addr	= vv->d_clipping.dma_handle+((sizeof(u32))*(numdwords));
+	vdma2->base_page	= 0x04;
+	vdma2->pitch		= 0x00;
+	vdma2->num_line_byte	= (0 << 16 | (sizeof(u32))*(numdwords-1) );
+
+	/* set clipping-mode. this depends on the field(s) used */
+	*clip_format &= 0xfffffff7;
+	if (V4L2_FIELD_HAS_BOTH(field)) {
+		*clip_format |= 0x00000008;
+	} else {
+		*clip_format |= 0x00000000;
+	}
+}
+
+/* disable clipping */
+static void saa7146_disable_clipping(struct saa7146_dev *dev)
+{
+	u32 clip_format	= saa7146_read(dev, CLIP_FORMAT_CTRL);
+
+	/* mask out relevant bits (=lower word)*/
+	clip_format &= MASK_W1;
+
+	/* upload clipping-registers*/
+	saa7146_write(dev, CLIP_FORMAT_CTRL,clip_format);
+	saa7146_write(dev, MC2, (MASK_05 | MASK_21));
+
+	/* disable video dma2 */
+	saa7146_write(dev, MC1, MASK_21);
+}
+
+static void saa7146_set_clipping_rect(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	enum v4l2_field field = vv->ov.win.field;
+	struct	saa7146_video_dma vdma2;
+	u32 clip_format;
+	u32 arbtr_ctrl;
+
+	/* check clipcount, disable clipping if clipcount == 0*/
+	if (vv->ov.nclips == 0) {
+		saa7146_disable_clipping(dev);
+		return;
+	}
+
+	clip_format = saa7146_read(dev, CLIP_FORMAT_CTRL);
+	arbtr_ctrl = saa7146_read(dev, PCI_BT_V1);
+
+	calculate_clipping_registers_rect(dev, fh, &vdma2, &clip_format, &arbtr_ctrl, field);
+
+	/* set clipping format */
+	clip_format &= 0xffff0008;
+	clip_format |= (SAA7146_CLIPPING_RECT << 4);
+
+	/* prepare video dma2 */
+	saa7146_write(dev, BASE_EVEN2,		vdma2.base_even);
+	saa7146_write(dev, BASE_ODD2,		vdma2.base_odd);
+	saa7146_write(dev, PROT_ADDR2,		vdma2.prot_addr);
+	saa7146_write(dev, BASE_PAGE2,		vdma2.base_page);
+	saa7146_write(dev, PITCH2,		vdma2.pitch);
+	saa7146_write(dev, NUM_LINE_BYTE2,	vdma2.num_line_byte);
+
+	/* prepare the rest */
+	saa7146_write(dev, CLIP_FORMAT_CTRL,clip_format);
+	saa7146_write(dev, PCI_BT_V1, arbtr_ctrl);
+
+	/* upload clip_control-register, clipping-registers, enable video dma2 */
+	saa7146_write(dev, MC2, (MASK_05 | MASK_21 | MASK_03 | MASK_19));
+	saa7146_write(dev, MC1, (MASK_05 | MASK_21));
+}
+
+static void saa7146_set_window(struct saa7146_dev *dev, int width, int height, enum v4l2_field field)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+
+	int source = vv->current_hps_source;
+	int sync = vv->current_hps_sync;
+
+	u32 hps_v_scale = 0, hps_v_gain  = 0, hps_ctrl = 0, hps_h_prescale = 0, hps_h_scale = 0;
+
+	/* set vertical scale */
+	hps_v_scale = 0; /* all bits get set by the function-call */
+	hps_v_gain  = 0; /* fixme: saa7146_read(dev, HPS_V_GAIN);*/
+	calculate_v_scale_registers(dev, field, vv->standard->v_field*2, height, &hps_v_scale, &hps_v_gain);
+
+	/* set horizontal scale */
+	hps_ctrl	= 0;
+	hps_h_prescale	= 0; /* all bits get set in the function */
+	hps_h_scale	= 0;
+	calculate_h_scale_registers(dev, vv->standard->h_pixels, width, vv->hflip, &hps_ctrl, &hps_v_gain, &hps_h_prescale, &hps_h_scale);
+
+	/* set hyo and hxo */
+	calculate_hxo_and_hyo(vv, &hps_h_scale, &hps_ctrl);
+	calculate_hps_source_and_sync(dev, source, sync, &hps_ctrl);
+
+	/* write out new register contents */
+	saa7146_write(dev, HPS_V_SCALE,	hps_v_scale);
+	saa7146_write(dev, HPS_V_GAIN,	hps_v_gain);
+	saa7146_write(dev, HPS_CTRL,	hps_ctrl);
+	saa7146_write(dev, HPS_H_PRESCALE,hps_h_prescale);
+	saa7146_write(dev, HPS_H_SCALE,	hps_h_scale);
+
+	/* upload shadow-ram registers */
+	saa7146_write(dev, MC2, (MASK_05 | MASK_06 | MASK_21 | MASK_22) );
+}
+
+/* calculate the new memory offsets for a desired position */
+static void saa7146_set_position(struct saa7146_dev *dev, int w_x, int w_y, int w_height, enum v4l2_field field, u32 pixelformat)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_format *sfmt = saa7146_format_by_fourcc(dev, pixelformat);
+
+	int b_depth = vv->ov_fmt->depth;
+	int b_bpl = vv->ov_fb.fmt.bytesperline;
+	/* The unsigned long cast is to remove a 64-bit compile warning since
+	   it looks like a 64-bit address is cast to a 32-bit value, even
+	   though the base pointer is really a 32-bit physical address that
+	   goes into a 32-bit DMA register.
+	   FIXME: might not work on some 64-bit platforms, but see the FIXME
+	   in struct v4l2_framebuffer (videodev2.h) for that.
+	 */
+	u32 base = (u32)(unsigned long)vv->ov_fb.base;
+
+	struct	saa7146_video_dma vdma1;
+
+	/* calculate memory offsets for picture, look if we shall top-down-flip */
+	vdma1.pitch	= 2*b_bpl;
+	if ( 0 == vv->vflip ) {
+		vdma1.base_even = base + (w_y * (vdma1.pitch/2)) + (w_x * (b_depth / 8));
+		vdma1.base_odd  = vdma1.base_even + (vdma1.pitch / 2);
+		vdma1.prot_addr = vdma1.base_even + (w_height * (vdma1.pitch / 2));
+	}
+	else {
+		vdma1.base_even = base + ((w_y+w_height) * (vdma1.pitch/2)) + (w_x * (b_depth / 8));
+		vdma1.base_odd  = vdma1.base_even - (vdma1.pitch / 2);
+		vdma1.prot_addr = vdma1.base_odd - (w_height * (vdma1.pitch / 2));
+	}
+
+	if (V4L2_FIELD_HAS_BOTH(field)) {
+	} else if (field == V4L2_FIELD_ALTERNATE) {
+		/* fixme */
+		vdma1.base_odd = vdma1.prot_addr;
+		vdma1.pitch /= 2;
+	} else if (field == V4L2_FIELD_TOP) {
+		vdma1.base_odd = vdma1.prot_addr;
+		vdma1.pitch /= 2;
+	} else if (field == V4L2_FIELD_BOTTOM) {
+		vdma1.base_odd = vdma1.base_even;
+		vdma1.base_even = vdma1.prot_addr;
+		vdma1.pitch /= 2;
+	}
+
+	if ( 0 != vv->vflip ) {
+		vdma1.pitch *= -1;
+	}
+
+	vdma1.base_page = sfmt->swap;
+	vdma1.num_line_byte = (vv->standard->v_field<<16)+vv->standard->h_pixels;
+
+	saa7146_write_out_dma(dev, 1, &vdma1);
+}
+
+static void saa7146_set_output_format(struct saa7146_dev *dev, unsigned long palette)
+{
+	u32 clip_format = saa7146_read(dev, CLIP_FORMAT_CTRL);
+
+	/* call helper function */
+	calculate_output_format_register(dev,palette,&clip_format);
+
+	/* update the hps registers */
+	saa7146_write(dev, CLIP_FORMAT_CTRL, clip_format);
+	saa7146_write(dev, MC2, (MASK_05 | MASK_21));
+}
+
+/* select input-source */
+void saa7146_set_hps_source_and_sync(struct saa7146_dev *dev, int source, int sync)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	u32 hps_ctrl = 0;
+
+	/* read old state */
+	hps_ctrl = saa7146_read(dev, HPS_CTRL);
+
+	hps_ctrl &= ~( MASK_31 | MASK_30 | MASK_28 );
+	hps_ctrl |= (source << 30) | (sync << 28);
+
+	/* write back & upload register */
+	saa7146_write(dev, HPS_CTRL, hps_ctrl);
+	saa7146_write(dev, MC2, (MASK_05 | MASK_21));
+
+	vv->current_hps_source = source;
+	vv->current_hps_sync = sync;
+}
+EXPORT_SYMBOL_GPL(saa7146_set_hps_source_and_sync);
+
+int saa7146_enable_overlay(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	saa7146_set_window(dev, vv->ov.win.w.width, vv->ov.win.w.height, vv->ov.win.field);
+	saa7146_set_position(dev, vv->ov.win.w.left, vv->ov.win.w.top, vv->ov.win.w.height, vv->ov.win.field, vv->ov_fmt->pixelformat);
+	saa7146_set_output_format(dev, vv->ov_fmt->trans);
+	saa7146_set_clipping_rect(fh);
+
+	/* enable video dma1 */
+	saa7146_write(dev, MC1, (MASK_06 | MASK_22));
+	return 0;
+}
+
+void saa7146_disable_overlay(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+
+	/* disable clipping + video dma1 */
+	saa7146_disable_clipping(dev);
+	saa7146_write(dev, MC1, MASK_22);
+}
+
+void saa7146_write_out_dma(struct saa7146_dev* dev, int which, struct saa7146_video_dma* vdma)
+{
+	int where = 0;
+
+	if( which < 1 || which > 3) {
+		return;
+	}
+
+	/* calculate starting address */
+	where  = (which-1)*0x18;
+
+	saa7146_write(dev, where,	vdma->base_odd);
+	saa7146_write(dev, where+0x04,	vdma->base_even);
+	saa7146_write(dev, where+0x08,	vdma->prot_addr);
+	saa7146_write(dev, where+0x0c,	vdma->pitch);
+	saa7146_write(dev, where+0x10,	vdma->base_page);
+	saa7146_write(dev, where+0x14,	vdma->num_line_byte);
+
+	/* upload */
+	saa7146_write(dev, MC2, (MASK_02<<(which-1))|(MASK_18<<(which-1)));
+/*
+	printk("vdma%d.base_even:     0x%08x\n", which,vdma->base_even);
+	printk("vdma%d.base_odd:      0x%08x\n", which,vdma->base_odd);
+	printk("vdma%d.prot_addr:     0x%08x\n", which,vdma->prot_addr);
+	printk("vdma%d.base_page:     0x%08x\n", which,vdma->base_page);
+	printk("vdma%d.pitch:         0x%08x\n", which,vdma->pitch);
+	printk("vdma%d.num_line_byte: 0x%08x\n", which,vdma->num_line_byte);
+*/
+}
+
+static int calculate_video_dma_grab_packed(struct saa7146_dev* dev, struct saa7146_buf *buf)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_video_dma vdma1;
+
+	struct saa7146_format *sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
+
+	int width = buf->fmt->width;
+	int height = buf->fmt->height;
+	int bytesperline = buf->fmt->bytesperline;
+	enum v4l2_field field = buf->fmt->field;
+
+	int depth = sfmt->depth;
+
+	DEB_CAP("[size=%dx%d,fields=%s]\n",
+		width, height, v4l2_field_names[field]);
+
+	if( bytesperline != 0) {
+		vdma1.pitch = bytesperline*2;
+	} else {
+		vdma1.pitch = (width*depth*2)/8;
+	}
+	vdma1.num_line_byte	= ((vv->standard->v_field<<16) + vv->standard->h_pixels);
+	vdma1.base_page		= buf->pt[0].dma | ME1 | sfmt->swap;
+
+	if( 0 != vv->vflip ) {
+		vdma1.prot_addr	= buf->pt[0].offset;
+		vdma1.base_even	= buf->pt[0].offset+(vdma1.pitch/2)*height;
+		vdma1.base_odd	= vdma1.base_even - (vdma1.pitch/2);
+	} else {
+		vdma1.base_even	= buf->pt[0].offset;
+		vdma1.base_odd	= vdma1.base_even + (vdma1.pitch/2);
+		vdma1.prot_addr	= buf->pt[0].offset+(vdma1.pitch/2)*height;
+	}
+
+	if (V4L2_FIELD_HAS_BOTH(field)) {
+	} else if (field == V4L2_FIELD_ALTERNATE) {
+		/* fixme */
+		if ( vv->last_field == V4L2_FIELD_TOP ) {
+			vdma1.base_odd	= vdma1.prot_addr;
+			vdma1.pitch /= 2;
+		} else if ( vv->last_field == V4L2_FIELD_BOTTOM ) {
+			vdma1.base_odd	= vdma1.base_even;
+			vdma1.base_even = vdma1.prot_addr;
+			vdma1.pitch /= 2;
+		}
+	} else if (field == V4L2_FIELD_TOP) {
+		vdma1.base_odd	= vdma1.prot_addr;
+		vdma1.pitch /= 2;
+	} else if (field == V4L2_FIELD_BOTTOM) {
+		vdma1.base_odd	= vdma1.base_even;
+		vdma1.base_even = vdma1.prot_addr;
+		vdma1.pitch /= 2;
+	}
+
+	if( 0 != vv->vflip ) {
+		vdma1.pitch *= -1;
+	}
+
+	saa7146_write_out_dma(dev, 1, &vdma1);
+	return 0;
+}
+
+static int calc_planar_422(struct saa7146_vv *vv, struct saa7146_buf *buf, struct saa7146_video_dma *vdma2, struct saa7146_video_dma *vdma3)
+{
+	int height = buf->fmt->height;
+	int width = buf->fmt->width;
+
+	vdma2->pitch	= width;
+	vdma3->pitch	= width;
+
+	/* fixme: look at bytesperline! */
+
+	if( 0 != vv->vflip ) {
+		vdma2->prot_addr	= buf->pt[1].offset;
+		vdma2->base_even	= ((vdma2->pitch/2)*height)+buf->pt[1].offset;
+		vdma2->base_odd		= vdma2->base_even - (vdma2->pitch/2);
+
+		vdma3->prot_addr	= buf->pt[2].offset;
+		vdma3->base_even	= ((vdma3->pitch/2)*height)+buf->pt[2].offset;
+		vdma3->base_odd		= vdma3->base_even - (vdma3->pitch/2);
+	} else {
+		vdma3->base_even	= buf->pt[2].offset;
+		vdma3->base_odd		= vdma3->base_even + (vdma3->pitch/2);
+		vdma3->prot_addr	= (vdma3->pitch/2)*height+buf->pt[2].offset;
+
+		vdma2->base_even	= buf->pt[1].offset;
+		vdma2->base_odd		= vdma2->base_even + (vdma2->pitch/2);
+		vdma2->prot_addr	= (vdma2->pitch/2)*height+buf->pt[1].offset;
+	}
+
+	return 0;
+}
+
+static int calc_planar_420(struct saa7146_vv *vv, struct saa7146_buf *buf, struct saa7146_video_dma *vdma2, struct saa7146_video_dma *vdma3)
+{
+	int height = buf->fmt->height;
+	int width = buf->fmt->width;
+
+	vdma2->pitch	= width/2;
+	vdma3->pitch	= width/2;
+
+	if( 0 != vv->vflip ) {
+		vdma2->prot_addr	= buf->pt[2].offset;
+		vdma2->base_even	= ((vdma2->pitch/2)*height)+buf->pt[2].offset;
+		vdma2->base_odd		= vdma2->base_even - (vdma2->pitch/2);
+
+		vdma3->prot_addr	= buf->pt[1].offset;
+		vdma3->base_even	= ((vdma3->pitch/2)*height)+buf->pt[1].offset;
+		vdma3->base_odd		= vdma3->base_even - (vdma3->pitch/2);
+
+	} else {
+		vdma3->base_even	= buf->pt[2].offset;
+		vdma3->base_odd		= vdma3->base_even + (vdma3->pitch);
+		vdma3->prot_addr	= (vdma3->pitch/2)*height+buf->pt[2].offset;
+
+		vdma2->base_even	= buf->pt[1].offset;
+		vdma2->base_odd		= vdma2->base_even + (vdma2->pitch);
+		vdma2->prot_addr	= (vdma2->pitch/2)*height+buf->pt[1].offset;
+	}
+	return 0;
+}
+
+static int calculate_video_dma_grab_planar(struct saa7146_dev* dev, struct saa7146_buf *buf)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_video_dma vdma1;
+	struct saa7146_video_dma vdma2;
+	struct saa7146_video_dma vdma3;
+
+	struct saa7146_format *sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
+
+	int width = buf->fmt->width;
+	int height = buf->fmt->height;
+	enum v4l2_field field = buf->fmt->field;
+
+	BUG_ON(0 == buf->pt[0].dma);
+	BUG_ON(0 == buf->pt[1].dma);
+	BUG_ON(0 == buf->pt[2].dma);
+
+	DEB_CAP("[size=%dx%d,fields=%s]\n",
+		width, height, v4l2_field_names[field]);
+
+	/* fixme: look at bytesperline! */
+
+	/* fixme: what happens for user space buffers here?. The offsets are
+	   most likely wrong, this version here only works for page-aligned
+	   buffers, modifications to the pagetable-functions are necessary...*/
+
+	vdma1.pitch		= width*2;
+	vdma1.num_line_byte	= ((vv->standard->v_field<<16) + vv->standard->h_pixels);
+	vdma1.base_page		= buf->pt[0].dma | ME1;
+
+	if( 0 != vv->vflip ) {
+		vdma1.prot_addr	= buf->pt[0].offset;
+		vdma1.base_even	= ((vdma1.pitch/2)*height)+buf->pt[0].offset;
+		vdma1.base_odd	= vdma1.base_even - (vdma1.pitch/2);
+	} else {
+		vdma1.base_even	= buf->pt[0].offset;
+		vdma1.base_odd	= vdma1.base_even + (vdma1.pitch/2);
+		vdma1.prot_addr	= (vdma1.pitch/2)*height+buf->pt[0].offset;
+	}
+
+	vdma2.num_line_byte	= 0; /* unused */
+	vdma2.base_page		= buf->pt[1].dma | ME1;
+
+	vdma3.num_line_byte	= 0; /* unused */
+	vdma3.base_page		= buf->pt[2].dma | ME1;
+
+	switch( sfmt->depth ) {
+		case 12: {
+			calc_planar_420(vv,buf,&vdma2,&vdma3);
+			break;
+		}
+		case 16: {
+			calc_planar_422(vv,buf,&vdma2,&vdma3);
+			break;
+		}
+		default: {
+			return -1;
+		}
+	}
+
+	if (V4L2_FIELD_HAS_BOTH(field)) {
+	} else if (field == V4L2_FIELD_ALTERNATE) {
+		/* fixme */
+		vdma1.base_odd	= vdma1.prot_addr;
+		vdma1.pitch /= 2;
+		vdma2.base_odd	= vdma2.prot_addr;
+		vdma2.pitch /= 2;
+		vdma3.base_odd	= vdma3.prot_addr;
+		vdma3.pitch /= 2;
+	} else if (field == V4L2_FIELD_TOP) {
+		vdma1.base_odd	= vdma1.prot_addr;
+		vdma1.pitch /= 2;
+		vdma2.base_odd	= vdma2.prot_addr;
+		vdma2.pitch /= 2;
+		vdma3.base_odd	= vdma3.prot_addr;
+		vdma3.pitch /= 2;
+	} else if (field == V4L2_FIELD_BOTTOM) {
+		vdma1.base_odd	= vdma1.base_even;
+		vdma1.base_even = vdma1.prot_addr;
+		vdma1.pitch /= 2;
+		vdma2.base_odd	= vdma2.base_even;
+		vdma2.base_even = vdma2.prot_addr;
+		vdma2.pitch /= 2;
+		vdma3.base_odd	= vdma3.base_even;
+		vdma3.base_even = vdma3.prot_addr;
+		vdma3.pitch /= 2;
+	}
+
+	if( 0 != vv->vflip ) {
+		vdma1.pitch *= -1;
+		vdma2.pitch *= -1;
+		vdma3.pitch *= -1;
+	}
+
+	saa7146_write_out_dma(dev, 1, &vdma1);
+	if( (sfmt->flags & FORMAT_BYTE_SWAP) != 0 ) {
+		saa7146_write_out_dma(dev, 3, &vdma2);
+		saa7146_write_out_dma(dev, 2, &vdma3);
+	} else {
+		saa7146_write_out_dma(dev, 2, &vdma2);
+		saa7146_write_out_dma(dev, 3, &vdma3);
+	}
+	return 0;
+}
+
+static void program_capture_engine(struct saa7146_dev *dev, int planar)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	int count = 0;
+
+	unsigned long e_wait = vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? CMD_E_FID_A : CMD_E_FID_B;
+	unsigned long o_wait = vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? CMD_O_FID_A : CMD_O_FID_B;
+
+	/* wait for o_fid_a/b / e_fid_a/b toggle only if rps register 0 is not set*/
+	WRITE_RPS0(CMD_PAUSE | CMD_OAN | CMD_SIG0 | o_wait);
+	WRITE_RPS0(CMD_PAUSE | CMD_OAN | CMD_SIG0 | e_wait);
+
+	/* set rps register 0 */
+	WRITE_RPS0(CMD_WR_REG | (1 << 8) | (MC2/4));
+	WRITE_RPS0(MASK_27 | MASK_11);
+
+	/* turn on video-dma1 */
+	WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+	WRITE_RPS0(MASK_06 | MASK_22);			/* => mask */
+	WRITE_RPS0(MASK_06 | MASK_22);			/* => values */
+	if( 0 != planar ) {
+		/* turn on video-dma2 */
+		WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+		WRITE_RPS0(MASK_05 | MASK_21);			/* => mask */
+		WRITE_RPS0(MASK_05 | MASK_21);			/* => values */
+
+		/* turn on video-dma3 */
+		WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+		WRITE_RPS0(MASK_04 | MASK_20);			/* => mask */
+		WRITE_RPS0(MASK_04 | MASK_20);			/* => values */
+	}
+
+	/* wait for o_fid_a/b / e_fid_a/b toggle */
+	if ( vv->last_field == V4L2_FIELD_INTERLACED ) {
+		WRITE_RPS0(CMD_PAUSE | o_wait);
+		WRITE_RPS0(CMD_PAUSE | e_wait);
+	} else if ( vv->last_field == V4L2_FIELD_TOP ) {
+		WRITE_RPS0(CMD_PAUSE | (vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? MASK_10 : MASK_09));
+		WRITE_RPS0(CMD_PAUSE | o_wait);
+	} else if ( vv->last_field == V4L2_FIELD_BOTTOM ) {
+		WRITE_RPS0(CMD_PAUSE | (vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? MASK_10 : MASK_09));
+		WRITE_RPS0(CMD_PAUSE | e_wait);
+	}
+
+	/* turn off video-dma1 */
+	WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+	WRITE_RPS0(MASK_22 | MASK_06);			/* => mask */
+	WRITE_RPS0(MASK_22);				/* => values */
+	if( 0 != planar ) {
+		/* turn off video-dma2 */
+		WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+		WRITE_RPS0(MASK_05 | MASK_21);			/* => mask */
+		WRITE_RPS0(MASK_21);				/* => values */
+
+		/* turn off video-dma3 */
+		WRITE_RPS0(CMD_WR_REG_MASK | (MC1/4));
+		WRITE_RPS0(MASK_04 | MASK_20);			/* => mask */
+		WRITE_RPS0(MASK_20);				/* => values */
+	}
+
+	/* generate interrupt */
+	WRITE_RPS0(CMD_INTERRUPT);
+
+	/* stop */
+	WRITE_RPS0(CMD_STOP);
+}
+
+void saa7146_set_capture(struct saa7146_dev *dev, struct saa7146_buf *buf, struct saa7146_buf *next)
+{
+	struct saa7146_format *sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
+	struct saa7146_vv *vv = dev->vv_data;
+	u32 vdma1_prot_addr;
+
+	DEB_CAP("buf:%p, next:%p\n", buf, next);
+
+	vdma1_prot_addr = saa7146_read(dev, PROT_ADDR1);
+	if( 0 == vdma1_prot_addr ) {
+		/* clear out beginning of streaming bit (rps register 0)*/
+		DEB_CAP("forcing sync to new frame\n");
+		saa7146_write(dev, MC2, MASK_27 );
+	}
+
+	saa7146_set_window(dev, buf->fmt->width, buf->fmt->height, buf->fmt->field);
+	saa7146_set_output_format(dev, sfmt->trans);
+	saa7146_disable_clipping(dev);
+
+	if ( vv->last_field == V4L2_FIELD_INTERLACED ) {
+	} else if ( vv->last_field == V4L2_FIELD_TOP ) {
+		vv->last_field = V4L2_FIELD_BOTTOM;
+	} else if ( vv->last_field == V4L2_FIELD_BOTTOM ) {
+		vv->last_field = V4L2_FIELD_TOP;
+	}
+
+	if( 0 != IS_PLANAR(sfmt->trans)) {
+		calculate_video_dma_grab_planar(dev, buf);
+		program_capture_engine(dev,1);
+	} else {
+		calculate_video_dma_grab_packed(dev, buf);
+		program_capture_engine(dev,0);
+	}
+
+/*
+	printk("vdma%d.base_even:     0x%08x\n", 1,saa7146_read(dev,BASE_EVEN1));
+	printk("vdma%d.base_odd:      0x%08x\n", 1,saa7146_read(dev,BASE_ODD1));
+	printk("vdma%d.prot_addr:     0x%08x\n", 1,saa7146_read(dev,PROT_ADDR1));
+	printk("vdma%d.base_page:     0x%08x\n", 1,saa7146_read(dev,BASE_PAGE1));
+	printk("vdma%d.pitch:         0x%08x\n", 1,saa7146_read(dev,PITCH1));
+	printk("vdma%d.num_line_byte: 0x%08x\n", 1,saa7146_read(dev,NUM_LINE_BYTE1));
+	printk("vdma%d => vptr      : 0x%08x\n", 1,saa7146_read(dev,PCI_VDP1));
+*/
+
+	/* write the address of the rps-program */
+	saa7146_write(dev, RPS_ADDR0, dev->d_rps0.dma_handle);
+
+	/* turn on rps */
+	saa7146_write(dev, MC1, (MASK_12 | MASK_28));
+}
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_i2c.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_i2c.c
new file mode 100644
index 000000000..7a33fe517
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_i2c.c
@@ -0,0 +1,421 @@
+// SPDX-License-Identifier: GPL-2.0
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "saa7146_vv.h"
+
+static u32 saa7146_i2c_func(struct i2c_adapter *adapter)
+{
+	/* DEB_I2C("'%s'\n", adapter->name); */
+
+	return	  I2C_FUNC_I2C
+		| I2C_FUNC_SMBUS_QUICK
+		| I2C_FUNC_SMBUS_READ_BYTE	| I2C_FUNC_SMBUS_WRITE_BYTE
+		| I2C_FUNC_SMBUS_READ_BYTE_DATA | I2C_FUNC_SMBUS_WRITE_BYTE_DATA;
+}
+
+/* this function returns the status-register of our i2c-device */
+static inline u32 saa7146_i2c_status(struct saa7146_dev *dev)
+{
+	u32 iicsta = saa7146_read(dev, I2C_STATUS);
+	/* DEB_I2C("status: 0x%08x\n", iicsta); */
+	return iicsta;
+}
+
+/* this function runs through the i2c-messages and prepares the data to be
+   sent through the saa7146. have a look at the specifications p. 122 ff
+   to understand this. it returns the number of u32s to send, or -1
+   in case of an error. */
+static int saa7146_i2c_msg_prepare(const struct i2c_msg *m, int num, __le32 *op)
+{
+	int h1, h2;
+	int i, j, addr;
+	int mem = 0, op_count = 0;
+
+	/* first determine size of needed memory */
+	for(i = 0; i < num; i++) {
+		mem += m[i].len + 1;
+	}
+
+	/* worst case: we need one u32 for three bytes to be send
+	   plus one extra byte to address the device */
+	mem = 1 + ((mem-1) / 3);
+
+	/* we assume that op points to a memory of at least
+	 * SAA7146_I2C_MEM bytes size. if we exceed this limit...
+	 */
+	if ((4 * mem) > SAA7146_I2C_MEM) {
+		/* DEB_I2C("cannot prepare i2c-message\n"); */
+		return -ENOMEM;
+	}
+
+	/* be careful: clear out the i2c-mem first */
+	memset(op,0,sizeof(__le32)*mem);
+
+	/* loop through all messages */
+	for(i = 0; i < num; i++) {
+
+		addr = i2c_8bit_addr_from_msg(&m[i]);
+		h1 = op_count/3; h2 = op_count%3;
+		op[h1] |= cpu_to_le32(	    (u8)addr << ((3-h2)*8));
+		op[h1] |= cpu_to_le32(SAA7146_I2C_START << ((3-h2)*2));
+		op_count++;
+
+		/* loop through all bytes of message i */
+		for(j = 0; j < m[i].len; j++) {
+			/* insert the data bytes */
+			h1 = op_count/3; h2 = op_count%3;
+			op[h1] |= cpu_to_le32( (u32)((u8)m[i].buf[j]) << ((3-h2)*8));
+			op[h1] |= cpu_to_le32(       SAA7146_I2C_CONT << ((3-h2)*2));
+			op_count++;
+		}
+
+	}
+
+	/* have a look at the last byte inserted:
+	  if it was: ...CONT change it to ...STOP */
+	h1 = (op_count-1)/3; h2 = (op_count-1)%3;
+	if ( SAA7146_I2C_CONT == (0x3 & (le32_to_cpu(op[h1]) >> ((3-h2)*2))) ) {
+		op[h1] &= ~cpu_to_le32(0x2 << ((3-h2)*2));
+		op[h1] |= cpu_to_le32(SAA7146_I2C_STOP << ((3-h2)*2));
+	}
+
+	/* return the number of u32s to send */
+	return mem;
+}
+
+/* this functions loops through all i2c-messages. normally, it should determine
+   which bytes were read through the adapter and write them back to the corresponding
+   i2c-message. but instead, we simply write back all bytes.
+   fixme: this could be improved. */
+static int saa7146_i2c_msg_cleanup(const struct i2c_msg *m, int num, __le32 *op)
+{
+	int i, j;
+	int op_count = 0;
+
+	/* loop through all messages */
+	for(i = 0; i < num; i++) {
+
+		op_count++;
+
+		/* loop through all bytes of message i */
+		for(j = 0; j < m[i].len; j++) {
+			/* write back all bytes that could have been read */
+			m[i].buf[j] = (le32_to_cpu(op[op_count/3]) >> ((3-(op_count%3))*8));
+			op_count++;
+		}
+	}
+
+	return 0;
+}
+
+/* this functions resets the i2c-device and returns 0 if everything was fine, otherwise -1 */
+static int saa7146_i2c_reset(struct saa7146_dev *dev)
+{
+	/* get current status */
+	u32 status = saa7146_i2c_status(dev);
+
+	/* clear registers for sure */
+	saa7146_write(dev, I2C_STATUS, dev->i2c_bitrate);
+	saa7146_write(dev, I2C_TRANSFER, 0);
+
+	/* check if any operation is still in progress */
+	if ( 0 != ( status & SAA7146_I2C_BUSY) ) {
+
+		/* yes, kill ongoing operation */
+		DEB_I2C("busy_state detected\n");
+
+		/* set "ABORT-OPERATION"-bit (bit 7)*/
+		saa7146_write(dev, I2C_STATUS, (dev->i2c_bitrate | MASK_07));
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+		msleep(SAA7146_I2C_DELAY);
+
+		/* clear all error-bits pending; this is needed because p.123, note 1 */
+		saa7146_write(dev, I2C_STATUS, dev->i2c_bitrate);
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+		msleep(SAA7146_I2C_DELAY);
+	}
+
+	/* check if any error is (still) present. (this can be necessary because p.123, note 1) */
+	status = saa7146_i2c_status(dev);
+
+	if ( dev->i2c_bitrate != status ) {
+
+		DEB_I2C("error_state detected. status:0x%08x\n", status);
+
+		/* Repeat the abort operation. This seems to be necessary
+		   after serious protocol errors caused by e.g. the SAA7740 */
+		saa7146_write(dev, I2C_STATUS, (dev->i2c_bitrate | MASK_07));
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+		msleep(SAA7146_I2C_DELAY);
+
+		/* clear all error-bits pending */
+		saa7146_write(dev, I2C_STATUS, dev->i2c_bitrate);
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+		msleep(SAA7146_I2C_DELAY);
+
+		/* the data sheet says it might be necessary to clear the status
+		   twice after an abort */
+		saa7146_write(dev, I2C_STATUS, dev->i2c_bitrate);
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+		msleep(SAA7146_I2C_DELAY);
+	}
+
+	/* if any error is still present, a fatal error has occurred ... */
+	status = saa7146_i2c_status(dev);
+	if ( dev->i2c_bitrate != status ) {
+		DEB_I2C("fatal error. status:0x%08x\n", status);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* this functions writes out the data-byte 'dword' to the i2c-device.
+   it returns 0 if ok, -1 if the transfer failed, -2 if the transfer
+   failed badly (e.g. address error) */
+static int saa7146_i2c_writeout(struct saa7146_dev *dev, __le32 *dword, int short_delay)
+{
+	u32 status = 0, mc2 = 0;
+	int trial = 0;
+	unsigned long timeout;
+
+	/* write out i2c-command */
+	DEB_I2C("before: 0x%08x (status: 0x%08x), %d\n",
+		*dword, saa7146_read(dev, I2C_STATUS), dev->i2c_op);
+
+	if( 0 != (SAA7146_USE_I2C_IRQ & dev->ext->flags)) {
+
+		saa7146_write(dev, I2C_STATUS,	 dev->i2c_bitrate);
+		saa7146_write(dev, I2C_TRANSFER, le32_to_cpu(*dword));
+
+		dev->i2c_op = 1;
+		SAA7146_ISR_CLEAR(dev, MASK_16|MASK_17);
+		SAA7146_IER_ENABLE(dev, MASK_16|MASK_17);
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+
+		timeout = HZ/100 + 1; /* 10ms */
+		timeout = wait_event_interruptible_timeout(dev->i2c_wq, dev->i2c_op == 0, timeout);
+		if (timeout == -ERESTARTSYS || dev->i2c_op) {
+			SAA7146_IER_DISABLE(dev, MASK_16|MASK_17);
+			SAA7146_ISR_CLEAR(dev, MASK_16|MASK_17);
+			if (timeout == -ERESTARTSYS)
+				/* a signal arrived */
+				return -ERESTARTSYS;
+
+			pr_warn("%s %s [irq]: timed out waiting for end of xfer\n",
+				dev->name, __func__);
+			return -EIO;
+		}
+		status = saa7146_read(dev, I2C_STATUS);
+	} else {
+		saa7146_write(dev, I2C_STATUS,	 dev->i2c_bitrate);
+		saa7146_write(dev, I2C_TRANSFER, le32_to_cpu(*dword));
+		saa7146_write(dev, MC2, (MASK_00 | MASK_16));
+
+		/* do not poll for i2c-status before upload is complete */
+		timeout = jiffies + HZ/100 + 1; /* 10ms */
+		while(1) {
+			mc2 = (saa7146_read(dev, MC2) & 0x1);
+			if( 0 != mc2 ) {
+				break;
+			}
+			if (time_after(jiffies,timeout)) {
+				pr_warn("%s %s: timed out waiting for MC2\n",
+					dev->name, __func__);
+				return -EIO;
+			}
+		}
+		/* wait until we get a transfer done or error */
+		timeout = jiffies + HZ/100 + 1; /* 10ms */
+		/* first read usually delivers bogus results... */
+		saa7146_i2c_status(dev);
+		while(1) {
+			status = saa7146_i2c_status(dev);
+			if ((status & 0x3) != 1)
+				break;
+			if (time_after(jiffies,timeout)) {
+				/* this is normal when probing the bus
+				 * (no answer from nonexisistant device...)
+				 */
+				pr_warn("%s %s [poll]: timed out waiting for end of xfer\n",
+					dev->name, __func__);
+				return -EIO;
+			}
+			if (++trial < 50 && short_delay)
+				udelay(10);
+			else
+				msleep(1);
+		}
+	}
+
+	/* give a detailed status report */
+	if ( 0 != (status & (SAA7146_I2C_SPERR | SAA7146_I2C_APERR |
+			     SAA7146_I2C_DTERR | SAA7146_I2C_DRERR |
+			     SAA7146_I2C_AL    | SAA7146_I2C_ERR   |
+			     SAA7146_I2C_BUSY)) ) {
+
+		if ( 0 == (status & SAA7146_I2C_ERR) ||
+		     0 == (status & SAA7146_I2C_BUSY) ) {
+			/* it may take some time until ERR goes high - ignore */
+			DEB_I2C("unexpected i2c status %04x\n", status);
+		}
+		if( 0 != (status & SAA7146_I2C_SPERR) ) {
+			DEB_I2C("error due to invalid start/stop condition\n");
+		}
+		if( 0 != (status & SAA7146_I2C_DTERR) ) {
+			DEB_I2C("error in data transmission\n");
+		}
+		if( 0 != (status & SAA7146_I2C_DRERR) ) {
+			DEB_I2C("error when receiving data\n");
+		}
+		if( 0 != (status & SAA7146_I2C_AL) ) {
+			DEB_I2C("error because arbitration lost\n");
+		}
+
+		/* we handle address-errors here */
+		if( 0 != (status & SAA7146_I2C_APERR) ) {
+			DEB_I2C("error in address phase\n");
+			return -EREMOTEIO;
+		}
+
+		return -EIO;
+	}
+
+	/* read back data, just in case we were reading ... */
+	*dword = cpu_to_le32(saa7146_read(dev, I2C_TRANSFER));
+
+	DEB_I2C("after: 0x%08x\n", *dword);
+	return 0;
+}
+
+static int saa7146_i2c_transfer(struct saa7146_dev *dev, const struct i2c_msg *msgs, int num, int retries)
+{
+	int i = 0, count = 0;
+	__le32 *buffer = dev->d_i2c.cpu_addr;
+	int err = 0;
+	int short_delay = 0;
+
+	if (mutex_lock_interruptible(&dev->i2c_lock))
+		return -ERESTARTSYS;
+
+	for(i=0;i<num;i++) {
+		DEB_I2C("msg:%d/%d\n", i+1, num);
+	}
+
+	/* prepare the message(s), get number of u32s to transfer */
+	count = saa7146_i2c_msg_prepare(msgs, num, buffer);
+	if ( 0 > count ) {
+		err = -EIO;
+		goto out;
+	}
+
+	if ( count > 3 || 0 != (SAA7146_I2C_SHORT_DELAY & dev->ext->flags) )
+		short_delay = 1;
+
+	do {
+		/* reset the i2c-device if necessary */
+		err = saa7146_i2c_reset(dev);
+		if ( 0 > err ) {
+			DEB_I2C("could not reset i2c-device\n");
+			goto out;
+		}
+
+		/* write out the u32s one after another */
+		for(i = 0; i < count; i++) {
+			err = saa7146_i2c_writeout(dev, &buffer[i], short_delay);
+			if ( 0 != err) {
+				/* this one is unsatisfying: some i2c slaves on some
+				   dvb cards don't acknowledge correctly, so the saa7146
+				   thinks that an address error occurred. in that case, the
+				   transaction should be retrying, even if an address error
+				   occurred. analog saa7146 based cards extensively rely on
+				   i2c address probing, however, and address errors indicate that a
+				   device is really *not* there. retrying in that case
+				   increases the time the device needs to probe greatly, so
+				   it should be avoided. So we bail out in irq mode after an
+				   address error and trust the saa7146 address error detection. */
+				if (-EREMOTEIO == err && 0 != (SAA7146_USE_I2C_IRQ & dev->ext->flags))
+					goto out;
+				DEB_I2C("error while sending message(s). starting again\n");
+				break;
+			}
+		}
+		if( 0 == err ) {
+			err = num;
+			break;
+		}
+
+		/* delay a bit before retrying */
+		msleep(10);
+
+	} while (err != num && retries--);
+
+	/* quit if any error occurred */
+	if (err != num)
+		goto out;
+
+	/* if any things had to be read, get the results */
+	if ( 0 != saa7146_i2c_msg_cleanup(msgs, num, buffer)) {
+		DEB_I2C("could not cleanup i2c-message\n");
+		err = -EIO;
+		goto out;
+	}
+
+	/* return the number of delivered messages */
+	DEB_I2C("transmission successful. (msg:%d)\n", err);
+out:
+	/* another bug in revision 0: the i2c-registers get uploaded randomly by other
+	   uploads, so we better clear them out before continuing */
+	if( 0 == dev->revision ) {
+		__le32 zero = 0;
+		saa7146_i2c_reset(dev);
+		if( 0 != saa7146_i2c_writeout(dev, &zero, short_delay)) {
+			pr_info("revision 0 error. this should never happen\n");
+		}
+	}
+
+	mutex_unlock(&dev->i2c_lock);
+	return err;
+}
+
+/* utility functions */
+static int saa7146_i2c_xfer(struct i2c_adapter* adapter, struct i2c_msg *msg, int num)
+{
+	struct v4l2_device *v4l2_dev = i2c_get_adapdata(adapter);
+	struct saa7146_dev *dev = to_saa7146_dev(v4l2_dev);
+
+	/* use helper function to transfer data */
+	return saa7146_i2c_transfer(dev, msg, num, adapter->retries);
+}
+
+
+/*****************************************************************************/
+/* i2c-adapter helper functions                                              */
+
+/* exported algorithm data */
+static const struct i2c_algorithm saa7146_algo = {
+	.master_xfer	= saa7146_i2c_xfer,
+	.functionality	= saa7146_i2c_func,
+};
+
+int saa7146_i2c_adapter_prepare(struct saa7146_dev *dev, struct i2c_adapter *i2c_adapter, u32 bitrate)
+{
+	DEB_EE("bitrate: 0x%08x\n", bitrate);
+
+	/* enable i2c-port pins */
+	saa7146_write(dev, MC1, (MASK_08 | MASK_24));
+
+	dev->i2c_bitrate = bitrate;
+	saa7146_i2c_reset(dev);
+
+	if (i2c_adapter) {
+		i2c_set_adapdata(i2c_adapter, &dev->v4l2_dev);
+		i2c_adapter->dev.parent    = &dev->pci->dev;
+		i2c_adapter->algo	   = &saa7146_algo;
+		i2c_adapter->algo_data     = NULL;
+		i2c_adapter->timeout = SAA7146_I2C_TIMEOUT;
+		i2c_adapter->retries = SAA7146_I2C_RETRIES;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_vbi.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_vbi.c
new file mode 100644
index 000000000..2d4a05d7b
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_vbi.c
@@ -0,0 +1,498 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "saa7146_vv.h"
+
+static int vbi_pixel_to_capture = 720 * 2;
+
+static int vbi_workaround(struct saa7146_dev *dev)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+
+	u32          *cpu;
+	dma_addr_t   dma_addr;
+
+	int count = 0;
+	int i;
+
+	DECLARE_WAITQUEUE(wait, current);
+
+	DEB_VBI("dev:%p\n", dev);
+
+	/* once again, a bug in the saa7146: the brs acquisition
+	   is buggy and especially the BXO-counter does not work
+	   as specified. there is this workaround, but please
+	   don't let me explain it. ;-) */
+
+	cpu = dma_alloc_coherent(&dev->pci->dev, 4096, &dma_addr, GFP_KERNEL);
+	if (NULL == cpu)
+		return -ENOMEM;
+
+	/* setup some basic programming, just for the workaround */
+	saa7146_write(dev, BASE_EVEN3,	dma_addr);
+	saa7146_write(dev, BASE_ODD3,	dma_addr+vbi_pixel_to_capture);
+	saa7146_write(dev, PROT_ADDR3,	dma_addr+4096);
+	saa7146_write(dev, PITCH3,	vbi_pixel_to_capture);
+	saa7146_write(dev, BASE_PAGE3,	0x0);
+	saa7146_write(dev, NUM_LINE_BYTE3, (2<<16)|((vbi_pixel_to_capture)<<0));
+	saa7146_write(dev, MC2, MASK_04|MASK_20);
+
+	/* load brs-control register */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (BRS_CTRL/4));
+	/* BXO = 1h, BRS to outbound */
+	WRITE_RPS1(0xc000008c);
+	/* wait for vbi_a or vbi_b*/
+	if ( 0 != (SAA7146_USE_PORT_B_FOR_VBI & dev->ext_vv_data->flags)) {
+		DEB_D("...using port b\n");
+		WRITE_RPS1(CMD_PAUSE | CMD_OAN | CMD_SIG1 | CMD_E_FID_B);
+		WRITE_RPS1(CMD_PAUSE | CMD_OAN | CMD_SIG1 | CMD_O_FID_B);
+/*
+		WRITE_RPS1(CMD_PAUSE | MASK_09);
+*/
+	} else {
+		DEB_D("...using port a\n");
+		WRITE_RPS1(CMD_PAUSE | MASK_10);
+	}
+	/* upload brs */
+	WRITE_RPS1(CMD_UPLOAD | MASK_08);
+	/* load brs-control register */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (BRS_CTRL/4));
+	/* BYO = 1, BXO = NQBIL (=1728 for PAL, for NTSC this is 858*2) - NumByte3 (=1440) = 288 */
+	WRITE_RPS1(((1728-(vbi_pixel_to_capture)) << 7) | MASK_19);
+	/* wait for brs_done */
+	WRITE_RPS1(CMD_PAUSE | MASK_08);
+	/* upload brs */
+	WRITE_RPS1(CMD_UPLOAD | MASK_08);
+	/* load video-dma3 NumLines3 and NumBytes3 */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (NUM_LINE_BYTE3/4));
+	/* dev->vbi_count*2 lines, 720 pixel (= 1440 Bytes) */
+	WRITE_RPS1((2 << 16) | (vbi_pixel_to_capture));
+	/* load brs-control register */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (BRS_CTRL/4));
+	/* Set BRS right: note: this is an experimental value for BXO (=> PAL!) */
+	WRITE_RPS1((540 << 7) | (5 << 19));  // 5 == vbi_start
+	/* wait for brs_done */
+	WRITE_RPS1(CMD_PAUSE | MASK_08);
+	/* upload brs and video-dma3*/
+	WRITE_RPS1(CMD_UPLOAD | MASK_08 | MASK_04);
+	/* load mc2 register: enable dma3 */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (MC1/4));
+	WRITE_RPS1(MASK_20 | MASK_04);
+	/* generate interrupt */
+	WRITE_RPS1(CMD_INTERRUPT);
+	/* stop rps1 */
+	WRITE_RPS1(CMD_STOP);
+
+	/* we have to do the workaround twice to be sure that
+	   everything is ok */
+	for(i = 0; i < 2; i++) {
+
+		/* indicate to the irq handler that we do the workaround */
+		saa7146_write(dev, MC2, MASK_31|MASK_15);
+
+		saa7146_write(dev, NUM_LINE_BYTE3, (1<<16)|(2<<0));
+		saa7146_write(dev, MC2, MASK_04|MASK_20);
+
+		/* enable rps1 irqs */
+		SAA7146_IER_ENABLE(dev,MASK_28);
+
+		/* prepare to wait to be woken up by the irq-handler */
+		add_wait_queue(&vv->vbi_wq, &wait);
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		/* start rps1 to enable workaround */
+		saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+		saa7146_write(dev, MC1, (MASK_13 | MASK_29));
+
+		schedule();
+
+		DEB_VBI("brs bug workaround %d/1\n", i);
+
+		remove_wait_queue(&vv->vbi_wq, &wait);
+		__set_current_state(TASK_RUNNING);
+
+		/* disable rps1 irqs */
+		SAA7146_IER_DISABLE(dev,MASK_28);
+
+		/* stop video-dma3 */
+		saa7146_write(dev, MC1, MASK_20);
+
+		if(signal_pending(current)) {
+
+			DEB_VBI("aborted (rps:0x%08x)\n",
+				saa7146_read(dev, RPS_ADDR1));
+
+			/* stop rps1 for sure */
+			saa7146_write(dev, MC1, MASK_29);
+
+			dma_free_coherent(&dev->pci->dev, 4096, cpu, dma_addr);
+			return -EINTR;
+		}
+	}
+
+	dma_free_coherent(&dev->pci->dev, 4096, cpu, dma_addr);
+	return 0;
+}
+
+static void saa7146_set_vbi_capture(struct saa7146_dev *dev, struct saa7146_buf *buf, struct saa7146_buf *next)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+
+	struct saa7146_video_dma vdma3;
+
+	int count = 0;
+	unsigned long e_wait = vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? CMD_E_FID_A : CMD_E_FID_B;
+	unsigned long o_wait = vv->current_hps_sync == SAA7146_HPS_SYNC_PORT_A ? CMD_O_FID_A : CMD_O_FID_B;
+
+/*
+	vdma3.base_even	= 0xc8000000+2560*70;
+	vdma3.base_odd	= 0xc8000000;
+	vdma3.prot_addr	= 0xc8000000+2560*164;
+	vdma3.pitch	= 2560;
+	vdma3.base_page	= 0;
+	vdma3.num_line_byte = (64<<16)|((vbi_pixel_to_capture)<<0); // set above!
+*/
+	vdma3.base_even	= buf->pt[2].offset;
+	vdma3.base_odd	= buf->pt[2].offset + 16 * vbi_pixel_to_capture;
+	vdma3.prot_addr	= buf->pt[2].offset + 16 * 2 * vbi_pixel_to_capture;
+	vdma3.pitch	= vbi_pixel_to_capture;
+	vdma3.base_page	= buf->pt[2].dma | ME1;
+	vdma3.num_line_byte = (16 << 16) | vbi_pixel_to_capture;
+
+	saa7146_write_out_dma(dev, 3, &vdma3);
+
+	/* write beginning of rps-program */
+	count = 0;
+
+	/* wait for o_fid_a/b / e_fid_a/b toggle only if bit 1 is not set */
+
+	/* we don't wait here for the first field anymore. this is different from the video
+	   capture and might cause that the first buffer is only half filled (with only
+	   one field). but since this is some sort of streaming data, this is not that negative.
+	   but by doing this, we can use the whole engine from videobuf-dma-sg.c... */
+
+/*
+	WRITE_RPS1(CMD_PAUSE | CMD_OAN | CMD_SIG1 | e_wait);
+	WRITE_RPS1(CMD_PAUSE | CMD_OAN | CMD_SIG1 | o_wait);
+*/
+	/* set bit 1 */
+	WRITE_RPS1(CMD_WR_REG | (1 << 8) | (MC2/4));
+	WRITE_RPS1(MASK_28 | MASK_12);
+
+	/* turn on video-dma3 */
+	WRITE_RPS1(CMD_WR_REG_MASK | (MC1/4));
+	WRITE_RPS1(MASK_04 | MASK_20);			/* => mask */
+	WRITE_RPS1(MASK_04 | MASK_20);			/* => values */
+
+	/* wait for o_fid_a/b / e_fid_a/b toggle */
+	WRITE_RPS1(CMD_PAUSE | o_wait);
+	WRITE_RPS1(CMD_PAUSE | e_wait);
+
+	/* generate interrupt */
+	WRITE_RPS1(CMD_INTERRUPT);
+
+	/* stop */
+	WRITE_RPS1(CMD_STOP);
+
+	/* enable rps1 irqs */
+	SAA7146_IER_ENABLE(dev, MASK_28);
+
+	/* write the address of the rps-program */
+	saa7146_write(dev, RPS_ADDR1, dev->d_rps1.dma_handle);
+
+	/* turn on rps */
+	saa7146_write(dev, MC1, (MASK_13 | MASK_29));
+}
+
+static int buffer_activate(struct saa7146_dev *dev,
+			   struct saa7146_buf *buf,
+			   struct saa7146_buf *next)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	buf->vb.state = VIDEOBUF_ACTIVE;
+
+	DEB_VBI("dev:%p, buf:%p, next:%p\n", dev, buf, next);
+	saa7146_set_vbi_capture(dev,buf,next);
+
+	mod_timer(&vv->vbi_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
+	return 0;
+}
+
+static int buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,enum v4l2_field field)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+
+	int err = 0;
+	int lines, llength, size;
+
+	lines   = 16 * 2 ; /* 2 fields */
+	llength = vbi_pixel_to_capture;
+	size = lines * llength;
+
+	DEB_VBI("vb:%p\n", vb);
+
+	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size) {
+		DEB_VBI("size mismatch\n");
+		return -EINVAL;
+	}
+
+	if (buf->vb.size != size)
+		saa7146_dma_free(dev,q,buf);
+
+	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
+		struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
+
+		buf->vb.width  = llength;
+		buf->vb.height = lines;
+		buf->vb.size   = size;
+		buf->vb.field  = field;	// FIXME: check this
+
+		saa7146_pgtable_free(dev->pci, &buf->pt[2]);
+		saa7146_pgtable_alloc(dev->pci, &buf->pt[2]);
+
+		err = videobuf_iolock(q,&buf->vb, NULL);
+		if (err)
+			goto oops;
+		err = saa7146_pgtable_build_single(dev->pci, &buf->pt[2],
+						 dma->sglist, dma->sglen);
+		if (0 != err)
+			return err;
+	}
+	buf->vb.state = VIDEOBUF_PREPARED;
+	buf->activate = buffer_activate;
+
+	return 0;
+
+ oops:
+	DEB_VBI("error out\n");
+	saa7146_dma_free(dev,q,buf);
+
+	return err;
+}
+
+static int buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
+{
+	int llength,lines;
+
+	lines   = 16 * 2 ; /* 2 fields */
+	llength = vbi_pixel_to_capture;
+
+	*size = lines * llength;
+	*count = 2;
+
+	DEB_VBI("count:%d, size:%d\n", *count, *size);
+
+	return 0;
+}
+
+static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+
+	DEB_VBI("vb:%p\n", vb);
+	saa7146_buffer_queue(dev, &vv->vbi_dmaq, buf);
+}
+
+static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh   = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+
+	DEB_VBI("vb:%p\n", vb);
+	saa7146_dma_free(dev,q,buf);
+}
+
+static const struct videobuf_queue_ops vbi_qops = {
+	.buf_setup    = buffer_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_queue    = buffer_queue,
+	.buf_release  = buffer_release,
+};
+
+/* ------------------------------------------------------------------ */
+
+static void vbi_stop(struct saa7146_fh *fh, struct file *file)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	unsigned long flags;
+	DEB_VBI("dev:%p, fh:%p\n", dev, fh);
+
+	spin_lock_irqsave(&dev->slock,flags);
+
+	/* disable rps1  */
+	saa7146_write(dev, MC1, MASK_29);
+
+	/* disable rps1 irqs */
+	SAA7146_IER_DISABLE(dev, MASK_28);
+
+	/* shut down dma 3 transfers */
+	saa7146_write(dev, MC1, MASK_20);
+
+	if (vv->vbi_dmaq.curr)
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
+
+	videobuf_queue_cancel(&fh->vbi_q);
+
+	vv->vbi_streaming = NULL;
+
+	del_timer(&vv->vbi_dmaq.timeout);
+	del_timer(&vv->vbi_read_timeout);
+
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+static void vbi_read_timeout(struct timer_list *t)
+{
+	struct saa7146_vv *vv = from_timer(vv, t, vbi_read_timeout);
+	struct file *file = vv->vbi_read_timeout_file;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+
+	DEB_VBI("dev:%p, fh:%p\n", dev, fh);
+
+	vbi_stop(fh, file);
+}
+
+static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
+{
+	DEB_VBI("dev:%p\n", dev);
+
+	INIT_LIST_HEAD(&vv->vbi_dmaq.queue);
+
+	timer_setup(&vv->vbi_dmaq.timeout, saa7146_buffer_timeout, 0);
+	vv->vbi_dmaq.dev              = dev;
+
+	init_waitqueue_head(&vv->vbi_wq);
+}
+
+static int vbi_open(struct saa7146_dev *dev, struct file *file)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
+
+	u32 arbtr_ctrl	= saa7146_read(dev, PCI_BT_V1);
+	int ret = 0;
+
+	DEB_VBI("dev:%p, fh:%p\n", dev, fh);
+
+	ret = saa7146_res_get(fh, RESOURCE_DMA3_BRS);
+	if (0 == ret) {
+		DEB_S("cannot get vbi RESOURCE_DMA3_BRS resource\n");
+		return -EBUSY;
+	}
+
+	/* adjust arbitrition control for video dma 3 */
+	arbtr_ctrl &= ~0x1f0000;
+	arbtr_ctrl |=  0x1d0000;
+	saa7146_write(dev, PCI_BT_V1, arbtr_ctrl);
+	saa7146_write(dev, MC2, (MASK_04|MASK_20));
+
+	videobuf_queue_sg_init(&fh->vbi_q, &vbi_qops,
+			    &dev->pci->dev, &dev->slock,
+			    V4L2_BUF_TYPE_VBI_CAPTURE,
+			    V4L2_FIELD_SEQ_TB, // FIXME: does this really work?
+			    sizeof(struct saa7146_buf),
+			    file, &dev->v4l2_lock);
+
+	vv->vbi_read_timeout.function = vbi_read_timeout;
+	vv->vbi_read_timeout_file = file;
+
+	/* initialize the brs */
+	if ( 0 != (SAA7146_USE_PORT_B_FOR_VBI & dev->ext_vv_data->flags)) {
+		saa7146_write(dev, BRS_CTRL, MASK_30|MASK_29 | (7 << 19));
+	} else {
+		saa7146_write(dev, BRS_CTRL, 0x00000001);
+
+		if (0 != (ret = vbi_workaround(dev))) {
+			DEB_VBI("vbi workaround failed!\n");
+			/* return ret;*/
+		}
+	}
+
+	/* upload brs register */
+	saa7146_write(dev, MC2, (MASK_08|MASK_24));
+	return 0;
+}
+
+static void vbi_close(struct saa7146_dev *dev, struct file *file)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = dev->vv_data;
+	DEB_VBI("dev:%p, fh:%p\n", dev, fh);
+
+	if( fh == vv->vbi_streaming ) {
+		vbi_stop(fh, file);
+	}
+	saa7146_res_free(fh, RESOURCE_DMA3_BRS);
+}
+
+static void vbi_irq_done(struct saa7146_dev *dev, unsigned long status)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	spin_lock(&dev->slock);
+
+	if (vv->vbi_dmaq.curr) {
+		DEB_VBI("dev:%p, curr:%p\n", dev, vv->vbi_dmaq.curr);
+		/* this must be += 2, one count for each field */
+		vv->vbi_fieldcount+=2;
+		vv->vbi_dmaq.curr->vb.field_count = vv->vbi_fieldcount;
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
+	} else {
+		DEB_VBI("dev:%p\n", dev);
+	}
+	saa7146_buffer_next(dev, &vv->vbi_dmaq, 1);
+
+	spin_unlock(&dev->slock);
+}
+
+static ssize_t vbi_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	ssize_t ret = 0;
+
+	DEB_VBI("dev:%p, fh:%p\n", dev, fh);
+
+	if( NULL == vv->vbi_streaming ) {
+		// fixme: check if dma3 is available
+		// fixme: activate vbi engine here if necessary. (really?)
+		vv->vbi_streaming = fh;
+	}
+
+	if( fh != vv->vbi_streaming ) {
+		DEB_VBI("open %p is already using vbi capture\n",
+			vv->vbi_streaming);
+		return -EBUSY;
+	}
+
+	mod_timer(&vv->vbi_read_timeout, jiffies+BUFFER_TIMEOUT);
+	ret = videobuf_read_stream(&fh->vbi_q, data, count, ppos, 1,
+				   file->f_flags & O_NONBLOCK);
+/*
+	printk("BASE_ODD3:      0x%08x\n", saa7146_read(dev, BASE_ODD3));
+	printk("BASE_EVEN3:     0x%08x\n", saa7146_read(dev, BASE_EVEN3));
+	printk("PROT_ADDR3:     0x%08x\n", saa7146_read(dev, PROT_ADDR3));
+	printk("PITCH3:         0x%08x\n", saa7146_read(dev, PITCH3));
+	printk("BASE_PAGE3:     0x%08x\n", saa7146_read(dev, BASE_PAGE3));
+	printk("NUM_LINE_BYTE3: 0x%08x\n", saa7146_read(dev, NUM_LINE_BYTE3));
+	printk("BRS_CTRL:       0x%08x\n", saa7146_read(dev, BRS_CTRL));
+*/
+	return ret;
+}
+
+const struct saa7146_use_ops saa7146_vbi_uops = {
+	.init		= vbi_init,
+	.open		= vbi_open,
+	.release	= vbi_close,
+	.irq_done	= vbi_irq_done,
+	.read		= vbi_read,
+};
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_video.c b/drivers/staging/media/deprecated/saa7146/common/saa7146_video.c
new file mode 100644
index 000000000..4598a4423
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_video.c
@@ -0,0 +1,1286 @@
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include "saa7146_vv.h"
+
+static int max_memory = 32;
+
+module_param(max_memory, int, 0644);
+MODULE_PARM_DESC(max_memory, "maximum memory usage for capture buffers (default: 32Mb)");
+
+#define IS_CAPTURE_ACTIVE(fh) \
+	(((vv->video_status & STATUS_CAPTURE) != 0) && (vv->video_fh == fh))
+
+#define IS_OVERLAY_ACTIVE(fh) \
+	(((vv->video_status & STATUS_OVERLAY) != 0) && (vv->video_fh == fh))
+
+/* format descriptions for capture and preview */
+static struct saa7146_format formats[] = {
+	{
+		.pixelformat	= V4L2_PIX_FMT_RGB332,
+		.trans		= RGB08_COMPOSED,
+		.depth		= 8,
+		.flags		= 0,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_RGB565,
+		.trans		= RGB16_COMPOSED,
+		.depth		= 16,
+		.flags		= 0,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_BGR24,
+		.trans		= RGB24_COMPOSED,
+		.depth		= 24,
+		.flags		= 0,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_BGR32,
+		.trans		= RGB32_COMPOSED,
+		.depth		= 32,
+		.flags		= 0,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_RGB32,
+		.trans		= RGB32_COMPOSED,
+		.depth		= 32,
+		.flags		= 0,
+		.swap		= 0x2,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_GREY,
+		.trans		= Y8,
+		.depth		= 8,
+		.flags		= 0,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUV422P,
+		.trans		= YUV422_DECOMPOSED,
+		.depth		= 16,
+		.flags		= FORMAT_BYTE_SWAP|FORMAT_IS_PLANAR,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YVU420,
+		.trans		= YUV420_DECOMPOSED,
+		.depth		= 12,
+		.flags		= FORMAT_BYTE_SWAP|FORMAT_IS_PLANAR,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUV420,
+		.trans		= YUV420_DECOMPOSED,
+		.depth		= 12,
+		.flags		= FORMAT_IS_PLANAR,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.trans		= YUV422_COMPOSED,
+		.depth		= 16,
+		.flags		= 0,
+	}
+};
+
+/* unfortunately, the saa7146 contains a bug which prevents it from doing on-the-fly byte swaps.
+   due to this, it's impossible to provide additional *packed* formats, which are simply byte swapped
+   (like V4L2_PIX_FMT_YUYV) ... 8-( */
+
+struct saa7146_format* saa7146_format_by_fourcc(struct saa7146_dev *dev, int fourcc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == fourcc) {
+			return formats+i;
+		}
+	}
+
+	DEB_D("unknown pixelformat:'%4.4s'\n", (char *)&fourcc);
+	return NULL;
+}
+
+static int vidioc_try_fmt_vid_overlay(struct file *file, void *fh, struct v4l2_format *f);
+
+int saa7146_start_preview(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct v4l2_format fmt;
+	int ret = 0, err = 0;
+
+	DEB_EE("dev:%p, fh:%p\n", dev, fh);
+
+	/* check if we have overlay information */
+	if (vv->ov.fh == NULL) {
+		DEB_D("no overlay data available. try S_FMT first.\n");
+		return -EAGAIN;
+	}
+
+	/* check if streaming capture is running */
+	if (IS_CAPTURE_ACTIVE(fh) != 0) {
+		DEB_D("streaming capture is active\n");
+		return -EBUSY;
+	}
+
+	/* check if overlay is running */
+	if (IS_OVERLAY_ACTIVE(fh) != 0) {
+		if (vv->video_fh == fh) {
+			DEB_D("overlay is already active\n");
+			return 0;
+		}
+		DEB_D("overlay is already active in another open\n");
+		return -EBUSY;
+	}
+
+	if (0 == saa7146_res_get(fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP)) {
+		DEB_D("cannot get necessary overlay resources\n");
+		return -EBUSY;
+	}
+
+	fmt.fmt.win = vv->ov.win;
+	err = vidioc_try_fmt_vid_overlay(NULL, fh, &fmt);
+	if (0 != err) {
+		saa7146_res_free(vv->video_fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP);
+		return -EBUSY;
+	}
+	vv->ov.win = fmt.fmt.win;
+
+	DEB_D("%dx%d+%d+%d 0x%08x field=%s\n",
+	      vv->ov.win.w.width, vv->ov.win.w.height,
+	      vv->ov.win.w.left, vv->ov.win.w.top,
+	      vv->ov_fmt->pixelformat, v4l2_field_names[vv->ov.win.field]);
+
+	if (0 != (ret = saa7146_enable_overlay(fh))) {
+		DEB_D("enabling overlay failed: %d\n", ret);
+		saa7146_res_free(vv->video_fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP);
+		return ret;
+	}
+
+	vv->video_status = STATUS_OVERLAY;
+	vv->video_fh = fh;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_start_preview);
+
+int saa7146_stop_preview(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	DEB_EE("dev:%p, fh:%p\n", dev, fh);
+
+	/* check if streaming capture is running */
+	if (IS_CAPTURE_ACTIVE(fh) != 0) {
+		DEB_D("streaming capture is active\n");
+		return -EBUSY;
+	}
+
+	/* check if overlay is running at all */
+	if ((vv->video_status & STATUS_OVERLAY) == 0) {
+		DEB_D("no active overlay\n");
+		return 0;
+	}
+
+	if (vv->video_fh != fh) {
+		DEB_D("overlay is active, but in another open\n");
+		return -EBUSY;
+	}
+
+	vv->video_status = 0;
+	vv->video_fh = NULL;
+
+	saa7146_disable_overlay(fh);
+
+	saa7146_res_free(fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7146_stop_preview);
+
+/********************************************************************************/
+/* common pagetable functions */
+
+static int saa7146_pgtable_build(struct saa7146_dev *dev, struct saa7146_buf *buf)
+{
+	struct pci_dev *pci = dev->pci;
+	struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
+	struct scatterlist *list = dma->sglist;
+	int length = dma->sglen;
+	struct saa7146_format *sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
+
+	DEB_EE("dev:%p, buf:%p, sg_len:%d\n", dev, buf, length);
+
+	if( 0 != IS_PLANAR(sfmt->trans)) {
+		struct saa7146_pgtable *pt1 = &buf->pt[0];
+		struct saa7146_pgtable *pt2 = &buf->pt[1];
+		struct saa7146_pgtable *pt3 = &buf->pt[2];
+		__le32  *ptr1, *ptr2, *ptr3;
+		__le32 fill;
+
+		int size = buf->fmt->width*buf->fmt->height;
+		int i,p,m1,m2,m3,o1,o2;
+
+		switch( sfmt->depth ) {
+			case 12: {
+				/* create some offsets inside the page table */
+				m1 = ((size+PAGE_SIZE)/PAGE_SIZE)-1;
+				m2 = ((size+(size/4)+PAGE_SIZE)/PAGE_SIZE)-1;
+				m3 = ((size+(size/2)+PAGE_SIZE)/PAGE_SIZE)-1;
+				o1 = size%PAGE_SIZE;
+				o2 = (size+(size/4))%PAGE_SIZE;
+				DEB_CAP("size:%d, m1:%d, m2:%d, m3:%d, o1:%d, o2:%d\n",
+					size, m1, m2, m3, o1, o2);
+				break;
+			}
+			case 16: {
+				/* create some offsets inside the page table */
+				m1 = ((size+PAGE_SIZE)/PAGE_SIZE)-1;
+				m2 = ((size+(size/2)+PAGE_SIZE)/PAGE_SIZE)-1;
+				m3 = ((2*size+PAGE_SIZE)/PAGE_SIZE)-1;
+				o1 = size%PAGE_SIZE;
+				o2 = (size+(size/2))%PAGE_SIZE;
+				DEB_CAP("size:%d, m1:%d, m2:%d, m3:%d, o1:%d, o2:%d\n",
+					size, m1, m2, m3, o1, o2);
+				break;
+			}
+			default: {
+				return -1;
+			}
+		}
+
+		ptr1 = pt1->cpu;
+		ptr2 = pt2->cpu;
+		ptr3 = pt3->cpu;
+
+		/* walk all pages, copy all page addresses to ptr1 */
+		for (i = 0; i < length; i++, list++) {
+			for (p = 0; p * 4096 < sg_dma_len(list); p++, ptr1++)
+				*ptr1 = cpu_to_le32(sg_dma_address(list) - list->offset);
+		}
+/*
+		ptr1 = pt1->cpu;
+		for(j=0;j<40;j++) {
+			printk("ptr1 %d: 0x%08x\n",j,ptr1[j]);
+		}
+*/
+
+		/* if we have a user buffer, the first page may not be
+		   aligned to a page boundary. */
+		pt1->offset = dma->sglist->offset;
+		pt2->offset = pt1->offset+o1;
+		pt3->offset = pt1->offset+o2;
+
+		/* create video-dma2 page table */
+		ptr1 = pt1->cpu;
+		for(i = m1; i <= m2 ; i++, ptr2++) {
+			*ptr2 = ptr1[i];
+		}
+		fill = *(ptr2-1);
+		for(;i<1024;i++,ptr2++) {
+			*ptr2 = fill;
+		}
+		/* create video-dma3 page table */
+		ptr1 = pt1->cpu;
+		for(i = m2; i <= m3; i++,ptr3++) {
+			*ptr3 = ptr1[i];
+		}
+		fill = *(ptr3-1);
+		for(;i<1024;i++,ptr3++) {
+			*ptr3 = fill;
+		}
+		/* finally: finish up video-dma1 page table */
+		ptr1 = pt1->cpu+m1;
+		fill = pt1->cpu[m1];
+		for(i=m1;i<1024;i++,ptr1++) {
+			*ptr1 = fill;
+		}
+/*
+		ptr1 = pt1->cpu;
+		ptr2 = pt2->cpu;
+		ptr3 = pt3->cpu;
+		for(j=0;j<40;j++) {
+			printk("ptr1 %d: 0x%08x\n",j,ptr1[j]);
+		}
+		for(j=0;j<40;j++) {
+			printk("ptr2 %d: 0x%08x\n",j,ptr2[j]);
+		}
+		for(j=0;j<40;j++) {
+			printk("ptr3 %d: 0x%08x\n",j,ptr3[j]);
+		}
+*/
+	} else {
+		struct saa7146_pgtable *pt = &buf->pt[0];
+		return saa7146_pgtable_build_single(pci, pt, list, length);
+	}
+
+	return 0;
+}
+
+
+/********************************************************************************/
+/* file operations */
+
+static int video_begin(struct saa7146_fh *fh)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_format *fmt = NULL;
+	unsigned int resource;
+	int ret = 0, err = 0;
+
+	DEB_EE("dev:%p, fh:%p\n", dev, fh);
+
+	if ((vv->video_status & STATUS_CAPTURE) != 0) {
+		if (vv->video_fh == fh) {
+			DEB_S("already capturing\n");
+			return 0;
+		}
+		DEB_S("already capturing in another open\n");
+		return -EBUSY;
+	}
+
+	if ((vv->video_status & STATUS_OVERLAY) != 0) {
+		DEB_S("warning: suspending overlay video for streaming capture\n");
+		vv->ov_suspend = vv->video_fh;
+		err = saa7146_stop_preview(vv->video_fh); /* side effect: video_status is now 0, video_fh is NULL */
+		if (0 != err) {
+			DEB_D("suspending video failed. aborting\n");
+			return err;
+		}
+	}
+
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
+	/* we need to have a valid format set here */
+	if (!fmt)
+		return -EINVAL;
+
+	if (0 != (fmt->flags & FORMAT_IS_PLANAR)) {
+		resource = RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP|RESOURCE_DMA3_BRS;
+	} else {
+		resource = RESOURCE_DMA1_HPS;
+	}
+
+	ret = saa7146_res_get(fh, resource);
+	if (0 == ret) {
+		DEB_S("cannot get capture resource %d\n", resource);
+		if (vv->ov_suspend != NULL) {
+			saa7146_start_preview(vv->ov_suspend);
+			vv->ov_suspend = NULL;
+		}
+		return -EBUSY;
+	}
+
+	/* clear out beginning of streaming bit (rps register 0)*/
+	saa7146_write(dev, MC2, MASK_27 );
+
+	/* enable rps0 irqs */
+	SAA7146_IER_ENABLE(dev, MASK_27);
+
+	vv->video_fh = fh;
+	vv->video_status = STATUS_CAPTURE;
+
+	return 0;
+}
+
+static int video_end(struct saa7146_fh *fh, struct file *file)
+{
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_dmaqueue *q = &vv->video_dmaq;
+	struct saa7146_format *fmt = NULL;
+	unsigned long flags;
+	unsigned int resource;
+	u32 dmas = 0;
+	DEB_EE("dev:%p, fh:%p\n", dev, fh);
+
+	if ((vv->video_status & STATUS_CAPTURE) != STATUS_CAPTURE) {
+		DEB_S("not capturing\n");
+		return 0;
+	}
+
+	if (vv->video_fh != fh) {
+		DEB_S("capturing, but in another open\n");
+		return -EBUSY;
+	}
+
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
+	/* we need to have a valid format set here */
+	if (!fmt)
+		return -EINVAL;
+
+	if (0 != (fmt->flags & FORMAT_IS_PLANAR)) {
+		resource = RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP|RESOURCE_DMA3_BRS;
+		dmas = MASK_22 | MASK_21 | MASK_20;
+	} else {
+		resource = RESOURCE_DMA1_HPS;
+		dmas = MASK_22;
+	}
+	spin_lock_irqsave(&dev->slock,flags);
+
+	/* disable rps0  */
+	saa7146_write(dev, MC1, MASK_28);
+
+	/* disable rps0 irqs */
+	SAA7146_IER_DISABLE(dev, MASK_27);
+
+	/* shut down all used video dma transfers */
+	saa7146_write(dev, MC1, dmas);
+
+	if (q->curr)
+		saa7146_buffer_finish(dev, q, VIDEOBUF_DONE);
+
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	vv->video_fh = NULL;
+	vv->video_status = 0;
+
+	saa7146_res_free(fh, resource);
+
+	if (vv->ov_suspend != NULL) {
+		saa7146_start_preview(vv->ov_suspend);
+		vv->ov_suspend = NULL;
+	}
+
+	return 0;
+}
+
+static int vidioc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	strscpy((char *)cap->driver, "saa7146 v4l2", sizeof(cap->driver));
+	strscpy((char *)cap->card, dev->ext->name, sizeof(cap->card));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
+			    V4L2_CAP_READWRITE | V4L2_CAP_STREAMING |
+			    V4L2_CAP_DEVICE_CAPS;
+	cap->capabilities |= dev->ext_vv_data->capabilities;
+	return 0;
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *fb)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	*fb = vv->ov_fb;
+	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	fb->flags = V4L2_FBUF_FLAG_PRIMARY;
+	return 0;
+}
+
+static int vidioc_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *fb)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_format *fmt;
+
+	DEB_EE("VIDIOC_S_FBUF\n");
+
+	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	/* check args */
+	fmt = saa7146_format_by_fourcc(dev, fb->fmt.pixelformat);
+	if (NULL == fmt)
+		return -EINVAL;
+
+	/* planar formats are not allowed for overlay video, clipping and video dma would clash */
+	if (fmt->flags & FORMAT_IS_PLANAR)
+		DEB_S("planar pixelformat '%4.4s' not allowed for overlay\n",
+		      (char *)&fmt->pixelformat);
+
+	/* check if overlay is running */
+	if (IS_OVERLAY_ACTIVE(fh) != 0) {
+		if (vv->video_fh != fh) {
+			DEB_D("refusing to change framebuffer information while overlay is active in another open\n");
+			return -EBUSY;
+		}
+	}
+
+	/* ok, accept it */
+	vv->ov_fb = *fb;
+	vv->ov_fmt = fmt;
+
+	if (vv->ov_fb.fmt.bytesperline < vv->ov_fb.fmt.width) {
+		vv->ov_fb.fmt.bytesperline = vv->ov_fb.fmt.width * fmt->depth / 8;
+		DEB_D("setting bytesperline to %d\n", vv->ov_fb.fmt.bytesperline);
+	}
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_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 saa7146_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
+	struct saa7146_vv *vv = dev->vv_data;
+	u32 val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0x00ffffff;
+		val |= (ctrl->val << 24);
+		saa7146_write(dev, BCS_CTRL, val);
+		saa7146_write(dev, MC2, MASK_22 | MASK_06);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xff00ffff;
+		val |= (ctrl->val << 16);
+		saa7146_write(dev, BCS_CTRL, val);
+		saa7146_write(dev, MC2, MASK_22 | MASK_06);
+		break;
+
+	case V4L2_CID_SATURATION:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xffffff00;
+		val |= (ctrl->val << 0);
+		saa7146_write(dev, BCS_CTRL, val);
+		saa7146_write(dev, MC2, MASK_22 | MASK_06);
+		break;
+
+	case V4L2_CID_HFLIP:
+		/* fixme: we can support changing VFLIP and HFLIP here... */
+		if ((vv->video_status & STATUS_CAPTURE))
+			return -EBUSY;
+		vv->hflip = ctrl->val;
+		break;
+
+	case V4L2_CID_VFLIP:
+		if ((vv->video_status & STATUS_CAPTURE))
+			return -EBUSY;
+		vv->vflip = ctrl->val;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if ((vv->video_status & STATUS_OVERLAY) != 0) { /* CHECK: && (vv->video_fh == fh)) */
+		struct saa7146_fh *fh = vv->video_fh;
+
+		saa7146_stop_preview(fh);
+		saa7146_start_preview(fh);
+	}
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *file, void *fh,
+		struct v4l2_streamparm *parm)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	parm->parm.capture.readbuffers = 1;
+	v4l2_video_std_frame_period(vv->standard->id,
+				    &parm->parm.capture.timeperframe);
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.pix = vv->video_fmt;
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_overlay(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.win = vv->ov.win;
+	return 0;
+}
+
+static int vidioc_g_fmt_vbi_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.vbi = vv->vbi_fmt;
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_format *fmt;
+	enum v4l2_field field;
+	int maxw, maxh;
+	int calc_bpl;
+
+	DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: dev:%p, fh:%p\n", dev, fh);
+
+	fmt = saa7146_format_by_fourcc(dev, f->fmt.pix.pixelformat);
+	if (NULL == fmt)
+		return -EINVAL;
+
+	field = f->fmt.pix.field;
+	maxw  = vv->standard->h_max_out;
+	maxh  = vv->standard->v_max_out;
+
+	if (V4L2_FIELD_ANY == field) {
+		field = (f->fmt.pix.height > maxh / 2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_BOTTOM;
+	}
+	switch (field) {
+	case V4L2_FIELD_ALTERNATE:
+		vv->last_field = V4L2_FIELD_TOP;
+		maxh = maxh / 2;
+		break;
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		vv->last_field = V4L2_FIELD_INTERLACED;
+		maxh = maxh / 2;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		vv->last_field = V4L2_FIELD_INTERLACED;
+		break;
+	default:
+		DEB_D("no known field mode '%d'\n", field);
+		return -EINVAL;
+	}
+
+	f->fmt.pix.field = field;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	if (f->fmt.pix.width > maxw)
+		f->fmt.pix.width = maxw;
+	if (f->fmt.pix.height > maxh)
+		f->fmt.pix.height = maxh;
+
+	calc_bpl = (f->fmt.pix.width * fmt->depth) / 8;
+
+	if (f->fmt.pix.bytesperline < calc_bpl)
+		f->fmt.pix.bytesperline = calc_bpl;
+
+	if (f->fmt.pix.bytesperline > (2 * PAGE_SIZE * fmt->depth) / 8) /* arbitrary constraint */
+		f->fmt.pix.bytesperline = calc_bpl;
+
+	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
+	DEB_D("w:%d, h:%d, bytesperline:%d, sizeimage:%d\n",
+	      f->fmt.pix.width, f->fmt.pix.height,
+	      f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
+
+	return 0;
+}
+
+
+static int vidioc_try_fmt_vid_overlay(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct v4l2_window *win = &f->fmt.win;
+	enum v4l2_field field;
+	int maxw, maxh;
+
+	DEB_EE("dev:%p\n", dev);
+
+	if (NULL == vv->ov_fb.base) {
+		DEB_D("no fb base set\n");
+		return -EINVAL;
+	}
+	if (NULL == vv->ov_fmt) {
+		DEB_D("no fb fmt set\n");
+		return -EINVAL;
+	}
+	if (win->w.width < 48 || win->w.height < 32) {
+		DEB_D("min width/height. (%d,%d)\n",
+		      win->w.width, win->w.height);
+		return -EINVAL;
+	}
+	if (win->clipcount > 16) {
+		DEB_D("clipcount too big\n");
+		return -EINVAL;
+	}
+
+	field = win->field;
+	maxw  = vv->standard->h_max_out;
+	maxh  = vv->standard->v_max_out;
+
+	if (V4L2_FIELD_ANY == field) {
+		field = (win->w.height > maxh / 2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_TOP;
+		}
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_ALTERNATE:
+		maxh = maxh / 2;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		DEB_D("no known field mode '%d'\n", field);
+		return -EINVAL;
+	}
+
+	win->field = field;
+	if (win->w.width > maxw)
+		win->w.width = maxw;
+	if (win->w.height > maxh)
+		win->w.height = maxh;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *__fh, struct v4l2_format *f)
+{
+	struct saa7146_fh *fh = __fh;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	int err;
+
+	DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: dev:%p, fh:%p\n", dev, fh);
+	if (IS_CAPTURE_ACTIVE(fh) != 0) {
+		DEB_EE("streaming capture is active\n");
+		return -EBUSY;
+	}
+	err = vidioc_try_fmt_vid_cap(file, fh, f);
+	if (0 != err)
+		return err;
+	vv->video_fmt = f->fmt.pix;
+	DEB_EE("set to pixelformat '%4.4s'\n",
+	       (char *)&vv->video_fmt.pixelformat);
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_overlay(struct file *file, void *__fh, struct v4l2_format *f)
+{
+	struct saa7146_fh *fh = __fh;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	int err;
+
+	DEB_EE("V4L2_BUF_TYPE_VIDEO_OVERLAY: dev:%p, fh:%p\n", dev, fh);
+	err = vidioc_try_fmt_vid_overlay(file, fh, f);
+	if (0 != err)
+		return err;
+	vv->ov.win    = f->fmt.win;
+	vv->ov.nclips = f->fmt.win.clipcount;
+	if (vv->ov.nclips > 16)
+		vv->ov.nclips = 16;
+	memcpy(vv->ov.clips, f->fmt.win.clips,
+	       sizeof(struct v4l2_clip) * vv->ov.nclips);
+
+	/* vv->ov.fh is used to indicate that we have valid overlay information, too */
+	vv->ov.fh = fh;
+
+	/* check if our current overlay is active */
+	if (IS_OVERLAY_ACTIVE(fh) != 0) {
+		saa7146_stop_preview(fh);
+		saa7146_start_preview(fh);
+	}
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	*norm = vv->standard->id;
+	return 0;
+}
+
+	/* the saa7146 supfhrts (used in conjunction with the saa7111a for example)
+	   PAL / NTSC / SECAM. if your hardware does not (or does more)
+	   -- override this function in your extension */
+/*
+	case VIDIOC_ENUMSTD:
+	{
+		struct v4l2_standard *e = arg;
+		if (e->index < 0 )
+			return -EINVAL;
+		if( e->index < dev->ext_vv_data->num_stds ) {
+			DEB_EE("VIDIOC_ENUMSTD: index:%d\n", e->index);
+			v4l2_video_std_construct(e, dev->ext_vv_data->stds[e->index].id, dev->ext_vv_data->stds[e->index].name);
+			return 0;
+		}
+		return -EINVAL;
+	}
+	*/
+
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	int found = 0;
+	int err, i;
+
+	DEB_EE("VIDIOC_S_STD\n");
+
+	if ((vv->video_status & STATUS_CAPTURE) == STATUS_CAPTURE) {
+		DEB_D("cannot change video standard while streaming capture is active\n");
+		return -EBUSY;
+	}
+
+	if ((vv->video_status & STATUS_OVERLAY) != 0) {
+		vv->ov_suspend = vv->video_fh;
+		err = saa7146_stop_preview(vv->video_fh); /* side effect: video_status is now 0, video_fh is NULL */
+		if (0 != err) {
+			DEB_D("suspending video failed. aborting\n");
+			return err;
+		}
+	}
+
+	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
+		if (id & dev->ext_vv_data->stds[i].id)
+			break;
+	if (i != dev->ext_vv_data->num_stds) {
+		vv->standard = &dev->ext_vv_data->stds[i];
+		if (NULL != dev->ext_vv_data->std_callback)
+			dev->ext_vv_data->std_callback(dev, vv->standard);
+		found = 1;
+	}
+
+	if (vv->ov_suspend != NULL) {
+		saa7146_start_preview(vv->ov_suspend);
+		vv->ov_suspend = NULL;
+	}
+
+	if (!found) {
+		DEB_EE("VIDIOC_S_STD: standard not found\n");
+		return -EINVAL;
+	}
+
+	DEB_EE("VIDIOC_S_STD: set to standard to '%s'\n", vv->standard->name);
+	return 0;
+}
+
+static int vidioc_overlay(struct file *file, void *fh, unsigned int on)
+{
+	int err;
+
+	DEB_D("VIDIOC_OVERLAY on:%d\n", on);
+	if (on)
+		err = saa7146_start_preview(fh);
+	else
+		err = saa7146_stop_preview(fh);
+	return err;
+}
+
+static int vidioc_reqbufs(struct file *file, void *__fh, struct v4l2_requestbuffers *b)
+{
+	struct saa7146_fh *fh = __fh;
+
+	if (b->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return videobuf_reqbufs(&fh->video_q, b);
+	if (b->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		return videobuf_reqbufs(&fh->vbi_q, b);
+	return -EINVAL;
+}
+
+static int vidioc_querybuf(struct file *file, void *__fh, struct v4l2_buffer *buf)
+{
+	struct saa7146_fh *fh = __fh;
+
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return videobuf_querybuf(&fh->video_q, buf);
+	if (buf->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		return videobuf_querybuf(&fh->vbi_q, buf);
+	return -EINVAL;
+}
+
+static int vidioc_qbuf(struct file *file, void *__fh, struct v4l2_buffer *buf)
+{
+	struct saa7146_fh *fh = __fh;
+
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return videobuf_qbuf(&fh->video_q, buf);
+	if (buf->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		return videobuf_qbuf(&fh->vbi_q, buf);
+	return -EINVAL;
+}
+
+static int vidioc_dqbuf(struct file *file, void *__fh, struct v4l2_buffer *buf)
+{
+	struct saa7146_fh *fh = __fh;
+
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return videobuf_dqbuf(&fh->video_q, buf, file->f_flags & O_NONBLOCK);
+	if (buf->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		return videobuf_dqbuf(&fh->vbi_q, buf, file->f_flags & O_NONBLOCK);
+	return -EINVAL;
+}
+
+static int vidioc_streamon(struct file *file, void *__fh, enum v4l2_buf_type type)
+{
+	struct saa7146_fh *fh = __fh;
+	int err;
+
+	DEB_D("VIDIOC_STREAMON, type:%d\n", type);
+
+	err = video_begin(fh);
+	if (err)
+		return err;
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return videobuf_streamon(&fh->video_q);
+	if (type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		return videobuf_streamon(&fh->vbi_q);
+	return -EINVAL;
+}
+
+static int vidioc_streamoff(struct file *file, void *__fh, enum v4l2_buf_type type)
+{
+	struct saa7146_fh *fh = __fh;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	int err;
+
+	DEB_D("VIDIOC_STREAMOFF, type:%d\n", type);
+
+	/* ugly: we need to copy some checks from video_end(),
+	   because videobuf_streamoff() relies on the capture running.
+	   check and fix this */
+	if ((vv->video_status & STATUS_CAPTURE) != STATUS_CAPTURE) {
+		DEB_S("not capturing\n");
+		return 0;
+	}
+
+	if (vv->video_fh != fh) {
+		DEB_S("capturing, but in another open\n");
+		return -EBUSY;
+	}
+
+	err = -EINVAL;
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		err = videobuf_streamoff(&fh->video_q);
+	else if (type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		err = videobuf_streamoff(&fh->vbi_q);
+	if (0 != err) {
+		DEB_D("warning: videobuf_streamoff() failed\n");
+		video_end(fh, file);
+	} else {
+		err = video_end(fh, file);
+	}
+	return err;
+}
+
+const struct v4l2_ioctl_ops saa7146_video_ioctl_ops = {
+	.vidioc_querycap             = vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap     = vidioc_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_overlay = 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_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_overlay		     = vidioc_overlay,
+	.vidioc_g_fbuf		     = vidioc_g_fbuf,
+	.vidioc_s_fbuf		     = vidioc_s_fbuf,
+	.vidioc_reqbufs              = vidioc_reqbufs,
+	.vidioc_querybuf             = vidioc_querybuf,
+	.vidioc_qbuf                 = vidioc_qbuf,
+	.vidioc_dqbuf                = vidioc_dqbuf,
+	.vidioc_g_std                = vidioc_g_std,
+	.vidioc_s_std                = vidioc_s_std,
+	.vidioc_streamon             = vidioc_streamon,
+	.vidioc_streamoff            = vidioc_streamoff,
+	.vidioc_g_parm		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+};
+
+const struct v4l2_ioctl_ops saa7146_vbi_ioctl_ops = {
+	.vidioc_querycap             = vidioc_querycap,
+	.vidioc_g_fmt_vbi_cap        = vidioc_g_fmt_vbi_cap,
+
+	.vidioc_reqbufs              = vidioc_reqbufs,
+	.vidioc_querybuf             = vidioc_querybuf,
+	.vidioc_qbuf                 = vidioc_qbuf,
+	.vidioc_dqbuf                = vidioc_dqbuf,
+	.vidioc_g_std                = vidioc_g_std,
+	.vidioc_s_std                = vidioc_s_std,
+	.vidioc_streamon             = vidioc_streamon,
+	.vidioc_streamoff            = vidioc_streamoff,
+	.vidioc_g_parm		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+};
+
+/*********************************************************************************/
+/* buffer handling functions                                                  */
+
+static int buffer_activate (struct saa7146_dev *dev,
+		     struct saa7146_buf *buf,
+		     struct saa7146_buf *next)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+
+	buf->vb.state = VIDEOBUF_ACTIVE;
+	saa7146_set_capture(dev,buf,next);
+
+	mod_timer(&vv->video_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
+	return 0;
+}
+
+static void release_all_pagetables(struct saa7146_dev *dev, struct saa7146_buf *buf)
+{
+	saa7146_pgtable_free(dev->pci, &buf->pt[0]);
+	saa7146_pgtable_free(dev->pci, &buf->pt[1]);
+	saa7146_pgtable_free(dev->pci, &buf->pt[2]);
+}
+
+static int buffer_prepare(struct videobuf_queue *q,
+			  struct videobuf_buffer *vb, enum v4l2_field field)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+	int size,err = 0;
+
+	DEB_CAP("vbuf:%p\n", vb);
+
+	/* sanity checks */
+	if (vv->video_fmt.width  < 48 ||
+	    vv->video_fmt.height < 32 ||
+	    vv->video_fmt.width  > vv->standard->h_max_out ||
+	    vv->video_fmt.height > vv->standard->v_max_out) {
+		DEB_D("w (%d) / h (%d) out of bounds\n",
+		      vv->video_fmt.width, vv->video_fmt.height);
+		return -EINVAL;
+	}
+
+	size = vv->video_fmt.sizeimage;
+	if (0 != buf->vb.baddr && buf->vb.bsize < size) {
+		DEB_D("size mismatch\n");
+		return -EINVAL;
+	}
+
+	DEB_CAP("buffer_prepare [size=%dx%d,bytes=%d,fields=%s]\n",
+		vv->video_fmt.width, vv->video_fmt.height,
+		size, v4l2_field_names[vv->video_fmt.field]);
+	if (buf->vb.width  != vv->video_fmt.width  ||
+	    buf->vb.bytesperline != vv->video_fmt.bytesperline ||
+	    buf->vb.height != vv->video_fmt.height ||
+	    buf->vb.size   != size ||
+	    buf->vb.field  != field      ||
+	    buf->vb.field  != vv->video_fmt.field  ||
+	    buf->fmt       != &vv->video_fmt) {
+		saa7146_dma_free(dev,q,buf);
+	}
+
+	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
+		struct saa7146_format *sfmt;
+
+		buf->vb.bytesperline  = vv->video_fmt.bytesperline;
+		buf->vb.width  = vv->video_fmt.width;
+		buf->vb.height = vv->video_fmt.height;
+		buf->vb.size   = size;
+		buf->vb.field  = field;
+		buf->fmt       = &vv->video_fmt;
+		buf->vb.field  = vv->video_fmt.field;
+
+		sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
+
+		release_all_pagetables(dev, buf);
+		if( 0 != IS_PLANAR(sfmt->trans)) {
+			saa7146_pgtable_alloc(dev->pci, &buf->pt[0]);
+			saa7146_pgtable_alloc(dev->pci, &buf->pt[1]);
+			saa7146_pgtable_alloc(dev->pci, &buf->pt[2]);
+		} else {
+			saa7146_pgtable_alloc(dev->pci, &buf->pt[0]);
+		}
+
+		err = videobuf_iolock(q,&buf->vb, &vv->ov_fb);
+		if (err)
+			goto oops;
+		err = saa7146_pgtable_build(dev,buf);
+		if (err)
+			goto oops;
+	}
+	buf->vb.state = VIDEOBUF_PREPARED;
+	buf->activate = buffer_activate;
+
+	return 0;
+
+ oops:
+	DEB_D("error out\n");
+	saa7146_dma_free(dev,q,buf);
+
+	return err;
+}
+
+static int buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
+
+	if (0 == *count || *count > MAX_SAA7146_CAPTURE_BUFFERS)
+		*count = MAX_SAA7146_CAPTURE_BUFFERS;
+
+	*size = vv->video_fmt.sizeimage;
+
+	/* check if we exceed the "max_memory" parameter */
+	if( (*count * *size) > (max_memory*1048576) ) {
+		*count = (max_memory*1048576) / *size;
+	}
+
+	DEB_CAP("%d buffers, %d bytes each\n", *count, *size);
+
+	return 0;
+}
+
+static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+
+	DEB_CAP("vbuf:%p\n", vb);
+	saa7146_buffer_queue(fh->dev, &vv->video_dmaq, buf);
+}
+
+static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	struct file *file = q->priv_data;
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
+
+	DEB_CAP("vbuf:%p\n", vb);
+
+	saa7146_dma_free(dev,q,buf);
+
+	release_all_pagetables(dev, buf);
+}
+
+static const struct videobuf_queue_ops video_qops = {
+	.buf_setup    = buffer_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_queue    = buffer_queue,
+	.buf_release  = buffer_release,
+};
+
+/********************************************************************************/
+/* file operations */
+
+static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
+{
+	INIT_LIST_HEAD(&vv->video_dmaq.queue);
+
+	timer_setup(&vv->video_dmaq.timeout, saa7146_buffer_timeout, 0);
+	vv->video_dmaq.dev              = dev;
+
+	/* set some default values */
+	vv->standard = &dev->ext_vv_data->stds[0];
+
+	/* FIXME: what's this? */
+	vv->current_hps_source = SAA7146_HPS_SOURCE_PORT_A;
+	vv->current_hps_sync = SAA7146_HPS_SYNC_PORT_A;
+}
+
+
+static int video_open(struct saa7146_dev *dev, struct file *file)
+{
+	struct saa7146_fh *fh = file->private_data;
+
+	videobuf_queue_sg_init(&fh->video_q, &video_qops,
+			    &dev->pci->dev, &dev->slock,
+			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			    V4L2_FIELD_INTERLACED,
+			    sizeof(struct saa7146_buf),
+			    file, &dev->v4l2_lock);
+
+	return 0;
+}
+
+
+static void video_close(struct saa7146_dev *dev, struct file *file)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = dev->vv_data;
+	struct videobuf_queue *q = &fh->video_q;
+
+	if (IS_CAPTURE_ACTIVE(fh) != 0)
+		video_end(fh, file);
+	else if (IS_OVERLAY_ACTIVE(fh) != 0)
+		saa7146_stop_preview(fh);
+
+	videobuf_stop(q);
+	/* hmm, why is this function declared void? */
+}
+
+
+static void video_irq_done(struct saa7146_dev *dev, unsigned long st)
+{
+	struct saa7146_vv *vv = dev->vv_data;
+	struct saa7146_dmaqueue *q = &vv->video_dmaq;
+
+	spin_lock(&dev->slock);
+	DEB_CAP("called\n");
+
+	/* only finish the buffer if we have one... */
+	if( NULL != q->curr ) {
+		saa7146_buffer_finish(dev,q,VIDEOBUF_DONE);
+	}
+	saa7146_buffer_next(dev,q,0);
+
+	spin_unlock(&dev->slock);
+}
+
+static ssize_t video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
+{
+	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_dev *dev = fh->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+	ssize_t ret = 0;
+
+	DEB_EE("called\n");
+
+	if ((vv->video_status & STATUS_CAPTURE) != 0) {
+		/* fixme: should we allow read() captures while streaming capture? */
+		if (vv->video_fh == fh) {
+			DEB_S("already capturing\n");
+			return -EBUSY;
+		}
+		DEB_S("already capturing in another open\n");
+		return -EBUSY;
+	}
+
+	ret = video_begin(fh);
+	if( 0 != ret) {
+		goto out;
+	}
+
+	ret = videobuf_read_one(&fh->video_q , data, count, ppos,
+				file->f_flags & O_NONBLOCK);
+	if (ret != 0) {
+		video_end(fh, file);
+	} else {
+		ret = video_end(fh, file);
+	}
+out:
+	/* restart overlay if it was active before */
+	if (vv->ov_suspend != NULL) {
+		saa7146_start_preview(vv->ov_suspend);
+		vv->ov_suspend = NULL;
+	}
+
+	return ret;
+}
+
+const struct saa7146_use_ops saa7146_video_uops = {
+	.init = video_init,
+	.open = video_open,
+	.release = video_close,
+	.irq_done = video_irq_done,
+	.read = video_read,
+};
diff --git a/drivers/staging/media/deprecated/saa7146/common/saa7146_vv.h b/drivers/staging/media/deprecated/saa7146/common/saa7146_vv.h
new file mode 100644
index 000000000..d7bd916fe
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/common/saa7146_vv.h
@@ -0,0 +1,266 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __SAA7146_VV__
+#define __SAA7146_VV__
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf-dma-sg.h>
+#include "saa7146.h"
+
+#define MAX_SAA7146_CAPTURE_BUFFERS	32	/* arbitrary */
+#define BUFFER_TIMEOUT     (HZ/2)  /* 0.5 seconds */
+
+#define WRITE_RPS0(x) do { \
+	dev->d_rps0.cpu_addr[ count++ ] = cpu_to_le32(x); \
+	} while (0);
+
+#define WRITE_RPS1(x) do { \
+	dev->d_rps1.cpu_addr[ count++ ] = cpu_to_le32(x); \
+	} while (0);
+
+struct	saa7146_video_dma {
+	u32 base_odd;
+	u32 base_even;
+	u32 prot_addr;
+	u32 pitch;
+	u32 base_page;
+	u32 num_line_byte;
+};
+
+#define FORMAT_BYTE_SWAP	0x1
+#define FORMAT_IS_PLANAR	0x2
+
+struct saa7146_format {
+	u32	pixelformat;
+	u32	trans;
+	u8	depth;
+	u8	flags;
+	u8	swap;
+};
+
+struct saa7146_standard
+{
+	char          *name;
+	v4l2_std_id   id;
+
+	int v_offset;	/* number of lines of vertical offset before processing */
+	int v_field;	/* number of lines in a field for HPS to process */
+
+	int h_offset;	/* horizontal offset of processing window */
+	int h_pixels;	/* number of horizontal pixels to process */
+
+	int v_max_out;
+	int h_max_out;
+};
+
+/* buffer for one video/vbi frame */
+struct saa7146_buf {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer vb;
+
+	/* saa7146 specific */
+	struct v4l2_pix_format  *fmt;
+	int (*activate)(struct saa7146_dev *dev,
+			struct saa7146_buf *buf,
+			struct saa7146_buf *next);
+
+	/* page tables */
+	struct saa7146_pgtable  pt[3];
+};
+
+struct saa7146_dmaqueue {
+	struct saa7146_dev	*dev;
+	struct saa7146_buf	*curr;
+	struct list_head	queue;
+	struct timer_list	timeout;
+};
+
+struct saa7146_overlay {
+	struct saa7146_fh	*fh;
+	struct v4l2_window	win;
+	struct v4l2_clip	clips[16];
+	int			nclips;
+};
+
+/* per open data */
+struct saa7146_fh {
+	/* Must be the first field! */
+	struct v4l2_fh		fh;
+	struct saa7146_dev	*dev;
+
+	/* video capture */
+	struct videobuf_queue	video_q;
+
+	/* vbi capture */
+	struct videobuf_queue	vbi_q;
+
+	unsigned int resources;	/* resource management for device open */
+};
+
+#define STATUS_OVERLAY	0x01
+#define STATUS_CAPTURE	0x02
+
+struct saa7146_vv
+{
+	/* vbi capture */
+	struct saa7146_dmaqueue		vbi_dmaq;
+	struct v4l2_vbi_format		vbi_fmt;
+	struct timer_list		vbi_read_timeout;
+	struct file			*vbi_read_timeout_file;
+	/* vbi workaround interrupt queue */
+	wait_queue_head_t		vbi_wq;
+	int				vbi_fieldcount;
+	struct saa7146_fh		*vbi_streaming;
+
+	int				video_status;
+	struct saa7146_fh		*video_fh;
+
+	/* video overlay */
+	struct saa7146_overlay		ov;
+	struct v4l2_framebuffer		ov_fb;
+	struct saa7146_format		*ov_fmt;
+	struct saa7146_fh		*ov_suspend;
+
+	/* video capture */
+	struct saa7146_dmaqueue		video_dmaq;
+	struct v4l2_pix_format		video_fmt;
+	enum v4l2_field			last_field;
+
+	/* common: fixme? shouldn't this be in saa7146_fh?
+	   (this leads to a more complicated question: shall the driver
+	   store the different settings (for example S_INPUT) for every open
+	   and restore it appropriately, or should all settings be common for
+	   all opens? currently, we do the latter, like all other
+	   drivers do... */
+	struct saa7146_standard	*standard;
+
+	int	vflip;
+	int	hflip;
+	int	current_hps_source;
+	int	current_hps_sync;
+
+	struct saa7146_dma	d_clipping;	/* pointer to clipping memory */
+
+	unsigned int resources;	/* resource management for device */
+};
+
+/* flags */
+#define SAA7146_USE_PORT_B_FOR_VBI	0x2     /* use input port b for vbi hardware bug workaround */
+
+struct saa7146_ext_vv
+{
+	/* information about the video capabilities of the device */
+	int	inputs;
+	int	audios;
+	u32	capabilities;
+	int	flags;
+
+	/* additionally supported transmission standards */
+	struct saa7146_standard *stds;
+	int num_stds;
+	int (*std_callback)(struct saa7146_dev*, struct saa7146_standard *);
+
+	/* the extension can override this */
+	struct v4l2_ioctl_ops vid_ops;
+	struct v4l2_ioctl_ops vbi_ops;
+	/* pointer to the saa7146 core ops */
+	const struct v4l2_ioctl_ops *core_ops;
+
+	struct v4l2_file_operations vbi_fops;
+};
+
+struct saa7146_use_ops  {
+	void (*init)(struct saa7146_dev *, struct saa7146_vv *);
+	int(*open)(struct saa7146_dev *, struct file *);
+	void (*release)(struct saa7146_dev *, struct file *);
+	void (*irq_done)(struct saa7146_dev *, unsigned long status);
+	ssize_t (*read)(struct file *, char __user *, size_t, loff_t *);
+};
+
+/* from saa7146_fops.c */
+int saa7146_register_device(struct video_device *vid, struct saa7146_dev *dev, char *name, int type);
+int saa7146_unregister_device(struct video_device *vid, struct saa7146_dev *dev);
+void saa7146_buffer_finish(struct saa7146_dev *dev, struct saa7146_dmaqueue *q, int state);
+void saa7146_buffer_next(struct saa7146_dev *dev, struct saa7146_dmaqueue *q,int vbi);
+int saa7146_buffer_queue(struct saa7146_dev *dev, struct saa7146_dmaqueue *q, struct saa7146_buf *buf);
+void saa7146_buffer_timeout(struct timer_list *t);
+void saa7146_dma_free(struct saa7146_dev* dev,struct videobuf_queue *q,
+						struct saa7146_buf *buf);
+
+int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv);
+int saa7146_vv_release(struct saa7146_dev* dev);
+
+/* from saa7146_hlp.c */
+int saa7146_enable_overlay(struct saa7146_fh *fh);
+void saa7146_disable_overlay(struct saa7146_fh *fh);
+
+void saa7146_set_capture(struct saa7146_dev *dev, struct saa7146_buf *buf, struct saa7146_buf *next);
+void saa7146_write_out_dma(struct saa7146_dev* dev, int which, struct saa7146_video_dma* vdma) ;
+void saa7146_set_hps_source_and_sync(struct saa7146_dev *saa, int source, int sync);
+void saa7146_set_gpio(struct saa7146_dev *saa, u8 pin, u8 data);
+
+/* from saa7146_video.c */
+extern const struct v4l2_ioctl_ops saa7146_video_ioctl_ops;
+extern const struct v4l2_ioctl_ops saa7146_vbi_ioctl_ops;
+extern const struct saa7146_use_ops saa7146_video_uops;
+int saa7146_start_preview(struct saa7146_fh *fh);
+int saa7146_stop_preview(struct saa7146_fh *fh);
+long saa7146_video_do_ioctl(struct file *file, unsigned int cmd, void *arg);
+int saa7146_s_ctrl(struct v4l2_ctrl *ctrl);
+
+/* from saa7146_vbi.c */
+extern const struct saa7146_use_ops saa7146_vbi_uops;
+
+/* resource management functions */
+int saa7146_res_get(struct saa7146_fh *fh, unsigned int bit);
+void saa7146_res_free(struct saa7146_fh *fh, unsigned int bits);
+
+#define RESOURCE_DMA1_HPS	0x1
+#define RESOURCE_DMA2_CLP	0x2
+#define RESOURCE_DMA3_BRS	0x4
+
+/* saa7146 source inputs */
+#define SAA7146_HPS_SOURCE_PORT_A	0x00
+#define SAA7146_HPS_SOURCE_PORT_B	0x01
+#define SAA7146_HPS_SOURCE_YPB_CPA	0x02
+#define SAA7146_HPS_SOURCE_YPA_CPB	0x03
+
+/* sync inputs */
+#define SAA7146_HPS_SYNC_PORT_A		0x00
+#define SAA7146_HPS_SYNC_PORT_B		0x01
+
+/* some memory sizes */
+/* max. 16 clipping rectangles */
+#define SAA7146_CLIPPING_MEM	(16 * 4 * sizeof(u32))
+
+/* some defines for the various clipping-modes */
+#define SAA7146_CLIPPING_RECT		0x4
+#define SAA7146_CLIPPING_RECT_INVERTED	0x5
+#define SAA7146_CLIPPING_MASK		0x6
+#define SAA7146_CLIPPING_MASK_INVERTED	0x7
+
+/* output formats: each entry holds four information */
+#define RGB08_COMPOSED	0x0217 /* composed is used in the sense of "not-planar" */
+/* this means: planar?=0, yuv2rgb-conversation-mode=2, dither=yes(=1), format-mode = 7 */
+#define RGB15_COMPOSED	0x0213
+#define RGB16_COMPOSED	0x0210
+#define RGB24_COMPOSED	0x0201
+#define RGB32_COMPOSED	0x0202
+
+#define Y8			0x0006
+#define YUV411_COMPOSED		0x0003
+#define YUV422_COMPOSED		0x0000
+/* this means: planar?=1, yuv2rgb-conversion-mode=0, dither=no(=0), format-mode = b */
+#define YUV411_DECOMPOSED	0x100b
+#define YUV422_DECOMPOSED	0x1009
+#define YUV420_DECOMPOSED	0x100a
+
+#define IS_PLANAR(x) (x & 0xf000)
+
+/* misc defines */
+#define SAA7146_NO_SWAP		(0x0)
+#define SAA7146_TWO_BYTE_SWAP	(0x1)
+#define SAA7146_FOUR_BYTE_SWAP	(0x2)
+
+#endif
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/Kconfig b/drivers/staging/media/deprecated/saa7146/saa7146/Kconfig
new file mode 100644
index 000000000..228e8d3f8
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/Kconfig
@@ -0,0 +1,48 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_HEXIUM_GEMINI
+	tristate "Hexium Gemini frame grabber (DEPRECATED)"
+	depends on PCI && VIDEO_DEV && I2C
+	select VIDEO_SAA7146_VV
+	help
+	  This is a video4linux driver for the Hexium Gemini frame
+	  grabber card by Hexium. Please note that the Gemini Dual
+	  card is *not* fully supported.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called hexium_gemini.
+
+config VIDEO_HEXIUM_ORION
+	tristate "Hexium HV-PCI6 and Orion frame grabber (DEPRECATED)"
+	depends on PCI && VIDEO_DEV && I2C
+	select VIDEO_SAA7146_VV
+	help
+	  This is a video4linux driver for the Hexium HV-PCI6 and
+	  Orion frame grabber cards by Hexium.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called hexium_orion.
+
+config VIDEO_MXB
+	tristate "Siemens-Nixdorf 'Multimedia eXtension Board' (DEPRECATED)"
+	depends on PCI && VIDEO_DEV && I2C
+	select VIDEO_SAA7146_VV
+	select VIDEO_TUNER
+	select VIDEO_SAA711X if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TDA9840 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TEA6415C if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TEA6420 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  This is a video4linux driver for the 'Multimedia eXtension Board'
+	  TV card by Siemens-Nixdorf.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mxb.
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/Makefile b/drivers/staging/media/deprecated/saa7146/saa7146/Makefile
new file mode 100644
index 000000000..37c9336f8
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_VIDEO_MXB) += mxb.o
+obj-$(CONFIG_VIDEO_HEXIUM_ORION) += hexium_orion.o
+obj-$(CONFIG_VIDEO_HEXIUM_GEMINI) += hexium_gemini.o
+
+ccflags-y += -I$(srctree)/drivers/media/i2c
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/TODO b/drivers/staging/media/deprecated/saa7146/saa7146/TODO
new file mode 100644
index 000000000..c9ae2ec79
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/TODO
@@ -0,0 +1,7 @@
+The saa7146-based drivers are one of the few drivers still not using
+the vb2 framework, so these drivers are now deprecated with the intent of
+removing them altogether by the beginning of 2023.
+
+In order to keep these drivers they have to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/hexium_gemini.c b/drivers/staging/media/deprecated/saa7146/saa7146/hexium_gemini.c
new file mode 100644
index 000000000..124e82bd4
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/hexium_gemini.c
@@ -0,0 +1,425 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    hexium_gemini.c - v4l2 driver for Hexium Gemini frame grabber cards
+
+    Visit http://www.mihu.de/linux/saa7146/ and follow the link
+    to "hexium" for further details about this card.
+
+    Copyright (C) 2003 Michael Hunold <michael@mihu.de>
+
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#define DEBUG_VARIABLE debug
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include "../common/saa7146_vv.h"
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "debug verbosity");
+
+/* global variables */
+static int hexium_num;
+
+#define HEXIUM_GEMINI			4
+#define HEXIUM_GEMINI_DUAL		5
+
+#define HEXIUM_INPUTS	9
+static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+};
+
+#define HEXIUM_AUDIOS	0
+
+struct hexium_data
+{
+	s8 adr;
+	u8 byte;
+};
+
+#define HEXIUM_GEMINI_V_1_0		1
+#define HEXIUM_GEMINI_DUAL_V_1_0	2
+
+struct hexium
+{
+	int type;
+
+	struct video_device	video_dev;
+	struct i2c_adapter	i2c_adapter;
+
+	int		cur_input;	/* current input */
+	v4l2_std_id	cur_std;	/* current standard */
+};
+
+/* Samsung KS0127B decoder default registers */
+static u8 hexium_ks0127b[0x100]={
+/*00*/ 0x00,0x52,0x30,0x40,0x01,0x0C,0x2A,0x10,
+/*08*/ 0x00,0x00,0x00,0x60,0x00,0x00,0x0F,0x06,
+/*10*/ 0x00,0x00,0xE4,0xC0,0x00,0x00,0x00,0x00,
+/*18*/ 0x14,0x9B,0xFE,0xFF,0xFC,0xFF,0x03,0x22,
+/*20*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*28*/ 0x00,0x00,0x00,0x00,0x00,0x2C,0x9B,0x00,
+/*30*/ 0x00,0x00,0x10,0x80,0x80,0x10,0x80,0x80,
+/*38*/ 0x01,0x04,0x00,0x00,0x00,0x29,0xC0,0x00,
+/*40*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*48*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*50*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*58*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*60*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*68*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*70*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*78*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*80*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*88*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*90*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*98*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*A0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*A8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*B0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*B8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*C0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*C8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*D0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*D8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*E0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*E8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*F0*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+/*F8*/ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
+};
+
+static struct hexium_data hexium_pal[] = {
+	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
+};
+
+static struct hexium_data hexium_ntsc[] = {
+	{ 0x01, 0x53 }, { 0x12, 0x04 }, { 0x2D, 0x23 }, { 0x2E, 0x81 }, { -1 , 0xFF }
+};
+
+static struct hexium_data hexium_secam[] = {
+	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
+};
+
+static struct hexium_data hexium_input_select[] = {
+	{ 0x02, 0x60 },
+	{ 0x02, 0x64 },
+	{ 0x02, 0x61 },
+	{ 0x02, 0x65 },
+	{ 0x02, 0x62 },
+	{ 0x02, 0x66 },
+	{ 0x02, 0x68 },
+	{ 0x02, 0x69 },
+	{ 0x02, 0x6A },
+};
+
+/* fixme: h_offset = 0 for Hexium Gemini *Dual*, which
+   are currently *not* supported*/
+static struct saa7146_standard hexium_standards[] = {
+	{
+		.name	= "PAL",	.id	= V4L2_STD_PAL,
+		.v_offset	= 28,	.v_field	= 288,
+		.h_offset	= 1,	.h_pixels	= 680,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id	= V4L2_STD_NTSC,
+		.v_offset	= 28,	.v_field	= 240,
+		.h_offset	= 1,	.h_pixels	= 640,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}, {
+		.name	= "SECAM",	.id	= V4L2_STD_SECAM,
+		.v_offset	= 28,	.v_field	= 288,
+		.h_offset	= 1,	.h_pixels	= 720,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}
+};
+
+/* bring hardware to a sane state. this has to be done, just in case someone
+   wants to capture from this device before it has been properly initialized.
+   the capture engine would badly fail, because no valid signal arrives on the
+   saa7146, thus leading to timeouts and stuff. */
+static int hexium_init_done(struct saa7146_dev *dev)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+	union i2c_smbus_data data;
+	int i = 0;
+
+	DEB_D("hexium_init_done called\n");
+
+	/* initialize the helper ics to useful values */
+	for (i = 0; i < sizeof(hexium_ks0127b); i++) {
+		data.byte = hexium_ks0127b[i];
+		if (0 != i2c_smbus_xfer(&hexium->i2c_adapter, 0x6c, 0, I2C_SMBUS_WRITE, i, I2C_SMBUS_BYTE_DATA, &data)) {
+			pr_err("hexium_init_done() failed for address 0x%02x\n",
+			       i);
+		}
+	}
+
+	return 0;
+}
+
+static int hexium_set_input(struct hexium *hexium, int input)
+{
+	union i2c_smbus_data data;
+
+	DEB_D("\n");
+
+	data.byte = hexium_input_select[input].byte;
+	if (0 != i2c_smbus_xfer(&hexium->i2c_adapter, 0x6c, 0, I2C_SMBUS_WRITE, hexium_input_select[input].adr, I2C_SMBUS_BYTE_DATA, &data)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int hexium_set_standard(struct hexium *hexium, struct hexium_data *vdec)
+{
+	union i2c_smbus_data data;
+	int i = 0;
+
+	DEB_D("\n");
+
+	while (vdec[i].adr != -1) {
+		data.byte = vdec[i].byte;
+		if (0 != i2c_smbus_xfer(&hexium->i2c_adapter, 0x6c, 0, I2C_SMBUS_WRITE, vdec[i].adr, I2C_SMBUS_BYTE_DATA, &data)) {
+			pr_err("hexium_init_done: hexium_set_standard() failed for address 0x%02x\n",
+			       i);
+			return -1;
+		}
+		i++;
+	}
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	DEB_EE("VIDIOC_ENUMINPUT %d\n", i->index);
+
+	if (i->index >= HEXIUM_INPUTS)
+		return -EINVAL;
+
+	memcpy(i, &hexium_inputs[i->index], sizeof(struct v4l2_input));
+
+	DEB_D("v4l2_ioctl: VIDIOC_ENUMINPUT %d\n", i->index);
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	*input = hexium->cur_input;
+
+	DEB_D("VIDIOC_G_INPUT: %d\n", *input);
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	DEB_EE("VIDIOC_S_INPUT %d\n", input);
+
+	if (input >= HEXIUM_INPUTS)
+		return -EINVAL;
+
+	hexium->cur_input = input;
+	hexium_set_input(hexium, input);
+	return 0;
+}
+
+static struct saa7146_ext_vv vv_data;
+
+/* this function only gets called when the probing was successful */
+static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
+{
+	struct hexium *hexium;
+	int ret;
+
+	DEB_EE("\n");
+
+	hexium = kzalloc(sizeof(*hexium), GFP_KERNEL);
+	if (!hexium)
+		return -ENOMEM;
+
+	dev->ext_priv = hexium;
+
+	/* enable i2c-port pins */
+	saa7146_write(dev, MC1, (MASK_08 | MASK_24 | MASK_10 | MASK_26));
+
+	strscpy(hexium->i2c_adapter.name, "hexium gemini",
+		sizeof(hexium->i2c_adapter.name));
+	saa7146_i2c_adapter_prepare(dev, &hexium->i2c_adapter, SAA7146_I2C_BUS_BIT_RATE_480);
+	if (i2c_add_adapter(&hexium->i2c_adapter) < 0) {
+		DEB_S("cannot register i2c-device. skipping.\n");
+		kfree(hexium);
+		return -EFAULT;
+	}
+
+	/*  set HWControl GPIO number 2 */
+	saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
+
+	saa7146_write(dev, DD1_INIT, 0x07000700);
+	saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+	saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	/* the rest */
+	hexium->cur_input = 0;
+	hexium_init_done(dev);
+
+	hexium_set_standard(hexium, hexium_pal);
+	hexium->cur_std = V4L2_STD_PAL;
+
+	hexium_set_input(hexium, 0);
+	hexium->cur_input = 0;
+
+	ret = saa7146_vv_init(dev, &vv_data);
+	if (ret) {
+		i2c_del_adapter(&hexium->i2c_adapter);
+		kfree(hexium);
+		return ret;
+	}
+
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+	ret = saa7146_register_device(&hexium->video_dev, dev, "hexium gemini", VFL_TYPE_VIDEO);
+	if (ret < 0) {
+		pr_err("cannot register capture v4l2 device. skipping.\n");
+		saa7146_vv_release(dev);
+		i2c_del_adapter(&hexium->i2c_adapter);
+		kfree(hexium);
+		return ret;
+	}
+
+	pr_info("found 'hexium gemini' frame grabber-%d\n", hexium_num);
+	hexium_num++;
+
+	return 0;
+}
+
+static int hexium_detach(struct saa7146_dev *dev)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	DEB_EE("dev:%p\n", dev);
+
+	saa7146_unregister_device(&hexium->video_dev, dev);
+	saa7146_vv_release(dev);
+
+	hexium_num--;
+
+	i2c_del_adapter(&hexium->i2c_adapter);
+	kfree(hexium);
+	return 0;
+}
+
+static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *std)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	if (V4L2_STD_PAL == std->id) {
+		hexium_set_standard(hexium, hexium_pal);
+		hexium->cur_std = V4L2_STD_PAL;
+		return 0;
+	} else if (V4L2_STD_NTSC == std->id) {
+		hexium_set_standard(hexium, hexium_ntsc);
+		hexium->cur_std = V4L2_STD_NTSC;
+		return 0;
+	} else if (V4L2_STD_SECAM == std->id) {
+		hexium_set_standard(hexium, hexium_secam);
+		hexium->cur_std = V4L2_STD_SECAM;
+		return 0;
+	}
+
+	return -1;
+}
+
+static struct saa7146_extension hexium_extension;
+
+static struct saa7146_pci_extension_data hexium_gemini_4bnc = {
+	.ext_priv = "Hexium Gemini (4 BNC)",
+	.ext = &hexium_extension,
+};
+
+static struct saa7146_pci_extension_data hexium_gemini_dual_4bnc = {
+	.ext_priv = "Hexium Gemini Dual (4 BNC)",
+	.ext = &hexium_extension,
+};
+
+static const struct pci_device_id pci_tbl[] = {
+	{
+	 .vendor = PCI_VENDOR_ID_PHILIPS,
+	 .device = PCI_DEVICE_ID_PHILIPS_SAA7146,
+	 .subvendor = 0x17c8,
+	 .subdevice = 0x2401,
+	 .driver_data = (unsigned long) &hexium_gemini_4bnc,
+	 },
+	{
+	 .vendor = PCI_VENDOR_ID_PHILIPS,
+	 .device = PCI_DEVICE_ID_PHILIPS_SAA7146,
+	 .subvendor = 0x17c8,
+	 .subdevice = 0x2402,
+	 .driver_data = (unsigned long) &hexium_gemini_dual_4bnc,
+	 },
+	{
+	 .vendor = 0,
+	 }
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_ext_vv vv_data = {
+	.inputs = HEXIUM_INPUTS,
+	.capabilities = 0,
+	.stds = &hexium_standards[0],
+	.num_stds = ARRAY_SIZE(hexium_standards),
+	.std_callback = &std_callback,
+};
+
+static struct saa7146_extension hexium_extension = {
+	.name = "hexium gemini",
+	.flags = SAA7146_USE_I2C_IRQ,
+
+	.pci_tbl = &pci_tbl[0],
+	.module = THIS_MODULE,
+
+	.attach = hexium_attach,
+	.detach = hexium_detach,
+
+	.irq_mask = 0,
+	.irq_func = NULL,
+};
+
+static int __init hexium_init_module(void)
+{
+	if (0 != saa7146_register_extension(&hexium_extension)) {
+		DEB_S("failed to register extension\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit hexium_cleanup_module(void)
+{
+	saa7146_unregister_extension(&hexium_extension);
+}
+
+module_init(hexium_init_module);
+module_exit(hexium_cleanup_module);
+
+MODULE_DESCRIPTION("video4linux-2 driver for Hexium Gemini frame grabber cards");
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/hexium_orion.c b/drivers/staging/media/deprecated/saa7146/saa7146/hexium_orion.c
new file mode 100644
index 000000000..ebd63998a
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/hexium_orion.c
@@ -0,0 +1,496 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    hexium_orion.c - v4l2 driver for the Hexium Orion frame grabber cards
+
+    Visit http://www.mihu.de/linux/saa7146/ and follow the link
+    to "hexium" for further details about this card.
+
+    Copyright (C) 2003 Michael Hunold <michael@mihu.de>
+
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#define DEBUG_VARIABLE debug
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include "../common/saa7146_vv.h"
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "debug verbosity");
+
+/* global variables */
+static int hexium_num;
+
+#define HEXIUM_HV_PCI6_ORION		1
+#define HEXIUM_ORION_1SVHS_3BNC		2
+#define HEXIUM_ORION_4BNC		3
+
+#define HEXIUM_INPUTS	9
+static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+};
+
+#define HEXIUM_AUDIOS	0
+
+struct hexium_data
+{
+	s8 adr;
+	u8 byte;
+};
+
+struct hexium
+{
+	int type;
+	struct video_device	video_dev;
+	struct i2c_adapter	i2c_adapter;
+
+	int cur_input;	/* current input */
+};
+
+/* Philips SAA7110 decoder default registers */
+static u8 hexium_saa7110[53]={
+/*00*/ 0x4C,0x3C,0x0D,0xEF,0xBD,0xF0,0x00,0x00,
+/*08*/ 0xF8,0xF8,0x60,0x60,0x40,0x86,0x18,0x90,
+/*10*/ 0x00,0x2C,0x40,0x46,0x42,0x1A,0xFF,0xDA,
+/*18*/ 0xF0,0x8B,0x00,0x00,0x00,0x00,0x00,0x00,
+/*20*/ 0xD9,0x17,0x40,0x41,0x80,0x41,0x80,0x4F,
+/*28*/ 0xFE,0x01,0x0F,0x0F,0x03,0x01,0x81,0x03,
+/*30*/ 0x44,0x75,0x01,0x8C,0x03
+};
+
+static struct {
+	struct hexium_data data[8];
+} hexium_input_select[] = {
+{
+	{ /* cvbs 1 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0xD9 },
+		{ 0x21, 0x17 }, // 0x16,
+		{ 0x22, 0x40 },
+		{ 0x2C, 0x03 },
+		{ 0x30, 0x44 },
+		{ 0x31, 0x75 }, // ??
+		{ 0x21, 0x16 }, // 0x03,
+	}
+}, {
+	{ /* cvbs 2 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0x78 },
+		{ 0x21, 0x07 }, // 0x03,
+		{ 0x22, 0xD2 },
+		{ 0x2C, 0x83 },
+		{ 0x30, 0x60 },
+		{ 0x31, 0xB5 }, // ?
+		{ 0x21, 0x03 },
+	}
+}, {
+	{ /* cvbs 3 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0xBA },
+		{ 0x21, 0x07 }, // 0x05,
+		{ 0x22, 0x91 },
+		{ 0x2C, 0x03 },
+		{ 0x30, 0x60 },
+		{ 0x31, 0xB5 }, // ??
+		{ 0x21, 0x05 }, // 0x03,
+	}
+}, {
+	{ /* cvbs 4 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0xD8 },
+		{ 0x21, 0x17 }, // 0x16,
+		{ 0x22, 0x40 },
+		{ 0x2C, 0x03 },
+		{ 0x30, 0x44 },
+		{ 0x31, 0x75 }, // ??
+		{ 0x21, 0x16 }, // 0x03,
+	}
+}, {
+	{ /* cvbs 5 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0xB8 },
+		{ 0x21, 0x07 }, // 0x05,
+		{ 0x22, 0x91 },
+		{ 0x2C, 0x03 },
+		{ 0x30, 0x60 },
+		{ 0x31, 0xB5 }, // ??
+		{ 0x21, 0x05 }, // 0x03,
+	}
+}, {
+	{ /* cvbs 6 */
+		{ 0x06, 0x00 },
+		{ 0x20, 0x7C },
+		{ 0x21, 0x07 }, // 0x03
+		{ 0x22, 0xD2 },
+		{ 0x2C, 0x83 },
+		{ 0x30, 0x60 },
+		{ 0x31, 0xB5 }, // ??
+		{ 0x21, 0x03 },
+	}
+}, {
+	{ /* y/c 1 */
+		{ 0x06, 0x80 },
+		{ 0x20, 0x59 },
+		{ 0x21, 0x17 },
+		{ 0x22, 0x42 },
+		{ 0x2C, 0xA3 },
+		{ 0x30, 0x44 },
+		{ 0x31, 0x75 },
+		{ 0x21, 0x12 },
+	}
+}, {
+	{ /* y/c 2 */
+		{ 0x06, 0x80 },
+		{ 0x20, 0x9A },
+		{ 0x21, 0x17 },
+		{ 0x22, 0xB1 },
+		{ 0x2C, 0x13 },
+		{ 0x30, 0x60 },
+		{ 0x31, 0xB5 },
+		{ 0x21, 0x14 },
+	}
+}, {
+	{ /* y/c 3 */
+		{ 0x06, 0x80 },
+		{ 0x20, 0x3C },
+		{ 0x21, 0x27 },
+		{ 0x22, 0xC1 },
+		{ 0x2C, 0x23 },
+		{ 0x30, 0x44 },
+		{ 0x31, 0x75 },
+		{ 0x21, 0x21 },
+	}
+}
+};
+
+static struct saa7146_standard hexium_standards[] = {
+	{
+		.name	= "PAL",	.id	= V4L2_STD_PAL,
+		.v_offset	= 16,	.v_field	= 288,
+		.h_offset	= 1,	.h_pixels	= 680,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id	= V4L2_STD_NTSC,
+		.v_offset	= 16,	.v_field	= 240,
+		.h_offset	= 1,	.h_pixels	= 640,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}, {
+		.name	= "SECAM",	.id	= V4L2_STD_SECAM,
+		.v_offset	= 16,	.v_field	= 288,
+		.h_offset	= 1,	.h_pixels	= 720,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}
+};
+
+/* this is only called for old HV-PCI6/Orion cards
+   without eeprom */
+static int hexium_probe(struct saa7146_dev *dev)
+{
+	struct hexium *hexium = NULL;
+	union i2c_smbus_data data;
+	int err = 0;
+
+	DEB_EE("\n");
+
+	/* there are no hexium orion cards with revision 0 saa7146s */
+	if (0 == dev->revision) {
+		return -EFAULT;
+	}
+
+	hexium = kzalloc(sizeof(*hexium), GFP_KERNEL);
+	if (!hexium)
+		return -ENOMEM;
+
+	/* enable i2c-port pins */
+	saa7146_write(dev, MC1, (MASK_08 | MASK_24 | MASK_10 | MASK_26));
+
+	saa7146_write(dev, DD1_INIT, 0x01000100);
+	saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+	saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	strscpy(hexium->i2c_adapter.name, "hexium orion",
+		sizeof(hexium->i2c_adapter.name));
+	saa7146_i2c_adapter_prepare(dev, &hexium->i2c_adapter, SAA7146_I2C_BUS_BIT_RATE_480);
+	if (i2c_add_adapter(&hexium->i2c_adapter) < 0) {
+		DEB_S("cannot register i2c-device. skipping.\n");
+		kfree(hexium);
+		return -EFAULT;
+	}
+
+	/* set SAA7110 control GPIO 0 */
+	saa7146_setgpio(dev, 0, SAA7146_GPIO_OUTHI);
+	/*  set HWControl GPIO number 2 */
+	saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
+
+	mdelay(10);
+
+	/* detect newer Hexium Orion cards by subsystem ids */
+	if (0x17c8 == dev->pci->subsystem_vendor && 0x0101 == dev->pci->subsystem_device) {
+		pr_info("device is a Hexium Orion w/ 1 SVHS + 3 BNC inputs\n");
+		/* we store the pointer in our private data field */
+		dev->ext_priv = hexium;
+		hexium->type = HEXIUM_ORION_1SVHS_3BNC;
+		return 0;
+	}
+
+	if (0x17c8 == dev->pci->subsystem_vendor && 0x2101 == dev->pci->subsystem_device) {
+		pr_info("device is a Hexium Orion w/ 4 BNC inputs\n");
+		/* we store the pointer in our private data field */
+		dev->ext_priv = hexium;
+		hexium->type = HEXIUM_ORION_4BNC;
+		return 0;
+	}
+
+	/* check if this is an old hexium Orion card by looking at
+	   a saa7110 at address 0x4e */
+	err = i2c_smbus_xfer(&hexium->i2c_adapter, 0x4e, 0, I2C_SMBUS_READ,
+			     0x00, I2C_SMBUS_BYTE_DATA, &data);
+	if (err == 0) {
+		pr_info("device is a Hexium HV-PCI6/Orion (old)\n");
+		/* we store the pointer in our private data field */
+		dev->ext_priv = hexium;
+		hexium->type = HEXIUM_HV_PCI6_ORION;
+		return 0;
+	}
+
+	i2c_del_adapter(&hexium->i2c_adapter);
+	kfree(hexium);
+	return -EFAULT;
+}
+
+/* bring hardware to a sane state. this has to be done, just in case someone
+   wants to capture from this device before it has been properly initialized.
+   the capture engine would badly fail, because no valid signal arrives on the
+   saa7146, thus leading to timeouts and stuff. */
+static int hexium_init_done(struct saa7146_dev *dev)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+	union i2c_smbus_data data;
+	int i = 0;
+
+	DEB_D("hexium_init_done called\n");
+
+	/* initialize the helper ics to useful values */
+	for (i = 0; i < sizeof(hexium_saa7110); i++) {
+		data.byte = hexium_saa7110[i];
+		if (0 != i2c_smbus_xfer(&hexium->i2c_adapter, 0x4e, 0, I2C_SMBUS_WRITE, i, I2C_SMBUS_BYTE_DATA, &data)) {
+			pr_err("failed for address 0x%02x\n", i);
+		}
+	}
+
+	return 0;
+}
+
+static int hexium_set_input(struct hexium *hexium, int input)
+{
+	union i2c_smbus_data data;
+	int i = 0;
+
+	DEB_D("\n");
+
+	for (i = 0; i < 8; i++) {
+		int adr = hexium_input_select[input].data[i].adr;
+		data.byte = hexium_input_select[input].data[i].byte;
+		if (0 != i2c_smbus_xfer(&hexium->i2c_adapter, 0x4e, 0, I2C_SMBUS_WRITE, adr, I2C_SMBUS_BYTE_DATA, &data)) {
+			return -1;
+		}
+		pr_debug("%d: 0x%02x => 0x%02x\n", input, adr, data.byte);
+	}
+
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	DEB_EE("VIDIOC_ENUMINPUT %d\n", i->index);
+
+	if (i->index >= HEXIUM_INPUTS)
+		return -EINVAL;
+
+	memcpy(i, &hexium_inputs[i->index], sizeof(struct v4l2_input));
+
+	DEB_D("v4l2_ioctl: VIDIOC_ENUMINPUT %d\n", i->index);
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	*input = hexium->cur_input;
+
+	DEB_D("VIDIOC_G_INPUT: %d\n", *input);
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	if (input >= HEXIUM_INPUTS)
+		return -EINVAL;
+
+	hexium->cur_input = input;
+	hexium_set_input(hexium, input);
+
+	return 0;
+}
+
+static struct saa7146_ext_vv vv_data;
+
+/* this function only gets called when the probing was successful */
+static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+	int ret;
+
+	DEB_EE("\n");
+
+	ret = saa7146_vv_init(dev, &vv_data);
+	if (ret) {
+		pr_err("Error in saa7146_vv_init()\n");
+		return ret;
+	}
+
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+	if (0 != saa7146_register_device(&hexium->video_dev, dev, "hexium orion", VFL_TYPE_VIDEO)) {
+		pr_err("cannot register capture v4l2 device. skipping.\n");
+		return -1;
+	}
+
+	pr_err("found 'hexium orion' frame grabber-%d\n", hexium_num);
+	hexium_num++;
+
+	/* the rest */
+	hexium->cur_input = 0;
+	hexium_init_done(dev);
+
+	return 0;
+}
+
+static int hexium_detach(struct saa7146_dev *dev)
+{
+	struct hexium *hexium = (struct hexium *) dev->ext_priv;
+
+	DEB_EE("dev:%p\n", dev);
+
+	saa7146_unregister_device(&hexium->video_dev, dev);
+	saa7146_vv_release(dev);
+
+	hexium_num--;
+
+	i2c_del_adapter(&hexium->i2c_adapter);
+	kfree(hexium);
+	return 0;
+}
+
+static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *std)
+{
+	return 0;
+}
+
+static struct saa7146_extension extension;
+
+static struct saa7146_pci_extension_data hexium_hv_pci6 = {
+	.ext_priv = "Hexium HV-PCI6 / Orion",
+	.ext = &extension,
+};
+
+static struct saa7146_pci_extension_data hexium_orion_1svhs_3bnc = {
+	.ext_priv = "Hexium HV-PCI6 / Orion (1 SVHS/3 BNC)",
+	.ext = &extension,
+};
+
+static struct saa7146_pci_extension_data hexium_orion_4bnc = {
+	.ext_priv = "Hexium HV-PCI6 / Orion (4 BNC)",
+	.ext = &extension,
+};
+
+static const struct pci_device_id pci_tbl[] = {
+	{
+	 .vendor = PCI_VENDOR_ID_PHILIPS,
+	 .device = PCI_DEVICE_ID_PHILIPS_SAA7146,
+	 .subvendor = 0x0000,
+	 .subdevice = 0x0000,
+	 .driver_data = (unsigned long) &hexium_hv_pci6,
+	 },
+	{
+	 .vendor = PCI_VENDOR_ID_PHILIPS,
+	 .device = PCI_DEVICE_ID_PHILIPS_SAA7146,
+	 .subvendor = 0x17c8,
+	 .subdevice = 0x0101,
+	 .driver_data = (unsigned long) &hexium_orion_1svhs_3bnc,
+	 },
+	{
+	 .vendor = PCI_VENDOR_ID_PHILIPS,
+	 .device = PCI_DEVICE_ID_PHILIPS_SAA7146,
+	 .subvendor = 0x17c8,
+	 .subdevice = 0x2101,
+	 .driver_data = (unsigned long) &hexium_orion_4bnc,
+	 },
+	{
+	 .vendor = 0,
+	 }
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_ext_vv vv_data = {
+	.inputs = HEXIUM_INPUTS,
+	.capabilities = 0,
+	.stds = &hexium_standards[0],
+	.num_stds = ARRAY_SIZE(hexium_standards),
+	.std_callback = &std_callback,
+};
+
+static struct saa7146_extension extension = {
+	.name = "hexium HV-PCI6 Orion",
+	.flags = 0,		// SAA7146_USE_I2C_IRQ,
+
+	.pci_tbl = &pci_tbl[0],
+	.module = THIS_MODULE,
+
+	.probe = hexium_probe,
+	.attach = hexium_attach,
+	.detach = hexium_detach,
+
+	.irq_mask = 0,
+	.irq_func = NULL,
+};
+
+static int __init hexium_init_module(void)
+{
+	if (0 != saa7146_register_extension(&extension)) {
+		DEB_S("failed to register extension\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit hexium_cleanup_module(void)
+{
+	saa7146_unregister_extension(&extension);
+}
+
+module_init(hexium_init_module);
+module_exit(hexium_cleanup_module);
+
+MODULE_DESCRIPTION("video4linux-2 driver for Hexium Orion frame grabber cards");
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/saa7146/mxb.c b/drivers/staging/media/deprecated/saa7146/saa7146/mxb.c
new file mode 100644
index 000000000..3e568f952
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/saa7146/mxb.c
@@ -0,0 +1,873 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    mxb - v4l2 driver for the Multimedia eXtension Board
+
+    Copyright (C) 1998-2006 Michael Hunold <michael@mihu.de>
+
+    Visit http://www.themm.net/~mihu/linux/saa7146/mxb.html
+    for further details about this card.
+
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#define DEBUG_VARIABLE debug
+
+#include <media/tuner.h>
+#include <media/v4l2-common.h>
+#include <media/i2c/saa7115.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+
+#include "../common/saa7146_vv.h"
+#include "tea6415c.h"
+#include "tea6420.h"
+
+#define MXB_AUDIOS	6
+
+#define I2C_SAA7111A  0x24
+#define	I2C_TDA9840   0x42
+#define	I2C_TEA6415C  0x43
+#define	I2C_TEA6420_1 0x4c
+#define	I2C_TEA6420_2 0x4d
+#define	I2C_TUNER     0x60
+
+#define MXB_BOARD_CAN_DO_VBI(dev)   (dev->revision != 0)
+
+/* global variable */
+static int mxb_num;
+
+/* initial frequence the tuner will be tuned to.
+   in verden (lower saxony, germany) 4148 is a
+   channel called "phoenix" */
+static int freq = 4148;
+module_param(freq, int, 0644);
+MODULE_PARM_DESC(freq, "initial frequency the tuner will be tuned to while setup");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Turn on/off device debugging (default:off).");
+
+#define MXB_INPUTS 4
+enum { TUNER, AUX1, AUX3, AUX3_YC };
+
+static struct v4l2_input mxb_inputs[MXB_INPUTS] = {
+	{ TUNER,   "Tuner",          V4L2_INPUT_TYPE_TUNER,  0x3f, 0,
+		V4L2_STD_PAL_BG | V4L2_STD_PAL_I, 0, V4L2_IN_CAP_STD },
+	{ AUX1,	   "AUX1",           V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3,	   "AUX3 Composite", V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3_YC, "AUX3 S-Video",   V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+};
+
+/* this array holds the information, which port of the saa7146 each
+   input actually uses. the mxb uses port 0 for every input */
+static struct {
+	int hps_source;
+	int hps_sync;
+} input_port_selection[MXB_INPUTS] = {
+	{ SAA7146_HPS_SOURCE_PORT_A, SAA7146_HPS_SYNC_PORT_A },
+	{ SAA7146_HPS_SOURCE_PORT_A, SAA7146_HPS_SYNC_PORT_A },
+	{ SAA7146_HPS_SOURCE_PORT_A, SAA7146_HPS_SYNC_PORT_A },
+	{ SAA7146_HPS_SOURCE_PORT_A, SAA7146_HPS_SYNC_PORT_A },
+};
+
+/* this array holds the information of the audio source (mxb_audios),
+   which has to be switched corresponding to the video source (mxb_channels) */
+static int video_audio_connect[MXB_INPUTS] =
+	{ 0, 1, 3, 3 };
+
+struct mxb_routing {
+	u32 input;
+	u32 output;
+};
+
+/* these are the available audio sources, which can switched
+   to the line- and cd-output individually */
+static struct v4l2_audio mxb_audios[MXB_AUDIOS] = {
+	    {
+		.index	= 0,
+		.name	= "Tuner",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 1,
+		.name	= "AUX1",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 2,
+		.name	= "AUX2",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 3,
+		.name	= "AUX3",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 4,
+		.name	= "Radio (X9)",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 5,
+		.name	= "CD-ROM (X10)",
+		.capability = V4L2_AUDCAP_STEREO,
+	}
+};
+
+/* These are the necessary input-output-pins for bringing one audio source
+   (see above) to the CD-output. Note that gain is set to 0 in this table. */
+static struct mxb_routing TEA6420_cd[MXB_AUDIOS + 1][2] = {
+	{ { 1, 1 }, { 1, 1 } },	/* Tuner */
+	{ { 5, 1 }, { 6, 1 } },	/* AUX 1 */
+	{ { 4, 1 }, { 6, 1 } },	/* AUX 2 */
+	{ { 3, 1 }, { 6, 1 } },	/* AUX 3 */
+	{ { 1, 1 }, { 3, 1 } },	/* Radio */
+	{ { 1, 1 }, { 2, 1 } },	/* CD-Rom */
+	{ { 6, 1 }, { 6, 1 } }	/* Mute */
+};
+
+/* These are the necessary input-output-pins for bringing one audio source
+   (see above) to the line-output. Note that gain is set to 0 in this table. */
+static struct mxb_routing TEA6420_line[MXB_AUDIOS + 1][2] = {
+	{ { 2, 3 }, { 1, 2 } },
+	{ { 5, 3 }, { 6, 2 } },
+	{ { 4, 3 }, { 6, 2 } },
+	{ { 3, 3 }, { 6, 2 } },
+	{ { 2, 3 }, { 3, 2 } },
+	{ { 2, 3 }, { 2, 2 } },
+	{ { 6, 3 }, { 6, 2 } }	/* Mute */
+};
+
+struct mxb
+{
+	struct video_device	video_dev;
+	struct video_device	vbi_dev;
+
+	struct i2c_adapter	i2c_adapter;
+
+	struct v4l2_subdev	*saa7111a;
+	struct v4l2_subdev	*tda9840;
+	struct v4l2_subdev	*tea6415c;
+	struct v4l2_subdev	*tuner;
+	struct v4l2_subdev	*tea6420_1;
+	struct v4l2_subdev	*tea6420_2;
+
+	int	cur_mode;	/* current audio mode (mono, stereo, ...) */
+	int	cur_input;	/* current input */
+	int	cur_audinput;	/* current audio input */
+	int	cur_mute;	/* current mute status */
+	struct v4l2_frequency	cur_freq;	/* current frequency the tuner is tuned to */
+};
+
+#define saa7111a_call(mxb, o, f, args...) \
+	v4l2_subdev_call(mxb->saa7111a, o, f, ##args)
+#define tda9840_call(mxb, o, f, args...) \
+	v4l2_subdev_call(mxb->tda9840, o, f, ##args)
+#define tea6415c_call(mxb, o, f, args...) \
+	v4l2_subdev_call(mxb->tea6415c, o, f, ##args)
+#define tuner_call(mxb, o, f, args...) \
+	v4l2_subdev_call(mxb->tuner, o, f, ##args)
+#define call_all(dev, o, f, args...) \
+	v4l2_device_call_until_err(&dev->v4l2_dev, 0, o, f, ##args)
+
+static void mxb_update_audmode(struct mxb *mxb)
+{
+	struct v4l2_tuner t = {
+		.audmode = mxb->cur_mode,
+	};
+
+	tda9840_call(mxb, tuner, s_tuner, &t);
+}
+
+static inline void tea6420_route(struct mxb *mxb, int idx)
+{
+	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
+		TEA6420_cd[idx][0].input, TEA6420_cd[idx][0].output, 0);
+	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
+		TEA6420_cd[idx][1].input, TEA6420_cd[idx][1].output, 0);
+	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
+		TEA6420_line[idx][0].input, TEA6420_line[idx][0].output, 0);
+	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
+		TEA6420_line[idx][1].input, TEA6420_line[idx][1].output, 0);
+}
+
+static struct saa7146_extension extension;
+
+static int mxb_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
+	struct mxb *mxb = dev->ext_priv;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		mxb->cur_mute = ctrl->val;
+		/* switch the audio-source */
+		tea6420_route(mxb, ctrl->val ? 6 :
+				video_audio_connect[mxb->cur_input]);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mxb_ctrl_ops = {
+	.s_ctrl = mxb_s_ctrl,
+};
+
+static int mxb_probe(struct saa7146_dev *dev)
+{
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct mxb *mxb = NULL;
+
+	v4l2_ctrl_new_std(hdl, &mxb_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (hdl->error)
+		return hdl->error;
+	mxb = kzalloc(sizeof(struct mxb), GFP_KERNEL);
+	if (mxb == NULL) {
+		DEB_D("not enough kernel memory\n");
+		return -ENOMEM;
+	}
+
+
+	snprintf(mxb->i2c_adapter.name, sizeof(mxb->i2c_adapter.name), "mxb%d", mxb_num);
+
+	saa7146_i2c_adapter_prepare(dev, &mxb->i2c_adapter, SAA7146_I2C_BUS_BIT_RATE_480);
+	if (i2c_add_adapter(&mxb->i2c_adapter) < 0) {
+		DEB_S("cannot register i2c-device. skipping.\n");
+		kfree(mxb);
+		return -EFAULT;
+	}
+
+	mxb->saa7111a = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"saa7111", I2C_SAA7111A, NULL);
+	mxb->tea6420_1 = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"tea6420", I2C_TEA6420_1, NULL);
+	mxb->tea6420_2 = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"tea6420", I2C_TEA6420_2, NULL);
+	mxb->tea6415c = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"tea6415c", I2C_TEA6415C, NULL);
+	mxb->tda9840 = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"tda9840", I2C_TDA9840, NULL);
+	mxb->tuner = v4l2_i2c_new_subdev(&dev->v4l2_dev, &mxb->i2c_adapter,
+			"tuner", I2C_TUNER, NULL);
+
+	/* check if all devices are present */
+	if (!mxb->tea6420_1 || !mxb->tea6420_2 || !mxb->tea6415c ||
+	    !mxb->tda9840 || !mxb->saa7111a || !mxb->tuner) {
+		pr_err("did not find all i2c devices. aborting\n");
+		i2c_del_adapter(&mxb->i2c_adapter);
+		kfree(mxb);
+		return -ENODEV;
+	}
+
+	/* all devices are present, probe was successful */
+
+	/* we store the pointer in our private data field */
+	dev->ext_priv = mxb;
+
+	v4l2_ctrl_handler_setup(hdl);
+
+	return 0;
+}
+
+/* some init data for the saa7740, the so-called 'sound arena module'.
+   there are no specs available, so we simply use some init values */
+static struct {
+	int	length;
+	char	data[9];
+} mxb_saa7740_init[] = {
+	{ 3, { 0x80, 0x00, 0x00 } },{ 3, { 0x80, 0x89, 0x00 } },
+	{ 3, { 0x80, 0xb0, 0x0a } },{ 3, { 0x00, 0x00, 0x00 } },
+	{ 3, { 0x49, 0x00, 0x00 } },{ 3, { 0x4a, 0x00, 0x00 } },
+	{ 3, { 0x4b, 0x00, 0x00 } },{ 3, { 0x4c, 0x00, 0x00 } },
+	{ 3, { 0x4d, 0x00, 0x00 } },{ 3, { 0x4e, 0x00, 0x00 } },
+	{ 3, { 0x4f, 0x00, 0x00 } },{ 3, { 0x50, 0x00, 0x00 } },
+	{ 3, { 0x51, 0x00, 0x00 } },{ 3, { 0x52, 0x00, 0x00 } },
+	{ 3, { 0x53, 0x00, 0x00 } },{ 3, { 0x54, 0x00, 0x00 } },
+	{ 3, { 0x55, 0x00, 0x00 } },{ 3, { 0x56, 0x00, 0x00 } },
+	{ 3, { 0x57, 0x00, 0x00 } },{ 3, { 0x58, 0x00, 0x00 } },
+	{ 3, { 0x59, 0x00, 0x00 } },{ 3, { 0x5a, 0x00, 0x00 } },
+	{ 3, { 0x5b, 0x00, 0x00 } },{ 3, { 0x5c, 0x00, 0x00 } },
+	{ 3, { 0x5d, 0x00, 0x00 } },{ 3, { 0x5e, 0x00, 0x00 } },
+	{ 3, { 0x5f, 0x00, 0x00 } },{ 3, { 0x60, 0x00, 0x00 } },
+	{ 3, { 0x61, 0x00, 0x00 } },{ 3, { 0x62, 0x00, 0x00 } },
+	{ 3, { 0x63, 0x00, 0x00 } },{ 3, { 0x64, 0x00, 0x00 } },
+	{ 3, { 0x65, 0x00, 0x00 } },{ 3, { 0x66, 0x00, 0x00 } },
+	{ 3, { 0x67, 0x00, 0x00 } },{ 3, { 0x68, 0x00, 0x00 } },
+	{ 3, { 0x69, 0x00, 0x00 } },{ 3, { 0x6a, 0x00, 0x00 } },
+	{ 3, { 0x6b, 0x00, 0x00 } },{ 3, { 0x6c, 0x00, 0x00 } },
+	{ 3, { 0x6d, 0x00, 0x00 } },{ 3, { 0x6e, 0x00, 0x00 } },
+	{ 3, { 0x6f, 0x00, 0x00 } },{ 3, { 0x70, 0x00, 0x00 } },
+	{ 3, { 0x71, 0x00, 0x00 } },{ 3, { 0x72, 0x00, 0x00 } },
+	{ 3, { 0x73, 0x00, 0x00 } },{ 3, { 0x74, 0x00, 0x00 } },
+	{ 3, { 0x75, 0x00, 0x00 } },{ 3, { 0x76, 0x00, 0x00 } },
+	{ 3, { 0x77, 0x00, 0x00 } },{ 3, { 0x41, 0x00, 0x42 } },
+	{ 3, { 0x42, 0x10, 0x42 } },{ 3, { 0x43, 0x20, 0x42 } },
+	{ 3, { 0x44, 0x30, 0x42 } },{ 3, { 0x45, 0x00, 0x01 } },
+	{ 3, { 0x46, 0x00, 0x01 } },{ 3, { 0x47, 0x00, 0x01 } },
+	{ 3, { 0x48, 0x00, 0x01 } },
+	{ 9, { 0x01, 0x03, 0xc5, 0x5c, 0x7a, 0x85, 0x01, 0x00, 0x54 } },
+	{ 9, { 0x21, 0x03, 0xc5, 0x5c, 0x7a, 0x85, 0x01, 0x00, 0x54 } },
+	{ 9, { 0x09, 0x0b, 0xb4, 0x6b, 0x74, 0x85, 0x95, 0x00, 0x34 } },
+	{ 9, { 0x29, 0x0b, 0xb4, 0x6b, 0x74, 0x85, 0x95, 0x00, 0x34 } },
+	{ 9, { 0x11, 0x17, 0x43, 0x62, 0x68, 0x89, 0xd1, 0xff, 0xb0 } },
+	{ 9, { 0x31, 0x17, 0x43, 0x62, 0x68, 0x89, 0xd1, 0xff, 0xb0 } },
+	{ 9, { 0x19, 0x20, 0x62, 0x51, 0x5a, 0x95, 0x19, 0x01, 0x50 } },
+	{ 9, { 0x39, 0x20, 0x62, 0x51, 0x5a, 0x95, 0x19, 0x01, 0x50 } },
+	{ 9, { 0x05, 0x3e, 0xd2, 0x69, 0x4e, 0x9a, 0x51, 0x00, 0xf0 } },
+	{ 9, { 0x25, 0x3e, 0xd2, 0x69, 0x4e, 0x9a, 0x51, 0x00, 0xf0 } },
+	{ 9, { 0x0d, 0x3d, 0xa1, 0x40, 0x7d, 0x9f, 0x29, 0xfe, 0x14 } },
+	{ 9, { 0x2d, 0x3d, 0xa1, 0x40, 0x7d, 0x9f, 0x29, 0xfe, 0x14 } },
+	{ 9, { 0x15, 0x73, 0xa1, 0x50, 0x5d, 0xa6, 0xf5, 0xfe, 0x38 } },
+	{ 9, { 0x35, 0x73, 0xa1, 0x50, 0x5d, 0xa6, 0xf5, 0xfe, 0x38 } },
+	{ 9, { 0x1d, 0xed, 0xd0, 0x68, 0x29, 0xb4, 0xe1, 0x00, 0xb8 } },
+	{ 9, { 0x3d, 0xed, 0xd0, 0x68, 0x29, 0xb4, 0xe1, 0x00, 0xb8 } },
+	{ 3, { 0x80, 0xb3, 0x0a } },
+	{-1, { 0 } }
+};
+
+/* bring hardware to a sane state. this has to be done, just in case someone
+   wants to capture from this device before it has been properly initialized.
+   the capture engine would badly fail, because no valid signal arrives on the
+   saa7146, thus leading to timeouts and stuff. */
+static int mxb_init_done(struct saa7146_dev* dev)
+{
+	struct mxb* mxb = (struct mxb*)dev->ext_priv;
+	struct i2c_msg msg;
+	struct tuner_setup tun_setup;
+	v4l2_std_id std = V4L2_STD_PAL_BG;
+
+	int i, err = 0;
+
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
+	/* select video mode in saa7111a */
+	saa7111a_call(mxb, video, s_std, std);
+
+	/* select tuner-output on saa7111a */
+	saa7111a_call(mxb, video, s_routing, SAA7115_COMPOSITE0,
+		SAA7111_FMT_CCIR, 0);
+
+	/* select a tuner type */
+	tun_setup.mode_mask = T_ANALOG_TV;
+	tun_setup.addr = ADDR_UNSET;
+	tun_setup.type = TUNER_PHILIPS_PAL;
+	tuner_call(mxb, tuner, s_type_addr, &tun_setup);
+	/* tune in some frequency on tuner */
+	mxb->cur_freq.tuner = 0;
+	mxb->cur_freq.type = V4L2_TUNER_ANALOG_TV;
+	mxb->cur_freq.frequency = freq;
+	tuner_call(mxb, tuner, s_frequency, &mxb->cur_freq);
+
+	/* set a default video standard */
+	/* These two gpio calls set the GPIO pins that control the tda9820 */
+	saa7146_write(dev, GPIO_CTRL, 0x00404050);
+	saa7111a_call(mxb, core, s_gpio, 1);
+	saa7111a_call(mxb, video, s_std, std);
+	tuner_call(mxb, video, s_std, std);
+
+	/* switch to tuner-channel on tea6415c */
+	tea6415c_call(mxb, video, s_routing, 3, 17, 0);
+
+	/* select tuner-output on multicable on tea6415c */
+	tea6415c_call(mxb, video, s_routing, 3, 13, 0);
+
+	/* the rest for mxb */
+	mxb->cur_input = 0;
+	mxb->cur_audinput = video_audio_connect[mxb->cur_input];
+	mxb->cur_mute = 1;
+
+	mxb->cur_mode = V4L2_TUNER_MODE_STEREO;
+	mxb_update_audmode(mxb);
+
+	/* check if the saa7740 (aka 'sound arena module') is present
+	   on the mxb. if so, we must initialize it. due to lack of
+	   information about the saa7740, the values were reverse
+	   engineered. */
+	msg.addr = 0x1b;
+	msg.flags = 0;
+	msg.len = mxb_saa7740_init[0].length;
+	msg.buf = &mxb_saa7740_init[0].data[0];
+
+	err = i2c_transfer(&mxb->i2c_adapter, &msg, 1);
+	if (err == 1) {
+		/* the sound arena module is a pos, that's probably the reason
+		   philips refuses to hand out a datasheet for the saa7740...
+		   it seems to screw up the i2c bus, so we disable fast irq
+		   based i2c transactions here and rely on the slow and safe
+		   polling method ... */
+		extension.flags &= ~SAA7146_USE_I2C_IRQ;
+		for (i = 1; ; i++) {
+			if (-1 == mxb_saa7740_init[i].length)
+				break;
+
+			msg.len = mxb_saa7740_init[i].length;
+			msg.buf = &mxb_saa7740_init[i].data[0];
+			err = i2c_transfer(&mxb->i2c_adapter, &msg, 1);
+			if (err != 1) {
+				DEB_D("failed to initialize 'sound arena module'\n");
+				goto err;
+			}
+		}
+		pr_info("'sound arena module' detected\n");
+	}
+err:
+	/* the rest for saa7146: you should definitely set some basic values
+	   for the input-port handling of the saa7146. */
+
+	/* ext->saa has been filled by the core driver */
+
+	/* some stuff is done via variables */
+	saa7146_set_hps_source_and_sync(dev, input_port_selection[mxb->cur_input].hps_source,
+			input_port_selection[mxb->cur_input].hps_sync);
+
+	/* some stuff is done via direct write to the registers */
+
+	/* this is ugly, but because of the fact that this is completely
+	   hardware dependend, it should be done directly... */
+	saa7146_write(dev, DD1_STREAM_B,	0x00000000);
+	saa7146_write(dev, DD1_INIT,		0x02000200);
+	saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	return 0;
+}
+
+/* interrupt-handler. this gets called when irq_mask is != 0.
+   it must clear the interrupt-bits in irq_mask it has handled */
+/*
+void mxb_irq_bh(struct saa7146_dev* dev, u32* irq_mask)
+{
+	struct mxb* mxb = (struct mxb*)dev->ext_priv;
+}
+*/
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	DEB_EE("VIDIOC_ENUMINPUT %d\n", i->index);
+	if (i->index >= MXB_INPUTS)
+		return -EINVAL;
+	memcpy(i, &mxb_inputs[i->index], sizeof(struct v4l2_input));
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+	*i = mxb->cur_input;
+
+	DEB_EE("VIDIOC_G_INPUT %d\n", *i);
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+	int err = 0;
+	int i = 0;
+
+	DEB_EE("VIDIOC_S_INPUT %d\n", input);
+
+	if (input >= MXB_INPUTS)
+		return -EINVAL;
+
+	mxb->cur_input = input;
+
+	saa7146_set_hps_source_and_sync(dev, input_port_selection[input].hps_source,
+			input_port_selection[input].hps_sync);
+
+	/* prepare switching of tea6415c and saa7111a;
+	   have a look at the 'background'-file for further information  */
+	switch (input) {
+	case TUNER:
+		i = SAA7115_COMPOSITE0;
+
+		err = tea6415c_call(mxb, video, s_routing, 3, 17, 0);
+
+		/* connect tuner-output always to multicable */
+		if (!err)
+			err = tea6415c_call(mxb, video, s_routing, 3, 13, 0);
+		break;
+	case AUX3_YC:
+		/* nothing to be done here. aux3_yc is
+		   directly connected to the saa711a */
+		i = SAA7115_SVIDEO1;
+		break;
+	case AUX3:
+		/* nothing to be done here. aux3 is
+		   directly connected to the saa711a */
+		i = SAA7115_COMPOSITE1;
+		break;
+	case AUX1:
+		i = SAA7115_COMPOSITE0;
+		err = tea6415c_call(mxb, video, s_routing, 1, 17, 0);
+		break;
+	}
+
+	if (err)
+		return err;
+
+	/* switch video in saa7111a */
+	if (saa7111a_call(mxb, video, s_routing, i, SAA7111_FMT_CCIR, 0))
+		pr_err("VIDIOC_S_INPUT: could not address saa7111a\n");
+
+	mxb->cur_audinput = video_audio_connect[input];
+	/* switch the audio-source only if necessary */
+	if (0 == mxb->cur_mute)
+		tea6420_route(mxb, mxb->cur_audinput);
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
+
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	if (t->index) {
+		DEB_D("VIDIOC_G_TUNER: channel %d does not have a tuner attached\n",
+		      t->index);
+		return -EINVAL;
+	}
+
+	DEB_EE("VIDIOC_G_TUNER: %d\n", t->index);
+
+	memset(t, 0, sizeof(*t));
+	strscpy(t->name, "TV Tuner", sizeof(t->name));
+	t->type = V4L2_TUNER_ANALOG_TV;
+	t->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO |
+			V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2 | V4L2_TUNER_CAP_SAP;
+	t->audmode = mxb->cur_mode;
+	return call_all(dev, tuner, g_tuner, t);
+}
+
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	if (t->index) {
+		DEB_D("VIDIOC_S_TUNER: channel %d does not have a tuner attached\n",
+		      t->index);
+		return -EINVAL;
+	}
+
+	mxb->cur_mode = t->audmode;
+	return call_all(dev, tuner, s_tuner, t);
+}
+
+static int vidioc_querystd(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	return call_all(dev, video, querystd, norm);
+}
+
+static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	if (f->tuner)
+		return -EINVAL;
+	*f = mxb->cur_freq;
+
+	DEB_EE("VIDIOC_G_FREQ: freq:0x%08x\n", mxb->cur_freq.frequency);
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	if (f->tuner)
+		return -EINVAL;
+
+	if (V4L2_TUNER_ANALOG_TV != f->type)
+		return -EINVAL;
+
+	DEB_EE("VIDIOC_S_FREQUENCY: freq:0x%08x\n", mxb->cur_freq.frequency);
+
+	/* tune in desired frequency */
+	tuner_call(mxb, tuner, s_frequency, f);
+	/* let the tuner subdev clamp the frequency to the tuner range */
+	mxb->cur_freq = *f;
+	tuner_call(mxb, tuner, g_frequency, &mxb->cur_freq);
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
+
+	if (mxb->cur_input)
+		return 0;
+
+	/* hack: changing the frequency should invalidate the vbi-counter (=> alevt) */
+	spin_lock(&dev->slock);
+	vv->vbi_fieldcount = 0;
+	spin_unlock(&dev->slock);
+
+	return 0;
+}
+
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	if (a->index >= MXB_AUDIOS)
+		return -EINVAL;
+	*a = mxb_audios[a->index];
+	return 0;
+}
+
+static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	DEB_EE("VIDIOC_G_AUDIO\n");
+	*a = mxb_audios[mxb->cur_audinput];
+	return 0;
+}
+
+static int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	DEB_D("VIDIOC_S_AUDIO %d\n", a->index);
+	if (a->index >= 32 ||
+	    !(mxb_inputs[mxb->cur_input].audioset & (1 << a->index)))
+		return -EINVAL;
+
+	if (mxb->cur_audinput != a->index) {
+		mxb->cur_audinput = a->index;
+		tea6420_route(mxb, a->index);
+		if (mxb->cur_audinput == 0)
+			mxb_update_audmode(mxb);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vidioc_g_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	if (reg->reg > pci_resource_len(dev->pci, 0) - 4)
+		return -EINVAL;
+	reg->val = saa7146_read(dev, reg->reg);
+	reg->size = 4;
+	return 0;
+}
+
+static int vidioc_s_register(struct file *file, void *fh, const struct v4l2_dbg_register *reg)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	if (reg->reg > pci_resource_len(dev->pci, 0) - 4)
+		return -EINVAL;
+	saa7146_write(dev, reg->reg, reg->val);
+	return 0;
+}
+#endif
+
+static struct saa7146_ext_vv vv_data;
+
+/* this function only gets called when the probing was successful */
+static int mxb_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
+{
+	struct mxb *mxb;
+	int ret;
+
+	DEB_EE("dev:%p\n", dev);
+
+	ret = saa7146_vv_init(dev, &vv_data);
+	if (ret) {
+		ERR("Error in saa7146_vv_init()");
+		return ret;
+	}
+
+	if (mxb_probe(dev)) {
+		saa7146_vv_release(dev);
+		return -1;
+	}
+	mxb = (struct mxb *)dev->ext_priv;
+
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data.vid_ops.vidioc_querystd = vidioc_querystd;
+	vv_data.vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data.vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data.vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data.vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data.vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data.vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data.vid_ops.vidioc_s_audio = vidioc_s_audio;
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	vv_data.vid_ops.vidioc_g_register = vidioc_g_register;
+	vv_data.vid_ops.vidioc_s_register = vidioc_s_register;
+#endif
+	if (saa7146_register_device(&mxb->video_dev, dev, "mxb", VFL_TYPE_VIDEO)) {
+		ERR("cannot register capture v4l2 device. skipping.\n");
+		saa7146_vv_release(dev);
+		return -1;
+	}
+
+	/* initialization stuff (vbi) (only for revision > 0 and for extensions which want it)*/
+	if (MXB_BOARD_CAN_DO_VBI(dev)) {
+		if (saa7146_register_device(&mxb->vbi_dev, dev, "mxb", VFL_TYPE_VBI)) {
+			ERR("cannot register vbi v4l2 device. skipping.\n");
+		}
+	}
+
+	pr_info("found Multimedia eXtension Board #%d\n", mxb_num);
+
+	mxb_num++;
+	mxb_init_done(dev);
+	return 0;
+}
+
+static int mxb_detach(struct saa7146_dev *dev)
+{
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	DEB_EE("dev:%p\n", dev);
+
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
+	saa7146_unregister_device(&mxb->video_dev,dev);
+	if (MXB_BOARD_CAN_DO_VBI(dev))
+		saa7146_unregister_device(&mxb->vbi_dev, dev);
+	saa7146_vv_release(dev);
+
+	mxb_num--;
+
+	i2c_del_adapter(&mxb->i2c_adapter);
+	kfree(mxb);
+
+	return 0;
+}
+
+static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *standard)
+{
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
+	if (V4L2_STD_PAL_I == standard->id) {
+		v4l2_std_id std = V4L2_STD_PAL_I;
+
+		DEB_D("VIDIOC_S_STD: setting mxb for PAL_I\n");
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
+		saa7146_write(dev, GPIO_CTRL, 0x00404050);
+		saa7111a_call(mxb, core, s_gpio, 0);
+		saa7111a_call(mxb, video, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, video, s_std, std);
+	} else {
+		v4l2_std_id std = V4L2_STD_PAL_BG;
+
+		if (mxb->cur_input)
+			std = standard->id;
+		DEB_D("VIDIOC_S_STD: setting mxb for PAL/NTSC/SECAM\n");
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
+		saa7146_write(dev, GPIO_CTRL, 0x00404050);
+		saa7111a_call(mxb, core, s_gpio, 1);
+		saa7111a_call(mxb, video, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, video, s_std, std);
+	}
+	return 0;
+}
+
+static struct saa7146_standard standard[] = {
+	{
+		.name	= "PAL-BG",	.id	= V4L2_STD_PAL_BG,
+		.v_offset	= 0x17,	.v_field	= 288,
+		.h_offset	= 0x14,	.h_pixels	= 680,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "PAL-I",	.id	= V4L2_STD_PAL_I,
+		.v_offset	= 0x17,	.v_field	= 288,
+		.h_offset	= 0x14,	.h_pixels	= 680,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}, {
+		.name	= "NTSC",	.id	= V4L2_STD_NTSC,
+		.v_offset	= 0x16,	.v_field	= 240,
+		.h_offset	= 0x06,	.h_pixels	= 708,
+		.v_max_out	= 480,	.h_max_out	= 640,
+	}, {
+		.name	= "SECAM",	.id	= V4L2_STD_SECAM,
+		.v_offset	= 0x14,	.v_field	= 288,
+		.h_offset	= 0x14,	.h_pixels	= 720,
+		.v_max_out	= 576,	.h_max_out	= 768,
+	}
+};
+
+static struct saa7146_pci_extension_data mxb = {
+	.ext_priv = "Multimedia eXtension Board",
+	.ext = &extension,
+};
+
+static const struct pci_device_id pci_tbl[] = {
+	{
+		.vendor    = PCI_VENDOR_ID_PHILIPS,
+		.device	   = PCI_DEVICE_ID_PHILIPS_SAA7146,
+		.subvendor = 0x0000,
+		.subdevice = 0x0000,
+		.driver_data = (unsigned long)&mxb,
+	}, {
+		.vendor	= 0,
+	}
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_ext_vv vv_data = {
+	.inputs		= MXB_INPUTS,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_VBI_CAPTURE | V4L2_CAP_AUDIO,
+	.stds		= &standard[0],
+	.num_stds	= ARRAY_SIZE(standard),
+	.std_callback	= &std_callback,
+};
+
+static struct saa7146_extension extension = {
+	.name		= "Multimedia eXtension Board",
+	.flags		= SAA7146_USE_I2C_IRQ,
+
+	.pci_tbl	= &pci_tbl[0],
+	.module		= THIS_MODULE,
+
+	.attach		= mxb_attach,
+	.detach		= mxb_detach,
+
+	.irq_mask	= 0,
+	.irq_func	= NULL,
+};
+
+static int __init mxb_init_module(void)
+{
+	if (saa7146_register_extension(&extension)) {
+		DEB_S("failed to register extension\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit mxb_cleanup_module(void)
+{
+	saa7146_unregister_extension(&extension);
+}
+
+module_init(mxb_init_module);
+module_exit(mxb_cleanup_module);
+
+MODULE_DESCRIPTION("video4linux-2 driver for the Siemens-Nixdorf 'Multimedia eXtension board'");
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/Kconfig b/drivers/staging/media/deprecated/saa7146/ttpci/Kconfig
new file mode 100644
index 000000000..8c85ed58e
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/Kconfig
@@ -0,0 +1,95 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config DVB_BUDGET_CORE
+	tristate "SAA7146 DVB cards (aka Budget, Nova-PCI) (DEPRECATED)"
+	depends on DVB_CORE && PCI && I2C
+	select VIDEO_SAA7146
+	select TTPCI_EEPROM
+	help
+	  Support for simple SAA7146 based DVB cards
+	  (so called Budget- or Nova-PCI cards) without onboard
+	  MPEG2 decoder.
+
+config DVB_BUDGET
+	tristate "Budget cards (DEPRECATED)"
+	depends on DVB_BUDGET_CORE && I2C
+	select DVB_STV0299 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_VES1X93 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_VES1820 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_L64781 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA8083 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_S5H1420 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA10086 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA826X if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA1004X if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_ISL6423 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV090x if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV6110x if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Support for simple SAA7146 based DVB cards (so called Budget-
+	  or Nova-PCI cards) without onboard MPEG2 decoder, and without
+	  analog inputs or an onboard Common Interface connector.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a card and want to use it.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called budget.
+
+config DVB_BUDGET_CI
+	tristate "Budget cards with onboard CI connector (DEPRECATED)"
+	depends on DVB_BUDGET_CORE && I2C
+	select DVB_STV0297 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0299 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA1004X if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB0899 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB6100 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0288 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB6000 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA10023 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_TDA827X if MEDIA_SUBDRV_AUTOSELECT
+	depends on RC_CORE
+	help
+	  Support for simple SAA7146 based DVB cards
+	  (so called Budget- or Nova-PCI cards) without onboard
+	  MPEG2 decoder, but with onboard Common Interface connector.
+
+	  Note: The Common Interface is not yet supported by this driver
+	  due to lack of information from the vendor.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a card and want to use it.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called budget-ci.
+
+config DVB_BUDGET_AV
+	tristate "Budget cards with analog video inputs (DEPRECATED)"
+	depends on DVB_BUDGET_CORE && I2C
+	select VIDEO_SAA7146_VV
+	depends on VIDEO_DEV	# dependencies of VIDEO_SAA7146_VV
+	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0299 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA1004X if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA10021 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA10023 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB0899 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA8261 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TUA6100 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Support for simple SAA7146 based DVB cards
+	  (so called Budget- or Nova-PCI cards) without onboard
+	  MPEG2 decoder, but with one or more analog video inputs.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a card and want to use it.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called budget-av.
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/Makefile b/drivers/staging/media/deprecated/saa7146/ttpci/Makefile
new file mode 100644
index 000000000..b0708f6e4
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/Makefile
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the kernel SAA7146 FULL TS DVB device driver
+#
+
+obj-$(CONFIG_DVB_BUDGET_CORE) += budget-core.o
+obj-$(CONFIG_DVB_BUDGET) += budget.o
+obj-$(CONFIG_DVB_BUDGET_AV) += budget-av.o
+obj-$(CONFIG_DVB_BUDGET_CI) += budget-ci.o
+
+ccflags-y += -I $(srctree)/drivers/media/dvb-frontends/
+ccflags-y += -I $(srctree)/drivers/media/tuners
+ccflags-y += -I $(srctree)/drivers/media/common
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/TODO b/drivers/staging/media/deprecated/saa7146/ttpci/TODO
new file mode 100644
index 000000000..c9ae2ec79
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/TODO
@@ -0,0 +1,7 @@
+The saa7146-based drivers are one of the few drivers still not using
+the vb2 framework, so these drivers are now deprecated with the intent of
+removing them altogether by the beginning of 2023.
+
+In order to keep these drivers they have to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/budget-av.c b/drivers/staging/media/deprecated/saa7146/ttpci/budget-av.c
new file mode 100644
index 000000000..0c61a2dec
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/budget-av.c
@@ -0,0 +1,1622 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * budget-av.c: driver for the SAA7146 based Budget DVB cards
+ *              with analog video in
+ *
+ * Compiled from various sources by Michael Hunold <michael@mihu.de>
+ *
+ * CI interface support (c) 2004 Olivier Gournet <ogournet@anevia.com> &
+ *                               Andrew de Quincey <adq_dvb@lidskialf.net>
+ *
+ * Copyright (C) 2002 Ralph Metzler <rjkm@metzlerbros.de>
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "budget.h"
+#include "stv0299.h"
+#include "stb0899_drv.h"
+#include "stb0899_reg.h"
+#include "stb0899_cfg.h"
+#include "tda8261.h"
+#include "tda8261_cfg.h"
+#include "tda1002x.h"
+#include "tda1004x.h"
+#include "tua6100.h"
+#include "dvb-pll.h"
+#include "../common/saa7146_vv.h"
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/spinlock.h>
+
+#include <media/dvb_ca_en50221.h>
+
+#define DEBICICAM		0x02420000
+
+#define SLOTSTATUS_NONE         1
+#define SLOTSTATUS_PRESENT      2
+#define SLOTSTATUS_RESET        4
+#define SLOTSTATUS_READY        8
+#define SLOTSTATUS_OCCUPIED     (SLOTSTATUS_PRESENT|SLOTSTATUS_RESET|SLOTSTATUS_READY)
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+struct budget_av {
+	struct budget budget;
+	struct video_device vd;
+	int cur_input;
+	int has_saa7113;
+	struct tasklet_struct ciintf_irq_tasklet;
+	int slot_status;
+	struct dvb_ca_en50221 ca;
+	u8 reinitialise_demod:1;
+};
+
+static int ciintf_slot_shutdown(struct dvb_ca_en50221 *ca, int slot);
+
+
+/* GPIO Connections:
+ * 0 - Vcc/Reset (Reset is controlled by capacitor). Resets the frontend *AS WELL*!
+ * 1 - CI memory select 0=>IO memory, 1=>Attribute Memory
+ * 2 - CI Card Enable (Active Low)
+ * 3 - CI Card Detect
+ */
+
+/****************************************************************************
+ * INITIALIZATION
+ ****************************************************************************/
+
+static u8 i2c_readreg(struct i2c_adapter *i2c, u8 id, u8 reg)
+{
+	u8 mm1[] = { 0x00 };
+	u8 mm2[] = { 0x00 };
+	struct i2c_msg msgs[2];
+
+	msgs[0].flags = 0;
+	msgs[1].flags = I2C_M_RD;
+	msgs[0].addr = msgs[1].addr = id / 2;
+	mm1[0] = reg;
+	msgs[0].len = 1;
+	msgs[1].len = 1;
+	msgs[0].buf = mm1;
+	msgs[1].buf = mm2;
+
+	i2c_transfer(i2c, msgs, 2);
+
+	return mm2[0];
+}
+
+static int i2c_readregs(struct i2c_adapter *i2c, u8 id, u8 reg, u8 * buf, u8 len)
+{
+	u8 mm1[] = { reg };
+	struct i2c_msg msgs[2] = {
+		{.addr = id / 2,.flags = 0,.buf = mm1,.len = 1},
+		{.addr = id / 2,.flags = I2C_M_RD,.buf = buf,.len = len}
+	};
+
+	if (i2c_transfer(i2c, msgs, 2) != 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int i2c_writereg(struct i2c_adapter *i2c, u8 id, u8 reg, u8 val)
+{
+	u8 msg[2] = { reg, val };
+	struct i2c_msg msgs;
+
+	msgs.flags = 0;
+	msgs.addr = id / 2;
+	msgs.len = 2;
+	msgs.buf = msg;
+	return i2c_transfer(i2c, &msgs, 1);
+}
+
+static int ciintf_read_attribute_mem(struct dvb_ca_en50221 *ca, int slot, int address)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	int result;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTHI);
+	udelay(1);
+
+	result = ttpci_budget_debiread(&budget_av->budget, DEBICICAM, address & 0xfff, 1, 0, 1);
+	if (result == -ETIMEDOUT) {
+		ciintf_slot_shutdown(ca, slot);
+		pr_info("cam ejected 1\n");
+	}
+	return result;
+}
+
+static int ciintf_write_attribute_mem(struct dvb_ca_en50221 *ca, int slot, int address, u8 value)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	int result;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTHI);
+	udelay(1);
+
+	result = ttpci_budget_debiwrite(&budget_av->budget, DEBICICAM, address & 0xfff, 1, value, 0, 1);
+	if (result == -ETIMEDOUT) {
+		ciintf_slot_shutdown(ca, slot);
+		pr_info("cam ejected 2\n");
+	}
+	return result;
+}
+
+static int ciintf_read_cam_control(struct dvb_ca_en50221 *ca, int slot, u8 address)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	int result;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTLO);
+	udelay(1);
+
+	result = ttpci_budget_debiread(&budget_av->budget, DEBICICAM, address & 3, 1, 0, 0);
+	if (result == -ETIMEDOUT) {
+		ciintf_slot_shutdown(ca, slot);
+		pr_info("cam ejected 3\n");
+		return -ETIMEDOUT;
+	}
+	return result;
+}
+
+static int ciintf_write_cam_control(struct dvb_ca_en50221 *ca, int slot, u8 address, u8 value)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	int result;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTLO);
+	udelay(1);
+
+	result = ttpci_budget_debiwrite(&budget_av->budget, DEBICICAM, address & 3, 1, value, 0, 0);
+	if (result == -ETIMEDOUT) {
+		ciintf_slot_shutdown(ca, slot);
+		pr_info("cam ejected 5\n");
+	}
+	return result;
+}
+
+static int ciintf_slot_reset(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	struct saa7146_dev *saa = budget_av->budget.dev;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	dprintk(1, "ciintf_slot_reset\n");
+	budget_av->slot_status = SLOTSTATUS_RESET;
+
+	saa7146_setgpio(saa, 2, SAA7146_GPIO_OUTHI); /* disable card */
+
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTHI); /* Vcc off */
+	msleep(2);
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTLO); /* Vcc on */
+	msleep(20); /* 20 ms Vcc settling time */
+
+	saa7146_setgpio(saa, 2, SAA7146_GPIO_OUTLO); /* enable card */
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTB);
+	msleep(20);
+
+	/* reinitialise the frontend if necessary */
+	if (budget_av->reinitialise_demod)
+		dvb_frontend_reinitialise(budget_av->budget.dvb_frontend);
+
+	return 0;
+}
+
+static int ciintf_slot_shutdown(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	struct saa7146_dev *saa = budget_av->budget.dev;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	dprintk(1, "ciintf_slot_shutdown\n");
+
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTB);
+	budget_av->slot_status = SLOTSTATUS_NONE;
+
+	return 0;
+}
+
+static int ciintf_slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	struct saa7146_dev *saa = budget_av->budget.dev;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	dprintk(1, "ciintf_slot_ts_enable: %d\n", budget_av->slot_status);
+
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTA);
+
+	return 0;
+}
+
+static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open)
+{
+	struct budget_av *budget_av = (struct budget_av *) ca->data;
+	struct saa7146_dev *saa = budget_av->budget.dev;
+	int result;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	/* test the card detect line - needs to be done carefully
+	 * since it never goes high for some CAMs on this interface (e.g. topuptv) */
+	if (budget_av->slot_status == SLOTSTATUS_NONE) {
+		saa7146_setgpio(saa, 3, SAA7146_GPIO_INPUT);
+		udelay(1);
+		if (saa7146_read(saa, PSR) & MASK_06) {
+			if (budget_av->slot_status == SLOTSTATUS_NONE) {
+				budget_av->slot_status = SLOTSTATUS_PRESENT;
+				pr_info("cam inserted A\n");
+			}
+		}
+		saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTLO);
+	}
+
+	/* We also try and read from IO memory to work round the above detection bug. If
+	 * there is no CAM, we will get a timeout. Only done if there is no cam
+	 * present, since this test actually breaks some cams :(
+	 *
+	 * if the CI interface is not open, we also do the above test since we
+	 * don't care if the cam has problems - we'll be resetting it on open() anyway */
+	if ((budget_av->slot_status == SLOTSTATUS_NONE) || (!open)) {
+		saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTLO);
+		result = ttpci_budget_debiread(&budget_av->budget, DEBICICAM, 0, 1, 0, 1);
+		if ((result >= 0) && (budget_av->slot_status == SLOTSTATUS_NONE)) {
+			budget_av->slot_status = SLOTSTATUS_PRESENT;
+			pr_info("cam inserted B\n");
+		} else if (result < 0) {
+			if (budget_av->slot_status != SLOTSTATUS_NONE) {
+				ciintf_slot_shutdown(ca, slot);
+				pr_info("cam ejected 5\n");
+				return 0;
+			}
+		}
+	}
+
+	/* read from attribute memory in reset/ready state to know when the CAM is ready */
+	if (budget_av->slot_status == SLOTSTATUS_RESET) {
+		result = ciintf_read_attribute_mem(ca, slot, 0);
+		if (result == 0x1d) {
+			budget_av->slot_status = SLOTSTATUS_READY;
+		}
+	}
+
+	/* work out correct return code */
+	if (budget_av->slot_status != SLOTSTATUS_NONE) {
+		if (budget_av->slot_status & SLOTSTATUS_READY) {
+			return DVB_CA_EN50221_POLL_CAM_PRESENT | DVB_CA_EN50221_POLL_CAM_READY;
+		}
+		return DVB_CA_EN50221_POLL_CAM_PRESENT;
+	}
+	return 0;
+}
+
+static int ciintf_init(struct budget_av *budget_av)
+{
+	struct saa7146_dev *saa = budget_av->budget.dev;
+	int result;
+
+	memset(&budget_av->ca, 0, sizeof(struct dvb_ca_en50221));
+
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTLO);
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_OUTLO);
+	saa7146_setgpio(saa, 2, SAA7146_GPIO_OUTLO);
+	saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTLO);
+
+	/* Enable DEBI pins */
+	saa7146_write(saa, MC1, MASK_27 | MASK_11);
+
+	/* register CI interface */
+	budget_av->ca.owner = THIS_MODULE;
+	budget_av->ca.read_attribute_mem = ciintf_read_attribute_mem;
+	budget_av->ca.write_attribute_mem = ciintf_write_attribute_mem;
+	budget_av->ca.read_cam_control = ciintf_read_cam_control;
+	budget_av->ca.write_cam_control = ciintf_write_cam_control;
+	budget_av->ca.slot_reset = ciintf_slot_reset;
+	budget_av->ca.slot_shutdown = ciintf_slot_shutdown;
+	budget_av->ca.slot_ts_enable = ciintf_slot_ts_enable;
+	budget_av->ca.poll_slot_status = ciintf_poll_slot_status;
+	budget_av->ca.data = budget_av;
+	budget_av->budget.ci_present = 1;
+	budget_av->slot_status = SLOTSTATUS_NONE;
+
+	if ((result = dvb_ca_en50221_init(&budget_av->budget.dvb_adapter,
+					  &budget_av->ca, 0, 1)) != 0) {
+		pr_err("ci initialisation failed\n");
+		goto error;
+	}
+
+	pr_info("ci interface initialised\n");
+	return 0;
+
+error:
+	saa7146_write(saa, MC1, MASK_27);
+	return result;
+}
+
+static void ciintf_deinit(struct budget_av *budget_av)
+{
+	struct saa7146_dev *saa = budget_av->budget.dev;
+
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_INPUT);
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_INPUT);
+	saa7146_setgpio(saa, 2, SAA7146_GPIO_INPUT);
+	saa7146_setgpio(saa, 3, SAA7146_GPIO_INPUT);
+
+	/* release the CA device */
+	dvb_ca_en50221_release(&budget_av->ca);
+
+	/* disable DEBI pins */
+	saa7146_write(saa, MC1, MASK_27);
+}
+
+
+static const u8 saa7113_tab[] = {
+	0x01, 0x08,
+	0x02, 0xc0,
+	0x03, 0x33,
+	0x04, 0x00,
+	0x05, 0x00,
+	0x06, 0xeb,
+	0x07, 0xe0,
+	0x08, 0x28,
+	0x09, 0x00,
+	0x0a, 0x80,
+	0x0b, 0x47,
+	0x0c, 0x40,
+	0x0d, 0x00,
+	0x0e, 0x01,
+	0x0f, 0x44,
+
+	0x10, 0x08,
+	0x11, 0x0c,
+	0x12, 0x7b,
+	0x13, 0x00,
+	0x15, 0x00, 0x16, 0x00, 0x17, 0x00,
+
+	0x57, 0xff,
+	0x40, 0x82, 0x58, 0x00, 0x59, 0x54, 0x5a, 0x07,
+	0x5b, 0x83, 0x5e, 0x00,
+	0xff
+};
+
+static int saa7113_init(struct budget_av *budget_av)
+{
+	struct budget *budget = &budget_av->budget;
+	struct saa7146_dev *saa = budget->dev;
+	const u8 *data = saa7113_tab;
+
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTHI);
+	msleep(200);
+
+	if (i2c_writereg(&budget->i2c_adap, 0x4a, 0x01, 0x08) != 1) {
+		dprintk(1, "saa7113 not found on KNC card\n");
+		return -ENODEV;
+	}
+
+	dprintk(1, "saa7113 detected and initializing\n");
+
+	while (*data != 0xff) {
+		i2c_writereg(&budget->i2c_adap, 0x4a, *data, *(data + 1));
+		data += 2;
+	}
+
+	dprintk(1, "saa7113  status=%02x\n", i2c_readreg(&budget->i2c_adap, 0x4a, 0x1f));
+
+	return 0;
+}
+
+static int saa7113_setinput(struct budget_av *budget_av, int input)
+{
+	struct budget *budget = &budget_av->budget;
+
+	if (1 != budget_av->has_saa7113)
+		return -ENODEV;
+
+	if (input == 1) {
+		i2c_writereg(&budget->i2c_adap, 0x4a, 0x02, 0xc7);
+		i2c_writereg(&budget->i2c_adap, 0x4a, 0x09, 0x80);
+	} else if (input == 0) {
+		i2c_writereg(&budget->i2c_adap, 0x4a, 0x02, 0xc0);
+		i2c_writereg(&budget->i2c_adap, 0x4a, 0x09, 0x00);
+	} else
+		return -EINVAL;
+
+	budget_av->cur_input = input;
+	return 0;
+}
+
+
+static int philips_su1278_ty_ci_set_symbol_rate(struct dvb_frontend *fe, u32 srate, u32 ratio)
+{
+	u8 aclk = 0;
+	u8 bclk = 0;
+	u8 m1;
+
+	aclk = 0xb5;
+	if (srate < 2000000)
+		bclk = 0x86;
+	else if (srate < 5000000)
+		bclk = 0x89;
+	else if (srate < 15000000)
+		bclk = 0x8f;
+	else if (srate < 45000000)
+		bclk = 0x95;
+
+	m1 = 0x14;
+	if (srate < 4000000)
+		m1 = 0x10;
+
+	stv0299_writereg(fe, 0x13, aclk);
+	stv0299_writereg(fe, 0x14, bclk);
+	stv0299_writereg(fe, 0x1f, (ratio >> 16) & 0xff);
+	stv0299_writereg(fe, 0x20, (ratio >> 8) & 0xff);
+	stv0299_writereg(fe, 0x21, (ratio) & 0xf0);
+	stv0299_writereg(fe, 0x0f, 0x80 | m1);
+
+	return 0;
+}
+
+static int philips_su1278_ty_ci_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u32 div;
+	u8 buf[4];
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+	struct i2c_msg msg = {.addr = 0x61,.flags = 0,.buf = buf,.len = sizeof(buf) };
+
+	if ((c->frequency < 950000) || (c->frequency > 2150000))
+		return -EINVAL;
+
+	div = (c->frequency + (125 - 1)) / 125;	/* round correctly */
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = 0x80 | ((div & 0x18000) >> 10) | 4;
+	buf[3] = 0x20;
+
+	if (c->symbol_rate < 4000000)
+		buf[3] |= 1;
+
+	if (c->frequency < 1250000)
+		buf[3] |= 0;
+	else if (c->frequency < 1550000)
+		buf[3] |= 0x40;
+	else if (c->frequency < 2050000)
+		buf[3] |= 0x80;
+	else if (c->frequency < 2150000)
+		buf[3] |= 0xC0;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static u8 typhoon_cinergy1200s_inittab[] = {
+	0x01, 0x15,
+	0x02, 0x30,
+	0x03, 0x00,
+	0x04, 0x7d,		/* F22FR = 0x7d, F22 = f_VCO / 128 / 0x7d = 22 kHz */
+	0x05, 0x35,		/* I2CT = 0, SCLT = 1, SDAT = 1 */
+	0x06, 0x40,		/* DAC not used, set to high impendance mode */
+	0x07, 0x00,		/* DAC LSB */
+	0x08, 0x40,		/* DiSEqC off */
+	0x09, 0x00,		/* FIFO */
+	0x0c, 0x51,		/* OP1 ctl = Normal, OP1 val = 1 (LNB Power ON) */
+	0x0d, 0x82,		/* DC offset compensation = ON, beta_agc1 = 2 */
+	0x0e, 0x23,		/* alpha_tmg = 2, beta_tmg = 3 */
+	0x10, 0x3f,		// AGC2  0x3d
+	0x11, 0x84,
+	0x12, 0xb9,
+	0x15, 0xc9,		// lock detector threshold
+	0x16, 0x00,
+	0x17, 0x00,
+	0x18, 0x00,
+	0x19, 0x00,
+	0x1a, 0x00,
+	0x1f, 0x50,
+	0x20, 0x00,
+	0x21, 0x00,
+	0x22, 0x00,
+	0x23, 0x00,
+	0x28, 0x00,		// out imp: normal  out type: parallel FEC mode:0
+	0x29, 0x1e,		// 1/2 threshold
+	0x2a, 0x14,		// 2/3 threshold
+	0x2b, 0x0f,		// 3/4 threshold
+	0x2c, 0x09,		// 5/6 threshold
+	0x2d, 0x05,		// 7/8 threshold
+	0x2e, 0x01,
+	0x31, 0x1f,		// test all FECs
+	0x32, 0x19,		// viterbi and synchro search
+	0x33, 0xfc,		// rs control
+	0x34, 0x93,		// error control
+	0x0f, 0x92,
+	0xff, 0xff
+};
+
+static const struct stv0299_config typhoon_config = {
+	.demod_address = 0x68,
+	.inittab = typhoon_cinergy1200s_inittab,
+	.mclk = 88000000UL,
+	.invert = 0,
+	.skip_reinit = 0,
+	.lock_output = STV0299_LOCKOUTPUT_1,
+	.volt13_op0_op1 = STV0299_VOLT13_OP0,
+	.min_delay_ms = 100,
+	.set_symbol_rate = philips_su1278_ty_ci_set_symbol_rate,
+};
+
+
+static const struct stv0299_config cinergy_1200s_config = {
+	.demod_address = 0x68,
+	.inittab = typhoon_cinergy1200s_inittab,
+	.mclk = 88000000UL,
+	.invert = 0,
+	.skip_reinit = 0,
+	.lock_output = STV0299_LOCKOUTPUT_0,
+	.volt13_op0_op1 = STV0299_VOLT13_OP0,
+	.min_delay_ms = 100,
+	.set_symbol_rate = philips_su1278_ty_ci_set_symbol_rate,
+};
+
+static const struct stv0299_config cinergy_1200s_1894_0010_config = {
+	.demod_address = 0x68,
+	.inittab = typhoon_cinergy1200s_inittab,
+	.mclk = 88000000UL,
+	.invert = 1,
+	.skip_reinit = 0,
+	.lock_output = STV0299_LOCKOUTPUT_1,
+	.volt13_op0_op1 = STV0299_VOLT13_OP0,
+	.min_delay_ms = 100,
+	.set_symbol_rate = philips_su1278_ty_ci_set_symbol_rate,
+};
+
+static int philips_cu1216_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+	u8 buf[6];
+	struct i2c_msg msg = {.addr = 0x60,.flags = 0,.buf = buf,.len = sizeof(buf) };
+	int i;
+
+#define CU1216_IF 36125000
+#define TUNER_MUL 62500
+
+	u32 div = (c->frequency + CU1216_IF + TUNER_MUL / 2) / TUNER_MUL;
+
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = 0xce;
+	buf[3] = (c->frequency < 150000000 ? 0x01 :
+		  c->frequency < 445000000 ? 0x02 : 0x04);
+	buf[4] = 0xde;
+	buf[5] = 0x20;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+
+	/* wait for the pll lock */
+	msg.flags = I2C_M_RD;
+	msg.len = 1;
+	for (i = 0; i < 20; i++) {
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 1);
+		if (i2c_transfer(&budget->i2c_adap, &msg, 1) == 1 && (buf[0] & 0x40))
+			break;
+		msleep(10);
+	}
+
+	/* switch the charge pump to the lower current */
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[2];
+	buf[2] &= ~0x40;
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static struct tda1002x_config philips_cu1216_config = {
+	.demod_address = 0x0c,
+	.invert = 1,
+};
+
+static struct tda1002x_config philips_cu1216_config_altaddress = {
+	.demod_address = 0x0d,
+	.invert = 0,
+};
+
+static struct tda10023_config philips_cu1216_tda10023_config = {
+	.demod_address = 0x0c,
+	.invert = 1,
+};
+
+static int philips_tu1216_tuner_init(struct dvb_frontend *fe)
+{
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+	static u8 tu1216_init[] = { 0x0b, 0xf5, 0x85, 0xab };
+	struct i2c_msg tuner_msg = {.addr = 0x60,.flags = 0,.buf = tu1216_init,.len = sizeof(tu1216_init) };
+
+	// setup PLL configuration
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget->i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+	msleep(1);
+
+	return 0;
+}
+
+static int philips_tu1216_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+	u8 tuner_buf[4];
+	struct i2c_msg tuner_msg = {.addr = 0x60,.flags = 0,.buf = tuner_buf,.len =
+			sizeof(tuner_buf) };
+	int tuner_frequency = 0;
+	u8 band, cp, filter;
+
+	// determine charge pump
+	tuner_frequency = c->frequency + 36166000;
+	if (tuner_frequency < 87000000)
+		return -EINVAL;
+	else if (tuner_frequency < 130000000)
+		cp = 3;
+	else if (tuner_frequency < 160000000)
+		cp = 5;
+	else if (tuner_frequency < 200000000)
+		cp = 6;
+	else if (tuner_frequency < 290000000)
+		cp = 3;
+	else if (tuner_frequency < 420000000)
+		cp = 5;
+	else if (tuner_frequency < 480000000)
+		cp = 6;
+	else if (tuner_frequency < 620000000)
+		cp = 3;
+	else if (tuner_frequency < 830000000)
+		cp = 5;
+	else if (tuner_frequency < 895000000)
+		cp = 7;
+	else
+		return -EINVAL;
+
+	// determine band
+	if (c->frequency < 49000000)
+		return -EINVAL;
+	else if (c->frequency < 161000000)
+		band = 1;
+	else if (c->frequency < 444000000)
+		band = 2;
+	else if (c->frequency < 861000000)
+		band = 4;
+	else
+		return -EINVAL;
+
+	// setup PLL filter
+	switch (c->bandwidth_hz) {
+	case 6000000:
+		filter = 0;
+		break;
+
+	case 7000000:
+		filter = 0;
+		break;
+
+	case 8000000:
+		filter = 1;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	// calculate divisor
+	// ((36166000+((1000000/6)/2)) + Finput)/(1000000/6)
+	tuner_frequency = (((c->frequency / 1000) * 6) + 217496) / 1000;
+
+	// setup tuner buffer
+	tuner_buf[0] = (tuner_frequency >> 8) & 0x7f;
+	tuner_buf[1] = tuner_frequency & 0xff;
+	tuner_buf[2] = 0xca;
+	tuner_buf[3] = (cp << 5) | (filter << 3) | band;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget->i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+
+	msleep(1);
+	return 0;
+}
+
+static int philips_tu1216_request_firmware(struct dvb_frontend *fe,
+					   const struct firmware **fw, char *name)
+{
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+
+	return request_firmware(fw, name, &budget->dev->pci->dev);
+}
+
+static struct tda1004x_config philips_tu1216_config = {
+
+	.demod_address = 0x8,
+	.invert = 1,
+	.invert_oclk = 1,
+	.xtal_freq = TDA10046_XTAL_4M,
+	.agc_config = TDA10046_AGC_DEFAULT,
+	.if_freq = TDA10046_FREQ_3617,
+	.request_firmware = philips_tu1216_request_firmware,
+};
+
+static u8 philips_sd1878_inittab[] = {
+	0x01, 0x15,
+	0x02, 0x30,
+	0x03, 0x00,
+	0x04, 0x7d,
+	0x05, 0x35,
+	0x06, 0x40,
+	0x07, 0x00,
+	0x08, 0x43,
+	0x09, 0x02,
+	0x0C, 0x51,
+	0x0D, 0x82,
+	0x0E, 0x23,
+	0x10, 0x3f,
+	0x11, 0x84,
+	0x12, 0xb9,
+	0x15, 0xc9,
+	0x16, 0x19,
+	0x17, 0x8c,
+	0x18, 0x59,
+	0x19, 0xf8,
+	0x1a, 0xfe,
+	0x1c, 0x7f,
+	0x1d, 0x00,
+	0x1e, 0x00,
+	0x1f, 0x50,
+	0x20, 0x00,
+	0x21, 0x00,
+	0x22, 0x00,
+	0x23, 0x00,
+	0x28, 0x00,
+	0x29, 0x28,
+	0x2a, 0x14,
+	0x2b, 0x0f,
+	0x2c, 0x09,
+	0x2d, 0x09,
+	0x31, 0x1f,
+	0x32, 0x19,
+	0x33, 0xfc,
+	0x34, 0x93,
+	0xff, 0xff
+};
+
+static int philips_sd1878_ci_set_symbol_rate(struct dvb_frontend *fe,
+		u32 srate, u32 ratio)
+{
+	u8 aclk = 0;
+	u8 bclk = 0;
+	u8 m1;
+
+	aclk = 0xb5;
+	if (srate < 2000000)
+		bclk = 0x86;
+	else if (srate < 5000000)
+		bclk = 0x89;
+	else if (srate < 15000000)
+		bclk = 0x8f;
+	else if (srate < 45000000)
+		bclk = 0x95;
+
+	m1 = 0x14;
+	if (srate < 4000000)
+		m1 = 0x10;
+
+	stv0299_writereg(fe, 0x0e, 0x23);
+	stv0299_writereg(fe, 0x0f, 0x94);
+	stv0299_writereg(fe, 0x10, 0x39);
+	stv0299_writereg(fe, 0x13, aclk);
+	stv0299_writereg(fe, 0x14, bclk);
+	stv0299_writereg(fe, 0x15, 0xc9);
+	stv0299_writereg(fe, 0x1f, (ratio >> 16) & 0xff);
+	stv0299_writereg(fe, 0x20, (ratio >> 8) & 0xff);
+	stv0299_writereg(fe, 0x21, (ratio) & 0xf0);
+	stv0299_writereg(fe, 0x0f, 0x80 | m1);
+
+	return 0;
+}
+
+static const struct stv0299_config philips_sd1878_config = {
+	.demod_address = 0x68,
+     .inittab = philips_sd1878_inittab,
+	.mclk = 88000000UL,
+	.invert = 0,
+	.skip_reinit = 0,
+	.lock_output = STV0299_LOCKOUTPUT_1,
+	.volt13_op0_op1 = STV0299_VOLT13_OP0,
+	.min_delay_ms = 100,
+	.set_symbol_rate = philips_sd1878_ci_set_symbol_rate,
+};
+
+/* KNC1 DVB-S (STB0899) Inittab	*/
+static const struct stb0899_s1_reg knc1_stb0899_s1_init_1[] = {
+
+	{ STB0899_DEV_ID		, 0x81 },
+	{ STB0899_DISCNTRL1		, 0x32 },
+	{ STB0899_DISCNTRL2		, 0x80 },
+	{ STB0899_DISRX_ST0		, 0x04 },
+	{ STB0899_DISRX_ST1		, 0x00 },
+	{ STB0899_DISPARITY		, 0x00 },
+	{ STB0899_DISSTATUS		, 0x20 },
+	{ STB0899_DISF22		, 0x8c },
+	{ STB0899_DISF22RX		, 0x9a },
+	{ STB0899_SYSREG		, 0x0b },
+	{ STB0899_ACRPRESC		, 0x11 },
+	{ STB0899_ACRDIV1		, 0x0a },
+	{ STB0899_ACRDIV2		, 0x05 },
+	{ STB0899_DACR1			, 0x00 },
+	{ STB0899_DACR2			, 0x00 },
+	{ STB0899_OUTCFG		, 0x00 },
+	{ STB0899_MODECFG		, 0x00 },
+	{ STB0899_IRQSTATUS_3		, 0x30 },
+	{ STB0899_IRQSTATUS_2		, 0x00 },
+	{ STB0899_IRQSTATUS_1		, 0x00 },
+	{ STB0899_IRQSTATUS_0		, 0x00 },
+	{ STB0899_IRQMSK_3		, 0xf3 },
+	{ STB0899_IRQMSK_2		, 0xfc },
+	{ STB0899_IRQMSK_1		, 0xff },
+	{ STB0899_IRQMSK_0		, 0xff },
+	{ STB0899_IRQCFG		, 0x00 },
+	{ STB0899_I2CCFG		, 0x88 },
+	{ STB0899_I2CRPT		, 0x58 }, /* Repeater=8, Stop=disabled */
+	{ STB0899_IOPVALUE5		, 0x00 },
+	{ STB0899_IOPVALUE4		, 0x20 },
+	{ STB0899_IOPVALUE3		, 0xc9 },
+	{ STB0899_IOPVALUE2		, 0x90 },
+	{ STB0899_IOPVALUE1		, 0x40 },
+	{ STB0899_IOPVALUE0		, 0x00 },
+	{ STB0899_GPIO00CFG		, 0x82 },
+	{ STB0899_GPIO01CFG		, 0x82 },
+	{ STB0899_GPIO02CFG		, 0x82 },
+	{ STB0899_GPIO03CFG		, 0x82 },
+	{ STB0899_GPIO04CFG		, 0x82 },
+	{ STB0899_GPIO05CFG		, 0x82 },
+	{ STB0899_GPIO06CFG		, 0x82 },
+	{ STB0899_GPIO07CFG		, 0x82 },
+	{ STB0899_GPIO08CFG		, 0x82 },
+	{ STB0899_GPIO09CFG		, 0x82 },
+	{ STB0899_GPIO10CFG		, 0x82 },
+	{ STB0899_GPIO11CFG		, 0x82 },
+	{ STB0899_GPIO12CFG		, 0x82 },
+	{ STB0899_GPIO13CFG		, 0x82 },
+	{ STB0899_GPIO14CFG		, 0x82 },
+	{ STB0899_GPIO15CFG		, 0x82 },
+	{ STB0899_GPIO16CFG		, 0x82 },
+	{ STB0899_GPIO17CFG		, 0x82 },
+	{ STB0899_GPIO18CFG		, 0x82 },
+	{ STB0899_GPIO19CFG		, 0x82 },
+	{ STB0899_GPIO20CFG		, 0x82 },
+	{ STB0899_SDATCFG		, 0xb8 },
+	{ STB0899_SCLTCFG		, 0xba },
+	{ STB0899_AGCRFCFG		, 0x08 }, /* 0x1c */
+	{ STB0899_GPIO22		, 0x82 }, /* AGCBB2CFG */
+	{ STB0899_GPIO21		, 0x91 }, /* AGCBB1CFG */
+	{ STB0899_DIRCLKCFG		, 0x82 },
+	{ STB0899_CLKOUT27CFG		, 0x7e },
+	{ STB0899_STDBYCFG		, 0x82 },
+	{ STB0899_CS0CFG		, 0x82 },
+	{ STB0899_CS1CFG		, 0x82 },
+	{ STB0899_DISEQCOCFG		, 0x20 },
+	{ STB0899_GPIO32CFG		, 0x82 },
+	{ STB0899_GPIO33CFG		, 0x82 },
+	{ STB0899_GPIO34CFG		, 0x82 },
+	{ STB0899_GPIO35CFG		, 0x82 },
+	{ STB0899_GPIO36CFG		, 0x82 },
+	{ STB0899_GPIO37CFG		, 0x82 },
+	{ STB0899_GPIO38CFG		, 0x82 },
+	{ STB0899_GPIO39CFG		, 0x82 },
+	{ STB0899_NCOARSE		, 0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
+	{ STB0899_SYNTCTRL		, 0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
+	{ STB0899_FILTCTRL		, 0x00 },
+	{ STB0899_SYSCTRL		, 0x00 },
+	{ STB0899_STOPCLK1		, 0x20 },
+	{ STB0899_STOPCLK2		, 0x00 },
+	{ STB0899_INTBUFSTATUS		, 0x00 },
+	{ STB0899_INTBUFCTRL		, 0x0a },
+	{ 0xffff			, 0xff },
+};
+
+static const struct stb0899_s1_reg knc1_stb0899_s1_init_3[] = {
+	{ STB0899_DEMOD			, 0x00 },
+	{ STB0899_RCOMPC		, 0xc9 },
+	{ STB0899_AGC1CN		, 0x41 },
+	{ STB0899_AGC1REF		, 0x08 },
+	{ STB0899_RTC			, 0x7a },
+	{ STB0899_TMGCFG		, 0x4e },
+	{ STB0899_AGC2REF		, 0x33 },
+	{ STB0899_TLSR			, 0x84 },
+	{ STB0899_CFD			, 0xee },
+	{ STB0899_ACLC			, 0x87 },
+	{ STB0899_BCLC			, 0x94 },
+	{ STB0899_EQON			, 0x41 },
+	{ STB0899_LDT			, 0xdd },
+	{ STB0899_LDT2			, 0xc9 },
+	{ STB0899_EQUALREF		, 0xb4 },
+	{ STB0899_TMGRAMP		, 0x10 },
+	{ STB0899_TMGTHD		, 0x30 },
+	{ STB0899_IDCCOMP		, 0xfb },
+	{ STB0899_QDCCOMP		, 0x03 },
+	{ STB0899_POWERI		, 0x3b },
+	{ STB0899_POWERQ		, 0x3d },
+	{ STB0899_RCOMP			, 0x81 },
+	{ STB0899_AGCIQIN		, 0x80 },
+	{ STB0899_AGC2I1		, 0x04 },
+	{ STB0899_AGC2I2		, 0xf5 },
+	{ STB0899_TLIR			, 0x25 },
+	{ STB0899_RTF			, 0x80 },
+	{ STB0899_DSTATUS		, 0x00 },
+	{ STB0899_LDI			, 0xca },
+	{ STB0899_CFRM			, 0xf1 },
+	{ STB0899_CFRL			, 0xf3 },
+	{ STB0899_NIRM			, 0x2a },
+	{ STB0899_NIRL			, 0x05 },
+	{ STB0899_ISYMB			, 0x17 },
+	{ STB0899_QSYMB			, 0xfa },
+	{ STB0899_SFRH			, 0x2f },
+	{ STB0899_SFRM			, 0x68 },
+	{ STB0899_SFRL			, 0x40 },
+	{ STB0899_SFRUPH		, 0x2f },
+	{ STB0899_SFRUPM		, 0x68 },
+	{ STB0899_SFRUPL		, 0x40 },
+	{ STB0899_EQUAI1		, 0xfd },
+	{ STB0899_EQUAQ1		, 0x04 },
+	{ STB0899_EQUAI2		, 0x0f },
+	{ STB0899_EQUAQ2		, 0xff },
+	{ STB0899_EQUAI3		, 0xdf },
+	{ STB0899_EQUAQ3		, 0xfa },
+	{ STB0899_EQUAI4		, 0x37 },
+	{ STB0899_EQUAQ4		, 0x0d },
+	{ STB0899_EQUAI5		, 0xbd },
+	{ STB0899_EQUAQ5		, 0xf7 },
+	{ STB0899_DSTATUS2		, 0x00 },
+	{ STB0899_VSTATUS		, 0x00 },
+	{ STB0899_VERROR		, 0xff },
+	{ STB0899_IQSWAP		, 0x2a },
+	{ STB0899_ECNT1M		, 0x00 },
+	{ STB0899_ECNT1L		, 0x00 },
+	{ STB0899_ECNT2M		, 0x00 },
+	{ STB0899_ECNT2L		, 0x00 },
+	{ STB0899_ECNT3M		, 0x00 },
+	{ STB0899_ECNT3L		, 0x00 },
+	{ STB0899_FECAUTO1		, 0x06 },
+	{ STB0899_FECM			, 0x01 },
+	{ STB0899_VTH12			, 0xf0 },
+	{ STB0899_VTH23			, 0xa0 },
+	{ STB0899_VTH34			, 0x78 },
+	{ STB0899_VTH56			, 0x4e },
+	{ STB0899_VTH67			, 0x48 },
+	{ STB0899_VTH78			, 0x38 },
+	{ STB0899_PRVIT			, 0xff },
+	{ STB0899_VITSYNC		, 0x19 },
+	{ STB0899_RSULC			, 0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
+	{ STB0899_TSULC			, 0x42 },
+	{ STB0899_RSLLC			, 0x40 },
+	{ STB0899_TSLPL			, 0x12 },
+	{ STB0899_TSCFGH		, 0x0c },
+	{ STB0899_TSCFGM		, 0x00 },
+	{ STB0899_TSCFGL		, 0x0c },
+	{ STB0899_TSOUT			, 0x4d }, /* 0x0d for CAM */
+	{ STB0899_RSSYNCDEL		, 0x00 },
+	{ STB0899_TSINHDELH		, 0x02 },
+	{ STB0899_TSINHDELM		, 0x00 },
+	{ STB0899_TSINHDELL		, 0x00 },
+	{ STB0899_TSLLSTKM		, 0x00 },
+	{ STB0899_TSLLSTKL		, 0x00 },
+	{ STB0899_TSULSTKM		, 0x00 },
+	{ STB0899_TSULSTKL		, 0xab },
+	{ STB0899_PCKLENUL		, 0x00 },
+	{ STB0899_PCKLENLL		, 0xcc },
+	{ STB0899_RSPCKLEN		, 0xcc },
+	{ STB0899_TSSTATUS		, 0x80 },
+	{ STB0899_ERRCTRL1		, 0xb6 },
+	{ STB0899_ERRCTRL2		, 0x96 },
+	{ STB0899_ERRCTRL3		, 0x89 },
+	{ STB0899_DMONMSK1		, 0x27 },
+	{ STB0899_DMONMSK0		, 0x03 },
+	{ STB0899_DEMAPVIT		, 0x5c },
+	{ STB0899_PLPARM		, 0x1f },
+	{ STB0899_PDELCTRL		, 0x48 },
+	{ STB0899_PDELCTRL2		, 0x00 },
+	{ STB0899_BBHCTRL1		, 0x00 },
+	{ STB0899_BBHCTRL2		, 0x00 },
+	{ STB0899_HYSTTHRESH		, 0x77 },
+	{ STB0899_MATCSTM		, 0x00 },
+	{ STB0899_MATCSTL		, 0x00 },
+	{ STB0899_UPLCSTM		, 0x00 },
+	{ STB0899_UPLCSTL		, 0x00 },
+	{ STB0899_DFLCSTM		, 0x00 },
+	{ STB0899_DFLCSTL		, 0x00 },
+	{ STB0899_SYNCCST		, 0x00 },
+	{ STB0899_SYNCDCSTM		, 0x00 },
+	{ STB0899_SYNCDCSTL		, 0x00 },
+	{ STB0899_ISI_ENTRY		, 0x00 },
+	{ STB0899_ISI_BIT_EN		, 0x00 },
+	{ STB0899_MATSTRM		, 0x00 },
+	{ STB0899_MATSTRL		, 0x00 },
+	{ STB0899_UPLSTRM		, 0x00 },
+	{ STB0899_UPLSTRL		, 0x00 },
+	{ STB0899_DFLSTRM		, 0x00 },
+	{ STB0899_DFLSTRL		, 0x00 },
+	{ STB0899_SYNCSTR		, 0x00 },
+	{ STB0899_SYNCDSTRM		, 0x00 },
+	{ STB0899_SYNCDSTRL		, 0x00 },
+	{ STB0899_CFGPDELSTATUS1	, 0x10 },
+	{ STB0899_CFGPDELSTATUS2	, 0x00 },
+	{ STB0899_BBFERRORM		, 0x00 },
+	{ STB0899_BBFERRORL		, 0x00 },
+	{ STB0899_UPKTERRORM		, 0x00 },
+	{ STB0899_UPKTERRORL		, 0x00 },
+	{ 0xffff			, 0xff },
+};
+
+/* STB0899 demodulator config for the KNC1 and clones */
+static struct stb0899_config knc1_dvbs2_config = {
+	.init_dev		= knc1_stb0899_s1_init_1,
+	.init_s2_demod		= stb0899_s2_init_2,
+	.init_s1_demod		= knc1_stb0899_s1_init_3,
+	.init_s2_fec		= stb0899_s2_init_4,
+	.init_tst		= stb0899_s1_init_5,
+
+	.postproc		= NULL,
+
+	.demod_address		= 0x68,
+//	.ts_output_mode		= STB0899_OUT_PARALLEL,	/* types = SERIAL/PARALLEL	*/
+	.block_sync_mode	= STB0899_SYNC_FORCED,	/* DSS, SYNC_FORCED/UNSYNCED	*/
+//	.ts_pfbit_toggle	= STB0899_MPEG_NORMAL,	/* DirecTV, MPEG toggling seq	*/
+
+	.xtal_freq		= 27000000,
+	.inversion		= IQ_SWAP_OFF,
+
+	.lo_clk			= 76500000,
+	.hi_clk			= 90000000,
+
+	.esno_ave		= STB0899_DVBS2_ESNO_AVE,
+	.esno_quant		= STB0899_DVBS2_ESNO_QUANT,
+	.avframes_coarse	= STB0899_DVBS2_AVFRAMES_COARSE,
+	.avframes_fine		= STB0899_DVBS2_AVFRAMES_FINE,
+	.miss_threshold		= STB0899_DVBS2_MISS_THRESHOLD,
+	.uwp_threshold_acq	= STB0899_DVBS2_UWP_THRESHOLD_ACQ,
+	.uwp_threshold_track	= STB0899_DVBS2_UWP_THRESHOLD_TRACK,
+	.uwp_threshold_sof	= STB0899_DVBS2_UWP_THRESHOLD_SOF,
+	.sof_search_timeout	= STB0899_DVBS2_SOF_SEARCH_TIMEOUT,
+
+	.btr_nco_bits		= STB0899_DVBS2_BTR_NCO_BITS,
+	.btr_gain_shift_offset	= STB0899_DVBS2_BTR_GAIN_SHIFT_OFFSET,
+	.crl_nco_bits		= STB0899_DVBS2_CRL_NCO_BITS,
+	.ldpc_max_iter		= STB0899_DVBS2_LDPC_MAX_ITER,
+
+	.tuner_get_frequency	= tda8261_get_frequency,
+	.tuner_set_frequency	= tda8261_set_frequency,
+	.tuner_set_bandwidth	= NULL,
+	.tuner_get_bandwidth	= tda8261_get_bandwidth,
+	.tuner_set_rfsiggain	= NULL
+};
+
+/*
+ * SD1878/SHA tuner config
+ * 1F, Single I/P, Horizontal mount, High Sensitivity
+ */
+static const struct tda8261_config sd1878c_config = {
+//	.name		= "SD1878/SHA",
+	.addr		= 0x60,
+	.step_size	= TDA8261_STEP_1000 /* kHz */
+};
+
+static u8 read_pwm(struct budget_av *budget_av)
+{
+	u8 b = 0xff;
+	u8 pwm;
+	struct i2c_msg msg[] = { {.addr = 0x50,.flags = 0,.buf = &b,.len = 1},
+	{.addr = 0x50,.flags = I2C_M_RD,.buf = &pwm,.len = 1}
+	};
+
+	if ((i2c_transfer(&budget_av->budget.i2c_adap, msg, 2) != 2)
+	    || (pwm == 0xff))
+		pwm = 0x48;
+
+	return pwm;
+}
+
+#define SUBID_DVBS_KNC1			0x0010
+#define SUBID_DVBS_KNC1_PLUS		0x0011
+#define SUBID_DVBS_TYPHOON		0x4f56
+#define SUBID_DVBS_CINERGY1200		0x1154
+#define SUBID_DVBS_CYNERGY1200N		0x1155
+#define SUBID_DVBS_TV_STAR		0x0014
+#define SUBID_DVBS_TV_STAR_PLUS_X4	0x0015
+#define SUBID_DVBS_TV_STAR_CI		0x0016
+#define SUBID_DVBS2_KNC1		0x0018
+#define SUBID_DVBS2_KNC1_OEM		0x0019
+#define SUBID_DVBS_EASYWATCH_1		0x001a
+#define SUBID_DVBS_EASYWATCH_2		0x001b
+#define SUBID_DVBS2_EASYWATCH		0x001d
+#define SUBID_DVBS_EASYWATCH		0x001e
+
+#define SUBID_DVBC_EASYWATCH		0x002a
+#define SUBID_DVBC_EASYWATCH_MK3	0x002c
+#define SUBID_DVBC_KNC1			0x0020
+#define SUBID_DVBC_KNC1_PLUS		0x0021
+#define SUBID_DVBC_KNC1_MK3		0x0022
+#define SUBID_DVBC_KNC1_TDA10024	0x0028
+#define SUBID_DVBC_KNC1_PLUS_MK3	0x0023
+#define SUBID_DVBC_CINERGY1200		0x1156
+#define SUBID_DVBC_CINERGY1200_MK3	0x1176
+
+#define SUBID_DVBT_EASYWATCH		0x003a
+#define SUBID_DVBT_KNC1_PLUS		0x0031
+#define SUBID_DVBT_KNC1			0x0030
+#define SUBID_DVBT_CINERGY1200		0x1157
+
+static void frontend_init(struct budget_av *budget_av)
+{
+	struct saa7146_dev * saa = budget_av->budget.dev;
+	struct dvb_frontend * fe = NULL;
+
+	/* Enable / PowerON Frontend */
+	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTLO);
+
+	/* Wait for PowerON */
+	msleep(100);
+
+	/* additional setup necessary for the PLUS cards */
+	switch (saa->pci->subsystem_device) {
+		case SUBID_DVBS_KNC1_PLUS:
+		case SUBID_DVBC_KNC1_PLUS:
+		case SUBID_DVBT_KNC1_PLUS:
+		case SUBID_DVBC_EASYWATCH:
+		case SUBID_DVBC_KNC1_PLUS_MK3:
+		case SUBID_DVBS2_KNC1:
+		case SUBID_DVBS2_KNC1_OEM:
+		case SUBID_DVBS2_EASYWATCH:
+			saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTHI);
+			break;
+	}
+
+	switch (saa->pci->subsystem_device) {
+
+	case SUBID_DVBS_KNC1:
+		/*
+		 * maybe that setting is needed for other dvb-s cards as well,
+		 * but so far it has been only confirmed for this type
+		 */
+		budget_av->reinitialise_demod = 1;
+		fallthrough;
+	case SUBID_DVBS_KNC1_PLUS:
+	case SUBID_DVBS_EASYWATCH_1:
+		if (saa->pci->subsystem_vendor == 0x1894) {
+			fe = dvb_attach(stv0299_attach, &cinergy_1200s_1894_0010_config,
+					     &budget_av->budget.i2c_adap);
+			if (fe) {
+				dvb_attach(tua6100_attach, fe, 0x60, &budget_av->budget.i2c_adap);
+			}
+		} else {
+			fe = dvb_attach(stv0299_attach, &typhoon_config,
+					     &budget_av->budget.i2c_adap);
+			if (fe) {
+				fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
+			}
+		}
+		break;
+
+	case SUBID_DVBS_TV_STAR:
+	case SUBID_DVBS_TV_STAR_PLUS_X4:
+	case SUBID_DVBS_TV_STAR_CI:
+	case SUBID_DVBS_CYNERGY1200N:
+	case SUBID_DVBS_EASYWATCH:
+	case SUBID_DVBS_EASYWATCH_2:
+		fe = dvb_attach(stv0299_attach, &philips_sd1878_config,
+				&budget_av->budget.i2c_adap);
+		if (fe) {
+			dvb_attach(dvb_pll_attach, fe, 0x60,
+				   &budget_av->budget.i2c_adap,
+				   DVB_PLL_PHILIPS_SD1878_TDA8261);
+		}
+		break;
+
+	case SUBID_DVBS_TYPHOON:
+		fe = dvb_attach(stv0299_attach, &typhoon_config,
+				    &budget_av->budget.i2c_adap);
+		if (fe) {
+			fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
+		}
+		break;
+	case SUBID_DVBS2_KNC1:
+	case SUBID_DVBS2_KNC1_OEM:
+	case SUBID_DVBS2_EASYWATCH:
+		budget_av->reinitialise_demod = 1;
+		if ((fe = dvb_attach(stb0899_attach, &knc1_dvbs2_config, &budget_av->budget.i2c_adap)))
+			dvb_attach(tda8261_attach, fe, &sd1878c_config, &budget_av->budget.i2c_adap);
+
+		break;
+	case SUBID_DVBS_CINERGY1200:
+		fe = dvb_attach(stv0299_attach, &cinergy_1200s_config,
+				    &budget_av->budget.i2c_adap);
+		if (fe) {
+			fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
+		}
+		break;
+
+	case SUBID_DVBC_KNC1:
+	case SUBID_DVBC_KNC1_PLUS:
+	case SUBID_DVBC_CINERGY1200:
+	case SUBID_DVBC_EASYWATCH:
+		budget_av->reinitialise_demod = 1;
+		budget_av->budget.dev->i2c_bitrate = SAA7146_I2C_BUS_BIT_RATE_240;
+		fe = dvb_attach(tda10021_attach, &philips_cu1216_config,
+				     &budget_av->budget.i2c_adap,
+				     read_pwm(budget_av));
+		if (fe == NULL)
+			fe = dvb_attach(tda10021_attach, &philips_cu1216_config_altaddress,
+					     &budget_av->budget.i2c_adap,
+					     read_pwm(budget_av));
+		if (fe) {
+			fe->ops.tuner_ops.set_params = philips_cu1216_tuner_set_params;
+		}
+		break;
+
+	case SUBID_DVBC_EASYWATCH_MK3:
+	case SUBID_DVBC_CINERGY1200_MK3:
+	case SUBID_DVBC_KNC1_MK3:
+	case SUBID_DVBC_KNC1_TDA10024:
+	case SUBID_DVBC_KNC1_PLUS_MK3:
+		budget_av->reinitialise_demod = 1;
+		budget_av->budget.dev->i2c_bitrate = SAA7146_I2C_BUS_BIT_RATE_240;
+		fe = dvb_attach(tda10023_attach,
+			&philips_cu1216_tda10023_config,
+			&budget_av->budget.i2c_adap,
+			read_pwm(budget_av));
+		if (fe) {
+			fe->ops.tuner_ops.set_params = philips_cu1216_tuner_set_params;
+		}
+		break;
+
+	case SUBID_DVBT_EASYWATCH:
+	case SUBID_DVBT_KNC1:
+	case SUBID_DVBT_KNC1_PLUS:
+	case SUBID_DVBT_CINERGY1200:
+		budget_av->reinitialise_demod = 1;
+		fe = dvb_attach(tda10046_attach, &philips_tu1216_config,
+				     &budget_av->budget.i2c_adap);
+		if (fe) {
+			fe->ops.tuner_ops.init = philips_tu1216_tuner_init;
+			fe->ops.tuner_ops.set_params = philips_tu1216_tuner_set_params;
+		}
+		break;
+	}
+
+	if (fe == NULL) {
+		pr_err("A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		       saa->pci->vendor,
+		       saa->pci->device,
+		       saa->pci->subsystem_vendor,
+		       saa->pci->subsystem_device);
+		return;
+	}
+
+	budget_av->budget.dvb_frontend = fe;
+
+	if (dvb_register_frontend(&budget_av->budget.dvb_adapter,
+				  budget_av->budget.dvb_frontend)) {
+		pr_err("Frontend registration failed!\n");
+		dvb_frontend_detach(budget_av->budget.dvb_frontend);
+		budget_av->budget.dvb_frontend = NULL;
+	}
+}
+
+
+static void budget_av_irq(struct saa7146_dev *dev, u32 * isr)
+{
+	struct budget_av *budget_av = (struct budget_av *) dev->ext_priv;
+
+	dprintk(8, "dev: %p, budget_av: %p\n", dev, budget_av);
+
+	if (*isr & MASK_10)
+		ttpci_budget_irq10_handler(dev, isr);
+}
+
+static int budget_av_detach(struct saa7146_dev *dev)
+{
+	struct budget_av *budget_av = (struct budget_av *) dev->ext_priv;
+	int err;
+
+	dprintk(2, "dev: %p\n", dev);
+
+	if (1 == budget_av->has_saa7113) {
+		saa7146_setgpio(dev, 0, SAA7146_GPIO_OUTLO);
+
+		msleep(200);
+
+		saa7146_unregister_device(&budget_av->vd, dev);
+
+		saa7146_vv_release(dev);
+	}
+
+	if (budget_av->budget.ci_present)
+		ciintf_deinit(budget_av);
+
+	if (budget_av->budget.dvb_frontend != NULL) {
+		dvb_unregister_frontend(budget_av->budget.dvb_frontend);
+		dvb_frontend_detach(budget_av->budget.dvb_frontend);
+	}
+	err = ttpci_budget_deinit(&budget_av->budget);
+
+	kfree(budget_av);
+
+	return err;
+}
+
+#define KNC1_INPUTS 2
+static struct v4l2_input knc1_inputs[KNC1_INPUTS] = {
+	{ 0, "Composite", V4L2_INPUT_TYPE_TUNER, 1, 0,
+		V4L2_STD_PAL_BG | V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ 1, "S-Video", V4L2_INPUT_TYPE_CAMERA, 2, 0,
+		V4L2_STD_PAL_BG | V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+};
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	dprintk(1, "VIDIOC_ENUMINPUT %d\n", i->index);
+	if (i->index >= KNC1_INPUTS)
+		return -EINVAL;
+	memcpy(i, &knc1_inputs[i->index], sizeof(struct v4l2_input));
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct budget_av *budget_av = (struct budget_av *)dev->ext_priv;
+
+	*i = budget_av->cur_input;
+
+	dprintk(1, "VIDIOC_G_INPUT %d\n", *i);
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct budget_av *budget_av = (struct budget_av *)dev->ext_priv;
+
+	dprintk(1, "VIDIOC_S_INPUT %d\n", input);
+	return saa7113_setinput(budget_av, input);
+}
+
+static struct saa7146_ext_vv vv_data;
+
+static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
+{
+	struct budget_av *budget_av;
+	u8 *mac;
+	int err;
+
+	dprintk(2, "dev: %p\n", dev);
+
+	if (!(budget_av = kzalloc(sizeof(struct budget_av), GFP_KERNEL)))
+		return -ENOMEM;
+
+	budget_av->has_saa7113 = 0;
+	budget_av->budget.ci_present = 0;
+
+	dev->ext_priv = budget_av;
+
+	err = ttpci_budget_init(&budget_av->budget, dev, info, THIS_MODULE,
+				adapter_nr);
+	if (err) {
+		kfree(budget_av);
+		return err;
+	}
+
+	/* knc1 initialization */
+	saa7146_write(dev, DD1_STREAM_B, 0x04000000);
+	saa7146_write(dev, DD1_INIT, 0x07000600);
+	saa7146_write(dev, MC2, MASK_09 | MASK_25 | MASK_10 | MASK_26);
+
+	if (saa7113_init(budget_av) == 0) {
+		budget_av->has_saa7113 = 1;
+		err = saa7146_vv_init(dev, &vv_data);
+		if (err != 0) {
+			/* fixme: proper cleanup here */
+			ERR("cannot init vv subsystem\n");
+			return err;
+		}
+		vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+		vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+		vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+
+		if ((err = saa7146_register_device(&budget_av->vd, dev, "knc1", VFL_TYPE_VIDEO))) {
+			/* fixme: proper cleanup here */
+			ERR("cannot register capture v4l2 device\n");
+			saa7146_vv_release(dev);
+			return err;
+		}
+
+		/* beware: this modifies dev->vv ... */
+		saa7146_set_hps_source_and_sync(dev, SAA7146_HPS_SOURCE_PORT_A,
+						SAA7146_HPS_SYNC_PORT_A);
+
+		saa7113_setinput(budget_av, 0);
+	}
+
+	/* fixme: find some sane values here... */
+	saa7146_write(dev, PCI_BT_V1, 0x1c00101f);
+
+	mac = budget_av->budget.dvb_adapter.proposed_mac;
+	if (i2c_readregs(&budget_av->budget.i2c_adap, 0xa0, 0x30, mac, 6)) {
+		pr_err("KNC1-%d: Could not read MAC from KNC1 card\n",
+		       budget_av->budget.dvb_adapter.num);
+		eth_zero_addr(mac);
+	} else {
+		pr_info("KNC1-%d: MAC addr = %pM\n",
+			budget_av->budget.dvb_adapter.num, mac);
+	}
+
+	budget_av->budget.dvb_adapter.priv = budget_av;
+	frontend_init(budget_av);
+	ciintf_init(budget_av);
+
+	ttpci_budget_init_hooks(&budget_av->budget);
+
+	return 0;
+}
+
+static struct saa7146_standard standard[] = {
+	{.name = "PAL",.id = V4L2_STD_PAL,
+	 .v_offset = 0x17,.v_field = 288,
+	 .h_offset = 0x14,.h_pixels = 680,
+	 .v_max_out = 576,.h_max_out = 768 },
+
+	{.name = "NTSC",.id = V4L2_STD_NTSC,
+	 .v_offset = 0x16,.v_field = 240,
+	 .h_offset = 0x06,.h_pixels = 708,
+	 .v_max_out = 480,.h_max_out = 640, },
+};
+
+static struct saa7146_ext_vv vv_data = {
+	.inputs = 2,
+	.capabilities = 0,	// perhaps later: V4L2_CAP_VBI_CAPTURE, but that need tweaking with the saa7113
+	.flags = 0,
+	.stds = &standard[0],
+	.num_stds = ARRAY_SIZE(standard),
+};
+
+static struct saa7146_extension budget_extension;
+
+MAKE_BUDGET_INFO(knc1s, "KNC1 DVB-S", BUDGET_KNC1S);
+MAKE_BUDGET_INFO(knc1s2,"KNC1 DVB-S2", BUDGET_KNC1S2);
+MAKE_BUDGET_INFO(sates2,"Satelco EasyWatch DVB-S2", BUDGET_KNC1S2);
+MAKE_BUDGET_INFO(knc1c, "KNC1 DVB-C", BUDGET_KNC1C);
+MAKE_BUDGET_INFO(knc1t, "KNC1 DVB-T", BUDGET_KNC1T);
+MAKE_BUDGET_INFO(kncxs, "KNC TV STAR DVB-S", BUDGET_TVSTAR);
+MAKE_BUDGET_INFO(satewpls, "Satelco EasyWatch DVB-S light", BUDGET_TVSTAR);
+MAKE_BUDGET_INFO(satewpls1, "Satelco EasyWatch DVB-S light", BUDGET_KNC1S);
+MAKE_BUDGET_INFO(satewps, "Satelco EasyWatch DVB-S", BUDGET_KNC1S);
+MAKE_BUDGET_INFO(satewplc, "Satelco EasyWatch DVB-C", BUDGET_KNC1CP);
+MAKE_BUDGET_INFO(satewcmk3, "Satelco EasyWatch DVB-C MK3", BUDGET_KNC1C_MK3);
+MAKE_BUDGET_INFO(satewt, "Satelco EasyWatch DVB-T", BUDGET_KNC1T);
+MAKE_BUDGET_INFO(knc1sp, "KNC1 DVB-S Plus", BUDGET_KNC1SP);
+MAKE_BUDGET_INFO(knc1spx4, "KNC1 DVB-S Plus X4", BUDGET_KNC1SP);
+MAKE_BUDGET_INFO(knc1cp, "KNC1 DVB-C Plus", BUDGET_KNC1CP);
+MAKE_BUDGET_INFO(knc1cmk3, "KNC1 DVB-C MK3", BUDGET_KNC1C_MK3);
+MAKE_BUDGET_INFO(knc1ctda10024, "KNC1 DVB-C TDA10024", BUDGET_KNC1C_TDA10024);
+MAKE_BUDGET_INFO(knc1cpmk3, "KNC1 DVB-C Plus MK3", BUDGET_KNC1CP_MK3);
+MAKE_BUDGET_INFO(knc1tp, "KNC1 DVB-T Plus", BUDGET_KNC1TP);
+MAKE_BUDGET_INFO(cin1200s, "TerraTec Cinergy 1200 DVB-S", BUDGET_CIN1200S);
+MAKE_BUDGET_INFO(cin1200sn, "TerraTec Cinergy 1200 DVB-S", BUDGET_CIN1200S);
+MAKE_BUDGET_INFO(cin1200c, "Terratec Cinergy 1200 DVB-C", BUDGET_CIN1200C);
+MAKE_BUDGET_INFO(cin1200cmk3, "Terratec Cinergy 1200 DVB-C MK3", BUDGET_CIN1200C_MK3);
+MAKE_BUDGET_INFO(cin1200t, "Terratec Cinergy 1200 DVB-T", BUDGET_CIN1200T);
+
+static const struct pci_device_id pci_tbl[] = {
+	MAKE_EXTENSION_PCI(knc1s, 0x1131, 0x4f56),
+	MAKE_EXTENSION_PCI(knc1s, 0x1131, 0x0010),
+	MAKE_EXTENSION_PCI(knc1s, 0x1894, 0x0010),
+	MAKE_EXTENSION_PCI(knc1sp, 0x1131, 0x0011),
+	MAKE_EXTENSION_PCI(knc1sp, 0x1894, 0x0011),
+	MAKE_EXTENSION_PCI(kncxs, 0x1894, 0x0014),
+	MAKE_EXTENSION_PCI(knc1spx4, 0x1894, 0x0015),
+	MAKE_EXTENSION_PCI(kncxs, 0x1894, 0x0016),
+	MAKE_EXTENSION_PCI(knc1s2, 0x1894, 0x0018),
+	MAKE_EXTENSION_PCI(knc1s2, 0x1894, 0x0019),
+	MAKE_EXTENSION_PCI(sates2, 0x1894, 0x001d),
+	MAKE_EXTENSION_PCI(satewpls, 0x1894, 0x001e),
+	MAKE_EXTENSION_PCI(satewpls1, 0x1894, 0x001a),
+	MAKE_EXTENSION_PCI(satewps, 0x1894, 0x001b),
+	MAKE_EXTENSION_PCI(satewplc, 0x1894, 0x002a),
+	MAKE_EXTENSION_PCI(satewcmk3, 0x1894, 0x002c),
+	MAKE_EXTENSION_PCI(satewt, 0x1894, 0x003a),
+	MAKE_EXTENSION_PCI(knc1c, 0x1894, 0x0020),
+	MAKE_EXTENSION_PCI(knc1cp, 0x1894, 0x0021),
+	MAKE_EXTENSION_PCI(knc1cmk3, 0x1894, 0x0022),
+	MAKE_EXTENSION_PCI(knc1ctda10024, 0x1894, 0x0028),
+	MAKE_EXTENSION_PCI(knc1cpmk3, 0x1894, 0x0023),
+	MAKE_EXTENSION_PCI(knc1t, 0x1894, 0x0030),
+	MAKE_EXTENSION_PCI(knc1tp, 0x1894, 0x0031),
+	MAKE_EXTENSION_PCI(cin1200s, 0x153b, 0x1154),
+	MAKE_EXTENSION_PCI(cin1200sn, 0x153b, 0x1155),
+	MAKE_EXTENSION_PCI(cin1200c, 0x153b, 0x1156),
+	MAKE_EXTENSION_PCI(cin1200cmk3, 0x153b, 0x1176),
+	MAKE_EXTENSION_PCI(cin1200t, 0x153b, 0x1157),
+	{
+	 .vendor = 0,
+	}
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_extension budget_extension = {
+	.name = "budget_av",
+	.flags = SAA7146_USE_I2C_IRQ,
+
+	.pci_tbl = pci_tbl,
+
+	.module = THIS_MODULE,
+	.attach = budget_av_attach,
+	.detach = budget_av_detach,
+
+	.irq_mask = MASK_10,
+	.irq_func = budget_av_irq,
+};
+
+static int __init budget_av_init(void)
+{
+	return saa7146_register_extension(&budget_extension);
+}
+
+static void __exit budget_av_exit(void)
+{
+	saa7146_unregister_extension(&budget_extension);
+}
+
+module_init(budget_av_init);
+module_exit(budget_av_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Ralph Metzler, Marcus Metzler, Michael Hunold, others");
+MODULE_DESCRIPTION("driver for the SAA7146 based so-called budget PCI DVB w/ analog input and CI-module (e.g. the KNC cards)");
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/budget-ci.c b/drivers/staging/media/deprecated/saa7146/ttpci/budget-ci.c
new file mode 100644
index 000000000..d59d18647
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/budget-ci.c
@@ -0,0 +1,1574 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * budget-ci.c: driver for the SAA7146 based Budget DVB cards
+ *
+ * Compiled from various sources by Michael Hunold <michael@mihu.de>
+ *
+ *     msp430 IR support contributed by Jack Thomasson <jkt@Helius.COM>
+ *     partially based on the Siemens DVB driver by Ralph+Marcus Metzler
+ *
+ * CI interface support (c) 2004 Andrew de Quincey <adq_dvb@lidskialf.net>
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <media/rc-core.h>
+
+#include "budget.h"
+
+#include <media/dvb_ca_en50221.h>
+#include "stv0299.h"
+#include "stv0297.h"
+#include "tda1004x.h"
+#include "stb0899_drv.h"
+#include "stb0899_reg.h"
+#include "stb0899_cfg.h"
+#include "stb6100.h"
+#include "stb6100_cfg.h"
+#include "lnbp21.h"
+#include "bsbe1.h"
+#include "bsru6.h"
+#include "tda1002x.h"
+#include "tda827x.h"
+#include "bsbe1-d01a.h"
+
+#define MODULE_NAME "budget_ci"
+
+/*
+ * Regarding DEBIADDR_IR:
+ * Some CI modules hang if random addresses are read.
+ * Using address 0x4000 for the IR read means that we
+ * use the same address as for CI version, which should
+ * be a safe default.
+ */
+#define DEBIADDR_IR		0x4000
+#define DEBIADDR_CICONTROL	0x0000
+#define DEBIADDR_CIVERSION	0x4000
+#define DEBIADDR_IO		0x1000
+#define DEBIADDR_ATTR		0x3000
+
+#define CICONTROL_RESET		0x01
+#define CICONTROL_ENABLETS	0x02
+#define CICONTROL_CAMDETECT	0x08
+
+#define DEBICICTL		0x00420000
+#define DEBICICAM		0x02420000
+
+#define SLOTSTATUS_NONE		1
+#define SLOTSTATUS_PRESENT	2
+#define SLOTSTATUS_RESET	4
+#define SLOTSTATUS_READY	8
+#define SLOTSTATUS_OCCUPIED	(SLOTSTATUS_PRESENT|SLOTSTATUS_RESET|SLOTSTATUS_READY)
+
+/* RC5 device wildcard */
+#define IR_DEVICE_ANY		255
+
+static int rc5_device = -1;
+module_param(rc5_device, int, 0644);
+MODULE_PARM_DESC(rc5_device, "only IR commands to given RC5 device (device = 0 - 31, any device = 255, default: autodetect)");
+
+static int ir_debug;
+module_param(ir_debug, int, 0644);
+MODULE_PARM_DESC(ir_debug, "enable debugging information for IR decoding");
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+struct budget_ci_ir {
+	struct rc_dev *dev;
+	struct tasklet_struct msp430_irq_tasklet;
+	char name[72]; /* 40 + 32 for (struct saa7146_dev).name */
+	char phys[32];
+	int rc5_device;
+	u32 ir_key;
+	bool have_command;
+	bool full_rc5;		/* Outputs a full RC5 code */
+};
+
+struct budget_ci {
+	struct budget budget;
+	struct tasklet_struct ciintf_irq_tasklet;
+	int slot_status;
+	int ci_irq;
+	struct dvb_ca_en50221 ca;
+	struct budget_ci_ir ir;
+	u8 tuner_pll_address; /* used for philips_tdm1316l configs */
+};
+
+static void msp430_ir_interrupt(struct tasklet_struct *t)
+{
+	struct budget_ci_ir *ir = from_tasklet(ir, t, msp430_irq_tasklet);
+	struct budget_ci *budget_ci = container_of(ir, typeof(*budget_ci), ir);
+	struct rc_dev *dev = budget_ci->ir.dev;
+	u32 command = ttpci_budget_debiread(&budget_ci->budget, DEBINOSWAP, DEBIADDR_IR, 2, 1, 0) >> 8;
+
+	/*
+	 * The msp430 chip can generate two different bytes, command and device
+	 *
+	 * type1: X1CCCCCC, C = command bits (0 - 63)
+	 * type2: X0TDDDDD, D = device bits (0 - 31), T = RC5 toggle bit
+	 *
+	 * Each signal from the remote control can generate one or more command
+	 * bytes and one or more device bytes. For the repeated bytes, the
+	 * highest bit (X) is set. The first command byte is always generated
+	 * before the first device byte. Other than that, no specific order
+	 * seems to apply. To make life interesting, bytes can also be lost.
+	 *
+	 * Only when we have a command and device byte, a keypress is
+	 * generated.
+	 */
+
+	if (ir_debug)
+		printk("budget_ci: received byte 0x%02x\n", command);
+
+	/* Remove repeat bit, we use every command */
+	command = command & 0x7f;
+
+	/* Is this a RC5 command byte? */
+	if (command & 0x40) {
+		budget_ci->ir.have_command = true;
+		budget_ci->ir.ir_key = command & 0x3f;
+		return;
+	}
+
+	/* It's a RC5 device byte */
+	if (!budget_ci->ir.have_command)
+		return;
+	budget_ci->ir.have_command = false;
+
+	if (budget_ci->ir.rc5_device != IR_DEVICE_ANY &&
+	    budget_ci->ir.rc5_device != (command & 0x1f))
+		return;
+
+	if (budget_ci->ir.full_rc5) {
+		rc_keydown(dev, RC_PROTO_RC5,
+			   RC_SCANCODE_RC5(budget_ci->ir.rc5_device, budget_ci->ir.ir_key),
+			   !!(command & 0x20));
+		return;
+	}
+
+	/* FIXME: We should generate complete scancodes for all devices */
+	rc_keydown(dev, RC_PROTO_UNKNOWN, budget_ci->ir.ir_key,
+		   !!(command & 0x20));
+}
+
+static int msp430_ir_init(struct budget_ci *budget_ci)
+{
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+	struct rc_dev *dev;
+	int error;
+
+	dev = rc_allocate_device(RC_DRIVER_SCANCODE);
+	if (!dev) {
+		printk(KERN_ERR "budget_ci: IR interface initialisation failed\n");
+		return -ENOMEM;
+	}
+
+	snprintf(budget_ci->ir.name, sizeof(budget_ci->ir.name),
+		 "Budget-CI dvb ir receiver %s", saa->name);
+	snprintf(budget_ci->ir.phys, sizeof(budget_ci->ir.phys),
+		 "pci-%s/ir0", pci_name(saa->pci));
+
+	dev->driver_name = MODULE_NAME;
+	dev->device_name = budget_ci->ir.name;
+	dev->input_phys = budget_ci->ir.phys;
+	dev->input_id.bustype = BUS_PCI;
+	dev->input_id.version = 1;
+	if (saa->pci->subsystem_vendor) {
+		dev->input_id.vendor = saa->pci->subsystem_vendor;
+		dev->input_id.product = saa->pci->subsystem_device;
+	} else {
+		dev->input_id.vendor = saa->pci->vendor;
+		dev->input_id.product = saa->pci->device;
+	}
+	dev->dev.parent = &saa->pci->dev;
+
+	if (rc5_device < 0)
+		budget_ci->ir.rc5_device = IR_DEVICE_ANY;
+	else
+		budget_ci->ir.rc5_device = rc5_device;
+
+	/* Select keymap and address */
+	switch (budget_ci->budget.dev->pci->subsystem_device) {
+	case 0x100c:
+	case 0x100f:
+	case 0x1011:
+	case 0x1012:
+		/* The hauppauge keymap is a superset of these remotes */
+		dev->map_name = RC_MAP_HAUPPAUGE;
+		budget_ci->ir.full_rc5 = true;
+
+		if (rc5_device < 0)
+			budget_ci->ir.rc5_device = 0x1f;
+		break;
+	case 0x1010:
+	case 0x1017:
+	case 0x1019:
+	case 0x101a:
+	case 0x101b:
+		/* for the Technotrend 1500 bundled remote */
+		dev->map_name = RC_MAP_TT_1500;
+		break;
+	default:
+		/* unknown remote */
+		dev->map_name = RC_MAP_BUDGET_CI_OLD;
+		break;
+	}
+	if (!budget_ci->ir.full_rc5)
+		dev->scancode_mask = 0xff;
+
+	error = rc_register_device(dev);
+	if (error) {
+		printk(KERN_ERR "budget_ci: could not init driver for IR device (code %d)\n", error);
+		rc_free_device(dev);
+		return error;
+	}
+
+	budget_ci->ir.dev = dev;
+
+	tasklet_setup(&budget_ci->ir.msp430_irq_tasklet, msp430_ir_interrupt);
+
+	SAA7146_IER_ENABLE(saa, MASK_06);
+	saa7146_setgpio(saa, 3, SAA7146_GPIO_IRQHI);
+
+	return 0;
+}
+
+static void msp430_ir_deinit(struct budget_ci *budget_ci)
+{
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+
+	SAA7146_IER_DISABLE(saa, MASK_06);
+	saa7146_setgpio(saa, 3, SAA7146_GPIO_INPUT);
+	tasklet_kill(&budget_ci->ir.msp430_irq_tasklet);
+
+	rc_unregister_device(budget_ci->ir.dev);
+}
+
+static int ciintf_read_attribute_mem(struct dvb_ca_en50221 *ca, int slot, int address)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	return ttpci_budget_debiread(&budget_ci->budget, DEBICICAM,
+				     DEBIADDR_ATTR | (address & 0xfff), 1, 1, 0);
+}
+
+static int ciintf_write_attribute_mem(struct dvb_ca_en50221 *ca, int slot, int address, u8 value)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	return ttpci_budget_debiwrite(&budget_ci->budget, DEBICICAM,
+				      DEBIADDR_ATTR | (address & 0xfff), 1, value, 1, 0);
+}
+
+static int ciintf_read_cam_control(struct dvb_ca_en50221 *ca, int slot, u8 address)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	return ttpci_budget_debiread(&budget_ci->budget, DEBICICAM,
+				     DEBIADDR_IO | (address & 3), 1, 1, 0);
+}
+
+static int ciintf_write_cam_control(struct dvb_ca_en50221 *ca, int slot, u8 address, u8 value)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	return ttpci_budget_debiwrite(&budget_ci->budget, DEBICICAM,
+				      DEBIADDR_IO | (address & 3), 1, value, 1, 0);
+}
+
+static int ciintf_slot_reset(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	if (budget_ci->ci_irq) {
+		// trigger on RISING edge during reset so we know when READY is re-asserted
+		saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQHI);
+	}
+	budget_ci->slot_status = SLOTSTATUS_RESET;
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 0, 1, 0);
+	msleep(1);
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1,
+			       CICONTROL_RESET, 1, 0);
+
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_OUTHI);
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTB);
+	return 0;
+}
+
+static int ciintf_slot_shutdown(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_OUTHI);
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTB);
+	return 0;
+}
+
+static int ciintf_slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+	int tmp;
+
+	if (slot != 0)
+		return -EINVAL;
+
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_OUTLO);
+
+	tmp = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 1, 0);
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1,
+			       tmp | CICONTROL_ENABLETS, 1, 0);
+
+	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTA);
+	return 0;
+}
+
+static void ciintf_interrupt(struct tasklet_struct *t)
+{
+	struct budget_ci *budget_ci = from_tasklet(budget_ci, t,
+						   ciintf_irq_tasklet);
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+	unsigned int flags;
+
+	// ensure we don't get spurious IRQs during initialisation
+	if (!budget_ci->budget.ci_present)
+		return;
+
+	// read the CAM status
+	flags = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 1, 0);
+	if (flags & CICONTROL_CAMDETECT) {
+
+		// GPIO should be set to trigger on falling edge if a CAM is present
+		saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQLO);
+
+		if (budget_ci->slot_status & SLOTSTATUS_NONE) {
+			// CAM insertion IRQ
+			budget_ci->slot_status = SLOTSTATUS_PRESENT;
+			dvb_ca_en50221_camchange_irq(&budget_ci->ca, 0,
+						     DVB_CA_EN50221_CAMCHANGE_INSERTED);
+
+		} else if (budget_ci->slot_status & SLOTSTATUS_RESET) {
+			// CAM ready (reset completed)
+			budget_ci->slot_status = SLOTSTATUS_READY;
+			dvb_ca_en50221_camready_irq(&budget_ci->ca, 0);
+
+		} else if (budget_ci->slot_status & SLOTSTATUS_READY) {
+			// FR/DA IRQ
+			dvb_ca_en50221_frda_irq(&budget_ci->ca, 0);
+		}
+	} else {
+
+		// trigger on rising edge if a CAM is not present - when a CAM is inserted, we
+		// only want to get the IRQ when it sets READY. If we trigger on the falling edge,
+		// the CAM might not actually be ready yet.
+		saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQHI);
+
+		// generate a CAM removal IRQ if we haven't already
+		if (budget_ci->slot_status & SLOTSTATUS_OCCUPIED) {
+			// CAM removal IRQ
+			budget_ci->slot_status = SLOTSTATUS_NONE;
+			dvb_ca_en50221_camchange_irq(&budget_ci->ca, 0,
+						     DVB_CA_EN50221_CAMCHANGE_REMOVED);
+		}
+	}
+}
+
+static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) ca->data;
+	unsigned int flags;
+
+	// ensure we don't get spurious IRQs during initialisation
+	if (!budget_ci->budget.ci_present)
+		return -EINVAL;
+
+	// read the CAM status
+	flags = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 1, 0);
+	if (flags & CICONTROL_CAMDETECT) {
+		// mark it as present if it wasn't before
+		if (budget_ci->slot_status & SLOTSTATUS_NONE) {
+			budget_ci->slot_status = SLOTSTATUS_PRESENT;
+		}
+
+		// during a RESET, we check if we can read from IO memory to see when CAM is ready
+		if (budget_ci->slot_status & SLOTSTATUS_RESET) {
+			if (ciintf_read_attribute_mem(ca, slot, 0) == 0x1d) {
+				budget_ci->slot_status = SLOTSTATUS_READY;
+			}
+		}
+	} else {
+		budget_ci->slot_status = SLOTSTATUS_NONE;
+	}
+
+	if (budget_ci->slot_status != SLOTSTATUS_NONE) {
+		if (budget_ci->slot_status & SLOTSTATUS_READY) {
+			return DVB_CA_EN50221_POLL_CAM_PRESENT | DVB_CA_EN50221_POLL_CAM_READY;
+		}
+		return DVB_CA_EN50221_POLL_CAM_PRESENT;
+	}
+
+	return 0;
+}
+
+static int ciintf_init(struct budget_ci *budget_ci)
+{
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+	int flags;
+	int result;
+	int ci_version;
+	int ca_flags;
+
+	memset(&budget_ci->ca, 0, sizeof(struct dvb_ca_en50221));
+
+	// enable DEBI pins
+	saa7146_write(saa, MC1, MASK_27 | MASK_11);
+
+	// test if it is there
+	ci_version = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CIVERSION, 1, 1, 0);
+	if ((ci_version & 0xa0) != 0xa0) {
+		result = -ENODEV;
+		goto error;
+	}
+
+	// determine whether a CAM is present or not
+	flags = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 1, 0);
+	budget_ci->slot_status = SLOTSTATUS_NONE;
+	if (flags & CICONTROL_CAMDETECT)
+		budget_ci->slot_status = SLOTSTATUS_PRESENT;
+
+	// version 0xa2 of the CI firmware doesn't generate interrupts
+	if (ci_version == 0xa2) {
+		ca_flags = 0;
+		budget_ci->ci_irq = 0;
+	} else {
+		ca_flags = DVB_CA_EN50221_FLAG_IRQ_CAMCHANGE |
+				DVB_CA_EN50221_FLAG_IRQ_FR |
+				DVB_CA_EN50221_FLAG_IRQ_DA;
+		budget_ci->ci_irq = 1;
+	}
+
+	// register CI interface
+	budget_ci->ca.owner = THIS_MODULE;
+	budget_ci->ca.read_attribute_mem = ciintf_read_attribute_mem;
+	budget_ci->ca.write_attribute_mem = ciintf_write_attribute_mem;
+	budget_ci->ca.read_cam_control = ciintf_read_cam_control;
+	budget_ci->ca.write_cam_control = ciintf_write_cam_control;
+	budget_ci->ca.slot_reset = ciintf_slot_reset;
+	budget_ci->ca.slot_shutdown = ciintf_slot_shutdown;
+	budget_ci->ca.slot_ts_enable = ciintf_slot_ts_enable;
+	budget_ci->ca.poll_slot_status = ciintf_poll_slot_status;
+	budget_ci->ca.data = budget_ci;
+	if ((result = dvb_ca_en50221_init(&budget_ci->budget.dvb_adapter,
+					  &budget_ci->ca,
+					  ca_flags, 1)) != 0) {
+		printk("budget_ci: CI interface detected, but initialisation failed.\n");
+		goto error;
+	}
+
+	// Setup CI slot IRQ
+	if (budget_ci->ci_irq) {
+		tasklet_setup(&budget_ci->ciintf_irq_tasklet, ciintf_interrupt);
+		if (budget_ci->slot_status != SLOTSTATUS_NONE) {
+			saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQLO);
+		} else {
+			saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQHI);
+		}
+		SAA7146_IER_ENABLE(saa, MASK_03);
+	}
+
+	// enable interface
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1,
+			       CICONTROL_RESET, 1, 0);
+
+	// success!
+	printk("budget_ci: CI interface initialised\n");
+	budget_ci->budget.ci_present = 1;
+
+	// forge a fake CI IRQ so the CAM state is setup correctly
+	if (budget_ci->ci_irq) {
+		flags = DVB_CA_EN50221_CAMCHANGE_REMOVED;
+		if (budget_ci->slot_status != SLOTSTATUS_NONE)
+			flags = DVB_CA_EN50221_CAMCHANGE_INSERTED;
+		dvb_ca_en50221_camchange_irq(&budget_ci->ca, 0, flags);
+	}
+
+	return 0;
+
+error:
+	saa7146_write(saa, MC1, MASK_27);
+	return result;
+}
+
+static void ciintf_deinit(struct budget_ci *budget_ci)
+{
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+
+	// disable CI interrupts
+	if (budget_ci->ci_irq) {
+		SAA7146_IER_DISABLE(saa, MASK_03);
+		saa7146_setgpio(saa, 0, SAA7146_GPIO_INPUT);
+		tasklet_kill(&budget_ci->ciintf_irq_tasklet);
+	}
+
+	// reset interface
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 0, 1, 0);
+	msleep(1);
+	ttpci_budget_debiwrite(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1,
+			       CICONTROL_RESET, 1, 0);
+
+	// disable TS data stream to CI interface
+	saa7146_setgpio(saa, 1, SAA7146_GPIO_INPUT);
+
+	// release the CA device
+	dvb_ca_en50221_release(&budget_ci->ca);
+
+	// disable DEBI pins
+	saa7146_write(saa, MC1, MASK_27);
+}
+
+static void budget_ci_irq(struct saa7146_dev *dev, u32 * isr)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) dev->ext_priv;
+
+	dprintk(8, "dev: %p, budget_ci: %p\n", dev, budget_ci);
+
+	if (*isr & MASK_06)
+		tasklet_schedule(&budget_ci->ir.msp430_irq_tasklet);
+
+	if (*isr & MASK_10)
+		ttpci_budget_irq10_handler(dev, isr);
+
+	if ((*isr & MASK_03) && (budget_ci->budget.ci_present) && (budget_ci->ci_irq))
+		tasklet_schedule(&budget_ci->ciintf_irq_tasklet);
+}
+
+static u8 philips_su1278_tt_inittab[] = {
+	0x01, 0x0f,
+	0x02, 0x30,
+	0x03, 0x00,
+	0x04, 0x5b,
+	0x05, 0x85,
+	0x06, 0x02,
+	0x07, 0x00,
+	0x08, 0x02,
+	0x09, 0x00,
+	0x0C, 0x01,
+	0x0D, 0x81,
+	0x0E, 0x44,
+	0x0f, 0x14,
+	0x10, 0x3c,
+	0x11, 0x84,
+	0x12, 0xda,
+	0x13, 0x97,
+	0x14, 0x95,
+	0x15, 0xc9,
+	0x16, 0x19,
+	0x17, 0x8c,
+	0x18, 0x59,
+	0x19, 0xf8,
+	0x1a, 0xfe,
+	0x1c, 0x7f,
+	0x1d, 0x00,
+	0x1e, 0x00,
+	0x1f, 0x50,
+	0x20, 0x00,
+	0x21, 0x00,
+	0x22, 0x00,
+	0x23, 0x00,
+	0x28, 0x00,
+	0x29, 0x28,
+	0x2a, 0x14,
+	0x2b, 0x0f,
+	0x2c, 0x09,
+	0x2d, 0x09,
+	0x31, 0x1f,
+	0x32, 0x19,
+	0x33, 0xfc,
+	0x34, 0x93,
+	0xff, 0xff
+};
+
+static int philips_su1278_tt_set_symbol_rate(struct dvb_frontend *fe, u32 srate, u32 ratio)
+{
+	stv0299_writereg(fe, 0x0e, 0x44);
+	if (srate >= 10000000) {
+		stv0299_writereg(fe, 0x13, 0x97);
+		stv0299_writereg(fe, 0x14, 0x95);
+		stv0299_writereg(fe, 0x15, 0xc9);
+		stv0299_writereg(fe, 0x17, 0x8c);
+		stv0299_writereg(fe, 0x1a, 0xfe);
+		stv0299_writereg(fe, 0x1c, 0x7f);
+		stv0299_writereg(fe, 0x2d, 0x09);
+	} else {
+		stv0299_writereg(fe, 0x13, 0x99);
+		stv0299_writereg(fe, 0x14, 0x8d);
+		stv0299_writereg(fe, 0x15, 0xce);
+		stv0299_writereg(fe, 0x17, 0x43);
+		stv0299_writereg(fe, 0x1a, 0x1d);
+		stv0299_writereg(fe, 0x1c, 0x12);
+		stv0299_writereg(fe, 0x2d, 0x05);
+	}
+	stv0299_writereg(fe, 0x0e, 0x23);
+	stv0299_writereg(fe, 0x0f, 0x94);
+	stv0299_writereg(fe, 0x10, 0x39);
+	stv0299_writereg(fe, 0x15, 0xc9);
+
+	stv0299_writereg(fe, 0x1f, (ratio >> 16) & 0xff);
+	stv0299_writereg(fe, 0x20, (ratio >> 8) & 0xff);
+	stv0299_writereg(fe, 0x21, (ratio) & 0xf0);
+
+	return 0;
+}
+
+static int philips_su1278_tt_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct budget_ci *budget_ci = (struct budget_ci *) fe->dvb->priv;
+	u32 div;
+	u8 buf[4];
+	struct i2c_msg msg = {.addr = 0x60,.flags = 0,.buf = buf,.len = sizeof(buf) };
+
+	if ((p->frequency < 950000) || (p->frequency > 2150000))
+		return -EINVAL;
+
+	div = (p->frequency + (500 - 1)) / 500;	/* round correctly */
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = 0x80 | ((div & 0x18000) >> 10) | 2;
+	buf[3] = 0x20;
+
+	if (p->symbol_rate < 4000000)
+		buf[3] |= 1;
+
+	if (p->frequency < 1250000)
+		buf[3] |= 0;
+	else if (p->frequency < 1550000)
+		buf[3] |= 0x40;
+	else if (p->frequency < 2050000)
+		buf[3] |= 0x80;
+	else if (p->frequency < 2150000)
+		buf[3] |= 0xC0;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static const struct stv0299_config philips_su1278_tt_config = {
+
+	.demod_address = 0x68,
+	.inittab = philips_su1278_tt_inittab,
+	.mclk = 64000000UL,
+	.invert = 0,
+	.skip_reinit = 1,
+	.lock_output = STV0299_LOCKOUTPUT_1,
+	.volt13_op0_op1 = STV0299_VOLT13_OP1,
+	.min_delay_ms = 50,
+	.set_symbol_rate = philips_su1278_tt_set_symbol_rate,
+};
+
+
+
+static int philips_tdm1316l_tuner_init(struct dvb_frontend *fe)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) fe->dvb->priv;
+	static u8 td1316_init[] = { 0x0b, 0xf5, 0x85, 0xab };
+	static u8 disable_mc44BC374c[] = { 0x1d, 0x74, 0xa0, 0x68 };
+	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address,.flags = 0,.buf = td1316_init,.len =
+			sizeof(td1316_init) };
+
+	// setup PLL configuration
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+	msleep(1);
+
+	// disable the mc44BC374c (do not check for errors)
+	tuner_msg.addr = 0x65;
+	tuner_msg.buf = disable_mc44BC374c;
+	tuner_msg.len = sizeof(disable_mc44BC374c);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1) != 1) {
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 1);
+		i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1);
+	}
+
+	return 0;
+}
+
+static int philips_tdm1316l_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct budget_ci *budget_ci = (struct budget_ci *) fe->dvb->priv;
+	u8 tuner_buf[4];
+	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address,.flags = 0,.buf = tuner_buf,.len = sizeof(tuner_buf) };
+	int tuner_frequency = 0;
+	u8 band, cp, filter;
+
+	// determine charge pump
+	tuner_frequency = p->frequency + 36130000;
+	if (tuner_frequency < 87000000)
+		return -EINVAL;
+	else if (tuner_frequency < 130000000)
+		cp = 3;
+	else if (tuner_frequency < 160000000)
+		cp = 5;
+	else if (tuner_frequency < 200000000)
+		cp = 6;
+	else if (tuner_frequency < 290000000)
+		cp = 3;
+	else if (tuner_frequency < 420000000)
+		cp = 5;
+	else if (tuner_frequency < 480000000)
+		cp = 6;
+	else if (tuner_frequency < 620000000)
+		cp = 3;
+	else if (tuner_frequency < 830000000)
+		cp = 5;
+	else if (tuner_frequency < 895000000)
+		cp = 7;
+	else
+		return -EINVAL;
+
+	// determine band
+	if (p->frequency < 49000000)
+		return -EINVAL;
+	else if (p->frequency < 159000000)
+		band = 1;
+	else if (p->frequency < 444000000)
+		band = 2;
+	else if (p->frequency < 861000000)
+		band = 4;
+	else
+		return -EINVAL;
+
+	// setup PLL filter and TDA9889
+	switch (p->bandwidth_hz) {
+	case 6000000:
+		tda1004x_writereg(fe, 0x0C, 0x14);
+		filter = 0;
+		break;
+
+	case 7000000:
+		tda1004x_writereg(fe, 0x0C, 0x80);
+		filter = 0;
+		break;
+
+	case 8000000:
+		tda1004x_writereg(fe, 0x0C, 0x14);
+		filter = 1;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	// calculate divisor
+	// ((36130000+((1000000/6)/2)) + Finput)/(1000000/6)
+	tuner_frequency = (((p->frequency / 1000) * 6) + 217280) / 1000;
+
+	// setup tuner buffer
+	tuner_buf[0] = tuner_frequency >> 8;
+	tuner_buf[1] = tuner_frequency & 0xff;
+	tuner_buf[2] = 0xca;
+	tuner_buf[3] = (cp << 5) | (filter << 3) | band;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+
+	msleep(1);
+	return 0;
+}
+
+static int philips_tdm1316l_request_firmware(struct dvb_frontend *fe,
+					     const struct firmware **fw, char *name)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) fe->dvb->priv;
+
+	return request_firmware(fw, name, &budget_ci->budget.dev->pci->dev);
+}
+
+static struct tda1004x_config philips_tdm1316l_config = {
+
+	.demod_address = 0x8,
+	.invert = 0,
+	.invert_oclk = 0,
+	.xtal_freq = TDA10046_XTAL_4M,
+	.agc_config = TDA10046_AGC_DEFAULT,
+	.if_freq = TDA10046_FREQ_3617,
+	.request_firmware = philips_tdm1316l_request_firmware,
+};
+
+static struct tda1004x_config philips_tdm1316l_config_invert = {
+
+	.demod_address = 0x8,
+	.invert = 1,
+	.invert_oclk = 0,
+	.xtal_freq = TDA10046_XTAL_4M,
+	.agc_config = TDA10046_AGC_DEFAULT,
+	.if_freq = TDA10046_FREQ_3617,
+	.request_firmware = philips_tdm1316l_request_firmware,
+};
+
+static int dvbc_philips_tdm1316l_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct budget_ci *budget_ci = (struct budget_ci *) fe->dvb->priv;
+	u8 tuner_buf[5];
+	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address,
+				    .flags = 0,
+				    .buf = tuner_buf,
+				    .len = sizeof(tuner_buf) };
+	int tuner_frequency = 0;
+	u8 band, cp, filter;
+
+	// determine charge pump
+	tuner_frequency = p->frequency + 36125000;
+	if (tuner_frequency < 87000000)
+		return -EINVAL;
+	else if (tuner_frequency < 130000000) {
+		cp = 3;
+		band = 1;
+	} else if (tuner_frequency < 160000000) {
+		cp = 5;
+		band = 1;
+	} else if (tuner_frequency < 200000000) {
+		cp = 6;
+		band = 1;
+	} else if (tuner_frequency < 290000000) {
+		cp = 3;
+		band = 2;
+	} else if (tuner_frequency < 420000000) {
+		cp = 5;
+		band = 2;
+	} else if (tuner_frequency < 480000000) {
+		cp = 6;
+		band = 2;
+	} else if (tuner_frequency < 620000000) {
+		cp = 3;
+		band = 4;
+	} else if (tuner_frequency < 830000000) {
+		cp = 5;
+		band = 4;
+	} else if (tuner_frequency < 895000000) {
+		cp = 7;
+		band = 4;
+	} else
+		return -EINVAL;
+
+	// assume PLL filter should always be 8MHz for the moment.
+	filter = 1;
+
+	// calculate divisor
+	tuner_frequency = (p->frequency + 36125000 + (62500/2)) / 62500;
+
+	// setup tuner buffer
+	tuner_buf[0] = tuner_frequency >> 8;
+	tuner_buf[1] = tuner_frequency & 0xff;
+	tuner_buf[2] = 0xc8;
+	tuner_buf[3] = (cp << 5) | (filter << 3) | band;
+	tuner_buf[4] = 0x80;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+
+	msleep(50);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer(&budget_ci->budget.i2c_adap, &tuner_msg, 1) != 1)
+		return -EIO;
+
+	msleep(1);
+
+	return 0;
+}
+
+static u8 dvbc_philips_tdm1316l_inittab[] = {
+	0x80, 0x01,
+	0x80, 0x00,
+	0x81, 0x01,
+	0x81, 0x00,
+	0x00, 0x09,
+	0x01, 0x69,
+	0x03, 0x00,
+	0x04, 0x00,
+	0x07, 0x00,
+	0x08, 0x00,
+	0x20, 0x00,
+	0x21, 0x40,
+	0x22, 0x00,
+	0x23, 0x00,
+	0x24, 0x40,
+	0x25, 0x88,
+	0x30, 0xff,
+	0x31, 0x00,
+	0x32, 0xff,
+	0x33, 0x00,
+	0x34, 0x50,
+	0x35, 0x7f,
+	0x36, 0x00,
+	0x37, 0x20,
+	0x38, 0x00,
+	0x40, 0x1c,
+	0x41, 0xff,
+	0x42, 0x29,
+	0x43, 0x20,
+	0x44, 0xff,
+	0x45, 0x00,
+	0x46, 0x00,
+	0x49, 0x04,
+	0x4a, 0x00,
+	0x4b, 0x7b,
+	0x52, 0x30,
+	0x55, 0xae,
+	0x56, 0x47,
+	0x57, 0xe1,
+	0x58, 0x3a,
+	0x5a, 0x1e,
+	0x5b, 0x34,
+	0x60, 0x00,
+	0x63, 0x00,
+	0x64, 0x00,
+	0x65, 0x00,
+	0x66, 0x00,
+	0x67, 0x00,
+	0x68, 0x00,
+	0x69, 0x00,
+	0x6a, 0x02,
+	0x6b, 0x00,
+	0x70, 0xff,
+	0x71, 0x00,
+	0x72, 0x00,
+	0x73, 0x00,
+	0x74, 0x0c,
+	0x80, 0x00,
+	0x81, 0x00,
+	0x82, 0x00,
+	0x83, 0x00,
+	0x84, 0x04,
+	0x85, 0x80,
+	0x86, 0x24,
+	0x87, 0x78,
+	0x88, 0x10,
+	0x89, 0x00,
+	0x90, 0x01,
+	0x91, 0x01,
+	0xa0, 0x04,
+	0xa1, 0x00,
+	0xa2, 0x00,
+	0xb0, 0x91,
+	0xb1, 0x0b,
+	0xc0, 0x53,
+	0xc1, 0x70,
+	0xc2, 0x12,
+	0xd0, 0x00,
+	0xd1, 0x00,
+	0xd2, 0x00,
+	0xd3, 0x00,
+	0xd4, 0x00,
+	0xd5, 0x00,
+	0xde, 0x00,
+	0xdf, 0x00,
+	0x61, 0x38,
+	0x62, 0x0a,
+	0x53, 0x13,
+	0x59, 0x08,
+	0xff, 0xff,
+};
+
+static struct stv0297_config dvbc_philips_tdm1316l_config = {
+	.demod_address = 0x1c,
+	.inittab = dvbc_philips_tdm1316l_inittab,
+	.invert = 0,
+	.stop_during_read = 1,
+};
+
+static struct tda10023_config tda10023_config = {
+	.demod_address = 0xc,
+	.invert = 0,
+	.xtal = 16000000,
+	.pll_m = 11,
+	.pll_p = 3,
+	.pll_n = 1,
+	.deltaf = 0xa511,
+};
+
+static struct tda827x_config tda827x_config = {
+	.config = 0,
+};
+
+/* TT S2-3200 DVB-S (STB0899) Inittab */
+static const struct stb0899_s1_reg tt3200_stb0899_s1_init_1[] = {
+
+	{ STB0899_DEV_ID		, 0x81 },
+	{ STB0899_DISCNTRL1		, 0x32 },
+	{ STB0899_DISCNTRL2		, 0x80 },
+	{ STB0899_DISRX_ST0		, 0x04 },
+	{ STB0899_DISRX_ST1		, 0x00 },
+	{ STB0899_DISPARITY		, 0x00 },
+	{ STB0899_DISSTATUS		, 0x20 },
+	{ STB0899_DISF22		, 0x8c },
+	{ STB0899_DISF22RX		, 0x9a },
+	{ STB0899_SYSREG		, 0x0b },
+	{ STB0899_ACRPRESC		, 0x11 },
+	{ STB0899_ACRDIV1		, 0x0a },
+	{ STB0899_ACRDIV2		, 0x05 },
+	{ STB0899_DACR1			, 0x00 },
+	{ STB0899_DACR2			, 0x00 },
+	{ STB0899_OUTCFG		, 0x00 },
+	{ STB0899_MODECFG		, 0x00 },
+	{ STB0899_IRQSTATUS_3		, 0x30 },
+	{ STB0899_IRQSTATUS_2		, 0x00 },
+	{ STB0899_IRQSTATUS_1		, 0x00 },
+	{ STB0899_IRQSTATUS_0		, 0x00 },
+	{ STB0899_IRQMSK_3		, 0xf3 },
+	{ STB0899_IRQMSK_2		, 0xfc },
+	{ STB0899_IRQMSK_1		, 0xff },
+	{ STB0899_IRQMSK_0		, 0xff },
+	{ STB0899_IRQCFG		, 0x00 },
+	{ STB0899_I2CCFG		, 0x88 },
+	{ STB0899_I2CRPT		, 0x48 }, /* 12k Pullup, Repeater=16, Stop=disabled */
+	{ STB0899_IOPVALUE5		, 0x00 },
+	{ STB0899_IOPVALUE4		, 0x20 },
+	{ STB0899_IOPVALUE3		, 0xc9 },
+	{ STB0899_IOPVALUE2		, 0x90 },
+	{ STB0899_IOPVALUE1		, 0x40 },
+	{ STB0899_IOPVALUE0		, 0x00 },
+	{ STB0899_GPIO00CFG		, 0x82 },
+	{ STB0899_GPIO01CFG		, 0x82 },
+	{ STB0899_GPIO02CFG		, 0x82 },
+	{ STB0899_GPIO03CFG		, 0x82 },
+	{ STB0899_GPIO04CFG		, 0x82 },
+	{ STB0899_GPIO05CFG		, 0x82 },
+	{ STB0899_GPIO06CFG		, 0x82 },
+	{ STB0899_GPIO07CFG		, 0x82 },
+	{ STB0899_GPIO08CFG		, 0x82 },
+	{ STB0899_GPIO09CFG		, 0x82 },
+	{ STB0899_GPIO10CFG		, 0x82 },
+	{ STB0899_GPIO11CFG		, 0x82 },
+	{ STB0899_GPIO12CFG		, 0x82 },
+	{ STB0899_GPIO13CFG		, 0x82 },
+	{ STB0899_GPIO14CFG		, 0x82 },
+	{ STB0899_GPIO15CFG		, 0x82 },
+	{ STB0899_GPIO16CFG		, 0x82 },
+	{ STB0899_GPIO17CFG		, 0x82 },
+	{ STB0899_GPIO18CFG		, 0x82 },
+	{ STB0899_GPIO19CFG		, 0x82 },
+	{ STB0899_GPIO20CFG		, 0x82 },
+	{ STB0899_SDATCFG		, 0xb8 },
+	{ STB0899_SCLTCFG		, 0xba },
+	{ STB0899_AGCRFCFG		, 0x1c }, /* 0x11 */
+	{ STB0899_GPIO22		, 0x82 }, /* AGCBB2CFG */
+	{ STB0899_GPIO21		, 0x91 }, /* AGCBB1CFG */
+	{ STB0899_DIRCLKCFG		, 0x82 },
+	{ STB0899_CLKOUT27CFG		, 0x7e },
+	{ STB0899_STDBYCFG		, 0x82 },
+	{ STB0899_CS0CFG		, 0x82 },
+	{ STB0899_CS1CFG		, 0x82 },
+	{ STB0899_DISEQCOCFG		, 0x20 },
+	{ STB0899_GPIO32CFG		, 0x82 },
+	{ STB0899_GPIO33CFG		, 0x82 },
+	{ STB0899_GPIO34CFG		, 0x82 },
+	{ STB0899_GPIO35CFG		, 0x82 },
+	{ STB0899_GPIO36CFG		, 0x82 },
+	{ STB0899_GPIO37CFG		, 0x82 },
+	{ STB0899_GPIO38CFG		, 0x82 },
+	{ STB0899_GPIO39CFG		, 0x82 },
+	{ STB0899_NCOARSE		, 0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
+	{ STB0899_SYNTCTRL		, 0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
+	{ STB0899_FILTCTRL		, 0x00 },
+	{ STB0899_SYSCTRL		, 0x00 },
+	{ STB0899_STOPCLK1		, 0x20 },
+	{ STB0899_STOPCLK2		, 0x00 },
+	{ STB0899_INTBUFSTATUS		, 0x00 },
+	{ STB0899_INTBUFCTRL		, 0x0a },
+	{ 0xffff			, 0xff },
+};
+
+static const struct stb0899_s1_reg tt3200_stb0899_s1_init_3[] = {
+	{ STB0899_DEMOD			, 0x00 },
+	{ STB0899_RCOMPC		, 0xc9 },
+	{ STB0899_AGC1CN		, 0x41 },
+	{ STB0899_AGC1REF		, 0x10 },
+	{ STB0899_RTC			, 0x7a },
+	{ STB0899_TMGCFG		, 0x4e },
+	{ STB0899_AGC2REF		, 0x34 },
+	{ STB0899_TLSR			, 0x84 },
+	{ STB0899_CFD			, 0xc7 },
+	{ STB0899_ACLC			, 0x87 },
+	{ STB0899_BCLC			, 0x94 },
+	{ STB0899_EQON			, 0x41 },
+	{ STB0899_LDT			, 0xdd },
+	{ STB0899_LDT2			, 0xc9 },
+	{ STB0899_EQUALREF		, 0xb4 },
+	{ STB0899_TMGRAMP		, 0x10 },
+	{ STB0899_TMGTHD		, 0x30 },
+	{ STB0899_IDCCOMP		, 0xfb },
+	{ STB0899_QDCCOMP		, 0x03 },
+	{ STB0899_POWERI		, 0x3b },
+	{ STB0899_POWERQ		, 0x3d },
+	{ STB0899_RCOMP			, 0x81 },
+	{ STB0899_AGCIQIN		, 0x80 },
+	{ STB0899_AGC2I1		, 0x04 },
+	{ STB0899_AGC2I2		, 0xf5 },
+	{ STB0899_TLIR			, 0x25 },
+	{ STB0899_RTF			, 0x80 },
+	{ STB0899_DSTATUS		, 0x00 },
+	{ STB0899_LDI			, 0xca },
+	{ STB0899_CFRM			, 0xf1 },
+	{ STB0899_CFRL			, 0xf3 },
+	{ STB0899_NIRM			, 0x2a },
+	{ STB0899_NIRL			, 0x05 },
+	{ STB0899_ISYMB			, 0x17 },
+	{ STB0899_QSYMB			, 0xfa },
+	{ STB0899_SFRH			, 0x2f },
+	{ STB0899_SFRM			, 0x68 },
+	{ STB0899_SFRL			, 0x40 },
+	{ STB0899_SFRUPH		, 0x2f },
+	{ STB0899_SFRUPM		, 0x68 },
+	{ STB0899_SFRUPL		, 0x40 },
+	{ STB0899_EQUAI1		, 0xfd },
+	{ STB0899_EQUAQ1		, 0x04 },
+	{ STB0899_EQUAI2		, 0x0f },
+	{ STB0899_EQUAQ2		, 0xff },
+	{ STB0899_EQUAI3		, 0xdf },
+	{ STB0899_EQUAQ3		, 0xfa },
+	{ STB0899_EQUAI4		, 0x37 },
+	{ STB0899_EQUAQ4		, 0x0d },
+	{ STB0899_EQUAI5		, 0xbd },
+	{ STB0899_EQUAQ5		, 0xf7 },
+	{ STB0899_DSTATUS2		, 0x00 },
+	{ STB0899_VSTATUS		, 0x00 },
+	{ STB0899_VERROR		, 0xff },
+	{ STB0899_IQSWAP		, 0x2a },
+	{ STB0899_ECNT1M		, 0x00 },
+	{ STB0899_ECNT1L		, 0x00 },
+	{ STB0899_ECNT2M		, 0x00 },
+	{ STB0899_ECNT2L		, 0x00 },
+	{ STB0899_ECNT3M		, 0x00 },
+	{ STB0899_ECNT3L		, 0x00 },
+	{ STB0899_FECAUTO1		, 0x06 },
+	{ STB0899_FECM			, 0x01 },
+	{ STB0899_VTH12			, 0xf0 },
+	{ STB0899_VTH23			, 0xa0 },
+	{ STB0899_VTH34			, 0x78 },
+	{ STB0899_VTH56			, 0x4e },
+	{ STB0899_VTH67			, 0x48 },
+	{ STB0899_VTH78			, 0x38 },
+	{ STB0899_PRVIT			, 0xff },
+	{ STB0899_VITSYNC		, 0x19 },
+	{ STB0899_RSULC			, 0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
+	{ STB0899_TSULC			, 0x42 },
+	{ STB0899_RSLLC			, 0x40 },
+	{ STB0899_TSLPL			, 0x12 },
+	{ STB0899_TSCFGH		, 0x0c },
+	{ STB0899_TSCFGM		, 0x00 },
+	{ STB0899_TSCFGL		, 0x0c },
+	{ STB0899_TSOUT			, 0x4d }, /* 0x0d for CAM */
+	{ STB0899_RSSYNCDEL		, 0x00 },
+	{ STB0899_TSINHDELH		, 0x02 },
+	{ STB0899_TSINHDELM		, 0x00 },
+	{ STB0899_TSINHDELL		, 0x00 },
+	{ STB0899_TSLLSTKM		, 0x00 },
+	{ STB0899_TSLLSTKL		, 0x00 },
+	{ STB0899_TSULSTKM		, 0x00 },
+	{ STB0899_TSULSTKL		, 0xab },
+	{ STB0899_PCKLENUL		, 0x00 },
+	{ STB0899_PCKLENLL		, 0xcc },
+	{ STB0899_RSPCKLEN		, 0xcc },
+	{ STB0899_TSSTATUS		, 0x80 },
+	{ STB0899_ERRCTRL1		, 0xb6 },
+	{ STB0899_ERRCTRL2		, 0x96 },
+	{ STB0899_ERRCTRL3		, 0x89 },
+	{ STB0899_DMONMSK1		, 0x27 },
+	{ STB0899_DMONMSK0		, 0x03 },
+	{ STB0899_DEMAPVIT		, 0x5c },
+	{ STB0899_PLPARM		, 0x1f },
+	{ STB0899_PDELCTRL		, 0x48 },
+	{ STB0899_PDELCTRL2		, 0x00 },
+	{ STB0899_BBHCTRL1		, 0x00 },
+	{ STB0899_BBHCTRL2		, 0x00 },
+	{ STB0899_HYSTTHRESH		, 0x77 },
+	{ STB0899_MATCSTM		, 0x00 },
+	{ STB0899_MATCSTL		, 0x00 },
+	{ STB0899_UPLCSTM		, 0x00 },
+	{ STB0899_UPLCSTL		, 0x00 },
+	{ STB0899_DFLCSTM		, 0x00 },
+	{ STB0899_DFLCSTL		, 0x00 },
+	{ STB0899_SYNCCST		, 0x00 },
+	{ STB0899_SYNCDCSTM		, 0x00 },
+	{ STB0899_SYNCDCSTL		, 0x00 },
+	{ STB0899_ISI_ENTRY		, 0x00 },
+	{ STB0899_ISI_BIT_EN		, 0x00 },
+	{ STB0899_MATSTRM		, 0x00 },
+	{ STB0899_MATSTRL		, 0x00 },
+	{ STB0899_UPLSTRM		, 0x00 },
+	{ STB0899_UPLSTRL		, 0x00 },
+	{ STB0899_DFLSTRM		, 0x00 },
+	{ STB0899_DFLSTRL		, 0x00 },
+	{ STB0899_SYNCSTR		, 0x00 },
+	{ STB0899_SYNCDSTRM		, 0x00 },
+	{ STB0899_SYNCDSTRL		, 0x00 },
+	{ STB0899_CFGPDELSTATUS1	, 0x10 },
+	{ STB0899_CFGPDELSTATUS2	, 0x00 },
+	{ STB0899_BBFERRORM		, 0x00 },
+	{ STB0899_BBFERRORL		, 0x00 },
+	{ STB0899_UPKTERRORM		, 0x00 },
+	{ STB0899_UPKTERRORL		, 0x00 },
+	{ 0xffff			, 0xff },
+};
+
+static struct stb0899_config tt3200_config = {
+	.init_dev		= tt3200_stb0899_s1_init_1,
+	.init_s2_demod		= stb0899_s2_init_2,
+	.init_s1_demod		= tt3200_stb0899_s1_init_3,
+	.init_s2_fec		= stb0899_s2_init_4,
+	.init_tst		= stb0899_s1_init_5,
+
+	.postproc		= NULL,
+
+	.demod_address		= 0x68,
+
+	.xtal_freq		= 27000000,
+	.inversion		= IQ_SWAP_ON,
+
+	.lo_clk			= 76500000,
+	.hi_clk			= 99000000,
+
+	.esno_ave		= STB0899_DVBS2_ESNO_AVE,
+	.esno_quant		= STB0899_DVBS2_ESNO_QUANT,
+	.avframes_coarse	= STB0899_DVBS2_AVFRAMES_COARSE,
+	.avframes_fine		= STB0899_DVBS2_AVFRAMES_FINE,
+	.miss_threshold		= STB0899_DVBS2_MISS_THRESHOLD,
+	.uwp_threshold_acq	= STB0899_DVBS2_UWP_THRESHOLD_ACQ,
+	.uwp_threshold_track	= STB0899_DVBS2_UWP_THRESHOLD_TRACK,
+	.uwp_threshold_sof	= STB0899_DVBS2_UWP_THRESHOLD_SOF,
+	.sof_search_timeout	= STB0899_DVBS2_SOF_SEARCH_TIMEOUT,
+
+	.btr_nco_bits		= STB0899_DVBS2_BTR_NCO_BITS,
+	.btr_gain_shift_offset	= STB0899_DVBS2_BTR_GAIN_SHIFT_OFFSET,
+	.crl_nco_bits		= STB0899_DVBS2_CRL_NCO_BITS,
+	.ldpc_max_iter		= STB0899_DVBS2_LDPC_MAX_ITER,
+
+	.tuner_get_frequency	= stb6100_get_frequency,
+	.tuner_set_frequency	= stb6100_set_frequency,
+	.tuner_set_bandwidth	= stb6100_set_bandwidth,
+	.tuner_get_bandwidth	= stb6100_get_bandwidth,
+	.tuner_set_rfsiggain	= NULL
+};
+
+static struct stb6100_config tt3200_stb6100_config = {
+	.tuner_address	= 0x60,
+	.refclock	= 27000000,
+};
+
+static void frontend_init(struct budget_ci *budget_ci)
+{
+	switch (budget_ci->budget.dev->pci->subsystem_device) {
+	case 0x100c:		// Hauppauge/TT Nova-CI budget (stv0299/ALPS BSRU6(tsa5059))
+		budget_ci->budget.dvb_frontend =
+			dvb_attach(stv0299_attach, &alps_bsru6_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget_ci->budget.dvb_frontend->tuner_priv = &budget_ci->budget.i2c_adap;
+			break;
+		}
+		break;
+
+	case 0x100f:		// Hauppauge/TT Nova-CI budget (stv0299b/Philips su1278(tsa5059))
+		budget_ci->budget.dvb_frontend =
+			dvb_attach(stv0299_attach, &philips_su1278_tt_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = philips_su1278_tt_tuner_set_params;
+			break;
+		}
+		break;
+
+	case 0x1010:		// TT DVB-C CI budget (stv0297/Philips tdm1316l(tda6651tt))
+		budget_ci->tuner_pll_address = 0x61;
+		budget_ci->budget.dvb_frontend =
+			dvb_attach(stv0297_attach, &dvbc_philips_tdm1316l_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = dvbc_philips_tdm1316l_tuner_set_params;
+			break;
+		}
+		break;
+
+	case 0x1011:		// Hauppauge/TT Nova-T budget (tda10045/Philips tdm1316l(tda6651tt) + TDA9889)
+		budget_ci->tuner_pll_address = 0x63;
+		budget_ci->budget.dvb_frontend =
+			dvb_attach(tda10045_attach, &philips_tdm1316l_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.init = philips_tdm1316l_tuner_init;
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = philips_tdm1316l_tuner_set_params;
+			break;
+		}
+		break;
+
+	case 0x1012:		// TT DVB-T CI budget (tda10046/Philips tdm1316l(tda6651tt))
+		budget_ci->tuner_pll_address = 0x60;
+		budget_ci->budget.dvb_frontend =
+			dvb_attach(tda10046_attach, &philips_tdm1316l_config_invert, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.init = philips_tdm1316l_tuner_init;
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = philips_tdm1316l_tuner_set_params;
+			break;
+		}
+		break;
+
+	case 0x1017:		// TT S-1500 PCI
+		budget_ci->budget.dvb_frontend = dvb_attach(stv0299_attach, &alps_bsbe1_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			budget_ci->budget.dvb_frontend->ops.tuner_ops.set_params = alps_bsbe1_tuner_set_params;
+			budget_ci->budget.dvb_frontend->tuner_priv = &budget_ci->budget.i2c_adap;
+
+			budget_ci->budget.dvb_frontend->ops.dishnetwork_send_legacy_command = NULL;
+			if (dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, LNBP21_LLC, 0) == NULL) {
+				printk("%s: No LNBP21 found!\n", __func__);
+				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+				budget_ci->budget.dvb_frontend = NULL;
+			}
+		}
+		break;
+
+	case 0x101a: /* TT Budget-C-1501 (philips tda10023/philips tda8274A) */
+		budget_ci->budget.dvb_frontend = dvb_attach(tda10023_attach, &tda10023_config, &budget_ci->budget.i2c_adap, 0x48);
+		if (budget_ci->budget.dvb_frontend) {
+			if (dvb_attach(tda827x_attach, budget_ci->budget.dvb_frontend, 0x61, &budget_ci->budget.i2c_adap, &tda827x_config) == NULL) {
+				printk(KERN_ERR "%s: No tda827x found!\n", __func__);
+				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+				budget_ci->budget.dvb_frontend = NULL;
+			}
+		}
+		break;
+
+	case 0x101b: /* TT S-1500B (BSBE1-D01A - STV0288/STB6000/LNBP21) */
+		budget_ci->budget.dvb_frontend = dvb_attach(stv0288_attach, &stv0288_bsbe1_d01a_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			if (dvb_attach(stb6000_attach, budget_ci->budget.dvb_frontend, 0x63, &budget_ci->budget.i2c_adap)) {
+				if (!dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, 0, 0)) {
+					printk(KERN_ERR "%s: No LNBP21 found!\n", __func__);
+					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+					budget_ci->budget.dvb_frontend = NULL;
+				}
+			} else {
+				printk(KERN_ERR "%s: No STB6000 found!\n", __func__);
+				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+				budget_ci->budget.dvb_frontend = NULL;
+			}
+		}
+		break;
+
+	case 0x1019:		// TT S2-3200 PCI
+		/*
+		 * NOTE! on some STB0899 versions, the internal PLL takes a longer time
+		 * to settle, aka LOCK. On the older revisions of the chip, we don't see
+		 * this, as a result on the newer chips the entire clock tree, will not
+		 * be stable after a freshly POWER 'ed up situation.
+		 * In this case, we should RESET the STB0899 (Active LOW) and wait for
+		 * PLL stabilization.
+		 *
+		 * On the TT S2 3200 and clones, the STB0899 demodulator's RESETB is
+		 * connected to the SAA7146 GPIO, GPIO2, Pin 142
+		 */
+		/* Reset Demodulator */
+		saa7146_setgpio(budget_ci->budget.dev, 2, SAA7146_GPIO_OUTLO);
+		/* Wait for everything to die */
+		msleep(50);
+		/* Pull it up out of Reset state */
+		saa7146_setgpio(budget_ci->budget.dev, 2, SAA7146_GPIO_OUTHI);
+		/* Wait for PLL to stabilize */
+		msleep(250);
+		/*
+		 * PLL state should be stable now. Ideally, we should check
+		 * for PLL LOCK status. But well, never mind!
+		 */
+		budget_ci->budget.dvb_frontend = dvb_attach(stb0899_attach, &tt3200_config, &budget_ci->budget.i2c_adap);
+		if (budget_ci->budget.dvb_frontend) {
+			if (dvb_attach(stb6100_attach, budget_ci->budget.dvb_frontend, &tt3200_stb6100_config, &budget_ci->budget.i2c_adap)) {
+				if (!dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, 0, 0)) {
+					printk("%s: No LNBP21 found!\n", __func__);
+					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+					budget_ci->budget.dvb_frontend = NULL;
+				}
+			} else {
+					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+					budget_ci->budget.dvb_frontend = NULL;
+			}
+		}
+		break;
+
+	}
+
+	if (budget_ci->budget.dvb_frontend == NULL) {
+		printk("budget-ci: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		       budget_ci->budget.dev->pci->vendor,
+		       budget_ci->budget.dev->pci->device,
+		       budget_ci->budget.dev->pci->subsystem_vendor,
+		       budget_ci->budget.dev->pci->subsystem_device);
+	} else {
+		if (dvb_register_frontend
+		    (&budget_ci->budget.dvb_adapter, budget_ci->budget.dvb_frontend)) {
+			printk("budget-ci: Frontend registration failed!\n");
+			dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+			budget_ci->budget.dvb_frontend = NULL;
+		}
+	}
+}
+
+static int budget_ci_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
+{
+	struct budget_ci *budget_ci;
+	int err;
+
+	budget_ci = kzalloc(sizeof(struct budget_ci), GFP_KERNEL);
+	if (!budget_ci) {
+		err = -ENOMEM;
+		goto out1;
+	}
+
+	dprintk(2, "budget_ci: %p\n", budget_ci);
+
+	dev->ext_priv = budget_ci;
+
+	err = ttpci_budget_init(&budget_ci->budget, dev, info, THIS_MODULE,
+				adapter_nr);
+	if (err)
+		goto out2;
+
+	err = msp430_ir_init(budget_ci);
+	if (err)
+		goto out3;
+
+	ciintf_init(budget_ci);
+
+	budget_ci->budget.dvb_adapter.priv = budget_ci;
+	frontend_init(budget_ci);
+
+	ttpci_budget_init_hooks(&budget_ci->budget);
+
+	return 0;
+
+out3:
+	ttpci_budget_deinit(&budget_ci->budget);
+out2:
+	kfree(budget_ci);
+out1:
+	return err;
+}
+
+static int budget_ci_detach(struct saa7146_dev *dev)
+{
+	struct budget_ci *budget_ci = (struct budget_ci *) dev->ext_priv;
+	struct saa7146_dev *saa = budget_ci->budget.dev;
+	int err;
+
+	if (budget_ci->budget.ci_present)
+		ciintf_deinit(budget_ci);
+	msp430_ir_deinit(budget_ci);
+	if (budget_ci->budget.dvb_frontend) {
+		dvb_unregister_frontend(budget_ci->budget.dvb_frontend);
+		dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+	}
+	err = ttpci_budget_deinit(&budget_ci->budget);
+
+	// disable frontend and CI interface
+	saa7146_setgpio(saa, 2, SAA7146_GPIO_INPUT);
+
+	kfree(budget_ci);
+
+	return err;
+}
+
+static struct saa7146_extension budget_extension;
+
+MAKE_BUDGET_INFO(ttbs2, "TT-Budget/S-1500 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(ttbci, "TT-Budget/WinTV-NOVA-CI PCI", BUDGET_TT_HW_DISEQC);
+MAKE_BUDGET_INFO(ttbt2, "TT-Budget/WinTV-NOVA-T	 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(ttbtci, "TT-Budget-T-CI PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(ttbcci, "TT-Budget-C-CI PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(ttc1501, "TT-Budget C-1501 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(tt3200, "TT-Budget S2-3200 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(ttbs1500b, "TT-Budget S-1500B PCI", BUDGET_TT);
+
+static const struct pci_device_id pci_tbl[] = {
+	MAKE_EXTENSION_PCI(ttbci, 0x13c2, 0x100c),
+	MAKE_EXTENSION_PCI(ttbci, 0x13c2, 0x100f),
+	MAKE_EXTENSION_PCI(ttbcci, 0x13c2, 0x1010),
+	MAKE_EXTENSION_PCI(ttbt2, 0x13c2, 0x1011),
+	MAKE_EXTENSION_PCI(ttbtci, 0x13c2, 0x1012),
+	MAKE_EXTENSION_PCI(ttbs2, 0x13c2, 0x1017),
+	MAKE_EXTENSION_PCI(ttc1501, 0x13c2, 0x101a),
+	MAKE_EXTENSION_PCI(tt3200, 0x13c2, 0x1019),
+	MAKE_EXTENSION_PCI(ttbs1500b, 0x13c2, 0x101b),
+	{
+	 .vendor = 0,
+	 }
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_extension budget_extension = {
+	.name = "budget_ci dvb",
+	.flags = SAA7146_USE_I2C_IRQ,
+
+	.module = THIS_MODULE,
+	.pci_tbl = &pci_tbl[0],
+	.attach = budget_ci_attach,
+	.detach = budget_ci_detach,
+
+	.irq_mask = MASK_03 | MASK_06 | MASK_10,
+	.irq_func = budget_ci_irq,
+};
+
+static int __init budget_ci_init(void)
+{
+	return saa7146_register_extension(&budget_extension);
+}
+
+static void __exit budget_ci_exit(void)
+{
+	saa7146_unregister_extension(&budget_extension);
+}
+
+module_init(budget_ci_init);
+module_exit(budget_ci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Hunold, Jack Thomasson, Andrew de Quincey, others");
+MODULE_DESCRIPTION("driver for the SAA7146 based so-called budget PCI DVB cards w/ CI-module produced by Siemens, Technotrend, Hauppauge");
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/budget-core.c b/drivers/staging/media/deprecated/saa7146/ttpci/budget-core.c
new file mode 100644
index 000000000..5d5796f24
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/budget-core.c
@@ -0,0 +1,603 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * budget-core.c: driver for the SAA7146 based Budget DVB cards
+ *
+ * Compiled from various sources by Michael Hunold <michael@mihu.de>
+ *
+ * Copyright (C) 2002 Ralph Metzler <rjkm@metzlerbros.de>
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *			 & Marcus Metzler for convergence integrated media GmbH
+ *
+ * 26feb2004 Support for FS Activy Card (Grundig tuner) by
+ *	     Michael Dreher <michael@5dot1.de>,
+ *	     Oliver Endriss <o.endriss@gmx.de>,
+ *	     Andreas 'randy' Weinberger
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+
+#include "budget.h"
+#include "ttpci-eeprom.h"
+
+#define TS_WIDTH		(2 * TS_SIZE)
+#define TS_WIDTH_ACTIVY		TS_SIZE
+#define TS_WIDTH_DVBC		TS_SIZE
+#define TS_HEIGHT_MASK		0xf00
+#define TS_HEIGHT_MASK_ACTIVY	0xc00
+#define TS_HEIGHT_MASK_DVBC	0xe00
+#define TS_MIN_BUFSIZE_K	188
+#define TS_MAX_BUFSIZE_K	1410
+#define TS_MAX_BUFSIZE_K_ACTIVY	564
+#define TS_MAX_BUFSIZE_K_DVBC	1316
+#define BUFFER_WARNING_WAIT	(30*HZ)
+
+int budget_debug;
+static int dma_buffer_size = TS_MIN_BUFSIZE_K;
+module_param_named(debug, budget_debug, int, 0644);
+module_param_named(bufsize, dma_buffer_size, int, 0444);
+MODULE_PARM_DESC(debug, "Turn on/off budget debugging (default:off).");
+MODULE_PARM_DESC(bufsize, "DMA buffer size in KB, default: 188, min: 188, max: 1410 (Activy: 564)");
+
+/****************************************************************************
+ * TT budget / WinTV Nova
+ ****************************************************************************/
+
+static int stop_ts_capture(struct budget *budget)
+{
+	dprintk(2, "budget: %p\n", budget);
+
+	saa7146_write(budget->dev, MC1, MASK_20);	// DMA3 off
+	SAA7146_IER_DISABLE(budget->dev, MASK_10);
+	return 0;
+}
+
+static int start_ts_capture(struct budget *budget)
+{
+	struct saa7146_dev *dev = budget->dev;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	if (!budget->feeding || !budget->fe_synced)
+		return 0;
+
+	saa7146_write(dev, MC1, MASK_20);	// DMA3 off
+
+	memset(budget->grabbing, 0x00, budget->buffer_size);
+
+	saa7146_write(dev, PCI_BT_V1, 0x001c0000 | (saa7146_read(dev, PCI_BT_V1) & ~0x001f0000));
+
+	budget->ttbp = 0;
+
+	/*
+	 *  Signal path on the Activy:
+	 *
+	 *  tuner -> SAA7146 port A -> SAA7146 BRS -> SAA7146 DMA3 -> memory
+	 *
+	 *  Since the tuner feeds 204 bytes packets into the SAA7146,
+	 *  DMA3 is configured to strip the trailing 16 FEC bytes:
+	 *      Pitch: 188, NumBytes3: 188, NumLines3: 1024
+	 */
+
+	switch(budget->card->type) {
+	case BUDGET_FS_ACTIVY:
+		saa7146_write(dev, DD1_INIT, 0x04000000);
+		saa7146_write(dev, MC2, (MASK_09 | MASK_25));
+		saa7146_write(dev, BRS_CTRL, 0x00000000);
+		break;
+	case BUDGET_PATCH:
+		saa7146_write(dev, DD1_INIT, 0x00000200);
+		saa7146_write(dev, MC2, (MASK_10 | MASK_26));
+		saa7146_write(dev, BRS_CTRL, 0x60000000);
+		break;
+	case BUDGET_CIN1200C_MK3:
+	case BUDGET_KNC1C_MK3:
+	case BUDGET_KNC1C_TDA10024:
+	case BUDGET_KNC1CP_MK3:
+		if (budget->video_port == BUDGET_VIDEO_PORTA) {
+			saa7146_write(dev, DD1_INIT, 0x06000200);
+			saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+			saa7146_write(dev, BRS_CTRL, 0x00000000);
+		} else {
+			saa7146_write(dev, DD1_INIT, 0x00000600);
+			saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+			saa7146_write(dev, BRS_CTRL, 0x60000000);
+		}
+		break;
+	default:
+		if (budget->video_port == BUDGET_VIDEO_PORTA) {
+			saa7146_write(dev, DD1_INIT, 0x06000200);
+			saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+			saa7146_write(dev, BRS_CTRL, 0x00000000);
+		} else {
+			saa7146_write(dev, DD1_INIT, 0x02000600);
+			saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+			saa7146_write(dev, BRS_CTRL, 0x60000000);
+		}
+	}
+
+	saa7146_write(dev, MC2, (MASK_08 | MASK_24));
+	mdelay(10);
+
+	saa7146_write(dev, BASE_ODD3, 0);
+	if (budget->buffer_size > budget->buffer_height * budget->buffer_width) {
+		// using odd/even buffers
+		saa7146_write(dev, BASE_EVEN3, budget->buffer_height * budget->buffer_width);
+	} else {
+		// using a single buffer
+		saa7146_write(dev, BASE_EVEN3, 0);
+	}
+	saa7146_write(dev, PROT_ADDR3, budget->buffer_size);
+	saa7146_write(dev, BASE_PAGE3, budget->pt.dma | ME1 | 0x90);
+
+	saa7146_write(dev, PITCH3, budget->buffer_width);
+	saa7146_write(dev, NUM_LINE_BYTE3,
+			(budget->buffer_height << 16) | budget->buffer_width);
+
+	saa7146_write(dev, MC2, (MASK_04 | MASK_20));
+
+	SAA7146_ISR_CLEAR(budget->dev, MASK_10);	/* VPE */
+	SAA7146_IER_ENABLE(budget->dev, MASK_10);	/* VPE */
+	saa7146_write(dev, MC1, (MASK_04 | MASK_20));	/* DMA3 on */
+
+	return 0;
+}
+
+static int budget_read_fe_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
+{
+	struct budget *budget = (struct budget *) fe->dvb->priv;
+	int synced;
+	int ret;
+
+	if (budget->read_fe_status)
+		ret = budget->read_fe_status(fe, status);
+	else
+		ret = -EINVAL;
+
+	if (!ret) {
+		synced = (*status & FE_HAS_LOCK);
+		if (synced != budget->fe_synced) {
+			budget->fe_synced = synced;
+			spin_lock(&budget->feedlock);
+			if (synced)
+				start_ts_capture(budget);
+			else
+				stop_ts_capture(budget);
+			spin_unlock(&budget->feedlock);
+		}
+	}
+	return ret;
+}
+
+static void vpeirq(struct tasklet_struct *t)
+{
+	struct budget *budget = from_tasklet(budget, t, vpe_tasklet);
+	u8 *mem = (u8 *) (budget->grabbing);
+	u32 olddma = budget->ttbp;
+	u32 newdma = saa7146_read(budget->dev, PCI_VDP3);
+	u32 count;
+
+	/* Ensure streamed PCI data is synced to CPU */
+	dma_sync_sg_for_cpu(&budget->dev->pci->dev, budget->pt.slist,
+			    budget->pt.nents, DMA_FROM_DEVICE);
+
+	/* nearest lower position divisible by 188 */
+	newdma -= newdma % 188;
+
+	if (newdma >= budget->buffer_size)
+		return;
+
+	budget->ttbp = newdma;
+
+	if (budget->feeding == 0 || newdma == olddma)
+		return;
+
+	if (newdma > olddma) {	/* no wraparound, dump olddma..newdma */
+		count = newdma - olddma;
+		dvb_dmx_swfilter_packets(&budget->demux, mem + olddma, count / 188);
+	} else {		/* wraparound, dump olddma..buflen and 0..newdma */
+		count = budget->buffer_size - olddma;
+		dvb_dmx_swfilter_packets(&budget->demux, mem + olddma, count / 188);
+		count += newdma;
+		dvb_dmx_swfilter_packets(&budget->demux, mem, newdma / 188);
+	}
+
+	if (count > budget->buffer_warning_threshold)
+		budget->buffer_warnings++;
+
+	if (budget->buffer_warnings && time_after(jiffies, budget->buffer_warning_time)) {
+		printk("%s %s: used %d times >80%% of buffer (%u bytes now)\n",
+			budget->dev->name, __func__, budget->buffer_warnings, count);
+		budget->buffer_warning_time = jiffies + BUFFER_WARNING_WAIT;
+		budget->buffer_warnings = 0;
+	}
+}
+
+
+static int ttpci_budget_debiread_nolock(struct budget *budget, u32 config,
+		int addr, int count, int nobusyloop)
+{
+	struct saa7146_dev *saa = budget->dev;
+	int result;
+
+	result = saa7146_wait_for_debi_done(saa, nobusyloop);
+	if (result < 0)
+		return result;
+
+	saa7146_write(saa, DEBI_COMMAND, (count << 17) | 0x10000 | (addr & 0xffff));
+	saa7146_write(saa, DEBI_CONFIG, config);
+	saa7146_write(saa, DEBI_PAGE, 0);
+	saa7146_write(saa, MC2, (2 << 16) | 2);
+
+	result = saa7146_wait_for_debi_done(saa, nobusyloop);
+	if (result < 0)
+		return result;
+
+	result = saa7146_read(saa, DEBI_AD);
+	result &= (0xffffffffUL >> ((4 - count) * 8));
+	return result;
+}
+
+int ttpci_budget_debiread(struct budget *budget, u32 config, int addr, int count,
+			  int uselocks, int nobusyloop)
+{
+	if (count > 4 || count <= 0)
+		return 0;
+
+	if (uselocks) {
+		unsigned long flags;
+		int result;
+
+		spin_lock_irqsave(&budget->debilock, flags);
+		result = ttpci_budget_debiread_nolock(budget, config, addr,
+						      count, nobusyloop);
+		spin_unlock_irqrestore(&budget->debilock, flags);
+		return result;
+	}
+	return ttpci_budget_debiread_nolock(budget, config, addr,
+					    count, nobusyloop);
+}
+
+static int ttpci_budget_debiwrite_nolock(struct budget *budget, u32 config,
+		int addr, int count, u32 value, int nobusyloop)
+{
+	struct saa7146_dev *saa = budget->dev;
+	int result;
+
+	result = saa7146_wait_for_debi_done(saa, nobusyloop);
+	if (result < 0)
+		return result;
+
+	saa7146_write(saa, DEBI_COMMAND, (count << 17) | 0x00000 | (addr & 0xffff));
+	saa7146_write(saa, DEBI_CONFIG, config);
+	saa7146_write(saa, DEBI_PAGE, 0);
+	saa7146_write(saa, DEBI_AD, value);
+	saa7146_write(saa, MC2, (2 << 16) | 2);
+
+	result = saa7146_wait_for_debi_done(saa, nobusyloop);
+	return result < 0 ? result : 0;
+}
+
+int ttpci_budget_debiwrite(struct budget *budget, u32 config, int addr,
+			   int count, u32 value, int uselocks, int nobusyloop)
+{
+	if (count > 4 || count <= 0)
+		return 0;
+
+	if (uselocks) {
+		unsigned long flags;
+		int result;
+
+		spin_lock_irqsave(&budget->debilock, flags);
+		result = ttpci_budget_debiwrite_nolock(budget, config, addr,
+						count, value, nobusyloop);
+		spin_unlock_irqrestore(&budget->debilock, flags);
+		return result;
+	}
+	return ttpci_budget_debiwrite_nolock(budget, config, addr,
+					     count, value, nobusyloop);
+}
+
+
+/****************************************************************************
+ * DVB API SECTION
+ ****************************************************************************/
+
+static int budget_start_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct budget *budget = (struct budget *) demux->priv;
+	int status = 0;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	if (!demux->dmx.frontend)
+		return -EINVAL;
+
+	spin_lock(&budget->feedlock);
+	feed->pusi_seen = false; /* have a clean section start */
+	if (budget->feeding++ == 0)
+		status = start_ts_capture(budget);
+	spin_unlock(&budget->feedlock);
+	return status;
+}
+
+static int budget_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct budget *budget = (struct budget *) demux->priv;
+	int status = 0;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	spin_lock(&budget->feedlock);
+	if (--budget->feeding == 0)
+		status = stop_ts_capture(budget);
+	spin_unlock(&budget->feedlock);
+	return status;
+}
+
+static int budget_register(struct budget *budget)
+{
+	struct dvb_demux *dvbdemux = &budget->demux;
+	int ret;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	dvbdemux->priv = (void *) budget;
+
+	dvbdemux->filternum = 256;
+	dvbdemux->feednum = 256;
+	dvbdemux->start_feed = budget_start_feed;
+	dvbdemux->stop_feed = budget_stop_feed;
+	dvbdemux->write_to_decoder = NULL;
+
+	dvbdemux->dmx.capabilities = (DMX_TS_FILTERING | DMX_SECTION_FILTERING |
+				      DMX_MEMORY_BASED_FILTERING);
+
+	dvb_dmx_init(&budget->demux);
+
+	budget->dmxdev.filternum = 256;
+	budget->dmxdev.demux = &dvbdemux->dmx;
+	budget->dmxdev.capabilities = 0;
+
+	dvb_dmxdev_init(&budget->dmxdev, &budget->dvb_adapter);
+
+	budget->hw_frontend.source = DMX_FRONTEND_0;
+
+	ret = dvbdemux->dmx.add_frontend(&dvbdemux->dmx, &budget->hw_frontend);
+
+	if (ret < 0)
+		goto err_release_dmx;
+
+	budget->mem_frontend.source = DMX_MEMORY_FE;
+	ret = dvbdemux->dmx.add_frontend(&dvbdemux->dmx, &budget->mem_frontend);
+	if (ret < 0)
+		goto err_release_dmx;
+
+	ret = dvbdemux->dmx.connect_frontend(&dvbdemux->dmx, &budget->hw_frontend);
+	if (ret < 0)
+		goto err_release_dmx;
+
+	dvb_net_init(&budget->dvb_adapter, &budget->dvb_net, &dvbdemux->dmx);
+
+	return 0;
+
+err_release_dmx:
+	dvb_dmxdev_release(&budget->dmxdev);
+	dvb_dmx_release(&budget->demux);
+	return ret;
+}
+
+static void budget_unregister(struct budget *budget)
+{
+	struct dvb_demux *dvbdemux = &budget->demux;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	dvb_net_release(&budget->dvb_net);
+
+	dvbdemux->dmx.close(&dvbdemux->dmx);
+	dvbdemux->dmx.remove_frontend(&dvbdemux->dmx, &budget->hw_frontend);
+	dvbdemux->dmx.remove_frontend(&dvbdemux->dmx, &budget->mem_frontend);
+
+	dvb_dmxdev_release(&budget->dmxdev);
+	dvb_dmx_release(&budget->demux);
+}
+
+int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
+		      struct saa7146_pci_extension_data *info,
+		      struct module *owner, short *adapter_nums)
+{
+	int ret = 0;
+	struct budget_info *bi = info->ext_priv;
+	int max_bufsize;
+	int height_mask;
+
+	memset(budget, 0, sizeof(struct budget));
+
+	dprintk(2, "dev: %p, budget: %p\n", dev, budget);
+
+	budget->card = bi;
+	budget->dev = (struct saa7146_dev *) dev;
+
+	switch(budget->card->type) {
+	case BUDGET_FS_ACTIVY:
+		budget->buffer_width = TS_WIDTH_ACTIVY;
+		max_bufsize = TS_MAX_BUFSIZE_K_ACTIVY;
+		height_mask = TS_HEIGHT_MASK_ACTIVY;
+		break;
+
+	case BUDGET_KNC1C:
+	case BUDGET_KNC1CP:
+	case BUDGET_CIN1200C:
+	case BUDGET_KNC1C_MK3:
+	case BUDGET_KNC1C_TDA10024:
+	case BUDGET_KNC1CP_MK3:
+	case BUDGET_CIN1200C_MK3:
+		budget->buffer_width = TS_WIDTH_DVBC;
+		max_bufsize = TS_MAX_BUFSIZE_K_DVBC;
+		height_mask = TS_HEIGHT_MASK_DVBC;
+		break;
+
+	default:
+		budget->buffer_width = TS_WIDTH;
+		max_bufsize = TS_MAX_BUFSIZE_K;
+		height_mask = TS_HEIGHT_MASK;
+	}
+
+	if (dma_buffer_size < TS_MIN_BUFSIZE_K)
+		dma_buffer_size = TS_MIN_BUFSIZE_K;
+	else if (dma_buffer_size > max_bufsize)
+		dma_buffer_size = max_bufsize;
+
+	budget->buffer_height = dma_buffer_size * 1024 / budget->buffer_width;
+	if (budget->buffer_height > 0xfff) {
+		budget->buffer_height /= 2;
+		budget->buffer_height &= height_mask;
+		budget->buffer_size = 2 * budget->buffer_height * budget->buffer_width;
+	} else {
+		budget->buffer_height &= height_mask;
+		budget->buffer_size = budget->buffer_height * budget->buffer_width;
+	}
+	budget->buffer_warning_threshold = budget->buffer_size * 80/100;
+	budget->buffer_warnings = 0;
+	budget->buffer_warning_time = jiffies;
+
+	dprintk(2, "%s: buffer type = %s, width = %d, height = %d\n",
+		budget->dev->name,
+		budget->buffer_size > budget->buffer_width * budget->buffer_height ? "odd/even" : "single",
+		budget->buffer_width, budget->buffer_height);
+	printk("%s: dma buffer size %u\n", budget->dev->name, budget->buffer_size);
+
+	ret = dvb_register_adapter(&budget->dvb_adapter, budget->card->name,
+				   owner, &budget->dev->pci->dev, adapter_nums);
+	if (ret < 0)
+		return ret;
+
+	/* set dd1 stream a & b */
+	saa7146_write(dev, DD1_STREAM_B, 0x00000000);
+	saa7146_write(dev, MC2, (MASK_09 | MASK_25));
+	saa7146_write(dev, MC2, (MASK_10 | MASK_26));
+	saa7146_write(dev, DD1_INIT, 0x02000000);
+	saa7146_write(dev, MC2, (MASK_09 | MASK_25 | MASK_10 | MASK_26));
+
+	if (bi->type != BUDGET_FS_ACTIVY)
+		budget->video_port = BUDGET_VIDEO_PORTB;
+	else
+		budget->video_port = BUDGET_VIDEO_PORTA;
+	spin_lock_init(&budget->feedlock);
+	spin_lock_init(&budget->debilock);
+
+	/* the Siemens DVB needs this if you want to have the i2c chips
+	   get recognized before the main driver is loaded */
+	if (bi->type != BUDGET_FS_ACTIVY)
+		saa7146_write(dev, GPIO_CTRL, 0x500000);	/* GPIO 3 = 1 */
+
+	strscpy(budget->i2c_adap.name, budget->card->name,
+		sizeof(budget->i2c_adap.name));
+
+	saa7146_i2c_adapter_prepare(dev, &budget->i2c_adap, SAA7146_I2C_BUS_BIT_RATE_120);
+	strscpy(budget->i2c_adap.name, budget->card->name,
+		sizeof(budget->i2c_adap.name));
+
+	if (i2c_add_adapter(&budget->i2c_adap) < 0) {
+		ret = -ENOMEM;
+		goto err_dvb_unregister;
+	}
+
+	ttpci_eeprom_parse_mac(&budget->i2c_adap, budget->dvb_adapter.proposed_mac);
+
+	budget->grabbing = saa7146_vmalloc_build_pgtable(dev->pci, budget->buffer_size, &budget->pt);
+	if (NULL == budget->grabbing) {
+		ret = -ENOMEM;
+		goto err_del_i2c;
+	}
+
+	saa7146_write(dev, PCI_BT_V1, 0x001c0000);
+	/* upload all */
+	saa7146_write(dev, GPIO_CTRL, 0x000000);
+
+	tasklet_setup(&budget->vpe_tasklet, vpeirq);
+
+	/* frontend power on */
+	if (bi->type != BUDGET_FS_ACTIVY)
+		saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
+
+	if ((ret = budget_register(budget)) == 0)
+		return 0; /* Everything OK */
+
+	/* An error occurred, cleanup resources */
+	saa7146_vfree_destroy_pgtable(dev->pci, budget->grabbing, &budget->pt);
+
+err_del_i2c:
+	i2c_del_adapter(&budget->i2c_adap);
+
+err_dvb_unregister:
+	dvb_unregister_adapter(&budget->dvb_adapter);
+
+	return ret;
+}
+
+void ttpci_budget_init_hooks(struct budget *budget)
+{
+	if (budget->dvb_frontend && !budget->read_fe_status) {
+		budget->read_fe_status = budget->dvb_frontend->ops.read_status;
+		budget->dvb_frontend->ops.read_status = budget_read_fe_status;
+	}
+}
+
+int ttpci_budget_deinit(struct budget *budget)
+{
+	struct saa7146_dev *dev = budget->dev;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	budget_unregister(budget);
+
+	tasklet_kill(&budget->vpe_tasklet);
+
+	saa7146_vfree_destroy_pgtable(dev->pci, budget->grabbing, &budget->pt);
+
+	i2c_del_adapter(&budget->i2c_adap);
+
+	dvb_unregister_adapter(&budget->dvb_adapter);
+
+	return 0;
+}
+
+void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 * isr)
+{
+	struct budget *budget = (struct budget *) dev->ext_priv;
+
+	dprintk(8, "dev: %p, budget: %p\n", dev, budget);
+
+	if (*isr & MASK_10)
+		tasklet_schedule(&budget->vpe_tasklet);
+}
+
+void ttpci_budget_set_video_port(struct saa7146_dev *dev, int video_port)
+{
+	struct budget *budget = (struct budget *) dev->ext_priv;
+
+	spin_lock(&budget->feedlock);
+	budget->video_port = video_port;
+	if (budget->feeding) {
+		stop_ts_capture(budget);
+		start_ts_capture(budget);
+	}
+	spin_unlock(&budget->feedlock);
+}
+
+EXPORT_SYMBOL_GPL(ttpci_budget_debiread);
+EXPORT_SYMBOL_GPL(ttpci_budget_debiwrite);
+EXPORT_SYMBOL_GPL(ttpci_budget_init);
+EXPORT_SYMBOL_GPL(ttpci_budget_init_hooks);
+EXPORT_SYMBOL_GPL(ttpci_budget_deinit);
+EXPORT_SYMBOL_GPL(ttpci_budget_irq10_handler);
+EXPORT_SYMBOL_GPL(ttpci_budget_set_video_port);
+EXPORT_SYMBOL_GPL(budget_debug);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/budget.c b/drivers/staging/media/deprecated/saa7146/ttpci/budget.c
new file mode 100644
index 000000000..a88711a3a
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/budget.c
@@ -0,0 +1,883 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * budget.c: driver for the SAA7146 based Budget DVB cards
+ *
+ * Compiled from various sources by Michael Hunold <michael@mihu.de>
+ *
+ * Copyright (C) 2002 Ralph Metzler <rjkm@metzlerbros.de>
+ *
+ * Copyright (C) 1999-2002 Ralph  Metzler
+ *                       & Marcus Metzler for convergence integrated media GmbH
+ *
+ * 26feb2004 Support for FS Activy Card (Grundig tuner) by
+ *           Michael Dreher <michael@5dot1.de>,
+ *           Oliver Endriss <o.endriss@gmx.de> and
+ *           Andreas 'randy' Weinberger
+ *
+ * the project's page is at https://linuxtv.org
+ */
+
+#include "budget.h"
+#include "stv0299.h"
+#include "ves1x93.h"
+#include "ves1820.h"
+#include "l64781.h"
+#include "tda8083.h"
+#include "s5h1420.h"
+#include "tda10086.h"
+#include "tda826x.h"
+#include "lnbp21.h"
+#include "bsru6.h"
+#include "bsbe1.h"
+#include "tdhd1.h"
+#include "stv6110x.h"
+#include "stv090x.h"
+#include "isl6423.h"
+#include "lnbh24.h"
+
+
+static int diseqc_method;
+module_param(diseqc_method, int, 0444);
+MODULE_PARM_DESC(diseqc_method, "Select DiSEqC method for subsystem id 13c2:1003, 0: default, 1: more reliable (for newer revisions only)");
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static void Set22K (struct budget *budget, int state)
+{
+	struct saa7146_dev *dev=budget->dev;
+	dprintk(2, "budget: %p\n", budget);
+	saa7146_setgpio(dev, 3, (state ? SAA7146_GPIO_OUTHI : SAA7146_GPIO_OUTLO));
+}
+
+/* Diseqc functions only for TT Budget card */
+/* taken from the Skyvision DVB driver by
+   Ralph Metzler <rjkm@metzlerbros.de> */
+
+static void DiseqcSendBit (struct budget *budget, int data)
+{
+	struct saa7146_dev *dev=budget->dev;
+	dprintk(2, "budget: %p\n", budget);
+
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+	udelay(data ? 500 : 1000);
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	udelay(data ? 1000 : 500);
+}
+
+static void DiseqcSendByte (struct budget *budget, int data)
+{
+	int i, par=1, d;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	for (i=7; i>=0; i--) {
+		d = (data>>i)&1;
+		par ^= d;
+		DiseqcSendBit(budget, d);
+	}
+
+	DiseqcSendBit(budget, par);
+}
+
+static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long burst)
+{
+	struct saa7146_dev *dev=budget->dev;
+	int i;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+	mdelay(16);
+
+	for (i=0; i<len; i++)
+		DiseqcSendByte(budget, msg[i]);
+
+	mdelay(16);
+
+	if (burst!=-1) {
+		if (burst)
+			DiseqcSendByte(budget, 0xff);
+		else {
+			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
+			mdelay(12);
+			udelay(500);
+			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
+		}
+		msleep(20);
+	}
+
+	return 0;
+}
+
+/*
+ *   Routines for the Fujitsu Siemens Activy budget card
+ *   22 kHz tone and DiSEqC are handled by the frontend.
+ *   Voltage must be set here.
+ *   GPIO 1: LNBP EN, GPIO 2: LNBP VSEL
+ */
+static int SetVoltage_Activy(struct budget *budget,
+			     enum fe_sec_voltage voltage)
+{
+	struct saa7146_dev *dev=budget->dev;
+
+	dprintk(2, "budget: %p\n", budget);
+
+	switch (voltage) {
+		case SEC_VOLTAGE_13:
+			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
+			saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTLO);
+			break;
+		case SEC_VOLTAGE_18:
+			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
+			saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
+			break;
+		case SEC_VOLTAGE_OFF:
+			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTLO);
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int siemens_budget_set_voltage(struct dvb_frontend *fe,
+				      enum fe_sec_voltage voltage)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	return SetVoltage_Activy (budget, voltage);
+}
+
+static int budget_set_tone(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		Set22K (budget, 1);
+		break;
+
+	case SEC_TONE_OFF:
+		Set22K (budget, 0);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int budget_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	SendDiSEqCMsg (budget, cmd->msg_len, cmd->msg, 0);
+
+	return 0;
+}
+
+static int budget_diseqc_send_burst(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd)
+{
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+
+	SendDiSEqCMsg (budget, 0, NULL, minicmd);
+
+	return 0;
+}
+
+static int alps_bsrv2_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+	u8 pwr = 0;
+	u8 buf[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = buf, .len = sizeof(buf) };
+	u32 div = (c->frequency + 479500) / 125;
+
+	if (c->frequency > 2000000)
+		pwr = 3;
+	else if (c->frequency > 1800000)
+		pwr = 2;
+	else if (c->frequency > 1600000)
+		pwr = 1;
+	else if (c->frequency > 1200000)
+		pwr = 0;
+	else if (c->frequency >= 1100000)
+		pwr = 1;
+	else pwr = 2;
+
+	buf[0] = (div >> 8) & 0x7f;
+	buf[1] = div & 0xff;
+	buf[2] = ((div & 0x18000) >> 10) | 0x95;
+	buf[3] = (pwr << 6) | 0x30;
+
+	// NOTE: since we're using a prescaler of 2, we set the
+	// divisor frequency to 62.5kHz and divide by 125 above
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	return 0;
+}
+
+static struct ves1x93_config alps_bsrv2_config =
+{
+	.demod_address = 0x08,
+	.xin = 90100000UL,
+	.invert_pwm = 0,
+};
+
+static int alps_tdbe2_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x62, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = (c->frequency + 35937500 + 31250) / 62500;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x85 | ((div >> 10) & 0x60);
+	data[3] = (c->frequency < 174000000 ? 0x88 : c->frequency < 470000000 ? 0x84 : 0x81);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	return 0;
+}
+
+static struct ves1820_config alps_tdbe2_config = {
+	.demod_address = 0x09,
+	.xin = 57840000UL,
+	.invert = 1,
+	.selagc = VES1820_SELAGC_SIGNAMPERR,
+};
+
+static int grundig_29504_401_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget *budget = fe->dvb->priv;
+	u8 *tuner_addr = fe->tuner_priv;
+	u32 div;
+	u8 cfg, cpump, band_select;
+	u8 data[4];
+	struct i2c_msg msg = { .flags = 0, .buf = data, .len = sizeof(data) };
+
+	if (tuner_addr)
+		msg.addr = *tuner_addr;
+	else
+		msg.addr = 0x61;
+
+	div = (36125000 + c->frequency) / 166666;
+
+	cfg = 0x88;
+
+	if (c->frequency < 175000000)
+		cpump = 2;
+	else if (c->frequency < 390000000)
+		cpump = 1;
+	else if (c->frequency < 470000000)
+		cpump = 2;
+	else if (c->frequency < 750000000)
+		cpump = 1;
+	else
+		cpump = 3;
+
+	if (c->frequency < 175000000)
+		band_select = 0x0e;
+	else if (c->frequency < 470000000)
+		band_select = 0x05;
+	else
+		band_select = 0x03;
+
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = ((div >> 10) & 0x60) | cfg;
+	data[3] = (cpump << 6) | band_select;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	return 0;
+}
+
+static struct l64781_config grundig_29504_401_config = {
+	.demod_address = 0x55,
+};
+
+static struct l64781_config grundig_29504_401_config_activy = {
+	.demod_address = 0x54,
+};
+
+static u8 tuner_address_grundig_29504_401_activy = 0x60;
+
+static int grundig_29504_451_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = c->frequency / 125;
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0x8e;
+	data[3] = 0x00;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	return 0;
+}
+
+static struct tda8083_config grundig_29504_451_config = {
+	.demod_address = 0x68,
+};
+
+static int s5h1420_tuner_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct budget* budget = (struct budget*) fe->dvb->priv;
+	u32 div;
+	u8 data[4];
+	struct i2c_msg msg = { .addr = 0x61, .flags = 0, .buf = data, .len = sizeof(data) };
+
+	div = c->frequency / 1000;
+	data[0] = (div >> 8) & 0x7f;
+	data[1] = div & 0xff;
+	data[2] = 0xc2;
+
+	if (div < 1450)
+		data[3] = 0x00;
+	else if (div < 1850)
+		data[3] = 0x40;
+	else if (div < 2000)
+		data[3] = 0x80;
+	else
+		data[3] = 0xc0;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+
+	return 0;
+}
+
+static struct s5h1420_config s5h1420_config = {
+	.demod_address = 0x53,
+	.invert = 1,
+	.cdclk_polarity = 1,
+};
+
+static struct tda10086_config tda10086_config = {
+	.demod_address = 0x0e,
+	.invert = 0,
+	.diseqc_tone = 1,
+	.xtal_freq = TDA10086_XTAL_16M,
+};
+
+static const struct stv0299_config alps_bsru6_config_activy = {
+	.demod_address = 0x68,
+	.inittab = alps_bsru6_inittab,
+	.mclk = 88000000UL,
+	.invert = 1,
+	.op0_off = 1,
+	.min_delay_ms = 100,
+	.set_symbol_rate = alps_bsru6_set_symbol_rate,
+};
+
+static const struct stv0299_config alps_bsbe1_config_activy = {
+	.demod_address = 0x68,
+	.inittab = alps_bsbe1_inittab,
+	.mclk = 88000000UL,
+	.invert = 1,
+	.op0_off = 1,
+	.min_delay_ms = 100,
+	.set_symbol_rate = alps_bsbe1_set_symbol_rate,
+};
+
+static int alps_tdhd1_204_request_firmware(struct dvb_frontend *fe, const struct firmware **fw, char *name)
+{
+	struct budget *budget = (struct budget *)fe->dvb->priv;
+
+	return request_firmware(fw, name, &budget->dev->pci->dev);
+}
+
+
+static int i2c_readreg(struct i2c_adapter *i2c, u8 adr, u8 reg)
+{
+	u8 val;
+	struct i2c_msg msg[] = {
+		{ .addr = adr, .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = adr, .flags = I2C_M_RD, .buf = &val, .len = 1 }
+	};
+
+	return (i2c_transfer(i2c, msg, 2) != 2) ? -EIO : val;
+}
+
+static u8 read_pwm(struct budget* budget)
+{
+	u8 b = 0xff;
+	u8 pwm;
+	struct i2c_msg msg[] = { { .addr = 0x50,.flags = 0,.buf = &b,.len = 1 },
+				 { .addr = 0x50,.flags = I2C_M_RD,.buf = &pwm,.len = 1} };
+
+	if ((i2c_transfer(&budget->i2c_adap, msg, 2) != 2) || (pwm == 0xff))
+		pwm = 0x48;
+
+	return pwm;
+}
+
+static struct stv090x_config tt1600_stv090x_config = {
+	.device			= STV0903,
+	.demod_mode		= STV090x_SINGLE,
+	.clk_mode		= STV090x_CLK_EXT,
+
+	.xtal			= 13500000,
+	.address		= 0x68,
+
+	.ts1_mode		= STV090x_TSMODE_DVBCI,
+	.ts2_mode		= STV090x_TSMODE_SERIAL_CONTINUOUS,
+
+	.repeater_level		= STV090x_RPTLEVEL_16,
+
+	.tuner_init		= NULL,
+	.tuner_sleep		= NULL,
+	.tuner_set_mode		= NULL,
+	.tuner_set_frequency	= NULL,
+	.tuner_get_frequency	= NULL,
+	.tuner_set_bandwidth	= NULL,
+	.tuner_get_bandwidth	= NULL,
+	.tuner_set_bbgain	= NULL,
+	.tuner_get_bbgain	= NULL,
+	.tuner_set_refclk	= NULL,
+	.tuner_get_status	= NULL,
+};
+
+static struct stv6110x_config tt1600_stv6110x_config = {
+	.addr			= 0x60,
+	.refclk			= 27000000,
+	.clk_div		= 2,
+};
+
+static struct isl6423_config tt1600_isl6423_config = {
+	.current_max		= SEC_CURRENT_515m,
+	.curlim			= SEC_CURRENT_LIM_ON,
+	.mod_extern		= 1,
+	.addr			= 0x08,
+};
+
+static void frontend_init(struct budget *budget)
+{
+	(void)alps_bsbe1_config; /* avoid warning */
+
+	switch(budget->dev->pci->subsystem_device) {
+	case 0x1003: // Hauppauge/TT Nova budget (stv0299/ALPS BSRU6(tsa5059) OR ves1893/ALPS BSRV2(sp5659))
+	case 0x1013:
+		// try the ALPS BSRV2 first of all
+		budget->dvb_frontend = dvb_attach(ves1x93_attach, &alps_bsrv2_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsrv2_tuner_set_params;
+			budget->dvb_frontend->ops.diseqc_send_master_cmd = budget_diseqc_send_master_cmd;
+			budget->dvb_frontend->ops.diseqc_send_burst = budget_diseqc_send_burst;
+			budget->dvb_frontend->ops.set_tone = budget_set_tone;
+			break;
+		}
+
+		// try the ALPS BSRU6 now
+		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+			if (budget->dev->pci->subsystem_device == 0x1003 && diseqc_method == 0) {
+				budget->dvb_frontend->ops.diseqc_send_master_cmd = budget_diseqc_send_master_cmd;
+				budget->dvb_frontend->ops.diseqc_send_burst = budget_diseqc_send_burst;
+				budget->dvb_frontend->ops.set_tone = budget_set_tone;
+			}
+			break;
+		}
+		break;
+
+	case 0x1004: // Hauppauge/TT DVB-C budget (ves1820/ALPS TDBE2(sp5659))
+
+		budget->dvb_frontend = dvb_attach(ves1820_attach, &alps_tdbe2_config, &budget->i2c_adap, read_pwm(budget));
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_tdbe2_tuner_set_params;
+			break;
+		}
+		break;
+
+	case 0x1005: // Hauppauge/TT Nova-T budget (L64781/Grundig 29504-401(tsa5060))
+
+		budget->dvb_frontend = dvb_attach(l64781_attach, &grundig_29504_401_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = grundig_29504_401_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = NULL;
+			break;
+		}
+		break;
+
+	case 0x4f52: /* Cards based on Philips Semi Sylt PCI ref. design */
+		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			printk(KERN_INFO "budget: tuner ALPS BSRU6 in Philips Semi. Sylt detected\n");
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+			break;
+		}
+		break;
+
+	case 0x4f60: /* Fujitsu Siemens Activy Budget-S PCI rev AL (stv0299/tsa5059) */
+	{
+		int subtype = i2c_readreg(&budget->i2c_adap, 0x50, 0x67);
+
+		if (subtype < 0)
+			break;
+		/* fixme: find a better way to identify the card */
+		if (subtype < 0x36) {
+			/* assume ALPS BSRU6 */
+			budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config_activy, &budget->i2c_adap);
+			if (budget->dvb_frontend) {
+				printk(KERN_INFO "budget: tuner ALPS BSRU6 detected\n");
+				budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+				budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+				budget->dvb_frontend->ops.set_voltage = siemens_budget_set_voltage;
+				budget->dvb_frontend->ops.dishnetwork_send_legacy_command = NULL;
+				break;
+			}
+		} else {
+			/* assume ALPS BSBE1 */
+			/* reset tuner */
+			saa7146_setgpio(budget->dev, 3, SAA7146_GPIO_OUTLO);
+			msleep(50);
+			saa7146_setgpio(budget->dev, 3, SAA7146_GPIO_OUTHI);
+			msleep(250);
+			budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsbe1_config_activy, &budget->i2c_adap);
+			if (budget->dvb_frontend) {
+				printk(KERN_INFO "budget: tuner ALPS BSBE1 detected\n");
+				budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsbe1_tuner_set_params;
+				budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+				budget->dvb_frontend->ops.set_voltage = siemens_budget_set_voltage;
+				budget->dvb_frontend->ops.dishnetwork_send_legacy_command = NULL;
+				break;
+			}
+		}
+		break;
+	}
+
+	case 0x4f61: // Fujitsu Siemens Activy Budget-S PCI rev GR (tda8083/Grundig 29504-451(tsa5522))
+		budget->dvb_frontend = dvb_attach(tda8083_attach, &grundig_29504_451_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = grundig_29504_451_tuner_set_params;
+			budget->dvb_frontend->ops.set_voltage = siemens_budget_set_voltage;
+			budget->dvb_frontend->ops.dishnetwork_send_legacy_command = NULL;
+		}
+		break;
+
+	case 0x5f60: /* Fujitsu Siemens Activy Budget-T PCI rev AL (tda10046/ALPS TDHD1-204A) */
+		budget->dvb_frontend = dvb_attach(tda10046_attach, &alps_tdhd1_204a_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_tdhd1_204a_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+		}
+		break;
+
+	case 0x5f61: /* Fujitsu Siemens Activy Budget-T PCI rev GR (L64781/Grundig 29504-401(tsa5060)) */
+		budget->dvb_frontend = dvb_attach(l64781_attach, &grundig_29504_401_config_activy, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			budget->dvb_frontend->tuner_priv = &tuner_address_grundig_29504_401_activy;
+			budget->dvb_frontend->ops.tuner_ops.set_params = grundig_29504_401_tuner_set_params;
+		}
+		break;
+
+	case 0x1016: // Hauppauge/TT Nova-S SE (samsung s5h1420/????(tda8260))
+	{
+		struct dvb_frontend *fe;
+
+		fe = dvb_attach(s5h1420_attach, &s5h1420_config, &budget->i2c_adap);
+		if (fe) {
+			fe->ops.tuner_ops.set_params = s5h1420_tuner_set_params;
+			budget->dvb_frontend = fe;
+			if (dvb_attach(lnbp21_attach, fe, &budget->i2c_adap,
+				       0, 0) == NULL) {
+				printk("%s: No LNBP21 found!\n", __func__);
+				goto error_out;
+			}
+			break;
+		}
+	}
+		fallthrough;
+	case 0x1018: // TT Budget-S-1401 (philips tda10086/philips tda8262)
+	{
+		struct dvb_frontend *fe;
+
+		// gpio2 is connected to CLB - reset it + leave it high
+		saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTLO);
+		msleep(1);
+		saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTHI);
+		msleep(1);
+
+		fe = dvb_attach(tda10086_attach, &tda10086_config, &budget->i2c_adap);
+		if (fe) {
+			budget->dvb_frontend = fe;
+			if (dvb_attach(tda826x_attach, fe, 0x60,
+				       &budget->i2c_adap, 0) == NULL)
+				printk("%s: No tda826x found!\n", __func__);
+			if (dvb_attach(lnbp21_attach, fe,
+				       &budget->i2c_adap, 0, 0) == NULL) {
+				printk("%s: No LNBP21 found!\n", __func__);
+				goto error_out;
+			}
+			break;
+		}
+	}
+		fallthrough;
+
+	case 0x101c: { /* TT S2-1600 */
+			const struct stv6110x_devctl *ctl;
+			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTLO);
+			msleep(50);
+			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTHI);
+			msleep(250);
+
+			budget->dvb_frontend = dvb_attach(stv090x_attach,
+							  &tt1600_stv090x_config,
+							  &budget->i2c_adap,
+							  STV090x_DEMODULATOR_0);
+
+			if (budget->dvb_frontend) {
+
+				ctl = dvb_attach(stv6110x_attach,
+						 budget->dvb_frontend,
+						 &tt1600_stv6110x_config,
+						 &budget->i2c_adap);
+
+				if (ctl) {
+					tt1600_stv090x_config.tuner_init	  = ctl->tuner_init;
+					tt1600_stv090x_config.tuner_sleep	  = ctl->tuner_sleep;
+					tt1600_stv090x_config.tuner_set_mode	  = ctl->tuner_set_mode;
+					tt1600_stv090x_config.tuner_set_frequency = ctl->tuner_set_frequency;
+					tt1600_stv090x_config.tuner_get_frequency = ctl->tuner_get_frequency;
+					tt1600_stv090x_config.tuner_set_bandwidth = ctl->tuner_set_bandwidth;
+					tt1600_stv090x_config.tuner_get_bandwidth = ctl->tuner_get_bandwidth;
+					tt1600_stv090x_config.tuner_set_bbgain	  = ctl->tuner_set_bbgain;
+					tt1600_stv090x_config.tuner_get_bbgain	  = ctl->tuner_get_bbgain;
+					tt1600_stv090x_config.tuner_set_refclk	  = ctl->tuner_set_refclk;
+					tt1600_stv090x_config.tuner_get_status	  = ctl->tuner_get_status;
+
+					/* call the init function once to initialize
+					   tuner's clock output divider and demod's
+					   master clock */
+					if (budget->dvb_frontend->ops.init)
+						budget->dvb_frontend->ops.init(budget->dvb_frontend);
+
+					if (dvb_attach(isl6423_attach,
+						       budget->dvb_frontend,
+						       &budget->i2c_adap,
+						       &tt1600_isl6423_config) == NULL) {
+						printk(KERN_ERR "%s: No Intersil ISL6423 found!\n", __func__);
+						goto error_out;
+					}
+				} else {
+					printk(KERN_ERR "%s: No STV6110(A) Silicon Tuner found!\n", __func__);
+					goto error_out;
+				}
+			}
+		}
+		break;
+
+	case 0x1020: { /* Omicom S2 */
+			const struct stv6110x_devctl *ctl;
+			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTLO);
+			msleep(50);
+			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTHI);
+			msleep(250);
+
+			budget->dvb_frontend = dvb_attach(stv090x_attach,
+							  &tt1600_stv090x_config,
+							  &budget->i2c_adap,
+							  STV090x_DEMODULATOR_0);
+
+			if (budget->dvb_frontend) {
+				printk(KERN_INFO "budget: Omicom S2 detected\n");
+
+				ctl = dvb_attach(stv6110x_attach,
+						 budget->dvb_frontend,
+						 &tt1600_stv6110x_config,
+						 &budget->i2c_adap);
+
+				if (ctl) {
+					tt1600_stv090x_config.tuner_init	  = ctl->tuner_init;
+					tt1600_stv090x_config.tuner_sleep	  = ctl->tuner_sleep;
+					tt1600_stv090x_config.tuner_set_mode	  = ctl->tuner_set_mode;
+					tt1600_stv090x_config.tuner_set_frequency = ctl->tuner_set_frequency;
+					tt1600_stv090x_config.tuner_get_frequency = ctl->tuner_get_frequency;
+					tt1600_stv090x_config.tuner_set_bandwidth = ctl->tuner_set_bandwidth;
+					tt1600_stv090x_config.tuner_get_bandwidth = ctl->tuner_get_bandwidth;
+					tt1600_stv090x_config.tuner_set_bbgain	  = ctl->tuner_set_bbgain;
+					tt1600_stv090x_config.tuner_get_bbgain	  = ctl->tuner_get_bbgain;
+					tt1600_stv090x_config.tuner_set_refclk	  = ctl->tuner_set_refclk;
+					tt1600_stv090x_config.tuner_get_status	  = ctl->tuner_get_status;
+
+					/* call the init function once to initialize
+					   tuner's clock output divider and demod's
+					   master clock */
+					if (budget->dvb_frontend->ops.init)
+						budget->dvb_frontend->ops.init(budget->dvb_frontend);
+
+					if (dvb_attach(lnbh24_attach,
+							budget->dvb_frontend,
+							&budget->i2c_adap,
+							LNBH24_PCL | LNBH24_TTX,
+							LNBH24_TEN, 0x14>>1) == NULL) {
+						printk(KERN_ERR
+						"No LNBH24 found!\n");
+						goto error_out;
+					}
+				} else {
+					printk(KERN_ERR "%s: No STV6110(A) Silicon Tuner found!\n", __func__);
+					goto error_out;
+				}
+			}
+		}
+		break;
+	}
+
+	if (budget->dvb_frontend == NULL) {
+		printk("budget: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		       budget->dev->pci->vendor,
+		       budget->dev->pci->device,
+		       budget->dev->pci->subsystem_vendor,
+		       budget->dev->pci->subsystem_device);
+	} else {
+		if (dvb_register_frontend(&budget->dvb_adapter, budget->dvb_frontend))
+			goto error_out;
+	}
+	return;
+
+error_out:
+	printk("budget: Frontend registration failed!\n");
+	dvb_frontend_detach(budget->dvb_frontend);
+	budget->dvb_frontend = NULL;
+	return;
+}
+
+static int budget_attach (struct saa7146_dev* dev, struct saa7146_pci_extension_data *info)
+{
+	struct budget *budget = NULL;
+	int err;
+
+	budget = kmalloc(sizeof(struct budget), GFP_KERNEL);
+	if( NULL == budget ) {
+		return -ENOMEM;
+	}
+
+	dprintk(2, "dev:%p, info:%p, budget:%p\n", dev, info, budget);
+
+	dev->ext_priv = budget;
+
+	err = ttpci_budget_init(budget, dev, info, THIS_MODULE, adapter_nr);
+	if (err) {
+		printk("==> failed\n");
+		kfree (budget);
+		return err;
+	}
+
+	budget->dvb_adapter.priv = budget;
+	frontend_init(budget);
+
+	ttpci_budget_init_hooks(budget);
+
+	return 0;
+}
+
+static int budget_detach (struct saa7146_dev* dev)
+{
+	struct budget *budget = (struct budget*) dev->ext_priv;
+	int err;
+
+	if (budget->dvb_frontend) {
+		dvb_unregister_frontend(budget->dvb_frontend);
+		dvb_frontend_detach(budget->dvb_frontend);
+	}
+
+	err = ttpci_budget_deinit (budget);
+
+	kfree (budget);
+	dev->ext_priv = NULL;
+
+	return err;
+}
+
+static struct saa7146_extension budget_extension;
+
+MAKE_BUDGET_INFO(ttbs,	"TT-Budget/WinTV-NOVA-S  PCI",	BUDGET_TT);
+MAKE_BUDGET_INFO(ttbc,	"TT-Budget/WinTV-NOVA-C  PCI",	BUDGET_TT);
+MAKE_BUDGET_INFO(ttbt,	"TT-Budget/WinTV-NOVA-T  PCI",	BUDGET_TT);
+MAKE_BUDGET_INFO(satel,	"SATELCO Multimedia PCI",	BUDGET_TT_HW_DISEQC);
+MAKE_BUDGET_INFO(ttbs1401, "TT-Budget-S-1401 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(tt1600, "TT-Budget S2-1600 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(fsacs0, "Fujitsu Siemens Activy Budget-S PCI (rev GR/grundig frontend)", BUDGET_FS_ACTIVY);
+MAKE_BUDGET_INFO(fsacs1, "Fujitsu Siemens Activy Budget-S PCI (rev AL/alps frontend)", BUDGET_FS_ACTIVY);
+MAKE_BUDGET_INFO(fsact,	 "Fujitsu Siemens Activy Budget-T PCI (rev GR/Grundig frontend)", BUDGET_FS_ACTIVY);
+MAKE_BUDGET_INFO(fsact1, "Fujitsu Siemens Activy Budget-T PCI (rev AL/ALPS TDHD1-204A)", BUDGET_FS_ACTIVY);
+MAKE_BUDGET_INFO(omicom, "Omicom S2 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(sylt,   "Philips Semi Sylt PCI", BUDGET_TT_HW_DISEQC);
+
+static const struct pci_device_id pci_tbl[] = {
+	MAKE_EXTENSION_PCI(ttbs,  0x13c2, 0x1003),
+	MAKE_EXTENSION_PCI(ttbc,  0x13c2, 0x1004),
+	MAKE_EXTENSION_PCI(ttbt,  0x13c2, 0x1005),
+	MAKE_EXTENSION_PCI(satel, 0x13c2, 0x1013),
+	MAKE_EXTENSION_PCI(ttbs,  0x13c2, 0x1016),
+	MAKE_EXTENSION_PCI(ttbs1401, 0x13c2, 0x1018),
+	MAKE_EXTENSION_PCI(tt1600, 0x13c2, 0x101c),
+	MAKE_EXTENSION_PCI(fsacs1,0x1131, 0x4f60),
+	MAKE_EXTENSION_PCI(fsacs0,0x1131, 0x4f61),
+	MAKE_EXTENSION_PCI(fsact1, 0x1131, 0x5f60),
+	MAKE_EXTENSION_PCI(fsact, 0x1131, 0x5f61),
+	MAKE_EXTENSION_PCI(omicom, 0x14c4, 0x1020),
+	MAKE_EXTENSION_PCI(sylt, 0x1131, 0x4f52),
+	{
+		.vendor    = 0,
+	}
+};
+
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+static struct saa7146_extension budget_extension = {
+	.name		= "budget dvb",
+	.flags		= SAA7146_USE_I2C_IRQ,
+
+	.module		= THIS_MODULE,
+	.pci_tbl	= pci_tbl,
+	.attach		= budget_attach,
+	.detach		= budget_detach,
+
+	.irq_mask	= MASK_10,
+	.irq_func	= ttpci_budget_irq10_handler,
+};
+
+static int __init budget_init(void)
+{
+	return saa7146_register_extension(&budget_extension);
+}
+
+static void __exit budget_exit(void)
+{
+	saa7146_unregister_extension(&budget_extension);
+}
+
+module_init(budget_init);
+module_exit(budget_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Ralph Metzler, Marcus Metzler, Michael Hunold, others");
+MODULE_DESCRIPTION("driver for the SAA7146 based so-called budget PCI DVB cards by Siemens, Technotrend, Hauppauge");
diff --git a/drivers/staging/media/deprecated/saa7146/ttpci/budget.h b/drivers/staging/media/deprecated/saa7146/ttpci/budget.h
new file mode 100644
index 000000000..82cc0df49
--- /dev/null
+++ b/drivers/staging/media/deprecated/saa7146/ttpci/budget.h
@@ -0,0 +1,129 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef __BUDGET_DVB__
+#define __BUDGET_DVB__
+
+#include <media/dvb_frontend.h>
+#include <media/dvbdev.h>
+#include <media/demux.h>
+#include <media/dvb_demux.h>
+#include <media/dmxdev.h>
+#include <media/dvb_net.h>
+
+#include <linux/module.h>
+#include <linux/mutex.h>
+
+#include "../common/saa7146.h"
+
+extern int budget_debug;
+
+#ifdef dprintk
+#undef dprintk
+#endif
+
+#define dprintk(level, fmt, arg...) do {				\
+	if (level & budget_debug)					\
+		printk(KERN_DEBUG KBUILD_MODNAME ": %s(): " fmt,	\
+		       __func__, ##arg);				\
+} while (0)
+
+#define TS_SIZE        188
+
+struct budget_info {
+	char *name;
+	int type;
+};
+
+/* place to store all the necessary device information */
+struct budget {
+
+	/* devices */
+	struct dvb_device dvb_dev;
+	struct dvb_net dvb_net;
+
+	struct saa7146_dev *dev;
+
+	struct i2c_adapter i2c_adap;
+	struct budget_info *card;
+
+	unsigned char *grabbing;
+	struct saa7146_pgtable pt;
+
+	struct tasklet_struct fidb_tasklet;
+	struct tasklet_struct vpe_tasklet;
+
+	struct dmxdev dmxdev;
+	struct dvb_demux demux;
+
+	struct dmx_frontend hw_frontend;
+	struct dmx_frontend mem_frontend;
+
+	int ci_present;
+	int video_port;
+
+	u32 buffer_width;
+	u32 buffer_height;
+	u32 buffer_size;
+	u32 buffer_warning_threshold;
+	u32 buffer_warnings;
+	unsigned long buffer_warning_time;
+
+	u32 ttbp;
+	int feeding;
+
+	spinlock_t feedlock;
+
+	spinlock_t debilock;
+
+	struct dvb_adapter dvb_adapter;
+	struct dvb_frontend *dvb_frontend;
+	int (*read_fe_status)(struct dvb_frontend *fe, enum fe_status *status);
+	int fe_synced;
+
+	void *priv;
+};
+
+#define MAKE_BUDGET_INFO(x_var,x_name,x_type) \
+static struct budget_info x_var ## _info = { \
+	.name=x_name,	\
+	.type=x_type };	\
+static struct saa7146_pci_extension_data x_var = { \
+	.ext_priv = &x_var ## _info, \
+	.ext = &budget_extension };
+
+#define BUDGET_TT		   0
+#define BUDGET_TT_HW_DISEQC	   1
+#define BUDGET_PATCH		   3
+#define BUDGET_FS_ACTIVY	   4
+#define BUDGET_CIN1200S		   5
+#define BUDGET_CIN1200C		   6
+#define BUDGET_CIN1200T		   7
+#define BUDGET_KNC1S		   8
+#define BUDGET_KNC1C		   9
+#define BUDGET_KNC1T		   10
+#define BUDGET_KNC1SP		   11
+#define BUDGET_KNC1CP		   12
+#define BUDGET_KNC1TP		   13
+#define BUDGET_TVSTAR		   14
+#define BUDGET_CIN1200C_MK3	   15
+#define BUDGET_KNC1C_MK3	   16
+#define BUDGET_KNC1CP_MK3	   17
+#define BUDGET_KNC1S2              18
+#define BUDGET_KNC1C_TDA10024	   19
+
+#define BUDGET_VIDEO_PORTA         0
+#define BUDGET_VIDEO_PORTB         1
+
+extern int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
+			     struct saa7146_pci_extension_data *info,
+			     struct module *owner, short *adapter_nums);
+extern void ttpci_budget_init_hooks(struct budget *budget);
+extern int ttpci_budget_deinit(struct budget *budget);
+extern void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 * isr);
+extern void ttpci_budget_set_video_port(struct saa7146_dev *dev, int video_port);
+extern int ttpci_budget_debiread(struct budget *budget, u32 config, int addr, int count,
+				 int uselocks, int nobusyloop);
+extern int ttpci_budget_debiwrite(struct budget *budget, u32 config, int addr, int count, u32 value,
+				  int uselocks, int nobusyloop);
+
+#endif
diff --git a/drivers/staging/media/deprecated/stkwebcam/Kconfig b/drivers/staging/media/deprecated/stkwebcam/Kconfig
new file mode 100644
index 000000000..7234498e6
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_STKWEBCAM
+	tristate "USB Syntek DC1125 Camera support (DEPRECATED)"
+	depends on VIDEO_DEV
+	depends on MEDIA_USB_SUPPORT && MEDIA_CAMERA_SUPPORT
+	help
+	  Say Y here if you want to use this type of camera.
+	  Supported devices are typically found in some Asus laptops,
+	  with USB id 174f:a311 and 05e1:0501. Other Syntek cameras
+	  may be supported by the stk11xx driver, from which this is
+	  derived, see <http://sourceforge.net/projects/syntekdriver/>
+
+	  This driver is deprecated and is scheduled for removal by
+	  the end of 2022. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called stkwebcam.
+
diff --git a/drivers/staging/media/deprecated/stkwebcam/Makefile b/drivers/staging/media/deprecated/stkwebcam/Makefile
new file mode 100644
index 000000000..17ad7b6f4
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0-only
+stkwebcam-objs	:=	stk-webcam.o stk-sensor.o
+
+obj-$(CONFIG_VIDEO_STKWEBCAM)     += stkwebcam.o
+
diff --git a/drivers/staging/media/deprecated/stkwebcam/TODO b/drivers/staging/media/deprecated/stkwebcam/TODO
new file mode 100644
index 000000000..735304a72
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/TODO
@@ -0,0 +1,12 @@
+This is a very old driver for very old hardware (specifically
+laptops that use this sensor). In addition according to reports
+the picture quality is quite bad.
+
+This is also one of the few drivers still not using the vb2
+framework (or even the old videobuf framework!), so this driver
+is now deprecated with the intent of removing it altogether by
+the end of 2022.
+
+In order to keep this driver it has to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/stkwebcam/stk-sensor.c b/drivers/staging/media/deprecated/stkwebcam/stk-sensor.c
new file mode 100644
index 000000000..94aa6a27f
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/stk-sensor.c
@@ -0,0 +1,587 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* stk-sensor.c: Driver for ov96xx sensor (used in some Syntek webcams)
+ *
+ * Copyright 2007-2008 Jaime Velasco Juan <jsagarribay@gmail.com>
+ *
+ * Some parts derived from ov7670.c:
+ * Copyright 2006 One Laptop Per Child Association, Inc.  Written
+ * by Jonathan Corbet with substantial inspiration from Mark
+ * McClelland's ovcamchip code.
+ *
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * This file may be distributed under the terms of the GNU General
+ */
+
+/* Controlling the sensor via the STK1125 vendor specific control interface:
+ * The camera uses an OmniVision sensor and the stk1125 provides an
+ * SCCB(i2c)-USB bridge which let us program the sensor.
+ * In my case the sensor id is 0x9652, it can be read from sensor's register
+ * 0x0A and 0x0B as follows:
+ * - read register #R:
+ *   output #R to index 0x0208
+ *   output 0x0070 to index 0x0200
+ *   input 1 byte from index 0x0201 (some kind of status register)
+ *     until its value is 0x01
+ *   input 1 byte from index 0x0209. This is the value of #R
+ * - write value V to register #R
+ *   output #R to index 0x0204
+ *   output V to index 0x0205
+ *   output 0x0005 to index 0x0200
+ *   input 1 byte from index 0x0201 until its value becomes 0x04
+ */
+
+/* It seems the i2c bus is controlled with these registers */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "stk-webcam.h"
+
+#define STK_IIC_BASE		(0x0200)
+#  define STK_IIC_OP		(STK_IIC_BASE)
+#    define STK_IIC_OP_TX	(0x05)
+#    define STK_IIC_OP_RX	(0x70)
+#  define STK_IIC_STAT		(STK_IIC_BASE+1)
+#    define STK_IIC_STAT_TX_OK	(0x04)
+#    define STK_IIC_STAT_RX_OK	(0x01)
+/* I don't know what does this register.
+ * when it is 0x00 or 0x01, we cannot talk to the sensor,
+ * other values work */
+#  define STK_IIC_ENABLE	(STK_IIC_BASE+2)
+#    define STK_IIC_ENABLE_NO	(0x00)
+/* This is what the driver writes in windows */
+#    define STK_IIC_ENABLE_YES	(0x1e)
+/*
+ * Address of the slave. Seems like the binary driver look for the
+ * sensor in multiple places, attempting a reset sequence.
+ * We only know about the ov9650
+ */
+#  define STK_IIC_ADDR		(STK_IIC_BASE+3)
+#  define STK_IIC_TX_INDEX	(STK_IIC_BASE+4)
+#  define STK_IIC_TX_VALUE	(STK_IIC_BASE+5)
+#  define STK_IIC_RX_INDEX	(STK_IIC_BASE+8)
+#  define STK_IIC_RX_VALUE	(STK_IIC_BASE+9)
+
+#define MAX_RETRIES		(50)
+
+#define SENSOR_ADDRESS		(0x60)
+
+/* From ov7670.c (These registers aren't fully accurate) */
+
+/* Registers */
+#define REG_GAIN	0x00	/* Gain lower 8 bits (rest in vref) */
+#define REG_BLUE	0x01	/* blue gain */
+#define REG_RED		0x02	/* red gain */
+#define REG_VREF	0x03	/* Pieces of GAIN, VSTART, VSTOP */
+#define REG_COM1	0x04	/* Control 1 */
+#define  COM1_CCIR656	  0x40  /* CCIR656 enable */
+#define  COM1_QFMT	  0x20  /* QVGA/QCIF format */
+#define  COM1_SKIP_0	  0x00  /* Do not skip any row */
+#define  COM1_SKIP_2	  0x04  /* Skip 2 rows of 4 */
+#define  COM1_SKIP_3	  0x08  /* Skip 3 rows of 4 */
+#define REG_BAVE	0x05	/* U/B Average level */
+#define REG_GbAVE	0x06	/* Y/Gb Average level */
+#define REG_AECHH	0x07	/* AEC MS 5 bits */
+#define REG_RAVE	0x08	/* V/R Average level */
+#define REG_COM2	0x09	/* Control 2 */
+#define  COM2_SSLEEP	  0x10	/* Soft sleep mode */
+#define REG_PID		0x0a	/* Product ID MSB */
+#define REG_VER		0x0b	/* Product ID LSB */
+#define REG_COM3	0x0c	/* Control 3 */
+#define  COM3_SWAP	  0x40	  /* Byte swap */
+#define  COM3_SCALEEN	  0x08	  /* Enable scaling */
+#define  COM3_DCWEN	  0x04	  /* Enable downsamp/crop/window */
+#define REG_COM4	0x0d	/* Control 4 */
+#define REG_COM5	0x0e	/* All "reserved" */
+#define REG_COM6	0x0f	/* Control 6 */
+#define REG_AECH	0x10	/* More bits of AEC value */
+#define REG_CLKRC	0x11	/* Clock control */
+#define   CLK_PLL	  0x80	  /* Enable internal PLL */
+#define   CLK_EXT	  0x40	  /* Use external clock directly */
+#define   CLK_SCALE	  0x3f	  /* Mask for internal clock scale */
+#define REG_COM7	0x12	/* Control 7 */
+#define   COM7_RESET	  0x80	  /* Register reset */
+#define   COM7_FMT_MASK	  0x38
+#define   COM7_FMT_SXGA	  0x00
+#define   COM7_FMT_VGA	  0x40
+#define	  COM7_FMT_CIF	  0x20	  /* CIF format */
+#define   COM7_FMT_QVGA	  0x10	  /* QVGA format */
+#define   COM7_FMT_QCIF	  0x08	  /* QCIF format */
+#define	  COM7_RGB	  0x04	  /* bits 0 and 2 - RGB format */
+#define	  COM7_YUV	  0x00	  /* YUV */
+#define	  COM7_BAYER	  0x01	  /* Bayer format */
+#define	  COM7_PBAYER	  0x05	  /* "Processed bayer" */
+#define REG_COM8	0x13	/* Control 8 */
+#define   COM8_FASTAEC	  0x80	  /* Enable fast AGC/AEC */
+#define   COM8_AECSTEP	  0x40	  /* Unlimited AEC step size */
+#define   COM8_BFILT	  0x20	  /* Band filter enable */
+#define   COM8_AGC	  0x04	  /* Auto gain enable */
+#define   COM8_AWB	  0x02	  /* White balance enable */
+#define   COM8_AEC	  0x01	  /* Auto exposure enable */
+#define REG_COM9	0x14	/* Control 9  - gain ceiling */
+#define REG_COM10	0x15	/* Control 10 */
+#define   COM10_HSYNC	  0x40	  /* HSYNC instead of HREF */
+#define   COM10_PCLK_HB	  0x20	  /* Suppress PCLK on horiz blank */
+#define   COM10_HREF_REV  0x08	  /* Reverse HREF */
+#define   COM10_VS_LEAD	  0x04	  /* VSYNC on clock leading edge */
+#define   COM10_VS_NEG	  0x02	  /* VSYNC negative */
+#define   COM10_HS_NEG	  0x01	  /* HSYNC negative */
+#define REG_HSTART	0x17	/* Horiz start high bits */
+#define REG_HSTOP	0x18	/* Horiz stop high bits */
+#define REG_VSTART	0x19	/* Vert start high bits */
+#define REG_VSTOP	0x1a	/* Vert stop high bits */
+#define REG_PSHFT	0x1b	/* Pixel delay after HREF */
+#define REG_MIDH	0x1c	/* Manuf. ID high */
+#define REG_MIDL	0x1d	/* Manuf. ID low */
+#define REG_MVFP	0x1e	/* Mirror / vflip */
+#define   MVFP_MIRROR	  0x20	  /* Mirror image */
+#define   MVFP_FLIP	  0x10	  /* Vertical flip */
+
+#define REG_AEW		0x24	/* AGC upper limit */
+#define REG_AEB		0x25	/* AGC lower limit */
+#define REG_VPT		0x26	/* AGC/AEC fast mode op region */
+#define REG_ADVFL	0x2d	/* Insert dummy lines (LSB) */
+#define REG_ADVFH	0x2e	/* Insert dummy lines (MSB) */
+#define REG_HSYST	0x30	/* HSYNC rising edge delay */
+#define REG_HSYEN	0x31	/* HSYNC falling edge delay */
+#define REG_HREF	0x32	/* HREF pieces */
+#define REG_TSLB	0x3a	/* lots of stuff */
+#define   TSLB_YLAST	  0x04	  /* UYVY or VYUY - see com13 */
+#define   TSLB_BYTEORD	  0x08	  /* swap bytes in 16bit mode? */
+#define REG_COM11	0x3b	/* Control 11 */
+#define   COM11_NIGHT	  0x80	  /* NIght mode enable */
+#define   COM11_NMFR	  0x60	  /* Two bit NM frame rate */
+#define   COM11_HZAUTO	  0x10	  /* Auto detect 50/60 Hz */
+#define	  COM11_50HZ	  0x08	  /* Manual 50Hz select */
+#define   COM11_EXP	  0x02
+#define REG_COM12	0x3c	/* Control 12 */
+#define   COM12_HREF	  0x80	  /* HREF always */
+#define REG_COM13	0x3d	/* Control 13 */
+#define   COM13_GAMMA	  0x80	  /* Gamma enable */
+#define	  COM13_UVSAT	  0x40	  /* UV saturation auto adjustment */
+#define	  COM13_CMATRIX	  0x10	  /* Enable color matrix for RGB or YUV */
+#define   COM13_UVSWAP	  0x01	  /* V before U - w/TSLB */
+#define REG_COM14	0x3e	/* Control 14 */
+#define   COM14_DCWEN	  0x10	  /* DCW/PCLK-scale enable */
+#define REG_EDGE	0x3f	/* Edge enhancement factor */
+#define REG_COM15	0x40	/* Control 15 */
+#define   COM15_R10F0	  0x00	  /* Data range 10 to F0 */
+#define	  COM15_R01FE	  0x80	  /*            01 to FE */
+#define   COM15_R00FF	  0xc0	  /*            00 to FF */
+#define   COM15_RGB565	  0x10	  /* RGB565 output */
+#define   COM15_RGBFIXME	  0x20	  /* FIXME  */
+#define   COM15_RGB555	  0x30	  /* RGB555 output */
+#define REG_COM16	0x41	/* Control 16 */
+#define   COM16_AWBGAIN   0x08	  /* AWB gain enable */
+#define REG_COM17	0x42	/* Control 17 */
+#define   COM17_AECWIN	  0xc0	  /* AEC window - must match COM4 */
+#define   COM17_CBAR	  0x08	  /* DSP Color bar */
+
+/*
+ * This matrix defines how the colors are generated, must be
+ * tweaked to adjust hue and saturation.
+ *
+ * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
+ *
+ * They are nine-bit signed quantities, with the sign bit
+ * stored in 0x58.  Sign for v-red is bit 0, and up from there.
+ */
+#define	REG_CMATRIX_BASE 0x4f
+#define   CMATRIX_LEN 6
+#define REG_CMATRIX_SIGN 0x58
+
+
+#define REG_BRIGHT	0x55	/* Brightness */
+#define REG_CONTRAS	0x56	/* Contrast control */
+
+#define REG_GFIX	0x69	/* Fix gain control */
+
+#define REG_RGB444	0x8c	/* RGB 444 control */
+#define   R444_ENABLE	  0x02	  /* Turn on RGB444, overrides 5x5 */
+#define   R444_RGBX	  0x01	  /* Empty nibble at end */
+
+#define REG_HAECC1	0x9f	/* Hist AEC/AGC control 1 */
+#define REG_HAECC2	0xa0	/* Hist AEC/AGC control 2 */
+
+#define REG_BD50MAX	0xa5	/* 50hz banding step limit */
+#define REG_HAECC3	0xa6	/* Hist AEC/AGC control 3 */
+#define REG_HAECC4	0xa7	/* Hist AEC/AGC control 4 */
+#define REG_HAECC5	0xa8	/* Hist AEC/AGC control 5 */
+#define REG_HAECC6	0xa9	/* Hist AEC/AGC control 6 */
+#define REG_HAECC7	0xaa	/* Hist AEC/AGC control 7 */
+#define REG_BD60MAX	0xab	/* 60hz banding step limit */
+
+
+
+
+/* Returns 0 if OK */
+static int stk_sensor_outb(struct stk_camera *dev, u8 reg, u8 val)
+{
+	int i = 0;
+	u8 tmpval = 0;
+
+	if (stk_camera_write_reg(dev, STK_IIC_TX_INDEX, reg))
+		return 1;
+	if (stk_camera_write_reg(dev, STK_IIC_TX_VALUE, val))
+		return 1;
+	if (stk_camera_write_reg(dev, STK_IIC_OP, STK_IIC_OP_TX))
+		return 1;
+	do {
+		if (stk_camera_read_reg(dev, STK_IIC_STAT, &tmpval))
+			return 1;
+		i++;
+	} while (tmpval == 0 && i < MAX_RETRIES);
+	if (tmpval != STK_IIC_STAT_TX_OK) {
+		if (tmpval)
+			pr_err("stk_sensor_outb failed, status=0x%02x\n",
+			       tmpval);
+		return 1;
+	} else
+		return 0;
+}
+
+static int stk_sensor_inb(struct stk_camera *dev, u8 reg, u8 *val)
+{
+	int i = 0;
+	u8 tmpval = 0;
+
+	if (stk_camera_write_reg(dev, STK_IIC_RX_INDEX, reg))
+		return 1;
+	if (stk_camera_write_reg(dev, STK_IIC_OP, STK_IIC_OP_RX))
+		return 1;
+	do {
+		if (stk_camera_read_reg(dev, STK_IIC_STAT, &tmpval))
+			return 1;
+		i++;
+	} while (tmpval == 0 && i < MAX_RETRIES);
+	if (tmpval != STK_IIC_STAT_RX_OK) {
+		if (tmpval)
+			pr_err("stk_sensor_inb failed, status=0x%02x\n",
+			       tmpval);
+		return 1;
+	}
+
+	if (stk_camera_read_reg(dev, STK_IIC_RX_VALUE, &tmpval))
+		return 1;
+
+	*val = tmpval;
+	return 0;
+}
+
+static int stk_sensor_write_regvals(struct stk_camera *dev,
+		struct regval *rv)
+{
+	int ret;
+	if (rv == NULL)
+		return 0;
+	while (rv->reg != 0xff || rv->val != 0xff) {
+		ret = stk_sensor_outb(dev, rv->reg, rv->val);
+		if (ret != 0)
+			return ret;
+		rv++;
+	}
+	return 0;
+}
+
+int stk_sensor_sleep(struct stk_camera *dev)
+{
+	u8 tmp;
+	return stk_sensor_inb(dev, REG_COM2, &tmp)
+		|| stk_sensor_outb(dev, REG_COM2, tmp|COM2_SSLEEP);
+}
+
+int stk_sensor_wakeup(struct stk_camera *dev)
+{
+	u8 tmp;
+	return stk_sensor_inb(dev, REG_COM2, &tmp)
+		|| stk_sensor_outb(dev, REG_COM2, tmp&~COM2_SSLEEP);
+}
+
+static struct regval ov_initvals[] = {
+	{REG_CLKRC, CLK_PLL},
+	{REG_COM11, 0x01},
+	{0x6a, 0x7d},
+	{REG_AECH, 0x40},
+	{REG_GAIN, 0x00},
+	{REG_BLUE, 0x80},
+	{REG_RED, 0x80},
+	/* Do not enable fast AEC for now */
+	/*{REG_COM8, COM8_FASTAEC|COM8_AECSTEP|COM8_BFILT|COM8_AGC|COM8_AEC},*/
+	{REG_COM8, COM8_AECSTEP|COM8_BFILT|COM8_AGC|COM8_AEC},
+	{0x39, 0x50}, {0x38, 0x93},
+	{0x37, 0x00}, {0x35, 0x81},
+	{REG_COM5, 0x20},
+	{REG_COM1, 0x00},
+	{REG_COM3, 0x00},
+	{REG_COM4, 0x00},
+	{REG_PSHFT, 0x00},
+	{0x16, 0x07},
+	{0x33, 0xe2}, {0x34, 0xbf},
+	{REG_COM16, 0x00},
+	{0x96, 0x04},
+	/* Gamma curve values */
+/*	{ 0x7a, 0x20 },		{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },		{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },		{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },		{ 0x81, 0x80 },
+	{ 0x82, 0x88 },		{ 0x83, 0x8f },
+	{ 0x84, 0x96 },		{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },		{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },		{ 0x89, 0xe8 },
+*/
+	{REG_GFIX, 0x40},
+	{0x8e, 0x00},
+	{REG_COM12, 0x73},
+	{0x8f, 0xdf}, {0x8b, 0x06},
+	{0x8c, 0x20},
+	{0x94, 0x88}, {0x95, 0x88},
+/*	{REG_COM15, 0xc1}, TODO */
+	{0x29, 0x3f},
+	{REG_COM6, 0x42},
+	{REG_BD50MAX, 0x80},
+	{REG_HAECC6, 0xb8}, {REG_HAECC7, 0x92},
+	{REG_BD60MAX, 0x0a},
+	{0x90, 0x00}, {0x91, 0x00},
+	{REG_HAECC1, 0x00}, {REG_HAECC2, 0x00},
+	{REG_AEW, 0x68}, {REG_AEB, 0x5c},
+	{REG_VPT, 0xc3},
+	{REG_COM9, 0x2e},
+	{0x2a, 0x00}, {0x2b, 0x00},
+
+	{0xff, 0xff}, /* END MARKER */
+};
+
+/* Probe the I2C bus and initialise the sensor chip */
+int stk_sensor_init(struct stk_camera *dev)
+{
+	u8 idl = 0;
+	u8 idh = 0;
+
+	if (stk_camera_write_reg(dev, STK_IIC_ENABLE, STK_IIC_ENABLE_YES)
+		|| stk_camera_write_reg(dev, STK_IIC_ADDR, SENSOR_ADDRESS)
+		|| stk_sensor_outb(dev, REG_COM7, COM7_RESET)) {
+		pr_err("Sensor resetting failed\n");
+		return -ENODEV;
+	}
+	msleep(10);
+	/* Read the manufacturer ID: ov = 0x7FA2 */
+	if (stk_sensor_inb(dev, REG_MIDH, &idh)
+	    || stk_sensor_inb(dev, REG_MIDL, &idl)) {
+		pr_err("Strange error reading sensor ID\n");
+		return -ENODEV;
+	}
+	if (idh != 0x7f || idl != 0xa2) {
+		pr_err("Huh? you don't have a sensor from ovt\n");
+		return -ENODEV;
+	}
+	if (stk_sensor_inb(dev, REG_PID, &idh)
+	    || stk_sensor_inb(dev, REG_VER, &idl)) {
+		pr_err("Could not read sensor model\n");
+		return -ENODEV;
+	}
+	stk_sensor_write_regvals(dev, ov_initvals);
+	msleep(10);
+	pr_info("OmniVision sensor detected, id %02X%02X at address %x\n",
+		idh, idl, SENSOR_ADDRESS);
+	return 0;
+}
+
+/* V4L2_PIX_FMT_UYVY */
+static struct regval ov_fmt_uyvy[] = {
+	{REG_TSLB, TSLB_YLAST|0x08 },
+	{ 0x4f, 0x80 },		/* "matrix coefficient 1" */
+	{ 0x50, 0x80 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x22 },		/* "matrix coefficient 4" */
+	{ 0x53, 0x5e },		/* "matrix coefficient 5" */
+	{ 0x54, 0x80 },		/* "matrix coefficient 6" */
+	{REG_COM13, COM13_UVSAT|COM13_CMATRIX},
+	{REG_COM15, COM15_R00FF },
+	{0xff, 0xff}, /* END MARKER */
+};
+/* V4L2_PIX_FMT_YUYV */
+static struct regval ov_fmt_yuyv[] = {
+	{REG_TSLB, 0 },
+	{ 0x4f, 0x80 },		/* "matrix coefficient 1" */
+	{ 0x50, 0x80 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x22 },		/* "matrix coefficient 4" */
+	{ 0x53, 0x5e },		/* "matrix coefficient 5" */
+	{ 0x54, 0x80 },		/* "matrix coefficient 6" */
+	{REG_COM13, COM13_UVSAT|COM13_CMATRIX},
+	{REG_COM15, COM15_R00FF },
+	{0xff, 0xff}, /* END MARKER */
+};
+
+/* V4L2_PIX_FMT_RGB565X rrrrrggg gggbbbbb */
+static struct regval ov_fmt_rgbr[] = {
+	{ REG_RGB444, 0 },	/* No RGB444 please */
+	{REG_TSLB, 0x00},
+	{ REG_COM1, 0x0 },
+	{ REG_COM9, 0x38 },	/* 16x gain ceiling; 0x8 is reserved bit */
+	{ 0x4f, 0xb3 },		/* "matrix coefficient 1" */
+	{ 0x50, 0xb3 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x3d },		/* "matrix coefficient 4" */
+	{ 0x53, 0xa7 },		/* "matrix coefficient 5" */
+	{ 0x54, 0xe4 },		/* "matrix coefficient 6" */
+	{ REG_COM13, COM13_GAMMA },
+	{ REG_COM15, COM15_RGB565|COM15_R00FF },
+	{ 0xff, 0xff },
+};
+
+/* V4L2_PIX_FMT_RGB565 gggbbbbb rrrrrggg */
+static struct regval ov_fmt_rgbp[] = {
+	{ REG_RGB444, 0 },	/* No RGB444 please */
+	{REG_TSLB, TSLB_BYTEORD },
+	{ REG_COM1, 0x0 },
+	{ REG_COM9, 0x38 },	/* 16x gain ceiling; 0x8 is reserved bit */
+	{ 0x4f, 0xb3 },		/* "matrix coefficient 1" */
+	{ 0x50, 0xb3 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x3d },		/* "matrix coefficient 4" */
+	{ 0x53, 0xa7 },		/* "matrix coefficient 5" */
+	{ 0x54, 0xe4 },		/* "matrix coefficient 6" */
+	{ REG_COM13, COM13_GAMMA },
+	{ REG_COM15, COM15_RGB565|COM15_R00FF },
+	{ 0xff, 0xff },
+};
+
+/* V4L2_PIX_FMT_SRGGB8 */
+static struct regval ov_fmt_bayer[] = {
+	/* This changes color order */
+	{REG_TSLB, 0x40}, /* BGGR */
+	/* {REG_TSLB, 0x08}, */ /* BGGR with vertical image flipping */
+	{REG_COM15, COM15_R00FF },
+	{0xff, 0xff}, /* END MARKER */
+};
+/*
+ * Store a set of start/stop values into the camera.
+ */
+static int stk_sensor_set_hw(struct stk_camera *dev,
+		int hstart, int hstop, int vstart, int vstop)
+{
+	int ret;
+	unsigned char v;
+/*
+ * Horizontal: 11 bits, top 8 live in hstart and hstop.  Bottom 3 of
+ * hstart are in href[2:0], bottom 3 of hstop in href[5:3].  There is
+ * a mystery "edge offset" value in the top two bits of href.
+ */
+	ret =  stk_sensor_outb(dev, REG_HSTART, (hstart >> 3) & 0xff);
+	ret += stk_sensor_outb(dev, REG_HSTOP, (hstop >> 3) & 0xff);
+	ret += stk_sensor_inb(dev, REG_HREF, &v);
+	v = (v & 0xc0) | ((hstop & 0x7) << 3) | (hstart & 0x7);
+	msleep(10);
+	ret += stk_sensor_outb(dev, REG_HREF, v);
+/*
+ * Vertical: similar arrangement (note: this is different from ov7670.c)
+ */
+	ret += stk_sensor_outb(dev, REG_VSTART, (vstart >> 3) & 0xff);
+	ret += stk_sensor_outb(dev, REG_VSTOP, (vstop >> 3) & 0xff);
+	ret += stk_sensor_inb(dev, REG_VREF, &v);
+	v = (v & 0xc0) | ((vstop & 0x7) << 3) | (vstart & 0x7);
+	msleep(10);
+	ret += stk_sensor_outb(dev, REG_VREF, v);
+	return ret;
+}
+
+
+int stk_sensor_configure(struct stk_camera *dev)
+{
+	int com7;
+	/*
+	 * We setup the sensor to output dummy lines in low-res modes,
+	 * so we don't get absurdly hight framerates.
+	 */
+	unsigned dummylines;
+	int flip;
+	struct regval *rv;
+
+	switch (dev->vsettings.mode) {
+	case MODE_QCIF: com7 = COM7_FMT_QCIF;
+		dummylines = 604;
+		break;
+	case MODE_QVGA: com7 = COM7_FMT_QVGA;
+		dummylines = 267;
+		break;
+	case MODE_CIF: com7 = COM7_FMT_CIF;
+		dummylines = 412;
+		break;
+	case MODE_VGA: com7 = COM7_FMT_VGA;
+		dummylines = 11;
+		break;
+	case MODE_SXGA: com7 = COM7_FMT_SXGA;
+		dummylines = 0;
+		break;
+	default:
+		pr_err("Unsupported mode %d\n", dev->vsettings.mode);
+		return -EFAULT;
+	}
+	switch (dev->vsettings.palette) {
+	case V4L2_PIX_FMT_UYVY:
+		com7 |= COM7_YUV;
+		rv = ov_fmt_uyvy;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		com7 |= COM7_YUV;
+		rv = ov_fmt_yuyv;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		com7 |= COM7_RGB;
+		rv = ov_fmt_rgbp;
+		break;
+	case V4L2_PIX_FMT_RGB565X:
+		com7 |= COM7_RGB;
+		rv = ov_fmt_rgbr;
+		break;
+	case V4L2_PIX_FMT_SBGGR8:
+		com7 |= COM7_PBAYER;
+		rv = ov_fmt_bayer;
+		break;
+	default:
+		pr_err("Unsupported colorspace\n");
+		return -EFAULT;
+	}
+	/*FIXME sometimes the sensor go to a bad state
+	stk_sensor_write_regvals(dev, ov_initvals); */
+	stk_sensor_outb(dev, REG_COM7, com7);
+	msleep(50);
+	stk_sensor_write_regvals(dev, rv);
+	flip = (dev->vsettings.vflip?MVFP_FLIP:0)
+		| (dev->vsettings.hflip?MVFP_MIRROR:0);
+	stk_sensor_outb(dev, REG_MVFP, flip);
+	if (dev->vsettings.palette == V4L2_PIX_FMT_SBGGR8
+			&& !dev->vsettings.vflip)
+		stk_sensor_outb(dev, REG_TSLB, 0x08);
+	stk_sensor_outb(dev, REG_ADVFH, dummylines >> 8);
+	stk_sensor_outb(dev, REG_ADVFL, dummylines & 0xff);
+	msleep(50);
+	switch (dev->vsettings.mode) {
+	case MODE_VGA:
+		if (stk_sensor_set_hw(dev, 302, 1582, 6, 486))
+			pr_err("stk_sensor_set_hw failed (VGA)\n");
+		break;
+	case MODE_SXGA:
+	case MODE_CIF:
+	case MODE_QVGA:
+	case MODE_QCIF:
+		/*FIXME These settings seem ignored by the sensor
+		if (stk_sensor_set_hw(dev, 220, 1500, 10, 1034))
+			pr_err("stk_sensor_set_hw failed (SXGA)\n");
+		*/
+		break;
+	}
+	msleep(10);
+	return 0;
+}
+
+int stk_sensor_set_brightness(struct stk_camera *dev, int br)
+{
+	if (br < 0 || br > 0xff)
+		return -EINVAL;
+	stk_sensor_outb(dev, REG_AEB, max(0x00, br - 6));
+	stk_sensor_outb(dev, REG_AEW, min(0xff, br + 6));
+	return 0;
+}
+
diff --git a/drivers/staging/media/deprecated/stkwebcam/stk-webcam.c b/drivers/staging/media/deprecated/stkwebcam/stk-webcam.c
new file mode 100644
index 000000000..787edb3d4
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/stk-webcam.c
@@ -0,0 +1,1434 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * stk-webcam.c : Driver for Syntek 1125 USB webcam controller
+ *
+ * Copyright (C) 2006 Nicolas VIVIEN
+ * Copyright 2007-2008 Jaime Velasco Juan <jsagarribay@gmail.com>
+ *
+ * Some parts are inspired from cafe_ccic.c
+ * Copyright 2006-2007 Jonathan Corbet
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+
+#include <linux/dmi.h>
+#include <linux/usb.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+
+#include "stk-webcam.h"
+
+
+static int hflip = -1;
+module_param(hflip, int, 0444);
+MODULE_PARM_DESC(hflip, "Horizontal image flip (mirror). Defaults to 0");
+
+static int vflip = -1;
+module_param(vflip, int, 0444);
+MODULE_PARM_DESC(vflip, "Vertical image flip. Defaults to 0");
+
+static int debug;
+module_param(debug, int, 0444);
+MODULE_PARM_DESC(debug, "Debug v4l ioctls. Defaults to 0");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jaime Velasco Juan <jsagarribay@gmail.com> and Nicolas VIVIEN");
+MODULE_DESCRIPTION("Syntek DC1125 webcam driver");
+
+/* Some cameras have audio interfaces, we aren't interested in those */
+static const struct usb_device_id stkwebcam_table[] = {
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x174f, 0xa311, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x05e1, 0x0501, 0xff, 0xff, 0xff) },
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, stkwebcam_table);
+
+/*
+ * The stk webcam laptop module is mounted upside down in some laptops :(
+ *
+ * Some background information (thanks to Hans de Goede for providing this):
+ *
+ * 1) Once upon a time the stkwebcam driver was written
+ *
+ * 2) The webcam in question was used mostly in Asus laptop models, including
+ * the laptop of the original author of the driver, and in these models, in
+ * typical Asus fashion (see the long long list for uvc cams inside v4l-utils),
+ * they mounted the webcam-module the wrong way up. So the hflip and vflip
+ * module options were given a default value of 1 (the correct value for
+ * upside down mounted models)
+ *
+ * 3) Years later I got a bug report from a user with a laptop with stkwebcam,
+ * where the module was actually mounted the right way up, and thus showed
+ * upside down under Linux. So now I was facing the choice of 2 options:
+ *
+ * a) Add a not-upside-down list to stkwebcam, which overrules the default.
+ *
+ * b) Do it like all the other drivers do, and make the default right for
+ *    cams mounted the proper way and add an upside-down model list, with
+ *    models where we need to flip-by-default.
+ *
+ * Despite knowing that going b) would cause a period of pain where we were
+ * building the table I opted to go for option b), since a) is just too ugly,
+ * and worse different from how every other driver does it leading to
+ * confusion in the long run. This change was made in kernel 3.6.
+ *
+ * So for any user report about upside-down images since kernel 3.6 ask them
+ * to provide the output of 'sudo dmidecode' so the laptop can be added in
+ * the table below.
+ */
+static const struct dmi_system_id stk_upside_down_dmi_table[] = {
+	{
+		.ident = "ASUS G1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "G1")
+		}
+	}, {
+		.ident = "ASUS F3JC",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "F3JC")
+		}
+	},
+	{
+		.ident = "T12Rg-H",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "HCL Infosystems Limited"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "T12Rg-H")
+		}
+	},
+	{
+		.ident = "ASUS A6VM",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "A6VM")
+		}
+	},
+	{
+		.ident = "ASUS A6JC",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "A6JC")
+		}
+	},
+	{}
+};
+
+
+/*
+ * Basic stuff
+ */
+int stk_camera_write_reg(struct stk_camera *dev, u16 index, u8 value)
+{
+	struct usb_device *udev = dev->udev;
+	int ret;
+
+	ret =  usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			0x01,
+			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			value,
+			index,
+			NULL,
+			0,
+			500);
+	if (ret < 0)
+		return ret;
+	else
+		return 0;
+}
+
+int stk_camera_read_reg(struct stk_camera *dev, u16 index, u8 *value)
+{
+	struct usb_device *udev = dev->udev;
+	int ret;
+
+	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+			0x00,
+			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			0x00,
+			index,
+			&dev->read_reg_scratch,
+			sizeof(u8),
+			500);
+	if (ret >= 0)
+		*value = dev->read_reg_scratch;
+
+	if (ret < 0)
+		return ret;
+	else
+		return 0;
+}
+
+static int stk_start_stream(struct stk_camera *dev)
+{
+	u8 value;
+	int i, ret;
+	u8 value_116, value_117;
+
+
+	if (!is_present(dev))
+		return -ENODEV;
+	if (!is_memallocd(dev) || !is_initialised(dev)) {
+		pr_err("FIXME: Buffers are not allocated\n");
+		return -EFAULT;
+	}
+	ret = usb_set_interface(dev->udev, 0, 5);
+
+	if (ret < 0)
+		pr_err("usb_set_interface failed !\n");
+	if (stk_sensor_wakeup(dev))
+		pr_err("error awaking the sensor\n");
+
+	stk_camera_read_reg(dev, 0x0116, &value_116);
+	stk_camera_read_reg(dev, 0x0117, &value_117);
+
+	stk_camera_write_reg(dev, 0x0116, 0x0000);
+	stk_camera_write_reg(dev, 0x0117, 0x0000);
+
+	stk_camera_read_reg(dev, 0x0100, &value);
+	stk_camera_write_reg(dev, 0x0100, value | 0x80);
+
+	stk_camera_write_reg(dev, 0x0116, value_116);
+	stk_camera_write_reg(dev, 0x0117, value_117);
+	for (i = 0; i < MAX_ISO_BUFS; i++) {
+		if (dev->isobufs[i].urb) {
+			ret = usb_submit_urb(dev->isobufs[i].urb, GFP_KERNEL);
+			atomic_inc(&dev->urbs_used);
+			if (ret)
+				return ret;
+		}
+	}
+	set_streaming(dev);
+	return 0;
+}
+
+static int stk_stop_stream(struct stk_camera *dev)
+{
+	u8 value;
+	int i;
+	if (is_present(dev)) {
+		stk_camera_read_reg(dev, 0x0100, &value);
+		stk_camera_write_reg(dev, 0x0100, value & ~0x80);
+		if (dev->isobufs != NULL) {
+			for (i = 0; i < MAX_ISO_BUFS; i++) {
+				if (dev->isobufs[i].urb)
+					usb_kill_urb(dev->isobufs[i].urb);
+			}
+		}
+		unset_streaming(dev);
+
+		if (usb_set_interface(dev->udev, 0, 0))
+			pr_err("usb_set_interface failed !\n");
+		if (stk_sensor_sleep(dev))
+			pr_err("error suspending the sensor\n");
+	}
+	return 0;
+}
+
+/*
+ * This seems to be the shortest init sequence we
+ * must do in order to find the sensor
+ * Bit 5 of reg. 0x0000 here is important, when reset to 0 the sensor
+ * is also reset. Maybe powers down it?
+ * Rest of values don't make a difference
+ */
+
+static struct regval stk1125_initvals[] = {
+	/*TODO: What means this sequence? */
+	{0x0000, 0x24},
+	{0x0100, 0x21},
+	{0x0002, 0x68},
+	{0x0003, 0x80},
+	{0x0005, 0x00},
+	{0x0007, 0x03},
+	{0x000d, 0x00},
+	{0x000f, 0x02},
+	{0x0300, 0x12},
+	{0x0350, 0x41},
+	{0x0351, 0x00},
+	{0x0352, 0x00},
+	{0x0353, 0x00},
+	{0x0018, 0x10},
+	{0x0019, 0x00},
+	{0x001b, 0x0e},
+	{0x001c, 0x46},
+	{0x0300, 0x80},
+	{0x001a, 0x04},
+	{0x0110, 0x00},
+	{0x0111, 0x00},
+	{0x0112, 0x00},
+	{0x0113, 0x00},
+
+	{0xffff, 0xff},
+};
+
+
+static int stk_initialise(struct stk_camera *dev)
+{
+	struct regval *rv;
+	int ret;
+	if (!is_present(dev))
+		return -ENODEV;
+	if (is_initialised(dev))
+		return 0;
+	rv = stk1125_initvals;
+	while (rv->reg != 0xffff) {
+		ret = stk_camera_write_reg(dev, rv->reg, rv->val);
+		if (ret)
+			return ret;
+		rv++;
+	}
+	if (stk_sensor_init(dev) == 0) {
+		set_initialised(dev);
+		return 0;
+	} else
+		return -1;
+}
+
+/* *********************************************** */
+/*
+ * This function is called as an URB transfert is complete (Isochronous pipe).
+ * So, the traitement is done in interrupt time, so it has be fast, not crash,
+ * and not stall. Neat.
+ */
+static void stk_isoc_handler(struct urb *urb)
+{
+	int i;
+	int ret;
+	int framelen;
+	unsigned long flags;
+
+	unsigned char *fill = NULL;
+	unsigned char *iso_buf = NULL;
+
+	struct stk_camera *dev;
+	struct stk_sio_buffer *fb;
+
+	dev = (struct stk_camera *) urb->context;
+
+	if (dev == NULL) {
+		pr_err("isoc_handler called with NULL device !\n");
+		return;
+	}
+
+	if (urb->status == -ENOENT || urb->status == -ECONNRESET
+		|| urb->status == -ESHUTDOWN) {
+		atomic_dec(&dev->urbs_used);
+		return;
+	}
+
+	spin_lock_irqsave(&dev->spinlock, flags);
+
+	if (urb->status != -EINPROGRESS && urb->status != 0) {
+		pr_err("isoc_handler: urb->status == %d\n", urb->status);
+		goto resubmit;
+	}
+
+	if (list_empty(&dev->sio_avail)) {
+		/*FIXME Stop streaming after a while */
+		pr_err_ratelimited("isoc_handler without available buffer!\n");
+		goto resubmit;
+	}
+	fb = list_first_entry(&dev->sio_avail,
+			struct stk_sio_buffer, list);
+	fill = fb->buffer + fb->v4lbuf.bytesused;
+
+	for (i = 0; i < urb->number_of_packets; i++) {
+		if (urb->iso_frame_desc[i].status != 0) {
+			if (urb->iso_frame_desc[i].status != -EXDEV)
+				pr_err("Frame %d has error %d\n",
+				       i, urb->iso_frame_desc[i].status);
+			continue;
+		}
+		framelen = urb->iso_frame_desc[i].actual_length;
+		iso_buf = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+
+		if (framelen <= 4)
+			continue; /* no data */
+
+		/*
+		 * we found something informational from there
+		 * the isoc frames have to type of headers
+		 * type1: 00 xx 00 00 or 20 xx 00 00
+		 * type2: 80 xx 00 00 00 00 00 00 or a0 xx 00 00 00 00 00 00
+		 * xx is a sequencer which has never been seen over 0x3f
+		 * imho data written down looks like bayer, i see similarities
+		 * after every 640 bytes
+		 */
+		if (*iso_buf & 0x80) {
+			framelen -= 8;
+			iso_buf += 8;
+			/* This marks a new frame */
+			if (fb->v4lbuf.bytesused != 0
+				&& fb->v4lbuf.bytesused != dev->frame_size) {
+				pr_err_ratelimited("frame %d, bytesused=%d, skipping\n",
+						   i, fb->v4lbuf.bytesused);
+				fb->v4lbuf.bytesused = 0;
+				fill = fb->buffer;
+			} else if (fb->v4lbuf.bytesused == dev->frame_size) {
+				if (list_is_singular(&dev->sio_avail)) {
+					/* Always reuse the last buffer */
+					fb->v4lbuf.bytesused = 0;
+					fill = fb->buffer;
+				} else {
+					list_move_tail(dev->sio_avail.next,
+						&dev->sio_full);
+					wake_up(&dev->wait_frame);
+					fb = list_first_entry(&dev->sio_avail,
+						struct stk_sio_buffer, list);
+					fb->v4lbuf.bytesused = 0;
+					fill = fb->buffer;
+				}
+			}
+		} else {
+			framelen -= 4;
+			iso_buf += 4;
+		}
+
+		/* Our buffer is full !!! */
+		if (framelen + fb->v4lbuf.bytesused > dev->frame_size) {
+			pr_err_ratelimited("Frame buffer overflow, lost sync\n");
+			/*FIXME Do something here? */
+			continue;
+		}
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+		memcpy(fill, iso_buf, framelen);
+		spin_lock_irqsave(&dev->spinlock, flags);
+		fill += framelen;
+
+		/* New size of our buffer */
+		fb->v4lbuf.bytesused += framelen;
+	}
+
+resubmit:
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+	urb->dev = dev->udev;
+	ret = usb_submit_urb(urb, GFP_ATOMIC);
+	if (ret != 0) {
+		pr_err("Error (%d) re-submitting urb in stk_isoc_handler\n",
+		       ret);
+	}
+}
+
+/* -------------------------------------------- */
+
+static int stk_prepare_iso(struct stk_camera *dev)
+{
+	void *kbuf;
+	int i, j;
+	struct urb *urb;
+	struct usb_device *udev;
+
+	if (dev == NULL)
+		return -ENXIO;
+	udev = dev->udev;
+
+	if (dev->isobufs)
+		pr_err("isobufs already allocated. Bad\n");
+	else
+		dev->isobufs = kcalloc(MAX_ISO_BUFS, sizeof(*dev->isobufs),
+				       GFP_KERNEL);
+	if (dev->isobufs == NULL) {
+		pr_err("Unable to allocate iso buffers\n");
+		return -ENOMEM;
+	}
+	for (i = 0; i < MAX_ISO_BUFS; i++) {
+		if (dev->isobufs[i].data == NULL) {
+			kbuf = kzalloc(ISO_BUFFER_SIZE, GFP_KERNEL);
+			if (kbuf == NULL) {
+				pr_err("Failed to allocate iso buffer %d\n", i);
+				goto isobufs_out;
+			}
+			dev->isobufs[i].data = kbuf;
+		} else
+			pr_err("isobuf data already allocated\n");
+		if (dev->isobufs[i].urb == NULL) {
+			urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
+			if (urb == NULL)
+				goto isobufs_out;
+			dev->isobufs[i].urb = urb;
+		} else {
+			pr_err("Killing URB\n");
+			usb_kill_urb(dev->isobufs[i].urb);
+			urb = dev->isobufs[i].urb;
+		}
+		urb->interval = 1;
+		urb->dev = udev;
+		urb->pipe = usb_rcvisocpipe(udev, dev->isoc_ep);
+		urb->transfer_flags = URB_ISO_ASAP;
+		urb->transfer_buffer = dev->isobufs[i].data;
+		urb->transfer_buffer_length = ISO_BUFFER_SIZE;
+		urb->complete = stk_isoc_handler;
+		urb->context = dev;
+		urb->start_frame = 0;
+		urb->number_of_packets = ISO_FRAMES_PER_DESC;
+
+		for (j = 0; j < ISO_FRAMES_PER_DESC; j++) {
+			urb->iso_frame_desc[j].offset = j * ISO_MAX_FRAME_SIZE;
+			urb->iso_frame_desc[j].length = ISO_MAX_FRAME_SIZE;
+		}
+	}
+	set_memallocd(dev);
+	return 0;
+
+isobufs_out:
+	for (i = 0; i < MAX_ISO_BUFS && dev->isobufs[i].data; i++)
+		kfree(dev->isobufs[i].data);
+	for (i = 0; i < MAX_ISO_BUFS && dev->isobufs[i].urb; i++)
+		usb_free_urb(dev->isobufs[i].urb);
+	kfree(dev->isobufs);
+	dev->isobufs = NULL;
+	return -ENOMEM;
+}
+
+static void stk_clean_iso(struct stk_camera *dev)
+{
+	int i;
+
+	if (dev == NULL || dev->isobufs == NULL)
+		return;
+
+	for (i = 0; i < MAX_ISO_BUFS; i++) {
+		struct urb *urb;
+
+		urb = dev->isobufs[i].urb;
+		if (urb) {
+			if (atomic_read(&dev->urbs_used) && is_present(dev))
+				usb_kill_urb(urb);
+			usb_free_urb(urb);
+		}
+		kfree(dev->isobufs[i].data);
+	}
+	kfree(dev->isobufs);
+	dev->isobufs = NULL;
+	unset_memallocd(dev);
+}
+
+static int stk_setup_siobuf(struct stk_camera *dev, int index)
+{
+	struct stk_sio_buffer *buf = dev->sio_bufs + index;
+	INIT_LIST_HEAD(&buf->list);
+	buf->v4lbuf.length = PAGE_ALIGN(dev->frame_size);
+	buf->buffer = vmalloc_user(buf->v4lbuf.length);
+	if (buf->buffer == NULL)
+		return -ENOMEM;
+	buf->mapcount = 0;
+	buf->dev = dev;
+	buf->v4lbuf.index = index;
+	buf->v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf->v4lbuf.flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	buf->v4lbuf.field = V4L2_FIELD_NONE;
+	buf->v4lbuf.memory = V4L2_MEMORY_MMAP;
+	buf->v4lbuf.m.offset = 2*index*buf->v4lbuf.length;
+	return 0;
+}
+
+static int stk_free_sio_buffers(struct stk_camera *dev)
+{
+	int i;
+	int nbufs;
+	unsigned long flags;
+	if (dev->n_sbufs == 0 || dev->sio_bufs == NULL)
+		return 0;
+	/*
+	* If any buffers are mapped, we cannot free them at all.
+	*/
+	for (i = 0; i < dev->n_sbufs; i++) {
+		if (dev->sio_bufs[i].mapcount > 0)
+			return -EBUSY;
+	}
+	/*
+	* OK, let's do it.
+	*/
+	spin_lock_irqsave(&dev->spinlock, flags);
+	INIT_LIST_HEAD(&dev->sio_avail);
+	INIT_LIST_HEAD(&dev->sio_full);
+	nbufs = dev->n_sbufs;
+	dev->n_sbufs = 0;
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+	for (i = 0; i < nbufs; i++)
+		vfree(dev->sio_bufs[i].buffer);
+	kfree(dev->sio_bufs);
+	dev->sio_bufs = NULL;
+	return 0;
+}
+
+static int stk_prepare_sio_buffers(struct stk_camera *dev, unsigned n_sbufs)
+{
+	int i;
+	if (dev->sio_bufs != NULL)
+		pr_err("sio_bufs already allocated\n");
+	else {
+		dev->sio_bufs = kcalloc(n_sbufs,
+					sizeof(struct stk_sio_buffer),
+					GFP_KERNEL);
+		if (dev->sio_bufs == NULL)
+			return -ENOMEM;
+		for (i = 0; i < n_sbufs; i++) {
+			if (stk_setup_siobuf(dev, i))
+				return (dev->n_sbufs > 1 ? 0 : -ENOMEM);
+			dev->n_sbufs = i+1;
+		}
+	}
+	return 0;
+}
+
+static int stk_allocate_buffers(struct stk_camera *dev, unsigned n_sbufs)
+{
+	int err;
+	err = stk_prepare_iso(dev);
+	if (err) {
+		stk_clean_iso(dev);
+		return err;
+	}
+	err = stk_prepare_sio_buffers(dev, n_sbufs);
+	if (err) {
+		stk_free_sio_buffers(dev);
+		return err;
+	}
+	return 0;
+}
+
+static void stk_free_buffers(struct stk_camera *dev)
+{
+	stk_clean_iso(dev);
+	stk_free_sio_buffers(dev);
+}
+/* -------------------------------------------- */
+
+/* v4l file operations */
+
+static int v4l_stk_open(struct file *fp)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+	int err;
+
+	if (dev == NULL || !is_present(dev))
+		return -ENXIO;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (!dev->first_init)
+		stk_camera_write_reg(dev, 0x0, 0x24);
+	else
+		dev->first_init = 0;
+
+	err = v4l2_fh_open(fp);
+	if (!err)
+		usb_autopm_get_interface(dev->interface);
+	mutex_unlock(&dev->lock);
+	return err;
+}
+
+static int v4l_stk_release(struct file *fp)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+
+	mutex_lock(&dev->lock);
+	if (dev->owner == fp) {
+		stk_stop_stream(dev);
+		stk_free_buffers(dev);
+		stk_camera_write_reg(dev, 0x0, 0x49); /* turn off the LED */
+		unset_initialised(dev);
+		dev->owner = NULL;
+	}
+
+	usb_autopm_put_interface(dev->interface);
+	mutex_unlock(&dev->lock);
+	return v4l2_fh_release(fp);
+}
+
+static ssize_t stk_read(struct file *fp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	int i;
+	int ret;
+	unsigned long flags;
+	struct stk_sio_buffer *sbuf;
+	struct stk_camera *dev = video_drvdata(fp);
+
+	if (!is_present(dev))
+		return -EIO;
+	if (dev->owner && (!dev->reading || dev->owner != fp))
+		return -EBUSY;
+	dev->owner = fp;
+	if (!is_streaming(dev)) {
+		if (stk_initialise(dev)
+			|| stk_allocate_buffers(dev, 3)
+			|| stk_start_stream(dev))
+			return -ENOMEM;
+		dev->reading = 1;
+		spin_lock_irqsave(&dev->spinlock, flags);
+		for (i = 0; i < dev->n_sbufs; i++) {
+			list_add_tail(&dev->sio_bufs[i].list, &dev->sio_avail);
+			dev->sio_bufs[i].v4lbuf.flags = V4L2_BUF_FLAG_QUEUED;
+		}
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+	}
+	if (*f_pos == 0) {
+		if (fp->f_flags & O_NONBLOCK && list_empty(&dev->sio_full))
+			return -EWOULDBLOCK;
+		ret = wait_event_interruptible(dev->wait_frame,
+			!list_empty(&dev->sio_full) || !is_present(dev));
+		if (ret)
+			return ret;
+		if (!is_present(dev))
+			return -EIO;
+	}
+	if (count + *f_pos > dev->frame_size)
+		count = dev->frame_size - *f_pos;
+	spin_lock_irqsave(&dev->spinlock, flags);
+	if (list_empty(&dev->sio_full)) {
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+		pr_err("BUG: No siobufs ready\n");
+		return 0;
+	}
+	sbuf = list_first_entry(&dev->sio_full, struct stk_sio_buffer, list);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	if (copy_to_user(buf, sbuf->buffer + *f_pos, count))
+		return -EFAULT;
+
+	*f_pos += count;
+
+	if (*f_pos >= dev->frame_size) {
+		*f_pos = 0;
+		spin_lock_irqsave(&dev->spinlock, flags);
+		list_move_tail(&sbuf->list, &dev->sio_avail);
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+	}
+	return count;
+}
+
+static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+	int ret;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	ret = stk_read(fp, buf, count, f_pos);
+	mutex_unlock(&dev->lock);
+	return ret;
+}
+
+static __poll_t v4l_stk_poll(struct file *fp, poll_table *wait)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+	__poll_t res = v4l2_ctrl_poll(fp, wait);
+
+	poll_wait(fp, &dev->wait_frame, wait);
+
+	if (!is_present(dev))
+		return EPOLLERR;
+
+	if (!list_empty(&dev->sio_full))
+		return res | EPOLLIN | EPOLLRDNORM;
+
+	return res;
+}
+
+
+static void stk_v4l_vm_open(struct vm_area_struct *vma)
+{
+	struct stk_sio_buffer *sbuf = vma->vm_private_data;
+	sbuf->mapcount++;
+}
+static void stk_v4l_vm_close(struct vm_area_struct *vma)
+{
+	struct stk_sio_buffer *sbuf = vma->vm_private_data;
+	sbuf->mapcount--;
+	if (sbuf->mapcount == 0)
+		sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_MAPPED;
+}
+static const struct vm_operations_struct stk_v4l_vm_ops = {
+	.open = stk_v4l_vm_open,
+	.close = stk_v4l_vm_close
+};
+
+static int v4l_stk_mmap(struct file *fp, struct vm_area_struct *vma)
+{
+	unsigned int i;
+	int ret;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct stk_camera *dev = video_drvdata(fp);
+	struct stk_sio_buffer *sbuf = NULL;
+
+	if (!(vma->vm_flags & VM_WRITE) || !(vma->vm_flags & VM_SHARED))
+		return -EINVAL;
+
+	for (i = 0; i < dev->n_sbufs; i++) {
+		if (dev->sio_bufs[i].v4lbuf.m.offset == offset) {
+			sbuf = dev->sio_bufs + i;
+			break;
+		}
+	}
+	if (sbuf == NULL)
+		return -EINVAL;
+	ret = remap_vmalloc_range(vma, sbuf->buffer, 0);
+	if (ret)
+		return ret;
+	vma->vm_flags |= VM_DONTEXPAND;
+	vma->vm_private_data = sbuf;
+	vma->vm_ops = &stk_v4l_vm_ops;
+	sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_MAPPED;
+	stk_v4l_vm_open(vma);
+	return 0;
+}
+
+/* v4l ioctl handlers */
+
+static int stk_vidioc_querycap(struct file *filp,
+		void *priv, struct v4l2_capability *cap)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+
+	strscpy(cap->driver, "stk", sizeof(cap->driver));
+	strscpy(cap->card, "stk", sizeof(cap->card));
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
+	return 0;
+}
+
+static int stk_vidioc_enum_input(struct file *filp,
+		void *priv, struct v4l2_input *input)
+{
+	if (input->index != 0)
+		return -EINVAL;
+
+	strscpy(input->name, "Syntek USB Camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	return 0;
+}
+
+
+static int stk_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int stk_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
+static int stk_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct stk_camera *dev =
+		container_of(ctrl->handler, struct stk_camera, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return stk_sensor_set_brightness(dev, ctrl->val);
+	case V4L2_CID_HFLIP:
+		if (dmi_check_system(stk_upside_down_dmi_table))
+			dev->vsettings.hflip = !ctrl->val;
+		else
+			dev->vsettings.hflip = ctrl->val;
+		return 0;
+	case V4L2_CID_VFLIP:
+		if (dmi_check_system(stk_upside_down_dmi_table))
+			dev->vsettings.vflip = !ctrl->val;
+		else
+			dev->vsettings.vflip = ctrl->val;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+
+static int stk_vidioc_enum_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_fmtdesc *fmtd)
+{
+	switch (fmtd->index) {
+	case 0:
+		fmtd->pixelformat = V4L2_PIX_FMT_RGB565;
+		break;
+	case 1:
+		fmtd->pixelformat = V4L2_PIX_FMT_RGB565X;
+		break;
+	case 2:
+		fmtd->pixelformat = V4L2_PIX_FMT_UYVY;
+		break;
+	case 3:
+		fmtd->pixelformat = V4L2_PIX_FMT_SBGGR8;
+		break;
+	case 4:
+		fmtd->pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct stk_size {
+	unsigned w;
+	unsigned h;
+	enum stk_mode m;
+} stk_sizes[] = {
+	{ .w = 1280, .h = 1024, .m = MODE_SXGA, },
+	{ .w = 640,  .h = 480,  .m = MODE_VGA,  },
+	{ .w = 352,  .h = 288,  .m = MODE_CIF,  },
+	{ .w = 320,  .h = 240,  .m = MODE_QVGA, },
+	{ .w = 176,  .h = 144,  .m = MODE_QCIF, },
+};
+
+static int stk_vidioc_g_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix_format = &f->fmt.pix;
+	struct stk_camera *dev = video_drvdata(filp);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(stk_sizes) &&
+			stk_sizes[i].m != dev->vsettings.mode; i++)
+		;
+	if (i == ARRAY_SIZE(stk_sizes)) {
+		pr_err("ERROR: mode invalid\n");
+		return -EINVAL;
+	}
+	pix_format->width = stk_sizes[i].w;
+	pix_format->height = stk_sizes[i].h;
+	pix_format->field = V4L2_FIELD_NONE;
+	pix_format->colorspace = V4L2_COLORSPACE_SRGB;
+	pix_format->pixelformat = dev->vsettings.palette;
+	if (dev->vsettings.palette == V4L2_PIX_FMT_SBGGR8)
+		pix_format->bytesperline = pix_format->width;
+	else
+		pix_format->bytesperline = 2 * pix_format->width;
+	pix_format->sizeimage = pix_format->bytesperline
+				* pix_format->height;
+	return 0;
+}
+
+static int stk_try_fmt_vid_cap(struct file *filp,
+		struct v4l2_format *fmtd, int *idx)
+{
+	int i;
+	switch (fmtd->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_SBGGR8:
+		break;
+	default:
+		return -EINVAL;
+	}
+	for (i = 1; i < ARRAY_SIZE(stk_sizes); i++) {
+		if (fmtd->fmt.pix.width > stk_sizes[i].w)
+			break;
+	}
+	if (i == ARRAY_SIZE(stk_sizes)
+		|| (abs(fmtd->fmt.pix.width - stk_sizes[i-1].w)
+			< abs(fmtd->fmt.pix.width - stk_sizes[i].w))) {
+		fmtd->fmt.pix.height = stk_sizes[i-1].h;
+		fmtd->fmt.pix.width = stk_sizes[i-1].w;
+		if (idx)
+			*idx = i - 1;
+	} else {
+		fmtd->fmt.pix.height = stk_sizes[i].h;
+		fmtd->fmt.pix.width = stk_sizes[i].w;
+		if (idx)
+			*idx = i;
+	}
+
+	fmtd->fmt.pix.field = V4L2_FIELD_NONE;
+	fmtd->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+	if (fmtd->fmt.pix.pixelformat == V4L2_PIX_FMT_SBGGR8)
+		fmtd->fmt.pix.bytesperline = fmtd->fmt.pix.width;
+	else
+		fmtd->fmt.pix.bytesperline = 2 * fmtd->fmt.pix.width;
+	fmtd->fmt.pix.sizeimage = fmtd->fmt.pix.bytesperline
+		* fmtd->fmt.pix.height;
+	return 0;
+}
+
+static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_format *fmtd)
+{
+	return stk_try_fmt_vid_cap(filp, fmtd, NULL);
+}
+
+static int stk_setup_format(struct stk_camera *dev)
+{
+	int i = 0;
+	int depth;
+	if (dev->vsettings.palette == V4L2_PIX_FMT_SBGGR8)
+		depth = 1;
+	else
+		depth = 2;
+	while (i < ARRAY_SIZE(stk_sizes) &&
+			stk_sizes[i].m != dev->vsettings.mode)
+		i++;
+	if (i == ARRAY_SIZE(stk_sizes)) {
+		pr_err("Something is broken in %s\n", __func__);
+		return -EFAULT;
+	}
+	/* This registers controls some timings, not sure of what. */
+	stk_camera_write_reg(dev, 0x001b, 0x0e);
+	if (dev->vsettings.mode == MODE_SXGA)
+		stk_camera_write_reg(dev, 0x001c, 0x0e);
+	else
+		stk_camera_write_reg(dev, 0x001c, 0x46);
+	/*
+	 * Registers 0x0115 0x0114 are the size of each line (bytes),
+	 * regs 0x0117 0x0116 are the height of the image.
+	 */
+	stk_camera_write_reg(dev, 0x0115,
+		((stk_sizes[i].w * depth) >> 8) & 0xff);
+	stk_camera_write_reg(dev, 0x0114,
+		(stk_sizes[i].w * depth) & 0xff);
+	stk_camera_write_reg(dev, 0x0117,
+		(stk_sizes[i].h >> 8) & 0xff);
+	stk_camera_write_reg(dev, 0x0116,
+		stk_sizes[i].h & 0xff);
+	return stk_sensor_configure(dev);
+}
+
+static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_format *fmtd)
+{
+	int ret;
+	int idx;
+	struct stk_camera *dev = video_drvdata(filp);
+
+	if (dev == NULL)
+		return -ENODEV;
+	if (!is_present(dev))
+		return -ENODEV;
+	if (is_streaming(dev))
+		return -EBUSY;
+	if (dev->owner)
+		return -EBUSY;
+	ret = stk_try_fmt_vid_cap(filp, fmtd, &idx);
+	if (ret)
+		return ret;
+
+	dev->vsettings.palette = fmtd->fmt.pix.pixelformat;
+	stk_free_buffers(dev);
+	dev->frame_size = fmtd->fmt.pix.sizeimage;
+	dev->vsettings.mode = stk_sizes[idx].m;
+
+	stk_initialise(dev);
+	return stk_setup_format(dev);
+}
+
+static int stk_vidioc_reqbufs(struct file *filp,
+		void *priv, struct v4l2_requestbuffers *rb)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+
+	if (dev == NULL)
+		return -ENODEV;
+	if (rb->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+	if (is_streaming(dev)
+		|| (dev->owner && dev->owner != filp))
+		return -EBUSY;
+	stk_free_buffers(dev);
+	if (rb->count == 0) {
+		stk_camera_write_reg(dev, 0x0, 0x49); /* turn off the LED */
+		unset_initialised(dev);
+		dev->owner = NULL;
+		return 0;
+	}
+	dev->owner = filp;
+
+	/*FIXME If they ask for zero, we must stop streaming and free */
+	if (rb->count < 3)
+		rb->count = 3;
+	/* Arbitrary limit */
+	else if (rb->count > 5)
+		rb->count = 5;
+
+	stk_allocate_buffers(dev, rb->count);
+	rb->count = dev->n_sbufs;
+	return 0;
+}
+
+static int stk_vidioc_querybuf(struct file *filp,
+		void *priv, struct v4l2_buffer *buf)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+	struct stk_sio_buffer *sbuf;
+
+	if (buf->index >= dev->n_sbufs)
+		return -EINVAL;
+	sbuf = dev->sio_bufs + buf->index;
+	*buf = sbuf->v4lbuf;
+	return 0;
+}
+
+static int stk_vidioc_qbuf(struct file *filp,
+		void *priv, struct v4l2_buffer *buf)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+	struct stk_sio_buffer *sbuf;
+	unsigned long flags;
+
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	if (buf->index >= dev->n_sbufs)
+		return -EINVAL;
+	sbuf = dev->sio_bufs + buf->index;
+	if (sbuf->v4lbuf.flags & V4L2_BUF_FLAG_QUEUED)
+		return 0;
+	sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_QUEUED;
+	sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_DONE;
+	spin_lock_irqsave(&dev->spinlock, flags);
+	list_add_tail(&sbuf->list, &dev->sio_avail);
+	*buf = sbuf->v4lbuf;
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+	return 0;
+}
+
+static int stk_vidioc_dqbuf(struct file *filp,
+		void *priv, struct v4l2_buffer *buf)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+	struct stk_sio_buffer *sbuf;
+	unsigned long flags;
+	int ret;
+
+	if (!is_streaming(dev))
+		return -EINVAL;
+
+	if (filp->f_flags & O_NONBLOCK && list_empty(&dev->sio_full))
+		return -EWOULDBLOCK;
+	ret = wait_event_interruptible(dev->wait_frame,
+		!list_empty(&dev->sio_full) || !is_present(dev));
+	if (ret)
+		return ret;
+	if (!is_present(dev))
+		return -EIO;
+
+	spin_lock_irqsave(&dev->spinlock, flags);
+	sbuf = list_first_entry(&dev->sio_full, struct stk_sio_buffer, list);
+	list_del_init(&sbuf->list);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+	sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_QUEUED;
+	sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_DONE;
+	sbuf->v4lbuf.sequence = ++dev->sequence;
+	v4l2_buffer_set_timestamp(&sbuf->v4lbuf, ktime_get_ns());
+
+	*buf = sbuf->v4lbuf;
+	return 0;
+}
+
+static int stk_vidioc_streamon(struct file *filp,
+		void *priv, enum v4l2_buf_type type)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+	if (is_streaming(dev))
+		return 0;
+	if (dev->sio_bufs == NULL)
+		return -EINVAL;
+	dev->sequence = 0;
+	return stk_start_stream(dev);
+}
+
+static int stk_vidioc_streamoff(struct file *filp,
+		void *priv, enum v4l2_buf_type type)
+{
+	struct stk_camera *dev = video_drvdata(filp);
+	unsigned long flags;
+	int i;
+	stk_stop_stream(dev);
+	spin_lock_irqsave(&dev->spinlock, flags);
+	INIT_LIST_HEAD(&dev->sio_avail);
+	INIT_LIST_HEAD(&dev->sio_full);
+	for (i = 0; i < dev->n_sbufs; i++) {
+		INIT_LIST_HEAD(&dev->sio_bufs[i].list);
+		dev->sio_bufs[i].v4lbuf.flags = 0;
+	}
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+	return 0;
+}
+
+
+static int stk_vidioc_g_parm(struct file *filp,
+		void *priv, struct v4l2_streamparm *sp)
+{
+	/*FIXME This is not correct */
+	sp->parm.capture.timeperframe.numerator = 1;
+	sp->parm.capture.timeperframe.denominator = 30;
+	sp->parm.capture.readbuffers = 2;
+	return 0;
+}
+
+static int stk_vidioc_enum_framesizes(struct file *filp,
+		void *priv, struct v4l2_frmsizeenum *frms)
+{
+	if (frms->index >= ARRAY_SIZE(stk_sizes))
+		return -EINVAL;
+	switch (frms->pixel_format) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_SBGGR8:
+		frms->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		frms->discrete.width = stk_sizes[frms->index].w;
+		frms->discrete.height = stk_sizes[frms->index].h;
+		return 0;
+	default: return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops stk_ctrl_ops = {
+	.s_ctrl = stk_s_ctrl,
+};
+
+static const struct v4l2_file_operations v4l_stk_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l_stk_open,
+	.release = v4l_stk_release,
+	.read = v4l_stk_read,
+	.poll = v4l_stk_poll,
+	.mmap = v4l_stk_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
+	.vidioc_querycap = stk_vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap = stk_vidioc_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = stk_vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = stk_vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = stk_vidioc_g_fmt_vid_cap,
+	.vidioc_enum_input = stk_vidioc_enum_input,
+	.vidioc_s_input = stk_vidioc_s_input,
+	.vidioc_g_input = stk_vidioc_g_input,
+	.vidioc_reqbufs = stk_vidioc_reqbufs,
+	.vidioc_querybuf = stk_vidioc_querybuf,
+	.vidioc_qbuf = stk_vidioc_qbuf,
+	.vidioc_dqbuf = stk_vidioc_dqbuf,
+	.vidioc_streamon = stk_vidioc_streamon,
+	.vidioc_streamoff = stk_vidioc_streamoff,
+	.vidioc_g_parm = stk_vidioc_g_parm,
+	.vidioc_enum_framesizes = stk_vidioc_enum_framesizes,
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static void stk_v4l_dev_release(struct video_device *vd)
+{
+	struct stk_camera *dev = vdev_to_camera(vd);
+
+	if (dev->sio_bufs != NULL || dev->isobufs != NULL)
+		pr_err("We are leaking memory\n");
+	usb_put_intf(dev->interface);
+	usb_put_dev(dev->udev);
+
+	v4l2_ctrl_handler_free(&dev->hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	kfree(dev);
+}
+
+static const struct video_device stk_v4l_data = {
+	.name = "stkwebcam",
+	.fops = &v4l_stk_fops,
+	.ioctl_ops = &v4l_stk_ioctl_ops,
+	.release = stk_v4l_dev_release,
+};
+
+
+static int stk_register_video_device(struct stk_camera *dev)
+{
+	int err;
+
+	dev->vdev = stk_v4l_data;
+	dev->vdev.lock = &dev->lock;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	dev->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+				V4L2_CAP_STREAMING;
+	video_set_drvdata(&dev->vdev, dev);
+	err = video_register_device(&dev->vdev, VFL_TYPE_VIDEO, -1);
+	if (err)
+		pr_err("v4l registration failed\n");
+	else
+		pr_info("Syntek USB2.0 Camera is now controlling device %s\n",
+			video_device_node_name(&dev->vdev));
+	return err;
+}
+
+
+/* USB Stuff */
+
+static int stk_camera_probe(struct usb_interface *interface,
+		const struct usb_device_id *id)
+{
+	struct v4l2_ctrl_handler *hdl;
+	int err = 0;
+	int i;
+
+	struct stk_camera *dev = NULL;
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+
+	dev = kzalloc(sizeof(struct stk_camera), GFP_KERNEL);
+	if (dev == NULL) {
+		pr_err("Out of memory !\n");
+		return -ENOMEM;
+	}
+	err = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
+	if (err < 0) {
+		dev_err(&udev->dev, "couldn't register v4l2_device\n");
+		kfree(dev);
+		return err;
+	}
+	hdl = &dev->hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 0xff, 0x1, 0x60);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 1);
+	if (hdl->error) {
+		err = hdl->error;
+		dev_err(&udev->dev, "couldn't register control\n");
+		goto error;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+
+	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->lock);
+	init_waitqueue_head(&dev->wait_frame);
+	dev->first_init = 1; /* webcam LED management */
+
+	dev->udev = usb_get_dev(udev);
+	dev->interface = interface;
+	usb_get_intf(interface);
+
+	if (hflip != -1)
+		dev->vsettings.hflip = hflip;
+	else if (dmi_check_system(stk_upside_down_dmi_table))
+		dev->vsettings.hflip = 1;
+	else
+		dev->vsettings.hflip = 0;
+	if (vflip != -1)
+		dev->vsettings.vflip = vflip;
+	else if (dmi_check_system(stk_upside_down_dmi_table))
+		dev->vsettings.vflip = 1;
+	else
+		dev->vsettings.vflip = 0;
+	dev->n_sbufs = 0;
+	set_present(dev);
+
+	/* Set up the endpoint information
+	 * use only the first isoc-in endpoint
+	 * for the current alternate setting */
+	iface_desc = interface->cur_altsetting;
+
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (!dev->isoc_ep
+			&& usb_endpoint_is_isoc_in(endpoint)) {
+			/* we found an isoc in endpoint */
+			dev->isoc_ep = usb_endpoint_num(endpoint);
+			break;
+		}
+	}
+	if (!dev->isoc_ep) {
+		pr_err("Could not find isoc-in endpoint\n");
+		err = -ENODEV;
+		goto error_put;
+	}
+	dev->vsettings.palette = V4L2_PIX_FMT_RGB565;
+	dev->vsettings.mode = MODE_VGA;
+	dev->frame_size = 640 * 480 * 2;
+
+	INIT_LIST_HEAD(&dev->sio_avail);
+	INIT_LIST_HEAD(&dev->sio_full);
+
+	usb_set_intfdata(interface, dev);
+
+	err = stk_register_video_device(dev);
+	if (err)
+		goto error_put;
+
+	return 0;
+
+error_put:
+	usb_put_intf(interface);
+	usb_put_dev(dev->udev);
+error:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	kfree(dev);
+	return err;
+}
+
+static void stk_camera_disconnect(struct usb_interface *interface)
+{
+	struct stk_camera *dev = usb_get_intfdata(interface);
+
+	usb_set_intfdata(interface, NULL);
+	unset_present(dev);
+
+	wake_up_interruptible(&dev->wait_frame);
+
+	pr_info("Syntek USB2.0 Camera release resources device %s\n",
+		video_device_node_name(&dev->vdev));
+
+	video_unregister_device(&dev->vdev);
+}
+
+#ifdef CONFIG_PM
+static int stk_camera_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct stk_camera *dev = usb_get_intfdata(intf);
+	if (is_streaming(dev)) {
+		stk_stop_stream(dev);
+		/* yes, this is ugly */
+		set_streaming(dev);
+	}
+	return 0;
+}
+
+static int stk_camera_resume(struct usb_interface *intf)
+{
+	struct stk_camera *dev = usb_get_intfdata(intf);
+	if (!is_initialised(dev))
+		return 0;
+	unset_initialised(dev);
+	stk_initialise(dev);
+	stk_camera_write_reg(dev, 0x0, 0x49);
+	stk_setup_format(dev);
+	if (is_streaming(dev))
+		stk_start_stream(dev);
+	return 0;
+}
+#endif
+
+static struct usb_driver stk_camera_driver = {
+	.name = "stkwebcam",
+	.probe = stk_camera_probe,
+	.disconnect = stk_camera_disconnect,
+	.id_table = stkwebcam_table,
+#ifdef CONFIG_PM
+	.suspend = stk_camera_suspend,
+	.resume = stk_camera_resume,
+#endif
+};
+
+module_usb_driver(stk_camera_driver);
diff --git a/drivers/staging/media/deprecated/stkwebcam/stk-webcam.h b/drivers/staging/media/deprecated/stkwebcam/stk-webcam.h
new file mode 100644
index 000000000..136decffe
--- /dev/null
+++ b/drivers/staging/media/deprecated/stkwebcam/stk-webcam.h
@@ -0,0 +1,123 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * stk-webcam.h : Driver for Syntek 1125 USB webcam controller
+ *
+ * Copyright (C) 2006 Nicolas VIVIEN
+ * Copyright 2007-2008 Jaime Velasco Juan <jsagarribay@gmail.com>
+ */
+
+#ifndef STKWEBCAM_H
+#define STKWEBCAM_H
+
+#include <linux/usb.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-common.h>
+
+#define DRIVER_VERSION		"v0.0.1"
+#define DRIVER_VERSION_NUM	0x000001
+
+#define MAX_ISO_BUFS		3
+#define ISO_FRAMES_PER_DESC	16
+#define ISO_MAX_FRAME_SIZE	3 * 1024
+#define ISO_BUFFER_SIZE		(ISO_FRAMES_PER_DESC * ISO_MAX_FRAME_SIZE)
+
+struct stk_iso_buf {
+	void *data;
+	int length;
+	int read;
+	struct urb *urb;
+};
+
+/* Streaming IO buffers */
+struct stk_sio_buffer {
+	struct v4l2_buffer v4lbuf;
+	char *buffer;
+	int mapcount;
+	struct stk_camera *dev;
+	struct list_head list;
+};
+
+enum stk_mode {MODE_VGA, MODE_SXGA, MODE_CIF, MODE_QVGA, MODE_QCIF};
+
+struct stk_video {
+	enum stk_mode mode;
+	__u32 palette;
+	int hflip;
+	int vflip;
+};
+
+enum stk_status {
+	S_PRESENT = 1,
+	S_INITIALISED = 2,
+	S_MEMALLOCD = 4,
+	S_STREAMING = 8,
+};
+#define is_present(dev)		((dev)->status & S_PRESENT)
+#define is_initialised(dev)	((dev)->status & S_INITIALISED)
+#define is_streaming(dev)	((dev)->status & S_STREAMING)
+#define is_memallocd(dev)	((dev)->status & S_MEMALLOCD)
+#define set_present(dev)	((dev)->status = S_PRESENT)
+#define unset_present(dev)	((dev)->status &= \
+					~(S_PRESENT|S_INITIALISED|S_STREAMING))
+#define set_initialised(dev)	((dev)->status |= S_INITIALISED)
+#define unset_initialised(dev)	((dev)->status &= ~S_INITIALISED)
+#define set_memallocd(dev)	((dev)->status |= S_MEMALLOCD)
+#define unset_memallocd(dev)	((dev)->status &= ~S_MEMALLOCD)
+#define set_streaming(dev)	((dev)->status |= S_STREAMING)
+#define unset_streaming(dev)	((dev)->status &= ~S_STREAMING)
+
+struct regval {
+	unsigned reg;
+	unsigned val;
+};
+
+struct stk_camera {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
+	struct video_device vdev;
+	struct usb_device *udev;
+	struct usb_interface *interface;
+	int webcam_model;
+	struct file *owner;
+	struct mutex lock;
+	int first_init;
+
+	u8 isoc_ep;
+
+	/* Not sure if this is right */
+	atomic_t urbs_used;
+
+	struct stk_video vsettings;
+
+	enum stk_status status;
+
+	spinlock_t spinlock;
+	wait_queue_head_t wait_frame;
+
+	struct stk_iso_buf *isobufs;
+
+	int frame_size;
+	/* Streaming buffers */
+	int reading;
+	unsigned int n_sbufs;
+	struct stk_sio_buffer *sio_bufs;
+	struct list_head sio_avail;
+	struct list_head sio_full;
+	unsigned sequence;
+
+	u8 read_reg_scratch;
+};
+
+#define vdev_to_camera(d) container_of(d, struct stk_camera, vdev)
+
+int stk_camera_write_reg(struct stk_camera *, u16, u8);
+int stk_camera_read_reg(struct stk_camera *, u16, u8 *);
+
+int stk_sensor_init(struct stk_camera *);
+int stk_sensor_configure(struct stk_camera *);
+int stk_sensor_sleep(struct stk_camera *dev);
+int stk_sensor_wakeup(struct stk_camera *dev);
+int stk_sensor_set_brightness(struct stk_camera *dev, int br);
+
+#endif
diff --git a/drivers/staging/media/deprecated/tm6000/Kconfig b/drivers/staging/media/deprecated/tm6000/Kconfig
new file mode 100644
index 000000000..73d72e49e
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/Kconfig
@@ -0,0 +1,37 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_TM6000
+	tristate "TV Master TM5600/6000/6010 driver (DEPRECATED)"
+	depends on VIDEO_DEV && I2C && INPUT && RC_CORE && USB
+	select VIDEO_TUNER
+	select MEDIA_TUNER_XC2028
+	select MEDIA_TUNER_XC5000
+	select VIDEOBUF_VMALLOC
+	help
+	  Support for TM5600/TM6000/TM6010 USB Device
+
+	  Since these cards have no MPEG decoder onboard, they transmit
+	  only compressed MPEG data over the usb bus, so you need
+	  an external software decoder to watch TV on your computer.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  Say Y if you own such a device and want to use it.
+
+config VIDEO_TM6000_ALSA
+	tristate "TV Master TM5600/6000/6010 audio support"
+	depends on VIDEO_TM6000 && SND
+	select SND_PCM
+	help
+	  This is a video4linux driver for direct (DMA) audio for
+	  TM5600/TM6000/TM6010 USB Devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tm6000-alsa.
+
+config VIDEO_TM6000_DVB
+	tristate "DVB Support for tm6000 based TV cards"
+	depends on VIDEO_TM6000 && DVB_CORE && USB
+	select DVB_ZL10353
+	help
+	  This adds support for DVB cards based on the tm5600/tm6000 chip.
diff --git a/drivers/staging/media/deprecated/tm6000/Makefile b/drivers/staging/media/deprecated/tm6000/Makefile
new file mode 100644
index 000000000..75247a02a
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/Makefile
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+tm6000-y := tm6000-cards.o \
+		   tm6000-core.o  \
+		   tm6000-i2c.o   \
+		   tm6000-video.o \
+		   tm6000-stds.o \
+		   tm6000-input.o
+
+obj-$(CONFIG_VIDEO_TM6000) += tm6000.o
+obj-$(CONFIG_VIDEO_TM6000_ALSA) += tm6000-alsa.o
+obj-$(CONFIG_VIDEO_TM6000_DVB) += tm6000-dvb.o
+
+ccflags-y += -I $(srctree)/drivers/media/tuners
+ccflags-y += -I $(srctree)/drivers/media/dvb-frontends
diff --git a/drivers/staging/media/deprecated/tm6000/TODO b/drivers/staging/media/deprecated/tm6000/TODO
new file mode 100644
index 000000000..ecb30a429
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/TODO
@@ -0,0 +1,7 @@
+This is one of the few drivers still not using the vb2
+framework, so this driver is now deprecated with the intent of
+removing it altogether by the beginning of 2023.
+
+In order to keep this driver it has to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-alsa.c b/drivers/staging/media/deprecated/tm6000/tm6000-alsa.c
new file mode 100644
index 000000000..a19a46770
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-alsa.c
@@ -0,0 +1,440 @@
+// SPDX-License-Identifier: GPL-2.0
+// Support for audio capture for tm5600/6000/6010
+// Copyright (c) 2007-2008 Mauro Carvalho Chehab <mchehab@kernel.org>
+//
+// Based on cx88-alsa.c
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/usb.h>
+#include <linux/slab.h>
+
+#include <linux/delay.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/control.h>
+#include <sound/initval.h>
+
+
+#include "tm6000.h"
+#include "tm6000-regs.h"
+
+#undef dprintk
+
+#define dprintk(level, fmt, arg...) do {				   \
+	if (debug >= level)						   \
+		printk(KERN_INFO "%s/1: " fmt, chip->core->name , ## arg); \
+	} while (0)
+
+/****************************************************************************
+			Module global static vars
+ ****************************************************************************/
+
+static int index[SNDRV_CARDS] = SNDRV_DEFAULT_IDX;	/* Index 0-MAX */
+
+static bool enable[SNDRV_CARDS] = SNDRV_DEFAULT_ENABLE_PNP;
+
+module_param_array(enable, bool, NULL, 0444);
+MODULE_PARM_DESC(enable, "Enable tm6000x soundcard. default enabled.");
+
+module_param_array(index, int, NULL, 0444);
+MODULE_PARM_DESC(index, "Index value for tm6000x capture interface(s).");
+
+
+/****************************************************************************
+				Module macros
+ ****************************************************************************/
+
+MODULE_DESCRIPTION("ALSA driver module for tm5600/tm6000/tm6010 based TV cards");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL v2");
+static unsigned int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debug messages");
+
+/****************************************************************************
+			Module specific functions
+ ****************************************************************************/
+
+/*
+ * BOARD Specific: Sets audio DMA
+ */
+
+static int _tm6000_start_audio_dma(struct snd_tm6000_card *chip)
+{
+	struct tm6000_core *core = chip->core;
+
+	dprintk(1, "Starting audio DMA\n");
+
+	/* Enables audio */
+	tm6000_set_reg_mask(core, TM6010_REQ07_RCC_ACTIVE_IF, 0x40, 0x40);
+
+	tm6000_set_audio_bitrate(core, 48000);
+
+	return 0;
+}
+
+/*
+ * BOARD Specific: Resets audio DMA
+ */
+static int _tm6000_stop_audio_dma(struct snd_tm6000_card *chip)
+{
+	struct tm6000_core *core = chip->core;
+
+	dprintk(1, "Stopping audio DMA\n");
+
+	/* Disables audio */
+	tm6000_set_reg_mask(core, TM6010_REQ07_RCC_ACTIVE_IF, 0x00, 0x40);
+
+	return 0;
+}
+
+/****************************************************************************
+				ALSA PCM Interface
+ ****************************************************************************/
+
+/*
+ * Digital hardware definition
+ */
+#define DEFAULT_FIFO_SIZE	4096
+
+static const struct snd_pcm_hardware snd_tm6000_digital_hw = {
+	.info = SNDRV_PCM_INFO_BATCH |
+		SNDRV_PCM_INFO_MMAP |
+		SNDRV_PCM_INFO_INTERLEAVED |
+		SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		SNDRV_PCM_INFO_MMAP_VALID,
+	.formats = SNDRV_PCM_FMTBIT_S16_LE,
+
+	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_KNOT,
+	.rate_min = 48000,
+	.rate_max = 48000,
+	.channels_min = 2,
+	.channels_max = 2,
+	.period_bytes_min = 64,
+	.period_bytes_max = 12544,
+	.periods_min = 2,
+	.periods_max = 98,
+	.buffer_bytes_max = 62720 * 8,
+};
+
+/*
+ * audio pcm capture open callback
+ */
+static int snd_tm6000_pcm_open(struct snd_pcm_substream *substream)
+{
+	struct snd_tm6000_card *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	int err;
+
+	err = snd_pcm_hw_constraint_pow2(runtime, 0,
+					 SNDRV_PCM_HW_PARAM_PERIODS);
+	if (err < 0)
+		goto _error;
+
+	chip->substream = substream;
+
+	runtime->hw = snd_tm6000_digital_hw;
+	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+
+	return 0;
+_error:
+	dprintk(1, "Error opening PCM!\n");
+	return err;
+}
+
+/*
+ * audio close callback
+ */
+static int snd_tm6000_close(struct snd_pcm_substream *substream)
+{
+	struct snd_tm6000_card *chip = snd_pcm_substream_chip(substream);
+	struct tm6000_core *core = chip->core;
+
+	if (atomic_read(&core->stream_started) > 0) {
+		atomic_set(&core->stream_started, 0);
+		schedule_work(&core->wq_trigger);
+	}
+
+	return 0;
+}
+
+static int tm6000_fillbuf(struct tm6000_core *core, char *buf, int size)
+{
+	struct snd_tm6000_card *chip = core->adev;
+	struct snd_pcm_substream *substream = chip->substream;
+	struct snd_pcm_runtime *runtime;
+	int period_elapsed = 0;
+	unsigned int stride, buf_pos;
+	int length;
+
+	if (atomic_read(&core->stream_started) == 0)
+		return 0;
+
+	if (!size || !substream) {
+		dprintk(1, "substream was NULL\n");
+		return -EINVAL;
+	}
+
+	runtime = substream->runtime;
+	if (!runtime || !runtime->dma_area) {
+		dprintk(1, "runtime was NULL\n");
+		return -EINVAL;
+	}
+
+	buf_pos = chip->buf_pos;
+	stride = runtime->frame_bits >> 3;
+
+	if (stride == 0) {
+		dprintk(1, "stride is zero\n");
+		return -EINVAL;
+	}
+
+	length = size / stride;
+	if (length == 0) {
+		dprintk(1, "%s: length was zero\n", __func__);
+		return -EINVAL;
+	}
+
+	dprintk(1, "Copying %d bytes at %p[%d] - buf size=%d x %d\n", size,
+		runtime->dma_area, buf_pos,
+		(unsigned int)runtime->buffer_size, stride);
+
+	if (buf_pos + length >= runtime->buffer_size) {
+		unsigned int cnt = runtime->buffer_size - buf_pos;
+		memcpy(runtime->dma_area + buf_pos * stride, buf, cnt * stride);
+		memcpy(runtime->dma_area, buf + cnt * stride,
+			length * stride - cnt * stride);
+	} else
+		memcpy(runtime->dma_area + buf_pos * stride, buf,
+			length * stride);
+
+	snd_pcm_stream_lock(substream);
+
+	chip->buf_pos += length;
+	if (chip->buf_pos >= runtime->buffer_size)
+		chip->buf_pos -= runtime->buffer_size;
+
+	chip->period_pos += length;
+	if (chip->period_pos >= runtime->period_size) {
+		chip->period_pos -= runtime->period_size;
+		period_elapsed = 1;
+	}
+
+	snd_pcm_stream_unlock(substream);
+
+	if (period_elapsed)
+		snd_pcm_period_elapsed(substream);
+
+	return 0;
+}
+
+/*
+ * prepare callback
+ */
+static int snd_tm6000_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_tm6000_card *chip = snd_pcm_substream_chip(substream);
+
+	chip->buf_pos = 0;
+	chip->period_pos = 0;
+
+	return 0;
+}
+
+
+/*
+ * trigger callback
+ */
+static void audio_trigger(struct work_struct *work)
+{
+	struct tm6000_core *core = container_of(work, struct tm6000_core,
+						wq_trigger);
+	struct snd_tm6000_card *chip = core->adev;
+
+	if (atomic_read(&core->stream_started)) {
+		dprintk(1, "starting capture");
+		_tm6000_start_audio_dma(chip);
+	} else {
+		dprintk(1, "stopping capture");
+		_tm6000_stop_audio_dma(chip);
+	}
+}
+
+static int snd_tm6000_card_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	struct snd_tm6000_card *chip = snd_pcm_substream_chip(substream);
+	struct tm6000_core *core = chip->core;
+	int err = 0;
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_START:
+		atomic_set(&core->stream_started, 1);
+		break;
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+	case SNDRV_PCM_TRIGGER_STOP:
+		atomic_set(&core->stream_started, 0);
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+	schedule_work(&core->wq_trigger);
+
+	return err;
+}
+/*
+ * pointer callback
+ */
+static snd_pcm_uframes_t snd_tm6000_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_tm6000_card *chip = snd_pcm_substream_chip(substream);
+
+	return chip->buf_pos;
+}
+
+/*
+ * operators
+ */
+static const struct snd_pcm_ops snd_tm6000_pcm_ops = {
+	.open = snd_tm6000_pcm_open,
+	.close = snd_tm6000_close,
+	.prepare = snd_tm6000_prepare,
+	.trigger = snd_tm6000_card_trigger,
+	.pointer = snd_tm6000_pointer,
+};
+
+/*
+ * create a PCM device
+ */
+
+/* FIXME: Control interface - How to control volume/mute? */
+
+/****************************************************************************
+			Basic Flow for Sound Devices
+ ****************************************************************************/
+
+/*
+ * Alsa Constructor - Component probe
+ */
+static int tm6000_audio_init(struct tm6000_core *dev)
+{
+	struct snd_card		*card;
+	struct snd_tm6000_card	*chip;
+	int			rc;
+	static int		devnr;
+	char			component[14];
+	struct snd_pcm		*pcm;
+
+	if (!dev)
+		return 0;
+
+	if (devnr >= SNDRV_CARDS)
+		return -ENODEV;
+
+	if (!enable[devnr])
+		return -ENOENT;
+
+	rc = snd_card_new(&dev->udev->dev, index[devnr], "tm6000",
+			  THIS_MODULE, 0, &card);
+	if (rc < 0) {
+		snd_printk(KERN_ERR "cannot create card instance %d\n", devnr);
+		return rc;
+	}
+	strscpy(card->driver, "tm6000-alsa", sizeof(card->driver));
+	strscpy(card->shortname, "TM5600/60x0", sizeof(card->shortname));
+	sprintf(card->longname, "TM5600/60x0 Audio at bus %d device %d",
+		dev->udev->bus->busnum, dev->udev->devnum);
+
+	sprintf(component, "USB%04x:%04x",
+		le16_to_cpu(dev->udev->descriptor.idVendor),
+		le16_to_cpu(dev->udev->descriptor.idProduct));
+	snd_component_add(card, component);
+
+	chip = kzalloc(sizeof(struct snd_tm6000_card), GFP_KERNEL);
+	if (!chip) {
+		rc = -ENOMEM;
+		goto error;
+	}
+
+	chip->core = dev;
+	chip->card = card;
+	dev->adev = chip;
+	spin_lock_init(&chip->reg_lock);
+
+	rc = snd_pcm_new(card, "TM6000 Audio", 0, 0, 1, &pcm);
+	if (rc < 0)
+		goto error_chip;
+
+	pcm->info_flags = 0;
+	pcm->private_data = chip;
+	strscpy(pcm->name, "Trident TM5600/60x0", sizeof(pcm->name));
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &snd_tm6000_pcm_ops);
+	snd_pcm_set_managed_buffer_all(pcm, SNDRV_DMA_TYPE_VMALLOC, NULL, 0, 0);
+
+	INIT_WORK(&dev->wq_trigger, audio_trigger);
+	rc = snd_card_register(card);
+	if (rc < 0)
+		goto error_chip;
+
+	dprintk(1, "Registered audio driver for %s\n", card->longname);
+
+	return 0;
+
+error_chip:
+	kfree(chip);
+	dev->adev = NULL;
+error:
+	snd_card_free(card);
+	return rc;
+}
+
+static int tm6000_audio_fini(struct tm6000_core *dev)
+{
+	struct snd_tm6000_card *chip;
+
+	if (!dev)
+		return 0;
+	chip = dev->adev;
+
+	if (!chip)
+		return 0;
+
+	if (!chip->card)
+		return 0;
+
+	snd_card_free(chip->card);
+	chip->card = NULL;
+	kfree(chip);
+	dev->adev = NULL;
+
+	return 0;
+}
+
+static struct tm6000_ops audio_ops = {
+	.type	= TM6000_AUDIO,
+	.name	= "TM6000 Audio Extension",
+	.init	= tm6000_audio_init,
+	.fini	= tm6000_audio_fini,
+	.fillbuf = tm6000_fillbuf,
+};
+
+static int __init tm6000_alsa_register(void)
+{
+	return tm6000_register_extension(&audio_ops);
+}
+
+static void __exit tm6000_alsa_unregister(void)
+{
+	tm6000_unregister_extension(&audio_ops);
+}
+
+module_init(tm6000_alsa_register);
+module_exit(tm6000_alsa_unregister);
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-cards.c b/drivers/staging/media/deprecated/tm6000/tm6000-cards.c
new file mode 100644
index 000000000..98f4a63ad
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-cards.c
@@ -0,0 +1,1397 @@
+// SPDX-License-Identifier: GPL-2.0
+// tm6000-cards.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+//
+// Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/usb.h>
+#include <linux/slab.h>
+#include <media/v4l2-common.h>
+#include <media/tuner.h>
+#include <media/i2c/tvaudio.h>
+#include <media/rc-map.h>
+
+#include "tm6000.h"
+#include "tm6000-regs.h"
+#include "xc2028.h"
+#include "xc5000.h"
+
+#define TM6000_BOARD_UNKNOWN			0
+#define TM5600_BOARD_GENERIC			1
+#define TM6000_BOARD_GENERIC			2
+#define TM6010_BOARD_GENERIC			3
+#define TM5600_BOARD_10MOONS_UT821		4
+#define TM5600_BOARD_10MOONS_UT330		5
+#define TM6000_BOARD_ADSTECH_DUAL_TV		6
+#define TM6000_BOARD_FREECOM_AND_SIMILAR	7
+#define TM6000_BOARD_ADSTECH_MINI_DUAL_TV	8
+#define TM6010_BOARD_HAUPPAUGE_900H		9
+#define TM6010_BOARD_BEHOLD_WANDER		10
+#define TM6010_BOARD_BEHOLD_VOYAGER		11
+#define TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE	12
+#define TM6010_BOARD_TWINHAN_TU501		13
+#define TM6010_BOARD_BEHOLD_WANDER_LITE		14
+#define TM6010_BOARD_BEHOLD_VOYAGER_LITE	15
+#define TM5600_BOARD_TERRATEC_GRABSTER		16
+
+#define is_generic(model) ((model == TM6000_BOARD_UNKNOWN) || \
+			   (model == TM5600_BOARD_GENERIC) || \
+			   (model == TM6000_BOARD_GENERIC) || \
+			   (model == TM6010_BOARD_GENERIC))
+
+#define TM6000_MAXBOARDS        16
+static unsigned int card[]     = {[0 ... (TM6000_MAXBOARDS - 1)] = UNSET };
+
+module_param_array(card,  int, NULL, 0444);
+
+static unsigned long tm6000_devused;
+
+
+struct tm6000_board {
+	char            *name;
+	char		eename[16];		/* EEPROM name */
+	unsigned	eename_size;		/* size of EEPROM name */
+	unsigned	eename_pos;		/* Position where it appears at ROM */
+
+	struct tm6000_capabilities caps;
+
+	enum		tm6000_devtype type;	/* variant of the chipset */
+	int             tuner_type;     /* type of the tuner */
+	int             tuner_addr;     /* tuner address */
+	int             demod_addr;     /* demodulator address */
+
+	struct tm6000_gpio gpio;
+
+	struct tm6000_input	vinput[3];
+	struct tm6000_input	rinput;
+
+	char		*ir_codes;
+};
+
+static struct tm6000_board tm6000_boards[] = {
+	[TM6000_BOARD_UNKNOWN] = {
+		.name         = "Unknown tm6000 video grabber",
+		.caps = {
+			.has_tuner	= 1,
+			.has_eeprom	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM5600_BOARD_GENERIC] = {
+		.name         = "Generic tm5600 board",
+		.type         = TM5600,
+		.tuner_type   = TUNER_XC2028,
+		.tuner_addr   = 0xc2 >> 1,
+		.caps = {
+			.has_tuner	= 1,
+			.has_eeprom	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6000_BOARD_GENERIC] = {
+		.name         = "Generic tm6000 board",
+		.tuner_type   = TUNER_XC2028,
+		.tuner_addr   = 0xc2 >> 1,
+		.caps = {
+			.has_tuner	= 1,
+			.has_eeprom	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6010_BOARD_GENERIC] = {
+		.name         = "Generic tm6010 board",
+		.type         = TM6010,
+		.tuner_type   = TUNER_XC2028,
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.caps = {
+			.has_tuner	= 1,
+			.has_dvb	= 1,
+			.has_zl10353	= 1,
+			.has_eeprom	= 1,
+			.has_remote	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_2,
+			.tuner_on	= TM6010_GPIO_3,
+			.demod_reset	= TM6010_GPIO_1,
+			.demod_on	= TM6010_GPIO_4,
+			.power_led	= TM6010_GPIO_7,
+			.dvb_led	= TM6010_GPIO_5,
+			.ir		= TM6010_GPIO_0,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM5600_BOARD_10MOONS_UT821] = {
+		.name         = "10Moons UT 821",
+		.tuner_type   = TUNER_XC2028,
+		.eename       = { '1', '0', 'M', 'O', 'O', 'N', 'S', '5', '6', '0', '0', 0xff, 0x45, 0x5b},
+		.eename_size  = 14,
+		.eename_pos   = 0x14,
+		.type         = TM5600,
+		.tuner_addr   = 0xc2 >> 1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_eeprom   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM5600_BOARD_10MOONS_UT330] = {
+		.name         = "10Moons UT 330",
+		.tuner_type   = TUNER_PHILIPS_FQ1216AME_MK4,
+		.tuner_addr   = 0xc8 >> 1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 0,
+			.has_zl10353  = 0,
+			.has_eeprom   = 1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6000_BOARD_ADSTECH_DUAL_TV] = {
+		.name         = "ADSTECH Dual TV USB",
+		.tuner_type   = TUNER_XC2028,
+		.tuner_addr   = 0xc8 >> 1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_tda9874  = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 1,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6000_BOARD_FREECOM_AND_SIMILAR] = {
+		.name         = "Freecom Hybrid Stick / Moka DVB-T Receiver Dual",
+		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 0,
+			.has_remote   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_4,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6000_BOARD_ADSTECH_MINI_DUAL_TV] = {
+		.name         = "ADSTECH Mini Dual TV USB",
+		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
+		.tuner_addr   = 0xc8 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 0,
+		},
+		.gpio = {
+			.tuner_reset	= TM6000_GPIO_4,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6010_BOARD_HAUPPAUGE_900H] = {
+		.name         = "Hauppauge WinTV HVR-900H / WinTV USB2-Stick",
+		.eename       = { 'H', 0, 'V', 0, 'R', 0, '9', 0, '0', 0, '0', 0, 'H', 0 },
+		.eename_size  = 14,
+		.eename_pos   = 0x42,
+		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.ir_codes = RC_MAP_HAUPPAUGE,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 1,
+			.has_remote   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_2,
+			.tuner_on	= TM6010_GPIO_3,
+			.demod_reset	= TM6010_GPIO_1,
+			.demod_on	= TM6010_GPIO_4,
+			.power_led	= TM6010_GPIO_7,
+			.dvb_led	= TM6010_GPIO_5,
+			.ir		= TM6010_GPIO_0,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6010_BOARD_BEHOLD_WANDER] = {
+		.name         = "Beholder Wander DVB-T/TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner      = 1,
+			.has_dvb        = 1,
+			.has_zl10353    = 1,
+			.has_eeprom     = 1,
+			.has_remote     = 1,
+			.has_radio	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.demod_reset	= TM6010_GPIO_1,
+			.power_led	= TM6010_GPIO_6,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+		.rinput = {
+			.type	= TM6000_INPUT_RADIO,
+			.amux	= TM6000_AMUX_ADC1,
+		},
+	},
+	[TM6010_BOARD_BEHOLD_VOYAGER] = {
+		.name         = "Beholder Voyager TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner      = 1,
+			.has_dvb        = 0,
+			.has_zl10353    = 0,
+			.has_eeprom     = 1,
+			.has_remote     = 1,
+			.has_radio	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.power_led	= TM6010_GPIO_6,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+		.rinput = {
+			.type	= TM6000_INPUT_RADIO,
+			.amux	= TM6000_AMUX_ADC1,
+		},
+	},
+	[TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE] = {
+		.name         = "Terratec Cinergy Hybrid XE / Cinergy Hybrid-Stick",
+		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 1,
+			.has_remote   = 1,
+			.has_radio    = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_2,
+			.tuner_on	= TM6010_GPIO_3,
+			.demod_reset	= TM6010_GPIO_1,
+			.demod_on	= TM6010_GPIO_4,
+			.power_led	= TM6010_GPIO_7,
+			.dvb_led	= TM6010_GPIO_5,
+			.ir		= TM6010_GPIO_0,
+		},
+		.ir_codes = RC_MAP_NEC_TERRATEC_CINERGY_XS,
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+		.rinput = {
+			.type = TM6000_INPUT_RADIO,
+			.amux = TM6000_AMUX_SIF1,
+		},
+	},
+	[TM5600_BOARD_TERRATEC_GRABSTER] = {
+		.name         = "Terratec Grabster AV 150/250 MX",
+		.type         = TM5600,
+		.tuner_type   = TUNER_ABSENT,
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_ADC1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6010_BOARD_TWINHAN_TU501] = {
+		.name         = "Twinhan TU501(704D1)",
+		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 1,
+			.has_remote   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_2,
+			.tuner_on	= TM6010_GPIO_3,
+			.demod_reset	= TM6010_GPIO_1,
+			.demod_on	= TM6010_GPIO_4,
+			.power_led	= TM6010_GPIO_7,
+			.dvb_led	= TM6010_GPIO_5,
+			.ir		= TM6010_GPIO_0,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			}, {
+			.type	= TM6000_INPUT_COMPOSITE1,
+			.vmux	= TM6000_VMUX_VIDEO_A,
+			.amux	= TM6000_AMUX_ADC2,
+			}, {
+			.type	= TM6000_INPUT_SVIDEO,
+			.vmux	= TM6000_VMUX_VIDEO_AB,
+			.amux	= TM6000_AMUX_ADC2,
+			},
+		},
+	},
+	[TM6010_BOARD_BEHOLD_WANDER_LITE] = {
+		.name         = "Beholder Wander Lite DVB-T/TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner      = 1,
+			.has_dvb        = 1,
+			.has_zl10353    = 1,
+			.has_eeprom     = 1,
+			.has_remote     = 0,
+			.has_radio	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.demod_reset	= TM6010_GPIO_1,
+			.power_led	= TM6010_GPIO_6,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			},
+		},
+		.rinput = {
+			.type	= TM6000_INPUT_RADIO,
+			.amux	= TM6000_AMUX_ADC1,
+		},
+	},
+	[TM6010_BOARD_BEHOLD_VOYAGER_LITE] = {
+		.name         = "Beholder Voyager Lite TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.type         = TM6010,
+		.caps = {
+			.has_tuner      = 1,
+			.has_dvb        = 0,
+			.has_zl10353    = 0,
+			.has_eeprom     = 1,
+			.has_remote     = 0,
+			.has_radio	= 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.power_led	= TM6010_GPIO_6,
+		},
+		.vinput = { {
+			.type	= TM6000_INPUT_TV,
+			.vmux	= TM6000_VMUX_VIDEO_B,
+			.amux	= TM6000_AMUX_SIF1,
+			},
+		},
+		.rinput = {
+			.type	= TM6000_INPUT_RADIO,
+			.amux	= TM6000_AMUX_ADC1,
+		},
+	},
+};
+
+/* table of devices that work with this driver */
+static const struct usb_device_id tm6000_id_table[] = {
+	{ USB_DEVICE(0x6000, 0x0001), .driver_info = TM5600_BOARD_GENERIC },
+	{ USB_DEVICE(0x6000, 0x0002), .driver_info = TM6010_BOARD_GENERIC },
+	{ USB_DEVICE(0x06e1, 0xf332), .driver_info = TM6000_BOARD_ADSTECH_DUAL_TV },
+	{ USB_DEVICE(0x14aa, 0x0620), .driver_info = TM6000_BOARD_FREECOM_AND_SIMILAR },
+	{ USB_DEVICE(0x06e1, 0xb339), .driver_info = TM6000_BOARD_ADSTECH_MINI_DUAL_TV },
+	{ USB_DEVICE(0x2040, 0x6600), .driver_info = TM6010_BOARD_HAUPPAUGE_900H },
+	{ USB_DEVICE(0x2040, 0x6601), .driver_info = TM6010_BOARD_HAUPPAUGE_900H },
+	{ USB_DEVICE(0x2040, 0x6610), .driver_info = TM6010_BOARD_HAUPPAUGE_900H },
+	{ USB_DEVICE(0x2040, 0x6611), .driver_info = TM6010_BOARD_HAUPPAUGE_900H },
+	{ USB_DEVICE(0x6000, 0xdec0), .driver_info = TM6010_BOARD_BEHOLD_WANDER },
+	{ USB_DEVICE(0x6000, 0xdec1), .driver_info = TM6010_BOARD_BEHOLD_VOYAGER },
+	{ USB_DEVICE(0x0ccd, 0x0086), .driver_info = TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE },
+	{ USB_DEVICE(0x0ccd, 0x00A5), .driver_info = TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE },
+	{ USB_DEVICE(0x0ccd, 0x0079), .driver_info = TM5600_BOARD_TERRATEC_GRABSTER },
+	{ USB_DEVICE(0x13d3, 0x3240), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
+	{ USB_DEVICE(0x13d3, 0x3241), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
+	{ USB_DEVICE(0x13d3, 0x3243), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
+	{ USB_DEVICE(0x13d3, 0x3264), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
+	{ USB_DEVICE(0x6000, 0xdec2), .driver_info = TM6010_BOARD_BEHOLD_WANDER_LITE },
+	{ USB_DEVICE(0x6000, 0xdec3), .driver_info = TM6010_BOARD_BEHOLD_VOYAGER_LITE },
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, tm6000_id_table);
+
+/* Control power led for show some activity */
+void tm6000_flash_led(struct tm6000_core *dev, u8 state)
+{
+	/* Power LED unconfigured */
+	if (!dev->gpio.power_led)
+		return;
+
+	/* ON Power LED */
+	if (state) {
+		switch (dev->model) {
+		case TM6010_BOARD_HAUPPAUGE_900H:
+		case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+		case TM6010_BOARD_TWINHAN_TU501:
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x00);
+			break;
+		case TM6010_BOARD_BEHOLD_WANDER:
+		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x01);
+			break;
+		}
+	}
+	/* OFF Power LED */
+	else {
+		switch (dev->model) {
+		case TM6010_BOARD_HAUPPAUGE_900H:
+		case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+		case TM6010_BOARD_TWINHAN_TU501:
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x01);
+			break;
+		case TM6010_BOARD_BEHOLD_WANDER:
+		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x00);
+			break;
+		}
+	}
+}
+
+/* Tuner callback to provide the proper gpio changes needed for xc5000 */
+int tm6000_xc5000_callback(void *ptr, int component, int command, int arg)
+{
+	int rc = 0;
+	struct tm6000_core *dev = ptr;
+
+	if (dev->tuner_type != TUNER_XC5000)
+		return 0;
+
+	switch (command) {
+	case XC5000_TUNER_RESET:
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+			       dev->gpio.tuner_reset, 0x01);
+		msleep(15);
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+			       dev->gpio.tuner_reset, 0x00);
+		msleep(15);
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+			       dev->gpio.tuner_reset, 0x01);
+		break;
+	}
+	return rc;
+}
+EXPORT_SYMBOL_GPL(tm6000_xc5000_callback);
+
+/* Tuner callback to provide the proper gpio changes needed for xc2028 */
+
+int tm6000_tuner_callback(void *ptr, int component, int command, int arg)
+{
+	int rc = 0;
+	struct tm6000_core *dev = ptr;
+
+	if (dev->tuner_type != TUNER_XC2028)
+		return 0;
+
+	switch (command) {
+	case XC2028_RESET_CLK:
+		tm6000_ir_wait(dev, 0);
+
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT,
+					0x02, arg);
+		msleep(10);
+		rc = tm6000_i2c_reset(dev, 10);
+		break;
+	case XC2028_TUNER_RESET:
+		/* Reset codes during load firmware */
+		switch (arg) {
+		case 0:
+			/* newer tuner can faster reset */
+			switch (dev->model) {
+			case TM5600_BOARD_10MOONS_UT821:
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x01);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       0x300, 0x01);
+				msleep(10);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x00);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       0x300, 0x00);
+				msleep(10);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x01);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       0x300, 0x01);
+				break;
+			case TM6010_BOARD_HAUPPAUGE_900H:
+			case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+			case TM6010_BOARD_TWINHAN_TU501:
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x01);
+				msleep(60);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x00);
+				msleep(75);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x01);
+				msleep(60);
+				break;
+			default:
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x00);
+				msleep(130);
+				tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+					       dev->gpio.tuner_reset, 0x01);
+				msleep(130);
+				break;
+			}
+
+			tm6000_ir_wait(dev, 1);
+			break;
+		case 1:
+			tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT,
+						0x02, 0x01);
+			msleep(10);
+			break;
+		case 2:
+			rc = tm6000_i2c_reset(dev, 100);
+			break;
+		}
+		break;
+	case XC2028_I2C_FLUSH:
+		tm6000_set_reg(dev, REQ_50_SET_START, 0, 0);
+		tm6000_set_reg(dev, REQ_51_SET_STOP, 0, 0);
+		break;
+	}
+	return rc;
+}
+EXPORT_SYMBOL_GPL(tm6000_tuner_callback);
+
+int tm6000_cards_setup(struct tm6000_core *dev)
+{
+	/*
+	 * Board-specific initialization sequence. Handles all GPIO
+	 * initialization sequences that are board-specific.
+	 * Up to now, all found devices use GPIO1 and GPIO4 at the same way.
+	 * Probably, they're all based on some reference device. Due to that,
+	 * there's a common routine at the end to handle those GPIO's. Devices
+	 * that use different pinups or init sequences can just return at
+	 * the board-specific session.
+	 */
+	switch (dev->model) {
+	case TM6010_BOARD_HAUPPAUGE_900H:
+	case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+	case TM6010_BOARD_TWINHAN_TU501:
+	case TM6010_BOARD_GENERIC:
+		/* Turn xceive 3028 on */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.tuner_on, 0x01);
+		msleep(15);
+		/* Turn zarlink zl10353 on */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_on, 0x00);
+		msleep(15);
+		/* Reset zarlink zl10353 */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_reset, 0x00);
+		msleep(50);
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_reset, 0x01);
+		msleep(15);
+		/* Turn zarlink zl10353 off */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_on, 0x01);
+		msleep(15);
+		/* ir ? */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.ir, 0x01);
+		msleep(15);
+		/* Power led on (blue) */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.power_led, 0x00);
+		msleep(15);
+		/* DVB led off (orange) */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.dvb_led, 0x01);
+		msleep(15);
+		/* Turn zarlink zl10353 on */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_on, 0x00);
+		msleep(15);
+		break;
+	case TM6010_BOARD_BEHOLD_WANDER:
+	case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		/* Power led on (blue) */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.power_led, 0x01);
+		msleep(15);
+		/* Reset zarlink zl10353 */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_reset, 0x00);
+		msleep(50);
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.demod_reset, 0x01);
+		msleep(15);
+		break;
+	case TM6010_BOARD_BEHOLD_VOYAGER:
+	case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
+		/* Power led on (blue) */
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.power_led, 0x01);
+		msleep(15);
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * Default initialization. Most of the devices seem to use GPIO1
+	 * and GPIO4.on the same way, so, this handles the common sequence
+	 * used by most devices.
+	 * If a device uses a different sequence or different GPIO pins for
+	 * reset, just add the code at the board-specific part
+	 */
+
+	if (dev->gpio.tuner_reset) {
+		int rc;
+		int i;
+
+		for (i = 0; i < 2; i++) {
+			rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+						dev->gpio.tuner_reset, 0x00);
+			if (rc < 0) {
+				printk(KERN_ERR "Error %i doing tuner reset\n", rc);
+				return rc;
+			}
+
+			msleep(10); /* Just to be conservative */
+			rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+						dev->gpio.tuner_reset, 0x01);
+			if (rc < 0) {
+				printk(KERN_ERR "Error %i doing tuner reset\n", rc);
+				return rc;
+			}
+		}
+	} else {
+		printk(KERN_ERR "Tuner reset is not configured\n");
+		return -1;
+	}
+
+	msleep(50);
+
+	return 0;
+};
+
+static void tm6000_config_tuner(struct tm6000_core *dev)
+{
+	struct tuner_setup tun_setup;
+
+	/* Load tuner module */
+	v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		"tuner", dev->tuner_addr, NULL);
+
+	memset(&tun_setup, 0, sizeof(tun_setup));
+	tun_setup.type = dev->tuner_type;
+	tun_setup.addr = dev->tuner_addr;
+
+	tun_setup.mode_mask = 0;
+	if (dev->caps.has_tuner)
+		tun_setup.mode_mask |= (T_ANALOG_TV | T_RADIO);
+
+	switch (dev->tuner_type) {
+	case TUNER_XC2028:
+		tun_setup.tuner_callback = tm6000_tuner_callback;
+		break;
+	case TUNER_XC5000:
+		tun_setup.tuner_callback = tm6000_xc5000_callback;
+		break;
+	}
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_type_addr, &tun_setup);
+
+	switch (dev->tuner_type) {
+	case TUNER_XC2028: {
+		struct v4l2_priv_tun_config xc2028_cfg;
+		struct xc2028_ctrl ctl;
+
+		memset(&xc2028_cfg, 0, sizeof(xc2028_cfg));
+		memset(&ctl, 0, sizeof(ctl));
+
+		ctl.demod = XC3028_FE_ZARLINK456;
+
+		xc2028_cfg.tuner = TUNER_XC2028;
+		xc2028_cfg.priv  = &ctl;
+
+		switch (dev->model) {
+		case TM6010_BOARD_HAUPPAUGE_900H:
+		case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+		case TM6010_BOARD_TWINHAN_TU501:
+			ctl.max_len = 80;
+			ctl.fname = "xc3028L-v36.fw";
+			break;
+		default:
+			if (dev->dev_type == TM6010)
+				ctl.fname = "xc3028-v27.fw";
+			else
+				ctl.fname = "xc3028-v24.fw";
+		}
+
+		printk(KERN_INFO "Setting firmware parameters for xc2028\n");
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config,
+				     &xc2028_cfg);
+
+		}
+		break;
+	case TUNER_XC5000:
+		{
+		struct v4l2_priv_tun_config  xc5000_cfg;
+		struct xc5000_config ctl = {
+			.i2c_address = dev->tuner_addr,
+			.if_khz      = 4570,
+			.radio_input = XC5000_RADIO_FM1_MONO,
+			};
+
+		xc5000_cfg.tuner = TUNER_XC5000;
+		xc5000_cfg.priv  = &ctl;
+
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config,
+				     &xc5000_cfg);
+		}
+		break;
+	default:
+		printk(KERN_INFO "Unknown tuner type. Tuner is not configured.\n");
+		break;
+	}
+}
+
+static int fill_board_specific_data(struct tm6000_core *dev)
+{
+	int rc;
+
+	dev->dev_type   = tm6000_boards[dev->model].type;
+	dev->tuner_type = tm6000_boards[dev->model].tuner_type;
+	dev->tuner_addr = tm6000_boards[dev->model].tuner_addr;
+
+	dev->gpio = tm6000_boards[dev->model].gpio;
+
+	dev->ir_codes = tm6000_boards[dev->model].ir_codes;
+
+	dev->demod_addr = tm6000_boards[dev->model].demod_addr;
+
+	dev->caps = tm6000_boards[dev->model].caps;
+
+	dev->vinput[0] = tm6000_boards[dev->model].vinput[0];
+	dev->vinput[1] = tm6000_boards[dev->model].vinput[1];
+	dev->vinput[2] = tm6000_boards[dev->model].vinput[2];
+	dev->rinput = tm6000_boards[dev->model].rinput;
+
+	/* setup per-model quirks */
+	switch (dev->model) {
+	case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+	case TM6010_BOARD_HAUPPAUGE_900H:
+		dev->quirks |= TM6000_QUIRK_NO_USB_DELAY;
+		break;
+
+	default:
+		break;
+	}
+
+	/* initialize hardware */
+	rc = tm6000_init(dev);
+	if (rc < 0)
+		return rc;
+
+	return v4l2_device_register(&dev->udev->dev, &dev->v4l2_dev);
+}
+
+
+static void use_alternative_detection_method(struct tm6000_core *dev)
+{
+	int i, model = -1;
+
+	if (!dev->eedata_size)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(tm6000_boards); i++) {
+		if (!tm6000_boards[i].eename_size)
+			continue;
+		if (dev->eedata_size < tm6000_boards[i].eename_pos +
+				       tm6000_boards[i].eename_size)
+			continue;
+
+		if (!memcmp(&dev->eedata[tm6000_boards[i].eename_pos],
+			    tm6000_boards[i].eename,
+			    tm6000_boards[i].eename_size)) {
+			model = i;
+			break;
+		}
+	}
+	if (model < 0) {
+		printk(KERN_INFO "Device has eeprom but is currently unknown\n");
+		return;
+	}
+
+	dev->model = model;
+
+	printk(KERN_INFO "Device identified via eeprom as %s (type = %d)\n",
+	       tm6000_boards[model].name, model);
+}
+
+#if defined(CONFIG_MODULES) && defined(MODULE)
+static void request_module_async(struct work_struct *work)
+{
+	struct tm6000_core *dev = container_of(work, struct tm6000_core,
+					       request_module_wk);
+
+	request_module("tm6000-alsa");
+
+	if (dev->caps.has_dvb)
+		request_module("tm6000-dvb");
+}
+
+static void request_modules(struct tm6000_core *dev)
+{
+	INIT_WORK(&dev->request_module_wk, request_module_async);
+	schedule_work(&dev->request_module_wk);
+}
+
+static void flush_request_modules(struct tm6000_core *dev)
+{
+	flush_work(&dev->request_module_wk);
+}
+#else
+#define request_modules(dev)
+#define flush_request_modules(dev)
+#endif /* CONFIG_MODULES */
+
+static int tm6000_init_dev(struct tm6000_core *dev)
+{
+	struct v4l2_frequency f;
+	int rc = 0;
+
+	mutex_init(&dev->lock);
+	mutex_lock(&dev->lock);
+
+	if (!is_generic(dev->model)) {
+		rc = fill_board_specific_data(dev);
+		if (rc < 0)
+			goto err;
+
+		/* register i2c bus */
+		rc = tm6000_i2c_register(dev);
+		if (rc < 0)
+			goto err;
+	} else {
+		/* register i2c bus */
+		rc = tm6000_i2c_register(dev);
+		if (rc < 0)
+			goto err;
+
+		use_alternative_detection_method(dev);
+
+		rc = fill_board_specific_data(dev);
+		if (rc < 0)
+			goto err;
+	}
+
+	/* Default values for STD and resolutions */
+	dev->width = 720;
+	dev->height = 480;
+	dev->norm = V4L2_STD_NTSC_M;
+
+	/* Configure tuner */
+	tm6000_config_tuner(dev);
+
+	/* Set video standard */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, dev->norm);
+
+	/* Set tuner frequency - also loads firmware on xc2028/xc3028 */
+	f.tuner = 0;
+	f.type = V4L2_TUNER_ANALOG_TV;
+	f.frequency = 3092;	/* 193.25 MHz */
+	dev->freq = f.frequency;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+
+	if (dev->caps.has_tda9874)
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+			"tvaudio", I2C_ADDR_TDA9874, NULL);
+
+	/* register and initialize V4L2 */
+	rc = tm6000_v4l2_register(dev);
+	if (rc < 0)
+		goto err;
+
+	tm6000_add_into_devlist(dev);
+	tm6000_init_extension(dev);
+
+	tm6000_ir_init(dev);
+
+	request_modules(dev);
+
+	mutex_unlock(&dev->lock);
+	return 0;
+
+err:
+	mutex_unlock(&dev->lock);
+	return rc;
+}
+
+/* high bandwidth multiplier, as encoded in highspeed endpoint descriptors */
+#define hb_mult(wMaxPacketSize) (1 + (((wMaxPacketSize) >> 11) & 0x03))
+
+static void get_max_endpoint(struct usb_device *udev,
+			     struct usb_host_interface *alt,
+			     char *msgtype,
+			     struct usb_host_endpoint *curr_e,
+			     struct tm6000_endpoint *tm_ep)
+{
+	u16 tmp = le16_to_cpu(curr_e->desc.wMaxPacketSize);
+	unsigned int size = tmp & 0x7ff;
+
+	if (udev->speed == USB_SPEED_HIGH)
+		size = size * hb_mult(tmp);
+
+	if (size > tm_ep->maxsize) {
+		tm_ep->endp = curr_e;
+		tm_ep->maxsize = size;
+		tm_ep->bInterfaceNumber = alt->desc.bInterfaceNumber;
+		tm_ep->bAlternateSetting = alt->desc.bAlternateSetting;
+
+		printk(KERN_INFO "tm6000: %s endpoint: 0x%02x (max size=%u bytes)\n",
+					msgtype, curr_e->desc.bEndpointAddress,
+					size);
+	}
+}
+
+/*
+ * tm6000_usb_probe()
+ * checks for supported devices
+ */
+static int tm6000_usb_probe(struct usb_interface *interface,
+			    const struct usb_device_id *id)
+{
+	struct usb_device *usbdev;
+	struct tm6000_core *dev;
+	int i, rc;
+	int nr = 0;
+	char *speed;
+
+	usbdev = usb_get_dev(interface_to_usbdev(interface));
+
+	/* Selects the proper interface */
+	rc = usb_set_interface(usbdev, 0, 1);
+	if (rc < 0)
+		goto report_failure;
+
+	/* Check to see next free device and mark as used */
+	nr = find_first_zero_bit(&tm6000_devused, TM6000_MAXBOARDS);
+	if (nr >= TM6000_MAXBOARDS) {
+		printk(KERN_ERR "tm6000: Supports only %i tm60xx boards.\n", TM6000_MAXBOARDS);
+		rc = -ENOMEM;
+		goto put_device;
+	}
+
+	/* Create and initialize dev struct */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		rc = -ENOMEM;
+		goto put_device;
+	}
+	spin_lock_init(&dev->slock);
+	mutex_init(&dev->usb_lock);
+
+	/* Increment usage count */
+	set_bit(nr, &tm6000_devused);
+	snprintf(dev->name, 29, "tm6000 #%d", nr);
+
+	dev->model = id->driver_info;
+	if (card[nr] < ARRAY_SIZE(tm6000_boards))
+		dev->model = card[nr];
+
+	dev->udev = usbdev;
+	dev->devno = nr;
+
+	switch (usbdev->speed) {
+	case USB_SPEED_LOW:
+		speed = "1.5";
+		break;
+	case USB_SPEED_UNKNOWN:
+	case USB_SPEED_FULL:
+		speed = "12";
+		break;
+	case USB_SPEED_HIGH:
+		speed = "480";
+		break;
+	default:
+		speed = "unknown";
+	}
+
+	/* Get endpoints */
+	for (i = 0; i < interface->num_altsetting; i++) {
+		int ep;
+
+		for (ep = 0; ep < interface->altsetting[i].desc.bNumEndpoints; ep++) {
+			struct usb_host_endpoint	*e;
+			int dir_out;
+
+			e = &interface->altsetting[i].endpoint[ep];
+
+			dir_out = ((e->desc.bEndpointAddress &
+					USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT);
+
+			printk(KERN_INFO "tm6000: alt %d, interface %i, class %i\n",
+			       i,
+			       interface->altsetting[i].desc.bInterfaceNumber,
+			       interface->altsetting[i].desc.bInterfaceClass);
+
+			switch (e->desc.bmAttributes) {
+			case USB_ENDPOINT_XFER_BULK:
+				if (!dir_out) {
+					get_max_endpoint(usbdev,
+							 &interface->altsetting[i],
+							 "Bulk IN", e,
+							 &dev->bulk_in);
+				} else {
+					get_max_endpoint(usbdev,
+							 &interface->altsetting[i],
+							 "Bulk OUT", e,
+							 &dev->bulk_out);
+				}
+				break;
+			case USB_ENDPOINT_XFER_ISOC:
+				if (!dir_out) {
+					get_max_endpoint(usbdev,
+							 &interface->altsetting[i],
+							 "ISOC IN", e,
+							 &dev->isoc_in);
+				} else {
+					get_max_endpoint(usbdev,
+							 &interface->altsetting[i],
+							 "ISOC OUT", e,
+							 &dev->isoc_out);
+				}
+				break;
+			case USB_ENDPOINT_XFER_INT:
+				if (!dir_out) {
+					get_max_endpoint(usbdev,
+							&interface->altsetting[i],
+							"INT IN", e,
+							&dev->int_in);
+				} else {
+					get_max_endpoint(usbdev,
+							&interface->altsetting[i],
+							"INT OUT", e,
+							&dev->int_out);
+				}
+				break;
+			}
+		}
+	}
+
+
+	printk(KERN_INFO "tm6000: New video device @ %s Mbps (%04x:%04x, ifnum %d)\n",
+		speed,
+		le16_to_cpu(dev->udev->descriptor.idVendor),
+		le16_to_cpu(dev->udev->descriptor.idProduct),
+		interface->altsetting->desc.bInterfaceNumber);
+
+/* check if the the device has the iso in endpoint at the correct place */
+	if (!dev->isoc_in.endp) {
+		printk(KERN_ERR "tm6000: probing error: no IN ISOC endpoint!\n");
+		rc = -ENODEV;
+		goto free_device;
+	}
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, dev);
+
+	printk(KERN_INFO "tm6000: Found %s\n", tm6000_boards[dev->model].name);
+
+	rc = tm6000_init_dev(dev);
+	if (rc < 0)
+		goto free_device;
+
+	return 0;
+
+free_device:
+	kfree(dev);
+report_failure:
+	printk(KERN_ERR "tm6000: Error %d while registering\n", rc);
+
+	clear_bit(nr, &tm6000_devused);
+put_device:
+	usb_put_dev(usbdev);
+	return rc;
+}
+
+/*
+ * tm6000_usb_disconnect()
+ * called when the device gets disconnected
+ * video device will be unregistered on v4l2_close in case it is still open
+ */
+static void tm6000_usb_disconnect(struct usb_interface *interface)
+{
+	struct tm6000_core *dev = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+
+	if (!dev)
+		return;
+
+	printk(KERN_INFO "tm6000: disconnecting %s\n", dev->name);
+
+	flush_request_modules(dev);
+
+	tm6000_ir_fini(dev);
+
+	if (dev->gpio.power_led) {
+		switch (dev->model) {
+		case TM6010_BOARD_HAUPPAUGE_900H:
+		case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+		case TM6010_BOARD_TWINHAN_TU501:
+			/* Power led off */
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x01);
+			msleep(15);
+			break;
+		case TM6010_BOARD_BEHOLD_WANDER:
+		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
+			/* Power led off */
+			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+				dev->gpio.power_led, 0x00);
+			msleep(15);
+			break;
+		}
+	}
+	tm6000_v4l2_unregister(dev);
+
+	tm6000_i2c_unregister(dev);
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	dev->state |= DEV_DISCONNECTED;
+
+	usb_put_dev(dev->udev);
+
+	tm6000_close_extension(dev);
+	tm6000_remove_from_devlist(dev);
+
+	clear_bit(dev->devno, &tm6000_devused);
+	kfree(dev);
+}
+
+static struct usb_driver tm6000_usb_driver = {
+		.name = "tm6000",
+		.probe = tm6000_usb_probe,
+		.disconnect = tm6000_usb_disconnect,
+		.id_table = tm6000_id_table,
+};
+
+module_usb_driver(tm6000_usb_driver);
+
+MODULE_DESCRIPTION("Trident TVMaster TM5600/TM6000/TM6010 USB2 adapter");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-core.c b/drivers/staging/media/deprecated/tm6000/tm6000-core.c
new file mode 100644
index 000000000..5c8cbc5d6
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-core.c
@@ -0,0 +1,916 @@
+// SPDX-License-Identifier: GPL-2.0
+// tm6000-core.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+//
+// Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+//
+// Copyright (c) 2007 Michel Ludwig <michel.ludwig@gmail.com>
+//     - DVB-T support
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+#include "tm6000.h"
+#include "tm6000-regs.h"
+#include <media/v4l2-common.h>
+#include <media/tuner.h>
+
+#define USB_TIMEOUT	(5 * HZ) /* ms */
+
+int tm6000_read_write_usb(struct tm6000_core *dev, u8 req_type, u8 req,
+			  u16 value, u16 index, u8 *buf, u16 len)
+{
+	int          ret, i;
+	unsigned int pipe;
+	u8	     *data = NULL;
+	int delay = 5000;
+
+	if (len) {
+		data = kzalloc(len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+	}
+
+	mutex_lock(&dev->usb_lock);
+
+	if (req_type & USB_DIR_IN)
+		pipe = usb_rcvctrlpipe(dev->udev, 0);
+	else {
+		pipe = usb_sndctrlpipe(dev->udev, 0);
+		memcpy(data, buf, len);
+	}
+
+	if (tm6000_debug & V4L2_DEBUG_I2C) {
+		printk(KERN_DEBUG "(dev %p, pipe %08x): ", dev->udev, pipe);
+
+		printk(KERN_CONT "%s: %02x %02x %02x %02x %02x %02x %02x %02x ",
+			(req_type & USB_DIR_IN) ? " IN" : "OUT",
+			req_type, req, value&0xff, value>>8, index&0xff,
+			index>>8, len&0xff, len>>8);
+
+		if (!(req_type & USB_DIR_IN)) {
+			printk(KERN_CONT ">>> ");
+			for (i = 0; i < len; i++)
+				printk(KERN_CONT " %02x", buf[i]);
+			printk(KERN_CONT "\n");
+		}
+	}
+
+	ret = usb_control_msg(dev->udev, pipe, req, req_type, value, index,
+			      data, len, USB_TIMEOUT);
+
+	if (req_type &  USB_DIR_IN)
+		memcpy(buf, data, len);
+
+	if (tm6000_debug & V4L2_DEBUG_I2C) {
+		if (ret < 0) {
+			if (req_type &  USB_DIR_IN)
+				printk(KERN_DEBUG "<<< (len=%d)\n", len);
+
+			printk(KERN_CONT "%s: Error #%d\n", __func__, ret);
+		} else if (req_type &  USB_DIR_IN) {
+			printk(KERN_CONT "<<< ");
+			for (i = 0; i < len; i++)
+				printk(KERN_CONT " %02x", buf[i]);
+			printk(KERN_CONT "\n");
+		}
+	}
+
+	kfree(data);
+
+	if (dev->quirks & TM6000_QUIRK_NO_USB_DELAY)
+		delay = 0;
+
+	if (req == REQ_16_SET_GET_I2C_WR1_RDN && !(req_type & USB_DIR_IN)) {
+		unsigned int tsleep;
+		/* Calculate delay time, 14000us for 64 bytes */
+		tsleep = (len * 200) + 200;
+		if (tsleep < delay)
+			tsleep = delay;
+		usleep_range(tsleep, tsleep + 1000);
+	}
+	else if (delay)
+		usleep_range(delay, delay + 1000);
+
+	mutex_unlock(&dev->usb_lock);
+	return ret;
+}
+
+int tm6000_set_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index)
+{
+	return
+		tm6000_read_write_usb(dev, USB_DIR_OUT | USB_TYPE_VENDOR,
+				      req, value, index, NULL, 0);
+}
+EXPORT_SYMBOL_GPL(tm6000_set_reg);
+
+int tm6000_get_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index)
+{
+	int rc;
+	u8 buf[1];
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR, req,
+					value, index, buf, 1);
+
+	if (rc < 0)
+		return rc;
+
+	return *buf;
+}
+EXPORT_SYMBOL_GPL(tm6000_get_reg);
+
+int tm6000_set_reg_mask(struct tm6000_core *dev, u8 req, u16 value,
+						u16 index, u16 mask)
+{
+	int rc;
+	u8 buf[1];
+	u8 new_index;
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR, req,
+					value, 0, buf, 1);
+
+	if (rc < 0)
+		return rc;
+
+	new_index = (buf[0] & ~mask) | (index & mask);
+
+	if (new_index == buf[0])
+		return 0;
+
+	return tm6000_read_write_usb(dev, USB_DIR_OUT | USB_TYPE_VENDOR,
+				      req, value, new_index, NULL, 0);
+}
+EXPORT_SYMBOL_GPL(tm6000_set_reg_mask);
+
+int tm6000_get_reg16(struct tm6000_core *dev, u8 req, u16 value, u16 index)
+{
+	int rc;
+	u8 buf[2];
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR, req,
+					value, index, buf, 2);
+
+	if (rc < 0)
+		return rc;
+
+	return buf[1]|buf[0]<<8;
+}
+
+int tm6000_get_reg32(struct tm6000_core *dev, u8 req, u16 value, u16 index)
+{
+	int rc;
+	u8 buf[4];
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR, req,
+					value, index, buf, 4);
+
+	if (rc < 0)
+		return rc;
+
+	return buf[3] | buf[2] << 8 | buf[1] << 16 | buf[0] << 24;
+}
+
+int tm6000_i2c_reset(struct tm6000_core *dev, u16 tsleep)
+{
+	int rc;
+
+	rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, TM6000_GPIO_CLK, 0);
+	if (rc < 0)
+		return rc;
+
+	msleep(tsleep);
+
+	rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, TM6000_GPIO_CLK, 1);
+	msleep(tsleep);
+
+	return rc;
+}
+
+void tm6000_set_fourcc_format(struct tm6000_core *dev)
+{
+	if (dev->dev_type == TM6010) {
+		int val;
+
+		val = tm6000_get_reg(dev, TM6010_REQ07_RCC_ACTIVE_IF, 0) & 0xfc;
+		if (dev->fourcc == V4L2_PIX_FMT_UYVY)
+			tm6000_set_reg(dev, TM6010_REQ07_RCC_ACTIVE_IF, val);
+		else
+			tm6000_set_reg(dev, TM6010_REQ07_RCC_ACTIVE_IF, val | 1);
+	} else {
+		if (dev->fourcc == V4L2_PIX_FMT_UYVY)
+			tm6000_set_reg(dev, TM6010_REQ07_RC1_TRESHOLD, 0xd0);
+		else
+			tm6000_set_reg(dev, TM6010_REQ07_RC1_TRESHOLD, 0x90);
+	}
+}
+
+static void tm6000_set_vbi(struct tm6000_core *dev)
+{
+	/*
+	 * FIXME:
+	 * VBI lines and start/end are different between 60Hz and 50Hz
+	 * So, it is very likely that we need to change the config to
+	 * something that takes it into account, doing something different
+	 * if (dev->norm & V4L2_STD_525_60)
+	 */
+
+	if (dev->dev_type == TM6010) {
+		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x01);
+		tm6000_set_reg(dev, TM6010_REQ07_R41_TELETEXT_VBI_CODE1, 0x27);
+		tm6000_set_reg(dev, TM6010_REQ07_R42_VBI_DATA_HIGH_LEVEL, 0x55);
+		tm6000_set_reg(dev, TM6010_REQ07_R43_VBI_DATA_TYPE_LINE7, 0x66);
+		tm6000_set_reg(dev, TM6010_REQ07_R44_VBI_DATA_TYPE_LINE8, 0x66);
+		tm6000_set_reg(dev, TM6010_REQ07_R45_VBI_DATA_TYPE_LINE9, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R46_VBI_DATA_TYPE_LINE10, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R47_VBI_DATA_TYPE_LINE11, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R48_VBI_DATA_TYPE_LINE12, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R49_VBI_DATA_TYPE_LINE13, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4A_VBI_DATA_TYPE_LINE14, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4B_VBI_DATA_TYPE_LINE15, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4C_VBI_DATA_TYPE_LINE16, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4D_VBI_DATA_TYPE_LINE17, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4E_VBI_DATA_TYPE_LINE18, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R4F_VBI_DATA_TYPE_LINE19, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R50_VBI_DATA_TYPE_LINE20, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R51_VBI_DATA_TYPE_LINE21, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R52_VBI_DATA_TYPE_LINE22, 0x66);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R53_VBI_DATA_TYPE_LINE23, 0x00);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R54_VBI_DATA_TYPE_RLINES, 0x00);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R55_VBI_LOOP_FILTER_GAIN, 0x01);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R56_VBI_LOOP_FILTER_I_GAIN, 0x00);
+		tm6000_set_reg(dev,
+			TM6010_REQ07_R57_VBI_LOOP_FILTER_P_GAIN, 0x02);
+		tm6000_set_reg(dev, TM6010_REQ07_R58_VBI_CAPTION_DTO1, 0x35);
+		tm6000_set_reg(dev, TM6010_REQ07_R59_VBI_CAPTION_DTO0, 0xa0);
+		tm6000_set_reg(dev, TM6010_REQ07_R5A_VBI_TELETEXT_DTO1, 0x11);
+		tm6000_set_reg(dev, TM6010_REQ07_R5B_VBI_TELETEXT_DTO0, 0x4c);
+		tm6000_set_reg(dev, TM6010_REQ07_R40_TELETEXT_VBI_CODE0, 0x01);
+		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x00);
+	}
+}
+
+int tm6000_init_analog_mode(struct tm6000_core *dev)
+{
+	struct v4l2_frequency f;
+
+	if (dev->dev_type == TM6010) {
+		u8 active = TM6010_REQ07_RCC_ACTIVE_IF_AUDIO_ENABLE;
+
+		if (!dev->radio)
+			active |= TM6010_REQ07_RCC_ACTIVE_IF_VIDEO_ENABLE;
+
+		/* Enable video and audio */
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RCC_ACTIVE_IF,
+							active, 0x60);
+		/* Disable TS input */
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE,
+							0x00, 0x40);
+	} else {
+		/* Enables soft reset */
+		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x01);
+
+		if (dev->scaler)
+			/* Disable Hfilter and Enable TS Drop err */
+			tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x20);
+		else	/* Enable Hfilter and disable TS Drop err */
+			tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x80);
+
+		tm6000_set_reg(dev, TM6010_REQ07_RC3_HSTART1, 0x88);
+		tm6000_set_reg(dev, TM6000_REQ07_RDA_CLK_SEL, 0x23);
+		tm6000_set_reg(dev, TM6010_REQ07_RD1_ADDR_FOR_REQ1, 0xc0);
+		tm6000_set_reg(dev, TM6010_REQ07_RD2_ADDR_FOR_REQ2, 0xd8);
+		tm6000_set_reg(dev, TM6010_REQ07_RD6_ENDP_REQ1_REQ2, 0x06);
+		tm6000_set_reg(dev, TM6000_REQ07_RDF_PWDOWN_ACLK, 0x1f);
+
+		/* AP Software reset */
+		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x08);
+		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x00);
+
+		tm6000_set_fourcc_format(dev);
+
+		/* Disables soft reset */
+		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x00);
+	}
+	msleep(20);
+
+	/* Tuner firmware can now be loaded */
+
+	/*
+	 * FIXME: This is a hack! xc3028 "sleeps" when no channel is detected
+	 * for more than a few seconds. Not sure why, as this behavior does
+	 * not happen on other devices with xc3028. So, I suspect that it
+	 * is yet another bug at tm6000. After start sleeping, decoding
+	 * doesn't start automatically. Instead, it requires some
+	 * I2C commands to wake it up. As we want to have image at the
+	 * beginning, we needed to add this hack. The better would be to
+	 * discover some way to make tm6000 to wake up without this hack.
+	 */
+	f.frequency = dev->freq;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+
+	msleep(100);
+	tm6000_set_standard(dev);
+	tm6000_set_vbi(dev);
+	tm6000_set_audio_bitrate(dev, 48000);
+
+	/* switch dvb led off */
+	if (dev->gpio.dvb_led) {
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+			dev->gpio.dvb_led, 0x01);
+	}
+
+	return 0;
+}
+
+int tm6000_init_digital_mode(struct tm6000_core *dev)
+{
+	if (dev->dev_type == TM6010) {
+		/* Disable video and audio */
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RCC_ACTIVE_IF,
+				0x00, 0x60);
+		/* Enable TS input */
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE,
+				0x40, 0x40);
+		/* all power down, but not the digital data port */
+		tm6000_set_reg(dev, TM6010_REQ07_RFE_POWER_DOWN, 0x28);
+		tm6000_set_reg(dev, TM6010_REQ08_RE2_POWER_DOWN_CTRL1, 0xfc);
+		tm6000_set_reg(dev, TM6010_REQ08_RE6_POWER_DOWN_CTRL2, 0xff);
+	} else  {
+		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x08);
+		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x01);
+		tm6000_set_reg(dev, TM6000_REQ07_RDF_PWDOWN_ACLK, 0x08);
+		tm6000_set_reg(dev, TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x0c);
+		tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0xff);
+		tm6000_set_reg(dev, TM6000_REQ07_REB_VADC_AADC_MODE, 0xd8);
+		tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x40);
+		tm6000_set_reg(dev, TM6010_REQ07_RC1_TRESHOLD, 0xd0);
+		tm6000_set_reg(dev, TM6010_REQ07_RC3_HSTART1, 0x09);
+		tm6000_set_reg(dev, TM6000_REQ07_RDA_CLK_SEL, 0x37);
+		tm6000_set_reg(dev, TM6010_REQ07_RD1_ADDR_FOR_REQ1, 0xd8);
+		tm6000_set_reg(dev, TM6010_REQ07_RD2_ADDR_FOR_REQ2, 0xc0);
+		tm6000_set_reg(dev, TM6010_REQ07_RD6_ENDP_REQ1_REQ2, 0x60);
+
+		tm6000_set_reg(dev, TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x0c);
+		tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0xff);
+		tm6000_set_reg(dev, TM6000_REQ07_REB_VADC_AADC_MODE, 0x08);
+		msleep(50);
+
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 0x0020, 0x00);
+		msleep(50);
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 0x0020, 0x01);
+		msleep(50);
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 0x0020, 0x00);
+		msleep(100);
+	}
+
+	/* switch dvb led on */
+	if (dev->gpio.dvb_led) {
+		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+			dev->gpio.dvb_led, 0x00);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(tm6000_init_digital_mode);
+
+struct reg_init {
+	u8 req;
+	u8 reg;
+	u8 val;
+};
+
+/* The meaning of those initializations are unknown */
+static struct reg_init tm6000_init_tab[] = {
+	/* REG  VALUE */
+	{ TM6000_REQ07_RDF_PWDOWN_ACLK, 0x1f },
+	{ TM6010_REQ07_RFF_SOFT_RESET, 0x08 },
+	{ TM6010_REQ07_RFF_SOFT_RESET, 0x00 },
+	{ TM6010_REQ07_RD5_POWERSAVE, 0x4f },
+	{ TM6000_REQ07_RDA_CLK_SEL, 0x23 },
+	{ TM6000_REQ07_RDB_OUT_SEL, 0x08 },
+	{ TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x00 },
+	{ TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x10 },
+	{ TM6000_REQ07_RE5_VADC_INP_LPF_SEL2, 0x00 },
+	{ TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0x00 },
+	{ TM6000_REQ07_REB_VADC_AADC_MODE, 0x64 },	/* 48000 bits/sample, external input */
+	{ TM6000_REQ07_REE_VADC_CTRL_SEL_CONTROL, 0xc2 },
+
+	{ TM6010_REQ07_R3F_RESET, 0x01 },		/* Start of soft reset */
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x00 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x07 },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x00 },
+	{ TM6010_REQ07_R05_NOISE_THRESHOLD, 0x64 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x01 },
+	{ TM6010_REQ07_R08_LUMA_CONTRAST_ADJ, 0x82 },
+	{ TM6010_REQ07_R09_LUMA_BRIGHTNESS_ADJ, 0x36 },
+	{ TM6010_REQ07_R0A_CHROMA_SATURATION_ADJ, 0x50 },
+	{ TM6010_REQ07_R0C_CHROMA_AGC_CONTROL, 0x6a },
+	{ TM6010_REQ07_R11_AGC_PEAK_CONTROL, 0xc9 },
+	{ TM6010_REQ07_R12_AGC_GATE_STARTH, 0x07 },
+	{ TM6010_REQ07_R13_AGC_GATE_STARTL, 0x3b },
+	{ TM6010_REQ07_R14_AGC_GATE_WIDTH, 0x47 },
+	{ TM6010_REQ07_R15_AGC_BP_DELAY, 0x6f },
+	{ TM6010_REQ07_R17_HLOOP_MAXSTATE, 0xcd },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x8b },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xa2 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe9 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R20_HSYNC_RISING_EDGE_TIME, 0x3c },
+	{ TM6010_REQ07_R21_HSYNC_PHASE_OFFSET, 0x3c },
+	{ TM6010_REQ07_R2D_CHROMA_BURST_END, 0x48 },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R32_VSYNC_HLOCK_MIN, 0x74 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x1c },
+	{ TM6010_REQ07_R34_VSYNC_AGC_MIN, 0x74 },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R36_VSYNC_VBI_MIN, 0x7a },
+	{ TM6010_REQ07_R37_VSYNC_VBI_MAX, 0x26 },
+	{ TM6010_REQ07_R38_VSYNC_THRESHOLD, 0x40 },
+	{ TM6010_REQ07_R39_VSYNC_TIME_CONSTANT, 0x0a },
+	{ TM6010_REQ07_R42_VBI_DATA_HIGH_LEVEL, 0x55 },
+	{ TM6010_REQ07_R51_VBI_DATA_TYPE_LINE21, 0x11 },
+	{ TM6010_REQ07_R55_VBI_LOOP_FILTER_GAIN, 0x01 },
+	{ TM6010_REQ07_R57_VBI_LOOP_FILTER_P_GAIN, 0x02 },
+	{ TM6010_REQ07_R58_VBI_CAPTION_DTO1, 0x35 },
+	{ TM6010_REQ07_R59_VBI_CAPTION_DTO0, 0xa0 },
+	{ TM6010_REQ07_R80_COMB_FILTER_TRESHOLD, 0x15 },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_RC1_TRESHOLD, 0xd0 },
+	{ TM6010_REQ07_RC3_HSTART1, 0x88 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },		/* End of the soft reset */
+	{ TM6010_REQ05_R18_IMASK7, 0x00 },
+};
+
+static struct reg_init tm6010_init_tab[] = {
+	{ TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x00 },
+	{ TM6010_REQ07_RC4_HSTART0, 0xa0 },
+	{ TM6010_REQ07_RC6_HEND0, 0x40 },
+	{ TM6010_REQ07_RCA_VEND0, 0x31 },
+	{ TM6010_REQ07_RCC_ACTIVE_IF, 0xe1 },
+	{ TM6010_REQ07_RE0_DVIDEO_SOURCE, 0x03 },
+	{ TM6010_REQ07_RFE_POWER_DOWN, 0x7f },
+
+	{ TM6010_REQ08_RE2_POWER_DOWN_CTRL1, 0xf0 },
+	{ TM6010_REQ08_RE3_ADC_IN1_SEL, 0xf4 },
+	{ TM6010_REQ08_RE4_ADC_IN2_SEL, 0xf8 },
+	{ TM6010_REQ08_RE6_POWER_DOWN_CTRL2, 0x00 },
+	{ TM6010_REQ08_REA_BUFF_DRV_CTRL, 0xf2 },
+	{ TM6010_REQ08_REB_SIF_GAIN_CTRL, 0xf0 },
+	{ TM6010_REQ08_REC_REVERSE_YC_CTRL, 0xc2 },
+	{ TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG, 0x60 },
+	{ TM6010_REQ08_RF1_AADC_POWER_DOWN, 0xfc },
+
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x00 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x07 },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x00 },
+	{ TM6010_REQ07_R05_NOISE_THRESHOLD, 0x64 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x01 },
+	{ TM6010_REQ07_R08_LUMA_CONTRAST_ADJ, 0x82 },
+	{ TM6010_REQ07_R09_LUMA_BRIGHTNESS_ADJ, 0x36 },
+	{ TM6010_REQ07_R0A_CHROMA_SATURATION_ADJ, 0x50 },
+	{ TM6010_REQ07_R0C_CHROMA_AGC_CONTROL, 0x6a },
+	{ TM6010_REQ07_R11_AGC_PEAK_CONTROL, 0xc9 },
+	{ TM6010_REQ07_R12_AGC_GATE_STARTH, 0x07 },
+	{ TM6010_REQ07_R13_AGC_GATE_STARTL, 0x3b },
+	{ TM6010_REQ07_R14_AGC_GATE_WIDTH, 0x47 },
+	{ TM6010_REQ07_R15_AGC_BP_DELAY, 0x6f },
+	{ TM6010_REQ07_R17_HLOOP_MAXSTATE, 0xcd },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x8b },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xa2 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe9 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R20_HSYNC_RISING_EDGE_TIME, 0x3c },
+	{ TM6010_REQ07_R21_HSYNC_PHASE_OFFSET, 0x3c },
+	{ TM6010_REQ07_R2D_CHROMA_BURST_END, 0x48 },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R32_VSYNC_HLOCK_MIN, 0x74 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x1c },
+	{ TM6010_REQ07_R34_VSYNC_AGC_MIN, 0x74 },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R36_VSYNC_VBI_MIN, 0x7a },
+	{ TM6010_REQ07_R37_VSYNC_VBI_MAX, 0x26 },
+	{ TM6010_REQ07_R38_VSYNC_THRESHOLD, 0x40 },
+	{ TM6010_REQ07_R39_VSYNC_TIME_CONSTANT, 0x0a },
+	{ TM6010_REQ07_R42_VBI_DATA_HIGH_LEVEL, 0x55 },
+	{ TM6010_REQ07_R51_VBI_DATA_TYPE_LINE21, 0x11 },
+	{ TM6010_REQ07_R55_VBI_LOOP_FILTER_GAIN, 0x01 },
+	{ TM6010_REQ07_R57_VBI_LOOP_FILTER_P_GAIN, 0x02 },
+	{ TM6010_REQ07_R58_VBI_CAPTION_DTO1, 0x35 },
+	{ TM6010_REQ07_R59_VBI_CAPTION_DTO0, 0xa0 },
+	{ TM6010_REQ07_R80_COMB_FILTER_TRESHOLD, 0x15 },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_RC1_TRESHOLD, 0xd0 },
+	{ TM6010_REQ07_RC3_HSTART1, 0x88 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+
+	{ TM6010_REQ05_R18_IMASK7, 0x00 },
+
+	{ TM6010_REQ07_RDC_IR_LEADER1, 0xaa },
+	{ TM6010_REQ07_RDD_IR_LEADER0, 0x30 },
+	{ TM6010_REQ07_RDE_IR_PULSE_CNT1, 0x20 },
+	{ TM6010_REQ07_RDF_IR_PULSE_CNT0, 0xd0 },
+	{ REQ_04_EN_DISABLE_MCU_INT, 0x02, 0x00 },
+	{ TM6010_REQ07_RD8_IR, 0x0f },
+
+	/* set remote wakeup key:any key wakeup */
+	{ TM6010_REQ07_RE5_REMOTE_WAKEUP,  0xfe },
+	{ TM6010_REQ07_RDA_IR_WAKEUP_SEL,  0xff },
+};
+
+int tm6000_init(struct tm6000_core *dev)
+{
+	int board, rc = 0, i, size;
+	struct reg_init *tab;
+
+	/* Check board revision */
+	board = tm6000_get_reg32(dev, REQ_40_GET_VERSION, 0, 0);
+	if (board >= 0) {
+		switch (board & 0xff) {
+		case 0xf3:
+			printk(KERN_INFO "Found tm6000\n");
+			if (dev->dev_type != TM6000)
+				dev->dev_type = TM6000;
+			break;
+		case 0xf4:
+			printk(KERN_INFO "Found tm6010\n");
+			if (dev->dev_type != TM6010)
+				dev->dev_type = TM6010;
+			break;
+		default:
+			printk(KERN_INFO "Unknown board version = 0x%08x\n", board);
+		}
+	} else
+		printk(KERN_ERR "Error %i while retrieving board version\n", board);
+
+	if (dev->dev_type == TM6010) {
+		tab = tm6010_init_tab;
+		size = ARRAY_SIZE(tm6010_init_tab);
+	} else {
+		tab = tm6000_init_tab;
+		size = ARRAY_SIZE(tm6000_init_tab);
+	}
+
+	/* Load board's initialization table */
+	for (i = 0; i < size; i++) {
+		rc = tm6000_set_reg(dev, tab[i].req, tab[i].reg, tab[i].val);
+		if (rc < 0) {
+			printk(KERN_ERR "Error %i while setting req %d, reg %d to value %d\n",
+			       rc,
+					tab[i].req, tab[i].reg, tab[i].val);
+			return rc;
+		}
+	}
+
+	msleep(5); /* Just to be conservative */
+
+	rc = tm6000_cards_setup(dev);
+
+	return rc;
+}
+
+
+int tm6000_set_audio_bitrate(struct tm6000_core *dev, int bitrate)
+{
+	int val = 0;
+	u8 areg_f0 = 0x60; /* ADC MCLK = 250 Fs */
+	u8 areg_0a = 0x91; /* SIF 48KHz */
+
+	switch (bitrate) {
+	case 48000:
+		areg_f0 = 0x60; /* ADC MCLK = 250 Fs */
+		areg_0a = 0x91; /* SIF 48KHz */
+		dev->audio_bitrate = bitrate;
+		break;
+	case 32000:
+		areg_f0 = 0x00; /* ADC MCLK = 375 Fs */
+		areg_0a = 0x90; /* SIF 32KHz */
+		dev->audio_bitrate = bitrate;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+
+	/* enable I2S, if we use sif or external I2S device */
+	if (dev->dev_type == TM6010) {
+		val = tm6000_set_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, areg_0a);
+		if (val < 0)
+			return val;
+
+		val = tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+							areg_f0, 0xf0);
+		if (val < 0)
+			return val;
+	} else {
+		val = tm6000_set_reg_mask(dev, TM6000_REQ07_REB_VADC_AADC_MODE,
+							areg_f0, 0xf0);
+		if (val < 0)
+			return val;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(tm6000_set_audio_bitrate);
+
+int tm6000_set_audio_rinput(struct tm6000_core *dev)
+{
+	if (dev->dev_type == TM6010) {
+		/* Audio crossbar setting, default SIF1 */
+		u8 areg_f0;
+		u8 areg_07 = 0x10;
+
+		switch (dev->rinput.amux) {
+		case TM6000_AMUX_SIF1:
+		case TM6000_AMUX_SIF2:
+			areg_f0 = 0x03;
+			areg_07 = 0x30;
+			break;
+		case TM6000_AMUX_ADC1:
+			areg_f0 = 0x00;
+			break;
+		case TM6000_AMUX_ADC2:
+			areg_f0 = 0x08;
+			break;
+		case TM6000_AMUX_I2S:
+			areg_f0 = 0x04;
+			break;
+		default:
+			printk(KERN_INFO "%s: audio input doesn't support\n",
+				dev->name);
+			return 0;
+			break;
+		}
+		/* Set audio input crossbar */
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+							areg_f0, 0x0f);
+		/* Mux overflow workaround */
+		tm6000_set_reg_mask(dev, TM6010_REQ07_R07_OUTPUT_CONTROL,
+			areg_07, 0xf0);
+	} else {
+		u8 areg_eb;
+		/* Audio setting, default LINE1 */
+		switch (dev->rinput.amux) {
+		case TM6000_AMUX_ADC1:
+			areg_eb = 0x00;
+			break;
+		case TM6000_AMUX_ADC2:
+			areg_eb = 0x04;
+			break;
+		default:
+			printk(KERN_INFO "%s: audio input doesn't support\n",
+				dev->name);
+			return 0;
+			break;
+		}
+		/* Set audio input */
+		tm6000_set_reg_mask(dev, TM6000_REQ07_REB_VADC_AADC_MODE,
+							areg_eb, 0x0f);
+	}
+	return 0;
+}
+
+static void tm6010_set_mute_sif(struct tm6000_core *dev, u8 mute)
+{
+	u8 mute_reg = 0;
+
+	if (mute)
+		mute_reg = 0x08;
+
+	tm6000_set_reg_mask(dev, TM6010_REQ08_R0A_A_I2S_MOD, mute_reg, 0x08);
+}
+
+static void tm6010_set_mute_adc(struct tm6000_core *dev, u8 mute)
+{
+	u8 mute_reg = 0;
+
+	if (mute)
+		mute_reg = 0x20;
+
+	if (dev->dev_type == TM6010) {
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF2_LEFT_CHANNEL_VOL,
+							mute_reg, 0x20);
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF3_RIGHT_CHANNEL_VOL,
+							mute_reg, 0x20);
+	} else {
+		tm6000_set_reg_mask(dev, TM6000_REQ07_REC_VADC_AADC_LVOL,
+							mute_reg, 0x20);
+		tm6000_set_reg_mask(dev, TM6000_REQ07_RED_VADC_AADC_RVOL,
+							mute_reg, 0x20);
+	}
+}
+
+int tm6000_tvaudio_set_mute(struct tm6000_core *dev, u8 mute)
+{
+	enum tm6000_mux mux;
+
+	if (dev->radio)
+		mux = dev->rinput.amux;
+	else
+		mux = dev->vinput[dev->input].amux;
+
+	switch (mux) {
+	case TM6000_AMUX_SIF1:
+	case TM6000_AMUX_SIF2:
+		if (dev->dev_type == TM6010)
+			tm6010_set_mute_sif(dev, mute);
+		else {
+			printk(KERN_INFO "ERROR: TM5600 and TM6000 don't has SIF audio inputs. Please check the %s configuration.\n",
+			       dev->name);
+			return -EINVAL;
+		}
+		break;
+	case TM6000_AMUX_ADC1:
+	case TM6000_AMUX_ADC2:
+		tm6010_set_mute_adc(dev, mute);
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void tm6010_set_volume_sif(struct tm6000_core *dev, int vol)
+{
+	u8 vol_reg;
+
+	vol_reg = vol & 0x0F;
+
+	if (vol < 0)
+		vol_reg |= 0x40;
+
+	tm6000_set_reg(dev, TM6010_REQ08_R07_A_LEFT_VOL, vol_reg);
+	tm6000_set_reg(dev, TM6010_REQ08_R08_A_RIGHT_VOL, vol_reg);
+}
+
+static void tm6010_set_volume_adc(struct tm6000_core *dev, int vol)
+{
+	u8 vol_reg;
+
+	vol_reg = (vol + 0x10) & 0x1f;
+
+	if (dev->dev_type == TM6010) {
+		tm6000_set_reg(dev, TM6010_REQ08_RF2_LEFT_CHANNEL_VOL, vol_reg);
+		tm6000_set_reg(dev, TM6010_REQ08_RF3_RIGHT_CHANNEL_VOL, vol_reg);
+	} else {
+		tm6000_set_reg(dev, TM6000_REQ07_REC_VADC_AADC_LVOL, vol_reg);
+		tm6000_set_reg(dev, TM6000_REQ07_RED_VADC_AADC_RVOL, vol_reg);
+	}
+}
+
+void tm6000_set_volume(struct tm6000_core *dev, int vol)
+{
+	enum tm6000_mux mux;
+
+	if (dev->radio) {
+		mux = dev->rinput.amux;
+		vol += 8; /* Offset to 0 dB */
+	} else
+		mux = dev->vinput[dev->input].amux;
+
+	switch (mux) {
+	case TM6000_AMUX_SIF1:
+	case TM6000_AMUX_SIF2:
+		if (dev->dev_type == TM6010)
+			tm6010_set_volume_sif(dev, vol);
+		else
+			printk(KERN_INFO "ERROR: TM5600 and TM6000 don't has SIF audio inputs. Please check the %s configuration.\n",
+			       dev->name);
+		break;
+	case TM6000_AMUX_ADC1:
+	case TM6000_AMUX_ADC2:
+		tm6010_set_volume_adc(dev, vol);
+		break;
+	default:
+		break;
+	}
+}
+
+static LIST_HEAD(tm6000_devlist);
+static DEFINE_MUTEX(tm6000_devlist_mutex);
+
+/*
+ * tm6000_realease_resource()
+ */
+
+void tm6000_remove_from_devlist(struct tm6000_core *dev)
+{
+	mutex_lock(&tm6000_devlist_mutex);
+	list_del(&dev->devlist);
+	mutex_unlock(&tm6000_devlist_mutex);
+};
+
+void tm6000_add_into_devlist(struct tm6000_core *dev)
+{
+	mutex_lock(&tm6000_devlist_mutex);
+	list_add_tail(&dev->devlist, &tm6000_devlist);
+	mutex_unlock(&tm6000_devlist_mutex);
+};
+
+/*
+ * Extension interface
+ */
+
+static LIST_HEAD(tm6000_extension_devlist);
+
+int tm6000_call_fillbuf(struct tm6000_core *dev, enum tm6000_ops_type type,
+			char *buf, int size)
+{
+	struct tm6000_ops *ops = NULL;
+
+	/* FIXME: tm6000_extension_devlist_lock should be a spinlock */
+
+	list_for_each_entry(ops, &tm6000_extension_devlist, next) {
+		if (ops->fillbuf && ops->type == type)
+			ops->fillbuf(dev, buf, size);
+	}
+
+	return 0;
+}
+
+int tm6000_register_extension(struct tm6000_ops *ops)
+{
+	struct tm6000_core *dev = NULL;
+
+	mutex_lock(&tm6000_devlist_mutex);
+	list_add_tail(&ops->next, &tm6000_extension_devlist);
+	list_for_each_entry(dev, &tm6000_devlist, devlist) {
+		ops->init(dev);
+		printk(KERN_INFO "%s: Initialized (%s) extension\n",
+		       dev->name, ops->name);
+	}
+	mutex_unlock(&tm6000_devlist_mutex);
+	return 0;
+}
+EXPORT_SYMBOL(tm6000_register_extension);
+
+void tm6000_unregister_extension(struct tm6000_ops *ops)
+{
+	struct tm6000_core *dev = NULL;
+
+	mutex_lock(&tm6000_devlist_mutex);
+	list_for_each_entry(dev, &tm6000_devlist, devlist)
+		ops->fini(dev);
+
+	printk(KERN_INFO "tm6000: Remove (%s) extension\n", ops->name);
+	list_del(&ops->next);
+	mutex_unlock(&tm6000_devlist_mutex);
+}
+EXPORT_SYMBOL(tm6000_unregister_extension);
+
+void tm6000_init_extension(struct tm6000_core *dev)
+{
+	struct tm6000_ops *ops = NULL;
+
+	mutex_lock(&tm6000_devlist_mutex);
+	list_for_each_entry(ops, &tm6000_extension_devlist, next) {
+		if (ops->init)
+			ops->init(dev);
+	}
+	mutex_unlock(&tm6000_devlist_mutex);
+}
+
+void tm6000_close_extension(struct tm6000_core *dev)
+{
+	struct tm6000_ops *ops = NULL;
+
+	mutex_lock(&tm6000_devlist_mutex);
+	list_for_each_entry(ops, &tm6000_extension_devlist, next) {
+		if (ops->fini)
+			ops->fini(dev);
+	}
+	mutex_unlock(&tm6000_devlist_mutex);
+}
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-dvb.c b/drivers/staging/media/deprecated/tm6000/tm6000-dvb.c
new file mode 100644
index 000000000..ee04973cb
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-dvb.c
@@ -0,0 +1,454 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  tm6000-dvb.c - dvb-t support for TM5600/TM6000/TM6010 USB video capture devices
+ *
+ *  Copyright (C) 2007 Michel Ludwig <michel.ludwig@gmail.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include "tm6000.h"
+#include "tm6000-regs.h"
+
+#include "zl10353.h"
+
+#include <media/tuner.h>
+
+#include "xc2028.h"
+#include "xc5000.h"
+
+MODULE_DESCRIPTION("DVB driver extension module for tm5600/6000/6010 based TV cards");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL");
+
+static int debug;
+
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debug message");
+
+static inline void print_err_status(struct tm6000_core *dev,
+				    int packet, int status)
+{
+	char *errmsg = "Unknown";
+
+	switch (status) {
+	case -ENOENT:
+		errmsg = "unlinked synchronously";
+		break;
+	case -ECONNRESET:
+		errmsg = "unlinked asynchronously";
+		break;
+	case -ENOSR:
+		errmsg = "Buffer error (overrun)";
+		break;
+	case -EPIPE:
+		errmsg = "Stalled (device not responding)";
+		break;
+	case -EOVERFLOW:
+		errmsg = "Babble (bad cable?)";
+		break;
+	case -EPROTO:
+		errmsg = "Bit-stuff error (bad cable?)";
+		break;
+	case -EILSEQ:
+		errmsg = "CRC/Timeout (could be anything)";
+		break;
+	case -ETIME:
+		errmsg = "Device does not respond";
+		break;
+	}
+	if (packet < 0) {
+		dprintk(dev, 1, "URB status %d [%s].\n",
+			status, errmsg);
+	} else {
+		dprintk(dev, 1, "URB packet %d, status %d [%s].\n",
+			packet, status, errmsg);
+	}
+}
+
+static void tm6000_urb_received(struct urb *urb)
+{
+	int ret;
+	struct tm6000_core *dev = urb->context;
+
+	switch (urb->status) {
+	case 0:
+	case -ETIMEDOUT:
+		break;
+	case -ENOENT:
+	case -ECONNRESET:
+	case -ESHUTDOWN:
+		return;
+	default:
+		print_err_status(dev, 0, urb->status);
+	}
+
+	if (urb->actual_length > 0)
+		dvb_dmx_swfilter(&dev->dvb->demux, urb->transfer_buffer,
+						   urb->actual_length);
+
+	if (dev->dvb->streams > 0) {
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			printk(KERN_ERR "tm6000:  error %s\n", __func__);
+			kfree(urb->transfer_buffer);
+			usb_free_urb(urb);
+			dev->dvb->bulk_urb = NULL;
+		}
+	}
+}
+
+static int tm6000_start_stream(struct tm6000_core *dev)
+{
+	int ret;
+	unsigned int pipe, size;
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	printk(KERN_INFO "tm6000: got start stream request %s\n", __func__);
+
+	if (dev->mode != TM6000_MODE_DIGITAL) {
+		tm6000_init_digital_mode(dev);
+		dev->mode = TM6000_MODE_DIGITAL;
+	}
+
+	dvb->bulk_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dvb->bulk_urb)
+		return -ENOMEM;
+
+	pipe = usb_rcvbulkpipe(dev->udev, dev->bulk_in.endp->desc.bEndpointAddress
+							  & USB_ENDPOINT_NUMBER_MASK);
+
+	size = usb_maxpacket(dev->udev, pipe);
+	size = size * 15; /* 512 x 8 or 12 or 15 */
+
+	dvb->bulk_urb->transfer_buffer = kzalloc(size, GFP_KERNEL);
+	if (!dvb->bulk_urb->transfer_buffer) {
+		usb_free_urb(dvb->bulk_urb);
+		dvb->bulk_urb = NULL;
+		return -ENOMEM;
+	}
+
+	usb_fill_bulk_urb(dvb->bulk_urb, dev->udev, pipe,
+						 dvb->bulk_urb->transfer_buffer,
+						 size,
+						 tm6000_urb_received, dev);
+
+	ret = usb_clear_halt(dev->udev, pipe);
+	if (ret < 0) {
+		printk(KERN_ERR "tm6000: error %i in %s during pipe reset\n",
+							ret, __func__);
+
+		kfree(dvb->bulk_urb->transfer_buffer);
+		usb_free_urb(dvb->bulk_urb);
+		dvb->bulk_urb = NULL;
+		return ret;
+	} else
+		printk(KERN_ERR "tm6000: pipe reset\n");
+
+/*	mutex_lock(&tm6000_driver.open_close_mutex); */
+	ret = usb_submit_urb(dvb->bulk_urb, GFP_ATOMIC);
+
+/*	mutex_unlock(&tm6000_driver.open_close_mutex); */
+	if (ret) {
+		printk(KERN_ERR "tm6000: submit of urb failed (error=%i)\n",
+									ret);
+
+		kfree(dvb->bulk_urb->transfer_buffer);
+		usb_free_urb(dvb->bulk_urb);
+		dvb->bulk_urb = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+static void tm6000_stop_stream(struct tm6000_core *dev)
+{
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	if (dvb->bulk_urb) {
+		printk(KERN_INFO "urb killing\n");
+		usb_kill_urb(dvb->bulk_urb);
+		printk(KERN_INFO "urb buffer free\n");
+		kfree(dvb->bulk_urb->transfer_buffer);
+		usb_free_urb(dvb->bulk_urb);
+		dvb->bulk_urb = NULL;
+	}
+}
+
+static int tm6000_start_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct tm6000_core *dev = demux->priv;
+	struct tm6000_dvb *dvb = dev->dvb;
+	printk(KERN_INFO "tm6000: got start feed request %s\n", __func__);
+
+	mutex_lock(&dvb->mutex);
+	if (dvb->streams == 0) {
+		dvb->streams = 1;
+/*		mutex_init(&tm6000_dev->streming_mutex); */
+		tm6000_start_stream(dev);
+	} else
+		++(dvb->streams);
+	mutex_unlock(&dvb->mutex);
+
+	return 0;
+}
+
+static int tm6000_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct tm6000_core *dev = demux->priv;
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	printk(KERN_INFO "tm6000: got stop feed request %s\n", __func__);
+
+	mutex_lock(&dvb->mutex);
+
+	printk(KERN_INFO "stream %#x\n", dvb->streams);
+	--(dvb->streams);
+	if (dvb->streams == 0) {
+		printk(KERN_INFO "stop stream\n");
+		tm6000_stop_stream(dev);
+/*		mutex_destroy(&tm6000_dev->streaming_mutex); */
+	}
+	mutex_unlock(&dvb->mutex);
+/*	mutex_destroy(&tm6000_dev->streaming_mutex); */
+
+	return 0;
+}
+
+static int tm6000_dvb_attach_frontend(struct tm6000_core *dev)
+{
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	if (dev->caps.has_zl10353) {
+		struct zl10353_config config = {
+				     .demod_address = dev->demod_addr,
+				     .no_tuner = 1,
+				     .parallel_ts = 1,
+				     .if2 = 45700,
+				     .disable_i2c_gate_ctrl = 1,
+				    };
+
+		dvb->frontend = dvb_attach(zl10353_attach, &config,
+							   &dev->i2c_adap);
+	} else {
+		printk(KERN_ERR "tm6000: no frontend defined for the device!\n");
+		return -1;
+	}
+
+	return (!dvb->frontend) ? -1 : 0;
+}
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static int register_dvb(struct tm6000_core *dev)
+{
+	int ret = -1;
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	mutex_init(&dvb->mutex);
+
+	dvb->streams = 0;
+
+	/* attach the frontend */
+	ret = tm6000_dvb_attach_frontend(dev);
+	if (ret < 0) {
+		printk(KERN_ERR "tm6000: couldn't attach the frontend!\n");
+		goto err;
+	}
+
+	ret = dvb_register_adapter(&dvb->adapter, "Trident TVMaster 6000 DVB-T",
+					THIS_MODULE, &dev->udev->dev, adapter_nr);
+	if (ret < 0) {
+		pr_err("tm6000: couldn't register the adapter!\n");
+		goto err;
+	}
+
+	dvb->adapter.priv = dev;
+
+	if (dvb->frontend) {
+		switch (dev->tuner_type) {
+		case TUNER_XC2028: {
+			struct xc2028_config cfg = {
+				.i2c_adap = &dev->i2c_adap,
+				.i2c_addr = dev->tuner_addr,
+			};
+
+			dvb->frontend->callback = tm6000_tuner_callback;
+			ret = dvb_register_frontend(&dvb->adapter, dvb->frontend);
+			if (ret < 0) {
+				printk(KERN_ERR
+					"tm6000: couldn't register frontend\n");
+				goto adapter_err;
+			}
+
+			if (!dvb_attach(xc2028_attach, dvb->frontend, &cfg)) {
+				printk(KERN_ERR "tm6000: couldn't register frontend (xc3028)\n");
+				ret = -EINVAL;
+				goto frontend_err;
+			}
+			printk(KERN_INFO "tm6000: XC2028/3028 asked to be attached to frontend!\n");
+			break;
+			}
+		case TUNER_XC5000: {
+			struct xc5000_config cfg = {
+				.i2c_address = dev->tuner_addr,
+			};
+
+			dvb->frontend->callback = tm6000_xc5000_callback;
+			ret = dvb_register_frontend(&dvb->adapter, dvb->frontend);
+			if (ret < 0) {
+				printk(KERN_ERR
+					"tm6000: couldn't register frontend\n");
+				goto adapter_err;
+			}
+
+			if (!dvb_attach(xc5000_attach, dvb->frontend, &dev->i2c_adap, &cfg)) {
+				printk(KERN_ERR "tm6000: couldn't register frontend (xc5000)\n");
+				ret = -EINVAL;
+				goto frontend_err;
+			}
+			printk(KERN_INFO "tm6000: XC5000 asked to be attached to frontend!\n");
+			break;
+			}
+		}
+	} else
+		printk(KERN_ERR "tm6000: no frontend found\n");
+
+	dvb->demux.dmx.capabilities = DMX_TS_FILTERING | DMX_SECTION_FILTERING
+							    | DMX_MEMORY_BASED_FILTERING;
+	dvb->demux.priv = dev;
+	dvb->demux.filternum = 8;
+	dvb->demux.feednum = 8;
+	dvb->demux.start_feed = tm6000_start_feed;
+	dvb->demux.stop_feed = tm6000_stop_feed;
+	dvb->demux.write_to_decoder = NULL;
+	ret = dvb_dmx_init(&dvb->demux);
+	if (ret < 0) {
+		printk(KERN_ERR "tm6000: dvb_dmx_init failed (errno = %d)\n", ret);
+		goto frontend_err;
+	}
+
+	dvb->dmxdev.filternum = dev->dvb->demux.filternum;
+	dvb->dmxdev.demux = &dev->dvb->demux.dmx;
+	dvb->dmxdev.capabilities = 0;
+
+	ret =  dvb_dmxdev_init(&dvb->dmxdev, &dvb->adapter);
+	if (ret < 0) {
+		printk(KERN_ERR "tm6000: dvb_dmxdev_init failed (errno = %d)\n", ret);
+		goto dvb_dmx_err;
+	}
+
+	return 0;
+
+dvb_dmx_err:
+	dvb_dmx_release(&dvb->demux);
+frontend_err:
+	if (dvb->frontend) {
+		dvb_unregister_frontend(dvb->frontend);
+		dvb_frontend_detach(dvb->frontend);
+	}
+adapter_err:
+	dvb_unregister_adapter(&dvb->adapter);
+err:
+	return ret;
+}
+
+static void unregister_dvb(struct tm6000_core *dev)
+{
+	struct tm6000_dvb *dvb = dev->dvb;
+
+	if (dvb->bulk_urb) {
+		struct urb *bulk_urb = dvb->bulk_urb;
+
+		kfree(bulk_urb->transfer_buffer);
+		bulk_urb->transfer_buffer = NULL;
+		usb_unlink_urb(bulk_urb);
+		usb_free_urb(bulk_urb);
+	}
+
+/*	mutex_lock(&tm6000_driver.open_close_mutex); */
+	if (dvb->frontend) {
+		dvb_unregister_frontend(dvb->frontend);
+		dvb_frontend_detach(dvb->frontend);
+	}
+
+	dvb_dmxdev_release(&dvb->dmxdev);
+	dvb_dmx_release(&dvb->demux);
+	dvb_unregister_adapter(&dvb->adapter);
+	mutex_destroy(&dvb->mutex);
+/*	mutex_unlock(&tm6000_driver.open_close_mutex); */
+}
+
+static int dvb_init(struct tm6000_core *dev)
+{
+	struct tm6000_dvb *dvb;
+	int rc;
+
+	if (!dev)
+		return 0;
+
+	if (!dev->caps.has_dvb)
+		return 0;
+
+	if (dev->udev->speed == USB_SPEED_FULL) {
+		printk(KERN_INFO "This USB2.0 device cannot be run on a USB1.1 port. (it lacks a hardware PID filter)\n");
+		return 0;
+	}
+
+	dvb = kzalloc(sizeof(struct tm6000_dvb), GFP_KERNEL);
+	if (!dvb)
+		return -ENOMEM;
+
+	dev->dvb = dvb;
+
+	rc = register_dvb(dev);
+	if (rc < 0) {
+		kfree(dvb);
+		dev->dvb = NULL;
+		return 0;
+	}
+
+	return 0;
+}
+
+static int dvb_fini(struct tm6000_core *dev)
+{
+	if (!dev)
+		return 0;
+
+	if (!dev->caps.has_dvb)
+		return 0;
+
+	if (dev->dvb) {
+		unregister_dvb(dev);
+		kfree(dev->dvb);
+		dev->dvb = NULL;
+	}
+
+	return 0;
+}
+
+static struct tm6000_ops dvb_ops = {
+	.type	= TM6000_DVB,
+	.name	= "TM6000 dvb Extension",
+	.init	= dvb_init,
+	.fini	= dvb_fini,
+};
+
+static int __init tm6000_dvb_register(void)
+{
+	return tm6000_register_extension(&dvb_ops);
+}
+
+static void __exit tm6000_dvb_unregister(void)
+{
+	tm6000_unregister_extension(&dvb_ops);
+}
+
+module_init(tm6000_dvb_register);
+module_exit(tm6000_dvb_unregister);
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-i2c.c b/drivers/staging/media/deprecated/tm6000/tm6000-i2c.c
new file mode 100644
index 000000000..7554b93b8
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-i2c.c
@@ -0,0 +1,317 @@
+// SPDX-License-Identifier: GPL-2.0
+// tm6000-i2c.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+//
+// Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+//
+// Copyright (c) 2007 Michel Ludwig <michel.ludwig@gmail.com>
+//	- Fix SMBus Read Byte command
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+
+#include "tm6000.h"
+#include "tm6000-regs.h"
+#include <media/v4l2-common.h>
+#include <media/tuner.h>
+#include "xc2028.h"
+
+
+/* ----------------------------------------------------------- */
+
+static unsigned int i2c_debug;
+module_param(i2c_debug, int, 0644);
+MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
+
+#define i2c_dprintk(lvl, fmt, args...) if (i2c_debug >= lvl) do { \
+			printk(KERN_DEBUG "%s at %s: " fmt, \
+			dev->name, __func__, ##args); } while (0)
+
+static int tm6000_i2c_send_regs(struct tm6000_core *dev, unsigned char addr,
+				__u8 reg, char *buf, int len)
+{
+	int rc;
+	unsigned int i2c_packet_limit = 16;
+
+	if (dev->dev_type == TM6010)
+		i2c_packet_limit = 80;
+
+	if (!buf)
+		return -1;
+
+	if (len < 1 || len > i2c_packet_limit) {
+		printk(KERN_ERR "Incorrect length of i2c packet = %d, limit set to %d\n",
+			len, i2c_packet_limit);
+		return -1;
+	}
+
+	/* capture mutex */
+	rc = tm6000_read_write_usb(dev, USB_DIR_OUT | USB_TYPE_VENDOR |
+		USB_RECIP_DEVICE, REQ_16_SET_GET_I2C_WR1_RDN,
+		addr | reg << 8, 0, buf, len);
+
+	if (rc < 0) {
+		/* release mutex */
+		return rc;
+	}
+
+	/* release mutex */
+	return rc;
+}
+
+/* Generic read - doesn't work fine with 16bit registers */
+static int tm6000_i2c_recv_regs(struct tm6000_core *dev, unsigned char addr,
+				__u8 reg, char *buf, int len)
+{
+	int rc;
+	u8 b[2];
+	unsigned int i2c_packet_limit = 16;
+
+	if (dev->dev_type == TM6010)
+		i2c_packet_limit = 64;
+
+	if (!buf)
+		return -1;
+
+	if (len < 1 || len > i2c_packet_limit) {
+		printk(KERN_ERR "Incorrect length of i2c packet = %d, limit set to %d\n",
+			len, i2c_packet_limit);
+		return -1;
+	}
+
+	/* capture mutex */
+	if ((dev->caps.has_zl10353) && (dev->demod_addr << 1 == addr) && (reg % 2 == 0)) {
+		/*
+		 * Workaround an I2C bug when reading from zl10353
+		 */
+		reg -= 1;
+		len += 1;
+
+		rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			REQ_16_SET_GET_I2C_WR1_RDN, addr | reg << 8, 0, b, len);
+
+		*buf = b[1];
+	} else {
+		rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			REQ_16_SET_GET_I2C_WR1_RDN, addr | reg << 8, 0, buf, len);
+	}
+
+	/* release mutex */
+	return rc;
+}
+
+/*
+ * read from a 16bit register
+ * for example xc2028, xc3028 or xc3028L
+ */
+static int tm6000_i2c_recv_regs16(struct tm6000_core *dev, unsigned char addr,
+				  __u16 reg, char *buf, int len)
+{
+	int rc;
+	unsigned char ureg;
+
+	if (!buf || len != 2)
+		return -1;
+
+	/* capture mutex */
+	if (dev->dev_type == TM6010) {
+		ureg = reg & 0xFF;
+		rc = tm6000_read_write_usb(dev, USB_DIR_OUT | USB_TYPE_VENDOR |
+			USB_RECIP_DEVICE, REQ_16_SET_GET_I2C_WR1_RDN,
+			addr | (reg & 0xFF00), 0, &ureg, 1);
+
+		if (rc < 0) {
+			/* release mutex */
+			return rc;
+		}
+
+		rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR |
+			USB_RECIP_DEVICE, REQ_35_AFTEK_TUNER_READ,
+			reg, 0, buf, len);
+	} else {
+		rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR |
+			USB_RECIP_DEVICE, REQ_14_SET_GET_I2C_WR2_RDN,
+			addr, reg, buf, len);
+	}
+
+	/* release mutex */
+	return rc;
+}
+
+static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
+			   struct i2c_msg msgs[], int num)
+{
+	struct tm6000_core *dev = i2c_adap->algo_data;
+	int addr, rc, i, byte;
+
+	for (i = 0; i < num; i++) {
+		addr = (msgs[i].addr << 1) & 0xff;
+		i2c_dprintk(2, "%s %s addr=0x%x len=%d:",
+			 (msgs[i].flags & I2C_M_RD) ? "read" : "write",
+			 i == num - 1 ? "stop" : "nonstop", addr, msgs[i].len);
+		if (msgs[i].flags & I2C_M_RD) {
+			/* read request without preceding register selection */
+			/*
+			 * The TM6000 only supports a read transaction
+			 * immediately after a 1 or 2 byte write to select
+			 * a register.  We cannot fulfill this request.
+			 */
+			i2c_dprintk(2, " read without preceding write not supported");
+			rc = -EOPNOTSUPP;
+			goto err;
+		} else if (i + 1 < num && msgs[i].len <= 2 &&
+			   (msgs[i + 1].flags & I2C_M_RD) &&
+			   msgs[i].addr == msgs[i + 1].addr) {
+			/* 1 or 2 byte write followed by a read */
+			if (i2c_debug >= 2)
+				for (byte = 0; byte < msgs[i].len; byte++)
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
+			i2c_dprintk(2, "; joined to read %s len=%d:",
+				    i == num - 2 ? "stop" : "nonstop",
+				    msgs[i + 1].len);
+
+			if (msgs[i].len == 2) {
+				rc = tm6000_i2c_recv_regs16(dev, addr,
+					msgs[i].buf[0] << 8 | msgs[i].buf[1],
+					msgs[i + 1].buf, msgs[i + 1].len);
+			} else {
+				rc = tm6000_i2c_recv_regs(dev, addr, msgs[i].buf[0],
+					msgs[i + 1].buf, msgs[i + 1].len);
+			}
+
+			i++;
+
+			if (addr == dev->tuner_addr << 1) {
+				tm6000_set_reg(dev, REQ_50_SET_START, 0, 0);
+				tm6000_set_reg(dev, REQ_51_SET_STOP, 0, 0);
+			}
+			if (i2c_debug >= 2)
+				for (byte = 0; byte < msgs[i].len; byte++)
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
+		} else {
+			/* write bytes */
+			if (i2c_debug >= 2)
+				for (byte = 0; byte < msgs[i].len; byte++)
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
+			rc = tm6000_i2c_send_regs(dev, addr, msgs[i].buf[0],
+				msgs[i].buf + 1, msgs[i].len - 1);
+		}
+		if (i2c_debug >= 2)
+			printk(KERN_CONT "\n");
+		if (rc < 0)
+			goto err;
+	}
+
+	return num;
+err:
+	i2c_dprintk(2, " ERROR: %i\n", rc);
+	return rc;
+}
+
+static int tm6000_i2c_eeprom(struct tm6000_core *dev)
+{
+	int i, rc;
+	unsigned char *p = dev->eedata;
+	unsigned char bytes[17];
+
+	dev->i2c_client.addr = 0xa0 >> 1;
+	dev->eedata_size = 0;
+
+	bytes[16] = '\0';
+	for (i = 0; i < sizeof(dev->eedata); ) {
+		*p = i;
+		rc = tm6000_i2c_recv_regs(dev, 0xa0, i, p, 1);
+		if (rc < 1) {
+			if (p == dev->eedata)
+				goto noeeprom;
+			else {
+				printk(KERN_WARNING
+				"%s: i2c eeprom read error (err=%d)\n",
+				dev->name, rc);
+			}
+			return -EINVAL;
+		}
+		dev->eedata_size++;
+		p++;
+		if (0 == (i % 16))
+			printk(KERN_INFO "%s: i2c eeprom %02x:", dev->name, i);
+		printk(KERN_CONT " %02x", dev->eedata[i]);
+		if ((dev->eedata[i] >= ' ') && (dev->eedata[i] <= 'z'))
+			bytes[i%16] = dev->eedata[i];
+		else
+			bytes[i%16] = '.';
+
+		i++;
+
+		if (0 == (i % 16)) {
+			bytes[16] = '\0';
+			printk(KERN_CONT "  %s\n", bytes);
+		}
+	}
+	if (0 != (i%16)) {
+		bytes[i%16] = '\0';
+		for (i %= 16; i < 16; i++)
+			printk(KERN_CONT "   ");
+		printk(KERN_CONT "  %s\n", bytes);
+	}
+
+	return 0;
+
+noeeprom:
+	printk(KERN_INFO "%s: Huh, no eeprom present (err=%d)?\n",
+	       dev->name, rc);
+	return -EINVAL;
+}
+
+/* ----------------------------------------------------------- */
+
+/*
+ * functionality()
+ */
+static u32 functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm tm6000_algo = {
+	.master_xfer   = tm6000_i2c_xfer,
+	.functionality = functionality,
+};
+
+/* ----------------------------------------------------------- */
+
+/*
+ * tm6000_i2c_register()
+ * register i2c bus
+ */
+int tm6000_i2c_register(struct tm6000_core *dev)
+{
+	int rc;
+
+	dev->i2c_adap.owner = THIS_MODULE;
+	dev->i2c_adap.algo = &tm6000_algo;
+	dev->i2c_adap.dev.parent = &dev->udev->dev;
+	strscpy(dev->i2c_adap.name, dev->name, sizeof(dev->i2c_adap.name));
+	dev->i2c_adap.algo_data = dev;
+	i2c_set_adapdata(&dev->i2c_adap, &dev->v4l2_dev);
+	rc = i2c_add_adapter(&dev->i2c_adap);
+	if (rc)
+		return rc;
+
+	dev->i2c_client.adapter = &dev->i2c_adap;
+	strscpy(dev->i2c_client.name, "tm6000 internal", I2C_NAME_SIZE);
+	tm6000_i2c_eeprom(dev);
+
+	return 0;
+}
+
+/*
+ * tm6000_i2c_unregister()
+ * unregister i2c_bus
+ */
+int tm6000_i2c_unregister(struct tm6000_core *dev)
+{
+	i2c_del_adapter(&dev->i2c_adap);
+	return 0;
+}
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-input.c b/drivers/staging/media/deprecated/tm6000/tm6000-input.c
new file mode 100644
index 000000000..5136e9e20
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-input.c
@@ -0,0 +1,503 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  tm6000-input.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+ *
+ *  Copyright (C) 2010 Stefan Ringel <stefan.ringel@arcor.de>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include <linux/input.h>
+#include <linux/usb.h>
+
+#include <media/rc-core.h>
+
+#include "tm6000.h"
+#include "tm6000-regs.h"
+
+static unsigned int ir_debug;
+module_param(ir_debug, int, 0644);
+MODULE_PARM_DESC(ir_debug, "debug message level");
+
+static unsigned int enable_ir = 1;
+module_param(enable_ir, int, 0644);
+MODULE_PARM_DESC(enable_ir, "enable ir (default is enable)");
+
+static unsigned int ir_clock_mhz = 12;
+module_param(ir_clock_mhz, int, 0644);
+MODULE_PARM_DESC(ir_clock_mhz, "ir clock, in MHz");
+
+#define URB_SUBMIT_DELAY	100	/* ms - Delay to submit an URB request on retrial and init */
+#define URB_INT_LED_DELAY	100	/* ms - Delay to turn led on again on int mode */
+
+#undef dprintk
+
+#define dprintk(level, fmt, arg...) do {\
+	if (ir_debug >= level) \
+		printk(KERN_DEBUG "%s/ir: " fmt, ir->name , ## arg); \
+	} while (0)
+
+struct tm6000_ir_poll_result {
+	u16 rc_data;
+};
+
+struct tm6000_IR {
+	struct tm6000_core	*dev;
+	struct rc_dev		*rc;
+	char			name[32];
+	char			phys[32];
+
+	/* poll expernal decoder */
+	int			polling;
+	struct delayed_work	work;
+	u8			wait:1;
+	u8			pwled:2;
+	u8			submit_urb:1;
+	struct urb		*int_urb;
+
+	/* IR device properties */
+	u64			rc_proto;
+};
+
+void tm6000_ir_wait(struct tm6000_core *dev, u8 state)
+{
+	struct tm6000_IR *ir = dev->ir;
+
+	if (!dev->ir)
+		return;
+
+	dprintk(2, "%s: %i\n",__func__, ir->wait);
+
+	if (state)
+		ir->wait = 1;
+	else
+		ir->wait = 0;
+}
+
+static int tm6000_ir_config(struct tm6000_IR *ir)
+{
+	struct tm6000_core *dev = ir->dev;
+	u32 pulse = 0, leader = 0;
+
+	dprintk(2, "%s\n",__func__);
+
+	/*
+	 * The IR decoder supports RC-5 or NEC, with a configurable timing.
+	 * The timing configuration there is not that accurate, as it uses
+	 * approximate values. The NEC spec mentions a 562.5 unit period,
+	 * and RC-5 uses a 888.8 period.
+	 * Currently, driver assumes a clock provided by a 12 MHz XTAL, but
+	 * a modprobe parameter can adjust it.
+	 * Adjustments are required for other timings.
+	 * It seems that the 900ms timing for NEC is used to detect a RC-5
+	 * IR, in order to discard such decoding
+	 */
+
+	switch (ir->rc_proto) {
+	case RC_PROTO_BIT_NEC:
+		leader = 900;	/* ms */
+		pulse  = 700;	/* ms - the actual value would be 562 */
+		break;
+	default:
+	case RC_PROTO_BIT_RC5:
+		leader = 900;	/* ms - from the NEC decoding */
+		pulse  = 1780;	/* ms - The actual value would be 1776 */
+		break;
+	}
+
+	pulse = ir_clock_mhz * pulse;
+	leader = ir_clock_mhz * leader;
+	if (ir->rc_proto == RC_PROTO_BIT_NEC)
+		leader = leader | 0x8000;
+
+	dprintk(2, "%s: %s, %d MHz, leader = 0x%04x, pulse = 0x%06x \n",
+		__func__,
+		(ir->rc_proto == RC_PROTO_BIT_NEC) ? "NEC" : "RC-5",
+		ir_clock_mhz, leader, pulse);
+
+	/* Remote WAKEUP = enable, normal mode, from IR decoder output */
+	tm6000_set_reg(dev, TM6010_REQ07_RE5_REMOTE_WAKEUP, 0xfe);
+
+	/* Enable IR reception on non-busrt mode */
+	tm6000_set_reg(dev, TM6010_REQ07_RD8_IR, 0x2f);
+
+	/* IR_WKUP_SEL = Low byte in decoded IR data */
+	tm6000_set_reg(dev, TM6010_REQ07_RDA_IR_WAKEUP_SEL, 0xff);
+	/* IR_WKU_ADD code */
+	tm6000_set_reg(dev, TM6010_REQ07_RDB_IR_WAKEUP_ADD, 0xff);
+
+	tm6000_set_reg(dev, TM6010_REQ07_RDC_IR_LEADER1, leader >> 8);
+	tm6000_set_reg(dev, TM6010_REQ07_RDD_IR_LEADER0, leader);
+
+	tm6000_set_reg(dev, TM6010_REQ07_RDE_IR_PULSE_CNT1, pulse >> 8);
+	tm6000_set_reg(dev, TM6010_REQ07_RDF_IR_PULSE_CNT0, pulse);
+
+	if (!ir->polling)
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 2, 0);
+	else
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 2, 1);
+	msleep(10);
+
+	/* Shows that IR is working via the LED */
+	tm6000_flash_led(dev, 0);
+	msleep(100);
+	tm6000_flash_led(dev, 1);
+	ir->pwled = 1;
+
+	return 0;
+}
+
+static void tm6000_ir_keydown(struct tm6000_IR *ir,
+			      const char *buf, unsigned int len)
+{
+	u8 device, command;
+	u32 scancode;
+	enum rc_proto protocol;
+
+	if (len < 1)
+		return;
+
+	command = buf[0];
+	device = (len > 1 ? buf[1] : 0x0);
+	switch (ir->rc_proto) {
+	case RC_PROTO_BIT_RC5:
+		protocol = RC_PROTO_RC5;
+		scancode = RC_SCANCODE_RC5(device, command);
+		break;
+	case RC_PROTO_BIT_NEC:
+		protocol = RC_PROTO_NEC;
+		scancode = RC_SCANCODE_NEC(device, command);
+		break;
+	default:
+		protocol = RC_PROTO_OTHER;
+		scancode = RC_SCANCODE_OTHER(device << 8 | command);
+		break;
+	}
+
+	dprintk(1, "%s, protocol: 0x%04x, scancode: 0x%08x\n",
+		__func__, protocol, scancode);
+	rc_keydown(ir->rc, protocol, scancode, 0);
+}
+
+static void tm6000_ir_urb_received(struct urb *urb)
+{
+	struct tm6000_core *dev = urb->context;
+	struct tm6000_IR *ir = dev->ir;
+	char *buf;
+
+	dprintk(2, "%s\n",__func__);
+	if (urb->status < 0 || urb->actual_length <= 0) {
+		printk(KERN_INFO "tm6000: IR URB failure: status: %i, length %i\n",
+		       urb->status, urb->actual_length);
+		ir->submit_urb = 1;
+		schedule_delayed_work(&ir->work, msecs_to_jiffies(URB_SUBMIT_DELAY));
+		return;
+	}
+	buf = urb->transfer_buffer;
+
+	if (ir_debug)
+		print_hex_dump(KERN_DEBUG, "tm6000: IR data: ",
+			       DUMP_PREFIX_OFFSET,16, 1,
+			       buf, urb->actual_length, false);
+
+	tm6000_ir_keydown(ir, urb->transfer_buffer, urb->actual_length);
+
+	usb_submit_urb(urb, GFP_ATOMIC);
+	/*
+	 * Flash the led. We can't do it here, as it is running on IRQ context.
+	 * So, use the scheduler to do it, in a few ms.
+	 */
+	ir->pwled = 2;
+	schedule_delayed_work(&ir->work, msecs_to_jiffies(10));
+}
+
+static void tm6000_ir_handle_key(struct work_struct *work)
+{
+	struct tm6000_IR *ir = container_of(work, struct tm6000_IR, work.work);
+	struct tm6000_core *dev = ir->dev;
+	int rc;
+	u8 buf[2];
+
+	if (ir->wait)
+		return;
+
+	dprintk(3, "%s\n",__func__);
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN |
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+		REQ_02_GET_IR_CODE, 0, 0, buf, 2);
+	if (rc < 0)
+		return;
+
+	/* Check if something was read */
+	if ((buf[0] & 0xff) == 0xff) {
+		if (!ir->pwled) {
+			tm6000_flash_led(dev, 1);
+			ir->pwled = 1;
+		}
+		return;
+	}
+
+	tm6000_ir_keydown(ir, buf, rc);
+	tm6000_flash_led(dev, 0);
+	ir->pwled = 0;
+
+	/* Re-schedule polling */
+	schedule_delayed_work(&ir->work, msecs_to_jiffies(ir->polling));
+}
+
+static void tm6000_ir_int_work(struct work_struct *work)
+{
+	struct tm6000_IR *ir = container_of(work, struct tm6000_IR, work.work);
+	struct tm6000_core *dev = ir->dev;
+	int rc;
+
+	dprintk(3, "%s, submit_urb = %d, pwled = %d\n",__func__, ir->submit_urb,
+		ir->pwled);
+
+	if (ir->submit_urb) {
+		dprintk(3, "Resubmit urb\n");
+		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 2, 0);
+
+		rc = usb_submit_urb(ir->int_urb, GFP_ATOMIC);
+		if (rc < 0) {
+			printk(KERN_ERR "tm6000: Can't submit an IR interrupt. Error %i\n",
+			       rc);
+			/* Retry in 100 ms */
+			schedule_delayed_work(&ir->work, msecs_to_jiffies(URB_SUBMIT_DELAY));
+			return;
+		}
+		ir->submit_urb = 0;
+	}
+
+	/* Led is enabled only if USB submit doesn't fail */
+	if (ir->pwled == 2) {
+		tm6000_flash_led(dev, 0);
+		ir->pwled = 0;
+		schedule_delayed_work(&ir->work, msecs_to_jiffies(URB_INT_LED_DELAY));
+	} else if (!ir->pwled) {
+		tm6000_flash_led(dev, 1);
+		ir->pwled = 1;
+	}
+}
+
+static int tm6000_ir_start(struct rc_dev *rc)
+{
+	struct tm6000_IR *ir = rc->priv;
+
+	dprintk(2, "%s\n",__func__);
+
+	schedule_delayed_work(&ir->work, 0);
+
+	return 0;
+}
+
+static void tm6000_ir_stop(struct rc_dev *rc)
+{
+	struct tm6000_IR *ir = rc->priv;
+
+	dprintk(2, "%s\n",__func__);
+
+	cancel_delayed_work_sync(&ir->work);
+}
+
+static int tm6000_ir_change_protocol(struct rc_dev *rc, u64 *rc_proto)
+{
+	struct tm6000_IR *ir = rc->priv;
+
+	if (!ir)
+		return 0;
+
+	dprintk(2, "%s\n",__func__);
+
+	ir->rc_proto = *rc_proto;
+
+	tm6000_ir_config(ir);
+	/* TODO */
+	return 0;
+}
+
+static int __tm6000_ir_int_start(struct rc_dev *rc)
+{
+	struct tm6000_IR *ir = rc->priv;
+	struct tm6000_core *dev;
+	int pipe, size;
+	int err = -ENOMEM;
+
+	if (!ir)
+		return -ENODEV;
+	dev = ir->dev;
+
+	dprintk(2, "%s\n",__func__);
+
+	ir->int_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!ir->int_urb)
+		return -ENOMEM;
+
+	pipe = usb_rcvintpipe(dev->udev,
+		dev->int_in.endp->desc.bEndpointAddress
+		& USB_ENDPOINT_NUMBER_MASK);
+
+	size = usb_maxpacket(dev->udev, pipe);
+	dprintk(1, "IR max size: %d\n", size);
+
+	ir->int_urb->transfer_buffer = kzalloc(size, GFP_ATOMIC);
+	if (!ir->int_urb->transfer_buffer) {
+		usb_free_urb(ir->int_urb);
+		return err;
+	}
+	dprintk(1, "int interval: %d\n", dev->int_in.endp->desc.bInterval);
+
+	usb_fill_int_urb(ir->int_urb, dev->udev, pipe,
+		ir->int_urb->transfer_buffer, size,
+		tm6000_ir_urb_received, dev,
+		dev->int_in.endp->desc.bInterval);
+
+	ir->submit_urb = 1;
+	schedule_delayed_work(&ir->work, msecs_to_jiffies(URB_SUBMIT_DELAY));
+
+	return 0;
+}
+
+static void __tm6000_ir_int_stop(struct rc_dev *rc)
+{
+	struct tm6000_IR *ir = rc->priv;
+
+	if (!ir || !ir->int_urb)
+		return;
+
+	dprintk(2, "%s\n",__func__);
+
+	usb_kill_urb(ir->int_urb);
+	kfree(ir->int_urb->transfer_buffer);
+	usb_free_urb(ir->int_urb);
+	ir->int_urb = NULL;
+}
+
+int tm6000_ir_int_start(struct tm6000_core *dev)
+{
+	struct tm6000_IR *ir = dev->ir;
+
+	if (!ir)
+		return 0;
+
+	return __tm6000_ir_int_start(ir->rc);
+}
+
+void tm6000_ir_int_stop(struct tm6000_core *dev)
+{
+	struct tm6000_IR *ir = dev->ir;
+
+	if (!ir || !ir->rc)
+		return;
+
+	__tm6000_ir_int_stop(ir->rc);
+}
+
+int tm6000_ir_init(struct tm6000_core *dev)
+{
+	struct tm6000_IR *ir;
+	struct rc_dev *rc;
+	int err = -ENOMEM;
+	u64 rc_proto;
+
+	if (!enable_ir)
+		return -ENODEV;
+
+	if (!dev->caps.has_remote)
+		return 0;
+
+	if (!dev->ir_codes)
+		return 0;
+
+	ir = kzalloc(sizeof(*ir), GFP_ATOMIC);
+	rc = rc_allocate_device(RC_DRIVER_SCANCODE);
+	if (!ir || !rc)
+		goto out;
+
+	dprintk(2, "%s\n", __func__);
+
+	/* record handles to ourself */
+	ir->dev = dev;
+	dev->ir = ir;
+	ir->rc = rc;
+
+	/* input setup */
+	rc->allowed_protocols = RC_PROTO_BIT_RC5 | RC_PROTO_BIT_NEC;
+	/* Needed, in order to support NEC remotes with 24 or 32 bits */
+	rc->scancode_mask = 0xffff;
+	rc->priv = ir;
+	rc->change_protocol = tm6000_ir_change_protocol;
+	if (dev->int_in.endp) {
+		rc->open    = __tm6000_ir_int_start;
+		rc->close   = __tm6000_ir_int_stop;
+		INIT_DELAYED_WORK(&ir->work, tm6000_ir_int_work);
+	} else {
+		rc->open  = tm6000_ir_start;
+		rc->close = tm6000_ir_stop;
+		ir->polling = 50;
+		INIT_DELAYED_WORK(&ir->work, tm6000_ir_handle_key);
+	}
+
+	snprintf(ir->name, sizeof(ir->name), "tm5600/60x0 IR (%s)",
+						dev->name);
+
+	usb_make_path(dev->udev, ir->phys, sizeof(ir->phys));
+	strlcat(ir->phys, "/input0", sizeof(ir->phys));
+
+	rc_proto = RC_PROTO_BIT_UNKNOWN;
+	tm6000_ir_change_protocol(rc, &rc_proto);
+
+	rc->device_name = ir->name;
+	rc->input_phys = ir->phys;
+	rc->input_id.bustype = BUS_USB;
+	rc->input_id.version = 1;
+	rc->input_id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
+	rc->input_id.product = le16_to_cpu(dev->udev->descriptor.idProduct);
+	rc->map_name = dev->ir_codes;
+	rc->driver_name = "tm6000";
+	rc->dev.parent = &dev->udev->dev;
+
+	/* ir register */
+	err = rc_register_device(rc);
+	if (err)
+		goto out;
+
+	return 0;
+
+out:
+	dev->ir = NULL;
+	rc_free_device(rc);
+	kfree(ir);
+	return err;
+}
+
+int tm6000_ir_fini(struct tm6000_core *dev)
+{
+	struct tm6000_IR *ir = dev->ir;
+
+	/* skip detach on non attached board */
+
+	if (!ir)
+		return 0;
+
+	dprintk(2, "%s\n",__func__);
+
+	if (!ir->polling)
+		__tm6000_ir_int_stop(ir->rc);
+
+	tm6000_ir_stop(ir->rc);
+
+	/* Turn off the led */
+	tm6000_flash_led(dev, 0);
+	ir->pwled = 0;
+
+	rc_unregister_device(ir->rc);
+
+	kfree(ir);
+	dev->ir = NULL;
+
+	return 0;
+}
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-regs.h b/drivers/staging/media/deprecated/tm6000/tm6000-regs.h
new file mode 100644
index 000000000..6a181f2e7
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-regs.h
@@ -0,0 +1,588 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * tm6000-regs.h - driver for TM5600/TM6000/TM6010 USB video capture devices
+ *
+ * Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+/*
+ * Define TV Master TM5600/TM6000/TM6010 Request codes
+ */
+#define REQ_00_SET_IR_VALUE		0
+#define REQ_01_SET_WAKEUP_IRCODE	1
+#define REQ_02_GET_IR_CODE		2
+#define REQ_03_SET_GET_MCU_PIN		3
+#define REQ_04_EN_DISABLE_MCU_INT	4
+#define REQ_05_SET_GET_USBREG		5
+	/* Write: RegNum, Value, 0 */
+	/* Read : RegNum, Value, 1, RegStatus */
+#define REQ_06_SET_GET_USBREG_BIT	6
+#define REQ_07_SET_GET_AVREG		7
+	/* Write: RegNum, Value, 0 */
+	/* Read : RegNum, Value, 1, RegStatus */
+#define REQ_08_SET_GET_AVREG_BIT	8
+#define REQ_09_SET_GET_TUNER_FQ		9
+#define REQ_10_SET_TUNER_SYSTEM		10
+#define REQ_11_SET_EEPROM_ADDR		11
+#define REQ_12_SET_GET_EEPROMBYTE	12
+#define REQ_13_GET_EEPROM_SEQREAD	13
+#define REQ_14_SET_GET_I2C_WR2_RDN	14
+#define REQ_15_SET_GET_I2CBYTE		15
+	/* Write: Subaddr, Slave Addr, value, 0 */
+	/* Read : Subaddr, Slave Addr, value, 1 */
+#define REQ_16_SET_GET_I2C_WR1_RDN	16
+	/* Subaddr, Slave Addr, 0, length */
+#define REQ_17_SET_GET_I2CFP		17
+	/* Write: Slave Addr, register, value */
+	/* Read : Slave Addr, register, 2, data */
+#define REQ_20_DATA_TRANSFER		20
+#define REQ_30_I2C_WRITE		30
+#define REQ_31_I2C_READ			31
+#define REQ_35_AFTEK_TUNER_READ		35
+#define REQ_40_GET_VERSION		40
+#define REQ_50_SET_START		50
+#define REQ_51_SET_STOP			51
+#define REQ_52_TRANSMIT_DATA		52
+#define REQ_53_SPI_INITIAL		53
+#define REQ_54_SPI_SETSTART		54
+#define REQ_55_SPI_INOUTDATA		55
+#define REQ_56_SPI_SETSTOP		56
+
+/*
+ * Define TV Master TM5600/TM6000/TM6010 GPIO lines
+ */
+
+#define TM6000_GPIO_CLK		0x101
+#define TM6000_GPIO_DATA	0x100
+
+#define TM6000_GPIO_1		0x102
+#define TM6000_GPIO_2		0x103
+#define TM6000_GPIO_3		0x104
+#define TM6000_GPIO_4		0x300
+#define TM6000_GPIO_5		0x301
+#define TM6000_GPIO_6		0x304
+#define TM6000_GPIO_7		0x305
+
+/* tm6010 defines GPIO with different values */
+#define TM6010_GPIO_0      0x0102
+#define TM6010_GPIO_1      0x0103
+#define TM6010_GPIO_2      0x0104
+#define TM6010_GPIO_3      0x0105
+#define TM6010_GPIO_4      0x0106
+#define TM6010_GPIO_5      0x0107
+#define TM6010_GPIO_6      0x0300
+#define TM6010_GPIO_7      0x0301
+#define TM6010_GPIO_9      0x0305
+/*
+ * Define TV Master TM5600/TM6000/TM6010 URB message codes and length
+ */
+
+enum {
+	TM6000_URB_MSG_VIDEO = 1,
+	TM6000_URB_MSG_AUDIO,
+	TM6000_URB_MSG_VBI,
+	TM6000_URB_MSG_PTS,
+	TM6000_URB_MSG_ERR,
+};
+
+/* Define specific TM6000 Video decoder registers */
+#define TM6000_REQ07_RD8_TEST_SEL			0x07, 0xd8
+#define TM6000_REQ07_RD9_A_SIM_SEL			0x07, 0xd9
+#define TM6000_REQ07_RDA_CLK_SEL			0x07, 0xda
+#define TM6000_REQ07_RDB_OUT_SEL			0x07, 0xdb
+#define TM6000_REQ07_RDC_NSEL_I2S			0x07, 0xdc
+#define TM6000_REQ07_RDD_GPIO2_MDRV			0x07, 0xdd
+#define TM6000_REQ07_RDE_GPIO1_MDRV			0x07, 0xde
+#define TM6000_REQ07_RDF_PWDOWN_ACLK			0x07, 0xdf
+#define TM6000_REQ07_RE0_VADC_REF_CTL			0x07, 0xe0
+#define TM6000_REQ07_RE1_VADC_DACLIMP			0x07, 0xe1
+#define TM6000_REQ07_RE2_VADC_STATUS_CTL		0x07, 0xe2
+#define TM6000_REQ07_RE3_VADC_INP_LPF_SEL1		0x07, 0xe3
+#define TM6000_REQ07_RE4_VADC_TARGET1			0x07, 0xe4
+#define TM6000_REQ07_RE5_VADC_INP_LPF_SEL2		0x07, 0xe5
+#define TM6000_REQ07_RE6_VADC_TARGET2			0x07, 0xe6
+#define TM6000_REQ07_RE7_VADC_AGAIN_CTL			0x07, 0xe7
+#define TM6000_REQ07_RE8_VADC_PWDOWN_CTL		0x07, 0xe8
+#define TM6000_REQ07_RE9_VADC_INPUT_CTL1		0x07, 0xe9
+#define TM6000_REQ07_REA_VADC_INPUT_CTL2		0x07, 0xea
+#define TM6000_REQ07_REB_VADC_AADC_MODE			0x07, 0xeb
+#define TM6000_REQ07_REC_VADC_AADC_LVOL			0x07, 0xec
+#define TM6000_REQ07_RED_VADC_AADC_RVOL			0x07, 0xed
+#define TM6000_REQ07_REE_VADC_CTRL_SEL_CONTROL		0x07, 0xee
+#define TM6000_REQ07_REF_VADC_GAIN_MAP_CTL		0x07, 0xef
+#define TM6000_REQ07_RFD_BIST_ERR_VST_LOW		0x07, 0xfd
+#define TM6000_REQ07_RFE_BIST_ERR_VST_HIGH		0x07, 0xfe
+
+/* Define TM6000/TM6010 Video decoder registers */
+#define TM6010_REQ07_R00_VIDEO_CONTROL0			0x07, 0x00
+#define TM6010_REQ07_R01_VIDEO_CONTROL1			0x07, 0x01
+#define TM6010_REQ07_R02_VIDEO_CONTROL2			0x07, 0x02
+#define TM6010_REQ07_R03_YC_SEP_CONTROL			0x07, 0x03
+#define TM6010_REQ07_R04_LUMA_HAGC_CONTROL		0x07, 0x04
+#define TM6010_REQ07_R05_NOISE_THRESHOLD		0x07, 0x05
+#define TM6010_REQ07_R06_AGC_GATE_THRESHOLD		0x07, 0x06
+#define TM6010_REQ07_R07_OUTPUT_CONTROL			0x07, 0x07
+#define TM6010_REQ07_R08_LUMA_CONTRAST_ADJ		0x07, 0x08
+#define TM6010_REQ07_R09_LUMA_BRIGHTNESS_ADJ		0x07, 0x09
+#define TM6010_REQ07_R0A_CHROMA_SATURATION_ADJ		0x07, 0x0a
+#define TM6010_REQ07_R0B_CHROMA_HUE_PHASE_ADJ		0x07, 0x0b
+#define TM6010_REQ07_R0C_CHROMA_AGC_CONTROL		0x07, 0x0c
+#define TM6010_REQ07_R0D_CHROMA_KILL_LEVEL		0x07, 0x0d
+#define TM6010_REQ07_R0F_CHROMA_AUTO_POSITION		0x07, 0x0f
+#define TM6010_REQ07_R10_AGC_PEAK_NOMINAL		0x07, 0x10
+#define TM6010_REQ07_R11_AGC_PEAK_CONTROL		0x07, 0x11
+#define TM6010_REQ07_R12_AGC_GATE_STARTH		0x07, 0x12
+#define TM6010_REQ07_R13_AGC_GATE_STARTL		0x07, 0x13
+#define TM6010_REQ07_R14_AGC_GATE_WIDTH			0x07, 0x14
+#define TM6010_REQ07_R15_AGC_BP_DELAY			0x07, 0x15
+#define TM6010_REQ07_R16_LOCK_COUNT			0x07, 0x16
+#define TM6010_REQ07_R17_HLOOP_MAXSTATE			0x07, 0x17
+#define TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3		0x07, 0x18
+#define TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2		0x07, 0x19
+#define TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1		0x07, 0x1a
+#define TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0		0x07, 0x1b
+#define TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3		0x07, 0x1c
+#define TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2		0x07, 0x1d
+#define TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1		0x07, 0x1e
+#define TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0		0x07, 0x1f
+#define TM6010_REQ07_R20_HSYNC_RISING_EDGE_TIME		0x07, 0x20
+#define TM6010_REQ07_R21_HSYNC_PHASE_OFFSET		0x07, 0x21
+#define TM6010_REQ07_R22_HSYNC_PLL_START_TIME		0x07, 0x22
+#define TM6010_REQ07_R23_HSYNC_PLL_END_TIME		0x07, 0x23
+#define TM6010_REQ07_R24_HSYNC_TIP_START_TIME		0x07, 0x24
+#define TM6010_REQ07_R25_HSYNC_TIP_END_TIME		0x07, 0x25
+#define TM6010_REQ07_R26_HSYNC_RISING_EDGE_START	0x07, 0x26
+#define TM6010_REQ07_R27_HSYNC_RISING_EDGE_END		0x07, 0x27
+#define TM6010_REQ07_R28_BACKPORCH_START		0x07, 0x28
+#define TM6010_REQ07_R29_BACKPORCH_END			0x07, 0x29
+#define TM6010_REQ07_R2A_HSYNC_FILTER_START		0x07, 0x2a
+#define TM6010_REQ07_R2B_HSYNC_FILTER_END		0x07, 0x2b
+#define TM6010_REQ07_R2C_CHROMA_BURST_START		0x07, 0x2c
+#define TM6010_REQ07_R2D_CHROMA_BURST_END		0x07, 0x2d
+#define TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART		0x07, 0x2e
+#define TM6010_REQ07_R2F_ACTIVE_VIDEO_HWIDTH		0x07, 0x2f
+#define TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART		0x07, 0x30
+#define TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT		0x07, 0x31
+#define TM6010_REQ07_R32_VSYNC_HLOCK_MIN		0x07, 0x32
+#define TM6010_REQ07_R33_VSYNC_HLOCK_MAX		0x07, 0x33
+#define TM6010_REQ07_R34_VSYNC_AGC_MIN			0x07, 0x34
+#define TM6010_REQ07_R35_VSYNC_AGC_MAX			0x07, 0x35
+#define TM6010_REQ07_R36_VSYNC_VBI_MIN			0x07, 0x36
+#define TM6010_REQ07_R37_VSYNC_VBI_MAX			0x07, 0x37
+#define TM6010_REQ07_R38_VSYNC_THRESHOLD		0x07, 0x38
+#define TM6010_REQ07_R39_VSYNC_TIME_CONSTANT		0x07, 0x39
+#define TM6010_REQ07_R3A_STATUS1			0x07, 0x3a
+#define TM6010_REQ07_R3B_STATUS2			0x07, 0x3b
+#define TM6010_REQ07_R3C_STATUS3			0x07, 0x3c
+#define TM6010_REQ07_R3F_RESET				0x07, 0x3f
+#define TM6010_REQ07_R40_TELETEXT_VBI_CODE0		0x07, 0x40
+#define TM6010_REQ07_R41_TELETEXT_VBI_CODE1		0x07, 0x41
+#define TM6010_REQ07_R42_VBI_DATA_HIGH_LEVEL		0x07, 0x42
+#define TM6010_REQ07_R43_VBI_DATA_TYPE_LINE7		0x07, 0x43
+#define TM6010_REQ07_R44_VBI_DATA_TYPE_LINE8		0x07, 0x44
+#define TM6010_REQ07_R45_VBI_DATA_TYPE_LINE9		0x07, 0x45
+#define TM6010_REQ07_R46_VBI_DATA_TYPE_LINE10		0x07, 0x46
+#define TM6010_REQ07_R47_VBI_DATA_TYPE_LINE11		0x07, 0x47
+#define TM6010_REQ07_R48_VBI_DATA_TYPE_LINE12		0x07, 0x48
+#define TM6010_REQ07_R49_VBI_DATA_TYPE_LINE13		0x07, 0x49
+#define TM6010_REQ07_R4A_VBI_DATA_TYPE_LINE14		0x07, 0x4a
+#define TM6010_REQ07_R4B_VBI_DATA_TYPE_LINE15		0x07, 0x4b
+#define TM6010_REQ07_R4C_VBI_DATA_TYPE_LINE16		0x07, 0x4c
+#define TM6010_REQ07_R4D_VBI_DATA_TYPE_LINE17		0x07, 0x4d
+#define TM6010_REQ07_R4E_VBI_DATA_TYPE_LINE18		0x07, 0x4e
+#define TM6010_REQ07_R4F_VBI_DATA_TYPE_LINE19		0x07, 0x4f
+#define TM6010_REQ07_R50_VBI_DATA_TYPE_LINE20		0x07, 0x50
+#define TM6010_REQ07_R51_VBI_DATA_TYPE_LINE21		0x07, 0x51
+#define TM6010_REQ07_R52_VBI_DATA_TYPE_LINE22		0x07, 0x52
+#define TM6010_REQ07_R53_VBI_DATA_TYPE_LINE23		0x07, 0x53
+#define TM6010_REQ07_R54_VBI_DATA_TYPE_RLINES		0x07, 0x54
+#define TM6010_REQ07_R55_VBI_LOOP_FILTER_GAIN		0x07, 0x55
+#define TM6010_REQ07_R56_VBI_LOOP_FILTER_I_GAIN		0x07, 0x56
+#define TM6010_REQ07_R57_VBI_LOOP_FILTER_P_GAIN		0x07, 0x57
+#define TM6010_REQ07_R58_VBI_CAPTION_DTO1		0x07, 0x58
+#define TM6010_REQ07_R59_VBI_CAPTION_DTO0		0x07, 0x59
+#define TM6010_REQ07_R5A_VBI_TELETEXT_DTO1		0x07, 0x5a
+#define TM6010_REQ07_R5B_VBI_TELETEXT_DTO0		0x07, 0x5b
+#define TM6010_REQ07_R5C_VBI_WSS625_DTO1		0x07, 0x5c
+#define TM6010_REQ07_R5D_VBI_WSS625_DTO0		0x07, 0x5d
+#define TM6010_REQ07_R5E_VBI_CAPTION_FRAME_START	0x07, 0x5e
+#define TM6010_REQ07_R5F_VBI_WSS625_FRAME_START		0x07, 0x5f
+#define TM6010_REQ07_R60_TELETEXT_FRAME_START		0x07, 0x60
+#define TM6010_REQ07_R61_VBI_CCDATA1			0x07, 0x61
+#define TM6010_REQ07_R62_VBI_CCDATA2			0x07, 0x62
+#define TM6010_REQ07_R63_VBI_WSS625_DATA1		0x07, 0x63
+#define TM6010_REQ07_R64_VBI_WSS625_DATA2		0x07, 0x64
+#define TM6010_REQ07_R65_VBI_DATA_STATUS		0x07, 0x65
+#define TM6010_REQ07_R66_VBI_CAPTION_START		0x07, 0x66
+#define TM6010_REQ07_R67_VBI_WSS625_START		0x07, 0x67
+#define TM6010_REQ07_R68_VBI_TELETEXT_START		0x07, 0x68
+#define TM6010_REQ07_R70_HSYNC_DTO_INC_STATUS3		0x07, 0x70
+#define TM6010_REQ07_R71_HSYNC_DTO_INC_STATUS2		0x07, 0x71
+#define TM6010_REQ07_R72_HSYNC_DTO_INC_STATUS1		0x07, 0x72
+#define TM6010_REQ07_R73_HSYNC_DTO_INC_STATUS0		0x07, 0x73
+#define TM6010_REQ07_R74_CHROMA_DTO_INC_STATUS3		0x07, 0x74
+#define TM6010_REQ07_R75_CHROMA_DTO_INC_STATUS2		0x07, 0x75
+#define TM6010_REQ07_R76_CHROMA_DTO_INC_STATUS1		0x07, 0x76
+#define TM6010_REQ07_R77_CHROMA_DTO_INC_STATUS0		0x07, 0x77
+#define TM6010_REQ07_R78_AGC_AGAIN_STATUS		0x07, 0x78
+#define TM6010_REQ07_R79_AGC_DGAIN_STATUS		0x07, 0x79
+#define TM6010_REQ07_R7A_CHROMA_MAG_STATUS		0x07, 0x7a
+#define TM6010_REQ07_R7B_CHROMA_GAIN_STATUS1		0x07, 0x7b
+#define TM6010_REQ07_R7C_CHROMA_GAIN_STATUS0		0x07, 0x7c
+#define TM6010_REQ07_R7D_CORDIC_FREQ_STATUS		0x07, 0x7d
+#define TM6010_REQ07_R7F_STATUS_NOISE			0x07, 0x7f
+#define TM6010_REQ07_R80_COMB_FILTER_TRESHOLD		0x07, 0x80
+#define TM6010_REQ07_R82_COMB_FILTER_CONFIG		0x07, 0x82
+#define TM6010_REQ07_R83_CHROMA_LOCK_CONFIG		0x07, 0x83
+#define TM6010_REQ07_R84_NOISE_NTSC_C			0x07, 0x84
+#define TM6010_REQ07_R85_NOISE_PAL_C			0x07, 0x85
+#define TM6010_REQ07_R86_NOISE_PHASE_C			0x07, 0x86
+#define TM6010_REQ07_R87_NOISE_PHASE_Y			0x07, 0x87
+#define TM6010_REQ07_R8A_CHROMA_LOOPFILTER_STATE	0x07, 0x8a
+#define TM6010_REQ07_R8B_CHROMA_HRESAMPLER		0x07, 0x8b
+#define TM6010_REQ07_R8D_CPUMP_DELAY_ADJ		0x07, 0x8d
+#define TM6010_REQ07_R8E_CPUMP_ADJ			0x07, 0x8e
+#define TM6010_REQ07_R8F_CPUMP_DELAY			0x07, 0x8f
+
+/* Define TM6000/TM6010 Miscellaneous registers */
+#define TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE		0x07, 0xc0
+#define TM6010_REQ07_RC1_TRESHOLD			0x07, 0xc1
+#define TM6010_REQ07_RC2_HSYNC_WIDTH			0x07, 0xc2
+#define TM6010_REQ07_RC3_HSTART1			0x07, 0xc3
+#define TM6010_REQ07_RC4_HSTART0			0x07, 0xc4
+#define TM6010_REQ07_RC5_HEND1				0x07, 0xc5
+#define TM6010_REQ07_RC6_HEND0				0x07, 0xc6
+#define TM6010_REQ07_RC7_VSTART1			0x07, 0xc7
+#define TM6010_REQ07_RC8_VSTART0			0x07, 0xc8
+#define TM6010_REQ07_RC9_VEND1				0x07, 0xc9
+#define TM6010_REQ07_RCA_VEND0				0x07, 0xca
+#define TM6010_REQ07_RCB_DELAY				0x07, 0xcb
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RCC_ACTIVE_IF			0x07, 0xcc
+#define TM6010_REQ07_RCC_ACTIVE_IF_VIDEO_ENABLE (1 << 5)
+#define TM6010_REQ07_RCC_ACTIVE_IF_AUDIO_ENABLE (1 << 6)
+#define TM6010_REQ07_RD0_USB_PERIPHERY_CONTROL		0x07, 0xd0
+#define TM6010_REQ07_RD1_ADDR_FOR_REQ1			0x07, 0xd1
+#define TM6010_REQ07_RD2_ADDR_FOR_REQ2			0x07, 0xd2
+#define TM6010_REQ07_RD3_ADDR_FOR_REQ3			0x07, 0xd3
+#define TM6010_REQ07_RD4_ADDR_FOR_REQ4			0x07, 0xd4
+#define TM6010_REQ07_RD5_POWERSAVE			0x07, 0xd5
+#define TM6010_REQ07_RD6_ENDP_REQ1_REQ2			0x07, 0xd6
+#define TM6010_REQ07_RD7_ENDP_REQ3_REQ4			0x07, 0xd7
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RD8_IR				0x07, 0xd8
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RD9_IR_BSIZE			0x07, 0xd9
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDA_IR_WAKEUP_SEL			0x07, 0xda
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDB_IR_WAKEUP_ADD			0x07, 0xdb
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDC_IR_LEADER1			0x07, 0xdc
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDD_IR_LEADER0			0x07, 0xdd
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDE_IR_PULSE_CNT1			0x07, 0xde
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RDF_IR_PULSE_CNT0			0x07, 0xdf
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE0_DVIDEO_SOURCE			0x07, 0xe0
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE0_DVIDEO_SOURCE_IF		0x07, 0xe1
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE2_OUT_SEL2			0x07, 0xe2
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE3_OUT_SEL1			0x07, 0xe3
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE4_OUT_SEL0			0x07, 0xe4
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE5_REMOTE_WAKEUP			0x07, 0xe5
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE7_PUB_GPIO			0x07, 0xe7
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE8_TYPESEL_MOS_I2S		0x07, 0xe8
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RE9_TYPESEL_MOS_TS			0x07, 0xe9
+/* ONLY for TM6010 */
+#define TM6010_REQ07_REA_TYPESEL_MOS_CCIR		0x07, 0xea
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF0_BIST_CRC_RESULT0		0x07, 0xf0
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF1_BIST_CRC_RESULT1		0x07, 0xf1
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF2_BIST_CRC_RESULT2		0x07, 0xf2
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF3_BIST_CRC_RESULT3		0x07, 0xf3
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF4_BIST_ERR_VST2			0x07, 0xf4
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF5_BIST_ERR_VST1			0x07, 0xf5
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF6_BIST_ERR_VST0			0x07, 0xf6
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RF7_BIST				0x07, 0xf7
+/* ONLY for TM6010 */
+#define TM6010_REQ07_RFE_POWER_DOWN			0x07, 0xfe
+#define TM6010_REQ07_RFF_SOFT_RESET			0x07, 0xff
+
+/* Define TM6000/TM6010 USB registers */
+#define TM6010_REQ05_R00_MAIN_CTRL		0x05, 0x00
+#define TM6010_REQ05_R01_DEVADDR		0x05, 0x01
+#define TM6010_REQ05_R02_TEST			0x05, 0x02
+#define TM6010_REQ05_R04_SOFN0			0x05, 0x04
+#define TM6010_REQ05_R05_SOFN1			0x05, 0x05
+#define TM6010_REQ05_R06_SOFTM0			0x05, 0x06
+#define TM6010_REQ05_R07_SOFTM1			0x05, 0x07
+#define TM6010_REQ05_R08_PHY_TEST		0x05, 0x08
+#define TM6010_REQ05_R09_VCTL			0x05, 0x09
+#define TM6010_REQ05_R0A_VSTA			0x05, 0x0a
+#define TM6010_REQ05_R0B_CX_CFG			0x05, 0x0b
+#define TM6010_REQ05_R0C_ENDP0_REG0		0x05, 0x0c
+#define TM6010_REQ05_R10_GMASK			0x05, 0x10
+#define TM6010_REQ05_R11_IMASK0			0x05, 0x11
+#define TM6010_REQ05_R12_IMASK1			0x05, 0x12
+#define TM6010_REQ05_R13_IMASK2			0x05, 0x13
+#define TM6010_REQ05_R14_IMASK3			0x05, 0x14
+#define TM6010_REQ05_R15_IMASK4			0x05, 0x15
+#define TM6010_REQ05_R16_IMASK5			0x05, 0x16
+#define TM6010_REQ05_R17_IMASK6			0x05, 0x17
+#define TM6010_REQ05_R18_IMASK7			0x05, 0x18
+#define TM6010_REQ05_R19_ZEROP0			0x05, 0x19
+#define TM6010_REQ05_R1A_ZEROP1			0x05, 0x1a
+#define TM6010_REQ05_R1C_FIFO_EMP0		0x05, 0x1c
+#define TM6010_REQ05_R1D_FIFO_EMP1		0x05, 0x1d
+#define TM6010_REQ05_R20_IRQ_GROUP		0x05, 0x20
+#define TM6010_REQ05_R21_IRQ_SOURCE0		0x05, 0x21
+#define TM6010_REQ05_R22_IRQ_SOURCE1		0x05, 0x22
+#define TM6010_REQ05_R23_IRQ_SOURCE2		0x05, 0x23
+#define TM6010_REQ05_R24_IRQ_SOURCE3		0x05, 0x24
+#define TM6010_REQ05_R25_IRQ_SOURCE4		0x05, 0x25
+#define TM6010_REQ05_R26_IRQ_SOURCE5		0x05, 0x26
+#define TM6010_REQ05_R27_IRQ_SOURCE6		0x05, 0x27
+#define TM6010_REQ05_R28_IRQ_SOURCE7		0x05, 0x28
+#define TM6010_REQ05_R29_SEQ_ERR0		0x05, 0x29
+#define TM6010_REQ05_R2A_SEQ_ERR1		0x05, 0x2a
+#define TM6010_REQ05_R2B_SEQ_ABORT0		0x05, 0x2b
+#define TM6010_REQ05_R2C_SEQ_ABORT1		0x05, 0x2c
+#define TM6010_REQ05_R2D_TX_ZERO0		0x05, 0x2d
+#define TM6010_REQ05_R2E_TX_ZERO1		0x05, 0x2e
+#define TM6010_REQ05_R2F_IDLE_CNT		0x05, 0x2f
+#define TM6010_REQ05_R30_FNO_P1			0x05, 0x30
+#define TM6010_REQ05_R31_FNO_P2			0x05, 0x31
+#define TM6010_REQ05_R32_FNO_P3			0x05, 0x32
+#define TM6010_REQ05_R33_FNO_P4			0x05, 0x33
+#define TM6010_REQ05_R34_FNO_P5			0x05, 0x34
+#define TM6010_REQ05_R35_FNO_P6			0x05, 0x35
+#define TM6010_REQ05_R36_FNO_P7			0x05, 0x36
+#define TM6010_REQ05_R37_FNO_P8			0x05, 0x37
+#define TM6010_REQ05_R38_FNO_P9			0x05, 0x38
+#define TM6010_REQ05_R30_FNO_P10		0x05, 0x39
+#define TM6010_REQ05_R30_FNO_P11		0x05, 0x3a
+#define TM6010_REQ05_R30_FNO_P12		0x05, 0x3b
+#define TM6010_REQ05_R30_FNO_P13		0x05, 0x3c
+#define TM6010_REQ05_R30_FNO_P14		0x05, 0x3d
+#define TM6010_REQ05_R30_FNO_P15		0x05, 0x3e
+#define TM6010_REQ05_R40_IN_MAXPS_LOW1		0x05, 0x40
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH1		0x05, 0x41
+#define TM6010_REQ05_R42_IN_MAXPS_LOW2		0x05, 0x42
+#define TM6010_REQ05_R43_IN_MAXPS_HIGH2		0x05, 0x43
+#define TM6010_REQ05_R44_IN_MAXPS_LOW3		0x05, 0x44
+#define TM6010_REQ05_R45_IN_MAXPS_HIGH3		0x05, 0x45
+#define TM6010_REQ05_R46_IN_MAXPS_LOW4		0x05, 0x46
+#define TM6010_REQ05_R47_IN_MAXPS_HIGH4		0x05, 0x47
+#define TM6010_REQ05_R48_IN_MAXPS_LOW5		0x05, 0x48
+#define TM6010_REQ05_R49_IN_MAXPS_HIGH5		0x05, 0x49
+#define TM6010_REQ05_R4A_IN_MAXPS_LOW6		0x05, 0x4a
+#define TM6010_REQ05_R4B_IN_MAXPS_HIGH6		0x05, 0x4b
+#define TM6010_REQ05_R4C_IN_MAXPS_LOW7		0x05, 0x4c
+#define TM6010_REQ05_R4D_IN_MAXPS_HIGH7		0x05, 0x4d
+#define TM6010_REQ05_R4E_IN_MAXPS_LOW8		0x05, 0x4e
+#define TM6010_REQ05_R4F_IN_MAXPS_HIGH8		0x05, 0x4f
+#define TM6010_REQ05_R50_IN_MAXPS_LOW9		0x05, 0x50
+#define TM6010_REQ05_R51_IN_MAXPS_HIGH9		0x05, 0x51
+#define TM6010_REQ05_R40_IN_MAXPS_LOW10		0x05, 0x52
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH10	0x05, 0x53
+#define TM6010_REQ05_R40_IN_MAXPS_LOW11		0x05, 0x54
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH11	0x05, 0x55
+#define TM6010_REQ05_R40_IN_MAXPS_LOW12		0x05, 0x56
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH12	0x05, 0x57
+#define TM6010_REQ05_R40_IN_MAXPS_LOW13		0x05, 0x58
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH13	0x05, 0x59
+#define TM6010_REQ05_R40_IN_MAXPS_LOW14		0x05, 0x5a
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH14	0x05, 0x5b
+#define TM6010_REQ05_R40_IN_MAXPS_LOW15		0x05, 0x5c
+#define TM6010_REQ05_R41_IN_MAXPS_HIGH15	0x05, 0x5d
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW1		0x05, 0x60
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH1	0x05, 0x61
+#define TM6010_REQ05_R62_OUT_MAXPS_LOW2		0x05, 0x62
+#define TM6010_REQ05_R63_OUT_MAXPS_HIGH2	0x05, 0x63
+#define TM6010_REQ05_R64_OUT_MAXPS_LOW3		0x05, 0x64
+#define TM6010_REQ05_R65_OUT_MAXPS_HIGH3	0x05, 0x65
+#define TM6010_REQ05_R66_OUT_MAXPS_LOW4		0x05, 0x66
+#define TM6010_REQ05_R67_OUT_MAXPS_HIGH4	0x05, 0x67
+#define TM6010_REQ05_R68_OUT_MAXPS_LOW5		0x05, 0x68
+#define TM6010_REQ05_R69_OUT_MAXPS_HIGH5	0x05, 0x69
+#define TM6010_REQ05_R6A_OUT_MAXPS_LOW6		0x05, 0x6a
+#define TM6010_REQ05_R6B_OUT_MAXPS_HIGH6	0x05, 0x6b
+#define TM6010_REQ05_R6C_OUT_MAXPS_LOW7		0x05, 0x6c
+#define TM6010_REQ05_R6D_OUT_MAXPS_HIGH7	0x05, 0x6d
+#define TM6010_REQ05_R6E_OUT_MAXPS_LOW8		0x05, 0x6e
+#define TM6010_REQ05_R6F_OUT_MAXPS_HIGH8	0x05, 0x6f
+#define TM6010_REQ05_R70_OUT_MAXPS_LOW9		0x05, 0x70
+#define TM6010_REQ05_R71_OUT_MAXPS_HIGH9	0x05, 0x71
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW10	0x05, 0x72
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH10	0x05, 0x73
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW11	0x05, 0x74
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH11	0x05, 0x75
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW12	0x05, 0x76
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH12	0x05, 0x77
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW13	0x05, 0x78
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH13	0x05, 0x79
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW14	0x05, 0x7a
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH14	0x05, 0x7b
+#define TM6010_REQ05_R60_OUT_MAXPS_LOW15	0x05, 0x7c
+#define TM6010_REQ05_R61_OUT_MAXPS_HIGH15	0x05, 0x7d
+#define TM6010_REQ05_R80_FIFO0			0x05, 0x80
+#define TM6010_REQ05_R81_FIFO1			0x05, 0x81
+#define TM6010_REQ05_R82_FIFO2			0x05, 0x82
+#define TM6010_REQ05_R83_FIFO3			0x05, 0x83
+#define TM6010_REQ05_R84_FIFO4			0x05, 0x84
+#define TM6010_REQ05_R85_FIFO5			0x05, 0x85
+#define TM6010_REQ05_R86_FIFO6			0x05, 0x86
+#define TM6010_REQ05_R87_FIFO7			0x05, 0x87
+#define TM6010_REQ05_R88_FIFO8			0x05, 0x88
+#define TM6010_REQ05_R89_FIFO9			0x05, 0x89
+#define TM6010_REQ05_R81_FIFO10			0x05, 0x8a
+#define TM6010_REQ05_R81_FIFO11			0x05, 0x8b
+#define TM6010_REQ05_R81_FIFO12			0x05, 0x8c
+#define TM6010_REQ05_R81_FIFO13			0x05, 0x8d
+#define TM6010_REQ05_R81_FIFO14			0x05, 0x8e
+#define TM6010_REQ05_R81_FIFO15			0x05, 0x8f
+#define TM6010_REQ05_R90_CFG_FIFO0		0x05, 0x90
+#define TM6010_REQ05_R91_CFG_FIFO1		0x05, 0x91
+#define TM6010_REQ05_R92_CFG_FIFO2		0x05, 0x92
+#define TM6010_REQ05_R93_CFG_FIFO3		0x05, 0x93
+#define TM6010_REQ05_R94_CFG_FIFO4		0x05, 0x94
+#define TM6010_REQ05_R95_CFG_FIFO5		0x05, 0x95
+#define TM6010_REQ05_R96_CFG_FIFO6		0x05, 0x96
+#define TM6010_REQ05_R97_CFG_FIFO7		0x05, 0x97
+#define TM6010_REQ05_R98_CFG_FIFO8		0x05, 0x98
+#define TM6010_REQ05_R99_CFG_FIFO9		0x05, 0x99
+#define TM6010_REQ05_R91_CFG_FIFO10		0x05, 0x9a
+#define TM6010_REQ05_R91_CFG_FIFO11		0x05, 0x9b
+#define TM6010_REQ05_R91_CFG_FIFO12		0x05, 0x9c
+#define TM6010_REQ05_R91_CFG_FIFO13		0x05, 0x9d
+#define TM6010_REQ05_R91_CFG_FIFO14		0x05, 0x9e
+#define TM6010_REQ05_R91_CFG_FIFO15		0x05, 0x9f
+#define TM6010_REQ05_RA0_CTL_FIFO0		0x05, 0xa0
+#define TM6010_REQ05_RA1_CTL_FIFO1		0x05, 0xa1
+#define TM6010_REQ05_RA2_CTL_FIFO2		0x05, 0xa2
+#define TM6010_REQ05_RA3_CTL_FIFO3		0x05, 0xa3
+#define TM6010_REQ05_RA4_CTL_FIFO4		0x05, 0xa4
+#define TM6010_REQ05_RA5_CTL_FIFO5		0x05, 0xa5
+#define TM6010_REQ05_RA6_CTL_FIFO6		0x05, 0xa6
+#define TM6010_REQ05_RA7_CTL_FIFO7		0x05, 0xa7
+#define TM6010_REQ05_RA8_CTL_FIFO8		0x05, 0xa8
+#define TM6010_REQ05_RA9_CTL_FIFO9		0x05, 0xa9
+#define TM6010_REQ05_RA1_CTL_FIFO10		0x05, 0xaa
+#define TM6010_REQ05_RA1_CTL_FIFO11		0x05, 0xab
+#define TM6010_REQ05_RA1_CTL_FIFO12		0x05, 0xac
+#define TM6010_REQ05_RA1_CTL_FIFO13		0x05, 0xad
+#define TM6010_REQ05_RA1_CTL_FIFO14		0x05, 0xae
+#define TM6010_REQ05_RA1_CTL_FIFO15		0x05, 0xaf
+#define TM6010_REQ05_RB0_BC_LOW_FIFO0		0x05, 0xb0
+#define TM6010_REQ05_RB1_BC_LOW_FIFO1		0x05, 0xb1
+#define TM6010_REQ05_RB2_BC_LOW_FIFO2		0x05, 0xb2
+#define TM6010_REQ05_RB3_BC_LOW_FIFO3		0x05, 0xb3
+#define TM6010_REQ05_RB4_BC_LOW_FIFO4		0x05, 0xb4
+#define TM6010_REQ05_RB5_BC_LOW_FIFO5		0x05, 0xb5
+#define TM6010_REQ05_RB6_BC_LOW_FIFO6		0x05, 0xb6
+#define TM6010_REQ05_RB7_BC_LOW_FIFO7		0x05, 0xb7
+#define TM6010_REQ05_RB8_BC_LOW_FIFO8		0x05, 0xb8
+#define TM6010_REQ05_RB9_BC_LOW_FIFO9		0x05, 0xb9
+#define TM6010_REQ05_RB1_BC_LOW_FIFO10		0x05, 0xba
+#define TM6010_REQ05_RB1_BC_LOW_FIFO11		0x05, 0xbb
+#define TM6010_REQ05_RB1_BC_LOW_FIFO12		0x05, 0xbc
+#define TM6010_REQ05_RB1_BC_LOW_FIFO13		0x05, 0xbd
+#define TM6010_REQ05_RB1_BC_LOW_FIFO14		0x05, 0xbe
+#define TM6010_REQ05_RB1_BC_LOW_FIFO15		0x05, 0xbf
+#define TM6010_REQ05_RC0_DATA_FIFO0		0x05, 0xc0
+#define TM6010_REQ05_RC4_DATA_FIFO1		0x05, 0xc4
+#define TM6010_REQ05_RC8_DATA_FIFO2		0x05, 0xc8
+#define TM6010_REQ05_RCC_DATA_FIFO3		0x05, 0xcc
+#define TM6010_REQ05_RD0_DATA_FIFO4		0x05, 0xd0
+#define TM6010_REQ05_RD4_DATA_FIFO5		0x05, 0xd4
+#define TM6010_REQ05_RD8_DATA_FIFO6		0x05, 0xd8
+#define TM6010_REQ05_RDC_DATA_FIFO7		0x05, 0xdc
+#define TM6010_REQ05_RE0_DATA_FIFO8		0x05, 0xe0
+#define TM6010_REQ05_RE4_DATA_FIFO9		0x05, 0xe4
+#define TM6010_REQ05_RC4_DATA_FIFO10		0x05, 0xe8
+#define TM6010_REQ05_RC4_DATA_FIFO11		0x05, 0xec
+#define TM6010_REQ05_RC4_DATA_FIFO12		0x05, 0xf0
+#define TM6010_REQ05_RC4_DATA_FIFO13		0x05, 0xf4
+#define TM6010_REQ05_RC4_DATA_FIFO14		0x05, 0xf8
+#define TM6010_REQ05_RC4_DATA_FIFO15		0x05, 0xfc
+
+/* Define TM6010 Audio decoder registers */
+/* This core available only in TM6010 */
+#define TM6010_REQ08_R00_A_VERSION		0x08, 0x00
+#define TM6010_REQ08_R01_A_INIT			0x08, 0x01
+#define TM6010_REQ08_R02_A_FIX_GAIN_CTRL	0x08, 0x02
+#define TM6010_REQ08_R03_A_AUTO_GAIN_CTRL	0x08, 0x03
+#define TM6010_REQ08_R04_A_SIF_AMP_CTRL		0x08, 0x04
+#define TM6010_REQ08_R05_A_STANDARD_MOD		0x08, 0x05
+#define TM6010_REQ08_R06_A_SOUND_MOD		0x08, 0x06
+#define TM6010_REQ08_R07_A_LEFT_VOL		0x08, 0x07
+#define TM6010_REQ08_R08_A_RIGHT_VOL		0x08, 0x08
+#define TM6010_REQ08_R09_A_MAIN_VOL		0x08, 0x09
+#define TM6010_REQ08_R0A_A_I2S_MOD		0x08, 0x0a
+#define TM6010_REQ08_R0B_A_ASD_THRES1		0x08, 0x0b
+#define TM6010_REQ08_R0C_A_ASD_THRES2		0x08, 0x0c
+#define TM6010_REQ08_R0D_A_AMD_THRES		0x08, 0x0d
+#define TM6010_REQ08_R0E_A_MONO_THRES1		0x08, 0x0e
+#define TM6010_REQ08_R0F_A_MONO_THRES2		0x08, 0x0f
+#define TM6010_REQ08_R10_A_MUTE_THRES1		0x08, 0x10
+#define TM6010_REQ08_R11_A_MUTE_THRES2		0x08, 0x11
+#define TM6010_REQ08_R12_A_AGC_U		0x08, 0x12
+#define TM6010_REQ08_R13_A_AGC_ERR_T		0x08, 0x13
+#define TM6010_REQ08_R14_A_AGC_GAIN_INIT	0x08, 0x14
+#define TM6010_REQ08_R15_A_AGC_STEP_THR		0x08, 0x15
+#define TM6010_REQ08_R16_A_AGC_GAIN_MAX		0x08, 0x16
+#define TM6010_REQ08_R17_A_AGC_GAIN_MIN		0x08, 0x17
+#define TM6010_REQ08_R18_A_TR_CTRL		0x08, 0x18
+#define TM6010_REQ08_R19_A_FH_2FH_GAIN		0x08, 0x19
+#define TM6010_REQ08_R1A_A_NICAM_SER_MAX	0x08, 0x1a
+#define TM6010_REQ08_R1B_A_NICAM_SER_MIN	0x08, 0x1b
+#define TM6010_REQ08_R1E_A_GAIN_DEEMPH_OUT	0x08, 0x1e
+#define TM6010_REQ08_R1F_A_TEST_INTF_SEL	0x08, 0x1f
+#define TM6010_REQ08_R20_A_TEST_PIN_SEL		0x08, 0x20
+#define TM6010_REQ08_R21_A_AGC_ERR		0x08, 0x21
+#define TM6010_REQ08_R22_A_AGC_GAIN		0x08, 0x22
+#define TM6010_REQ08_R23_A_NICAM_INFO		0x08, 0x23
+#define TM6010_REQ08_R24_A_SER			0x08, 0x24
+#define TM6010_REQ08_R25_A_C1_AMP		0x08, 0x25
+#define TM6010_REQ08_R26_A_C2_AMP		0x08, 0x26
+#define TM6010_REQ08_R27_A_NOISE_AMP		0x08, 0x27
+#define TM6010_REQ08_R28_A_AUDIO_MODE_RES	0x08, 0x28
+
+/* Define TM6010 Video ADC registers */
+#define TM6010_REQ08_RE0_ADC_REF		0x08, 0xe0
+#define TM6010_REQ08_RE1_DAC_CLMP		0x08, 0xe1
+#define TM6010_REQ08_RE2_POWER_DOWN_CTRL1	0x08, 0xe2
+#define TM6010_REQ08_RE3_ADC_IN1_SEL		0x08, 0xe3
+#define TM6010_REQ08_RE4_ADC_IN2_SEL		0x08, 0xe4
+#define TM6010_REQ08_RE5_GAIN_PARAM		0x08, 0xe5
+#define TM6010_REQ08_RE6_POWER_DOWN_CTRL2	0x08, 0xe6
+#define TM6010_REQ08_RE7_REG_GAIN_Y		0x08, 0xe7
+#define TM6010_REQ08_RE8_REG_GAIN_C		0x08, 0xe8
+#define TM6010_REQ08_RE9_BIAS_CTRL		0x08, 0xe9
+#define TM6010_REQ08_REA_BUFF_DRV_CTRL		0x08, 0xea
+#define TM6010_REQ08_REB_SIF_GAIN_CTRL		0x08, 0xeb
+#define TM6010_REQ08_REC_REVERSE_YC_CTRL	0x08, 0xec
+#define TM6010_REQ08_RED_GAIN_SEL		0x08, 0xed
+
+/* Define TM6010 Audio ADC registers */
+#define TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG	0x08, 0xf0
+#define TM6010_REQ08_RF1_AADC_POWER_DOWN	0x08, 0xf1
+#define TM6010_REQ08_RF2_LEFT_CHANNEL_VOL	0x08, 0xf2
+#define TM6010_REQ08_RF3_RIGHT_CHANNEL_VOL	0x08, 0xf3
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-stds.c b/drivers/staging/media/deprecated/tm6000/tm6000-stds.c
new file mode 100644
index 000000000..858cb4f3a
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-stds.c
@@ -0,0 +1,623 @@
+// SPDX-License-Identifier: GPL-2.0
+// tm6000-stds.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+//
+// Copyright (c) 2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include "tm6000.h"
+#include "tm6000-regs.h"
+
+static unsigned int tm6010_a_mode;
+module_param(tm6010_a_mode, int, 0644);
+MODULE_PARM_DESC(tm6010_a_mode, "set tm6010 sif audio mode");
+
+struct tm6000_reg_settings {
+	unsigned char req;
+	unsigned char reg;
+	unsigned char value;
+};
+
+
+struct tm6000_std_settings {
+	v4l2_std_id id;
+	struct tm6000_reg_settings *common;
+};
+
+static struct tm6000_reg_settings composite_pal_m[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x04 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x00 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x83 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x0a },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe0 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x20 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings composite_pal_nc[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x36 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x02 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x91 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x1f },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0x0c },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x8c },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x2c },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings composite_pal[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x32 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x02 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x25 },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0xd5 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x63 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0x50 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x8c },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x2c },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings composite_secam[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x38 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x02 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x24 },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x92 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xe8 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xed },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x8c },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x2c },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x2c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x18 },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0xff },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings composite_ntsc[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x00 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0f },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x00 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x8b },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xa2 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe9 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x1c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdd },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_std_settings composite_stds[] = {
+	{ .id = V4L2_STD_PAL_M, .common = composite_pal_m, },
+	{ .id = V4L2_STD_PAL_Nc, .common = composite_pal_nc, },
+	{ .id = V4L2_STD_PAL, .common = composite_pal, },
+	{ .id = V4L2_STD_SECAM, .common = composite_secam, },
+	{ .id = V4L2_STD_NTSC, .common = composite_ntsc, },
+};
+
+static struct tm6000_reg_settings svideo_pal_m[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x05 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x04 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x83 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x0a },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe0 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings svideo_pal_nc[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x37 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x04 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x91 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x1f },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0x0c },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings svideo_pal[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x33 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x04 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x30 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x25 },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0xd5 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0x63 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0x50 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x8c },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x2a },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x0c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x52 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdc },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings svideo_secam[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x39 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0e },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x03 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x31 },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x24 },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x92 },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xe8 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xed },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x8c },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x2a },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0xc1 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x2c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x18 },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0xff },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_reg_settings svideo_ntsc[] = {
+	{ TM6010_REQ07_R3F_RESET, 0x01 },
+	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x01 },
+	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x0f },
+	{ TM6010_REQ07_R02_VIDEO_CONTROL2, 0x5f },
+	{ TM6010_REQ07_R03_YC_SEP_CONTROL, 0x03 },
+	{ TM6010_REQ07_R07_OUTPUT_CONTROL, 0x30 },
+	{ TM6010_REQ07_R17_HLOOP_MAXSTATE, 0x8b },
+	{ TM6010_REQ07_R18_CHROMA_DTO_INCREMENT3, 0x1e },
+	{ TM6010_REQ07_R19_CHROMA_DTO_INCREMENT2, 0x8b },
+	{ TM6010_REQ07_R1A_CHROMA_DTO_INCREMENT1, 0xa2 },
+	{ TM6010_REQ07_R1B_CHROMA_DTO_INCREMENT0, 0xe9 },
+	{ TM6010_REQ07_R1C_HSYNC_DTO_INCREMENT3, 0x1c },
+	{ TM6010_REQ07_R1D_HSYNC_DTO_INCREMENT2, 0xcc },
+	{ TM6010_REQ07_R1E_HSYNC_DTO_INCREMENT1, 0xcc },
+	{ TM6010_REQ07_R1F_HSYNC_DTO_INCREMENT0, 0xcd },
+	{ TM6010_REQ07_R2E_ACTIVE_VIDEO_HSTART, 0x88 },
+	{ TM6010_REQ07_R30_ACTIVE_VIDEO_VSTART, 0x22 },
+	{ TM6010_REQ07_R31_ACTIVE_VIDEO_VHIGHT, 0x61 },
+	{ TM6010_REQ07_R33_VSYNC_HLOCK_MAX, 0x1c },
+	{ TM6010_REQ07_R35_VSYNC_AGC_MAX, 0x1c },
+	{ TM6010_REQ07_R82_COMB_FILTER_CONFIG, 0x42 },
+	{ TM6010_REQ07_R83_CHROMA_LOCK_CONFIG, 0x6f },
+	{ TM6010_REQ07_R04_LUMA_HAGC_CONTROL, 0xdd },
+	{ TM6010_REQ07_R0D_CHROMA_KILL_LEVEL, 0x07 },
+	{ TM6010_REQ07_R3F_RESET, 0x00 },
+	{ 0, 0, 0 }
+};
+
+static struct tm6000_std_settings svideo_stds[] = {
+	{ .id = V4L2_STD_PAL_M, .common = svideo_pal_m, },
+	{ .id = V4L2_STD_PAL_Nc, .common = svideo_pal_nc, },
+	{ .id = V4L2_STD_PAL, .common = svideo_pal, },
+	{ .id = V4L2_STD_SECAM, .common = svideo_secam, },
+	{ .id = V4L2_STD_NTSC, .common = svideo_ntsc, },
+};
+
+static int tm6000_set_audio_std(struct tm6000_core *dev)
+{
+	uint8_t areg_02 = 0x04; /* GC1 Fixed gain 0dB */
+	uint8_t areg_05 = 0x01; /* Auto 4.5 = M Japan, Auto 6.5 = DK */
+	uint8_t areg_06 = 0x02; /* Auto de-emphasis, manual channel mode */
+
+	if (dev->radio) {
+		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ08_R02_A_FIX_GAIN_CTRL, 0x04);
+		tm6000_set_reg(dev, TM6010_REQ08_R03_A_AUTO_GAIN_CTRL, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ08_R04_A_SIF_AMP_CTRL, 0x80);
+		tm6000_set_reg(dev, TM6010_REQ08_R05_A_STANDARD_MOD, 0x0c);
+		/* set mono or stereo */
+		if (dev->amode == V4L2_TUNER_MODE_MONO)
+			tm6000_set_reg(dev, TM6010_REQ08_R06_A_SOUND_MOD, 0x00);
+		else if (dev->amode == V4L2_TUNER_MODE_STEREO)
+			tm6000_set_reg(dev, TM6010_REQ08_R06_A_SOUND_MOD, 0x02);
+		tm6000_set_reg(dev, TM6010_REQ08_R09_A_MAIN_VOL, 0x18);
+		tm6000_set_reg(dev, TM6010_REQ08_R0C_A_ASD_THRES2, 0x0a);
+		tm6000_set_reg(dev, TM6010_REQ08_R0D_A_AMD_THRES, 0x40);
+		tm6000_set_reg(dev, TM6010_REQ08_RF1_AADC_POWER_DOWN, 0xfe);
+		tm6000_set_reg(dev, TM6010_REQ08_R1E_A_GAIN_DEEMPH_OUT, 0x13);
+		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x80);
+		tm6000_set_reg(dev, TM6010_REQ07_RFE_POWER_DOWN, 0xff);
+		return 0;
+	}
+
+	/*
+	 * STD/MN shouldn't be affected by tm6010_a_mode, as there's just one
+	 * audio standard for each V4L2_STD type.
+	 */
+	if ((dev->norm & V4L2_STD_NTSC) == V4L2_STD_NTSC_M_KR) {
+		areg_05 |= 0x04;
+	} else if ((dev->norm & V4L2_STD_NTSC) == V4L2_STD_NTSC_M_JP) {
+		areg_05 |= 0x43;
+	} else if (dev->norm & V4L2_STD_MN) {
+		areg_05 |= 0x22;
+	} else switch (tm6010_a_mode) {
+	/* auto */
+	case 0:
+		if ((dev->norm & V4L2_STD_SECAM) == V4L2_STD_SECAM_L)
+			areg_05 |= 0x00;
+		else	/* Other PAL/SECAM standards */
+			areg_05 |= 0x10;
+		break;
+	/* A2 */
+	case 1:
+		if (dev->norm & V4L2_STD_DK)
+			areg_05 = 0x09;
+		else
+			areg_05 = 0x05;
+		break;
+	/* NICAM */
+	case 2:
+		if (dev->norm & V4L2_STD_DK) {
+			areg_05 = 0x06;
+		} else if (dev->norm & V4L2_STD_PAL_I) {
+			areg_05 = 0x08;
+		} else if (dev->norm & V4L2_STD_SECAM_L) {
+			areg_05 = 0x0a;
+			areg_02 = 0x02;
+		} else {
+			areg_05 = 0x07;
+		}
+		break;
+	/* other */
+	case 3:
+		if (dev->norm & V4L2_STD_DK) {
+			areg_05 = 0x0b;
+		} else {
+			areg_05 = 0x02;
+		}
+		break;
+	}
+
+	tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R02_A_FIX_GAIN_CTRL, areg_02);
+	tm6000_set_reg(dev, TM6010_REQ08_R03_A_AUTO_GAIN_CTRL, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R04_A_SIF_AMP_CTRL, 0xa0);
+	tm6000_set_reg(dev, TM6010_REQ08_R05_A_STANDARD_MOD, areg_05);
+	tm6000_set_reg(dev, TM6010_REQ08_R06_A_SOUND_MOD, areg_06);
+	tm6000_set_reg(dev, TM6010_REQ08_R07_A_LEFT_VOL, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R08_A_RIGHT_VOL, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R09_A_MAIN_VOL, 0x08);
+	tm6000_set_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, 0x91);
+	tm6000_set_reg(dev, TM6010_REQ08_R0B_A_ASD_THRES1, 0x20);
+	tm6000_set_reg(dev, TM6010_REQ08_R0C_A_ASD_THRES2, 0x12);
+	tm6000_set_reg(dev, TM6010_REQ08_R0D_A_AMD_THRES, 0x20);
+	tm6000_set_reg(dev, TM6010_REQ08_R0E_A_MONO_THRES1, 0xf0);
+	tm6000_set_reg(dev, TM6010_REQ08_R0F_A_MONO_THRES2, 0x80);
+	tm6000_set_reg(dev, TM6010_REQ08_R10_A_MUTE_THRES1, 0xc0);
+	tm6000_set_reg(dev, TM6010_REQ08_R11_A_MUTE_THRES2, 0x80);
+	tm6000_set_reg(dev, TM6010_REQ08_R12_A_AGC_U, 0x12);
+	tm6000_set_reg(dev, TM6010_REQ08_R13_A_AGC_ERR_T, 0xfe);
+	tm6000_set_reg(dev, TM6010_REQ08_R14_A_AGC_GAIN_INIT, 0x20);
+	tm6000_set_reg(dev, TM6010_REQ08_R15_A_AGC_STEP_THR, 0x14);
+	tm6000_set_reg(dev, TM6010_REQ08_R16_A_AGC_GAIN_MAX, 0xfe);
+	tm6000_set_reg(dev, TM6010_REQ08_R17_A_AGC_GAIN_MIN, 0x01);
+	tm6000_set_reg(dev, TM6010_REQ08_R18_A_TR_CTRL, 0xa0);
+	tm6000_set_reg(dev, TM6010_REQ08_R19_A_FH_2FH_GAIN, 0x32);
+	tm6000_set_reg(dev, TM6010_REQ08_R1A_A_NICAM_SER_MAX, 0x64);
+	tm6000_set_reg(dev, TM6010_REQ08_R1B_A_NICAM_SER_MIN, 0x20);
+	tm6000_set_reg(dev, REQ_08_SET_GET_AVREG_BIT, 0x1c, 0x00);
+	tm6000_set_reg(dev, REQ_08_SET_GET_AVREG_BIT, 0x1d, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R1E_A_GAIN_DEEMPH_OUT, 0x13);
+	tm6000_set_reg(dev, TM6010_REQ08_R1F_A_TEST_INTF_SEL, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R20_A_TEST_PIN_SEL, 0x00);
+	tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x80);
+
+	return 0;
+}
+
+void tm6000_get_std_res(struct tm6000_core *dev)
+{
+	/* Currently, those are the only supported resoltions */
+	if (dev->norm & V4L2_STD_525_60)
+		dev->height = 480;
+	else
+		dev->height = 576;
+
+	dev->width = 720;
+}
+
+static int tm6000_load_std(struct tm6000_core *dev, struct tm6000_reg_settings *set)
+{
+	int i, rc;
+
+	/* Load board's initialization table */
+	for (i = 0; set[i].req; i++) {
+		rc = tm6000_set_reg(dev, set[i].req, set[i].reg, set[i].value);
+		if (rc < 0) {
+			printk(KERN_ERR "Error %i while setting req %d, reg %d to value %d\n",
+			       rc, set[i].req, set[i].reg, set[i].value);
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+int tm6000_set_standard(struct tm6000_core *dev)
+{
+	struct tm6000_input *input;
+	int i, rc = 0;
+	u8 reg_07_fe = 0x8a;
+	u8 reg_08_f1 = 0xfc;
+	u8 reg_08_e2 = 0xf0;
+	u8 reg_08_e6 = 0x0f;
+
+	tm6000_get_std_res(dev);
+
+	if (!dev->radio)
+		input = &dev->vinput[dev->input];
+	else
+		input = &dev->rinput;
+
+	if (dev->dev_type == TM6010) {
+		switch (input->vmux) {
+		case TM6000_VMUX_VIDEO_A:
+			tm6000_set_reg(dev, TM6010_REQ08_RE3_ADC_IN1_SEL, 0xf4);
+			tm6000_set_reg(dev, TM6010_REQ08_REA_BUFF_DRV_CTRL, 0xf1);
+			tm6000_set_reg(dev, TM6010_REQ08_REB_SIF_GAIN_CTRL, 0xe0);
+			tm6000_set_reg(dev, TM6010_REQ08_REC_REVERSE_YC_CTRL, 0xc2);
+			tm6000_set_reg(dev, TM6010_REQ08_RED_GAIN_SEL, 0xe8);
+			reg_07_fe |= 0x01;
+			break;
+		case TM6000_VMUX_VIDEO_B:
+			tm6000_set_reg(dev, TM6010_REQ08_RE3_ADC_IN1_SEL, 0xf8);
+			tm6000_set_reg(dev, TM6010_REQ08_REA_BUFF_DRV_CTRL, 0xf1);
+			tm6000_set_reg(dev, TM6010_REQ08_REB_SIF_GAIN_CTRL, 0xe0);
+			tm6000_set_reg(dev, TM6010_REQ08_REC_REVERSE_YC_CTRL, 0xc2);
+			tm6000_set_reg(dev, TM6010_REQ08_RED_GAIN_SEL, 0xe8);
+			reg_07_fe |= 0x01;
+			break;
+		case TM6000_VMUX_VIDEO_AB:
+			tm6000_set_reg(dev, TM6010_REQ08_RE3_ADC_IN1_SEL, 0xfc);
+			tm6000_set_reg(dev, TM6010_REQ08_RE4_ADC_IN2_SEL, 0xf8);
+			reg_08_e6 = 0x00;
+			tm6000_set_reg(dev, TM6010_REQ08_REA_BUFF_DRV_CTRL, 0xf2);
+			tm6000_set_reg(dev, TM6010_REQ08_REB_SIF_GAIN_CTRL, 0xf0);
+			tm6000_set_reg(dev, TM6010_REQ08_REC_REVERSE_YC_CTRL, 0xc2);
+			tm6000_set_reg(dev, TM6010_REQ08_RED_GAIN_SEL, 0xe0);
+			break;
+		default:
+			break;
+		}
+		switch (input->amux) {
+		case TM6000_AMUX_ADC1:
+			tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+				0x00, 0x0f);
+			/* Mux overflow workaround */
+			tm6000_set_reg_mask(dev, TM6010_REQ07_R07_OUTPUT_CONTROL,
+				0x10, 0xf0);
+			break;
+		case TM6000_AMUX_ADC2:
+			tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+				0x08, 0x0f);
+			/* Mux overflow workaround */
+			tm6000_set_reg_mask(dev, TM6010_REQ07_R07_OUTPUT_CONTROL,
+				0x10, 0xf0);
+			break;
+		case TM6000_AMUX_SIF1:
+			reg_08_e2 |= 0x02;
+			reg_08_e6 = 0x08;
+			reg_07_fe |= 0x40;
+			reg_08_f1 |= 0x02;
+			tm6000_set_reg(dev, TM6010_REQ08_RE4_ADC_IN2_SEL, 0xf3);
+			tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+				0x02, 0x0f);
+			/* Mux overflow workaround */
+			tm6000_set_reg_mask(dev, TM6010_REQ07_R07_OUTPUT_CONTROL,
+				0x30, 0xf0);
+			break;
+		case TM6000_AMUX_SIF2:
+			reg_08_e2 |= 0x02;
+			reg_08_e6 = 0x08;
+			reg_07_fe |= 0x40;
+			reg_08_f1 |= 0x02;
+			tm6000_set_reg(dev, TM6010_REQ08_RE4_ADC_IN2_SEL, 0xf7);
+			tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+				0x02, 0x0f);
+			/* Mux overflow workaround */
+			tm6000_set_reg_mask(dev, TM6010_REQ07_R07_OUTPUT_CONTROL,
+				0x30, 0xf0);
+			break;
+		default:
+			break;
+		}
+		tm6000_set_reg(dev, TM6010_REQ08_RE2_POWER_DOWN_CTRL1, reg_08_e2);
+		tm6000_set_reg(dev, TM6010_REQ08_RE6_POWER_DOWN_CTRL2, reg_08_e6);
+		tm6000_set_reg(dev, TM6010_REQ08_RF1_AADC_POWER_DOWN, reg_08_f1);
+		tm6000_set_reg(dev, TM6010_REQ07_RFE_POWER_DOWN, reg_07_fe);
+	} else {
+		switch (input->vmux) {
+		case TM6000_VMUX_VIDEO_A:
+			tm6000_set_reg(dev, TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x10);
+			tm6000_set_reg(dev, TM6000_REQ07_RE5_VADC_INP_LPF_SEL2, 0x00);
+			tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0x0f);
+			tm6000_set_reg(dev,
+			    REQ_03_SET_GET_MCU_PIN, input->v_gpio, 0);
+			break;
+		case TM6000_VMUX_VIDEO_B:
+			tm6000_set_reg(dev, TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x00);
+			tm6000_set_reg(dev, TM6000_REQ07_RE5_VADC_INP_LPF_SEL2, 0x00);
+			tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0x0f);
+			tm6000_set_reg(dev,
+			    REQ_03_SET_GET_MCU_PIN, input->v_gpio, 0);
+			break;
+		case TM6000_VMUX_VIDEO_AB:
+			tm6000_set_reg(dev, TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x10);
+			tm6000_set_reg(dev, TM6000_REQ07_RE5_VADC_INP_LPF_SEL2, 0x10);
+			tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0x00);
+			tm6000_set_reg(dev,
+			    REQ_03_SET_GET_MCU_PIN, input->v_gpio, 1);
+			break;
+		default:
+			break;
+		}
+		switch (input->amux) {
+		case TM6000_AMUX_ADC1:
+			tm6000_set_reg_mask(dev,
+				TM6000_REQ07_REB_VADC_AADC_MODE, 0x00, 0x0f);
+			break;
+		case TM6000_AMUX_ADC2:
+			tm6000_set_reg_mask(dev,
+				TM6000_REQ07_REB_VADC_AADC_MODE, 0x04, 0x0f);
+			break;
+		default:
+			break;
+		}
+	}
+	if (input->type == TM6000_INPUT_SVIDEO) {
+		for (i = 0; i < ARRAY_SIZE(svideo_stds); i++) {
+			if (dev->norm & svideo_stds[i].id) {
+				rc = tm6000_load_std(dev, svideo_stds[i].common);
+				goto ret;
+			}
+		}
+		return -EINVAL;
+	} else {
+		for (i = 0; i < ARRAY_SIZE(composite_stds); i++) {
+			if (dev->norm & composite_stds[i].id) {
+				rc = tm6000_load_std(dev, composite_stds[i].common);
+				goto ret;
+			}
+		}
+		return -EINVAL;
+	}
+
+ret:
+	if (rc < 0)
+		return rc;
+
+	if ((dev->dev_type == TM6010) &&
+	    ((input->amux == TM6000_AMUX_SIF1) ||
+	    (input->amux == TM6000_AMUX_SIF2)))
+		tm6000_set_audio_std(dev);
+
+	msleep(40);
+
+	return 0;
+}
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-usb-isoc.h b/drivers/staging/media/deprecated/tm6000/tm6000-usb-isoc.h
new file mode 100644
index 000000000..e3c6933f8
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-usb-isoc.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * tm6000-buf.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+ *
+ * Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+#include <linux/videodev2.h>
+
+#define TM6000_URB_MSG_LEN 180
+
+struct usb_isoc_ctl {
+		/* max packet size of isoc transaction */
+	int				max_pkt_size;
+
+		/* number of allocated urbs */
+	int				num_bufs;
+
+		/* urb for isoc transfers */
+	struct urb			**urb;
+
+		/* transfer buffers for isoc transfer */
+	char				**transfer_buffer;
+
+		/* Last buffer command and region */
+	u8				cmd;
+	int				pos, size, pktsize;
+
+		/* Last field: ODD or EVEN? */
+	int				vfield, field;
+
+		/* Stores incomplete commands */
+	u32				tmp_buf;
+	int				tmp_buf_len;
+
+		/* Stores already requested buffers */
+	struct tm6000_buffer		*buf;
+};
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000-video.c b/drivers/staging/media/deprecated/tm6000/tm6000-video.c
new file mode 100644
index 000000000..e06ed21ed
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000-video.c
@@ -0,0 +1,1703 @@
+// SPDX-License-Identifier: GPL-2.0
+// tm6000-video.c - driver for TM5600/TM6000/TM6010 USB video capture devices
+//
+// Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+//
+// Copyright (c) 2007 Michel Ludwig <michel.ludwig@gmail.com>
+//	- Fixed module load/unload
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/random.h>
+#include <linux/usb.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/tuner.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/highmem.h>
+#include <linux/freezer.h>
+
+#include "tm6000-regs.h"
+#include "tm6000.h"
+
+#define BUFFER_TIMEOUT     msecs_to_jiffies(2000)  /* 2 seconds */
+
+/* Limits minimum and default number of buffers */
+#define TM6000_MIN_BUF 4
+#define TM6000_DEF_BUF 8
+
+#define TM6000_NUM_URB_BUF 8
+
+#define TM6000_MAX_ISO_PACKETS	46	/* Max number of ISO packets */
+
+/* Declare static vars that will be used as parameters */
+static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
+static int video_nr = -1;		/* /dev/videoN, -1 for autodetect */
+static int radio_nr = -1;		/* /dev/radioN, -1 for autodetect */
+static bool keep_urb;			/* keep urb buffers allocated */
+
+/* Debug level */
+int tm6000_debug;
+EXPORT_SYMBOL_GPL(tm6000_debug);
+
+static struct tm6000_fmt format[] = {
+	{
+		.fourcc   = V4L2_PIX_FMT_YUYV,
+		.depth    = 16,
+	}, {
+		.fourcc   = V4L2_PIX_FMT_UYVY,
+		.depth    = 16,
+	}, {
+		.fourcc   = V4L2_PIX_FMT_TM6000,
+		.depth    = 16,
+	}
+};
+
+/* ------------------------------------------------------------------
+ *	DMA and thread functions
+ * ------------------------------------------------------------------
+ */
+
+#define norm_maxw(a) 720
+#define norm_maxh(a) 576
+
+#define norm_minw(a) norm_maxw(a)
+#define norm_minh(a) norm_maxh(a)
+
+/*
+ * video-buf generic routine to get the next available buffer
+ */
+static inline void get_next_buf(struct tm6000_dmaqueue *dma_q,
+			       struct tm6000_buffer   **buf)
+{
+	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
+
+	if (list_empty(&dma_q->active)) {
+		dprintk(dev, V4L2_DEBUG_QUEUE, "No active queue to serve\n");
+		*buf = NULL;
+		return;
+	}
+
+	*buf = list_entry(dma_q->active.next,
+			struct tm6000_buffer, vb.queue);
+}
+
+/*
+ * Announces that a buffer were filled and request the next
+ */
+static inline void buffer_filled(struct tm6000_core *dev,
+				 struct tm6000_dmaqueue *dma_q,
+				 struct tm6000_buffer *buf)
+{
+	/* Advice that buffer was filled */
+	dprintk(dev, V4L2_DEBUG_ISOC, "[%p/%d] wakeup\n", buf, buf->vb.i);
+	buf->vb.state = VIDEOBUF_DONE;
+	buf->vb.field_count++;
+	buf->vb.ts = ktime_get_ns();
+
+	list_del(&buf->vb.queue);
+	wake_up(&buf->vb.done);
+}
+
+/*
+ * Identify the tm5600/6000 buffer header type and properly handles
+ */
+static int copy_streams(u8 *data, unsigned long len,
+			struct urb *urb)
+{
+	struct tm6000_dmaqueue  *dma_q = urb->context;
+	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
+	u8 *ptr = data, *endp = data+len;
+	unsigned long header = 0;
+	int rc = 0;
+	unsigned int cmd, cpysize, pktsize, size, field, block, line, pos = 0;
+	struct tm6000_buffer *vbuf = NULL;
+	char *voutp = NULL;
+	unsigned int linewidth;
+
+	if (!dev->radio) {
+		/* get video buffer */
+		get_next_buf(dma_q, &vbuf);
+
+		if (!vbuf)
+			return rc;
+		voutp = videobuf_to_vmalloc(&vbuf->vb);
+
+		if (!voutp)
+			return 0;
+	}
+
+	for (ptr = data; ptr < endp;) {
+		if (!dev->isoc_ctl.cmd) {
+			/* Header */
+			if (dev->isoc_ctl.tmp_buf_len > 0) {
+				/* from last urb or packet */
+				header = dev->isoc_ctl.tmp_buf;
+				if (4 - dev->isoc_ctl.tmp_buf_len > 0) {
+					memcpy((u8 *)&header +
+						dev->isoc_ctl.tmp_buf_len,
+						ptr,
+						4 - dev->isoc_ctl.tmp_buf_len);
+					ptr += 4 - dev->isoc_ctl.tmp_buf_len;
+				}
+				dev->isoc_ctl.tmp_buf_len = 0;
+			} else {
+				if (ptr + 3 >= endp) {
+					/* have incomplete header */
+					dev->isoc_ctl.tmp_buf_len = endp - ptr;
+					memcpy(&dev->isoc_ctl.tmp_buf, ptr,
+						dev->isoc_ctl.tmp_buf_len);
+					return rc;
+				}
+				/* Seek for sync */
+				for (; ptr < endp - 3; ptr++) {
+					if (*(ptr + 3) == 0x47)
+						break;
+				}
+				/* Get message header */
+				header = *(unsigned long *)ptr;
+				ptr += 4;
+			}
+
+			/* split the header fields */
+			size = ((header & 0x7e) << 1);
+			if (size > 0)
+				size -= 4;
+			block = (header >> 7) & 0xf;
+			field = (header >> 11) & 0x1;
+			line  = (header >> 12) & 0x1ff;
+			cmd   = (header >> 21) & 0x7;
+			/* Validates header fields */
+			if (size > TM6000_URB_MSG_LEN)
+				size = TM6000_URB_MSG_LEN;
+			pktsize = TM6000_URB_MSG_LEN;
+			/*
+			 * calculate position in buffer and change the buffer
+			 */
+			switch (cmd) {
+			case TM6000_URB_MSG_VIDEO:
+				if (!dev->radio) {
+					if ((dev->isoc_ctl.vfield != field) &&
+						(field == 1)) {
+						/*
+						 * Announces that a new buffer
+						 * were filled
+						 */
+						buffer_filled(dev, dma_q, vbuf);
+						dprintk(dev, V4L2_DEBUG_ISOC,
+							"new buffer filled\n");
+						get_next_buf(dma_q, &vbuf);
+						if (!vbuf)
+							return rc;
+						voutp = videobuf_to_vmalloc(&vbuf->vb);
+						if (!voutp)
+							return rc;
+						memset(voutp, 0, vbuf->vb.size);
+					}
+					linewidth = vbuf->vb.width << 1;
+					pos = ((line << 1) - field - 1) *
+					linewidth + block * TM6000_URB_MSG_LEN;
+					/* Don't allow to write out of the buffer */
+					if (pos + size > vbuf->vb.size)
+						cmd = TM6000_URB_MSG_ERR;
+					dev->isoc_ctl.vfield = field;
+				}
+				break;
+			case TM6000_URB_MSG_VBI:
+				break;
+			case TM6000_URB_MSG_AUDIO:
+			case TM6000_URB_MSG_PTS:
+				size = pktsize; /* Size is always 180 bytes */
+				break;
+			}
+		} else {
+			/* Continue the last copy */
+			cmd = dev->isoc_ctl.cmd;
+			size = dev->isoc_ctl.size;
+			pos = dev->isoc_ctl.pos;
+			pktsize = dev->isoc_ctl.pktsize;
+			field = dev->isoc_ctl.field;
+		}
+		cpysize = (endp - ptr > size) ? size : endp - ptr;
+		if (cpysize) {
+			/* copy data in different buffers */
+			switch (cmd) {
+			case TM6000_URB_MSG_VIDEO:
+				/* Fills video buffer */
+				if (vbuf)
+					memcpy(&voutp[pos], ptr, cpysize);
+				break;
+			case TM6000_URB_MSG_AUDIO: {
+				int i;
+				for (i = 0; i < cpysize; i += 2)
+					swab16s((u16 *)(ptr + i));
+
+				tm6000_call_fillbuf(dev, TM6000_AUDIO, ptr, cpysize);
+				break;
+			}
+			case TM6000_URB_MSG_VBI:
+				/* Need some code to copy vbi buffer */
+				break;
+			case TM6000_URB_MSG_PTS: {
+				/* Need some code to copy pts */
+				u32 pts;
+				pts = *(u32 *)ptr;
+				dprintk(dev, V4L2_DEBUG_ISOC, "field %d, PTS %x",
+					field, pts);
+				break;
+			}
+			}
+		}
+		if (ptr + pktsize > endp) {
+			/*
+			 * End of URB packet, but cmd processing is not
+			 * complete. Preserve the state for a next packet
+			 */
+			dev->isoc_ctl.pos = pos + cpysize;
+			dev->isoc_ctl.size = size - cpysize;
+			dev->isoc_ctl.cmd = cmd;
+			dev->isoc_ctl.field = field;
+			dev->isoc_ctl.pktsize = pktsize - (endp - ptr);
+			ptr += endp - ptr;
+		} else {
+			dev->isoc_ctl.cmd = 0;
+			ptr += pktsize;
+		}
+	}
+	return 0;
+}
+
+/*
+ * Identify the tm5600/6000 buffer header type and properly handles
+ */
+static int copy_multiplexed(u8 *ptr, unsigned long len,
+			struct urb *urb)
+{
+	struct tm6000_dmaqueue  *dma_q = urb->context;
+	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
+	unsigned int pos = dev->isoc_ctl.pos, cpysize;
+	int rc = 1;
+	struct tm6000_buffer *buf;
+	char *outp = NULL;
+
+	get_next_buf(dma_q, &buf);
+	if (buf)
+		outp = videobuf_to_vmalloc(&buf->vb);
+
+	if (!outp)
+		return 0;
+
+	while (len > 0) {
+		cpysize = min(len, buf->vb.size-pos);
+		memcpy(&outp[pos], ptr, cpysize);
+		pos += cpysize;
+		ptr += cpysize;
+		len -= cpysize;
+		if (pos >= buf->vb.size) {
+			pos = 0;
+			/* Announces that a new buffer were filled */
+			buffer_filled(dev, dma_q, buf);
+			dprintk(dev, V4L2_DEBUG_ISOC, "new buffer filled\n");
+			get_next_buf(dma_q, &buf);
+			if (!buf)
+				break;
+			outp = videobuf_to_vmalloc(&(buf->vb));
+			if (!outp)
+				return rc;
+			pos = 0;
+		}
+	}
+
+	dev->isoc_ctl.pos = pos;
+	return rc;
+}
+
+static inline void print_err_status(struct tm6000_core *dev,
+				     int packet, int status)
+{
+	char *errmsg = "Unknown";
+
+	switch (status) {
+	case -ENOENT:
+		errmsg = "unlinked synchronously";
+		break;
+	case -ECONNRESET:
+		errmsg = "unlinked asynchronously";
+		break;
+	case -ENOSR:
+		errmsg = "Buffer error (overrun)";
+		break;
+	case -EPIPE:
+		errmsg = "Stalled (device not responding)";
+		break;
+	case -EOVERFLOW:
+		errmsg = "Babble (bad cable?)";
+		break;
+	case -EPROTO:
+		errmsg = "Bit-stuff error (bad cable?)";
+		break;
+	case -EILSEQ:
+		errmsg = "CRC/Timeout (could be anything)";
+		break;
+	case -ETIME:
+		errmsg = "Device does not respond";
+		break;
+	}
+	if (packet < 0) {
+		dprintk(dev, V4L2_DEBUG_QUEUE, "URB status %d [%s].\n",
+			status, errmsg);
+	} else {
+		dprintk(dev, V4L2_DEBUG_QUEUE, "URB packet %d, status %d [%s].\n",
+			packet, status, errmsg);
+	}
+}
+
+
+/*
+ * Controls the isoc copy of each urb packet
+ */
+static inline int tm6000_isoc_copy(struct urb *urb)
+{
+	struct tm6000_dmaqueue  *dma_q = urb->context;
+	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
+	int i, len = 0, rc = 1, status;
+	char *p;
+
+	if (urb->status < 0) {
+		print_err_status(dev, -1, urb->status);
+		return 0;
+	}
+
+	for (i = 0; i < urb->number_of_packets; i++) {
+		status = urb->iso_frame_desc[i].status;
+
+		if (status < 0) {
+			print_err_status(dev, i, status);
+			continue;
+		}
+
+		len = urb->iso_frame_desc[i].actual_length;
+
+		if (len > 0) {
+			p = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+			if (!urb->iso_frame_desc[i].status) {
+				if ((dev->fourcc) == V4L2_PIX_FMT_TM6000) {
+					rc = copy_multiplexed(p, len, urb);
+					if (rc <= 0)
+						return rc;
+				} else {
+					copy_streams(p, len, urb);
+				}
+			}
+		}
+	}
+	return rc;
+}
+
+/* ------------------------------------------------------------------
+ *	URB control
+ * ------------------------------------------------------------------
+ */
+
+/*
+ * IRQ callback, called by URB callback
+ */
+static void tm6000_irq_callback(struct urb *urb)
+{
+	struct tm6000_dmaqueue  *dma_q = urb->context;
+	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
+	unsigned long flags;
+	int i;
+
+	switch (urb->status) {
+	case 0:
+	case -ETIMEDOUT:
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		return;
+
+	default:
+		tm6000_err("urb completion error %d.\n", urb->status);
+		break;
+	}
+
+	spin_lock_irqsave(&dev->slock, flags);
+	tm6000_isoc_copy(urb);
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	/* Reset urb buffers */
+	for (i = 0; i < urb->number_of_packets; i++) {
+		urb->iso_frame_desc[i].status = 0;
+		urb->iso_frame_desc[i].actual_length = 0;
+	}
+
+	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
+	if (urb->status)
+		tm6000_err("urb resubmit failed (error=%i)\n",
+			urb->status);
+}
+
+/*
+ * Allocate URB buffers
+ */
+static int tm6000_alloc_urb_buffers(struct tm6000_core *dev)
+{
+	int num_bufs = TM6000_NUM_URB_BUF;
+	int i;
+
+	if (dev->urb_buffer)
+		return 0;
+
+	dev->urb_buffer = kmalloc_array(num_bufs, sizeof(*dev->urb_buffer),
+					GFP_KERNEL);
+	if (!dev->urb_buffer)
+		return -ENOMEM;
+
+	dev->urb_dma = kmalloc_array(num_bufs, sizeof(*dev->urb_dma),
+				     GFP_KERNEL);
+	if (!dev->urb_dma)
+		return -ENOMEM;
+
+	for (i = 0; i < num_bufs; i++) {
+		dev->urb_buffer[i] = usb_alloc_coherent(
+					dev->udev, dev->urb_size,
+					GFP_KERNEL, &dev->urb_dma[i]);
+		if (!dev->urb_buffer[i]) {
+			tm6000_err("unable to allocate %i bytes for transfer buffer %i\n",
+				    dev->urb_size, i);
+			return -ENOMEM;
+		}
+		memset(dev->urb_buffer[i], 0, dev->urb_size);
+	}
+
+	return 0;
+}
+
+/*
+ * Free URB buffers
+ */
+static int tm6000_free_urb_buffers(struct tm6000_core *dev)
+{
+	int i;
+
+	if (!dev->urb_buffer)
+		return 0;
+
+	for (i = 0; i < TM6000_NUM_URB_BUF; i++) {
+		if (dev->urb_buffer[i]) {
+			usb_free_coherent(dev->udev,
+					dev->urb_size,
+					dev->urb_buffer[i],
+					dev->urb_dma[i]);
+			dev->urb_buffer[i] = NULL;
+		}
+	}
+	kfree(dev->urb_buffer);
+	kfree(dev->urb_dma);
+	dev->urb_buffer = NULL;
+	dev->urb_dma = NULL;
+
+	return 0;
+}
+
+/*
+ * Stop and Deallocate URBs
+ */
+static void tm6000_uninit_isoc(struct tm6000_core *dev)
+{
+	struct urb *urb;
+	int i;
+
+	dev->isoc_ctl.buf = NULL;
+	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
+		urb = dev->isoc_ctl.urb[i];
+		if (urb) {
+			usb_kill_urb(urb);
+			usb_unlink_urb(urb);
+			usb_free_urb(urb);
+			dev->isoc_ctl.urb[i] = NULL;
+		}
+		dev->isoc_ctl.transfer_buffer[i] = NULL;
+	}
+
+	if (!keep_urb)
+		tm6000_free_urb_buffers(dev);
+
+	kfree(dev->isoc_ctl.urb);
+	kfree(dev->isoc_ctl.transfer_buffer);
+
+	dev->isoc_ctl.urb = NULL;
+	dev->isoc_ctl.transfer_buffer = NULL;
+	dev->isoc_ctl.num_bufs = 0;
+}
+
+/*
+ * Assign URBs and start IRQ
+ */
+static int tm6000_prepare_isoc(struct tm6000_core *dev)
+{
+	struct tm6000_dmaqueue *dma_q = &dev->vidq;
+	int i, j, sb_size, pipe, size, max_packets;
+	int num_bufs = TM6000_NUM_URB_BUF;
+	struct urb *urb;
+
+	/* De-allocates all pending stuff */
+	tm6000_uninit_isoc(dev);
+	/* Stop interrupt USB pipe */
+	tm6000_ir_int_stop(dev);
+
+	usb_set_interface(dev->udev,
+			  dev->isoc_in.bInterfaceNumber,
+			  dev->isoc_in.bAlternateSetting);
+
+	/* Start interrupt USB pipe */
+	tm6000_ir_int_start(dev);
+
+	pipe = usb_rcvisocpipe(dev->udev,
+			       dev->isoc_in.endp->desc.bEndpointAddress &
+			       USB_ENDPOINT_NUMBER_MASK);
+
+	size = usb_maxpacket(dev->udev, pipe);
+
+	if (size > dev->isoc_in.maxsize)
+		size = dev->isoc_in.maxsize;
+
+	dev->isoc_ctl.max_pkt_size = size;
+
+	max_packets = TM6000_MAX_ISO_PACKETS;
+	sb_size = max_packets * size;
+	dev->urb_size = sb_size;
+
+	dev->isoc_ctl.num_bufs = num_bufs;
+
+	dev->isoc_ctl.urb = kmalloc_array(num_bufs, sizeof(void *),
+					  GFP_KERNEL);
+	if (!dev->isoc_ctl.urb)
+		return -ENOMEM;
+
+	dev->isoc_ctl.transfer_buffer = kmalloc_array(num_bufs,
+						      sizeof(void *),
+						      GFP_KERNEL);
+	if (!dev->isoc_ctl.transfer_buffer) {
+		kfree(dev->isoc_ctl.urb);
+		return -ENOMEM;
+	}
+
+	dprintk(dev, V4L2_DEBUG_QUEUE, "Allocating %d x %d packets (%d bytes) of %d bytes each to handle %u size\n",
+		    max_packets, num_bufs, sb_size,
+		    dev->isoc_in.maxsize, size);
+
+
+	if (tm6000_alloc_urb_buffers(dev) < 0) {
+		tm6000_err("cannot allocate memory for urb buffers\n");
+
+		/* call free, as some buffers might have been allocated */
+		tm6000_free_urb_buffers(dev);
+		kfree(dev->isoc_ctl.urb);
+		kfree(dev->isoc_ctl.transfer_buffer);
+		return -ENOMEM;
+	}
+
+	/* allocate urbs and transfer buffers */
+	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
+		urb = usb_alloc_urb(max_packets, GFP_KERNEL);
+		if (!urb) {
+			tm6000_uninit_isoc(dev);
+			tm6000_free_urb_buffers(dev);
+			return -ENOMEM;
+		}
+		dev->isoc_ctl.urb[i] = urb;
+
+		urb->transfer_dma = dev->urb_dma[i];
+		dev->isoc_ctl.transfer_buffer[i] = dev->urb_buffer[i];
+
+		usb_fill_bulk_urb(urb, dev->udev, pipe,
+				  dev->isoc_ctl.transfer_buffer[i], sb_size,
+				  tm6000_irq_callback, dma_q);
+		urb->interval = dev->isoc_in.endp->desc.bInterval;
+		urb->number_of_packets = max_packets;
+		urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
+
+		for (j = 0; j < max_packets; j++) {
+			urb->iso_frame_desc[j].offset = size * j;
+			urb->iso_frame_desc[j].length = size;
+		}
+	}
+
+	return 0;
+}
+
+static int tm6000_start_thread(struct tm6000_core *dev)
+{
+	struct tm6000_dmaqueue *dma_q = &dev->vidq;
+	int i;
+
+	dma_q->frame = 0;
+	dma_q->ini_jiffies = jiffies;
+
+	init_waitqueue_head(&dma_q->wq);
+
+	/* submit urbs and enables IRQ */
+	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
+		int rc = usb_submit_urb(dev->isoc_ctl.urb[i], GFP_ATOMIC);
+		if (rc) {
+			tm6000_err("submit of urb %i failed (error=%i)\n", i,
+				   rc);
+			tm6000_uninit_isoc(dev);
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+/* ------------------------------------------------------------------
+ *	Videobuf operations
+ * ------------------------------------------------------------------
+ */
+
+static int
+buffer_setup(struct videobuf_queue *vq, unsigned int *count, unsigned int *size)
+{
+	struct tm6000_fh *fh = vq->priv_data;
+
+	*size = fh->fmt->depth * fh->width * fh->height >> 3;
+	if (0 == *count)
+		*count = TM6000_DEF_BUF;
+
+	if (*count < TM6000_MIN_BUF)
+		*count = TM6000_MIN_BUF;
+
+	while (*size * *count > vid_limit * 1024 * 1024)
+		(*count)--;
+
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct tm6000_buffer *buf)
+{
+	struct tm6000_fh *fh = vq->priv_data;
+	struct tm6000_core   *dev = fh->dev;
+	unsigned long flags;
+
+	/* We used to wait for the buffer to finish here, but this didn't work
+	   because, as we were keeping the state as VIDEOBUF_QUEUED,
+	   videobuf_queue_cancel marked it as finished for us.
+	   (Also, it could wedge forever if the hardware was misconfigured.)
+
+	   This should be safe; by the time we get here, the buffer isn't
+	   queued anymore. If we ever start marking the buffers as
+	   VIDEOBUF_ACTIVE, it won't be, though.
+	*/
+	spin_lock_irqsave(&dev->slock, flags);
+	if (dev->isoc_ctl.buf == buf)
+		dev->isoc_ctl.buf = NULL;
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	videobuf_vmalloc_free(&buf->vb);
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int
+buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
+						enum v4l2_field field)
+{
+	struct tm6000_fh     *fh  = vq->priv_data;
+	struct tm6000_buffer *buf = container_of(vb, struct tm6000_buffer, vb);
+	struct tm6000_core   *dev = fh->dev;
+	int rc = 0;
+
+	BUG_ON(NULL == fh->fmt);
+
+
+	/* FIXME: It assumes depth=2 */
+	/* The only currently supported format is 16 bits/pixel */
+	buf->vb.size = fh->fmt->depth*fh->width*fh->height >> 3;
+	if (0 != buf->vb.baddr  &&  buf->vb.bsize < buf->vb.size)
+		return -EINVAL;
+
+	if (buf->fmt       != fh->fmt    ||
+	    buf->vb.width  != fh->width  ||
+	    buf->vb.height != fh->height ||
+	    buf->vb.field  != field) {
+		buf->fmt       = fh->fmt;
+		buf->vb.width  = fh->width;
+		buf->vb.height = fh->height;
+		buf->vb.field  = field;
+		buf->vb.state = VIDEOBUF_NEEDS_INIT;
+	}
+
+	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
+		rc = videobuf_iolock(vq, &buf->vb, NULL);
+		if (rc != 0)
+			goto fail;
+	}
+
+	if (!dev->isoc_ctl.num_bufs) {
+		rc = tm6000_prepare_isoc(dev);
+		if (rc < 0)
+			goto fail;
+
+		rc = tm6000_start_thread(dev);
+		if (rc < 0)
+			goto fail;
+
+	}
+
+	buf->vb.state = VIDEOBUF_PREPARED;
+	return 0;
+
+fail:
+	free_buffer(vq, buf);
+	return rc;
+}
+
+static void
+buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct tm6000_buffer    *buf     = container_of(vb, struct tm6000_buffer, vb);
+	struct tm6000_fh        *fh      = vq->priv_data;
+	struct tm6000_core      *dev     = fh->dev;
+	struct tm6000_dmaqueue  *vidq    = &dev->vidq;
+
+	buf->vb.state = VIDEOBUF_QUEUED;
+	list_add_tail(&buf->vb.queue, &vidq->active);
+}
+
+static void buffer_release(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct tm6000_buffer   *buf  = container_of(vb, struct tm6000_buffer, vb);
+
+	free_buffer(vq, buf);
+}
+
+static const struct videobuf_queue_ops tm6000_video_qops = {
+	.buf_setup      = buffer_setup,
+	.buf_prepare    = buffer_prepare,
+	.buf_queue      = buffer_queue,
+	.buf_release    = buffer_release,
+};
+
+/* ------------------------------------------------------------------
+ *	IOCTL handling
+ * ------------------------------------------------------------------
+ */
+
+static bool is_res_read(struct tm6000_core *dev, struct tm6000_fh *fh)
+{
+	/* Is the current fh handling it? if so, that's OK */
+	if (dev->resources == fh && dev->is_res_read)
+		return true;
+
+	return false;
+}
+
+static bool is_res_streaming(struct tm6000_core *dev, struct tm6000_fh *fh)
+{
+	/* Is the current fh handling it? if so, that's OK */
+	if (dev->resources == fh)
+		return true;
+
+	return false;
+}
+
+static bool res_get(struct tm6000_core *dev, struct tm6000_fh *fh,
+		   bool is_res_read)
+{
+	/* Is the current fh handling it? if so, that's OK */
+	if (dev->resources == fh && dev->is_res_read == is_res_read)
+		return true;
+
+	/* is it free? */
+	if (dev->resources)
+		return false;
+
+	/* grab it */
+	dev->resources = fh;
+	dev->is_res_read = is_res_read;
+	dprintk(dev, V4L2_DEBUG_RES_LOCK, "res: get\n");
+	return true;
+}
+
+static void res_free(struct tm6000_core *dev, struct tm6000_fh *fh)
+{
+	/* Is the current fh handling it? if so, that's OK */
+	if (dev->resources != fh)
+		return;
+
+	dev->resources = NULL;
+	dprintk(dev, V4L2_DEBUG_RES_LOCK, "res: put\n");
+}
+
+/* ------------------------------------------------------------------
+ *	IOCTL vidioc handling
+ * ------------------------------------------------------------------
+ */
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct tm6000_core *dev = ((struct tm6000_fh *)priv)->dev;
+
+	strscpy(cap->driver, "tm6000", sizeof(cap->driver));
+	strscpy(cap->card, "Trident TM5600/6000/6010", sizeof(cap->card));
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+			    V4L2_CAP_DEVICE_CAPS;
+	if (dev->tuner_type != TUNER_ABSENT)
+		cap->capabilities |= V4L2_CAP_TUNER;
+	if (dev->caps.has_radio)
+		cap->capabilities |= V4L2_CAP_RADIO;
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	if (f->index >= ARRAY_SIZE(format))
+		return -EINVAL;
+
+	f->pixelformat = format[f->index].fourcc;
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct tm6000_fh  *fh = priv;
+
+	f->fmt.pix.width        = fh->width;
+	f->fmt.pix.height       = fh->height;
+	f->fmt.pix.field        = fh->vb_vidq.field;
+	f->fmt.pix.pixelformat  = fh->fmt->fourcc;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.bytesperline =
+		(f->fmt.pix.width * fh->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage =
+		f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+	return 0;
+}
+
+static struct tm6000_fmt *format_by_fourcc(unsigned int fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(format); i++)
+		if (format[i].fourcc == fourcc)
+			return format+i;
+	return NULL;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct tm6000_core *dev = ((struct tm6000_fh *)priv)->dev;
+	struct tm6000_fmt *fmt;
+	enum v4l2_field field;
+
+	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt) {
+		dprintk(dev, 2, "Fourcc format (0x%08x) invalid.\n",
+			f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	field = V4L2_FIELD_INTERLACED;
+
+	tm6000_get_std_res(dev);
+
+	f->fmt.pix.width  = dev->width;
+	f->fmt.pix.height = dev->height;
+
+	f->fmt.pix.width &= ~0x01;
+
+	f->fmt.pix.field = field;
+
+	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.colorspace   = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+/*FIXME: This seems to be generic enough to be at videodev2 */
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct tm6000_fh  *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+	int ret = vidioc_try_fmt_vid_cap(file, fh, f);
+	if (ret < 0)
+		return ret;
+
+	fh->fmt           = format_by_fourcc(f->fmt.pix.pixelformat);
+	fh->width         = f->fmt.pix.width;
+	fh->height        = f->fmt.pix.height;
+	fh->vb_vidq.field = f->fmt.pix.field;
+	fh->type          = f->type;
+
+	dev->fourcc       = f->fmt.pix.pixelformat;
+
+	tm6000_set_fourcc_format(dev);
+
+	return 0;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+			   struct v4l2_requestbuffers *p)
+{
+	struct tm6000_fh  *fh = priv;
+
+	return videobuf_reqbufs(&fh->vb_vidq, p);
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+			    struct v4l2_buffer *p)
+{
+	struct tm6000_fh  *fh = priv;
+
+	return videobuf_querybuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct tm6000_fh  *fh = priv;
+
+	return videobuf_qbuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct tm6000_fh  *fh = priv;
+
+	return videobuf_dqbuf(&fh->vb_vidq, p,
+				file->f_flags & O_NONBLOCK);
+}
+
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct tm6000_fh *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (i != fh->type)
+		return -EINVAL;
+
+	if (!res_get(dev, fh, false))
+		return -EBUSY;
+	return videobuf_streamon(&fh->vb_vidq);
+}
+
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct tm6000_fh *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (i != fh->type)
+		return -EINVAL;
+
+	videobuf_streamoff(&fh->vb_vidq);
+	res_free(dev, fh);
+
+	return 0;
+}
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
+{
+	int rc = 0;
+	struct tm6000_fh *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	dev->norm = norm;
+	rc = tm6000_init_analog_mode(dev);
+
+	fh->width  = dev->width;
+	fh->height = dev->height;
+
+	if (rc < 0)
+		return rc;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, dev->norm);
+
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
+{
+	struct tm6000_fh *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	*norm = dev->norm;
+	return 0;
+}
+
+static const char *iname[] = {
+	[TM6000_INPUT_TV] = "Television",
+	[TM6000_INPUT_COMPOSITE1] = "Composite 1",
+	[TM6000_INPUT_COMPOSITE2] = "Composite 2",
+	[TM6000_INPUT_SVIDEO] = "S-Video",
+};
+
+static int vidioc_enum_input(struct file *file, void *priv,
+				struct v4l2_input *i)
+{
+	struct tm6000_fh   *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+	unsigned int n;
+
+	n = i->index;
+	if (n >= 3)
+		return -EINVAL;
+
+	if (!dev->vinput[n].type)
+		return -EINVAL;
+
+	i->index = n;
+
+	if (dev->vinput[n].type == TM6000_INPUT_TV)
+		i->type = V4L2_INPUT_TYPE_TUNER;
+	else
+		i->type = V4L2_INPUT_TYPE_CAMERA;
+
+	strscpy(i->name, iname[dev->vinput[n].type], sizeof(i->name));
+
+	i->std = TM6000_STD;
+
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct tm6000_fh   *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	*i = dev->input;
+
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct tm6000_fh   *fh = priv;
+	struct tm6000_core *dev = fh->dev;
+	int rc = 0;
+
+	if (i >= 3)
+		return -EINVAL;
+	if (!dev->vinput[i].type)
+		return -EINVAL;
+
+	dev->input = i;
+
+	rc = vidioc_s_std(file, priv, dev->norm);
+
+	return rc;
+}
+
+/* --- controls ---------------------------------------------- */
+
+static int tm6000_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tm6000_core *dev = container_of(ctrl->handler, struct tm6000_core, ctrl_handler);
+	u8  val = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		tm6000_set_reg(dev, TM6010_REQ07_R08_LUMA_CONTRAST_ADJ, val);
+		return 0;
+	case V4L2_CID_BRIGHTNESS:
+		tm6000_set_reg(dev, TM6010_REQ07_R09_LUMA_BRIGHTNESS_ADJ, val);
+		return 0;
+	case V4L2_CID_SATURATION:
+		tm6000_set_reg(dev, TM6010_REQ07_R0A_CHROMA_SATURATION_ADJ, val);
+		return 0;
+	case V4L2_CID_HUE:
+		tm6000_set_reg(dev, TM6010_REQ07_R0B_CHROMA_HUE_PHASE_ADJ, val);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops tm6000_ctrl_ops = {
+	.s_ctrl = tm6000_s_ctrl,
+};
+
+static int tm6000_radio_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tm6000_core *dev = container_of(ctrl->handler,
+			struct tm6000_core, radio_ctrl_handler);
+	u8  val = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		dev->ctl_mute = val;
+		tm6000_tvaudio_set_mute(dev, val);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		dev->ctl_volume = val;
+		tm6000_set_volume(dev, val);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops tm6000_radio_ctrl_ops = {
+	.s_ctrl = tm6000_radio_s_ctrl,
+};
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
+{
+	struct tm6000_fh   *fh  = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (UNSET == dev->tuner_type)
+		return -ENOTTY;
+	if (0 != t->index)
+		return -EINVAL;
+
+	strscpy(t->name, "Television", sizeof(t->name));
+	t->type       = V4L2_TUNER_ANALOG_TV;
+	t->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO;
+	t->rangehigh  = 0xffffffffUL;
+	t->rxsubchans = V4L2_TUNER_SUB_STEREO;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+
+	t->audmode = dev->amode;
+
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+				const struct v4l2_tuner *t)
+{
+	struct tm6000_fh   *fh  = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (UNSET == dev->tuner_type)
+		return -ENOTTY;
+	if (0 != t->index)
+		return -EINVAL;
+
+	if (t->audmode > V4L2_TUNER_MODE_STEREO)
+		dev->amode = V4L2_TUNER_MODE_STEREO;
+	else
+		dev->amode = t->audmode;
+	dprintk(dev, 3, "audio mode: %x\n", t->audmode);
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
+
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct tm6000_fh   *fh  = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (UNSET == dev->tuner_type)
+		return -ENOTTY;
+	if (f->tuner)
+		return -EINVAL;
+
+	f->frequency = dev->freq;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, f);
+
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				const struct v4l2_frequency *f)
+{
+	struct tm6000_fh   *fh  = priv;
+	struct tm6000_core *dev = fh->dev;
+
+	if (UNSET == dev->tuner_type)
+		return -ENOTTY;
+	if (f->tuner != 0)
+		return -EINVAL;
+
+	dev->freq = f->frequency;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, f);
+
+	return 0;
+}
+
+static int radio_g_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *t)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	if (0 != t->index)
+		return -EINVAL;
+
+	memset(t, 0, sizeof(*t));
+	strscpy(t->name, "Radio", sizeof(t->name));
+	t->type = V4L2_TUNER_RADIO;
+	t->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	t->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	t->audmode = V4L2_TUNER_MODE_STEREO;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+
+	return 0;
+}
+
+static int radio_s_tuner(struct file *file, void *priv,
+					const struct v4l2_tuner *t)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	if (0 != t->index)
+		return -EINVAL;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
+	return 0;
+}
+
+/* ------------------------------------------------------------------
+	File operations for the device
+   ------------------------------------------------------------------*/
+
+static int __tm6000_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct tm6000_core *dev = video_drvdata(file);
+	struct tm6000_fh *fh;
+	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	int rc;
+	int radio = 0;
+
+	dprintk(dev, V4L2_DEBUG_OPEN, "tm6000: open called (dev=%s)\n",
+		video_device_node_name(vdev));
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_VIDEO:
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		break;
+	case VFL_TYPE_VBI:
+		type = V4L2_BUF_TYPE_VBI_CAPTURE;
+		break;
+	case VFL_TYPE_RADIO:
+		radio = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* If more than one user, mutex should be added */
+	dev->users++;
+
+	dprintk(dev, V4L2_DEBUG_OPEN, "open dev=%s type=%s users=%d\n",
+		video_device_node_name(vdev), v4l2_type_names[type],
+		dev->users);
+
+	/* allocate + initialize per filehandle data */
+	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+	if (NULL == fh) {
+		dev->users--;
+		return -ENOMEM;
+	}
+
+	v4l2_fh_init(&fh->fh, vdev);
+	file->private_data = fh;
+	fh->dev      = dev;
+	fh->radio    = radio;
+	dev->radio   = radio;
+	fh->type     = type;
+	dev->fourcc  = format[0].fourcc;
+
+	fh->fmt      = format_by_fourcc(dev->fourcc);
+
+	tm6000_get_std_res(dev);
+
+	fh->width = dev->width;
+	fh->height = dev->height;
+
+	dprintk(dev, V4L2_DEBUG_OPEN, "Open: fh=%p, dev=%p, dev->vidq=%p\n",
+		fh, dev, &dev->vidq);
+	dprintk(dev, V4L2_DEBUG_OPEN, "Open: list_empty queued=%d\n",
+		list_empty(&dev->vidq.queued));
+	dprintk(dev, V4L2_DEBUG_OPEN, "Open: list_empty active=%d\n",
+		list_empty(&dev->vidq.active));
+
+	/* initialize hardware on analog mode */
+	rc = tm6000_init_analog_mode(dev);
+	if (rc < 0) {
+		v4l2_fh_exit(&fh->fh);
+		kfree(fh);
+		return rc;
+	}
+
+	dev->mode = TM6000_MODE_ANALOG;
+
+	if (!fh->radio) {
+		videobuf_queue_vmalloc_init(&fh->vb_vidq, &tm6000_video_qops,
+				NULL, &dev->slock,
+				fh->type,
+				V4L2_FIELD_INTERLACED,
+				sizeof(struct tm6000_buffer), fh, &dev->lock);
+	} else {
+		dprintk(dev, V4L2_DEBUG_OPEN, "video_open: setting radio device\n");
+		tm6000_set_audio_rinput(dev);
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_radio);
+		tm6000_prepare_isoc(dev);
+		tm6000_start_thread(dev);
+	}
+	v4l2_fh_add(&fh->fh);
+
+	return 0;
+}
+
+static int tm6000_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	int res;
+
+	mutex_lock(vdev->lock);
+	res = __tm6000_open(file);
+	mutex_unlock(vdev->lock);
+	return res;
+}
+
+static ssize_t
+tm6000_read(struct file *file, char __user *data, size_t count, loff_t *pos)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		int res;
+
+		if (!res_get(fh->dev, fh, true))
+			return -EBUSY;
+
+		if (mutex_lock_interruptible(&dev->lock))
+			return -ERESTARTSYS;
+		res = videobuf_read_stream(&fh->vb_vidq, data, count, pos, 0,
+					file->f_flags & O_NONBLOCK);
+		mutex_unlock(&dev->lock);
+		return res;
+	}
+	return 0;
+}
+
+static __poll_t
+__tm6000_poll(struct file *file, struct poll_table_struct *wait)
+{
+	__poll_t req_events = poll_requested_events(wait);
+	struct tm6000_fh        *fh = file->private_data;
+	struct tm6000_buffer    *buf;
+	__poll_t res = 0;
+
+	if (v4l2_event_pending(&fh->fh))
+		res = EPOLLPRI;
+	else if (req_events & EPOLLPRI)
+		poll_wait(file, &fh->fh.wait, wait);
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != fh->type)
+		return res | EPOLLERR;
+
+	if (!!is_res_streaming(fh->dev, fh))
+		return res | EPOLLERR;
+
+	if (!is_res_read(fh->dev, fh)) {
+		/* streaming capture */
+		if (list_empty(&fh->vb_vidq.stream))
+			return res | EPOLLERR;
+		buf = list_entry(fh->vb_vidq.stream.next, struct tm6000_buffer, vb.stream);
+		poll_wait(file, &buf->vb.done, wait);
+		if (buf->vb.state == VIDEOBUF_DONE ||
+		    buf->vb.state == VIDEOBUF_ERROR)
+			return res | EPOLLIN | EPOLLRDNORM;
+	} else if (req_events & (EPOLLIN | EPOLLRDNORM)) {
+		/* read() capture */
+		return res | videobuf_poll_stream(file, &fh->vb_vidq, wait);
+	}
+	return res;
+}
+
+static __poll_t tm6000_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+	__poll_t res;
+
+	mutex_lock(&dev->lock);
+	res = __tm6000_poll(file, wait);
+	mutex_unlock(&dev->lock);
+	return res;
+}
+
+static int tm6000_release(struct file *file)
+{
+	struct tm6000_fh         *fh = file->private_data;
+	struct tm6000_core      *dev = fh->dev;
+	struct video_device    *vdev = video_devdata(file);
+
+	dprintk(dev, V4L2_DEBUG_OPEN, "tm6000: close called (dev=%s, users=%d)\n",
+		video_device_node_name(vdev), dev->users);
+
+	mutex_lock(&dev->lock);
+	dev->users--;
+
+	res_free(dev, fh);
+
+	if (!dev->users) {
+		tm6000_uninit_isoc(dev);
+
+		/* Stop interrupt USB pipe */
+		tm6000_ir_int_stop(dev);
+
+		usb_reset_configuration(dev->udev);
+
+		if (dev->int_in.endp)
+			usb_set_interface(dev->udev,
+					dev->isoc_in.bInterfaceNumber, 2);
+		else
+			usb_set_interface(dev->udev,
+					dev->isoc_in.bInterfaceNumber, 0);
+
+		/* Start interrupt USB pipe */
+		tm6000_ir_int_start(dev);
+
+		if (!fh->radio)
+			videobuf_mmap_free(&fh->vb_vidq);
+	}
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	kfree(fh);
+	mutex_unlock(&dev->lock);
+
+	return 0;
+}
+
+static int tm6000_mmap(struct file *file, struct vm_area_struct * vma)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+	int res;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	res = videobuf_mmap_mapper(&fh->vb_vidq, vma);
+	mutex_unlock(&dev->lock);
+	return res;
+}
+
+static const struct v4l2_file_operations tm6000_fops = {
+	.owner = THIS_MODULE,
+	.open = tm6000_open,
+	.release = tm6000_release,
+	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
+	.read = tm6000_read,
+	.poll = tm6000_poll,
+	.mmap = tm6000_mmap,
+};
+
+static const struct v4l2_ioctl_ops video_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_s_std             = vidioc_s_std,
+	.vidioc_g_std             = vidioc_g_std,
+	.vidioc_enum_input        = vidioc_enum_input,
+	.vidioc_g_input           = vidioc_g_input,
+	.vidioc_s_input           = vidioc_s_input,
+	.vidioc_g_tuner           = vidioc_g_tuner,
+	.vidioc_s_tuner           = vidioc_s_tuner,
+	.vidioc_g_frequency       = vidioc_g_frequency,
+	.vidioc_s_frequency       = vidioc_s_frequency,
+	.vidioc_streamon          = vidioc_streamon,
+	.vidioc_streamoff         = vidioc_streamoff,
+	.vidioc_reqbufs           = vidioc_reqbufs,
+	.vidioc_querybuf          = vidioc_querybuf,
+	.vidioc_qbuf              = vidioc_qbuf,
+	.vidioc_dqbuf             = vidioc_dqbuf,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static struct video_device tm6000_template = {
+	.name		= "tm6000",
+	.fops           = &tm6000_fops,
+	.ioctl_ops      = &video_ioctl_ops,
+	.release	= video_device_release_empty,
+	.tvnorms        = TM6000_STD,
+};
+
+static const struct v4l2_file_operations radio_fops = {
+	.owner		= THIS_MODULE,
+	.open		= tm6000_open,
+	.poll		= v4l2_ctrl_poll,
+	.release	= tm6000_release,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops radio_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_g_tuner		= radio_g_tuner,
+	.vidioc_s_tuner		= radio_s_tuner,
+	.vidioc_g_frequency	= vidioc_g_frequency,
+	.vidioc_s_frequency	= vidioc_s_frequency,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static struct video_device tm6000_radio_template = {
+	.name			= "tm6000",
+	.fops			= &radio_fops,
+	.ioctl_ops		= &radio_ioctl_ops,
+};
+
+/* -----------------------------------------------------------------
+ *	Initialization and module stuff
+ * ------------------------------------------------------------------
+ */
+
+static void vdev_init(struct tm6000_core *dev,
+		struct video_device *vfd,
+		const struct video_device
+		*template, const char *type_name)
+{
+	*vfd = *template;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->release = video_device_release_empty;
+	vfd->lock = &dev->lock;
+
+	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
+
+	video_set_drvdata(vfd, dev);
+}
+
+int tm6000_v4l2_register(struct tm6000_core *dev)
+{
+	int ret = 0;
+
+	v4l2_ctrl_handler_init(&dev->ctrl_handler, 6);
+	v4l2_ctrl_handler_init(&dev->radio_ctrl_handler, 2);
+	v4l2_ctrl_new_std(&dev->radio_ctrl_handler, &tm6000_radio_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&dev->radio_ctrl_handler, &tm6000_radio_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, -15, 15, 1, 0);
+	v4l2_ctrl_new_std(&dev->ctrl_handler, &tm6000_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 54);
+	v4l2_ctrl_new_std(&dev->ctrl_handler, &tm6000_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 119);
+	v4l2_ctrl_new_std(&dev->ctrl_handler, &tm6000_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 112);
+	v4l2_ctrl_new_std(&dev->ctrl_handler, &tm6000_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_add_handler(&dev->ctrl_handler,
+			&dev->radio_ctrl_handler, NULL, false);
+
+	if (dev->radio_ctrl_handler.error)
+		ret = dev->radio_ctrl_handler.error;
+	if (!ret && dev->ctrl_handler.error)
+		ret = dev->ctrl_handler.error;
+	if (ret)
+		goto free_ctrl;
+
+	vdev_init(dev, &dev->vfd, &tm6000_template, "video");
+
+	dev->vfd.ctrl_handler = &dev->ctrl_handler;
+	dev->vfd.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+			       V4L2_CAP_READWRITE;
+	if (dev->tuner_type != TUNER_ABSENT)
+		dev->vfd.device_caps |= V4L2_CAP_TUNER;
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&dev->vidq.active);
+	INIT_LIST_HEAD(&dev->vidq.queued);
+
+	ret = video_register_device(&dev->vfd, VFL_TYPE_VIDEO, video_nr);
+
+	if (ret < 0) {
+		printk(KERN_INFO "%s: can't register video device\n",
+		       dev->name);
+		goto free_ctrl;
+	}
+
+	printk(KERN_INFO "%s: registered device %s\n",
+	       dev->name, video_device_node_name(&dev->vfd));
+
+	if (dev->caps.has_radio) {
+		vdev_init(dev, &dev->radio_dev, &tm6000_radio_template,
+							   "radio");
+		dev->radio_dev.ctrl_handler = &dev->radio_ctrl_handler;
+		dev->radio_dev.device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+		ret = video_register_device(&dev->radio_dev, VFL_TYPE_RADIO,
+					    radio_nr);
+		if (ret < 0) {
+			printk(KERN_INFO "%s: can't register radio device\n",
+			       dev->name);
+			goto unreg_video;
+		}
+
+		printk(KERN_INFO "%s: registered device %s\n",
+		       dev->name, video_device_node_name(&dev->radio_dev));
+	}
+
+	printk(KERN_INFO "Trident TVMaster TM5600/TM6000/TM6010 USB2 board (Load status: %d)\n", ret);
+	return ret;
+
+unreg_video:
+	video_unregister_device(&dev->vfd);
+free_ctrl:
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_ctrl_handler_free(&dev->radio_ctrl_handler);
+	return ret;
+}
+
+int tm6000_v4l2_unregister(struct tm6000_core *dev)
+{
+	video_unregister_device(&dev->vfd);
+
+	/* if URB buffers are still allocated free them now */
+	tm6000_free_urb_buffers(dev);
+
+	video_unregister_device(&dev->radio_dev);
+	return 0;
+}
+
+int tm6000_v4l2_exit(void)
+{
+	return 0;
+}
+
+module_param(video_nr, int, 0);
+MODULE_PARM_DESC(video_nr, "Allow changing video device number");
+
+module_param_named(debug, tm6000_debug, int, 0444);
+MODULE_PARM_DESC(debug, "activates debug info");
+
+module_param(vid_limit, int, 0644);
+MODULE_PARM_DESC(vid_limit, "capture memory limit in megabytes");
+
+module_param(keep_urb, bool, 0);
+MODULE_PARM_DESC(keep_urb, "Keep urb buffers allocated even when the device is closed by the user");
diff --git a/drivers/staging/media/deprecated/tm6000/tm6000.h b/drivers/staging/media/deprecated/tm6000/tm6000.h
new file mode 100644
index 000000000..c08c95312
--- /dev/null
+++ b/drivers/staging/media/deprecated/tm6000/tm6000.h
@@ -0,0 +1,396 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * tm6000.h - driver for TM5600/TM6000/TM6010 USB video capture devices
+ *
+ * Copyright (c) 2006-2007 Mauro Carvalho Chehab <mchehab@kernel.org>
+ *
+ * Copyright (c) 2007 Michel Ludwig <michel.ludwig@gmail.com>
+ *	- DVB-T support
+ */
+
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/videobuf-vmalloc.h>
+#include "tm6000-usb-isoc.h"
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+
+#include <linux/dvb/frontend.h>
+#include <media/dvb_demux.h>
+#include <media/dvb_frontend.h>
+#include <media/dmxdev.h>
+
+/* Inputs */
+enum tm6000_itype {
+	TM6000_INPUT_TV	= 1,
+	TM6000_INPUT_COMPOSITE1,
+	TM6000_INPUT_COMPOSITE2,
+	TM6000_INPUT_SVIDEO,
+	TM6000_INPUT_DVB,
+	TM6000_INPUT_RADIO,
+};
+
+enum tm6000_mux {
+	TM6000_VMUX_VIDEO_A = 1,
+	TM6000_VMUX_VIDEO_B,
+	TM6000_VMUX_VIDEO_AB,
+	TM6000_AMUX_ADC1,
+	TM6000_AMUX_ADC2,
+	TM6000_AMUX_SIF1,
+	TM6000_AMUX_SIF2,
+	TM6000_AMUX_I2S,
+};
+
+enum tm6000_devtype {
+	TM6000 = 0,
+	TM5600,
+	TM6010,
+};
+
+struct tm6000_input {
+	enum tm6000_itype	type;
+	enum tm6000_mux		vmux;
+	enum tm6000_mux		amux;
+	unsigned int		v_gpio;
+	unsigned int		a_gpio;
+};
+
+/* ------------------------------------------------------------------
+ *	Basic structures
+ * ------------------------------------------------------------------
+ */
+
+struct tm6000_fmt {
+	u32   fourcc;          /* v4l2 format id */
+	int   depth;
+};
+
+/* buffer for one video frame */
+struct tm6000_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer vb;
+
+	struct tm6000_fmt      *fmt;
+};
+
+struct tm6000_dmaqueue {
+	struct list_head       active;
+	struct list_head       queued;
+
+	/* thread for generating video stream*/
+	struct task_struct         *kthread;
+	wait_queue_head_t          wq;
+	/* Counters to control fps rate */
+	int                        frame;
+	int                        ini_jiffies;
+};
+
+/* device states */
+enum tm6000_core_state {
+	DEV_INITIALIZED   = 0x01,
+	DEV_DISCONNECTED  = 0x02,
+	DEV_MISCONFIGURED = 0x04,
+};
+
+/* io methods */
+enum tm6000_io_method {
+	IO_NONE,
+	IO_READ,
+	IO_MMAP,
+};
+
+enum tm6000_mode {
+	TM6000_MODE_UNKNOWN = 0,
+	TM6000_MODE_ANALOG,
+	TM6000_MODE_DIGITAL,
+};
+
+struct tm6000_gpio {
+	int		tuner_reset;
+	int		tuner_on;
+	int		demod_reset;
+	int		demod_on;
+	int		power_led;
+	int		dvb_led;
+	int		ir;
+};
+
+struct tm6000_capabilities {
+	unsigned int    has_tuner:1;
+	unsigned int    has_tda9874:1;
+	unsigned int    has_dvb:1;
+	unsigned int    has_zl10353:1;
+	unsigned int    has_eeprom:1;
+	unsigned int    has_remote:1;
+	unsigned int    has_radio:1;
+};
+
+struct tm6000_dvb {
+	struct dvb_adapter	adapter;
+	struct dvb_demux	demux;
+	struct dvb_frontend	*frontend;
+	struct dmxdev		dmxdev;
+	unsigned int		streams;
+	struct urb		*bulk_urb;
+	struct mutex		mutex;
+};
+
+struct snd_tm6000_card {
+	struct snd_card			*card;
+	spinlock_t			reg_lock;
+	struct tm6000_core		*core;
+	struct snd_pcm_substream	*substream;
+
+	/* temporary data for buffer fill processing */
+	unsigned			buf_pos;
+	unsigned			period_pos;
+};
+
+struct tm6000_endpoint {
+	struct usb_host_endpoint	*endp;
+	__u8				bInterfaceNumber;
+	__u8				bAlternateSetting;
+	unsigned			maxsize;
+};
+
+#define TM6000_QUIRK_NO_USB_DELAY (1 << 0)
+
+struct tm6000_core {
+	/* generic device properties */
+	char				name[30];	/* name (including minor) of the device */
+	int				model;		/* index in the device_data struct */
+	int				devno;		/* marks the number of this device */
+	enum tm6000_devtype		dev_type;	/* type of device */
+	unsigned char			eedata[256];	/* Eeprom data */
+	unsigned			eedata_size;	/* Size of the eeprom info */
+
+	v4l2_std_id                     norm;           /* Current norm */
+	int				width, height;	/* Selected resolution */
+
+	enum tm6000_core_state		state;
+
+	/* Device Capabilities*/
+	struct tm6000_capabilities	caps;
+
+	/* Used to load alsa/dvb */
+	struct work_struct		request_module_wk;
+
+	/* Tuner configuration */
+	int				tuner_type;		/* type of the tuner */
+	int				tuner_addr;		/* tuner address */
+
+	struct tm6000_gpio		gpio;
+
+	char				*ir_codes;
+
+	__u8				radio;
+
+	/* Demodulator configuration */
+	int				demod_addr;	/* demodulator address */
+
+	int				audio_bitrate;
+	/* i2c i/o */
+	struct i2c_adapter		i2c_adap;
+	struct i2c_client		i2c_client;
+
+
+	/* extension */
+	struct list_head		devlist;
+
+	/* video for linux */
+	int				users;
+
+	/* various device info */
+	struct tm6000_fh		*resources;	/* Points to fh that is streaming */
+	bool				is_res_read;
+
+	struct video_device		vfd;
+	struct video_device		radio_dev;
+	struct tm6000_dmaqueue		vidq;
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct v4l2_ctrl_handler	radio_ctrl_handler;
+
+	int				input;
+	struct tm6000_input		vinput[3];	/* video input */
+	struct tm6000_input		rinput;		/* radio input */
+
+	int				freq;
+	unsigned int			fourcc;
+
+	enum tm6000_mode		mode;
+
+	int				ctl_mute;             /* audio */
+	int				ctl_volume;
+	int				amode;
+
+	/* DVB-T support */
+	struct tm6000_dvb		*dvb;
+
+	/* audio support */
+	struct snd_tm6000_card		*adev;
+	struct work_struct		wq_trigger;   /* Trigger to start/stop audio for alsa module */
+	atomic_t			stream_started;  /* stream should be running if true */
+
+	struct tm6000_IR		*ir;
+
+	/* locks */
+	struct mutex			lock;
+	struct mutex			usb_lock;
+
+	/* usb transfer */
+	struct usb_device		*udev;		/* the usb device */
+
+	struct tm6000_endpoint		bulk_in, bulk_out, isoc_in, isoc_out;
+	struct tm6000_endpoint		int_in, int_out;
+
+	/* scaler!=0 if scaler is active*/
+	int				scaler;
+
+		/* Isoc control struct */
+	struct usb_isoc_ctl          isoc_ctl;
+
+	spinlock_t                   slock;
+
+	/* urb dma buffers */
+	char				**urb_buffer;
+	dma_addr_t			*urb_dma;
+	unsigned int			urb_size;
+
+	unsigned long quirks;
+};
+
+enum tm6000_ops_type {
+	TM6000_AUDIO = 0x10,
+	TM6000_DVB = 0x20,
+};
+
+struct tm6000_ops {
+	struct list_head	next;
+	char			*name;
+	enum tm6000_ops_type	type;
+	int (*init)(struct tm6000_core *);
+	int (*fini)(struct tm6000_core *);
+	int (*fillbuf)(struct tm6000_core *, char *buf, int size);
+};
+
+struct tm6000_fh {
+	struct v4l2_fh		     fh;
+	struct tm6000_core           *dev;
+	unsigned int                 radio;
+
+	/* video capture */
+	struct tm6000_fmt            *fmt;
+	unsigned int                 width, height;
+	struct videobuf_queue        vb_vidq;
+
+	enum v4l2_buf_type           type;
+};
+
+#define TM6000_STD	(V4L2_STD_PAL|V4L2_STD_PAL_N|V4L2_STD_PAL_Nc|    \
+			V4L2_STD_PAL_M|V4L2_STD_PAL_60|V4L2_STD_NTSC_M| \
+			V4L2_STD_NTSC_M_JP|V4L2_STD_SECAM)
+
+/* In tm6000-cards.c */
+
+int tm6000_tuner_callback(void *ptr, int component, int command, int arg);
+int tm6000_xc5000_callback(void *ptr, int component, int command, int arg);
+int tm6000_cards_setup(struct tm6000_core *dev);
+void tm6000_flash_led(struct tm6000_core *dev, u8 state);
+
+/* In tm6000-core.c */
+
+int tm6000_read_write_usb(struct tm6000_core *dev, u8 reqtype, u8 req,
+			   u16 value, u16 index, u8 *buf, u16 len);
+int tm6000_get_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_get_reg16(struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_get_reg32(struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_set_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_set_reg_mask(struct tm6000_core *dev, u8 req, u16 value,
+						u16 index, u16 mask);
+int tm6000_i2c_reset(struct tm6000_core *dev, u16 tsleep);
+int tm6000_init(struct tm6000_core *dev);
+int tm6000_reset(struct tm6000_core *dev);
+
+int tm6000_init_analog_mode(struct tm6000_core *dev);
+int tm6000_init_digital_mode(struct tm6000_core *dev);
+int tm6000_set_audio_bitrate(struct tm6000_core *dev, int bitrate);
+int tm6000_set_audio_rinput(struct tm6000_core *dev);
+int tm6000_tvaudio_set_mute(struct tm6000_core *dev, u8 mute);
+void tm6000_set_volume(struct tm6000_core *dev, int vol);
+
+int tm6000_v4l2_register(struct tm6000_core *dev);
+int tm6000_v4l2_unregister(struct tm6000_core *dev);
+int tm6000_v4l2_exit(void);
+void tm6000_set_fourcc_format(struct tm6000_core *dev);
+
+void tm6000_remove_from_devlist(struct tm6000_core *dev);
+void tm6000_add_into_devlist(struct tm6000_core *dev);
+int tm6000_register_extension(struct tm6000_ops *ops);
+void tm6000_unregister_extension(struct tm6000_ops *ops);
+void tm6000_init_extension(struct tm6000_core *dev);
+void tm6000_close_extension(struct tm6000_core *dev);
+int tm6000_call_fillbuf(struct tm6000_core *dev, enum tm6000_ops_type type,
+			char *buf, int size);
+
+
+/* In tm6000-stds.c */
+void tm6000_get_std_res(struct tm6000_core *dev);
+int tm6000_set_standard(struct tm6000_core *dev);
+
+/* In tm6000-i2c.c */
+int tm6000_i2c_register(struct tm6000_core *dev);
+int tm6000_i2c_unregister(struct tm6000_core *dev);
+
+/* In tm6000-queue.c */
+
+int tm6000_v4l2_mmap(struct file *filp, struct vm_area_struct *vma);
+
+int tm6000_vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type i);
+int tm6000_vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type i);
+int tm6000_vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *rb);
+int tm6000_vidioc_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *b);
+int tm6000_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *b);
+int tm6000_vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b);
+ssize_t tm6000_v4l2_read(struct file *filp, char __user * buf, size_t count,
+			 loff_t *f_pos);
+unsigned int tm6000_v4l2_poll(struct file *file,
+			      struct poll_table_struct *wait);
+int tm6000_queue_init(struct tm6000_core *dev);
+
+/* In tm6000-alsa.c */
+/*int tm6000_audio_init(struct tm6000_core *dev, int idx);*/
+
+/* In tm6000-input.c */
+int tm6000_ir_init(struct tm6000_core *dev);
+int tm6000_ir_fini(struct tm6000_core *dev);
+void tm6000_ir_wait(struct tm6000_core *dev, u8 state);
+int tm6000_ir_int_start(struct tm6000_core *dev);
+void tm6000_ir_int_stop(struct tm6000_core *dev);
+
+/* Debug stuff */
+
+extern int tm6000_debug;
+
+#define dprintk(dev, level, fmt, arg...) do {\
+	if (tm6000_debug & level) \
+		printk(KERN_INFO "(%lu) %s %s :"fmt, jiffies, \
+			 dev->name, __func__ , ##arg); } while (0)
+
+#define V4L2_DEBUG_REG		0x0004
+#define V4L2_DEBUG_I2C		0x0008
+#define V4L2_DEBUG_QUEUE	0x0010
+#define V4L2_DEBUG_ISOC		0x0020
+#define V4L2_DEBUG_RES_LOCK	0x0040	/* Resource locking */
+#define V4L2_DEBUG_OPEN		0x0080	/* video open/close debug */
+
+#define tm6000_err(fmt, arg...) do {\
+	printk(KERN_ERR "tm6000 %s :"fmt, \
+		__func__ , ##arg); } while (0)
diff --git a/drivers/staging/media/deprecated/vpfe_capture/Kconfig b/drivers/staging/media/deprecated/vpfe_capture/Kconfig
new file mode 100644
index 000000000..10250e7e5
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/Kconfig
@@ -0,0 +1,58 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_DM6446_CCDC
+	tristate "TI DM6446 CCDC video capture driver"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on I2C
+	select VIDEOBUF_DMA_CONTIG
+	help
+	  Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
+	  with decoder modules such as TVP5146 over BT656 or
+	  sensor module such as MT9T001 over a raw interface. This
+	  module configures the interface and CCDC/ISIF to do
+	  video frame capture from slave decoders.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpfe_capture.ko and dm644x_ccdc.ko
+
+config VIDEO_DM355_CCDC
+	tristate "TI DM355 CCDC video capture driver"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on I2C
+	select VIDEOBUF_DMA_CONTIG
+	help
+	  Enables DM355 CCD hw module. DM355 CCDC hw interfaces
+	  with decoder modules such as TVP5146 over BT656 or
+	  sensor module such as MT9T001 over a raw interface. This
+	  module configures the interface and CCDC/ISIF to do
+	  video frame capture from a slave decoders
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpfe_capture.ko and dm355_ccdc.ko
+
+config VIDEO_DM365_ISIF
+	tristate "TI DM365 ISIF video capture driver"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on I2C
+	select VIDEOBUF_DMA_CONTIG
+	help
+	  Enables ISIF hw module. This is the hardware module for
+	  configuring ISIF in VPFE to capture Raw Bayer RGB data from
+	  a image sensor or YUV data from a YUV source.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpfe_capture.ko and isif.ko
diff --git a/drivers/staging/media/deprecated/vpfe_capture/Makefile b/drivers/staging/media/deprecated/vpfe_capture/Makefile
new file mode 100644
index 000000000..609e8dc09
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_VIDEO_DM6446_CCDC) += vpfe_capture.o dm644x_ccdc.o
+obj-$(CONFIG_VIDEO_DM355_CCDC) += vpfe_capture.o dm355_ccdc.o
+obj-$(CONFIG_VIDEO_DM365_ISIF) += vpfe_capture.o isif.o
diff --git a/drivers/staging/media/deprecated/vpfe_capture/TODO b/drivers/staging/media/deprecated/vpfe_capture/TODO
new file mode 100644
index 000000000..ce654d733
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/TODO
@@ -0,0 +1,7 @@
+These are one of the few drivers still not using the vb2
+framework, so these drivers are now deprecated with the intent of
+removing them altogether by the beginning of 2023.
+
+In order to keep these drivers they have to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/vpfe_capture/ccdc_hw_device.h b/drivers/staging/media/deprecated/vpfe_capture/ccdc_hw_device.h
new file mode 100644
index 000000000..a545052a9
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/ccdc_hw_device.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * ccdc device API
+ */
+#ifndef _CCDC_HW_DEVICE_H
+#define _CCDC_HW_DEVICE_H
+
+#ifdef __KERNEL__
+#include <linux/videodev2.h>
+#include <linux/device.h>
+#include <media/davinci/vpfe_types.h>
+#include <media/davinci/ccdc_types.h>
+
+/*
+ * ccdc hw operations
+ */
+struct ccdc_hw_ops {
+	/* Pointer to initialize function to initialize ccdc device */
+	int (*open) (struct device *dev);
+	/* Pointer to deinitialize function */
+	int (*close) (struct device *dev);
+	/* set ccdc base address */
+	void (*set_ccdc_base)(void *base, int size);
+	/* Pointer to function to enable or disable ccdc */
+	void (*enable) (int en);
+	/* reset sbl. only for 6446 */
+	void (*reset) (void);
+	/* enable output to sdram */
+	void (*enable_out_to_sdram) (int en);
+	/* Pointer to function to set hw parameters */
+	int (*set_hw_if_params) (struct vpfe_hw_if_param *param);
+	/* get interface parameters */
+	int (*get_hw_if_params) (struct vpfe_hw_if_param *param);
+	/* Pointer to function to configure ccdc */
+	int (*configure) (void);
+
+	/* Pointer to function to set buffer type */
+	int (*set_buftype) (enum ccdc_buftype buf_type);
+	/* Pointer to function to get buffer type */
+	enum ccdc_buftype (*get_buftype) (void);
+	/* Pointer to function to set frame format */
+	int (*set_frame_format) (enum ccdc_frmfmt frm_fmt);
+	/* Pointer to function to get frame format */
+	enum ccdc_frmfmt (*get_frame_format) (void);
+	/* enumerate hw pix formats */
+	int (*enum_pix)(u32 *hw_pix, int i);
+	/* Pointer to function to set buffer type */
+	u32 (*get_pixel_format) (void);
+	/* Pointer to function to get pixel format. */
+	int (*set_pixel_format) (u32 pixfmt);
+	/* Pointer to function to set image window */
+	int (*set_image_window) (struct v4l2_rect *win);
+	/* Pointer to function to set image window */
+	void (*get_image_window) (struct v4l2_rect *win);
+	/* Pointer to function to get line length */
+	unsigned int (*get_line_length) (void);
+
+	/* Pointer to function to set frame buffer address */
+	void (*setfbaddr) (unsigned long addr);
+	/* Pointer to function to get field id */
+	int (*getfid) (void);
+};
+
+struct ccdc_hw_device {
+	/* ccdc device name */
+	char name[32];
+	/* module owner */
+	struct module *owner;
+	/* hw ops */
+	struct ccdc_hw_ops hw_ops;
+};
+
+/* Used by CCDC module to register & unregister with vpfe capture driver */
+int vpfe_register_ccdc_device(const struct ccdc_hw_device *dev);
+void vpfe_unregister_ccdc_device(const struct ccdc_hw_device *dev);
+
+#endif
+#endif
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.c b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.c
new file mode 100644
index 000000000..da8db53e9
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.c
@@ -0,0 +1,934 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2005-2009 Texas Instruments Inc
+ *
+ * CCDC hardware module for DM355
+ * ------------------------------
+ *
+ * This module is for configuring DM355 CCD controller of VPFE to capture
+ * Raw yuv or Bayer RGB data from a decoder. CCDC has several modules
+ * such as Defect Pixel Correction, Color Space Conversion etc to
+ * pre-process the Bayer RGB data, before writing it to SDRAM.
+ *
+ * TODO: 1) Raw bayer parameter settings and bayer capture
+ *	 2) Split module parameter structure to module specific ioctl structs
+ *	 3) add support for lense shading correction
+ *	 4) investigate if enum used for user space type definition
+ *	    to be replaced by #defines or integer
+ */
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include "dm355_ccdc.h"
+#include <media/davinci/vpss.h>
+
+#include "dm355_ccdc_regs.h"
+#include "ccdc_hw_device.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("CCDC Driver for DM355");
+MODULE_AUTHOR("Texas Instruments");
+
+static struct ccdc_oper_config {
+	struct device *dev;
+	/* CCDC interface type */
+	enum vpfe_hw_if_type if_type;
+	/* Raw Bayer configuration */
+	struct ccdc_params_raw bayer;
+	/* YCbCr configuration */
+	struct ccdc_params_ycbcr ycbcr;
+	/* ccdc base address */
+	void __iomem *base_addr;
+} ccdc_cfg = {
+	/* Raw configurations */
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = CCDC_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.gain = {
+			.r_ye = 256,
+			.gb_g = 256,
+			.gr_cy = 256,
+			.b_mg = 256
+		},
+		.config_params = {
+			.datasft = 2,
+			.mfilt1 = CCDC_NO_MEDIAN_FILTER1,
+			.mfilt2 = CCDC_NO_MEDIAN_FILTER2,
+			.alaw = {
+				.gamma_wd = 2,
+			},
+			.blk_clamp = {
+				.sample_pixel = 1,
+				.dc_sub = 25
+			},
+			.col_pat_field0 = {
+				.olop = CCDC_GREEN_BLUE,
+				.olep = CCDC_BLUE,
+				.elop = CCDC_RED,
+				.elep = CCDC_GREEN_RED
+			},
+			.col_pat_field1 = {
+				.olop = CCDC_GREEN_BLUE,
+				.olep = CCDC_BLUE,
+				.elop = CCDC_RED,
+				.elep = CCDC_GREEN_RED
+			},
+		},
+	},
+	/* YCbCr configuration */
+	.ycbcr = {
+		.win = CCDC_WIN_PAL,
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.bt656_enable = 1,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED
+	},
+};
+
+
+/* Raw Bayer formats */
+static u32 ccdc_raw_bayer_pix_formats[] =
+		{V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static u32 ccdc_raw_yuv_pix_formats[] =
+		{V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(ccdc_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, ccdc_cfg.base_addr + offset);
+}
+
+static void ccdc_enable(int en)
+{
+	unsigned int temp;
+	temp = regr(SYNCEN);
+	temp &= (~CCDC_SYNCEN_VDHDEN_MASK);
+	temp |= (en & CCDC_SYNCEN_VDHDEN_MASK);
+	regw(temp, SYNCEN);
+}
+
+static void ccdc_enable_output_to_sdram(int en)
+{
+	unsigned int temp;
+	temp = regr(SYNCEN);
+	temp &= (~(CCDC_SYNCEN_WEN_MASK));
+	temp |= ((en << CCDC_SYNCEN_WEN_SHIFT) & CCDC_SYNCEN_WEN_MASK);
+	regw(temp, SYNCEN);
+}
+
+static void ccdc_config_gain_offset(void)
+{
+	/* configure gain */
+	regw(ccdc_cfg.bayer.gain.r_ye, RYEGAIN);
+	regw(ccdc_cfg.bayer.gain.gr_cy, GRCYGAIN);
+	regw(ccdc_cfg.bayer.gain.gb_g, GBGGAIN);
+	regw(ccdc_cfg.bayer.gain.b_mg, BMGGAIN);
+	/* configure offset */
+	regw(ccdc_cfg.bayer.ccdc_offset, OFFSET);
+}
+
+/*
+ * ccdc_restore_defaults()
+ * This function restore power on defaults in the ccdc registers
+ */
+static int ccdc_restore_defaults(void)
+{
+	int i;
+
+	dev_dbg(ccdc_cfg.dev, "\nstarting ccdc_restore_defaults...");
+	/* set all registers to zero */
+	for (i = 0; i <= CCDC_REG_LAST; i += 4)
+		regw(0, i);
+
+	/* now override the values with power on defaults in registers */
+	regw(MODESET_DEFAULT, MODESET);
+	/* no culling support */
+	regw(CULH_DEFAULT, CULH);
+	regw(CULV_DEFAULT, CULV);
+	/* Set default Gain and Offset */
+	ccdc_cfg.bayer.gain.r_ye = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.gb_g = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.gr_cy = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.b_mg = GAIN_DEFAULT;
+	ccdc_config_gain_offset();
+	regw(OUTCLIP_DEFAULT, OUTCLIP);
+	regw(LSCCFG2_DEFAULT, LSCCFG2);
+	/* select ccdc input */
+	if (vpss_select_ccdc_source(VPSS_CCDCIN)) {
+		dev_dbg(ccdc_cfg.dev, "\ncouldn't select ccdc input source");
+		return -EFAULT;
+	}
+	/* select ccdc clock */
+	if (vpss_enable_clock(VPSS_CCDC_CLOCK, 1) < 0) {
+		dev_dbg(ccdc_cfg.dev, "\ncouldn't enable ccdc clock");
+		return -EFAULT;
+	}
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_restore_defaults...");
+	return 0;
+}
+
+static int ccdc_open(struct device *device)
+{
+	return ccdc_restore_defaults();
+}
+
+static int ccdc_close(struct device *device)
+{
+	/* disable clock */
+	vpss_enable_clock(VPSS_CCDC_CLOCK, 0);
+	/* do nothing for now */
+	return 0;
+}
+/*
+ * ccdc_setwin()
+ * This function will configure the window size to
+ * be capture in CCDC reg.
+ */
+static void ccdc_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt, int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int mid_img = 0;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_setwin...");
+
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = ((image_win->width) << (ppc - 1)) - 1;
+
+	/* Writing the horizontal info into the registers */
+	regw(horz_start, SPH);
+	regw(horz_nr_pixels, NPH);
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		/* configure VDINT0 and VDINT1 */
+		regw(vert_start, VDINT0);
+	} else {
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/* configure VDINT0 and VDINT1 */
+		mid_img = vert_start + (image_win->height / 2);
+		regw(vert_start, VDINT0);
+		regw(mid_img, VDINT1);
+	}
+	regw(vert_start & CCDC_START_VER_ONE_MASK, SLV0);
+	regw(vert_start & CCDC_START_VER_TWO_MASK, SLV1);
+	regw(vert_nr_lines & CCDC_NUM_LINES_VER, NLV);
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_setwin...");
+}
+
+/* This function will configure CCDC for YCbCr video capture */
+static void ccdc_config_ycbcr(void)
+{
+	struct ccdc_params_ycbcr *params = &ccdc_cfg.ycbcr;
+	u32 temp;
+
+	/* first set the CCDC power on defaults values in all registers */
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_ycbcr...");
+	ccdc_restore_defaults();
+
+	/* configure pixel format & video frame format */
+	temp = (((params->pix_fmt & CCDC_INPUT_MODE_MASK) <<
+		CCDC_INPUT_MODE_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) <<
+		CCDC_FRM_FMT_SHIFT));
+
+	/* setup BT.656 sync mode */
+	if (params->bt656_enable) {
+		regw(CCDC_REC656IF_BT656_EN, REC656IF);
+		/*
+		 * configure the FID, VD, HD pin polarity fld,hd pol positive,
+		 * vd negative, 8-bit pack mode
+		 */
+		temp |= CCDC_VD_POL_NEGATIVE;
+	} else {		/* y/c external sync mode */
+		temp |= (((params->fid_pol & CCDC_FID_POL_MASK) <<
+			CCDC_FID_POL_SHIFT) |
+			((params->hd_pol & CCDC_HD_POL_MASK) <<
+			CCDC_HD_POL_SHIFT) |
+			((params->vd_pol & CCDC_VD_POL_MASK) <<
+			CCDC_VD_POL_SHIFT));
+	}
+
+	/* pack the data to 8-bit */
+	temp |= CCDC_DATA_PACK_ENABLE;
+
+	regw(temp, MODESET);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 2);
+
+	/* configure the order of y cb cr in SD-RAM */
+	temp = (params->pix_order << CCDC_Y8POS_SHIFT);
+	temp |= CCDC_LATCH_ON_VSYNC_DISABLE | CCDC_CCDCFG_FIDMD_NO_LATCH_VSYNC;
+	regw(temp, CCDCFG);
+
+	/*
+	 * configure the horizontal line offset. This is done by rounding up
+	 * width to a multiple of 16 pixels and multiply by two to account for
+	 * y:cb:cr 4:2:2 data
+	 */
+	regw(((params->win.width * 2 + 31) >> 5), HSIZE);
+
+	/* configure the memory line offset */
+	if (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED) {
+		/* two fields are interleaved in memory */
+		regw(CCDC_SDOFST_FIELD_INTERLEAVED, SDOFST);
+	}
+
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_config_ycbcr...\n");
+}
+
+/*
+ * ccdc_config_black_clamp()
+ * configure parameters for Optical Black Clamp
+ */
+static void ccdc_config_black_clamp(struct ccdc_black_clamp *bclamp)
+{
+	u32 val;
+
+	if (!bclamp->b_clamp_enable) {
+		/* configure DCSub */
+		regw(bclamp->dc_sub & CCDC_BLK_DC_SUB_MASK, DCSUB);
+		regw(0x0000, CLAMP);
+		return;
+	}
+	/* Enable the Black clamping, set sample lines and pixels */
+	val = (bclamp->start_pixel & CCDC_BLK_ST_PXL_MASK) |
+	      ((bclamp->sample_pixel & CCDC_BLK_SAMPLE_LN_MASK) <<
+		CCDC_BLK_SAMPLE_LN_SHIFT) | CCDC_BLK_CLAMP_ENABLE;
+	regw(val, CLAMP);
+
+	/* If Black clamping is enable then make dcsub 0 */
+	val = (bclamp->sample_ln & CCDC_NUM_LINE_CALC_MASK)
+			<< CCDC_NUM_LINE_CALC_SHIFT;
+	regw(val, DCSUB);
+}
+
+/*
+ * ccdc_config_black_compense()
+ * configure parameters for Black Compensation
+ */
+static void ccdc_config_black_compense(struct ccdc_black_compensation *bcomp)
+{
+	u32 val;
+
+	val = (bcomp->b & CCDC_BLK_COMP_MASK) |
+		((bcomp->gb & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_GB_COMP_SHIFT);
+	regw(val, BLKCMP1);
+
+	val = ((bcomp->gr & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_GR_COMP_SHIFT) |
+		((bcomp->r & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_R_COMP_SHIFT);
+	regw(val, BLKCMP0);
+}
+
+/*
+ * ccdc_write_dfc_entry()
+ * write an entry in the dfc table.
+ */
+static int ccdc_write_dfc_entry(int index, struct ccdc_vertical_dft *dfc)
+{
+/* TODO This is to be re-visited and adjusted */
+#define DFC_WRITE_WAIT_COUNT	1000
+	u32 val, count = DFC_WRITE_WAIT_COUNT;
+
+	regw(dfc->dft_corr_vert[index], DFCMEM0);
+	regw(dfc->dft_corr_horz[index], DFCMEM1);
+	regw(dfc->dft_corr_sub1[index], DFCMEM2);
+	regw(dfc->dft_corr_sub2[index], DFCMEM3);
+	regw(dfc->dft_corr_sub3[index], DFCMEM4);
+	/* set WR bit to write */
+	val = regr(DFCMEMCTL) | CCDC_DFCMEMCTL_DFCMWR_MASK;
+	regw(val, DFCMEMCTL);
+
+	/*
+	 * Assume, it is very short. If we get an error, we need to
+	 * adjust this value
+	 */
+	while (regr(DFCMEMCTL) & CCDC_DFCMEMCTL_DFCMWR_MASK)
+		count--;
+	/*
+	 * TODO We expect the count to be non-zero to be successful. Adjust
+	 * the count if write requires more time
+	 */
+
+	if (count) {
+		dev_err(ccdc_cfg.dev, "defect table write timeout !!!\n");
+		return -1;
+	}
+	return 0;
+}
+
+/*
+ * ccdc_config_vdfc()
+ * configure parameters for Vertical Defect Correction
+ */
+static int ccdc_config_vdfc(struct ccdc_vertical_dft *dfc)
+{
+	u32 val;
+	int i;
+
+	/* Configure General Defect Correction. The table used is from IPIPE */
+	val = dfc->gen_dft_en & CCDC_DFCCTL_GDFCEN_MASK;
+
+	/* Configure Vertical Defect Correction if needed */
+	if (!dfc->ver_dft_en) {
+		/* Enable only General Defect Correction */
+		regw(val, DFCCTL);
+		return 0;
+	}
+
+	if (dfc->table_size > CCDC_DFT_TABLE_SIZE)
+		return -EINVAL;
+
+	val |= CCDC_DFCCTL_VDFC_DISABLE;
+	val |= (dfc->dft_corr_ctl.vdfcsl & CCDC_DFCCTL_VDFCSL_MASK) <<
+		CCDC_DFCCTL_VDFCSL_SHIFT;
+	val |= (dfc->dft_corr_ctl.vdfcuda & CCDC_DFCCTL_VDFCUDA_MASK) <<
+		CCDC_DFCCTL_VDFCUDA_SHIFT;
+	val |= (dfc->dft_corr_ctl.vdflsft & CCDC_DFCCTL_VDFLSFT_MASK) <<
+		CCDC_DFCCTL_VDFLSFT_SHIFT;
+	regw(val , DFCCTL);
+
+	/* clear address ptr to offset 0 */
+	val = CCDC_DFCMEMCTL_DFCMARST_MASK << CCDC_DFCMEMCTL_DFCMARST_SHIFT;
+
+	/* write defect table entries */
+	for (i = 0; i < dfc->table_size; i++) {
+		/* increment address for non zero index */
+		if (i != 0)
+			val = CCDC_DFCMEMCTL_INC_ADDR;
+		regw(val, DFCMEMCTL);
+		if (ccdc_write_dfc_entry(i, dfc) < 0)
+			return -EFAULT;
+	}
+
+	/* update saturation level and enable dfc */
+	regw(dfc->saturation_ctl & CCDC_VDC_DFCVSAT_MASK, DFCVSAT);
+	val = regr(DFCCTL) | (CCDC_DFCCTL_VDFCEN_MASK <<
+			CCDC_DFCCTL_VDFCEN_SHIFT);
+	regw(val, DFCCTL);
+	return 0;
+}
+
+/*
+ * ccdc_config_csc()
+ * configure parameters for color space conversion
+ * Each register CSCM0-7 has two values in S8Q5 format.
+ */
+static void ccdc_config_csc(struct ccdc_csc *csc)
+{
+	u32 val1 = 0, val2;
+	int i;
+
+	if (!csc->enable)
+		return;
+
+	/* Enable the CSC sub-module */
+	regw(CCDC_CSC_ENABLE, CSCCTL);
+
+	/* Converting the co-eff as per the format of the register */
+	for (i = 0; i < CCDC_CSC_COEFF_TABLE_SIZE; i++) {
+		if ((i % 2) == 0) {
+			/* CSCM - LSB */
+			val1 = (csc->coeff[i].integer &
+				CCDC_CSC_COEF_INTEG_MASK)
+				<< CCDC_CSC_COEF_INTEG_SHIFT;
+			/*
+			 * convert decimal part to binary. Use 2 decimal
+			 * precision, user values range from .00 - 0.99
+			 */
+			val1 |= (((csc->coeff[i].decimal &
+				CCDC_CSC_COEF_DECIMAL_MASK) *
+				CCDC_CSC_DEC_MAX) / 100);
+		} else {
+
+			/* CSCM - MSB */
+			val2 = (csc->coeff[i].integer &
+				CCDC_CSC_COEF_INTEG_MASK)
+				<< CCDC_CSC_COEF_INTEG_SHIFT;
+			val2 |= (((csc->coeff[i].decimal &
+				 CCDC_CSC_COEF_DECIMAL_MASK) *
+				 CCDC_CSC_DEC_MAX) / 100);
+			val2 <<= CCDC_CSCM_MSB_SHIFT;
+			val2 |= val1;
+			regw(val2, (CSCM0 + ((i - 1) << 1)));
+		}
+	}
+}
+
+/*
+ * ccdc_config_color_patterns()
+ * configure parameters for color patterns
+ */
+static void ccdc_config_color_patterns(struct ccdc_col_pat *pat0,
+				       struct ccdc_col_pat *pat1)
+{
+	u32 val;
+
+	val = (pat0->olop | (pat0->olep << 2) | (pat0->elop << 4) |
+		(pat0->elep << 6) | (pat1->olop << 8) | (pat1->olep << 10) |
+		(pat1->elop << 12) | (pat1->elep << 14));
+	regw(val, COLPTN);
+}
+
+/* This function will configure CCDC for Raw mode image capture */
+static int ccdc_config_raw(void)
+{
+	struct ccdc_params_raw *params = &ccdc_cfg.bayer;
+	struct ccdc_config_params_raw *config_params =
+					&ccdc_cfg.bayer.config_params;
+	unsigned int val;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_raw...");
+
+	/* restore power on defaults to register */
+	ccdc_restore_defaults();
+
+	/* CCDCFG register:
+	 * set CCD Not to swap input since input is RAW data
+	 * set FID detection function to Latch at V-Sync
+	 * set WENLOG - ccdc valid area to AND
+	 * set TRGSEL to WENBIT
+	 * set EXTRG to DISABLE
+	 * disable latching function on VSYNC - shadowed registers
+	 */
+	regw(CCDC_YCINSWP_RAW | CCDC_CCDCFG_FIDMD_LATCH_VSYNC |
+	     CCDC_CCDCFG_WENLOG_AND | CCDC_CCDCFG_TRGSEL_WEN |
+	     CCDC_CCDCFG_EXTRG_DISABLE | CCDC_LATCH_ON_VSYNC_DISABLE, CCDCFG);
+
+	/*
+	 * Set VDHD direction to input,  input type to raw input
+	 * normal data polarity, do not use external WEN
+	 */
+	val = (CCDC_VDHDOUT_INPUT | CCDC_RAW_IP_MODE | CCDC_DATAPOL_NORMAL |
+		CCDC_EXWEN_DISABLE);
+
+	/*
+	 * Configure the vertical sync polarity (MODESET.VDPOL), horizontal
+	 * sync polarity (MODESET.HDPOL), field id polarity (MODESET.FLDPOL),
+	 * frame format(progressive or interlace), & pixel format (Input mode)
+	 */
+	val |= (((params->vd_pol & CCDC_VD_POL_MASK) << CCDC_VD_POL_SHIFT) |
+		((params->hd_pol & CCDC_HD_POL_MASK) << CCDC_HD_POL_SHIFT) |
+		((params->fid_pol & CCDC_FID_POL_MASK) << CCDC_FID_POL_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) << CCDC_FRM_FMT_SHIFT) |
+		((params->pix_fmt & CCDC_PIX_FMT_MASK) << CCDC_PIX_FMT_SHIFT));
+
+	/* set pack for alaw compression */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable)
+		val |= CCDC_DATA_PACK_ENABLE;
+
+	/* Configure for LPF */
+	if (config_params->lpf_enable)
+		val |= (config_params->lpf_enable & CCDC_LPF_MASK) <<
+			CCDC_LPF_SHIFT;
+
+	/* Configure the data shift */
+	val |= (config_params->datasft & CCDC_DATASFT_MASK) <<
+		CCDC_DATASFT_SHIFT;
+	regw(val , MODESET);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to MODESET...\n", val);
+
+	/* Configure the Median Filter threshold */
+	regw((config_params->med_filt_thres) & CCDC_MED_FILT_THRESH, MEDFILT);
+
+	/* Configure GAMMAWD register. defaur 11-2, and Mosaic cfa pattern */
+	val = CCDC_GAMMA_BITS_11_2 << CCDC_GAMMAWD_INPUT_SHIFT |
+		CCDC_CFA_MOSAIC;
+
+	/* Enable and configure aLaw register if needed */
+	if (config_params->alaw.enable) {
+		val |= (CCDC_ALAW_ENABLE |
+			((config_params->alaw.gamma_wd &
+			CCDC_ALAW_GAMMA_WD_MASK) <<
+			CCDC_GAMMAWD_INPUT_SHIFT));
+	}
+
+	/* Configure Median filter1 & filter2 */
+	val |= ((config_params->mfilt1 << CCDC_MFILT1_SHIFT) |
+		(config_params->mfilt2 << CCDC_MFILT2_SHIFT));
+
+	regw(val, GAMMAWD);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to GAMMAWD...\n", val);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 1);
+
+	/* Optical Clamp Averaging */
+	ccdc_config_black_clamp(&config_params->blk_clamp);
+
+	/* Black level compensation */
+	ccdc_config_black_compense(&config_params->blk_comp);
+
+	/* Vertical Defect Correction if needed */
+	if (ccdc_config_vdfc(&config_params->vertical_dft) < 0)
+		return -EFAULT;
+
+	/* color space conversion */
+	ccdc_config_csc(&config_params->csc);
+
+	/* color pattern */
+	ccdc_config_color_patterns(&config_params->col_pat_field0,
+				   &config_params->col_pat_field1);
+
+	/* Configure the Gain  & offset control */
+	ccdc_config_gain_offset();
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting %x to COLPTN...\n", val);
+
+	/* Configure DATAOFST  register */
+	val = (config_params->data_offset.horz_offset & CCDC_DATAOFST_MASK) <<
+		CCDC_DATAOFST_H_SHIFT;
+	val |= (config_params->data_offset.vert_offset & CCDC_DATAOFST_MASK) <<
+		CCDC_DATAOFST_V_SHIFT;
+	regw(val, DATAOFST);
+
+	/* configuring HSIZE register */
+	val = (params->horz_flip_enable & CCDC_HSIZE_FLIP_MASK) <<
+		CCDC_HSIZE_FLIP_SHIFT;
+
+	/* If pack 8 is enable then 1 pixel will take 1 byte */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable) {
+		val |= (((params->win.width) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK;
+
+		/* adjust to multiple of 32 */
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to HSIZE...\n",
+		       (((params->win.width) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK);
+	} else {
+		/* else one pixel will take 2 byte */
+		val |= (((params->win.width * 2) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK;
+
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to HSIZE...\n",
+		       (((params->win.width * 2) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK);
+	}
+	regw(val, HSIZE);
+
+	/* Configure SDOFST register */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_enable) {
+			/* For interlace inverse mode */
+			regw(CCDC_SDOFST_INTERLACE_INVERSE, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_INTERLACE_INVERSE);
+		} else {
+			/* For interlace non inverse mode */
+			regw(CCDC_SDOFST_INTERLACE_NORMAL, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_INTERLACE_NORMAL);
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		if (params->image_invert_enable) {
+			/* For progessive inverse mode */
+			regw(CCDC_SDOFST_PROGRESSIVE_INVERSE, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_PROGRESSIVE_INVERSE);
+		} else {
+			/* For progessive non inverse mode */
+			regw(CCDC_SDOFST_PROGRESSIVE_NORMAL, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_PROGRESSIVE_NORMAL);
+		}
+	}
+	dev_dbg(ccdc_cfg.dev, "\nend of ccdc_config_raw...");
+	return 0;
+}
+
+static int ccdc_configure(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_config_raw();
+	else
+		ccdc_config_ycbcr();
+	return 0;
+}
+
+static int ccdc_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.buf_type = buf_type;
+	else
+		ccdc_cfg.ycbcr.buf_type = buf_type;
+	return 0;
+}
+static enum ccdc_buftype ccdc_get_buftype(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.buf_type;
+	return ccdc_cfg.ycbcr.buf_type;
+}
+
+static int ccdc_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(ccdc_raw_bayer_pix_formats)) {
+			*pix = ccdc_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(ccdc_raw_yuv_pix_formats)) {
+			*pix = ccdc_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+	return ret;
+}
+
+static int ccdc_set_pixel_format(u32 pixfmt)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8)
+			alaw->enable = 1;
+		else if (pixfmt != V4L2_PIX_FMT_SBGGR16)
+			return -EINVAL;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+	}
+	return 0;
+}
+static u32 ccdc_get_pixel_format(void)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+	u32 pixfmt;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		if (alaw->enable)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (ccdc_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+static int ccdc_set_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.win = *win;
+	else
+		ccdc_cfg.ycbcr.win = *win;
+	return 0;
+}
+
+static void ccdc_get_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		*win = ccdc_cfg.bayer.win;
+	else
+		*win = ccdc_cfg.ycbcr.win;
+}
+
+static unsigned int ccdc_get_line_length(void)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int len;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if ((config_params->alaw.enable) ||
+		    (config_params->data_sz == CCDC_DATA_8BITS))
+			len = ccdc_cfg.bayer.win.width;
+		else
+			len = ccdc_cfg.bayer.win.width * 2;
+	} else
+		len = ccdc_cfg.ycbcr.win.width * 2;
+	return ALIGN(len, 32);
+}
+
+static int ccdc_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		ccdc_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+
+static enum ccdc_frmfmt ccdc_get_frame_format(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.frm_fmt;
+	else
+		return ccdc_cfg.ycbcr.frm_fmt;
+}
+
+static int ccdc_getfid(void)
+{
+	return  (regr(MODESET) >> 15) & 1;
+}
+
+/* misc operations */
+static inline void ccdc_setfbaddr(unsigned long addr)
+{
+	regw((addr >> 21) & 0x007f, STADRH);
+	regw((addr >> 5) & 0x0ffff, STADRL);
+}
+
+static int ccdc_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	ccdc_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_YCBCR_SYNC_16:
+	case VPFE_YCBCR_SYNC_8:
+		ccdc_cfg.ycbcr.vd_pol = params->vdpol;
+		ccdc_cfg.ycbcr.hd_pol = params->hdpol;
+		break;
+	default:
+		/* TODO add support for raw bayer here */
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct ccdc_hw_device ccdc_hw_dev = {
+	.name = "DM355 CCDC",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = ccdc_open,
+		.close = ccdc_close,
+		.enable = ccdc_enable,
+		.enable_out_to_sdram = ccdc_enable_output_to_sdram,
+		.set_hw_if_params = ccdc_set_hw_if_params,
+		.configure = ccdc_configure,
+		.set_buftype = ccdc_set_buftype,
+		.get_buftype = ccdc_get_buftype,
+		.enum_pix = ccdc_enum_pix,
+		.set_pixel_format = ccdc_set_pixel_format,
+		.get_pixel_format = ccdc_get_pixel_format,
+		.set_frame_format = ccdc_set_frame_format,
+		.get_frame_format = ccdc_get_frame_format,
+		.set_image_window = ccdc_set_image_window,
+		.get_image_window = ccdc_get_image_window,
+		.get_line_length = ccdc_get_line_length,
+		.setfbaddr = ccdc_setfbaddr,
+		.getfid = ccdc_getfid,
+	},
+};
+
+static int dm355_ccdc_probe(struct platform_device *pdev)
+{
+	void (*setup_pinmux)(void);
+	struct resource	*res;
+	int status = 0;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&ccdc_hw_dev);
+	if (status < 0)
+		return status;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		status = -ENODEV;
+		goto fail_nores;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), res->name);
+	if (!res) {
+		status = -EBUSY;
+		goto fail_nores;
+	}
+
+	ccdc_cfg.base_addr = ioremap(res->start, resource_size(res));
+	if (!ccdc_cfg.base_addr) {
+		status = -ENOMEM;
+		goto fail_nomem;
+	}
+
+	/* Platform data holds setup_pinmux function ptr */
+	if (NULL == pdev->dev.platform_data) {
+		status = -ENODEV;
+		goto fail_nomap;
+	}
+	setup_pinmux = pdev->dev.platform_data;
+	/*
+	 * setup Mux configuration for ccdc which may be different for
+	 * different SoCs using this CCDC
+	 */
+	setup_pinmux();
+	ccdc_cfg.dev = &pdev->dev;
+	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
+	return 0;
+fail_nomap:
+	iounmap(ccdc_cfg.base_addr);
+fail_nomem:
+	release_mem_region(res->start, resource_size(res));
+fail_nores:
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return status;
+}
+
+static int dm355_ccdc_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+
+	iounmap(ccdc_cfg.base_addr);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, resource_size(res));
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return 0;
+}
+
+static struct platform_driver dm355_ccdc_driver = {
+	.driver = {
+		.name	= "dm355_ccdc",
+	},
+	.remove = dm355_ccdc_remove,
+	.probe = dm355_ccdc_probe,
+};
+
+module_platform_driver(dm355_ccdc_driver);
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.h b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.h
new file mode 100644
index 000000000..1f3d00aa4
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc.h
@@ -0,0 +1,308 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2005-2009 Texas Instruments Inc
+ */
+#ifndef _DM355_CCDC_H
+#define _DM355_CCDC_H
+#include <media/davinci/ccdc_types.h>
+#include <media/davinci/vpfe_types.h>
+
+/* enum for No of pixel per line to be avg. in Black Clamping */
+enum ccdc_sample_length {
+	CCDC_SAMPLE_1PIXELS,
+	CCDC_SAMPLE_2PIXELS,
+	CCDC_SAMPLE_4PIXELS,
+	CCDC_SAMPLE_8PIXELS,
+	CCDC_SAMPLE_16PIXELS
+};
+
+/* enum for No of lines in Black Clamping */
+enum ccdc_sample_line {
+	CCDC_SAMPLE_1LINES,
+	CCDC_SAMPLE_2LINES,
+	CCDC_SAMPLE_4LINES,
+	CCDC_SAMPLE_8LINES,
+	CCDC_SAMPLE_16LINES
+};
+
+/* enum for Alaw gamma width */
+enum ccdc_gamma_width {
+	CCDC_GAMMA_BITS_13_4,
+	CCDC_GAMMA_BITS_12_3,
+	CCDC_GAMMA_BITS_11_2,
+	CCDC_GAMMA_BITS_10_1,
+	CCDC_GAMMA_BITS_09_0
+};
+
+enum ccdc_colpats {
+	CCDC_RED,
+	CCDC_GREEN_RED,
+	CCDC_GREEN_BLUE,
+	CCDC_BLUE
+};
+
+struct ccdc_col_pat {
+	enum ccdc_colpats olop;
+	enum ccdc_colpats olep;
+	enum ccdc_colpats elop;
+	enum ccdc_colpats elep;
+};
+
+enum ccdc_datasft {
+	CCDC_DATA_NO_SHIFT,
+	CCDC_DATA_SHIFT_1BIT,
+	CCDC_DATA_SHIFT_2BIT,
+	CCDC_DATA_SHIFT_3BIT,
+	CCDC_DATA_SHIFT_4BIT,
+	CCDC_DATA_SHIFT_5BIT,
+	CCDC_DATA_SHIFT_6BIT
+};
+
+enum ccdc_data_size {
+	CCDC_DATA_16BITS,
+	CCDC_DATA_15BITS,
+	CCDC_DATA_14BITS,
+	CCDC_DATA_13BITS,
+	CCDC_DATA_12BITS,
+	CCDC_DATA_11BITS,
+	CCDC_DATA_10BITS,
+	CCDC_DATA_8BITS
+};
+enum ccdc_mfilt1 {
+	CCDC_NO_MEDIAN_FILTER1,
+	CCDC_AVERAGE_FILTER1,
+	CCDC_MEDIAN_FILTER1
+};
+
+enum ccdc_mfilt2 {
+	CCDC_NO_MEDIAN_FILTER2,
+	CCDC_AVERAGE_FILTER2,
+	CCDC_MEDIAN_FILTER2
+};
+
+/* structure for ALaw */
+struct ccdc_a_law {
+	/* Enable/disable A-Law */
+	unsigned char enable;
+	/* Gamma Width Input */
+	enum ccdc_gamma_width gamma_wd;
+};
+
+/* structure for Black Clamping */
+struct ccdc_black_clamp {
+	/* only if bClampEnable is TRUE */
+	unsigned char b_clamp_enable;
+	/* only if bClampEnable is TRUE */
+	enum ccdc_sample_length sample_pixel;
+	/* only if bClampEnable is TRUE */
+	enum ccdc_sample_line sample_ln;
+	/* only if bClampEnable is TRUE */
+	unsigned short start_pixel;
+	/* only if bClampEnable is FALSE */
+	unsigned short sgain;
+	unsigned short dc_sub;
+};
+
+/* structure for Black Level Compensation */
+struct ccdc_black_compensation {
+	/* Constant value to subtract from Red component */
+	unsigned char r;
+	/* Constant value to subtract from Gr component */
+	unsigned char gr;
+	/* Constant value to subtract from Blue component */
+	unsigned char b;
+	/* Constant value to subtract from Gb component */
+	unsigned char gb;
+};
+
+struct ccdc_float {
+	int integer;
+	unsigned int decimal;
+};
+
+#define CCDC_CSC_COEFF_TABLE_SIZE	16
+/* structure for color space converter */
+struct ccdc_csc {
+	unsigned char enable;
+	/*
+	 * S8Q5. Use 2 decimal precision, user values range from -3.00 to 3.99.
+	 * example - to use 1.03, set integer part as 1, and decimal part as 3
+	 * to use -1.03, set integer part as -1 and decimal part as 3
+	 */
+	struct ccdc_float coeff[CCDC_CSC_COEFF_TABLE_SIZE];
+};
+
+/* Structures for Vertical Defect Correction*/
+enum ccdc_vdf_csl {
+	CCDC_VDF_NORMAL,
+	CCDC_VDF_HORZ_INTERPOL_SAT,
+	CCDC_VDF_HORZ_INTERPOL
+};
+
+enum ccdc_vdf_cuda {
+	CCDC_VDF_WHOLE_LINE_CORRECT,
+	CCDC_VDF_UPPER_DISABLE
+};
+
+enum ccdc_dfc_mwr {
+	CCDC_DFC_MWR_WRITE_COMPLETE,
+	CCDC_DFC_WRITE_REG
+};
+
+enum ccdc_dfc_mrd {
+	CCDC_DFC_READ_COMPLETE,
+	CCDC_DFC_READ_REG
+};
+
+enum ccdc_dfc_ma_rst {
+	CCDC_DFC_INCR_ADDR,
+	CCDC_DFC_CLR_ADDR
+};
+
+enum ccdc_dfc_mclr {
+	CCDC_DFC_CLEAR_COMPLETE,
+	CCDC_DFC_CLEAR
+};
+
+struct ccdc_dft_corr_ctl {
+	enum ccdc_vdf_csl vdfcsl;
+	enum ccdc_vdf_cuda vdfcuda;
+	unsigned int vdflsft;
+};
+
+struct ccdc_dft_corr_mem_ctl {
+	enum ccdc_dfc_mwr dfcmwr;
+	enum ccdc_dfc_mrd dfcmrd;
+	enum ccdc_dfc_ma_rst dfcmarst;
+	enum ccdc_dfc_mclr dfcmclr;
+};
+
+#define CCDC_DFT_TABLE_SIZE	16
+/*
+ * Main Structure for vertical defect correction. Vertical defect
+ * correction can correct up to 16 defects if defects less than 16
+ * then pad the rest with 0
+ */
+struct ccdc_vertical_dft {
+	unsigned char ver_dft_en;
+	unsigned char gen_dft_en;
+	unsigned int saturation_ctl;
+	struct ccdc_dft_corr_ctl dft_corr_ctl;
+	struct ccdc_dft_corr_mem_ctl dft_corr_mem_ctl;
+	int table_size;
+	unsigned int dft_corr_horz[CCDC_DFT_TABLE_SIZE];
+	unsigned int dft_corr_vert[CCDC_DFT_TABLE_SIZE];
+	unsigned int dft_corr_sub1[CCDC_DFT_TABLE_SIZE];
+	unsigned int dft_corr_sub2[CCDC_DFT_TABLE_SIZE];
+	unsigned int dft_corr_sub3[CCDC_DFT_TABLE_SIZE];
+};
+
+struct ccdc_data_offset {
+	unsigned char horz_offset;
+	unsigned char vert_offset;
+};
+
+/*
+ * Structure for CCDC configuration parameters for raw capture mode passed
+ * by application
+ */
+struct ccdc_config_params_raw {
+	/* data shift to be applied before storing */
+	enum ccdc_datasft datasft;
+	/* data size value from 8 to 16 bits */
+	enum ccdc_data_size data_sz;
+	/* median filter for sdram */
+	enum ccdc_mfilt1 mfilt1;
+	enum ccdc_mfilt2 mfilt2;
+	/* low pass filter enable/disable */
+	unsigned char lpf_enable;
+	/* Threshold of median filter */
+	int med_filt_thres;
+	/*
+	 * horz and vertical data offset. Applicable for defect correction
+	 * and lsc
+	 */
+	struct ccdc_data_offset data_offset;
+	/* Structure for Optional A-Law */
+	struct ccdc_a_law alaw;
+	/* Structure for Optical Black Clamp */
+	struct ccdc_black_clamp blk_clamp;
+	/* Structure for Black Compensation */
+	struct ccdc_black_compensation blk_comp;
+	/* structure for vertical Defect Correction Module Configuration */
+	struct ccdc_vertical_dft vertical_dft;
+	/* structure for color space converter Module Configuration */
+	struct ccdc_csc csc;
+	/* color patters for bayer capture */
+	struct ccdc_col_pat col_pat_field0;
+	struct ccdc_col_pat col_pat_field1;
+};
+
+#ifdef __KERNEL__
+#include <linux/io.h>
+
+#define CCDC_WIN_PAL	{0, 0, 720, 576}
+#define CCDC_WIN_VGA	{0, 0, 640, 480}
+
+struct ccdc_params_ycbcr {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	/* video window */
+	struct v4l2_rect win;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* enable BT.656 embedded sync mode */
+	int bt656_enable;
+	/* cb:y:cr:y or y:cb:y:cr in memory */
+	enum ccdc_pixorder pix_order;
+	/* interleaved or separated fields  */
+	enum ccdc_buftype buf_type;
+};
+
+/* Gain applied to Raw Bayer data */
+struct ccdc_gain {
+	unsigned short r_ye;
+	unsigned short gr_cy;
+	unsigned short gb_g;
+	unsigned short b_mg;
+};
+
+/* Structure for CCDC configuration parameters for raw capture mode */
+struct ccdc_params_raw {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	/* video window */
+	struct v4l2_rect win;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* interleaved or separated fields */
+	enum ccdc_buftype buf_type;
+	/* Gain values */
+	struct ccdc_gain gain;
+	/* offset */
+	unsigned int ccdc_offset;
+	/* horizontal flip enable */
+	unsigned char horz_flip_enable;
+	/*
+	 * enable to store the image in inverse order in memory
+	 * (bottom to top)
+	 */
+	unsigned char image_invert_enable;
+	/* Configurable part of raw data */
+	struct ccdc_config_params_raw config_params;
+};
+
+#endif
+#endif				/* DM355_CCDC_H */
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc_regs.h b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc_regs.h
new file mode 100644
index 000000000..eb381f075
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm355_ccdc_regs.h
@@ -0,0 +1,297 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2005-2009 Texas Instruments Inc
+ */
+#ifndef _DM355_CCDC_REGS_H
+#define _DM355_CCDC_REGS_H
+
+/**************************************************************************\
+* Register OFFSET Definitions
+\**************************************************************************/
+#define SYNCEN				0x00
+#define MODESET				0x04
+#define HDWIDTH				0x08
+#define VDWIDTH				0x0c
+#define PPLN				0x10
+#define LPFR				0x14
+#define SPH				0x18
+#define NPH				0x1c
+#define SLV0				0x20
+#define SLV1				0x24
+#define NLV				0x28
+#define CULH				0x2c
+#define CULV				0x30
+#define HSIZE				0x34
+#define SDOFST				0x38
+#define STADRH				0x3c
+#define STADRL				0x40
+#define CLAMP				0x44
+#define DCSUB				0x48
+#define COLPTN				0x4c
+#define BLKCMP0				0x50
+#define BLKCMP1				0x54
+#define MEDFILT				0x58
+#define RYEGAIN				0x5c
+#define GRCYGAIN			0x60
+#define GBGGAIN				0x64
+#define BMGGAIN				0x68
+#define OFFSET				0x6c
+#define OUTCLIP				0x70
+#define VDINT0				0x74
+#define VDINT1				0x78
+#define RSV0				0x7c
+#define GAMMAWD				0x80
+#define REC656IF			0x84
+#define CCDCFG				0x88
+#define FMTCFG				0x8c
+#define FMTPLEN				0x90
+#define FMTSPH				0x94
+#define FMTLNH				0x98
+#define FMTSLV				0x9c
+#define FMTLNV				0xa0
+#define FMTRLEN				0xa4
+#define FMTHCNT				0xa8
+#define FMT_ADDR_PTR_B			0xac
+#define FMT_ADDR_PTR(i)			(FMT_ADDR_PTR_B + (i * 4))
+#define FMTPGM_VF0			0xcc
+#define FMTPGM_VF1			0xd0
+#define FMTPGM_AP0			0xd4
+#define FMTPGM_AP1			0xd8
+#define FMTPGM_AP2			0xdc
+#define FMTPGM_AP3                      0xe0
+#define FMTPGM_AP4                      0xe4
+#define FMTPGM_AP5                      0xe8
+#define FMTPGM_AP6                      0xec
+#define FMTPGM_AP7                      0xf0
+#define LSCCFG1                         0xf4
+#define LSCCFG2                         0xf8
+#define LSCH0                           0xfc
+#define LSCV0                           0x100
+#define LSCKH                           0x104
+#define LSCKV                           0x108
+#define LSCMEMCTL                       0x10c
+#define LSCMEMD                         0x110
+#define LSCMEMQ                         0x114
+#define DFCCTL                          0x118
+#define DFCVSAT                         0x11c
+#define DFCMEMCTL                       0x120
+#define DFCMEM0                         0x124
+#define DFCMEM1                         0x128
+#define DFCMEM2                         0x12c
+#define DFCMEM3                         0x130
+#define DFCMEM4                         0x134
+#define CSCCTL                          0x138
+#define CSCM0                           0x13c
+#define CSCM1                           0x140
+#define CSCM2                           0x144
+#define CSCM3                           0x148
+#define CSCM4                           0x14c
+#define CSCM5                           0x150
+#define CSCM6                           0x154
+#define CSCM7                           0x158
+#define DATAOFST			0x15c
+#define CCDC_REG_LAST			DATAOFST
+/**************************************************************
+*	Define for various register bit mask and shifts for CCDC
+*
+**************************************************************/
+#define CCDC_RAW_IP_MODE			0
+#define CCDC_VDHDOUT_INPUT			0
+#define CCDC_YCINSWP_RAW			(0 << 4)
+#define CCDC_EXWEN_DISABLE			0
+#define CCDC_DATAPOL_NORMAL			0
+#define CCDC_CCDCFG_FIDMD_LATCH_VSYNC		0
+#define CCDC_CCDCFG_FIDMD_NO_LATCH_VSYNC	(1 << 6)
+#define CCDC_CCDCFG_WENLOG_AND			0
+#define CCDC_CCDCFG_TRGSEL_WEN			0
+#define CCDC_CCDCFG_EXTRG_DISABLE		0
+#define CCDC_CFA_MOSAIC				0
+#define CCDC_Y8POS_SHIFT			11
+
+#define CCDC_VDC_DFCVSAT_MASK			0x3fff
+#define CCDC_DATAOFST_MASK			0x0ff
+#define CCDC_DATAOFST_H_SHIFT			0
+#define CCDC_DATAOFST_V_SHIFT			8
+#define CCDC_GAMMAWD_CFA_MASK			1
+#define CCDC_GAMMAWD_CFA_SHIFT			5
+#define CCDC_GAMMAWD_INPUT_SHIFT		2
+#define CCDC_FID_POL_MASK			1
+#define CCDC_FID_POL_SHIFT			4
+#define CCDC_HD_POL_MASK			1
+#define CCDC_HD_POL_SHIFT			3
+#define CCDC_VD_POL_MASK			1
+#define CCDC_VD_POL_SHIFT			2
+#define CCDC_VD_POL_NEGATIVE			(1 << 2)
+#define CCDC_FRM_FMT_MASK			1
+#define CCDC_FRM_FMT_SHIFT			7
+#define CCDC_DATA_SZ_MASK			7
+#define CCDC_DATA_SZ_SHIFT			8
+#define CCDC_VDHDOUT_MASK			1
+#define CCDC_VDHDOUT_SHIFT			0
+#define CCDC_EXWEN_MASK				1
+#define CCDC_EXWEN_SHIFT			5
+#define CCDC_INPUT_MODE_MASK			3
+#define CCDC_INPUT_MODE_SHIFT			12
+#define CCDC_PIX_FMT_MASK			3
+#define CCDC_PIX_FMT_SHIFT			12
+#define CCDC_DATAPOL_MASK			1
+#define CCDC_DATAPOL_SHIFT			6
+#define CCDC_WEN_ENABLE				(1 << 1)
+#define CCDC_VDHDEN_ENABLE			(1 << 16)
+#define CCDC_LPF_ENABLE				(1 << 14)
+#define CCDC_ALAW_ENABLE			1
+#define CCDC_ALAW_GAMMA_WD_MASK			7
+#define CCDC_REC656IF_BT656_EN			3
+
+#define CCDC_FMTCFG_FMTMODE_MASK		3
+#define CCDC_FMTCFG_FMTMODE_SHIFT		1
+#define CCDC_FMTCFG_LNUM_MASK			3
+#define CCDC_FMTCFG_LNUM_SHIFT			4
+#define CCDC_FMTCFG_ADDRINC_MASK		7
+#define CCDC_FMTCFG_ADDRINC_SHIFT		8
+
+#define CCDC_CCDCFG_FIDMD_SHIFT			6
+#define	CCDC_CCDCFG_WENLOG_SHIFT		8
+#define CCDC_CCDCFG_TRGSEL_SHIFT		9
+#define CCDC_CCDCFG_EXTRG_SHIFT			10
+#define CCDC_CCDCFG_MSBINVI_SHIFT		13
+
+#define CCDC_HSIZE_FLIP_SHIFT			12
+#define CCDC_HSIZE_FLIP_MASK			1
+#define CCDC_HSIZE_VAL_MASK			0xFFF
+#define CCDC_SDOFST_FIELD_INTERLEAVED		0x249
+#define CCDC_SDOFST_INTERLACE_INVERSE		0x4B6D
+#define CCDC_SDOFST_INTERLACE_NORMAL		0x0B6D
+#define CCDC_SDOFST_PROGRESSIVE_INVERSE		0x4000
+#define CCDC_SDOFST_PROGRESSIVE_NORMAL		0
+#define CCDC_START_PX_HOR_MASK			0x7FFF
+#define CCDC_NUM_PX_HOR_MASK			0x7FFF
+#define CCDC_START_VER_ONE_MASK			0x7FFF
+#define CCDC_START_VER_TWO_MASK			0x7FFF
+#define CCDC_NUM_LINES_VER			0x7FFF
+
+#define CCDC_BLK_CLAMP_ENABLE			(1 << 15)
+#define CCDC_BLK_SGAIN_MASK			0x1F
+#define CCDC_BLK_ST_PXL_MASK			0x1FFF
+#define CCDC_BLK_SAMPLE_LN_MASK			3
+#define CCDC_BLK_SAMPLE_LN_SHIFT		13
+
+#define CCDC_NUM_LINE_CALC_MASK			3
+#define CCDC_NUM_LINE_CALC_SHIFT		14
+
+#define CCDC_BLK_DC_SUB_MASK			0x3FFF
+#define CCDC_BLK_COMP_MASK			0xFF
+#define CCDC_BLK_COMP_GB_COMP_SHIFT		8
+#define CCDC_BLK_COMP_GR_COMP_SHIFT		0
+#define CCDC_BLK_COMP_R_COMP_SHIFT		8
+#define CCDC_LATCH_ON_VSYNC_DISABLE		(1 << 15)
+#define CCDC_LATCH_ON_VSYNC_ENABLE		(0 << 15)
+#define CCDC_FPC_ENABLE				(1 << 15)
+#define CCDC_FPC_FPC_NUM_MASK			0x7FFF
+#define CCDC_DATA_PACK_ENABLE			(1 << 11)
+#define CCDC_FMT_HORZ_FMTLNH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_SHIFT		16
+#define CCDC_FMT_VERT_FMTLNV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_SHIFT		16
+#define CCDC_VP_OUT_VERT_NUM_MASK		0x3FFF
+#define CCDC_VP_OUT_VERT_NUM_SHIFT		17
+#define CCDC_VP_OUT_HORZ_NUM_MASK		0x1FFF
+#define CCDC_VP_OUT_HORZ_NUM_SHIFT		4
+#define CCDC_VP_OUT_HORZ_ST_MASK		0xF
+
+#define CCDC_CSC_COEF_INTEG_MASK		7
+#define CCDC_CSC_COEF_DECIMAL_MASK		0x1f
+#define CCDC_CSC_COEF_INTEG_SHIFT		5
+#define CCDC_CSCM_MSB_SHIFT			8
+#define CCDC_CSC_ENABLE				1
+#define CCDC_CSC_DEC_MAX			32
+
+#define CCDC_MFILT1_SHIFT			10
+#define CCDC_MFILT2_SHIFT			8
+#define CCDC_MED_FILT_THRESH			0x3FFF
+#define CCDC_LPF_MASK				1
+#define CCDC_LPF_SHIFT				14
+#define CCDC_OFFSET_MASK			0x3FF
+#define CCDC_DATASFT_MASK			7
+#define CCDC_DATASFT_SHIFT			8
+
+#define CCDC_DF_ENABLE				1
+
+#define CCDC_FMTPLEN_P0_MASK			0xF
+#define CCDC_FMTPLEN_P1_MASK			0xF
+#define CCDC_FMTPLEN_P2_MASK			7
+#define CCDC_FMTPLEN_P3_MASK			7
+#define CCDC_FMTPLEN_P0_SHIFT			0
+#define CCDC_FMTPLEN_P1_SHIFT			4
+#define CCDC_FMTPLEN_P2_SHIFT			8
+#define CCDC_FMTPLEN_P3_SHIFT			12
+
+#define CCDC_FMTSPH_MASK			0x1FFF
+#define CCDC_FMTLNH_MASK			0x1FFF
+#define CCDC_FMTSLV_MASK			0x1FFF
+#define CCDC_FMTLNV_MASK			0x7FFF
+#define CCDC_FMTRLEN_MASK			0x1FFF
+#define CCDC_FMTHCNT_MASK			0x1FFF
+
+#define CCDC_ADP_INIT_MASK			0x1FFF
+#define CCDC_ADP_LINE_SHIFT			13
+#define CCDC_ADP_LINE_MASK			3
+#define CCDC_FMTPGN_APTR_MASK			7
+
+#define CCDC_DFCCTL_GDFCEN_MASK			1
+#define CCDC_DFCCTL_VDFCEN_MASK			1
+#define CCDC_DFCCTL_VDFC_DISABLE		(0 << 4)
+#define CCDC_DFCCTL_VDFCEN_SHIFT		4
+#define CCDC_DFCCTL_VDFCSL_MASK			3
+#define CCDC_DFCCTL_VDFCSL_SHIFT		5
+#define CCDC_DFCCTL_VDFCUDA_MASK		1
+#define CCDC_DFCCTL_VDFCUDA_SHIFT		7
+#define CCDC_DFCCTL_VDFLSFT_MASK		3
+#define CCDC_DFCCTL_VDFLSFT_SHIFT		8
+#define CCDC_DFCMEMCTL_DFCMARST_MASK		1
+#define CCDC_DFCMEMCTL_DFCMARST_SHIFT		2
+#define CCDC_DFCMEMCTL_DFCMWR_MASK		1
+#define CCDC_DFCMEMCTL_DFCMWR_SHIFT		0
+#define CCDC_DFCMEMCTL_INC_ADDR			(0 << 2)
+
+#define CCDC_LSCCFG_GFTSF_MASK			7
+#define CCDC_LSCCFG_GFTSF_SHIFT			1
+#define CCDC_LSCCFG_GFTINV_MASK			0xf
+#define CCDC_LSCCFG_GFTINV_SHIFT		4
+#define CCDC_LSC_GFTABLE_SEL_MASK		3
+#define CCDC_LSC_GFTABLE_EPEL_SHIFT		8
+#define CCDC_LSC_GFTABLE_OPEL_SHIFT		10
+#define CCDC_LSC_GFTABLE_EPOL_SHIFT		12
+#define CCDC_LSC_GFTABLE_OPOL_SHIFT		14
+#define CCDC_LSC_GFMODE_MASK			3
+#define CCDC_LSC_GFMODE_SHIFT			4
+#define CCDC_LSC_DISABLE			0
+#define CCDC_LSC_ENABLE				1
+#define CCDC_LSC_TABLE1_SLC			0
+#define CCDC_LSC_TABLE2_SLC			1
+#define CCDC_LSC_TABLE3_SLC			2
+#define CCDC_LSC_MEMADDR_RESET			(1 << 2)
+#define CCDC_LSC_MEMADDR_INCR			(0 << 2)
+#define CCDC_LSC_FRAC_MASK_T1			0xFF
+#define CCDC_LSC_INT_MASK			3
+#define CCDC_LSC_FRAC_MASK			0x3FFF
+#define CCDC_LSC_CENTRE_MASK			0x3FFF
+#define CCDC_LSC_COEF_MASK			0xff
+#define CCDC_LSC_COEFL_SHIFT			0
+#define CCDC_LSC_COEFU_SHIFT			8
+#define CCDC_GAIN_MASK				0x7FF
+#define CCDC_SYNCEN_VDHDEN_MASK			(1 << 0)
+#define CCDC_SYNCEN_WEN_MASK			(1 << 1)
+#define CCDC_SYNCEN_WEN_SHIFT			1
+
+/* Power on Defaults in hardware */
+#define MODESET_DEFAULT				0x200
+#define CULH_DEFAULT				0xFFFF
+#define CULV_DEFAULT				0xFF
+#define GAIN_DEFAULT				256
+#define OUTCLIP_DEFAULT				0x3FFF
+#define LSCCFG2_DEFAULT				0xE
+
+#endif
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.c b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.c
new file mode 100644
index 000000000..4a93e5ad6
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.c
@@ -0,0 +1,879 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2006-2009 Texas Instruments Inc
+ *
+ * CCDC hardware module for DM6446
+ * ------------------------------
+ *
+ * This module is for configuring CCD controller of DM6446 VPFE to capture
+ * Raw yuv or Bayer RGB data from a decoder. CCDC has several modules
+ * such as Defect Pixel Correction, Color Space Conversion etc to
+ * pre-process the Raw Bayer RGB data, before writing it to SDRAM.
+ * This file is named DM644x so that other variants such DM6443
+ * may be supported using the same module.
+ *
+ * TODO: Test Raw bayer parameter settings and bayer capture
+ *	 Split module parameter structure to module specific ioctl structs
+ *	 investigate if enum used for user space type definition
+ *	 to be replaced by #defines or integer
+ */
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <linux/gfp.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include "dm644x_ccdc.h"
+#include <media/davinci/vpss.h>
+
+#include "dm644x_ccdc_regs.h"
+#include "ccdc_hw_device.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("CCDC Driver for DM6446");
+MODULE_AUTHOR("Texas Instruments");
+
+static struct ccdc_oper_config {
+	struct device *dev;
+	/* CCDC interface type */
+	enum vpfe_hw_if_type if_type;
+	/* Raw Bayer configuration */
+	struct ccdc_params_raw bayer;
+	/* YCbCr configuration */
+	struct ccdc_params_ycbcr ycbcr;
+	/* ccdc base address */
+	void __iomem *base_addr;
+} ccdc_cfg = {
+	/* Raw configurations */
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = CCDC_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.config_params = {
+			.data_sz = CCDC_DATA_10BITS,
+		},
+	},
+	.ycbcr = {
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.win = CCDC_WIN_PAL,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.bt656_enable = 1,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED
+	},
+};
+
+#define CCDC_MAX_RAW_YUV_FORMATS	2
+
+/* Raw Bayer formats */
+static u32 ccdc_raw_bayer_pix_formats[] =
+	{V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static u32 ccdc_raw_yuv_pix_formats[] =
+	{V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* CCDC Save/Restore context */
+static u32 ccdc_ctx[CCDC_REG_END / sizeof(u32)];
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(ccdc_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, ccdc_cfg.base_addr + offset);
+}
+
+static void ccdc_enable(int flag)
+{
+	regw(flag, CCDC_PCR);
+}
+
+static void ccdc_enable_vport(int flag)
+{
+	if (flag)
+		/* enable video port */
+		regw(CCDC_ENABLE_VIDEO_PORT, CCDC_FMTCFG);
+	else
+		regw(CCDC_DISABLE_VIDEO_PORT, CCDC_FMTCFG);
+}
+
+/*
+ * ccdc_setwin()
+ * This function will configure the window size
+ * to be capture in CCDC reg
+ */
+static void ccdc_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt,
+			int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int val = 0, mid_img = 0;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_setwin...");
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = (image_win->width << (ppc - 1)) - 1;
+	regw((horz_start << CCDC_HORZ_INFO_SPH_SHIFT) | horz_nr_pixels,
+	     CCDC_HORZ_INFO);
+
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		/* configure VDINT0 */
+		val = (vert_start << CCDC_VDINT_VDINT0_SHIFT);
+		regw(val, CCDC_VDINT);
+
+	} else {
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/*
+		 * configure VDINT0 and VDINT1. VDINT1 will be at half
+		 * of image height
+		 */
+		mid_img = vert_start + (image_win->height / 2);
+		val = (vert_start << CCDC_VDINT_VDINT0_SHIFT) |
+		    (mid_img & CCDC_VDINT_VDINT1_MASK);
+		regw(val, CCDC_VDINT);
+
+	}
+	regw((vert_start << CCDC_VERT_START_SLV0_SHIFT) | vert_start,
+	     CCDC_VERT_START);
+	regw(vert_nr_lines, CCDC_VERT_LINES);
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_setwin...");
+}
+
+static void ccdc_readregs(void)
+{
+	unsigned int val = 0;
+
+	val = regr(CCDC_ALAW);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to ALAW...\n", val);
+	val = regr(CCDC_CLAMP);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to CLAMP...\n", val);
+	val = regr(CCDC_DCSUB);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to DCSUB...\n", val);
+	val = regr(CCDC_BLKCMP);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to BLKCMP...\n", val);
+	val = regr(CCDC_FPC_ADDR);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FPC_ADDR...\n", val);
+	val = regr(CCDC_FPC);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FPC...\n", val);
+	val = regr(CCDC_FMTCFG);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMTCFG...\n", val);
+	val = regr(CCDC_COLPTN);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to COLPTN...\n", val);
+	val = regr(CCDC_FMT_HORZ);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMT_HORZ...\n", val);
+	val = regr(CCDC_FMT_VERT);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMT_VERT...\n", val);
+	val = regr(CCDC_HSIZE_OFF);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to HSIZE_OFF...\n", val);
+	val = regr(CCDC_SDOFST);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to SDOFST...\n", val);
+	val = regr(CCDC_VP_OUT);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VP_OUT...\n", val);
+	val = regr(CCDC_SYN_MODE);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to SYN_MODE...\n", val);
+	val = regr(CCDC_HORZ_INFO);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to HORZ_INFO...\n", val);
+	val = regr(CCDC_VERT_START);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VERT_START...\n", val);
+	val = regr(CCDC_VERT_LINES);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VERT_LINES...\n", val);
+}
+
+static int ccdc_close(struct device *dev)
+{
+	return 0;
+}
+
+/*
+ * ccdc_restore_defaults()
+ * This function will write defaults to all CCDC registers
+ */
+static void ccdc_restore_defaults(void)
+{
+	int i;
+
+	/* disable CCDC */
+	ccdc_enable(0);
+	/* set all registers to default value */
+	for (i = 4; i <= 0x94; i += 4)
+		regw(0,  i);
+	regw(CCDC_NO_CULLING, CCDC_CULLING);
+	regw(CCDC_GAMMA_BITS_11_2, CCDC_ALAW);
+}
+
+static int ccdc_open(struct device *device)
+{
+	ccdc_restore_defaults();
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_enable_vport(1);
+	return 0;
+}
+
+static void ccdc_sbl_reset(void)
+{
+	vpss_clear_wbl_overflow(VPSS_PCR_CCDC_WBL_O);
+}
+
+/*
+ * ccdc_config_ycbcr()
+ * This function will configure CCDC for YCbCr video capture
+ */
+static void ccdc_config_ycbcr(void)
+{
+	struct ccdc_params_ycbcr *params = &ccdc_cfg.ycbcr;
+	u32 syn_mode;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_ycbcr...");
+	/*
+	 * first restore the CCDC registers to default values
+	 * This is important since we assume default values to be set in
+	 * a lot of registers that we didn't touch
+	 */
+	ccdc_restore_defaults();
+
+	/*
+	 * configure pixel format, frame format, configure video frame
+	 * format, enable output to SDRAM, enable internal timing generator
+	 * and 8bit pack mode
+	 */
+	syn_mode = (((params->pix_fmt & CCDC_SYN_MODE_INPMOD_MASK) <<
+		    CCDC_SYN_MODE_INPMOD_SHIFT) |
+		    ((params->frm_fmt & CCDC_SYN_FLDMODE_MASK) <<
+		    CCDC_SYN_FLDMODE_SHIFT) | CCDC_VDHDEN_ENABLE |
+		    CCDC_WEN_ENABLE | CCDC_DATA_PACK_ENABLE);
+
+	/* setup BT.656 sync mode */
+	if (params->bt656_enable) {
+		regw(CCDC_REC656IF_BT656_EN, CCDC_REC656IF);
+
+		/*
+		 * configure the FID, VD, HD pin polarity,
+		 * fld,hd pol positive, vd negative, 8-bit data
+		 */
+		syn_mode |= CCDC_SYN_MODE_VD_POL_NEGATIVE;
+		if (ccdc_cfg.if_type == VPFE_BT656_10BIT)
+			syn_mode |= CCDC_SYN_MODE_10BITS;
+		else
+			syn_mode |= CCDC_SYN_MODE_8BITS;
+	} else {
+		/* y/c external sync mode */
+		syn_mode |= (((params->fid_pol & CCDC_FID_POL_MASK) <<
+			     CCDC_FID_POL_SHIFT) |
+			     ((params->hd_pol & CCDC_HD_POL_MASK) <<
+			     CCDC_HD_POL_SHIFT) |
+			     ((params->vd_pol & CCDC_VD_POL_MASK) <<
+			     CCDC_VD_POL_SHIFT));
+	}
+	regw(syn_mode, CCDC_SYN_MODE);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 2);
+
+	/*
+	 * configure the order of y cb cr in SDRAM, and disable latch
+	 * internal register on vsync
+	 */
+	if (ccdc_cfg.if_type == VPFE_BT656_10BIT)
+		regw((params->pix_order << CCDC_CCDCFG_Y8POS_SHIFT) |
+			CCDC_LATCH_ON_VSYNC_DISABLE | CCDC_CCDCFG_BW656_10BIT,
+			CCDC_CCDCFG);
+	else
+		regw((params->pix_order << CCDC_CCDCFG_Y8POS_SHIFT) |
+			CCDC_LATCH_ON_VSYNC_DISABLE, CCDC_CCDCFG);
+
+	/*
+	 * configure the horizontal line offset. This should be a
+	 * on 32 byte boundary. So clear LSB 5 bits
+	 */
+	regw(((params->win.width * 2  + 31) & ~0x1f), CCDC_HSIZE_OFF);
+
+	/* configure the memory line offset */
+	if (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED)
+		/* two fields are interleaved in memory */
+		regw(CCDC_SDOFST_FIELD_INTERLEAVED, CCDC_SDOFST);
+
+	ccdc_sbl_reset();
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_config_ycbcr...\n");
+}
+
+static void ccdc_config_black_clamp(struct ccdc_black_clamp *bclamp)
+{
+	u32 val;
+
+	if (!bclamp->enable) {
+		/* configure DCSub */
+		val = (bclamp->dc_sub) & CCDC_BLK_DC_SUB_MASK;
+		regw(val, CCDC_DCSUB);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to DCSUB...\n", val);
+		regw(CCDC_CLAMP_DEFAULT_VAL, CCDC_CLAMP);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x0000 to CLAMP...\n");
+		return;
+	}
+	/*
+	 * Configure gain,  Start pixel, No of line to be avg,
+	 * No of pixel/line to be avg, & Enable the Black clamping
+	 */
+	val = ((bclamp->sgain & CCDC_BLK_SGAIN_MASK) |
+	       ((bclamp->start_pixel & CCDC_BLK_ST_PXL_MASK) <<
+		CCDC_BLK_ST_PXL_SHIFT) |
+	       ((bclamp->sample_ln & CCDC_BLK_SAMPLE_LINE_MASK) <<
+		CCDC_BLK_SAMPLE_LINE_SHIFT) |
+	       ((bclamp->sample_pixel & CCDC_BLK_SAMPLE_LN_MASK) <<
+		CCDC_BLK_SAMPLE_LN_SHIFT) | CCDC_BLK_CLAMP_ENABLE);
+	regw(val, CCDC_CLAMP);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to CLAMP...\n", val);
+	/* If Black clamping is enable then make dcsub 0 */
+	regw(CCDC_DCSUB_DEFAULT_VAL, CCDC_DCSUB);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x00000000 to DCSUB...\n");
+}
+
+static void ccdc_config_black_compense(struct ccdc_black_compensation *bcomp)
+{
+	u32 val;
+
+	val = ((bcomp->b & CCDC_BLK_COMP_MASK) |
+	      ((bcomp->gb & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_GB_COMP_SHIFT) |
+	      ((bcomp->gr & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_GR_COMP_SHIFT) |
+	      ((bcomp->r & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_R_COMP_SHIFT));
+	regw(val, CCDC_BLKCMP);
+}
+
+/*
+ * ccdc_config_raw()
+ * This function will configure CCDC for Raw capture mode
+ */
+static void ccdc_config_raw(void)
+{
+	struct ccdc_params_raw *params = &ccdc_cfg.bayer;
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int syn_mode = 0;
+	unsigned int val;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_raw...");
+
+	/*      Reset CCDC */
+	ccdc_restore_defaults();
+
+	/* Disable latching function registers on VSYNC  */
+	regw(CCDC_LATCH_ON_VSYNC_DISABLE, CCDC_CCDCFG);
+
+	/*
+	 * Configure the vertical sync polarity(SYN_MODE.VDPOL),
+	 * horizontal sync polarity (SYN_MODE.HDPOL), frame id polarity
+	 * (SYN_MODE.FLDPOL), frame format(progressive or interlace),
+	 * data size(SYNMODE.DATSIZ), &pixel format (Input mode), output
+	 * SDRAM, enable internal timing generator
+	 */
+	syn_mode =
+		(((params->vd_pol & CCDC_VD_POL_MASK) << CCDC_VD_POL_SHIFT) |
+		((params->hd_pol & CCDC_HD_POL_MASK) << CCDC_HD_POL_SHIFT) |
+		((params->fid_pol & CCDC_FID_POL_MASK) << CCDC_FID_POL_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) << CCDC_FRM_FMT_SHIFT) |
+		((config_params->data_sz & CCDC_DATA_SZ_MASK) <<
+		CCDC_DATA_SZ_SHIFT) |
+		((params->pix_fmt & CCDC_PIX_FMT_MASK) << CCDC_PIX_FMT_SHIFT) |
+		CCDC_WEN_ENABLE | CCDC_VDHDEN_ENABLE);
+
+	/* Enable and configure aLaw register if needed */
+	if (config_params->alaw.enable) {
+		val = ((config_params->alaw.gamma_wd &
+		      CCDC_ALAW_GAMMA_WD_MASK) | CCDC_ALAW_ENABLE);
+		regw(val, CCDC_ALAW);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to ALAW...\n", val);
+	}
+
+	/* Configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, CCDC_PPC_RAW);
+
+	/* Configure Black Clamp */
+	ccdc_config_black_clamp(&config_params->blk_clamp);
+
+	/* Configure Black level compensation */
+	ccdc_config_black_compense(&config_params->blk_comp);
+
+	/* If data size is 8 bit then pack the data */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable)
+		syn_mode |= CCDC_DATA_PACK_ENABLE;
+
+	/* disable video port */
+	val = CCDC_DISABLE_VIDEO_PORT;
+
+	if (config_params->data_sz == CCDC_DATA_8BITS)
+		val |= (CCDC_DATA_10BITS & CCDC_FMTCFG_VPIN_MASK)
+		    << CCDC_FMTCFG_VPIN_SHIFT;
+	else
+		val |= (config_params->data_sz & CCDC_FMTCFG_VPIN_MASK)
+		    << CCDC_FMTCFG_VPIN_SHIFT;
+	/* Write value in FMTCFG */
+	regw(val, CCDC_FMTCFG);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMTCFG...\n", val);
+	/* Configure the color pattern according to mt9t001 sensor */
+	regw(CCDC_COLPTN_VAL, CCDC_COLPTN);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0xBB11BB11 to COLPTN...\n");
+	/*
+	 * Configure Data formatter(Video port) pixel selection
+	 * (FMT_HORZ, FMT_VERT)
+	 */
+	val = ((params->win.left & CCDC_FMT_HORZ_FMTSPH_MASK) <<
+	      CCDC_FMT_HORZ_FMTSPH_SHIFT) |
+	      (params->win.width & CCDC_FMT_HORZ_FMTLNH_MASK);
+	regw(val, CCDC_FMT_HORZ);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMT_HORZ...\n", val);
+	val = (params->win.top & CCDC_FMT_VERT_FMTSLV_MASK)
+	    << CCDC_FMT_VERT_FMTSLV_SHIFT;
+	if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE)
+		val |= (params->win.height) & CCDC_FMT_VERT_FMTLNV_MASK;
+	else
+		val |= (params->win.height >> 1) & CCDC_FMT_VERT_FMTLNV_MASK;
+
+	dev_dbg(ccdc_cfg.dev, "\nparams->win.height  0x%x ...\n",
+	       params->win.height);
+	regw(val, CCDC_FMT_VERT);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMT_VERT...\n", val);
+
+	dev_dbg(ccdc_cfg.dev, "\nbelow regw(val, FMT_VERT)...");
+
+	/*
+	 * Configure Horizontal offset register. If pack 8 is enabled then
+	 * 1 pixel will take 1 byte
+	 */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	    config_params->alaw.enable)
+		regw((params->win.width + CCDC_32BYTE_ALIGN_VAL) &
+		    CCDC_HSIZE_OFF_MASK, CCDC_HSIZE_OFF);
+	else
+		/* else one pixel will take 2 byte */
+		regw(((params->win.width * CCDC_TWO_BYTES_PER_PIXEL) +
+		    CCDC_32BYTE_ALIGN_VAL) & CCDC_HSIZE_OFF_MASK,
+		    CCDC_HSIZE_OFF);
+
+	/* Set value for SDOFST */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_enable) {
+			/* For intelace inverse mode */
+			regw(CCDC_INTERLACED_IMAGE_INVERT, CCDC_SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting 0x4B6D to SDOFST..\n");
+		}
+
+		else {
+			/* For intelace non inverse mode */
+			regw(CCDC_INTERLACED_NO_IMAGE_INVERT, CCDC_SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting 0x0249 to SDOFST..\n");
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		regw(CCDC_PROGRESSIVE_NO_IMAGE_INVERT, CCDC_SDOFST);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x0000 to SDOFST...\n");
+	}
+
+	/*
+	 * Configure video port pixel selection (VPOUT)
+	 * Here -1 is to make the height value less than FMT_VERT.FMTLNV
+	 */
+	if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE)
+		val = (((params->win.height - 1) & CCDC_VP_OUT_VERT_NUM_MASK))
+		    << CCDC_VP_OUT_VERT_NUM_SHIFT;
+	else
+		val =
+		    ((((params->win.height >> CCDC_INTERLACED_HEIGHT_SHIFT) -
+		     1) & CCDC_VP_OUT_VERT_NUM_MASK)) <<
+		    CCDC_VP_OUT_VERT_NUM_SHIFT;
+
+	val |= ((((params->win.width))) & CCDC_VP_OUT_HORZ_NUM_MASK)
+	    << CCDC_VP_OUT_HORZ_NUM_SHIFT;
+	val |= (params->win.left) & CCDC_VP_OUT_HORZ_ST_MASK;
+	regw(val, CCDC_VP_OUT);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to VP_OUT...\n", val);
+	regw(syn_mode, CCDC_SYN_MODE);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to SYN_MODE...\n", syn_mode);
+
+	ccdc_sbl_reset();
+	dev_dbg(ccdc_cfg.dev, "\nend of ccdc_config_raw...");
+	ccdc_readregs();
+}
+
+static int ccdc_configure(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_config_raw();
+	else
+		ccdc_config_ycbcr();
+	return 0;
+}
+
+static int ccdc_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.buf_type = buf_type;
+	else
+		ccdc_cfg.ycbcr.buf_type = buf_type;
+	return 0;
+}
+
+static enum ccdc_buftype ccdc_get_buftype(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.buf_type;
+	return ccdc_cfg.ycbcr.buf_type;
+}
+
+static int ccdc_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(ccdc_raw_bayer_pix_formats)) {
+			*pix = ccdc_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(ccdc_raw_yuv_pix_formats)) {
+			*pix = ccdc_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+	return ret;
+}
+
+static int ccdc_set_pixel_format(u32 pixfmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8)
+			ccdc_cfg.bayer.config_params.alaw.enable = 1;
+		else if (pixfmt != V4L2_PIX_FMT_SBGGR16)
+			return -EINVAL;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static u32 ccdc_get_pixel_format(void)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+	u32 pixfmt;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		if (alaw->enable)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (ccdc_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+
+static int ccdc_set_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.win = *win;
+	else
+		ccdc_cfg.ycbcr.win = *win;
+	return 0;
+}
+
+static void ccdc_get_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		*win = ccdc_cfg.bayer.win;
+	else
+		*win = ccdc_cfg.ycbcr.win;
+}
+
+static unsigned int ccdc_get_line_length(void)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int len;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if ((config_params->alaw.enable) ||
+		    (config_params->data_sz == CCDC_DATA_8BITS))
+			len = ccdc_cfg.bayer.win.width;
+		else
+			len = ccdc_cfg.bayer.win.width * 2;
+	} else
+		len = ccdc_cfg.ycbcr.win.width * 2;
+	return ALIGN(len, 32);
+}
+
+static int ccdc_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		ccdc_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+
+static enum ccdc_frmfmt ccdc_get_frame_format(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.frm_fmt;
+	else
+		return ccdc_cfg.ycbcr.frm_fmt;
+}
+
+static int ccdc_getfid(void)
+{
+	return (regr(CCDC_SYN_MODE) >> 15) & 1;
+}
+
+/* misc operations */
+static inline void ccdc_setfbaddr(unsigned long addr)
+{
+	regw(addr & 0xffffffe0, CCDC_SDR_ADDR);
+}
+
+static int ccdc_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	ccdc_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_YCBCR_SYNC_16:
+	case VPFE_YCBCR_SYNC_8:
+	case VPFE_BT656_10BIT:
+		ccdc_cfg.ycbcr.vd_pol = params->vdpol;
+		ccdc_cfg.ycbcr.hd_pol = params->hdpol;
+		break;
+	default:
+		/* TODO add support for raw bayer here */
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void ccdc_save_context(void)
+{
+	ccdc_ctx[CCDC_PCR >> 2] = regr(CCDC_PCR);
+	ccdc_ctx[CCDC_SYN_MODE >> 2] = regr(CCDC_SYN_MODE);
+	ccdc_ctx[CCDC_HD_VD_WID >> 2] = regr(CCDC_HD_VD_WID);
+	ccdc_ctx[CCDC_PIX_LINES >> 2] = regr(CCDC_PIX_LINES);
+	ccdc_ctx[CCDC_HORZ_INFO >> 2] = regr(CCDC_HORZ_INFO);
+	ccdc_ctx[CCDC_VERT_START >> 2] = regr(CCDC_VERT_START);
+	ccdc_ctx[CCDC_VERT_LINES >> 2] = regr(CCDC_VERT_LINES);
+	ccdc_ctx[CCDC_CULLING >> 2] = regr(CCDC_CULLING);
+	ccdc_ctx[CCDC_HSIZE_OFF >> 2] = regr(CCDC_HSIZE_OFF);
+	ccdc_ctx[CCDC_SDOFST >> 2] = regr(CCDC_SDOFST);
+	ccdc_ctx[CCDC_SDR_ADDR >> 2] = regr(CCDC_SDR_ADDR);
+	ccdc_ctx[CCDC_CLAMP >> 2] = regr(CCDC_CLAMP);
+	ccdc_ctx[CCDC_DCSUB >> 2] = regr(CCDC_DCSUB);
+	ccdc_ctx[CCDC_COLPTN >> 2] = regr(CCDC_COLPTN);
+	ccdc_ctx[CCDC_BLKCMP >> 2] = regr(CCDC_BLKCMP);
+	ccdc_ctx[CCDC_FPC >> 2] = regr(CCDC_FPC);
+	ccdc_ctx[CCDC_FPC_ADDR >> 2] = regr(CCDC_FPC_ADDR);
+	ccdc_ctx[CCDC_VDINT >> 2] = regr(CCDC_VDINT);
+	ccdc_ctx[CCDC_ALAW >> 2] = regr(CCDC_ALAW);
+	ccdc_ctx[CCDC_REC656IF >> 2] = regr(CCDC_REC656IF);
+	ccdc_ctx[CCDC_CCDCFG >> 2] = regr(CCDC_CCDCFG);
+	ccdc_ctx[CCDC_FMTCFG >> 2] = regr(CCDC_FMTCFG);
+	ccdc_ctx[CCDC_FMT_HORZ >> 2] = regr(CCDC_FMT_HORZ);
+	ccdc_ctx[CCDC_FMT_VERT >> 2] = regr(CCDC_FMT_VERT);
+	ccdc_ctx[CCDC_FMT_ADDR0 >> 2] = regr(CCDC_FMT_ADDR0);
+	ccdc_ctx[CCDC_FMT_ADDR1 >> 2] = regr(CCDC_FMT_ADDR1);
+	ccdc_ctx[CCDC_FMT_ADDR2 >> 2] = regr(CCDC_FMT_ADDR2);
+	ccdc_ctx[CCDC_FMT_ADDR3 >> 2] = regr(CCDC_FMT_ADDR3);
+	ccdc_ctx[CCDC_FMT_ADDR4 >> 2] = regr(CCDC_FMT_ADDR4);
+	ccdc_ctx[CCDC_FMT_ADDR5 >> 2] = regr(CCDC_FMT_ADDR5);
+	ccdc_ctx[CCDC_FMT_ADDR6 >> 2] = regr(CCDC_FMT_ADDR6);
+	ccdc_ctx[CCDC_FMT_ADDR7 >> 2] = regr(CCDC_FMT_ADDR7);
+	ccdc_ctx[CCDC_PRGEVEN_0 >> 2] = regr(CCDC_PRGEVEN_0);
+	ccdc_ctx[CCDC_PRGEVEN_1 >> 2] = regr(CCDC_PRGEVEN_1);
+	ccdc_ctx[CCDC_PRGODD_0 >> 2] = regr(CCDC_PRGODD_0);
+	ccdc_ctx[CCDC_PRGODD_1 >> 2] = regr(CCDC_PRGODD_1);
+	ccdc_ctx[CCDC_VP_OUT >> 2] = regr(CCDC_VP_OUT);
+}
+
+static void ccdc_restore_context(void)
+{
+	regw(ccdc_ctx[CCDC_SYN_MODE >> 2], CCDC_SYN_MODE);
+	regw(ccdc_ctx[CCDC_HD_VD_WID >> 2], CCDC_HD_VD_WID);
+	regw(ccdc_ctx[CCDC_PIX_LINES >> 2], CCDC_PIX_LINES);
+	regw(ccdc_ctx[CCDC_HORZ_INFO >> 2], CCDC_HORZ_INFO);
+	regw(ccdc_ctx[CCDC_VERT_START >> 2], CCDC_VERT_START);
+	regw(ccdc_ctx[CCDC_VERT_LINES >> 2], CCDC_VERT_LINES);
+	regw(ccdc_ctx[CCDC_CULLING >> 2], CCDC_CULLING);
+	regw(ccdc_ctx[CCDC_HSIZE_OFF >> 2], CCDC_HSIZE_OFF);
+	regw(ccdc_ctx[CCDC_SDOFST >> 2], CCDC_SDOFST);
+	regw(ccdc_ctx[CCDC_SDR_ADDR >> 2], CCDC_SDR_ADDR);
+	regw(ccdc_ctx[CCDC_CLAMP >> 2], CCDC_CLAMP);
+	regw(ccdc_ctx[CCDC_DCSUB >> 2], CCDC_DCSUB);
+	regw(ccdc_ctx[CCDC_COLPTN >> 2], CCDC_COLPTN);
+	regw(ccdc_ctx[CCDC_BLKCMP >> 2], CCDC_BLKCMP);
+	regw(ccdc_ctx[CCDC_FPC >> 2], CCDC_FPC);
+	regw(ccdc_ctx[CCDC_FPC_ADDR >> 2], CCDC_FPC_ADDR);
+	regw(ccdc_ctx[CCDC_VDINT >> 2], CCDC_VDINT);
+	regw(ccdc_ctx[CCDC_ALAW >> 2], CCDC_ALAW);
+	regw(ccdc_ctx[CCDC_REC656IF >> 2], CCDC_REC656IF);
+	regw(ccdc_ctx[CCDC_CCDCFG >> 2], CCDC_CCDCFG);
+	regw(ccdc_ctx[CCDC_FMTCFG >> 2], CCDC_FMTCFG);
+	regw(ccdc_ctx[CCDC_FMT_HORZ >> 2], CCDC_FMT_HORZ);
+	regw(ccdc_ctx[CCDC_FMT_VERT >> 2], CCDC_FMT_VERT);
+	regw(ccdc_ctx[CCDC_FMT_ADDR0 >> 2], CCDC_FMT_ADDR0);
+	regw(ccdc_ctx[CCDC_FMT_ADDR1 >> 2], CCDC_FMT_ADDR1);
+	regw(ccdc_ctx[CCDC_FMT_ADDR2 >> 2], CCDC_FMT_ADDR2);
+	regw(ccdc_ctx[CCDC_FMT_ADDR3 >> 2], CCDC_FMT_ADDR3);
+	regw(ccdc_ctx[CCDC_FMT_ADDR4 >> 2], CCDC_FMT_ADDR4);
+	regw(ccdc_ctx[CCDC_FMT_ADDR5 >> 2], CCDC_FMT_ADDR5);
+	regw(ccdc_ctx[CCDC_FMT_ADDR6 >> 2], CCDC_FMT_ADDR6);
+	regw(ccdc_ctx[CCDC_FMT_ADDR7 >> 2], CCDC_FMT_ADDR7);
+	regw(ccdc_ctx[CCDC_PRGEVEN_0 >> 2], CCDC_PRGEVEN_0);
+	regw(ccdc_ctx[CCDC_PRGEVEN_1 >> 2], CCDC_PRGEVEN_1);
+	regw(ccdc_ctx[CCDC_PRGODD_0 >> 2], CCDC_PRGODD_0);
+	regw(ccdc_ctx[CCDC_PRGODD_1 >> 2], CCDC_PRGODD_1);
+	regw(ccdc_ctx[CCDC_VP_OUT >> 2], CCDC_VP_OUT);
+	regw(ccdc_ctx[CCDC_PCR >> 2], CCDC_PCR);
+}
+static const struct ccdc_hw_device ccdc_hw_dev = {
+	.name = "DM6446 CCDC",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = ccdc_open,
+		.close = ccdc_close,
+		.reset = ccdc_sbl_reset,
+		.enable = ccdc_enable,
+		.set_hw_if_params = ccdc_set_hw_if_params,
+		.configure = ccdc_configure,
+		.set_buftype = ccdc_set_buftype,
+		.get_buftype = ccdc_get_buftype,
+		.enum_pix = ccdc_enum_pix,
+		.set_pixel_format = ccdc_set_pixel_format,
+		.get_pixel_format = ccdc_get_pixel_format,
+		.set_frame_format = ccdc_set_frame_format,
+		.get_frame_format = ccdc_get_frame_format,
+		.set_image_window = ccdc_set_image_window,
+		.get_image_window = ccdc_get_image_window,
+		.get_line_length = ccdc_get_line_length,
+		.setfbaddr = ccdc_setfbaddr,
+		.getfid = ccdc_getfid,
+	},
+};
+
+static int dm644x_ccdc_probe(struct platform_device *pdev)
+{
+	struct resource	*res;
+	int status = 0;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&ccdc_hw_dev);
+	if (status < 0)
+		return status;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		status = -ENODEV;
+		goto fail_nores;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), res->name);
+	if (!res) {
+		status = -EBUSY;
+		goto fail_nores;
+	}
+
+	ccdc_cfg.base_addr = ioremap(res->start, resource_size(res));
+	if (!ccdc_cfg.base_addr) {
+		status = -ENOMEM;
+		goto fail_nomem;
+	}
+
+	ccdc_cfg.dev = &pdev->dev;
+	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
+	return 0;
+fail_nomem:
+	release_mem_region(res->start, resource_size(res));
+fail_nores:
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return status;
+}
+
+static int dm644x_ccdc_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+
+	iounmap(ccdc_cfg.base_addr);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, resource_size(res));
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return 0;
+}
+
+static int dm644x_ccdc_suspend(struct device *dev)
+{
+	/* Save CCDC context */
+	ccdc_save_context();
+	/* Disable CCDC */
+	ccdc_enable(0);
+
+	return 0;
+}
+
+static int dm644x_ccdc_resume(struct device *dev)
+{
+	/* Restore CCDC context */
+	ccdc_restore_context();
+
+	return 0;
+}
+
+static const struct dev_pm_ops dm644x_ccdc_pm_ops = {
+	.suspend = dm644x_ccdc_suspend,
+	.resume = dm644x_ccdc_resume,
+};
+
+static struct platform_driver dm644x_ccdc_driver = {
+	.driver = {
+		.name	= "dm644x_ccdc",
+		.pm = &dm644x_ccdc_pm_ops,
+	},
+	.remove = dm644x_ccdc_remove,
+	.probe = dm644x_ccdc_probe,
+};
+
+module_platform_driver(dm644x_ccdc_driver);
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.h b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.h
new file mode 100644
index 000000000..c20dba3d7
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc.h
@@ -0,0 +1,171 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2006-2009 Texas Instruments Inc
+ */
+#ifndef _DM644X_CCDC_H
+#define _DM644X_CCDC_H
+#include <media/davinci/ccdc_types.h>
+#include <media/davinci/vpfe_types.h>
+
+/* enum for No of pixel per line to be avg. in Black Clamping*/
+enum ccdc_sample_length {
+	CCDC_SAMPLE_1PIXELS,
+	CCDC_SAMPLE_2PIXELS,
+	CCDC_SAMPLE_4PIXELS,
+	CCDC_SAMPLE_8PIXELS,
+	CCDC_SAMPLE_16PIXELS
+};
+
+/* enum for No of lines in Black Clamping */
+enum ccdc_sample_line {
+	CCDC_SAMPLE_1LINES,
+	CCDC_SAMPLE_2LINES,
+	CCDC_SAMPLE_4LINES,
+	CCDC_SAMPLE_8LINES,
+	CCDC_SAMPLE_16LINES
+};
+
+/* enum for Alaw gamma width */
+enum ccdc_gamma_width {
+	CCDC_GAMMA_BITS_15_6,	/* use bits 15-6 for gamma */
+	CCDC_GAMMA_BITS_14_5,
+	CCDC_GAMMA_BITS_13_4,
+	CCDC_GAMMA_BITS_12_3,
+	CCDC_GAMMA_BITS_11_2,
+	CCDC_GAMMA_BITS_10_1,
+	CCDC_GAMMA_BITS_09_0	/* use bits 9-0 for gamma */
+};
+
+/* returns the highest bit used for the gamma */
+static inline u8 ccdc_gamma_width_max_bit(enum ccdc_gamma_width width)
+{
+	return 15 - width;
+}
+
+enum ccdc_data_size {
+	CCDC_DATA_16BITS,
+	CCDC_DATA_15BITS,
+	CCDC_DATA_14BITS,
+	CCDC_DATA_13BITS,
+	CCDC_DATA_12BITS,
+	CCDC_DATA_11BITS,
+	CCDC_DATA_10BITS,
+	CCDC_DATA_8BITS
+};
+
+/* returns the highest bit used for this data size */
+static inline u8 ccdc_data_size_max_bit(enum ccdc_data_size sz)
+{
+	return sz == CCDC_DATA_8BITS ? 7 : 15 - sz;
+}
+
+/* structure for ALaw */
+struct ccdc_a_law {
+	/* Enable/disable A-Law */
+	unsigned char enable;
+	/* Gamma Width Input */
+	enum ccdc_gamma_width gamma_wd;
+};
+
+/* structure for Black Clamping */
+struct ccdc_black_clamp {
+	unsigned char enable;
+	/* only if bClampEnable is TRUE */
+	enum ccdc_sample_length sample_pixel;
+	/* only if bClampEnable is TRUE */
+	enum ccdc_sample_line sample_ln;
+	/* only if bClampEnable is TRUE */
+	unsigned short start_pixel;
+	/* only if bClampEnable is TRUE */
+	unsigned short sgain;
+	/* only if bClampEnable is FALSE */
+	unsigned short dc_sub;
+};
+
+/* structure for Black Level Compensation */
+struct ccdc_black_compensation {
+	/* Constant value to subtract from Red component */
+	char r;
+	/* Constant value to subtract from Gr component */
+	char gr;
+	/* Constant value to subtract from Blue component */
+	char b;
+	/* Constant value to subtract from Gb component */
+	char gb;
+};
+
+/* Structure for CCDC configuration parameters for raw capture mode passed
+ * by application
+ */
+struct ccdc_config_params_raw {
+	/* data size value from 8 to 16 bits */
+	enum ccdc_data_size data_sz;
+	/* Structure for Optional A-Law */
+	struct ccdc_a_law alaw;
+	/* Structure for Optical Black Clamp */
+	struct ccdc_black_clamp blk_clamp;
+	/* Structure for Black Compensation */
+	struct ccdc_black_compensation blk_comp;
+};
+
+
+#ifdef __KERNEL__
+#include <linux/io.h>
+/* Define to enable/disable video port */
+#define FP_NUM_BYTES		4
+/* Define for extra pixel/line and extra lines/frame */
+#define NUM_EXTRAPIXELS		8
+#define NUM_EXTRALINES		8
+
+/* settings for commonly used video formats */
+#define CCDC_WIN_PAL     {0, 0, 720, 576}
+/* ntsc square pixel */
+#define CCDC_WIN_VGA	{0, 0, (640 + NUM_EXTRAPIXELS), (480 + NUM_EXTRALINES)}
+
+/* Structure for CCDC configuration parameters for raw capture mode */
+struct ccdc_params_raw {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	/* video window */
+	struct v4l2_rect win;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* interleaved or separated fields */
+	enum ccdc_buftype buf_type;
+	/*
+	 * enable to store the image in inverse
+	 * order in memory(bottom to top)
+	 */
+	unsigned char image_invert_enable;
+	/* configurable parameters */
+	struct ccdc_config_params_raw config_params;
+};
+
+struct ccdc_params_ycbcr {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	/* video window */
+	struct v4l2_rect win;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* enable BT.656 embedded sync mode */
+	int bt656_enable;
+	/* cb:y:cr:y or y:cb:y:cr in memory */
+	enum ccdc_pixorder pix_order;
+	/* interleaved or separated fields  */
+	enum ccdc_buftype buf_type;
+};
+#endif
+#endif				/* _DM644X_CCDC_H */
diff --git a/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc_regs.h b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc_regs.h
new file mode 100644
index 000000000..c4894f6a2
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/dm644x_ccdc_regs.h
@@ -0,0 +1,140 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2006-2009 Texas Instruments Inc
+ */
+#ifndef _DM644X_CCDC_REGS_H
+#define _DM644X_CCDC_REGS_H
+
+/**************************************************************************\
+* Register OFFSET Definitions
+\**************************************************************************/
+#define CCDC_PID				0x0
+#define CCDC_PCR				0x4
+#define CCDC_SYN_MODE				0x8
+#define CCDC_HD_VD_WID				0xc
+#define CCDC_PIX_LINES				0x10
+#define CCDC_HORZ_INFO				0x14
+#define CCDC_VERT_START				0x18
+#define CCDC_VERT_LINES				0x1c
+#define CCDC_CULLING				0x20
+#define CCDC_HSIZE_OFF				0x24
+#define CCDC_SDOFST				0x28
+#define CCDC_SDR_ADDR				0x2c
+#define CCDC_CLAMP				0x30
+#define CCDC_DCSUB				0x34
+#define CCDC_COLPTN				0x38
+#define CCDC_BLKCMP				0x3c
+#define CCDC_FPC				0x40
+#define CCDC_FPC_ADDR				0x44
+#define CCDC_VDINT				0x48
+#define CCDC_ALAW				0x4c
+#define CCDC_REC656IF				0x50
+#define CCDC_CCDCFG				0x54
+#define CCDC_FMTCFG				0x58
+#define CCDC_FMT_HORZ				0x5c
+#define CCDC_FMT_VERT				0x60
+#define CCDC_FMT_ADDR0				0x64
+#define CCDC_FMT_ADDR1				0x68
+#define CCDC_FMT_ADDR2				0x6c
+#define CCDC_FMT_ADDR3				0x70
+#define CCDC_FMT_ADDR4				0x74
+#define CCDC_FMT_ADDR5				0x78
+#define CCDC_FMT_ADDR6				0x7c
+#define CCDC_FMT_ADDR7				0x80
+#define CCDC_PRGEVEN_0				0x84
+#define CCDC_PRGEVEN_1				0x88
+#define CCDC_PRGODD_0				0x8c
+#define CCDC_PRGODD_1				0x90
+#define CCDC_VP_OUT				0x94
+#define CCDC_REG_END				0x98
+
+/***************************************************************
+*	Define for various register bit mask and shifts for CCDC
+****************************************************************/
+#define CCDC_FID_POL_MASK			1
+#define CCDC_FID_POL_SHIFT			4
+#define CCDC_HD_POL_MASK			1
+#define CCDC_HD_POL_SHIFT			3
+#define CCDC_VD_POL_MASK			1
+#define CCDC_VD_POL_SHIFT			2
+#define CCDC_HSIZE_OFF_MASK			0xffffffe0
+#define CCDC_32BYTE_ALIGN_VAL			31
+#define CCDC_FRM_FMT_MASK			0x1
+#define CCDC_FRM_FMT_SHIFT			7
+#define CCDC_DATA_SZ_MASK			7
+#define CCDC_DATA_SZ_SHIFT			8
+#define CCDC_PIX_FMT_MASK			3
+#define CCDC_PIX_FMT_SHIFT			12
+#define CCDC_VP2SDR_DISABLE			0xFFFBFFFF
+#define CCDC_WEN_ENABLE				BIT(17)
+#define CCDC_SDR2RSZ_DISABLE			0xFFF7FFFF
+#define CCDC_VDHDEN_ENABLE			BIT(16)
+#define CCDC_LPF_ENABLE				BIT(14)
+#define CCDC_ALAW_ENABLE			BIT(3)
+#define CCDC_ALAW_GAMMA_WD_MASK			7
+#define CCDC_BLK_CLAMP_ENABLE			BIT(31)
+#define CCDC_BLK_SGAIN_MASK			0x1F
+#define CCDC_BLK_ST_PXL_MASK			0x7FFF
+#define CCDC_BLK_ST_PXL_SHIFT			10
+#define CCDC_BLK_SAMPLE_LN_MASK			7
+#define CCDC_BLK_SAMPLE_LN_SHIFT		28
+#define CCDC_BLK_SAMPLE_LINE_MASK		7
+#define CCDC_BLK_SAMPLE_LINE_SHIFT		25
+#define CCDC_BLK_DC_SUB_MASK			0x03FFF
+#define CCDC_BLK_COMP_MASK			0xFF
+#define CCDC_BLK_COMP_GB_COMP_SHIFT		8
+#define CCDC_BLK_COMP_GR_COMP_SHIFT		16
+#define CCDC_BLK_COMP_R_COMP_SHIFT		24
+#define CCDC_LATCH_ON_VSYNC_DISABLE		BIT(15)
+#define CCDC_FPC_ENABLE				BIT(15)
+#define CCDC_FPC_DISABLE			0
+#define CCDC_FPC_FPC_NUM_MASK			0x7FFF
+#define CCDC_DATA_PACK_ENABLE			BIT(11)
+#define CCDC_FMTCFG_VPIN_MASK			7
+#define CCDC_FMTCFG_VPIN_SHIFT			12
+#define CCDC_FMT_HORZ_FMTLNH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_SHIFT		16
+#define CCDC_FMT_VERT_FMTLNV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_SHIFT		16
+#define CCDC_VP_OUT_VERT_NUM_MASK		0x3FFF
+#define CCDC_VP_OUT_VERT_NUM_SHIFT		17
+#define CCDC_VP_OUT_HORZ_NUM_MASK		0x1FFF
+#define CCDC_VP_OUT_HORZ_NUM_SHIFT		4
+#define CCDC_VP_OUT_HORZ_ST_MASK		0xF
+#define CCDC_HORZ_INFO_SPH_SHIFT		16
+#define CCDC_VERT_START_SLV0_SHIFT		16
+#define CCDC_VDINT_VDINT0_SHIFT			16
+#define CCDC_VDINT_VDINT1_MASK			0xFFFF
+#define CCDC_PPC_RAW				1
+#define CCDC_DCSUB_DEFAULT_VAL			0
+#define CCDC_CLAMP_DEFAULT_VAL			0
+#define CCDC_ENABLE_VIDEO_PORT			0x8000
+#define CCDC_DISABLE_VIDEO_PORT			0
+#define CCDC_COLPTN_VAL				0xBB11BB11
+#define CCDC_TWO_BYTES_PER_PIXEL		2
+#define CCDC_INTERLACED_IMAGE_INVERT		0x4B6D
+#define CCDC_INTERLACED_NO_IMAGE_INVERT		0x0249
+#define CCDC_PROGRESSIVE_IMAGE_INVERT		0x4000
+#define CCDC_PROGRESSIVE_NO_IMAGE_INVERT	0
+#define CCDC_INTERLACED_HEIGHT_SHIFT		1
+#define CCDC_SYN_MODE_INPMOD_SHIFT		12
+#define CCDC_SYN_MODE_INPMOD_MASK		3
+#define CCDC_SYN_MODE_8BITS			(7 << 8)
+#define CCDC_SYN_MODE_10BITS			(6 << 8)
+#define CCDC_SYN_MODE_11BITS			(5 << 8)
+#define CCDC_SYN_MODE_12BITS			(4 << 8)
+#define CCDC_SYN_MODE_13BITS			(3 << 8)
+#define CCDC_SYN_MODE_14BITS			(2 << 8)
+#define CCDC_SYN_MODE_15BITS			(1 << 8)
+#define CCDC_SYN_MODE_16BITS			(0 << 8)
+#define CCDC_SYN_FLDMODE_MASK			1
+#define CCDC_SYN_FLDMODE_SHIFT			7
+#define CCDC_REC656IF_BT656_EN			3
+#define CCDC_SYN_MODE_VD_POL_NEGATIVE		BIT(2)
+#define CCDC_CCDCFG_Y8POS_SHIFT			11
+#define CCDC_CCDCFG_BW656_10BIT			BIT(5)
+#define CCDC_SDOFST_FIELD_INTERLEAVED		0x249
+#define CCDC_NO_CULLING				0xffff00ff
+#endif
diff --git a/drivers/staging/media/deprecated/vpfe_capture/isif.c b/drivers/staging/media/deprecated/vpfe_capture/isif.c
new file mode 100644
index 000000000..4059891c2
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/isif.c
@@ -0,0 +1,1127 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * Image Sensor Interface (ISIF) driver
+ *
+ * This driver is for configuring the ISIF IP available on DM365 or any other
+ * TI SoCs. This is used for capturing yuv or bayer video or image data
+ * from a decoder or sensor. This IP is similar to the CCDC IP on DM355
+ * and DM6446, but with enhanced or additional ip blocks. The driver
+ * configures the ISIF upon commands from the vpfe bridge driver through
+ * ccdc_hw_device interface.
+ *
+ * TODO: 1) Raw bayer parameter settings and bayer capture
+ *	 2) Add support for control ioctl
+ */
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include "isif.h"
+#include <media/davinci/vpss.h>
+
+#include "isif_regs.h"
+#include "ccdc_hw_device.h"
+
+/* Defaults for module configuration parameters */
+static const struct isif_config_params_raw isif_config_defaults = {
+	.linearize = {
+		.en = 0,
+		.corr_shft = ISIF_NO_SHIFT,
+		.scale_fact = {1, 0},
+	},
+	.df_csc = {
+		.df_or_csc = 0,
+		.csc = {
+			.en = 0,
+		},
+	},
+	.dfc = {
+		.en = 0,
+	},
+	.bclamp = {
+		.en = 0,
+	},
+	.gain_offset = {
+		.gain = {
+			.r_ye = {1, 0},
+			.gr_cy = {1, 0},
+			.gb_g = {1, 0},
+			.b_mg = {1, 0},
+		},
+	},
+	.culling = {
+		.hcpat_odd = 0xff,
+		.hcpat_even = 0xff,
+		.vcpat = 0xff,
+	},
+	.compress = {
+		.alg = ISIF_ALAW,
+	},
+};
+
+/* ISIF operation configuration */
+static struct isif_oper_config {
+	struct device *dev;
+	enum vpfe_hw_if_type if_type;
+	struct isif_ycbcr_config ycbcr;
+	struct isif_params_raw bayer;
+	enum isif_data_pack data_pack;
+	/* ISIF base address */
+	void __iomem *base_addr;
+	/* ISIF Linear Table 0 */
+	void __iomem *linear_tbl0_addr;
+	/* ISIF Linear Table 1 */
+	void __iomem *linear_tbl1_addr;
+} isif_cfg = {
+	.ycbcr = {
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.win = ISIF_WIN_NTSC,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED,
+	},
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = ISIF_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.gain = {
+			.r_ye = {1, 0},
+			.gr_cy = {1, 0},
+			.gb_g = {1, 0},
+			.b_mg = {1, 0},
+		},
+		.cfa_pat = ISIF_CFA_PAT_MOSAIC,
+		.data_msb = ISIF_BIT_MSB_11,
+		.config_params = {
+			.data_shift = ISIF_NO_SHIFT,
+			.col_pat_field0 = {
+				.olop = ISIF_GREEN_BLUE,
+				.olep = ISIF_BLUE,
+				.elop = ISIF_RED,
+				.elep = ISIF_GREEN_RED,
+			},
+			.col_pat_field1 = {
+				.olop = ISIF_GREEN_BLUE,
+				.olep = ISIF_BLUE,
+				.elop = ISIF_RED,
+				.elep = ISIF_GREEN_RED,
+			},
+			.test_pat_gen = 0,
+		},
+	},
+	.data_pack = ISIF_DATA_PACK8,
+};
+
+/* Raw Bayer formats */
+static const u32 isif_raw_bayer_pix_formats[] = {
+	V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static const u32 isif_raw_yuv_pix_formats[] = {
+	V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(isif_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, isif_cfg.base_addr + offset);
+}
+
+/* reg_modify() - read, modify and write register */
+static inline u32 reg_modify(u32 mask, u32 val, u32 offset)
+{
+	u32 new_val = (regr(offset) & ~mask) | (val & mask);
+
+	regw(new_val, offset);
+	return new_val;
+}
+
+static inline void regw_lin_tbl(u32 val, u32 offset, int i)
+{
+	if (!i)
+		__raw_writel(val, isif_cfg.linear_tbl0_addr + offset);
+	else
+		__raw_writel(val, isif_cfg.linear_tbl1_addr + offset);
+}
+
+static void isif_disable_all_modules(void)
+{
+	/* disable BC */
+	regw(0, CLAMPCFG);
+	/* disable vdfc */
+	regw(0, DFCCTL);
+	/* disable CSC */
+	regw(0, CSCCTL);
+	/* disable linearization */
+	regw(0, LINCFG0);
+	/* disable other modules here as they are supported */
+}
+
+static void isif_enable(int en)
+{
+	if (!en) {
+		/* Before disable isif, disable all ISIF modules */
+		isif_disable_all_modules();
+		/*
+		 * wait for next VD. Assume lowest scan rate is 12 Hz. So
+		 * 100 msec delay is good enough
+		 */
+		msleep(100);
+	}
+	reg_modify(ISIF_SYNCEN_VDHDEN_MASK, en, SYNCEN);
+}
+
+static void isif_enable_output_to_sdram(int en)
+{
+	reg_modify(ISIF_SYNCEN_WEN_MASK, en << ISIF_SYNCEN_WEN_SHIFT, SYNCEN);
+}
+
+static void isif_config_culling(struct isif_cul *cul)
+{
+	u32 val;
+
+	/* Horizontal pattern */
+	val = (cul->hcpat_even << CULL_PAT_EVEN_LINE_SHIFT) | cul->hcpat_odd;
+	regw(val, CULH);
+
+	/* vertical pattern */
+	regw(cul->vcpat, CULV);
+
+	/* LPF */
+	reg_modify(ISIF_LPF_MASK << ISIF_LPF_SHIFT,
+		  cul->en_lpf << ISIF_LPF_SHIFT, MODESET);
+}
+
+static void isif_config_gain_offset(void)
+{
+	struct isif_gain_offsets_adj *gain_off_p =
+		&isif_cfg.bayer.config_params.gain_offset;
+	u32 val;
+
+	val = (!!gain_off_p->gain_sdram_en << GAIN_SDRAM_EN_SHIFT) |
+	      (!!gain_off_p->gain_ipipe_en << GAIN_IPIPE_EN_SHIFT) |
+	      (!!gain_off_p->gain_h3a_en << GAIN_H3A_EN_SHIFT) |
+	      (!!gain_off_p->offset_sdram_en << OFST_SDRAM_EN_SHIFT) |
+	      (!!gain_off_p->offset_ipipe_en << OFST_IPIPE_EN_SHIFT) |
+	      (!!gain_off_p->offset_h3a_en << OFST_H3A_EN_SHIFT);
+
+	reg_modify(GAIN_OFFSET_EN_MASK, val, CGAMMAWD);
+
+	val = (gain_off_p->gain.r_ye.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.r_ye.decimal;
+	regw(val, CRGAIN);
+
+	val = (gain_off_p->gain.gr_cy.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.gr_cy.decimal;
+	regw(val, CGRGAIN);
+
+	val = (gain_off_p->gain.gb_g.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.gb_g.decimal;
+	regw(val, CGBGAIN);
+
+	val = (gain_off_p->gain.b_mg.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.b_mg.decimal;
+	regw(val, CBGAIN);
+
+	regw(gain_off_p->offset, COFSTA);
+}
+
+static void isif_restore_defaults(void)
+{
+	enum vpss_ccdc_source_sel source = VPSS_CCDCIN;
+
+	dev_dbg(isif_cfg.dev, "\nstarting isif_restore_defaults...");
+	isif_cfg.bayer.config_params = isif_config_defaults;
+	/* Enable clock to ISIF, IPIPEIF and BL */
+	vpss_enable_clock(VPSS_CCDC_CLOCK, 1);
+	vpss_enable_clock(VPSS_IPIPEIF_CLOCK, 1);
+	vpss_enable_clock(VPSS_BL_CLOCK, 1);
+	/* Set default offset and gain */
+	isif_config_gain_offset();
+	vpss_select_ccdc_source(source);
+	dev_dbg(isif_cfg.dev, "\nEnd of isif_restore_defaults...");
+}
+
+static int isif_open(struct device *device)
+{
+	isif_restore_defaults();
+	return 0;
+}
+
+/* This function will configure the window size to be capture in ISIF reg */
+static void isif_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt, int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int mid_img = 0;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_setwin...");
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = ((image_win->width) << (ppc - 1)) - 1;
+
+	/* Writing the horizontal info into the registers */
+	regw(horz_start & START_PX_HOR_MASK, SPH);
+	regw(horz_nr_pixels & NUM_PX_HOR_MASK, LNH);
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* To account for VD since line 0 doesn't have any data */
+		vert_start += 1;
+	} else {
+		/* To account for VD since line 0 doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/* configure VDINT0 and VDINT1 */
+		mid_img = vert_start + (image_win->height / 2);
+		regw(mid_img, VDINT1);
+	}
+
+	regw(0, VDINT0);
+	regw(vert_start & START_VER_ONE_MASK, SLV0);
+	regw(vert_start & START_VER_TWO_MASK, SLV1);
+	regw(vert_nr_lines & NUM_LINES_VER, LNV);
+}
+
+static void isif_config_bclamp(struct isif_black_clamp *bc)
+{
+	u32 val;
+
+	/*
+	 * DC Offset is always added to image data irrespective of bc enable
+	 * status
+	 */
+	regw(bc->dc_offset, CLDCOFST);
+
+	if (bc->en) {
+		val = bc->bc_mode_color << ISIF_BC_MODE_COLOR_SHIFT;
+
+		/* Enable BC and horizontal clamp calculation parameters */
+		val = val | 1 | (bc->horz.mode << ISIF_HORZ_BC_MODE_SHIFT);
+
+		regw(val, CLAMPCFG);
+
+		if (bc->horz.mode != ISIF_HORZ_BC_DISABLE) {
+			/*
+			 * Window count for calculation
+			 * Base window selection
+			 * pixel limit
+			 * Horizontal size of window
+			 * vertical size of the window
+			 * Horizontal start position of the window
+			 * Vertical start position of the window
+			 */
+			val = bc->horz.win_count_calc |
+			      ((!!bc->horz.base_win_sel_calc) <<
+				ISIF_HORZ_BC_WIN_SEL_SHIFT) |
+			      ((!!bc->horz.clamp_pix_limit) <<
+				ISIF_HORZ_BC_PIX_LIMIT_SHIFT) |
+			      (bc->horz.win_h_sz_calc <<
+				ISIF_HORZ_BC_WIN_H_SIZE_SHIFT) |
+			      (bc->horz.win_v_sz_calc <<
+				ISIF_HORZ_BC_WIN_V_SIZE_SHIFT);
+			regw(val, CLHWIN0);
+
+			regw(bc->horz.win_start_h_calc, CLHWIN1);
+			regw(bc->horz.win_start_v_calc, CLHWIN2);
+		}
+
+		/* vertical clamp calculation parameters */
+
+		/* Reset clamp value sel for previous line */
+		val |=
+		(bc->vert.reset_val_sel << ISIF_VERT_BC_RST_VAL_SEL_SHIFT) |
+		(bc->vert.line_ave_coef << ISIF_VERT_BC_LINE_AVE_COEF_SHIFT);
+		regw(val, CLVWIN0);
+
+		/* Optical Black horizontal start position */
+		regw(bc->vert.ob_start_h, CLVWIN1);
+		/* Optical Black vertical start position */
+		regw(bc->vert.ob_start_v, CLVWIN2);
+		/* Optical Black vertical size for calculation */
+		regw(bc->vert.ob_v_sz_calc, CLVWIN3);
+		/* Vertical start position for BC subtraction */
+		regw(bc->vert_start_sub, CLSV);
+	}
+}
+
+static void isif_config_linearization(struct isif_linearize *linearize)
+{
+	u32 val, i;
+
+	if (!linearize->en) {
+		regw(0, LINCFG0);
+		return;
+	}
+
+	/* shift value for correction & enable linearization (set lsb) */
+	val = (linearize->corr_shft << ISIF_LIN_CORRSFT_SHIFT) | 1;
+	regw(val, LINCFG0);
+
+	/* Scale factor */
+	val = ((!!linearize->scale_fact.integer) <<
+	       ISIF_LIN_SCALE_FACT_INTEG_SHIFT) |
+	       linearize->scale_fact.decimal;
+	regw(val, LINCFG1);
+
+	for (i = 0; i < ISIF_LINEAR_TAB_SIZE; i++) {
+		if (i % 2)
+			regw_lin_tbl(linearize->table[i], ((i >> 1) << 2), 1);
+		else
+			regw_lin_tbl(linearize->table[i], ((i >> 1) << 2), 0);
+	}
+}
+
+static int isif_config_dfc(struct isif_dfc *vdfc)
+{
+	/* initialize retries to loop for max ~ 250 usec */
+	u32 val, count, retries = loops_per_jiffy / (4000/HZ);
+	int i;
+
+	if (!vdfc->en)
+		return 0;
+
+	/* Correction mode */
+	val = (vdfc->corr_mode << ISIF_VDFC_CORR_MOD_SHIFT);
+
+	/* Correct whole line or partial */
+	if (vdfc->corr_whole_line)
+		val |= 1 << ISIF_VDFC_CORR_WHOLE_LN_SHIFT;
+
+	/* level shift value */
+	val |= vdfc->def_level_shift << ISIF_VDFC_LEVEL_SHFT_SHIFT;
+
+	regw(val, DFCCTL);
+
+	/* Defect saturation level */
+	regw(vdfc->def_sat_level, VDFSATLV);
+
+	regw(vdfc->table[0].pos_vert, DFCMEM0);
+	regw(vdfc->table[0].pos_horz, DFCMEM1);
+	if (vdfc->corr_mode == ISIF_VDFC_NORMAL ||
+	    vdfc->corr_mode == ISIF_VDFC_HORZ_INTERPOL_IF_SAT) {
+		regw(vdfc->table[0].level_at_pos, DFCMEM2);
+		regw(vdfc->table[0].level_up_pixels, DFCMEM3);
+		regw(vdfc->table[0].level_low_pixels, DFCMEM4);
+	}
+
+	/* set DFCMARST and set DFCMWR */
+	val = regr(DFCMEMCTL) | (1 << ISIF_DFCMEMCTL_DFCMARST_SHIFT) | 1;
+	regw(val, DFCMEMCTL);
+
+	count = retries;
+	while (count && (regr(DFCMEMCTL) & 0x1))
+		count--;
+
+	if (!count) {
+		dev_dbg(isif_cfg.dev, "defect table write timeout !!!\n");
+		return -1;
+	}
+
+	for (i = 1; i < vdfc->num_vdefects; i++) {
+		regw(vdfc->table[i].pos_vert, DFCMEM0);
+		regw(vdfc->table[i].pos_horz, DFCMEM1);
+		if (vdfc->corr_mode == ISIF_VDFC_NORMAL ||
+		    vdfc->corr_mode == ISIF_VDFC_HORZ_INTERPOL_IF_SAT) {
+			regw(vdfc->table[i].level_at_pos, DFCMEM2);
+			regw(vdfc->table[i].level_up_pixels, DFCMEM3);
+			regw(vdfc->table[i].level_low_pixels, DFCMEM4);
+		}
+		val = regr(DFCMEMCTL);
+		/* clear DFCMARST and set DFCMWR */
+		val &= ~BIT(ISIF_DFCMEMCTL_DFCMARST_SHIFT);
+		val |= 1;
+		regw(val, DFCMEMCTL);
+
+		count = retries;
+		while (count && (regr(DFCMEMCTL) & 0x1))
+			count--;
+
+		if (!count) {
+			dev_err(isif_cfg.dev,
+				"defect table write timeout !!!\n");
+			return -1;
+		}
+	}
+	if (vdfc->num_vdefects < ISIF_VDFC_TABLE_SIZE) {
+		/* Extra cycle needed */
+		regw(0, DFCMEM0);
+		regw(0x1FFF, DFCMEM1);
+		regw(1, DFCMEMCTL);
+	}
+
+	/* enable VDFC */
+	reg_modify((1 << ISIF_VDFC_EN_SHIFT), (1 << ISIF_VDFC_EN_SHIFT),
+		   DFCCTL);
+	return 0;
+}
+
+static void isif_config_csc(struct isif_df_csc *df_csc)
+{
+	u32 val1 = 0, val2 = 0, i;
+
+	if (!df_csc->csc.en) {
+		regw(0, CSCCTL);
+		return;
+	}
+	for (i = 0; i < ISIF_CSC_NUM_COEFF; i++) {
+		if ((i % 2) == 0) {
+			/* CSCM - LSB */
+			val1 = (df_csc->csc.coeff[i].integer <<
+				ISIF_CSC_COEF_INTEG_SHIFT) |
+				df_csc->csc.coeff[i].decimal;
+		} else {
+
+			/* CSCM - MSB */
+			val2 = (df_csc->csc.coeff[i].integer <<
+				ISIF_CSC_COEF_INTEG_SHIFT) |
+				df_csc->csc.coeff[i].decimal;
+			val2 <<= ISIF_CSCM_MSB_SHIFT;
+			val2 |= val1;
+			regw(val2, (CSCM0 + ((i - 1) << 1)));
+		}
+	}
+
+	/* program the active area */
+	regw(df_csc->start_pix, FMTSPH);
+	/*
+	 * one extra pixel as required for CSC. Actually number of
+	 * pixel - 1 should be configured in this register. So we
+	 * need to subtract 1 before writing to FMTSPH, but we will
+	 * not do this since csc requires one extra pixel
+	 */
+	regw(df_csc->num_pixels, FMTLNH);
+	regw(df_csc->start_line, FMTSLV);
+	/*
+	 * one extra line as required for CSC. See reason documented for
+	 * num_pixels
+	 */
+	regw(df_csc->num_lines, FMTLNV);
+
+	/* Enable CSC */
+	regw(1, CSCCTL);
+}
+
+static int isif_config_raw(void)
+{
+	struct isif_params_raw *params = &isif_cfg.bayer;
+	struct isif_config_params_raw *module_params =
+		&isif_cfg.bayer.config_params;
+	struct vpss_pg_frame_size frame_size;
+	struct vpss_sync_pol sync;
+	u32 val;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_config_raw..\n");
+
+	/*
+	 * Configure CCDCFG register:-
+	 * Set CCD Not to swap input since input is RAW data
+	 * Set FID detection function to Latch at V-Sync
+	 * Set WENLOG - isif valid area
+	 * Set TRGSEL
+	 * Set EXTRG
+	 * Packed to 8 or 16 bits
+	 */
+
+	val = ISIF_YCINSWP_RAW | ISIF_CCDCFG_FIDMD_LATCH_VSYNC |
+		ISIF_CCDCFG_WENLOG_AND | ISIF_CCDCFG_TRGSEL_WEN |
+		ISIF_CCDCFG_EXTRG_DISABLE | isif_cfg.data_pack;
+
+	dev_dbg(isif_cfg.dev, "Writing 0x%x to ...CCDCFG \n", val);
+	regw(val, CCDCFG);
+
+	/*
+	 * Configure the vertical sync polarity(MODESET.VDPOL)
+	 * Configure the horizontal sync polarity (MODESET.HDPOL)
+	 * Configure frame id polarity (MODESET.FLDPOL)
+	 * Configure data polarity
+	 * Configure External WEN Selection
+	 * Configure frame format(progressive or interlace)
+	 * Configure pixel format (Input mode)
+	 * Configure the data shift
+	 */
+
+	val = ISIF_VDHDOUT_INPUT | (params->vd_pol << ISIF_VD_POL_SHIFT) |
+		(params->hd_pol << ISIF_HD_POL_SHIFT) |
+		(params->fid_pol << ISIF_FID_POL_SHIFT) |
+		(ISIF_DATAPOL_NORMAL << ISIF_DATAPOL_SHIFT) |
+		(ISIF_EXWEN_DISABLE << ISIF_EXWEN_SHIFT) |
+		(params->frm_fmt << ISIF_FRM_FMT_SHIFT) |
+		(params->pix_fmt << ISIF_INPUT_SHIFT) |
+		(params->config_params.data_shift << ISIF_DATASFT_SHIFT);
+
+	regw(val, MODESET);
+	dev_dbg(isif_cfg.dev, "Writing 0x%x to MODESET...\n", val);
+
+	/*
+	 * Configure GAMMAWD register
+	 * CFA pattern setting
+	 */
+	val = params->cfa_pat << ISIF_GAMMAWD_CFA_SHIFT;
+
+	/* Gamma msb */
+	if (module_params->compress.alg == ISIF_ALAW)
+		val |= ISIF_ALAW_ENABLE;
+
+	val |= (params->data_msb << ISIF_ALAW_GAMMA_WD_SHIFT);
+	regw(val, CGAMMAWD);
+
+	/* Configure DPCM compression settings */
+	if (module_params->compress.alg == ISIF_DPCM) {
+		val =  BIT(ISIF_DPCM_EN_SHIFT) |
+		       (module_params->compress.pred <<
+		       ISIF_DPCM_PREDICTOR_SHIFT);
+	}
+
+	regw(val, MISC);
+
+	/* Configure Gain & Offset */
+	isif_config_gain_offset();
+
+	/* Configure Color pattern */
+	val = (params->config_params.col_pat_field0.olop) |
+	      (params->config_params.col_pat_field0.olep << 2) |
+	      (params->config_params.col_pat_field0.elop << 4) |
+	      (params->config_params.col_pat_field0.elep << 6) |
+	      (params->config_params.col_pat_field1.olop << 8) |
+	      (params->config_params.col_pat_field1.olep << 10) |
+	      (params->config_params.col_pat_field1.elop << 12) |
+	      (params->config_params.col_pat_field1.elep << 14);
+	regw(val, CCOLP);
+	dev_dbg(isif_cfg.dev, "Writing %x to CCOLP ...\n", val);
+
+	/* Configure HSIZE register  */
+	val = (!!params->horz_flip_en) << ISIF_HSIZE_FLIP_SHIFT;
+
+	/* calculate line offset in 32 bytes based on pack value */
+	if (isif_cfg.data_pack == ISIF_PACK_8BIT)
+		val |= ((params->win.width + 31) >> 5);
+	else if (isif_cfg.data_pack == ISIF_PACK_12BIT)
+		val |= (((params->win.width +
+		       (params->win.width >> 2)) + 31) >> 5);
+	else
+		val |= (((params->win.width * 2) + 31) >> 5);
+	regw(val, HSIZE);
+
+	/* Configure SDOFST register  */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_en) {
+			/* For interlace inverse mode */
+			regw(0x4B6D, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x4B6D to SDOFST...\n");
+		} else {
+			/* For interlace non inverse mode */
+			regw(0x0B6D, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x0B6D to SDOFST...\n");
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		if (params->image_invert_en) {
+			/* For progressive inverse mode */
+			regw(0x4000, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x4000 to SDOFST...\n");
+		} else {
+			/* For progressive non inverse mode */
+			regw(0x0000, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x0000 to SDOFST...\n");
+		}
+	}
+
+	/* Configure video window */
+	isif_setwin(&params->win, params->frm_fmt, 1);
+
+	/* Configure Black Clamp */
+	isif_config_bclamp(&module_params->bclamp);
+
+	/* Configure Vertical Defection Pixel Correction */
+	if (isif_config_dfc(&module_params->dfc) < 0)
+		return -EFAULT;
+
+	if (!module_params->df_csc.df_or_csc)
+		/* Configure Color Space Conversion */
+		isif_config_csc(&module_params->df_csc);
+
+	isif_config_linearization(&module_params->linearize);
+
+	/* Configure Culling */
+	isif_config_culling(&module_params->culling);
+
+	/* Configure horizontal and vertical offsets(DFC,LSC,Gain) */
+	regw(module_params->horz_offset, DATAHOFST);
+	regw(module_params->vert_offset, DATAVOFST);
+
+	/* Setup test pattern if enabled */
+	if (params->config_params.test_pat_gen) {
+		/* Use the HD/VD pol settings from user */
+		sync.ccdpg_hdpol = params->hd_pol;
+		sync.ccdpg_vdpol = params->vd_pol;
+		dm365_vpss_set_sync_pol(sync);
+		frame_size.hlpfr = isif_cfg.bayer.win.width;
+		frame_size.pplen = isif_cfg.bayer.win.height;
+		dm365_vpss_set_pg_frame_size(frame_size);
+		vpss_select_ccdc_source(VPSS_PGLPBK);
+	}
+
+	dev_dbg(isif_cfg.dev, "\nEnd of isif_config_ycbcr...\n");
+	return 0;
+}
+
+static int isif_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		isif_cfg.bayer.buf_type = buf_type;
+	else
+		isif_cfg.ycbcr.buf_type = buf_type;
+
+	return 0;
+
+}
+static enum ccdc_buftype isif_get_buftype(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_cfg.bayer.buf_type;
+
+	return isif_cfg.ycbcr.buf_type;
+}
+
+static int isif_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(isif_raw_bayer_pix_formats)) {
+			*pix = isif_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(isif_raw_yuv_pix_formats)) {
+			*pix = isif_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
+static int isif_set_pixel_format(unsigned int pixfmt)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8) {
+			if ((isif_cfg.bayer.config_params.compress.alg !=
+			     ISIF_ALAW) &&
+			    (isif_cfg.bayer.config_params.compress.alg !=
+			     ISIF_DPCM)) {
+				dev_dbg(isif_cfg.dev,
+					"Either configure A-Law or DPCM\n");
+				return -EINVAL;
+			}
+			isif_cfg.data_pack = ISIF_PACK_8BIT;
+		} else if (pixfmt == V4L2_PIX_FMT_SBGGR16) {
+			isif_cfg.bayer.config_params.compress.alg =
+					ISIF_NO_COMPRESSION;
+			isif_cfg.data_pack = ISIF_PACK_16BIT;
+		} else
+			return -EINVAL;
+		isif_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+		isif_cfg.data_pack = ISIF_PACK_8BIT;
+	}
+	return 0;
+}
+
+static u32 isif_get_pixel_format(void)
+{
+	u32 pixfmt;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		if (isif_cfg.bayer.config_params.compress.alg == ISIF_ALAW ||
+		    isif_cfg.bayer.config_params.compress.alg == ISIF_DPCM)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (isif_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+
+static int isif_set_image_window(struct v4l2_rect *win)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		isif_cfg.bayer.win.top = win->top;
+		isif_cfg.bayer.win.left = win->left;
+		isif_cfg.bayer.win.width = win->width;
+		isif_cfg.bayer.win.height = win->height;
+	} else {
+		isif_cfg.ycbcr.win.top = win->top;
+		isif_cfg.ycbcr.win.left = win->left;
+		isif_cfg.ycbcr.win.width = win->width;
+		isif_cfg.ycbcr.win.height = win->height;
+	}
+	return 0;
+}
+
+static void isif_get_image_window(struct v4l2_rect *win)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		*win = isif_cfg.bayer.win;
+	else
+		*win = isif_cfg.ycbcr.win;
+}
+
+static unsigned int isif_get_line_length(void)
+{
+	unsigned int len;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (isif_cfg.data_pack == ISIF_PACK_8BIT)
+			len = ((isif_cfg.bayer.win.width));
+		else if (isif_cfg.data_pack == ISIF_PACK_12BIT)
+			len = (((isif_cfg.bayer.win.width * 2) +
+				 (isif_cfg.bayer.win.width >> 2)));
+		else
+			len = (((isif_cfg.bayer.win.width * 2)));
+	} else
+		len = (((isif_cfg.ycbcr.win.width * 2)));
+	return ALIGN(len, 32);
+}
+
+static int isif_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		isif_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		isif_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+static enum ccdc_frmfmt isif_get_frame_format(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_cfg.bayer.frm_fmt;
+	return isif_cfg.ycbcr.frm_fmt;
+}
+
+static int isif_getfid(void)
+{
+	return (regr(MODESET) >> 15) & 0x1;
+}
+
+/* misc operations */
+static void isif_setfbaddr(unsigned long addr)
+{
+	regw((addr >> 21) & 0x07ff, CADU);
+	regw((addr >> 5) & 0x0ffff, CADL);
+}
+
+static int isif_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	isif_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_BT656_10BIT:
+	case VPFE_YCBCR_SYNC_8:
+		isif_cfg.ycbcr.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT;
+		isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		break;
+	case VPFE_BT1120:
+	case VPFE_YCBCR_SYNC_16:
+		isif_cfg.ycbcr.pix_fmt = CCDC_PIXFMT_YCBCR_16BIT;
+		isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		break;
+	case VPFE_RAW_BAYER:
+		isif_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		break;
+	default:
+		dev_dbg(isif_cfg.dev, "Invalid interface type\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* This function will configure ISIF for YCbCr parameters. */
+static int isif_config_ycbcr(void)
+{
+	struct isif_ycbcr_config *params = &isif_cfg.ycbcr;
+	u32 modeset = 0, ccdcfg = 0;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_config_ycbcr...");
+
+	/* configure pixel format or input mode */
+	modeset = modeset | (params->pix_fmt << ISIF_INPUT_SHIFT) |
+		  (params->frm_fmt << ISIF_FRM_FMT_SHIFT) |
+		  (params->fid_pol << ISIF_FID_POL_SHIFT) |
+		  (params->hd_pol << ISIF_HD_POL_SHIFT) |
+		  (params->vd_pol << ISIF_VD_POL_SHIFT);
+
+	/* pack the data to 8-bit ISIFCFG */
+	switch (isif_cfg.if_type) {
+	case VPFE_BT656:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		modeset |= (VPFE_PINPOL_NEGATIVE << ISIF_VD_POL_SHIFT);
+		regw(3, REC656IF);
+		ccdcfg = ccdcfg | ISIF_DATA_PACK8 | ISIF_YCINSWP_YCBCR;
+		break;
+	case VPFE_BT656_10BIT:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		/* setup BT.656, embedded sync  */
+		regw(3, REC656IF);
+		/* enable 10 bit mode in ccdcfg */
+		ccdcfg = ccdcfg | ISIF_DATA_PACK8 | ISIF_YCINSWP_YCBCR |
+			ISIF_BW656_ENABLE;
+		break;
+	case VPFE_BT1120:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_16BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		regw(3, REC656IF);
+		break;
+
+	case VPFE_YCBCR_SYNC_8:
+		ccdcfg |= ISIF_DATA_PACK8;
+		ccdcfg |= ISIF_YCINSWP_YCBCR;
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		break;
+	case VPFE_YCBCR_SYNC_16:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_16BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		break;
+	default:
+		/* should never come here */
+		dev_dbg(isif_cfg.dev, "Invalid interface type\n");
+		return -EINVAL;
+	}
+
+	regw(modeset, MODESET);
+
+	/* Set up pix order */
+	ccdcfg |= params->pix_order << ISIF_PIX_ORDER_SHIFT;
+
+	regw(ccdcfg, CCDCFG);
+
+	/* configure video window */
+	if ((isif_cfg.if_type == VPFE_BT1120) ||
+	    (isif_cfg.if_type == VPFE_YCBCR_SYNC_16))
+		isif_setwin(&params->win, params->frm_fmt, 1);
+	else
+		isif_setwin(&params->win, params->frm_fmt, 2);
+
+	/*
+	 * configure the horizontal line offset
+	 * this is done by rounding up width to a multiple of 16 pixels
+	 * and multiply by two to account for y:cb:cr 4:2:2 data
+	 */
+	regw(((((params->win.width * 2) + 31) & 0xffffffe0) >> 5), HSIZE);
+
+	/* configure the memory line offset */
+	if ((params->frm_fmt == CCDC_FRMFMT_INTERLACED) &&
+	    (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED))
+		/* two fields are interleaved in memory */
+		regw(0x00000249, SDOFST);
+
+	return 0;
+}
+
+static int isif_configure(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_config_raw();
+	return isif_config_ycbcr();
+}
+
+static int isif_close(struct device *device)
+{
+	/* copy defaults to module params */
+	isif_cfg.bayer.config_params = isif_config_defaults;
+	return 0;
+}
+
+static const struct ccdc_hw_device isif_hw_dev = {
+	.name = "ISIF",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = isif_open,
+		.close = isif_close,
+		.enable = isif_enable,
+		.enable_out_to_sdram = isif_enable_output_to_sdram,
+		.set_hw_if_params = isif_set_hw_if_params,
+		.configure = isif_configure,
+		.set_buftype = isif_set_buftype,
+		.get_buftype = isif_get_buftype,
+		.enum_pix = isif_enum_pix,
+		.set_pixel_format = isif_set_pixel_format,
+		.get_pixel_format = isif_get_pixel_format,
+		.set_frame_format = isif_set_frame_format,
+		.get_frame_format = isif_get_frame_format,
+		.set_image_window = isif_set_image_window,
+		.get_image_window = isif_get_image_window,
+		.get_line_length = isif_get_line_length,
+		.setfbaddr = isif_setfbaddr,
+		.getfid = isif_getfid,
+	},
+};
+
+static int isif_probe(struct platform_device *pdev)
+{
+	void (*setup_pinmux)(void);
+	struct resource	*res;
+	void __iomem *addr;
+	int status = 0, i;
+
+	/* Platform data holds setup_pinmux function ptr */
+	if (!pdev->dev.platform_data)
+		return -ENODEV;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&isif_hw_dev);
+	if (status < 0)
+		return status;
+
+	setup_pinmux = pdev->dev.platform_data;
+	/*
+	 * setup Mux configuration for ccdc which may be different for
+	 * different SoCs using this CCDC
+	 */
+	setup_pinmux();
+
+	i = 0;
+	/* Get the ISIF base address, linearization table0 and table1 addr. */
+	while (i < 3) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res) {
+			status = -ENODEV;
+			goto fail_nobase_res;
+		}
+		res = request_mem_region(res->start, resource_size(res),
+					 res->name);
+		if (!res) {
+			status = -EBUSY;
+			goto fail_nobase_res;
+		}
+		addr = ioremap(res->start, resource_size(res));
+		if (!addr) {
+			status = -ENOMEM;
+			goto fail_base_iomap;
+		}
+		switch (i) {
+		case 0:
+			/* ISIF base address */
+			isif_cfg.base_addr = addr;
+			break;
+		case 1:
+			/* ISIF linear tbl0 address */
+			isif_cfg.linear_tbl0_addr = addr;
+			break;
+		default:
+			/* ISIF linear tbl0 address */
+			isif_cfg.linear_tbl1_addr = addr;
+			break;
+		}
+		i++;
+	}
+	isif_cfg.dev = &pdev->dev;
+
+	printk(KERN_NOTICE "%s is registered with vpfe.\n",
+		isif_hw_dev.name);
+	return 0;
+fail_base_iomap:
+	release_mem_region(res->start, resource_size(res));
+	i--;
+fail_nobase_res:
+	if (isif_cfg.base_addr) {
+		iounmap(isif_cfg.base_addr);
+		isif_cfg.base_addr = NULL;
+	}
+	if (isif_cfg.linear_tbl0_addr) {
+		iounmap(isif_cfg.linear_tbl0_addr);
+		isif_cfg.linear_tbl0_addr = NULL;
+	}
+
+	while (i >= 0) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (res)
+			release_mem_region(res->start, resource_size(res));
+		i--;
+	}
+	vpfe_unregister_ccdc_device(&isif_hw_dev);
+	return status;
+}
+
+static int isif_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+	int i = 0;
+
+	iounmap(isif_cfg.base_addr);
+	isif_cfg.base_addr = NULL;
+	iounmap(isif_cfg.linear_tbl0_addr);
+	isif_cfg.linear_tbl0_addr = NULL;
+	iounmap(isif_cfg.linear_tbl1_addr);
+	isif_cfg.linear_tbl1_addr = NULL;
+	while (i < 3) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		release_mem_region(res->start, resource_size(res));
+		i++;
+	}
+	vpfe_unregister_ccdc_device(&isif_hw_dev);
+	return 0;
+}
+
+static struct platform_driver isif_driver = {
+	.driver = {
+		.name	= "isif",
+	},
+	.remove = isif_remove,
+	.probe = isif_probe,
+};
+
+module_platform_driver(isif_driver);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/deprecated/vpfe_capture/isif.h b/drivers/staging/media/deprecated/vpfe_capture/isif.h
new file mode 100644
index 000000000..8369acd26
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/isif.h
@@ -0,0 +1,518 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * isif header file
+ */
+#ifndef _ISIF_H
+#define _ISIF_H
+
+#include <media/davinci/ccdc_types.h>
+#include <media/davinci/vpfe_types.h>
+
+/* isif float type S8Q8/U8Q8 */
+struct isif_float_8 {
+	/* 8 bit integer part */
+	__u8 integer;
+	/* 8 bit decimal part */
+	__u8 decimal;
+};
+
+/* isif float type U16Q16/S16Q16 */
+struct isif_float_16 {
+	/* 16 bit integer part */
+	__u16 integer;
+	/* 16 bit decimal part */
+	__u16 decimal;
+};
+
+/************************************************************************
+ *   Vertical Defect Correction parameters
+ ***********************************************************************/
+/* Defect Correction (DFC) table entry */
+struct isif_vdfc_entry {
+	/* vertical position of defect */
+	__u16 pos_vert;
+	/* horizontal position of defect */
+	__u16 pos_horz;
+	/*
+	 * Defect level of Vertical line defect position. This is subtracted
+	 * from the data at the defect position
+	 */
+	__u8 level_at_pos;
+	/*
+	 * Defect level of the pixels upper than the vertical line defect.
+	 * This is subtracted from the data
+	 */
+	__u8 level_up_pixels;
+	/*
+	 * Defect level of the pixels lower than the vertical line defect.
+	 * This is subtracted from the data
+	 */
+	__u8 level_low_pixels;
+};
+
+#define ISIF_VDFC_TABLE_SIZE		8
+struct isif_dfc {
+	/* enable vertical defect correction */
+	__u8 en;
+	/* Defect level subtraction. Just fed through if saturating */
+#define	ISIF_VDFC_NORMAL		0
+	/*
+	 * Defect level subtraction. Horizontal interpolation ((i-2)+(i+2))/2
+	 * if data saturating
+	 */
+#define ISIF_VDFC_HORZ_INTERPOL_IF_SAT	1
+	/* Horizontal interpolation (((i-2)+(i+2))/2) */
+#define	ISIF_VDFC_HORZ_INTERPOL		2
+	/* one of the vertical defect correction modes above */
+	__u8 corr_mode;
+	/* 0 - whole line corrected, 1 - not pixels upper than the defect */
+	__u8 corr_whole_line;
+#define ISIF_VDFC_NO_SHIFT		0
+#define ISIF_VDFC_SHIFT_1		1
+#define ISIF_VDFC_SHIFT_2		2
+#define ISIF_VDFC_SHIFT_3		3
+#define ISIF_VDFC_SHIFT_4		4
+	/*
+	 * defect level shift value. level_at_pos, level_upper_pos,
+	 * and level_lower_pos can be shifted up by this value. Choose
+	 * one of the values above
+	 */
+	__u8 def_level_shift;
+	/* defect saturation level */
+	__u16 def_sat_level;
+	/* number of vertical defects. Max is ISIF_VDFC_TABLE_SIZE */
+	__u16 num_vdefects;
+	/* VDFC table ptr */
+	struct isif_vdfc_entry table[ISIF_VDFC_TABLE_SIZE];
+};
+
+struct isif_horz_bclamp {
+
+	/* Horizontal clamp disabled. Only vertical clamp value is subtracted */
+#define	ISIF_HORZ_BC_DISABLE		0
+	/*
+	 * Horizontal clamp value is calculated and subtracted from image data
+	 * along with vertical clamp value
+	 */
+#define ISIF_HORZ_BC_CLAMP_CALC_ENABLED	1
+	/*
+	 * Horizontal clamp value calculated from previous image is subtracted
+	 * from image data along with vertical clamp value.
+	 */
+#define ISIF_HORZ_BC_CLAMP_NOT_UPDATED	2
+	/* horizontal clamp mode. One of the values above */
+	__u8 mode;
+	/*
+	 * pixel value limit enable.
+	 *  0 - limit disabled
+	 *  1 - pixel value limited to 1023
+	 */
+	__u8 clamp_pix_limit;
+	/* Select Most left window for bc calculation */
+#define	ISIF_SEL_MOST_LEFT_WIN		0
+	/* Select Most right window for bc calculation */
+#define ISIF_SEL_MOST_RIGHT_WIN		1
+	/* Select most left or right window for clamp val calculation */
+	__u8 base_win_sel_calc;
+	/* Window count per color for calculation. range 1-32 */
+	__u8 win_count_calc;
+	/* Window start position - horizontal for calculation. 0 - 8191 */
+	__u16 win_start_h_calc;
+	/* Window start position - vertical for calculation 0 - 8191 */
+	__u16 win_start_v_calc;
+#define ISIF_HORZ_BC_SZ_H_2PIXELS	0
+#define ISIF_HORZ_BC_SZ_H_4PIXELS	1
+#define ISIF_HORZ_BC_SZ_H_8PIXELS	2
+#define ISIF_HORZ_BC_SZ_H_16PIXELS	3
+	/* Width of the sample window in pixels for calculation */
+	__u8 win_h_sz_calc;
+#define ISIF_HORZ_BC_SZ_V_32PIXELS	0
+#define ISIF_HORZ_BC_SZ_V_64PIXELS	1
+#define	ISIF_HORZ_BC_SZ_V_128PIXELS	2
+#define ISIF_HORZ_BC_SZ_V_256PIXELS	3
+	/* Height of the sample window in pixels for calculation */
+	__u8 win_v_sz_calc;
+};
+
+/************************************************************************
+ *  Black Clamp parameters
+ ***********************************************************************/
+struct isif_vert_bclamp {
+	/* Reset value used is the clamp value calculated */
+#define	ISIF_VERT_BC_USE_HORZ_CLAMP_VAL		0
+	/* Reset value used is reset_clamp_val configured */
+#define	ISIF_VERT_BC_USE_CONFIG_CLAMP_VAL	1
+	/* No update, previous image value is used */
+#define	ISIF_VERT_BC_NO_UPDATE			2
+	/*
+	 * Reset value selector for vertical clamp calculation. Use one of
+	 * the above values
+	 */
+	__u8 reset_val_sel;
+	/* U8Q8. Line average coefficient used in vertical clamp calculation */
+	__u8 line_ave_coef;
+	/* Height of the optical black region for calculation */
+	__u16 ob_v_sz_calc;
+	/* Optical black region start position - horizontal. 0 - 8191 */
+	__u16 ob_start_h;
+	/* Optical black region start position - vertical 0 - 8191 */
+	__u16 ob_start_v;
+};
+
+struct isif_black_clamp {
+	/*
+	 * This offset value is added irrespective of the clamp enable status.
+	 * S13
+	 */
+	__u16 dc_offset;
+	/*
+	 * Enable black/digital clamp value to be subtracted from the image data
+	 */
+	__u8 en;
+	/*
+	 * black clamp mode. same/separate clamp for 4 colors
+	 * 0 - disable - same clamp value for all colors
+	 * 1 - clamp value calculated separately for all colors
+	 */
+	__u8 bc_mode_color;
+	/* Vertical start position for bc subtraction */
+	__u16 vert_start_sub;
+	/* Black clamp for horizontal direction */
+	struct isif_horz_bclamp horz;
+	/* Black clamp for vertical direction */
+	struct isif_vert_bclamp vert;
+};
+
+/*************************************************************************
+** Color Space Conversion (CSC)
+*************************************************************************/
+#define ISIF_CSC_NUM_COEFF	16
+struct isif_color_space_conv {
+	/* Enable color space conversion */
+	__u8 en;
+	/*
+	 * csc coefficient table. S8Q5, M00 at index 0, M01 at index 1, and
+	 * so forth
+	 */
+	struct isif_float_8 coeff[ISIF_CSC_NUM_COEFF];
+};
+
+
+/*************************************************************************
+**  Black  Compensation parameters
+*************************************************************************/
+struct isif_black_comp {
+	/* Comp for Red */
+	__s8 r_comp;
+	/* Comp for Gr */
+	__s8 gr_comp;
+	/* Comp for Blue */
+	__s8 b_comp;
+	/* Comp for Gb */
+	__s8 gb_comp;
+};
+
+/*************************************************************************
+**  Gain parameters
+*************************************************************************/
+struct isif_gain {
+	/* Gain for Red or ye */
+	struct isif_float_16 r_ye;
+	/* Gain for Gr or cy */
+	struct isif_float_16 gr_cy;
+	/* Gain for Gb or g */
+	struct isif_float_16 gb_g;
+	/* Gain for Blue or mg */
+	struct isif_float_16 b_mg;
+};
+
+#define ISIF_LINEAR_TAB_SIZE	192
+/*************************************************************************
+**  Linearization parameters
+*************************************************************************/
+struct isif_linearize {
+	/* Enable or Disable linearization of data */
+	__u8 en;
+	/* Shift value applied */
+	__u8 corr_shft;
+	/* scale factor applied U11Q10 */
+	struct isif_float_16 scale_fact;
+	/* Size of the linear table */
+	__u16 table[ISIF_LINEAR_TAB_SIZE];
+};
+
+/* Color patterns */
+#define ISIF_RED	0
+#define	ISIF_GREEN_RED	1
+#define ISIF_GREEN_BLUE	2
+#define ISIF_BLUE	3
+struct isif_col_pat {
+	__u8 olop;
+	__u8 olep;
+	__u8 elop;
+	__u8 elep;
+};
+
+/*************************************************************************
+**  Data formatter parameters
+*************************************************************************/
+struct isif_fmtplen {
+	/*
+	 * number of program entries for SET0, range 1 - 16
+	 * when fmtmode is ISIF_SPLIT, 1 - 8 when fmtmode is
+	 * ISIF_COMBINE
+	 */
+	__u16 plen0;
+	/*
+	 * number of program entries for SET1, range 1 - 16
+	 * when fmtmode is ISIF_SPLIT, 1 - 8 when fmtmode is
+	 * ISIF_COMBINE
+	 */
+	__u16 plen1;
+	/**
+	 * number of program entries for SET2, range 1 - 16
+	 * when fmtmode is ISIF_SPLIT, 1 - 8 when fmtmode is
+	 * ISIF_COMBINE
+	 */
+	__u16 plen2;
+	/**
+	 * number of program entries for SET3, range 1 - 16
+	 * when fmtmode is ISIF_SPLIT, 1 - 8 when fmtmode is
+	 * ISIF_COMBINE
+	 */
+	__u16 plen3;
+};
+
+struct isif_fmt_cfg {
+#define ISIF_SPLIT		0
+#define ISIF_COMBINE		1
+	/* Split or combine or line alternate */
+	__u8 fmtmode;
+	/* enable or disable line alternating mode */
+	__u8 ln_alter_en;
+#define ISIF_1LINE		0
+#define	ISIF_2LINES		1
+#define	ISIF_3LINES		2
+#define	ISIF_4LINES		3
+	/* Split/combine line number */
+	__u8 lnum;
+	/* Address increment Range 1 - 16 */
+	__u8 addrinc;
+};
+
+struct isif_fmt_addr_ptr {
+	/* Initial address */
+	__u32 init_addr;
+	/* output line number */
+#define ISIF_1STLINE		0
+#define	ISIF_2NDLINE		1
+#define	ISIF_3RDLINE		2
+#define	ISIF_4THLINE		3
+	__u8 out_line;
+};
+
+struct isif_fmtpgm_ap {
+	/* program address pointer */
+	__u8 pgm_aptr;
+	/* program address increment or decrement */
+	__u8 pgmupdt;
+};
+
+struct isif_data_formatter {
+	/* Enable/Disable data formatter */
+	__u8 en;
+	/* data formatter configuration */
+	struct isif_fmt_cfg cfg;
+	/* Formatter program entries length */
+	struct isif_fmtplen plen;
+	/* first pixel in a line fed to formatter */
+	__u16 fmtrlen;
+	/* HD interval for output line. Only valid when split line */
+	__u16 fmthcnt;
+	/* formatter address pointers */
+	struct isif_fmt_addr_ptr fmtaddr_ptr[16];
+	/* program enable/disable */
+	__u8 pgm_en[32];
+	/* program address pointers */
+	struct isif_fmtpgm_ap fmtpgm_ap[32];
+};
+
+struct isif_df_csc {
+	/* Color Space Conversion configuration, 0 - csc, 1 - df */
+	__u8 df_or_csc;
+	/* csc configuration valid if df_or_csc is 0 */
+	struct isif_color_space_conv csc;
+	/* data formatter configuration valid if df_or_csc is 1 */
+	struct isif_data_formatter df;
+	/* start pixel in a line at the input */
+	__u32 start_pix;
+	/* number of pixels in input line */
+	__u32 num_pixels;
+	/* start line at the input */
+	__u32 start_line;
+	/* number of lines at the input */
+	__u32 num_lines;
+};
+
+struct isif_gain_offsets_adj {
+	/* Gain adjustment per color */
+	struct isif_gain gain;
+	/* Offset adjustment */
+	__u16 offset;
+	/* Enable or Disable Gain adjustment for SDRAM data */
+	__u8 gain_sdram_en;
+	/* Enable or Disable Gain adjustment for IPIPE data */
+	__u8 gain_ipipe_en;
+	/* Enable or Disable Gain adjustment for H3A data */
+	__u8 gain_h3a_en;
+	/* Enable or Disable Gain adjustment for SDRAM data */
+	__u8 offset_sdram_en;
+	/* Enable or Disable Gain adjustment for IPIPE data */
+	__u8 offset_ipipe_en;
+	/* Enable or Disable Gain adjustment for H3A data */
+	__u8 offset_h3a_en;
+};
+
+struct isif_cul {
+	/* Horizontal Cull pattern for odd lines */
+	__u8 hcpat_odd;
+	/* Horizontal Cull pattern for even lines */
+	__u8 hcpat_even;
+	/* Vertical Cull pattern */
+	__u8 vcpat;
+	/* Enable or disable lpf. Apply when cull is enabled */
+	__u8 en_lpf;
+};
+
+struct isif_compress {
+#define ISIF_ALAW		0
+#define ISIF_DPCM		1
+#define ISIF_NO_COMPRESSION	2
+	/* Compression Algorithm used */
+	__u8 alg;
+	/* Choose Predictor1 for DPCM compression */
+#define ISIF_DPCM_PRED1		0
+	/* Choose Predictor2 for DPCM compression */
+#define ISIF_DPCM_PRED2		1
+	/* Predictor for DPCM compression */
+	__u8 pred;
+};
+
+/* all the stuff in this struct will be provided by userland */
+struct isif_config_params_raw {
+	/* Linearization parameters for image sensor data input */
+	struct isif_linearize linearize;
+	/* Data formatter or CSC */
+	struct isif_df_csc df_csc;
+	/* Defect Pixel Correction (DFC) configuration */
+	struct isif_dfc dfc;
+	/* Black/Digital Clamp configuration */
+	struct isif_black_clamp bclamp;
+	/* Gain, offset adjustments */
+	struct isif_gain_offsets_adj gain_offset;
+	/* Culling */
+	struct isif_cul culling;
+	/* A-Law and DPCM compression options */
+	struct isif_compress compress;
+	/* horizontal offset for Gain/LSC/DFC */
+	__u16 horz_offset;
+	/* vertical offset for Gain/LSC/DFC */
+	__u16 vert_offset;
+	/* color pattern for field 0 */
+	struct isif_col_pat col_pat_field0;
+	/* color pattern for field 1 */
+	struct isif_col_pat col_pat_field1;
+#define ISIF_NO_SHIFT		0
+#define	ISIF_1BIT_SHIFT		1
+#define	ISIF_2BIT_SHIFT		2
+#define	ISIF_3BIT_SHIFT		3
+#define	ISIF_4BIT_SHIFT		4
+#define ISIF_5BIT_SHIFT		5
+#define ISIF_6BIT_SHIFT		6
+	/* Data shift applied before storing to SDRAM */
+	__u8 data_shift;
+	/* enable input test pattern generation */
+	__u8 test_pat_gen;
+};
+
+#ifdef __KERNEL__
+struct isif_ycbcr_config {
+	/* isif pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* isif frame format */
+	enum ccdc_frmfmt frm_fmt;
+	/* ISIF crop window */
+	struct v4l2_rect win;
+	/* field polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* interface VD polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* interface HD polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* isif pix order. Only used for ycbcr capture */
+	enum ccdc_pixorder pix_order;
+	/* isif buffer type. Only used for ycbcr capture */
+	enum ccdc_buftype buf_type;
+};
+
+/* MSB of image data connected to sensor port */
+enum isif_data_msb {
+	ISIF_BIT_MSB_15,
+	ISIF_BIT_MSB_14,
+	ISIF_BIT_MSB_13,
+	ISIF_BIT_MSB_12,
+	ISIF_BIT_MSB_11,
+	ISIF_BIT_MSB_10,
+	ISIF_BIT_MSB_9,
+	ISIF_BIT_MSB_8,
+	ISIF_BIT_MSB_7
+};
+
+enum isif_cfa_pattern {
+	ISIF_CFA_PAT_MOSAIC,
+	ISIF_CFA_PAT_STRIPE
+};
+
+struct isif_params_raw {
+	/* isif pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* isif frame format */
+	enum ccdc_frmfmt frm_fmt;
+	/* video window */
+	struct v4l2_rect win;
+	/* field polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* interface VD polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* interface HD polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* buffer type. Applicable for interlaced mode */
+	enum ccdc_buftype buf_type;
+	/* Gain values */
+	struct isif_gain gain;
+	/* cfa pattern */
+	enum isif_cfa_pattern cfa_pat;
+	/* Data MSB position */
+	enum isif_data_msb data_msb;
+	/* Enable horizontal flip */
+	unsigned char horz_flip_en;
+	/* Enable image invert vertically */
+	unsigned char image_invert_en;
+
+	/* all the userland defined stuff*/
+	struct isif_config_params_raw config_params;
+};
+
+enum isif_data_pack {
+	ISIF_PACK_16BIT,
+	ISIF_PACK_12BIT,
+	ISIF_PACK_8BIT
+};
+
+#define ISIF_WIN_NTSC				{0, 0, 720, 480}
+#define ISIF_WIN_VGA				{0, 0, 640, 480}
+
+#endif
+#endif
diff --git a/drivers/staging/media/deprecated/vpfe_capture/isif_regs.h b/drivers/staging/media/deprecated/vpfe_capture/isif_regs.h
new file mode 100644
index 000000000..d68d38841
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/isif_regs.h
@@ -0,0 +1,256 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ */
+#ifndef _ISIF_REGS_H
+#define _ISIF_REGS_H
+
+/* ISIF registers relative offsets */
+#define SYNCEN					0x00
+#define MODESET					0x04
+#define HDW					0x08
+#define VDW					0x0c
+#define PPLN					0x10
+#define LPFR					0x14
+#define SPH					0x18
+#define LNH					0x1c
+#define SLV0					0x20
+#define SLV1					0x24
+#define LNV					0x28
+#define CULH					0x2c
+#define CULV					0x30
+#define HSIZE					0x34
+#define SDOFST					0x38
+#define CADU					0x3c
+#define CADL					0x40
+#define LINCFG0					0x44
+#define LINCFG1					0x48
+#define CCOLP					0x4c
+#define CRGAIN					0x50
+#define CGRGAIN					0x54
+#define CGBGAIN					0x58
+#define CBGAIN					0x5c
+#define COFSTA					0x60
+#define FLSHCFG0				0x64
+#define FLSHCFG1				0x68
+#define FLSHCFG2				0x6c
+#define VDINT0					0x70
+#define VDINT1					0x74
+#define VDINT2					0x78
+#define MISC					0x7c
+#define CGAMMAWD				0x80
+#define REC656IF				0x84
+#define CCDCFG					0x88
+/*****************************************************
+* Defect Correction registers
+*****************************************************/
+#define DFCCTL					0x8c
+#define VDFSATLV				0x90
+#define DFCMEMCTL				0x94
+#define DFCMEM0					0x98
+#define DFCMEM1					0x9c
+#define DFCMEM2					0xa0
+#define DFCMEM3					0xa4
+#define DFCMEM4					0xa8
+/****************************************************
+* Black Clamp registers
+****************************************************/
+#define CLAMPCFG				0xac
+#define CLDCOFST				0xb0
+#define CLSV					0xb4
+#define CLHWIN0					0xb8
+#define CLHWIN1					0xbc
+#define CLHWIN2					0xc0
+#define CLVRV					0xc4
+#define CLVWIN0					0xc8
+#define CLVWIN1					0xcc
+#define CLVWIN2					0xd0
+#define CLVWIN3					0xd4
+/****************************************************
+* Lense Shading Correction
+****************************************************/
+#define DATAHOFST				0xd8
+#define DATAVOFST				0xdc
+#define LSCHVAL					0xe0
+#define LSCVVAL					0xe4
+#define TWODLSCCFG				0xe8
+#define TWODLSCOFST				0xec
+#define TWODLSCINI				0xf0
+#define TWODLSCGRBU				0xf4
+#define TWODLSCGRBL				0xf8
+#define TWODLSCGROF				0xfc
+#define TWODLSCORBU				0x100
+#define TWODLSCORBL				0x104
+#define TWODLSCOROF				0x108
+#define TWODLSCIRQEN				0x10c
+#define TWODLSCIRQST				0x110
+/****************************************************
+* Data formatter
+****************************************************/
+#define FMTCFG					0x114
+#define FMTPLEN					0x118
+#define FMTSPH					0x11c
+#define FMTLNH					0x120
+#define FMTSLV					0x124
+#define FMTLNV					0x128
+#define FMTRLEN					0x12c
+#define FMTHCNT					0x130
+#define FMTAPTR_BASE				0x134
+/* Below macro for addresses FMTAPTR0 - FMTAPTR15 */
+#define FMTAPTR(i)			(FMTAPTR_BASE + (i * 4))
+#define FMTPGMVF0				0x174
+#define FMTPGMVF1				0x178
+#define FMTPGMAPU0				0x17c
+#define FMTPGMAPU1				0x180
+#define FMTPGMAPS0				0x184
+#define FMTPGMAPS1				0x188
+#define FMTPGMAPS2				0x18c
+#define FMTPGMAPS3				0x190
+#define FMTPGMAPS4				0x194
+#define FMTPGMAPS5				0x198
+#define FMTPGMAPS6				0x19c
+#define FMTPGMAPS7				0x1a0
+/************************************************
+* Color Space Converter
+************************************************/
+#define CSCCTL					0x1a4
+#define CSCM0					0x1a8
+#define CSCM1					0x1ac
+#define CSCM2					0x1b0
+#define CSCM3					0x1b4
+#define CSCM4					0x1b8
+#define CSCM5					0x1bc
+#define CSCM6					0x1c0
+#define CSCM7					0x1c4
+#define OBWIN0					0x1c8
+#define OBWIN1					0x1cc
+#define OBWIN2					0x1d0
+#define OBWIN3					0x1d4
+#define OBVAL0					0x1d8
+#define OBVAL1					0x1dc
+#define OBVAL2					0x1e0
+#define OBVAL3					0x1e4
+#define OBVAL4					0x1e8
+#define OBVAL5					0x1ec
+#define OBVAL6					0x1f0
+#define OBVAL7					0x1f4
+#define CLKCTL					0x1f8
+
+/* Masks & Shifts below */
+#define START_PX_HOR_MASK			0x7FFF
+#define NUM_PX_HOR_MASK				0x7FFF
+#define START_VER_ONE_MASK			0x7FFF
+#define START_VER_TWO_MASK			0x7FFF
+#define NUM_LINES_VER				0x7FFF
+
+/* gain - offset masks */
+#define GAIN_INTEGER_SHIFT			9
+#define OFFSET_MASK				0xFFF
+#define GAIN_SDRAM_EN_SHIFT			12
+#define GAIN_IPIPE_EN_SHIFT			13
+#define GAIN_H3A_EN_SHIFT			14
+#define OFST_SDRAM_EN_SHIFT			8
+#define OFST_IPIPE_EN_SHIFT			9
+#define OFST_H3A_EN_SHIFT			10
+#define GAIN_OFFSET_EN_MASK			0x7700
+
+/* Culling */
+#define CULL_PAT_EVEN_LINE_SHIFT		8
+
+/* CCDCFG register */
+#define ISIF_YCINSWP_RAW			(0x00 << 4)
+#define ISIF_YCINSWP_YCBCR			(0x01 << 4)
+#define ISIF_CCDCFG_FIDMD_LATCH_VSYNC		(0x00 << 6)
+#define ISIF_CCDCFG_WENLOG_AND			(0x00 << 8)
+#define ISIF_CCDCFG_TRGSEL_WEN			(0x00 << 9)
+#define ISIF_CCDCFG_EXTRG_DISABLE		(0x00 << 10)
+#define ISIF_LATCH_ON_VSYNC_DISABLE		(0x01 << 15)
+#define ISIF_LATCH_ON_VSYNC_ENABLE		(0x00 << 15)
+#define ISIF_DATA_PACK_MASK			3
+#define ISIF_DATA_PACK16			0
+#define ISIF_DATA_PACK12			1
+#define ISIF_DATA_PACK8				2
+#define ISIF_PIX_ORDER_SHIFT			11
+#define ISIF_BW656_ENABLE			(0x01 << 5)
+
+/* MODESET registers */
+#define ISIF_VDHDOUT_INPUT			(0x00 << 0)
+#define ISIF_INPUT_SHIFT			12
+#define ISIF_RAW_INPUT_MODE			0
+#define ISIF_FID_POL_SHIFT			4
+#define ISIF_HD_POL_SHIFT			3
+#define ISIF_VD_POL_SHIFT			2
+#define ISIF_DATAPOL_NORMAL			0
+#define ISIF_DATAPOL_SHIFT			6
+#define ISIF_EXWEN_DISABLE			0
+#define ISIF_EXWEN_SHIFT			5
+#define ISIF_FRM_FMT_SHIFT			7
+#define ISIF_DATASFT_SHIFT			8
+#define ISIF_LPF_SHIFT				14
+#define ISIF_LPF_MASK				1
+
+/* GAMMAWD registers */
+#define ISIF_ALAW_GAMMA_WD_MASK			0xF
+#define ISIF_ALAW_GAMMA_WD_SHIFT		1
+#define ISIF_ALAW_ENABLE			1
+#define ISIF_GAMMAWD_CFA_SHIFT			5
+
+/* HSIZE registers */
+#define ISIF_HSIZE_FLIP_MASK			1
+#define ISIF_HSIZE_FLIP_SHIFT			12
+
+/* MISC registers */
+#define ISIF_DPCM_EN_SHIFT			12
+#define ISIF_DPCM_PREDICTOR_SHIFT		13
+
+/* Black clamp related */
+#define ISIF_BC_MODE_COLOR_SHIFT		4
+#define ISIF_HORZ_BC_MODE_SHIFT			1
+#define ISIF_HORZ_BC_WIN_SEL_SHIFT		5
+#define ISIF_HORZ_BC_PIX_LIMIT_SHIFT		6
+#define ISIF_HORZ_BC_WIN_H_SIZE_SHIFT		8
+#define ISIF_HORZ_BC_WIN_V_SIZE_SHIFT		12
+#define	ISIF_VERT_BC_RST_VAL_SEL_SHIFT		4
+#define ISIF_VERT_BC_LINE_AVE_COEF_SHIFT	8
+
+/* VDFC registers */
+#define ISIF_VDFC_EN_SHIFT			4
+#define ISIF_VDFC_CORR_MOD_SHIFT		5
+#define ISIF_VDFC_CORR_WHOLE_LN_SHIFT		7
+#define ISIF_VDFC_LEVEL_SHFT_SHIFT		8
+#define ISIF_VDFC_POS_MASK			0x1FFF
+#define ISIF_DFCMEMCTL_DFCMARST_SHIFT		2
+
+/* CSC registers */
+#define ISIF_CSC_COEF_INTEG_MASK		7
+#define ISIF_CSC_COEF_DECIMAL_MASK		0x1f
+#define ISIF_CSC_COEF_INTEG_SHIFT		5
+#define ISIF_CSCM_MSB_SHIFT			8
+#define ISIF_DF_CSC_SPH_MASK			0x1FFF
+#define ISIF_DF_CSC_LNH_MASK			0x1FFF
+#define ISIF_DF_CSC_SLV_MASK			0x1FFF
+#define ISIF_DF_CSC_LNV_MASK			0x1FFF
+#define ISIF_DF_NUMLINES			0x7FFF
+#define ISIF_DF_NUMPIX				0x1FFF
+
+/* Offsets for LSC/DFC/Gain */
+#define ISIF_DATA_H_OFFSET_MASK			0x1FFF
+#define ISIF_DATA_V_OFFSET_MASK			0x1FFF
+
+/* Linearization */
+#define ISIF_LIN_CORRSFT_SHIFT			4
+#define ISIF_LIN_SCALE_FACT_INTEG_SHIFT		10
+
+
+/* Pattern registers */
+#define ISIF_PG_EN				(1 << 3)
+#define ISIF_SEL_PG_SRC				(3 << 4)
+#define ISIF_PG_VD_POL_SHIFT			0
+#define ISIF_PG_HD_POL_SHIFT			1
+
+/*random other junk*/
+#define ISIF_SYNCEN_VDHDEN_MASK			(1 << 0)
+#define ISIF_SYNCEN_WEN_MASK			(1 << 1)
+#define ISIF_SYNCEN_WEN_SHIFT			1
+
+#endif
diff --git a/drivers/staging/media/deprecated/vpfe_capture/vpfe_capture.c b/drivers/staging/media/deprecated/vpfe_capture/vpfe_capture.c
new file mode 100644
index 000000000..0a2226b32
--- /dev/null
+++ b/drivers/staging/media/deprecated/vpfe_capture/vpfe_capture.c
@@ -0,0 +1,1902 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * Driver name : VPFE Capture driver
+ *    VPFE Capture driver allows applications to capture and stream video
+ *    frames on DaVinci SoCs (DM6446, DM355 etc) from a YUV source such as
+ *    TVP5146 or  Raw Bayer RGB image data from an image sensor
+ *    such as Microns' MT9T001, MT9T031 etc.
+ *
+ *    These SoCs have, in common, a Video Processing Subsystem (VPSS) that
+ *    consists of a Video Processing Front End (VPFE) for capturing
+ *    video/raw image data and Video Processing Back End (VPBE) for displaying
+ *    YUV data through an in-built analog encoder or Digital LCD port. This
+ *    driver is for capture through VPFE. A typical EVM using these SoCs have
+ *    following high level configuration.
+ *
+ *    decoder(TVP5146/		YUV/
+ *	     MT9T001)   -->  Raw Bayer RGB ---> MUX -> VPFE (CCDC/ISIF)
+ *				data input              |      |
+ *							V      |
+ *						      SDRAM    |
+ *							       V
+ *							   Image Processor
+ *							       |
+ *							       V
+ *							     SDRAM
+ *    The data flow happens from a decoder connected to the VPFE over a
+ *    YUV embedded (BT.656/BT.1120) or separate sync or raw bayer rgb interface
+ *    and to the input of VPFE through an optional MUX (if more inputs are
+ *    to be interfaced on the EVM). The input data is first passed through
+ *    CCDC (CCD Controller, a.k.a Image Sensor Interface, ISIF). The CCDC
+ *    does very little or no processing on YUV data and does pre-process Raw
+ *    Bayer RGB data through modules such as Defect Pixel Correction (DFC)
+ *    Color Space Conversion (CSC), data gain/offset etc. After this, data
+ *    can be written to SDRAM or can be connected to the image processing
+ *    block such as IPIPE (on DM355 only).
+ *
+ *    Features supported
+ *		- MMAP IO
+ *		- Capture using TVP5146 over BT.656
+ *		- support for interfacing decoders using sub device model
+ *		- Work with DM355 or DM6446 CCDC to do Raw Bayer RGB/YUV
+ *		  data capture to SDRAM.
+ *    TODO list
+ *		- Support multiple REQBUF after open
+ *		- Support for de-allocating buffers through REQBUF
+ *		- Support for Raw Bayer RGB capture
+ *		- Support for chaining Image Processor
+ *		- Support for static allocation of buffers
+ *		- Support for USERPTR IO
+ *		- Support for STREAMON before QBUF
+ *		- Support for control ioctls
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <media/v4l2-common.h>
+#include <linux/io.h>
+#include <media/davinci/vpfe_capture.h>
+#include "ccdc_hw_device.h"
+
+static int debug;
+static u32 numbuffers = 3;
+static u32 bufsize = (720 * 576 * 2);
+
+module_param(numbuffers, uint, S_IRUGO);
+module_param(bufsize, uint, S_IRUGO);
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(numbuffers, "buffer count (default:3)");
+MODULE_PARM_DESC(bufsize, "buffer size in bytes (default:720 x 576 x 2)");
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+MODULE_DESCRIPTION("VPFE Video for Linux Capture Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
+
+/* standard information */
+struct vpfe_standard {
+	v4l2_std_id std_id;
+	unsigned int width;
+	unsigned int height;
+	struct v4l2_fract pixelaspect;
+	/* 0 - progressive, 1 - interlaced */
+	int frame_format;
+};
+
+/* ccdc configuration */
+struct ccdc_config {
+	/* This make sure vpfe is probed and ready to go */
+	int vpfe_probed;
+	/* name of ccdc device */
+	char name[32];
+};
+
+/* data structures */
+static struct vpfe_config_params config_params = {
+	.min_numbuffers = 3,
+	.numbuffers = 3,
+	.min_bufsize = 720 * 480 * 2,
+	.device_bufsize = 720 * 576 * 2,
+};
+
+/* ccdc device registered */
+static const struct ccdc_hw_device *ccdc_dev;
+/* lock for accessing ccdc information */
+static DEFINE_MUTEX(ccdc_lock);
+/* ccdc configuration */
+static struct ccdc_config *ccdc_cfg;
+
+static const struct vpfe_standard vpfe_standards[] = {
+	{V4L2_STD_525_60, 720, 480, {11, 10}, 1},
+	{V4L2_STD_625_50, 720, 576, {54, 59}, 1},
+};
+
+/* Used when raw Bayer image from ccdc is directly captured to SDRAM */
+static const struct vpfe_pixel_format vpfe_pix_fmts[] = {
+	{
+		.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		.bpp = 1,
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_SBGGR16,
+		.bpp = 2,
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+		.bpp = 1,
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.bpp = 2,
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_YUYV,
+		.bpp = 2,
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_NV12,
+		.bpp = 1,
+	},
+};
+
+/*
+ * vpfe_lookup_pix_format()
+ * lookup an entry in the vpfe pix format table based on pix_format
+ */
+static const struct vpfe_pixel_format *vpfe_lookup_pix_format(u32 pix_format)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe_pix_fmts); i++) {
+		if (pix_format == vpfe_pix_fmts[i].pixelformat)
+			return &vpfe_pix_fmts[i];
+	}
+	return NULL;
+}
+
+/*
+ * vpfe_register_ccdc_device. CCDC module calls this to
+ * register with vpfe capture
+ */
+int vpfe_register_ccdc_device(const struct ccdc_hw_device *dev)
+{
+	int ret = 0;
+	printk(KERN_NOTICE "vpfe_register_ccdc_device: %s\n", dev->name);
+
+	if (!dev->hw_ops.open ||
+	    !dev->hw_ops.enable ||
+	    !dev->hw_ops.set_hw_if_params ||
+	    !dev->hw_ops.configure ||
+	    !dev->hw_ops.set_buftype ||
+	    !dev->hw_ops.get_buftype ||
+	    !dev->hw_ops.enum_pix ||
+	    !dev->hw_ops.set_frame_format ||
+	    !dev->hw_ops.get_frame_format ||
+	    !dev->hw_ops.get_pixel_format ||
+	    !dev->hw_ops.set_pixel_format ||
+	    !dev->hw_ops.set_image_window ||
+	    !dev->hw_ops.get_image_window ||
+	    !dev->hw_ops.get_line_length ||
+	    !dev->hw_ops.getfid)
+		return -EINVAL;
+
+	mutex_lock(&ccdc_lock);
+	if (!ccdc_cfg) {
+		/*
+		 * TODO. Will this ever happen? if so, we need to fix it.
+		 * Probably we need to add the request to a linked list and
+		 * walk through it during vpfe probe
+		 */
+		printk(KERN_ERR "vpfe capture not initialized\n");
+		ret = -EFAULT;
+		goto unlock;
+	}
+
+	if (strcmp(dev->name, ccdc_cfg->name)) {
+		/* ignore this ccdc */
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (ccdc_dev) {
+		printk(KERN_ERR "ccdc already registered\n");
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	ccdc_dev = dev;
+unlock:
+	mutex_unlock(&ccdc_lock);
+	return ret;
+}
+EXPORT_SYMBOL(vpfe_register_ccdc_device);
+
+/*
+ * vpfe_unregister_ccdc_device. CCDC module calls this to
+ * unregister with vpfe capture
+ */
+void vpfe_unregister_ccdc_device(const struct ccdc_hw_device *dev)
+{
+	if (!dev) {
+		printk(KERN_ERR "invalid ccdc device ptr\n");
+		return;
+	}
+
+	printk(KERN_NOTICE "vpfe_unregister_ccdc_device, dev->name = %s\n",
+		dev->name);
+
+	if (strcmp(dev->name, ccdc_cfg->name)) {
+		/* ignore this ccdc */
+		return;
+	}
+
+	mutex_lock(&ccdc_lock);
+	ccdc_dev = NULL;
+	mutex_unlock(&ccdc_lock);
+}
+EXPORT_SYMBOL(vpfe_unregister_ccdc_device);
+
+/*
+ * vpfe_config_ccdc_image_format()
+ * For a pix format, configure ccdc to setup the capture
+ */
+static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frm_fmt = CCDC_FRMFMT_INTERLACED;
+	int ret = 0;
+
+	if (ccdc_dev->hw_ops.set_pixel_format(
+			vpfe_dev->fmt.fmt.pix.pixelformat) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"couldn't set pix format in ccdc\n");
+		return -EINVAL;
+	}
+	/* configure the image window */
+	ccdc_dev->hw_ops.set_image_window(&vpfe_dev->crop);
+
+	switch (vpfe_dev->fmt.fmt.pix.field) {
+	case V4L2_FIELD_INTERLACED:
+		/* do nothing, since it is default */
+		ret = ccdc_dev->hw_ops.set_buftype(
+				CCDC_BUFTYPE_FLD_INTERLEAVED);
+		break;
+	case V4L2_FIELD_NONE:
+		frm_fmt = CCDC_FRMFMT_PROGRESSIVE;
+		/* buffer type only applicable for interlaced scan */
+		break;
+	case V4L2_FIELD_SEQ_TB:
+		ret = ccdc_dev->hw_ops.set_buftype(
+				CCDC_BUFTYPE_FLD_SEPARATED);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set the frame format */
+	if (!ret)
+		ret = ccdc_dev->hw_ops.set_frame_format(frm_fmt);
+	return ret;
+}
+/*
+ * vpfe_config_image_format()
+ * For a given standard, this functions sets up the default
+ * pix format & crop values in the vpfe device and ccdc.  It first
+ * starts with defaults based values from the standard table.
+ * It then checks if sub device supports get_fmt and then override the
+ * values based on that.Sets crop values to match with scan resolution
+ * starting at 0,0. It calls vpfe_config_ccdc_image_format() set the
+ * values in ccdc
+ */
+static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
+				    v4l2_std_id std_id)
+{
+	struct vpfe_subdev_info *sdinfo = vpfe_dev->current_subdev;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
+	struct v4l2_pix_format *pix = &vpfe_dev->fmt.fmt.pix;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
+		if (vpfe_standards[i].std_id & std_id) {
+			vpfe_dev->std_info.active_pixels =
+					vpfe_standards[i].width;
+			vpfe_dev->std_info.active_lines =
+					vpfe_standards[i].height;
+			vpfe_dev->std_info.frame_format =
+					vpfe_standards[i].frame_format;
+			vpfe_dev->std_index = i;
+			break;
+		}
+	}
+
+	if (i ==  ARRAY_SIZE(vpfe_standards)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "standard not supported\n");
+		return -EINVAL;
+	}
+
+	vpfe_dev->crop.top = 0;
+	vpfe_dev->crop.left = 0;
+	vpfe_dev->crop.width = vpfe_dev->std_info.active_pixels;
+	vpfe_dev->crop.height = vpfe_dev->std_info.active_lines;
+	pix->width = vpfe_dev->crop.width;
+	pix->height = vpfe_dev->crop.height;
+
+	/* first field and frame format based on standard frame format */
+	if (vpfe_dev->std_info.frame_format) {
+		pix->field = V4L2_FIELD_INTERLACED;
+		/* assume V4L2_PIX_FMT_UYVY as default */
+		pix->pixelformat = V4L2_PIX_FMT_UYVY;
+		v4l2_fill_mbus_format(mbus_fmt, pix,
+				MEDIA_BUS_FMT_YUYV10_2X10);
+	} else {
+		pix->field = V4L2_FIELD_NONE;
+		/* assume V4L2_PIX_FMT_SBGGR8 */
+		pix->pixelformat = V4L2_PIX_FMT_SBGGR8;
+		v4l2_fill_mbus_format(mbus_fmt, pix,
+				MEDIA_BUS_FMT_SBGGR8_1X8);
+	}
+
+	/* if sub device supports get_fmt, override the defaults */
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev,
+			sdinfo->grp_id, pad, get_fmt, NULL, &fmt);
+
+	if (ret && ret != -ENOIOCTLCMD) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"error in getting get_fmt from sub device\n");
+		return ret;
+	}
+	v4l2_fill_pix_format(pix, mbus_fmt);
+	pix->bytesperline = pix->width * 2;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	/* Sets the values in CCDC */
+	ret = vpfe_config_ccdc_image_format(vpfe_dev);
+	if (ret)
+		return ret;
+
+	/* Update the values of sizeimage and bytesperline */
+	pix->bytesperline = ccdc_dev->hw_ops.get_line_length();
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return 0;
+}
+
+static int vpfe_initialize_device(struct vpfe_device *vpfe_dev)
+{
+	int ret;
+
+	/* set first input of current subdevice as the current input */
+	vpfe_dev->current_input = 0;
+
+	/* set default standard */
+	vpfe_dev->std_index = 0;
+
+	/* Configure the default format information */
+	ret = vpfe_config_image_format(vpfe_dev,
+				vpfe_standards[vpfe_dev->std_index].std_id);
+	if (ret)
+		return ret;
+
+	/* now open the ccdc device to initialize it */
+	mutex_lock(&ccdc_lock);
+	if (!ccdc_dev) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "ccdc device not registered\n");
+		ret = -ENODEV;
+		goto unlock;
+	}
+
+	if (!try_module_get(ccdc_dev->owner)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Couldn't lock ccdc module\n");
+		ret = -ENODEV;
+		goto unlock;
+	}
+	ret = ccdc_dev->hw_ops.open(vpfe_dev->pdev);
+	if (!ret)
+		vpfe_dev->initialized = 1;
+
+	/* Clear all VPFE/CCDC interrupts */
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(-1);
+
+unlock:
+	mutex_unlock(&ccdc_lock);
+	return ret;
+}
+
+/*
+ * vpfe_open : It creates object of file handle structure and
+ * stores it in private_data  member of filepointer
+ */
+static int vpfe_open(struct file *file)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	struct vpfe_fh *fh;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_open\n");
+
+	if (!vpfe_dev->cfg->num_subdevs) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "No decoder registered\n");
+		return -ENODEV;
+	}
+
+	/* Allocate memory for the file handle object */
+	fh = kmalloc(sizeof(*fh), GFP_KERNEL);
+	if (!fh)
+		return -ENOMEM;
+
+	/* store pointer to fh in private_data member of file */
+	file->private_data = fh;
+	fh->vpfe_dev = vpfe_dev;
+	v4l2_fh_init(&fh->fh, vdev);
+	mutex_lock(&vpfe_dev->lock);
+	/* If decoder is not initialized. initialize it */
+	if (!vpfe_dev->initialized) {
+		if (vpfe_initialize_device(vpfe_dev)) {
+			mutex_unlock(&vpfe_dev->lock);
+			v4l2_fh_exit(&fh->fh);
+			kfree(fh);
+			return -ENODEV;
+		}
+	}
+	/* Increment device usrs counter */
+	vpfe_dev->usrs++;
+	/* Set io_allowed member to false */
+	fh->io_allowed = 0;
+	v4l2_fh_add(&fh->fh);
+	mutex_unlock(&vpfe_dev->lock);
+	return 0;
+}
+
+static void vpfe_schedule_next_buffer(struct vpfe_device *vpfe_dev)
+{
+	unsigned long addr;
+
+	vpfe_dev->next_frm = list_entry(vpfe_dev->dma_queue.next,
+					struct videobuf_buffer, queue);
+	list_del(&vpfe_dev->next_frm->queue);
+	vpfe_dev->next_frm->state = VIDEOBUF_ACTIVE;
+	addr = videobuf_to_dma_contig(vpfe_dev->next_frm);
+
+	ccdc_dev->hw_ops.setfbaddr(addr);
+}
+
+static void vpfe_schedule_bottom_field(struct vpfe_device *vpfe_dev)
+{
+	unsigned long addr;
+
+	addr = videobuf_to_dma_contig(vpfe_dev->cur_frm);
+	addr += vpfe_dev->field_off;
+	ccdc_dev->hw_ops.setfbaddr(addr);
+}
+
+static void vpfe_process_buffer_complete(struct vpfe_device *vpfe_dev)
+{
+	vpfe_dev->cur_frm->ts = ktime_get_ns();
+	vpfe_dev->cur_frm->state = VIDEOBUF_DONE;
+	vpfe_dev->cur_frm->size = vpfe_dev->fmt.fmt.pix.sizeimage;
+	wake_up_interruptible(&vpfe_dev->cur_frm->done);
+	vpfe_dev->cur_frm = vpfe_dev->next_frm;
+}
+
+/* ISR for VINT0*/
+static irqreturn_t vpfe_isr(int irq, void *dev_id)
+{
+	struct vpfe_device *vpfe_dev = dev_id;
+	enum v4l2_field field;
+	int fid;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "\nStarting vpfe_isr...\n");
+	field = vpfe_dev->fmt.fmt.pix.field;
+
+	/* if streaming not started, don't do anything */
+	if (!vpfe_dev->started)
+		goto clear_intr;
+
+	/* only for 6446 this will be applicable */
+	if (ccdc_dev->hw_ops.reset)
+		ccdc_dev->hw_ops.reset();
+
+	if (field == V4L2_FIELD_NONE) {
+		/* handle progressive frame capture */
+		v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+			"frame format is progressive...\n");
+		if (vpfe_dev->cur_frm != vpfe_dev->next_frm)
+			vpfe_process_buffer_complete(vpfe_dev);
+		goto clear_intr;
+	}
+
+	/* interlaced or TB capture check which field we are in hardware */
+	fid = ccdc_dev->hw_ops.getfid();
+
+	/* switch the software maintained field id */
+	vpfe_dev->field_id ^= 1;
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "field id = %x:%x.\n",
+		fid, vpfe_dev->field_id);
+	if (fid == vpfe_dev->field_id) {
+		/* we are in-sync here,continue */
+		if (fid == 0) {
+			/*
+			 * One frame is just being captured. If the next frame
+			 * is available, release the current frame and move on
+			 */
+			if (vpfe_dev->cur_frm != vpfe_dev->next_frm)
+				vpfe_process_buffer_complete(vpfe_dev);
+			/*
+			 * based on whether the two fields are stored
+			 * interleavely or separately in memory, reconfigure
+			 * the CCDC memory address
+			 */
+			if (field == V4L2_FIELD_SEQ_TB)
+				vpfe_schedule_bottom_field(vpfe_dev);
+			goto clear_intr;
+		}
+		/*
+		 * if one field is just being captured configure
+		 * the next frame get the next frame from the empty
+		 * queue if no frame is available hold on to the
+		 * current buffer
+		 */
+		spin_lock(&vpfe_dev->dma_queue_lock);
+		if (!list_empty(&vpfe_dev->dma_queue) &&
+		    vpfe_dev->cur_frm == vpfe_dev->next_frm)
+			vpfe_schedule_next_buffer(vpfe_dev);
+		spin_unlock(&vpfe_dev->dma_queue_lock);
+	} else if (fid == 0) {
+		/*
+		 * out of sync. Recover from any hardware out-of-sync.
+		 * May loose one frame
+		 */
+		vpfe_dev->field_id = fid;
+	}
+clear_intr:
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(irq);
+
+	return IRQ_HANDLED;
+}
+
+/* vdint1_isr - isr handler for VINT1 interrupt */
+static irqreturn_t vdint1_isr(int irq, void *dev_id)
+{
+	struct vpfe_device *vpfe_dev = dev_id;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "\nInside vdint1_isr...\n");
+
+	/* if streaming not started, don't do anything */
+	if (!vpfe_dev->started) {
+		if (vpfe_dev->cfg->clr_intr)
+			vpfe_dev->cfg->clr_intr(irq);
+		return IRQ_HANDLED;
+	}
+
+	spin_lock(&vpfe_dev->dma_queue_lock);
+	if ((vpfe_dev->fmt.fmt.pix.field == V4L2_FIELD_NONE) &&
+	    !list_empty(&vpfe_dev->dma_queue) &&
+	    vpfe_dev->cur_frm == vpfe_dev->next_frm)
+		vpfe_schedule_next_buffer(vpfe_dev);
+	spin_unlock(&vpfe_dev->dma_queue_lock);
+
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void vpfe_detach_irq(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = ccdc_dev->hw_ops.get_frame_format();
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE)
+		free_irq(vpfe_dev->ccdc_irq1, vpfe_dev);
+}
+
+static int vpfe_attach_irq(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = ccdc_dev->hw_ops.get_frame_format();
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE) {
+		return request_irq(vpfe_dev->ccdc_irq1, vdint1_isr,
+				    0, "vpfe_capture1",
+				    vpfe_dev);
+	}
+	return 0;
+}
+
+/* vpfe_stop_ccdc_capture: stop streaming in ccdc/isif */
+static void vpfe_stop_ccdc_capture(struct vpfe_device *vpfe_dev)
+{
+	vpfe_dev->started = 0;
+	ccdc_dev->hw_ops.enable(0);
+	if (ccdc_dev->hw_ops.enable_out_to_sdram)
+		ccdc_dev->hw_ops.enable_out_to_sdram(0);
+}
+
+/*
+ * vpfe_release : This function deletes buffer queue, frees the
+ * buffers and the vpfe file  handle
+ */
+static int vpfe_release(struct file *file)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_release\n");
+
+	/* Get the device lock */
+	mutex_lock(&vpfe_dev->lock);
+	/* if this instance is doing IO */
+	if (fh->io_allowed) {
+		if (vpfe_dev->started) {
+			sdinfo = vpfe_dev->current_subdev;
+			ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev,
+							 sdinfo->grp_id,
+							 video, s_stream, 0);
+			if (ret && (ret != -ENOIOCTLCMD))
+				v4l2_err(&vpfe_dev->v4l2_dev,
+				"stream off failed in subdev\n");
+			vpfe_stop_ccdc_capture(vpfe_dev);
+			vpfe_detach_irq(vpfe_dev);
+			videobuf_streamoff(&vpfe_dev->buffer_queue);
+		}
+		vpfe_dev->io_usrs = 0;
+		vpfe_dev->numbuffers = config_params.numbuffers;
+		videobuf_stop(&vpfe_dev->buffer_queue);
+		videobuf_mmap_free(&vpfe_dev->buffer_queue);
+	}
+
+	/* Decrement device usrs counter */
+	vpfe_dev->usrs--;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	/* If this is the last file handle */
+	if (!vpfe_dev->usrs) {
+		vpfe_dev->initialized = 0;
+		if (ccdc_dev->hw_ops.close)
+			ccdc_dev->hw_ops.close(vpfe_dev->pdev);
+		module_put(ccdc_dev->owner);
+	}
+	mutex_unlock(&vpfe_dev->lock);
+	file->private_data = NULL;
+	/* Free memory allocated to file handle object */
+	kfree(fh);
+	return 0;
+}
+
+/*
+ * vpfe_mmap : It is used to map kernel space buffers
+ * into user spaces
+ */
+static int vpfe_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	/* Get the device object and file handle object */
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_mmap\n");
+
+	return videobuf_mmap_mapper(&vpfe_dev->buffer_queue, vma);
+}
+
+/*
+ * vpfe_poll: It is used for select/poll system call
+ */
+static __poll_t vpfe_poll(struct file *file, poll_table *wait)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_poll\n");
+
+	if (vpfe_dev->started)
+		return videobuf_poll_stream(file,
+					    &vpfe_dev->buffer_queue, wait);
+	return 0;
+}
+
+/* vpfe capture driver file operations */
+static const struct v4l2_file_operations vpfe_fops = {
+	.owner = THIS_MODULE,
+	.open = vpfe_open,
+	.release = vpfe_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vpfe_mmap,
+	.poll = vpfe_poll
+};
+
+/*
+ * vpfe_check_format()
+ * This function adjust the input pixel format as per hardware
+ * capabilities and update the same in pixfmt.
+ * Following algorithm used :-
+ *
+ *	If given pixformat is not in the vpfe list of pix formats or not
+ *	supported by the hardware, current value of pixformat in the device
+ *	is used
+ *	If given field is not supported, then current field is used. If field
+ *	is different from current, then it is matched with that from sub device.
+ *	Minimum height is 2 lines for interlaced or tb field and 1 line for
+ *	progressive. Maximum height is clamped to active active lines of scan
+ *	Minimum width is 32 bytes in memory and width is clamped to active
+ *	pixels of scan.
+ *	bytesperline is a multiple of 32.
+ */
+static const struct vpfe_pixel_format *
+	vpfe_check_format(struct vpfe_device *vpfe_dev,
+			  struct v4l2_pix_format *pixfmt)
+{
+	u32 min_height = 1, min_width = 32, max_width, max_height;
+	const struct vpfe_pixel_format *vpfe_pix_fmt;
+	u32 pix;
+	int temp, found;
+
+	vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	if (!vpfe_pix_fmt) {
+		/*
+		 * use current pixel format in the vpfe device. We
+		 * will find this pix format in the table
+		 */
+		pixfmt->pixelformat = vpfe_dev->fmt.fmt.pix.pixelformat;
+		vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	}
+
+	/* check if hw supports it */
+	temp = 0;
+	found = 0;
+	while (ccdc_dev->hw_ops.enum_pix(&pix, temp) >= 0) {
+		if (vpfe_pix_fmt->pixelformat == pix) {
+			found = 1;
+			break;
+		}
+		temp++;
+	}
+
+	if (!found) {
+		/* use current pixel format */
+		pixfmt->pixelformat = vpfe_dev->fmt.fmt.pix.pixelformat;
+		/*
+		 * Since this is currently used in the vpfe device, we
+		 * will find this pix format in the table
+		 */
+		vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	}
+
+	/* check what field format is supported */
+	if (pixfmt->field == V4L2_FIELD_ANY) {
+		/* if field is any, use current value as default */
+		pixfmt->field = vpfe_dev->fmt.fmt.pix.field;
+	}
+
+	/*
+	 * if field is not same as current field in the vpfe device
+	 * try matching the field with the sub device field
+	 */
+	if (vpfe_dev->fmt.fmt.pix.field != pixfmt->field) {
+		/*
+		 * If field value is not in the supported fields, use current
+		 * field used in the device as default
+		 */
+		switch (pixfmt->field) {
+		case V4L2_FIELD_INTERLACED:
+		case V4L2_FIELD_SEQ_TB:
+			/* if sub device is supporting progressive, use that */
+			if (!vpfe_dev->std_info.frame_format)
+				pixfmt->field = V4L2_FIELD_NONE;
+			break;
+		case V4L2_FIELD_NONE:
+			if (vpfe_dev->std_info.frame_format)
+				pixfmt->field = V4L2_FIELD_INTERLACED;
+			break;
+
+		default:
+			/* use current field as default */
+			pixfmt->field = vpfe_dev->fmt.fmt.pix.field;
+			break;
+		}
+	}
+
+	/* Now adjust image resolutions supported */
+	if (pixfmt->field == V4L2_FIELD_INTERLACED ||
+	    pixfmt->field == V4L2_FIELD_SEQ_TB)
+		min_height = 2;
+
+	max_width = vpfe_dev->std_info.active_pixels;
+	max_height = vpfe_dev->std_info.active_lines;
+	min_width /= vpfe_pix_fmt->bpp;
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "width = %d, height = %d, bpp = %d\n",
+		  pixfmt->width, pixfmt->height, vpfe_pix_fmt->bpp);
+
+	pixfmt->width = clamp((pixfmt->width), min_width, max_width);
+	pixfmt->height = clamp((pixfmt->height), min_height, max_height);
+
+	/* If interlaced, adjust height to be a multiple of 2 */
+	if (pixfmt->field == V4L2_FIELD_INTERLACED)
+		pixfmt->height &= (~1);
+	/*
+	 * recalculate bytesperline and sizeimage since width
+	 * and height might have changed
+	 */
+	pixfmt->bytesperline = (((pixfmt->width * vpfe_pix_fmt->bpp) + 31)
+				& ~31);
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+		pixfmt->sizeimage =
+			pixfmt->bytesperline * pixfmt->height +
+			((pixfmt->bytesperline * pixfmt->height) >> 1);
+	else
+		pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "adjusted width = %d, height = %d, bpp = %d, bytesperline = %d, sizeimage = %d\n",
+		 pixfmt->width, pixfmt->height, vpfe_pix_fmt->bpp,
+		 pixfmt->bytesperline, pixfmt->sizeimage);
+	return vpfe_pix_fmt;
+}
+
+static int vpfe_querycap(struct file *file, void  *priv,
+			       struct v4l2_capability *cap)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querycap\n");
+
+	strscpy(cap->driver, CAPTURE_DRV_NAME, sizeof(cap->driver));
+	strscpy(cap->bus_info, "VPFE", sizeof(cap->bus_info));
+	strscpy(cap->card, vpfe_dev->cfg->card_name, sizeof(cap->card));
+	return 0;
+}
+
+static int vpfe_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_fmt_vid_cap\n");
+	/* Fill in the information about format */
+	*fmt = vpfe_dev->fmt;
+	return 0;
+}
+
+static int vpfe_enum_fmt_vid_cap(struct file *file, void  *priv,
+				   struct v4l2_fmtdesc *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmt;
+	u32 pix;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_fmt_vid_cap\n");
+
+	if (ccdc_dev->hw_ops.enum_pix(&pix, fmt->index) < 0)
+		return -EINVAL;
+
+	/* Fill in the information about format */
+	pix_fmt = vpfe_lookup_pix_format(pix);
+	if (pix_fmt) {
+		fmt->pixelformat = pix_fmt->pixelformat;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int vpfe_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmts;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_fmt_vid_cap\n");
+
+	/* If streaming is started, return error */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Streaming is started\n");
+		return -EBUSY;
+	}
+
+	/* Check for valid frame format */
+	pix_fmts = vpfe_check_format(vpfe_dev, &fmt->fmt.pix);
+	if (!pix_fmts)
+		return -EINVAL;
+
+	/* store the pixel format in the device  object */
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	/* First detach any IRQ if currently attached */
+	vpfe_detach_irq(vpfe_dev);
+	vpfe_dev->fmt = *fmt;
+	/* set image capture parameters in the ccdc */
+	ret = vpfe_config_ccdc_image_format(vpfe_dev);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmts;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_try_fmt_vid_cap\n");
+
+	pix_fmts = vpfe_check_format(vpfe_dev, &f->fmt.pix);
+	if (!pix_fmts)
+		return -EINVAL;
+	return 0;
+}
+
+/*
+ * vpfe_get_subdev_input_index - Get subdev index and subdev input index for a
+ * given app input index
+ */
+static int vpfe_get_subdev_input_index(struct vpfe_device *vpfe_dev,
+					int *subdev_index,
+					int *subdev_input_index,
+					int app_input_index)
+{
+	struct vpfe_config *cfg = vpfe_dev->cfg;
+	struct vpfe_subdev_info *sdinfo;
+	int i, j = 0;
+
+	for (i = 0; i < cfg->num_subdevs; i++) {
+		sdinfo = &cfg->sub_devs[i];
+		if (app_input_index < (j + sdinfo->num_inputs)) {
+			*subdev_index = i;
+			*subdev_input_index = app_input_index - j;
+			return 0;
+		}
+		j += sdinfo->num_inputs;
+	}
+	return -EINVAL;
+}
+
+/*
+ * vpfe_get_app_input - Get app input index for a given subdev input index
+ * driver stores the input index of the current sub device and translate it
+ * when application request the current input
+ */
+static int vpfe_get_app_input_index(struct vpfe_device *vpfe_dev,
+				    int *app_input_index)
+{
+	struct vpfe_config *cfg = vpfe_dev->cfg;
+	struct vpfe_subdev_info *sdinfo;
+	int i, j = 0;
+
+	for (i = 0; i < cfg->num_subdevs; i++) {
+		sdinfo = &cfg->sub_devs[i];
+		if (!strcmp(sdinfo->name, vpfe_dev->current_subdev->name)) {
+			if (vpfe_dev->current_input >= sdinfo->num_inputs)
+				return -1;
+			*app_input_index = j + vpfe_dev->current_input;
+			return 0;
+		}
+		j += sdinfo->num_inputs;
+	}
+	return -EINVAL;
+}
+
+static int vpfe_enum_input(struct file *file, void *priv,
+				 struct v4l2_input *inp)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int subdev, index ;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_input\n");
+
+	if (vpfe_get_subdev_input_index(vpfe_dev,
+					&subdev,
+					&index,
+					inp->index) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "input information not found for the subdev\n");
+		return -EINVAL;
+	}
+	sdinfo = &vpfe_dev->cfg->sub_devs[subdev];
+	*inp = sdinfo->inputs[index];
+	return 0;
+}
+
+static int vpfe_g_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_input\n");
+
+	return vpfe_get_app_input_index(vpfe_dev, index);
+}
+
+
+static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_subdev *sd;
+	struct vpfe_subdev_info *sdinfo;
+	int subdev_index, inp_index;
+	struct vpfe_route *route;
+	u32 input, output;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_input\n");
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	/*
+	 * If streaming is started return device busy
+	 * error
+	 */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Streaming is on\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+	ret = vpfe_get_subdev_input_index(vpfe_dev,
+					  &subdev_index,
+					  &inp_index,
+					  index);
+	if (ret < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "invalid input index\n");
+		goto unlock_out;
+	}
+
+	sdinfo = &vpfe_dev->cfg->sub_devs[subdev_index];
+	sd = vpfe_dev->sd[subdev_index];
+	route = &sdinfo->routes[inp_index];
+	if (route && sdinfo->can_route) {
+		input = route->input;
+		output = route->output;
+	} else {
+		input = 0;
+		output = 0;
+	}
+
+	if (sd)
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+
+	if (ret) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"vpfe_doioctl:error in setting input in decoder\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	vpfe_dev->current_subdev = sdinfo;
+	if (sd)
+		vpfe_dev->v4l2_dev.ctrl_handler = sd->ctrl_handler;
+	vpfe_dev->current_input = index;
+	vpfe_dev->std_index = 0;
+
+	/* set the bus/interface parameter for the sub device in ccdc */
+	ret = ccdc_dev->hw_ops.set_hw_if_params(&sdinfo->ccdc_if_params);
+	if (ret)
+		goto unlock_out;
+
+	/* set the default image parameters in the device */
+	ret = vpfe_config_image_format(vpfe_dev,
+				vpfe_standards[vpfe_dev->std_index].std_id);
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querystd\n");
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	sdinfo = vpfe_dev->current_subdev;
+	if (ret)
+		return ret;
+	/* Call querystd function of decoder device */
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					 video, querystd, std_id);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_std\n");
+
+	/* Call decoder driver function to set the standard */
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	sdinfo = vpfe_dev->current_subdev;
+	/* If streaming is started, return device busy error */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "streaming is started\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					 video, s_std, std_id);
+	if (ret < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
+		goto unlock_out;
+	}
+	ret = vpfe_config_image_format(vpfe_dev, std_id);
+
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_g_std(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_std\n");
+
+	*std_id = vpfe_standards[vpfe_dev->std_index].std_id;
+	return 0;
+}
+/*
+ *  Videobuf operations
+ */
+static int vpfe_videobuf_setup(struct videobuf_queue *vq,
+				unsigned int *count,
+				unsigned int *size)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_setup\n");
+	*size = vpfe_dev->fmt.fmt.pix.sizeimage;
+	if (vpfe_dev->memory == V4L2_MEMORY_MMAP &&
+		vpfe_dev->fmt.fmt.pix.sizeimage > config_params.device_bufsize)
+		*size = config_params.device_bufsize;
+
+	if (*count < config_params.min_numbuffers)
+		*count = config_params.min_numbuffers;
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"count=%d, size=%d\n", *count, *size);
+	return 0;
+}
+
+static int vpfe_videobuf_prepare(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb,
+				enum v4l2_field field)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long addr;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_prepare\n");
+
+	/* If buffer is not initialized, initialize it */
+	if (VIDEOBUF_NEEDS_INIT == vb->state) {
+		vb->width = vpfe_dev->fmt.fmt.pix.width;
+		vb->height = vpfe_dev->fmt.fmt.pix.height;
+		vb->size = vpfe_dev->fmt.fmt.pix.sizeimage;
+		vb->field = field;
+
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret < 0)
+			return ret;
+
+		addr = videobuf_to_dma_contig(vb);
+		/* Make sure user addresses are aligned to 32 bytes */
+		if (!ALIGN(addr, 32))
+			return -EINVAL;
+
+		vb->state = VIDEOBUF_PREPARED;
+	}
+	return 0;
+}
+
+static void vpfe_videobuf_queue(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb)
+{
+	/* Get the file handle object and device object */
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long flags;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_queue\n");
+
+	/* add the buffer to the DMA queue */
+	spin_lock_irqsave(&vpfe_dev->dma_queue_lock, flags);
+	list_add_tail(&vb->queue, &vpfe_dev->dma_queue);
+	spin_unlock_irqrestore(&vpfe_dev->dma_queue_lock, flags);
+
+	/* Change state of the buffer */
+	vb->state = VIDEOBUF_QUEUED;
+}
+
+static void vpfe_videobuf_release(struct videobuf_queue *vq,
+				  struct videobuf_buffer *vb)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long flags;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_videobuf_release\n");
+
+	/*
+	 * We need to flush the buffer from the dma queue since
+	 * they are de-allocated
+	 */
+	spin_lock_irqsave(&vpfe_dev->dma_queue_lock, flags);
+	INIT_LIST_HEAD(&vpfe_dev->dma_queue);
+	spin_unlock_irqrestore(&vpfe_dev->dma_queue_lock, flags);
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static const struct videobuf_queue_ops vpfe_videobuf_qops = {
+	.buf_setup      = vpfe_videobuf_setup,
+	.buf_prepare    = vpfe_videobuf_prepare,
+	.buf_queue      = vpfe_videobuf_queue,
+	.buf_release    = vpfe_videobuf_release,
+};
+
+/*
+ * vpfe_reqbufs. currently support REQBUF only once opening
+ * the device.
+ */
+static int vpfe_reqbufs(struct file *file, void *priv,
+			struct v4l2_requestbuffers *req_buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_reqbufs\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != req_buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buffer type\n");
+		return -EINVAL;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	if (vpfe_dev->io_usrs != 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Only one IO user allowed\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+
+	vpfe_dev->memory = req_buf->memory;
+	videobuf_queue_dma_contig_init(&vpfe_dev->buffer_queue,
+				&vpfe_videobuf_qops,
+				vpfe_dev->pdev,
+				&vpfe_dev->irqlock,
+				req_buf->type,
+				vpfe_dev->fmt.fmt.pix.field,
+				sizeof(struct videobuf_buffer),
+				fh, NULL);
+
+	fh->io_allowed = 1;
+	vpfe_dev->io_usrs = 1;
+	INIT_LIST_HEAD(&vpfe_dev->dma_queue);
+	ret = videobuf_reqbufs(&vpfe_dev->buffer_queue, req_buf);
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_querybuf(struct file *file, void *priv,
+			 struct v4l2_buffer *buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querybuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return  -EINVAL;
+	}
+
+	if (vpfe_dev->memory != V4L2_MEMORY_MMAP) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid memory\n");
+		return -EINVAL;
+	}
+	/* Call videobuf_querybuf to get information */
+	return videobuf_querybuf(&vpfe_dev->buffer_queue, buf);
+}
+
+static int vpfe_qbuf(struct file *file, void *priv,
+		     struct v4l2_buffer *p)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_qbuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != p->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * If this file handle is not allowed to do IO,
+	 * return error
+	 */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+	return videobuf_qbuf(&vpfe_dev->buffer_queue, p);
+}
+
+static int vpfe_dqbuf(struct file *file, void *priv,
+		      struct v4l2_buffer *buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_dqbuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+	return videobuf_dqbuf(&vpfe_dev->buffer_queue,
+				      buf, file->f_flags & O_NONBLOCK);
+}
+
+/*
+ * vpfe_calculate_offsets : This function calculates buffers offset
+ * for top and bottom field
+ */
+static void vpfe_calculate_offsets(struct vpfe_device *vpfe_dev)
+{
+	struct v4l2_rect image_win;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_calculate_offsets\n");
+
+	ccdc_dev->hw_ops.get_image_window(&image_win);
+	vpfe_dev->field_off = image_win.height * image_win.width;
+}
+
+/* vpfe_start_ccdc_capture: start streaming in ccdc/isif */
+static void vpfe_start_ccdc_capture(struct vpfe_device *vpfe_dev)
+{
+	ccdc_dev->hw_ops.enable(1);
+	if (ccdc_dev->hw_ops.enable_out_to_sdram)
+		ccdc_dev->hw_ops.enable_out_to_sdram(1);
+	vpfe_dev->started = 1;
+}
+
+/*
+ * vpfe_streamon. Assume the DMA queue is not empty.
+ * application is expected to call QBUF before calling
+ * this ioctl. If not, driver returns error
+ */
+static int vpfe_streamon(struct file *file, void *priv,
+			 enum v4l2_buf_type buf_type)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	unsigned long addr;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_streamon\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf_type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/* If file handle is not allowed IO, return error */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+
+	sdinfo = vpfe_dev->current_subdev;
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					video, s_stream, 1);
+
+	if (ret && (ret != -ENOIOCTLCMD)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "stream on failed in subdev\n");
+		return -EINVAL;
+	}
+
+	/* If buffer queue is empty, return error */
+	if (list_empty(&vpfe_dev->buffer_queue.stream)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "buffer queue is empty\n");
+		return -EIO;
+	}
+
+	/* Call videobuf_streamon to start streaming * in videobuf */
+	ret = videobuf_streamon(&vpfe_dev->buffer_queue);
+	if (ret)
+		return ret;
+
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		goto streamoff;
+	/* Get the next frame from the buffer queue */
+	vpfe_dev->next_frm = list_entry(vpfe_dev->dma_queue.next,
+					struct videobuf_buffer, queue);
+	vpfe_dev->cur_frm = vpfe_dev->next_frm;
+	/* Remove buffer from the buffer queue */
+	list_del(&vpfe_dev->cur_frm->queue);
+	/* Mark state of the current frame to active */
+	vpfe_dev->cur_frm->state = VIDEOBUF_ACTIVE;
+	/* Initialize field_id and started member */
+	vpfe_dev->field_id = 0;
+	addr = videobuf_to_dma_contig(vpfe_dev->cur_frm);
+
+	/* Calculate field offset */
+	vpfe_calculate_offsets(vpfe_dev);
+
+	if (vpfe_attach_irq(vpfe_dev) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			 "Error in attaching interrupt handle\n");
+		ret = -EFAULT;
+		goto unlock_out;
+	}
+	if (ccdc_dev->hw_ops.configure() < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			 "Error in configuring ccdc\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	ccdc_dev->hw_ops.setfbaddr((unsigned long)(addr));
+	vpfe_start_ccdc_capture(vpfe_dev);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+streamoff:
+	videobuf_streamoff(&vpfe_dev->buffer_queue);
+	return ret;
+}
+
+static int vpfe_streamoff(struct file *file, void *priv,
+			  enum v4l2_buf_type buf_type)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_streamoff\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf_type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/* If io is allowed for this file handle, return error */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+
+	/* If streaming is not started, return error */
+	if (!vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "device started\n");
+		return -EINVAL;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	vpfe_stop_ccdc_capture(vpfe_dev);
+	vpfe_detach_irq(vpfe_dev);
+
+	sdinfo = vpfe_dev->current_subdev;
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					video, s_stream, 0);
+
+	if (ret && (ret != -ENOIOCTLCMD))
+		v4l2_err(&vpfe_dev->v4l2_dev, "stream off failed in subdev\n");
+	ret = videobuf_streamoff(&vpfe_dev->buffer_queue);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_g_pixelaspect(struct file *file, void *priv,
+			      int type, struct v4l2_fract *f)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_pixelaspect\n");
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	/* If std_index is invalid, then just return (== 1:1 aspect) */
+	if (vpfe_dev->std_index >= ARRAY_SIZE(vpfe_standards))
+		return 0;
+
+	*f = vpfe_standards[vpfe_dev->std_index].pixelaspect;
+	return 0;
+}
+
+static int vpfe_g_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_selection\n");
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = vpfe_dev->crop;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.width = vpfe_standards[vpfe_dev->std_index].width;
+		sel->r.height = vpfe_standards[vpfe_dev->std_index].height;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vpfe_s_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_rect rect = sel->r;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_selection\n");
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	if (vpfe_dev->started) {
+		/* make sure streaming is not started */
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"Cannot change crop when streaming is ON\n");
+		return -EBUSY;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	if (rect.top < 0 || rect.left < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"doesn't support negative values for top & left\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+
+	/* adjust the width to 16 pixel boundary */
+	rect.width = ((rect.width + 15) & ~0xf);
+
+	/* make sure parameters are valid */
+	if ((rect.left + rect.width >
+		vpfe_dev->std_info.active_pixels) ||
+	    (rect.top + rect.height >
+		vpfe_dev->std_info.active_lines)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_SELECTION params\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	ccdc_dev->hw_ops.set_image_window(&rect);
+	vpfe_dev->fmt.fmt.pix.width = rect.width;
+	vpfe_dev->fmt.fmt.pix.height = rect.height;
+	vpfe_dev->fmt.fmt.pix.bytesperline =
+		ccdc_dev->hw_ops.get_line_length();
+	vpfe_dev->fmt.fmt.pix.sizeimage =
+		vpfe_dev->fmt.fmt.pix.bytesperline *
+		vpfe_dev->fmt.fmt.pix.height;
+	vpfe_dev->crop = rect;
+	sel->r = rect;
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+/* vpfe capture ioctl operations */
+static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
+	.vidioc_querycap	 = vpfe_querycap,
+	.vidioc_g_fmt_vid_cap    = vpfe_g_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap = vpfe_enum_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap    = vpfe_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap  = vpfe_try_fmt_vid_cap,
+	.vidioc_enum_input	 = vpfe_enum_input,
+	.vidioc_g_input		 = vpfe_g_input,
+	.vidioc_s_input		 = vpfe_s_input,
+	.vidioc_querystd	 = vpfe_querystd,
+	.vidioc_s_std		 = vpfe_s_std,
+	.vidioc_g_std		 = vpfe_g_std,
+	.vidioc_reqbufs		 = vpfe_reqbufs,
+	.vidioc_querybuf	 = vpfe_querybuf,
+	.vidioc_qbuf		 = vpfe_qbuf,
+	.vidioc_dqbuf		 = vpfe_dqbuf,
+	.vidioc_streamon	 = vpfe_streamon,
+	.vidioc_streamoff	 = vpfe_streamoff,
+	.vidioc_g_pixelaspect	 = vpfe_g_pixelaspect,
+	.vidioc_g_selection	 = vpfe_g_selection,
+	.vidioc_s_selection	 = vpfe_s_selection,
+};
+
+static struct vpfe_device *vpfe_initialize(void)
+{
+	struct vpfe_device *vpfe_dev;
+
+	/* Default number of buffers should be 3 */
+	if ((numbuffers > 0) &&
+	    (numbuffers < config_params.min_numbuffers))
+		numbuffers = config_params.min_numbuffers;
+
+	/*
+	 * Set buffer size to min buffers size if invalid buffer size is
+	 * given
+	 */
+	if (bufsize < config_params.min_bufsize)
+		bufsize = config_params.min_bufsize;
+
+	config_params.numbuffers = numbuffers;
+
+	if (numbuffers)
+		config_params.device_bufsize = bufsize;
+
+	/* Allocate memory for device objects */
+	vpfe_dev = kzalloc(sizeof(*vpfe_dev), GFP_KERNEL);
+
+	return vpfe_dev;
+}
+
+/*
+ * vpfe_probe : This function creates device entries by register
+ * itself to the V4L2 driver and initializes fields of each
+ * device objects
+ */
+static int vpfe_probe(struct platform_device *pdev)
+{
+	struct vpfe_subdev_info *sdinfo;
+	struct vpfe_config *vpfe_cfg;
+	struct resource *res1;
+	struct vpfe_device *vpfe_dev;
+	struct i2c_adapter *i2c_adap;
+	struct video_device *vfd;
+	int ret, i, j;
+	int num_subdevs = 0;
+
+	/* Get the pointer to the device object */
+	vpfe_dev = vpfe_initialize();
+
+	if (!vpfe_dev) {
+		v4l2_err(pdev->dev.driver,
+			"Failed to allocate memory for vpfe_dev\n");
+		return -ENOMEM;
+	}
+
+	vpfe_dev->pdev = &pdev->dev;
+
+	if (!pdev->dev.platform_data) {
+		v4l2_err(pdev->dev.driver, "Unable to get vpfe config\n");
+		ret = -ENODEV;
+		goto probe_free_dev_mem;
+	}
+
+	vpfe_cfg = pdev->dev.platform_data;
+	vpfe_dev->cfg = vpfe_cfg;
+	if (!vpfe_cfg->ccdc || !vpfe_cfg->card_name || !vpfe_cfg->sub_devs) {
+		v4l2_err(pdev->dev.driver, "null ptr in vpfe_cfg\n");
+		ret = -ENOENT;
+		goto probe_free_dev_mem;
+	}
+
+	/* Allocate memory for ccdc configuration */
+	ccdc_cfg = kmalloc(sizeof(*ccdc_cfg), GFP_KERNEL);
+	if (!ccdc_cfg) {
+		ret = -ENOMEM;
+		goto probe_free_dev_mem;
+	}
+
+	mutex_lock(&ccdc_lock);
+
+	strscpy(ccdc_cfg->name, vpfe_cfg->ccdc, sizeof(ccdc_cfg->name));
+	/* Get VINT0 irq resource */
+	res1 = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res1) {
+		v4l2_err(pdev->dev.driver,
+			 "Unable to get interrupt for VINT0\n");
+		ret = -ENODEV;
+		goto probe_free_ccdc_cfg_mem;
+	}
+	vpfe_dev->ccdc_irq0 = res1->start;
+
+	/* Get VINT1 irq resource */
+	res1 = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!res1) {
+		v4l2_err(pdev->dev.driver,
+			 "Unable to get interrupt for VINT1\n");
+		ret = -ENODEV;
+		goto probe_free_ccdc_cfg_mem;
+	}
+	vpfe_dev->ccdc_irq1 = res1->start;
+
+	ret = request_irq(vpfe_dev->ccdc_irq0, vpfe_isr, 0,
+			  "vpfe_capture0", vpfe_dev);
+
+	if (0 != ret) {
+		v4l2_err(pdev->dev.driver, "Unable to request interrupt\n");
+		goto probe_free_ccdc_cfg_mem;
+	}
+
+	vfd = &vpfe_dev->video_dev;
+	/* Initialize field of video device */
+	vfd->release		= video_device_release_empty;
+	vfd->fops		= &vpfe_fops;
+	vfd->ioctl_ops		= &vpfe_ioctl_ops;
+	vfd->tvnorms		= 0;
+	vfd->v4l2_dev		= &vpfe_dev->v4l2_dev;
+	vfd->device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	snprintf(vfd->name, sizeof(vfd->name),
+		 "%s_V%d.%d.%d",
+		 CAPTURE_DRV_NAME,
+		 (VPFE_CAPTURE_VERSION_CODE >> 16) & 0xff,
+		 (VPFE_CAPTURE_VERSION_CODE >> 8) & 0xff,
+		 (VPFE_CAPTURE_VERSION_CODE) & 0xff);
+
+	ret = v4l2_device_register(&pdev->dev, &vpfe_dev->v4l2_dev);
+	if (ret) {
+		v4l2_err(pdev->dev.driver,
+			"Unable to register v4l2 device.\n");
+		goto probe_out_release_irq;
+	}
+	v4l2_info(&vpfe_dev->v4l2_dev, "v4l2 device registered\n");
+	spin_lock_init(&vpfe_dev->irqlock);
+	spin_lock_init(&vpfe_dev->dma_queue_lock);
+	mutex_init(&vpfe_dev->lock);
+
+	/* Initialize field of the device objects */
+	vpfe_dev->numbuffers = config_params.numbuffers;
+
+	/* register video device */
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"trying to register vpfe device.\n");
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"video_dev=%p\n", &vpfe_dev->video_dev);
+	vpfe_dev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ret = video_register_device(&vpfe_dev->video_dev,
+				    VFL_TYPE_VIDEO, -1);
+
+	if (ret) {
+		v4l2_err(pdev->dev.driver,
+			"Unable to register video device.\n");
+		goto probe_out_v4l2_unregister;
+	}
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "video device registered\n");
+	/* set the driver data in platform device */
+	platform_set_drvdata(pdev, vpfe_dev);
+	/* set driver private data */
+	video_set_drvdata(&vpfe_dev->video_dev, vpfe_dev);
+	i2c_adap = i2c_get_adapter(vpfe_cfg->i2c_adapter_id);
+	num_subdevs = vpfe_cfg->num_subdevs;
+	vpfe_dev->sd = kmalloc_array(num_subdevs,
+				     sizeof(*vpfe_dev->sd),
+				     GFP_KERNEL);
+	if (!vpfe_dev->sd) {
+		ret = -ENOMEM;
+		goto probe_out_video_unregister;
+	}
+
+	for (i = 0; i < num_subdevs; i++) {
+		struct v4l2_input *inps;
+
+		sdinfo = &vpfe_cfg->sub_devs[i];
+
+		/* Load up the subdevice */
+		vpfe_dev->sd[i] =
+			v4l2_i2c_new_subdev_board(&vpfe_dev->v4l2_dev,
+						  i2c_adap,
+						  &sdinfo->board_info,
+						  NULL);
+		if (vpfe_dev->sd[i]) {
+			v4l2_info(&vpfe_dev->v4l2_dev,
+				  "v4l2 sub device %s registered\n",
+				  sdinfo->name);
+			vpfe_dev->sd[i]->grp_id = sdinfo->grp_id;
+			/* update tvnorms from the sub devices */
+			for (j = 0; j < sdinfo->num_inputs; j++) {
+				inps = &sdinfo->inputs[j];
+				vfd->tvnorms |= inps->std;
+			}
+		} else {
+			v4l2_info(&vpfe_dev->v4l2_dev,
+				  "v4l2 sub device %s register fails\n",
+				  sdinfo->name);
+			ret = -ENXIO;
+			goto probe_sd_out;
+		}
+	}
+
+	/* set first sub device as current one */
+	vpfe_dev->current_subdev = &vpfe_cfg->sub_devs[0];
+	vpfe_dev->v4l2_dev.ctrl_handler = vpfe_dev->sd[0]->ctrl_handler;
+
+	/* We have at least one sub device to work with */
+	mutex_unlock(&ccdc_lock);
+	return 0;
+
+probe_sd_out:
+	kfree(vpfe_dev->sd);
+probe_out_video_unregister:
+	video_unregister_device(&vpfe_dev->video_dev);
+probe_out_v4l2_unregister:
+	v4l2_device_unregister(&vpfe_dev->v4l2_dev);
+probe_out_release_irq:
+	free_irq(vpfe_dev->ccdc_irq0, vpfe_dev);
+probe_free_ccdc_cfg_mem:
+	kfree(ccdc_cfg);
+	mutex_unlock(&ccdc_lock);
+probe_free_dev_mem:
+	kfree(vpfe_dev);
+	return ret;
+}
+
+/*
+ * vpfe_remove : It un-register device from V4L2 driver
+ */
+static int vpfe_remove(struct platform_device *pdev)
+{
+	struct vpfe_device *vpfe_dev = platform_get_drvdata(pdev);
+
+	v4l2_info(pdev->dev.driver, "vpfe_remove\n");
+
+	free_irq(vpfe_dev->ccdc_irq0, vpfe_dev);
+	kfree(vpfe_dev->sd);
+	v4l2_device_unregister(&vpfe_dev->v4l2_dev);
+	video_unregister_device(&vpfe_dev->video_dev);
+	kfree(vpfe_dev);
+	kfree(ccdc_cfg);
+	return 0;
+}
+
+static int vpfe_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int vpfe_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops vpfe_dev_pm_ops = {
+	.suspend = vpfe_suspend,
+	.resume = vpfe_resume,
+};
+
+static struct platform_driver vpfe_driver = {
+	.driver = {
+		.name = CAPTURE_DRV_NAME,
+		.pm = &vpfe_dev_pm_ops,
+	},
+	.probe = vpfe_probe,
+	.remove = vpfe_remove,
+};
+
+module_platform_driver(vpfe_driver);
diff --git a/drivers/staging/media/deprecated/zr364xx/Kconfig b/drivers/staging/media/deprecated/zr364xx/Kconfig
new file mode 100644
index 000000000..ea29c9d8d
--- /dev/null
+++ b/drivers/staging/media/deprecated/zr364xx/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config USB_ZR364XX
+	tristate "USB ZR364XX Camera support (DEPRECATED)"
+	depends on USB && VIDEO_DEV
+	select VIDEOBUF_GEN
+	select VIDEOBUF_VMALLOC
+	help
+	  Say Y here if you want to connect this type of camera to your
+	  computer's USB port.
+	  See <file:Documentation/admin-guide/media/zr364xx.rst> for more info
+	  and list of supported cameras.
+
+	  This driver is deprecated and is scheduled for removal by
+	  the beginning of 2023. See the TODO file for more information.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called zr364xx.
+
diff --git a/drivers/staging/media/deprecated/zr364xx/Makefile b/drivers/staging/media/deprecated/zr364xx/Makefile
new file mode 100644
index 000000000..edab017d4
--- /dev/null
+++ b/drivers/staging/media/deprecated/zr364xx/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_USB_ZR364XX)       += zr364xx.o
+
diff --git a/drivers/staging/media/deprecated/zr364xx/TODO b/drivers/staging/media/deprecated/zr364xx/TODO
new file mode 100644
index 000000000..ecb30a429
--- /dev/null
+++ b/drivers/staging/media/deprecated/zr364xx/TODO
@@ -0,0 +1,7 @@
+This is one of the few drivers still not using the vb2
+framework, so this driver is now deprecated with the intent of
+removing it altogether by the beginning of 2023.
+
+In order to keep this driver it has to be converted to vb2.
+If someone is interested in doing this work, then contact the
+linux-media mailinglist (https://linuxtv.org/lists.php).
diff --git a/drivers/staging/media/deprecated/zr364xx/zr364xx.c b/drivers/staging/media/deprecated/zr364xx/zr364xx.c
new file mode 100644
index 000000000..538a33004
--- /dev/null
+++ b/drivers/staging/media/deprecated/zr364xx/zr364xx.c
@@ -0,0 +1,1635 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Zoran 364xx based USB webcam module version 0.73
+ *
+ * Allows you to use your USB webcam with V4L2 applications
+ * This is still in heavy development !
+ *
+ * Copyright (C) 2004  Antoine Jacquet <royale@zerezo.com>
+ * http://royale.zerezo.com/zr364xx/
+ *
+ * Heavily inspired by usb-skeleton.c, vicam.c, cpia.c and spca50x.c drivers
+ * V4L2 version inspired by meye.c driver
+ *
+ * Some video buffer code by Lamarque based on s2255drv.c and vivi.c drivers.
+ */
+
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+#include <linux/highmem.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf-vmalloc.h>
+
+
+/* Version Information */
+#define DRIVER_VERSION "0.7.4"
+#define DRIVER_AUTHOR "Antoine Jacquet, http://royale.zerezo.com/"
+#define DRIVER_DESC "Zoran 364xx"
+
+
+/* Camera */
+#define FRAMES 1
+#define MAX_FRAME_SIZE 200000
+#define BUFFER_SIZE 0x1000
+#define CTRL_TIMEOUT 500
+
+#define ZR364XX_DEF_BUFS	4
+#define ZR364XX_READ_IDLE	0
+#define ZR364XX_READ_FRAME	1
+
+/* Debug macro */
+#define DBG(fmt, args...) \
+	do { \
+		if (debug) { \
+			printk(KERN_INFO KBUILD_MODNAME " " fmt, ##args); \
+		} \
+	} while (0)
+
+/*#define FULL_DEBUG 1*/
+#ifdef FULL_DEBUG
+#define _DBG DBG
+#else
+#define _DBG(fmt, args...)
+#endif
+
+/* Init methods, need to find nicer names for these
+ * the exact names of the chipsets would be the best if someone finds it */
+#define METHOD0 0
+#define METHOD1 1
+#define METHOD2 2
+#define METHOD3 3
+
+
+/* Module parameters */
+static int debug;
+static int mode;
+
+
+/* Module parameters interface */
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level");
+module_param(mode, int, 0644);
+MODULE_PARM_DESC(mode, "0 = 320x240, 1 = 160x120, 2 = 640x480");
+
+
+/* Devices supported by this driver
+ * .driver_info contains the init method used by the camera */
+static const struct usb_device_id device_table[] = {
+	{USB_DEVICE(0x08ca, 0x0109), .driver_info = METHOD0 },
+	{USB_DEVICE(0x041e, 0x4024), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0d64, 0x0108), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0546, 0x3187), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0d64, 0x3108), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0595, 0x4343), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0bb0, 0x500d), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0feb, 0x2004), .driver_info = METHOD0 },
+	{USB_DEVICE(0x055f, 0xb500), .driver_info = METHOD0 },
+	{USB_DEVICE(0x08ca, 0x2062), .driver_info = METHOD2 },
+	{USB_DEVICE(0x052b, 0x1a18), .driver_info = METHOD1 },
+	{USB_DEVICE(0x04c8, 0x0729), .driver_info = METHOD0 },
+	{USB_DEVICE(0x04f2, 0xa208), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0784, 0x0040), .driver_info = METHOD1 },
+	{USB_DEVICE(0x06d6, 0x0034), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0a17, 0x0062), .driver_info = METHOD2 },
+	{USB_DEVICE(0x06d6, 0x003b), .driver_info = METHOD0 },
+	{USB_DEVICE(0x0a17, 0x004e), .driver_info = METHOD2 },
+	{USB_DEVICE(0x041e, 0x405d), .driver_info = METHOD2 },
+	{USB_DEVICE(0x08ca, 0x2102), .driver_info = METHOD3 },
+	{USB_DEVICE(0x06d6, 0x003d), .driver_info = METHOD0 },
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, device_table);
+
+/* frame structure */
+struct zr364xx_framei {
+	unsigned long ulState;	/* ulState:ZR364XX_READ_IDLE,
+					   ZR364XX_READ_FRAME */
+	void *lpvbits;		/* image data */
+	unsigned long cur_size;	/* current data copied to it */
+};
+
+/* image buffer structure */
+struct zr364xx_bufferi {
+	unsigned long dwFrames;			/* number of frames in buffer */
+	struct zr364xx_framei frame[FRAMES];	/* array of FRAME structures */
+};
+
+struct zr364xx_dmaqueue {
+	struct list_head	active;
+	struct zr364xx_camera	*cam;
+};
+
+struct zr364xx_pipeinfo {
+	u32 transfer_size;
+	u8 *transfer_buffer;
+	u32 state;
+	void *stream_urb;
+	void *cam;	/* back pointer to zr364xx_camera struct */
+	u32 err_count;
+	u32 idx;
+};
+
+struct zr364xx_fmt {
+	u32 fourcc;
+	int depth;
+};
+
+/* image formats.  */
+static const struct zr364xx_fmt formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_JPEG,
+		.depth = 24
+	}
+};
+
+/* Camera stuff */
+struct zr364xx_camera {
+	struct usb_device *udev;	/* save off the usb device pointer */
+	struct usb_interface *interface;/* the interface for this device */
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct video_device vdev;	/* v4l video device */
+	struct v4l2_fh *owner;		/* owns the streaming */
+	int nb;
+	struct zr364xx_bufferi		buffer;
+	int skip;
+	int width;
+	int height;
+	int method;
+	struct mutex lock;
+
+	spinlock_t		slock;
+	struct zr364xx_dmaqueue	vidq;
+	int			last_frame;
+	int			cur_frame;
+	unsigned long		frame_count;
+	int			b_acquire;
+	struct zr364xx_pipeinfo	pipe[1];
+
+	u8			read_endpoint;
+
+	const struct zr364xx_fmt *fmt;
+	struct videobuf_queue	vb_vidq;
+	bool was_streaming;
+};
+
+/* buffer for one video frame */
+struct zr364xx_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer vb;
+	const struct zr364xx_fmt *fmt;
+};
+
+/* function used to send initialisation commands to the camera */
+static int send_control_msg(struct usb_device *udev, u8 request, u16 value,
+			    u16 index, unsigned char *cp, u16 size)
+{
+	int status;
+
+	unsigned char *transfer_buffer = kmemdup(cp, size, GFP_KERNEL);
+	if (!transfer_buffer)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev,
+				 usb_sndctrlpipe(udev, 0),
+				 request,
+				 USB_DIR_OUT | USB_TYPE_VENDOR |
+				 USB_RECIP_DEVICE, value, index,
+				 transfer_buffer, size, CTRL_TIMEOUT);
+
+	kfree(transfer_buffer);
+	return status;
+}
+
+
+/* Control messages sent to the camera to initialize it
+ * and launch the capture */
+typedef struct {
+	unsigned int value;
+	unsigned int size;
+	unsigned char *bytes;
+} message;
+
+/* method 0 */
+static unsigned char m0d1[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+static unsigned char m0d2[] = { 0, 0, 0, 0, 0, 0 };
+static unsigned char m0d3[] = { 0, 0 };
+static message m0[] = {
+	{0x1f30, 0, NULL},
+	{0xd000, 0, NULL},
+	{0x3370, sizeof(m0d1), m0d1},
+	{0x2000, 0, NULL},
+	{0x2f0f, 0, NULL},
+	{0x2610, sizeof(m0d2), m0d2},
+	{0xe107, 0, NULL},
+	{0x2502, 0, NULL},
+	{0x1f70, 0, NULL},
+	{0xd000, 0, NULL},
+	{0x9a01, sizeof(m0d3), m0d3},
+	{-1, -1, NULL}
+};
+
+/* method 1 */
+static unsigned char m1d1[] = { 0xff, 0xff };
+static unsigned char m1d2[] = { 0x00, 0x00 };
+static message m1[] = {
+	{0x1f30, 0, NULL},
+	{0xd000, 0, NULL},
+	{0xf000, 0, NULL},
+	{0x2000, 0, NULL},
+	{0x2f0f, 0, NULL},
+	{0x2650, 0, NULL},
+	{0xe107, 0, NULL},
+	{0x2502, sizeof(m1d1), m1d1},
+	{0x1f70, 0, NULL},
+	{0xd000, 0, NULL},
+	{0xd000, 0, NULL},
+	{0xd000, 0, NULL},
+	{0x9a01, sizeof(m1d2), m1d2},
+	{-1, -1, NULL}
+};
+
+/* method 2 */
+static unsigned char m2d1[] = { 0xff, 0xff };
+static message m2[] = {
+	{0x1f30, 0, NULL},
+	{0xf000, 0, NULL},
+	{0x2000, 0, NULL},
+	{0x2f0f, 0, NULL},
+	{0x2650, 0, NULL},
+	{0xe107, 0, NULL},
+	{0x2502, sizeof(m2d1), m2d1},
+	{0x1f70, 0, NULL},
+	{-1, -1, NULL}
+};
+
+/* init table */
+static message *init[4] = { m0, m1, m2, m2 };
+
+
+/* JPEG static data in header (Huffman table, etc) */
+static unsigned char header1[] = {
+	0xFF, 0xD8,
+	/*
+	0xFF, 0xE0, 0x00, 0x10, 'J', 'F', 'I', 'F',
+	0x00, 0x01, 0x01, 0x00, 0x33, 0x8A, 0x00, 0x00, 0x33, 0x88,
+	*/
+	0xFF, 0xDB, 0x00, 0x84
+};
+static unsigned char header2[] = {
+	0xFF, 0xC4, 0x00, 0x1F, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+	0xFF, 0xC4, 0x00, 0xB5, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02,
+	0x04, 0x03, 0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01,
+	0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06,
+	0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xA1,
+	0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15, 0x52, 0xD1, 0xF0, 0x24, 0x33,
+	0x62, 0x72, 0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x25,
+	0x26, 0x27, 0x28, 0x29, 0x2A, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
+	0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54,
+	0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67,
+	0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A,
+	0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94,
+	0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6,
+	0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8,
+	0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA,
+	0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE1, 0xE2,
+	0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3,
+	0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, 0xFF, 0xC4, 0x00, 0x1F,
+	0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04,
+	0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0xFF, 0xC4, 0x00, 0xB5,
+	0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05,
+	0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11,
+	0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
+	0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xA1, 0xB1, 0xC1,
+	0x09, 0x23, 0x33, 0x52, 0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16,
+	0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18, 0x19, 0x1A, 0x26, 0x27,
+	0x28, 0x29, 0x2A, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44,
+	0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57,
+	0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A,
+	0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x82, 0x83, 0x84,
+	0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96,
+	0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8,
+	0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA,
+	0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3,
+	0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE2, 0xE3, 0xE4, 0xE5,
+	0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7,
+	0xF8, 0xF9, 0xFA, 0xFF, 0xC0, 0x00, 0x11, 0x08, 0x00, 0xF0, 0x01,
+	0x40, 0x03, 0x01, 0x21, 0x00, 0x02, 0x11, 0x01, 0x03, 0x11, 0x01,
+	0xFF, 0xDA, 0x00, 0x0C, 0x03, 0x01, 0x00, 0x02, 0x11, 0x03, 0x11,
+	0x00, 0x3F, 0x00
+};
+static unsigned char header3;
+
+/* ------------------------------------------------------------------
+   Videobuf operations
+   ------------------------------------------------------------------*/
+
+static int buffer_setup(struct videobuf_queue *vq, unsigned int *count,
+			unsigned int *size)
+{
+	struct zr364xx_camera *cam = vq->priv_data;
+
+	*size = cam->width * cam->height * (cam->fmt->depth >> 3);
+
+	if (*count == 0)
+		*count = ZR364XX_DEF_BUFS;
+
+	if (*size * *count > ZR364XX_DEF_BUFS * 1024 * 1024)
+		*count = (ZR364XX_DEF_BUFS * 1024 * 1024) / *size;
+
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct zr364xx_buffer *buf)
+{
+	_DBG("%s\n", __func__);
+
+	videobuf_vmalloc_free(&buf->vb);
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
+			  enum v4l2_field field)
+{
+	struct zr364xx_camera *cam = vq->priv_data;
+	struct zr364xx_buffer *buf = container_of(vb, struct zr364xx_buffer,
+						  vb);
+	int rc;
+
+	DBG("%s, field=%d\n", __func__, field);
+	if (!cam->fmt)
+		return -EINVAL;
+
+	buf->vb.size = cam->width * cam->height * (cam->fmt->depth >> 3);
+
+	if (buf->vb.baddr != 0 && buf->vb.bsize < buf->vb.size) {
+		DBG("invalid buffer prepare\n");
+		return -EINVAL;
+	}
+
+	buf->fmt = cam->fmt;
+	buf->vb.width = cam->width;
+	buf->vb.height = cam->height;
+	buf->vb.field = field;
+
+	if (buf->vb.state == VIDEOBUF_NEEDS_INIT) {
+		rc = videobuf_iolock(vq, &buf->vb, NULL);
+		if (rc < 0)
+			goto fail;
+	}
+
+	buf->vb.state = VIDEOBUF_PREPARED;
+	return 0;
+fail:
+	free_buffer(vq, buf);
+	return rc;
+}
+
+static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct zr364xx_buffer *buf = container_of(vb, struct zr364xx_buffer,
+						  vb);
+	struct zr364xx_camera *cam = vq->priv_data;
+
+	_DBG("%s\n", __func__);
+
+	buf->vb.state = VIDEOBUF_QUEUED;
+	list_add_tail(&buf->vb.queue, &cam->vidq.active);
+}
+
+static void buffer_release(struct videobuf_queue *vq,
+			   struct videobuf_buffer *vb)
+{
+	struct zr364xx_buffer *buf = container_of(vb, struct zr364xx_buffer,
+						  vb);
+
+	_DBG("%s\n", __func__);
+	free_buffer(vq, buf);
+}
+
+static const struct videobuf_queue_ops zr364xx_video_qops = {
+	.buf_setup = buffer_setup,
+	.buf_prepare = buffer_prepare,
+	.buf_queue = buffer_queue,
+	.buf_release = buffer_release,
+};
+
+/********************/
+/* V4L2 integration */
+/********************/
+static int zr364xx_vidioc_streamon(struct file *file, void *priv,
+				   enum v4l2_buf_type type);
+
+static ssize_t zr364xx_read(struct file *file, char __user *buf, size_t count,
+			    loff_t * ppos)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	int err = 0;
+
+	_DBG("%s\n", __func__);
+
+	if (!buf)
+		return -EINVAL;
+
+	if (!count)
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&cam->lock))
+		return -ERESTARTSYS;
+
+	err = zr364xx_vidioc_streamon(file, file->private_data,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	if (err == 0) {
+		DBG("%s: reading %d bytes at pos %d.\n", __func__,
+				(int) count, (int) *ppos);
+
+		/* NoMan Sux ! */
+		err = videobuf_read_one(&cam->vb_vidq, buf, count, ppos,
+					file->f_flags & O_NONBLOCK);
+	}
+	mutex_unlock(&cam->lock);
+	return err;
+}
+
+/* video buffer vmalloc implementation based partly on VIVI driver which is
+ *          Copyright (c) 2006 by
+ *                  Mauro Carvalho Chehab <mchehab--a.t--infradead.org>
+ *                  Ted Walther <ted--a.t--enumera.com>
+ *                  John Sokol <sokol--a.t--videotechnology.com>
+ *                  http://v4l.videotechnology.com/
+ *
+ */
+static void zr364xx_fillbuff(struct zr364xx_camera *cam,
+			     struct zr364xx_buffer *buf,
+			     int jpgsize)
+{
+	int pos = 0;
+	const char *tmpbuf;
+	char *vbuf = videobuf_to_vmalloc(&buf->vb);
+	unsigned long last_frame;
+
+	if (!vbuf)
+		return;
+
+	last_frame = cam->last_frame;
+	if (last_frame != -1) {
+		tmpbuf = (const char *)cam->buffer.frame[last_frame].lpvbits;
+		switch (buf->fmt->fourcc) {
+		case V4L2_PIX_FMT_JPEG:
+			buf->vb.size = jpgsize;
+			memcpy(vbuf, tmpbuf, buf->vb.size);
+			break;
+		default:
+			printk(KERN_DEBUG KBUILD_MODNAME ": unknown format?\n");
+		}
+		cam->last_frame = -1;
+	} else {
+		printk(KERN_ERR KBUILD_MODNAME ": =======no frame\n");
+		return;
+	}
+	DBG("%s: Buffer %p size= %d\n", __func__, vbuf, pos);
+	/* tell v4l buffer was filled */
+
+	buf->vb.field_count = cam->frame_count * 2;
+	buf->vb.ts = ktime_get_ns();
+	buf->vb.state = VIDEOBUF_DONE;
+}
+
+static int zr364xx_got_frame(struct zr364xx_camera *cam, int jpgsize)
+{
+	struct zr364xx_dmaqueue *dma_q = &cam->vidq;
+	struct zr364xx_buffer *buf;
+	unsigned long flags = 0;
+	int rc = 0;
+
+	DBG("wakeup: %p\n", &dma_q);
+	spin_lock_irqsave(&cam->slock, flags);
+
+	if (list_empty(&dma_q->active)) {
+		DBG("No active queue to serve\n");
+		rc = -1;
+		goto unlock;
+	}
+	buf = list_entry(dma_q->active.next,
+			 struct zr364xx_buffer, vb.queue);
+
+	if (!waitqueue_active(&buf->vb.done)) {
+		/* no one active */
+		rc = -1;
+		goto unlock;
+	}
+	list_del(&buf->vb.queue);
+	buf->vb.ts = ktime_get_ns();
+	DBG("[%p/%d] wakeup\n", buf, buf->vb.i);
+	zr364xx_fillbuff(cam, buf, jpgsize);
+	wake_up(&buf->vb.done);
+	DBG("wakeup [buf/i] [%p/%d]\n", buf, buf->vb.i);
+unlock:
+	spin_unlock_irqrestore(&cam->slock, flags);
+	return rc;
+}
+
+/* this function moves the usb stream read pipe data
+ * into the system buffers.
+ * returns 0 on success, EAGAIN if more data to process (call this
+ * function again).
+ */
+static int zr364xx_read_video_callback(struct zr364xx_camera *cam,
+					struct zr364xx_pipeinfo *pipe_info,
+					struct urb *purb)
+{
+	unsigned char *pdest;
+	unsigned char *psrc;
+	s32 idx = cam->cur_frame;
+	struct zr364xx_framei *frm = &cam->buffer.frame[idx];
+	int i = 0;
+	unsigned char *ptr = NULL;
+
+	_DBG("buffer to user\n");
+
+	/* swap bytes if camera needs it */
+	if (cam->method == METHOD0) {
+		u16 *buf = (u16 *)pipe_info->transfer_buffer;
+		for (i = 0; i < purb->actual_length/2; i++)
+			swab16s(buf + i);
+	}
+
+	/* search done.  now find out if should be acquiring */
+	if (!cam->b_acquire) {
+		/* we found a frame, but this channel is turned off */
+		frm->ulState = ZR364XX_READ_IDLE;
+		return -EINVAL;
+	}
+
+	psrc = (u8 *)pipe_info->transfer_buffer;
+	ptr = pdest = frm->lpvbits;
+
+	if (frm->ulState == ZR364XX_READ_IDLE) {
+		if (purb->actual_length < 128) {
+			/* header incomplete */
+			dev_info(&cam->udev->dev,
+				 "%s: buffer (%d bytes) too small to hold jpeg header. Discarding.\n",
+				 __func__, purb->actual_length);
+			return -EINVAL;
+		}
+
+		frm->ulState = ZR364XX_READ_FRAME;
+		frm->cur_size = 0;
+
+		_DBG("jpeg header, ");
+		memcpy(ptr, header1, sizeof(header1));
+		ptr += sizeof(header1);
+		header3 = 0;
+		memcpy(ptr, &header3, 1);
+		ptr++;
+		memcpy(ptr, psrc, 64);
+		ptr += 64;
+		header3 = 1;
+		memcpy(ptr, &header3, 1);
+		ptr++;
+		memcpy(ptr, psrc + 64, 64);
+		ptr += 64;
+		memcpy(ptr, header2, sizeof(header2));
+		ptr += sizeof(header2);
+		memcpy(ptr, psrc + 128,
+		       purb->actual_length - 128);
+		ptr += purb->actual_length - 128;
+		_DBG("header : %d %d %d %d %d %d %d %d %d\n",
+		    psrc[0], psrc[1], psrc[2],
+		    psrc[3], psrc[4], psrc[5],
+		    psrc[6], psrc[7], psrc[8]);
+		frm->cur_size = ptr - pdest;
+	} else {
+		if (frm->cur_size + purb->actual_length > MAX_FRAME_SIZE) {
+			dev_info(&cam->udev->dev,
+				 "%s: buffer (%d bytes) too small to hold frame data. Discarding frame data.\n",
+				 __func__, MAX_FRAME_SIZE);
+		} else {
+			pdest += frm->cur_size;
+			memcpy(pdest, psrc, purb->actual_length);
+			frm->cur_size += purb->actual_length;
+		}
+	}
+	/*_DBG("cur_size %lu urb size %d\n", frm->cur_size,
+		purb->actual_length);*/
+
+	if (purb->actual_length < pipe_info->transfer_size) {
+		_DBG("****************Buffer[%d]full*************\n", idx);
+		cam->last_frame = cam->cur_frame;
+		cam->cur_frame++;
+		/* end of system frame ring buffer, start at zero */
+		if (cam->cur_frame == cam->buffer.dwFrames)
+			cam->cur_frame = 0;
+
+		/* frame ready */
+		/* go back to find the JPEG EOI marker */
+		ptr = pdest = frm->lpvbits;
+		ptr += frm->cur_size - 2;
+		while (ptr > pdest) {
+			if (*ptr == 0xFF && *(ptr + 1) == 0xD9
+			    && *(ptr + 2) == 0xFF)
+				break;
+			ptr--;
+		}
+		if (ptr == pdest)
+			DBG("No EOI marker\n");
+
+		/* Sometimes there is junk data in the middle of the picture,
+		 * we want to skip this bogus frames */
+		while (ptr > pdest) {
+			if (*ptr == 0xFF && *(ptr + 1) == 0xFF
+			    && *(ptr + 2) == 0xFF)
+				break;
+			ptr--;
+		}
+		if (ptr != pdest) {
+			DBG("Bogus frame ? %d\n", ++(cam->nb));
+		} else if (cam->b_acquire) {
+			/* we skip the 2 first frames which are usually buggy */
+			if (cam->skip)
+				cam->skip--;
+			else {
+				_DBG("jpeg(%lu): %d %d %d %d %d %d %d %d\n",
+				    frm->cur_size,
+				    pdest[0], pdest[1], pdest[2], pdest[3],
+				    pdest[4], pdest[5], pdest[6], pdest[7]);
+
+				zr364xx_got_frame(cam, frm->cur_size);
+			}
+		}
+		cam->frame_count++;
+		frm->ulState = ZR364XX_READ_IDLE;
+		frm->cur_size = 0;
+	}
+	/* done successfully */
+	return 0;
+}
+
+static int zr364xx_vidioc_querycap(struct file *file, void *priv,
+				   struct v4l2_capability *cap)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+
+	strscpy(cap->driver, DRIVER_DESC, sizeof(cap->driver));
+	if (cam->udev->product)
+		strscpy(cap->card, cam->udev->product, sizeof(cap->card));
+	strscpy(cap->bus_info, dev_name(&cam->udev->dev),
+		sizeof(cap->bus_info));
+	return 0;
+}
+
+static int zr364xx_vidioc_enum_input(struct file *file, void *priv,
+				     struct v4l2_input *i)
+{
+	if (i->index != 0)
+		return -EINVAL;
+	strscpy(i->name, DRIVER_DESC " Camera", sizeof(i->name));
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	return 0;
+}
+
+static int zr364xx_vidioc_g_input(struct file *file, void *priv,
+				  unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int zr364xx_vidioc_s_input(struct file *file, void *priv,
+				  unsigned int i)
+{
+	if (i != 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int zr364xx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct zr364xx_camera *cam =
+		container_of(ctrl->handler, struct zr364xx_camera, ctrl_handler);
+	int temp;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		/* hardware brightness */
+		send_control_msg(cam->udev, 1, 0x2001, 0, NULL, 0);
+		temp = (0x60 << 8) + 127 - ctrl->val;
+		send_control_msg(cam->udev, 1, temp, 0, NULL, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int zr364xx_vidioc_enum_fmt_vid_cap(struct file *file,
+				       void *priv, struct v4l2_fmtdesc *f)
+{
+	if (f->index > 0)
+		return -EINVAL;
+	f->pixelformat = formats[0].fourcc;
+	return 0;
+}
+
+static char *decode_fourcc(__u32 pixelformat, char *buf)
+{
+	buf[0] = pixelformat & 0xff;
+	buf[1] = (pixelformat >> 8) & 0xff;
+	buf[2] = (pixelformat >> 16) & 0xff;
+	buf[3] = (pixelformat >> 24) & 0xff;
+	buf[4] = '\0';
+	return buf;
+}
+
+static int zr364xx_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	char pixelformat_name[5];
+
+	if (!cam)
+		return -ENODEV;
+
+	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_JPEG) {
+		DBG("%s: unsupported pixelformat V4L2_PIX_FMT_%s\n", __func__,
+		    decode_fourcc(f->fmt.pix.pixelformat, pixelformat_name));
+		return -EINVAL;
+	}
+
+	if (!(f->fmt.pix.width == 160 && f->fmt.pix.height == 120) &&
+	    !(f->fmt.pix.width == 640 && f->fmt.pix.height == 480)) {
+		f->fmt.pix.width = 320;
+		f->fmt.pix.height = 240;
+	}
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	DBG("%s: V4L2_PIX_FMT_%s (%d) ok!\n", __func__,
+	    decode_fourcc(f->fmt.pix.pixelformat, pixelformat_name),
+	    f->fmt.pix.field);
+	return 0;
+}
+
+static int zr364xx_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct zr364xx_camera *cam;
+
+	if (!file)
+		return -ENODEV;
+	cam = video_drvdata(file);
+
+	f->fmt.pix.pixelformat = formats[0].fourcc;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.width = cam->width;
+	f->fmt.pix.height = cam->height;
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	return 0;
+}
+
+static int zr364xx_vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	struct videobuf_queue *q = &cam->vb_vidq;
+	char pixelformat_name[5];
+	int ret = zr364xx_vidioc_try_fmt_vid_cap(file, cam, f);
+	int i;
+
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&q->vb_lock);
+
+	if (videobuf_queue_is_busy(&cam->vb_vidq)) {
+		DBG("%s queue busy\n", __func__);
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (cam->owner) {
+		DBG("%s can't change format after started\n", __func__);
+		ret = -EBUSY;
+		goto out;
+	}
+
+	cam->width = f->fmt.pix.width;
+	cam->height = f->fmt.pix.height;
+	DBG("%s: %dx%d mode selected\n", __func__,
+		 cam->width, cam->height);
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	cam->vb_vidq.field = f->fmt.pix.field;
+
+	if (f->fmt.pix.width == 160 && f->fmt.pix.height == 120)
+		mode = 1;
+	else if (f->fmt.pix.width == 640 && f->fmt.pix.height == 480)
+		mode = 2;
+	else
+		mode = 0;
+
+	m0d1[0] = mode;
+	m1[2].value = 0xf000 + mode;
+	m2[1].value = 0xf000 + mode;
+
+	/* special case for METHOD3, the modes are different */
+	if (cam->method == METHOD3) {
+		switch (mode) {
+		case 1:
+			m2[1].value = 0xf000 + 4;
+			break;
+		case 2:
+			m2[1].value = 0xf000 + 0;
+			break;
+		default:
+			m2[1].value = 0xf000 + 1;
+			break;
+		}
+	}
+
+	header2[437] = cam->height / 256;
+	header2[438] = cam->height % 256;
+	header2[439] = cam->width / 256;
+	header2[440] = cam->width % 256;
+
+	for (i = 0; init[cam->method][i].size != -1; i++) {
+		ret =
+		    send_control_msg(cam->udev, 1, init[cam->method][i].value,
+				     0, init[cam->method][i].bytes,
+				     init[cam->method][i].size);
+		if (ret < 0) {
+			dev_err(&cam->udev->dev,
+			   "error during resolution change sequence: %d\n", i);
+			goto out;
+		}
+	}
+
+	/* Added some delay here, since opening/closing the camera quickly,
+	 * like Ekiga does during its startup, can crash the webcam
+	 */
+	mdelay(100);
+	cam->skip = 2;
+	ret = 0;
+
+out:
+	mutex_unlock(&q->vb_lock);
+
+	DBG("%s: V4L2_PIX_FMT_%s (%d) ok!\n", __func__,
+	    decode_fourcc(f->fmt.pix.pixelformat, pixelformat_name),
+	    f->fmt.pix.field);
+	return ret;
+}
+
+static int zr364xx_vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+	return videobuf_reqbufs(&cam->vb_vidq, p);
+}
+
+static int zr364xx_vidioc_querybuf(struct file *file,
+				void *priv,
+				struct v4l2_buffer *p)
+{
+	int rc;
+	struct zr364xx_camera *cam = video_drvdata(file);
+	rc = videobuf_querybuf(&cam->vb_vidq, p);
+	return rc;
+}
+
+static int zr364xx_vidioc_qbuf(struct file *file,
+				void *priv,
+				struct v4l2_buffer *p)
+{
+	int rc;
+	struct zr364xx_camera *cam = video_drvdata(file);
+	_DBG("%s\n", __func__);
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+	rc = videobuf_qbuf(&cam->vb_vidq, p);
+	return rc;
+}
+
+static int zr364xx_vidioc_dqbuf(struct file *file,
+				void *priv,
+				struct v4l2_buffer *p)
+{
+	int rc;
+	struct zr364xx_camera *cam = video_drvdata(file);
+	_DBG("%s\n", __func__);
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+	rc = videobuf_dqbuf(&cam->vb_vidq, p, file->f_flags & O_NONBLOCK);
+	return rc;
+}
+
+static void read_pipe_completion(struct urb *purb)
+{
+	struct zr364xx_pipeinfo *pipe_info;
+	struct zr364xx_camera *cam;
+	int pipe;
+
+	pipe_info = purb->context;
+	_DBG("%s %p, status %d\n", __func__, purb, purb->status);
+	if (!pipe_info) {
+		printk(KERN_ERR KBUILD_MODNAME ": no context!\n");
+		return;
+	}
+
+	cam = pipe_info->cam;
+	if (!cam) {
+		printk(KERN_ERR KBUILD_MODNAME ": no context!\n");
+		return;
+	}
+
+	/* if shutting down, do not resubmit, exit immediately */
+	if (purb->status == -ESHUTDOWN) {
+		DBG("%s, err shutdown\n", __func__);
+		pipe_info->err_count++;
+		return;
+	}
+
+	if (pipe_info->state == 0) {
+		DBG("exiting USB pipe\n");
+		return;
+	}
+
+	if (purb->actual_length > pipe_info->transfer_size) {
+		dev_err(&cam->udev->dev, "wrong number of bytes\n");
+		return;
+	}
+
+	if (purb->status == 0)
+		zr364xx_read_video_callback(cam, pipe_info, purb);
+	else {
+		pipe_info->err_count++;
+		DBG("%s: failed URB %d\n", __func__, purb->status);
+	}
+
+	pipe = usb_rcvbulkpipe(cam->udev, cam->read_endpoint);
+
+	/* reuse urb */
+	usb_fill_bulk_urb(pipe_info->stream_urb, cam->udev,
+			  pipe,
+			  pipe_info->transfer_buffer,
+			  pipe_info->transfer_size,
+			  read_pipe_completion, pipe_info);
+
+	if (pipe_info->state != 0) {
+		purb->status = usb_submit_urb(pipe_info->stream_urb,
+					      GFP_ATOMIC);
+
+		if (purb->status)
+			dev_err(&cam->udev->dev,
+				"error submitting urb (error=%i)\n",
+				purb->status);
+	} else
+		DBG("read pipe complete state 0\n");
+}
+
+static int zr364xx_start_readpipe(struct zr364xx_camera *cam)
+{
+	int pipe;
+	int retval;
+	struct zr364xx_pipeinfo *pipe_info = cam->pipe;
+	pipe = usb_rcvbulkpipe(cam->udev, cam->read_endpoint);
+	DBG("%s: start pipe IN x%x\n", __func__, cam->read_endpoint);
+
+	pipe_info->state = 1;
+	pipe_info->err_count = 0;
+	pipe_info->stream_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!pipe_info->stream_urb)
+		return -ENOMEM;
+	/* transfer buffer allocated in board_init */
+	usb_fill_bulk_urb(pipe_info->stream_urb, cam->udev,
+			  pipe,
+			  pipe_info->transfer_buffer,
+			  pipe_info->transfer_size,
+			  read_pipe_completion, pipe_info);
+
+	DBG("submitting URB %p\n", pipe_info->stream_urb);
+	retval = usb_submit_urb(pipe_info->stream_urb, GFP_KERNEL);
+	if (retval) {
+		usb_free_urb(pipe_info->stream_urb);
+		printk(KERN_ERR KBUILD_MODNAME ": start read pipe failed\n");
+		return retval;
+	}
+
+	return 0;
+}
+
+static void zr364xx_stop_readpipe(struct zr364xx_camera *cam)
+{
+	struct zr364xx_pipeinfo *pipe_info;
+
+	if (!cam) {
+		printk(KERN_ERR KBUILD_MODNAME ": invalid device\n");
+		return;
+	}
+	DBG("stop read pipe\n");
+	pipe_info = cam->pipe;
+	if (pipe_info) {
+		if (pipe_info->state != 0)
+			pipe_info->state = 0;
+
+		if (pipe_info->stream_urb) {
+			/* cancel urb */
+			usb_kill_urb(pipe_info->stream_urb);
+			usb_free_urb(pipe_info->stream_urb);
+			pipe_info->stream_urb = NULL;
+		}
+	}
+	return;
+}
+
+/* starts acquisition process */
+static int zr364xx_start_acquire(struct zr364xx_camera *cam)
+{
+	int j;
+
+	DBG("start acquire\n");
+
+	cam->last_frame = -1;
+	cam->cur_frame = 0;
+	for (j = 0; j < FRAMES; j++) {
+		cam->buffer.frame[j].ulState = ZR364XX_READ_IDLE;
+		cam->buffer.frame[j].cur_size = 0;
+	}
+	cam->b_acquire = 1;
+	return 0;
+}
+
+static inline int zr364xx_stop_acquire(struct zr364xx_camera *cam)
+{
+	cam->b_acquire = 0;
+	return 0;
+}
+
+static int zr364xx_prepare(struct zr364xx_camera *cam)
+{
+	int res;
+	int i, j;
+
+	for (i = 0; init[cam->method][i].size != -1; i++) {
+		res = send_control_msg(cam->udev, 1, init[cam->method][i].value,
+				     0, init[cam->method][i].bytes,
+				     init[cam->method][i].size);
+		if (res < 0) {
+			dev_err(&cam->udev->dev,
+				"error during open sequence: %d\n", i);
+			return res;
+		}
+	}
+
+	cam->skip = 2;
+	cam->last_frame = -1;
+	cam->cur_frame = 0;
+	cam->frame_count = 0;
+	for (j = 0; j < FRAMES; j++) {
+		cam->buffer.frame[j].ulState = ZR364XX_READ_IDLE;
+		cam->buffer.frame[j].cur_size = 0;
+	}
+	v4l2_ctrl_handler_setup(&cam->ctrl_handler);
+	return 0;
+}
+
+static int zr364xx_vidioc_streamon(struct file *file, void *priv,
+				   enum v4l2_buf_type type)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	int res;
+
+	DBG("%s\n", __func__);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+
+	res = zr364xx_prepare(cam);
+	if (res)
+		return res;
+	res = videobuf_streamon(&cam->vb_vidq);
+	if (res == 0) {
+		zr364xx_start_acquire(cam);
+		cam->owner = file->private_data;
+	}
+	return res;
+}
+
+static int zr364xx_vidioc_streamoff(struct file *file, void *priv,
+				    enum v4l2_buf_type type)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+
+	DBG("%s\n", __func__);
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+	zr364xx_stop_acquire(cam);
+	return videobuf_streamoff(&cam->vb_vidq);
+}
+
+
+/* open the camera */
+static int zr364xx_open(struct file *file)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	int err;
+
+	DBG("%s\n", __func__);
+
+	if (mutex_lock_interruptible(&cam->lock))
+		return -ERESTARTSYS;
+
+	err = v4l2_fh_open(file);
+	if (err)
+		goto out;
+
+	/* Added some delay here, since opening/closing the camera quickly,
+	 * like Ekiga does during its startup, can crash the webcam
+	 */
+	mdelay(100);
+	err = 0;
+
+out:
+	mutex_unlock(&cam->lock);
+	DBG("%s: %d\n", __func__, err);
+	return err;
+}
+
+static void zr364xx_board_uninit(struct zr364xx_camera *cam)
+{
+	unsigned long i;
+
+	zr364xx_stop_readpipe(cam);
+
+	/* release sys buffers */
+	for (i = 0; i < FRAMES; i++) {
+		if (cam->buffer.frame[i].lpvbits) {
+			DBG("vfree %p\n", cam->buffer.frame[i].lpvbits);
+			vfree(cam->buffer.frame[i].lpvbits);
+		}
+		cam->buffer.frame[i].lpvbits = NULL;
+	}
+
+	/* release transfer buffer */
+	kfree(cam->pipe->transfer_buffer);
+}
+
+static void zr364xx_release(struct v4l2_device *v4l2_dev)
+{
+	struct zr364xx_camera *cam =
+		container_of(v4l2_dev, struct zr364xx_camera, v4l2_dev);
+
+	videobuf_mmap_free(&cam->vb_vidq);
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
+	zr364xx_board_uninit(cam);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+}
+
+/* release the camera */
+static int zr364xx_close(struct file *file)
+{
+	struct zr364xx_camera *cam;
+	struct usb_device *udev;
+	int i;
+
+	DBG("%s\n", __func__);
+	cam = video_drvdata(file);
+
+	mutex_lock(&cam->lock);
+	udev = cam->udev;
+
+	if (file->private_data == cam->owner) {
+		/* turn off stream */
+		if (cam->b_acquire)
+			zr364xx_stop_acquire(cam);
+		videobuf_streamoff(&cam->vb_vidq);
+
+		for (i = 0; i < 2; i++) {
+			send_control_msg(udev, 1, init[cam->method][i].value,
+					0, init[cam->method][i].bytes,
+					init[cam->method][i].size);
+		}
+		cam->owner = NULL;
+	}
+
+	/* Added some delay here, since opening/closing the camera quickly,
+	 * like Ekiga does during its startup, can crash the webcam
+	 */
+	mdelay(100);
+	mutex_unlock(&cam->lock);
+	return v4l2_fh_release(file);
+}
+
+
+static int zr364xx_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	int ret;
+
+	if (!cam) {
+		DBG("%s: cam == NULL\n", __func__);
+		return -ENODEV;
+	}
+	DBG("mmap called, vma=%p\n", vma);
+
+	ret = videobuf_mmap_mapper(&cam->vb_vidq, vma);
+
+	DBG("vma start=0x%08lx, size=%ld, ret=%d\n",
+		(unsigned long)vma->vm_start,
+		(unsigned long)vma->vm_end - (unsigned long)vma->vm_start, ret);
+	return ret;
+}
+
+static __poll_t zr364xx_poll(struct file *file,
+			       struct poll_table_struct *wait)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	struct videobuf_queue *q = &cam->vb_vidq;
+	__poll_t res = v4l2_ctrl_poll(file, wait);
+
+	_DBG("%s\n", __func__);
+
+	return res | videobuf_poll_stream(file, q, wait);
+}
+
+static const struct v4l2_ctrl_ops zr364xx_ctrl_ops = {
+	.s_ctrl = zr364xx_s_ctrl,
+};
+
+static const struct v4l2_file_operations zr364xx_fops = {
+	.owner = THIS_MODULE,
+	.open = zr364xx_open,
+	.release = zr364xx_close,
+	.read = zr364xx_read,
+	.mmap = zr364xx_mmap,
+	.unlocked_ioctl = video_ioctl2,
+	.poll = zr364xx_poll,
+};
+
+static const struct v4l2_ioctl_ops zr364xx_ioctl_ops = {
+	.vidioc_querycap	= zr364xx_vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap = zr364xx_vidioc_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= zr364xx_vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= zr364xx_vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= zr364xx_vidioc_g_fmt_vid_cap,
+	.vidioc_enum_input	= zr364xx_vidioc_enum_input,
+	.vidioc_g_input		= zr364xx_vidioc_g_input,
+	.vidioc_s_input		= zr364xx_vidioc_s_input,
+	.vidioc_streamon	= zr364xx_vidioc_streamon,
+	.vidioc_streamoff	= zr364xx_vidioc_streamoff,
+	.vidioc_reqbufs         = zr364xx_vidioc_reqbufs,
+	.vidioc_querybuf        = zr364xx_vidioc_querybuf,
+	.vidioc_qbuf            = zr364xx_vidioc_qbuf,
+	.vidioc_dqbuf           = zr364xx_vidioc_dqbuf,
+	.vidioc_log_status      = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct video_device zr364xx_template = {
+	.name = DRIVER_DESC,
+	.fops = &zr364xx_fops,
+	.ioctl_ops = &zr364xx_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+		       V4L2_CAP_STREAMING,
+};
+
+
+
+/*******************/
+/* USB integration */
+/*******************/
+static int zr364xx_board_init(struct zr364xx_camera *cam)
+{
+	struct zr364xx_pipeinfo *pipe = cam->pipe;
+	unsigned long i;
+	int err;
+
+	DBG("board init: %p\n", cam);
+	memset(pipe, 0, sizeof(*pipe));
+	pipe->cam = cam;
+	pipe->transfer_size = BUFFER_SIZE;
+
+	pipe->transfer_buffer = kzalloc(pipe->transfer_size,
+					GFP_KERNEL);
+	if (!pipe->transfer_buffer) {
+		DBG("out of memory!\n");
+		return -ENOMEM;
+	}
+
+	cam->b_acquire = 0;
+	cam->frame_count = 0;
+
+	/*** start create system buffers ***/
+	for (i = 0; i < FRAMES; i++) {
+		/* always allocate maximum size for system buffers */
+		cam->buffer.frame[i].lpvbits = vmalloc(MAX_FRAME_SIZE);
+
+		DBG("valloc %p, idx %lu, pdata %p\n",
+			&cam->buffer.frame[i], i,
+			cam->buffer.frame[i].lpvbits);
+		if (!cam->buffer.frame[i].lpvbits) {
+			printk(KERN_INFO KBUILD_MODNAME ": out of memory. Using less frames\n");
+			break;
+		}
+	}
+
+	if (i == 0) {
+		printk(KERN_INFO KBUILD_MODNAME ": out of memory. Aborting\n");
+		err = -ENOMEM;
+		goto err_free;
+	} else
+		cam->buffer.dwFrames = i;
+
+	/* make sure internal states are set */
+	for (i = 0; i < FRAMES; i++) {
+		cam->buffer.frame[i].ulState = ZR364XX_READ_IDLE;
+		cam->buffer.frame[i].cur_size = 0;
+	}
+
+	cam->cur_frame = 0;
+	cam->last_frame = -1;
+	/*** end create system buffers ***/
+
+	/* start read pipe */
+	err = zr364xx_start_readpipe(cam);
+	if (err)
+		goto err_free_frames;
+
+	DBG(": board initialized\n");
+	return 0;
+
+err_free_frames:
+	for (i = 0; i < FRAMES; i++)
+		vfree(cam->buffer.frame[i].lpvbits);
+err_free:
+	kfree(cam->pipe->transfer_buffer);
+	cam->pipe->transfer_buffer = NULL;
+	return err;
+}
+
+static int zr364xx_probe(struct usb_interface *intf,
+			 const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct zr364xx_camera *cam = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	struct v4l2_ctrl_handler *hdl;
+	int err;
+	int i;
+
+	DBG("probing...\n");
+
+	dev_info(&intf->dev, DRIVER_DESC " compatible webcam plugged\n");
+	dev_info(&intf->dev, "model %04x:%04x detected\n",
+		 le16_to_cpu(udev->descriptor.idVendor),
+		 le16_to_cpu(udev->descriptor.idProduct));
+
+	cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+	if (!cam)
+		return -ENOMEM;
+
+	err = v4l2_device_register(&intf->dev, &cam->v4l2_dev);
+	if (err < 0) {
+		dev_err(&udev->dev, "couldn't register v4l2_device\n");
+		goto free_cam;
+	}
+	hdl = &cam->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 1);
+	v4l2_ctrl_new_std(hdl, &zr364xx_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 127, 1, 64);
+	if (hdl->error) {
+		err = hdl->error;
+		dev_err(&udev->dev, "couldn't register control\n");
+		goto free_hdlr_and_unreg_dev;
+	}
+	/* save the init method used by this camera */
+	cam->method = id->driver_info;
+	mutex_init(&cam->lock);
+	cam->vdev = zr364xx_template;
+	cam->vdev.lock = &cam->lock;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	cam->vdev.ctrl_handler = &cam->ctrl_handler;
+	video_set_drvdata(&cam->vdev, cam);
+
+	cam->udev = udev;
+
+	switch (mode) {
+	case 1:
+		dev_info(&udev->dev, "160x120 mode selected\n");
+		cam->width = 160;
+		cam->height = 120;
+		break;
+	case 2:
+		dev_info(&udev->dev, "640x480 mode selected\n");
+		cam->width = 640;
+		cam->height = 480;
+		break;
+	default:
+		dev_info(&udev->dev, "320x240 mode selected\n");
+		cam->width = 320;
+		cam->height = 240;
+		break;
+	}
+
+	m0d1[0] = mode;
+	m1[2].value = 0xf000 + mode;
+	m2[1].value = 0xf000 + mode;
+
+	/* special case for METHOD3, the modes are different */
+	if (cam->method == METHOD3) {
+		switch (mode) {
+		case 1:
+			m2[1].value = 0xf000 + 4;
+			break;
+		case 2:
+			m2[1].value = 0xf000 + 0;
+			break;
+		default:
+			m2[1].value = 0xf000 + 1;
+			break;
+		}
+	}
+
+	header2[437] = cam->height / 256;
+	header2[438] = cam->height % 256;
+	header2[439] = cam->width / 256;
+	header2[440] = cam->width % 256;
+
+	cam->nb = 0;
+
+	DBG("dev: %p, udev %p interface %p\n", cam, cam->udev, intf);
+
+	/* set up the endpoint information  */
+	iface_desc = intf->cur_altsetting;
+	DBG("num endpoints %d\n", iface_desc->desc.bNumEndpoints);
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		if (!cam->read_endpoint && usb_endpoint_is_bulk_in(endpoint)) {
+			/* we found the bulk in endpoint */
+			cam->read_endpoint = endpoint->bEndpointAddress;
+		}
+	}
+
+	if (!cam->read_endpoint) {
+		err = -ENOMEM;
+		dev_err(&intf->dev, "Could not find bulk-in endpoint\n");
+		goto free_hdlr_and_unreg_dev;
+	}
+
+	/* v4l */
+	INIT_LIST_HEAD(&cam->vidq.active);
+	cam->vidq.cam = cam;
+
+	usb_set_intfdata(intf, cam);
+
+	/* load zr364xx board specific */
+	err = zr364xx_board_init(cam);
+	if (err)
+		goto free_hdlr_and_unreg_dev;
+	err = v4l2_ctrl_handler_setup(hdl);
+	if (err)
+		goto board_uninit;
+
+	spin_lock_init(&cam->slock);
+
+	cam->fmt = formats;
+
+	videobuf_queue_vmalloc_init(&cam->vb_vidq, &zr364xx_video_qops,
+				    NULL, &cam->slock,
+				    V4L2_BUF_TYPE_VIDEO_CAPTURE,
+				    V4L2_FIELD_NONE,
+				    sizeof(struct zr364xx_buffer), cam, &cam->lock);
+
+	err = video_register_device(&cam->vdev, VFL_TYPE_VIDEO, -1);
+	if (err) {
+		dev_err(&udev->dev, "video_register_device failed\n");
+		goto board_uninit;
+	}
+	cam->v4l2_dev.release = zr364xx_release;
+
+	dev_info(&udev->dev, DRIVER_DESC " controlling device %s\n",
+		 video_device_node_name(&cam->vdev));
+	return 0;
+
+board_uninit:
+	zr364xx_board_uninit(cam);
+free_hdlr_and_unreg_dev:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&cam->v4l2_dev);
+free_cam:
+	kfree(cam);
+	return err;
+}
+
+
+static void zr364xx_disconnect(struct usb_interface *intf)
+{
+	struct zr364xx_camera *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->lock);
+	usb_set_intfdata(intf, NULL);
+	dev_info(&intf->dev, DRIVER_DESC " webcam unplugged\n");
+	video_unregister_device(&cam->vdev);
+	v4l2_device_disconnect(&cam->v4l2_dev);
+
+	/* stops the read pipe if it is running */
+	if (cam->b_acquire)
+		zr364xx_stop_acquire(cam);
+
+	zr364xx_stop_readpipe(cam);
+	mutex_unlock(&cam->lock);
+	v4l2_device_put(&cam->v4l2_dev);
+}
+
+
+#ifdef CONFIG_PM
+static int zr364xx_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct zr364xx_camera *cam = usb_get_intfdata(intf);
+
+	cam->was_streaming = cam->b_acquire;
+	if (!cam->was_streaming)
+		return 0;
+	zr364xx_stop_acquire(cam);
+	zr364xx_stop_readpipe(cam);
+	return 0;
+}
+
+static int zr364xx_resume(struct usb_interface *intf)
+{
+	struct zr364xx_camera *cam = usb_get_intfdata(intf);
+	int res;
+
+	if (!cam->was_streaming)
+		return 0;
+
+	res = zr364xx_start_readpipe(cam);
+	if (res)
+		return res;
+
+	res = zr364xx_prepare(cam);
+	if (res)
+		goto err_prepare;
+
+	zr364xx_start_acquire(cam);
+	return 0;
+
+err_prepare:
+	zr364xx_stop_readpipe(cam);
+	return res;
+}
+#endif
+
+/**********************/
+/* Module integration */
+/**********************/
+
+static struct usb_driver zr364xx_driver = {
+	.name = "zr364xx",
+	.probe = zr364xx_probe,
+	.disconnect = zr364xx_disconnect,
+#ifdef CONFIG_PM
+	.suspend = zr364xx_suspend,
+	.resume = zr364xx_resume,
+	.reset_resume = zr364xx_resume,
+#endif
+	.id_table = device_table
+};
+
+module_usb_driver(zr364xx_driver);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
diff --git a/drivers/staging/media/imx/Kconfig b/drivers/staging/media/imx/Kconfig
new file mode 100644
index 000000000..0bacac302
--- /dev/null
+++ b/drivers/staging/media/imx/Kconfig
@@ -0,0 +1,34 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_IMX_MEDIA
+	tristate "i.MX5/6 V4L2 media core driver"
+	depends on ARCH_MXC || COMPILE_TEST
+	depends on HAS_DMA
+	depends on VIDEO_DEV
+	depends on VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select V4L2_MEM2MEM_DEV
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Say yes here to enable support for video4linux media controller
+	  driver for the i.MX5/6 SOC.
+
+if VIDEO_IMX_MEDIA
+menu "i.MX5/6/7/8 Media Sub devices"
+
+config VIDEO_IMX_CSI
+	tristate "i.MX5/6 Camera Sensor Interface driver"
+	depends on IMX_IPUV3_CORE
+	default y
+	help
+	  A video4linux camera sensor interface driver for i.MX5/6.
+
+config VIDEO_IMX7_CSI
+	tristate "i.MX6UL/L / i.MX7 / i.MX8M Camera Sensor Interface driver"
+	default y
+	help
+	  Enable support for video4linux camera sensor interface driver for
+	  i.MX6UL/L, i.MX7 or i.MX8M.
+endmenu
+endif
diff --git a/drivers/staging/media/imx/Makefile b/drivers/staging/media/imx/Makefile
new file mode 100644
index 000000000..d82be8981
--- /dev/null
+++ b/drivers/staging/media/imx/Makefile
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+imx-media-common-objs := imx-media-capture.o imx-media-dev-common.o \
+	imx-media-of.o imx-media-utils.o
+
+imx6-media-objs := imx-media-dev.o imx-media-internal-sd.o \
+	imx-ic-common.o imx-ic-prp.o imx-ic-prpencvf.o imx-media-vdic.o \
+	imx-media-csc-scaler.o
+
+imx6-media-csi-objs := imx-media-csi.o imx-media-fim.o
+
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-common.o
+
+obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-media.o
+obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-media-csi.o
+obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-mipi-csi2.o
+
+obj-$(CONFIG_VIDEO_IMX7_CSI) += imx7-media-csi.o
+obj-$(CONFIG_VIDEO_IMX7_CSI) += imx8mq-mipi-csi2.o
diff --git a/drivers/staging/media/imx/TODO b/drivers/staging/media/imx/TODO
new file mode 100644
index 000000000..5d3a337c8
--- /dev/null
+++ b/drivers/staging/media/imx/TODO
@@ -0,0 +1,54 @@
+
+- The Frame Interval Monitor could be exported to v4l2-core for
+  general use.
+
+- The CSI subdevice parses its nearest upstream neighbor's device-tree
+  bus config in order to setup the CSI. Laurent Pinchart argues that
+  instead the CSI subdev should call its neighbor's g_mbus_config op
+  (which should be propagated if necessary) to get this info. However
+  Hans Verkuil is planning to remove the g_mbus_config op. For now this
+  driver uses the parsed DT bus config method until this issue is
+  resolved.
+
+  2020-06: g_mbus has been removed in favour of the get_mbus_config pad
+  operation which should be used to avoid parsing the remote endpoint
+  configuration.
+
+- This media driver supports inheriting V4L2 controls to the
+  video capture devices, from the subdevices in the capture device's
+  pipeline. The controls for each capture device are updated in the
+  link_notify callback when the pipeline is modified. This feature should be
+  removed, userspace should use the subdev-based userspace API instead.
+
+- Similarly to the legacy control handling, legacy format handling where
+  formats on the video nodes are influenced by the active format of the
+  connected subdev should be removed.
+
+- i.MX7: all of the above, since it uses the imx media core
+
+- i.MX7: use Frame Interval Monitor
+
+- imx7-media-csi: Restrict the supported formats list to the SoC version.
+
+  The imx7 CSI bridge can be configured to sample pixel components from the Rx
+  queue in single (8bpp) or double (16bpp) component modes. Image format
+  variants with different sample sizes (ie YUYV_2X8 vs YUYV_1X16) determine the
+  pixel components sampling size per each clock cycle and their packing mode
+  (see imx7_csi_configure() for details).
+
+  As the imx7 CSI bridge can be interfaced with different IP blocks depending on
+  the SoC model it is integrated on, the Rx queue sampling size should match
+  the size of the samples transferred by the transmitting IP block.
+
+  To avoid mis-configurations of the capture pipeline, the enumeration of the
+  supported formats should be restricted to match the pixel source transmitting
+  mode.
+
+  Example: i.MX8MM SoC integrates the CSI bridge with the Samsung CSIS CSI-2
+  receiver which operates in dual pixel sampling mode. The CSI bridge should
+  only expose the 1X16 formats variant which instructs it to operate in dual
+  pixel sampling mode. When the CSI bridge is instead integrated on an i.MX7,
+  which supports both serial and parallel input, it should expose both variants.
+
+  This currently only applies to YUYV formats, but other formats might need
+  to be handled in the same way.
diff --git a/drivers/staging/media/imx/imx-ic-common.c b/drivers/staging/media/imx/imx-ic-common.c
new file mode 100644
index 000000000..6df1ffb53
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-common.c
@@ -0,0 +1,87 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2014-2016 Mentor Graphics Inc.
+ */
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+#define IC_TASK_PRP IC_NUM_TASKS
+#define IC_NUM_OPS  (IC_NUM_TASKS + 1)
+
+static struct imx_ic_ops *ic_ops[IC_NUM_OPS] = {
+	[IC_TASK_PRP]            = &imx_ic_prp_ops,
+	[IC_TASK_ENCODER]        = &imx_ic_prpencvf_ops,
+	[IC_TASK_VIEWFINDER]     = &imx_ic_prpencvf_ops,
+};
+
+struct v4l2_subdev *imx_media_ic_register(struct v4l2_device *v4l2_dev,
+					  struct device *ipu_dev,
+					  struct ipu_soc *ipu,
+					  u32 grp_id)
+{
+	struct imx_ic_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->ipu_dev = ipu_dev;
+	priv->ipu = ipu;
+
+	/* get our IC task id */
+	switch (grp_id) {
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRP:
+		priv->task_id = IC_TASK_PRP;
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRPENC:
+		priv->task_id = IC_TASK_ENCODER;
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRPVF:
+		priv->task_id = IC_TASK_VIEWFINDER;
+		break;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+
+	v4l2_subdev_init(&priv->sd, ic_ops[priv->task_id]->subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = ic_ops[priv->task_id]->internal_ops;
+	priv->sd.entity.ops = ic_ops[priv->task_id]->entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+	priv->sd.owner = ipu_dev->driver->owner;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	priv->sd.grp_id = grp_id;
+	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
+				    priv->sd.grp_id, ipu_get_num(ipu));
+
+	ret = ic_ops[priv->task_id]->init(priv);
+	if (ret)
+		return ERR_PTR(ret);
+
+	ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
+	if (ret) {
+		ic_ops[priv->task_id]->remove(priv);
+		return ERR_PTR(ret);
+	}
+
+	return &priv->sd;
+}
+
+int imx_media_ic_unregister(struct v4l2_subdev *sd)
+{
+	struct imx_ic_priv *priv = container_of(sd, struct imx_ic_priv, sd);
+
+	v4l2_info(sd, "Removing\n");
+
+	ic_ops[priv->task_id]->remove(priv);
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
diff --git a/drivers/staging/media/imx/imx-ic-prp.c b/drivers/staging/media/imx/imx-ic-prp.c
new file mode 100644
index 000000000..ac5fb3320
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-prp.c
@@ -0,0 +1,514 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC
+ *
+ * This subdevice handles capture of video frames from the CSI or VDIC,
+ * which are routed directly to the Image Converter preprocess tasks,
+ * for resizing, colorspace conversion, and rotation.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ */
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+/*
+ * Min/Max supported width and heights.
+ */
+#define MIN_W        32
+#define MIN_H        32
+#define MAX_W      4096
+#define MAX_H      4096
+#define W_ALIGN    4 /* multiple of 16 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+struct prp_priv {
+	struct imx_ic_priv *ic_priv;
+	struct media_pad pad[PRP_NUM_PADS];
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	struct v4l2_subdev *src_sd;
+	struct v4l2_subdev *sink_sd_prpenc;
+	struct v4l2_subdev *sink_sd_prpvf;
+
+	/* the CSI id at link validate */
+	int csi_id;
+
+	struct v4l2_mbus_framefmt format_mbus;
+	struct v4l2_fract frame_interval;
+
+	int stream_count;
+};
+
+static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+
+	return ic_priv->task_priv;
+}
+
+static int prp_start(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	bool src_is_vdic;
+
+	/* set IC to receive from CSI or VDI depending on source */
+	src_is_vdic = !!(priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_VDIC);
+
+	ipu_set_ic_src_mux(ic_priv->ipu, priv->csi_id, src_is_vdic);
+
+	return 0;
+}
+
+static void prp_stop(struct prp_priv *priv)
+{
+}
+
+static struct v4l2_mbus_framefmt *
+__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_state *sd_state,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ic_priv->sd, sd_state, pad);
+	else
+		return &priv->format_mbus;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int prp_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *infmt;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	switch (code->pad) {
+	case PRP_SINK_PAD:
+		ret = imx_media_enum_ipu_formats(&code->code, code->index,
+						 PIXFMT_SEL_YUV_RGB);
+		break;
+	case PRP_SRC_PAD_PRPENC:
+	case PRP_SRC_PAD_PRPVF:
+		if (code->index != 0) {
+			ret = -EINVAL;
+			goto out;
+		}
+		infmt = __prp_get_fmt(priv, sd_state, PRP_SINK_PAD,
+				      code->which);
+		code->code = infmt->code;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __prp_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt, *infmt;
+	const struct imx_media_pixfmt *cc;
+	int ret = 0;
+	u32 code;
+
+	if (sdformat->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	infmt = __prp_get_fmt(priv, sd_state, PRP_SINK_PAD, sdformat->which);
+
+	switch (sdformat->pad) {
+	case PRP_SINK_PAD:
+		v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W,
+				      W_ALIGN, &sdformat->format.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+
+		cc = imx_media_find_ipu_format(sdformat->format.code,
+					       PIXFMT_SEL_YUV_RGB);
+		if (!cc) {
+			imx_media_enum_ipu_formats(&code, 0,
+						   PIXFMT_SEL_YUV_RGB);
+			cc = imx_media_find_ipu_format(code,
+						       PIXFMT_SEL_YUV_RGB);
+			sdformat->format.code = cc->codes[0];
+		}
+
+		if (sdformat->format.field == V4L2_FIELD_ANY)
+			sdformat->format.field = V4L2_FIELD_NONE;
+		break;
+	case PRP_SRC_PAD_PRPENC:
+	case PRP_SRC_PAD_PRPVF:
+		/* Output pads mirror input pad */
+		sdformat->format = *infmt;
+		break;
+	}
+
+	imx_media_try_colorimetry(&sdformat->format, true);
+
+	fmt = __prp_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(ic_priv->ipu_dev, "%s: link setup %s -> %s",
+		ic_priv->sd.name, remote->entity->name, local->entity->name);
+
+	remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			if (priv->sink_sd_prpenc &&
+			    (remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_VDIC)) {
+				ret = -EINVAL;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a source pad */
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		switch (local->index) {
+		case PRP_SRC_PAD_PRPENC:
+			if (priv->sink_sd_prpenc) {
+				ret = -EBUSY;
+				goto out;
+			}
+			if (priv->src_sd && (priv->src_sd->grp_id &
+					     IMX_MEDIA_GRP_ID_IPU_VDIC)) {
+				ret = -EINVAL;
+				goto out;
+			}
+			priv->sink_sd_prpenc = remote_sd;
+			break;
+		case PRP_SRC_PAD_PRPVF:
+			if (priv->sink_sd_prpvf) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->sink_sd_prpvf = remote_sd;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		switch (local->index) {
+		case PRP_SRC_PAD_PRPENC:
+			priv->sink_sd_prpenc = NULL;
+			break;
+		case PRP_SRC_PAD_PRPVF:
+			priv->sink_sd_prpvf = NULL;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_validate(struct v4l2_subdev *sd,
+			     struct media_link *link,
+			     struct v4l2_subdev_format *source_fmt,
+			     struct v4l2_subdev_format *sink_fmt)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	struct v4l2_subdev *csi;
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	csi = imx_media_pipeline_subdev(&ic_priv->sd.entity,
+					IMX_MEDIA_GRP_ID_IPU_CSI, true);
+	if (IS_ERR(csi))
+		csi = NULL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_VDIC) {
+		/*
+		 * the ->PRPENC link cannot be enabled if the source
+		 * is the VDIC
+		 */
+		if (priv->sink_sd_prpenc) {
+			ret = -EINVAL;
+			goto out;
+		}
+	} else {
+		/* the source is a CSI */
+		if (!csi) {
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+	if (csi) {
+		switch (csi->grp_id) {
+		case IMX_MEDIA_GRP_ID_IPU_CSI0:
+			priv->csi_id = 0;
+			break;
+		case IMX_MEDIA_GRP_ID_IPU_CSI1:
+			priv->csi_id = 1;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		priv->csi_id = 0;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || (!priv->sink_sd_prpenc && !priv->sink_sd_prpvf)) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(ic_priv->ipu_dev, "%s: stream %s\n", sd->name,
+		enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = prp_start(priv);
+	else
+		prp_stop(priv);
+	if (ret)
+		goto out;
+
+	/* start/stop upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, enable);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret) {
+		if (enable)
+			prp_stop(priv);
+		goto out;
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+	fi->interval = priv->frame_interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	/* No limits on valid frame intervals */
+	if (fi->interval.numerator == 0 || fi->interval.denominator == 0)
+		fi->interval = priv->frame_interval;
+	else
+		priv->frame_interval = fi->interval;
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_registered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	u32 code;
+
+	/* init default frame interval */
+	priv->frame_interval.numerator = 1;
+	priv->frame_interval.denominator = 30;
+
+	/* set a default mbus format  */
+	imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
+
+	return imx_media_init_mbus_fmt(&priv->format_mbus,
+				       IMX_MEDIA_DEF_PIX_WIDTH,
+				       IMX_MEDIA_DEF_PIX_HEIGHT, code,
+				       V4L2_FIELD_NONE, NULL);
+}
+
+static const struct v4l2_subdev_pad_ops prp_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
+	.enum_mbus_code = prp_enum_mbus_code,
+	.get_fmt = prp_get_fmt,
+	.set_fmt = prp_set_fmt,
+	.link_validate = prp_link_validate,
+};
+
+static const struct v4l2_subdev_video_ops prp_video_ops = {
+	.g_frame_interval = prp_g_frame_interval,
+	.s_frame_interval = prp_s_frame_interval,
+	.s_stream = prp_s_stream,
+};
+
+static const struct media_entity_operations prp_entity_ops = {
+	.link_setup = prp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops prp_subdev_ops = {
+	.video = &prp_video_ops,
+	.pad = &prp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops prp_internal_ops = {
+	.registered = prp_registered,
+};
+
+static int prp_init(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv;
+	int i;
+
+	priv = devm_kzalloc(ic_priv->ipu_dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	mutex_init(&priv->lock);
+	ic_priv->task_priv = priv;
+	priv->ic_priv = ic_priv;
+
+	for (i = 0; i < PRP_NUM_PADS; i++)
+		priv->pad[i].flags = (i == PRP_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	return media_entity_pads_init(&ic_priv->sd.entity, PRP_NUM_PADS,
+				      priv->pad);
+}
+
+static void prp_remove(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv = ic_priv->task_priv;
+
+	mutex_destroy(&priv->lock);
+}
+
+struct imx_ic_ops imx_ic_prp_ops = {
+	.subdev_ops = &prp_subdev_ops,
+	.internal_ops = &prp_internal_ops,
+	.entity_ops = &prp_entity_ops,
+	.init = prp_init,
+	.remove = prp_remove,
+};
diff --git a/drivers/staging/media/imx/imx-ic-prpencvf.c b/drivers/staging/media/imx/imx-ic-prpencvf.c
new file mode 100644
index 000000000..9b81cfbcd
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-prpencvf.c
@@ -0,0 +1,1372 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC
+ *
+ * This subdevice handles capture of video frames from the CSI or VDIC,
+ * which are routed directly to the Image Converter preprocess tasks,
+ * for resizing, colorspace conversion, and rotation.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ */
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+/*
+ * Min/Max supported width and heights.
+ *
+ * We allow planar output, so we have to align width at the source pad
+ * by 16 pixels to meet IDMAC alignment requirements for possible planar
+ * output.
+ *
+ * TODO: move this into pad format negotiation, if capture device
+ * has not requested a planar format, we should allow 8 pixel
+ * alignment at the source pad.
+ */
+#define MIN_W_SINK   32
+#define MIN_H_SINK   32
+#define MAX_W_SINK 4096
+#define MAX_H_SINK 4096
+#define W_ALIGN_SINK  3 /* multiple of 8 pixels */
+#define H_ALIGN_SINK  1 /* multiple of 2 lines */
+
+#define MAX_W_SRC  1024
+#define MAX_H_SRC  1024
+#define W_ALIGN_SRC   1 /* multiple of 2 pixels */
+#define H_ALIGN_SRC   1 /* multiple of 2 lines */
+
+#define S_ALIGN       1 /* multiple of 2 */
+
+struct prp_priv {
+	struct imx_ic_priv *ic_priv;
+	struct media_pad pad[PRPENCVF_NUM_PADS];
+	/* the video device at output pad */
+	struct imx_media_video_dev *vdev;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	/* IPU units we require */
+	struct ipu_ic *ic;
+	struct ipuv3_channel *out_ch;
+	struct ipuv3_channel *rot_in_ch;
+	struct ipuv3_channel *rot_out_ch;
+
+	/* active vb2 buffers to send to video dev sink */
+	struct imx_media_buffer *active_vb2_buf[2];
+	struct imx_media_dma_buf underrun_buf;
+
+	int ipu_buf_num;  /* ipu double buffer index: 0-1 */
+
+	/* the sink for the captured frames */
+	struct media_entity *sink;
+	/* the source subdev */
+	struct v4l2_subdev *src_sd;
+
+	struct v4l2_mbus_framefmt format_mbus[PRPENCVF_NUM_PADS];
+	const struct imx_media_pixfmt *cc[PRPENCVF_NUM_PADS];
+	struct v4l2_fract frame_interval;
+
+	struct imx_media_dma_buf rot_buf[2];
+
+	/* controls */
+	struct v4l2_ctrl_handler ctrl_hdlr;
+	int  rotation; /* degrees */
+	bool hflip;
+	bool vflip;
+
+	/* derived from rotation, hflip, vflip controls */
+	enum ipu_rotate_mode rot_mode;
+
+	spinlock_t irqlock; /* protect eof_irq handler */
+
+	struct timer_list eof_timeout_timer;
+	int eof_irq;
+	int nfb4eof_irq;
+
+	int stream_count;
+	u32 frame_sequence; /* frame sequence counter */
+	bool last_eof;  /* waiting for last EOF at stream off */
+	bool nfb4eof;    /* NFB4EOF encountered during streaming */
+	bool interweave_swap; /* swap top/bottom lines when interweaving */
+	struct completion last_eof_comp;
+};
+
+static const struct prp_channels {
+	u32 out_ch;
+	u32 rot_in_ch;
+	u32 rot_out_ch;
+} prp_channel[] = {
+	[IC_TASK_ENCODER] = {
+		.out_ch = IPUV3_CHANNEL_IC_PRP_ENC_MEM,
+		.rot_in_ch = IPUV3_CHANNEL_MEM_ROT_ENC,
+		.rot_out_ch = IPUV3_CHANNEL_ROT_ENC_MEM,
+	},
+	[IC_TASK_VIEWFINDER] = {
+		.out_ch = IPUV3_CHANNEL_IC_PRP_VF_MEM,
+		.rot_in_ch = IPUV3_CHANNEL_MEM_ROT_VF,
+		.rot_out_ch = IPUV3_CHANNEL_ROT_VF_MEM,
+	},
+};
+
+static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+
+	return ic_priv->task_priv;
+}
+
+static void prp_put_ipu_resources(struct prp_priv *priv)
+{
+	if (priv->ic)
+		ipu_ic_put(priv->ic);
+	priv->ic = NULL;
+
+	if (priv->out_ch)
+		ipu_idmac_put(priv->out_ch);
+	priv->out_ch = NULL;
+
+	if (priv->rot_in_ch)
+		ipu_idmac_put(priv->rot_in_ch);
+	priv->rot_in_ch = NULL;
+
+	if (priv->rot_out_ch)
+		ipu_idmac_put(priv->rot_out_ch);
+	priv->rot_out_ch = NULL;
+}
+
+static int prp_get_ipu_resources(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	struct ipu_ic *ic;
+	struct ipuv3_channel *out_ch, *rot_in_ch, *rot_out_ch;
+	int ret, task = ic_priv->task_id;
+
+	ic = ipu_ic_get(ic_priv->ipu, task);
+	if (IS_ERR(ic)) {
+		v4l2_err(&ic_priv->sd, "failed to get IC\n");
+		ret = PTR_ERR(ic);
+		goto out;
+	}
+	priv->ic = ic;
+
+	out_ch = ipu_idmac_get(ic_priv->ipu, prp_channel[task].out_ch);
+	if (IS_ERR(out_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].out_ch);
+		ret = PTR_ERR(out_ch);
+		goto out;
+	}
+	priv->out_ch = out_ch;
+
+	rot_in_ch = ipu_idmac_get(ic_priv->ipu, prp_channel[task].rot_in_ch);
+	if (IS_ERR(rot_in_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].rot_in_ch);
+		ret = PTR_ERR(rot_in_ch);
+		goto out;
+	}
+	priv->rot_in_ch = rot_in_ch;
+
+	rot_out_ch = ipu_idmac_get(ic_priv->ipu, prp_channel[task].rot_out_ch);
+	if (IS_ERR(rot_out_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].rot_out_ch);
+		ret = PTR_ERR(rot_out_ch);
+		goto out;
+	}
+	priv->rot_out_ch = rot_out_ch;
+
+	return 0;
+out:
+	prp_put_ipu_resources(priv);
+	return ret;
+}
+
+static void prp_vb2_buf_done(struct prp_priv *priv, struct ipuv3_channel *ch)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *done, *next;
+	struct vb2_buffer *vb;
+	dma_addr_t phys;
+
+	done = priv->active_vb2_buf[priv->ipu_buf_num];
+	if (done) {
+		done->vbuf.field = vdev->fmt.field;
+		done->vbuf.sequence = priv->frame_sequence;
+		vb = &done->vbuf.vb2_buf;
+		vb->timestamp = ktime_get_ns();
+		vb2_buffer_done(vb, priv->nfb4eof ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	}
+
+	priv->frame_sequence++;
+	priv->nfb4eof = false;
+
+	/* get next queued buffer */
+	next = imx_media_capture_device_next_buf(vdev);
+	if (next) {
+		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
+		priv->active_vb2_buf[priv->ipu_buf_num] = next;
+	} else {
+		phys = priv->underrun_buf.phys;
+		priv->active_vb2_buf[priv->ipu_buf_num] = NULL;
+	}
+
+	if (ipu_idmac_buffer_is_ready(ch, priv->ipu_buf_num))
+		ipu_idmac_clear_buffer(ch, priv->ipu_buf_num);
+
+	if (priv->interweave_swap && ch == priv->out_ch)
+		phys += vdev->fmt.bytesperline;
+
+	ipu_cpmem_set_buffer(ch, priv->ipu_buf_num, phys);
+}
+
+static irqreturn_t prp_eof_interrupt(int irq, void *dev_id)
+{
+	struct prp_priv *priv = dev_id;
+	struct ipuv3_channel *channel;
+
+	spin_lock(&priv->irqlock);
+
+	if (priv->last_eof) {
+		complete(&priv->last_eof_comp);
+		priv->last_eof = false;
+		goto unlock;
+	}
+
+	channel = (ipu_rot_mode_is_irt(priv->rot_mode)) ?
+		priv->rot_out_ch : priv->out_ch;
+
+	prp_vb2_buf_done(priv, channel);
+
+	/* select new IPU buf */
+	ipu_idmac_select_buffer(channel, priv->ipu_buf_num);
+	/* toggle IPU double-buffer index */
+	priv->ipu_buf_num ^= 1;
+
+	/* bump the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+unlock:
+	spin_unlock(&priv->irqlock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t prp_nfb4eof_interrupt(int irq, void *dev_id)
+{
+	struct prp_priv *priv = dev_id;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	spin_lock(&priv->irqlock);
+
+	/*
+	 * this is not an unrecoverable error, just mark
+	 * the next captured frame with vb2 error flag.
+	 */
+	priv->nfb4eof = true;
+
+	v4l2_err(&ic_priv->sd, "NFB4EOF\n");
+
+	spin_unlock(&priv->irqlock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * EOF timeout timer function.
+ */
+/*
+ * EOF timeout timer function. This is an unrecoverable condition
+ * without a stream restart.
+ */
+static void prp_eof_timeout(struct timer_list *t)
+{
+	struct prp_priv *priv = from_timer(priv, t, eof_timeout_timer);
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	v4l2_err(&ic_priv->sd, "EOF timeout\n");
+
+	/* signal a fatal error to capture device */
+	imx_media_capture_device_error(vdev);
+}
+
+static void prp_setup_vb2_buf(struct prp_priv *priv, dma_addr_t *phys)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *buf;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		buf = imx_media_capture_device_next_buf(vdev);
+		if (buf) {
+			priv->active_vb2_buf[i] = buf;
+			phys[i] = vb2_dma_contig_plane_dma_addr(
+				&buf->vbuf.vb2_buf, 0);
+		} else {
+			priv->active_vb2_buf[i] = NULL;
+			phys[i] = priv->underrun_buf.phys;
+		}
+	}
+}
+
+static void prp_unsetup_vb2_buf(struct prp_priv *priv,
+				enum vb2_buffer_state return_status)
+{
+	struct imx_media_buffer *buf;
+	int i;
+
+	/* return any remaining active frames with return_status */
+	for (i = 0; i < 2; i++) {
+		buf = priv->active_vb2_buf[i];
+		if (buf) {
+			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vb2_buffer_done(vb, return_status);
+		}
+	}
+}
+
+static int prp_setup_channel(struct prp_priv *priv,
+			     struct ipuv3_channel *channel,
+			     enum ipu_rotate_mode rot_mode,
+			     dma_addr_t addr0, dma_addr_t addr1,
+			     bool rot_swap_width_height)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *outcc;
+	struct v4l2_mbus_framefmt *outfmt;
+	unsigned int burst_size;
+	struct ipu_image image;
+	bool interweave;
+	int ret;
+
+	outfmt = &priv->format_mbus[PRPENCVF_SRC_PAD];
+	outcc = vdev->cc;
+
+	ipu_cpmem_zero(channel);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt;
+	image.rect = vdev->compose;
+
+	/*
+	 * If the field type at capture interface is interlaced, and
+	 * the output IDMAC pad is sequential, enable interweave at
+	 * the IDMAC output channel.
+	 */
+	interweave = V4L2_FIELD_IS_INTERLACED(image.pix.field) &&
+		V4L2_FIELD_IS_SEQUENTIAL(outfmt->field);
+	priv->interweave_swap = interweave &&
+		image.pix.field == V4L2_FIELD_INTERLACED_BT;
+
+	if (rot_swap_width_height) {
+		swap(image.pix.width, image.pix.height);
+		swap(image.rect.width, image.rect.height);
+		/* recalc stride using swapped width */
+		image.pix.bytesperline = outcc->planar ?
+			image.pix.width :
+			(image.pix.width * outcc->bpp) >> 3;
+	}
+
+	if (priv->interweave_swap && channel == priv->out_ch) {
+		/* start interweave scan at 1st top line (2nd line) */
+		image.rect.top = 1;
+	}
+
+	image.phys0 = addr0;
+	image.phys1 = addr1;
+
+	/*
+	 * Skip writing U and V components to odd rows in the output
+	 * channels for planar 4:2:0 (but not when enabling IDMAC
+	 * interweaving, they are incompatible).
+	 */
+	if ((channel == priv->out_ch && !interweave) ||
+	    channel == priv->rot_out_ch) {
+		switch (image.pix.pixelformat) {
+		case V4L2_PIX_FMT_YUV420:
+		case V4L2_PIX_FMT_YVU420:
+		case V4L2_PIX_FMT_NV12:
+			ipu_cpmem_skip_odd_chroma_rows(channel);
+			break;
+		}
+	}
+
+	ret = ipu_cpmem_set_image(channel, &image);
+	if (ret)
+		return ret;
+
+	if (channel == priv->rot_in_ch ||
+	    channel == priv->rot_out_ch) {
+		burst_size = 8;
+		ipu_cpmem_set_block_mode(channel);
+	} else {
+		burst_size = (image.pix.width & 0xf) ? 8 : 16;
+	}
+
+	ipu_cpmem_set_burstsize(channel, burst_size);
+
+	if (rot_mode)
+		ipu_cpmem_set_rotation(channel, rot_mode);
+
+	if (interweave && channel == priv->out_ch)
+		ipu_cpmem_interlaced_scan(channel,
+					  priv->interweave_swap ?
+					  -image.pix.bytesperline :
+					  image.pix.bytesperline,
+					  image.pix.pixelformat);
+
+	ret = ipu_ic_task_idma_init(priv->ic, channel,
+				    image.pix.width, image.pix.height,
+				    burst_size, rot_mode);
+	if (ret)
+		return ret;
+
+	ipu_cpmem_set_axi_id(channel, 1);
+
+	ipu_idmac_set_double_buffer(channel, true);
+
+	return 0;
+}
+
+static int prp_setup_rotation(struct prp_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	const struct imx_media_pixfmt *outcc, *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_pix_format *outfmt;
+	struct ipu_ic_csc csc;
+	dma_addr_t phys[2];
+	int ret;
+
+	infmt = &priv->format_mbus[PRPENCVF_SINK_PAD];
+	outfmt = &vdev->fmt;
+	incc = priv->cc[PRPENCVF_SINK_PAD];
+	outcc = vdev->cc;
+
+	ret = ipu_ic_calc_csc(&csc,
+			      infmt->ycbcr_enc, infmt->quantization,
+			      incc->cs,
+			      outfmt->ycbcr_enc, outfmt->quantization,
+			      outcc->cs);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_calc_csc failed, %d\n",
+			 ret);
+		return ret;
+	}
+
+	ret = imx_media_alloc_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[0],
+				      outfmt->sizeimage);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[0], %d\n", ret);
+		return ret;
+	}
+	ret = imx_media_alloc_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[1],
+				      outfmt->sizeimage);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[1], %d\n", ret);
+		goto free_rot0;
+	}
+
+	ret = ipu_ic_task_init(priv->ic, &csc,
+			       infmt->width, infmt->height,
+			       outfmt->height, outfmt->width);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	/* init the IC-PRP-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->out_ch, IPU_ROTATE_NONE,
+				priv->rot_buf[0].phys, priv->rot_buf[1].phys,
+				true);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(out_ch) failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	/* init the MEM-->IC-PRP ROT IDMAC channel */
+	ret = prp_setup_channel(priv, priv->rot_in_ch, priv->rot_mode,
+				priv->rot_buf[0].phys, priv->rot_buf[1].phys,
+				true);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(rot_in_ch) failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	prp_setup_vb2_buf(priv, phys);
+
+	/* init the destination IC-PRP ROT-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->rot_out_ch, IPU_ROTATE_NONE,
+				phys[0], phys[1],
+				false);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(rot_out_ch) failed, %d\n", ret);
+		goto unsetup_vb2;
+	}
+
+	/* now link IC-PRP-->MEM to MEM-->IC-PRP ROT */
+	ipu_idmac_link(priv->out_ch, priv->rot_in_ch);
+
+	/* enable the IC */
+	ipu_ic_enable(priv->ic);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->out_ch, 0);
+	ipu_idmac_select_buffer(priv->out_ch, 1);
+	ipu_idmac_select_buffer(priv->rot_out_ch, 0);
+	ipu_idmac_select_buffer(priv->rot_out_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->out_ch);
+	ipu_idmac_enable_channel(priv->rot_in_ch);
+	ipu_idmac_enable_channel(priv->rot_out_ch);
+
+	/* and finally enable the IC PRP task */
+	ipu_ic_task_enable(priv->ic);
+
+	return 0;
+
+unsetup_vb2:
+	prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+free_rot1:
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[1]);
+free_rot0:
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[0]);
+	return ret;
+}
+
+static void prp_unsetup_rotation(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	ipu_ic_task_disable(priv->ic);
+
+	ipu_idmac_disable_channel(priv->out_ch);
+	ipu_idmac_disable_channel(priv->rot_in_ch);
+	ipu_idmac_disable_channel(priv->rot_out_ch);
+
+	ipu_idmac_unlink(priv->out_ch, priv->rot_in_ch);
+
+	ipu_ic_disable(priv->ic);
+
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[0]);
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->rot_buf[1]);
+}
+
+static int prp_setup_norotation(struct prp_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	const struct imx_media_pixfmt *outcc, *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_pix_format *outfmt;
+	struct ipu_ic_csc csc;
+	dma_addr_t phys[2];
+	int ret;
+
+	infmt = &priv->format_mbus[PRPENCVF_SINK_PAD];
+	outfmt = &vdev->fmt;
+	incc = priv->cc[PRPENCVF_SINK_PAD];
+	outcc = vdev->cc;
+
+	ret = ipu_ic_calc_csc(&csc,
+			      infmt->ycbcr_enc, infmt->quantization,
+			      incc->cs,
+			      outfmt->ycbcr_enc, outfmt->quantization,
+			      outcc->cs);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_calc_csc failed, %d\n",
+			 ret);
+		return ret;
+	}
+
+	ret = ipu_ic_task_init(priv->ic, &csc,
+			       infmt->width, infmt->height,
+			       outfmt->width, outfmt->height);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret);
+		return ret;
+	}
+
+	prp_setup_vb2_buf(priv, phys);
+
+	/* init the IC PRP-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->out_ch, priv->rot_mode,
+				phys[0], phys[1], false);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(out_ch) failed, %d\n", ret);
+		goto unsetup_vb2;
+	}
+
+	ipu_cpmem_dump(priv->out_ch);
+	ipu_ic_dump(priv->ic);
+	ipu_dump(ic_priv->ipu);
+
+	ipu_ic_enable(priv->ic);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->out_ch, 0);
+	ipu_idmac_select_buffer(priv->out_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->out_ch);
+
+	/* enable the IC task */
+	ipu_ic_task_enable(priv->ic);
+
+	return 0;
+
+unsetup_vb2:
+	prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void prp_unsetup_norotation(struct prp_priv *priv)
+{
+	ipu_ic_task_disable(priv->ic);
+	ipu_idmac_disable_channel(priv->out_ch);
+	ipu_ic_disable(priv->ic);
+}
+
+static void prp_unsetup(struct prp_priv *priv,
+			enum vb2_buffer_state state)
+{
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		prp_unsetup_rotation(priv);
+	else
+		prp_unsetup_norotation(priv);
+
+	prp_unsetup_vb2_buf(priv, state);
+}
+
+static int prp_start(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	struct imx_media_video_dev *vdev = priv->vdev;
+	int ret;
+
+	ret = prp_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	ret = imx_media_alloc_dma_buf(ic_priv->ipu_dev, &priv->underrun_buf,
+				      vdev->fmt.sizeimage);
+	if (ret)
+		goto out_put_ipu;
+
+	priv->ipu_buf_num = 0;
+
+	/* init EOF completion waitq */
+	init_completion(&priv->last_eof_comp);
+	priv->frame_sequence = 0;
+	priv->last_eof = false;
+	priv->nfb4eof = false;
+
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		ret = prp_setup_rotation(priv);
+	else
+		ret = prp_setup_norotation(priv);
+	if (ret)
+		goto out_free_underrun;
+
+	priv->nfb4eof_irq = ipu_idmac_channel_irq(ic_priv->ipu,
+						  priv->out_ch,
+						  IPU_IRQ_NFB4EOF);
+	ret = devm_request_irq(ic_priv->ipu_dev, priv->nfb4eof_irq,
+			       prp_nfb4eof_interrupt, 0,
+			       "imx-ic-prp-nfb4eof", priv);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "Error registering NFB4EOF irq: %d\n", ret);
+		goto out_unsetup;
+	}
+
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		priv->eof_irq = ipu_idmac_channel_irq(
+			ic_priv->ipu, priv->rot_out_ch, IPU_IRQ_EOF);
+	else
+		priv->eof_irq = ipu_idmac_channel_irq(
+			ic_priv->ipu, priv->out_ch, IPU_IRQ_EOF);
+
+	ret = devm_request_irq(ic_priv->ipu_dev, priv->eof_irq,
+			       prp_eof_interrupt, 0,
+			       "imx-ic-prp-eof", priv);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "Error registering eof irq: %d\n", ret);
+		goto out_free_nfb4eof_irq;
+	}
+
+	/* start upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, 1);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "upstream stream on failed: %d\n", ret);
+		goto out_free_eof_irq;
+	}
+
+	/* start the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+	return 0;
+
+out_free_eof_irq:
+	devm_free_irq(ic_priv->ipu_dev, priv->eof_irq, priv);
+out_free_nfb4eof_irq:
+	devm_free_irq(ic_priv->ipu_dev, priv->nfb4eof_irq, priv);
+out_unsetup:
+	prp_unsetup(priv, VB2_BUF_STATE_QUEUED);
+out_free_underrun:
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->underrun_buf);
+out_put_ipu:
+	prp_put_ipu_resources(priv);
+	return ret;
+}
+
+static void prp_stop(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	unsigned long flags;
+	int ret;
+
+	/* mark next EOF interrupt as the last before stream off */
+	spin_lock_irqsave(&priv->irqlock, flags);
+	priv->last_eof = true;
+	spin_unlock_irqrestore(&priv->irqlock, flags);
+
+	/*
+	 * and then wait for interrupt handler to mark completion.
+	 */
+	ret = wait_for_completion_timeout(
+		&priv->last_eof_comp,
+		msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(&ic_priv->sd, "wait last EOF timeout\n");
+
+	/* stop upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD)
+		v4l2_warn(&ic_priv->sd,
+			  "upstream stream off failed: %d\n", ret);
+
+	devm_free_irq(ic_priv->ipu_dev, priv->eof_irq, priv);
+	devm_free_irq(ic_priv->ipu_dev, priv->nfb4eof_irq, priv);
+
+	prp_unsetup(priv, VB2_BUF_STATE_ERROR);
+
+	imx_media_free_dma_buf(ic_priv->ipu_dev, &priv->underrun_buf);
+
+	/* cancel the EOF timeout timer */
+	del_timer_sync(&priv->eof_timeout_timer);
+
+	prp_put_ipu_resources(priv);
+}
+
+static struct v4l2_mbus_framefmt *
+__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_state *sd_state,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ic_priv->sd, sd_state, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+/*
+ * Applies IC resizer and IDMAC alignment restrictions to output
+ * rectangle given the input rectangle, and depending on given
+ * rotation mode.
+ *
+ * The IC resizer cannot downsize more than 4:1. Note also that
+ * for 90 or 270 rotation, _both_ output width and height must
+ * be aligned by W_ALIGN_SRC, because the intermediate rotation
+ * buffer swaps output width/height, and the final output buffer
+ * does not.
+ *
+ * Returns true if the output rectangle was modified.
+ */
+static bool prp_bound_align_output(struct v4l2_mbus_framefmt *outfmt,
+				   struct v4l2_mbus_framefmt *infmt,
+				   enum ipu_rotate_mode rot_mode)
+{
+	u32 orig_width = outfmt->width;
+	u32 orig_height = outfmt->height;
+
+	if (ipu_rot_mode_is_irt(rot_mode))
+		v4l_bound_align_image(&outfmt->width,
+				      infmt->height / 4, MAX_H_SRC,
+				      W_ALIGN_SRC,
+				      &outfmt->height,
+				      infmt->width / 4, MAX_W_SRC,
+				      W_ALIGN_SRC, S_ALIGN);
+	else
+		v4l_bound_align_image(&outfmt->width,
+				      infmt->width / 4, MAX_W_SRC,
+				      W_ALIGN_SRC,
+				      &outfmt->height,
+				      infmt->height / 4, MAX_H_SRC,
+				      H_ALIGN_SRC, S_ALIGN);
+
+	return outfmt->width != orig_width || outfmt->height != orig_height;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int prp_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	return imx_media_enum_ipu_formats(&code->code, code->index,
+					  PIXFMT_SEL_YUV_RGB);
+}
+
+static int prp_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __prp_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void prp_try_fmt(struct prp_priv *priv,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat,
+			const struct imx_media_pixfmt **cc)
+{
+	struct v4l2_mbus_framefmt *infmt;
+
+	*cc = imx_media_find_ipu_format(sdformat->format.code,
+					PIXFMT_SEL_YUV_RGB);
+	if (!*cc) {
+		u32 code;
+
+		imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV_RGB);
+		*cc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV_RGB);
+
+		sdformat->format.code = (*cc)->codes[0];
+	}
+
+	infmt = __prp_get_fmt(priv, sd_state, PRPENCVF_SINK_PAD,
+			      sdformat->which);
+
+	if (sdformat->pad == PRPENCVF_SRC_PAD) {
+		sdformat->format.field = infmt->field;
+
+		prp_bound_align_output(&sdformat->format, infmt,
+				       priv->rot_mode);
+
+		/* propagate colorimetry from sink */
+		sdformat->format.colorspace = infmt->colorspace;
+		sdformat->format.xfer_func = infmt->xfer_func;
+	} else {
+		v4l_bound_align_image(&sdformat->format.width,
+				      MIN_W_SINK, MAX_W_SINK, W_ALIGN_SINK,
+				      &sdformat->format.height,
+				      MIN_H_SINK, MAX_H_SINK, H_ALIGN_SINK,
+				      S_ALIGN);
+
+		if (sdformat->format.field == V4L2_FIELD_ANY)
+			sdformat->format.field = V4L2_FIELD_NONE;
+	}
+
+	imx_media_try_colorimetry(&sdformat->format, true);
+}
+
+static int prp_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	prp_try_fmt(priv, sd_state, sdformat, &cc);
+
+	fmt = __prp_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	/* propagate a default format to source pad */
+	if (sdformat->pad == PRPENCVF_SINK_PAD) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_mbus_framefmt *outfmt;
+		struct v4l2_subdev_format format;
+
+		format.pad = PRPENCVF_SRC_PAD;
+		format.which = sdformat->which;
+		format.format = sdformat->format;
+		prp_try_fmt(priv, sd_state, &format, &outcc);
+
+		outfmt = __prp_get_fmt(priv, sd_state, PRPENCVF_SRC_PAD,
+				       sdformat->which);
+		*outfmt = format.format;
+		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			priv->cc[PRPENCVF_SRC_PAD] = outcc;
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->cc[sdformat->pad] = cc;
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_subdev_format format = {};
+	const struct imx_media_pixfmt *cc;
+	int ret = 0;
+
+	if (fse->pad >= PRPENCVF_NUM_PADS || fse->index != 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	format.pad = fse->pad;
+	format.which = fse->which;
+	format.format.code = fse->code;
+	format.format.width = 1;
+	format.format.height = 1;
+	prp_try_fmt(priv, sd_state, &format, &cc);
+	fse->min_width = format.format.width;
+	fse->min_height = format.format.height;
+
+	if (format.format.code != fse->code) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	format.format.code = fse->code;
+	format.format.width = -1;
+	format.format.height = -1;
+	prp_try_fmt(priv, sd_state, &format, &cc);
+	fse->max_width = format.format.width;
+	fse->max_height = format.format.height;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(ic_priv->ipu_dev, "%s: link setup %s -> %s",
+		ic_priv->sd.name, remote->entity->name, local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is the source pad */
+
+	/* the remote must be the device node */
+	if (!is_media_entity_v4l2_video_device(remote->entity)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->sink) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		priv->sink = NULL;
+		goto out;
+	}
+
+	priv->sink = remote->entity;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct prp_priv *priv = container_of(ctrl->handler,
+					       struct prp_priv, ctrl_hdlr);
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	enum ipu_rotate_mode rot_mode;
+	int rotation, ret = 0;
+	bool hflip, vflip;
+
+	mutex_lock(&priv->lock);
+
+	rotation = priv->rotation;
+	hflip = priv->hflip;
+	vflip = priv->vflip;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		hflip = (ctrl->val == 1);
+		break;
+	case V4L2_CID_VFLIP:
+		vflip = (ctrl->val == 1);
+		break;
+	case V4L2_CID_ROTATE:
+		rotation = ctrl->val;
+		break;
+	default:
+		v4l2_err(&ic_priv->sd, "Invalid control\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = ipu_degrees_to_rot_mode(&rot_mode, rotation, hflip, vflip);
+	if (ret)
+		goto out;
+
+	if (rot_mode != priv->rot_mode) {
+		struct v4l2_mbus_framefmt outfmt, infmt;
+
+		/* can't change rotation mid-streaming */
+		if (priv->stream_count > 0) {
+			ret = -EBUSY;
+			goto out;
+		}
+
+		outfmt = priv->format_mbus[PRPENCVF_SRC_PAD];
+		infmt = priv->format_mbus[PRPENCVF_SINK_PAD];
+
+		if (prp_bound_align_output(&outfmt, &infmt, rot_mode)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		priv->rot_mode = rot_mode;
+		priv->rotation = rotation;
+		priv->hflip = hflip;
+		priv->vflip = vflip;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops prp_ctrl_ops = {
+	.s_ctrl = prp_s_ctrl,
+};
+
+static int prp_init_controls(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdlr, 3);
+
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_HFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_VFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_ROTATE,
+			  0, 270, 90, 0);
+
+	ic_priv->sd.ctrl_handler = hdlr;
+
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto out_free;
+	}
+
+	v4l2_ctrl_handler_setup(hdlr);
+	return 0;
+
+out_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+static int prp_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || !priv->sink) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(ic_priv->ipu_dev, "%s: stream %s\n", sd->name,
+		enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = prp_start(priv);
+	else
+		prp_stop(priv);
+	if (ret)
+		goto out;
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+	fi->interval = priv->frame_interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	/* No limits on valid frame intervals */
+	if (fi->interval.numerator == 0 || fi->interval.denominator == 0)
+		fi->interval = priv->frame_interval;
+	else
+		priv->frame_interval = fi->interval;
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_registered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	int i, ret;
+	u32 code;
+
+	/* set a default mbus format  */
+	imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
+
+	for (i = 0; i < PRPENCVF_NUM_PADS; i++) {
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      IMX_MEDIA_DEF_PIX_WIDTH,
+					      IMX_MEDIA_DEF_PIX_HEIGHT, code,
+					      V4L2_FIELD_NONE, &priv->cc[i]);
+		if (ret)
+			return ret;
+	}
+
+	/* init default frame interval */
+	priv->frame_interval.numerator = 1;
+	priv->frame_interval.denominator = 30;
+
+	priv->vdev = imx_media_capture_device_init(ic_priv->ipu_dev,
+						   &ic_priv->sd,
+						   PRPENCVF_SRC_PAD, true);
+	if (IS_ERR(priv->vdev))
+		return PTR_ERR(priv->vdev);
+
+	ret = imx_media_capture_device_register(priv->vdev, 0);
+	if (ret)
+		goto remove_vdev;
+
+	ret = prp_init_controls(priv);
+	if (ret)
+		goto unreg_vdev;
+
+	return 0;
+
+unreg_vdev:
+	imx_media_capture_device_unregister(priv->vdev);
+remove_vdev:
+	imx_media_capture_device_remove(priv->vdev);
+	return ret;
+}
+
+static void prp_unregistered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	imx_media_capture_device_unregister(priv->vdev);
+	imx_media_capture_device_remove(priv->vdev);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+
+static const struct v4l2_subdev_pad_ops prp_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
+	.enum_mbus_code = prp_enum_mbus_code,
+	.enum_frame_size = prp_enum_frame_size,
+	.get_fmt = prp_get_fmt,
+	.set_fmt = prp_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops prp_video_ops = {
+	.g_frame_interval = prp_g_frame_interval,
+	.s_frame_interval = prp_s_frame_interval,
+	.s_stream = prp_s_stream,
+};
+
+static const struct media_entity_operations prp_entity_ops = {
+	.link_setup = prp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops prp_subdev_ops = {
+	.video = &prp_video_ops,
+	.pad = &prp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops prp_internal_ops = {
+	.registered = prp_registered,
+	.unregistered = prp_unregistered,
+};
+
+static int prp_init(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv;
+	int i, ret;
+
+	priv = devm_kzalloc(ic_priv->ipu_dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	ic_priv->task_priv = priv;
+	priv->ic_priv = ic_priv;
+
+	spin_lock_init(&priv->irqlock);
+	timer_setup(&priv->eof_timeout_timer, prp_eof_timeout, 0);
+
+	mutex_init(&priv->lock);
+
+	for (i = 0; i < PRPENCVF_NUM_PADS; i++) {
+		priv->pad[i].flags = (i == PRPENCVF_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	ret = media_entity_pads_init(&ic_priv->sd.entity, PRPENCVF_NUM_PADS,
+				     priv->pad);
+	if (ret)
+		mutex_destroy(&priv->lock);
+
+	return ret;
+}
+
+static void prp_remove(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv = ic_priv->task_priv;
+
+	mutex_destroy(&priv->lock);
+}
+
+struct imx_ic_ops imx_ic_prpencvf_ops = {
+	.subdev_ops = &prp_subdev_ops,
+	.internal_ops = &prp_internal_ops,
+	.entity_ops = &prp_entity_ops,
+	.init = prp_init,
+	.remove = prp_remove,
+};
diff --git a/drivers/staging/media/imx/imx-ic.h b/drivers/staging/media/imx/imx-ic.h
new file mode 100644
index 000000000..587c191c3
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#ifndef _IMX_IC_H
+#define _IMX_IC_H
+
+#include <media/v4l2-subdev.h>
+
+struct imx_ic_priv {
+	struct device *ipu_dev;
+	struct ipu_soc *ipu;
+	struct v4l2_subdev sd;
+	int    task_id;
+	void   *task_priv;
+};
+
+struct imx_ic_ops {
+	const struct v4l2_subdev_ops *subdev_ops;
+	const struct v4l2_subdev_internal_ops *internal_ops;
+	const struct media_entity_operations *entity_ops;
+
+	int (*init)(struct imx_ic_priv *ic_priv);
+	void (*remove)(struct imx_ic_priv *ic_priv);
+};
+
+extern struct imx_ic_ops imx_ic_prp_ops;
+extern struct imx_ic_ops imx_ic_prpencvf_ops;
+
+#endif
diff --git a/drivers/staging/media/imx/imx-media-capture.c b/drivers/staging/media/imx/imx-media-capture.c
new file mode 100644
index 000000000..5cc67786b
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-capture.c
@@ -0,0 +1,1051 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Video Capture Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2012-2016 Mentor Graphics Inc.
+ */
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+#define IMX_CAPTURE_NAME "imx-capture"
+
+struct capture_priv {
+	struct imx_media_dev *md;		/* Media device */
+	struct device *dev;			/* Physical device */
+
+	struct imx_media_video_dev vdev;	/* Video device */
+	struct media_pad vdev_pad;		/* Video device pad */
+
+	struct v4l2_subdev *src_sd;		/* Source subdev */
+	int src_sd_pad;				/* Source subdev pad */
+
+	struct mutex mutex;			/* Protect vdev operations */
+
+	struct vb2_queue q;			/* The videobuf2 queue */
+	struct list_head ready_q;		/* List of queued buffers */
+	spinlock_t q_lock;			/* Protect ready_q */
+
+	struct v4l2_ctrl_handler ctrl_hdlr;	/* Controls inherited from subdevs */
+
+	bool legacy_api;			/* Use the legacy (pre-MC) API */
+};
+
+#define to_capture_priv(v) container_of(v, struct capture_priv, vdev)
+
+/* In bytes, per queue */
+#define VID_MEM_LIMIT	SZ_64M
+
+/* -----------------------------------------------------------------------------
+ * MC-Centric Video IOCTLs
+ */
+
+static const struct imx_media_pixfmt *capture_find_format(u32 code, u32 fourcc)
+{
+	const struct imx_media_pixfmt *cc;
+
+	cc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV_RGB);
+	if (cc) {
+		enum imx_pixfmt_sel fmt_sel = cc->cs == IPUV3_COLORSPACE_YUV
+					    ? PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
+
+		cc = imx_media_find_pixel_format(fourcc, fmt_sel);
+		if (!cc) {
+			imx_media_enum_pixel_formats(&fourcc, 0, fmt_sel, 0);
+			cc = imx_media_find_pixel_format(fourcc, fmt_sel);
+		}
+
+		return cc;
+	}
+
+	return imx_media_find_mbus_format(code, PIXFMT_SEL_ANY);
+}
+
+static int capture_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	strscpy(cap->driver, IMX_CAPTURE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, IMX_CAPTURE_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", dev_name(priv->dev));
+
+	return 0;
+}
+
+static int capture_enum_fmt_vid_cap(struct file *file, void *fh,
+				    struct v4l2_fmtdesc *f)
+{
+	return imx_media_enum_pixel_formats(&f->pixelformat, f->index,
+					    PIXFMT_SEL_ANY, f->mbus_code);
+}
+
+static int capture_enum_framesizes(struct file *file, void *fh,
+				   struct v4l2_frmsizeenum *fsize)
+{
+	const struct imx_media_pixfmt *cc;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	cc = imx_media_find_pixel_format(fsize->pixel_format, PIXFMT_SEL_ANY);
+	if (!cc)
+		return -EINVAL;
+
+	/*
+	 * TODO: The constraints are hardware-specific and may depend on the
+	 * pixel format. This should come from the driver using
+	 * imx_media_capture.
+	 */
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = 1;
+	fsize->stepwise.max_width = 65535;
+	fsize->stepwise.min_height = 1;
+	fsize->stepwise.max_height = 65535;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int capture_g_fmt_vid_cap(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	f->fmt.pix = priv->vdev.fmt;
+
+	return 0;
+}
+
+static const struct imx_media_pixfmt *
+__capture_try_fmt(struct v4l2_pix_format *pixfmt, struct v4l2_rect *compose)
+{
+	struct v4l2_mbus_framefmt fmt_src;
+	const struct imx_media_pixfmt *cc;
+
+	/*
+	 * Find the pixel format, default to the first supported format if not
+	 * found.
+	 */
+	cc = imx_media_find_pixel_format(pixfmt->pixelformat, PIXFMT_SEL_ANY);
+	if (!cc) {
+		imx_media_enum_pixel_formats(&pixfmt->pixelformat, 0,
+					     PIXFMT_SEL_ANY, 0);
+		cc = imx_media_find_pixel_format(pixfmt->pixelformat,
+						 PIXFMT_SEL_ANY);
+	}
+
+	/* Allow IDMAC interweave but enforce field order from source. */
+	if (V4L2_FIELD_IS_INTERLACED(pixfmt->field)) {
+		switch (pixfmt->field) {
+		case V4L2_FIELD_SEQ_TB:
+			pixfmt->field = V4L2_FIELD_INTERLACED_TB;
+			break;
+		case V4L2_FIELD_SEQ_BT:
+			pixfmt->field = V4L2_FIELD_INTERLACED_BT;
+			break;
+		default:
+			break;
+		}
+	}
+
+	v4l2_fill_mbus_format(&fmt_src, pixfmt, 0);
+	imx_media_mbus_fmt_to_pix_fmt(pixfmt, &fmt_src, cc);
+
+	if (compose) {
+		compose->width = fmt_src.width;
+		compose->height = fmt_src.height;
+	}
+
+	return cc;
+}
+
+static int capture_try_fmt_vid_cap(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	__capture_try_fmt(&f->fmt.pix, NULL);
+	return 0;
+}
+
+static int capture_s_fmt_vid_cap(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc;
+
+	if (vb2_is_busy(&priv->q)) {
+		dev_err(priv->dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	cc = __capture_try_fmt(&f->fmt.pix, &priv->vdev.compose);
+
+	priv->vdev.cc = cc;
+	priv->vdev.fmt = f->fmt.pix;
+
+	return 0;
+}
+
+static int capture_g_selection(struct file *file, void *fh,
+			       struct v4l2_selection *s)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		/* The compose rectangle is fixed to the source format. */
+		s->r = priv->vdev.compose;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		/*
+		 * The hardware writes with a configurable but fixed DMA burst
+		 * size. If the source format width is not burst size aligned,
+		 * the written frame contains padding to the right.
+		 */
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = priv->vdev.fmt.width;
+		s->r.height = priv->vdev.fmt.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int capture_subscribe_event(struct v4l2_fh *fh,
+				   const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ioctl_ops capture_ioctl_ops = {
+	.vidioc_querycap		= capture_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= capture_enum_fmt_vid_cap,
+	.vidioc_enum_framesizes		= capture_enum_framesizes,
+
+	.vidioc_g_fmt_vid_cap		= capture_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= capture_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= capture_s_fmt_vid_cap,
+
+	.vidioc_g_selection		= capture_g_selection,
+
+	.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_subscribe_event		= capture_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/* -----------------------------------------------------------------------------
+ * Legacy Video IOCTLs
+ */
+
+static int capture_legacy_enum_framesizes(struct file *file, void *fh,
+					  struct v4l2_frmsizeenum *fsize)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.pad = priv->src_sd_pad,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	cc = imx_media_find_pixel_format(fsize->pixel_format, PIXFMT_SEL_ANY);
+	if (!cc)
+		return -EINVAL;
+
+	fse.code = cc->codes ? cc->codes[0] : 0;
+
+	ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+
+	if (fse.min_width == fse.max_width &&
+	    fse.min_height == fse.max_height) {
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = fse.min_width;
+		fsize->discrete.height = fse.min_height;
+	} else {
+		fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+		fsize->stepwise.min_width = fse.min_width;
+		fsize->stepwise.max_width = fse.max_width;
+		fsize->stepwise.min_height = fse.min_height;
+		fsize->stepwise.max_height = fse.max_height;
+		fsize->stepwise.step_width = 1;
+		fsize->stepwise.step_height = 1;
+	}
+
+	return 0;
+}
+
+static int capture_legacy_enum_frameintervals(struct file *file, void *fh,
+					      struct v4l2_frmivalenum *fival)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = fival->index,
+		.pad = priv->src_sd_pad,
+		.width = fival->width,
+		.height = fival->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	cc = imx_media_find_pixel_format(fival->pixel_format, PIXFMT_SEL_ANY);
+	if (!cc)
+		return -EINVAL;
+
+	fie.code = cc->codes ? cc->codes[0] : 0;
+
+	ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_interval,
+			       NULL, &fie);
+	if (ret)
+		return ret;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return 0;
+}
+
+static int capture_legacy_enum_fmt_vid_cap(struct file *file, void *fh,
+					   struct v4l2_fmtdesc *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc_src;
+	struct v4l2_subdev_format fmt_src;
+	u32 fourcc;
+	int ret;
+
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret) {
+		dev_err(priv->dev, "failed to get src_sd format\n");
+		return ret;
+	}
+
+	cc_src = imx_media_find_ipu_format(fmt_src.format.code,
+					   PIXFMT_SEL_YUV_RGB);
+	if (cc_src) {
+		enum imx_pixfmt_sel fmt_sel =
+			(cc_src->cs == IPUV3_COLORSPACE_YUV) ?
+			PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
+
+		ret = imx_media_enum_pixel_formats(&fourcc, f->index, fmt_sel,
+						   0);
+		if (ret)
+			return ret;
+	} else {
+		cc_src = imx_media_find_mbus_format(fmt_src.format.code,
+						    PIXFMT_SEL_ANY);
+		if (WARN_ON(!cc_src))
+			return -EINVAL;
+
+		if (f->index != 0)
+			return -EINVAL;
+		fourcc = cc_src->fourcc;
+	}
+
+	f->pixelformat = fourcc;
+
+	return 0;
+}
+
+static const struct imx_media_pixfmt *
+__capture_legacy_try_fmt(struct capture_priv *priv,
+			 struct v4l2_subdev_format *fmt_src,
+			 struct v4l2_pix_format *pixfmt)
+{
+	const struct imx_media_pixfmt *cc;
+
+	cc = capture_find_format(fmt_src->format.code, pixfmt->pixelformat);
+	if (WARN_ON(!cc))
+		return NULL;
+
+	/* allow IDMAC interweave but enforce field order from source */
+	if (V4L2_FIELD_IS_INTERLACED(pixfmt->field)) {
+		switch (fmt_src->format.field) {
+		case V4L2_FIELD_SEQ_TB:
+			fmt_src->format.field = V4L2_FIELD_INTERLACED_TB;
+			break;
+		case V4L2_FIELD_SEQ_BT:
+			fmt_src->format.field = V4L2_FIELD_INTERLACED_BT;
+			break;
+		default:
+			break;
+		}
+	}
+
+	imx_media_mbus_fmt_to_pix_fmt(pixfmt, &fmt_src->format, cc);
+
+	return cc;
+}
+
+static int capture_legacy_try_fmt_vid_cap(struct file *file, void *fh,
+					  struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_format fmt_src;
+	int ret;
+
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret)
+		return ret;
+
+	if (!__capture_legacy_try_fmt(priv, &fmt_src, &f->fmt.pix))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int capture_legacy_s_fmt_vid_cap(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_format fmt_src;
+	const struct imx_media_pixfmt *cc;
+	int ret;
+
+	if (vb2_is_busy(&priv->q)) {
+		dev_err(priv->dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret)
+		return ret;
+
+	cc = __capture_legacy_try_fmt(priv, &fmt_src, &f->fmt.pix);
+	if (!cc)
+		return -EINVAL;
+
+	priv->vdev.cc = cc;
+	priv->vdev.fmt = f->fmt.pix;
+	priv->vdev.compose.width = fmt_src.format.width;
+	priv->vdev.compose.height = fmt_src.format.height;
+
+	return 0;
+}
+
+static int capture_legacy_querystd(struct file *file, void *fh,
+				   v4l2_std_id *std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	return v4l2_subdev_call(priv->src_sd, video, querystd, std);
+}
+
+static int capture_legacy_g_std(struct file *file, void *fh, v4l2_std_id *std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	return v4l2_subdev_call(priv->src_sd, video, g_std, std);
+}
+
+static int capture_legacy_s_std(struct file *file, void *fh, v4l2_std_id std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	if (vb2_is_busy(&priv->q))
+		return -EBUSY;
+
+	return v4l2_subdev_call(priv->src_sd, video, s_std, std);
+}
+
+static int capture_legacy_g_parm(struct file *file, void *fh,
+				 struct v4l2_streamparm *a)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_frame_interval fi = {
+		.pad = priv->src_sd_pad,
+	};
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	ret = v4l2_subdev_call(priv->src_sd, video, g_frame_interval, &fi);
+	if (ret < 0)
+		return ret;
+
+	a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.capture.timeperframe = fi.interval;
+
+	return 0;
+}
+
+static int capture_legacy_s_parm(struct file *file, void *fh,
+				 struct v4l2_streamparm *a)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_frame_interval fi = {
+		.pad = priv->src_sd_pad,
+	};
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	fi.interval = a->parm.capture.timeperframe;
+	ret = v4l2_subdev_call(priv->src_sd, video, s_frame_interval, &fi);
+	if (ret < 0)
+		return ret;
+
+	a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.capture.timeperframe = fi.interval;
+
+	return 0;
+}
+
+static int capture_legacy_subscribe_event(struct v4l2_fh *fh,
+					  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ioctl_ops capture_legacy_ioctl_ops = {
+	.vidioc_querycap		= capture_querycap,
+
+	.vidioc_enum_framesizes		= capture_legacy_enum_framesizes,
+	.vidioc_enum_frameintervals	= capture_legacy_enum_frameintervals,
+
+	.vidioc_enum_fmt_vid_cap	= capture_legacy_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= capture_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= capture_legacy_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= capture_legacy_s_fmt_vid_cap,
+
+	.vidioc_querystd		= capture_legacy_querystd,
+	.vidioc_g_std			= capture_legacy_g_std,
+	.vidioc_s_std			= capture_legacy_s_std,
+
+	.vidioc_g_selection		= capture_g_selection,
+
+	.vidioc_g_parm			= capture_legacy_g_parm,
+	.vidioc_s_parm			= capture_legacy_s_parm,
+
+	.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_subscribe_event		= capture_legacy_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/* -----------------------------------------------------------------------------
+ * Queue Operations
+ */
+
+static int capture_queue_setup(struct vb2_queue *vq,
+			       unsigned int *nbuffers,
+			       unsigned int *nplanes,
+			       unsigned int sizes[],
+			       struct device *alloc_devs[])
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &priv->vdev.fmt;
+	unsigned int count = *nbuffers;
+
+	if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (*nplanes) {
+		if (*nplanes != 1 || sizes[0] < pix->sizeimage)
+			return -EINVAL;
+		count += vq->num_buffers;
+	}
+
+	count = min_t(__u32, VID_MEM_LIMIT / pix->sizeimage, count);
+
+	if (*nplanes)
+		*nbuffers = (count < vq->num_buffers) ? 0 :
+			count - vq->num_buffers;
+	else
+		*nbuffers = count;
+
+	*nplanes = 1;
+	sizes[0] = pix->sizeimage;
+
+	return 0;
+}
+
+static int capture_buf_init(struct vb2_buffer *vb)
+{
+	struct imx_media_buffer *buf = to_imx_media_vb(vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
+
+static int capture_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &priv->vdev.fmt;
+
+	if (vb2_plane_size(vb, 0) < pix->sizeimage) {
+		dev_err(priv->dev,
+			"data will not fit into plane (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), (long)pix->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, pix->sizeimage);
+
+	return 0;
+}
+
+static void capture_buf_queue(struct vb2_buffer *vb)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vb->vb2_queue);
+	struct imx_media_buffer *buf = to_imx_media_vb(vb);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+
+	list_add_tail(&buf->list, &priv->ready_q);
+
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static int capture_validate_fmt(struct capture_priv *priv)
+{
+	struct v4l2_subdev_format fmt_src;
+	const struct imx_media_pixfmt *cc;
+	int ret;
+
+	/* Retrieve the media bus format on the source subdev. */
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret)
+		return ret;
+
+	/*
+	 * Verify that the media bus size matches the size set on the video
+	 * node. It is sufficient to check the compose rectangle size without
+	 * checking the rounded size from vdev.fmt, as the rounded size is
+	 * derived directly from the compose rectangle size, and will thus
+	 * always match if the compose rectangle matches.
+	 */
+	if (priv->vdev.compose.width != fmt_src.format.width ||
+	    priv->vdev.compose.height != fmt_src.format.height)
+		return -EPIPE;
+
+	/*
+	 * Verify that the media bus code is compatible with the pixel format
+	 * set on the video node.
+	 */
+	cc = capture_find_format(fmt_src.format.code, 0);
+	if (!cc || priv->vdev.cc->cs != cc->cs)
+		return -EPIPE;
+
+	return 0;
+}
+
+static int capture_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct imx_media_buffer *buf, *tmp;
+	unsigned long flags;
+	int ret;
+
+	ret = capture_validate_fmt(priv);
+	if (ret) {
+		dev_err(priv->dev, "capture format not valid\n");
+		goto return_bufs;
+	}
+
+	ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
+					    true);
+	if (ret) {
+		dev_err(priv->dev, "pipeline start failed with %d\n", ret);
+		goto return_bufs;
+	}
+
+	return 0;
+
+return_bufs:
+	spin_lock_irqsave(&priv->q_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &priv->ready_q, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+	return ret;
+}
+
+static void capture_stop_streaming(struct vb2_queue *vq)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct imx_media_buffer *frame;
+	struct imx_media_buffer *tmp;
+	unsigned long flags;
+	int ret;
+
+	ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
+					    false);
+	if (ret)
+		dev_warn(priv->dev, "pipeline stop failed with %d\n", ret);
+
+	/* release all active buffers */
+	spin_lock_irqsave(&priv->q_lock, flags);
+	list_for_each_entry_safe(frame, tmp, &priv->ready_q, list) {
+		list_del(&frame->list);
+		vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static const struct vb2_ops capture_qops = {
+	.queue_setup	 = capture_queue_setup,
+	.buf_init        = capture_buf_init,
+	.buf_prepare	 = capture_buf_prepare,
+	.buf_queue	 = capture_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = capture_start_streaming,
+	.stop_streaming  = capture_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * File Operations
+ */
+
+static int capture_open(struct file *file)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct video_device *vfd = priv->vdev.vfd;
+	int ret;
+
+	if (mutex_lock_interruptible(&priv->mutex))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret) {
+		dev_err(priv->dev, "v4l2_fh_open failed\n");
+		goto out;
+	}
+
+	ret = v4l2_pipeline_pm_get(&vfd->entity);
+	if (ret)
+		v4l2_fh_release(file);
+
+out:
+	mutex_unlock(&priv->mutex);
+	return ret;
+}
+
+static int capture_release(struct file *file)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct video_device *vfd = priv->vdev.vfd;
+	struct vb2_queue *vq = &priv->q;
+
+	mutex_lock(&priv->mutex);
+
+	if (file->private_data == vq->owner) {
+		vb2_queue_release(vq);
+		vq->owner = NULL;
+	}
+
+	v4l2_pipeline_pm_put(&vfd->entity);
+
+	v4l2_fh_release(file);
+	mutex_unlock(&priv->mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations capture_fops = {
+	.owner		= THIS_MODULE,
+	.open		= capture_open,
+	.release	= capture_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * Public API
+ */
+
+struct imx_media_buffer *
+imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct imx_media_buffer *buf = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+
+	/* get next queued buffer */
+	if (!list_empty(&priv->ready_q)) {
+		buf = list_entry(priv->ready_q.next, struct imx_media_buffer,
+				 list);
+		list_del(&buf->list);
+	}
+
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+
+	return buf;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_next_buf);
+
+void imx_media_capture_device_error(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct vb2_queue *vq = &priv->q;
+	unsigned long flags;
+
+	if (!vb2_is_streaming(vq))
+		return;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+	vb2_queue_error(vq);
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_error);
+
+static int capture_init_format(struct capture_priv *priv)
+{
+	struct v4l2_subdev_format fmt_src = {
+		.pad = priv->src_sd_pad,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct imx_media_video_dev *vdev = &priv->vdev;
+	int ret;
+
+	if (priv->legacy_api) {
+		ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL,
+				       &fmt_src);
+		if (ret) {
+			dev_err(priv->dev, "failed to get source format\n");
+			return ret;
+		}
+	} else {
+		fmt_src.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+		fmt_src.format.width = IMX_MEDIA_DEF_PIX_WIDTH;
+		fmt_src.format.height = IMX_MEDIA_DEF_PIX_HEIGHT;
+	}
+
+	imx_media_mbus_fmt_to_pix_fmt(&vdev->fmt, &fmt_src.format, NULL);
+	vdev->compose.width = fmt_src.format.width;
+	vdev->compose.height = fmt_src.format.height;
+
+	vdev->cc = imx_media_find_pixel_format(vdev->fmt.pixelformat,
+					       PIXFMT_SEL_ANY);
+
+	return 0;
+}
+
+int imx_media_capture_device_register(struct imx_media_video_dev *vdev,
+				      u32 link_flags)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct v4l2_subdev *sd = priv->src_sd;
+	struct v4l2_device *v4l2_dev = sd->v4l2_dev;
+	struct video_device *vfd = vdev->vfd;
+	int ret;
+
+	/* get media device */
+	priv->md = container_of(v4l2_dev->mdev, struct imx_media_dev, md);
+
+	vfd->v4l2_dev = v4l2_dev;
+
+	/* Initialize the default format and compose rectangle. */
+	ret = capture_init_format(priv);
+	if (ret < 0)
+		return ret;
+
+	/* Register the video device. */
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(priv->dev, "Failed to register video device\n");
+		return ret;
+	}
+
+	dev_info(priv->dev, "Registered %s as /dev/%s\n", vfd->name,
+		 video_device_node_name(vfd));
+
+	/* Create the link from the src_sd devnode pad to device node. */
+	if (link_flags & MEDIA_LNK_FL_IMMUTABLE)
+		link_flags |= MEDIA_LNK_FL_ENABLED;
+	ret = media_create_pad_link(&sd->entity, priv->src_sd_pad,
+				    &vfd->entity, 0, link_flags);
+	if (ret) {
+		dev_err(priv->dev, "failed to create link to device node\n");
+		video_unregister_device(vfd);
+		return ret;
+	}
+
+	/* Add vdev to the video devices list. */
+	imx_media_add_video_device(priv->md, vdev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_register);
+
+void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct video_device *vfd = priv->vdev.vfd;
+
+	media_entity_cleanup(&vfd->entity);
+	video_unregister_device(vfd);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_unregister);
+
+struct imx_media_video_dev *
+imx_media_capture_device_init(struct device *dev, struct v4l2_subdev *src_sd,
+			      int pad, bool legacy_api)
+{
+	struct capture_priv *priv;
+	struct video_device *vfd;
+	struct vb2_queue *vq;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->src_sd = src_sd;
+	priv->src_sd_pad = pad;
+	priv->dev = dev;
+	priv->legacy_api = legacy_api;
+
+	mutex_init(&priv->mutex);
+	INIT_LIST_HEAD(&priv->ready_q);
+	spin_lock_init(&priv->q_lock);
+
+	/* Allocate and initialize the video device. */
+	vfd = video_device_alloc();
+	if (!vfd)
+		return ERR_PTR(-ENOMEM);
+
+	vfd->fops = &capture_fops;
+	vfd->ioctl_ops = legacy_api ? &capture_legacy_ioctl_ops
+		       : &capture_ioctl_ops;
+	vfd->minor = -1;
+	vfd->release = video_device_release;
+	vfd->vfl_dir = VFL_DIR_RX;
+	vfd->tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
+	vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
+			 | (!legacy_api ? V4L2_CAP_IO_MC : 0);
+	vfd->lock = &priv->mutex;
+	vfd->queue = &priv->q;
+
+	snprintf(vfd->name, sizeof(vfd->name), "%s capture", src_sd->name);
+
+	video_set_drvdata(vfd, priv);
+	priv->vdev.vfd = vfd;
+	INIT_LIST_HEAD(&priv->vdev.list);
+
+	/* Initialize the video device pad. */
+	priv->vdev_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vfd->entity, 1, &priv->vdev_pad);
+	if (ret) {
+		video_device_release(vfd);
+		return ERR_PTR(ret);
+	}
+
+	/* Initialize the vb2 queue. */
+	vq = &priv->q;
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	vq->drv_priv = priv;
+	vq->buf_struct_size = sizeof(struct imx_media_buffer);
+	vq->ops = &capture_qops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->lock = &priv->mutex;
+	vq->min_buffers_needed = 2;
+	vq->dev = priv->dev;
+
+	ret = vb2_queue_init(vq);
+	if (ret) {
+		dev_err(priv->dev, "vb2_queue_init failed\n");
+		video_device_release(vfd);
+		return ERR_PTR(ret);
+	}
+
+	if (legacy_api) {
+		/* Initialize the control handler. */
+		v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+		vfd->ctrl_handler = &priv->ctrl_hdlr;
+	}
+
+	return &priv->vdev;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_init);
+
+void imx_media_capture_device_remove(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_remove);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 video capture interface driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-csc-scaler.c b/drivers/staging/media/imx/imx-media-csc-scaler.c
new file mode 100644
index 000000000..1fd39a2fc
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-csc-scaler.c
@@ -0,0 +1,924 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * i.MX IPUv3 IC PP mem2mem CSC/Scaler driver
+ *
+ * Copyright (C) 2011 Pengutronix, Sascha Hauer
+ * Copyright (C) 2018 Pengutronix, Philipp Zabel
+ */
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <video/imx-ipu-v3.h>
+#include <video/imx-ipu-image-convert.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "imx-media.h"
+
+#define fh_to_ctx(__fh)	container_of(__fh, struct ipu_csc_scaler_ctx, fh)
+
+#define IMX_CSC_SCALER_NAME "imx-csc-scaler"
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+struct ipu_csc_scaler_priv {
+	struct imx_media_video_dev	vdev;
+
+	struct v4l2_m2m_dev		*m2m_dev;
+	struct device			*dev;
+
+	struct imx_media_dev		*md;
+
+	struct mutex			mutex;	/* mem2mem device mutex */
+};
+
+#define vdev_to_priv(v) container_of(v, struct ipu_csc_scaler_priv, vdev)
+
+/* Per-queue, driver-specific private data */
+struct ipu_csc_scaler_q_data {
+	struct v4l2_pix_format		cur_fmt;
+	struct v4l2_rect		rect;
+};
+
+struct ipu_csc_scaler_ctx {
+	struct ipu_csc_scaler_priv	*priv;
+
+	struct v4l2_fh			fh;
+	struct ipu_csc_scaler_q_data	q_data[2];
+	struct ipu_image_convert_ctx	*icc;
+
+	struct v4l2_ctrl_handler	ctrl_hdlr;
+	int				rotate;
+	bool				hflip;
+	bool				vflip;
+	enum ipu_rotate_mode		rot_mode;
+	unsigned int			sequence;
+};
+
+static struct ipu_csc_scaler_q_data *get_q_data(struct ipu_csc_scaler_ctx *ctx,
+						enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->q_data[V4L2_M2M_SRC];
+	else
+		return &ctx->q_data[V4L2_M2M_DST];
+}
+
+/*
+ * mem2mem callbacks
+ */
+
+static void job_abort(void *_ctx)
+{
+	struct ipu_csc_scaler_ctx *ctx = _ctx;
+
+	if (ctx->icc)
+		ipu_image_convert_abort(ctx->icc);
+}
+
+static void ipu_ic_pp_complete(struct ipu_image_convert_run *run, void *_ctx)
+{
+	struct ipu_csc_scaler_ctx *ctx = _ctx;
+	struct ipu_csc_scaler_priv *priv = ctx->priv;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
+
+	src_buf->sequence = ctx->sequence++;
+	dst_buf->sequence = src_buf->sequence;
+
+	v4l2_m2m_buf_done(src_buf, run->status ? VB2_BUF_STATE_ERROR :
+						 VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst_buf, run->status ? VB2_BUF_STATE_ERROR :
+						 VB2_BUF_STATE_DONE);
+
+	v4l2_m2m_job_finish(priv->m2m_dev, ctx->fh.m2m_ctx);
+	kfree(run);
+}
+
+static void device_run(void *_ctx)
+{
+	struct ipu_csc_scaler_ctx *ctx = _ctx;
+	struct ipu_csc_scaler_priv *priv = ctx->priv;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct ipu_image_convert_run *run;
+	int ret;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	run = kzalloc(sizeof(*run), GFP_KERNEL);
+	if (!run)
+		goto err;
+
+	run->ctx = ctx->icc;
+	run->in_phys = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+	run->out_phys = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+
+	ret = ipu_image_convert_queue(run);
+	if (ret < 0) {
+		v4l2_err(ctx->priv->vdev.vfd->v4l2_dev,
+			 "%s: failed to queue: %d\n", __func__, ret);
+		goto err;
+	}
+
+	return;
+
+err:
+	v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+	v4l2_m2m_job_finish(priv->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+/*
+ * Video ioctls
+ */
+static int ipu_csc_scaler_querycap(struct file *file, void *priv,
+				   struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, IMX_CSC_SCALER_NAME, sizeof(cap->driver));
+	strscpy(cap->card, IMX_CSC_SCALER_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", IMX_CSC_SCALER_NAME);
+
+	return 0;
+}
+
+static int ipu_csc_scaler_enum_fmt(struct file *file, void *fh,
+				   struct v4l2_fmtdesc *f)
+{
+	u32 fourcc;
+	int ret;
+
+	ret = imx_media_enum_pixel_formats(&fourcc, f->index,
+					   PIXFMT_SEL_YUV_RGB, 0);
+	if (ret)
+		return ret;
+
+	f->pixelformat = fourcc;
+
+	return 0;
+}
+
+static int ipu_csc_scaler_g_fmt(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(priv);
+	struct ipu_csc_scaler_q_data *q_data;
+
+	q_data = get_q_data(ctx, f->type);
+
+	f->fmt.pix = q_data->cur_fmt;
+
+	return 0;
+}
+
+static int ipu_csc_scaler_try_fmt(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(priv);
+	struct ipu_csc_scaler_q_data *q_data = get_q_data(ctx, f->type);
+	struct ipu_image test_in, test_out;
+	enum v4l2_field field;
+
+	field = f->fmt.pix.field;
+	if (field == V4L2_FIELD_ANY)
+		field = V4L2_FIELD_NONE;
+	else if (field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		struct ipu_csc_scaler_q_data *q_data_in =
+			get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+		test_out.pix = f->fmt.pix;
+		test_in.pix = q_data_in->cur_fmt;
+	} else {
+		struct ipu_csc_scaler_q_data *q_data_out =
+			get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+		test_in.pix = f->fmt.pix;
+		test_out.pix = q_data_out->cur_fmt;
+	}
+
+	ipu_image_convert_adjust(&test_in, &test_out, ctx->rot_mode);
+
+	f->fmt.pix = (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
+		test_out.pix : test_in.pix;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		f->fmt.pix.colorspace = q_data->cur_fmt.colorspace;
+		f->fmt.pix.ycbcr_enc = q_data->cur_fmt.ycbcr_enc;
+		f->fmt.pix.xfer_func = q_data->cur_fmt.xfer_func;
+		f->fmt.pix.quantization = q_data->cur_fmt.quantization;
+	} else if (f->fmt.pix.colorspace == V4L2_COLORSPACE_DEFAULT) {
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+		f->fmt.pix.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		f->fmt.pix.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+		f->fmt.pix.quantization = V4L2_QUANTIZATION_DEFAULT;
+	}
+
+	return 0;
+}
+
+static int ipu_csc_scaler_s_fmt(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct ipu_csc_scaler_q_data *q_data;
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(priv);
+	struct vb2_queue *vq;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_busy(vq)) {
+		v4l2_err(ctx->priv->vdev.vfd->v4l2_dev, "%s: queue busy\n",
+			 __func__);
+		return -EBUSY;
+	}
+
+	q_data = get_q_data(ctx, f->type);
+
+	ret = ipu_csc_scaler_try_fmt(file, priv, f);
+	if (ret < 0)
+		return ret;
+
+	q_data->cur_fmt.width = f->fmt.pix.width;
+	q_data->cur_fmt.height = f->fmt.pix.height;
+	q_data->cur_fmt.pixelformat = f->fmt.pix.pixelformat;
+	q_data->cur_fmt.field = f->fmt.pix.field;
+	q_data->cur_fmt.bytesperline = f->fmt.pix.bytesperline;
+	q_data->cur_fmt.sizeimage = f->fmt.pix.sizeimage;
+
+	/* Reset cropping/composing rectangle */
+	q_data->rect.left = 0;
+	q_data->rect.top = 0;
+	q_data->rect.width = q_data->cur_fmt.width;
+	q_data->rect.height = q_data->cur_fmt.height;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/* Set colorimetry on the output queue */
+		q_data->cur_fmt.colorspace = f->fmt.pix.colorspace;
+		q_data->cur_fmt.ycbcr_enc = f->fmt.pix.ycbcr_enc;
+		q_data->cur_fmt.xfer_func = f->fmt.pix.xfer_func;
+		q_data->cur_fmt.quantization = f->fmt.pix.quantization;
+		/* Propagate colorimetry to the capture queue */
+		q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+		q_data->cur_fmt.colorspace = f->fmt.pix.colorspace;
+		q_data->cur_fmt.ycbcr_enc = f->fmt.pix.ycbcr_enc;
+		q_data->cur_fmt.xfer_func = f->fmt.pix.xfer_func;
+		q_data->cur_fmt.quantization = f->fmt.pix.quantization;
+	}
+
+	/*
+	 * TODO: Setting colorimetry on the capture queue is currently not
+	 * supported by the V4L2 API
+	 */
+
+	return 0;
+}
+
+static int ipu_csc_scaler_g_selection(struct file *file, void *priv,
+				      struct v4l2_selection *s)
+{
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(priv);
+	struct ipu_csc_scaler_q_data *q_data;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (s->target == V4L2_SEL_TGT_CROP ||
+	    s->target == V4L2_SEL_TGT_COMPOSE) {
+		s->r = q_data->rect;
+	} else {
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = q_data->cur_fmt.width;
+		s->r.height = q_data->cur_fmt.height;
+	}
+
+	return 0;
+}
+
+static int ipu_csc_scaler_s_selection(struct file *file, void *priv,
+				      struct v4l2_selection *s)
+{
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(priv);
+	struct ipu_csc_scaler_q_data *q_data;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, s->type);
+
+	/* The input's frame width to the IC must be a multiple of 8 pixels
+	 * When performing resizing the frame width must be multiple of burst
+	 * size - 8 or 16 pixels as defined by CB#_BURST_16 parameter.
+	 */
+	if (s->flags & V4L2_SEL_FLAG_GE)
+		s->r.width = round_up(s->r.width, 8);
+	if (s->flags & V4L2_SEL_FLAG_LE)
+		s->r.width = round_down(s->r.width, 8);
+	s->r.width = clamp_t(unsigned int, s->r.width, 8,
+			     round_down(q_data->cur_fmt.width, 8));
+	s->r.height = clamp_t(unsigned int, s->r.height, 1,
+			      q_data->cur_fmt.height);
+	s->r.left = clamp_t(unsigned int, s->r.left, 0,
+			    q_data->cur_fmt.width - s->r.width);
+	s->r.top = clamp_t(unsigned int, s->r.top, 0,
+			   q_data->cur_fmt.height - s->r.height);
+
+	/* V4L2_SEL_FLAG_KEEP_CONFIG is only valid for subdevices */
+	q_data->rect = s->r;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops ipu_csc_scaler_ioctl_ops = {
+	.vidioc_querycap		= ipu_csc_scaler_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= ipu_csc_scaler_enum_fmt,
+	.vidioc_g_fmt_vid_cap		= ipu_csc_scaler_g_fmt,
+	.vidioc_try_fmt_vid_cap		= ipu_csc_scaler_try_fmt,
+	.vidioc_s_fmt_vid_cap		= ipu_csc_scaler_s_fmt,
+
+	.vidioc_enum_fmt_vid_out	= ipu_csc_scaler_enum_fmt,
+	.vidioc_g_fmt_vid_out		= ipu_csc_scaler_g_fmt,
+	.vidioc_try_fmt_vid_out		= ipu_csc_scaler_try_fmt,
+	.vidioc_s_fmt_vid_out		= ipu_csc_scaler_s_fmt,
+
+	.vidioc_g_selection		= ipu_csc_scaler_g_selection,
+	.vidioc_s_selection		= ipu_csc_scaler_s_selection,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+
+	.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 ipu_csc_scaler_queue_setup(struct vb2_queue *vq,
+				      unsigned int *nbuffers,
+				      unsigned int *nplanes,
+				      unsigned int sizes[],
+				      struct device *alloc_devs[])
+{
+	struct ipu_csc_scaler_ctx *ctx = vb2_get_drv_priv(vq);
+	struct ipu_csc_scaler_q_data *q_data;
+	unsigned int size, count = *nbuffers;
+
+	q_data = get_q_data(ctx, vq->type);
+
+	size = q_data->cur_fmt.sizeimage;
+
+	*nbuffers = count;
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	dev_dbg(ctx->priv->dev, "get %d buffer(s) of size %d each.\n",
+		count, size);
+
+	return 0;
+}
+
+static int ipu_csc_scaler_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct ipu_csc_scaler_ctx *ctx = vb2_get_drv_priv(vq);
+	struct ipu_csc_scaler_q_data *q_data;
+	unsigned long size;
+
+	dev_dbg(ctx->priv->dev, "type: %d\n", vq->type);
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE) {
+			dev_dbg(ctx->priv->dev, "%s: field isn't supported\n",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	q_data = get_q_data(ctx, vq->type);
+	size = q_data->cur_fmt.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_dbg(ctx->priv->dev,
+			"%s: data will not fit into plane (%lu < %lu)\n",
+			__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void ipu_csc_scaler_buf_queue(struct vb2_buffer *vb)
+{
+	struct ipu_csc_scaler_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static void ipu_image_from_q_data(struct ipu_image *im,
+				  struct ipu_csc_scaler_q_data *q_data)
+{
+	struct v4l2_pix_format *fmt = &q_data->cur_fmt;
+
+	im->pix = *fmt;
+	if (fmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
+		im->pix.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	if (fmt->quantization == V4L2_QUANTIZATION_DEFAULT)
+		im->pix.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	im->rect = q_data->rect;
+}
+
+static int ipu_csc_scaler_start_streaming(struct vb2_queue *q,
+					  unsigned int count)
+{
+	const enum ipu_ic_task ic_task = IC_TASK_POST_PROCESSOR;
+	struct ipu_csc_scaler_ctx *ctx = vb2_get_drv_priv(q);
+	struct ipu_csc_scaler_priv *priv = ctx->priv;
+	struct ipu_soc *ipu = priv->md->ipu[0];
+	struct ipu_csc_scaler_q_data *q_data;
+	struct vb2_queue *other_q;
+	struct ipu_image in, out;
+
+	other_q = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+				  (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
+				  V4L2_BUF_TYPE_VIDEO_OUTPUT :
+				  V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	if (!vb2_is_streaming(other_q))
+		return 0;
+
+	if (ctx->icc) {
+		v4l2_warn(ctx->priv->vdev.vfd->v4l2_dev, "removing old ICC\n");
+		ipu_image_convert_unprepare(ctx->icc);
+	}
+
+	q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	ipu_image_from_q_data(&in, q_data);
+
+	q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	ipu_image_from_q_data(&out, q_data);
+
+	ctx->icc = ipu_image_convert_prepare(ipu, ic_task, &in, &out,
+					     ctx->rot_mode,
+					     ipu_ic_pp_complete, ctx);
+	if (IS_ERR(ctx->icc)) {
+		struct vb2_v4l2_buffer *buf;
+		int ret = PTR_ERR(ctx->icc);
+
+		ctx->icc = NULL;
+		v4l2_err(ctx->priv->vdev.vfd->v4l2_dev, "%s: error %d\n",
+			 __func__, ret);
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ipu_csc_scaler_stop_streaming(struct vb2_queue *q)
+{
+	struct ipu_csc_scaler_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *buf;
+
+	if (ctx->icc) {
+		ipu_image_convert_unprepare(ctx->icc);
+		ctx->icc = NULL;
+	}
+
+	ctx->sequence = 0;
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	} else {
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static const struct vb2_ops ipu_csc_scaler_qops = {
+	.queue_setup		= ipu_csc_scaler_queue_setup,
+	.buf_prepare		= ipu_csc_scaler_buf_prepare,
+	.buf_queue		= ipu_csc_scaler_buf_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.start_streaming	= ipu_csc_scaler_start_streaming,
+	.stop_streaming		= ipu_csc_scaler_stop_streaming,
+};
+
+static int ipu_csc_scaler_queue_init(void *priv, struct vb2_queue *src_vq,
+				     struct vb2_queue *dst_vq)
+{
+	struct ipu_csc_scaler_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &ipu_csc_scaler_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->priv->mutex;
+	src_vq->dev = ctx->priv->dev;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &ipu_csc_scaler_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->priv->mutex;
+	dst_vq->dev = ctx->priv->dev;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int ipu_csc_scaler_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ipu_csc_scaler_ctx *ctx = container_of(ctrl->handler,
+						      struct ipu_csc_scaler_ctx,
+						      ctrl_hdlr);
+	enum ipu_rotate_mode rot_mode;
+	int rotate;
+	bool hflip, vflip;
+	int ret = 0;
+
+	rotate = ctx->rotate;
+	hflip = ctx->hflip;
+	vflip = ctx->vflip;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		vflip = ctrl->val;
+		break;
+	case V4L2_CID_ROTATE:
+		rotate = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ipu_degrees_to_rot_mode(&rot_mode, rotate, hflip, vflip);
+	if (ret)
+		return ret;
+
+	if (rot_mode != ctx->rot_mode) {
+		struct v4l2_pix_format *in_fmt, *out_fmt;
+		struct ipu_image test_in, test_out;
+
+		in_fmt = &ctx->q_data[V4L2_M2M_SRC].cur_fmt;
+		out_fmt = &ctx->q_data[V4L2_M2M_DST].cur_fmt;
+
+		test_in.pix = *in_fmt;
+		test_out.pix = *out_fmt;
+
+		if (ipu_rot_mode_is_irt(rot_mode) !=
+		    ipu_rot_mode_is_irt(ctx->rot_mode)) {
+			/* Switch width & height to keep aspect ratio intact */
+			test_out.pix.width = out_fmt->height;
+			test_out.pix.height = out_fmt->width;
+		}
+
+		ipu_image_convert_adjust(&test_in, &test_out, ctx->rot_mode);
+
+		/* Check if output format needs to be changed */
+		if (test_in.pix.width != in_fmt->width ||
+		    test_in.pix.height != in_fmt->height ||
+		    test_in.pix.bytesperline != in_fmt->bytesperline ||
+		    test_in.pix.sizeimage != in_fmt->sizeimage) {
+			struct vb2_queue *out_q;
+
+			out_q = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+						V4L2_BUF_TYPE_VIDEO_OUTPUT);
+			if (vb2_is_busy(out_q))
+				return -EBUSY;
+		}
+
+		/* Check if capture format needs to be changed */
+		if (test_out.pix.width != out_fmt->width ||
+		    test_out.pix.height != out_fmt->height ||
+		    test_out.pix.bytesperline != out_fmt->bytesperline ||
+		    test_out.pix.sizeimage != out_fmt->sizeimage) {
+			struct vb2_queue *cap_q;
+
+			cap_q = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+						V4L2_BUF_TYPE_VIDEO_CAPTURE);
+			if (vb2_is_busy(cap_q))
+				return -EBUSY;
+		}
+
+		*in_fmt = test_in.pix;
+		*out_fmt = test_out.pix;
+
+		ctx->rot_mode = rot_mode;
+		ctx->rotate = rotate;
+		ctx->hflip = hflip;
+		ctx->vflip = vflip;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ipu_csc_scaler_ctrl_ops = {
+	.s_ctrl = ipu_csc_scaler_s_ctrl,
+};
+
+static int ipu_csc_scaler_init_controls(struct ipu_csc_scaler_ctx *ctx)
+{
+	struct v4l2_ctrl_handler *hdlr = &ctx->ctrl_hdlr;
+
+	v4l2_ctrl_handler_init(hdlr, 3);
+
+	v4l2_ctrl_new_std(hdlr, &ipu_csc_scaler_ctrl_ops, V4L2_CID_HFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &ipu_csc_scaler_ctrl_ops, V4L2_CID_VFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &ipu_csc_scaler_ctrl_ops, V4L2_CID_ROTATE,
+			  0, 270, 90, 0);
+
+	if (hdlr->error) {
+		v4l2_ctrl_handler_free(hdlr);
+		return hdlr->error;
+	}
+
+	v4l2_ctrl_handler_setup(hdlr);
+	return 0;
+}
+
+#define DEFAULT_WIDTH	720
+#define DEFAULT_HEIGHT	576
+static const struct ipu_csc_scaler_q_data ipu_csc_scaler_q_data_default = {
+	.cur_fmt = {
+		.width = DEFAULT_WIDTH,
+		.height = DEFAULT_HEIGHT,
+		.pixelformat = V4L2_PIX_FMT_YUV420,
+		.field = V4L2_FIELD_NONE,
+		.bytesperline = DEFAULT_WIDTH,
+		.sizeimage = DEFAULT_WIDTH * DEFAULT_HEIGHT * 3 / 2,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+	},
+	.rect = {
+		.width = DEFAULT_WIDTH,
+		.height = DEFAULT_HEIGHT,
+	},
+};
+
+/*
+ * File operations
+ */
+static int ipu_csc_scaler_open(struct file *file)
+{
+	struct ipu_csc_scaler_priv *priv = video_drvdata(file);
+	struct ipu_csc_scaler_ctx *ctx = NULL;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->rot_mode = IPU_ROTATE_NONE;
+
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	ctx->priv = priv;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(priv->m2m_dev, ctx,
+					    &ipu_csc_scaler_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto err_ctx;
+	}
+
+	ret = ipu_csc_scaler_init_controls(ctx);
+	if (ret)
+		goto err_ctrls;
+
+	ctx->fh.ctrl_handler = &ctx->ctrl_hdlr;
+
+	ctx->q_data[V4L2_M2M_SRC] = ipu_csc_scaler_q_data_default;
+	ctx->q_data[V4L2_M2M_DST] = ipu_csc_scaler_q_data_default;
+
+	dev_dbg(priv->dev, "Created instance %p, m2m_ctx: %p\n", ctx,
+		ctx->fh.m2m_ctx);
+
+	return 0;
+
+err_ctrls:
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+err_ctx:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	return ret;
+}
+
+static int ipu_csc_scaler_release(struct file *file)
+{
+	struct ipu_csc_scaler_priv *priv = video_drvdata(file);
+	struct ipu_csc_scaler_ctx *ctx = fh_to_ctx(file->private_data);
+
+	dev_dbg(priv->dev, "Releasing instance %p\n", ctx);
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations ipu_csc_scaler_fops = {
+	.owner		= THIS_MODULE,
+	.open		= ipu_csc_scaler_open,
+	.release	= ipu_csc_scaler_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= device_run,
+	.job_abort	= job_abort,
+};
+
+static void ipu_csc_scaler_video_device_release(struct video_device *vdev)
+{
+	struct ipu_csc_scaler_priv *priv = video_get_drvdata(vdev);
+
+	v4l2_m2m_release(priv->m2m_dev);
+	video_device_release(vdev);
+	kfree(priv);
+}
+
+static const struct video_device ipu_csc_scaler_videodev_template = {
+	.name		= "ipu_ic_pp csc/scaler",
+	.fops		= &ipu_csc_scaler_fops,
+	.ioctl_ops	= &ipu_csc_scaler_ioctl_ops,
+	.minor		= -1,
+	.release	= ipu_csc_scaler_video_device_release,
+	.vfl_dir	= VFL_DIR_M2M,
+	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
+};
+
+int imx_media_csc_scaler_device_register(struct imx_media_video_dev *vdev)
+{
+	struct ipu_csc_scaler_priv *priv = vdev_to_priv(vdev);
+	struct video_device *vfd = vdev->vfd;
+	int ret;
+
+	vfd->v4l2_dev = &priv->md->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		v4l2_err(vfd->v4l2_dev, "Failed to register video device\n");
+		return ret;
+	}
+
+	v4l2_info(vfd->v4l2_dev, "Registered %s as /dev/%s\n", vfd->name,
+		  video_device_node_name(vfd));
+
+	return 0;
+}
+
+void imx_media_csc_scaler_device_unregister(struct imx_media_video_dev *vdev)
+{
+	struct ipu_csc_scaler_priv *priv = vdev_to_priv(vdev);
+	struct video_device *vfd = priv->vdev.vfd;
+
+	video_unregister_device(vfd);
+}
+
+struct imx_media_video_dev *
+imx_media_csc_scaler_device_init(struct imx_media_dev *md)
+{
+	struct ipu_csc_scaler_priv *priv;
+	struct video_device *vfd;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->md = md;
+	priv->dev = md->md.dev;
+
+	mutex_init(&priv->mutex);
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		ret = -ENOMEM;
+		goto err_vfd;
+	}
+
+	*vfd = ipu_csc_scaler_videodev_template;
+	vfd->lock = &priv->mutex;
+	priv->vdev.vfd = vfd;
+
+	INIT_LIST_HEAD(&priv->vdev.list);
+
+	video_set_drvdata(vfd, priv);
+
+	priv->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(priv->m2m_dev)) {
+		ret = PTR_ERR(priv->m2m_dev);
+		v4l2_err(&md->v4l2_dev, "Failed to init mem2mem device: %d\n",
+			 ret);
+		goto err_m2m;
+	}
+
+	return &priv->vdev;
+
+err_m2m:
+	video_set_drvdata(vfd, NULL);
+err_vfd:
+	kfree(priv);
+	return ERR_PTR(ret);
+}
+
+MODULE_DESCRIPTION("i.MX IPUv3 mem2mem scaler/CSC driver");
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c
new file mode 100644
index 000000000..b2b1f4dd4
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-csi.c
@@ -0,0 +1,2089 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Capture CSI Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
+ */
+#include <linux/delay.h>
+#include <linux/gcd.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+/*
+ * Min/Max supported width and heights.
+ *
+ * We allow planar output, so we have to align width by 16 pixels
+ * to meet IDMAC alignment requirements.
+ *
+ * TODO: move this into pad format negotiation, if capture device
+ * has not requested planar formats, we should allow 8 pixel
+ * alignment.
+ */
+#define MIN_W       32
+#define MIN_H       32
+#define MAX_W      4096
+#define MAX_H      4096
+#define W_ALIGN    1 /* multiple of 2 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+/*
+ * struct csi_skip_desc - CSI frame skipping descriptor
+ * @keep - number of frames kept per max_ratio frames
+ * @max_ratio - width of skip_smfc, written to MAX_RATIO bitfield
+ * @skip_smfc - skip pattern written to the SKIP_SMFC bitfield
+ */
+struct csi_skip_desc {
+	u8 keep;
+	u8 max_ratio;
+	u8 skip_smfc;
+};
+
+struct csi_priv {
+	struct device *dev;
+	struct ipu_soc *ipu;
+	struct v4l2_subdev sd;
+	struct media_pad pad[CSI_NUM_PADS];
+	struct v4l2_async_notifier notifier;
+
+	/* the video device at IDMAC output pad */
+	struct imx_media_video_dev *vdev;
+	struct imx_media_fim *fim;
+	int csi_id;
+	int smfc_id;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int active_output_pad;
+
+	struct ipuv3_channel *idmac_ch;
+	struct ipu_smfc *smfc;
+	struct ipu_csi *csi;
+
+	struct v4l2_mbus_framefmt format_mbus[CSI_NUM_PADS];
+	const struct imx_media_pixfmt *cc[CSI_NUM_PADS];
+	struct v4l2_fract frame_interval[CSI_NUM_PADS];
+	struct v4l2_rect crop;
+	struct v4l2_rect compose;
+	const struct csi_skip_desc *skip;
+
+	/* active vb2 buffers to send to video dev sink */
+	struct imx_media_buffer *active_vb2_buf[2];
+	struct imx_media_dma_buf underrun_buf;
+
+	int ipu_buf_num;  /* ipu double buffer index: 0-1 */
+
+	/* the sink for the captured frames */
+	struct media_entity *sink;
+	enum ipu_csi_dest dest;
+	/* the source subdev */
+	struct v4l2_subdev *src_sd;
+
+	/* the mipi virtual channel number at link validate */
+	int vc_num;
+
+	/* the upstream endpoint CSI is receiving from */
+	struct v4l2_fwnode_endpoint upstream_ep;
+
+	spinlock_t irqlock; /* protect eof_irq handler */
+	struct timer_list eof_timeout_timer;
+	int eof_irq;
+	int nfb4eof_irq;
+
+	struct v4l2_ctrl_handler ctrl_hdlr;
+
+	int stream_count; /* streaming counter */
+	u32 frame_sequence; /* frame sequence counter */
+	bool last_eof;   /* waiting for last EOF at stream off */
+	bool nfb4eof;    /* NFB4EOF encountered during streaming */
+	bool interweave_swap; /* swap top/bottom lines when interweaving */
+	struct completion last_eof_comp;
+};
+
+static inline struct csi_priv *sd_to_dev(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi_priv, sd);
+}
+
+static inline struct csi_priv *notifier_to_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct csi_priv, notifier);
+}
+
+static inline bool is_parallel_bus(struct v4l2_fwnode_endpoint *ep)
+{
+	return ep->bus_type != V4L2_MBUS_CSI2_DPHY;
+}
+
+static inline bool is_parallel_16bit_bus(struct v4l2_fwnode_endpoint *ep)
+{
+	return is_parallel_bus(ep) && ep->bus.parallel.bus_width >= 16;
+}
+
+/*
+ * Check for conditions that require the IPU to handle the
+ * data internally as generic data, aka passthrough mode:
+ * - raw bayer media bus formats, or
+ * - BT.656 and BT.1120 (8/10-bit YUV422) data can always be processed
+ *   on-the-fly
+ * - the CSI is receiving from a 16-bit parallel bus, or
+ * - the CSI is receiving from an 8-bit parallel bus and the incoming
+ *   media bus format is other than UYVY8_2X8/YUYV8_2X8.
+ */
+static inline bool requires_passthrough(struct v4l2_fwnode_endpoint *ep,
+					struct v4l2_mbus_framefmt *infmt,
+					const struct imx_media_pixfmt *incc)
+{
+	if (ep->bus_type == V4L2_MBUS_BT656) // including BT.1120
+		return false;
+
+	return incc->bayer || is_parallel_16bit_bus(ep) ||
+		(is_parallel_bus(ep) &&
+		 infmt->code != MEDIA_BUS_FMT_UYVY8_2X8 &&
+		 infmt->code != MEDIA_BUS_FMT_YUYV8_2X8);
+}
+
+/*
+ * Parses the fwnode endpoint from the source pad of the entity
+ * connected to this CSI. This will either be the entity directly
+ * upstream from the CSI-2 receiver, directly upstream from the
+ * video mux, or directly upstream from the CSI itself. The endpoint
+ * is needed to determine the bus type and bus config coming into
+ * the CSI.
+ */
+static int csi_get_upstream_endpoint(struct csi_priv *priv,
+				     struct v4l2_fwnode_endpoint *ep)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_subdev *sd;
+	struct media_pad *pad;
+
+	if (!IS_ENABLED(CONFIG_OF))
+		return -ENXIO;
+
+	if (!priv->src_sd)
+		return -EPIPE;
+
+	sd = priv->src_sd;
+
+	switch (sd->grp_id) {
+	case IMX_MEDIA_GRP_ID_CSI_MUX:
+		/*
+		 * CSI is connected directly to CSI mux, skip up to
+		 * CSI-2 receiver if it is in the path, otherwise stay
+		 * with the CSI mux.
+		 */
+		sd = imx_media_pipeline_subdev(&sd->entity,
+					       IMX_MEDIA_GRP_ID_CSI2,
+					       true);
+		if (IS_ERR(sd))
+			sd = priv->src_sd;
+		break;
+	case IMX_MEDIA_GRP_ID_CSI2:
+		break;
+	default:
+		/*
+		 * the source is neither the CSI mux nor the CSI-2 receiver,
+		 * get the source pad directly upstream from CSI itself.
+		 */
+		sd = &priv->sd;
+		break;
+	}
+
+	/* get source pad of entity directly upstream from sd */
+	pad = imx_media_pipeline_pad(&sd->entity, 0, 0, true);
+	if (!pad)
+		return -ENODEV;
+
+	endpoint = imx_media_get_pad_fwnode(pad);
+	if (IS_ERR(endpoint))
+		return PTR_ERR(endpoint);
+
+	v4l2_fwnode_endpoint_parse(endpoint, ep);
+
+	fwnode_handle_put(endpoint);
+
+	return 0;
+}
+
+static void csi_idmac_put_ipu_resources(struct csi_priv *priv)
+{
+	if (priv->idmac_ch)
+		ipu_idmac_put(priv->idmac_ch);
+	priv->idmac_ch = NULL;
+
+	if (priv->smfc)
+		ipu_smfc_put(priv->smfc);
+	priv->smfc = NULL;
+}
+
+static int csi_idmac_get_ipu_resources(struct csi_priv *priv)
+{
+	int ch_num, ret;
+	struct ipu_smfc *smfc;
+	struct ipuv3_channel *idmac_ch;
+
+	ch_num = IPUV3_CHANNEL_CSI0 + priv->smfc_id;
+
+	smfc = ipu_smfc_get(priv->ipu, ch_num);
+	if (IS_ERR(smfc)) {
+		v4l2_err(&priv->sd, "failed to get SMFC\n");
+		ret = PTR_ERR(smfc);
+		goto out;
+	}
+	priv->smfc = smfc;
+
+	idmac_ch = ipu_idmac_get(priv->ipu, ch_num);
+	if (IS_ERR(idmac_ch)) {
+		v4l2_err(&priv->sd, "could not get IDMAC channel %u\n",
+			 ch_num);
+		ret = PTR_ERR(idmac_ch);
+		goto out;
+	}
+	priv->idmac_ch = idmac_ch;
+
+	return 0;
+out:
+	csi_idmac_put_ipu_resources(priv);
+	return ret;
+}
+
+static void csi_vb2_buf_done(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *done, *next;
+	struct vb2_buffer *vb;
+	dma_addr_t phys;
+
+	done = priv->active_vb2_buf[priv->ipu_buf_num];
+	if (done) {
+		done->vbuf.field = vdev->fmt.field;
+		done->vbuf.sequence = priv->frame_sequence;
+		vb = &done->vbuf.vb2_buf;
+		vb->timestamp = ktime_get_ns();
+		vb2_buffer_done(vb, priv->nfb4eof ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	}
+
+	priv->frame_sequence++;
+	priv->nfb4eof = false;
+
+	/* get next queued buffer */
+	next = imx_media_capture_device_next_buf(vdev);
+	if (next) {
+		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
+		priv->active_vb2_buf[priv->ipu_buf_num] = next;
+	} else {
+		phys = priv->underrun_buf.phys;
+		priv->active_vb2_buf[priv->ipu_buf_num] = NULL;
+	}
+
+	if (ipu_idmac_buffer_is_ready(priv->idmac_ch, priv->ipu_buf_num))
+		ipu_idmac_clear_buffer(priv->idmac_ch, priv->ipu_buf_num);
+
+	if (priv->interweave_swap)
+		phys += vdev->fmt.bytesperline;
+
+	ipu_cpmem_set_buffer(priv->idmac_ch, priv->ipu_buf_num, phys);
+}
+
+static irqreturn_t csi_idmac_eof_interrupt(int irq, void *dev_id)
+{
+	struct csi_priv *priv = dev_id;
+
+	spin_lock(&priv->irqlock);
+
+	if (priv->last_eof) {
+		complete(&priv->last_eof_comp);
+		priv->last_eof = false;
+		goto unlock;
+	}
+
+	if (priv->fim)
+		/* call frame interval monitor */
+		imx_media_fim_eof_monitor(priv->fim, ktime_get());
+
+	csi_vb2_buf_done(priv);
+
+	/* select new IPU buf */
+	ipu_idmac_select_buffer(priv->idmac_ch, priv->ipu_buf_num);
+	/* toggle IPU double-buffer index */
+	priv->ipu_buf_num ^= 1;
+
+	/* bump the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+unlock:
+	spin_unlock(&priv->irqlock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t csi_idmac_nfb4eof_interrupt(int irq, void *dev_id)
+{
+	struct csi_priv *priv = dev_id;
+
+	spin_lock(&priv->irqlock);
+
+	/*
+	 * this is not an unrecoverable error, just mark
+	 * the next captured frame with vb2 error flag.
+	 */
+	priv->nfb4eof = true;
+
+	v4l2_err(&priv->sd, "NFB4EOF\n");
+
+	spin_unlock(&priv->irqlock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * EOF timeout timer function. This is an unrecoverable condition
+ * without a stream restart.
+ */
+static void csi_idmac_eof_timeout(struct timer_list *t)
+{
+	struct csi_priv *priv = from_timer(priv, t, eof_timeout_timer);
+	struct imx_media_video_dev *vdev = priv->vdev;
+
+	v4l2_err(&priv->sd, "EOF timeout\n");
+
+	/* signal a fatal error to capture device */
+	imx_media_capture_device_error(vdev);
+}
+
+static void csi_idmac_setup_vb2_buf(struct csi_priv *priv, dma_addr_t *phys)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *buf;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		buf = imx_media_capture_device_next_buf(vdev);
+		if (buf) {
+			priv->active_vb2_buf[i] = buf;
+			phys[i] = vb2_dma_contig_plane_dma_addr(
+				&buf->vbuf.vb2_buf, 0);
+		} else {
+			priv->active_vb2_buf[i] = NULL;
+			phys[i] = priv->underrun_buf.phys;
+		}
+	}
+}
+
+static void csi_idmac_unsetup_vb2_buf(struct csi_priv *priv,
+				      enum vb2_buffer_state return_status)
+{
+	struct imx_media_buffer *buf;
+	int i;
+
+	/* return any remaining active frames with return_status */
+	for (i = 0; i < 2; i++) {
+		buf = priv->active_vb2_buf[i];
+		if (buf) {
+			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vb2_buffer_done(vb, return_status);
+		}
+	}
+}
+
+/* init the SMFC IDMAC channel */
+static int csi_idmac_setup_channel(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_mbus_framefmt *outfmt;
+	bool passthrough, interweave;
+	struct ipu_image image;
+	u32 passthrough_bits;
+	u32 passthrough_cycles;
+	dma_addr_t phys[2];
+	u32 burst_size;
+	int ret;
+
+	infmt = &priv->format_mbus[CSI_SINK_PAD];
+	incc = priv->cc[CSI_SINK_PAD];
+	outfmt = &priv->format_mbus[CSI_SRC_PAD_IDMAC];
+
+	ipu_cpmem_zero(priv->idmac_ch);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt;
+	image.rect = vdev->compose;
+
+	csi_idmac_setup_vb2_buf(priv, phys);
+
+	image.phys0 = phys[0];
+	image.phys1 = phys[1];
+
+	passthrough = requires_passthrough(&priv->upstream_ep, infmt, incc);
+	passthrough_cycles = 1;
+
+	/*
+	 * If the field type at capture interface is interlaced, and
+	 * the output IDMAC pad is sequential, enable interweave at
+	 * the IDMAC output channel.
+	 */
+	interweave = V4L2_FIELD_IS_INTERLACED(image.pix.field) &&
+		V4L2_FIELD_IS_SEQUENTIAL(outfmt->field);
+	priv->interweave_swap = interweave &&
+		image.pix.field == V4L2_FIELD_INTERLACED_BT;
+
+	switch (image.pix.pixelformat) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+	case V4L2_PIX_FMT_GREY:
+		burst_size = 16;
+		passthrough_bits = 8;
+		break;
+	case V4L2_PIX_FMT_SBGGR16:
+	case V4L2_PIX_FMT_SGBRG16:
+	case V4L2_PIX_FMT_SGRBG16:
+	case V4L2_PIX_FMT_SRGGB16:
+	case V4L2_PIX_FMT_Y10:
+	case V4L2_PIX_FMT_Y12:
+		burst_size = 8;
+		passthrough_bits = 16;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_NV12:
+		burst_size = (image.pix.width & 0x3f) ?
+			     ((image.pix.width & 0x1f) ?
+			      ((image.pix.width & 0xf) ? 8 : 16) : 32) : 64;
+		passthrough_bits = 16;
+		/*
+		 * Skip writing U and V components to odd rows (but not
+		 * when enabling IDMAC interweaving, they are incompatible).
+		 */
+		if (!interweave)
+			ipu_cpmem_skip_odd_chroma_rows(priv->idmac_ch);
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+		burst_size = (image.pix.width & 0x1f) ?
+			     ((image.pix.width & 0xf) ? 8 : 16) : 32;
+		passthrough_bits = 16;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		if (passthrough) {
+			burst_size = 16;
+			passthrough_bits = 8;
+			passthrough_cycles = incc->cycles;
+			break;
+		}
+		fallthrough;	/* non-passthrough RGB565 (CSI-2 bus) */
+	default:
+		burst_size = (image.pix.width & 0xf) ? 8 : 16;
+		passthrough_bits = 16;
+		break;
+	}
+
+	if (passthrough) {
+		if (priv->interweave_swap) {
+			/* start interweave scan at 1st top line (2nd line) */
+			image.phys0 += image.pix.bytesperline;
+			image.phys1 += image.pix.bytesperline;
+		}
+
+		ipu_cpmem_set_resolution(priv->idmac_ch,
+					 image.rect.width * passthrough_cycles,
+					 image.rect.height);
+		ipu_cpmem_set_stride(priv->idmac_ch, image.pix.bytesperline);
+		ipu_cpmem_set_buffer(priv->idmac_ch, 0, image.phys0);
+		ipu_cpmem_set_buffer(priv->idmac_ch, 1, image.phys1);
+		ipu_cpmem_set_format_passthrough(priv->idmac_ch,
+						 passthrough_bits);
+	} else {
+		if (priv->interweave_swap) {
+			/* start interweave scan at 1st top line (2nd line) */
+			image.rect.top = 1;
+		}
+
+		ret = ipu_cpmem_set_image(priv->idmac_ch, &image);
+		if (ret)
+			goto unsetup_vb2;
+	}
+
+	ipu_cpmem_set_burstsize(priv->idmac_ch, burst_size);
+
+	/*
+	 * Set the channel for the direct CSI-->memory via SMFC
+	 * use-case to very high priority, by enabling the watermark
+	 * signal in the SMFC, enabling WM in the channel, and setting
+	 * the channel priority to high.
+	 *
+	 * Refer to the i.mx6 rev. D TRM Table 36-8: Calculated priority
+	 * value.
+	 *
+	 * The WM's are set very low by intention here to ensure that
+	 * the SMFC FIFOs do not overflow.
+	 */
+	ipu_smfc_set_watermark(priv->smfc, 0x02, 0x01);
+	ipu_cpmem_set_high_priority(priv->idmac_ch);
+	ipu_idmac_enable_watermark(priv->idmac_ch, true);
+	ipu_cpmem_set_axi_id(priv->idmac_ch, 0);
+
+	burst_size = passthrough ?
+		(burst_size >> 3) - 1 : (burst_size >> 2) - 1;
+
+	ipu_smfc_set_burstsize(priv->smfc, burst_size);
+
+	if (interweave)
+		ipu_cpmem_interlaced_scan(priv->idmac_ch,
+					  priv->interweave_swap ?
+					  -image.pix.bytesperline :
+					  image.pix.bytesperline,
+					  image.pix.pixelformat);
+
+	ipu_idmac_set_double_buffer(priv->idmac_ch, true);
+
+	return 0;
+
+unsetup_vb2:
+	csi_idmac_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void csi_idmac_unsetup(struct csi_priv *priv,
+			      enum vb2_buffer_state state)
+{
+	ipu_idmac_disable_channel(priv->idmac_ch);
+	ipu_smfc_disable(priv->smfc);
+
+	csi_idmac_unsetup_vb2_buf(priv, state);
+}
+
+static int csi_idmac_setup(struct csi_priv *priv)
+{
+	int ret;
+
+	ret = csi_idmac_setup_channel(priv);
+	if (ret)
+		return ret;
+
+	ipu_cpmem_dump(priv->idmac_ch);
+	ipu_dump(priv->ipu);
+
+	ipu_smfc_enable(priv->smfc);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->idmac_ch, 0);
+	ipu_idmac_select_buffer(priv->idmac_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->idmac_ch);
+
+	return 0;
+}
+
+static int csi_idmac_start(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	int ret;
+
+	ret = csi_idmac_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	ipu_smfc_map_channel(priv->smfc, priv->csi_id, priv->vc_num);
+
+	ret = imx_media_alloc_dma_buf(priv->dev, &priv->underrun_buf,
+				      vdev->fmt.sizeimage);
+	if (ret)
+		goto out_put_ipu;
+
+	priv->ipu_buf_num = 0;
+
+	/* init EOF completion waitq */
+	init_completion(&priv->last_eof_comp);
+	priv->frame_sequence = 0;
+	priv->last_eof = false;
+	priv->nfb4eof = false;
+
+	ret = csi_idmac_setup(priv);
+	if (ret) {
+		v4l2_err(&priv->sd, "csi_idmac_setup failed: %d\n", ret);
+		goto out_free_dma_buf;
+	}
+
+	priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu,
+						  priv->idmac_ch,
+						  IPU_IRQ_NFB4EOF);
+	ret = devm_request_irq(priv->dev, priv->nfb4eof_irq,
+			       csi_idmac_nfb4eof_interrupt, 0,
+			       "imx-smfc-nfb4eof", priv);
+	if (ret) {
+		v4l2_err(&priv->sd,
+			 "Error registering NFB4EOF irq: %d\n", ret);
+		goto out_unsetup;
+	}
+
+	priv->eof_irq = ipu_idmac_channel_irq(priv->ipu, priv->idmac_ch,
+					      IPU_IRQ_EOF);
+
+	ret = devm_request_irq(priv->dev, priv->eof_irq,
+			       csi_idmac_eof_interrupt, 0,
+			       "imx-smfc-eof", priv);
+	if (ret) {
+		v4l2_err(&priv->sd,
+			 "Error registering eof irq: %d\n", ret);
+		goto out_free_nfb4eof_irq;
+	}
+
+	/* start the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+	return 0;
+
+out_free_nfb4eof_irq:
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);
+out_unsetup:
+	csi_idmac_unsetup(priv, VB2_BUF_STATE_QUEUED);
+out_free_dma_buf:
+	imx_media_free_dma_buf(priv->dev, &priv->underrun_buf);
+out_put_ipu:
+	csi_idmac_put_ipu_resources(priv);
+	return ret;
+}
+
+static void csi_idmac_wait_last_eof(struct csi_priv *priv)
+{
+	unsigned long flags;
+	int ret;
+
+	/* mark next EOF interrupt as the last before stream off */
+	spin_lock_irqsave(&priv->irqlock, flags);
+	priv->last_eof = true;
+	spin_unlock_irqrestore(&priv->irqlock, flags);
+
+	/*
+	 * and then wait for interrupt handler to mark completion.
+	 */
+	ret = wait_for_completion_timeout(
+		&priv->last_eof_comp, msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(&priv->sd, "wait last EOF timeout\n");
+}
+
+static void csi_idmac_stop(struct csi_priv *priv)
+{
+	devm_free_irq(priv->dev, priv->eof_irq, priv);
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);
+
+	csi_idmac_unsetup(priv, VB2_BUF_STATE_ERROR);
+
+	imx_media_free_dma_buf(priv->dev, &priv->underrun_buf);
+
+	/* cancel the EOF timeout timer */
+	del_timer_sync(&priv->eof_timeout_timer);
+
+	csi_idmac_put_ipu_resources(priv);
+}
+
+/* Update the CSI whole sensor and active windows */
+static int csi_setup(struct csi_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt, *outfmt;
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_config mbus_cfg;
+	struct v4l2_mbus_framefmt if_fmt;
+	struct v4l2_rect crop;
+
+	infmt = &priv->format_mbus[CSI_SINK_PAD];
+	incc = priv->cc[CSI_SINK_PAD];
+	outfmt = &priv->format_mbus[priv->active_output_pad];
+
+	/* compose mbus_config from the upstream endpoint */
+	mbus_cfg.type = priv->upstream_ep.bus_type;
+	if (is_parallel_bus(&priv->upstream_ep))
+		mbus_cfg.bus.parallel = priv->upstream_ep.bus.parallel;
+	else
+		mbus_cfg.bus.mipi_csi2 = priv->upstream_ep.bus.mipi_csi2;
+
+	if_fmt = *infmt;
+	crop = priv->crop;
+
+	/*
+	 * if cycles is set, we need to handle this over multiple cycles as
+	 * generic/bayer data
+	 */
+	if (is_parallel_bus(&priv->upstream_ep) && incc->cycles) {
+		if_fmt.width *= incc->cycles;
+		crop.width *= incc->cycles;
+	}
+
+	ipu_csi_set_window(priv->csi, &crop);
+
+	ipu_csi_set_downsize(priv->csi,
+			     priv->crop.width == 2 * priv->compose.width,
+			     priv->crop.height == 2 * priv->compose.height);
+
+	ipu_csi_init_interface(priv->csi, &mbus_cfg, &if_fmt, outfmt);
+
+	ipu_csi_set_dest(priv->csi, priv->dest);
+
+	if (priv->dest == IPU_CSI_DEST_IDMAC)
+		ipu_csi_set_skip_smfc(priv->csi, priv->skip->skip_smfc,
+				      priv->skip->max_ratio - 1, 0);
+
+	ipu_csi_dump(priv->csi);
+
+	return 0;
+}
+
+static int csi_start(struct csi_priv *priv)
+{
+	struct v4l2_fract *input_fi, *output_fi;
+	int ret;
+
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+	output_fi = &priv->frame_interval[priv->active_output_pad];
+
+	/* start upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, 1);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret)
+		return ret;
+
+	/* Skip first few frames from a BT.656 source */
+	if (priv->upstream_ep.bus_type == V4L2_MBUS_BT656) {
+		u32 delay_usec, bad_frames = 20;
+
+		delay_usec = DIV_ROUND_UP_ULL((u64)USEC_PER_SEC *
+			input_fi->numerator * bad_frames,
+			input_fi->denominator);
+
+		usleep_range(delay_usec, delay_usec + 1000);
+	}
+
+	if (priv->dest == IPU_CSI_DEST_IDMAC) {
+		ret = csi_idmac_start(priv);
+		if (ret)
+			goto stop_upstream;
+	}
+
+	ret = csi_setup(priv);
+	if (ret)
+		goto idmac_stop;
+
+	/* start the frame interval monitor */
+	if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC) {
+		ret = imx_media_fim_set_stream(priv->fim, output_fi, true);
+		if (ret)
+			goto idmac_stop;
+	}
+
+	ret = ipu_csi_enable(priv->csi);
+	if (ret) {
+		v4l2_err(&priv->sd, "CSI enable error: %d\n", ret);
+		goto fim_off;
+	}
+
+	return 0;
+
+fim_off:
+	if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC)
+		imx_media_fim_set_stream(priv->fim, NULL, false);
+idmac_stop:
+	if (priv->dest == IPU_CSI_DEST_IDMAC)
+		csi_idmac_stop(priv);
+stop_upstream:
+	v4l2_subdev_call(priv->src_sd, video, s_stream, 0);
+	return ret;
+}
+
+static void csi_stop(struct csi_priv *priv)
+{
+	if (priv->dest == IPU_CSI_DEST_IDMAC)
+		csi_idmac_wait_last_eof(priv);
+
+	/*
+	 * Disable the CSI asap, after syncing with the last EOF.
+	 * Doing so after the IDMA channel is disabled has shown to
+	 * create hard system-wide hangs.
+	 */
+	ipu_csi_disable(priv->csi);
+
+	/* stop upstream */
+	v4l2_subdev_call(priv->src_sd, video, s_stream, 0);
+
+	if (priv->dest == IPU_CSI_DEST_IDMAC) {
+		csi_idmac_stop(priv);
+
+		/* stop the frame interval monitor */
+		if (priv->fim)
+			imx_media_fim_set_stream(priv->fim, NULL, false);
+	}
+}
+
+static const struct csi_skip_desc csi_skip[12] = {
+	{ 1, 1, 0x00 }, /* Keep all frames */
+	{ 5, 6, 0x10 }, /* Skip every sixth frame */
+	{ 4, 5, 0x08 }, /* Skip every fifth frame */
+	{ 3, 4, 0x04 }, /* Skip every fourth frame */
+	{ 2, 3, 0x02 }, /* Skip every third frame */
+	{ 3, 5, 0x0a }, /* Skip frames 1 and 3 of every 5 */
+	{ 1, 2, 0x01 }, /* Skip every second frame */
+	{ 2, 5, 0x0b }, /* Keep frames 1 and 4 of every 5 */
+	{ 1, 3, 0x03 }, /* Keep one in three frames */
+	{ 1, 4, 0x07 }, /* Keep one in four frames */
+	{ 1, 5, 0x0f }, /* Keep one in five frames */
+	{ 1, 6, 0x1f }, /* Keep one in six frames */
+};
+
+static void csi_apply_skip_interval(const struct csi_skip_desc *skip,
+				    struct v4l2_fract *interval)
+{
+	unsigned int div;
+
+	interval->numerator *= skip->max_ratio;
+	interval->denominator *= skip->keep;
+
+	/* Reduce fraction to lowest terms */
+	div = gcd(interval->numerator, interval->denominator);
+	if (div > 1) {
+		interval->numerator /= div;
+		interval->denominator /= div;
+	}
+}
+
+/*
+ * Find the skip pattern to produce the output frame interval closest to the
+ * requested one, for the given input frame interval. Updates the output frame
+ * interval to the exact value.
+ */
+static const struct csi_skip_desc *csi_find_best_skip(struct v4l2_fract *in,
+						      struct v4l2_fract *out)
+{
+	const struct csi_skip_desc *skip = &csi_skip[0], *best_skip = skip;
+	u32 min_err = UINT_MAX;
+	u64 want_us;
+	int i;
+
+	/* Default to 1:1 ratio */
+	if (out->numerator == 0 || out->denominator == 0 ||
+	    in->numerator == 0 || in->denominator == 0) {
+		*out = *in;
+		return best_skip;
+	}
+
+	want_us = div_u64((u64)USEC_PER_SEC * out->numerator, out->denominator);
+
+	/* Find the reduction closest to the requested time per frame */
+	for (i = 0; i < ARRAY_SIZE(csi_skip); i++, skip++) {
+		u64 tmp, err;
+
+		tmp = div_u64((u64)USEC_PER_SEC * in->numerator *
+			      skip->max_ratio, in->denominator * skip->keep);
+
+		err = abs((s64)tmp - want_us);
+		if (err < min_err) {
+			min_err = err;
+			best_skip = skip;
+		}
+	}
+
+	*out = *in;
+	csi_apply_skip_interval(best_skip, out);
+
+	return best_skip;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int csi_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+
+	if (fi->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fi->interval = priv->frame_interval[fi->pad];
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int csi_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+
+	switch (fi->pad) {
+	case CSI_SINK_PAD:
+		/* No limits on valid input frame intervals */
+		if (fi->interval.numerator == 0 ||
+		    fi->interval.denominator == 0)
+			fi->interval = *input_fi;
+		/* Reset output intervals and frame skipping ratio to 1:1 */
+		priv->frame_interval[CSI_SRC_PAD_IDMAC] = fi->interval;
+		priv->frame_interval[CSI_SRC_PAD_DIRECT] = fi->interval;
+		priv->skip = &csi_skip[0];
+		break;
+	case CSI_SRC_PAD_IDMAC:
+		/*
+		 * frame interval at IDMAC output pad depends on input
+		 * interval, modified by frame skipping.
+		 */
+		priv->skip = csi_find_best_skip(input_fi, &fi->interval);
+		break;
+	case CSI_SRC_PAD_DIRECT:
+		/*
+		 * frame interval at DIRECT output pad is same as input
+		 * interval.
+		 */
+		fi->interval = *input_fi;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	priv->frame_interval[fi->pad] = fi->interval;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || !priv->sink) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	if (enable) {
+		dev_dbg(priv->dev, "stream ON\n");
+		ret = csi_start(priv);
+		if (ret)
+			goto out;
+	} else {
+		dev_dbg(priv->dev, "stream OFF\n");
+		csi_stop(priv);
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(priv->dev, "link setup %s -> %s\n", remote->entity->name,
+		local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a source pad */
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->sink) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+		v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+		priv->sink = NULL;
+		/* do not apply IC burst alignment in csi_try_crop */
+		priv->active_output_pad = CSI_SRC_PAD_IDMAC;
+		goto out;
+	}
+
+	/* record which output pad is now active */
+	priv->active_output_pad = local->index;
+
+	/* set CSI destination */
+	if (local->index == CSI_SRC_PAD_IDMAC) {
+		if (!is_media_entity_v4l2_video_device(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (priv->fim) {
+			ret = imx_media_fim_add_controls(priv->fim);
+			if (ret)
+				goto out;
+		}
+
+		priv->dest = IPU_CSI_DEST_IDMAC;
+	} else {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+		switch (remote_sd->grp_id) {
+		case IMX_MEDIA_GRP_ID_IPU_VDIC:
+			priv->dest = IPU_CSI_DEST_VDIC;
+			break;
+		case IMX_MEDIA_GRP_ID_IPU_IC_PRP:
+			priv->dest = IPU_CSI_DEST_IC;
+			break;
+		default:
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+	priv->sink = remote->entity;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_link_validate(struct v4l2_subdev *sd,
+			     struct media_link *link,
+			     struct v4l2_subdev_format *source_fmt,
+			     struct v4l2_subdev_format *sink_fmt)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fwnode_endpoint upstream_ep = { .bus_type = 0 };
+	bool is_csi2;
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	ret = csi_get_upstream_endpoint(priv, &upstream_ep);
+	if (ret) {
+		v4l2_err(&priv->sd, "failed to find upstream endpoint\n");
+		return ret;
+	}
+
+	mutex_lock(&priv->lock);
+
+	priv->upstream_ep = upstream_ep;
+	is_csi2 = !is_parallel_bus(&upstream_ep);
+	if (is_csi2) {
+		/*
+		 * NOTE! It seems the virtual channels from the mipi csi-2
+		 * receiver are used only for routing by the video mux's,
+		 * or for hard-wired routing to the CSI's. Once the stream
+		 * enters the CSI's however, they are treated internally
+		 * in the IPU as virtual channel 0.
+		 */
+		ipu_csi_set_mipi_datatype(priv->csi, 0,
+					  &priv->format_mbus[CSI_SINK_PAD]);
+	}
+
+	/* select either parallel or MIPI-CSI2 as input to CSI */
+	ipu_set_csi_src_mux(priv->ipu, priv->csi_id, is_csi2);
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__csi_get_fmt(struct csi_priv *priv, struct v4l2_subdev_state *sd_state,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&priv->sd, sd_state, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+static struct v4l2_rect *
+__csi_get_crop(struct csi_priv *priv, struct v4l2_subdev_state *sd_state,
+	       enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&priv->sd, sd_state,
+						CSI_SINK_PAD);
+	else
+		return &priv->crop;
+}
+
+static struct v4l2_rect *
+__csi_get_compose(struct csi_priv *priv, struct v4l2_subdev_state *sd_state,
+		  enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_compose(&priv->sd, sd_state,
+						   CSI_SINK_PAD);
+	else
+		return &priv->compose;
+}
+
+static void csi_try_crop(struct csi_priv *priv,
+			 struct v4l2_rect *crop,
+			 struct v4l2_subdev_state *sd_state,
+			 struct v4l2_mbus_framefmt *infmt,
+			 struct v4l2_fwnode_endpoint *upstream_ep)
+{
+	u32 in_height;
+
+	crop->width = min_t(__u32, infmt->width, crop->width);
+	if (crop->left + crop->width > infmt->width)
+		crop->left = infmt->width - crop->width;
+	/* adjust crop left/width to h/w alignment restrictions */
+	crop->left &= ~0x3;
+	if (priv->active_output_pad == CSI_SRC_PAD_DIRECT)
+		crop->width &= ~0x7; /* multiple of 8 pixels (IC burst) */
+	else
+		crop->width &= ~0x1; /* multiple of 2 pixels */
+
+	in_height = infmt->height;
+	if (infmt->field == V4L2_FIELD_ALTERNATE)
+		in_height *= 2;
+
+	/*
+	 * FIXME: not sure why yet, but on interlaced bt.656,
+	 * changing the vertical cropping causes loss of vertical
+	 * sync, so fix it to NTSC/PAL active lines. NTSC contains
+	 * 2 extra lines of active video that need to be cropped.
+	 */
+	if (upstream_ep->bus_type == V4L2_MBUS_BT656 &&
+	    (V4L2_FIELD_HAS_BOTH(infmt->field) ||
+	     infmt->field == V4L2_FIELD_ALTERNATE)) {
+		crop->height = in_height;
+		crop->top = (in_height == 480) ? 2 : 0;
+	} else {
+		crop->height = min_t(__u32, in_height, crop->height);
+		if (crop->top + crop->height > in_height)
+			crop->top = in_height - crop->height;
+	}
+}
+
+static int csi_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fwnode_endpoint upstream_ep = { .bus_type = 0 };
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	infmt = __csi_get_fmt(priv, sd_state, CSI_SINK_PAD, code->which);
+	incc = imx_media_find_mbus_format(infmt->code, PIXFMT_SEL_ANY);
+
+	switch (code->pad) {
+	case CSI_SINK_PAD:
+		ret = imx_media_enum_mbus_formats(&code->code, code->index,
+						  PIXFMT_SEL_ANY);
+		break;
+	case CSI_SRC_PAD_DIRECT:
+	case CSI_SRC_PAD_IDMAC:
+		ret = csi_get_upstream_endpoint(priv, &upstream_ep);
+		if (ret) {
+			v4l2_err(&priv->sd, "failed to find upstream endpoint\n");
+			goto out;
+		}
+
+		if (requires_passthrough(&upstream_ep, infmt, incc)) {
+			if (code->index != 0) {
+				ret = -EINVAL;
+				goto out;
+			}
+			code->code = infmt->code;
+		} else {
+			enum imx_pixfmt_sel fmt_sel =
+				(incc->cs == IPUV3_COLORSPACE_YUV) ?
+				PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
+
+			ret = imx_media_enum_ipu_formats(&code->code,
+							 code->index,
+							 fmt_sel);
+		}
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	if (fse->pad >= CSI_NUM_PADS ||
+	    fse->index > (fse->pad == CSI_SINK_PAD ? 0 : 3))
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (fse->pad == CSI_SINK_PAD) {
+		fse->min_width = MIN_W;
+		fse->max_width = MAX_W;
+		fse->min_height = MIN_H;
+		fse->max_height = MAX_H;
+	} else {
+		crop = __csi_get_crop(priv, sd_state, fse->which);
+
+		fse->min_width = fse->index & 1 ?
+			crop->width / 2 : crop->width;
+		fse->max_width = fse->min_width;
+		fse->min_height = fse->index & 2 ?
+			crop->height / 2 : crop->height;
+		fse->max_height = fse->min_height;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_enum_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi;
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	if (fie->pad >= CSI_NUM_PADS ||
+	    fie->index >= (fie->pad != CSI_SRC_PAD_IDMAC ?
+			   1 : ARRAY_SIZE(csi_skip)))
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+	crop = __csi_get_crop(priv, sd_state, fie->which);
+
+	if ((fie->width != crop->width && fie->width != crop->width / 2) ||
+	    (fie->height != crop->height && fie->height != crop->height / 2)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	fie->interval = *input_fi;
+
+	if (fie->pad == CSI_SRC_PAD_IDMAC)
+		csi_apply_skip_interval(&csi_skip[fie->index],
+					&fie->interval);
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __csi_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void csi_try_field(struct csi_priv *priv,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *sdformat)
+{
+	struct v4l2_mbus_framefmt *infmt =
+		__csi_get_fmt(priv, sd_state, CSI_SINK_PAD, sdformat->which);
+
+	/*
+	 * no restrictions on sink pad field type except must
+	 * be initialized.
+	 */
+	if (sdformat->pad == CSI_SINK_PAD) {
+		if (sdformat->format.field == V4L2_FIELD_ANY)
+			sdformat->format.field = V4L2_FIELD_NONE;
+		return;
+	}
+
+	switch (infmt->field) {
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
+		/*
+		 * If the user requests sequential at the source pad,
+		 * allow it (along with possibly inverting field order).
+		 * Otherwise passthrough the field type.
+		 */
+		if (!V4L2_FIELD_IS_SEQUENTIAL(sdformat->format.field))
+			sdformat->format.field = infmt->field;
+		break;
+	case V4L2_FIELD_ALTERNATE:
+		/*
+		 * This driver does not support alternate field mode, and
+		 * the CSI captures a whole frame, so the CSI never presents
+		 * alternate mode at its source pads. If user has not
+		 * already requested sequential, translate ALTERNATE at
+		 * sink pad to SEQ_TB or SEQ_BT at the source pad depending
+		 * on input height (assume NTSC BT order if 480 total active
+		 * frame lines, otherwise PAL TB order).
+		 */
+		if (!V4L2_FIELD_IS_SEQUENTIAL(sdformat->format.field))
+			sdformat->format.field = (infmt->height == 480 / 2) ?
+				V4L2_FIELD_SEQ_BT : V4L2_FIELD_SEQ_TB;
+		break;
+	default:
+		/* Passthrough for all other input field types */
+		sdformat->format.field = infmt->field;
+		break;
+	}
+}
+
+static void csi_try_fmt(struct csi_priv *priv,
+			struct v4l2_fwnode_endpoint *upstream_ep,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat,
+			struct v4l2_rect *crop,
+			struct v4l2_rect *compose,
+			const struct imx_media_pixfmt **cc)
+{
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	u32 code;
+
+	infmt = __csi_get_fmt(priv, sd_state, CSI_SINK_PAD, sdformat->which);
+
+	switch (sdformat->pad) {
+	case CSI_SRC_PAD_DIRECT:
+	case CSI_SRC_PAD_IDMAC:
+		incc = imx_media_find_mbus_format(infmt->code, PIXFMT_SEL_ANY);
+
+		sdformat->format.width = compose->width;
+		sdformat->format.height = compose->height;
+
+		if (requires_passthrough(upstream_ep, infmt, incc)) {
+			sdformat->format.code = infmt->code;
+			*cc = incc;
+		} else {
+			enum imx_pixfmt_sel fmt_sel =
+				(incc->cs == IPUV3_COLORSPACE_YUV) ?
+				PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
+
+			*cc = imx_media_find_ipu_format(sdformat->format.code,
+							fmt_sel);
+			if (!*cc) {
+				imx_media_enum_ipu_formats(&code, 0, fmt_sel);
+				*cc = imx_media_find_ipu_format(code, fmt_sel);
+				sdformat->format.code = (*cc)->codes[0];
+			}
+		}
+
+		csi_try_field(priv, sd_state, sdformat);
+
+		/* propagate colorimetry from sink */
+		sdformat->format.colorspace = infmt->colorspace;
+		sdformat->format.xfer_func = infmt->xfer_func;
+		sdformat->format.quantization = infmt->quantization;
+		sdformat->format.ycbcr_enc = infmt->ycbcr_enc;
+
+		break;
+	case CSI_SINK_PAD:
+		v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W,
+				      W_ALIGN, &sdformat->format.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+
+		*cc = imx_media_find_mbus_format(sdformat->format.code,
+						 PIXFMT_SEL_ANY);
+		if (!*cc) {
+			imx_media_enum_mbus_formats(&code, 0,
+						    PIXFMT_SEL_YUV_RGB);
+			*cc = imx_media_find_mbus_format(code,
+							 PIXFMT_SEL_YUV_RGB);
+			sdformat->format.code = (*cc)->codes[0];
+		}
+
+		csi_try_field(priv, sd_state, sdformat);
+
+		/* Reset crop and compose rectangles */
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = sdformat->format.width;
+		crop->height = sdformat->format.height;
+		if (sdformat->format.field == V4L2_FIELD_ALTERNATE)
+			crop->height *= 2;
+		csi_try_crop(priv, crop, sd_state, &sdformat->format,
+			     upstream_ep);
+		compose->left = 0;
+		compose->top = 0;
+		compose->width = crop->width;
+		compose->height = crop->height;
+
+		break;
+	}
+
+	imx_media_try_colorimetry(&sdformat->format,
+			priv->active_output_pad == CSI_SRC_PAD_DIRECT);
+}
+
+static int csi_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fwnode_endpoint upstream_ep = { .bus_type = 0 };
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_rect *crop, *compose;
+	int ret;
+
+	if (sdformat->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	ret = csi_get_upstream_endpoint(priv, &upstream_ep);
+	if (ret) {
+		v4l2_err(&priv->sd, "failed to find upstream endpoint\n");
+		return ret;
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	crop = __csi_get_crop(priv, sd_state, sdformat->which);
+	compose = __csi_get_compose(priv, sd_state, sdformat->which);
+
+	csi_try_fmt(priv, &upstream_ep, sd_state, sdformat, crop, compose,
+		    &cc);
+
+	fmt = __csi_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	if (sdformat->pad == CSI_SINK_PAD) {
+		int pad;
+
+		/* propagate format to source pads */
+		for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) {
+			const struct imx_media_pixfmt *outcc;
+			struct v4l2_mbus_framefmt *outfmt;
+			struct v4l2_subdev_format format;
+
+			format.pad = pad;
+			format.which = sdformat->which;
+			format.format = sdformat->format;
+			csi_try_fmt(priv, &upstream_ep, sd_state, &format,
+				    NULL, compose, &outcc);
+
+			outfmt = __csi_get_fmt(priv, sd_state, pad,
+					       sdformat->which);
+			*outfmt = format.format;
+
+			if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+				priv->cc[pad] = outcc;
+		}
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->cc[sdformat->pad] = cc;
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_get_selection(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_rect *crop, *compose;
+	int ret = 0;
+
+	if (sel->pad != CSI_SINK_PAD)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	infmt = __csi_get_fmt(priv, sd_state, CSI_SINK_PAD, sel->which);
+	crop = __csi_get_crop(priv, sd_state, sel->which);
+	compose = __csi_get_compose(priv, sd_state, sel->which);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = infmt->width;
+		sel->r.height = infmt->height;
+		if (infmt->field == V4L2_FIELD_ALTERNATE)
+			sel->r.height *= 2;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *crop;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = crop->width;
+		sel->r.height = crop->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *compose;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_set_scale(u32 *compose, u32 crop, u32 flags)
+{
+	if ((flags & (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE)) ==
+		     (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE) &&
+	    *compose != crop && *compose != crop / 2)
+		return -ERANGE;
+
+	if (*compose <= crop / 2 ||
+	    (*compose < crop * 3 / 4 && !(flags & V4L2_SEL_FLAG_GE)) ||
+	    (*compose < crop && (flags & V4L2_SEL_FLAG_LE)))
+		*compose = crop / 2;
+	else
+		*compose = crop;
+
+	return 0;
+}
+
+static int csi_set_selection(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fwnode_endpoint upstream_ep = { .bus_type = 0 };
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_rect *crop, *compose;
+	int pad, ret;
+
+	if (sel->pad != CSI_SINK_PAD)
+		return -EINVAL;
+
+	ret = csi_get_upstream_endpoint(priv, &upstream_ep);
+	if (ret) {
+		v4l2_err(&priv->sd, "failed to find upstream endpoint\n");
+		return ret;
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	infmt = __csi_get_fmt(priv, sd_state, CSI_SINK_PAD, sel->which);
+	crop = __csi_get_crop(priv, sd_state, sel->which);
+	compose = __csi_get_compose(priv, sd_state, sel->which);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		/*
+		 * Modifying the crop rectangle always changes the format on
+		 * the source pads. If the KEEP_CONFIG flag is set, just return
+		 * the current crop rectangle.
+		 */
+		if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+			sel->r = priv->crop;
+			if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+				*crop = sel->r;
+			goto out;
+		}
+
+		csi_try_crop(priv, &sel->r, sd_state, infmt, &upstream_ep);
+
+		*crop = sel->r;
+
+		/* Reset scaling to 1:1 */
+		compose->width = crop->width;
+		compose->height = crop->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		/*
+		 * Modifying the compose rectangle always changes the format on
+		 * the source pads. If the KEEP_CONFIG flag is set, just return
+		 * the current compose rectangle.
+		 */
+		if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+			sel->r = priv->compose;
+			if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+				*compose = sel->r;
+			goto out;
+		}
+
+		sel->r.left = 0;
+		sel->r.top = 0;
+		ret = csi_set_scale(&sel->r.width, crop->width, sel->flags);
+		if (ret)
+			goto out;
+		ret = csi_set_scale(&sel->r.height, crop->height, sel->flags);
+		if (ret)
+			goto out;
+
+		*compose = sel->r;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Reset source pads to sink compose rectangle */
+	for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) {
+		struct v4l2_mbus_framefmt *outfmt;
+
+		outfmt = __csi_get_fmt(priv, sd_state, pad, sel->which);
+		outfmt->width = compose->width;
+		outfmt->height = compose->height;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+			       struct v4l2_event_subscription *sub)
+{
+	if (sub->type != V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR)
+		return -EINVAL;
+	if (sub->id != 0)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, 0, NULL);
+}
+
+static int csi_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				 struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
+
+static int csi_registered(struct v4l2_subdev *sd)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct ipu_csi *csi;
+	int i, ret;
+	u32 code;
+
+	/* get handle to IPU CSI */
+	csi = ipu_csi_get(priv->ipu, priv->csi_id);
+	if (IS_ERR(csi)) {
+		v4l2_err(&priv->sd, "failed to get CSI%d\n", priv->csi_id);
+		return PTR_ERR(csi);
+	}
+	priv->csi = csi;
+
+	for (i = 0; i < CSI_NUM_PADS; i++) {
+		code = 0;
+		if (i != CSI_SINK_PAD)
+			imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
+
+		/* set a default mbus format  */
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      IMX_MEDIA_DEF_PIX_WIDTH,
+					      IMX_MEDIA_DEF_PIX_HEIGHT, code,
+					      V4L2_FIELD_NONE, &priv->cc[i]);
+		if (ret)
+			goto put_csi;
+
+		/* init default frame interval */
+		priv->frame_interval[i].numerator = 1;
+		priv->frame_interval[i].denominator = 30;
+	}
+
+	/* disable frame skipping */
+	priv->skip = &csi_skip[0];
+
+	/* init default crop and compose rectangle sizes */
+	priv->crop.width = IMX_MEDIA_DEF_PIX_WIDTH;
+	priv->crop.height = IMX_MEDIA_DEF_PIX_HEIGHT;
+	priv->compose.width = IMX_MEDIA_DEF_PIX_WIDTH;
+	priv->compose.height = IMX_MEDIA_DEF_PIX_HEIGHT;
+
+	priv->fim = imx_media_fim_init(&priv->sd);
+	if (IS_ERR(priv->fim)) {
+		ret = PTR_ERR(priv->fim);
+		goto put_csi;
+	}
+
+	priv->vdev = imx_media_capture_device_init(priv->sd.dev, &priv->sd,
+						   CSI_SRC_PAD_IDMAC, true);
+	if (IS_ERR(priv->vdev)) {
+		ret = PTR_ERR(priv->vdev);
+		goto free_fim;
+	}
+
+	ret = imx_media_capture_device_register(priv->vdev, 0);
+	if (ret)
+		goto remove_vdev;
+
+	return 0;
+
+remove_vdev:
+	imx_media_capture_device_remove(priv->vdev);
+free_fim:
+	if (priv->fim)
+		imx_media_fim_free(priv->fim);
+put_csi:
+	ipu_csi_put(priv->csi);
+	return ret;
+}
+
+static void csi_unregistered(struct v4l2_subdev *sd)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+
+	imx_media_capture_device_unregister(priv->vdev);
+	imx_media_capture_device_remove(priv->vdev);
+
+	if (priv->fim)
+		imx_media_fim_free(priv->fim);
+
+	if (priv->csi)
+		ipu_csi_put(priv->csi);
+}
+
+/*
+ * The CSI has only one fwnode endpoint, at the sink pad. Verify the
+ * endpoint belongs to us, and return CSI_SINK_PAD.
+ */
+static int csi_get_fwnode_pad(struct media_entity *entity,
+			      struct fwnode_endpoint *endpoint)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct fwnode_handle *csi_port = dev_fwnode(priv->dev);
+	struct fwnode_handle *csi_ep;
+	int ret;
+
+	csi_ep = fwnode_get_next_child_node(csi_port, NULL);
+
+	ret = endpoint->local_fwnode == csi_ep ? CSI_SINK_PAD : -ENXIO;
+
+	fwnode_handle_put(csi_ep);
+
+	return ret;
+}
+
+static const struct media_entity_operations csi_entity_ops = {
+	.link_setup = csi_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+	.get_fwnode_pad = csi_get_fwnode_pad,
+};
+
+static const struct v4l2_subdev_core_ops csi_core_ops = {
+	.subscribe_event = csi_subscribe_event,
+	.unsubscribe_event = csi_unsubscribe_event,
+};
+
+static const struct v4l2_subdev_video_ops csi_video_ops = {
+	.g_frame_interval = csi_g_frame_interval,
+	.s_frame_interval = csi_s_frame_interval,
+	.s_stream = csi_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
+	.enum_mbus_code = csi_enum_mbus_code,
+	.enum_frame_size = csi_enum_frame_size,
+	.enum_frame_interval = csi_enum_frame_interval,
+	.get_fmt = csi_get_fmt,
+	.set_fmt = csi_set_fmt,
+	.get_selection = csi_get_selection,
+	.set_selection = csi_set_selection,
+	.link_validate = csi_link_validate,
+};
+
+static const struct v4l2_subdev_ops csi_subdev_ops = {
+	.core = &csi_core_ops,
+	.video = &csi_video_ops,
+	.pad = &csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi_internal_ops = {
+	.registered = csi_registered,
+	.unregistered = csi_unregistered,
+};
+
+static int imx_csi_notify_bound(struct v4l2_async_notifier *notifier,
+				struct v4l2_subdev *sd,
+				struct v4l2_async_subdev *asd)
+{
+	struct csi_priv *priv = notifier_to_dev(notifier);
+	struct media_pad *sink = &priv->sd.entity.pads[CSI_SINK_PAD];
+
+	/*
+	 * If the subdev is a video mux, it must be one of the CSI
+	 * muxes. Mark it as such via its group id.
+	 */
+	if (sd->entity.function == MEDIA_ENT_F_VID_MUX)
+		sd->grp_id = IMX_MEDIA_GRP_ID_CSI_MUX;
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, 0);
+}
+
+static const struct v4l2_async_notifier_operations csi_notify_ops = {
+	.bound = imx_csi_notify_bound,
+};
+
+static int imx_csi_async_register(struct csi_priv *priv)
+{
+	struct v4l2_async_subdev *asd = NULL;
+	struct fwnode_handle *ep;
+	unsigned int port;
+	int ret;
+
+	v4l2_async_nf_init(&priv->notifier);
+
+	/* get this CSI's port id */
+	ret = fwnode_property_read_u32(dev_fwnode(priv->dev), "reg", &port);
+	if (ret < 0)
+		return ret;
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(priv->dev->parent),
+					     port, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (ep) {
+		asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep,
+						      struct v4l2_async_subdev);
+
+		fwnode_handle_put(ep);
+
+		if (IS_ERR(asd)) {
+			ret = PTR_ERR(asd);
+			/* OK if asd already exists */
+			if (ret != -EEXIST)
+				return ret;
+		}
+	}
+
+	priv->notifier.ops = &csi_notify_ops;
+
+	ret = v4l2_async_subdev_nf_register(&priv->sd, &priv->notifier);
+	if (ret)
+		return ret;
+
+	return v4l2_async_register_subdev(&priv->sd);
+}
+
+static int imx_csi_probe(struct platform_device *pdev)
+{
+	struct ipu_client_platformdata *pdata;
+	struct pinctrl *pinctrl;
+	struct csi_priv *priv;
+	int i, ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, &priv->sd);
+	priv->dev = &pdev->dev;
+
+	ret = dma_set_coherent_mask(priv->dev, DMA_BIT_MASK(32));
+	if (ret)
+		return ret;
+
+	/* get parent IPU */
+	priv->ipu = dev_get_drvdata(priv->dev->parent);
+
+	/* get our CSI id */
+	pdata = priv->dev->platform_data;
+	priv->csi_id = pdata->csi;
+	priv->smfc_id = (priv->csi_id == 0) ? 0 : 2;
+
+	priv->active_output_pad = CSI_SRC_PAD_IDMAC;
+
+	timer_setup(&priv->eof_timeout_timer, csi_idmac_eof_timeout, 0);
+	spin_lock_init(&priv->irqlock);
+
+	v4l2_subdev_init(&priv->sd, &csi_subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = &csi_internal_ops;
+	priv->sd.entity.ops = &csi_entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	priv->sd.dev = &pdev->dev;
+	priv->sd.fwnode = of_fwnode_handle(pdata->of_node);
+	priv->sd.owner = THIS_MODULE;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	priv->sd.grp_id = priv->csi_id ?
+		IMX_MEDIA_GRP_ID_IPU_CSI1 : IMX_MEDIA_GRP_ID_IPU_CSI0;
+	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
+				    priv->sd.grp_id, ipu_get_num(priv->ipu));
+
+	for (i = 0; i < CSI_NUM_PADS; i++)
+		priv->pad[i].flags = (i == CSI_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&priv->sd.entity, CSI_NUM_PADS,
+				     priv->pad);
+	if (ret)
+		return ret;
+
+	mutex_init(&priv->lock);
+
+	v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+	priv->sd.ctrl_handler = &priv->ctrl_hdlr;
+
+	/*
+	 * The IPUv3 driver did not assign an of_node to this
+	 * device. As a result, pinctrl does not automatically
+	 * configure our pin groups, so we need to do that manually
+	 * here, after setting this device's of_node.
+	 */
+	priv->dev->of_node = pdata->of_node;
+	pinctrl = devm_pinctrl_get_select_default(priv->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		dev_dbg(priv->dev,
+			"devm_pinctrl_get_select_default() failed: %d\n", ret);
+		if (ret != -ENODEV)
+			goto free;
+	}
+
+	ret = imx_csi_async_register(priv);
+	if (ret)
+		goto cleanup;
+
+	return 0;
+
+cleanup:
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+free:
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+	mutex_destroy(&priv->lock);
+	return ret;
+}
+
+static int imx_csi_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csi_priv *priv = sd_to_dev(sd);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+	mutex_destroy(&priv->lock);
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct platform_device_id imx_csi_ids[] = {
+	{ .name = "imx-ipuv3-csi" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, imx_csi_ids);
+
+static struct platform_driver imx_csi_driver = {
+	.probe = imx_csi_probe,
+	.remove = imx_csi_remove,
+	.id_table = imx_csi_ids,
+	.driver = {
+		.name = "imx-ipuv3-csi",
+	},
+};
+module_platform_driver(imx_csi_driver);
+
+MODULE_DESCRIPTION("i.MX CSI subdev driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-dev-common.c b/drivers/staging/media/imx/imx-media-dev-common.c
new file mode 100644
index 000000000..e6d6ed3b1
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-dev-common.c
@@ -0,0 +1,417 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * V4L2 Media Controller Driver for Freescale common i.MX5/6/7 SOC
+ *
+ * Copyright (c) 2019 Linaro Ltd
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+
+#include <linux/of_graph.h>
+#include <linux/of_platform.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include "imx-media.h"
+
+static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct imx_media_dev, notifier);
+}
+
+/* async subdev bound notifier */
+static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
+				  struct v4l2_subdev *sd,
+				  struct v4l2_async_subdev *asd)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+
+	dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name);
+
+	return 0;
+}
+
+/*
+ * Create the missing media links from the CSI-2 receiver.
+ * Called after all async subdevs have bound.
+ */
+static void imx_media_create_csi2_links(struct imx_media_dev *imxmd)
+{
+	struct v4l2_subdev *sd, *csi2 = NULL;
+
+	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
+		if (sd->grp_id == IMX_MEDIA_GRP_ID_CSI2) {
+			csi2 = sd;
+			break;
+		}
+	}
+	if (!csi2)
+		return;
+
+	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
+		/* skip if not a CSI or a CSI mux */
+		if (!(sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) &&
+		    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI) &&
+		    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI_MUX))
+			continue;
+
+		v4l2_create_fwnode_links(csi2, sd);
+	}
+}
+
+/*
+ * adds given video device to given imx-media source pad vdev list.
+ * Continues upstream from the pad entity's sink pads.
+ */
+static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
+				     struct imx_media_video_dev *vdev,
+				     struct media_pad *srcpad)
+{
+	struct media_entity *entity = srcpad->entity;
+	struct imx_media_pad_vdev *pad_vdev;
+	struct list_head *pad_vdev_list;
+	struct media_link *link;
+	struct v4l2_subdev *sd;
+	int i, ret;
+
+	/* skip this entity if not a v4l2_subdev */
+	if (!is_media_entity_v4l2_subdev(entity))
+		return 0;
+
+	sd = media_entity_to_v4l2_subdev(entity);
+
+	pad_vdev_list = to_pad_vdev_list(sd, srcpad->index);
+	if (!pad_vdev_list) {
+		v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n",
+			  entity->name, srcpad->index);
+		/*
+		 * shouldn't happen, but no reason to fail driver load,
+		 * just skip this entity.
+		 */
+		return 0;
+	}
+
+	/* just return if we've been here before */
+	list_for_each_entry(pad_vdev, pad_vdev_list, list) {
+		if (pad_vdev->vdev == vdev)
+			return 0;
+	}
+
+	dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
+		vdev->vfd->entity.name, entity->name, srcpad->index);
+
+	pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL);
+	if (!pad_vdev)
+		return -ENOMEM;
+
+	/* attach this vdev to this pad */
+	pad_vdev->vdev = vdev;
+	list_add_tail(&pad_vdev->list, pad_vdev_list);
+
+	/* move upstream from this entity's sink pads */
+	for (i = 0; i < entity->num_pads; i++) {
+		struct media_pad *pad = &entity->pads[i];
+
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			continue;
+
+		list_for_each_entry(link, &entity->links, list) {
+			if (link->sink != pad)
+				continue;
+			ret = imx_media_add_vdev_to_pad(imxmd, vdev,
+							link->source);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * For every subdevice, allocate an array of list_head's, one list_head
+ * for each pad, to hold the list of video devices reachable from that
+ * pad.
+ */
+static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd)
+{
+	struct list_head *vdev_lists;
+	struct media_entity *entity;
+	struct v4l2_subdev *sd;
+	int i;
+
+	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
+		entity = &sd->entity;
+		vdev_lists = devm_kcalloc(imxmd->md.dev,
+					  entity->num_pads, sizeof(*vdev_lists),
+					  GFP_KERNEL);
+		if (!vdev_lists)
+			return -ENOMEM;
+
+		/* attach to the subdev's host private pointer */
+		sd->host_priv = vdev_lists;
+
+		for (i = 0; i < entity->num_pads; i++)
+			INIT_LIST_HEAD(to_pad_vdev_list(sd, i));
+	}
+
+	return 0;
+}
+
+/* form the vdev lists in all imx-media source pads */
+static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
+{
+	struct imx_media_video_dev *vdev;
+	struct media_link *link;
+	int ret;
+
+	ret = imx_media_alloc_pad_vdev_lists(imxmd);
+	if (ret)
+		return ret;
+
+	list_for_each_entry(vdev, &imxmd->vdev_list, list) {
+		link = list_first_entry(&vdev->vfd->entity.links,
+					struct media_link, list);
+		ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* async subdev complete notifier */
+int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+	int ret;
+
+	mutex_lock(&imxmd->mutex);
+
+	imx_media_create_csi2_links(imxmd);
+
+	ret = imx_media_create_pad_vdev_lists(imxmd);
+	if (ret)
+		goto unlock;
+
+	ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
+unlock:
+	mutex_unlock(&imxmd->mutex);
+	if (ret)
+		return ret;
+
+	return media_device_register(&imxmd->md);
+}
+EXPORT_SYMBOL_GPL(imx_media_probe_complete);
+
+/*
+ * adds controls to a video device from an entity subdevice.
+ * Continues upstream from the entity's sink pads.
+ */
+static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
+				      struct video_device *vfd,
+				      struct media_entity *entity)
+{
+	int i, ret = 0;
+
+	if (is_media_entity_v4l2_subdev(entity)) {
+		struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+
+		dev_dbg(imxmd->md.dev,
+			"adding controls to %s from %s\n",
+			vfd->entity.name, sd->entity.name);
+
+		ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
+					    sd->ctrl_handler,
+					    NULL, true);
+		if (ret)
+			return ret;
+	}
+
+	/* move upstream */
+	for (i = 0; i < entity->num_pads; i++) {
+		struct media_pad *pad, *spad = &entity->pads[i];
+
+		if (!(spad->flags & MEDIA_PAD_FL_SINK))
+			continue;
+
+		pad = media_pad_remote_pad_first(spad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			continue;
+
+		ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int imx_media_link_notify(struct media_link *link, u32 flags,
+				 unsigned int notification)
+{
+	struct imx_media_dev *imxmd = container_of(link->graph_obj.mdev,
+						   struct imx_media_dev, md);
+	struct media_entity *source = link->source->entity;
+	struct imx_media_pad_vdev *pad_vdev;
+	struct list_head *pad_vdev_list;
+	struct video_device *vfd;
+	struct v4l2_subdev *sd;
+	int pad_idx, ret;
+
+	ret = v4l2_pipeline_link_notify(link, flags, notification);
+	if (ret)
+		return ret;
+
+	/* don't bother if source is not a subdev */
+	if (!is_media_entity_v4l2_subdev(source))
+		return 0;
+
+	sd = media_entity_to_v4l2_subdev(source);
+	pad_idx = link->source->index;
+
+	pad_vdev_list = to_pad_vdev_list(sd, pad_idx);
+	if (!pad_vdev_list) {
+		/* nothing to do if source sd has no pad vdev list */
+		return 0;
+	}
+
+	/*
+	 * Before disabling a link, reset controls for all video
+	 * devices reachable from this link.
+	 *
+	 * After enabling a link, refresh controls for all video
+	 * devices reachable from this link.
+	 */
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
+	    !(flags & MEDIA_LNK_FL_ENABLED)) {
+		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
+			vfd = pad_vdev->vdev->vfd;
+			if (!vfd->ctrl_handler)
+				continue;
+			dev_dbg(imxmd->md.dev,
+				"reset controls for %s\n",
+				vfd->entity.name);
+			v4l2_ctrl_handler_free(vfd->ctrl_handler);
+			v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
+		}
+	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
+		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
+			vfd = pad_vdev->vdev->vfd;
+			if (!vfd->ctrl_handler)
+				continue;
+			dev_dbg(imxmd->md.dev,
+				"refresh controls for %s\n",
+				vfd->entity.name);
+			ret = imx_media_inherit_controls(imxmd, vfd,
+							 &vfd->entity);
+			if (ret)
+				break;
+		}
+	}
+
+	return ret;
+}
+
+static void imx_media_notify(struct v4l2_subdev *sd, unsigned int notification,
+			     void *arg)
+{
+	struct media_entity *entity = &sd->entity;
+	int i;
+
+	if (notification != V4L2_DEVICE_NOTIFY_EVENT)
+		return;
+
+	for (i = 0; i < entity->num_pads; i++) {
+		struct media_pad *pad = &entity->pads[i];
+		struct imx_media_pad_vdev *pad_vdev;
+		struct list_head *pad_vdev_list;
+
+		pad_vdev_list = to_pad_vdev_list(sd, pad->index);
+		if (!pad_vdev_list)
+			continue;
+		list_for_each_entry(pad_vdev, pad_vdev_list, list)
+			v4l2_event_queue(pad_vdev->vdev->vfd, arg);
+	}
+}
+
+static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
+	.bound = imx_media_subdev_bound,
+	.complete = imx_media_probe_complete,
+};
+
+static const struct media_device_ops imx_media_md_ops = {
+	.link_notify = imx_media_link_notify,
+};
+
+struct imx_media_dev *imx_media_dev_init(struct device *dev,
+					 const struct media_device_ops *ops)
+{
+	struct imx_media_dev *imxmd;
+	int ret;
+
+	imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
+	if (!imxmd)
+		return ERR_PTR(-ENOMEM);
+
+	dev_set_drvdata(dev, imxmd);
+
+	strscpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
+	imxmd->md.ops = ops ? ops : &imx_media_md_ops;
+	imxmd->md.dev = dev;
+
+	mutex_init(&imxmd->mutex);
+
+	imxmd->v4l2_dev.mdev = &imxmd->md;
+	imxmd->v4l2_dev.notify = imx_media_notify;
+	strscpy(imxmd->v4l2_dev.name, "imx-media",
+		sizeof(imxmd->v4l2_dev.name));
+	snprintf(imxmd->md.bus_info, sizeof(imxmd->md.bus_info),
+		 "platform:%s", dev_name(imxmd->md.dev));
+
+	media_device_init(&imxmd->md);
+
+	ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "Failed to register v4l2_device: %d\n", ret);
+		goto cleanup;
+	}
+
+	INIT_LIST_HEAD(&imxmd->vdev_list);
+
+	v4l2_async_nf_init(&imxmd->notifier);
+
+	return imxmd;
+
+cleanup:
+	media_device_cleanup(&imxmd->md);
+
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(imx_media_dev_init);
+
+int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
+			    const struct v4l2_async_notifier_operations *ops)
+{
+	int ret;
+
+	/* no subdevs? just bail */
+	if (list_empty(&imxmd->notifier.asd_list)) {
+		v4l2_err(&imxmd->v4l2_dev, "no subdevs\n");
+		return -ENODEV;
+	}
+
+	/* prepare the async subdev notifier and register it */
+	imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops;
+	ret = v4l2_async_nf_register(&imxmd->v4l2_dev, &imxmd->notifier);
+	if (ret) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "v4l2_async_nf_register failed with %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);
diff --git a/drivers/staging/media/imx/imx-media-dev.c b/drivers/staging/media/imx/imx-media-dev.c
new file mode 100644
index 000000000..f85462214
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-dev.c
@@ -0,0 +1,145 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016-2019 Mentor Graphics Inc.
+ */
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-event.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct imx_media_dev, notifier);
+}
+
+/* async subdev bound notifier */
+static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
+				  struct v4l2_subdev *sd,
+				  struct v4l2_async_subdev *asd)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+	int ret;
+
+	if (sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) {
+		/* register the IPU internal subdevs */
+		ret = imx_media_register_ipu_internal_subdevs(imxmd, sd);
+		if (ret)
+			return ret;
+	}
+
+	dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name);
+
+	return 0;
+}
+
+/* async subdev complete notifier */
+static int imx6_media_probe_complete(struct v4l2_async_notifier *notifier)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+	int ret;
+
+	/* call the imx5/6/7 common probe completion handler */
+	ret = imx_media_probe_complete(notifier);
+	if (ret)
+		return ret;
+
+	mutex_lock(&imxmd->mutex);
+
+	imxmd->m2m_vdev = imx_media_csc_scaler_device_init(imxmd);
+	if (IS_ERR(imxmd->m2m_vdev)) {
+		ret = PTR_ERR(imxmd->m2m_vdev);
+		imxmd->m2m_vdev = NULL;
+		goto unlock;
+	}
+
+	ret = imx_media_csc_scaler_device_register(imxmd->m2m_vdev);
+unlock:
+	mutex_unlock(&imxmd->mutex);
+	return ret;
+}
+
+/* async subdev complete notifier */
+static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
+	.bound = imx_media_subdev_bound,
+	.complete = imx6_media_probe_complete,
+};
+
+static int imx_media_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct imx_media_dev *imxmd;
+	int ret;
+
+	imxmd = imx_media_dev_init(dev, NULL);
+	if (IS_ERR(imxmd))
+		return PTR_ERR(imxmd);
+
+	ret = imx_media_add_of_subdevs(imxmd, node);
+	if (ret) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "add_of_subdevs failed with %d\n", ret);
+		goto cleanup;
+	}
+
+	ret = imx_media_dev_notifier_register(imxmd, &imx_media_notifier_ops);
+	if (ret)
+		goto cleanup;
+
+	return 0;
+
+cleanup:
+	v4l2_async_nf_cleanup(&imxmd->notifier);
+	v4l2_device_unregister(&imxmd->v4l2_dev);
+	media_device_cleanup(&imxmd->md);
+
+	return ret;
+}
+
+static int imx_media_remove(struct platform_device *pdev)
+{
+	struct imx_media_dev *imxmd =
+		(struct imx_media_dev *)platform_get_drvdata(pdev);
+
+	v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n");
+
+	if (imxmd->m2m_vdev) {
+		imx_media_csc_scaler_device_unregister(imxmd->m2m_vdev);
+		imxmd->m2m_vdev = NULL;
+	}
+
+	v4l2_async_nf_unregister(&imxmd->notifier);
+	imx_media_unregister_ipu_internal_subdevs(imxmd);
+	v4l2_async_nf_cleanup(&imxmd->notifier);
+	media_device_unregister(&imxmd->md);
+	v4l2_device_unregister(&imxmd->v4l2_dev);
+	media_device_cleanup(&imxmd->md);
+
+	return 0;
+}
+
+static const struct of_device_id imx_media_dt_ids[] = {
+	{ .compatible = "fsl,imx-capture-subsystem" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx_media_dt_ids);
+
+static struct platform_driver imx_media_pdrv = {
+	.probe		= imx_media_probe,
+	.remove		= imx_media_remove,
+	.driver		= {
+		.name	= "imx-media",
+		.of_match_table	= imx_media_dt_ids,
+	},
+};
+
+module_platform_driver(imx_media_pdrv);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-fim.c b/drivers/staging/media/imx/imx-media-fim.c
new file mode 100644
index 000000000..3a9182933
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-fim.c
@@ -0,0 +1,485 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Frame Interval Monitor.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#include <linux/delay.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+enum {
+	FIM_CL_ENABLE = 0,
+	FIM_CL_NUM,
+	FIM_CL_TOLERANCE_MIN,
+	FIM_CL_TOLERANCE_MAX,
+	FIM_CL_NUM_SKIP,
+	FIM_NUM_CONTROLS,
+};
+
+enum {
+	FIM_CL_ICAP_EDGE = 0,
+	FIM_CL_ICAP_CHANNEL,
+	FIM_NUM_ICAP_CONTROLS,
+};
+
+#define FIM_CL_ENABLE_DEF          0 /* FIM disabled by default */
+#define FIM_CL_NUM_DEF             8 /* average 8 frames */
+#define FIM_CL_NUM_SKIP_DEF        2 /* skip 2 frames after restart */
+#define FIM_CL_TOLERANCE_MIN_DEF  50 /* usec */
+#define FIM_CL_TOLERANCE_MAX_DEF   0 /* no max tolerance (unbounded) */
+
+struct imx_media_fim {
+	/* the owning subdev of this fim instance */
+	struct v4l2_subdev *sd;
+
+	/* FIM's control handler */
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* control clusters */
+	struct v4l2_ctrl  *ctrl[FIM_NUM_CONTROLS];
+	struct v4l2_ctrl  *icap_ctrl[FIM_NUM_ICAP_CONTROLS];
+
+	spinlock_t        lock; /* protect control values */
+
+	/* current control values */
+	bool              enabled;
+	int               num_avg;
+	int               num_skip;
+	unsigned long     tolerance_min; /* usec */
+	unsigned long     tolerance_max; /* usec */
+	/* input capture method of measuring FI */
+	int               icap_channel;
+	int               icap_flags;
+
+	int               counter;
+	ktime_t		  last_ts;
+	unsigned long     sum;       /* usec */
+	unsigned long     nominal;   /* usec */
+
+	struct completion icap_first_event;
+	bool              stream_on;
+};
+
+#define icap_enabled(fim) ((fim)->icap_flags != IRQ_TYPE_NONE)
+
+static void update_fim_nominal(struct imx_media_fim *fim,
+			       const struct v4l2_fract *fi)
+{
+	if (fi->denominator == 0) {
+		dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n");
+		fim->enabled = false;
+		return;
+	}
+
+	fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator,
+					     fi->denominator);
+
+	dev_dbg(fim->sd->dev, "FI=%lu usec\n", fim->nominal);
+}
+
+static void reset_fim(struct imx_media_fim *fim, bool curval)
+{
+	struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL];
+	struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE];
+	struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE];
+	struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM];
+	struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP];
+	struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN];
+	struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX];
+
+	if (curval) {
+		fim->enabled = en->cur.val;
+		fim->icap_flags = icap_edge->cur.val;
+		fim->icap_channel = icap_chan->cur.val;
+		fim->num_avg = num->cur.val;
+		fim->num_skip = skip->cur.val;
+		fim->tolerance_min = tol_min->cur.val;
+		fim->tolerance_max = tol_max->cur.val;
+	} else {
+		fim->enabled = en->val;
+		fim->icap_flags = icap_edge->val;
+		fim->icap_channel = icap_chan->val;
+		fim->num_avg = num->val;
+		fim->num_skip = skip->val;
+		fim->tolerance_min = tol_min->val;
+		fim->tolerance_max = tol_max->val;
+	}
+
+	/* disable tolerance range if max <= min */
+	if (fim->tolerance_max <= fim->tolerance_min)
+		fim->tolerance_max = 0;
+
+	/* num_skip must be >= 1 if input capture not used */
+	if (!icap_enabled(fim))
+		fim->num_skip = max_t(int, fim->num_skip, 1);
+
+	fim->counter = -fim->num_skip;
+	fim->sum = 0;
+}
+
+static void send_fim_event(struct imx_media_fim *fim, unsigned long error)
+{
+	static const struct v4l2_event ev = {
+		.type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR,
+	};
+
+	v4l2_subdev_notify_event(fim->sd, &ev);
+}
+
+/*
+ * Monitor an averaged frame interval. If the average deviates too much
+ * from the nominal frame rate, send the frame interval error event. The
+ * frame intervals are averaged in order to quiet noise from
+ * (presumably random) interrupt latency.
+ */
+static void frame_interval_monitor(struct imx_media_fim *fim,
+				   ktime_t timestamp)
+{
+	long long interval, error;
+	unsigned long error_avg;
+	bool send_event = false;
+
+	if (!fim->enabled || ++fim->counter <= 0)
+		goto out_update_ts;
+
+	/* max error is less than l00µs, so use 32-bit division or fail */
+	interval = ktime_to_ns(ktime_sub(timestamp, fim->last_ts));
+	error = abs(interval - NSEC_PER_USEC * (u64)fim->nominal);
+	if (error > U32_MAX)
+		error = U32_MAX;
+	else
+		error = abs((u32)error / NSEC_PER_USEC);
+
+	if (fim->tolerance_max && error >= fim->tolerance_max) {
+		dev_dbg(fim->sd->dev,
+			"FIM: %llu ignored, out of tolerance bounds\n",
+			error);
+		fim->counter--;
+		goto out_update_ts;
+	}
+
+	fim->sum += error;
+
+	if (fim->counter == fim->num_avg) {
+		error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg);
+
+		if (error_avg > fim->tolerance_min)
+			send_event = true;
+
+		dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n",
+			error_avg, send_event ? " (!!!)" : "");
+
+		fim->counter = 0;
+		fim->sum = 0;
+	}
+
+out_update_ts:
+	fim->last_ts = timestamp;
+	if (send_event)
+		send_fim_event(fim, error_avg);
+}
+
+#ifdef CONFIG_IMX_GPT_ICAP
+/*
+ * Input Capture method of measuring frame intervals. Not subject
+ * to interrupt latency.
+ */
+static void fim_input_capture_handler(int channel, void *dev_id,
+				      ktime_t timestamp)
+{
+	struct imx_media_fim *fim = dev_id;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	frame_interval_monitor(fim, timestamp);
+
+	if (!completion_done(&fim->icap_first_event))
+		complete(&fim->icap_first_event);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+}
+
+static int fim_request_input_capture(struct imx_media_fim *fim)
+{
+	init_completion(&fim->icap_first_event);
+
+	return mxc_request_input_capture(fim->icap_channel,
+					 fim_input_capture_handler,
+					 fim->icap_flags, fim);
+}
+
+static void fim_free_input_capture(struct imx_media_fim *fim)
+{
+	mxc_free_input_capture(fim->icap_channel, fim);
+}
+
+#else /* CONFIG_IMX_GPT_ICAP */
+
+static int fim_request_input_capture(struct imx_media_fim *fim)
+{
+	return 0;
+}
+
+static void fim_free_input_capture(struct imx_media_fim *fim)
+{
+}
+
+#endif /* CONFIG_IMX_GPT_ICAP */
+
+/*
+ * In case we are monitoring the first frame interval after streamon
+ * (when fim->num_skip = 0), we need a valid fim->last_ts before we
+ * can begin. This only applies to the input capture method. It is not
+ * possible to accurately measure the first FI after streamon using the
+ * EOF method, so fim->num_skip minimum is set to 1 in that case, so this
+ * function is a noop when the EOF method is used.
+ */
+static void fim_acquire_first_ts(struct imx_media_fim *fim)
+{
+	unsigned long ret;
+
+	if (!fim->enabled || fim->num_skip > 0)
+		return;
+
+	ret = wait_for_completion_timeout(
+		&fim->icap_first_event,
+		msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(fim->sd, "wait first icap event timeout\n");
+}
+
+/* FIM Controls */
+static int fim_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx_media_fim *fim = container_of(ctrl->handler,
+						 struct imx_media_fim,
+						 ctrl_handler);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	switch (ctrl->id) {
+	case V4L2_CID_IMX_FIM_ENABLE:
+		break;
+	case V4L2_CID_IMX_FIM_ICAP_EDGE:
+		if (fim->stream_on)
+			ret = -EBUSY;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	if (!ret)
+		reset_fim(fim, false);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops fim_ctrl_ops = {
+	.s_ctrl = fim_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config fim_ctrl[] = {
+	[FIM_CL_ENABLE] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ENABLE,
+		.name = "FIM Enable",
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.def = FIM_CL_ENABLE_DEF,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+	},
+	[FIM_CL_NUM] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_NUM,
+		.name = "FIM Num Average",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_NUM_DEF,
+		.min =  1, /* no averaging */
+		.max = 64, /* average 64 frames */
+		.step = 1,
+	},
+	[FIM_CL_TOLERANCE_MIN] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_TOLERANCE_MIN,
+		.name = "FIM Tolerance Min",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_TOLERANCE_MIN_DEF,
+		.min =    2,
+		.max =  200,
+		.step =   1,
+	},
+	[FIM_CL_TOLERANCE_MAX] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_TOLERANCE_MAX,
+		.name = "FIM Tolerance Max",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_TOLERANCE_MAX_DEF,
+		.min =    0,
+		.max =  500,
+		.step =   1,
+	},
+	[FIM_CL_NUM_SKIP] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_NUM_SKIP,
+		.name = "FIM Num Skip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_NUM_SKIP_DEF,
+		.min =   0, /* skip no frames */
+		.max = 256, /* skip 256 frames */
+		.step =  1,
+	},
+};
+
+static const struct v4l2_ctrl_config fim_icap_ctrl[] = {
+	[FIM_CL_ICAP_EDGE] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ICAP_EDGE,
+		.name = "FIM Input Capture Edge",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def =  IRQ_TYPE_NONE, /* input capture disabled by default */
+		.min =  IRQ_TYPE_NONE,
+		.max =  IRQ_TYPE_EDGE_BOTH,
+		.step = 1,
+	},
+	[FIM_CL_ICAP_CHANNEL] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ICAP_CHANNEL,
+		.name = "FIM Input Capture Channel",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def =  0,
+		.min =  0,
+		.max =  1,
+		.step = 1,
+	},
+};
+
+static int init_fim_controls(struct imx_media_fim *fim)
+{
+	struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler;
+	int i, ret;
+
+	v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS);
+
+	for (i = 0; i < FIM_NUM_CONTROLS; i++)
+		fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr,
+						    &fim_ctrl[i],
+						    NULL);
+	for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++)
+		fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr,
+							 &fim_icap_ctrl[i],
+							 NULL);
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto err_free;
+	}
+
+	v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl);
+	v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl);
+
+	return 0;
+err_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+/*
+ * Monitor frame intervals via EOF interrupt. This method is
+ * subject to uncertainty errors introduced by interrupt latency.
+ *
+ * This is a noop if the Input Capture method is being used, since
+ * the frame_interval_monitor() is called by the input capture event
+ * callback handler in that case.
+ */
+void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	if (!icap_enabled(fim))
+		frame_interval_monitor(fim, timestamp);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+}
+
+/* Called by the subdev in its s_stream callback */
+int imx_media_fim_set_stream(struct imx_media_fim *fim,
+			     const struct v4l2_fract *fi,
+			     bool on)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]);
+
+	if (fim->stream_on == on)
+		goto out;
+
+	if (on) {
+		spin_lock_irqsave(&fim->lock, flags);
+		reset_fim(fim, true);
+		update_fim_nominal(fim, fi);
+		spin_unlock_irqrestore(&fim->lock, flags);
+
+		if (icap_enabled(fim)) {
+			ret = fim_request_input_capture(fim);
+			if (ret)
+				goto out;
+			fim_acquire_first_ts(fim);
+		}
+	} else {
+		if (icap_enabled(fim))
+			fim_free_input_capture(fim);
+	}
+
+	fim->stream_on = on;
+out:
+	v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
+	return ret;
+}
+
+int imx_media_fim_add_controls(struct imx_media_fim *fim)
+{
+	/* add the FIM controls to the calling subdev ctrl handler */
+	return v4l2_ctrl_add_handler(fim->sd->ctrl_handler,
+				     &fim->ctrl_handler, NULL, false);
+}
+
+/* Called by the subdev in its subdev registered callback */
+struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd)
+{
+	struct imx_media_fim *fim;
+	int ret;
+
+	fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL);
+	if (!fim)
+		return ERR_PTR(-ENOMEM);
+
+	fim->sd = sd;
+
+	spin_lock_init(&fim->lock);
+
+	ret = init_fim_controls(fim);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return fim;
+}
+
+void imx_media_fim_free(struct imx_media_fim *fim)
+{
+	v4l2_ctrl_handler_free(&fim->ctrl_handler);
+}
diff --git a/drivers/staging/media/imx/imx-media-internal-sd.c b/drivers/staging/media/imx/imx-media-internal-sd.c
new file mode 100644
index 000000000..da4109b2f
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-internal-sd.c
@@ -0,0 +1,306 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Media driver for Freescale i.MX5/6 SOC
+ *
+ * Adds the IPU internal subdevices and the media links between them.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#include <linux/platform_device.h>
+#include "imx-media.h"
+
+/* max pads per internal-sd */
+#define MAX_INTERNAL_PADS   8
+/* max links per internal-sd pad */
+#define MAX_INTERNAL_LINKS  8
+
+struct internal_subdev;
+
+struct internal_link {
+	int remote;
+	int local_pad;
+	int remote_pad;
+};
+
+struct internal_pad {
+	int num_links;
+	struct internal_link link[MAX_INTERNAL_LINKS];
+};
+
+struct internal_subdev {
+	u32 grp_id;
+	struct internal_pad pad[MAX_INTERNAL_PADS];
+
+	struct v4l2_subdev * (*sync_register)(struct v4l2_device *v4l2_dev,
+					      struct device *ipu_dev,
+					      struct ipu_soc *ipu,
+					      u32 grp_id);
+	int (*sync_unregister)(struct v4l2_subdev *sd);
+};
+
+static const struct internal_subdev int_subdev[NUM_IPU_SUBDEVS] = {
+	[IPU_CSI0] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_CSI0,
+		.pad[CSI_SRC_PAD_DIRECT] = {
+			.num_links = 2,
+			.link = {
+				{
+					.local_pad = CSI_SRC_PAD_DIRECT,
+					.remote = IPU_IC_PRP,
+					.remote_pad = PRP_SINK_PAD,
+				}, {
+					.local_pad = CSI_SRC_PAD_DIRECT,
+					.remote = IPU_VDIC,
+					.remote_pad = VDIC_SINK_PAD_DIRECT,
+				},
+			},
+		},
+	},
+
+	[IPU_CSI1] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_CSI1,
+		.pad[CSI_SRC_PAD_DIRECT] = {
+			.num_links = 2,
+			.link = {
+				{
+					.local_pad = CSI_SRC_PAD_DIRECT,
+					.remote = IPU_IC_PRP,
+					.remote_pad = PRP_SINK_PAD,
+				}, {
+					.local_pad = CSI_SRC_PAD_DIRECT,
+					.remote = IPU_VDIC,
+					.remote_pad = VDIC_SINK_PAD_DIRECT,
+				},
+			},
+		},
+	},
+
+	[IPU_VDIC] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_VDIC,
+		.sync_register = imx_media_vdic_register,
+		.sync_unregister = imx_media_vdic_unregister,
+		.pad[VDIC_SRC_PAD_DIRECT] = {
+			.num_links = 1,
+			.link = {
+				{
+					.local_pad = VDIC_SRC_PAD_DIRECT,
+					.remote = IPU_IC_PRP,
+					.remote_pad = PRP_SINK_PAD,
+				},
+			},
+		},
+	},
+
+	[IPU_IC_PRP] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_IC_PRP,
+		.sync_register = imx_media_ic_register,
+		.sync_unregister = imx_media_ic_unregister,
+		.pad[PRP_SRC_PAD_PRPENC] = {
+			.num_links = 1,
+			.link = {
+				{
+					.local_pad = PRP_SRC_PAD_PRPENC,
+					.remote = IPU_IC_PRPENC,
+					.remote_pad = PRPENCVF_SINK_PAD,
+				},
+			},
+		},
+		.pad[PRP_SRC_PAD_PRPVF] = {
+			.num_links = 1,
+			.link = {
+				{
+					.local_pad = PRP_SRC_PAD_PRPVF,
+					.remote = IPU_IC_PRPVF,
+					.remote_pad = PRPENCVF_SINK_PAD,
+				},
+			},
+		},
+	},
+
+	[IPU_IC_PRPENC] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_IC_PRPENC,
+		.sync_register = imx_media_ic_register,
+		.sync_unregister = imx_media_ic_unregister,
+	},
+
+	[IPU_IC_PRPVF] = {
+		.grp_id = IMX_MEDIA_GRP_ID_IPU_IC_PRPVF,
+		.sync_register = imx_media_ic_register,
+		.sync_unregister = imx_media_ic_unregister,
+	},
+};
+
+static int create_internal_link(struct imx_media_dev *imxmd,
+				struct v4l2_subdev *src,
+				struct v4l2_subdev *sink,
+				const struct internal_link *link)
+{
+	int ret;
+
+	/* skip if this link already created */
+	if (media_entity_find_link(&src->entity.pads[link->local_pad],
+				   &sink->entity.pads[link->remote_pad]))
+		return 0;
+
+	dev_dbg(imxmd->md.dev, "%s:%d -> %s:%d\n",
+		src->name, link->local_pad,
+		sink->name, link->remote_pad);
+
+	ret = media_create_pad_link(&src->entity, link->local_pad,
+				    &sink->entity, link->remote_pad, 0);
+	if (ret)
+		v4l2_err(&imxmd->v4l2_dev, "%s failed: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int create_ipu_internal_links(struct imx_media_dev *imxmd,
+				     const struct internal_subdev *intsd,
+				     struct v4l2_subdev *sd,
+				     int ipu_id)
+{
+	const struct internal_pad *intpad;
+	const struct internal_link *link;
+	struct media_pad *pad;
+	int i, j, ret;
+
+	/* create the source->sink links */
+	for (i = 0; i < sd->entity.num_pads; i++) {
+		intpad = &intsd->pad[i];
+		pad = &sd->entity.pads[i];
+
+		if (!(pad->flags & MEDIA_PAD_FL_SOURCE))
+			continue;
+
+		for (j = 0; j < intpad->num_links; j++) {
+			struct v4l2_subdev *sink;
+
+			link = &intpad->link[j];
+			sink = imxmd->sync_sd[ipu_id][link->remote];
+
+			ret = create_internal_link(imxmd, sd, sink, link);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int imx_media_register_ipu_internal_subdevs(struct imx_media_dev *imxmd,
+					    struct v4l2_subdev *csi)
+{
+	struct device *ipu_dev = csi->dev->parent;
+	const struct internal_subdev *intsd;
+	struct v4l2_subdev *sd;
+	struct ipu_soc *ipu;
+	int i, ipu_id, ret;
+
+	ipu = dev_get_drvdata(ipu_dev);
+	if (!ipu) {
+		v4l2_err(&imxmd->v4l2_dev, "invalid IPU device!\n");
+		return -ENODEV;
+	}
+
+	ipu_id = ipu_get_num(ipu);
+	if (ipu_id > 1) {
+		v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id);
+		return -ENODEV;
+	}
+
+	mutex_lock(&imxmd->mutex);
+
+	/* record this IPU */
+	if (!imxmd->ipu[ipu_id])
+		imxmd->ipu[ipu_id] = ipu;
+
+	/* register the synchronous subdevs */
+	for (i = 0; i < NUM_IPU_SUBDEVS; i++) {
+		intsd = &int_subdev[i];
+
+		sd = imxmd->sync_sd[ipu_id][i];
+
+		/*
+		 * skip if this sync subdev already registered or its
+		 * not a sync subdev (one of the CSIs)
+		 */
+		if (sd || !intsd->sync_register)
+			continue;
+
+		mutex_unlock(&imxmd->mutex);
+		sd = intsd->sync_register(&imxmd->v4l2_dev, ipu_dev, ipu,
+					  intsd->grp_id);
+		mutex_lock(&imxmd->mutex);
+		if (IS_ERR(sd)) {
+			ret = PTR_ERR(sd);
+			goto err_unwind;
+		}
+
+		imxmd->sync_sd[ipu_id][i] = sd;
+	}
+
+	/*
+	 * all the sync subdevs are registered, create the media links
+	 * between them.
+	 */
+	for (i = 0; i < NUM_IPU_SUBDEVS; i++) {
+		intsd = &int_subdev[i];
+
+		if (intsd->grp_id == csi->grp_id) {
+			sd = csi;
+		} else {
+			sd = imxmd->sync_sd[ipu_id][i];
+			if (!sd)
+				continue;
+		}
+
+		ret = create_ipu_internal_links(imxmd, intsd, sd, ipu_id);
+		if (ret) {
+			mutex_unlock(&imxmd->mutex);
+			imx_media_unregister_ipu_internal_subdevs(imxmd);
+			return ret;
+		}
+	}
+
+	mutex_unlock(&imxmd->mutex);
+	return 0;
+
+err_unwind:
+	while (--i >= 0) {
+		intsd = &int_subdev[i];
+		sd = imxmd->sync_sd[ipu_id][i];
+		if (!sd || !intsd->sync_unregister)
+			continue;
+		mutex_unlock(&imxmd->mutex);
+		intsd->sync_unregister(sd);
+		mutex_lock(&imxmd->mutex);
+	}
+
+	mutex_unlock(&imxmd->mutex);
+	return ret;
+}
+
+void imx_media_unregister_ipu_internal_subdevs(struct imx_media_dev *imxmd)
+{
+	const struct internal_subdev *intsd;
+	struct v4l2_subdev *sd;
+	int i, j;
+
+	mutex_lock(&imxmd->mutex);
+
+	for (i = 0; i < 2; i++) {
+		for (j = 0; j < NUM_IPU_SUBDEVS; j++) {
+			intsd = &int_subdev[j];
+			sd = imxmd->sync_sd[i][j];
+
+			if (!sd || !intsd->sync_unregister)
+				continue;
+
+			mutex_unlock(&imxmd->mutex);
+			intsd->sync_unregister(sd);
+			mutex_lock(&imxmd->mutex);
+		}
+	}
+
+	mutex_unlock(&imxmd->mutex);
+}
diff --git a/drivers/staging/media/imx/imx-media-of.c b/drivers/staging/media/imx/imx-media-of.c
new file mode 100644
index 000000000..59f1eb7b6
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-of.c
@@ -0,0 +1,76 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Media driver for Freescale i.MX5/6 SOC
+ *
+ * Open Firmware parsing.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#include <linux/of_platform.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <linux/of_graph.h>
+#include <video/imx-ipu-v3.h>
+#include "imx-media.h"
+
+int imx_media_of_add_csi(struct imx_media_dev *imxmd,
+			 struct device_node *csi_np)
+{
+	struct v4l2_async_subdev *asd;
+	int ret = 0;
+
+	if (!of_device_is_available(csi_np)) {
+		dev_dbg(imxmd->md.dev, "%s: %pOFn not enabled\n", __func__,
+			csi_np);
+		return -ENODEV;
+	}
+
+	/* add CSI fwnode to async notifier */
+	asd = v4l2_async_nf_add_fwnode(&imxmd->notifier,
+				       of_fwnode_handle(csi_np),
+				       struct v4l2_async_subdev);
+	if (IS_ERR(asd)) {
+		ret = PTR_ERR(asd);
+		if (ret == -EEXIST)
+			dev_dbg(imxmd->md.dev, "%s: already added %pOFn\n",
+				__func__, csi_np);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_of_add_csi);
+
+int imx_media_add_of_subdevs(struct imx_media_dev *imxmd,
+			     struct device_node *np)
+{
+	struct device_node *csi_np;
+	int i, ret;
+
+	for (i = 0; ; i++) {
+		csi_np = of_parse_phandle(np, "ports", i);
+		if (!csi_np)
+			break;
+
+		ret = imx_media_of_add_csi(imxmd, csi_np);
+		if (ret) {
+			/* unavailable or already added is not an error */
+			if (ret == -ENODEV || ret == -EEXIST) {
+				of_node_put(csi_np);
+				continue;
+			}
+
+			/* other error, can't continue */
+			goto err_out;
+		}
+	}
+
+	return 0;
+
+err_out:
+	of_node_put(csi_np);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_add_of_subdevs);
diff --git a/drivers/staging/media/imx/imx-media-utils.c b/drivers/staging/media/imx/imx-media-utils.c
new file mode 100644
index 000000000..4985f21b4
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-utils.c
@@ -0,0 +1,886 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#include <linux/module.h>
+#include "imx-media.h"
+
+#define IMX_BUS_FMTS(fmt...) (const u32[]) {fmt, 0}
+
+/*
+ * List of supported pixel formats for the subdevs.
+ */
+static const struct imx_media_pixfmt pixel_formats[] = {
+	/*** YUV formats start here ***/
+	{
+		.fourcc	= V4L2_PIX_FMT_UYVY,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_UYVY8_2X8,
+			MEDIA_BUS_FMT_UYVY8_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_YUYV8_2X8,
+			MEDIA_BUS_FMT_YUYV8_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_YUV420,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YVU420,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV12,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV16,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUV32,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_AYUV8_1X32),
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 32,
+		.ipufmt = true,
+	},
+	/*** RGB formats start here ***/
+	{
+		.fourcc	= V4L2_PIX_FMT_RGB565,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_RGB565_2X8_LE),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.cycles = 2,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB24,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_RGB888_1X24,
+			MEDIA_BUS_FMT_RGB888_2X12_LE
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 24,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_BGR24,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 24,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_XRGB32,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_ARGB8888_1X32),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_XRGB32,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_ARGB8888_1X32),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+		.ipufmt = true,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_XBGR32,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_BGRX32,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGBX32,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+	},
+	/*** raw bayer and grayscale formats start here ***/
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR8_1X8),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG8_1X8),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG8_1X8),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB8_1X8),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR16,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_SBGGR10_1X10,
+			MEDIA_BUS_FMT_SBGGR12_1X12,
+			MEDIA_BUS_FMT_SBGGR14_1X14,
+			MEDIA_BUS_FMT_SBGGR16_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG16,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_SGBRG10_1X10,
+			MEDIA_BUS_FMT_SGBRG12_1X12,
+			MEDIA_BUS_FMT_SGBRG14_1X14,
+			MEDIA_BUS_FMT_SGBRG16_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG16,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_SGRBG10_1X10,
+			MEDIA_BUS_FMT_SGRBG12_1X12,
+			MEDIA_BUS_FMT_SGRBG14_1X14,
+			MEDIA_BUS_FMT_SGRBG16_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB16,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_SRGGB10_1X10,
+			MEDIA_BUS_FMT_SRGGB12_1X12,
+			MEDIA_BUS_FMT_SRGGB14_1X14,
+			MEDIA_BUS_FMT_SRGGB16_1X16
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.codes = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_Y8_1X8,
+			MEDIA_BUS_FMT_Y10_1X10,
+			MEDIA_BUS_FMT_Y12_1X12
+		),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y10,
+		.codes = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y10_1X10),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y12,
+		.codes = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y12_1X12),
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	},
+};
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given fourcc
+ * that matches the requested selection criteria and return it.
+ *
+ * @fourcc: Search for an entry with the given fourcc pixel format.
+ * @fmt_sel: Allow entries only with the given selection criteria.
+ */
+const struct imx_media_pixfmt *
+imx_media_find_pixel_format(u32 fourcc, enum imx_pixfmt_sel fmt_sel)
+{
+	bool sel_ipu = fmt_sel & PIXFMT_SEL_IPU;
+	unsigned int i;
+
+	fmt_sel &= ~PIXFMT_SEL_IPU;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx_media_pixfmt *fmt = &pixel_formats[i];
+		enum imx_pixfmt_sel sel;
+
+		if (sel_ipu != fmt->ipufmt)
+			continue;
+
+		sel = fmt->bayer ? PIXFMT_SEL_BAYER :
+			((fmt->cs == IPUV3_COLORSPACE_YUV) ?
+			 PIXFMT_SEL_YUV : PIXFMT_SEL_RGB);
+
+		if ((fmt_sel & sel) && fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_pixel_format);
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given media
+ * bus code that matches the requested selection criteria and return it.
+ *
+ * @code: Search for an entry with the given media-bus code.
+ * @fmt_sel: Allow entries only with the given selection criteria.
+ */
+const struct imx_media_pixfmt *
+imx_media_find_mbus_format(u32 code, enum imx_pixfmt_sel fmt_sel)
+{
+	bool sel_ipu = fmt_sel & PIXFMT_SEL_IPU;
+	unsigned int i;
+
+	fmt_sel &= ~PIXFMT_SEL_IPU;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx_media_pixfmt *fmt = &pixel_formats[i];
+		enum imx_pixfmt_sel sel;
+		unsigned int j;
+
+		if (sel_ipu != fmt->ipufmt)
+			continue;
+
+		sel = fmt->bayer ? PIXFMT_SEL_BAYER :
+			((fmt->cs == IPUV3_COLORSPACE_YUV) ?
+			 PIXFMT_SEL_YUV : PIXFMT_SEL_RGB);
+
+		if (!(fmt_sel & sel) || !fmt->codes)
+			continue;
+
+		for (j = 0; fmt->codes[j]; j++) {
+			if (code == fmt->codes[j])
+				return fmt;
+		}
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_mbus_format);
+
+/*
+ * Enumerate entries in the pixel_formats[] array that match the
+ * requested selection criteria. Return the fourcc that matches the
+ * selection criteria at the requested match index.
+ *
+ * @fourcc: The returned fourcc that matches the search criteria at
+ *          the requested match index.
+ * @index: The requested match index.
+ * @fmt_sel: Include in the enumeration entries with the given selection
+ *           criteria.
+ * @code: If non-zero, only include in the enumeration entries matching this
+ *	media bus code.
+ */
+int imx_media_enum_pixel_formats(u32 *fourcc, u32 index,
+				 enum imx_pixfmt_sel fmt_sel, u32 code)
+{
+	bool sel_ipu = fmt_sel & PIXFMT_SEL_IPU;
+	unsigned int i;
+
+	fmt_sel &= ~PIXFMT_SEL_IPU;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx_media_pixfmt *fmt = &pixel_formats[i];
+		enum imx_pixfmt_sel sel;
+
+		if (sel_ipu != fmt->ipufmt)
+			continue;
+
+		sel = fmt->bayer ? PIXFMT_SEL_BAYER :
+			((fmt->cs == IPUV3_COLORSPACE_YUV) ?
+			 PIXFMT_SEL_YUV : PIXFMT_SEL_RGB);
+
+		if (!(fmt_sel & sel))
+			continue;
+
+		/*
+		 * If a media bus code is specified, only consider formats that
+		 * match it.
+		 */
+		if (code) {
+			unsigned int j;
+
+			if (!fmt->codes)
+				continue;
+
+			for (j = 0; fmt->codes[j]; j++) {
+				if (code == fmt->codes[j])
+					break;
+			}
+
+			if (!fmt->codes[j])
+				continue;
+		}
+
+		if (index == 0) {
+			*fourcc = fmt->fourcc;
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(imx_media_enum_pixel_formats);
+
+/*
+ * Enumerate entries in the pixel_formats[] array that match the
+ * requested search criteria. Return the media-bus code that matches
+ * the search criteria at the requested match index.
+ *
+ * @code: The returned media-bus code that matches the search criteria at
+ *        the requested match index.
+ * @index: The requested match index.
+ * @fmt_sel: Include in the enumeration entries with the given selection
+ *           criteria.
+ */
+int imx_media_enum_mbus_formats(u32 *code, u32 index,
+				enum imx_pixfmt_sel fmt_sel)
+{
+	bool sel_ipu = fmt_sel & PIXFMT_SEL_IPU;
+	unsigned int i;
+
+	fmt_sel &= ~PIXFMT_SEL_IPU;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx_media_pixfmt *fmt = &pixel_formats[i];
+		enum imx_pixfmt_sel sel;
+		unsigned int j;
+
+		if (sel_ipu != fmt->ipufmt)
+			continue;
+
+		sel = fmt->bayer ? PIXFMT_SEL_BAYER :
+			((fmt->cs == IPUV3_COLORSPACE_YUV) ?
+			 PIXFMT_SEL_YUV : PIXFMT_SEL_RGB);
+
+		if (!(fmt_sel & sel) || !fmt->codes)
+			continue;
+
+		for (j = 0; fmt->codes[j]; j++) {
+			if (index == 0) {
+				*code = fmt->codes[j];
+				return 0;
+			}
+
+			index--;
+		}
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(imx_media_enum_mbus_formats);
+
+int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+			    u32 width, u32 height, u32 code, u32 field,
+			    const struct imx_media_pixfmt **cc)
+{
+	const struct imx_media_pixfmt *lcc;
+
+	mbus->width = width;
+	mbus->height = height;
+	mbus->field = field;
+
+	if (code == 0)
+		imx_media_enum_mbus_formats(&code, 0, PIXFMT_SEL_YUV);
+
+	lcc = imx_media_find_mbus_format(code, PIXFMT_SEL_ANY);
+	if (!lcc) {
+		lcc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV_RGB);
+		if (!lcc)
+			return -EINVAL;
+	}
+
+	mbus->code = code;
+
+	mbus->colorspace = V4L2_COLORSPACE_SRGB;
+	mbus->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mbus->colorspace);
+	mbus->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mbus->colorspace);
+	mbus->quantization =
+		V4L2_MAP_QUANTIZATION_DEFAULT(lcc->cs == IPUV3_COLORSPACE_RGB,
+					      mbus->colorspace,
+					      mbus->ycbcr_enc);
+
+	if (cc)
+		*cc = lcc;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_init_mbus_fmt);
+
+/*
+ * Initializes the TRY format to the ACTIVE format on all pads
+ * of a subdev. Can be used as the .init_cfg pad operation.
+ */
+int imx_media_init_cfg(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_mbus_framefmt *mf_try;
+	unsigned int pad;
+	int ret;
+
+	for (pad = 0; pad < sd->entity.num_pads; pad++) {
+		struct v4l2_subdev_format format = {
+			.pad = pad,
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &format);
+		if (ret)
+			continue;
+
+		mf_try = v4l2_subdev_get_try_format(sd, sd_state, pad);
+		*mf_try = format.format;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_init_cfg);
+
+/*
+ * Default the colorspace in tryfmt to SRGB if set to an unsupported
+ * colorspace or not initialized. Then set the remaining colorimetry
+ * parameters based on the colorspace if they are uninitialized.
+ *
+ * tryfmt->code must be set on entry.
+ *
+ * If this format is destined to be routed through the Image Converter,
+ * Y`CbCr encoding must be fixed. The IC supports only BT.601 Y`CbCr
+ * or Rec.709 Y`CbCr encoding.
+ */
+void imx_media_try_colorimetry(struct v4l2_mbus_framefmt *tryfmt,
+			       bool ic_route)
+{
+	const struct imx_media_pixfmt *cc;
+	bool is_rgb = false;
+
+	cc = imx_media_find_mbus_format(tryfmt->code, PIXFMT_SEL_ANY);
+	if (!cc)
+		cc = imx_media_find_ipu_format(tryfmt->code,
+					       PIXFMT_SEL_YUV_RGB);
+
+	if (cc && cc->cs == IPUV3_COLORSPACE_RGB)
+		is_rgb = true;
+
+	switch (tryfmt->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_JPEG:
+	case V4L2_COLORSPACE_SRGB:
+	case V4L2_COLORSPACE_BT2020:
+	case V4L2_COLORSPACE_OPRGB:
+	case V4L2_COLORSPACE_DCI_P3:
+	case V4L2_COLORSPACE_RAW:
+		break;
+	default:
+		tryfmt->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	}
+
+	if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT)
+		tryfmt->xfer_func =
+			V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace);
+
+	if (ic_route) {
+		if (tryfmt->ycbcr_enc != V4L2_YCBCR_ENC_601 &&
+		    tryfmt->ycbcr_enc != V4L2_YCBCR_ENC_709)
+			tryfmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	} else {
+		if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) {
+			tryfmt->ycbcr_enc =
+				V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace);
+		}
+	}
+
+	if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT)
+		tryfmt->quantization =
+			V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb,
+						      tryfmt->colorspace,
+						      tryfmt->ycbcr_enc);
+}
+EXPORT_SYMBOL_GPL(imx_media_try_colorimetry);
+
+int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+				  const struct v4l2_mbus_framefmt *mbus,
+				  const struct imx_media_pixfmt *cc)
+{
+	u32 width;
+	u32 stride;
+
+	if (!cc) {
+		cc = imx_media_find_ipu_format(mbus->code,
+					       PIXFMT_SEL_YUV_RGB);
+		if (!cc)
+			cc = imx_media_find_mbus_format(mbus->code,
+							PIXFMT_SEL_ANY);
+		if (!cc)
+			return -EINVAL;
+	}
+
+	/*
+	 * TODO: the IPU currently does not support the AYUV32 format,
+	 * so until it does convert to a supported YUV format.
+	 */
+	if (cc->ipufmt && cc->cs == IPUV3_COLORSPACE_YUV) {
+		u32 code;
+
+		imx_media_enum_mbus_formats(&code, 0, PIXFMT_SEL_YUV);
+		cc = imx_media_find_mbus_format(code, PIXFMT_SEL_YUV);
+	}
+
+	/* Round up width for minimum burst size */
+	width = round_up(mbus->width, 8);
+
+	/* Round up stride for IDMAC line start address alignment */
+	if (cc->planar)
+		stride = round_up(width, 16);
+	else
+		stride = round_up((width * cc->bpp) >> 3, 8);
+
+	pix->width = width;
+	pix->height = mbus->height;
+	pix->pixelformat = cc->fourcc;
+	pix->colorspace = mbus->colorspace;
+	pix->xfer_func = mbus->xfer_func;
+	pix->ycbcr_enc = mbus->ycbcr_enc;
+	pix->quantization = mbus->quantization;
+	pix->field = mbus->field;
+	pix->bytesperline = stride;
+	pix->sizeimage = cc->planar ? ((stride * pix->height * cc->bpp) >> 3) :
+			 stride * pix->height;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_mbus_fmt_to_pix_fmt);
+
+void imx_media_free_dma_buf(struct device *dev,
+			    struct imx_media_dma_buf *buf)
+{
+	if (buf->virt)
+		dma_free_coherent(dev, buf->len, buf->virt, buf->phys);
+
+	buf->virt = NULL;
+	buf->phys = 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_free_dma_buf);
+
+int imx_media_alloc_dma_buf(struct device *dev,
+			    struct imx_media_dma_buf *buf,
+			    int size)
+{
+	imx_media_free_dma_buf(dev, buf);
+
+	buf->len = PAGE_ALIGN(size);
+	buf->virt = dma_alloc_coherent(dev, buf->len, &buf->phys,
+				       GFP_DMA | GFP_KERNEL);
+	if (!buf->virt) {
+		dev_err(dev, "%s: failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_alloc_dma_buf);
+
+/* form a subdev name given a group id and ipu id */
+void imx_media_grp_id_to_sd_name(char *sd_name, int sz, u32 grp_id, int ipu_id)
+{
+	int id;
+
+	switch (grp_id) {
+	case IMX_MEDIA_GRP_ID_IPU_CSI0...IMX_MEDIA_GRP_ID_IPU_CSI1:
+		id = (grp_id >> IMX_MEDIA_GRP_ID_IPU_CSI_BIT) - 1;
+		snprintf(sd_name, sz, "ipu%d_csi%d", ipu_id + 1, id);
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_VDIC:
+		snprintf(sd_name, sz, "ipu%d_vdic", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRP:
+		snprintf(sd_name, sz, "ipu%d_ic_prp", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRPENC:
+		snprintf(sd_name, sz, "ipu%d_ic_prpenc", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IPU_IC_PRPVF:
+		snprintf(sd_name, sz, "ipu%d_ic_prpvf", ipu_id + 1);
+		break;
+	default:
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(imx_media_grp_id_to_sd_name);
+
+struct v4l2_subdev *
+imx_media_find_subdev_by_fwnode(struct imx_media_dev *imxmd,
+				struct fwnode_handle *fwnode)
+{
+	struct v4l2_subdev *sd;
+
+	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
+		if (sd->fwnode == fwnode)
+			return sd;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_fwnode);
+
+struct v4l2_subdev *
+imx_media_find_subdev_by_devname(struct imx_media_dev *imxmd,
+				 const char *devname)
+{
+	struct v4l2_subdev *sd;
+
+	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
+		if (!strcmp(devname, dev_name(sd->dev)))
+			return sd;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_devname);
+
+/*
+ * Adds a video device to the master video device list. This is called
+ * when a video device is registered.
+ */
+void imx_media_add_video_device(struct imx_media_dev *imxmd,
+				struct imx_media_video_dev *vdev)
+{
+	mutex_lock(&imxmd->mutex);
+
+	list_add_tail(&vdev->list, &imxmd->vdev_list);
+
+	mutex_unlock(&imxmd->mutex);
+}
+EXPORT_SYMBOL_GPL(imx_media_add_video_device);
+
+/*
+ * Search upstream/downstream for a subdevice or video device pad in the
+ * current pipeline, starting from start_entity. Returns the device's
+ * source/sink pad that it was reached from. Must be called with
+ * mdev->graph_mutex held.
+ *
+ * If grp_id != 0, finds a subdevice's pad of given grp_id.
+ * Else If buftype != 0, finds a video device's pad of given buffer type.
+ * Else, returns the nearest source/sink pad to start_entity.
+ */
+struct media_pad *
+imx_media_pipeline_pad(struct media_entity *start_entity, u32 grp_id,
+		       enum v4l2_buf_type buftype, bool upstream)
+{
+	struct media_entity *me = start_entity;
+	struct media_pad *pad = NULL;
+	struct video_device *vfd;
+	struct v4l2_subdev *sd;
+	int i;
+
+	for (i = 0; i < me->num_pads; i++) {
+		struct media_pad *spad = &me->pads[i];
+
+		if ((upstream && !(spad->flags & MEDIA_PAD_FL_SINK)) ||
+		    (!upstream && !(spad->flags & MEDIA_PAD_FL_SOURCE)))
+			continue;
+
+		pad = media_pad_remote_pad_first(spad);
+		if (!pad)
+			continue;
+
+		if (grp_id) {
+			if (is_media_entity_v4l2_subdev(pad->entity)) {
+				sd = media_entity_to_v4l2_subdev(pad->entity);
+				if (sd->grp_id & grp_id)
+					return pad;
+			}
+
+			return imx_media_pipeline_pad(pad->entity, grp_id,
+						      buftype, upstream);
+		} else if (buftype) {
+			if (is_media_entity_v4l2_video_device(pad->entity)) {
+				vfd = media_entity_to_video_device(pad->entity);
+				if (buftype == vfd->queue->type)
+					return pad;
+			}
+
+			return imx_media_pipeline_pad(pad->entity, grp_id,
+						      buftype, upstream);
+		} else {
+			return pad;
+		}
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_pad);
+
+/*
+ * Search upstream/downstream for a subdev or video device in the current
+ * pipeline. Must be called with mdev->graph_mutex held.
+ */
+static struct media_entity *
+find_pipeline_entity(struct media_entity *start, u32 grp_id,
+		     enum v4l2_buf_type buftype, bool upstream)
+{
+	struct media_pad *pad = NULL;
+	struct video_device *vfd;
+	struct v4l2_subdev *sd;
+
+	if (grp_id && is_media_entity_v4l2_subdev(start)) {
+		sd = media_entity_to_v4l2_subdev(start);
+		if (sd->grp_id & grp_id)
+			return &sd->entity;
+	} else if (buftype && is_media_entity_v4l2_video_device(start)) {
+		vfd = media_entity_to_video_device(start);
+		if (buftype == vfd->queue->type)
+			return &vfd->entity;
+	}
+
+	pad = imx_media_pipeline_pad(start, grp_id, buftype, upstream);
+
+	return pad ? pad->entity : NULL;
+}
+
+/*
+ * Find the upstream mipi-csi2 virtual channel reached from the given
+ * start entity in the current pipeline.
+ * Must be called with mdev->graph_mutex held.
+ */
+int imx_media_pipeline_csi2_channel(struct media_entity *start_entity)
+{
+	struct media_pad *pad;
+	int ret = -EPIPE;
+
+	pad = imx_media_pipeline_pad(start_entity, IMX_MEDIA_GRP_ID_CSI2,
+				     0, true);
+	if (pad)
+		ret = pad->index - 1;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_csi2_channel);
+
+/*
+ * Find a subdev reached upstream from the given start entity in
+ * the current pipeline.
+ * Must be called with mdev->graph_mutex held.
+ */
+struct v4l2_subdev *
+imx_media_pipeline_subdev(struct media_entity *start_entity, u32 grp_id,
+			  bool upstream)
+{
+	struct media_entity *me;
+
+	me = find_pipeline_entity(start_entity, grp_id, 0, upstream);
+	if (!me)
+		return ERR_PTR(-ENODEV);
+
+	return media_entity_to_v4l2_subdev(me);
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_subdev);
+
+/*
+ * Find a subdev reached upstream from the given start entity in
+ * the current pipeline.
+ * Must be called with mdev->graph_mutex held.
+ */
+struct video_device *
+imx_media_pipeline_video_device(struct media_entity *start_entity,
+				enum v4l2_buf_type buftype, bool upstream)
+{
+	struct media_entity *me;
+
+	me = find_pipeline_entity(start_entity, 0, buftype, upstream);
+	if (!me)
+		return ERR_PTR(-ENODEV);
+
+	return media_entity_to_video_device(me);
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_video_device);
+
+/*
+ * Find a fwnode endpoint that maps to the given subdevice's pad.
+ * If there are multiple endpoints that map to the pad, only the
+ * first endpoint encountered is returned.
+ *
+ * On success the refcount of the returned fwnode endpoint is
+ * incremented.
+ */
+struct fwnode_handle *imx_media_get_pad_fwnode(struct media_pad *pad)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_subdev *sd;
+
+	if (!is_media_entity_v4l2_subdev(pad->entity))
+		return ERR_PTR(-ENODEV);
+
+	sd = media_entity_to_v4l2_subdev(pad->entity);
+
+	fwnode_graph_for_each_endpoint(dev_fwnode(sd->dev), endpoint) {
+		int pad_idx = media_entity_get_fwnode_pad(&sd->entity,
+							  endpoint,
+							  pad->flags);
+		if (pad_idx < 0)
+			continue;
+
+		if (pad_idx == pad->index)
+			return endpoint;
+	}
+
+	return ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(imx_media_get_pad_fwnode);
+
+/*
+ * Turn current pipeline streaming on/off starting from entity.
+ */
+int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd,
+				  struct media_entity *entity,
+				  bool on)
+{
+	struct v4l2_subdev *sd;
+	int ret = 0;
+
+	if (!is_media_entity_v4l2_subdev(entity))
+		return -EINVAL;
+	sd = media_entity_to_v4l2_subdev(entity);
+
+	mutex_lock(&imxmd->md.graph_mutex);
+
+	if (on) {
+		ret = __media_pipeline_start(entity->pads, &imxmd->pipe);
+		if (ret)
+			goto out;
+		ret = v4l2_subdev_call(sd, video, s_stream, 1);
+		if (ret)
+			__media_pipeline_stop(entity->pads);
+	} else {
+		v4l2_subdev_call(sd, video, s_stream, 0);
+		if (media_pad_pipeline(entity->pads))
+			__media_pipeline_stop(entity->pads);
+	}
+
+out:
+	mutex_unlock(&imxmd->md.graph_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_set_stream);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-vdic.c b/drivers/staging/media/imx/imx-media-vdic.c
new file mode 100644
index 000000000..3c2093c52
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-vdic.c
@@ -0,0 +1,971 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2017 Mentor Graphics Inc.
+ */
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+/*
+ * This subdev implements two different video pipelines:
+ *
+ * CSI -> VDIC
+ *
+ * In this pipeline, the CSI sends a single interlaced field F(n-1)
+ * directly to the VDIC (and optionally the following field F(n)
+ * can be sent to memory via IDMAC channel 13). This pipeline only works
+ * in VDIC's high motion mode, which only requires a single field for
+ * processing. The other motion modes (low and medium) require three
+ * fields, so this pipeline does not work in those modes. Also, it is
+ * not clear how this pipeline can deal with the various field orders
+ * (sequential BT/TB, interlaced BT/TB).
+ *
+ * MEM -> CH8,9,10 -> VDIC
+ *
+ * In this pipeline, previous field F(n-1), current field F(n), and next
+ * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10.
+ * These memory buffers can come from a video output or mem2mem device.
+ * All motion modes are supported by this pipeline.
+ *
+ * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be
+ * used in high motion mode.
+ */
+
+struct vdic_priv;
+
+struct vdic_pipeline_ops {
+	int (*setup)(struct vdic_priv *priv);
+	void (*start)(struct vdic_priv *priv);
+	void (*stop)(struct vdic_priv *priv);
+	void (*disable)(struct vdic_priv *priv);
+};
+
+/*
+ * Min/Max supported width and heights.
+ */
+#define MIN_W        32
+#define MIN_H        32
+#define MAX_W_VDIC  968
+#define MAX_H_VDIC 2048
+#define W_ALIGN    4 /* multiple of 16 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+struct vdic_priv {
+	struct device *ipu_dev;
+	struct ipu_soc *ipu;
+
+	struct v4l2_subdev   sd;
+	struct media_pad pad[VDIC_NUM_PADS];
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	/* IPU units we require */
+	struct ipu_vdi *vdi;
+
+	int active_input_pad;
+
+	struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */
+	struct ipuv3_channel *vdi_in_ch;   /* F(n) transfer channel */
+	struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */
+
+	/* pipeline operations */
+	struct vdic_pipeline_ops *ops;
+
+	/* current and previous input buffers indirect path */
+	struct imx_media_buffer *curr_in_buf;
+	struct imx_media_buffer *prev_in_buf;
+
+	/*
+	 * translated field type, input line stride, and field size
+	 * for indirect path
+	 */
+	u32 fieldtype;
+	u32 in_stride;
+	u32 field_size;
+
+	/* the source (a video device or subdev) */
+	struct media_entity *src;
+	/* the sink that will receive the progressive out buffers */
+	struct v4l2_subdev *sink_sd;
+
+	struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS];
+	const struct imx_media_pixfmt *cc[VDIC_NUM_PADS];
+	struct v4l2_fract frame_interval[VDIC_NUM_PADS];
+
+	/* the video device at IDMAC input pad */
+	struct imx_media_video_dev *vdev;
+
+	bool csi_direct;  /* using direct CSI->VDIC->IC pipeline */
+
+	/* motion select control */
+	struct v4l2_ctrl_handler ctrl_hdlr;
+	enum ipu_motion_sel motion;
+
+	int stream_count;
+};
+
+static void vdic_put_ipu_resources(struct vdic_priv *priv)
+{
+	if (priv->vdi_in_ch_p)
+		ipu_idmac_put(priv->vdi_in_ch_p);
+	priv->vdi_in_ch_p = NULL;
+
+	if (priv->vdi_in_ch)
+		ipu_idmac_put(priv->vdi_in_ch);
+	priv->vdi_in_ch = NULL;
+
+	if (priv->vdi_in_ch_n)
+		ipu_idmac_put(priv->vdi_in_ch_n);
+	priv->vdi_in_ch_n = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->vdi))
+		ipu_vdi_put(priv->vdi);
+	priv->vdi = NULL;
+}
+
+static int vdic_get_ipu_resources(struct vdic_priv *priv)
+{
+	int ret, err_chan;
+	struct ipuv3_channel *ch;
+	struct ipu_vdi *vdi;
+
+	vdi = ipu_vdi_get(priv->ipu);
+	if (IS_ERR(vdi)) {
+		v4l2_err(&priv->sd, "failed to get VDIC\n");
+		ret = PTR_ERR(vdi);
+		goto out;
+	}
+	priv->vdi = vdi;
+
+	if (!priv->csi_direct) {
+		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_PREV);
+		if (IS_ERR(ch)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_PREV;
+			ret = PTR_ERR(ch);
+			goto out_err_chan;
+		}
+		priv->vdi_in_ch_p = ch;
+
+		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_CUR);
+		if (IS_ERR(ch)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_CUR;
+			ret = PTR_ERR(ch);
+			goto out_err_chan;
+		}
+		priv->vdi_in_ch = ch;
+
+		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_NEXT);
+		if (IS_ERR(ch)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT;
+			ret = PTR_ERR(ch);
+			goto out_err_chan;
+		}
+		priv->vdi_in_ch_n = ch;
+	}
+
+	return 0;
+
+out_err_chan:
+	v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
+out:
+	vdic_put_ipu_resources(priv);
+	return ret;
+}
+
+/*
+ * This function is currently unused, but will be called when the
+ * output/mem2mem device at the IDMAC input pad sends us a new
+ * buffer. It kicks off the IDMAC read channels to bring in the
+ * buffer fields from memory and begin the conversions.
+ */
+static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv,
+						  struct imx_media_buffer *curr)
+{
+	dma_addr_t prev_phys, curr_phys, next_phys;
+	struct imx_media_buffer *prev;
+	struct vb2_buffer *curr_vb, *prev_vb;
+	u32 fs = priv->field_size;
+	u32 is = priv->in_stride;
+
+	/* current input buffer is now previous */
+	priv->prev_in_buf = priv->curr_in_buf;
+	priv->curr_in_buf = curr;
+	prev = priv->prev_in_buf ? priv->prev_in_buf : curr;
+
+	prev_vb = &prev->vbuf.vb2_buf;
+	curr_vb = &curr->vbuf.vb2_buf;
+
+	switch (priv->fieldtype) {
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
+		break;
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+	case V4L2_FIELD_INTERLACED:
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
+		break;
+	default:
+		/*
+		 * can't get here, priv->fieldtype can only be one of
+		 * the above. This is to quiet smatch errors.
+		 */
+		return;
+	}
+
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);
+
+	ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
+	ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
+	ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
+}
+
+static int setup_vdi_channel(struct vdic_priv *priv,
+			     struct ipuv3_channel *channel,
+			     dma_addr_t phys0, dma_addr_t phys1)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	unsigned int burst_size;
+	struct ipu_image image;
+	int ret;
+
+	ipu_cpmem_zero(channel);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt;
+	image.rect = vdev->compose;
+	/* one field to VDIC channels */
+	image.pix.height /= 2;
+	image.rect.height /= 2;
+	image.phys0 = phys0;
+	image.phys1 = phys1;
+
+	ret = ipu_cpmem_set_image(channel, &image);
+	if (ret)
+		return ret;
+
+	burst_size = (image.pix.width & 0xf) ? 8 : 16;
+	ipu_cpmem_set_burstsize(channel, burst_size);
+
+	ipu_cpmem_set_axi_id(channel, 1);
+
+	ipu_idmac_set_double_buffer(channel, false);
+
+	return 0;
+}
+
+static int vdic_setup_direct(struct vdic_priv *priv)
+{
+	/* set VDIC to receive from CSI for direct path */
+	ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
+		     IPUV3_CHANNEL_CSI_VDI_PREV);
+
+	return 0;
+}
+
+static void vdic_start_direct(struct vdic_priv *priv)
+{
+}
+
+static void vdic_stop_direct(struct vdic_priv *priv)
+{
+}
+
+static void vdic_disable_direct(struct vdic_priv *priv)
+{
+	ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
+		       IPUV3_CHANNEL_CSI_VDI_PREV);
+}
+
+static int vdic_setup_indirect(struct vdic_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt;
+	const struct imx_media_pixfmt *incc;
+	int in_size, ret;
+
+	infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
+	incc = priv->cc[VDIC_SINK_PAD_IDMAC];
+
+	in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
+
+	/* 1/2 full image size */
+	priv->field_size = in_size / 2;
+	priv->in_stride = incc->planar ?
+		infmt->width : (infmt->width * incc->bpp) >> 3;
+
+	priv->prev_in_buf = NULL;
+	priv->curr_in_buf = NULL;
+
+	priv->fieldtype = infmt->field;
+
+	/* init the vdi-in channels */
+	ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
+	if (ret)
+		return ret;
+	ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
+	if (ret)
+		return ret;
+	return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
+}
+
+static void vdic_start_indirect(struct vdic_priv *priv)
+{
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->vdi_in_ch_p);
+	ipu_idmac_enable_channel(priv->vdi_in_ch);
+	ipu_idmac_enable_channel(priv->vdi_in_ch_n);
+}
+
+static void vdic_stop_indirect(struct vdic_priv *priv)
+{
+	/* disable channels */
+	ipu_idmac_disable_channel(priv->vdi_in_ch_p);
+	ipu_idmac_disable_channel(priv->vdi_in_ch);
+	ipu_idmac_disable_channel(priv->vdi_in_ch_n);
+}
+
+static void vdic_disable_indirect(struct vdic_priv *priv)
+{
+}
+
+static struct vdic_pipeline_ops direct_ops = {
+	.setup = vdic_setup_direct,
+	.start = vdic_start_direct,
+	.stop = vdic_stop_direct,
+	.disable = vdic_disable_direct,
+};
+
+static struct vdic_pipeline_ops indirect_ops = {
+	.setup = vdic_setup_indirect,
+	.start = vdic_start_indirect,
+	.stop = vdic_stop_indirect,
+	.disable = vdic_disable_indirect,
+};
+
+static int vdic_start(struct vdic_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt;
+	int ret;
+
+	infmt = &priv->format_mbus[priv->active_input_pad];
+
+	priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
+
+	ret = vdic_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	/*
+	 * init the VDIC.
+	 *
+	 * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
+	 * only supports 4:2:2 or 4:2:0, and this subdev will only
+	 * negotiate 4:2:2 at its sink pads.
+	 */
+	ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
+		      infmt->width, infmt->height);
+	ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
+	ipu_vdi_set_motion(priv->vdi, priv->motion);
+
+	ret = priv->ops->setup(priv);
+	if (ret)
+		goto out_put_ipu;
+
+	ipu_vdi_enable(priv->vdi);
+
+	priv->ops->start(priv);
+
+	return 0;
+
+out_put_ipu:
+	vdic_put_ipu_resources(priv);
+	return ret;
+}
+
+static void vdic_stop(struct vdic_priv *priv)
+{
+	priv->ops->stop(priv);
+	ipu_vdi_disable(priv->vdi);
+	priv->ops->disable(priv);
+
+	vdic_put_ipu_resources(priv);
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vdic_priv *priv = container_of(ctrl->handler,
+					      struct vdic_priv, ctrl_hdlr);
+	enum ipu_motion_sel motion;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	switch (ctrl->id) {
+	case V4L2_CID_DEINTERLACING_MODE:
+		motion = ctrl->val;
+		if (motion != priv->motion) {
+			/* can't change motion control mid-streaming */
+			if (priv->stream_count > 0) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->motion = motion;
+		}
+		break;
+	default:
+		v4l2_err(&priv->sd, "Invalid control\n");
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
+	.s_ctrl = vdic_s_ctrl,
+};
+
+static const char * const vdic_ctrl_motion_menu[] = {
+	"No Motion Compensation",
+	"Low Motion",
+	"Medium Motion",
+	"High Motion",
+};
+
+static int vdic_init_controls(struct vdic_priv *priv)
+{
+	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdlr, 1);
+
+	v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
+				     V4L2_CID_DEINTERLACING_MODE,
+				     HIGH_MOTION, 0, HIGH_MOTION,
+				     vdic_ctrl_motion_menu);
+
+	priv->sd.ctrl_handler = hdlr;
+
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto out_free;
+	}
+
+	v4l2_ctrl_handler_setup(hdlr);
+	return 0;
+
+out_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *src_sd = NULL;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src || !priv->sink_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	if (priv->csi_direct)
+		src_sd = media_entity_to_v4l2_subdev(priv->src);
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(priv->ipu_dev, "%s: stream %s\n", sd->name,
+		enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = vdic_start(priv);
+	else
+		vdic_stop(priv);
+	if (ret)
+		goto out;
+
+	if (src_sd) {
+		/* start/stop upstream */
+		ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
+		ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+		if (ret) {
+			if (enable)
+				vdic_stop(priv);
+			goto out;
+		}
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_state *sd_state,
+	       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&priv->sd, sd_state, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	return imx_media_enum_ipu_formats(&code->code, code->index,
+					  PIXFMT_SEL_YUV);
+}
+
+static int vdic_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void vdic_try_fmt(struct vdic_priv *priv,
+			 struct v4l2_subdev_state *sd_state,
+			 struct v4l2_subdev_format *sdformat,
+			 const struct imx_media_pixfmt **cc)
+{
+	struct v4l2_mbus_framefmt *infmt;
+
+	*cc = imx_media_find_ipu_format(sdformat->format.code,
+					PIXFMT_SEL_YUV);
+	if (!*cc) {
+		u32 code;
+
+		imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
+		*cc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV);
+		sdformat->format.code = (*cc)->codes[0];
+	}
+
+	infmt = __vdic_get_fmt(priv, sd_state, priv->active_input_pad,
+			       sdformat->which);
+
+	switch (sdformat->pad) {
+	case VDIC_SRC_PAD_DIRECT:
+		sdformat->format = *infmt;
+		/* output is always progressive! */
+		sdformat->format.field = V4L2_FIELD_NONE;
+		break;
+	case VDIC_SINK_PAD_DIRECT:
+	case VDIC_SINK_PAD_IDMAC:
+		v4l_bound_align_image(&sdformat->format.width,
+				      MIN_W, MAX_W_VDIC, W_ALIGN,
+				      &sdformat->format.height,
+				      MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
+
+		/* input must be interlaced! Choose SEQ_TB if not */
+		if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
+			sdformat->format.field = V4L2_FIELD_SEQ_TB;
+		break;
+	}
+
+	imx_media_try_colorimetry(&sdformat->format, true);
+}
+
+static int vdic_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	vdic_try_fmt(priv, sd_state, sdformat, &cc);
+
+	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	/* propagate format to source pad */
+	if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
+	    sdformat->pad == VDIC_SINK_PAD_IDMAC) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_mbus_framefmt *outfmt;
+		struct v4l2_subdev_format format;
+
+		format.pad = VDIC_SRC_PAD_DIRECT;
+		format.which = sdformat->which;
+		format.format = sdformat->format;
+		vdic_try_fmt(priv, sd_state, &format, &outcc);
+
+		outfmt = __vdic_get_fmt(priv, sd_state, VDIC_SRC_PAD_DIRECT,
+					sdformat->which);
+		*outfmt = format.format;
+		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->cc[sdformat->pad] = cc;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s",
+		sd->name, remote->entity->name, local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SOURCE) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->sink_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->sink_sd = remote_sd;
+		} else {
+			priv->sink_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a sink pad */
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->src) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		priv->src = NULL;
+		goto out;
+	}
+
+	if (local->index == VDIC_SINK_PAD_IDMAC) {
+		struct imx_media_video_dev *vdev = priv->vdev;
+
+		if (!is_media_entity_v4l2_video_device(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+		if (!vdev) {
+			ret = -ENODEV;
+			goto out;
+		}
+
+		priv->csi_direct = false;
+	} else {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		/* direct pad must connect to a CSI */
+		if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) ||
+		    remote->index != CSI_SRC_PAD_DIRECT) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		priv->csi_direct = true;
+	}
+
+	priv->src = remote->entity;
+	/* record which input pad is now active */
+	priv->active_input_pad = local->index;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_link_validate(struct v4l2_subdev *sd,
+			      struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->csi_direct && priv->motion != HIGH_MOTION) {
+		v4l2_err(&priv->sd,
+			 "direct CSI pipeline requires high motion\n");
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	if (fi->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fi->interval = priv->frame_interval[fi->pad];
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int vdic_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi, *output_fi;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[priv->active_input_pad];
+	output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
+
+	switch (fi->pad) {
+	case VDIC_SINK_PAD_DIRECT:
+	case VDIC_SINK_PAD_IDMAC:
+		/* No limits on valid input frame intervals */
+		if (fi->interval.numerator == 0 ||
+		    fi->interval.denominator == 0)
+			fi->interval = priv->frame_interval[fi->pad];
+		/* Reset output interval */
+		*output_fi = fi->interval;
+		if (priv->csi_direct)
+			output_fi->denominator *= 2;
+		break;
+	case VDIC_SRC_PAD_DIRECT:
+		/*
+		 * frame rate at output pad is double input
+		 * rate when using direct CSI->VDIC pipeline.
+		 *
+		 * TODO: implement VDIC frame skipping
+		 */
+		fi->interval = *input_fi;
+		if (priv->csi_direct)
+			fi->interval.denominator *= 2;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	priv->frame_interval[fi->pad] = fi->interval;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_registered(struct v4l2_subdev *sd)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	int i, ret;
+	u32 code;
+
+	for (i = 0; i < VDIC_NUM_PADS; i++) {
+		code = 0;
+		if (i != VDIC_SINK_PAD_IDMAC)
+			imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
+
+		/* set a default mbus format  */
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      IMX_MEDIA_DEF_PIX_WIDTH,
+					      IMX_MEDIA_DEF_PIX_HEIGHT, code,
+					      V4L2_FIELD_NONE, &priv->cc[i]);
+		if (ret)
+			return ret;
+
+		/* init default frame interval */
+		priv->frame_interval[i].numerator = 1;
+		priv->frame_interval[i].denominator = 30;
+		if (i == VDIC_SRC_PAD_DIRECT)
+			priv->frame_interval[i].denominator *= 2;
+	}
+
+	priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
+
+	return vdic_init_controls(priv);
+}
+
+static void vdic_unregistered(struct v4l2_subdev *sd)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+
+static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
+	.enum_mbus_code = vdic_enum_mbus_code,
+	.get_fmt = vdic_get_fmt,
+	.set_fmt = vdic_set_fmt,
+	.link_validate = vdic_link_validate,
+};
+
+static const struct v4l2_subdev_video_ops vdic_video_ops = {
+	.g_frame_interval = vdic_g_frame_interval,
+	.s_frame_interval = vdic_s_frame_interval,
+	.s_stream = vdic_s_stream,
+};
+
+static const struct media_entity_operations vdic_entity_ops = {
+	.link_setup = vdic_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops vdic_subdev_ops = {
+	.video = &vdic_video_ops,
+	.pad = &vdic_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
+	.registered = vdic_registered,
+	.unregistered = vdic_unregistered,
+};
+
+struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
+					    struct device *ipu_dev,
+					    struct ipu_soc *ipu,
+					    u32 grp_id)
+{
+	struct vdic_priv *priv;
+	int i, ret;
+
+	priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->ipu_dev = ipu_dev;
+	priv->ipu = ipu;
+
+	v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = &vdic_internal_ops;
+	priv->sd.entity.ops = &vdic_entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	priv->sd.owner = ipu_dev->driver->owner;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	priv->sd.grp_id = grp_id;
+	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
+				    priv->sd.grp_id, ipu_get_num(ipu));
+
+	mutex_init(&priv->lock);
+
+	for (i = 0; i < VDIC_NUM_PADS; i++)
+		priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
+			MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
+
+	ret = media_entity_pads_init(&priv->sd.entity, VDIC_NUM_PADS,
+				     priv->pad);
+	if (ret)
+		goto free;
+
+	ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
+	if (ret)
+		goto free;
+
+	return &priv->sd;
+free:
+	mutex_destroy(&priv->lock);
+	return ERR_PTR(ret);
+}
+
+int imx_media_vdic_unregister(struct v4l2_subdev *sd)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	v4l2_info(sd, "Removing\n");
+
+	v4l2_device_unregister_subdev(sd);
+	mutex_destroy(&priv->lock);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
diff --git a/drivers/staging/media/imx/imx-media.h b/drivers/staging/media/imx/imx-media.h
new file mode 100644
index 000000000..f263fc3ad
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media.h
@@ -0,0 +1,313 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ */
+#ifndef _IMX_MEDIA_H
+#define _IMX_MEDIA_H
+
+#include <linux/platform_device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+
+#define IMX_MEDIA_DEF_PIX_WIDTH		640
+#define IMX_MEDIA_DEF_PIX_HEIGHT	480
+
+/*
+ * Enumeration of the IPU internal sub-devices
+ */
+enum {
+	IPU_CSI0 = 0,
+	IPU_CSI1,
+	IPU_VDIC,
+	IPU_IC_PRP,
+	IPU_IC_PRPENC,
+	IPU_IC_PRPVF,
+	NUM_IPU_SUBDEVS,
+};
+
+/*
+ * Pad definitions for the subdevs with multiple source or
+ * sink pads
+ */
+
+/* ipu_csi */
+enum {
+	CSI_SINK_PAD = 0,
+	CSI_SRC_PAD_DIRECT,
+	CSI_SRC_PAD_IDMAC,
+	CSI_NUM_PADS,
+};
+
+/* ipu_vdic */
+enum {
+	VDIC_SINK_PAD_DIRECT = 0,
+	VDIC_SINK_PAD_IDMAC,
+	VDIC_SRC_PAD_DIRECT,
+	VDIC_NUM_PADS,
+};
+
+/* ipu_ic_prp */
+enum {
+	PRP_SINK_PAD = 0,
+	PRP_SRC_PAD_PRPENC,
+	PRP_SRC_PAD_PRPVF,
+	PRP_NUM_PADS,
+};
+
+/* ipu_ic_prpencvf */
+enum {
+	PRPENCVF_SINK_PAD = 0,
+	PRPENCVF_SRC_PAD,
+	PRPENCVF_NUM_PADS,
+};
+
+/* How long to wait for EOF interrupts in the buffer-capture subdevs */
+#define IMX_MEDIA_EOF_TIMEOUT       2000
+
+struct imx_media_pixfmt {
+	/* the in-memory FourCC pixel format */
+	u32     fourcc;
+	/*
+	 * the set of equivalent media bus codes for the fourcc.
+	 * NOTE! codes pointer is NULL for in-memory-only formats.
+	 */
+	const u32 *codes;
+	int     bpp;     /* total bpp */
+	/* cycles per pixel for generic (bayer) formats for the parallel bus */
+	int	cycles;
+	enum ipu_color_space cs;
+	bool    planar;  /* is a planar format */
+	bool    bayer;   /* is a raw bayer format */
+	bool    ipufmt;  /* is one of the IPU internal formats */
+};
+
+enum imx_pixfmt_sel {
+	PIXFMT_SEL_YUV   = BIT(0), /* select YUV formats */
+	PIXFMT_SEL_RGB   = BIT(1), /* select RGB formats */
+	PIXFMT_SEL_BAYER = BIT(2), /* select BAYER formats */
+	PIXFMT_SEL_IPU   = BIT(3), /* select IPU-internal formats */
+	PIXFMT_SEL_YUV_RGB = PIXFMT_SEL_YUV | PIXFMT_SEL_RGB,
+	PIXFMT_SEL_ANY = PIXFMT_SEL_YUV | PIXFMT_SEL_RGB | PIXFMT_SEL_BAYER,
+};
+
+struct imx_media_buffer {
+	struct vb2_v4l2_buffer vbuf; /* v4l buffer must be first */
+	struct list_head  list;
+};
+
+struct imx_media_video_dev {
+	struct video_device *vfd;
+
+	/* the user format */
+	struct v4l2_pix_format fmt;
+	/* the compose rectangle */
+	struct v4l2_rect compose;
+	const struct imx_media_pixfmt *cc;
+
+	/* links this vdev to master list */
+	struct list_head list;
+};
+
+static inline struct imx_media_buffer *to_imx_media_vb(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	return container_of(vbuf, struct imx_media_buffer, vbuf);
+}
+
+/*
+ * to support control inheritance to video devices, this
+ * retrieves a pad's list_head of video devices that can
+ * be reached from the pad. Note that only the lists in
+ * source pads get populated, sink pads have empty lists.
+ */
+static inline struct list_head *
+to_pad_vdev_list(struct v4l2_subdev *sd, int pad_index)
+{
+	struct list_head *vdev_list = sd->host_priv;
+
+	return vdev_list ? &vdev_list[pad_index] : NULL;
+}
+
+/* an entry in a pad's video device list */
+struct imx_media_pad_vdev {
+	struct imx_media_video_dev *vdev;
+	struct list_head list;
+};
+
+struct imx_media_dev {
+	struct media_device md;
+	struct v4l2_device  v4l2_dev;
+
+	/* the pipeline object */
+	struct media_pipeline pipe;
+
+	struct mutex mutex; /* protect elements below */
+
+	/* master video device list */
+	struct list_head vdev_list;
+
+	/* IPUs this media driver control, valid after subdevs bound */
+	struct ipu_soc *ipu[2];
+
+	/* for async subdev registration */
+	struct v4l2_async_notifier notifier;
+
+	/* IC scaler/CSC mem2mem video device */
+	struct imx_media_video_dev *m2m_vdev;
+
+	/* the IPU internal subdev's registered synchronously */
+	struct v4l2_subdev *sync_sd[2][NUM_IPU_SUBDEVS];
+};
+
+/* imx-media-utils.c */
+const struct imx_media_pixfmt *
+imx_media_find_pixel_format(u32 fourcc, enum imx_pixfmt_sel sel);
+int imx_media_enum_pixel_formats(u32 *fourcc, u32 index,
+				 enum imx_pixfmt_sel sel, u32 code);
+const struct imx_media_pixfmt *
+imx_media_find_mbus_format(u32 code, enum imx_pixfmt_sel sel);
+int imx_media_enum_mbus_formats(u32 *code, u32 index,
+				enum imx_pixfmt_sel sel);
+
+static inline const struct imx_media_pixfmt *
+imx_media_find_ipu_format(u32 code, enum imx_pixfmt_sel fmt_sel)
+{
+	return imx_media_find_mbus_format(code, fmt_sel | PIXFMT_SEL_IPU);
+}
+
+static inline int imx_media_enum_ipu_formats(u32 *code, u32 index,
+					     enum imx_pixfmt_sel fmt_sel)
+{
+	return imx_media_enum_mbus_formats(code, index,
+					  fmt_sel | PIXFMT_SEL_IPU);
+}
+
+int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+			    u32 width, u32 height, u32 code, u32 field,
+			    const struct imx_media_pixfmt **cc);
+int imx_media_init_cfg(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state);
+void imx_media_try_colorimetry(struct v4l2_mbus_framefmt *tryfmt,
+			       bool ic_route);
+int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+				  const struct v4l2_mbus_framefmt *mbus,
+				  const struct imx_media_pixfmt *cc);
+void imx_media_grp_id_to_sd_name(char *sd_name, int sz,
+				 u32 grp_id, int ipu_id);
+struct v4l2_subdev *
+imx_media_find_subdev_by_fwnode(struct imx_media_dev *imxmd,
+				struct fwnode_handle *fwnode);
+struct v4l2_subdev *
+imx_media_find_subdev_by_devname(struct imx_media_dev *imxmd,
+				 const char *devname);
+void imx_media_add_video_device(struct imx_media_dev *imxmd,
+				struct imx_media_video_dev *vdev);
+int imx_media_pipeline_csi2_channel(struct media_entity *start_entity);
+struct media_pad *
+imx_media_pipeline_pad(struct media_entity *start_entity, u32 grp_id,
+		       enum v4l2_buf_type buftype, bool upstream);
+struct v4l2_subdev *
+imx_media_pipeline_subdev(struct media_entity *start_entity, u32 grp_id,
+			  bool upstream);
+struct video_device *
+imx_media_pipeline_video_device(struct media_entity *start_entity,
+				enum v4l2_buf_type buftype, bool upstream);
+struct fwnode_handle *imx_media_get_pad_fwnode(struct media_pad *pad);
+
+struct imx_media_dma_buf {
+	void          *virt;
+	dma_addr_t     phys;
+	unsigned long  len;
+};
+
+void imx_media_free_dma_buf(struct device *dev,
+			    struct imx_media_dma_buf *buf);
+int imx_media_alloc_dma_buf(struct device *dev,
+			    struct imx_media_dma_buf *buf,
+			    int size);
+
+int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd,
+				  struct media_entity *entity,
+				  bool on);
+
+/* imx-media-dev-common.c */
+int imx_media_probe_complete(struct v4l2_async_notifier *notifier);
+struct imx_media_dev *imx_media_dev_init(struct device *dev,
+					 const struct media_device_ops *ops);
+int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
+			    const struct v4l2_async_notifier_operations *ops);
+
+/* imx-media-fim.c */
+struct imx_media_fim;
+void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp);
+int imx_media_fim_set_stream(struct imx_media_fim *fim,
+			     const struct v4l2_fract *frame_interval,
+			     bool on);
+int imx_media_fim_add_controls(struct imx_media_fim *fim);
+struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd);
+void imx_media_fim_free(struct imx_media_fim *fim);
+
+/* imx-media-internal-sd.c */
+int imx_media_register_ipu_internal_subdevs(struct imx_media_dev *imxmd,
+					    struct v4l2_subdev *csi);
+void imx_media_unregister_ipu_internal_subdevs(struct imx_media_dev *imxmd);
+
+/* imx-media-of.c */
+int imx_media_add_of_subdevs(struct imx_media_dev *dev,
+			     struct device_node *np);
+int imx_media_of_add_csi(struct imx_media_dev *imxmd,
+			 struct device_node *csi_np);
+
+/* imx-media-vdic.c */
+struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
+					    struct device *ipu_dev,
+					    struct ipu_soc *ipu,
+					    u32 grp_id);
+int imx_media_vdic_unregister(struct v4l2_subdev *sd);
+
+/* imx-ic-common.c */
+struct v4l2_subdev *imx_media_ic_register(struct v4l2_device *v4l2_dev,
+					  struct device *ipu_dev,
+					  struct ipu_soc *ipu,
+					  u32 grp_id);
+int imx_media_ic_unregister(struct v4l2_subdev *sd);
+
+/* imx-media-capture.c */
+struct imx_media_video_dev *
+imx_media_capture_device_init(struct device *dev, struct v4l2_subdev *src_sd,
+			      int pad, bool legacy_api);
+void imx_media_capture_device_remove(struct imx_media_video_dev *vdev);
+int imx_media_capture_device_register(struct imx_media_video_dev *vdev,
+				      u32 link_flags);
+void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev);
+struct imx_media_buffer *
+imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev);
+void imx_media_capture_device_error(struct imx_media_video_dev *vdev);
+
+/* imx-media-csc-scaler.c */
+struct imx_media_video_dev *
+imx_media_csc_scaler_device_init(struct imx_media_dev *dev);
+int imx_media_csc_scaler_device_register(struct imx_media_video_dev *vdev);
+void imx_media_csc_scaler_device_unregister(struct imx_media_video_dev *vdev);
+
+/* subdev group ids */
+#define IMX_MEDIA_GRP_ID_CSI2          BIT(8)
+#define IMX_MEDIA_GRP_ID_CSI           BIT(9)
+#define IMX_MEDIA_GRP_ID_IPU_CSI_BIT   10
+#define IMX_MEDIA_GRP_ID_IPU_CSI       (0x3 << IMX_MEDIA_GRP_ID_IPU_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_IPU_CSI0      BIT(IMX_MEDIA_GRP_ID_IPU_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_IPU_CSI1      (2 << IMX_MEDIA_GRP_ID_IPU_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_IPU_VDIC      BIT(12)
+#define IMX_MEDIA_GRP_ID_IPU_IC_PRP    BIT(13)
+#define IMX_MEDIA_GRP_ID_IPU_IC_PRPENC BIT(14)
+#define IMX_MEDIA_GRP_ID_IPU_IC_PRPVF  BIT(15)
+#define IMX_MEDIA_GRP_ID_CSI_MUX       BIT(16)
+
+#endif
diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
new file mode 100644
index 000000000..c4cb558a8
--- /dev/null
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -0,0 +1,803 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ */
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include "imx-media.h"
+
+/*
+ * there must be 5 pads: 1 input pad from sensor, and
+ * the 4 virtual channel output pads
+ */
+#define CSI2_SINK_PAD       0
+#define CSI2_NUM_SINK_PADS  1
+#define CSI2_NUM_SRC_PADS   4
+#define CSI2_NUM_PADS       5
+
+/*
+ * The default maximum bit-rate per lane in Mbps, if the
+ * source subdev does not provide V4L2_CID_LINK_FREQ.
+ */
+#define CSI2_DEFAULT_MAX_MBPS 849
+
+struct csi2_dev {
+	struct device          *dev;
+	struct v4l2_subdev      sd;
+	struct v4l2_async_notifier notifier;
+	struct media_pad       pad[CSI2_NUM_PADS];
+	struct clk             *dphy_clk;
+	struct clk             *pllref_clk;
+	struct clk             *pix_clk; /* what is this? */
+	void __iomem           *base;
+
+	struct v4l2_subdev	*remote;
+	unsigned int		remote_pad;
+	unsigned short		data_lanes;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	struct v4l2_mbus_framefmt format_mbus;
+
+	int                     stream_count;
+	struct v4l2_subdev      *src_sd;
+	bool                    sink_linked[CSI2_NUM_SRC_PADS];
+};
+
+#define DEVICE_NAME "imx6-mipi-csi2"
+
+/* Register offsets */
+#define CSI2_VERSION            0x000
+#define CSI2_N_LANES            0x004
+#define CSI2_PHY_SHUTDOWNZ      0x008
+#define CSI2_DPHY_RSTZ          0x00c
+#define CSI2_RESETN             0x010
+#define CSI2_PHY_STATE          0x014
+#define PHY_STOPSTATEDATA_BIT   4
+#define PHY_STOPSTATEDATA(n)    BIT(PHY_STOPSTATEDATA_BIT + (n))
+#define PHY_RXCLKACTIVEHS       BIT(8)
+#define PHY_RXULPSCLKNOT        BIT(9)
+#define PHY_STOPSTATECLK        BIT(10)
+#define CSI2_DATA_IDS_1         0x018
+#define CSI2_DATA_IDS_2         0x01c
+#define CSI2_ERR1               0x020
+#define CSI2_ERR2               0x024
+#define CSI2_MSK1               0x028
+#define CSI2_MSK2               0x02c
+#define CSI2_PHY_TST_CTRL0      0x030
+#define PHY_TESTCLR		BIT(0)
+#define PHY_TESTCLK		BIT(1)
+#define CSI2_PHY_TST_CTRL1      0x034
+#define PHY_TESTEN		BIT(16)
+/*
+ * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is
+ * not part of the MIPI CSI-2 core, but its registers fall in the
+ * same register map range.
+ */
+#define CSI2IPU_GASKET		0xf00
+#define CSI2IPU_YUV422_YUYV	BIT(2)
+
+static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi2_dev, sd);
+}
+
+static inline struct csi2_dev *notifier_to_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct csi2_dev, notifier);
+}
+
+/*
+ * The required sequence of MIPI CSI-2 startup as specified in the i.MX6
+ * reference manual is as follows:
+ *
+ * 1. Deassert presetn signal (global reset).
+ *        It's not clear what this "global reset" signal is (maybe APB
+ *        global reset), but in any case this step would be probably
+ *        be carried out during driver load in csi2_probe().
+ *
+ * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state.
+ *        This must be carried out by the MIPI sensor's s_power(ON) subdev
+ *        op.
+ *
+ * 3. D-PHY initialization.
+ * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ,
+ *    deassert PHY_RSTZ, deassert CSI2_RESETN).
+ * 5. Read the PHY status register (PHY_STATE) to confirm that all data and
+ *    clock lanes of the D-PHY are in LP-11 state.
+ * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the
+ *    D-PHY clock lane.
+ * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
+ *    to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
+ *
+ * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
+ * 6 is accomplished by calling the source subdev's s_stream(ON) between
+ * steps 5 and 7.
+ */
+
+static void csi2_enable(struct csi2_dev *csi2, bool enable)
+{
+	if (enable) {
+		writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ);
+		writel(0x1, csi2->base + CSI2_DPHY_RSTZ);
+		writel(0x1, csi2->base + CSI2_RESETN);
+	} else {
+		writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ);
+		writel(0x0, csi2->base + CSI2_DPHY_RSTZ);
+		writel(0x0, csi2->base + CSI2_RESETN);
+	}
+}
+
+static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes)
+{
+	writel(lanes - 1, csi2->base + CSI2_N_LANES);
+}
+
+static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2,
+				   u32 test_code, u32 test_data)
+{
+	/* Clear PHY test interface */
+	writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Raise test interface strobe signal */
+	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Configure address write on falling edge and lower strobe signal */
+	writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Configure data write on rising edge and raise strobe signal */
+	writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Clear strobe signal */
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+}
+
+/*
+ * This table is based on the table documented at
+ * https://community.nxp.com/docs/DOC-94312. It assumes
+ * a 27MHz D-PHY pll reference clock.
+ */
+static const struct {
+	u32 max_mbps;
+	u32 hsfreqrange_sel;
+} hsfreq_map[] = {
+	{ 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
+	{140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
+	{200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
+	{270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
+	{400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
+	{600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
+	{800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
+	{1000, 0x74},
+};
+
+static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
+		if (hsfreq_map[i].max_mbps > max_mbps)
+			return hsfreq_map[i].hsfreqrange_sel;
+
+	return -EINVAL;
+}
+
+static int csi2_dphy_init(struct csi2_dev *csi2)
+{
+	struct v4l2_ctrl *ctrl;
+	u32 mbps_per_lane;
+	int sel;
+
+	ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
+			      V4L2_CID_LINK_FREQ);
+	if (!ctrl)
+		mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
+	else
+		mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
+						 USEC_PER_SEC);
+
+	sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
+	if (sel < 0)
+		return sel;
+
+	dw_mipi_csi2_phy_write(csi2, 0x44, sel);
+
+	return 0;
+}
+
+/*
+ * Waits for ultra-low-power state on D-PHY clock lane. This is currently
+ * unused and may not be needed at all, but keep around just in case.
+ */
+static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2)
+{
+	u32 reg;
+	int ret;
+
+	/* wait for ULP on clock lane */
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 !(reg & PHY_RXULPSCLKNOT), 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg);
+		return ret;
+	}
+
+	/* wait until no errors on bus */
+	ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg,
+				 reg == 0x0, 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Waits for low-power LP-11 state on data and clock lanes. */
+static void csi2_dphy_wait_stopstate(struct csi2_dev *csi2, unsigned int lanes)
+{
+	u32 mask, reg;
+	int ret;
+
+	mask = PHY_STOPSTATECLK | (((1 << lanes) - 1) << PHY_STOPSTATEDATA_BIT);
+
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 (reg & mask) == mask, 0, 500000);
+	if (ret) {
+		v4l2_warn(&csi2->sd, "LP-11 wait timeout, likely a sensor driver bug, expect capture failures.\n");
+		v4l2_warn(&csi2->sd, "phy_state = 0x%08x\n", reg);
+	}
+}
+
+/* Wait for active clock on the clock lane. */
+static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2)
+{
+	u32 reg;
+	int ret;
+
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 (reg & PHY_RXCLKACTIVEHS), 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n",
+			 reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Setup the i.MX CSI2IPU Gasket */
+static void csi2ipu_gasket_init(struct csi2_dev *csi2)
+{
+	u32 reg = 0;
+
+	switch (csi2->format_mbus.code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		reg = CSI2IPU_YUV422_YUYV;
+		break;
+	default:
+		break;
+	}
+
+	writel(reg, csi2->base + CSI2IPU_GASKET);
+}
+
+static int csi2_get_active_lanes(struct csi2_dev *csi2, unsigned int *lanes)
+{
+	struct v4l2_mbus_config mbus_config = { 0 };
+	int ret;
+
+	*lanes = csi2->data_lanes;
+
+	ret = v4l2_subdev_call(csi2->remote, pad, get_mbus_config,
+			       csi2->remote_pad, &mbus_config);
+	if (ret == -ENOIOCTLCMD) {
+		dev_dbg(csi2->dev, "No remote mbus configuration available\n");
+		return 0;
+	}
+
+	if (ret) {
+		dev_err(csi2->dev, "Failed to get remote mbus configuration\n");
+		return ret;
+	}
+
+	if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(csi2->dev, "Unsupported media bus type %u\n",
+			mbus_config.type);
+		return -EINVAL;
+	}
+
+	if (mbus_config.bus.mipi_csi2.num_data_lanes > csi2->data_lanes) {
+		dev_err(csi2->dev,
+			"Unsupported mbus config: too many data lanes %u\n",
+			mbus_config.bus.mipi_csi2.num_data_lanes);
+		return -EINVAL;
+	}
+
+	*lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+
+	return 0;
+}
+
+static int csi2_start(struct csi2_dev *csi2)
+{
+	unsigned int lanes;
+	int ret;
+
+	ret = clk_prepare_enable(csi2->pix_clk);
+	if (ret)
+		return ret;
+
+	/* setup the gasket */
+	csi2ipu_gasket_init(csi2);
+
+	/* Step 3 */
+	ret = csi2_dphy_init(csi2);
+	if (ret)
+		goto err_disable_clk;
+
+	ret = csi2_get_active_lanes(csi2, &lanes);
+	if (ret)
+		goto err_disable_clk;
+
+	/* Step 4 */
+	csi2_set_lanes(csi2, lanes);
+	csi2_enable(csi2, true);
+
+	/* Step 5 */
+	ret = v4l2_subdev_call(csi2->src_sd, video, pre_streamon,
+			       V4L2_SUBDEV_PRE_STREAMON_FL_MANUAL_LP);
+	if (ret && ret != -ENOIOCTLCMD)
+		goto err_assert_reset;
+	csi2_dphy_wait_stopstate(csi2, lanes);
+
+	/* Step 6 */
+	ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret)
+		goto err_stop_lp11;
+
+	/* Step 7 */
+	ret = csi2_dphy_wait_clock_lane(csi2);
+	if (ret)
+		goto err_stop_upstream;
+
+	return 0;
+
+err_stop_upstream:
+	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
+err_stop_lp11:
+	v4l2_subdev_call(csi2->src_sd, video, post_streamoff);
+err_assert_reset:
+	csi2_enable(csi2, false);
+err_disable_clk:
+	clk_disable_unprepare(csi2->pix_clk);
+	return ret;
+}
+
+static void csi2_stop(struct csi2_dev *csi2)
+{
+	/* stop upstream */
+	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
+	v4l2_subdev_call(csi2->src_sd, video, post_streamoff);
+
+	csi2_enable(csi2, false);
+	clk_disable_unprepare(csi2->pix_clk);
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int i, ret = 0;
+
+	mutex_lock(&csi2->lock);
+
+	if (!csi2->src_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	for (i = 0; i < CSI2_NUM_SRC_PADS; i++) {
+		if (csi2->sink_linked[i])
+			break;
+	}
+	if (i >= CSI2_NUM_SRC_PADS) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (csi2->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
+	if (enable)
+		ret = csi2_start(csi2);
+	else
+		csi2_stop(csi2);
+	if (ret)
+		goto out;
+
+update_count:
+	csi2->stream_count += enable ? 1 : -1;
+	if (csi2->stream_count < 0)
+		csi2->stream_count = 0;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name,
+		local->entity->name);
+
+	remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+	mutex_lock(&csi2->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SOURCE) {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->sink_linked[local->index - 1]) {
+				ret = -EBUSY;
+				goto out;
+			}
+			csi2->sink_linked[local->index - 1] = true;
+		} else {
+			csi2->sink_linked[local->index - 1] = false;
+		}
+	} else {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			csi2->src_sd = remote_sd;
+		} else {
+			csi2->src_sd = NULL;
+		}
+	}
+
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_state *sd_state,
+	       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->sd, sd_state, pad);
+	else
+		return &csi2->format_mbus;
+}
+
+static int csi2_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	mutex_lock(&csi2->lock);
+
+	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
+
+	sdformat->format = *fmt;
+
+	mutex_unlock(&csi2->lock);
+
+	return 0;
+}
+
+static int csi2_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= CSI2_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&csi2->lock);
+
+	if (csi2->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	/* Output pads mirror active input pad, no limits on input pads */
+	if (sdformat->pad != CSI2_SINK_PAD)
+		sdformat->format = csi2->format_mbus;
+
+	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
+
+	*fmt = sdformat->format;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+static int csi2_registered(struct v4l2_subdev *sd)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+
+	/* set a default mbus format  */
+	return imx_media_init_mbus_fmt(&csi2->format_mbus,
+				      IMX_MEDIA_DEF_PIX_WIDTH,
+				      IMX_MEDIA_DEF_PIX_HEIGHT, 0,
+				      V4L2_FIELD_NONE, NULL);
+}
+
+static const struct media_entity_operations csi2_entity_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+};
+
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
+	.get_fmt = csi2_get_fmt,
+	.set_fmt = csi2_set_fmt,
+};
+
+static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.registered = csi2_registered,
+};
+
+static int csi2_notify_bound(struct v4l2_async_notifier *notifier,
+			     struct v4l2_subdev *sd,
+			     struct v4l2_async_subdev *asd)
+{
+	struct csi2_dev *csi2 = notifier_to_dev(notifier);
+	struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD];
+	int pad;
+
+	pad = media_entity_get_fwnode_pad(&sd->entity, asd->match.fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (pad < 0) {
+		dev_err(csi2->dev, "Failed to find pad for %s\n", sd->name);
+		return pad;
+	}
+
+	csi2->remote = sd;
+	csi2->remote_pad = pad;
+
+	dev_dbg(csi2->dev, "Bound %s pad: %d\n", sd->name, pad);
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, 0);
+}
+
+static void csi2_notify_unbind(struct v4l2_async_notifier *notifier,
+			       struct v4l2_subdev *sd,
+			       struct v4l2_async_subdev *asd)
+{
+	struct csi2_dev *csi2 = notifier_to_dev(notifier);
+
+	csi2->remote = NULL;
+}
+
+static const struct v4l2_async_notifier_operations csi2_notify_ops = {
+	.bound = csi2_notify_bound,
+	.unbind = csi2_notify_unbind,
+};
+
+static int csi2_async_register(struct csi2_dev *csi2)
+{
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct v4l2_async_subdev *asd;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_nf_init(&csi2->notifier);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi2->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENOTCONN;
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &vep);
+	if (ret)
+		goto err_parse;
+
+	csi2->data_lanes = vep.bus.mipi_csi2.num_data_lanes;
+
+	dev_dbg(csi2->dev, "data lanes: %d\n", vep.bus.mipi_csi2.num_data_lanes);
+	dev_dbg(csi2->dev, "flags: 0x%08x\n", vep.bus.mipi_csi2.flags);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&csi2->notifier, ep,
+					      struct v4l2_async_subdev);
+	fwnode_handle_put(ep);
+
+	if (IS_ERR(asd))
+		return PTR_ERR(asd);
+
+	csi2->notifier.ops = &csi2_notify_ops;
+
+	ret = v4l2_async_subdev_nf_register(&csi2->sd, &csi2->notifier);
+	if (ret)
+		return ret;
+
+	return v4l2_async_register_subdev(&csi2->sd);
+
+err_parse:
+	fwnode_handle_put(ep);
+	return ret;
+}
+
+static int csi2_probe(struct platform_device *pdev)
+{
+	struct csi2_dev *csi2;
+	struct resource *res;
+	int i, ret;
+
+	csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
+	if (!csi2)
+		return -ENOMEM;
+
+	csi2->dev = &pdev->dev;
+
+	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
+	v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
+	csi2->sd.internal_ops = &csi2_internal_ops;
+	csi2->sd.entity.ops = &csi2_entity_ops;
+	csi2->sd.dev = &pdev->dev;
+	csi2->sd.owner = THIS_MODULE;
+	csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strscpy(csi2->sd.name, DEVICE_NAME, sizeof(csi2->sd.name));
+	csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2;
+
+	for (i = 0; i < CSI2_NUM_PADS; i++) {
+		csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
+		MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	ret = media_entity_pads_init(&csi2->sd.entity, CSI2_NUM_PADS,
+				     csi2->pad);
+	if (ret)
+		return ret;
+
+	csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
+	if (IS_ERR(csi2->pllref_clk)) {
+		v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
+		return PTR_ERR(csi2->pllref_clk);
+	}
+
+	csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
+	if (IS_ERR(csi2->dphy_clk)) {
+		v4l2_err(&csi2->sd, "failed to get dphy clock\n");
+		return PTR_ERR(csi2->dphy_clk);
+	}
+
+	csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
+	if (IS_ERR(csi2->pix_clk)) {
+		v4l2_err(&csi2->sd, "failed to get pixel clock\n");
+		return PTR_ERR(csi2->pix_clk);
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		v4l2_err(&csi2->sd, "failed to get platform resources\n");
+		return -ENODEV;
+	}
+
+	csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE);
+	if (!csi2->base)
+		return -ENOMEM;
+
+	mutex_init(&csi2->lock);
+
+	ret = clk_prepare_enable(csi2->pllref_clk);
+	if (ret) {
+		v4l2_err(&csi2->sd, "failed to enable pllref_clk\n");
+		goto rmmutex;
+	}
+
+	ret = clk_prepare_enable(csi2->dphy_clk);
+	if (ret) {
+		v4l2_err(&csi2->sd, "failed to enable dphy_clk\n");
+		goto pllref_off;
+	}
+
+	platform_set_drvdata(pdev, &csi2->sd);
+
+	ret = csi2_async_register(csi2);
+	if (ret)
+		goto clean_notifier;
+
+	return 0;
+
+clean_notifier:
+	v4l2_async_nf_unregister(&csi2->notifier);
+	v4l2_async_nf_cleanup(&csi2->notifier);
+	clk_disable_unprepare(csi2->dphy_clk);
+pllref_off:
+	clk_disable_unprepare(csi2->pllref_clk);
+rmmutex:
+	mutex_destroy(&csi2->lock);
+	return ret;
+}
+
+static int csi2_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+
+	v4l2_async_nf_unregister(&csi2->notifier);
+	v4l2_async_nf_cleanup(&csi2->notifier);
+	v4l2_async_unregister_subdev(sd);
+	clk_disable_unprepare(csi2->dphy_clk);
+	clk_disable_unprepare(csi2->pllref_clk);
+	mutex_destroy(&csi2->lock);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct of_device_id csi2_dt_ids[] = {
+	{ .compatible = "fsl,imx6-mipi-csi2", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, csi2_dt_ids);
+
+static struct platform_driver csi2_driver = {
+	.driver = {
+		.name = DEVICE_NAME,
+		.of_match_table = csi2_dt_ids,
+	},
+	.probe = csi2_probe,
+	.remove = csi2_remove,
+};
+
+module_platform_driver(csi2_driver);
+
+MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx7-media-csi.c b/drivers/staging/media/imx/imx7-media-csi.c
new file mode 100644
index 000000000..5f6376c32
--- /dev/null
+++ b/drivers/staging/media/imx/imx7-media-csi.c
@@ -0,0 +1,2310 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * V4L2 Capture CSI Subdev for Freescale i.MX6UL/L / i.MX7 SOC
+ *
+ * Copyright (c) 2019 Linaro Ltd
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gcd.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define IMX7_CSI_PAD_SINK		0
+#define IMX7_CSI_PAD_SRC		1
+#define IMX7_CSI_PADS_NUM		2
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN			BIT(31)
+#define BIT_EXT_VSYNC			BIT(30)
+#define BIT_EOF_INT_EN			BIT(29)
+#define BIT_PRP_IF_EN			BIT(28)
+#define BIT_CCIR_MODE			BIT(27)
+#define BIT_COF_INT_EN			BIT(26)
+#define BIT_SF_OR_INTEN			BIT(25)
+#define BIT_RF_OR_INTEN			BIT(24)
+#define BIT_SFF_DMA_DONE_INTEN		BIT(22)
+#define BIT_STATFF_INTEN		BIT(21)
+#define BIT_FB2_DMA_DONE_INTEN		BIT(20)
+#define BIT_FB1_DMA_DONE_INTEN		BIT(19)
+#define BIT_RXFF_INTEN			BIT(18)
+#define BIT_SOF_POL			BIT(17)
+#define BIT_SOF_INTEN			BIT(16)
+#define BIT_MCLKDIV(n)			((n) << 12)
+#define BIT_MCLKDIV_MASK		(0xf << 12)
+#define BIT_HSYNC_POL			BIT(11)
+#define BIT_CCIR_EN			BIT(10)
+#define BIT_MCLKEN			BIT(9)
+#define BIT_FCC				BIT(8)
+#define BIT_PACK_DIR			BIT(7)
+#define BIT_CLR_STATFIFO		BIT(6)
+#define BIT_CLR_RXFIFO			BIT(5)
+#define BIT_GCLK_MODE			BIT(4)
+#define BIT_INV_DATA			BIT(3)
+#define BIT_INV_PCLK			BIT(2)
+#define BIT_REDGE			BIT(1)
+#define BIT_PIXEL_BIT			BIT(0)
+
+/* control reg 2 */
+#define BIT_DMA_BURST_TYPE_RFF_INCR4	(1 << 30)
+#define BIT_DMA_BURST_TYPE_RFF_INCR8	(2 << 30)
+#define BIT_DMA_BURST_TYPE_RFF_INCR16	(3 << 30)
+#define BIT_DMA_BURST_TYPE_RFF_MASK	(3 << 30)
+
+/* control reg 3 */
+#define BIT_FRMCNT(n)			((n) << 16)
+#define BIT_FRMCNT_MASK			(0xffff << 16)
+#define BIT_FRMCNT_RST			BIT(15)
+#define BIT_DMA_REFLASH_RFF		BIT(14)
+#define BIT_DMA_REFLASH_SFF		BIT(13)
+#define BIT_DMA_REQ_EN_RFF		BIT(12)
+#define BIT_DMA_REQ_EN_SFF		BIT(11)
+#define BIT_STATFF_LEVEL(n)		((n) << 8)
+#define BIT_STATFF_LEVEL_MASK		(0x7 << 8)
+#define BIT_HRESP_ERR_EN		BIT(7)
+#define BIT_RXFF_LEVEL(n)		((n) << 4)
+#define BIT_RXFF_LEVEL_MASK		(0x7 << 4)
+#define BIT_TWO_8BIT_SENSOR		BIT(3)
+#define BIT_ZERO_PACK_EN		BIT(2)
+#define BIT_ECC_INT_EN			BIT(1)
+#define BIT_ECC_AUTO_EN			BIT(0)
+
+/* csi status reg */
+#define BIT_ADDR_CH_ERR_INT		BIT(28)
+#define BIT_FIELD0_INT			BIT(27)
+#define BIT_FIELD1_INT			BIT(26)
+#define BIT_SFF_OR_INT			BIT(25)
+#define BIT_RFF_OR_INT			BIT(24)
+#define BIT_DMA_TSF_DONE_SFF		BIT(22)
+#define BIT_STATFF_INT			BIT(21)
+#define BIT_DMA_TSF_DONE_FB2		BIT(20)
+#define BIT_DMA_TSF_DONE_FB1		BIT(19)
+#define BIT_RXFF_INT			BIT(18)
+#define BIT_EOF_INT			BIT(17)
+#define BIT_SOF_INT			BIT(16)
+#define BIT_F2_INT			BIT(15)
+#define BIT_F1_INT			BIT(14)
+#define BIT_COF_INT			BIT(13)
+#define BIT_HRESP_ERR_INT		BIT(7)
+#define BIT_ECC_INT			BIT(1)
+#define BIT_DRDY			BIT(0)
+
+/* csi image parameter reg */
+#define BIT_IMAGE_WIDTH(n)		((n) << 16)
+#define BIT_IMAGE_HEIGHT(n)		(n)
+
+/* csi control reg 18 */
+#define BIT_CSI_HW_ENABLE		BIT(31)
+#define BIT_MIPI_DATA_FORMAT_RAW8	(0x2a << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW10	(0x2b << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW12	(0x2c << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW14	(0x2d << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1e << 25)
+#define BIT_MIPI_DATA_FORMAT_MASK	(0x3f << 25)
+#define BIT_DATA_FROM_MIPI		BIT(22)
+#define BIT_MIPI_YU_SWAP		BIT(21)
+#define BIT_MIPI_DOUBLE_CMPNT		BIT(20)
+#define BIT_MASK_OPTION_FIRST_FRAME	(0 << 18)
+#define BIT_MASK_OPTION_CSI_EN		(1 << 18)
+#define BIT_MASK_OPTION_SECOND_FRAME	(2 << 18)
+#define BIT_MASK_OPTION_ON_DATA		(3 << 18)
+#define BIT_BASEADDR_CHG_ERR_EN		BIT(9)
+#define BIT_BASEADDR_SWITCH_SEL		BIT(5)
+#define BIT_BASEADDR_SWITCH_EN		BIT(4)
+#define BIT_PARALLEL24_EN		BIT(3)
+#define BIT_DEINTERLACE_EN		BIT(2)
+#define BIT_TVDECODER_IN_EN		BIT(1)
+#define BIT_NTSC_EN			BIT(0)
+
+#define CSI_MCLK_VF			1
+#define CSI_MCLK_ENC			2
+#define CSI_MCLK_RAW			4
+#define CSI_MCLK_I2C			8
+
+#define CSI_CSICR1			0x00
+#define CSI_CSICR2			0x04
+#define CSI_CSICR3			0x08
+#define CSI_STATFIFO			0x0c
+#define CSI_CSIRXFIFO			0x10
+#define CSI_CSIRXCNT			0x14
+#define CSI_CSISR			0x18
+
+#define CSI_CSIDBG			0x1c
+#define CSI_CSIDMASA_STATFIFO		0x20
+#define CSI_CSIDMATS_STATFIFO		0x24
+#define CSI_CSIDMASA_FB1		0x28
+#define CSI_CSIDMASA_FB2		0x2c
+#define CSI_CSIFBUF_PARA		0x30
+#define CSI_CSIIMAG_PARA		0x34
+
+#define CSI_CSICR18			0x48
+#define CSI_CSICR19			0x4c
+
+#define IMX7_CSI_VIDEO_NAME		"imx-capture"
+/* In bytes, per queue */
+#define IMX7_CSI_VIDEO_MEM_LIMIT	SZ_512M
+#define IMX7_CSI_VIDEO_EOF_TIMEOUT	2000
+
+#define IMX7_CSI_DEF_MBUS_CODE		MEDIA_BUS_FMT_UYVY8_2X8
+#define IMX7_CSI_DEF_PIX_FORMAT		V4L2_PIX_FMT_UYVY
+#define IMX7_CSI_DEF_PIX_WIDTH		640
+#define IMX7_CSI_DEF_PIX_HEIGHT		480
+
+enum imx_csi_model {
+	IMX7_CSI_IMX7 = 0,
+	IMX7_CSI_IMX8MQ,
+};
+
+struct imx7_csi_pixfmt {
+	/* the in-memory FourCC pixel format */
+	u32     fourcc;
+	/*
+	 * the set of equivalent media bus codes for the fourcc.
+	 * NOTE! codes pointer is NULL for in-memory-only formats.
+	 */
+	const u32 *codes;
+	int     bpp;     /* total bpp */
+	bool	yuv;
+};
+
+struct imx7_csi_vb2_buffer {
+	struct vb2_v4l2_buffer vbuf;
+	struct list_head list;
+};
+
+static inline struct imx7_csi_vb2_buffer *
+to_imx7_csi_vb2_buffer(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	return container_of(vbuf, struct imx7_csi_vb2_buffer, vbuf);
+}
+
+struct imx7_csi_dma_buf {
+	void *virt;
+	dma_addr_t phys;
+	unsigned long len;
+};
+
+struct imx7_csi {
+	struct device *dev;
+
+	/* Resources and locks */
+	void __iomem *regbase;
+	int irq;
+	struct clk *mclk;
+
+	struct mutex lock; /* Protects is_streaming, format_mbus, cc */
+	spinlock_t irqlock; /* Protects last_eof */
+
+	/* Media and V4L2 device */
+	struct media_device mdev;
+	struct v4l2_device v4l2_dev;
+	struct v4l2_async_notifier notifier;
+	struct media_pipeline pipe;
+
+	struct v4l2_subdev *src_sd;
+	bool is_csi2;
+
+	/* V4L2 subdev */
+	struct v4l2_subdev sd;
+	struct media_pad pad[IMX7_CSI_PADS_NUM];
+
+	struct v4l2_mbus_framefmt format_mbus[IMX7_CSI_PADS_NUM];
+	const struct imx7_csi_pixfmt *cc[IMX7_CSI_PADS_NUM];
+
+	/* Video device */
+	struct video_device *vdev;		/* Video device */
+	struct media_pad vdev_pad;		/* Video device pad */
+
+	struct v4l2_pix_format vdev_fmt;	/* The user format */
+	const struct imx7_csi_pixfmt *vdev_cc;
+	struct v4l2_rect vdev_compose;		/* The compose rectangle */
+
+	struct mutex vdev_mutex;		/* Protect vdev operations */
+
+	struct vb2_queue q;			/* The videobuf2 queue */
+	struct list_head ready_q;		/* List of queued buffers */
+	spinlock_t q_lock;			/* Protect ready_q */
+
+	/* Buffers and streaming state */
+	struct imx7_csi_vb2_buffer *active_vb2_buf[2];
+	struct imx7_csi_dma_buf underrun_buf;
+
+	bool is_streaming;
+	int buf_num;
+	u32 frame_sequence;
+
+	bool last_eof;
+	struct completion last_eof_completion;
+
+	enum imx_csi_model model;
+};
+
+static struct imx7_csi *
+imx7_csi_notifier_to_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct imx7_csi, notifier);
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware Configuration
+ */
+
+static u32 imx7_csi_reg_read(struct imx7_csi *csi, unsigned int offset)
+{
+	return readl(csi->regbase + offset);
+}
+
+static void imx7_csi_reg_write(struct imx7_csi *csi, unsigned int value,
+			       unsigned int offset)
+{
+	writel(value, csi->regbase + offset);
+}
+
+static u32 imx7_csi_irq_clear(struct imx7_csi *csi)
+{
+	u32 isr;
+
+	isr = imx7_csi_reg_read(csi, CSI_CSISR);
+	imx7_csi_reg_write(csi, isr, CSI_CSISR);
+
+	return isr;
+}
+
+static void imx7_csi_init_default(struct imx7_csi *csi)
+{
+	imx7_csi_reg_write(csi, BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE |
+			   BIT_HSYNC_POL | BIT_FCC | BIT_MCLKDIV(1) |
+			   BIT_MCLKEN, CSI_CSICR1);
+	imx7_csi_reg_write(csi, 0, CSI_CSICR2);
+	imx7_csi_reg_write(csi, BIT_FRMCNT_RST, CSI_CSICR3);
+
+	imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(IMX7_CSI_DEF_PIX_WIDTH) |
+			   BIT_IMAGE_HEIGHT(IMX7_CSI_DEF_PIX_HEIGHT),
+			   CSI_CSIIMAG_PARA);
+
+	imx7_csi_reg_write(csi, BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+}
+
+static void imx7_csi_hw_enable_irq(struct imx7_csi *csi)
+{
+	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
+
+	cr1 |= BIT_RFF_OR_INT;
+	cr1 |= BIT_FB1_DMA_DONE_INTEN;
+	cr1 |= BIT_FB2_DMA_DONE_INTEN;
+
+	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
+}
+
+static void imx7_csi_hw_disable_irq(struct imx7_csi *csi)
+{
+	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
+
+	cr1 &= ~BIT_RFF_OR_INT;
+	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
+	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
+
+	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
+}
+
+static void imx7_csi_hw_enable(struct imx7_csi *csi)
+{
+	u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
+
+	cr |= BIT_CSI_HW_ENABLE;
+
+	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
+}
+
+static void imx7_csi_hw_disable(struct imx7_csi *csi)
+{
+	u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
+
+	cr &= ~BIT_CSI_HW_ENABLE;
+
+	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
+}
+
+static void imx7_csi_dma_reflash(struct imx7_csi *csi)
+{
+	u32 cr3;
+
+	cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
+	cr3 |= BIT_DMA_REFLASH_RFF;
+	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
+}
+
+static void imx7_csi_rx_fifo_clear(struct imx7_csi *csi)
+{
+	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1) & ~BIT_FCC;
+
+	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
+	imx7_csi_reg_write(csi, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
+	imx7_csi_reg_write(csi, cr1 | BIT_FCC, CSI_CSICR1);
+}
+
+static void imx7_csi_dmareq_rff_enable(struct imx7_csi *csi)
+{
+	u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
+
+	cr3 |= BIT_DMA_REQ_EN_RFF;
+	cr3 |= BIT_HRESP_ERR_EN;
+	cr3 &= ~BIT_RXFF_LEVEL_MASK;
+	cr3 |= BIT_RXFF_LEVEL(2);
+
+	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
+}
+
+static void imx7_csi_dmareq_rff_disable(struct imx7_csi *csi)
+{
+	u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
+
+	cr3 &= ~BIT_DMA_REQ_EN_RFF;
+	cr3 &= ~BIT_HRESP_ERR_EN;
+	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
+}
+
+static void imx7_csi_update_buf(struct imx7_csi *csi, dma_addr_t phys,
+				int buf_num)
+{
+	if (buf_num == 1)
+		imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB2);
+	else
+		imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB1);
+}
+
+static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi);
+
+static void imx7_csi_setup_vb2_buf(struct imx7_csi *csi)
+{
+	struct imx7_csi_vb2_buffer *buf;
+	struct vb2_buffer *vb2_buf;
+	dma_addr_t phys[2];
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		buf = imx7_csi_video_next_buf(csi);
+		if (buf) {
+			csi->active_vb2_buf[i] = buf;
+			vb2_buf = &buf->vbuf.vb2_buf;
+			phys[i] = vb2_dma_contig_plane_dma_addr(vb2_buf, 0);
+		} else {
+			csi->active_vb2_buf[i] = NULL;
+			phys[i] = csi->underrun_buf.phys;
+		}
+
+		imx7_csi_update_buf(csi, phys[i], i);
+	}
+}
+
+static void imx7_csi_dma_unsetup_vb2_buf(struct imx7_csi *csi,
+					 enum vb2_buffer_state return_status)
+{
+	struct imx7_csi_vb2_buffer *buf;
+	int i;
+
+	/* return any remaining active frames with return_status */
+	for (i = 0; i < 2; i++) {
+		buf = csi->active_vb2_buf[i];
+		if (buf) {
+			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vb2_buffer_done(vb, return_status);
+			csi->active_vb2_buf[i] = NULL;
+		}
+	}
+}
+
+static void imx7_csi_free_dma_buf(struct imx7_csi *csi,
+				  struct imx7_csi_dma_buf *buf)
+{
+	if (buf->virt)
+		dma_free_coherent(csi->dev, buf->len, buf->virt, buf->phys);
+
+	buf->virt = NULL;
+	buf->phys = 0;
+}
+
+static int imx7_csi_alloc_dma_buf(struct imx7_csi *csi,
+				  struct imx7_csi_dma_buf *buf, int size)
+{
+	imx7_csi_free_dma_buf(csi, buf);
+
+	buf->len = PAGE_ALIGN(size);
+	buf->virt = dma_alloc_coherent(csi->dev, buf->len, &buf->phys,
+				       GFP_DMA | GFP_KERNEL);
+	if (!buf->virt)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int imx7_csi_dma_setup(struct imx7_csi *csi)
+{
+	int ret;
+
+	ret = imx7_csi_alloc_dma_buf(csi, &csi->underrun_buf,
+				     csi->vdev_fmt.sizeimage);
+	if (ret < 0) {
+		v4l2_warn(&csi->sd, "consider increasing the CMA area\n");
+		return ret;
+	}
+
+	csi->frame_sequence = 0;
+	csi->last_eof = false;
+	init_completion(&csi->last_eof_completion);
+
+	imx7_csi_setup_vb2_buf(csi);
+
+	return 0;
+}
+
+static void imx7_csi_dma_cleanup(struct imx7_csi *csi,
+				 enum vb2_buffer_state return_status)
+{
+	imx7_csi_dma_unsetup_vb2_buf(csi, return_status);
+	imx7_csi_free_dma_buf(csi, &csi->underrun_buf);
+}
+
+static void imx7_csi_dma_stop(struct imx7_csi *csi)
+{
+	unsigned long timeout_jiffies;
+	unsigned long flags;
+	int ret;
+
+	/* mark next EOF interrupt as the last before stream off */
+	spin_lock_irqsave(&csi->irqlock, flags);
+	csi->last_eof = true;
+	spin_unlock_irqrestore(&csi->irqlock, flags);
+
+	/*
+	 * and then wait for interrupt handler to mark completion.
+	 */
+	timeout_jiffies = msecs_to_jiffies(IMX7_CSI_VIDEO_EOF_TIMEOUT);
+	ret = wait_for_completion_timeout(&csi->last_eof_completion,
+					  timeout_jiffies);
+	if (ret == 0)
+		v4l2_warn(&csi->sd, "wait last EOF timeout\n");
+
+	imx7_csi_hw_disable_irq(csi);
+}
+
+static void imx7_csi_configure(struct imx7_csi *csi)
+{
+	struct v4l2_pix_format *out_pix = &csi->vdev_fmt;
+	int width = out_pix->width;
+	u32 stride = 0;
+	u32 cr3 = BIT_FRMCNT_RST;
+	u32 cr1, cr18;
+
+	cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
+
+	cr18 &= ~(BIT_CSI_HW_ENABLE | BIT_MIPI_DATA_FORMAT_MASK |
+		  BIT_DATA_FROM_MIPI | BIT_MIPI_DOUBLE_CMPNT |
+		  BIT_BASEADDR_CHG_ERR_EN | BIT_BASEADDR_SWITCH_SEL |
+		  BIT_BASEADDR_SWITCH_EN | BIT_DEINTERLACE_EN);
+
+	if (out_pix->field == V4L2_FIELD_INTERLACED) {
+		cr18 |= BIT_DEINTERLACE_EN;
+		stride = out_pix->width;
+	}
+
+	if (!csi->is_csi2) {
+		cr1 = BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE | BIT_HSYNC_POL
+		    | BIT_FCC | BIT_MCLKDIV(1) | BIT_MCLKEN;
+
+		cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
+			BIT_BASEADDR_CHG_ERR_EN;
+
+		if (out_pix->pixelformat == V4L2_PIX_FMT_UYVY ||
+		    out_pix->pixelformat == V4L2_PIX_FMT_YUYV)
+			width *= 2;
+	} else {
+		cr1 = BIT_SOF_POL | BIT_REDGE | BIT_HSYNC_POL | BIT_FCC
+		    | BIT_MCLKDIV(1) | BIT_MCLKEN;
+
+		cr18 |= BIT_DATA_FROM_MIPI;
+
+		switch (csi->format_mbus[IMX7_CSI_PAD_SINK].code) {
+		case MEDIA_BUS_FMT_Y8_1X8:
+		case MEDIA_BUS_FMT_SBGGR8_1X8:
+		case MEDIA_BUS_FMT_SGBRG8_1X8:
+		case MEDIA_BUS_FMT_SGRBG8_1X8:
+		case MEDIA_BUS_FMT_SRGGB8_1X8:
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
+			break;
+		case MEDIA_BUS_FMT_Y10_1X10:
+		case MEDIA_BUS_FMT_SBGGR10_1X10:
+		case MEDIA_BUS_FMT_SGBRG10_1X10:
+		case MEDIA_BUS_FMT_SGRBG10_1X10:
+		case MEDIA_BUS_FMT_SRGGB10_1X10:
+			cr3 |= BIT_TWO_8BIT_SENSOR;
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
+			break;
+		case MEDIA_BUS_FMT_Y12_1X12:
+		case MEDIA_BUS_FMT_SBGGR12_1X12:
+		case MEDIA_BUS_FMT_SGBRG12_1X12:
+		case MEDIA_BUS_FMT_SGRBG12_1X12:
+		case MEDIA_BUS_FMT_SRGGB12_1X12:
+			cr3 |= BIT_TWO_8BIT_SENSOR;
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
+			break;
+		case MEDIA_BUS_FMT_Y14_1X14:
+		case MEDIA_BUS_FMT_SBGGR14_1X14:
+		case MEDIA_BUS_FMT_SGBRG14_1X14:
+		case MEDIA_BUS_FMT_SGRBG14_1X14:
+		case MEDIA_BUS_FMT_SRGGB14_1X14:
+			cr3 |= BIT_TWO_8BIT_SENSOR;
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
+			break;
+
+		/*
+		 * The CSI bridge has a 16-bit input bus. Depending on the
+		 * connected source, data may be transmitted with 8 or 10 bits
+		 * per clock sample (in bits [9:2] or [9:0] respectively) or
+		 * with 16 bits per clock sample (in bits [15:0]). The data is
+		 * then packed into a 32-bit FIFO (as shown in figure 13-11 of
+		 * the i.MX8MM reference manual rev. 3).
+		 *
+		 * The data packing in a 32-bit FIFO input word is controlled by
+		 * the CR3 TWO_8BIT_SENSOR field (also known as SENSOR_16BITS in
+		 * the i.MX8MM reference manual). When set to 0, data packing
+		 * groups four 8-bit input samples (bits [9:2]). When set to 1,
+		 * data packing groups two 16-bit input samples (bits [15:0]).
+		 *
+		 * The register field CR18 MIPI_DOUBLE_CMPNT also needs to be
+		 * configured according to the input format for YUV 4:2:2 data.
+		 * The field controls the gasket between the CSI-2 receiver and
+		 * the CSI bridge. On i.MX7 and i.MX8MM, the field must be set
+		 * to 1 when the CSIS outputs 16-bit samples. On i.MX8MQ, the
+		 * gasket ignores the MIPI_DOUBLE_CMPNT bit and YUV 4:2:2 always
+		 * uses 16-bit samples. Setting MIPI_DOUBLE_CMPNT in that case
+		 * has no effect, but doesn't cause any issue.
+		 */
+		case MEDIA_BUS_FMT_UYVY8_2X8:
+		case MEDIA_BUS_FMT_YUYV8_2X8:
+			cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
+			break;
+		case MEDIA_BUS_FMT_UYVY8_1X16:
+		case MEDIA_BUS_FMT_YUYV8_1X16:
+			cr3 |= BIT_TWO_8BIT_SENSOR;
+			cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B |
+				BIT_MIPI_DOUBLE_CMPNT;
+			break;
+		}
+	}
+
+	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
+	imx7_csi_reg_write(csi, BIT_DMA_BURST_TYPE_RFF_INCR16, CSI_CSICR2);
+	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
+	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
+
+	imx7_csi_reg_write(csi, (width * out_pix->height) >> 2, CSI_CSIRXCNT);
+	imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(width) |
+			   BIT_IMAGE_HEIGHT(out_pix->height),
+			   CSI_CSIIMAG_PARA);
+	imx7_csi_reg_write(csi, stride, CSI_CSIFBUF_PARA);
+}
+
+static int imx7_csi_init(struct imx7_csi *csi)
+{
+	int ret;
+
+	ret = clk_prepare_enable(csi->mclk);
+	if (ret < 0)
+		return ret;
+
+	imx7_csi_configure(csi);
+
+	ret = imx7_csi_dma_setup(csi);
+	if (ret < 0) {
+		clk_disable_unprepare(csi->mclk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void imx7_csi_deinit(struct imx7_csi *csi,
+			    enum vb2_buffer_state return_status)
+{
+	imx7_csi_dma_cleanup(csi, return_status);
+	imx7_csi_init_default(csi);
+	imx7_csi_dmareq_rff_disable(csi);
+	clk_disable_unprepare(csi->mclk);
+}
+
+static void imx7_csi_baseaddr_switch_on_second_frame(struct imx7_csi *csi)
+{
+	u32 cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
+
+	cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
+		BIT_BASEADDR_CHG_ERR_EN;
+	cr18 |= BIT_MASK_OPTION_SECOND_FRAME;
+	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
+}
+
+static void imx7_csi_enable(struct imx7_csi *csi)
+{
+	/* Clear the Rx FIFO and reflash the DMA controller. */
+	imx7_csi_rx_fifo_clear(csi);
+	imx7_csi_dma_reflash(csi);
+
+	usleep_range(2000, 3000);
+
+	/* Clear and enable the interrupts. */
+	imx7_csi_irq_clear(csi);
+	imx7_csi_hw_enable_irq(csi);
+
+	/* Enable the RxFIFO DMA and the CSI. */
+	imx7_csi_dmareq_rff_enable(csi);
+	imx7_csi_hw_enable(csi);
+
+	if (csi->model == IMX7_CSI_IMX8MQ)
+		imx7_csi_baseaddr_switch_on_second_frame(csi);
+}
+
+static void imx7_csi_disable(struct imx7_csi *csi)
+{
+	imx7_csi_dma_stop(csi);
+
+	imx7_csi_dmareq_rff_disable(csi);
+
+	imx7_csi_hw_disable_irq(csi);
+
+	imx7_csi_hw_disable(csi);
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt Handling
+ */
+
+static void imx7_csi_error_recovery(struct imx7_csi *csi)
+{
+	imx7_csi_hw_disable(csi);
+
+	imx7_csi_rx_fifo_clear(csi);
+
+	imx7_csi_dma_reflash(csi);
+
+	imx7_csi_hw_enable(csi);
+}
+
+static void imx7_csi_vb2_buf_done(struct imx7_csi *csi)
+{
+	struct imx7_csi_vb2_buffer *done, *next;
+	struct vb2_buffer *vb;
+	dma_addr_t phys;
+
+	done = csi->active_vb2_buf[csi->buf_num];
+	if (done) {
+		done->vbuf.field = csi->vdev_fmt.field;
+		done->vbuf.sequence = csi->frame_sequence;
+		vb = &done->vbuf.vb2_buf;
+		vb->timestamp = ktime_get_ns();
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	}
+	csi->frame_sequence++;
+
+	/* get next queued buffer */
+	next = imx7_csi_video_next_buf(csi);
+	if (next) {
+		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
+		csi->active_vb2_buf[csi->buf_num] = next;
+	} else {
+		phys = csi->underrun_buf.phys;
+		csi->active_vb2_buf[csi->buf_num] = NULL;
+	}
+
+	imx7_csi_update_buf(csi, phys, csi->buf_num);
+}
+
+static irqreturn_t imx7_csi_irq_handler(int irq, void *data)
+{
+	struct imx7_csi *csi =  data;
+	u32 status;
+
+	spin_lock(&csi->irqlock);
+
+	status = imx7_csi_irq_clear(csi);
+
+	if (status & BIT_RFF_OR_INT) {
+		dev_warn(csi->dev, "Rx fifo overflow\n");
+		imx7_csi_error_recovery(csi);
+	}
+
+	if (status & BIT_HRESP_ERR_INT) {
+		dev_warn(csi->dev, "Hresponse error detected\n");
+		imx7_csi_error_recovery(csi);
+	}
+
+	if (status & BIT_ADDR_CH_ERR_INT) {
+		imx7_csi_hw_disable(csi);
+
+		imx7_csi_dma_reflash(csi);
+
+		imx7_csi_hw_enable(csi);
+	}
+
+	if ((status & BIT_DMA_TSF_DONE_FB1) &&
+	    (status & BIT_DMA_TSF_DONE_FB2)) {
+		/*
+		 * For both FB1 and FB2 interrupter bits set case,
+		 * CSI DMA is work in one of FB1 and FB2 buffer,
+		 * but software can not know the state.
+		 * Skip it to avoid base address updated
+		 * when csi work in field0 and field1 will write to
+		 * new base address.
+		 */
+	} else if (status & BIT_DMA_TSF_DONE_FB1) {
+		csi->buf_num = 0;
+	} else if (status & BIT_DMA_TSF_DONE_FB2) {
+		csi->buf_num = 1;
+	}
+
+	if ((status & BIT_DMA_TSF_DONE_FB1) ||
+	    (status & BIT_DMA_TSF_DONE_FB2)) {
+		imx7_csi_vb2_buf_done(csi);
+
+		if (csi->last_eof) {
+			complete(&csi->last_eof_completion);
+			csi->last_eof = false;
+		}
+	}
+
+	spin_unlock(&csi->irqlock);
+
+	return IRQ_HANDLED;
+}
+
+/* -----------------------------------------------------------------------------
+ * Format Helpers
+ */
+
+#define IMX_BUS_FMTS(fmt...) (const u32[]) {fmt, 0}
+
+/*
+ * List of supported pixel formats for the subdevs. Keep V4L2_PIX_FMT_UYVY and
+ * MEDIA_BUS_FMT_UYVY8_2X8 first to match IMX7_CSI_DEF_PIX_FORMAT and
+ * IMX7_CSI_DEF_MBUS_CODE.
+ */
+static const struct imx7_csi_pixfmt pixel_formats[] = {
+	/*** YUV formats start here ***/
+	{
+		.fourcc	= V4L2_PIX_FMT_UYVY,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_UYVY8_2X8,
+			MEDIA_BUS_FMT_UYVY8_1X16
+		),
+		.yuv	= true,
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.codes  = IMX_BUS_FMTS(
+			MEDIA_BUS_FMT_YUYV8_2X8,
+			MEDIA_BUS_FMT_YUYV8_1X16
+		),
+		.yuv	= true,
+		.bpp    = 16,
+	},
+	/*** raw bayer and grayscale formats start here ***/
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR10,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG10,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG10,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB10,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR12,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG12,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG12,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB12,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR14,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG14,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG14,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB14,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y10,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y12,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y14,
+		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y14_1X14),
+		.bpp    = 16,
+	},
+};
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given fourcc
+ * return it.
+ */
+static const struct imx7_csi_pixfmt *imx7_csi_find_pixel_format(u32 fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
+
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given media
+ * bus code and return it.
+ */
+static const struct imx7_csi_pixfmt *imx7_csi_find_mbus_format(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
+		unsigned int j;
+
+		if (!fmt->codes)
+			continue;
+
+		for (j = 0; fmt->codes[j]; j++) {
+			if (code == fmt->codes[j])
+				return fmt;
+		}
+	}
+
+	return NULL;
+}
+
+/*
+ * Enumerate entries in the pixel_formats[] array that match the
+ * requested search criteria. Return the media-bus code that matches
+ * the search criteria at the requested match index.
+ *
+ * @code: The returned media-bus code that matches the search criteria at
+ *        the requested match index.
+ * @index: The requested match index.
+ */
+static int imx7_csi_enum_mbus_formats(u32 *code, u32 index)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
+		unsigned int j;
+
+		if (!fmt->codes)
+			continue;
+
+		for (j = 0; fmt->codes[j]; j++) {
+			if (index == 0) {
+				*code = fmt->codes[j];
+				return 0;
+			}
+
+			index--;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int imx7_csi_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+					const struct v4l2_mbus_framefmt *mbus,
+					const struct imx7_csi_pixfmt *cc)
+{
+	u32 width;
+	u32 stride;
+
+	if (!cc) {
+		cc = imx7_csi_find_mbus_format(mbus->code);
+		if (!cc)
+			return -EINVAL;
+	}
+
+	/* Round up width for minimum burst size */
+	width = round_up(mbus->width, 8);
+
+	/* Round up stride for IDMAC line start address alignment */
+	stride = round_up((width * cc->bpp) >> 3, 8);
+
+	pix->width = width;
+	pix->height = mbus->height;
+	pix->pixelformat = cc->fourcc;
+	pix->colorspace = mbus->colorspace;
+	pix->xfer_func = mbus->xfer_func;
+	pix->ycbcr_enc = mbus->ycbcr_enc;
+	pix->quantization = mbus->quantization;
+	pix->field = mbus->field;
+	pix->bytesperline = stride;
+	pix->sizeimage = stride * pix->height;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video Capture Device - IOCTLs
+ */
+
+static int imx7_csi_video_querycap(struct file *file, void *fh,
+				   struct v4l2_capability *cap)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+
+	strscpy(cap->driver, IMX7_CSI_VIDEO_NAME, sizeof(cap->driver));
+	strscpy(cap->card, IMX7_CSI_VIDEO_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", dev_name(csi->dev));
+
+	return 0;
+}
+
+static int imx7_csi_video_enum_fmt_vid_cap(struct file *file, void *fh,
+					   struct v4l2_fmtdesc *f)
+{
+	unsigned int index = f->index;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
+
+		/*
+		 * If a media bus code is specified, only consider formats that
+		 * match it.
+		 */
+		if (f->mbus_code) {
+			unsigned int j;
+
+			if (!fmt->codes)
+				continue;
+
+			for (j = 0; fmt->codes[j]; j++) {
+				if (f->mbus_code == fmt->codes[j])
+					break;
+			}
+
+			if (!fmt->codes[j])
+				continue;
+		}
+
+		if (index == 0) {
+			f->pixelformat = fmt->fourcc;
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
+}
+
+static int imx7_csi_video_enum_framesizes(struct file *file, void *fh,
+					  struct v4l2_frmsizeenum *fsize)
+{
+	const struct imx7_csi_pixfmt *cc;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	cc = imx7_csi_find_pixel_format(fsize->pixel_format);
+	if (!cc)
+		return -EINVAL;
+
+	/*
+	 * TODO: The constraints are hardware-specific and may depend on the
+	 * pixel format. This should come from the driver using
+	 * imx_media_capture.
+	 */
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = 1;
+	fsize->stepwise.max_width = 65535;
+	fsize->stepwise.min_height = 1;
+	fsize->stepwise.max_height = 65535;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int imx7_csi_video_g_fmt_vid_cap(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+
+	f->fmt.pix = csi->vdev_fmt;
+
+	return 0;
+}
+
+static const struct imx7_csi_pixfmt *
+__imx7_csi_video_try_fmt(struct v4l2_pix_format *pixfmt,
+			 struct v4l2_rect *compose)
+{
+	struct v4l2_mbus_framefmt fmt_src;
+	const struct imx7_csi_pixfmt *cc;
+
+	/*
+	 * Find the pixel format, default to the first supported format if not
+	 * found.
+	 */
+	cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
+	if (!cc) {
+		pixfmt->pixelformat = IMX7_CSI_DEF_PIX_FORMAT;
+		cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
+	}
+
+	/* Allow IDMAC interweave but enforce field order from source. */
+	if (V4L2_FIELD_IS_INTERLACED(pixfmt->field)) {
+		switch (pixfmt->field) {
+		case V4L2_FIELD_SEQ_TB:
+			pixfmt->field = V4L2_FIELD_INTERLACED_TB;
+			break;
+		case V4L2_FIELD_SEQ_BT:
+			pixfmt->field = V4L2_FIELD_INTERLACED_BT;
+			break;
+		default:
+			break;
+		}
+	}
+
+	v4l2_fill_mbus_format(&fmt_src, pixfmt, 0);
+	imx7_csi_mbus_fmt_to_pix_fmt(pixfmt, &fmt_src, cc);
+
+	if (compose) {
+		compose->width = fmt_src.width;
+		compose->height = fmt_src.height;
+	}
+
+	return cc;
+}
+
+static int imx7_csi_video_try_fmt_vid_cap(struct file *file, void *fh,
+					  struct v4l2_format *f)
+{
+	__imx7_csi_video_try_fmt(&f->fmt.pix, NULL);
+	return 0;
+}
+
+static int imx7_csi_video_s_fmt_vid_cap(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+	const struct imx7_csi_pixfmt *cc;
+
+	if (vb2_is_busy(&csi->q)) {
+		dev_err(csi->dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	cc = __imx7_csi_video_try_fmt(&f->fmt.pix, &csi->vdev_compose);
+
+	csi->vdev_cc = cc;
+	csi->vdev_fmt = f->fmt.pix;
+
+	return 0;
+}
+
+static int imx7_csi_video_g_selection(struct file *file, void *fh,
+				      struct v4l2_selection *s)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		/* The compose rectangle is fixed to the source format. */
+		s->r = csi->vdev_compose;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		/*
+		 * The hardware writes with a configurable but fixed DMA burst
+		 * size. If the source format width is not burst size aligned,
+		 * the written frame contains padding to the right.
+		 */
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = csi->vdev_fmt.width;
+		s->r.height = csi->vdev_fmt.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops imx7_csi_video_ioctl_ops = {
+	.vidioc_querycap		= imx7_csi_video_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= imx7_csi_video_enum_fmt_vid_cap,
+	.vidioc_enum_framesizes		= imx7_csi_video_enum_framesizes,
+
+	.vidioc_g_fmt_vid_cap		= imx7_csi_video_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= imx7_csi_video_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= imx7_csi_video_s_fmt_vid_cap,
+
+	.vidioc_g_selection		= imx7_csi_video_g_selection,
+
+	.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,
+};
+
+/* -----------------------------------------------------------------------------
+ * Video Capture Device - Queue Operations
+ */
+
+static int imx7_csi_video_queue_setup(struct vb2_queue *vq,
+				      unsigned int *nbuffers,
+				      unsigned int *nplanes,
+				      unsigned int sizes[],
+				      struct device *alloc_devs[])
+{
+	struct imx7_csi *csi = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &csi->vdev_fmt;
+	unsigned int count = *nbuffers;
+
+	if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (*nplanes) {
+		if (*nplanes != 1 || sizes[0] < pix->sizeimage)
+			return -EINVAL;
+		count += vq->num_buffers;
+	}
+
+	count = min_t(__u32, IMX7_CSI_VIDEO_MEM_LIMIT / pix->sizeimage, count);
+
+	if (*nplanes)
+		*nbuffers = (count < vq->num_buffers) ? 0 :
+			count - vq->num_buffers;
+	else
+		*nbuffers = count;
+
+	*nplanes = 1;
+	sizes[0] = pix->sizeimage;
+
+	return 0;
+}
+
+static int imx7_csi_video_buf_init(struct vb2_buffer *vb)
+{
+	struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
+
+static int imx7_csi_video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_pix_format *pix = &csi->vdev_fmt;
+
+	if (vb2_plane_size(vb, 0) < pix->sizeimage) {
+		dev_err(csi->dev,
+			"data will not fit into plane (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), (long)pix->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, pix->sizeimage);
+
+	return 0;
+}
+
+static void imx7_csi_video_buf_queue(struct vb2_buffer *vb)
+{
+	struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
+	struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
+	unsigned long flags;
+
+	spin_lock_irqsave(&csi->q_lock, flags);
+
+	list_add_tail(&buf->list, &csi->ready_q);
+
+	spin_unlock_irqrestore(&csi->q_lock, flags);
+}
+
+static int imx7_csi_video_validate_fmt(struct imx7_csi *csi)
+{
+	struct v4l2_subdev_format fmt_src;
+	const struct imx7_csi_pixfmt *cc;
+	int ret;
+
+	/* Retrieve the media bus format on the source subdev. */
+	fmt_src.pad = IMX7_CSI_PAD_SRC;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(&csi->sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret)
+		return ret;
+
+	/*
+	 * Verify that the media bus size matches the size set on the video
+	 * node. It is sufficient to check the compose rectangle size without
+	 * checking the rounded size from pix_fmt, as the rounded size is
+	 * derived directly from the compose rectangle size, and will thus
+	 * always match if the compose rectangle matches.
+	 */
+	if (csi->vdev_compose.width != fmt_src.format.width ||
+	    csi->vdev_compose.height != fmt_src.format.height)
+		return -EPIPE;
+
+	/*
+	 * Verify that the media bus code is compatible with the pixel format
+	 * set on the video node.
+	 */
+	cc = imx7_csi_find_mbus_format(fmt_src.format.code);
+	if (!cc || csi->vdev_cc->yuv != cc->yuv)
+		return -EPIPE;
+
+	return 0;
+}
+
+static int imx7_csi_video_start_streaming(struct vb2_queue *vq,
+					  unsigned int count)
+{
+	struct imx7_csi *csi = vb2_get_drv_priv(vq);
+	struct imx7_csi_vb2_buffer *buf, *tmp;
+	unsigned long flags;
+	int ret;
+
+	ret = imx7_csi_video_validate_fmt(csi);
+	if (ret) {
+		dev_err(csi->dev, "capture format not valid\n");
+		goto err_buffers;
+	}
+
+	mutex_lock(&csi->mdev.graph_mutex);
+
+	ret = __video_device_pipeline_start(csi->vdev, &csi->pipe);
+	if (ret)
+		goto err_unlock;
+
+	ret = v4l2_subdev_call(&csi->sd, video, s_stream, 1);
+	if (ret)
+		goto err_stop;
+
+	mutex_unlock(&csi->mdev.graph_mutex);
+
+	return 0;
+
+err_stop:
+	__video_device_pipeline_stop(csi->vdev);
+err_unlock:
+	mutex_unlock(&csi->mdev.graph_mutex);
+	dev_err(csi->dev, "pipeline start failed with %d\n", ret);
+err_buffers:
+	spin_lock_irqsave(&csi->q_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &csi->ready_q, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&csi->q_lock, flags);
+	return ret;
+}
+
+static void imx7_csi_video_stop_streaming(struct vb2_queue *vq)
+{
+	struct imx7_csi *csi = vb2_get_drv_priv(vq);
+	struct imx7_csi_vb2_buffer *frame;
+	struct imx7_csi_vb2_buffer *tmp;
+	unsigned long flags;
+
+	mutex_lock(&csi->mdev.graph_mutex);
+	v4l2_subdev_call(&csi->sd, video, s_stream, 0);
+	__video_device_pipeline_stop(csi->vdev);
+	mutex_unlock(&csi->mdev.graph_mutex);
+
+	/* release all active buffers */
+	spin_lock_irqsave(&csi->q_lock, flags);
+	list_for_each_entry_safe(frame, tmp, &csi->ready_q, list) {
+		list_del(&frame->list);
+		vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&csi->q_lock, flags);
+}
+
+static const struct vb2_ops imx7_csi_video_qops = {
+	.queue_setup	 = imx7_csi_video_queue_setup,
+	.buf_init        = imx7_csi_video_buf_init,
+	.buf_prepare	 = imx7_csi_video_buf_prepare,
+	.buf_queue	 = imx7_csi_video_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = imx7_csi_video_start_streaming,
+	.stop_streaming  = imx7_csi_video_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * Video Capture Device - File Operations
+ */
+
+static int imx7_csi_video_open(struct file *file)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+	int ret;
+
+	if (mutex_lock_interruptible(&csi->vdev_mutex))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret) {
+		dev_err(csi->dev, "v4l2_fh_open failed\n");
+		goto out;
+	}
+
+	ret = v4l2_pipeline_pm_get(&csi->vdev->entity);
+	if (ret)
+		v4l2_fh_release(file);
+
+out:
+	mutex_unlock(&csi->vdev_mutex);
+	return ret;
+}
+
+static int imx7_csi_video_release(struct file *file)
+{
+	struct imx7_csi *csi = video_drvdata(file);
+	struct vb2_queue *vq = &csi->q;
+
+	mutex_lock(&csi->vdev_mutex);
+
+	if (file->private_data == vq->owner) {
+		vb2_queue_release(vq);
+		vq->owner = NULL;
+	}
+
+	v4l2_pipeline_pm_put(&csi->vdev->entity);
+
+	v4l2_fh_release(file);
+	mutex_unlock(&csi->vdev_mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations imx7_csi_video_fops = {
+	.owner		= THIS_MODULE,
+	.open		= imx7_csi_video_open,
+	.release	= imx7_csi_video_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * Video Capture Device - Init & Cleanup
+ */
+
+static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi)
+{
+	struct imx7_csi_vb2_buffer *buf = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&csi->q_lock, flags);
+
+	/* get next queued buffer */
+	if (!list_empty(&csi->ready_q)) {
+		buf = list_entry(csi->ready_q.next, struct imx7_csi_vb2_buffer,
+				 list);
+		list_del(&buf->list);
+	}
+
+	spin_unlock_irqrestore(&csi->q_lock, flags);
+
+	return buf;
+}
+
+static int imx7_csi_video_init_format(struct imx7_csi *csi)
+{
+	struct v4l2_subdev_format fmt_src = {
+		.pad = IMX7_CSI_PAD_SRC,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	fmt_src.format.code = IMX7_CSI_DEF_MBUS_CODE;
+	fmt_src.format.width = IMX7_CSI_DEF_PIX_WIDTH;
+	fmt_src.format.height = IMX7_CSI_DEF_PIX_HEIGHT;
+
+	imx7_csi_mbus_fmt_to_pix_fmt(&csi->vdev_fmt, &fmt_src.format, NULL);
+	csi->vdev_compose.width = fmt_src.format.width;
+	csi->vdev_compose.height = fmt_src.format.height;
+
+	csi->vdev_cc = imx7_csi_find_pixel_format(csi->vdev_fmt.pixelformat);
+
+	return 0;
+}
+
+static int imx7_csi_video_register(struct imx7_csi *csi)
+{
+	struct v4l2_subdev *sd = &csi->sd;
+	struct v4l2_device *v4l2_dev = sd->v4l2_dev;
+	struct video_device *vdev = csi->vdev;
+	int ret;
+
+	vdev->v4l2_dev = v4l2_dev;
+
+	/* Initialize the default format and compose rectangle. */
+	ret = imx7_csi_video_init_format(csi);
+	if (ret < 0)
+		return ret;
+
+	/* Register the video device. */
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(csi->dev, "Failed to register video device\n");
+		return ret;
+	}
+
+	dev_info(csi->dev, "Registered %s as /dev/%s\n", vdev->name,
+		 video_device_node_name(vdev));
+
+	/* Create the link from the CSI subdev to the video device. */
+	ret = media_create_pad_link(&sd->entity, IMX7_CSI_PAD_SRC,
+				    &vdev->entity, 0, MEDIA_LNK_FL_IMMUTABLE |
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret) {
+		dev_err(csi->dev, "failed to create link to device node\n");
+		video_unregister_device(vdev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void imx7_csi_video_unregister(struct imx7_csi *csi)
+{
+	media_entity_cleanup(&csi->vdev->entity);
+	video_unregister_device(csi->vdev);
+}
+
+static int imx7_csi_video_init(struct imx7_csi *csi)
+{
+	struct video_device *vdev;
+	struct vb2_queue *vq;
+	int ret;
+
+	mutex_init(&csi->vdev_mutex);
+	INIT_LIST_HEAD(&csi->ready_q);
+	spin_lock_init(&csi->q_lock);
+
+	/* Allocate and initialize the video device. */
+	vdev = video_device_alloc();
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->fops = &imx7_csi_video_fops;
+	vdev->ioctl_ops = &imx7_csi_video_ioctl_ops;
+	vdev->minor = -1;
+	vdev->release = video_device_release;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
+			 | V4L2_CAP_IO_MC;
+	vdev->lock = &csi->vdev_mutex;
+	vdev->queue = &csi->q;
+
+	snprintf(vdev->name, sizeof(vdev->name), "%s capture", csi->sd.name);
+
+	video_set_drvdata(vdev, csi);
+	csi->vdev = vdev;
+
+	/* Initialize the video device pad. */
+	csi->vdev_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, &csi->vdev_pad);
+	if (ret) {
+		video_device_release(vdev);
+		return ret;
+	}
+
+	/* Initialize the vb2 queue. */
+	vq = &csi->q;
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	vq->drv_priv = csi;
+	vq->buf_struct_size = sizeof(struct imx7_csi_vb2_buffer);
+	vq->ops = &imx7_csi_video_qops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->lock = &csi->vdev_mutex;
+	vq->min_buffers_needed = 2;
+	vq->dev = csi->dev;
+
+	ret = vb2_queue_init(vq);
+	if (ret) {
+		dev_err(csi->dev, "vb2_queue_init failed\n");
+		video_device_release(vdev);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdev Operations
+ */
+
+static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&csi->lock);
+
+	if (!csi->src_sd) {
+		ret = -EPIPE;
+		goto out_unlock;
+	}
+
+	if (csi->is_streaming == !!enable)
+		goto out_unlock;
+
+	if (enable) {
+		ret = imx7_csi_init(csi);
+		if (ret < 0)
+			goto out_unlock;
+
+		ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
+		if (ret < 0) {
+			imx7_csi_deinit(csi, VB2_BUF_STATE_QUEUED);
+			goto out_unlock;
+		}
+
+		imx7_csi_enable(csi);
+	} else {
+		imx7_csi_disable(csi);
+
+		v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
+
+		imx7_csi_deinit(csi, VB2_BUF_STATE_ERROR);
+	}
+
+	csi->is_streaming = !!enable;
+
+out_unlock:
+	mutex_unlock(&csi->lock);
+
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+imx7_csi_get_format(struct imx7_csi *csi,
+		    struct v4l2_subdev_state *sd_state,
+		    unsigned int pad,
+		    enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi->sd, sd_state, pad);
+
+	return &csi->format_mbus[pad];
+}
+
+static int imx7_csi_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
+{
+	const enum v4l2_subdev_format_whence which =
+		sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	const struct imx7_csi_pixfmt *cc;
+	int i;
+
+	cc = imx7_csi_find_mbus_format(IMX7_CSI_DEF_MBUS_CODE);
+
+	for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
+		struct v4l2_mbus_framefmt *mf =
+			imx7_csi_get_format(csi, sd_state, i, which);
+
+		mf->code = IMX7_CSI_DEF_MBUS_CODE;
+		mf->width = IMX7_CSI_DEF_PIX_WIDTH;
+		mf->height = IMX7_CSI_DEF_PIX_HEIGHT;
+		mf->field = V4L2_FIELD_NONE;
+
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		mf->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mf->colorspace);
+		mf->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mf->colorspace);
+		mf->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(!cc->yuv,
+					mf->colorspace, mf->ycbcr_enc);
+
+		csi->cc[i] = cc;
+	}
+
+	return 0;
+}
+
+static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *in_fmt;
+	int ret = 0;
+
+	mutex_lock(&csi->lock);
+
+	in_fmt = imx7_csi_get_format(csi, sd_state, IMX7_CSI_PAD_SINK,
+				     code->which);
+
+	switch (code->pad) {
+	case IMX7_CSI_PAD_SINK:
+		ret = imx7_csi_enum_mbus_formats(&code->code, code->index);
+		break;
+	case IMX7_CSI_PAD_SRC:
+		if (code->index != 0) {
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
+		code->code = in_fmt->code;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+out_unlock:
+	mutex_unlock(&csi->lock);
+
+	return ret;
+}
+
+static int imx7_csi_get_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *sdformat)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	mutex_lock(&csi->lock);
+
+	fmt = imx7_csi_get_format(csi, sd_state, sdformat->pad,
+				  sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	sdformat->format = *fmt;
+
+out_unlock:
+	mutex_unlock(&csi->lock);
+
+	return ret;
+}
+
+/*
+ * Default the colorspace in tryfmt to SRGB if set to an unsupported
+ * colorspace or not initialized. Then set the remaining colorimetry
+ * parameters based on the colorspace if they are uninitialized.
+ *
+ * tryfmt->code must be set on entry.
+ */
+static void imx7_csi_try_colorimetry(struct v4l2_mbus_framefmt *tryfmt)
+{
+	const struct imx7_csi_pixfmt *cc;
+	bool is_rgb = false;
+
+	cc = imx7_csi_find_mbus_format(tryfmt->code);
+	if (cc && !cc->yuv)
+		is_rgb = true;
+
+	switch (tryfmt->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_JPEG:
+	case V4L2_COLORSPACE_SRGB:
+	case V4L2_COLORSPACE_BT2020:
+	case V4L2_COLORSPACE_OPRGB:
+	case V4L2_COLORSPACE_DCI_P3:
+	case V4L2_COLORSPACE_RAW:
+		break;
+	default:
+		tryfmt->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	}
+
+	if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT)
+		tryfmt->xfer_func =
+			V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace);
+
+	if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
+		tryfmt->ycbcr_enc =
+			V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace);
+
+	if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT)
+		tryfmt->quantization =
+			V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb,
+						      tryfmt->colorspace,
+						      tryfmt->ycbcr_enc);
+}
+
+static int imx7_csi_try_fmt(struct imx7_csi *csi,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *sdformat,
+			    const struct imx7_csi_pixfmt **cc)
+{
+	const struct imx7_csi_pixfmt *in_cc;
+	struct v4l2_mbus_framefmt *in_fmt;
+	u32 code;
+
+	in_fmt = imx7_csi_get_format(csi, sd_state, IMX7_CSI_PAD_SINK,
+				     sdformat->which);
+	if (!in_fmt)
+		return -EINVAL;
+
+	switch (sdformat->pad) {
+	case IMX7_CSI_PAD_SRC:
+		in_cc = imx7_csi_find_mbus_format(in_fmt->code);
+
+		sdformat->format.width = in_fmt->width;
+		sdformat->format.height = in_fmt->height;
+		sdformat->format.code = in_fmt->code;
+		sdformat->format.field = in_fmt->field;
+		*cc = in_cc;
+
+		sdformat->format.colorspace = in_fmt->colorspace;
+		sdformat->format.xfer_func = in_fmt->xfer_func;
+		sdformat->format.quantization = in_fmt->quantization;
+		sdformat->format.ycbcr_enc = in_fmt->ycbcr_enc;
+		break;
+	case IMX7_CSI_PAD_SINK:
+		*cc = imx7_csi_find_mbus_format(sdformat->format.code);
+		if (!*cc) {
+			code = IMX7_CSI_DEF_MBUS_CODE;
+			*cc = imx7_csi_find_mbus_format(code);
+			sdformat->format.code = code;
+		}
+
+		if (sdformat->format.field != V4L2_FIELD_INTERLACED)
+			sdformat->format.field = V4L2_FIELD_NONE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	imx7_csi_try_colorimetry(&sdformat->format);
+
+	return 0;
+}
+
+static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *sdformat)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	const struct imx7_csi_pixfmt *outcc;
+	struct v4l2_mbus_framefmt *outfmt;
+	const struct imx7_csi_pixfmt *cc;
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_subdev_format format;
+	int ret = 0;
+
+	if (sdformat->pad >= IMX7_CSI_PADS_NUM)
+		return -EINVAL;
+
+	mutex_lock(&csi->lock);
+
+	if (csi->is_streaming) {
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+
+	ret = imx7_csi_try_fmt(csi, sd_state, sdformat, &cc);
+	if (ret < 0)
+		goto out_unlock;
+
+	fmt = imx7_csi_get_format(csi, sd_state, sdformat->pad,
+				  sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	*fmt = sdformat->format;
+
+	if (sdformat->pad == IMX7_CSI_PAD_SINK) {
+		/* propagate format to source pads */
+		format.pad = IMX7_CSI_PAD_SRC;
+		format.which = sdformat->which;
+		format.format = sdformat->format;
+		if (imx7_csi_try_fmt(csi, sd_state, &format, &outcc)) {
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+		outfmt = imx7_csi_get_format(csi, sd_state, IMX7_CSI_PAD_SRC,
+					     sdformat->which);
+		*outfmt = format.format;
+
+		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			csi->cc[IMX7_CSI_PAD_SRC] = outcc;
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		csi->cc[sdformat->pad] = cc;
+
+out_unlock:
+	mutex_unlock(&csi->lock);
+
+	return ret;
+}
+
+static int imx7_csi_pad_link_validate(struct v4l2_subdev *sd,
+				      struct media_link *link,
+				      struct v4l2_subdev_format *source_fmt,
+				      struct v4l2_subdev_format *sink_fmt)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	struct media_pad *pad = NULL;
+	unsigned int i;
+	int ret;
+
+	if (!csi->src_sd)
+		return -EPIPE;
+
+	/*
+	 * Validate the source link, and record whether the source uses the
+	 * parallel input or the CSI-2 receiver.
+	 */
+	ret = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	switch (csi->src_sd->entity.function) {
+	case MEDIA_ENT_F_VID_IF_BRIDGE:
+		/* The input is the CSI-2 receiver. */
+		csi->is_csi2 = true;
+		break;
+
+	case MEDIA_ENT_F_VID_MUX:
+		/* The input is the mux, check its input. */
+		for (i = 0; i < csi->src_sd->entity.num_pads; i++) {
+			struct media_pad *spad = &csi->src_sd->entity.pads[i];
+
+			if (!(spad->flags & MEDIA_PAD_FL_SINK))
+				continue;
+
+			pad = media_pad_remote_pad_first(spad);
+			if (pad)
+				break;
+		}
+
+		if (!pad)
+			return -ENODEV;
+
+		csi->is_csi2 = pad->entity->function == MEDIA_ENT_F_VID_IF_BRIDGE;
+		break;
+
+	default:
+		/*
+		 * The input is an external entity, it must use the parallel
+		 * bus.
+		 */
+		csi->is_csi2 = false;
+		break;
+	}
+
+	return 0;
+}
+
+static int imx7_csi_registered(struct v4l2_subdev *sd)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = imx7_csi_video_init(csi);
+	if (ret)
+		return ret;
+
+	ret = imx7_csi_video_register(csi);
+	if (ret)
+		return ret;
+
+	ret = v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
+	if (ret)
+		goto err_unreg;
+
+	ret = media_device_register(&csi->mdev);
+	if (ret)
+		goto err_unreg;
+
+	return 0;
+
+err_unreg:
+	imx7_csi_video_unregister(csi);
+	return ret;
+}
+
+static void imx7_csi_unregistered(struct v4l2_subdev *sd)
+{
+	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
+
+	imx7_csi_video_unregister(csi);
+}
+
+static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
+	.s_stream	= imx7_csi_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
+	.init_cfg	= imx7_csi_init_cfg,
+	.enum_mbus_code	= imx7_csi_enum_mbus_code,
+	.get_fmt	= imx7_csi_get_fmt,
+	.set_fmt	= imx7_csi_set_fmt,
+	.link_validate	= imx7_csi_pad_link_validate,
+};
+
+static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
+	.video		= &imx7_csi_video_ops,
+	.pad		= &imx7_csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
+	.registered	= imx7_csi_registered,
+	.unregistered	= imx7_csi_unregistered,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media Entity Operations
+ */
+
+static const struct media_entity_operations imx7_csi_entity_ops = {
+	.link_validate	= v4l2_subdev_link_validate,
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe & Remove
+ */
+
+static int imx7_csi_notify_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *sd,
+				 struct v4l2_async_subdev *asd)
+{
+	struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
+	struct media_pad *sink = &csi->sd.entity.pads[IMX7_CSI_PAD_SINK];
+
+	csi->src_sd = sd;
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
+					       MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static int imx7_csi_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
+
+	return v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations imx7_csi_notify_ops = {
+	.bound = imx7_csi_notify_bound,
+	.complete = imx7_csi_notify_complete,
+};
+
+static int imx7_csi_async_register(struct imx7_csi *csi)
+{
+	struct v4l2_async_subdev *asd;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_nf_init(&csi->notifier);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (ep) {
+		asd = v4l2_async_nf_add_fwnode_remote(&csi->notifier, ep,
+						      struct v4l2_async_subdev);
+
+		fwnode_handle_put(ep);
+
+		if (IS_ERR(asd)) {
+			ret = PTR_ERR(asd);
+			/* OK if asd already exists */
+			if (ret != -EEXIST)
+				return ret;
+		}
+	}
+
+	csi->notifier.ops = &imx7_csi_notify_ops;
+
+	ret = v4l2_async_nf_register(&csi->v4l2_dev, &csi->notifier);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void imx7_csi_media_cleanup(struct imx7_csi *csi)
+{
+	v4l2_device_unregister(&csi->v4l2_dev);
+	media_device_unregister(&csi->mdev);
+	media_device_cleanup(&csi->mdev);
+}
+
+static const struct media_device_ops imx7_csi_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+static int imx7_csi_media_dev_init(struct imx7_csi *csi)
+{
+	int ret;
+
+	strscpy(csi->mdev.model, "imx-media", sizeof(csi->mdev.model));
+	csi->mdev.ops = &imx7_csi_media_ops;
+	csi->mdev.dev = csi->dev;
+
+	csi->v4l2_dev.mdev = &csi->mdev;
+	strscpy(csi->v4l2_dev.name, "imx-media",
+		sizeof(csi->v4l2_dev.name));
+	snprintf(csi->mdev.bus_info, sizeof(csi->mdev.bus_info),
+		 "platform:%s", dev_name(csi->mdev.dev));
+
+	media_device_init(&csi->mdev);
+
+	ret = v4l2_device_register(csi->dev, &csi->v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(&csi->v4l2_dev,
+			 "Failed to register v4l2_device: %d\n", ret);
+		goto cleanup;
+	}
+
+	return 0;
+
+cleanup:
+	media_device_cleanup(&csi->mdev);
+
+	return ret;
+}
+
+static int imx7_csi_media_init(struct imx7_csi *csi)
+{
+	unsigned int i;
+	int ret;
+
+	/* add media device */
+	ret = imx7_csi_media_dev_init(csi);
+	if (ret)
+		return ret;
+
+	v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
+	v4l2_set_subdevdata(&csi->sd, csi);
+	csi->sd.internal_ops = &imx7_csi_internal_ops;
+	csi->sd.entity.ops = &imx7_csi_entity_ops;
+	csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csi->sd.dev = csi->dev;
+	csi->sd.owner = THIS_MODULE;
+	csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
+
+	for (i = 0; i < IMX7_CSI_PADS_NUM; i++)
+		csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&csi->sd.entity, IMX7_CSI_PADS_NUM,
+				     csi->pad);
+	if (ret)
+		goto error;
+
+	ret = v4l2_device_register_subdev(&csi->v4l2_dev, &csi->sd);
+	if (ret)
+		goto error;
+
+	return 0;
+
+error:
+	imx7_csi_media_cleanup(csi);
+	return ret;
+}
+
+static int imx7_csi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct imx7_csi *csi;
+	int ret;
+
+	csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
+	if (!csi)
+		return -ENOMEM;
+
+	csi->dev = dev;
+	platform_set_drvdata(pdev, csi);
+
+	spin_lock_init(&csi->irqlock);
+	mutex_init(&csi->lock);
+
+	/* Acquire resources and install interrupt handler. */
+	csi->mclk = devm_clk_get(&pdev->dev, "mclk");
+	if (IS_ERR(csi->mclk)) {
+		ret = PTR_ERR(csi->mclk);
+		dev_err(dev, "Failed to get mclk: %d", ret);
+		goto destroy_mutex;
+	}
+
+	csi->irq = platform_get_irq(pdev, 0);
+	if (csi->irq < 0) {
+		ret = csi->irq;
+		goto destroy_mutex;
+	}
+
+	csi->regbase = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(csi->regbase)) {
+		ret = PTR_ERR(csi->regbase);
+		goto destroy_mutex;
+	}
+
+	csi->model = (enum imx_csi_model)(uintptr_t)of_device_get_match_data(&pdev->dev);
+
+	ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
+			       (void *)csi);
+	if (ret < 0) {
+		dev_err(dev, "Request CSI IRQ failed.\n");
+		goto destroy_mutex;
+	}
+
+	/* Initialize all the media device infrastructure. */
+	ret = imx7_csi_media_init(csi);
+	if (ret)
+		goto destroy_mutex;
+
+	/* Set the default mbus formats. */
+	ret = imx7_csi_init_cfg(&csi->sd, NULL);
+	if (ret)
+		goto media_cleanup;
+
+	ret = imx7_csi_async_register(csi);
+	if (ret)
+		goto subdev_notifier_cleanup;
+
+	return 0;
+
+subdev_notifier_cleanup:
+	v4l2_async_nf_unregister(&csi->notifier);
+	v4l2_async_nf_cleanup(&csi->notifier);
+media_cleanup:
+	imx7_csi_media_cleanup(csi);
+
+destroy_mutex:
+	mutex_destroy(&csi->lock);
+
+	return ret;
+}
+
+static int imx7_csi_remove(struct platform_device *pdev)
+{
+	struct imx7_csi *csi = platform_get_drvdata(pdev);
+
+	imx7_csi_media_cleanup(csi);
+
+	v4l2_async_nf_unregister(&csi->notifier);
+	v4l2_async_nf_cleanup(&csi->notifier);
+	v4l2_async_unregister_subdev(&csi->sd);
+
+	mutex_destroy(&csi->lock);
+
+	return 0;
+}
+
+static const struct of_device_id imx7_csi_of_match[] = {
+	{ .compatible = "fsl,imx8mq-csi", .data = (void *)IMX7_CSI_IMX8MQ },
+	{ .compatible = "fsl,imx7-csi", .data = (void *)IMX7_CSI_IMX7 },
+	{ .compatible = "fsl,imx6ul-csi", .data = (void *)IMX7_CSI_IMX7 },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
+
+static struct platform_driver imx7_csi_driver = {
+	.probe = imx7_csi_probe,
+	.remove = imx7_csi_remove,
+	.driver = {
+		.of_match_table = imx7_csi_of_match,
+		.name = "imx7-csi",
+	},
+};
+module_platform_driver(imx7_csi_driver);
+
+MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
+MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:imx7-csi");
diff --git a/drivers/staging/media/imx/imx8mq-mipi-csi2.c b/drivers/staging/media/imx/imx8mq-mipi-csi2.c
new file mode 100644
index 000000000..83194328d
--- /dev/null
+++ b/drivers/staging/media/imx/imx8mq-mipi-csi2.c
@@ -0,0 +1,1004 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * NXP i.MX8MQ SoC series MIPI-CSI2 receiver driver
+ *
+ * Copyright (C) 2021 Purism SPC
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/interconnect.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+
+#define MIPI_CSI2_DRIVER_NAME			"imx8mq-mipi-csi2"
+#define MIPI_CSI2_SUBDEV_NAME			MIPI_CSI2_DRIVER_NAME
+
+#define MIPI_CSI2_PAD_SINK			0
+#define MIPI_CSI2_PAD_SOURCE			1
+#define MIPI_CSI2_PADS_NUM			2
+
+#define MIPI_CSI2_DEF_PIX_WIDTH			640
+#define MIPI_CSI2_DEF_PIX_HEIGHT		480
+
+/* Register map definition */
+
+/* i.MX8MQ CSI-2 controller CSR */
+#define CSI2RX_CFG_NUM_LANES			0x100
+#define CSI2RX_CFG_DISABLE_DATA_LANES		0x104
+#define CSI2RX_BIT_ERR				0x108
+#define CSI2RX_IRQ_STATUS			0x10c
+#define CSI2RX_IRQ_MASK				0x110
+#define CSI2RX_IRQ_MASK_ALL			0x1ff
+#define CSI2RX_IRQ_MASK_ULPS_STATUS_CHANGE	0x8
+#define CSI2RX_ULPS_STATUS			0x114
+#define CSI2RX_PPI_ERRSOT_HS			0x118
+#define CSI2RX_PPI_ERRSOTSYNC_HS		0x11c
+#define CSI2RX_PPI_ERRESC			0x120
+#define CSI2RX_PPI_ERRSYNCESC			0x124
+#define CSI2RX_PPI_ERRCONTROL			0x128
+#define CSI2RX_CFG_DISABLE_PAYLOAD_0		0x12c
+#define CSI2RX_CFG_VID_VC_IGNORE		0x180
+#define CSI2RX_CFG_VID_VC			0x184
+#define CSI2RX_CFG_VID_P_FIFO_SEND_LEVEL	0x188
+#define CSI2RX_CFG_DISABLE_PAYLOAD_1		0x130
+
+enum {
+	ST_POWERED	= 1,
+	ST_STREAMING	= 2,
+	ST_SUSPENDED	= 4,
+};
+
+enum imx8mq_mipi_csi_clk {
+	CSI2_CLK_CORE,
+	CSI2_CLK_ESC,
+	CSI2_CLK_UI,
+	CSI2_NUM_CLKS,
+};
+
+static const char * const imx8mq_mipi_csi_clk_id[CSI2_NUM_CLKS] = {
+	[CSI2_CLK_CORE] = "core",
+	[CSI2_CLK_ESC] = "esc",
+	[CSI2_CLK_UI] = "ui",
+};
+
+#define CSI2_NUM_CLKS	ARRAY_SIZE(imx8mq_mipi_csi_clk_id)
+
+#define	GPR_CSI2_1_RX_ENABLE		BIT(13)
+#define	GPR_CSI2_1_VID_INTFC_ENB	BIT(12)
+#define	GPR_CSI2_1_HSEL			BIT(10)
+#define	GPR_CSI2_1_CONT_CLK_MODE	BIT(8)
+#define	GPR_CSI2_1_S_PRG_RXHS_SETTLE(x)	(((x) & 0x3f) << 2)
+
+/*
+ * The send level configures the number of entries that must accumulate in
+ * the Pixel FIFO before the data will be transferred to the video output.
+ * The exact value needed for this configuration is dependent on the rate at
+ * which the sensor transfers data to the CSI-2 Controller and the user
+ * video clock.
+ *
+ * The calculation is the classical rate-in rate-out type of problem: If the
+ * video bandwidth is 10% faster than the incoming mipi data and the video
+ * line length is 500 pixels, then the fifo should be allowed to fill
+ * 10% of the line length or 50 pixels. If the gap data is ok, then the level
+ * can be set to 16 and ignored.
+ */
+#define CSI2RX_SEND_LEVEL			64
+
+struct csi_state {
+	struct device *dev;
+	void __iomem *regs;
+	struct clk_bulk_data clks[CSI2_NUM_CLKS];
+	struct reset_control *rst;
+	struct regulator *mipi_phy_regulator;
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[MIPI_CSI2_PADS_NUM];
+	struct v4l2_async_notifier notifier;
+	struct v4l2_subdev *src_sd;
+
+	struct v4l2_mbus_config_mipi_csi2 bus;
+
+	struct mutex lock; /* Protect csi2_fmt, format_mbus, state, hs_settle */
+	const struct csi2_pix_format *csi2_fmt;
+	struct v4l2_mbus_framefmt format_mbus[MIPI_CSI2_PADS_NUM];
+	u32 state;
+	u32 hs_settle;
+
+	struct regmap *phy_gpr;
+	u8 phy_gpr_reg;
+
+	struct icc_path			*icc_path;
+	s32				icc_path_bw;
+};
+
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+
+struct csi2_pix_format {
+	u32 code;
+	u8 width;
+};
+
+static const struct csi2_pix_format imx8mq_mipi_csi_formats[] = {
+	/* RAW (Bayer and greyscale) formats. */
+	{
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.width = 8,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.width = 8,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.width = 8,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.width = 8,
+	}, {
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.width = 8,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.width = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.width = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.width = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.width = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_Y10_1X10,
+		.width = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.width = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.width = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.width = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.width = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_Y12_1X12,
+		.width = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR14_1X14,
+		.width = 14,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG14_1X14,
+		.width = 14,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG14_1X14,
+		.width = 14,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB14_1X14,
+		.width = 14,
+	},
+	/* YUV formats */
+	{
+		.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		.width = 16,
+	}, {
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.width = 16,
+	}
+};
+
+static const struct csi2_pix_format *find_csi2_format(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx8mq_mipi_csi_formats); i++)
+		if (code == imx8mq_mipi_csi_formats[i].code)
+			return &imx8mq_mipi_csi_formats[i];
+	return NULL;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware configuration
+ */
+
+static inline void imx8mq_mipi_csi_write(struct csi_state *state, u32 reg, u32 val)
+{
+	writel(val, state->regs + reg);
+}
+
+static int imx8mq_mipi_csi_sw_reset(struct csi_state *state)
+{
+	int ret;
+
+	/*
+	 * these are most likely self-clearing reset bits. to make it
+	 * more clear, the reset-imx7 driver should implement the
+	 * .reset() operation.
+	 */
+	ret = reset_control_assert(state->rst);
+	if (ret < 0) {
+		dev_err(state->dev, "Failed to assert resets: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void imx8mq_mipi_csi_system_enable(struct csi_state *state, int on)
+{
+	if (!on) {
+		imx8mq_mipi_csi_write(state, CSI2RX_CFG_DISABLE_DATA_LANES, 0xf);
+		return;
+	}
+
+	regmap_update_bits(state->phy_gpr,
+			   state->phy_gpr_reg,
+			   0x3fff,
+			   GPR_CSI2_1_RX_ENABLE |
+			   GPR_CSI2_1_VID_INTFC_ENB |
+			   GPR_CSI2_1_HSEL |
+			   GPR_CSI2_1_CONT_CLK_MODE |
+			   GPR_CSI2_1_S_PRG_RXHS_SETTLE(state->hs_settle));
+}
+
+static void imx8mq_mipi_csi_set_params(struct csi_state *state)
+{
+	int lanes = state->bus.num_data_lanes;
+
+	imx8mq_mipi_csi_write(state, CSI2RX_CFG_NUM_LANES, lanes - 1);
+	imx8mq_mipi_csi_write(state, CSI2RX_CFG_DISABLE_DATA_LANES,
+			      (0xf << lanes) & 0xf);
+	imx8mq_mipi_csi_write(state, CSI2RX_IRQ_MASK, CSI2RX_IRQ_MASK_ALL);
+	/*
+	 * 0x180 bit 0 controls the Virtual Channel behaviour: when set the
+	 * interface ignores the Virtual Channel (VC) field in received packets;
+	 * when cleared it causes the interface to only accept packets whose VC
+	 * matches the value to which VC is set at offset 0x184.
+	 */
+	imx8mq_mipi_csi_write(state, CSI2RX_CFG_VID_VC_IGNORE, 1);
+	imx8mq_mipi_csi_write(state, CSI2RX_CFG_VID_P_FIFO_SEND_LEVEL,
+			      CSI2RX_SEND_LEVEL);
+}
+
+static int imx8mq_mipi_csi_clk_enable(struct csi_state *state)
+{
+	return clk_bulk_prepare_enable(CSI2_NUM_CLKS, state->clks);
+}
+
+static void imx8mq_mipi_csi_clk_disable(struct csi_state *state)
+{
+	clk_bulk_disable_unprepare(CSI2_NUM_CLKS, state->clks);
+}
+
+static int imx8mq_mipi_csi_clk_get(struct csi_state *state)
+{
+	unsigned int i;
+
+	for (i = 0; i < CSI2_NUM_CLKS; i++)
+		state->clks[i].id = imx8mq_mipi_csi_clk_id[i];
+
+	return devm_clk_bulk_get(state->dev, CSI2_NUM_CLKS, state->clks);
+}
+
+static int imx8mq_mipi_csi_calc_hs_settle(struct csi_state *state)
+{
+	s64 link_freq;
+	u32 lane_rate;
+	unsigned long esc_clk_rate;
+	u32 min_ths_settle, max_ths_settle, ths_settle_ns, esc_clk_period_ns;
+
+	/* Calculate the line rate from the pixel rate. */
+	link_freq = v4l2_get_link_freq(state->src_sd->ctrl_handler,
+				       state->csi2_fmt->width,
+				       state->bus.num_data_lanes * 2);
+	if (link_freq < 0) {
+		dev_err(state->dev, "Unable to obtain link frequency: %d\n",
+			(int)link_freq);
+		return link_freq;
+	}
+
+	lane_rate = link_freq * 2;
+	if (lane_rate < 80000000 || lane_rate > 1500000000) {
+		dev_dbg(state->dev, "Out-of-bound lane rate %u\n", lane_rate);
+		return -EINVAL;
+	}
+
+	/*
+	 * The D-PHY specification requires Ths-settle to be in the range
+	 * 85ns + 6*UI to 140ns + 10*UI, with the unit interval UI being half
+	 * the clock period.
+	 *
+	 * The Ths-settle value is expressed in the hardware as a multiple of
+	 * the Esc clock period:
+	 *
+	 * Ths-settle = (PRG_RXHS_SETTLE + 1) * Tperiod of RxClkInEsc
+	 *
+	 * Due to the one cycle inaccuracy introduced by rounding, the
+	 * documentation recommends picking a value away from the boundaries.
+	 * Let's pick the average.
+	 */
+	esc_clk_rate = clk_get_rate(state->clks[CSI2_CLK_ESC].clk);
+	if (!esc_clk_rate) {
+		dev_err(state->dev, "Could not get esc clock rate.\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(state->dev, "esc clk rate: %lu\n", esc_clk_rate);
+	esc_clk_period_ns = 1000000000 / esc_clk_rate;
+
+	min_ths_settle = 85 + 6 * 1000000 / (lane_rate / 1000);
+	max_ths_settle = 140 + 10 * 1000000 / (lane_rate / 1000);
+	ths_settle_ns = (min_ths_settle + max_ths_settle) / 2;
+
+	state->hs_settle = ths_settle_ns / esc_clk_period_ns - 1;
+
+	dev_dbg(state->dev, "lane rate %u Ths_settle %u hs_settle %u\n",
+		lane_rate, ths_settle_ns, state->hs_settle);
+
+	return 0;
+}
+
+static int imx8mq_mipi_csi_start_stream(struct csi_state *state)
+{
+	int ret;
+
+	ret = imx8mq_mipi_csi_sw_reset(state);
+	if (ret)
+		return ret;
+
+	imx8mq_mipi_csi_set_params(state);
+	ret = imx8mq_mipi_csi_calc_hs_settle(state);
+	if (ret)
+		return ret;
+
+	imx8mq_mipi_csi_system_enable(state, true);
+
+	return 0;
+}
+
+static void imx8mq_mipi_csi_stop_stream(struct csi_state *state)
+{
+	imx8mq_mipi_csi_system_enable(state, false);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static struct csi_state *mipi_sd_to_csi2_state(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi_state, sd);
+}
+
+static int imx8mq_mipi_csi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	int ret = 0;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(state->dev);
+		if (ret < 0)
+			return ret;
+	}
+
+	mutex_lock(&state->lock);
+
+	if (enable) {
+		if (state->state & ST_SUSPENDED) {
+			ret = -EBUSY;
+			goto unlock;
+		}
+
+		ret = imx8mq_mipi_csi_start_stream(state);
+		if (ret < 0)
+			goto unlock;
+
+		ret = v4l2_subdev_call(state->src_sd, video, s_stream, 1);
+		if (ret < 0)
+			goto unlock;
+
+		state->state |= ST_STREAMING;
+	} else {
+		v4l2_subdev_call(state->src_sd, video, s_stream, 0);
+		imx8mq_mipi_csi_stop_stream(state);
+		state->state &= ~ST_STREAMING;
+	}
+
+unlock:
+	mutex_unlock(&state->lock);
+
+	if (!enable || ret < 0)
+		pm_runtime_put(state->dev);
+
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+imx8mq_mipi_csi_get_format(struct csi_state *state,
+			   struct v4l2_subdev_state *sd_state,
+			   enum v4l2_subdev_format_whence which,
+			   unsigned int pad)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&state->sd, sd_state, pad);
+
+	return &state->format_mbus[pad];
+}
+
+static int imx8mq_mipi_csi_init_cfg(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state)
+{
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	struct v4l2_mbus_framefmt *fmt_sink;
+	struct v4l2_mbus_framefmt *fmt_source;
+	enum v4l2_subdev_format_whence which;
+
+	which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt_sink = imx8mq_mipi_csi_get_format(state, sd_state, which,
+					      MIPI_CSI2_PAD_SINK);
+
+	fmt_sink->code = MEDIA_BUS_FMT_SGBRG10_1X10;
+	fmt_sink->width = MIPI_CSI2_DEF_PIX_WIDTH;
+	fmt_sink->height = MIPI_CSI2_DEF_PIX_HEIGHT;
+	fmt_sink->field = V4L2_FIELD_NONE;
+
+	fmt_sink->colorspace = V4L2_COLORSPACE_RAW;
+	fmt_sink->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt_sink->colorspace);
+	fmt_sink->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt_sink->colorspace);
+	fmt_sink->quantization =
+		V4L2_MAP_QUANTIZATION_DEFAULT(false, fmt_sink->colorspace,
+					      fmt_sink->ycbcr_enc);
+
+	fmt_source = imx8mq_mipi_csi_get_format(state, sd_state, which,
+						MIPI_CSI2_PAD_SOURCE);
+	*fmt_source = *fmt_sink;
+
+	return 0;
+}
+
+static int imx8mq_mipi_csi_get_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = imx8mq_mipi_csi_get_format(state, sd_state, sdformat->which,
+					 sdformat->pad);
+
+	mutex_lock(&state->lock);
+
+	sdformat->format = *fmt;
+
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int imx8mq_mipi_csi_enum_mbus_code(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_state *sd_state,
+					  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	/*
+	 * We can't transcode in any way, the source format is identical
+	 * to the sink format.
+	 */
+	if (code->pad == MIPI_CSI2_PAD_SOURCE) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		if (code->index > 0)
+			return -EINVAL;
+
+		fmt = imx8mq_mipi_csi_get_format(state, sd_state, code->which,
+						 code->pad);
+		code->code = fmt->code;
+		return 0;
+	}
+
+	if (code->pad != MIPI_CSI2_PAD_SINK)
+		return -EINVAL;
+
+	if (code->index >= ARRAY_SIZE(imx8mq_mipi_csi_formats))
+		return -EINVAL;
+
+	code->code = imx8mq_mipi_csi_formats[code->index].code;
+
+	return 0;
+}
+
+static int imx8mq_mipi_csi_set_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	struct csi2_pix_format const *csi2_fmt;
+	struct v4l2_mbus_framefmt *fmt;
+
+	/*
+	 * The device can't transcode in any way, the source format can't be
+	 * modified.
+	 */
+	if (sdformat->pad == MIPI_CSI2_PAD_SOURCE)
+		return imx8mq_mipi_csi_get_fmt(sd, sd_state, sdformat);
+
+	if (sdformat->pad != MIPI_CSI2_PAD_SINK)
+		return -EINVAL;
+
+	csi2_fmt = find_csi2_format(sdformat->format.code);
+	if (!csi2_fmt)
+		csi2_fmt = &imx8mq_mipi_csi_formats[0];
+
+	fmt = imx8mq_mipi_csi_get_format(state, sd_state, sdformat->which,
+					 sdformat->pad);
+
+	mutex_lock(&state->lock);
+
+	fmt->code = csi2_fmt->code;
+	fmt->width = sdformat->format.width;
+	fmt->height = sdformat->format.height;
+
+	sdformat->format = *fmt;
+
+	/* Propagate the format from sink to source. */
+	fmt = imx8mq_mipi_csi_get_format(state, sd_state, sdformat->which,
+					 MIPI_CSI2_PAD_SOURCE);
+	*fmt = sdformat->format;
+
+	/* Store the CSI2 format descriptor for active formats. */
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		state->csi2_fmt = csi2_fmt;
+
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops imx8mq_mipi_csi_video_ops = {
+	.s_stream	= imx8mq_mipi_csi_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx8mq_mipi_csi_pad_ops = {
+	.init_cfg		= imx8mq_mipi_csi_init_cfg,
+	.enum_mbus_code		= imx8mq_mipi_csi_enum_mbus_code,
+	.get_fmt		= imx8mq_mipi_csi_get_fmt,
+	.set_fmt		= imx8mq_mipi_csi_set_fmt,
+};
+
+static const struct v4l2_subdev_ops imx8mq_mipi_csi_subdev_ops = {
+	.video	= &imx8mq_mipi_csi_video_ops,
+	.pad	= &imx8mq_mipi_csi_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+static const struct media_entity_operations imx8mq_mipi_csi_entity_ops = {
+	.link_validate	= v4l2_subdev_link_validate,
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+};
+
+/* -----------------------------------------------------------------------------
+ * Async subdev notifier
+ */
+
+static struct csi_state *
+mipi_notifier_to_csi2_state(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct csi_state, notifier);
+}
+
+static int imx8mq_mipi_csi_notify_bound(struct v4l2_async_notifier *notifier,
+					struct v4l2_subdev *sd,
+					struct v4l2_async_subdev *asd)
+{
+	struct csi_state *state = mipi_notifier_to_csi2_state(notifier);
+	struct media_pad *sink = &state->sd.entity.pads[MIPI_CSI2_PAD_SINK];
+
+	state->src_sd = sd;
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
+					       MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static const struct v4l2_async_notifier_operations imx8mq_mipi_csi_notify_ops = {
+	.bound = imx8mq_mipi_csi_notify_bound,
+};
+
+static int imx8mq_mipi_csi_async_register(struct csi_state *state)
+{
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct v4l2_async_subdev *asd;
+	struct fwnode_handle *ep;
+	unsigned int i;
+	int ret;
+
+	v4l2_async_nf_init(&state->notifier);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(state->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENOTCONN;
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &vep);
+	if (ret)
+		goto err_parse;
+
+	for (i = 0; i < vep.bus.mipi_csi2.num_data_lanes; ++i) {
+		if (vep.bus.mipi_csi2.data_lanes[i] != i + 1) {
+			dev_err(state->dev,
+				"data lanes reordering is not supported");
+			ret = -EINVAL;
+			goto err_parse;
+		}
+	}
+
+	state->bus = vep.bus.mipi_csi2;
+
+	dev_dbg(state->dev, "data lanes: %d flags: 0x%08x\n",
+		state->bus.num_data_lanes,
+		state->bus.flags);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&state->notifier, ep,
+					      struct v4l2_async_subdev);
+	if (IS_ERR(asd)) {
+		ret = PTR_ERR(asd);
+		goto err_parse;
+	}
+
+	fwnode_handle_put(ep);
+
+	state->notifier.ops = &imx8mq_mipi_csi_notify_ops;
+
+	ret = v4l2_async_subdev_nf_register(&state->sd, &state->notifier);
+	if (ret)
+		return ret;
+
+	return v4l2_async_register_subdev(&state->sd);
+
+err_parse:
+	fwnode_handle_put(ep);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Suspend/resume
+ */
+
+static void imx8mq_mipi_csi_pm_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	mutex_lock(&state->lock);
+
+	if (state->state & ST_POWERED) {
+		imx8mq_mipi_csi_stop_stream(state);
+		imx8mq_mipi_csi_clk_disable(state);
+		state->state &= ~ST_POWERED;
+	}
+
+	mutex_unlock(&state->lock);
+}
+
+static int imx8mq_mipi_csi_pm_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	if (!(state->state & ST_POWERED)) {
+		state->state |= ST_POWERED;
+		ret = imx8mq_mipi_csi_clk_enable(state);
+	}
+	if (state->state & ST_STREAMING) {
+		ret = imx8mq_mipi_csi_start_stream(state);
+		if (ret)
+			goto unlock;
+	}
+
+	state->state &= ~ST_SUSPENDED;
+
+unlock:
+	mutex_unlock(&state->lock);
+
+	return ret ? -EAGAIN : 0;
+}
+
+static int __maybe_unused imx8mq_mipi_csi_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	imx8mq_mipi_csi_pm_suspend(dev);
+
+	state->state |= ST_SUSPENDED;
+
+	return 0;
+}
+
+static int __maybe_unused imx8mq_mipi_csi_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	if (!(state->state & ST_SUSPENDED))
+		return 0;
+
+	return imx8mq_mipi_csi_pm_resume(dev);
+}
+
+static int __maybe_unused imx8mq_mipi_csi_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	int ret;
+
+	imx8mq_mipi_csi_pm_suspend(dev);
+
+	ret = icc_set_bw(state->icc_path, 0, 0);
+	if (ret)
+		dev_err(dev, "icc_set_bw failed with %d\n", ret);
+
+	return ret;
+}
+
+static int __maybe_unused imx8mq_mipi_csi_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+	int ret;
+
+	ret = icc_set_bw(state->icc_path, 0, state->icc_path_bw);
+	if (ret) {
+		dev_err(dev, "icc_set_bw failed with %d\n", ret);
+		return ret;
+	}
+
+	return imx8mq_mipi_csi_pm_resume(dev);
+}
+
+static const struct dev_pm_ops imx8mq_mipi_csi_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx8mq_mipi_csi_runtime_suspend,
+			   imx8mq_mipi_csi_runtime_resume,
+			   NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(imx8mq_mipi_csi_suspend, imx8mq_mipi_csi_resume)
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe/remove & platform driver
+ */
+
+static int imx8mq_mipi_csi_subdev_init(struct csi_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_subdev_init(sd, &imx8mq_mipi_csi_subdev_ops);
+	sd->owner = THIS_MODULE;
+	snprintf(sd->name, sizeof(sd->name), "%s %s",
+		 MIPI_CSI2_SUBDEV_NAME, dev_name(state->dev));
+
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->entity.ops = &imx8mq_mipi_csi_entity_ops;
+
+	sd->dev = state->dev;
+
+	state->csi2_fmt = &imx8mq_mipi_csi_formats[0];
+	imx8mq_mipi_csi_init_cfg(sd, NULL);
+
+	state->pads[MIPI_CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+					 | MEDIA_PAD_FL_MUST_CONNECT;
+	state->pads[MIPI_CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE
+					   | MEDIA_PAD_FL_MUST_CONNECT;
+	return media_entity_pads_init(&sd->entity, MIPI_CSI2_PADS_NUM,
+				      state->pads);
+}
+
+static void imx8mq_mipi_csi_release_icc(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(&pdev->dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	icc_put(state->icc_path);
+}
+
+static int imx8mq_mipi_csi_init_icc(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(&pdev->dev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	/* Optional interconnect request */
+	state->icc_path = of_icc_get(&pdev->dev, "dram");
+	if (IS_ERR_OR_NULL(state->icc_path))
+		return PTR_ERR_OR_ZERO(state->icc_path);
+
+	state->icc_path_bw = MBps_to_icc(700);
+
+	return 0;
+}
+
+static int imx8mq_mipi_csi_parse_dt(struct csi_state *state)
+{
+	struct device *dev = state->dev;
+	struct device_node *np = state->dev->of_node;
+	struct device_node *node;
+	phandle ph;
+	u32 out_val[2];
+	int ret = 0;
+
+	state->rst = devm_reset_control_array_get_exclusive(dev);
+	if (IS_ERR(state->rst)) {
+		dev_err(dev, "Failed to get reset: %pe\n", state->rst);
+		return PTR_ERR(state->rst);
+	}
+
+	ret = of_property_read_u32_array(np, "fsl,mipi-phy-gpr", out_val,
+					 ARRAY_SIZE(out_val));
+	if (ret) {
+		dev_err(dev, "no fsl,mipi-phy-gpr property found: %d\n", ret);
+		return ret;
+	}
+
+	ph = *out_val;
+
+	node = of_find_node_by_phandle(ph);
+	if (!node) {
+		dev_err(dev, "Error finding node by phandle\n");
+		return -ENODEV;
+	}
+	state->phy_gpr = syscon_node_to_regmap(node);
+	of_node_put(node);
+	if (IS_ERR(state->phy_gpr)) {
+		dev_err(dev, "failed to get gpr regmap: %pe\n", state->phy_gpr);
+		return PTR_ERR(state->phy_gpr);
+	}
+
+	state->phy_gpr_reg = out_val[1];
+	dev_dbg(dev, "phy gpr register set to 0x%x\n", state->phy_gpr_reg);
+
+	return ret;
+}
+
+static int imx8mq_mipi_csi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct csi_state *state;
+	int ret;
+
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->dev = dev;
+
+	ret = imx8mq_mipi_csi_parse_dt(state);
+	if (ret < 0) {
+		dev_err(dev, "Failed to parse device tree: %d\n", ret);
+		return ret;
+	}
+
+	/* Acquire resources. */
+	state->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(state->regs))
+		return PTR_ERR(state->regs);
+
+	ret = imx8mq_mipi_csi_clk_get(state);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, &state->sd);
+
+	mutex_init(&state->lock);
+
+	ret = imx8mq_mipi_csi_subdev_init(state);
+	if (ret < 0)
+		goto mutex;
+
+	ret = imx8mq_mipi_csi_init_icc(pdev);
+	if (ret)
+		goto mutex;
+
+	/* Enable runtime PM. */
+	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = imx8mq_mipi_csi_runtime_resume(dev);
+		if (ret < 0)
+			goto icc;
+	}
+
+	ret = imx8mq_mipi_csi_async_register(state);
+	if (ret < 0)
+		goto cleanup;
+
+	return 0;
+
+cleanup:
+	pm_runtime_disable(&pdev->dev);
+	imx8mq_mipi_csi_runtime_suspend(&pdev->dev);
+
+	media_entity_cleanup(&state->sd.entity);
+	v4l2_async_nf_unregister(&state->notifier);
+	v4l2_async_nf_cleanup(&state->notifier);
+	v4l2_async_unregister_subdev(&state->sd);
+icc:
+	imx8mq_mipi_csi_release_icc(pdev);
+mutex:
+	mutex_destroy(&state->lock);
+
+	return ret;
+}
+
+static int imx8mq_mipi_csi_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csi_state *state = mipi_sd_to_csi2_state(sd);
+
+	v4l2_async_nf_unregister(&state->notifier);
+	v4l2_async_nf_cleanup(&state->notifier);
+	v4l2_async_unregister_subdev(&state->sd);
+
+	pm_runtime_disable(&pdev->dev);
+	imx8mq_mipi_csi_runtime_suspend(&pdev->dev);
+	media_entity_cleanup(&state->sd.entity);
+	mutex_destroy(&state->lock);
+	pm_runtime_set_suspended(&pdev->dev);
+	imx8mq_mipi_csi_release_icc(pdev);
+
+	return 0;
+}
+
+static const struct of_device_id imx8mq_mipi_csi_of_match[] = {
+	{ .compatible = "fsl,imx8mq-mipi-csi2", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, imx8mq_mipi_csi_of_match);
+
+static struct platform_driver imx8mq_mipi_csi_driver = {
+	.probe		= imx8mq_mipi_csi_probe,
+	.remove		= imx8mq_mipi_csi_remove,
+	.driver		= {
+		.of_match_table = imx8mq_mipi_csi_of_match,
+		.name		= MIPI_CSI2_DRIVER_NAME,
+		.pm		= &imx8mq_mipi_csi_pm_ops,
+	},
+};
+
+module_platform_driver(imx8mq_mipi_csi_driver);
+
+MODULE_DESCRIPTION("i.MX8MQ MIPI CSI-2 receiver driver");
+MODULE_AUTHOR("Martin Kepplinger <martin.kepplinger@puri.sm>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:imx8mq-mipi-csi2");
diff --git a/drivers/staging/media/ipu3/Kconfig b/drivers/staging/media/ipu3/Kconfig
new file mode 100644
index 000000000..114a1d8e7
--- /dev/null
+++ b/drivers/staging/media/ipu3/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_IPU3_IMGU
+	tristate "Intel ipu3-imgu driver"
+	depends on PCI && VIDEO_DEV
+	depends on X86
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select IOMMU_IOVA
+	select VIDEOBUF2_DMA_SG
+	help
+	  This is the Video4Linux2 driver for Intel IPU3 image processing unit,
+	  found in Intel Skylake and Kaby Lake SoCs and used for processing
+	  images and video.
+
+	  Say Y or M here if you have a Skylake/Kaby Lake SoC with a MIPI
+	  camera. The module will be called ipu3-imgu.
diff --git a/drivers/staging/media/ipu3/Makefile b/drivers/staging/media/ipu3/Makefile
new file mode 100644
index 000000000..9def80ef2
--- /dev/null
+++ b/drivers/staging/media/ipu3/Makefile
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the IPU3 ImgU drivers
+#
+
+ipu3-imgu-objs += \
+		ipu3-mmu.o ipu3-dmamap.o \
+		ipu3-tables.o ipu3-css-pool.o \
+		ipu3-css-fw.o ipu3-css-params.o \
+		ipu3-css.o ipu3-v4l2.o ipu3.o
+
+obj-$(CONFIG_VIDEO_IPU3_IMGU) += ipu3-imgu.o
diff --git a/drivers/staging/media/ipu3/TODO b/drivers/staging/media/ipu3/TODO
new file mode 100644
index 000000000..3fd1fc10b
--- /dev/null
+++ b/drivers/staging/media/ipu3/TODO
@@ -0,0 +1,12 @@
+This is a list of things that need to be done to get this driver out of the
+staging directory.
+
+- Request API conversion. Remove of the dual pipeline and associate buffers
+  as well as formats and the binary used to a request. Remove the
+  opportunistic buffer management. (Sakari)
+
+- IPU3 driver documentation (Laurent)
+  Comments on configuring v4l2 subdevs for CIO2 and ImgU.
+
+- Elaborate the functionality of different selection rectangles in driver
+  documentation. This may require driver changes as well.
diff --git a/drivers/staging/media/ipu3/include/uapi/intel-ipu3.h b/drivers/staging/media/ipu3/include/uapi/intel-ipu3.h
new file mode 100644
index 000000000..caa358e0b
--- /dev/null
+++ b/drivers/staging/media/ipu3/include/uapi/intel-ipu3.h
@@ -0,0 +1,2822 @@
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+/* Copyright (C) 2017 - 2018 Intel Corporation */
+
+#ifndef __IPU3_UAPI_H
+#define __IPU3_UAPI_H
+
+#include <linux/types.h>
+
+/* from /drivers/staging/media/ipu3/include/videodev2.h */
+
+/* Vendor specific - used for IPU3 camera sub-system */
+/* IPU3 processing parameters */
+#define V4L2_META_FMT_IPU3_PARAMS	v4l2_fourcc('i', 'p', '3', 'p')
+/* IPU3 3A statistics */
+#define V4L2_META_FMT_IPU3_STAT_3A	v4l2_fourcc('i', 'p', '3', 's')
+
+/* from include/uapi/linux/v4l2-controls.h */
+#define V4L2_CID_INTEL_IPU3_BASE	(V4L2_CID_USER_BASE + 0x10c0)
+#define V4L2_CID_INTEL_IPU3_MODE	(V4L2_CID_INTEL_IPU3_BASE + 1)
+
+/******************* ipu3_uapi_stats_3a *******************/
+
+#define IPU3_UAPI_MAX_STRIPES				2
+#define IPU3_UAPI_MAX_BUBBLE_SIZE			10
+
+#define IPU3_UAPI_GRID_START_MASK			((1 << 12) - 1)
+#define IPU3_UAPI_GRID_Y_START_EN			(1 << 15)
+
+/* controls generation of meta_data (like FF enable/disable) */
+#define IPU3_UAPI_AWB_RGBS_THR_B_EN			(1 << 14)
+#define IPU3_UAPI_AWB_RGBS_THR_B_INCL_SAT		(1 << 15)
+
+/**
+ * struct ipu3_uapi_grid_config - Grid plane config
+ *
+ * @width:	Grid horizontal dimensions, in number of grid blocks(cells).
+ *		For AWB, the range is (16, 80).
+ *		For AF/AE, the range is (16, 32).
+ * @height:	Grid vertical dimensions, in number of grid cells.
+ *		For AWB, the range is (16, 60).
+ *		For AF/AE, the range is (16, 24).
+ * @block_width_log2:	Log2 of the width of each cell in pixels.
+ *			For AWB, the range is [3, 6].
+ *			For AF/AE, the range is [3, 7].
+ * @block_height_log2:	Log2 of the height of each cell in pixels.
+ *			For AWB, the range is [3, 6].
+ *			For AF/AE, the range is [3, 7].
+ * @height_per_slice:	The number of blocks in vertical axis per slice.
+ *			Default 2.
+ * @x_start: X value of top left corner of Region of Interest(ROI).
+ * @y_start: Y value of top left corner of ROI
+ * @x_end: X value of bottom right corner of ROI
+ * @y_end: Y value of bottom right corner of ROI
+ *
+ * Due to the size of total amount of collected data, most statistics
+ * create a grid-based output, and the data is then divided into "slices".
+ */
+struct ipu3_uapi_grid_config {
+	__u8 width;
+	__u8 height;
+	__u16 block_width_log2:3;
+	__u16 block_height_log2:3;
+	__u16 height_per_slice:8;
+	__u16 x_start;
+	__u16 y_start;
+	__u16 x_end;
+	__u16 y_end;
+} __packed;
+
+/**
+ * struct ipu3_uapi_awb_set_item - Memory layout for each cell in AWB
+ *
+ * @Gr_avg:	Green average for red lines in the cell.
+ * @R_avg:	Red average in the cell.
+ * @B_avg:	Blue average in the cell.
+ * @Gb_avg:	Green average for blue lines in the cell.
+ * @sat_ratio:  Percentage of pixels over the thresholds specified in
+ *		ipu3_uapi_awb_config_s, coded from 0 to 255.
+ * @padding0:   Unused byte for padding.
+ * @padding1:   Unused byte for padding.
+ * @padding2:   Unused byte for padding.
+ */
+struct ipu3_uapi_awb_set_item {
+	__u8 Gr_avg;
+	__u8 R_avg;
+	__u8 B_avg;
+	__u8 Gb_avg;
+	__u8 sat_ratio;
+	__u8 padding0;
+	__u8 padding1;
+	__u8 padding2;
+} __attribute__((packed));
+
+/*
+ * The grid based data is divided into "slices" called set, each slice of setX
+ * refers to ipu3_uapi_grid_config width * height_per_slice.
+ */
+#define IPU3_UAPI_AWB_MAX_SETS				60
+/* Based on grid size 80 * 60 and cell size 16 x 16 */
+#define IPU3_UAPI_AWB_SET_SIZE				160
+#define IPU3_UAPI_AWB_SPARE_FOR_BUBBLES \
+	(IPU3_UAPI_MAX_BUBBLE_SIZE * IPU3_UAPI_MAX_STRIPES)
+#define IPU3_UAPI_AWB_MAX_BUFFER_SIZE \
+	(IPU3_UAPI_AWB_MAX_SETS * \
+	 (IPU3_UAPI_AWB_SET_SIZE + IPU3_UAPI_AWB_SPARE_FOR_BUBBLES))
+
+/**
+ * struct ipu3_uapi_awb_raw_buffer - AWB raw buffer
+ *
+ * @meta_data: buffer to hold auto white balance meta data which is
+ *		the average values for each color channel.
+ */
+struct ipu3_uapi_awb_raw_buffer {
+	struct ipu3_uapi_awb_set_item meta_data[IPU3_UAPI_AWB_MAX_BUFFER_SIZE]
+		__attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_awb_config_s - AWB config
+ *
+ * @rgbs_thr_gr: gr threshold value.
+ * @rgbs_thr_r: Red threshold value.
+ * @rgbs_thr_gb: gb threshold value.
+ * @rgbs_thr_b: Blue threshold value.
+ * @grid: &ipu3_uapi_grid_config, the default grid resolution is 16x16 cells.
+ *
+ * The threshold is a saturation measure range [0, 8191], 8191 is default.
+ * Values over threshold may be optionally rejected for averaging.
+ */
+struct ipu3_uapi_awb_config_s {
+	__u16 rgbs_thr_gr;
+	__u16 rgbs_thr_r;
+	__u16 rgbs_thr_gb;
+	__u16 rgbs_thr_b;
+	struct ipu3_uapi_grid_config grid;
+} __attribute__((aligned(32))) __packed;
+
+/**
+ * struct ipu3_uapi_awb_config - AWB config wrapper
+ *
+ * @config: config for auto white balance as defined by &ipu3_uapi_awb_config_s
+ */
+struct ipu3_uapi_awb_config {
+	struct ipu3_uapi_awb_config_s config __attribute__((aligned(32)));
+} __packed;
+
+#define IPU3_UAPI_AE_COLORS				4	/* R, G, B, Y */
+#define IPU3_UAPI_AE_BINS				256
+#define IPU3_UAPI_AE_WEIGHTS				96
+
+/**
+ * struct ipu3_uapi_ae_raw_buffer - AE global weighted histogram
+ *
+ * @vals: Sum of IPU3_UAPI_AE_COLORS in cell
+ *
+ * Each histogram contains IPU3_UAPI_AE_BINS bins. Each bin has 24 bit unsigned
+ * for counting the number of the pixel.
+ */
+struct ipu3_uapi_ae_raw_buffer {
+	__u32 vals[IPU3_UAPI_AE_BINS * IPU3_UAPI_AE_COLORS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_ae_raw_buffer_aligned - AE raw buffer
+ *
+ * @buff: &ipu3_uapi_ae_raw_buffer to hold full frame meta data.
+ */
+struct ipu3_uapi_ae_raw_buffer_aligned {
+	struct ipu3_uapi_ae_raw_buffer buff __attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_ae_grid_config - AE weight grid
+ *
+ * @width: Grid horizontal dimensions. Value: [16, 32], default 16.
+ * @height: Grid vertical dimensions. Value: [16, 24], default 16.
+ * @block_width_log2: Log2 of the width of the grid cell, value: [3, 7].
+ * @block_height_log2: Log2 of the height of the grid cell, value: [3, 7].
+ *			default is 3 (cell size 8x8), 4 cell per grid.
+ * @reserved0: reserved
+ * @ae_en: 0: does not write to &ipu3_uapi_ae_raw_buffer_aligned array,
+ *		1: write normally.
+ * @rst_hist_array: write 1 to trigger histogram array reset.
+ * @done_rst_hist_array: flag for histogram array reset done.
+ * @x_start: X value of top left corner of ROI, default 0.
+ * @y_start: Y value of top left corner of ROI, default 0.
+ * @x_end: X value of bottom right corner of ROI
+ * @y_end: Y value of bottom right corner of ROI
+ *
+ * The AE block accumulates 4 global weighted histograms(R, G, B, Y) over
+ * a defined ROI within the frame. The contribution of each pixel into the
+ * histogram, defined by &ipu3_uapi_ae_weight_elem LUT, is indexed by a grid.
+ */
+struct ipu3_uapi_ae_grid_config {
+	__u8 width;
+	__u8 height;
+	__u8 block_width_log2:4;
+	__u8 block_height_log2:4;
+	__u8 reserved0:5;
+	__u8 ae_en:1;
+	__u8 rst_hist_array:1;
+	__u8 done_rst_hist_array:1;
+	__u16 x_start;
+	__u16 y_start;
+	__u16 x_end;
+	__u16 y_end;
+} __packed;
+
+/**
+ * struct ipu3_uapi_ae_weight_elem - AE weights LUT
+ *
+ * @cell0: weighted histogram grid value.
+ * @cell1: weighted histogram grid value.
+ * @cell2: weighted histogram grid value.
+ * @cell3: weighted histogram grid value.
+ * @cell4: weighted histogram grid value.
+ * @cell5: weighted histogram grid value.
+ * @cell6: weighted histogram grid value.
+ * @cell7: weighted histogram grid value.
+ *
+ * Use weighted grid value to give a different contribution factor to each cell.
+ * Precision u4, range [0, 15].
+ */
+struct ipu3_uapi_ae_weight_elem {
+	__u32 cell0:4;
+	__u32 cell1:4;
+	__u32 cell2:4;
+	__u32 cell3:4;
+	__u32 cell4:4;
+	__u32 cell5:4;
+	__u32 cell6:4;
+	__u32 cell7:4;
+} __packed;
+
+/**
+ * struct ipu3_uapi_ae_ccm - AE coefficients for WB and CCM
+ *
+ * @gain_gr: WB gain factor for the gr channels. Default 256.
+ * @gain_r: WB gain factor for the r channel. Default 256.
+ * @gain_b: WB gain factor for the b channel. Default 256.
+ * @gain_gb: WB gain factor for the gb channels. Default 256.
+ * @mat: 4x4 matrix that transforms Bayer quad output from WB to RGB+Y.
+ *
+ * Default:
+ *	128, 0, 0, 0,
+ *	0, 128, 0, 0,
+ *	0, 0, 128, 0,
+ *	0, 0, 0, 128,
+ *
+ * As part of the raw frame pre-process stage, the WB and color conversion need
+ * to be applied to expose the impact of these gain operations.
+ */
+struct ipu3_uapi_ae_ccm {
+	__u16 gain_gr;
+	__u16 gain_r;
+	__u16 gain_b;
+	__u16 gain_gb;
+	__s16 mat[16];
+} __packed;
+
+/**
+ * struct ipu3_uapi_ae_config - AE config
+ *
+ * @grid_cfg:	config for auto exposure statistics grid. See struct
+ *		&ipu3_uapi_ae_grid_config, as Imgu did not support output
+ *		auto exposure statistics, so user can ignore this configuration
+ *		and use the RGB table in auto-whitebalance statistics instead.
+ * @weights:	&IPU3_UAPI_AE_WEIGHTS is based on 32x24 blocks in the grid.
+ *		Each grid cell has a corresponding value in weights LUT called
+ *		grid value, global histogram is updated based on grid value and
+ *		pixel value.
+ * @ae_ccm:	Color convert matrix pre-processing block.
+ *
+ * Calculate AE grid from image resolution, resample ae weights.
+ */
+struct ipu3_uapi_ae_config {
+	struct ipu3_uapi_ae_grid_config grid_cfg __attribute__((aligned(32)));
+	struct ipu3_uapi_ae_weight_elem weights[IPU3_UAPI_AE_WEIGHTS]
+						__attribute__((aligned(32)));
+	struct ipu3_uapi_ae_ccm ae_ccm __attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_af_filter_config - AF 2D filter for contrast measurements
+ *
+ * @y1_coeff_0:	filter Y1, structure: 3x11, support both symmetry and
+ *		anti-symmetry type. A12 is center, A1-A11 are neighbours.
+ *		for analyzing low frequency content, used to calculate sum
+ *		of gradients in x direction.
+ * @y1_coeff_0.a1:	filter1 coefficients A1, u8, default 0.
+ * @y1_coeff_0.a2:	filter1 coefficients A2, u8, default 0.
+ * @y1_coeff_0.a3:	filter1 coefficients A3, u8, default 0.
+ * @y1_coeff_0.a4:	filter1 coefficients A4, u8, default 0.
+ * @y1_coeff_1:		Struct
+ * @y1_coeff_1.a5:	filter1 coefficients A5, u8, default 0.
+ * @y1_coeff_1.a6:	filter1 coefficients A6, u8, default 0.
+ * @y1_coeff_1.a7:	filter1 coefficients A7, u8, default 0.
+ * @y1_coeff_1.a8:	filter1 coefficients A8, u8, default 0.
+ * @y1_coeff_2:		Struct
+ * @y1_coeff_2.a9:	filter1 coefficients A9, u8, default 0.
+ * @y1_coeff_2.a10:	filter1 coefficients A10, u8, default 0.
+ * @y1_coeff_2.a11:	filter1 coefficients A11, u8, default 0.
+ * @y1_coeff_2.a12:	filter1 coefficients A12, u8, default 128.
+ * @y1_sign_vec:	Each bit corresponds to one coefficient sign bit,
+ *			0: positive, 1: negative, default 0.
+ * @y2_coeff_0:	Y2, same structure as Y1. For analyzing high frequency content.
+ * @y2_coeff_0.a1:	filter2 coefficients A1, u8, default 0.
+ * @y2_coeff_0.a2:	filter2 coefficients A2, u8, default 0.
+ * @y2_coeff_0.a3:	filter2 coefficients A3, u8, default 0.
+ * @y2_coeff_0.a4:	filter2 coefficients A4, u8, default 0.
+ * @y2_coeff_1:	Struct
+ * @y2_coeff_1.a5:	filter2 coefficients A5, u8, default 0.
+ * @y2_coeff_1.a6:	filter2 coefficients A6, u8, default 0.
+ * @y2_coeff_1.a7:	filter2 coefficients A7, u8, default 0.
+ * @y2_coeff_1.a8:	filter2 coefficients A8, u8, default 0.
+ * @y2_coeff_2:	Struct
+ * @y2_coeff_2.a9:	filter1 coefficients A9, u8, default 0.
+ * @y2_coeff_2.a10:	filter1 coefficients A10, u8, default 0.
+ * @y2_coeff_2.a11:	filter1 coefficients A11, u8, default 0.
+ * @y2_coeff_2.a12:	filter1 coefficients A12, u8, default 128.
+ * @y2_sign_vec:	Each bit corresponds to one coefficient sign bit,
+ *			0: positive, 1: negative, default 0.
+ * @y_calc:	Pre-processing that converts Bayer quad to RGB+Y values to be
+ *		used for building histogram. Range [0, 32], default 8.
+ * Rule:
+ *		y_gen_rate_gr + y_gen_rate_r + y_gen_rate_b + y_gen_rate_gb = 32
+ *		A single Y is calculated based on sum of Gr/R/B/Gb based on
+ *		their contribution ratio.
+ * @y_calc.y_gen_rate_gr:	Contribution ratio Gr for Y
+ * @y_calc.y_gen_rate_r:	Contribution ratio R for Y
+ * @y_calc.y_gen_rate_b:	Contribution ratio B for Y
+ * @y_calc.y_gen_rate_gb:	Contribution ratio Gb for Y
+ * @nf:	The shift right value that should be applied during the Y1/Y2 filter to
+ *	make sure the total memory needed is 2 bytes per grid cell.
+ * @nf.reserved0:	reserved
+ * @nf.y1_nf:	Normalization factor for the convolution coeffs of y1,
+ *		should be log2 of the sum of the abs values of the filter
+ *		coeffs, default 7 (2^7 = 128).
+ * @nf.reserved1:	reserved
+ * @nf.y2_nf:	Normalization factor for y2, should be log2 of the sum of the
+ *		abs values of the filter coeffs.
+ * @nf.reserved2:	reserved
+ */
+struct ipu3_uapi_af_filter_config {
+	struct {
+		__u8 a1;
+		__u8 a2;
+		__u8 a3;
+		__u8 a4;
+	} y1_coeff_0;
+	struct {
+		__u8 a5;
+		__u8 a6;
+		__u8 a7;
+		__u8 a8;
+	} y1_coeff_1;
+	struct {
+		__u8 a9;
+		__u8 a10;
+		__u8 a11;
+		__u8 a12;
+	} y1_coeff_2;
+
+	__u32 y1_sign_vec;
+
+	struct {
+		__u8 a1;
+		__u8 a2;
+		__u8 a3;
+		__u8 a4;
+	} y2_coeff_0;
+	struct {
+		__u8 a5;
+		__u8 a6;
+		__u8 a7;
+		__u8 a8;
+	} y2_coeff_1;
+	struct {
+		__u8 a9;
+		__u8 a10;
+		__u8 a11;
+		__u8 a12;
+	} y2_coeff_2;
+
+	__u32 y2_sign_vec;
+
+	struct {
+		__u8 y_gen_rate_gr;
+		__u8 y_gen_rate_r;
+		__u8 y_gen_rate_b;
+		__u8 y_gen_rate_gb;
+	} y_calc;
+
+	struct {
+		__u32 reserved0:8;
+		__u32 y1_nf:4;
+		__u32 reserved1:4;
+		__u32 y2_nf:4;
+		__u32 reserved2:12;
+	} nf;
+} __packed;
+
+#define IPU3_UAPI_AF_MAX_SETS				24
+#define IPU3_UAPI_AF_MD_ITEM_SIZE			4
+#define IPU3_UAPI_AF_SPARE_FOR_BUBBLES \
+	(IPU3_UAPI_MAX_BUBBLE_SIZE * IPU3_UAPI_MAX_STRIPES * \
+	 IPU3_UAPI_AF_MD_ITEM_SIZE)
+#define IPU3_UAPI_AF_Y_TABLE_SET_SIZE			128
+#define IPU3_UAPI_AF_Y_TABLE_MAX_SIZE \
+	(IPU3_UAPI_AF_MAX_SETS * \
+	 (IPU3_UAPI_AF_Y_TABLE_SET_SIZE + IPU3_UAPI_AF_SPARE_FOR_BUBBLES) * \
+	 IPU3_UAPI_MAX_STRIPES)
+
+/**
+ * struct ipu3_uapi_af_raw_buffer - AF meta data
+ *
+ * @y_table:	Each color component will be convolved separately with filter1
+ *		and filter2 and the result will be summed out and averaged for
+ *		each cell.
+ */
+struct ipu3_uapi_af_raw_buffer {
+	__u8 y_table[IPU3_UAPI_AF_Y_TABLE_MAX_SIZE] __attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_af_config_s - AF config
+ *
+ * @filter_config: AF uses Y1 and Y2 filters as configured in
+ *		   &ipu3_uapi_af_filter_config
+ * @padding: paddings
+ * @grid_cfg: See &ipu3_uapi_grid_config, default resolution 16x16. Use large
+ *	      grid size for large image and vice versa.
+ */
+struct ipu3_uapi_af_config_s {
+	struct ipu3_uapi_af_filter_config filter_config __attribute__((aligned(32)));
+	__u8 padding[4];
+	struct ipu3_uapi_grid_config grid_cfg __attribute__((aligned(32)));
+} __packed;
+
+#define IPU3_UAPI_AWB_FR_MAX_SETS			24
+#define IPU3_UAPI_AWB_FR_MD_ITEM_SIZE			8
+#define IPU3_UAPI_AWB_FR_BAYER_TBL_SIZE			256
+#define IPU3_UAPI_AWB_FR_SPARE_FOR_BUBBLES \
+	(IPU3_UAPI_MAX_BUBBLE_SIZE * IPU3_UAPI_MAX_STRIPES * \
+	 IPU3_UAPI_AWB_FR_MD_ITEM_SIZE)
+#define IPU3_UAPI_AWB_FR_BAYER_TABLE_MAX_SIZE \
+	(IPU3_UAPI_AWB_FR_MAX_SETS * \
+	(IPU3_UAPI_AWB_FR_BAYER_TBL_SIZE + \
+	 IPU3_UAPI_AWB_FR_SPARE_FOR_BUBBLES) * IPU3_UAPI_MAX_STRIPES)
+
+/**
+ * struct ipu3_uapi_awb_fr_raw_buffer - AWB filter response meta data
+ *
+ * @meta_data: Statistics output on the grid after convolving with 1D filter.
+ */
+struct ipu3_uapi_awb_fr_raw_buffer {
+	__u8 meta_data[IPU3_UAPI_AWB_FR_BAYER_TABLE_MAX_SIZE]
+		__attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_awb_fr_config_s - AWB filter response config
+ *
+ * @grid_cfg:	grid config, default 16x16.
+ * @bayer_coeff:	1D Filter 1x11 center symmetry/anti-symmetry.
+ *			coefficients defaults { 0, 0, 0, 0, 0, 128 }.
+ *			Applied on whole image for each Bayer channel separately
+ *			by a weighted sum of its 11x1 neighbors.
+ * @reserved1:	reserved
+ * @bayer_sign:	sign of filter coefficients, default 0.
+ * @bayer_nf:	normalization factor for the convolution coeffs, to make sure
+ *		total memory needed is within pre-determined range.
+ *		NF should be the log2 of the sum of the abs values of the
+ *		filter coeffs, range [7, 14], default 7.
+ * @reserved2:	reserved
+ */
+struct ipu3_uapi_awb_fr_config_s {
+	struct ipu3_uapi_grid_config grid_cfg;
+	__u8 bayer_coeff[6];
+	__u16 reserved1;
+	__u32 bayer_sign;
+	__u8 bayer_nf;
+	__u8 reserved2[7];
+} __packed;
+
+/**
+ * struct ipu3_uapi_4a_config - 4A config
+ *
+ * @awb_config: &ipu3_uapi_awb_config_s, default resolution 16x16
+ * @ae_grd_config: auto exposure statistics &ipu3_uapi_ae_grid_config
+ * @padding: paddings
+ * @af_config: auto focus config &ipu3_uapi_af_config_s
+ * @awb_fr_config: &ipu3_uapi_awb_fr_config_s, default resolution 16x16
+ */
+struct ipu3_uapi_4a_config {
+	struct ipu3_uapi_awb_config_s awb_config __attribute__((aligned(32)));
+	struct ipu3_uapi_ae_grid_config ae_grd_config;
+	__u8 padding[20];
+	struct ipu3_uapi_af_config_s af_config;
+	struct ipu3_uapi_awb_fr_config_s awb_fr_config
+		__attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_bubble_info - Bubble info for host side debugging
+ *
+ * @num_of_stripes: A single frame is divided into several parts called stripes
+ *		    due to limitation on line buffer memory.
+ *		    The separation between the stripes is vertical. Each such
+ *		    stripe is processed as a single frame by the ISP pipe.
+ * @padding: padding bytes.
+ * @num_sets: number of sets.
+ * @padding1: padding bytes.
+ * @size_of_set: set size.
+ * @padding2: padding bytes.
+ * @bubble_size: is the amount of padding in the bubble expressed in "sets".
+ * @padding3: padding bytes.
+ */
+struct ipu3_uapi_bubble_info {
+	__u32 num_of_stripes __attribute__((aligned(32)));
+	__u8 padding[28];
+	__u32 num_sets;
+	__u8 padding1[28];
+	__u32 size_of_set;
+	__u8 padding2[28];
+	__u32 bubble_size;
+	__u8 padding3[28];
+} __packed;
+
+/*
+ * struct ipu3_uapi_stats_3a_bubble_info_per_stripe
+ */
+struct ipu3_uapi_stats_3a_bubble_info_per_stripe {
+	struct ipu3_uapi_bubble_info awb[IPU3_UAPI_MAX_STRIPES];
+	struct ipu3_uapi_bubble_info af[IPU3_UAPI_MAX_STRIPES];
+	struct ipu3_uapi_bubble_info awb_fr[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+/**
+ * struct ipu3_uapi_ff_status - Enable bits for each 3A fixed function
+ *
+ * @awb_en: auto white balance enable
+ * @padding: padding config
+ * @ae_en: auto exposure enable
+ * @padding1: padding config
+ * @af_en: auto focus enable
+ * @padding2: padding config
+ * @awb_fr_en: awb filter response enable bit
+ * @padding3: padding config
+ */
+struct ipu3_uapi_ff_status {
+	__u32 awb_en __attribute__((aligned(32)));
+	__u8 padding[28];
+	__u32 ae_en;
+	__u8 padding1[28];
+	__u32 af_en;
+	__u8 padding2[28];
+	__u32 awb_fr_en;
+	__u8 padding3[28];
+} __packed;
+
+/**
+ * struct ipu3_uapi_stats_3a - 3A statistics
+ *
+ * @awb_raw_buffer: auto white balance meta data &ipu3_uapi_awb_raw_buffer
+ * @ae_raw_buffer: auto exposure raw data &ipu3_uapi_ae_raw_buffer_aligned
+ *                 current Imgu does not output the auto exposure statistics
+ *                 to ae_raw_buffer, the user such as 3A algorithm can use the
+ *                 RGB table in &ipu3_uapi_awb_raw_buffer to do auto-exposure.
+ * @af_raw_buffer: &ipu3_uapi_af_raw_buffer for auto focus meta data
+ * @awb_fr_raw_buffer: value as specified by &ipu3_uapi_awb_fr_raw_buffer
+ * @stats_4a_config: 4a statistics config as defined by &ipu3_uapi_4a_config.
+ * @ae_join_buffers: 1 to use ae_raw_buffer.
+ * @padding: padding config
+ * @stats_3a_bubble_per_stripe: a &ipu3_uapi_stats_3a_bubble_info_per_stripe
+ * @stats_3a_status: 3a statistics status set in &ipu3_uapi_ff_status
+ */
+struct ipu3_uapi_stats_3a {
+	struct ipu3_uapi_awb_raw_buffer awb_raw_buffer;
+	struct ipu3_uapi_ae_raw_buffer_aligned
+			ae_raw_buffer[IPU3_UAPI_MAX_STRIPES];
+	struct ipu3_uapi_af_raw_buffer af_raw_buffer;
+	struct ipu3_uapi_awb_fr_raw_buffer awb_fr_raw_buffer;
+	struct ipu3_uapi_4a_config stats_4a_config;
+	__u32 ae_join_buffers;
+	__u8 padding[28];
+	struct ipu3_uapi_stats_3a_bubble_info_per_stripe
+			stats_3a_bubble_per_stripe;
+	struct ipu3_uapi_ff_status stats_3a_status;
+} __packed;
+
+/******************* ipu3_uapi_acc_param *******************/
+
+#define IPU3_UAPI_ISP_VEC_ELEMS				64
+#define IPU3_UAPI_ISP_TNR3_VMEM_LEN			9
+
+#define IPU3_UAPI_BNR_LUT_SIZE				32
+
+/* number of elements in gamma correction LUT */
+#define IPU3_UAPI_GAMMA_CORR_LUT_ENTRIES		256
+
+/* largest grid is 73x56, for grid_height_per_slice of 2, 73x2 = 146 */
+#define IPU3_UAPI_SHD_MAX_CELLS_PER_SET			146
+#define IPU3_UAPI_SHD_MAX_CFG_SETS			28
+/* Normalization shift aka nf */
+#define IPU3_UAPI_SHD_BLGR_NF_SHIFT			13
+#define IPU3_UAPI_SHD_BLGR_NF_MASK			7
+
+#define IPU3_UAPI_YUVP2_TCC_MACC_TABLE_ELEMENTS		16
+#define IPU3_UAPI_YUVP2_TCC_INV_Y_LUT_ELEMENTS		14
+#define IPU3_UAPI_YUVP2_TCC_GAIN_PCWL_LUT_ELEMENTS	258
+#define IPU3_UAPI_YUVP2_TCC_R_SQR_LUT_ELEMENTS		24
+
+#define IPU3_UAPI_ANR_LUT_SIZE				26
+#define IPU3_UAPI_ANR_PYRAMID_SIZE			22
+
+#define IPU3_UAPI_LIN_LUT_SIZE				64
+
+/* Bayer Noise Reduction related structs */
+
+/**
+ * struct ipu3_uapi_bnr_static_config_wb_gains_config - White balance gains
+ *
+ * @gr:	white balance gain for Gr channel.
+ * @r:	white balance gain for R channel.
+ * @b:	white balance gain for B channel.
+ * @gb:	white balance gain for Gb channel.
+ *
+ * For BNR parameters WB gain factor for the three channels [Ggr, Ggb, Gb, Gr].
+ * Their precision is U3.13 and the range is (0, 8) and the actual gain is
+ * Gx + 1, it is typically Gx = 1.
+ *
+ * Pout = {Pin * (1 + Gx)}.
+ */
+struct ipu3_uapi_bnr_static_config_wb_gains_config {
+	__u16 gr;
+	__u16 r;
+	__u16 b;
+	__u16 gb;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_wb_gains_thr_config - Threshold config
+ *
+ * @gr:	white balance threshold gain for Gr channel.
+ * @r:	white balance threshold gain for R channel.
+ * @b:	white balance threshold gain for B channel.
+ * @gb:	white balance threshold gain for Gb channel.
+ *
+ * Defines the threshold that specifies how different a defect pixel can be from
+ * its neighbors.(used by dynamic defect pixel correction sub block)
+ * Precision u4.4 range [0, 8].
+ */
+struct ipu3_uapi_bnr_static_config_wb_gains_thr_config {
+	__u8 gr;
+	__u8 r;
+	__u8 b;
+	__u8 gb;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_thr_coeffs_config - Noise model
+ *				coefficients that controls noise threshold
+ *
+ * @cf:	Free coefficient for threshold calculation, range [0, 8191], default 0.
+ * @reserved0:	reserved
+ * @cg:	Gain coefficient for threshold calculation, [0, 31], default 8.
+ * @ci:	Intensity coefficient for threshold calculation. range [0, 0x1f]
+ *	default 6.
+ *	format: u3.2 (3 most significant bits represent whole number,
+ *	2 least significant bits represent the fractional part
+ *	with each count representing 0.25)
+ *	e.g. 6 in binary format is 00110, that translates to 1.5
+ * @reserved1:	reserved
+ * @r_nf:	Normalization shift value for r^2 calculation, range [12, 20]
+ *		where r is a radius of pixel [row, col] from centor of sensor.
+ *		default 14.
+ *
+ * Threshold used to distinguish between noise and details.
+ */
+struct ipu3_uapi_bnr_static_config_thr_coeffs_config {
+	__u32 cf:13;
+	__u32 reserved0:3;
+	__u32 cg:5;
+	__u32 ci:5;
+	__u32 reserved1:1;
+	__u32 r_nf:5;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_thr_ctrl_shd_config - Shading config
+ *
+ * @gr:	Coefficient defines lens shading gain approximation for gr channel
+ * @r:	Coefficient defines lens shading gain approximation for r channel
+ * @b:	Coefficient defines lens shading gain approximation for b channel
+ * @gb:	Coefficient defines lens shading gain approximation for gb channel
+ *
+ * Parameters for noise model (NM) adaptation of BNR due to shading correction.
+ * All above have precision of u3.3, default to 0.
+ */
+struct ipu3_uapi_bnr_static_config_thr_ctrl_shd_config {
+	__u8 gr;
+	__u8 r;
+	__u8 b;
+	__u8 gb;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_opt_center_config - Optical center config
+ *
+ * @x_reset:	Reset value of X (col start - X center). Precision s12.0.
+ * @reserved0:	reserved
+ * @y_reset:	Reset value of Y (row start - Y center). Precision s12.0.
+ * @reserved2:	reserved
+ *
+ * Distance from corner to optical center for NM adaptation due to shading
+ * correction (should be calculated based on shading tables)
+ */
+struct ipu3_uapi_bnr_static_config_opt_center_config {
+	__s32 x_reset:13;
+	__u32 reserved0:3;
+	__s32 y_reset:13;
+	__u32 reserved2:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_lut_config - BNR square root lookup table
+ *
+ * @values: pre-calculated values of square root function.
+ *
+ * LUT implementation of square root operation.
+ */
+struct ipu3_uapi_bnr_static_config_lut_config {
+	__u8 values[IPU3_UAPI_BNR_LUT_SIZE];
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_bp_ctrl_config - Detect bad pixels (bp)
+ *
+ * @bp_thr_gain:	Defines the threshold that specifies how different a
+ *			defect pixel can be from its neighbors. Threshold is
+ *			dependent on de-noise threshold calculated by algorithm.
+ *			Range [4, 31], default 4.
+ * @reserved0:	reserved
+ * @defect_mode:	Mode of addressed defect pixels,
+ *			0 - single defect pixel is expected,
+ *			1 - 2 adjacent defect pixels are expected, default 1.
+ * @bp_gain:	Defines how 2nd derivation that passes through a defect pixel
+ *		is different from 2nd derivations that pass through
+ *		neighbor pixels. u4.2, range [0, 256], default 8.
+ * @reserved1:	reserved
+ * @w0_coeff:	Blending coefficient of defect pixel correction.
+ *		Precision u4, range [0, 8], default 8.
+ * @reserved2:	reserved
+ * @w1_coeff:	Enable influence of incorrect defect pixel correction to be
+ *		avoided. Precision u4, range [1, 8], default 8.
+ * @reserved3:	reserved
+ */
+struct ipu3_uapi_bnr_static_config_bp_ctrl_config {
+	__u32 bp_thr_gain:5;
+	__u32 reserved0:2;
+	__u32 defect_mode:1;
+	__u32 bp_gain:6;
+	__u32 reserved1:18;
+	__u32 w0_coeff:4;
+	__u32 reserved2:4;
+	__u32 w1_coeff:4;
+	__u32 reserved3:20;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_dn_detect_ctrl_config - Denoising config
+ *
+ * @alpha:	Weight of central element of smoothing filter.
+ * @beta:	Weight of peripheral elements of smoothing filter, default 4.
+ * @gamma:	Weight of diagonal elements of smoothing filter, default 4.
+ *
+ * beta and gamma parameter define the strength of the noise removal filter.
+ *		All above has precision u0.4, range [0, 0xf]
+ *		format: u0.4 (no / zero bits represent whole number,
+ *		4 bits represent the fractional part
+ *		with each count representing 0.0625)
+ *		e.g. 0xf translates to 0.0625x15 = 0.9375
+ *
+ * @reserved0:	reserved
+ * @max_inf:	Maximum increase of peripheral or diagonal element influence
+ *		relative to the pre-defined value range: [0x5, 0xa]
+ * @reserved1:	reserved
+ * @gd_enable:	Green disparity enable control, 0 - disable, 1 - enable.
+ * @bpc_enable:	Bad pixel correction enable control, 0 - disable, 1 - enable.
+ * @bnr_enable:	Bayer noise removal enable control, 0 - disable, 1 - enable.
+ * @ff_enable:	Fixed function enable, 0 - disable, 1 - enable.
+ * @reserved2:	reserved
+ */
+struct ipu3_uapi_bnr_static_config_dn_detect_ctrl_config {
+	__u32 alpha:4;
+	__u32 beta:4;
+	__u32 gamma:4;
+	__u32 reserved0:4;
+	__u32 max_inf:4;
+	__u32 reserved1:7;
+	__u32 gd_enable:1;
+	__u32 bpc_enable:1;
+	__u32 bnr_enable:1;
+	__u32 ff_enable:1;
+	__u32 reserved2:1;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_opt_center_sqr_config - BNR optical square
+ *
+ * @x_sqr_reset: Reset value of X^2.
+ * @y_sqr_reset: Reset value of Y^2.
+ *
+ * Please note:
+ *
+ *    #. X and Y ref to
+ *       &ipu3_uapi_bnr_static_config_opt_center_config
+ *    #. Both structs are used in threshold formula to calculate r^2, where r
+ *       is a radius of pixel [row, col] from centor of sensor.
+ */
+struct ipu3_uapi_bnr_static_config_opt_center_sqr_config {
+	__u32 x_sqr_reset;
+	__u32 y_sqr_reset;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config - BNR static config
+ *
+ * @wb_gains:	white balance gains &ipu3_uapi_bnr_static_config_wb_gains_config
+ * @wb_gains_thr:	white balance gains threshold as defined by
+ *			&ipu3_uapi_bnr_static_config_wb_gains_thr_config
+ * @thr_coeffs:	coefficients of threshold
+ *		&ipu3_uapi_bnr_static_config_thr_coeffs_config
+ * @thr_ctrl_shd:	control of shading threshold
+ *			&ipu3_uapi_bnr_static_config_thr_ctrl_shd_config
+ * @opt_center:	optical center &ipu3_uapi_bnr_static_config_opt_center_config
+ *
+ * Above parameters and opt_center_sqr are used for white balance and shading.
+ *
+ * @lut:	lookup table &ipu3_uapi_bnr_static_config_lut_config
+ * @bp_ctrl:	detect and remove bad pixels as defined in struct
+ *		&ipu3_uapi_bnr_static_config_bp_ctrl_config
+ * @dn_detect_ctrl:	detect and remove noise.
+ *			&ipu3_uapi_bnr_static_config_dn_detect_ctrl_config
+ * @column_size:	The number of pixels in column.
+ * @opt_center_sqr:	Reset value of r^2 to optical center, see
+ *			&ipu3_uapi_bnr_static_config_opt_center_sqr_config.
+ */
+struct ipu3_uapi_bnr_static_config {
+	struct ipu3_uapi_bnr_static_config_wb_gains_config wb_gains;
+	struct ipu3_uapi_bnr_static_config_wb_gains_thr_config wb_gains_thr;
+	struct ipu3_uapi_bnr_static_config_thr_coeffs_config thr_coeffs;
+	struct ipu3_uapi_bnr_static_config_thr_ctrl_shd_config thr_ctrl_shd;
+	struct ipu3_uapi_bnr_static_config_opt_center_config opt_center;
+	struct ipu3_uapi_bnr_static_config_lut_config lut;
+	struct ipu3_uapi_bnr_static_config_bp_ctrl_config bp_ctrl;
+	struct ipu3_uapi_bnr_static_config_dn_detect_ctrl_config dn_detect_ctrl;
+	__u32 column_size;
+	struct ipu3_uapi_bnr_static_config_opt_center_sqr_config opt_center_sqr;
+} __packed;
+
+/**
+ * struct ipu3_uapi_bnr_static_config_green_disparity - Correct green disparity
+ *
+ * @gd_red:	Shading gain coeff for gr disparity level in bright red region.
+ *		Precision u0.6, default 4(0.0625).
+ * @reserved0:	reserved
+ * @gd_green:	Shading gain coeff for gr disparity level in bright green
+ *		region. Precision u0.6, default 4(0.0625).
+ * @reserved1:	reserved
+ * @gd_blue:	Shading gain coeff for gr disparity level in bright blue region.
+ *		Precision u0.6, default 4(0.0625).
+ * @reserved2:	reserved
+ * @gd_black:	Maximal green disparity level in dark region (stronger disparity
+ *		assumed to be image detail). Precision u14, default 80.
+ * @reserved3:	reserved
+ * @gd_shading:	Change maximal green disparity level according to square
+ *		distance from image center.
+ * @reserved4:	reserved
+ * @gd_support:	Lower bound for the number of second green color pixels in
+ *		current pixel neighborhood with less than threshold difference
+ *		from it.
+ *
+ * The shading gain coeff of red, green, blue and black are used to calculate
+ * threshold given a pixel's color value and its coordinates in the image.
+ *
+ * @reserved5:	reserved
+ * @gd_clip:	Turn green disparity clip on/off, [0, 1], default 1.
+ * @gd_central_weight:	Central pixel weight in 9 pixels weighted sum.
+ */
+struct ipu3_uapi_bnr_static_config_green_disparity {
+	__u32 gd_red:6;
+	__u32 reserved0:2;
+	__u32 gd_green:6;
+	__u32 reserved1:2;
+	__u32 gd_blue:6;
+	__u32 reserved2:10;
+	__u32 gd_black:14;
+	__u32 reserved3:2;
+	__u32 gd_shading:7;
+	__u32 reserved4:1;
+	__u32 gd_support:2;
+	__u32 reserved5:1;
+	__u32 gd_clip:1;
+	__u32 gd_central_weight:4;
+} __packed;
+
+/**
+ * struct ipu3_uapi_dm_config - De-mosaic parameters
+ *
+ * @dm_en:	de-mosaic enable.
+ * @ch_ar_en:	Checker artifacts removal enable flag. Default 0.
+ * @fcc_en:	False color correction (FCC) enable flag. Default 0.
+ * @reserved0:	reserved
+ * @frame_width:	do not care
+ * @gamma_sc:	Sharpening coefficient (coefficient of 2-d derivation of
+ *		complementary color in Hamilton-Adams interpolation).
+ *		u5, range [0, 31], default 8.
+ * @reserved1:	reserved
+ * @lc_ctrl:	Parameter that controls weights of Chroma Homogeneity metric
+ *		in calculation of final homogeneity metric.
+ *		u5, range [0, 31], default 7.
+ * @reserved2:	reserved
+ * @cr_param1:	First parameter that defines Checker artifact removal
+ *		feature gain. Precision u5, range [0, 31], default 8.
+ * @reserved3:	reserved
+ * @cr_param2:	Second parameter that defines Checker artifact removal
+ *		feature gain. Precision u5, range [0, 31], default 8.
+ * @reserved4:	reserved
+ * @coring_param:	Defines power of false color correction operation.
+ *			low for preserving edge colors, high for preserving gray
+ *			edge artifacts.
+ *			Precision u1.4, range [0, 1.9375], default 4 (0.25).
+ * @reserved5:	reserved
+ *
+ * The demosaic fixed function block is responsible to covert Bayer(mosaiced)
+ * images into color images based on demosaicing algorithm.
+ */
+struct ipu3_uapi_dm_config {
+	__u32 dm_en:1;
+	__u32 ch_ar_en:1;
+	__u32 fcc_en:1;
+	__u32 reserved0:13;
+	__u32 frame_width:16;
+
+	__u32 gamma_sc:5;
+	__u32 reserved1:3;
+	__u32 lc_ctrl:5;
+	__u32 reserved2:3;
+	__u32 cr_param1:5;
+	__u32 reserved3:3;
+	__u32 cr_param2:5;
+	__u32 reserved4:3;
+
+	__u32 coring_param:5;
+	__u32 reserved5:27;
+} __packed;
+
+/**
+ * struct ipu3_uapi_ccm_mat_config - Color correction matrix
+ *
+ * @coeff_m11: CCM 3x3 coefficient, range [-65536, 65535]
+ * @coeff_m12: CCM 3x3 coefficient, range [-8192, 8191]
+ * @coeff_m13: CCM 3x3 coefficient, range [-32768, 32767]
+ * @coeff_o_r: Bias 3x1 coefficient, range [-8191, 8181]
+ * @coeff_m21: CCM 3x3 coefficient, range [-32767, 32767]
+ * @coeff_m22: CCM 3x3 coefficient, range [-8192, 8191]
+ * @coeff_m23: CCM 3x3 coefficient, range [-32768, 32767]
+ * @coeff_o_g: Bias 3x1 coefficient, range [-8191, 8181]
+ * @coeff_m31: CCM 3x3 coefficient, range [-32768, 32767]
+ * @coeff_m32: CCM 3x3 coefficient, range [-8192, 8191]
+ * @coeff_m33: CCM 3x3 coefficient, range [-32768, 32767]
+ * @coeff_o_b: Bias 3x1 coefficient, range [-8191, 8181]
+ *
+ * Transform sensor specific color space to standard sRGB by applying 3x3 matrix
+ * and adding a bias vector O. The transformation is basically a rotation and
+ * translation in the 3-dimensional color spaces. Here are the defaults:
+ *
+ *	9775,	-2671,	1087,	0
+ *	-1071,	8303,	815,	0
+ *	-23,	-7887,	16103,	0
+ */
+struct ipu3_uapi_ccm_mat_config {
+	__s16 coeff_m11;
+	__s16 coeff_m12;
+	__s16 coeff_m13;
+	__s16 coeff_o_r;
+	__s16 coeff_m21;
+	__s16 coeff_m22;
+	__s16 coeff_m23;
+	__s16 coeff_o_g;
+	__s16 coeff_m31;
+	__s16 coeff_m32;
+	__s16 coeff_m33;
+	__s16 coeff_o_b;
+} __packed;
+
+/**
+ * struct ipu3_uapi_gamma_corr_ctrl - Gamma correction
+ *
+ * @enable: gamma correction enable.
+ * @reserved: reserved
+ */
+struct ipu3_uapi_gamma_corr_ctrl {
+	__u32 enable:1;
+	__u32 reserved:31;
+} __packed;
+
+/**
+ * struct ipu3_uapi_gamma_corr_lut - Per-pixel tone mapping implemented as LUT.
+ *
+ * @lut:	256 tabulated values of the gamma function. LUT[1].. LUT[256]
+ *		format u13.0, range [0, 8191].
+ *
+ * The tone mapping operation is done by a Piece wise linear graph
+ * that is implemented as a lookup table(LUT). The pixel component input
+ * intensity is the X-axis of the graph which is the table entry.
+ */
+struct ipu3_uapi_gamma_corr_lut {
+	__u16 lut[IPU3_UAPI_GAMMA_CORR_LUT_ENTRIES];
+} __packed;
+
+/**
+ * struct ipu3_uapi_gamma_config - Gamma config
+ *
+ * @gc_ctrl: control of gamma correction &ipu3_uapi_gamma_corr_ctrl
+ * @gc_lut: lookup table of gamma correction &ipu3_uapi_gamma_corr_lut
+ */
+struct ipu3_uapi_gamma_config {
+	struct ipu3_uapi_gamma_corr_ctrl gc_ctrl __attribute__((aligned(32)));
+	struct ipu3_uapi_gamma_corr_lut gc_lut __attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_csc_mat_config - Color space conversion matrix config
+ *
+ * @coeff_c11:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_c12:	Conversion matrix value, format s0.14, range [-8192, 8191].
+ * @coeff_c13:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_b1:	Bias 3x1 coefficient, s13.0 range [-8192, 8191].
+ * @coeff_c21:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_c22:	Conversion matrix value, format s0.14, range [-8192, 8191].
+ * @coeff_c23:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_b2:	Bias 3x1 coefficient, s13.0 range [-8192, 8191].
+ * @coeff_c31:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_c32:	Conversion matrix value, format s0.14, range [-8192, 8191].
+ * @coeff_c33:	Conversion matrix value, format s0.14, range [-16384, 16383].
+ * @coeff_b3:	Bias 3x1 coefficient, s13.0 range [-8192, 8191].
+ *
+ * To transform each pixel from RGB to YUV (Y - brightness/luminance,
+ * UV -chroma) by applying the pixel's values by a 3x3 matrix and adding an
+ * optional bias 3x1 vector. Here are the default values for the matrix:
+ *
+ *	4898,   9617,  1867, 0,
+ *	-2410, -4732,  7143, 0,
+ *	10076, -8437, -1638, 0,
+ *
+ *	(i.e. for real number 0.299, 0.299 * 2^14 becomes 4898.)
+ */
+struct ipu3_uapi_csc_mat_config {
+	__s16 coeff_c11;
+	__s16 coeff_c12;
+	__s16 coeff_c13;
+	__s16 coeff_b1;
+	__s16 coeff_c21;
+	__s16 coeff_c22;
+	__s16 coeff_c23;
+	__s16 coeff_b2;
+	__s16 coeff_c31;
+	__s16 coeff_c32;
+	__s16 coeff_c33;
+	__s16 coeff_b3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_cds_params - Chroma down-scaling
+ *
+ * @ds_c00:	range [0, 3]
+ * @ds_c01:	range [0, 3]
+ * @ds_c02:	range [0, 3]
+ * @ds_c03:	range [0, 3]
+ * @ds_c10:	range [0, 3]
+ * @ds_c11:	range [0, 3]
+ * @ds_c12:	range [0, 3]
+ * @ds_c13:	range [0, 3]
+ *
+ * In case user does not provide, above 4x2 filter will use following defaults:
+ *	1, 3, 3, 1,
+ *	1, 3, 3, 1,
+ *
+ * @ds_nf:	Normalization factor for Chroma output downscaling filter,
+ *		range 0,4, default 2.
+ * @reserved0:	reserved
+ * @csc_en:	Color space conversion enable
+ * @uv_bin_output:	0: output YUV 4.2.0, 1: output YUV 4.2.2(default).
+ * @reserved1:	reserved
+ */
+struct ipu3_uapi_cds_params {
+	__u32 ds_c00:2;
+	__u32 ds_c01:2;
+	__u32 ds_c02:2;
+	__u32 ds_c03:2;
+	__u32 ds_c10:2;
+	__u32 ds_c11:2;
+	__u32 ds_c12:2;
+	__u32 ds_c13:2;
+	__u32 ds_nf:5;
+	__u32 reserved0:3;
+	__u32 csc_en:1;
+	__u32 uv_bin_output:1;
+	__u32 reserved1:6;
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_grid_config - Bayer shading(darkening) correction
+ *
+ * @width:	Grid horizontal dimensions, u8, [8, 128], default 73
+ * @height:	Grid vertical dimensions, u8, [8, 128], default 56
+ * @block_width_log2:	Log2 of the width of the grid cell in pixel count
+ *			u4, [0, 15], default value 5.
+ * @reserved0:	reserved
+ * @block_height_log2:	Log2 of the height of the grid cell in pixel count
+ *			u4, [0, 15], default value 6.
+ * @reserved1:	reserved
+ * @grid_height_per_slice:	SHD_MAX_CELLS_PER_SET/width.
+ *				(with SHD_MAX_CELLS_PER_SET = 146).
+ * @x_start:	X value of top left corner of sensor relative to ROI
+ *		s13, [-4096, 0], default 0, only negative values.
+ * @y_start:	Y value of top left corner of sensor relative to ROI
+ *		s13, [-4096, 0], default 0, only negative values.
+ */
+struct ipu3_uapi_shd_grid_config {
+	/* reg 0 */
+	__u8 width;
+	__u8 height;
+	__u8 block_width_log2:3;
+	__u8 reserved0:1;
+	__u8 block_height_log2:3;
+	__u8 reserved1:1;
+	__u8 grid_height_per_slice;
+	/* reg 1 */
+	__s16 x_start;
+	__s16 y_start;
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_general_config - Shading general config
+ *
+ * @init_set_vrt_offst_ul: set vertical offset,
+ *			y_start >> block_height_log2 % grid_height_per_slice.
+ * @shd_enable: shading enable.
+ * @gain_factor: Gain factor. Shift calculated anti shading value. Precision u2.
+ *		0x0 - gain factor [1, 5], means no shift interpolated value.
+ *		0x1 - gain factor [1, 9], means shift interpolated by 1.
+ *		0x2 - gain factor [1, 17], means shift interpolated by 2.
+ * @reserved: reserved
+ *
+ * Correction is performed by multiplying a gain factor for each of the 4 Bayer
+ * channels as a function of the pixel location in the sensor.
+ */
+struct ipu3_uapi_shd_general_config {
+	__u32 init_set_vrt_offst_ul:8;
+	__u32 shd_enable:1;
+	__u32 gain_factor:2;
+	__u32 reserved:21;
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_black_level_config - Black level correction
+ *
+ * @bl_r:	Bios values for green red. s11 range [-2048, 2047].
+ * @bl_gr:	Bios values for green blue. s11 range [-2048, 2047].
+ * @bl_gb:	Bios values for red. s11 range [-2048, 2047].
+ * @bl_b:	Bios values for blue. s11 range [-2048, 2047].
+ */
+struct ipu3_uapi_shd_black_level_config {
+	__s16 bl_r;
+	__s16 bl_gr;
+	__s16 bl_gb;
+	__s16 bl_b;
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_config_static - Shading config static
+ *
+ * @grid:	shading grid config &ipu3_uapi_shd_grid_config
+ * @general:	shading general config &ipu3_uapi_shd_general_config
+ * @black_level:	black level config for shading correction as defined by
+ *			&ipu3_uapi_shd_black_level_config
+ */
+struct ipu3_uapi_shd_config_static {
+	struct ipu3_uapi_shd_grid_config grid;
+	struct ipu3_uapi_shd_general_config general;
+	struct ipu3_uapi_shd_black_level_config black_level;
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_lut - Shading gain factor lookup table.
+ *
+ * @sets: array
+ * @sets.r_and_gr: Red and GreenR Lookup table.
+ * @sets.r_and_gr.r: Red shading factor.
+ * @sets.r_and_gr.gr: GreenR shading factor.
+ * @sets.reserved1: reserved
+ * @sets.gb_and_b: GreenB and Blue Lookup table.
+ * @sets.gb_and_b.gb: GreenB shading factor.
+ * @sets.gb_and_b.b: Blue shading factor.
+ * @sets.reserved2: reserved
+ *
+ * Map to shading correction LUT register set.
+ */
+struct ipu3_uapi_shd_lut {
+	struct {
+		struct {
+			__u16 r;
+			__u16 gr;
+		} r_and_gr[IPU3_UAPI_SHD_MAX_CELLS_PER_SET];
+		__u8 reserved1[24];
+		struct {
+			__u16 gb;
+			__u16 b;
+		} gb_and_b[IPU3_UAPI_SHD_MAX_CELLS_PER_SET];
+		__u8 reserved2[24];
+	} sets[IPU3_UAPI_SHD_MAX_CFG_SETS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_shd_config - Shading config
+ *
+ * @shd:	shading static config, see &ipu3_uapi_shd_config_static
+ * @shd_lut:	shading lookup table &ipu3_uapi_shd_lut
+ */
+struct ipu3_uapi_shd_config {
+	struct ipu3_uapi_shd_config_static shd __attribute__((aligned(32)));
+	struct ipu3_uapi_shd_lut shd_lut __attribute__((aligned(32)));
+} __packed;
+
+/* Image Enhancement Filter directed */
+
+/**
+ * struct ipu3_uapi_iefd_cux2 - IEFd Config Unit 2 parameters
+ *
+ * @x0:		X0 point of Config Unit, u9.0, default 0.
+ * @x1:		X1 point of Config Unit, u9.0, default 0.
+ * @a01:	Slope A of Config Unit, s4.4, default 0.
+ * @b01:	Slope B, always 0.
+ *
+ * Calculate weight for blending directed and non-directed denoise elements
+ *
+ * Note:
+ * Each instance of Config Unit needs X coordinate of n points and
+ * slope A factor between points calculated by driver based on calibration
+ * parameters.
+ *
+ * All CU inputs are unsigned, they will be converted to signed when written
+ * to register, i.e. a01 will be written to 9 bit register in s4.4 format.
+ * The data precision s4.4 means 4 bits for integer parts and 4 bits for the
+ * fractional part, the first bit indicates positive or negative value.
+ * For userspace software (commonly the imaging library), the computation for
+ * the CU slope values should be based on the slope resolution 1/16 (binary
+ * 0.0001 - the minimal interval value), the slope value range is [-256, +255].
+ * This applies to &ipu3_uapi_iefd_cux6_ed, &ipu3_uapi_iefd_cux2_1,
+ * &ipu3_uapi_iefd_cux2_1, &ipu3_uapi_iefd_cux4 and &ipu3_uapi_iefd_cux6_rad.
+ */
+struct ipu3_uapi_iefd_cux2 {
+	__u32 x0:9;
+	__u32 x1:9;
+	__u32 a01:9;
+	__u32 b01:5;
+} __packed;
+
+/**
+ * struct ipu3_uapi_iefd_cux6_ed - Calculate power of non-directed sharpening
+ *				   element, Config Unit 6 for edge detail (ED).
+ *
+ * @x0:	X coordinate of point 0, u9.0, default 0.
+ * @x1:	X coordinate of point 1, u9.0, default 0.
+ * @x2:	X coordinate of point 2, u9.0, default 0.
+ * @reserved0:	reserved
+ * @x3:	X coordinate of point 3, u9.0, default 0.
+ * @x4:	X coordinate of point 4, u9.0, default 0.
+ * @x5:	X coordinate of point 5, u9.0, default 0.
+ * @reserved1:	reserved
+ * @a01:	slope A points 01, s4.4, default 0.
+ * @a12:	slope A points 12, s4.4, default 0.
+ * @a23:	slope A points 23, s4.4, default 0.
+ * @reserved2:	reserved
+ * @a34:	slope A points 34, s4.4, default 0.
+ * @a45:	slope A points 45, s4.4, default 0.
+ * @reserved3:	reserved
+ * @b01:	slope B points 01, s4.4, default 0.
+ * @b12:	slope B points 12, s4.4, default 0.
+ * @b23:	slope B points 23, s4.4, default 0.
+ * @reserved4:	reserved
+ * @b34:	slope B points 34, s4.4, default 0.
+ * @b45:	slope B points 45, s4.4, default 0.
+ * @reserved5:	reserved.
+ */
+struct ipu3_uapi_iefd_cux6_ed {
+	__u32 x0:9;
+	__u32 x1:9;
+	__u32 x2:9;
+	__u32 reserved0:5;
+
+	__u32 x3:9;
+	__u32 x4:9;
+	__u32 x5:9;
+	__u32 reserved1:5;
+
+	__u32 a01:9;
+	__u32 a12:9;
+	__u32 a23:9;
+	__u32 reserved2:5;
+
+	__u32 a34:9;
+	__u32 a45:9;
+	__u32 reserved3:14;
+
+	__u32 b01:9;
+	__u32 b12:9;
+	__u32 b23:9;
+	__u32 reserved4:5;
+
+	__u32 b34:9;
+	__u32 b45:9;
+	__u32 reserved5:14;
+} __packed;
+
+/**
+ * struct ipu3_uapi_iefd_cux2_1 - Calculate power of non-directed denoise
+ *				  element apply.
+ * @x0: X0 point of Config Unit, u9.0, default 0.
+ * @x1: X1 point of Config Unit, u9.0, default 0.
+ * @a01: Slope A of Config Unit, s4.4, default 0.
+ * @reserved1: reserved
+ * @b01: offset B0 of Config Unit, u7.0, default 0.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_iefd_cux2_1 {
+	__u32 x0:9;
+	__u32 x1:9;
+	__u32 a01:9;
+	__u32 reserved1:5;
+
+	__u32 b01:8;
+	__u32 reserved2:24;
+} __packed;
+
+/**
+ * struct ipu3_uapi_iefd_cux4 - Calculate power of non-directed sharpening
+ *				element.
+ *
+ * @x0:	X0 point of Config Unit, u9.0, default 0.
+ * @x1:	X1 point of Config Unit, u9.0, default 0.
+ * @x2:	X2 point of Config Unit, u9.0, default 0.
+ * @reserved0:	reserved
+ * @x3:	X3 point of Config Unit, u9.0, default 0.
+ * @a01:	Slope A0 of Config Unit, s4.4, default 0.
+ * @a12:	Slope A1 of Config Unit, s4.4, default 0.
+ * @reserved1:	reserved
+ * @a23:	Slope A2 of Config Unit, s4.4, default 0.
+ * @b01:	Offset B0 of Config Unit, s7.0, default 0.
+ * @b12:	Offset B1 of Config Unit, s7.0, default 0.
+ * @reserved2:	reserved
+ * @b23:	Offset B2 of Config Unit, s7.0, default 0.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_iefd_cux4 {
+	__u32 x0:9;
+	__u32 x1:9;
+	__u32 x2:9;
+	__u32 reserved0:5;
+
+	__u32 x3:9;
+	__u32 a01:9;
+	__u32 a12:9;
+	__u32 reserved1:5;
+
+	__u32 a23:9;
+	__u32 b01:8;
+	__u32 b12:8;
+	__u32 reserved2:7;
+
+	__u32 b23:8;
+	__u32 reserved3:24;
+} __packed;
+
+/**
+ * struct ipu3_uapi_iefd_cux6_rad - Radial Config Unit (CU)
+ *
+ * @x0:	x0 points of Config Unit radial, u8.0
+ * @x1:	x1 points of Config Unit radial, u8.0
+ * @x2:	x2 points of Config Unit radial, u8.0
+ * @x3:	x3 points of Config Unit radial, u8.0
+ * @x4:	x4 points of Config Unit radial, u8.0
+ * @x5:	x5 points of Config Unit radial, u8.0
+ * @reserved1: reserved
+ * @a01:	Slope A of Config Unit radial, s7.8
+ * @a12:	Slope A of Config Unit radial, s7.8
+ * @a23:	Slope A of Config Unit radial, s7.8
+ * @a34:	Slope A of Config Unit radial, s7.8
+ * @a45:	Slope A of Config Unit radial, s7.8
+ * @reserved2: reserved
+ * @b01:	Slope B of Config Unit radial, s9.0
+ * @b12:	Slope B of Config Unit radial, s9.0
+ * @b23:	Slope B of Config Unit radial, s9.0
+ * @reserved4: reserved
+ * @b34:	Slope B of Config Unit radial, s9.0
+ * @b45:	Slope B of Config Unit radial, s9.0
+ * @reserved5: reserved
+ */
+struct ipu3_uapi_iefd_cux6_rad {
+	__u32 x0:8;
+	__u32 x1:8;
+	__u32 x2:8;
+	__u32 x3:8;
+
+	__u32 x4:8;
+	__u32 x5:8;
+	__u32 reserved1:16;
+
+	__u32 a01:16;
+	__u32 a12:16;
+
+	__u32 a23:16;
+	__u32 a34:16;
+
+	__u32 a45:16;
+	__u32 reserved2:16;
+
+	__u32 b01:10;
+	__u32 b12:10;
+	__u32 b23:10;
+	__u32 reserved4:2;
+
+	__u32 b34:10;
+	__u32 b45:10;
+	__u32 reserved5:12;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_cfg_units - IEFd Config Units parameters
+ *
+ * @cu_1: calculate weight for blending directed and
+ *	  non-directed denoise elements. See &ipu3_uapi_iefd_cux2
+ * @cu_ed: calculate power of non-directed sharpening element, see
+ *	   &ipu3_uapi_iefd_cux6_ed
+ * @cu_3: calculate weight for blending directed and
+ *	  non-directed denoise elements. A &ipu3_uapi_iefd_cux2
+ * @cu_5: calculate power of non-directed denoise element apply, use
+ *	  &ipu3_uapi_iefd_cux2_1
+ * @cu_6: calculate power of non-directed sharpening element. See
+ *	  &ipu3_uapi_iefd_cux4
+ * @cu_7: calculate weight for blending directed and
+ *	  non-directed denoise elements. Use &ipu3_uapi_iefd_cux2
+ * @cu_unsharp: Config Unit of unsharp &ipu3_uapi_iefd_cux4
+ * @cu_radial: Config Unit of radial &ipu3_uapi_iefd_cux6_rad
+ * @cu_vssnlm: Config Unit of vssnlm &ipu3_uapi_iefd_cux2
+ */
+struct ipu3_uapi_yuvp1_iefd_cfg_units {
+	struct ipu3_uapi_iefd_cux2 cu_1;
+	struct ipu3_uapi_iefd_cux6_ed cu_ed;
+	struct ipu3_uapi_iefd_cux2 cu_3;
+	struct ipu3_uapi_iefd_cux2_1 cu_5;
+	struct ipu3_uapi_iefd_cux4 cu_6;
+	struct ipu3_uapi_iefd_cux2 cu_7;
+	struct ipu3_uapi_iefd_cux4 cu_unsharp;
+	struct ipu3_uapi_iefd_cux6_rad cu_radial;
+	struct ipu3_uapi_iefd_cux2 cu_vssnlm;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_config_s - IEFd config
+ *
+ * @horver_diag_coeff: Gradient compensation. Compared with vertical /
+ *		       horizontal (0 / 90 degree), coefficient of diagonal (45 /
+ *		       135 degree) direction should be corrected by approx.
+ *		       1/sqrt(2).
+ * @reserved0: reserved
+ * @clamp_stitch: Slope to stitch between clamped and unclamped edge values
+ * @reserved1: reserved
+ * @direct_metric_update: Update coeff for direction metric
+ * @reserved2: reserved
+ * @ed_horver_diag_coeff: Radial Coefficient that compensates for
+ *			  different distance for vertical/horizontal and
+ *			  diagonal gradient calculation (approx. 1/sqrt(2))
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp1_iefd_config_s {
+	__u32 horver_diag_coeff:7;
+	__u32 reserved0:1;
+	__u32 clamp_stitch:6;
+	__u32 reserved1:2;
+	__u32 direct_metric_update:5;
+	__u32 reserved2:3;
+	__u32 ed_horver_diag_coeff:7;
+	__u32 reserved3:1;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_control - IEFd control
+ *
+ * @iefd_en:	Enable IEFd
+ * @denoise_en:	Enable denoise
+ * @direct_smooth_en:	Enable directional smooth
+ * @rad_en:	Enable radial update
+ * @vssnlm_en:	Enable VSSNLM output filter
+ * @reserved:	reserved
+ */
+struct ipu3_uapi_yuvp1_iefd_control {
+	__u32 iefd_en:1;
+	__u32 denoise_en:1;
+	__u32 direct_smooth_en:1;
+	__u32 rad_en:1;
+	__u32 vssnlm_en:1;
+	__u32 reserved:27;
+} __packed;
+
+/**
+ * struct ipu3_uapi_sharp_cfg - Sharpening config
+ *
+ * @nega_lmt_txt: Sharpening limit for negative overshoots for texture.
+ * @reserved0: reserved
+ * @posi_lmt_txt: Sharpening limit for positive overshoots for texture.
+ * @reserved1: reserved
+ * @nega_lmt_dir: Sharpening limit for negative overshoots for direction (edge).
+ * @reserved2: reserved
+ * @posi_lmt_dir: Sharpening limit for positive overshoots for direction (edge).
+ * @reserved3: reserved
+ *
+ * Fixed point type u13.0, range [0, 8191].
+ */
+struct ipu3_uapi_sharp_cfg {
+	__u32 nega_lmt_txt:13;
+	__u32 reserved0:19;
+	__u32 posi_lmt_txt:13;
+	__u32 reserved1:19;
+	__u32 nega_lmt_dir:13;
+	__u32 reserved2:19;
+	__u32 posi_lmt_dir:13;
+	__u32 reserved3:19;
+} __packed;
+
+/**
+ * struct ipu3_uapi_far_w - Sharpening config for far sub-group
+ *
+ * @dir_shrp:	Weight of wide direct sharpening, u1.6, range [0, 64], default 64.
+ * @reserved0:	reserved
+ * @dir_dns:	Weight of wide direct denoising, u1.6, range [0, 64], default 0.
+ * @reserved1:	reserved
+ * @ndir_dns_powr:	Power of non-direct denoising,
+ *			Precision u1.6, range [0, 64], default 64.
+ * @reserved2:	reserved
+ */
+struct ipu3_uapi_far_w {
+	__u32 dir_shrp:7;
+	__u32 reserved0:1;
+	__u32 dir_dns:7;
+	__u32 reserved1:1;
+	__u32 ndir_dns_powr:7;
+	__u32 reserved2:9;
+} __packed;
+
+/**
+ * struct ipu3_uapi_unsharp_cfg - Unsharp config
+ *
+ * @unsharp_weight: Unsharp mask blending weight.
+ *		    u1.6, range [0, 64], default 16.
+ *		    0 - disabled, 64 - use only unsharp.
+ * @reserved0: reserved
+ * @unsharp_amount: Unsharp mask amount, u4.5, range [0, 511], default 0.
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_unsharp_cfg {
+	__u32 unsharp_weight:7;
+	__u32 reserved0:1;
+	__u32 unsharp_amount:9;
+	__u32 reserved1:15;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_shrp_cfg - IEFd sharpness config
+ *
+ * @cfg: sharpness config &ipu3_uapi_sharp_cfg
+ * @far_w: wide range config, value as specified by &ipu3_uapi_far_w:
+ *	The 5x5 environment is separated into 2 sub-groups, the 3x3 nearest
+ *	neighbors (8 pixels called Near), and the second order neighborhood
+ *	around them (16 pixels called Far).
+ * @unshrp_cfg: unsharpness config. &ipu3_uapi_unsharp_cfg
+ */
+struct ipu3_uapi_yuvp1_iefd_shrp_cfg {
+	struct ipu3_uapi_sharp_cfg cfg;
+	struct ipu3_uapi_far_w far_w;
+	struct ipu3_uapi_unsharp_cfg unshrp_cfg;
+} __packed;
+
+/**
+ * struct ipu3_uapi_unsharp_coef0 - Unsharp mask coefficients
+ *
+ * @c00: Coeff11, s0.8, range [-255, 255], default 1.
+ * @c01: Coeff12, s0.8, range [-255, 255], default 5.
+ * @c02: Coeff13, s0.8, range [-255, 255], default 9.
+ * @reserved: reserved
+ *
+ * Configurable registers for common sharpening support.
+ */
+struct ipu3_uapi_unsharp_coef0 {
+	__u32 c00:9;
+	__u32 c01:9;
+	__u32 c02:9;
+	__u32 reserved:5;
+} __packed;
+
+/**
+ * struct ipu3_uapi_unsharp_coef1 - Unsharp mask coefficients
+ *
+ * @c11: Coeff22, s0.8, range [-255, 255], default 29.
+ * @c12: Coeff23, s0.8, range [-255, 255], default 55.
+ * @c22: Coeff33, s0.8, range [-255, 255], default 96.
+ * @reserved: reserved
+ */
+struct ipu3_uapi_unsharp_coef1 {
+	__u32 c11:9;
+	__u32 c12:9;
+	__u32 c22:9;
+	__u32 reserved:5;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_unshrp_cfg - Unsharp mask config
+ *
+ * @unsharp_coef0: unsharp coefficient 0 config. See &ipu3_uapi_unsharp_coef0
+ * @unsharp_coef1: unsharp coefficient 1 config. See &ipu3_uapi_unsharp_coef1
+ */
+struct ipu3_uapi_yuvp1_iefd_unshrp_cfg {
+	struct ipu3_uapi_unsharp_coef0 unsharp_coef0;
+	struct ipu3_uapi_unsharp_coef1 unsharp_coef1;
+} __packed;
+
+/**
+ * struct ipu3_uapi_radial_reset_xy - Radial coordinate reset
+ *
+ * @x:	Radial reset of x coordinate. Precision s12, [-4095, 4095], default 0.
+ * @reserved0:	reserved
+ * @y:	Radial center y coordinate. Precision s12, [-4095, 4095], default 0.
+ * @reserved1:	reserved
+ */
+struct ipu3_uapi_radial_reset_xy {
+	__s32 x:13;
+	__u32 reserved0:3;
+	__s32 y:13;
+	__u32 reserved1:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_radial_reset_x2 - Radial X^2 reset
+ *
+ * @x2:	Radial reset of x^2 coordinate. Precision u24, default 0.
+ * @reserved:	reserved
+ */
+struct ipu3_uapi_radial_reset_x2 {
+	__u32 x2:24;
+	__u32 reserved:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_radial_reset_y2 - Radial Y^2 reset
+ *
+ * @y2:	Radial reset of y^2 coordinate. Precision u24, default 0.
+ * @reserved:	reserved
+ */
+struct ipu3_uapi_radial_reset_y2 {
+	__u32 y2:24;
+	__u32 reserved:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_radial_cfg - Radial config
+ *
+ * @rad_nf: Radial. R^2 normalization factor is scale down by 2^ - (15 + scale)
+ * @reserved0: reserved
+ * @rad_inv_r2: Radial R^-2 normelized to (0.5..1).
+ *		Precision u7, range [0, 127].
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_radial_cfg {
+	__u32 rad_nf:4;
+	__u32 reserved0:4;
+	__u32 rad_inv_r2:7;
+	__u32 reserved1:17;
+} __packed;
+
+/**
+ * struct ipu3_uapi_rad_far_w - Radial FAR sub-group
+ *
+ * @rad_dir_far_sharp_w: Weight of wide direct sharpening, u1.6, range [0, 64],
+ *			 default 64.
+ * @rad_dir_far_dns_w: Weight of wide direct denoising, u1.6, range [0, 64],
+ *			 default 0.
+ * @rad_ndir_far_dns_power: power of non-direct sharpening, u1.6, range [0, 64],
+ *			 default 0.
+ * @reserved: reserved
+ */
+struct ipu3_uapi_rad_far_w {
+	__u32 rad_dir_far_sharp_w:8;
+	__u32 rad_dir_far_dns_w:8;
+	__u32 rad_ndir_far_dns_power:8;
+	__u32 reserved:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_cu_cfg0 - Radius Config Unit cfg0 register
+ *
+ * @cu6_pow: Power of CU6. Power of non-direct sharpening, u3.4.
+ * @reserved0: reserved
+ * @cu_unsharp_pow: Power of unsharp mask, u2.4.
+ * @reserved1: reserved
+ * @rad_cu6_pow: Radial/corner CU6. Directed sharpening power, u3.4.
+ * @reserved2: reserved
+ * @rad_cu_unsharp_pow: Radial power of unsharp mask, u2.4.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_cu_cfg0 {
+	__u32 cu6_pow:7;
+	__u32 reserved0:1;
+	__u32 cu_unsharp_pow:7;
+	__u32 reserved1:1;
+	__u32 rad_cu6_pow:7;
+	__u32 reserved2:1;
+	__u32 rad_cu_unsharp_pow:6;
+	__u32 reserved3:2;
+} __packed;
+
+/**
+ * struct ipu3_uapi_cu_cfg1 - Radius Config Unit cfg1 register
+ *
+ * @rad_cu6_x1: X1 point of Config Unit 6, precision u9.0.
+ * @reserved0: reserved
+ * @rad_cu_unsharp_x1: X1 point for Config Unit unsharp for radial/corner point
+ *			precision u9.0.
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_cu_cfg1 {
+	__u32 rad_cu6_x1:9;
+	__u32 reserved0:1;
+	__u32 rad_cu_unsharp_x1:9;
+	__u32 reserved1:13;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_rad_cfg - IEFd parameters changed radially over
+ *					 the picture plane.
+ *
+ * @reset_xy: reset xy value in radial calculation. &ipu3_uapi_radial_reset_xy
+ * @reset_x2: reset x square value in radial calculation. See struct
+ *	      &ipu3_uapi_radial_reset_x2
+ * @reset_y2: reset y square value in radial calculation. See struct
+ *	      &ipu3_uapi_radial_reset_y2
+ * @cfg: radial config defined in &ipu3_uapi_radial_cfg
+ * @rad_far_w: weight for wide range radial. &ipu3_uapi_rad_far_w
+ * @cu_cfg0: configuration unit 0. See &ipu3_uapi_cu_cfg0
+ * @cu_cfg1: configuration unit 1. See &ipu3_uapi_cu_cfg1
+ */
+struct ipu3_uapi_yuvp1_iefd_rad_cfg {
+	struct ipu3_uapi_radial_reset_xy reset_xy;
+	struct ipu3_uapi_radial_reset_x2 reset_x2;
+	struct ipu3_uapi_radial_reset_y2 reset_y2;
+	struct ipu3_uapi_radial_cfg cfg;
+	struct ipu3_uapi_rad_far_w rad_far_w;
+	struct ipu3_uapi_cu_cfg0 cu_cfg0;
+	struct ipu3_uapi_cu_cfg1 cu_cfg1;
+} __packed;
+
+/* Vssnlm - Very small scale non-local mean algorithm */
+
+/**
+ * struct ipu3_uapi_vss_lut_x - Vssnlm LUT x0/x1/x2
+ *
+ * @vs_x0: Vssnlm LUT x0, precision u8, range [0, 255], default 16.
+ * @vs_x1: Vssnlm LUT x1, precision u8, range [0, 255], default 32.
+ * @vs_x2: Vssnlm LUT x2, precision u8, range [0, 255], default 64.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_vss_lut_x {
+	__u32 vs_x0:8;
+	__u32 vs_x1:8;
+	__u32 vs_x2:8;
+	__u32 reserved2:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_vss_lut_y - Vssnlm LUT y0/y1/y2
+ *
+ * @vs_y1: Vssnlm LUT y1, precision u4, range [0, 8], default 1.
+ * @reserved0: reserved
+ * @vs_y2: Vssnlm LUT y2, precision u4, range [0, 8], default 3.
+ * @reserved1: reserved
+ * @vs_y3: Vssnlm LUT y3, precision u4, range [0, 8], default 8.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_vss_lut_y {
+	__u32 vs_y1:4;
+	__u32 reserved0:4;
+	__u32 vs_y2:4;
+	__u32 reserved1:4;
+	__u32 vs_y3:4;
+	__u32 reserved2:12;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_vssnlm_cfg - IEFd Vssnlm Lookup table
+ *
+ * @vss_lut_x: vss lookup table. See &ipu3_uapi_vss_lut_x description
+ * @vss_lut_y: vss lookup table. See &ipu3_uapi_vss_lut_y description
+ */
+struct ipu3_uapi_yuvp1_iefd_vssnlm_cfg {
+	struct ipu3_uapi_vss_lut_x vss_lut_x;
+	struct ipu3_uapi_vss_lut_y vss_lut_y;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_iefd_config - IEFd config
+ *
+ * @units: configuration unit setting, &ipu3_uapi_yuvp1_iefd_cfg_units
+ * @config: configuration, as defined by &ipu3_uapi_yuvp1_iefd_config_s
+ * @control: control setting, as defined by &ipu3_uapi_yuvp1_iefd_control
+ * @sharp: sharpness setting, as defined by &ipu3_uapi_yuvp1_iefd_shrp_cfg
+ * @unsharp: unsharpness setting, as defined by &ipu3_uapi_yuvp1_iefd_unshrp_cfg
+ * @rad: radial setting, as defined by &ipu3_uapi_yuvp1_iefd_rad_cfg
+ * @vsslnm: vsslnm setting, as defined by &ipu3_uapi_yuvp1_iefd_vssnlm_cfg
+ */
+struct ipu3_uapi_yuvp1_iefd_config {
+	struct ipu3_uapi_yuvp1_iefd_cfg_units units;
+	struct ipu3_uapi_yuvp1_iefd_config_s config;
+	struct ipu3_uapi_yuvp1_iefd_control control;
+	struct ipu3_uapi_yuvp1_iefd_shrp_cfg sharp;
+	struct ipu3_uapi_yuvp1_iefd_unshrp_cfg unsharp;
+	struct ipu3_uapi_yuvp1_iefd_rad_cfg rad;
+	struct ipu3_uapi_yuvp1_iefd_vssnlm_cfg vsslnm;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_yds_config - Y Down-Sampling config
+ *
+ * @c00: range [0, 3], default 0x0
+ * @c01: range [0, 3], default 0x1
+ * @c02: range [0, 3], default 0x1
+ * @c03: range [0, 3], default 0x0
+ * @c10: range [0, 3], default 0x0
+ * @c11: range [0, 3], default 0x1
+ * @c12: range [0, 3], default 0x1
+ * @c13: range [0, 3], default 0x0
+ *
+ * Above are 4x2 filter coefficients for chroma output downscaling.
+ *
+ * @norm_factor: Normalization factor, range [0, 4], default 2
+ *		0 - divide by 1
+ *		1 - divide by 2
+ *		2 - divide by 4
+ *		3 - divide by 8
+ *		4 - divide by 16
+ * @reserved0: reserved
+ * @bin_output: Down sampling on Luma channel in two optional modes
+ *		0 - Bin output 4.2.0 (default), 1 output 4.2.2.
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_yuvp1_yds_config {
+	__u32 c00:2;
+	__u32 c01:2;
+	__u32 c02:2;
+	__u32 c03:2;
+	__u32 c10:2;
+	__u32 c11:2;
+	__u32 c12:2;
+	__u32 c13:2;
+	__u32 norm_factor:5;
+	__u32 reserved0:4;
+	__u32 bin_output:1;
+	__u32 reserved1:6;
+} __packed;
+
+/* Chroma Noise Reduction */
+
+/**
+ * struct ipu3_uapi_yuvp1_chnr_enable_config - Chroma noise reduction enable
+ *
+ * @enable: enable/disable chroma noise reduction
+ * @yuv_mode: 0 - YUV420, 1 - YUV422
+ * @reserved0: reserved
+ * @col_size: number of columns in the frame, max width is 2560
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_yuvp1_chnr_enable_config {
+	__u32 enable:1;
+	__u32 yuv_mode:1;
+	__u32 reserved0:14;
+	__u32 col_size:12;
+	__u32 reserved1:4;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_chnr_coring_config - Coring thresholds for UV
+ *
+ * @u: U coring level, u0.13, range [0.0, 1.0], default 0.0
+ * @reserved0: reserved
+ * @v: V coring level, u0.13, range [0.0, 1.0], default 0.0
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_yuvp1_chnr_coring_config {
+	__u32 u:13;
+	__u32 reserved0:3;
+	__u32 v:13;
+	__u32 reserved1:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_chnr_sense_gain_config - Chroma noise reduction gains
+ *
+ * All sensitivity gain parameters have precision u13.0, range [0, 8191].
+ *
+ * @vy: Sensitivity of horizontal edge of Y, default 100
+ * @vu: Sensitivity of horizontal edge of U, default 100
+ * @vv: Sensitivity of horizontal edge of V, default 100
+ * @reserved0: reserved
+ * @hy: Sensitivity of vertical edge of Y, default 50
+ * @hu: Sensitivity of vertical edge of U, default 50
+ * @hv: Sensitivity of vertical edge of V, default 50
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_yuvp1_chnr_sense_gain_config {
+	__u32 vy:8;
+	__u32 vu:8;
+	__u32 vv:8;
+	__u32 reserved0:8;
+
+	__u32 hy:8;
+	__u32 hu:8;
+	__u32 hv:8;
+	__u32 reserved1:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_chnr_iir_fir_config - Chroma IIR/FIR filter config
+ *
+ * @fir_0h: Value of center tap in horizontal FIR, range [0, 32], default 8.
+ * @reserved0: reserved
+ * @fir_1h: Value of distance 1 in horizontal FIR, range [0, 32], default 12.
+ * @reserved1: reserved
+ * @fir_2h: Value of distance 2 tap in horizontal FIR, range [0, 32], default 0.
+ * @dalpha_clip_val: weight for previous row in IIR, range [1, 256], default 0.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_yuvp1_chnr_iir_fir_config {
+	__u32 fir_0h:6;
+	__u32 reserved0:2;
+	__u32 fir_1h:6;
+	__u32 reserved1:2;
+	__u32 fir_2h:6;
+	__u32 dalpha_clip_val:9;
+	__u32 reserved2:1;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_chnr_config - Chroma noise reduction config
+ *
+ * @enable: chroma noise reduction enable, see
+ *	    &ipu3_uapi_yuvp1_chnr_enable_config
+ * @coring: coring config for chroma noise reduction, see
+ *	    &ipu3_uapi_yuvp1_chnr_coring_config
+ * @sense_gain: sensitivity config for chroma noise reduction, see
+ *		ipu3_uapi_yuvp1_chnr_sense_gain_config
+ * @iir_fir: iir and fir config for chroma noise reduction, see
+ *	     ipu3_uapi_yuvp1_chnr_iir_fir_config
+ */
+struct ipu3_uapi_yuvp1_chnr_config {
+	struct ipu3_uapi_yuvp1_chnr_enable_config enable;
+	struct ipu3_uapi_yuvp1_chnr_coring_config coring;
+	struct ipu3_uapi_yuvp1_chnr_sense_gain_config sense_gain;
+	struct ipu3_uapi_yuvp1_chnr_iir_fir_config iir_fir;
+} __packed;
+
+/* Edge Enhancement and Noise Reduction */
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_lpf_config - Luma(Y) edge enhancement low-pass
+ *					       filter coefficients
+ *
+ * @a_diag: Smoothing diagonal coefficient, u5.0.
+ * @reserved0: reserved
+ * @a_periph: Image smoothing perpherial, u5.0.
+ * @reserved1: reserved
+ * @a_cent: Image Smoothing center coefficient, u5.0.
+ * @reserved2: reserved
+ * @enable: 0: Y_EE_NR disabled, output = input; 1: Y_EE_NR enabled.
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_lpf_config {
+	__u32 a_diag:5;
+	__u32 reserved0:3;
+	__u32 a_periph:5;
+	__u32 reserved1:3;
+	__u32 a_cent:5;
+	__u32 reserved2:9;
+	__u32 enable:1;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_sense_config - Luma(Y) edge enhancement
+ *					noise reduction sensitivity gains
+ *
+ * @edge_sense_0: Sensitivity of edge in dark area. u13.0, default 8191.
+ * @reserved0: reserved
+ * @delta_edge_sense: Difference in the sensitivity of edges between
+ *		      the bright and dark areas. u13.0, default 0.
+ * @reserved1: reserved
+ * @corner_sense_0: Sensitivity of corner in dark area. u13.0, default 0.
+ * @reserved2: reserved
+ * @delta_corner_sense: Difference in the sensitivity of corners between
+ *			the bright and dark areas. u13.0, default 8191.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_sense_config {
+	__u32 edge_sense_0:13;
+	__u32 reserved0:3;
+	__u32 delta_edge_sense:13;
+	__u32 reserved1:3;
+	__u32 corner_sense_0:13;
+	__u32 reserved2:3;
+	__u32 delta_corner_sense:13;
+	__u32 reserved3:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_gain_config - Luma(Y) edge enhancement
+ *						noise reduction gain config
+ *
+ * @gain_pos_0: Gain for positive edge in dark area. u5.0, [0, 16], default 2.
+ * @reserved0: reserved
+ * @delta_gain_posi: Difference in the gain of edges between the bright and
+ *		     dark areas for positive edges. u5.0, [0, 16], default 0.
+ * @reserved1: reserved
+ * @gain_neg_0: Gain for negative edge in dark area. u5.0, [0, 16], default 8.
+ * @reserved2: reserved
+ * @delta_gain_neg: Difference in the gain of edges between the bright and
+ *		    dark areas for negative edges. u5.0, [0, 16], default 0.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_gain_config {
+	__u32 gain_pos_0:5;
+	__u32 reserved0:3;
+	__u32 delta_gain_posi:5;
+	__u32 reserved1:3;
+	__u32 gain_neg_0:5;
+	__u32 reserved2:3;
+	__u32 delta_gain_neg:5;
+	__u32 reserved3:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_clip_config - Luma(Y) edge enhancement
+ *					noise reduction clipping config
+ *
+ * @clip_pos_0: Limit of positive edge in dark area
+ *		u5, value [0, 16], default 8.
+ * @reserved0: reserved
+ * @delta_clip_posi: Difference in the limit of edges between the bright
+ *		     and dark areas for positive edges.
+ *		     u5, value [0, 16], default 8.
+ * @reserved1: reserved
+ * @clip_neg_0: Limit of negative edge in dark area
+ *		u5, value [0, 16], default 8.
+ * @reserved2: reserved
+ * @delta_clip_neg: Difference in the limit of edges between the bright
+ *		    and dark areas for negative edges.
+ *		    u5, value [0, 16], default 8.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_clip_config {
+	__u32 clip_pos_0:5;
+	__u32 reserved0:3;
+	__u32 delta_clip_posi:5;
+	__u32 reserved1:3;
+	__u32 clip_neg_0:5;
+	__u32 reserved2:3;
+	__u32 delta_clip_neg:5;
+	__u32 reserved3:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_frng_config - Luma(Y) edge enhancement
+ *						noise reduction fringe config
+ *
+ * @gain_exp: Common exponent of gains, u4, [0, 8], default 2.
+ * @reserved0: reserved
+ * @min_edge: Threshold for edge and smooth stitching, u13.
+ * @reserved1: reserved
+ * @lin_seg_param: Power of LinSeg, u4.
+ * @reserved2: reserved
+ * @t1: Parameter for enabling/disabling the edge enhancement, u1.0, [0, 1],
+ *	default 1.
+ * @t2: Parameter for enabling/disabling the smoothing, u1.0, [0, 1],
+ *	default 1.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_frng_config {
+	__u32 gain_exp:4;
+	__u32 reserved0:28;
+	__u32 min_edge:13;
+	__u32 reserved1:3;
+	__u32 lin_seg_param:4;
+	__u32 reserved2:4;
+	__u32 t1:1;
+	__u32 t2:1;
+	__u32 reserved3:6;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_diag_config - Luma(Y) edge enhancement
+ *					noise reduction diagonal config
+ *
+ * @diag_disc_g: Coefficient that prioritize diagonal edge direction on
+ *		 horizontal or vertical for final enhancement.
+ *		 u4.0, [1, 15], default 1.
+ * @reserved0: reserved
+ * @hvw_hor: Weight of horizontal/vertical edge enhancement for hv edge.
+ *		u2.2, [1, 15], default 4.
+ * @dw_hor: Weight of diagonal edge enhancement for hv edge.
+ *		u2.2, [1, 15], default 1.
+ * @hvw_diag: Weight of horizontal/vertical edge enhancement for diagonal edge.
+ *		u2.2, [1, 15], default 1.
+ * @dw_diag: Weight of diagonal edge enhancement for diagonal edge.
+ *		u2.2, [1, 15], default 4.
+ * @reserved1: reserved
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_diag_config {
+	__u32 diag_disc_g:4;
+	__u32 reserved0:4;
+	__u32 hvw_hor:4;
+	__u32 dw_hor:4;
+	__u32 hvw_diag:4;
+	__u32 dw_diag:4;
+	__u32 reserved1:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_fc_coring_config - Luma(Y) edge enhancement
+ *		noise reduction false color correction (FCC) coring config
+ *
+ * @pos_0: Gain for positive edge in dark, u13.0, [0, 16], default 0.
+ * @reserved0: reserved
+ * @pos_delta: Gain for positive edge in bright, value: pos_0 + pos_delta <=16
+ *		u13.0, default 0.
+ * @reserved1: reserved
+ * @neg_0: Gain for negative edge in dark area, u13.0, range [0, 16], default 0.
+ * @reserved2: reserved
+ * @neg_delta: Gain for negative edge in bright area. neg_0 + neg_delta <=16
+ *		u13.0, default 0.
+ * @reserved3: reserved
+ *
+ * Coring is a simple soft thresholding technique.
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_fc_coring_config {
+	__u32 pos_0:13;
+	__u32 reserved0:3;
+	__u32 pos_delta:13;
+	__u32 reserved1:3;
+	__u32 neg_0:13;
+	__u32 reserved2:3;
+	__u32 neg_delta:13;
+	__u32 reserved3:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp1_y_ee_nr_config - Edge enhancement and noise reduction
+ *
+ * @lpf: low-pass filter config. See &ipu3_uapi_yuvp1_y_ee_nr_lpf_config
+ * @sense: sensitivity config. See &ipu3_uapi_yuvp1_y_ee_nr_sense_config
+ * @gain: gain config as defined in &ipu3_uapi_yuvp1_y_ee_nr_gain_config
+ * @clip: clip config as defined in &ipu3_uapi_yuvp1_y_ee_nr_clip_config
+ * @frng: fringe config as defined in &ipu3_uapi_yuvp1_y_ee_nr_frng_config
+ * @diag: diagonal edge config. See &ipu3_uapi_yuvp1_y_ee_nr_diag_config
+ * @fc_coring: coring config for fringe control. See
+ *	       &ipu3_uapi_yuvp1_y_ee_nr_fc_coring_config
+ */
+struct ipu3_uapi_yuvp1_y_ee_nr_config {
+	struct ipu3_uapi_yuvp1_y_ee_nr_lpf_config lpf;
+	struct ipu3_uapi_yuvp1_y_ee_nr_sense_config sense;
+	struct ipu3_uapi_yuvp1_y_ee_nr_gain_config gain;
+	struct ipu3_uapi_yuvp1_y_ee_nr_clip_config clip;
+	struct ipu3_uapi_yuvp1_y_ee_nr_frng_config frng;
+	struct ipu3_uapi_yuvp1_y_ee_nr_diag_config diag;
+	struct ipu3_uapi_yuvp1_y_ee_nr_fc_coring_config fc_coring;
+} __packed;
+
+/* Total Color Correction */
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_gen_control_static_config - Total color correction
+ *				general control config
+ *
+ * @en:	0 - TCC disabled. Output = input 1 - TCC enabled.
+ * @blend_shift:	blend shift, Range[3, 4], default NA.
+ * @gain_according_to_y_only:	0: Gain is calculated according to YUV,
+ *				1: Gain is calculated according to Y only
+ * @reserved0: reserved
+ * @gamma:	Final blending coefficients. Values[-16, 16], default NA.
+ * @reserved1: reserved
+ * @delta:	Final blending coefficients. Values[-16, 16], default NA.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_yuvp2_tcc_gen_control_static_config {
+	__u32 en:1;
+	__u32 blend_shift:3;
+	__u32 gain_according_to_y_only:1;
+	__u32 reserved0:11;
+	__s32 gamma:5;
+	__u32 reserved1:3;
+	__s32 delta:5;
+	__u32 reserved2:3;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_macc_elem_static_config - Total color correction
+ *				multi-axis color control (MACC) config
+ *
+ * @a: a coefficient for 2x2 MACC conversion matrix.
+ * @reserved0: reserved
+ * @b: b coefficient  2x2 MACC conversion matrix.
+ * @reserved1: reserved
+ * @c: c coefficient for 2x2 MACC conversion matrix.
+ * @reserved2: reserved
+ * @d: d coefficient for 2x2 MACC conversion matrix.
+ * @reserved3: reserved
+ */
+struct ipu3_uapi_yuvp2_tcc_macc_elem_static_config {
+	__s32 a:12;
+	__u32 reserved0:4;
+	__s32 b:12;
+	__u32 reserved1:4;
+	__s32 c:12;
+	__u32 reserved2:4;
+	__s32 d:12;
+	__u32 reserved3:4;
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_macc_table_static_config - Total color correction
+ *				multi-axis color control (MACC) table array
+ *
+ * @entries: config for multi axis color correction, as specified by
+ *	     &ipu3_uapi_yuvp2_tcc_macc_elem_static_config
+ */
+struct ipu3_uapi_yuvp2_tcc_macc_table_static_config {
+	struct ipu3_uapi_yuvp2_tcc_macc_elem_static_config
+		entries[IPU3_UAPI_YUVP2_TCC_MACC_TABLE_ELEMENTS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_inv_y_lut_static_config - Total color correction
+ *				inverse y lookup table
+ *
+ * @entries: lookup table for inverse y estimation, and use it to estimate the
+ *	     ratio between luma and chroma. Chroma by approximate the absolute
+ *	     value of the radius on the chroma plane (R = sqrt(u^2+v^2) ) and
+ *	     luma by approximate by 1/Y.
+ */
+struct ipu3_uapi_yuvp2_tcc_inv_y_lut_static_config {
+	__u16 entries[IPU3_UAPI_YUVP2_TCC_INV_Y_LUT_ELEMENTS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_gain_pcwl_lut_static_config - Total color
+ *					correction lookup table for PCWL
+ *
+ * @entries: lookup table for gain piece wise linear transformation (PCWL)
+ */
+struct ipu3_uapi_yuvp2_tcc_gain_pcwl_lut_static_config {
+	__u16 entries[IPU3_UAPI_YUVP2_TCC_GAIN_PCWL_LUT_ELEMENTS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_r_sqr_lut_static_config - Total color correction
+ *				lookup table for r square root
+ *
+ * @entries: lookup table for r square root estimation
+ */
+struct ipu3_uapi_yuvp2_tcc_r_sqr_lut_static_config {
+	__s16 entries[IPU3_UAPI_YUVP2_TCC_R_SQR_LUT_ELEMENTS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_yuvp2_tcc_static_config- Total color correction static
+ *
+ * @gen_control: general config for Total Color Correction
+ * @macc_table: config for multi axis color correction
+ * @inv_y_lut: lookup table for inverse y estimation
+ * @gain_pcwl: lookup table for gain PCWL
+ * @r_sqr_lut: lookup table for r square root estimation.
+ */
+struct ipu3_uapi_yuvp2_tcc_static_config {
+	struct ipu3_uapi_yuvp2_tcc_gen_control_static_config gen_control;
+	struct ipu3_uapi_yuvp2_tcc_macc_table_static_config macc_table;
+	struct ipu3_uapi_yuvp2_tcc_inv_y_lut_static_config inv_y_lut;
+	struct ipu3_uapi_yuvp2_tcc_gain_pcwl_lut_static_config gain_pcwl;
+	struct ipu3_uapi_yuvp2_tcc_r_sqr_lut_static_config r_sqr_lut;
+} __packed;
+
+/* Advanced Noise Reduction related structs */
+
+/*
+ * struct ipu3_uapi_anr_alpha - Advanced noise reduction alpha
+ *
+ * Tunable parameters that are subject to modification according to the
+ * total gain used.
+ */
+struct ipu3_uapi_anr_alpha {
+	__u16 gr;
+	__u16 r;
+	__u16 b;
+	__u16 gb;
+	__u16 dc_gr;
+	__u16 dc_r;
+	__u16 dc_b;
+	__u16 dc_gb;
+} __packed;
+
+/*
+ * struct ipu3_uapi_anr_beta - Advanced noise reduction beta
+ *
+ * Tunable parameters that are subject to modification according to the
+ * total gain used.
+ */
+struct ipu3_uapi_anr_beta {
+	__u16 beta_gr;
+	__u16 beta_r;
+	__u16 beta_b;
+	__u16 beta_gb;
+} __packed;
+
+/*
+ * struct ipu3_uapi_anr_plane_color - Advanced noise reduction per plane R, Gr,
+ *				      Gb and B register settings
+ *
+ * Tunable parameters that are subject to modification according to the
+ * total gain used.
+ */
+struct ipu3_uapi_anr_plane_color {
+	__u16 reg_w_gr[16];
+	__u16 reg_w_r[16];
+	__u16 reg_w_b[16];
+	__u16 reg_w_gb[16];
+} __packed;
+
+/**
+ * struct ipu3_uapi_anr_transform_config - Advanced noise reduction transform
+ *
+ * @enable: advanced noise reduction enabled.
+ * @adaptive_treshhold_en: On IPU3, adaptive threshold is always enabled.
+ * @reserved1: reserved
+ * @reserved2: reserved
+ * @alpha: using following defaults:
+ *		13, 13, 13, 13, 0, 0, 0, 0
+ *		11, 11, 11, 11, 0, 0, 0, 0
+ *		14,  14, 14, 14, 0, 0, 0, 0
+ * @beta: use following defaults:
+ *		24, 24, 24, 24
+ *		21, 20, 20, 21
+ *		25, 25, 25, 25
+ * @color: use defaults defined in driver/media/pci/intel/ipu3-tables.c
+ * @sqrt_lut: 11 bits per element, values =
+ *					[724 768 810 849 887
+ *					923 958 991 1024 1056
+ *					1116 1145 1173 1201 1086
+ *					1228 1254 1280 1305 1330
+ *					1355 1379 1402 1425 1448]
+ * @xreset: Reset value of X for r^2 calculation Value: col_start-X_center
+ *	Constraint: Xreset + FrameWdith=4095 Xreset= -4095, default -1632.
+ * @reserved3: reserved
+ * @yreset: Reset value of Y for r^2 calculation Value: row_start-Y_center
+ *	 Constraint: Yreset + FrameHeight=4095 Yreset= -4095, default -1224.
+ * @reserved4: reserved
+ * @x_sqr_reset: Reset value of X^2 for r^2 calculation Value = (Xreset)^2
+ * @r_normfactor: Normalization factor for R. Default 14.
+ * @reserved5: reserved
+ * @y_sqr_reset: Reset value of Y^2 for r^2 calculation Value = (Yreset)^2
+ * @gain_scale: Parameter describing shading gain as a function of distance
+ *		from the image center.
+ *		A single value per frame, loaded by the driver. Default 115.
+ */
+struct ipu3_uapi_anr_transform_config {
+	__u32 enable:1;			/* 0 or 1, disabled or enabled */
+	__u32 adaptive_treshhold_en:1;	/* On IPU3, always enabled */
+
+	__u32 reserved1:30;
+	__u8 reserved2[44];
+
+	struct ipu3_uapi_anr_alpha alpha[3];
+	struct ipu3_uapi_anr_beta beta[3];
+	struct ipu3_uapi_anr_plane_color color[3];
+
+	__u16 sqrt_lut[IPU3_UAPI_ANR_LUT_SIZE];	/* 11 bits per element */
+
+	__s16 xreset:13;
+	__u16 reserved3:3;
+	__s16 yreset:13;
+	__u16 reserved4:3;
+
+	__u32 x_sqr_reset:24;
+	__u32 r_normfactor:5;
+	__u32 reserved5:3;
+
+	__u32 y_sqr_reset:24;
+	__u32 gain_scale:8;
+} __packed;
+
+/**
+ * struct ipu3_uapi_anr_stitch_pyramid - ANR stitch pyramid
+ *
+ * @entry0: pyramid LUT entry0, range [0x0, 0x3f]
+ * @entry1: pyramid LUT entry1, range [0x0, 0x3f]
+ * @entry2: pyramid LUT entry2, range [0x0, 0x3f]
+ * @reserved: reserved
+ */
+struct ipu3_uapi_anr_stitch_pyramid {
+	__u32 entry0:6;
+	__u32 entry1:6;
+	__u32 entry2:6;
+	__u32 reserved:14;
+} __packed;
+
+/**
+ * struct ipu3_uapi_anr_stitch_config - ANR stitch config
+ *
+ * @anr_stitch_en: enable stitch. Enabled with 1.
+ * @reserved: reserved
+ * @pyramid: pyramid table as defined by &ipu3_uapi_anr_stitch_pyramid
+ *		default values:
+ *		{ 1, 3, 5 }, { 7, 7, 5 }, { 3, 1, 3 },
+ *		{ 9, 15, 21 }, { 21, 15, 9 }, { 3, 5, 15 },
+ *		{ 25, 35, 35 }, { 25, 15, 5 }, { 7, 21, 35 },
+ *		{ 49, 49, 35 }, { 21, 7, 7 }, { 21, 35, 49 },
+ *		{ 49, 35, 21 }, { 7, 5, 15 }, { 25, 35, 35 },
+ *		{ 25, 15, 5 }, { 3, 9, 15 }, { 21, 21, 15 },
+ *		{ 9, 3, 1 }, { 3, 5, 7 }, { 7, 5, 3}, { 1 }
+ */
+struct ipu3_uapi_anr_stitch_config {
+	__u32 anr_stitch_en;
+	__u8 reserved[44];
+	struct ipu3_uapi_anr_stitch_pyramid pyramid[IPU3_UAPI_ANR_PYRAMID_SIZE];
+} __packed;
+
+/**
+ * struct ipu3_uapi_anr_config - ANR config
+ *
+ * @transform:	advanced noise reduction transform config as specified by
+ *		&ipu3_uapi_anr_transform_config
+ * @stitch: create 4x4 patch from 4 surrounding 8x8 patches.
+ */
+struct ipu3_uapi_anr_config {
+	struct ipu3_uapi_anr_transform_config transform __attribute__((aligned(32)));
+	struct ipu3_uapi_anr_stitch_config stitch __attribute__((aligned(32)));
+} __packed;
+
+/**
+ * struct ipu3_uapi_acc_param - Accelerator cluster parameters
+ *
+ * ACC refers to the HW cluster containing all Fixed Functions (FFs). Each FF
+ * implements a specific algorithm.
+ *
+ * @bnr:	parameters for bayer noise reduction static config. See
+ *		&ipu3_uapi_bnr_static_config
+ * @green_disparity:	disparity static config between gr and gb channel.
+ *			See &ipu3_uapi_bnr_static_config_green_disparity
+ * @dm:	de-mosaic config. See &ipu3_uapi_dm_config
+ * @ccm:	color correction matrix. See &ipu3_uapi_ccm_mat_config
+ * @gamma:	gamma correction config. See &ipu3_uapi_gamma_config
+ * @csc:	color space conversion matrix. See &ipu3_uapi_csc_mat_config
+ * @cds:	color down sample config. See &ipu3_uapi_cds_params
+ * @shd:	lens shading correction config. See &ipu3_uapi_shd_config
+ * @iefd:	Image enhancement filter and denoise config.
+ *		&ipu3_uapi_yuvp1_iefd_config
+ * @yds_c0:	y down scaler config. &ipu3_uapi_yuvp1_yds_config
+ * @chnr_c0:	chroma noise reduction config. &ipu3_uapi_yuvp1_chnr_config
+ * @y_ee_nr:	y edge enhancement and noise reduction config.
+ *		&ipu3_uapi_yuvp1_y_ee_nr_config
+ * @yds:	y down scaler config. See &ipu3_uapi_yuvp1_yds_config
+ * @chnr:	chroma noise reduction config. See &ipu3_uapi_yuvp1_chnr_config
+ * @reserved1: reserved
+ * @yds2:	y channel down scaler config. See &ipu3_uapi_yuvp1_yds_config
+ * @tcc:	total color correction config as defined in struct
+ *		&ipu3_uapi_yuvp2_tcc_static_config
+ * @reserved2: reserved
+ * @anr:	advanced noise reduction config.See &ipu3_uapi_anr_config
+ * @awb_fr:	AWB filter response config. See ipu3_uapi_awb_fr_config
+ * @ae:	auto exposure config  As specified by &ipu3_uapi_ae_config
+ * @af:	auto focus config. As specified by &ipu3_uapi_af_config
+ * @awb:	auto white balance config. As specified by &ipu3_uapi_awb_config
+ */
+struct ipu3_uapi_acc_param {
+	struct ipu3_uapi_bnr_static_config bnr;
+	struct ipu3_uapi_bnr_static_config_green_disparity
+				green_disparity __attribute__((aligned(32)));
+	struct ipu3_uapi_dm_config dm __attribute__((aligned(32)));
+	struct ipu3_uapi_ccm_mat_config ccm __attribute__((aligned(32)));
+	struct ipu3_uapi_gamma_config gamma __attribute__((aligned(32)));
+	struct ipu3_uapi_csc_mat_config csc __attribute__((aligned(32)));
+	struct ipu3_uapi_cds_params cds __attribute__((aligned(32)));
+	struct ipu3_uapi_shd_config shd __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_iefd_config iefd __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_yds_config yds_c0 __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_chnr_config chnr_c0 __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_y_ee_nr_config y_ee_nr __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_yds_config yds __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_chnr_config chnr __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp1_yds_config yds2 __attribute__((aligned(32)));
+	struct ipu3_uapi_yuvp2_tcc_static_config tcc __attribute__((aligned(32)));
+	struct ipu3_uapi_anr_config anr;
+	struct ipu3_uapi_awb_fr_config_s awb_fr;
+	struct ipu3_uapi_ae_config ae;
+	struct ipu3_uapi_af_config_s af;
+	struct ipu3_uapi_awb_config awb;
+} __packed;
+
+/**
+ * struct ipu3_uapi_isp_lin_vmem_params - Linearization parameters
+ *
+ * @lin_lutlow_gr: linearization look-up table for GR channel interpolation.
+ * @lin_lutlow_r: linearization look-up table for R channel interpolation.
+ * @lin_lutlow_b: linearization look-up table for B channel interpolation.
+ * @lin_lutlow_gb: linearization look-up table for GB channel interpolation.
+ *			lin_lutlow_gr / lin_lutlow_r / lin_lutlow_b /
+ *			lin_lutlow_gb <= LIN_MAX_VALUE - 1.
+ * @lin_lutdif_gr:	lin_lutlow_gr[i+1] - lin_lutlow_gr[i].
+ * @lin_lutdif_r:	lin_lutlow_r[i+1] - lin_lutlow_r[i].
+ * @lin_lutdif_b:	lin_lutlow_b[i+1] - lin_lutlow_b[i].
+ * @lin_lutdif_gb:	lin_lutlow_gb[i+1] - lin_lutlow_gb[i].
+ */
+struct ipu3_uapi_isp_lin_vmem_params {
+	__s16 lin_lutlow_gr[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutlow_r[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutlow_b[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutlow_gb[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutdif_gr[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutdif_r[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutdif_b[IPU3_UAPI_LIN_LUT_SIZE];
+	__s16 lin_lutdif_gb[IPU3_UAPI_LIN_LUT_SIZE];
+} __packed;
+
+/* Temporal Noise Reduction */
+
+/**
+ * struct ipu3_uapi_isp_tnr3_vmem_params - Temporal noise reduction vector
+ *					   memory parameters
+ *
+ * @slope: slope setting in interpolation curve for temporal noise reduction.
+ * @reserved1: reserved
+ * @sigma: knee point setting in interpolation curve for temporal
+ *	   noise reduction.
+ * @reserved2: reserved
+ */
+struct ipu3_uapi_isp_tnr3_vmem_params {
+	__u16 slope[IPU3_UAPI_ISP_TNR3_VMEM_LEN];
+	__u16 reserved1[IPU3_UAPI_ISP_VEC_ELEMS
+						- IPU3_UAPI_ISP_TNR3_VMEM_LEN];
+	__u16 sigma[IPU3_UAPI_ISP_TNR3_VMEM_LEN];
+	__u16 reserved2[IPU3_UAPI_ISP_VEC_ELEMS
+						- IPU3_UAPI_ISP_TNR3_VMEM_LEN];
+} __packed;
+
+/**
+ * struct ipu3_uapi_isp_tnr3_params - Temporal noise reduction v3 parameters
+ *
+ * @knee_y1: Knee point TNR3 assumes standard deviation of Y,U and
+ *	V at Y1 are TnrY1_Sigma_Y, U and V.
+ * @knee_y2: Knee point TNR3 assumes standard deviation of Y,U and
+ *		V at Y2 are TnrY2_Sigma_Y, U and V.
+ * @maxfb_y: Max feedback gain for Y
+ * @maxfb_u: Max feedback gain for U
+ * @maxfb_v: Max feedback gain for V
+ * @round_adj_y: rounding Adjust for Y
+ * @round_adj_u: rounding Adjust for U
+ * @round_adj_v: rounding Adjust for V
+ * @ref_buf_select: selection of the reference frame buffer to be used.
+ */
+struct ipu3_uapi_isp_tnr3_params {
+	__u32 knee_y1;
+	__u32 knee_y2;
+	__u32 maxfb_y;
+	__u32 maxfb_u;
+	__u32 maxfb_v;
+	__u32 round_adj_y;
+	__u32 round_adj_u;
+	__u32 round_adj_v;
+	__u32 ref_buf_select;
+} __packed;
+
+/* Extreme Noise Reduction version 3 */
+
+/**
+ * struct ipu3_uapi_isp_xnr3_vmem_params - Extreme noise reduction v3
+ *					   vector memory parameters
+ *
+ * @x: xnr3 parameters.
+ * @a: xnr3 parameters.
+ * @b: xnr3 parameters.
+ * @c: xnr3 parameters.
+ */
+struct ipu3_uapi_isp_xnr3_vmem_params {
+	__u16 x[IPU3_UAPI_ISP_VEC_ELEMS];
+	__u16 a[IPU3_UAPI_ISP_VEC_ELEMS];
+	__u16 b[IPU3_UAPI_ISP_VEC_ELEMS];
+	__u16 c[IPU3_UAPI_ISP_VEC_ELEMS];
+} __packed;
+
+/**
+ * struct ipu3_uapi_xnr3_alpha_params - Extreme noise reduction v3
+ *					alpha tuning parameters
+ *
+ * @y0: Sigma for Y range similarity in dark area.
+ * @u0: Sigma for U range similarity in dark area.
+ * @v0: Sigma for V range similarity in dark area.
+ * @ydiff: Sigma difference for Y between bright area and dark area.
+ * @udiff: Sigma difference for U between bright area and dark area.
+ * @vdiff: Sigma difference for V between bright area and dark area.
+ */
+struct ipu3_uapi_xnr3_alpha_params {
+	__u32 y0;
+	__u32 u0;
+	__u32 v0;
+	__u32 ydiff;
+	__u32 udiff;
+	__u32 vdiff;
+} __packed;
+
+/**
+ * struct ipu3_uapi_xnr3_coring_params - Extreme noise reduction v3
+ *					 coring parameters
+ *
+ * @u0: Coring Threshold of U channel in dark area.
+ * @v0: Coring Threshold of V channel in dark area.
+ * @udiff: Threshold difference of U channel between bright and dark area.
+ * @vdiff: Threshold difference of V channel between bright and dark area.
+ */
+struct ipu3_uapi_xnr3_coring_params {
+	__u32 u0;
+	__u32 v0;
+	__u32 udiff;
+	__u32 vdiff;
+} __packed;
+
+/**
+ * struct ipu3_uapi_xnr3_blending_params - Blending factor
+ *
+ * @strength: The factor for blending output with input. This is tuning
+ *	      parameterHigher values lead to more aggressive XNR operation.
+ */
+struct ipu3_uapi_xnr3_blending_params {
+	__u32 strength;
+} __packed;
+
+/**
+ * struct ipu3_uapi_isp_xnr3_params - Extreme noise reduction v3 parameters
+ *
+ * @alpha: parameters for xnr3 alpha. See &ipu3_uapi_xnr3_alpha_params
+ * @coring: parameters for xnr3 coring. See &ipu3_uapi_xnr3_coring_params
+ * @blending: parameters for xnr3 blending. See &ipu3_uapi_xnr3_blending_params
+ */
+struct ipu3_uapi_isp_xnr3_params {
+	struct ipu3_uapi_xnr3_alpha_params alpha;
+	struct ipu3_uapi_xnr3_coring_params coring;
+	struct ipu3_uapi_xnr3_blending_params blending;
+} __packed;
+
+/***** Obgrid (optical black level compensation) table entry *****/
+
+/**
+ * struct ipu3_uapi_obgrid_param - Optical black level compensation parameters
+ *
+ * @gr: Grid table values for color GR
+ * @r: Grid table values for color R
+ * @b: Grid table values for color B
+ * @gb: Grid table values for color GB
+ *
+ * Black level is different for red, green, and blue channels. So black level
+ * compensation is different per channel.
+ */
+struct ipu3_uapi_obgrid_param {
+	__u16 gr;
+	__u16 r;
+	__u16 b;
+	__u16 gb;
+} __packed;
+
+/******************* V4L2_META_FMT_IPU3_PARAMS *******************/
+
+/**
+ * struct ipu3_uapi_flags - bits to indicate which pipeline needs update
+ *
+ * @gdc: 0 = no update, 1 = update.
+ * @obgrid: 0 = no update, 1 = update.
+ * @reserved1: Not used.
+ * @acc_bnr: 0 = no update, 1 = update.
+ * @acc_green_disparity: 0 = no update, 1 = update.
+ * @acc_dm: 0 = no update, 1 = update.
+ * @acc_ccm: 0 = no update, 1 = update.
+ * @acc_gamma: 0 = no update, 1 = update.
+ * @acc_csc: 0 = no update, 1 = update.
+ * @acc_cds: 0 = no update, 1 = update.
+ * @acc_shd: 0 = no update, 1 = update.
+ * @reserved2: Not used.
+ * @acc_iefd: 0 = no update, 1 = update.
+ * @acc_yds_c0: 0 = no update, 1 = update.
+ * @acc_chnr_c0: 0 = no update, 1 = update.
+ * @acc_y_ee_nr: 0 = no update, 1 = update.
+ * @acc_yds: 0 = no update, 1 = update.
+ * @acc_chnr: 0 = no update, 1 = update.
+ * @acc_ytm: 0 = no update, 1 = update.
+ * @acc_yds2: 0 = no update, 1 = update.
+ * @acc_tcc: 0 = no update, 1 = update.
+ * @acc_dpc: 0 = no update, 1 = update.
+ * @acc_bds: 0 = no update, 1 = update.
+ * @acc_anr: 0 = no update, 1 = update.
+ * @acc_awb_fr: 0 = no update, 1 = update.
+ * @acc_ae: 0 = no update, 1 = update.
+ * @acc_af: 0 = no update, 1 = update.
+ * @acc_awb: 0 = no update, 1 = update.
+ * @__acc_osys: 0 = no update, 1 = update.
+ * @reserved3: Not used.
+ * @lin_vmem_params: 0 = no update, 1 = update.
+ * @tnr3_vmem_params: 0 = no update, 1 = update.
+ * @xnr3_vmem_params: 0 = no update, 1 = update.
+ * @tnr3_dmem_params: 0 = no update, 1 = update.
+ * @xnr3_dmem_params: 0 = no update, 1 = update.
+ * @reserved4: Not used.
+ * @obgrid_param: 0 = no update, 1 = update.
+ * @reserved5: Not used.
+ */
+struct ipu3_uapi_flags {
+	__u32 gdc:1;
+	__u32 obgrid:1;
+	__u32 reserved1:30;
+
+	__u32 acc_bnr:1;
+	__u32 acc_green_disparity:1;
+	__u32 acc_dm:1;
+	__u32 acc_ccm:1;
+	__u32 acc_gamma:1;
+	__u32 acc_csc:1;
+	__u32 acc_cds:1;
+	__u32 acc_shd:1;
+	__u32 reserved2:2;
+	__u32 acc_iefd:1;
+	__u32 acc_yds_c0:1;
+	__u32 acc_chnr_c0:1;
+	__u32 acc_y_ee_nr:1;
+	__u32 acc_yds:1;
+	__u32 acc_chnr:1;
+	__u32 acc_ytm:1;
+	__u32 acc_yds2:1;
+	__u32 acc_tcc:1;
+	__u32 acc_dpc:1;
+	__u32 acc_bds:1;
+	__u32 acc_anr:1;
+	__u32 acc_awb_fr:1;
+	__u32 acc_ae:1;
+	__u32 acc_af:1;
+	__u32 acc_awb:1;
+	__u32 reserved3:4;
+
+	__u32 lin_vmem_params:1;
+	__u32 tnr3_vmem_params:1;
+	__u32 xnr3_vmem_params:1;
+	__u32 tnr3_dmem_params:1;
+	__u32 xnr3_dmem_params:1;
+	__u32 reserved4:1;
+	__u32 obgrid_param:1;
+	__u32 reserved5:25;
+} __packed;
+
+/**
+ * struct ipu3_uapi_params - V4L2_META_FMT_IPU3_PARAMS
+ *
+ * @use:	select which parameters to apply, see &ipu3_uapi_flags
+ * @acc_param:	ACC parameters, as specified by &ipu3_uapi_acc_param
+ * @lin_vmem_params:	linearization VMEM, as specified by
+ *			&ipu3_uapi_isp_lin_vmem_params
+ * @tnr3_vmem_params:	tnr3 VMEM as specified by
+ *			&ipu3_uapi_isp_tnr3_vmem_params
+ * @xnr3_vmem_params:	xnr3 VMEM as specified by
+ *			&ipu3_uapi_isp_xnr3_vmem_params
+ * @tnr3_dmem_params:	tnr3 DMEM as specified by &ipu3_uapi_isp_tnr3_params
+ * @xnr3_dmem_params:	xnr3 DMEM as specified by &ipu3_uapi_isp_xnr3_params
+ * @obgrid_param:	obgrid parameters as specified by
+ *			&ipu3_uapi_obgrid_param
+ *
+ * The video queue "parameters" is of format V4L2_META_FMT_IPU3_PARAMS.
+ * This is a "single plane" v4l2_meta_format using V4L2_BUF_TYPE_META_OUTPUT.
+ *
+ * struct ipu3_uapi_params as defined below contains a lot of parameters and
+ * ipu3_uapi_flags selects which parameters to apply.
+ */
+struct ipu3_uapi_params {
+	/* Flags which of the settings below are to be applied */
+	struct ipu3_uapi_flags use __attribute__((aligned(32)));
+
+	/* Accelerator cluster parameters */
+	struct ipu3_uapi_acc_param acc_param;
+
+	/* ISP vector address space parameters */
+	struct ipu3_uapi_isp_lin_vmem_params lin_vmem_params;
+	struct ipu3_uapi_isp_tnr3_vmem_params tnr3_vmem_params;
+	struct ipu3_uapi_isp_xnr3_vmem_params xnr3_vmem_params;
+
+	/* ISP data memory (DMEM) parameters */
+	struct ipu3_uapi_isp_tnr3_params tnr3_dmem_params;
+	struct ipu3_uapi_isp_xnr3_params xnr3_dmem_params;
+
+	/* Optical black level compensation */
+	struct ipu3_uapi_obgrid_param obgrid_param;
+} __packed;
+
+#endif /* __IPU3_UAPI_H */
diff --git a/drivers/staging/media/ipu3/ipu3-abi.h b/drivers/staging/media/ipu3/ipu3-abi.h
new file mode 100644
index 000000000..c76935b43
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-abi.h
@@ -0,0 +1,2011 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_ABI_H
+#define __IPU3_ABI_H
+
+#include "include/uapi/intel-ipu3.h"
+
+/******************* IMGU Hardware information *******************/
+
+typedef u32 imgu_addr_t;
+
+#define IMGU_ISP_VMEM_ALIGN			128
+#define IMGU_DVS_BLOCK_W			64
+#define IMGU_DVS_BLOCK_H			32
+#define IMGU_GDC_BUF_X				(2 * IMGU_DVS_BLOCK_W)
+#define IMGU_GDC_BUF_Y				IMGU_DVS_BLOCK_H
+/* n = 0..1 */
+#define IMGU_SP_PMEM_BASE(n)			(0x20000 + (n) * 0x4000)
+#define IMGU_MAX_BQ_GRID_WIDTH			80
+#define IMGU_MAX_BQ_GRID_HEIGHT			60
+#define IMGU_OBGRID_TILE_SIZE			16
+#define IMGU_PIXELS_PER_WORD			50
+#define IMGU_BYTES_PER_WORD			64
+#define IMGU_STRIPE_FIXED_HALF_OVERLAP		2
+#define IMGU_SHD_SETS				3
+#define IMGU_BDS_MIN_CLIP_VAL			0
+#define IMGU_BDS_MAX_CLIP_VAL			2
+
+#define IMGU_ABI_AWB_MAX_CELLS_PER_SET		160
+#define IMGU_ABI_AF_MAX_CELLS_PER_SET		32
+#define IMGU_ABI_AWB_FR_MAX_CELLS_PER_SET	32
+
+#define IMGU_ABI_ACC_OP_IDLE			0
+#define IMGU_ABI_ACC_OP_END_OF_ACK		1
+#define IMGU_ABI_ACC_OP_END_OF_OPS		2
+#define IMGU_ABI_ACC_OP_NO_OPS			3
+
+#define IMGU_ABI_ACC_OPTYPE_PROCESS_LINES	0
+#define IMGU_ABI_ACC_OPTYPE_TRANSFER_DATA	1
+
+/* Register definitions */
+
+/* PM_CTRL_0_5_0_IMGHMMADR */
+#define IMGU_REG_PM_CTRL			0x0
+#define IMGU_PM_CTRL_START			BIT(0)
+#define IMGU_PM_CTRL_CFG_DONE			BIT(1)
+#define IMGU_PM_CTRL_RACE_TO_HALT		BIT(2)
+#define IMGU_PM_CTRL_NACK_ALL			BIT(3)
+#define IMGU_PM_CTRL_CSS_PWRDN			BIT(4)
+#define IMGU_PM_CTRL_RST_AT_EOF			BIT(5)
+#define IMGU_PM_CTRL_FORCE_HALT			BIT(6)
+#define IMGU_PM_CTRL_FORCE_UNHALT		BIT(7)
+#define IMGU_PM_CTRL_FORCE_PWRDN		BIT(8)
+#define IMGU_PM_CTRL_FORCE_RESET		BIT(9)
+
+/* SYSTEM_REQ_0_5_0_IMGHMMADR */
+#define IMGU_REG_SYSTEM_REQ			0x18
+#define IMGU_SYSTEM_REQ_FREQ_MASK		0x3f
+#define IMGU_SYSTEM_REQ_FREQ_DIVIDER		25
+#define IMGU_REG_INT_STATUS			0x30
+#define IMGU_REG_INT_ENABLE			0x34
+#define IMGU_REG_INT_CSS_IRQ			BIT(31)
+/* STATE_0_5_0_IMGHMMADR */
+#define IMGU_REG_STATE				0x130
+#define IMGU_STATE_HALT_STS			BIT(0)
+#define IMGU_STATE_IDLE_STS			BIT(1)
+#define IMGU_STATE_POWER_UP			BIT(2)
+#define IMGU_STATE_POWER_DOWN			BIT(3)
+#define IMGU_STATE_CSS_BUSY_MASK		0xc0
+#define IMGU_STATE_PM_FSM_MASK			0x180
+#define IMGU_STATE_PWRDNM_FSM_MASK		0x1E00000
+/* PM_STS_0_5_0_IMGHMMADR */
+#define IMGU_REG_PM_STS				0x140
+
+#define IMGU_REG_BASE				0x4000
+
+#define IMGU_REG_ISP_CTRL			(IMGU_REG_BASE + 0x00)
+#define IMGU_CTRL_RST				BIT(0)
+#define IMGU_CTRL_START				BIT(1)
+#define IMGU_CTRL_BREAK				BIT(2)
+#define IMGU_CTRL_RUN				BIT(3)
+#define IMGU_CTRL_BROKEN			BIT(4)
+#define IMGU_CTRL_IDLE				BIT(5)
+#define IMGU_CTRL_SLEEPING			BIT(6)
+#define IMGU_CTRL_STALLING			BIT(7)
+#define IMGU_CTRL_IRQ_CLEAR			BIT(8)
+#define IMGU_CTRL_IRQ_READY			BIT(10)
+#define IMGU_CTRL_IRQ_SLEEPING			BIT(11)
+#define IMGU_CTRL_ICACHE_INV			BIT(12)
+#define IMGU_CTRL_IPREFETCH_EN			BIT(13)
+#define IMGU_REG_ISP_START_ADDR			(IMGU_REG_BASE + 0x04)
+#define IMGU_REG_ISP_ICACHE_ADDR		(IMGU_REG_BASE + 0x10)
+#define IMGU_REG_ISP_PC				(IMGU_REG_BASE + 0x1c)
+
+/* SP Registers, sp = 0:SP0; 1:SP1 */
+#define IMGU_REG_SP_CTRL(sp)		(IMGU_REG_BASE + (sp) * 0x100 + 0x100)
+	/* For bits in IMGU_REG_SP_CTRL, see IMGU_CTRL_* */
+#define IMGU_REG_SP_START_ADDR(sp)	(IMGU_REG_BASE + (sp) * 0x100 + 0x104)
+#define IMGU_REG_SP_ICACHE_ADDR(sp)	(IMGU_REG_BASE + (sp) * 0x100 + 0x11c)
+#define IMGU_REG_SP_CTRL_SINK(sp)	(IMGU_REG_BASE + (sp) * 0x100 + 0x130)
+#define IMGU_REG_SP_PC(sp)		(IMGU_REG_BASE + (sp) * 0x100 + 0x134)
+
+#define IMGU_REG_TLB_INVALIDATE		(IMGU_REG_BASE + 0x300)
+#define IMGU_TLB_INVALIDATE			1
+#define IMGU_REG_L1_PHYS		(IMGU_REG_BASE + 0x304) /* 27-bit pfn */
+
+#define IMGU_REG_CIO_GATE_BURST_STATE	(IMGU_REG_BASE + 0x404)
+#define IMGU_CIO_GATE_BURST_MASK        0x80
+
+#define IMGU_REG_GP_BUSY		(IMGU_REG_BASE + 0x500)
+#define IMGU_REG_GP_STARVING		(IMGU_REG_BASE + 0x504)
+#define IMGU_REG_GP_WORKLOAD		(IMGU_REG_BASE + 0x508)
+#define IMGU_REG_GP_IRQ(n)	(IMGU_REG_BASE + (n) * 4 + 0x50c) /* n = 0..4 */
+#define IMGU_REG_GP_SP1_STRMON_STAT	(IMGU_REG_BASE + 0x520)
+#define IMGU_REG_GP_SP2_STRMON_STAT	(IMGU_REG_BASE + 0x524)
+#define IMGU_REG_GP_ISP_STRMON_STAT	(IMGU_REG_BASE + 0x528)
+#define IMGU_REG_GP_MOD_STRMON_STAT	(IMGU_REG_BASE + 0x52c)
+
+/* Port definitions for the streaming monitors. */
+/* For each definition there is signal pair : valid [bit 0]- accept [bit 1] */
+#define IMGU_GP_STRMON_STAT_SP1_PORT_SP12DMA		BIT(0)
+#define IMGU_GP_STRMON_STAT_SP1_PORT_DMA2SP1		BIT(2)
+#define IMGU_GP_STRMON_STAT_SP1_PORT_SP12SP2		BIT(4)
+#define IMGU_GP_STRMON_STAT_SP1_PORT_SP22SP1		BIT(6)
+#define IMGU_GP_STRMON_STAT_SP1_PORT_SP12ISP		BIT(8)
+#define IMGU_GP_STRMON_STAT_SP1_PORT_ISP2SP1		BIT(10)
+
+#define IMGU_GP_STRMON_STAT_SP2_PORT_SP22DMA		BIT(0)
+#define IMGU_GP_STRMON_STAT_SP2_PORT_DMA2SP2		BIT(2)
+#define IMGU_GP_STRMON_STAT_SP2_PORT_SP22SP1		BIT(4)
+#define IMGU_GP_STRMON_STAT_SP2_PORT_SP12SP2		BIT(6)
+
+#define IMGU_GP_STRMON_STAT_ISP_PORT_ISP2DMA		BIT(0)
+#define IMGU_GP_STRMON_STAT_ISP_PORT_DMA2ISP		BIT(2)
+#define IMGU_GP_STRMON_STAT_ISP_PORT_ISP2SP1		BIT(4)
+#define IMGU_GP_STRMON_STAT_ISP_PORT_SP12ISP		BIT(6)
+
+/* Between the devices and the fifo */
+#define IMGU_GP_STRMON_STAT_MOD_PORT_SP12DMA		BIT(0)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_DMA2SP1		BIT(2)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_SP22DMA		BIT(4)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_DMA2SP2		BIT(6)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_ISP2DMA		BIT(8)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_DMA2ISP		BIT(10)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_CELLS2GDC		BIT(12)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_GDC2CELLS		BIT(14)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_CELLS2DECOMP	BIT(16)
+#define IMGU_GP_STRMON_STAT_MOD_PORT_DECOMP2CELLS	BIT(18)
+/* n = 1..6 */
+#define IMGU_GP_STRMON_STAT_MOD_PORT_S2V(n)	(1 << (((n) - 1) * 2 + 20))
+
+/* n = 1..15 */
+#define IMGU_GP_STRMON_STAT_ACCS_PORT_ACC(n)		(1 << (((n) - 1) * 2))
+
+/* After FIFO and demux before SP1, n = 1..15 */
+#define IMGU_GP_STRMON_STAT_ACCS2SP1_MON_PORT_ACC(n)	(1 << (((n) - 1) * 2))
+
+/* After FIFO and demux before SP2, n = 1..15 */
+#define IMGU_GP_STRMON_STAT_ACCS2SP2_MON_PORT_ACC(n)	(1 << (((n) - 1) * 2))
+
+#define IMGU_REG_GP_HALT				(IMGU_REG_BASE + 0x5dc)
+
+					/* n = 0..2 (main ctrl, SP0, SP1) */
+#define IMGU_REG_IRQCTRL_BASE(n)	(IMGU_REG_BASE + (n) * 0x100 + 0x700)
+#define IMGU_IRQCTRL_MAIN			0
+#define IMGU_IRQCTRL_SP0			1
+#define IMGU_IRQCTRL_SP1			2
+#define IMGU_IRQCTRL_NUM			3
+#define IMGU_IRQCTRL_IRQ_SP1			BIT(0)
+#define IMGU_IRQCTRL_IRQ_SP2			BIT(1)
+#define IMGU_IRQCTRL_IRQ_ISP			BIT(2)
+#define IMGU_IRQCTRL_IRQ_SP1_STREAM_MON		BIT(3)
+#define IMGU_IRQCTRL_IRQ_SP2_STREAM_MON		BIT(4)
+#define IMGU_IRQCTRL_IRQ_ISP_STREAM_MON		BIT(5)
+#define IMGU_IRQCTRL_IRQ_MOD_STREAM_MON		BIT(6)
+#define IMGU_IRQCTRL_IRQ_MOD_ISP_STREAM_MON	BIT(7)
+#define IMGU_IRQCTRL_IRQ_ACCS_STREAM_MON	BIT(8)
+#define IMGU_IRQCTRL_IRQ_ACCS_SP1_STREAM_MON	BIT(9)
+#define IMGU_IRQCTRL_IRQ_ACCS_SP2_STREAM_MON	BIT(10)
+#define IMGU_IRQCTRL_IRQ_ISP_PMEM_ERROR		BIT(11)
+#define IMGU_IRQCTRL_IRQ_ISP_BAMEM_ERROR	BIT(12)
+#define IMGU_IRQCTRL_IRQ_ISP_VMEM_ERROR		BIT(13)
+#define IMGU_IRQCTRL_IRQ_ISP_DMEM_ERROR		BIT(14)
+#define IMGU_IRQCTRL_IRQ_SP1_ICACHE_MEM_ERROR	BIT(15)
+#define IMGU_IRQCTRL_IRQ_SP1_DMEM_ERROR		BIT(16)
+#define IMGU_IRQCTRL_IRQ_SP2_ICACHE_MEM_ERROR	BIT(17)
+#define IMGU_IRQCTRL_IRQ_SP2_DMEM_ERROR		BIT(18)
+#define IMGU_IRQCTRL_IRQ_ACCS_SCRATCH_MEM_ERROR	BIT(19)
+#define IMGU_IRQCTRL_IRQ_GP_TIMER(n)		BIT(20 + (n)) /* n=0..1 */
+#define IMGU_IRQCTRL_IRQ_DMA			BIT(22)
+#define IMGU_IRQCTRL_IRQ_SW_PIN(n)		BIT(23 + (n)) /* n=0..4 */
+#define IMGU_IRQCTRL_IRQ_ACC_SYS		BIT(28)
+#define IMGU_IRQCTRL_IRQ_OUT_FORM_IRQ_CTRL	BIT(29)
+#define IMGU_IRQCTRL_IRQ_SP1_IRQ_CTRL		BIT(30)
+#define IMGU_IRQCTRL_IRQ_SP2_IRQ_CTRL		BIT(31)
+#define IMGU_REG_IRQCTRL_EDGE(n)	(IMGU_REG_IRQCTRL_BASE(n) + 0x00)
+#define IMGU_REG_IRQCTRL_MASK(n)	(IMGU_REG_IRQCTRL_BASE(n) + 0x04)
+#define IMGU_REG_IRQCTRL_STATUS(n)	(IMGU_REG_IRQCTRL_BASE(n) + 0x08)
+#define IMGU_REG_IRQCTRL_CLEAR(n)	(IMGU_REG_IRQCTRL_BASE(n) + 0x0c)
+#define IMGU_REG_IRQCTRL_ENABLE(n)	(IMGU_REG_IRQCTRL_BASE(n) + 0x10)
+#define IMGU_REG_IRQCTRL_EDGE_NOT_PULSE(n) (IMGU_REG_IRQCTRL_BASE(n) + 0x14)
+#define IMGU_REG_IRQCTRL_STR_OUT_ENABLE(n) (IMGU_REG_IRQCTRL_BASE(n) + 0x18)
+
+#define IMGU_REG_GP_TIMER		(IMGU_REG_BASE + 0xa34)
+
+#define IMGU_REG_SP_DMEM_BASE(n)	(IMGU_REG_BASE + (n) * 0x4000 + 0x4000)
+#define IMGU_REG_ISP_DMEM_BASE		(IMGU_REG_BASE + 0xc000)
+
+#define IMGU_REG_GDC_BASE		(IMGU_REG_BASE + 0x18000)
+#define IMGU_REG_GDC_LUT_BASE		(IMGU_REG_GDC_BASE + 0x140)
+#define IMGU_GDC_LUT_MASK		((1 << 12) - 1) /* Range -1024..+1024 */
+
+#define IMGU_SCALER_PHASES			32
+#define IMGU_SCALER_COEFF_BITS			24
+#define IMGU_SCALER_PHASE_COUNTER_PREC_REF	6
+#define IMGU_SCALER_MAX_EXPONENT_SHIFT		3
+#define IMGU_SCALER_FILTER_TAPS			4
+#define IMGU_SCALER_TAPS_Y			IMGU_SCALER_FILTER_TAPS
+#define IMGU_SCALER_TAPS_UV			(IMGU_SCALER_FILTER_TAPS / 2)
+#define IMGU_SCALER_FIR_PHASES \
+		(IMGU_SCALER_PHASES << IMGU_SCALER_PHASE_COUNTER_PREC_REF)
+
+/******************* imgu_abi_acc_param *******************/
+
+#define IMGU_ABI_SHD_MAX_PROCESS_LINES		31
+#define IMGU_ABI_SHD_MAX_TRANSFERS		31
+#define IMGU_ABI_SHD_MAX_OPERATIONS \
+		(IMGU_ABI_SHD_MAX_PROCESS_LINES + IMGU_ABI_SHD_MAX_TRANSFERS)
+#define IMGU_ABI_SHD_MAX_CELLS_PER_SET		146
+/* largest grid is 73x56 */
+#define IMGU_ABI_SHD_MAX_CFG_SETS		(2 * 28)
+
+#define IMGU_ABI_DVS_STAT_MAX_OPERATIONS	100
+#define IMGU_ABI_DVS_STAT_MAX_PROCESS_LINES	52
+#define IMGU_ABI_DVS_STAT_MAX_TRANSFERS		52
+
+#define IMGU_ABI_BDS_SAMPLE_PATTERN_ARRAY_SIZE	8
+#define IMGU_ABI_BDS_PHASE_COEFFS_ARRAY_SIZE	32
+
+#define IMGU_ABI_AWB_FR_MAX_TRANSFERS		30
+#define IMGU_ABI_AWB_FR_MAX_PROCESS_LINES	30
+#define IMGU_ABI_AWB_FR_MAX_OPERATIONS \
+	(IMGU_ABI_AWB_FR_MAX_TRANSFERS + IMGU_ABI_AWB_FR_MAX_PROCESS_LINES)
+
+#define IMGU_ABI_AF_MAX_TRANSFERS		30
+#define IMGU_ABI_AF_MAX_PROCESS_LINES		30
+#define IMGU_ABI_AF_MAX_OPERATIONS \
+		(IMGU_ABI_AF_MAX_TRANSFERS + IMGU_ABI_AF_MAX_PROCESS_LINES)
+
+#define IMGU_ABI_AWB_MAX_PROCESS_LINES		68
+#define IMGU_ABI_AWB_MAX_TRANSFERS		68
+#define IMGU_ABI_AWB_MAX_OPERATIONS \
+		(IMGU_ABI_AWB_MAX_PROCESS_LINES + IMGU_ABI_AWB_MAX_TRANSFERS)
+
+#define IMGU_ABI_OSYS_PIN_VF			0
+#define IMGU_ABI_OSYS_PIN_OUT			1
+#define IMGU_ABI_OSYS_PINS			2
+
+#define IMGU_ABI_DVS_STAT_LEVELS		3
+#define IMGU_ABI_YUVP2_YTM_LUT_ENTRIES		256
+#define IMGU_ABI_GDC_FRAC_BITS			8
+#define IMGU_ABI_BINARY_MAX_OUTPUT_PORTS	2
+#define IMGU_ABI_MAX_BINARY_NAME		64
+#define IMGU_ABI_ISP_DDR_WORD_BITS		256
+#define IMGU_ABI_ISP_DDR_WORD_BYTES	(IMGU_ABI_ISP_DDR_WORD_BITS / 8)
+#define IMGU_ABI_MAX_STAGES			3
+#define IMGU_ABI_MAX_IF_CONFIGS			3
+#define IMGU_ABI_PIPE_CONFIG_ACQUIRE_ISP	BIT(31)
+#define IMGU_ABI_PORT_CONFIG_TYPE_INPUT_HOST	BIT(0)
+#define IMGU_ABI_PORT_CONFIG_TYPE_OUTPUT_HOST	BIT(4)
+#define IMGU_ABI_MAX_SP_THREADS			4
+#define IMGU_ABI_FRAMES_REF			3
+#define IMGU_ABI_FRAMES_TNR			4
+#define IMGU_ABI_BUF_SETS_TNR			1
+
+#define IMGU_ABI_EVENT_BUFFER_ENQUEUED(thread, queue)	\
+				(0 << 24 | (thread) << 16 | (queue) << 8)
+#define IMGU_ABI_EVENT_BUFFER_DEQUEUED(queue)	(1 << 24 | (queue) << 8)
+#define IMGU_ABI_EVENT_EVENT_DEQUEUED		(2 << 24)
+#define IMGU_ABI_EVENT_START_STREAM		(3 << 24)
+#define IMGU_ABI_EVENT_STOP_STREAM		(4 << 24)
+#define IMGU_ABI_EVENT_MIPI_BUFFERS_READY	(5 << 24)
+#define IMGU_ABI_EVENT_UNLOCK_RAW_BUFFER	(6 << 24)
+#define IMGU_ABI_EVENT_STAGE_ENABLE_DISABLE	(7 << 24)
+
+#define IMGU_ABI_HOST2SP_BUFQ_SIZE	3
+#define IMGU_ABI_SP2HOST_BUFQ_SIZE	(2 * IMGU_ABI_MAX_SP_THREADS)
+#define IMGU_ABI_HOST2SP_EVTQ_SIZE	(IMGU_ABI_QUEUE_NUM * \
+		IMGU_ABI_MAX_SP_THREADS * 2 + IMGU_ABI_MAX_SP_THREADS * 4)
+#define IMGU_ABI_SP2HOST_EVTQ_SIZE	(6 * IMGU_ABI_MAX_SP_THREADS)
+
+#define IMGU_ABI_EVTTYPE_EVENT_SHIFT	0
+#define IMGU_ABI_EVTTYPE_EVENT_MASK	(0xff << IMGU_ABI_EVTTYPE_EVENT_SHIFT)
+#define IMGU_ABI_EVTTYPE_PIPE_SHIFT	8
+#define IMGU_ABI_EVTTYPE_PIPE_MASK	(0xff << IMGU_ABI_EVTTYPE_PIPE_SHIFT)
+#define IMGU_ABI_EVTTYPE_PIPEID_SHIFT	16
+#define IMGU_ABI_EVTTYPE_PIPEID_MASK	(0xff << IMGU_ABI_EVTTYPE_PIPEID_SHIFT)
+#define IMGU_ABI_EVTTYPE_MODULEID_SHIFT	8
+#define IMGU_ABI_EVTTYPE_MODULEID_MASK (0xff << IMGU_ABI_EVTTYPE_MODULEID_SHIFT)
+#define IMGU_ABI_EVTTYPE_LINENO_SHIFT	16
+#define IMGU_ABI_EVTTYPE_LINENO_MASK   (0xffff << IMGU_ABI_EVTTYPE_LINENO_SHIFT)
+
+/* Output frame ready */
+#define IMGU_ABI_EVTTYPE_OUT_FRAME_DONE			0
+/* Second output frame ready */
+#define IMGU_ABI_EVTTYPE_2ND_OUT_FRAME_DONE		1
+/* Viewfinder Output frame ready */
+#define IMGU_ABI_EVTTYPE_VF_OUT_FRAME_DONE		2
+/* Second viewfinder Output frame ready */
+#define IMGU_ABI_EVTTYPE_2ND_VF_OUT_FRAME_DONE		3
+/* Indication that 3A statistics are available */
+#define IMGU_ABI_EVTTYPE_3A_STATS_DONE			4
+/* Indication that DIS statistics are available */
+#define IMGU_ABI_EVTTYPE_DIS_STATS_DONE			5
+/* Pipeline Done event, sent after last pipeline stage */
+#define IMGU_ABI_EVTTYPE_PIPELINE_DONE			6
+/* Frame tagged */
+#define IMGU_ABI_EVTTYPE_FRAME_TAGGED			7
+/* Input frame ready */
+#define IMGU_ABI_EVTTYPE_INPUT_FRAME_DONE		8
+/* Metadata ready */
+#define IMGU_ABI_EVTTYPE_METADATA_DONE			9
+/* Indication that LACE statistics are available */
+#define IMGU_ABI_EVTTYPE_LACE_STATS_DONE		10
+/* Extension stage executed */
+#define IMGU_ABI_EVTTYPE_ACC_STAGE_COMPLETE		11
+/* Timing measurement data */
+#define IMGU_ABI_EVTTYPE_TIMER				12
+/* End Of Frame event, sent when in buffered sensor mode */
+#define IMGU_ABI_EVTTYPE_PORT_EOF			13
+/* Performance warning encountered by FW */
+#define IMGU_ABI_EVTTYPE_FW_WARNING			14
+/* Assertion hit by FW */
+#define IMGU_ABI_EVTTYPE_FW_ASSERT			15
+
+#define IMGU_ABI_NUM_CONTINUOUS_FRAMES		10
+#define IMGU_ABI_SP_COMM_COMMAND		0x00
+
+/*
+ * The host2sp_cmd_ready command is the only command written by the SP
+ * It acknowledges that is previous command has been received.
+ * (this does not mean that the command has been executed)
+ * It also indicates that a new command can be send (it is a queue
+ * with depth 1).
+ */
+#define IMGU_ABI_SP_COMM_COMMAND_READY		1
+/* Command written by the Host */
+#define IMGU_ABI_SP_COMM_COMMAND_DUMMY		2	/* No action */
+#define IMGU_ABI_SP_COMM_COMMAND_START_FLASH	3	/* Start the flash */
+#define IMGU_ABI_SP_COMM_COMMAND_TERMINATE	4	/* Terminate */
+
+/* n = 0..IPU3_CSS_PIPE_ID_NUM-1 */
+#define IMGU_ABI_SP_COMM_EVENT_IRQ_MASK(n)		((n) * 4 + 0x60)
+#define IMGU_ABI_SP_COMM_EVENT_IRQ_MASK_OR_SHIFT	0
+#define IMGU_ABI_SP_COMM_EVENT_IRQ_MASK_AND_SHIFT	16
+
+#define IMGU_ABI_BL_DMACMD_TYPE_SP_PMEM		1	/* sp_pmem */
+
+/***** For parameter computation *****/
+
+#define IMGU_HIVE_OF_SYS_SCALER_TO_FA_OFFSET	0xC
+#define IMGU_HIVE_OF_SYS_OF_TO_FA_OFFSET	0x8
+#define IMGU_HIVE_OF_SYS_OF_SYSTEM_NWAYS	32
+
+#define IMGU_SCALER_ELEMS_PER_VEC		0x10
+#define IMGU_SCALER_FILTER_TAPS_Y		0x4
+#define IMGU_SCALER_OUT_BPP			0x8
+
+#define IMGU_SCALER_MS_TO_OUTFORMACC_SL_ADDR	0x400
+#define IMGU_SCALER_TO_OF_ACK_FA_ADDR \
+	(0xC00  + IMGU_HIVE_OF_SYS_SCALER_TO_FA_OFFSET)
+#define IMGU_OF_TO_ACK_FA_ADDR (0xC00 + IMGU_HIVE_OF_SYS_OF_TO_FA_OFFSET)
+#define IMGU_OUTFORMACC_MS_TO_SCALER_SL_ADDR 0
+#define IMGU_SCALER_INTR_BPP			10
+
+#define IMGU_PS_SNR_PRESERVE_BITS		3
+#define IMGU_CNTX_BPP				11
+#define IMGU_SCALER_FILTER_TAPS_UV	(IMGU_SCALER_FILTER_TAPS_Y / 2)
+
+#define IMGU_VMEM2_ELEMS_PER_VEC	(IMGU_SCALER_ELEMS_PER_VEC)
+#define IMGU_STRIDE_Y			(IMGU_SCALER_FILTER_TAPS_Y + 1)
+#define IMGU_MAX_FRAME_WIDTH		3840
+#define IMGU_VMEM3_ELEMS_PER_VEC	(IMGU_SCALER_ELEMS_PER_VEC)
+
+#define IMGU_VER_CNTX_WORDS		DIV_ROUND_UP((IMGU_SCALER_OUT_BPP + \
+	IMGU_PS_SNR_PRESERVE_BITS), IMGU_CNTX_BPP)	/* 1 */
+#define IMGU_MAX_INPUT_BLOCK_HEIGHT	64
+#define IMGU_HOR_CNTX_WORDS		DIV_ROUND_UP((IMGU_SCALER_INTR_BPP + \
+	IMGU_PS_SNR_PRESERVE_BITS), IMGU_CNTX_BPP)	/* 2 */
+#define IMGU_MAX_OUTPUT_BLOCK_WIDTH		128
+#define IMGU_CNTX_STRIDE_UV		(IMGU_SCALER_FILTER_TAPS_UV + 1)
+
+#define IMGU_OSYS_DMA_CROP_W_LIMIT		64
+#define IMGU_OSYS_DMA_CROP_H_LIMIT		4
+#define IMGU_OSYS_BLOCK_WIDTH			(2 * IPU3_UAPI_ISP_VEC_ELEMS)
+#define IMGU_OSYS_BLOCK_HEIGHT			32
+#define IMGU_OSYS_PHASES			0x20
+#define IMGU_OSYS_FILTER_TAPS			0x4
+#define IMGU_OSYS_PHASE_COUNTER_PREC_REF	6
+#define IMGU_OSYS_NUM_INPUT_BUFFERS		2
+#define IMGU_OSYS_FIR_PHASES \
+	(IMGU_OSYS_PHASES << IMGU_OSYS_PHASE_COUNTER_PREC_REF)
+#define IMGU_OSYS_TAPS_UV			(IMGU_OSYS_FILTER_TAPS / 2)
+#define IMGU_OSYS_TAPS_Y			(IMGU_OSYS_FILTER_TAPS)
+#define IMGU_OSYS_NUM_INTERM_BUFFERS		2
+
+#define IMGU_VMEM1_Y_SIZE \
+	(IMGU_OSYS_BLOCK_HEIGHT * IMGU_VMEM1_Y_STRIDE)
+#define IMGU_VMEM1_UV_SIZE			(IMGU_VMEM1_Y_SIZE / 4)
+#define IMGU_VMEM1_OUT_BUF_ADDR			(IMGU_VMEM1_INP_BUF_ADDR + \
+	(IMGU_OSYS_NUM_INPUT_BUFFERS * IMGU_VMEM1_BUF_SIZE))
+#define IMGU_OSYS_NUM_OUTPUT_BUFFERS		2
+
+/* transpose of input height */
+#define IMGU_VMEM2_VECS_PER_LINE \
+	(DIV_ROUND_UP(IMGU_OSYS_BLOCK_HEIGHT, IMGU_VMEM2_ELEMS_PER_VEC))
+/* size in words (vectors)  */
+#define IMGU_VMEM2_BUF_SIZE \
+	(IMGU_VMEM2_VECS_PER_LINE * IMGU_VMEM2_LINES_PER_BLOCK)
+#define IMGU_VMEM3_VER_Y_SIZE	\
+			((IMGU_STRIDE_Y * IMGU_MAX_FRAME_WIDTH \
+			 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_VER_CNTX_WORDS)
+#define IMGU_VMEM3_HOR_Y_SIZE \
+	((IMGU_STRIDE_Y * IMGU_MAX_INPUT_BLOCK_HEIGHT \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_HOR_CNTX_WORDS)
+#define IMGU_VMEM3_VER_Y_EXTRA \
+	((IMGU_STRIDE_Y * IMGU_MAX_OUTPUT_BLOCK_WIDTH \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_VER_CNTX_WORDS)
+#define IMGU_VMEM3_VER_U_SIZE \
+	(((IMGU_CNTX_STRIDE_UV * IMGU_MAX_FRAME_WIDTH \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_VER_CNTX_WORDS) / 2)
+#define IMGU_VMEM3_HOR_U_SIZE \
+	(((IMGU_STRIDE_Y * IMGU_MAX_INPUT_BLOCK_HEIGHT \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_HOR_CNTX_WORDS) / 2)
+#define IMGU_VMEM3_VER_U_EXTRA \
+	(((IMGU_CNTX_STRIDE_UV * IMGU_MAX_OUTPUT_BLOCK_WIDTH \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_VER_CNTX_WORDS) / 2)
+#define IMGU_VMEM3_VER_V_SIZE \
+	(((IMGU_CNTX_STRIDE_UV * IMGU_MAX_FRAME_WIDTH \
+	 / IMGU_VMEM3_ELEMS_PER_VEC) * IMGU_VER_CNTX_WORDS) / 2)
+
+#define IMGU_ISP_VEC_NELEMS		64
+#define IMGU_LUMA_TO_CHROMA_RATIO	2
+#define IMGU_INPUT_BLOCK_WIDTH			(128)
+#define IMGU_FIFO_ADDR_SCALER_TO_FMT \
+	(IMGU_SCALER_MS_TO_OUTFORMACC_SL_ADDR >> 2)
+#define IMGU_FIFO_ADDR_SCALER_TO_SP	(IMGU_SCALER_TO_OF_ACK_FA_ADDR >> 2)
+#define IMGU_VMEM1_INP_BUF_ADDR		0
+#define IMGU_VMEM1_Y_STRIDE \
+	(IMGU_OSYS_BLOCK_WIDTH / IMGU_VMEM1_ELEMS_PER_VEC)
+#define IMGU_VMEM1_BUF_SIZE	(IMGU_VMEM1_V_OFFSET + IMGU_VMEM1_UV_SIZE)
+
+#define IMGU_VMEM1_U_OFFSET		(IMGU_VMEM1_Y_SIZE)
+#define IMGU_VMEM1_V_OFFSET	(IMGU_VMEM1_U_OFFSET + IMGU_VMEM1_UV_SIZE)
+#define IMGU_VMEM1_UV_STRIDE		(IMGU_VMEM1_Y_STRIDE / 2)
+#define IMGU_VMEM1_INT_BUF_ADDR		(IMGU_VMEM1_OUT_BUF_ADDR + \
+	(IMGU_OSYS_NUM_OUTPUT_BUFFERS * IMGU_VMEM1_BUF_SIZE))
+
+#define IMGU_VMEM1_ELEMS_PER_VEC	(IMGU_HIVE_OF_SYS_OF_SYSTEM_NWAYS)
+#define IMGU_VMEM2_BUF_Y_ADDR		0
+#define IMGU_VMEM2_BUF_Y_STRIDE		(IMGU_VMEM2_VECS_PER_LINE)
+#define IMGU_VMEM2_BUF_U_ADDR \
+	(IMGU_VMEM2_BUF_Y_ADDR + IMGU_VMEM2_BUF_SIZE)
+#define IMGU_VMEM2_BUF_V_ADDR \
+	(IMGU_VMEM2_BUF_U_ADDR + IMGU_VMEM2_BUF_SIZE / 4)
+#define IMGU_VMEM2_BUF_UV_STRIDE	(IMGU_VMEM2_VECS_PER_LINE / 2)
+/* 1.5 x depth of intermediate buffer */
+#define IMGU_VMEM2_LINES_PER_BLOCK	192
+#define IMGU_VMEM3_HOR_Y_ADDR \
+	(IMGU_VMEM3_VER_Y_ADDR + IMGU_VMEM3_VER_Y_SIZE)
+#define IMGU_VMEM3_HOR_U_ADDR \
+	(IMGU_VMEM3_VER_U_ADDR + IMGU_VMEM3_VER_U_SIZE)
+#define IMGU_VMEM3_HOR_V_ADDR \
+	(IMGU_VMEM3_VER_V_ADDR + IMGU_VMEM3_VER_V_SIZE)
+#define IMGU_VMEM3_VER_Y_ADDR		0
+#define IMGU_VMEM3_VER_U_ADDR \
+	(IMGU_VMEM3_VER_Y_ADDR + IMGU_VMEM3_VER_Y_SIZE + \
+	max(IMGU_VMEM3_HOR_Y_SIZE, IMGU_VMEM3_VER_Y_EXTRA))
+#define IMGU_VMEM3_VER_V_ADDR \
+	(IMGU_VMEM3_VER_U_ADDR + IMGU_VMEM3_VER_U_SIZE + \
+	max(IMGU_VMEM3_HOR_U_SIZE, IMGU_VMEM3_VER_U_EXTRA))
+#define IMGU_FIFO_ADDR_FMT_TO_SP	(IMGU_OF_TO_ACK_FA_ADDR >> 2)
+#define IMGU_FIFO_ADDR_FMT_TO_SCALER (IMGU_OUTFORMACC_MS_TO_SCALER_SL_ADDR >> 2)
+#define IMGU_VMEM1_HST_BUF_ADDR		(IMGU_VMEM1_INT_BUF_ADDR + \
+	(IMGU_OSYS_NUM_INTERM_BUFFERS * IMGU_VMEM1_BUF_SIZE))
+#define IMGU_VMEM1_HST_BUF_STRIDE	120
+#define IMGU_VMEM1_HST_BUF_NLINES	3
+
+enum imgu_abi_frame_format {
+	IMGU_ABI_FRAME_FORMAT_NV11,	/* 12 bit YUV 411, Y, UV plane */
+	IMGU_ABI_FRAME_FORMAT_NV12,	/* 12 bit YUV 420, Y, UV plane */
+	IMGU_ABI_FRAME_FORMAT_NV12_16,	/* 16 bit YUV 420, Y, UV plane */
+	IMGU_ABI_FRAME_FORMAT_NV12_TILEY,/* 12 bit YUV 420,Intel tiled format */
+	IMGU_ABI_FRAME_FORMAT_NV16,	/* 16 bit YUV 422, Y, UV plane */
+	IMGU_ABI_FRAME_FORMAT_NV21,	/* 12 bit YUV 420, Y, VU plane */
+	IMGU_ABI_FRAME_FORMAT_NV61,	/* 16 bit YUV 422, Y, VU plane */
+	IMGU_ABI_FRAME_FORMAT_YV12,	/* 12 bit YUV 420, Y, V, U plane */
+	IMGU_ABI_FRAME_FORMAT_YV16,	/* 16 bit YUV 422, Y, V, U plane */
+	IMGU_ABI_FRAME_FORMAT_YUV420,	/* 12 bit YUV 420, Y, U, V plane */
+	IMGU_ABI_FRAME_FORMAT_YUV420_16,/* yuv420, 16 bits per subpixel */
+	IMGU_ABI_FRAME_FORMAT_YUV422,	/* 16 bit YUV 422, Y, U, V plane */
+	IMGU_ABI_FRAME_FORMAT_YUV422_16,/* yuv422, 16 bits per subpixel */
+	IMGU_ABI_FRAME_FORMAT_UYVY,	/* 16 bit YUV 422, UYVY interleaved */
+	IMGU_ABI_FRAME_FORMAT_YUYV,	/* 16 bit YUV 422, YUYV interleaved */
+	IMGU_ABI_FRAME_FORMAT_YUV444,	/* 24 bit YUV 444, Y, U, V plane */
+	IMGU_ABI_FRAME_FORMAT_YUV_LINE,	/* Internal format, 2 y lines */
+					/* followed by a uv-interleaved line */
+	IMGU_ABI_FRAME_FORMAT_RAW,	/* RAW, 1 plane */
+	IMGU_ABI_FRAME_FORMAT_RGB565,	/* 16 bit RGB, 1 plane. Each 3 sub
+					 * pixels are packed into one 16 bit
+					 * value, 5 bits for R, 6 bits for G
+					 * and 5 bits for B.
+					 */
+	IMGU_ABI_FRAME_FORMAT_PLANAR_RGB888, /* 24 bit RGB, 3 planes */
+	IMGU_ABI_FRAME_FORMAT_RGBA888,	/* 32 bit RGBA, 1 plane, A=Alpha
+					 * (alpha is unused)
+					 */
+	IMGU_ABI_FRAME_FORMAT_QPLANE6,	/* Internal, for advanced ISP */
+	IMGU_ABI_FRAME_FORMAT_BINARY_8,	/* byte stream, used for jpeg. For
+					 * frames of this type, we set the
+					 * height to 1 and the width to the
+					 * number of allocated bytes.
+					 */
+	IMGU_ABI_FRAME_FORMAT_MIPI,	/* MIPI frame, 1 plane */
+	IMGU_ABI_FRAME_FORMAT_RAW_PACKED,	 /* RAW, 1 plane, packed */
+	IMGU_ABI_FRAME_FORMAT_CSI_MIPI_YUV420_8, /* 8 bit per Y/U/V. Y odd line
+						  * UYVY interleaved even line
+						  */
+	IMGU_ABI_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8, /* Legacy YUV420.
+							 * UY odd line;
+							 * VY even line
+							 */
+	IMGU_ABI_FRAME_FORMAT_CSI_MIPI_YUV420_10,/* 10 bit per Y/U/V. Y odd
+						  * line; UYVY interleaved
+						  * even line
+						  */
+	IMGU_ABI_FRAME_FORMAT_YCGCO444_16, /* Internal format for ISP2.7,
+					    * 16 bits per plane YUV 444,
+					    * Y, U, V plane
+					    */
+	IMGU_ABI_FRAME_FORMAT_NUM
+};
+
+enum imgu_abi_bayer_order {
+	IMGU_ABI_BAYER_ORDER_GRBG,
+	IMGU_ABI_BAYER_ORDER_RGGB,
+	IMGU_ABI_BAYER_ORDER_BGGR,
+	IMGU_ABI_BAYER_ORDER_GBRG
+};
+
+enum imgu_abi_osys_format {
+	IMGU_ABI_OSYS_FORMAT_YUV420,
+	IMGU_ABI_OSYS_FORMAT_YV12,
+	IMGU_ABI_OSYS_FORMAT_NV12,
+	IMGU_ABI_OSYS_FORMAT_NV21,
+	IMGU_ABI_OSYS_FORMAT_YUV_LINE,
+	IMGU_ABI_OSYS_FORMAT_YUY2,	/* = IMGU_ABI_OSYS_FORMAT_YUYV */
+	IMGU_ABI_OSYS_FORMAT_NV16,
+	IMGU_ABI_OSYS_FORMAT_RGBA,
+	IMGU_ABI_OSYS_FORMAT_BGRA
+};
+
+enum imgu_abi_osys_tiling {
+	IMGU_ABI_OSYS_TILING_NONE,
+	IMGU_ABI_OSYS_TILING_Y,
+	IMGU_ABI_OSYS_TILING_YF,
+};
+
+enum imgu_abi_osys_procmode {
+	IMGU_ABI_OSYS_PROCMODE_BYPASS,
+	IMGU_ABI_OSYS_PROCMODE_UPSCALE,
+	IMGU_ABI_OSYS_PROCMODE_DOWNSCALE,
+};
+
+enum imgu_abi_queue_id {
+	IMGU_ABI_QUEUE_EVENT_ID = -1,
+	IMGU_ABI_QUEUE_A_ID = 0,
+	IMGU_ABI_QUEUE_B_ID,
+	IMGU_ABI_QUEUE_C_ID,
+	IMGU_ABI_QUEUE_D_ID,
+	IMGU_ABI_QUEUE_E_ID,
+	IMGU_ABI_QUEUE_F_ID,
+	IMGU_ABI_QUEUE_G_ID,
+	IMGU_ABI_QUEUE_H_ID,		/* input frame queue for skycam */
+	IMGU_ABI_QUEUE_NUM
+};
+
+enum imgu_abi_buffer_type {
+	IMGU_ABI_BUFFER_TYPE_INVALID = -1,
+	IMGU_ABI_BUFFER_TYPE_3A_STATISTICS = 0,
+	IMGU_ABI_BUFFER_TYPE_DIS_STATISTICS,
+	IMGU_ABI_BUFFER_TYPE_LACE_STATISTICS,
+	IMGU_ABI_BUFFER_TYPE_INPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_OUTPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_SEC_OUTPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_VF_OUTPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_RAW_OUTPUT_FRAME,
+	IMGU_ABI_BUFFER_TYPE_CUSTOM_INPUT,
+	IMGU_ABI_BUFFER_TYPE_CUSTOM_OUTPUT,
+	IMGU_ABI_BUFFER_TYPE_METADATA,
+	IMGU_ABI_BUFFER_TYPE_PARAMETER_SET,
+	IMGU_ABI_BUFFER_TYPE_PER_FRAME_PARAMETER_SET,
+	IMGU_ABI_NUM_DYNAMIC_BUFFER_TYPE,
+	IMGU_ABI_NUM_BUFFER_TYPE
+};
+
+enum imgu_abi_raw_type {
+	IMGU_ABI_RAW_TYPE_BAYER,
+	IMGU_ABI_RAW_TYPE_IR_ON_GR,
+	IMGU_ABI_RAW_TYPE_IR_ON_GB
+};
+
+enum imgu_abi_memories {
+	IMGU_ABI_MEM_ISP_PMEM0 = 0,
+	IMGU_ABI_MEM_ISP_DMEM0,
+	IMGU_ABI_MEM_ISP_VMEM0,
+	IMGU_ABI_MEM_ISP_VAMEM0,
+	IMGU_ABI_MEM_ISP_VAMEM1,
+	IMGU_ABI_MEM_ISP_VAMEM2,
+	IMGU_ABI_MEM_ISP_HMEM0,
+	IMGU_ABI_MEM_SP0_DMEM0,
+	IMGU_ABI_MEM_SP1_DMEM0,
+	IMGU_ABI_MEM_DDR,
+	IMGU_ABI_NUM_MEMORIES
+};
+
+enum imgu_abi_param_class {
+	IMGU_ABI_PARAM_CLASS_PARAM,	/* Late binding parameters, like 3A */
+	IMGU_ABI_PARAM_CLASS_CONFIG,	/* Pipe config time parameters */
+	IMGU_ABI_PARAM_CLASS_STATE,	/* State parameters, eg. buffer index */
+	IMGU_ABI_PARAM_CLASS_NUM
+};
+
+enum imgu_abi_bin_input_src {
+	IMGU_ABI_BINARY_INPUT_SOURCE_SENSOR,
+	IMGU_ABI_BINARY_INPUT_SOURCE_MEMORY,
+	IMGU_ABI_BINARY_INPUT_SOURCE_VARIABLE,
+};
+
+enum imgu_abi_sp_swstate {
+	IMGU_ABI_SP_SWSTATE_TERMINATED,
+	IMGU_ABI_SP_SWSTATE_INITIALIZED,
+	IMGU_ABI_SP_SWSTATE_CONNECTED,
+	IMGU_ABI_SP_SWSTATE_RUNNING,
+};
+
+enum imgu_abi_bl_swstate {
+	IMGU_ABI_BL_SWSTATE_OK = 0x100,
+	IMGU_ABI_BL_SWSTATE_BUSY,
+	IMGU_ABI_BL_SWSTATE_ERR,
+};
+
+/* The type of pipe stage */
+enum imgu_abi_stage_type {
+	IMGU_ABI_STAGE_TYPE_SP,
+	IMGU_ABI_STAGE_TYPE_ISP,
+};
+
+struct imgu_abi_acc_operation {
+	/*
+	 * zero means on init,
+	 * others mean upon receiving an ack signal from the BC acc.
+	 */
+	u8 op_indicator;
+	u8 op_type;
+} __packed;
+
+struct imgu_abi_acc_process_lines_cmd_data {
+	u16 lines;
+	u8 cfg_set;
+	u8 reserved;		/* Align to 4 bytes */
+} __packed;
+
+/* Bayer shading definitions */
+
+struct imgu_abi_shd_transfer_luts_set_data {
+	u8 set_number;
+	u8 padding[3];
+	imgu_addr_t rg_lut_ddr_addr;
+	imgu_addr_t bg_lut_ddr_addr;
+	u32 align_dummy;
+} __packed;
+
+struct imgu_abi_shd_grid_config {
+	/* reg 0 */
+	u32 grid_width:8;
+	u32 grid_height:8;
+	u32 block_width:3;
+	u32 reserved0:1;
+	u32 block_height:3;
+	u32 reserved1:1;
+	u32 grid_height_per_slice:8;
+	/* reg 1 */
+	s32 x_start:13;
+	s32 reserved2:3;
+	s32 y_start:13;
+	s32 reserved3:3;
+} __packed;
+
+struct imgu_abi_shd_general_config {
+	u32 init_set_vrt_offst_ul:8;
+	u32 shd_enable:1;
+	/* aka 'gf' */
+	u32 gain_factor:2;
+	u32 reserved:21;
+} __packed;
+
+struct imgu_abi_shd_black_level_config {
+	/* reg 0 */
+	s32 bl_r:12;
+	s32 reserved0:4;
+	s32 bl_gr:12;
+	u32 reserved1:1;
+	/* aka 'nf' */
+	u32 normalization_shift:3;
+	/* reg 1 */
+	s32 bl_gb:12;
+	s32 reserved2:4;
+	s32 bl_b:12;
+	s32 reserved3:4;
+} __packed;
+
+struct imgu_abi_shd_intra_frame_operations_data {
+	struct imgu_abi_acc_operation
+		operation_list[IMGU_ABI_SHD_MAX_OPERATIONS] __aligned(32);
+	struct imgu_abi_acc_process_lines_cmd_data
+		process_lines_data[IMGU_ABI_SHD_MAX_PROCESS_LINES] __aligned(32);
+	struct imgu_abi_shd_transfer_luts_set_data
+		transfer_data[IMGU_ABI_SHD_MAX_TRANSFERS] __aligned(32);
+} __packed;
+
+struct imgu_abi_shd_config {
+	struct ipu3_uapi_shd_config_static shd __aligned(32);
+	struct imgu_abi_shd_intra_frame_operations_data shd_ops __aligned(32);
+	struct ipu3_uapi_shd_lut shd_lut __aligned(32);
+} __packed;
+
+struct imgu_abi_stripe_input_frame_resolution {
+	u16 width;
+	u16 height;
+	u32 bayer_order;		/* enum ipu3_uapi_bayer_order */
+	u32 raw_bit_depth;
+} __packed;
+
+/* Stripe-based processing */
+
+struct imgu_abi_stripes {
+	/* offset from start of frame - measured in pixels */
+	u16 offset;
+	/* stripe width - measured in pixels */
+	u16 width;
+	/* stripe width - measured in pixels */
+	u16 height;
+} __packed;
+
+struct imgu_abi_stripe_data {
+	/*
+	 * number of stripes for current processing source
+	 * - VLIW binary parameter we currently support 1 or 2 stripes
+	 */
+	u16 num_of_stripes;
+
+	u8 padding[2];
+
+	/*
+	 * the following data is derived from resolution-related
+	 * pipe config and from num_of_stripes
+	 */
+
+	/*
+	 *'input-stripes' - before input cropping
+	 * used by input feeder
+	 */
+	struct imgu_abi_stripe_input_frame_resolution input_frame;
+
+	/*'effective-stripes' - after input cropping used dpc, bds */
+	struct imgu_abi_stripes effective_stripes[IPU3_UAPI_MAX_STRIPES];
+
+	/* 'down-scaled-stripes' - after down-scaling ONLY. used by BDS */
+	struct imgu_abi_stripes down_scaled_stripes[IPU3_UAPI_MAX_STRIPES];
+
+	/*
+	 *'bds-out-stripes' - after bayer down-scaling and padding.
+	 * used by all algos starting with norm up to the ref-frame for GDC
+	 * (currently up to the output kernel)
+	 */
+	struct imgu_abi_stripes bds_out_stripes[IPU3_UAPI_MAX_STRIPES];
+
+	/* 'bds-out-stripes (no overlap)' - used for ref kernel */
+	struct imgu_abi_stripes
+			bds_out_stripes_no_overlap[IPU3_UAPI_MAX_STRIPES];
+
+	/*
+	 * input resolution for output system (equal to bds_out - envelope)
+	 * output-system input frame width as configured by user
+	 */
+	u16 output_system_in_frame_width;
+	/* output-system input frame height as configured by user */
+	u16 output_system_in_frame_height;
+
+	/*
+	 * 'output-stripes' - accounts for stiching on the output (no overlap)
+	 * used by the output kernel
+	 */
+	struct imgu_abi_stripes output_stripes[IPU3_UAPI_MAX_STRIPES];
+
+	/*
+	 * 'block-stripes' - accounts for stiching by the output system
+	 * (1 or more blocks overlap)
+	 * used by DVS, TNR and the output system kernel
+	 */
+	struct imgu_abi_stripes block_stripes[IPU3_UAPI_MAX_STRIPES];
+
+	u16 effective_frame_width;	/* Needed for vertical cropping */
+	u16 bds_frame_width;
+	u16 out_frame_width;	/* Output frame width as configured by user */
+	u16 out_frame_height;	/* Output frame height as configured by user */
+
+	/* GDC in buffer (A.K.A delay frame,ref buffer) info */
+	u16 gdc_in_buffer_width;	/* GDC in buffer width  */
+	u16 gdc_in_buffer_height;	/* GDC in buffer height */
+	/* GDC in buffer first valid pixel x offset */
+	u16 gdc_in_buffer_offset_x;
+	/* GDC in buffer first valid pixel y offset */
+	u16 gdc_in_buffer_offset_y;
+
+	/* Display frame width as configured by user */
+	u16 display_frame_width;
+	/* Display frame height as configured by user */
+	u16 display_frame_height;
+	u16 bds_aligned_frame_width;
+	/* Number of vectors to left-crop when writing stripes (not stripe 0) */
+	u16 half_overlap_vectors;
+	/* Decimate ISP and fixed func resolutions after BDS (ir_extraction) */
+	u16 ir_ext_decimation;
+	u8 padding1[2];
+} __packed;
+
+/* Input feeder related structs */
+
+struct imgu_abi_input_feeder_data {
+	u32 row_stride;			/* row stride */
+	u32 start_row_address;		/* start row address */
+	u32 start_pixel;		/* start pixel */
+} __packed;
+
+struct imgu_abi_input_feeder_data_aligned {
+	struct imgu_abi_input_feeder_data data __aligned(32);
+} __packed;
+
+struct imgu_abi_input_feeder_data_per_stripe {
+	struct imgu_abi_input_feeder_data_aligned
+		input_feeder_data[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+struct imgu_abi_input_feeder_config {
+	struct imgu_abi_input_feeder_data data;
+	struct imgu_abi_input_feeder_data_per_stripe data_per_stripe
+		__aligned(32);
+} __packed;
+
+/* DVS related definitions */
+
+struct imgu_abi_dvs_stat_grd_config {
+	u8 grid_width;
+	u8 grid_height;
+	u8 block_width;
+	u8 block_height;
+	u16 x_start;
+	u16 y_start;
+	u16 enable;
+	u16 x_end;
+	u16 y_end;
+} __packed;
+
+struct imgu_abi_dvs_stat_cfg {
+	u8 reserved0[4];
+	struct imgu_abi_dvs_stat_grd_config
+					grd_config[IMGU_ABI_DVS_STAT_LEVELS];
+	u8 reserved1[18];
+} __packed;
+
+struct imgu_abi_dvs_stat_transfer_op_data {
+	u8 set_number;
+} __packed;
+
+struct imgu_abi_dvs_stat_intra_frame_operations_data {
+	struct imgu_abi_acc_operation
+		ops[IMGU_ABI_DVS_STAT_MAX_OPERATIONS] __aligned(32);
+	struct imgu_abi_acc_process_lines_cmd_data
+		process_lines_data[IMGU_ABI_DVS_STAT_MAX_PROCESS_LINES]
+		__aligned(32);
+	struct imgu_abi_dvs_stat_transfer_op_data
+		transfer_data[IMGU_ABI_DVS_STAT_MAX_TRANSFERS] __aligned(32);
+} __packed;
+
+struct imgu_abi_dvs_stat_config {
+	struct imgu_abi_dvs_stat_cfg cfg __aligned(32);
+	u8 reserved0[128];
+	struct imgu_abi_dvs_stat_intra_frame_operations_data operations_data;
+	u8 reserved1[64];
+} __packed;
+
+/* Y-tone Mapping */
+
+struct imgu_abi_yuvp2_y_tm_lut_static_config {
+	u16 entries[IMGU_ABI_YUVP2_YTM_LUT_ENTRIES];
+	u32 enable;
+} __packed;
+
+/* Output formatter related structs */
+
+struct imgu_abi_osys_formatter_params {
+	u32 format;
+	u32 flip;
+	u32 mirror;
+	u32 tiling;
+	u32 reduce_range;
+	u32 alpha_blending;
+	u32 release_inp_addr;
+	u32 release_inp_en;
+	u32 process_out_buf_addr;
+	u32 image_width_vecs;
+	u32 image_height_lines;
+	u32 inp_buff_y_st_addr;
+	u32 inp_buff_y_line_stride;
+	u32 inp_buff_y_buffer_stride;
+	u32 int_buff_u_st_addr;
+	u32 int_buff_v_st_addr;
+	u32 inp_buff_uv_line_stride;
+	u32 inp_buff_uv_buffer_stride;
+	u32 out_buff_level;
+	u32 out_buff_nr_y_lines;
+	u32 out_buff_u_st_offset;
+	u32 out_buff_v_st_offset;
+	u32 out_buff_y_line_stride;
+	u32 out_buff_uv_line_stride;
+	u32 hist_buff_st_addr;
+	u32 hist_buff_line_stride;
+	u32 hist_buff_nr_lines;
+} __packed;
+
+struct imgu_abi_osys_formatter {
+	struct imgu_abi_osys_formatter_params param __aligned(32);
+} __packed;
+
+struct imgu_abi_osys_scaler_params {
+	u32 inp_buf_y_st_addr;
+	u32 inp_buf_y_line_stride;
+	u32 inp_buf_y_buffer_stride;
+	u32 inp_buf_u_st_addr;
+	u32 inp_buf_v_st_addr;
+	u32 inp_buf_uv_line_stride;
+	u32 inp_buf_uv_buffer_stride;
+	u32 inp_buf_chunk_width;
+	u32 inp_buf_nr_buffers;
+	/* Output buffers */
+	u32 out_buf_y_st_addr;
+	u32 out_buf_y_line_stride;
+	u32 out_buf_y_buffer_stride;
+	u32 out_buf_u_st_addr;
+	u32 out_buf_v_st_addr;
+	u32 out_buf_uv_line_stride;
+	u32 out_buf_uv_buffer_stride;
+	u32 out_buf_nr_buffers;
+	/* Intermediate buffers */
+	u32 int_buf_y_st_addr;
+	u32 int_buf_y_line_stride;
+	u32 int_buf_u_st_addr;
+	u32 int_buf_v_st_addr;
+	u32 int_buf_uv_line_stride;
+	u32 int_buf_height;
+	u32 int_buf_chunk_width;
+	u32 int_buf_chunk_height;
+	/* Context buffers */
+	u32 ctx_buf_hor_y_st_addr;
+	u32 ctx_buf_hor_u_st_addr;
+	u32 ctx_buf_hor_v_st_addr;
+	u32 ctx_buf_ver_y_st_addr;
+	u32 ctx_buf_ver_u_st_addr;
+	u32 ctx_buf_ver_v_st_addr;
+	/* Addresses for release-input and process-output tokens */
+	u32 release_inp_buf_addr;
+	u32 release_inp_buf_en;
+	u32 release_out_buf_en;
+	u32 process_out_buf_addr;
+	/* Settings dimensions, padding, cropping */
+	u32 input_image_y_width;
+	u32 input_image_y_height;
+	u32 input_image_y_start_column;
+	u32 input_image_uv_start_column;
+	u32 input_image_y_left_pad;
+	u32 input_image_uv_left_pad;
+	u32 input_image_y_right_pad;
+	u32 input_image_uv_right_pad;
+	u32 input_image_y_top_pad;
+	u32 input_image_uv_top_pad;
+	u32 input_image_y_bottom_pad;
+	u32 input_image_uv_bottom_pad;
+	u32 processing_mode;	/* enum imgu_abi_osys_procmode */
+	u32 scaling_ratio;
+	u32 y_left_phase_init;
+	u32 uv_left_phase_init;
+	u32 y_top_phase_init;
+	u32 uv_top_phase_init;
+	u32 coeffs_exp_shift;
+	u32 out_y_left_crop;
+	u32 out_uv_left_crop;
+	u32 out_y_top_crop;
+	u32 out_uv_top_crop;
+} __packed;
+
+struct imgu_abi_osys_scaler {
+	struct imgu_abi_osys_scaler_params param __aligned(32);
+} __packed;
+
+struct imgu_abi_osys_frame_params {
+	/* Output pins */
+	u32 enable;
+	u32 format;		/* enum imgu_abi_osys_format */
+	u32 flip;
+	u32 mirror;
+	u32 tiling;		/* enum imgu_abi_osys_tiling */
+	u32 width;
+	u32 height;
+	u32 stride;
+	u32 scaled;
+} __packed;
+
+struct imgu_abi_osys_frame {
+	struct imgu_abi_osys_frame_params param __aligned(32);
+} __packed;
+
+struct imgu_abi_osys_stripe {
+	/* Input resolution */
+	u32 input_width;
+	u32 input_height;
+	/* Output Stripe */
+	u32 output_width[IMGU_ABI_OSYS_PINS];
+	u32 output_height[IMGU_ABI_OSYS_PINS];
+	u32 output_offset[IMGU_ABI_OSYS_PINS];
+	u32 buf_stride[IMGU_ABI_OSYS_PINS];
+	/* Scaler params */
+	u32 block_width;
+	u32 block_height;
+	/* Output Crop factor */
+	u32 crop_top[IMGU_ABI_OSYS_PINS];
+	u32 crop_left[IMGU_ABI_OSYS_PINS];
+} __packed;
+
+struct imgu_abi_osys_config {
+	struct imgu_abi_osys_formatter
+		formatter[IPU3_UAPI_MAX_STRIPES][IMGU_ABI_OSYS_PINS];
+	struct imgu_abi_osys_scaler scaler[IPU3_UAPI_MAX_STRIPES];
+	struct imgu_abi_osys_frame frame[IMGU_ABI_OSYS_PINS];
+	struct imgu_abi_osys_stripe stripe[IPU3_UAPI_MAX_STRIPES];
+	/* 32 packed coefficients for luma and chroma */
+	s8 scaler_coeffs_chroma[128];
+	s8 scaler_coeffs_luma[128];
+} __packed;
+
+/* BDS */
+
+struct imgu_abi_bds_hor_ctrl0 {
+	u32 sample_patrn_length:9;
+	u32 reserved0:3;
+	u32 hor_ds_en:1;
+	u32 min_clip_val:1;
+	u32 max_clip_val:2;
+	u32 out_frame_width:13;
+	u32 reserved1:3;
+} __packed;
+
+struct imgu_abi_bds_ptrn_arr {
+	u32 elems[IMGU_ABI_BDS_SAMPLE_PATTERN_ARRAY_SIZE];
+} __packed;
+
+struct imgu_abi_bds_phase_entry {
+	s8 coeff_min2;
+	s8 coeff_min1;
+	s8 coeff_0;
+	s8 nf;
+	s8 coeff_pls1;
+	s8 coeff_pls2;
+	s8 coeff_pls3;
+	u8 reserved;
+} __packed;
+
+struct imgu_abi_bds_phase_arr {
+	struct imgu_abi_bds_phase_entry
+		even[IMGU_ABI_BDS_PHASE_COEFFS_ARRAY_SIZE];
+	struct imgu_abi_bds_phase_entry
+		odd[IMGU_ABI_BDS_PHASE_COEFFS_ARRAY_SIZE];
+} __packed;
+
+struct imgu_abi_bds_hor_ctrl1 {
+	u32 hor_crop_start:13;
+	u32 reserved0:3;
+	u32 hor_crop_end:13;
+	u32 reserved1:1;
+	u32 hor_crop_en:1;
+	u32 reserved2:1;
+} __packed;
+
+struct imgu_abi_bds_hor_ctrl2 {
+	u32 input_frame_height:13;
+	u32 reserved0:19;
+} __packed;
+
+struct imgu_abi_bds_hor {
+	struct imgu_abi_bds_hor_ctrl0 hor_ctrl0;
+	struct imgu_abi_bds_ptrn_arr hor_ptrn_arr;
+	struct imgu_abi_bds_phase_arr hor_phase_arr;
+	struct imgu_abi_bds_hor_ctrl1 hor_ctrl1;
+	struct imgu_abi_bds_hor_ctrl2 hor_ctrl2;
+} __packed;
+
+struct imgu_abi_bds_ver_ctrl0 {
+	u32 sample_patrn_length:9;
+	u32 reserved0:3;
+	u32 ver_ds_en:1;
+	u32 min_clip_val:1;
+	u32 max_clip_val:2;
+	u32 reserved1:16;
+} __packed;
+
+struct imgu_abi_bds_ver_ctrl1 {
+	u32 out_frame_width:13;
+	u32 reserved0:3;
+	u32 out_frame_height:13;
+	u32 reserved1:3;
+} __packed;
+
+struct imgu_abi_bds_ver {
+	struct imgu_abi_bds_ver_ctrl0 ver_ctrl0;
+	struct imgu_abi_bds_ptrn_arr ver_ptrn_arr;
+	struct imgu_abi_bds_phase_arr ver_phase_arr;
+	struct imgu_abi_bds_ver_ctrl1 ver_ctrl1;
+} __packed;
+
+struct imgu_abi_bds_per_stripe_data {
+	struct imgu_abi_bds_hor_ctrl0 hor_ctrl0;
+	struct imgu_abi_bds_ver_ctrl1 ver_ctrl1;
+	struct imgu_abi_bds_hor_ctrl1 crop;
+} __packed;
+
+struct imgu_abi_bds_per_stripe_data_aligned {
+	struct imgu_abi_bds_per_stripe_data data __aligned(32);
+} __packed;
+
+struct imgu_abi_bds_per_stripe {
+	struct imgu_abi_bds_per_stripe_data_aligned
+		aligned_data[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+struct imgu_abi_bds_config {
+	struct imgu_abi_bds_hor hor __aligned(32);
+	struct imgu_abi_bds_ver ver __aligned(32);
+	struct imgu_abi_bds_per_stripe per_stripe __aligned(32);
+	u32 enabled;
+} __packed;
+
+/* ANR */
+
+struct imgu_abi_anr_search_config {
+	u32 enable;
+	u16 frame_width;
+	u16 frame_height;
+} __packed;
+
+struct imgu_abi_anr_stitch_config {
+	u32 anr_stitch_en;
+	u16 frame_width;
+	u16 frame_height;
+	u8 reserved[40];
+	struct ipu3_uapi_anr_stitch_pyramid pyramid[IPU3_UAPI_ANR_PYRAMID_SIZE];
+} __packed;
+
+struct imgu_abi_anr_tile2strm_config {
+	u32 enable;
+	u16 frame_width;
+	u16 frame_height;
+} __packed;
+
+struct imgu_abi_anr_config {
+	struct imgu_abi_anr_search_config search __aligned(32);
+	struct ipu3_uapi_anr_transform_config transform __aligned(32);
+	struct imgu_abi_anr_stitch_config stitch __aligned(32);
+	struct imgu_abi_anr_tile2strm_config tile2strm __aligned(32);
+} __packed;
+
+/* AF */
+
+struct imgu_abi_af_frame_size {
+	u16 width;
+	u16 height;
+} __packed;
+
+struct imgu_abi_af_config_s {
+	struct ipu3_uapi_af_filter_config filter_config __aligned(32);
+	struct imgu_abi_af_frame_size frame_size;
+	struct ipu3_uapi_grid_config grid_cfg __aligned(32);
+} __packed;
+
+struct imgu_abi_af_intra_frame_operations_data {
+	struct imgu_abi_acc_operation ops[IMGU_ABI_AF_MAX_OPERATIONS]
+		__aligned(32);
+	struct imgu_abi_acc_process_lines_cmd_data
+		process_lines_data[IMGU_ABI_AF_MAX_PROCESS_LINES] __aligned(32);
+} __packed;
+
+struct imgu_abi_af_stripe_config {
+	struct imgu_abi_af_frame_size frame_size __aligned(32);
+	struct ipu3_uapi_grid_config grid_cfg __aligned(32);
+} __packed;
+
+struct imgu_abi_af_config {
+	struct imgu_abi_af_config_s config;
+	struct imgu_abi_af_intra_frame_operations_data operations_data;
+	struct imgu_abi_af_stripe_config stripes[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+/* AE */
+
+struct imgu_abi_ae_config {
+	struct ipu3_uapi_ae_grid_config grid_cfg __aligned(32);
+	struct ipu3_uapi_ae_weight_elem weights[IPU3_UAPI_AE_WEIGHTS]
+								__aligned(32);
+	struct ipu3_uapi_ae_ccm ae_ccm __aligned(32);
+	struct {
+		struct ipu3_uapi_ae_grid_config grid __aligned(32);
+	} stripes[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+/* AWB_FR */
+
+struct imgu_abi_awb_fr_intra_frame_operations_data {
+	struct imgu_abi_acc_operation ops[IMGU_ABI_AWB_FR_MAX_OPERATIONS]
+								__aligned(32);
+	struct imgu_abi_acc_process_lines_cmd_data
+	      process_lines_data[IMGU_ABI_AWB_FR_MAX_PROCESS_LINES] __aligned(32);
+} __packed;
+
+struct imgu_abi_awb_fr_config {
+	struct ipu3_uapi_awb_fr_config_s config;
+	struct imgu_abi_awb_fr_intra_frame_operations_data operations_data;
+	struct ipu3_uapi_awb_fr_config_s stripes[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+struct imgu_abi_acc_transfer_op_data {
+	u8 set_number;
+} __packed;
+
+struct imgu_abi_awb_intra_frame_operations_data {
+	struct imgu_abi_acc_operation ops[IMGU_ABI_AWB_MAX_OPERATIONS]
+		__aligned(32);
+	struct imgu_abi_acc_process_lines_cmd_data
+		process_lines_data[IMGU_ABI_AWB_MAX_PROCESS_LINES] __aligned(32);
+	struct imgu_abi_acc_transfer_op_data
+		transfer_data[IMGU_ABI_AWB_MAX_TRANSFERS] __aligned(32);
+} __aligned(32) __packed;
+
+struct imgu_abi_awb_config {
+	struct ipu3_uapi_awb_config_s config __aligned(32);
+	struct imgu_abi_awb_intra_frame_operations_data operations_data;
+	struct ipu3_uapi_awb_config_s stripes[IPU3_UAPI_MAX_STRIPES];
+} __packed;
+
+struct imgu_abi_acc_param {
+	struct imgu_abi_stripe_data stripe;
+	u8 padding[8];
+	struct imgu_abi_input_feeder_config input_feeder;
+	struct ipu3_uapi_bnr_static_config bnr;
+	struct ipu3_uapi_bnr_static_config_green_disparity green_disparity
+		__aligned(32);
+	struct ipu3_uapi_dm_config dm __aligned(32);
+	struct ipu3_uapi_ccm_mat_config ccm __aligned(32);
+	struct ipu3_uapi_gamma_config gamma __aligned(32);
+	struct ipu3_uapi_csc_mat_config csc __aligned(32);
+	struct ipu3_uapi_cds_params cds __aligned(32);
+	struct imgu_abi_shd_config shd __aligned(32);
+	struct imgu_abi_dvs_stat_config dvs_stat;
+	u8 padding1[224];	/* reserved for lace_stat */
+	struct ipu3_uapi_yuvp1_iefd_config iefd __aligned(32);
+	struct ipu3_uapi_yuvp1_yds_config yds_c0 __aligned(32);
+	struct ipu3_uapi_yuvp1_chnr_config chnr_c0 __aligned(32);
+	struct ipu3_uapi_yuvp1_y_ee_nr_config y_ee_nr __aligned(32);
+	struct ipu3_uapi_yuvp1_yds_config yds __aligned(32);
+	struct ipu3_uapi_yuvp1_chnr_config chnr __aligned(32);
+	struct imgu_abi_yuvp2_y_tm_lut_static_config ytm __aligned(32);
+	struct ipu3_uapi_yuvp1_yds_config yds2 __aligned(32);
+	struct ipu3_uapi_yuvp2_tcc_static_config tcc __aligned(32);
+	/* reserved for defect pixel correction */
+	u8 dpc[240832] __aligned(32);
+	struct imgu_abi_bds_config bds;
+	struct imgu_abi_anr_config anr;
+	struct imgu_abi_awb_fr_config awb_fr;
+	struct imgu_abi_ae_config ae;
+	struct imgu_abi_af_config af;
+	struct imgu_abi_awb_config awb;
+	struct imgu_abi_osys_config osys;
+} __packed;
+
+/***** Morphing table entry *****/
+
+struct imgu_abi_gdc_warp_param {
+	u32 origin_x;
+	u32 origin_y;
+	u32 in_addr_offset;
+	u32 in_block_width;
+	u32 in_block_height;
+	u32 p0_x;
+	u32 p0_y;
+	u32 p1_x;
+	u32 p1_y;
+	u32 p2_x;
+	u32 p2_y;
+	u32 p3_x;
+	u32 p3_y;
+	u32 in_block_width_a;
+	u32 in_block_width_b;
+	u32 padding;		/* struct size multiple of DDR word */
+} __packed;
+
+/******************* Firmware ABI definitions *******************/
+
+/***** struct imgu_abi_sp_stage *****/
+
+struct imgu_abi_crop_pos {
+	u16 x;
+	u16 y;
+} __packed;
+
+struct imgu_abi_sp_resolution {
+	u16 width;			/* Width of valid data in pixels */
+	u16 height;			/* Height of valid data in lines */
+} __packed;
+
+/*
+ * Frame info struct. This describes the contents of an image frame buffer.
+ */
+struct imgu_abi_frame_sp_info {
+	struct imgu_abi_sp_resolution res;
+	u16 padded_width;		/* stride of line in memory
+					 * (in pixels)
+					 */
+	u8 format;			/* format of the frame data */
+	u8 raw_bit_depth;		/* number of valid bits per pixel,
+					 * only valid for RAW bayer frames
+					 */
+	u8 raw_bayer_order;		/* bayer order, only valid
+					 * for RAW bayer frames
+					 */
+	u8 raw_type;		/* To choose the proper raw frame type. for
+				 * Legacy SKC pipes/Default is set to
+				 * IMGU_ABI_RAW_TYPE_BAYER. For RGB IR sensor -
+				 * driver should set it to:
+				 * IronGr case - IMGU_ABI_RAW_TYPE_IR_ON_GR
+				 * IronGb case - IMGU_ABI_RAW_TYPE_IR_ON_GB
+				 */
+	u8 padding[2];			/* Extend to 32 bit multiple */
+} __packed;
+
+struct imgu_abi_buffer_sp {
+	union {
+		imgu_addr_t xmem_addr;
+		s32 queue_id;	/* enum imgu_abi_queue_id */
+	} buf_src;
+	s32 buf_type;	/* enum imgu_abi_buffer_type */
+} __packed;
+
+struct imgu_abi_frame_sp_plane {
+	u32 offset;		/* offset in bytes to start of frame data */
+				/* offset is wrt data in imgu_abi_sp_sp_frame */
+} __packed;
+
+struct imgu_abi_frame_sp_rgb_planes {
+	struct imgu_abi_frame_sp_plane r;
+	struct imgu_abi_frame_sp_plane g;
+	struct imgu_abi_frame_sp_plane b;
+} __packed;
+
+struct imgu_abi_frame_sp_yuv_planes {
+	struct imgu_abi_frame_sp_plane y;
+	struct imgu_abi_frame_sp_plane u;
+	struct imgu_abi_frame_sp_plane v;
+} __packed;
+
+struct imgu_abi_frame_sp_nv_planes {
+	struct imgu_abi_frame_sp_plane y;
+	struct imgu_abi_frame_sp_plane uv;
+} __packed;
+
+struct imgu_abi_frame_sp_plane6 {
+	struct imgu_abi_frame_sp_plane r;
+	struct imgu_abi_frame_sp_plane r_at_b;
+	struct imgu_abi_frame_sp_plane gr;
+	struct imgu_abi_frame_sp_plane gb;
+	struct imgu_abi_frame_sp_plane b;
+	struct imgu_abi_frame_sp_plane b_at_r;
+} __packed;
+
+struct imgu_abi_frame_sp_binary_plane {
+	u32 size;
+	struct imgu_abi_frame_sp_plane data;
+} __packed;
+
+struct imgu_abi_frame_sp {
+	struct imgu_abi_frame_sp_info info;
+	struct imgu_abi_buffer_sp buf_attr;
+	union {
+		struct imgu_abi_frame_sp_plane raw;
+		struct imgu_abi_frame_sp_plane rgb;
+		struct imgu_abi_frame_sp_rgb_planes planar_rgb;
+		struct imgu_abi_frame_sp_plane yuyv;
+		struct imgu_abi_frame_sp_yuv_planes yuv;
+		struct imgu_abi_frame_sp_nv_planes nv;
+		struct imgu_abi_frame_sp_plane6 plane6;
+		struct imgu_abi_frame_sp_binary_plane binary;
+	} planes;
+} __packed;
+
+struct imgu_abi_resolution {
+	u32 width;
+	u32 height;
+} __packed;
+
+struct imgu_abi_frames_sp {
+	struct imgu_abi_frame_sp in;
+	struct imgu_abi_frame_sp out[IMGU_ABI_BINARY_MAX_OUTPUT_PORTS];
+	struct imgu_abi_resolution effective_in_res;
+	struct imgu_abi_frame_sp out_vf;
+	struct imgu_abi_frame_sp_info internal_frame_info;
+	struct imgu_abi_buffer_sp s3a_buf;
+	struct imgu_abi_buffer_sp dvs_buf;
+	struct imgu_abi_buffer_sp lace_buf;
+} __packed;
+
+struct imgu_abi_uds_info {
+	u16 curr_dx;
+	u16 curr_dy;
+	u16 xc;
+	u16 yc;
+} __packed;
+
+/* Information for a single pipeline stage */
+struct imgu_abi_sp_stage {
+	/* Multiple boolean flags can be stored in an integer */
+	u8 num;			/* Stage number */
+	u8 isp_online;
+	u8 isp_copy_vf;
+	u8 isp_copy_output;
+	u8 sp_enable_xnr;
+	u8 isp_deci_log_factor;
+	u8 isp_vf_downscale_bits;
+	u8 deinterleaved;
+	/*
+	 * NOTE: Programming the input circuit can only be done at the
+	 * start of a session. It is illegal to program it during execution
+	 * The input circuit defines the connectivity
+	 */
+	u8 program_input_circuit;
+	u8 func;
+	u8 stage_type;		/* enum imgu_abi_stage_type */
+	u8 num_stripes;
+	u8 isp_pipe_version;
+	struct {
+		u8 vf_output;
+		u8 s3a;
+		u8 sdis;
+		u8 dvs_stats;
+		u8 lace_stats;
+	} enable;
+
+	struct imgu_abi_crop_pos sp_out_crop_pos;
+	u8 padding[2];
+	struct imgu_abi_frames_sp frames;
+	struct imgu_abi_resolution dvs_envelope;
+	struct imgu_abi_uds_info uds;
+	imgu_addr_t isp_stage_addr;
+	imgu_addr_t xmem_bin_addr;
+	imgu_addr_t xmem_map_addr;
+
+	u16 top_cropping;
+	u16 row_stripes_height;
+	u16 row_stripes_overlap_lines;
+	u8 if_config_index;	/* Which should be applied by this stage. */
+	u8 padding2;
+} __packed;
+
+/***** struct imgu_abi_isp_stage *****/
+
+struct imgu_abi_isp_param_memory_offsets {
+	u32 offsets[IMGU_ABI_PARAM_CLASS_NUM];	/* offset wrt hdr in bytes */
+} __packed;
+
+/*
+ * Blob descriptor.
+ * This structure describes an SP or ISP blob.
+ * It describes the test, data and bss sections as well as position in a
+ * firmware file.
+ * For convenience, it contains dynamic data after loading.
+ */
+struct imgu_abi_blob_info {
+	/* Static blob data */
+	u32 offset;			/* Blob offset in fw file */
+	struct imgu_abi_isp_param_memory_offsets memory_offsets;
+					/* offset wrt hdr in bytes */
+	u32 prog_name_offset;		/* offset wrt hdr in bytes */
+	u32 size;			/* Size of blob */
+	u32 padding_size;		/* total cumulative of bytes added
+					 * due to section alignment
+					 */
+	u32 icache_source;		/* Position of icache in blob */
+	u32 icache_size;		/* Size of icache section */
+	u32 icache_padding;	/* added due to icache section alignment */
+	u32 text_source;		/* Position of text in blob */
+	u32 text_size;			/* Size of text section */
+	u32 text_padding;	/* bytes added due to text section alignment */
+	u32 data_source;		/* Position of data in blob */
+	u32 data_target;		/* Start of data in SP dmem */
+	u32 data_size;			/* Size of text section */
+	u32 data_padding;	/* bytes added due to data section alignment */
+	u32 bss_target;		/* Start position of bss in SP dmem */
+	u32 bss_size;			/* Size of bss section
+					 * Dynamic data filled by loader
+					 */
+	u64 code __aligned(8);	/* Code section absolute pointer */
+					/* within fw, code = icache + text */
+	u64 data __aligned(8);	/* Data section absolute pointer */
+					/* within fw, data = data + bss */
+} __packed;
+
+struct imgu_abi_binary_pipeline_info {
+	u32 mode;
+	u32 isp_pipe_version;
+	u32 pipelining;
+	u32 c_subsampling;
+	u32 top_cropping;
+	u32 left_cropping;
+	u32 variable_resolution;
+} __packed;
+
+struct imgu_abi_binary_input_info {
+	u32 min_width;
+	u32 min_height;
+	u32 max_width;
+	u32 max_height;
+	u32 source;	/* enum imgu_abi_bin_input_src */
+} __packed;
+
+struct imgu_abi_binary_output_info {
+	u32 min_width;
+	u32 min_height;
+	u32 max_width;
+	u32 max_height;
+	u32 num_chunks;
+	u32 variable_format;
+} __packed;
+
+struct imgu_abi_binary_internal_info {
+	u32 max_width;
+	u32 max_height;
+} __packed;
+
+struct imgu_abi_binary_bds_info {
+	u32 supported_bds_factors;
+} __packed;
+
+struct imgu_abi_binary_dvs_info {
+	u32 max_envelope_width;
+	u32 max_envelope_height;
+} __packed;
+
+struct imgu_abi_binary_vf_dec_info {
+	u32 is_variable;
+	u32 max_log_downscale;
+} __packed;
+
+struct imgu_abi_binary_s3a_info {
+	u32 s3atbl_use_dmem;
+	u32 fixed_s3a_deci_log;
+} __packed;
+
+struct imgu_abi_binary_dpc_info {
+	u32 bnr_lite;			/* bnr lite enable flag */
+} __packed;
+
+struct imgu_abi_binary_iterator_info {
+	u32 num_stripes;
+	u32 row_stripes_height;
+	u32 row_stripes_overlap_lines;
+} __packed;
+
+struct imgu_abi_binary_address_info {
+	u32 isp_addresses;		/* Address in ISP dmem */
+	u32 main_entry;			/* Address of entry fct */
+	u32 in_frame;			/* Address in ISP dmem */
+	u32 out_frame;			/* Address in ISP dmem */
+	u32 in_data;			/* Address in ISP dmem */
+	u32 out_data;			/* Address in ISP dmem */
+	u32 sh_dma_cmd_ptr;		/* In ISP dmem */
+} __packed;
+
+struct imgu_abi_binary_uds_info {
+	u16 bpp;
+	u16 use_bci;
+	u16 use_str;
+	u16 woix;
+	u16 woiy;
+	u16 extra_out_vecs;
+	u16 vectors_per_line_in;
+	u16 vectors_per_line_out;
+	u16 vectors_c_per_line_in;
+	u16 vectors_c_per_line_out;
+	u16 vmem_gdc_in_block_height_y;
+	u16 vmem_gdc_in_block_height_c;
+} __packed;
+
+struct imgu_abi_binary_block_info {
+	u32 block_width;
+	u32 block_height;
+	u32 output_block_height;
+} __packed;
+
+struct imgu_abi_isp_data {
+	imgu_addr_t address;		/* ISP address */
+	u32 size;			/* Disabled if 0 */
+} __packed;
+
+struct imgu_abi_isp_param_segments {
+	struct imgu_abi_isp_data
+			params[IMGU_ABI_PARAM_CLASS_NUM][IMGU_ABI_NUM_MEMORIES];
+} __packed;
+
+struct imgu_abi_binary_info {
+	u32 id __aligned(8);		/* IMGU_ABI_BINARY_ID_* */
+	struct imgu_abi_binary_pipeline_info pipeline;
+	struct imgu_abi_binary_input_info input;
+	struct imgu_abi_binary_output_info output;
+	struct imgu_abi_binary_internal_info internal;
+	struct imgu_abi_binary_bds_info bds;
+	struct imgu_abi_binary_dvs_info dvs;
+	struct imgu_abi_binary_vf_dec_info vf_dec;
+	struct imgu_abi_binary_s3a_info s3a;
+	struct imgu_abi_binary_dpc_info dpc_bnr; /* DPC related binary info */
+	struct imgu_abi_binary_iterator_info iterator;
+	struct imgu_abi_binary_address_info addresses;
+	struct imgu_abi_binary_uds_info uds;
+	struct imgu_abi_binary_block_info block;
+	struct imgu_abi_isp_param_segments mem_initializers;
+	struct {
+		u8 input_feeder;
+		u8 output_system;
+		u8 obgrid;
+		u8 lin;
+		u8 dpc_acc;
+		u8 bds_acc;
+		u8 shd_acc;
+		u8 shd_ff;
+		u8 stats_3a_raw_buffer;
+		u8 acc_bayer_denoise;
+		u8 bnr_ff;
+		u8 awb_acc;
+		u8 awb_fr_acc;
+		u8 anr_acc;
+		u8 rgbpp_acc;
+		u8 rgbpp_ff;
+		u8 demosaic_acc;
+		u8 demosaic_ff;
+		u8 dvs_stats;
+		u8 lace_stats;
+		u8 yuvp1_b0_acc;
+		u8 yuvp1_c0_acc;
+		u8 yuvp2_acc;
+		u8 ae;
+		u8 af;
+		u8 dergb;
+		u8 rgb2yuv;
+		u8 high_quality;
+		u8 kerneltest;
+		u8 routing_shd_to_bnr;		/* connect SHD with BNR ACCs */
+		u8 routing_bnr_to_anr;		/* connect BNR with ANR ACCs */
+		u8 routing_anr_to_de;		/* connect ANR with DE ACCs */
+		u8 routing_rgb_to_yuvp1;	/* connect RGB with YUVP1 */
+		u8 routing_yuvp1_to_yuvp2;	/* connect YUVP1 with YUVP2 */
+		u8 luma_only;
+		u8 input_yuv;
+		u8 input_raw;
+		u8 reduced_pipe;
+		u8 vf_veceven;
+		u8 dis;
+		u8 dvs_envelope;
+		u8 uds;
+		u8 dvs_6axis;
+		u8 block_output;
+		u8 streaming_dma;
+		u8 ds;
+		u8 bayer_fir_6db;
+		u8 raw_binning;
+		u8 continuous;
+		u8 s3a;
+		u8 fpnr;
+		u8 sc;
+		u8 macc;
+		u8 output;
+		u8 ref_frame;
+		u8 tnr;
+		u8 xnr;
+		u8 params;
+		u8 ca_gdc;
+		u8 isp_addresses;
+		u8 in_frame;
+		u8 out_frame;
+		u8 high_speed;
+		u8 dpc;
+		u8 padding[2];
+		u8 rgbir;
+	} enable;
+	struct {
+		u8 ref_y_channel;
+		u8 ref_c_channel;
+		u8 tnr_channel;
+		u8 tnr_out_channel;
+		u8 dvs_coords_channel;
+		u8 output_channel;
+		u8 c_channel;
+		u8 vfout_channel;
+		u8 vfout_c_channel;
+		u8 vfdec_bits_per_pixel;
+		u8 claimed_by_isp;
+		u8 padding[2];
+	} dma;
+} __packed;
+
+struct imgu_abi_isp_stage {
+	struct imgu_abi_blob_info blob_info;
+	struct imgu_abi_binary_info binary_info;
+	char binary_name[IMGU_ABI_MAX_BINARY_NAME];
+	struct imgu_abi_isp_param_segments mem_initializers;
+} __packed;
+
+/***** struct imgu_abi_ddr_address_map and parameter set *****/
+
+/* xmem address map allocation */
+struct imgu_abi_ddr_address_map {
+	imgu_addr_t isp_mem_param[IMGU_ABI_MAX_STAGES][IMGU_ABI_NUM_MEMORIES];
+	imgu_addr_t obgrid_tbl[IPU3_UAPI_MAX_STRIPES];
+	imgu_addr_t acc_cluster_params_for_sp;
+	imgu_addr_t dvs_6axis_params_y;
+} __packed;
+
+struct imgu_abi_parameter_set_info {
+	/* Pointers to Parameters in ISP format IMPT */
+	struct imgu_abi_ddr_address_map mem_map;
+	/* Unique ID to track per-frame configurations */
+	u32 isp_parameters_id;
+	/* Output frame to which this config has to be applied (optional) */
+	imgu_addr_t output_frame_ptr;
+} __packed;
+
+/***** struct imgu_abi_sp_group *****/
+
+/* SP configuration information */
+struct imgu_abi_sp_config {
+	u8 no_isp_sync;		/* Signal host immediately after start */
+	u8 enable_raw_pool_locking;    /* Enable Raw Buffer Locking for HALv3 */
+	u8 lock_all;
+	u8 disable_cont_vf;
+	u8 disable_preview_on_capture;
+	u8 padding[3];
+} __packed;
+
+/* Information for a pipeline */
+struct imgu_abi_sp_pipeline {
+	u32 pipe_id;			/* the pipe ID */
+	u32 pipe_num;			/* the dynamic pipe number */
+	u32 thread_id;			/* the sp thread ID */
+	u32 pipe_config;		/* the pipe config */
+	u32 pipe_qos_config;		/* Bitmap of multiple QOS extension fw
+					 * state, 0xffffffff indicates non
+					 * QOS pipe.
+					 */
+	u32 inout_port_config;
+	u32 required_bds_factor;
+	u32 dvs_frame_delay;
+	u32 num_stages;		/* the pipe config */
+	u32 running;			/* needed for pipe termination */
+	imgu_addr_t sp_stage_addr[IMGU_ABI_MAX_STAGES];
+	imgu_addr_t scaler_pp_lut;	/* Early bound LUT */
+	u32 stage;			/* stage ptr is only used on sp */
+	s32 num_execs;			/* number of times to run if this is
+					 * an acceleration pipe.
+					 */
+	union {
+		struct {
+			u32 bytes_available;
+		} bin;
+		struct {
+			u32 height;
+			u32 width;
+			u32 padded_width;
+			u32 max_input_width;
+			u32 raw_bit_depth;
+		} raw;
+	} copy;
+
+	/* Parameters passed to Shading Correction kernel. */
+	struct {
+		/* Origin X (bqs) of internal frame on shading table */
+		u32 internal_frame_origin_x_bqs_on_sctbl;
+		/* Origin Y (bqs) of internal frame on shading table */
+		u32 internal_frame_origin_y_bqs_on_sctbl;
+	} shading;
+} __packed;
+
+struct imgu_abi_sp_debug_command {
+	/*
+	 * The DMA software-mask,
+	 *      Bit 31...24: unused.
+	 *      Bit 23...16: unused.
+	 *      Bit 15...08: reading-request enabling bits for DMA channel 7..0
+	 *      Bit 07...00: writing-request enabling bits for DMA channel 7..0
+	 *
+	 * For example, "0...0 0...0 11111011 11111101" indicates that the
+	 * writing request through DMA Channel 1 and the reading request
+	 * through DMA channel 2 are both disabled. The others are enabled.
+	 */
+	u32 dma_sw_reg;
+} __packed;
+
+/*
+ * Group all host initialized SP variables into this struct.
+ * This is initialized every stage through dma.
+ * The stage part itself is transferred through imgu_abi_sp_stage.
+ */
+struct imgu_abi_sp_group {
+	struct imgu_abi_sp_config config;
+	struct imgu_abi_sp_pipeline pipe[IMGU_ABI_MAX_SP_THREADS];
+	struct imgu_abi_sp_debug_command debug;
+} __packed;
+
+/***** parameter and state class binary configurations *****/
+
+struct imgu_abi_isp_iterator_config {
+	struct imgu_abi_frame_sp_info input_info;
+	struct imgu_abi_frame_sp_info internal_info;
+	struct imgu_abi_frame_sp_info output_info;
+	struct imgu_abi_frame_sp_info vf_info;
+	struct imgu_abi_sp_resolution dvs_envelope;
+} __packed;
+
+struct imgu_abi_dma_port_config {
+	u8 crop, elems;
+	u16 width;
+	u32 stride;
+} __packed;
+
+struct imgu_abi_isp_ref_config {
+	u32 width_a_over_b;
+	struct imgu_abi_dma_port_config port_b;
+	u32 ref_frame_addr_y[IMGU_ABI_FRAMES_REF];
+	u32 ref_frame_addr_c[IMGU_ABI_FRAMES_REF];
+	u32 dvs_frame_delay;
+} __packed;
+
+struct imgu_abi_isp_ref_dmem_state {
+	u32 ref_in_buf_idx;
+	u32 ref_out_buf_idx;
+} __packed;
+
+struct imgu_abi_isp_dvs_config {
+	u32 num_horizontal_blocks;
+	u32 num_vertical_blocks;
+} __packed;
+
+struct imgu_abi_isp_tnr3_config {
+	u32 width_a_over_b;
+	u32 frame_height;
+	struct imgu_abi_dma_port_config port_b;
+	u32 delay_frame;
+	u32 frame_addr[IMGU_ABI_FRAMES_TNR];
+} __packed;
+
+struct imgu_abi_isp_tnr3_dmem_state {
+	u32 in_bufidx;
+	u32 out_bufidx;
+	u32 total_frame_counter;
+	u32 buffer_frame_counter[IMGU_ABI_BUF_SETS_TNR];
+	u32 bypass_filter;
+} __packed;
+
+/***** Queues *****/
+
+struct imgu_abi_queue_info {
+	u8 size;		/* the maximum number of elements*/
+	u8 step;		/* number of bytes per element */
+	u8 start;		/* index of the oldest element */
+	u8 end;			/* index at which to write the new element */
+} __packed;
+
+struct imgu_abi_queues {
+	/*
+	 * Queues for the dynamic frame information,
+	 * i.e. the "in_frame" buffer, the "out_frame"
+	 * buffer and the "vf_out_frame" buffer.
+	 */
+	struct imgu_abi_queue_info host2sp_bufq_info
+			[IMGU_ABI_MAX_SP_THREADS][IMGU_ABI_QUEUE_NUM];
+	u32 host2sp_bufq[IMGU_ABI_MAX_SP_THREADS][IMGU_ABI_QUEUE_NUM]
+			[IMGU_ABI_HOST2SP_BUFQ_SIZE];
+	struct imgu_abi_queue_info sp2host_bufq_info[IMGU_ABI_QUEUE_NUM];
+	u32 sp2host_bufq[IMGU_ABI_QUEUE_NUM][IMGU_ABI_SP2HOST_BUFQ_SIZE];
+
+	/*
+	 * The queues for the events.
+	 */
+	struct imgu_abi_queue_info host2sp_evtq_info;
+	u32 host2sp_evtq[IMGU_ABI_HOST2SP_EVTQ_SIZE];
+	struct imgu_abi_queue_info sp2host_evtq_info;
+	u32 sp2host_evtq[IMGU_ABI_SP2HOST_EVTQ_SIZE];
+} __packed;
+
+/***** Buffer descriptor *****/
+
+struct imgu_abi_metadata_info {
+	struct imgu_abi_resolution resolution;	/* Resolution */
+	u32 stride;				/* Stride in bytes */
+	u32 size;				/* Total size in bytes */
+} __packed;
+
+struct imgu_abi_isp_3a_statistics {
+	union {
+		struct {
+			imgu_addr_t s3a_tbl;
+		} dmem;
+		struct {
+			imgu_addr_t s3a_tbl_hi;
+			imgu_addr_t s3a_tbl_lo;
+		} vmem;
+	} data;
+	struct {
+		imgu_addr_t rgby_tbl;
+	} data_hmem;
+	u32 exp_id;	/* exposure id, to match statistics to a frame, */
+	u32 isp_config_id;		/* Tracks per-frame configs */
+	imgu_addr_t data_ptr;		/* pointer to base of all data */
+	u32 size;			/* total size of all data */
+	u32 dmem_size;
+	u32 vmem_size;			/* both lo and hi have this size */
+	u32 hmem_size;
+} __packed;
+
+struct imgu_abi_metadata {
+	struct imgu_abi_metadata_info info;	/* Layout info */
+	imgu_addr_t address;		/* CSS virtual address */
+	u32 exp_id;			/* Exposure ID */
+} __packed;
+
+struct imgu_abi_time_meas {
+	u32 start_timer_value;		/* measured time in ticks */
+	u32 end_timer_value;		/* measured time in ticks */
+} __packed;
+
+struct imgu_abi_buffer {
+	union {
+		struct imgu_abi_isp_3a_statistics s3a;
+		u8 reserved[28];
+		imgu_addr_t skc_dvs_statistics;
+		imgu_addr_t lace_stat;
+		struct imgu_abi_metadata metadata;
+		struct {
+			imgu_addr_t frame_data;
+			u32 flashed;
+			u32 exp_id;
+			u32 isp_parameters_id;   /* Tracks per-frame configs */
+			u32 padded_width;
+		} frame;
+		imgu_addr_t ddr_ptrs;
+	} payload;
+	/*
+	 * kernel_ptr is present for host administration purposes only.
+	 * type is uint64_t in order to be 64-bit host compatible.
+	 * uint64_t does not exist on SP/ISP.
+	 * Size of the struct is checked by sp.hive.c.
+	 */
+	u64 cookie_ptr __aligned(8);
+	u64 kernel_ptr;
+	struct imgu_abi_time_meas timing_data;
+	u32 isys_eof_clock_tick;
+} __packed;
+
+struct imgu_abi_bl_dma_cmd_entry {
+	u32 src_addr;			/* virtual DDR address */
+	u32 size;			/* number of bytes to transferred */
+	u32 dst_type;
+	u32 dst_addr;			/* hmm address of xMEM or MMIO */
+} __packed;
+
+struct imgu_abi_sp_init_dmem_cfg {
+	u32 ddr_data_addr;		/* data segment address in ddr  */
+	u32 dmem_data_addr;		/* data segment address in dmem */
+	u32 dmem_bss_addr;		/* bss segment address in dmem  */
+	u32 data_size;			/* data segment size            */
+	u32 bss_size;			/* bss segment size             */
+	u32 sp_id;			/* sp id */
+} __packed;
+
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-css-fw.c b/drivers/staging/media/ipu3/ipu3-css-fw.c
new file mode 100644
index 000000000..2b659b0cc
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-fw.c
@@ -0,0 +1,264 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#include "ipu3-css.h"
+#include "ipu3-css-fw.h"
+#include "ipu3-dmamap.h"
+
+static void imgu_css_fw_show_binary(struct device *dev, struct imgu_fw_info *bi,
+				    const char *name)
+{
+	unsigned int i;
+
+	dev_dbg(dev, "found firmware binary type %i size %i name %s\n",
+		bi->type, bi->blob.size, name);
+	if (bi->type != IMGU_FW_ISP_FIRMWARE)
+		return;
+
+	dev_dbg(dev, "    id %i mode %i bds 0x%x veceven %i/%i out_pins %i\n",
+		bi->info.isp.sp.id, bi->info.isp.sp.pipeline.mode,
+		bi->info.isp.sp.bds.supported_bds_factors,
+		bi->info.isp.sp.enable.vf_veceven,
+		bi->info.isp.sp.vf_dec.is_variable,
+		bi->info.isp.num_output_pins);
+
+	dev_dbg(dev, "    input (%i,%i)-(%i,%i) formats %s%s%s\n",
+		bi->info.isp.sp.input.min_width,
+		bi->info.isp.sp.input.min_height,
+		bi->info.isp.sp.input.max_width,
+		bi->info.isp.sp.input.max_height,
+		bi->info.isp.sp.enable.input_yuv ? "yuv420 " : "",
+		bi->info.isp.sp.enable.input_feeder ||
+		bi->info.isp.sp.enable.input_raw ? "raw8 raw10 " : "",
+		bi->info.isp.sp.enable.input_raw ? "raw12" : "");
+
+	dev_dbg(dev, "    internal (%i,%i)\n",
+		bi->info.isp.sp.internal.max_width,
+		bi->info.isp.sp.internal.max_height);
+
+	dev_dbg(dev, "    output (%i,%i)-(%i,%i) formats",
+		bi->info.isp.sp.output.min_width,
+		bi->info.isp.sp.output.min_height,
+		bi->info.isp.sp.output.max_width,
+		bi->info.isp.sp.output.max_height);
+	for (i = 0; i < bi->info.isp.num_output_formats; i++)
+		dev_dbg(dev, " %i", bi->info.isp.output_formats[i]);
+	dev_dbg(dev, " vf");
+	for (i = 0; i < bi->info.isp.num_vf_formats; i++)
+		dev_dbg(dev, " %i", bi->info.isp.vf_formats[i]);
+	dev_dbg(dev, "\n");
+}
+
+unsigned int imgu_css_fw_obgrid_size(const struct imgu_fw_info *bi)
+{
+	unsigned int width = DIV_ROUND_UP(bi->info.isp.sp.internal.max_width,
+					  IMGU_OBGRID_TILE_SIZE * 2) + 1;
+	unsigned int height = DIV_ROUND_UP(bi->info.isp.sp.internal.max_height,
+					   IMGU_OBGRID_TILE_SIZE * 2) + 1;
+	unsigned int obgrid_size;
+
+	width = ALIGN(width, IPU3_UAPI_ISP_VEC_ELEMS / 4);
+	obgrid_size = PAGE_ALIGN(width * height *
+				 sizeof(struct ipu3_uapi_obgrid_param)) *
+				 bi->info.isp.sp.iterator.num_stripes;
+	return obgrid_size;
+}
+
+void *imgu_css_fw_pipeline_params(struct imgu_css *css, unsigned int pipe,
+				  enum imgu_abi_param_class cls,
+				  enum imgu_abi_memories mem,
+				  struct imgu_fw_isp_parameter *par,
+				  size_t par_size, void *binary_params)
+{
+	struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css->pipes[pipe].bindex];
+
+	if (par->offset + par->size >
+	    bi->info.isp.sp.mem_initializers.params[cls][mem].size)
+		return NULL;
+
+	if (par->size != par_size)
+		pr_warn("parameter size doesn't match defined size\n");
+
+	if (par->size < par_size)
+		return NULL;
+
+	return binary_params + par->offset;
+}
+
+void imgu_css_fw_cleanup(struct imgu_css *css)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+
+	if (css->binary) {
+		unsigned int i;
+
+		for (i = 0; i < css->fwp->file_header.binary_nr; i++)
+			imgu_dmamap_free(imgu, &css->binary[i]);
+		kfree(css->binary);
+	}
+	if (css->fw)
+		release_firmware(css->fw);
+
+	css->binary = NULL;
+	css->fw = NULL;
+}
+
+int imgu_css_fw_init(struct imgu_css *css)
+{
+	static const u32 BLOCK_MAX = 65536;
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	struct device *dev = css->dev;
+	unsigned int i, j, binary_nr;
+	int r;
+
+	r = request_firmware(&css->fw, IMGU_FW_NAME_20161208, css->dev);
+	if (r == -ENOENT)
+		r = request_firmware(&css->fw, IMGU_FW_NAME, css->dev);
+	if (r)
+		return r;
+
+	/* Check and display fw header info */
+
+	css->fwp = (struct imgu_fw_header *)css->fw->data;
+	if (css->fw->size < struct_size(css->fwp, binary_header, 1) ||
+	    css->fwp->file_header.h_size != sizeof(struct imgu_fw_bi_file_h))
+		goto bad_fw;
+	if (struct_size(css->fwp, binary_header,
+			css->fwp->file_header.binary_nr) > css->fw->size)
+		goto bad_fw;
+
+	dev_info(dev, "loaded firmware version %.64s, %u binaries, %zu bytes\n",
+		 css->fwp->file_header.version, css->fwp->file_header.binary_nr,
+		 css->fw->size);
+
+	/* Validate and display info on fw binaries */
+
+	binary_nr = css->fwp->file_header.binary_nr;
+
+	css->fw_bl = -1;
+	css->fw_sp[0] = -1;
+	css->fw_sp[1] = -1;
+
+	for (i = 0; i < binary_nr; i++) {
+		struct imgu_fw_info *bi = &css->fwp->binary_header[i];
+		const char *name = (void *)css->fwp + bi->blob.prog_name_offset;
+		size_t len;
+
+		if (bi->blob.prog_name_offset >= css->fw->size)
+			goto bad_fw;
+		len = strnlen(name, css->fw->size - bi->blob.prog_name_offset);
+		if (len + 1 > css->fw->size - bi->blob.prog_name_offset ||
+		    len + 1 >= IMGU_ABI_MAX_BINARY_NAME)
+			goto bad_fw;
+
+		if (bi->blob.size != bi->blob.text_size + bi->blob.icache_size
+		    + bi->blob.data_size + bi->blob.padding_size)
+			goto bad_fw;
+		if (bi->blob.offset + bi->blob.size > css->fw->size)
+			goto bad_fw;
+
+		if (bi->type == IMGU_FW_BOOTLOADER_FIRMWARE) {
+			css->fw_bl = i;
+			if (bi->info.bl.sw_state >= css->iomem_length ||
+			    bi->info.bl.num_dma_cmds >= css->iomem_length ||
+			    bi->info.bl.dma_cmd_list >= css->iomem_length)
+				goto bad_fw;
+		}
+		if (bi->type == IMGU_FW_SP_FIRMWARE ||
+		    bi->type == IMGU_FW_SP1_FIRMWARE) {
+			css->fw_sp[bi->type == IMGU_FW_SP_FIRMWARE ? 0 : 1] = i;
+			if (bi->info.sp.per_frame_data >= css->iomem_length ||
+			    bi->info.sp.init_dmem_data >= css->iomem_length ||
+			    bi->info.sp.host_sp_queue >= css->iomem_length ||
+			    bi->info.sp.isp_started >= css->iomem_length ||
+			    bi->info.sp.sw_state >= css->iomem_length ||
+			    bi->info.sp.sleep_mode >= css->iomem_length ||
+			    bi->info.sp.invalidate_tlb >= css->iomem_length ||
+			    bi->info.sp.host_sp_com >= css->iomem_length ||
+			    bi->info.sp.output + 12 >= css->iomem_length ||
+			    bi->info.sp.host_sp_queues_initialized >=
+			    css->iomem_length)
+				goto bad_fw;
+		}
+		if (bi->type != IMGU_FW_ISP_FIRMWARE)
+			continue;
+
+		if (bi->info.isp.sp.pipeline.mode >= IPU3_CSS_PIPE_ID_NUM)
+			goto bad_fw;
+
+		if (bi->info.isp.sp.iterator.num_stripes >
+		    IPU3_UAPI_MAX_STRIPES)
+			goto bad_fw;
+
+		if (bi->info.isp.num_vf_formats > IMGU_ABI_FRAME_FORMAT_NUM ||
+		    bi->info.isp.num_output_formats > IMGU_ABI_FRAME_FORMAT_NUM)
+			goto bad_fw;
+
+		for (j = 0; j < bi->info.isp.num_output_formats; j++)
+			if (bi->info.isp.output_formats[j] >=
+			    IMGU_ABI_FRAME_FORMAT_NUM)
+				goto bad_fw;
+		for (j = 0; j < bi->info.isp.num_vf_formats; j++)
+			if (bi->info.isp.vf_formats[j] >=
+			    IMGU_ABI_FRAME_FORMAT_NUM)
+				goto bad_fw;
+
+		if (bi->info.isp.sp.block.block_width <= 0 ||
+		    bi->info.isp.sp.block.block_width > BLOCK_MAX ||
+		    bi->info.isp.sp.block.output_block_height <= 0 ||
+		    bi->info.isp.sp.block.output_block_height > BLOCK_MAX)
+			goto bad_fw;
+
+		if (bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_PARAM]
+		    + sizeof(struct imgu_fw_param_memory_offsets) >
+		    css->fw->size ||
+		    bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_CONFIG]
+		    + sizeof(struct imgu_fw_config_memory_offsets) >
+		    css->fw->size ||
+		    bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_STATE]
+		    + sizeof(struct imgu_fw_state_memory_offsets) >
+		    css->fw->size)
+			goto bad_fw;
+
+		imgu_css_fw_show_binary(dev, bi, name);
+	}
+
+	if (css->fw_bl == -1 || css->fw_sp[0] == -1 || css->fw_sp[1] == -1)
+		goto bad_fw;
+
+	/* Allocate and map fw binaries into IMGU */
+
+	css->binary = kcalloc(binary_nr, sizeof(*css->binary), GFP_KERNEL);
+	if (!css->binary) {
+		r = -ENOMEM;
+		goto error_out;
+	}
+
+	for (i = 0; i < css->fwp->file_header.binary_nr; i++) {
+		struct imgu_fw_info *bi = &css->fwp->binary_header[i];
+		void *blob = (void *)css->fwp + bi->blob.offset;
+		size_t size = bi->blob.size;
+
+		if (!imgu_dmamap_alloc(imgu, &css->binary[i], size)) {
+			r = -ENOMEM;
+			goto error_out;
+		}
+		memcpy(css->binary[i].vaddr, blob, size);
+	}
+
+	return 0;
+
+bad_fw:
+	dev_err(dev, "invalid firmware binary, size %u\n", (int)css->fw->size);
+	r = -ENODEV;
+
+error_out:
+	imgu_css_fw_cleanup(css);
+	return r;
+}
diff --git a/drivers/staging/media/ipu3/ipu3-css-fw.h b/drivers/staging/media/ipu3/ipu3-css-fw.h
new file mode 100644
index 000000000..f9403da75
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-fw.h
@@ -0,0 +1,191 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_CSS_FW_H
+#define __IPU3_CSS_FW_H
+
+/******************* Firmware file definitions *******************/
+
+#define IMGU_FW_NAME		"intel/ipu3-fw.bin"
+#define IMGU_FW_NAME_20161208	\
+	"intel/irci_irci_ecr-master_20161208_0213_20170112_1500.bin"
+
+typedef u32 imgu_fw_ptr;
+
+enum imgu_fw_type {
+	IMGU_FW_SP_FIRMWARE,	/* Firmware for the SP */
+	IMGU_FW_SP1_FIRMWARE,	/* Firmware for the SP1 */
+	IMGU_FW_ISP_FIRMWARE,	/* Firmware for the ISP */
+	IMGU_FW_BOOTLOADER_FIRMWARE,	/* Firmware for the BootLoader */
+	IMGU_FW_ACC_FIRMWARE	/* Firmware for accelerations */
+};
+
+enum imgu_fw_acc_type {
+	IMGU_FW_ACC_NONE,	/* Normal binary */
+	IMGU_FW_ACC_OUTPUT,	/* Accelerator stage on output frame */
+	IMGU_FW_ACC_VIEWFINDER,	/* Accelerator stage on viewfinder frame */
+	IMGU_FW_ACC_STANDALONE,	/* Stand-alone acceleration */
+};
+
+struct imgu_fw_isp_parameter {
+	u32 offset;		/* Offset in isp_<mem> config, params, etc. */
+	u32 size;		/* Disabled if 0 */
+};
+
+struct imgu_fw_param_memory_offsets {
+	struct {
+		struct imgu_fw_isp_parameter lin;	/* lin_vmem_params */
+		struct imgu_fw_isp_parameter tnr3;	/* tnr3_vmem_params */
+		struct imgu_fw_isp_parameter xnr3;	/* xnr3_vmem_params */
+	} vmem;
+	struct {
+		struct imgu_fw_isp_parameter tnr;
+		struct imgu_fw_isp_parameter tnr3;	/* tnr3_params */
+		struct imgu_fw_isp_parameter xnr3;	/* xnr3_params */
+		struct imgu_fw_isp_parameter plane_io_config;	/* 192 bytes */
+		struct imgu_fw_isp_parameter rgbir;	/* rgbir_params */
+	} dmem;
+};
+
+struct imgu_fw_config_memory_offsets {
+	struct {
+		struct imgu_fw_isp_parameter iterator;
+		struct imgu_fw_isp_parameter dvs;
+		struct imgu_fw_isp_parameter output;
+		struct imgu_fw_isp_parameter raw;
+		struct imgu_fw_isp_parameter input_yuv;
+		struct imgu_fw_isp_parameter tnr;
+		struct imgu_fw_isp_parameter tnr3;
+		struct imgu_fw_isp_parameter ref;
+	} dmem;
+};
+
+struct imgu_fw_state_memory_offsets {
+	struct {
+		struct imgu_fw_isp_parameter tnr;
+		struct imgu_fw_isp_parameter tnr3;
+		struct imgu_fw_isp_parameter ref;
+	} dmem;
+};
+
+union imgu_fw_all_memory_offsets {
+	struct {
+		u64 imgu_fw_mem_offsets[3]; /* params, config, state */
+	} offsets;
+	struct {
+		u64 ptr;
+	} array[IMGU_ABI_PARAM_CLASS_NUM];
+};
+
+struct imgu_fw_binary_xinfo {
+	/* Part that is of interest to the SP. */
+	struct imgu_abi_binary_info sp;
+
+	/* Rest of the binary info, only interesting to the host. */
+	u32 type;	/* enum imgu_fw_acc_type */
+
+	u32 num_output_formats __aligned(8);
+	u32 output_formats[IMGU_ABI_FRAME_FORMAT_NUM];	/* enum frame_format */
+
+	/* number of supported vf formats */
+	u32 num_vf_formats __aligned(8);
+	/* types of supported vf formats */
+	u32 vf_formats[IMGU_ABI_FRAME_FORMAT_NUM];	/* enum frame_format */
+	u8 num_output_pins;
+	imgu_fw_ptr xmem_addr;
+
+	u64 imgu_fw_blob_descr_ptr __aligned(8);
+	u32 blob_index __aligned(8);
+	union imgu_fw_all_memory_offsets mem_offsets __aligned(8);
+	struct imgu_fw_binary_xinfo *next __aligned(8);
+};
+
+struct imgu_fw_sp_info {
+	u32 init_dmem_data;	/* data sect config, stored to dmem */
+	u32 per_frame_data;	/* Per frame data, stored to dmem */
+	u32 group;		/* Per pipeline data, loaded by dma */
+	u32 output;		/* SP output data, loaded by dmem */
+	u32 host_sp_queue;	/* Host <-> SP queues */
+	u32 host_sp_com;	/* Host <-> SP commands */
+	u32 isp_started;	/* P'ed from sensor thread, csim only */
+	u32 sw_state;		/* Polled from css, enum imgu_abi_sp_swstate */
+	u32 host_sp_queues_initialized;	/* Polled from the SP */
+	u32 sleep_mode;		/* different mode to halt SP */
+	u32 invalidate_tlb;	/* inform SP to invalidate mmu TLB */
+	u32 debug_buffer_ddr_address;	/* the addr of DDR debug queue */
+
+	/* input system perf count array */
+	u32 perf_counter_input_system_error;
+	u32 threads_stack;	/* sp thread's stack pointers */
+	u32 threads_stack_size;	/* sp thread's stack sizes */
+	u32 curr_binary_id;	/* current binary id */
+	u32 raw_copy_line_count;	/* raw copy line counter */
+	u32 ddr_parameter_address;	/* acc param ddrptr, sp dmem */
+	u32 ddr_parameter_size;	/* acc param size, sp dmem */
+	/* Entry functions */
+	u32 sp_entry;		/* The SP entry function */
+	u32 tagger_frames_addr;	/* Base address of tagger state */
+};
+
+struct imgu_fw_bl_info {
+	u32 num_dma_cmds;	/* Number of cmds sent by CSS */
+	u32 dma_cmd_list;	/* Dma command list sent by CSS */
+	u32 sw_state;		/* Polled from css, enum imgu_abi_bl_swstate */
+	/* Entry functions */
+	u32 bl_entry;		/* The SP entry function */
+};
+
+struct imgu_fw_acc_info {
+	u32 per_frame_data;	/* Dummy for now */
+};
+
+union imgu_fw_union {
+	struct imgu_fw_binary_xinfo isp;	/* ISP info */
+	struct imgu_fw_sp_info sp;	/* SP info */
+	struct imgu_fw_sp_info sp1;	/* SP1 info */
+	struct imgu_fw_bl_info bl;	/* Bootloader info */
+	struct imgu_fw_acc_info acc;	/* Accelerator info */
+};
+
+struct imgu_fw_info {
+	size_t header_size;	/* size of fw header */
+	u32 type __aligned(8);	/* enum imgu_fw_type */
+
+	union imgu_fw_union info;	/* Binary info */
+	struct imgu_abi_blob_info blob;	/* Blob info */
+	/* Dynamic part */
+	u64 next;
+
+	u32 loaded __aligned(8);	/* Firmware has been loaded */
+	const u64 isp_code __aligned(8);	/* ISP pointer to code */
+	/* Firmware handle between user space and kernel */
+	u32 handle __aligned(8);
+	/* Sections to copy from/to ISP */
+	struct imgu_abi_isp_param_segments mem_initializers;
+	/* Initializer for local ISP memories */
+};
+
+struct imgu_fw_bi_file_h {
+	char version[64];	/* branch tag + week day + time */
+	int binary_nr;		/* Number of binaries */
+	unsigned int h_size;	/* sizeof(struct imgu_fw_bi_file_h) */
+};
+
+struct imgu_fw_header {
+	struct imgu_fw_bi_file_h file_header;
+	struct imgu_fw_info binary_header[];	/* binary_nr items */
+};
+
+/******************* Firmware functions *******************/
+
+int imgu_css_fw_init(struct imgu_css *css);
+void imgu_css_fw_cleanup(struct imgu_css *css);
+
+unsigned int imgu_css_fw_obgrid_size(const struct imgu_fw_info *bi);
+void *imgu_css_fw_pipeline_params(struct imgu_css *css, unsigned int pipe,
+				  enum imgu_abi_param_class cls,
+				  enum imgu_abi_memories mem,
+				  struct imgu_fw_isp_parameter *par,
+				  size_t par_size, void *binary_params);
+
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-css-params.c b/drivers/staging/media/ipu3/ipu3-css-params.c
new file mode 100644
index 000000000..76ad802d6
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-params.c
@@ -0,0 +1,2959 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/device.h>
+
+#include "ipu3-css.h"
+#include "ipu3-css-fw.h"
+#include "ipu3-tables.h"
+#include "ipu3-css-params.h"
+
+#define DIV_ROUND_CLOSEST_DOWN(a, b)	(((a) + ((b) / 2) - 1) / (b))
+#define roundclosest_down(a, b)		(DIV_ROUND_CLOSEST_DOWN(a, b) * (b))
+
+#define IPU3_UAPI_ANR_MAX_RESET		((1 << 12) - 1)
+#define IPU3_UAPI_ANR_MIN_RESET		(((-1) << 12) + 1)
+
+struct imgu_css_scaler_info {
+	unsigned int phase_step;	/* Same for luma/chroma */
+	int exp_shift;
+
+	unsigned int phase_init;	/* luma/chroma dependent */
+	int pad_left;
+	int pad_right;
+	int crop_left;
+	int crop_top;
+};
+
+static unsigned int imgu_css_scaler_get_exp(unsigned int counter,
+					    unsigned int divider)
+{
+	int i = fls(divider) - fls(counter);
+
+	if (i <= 0)
+		return 0;
+
+	if (divider >> i < counter)
+		i = i - 1;
+
+	return i;
+}
+
+/* Set up the CSS scaler look up table */
+static void
+imgu_css_scaler_setup_lut(unsigned int taps, unsigned int input_width,
+			  unsigned int output_width, int phase_step_correction,
+			  const int *coeffs, unsigned int coeffs_size,
+			  s8 coeff_lut[], struct imgu_css_scaler_info *info)
+{
+	int tap, phase, phase_sum_left, phase_sum_right;
+	int exponent = imgu_css_scaler_get_exp(output_width, input_width);
+	int mantissa = (1 << exponent) * output_width;
+	unsigned int phase_step, phase_taps;
+
+	if (input_width == output_width) {
+		for (phase = 0; phase < IMGU_SCALER_PHASES; phase++) {
+			phase_taps = phase * IMGU_SCALER_FILTER_TAPS;
+			for (tap = 0; tap < taps; tap++)
+				coeff_lut[phase_taps + tap] = 0;
+		}
+
+		info->phase_step = IMGU_SCALER_PHASES *
+			(1 << IMGU_SCALER_PHASE_COUNTER_PREC_REF);
+		info->exp_shift = 0;
+		info->pad_left = 0;
+		info->pad_right = 0;
+		info->phase_init = 0;
+		info->crop_left = 0;
+		info->crop_top = 0;
+		return;
+	}
+
+	for (phase = 0; phase < IMGU_SCALER_PHASES; phase++) {
+		phase_taps = phase * IMGU_SCALER_FILTER_TAPS;
+		for (tap = 0; tap < taps; tap++) {
+			/* flip table to for convolution reverse indexing */
+			s64 coeff = coeffs[coeffs_size -
+				((tap * (coeffs_size / taps)) + phase) - 1];
+			coeff *= mantissa;
+			coeff = div64_long(coeff, input_width);
+
+			/* Add +"0.5" */
+			coeff += 1 << (IMGU_SCALER_COEFF_BITS - 1);
+			coeff >>= IMGU_SCALER_COEFF_BITS;
+			coeff_lut[phase_taps + tap] = coeff;
+		}
+	}
+
+	phase_step = IMGU_SCALER_PHASES *
+			(1 << IMGU_SCALER_PHASE_COUNTER_PREC_REF) *
+			output_width / input_width;
+	phase_step += phase_step_correction;
+	phase_sum_left = (taps / 2 * IMGU_SCALER_PHASES *
+			(1 << IMGU_SCALER_PHASE_COUNTER_PREC_REF)) -
+			(1 << (IMGU_SCALER_PHASE_COUNTER_PREC_REF - 1));
+	phase_sum_right = (taps / 2 * IMGU_SCALER_PHASES *
+			(1 << IMGU_SCALER_PHASE_COUNTER_PREC_REF)) +
+			(1 << (IMGU_SCALER_PHASE_COUNTER_PREC_REF - 1));
+
+	info->exp_shift = IMGU_SCALER_MAX_EXPONENT_SHIFT - exponent;
+	info->pad_left = (phase_sum_left % phase_step == 0) ?
+		phase_sum_left / phase_step - 1 : phase_sum_left / phase_step;
+	info->pad_right = (phase_sum_right % phase_step == 0) ?
+		phase_sum_right / phase_step - 1 : phase_sum_right / phase_step;
+	info->phase_init = phase_sum_left - phase_step * info->pad_left;
+	info->phase_step = phase_step;
+	info->crop_left = taps - 1;
+	info->crop_top = taps - 1;
+}
+
+/*
+ * Calculates the exact output image width/height, based on phase_step setting
+ * (must be perfectly aligned with hardware).
+ */
+static unsigned int
+imgu_css_scaler_calc_scaled_output(unsigned int input,
+				   struct imgu_css_scaler_info *info)
+{
+	unsigned int arg1 = input * info->phase_step +
+			(1 - IMGU_SCALER_TAPS_Y / 2) * IMGU_SCALER_FIR_PHASES -
+			IMGU_SCALER_FIR_PHASES / (2 * IMGU_SCALER_PHASES);
+	unsigned int arg2 = ((IMGU_SCALER_TAPS_Y / 2) * IMGU_SCALER_FIR_PHASES +
+			IMGU_SCALER_FIR_PHASES / (2 * IMGU_SCALER_PHASES)) *
+			IMGU_SCALER_FIR_PHASES + info->phase_step / 2;
+
+	return ((arg1 + (arg2 - IMGU_SCALER_FIR_PHASES * info->phase_step) /
+		IMGU_SCALER_FIR_PHASES) / (2 * IMGU_SCALER_FIR_PHASES)) * 2;
+}
+
+/*
+ * Calculate the output width and height, given the luma
+ * and chroma details of a scaler
+ */
+static void
+imgu_css_scaler_calc(u32 input_width, u32 input_height, u32 target_width,
+		     u32 target_height, struct imgu_abi_osys_config *cfg,
+		     struct imgu_css_scaler_info *info_luma,
+		     struct imgu_css_scaler_info *info_chroma,
+		     unsigned int *output_width, unsigned int *output_height,
+		     unsigned int *procmode)
+{
+	u32 out_width = target_width;
+	u32 out_height = target_height;
+	const unsigned int height_alignment = 2;
+	int phase_step_correction = -1;
+
+	/*
+	 * Calculate scaled output width. If the horizontal and vertical scaling
+	 * factor is different, then choose the biggest and crop off excess
+	 * lines or columns after formatting.
+	 */
+	if (target_height * input_width > target_width * input_height)
+		target_width = DIV_ROUND_UP(target_height * input_width,
+					    input_height);
+
+	if (input_width == target_width)
+		*procmode = IMGU_ABI_OSYS_PROCMODE_BYPASS;
+	else
+		*procmode = IMGU_ABI_OSYS_PROCMODE_DOWNSCALE;
+
+	memset(&cfg->scaler_coeffs_chroma, 0,
+	       sizeof(cfg->scaler_coeffs_chroma));
+	memset(&cfg->scaler_coeffs_luma, 0, sizeof(cfg->scaler_coeffs_luma));
+	do {
+		phase_step_correction++;
+
+		imgu_css_scaler_setup_lut(IMGU_SCALER_TAPS_Y,
+					  input_width, target_width,
+					  phase_step_correction,
+					  imgu_css_downscale_4taps,
+					  IMGU_SCALER_DOWNSCALE_4TAPS_LEN,
+					  cfg->scaler_coeffs_luma, info_luma);
+
+		imgu_css_scaler_setup_lut(IMGU_SCALER_TAPS_UV,
+					  input_width, target_width,
+					  phase_step_correction,
+					  imgu_css_downscale_2taps,
+					  IMGU_SCALER_DOWNSCALE_2TAPS_LEN,
+					  cfg->scaler_coeffs_chroma,
+					  info_chroma);
+
+		out_width = imgu_css_scaler_calc_scaled_output(input_width,
+							       info_luma);
+		out_height = imgu_css_scaler_calc_scaled_output(input_height,
+								info_luma);
+	} while ((out_width < target_width || out_height < target_height ||
+		 !IS_ALIGNED(out_height, height_alignment)) &&
+		 phase_step_correction <= 5);
+
+	*output_width = out_width;
+	*output_height = out_height;
+}
+
+/********************** Osys routines for scaler****************************/
+
+static void imgu_css_osys_set_format(enum imgu_abi_frame_format host_format,
+				     unsigned int *osys_format,
+				     unsigned int *osys_tiling)
+{
+	*osys_format = IMGU_ABI_OSYS_FORMAT_YUV420;
+	*osys_tiling = IMGU_ABI_OSYS_TILING_NONE;
+
+	switch (host_format) {
+	case IMGU_ABI_FRAME_FORMAT_YUV420:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_YUV420;
+		break;
+	case IMGU_ABI_FRAME_FORMAT_YV12:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_YV12;
+		break;
+	case IMGU_ABI_FRAME_FORMAT_NV12:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_NV12;
+		break;
+	case IMGU_ABI_FRAME_FORMAT_NV16:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_NV16;
+		break;
+	case IMGU_ABI_FRAME_FORMAT_NV21:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_NV21;
+		break;
+	case IMGU_ABI_FRAME_FORMAT_NV12_TILEY:
+		*osys_format = IMGU_ABI_OSYS_FORMAT_NV12;
+		*osys_tiling = IMGU_ABI_OSYS_TILING_Y;
+		break;
+	default:
+		/* For now, assume use default values */
+		break;
+	}
+}
+
+/*
+ * Function calculates input frame stripe offset, based
+ * on output frame stripe offset and filter parameters.
+ */
+static int imgu_css_osys_calc_stripe_offset(int stripe_offset_out,
+					    int fir_phases, int phase_init,
+					    int phase_step, int pad_left)
+{
+	int stripe_offset_inp = stripe_offset_out * fir_phases -
+				pad_left * phase_step;
+
+	return DIV_ROUND_UP(stripe_offset_inp - phase_init, phase_step);
+}
+
+/*
+ * Calculate input frame phase, given the output frame
+ * stripe offset and filter parameters
+ */
+static int imgu_css_osys_calc_stripe_phase_init(int stripe_offset_out,
+						int fir_phases, int phase_init,
+						int phase_step, int pad_left)
+{
+	int stripe_offset_inp =
+		imgu_css_osys_calc_stripe_offset(stripe_offset_out,
+						 fir_phases, phase_init,
+						 phase_step, pad_left);
+
+	return phase_init + ((pad_left + stripe_offset_inp) * phase_step) -
+		stripe_offset_out * fir_phases;
+}
+
+/*
+ * This function calculates input frame stripe width,
+ * based on output frame stripe offset and filter parameters
+ */
+static int imgu_css_osys_calc_inp_stripe_width(int stripe_width_out,
+					       int fir_phases, int phase_init,
+					       int phase_step, int fir_taps,
+					       int pad_left, int pad_right)
+{
+	int stripe_width_inp = (stripe_width_out + fir_taps - 1) * fir_phases;
+
+	stripe_width_inp = DIV_ROUND_UP(stripe_width_inp - phase_init,
+					phase_step);
+
+	return stripe_width_inp - pad_left - pad_right;
+}
+
+/*
+ * This function calculates output frame stripe width, basedi
+ * on output frame stripe offset and filter parameters
+ */
+static int imgu_css_osys_out_stripe_width(int stripe_width_inp, int fir_phases,
+					  int phase_init, int phase_step,
+					  int fir_taps, int pad_left,
+					  int pad_right, int column_offset)
+{
+	int stripe_width_out = (pad_left + stripe_width_inp +
+				pad_right - column_offset) * phase_step;
+
+	stripe_width_out = (stripe_width_out + phase_init) / fir_phases;
+
+	return stripe_width_out - (fir_taps - 1);
+}
+
+struct imgu_css_reso {
+	unsigned int input_width;
+	unsigned int input_height;
+	enum imgu_abi_frame_format input_format;
+	unsigned int pin_width[IMGU_ABI_OSYS_PINS];
+	unsigned int pin_height[IMGU_ABI_OSYS_PINS];
+	unsigned int pin_stride[IMGU_ABI_OSYS_PINS];
+	enum imgu_abi_frame_format pin_format[IMGU_ABI_OSYS_PINS];
+	int chunk_width;
+	int chunk_height;
+	int block_height;
+	int block_width;
+};
+
+struct imgu_css_frame_params {
+	/* Output pins */
+	unsigned int enable;
+	unsigned int format;
+	unsigned int flip;
+	unsigned int mirror;
+	unsigned int tiling;
+	unsigned int reduce_range;
+	unsigned int width;
+	unsigned int height;
+	unsigned int stride;
+	unsigned int scaled;
+	unsigned int crop_left;
+	unsigned int crop_top;
+};
+
+struct imgu_css_stripe_params {
+	unsigned int processing_mode;
+	unsigned int phase_step;
+	unsigned int exp_shift;
+	unsigned int phase_init_left_y;
+	unsigned int phase_init_left_uv;
+	unsigned int phase_init_top_y;
+	unsigned int phase_init_top_uv;
+	unsigned int pad_left_y;
+	unsigned int pad_left_uv;
+	unsigned int pad_right_y;
+	unsigned int pad_right_uv;
+	unsigned int pad_top_y;
+	unsigned int pad_top_uv;
+	unsigned int pad_bottom_y;
+	unsigned int pad_bottom_uv;
+	unsigned int crop_left_y;
+	unsigned int crop_top_y;
+	unsigned int crop_left_uv;
+	unsigned int crop_top_uv;
+	unsigned int start_column_y;
+	unsigned int start_column_uv;
+	unsigned int chunk_width;
+	unsigned int chunk_height;
+	unsigned int block_width;
+	unsigned int block_height;
+	unsigned int input_width;
+	unsigned int input_height;
+	int output_width[IMGU_ABI_OSYS_PINS];
+	int output_height[IMGU_ABI_OSYS_PINS];
+	int output_offset[IMGU_ABI_OSYS_PINS];
+};
+
+/*
+ * frame_params - size IMGU_ABI_OSYS_PINS
+ * stripe_params - size IPU3_UAPI_MAX_STRIPES
+ */
+static int imgu_css_osys_calc_frame_and_stripe_params(
+		struct imgu_css *css, unsigned int stripes,
+		struct imgu_abi_osys_config *osys,
+		struct imgu_css_scaler_info *scaler_luma,
+		struct imgu_css_scaler_info *scaler_chroma,
+		struct imgu_css_frame_params frame_params[],
+		struct imgu_css_stripe_params stripe_params[],
+		unsigned int pipe)
+{
+	struct imgu_css_reso reso;
+	unsigned int output_width, pin, s;
+	u32 input_width, input_height, target_width, target_height;
+	unsigned int procmode = 0;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	input_width = css_pipe->rect[IPU3_CSS_RECT_GDC].width;
+	input_height = css_pipe->rect[IPU3_CSS_RECT_GDC].height;
+	target_width = css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.width;
+	target_height = css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.height;
+
+	/* Frame parameters */
+
+	/* Input width for Output System is output width of DVS (with GDC) */
+	reso.input_width = css_pipe->rect[IPU3_CSS_RECT_GDC].width;
+
+	/* Input height for Output System is output height of DVS (with GDC) */
+	reso.input_height = css_pipe->rect[IPU3_CSS_RECT_GDC].height;
+
+	reso.input_format =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].css_fmt->frame_format;
+
+	reso.pin_width[IMGU_ABI_OSYS_PIN_OUT] =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.width;
+	reso.pin_height[IMGU_ABI_OSYS_PIN_OUT] =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.height;
+	reso.pin_stride[IMGU_ABI_OSYS_PIN_OUT] =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].width_pad;
+	reso.pin_format[IMGU_ABI_OSYS_PIN_OUT] =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].css_fmt->frame_format;
+
+	reso.pin_width[IMGU_ABI_OSYS_PIN_VF] =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.width;
+	reso.pin_height[IMGU_ABI_OSYS_PIN_VF] =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.height;
+	reso.pin_stride[IMGU_ABI_OSYS_PIN_VF] =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].width_pad;
+	reso.pin_format[IMGU_ABI_OSYS_PIN_VF] =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].css_fmt->frame_format;
+
+	/* Configure the frame parameters for all output pins */
+
+	frame_params[IMGU_ABI_OSYS_PIN_OUT].width =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.width;
+	frame_params[IMGU_ABI_OSYS_PIN_OUT].height =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.height;
+	frame_params[IMGU_ABI_OSYS_PIN_VF].width =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.width;
+	frame_params[IMGU_ABI_OSYS_PIN_VF].height =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.height;
+	frame_params[IMGU_ABI_OSYS_PIN_VF].crop_top = 0;
+	frame_params[IMGU_ABI_OSYS_PIN_VF].crop_left = 0;
+
+	for (pin = 0; pin < IMGU_ABI_OSYS_PINS; pin++) {
+		int enable = 0;
+		int scaled = 0;
+		unsigned int format = 0;
+		unsigned int tiling = 0;
+
+		frame_params[pin].flip = 0;
+		frame_params[pin].mirror = 0;
+		frame_params[pin].reduce_range = 0;
+		if (reso.pin_width[pin] != 0 && reso.pin_height[pin] != 0) {
+			enable = 1;
+			if (pin == IMGU_ABI_OSYS_PIN_OUT) {
+				if (reso.input_width < reso.pin_width[pin] ||
+				    reso.input_height < reso.pin_height[pin])
+					return -EINVAL;
+				/*
+				 * When input and output resolution is
+				 * different instead of scaling, cropping
+				 * should happen. Determine the crop factor
+				 * to do the symmetric cropping
+				 */
+				frame_params[pin].crop_left = roundclosest_down(
+						(reso.input_width -
+						 reso.pin_width[pin]) / 2,
+						 IMGU_OSYS_DMA_CROP_W_LIMIT);
+				frame_params[pin].crop_top = roundclosest_down(
+						(reso.input_height -
+						 reso.pin_height[pin]) / 2,
+						 IMGU_OSYS_DMA_CROP_H_LIMIT);
+			} else {
+				if (reso.pin_width[pin] != reso.input_width ||
+				    reso.pin_height[pin] != reso.input_height) {
+					/*
+					 * If resolution is different at input
+					 * and output of OSYS, scaling is
+					 * considered except when pin is MAIN.
+					 * Later it will be decide whether
+					 * scaler factor is 1 or other
+					 * and cropping has to be done or not.
+					 */
+					scaled = 1;
+				}
+			}
+			imgu_css_osys_set_format(reso.pin_format[pin], &format,
+						 &tiling);
+		} else {
+			enable = 0;
+		}
+		frame_params[pin].enable = enable;
+		frame_params[pin].format = format;
+		frame_params[pin].tiling = tiling;
+		frame_params[pin].stride = reso.pin_stride[pin];
+		frame_params[pin].scaled = scaled;
+	}
+
+	imgu_css_scaler_calc(input_width, input_height, target_width,
+			     target_height, osys, scaler_luma, scaler_chroma,
+			     &reso.pin_width[IMGU_ABI_OSYS_PIN_VF],
+			     &reso.pin_height[IMGU_ABI_OSYS_PIN_VF], &procmode);
+	dev_dbg(css->dev, "osys scaler procmode is %u", procmode);
+	output_width = reso.pin_width[IMGU_ABI_OSYS_PIN_VF];
+
+	if (output_width < reso.input_width / 2) {
+		/* Scaling factor <= 0.5 */
+		reso.chunk_width = IMGU_OSYS_BLOCK_WIDTH;
+		reso.block_width = IMGU_OSYS_BLOCK_WIDTH;
+	} else { /* 0.5 <= Scaling factor <= 1.0 */
+		reso.chunk_width = IMGU_OSYS_BLOCK_WIDTH / 2;
+		reso.block_width = IMGU_OSYS_BLOCK_WIDTH;
+	}
+
+	if (output_width <= reso.input_width * 7 / 8) {
+		/* Scaling factor <= 0.875 */
+		reso.chunk_height = IMGU_OSYS_BLOCK_HEIGHT;
+		reso.block_height = IMGU_OSYS_BLOCK_HEIGHT;
+	} else { /* 1.0 <= Scaling factor <= 1.75 */
+		reso.chunk_height = IMGU_OSYS_BLOCK_HEIGHT / 2;
+		reso.block_height = IMGU_OSYS_BLOCK_HEIGHT;
+	}
+
+	/*
+	 * Calculate scaler configuration parameters based on input and output
+	 * resolution.
+	 */
+
+	if (frame_params[IMGU_ABI_OSYS_PIN_VF].enable) {
+		/*
+		 * When aspect ratio is different between target resolution and
+		 * required resolution, determine the crop factor to do
+		 * symmetric cropping
+		 */
+		u32 w = reso.pin_width[IMGU_ABI_OSYS_PIN_VF] -
+			frame_params[IMGU_ABI_OSYS_PIN_VF].width;
+		u32 h = reso.pin_height[IMGU_ABI_OSYS_PIN_VF] -
+			frame_params[IMGU_ABI_OSYS_PIN_VF].height;
+
+		frame_params[IMGU_ABI_OSYS_PIN_VF].crop_left =
+			roundclosest_down(w / 2, IMGU_OSYS_DMA_CROP_W_LIMIT);
+		frame_params[IMGU_ABI_OSYS_PIN_VF].crop_top =
+			roundclosest_down(h / 2, IMGU_OSYS_DMA_CROP_H_LIMIT);
+
+		if (reso.input_height % 4 || reso.input_width % 8) {
+			dev_err(css->dev, "OSYS input width is not multiple of 8 or\n");
+			dev_err(css->dev, "height is not multiple of 4\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Stripe parameters */
+
+	if (frame_params[IMGU_ABI_OSYS_PIN_VF].enable) {
+		output_width = reso.pin_width[IMGU_ABI_OSYS_PIN_VF];
+	} else {
+		/*
+		 * in case scaler output is not enabled
+		 * take output width as input width since
+		 * there is no scaling at main pin.
+		 * Due to the fact that main pin can be different
+		 * from input resolution to osys in the case of cropping,
+		 * main pin resolution is not taken.
+		 */
+		output_width = reso.input_width;
+	}
+
+	for (s = 0; s < stripes; s++) {
+		int stripe_offset_inp_y = 0;
+		int stripe_offset_inp_uv = 0;
+		int stripe_offset_out_y = 0;
+		int stripe_offset_out_uv = 0;
+		int stripe_phase_init_y = scaler_luma->phase_init;
+		int stripe_phase_init_uv = scaler_chroma->phase_init;
+		int stripe_offset_blk_y = 0;
+		int stripe_offset_blk_uv = 0;
+		int stripe_offset_col_y = 0;
+		int stripe_offset_col_uv = 0;
+		int stripe_pad_left_y = scaler_luma->pad_left;
+		int stripe_pad_left_uv = scaler_chroma->pad_left;
+		int stripe_pad_right_y = scaler_luma->pad_right;
+		int stripe_pad_right_uv = scaler_chroma->pad_right;
+		int stripe_crop_left_y = scaler_luma->crop_left;
+		int stripe_crop_left_uv = scaler_chroma->crop_left;
+		int stripe_input_width_y = reso.input_width;
+		int stripe_input_width_uv = 0;
+		int stripe_output_width_y = output_width;
+		int stripe_output_width_uv = 0;
+		int chunk_floor_y = 0;
+		int chunk_floor_uv = 0;
+		int chunk_ceil_uv = 0;
+
+		if (stripes > 1) {
+			if (s > 0) {
+				/* Calculate stripe offsets */
+				stripe_offset_out_y =
+					output_width * s / stripes;
+				stripe_offset_out_y =
+					rounddown(stripe_offset_out_y,
+						  IPU3_UAPI_ISP_VEC_ELEMS);
+				stripe_offset_out_uv = stripe_offset_out_y /
+						IMGU_LUMA_TO_CHROMA_RATIO;
+				stripe_offset_inp_y =
+					imgu_css_osys_calc_stripe_offset(
+						stripe_offset_out_y,
+						IMGU_OSYS_FIR_PHASES,
+						scaler_luma->phase_init,
+						scaler_luma->phase_step,
+						scaler_luma->pad_left);
+				stripe_offset_inp_uv =
+					imgu_css_osys_calc_stripe_offset(
+						stripe_offset_out_uv,
+						IMGU_OSYS_FIR_PHASES,
+						scaler_chroma->phase_init,
+						scaler_chroma->phase_step,
+						scaler_chroma->pad_left);
+
+				/* Calculate stripe phase init */
+				stripe_phase_init_y =
+					imgu_css_osys_calc_stripe_phase_init(
+						stripe_offset_out_y,
+						IMGU_OSYS_FIR_PHASES,
+						scaler_luma->phase_init,
+						scaler_luma->phase_step,
+						scaler_luma->pad_left);
+				stripe_phase_init_uv =
+					imgu_css_osys_calc_stripe_phase_init(
+						stripe_offset_out_uv,
+						IMGU_OSYS_FIR_PHASES,
+						scaler_chroma->phase_init,
+						scaler_chroma->phase_step,
+						scaler_chroma->pad_left);
+
+				/*
+				 * Chunk boundary corner case - luma and chroma
+				 * start from different input chunks.
+				 */
+				chunk_floor_y = rounddown(stripe_offset_inp_y,
+							  reso.chunk_width);
+				chunk_floor_uv =
+					rounddown(stripe_offset_inp_uv,
+						  reso.chunk_width /
+						  IMGU_LUMA_TO_CHROMA_RATIO);
+
+				if (chunk_floor_y != chunk_floor_uv *
+				    IMGU_LUMA_TO_CHROMA_RATIO) {
+					/*
+					 * Match starting luma/chroma chunks.
+					 * Decrease offset for UV and add output
+					 * cropping.
+					 */
+					stripe_offset_inp_uv -= 1;
+					stripe_crop_left_uv += 1;
+					stripe_phase_init_uv -=
+						scaler_luma->phase_step;
+					if (stripe_phase_init_uv < 0)
+						stripe_phase_init_uv =
+							stripe_phase_init_uv +
+							IMGU_OSYS_FIR_PHASES;
+				}
+				/*
+				 * FW workaround for a HW bug: if the first
+				 * chroma pixel is generated exactly at the end
+				 * of chunck scaler HW may not output the pixel
+				 * for downscale factors smaller than 1.5
+				 * (timing issue).
+				 */
+				chunk_ceil_uv =
+					roundup(stripe_offset_inp_uv,
+						reso.chunk_width /
+						IMGU_LUMA_TO_CHROMA_RATIO);
+
+				if (stripe_offset_inp_uv ==
+				    chunk_ceil_uv - IMGU_OSYS_TAPS_UV) {
+					/*
+					 * Decrease input offset and add
+					 * output cropping
+					 */
+					stripe_offset_inp_uv -= 1;
+					stripe_phase_init_uv -=
+						scaler_luma->phase_step;
+					if (stripe_phase_init_uv < 0) {
+						stripe_phase_init_uv +=
+							IMGU_OSYS_FIR_PHASES;
+						stripe_crop_left_uv += 1;
+					}
+				}
+
+				/*
+				 * Calculate block and column offsets for the
+				 * input stripe
+				 */
+				stripe_offset_blk_y =
+					rounddown(stripe_offset_inp_y,
+						  IMGU_INPUT_BLOCK_WIDTH);
+				stripe_offset_blk_uv =
+					rounddown(stripe_offset_inp_uv,
+						  IMGU_INPUT_BLOCK_WIDTH /
+						  IMGU_LUMA_TO_CHROMA_RATIO);
+				stripe_offset_col_y = stripe_offset_inp_y -
+							stripe_offset_blk_y;
+				stripe_offset_col_uv = stripe_offset_inp_uv -
+							stripe_offset_blk_uv;
+
+				/* Left padding is only for the first stripe */
+				stripe_pad_left_y = 0;
+				stripe_pad_left_uv = 0;
+			}
+
+			/* Right padding is only for the last stripe */
+			if (s < stripes - 1) {
+				int next_offset;
+
+				stripe_pad_right_y = 0;
+				stripe_pad_right_uv = 0;
+
+				next_offset = output_width * (s + 1) / stripes;
+				next_offset = rounddown(next_offset, 64);
+				stripe_output_width_y = next_offset -
+							stripe_offset_out_y;
+			} else {
+				stripe_output_width_y = output_width -
+							stripe_offset_out_y;
+			}
+
+			/* Calculate target output stripe width */
+			stripe_output_width_uv = stripe_output_width_y /
+						IMGU_LUMA_TO_CHROMA_RATIO;
+			/* Calculate input stripe width */
+			stripe_input_width_y = stripe_offset_col_y +
+				imgu_css_osys_calc_inp_stripe_width(
+						stripe_output_width_y,
+						IMGU_OSYS_FIR_PHASES,
+						stripe_phase_init_y,
+						scaler_luma->phase_step,
+						IMGU_OSYS_TAPS_Y,
+						stripe_pad_left_y,
+						stripe_pad_right_y);
+
+			stripe_input_width_uv = stripe_offset_col_uv +
+				imgu_css_osys_calc_inp_stripe_width(
+						stripe_output_width_uv,
+						IMGU_OSYS_FIR_PHASES,
+						stripe_phase_init_uv,
+						scaler_chroma->phase_step,
+						IMGU_OSYS_TAPS_UV,
+						stripe_pad_left_uv,
+						stripe_pad_right_uv);
+
+			stripe_input_width_uv = max(DIV_ROUND_UP(
+						    stripe_input_width_y,
+						    IMGU_LUMA_TO_CHROMA_RATIO),
+						    stripe_input_width_uv);
+
+			stripe_input_width_y = stripe_input_width_uv *
+						IMGU_LUMA_TO_CHROMA_RATIO;
+
+			if (s >= stripes - 1) {
+				stripe_input_width_y = reso.input_width -
+					stripe_offset_blk_y;
+				/*
+				 * The scaler requires that the last stripe
+				 * spans at least two input blocks.
+				 */
+			}
+
+			/*
+			 * Spec: input stripe width must be a multiple of 8.
+			 * Increase the input width and recalculate the output
+			 * width. This may produce an extra column of junk
+			 * blocks which will be overwritten by the
+			 * next stripe.
+			 */
+			stripe_input_width_y = ALIGN(stripe_input_width_y, 8);
+			stripe_output_width_y =
+				imgu_css_osys_out_stripe_width(
+						stripe_input_width_y,
+						IMGU_OSYS_FIR_PHASES,
+						stripe_phase_init_y,
+						scaler_luma->phase_step,
+						IMGU_OSYS_TAPS_Y,
+						stripe_pad_left_y,
+						stripe_pad_right_y,
+						stripe_offset_col_y);
+
+			stripe_output_width_y =
+					rounddown(stripe_output_width_y,
+						  IMGU_LUMA_TO_CHROMA_RATIO);
+		}
+		/*
+		 * Following section executes and process parameters
+		 * for both cases - Striping or No Striping.
+		 */
+		{
+			unsigned int i;
+			/*Input resolution */
+
+			stripe_params[s].input_width = stripe_input_width_y;
+			stripe_params[s].input_height = reso.input_height;
+
+			for (i = 0; i < IMGU_ABI_OSYS_PINS; i++) {
+				if (frame_params[i].scaled) {
+					/*
+					 * Output stripe resolution and offset
+					 * as produced by the scaler; actual
+					 * output resolution may be slightly
+					 * smaller.
+					 */
+					stripe_params[s].output_width[i] =
+						stripe_output_width_y;
+					stripe_params[s].output_height[i] =
+						reso.pin_height[i];
+					stripe_params[s].output_offset[i] =
+						stripe_offset_out_y;
+				} else {
+					/* Unscaled pin */
+					stripe_params[s].output_width[i] =
+						stripe_params[s].input_width;
+					stripe_params[s].output_height[i] =
+						stripe_params[s].input_height;
+					stripe_params[s].output_offset[i] =
+						stripe_offset_blk_y;
+				}
+			}
+
+			/* If no pin use scale, we use BYPASS mode */
+			stripe_params[s].processing_mode = procmode;
+			stripe_params[s].phase_step = scaler_luma->phase_step;
+			stripe_params[s].exp_shift = scaler_luma->exp_shift;
+			stripe_params[s].phase_init_left_y =
+				stripe_phase_init_y;
+			stripe_params[s].phase_init_left_uv =
+				stripe_phase_init_uv;
+			stripe_params[s].phase_init_top_y =
+				scaler_luma->phase_init;
+			stripe_params[s].phase_init_top_uv =
+				scaler_chroma->phase_init;
+			stripe_params[s].pad_left_y = stripe_pad_left_y;
+			stripe_params[s].pad_left_uv = stripe_pad_left_uv;
+			stripe_params[s].pad_right_y = stripe_pad_right_y;
+			stripe_params[s].pad_right_uv = stripe_pad_right_uv;
+			stripe_params[s].pad_top_y = scaler_luma->pad_left;
+			stripe_params[s].pad_top_uv = scaler_chroma->pad_left;
+			stripe_params[s].pad_bottom_y = scaler_luma->pad_right;
+			stripe_params[s].pad_bottom_uv =
+				scaler_chroma->pad_right;
+			stripe_params[s].crop_left_y = stripe_crop_left_y;
+			stripe_params[s].crop_top_y = scaler_luma->crop_top;
+			stripe_params[s].crop_left_uv = stripe_crop_left_uv;
+			stripe_params[s].crop_top_uv = scaler_chroma->crop_top;
+			stripe_params[s].start_column_y = stripe_offset_col_y;
+			stripe_params[s].start_column_uv = stripe_offset_col_uv;
+			stripe_params[s].chunk_width = reso.chunk_width;
+			stripe_params[s].chunk_height = reso.chunk_height;
+			stripe_params[s].block_width = reso.block_width;
+			stripe_params[s].block_height = reso.block_height;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * This function configures the Output Formatter System, given the number of
+ * stripes, scaler luma and chrome parameters
+ */
+static int imgu_css_osys_calc(struct imgu_css *css, unsigned int pipe,
+			      unsigned int stripes,
+			      struct imgu_abi_osys_config *osys,
+			      struct imgu_css_scaler_info *scaler_luma,
+			      struct imgu_css_scaler_info *scaler_chroma,
+			      struct imgu_abi_stripes block_stripes[])
+{
+	struct imgu_css_frame_params frame_params[IMGU_ABI_OSYS_PINS];
+	struct imgu_css_stripe_params stripe_params[IPU3_UAPI_MAX_STRIPES];
+	struct imgu_abi_osys_formatter_params *param;
+	unsigned int pin, s;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	memset(osys, 0, sizeof(*osys));
+
+	/* Compute the frame and stripe params */
+	if (imgu_css_osys_calc_frame_and_stripe_params(css, stripes, osys,
+						       scaler_luma,
+						       scaler_chroma,
+						       frame_params,
+						       stripe_params, pipe))
+		return -EINVAL;
+
+	/* Output formatter system parameters */
+
+	for (s = 0; s < stripes; s++) {
+		struct imgu_abi_osys_scaler_params *scaler =
+					&osys->scaler[s].param;
+		int fifo_addr_fmt = IMGU_FIFO_ADDR_SCALER_TO_FMT;
+		int fifo_addr_ack = IMGU_FIFO_ADDR_SCALER_TO_SP;
+
+		/* OUTPUT 0 / PIN 0 is only Scaler output */
+		scaler->inp_buf_y_st_addr = IMGU_VMEM1_INP_BUF_ADDR;
+
+		/*
+		 * = (IMGU_OSYS_BLOCK_WIDTH / IMGU_VMEM1_ELEMS_PER_VEC)
+		 * = (2 * IPU3_UAPI_ISP_VEC_ELEMS) /
+		 *   (IMGU_HIVE_OF_SYS_OF_SYSTEM_NWAYS)
+		 * = 2 * 64 / 32 = 4
+		 */
+		scaler->inp_buf_y_line_stride = IMGU_VMEM1_Y_STRIDE;
+		/*
+		 * = (IMGU_VMEM1_V_OFFSET + VMEM1_uv_size)
+		 * = (IMGU_VMEM1_U_OFFSET + VMEM1_uv_size) +
+		 *	(VMEM1_y_size / 4)
+		 * = (VMEM1_y_size) + (VMEM1_y_size / 4) +
+		 * (IMGU_OSYS_BLOCK_HEIGHT * IMGU_VMEM1_Y_STRIDE)/4
+		 * = (IMGU_OSYS_BLOCK_HEIGHT * IMGU_VMEM1_Y_STRIDE)
+		 */
+		scaler->inp_buf_y_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+		scaler->inp_buf_u_st_addr = IMGU_VMEM1_INP_BUF_ADDR +
+						IMGU_VMEM1_U_OFFSET;
+		scaler->inp_buf_v_st_addr = IMGU_VMEM1_INP_BUF_ADDR +
+						IMGU_VMEM1_V_OFFSET;
+		scaler->inp_buf_uv_line_stride = IMGU_VMEM1_UV_STRIDE;
+		scaler->inp_buf_uv_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+		scaler->inp_buf_chunk_width = stripe_params[s].chunk_width;
+		scaler->inp_buf_nr_buffers = IMGU_OSYS_NUM_INPUT_BUFFERS;
+
+		/* Output buffers */
+		scaler->out_buf_y_st_addr = IMGU_VMEM1_INT_BUF_ADDR;
+		scaler->out_buf_y_line_stride = stripe_params[s].block_width /
+						IMGU_VMEM1_ELEMS_PER_VEC;
+		scaler->out_buf_y_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+		scaler->out_buf_u_st_addr = IMGU_VMEM1_INT_BUF_ADDR +
+						IMGU_VMEM1_U_OFFSET;
+		scaler->out_buf_v_st_addr = IMGU_VMEM1_INT_BUF_ADDR +
+						IMGU_VMEM1_V_OFFSET;
+		scaler->out_buf_uv_line_stride = stripe_params[s].block_width /
+						IMGU_VMEM1_ELEMS_PER_VEC / 2;
+		scaler->out_buf_uv_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+		scaler->out_buf_nr_buffers = IMGU_OSYS_NUM_INTERM_BUFFERS;
+
+		/* Intermediate buffers */
+		scaler->int_buf_y_st_addr = IMGU_VMEM2_BUF_Y_ADDR;
+		scaler->int_buf_y_line_stride = IMGU_VMEM2_BUF_Y_STRIDE;
+		scaler->int_buf_u_st_addr = IMGU_VMEM2_BUF_U_ADDR;
+		scaler->int_buf_v_st_addr = IMGU_VMEM2_BUF_V_ADDR;
+		scaler->int_buf_uv_line_stride = IMGU_VMEM2_BUF_UV_STRIDE;
+		scaler->int_buf_height = IMGU_VMEM2_LINES_PER_BLOCK;
+		scaler->int_buf_chunk_width = stripe_params[s].chunk_height;
+		scaler->int_buf_chunk_height = stripe_params[s].block_width;
+
+		/* Context buffers */
+		scaler->ctx_buf_hor_y_st_addr = IMGU_VMEM3_HOR_Y_ADDR;
+		scaler->ctx_buf_hor_u_st_addr = IMGU_VMEM3_HOR_U_ADDR;
+		scaler->ctx_buf_hor_v_st_addr = IMGU_VMEM3_HOR_V_ADDR;
+		scaler->ctx_buf_ver_y_st_addr = IMGU_VMEM3_VER_Y_ADDR;
+		scaler->ctx_buf_ver_u_st_addr = IMGU_VMEM3_VER_U_ADDR;
+		scaler->ctx_buf_ver_v_st_addr = IMGU_VMEM3_VER_V_ADDR;
+
+		/* Addresses for release-input and process-output tokens */
+		scaler->release_inp_buf_addr = fifo_addr_ack;
+		scaler->release_inp_buf_en = 1;
+		scaler->release_out_buf_en = 1;
+		scaler->process_out_buf_addr = fifo_addr_fmt;
+
+		/* Settings dimensions, padding, cropping */
+		scaler->input_image_y_width = stripe_params[s].input_width;
+		scaler->input_image_y_height = stripe_params[s].input_height;
+		scaler->input_image_y_start_column =
+					stripe_params[s].start_column_y;
+		scaler->input_image_uv_start_column =
+					stripe_params[s].start_column_uv;
+		scaler->input_image_y_left_pad = stripe_params[s].pad_left_y;
+		scaler->input_image_uv_left_pad = stripe_params[s].pad_left_uv;
+		scaler->input_image_y_right_pad = stripe_params[s].pad_right_y;
+		scaler->input_image_uv_right_pad =
+					stripe_params[s].pad_right_uv;
+		scaler->input_image_y_top_pad = stripe_params[s].pad_top_y;
+		scaler->input_image_uv_top_pad = stripe_params[s].pad_top_uv;
+		scaler->input_image_y_bottom_pad =
+					stripe_params[s].pad_bottom_y;
+		scaler->input_image_uv_bottom_pad =
+					stripe_params[s].pad_bottom_uv;
+		scaler->processing_mode = stripe_params[s].processing_mode;
+		scaler->scaling_ratio = stripe_params[s].phase_step;
+		scaler->y_left_phase_init = stripe_params[s].phase_init_left_y;
+		scaler->uv_left_phase_init =
+					stripe_params[s].phase_init_left_uv;
+		scaler->y_top_phase_init = stripe_params[s].phase_init_top_y;
+		scaler->uv_top_phase_init = stripe_params[s].phase_init_top_uv;
+		scaler->coeffs_exp_shift = stripe_params[s].exp_shift;
+		scaler->out_y_left_crop = stripe_params[s].crop_left_y;
+		scaler->out_uv_left_crop = stripe_params[s].crop_left_uv;
+		scaler->out_y_top_crop = stripe_params[s].crop_top_y;
+		scaler->out_uv_top_crop = stripe_params[s].crop_top_uv;
+
+		for (pin = 0; pin < IMGU_ABI_OSYS_PINS; pin++) {
+			int in_fifo_addr;
+			int out_fifo_addr;
+			int block_width_vecs;
+			int input_width_s;
+			int input_width_vecs;
+			int input_buf_y_st_addr;
+			int input_buf_u_st_addr;
+			int input_buf_v_st_addr;
+			int input_buf_y_line_stride;
+			int input_buf_uv_line_stride;
+			int output_buf_y_line_stride;
+			int output_buf_uv_line_stride;
+			int output_buf_nr_y_lines;
+			int block_height;
+			int block_width;
+			struct imgu_abi_osys_frame_params *fr_pr;
+
+			fr_pr = &osys->frame[pin].param;
+
+			/* Frame parameters */
+			fr_pr->enable = frame_params[pin].enable;
+			fr_pr->format = frame_params[pin].format;
+			fr_pr->mirror = frame_params[pin].mirror;
+			fr_pr->flip = frame_params[pin].flip;
+			fr_pr->tiling = frame_params[pin].tiling;
+			fr_pr->width = frame_params[pin].width;
+			fr_pr->height = frame_params[pin].height;
+			fr_pr->stride = frame_params[pin].stride;
+			fr_pr->scaled = frame_params[pin].scaled;
+
+			/* Stripe parameters */
+			osys->stripe[s].crop_top[pin] =
+				frame_params[pin].crop_top;
+			osys->stripe[s].input_width =
+				stripe_params[s].input_width;
+			osys->stripe[s].input_height =
+				stripe_params[s].input_height;
+			osys->stripe[s].block_height =
+				stripe_params[s].block_height;
+			osys->stripe[s].block_width =
+				stripe_params[s].block_width;
+			osys->stripe[s].output_width[pin] =
+				stripe_params[s].output_width[pin];
+			osys->stripe[s].output_height[pin] =
+				stripe_params[s].output_height[pin];
+
+			if (s == 0) {
+				/* Only first stripe should do left cropping */
+				osys->stripe[s].crop_left[pin] =
+					frame_params[pin].crop_left;
+				osys->stripe[s].output_offset[pin] =
+					stripe_params[s].output_offset[pin];
+			} else {
+				/*
+				 * Stripe offset for other strips should be
+				 * adjusted according to the cropping done
+				 * at the first strip
+				 */
+				osys->stripe[s].crop_left[pin] = 0;
+				osys->stripe[s].output_offset[pin] =
+					(stripe_params[s].output_offset[pin] -
+					 osys->stripe[0].crop_left[pin]);
+			}
+
+			if (!frame_params[pin].enable)
+				continue;
+
+			/* Formatter: configurations */
+
+			/*
+			 * Get the dimensions of the input blocks of the
+			 * formatter, which is the same as the output
+			 * blocks of the scaler.
+			 */
+			if (frame_params[pin].scaled) {
+				block_height = stripe_params[s].block_height;
+				block_width = stripe_params[s].block_width;
+			} else {
+				block_height = IMGU_OSYS_BLOCK_HEIGHT;
+				block_width = IMGU_OSYS_BLOCK_WIDTH;
+			}
+			block_width_vecs =
+					block_width / IMGU_VMEM1_ELEMS_PER_VEC;
+			/*
+			 * The input/output line stride depends on the
+			 * block size.
+			 */
+			input_buf_y_line_stride = block_width_vecs;
+			input_buf_uv_line_stride = block_width_vecs / 2;
+			output_buf_y_line_stride = block_width_vecs;
+			output_buf_uv_line_stride = block_width_vecs / 2;
+			output_buf_nr_y_lines = block_height;
+			if (frame_params[pin].format ==
+			    IMGU_ABI_OSYS_FORMAT_NV12 ||
+			    frame_params[pin].format ==
+			    IMGU_ABI_OSYS_FORMAT_NV21)
+				output_buf_uv_line_stride =
+					output_buf_y_line_stride;
+
+			/*
+			 * Tiled outputs use a different output buffer
+			 * configuration. The input (= scaler output) block
+			 * width translates to a tile height, and the block
+			 * height to the tile width. The default block size of
+			 * 128x32 maps exactly onto a 4kB tile (512x8) for Y.
+			 * For UV, the tile width is always half.
+			 */
+			if (frame_params[pin].tiling) {
+				output_buf_nr_y_lines = 8;
+				output_buf_y_line_stride = 512 /
+					IMGU_VMEM1_ELEMS_PER_VEC;
+				output_buf_uv_line_stride = 256 /
+					IMGU_VMEM1_ELEMS_PER_VEC;
+			}
+
+			/*
+			 * Store the output buffer line stride. Will be
+			 * used to compute buffer offsets in boundary
+			 * conditions when output blocks are partially
+			 * outside the image.
+			 */
+			osys->stripe[s].buf_stride[pin] =
+				output_buf_y_line_stride *
+				IMGU_HIVE_OF_SYS_OF_SYSTEM_NWAYS;
+			if (frame_params[pin].scaled) {
+				/*
+				 * The input buffs are the intermediate
+				 * buffers (scalers' output)
+				 */
+				input_buf_y_st_addr = IMGU_VMEM1_INT_BUF_ADDR;
+				input_buf_u_st_addr = IMGU_VMEM1_INT_BUF_ADDR +
+							IMGU_VMEM1_U_OFFSET;
+				input_buf_v_st_addr = IMGU_VMEM1_INT_BUF_ADDR +
+							IMGU_VMEM1_V_OFFSET;
+			} else {
+				/*
+				 * The input bufferss are the buffers
+				 * filled by the SP
+				 */
+				input_buf_y_st_addr = IMGU_VMEM1_INP_BUF_ADDR;
+				input_buf_u_st_addr = IMGU_VMEM1_INP_BUF_ADDR +
+							IMGU_VMEM1_U_OFFSET;
+				input_buf_v_st_addr = IMGU_VMEM1_INP_BUF_ADDR +
+							IMGU_VMEM1_V_OFFSET;
+			}
+
+			/*
+			 * The formatter input width must be rounded to
+			 * the block width. Otherwise the formatter will
+			 * not recognize the end of the line, resulting
+			 * in incorrect tiling (system may hang!) and
+			 * possibly other problems.
+			 */
+			input_width_s =
+				roundup(stripe_params[s].output_width[pin],
+					block_width);
+			input_width_vecs = input_width_s /
+					IMGU_VMEM1_ELEMS_PER_VEC;
+			out_fifo_addr = IMGU_FIFO_ADDR_FMT_TO_SP;
+			/*
+			 * Process-output tokens must be sent to the SP.
+			 * When scaling, the release-input tokens can be
+			 * sent directly to the scaler, otherwise the
+			 * formatter should send them to the SP.
+			 */
+			if (frame_params[pin].scaled)
+				in_fifo_addr = IMGU_FIFO_ADDR_FMT_TO_SCALER;
+			else
+				in_fifo_addr = IMGU_FIFO_ADDR_FMT_TO_SP;
+
+			/* Formatter */
+			param = &osys->formatter[s][pin].param;
+
+			param->format = frame_params[pin].format;
+			param->flip = frame_params[pin].flip;
+			param->mirror = frame_params[pin].mirror;
+			param->tiling = frame_params[pin].tiling;
+			param->reduce_range = frame_params[pin].reduce_range;
+			param->alpha_blending = 0;
+			param->release_inp_addr = in_fifo_addr;
+			param->release_inp_en = 1;
+			param->process_out_buf_addr = out_fifo_addr;
+			param->image_width_vecs = input_width_vecs;
+			param->image_height_lines =
+				stripe_params[s].output_height[pin];
+			param->inp_buff_y_st_addr = input_buf_y_st_addr;
+			param->inp_buff_y_line_stride = input_buf_y_line_stride;
+			param->inp_buff_y_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+			param->int_buff_u_st_addr = input_buf_u_st_addr;
+			param->int_buff_v_st_addr = input_buf_v_st_addr;
+			param->inp_buff_uv_line_stride =
+				input_buf_uv_line_stride;
+			param->inp_buff_uv_buffer_stride = IMGU_VMEM1_BUF_SIZE;
+			param->out_buff_level = 0;
+			param->out_buff_nr_y_lines = output_buf_nr_y_lines;
+			param->out_buff_u_st_offset = IMGU_VMEM1_U_OFFSET;
+			param->out_buff_v_st_offset = IMGU_VMEM1_V_OFFSET;
+			param->out_buff_y_line_stride =
+				output_buf_y_line_stride;
+			param->out_buff_uv_line_stride =
+				output_buf_uv_line_stride;
+			param->hist_buff_st_addr = IMGU_VMEM1_HST_BUF_ADDR;
+			param->hist_buff_line_stride =
+				IMGU_VMEM1_HST_BUF_STRIDE;
+			param->hist_buff_nr_lines = IMGU_VMEM1_HST_BUF_NLINES;
+		}
+	}
+
+	block_stripes[0].offset = 0;
+	if (stripes <= 1) {
+		block_stripes[0].width = stripe_params[0].input_width;
+		block_stripes[0].height = stripe_params[0].input_height;
+	} else {
+		struct imgu_fw_info *bi =
+			&css->fwp->binary_header[css_pipe->bindex];
+		unsigned int sp_block_width =
+				bi->info.isp.sp.block.block_width *
+				IPU3_UAPI_ISP_VEC_ELEMS;
+
+		block_stripes[0].width = roundup(stripe_params[0].input_width,
+						 sp_block_width);
+		block_stripes[1].offset =
+			rounddown(css_pipe->rect[IPU3_CSS_RECT_GDC].width -
+				  stripe_params[1].input_width, sp_block_width);
+		block_stripes[1].width =
+			roundup(css_pipe->rect[IPU3_CSS_RECT_GDC].width -
+				block_stripes[1].offset, sp_block_width);
+		block_stripes[0].height = css_pipe->rect[IPU3_CSS_RECT_GDC].height;
+		block_stripes[1].height = block_stripes[0].height;
+	}
+
+	return 0;
+}
+
+/*********************** Mostly 3A operations ******************************/
+
+/*
+ * This function creates a "TO-DO list" (operations) for the sp code.
+ *
+ * There are 2 types of operations:
+ * 1. Transfer: Issue DMA transfer request for copying grid cells from DDR to
+ *    accelerator space (NOTE that this space is limited) associated data:
+ *    DDR address + accelerator's config set index(acc's address).
+ *
+ * 2. Issue "Process Lines Command" to shd accelerator
+ *    associated data: #lines + which config set to use (actually, accelerator
+ *    will use x AND (x+1)%num_of_sets - NOTE that this implies the restriction
+ *    of not touching config sets x & (x+1)%num_of_sets when process_lines(x)
+ *    is active).
+ *
+ * Basically there are 2 types of operations "chunks":
+ * 1. "initial chunk": Initially, we do as much transfers as we can (and need)
+ *    [0 - max sets(3) ] followed by 1 or 2 "process lines" operations.
+ *
+ * 2. "regular chunk" - 1 transfer followed by 1 process line operation.
+ *    (in some cases we might need additional transfer ate the last chunk).
+ *
+ * for some case:
+ * --> init
+ *	tr (0)
+ *	tr (1)
+ *	tr (2)
+ *	pl (0)
+ *	pl (1)
+ * --> ack (0)
+ *	tr (3)
+ *	pl (2)
+ * --> ack (1)
+ *	pl (3)
+ * --> ack (2)
+ *	do nothing
+ * --> ack (3)
+ *	do nothing
+ */
+
+static int
+imgu_css_shd_ops_calc(struct imgu_abi_shd_intra_frame_operations_data *ops,
+		      const struct ipu3_uapi_shd_grid_config *grid,
+		      unsigned int image_height)
+{
+	unsigned int block_height = 1 << grid->block_height_log2;
+	unsigned int grid_height_per_slice = grid->grid_height_per_slice;
+	unsigned int set_height = grid_height_per_slice * block_height;
+
+	/* We currently support only abs(y_start) > grid_height_per_slice */
+	unsigned int positive_y_start = (unsigned int)-grid->y_start;
+	unsigned int first_process_lines =
+				set_height - (positive_y_start % set_height);
+	unsigned int last_set_height;
+	unsigned int num_of_sets;
+
+	struct imgu_abi_acc_operation *p_op;
+	struct imgu_abi_acc_process_lines_cmd_data *p_pl;
+	struct imgu_abi_shd_transfer_luts_set_data *p_tr;
+
+	unsigned int op_idx, pl_idx, tr_idx;
+	unsigned char tr_set_num, pl_cfg_set;
+
+	/*
+	 * When the number of lines for the last process lines command
+	 * is equal to a set height, we need another line of grid cell -
+	 * additional transfer is required.
+	 */
+	unsigned char last_tr = 0;
+
+	/* Add "process lines" command to the list of operations */
+	bool add_pl;
+	/* Add DMA xfer (config set) command to the list of ops */
+	bool add_tr;
+
+	/*
+	 * Available partial grid (the part that fits into #IMGU_SHD_SETS sets)
+	 * doesn't cover whole frame - need to process in chunks
+	 */
+	if (image_height > first_process_lines) {
+		last_set_height =
+			(image_height - first_process_lines) % set_height;
+		num_of_sets = last_set_height > 0 ?
+			(image_height - first_process_lines) / set_height + 2 :
+			(image_height - first_process_lines) / set_height + 1;
+		last_tr = (set_height - last_set_height <= block_height ||
+			   last_set_height == 0) ? 1 : 0;
+	} else { /* partial grid covers whole frame */
+		last_set_height = 0;
+		num_of_sets = 1;
+		first_process_lines = image_height;
+		last_tr = set_height - image_height <= block_height ? 1 : 0;
+	}
+
+	/* Init operations lists and counters */
+	p_op = ops->operation_list;
+	op_idx = 0;
+	p_pl = ops->process_lines_data;
+	pl_idx = 0;
+	p_tr = ops->transfer_data;
+	tr_idx = 0;
+
+	memset(ops, 0, sizeof(*ops));
+
+	/* Cyclic counters that holds config set number [0,IMGU_SHD_SETS) */
+	tr_set_num = 0;
+	pl_cfg_set = 0;
+
+	/*
+	 * Always start with a transfer - process lines command must be
+	 * initiated only after appropriate config sets are in place
+	 * (2 configuration sets per process line command, except for last one).
+	 */
+	add_pl = false;
+	add_tr = true;
+
+	while (add_pl || add_tr) {
+		/* Transfer ops */
+		if (add_tr) {
+			if (op_idx >= IMGU_ABI_SHD_MAX_OPERATIONS ||
+			    tr_idx >= IMGU_ABI_SHD_MAX_TRANSFERS)
+				return -EINVAL;
+			p_op[op_idx].op_type =
+				IMGU_ABI_ACC_OPTYPE_TRANSFER_DATA;
+			p_op[op_idx].op_indicator = IMGU_ABI_ACC_OP_IDLE;
+			op_idx++;
+			p_tr[tr_idx].set_number = tr_set_num;
+			tr_idx++;
+			tr_set_num = (tr_set_num + 1) % IMGU_SHD_SETS;
+		}
+
+		/* Process-lines ops */
+		if (add_pl) {
+			if (op_idx >= IMGU_ABI_SHD_MAX_OPERATIONS ||
+			    pl_idx >= IMGU_ABI_SHD_MAX_PROCESS_LINES)
+				return -EINVAL;
+			p_op[op_idx].op_type =
+				IMGU_ABI_ACC_OPTYPE_PROCESS_LINES;
+
+			/*
+			 * In case we have 2 process lines commands -
+			 * don't stop after the first one
+			 */
+			if (pl_idx == 0 && num_of_sets != 1)
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_IDLE;
+			/*
+			 * Initiate last process lines command -
+			 * end of operation list.
+			 */
+			else if (pl_idx == num_of_sets - 1)
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_END_OF_OPS;
+			/*
+			 * Intermediate process line command - end of operation
+			 * "chunk" (meaning few "transfers" followed by few
+			 * "process lines" commands).
+			 */
+			else
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_END_OF_ACK;
+
+			op_idx++;
+
+			/* first process line operation */
+			if (pl_idx == 0)
+				p_pl[pl_idx].lines = first_process_lines;
+			/* Last process line operation */
+			else if (pl_idx == num_of_sets - 1 &&
+				 last_set_height > 0)
+				p_pl[pl_idx].lines = last_set_height;
+			else	/* "regular" process lines operation */
+				p_pl[pl_idx].lines = set_height;
+
+			p_pl[pl_idx].cfg_set = pl_cfg_set;
+			pl_idx++;
+			pl_cfg_set = (pl_cfg_set + 1) % IMGU_SHD_SETS;
+		}
+
+		/*
+		 * Initially, we always transfer
+		 * min(IMGU_SHD_SETS, num_of_sets) - after that we fill in the
+		 * corresponding process lines commands.
+		 */
+		if (tr_idx == IMGU_SHD_SETS ||
+		    tr_idx == num_of_sets + last_tr) {
+			add_tr = false;
+			add_pl = true;
+		}
+
+		/*
+		 * We have finished the "initial" operations chunk -
+		 * be ready to get more chunks.
+		 */
+		if (pl_idx == 2) {
+			add_tr = true;
+			add_pl = true;
+		}
+
+		/* Stop conditions for each operation type */
+		if (tr_idx == num_of_sets + last_tr)
+			add_tr = false;
+		if (pl_idx == num_of_sets)
+			add_pl = false;
+	}
+
+	return 0;
+}
+
+/*
+ * The follow handshake procotol is the same for AF, AWB and AWB FR.
+ *
+ * for n sets of meta-data, the flow is:
+ * --> init
+ *  process-lines  (0)
+ *  process-lines  (1)	 eoc
+ *  --> ack (0)
+ *  read-meta-data (0)
+ *  process-lines  (2)	 eoc
+ *  --> ack (1)
+ *  read-meta-data (1)
+ *  process-lines  (3)	 eoc
+ *  ...
+ *
+ *  --> ack (n-3)
+ *  read-meta-data (n-3)
+ *  process-lines  (n-1) eoc
+ *  --> ack (n-2)
+ *  read-meta-data (n-2) eoc
+ *  --> ack (n-1)
+ *  read-meta-data (n-1) eof
+ *
+ * for 2 sets we get:
+ * --> init
+ * pl (0)
+ * pl (1) eoc
+ * --> ack (0)
+ * pl (2) - rest of image, if applicable)
+ * rmd (0) eoc
+ * --> ack (1)
+ * rmd (1) eof
+ * --> (ack (2))
+ * do nothing
+ *
+ * for only one set:
+ *
+ * --> init
+ * pl(0)   eoc
+ * --> ack (0)
+ * rmd (0) eof
+ *
+ * grid smaller than image case
+ * for example 128x128 grid (block size 8x8, 16x16 num of blocks)
+ * start at (0,0)
+ * 1st set holds 160 cells - 10 blocks vertical, 16 horizontal
+ * => 1st process lines = 80
+ * we're left with 128-80=48 lines (6 blocks vertical)
+ * => 2nd process lines = 48
+ * last process lines to cover the image - image_height - 128
+ *
+ * --> init
+ * pl (0) first
+ * pl (1) last-in-grid
+ * --> ack (0)
+ * rmd (0)
+ * pl (2) after-grid
+ * --> ack (1)
+ * rmd (1) eof
+ * --> ack (2)
+ * do nothing
+ */
+struct process_lines {
+	unsigned int image_height;
+	unsigned short grid_height;
+	unsigned short block_height;
+	unsigned short y_start;
+	unsigned char grid_height_per_slice;
+
+	unsigned short max_op; /* max operation */
+	unsigned short max_tr; /* max transaction */
+	unsigned char acc_enable;
+};
+
+/* Helper to config intra_frame_operations_data. */
+static int
+imgu_css_acc_process_lines(const struct process_lines *pl,
+			   struct imgu_abi_acc_operation *p_op,
+			   struct imgu_abi_acc_process_lines_cmd_data *p_pl,
+			   struct imgu_abi_acc_transfer_op_data *p_tr)
+{
+	unsigned short op_idx = 0, pl_idx = 0, tr_idx = 0;
+	unsigned char tr_set_num = 0, pl_cfg_set = 0;
+	const unsigned short grid_last_line =
+			pl->y_start + pl->grid_height * pl->block_height;
+	const unsigned short process_lines =
+			pl->grid_height_per_slice * pl->block_height;
+
+	unsigned int process_lines_after_grid;
+	unsigned short first_process_lines;
+	unsigned short last_process_lines_in_grid;
+
+	unsigned short num_of_process_lines;
+	unsigned short num_of_sets;
+
+	if (pl->grid_height_per_slice == 0)
+		return -EINVAL;
+
+	if (pl->acc_enable && grid_last_line > pl->image_height)
+		return -EINVAL;
+
+	num_of_sets = pl->grid_height / pl->grid_height_per_slice;
+	if (num_of_sets * pl->grid_height_per_slice < pl->grid_height)
+		num_of_sets++;
+
+	/* Account for two line delay inside the FF */
+	if (pl->max_op == IMGU_ABI_AF_MAX_OPERATIONS) {
+		first_process_lines = process_lines + pl->y_start + 2;
+		last_process_lines_in_grid =
+			(grid_last_line - first_process_lines) -
+			((num_of_sets - 2) * process_lines) + 4;
+		process_lines_after_grid =
+			pl->image_height - grid_last_line - 4;
+	} else {
+		first_process_lines = process_lines + pl->y_start;
+		last_process_lines_in_grid =
+			(grid_last_line - first_process_lines) -
+			((num_of_sets - 2) * process_lines);
+		process_lines_after_grid = pl->image_height - grid_last_line;
+	}
+
+	num_of_process_lines = num_of_sets;
+	if (process_lines_after_grid > 0)
+		num_of_process_lines++;
+
+	while (tr_idx < num_of_sets || pl_idx < num_of_process_lines) {
+		/* Read meta-data */
+		if (pl_idx >= 2 || (pl_idx == 1 && num_of_sets == 1)) {
+			if (op_idx >= pl->max_op || tr_idx >= pl->max_tr)
+				return -EINVAL;
+
+			p_op[op_idx].op_type =
+				IMGU_ABI_ACC_OPTYPE_TRANSFER_DATA;
+
+			if (tr_idx == num_of_sets - 1)
+				/* The last operation is always a tr */
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_END_OF_OPS;
+			else if (tr_idx == num_of_sets - 2)
+				if (process_lines_after_grid == 0)
+					/*
+					 * No additional pl op left -
+					 * this op is left as lats of cycle
+					 */
+					p_op[op_idx].op_indicator =
+						IMGU_ABI_ACC_OP_END_OF_ACK;
+				else
+					/*
+					 * We still have to process-lines after
+					 * the grid so have one more pl op
+					 */
+					p_op[op_idx].op_indicator =
+						IMGU_ABI_ACC_OP_IDLE;
+			else
+				/* Default - usually there's a pl after a tr */
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_IDLE;
+
+			op_idx++;
+			if (p_tr) {
+				p_tr[tr_idx].set_number = tr_set_num;
+				tr_set_num = 1 - tr_set_num;
+			}
+			tr_idx++;
+		}
+
+		/* process_lines */
+		if (pl_idx < num_of_process_lines) {
+			if (op_idx >= pl->max_op || pl_idx >= pl->max_tr)
+				return -EINVAL;
+
+			p_op[op_idx].op_type =
+				IMGU_ABI_ACC_OPTYPE_PROCESS_LINES;
+			if (pl_idx == 0)
+				if (num_of_process_lines == 1)
+					/* Only one pl op */
+					p_op[op_idx].op_indicator =
+						IMGU_ABI_ACC_OP_END_OF_ACK;
+				else
+					/* On init - do two pl ops */
+					p_op[op_idx].op_indicator =
+						IMGU_ABI_ACC_OP_IDLE;
+			else
+				/* Usually pl is the end of the ack cycle */
+				p_op[op_idx].op_indicator =
+					IMGU_ABI_ACC_OP_END_OF_ACK;
+
+			op_idx++;
+
+			if (pl_idx == 0)
+				/* First process line */
+				p_pl[pl_idx].lines = first_process_lines;
+			else if (pl_idx == num_of_sets - 1)
+				/* Last in grid */
+				p_pl[pl_idx].lines = last_process_lines_in_grid;
+			else if (pl_idx == num_of_process_lines - 1)
+				/* After the grid */
+				p_pl[pl_idx].lines = process_lines_after_grid;
+			else
+				/* Inside the grid */
+				p_pl[pl_idx].lines = process_lines;
+
+			if (p_tr) {
+				p_pl[pl_idx].cfg_set = pl_cfg_set;
+				pl_cfg_set = 1 - pl_cfg_set;
+			}
+			pl_idx++;
+		}
+	}
+
+	return 0;
+}
+
+static int imgu_css_af_ops_calc(struct imgu_css *css, unsigned int pipe,
+				struct imgu_abi_af_config *af_config)
+{
+	struct imgu_abi_af_intra_frame_operations_data *to =
+		&af_config->operations_data;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+
+	struct process_lines pl = {
+		.image_height = css_pipe->rect[IPU3_CSS_RECT_BDS].height,
+		.grid_height = af_config->config.grid_cfg.height,
+		.block_height =
+			1 << af_config->config.grid_cfg.block_height_log2,
+		.y_start = af_config->config.grid_cfg.y_start &
+			IPU3_UAPI_GRID_START_MASK,
+		.grid_height_per_slice =
+			af_config->stripes[0].grid_cfg.height_per_slice,
+		.max_op = IMGU_ABI_AF_MAX_OPERATIONS,
+		.max_tr = IMGU_ABI_AF_MAX_TRANSFERS,
+		.acc_enable = bi->info.isp.sp.enable.af,
+	};
+
+	return imgu_css_acc_process_lines(&pl, to->ops, to->process_lines_data,
+					  NULL);
+}
+
+static int
+imgu_css_awb_fr_ops_calc(struct imgu_css *css, unsigned int pipe,
+			 struct imgu_abi_awb_fr_config *awb_fr_config)
+{
+	struct imgu_abi_awb_fr_intra_frame_operations_data *to =
+		&awb_fr_config->operations_data;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+	struct process_lines pl = {
+		.image_height = css_pipe->rect[IPU3_CSS_RECT_BDS].height,
+		.grid_height = awb_fr_config->config.grid_cfg.height,
+		.block_height =
+			1 << awb_fr_config->config.grid_cfg.block_height_log2,
+		.y_start = awb_fr_config->config.grid_cfg.y_start &
+			IPU3_UAPI_GRID_START_MASK,
+		.grid_height_per_slice =
+			awb_fr_config->stripes[0].grid_cfg.height_per_slice,
+		.max_op = IMGU_ABI_AWB_FR_MAX_OPERATIONS,
+		.max_tr = IMGU_ABI_AWB_FR_MAX_PROCESS_LINES,
+		.acc_enable = bi->info.isp.sp.enable.awb_fr_acc,
+	};
+
+	return imgu_css_acc_process_lines(&pl, to->ops, to->process_lines_data,
+					  NULL);
+}
+
+static int imgu_css_awb_ops_calc(struct imgu_css *css, unsigned int pipe,
+				 struct imgu_abi_awb_config *awb_config)
+{
+	struct imgu_abi_awb_intra_frame_operations_data *to =
+		&awb_config->operations_data;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+
+	struct process_lines pl = {
+		.image_height = css_pipe->rect[IPU3_CSS_RECT_BDS].height,
+		.grid_height = awb_config->config.grid.height,
+		.block_height =
+			1 << awb_config->config.grid.block_height_log2,
+		.y_start = awb_config->config.grid.y_start,
+		.grid_height_per_slice =
+			awb_config->stripes[0].grid.height_per_slice,
+		.max_op = IMGU_ABI_AWB_MAX_OPERATIONS,
+		.max_tr = IMGU_ABI_AWB_MAX_TRANSFERS,
+		.acc_enable = bi->info.isp.sp.enable.awb_acc,
+	};
+
+	return imgu_css_acc_process_lines(&pl, to->ops, to->process_lines_data,
+					  to->transfer_data);
+}
+
+static u16 imgu_css_grid_end(u16 start, u8 width, u8 block_width_log2)
+{
+	return (start & IPU3_UAPI_GRID_START_MASK) +
+		(width << block_width_log2) - 1;
+}
+
+static void imgu_css_grid_end_calc(struct ipu3_uapi_grid_config *grid_cfg)
+{
+	grid_cfg->x_end = imgu_css_grid_end(grid_cfg->x_start, grid_cfg->width,
+					    grid_cfg->block_width_log2);
+	grid_cfg->y_end = imgu_css_grid_end(grid_cfg->y_start, grid_cfg->height,
+					    grid_cfg->block_height_log2);
+}
+
+/****************** config computation *****************************/
+
+static int imgu_css_cfg_acc_stripe(struct imgu_css *css, unsigned int pipe,
+				   struct imgu_abi_acc_param *acc)
+{
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	const struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+	struct imgu_css_scaler_info scaler_luma, scaler_chroma;
+	const unsigned int stripes = bi->info.isp.sp.iterator.num_stripes;
+	const unsigned int f = IPU3_UAPI_ISP_VEC_ELEMS * 2;
+	unsigned int bds_ds, i;
+
+	memset(acc, 0, sizeof(*acc));
+
+	/* acc_param: osys_config */
+
+	if (imgu_css_osys_calc(css, pipe, stripes, &acc->osys, &scaler_luma,
+			       &scaler_chroma, acc->stripe.block_stripes))
+		return -EINVAL;
+
+	/* acc_param: stripe data */
+
+	/*
+	 * For the striped case the approach is as follows:
+	 * 1. down-scaled stripes are calculated - with 128 overlap
+	 *    (this is the main limiter therefore it's first)
+	 * 2. input stripes are derived by up-scaling the down-scaled stripes
+	 *    (there are no alignment requirements on input stripes)
+	 * 3. output stripes are derived from down-scaled stripes too
+	 */
+
+	acc->stripe.num_of_stripes = stripes;
+	acc->stripe.input_frame.width =
+		css_pipe->queue[IPU3_CSS_QUEUE_IN].fmt.mpix.width;
+	acc->stripe.input_frame.height =
+		css_pipe->queue[IPU3_CSS_QUEUE_IN].fmt.mpix.height;
+	acc->stripe.input_frame.bayer_order =
+		css_pipe->queue[IPU3_CSS_QUEUE_IN].css_fmt->bayer_order;
+
+	for (i = 0; i < stripes; i++)
+		acc->stripe.bds_out_stripes[i].height =
+					css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	acc->stripe.bds_out_stripes[0].offset = 0;
+	if (stripes <= 1) {
+		acc->stripe.bds_out_stripes[0].width =
+			ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, f);
+	} else {
+		/* Image processing is divided into two stripes */
+		acc->stripe.bds_out_stripes[0].width =
+			acc->stripe.bds_out_stripes[1].width =
+			(css_pipe->rect[IPU3_CSS_RECT_BDS].width / 2 & ~(f - 1)) + f;
+		/*
+		 * Sum of width of the two stripes should not be smaller
+		 * than output width and must be even times of overlapping
+		 * unit f.
+		 */
+		if ((css_pipe->rect[IPU3_CSS_RECT_BDS].width / f & 1) !=
+		    !!(css_pipe->rect[IPU3_CSS_RECT_BDS].width & (f - 1)))
+			acc->stripe.bds_out_stripes[0].width += f;
+		if ((css_pipe->rect[IPU3_CSS_RECT_BDS].width / f & 1) &&
+		    (css_pipe->rect[IPU3_CSS_RECT_BDS].width & (f - 1))) {
+			acc->stripe.bds_out_stripes[0].width += f;
+			acc->stripe.bds_out_stripes[1].width += f;
+		}
+		/* Overlap between stripes is IPU3_UAPI_ISP_VEC_ELEMS * 4 */
+		acc->stripe.bds_out_stripes[1].offset =
+			acc->stripe.bds_out_stripes[0].width - 2 * f;
+	}
+
+	acc->stripe.effective_stripes[0].height =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height;
+	acc->stripe.effective_stripes[0].offset = 0;
+	acc->stripe.bds_out_stripes_no_overlap[0].height =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	acc->stripe.bds_out_stripes_no_overlap[0].offset = 0;
+	acc->stripe.output_stripes[0].height =
+				css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.height;
+	acc->stripe.output_stripes[0].offset = 0;
+	if (stripes <= 1) {
+		acc->stripe.down_scaled_stripes[0].width =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+		acc->stripe.down_scaled_stripes[0].height =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+		acc->stripe.down_scaled_stripes[0].offset = 0;
+
+		acc->stripe.effective_stripes[0].width =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].width;
+		acc->stripe.bds_out_stripes_no_overlap[0].width =
+			ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, f);
+
+		acc->stripe.output_stripes[0].width =
+			css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.width;
+	} else { /* Two stripes */
+		bds_ds = css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].width *
+				IMGU_BDS_GRANULARITY /
+				css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+
+		acc->stripe.down_scaled_stripes[0] =
+			acc->stripe.bds_out_stripes[0];
+		acc->stripe.down_scaled_stripes[1] =
+			acc->stripe.bds_out_stripes[1];
+		if (!IS_ALIGNED(css_pipe->rect[IPU3_CSS_RECT_BDS].width, f))
+			acc->stripe.down_scaled_stripes[1].width +=
+				(css_pipe->rect[IPU3_CSS_RECT_BDS].width
+				& (f - 1)) - f;
+
+		acc->stripe.effective_stripes[0].width = bds_ds *
+			acc->stripe.down_scaled_stripes[0].width /
+			IMGU_BDS_GRANULARITY;
+		acc->stripe.effective_stripes[1].width = bds_ds *
+			acc->stripe.down_scaled_stripes[1].width /
+			IMGU_BDS_GRANULARITY;
+		acc->stripe.effective_stripes[1].height =
+			css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height;
+		acc->stripe.effective_stripes[1].offset = bds_ds *
+			acc->stripe.down_scaled_stripes[1].offset /
+			IMGU_BDS_GRANULARITY;
+
+		acc->stripe.bds_out_stripes_no_overlap[0].width =
+		acc->stripe.bds_out_stripes_no_overlap[1].offset =
+			ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, 2 * f) / 2;
+		acc->stripe.bds_out_stripes_no_overlap[1].width =
+			DIV_ROUND_UP(css_pipe->rect[IPU3_CSS_RECT_BDS].width, f)
+			/ 2 * f;
+		acc->stripe.bds_out_stripes_no_overlap[1].height =
+			css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+
+		acc->stripe.output_stripes[0].width =
+			acc->stripe.down_scaled_stripes[0].width - f;
+		acc->stripe.output_stripes[1].width =
+			acc->stripe.down_scaled_stripes[1].width - f;
+		acc->stripe.output_stripes[1].height =
+			css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.height;
+		acc->stripe.output_stripes[1].offset =
+			acc->stripe.output_stripes[0].width;
+	}
+
+	acc->stripe.output_system_in_frame_width =
+		css_pipe->rect[IPU3_CSS_RECT_GDC].width;
+	acc->stripe.output_system_in_frame_height =
+		css_pipe->rect[IPU3_CSS_RECT_GDC].height;
+
+	acc->stripe.effective_frame_width =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].width;
+	acc->stripe.bds_frame_width = css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	acc->stripe.out_frame_width =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.width;
+	acc->stripe.out_frame_height =
+		css_pipe->queue[IPU3_CSS_QUEUE_OUT].fmt.mpix.height;
+	acc->stripe.gdc_in_buffer_width =
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperline /
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperpixel;
+	acc->stripe.gdc_in_buffer_height =
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].height;
+	acc->stripe.gdc_in_buffer_offset_x = IMGU_GDC_BUF_X;
+	acc->stripe.gdc_in_buffer_offset_y = IMGU_GDC_BUF_Y;
+	acc->stripe.display_frame_width =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.width;
+	acc->stripe.display_frame_height =
+		css_pipe->queue[IPU3_CSS_QUEUE_VF].fmt.mpix.height;
+	acc->stripe.bds_aligned_frame_width =
+		roundup(css_pipe->rect[IPU3_CSS_RECT_BDS].width,
+			2 * IPU3_UAPI_ISP_VEC_ELEMS);
+
+	if (stripes > 1)
+		acc->stripe.half_overlap_vectors =
+			IMGU_STRIPE_FIXED_HALF_OVERLAP;
+	else
+		acc->stripe.half_overlap_vectors = 0;
+
+	return 0;
+}
+
+static void imgu_css_cfg_acc_dvs(struct imgu_css *css,
+				 struct imgu_abi_acc_param *acc,
+				 unsigned int pipe)
+{
+	unsigned int i;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	/* Disable DVS statistics */
+	acc->dvs_stat.operations_data.process_lines_data[0].lines =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	acc->dvs_stat.operations_data.process_lines_data[0].cfg_set = 0;
+	acc->dvs_stat.operations_data.ops[0].op_type =
+		IMGU_ABI_ACC_OPTYPE_PROCESS_LINES;
+	acc->dvs_stat.operations_data.ops[0].op_indicator =
+		IMGU_ABI_ACC_OP_NO_OPS;
+	for (i = 0; i < IMGU_ABI_DVS_STAT_LEVELS; i++)
+		acc->dvs_stat.cfg.grd_config[i].enable = 0;
+}
+
+static void acc_bds_per_stripe_data(struct imgu_css *css,
+				    struct imgu_abi_acc_param *acc,
+				    const int i, unsigned int pipe)
+{
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	acc->bds.per_stripe.aligned_data[i].data.crop.hor_crop_en = 0;
+	acc->bds.per_stripe.aligned_data[i].data.crop.hor_crop_start = 0;
+	acc->bds.per_stripe.aligned_data[i].data.crop.hor_crop_end = 0;
+	acc->bds.per_stripe.aligned_data[i].data.hor_ctrl0 =
+		acc->bds.hor.hor_ctrl0;
+	acc->bds.per_stripe.aligned_data[i].data.hor_ctrl0.out_frame_width =
+		acc->stripe.down_scaled_stripes[i].width;
+	acc->bds.per_stripe.aligned_data[i].data.ver_ctrl1.out_frame_width =
+		acc->stripe.down_scaled_stripes[i].width;
+	acc->bds.per_stripe.aligned_data[i].data.ver_ctrl1.out_frame_height =
+		css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+}
+
+/*
+ * Configure `acc' parameters. `acc_old' contains the old values (or is NULL)
+ * and `acc_user' contains new prospective values. `use' contains flags
+ * telling which fields to take from the old values (or generate if it is NULL)
+ * and which to take from the new user values.
+ */
+int imgu_css_cfg_acc(struct imgu_css *css, unsigned int pipe,
+		     struct ipu3_uapi_flags *use,
+		     struct imgu_abi_acc_param *acc,
+		     struct imgu_abi_acc_param *acc_old,
+		     struct ipu3_uapi_acc_param *acc_user)
+{
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	const struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+	const unsigned int stripes = bi->info.isp.sp.iterator.num_stripes;
+	const unsigned int tnr_frame_width =
+		acc->stripe.bds_aligned_frame_width;
+	const unsigned int min_overlap = 10;
+	const struct v4l2_pix_format_mplane *pixm =
+		&css_pipe->queue[IPU3_CSS_QUEUE_IN].fmt.mpix;
+	const struct imgu_css_bds_config *cfg_bds;
+	struct imgu_abi_input_feeder_data *feeder_data;
+
+	unsigned int bds_ds, ofs_x, ofs_y, i, width, height;
+	u8 b_w_log2; /* Block width log2 */
+
+	/* Update stripe using chroma and luma */
+
+	if (imgu_css_cfg_acc_stripe(css, pipe, acc))
+		return -EINVAL;
+
+	/* acc_param: input_feeder_config */
+
+	ofs_x = ((pixm->width -
+		  css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].width) >> 1) & ~1;
+	ofs_x += css_pipe->queue[IPU3_CSS_QUEUE_IN].css_fmt->bayer_order ==
+		IMGU_ABI_BAYER_ORDER_RGGB ||
+		css_pipe->queue[IPU3_CSS_QUEUE_IN].css_fmt->bayer_order ==
+		IMGU_ABI_BAYER_ORDER_GBRG ? 1 : 0;
+	ofs_y = ((pixm->height -
+		  css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height) >> 1) & ~1;
+	ofs_y += css_pipe->queue[IPU3_CSS_QUEUE_IN].css_fmt->bayer_order ==
+		IMGU_ABI_BAYER_ORDER_BGGR ||
+		css_pipe->queue[IPU3_CSS_QUEUE_IN].css_fmt->bayer_order ==
+		IMGU_ABI_BAYER_ORDER_GBRG ? 1 : 0;
+	acc->input_feeder.data.row_stride = pixm->plane_fmt[0].bytesperline;
+	acc->input_feeder.data.start_row_address =
+		ofs_x / IMGU_PIXELS_PER_WORD * IMGU_BYTES_PER_WORD +
+		ofs_y * acc->input_feeder.data.row_stride;
+	acc->input_feeder.data.start_pixel = ofs_x % IMGU_PIXELS_PER_WORD;
+
+	acc->input_feeder.data_per_stripe.input_feeder_data[0].data =
+		acc->input_feeder.data;
+
+	ofs_x += acc->stripe.effective_stripes[1].offset;
+
+	feeder_data =
+		&acc->input_feeder.data_per_stripe.input_feeder_data[1].data;
+	feeder_data->row_stride = acc->input_feeder.data.row_stride;
+	feeder_data->start_row_address =
+		ofs_x / IMGU_PIXELS_PER_WORD * IMGU_BYTES_PER_WORD +
+		ofs_y * acc->input_feeder.data.row_stride;
+	feeder_data->start_pixel = ofs_x % IMGU_PIXELS_PER_WORD;
+
+	/* acc_param: bnr_static_config */
+
+	/*
+	 * Originate from user or be the original default values if user has
+	 * never set them before, when user gives a new set of parameters,
+	 * for each chunk in the parameter structure there is a flag use->xxx
+	 * whether to use the user-provided parameter or not. If not, the
+	 * parameter remains unchanged in the driver:
+	 * it's value is taken from acc_old.
+	 */
+	if (use && use->acc_bnr) {
+		/* Take values from user */
+		acc->bnr = acc_user->bnr;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->bnr = acc_old->bnr;
+	} else {
+		/* Calculate from scratch */
+		acc->bnr = imgu_css_bnr_defaults;
+	}
+
+	acc->bnr.column_size = tnr_frame_width;
+
+	/* acc_param: bnr_static_config_green_disparity */
+
+	if (use && use->acc_green_disparity) {
+		/* Take values from user */
+		acc->green_disparity = acc_user->green_disparity;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->green_disparity = acc_old->green_disparity;
+	} else {
+		/* Calculate from scratch */
+		memset(&acc->green_disparity, 0, sizeof(acc->green_disparity));
+	}
+
+	/* acc_param: dm_config */
+
+	if (use && use->acc_dm) {
+		/* Take values from user */
+		acc->dm = acc_user->dm;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->dm = acc_old->dm;
+	} else {
+		/* Calculate from scratch */
+		acc->dm = imgu_css_dm_defaults;
+	}
+
+	acc->dm.frame_width = tnr_frame_width;
+
+	/* acc_param: ccm_mat_config */
+
+	if (use && use->acc_ccm) {
+		/* Take values from user */
+		acc->ccm = acc_user->ccm;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->ccm = acc_old->ccm;
+	} else {
+		/* Calculate from scratch */
+		acc->ccm = imgu_css_ccm_defaults;
+	}
+
+	/* acc_param: gamma_config */
+
+	if (use && use->acc_gamma) {
+		/* Take values from user */
+		acc->gamma = acc_user->gamma;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->gamma = acc_old->gamma;
+	} else {
+		/* Calculate from scratch */
+		acc->gamma.gc_ctrl.enable = 1;
+		acc->gamma.gc_lut = imgu_css_gamma_lut;
+	}
+
+	/* acc_param: csc_mat_config */
+
+	if (use && use->acc_csc) {
+		/* Take values from user */
+		acc->csc = acc_user->csc;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->csc = acc_old->csc;
+	} else {
+		/* Calculate from scratch */
+		acc->csc = imgu_css_csc_defaults;
+	}
+
+	/* acc_param: cds_params */
+
+	if (use && use->acc_cds) {
+		/* Take values from user */
+		acc->cds = acc_user->cds;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->cds = acc_old->cds;
+	} else {
+		/* Calculate from scratch */
+		acc->cds = imgu_css_cds_defaults;
+	}
+
+	/* acc_param: shd_config */
+
+	if (use && use->acc_shd) {
+		/* Take values from user */
+		acc->shd.shd = acc_user->shd.shd;
+		acc->shd.shd_lut = acc_user->shd.shd_lut;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->shd.shd = acc_old->shd.shd;
+		acc->shd.shd_lut = acc_old->shd.shd_lut;
+	} else {
+		/* Calculate from scratch */
+		acc->shd.shd = imgu_css_shd_defaults;
+		memset(&acc->shd.shd_lut, 0, sizeof(acc->shd.shd_lut));
+	}
+
+	if (acc->shd.shd.grid.width <= 0)
+		return -EINVAL;
+
+	acc->shd.shd.grid.grid_height_per_slice =
+		IMGU_ABI_SHD_MAX_CELLS_PER_SET / acc->shd.shd.grid.width;
+
+	if (acc->shd.shd.grid.grid_height_per_slice <= 0)
+		return -EINVAL;
+
+	acc->shd.shd.general.init_set_vrt_offst_ul =
+				(-acc->shd.shd.grid.y_start >>
+				 acc->shd.shd.grid.block_height_log2) %
+				acc->shd.shd.grid.grid_height_per_slice;
+
+	if (imgu_css_shd_ops_calc(&acc->shd.shd_ops, &acc->shd.shd.grid,
+				  css_pipe->rect[IPU3_CSS_RECT_BDS].height))
+		return -EINVAL;
+
+	/* acc_param: dvs_stat_config */
+	imgu_css_cfg_acc_dvs(css, acc, pipe);
+
+	/* acc_param: yuvp1_iefd_config */
+
+	if (use && use->acc_iefd) {
+		/* Take values from user */
+		acc->iefd = acc_user->iefd;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->iefd = acc_old->iefd;
+	} else {
+		/* Calculate from scratch */
+		acc->iefd = imgu_css_iefd_defaults;
+	}
+
+	/* acc_param: yuvp1_yds_config yds_c0 */
+
+	if (use && use->acc_yds_c0) {
+		/* Take values from user */
+		acc->yds_c0 = acc_user->yds_c0;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->yds_c0 = acc_old->yds_c0;
+	} else {
+		/* Calculate from scratch */
+		acc->yds_c0 = imgu_css_yds_defaults;
+	}
+
+	/* acc_param: yuvp1_chnr_config chnr_c0 */
+
+	if (use && use->acc_chnr_c0) {
+		/* Take values from user */
+		acc->chnr_c0 = acc_user->chnr_c0;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->chnr_c0 = acc_old->chnr_c0;
+	} else {
+		/* Calculate from scratch */
+		acc->chnr_c0 = imgu_css_chnr_defaults;
+	}
+
+	/* acc_param: yuvp1_y_ee_nr_config */
+
+	if (use && use->acc_y_ee_nr) {
+		/* Take values from user */
+		acc->y_ee_nr = acc_user->y_ee_nr;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->y_ee_nr = acc_old->y_ee_nr;
+	} else {
+		/* Calculate from scratch */
+		acc->y_ee_nr = imgu_css_y_ee_nr_defaults;
+	}
+
+	/* acc_param: yuvp1_yds_config yds */
+
+	if (use && use->acc_yds) {
+		/* Take values from user */
+		acc->yds = acc_user->yds;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->yds = acc_old->yds;
+	} else {
+		/* Calculate from scratch */
+		acc->yds = imgu_css_yds_defaults;
+	}
+
+	/* acc_param: yuvp1_chnr_config chnr */
+
+	if (use && use->acc_chnr) {
+		/* Take values from user */
+		acc->chnr = acc_user->chnr;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->chnr = acc_old->chnr;
+	} else {
+		/* Calculate from scratch */
+		acc->chnr = imgu_css_chnr_defaults;
+	}
+
+	/* acc_param: yuvp2_y_tm_lut_static_config */
+
+	for (i = 0; i < IMGU_ABI_YUVP2_YTM_LUT_ENTRIES; i++)
+		acc->ytm.entries[i] = i * 32;
+	acc->ytm.enable = 0;	/* Always disabled on IPU3 */
+
+	/* acc_param: yuvp1_yds_config yds2 */
+
+	if (use && use->acc_yds2) {
+		/* Take values from user */
+		acc->yds2 = acc_user->yds2;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->yds2 = acc_old->yds2;
+	} else {
+		/* Calculate from scratch */
+		acc->yds2 = imgu_css_yds_defaults;
+	}
+
+	/* acc_param: yuvp2_tcc_static_config */
+
+	if (use && use->acc_tcc) {
+		/* Take values from user */
+		acc->tcc = acc_user->tcc;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->tcc = acc_old->tcc;
+	} else {
+		/* Calculate from scratch */
+		memset(&acc->tcc, 0, sizeof(acc->tcc));
+
+		acc->tcc.gen_control.en = 1;
+		acc->tcc.gen_control.blend_shift = 3;
+		acc->tcc.gen_control.gain_according_to_y_only = 1;
+		acc->tcc.gen_control.gamma = 8;
+		acc->tcc.gen_control.delta = 0;
+
+		for (i = 0; i < IPU3_UAPI_YUVP2_TCC_MACC_TABLE_ELEMENTS; i++) {
+			acc->tcc.macc_table.entries[i].a = 1024;
+			acc->tcc.macc_table.entries[i].b = 0;
+			acc->tcc.macc_table.entries[i].c = 0;
+			acc->tcc.macc_table.entries[i].d = 1024;
+		}
+
+		acc->tcc.inv_y_lut.entries[6] = 1023;
+		for (i = 7; i < IPU3_UAPI_YUVP2_TCC_INV_Y_LUT_ELEMENTS; i++)
+			acc->tcc.inv_y_lut.entries[i] = 1024 >> (i - 6);
+
+		acc->tcc.gain_pcwl = imgu_css_tcc_gain_pcwl_lut;
+		acc->tcc.r_sqr_lut = imgu_css_tcc_r_sqr_lut;
+	}
+
+	/* acc_param: dpc_config */
+
+	if (use && use->acc_dpc)
+		return -EINVAL;	/* Not supported yet */
+
+	/* Just disable by default */
+	memset(&acc->dpc, 0, sizeof(acc->dpc));
+
+	/* acc_param: bds_config */
+
+	bds_ds = (css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height *
+		  IMGU_BDS_GRANULARITY) / css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	if (bds_ds < IMGU_BDS_MIN_SF_INV ||
+	    bds_ds - IMGU_BDS_MIN_SF_INV >= ARRAY_SIZE(imgu_css_bds_configs))
+		return -EINVAL;
+
+	cfg_bds = &imgu_css_bds_configs[bds_ds - IMGU_BDS_MIN_SF_INV];
+	acc->bds.hor.hor_ctrl1.hor_crop_en = 0;
+	acc->bds.hor.hor_ctrl1.hor_crop_start = 0;
+	acc->bds.hor.hor_ctrl1.hor_crop_end = 0;
+	acc->bds.hor.hor_ctrl0.sample_patrn_length =
+				cfg_bds->sample_patrn_length;
+	acc->bds.hor.hor_ctrl0.hor_ds_en = cfg_bds->hor_ds_en;
+	acc->bds.hor.hor_ctrl0.min_clip_val = IMGU_BDS_MIN_CLIP_VAL;
+	acc->bds.hor.hor_ctrl0.max_clip_val = IMGU_BDS_MAX_CLIP_VAL;
+	acc->bds.hor.hor_ctrl0.out_frame_width =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	acc->bds.hor.hor_ptrn_arr = cfg_bds->ptrn_arr;
+	acc->bds.hor.hor_phase_arr = cfg_bds->hor_phase_arr;
+	acc->bds.hor.hor_ctrl2.input_frame_height =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height;
+	acc->bds.ver.ver_ctrl0.min_clip_val = IMGU_BDS_MIN_CLIP_VAL;
+	acc->bds.ver.ver_ctrl0.max_clip_val = IMGU_BDS_MAX_CLIP_VAL;
+	acc->bds.ver.ver_ctrl0.sample_patrn_length =
+				cfg_bds->sample_patrn_length;
+	acc->bds.ver.ver_ctrl0.ver_ds_en = cfg_bds->ver_ds_en;
+	acc->bds.ver.ver_ptrn_arr = cfg_bds->ptrn_arr;
+	acc->bds.ver.ver_phase_arr = cfg_bds->ver_phase_arr;
+	acc->bds.ver.ver_ctrl1.out_frame_width =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	acc->bds.ver.ver_ctrl1.out_frame_height =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	for (i = 0; i < stripes; i++)
+		acc_bds_per_stripe_data(css, acc, i, pipe);
+
+	acc->bds.enabled = cfg_bds->hor_ds_en || cfg_bds->ver_ds_en;
+
+	/* acc_param: anr_config */
+
+	if (use && use->acc_anr) {
+		/* Take values from user */
+		acc->anr.transform = acc_user->anr.transform;
+		acc->anr.stitch.anr_stitch_en =
+			acc_user->anr.stitch.anr_stitch_en;
+		memcpy(acc->anr.stitch.pyramid, acc_user->anr.stitch.pyramid,
+		       sizeof(acc->anr.stitch.pyramid));
+	} else if (acc_old) {
+		/* Use old value */
+		acc->anr.transform = acc_old->anr.transform;
+		acc->anr.stitch.anr_stitch_en =
+			acc_old->anr.stitch.anr_stitch_en;
+		memcpy(acc->anr.stitch.pyramid, acc_old->anr.stitch.pyramid,
+		       sizeof(acc->anr.stitch.pyramid));
+	} else {
+		/* Calculate from scratch */
+		acc->anr = imgu_css_anr_defaults;
+	}
+
+	/* Always enabled */
+	acc->anr.search.enable = 1;
+	acc->anr.transform.enable = 1;
+	acc->anr.tile2strm.enable = 1;
+	acc->anr.tile2strm.frame_width =
+		ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, IMGU_ISP_VMEM_ALIGN);
+	acc->anr.search.frame_width = acc->anr.tile2strm.frame_width;
+	acc->anr.stitch.frame_width = acc->anr.tile2strm.frame_width;
+	acc->anr.tile2strm.frame_height = css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	acc->anr.search.frame_height = acc->anr.tile2strm.frame_height;
+	acc->anr.stitch.frame_height = acc->anr.tile2strm.frame_height;
+
+	width = ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, IMGU_ISP_VMEM_ALIGN);
+	height = css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+
+	if (acc->anr.transform.xreset + width > IPU3_UAPI_ANR_MAX_RESET)
+		acc->anr.transform.xreset = IPU3_UAPI_ANR_MAX_RESET - width;
+	if (acc->anr.transform.xreset < IPU3_UAPI_ANR_MIN_RESET)
+		acc->anr.transform.xreset = IPU3_UAPI_ANR_MIN_RESET;
+
+	if (acc->anr.transform.yreset + height > IPU3_UAPI_ANR_MAX_RESET)
+		acc->anr.transform.yreset = IPU3_UAPI_ANR_MAX_RESET - height;
+	if (acc->anr.transform.yreset < IPU3_UAPI_ANR_MIN_RESET)
+		acc->anr.transform.yreset = IPU3_UAPI_ANR_MIN_RESET;
+
+	/* acc_param: awb_fr_config */
+
+	if (use && use->acc_awb_fr) {
+		/* Take values from user */
+		acc->awb_fr.config = acc_user->awb_fr;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->awb_fr.config = acc_old->awb_fr.config;
+	} else {
+		/* Set from scratch */
+		acc->awb_fr.config = imgu_css_awb_fr_defaults;
+	}
+
+	imgu_css_grid_end_calc(&acc->awb_fr.config.grid_cfg);
+
+	if (acc->awb_fr.config.grid_cfg.width <= 0)
+		return -EINVAL;
+
+	acc->awb_fr.config.grid_cfg.height_per_slice =
+		IMGU_ABI_AWB_FR_MAX_CELLS_PER_SET /
+		acc->awb_fr.config.grid_cfg.width;
+
+	for (i = 0; i < stripes; i++)
+		acc->awb_fr.stripes[i] = acc->awb_fr.config;
+
+	if (acc->awb_fr.config.grid_cfg.x_start >=
+	    acc->stripe.down_scaled_stripes[1].offset + min_overlap) {
+		/* Enable only for rightmost stripe, disable left */
+		acc->awb_fr.stripes[0].grid_cfg.y_start &=
+					~IPU3_UAPI_GRID_Y_START_EN;
+	} else if (acc->awb_fr.config.grid_cfg.x_end <=
+		   acc->stripe.bds_out_stripes[0].width - min_overlap) {
+		/* Enable only for leftmost stripe, disable right */
+		acc->awb_fr.stripes[1].grid_cfg.y_start &=
+					~IPU3_UAPI_GRID_Y_START_EN;
+	} else {
+		/* Enable for both stripes */
+		u16 end; /* width for grid end */
+
+		acc->awb_fr.stripes[0].grid_cfg.width =
+			(acc->stripe.bds_out_stripes[0].width - min_overlap -
+			 acc->awb_fr.config.grid_cfg.x_start + 1) >>
+			acc->awb_fr.config.grid_cfg.block_width_log2;
+		acc->awb_fr.stripes[1].grid_cfg.width =
+			acc->awb_fr.config.grid_cfg.width -
+			acc->awb_fr.stripes[0].grid_cfg.width;
+
+		b_w_log2 = acc->awb_fr.stripes[0].grid_cfg.block_width_log2;
+		end = imgu_css_grid_end(acc->awb_fr.stripes[0].grid_cfg.x_start,
+					acc->awb_fr.stripes[0].grid_cfg.width,
+					b_w_log2);
+		acc->awb_fr.stripes[0].grid_cfg.x_end = end;
+
+		acc->awb_fr.stripes[1].grid_cfg.x_start =
+			(acc->awb_fr.stripes[0].grid_cfg.x_end + 1 -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+		b_w_log2 = acc->awb_fr.stripes[1].grid_cfg.block_width_log2;
+		end = imgu_css_grid_end(acc->awb_fr.stripes[1].grid_cfg.x_start,
+					acc->awb_fr.stripes[1].grid_cfg.width,
+					b_w_log2);
+		acc->awb_fr.stripes[1].grid_cfg.x_end = end;
+
+		/*
+		 * To reduce complexity of debubbling and loading
+		 * statistics fix grid_height_per_slice to 1 for both
+		 * stripes.
+		 */
+		for (i = 0; i < stripes; i++)
+			acc->awb_fr.stripes[i].grid_cfg.height_per_slice = 1;
+	}
+
+	if (imgu_css_awb_fr_ops_calc(css, pipe, &acc->awb_fr))
+		return -EINVAL;
+
+	/* acc_param: ae_config */
+
+	if (use && use->acc_ae) {
+		/* Take values from user */
+		acc->ae.grid_cfg = acc_user->ae.grid_cfg;
+		acc->ae.ae_ccm = acc_user->ae.ae_ccm;
+		for (i = 0; i < IPU3_UAPI_AE_WEIGHTS; i++)
+			acc->ae.weights[i] = acc_user->ae.weights[i];
+	} else if (acc_old) {
+		/* Use old value */
+		acc->ae.grid_cfg = acc_old->ae.grid_cfg;
+		acc->ae.ae_ccm = acc_old->ae.ae_ccm;
+		for (i = 0; i < IPU3_UAPI_AE_WEIGHTS; i++)
+			acc->ae.weights[i] = acc_old->ae.weights[i];
+	} else {
+		/* Set from scratch */
+		static const struct ipu3_uapi_ae_weight_elem
+			weight_def = { 1, 1, 1, 1, 1, 1, 1, 1 };
+
+		acc->ae.grid_cfg = imgu_css_ae_grid_defaults;
+		acc->ae.ae_ccm = imgu_css_ae_ccm_defaults;
+		for (i = 0; i < IPU3_UAPI_AE_WEIGHTS; i++)
+			acc->ae.weights[i] = weight_def;
+	}
+
+	b_w_log2 = acc->ae.grid_cfg.block_width_log2;
+	acc->ae.grid_cfg.x_end = imgu_css_grid_end(acc->ae.grid_cfg.x_start,
+						   acc->ae.grid_cfg.width,
+						   b_w_log2);
+	b_w_log2 = acc->ae.grid_cfg.block_height_log2;
+	acc->ae.grid_cfg.y_end = imgu_css_grid_end(acc->ae.grid_cfg.y_start,
+						   acc->ae.grid_cfg.height,
+						   b_w_log2);
+
+	for (i = 0; i < stripes; i++)
+		acc->ae.stripes[i].grid = acc->ae.grid_cfg;
+
+	if (acc->ae.grid_cfg.x_start >=
+	    acc->stripe.down_scaled_stripes[1].offset) {
+		/* Enable only for rightmost stripe, disable left */
+		acc->ae.stripes[0].grid.ae_en = 0;
+	} else if (acc->ae.grid_cfg.x_end <=
+		   acc->stripe.bds_out_stripes[0].width) {
+		/* Enable only for leftmost stripe, disable right */
+		acc->ae.stripes[1].grid.ae_en = 0;
+	} else {
+		/* Enable for both stripes */
+		u8 b_w_log2;
+
+		acc->ae.stripes[0].grid.width =
+			(acc->stripe.bds_out_stripes[0].width -
+			 acc->ae.grid_cfg.x_start + 1) >>
+			acc->ae.grid_cfg.block_width_log2;
+
+		acc->ae.stripes[1].grid.width =
+			acc->ae.grid_cfg.width - acc->ae.stripes[0].grid.width;
+
+		b_w_log2 = acc->ae.stripes[0].grid.block_width_log2;
+		acc->ae.stripes[0].grid.x_end =
+			imgu_css_grid_end(acc->ae.stripes[0].grid.x_start,
+					  acc->ae.stripes[0].grid.width,
+					  b_w_log2);
+
+		acc->ae.stripes[1].grid.x_start =
+			(acc->ae.stripes[0].grid.x_end + 1 -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+		b_w_log2 = acc->ae.stripes[1].grid.block_width_log2;
+		acc->ae.stripes[1].grid.x_end =
+			imgu_css_grid_end(acc->ae.stripes[1].grid.x_start,
+					  acc->ae.stripes[1].grid.width,
+					  b_w_log2);
+	}
+
+	/* acc_param: af_config */
+
+	if (use && use->acc_af) {
+		/* Take values from user */
+		acc->af.config.filter_config = acc_user->af.filter_config;
+		acc->af.config.grid_cfg = acc_user->af.grid_cfg;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->af.config = acc_old->af.config;
+	} else {
+		/* Set from scratch */
+		acc->af.config.filter_config =
+				imgu_css_af_defaults.filter_config;
+		acc->af.config.grid_cfg = imgu_css_af_defaults.grid_cfg;
+	}
+
+	imgu_css_grid_end_calc(&acc->af.config.grid_cfg);
+
+	if (acc->af.config.grid_cfg.width <= 0)
+		return -EINVAL;
+
+	acc->af.config.grid_cfg.height_per_slice =
+		IMGU_ABI_AF_MAX_CELLS_PER_SET / acc->af.config.grid_cfg.width;
+	acc->af.config.frame_size.width =
+		ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width, IMGU_ISP_VMEM_ALIGN);
+	acc->af.config.frame_size.height =
+		css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+
+	if (acc->stripe.bds_out_stripes[0].width <= min_overlap)
+		return -EINVAL;
+
+	for (i = 0; i < stripes; i++) {
+		acc->af.stripes[i].grid_cfg = acc->af.config.grid_cfg;
+		acc->af.stripes[i].frame_size.height =
+				css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+		acc->af.stripes[i].frame_size.width =
+			acc->stripe.bds_out_stripes[i].width;
+	}
+
+	if (acc->af.config.grid_cfg.x_start >=
+	    acc->stripe.down_scaled_stripes[1].offset + min_overlap) {
+		/* Enable only for rightmost stripe, disable left */
+		acc->af.stripes[0].grid_cfg.y_start &=
+			~IPU3_UAPI_GRID_Y_START_EN;
+		acc->af.stripes[1].grid_cfg.x_start =
+			(acc->af.stripes[1].grid_cfg.x_start -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+		b_w_log2 = acc->af.stripes[1].grid_cfg.block_width_log2;
+		acc->af.stripes[1].grid_cfg.x_end =
+			imgu_css_grid_end(acc->af.stripes[1].grid_cfg.x_start,
+					  acc->af.stripes[1].grid_cfg.width,
+					  b_w_log2);
+	} else if (acc->af.config.grid_cfg.x_end <=
+		   acc->stripe.bds_out_stripes[0].width - min_overlap) {
+		/* Enable only for leftmost stripe, disable right */
+		acc->af.stripes[1].grid_cfg.y_start &=
+			~IPU3_UAPI_GRID_Y_START_EN;
+	} else {
+		/* Enable for both stripes */
+
+		acc->af.stripes[0].grid_cfg.width =
+			(acc->stripe.bds_out_stripes[0].width - min_overlap -
+			 acc->af.config.grid_cfg.x_start + 1) >>
+			acc->af.config.grid_cfg.block_width_log2;
+		acc->af.stripes[1].grid_cfg.width =
+			acc->af.config.grid_cfg.width -
+			acc->af.stripes[0].grid_cfg.width;
+
+		b_w_log2 = acc->af.stripes[0].grid_cfg.block_width_log2;
+		acc->af.stripes[0].grid_cfg.x_end =
+			imgu_css_grid_end(acc->af.stripes[0].grid_cfg.x_start,
+					  acc->af.stripes[0].grid_cfg.width,
+					  b_w_log2);
+
+		acc->af.stripes[1].grid_cfg.x_start =
+			(acc->af.stripes[0].grid_cfg.x_end + 1 -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+
+		b_w_log2 = acc->af.stripes[1].grid_cfg.block_width_log2;
+		acc->af.stripes[1].grid_cfg.x_end =
+			imgu_css_grid_end(acc->af.stripes[1].grid_cfg.x_start,
+					  acc->af.stripes[1].grid_cfg.width,
+					  b_w_log2);
+
+		/*
+		 * To reduce complexity of debubbling and loading statistics
+		 * fix grid_height_per_slice to 1 for both stripes
+		 */
+		for (i = 0; i < stripes; i++)
+			acc->af.stripes[i].grid_cfg.height_per_slice = 1;
+	}
+
+	if (imgu_css_af_ops_calc(css, pipe, &acc->af))
+		return -EINVAL;
+
+	/* acc_param: awb_config */
+
+	if (use && use->acc_awb) {
+		/* Take values from user */
+		acc->awb.config = acc_user->awb.config;
+	} else if (acc_old) {
+		/* Use old value */
+		acc->awb.config = acc_old->awb.config;
+	} else {
+		/* Set from scratch */
+		acc->awb.config = imgu_css_awb_defaults;
+	}
+
+	if (acc->awb.config.grid.width <= 0)
+		return -EINVAL;
+
+	acc->awb.config.grid.height_per_slice =
+		IMGU_ABI_AWB_MAX_CELLS_PER_SET / acc->awb.config.grid.width,
+	imgu_css_grid_end_calc(&acc->awb.config.grid);
+
+	for (i = 0; i < stripes; i++)
+		acc->awb.stripes[i] = acc->awb.config;
+
+	if (acc->awb.config.grid.x_start >=
+	    acc->stripe.down_scaled_stripes[1].offset + min_overlap) {
+		/* Enable only for rightmost stripe, disable left */
+		acc->awb.stripes[0].rgbs_thr_b &= ~IPU3_UAPI_AWB_RGBS_THR_B_EN;
+
+		acc->awb.stripes[1].grid.x_start =
+			(acc->awb.stripes[1].grid.x_start -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+
+		b_w_log2 = acc->awb.stripes[1].grid.block_width_log2;
+		acc->awb.stripes[1].grid.x_end =
+			imgu_css_grid_end(acc->awb.stripes[1].grid.x_start,
+					  acc->awb.stripes[1].grid.width,
+					  b_w_log2);
+	} else if (acc->awb.config.grid.x_end <=
+		   acc->stripe.bds_out_stripes[0].width - min_overlap) {
+		/* Enable only for leftmost stripe, disable right */
+		acc->awb.stripes[1].rgbs_thr_b &= ~IPU3_UAPI_AWB_RGBS_THR_B_EN;
+	} else {
+		/* Enable for both stripes */
+
+		acc->awb.stripes[0].grid.width =
+			(acc->stripe.bds_out_stripes[0].width -
+			 acc->awb.config.grid.x_start + 1) >>
+			acc->awb.config.grid.block_width_log2;
+		acc->awb.stripes[1].grid.width = acc->awb.config.grid.width -
+				acc->awb.stripes[0].grid.width;
+
+		b_w_log2 = acc->awb.stripes[0].grid.block_width_log2;
+		acc->awb.stripes[0].grid.x_end =
+			imgu_css_grid_end(acc->awb.stripes[0].grid.x_start,
+					  acc->awb.stripes[0].grid.width,
+					  b_w_log2);
+
+		acc->awb.stripes[1].grid.x_start =
+			(acc->awb.stripes[0].grid.x_end + 1 -
+			 acc->stripe.down_scaled_stripes[1].offset) &
+			IPU3_UAPI_GRID_START_MASK;
+
+		b_w_log2 = acc->awb.stripes[1].grid.block_width_log2;
+		acc->awb.stripes[1].grid.x_end =
+			imgu_css_grid_end(acc->awb.stripes[1].grid.x_start,
+					  acc->awb.stripes[1].grid.width,
+					  b_w_log2);
+
+		/*
+		 * To reduce complexity of debubbling and loading statistics
+		 * fix grid_height_per_slice to 1 for both stripes
+		 */
+		for (i = 0; i < stripes; i++)
+			acc->awb.stripes[i].grid.height_per_slice = 1;
+	}
+
+	if (imgu_css_awb_ops_calc(css, pipe, &acc->awb))
+		return -EINVAL;
+
+	return 0;
+}
+
+/*
+ * Fill the indicated structure in `new_binary_params' from the possible
+ * sources based on `use_user' flag: if the flag is false, copy from
+ * `old_binary_params', or if the flag is true, copy from `user_setting'
+ * and return NULL (or error pointer on error).
+ * If the flag is false and `old_binary_params' is NULL, return pointer
+ * to the structure inside `new_binary_params'. In that case the caller
+ * should calculate and fill the structure from scratch.
+ */
+static void *imgu_css_cfg_copy(struct imgu_css *css,
+			       unsigned int pipe, bool use_user,
+			       void *user_setting, void *old_binary_params,
+			       void *new_binary_params,
+			       enum imgu_abi_memories m,
+			       struct imgu_fw_isp_parameter *par,
+			       size_t par_size)
+{
+	const enum imgu_abi_param_class c = IMGU_ABI_PARAM_CLASS_PARAM;
+	void *new_setting, *old_setting;
+
+	new_setting = imgu_css_fw_pipeline_params(css, pipe, c, m, par,
+						  par_size, new_binary_params);
+	if (!new_setting)
+		return ERR_PTR(-EPROTO);	/* Corrupted firmware */
+
+	if (use_user) {
+		/* Take new user parameters */
+		memcpy(new_setting, user_setting, par_size);
+	} else if (old_binary_params) {
+		/* Take previous value */
+		old_setting = imgu_css_fw_pipeline_params(css, pipe, c, m, par,
+							  par_size,
+							  old_binary_params);
+		if (!old_setting)
+			return ERR_PTR(-EPROTO);
+		memcpy(new_setting, old_setting, par_size);
+	} else {
+		return new_setting;	/* Need to calculate */
+	}
+
+	return NULL;		/* Copied from other value */
+}
+
+/*
+ * Configure VMEM0 parameters (late binding parameters).
+ */
+int imgu_css_cfg_vmem0(struct imgu_css *css, unsigned int pipe,
+		       struct ipu3_uapi_flags *use,
+		       void *vmem0, void *vmem0_old,
+		       struct ipu3_uapi_params *user)
+{
+	const struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css->pipes[pipe].bindex];
+	struct imgu_fw_param_memory_offsets *pofs = (void *)css->fwp +
+		bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_PARAM];
+	struct ipu3_uapi_isp_lin_vmem_params *lin_vmem = NULL;
+	struct ipu3_uapi_isp_tnr3_vmem_params *tnr_vmem = NULL;
+	struct ipu3_uapi_isp_xnr3_vmem_params *xnr_vmem = NULL;
+	const enum imgu_abi_param_class c = IMGU_ABI_PARAM_CLASS_PARAM;
+	const enum imgu_abi_memories m = IMGU_ABI_MEM_ISP_VMEM0;
+	unsigned int i;
+
+	/* Configure VMEM0 */
+
+	memset(vmem0, 0, bi->info.isp.sp.mem_initializers.params[c][m].size);
+
+	/* Configure Linearization VMEM0 parameters */
+
+	lin_vmem = imgu_css_cfg_copy(css, pipe, use && use->lin_vmem_params,
+				     &user->lin_vmem_params, vmem0_old, vmem0,
+				     m, &pofs->vmem.lin, sizeof(*lin_vmem));
+	if (!IS_ERR_OR_NULL(lin_vmem)) {
+		/* Generate parameter from scratch */
+		for (i = 0; i < IPU3_UAPI_LIN_LUT_SIZE; i++) {
+			lin_vmem->lin_lutlow_gr[i] = 32 * i;
+			lin_vmem->lin_lutlow_r[i] = 32 * i;
+			lin_vmem->lin_lutlow_b[i] = 32 * i;
+			lin_vmem->lin_lutlow_gb[i] = 32 * i;
+
+			lin_vmem->lin_lutdif_gr[i] = 32;
+			lin_vmem->lin_lutdif_r[i] = 32;
+			lin_vmem->lin_lutdif_b[i] = 32;
+			lin_vmem->lin_lutdif_gb[i] = 32;
+		}
+	}
+
+	/* Configure TNR3 VMEM parameters */
+	if (css->pipes[pipe].pipe_id == IPU3_CSS_PIPE_ID_VIDEO) {
+		tnr_vmem = imgu_css_cfg_copy(css, pipe,
+					     use && use->tnr3_vmem_params,
+					     &user->tnr3_vmem_params,
+					     vmem0_old, vmem0, m,
+					     &pofs->vmem.tnr3,
+					     sizeof(*tnr_vmem));
+		if (!IS_ERR_OR_NULL(tnr_vmem)) {
+			/* Generate parameter from scratch */
+			for (i = 0; i < IPU3_UAPI_ISP_TNR3_VMEM_LEN; i++)
+				tnr_vmem->sigma[i] = 256;
+		}
+	}
+	i = IPU3_UAPI_ISP_TNR3_VMEM_LEN;
+
+	/* Configure XNR3 VMEM parameters */
+
+	xnr_vmem = imgu_css_cfg_copy(css, pipe, use && use->xnr3_vmem_params,
+				     &user->xnr3_vmem_params, vmem0_old, vmem0,
+				     m, &pofs->vmem.xnr3, sizeof(*xnr_vmem));
+	if (!IS_ERR_OR_NULL(xnr_vmem)) {
+		xnr_vmem->x[i] = imgu_css_xnr3_vmem_defaults.x
+			[i % IMGU_XNR3_VMEM_LUT_LEN];
+		xnr_vmem->a[i] = imgu_css_xnr3_vmem_defaults.a
+			[i % IMGU_XNR3_VMEM_LUT_LEN];
+		xnr_vmem->b[i] = imgu_css_xnr3_vmem_defaults.b
+			[i % IMGU_XNR3_VMEM_LUT_LEN];
+		xnr_vmem->c[i] = imgu_css_xnr3_vmem_defaults.c
+			[i % IMGU_XNR3_VMEM_LUT_LEN];
+	}
+
+	return IS_ERR(lin_vmem) || IS_ERR(tnr_vmem) || IS_ERR(xnr_vmem) ?
+		-EPROTO : 0;
+}
+
+/*
+ * Configure DMEM0 parameters (late binding parameters).
+ */
+int imgu_css_cfg_dmem0(struct imgu_css *css, unsigned int pipe,
+		       struct ipu3_uapi_flags *use,
+		       void *dmem0, void *dmem0_old,
+		       struct ipu3_uapi_params *user)
+{
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	const struct imgu_fw_info *bi =
+		&css->fwp->binary_header[css_pipe->bindex];
+	struct imgu_fw_param_memory_offsets *pofs = (void *)css->fwp +
+		bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_PARAM];
+
+	struct ipu3_uapi_isp_tnr3_params *tnr_dmem = NULL;
+	struct ipu3_uapi_isp_xnr3_params *xnr_dmem;
+
+	const enum imgu_abi_param_class c = IMGU_ABI_PARAM_CLASS_PARAM;
+	const enum imgu_abi_memories m = IMGU_ABI_MEM_ISP_DMEM0;
+
+	/* Configure DMEM0 */
+
+	memset(dmem0, 0, bi->info.isp.sp.mem_initializers.params[c][m].size);
+
+	/* Configure TNR3 DMEM0 parameters */
+	if (css_pipe->pipe_id == IPU3_CSS_PIPE_ID_VIDEO) {
+		tnr_dmem = imgu_css_cfg_copy(css, pipe,
+					     use && use->tnr3_dmem_params,
+					     &user->tnr3_dmem_params,
+					     dmem0_old, dmem0, m,
+					     &pofs->dmem.tnr3,
+					     sizeof(*tnr_dmem));
+		if (!IS_ERR_OR_NULL(tnr_dmem)) {
+			/* Generate parameter from scratch */
+			tnr_dmem->knee_y1 = 768;
+			tnr_dmem->knee_y2 = 1280;
+		}
+	}
+
+	/* Configure XNR3 DMEM0 parameters */
+
+	xnr_dmem = imgu_css_cfg_copy(css, pipe, use && use->xnr3_dmem_params,
+				     &user->xnr3_dmem_params, dmem0_old, dmem0,
+				     m, &pofs->dmem.xnr3, sizeof(*xnr_dmem));
+	if (!IS_ERR_OR_NULL(xnr_dmem)) {
+		/* Generate parameter from scratch */
+		xnr_dmem->alpha.y0 = 2047;
+		xnr_dmem->alpha.u0 = 2047;
+		xnr_dmem->alpha.v0 = 2047;
+	}
+
+	return IS_ERR(tnr_dmem) || IS_ERR(xnr_dmem) ? -EPROTO : 0;
+}
+
+/* Generate unity morphing table without morphing effect */
+void imgu_css_cfg_gdc_table(struct imgu_abi_gdc_warp_param *gdc,
+			    int frame_in_x, int frame_in_y,
+			    int frame_out_x, int frame_out_y,
+			    int env_w, int env_h)
+{
+	static const unsigned int FRAC_BITS = IMGU_ABI_GDC_FRAC_BITS;
+	static const unsigned int XMEM_ALIGN = 1 << 4;
+	const unsigned int XMEM_ALIGN_MASK = ~(XMEM_ALIGN - 1);
+	static const unsigned int BCI_ENV = 4;
+	static const unsigned int BYP = 2;	/* Bytes per pixel */
+	const unsigned int OFFSET_X = 2 * IMGU_DVS_BLOCK_W + env_w + 1;
+	const unsigned int OFFSET_Y = IMGU_DVS_BLOCK_H + env_h + 1;
+
+	struct imgu_abi_gdc_warp_param gdc_luma, gdc_chroma;
+
+	unsigned int blocks_x = ALIGN(DIV_ROUND_UP(frame_out_x,
+						   IMGU_DVS_BLOCK_W), 2);
+	unsigned int blocks_y = DIV_ROUND_UP(frame_out_y, IMGU_DVS_BLOCK_H);
+	unsigned int y0, x0, x1, x, y;
+
+	/* Global luma settings */
+	gdc_luma.origin_x = 0;
+	gdc_luma.origin_y = 0;
+	gdc_luma.p0_x = (OFFSET_X - (OFFSET_X & XMEM_ALIGN_MASK)) << FRAC_BITS;
+	gdc_luma.p0_y = 0;
+	gdc_luma.p1_x = gdc_luma.p0_x + (IMGU_DVS_BLOCK_W << FRAC_BITS);
+	gdc_luma.p1_y = gdc_luma.p0_y;
+	gdc_luma.p2_x = gdc_luma.p0_x;
+	gdc_luma.p2_y = gdc_luma.p0_y + (IMGU_DVS_BLOCK_H << FRAC_BITS);
+	gdc_luma.p3_x = gdc_luma.p1_x;
+	gdc_luma.p3_y = gdc_luma.p2_y;
+
+	gdc_luma.in_block_width = IMGU_DVS_BLOCK_W + BCI_ENV +
+					OFFSET_X - (OFFSET_X & XMEM_ALIGN_MASK);
+	gdc_luma.in_block_width_a = DIV_ROUND_UP(gdc_luma.in_block_width,
+						 IPU3_UAPI_ISP_VEC_ELEMS);
+	gdc_luma.in_block_width_b = DIV_ROUND_UP(gdc_luma.in_block_width,
+						 IMGU_ABI_ISP_DDR_WORD_BYTES /
+						 BYP);
+	gdc_luma.in_block_height = IMGU_DVS_BLOCK_H + BCI_ENV;
+	gdc_luma.padding = 0;
+
+	/* Global chroma settings */
+	gdc_chroma.origin_x = 0;
+	gdc_chroma.origin_y = 0;
+	gdc_chroma.p0_x = (OFFSET_X / 2 - (OFFSET_X / 2 & XMEM_ALIGN_MASK)) <<
+			   FRAC_BITS;
+	gdc_chroma.p0_y = 0;
+	gdc_chroma.p1_x = gdc_chroma.p0_x + (IMGU_DVS_BLOCK_W << FRAC_BITS);
+	gdc_chroma.p1_y = gdc_chroma.p0_y;
+	gdc_chroma.p2_x = gdc_chroma.p0_x;
+	gdc_chroma.p2_y = gdc_chroma.p0_y + (IMGU_DVS_BLOCK_H / 2 << FRAC_BITS);
+	gdc_chroma.p3_x = gdc_chroma.p1_x;
+	gdc_chroma.p3_y = gdc_chroma.p2_y;
+
+	gdc_chroma.in_block_width = IMGU_DVS_BLOCK_W + BCI_ENV;
+	gdc_chroma.in_block_width_a = DIV_ROUND_UP(gdc_chroma.in_block_width,
+						   IPU3_UAPI_ISP_VEC_ELEMS);
+	gdc_chroma.in_block_width_b = DIV_ROUND_UP(gdc_chroma.in_block_width,
+						   IMGU_ABI_ISP_DDR_WORD_BYTES /
+						   BYP);
+	gdc_chroma.in_block_height = IMGU_DVS_BLOCK_H / 2 + BCI_ENV;
+	gdc_chroma.padding = 0;
+
+	/* Calculate block offsets for luma and chroma */
+	for (y0 = 0; y0 < blocks_y; y0++) {
+		for (x0 = 0; x0 < blocks_x / 2; x0++) {
+			for (x1 = 0; x1 < 2; x1++) {
+				/* Luma blocks */
+				x = (x0 * 2 + x1) * IMGU_DVS_BLOCK_W + OFFSET_X;
+				x &= XMEM_ALIGN_MASK;
+				y = y0 * IMGU_DVS_BLOCK_H + OFFSET_Y;
+				*gdc = gdc_luma;
+				gdc->in_addr_offset =
+					(y * frame_in_x + x) * BYP;
+				gdc++;
+			}
+
+			/* Chroma block */
+			x = x0 * IMGU_DVS_BLOCK_W + OFFSET_X / 2;
+			x &= XMEM_ALIGN_MASK;
+			y = y0 * (IMGU_DVS_BLOCK_H / 2) + OFFSET_Y / 2;
+			*gdc = gdc_chroma;
+			gdc->in_addr_offset = (y * frame_in_x + x) * BYP;
+			gdc++;
+		}
+	}
+}
diff --git a/drivers/staging/media/ipu3/ipu3-css-params.h b/drivers/staging/media/ipu3/ipu3-css-params.h
new file mode 100644
index 000000000..ffaec6b7d
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-params.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_PARAMS_H
+#define __IPU3_PARAMS_H
+
+int imgu_css_cfg_acc(struct imgu_css *css, unsigned int pipe,
+		     struct ipu3_uapi_flags *use,
+		     struct imgu_abi_acc_param *acc,
+		     struct imgu_abi_acc_param *acc_old,
+		     struct ipu3_uapi_acc_param *acc_user);
+
+int imgu_css_cfg_vmem0(struct imgu_css *css, unsigned int pipe,
+		       struct ipu3_uapi_flags *use,
+		       void *vmem0, void *vmem0_old,
+		       struct ipu3_uapi_params *user);
+
+int imgu_css_cfg_dmem0(struct imgu_css *css, unsigned int pipe,
+		       struct ipu3_uapi_flags *use,
+		       void *dmem0, void *dmem0_old,
+		       struct ipu3_uapi_params *user);
+
+void imgu_css_cfg_gdc_table(struct imgu_abi_gdc_warp_param *gdc,
+			    int frame_in_x, int frame_in_y,
+			    int frame_out_x, int frame_out_y,
+			    int env_w, int env_h);
+
+#endif /*__IPU3_PARAMS_H */
diff --git a/drivers/staging/media/ipu3/ipu3-css-pool.c b/drivers/staging/media/ipu3/ipu3-css-pool.c
new file mode 100644
index 000000000..fa5b7d3ac
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-pool.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/device.h>
+
+#include "ipu3.h"
+#include "ipu3-css-pool.h"
+#include "ipu3-dmamap.h"
+
+int imgu_css_dma_buffer_resize(struct imgu_device *imgu,
+			       struct imgu_css_map *map, size_t size)
+{
+	if (map->size < size && map->vaddr) {
+		dev_warn(&imgu->pci_dev->dev, "dma buf resized from %zu to %zu",
+			 map->size, size);
+
+		imgu_dmamap_free(imgu, map);
+		if (!imgu_dmamap_alloc(imgu, map, size))
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void imgu_css_pool_cleanup(struct imgu_device *imgu, struct imgu_css_pool *pool)
+{
+	unsigned int i;
+
+	for (i = 0; i < IPU3_CSS_POOL_SIZE; i++)
+		imgu_dmamap_free(imgu, &pool->entry[i].param);
+}
+
+int imgu_css_pool_init(struct imgu_device *imgu, struct imgu_css_pool *pool,
+		       size_t size)
+{
+	unsigned int i;
+
+	for (i = 0; i < IPU3_CSS_POOL_SIZE; i++) {
+		pool->entry[i].valid = false;
+		if (size == 0) {
+			pool->entry[i].param.vaddr = NULL;
+			continue;
+		}
+
+		if (!imgu_dmamap_alloc(imgu, &pool->entry[i].param, size))
+			goto fail;
+	}
+
+	pool->last = IPU3_CSS_POOL_SIZE;
+
+	return 0;
+
+fail:
+	imgu_css_pool_cleanup(imgu, pool);
+	return -ENOMEM;
+}
+
+/*
+ * Allocate a new parameter via recycling the oldest entry in the pool.
+ */
+void imgu_css_pool_get(struct imgu_css_pool *pool)
+{
+	/* Get the oldest entry */
+	u32 n = (pool->last + 1) % IPU3_CSS_POOL_SIZE;
+
+	pool->entry[n].valid = true;
+	pool->last = n;
+}
+
+/*
+ * Undo, for all practical purposes, the effect of pool_get().
+ */
+void imgu_css_pool_put(struct imgu_css_pool *pool)
+{
+	pool->entry[pool->last].valid = false;
+	pool->last = (pool->last + IPU3_CSS_POOL_SIZE - 1) % IPU3_CSS_POOL_SIZE;
+}
+
+/**
+ * imgu_css_pool_last - Retrieve the nth pool entry from last
+ *
+ * @pool: a pointer to &struct imgu_css_pool.
+ * @n: the distance to the last index.
+ *
+ * Returns:
+ *  The nth entry from last or null map to indicate no frame stored.
+ */
+const struct imgu_css_map *
+imgu_css_pool_last(struct imgu_css_pool *pool, unsigned int n)
+{
+	static const struct imgu_css_map null_map = { 0 };
+	int i = (pool->last + IPU3_CSS_POOL_SIZE - n) % IPU3_CSS_POOL_SIZE;
+
+	WARN_ON(n >= IPU3_CSS_POOL_SIZE);
+
+	if (!pool->entry[i].valid)
+		return &null_map;
+
+	return &pool->entry[i].param;
+}
diff --git a/drivers/staging/media/ipu3/ipu3-css-pool.h b/drivers/staging/media/ipu3/ipu3-css-pool.h
new file mode 100644
index 000000000..3f9e32e0e
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css-pool.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_UTIL_H
+#define __IPU3_UTIL_H
+
+struct device;
+struct imgu_device;
+
+#define IPU3_CSS_POOL_SIZE		4
+
+/**
+ * struct imgu_css_map - store DMA mapping info for buffer
+ *
+ * @size:		size of the buffer in bytes.
+ * @vaddr:		kernel virtual address.
+ * @daddr:		iova dma address to access IPU3.
+ * @pages:		pages mapped to this buffer
+ */
+struct imgu_css_map {
+	size_t size;
+	void *vaddr;
+	dma_addr_t daddr;
+	struct page **pages;
+};
+
+/**
+ * struct imgu_css_pool - circular buffer pool definition
+ *
+ * @entry:		array with IPU3_CSS_POOL_SIZE elements.
+ * @entry.param:	a &struct imgu_css_map for storing the mem mapping.
+ * @entry.valid:	used to mark if the entry has valid data.
+ * @last:		write pointer, initialized to IPU3_CSS_POOL_SIZE.
+ */
+struct imgu_css_pool {
+	struct {
+		struct imgu_css_map param;
+		bool valid;
+	} entry[IPU3_CSS_POOL_SIZE];
+	u32 last;
+};
+
+int imgu_css_dma_buffer_resize(struct imgu_device *imgu,
+			       struct imgu_css_map *map, size_t size);
+void imgu_css_pool_cleanup(struct imgu_device *imgu,
+			   struct imgu_css_pool *pool);
+int imgu_css_pool_init(struct imgu_device *imgu, struct imgu_css_pool *pool,
+		       size_t size);
+void imgu_css_pool_get(struct imgu_css_pool *pool);
+void imgu_css_pool_put(struct imgu_css_pool *pool);
+const struct imgu_css_map *imgu_css_pool_last(struct imgu_css_pool *pool,
+					      u32 last);
+
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-css.c b/drivers/staging/media/ipu3/ipu3-css.c
new file mode 100644
index 000000000..8c70497d7
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css.c
@@ -0,0 +1,2357 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/device.h>
+#include <linux/iopoll.h>
+#include <linux/slab.h>
+
+#include "ipu3.h"
+#include "ipu3-css.h"
+#include "ipu3-css-fw.h"
+#include "ipu3-css-params.h"
+#include "ipu3-dmamap.h"
+#include "ipu3-tables.h"
+
+/* IRQ configuration */
+#define IMGU_IRQCTRL_IRQ_MASK	(IMGU_IRQCTRL_IRQ_SP1 | \
+				 IMGU_IRQCTRL_IRQ_SP2 | \
+				 IMGU_IRQCTRL_IRQ_SW_PIN(0) | \
+				 IMGU_IRQCTRL_IRQ_SW_PIN(1))
+
+#define IPU3_CSS_FORMAT_BPP_DEN	50	/* Denominator */
+
+/* Some sane limits for resolutions */
+#define IPU3_CSS_MIN_RES	32
+#define IPU3_CSS_MAX_H		3136
+#define IPU3_CSS_MAX_W		4224
+
+/* minimal envelope size(GDC in - out) should be 4 */
+#define MIN_ENVELOPE            4
+
+/*
+ * pre-allocated buffer size for CSS ABI, auxiliary frames
+ * after BDS and before GDC. Those values should be tuned
+ * to big enough to avoid buffer re-allocation when
+ * streaming to lower streaming latency.
+ */
+#define CSS_ABI_SIZE    136
+#define CSS_BDS_SIZE    (4480 * 3200 * 3)
+#define CSS_GDC_SIZE    (4224 * 3200 * 12 / 8)
+
+#define IPU3_CSS_QUEUE_TO_FLAGS(q)	(1 << (q))
+#define IPU3_CSS_FORMAT_FL_IN		\
+			IPU3_CSS_QUEUE_TO_FLAGS(IPU3_CSS_QUEUE_IN)
+#define IPU3_CSS_FORMAT_FL_OUT		\
+			IPU3_CSS_QUEUE_TO_FLAGS(IPU3_CSS_QUEUE_OUT)
+#define IPU3_CSS_FORMAT_FL_VF		\
+			IPU3_CSS_QUEUE_TO_FLAGS(IPU3_CSS_QUEUE_VF)
+
+/* Formats supported by IPU3 Camera Sub System */
+static const struct imgu_css_format imgu_css_formats[] = {
+	{
+		.pixelformat = V4L2_PIX_FMT_NV12,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.frame_format = IMGU_ABI_FRAME_FORMAT_NV12,
+		.osys_format = IMGU_ABI_OSYS_FORMAT_NV12,
+		.osys_tiling = IMGU_ABI_OSYS_TILING_NONE,
+		.chroma_decim = 4,
+		.width_align = IPU3_UAPI_ISP_VEC_ELEMS,
+		.flags = IPU3_CSS_FORMAT_FL_OUT | IPU3_CSS_FORMAT_FL_VF,
+	}, {
+		/* Each 32 bytes contains 25 10-bit pixels */
+		.pixelformat = V4L2_PIX_FMT_IPU3_SBGGR10,
+		.colorspace = V4L2_COLORSPACE_RAW,
+		.frame_format = IMGU_ABI_FRAME_FORMAT_RAW_PACKED,
+		.bayer_order = IMGU_ABI_BAYER_ORDER_BGGR,
+		.bit_depth = 10,
+		.width_align = 2 * IPU3_UAPI_ISP_VEC_ELEMS,
+		.flags = IPU3_CSS_FORMAT_FL_IN,
+	}, {
+		.pixelformat = V4L2_PIX_FMT_IPU3_SGBRG10,
+		.colorspace = V4L2_COLORSPACE_RAW,
+		.frame_format = IMGU_ABI_FRAME_FORMAT_RAW_PACKED,
+		.bayer_order = IMGU_ABI_BAYER_ORDER_GBRG,
+		.bit_depth = 10,
+		.width_align = 2 * IPU3_UAPI_ISP_VEC_ELEMS,
+		.flags = IPU3_CSS_FORMAT_FL_IN,
+	}, {
+		.pixelformat = V4L2_PIX_FMT_IPU3_SGRBG10,
+		.colorspace = V4L2_COLORSPACE_RAW,
+		.frame_format = IMGU_ABI_FRAME_FORMAT_RAW_PACKED,
+		.bayer_order = IMGU_ABI_BAYER_ORDER_GRBG,
+		.bit_depth = 10,
+		.width_align = 2 * IPU3_UAPI_ISP_VEC_ELEMS,
+		.flags = IPU3_CSS_FORMAT_FL_IN,
+	}, {
+		.pixelformat = V4L2_PIX_FMT_IPU3_SRGGB10,
+		.colorspace = V4L2_COLORSPACE_RAW,
+		.frame_format = IMGU_ABI_FRAME_FORMAT_RAW_PACKED,
+		.bayer_order = IMGU_ABI_BAYER_ORDER_RGGB,
+		.bit_depth = 10,
+		.width_align = 2 * IPU3_UAPI_ISP_VEC_ELEMS,
+		.flags = IPU3_CSS_FORMAT_FL_IN,
+	},
+};
+
+static const struct {
+	enum imgu_abi_queue_id qid;
+	size_t ptr_ofs;
+} imgu_css_queues[IPU3_CSS_QUEUES] = {
+	[IPU3_CSS_QUEUE_IN] = {
+		IMGU_ABI_QUEUE_C_ID,
+		offsetof(struct imgu_abi_buffer, payload.frame.frame_data)
+	},
+	[IPU3_CSS_QUEUE_OUT] = {
+		IMGU_ABI_QUEUE_D_ID,
+		offsetof(struct imgu_abi_buffer, payload.frame.frame_data)
+	},
+	[IPU3_CSS_QUEUE_VF] = {
+		IMGU_ABI_QUEUE_E_ID,
+		offsetof(struct imgu_abi_buffer, payload.frame.frame_data)
+	},
+	[IPU3_CSS_QUEUE_STAT_3A] = {
+		IMGU_ABI_QUEUE_F_ID,
+		offsetof(struct imgu_abi_buffer, payload.s3a.data_ptr)
+	},
+};
+
+/* Initialize queue based on given format, adjust format as needed */
+static int imgu_css_queue_init(struct imgu_css_queue *queue,
+			       struct v4l2_pix_format_mplane *fmt, u32 flags)
+{
+	struct v4l2_pix_format_mplane *const f = &queue->fmt.mpix;
+	unsigned int i;
+	u32 sizeimage;
+
+	INIT_LIST_HEAD(&queue->bufs);
+
+	queue->css_fmt = NULL;	/* Disable */
+	if (!fmt)
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(imgu_css_formats); i++) {
+		if (!(imgu_css_formats[i].flags & flags))
+			continue;
+		queue->css_fmt = &imgu_css_formats[i];
+		if (imgu_css_formats[i].pixelformat == fmt->pixelformat)
+			break;
+	}
+	if (!queue->css_fmt)
+		return -EINVAL;	/* Could not find any suitable format */
+
+	queue->fmt.mpix = *fmt;
+
+	f->width = ALIGN(clamp_t(u32, f->width,
+				 IPU3_CSS_MIN_RES, IPU3_CSS_MAX_W), 2);
+	f->height = ALIGN(clamp_t(u32, f->height,
+				  IPU3_CSS_MIN_RES, IPU3_CSS_MAX_H), 2);
+	queue->width_pad = ALIGN(f->width, queue->css_fmt->width_align);
+	f->plane_fmt[0].bytesperline =
+		imgu_bytesperline(f->width, queue->css_fmt->frame_format);
+	sizeimage = f->height * f->plane_fmt[0].bytesperline;
+	if (queue->css_fmt->chroma_decim)
+		sizeimage += 2 * sizeimage / queue->css_fmt->chroma_decim;
+
+	f->plane_fmt[0].sizeimage = sizeimage;
+	f->field = V4L2_FIELD_NONE;
+	f->num_planes = 1;
+	f->colorspace = queue->css_fmt->colorspace;
+	f->flags = 0;
+	f->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	f->quantization = V4L2_QUANTIZATION_DEFAULT;
+	f->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	memset(f->reserved, 0, sizeof(f->reserved));
+
+	return 0;
+}
+
+static bool imgu_css_queue_enabled(struct imgu_css_queue *q)
+{
+	return q->css_fmt;
+}
+
+/******************* css hw *******************/
+
+/* In the style of writesl() defined in include/asm-generic/io.h */
+static inline void writes(const void *mem, ssize_t count, void __iomem *addr)
+{
+	if (count >= 4) {
+		const u32 *buf = mem;
+
+		count /= 4;
+		do {
+			writel(*buf++, addr);
+			addr += 4;
+		} while (--count);
+	}
+}
+
+/* Wait until register `reg', masked with `mask', becomes `cmp' */
+static int imgu_hw_wait(void __iomem *base, int reg, u32 mask, u32 cmp)
+{
+	u32 val;
+
+	return readl_poll_timeout(base + reg, val, (val & mask) == cmp,
+				  1000, 100 * 1000);
+}
+
+/* Initialize the IPU3 CSS hardware and associated h/w blocks */
+
+int imgu_css_set_powerup(struct device *dev, void __iomem *base,
+			 unsigned int freq)
+{
+	u32 pm_ctrl, state, val;
+
+	dev_dbg(dev, "%s with freq %u\n", __func__, freq);
+	/* Clear the CSS busy signal */
+	readl(base + IMGU_REG_GP_BUSY);
+	writel(0, base + IMGU_REG_GP_BUSY);
+
+	/* Wait for idle signal */
+	if (imgu_hw_wait(base, IMGU_REG_STATE, IMGU_STATE_IDLE_STS,
+			 IMGU_STATE_IDLE_STS)) {
+		dev_err(dev, "failed to set CSS idle\n");
+		goto fail;
+	}
+
+	/* Reset the css */
+	writel(readl(base + IMGU_REG_PM_CTRL) | IMGU_PM_CTRL_FORCE_RESET,
+	       base + IMGU_REG_PM_CTRL);
+
+	usleep_range(200, 300);
+
+	/** Prepare CSS */
+
+	pm_ctrl = readl(base + IMGU_REG_PM_CTRL);
+	state = readl(base + IMGU_REG_STATE);
+
+	dev_dbg(dev, "CSS pm_ctrl 0x%x state 0x%x (power %s)\n",
+		pm_ctrl, state, state & IMGU_STATE_POWER_DOWN ? "down" : "up");
+
+	/* Power up CSS using wrapper */
+	if (state & IMGU_STATE_POWER_DOWN) {
+		writel(IMGU_PM_CTRL_RACE_TO_HALT | IMGU_PM_CTRL_START,
+		       base + IMGU_REG_PM_CTRL);
+		if (imgu_hw_wait(base, IMGU_REG_PM_CTRL,
+				 IMGU_PM_CTRL_START, 0)) {
+			dev_err(dev, "failed to power up CSS\n");
+			goto fail;
+		}
+		usleep_range(2000, 3000);
+	} else {
+		writel(IMGU_PM_CTRL_RACE_TO_HALT, base + IMGU_REG_PM_CTRL);
+	}
+
+	/* Set the busy bit */
+	writel(readl(base + IMGU_REG_GP_BUSY) | 1, base + IMGU_REG_GP_BUSY);
+
+	/* Set CSS clock frequency */
+	pm_ctrl = readl(base + IMGU_REG_PM_CTRL);
+	val = pm_ctrl & ~(IMGU_PM_CTRL_CSS_PWRDN | IMGU_PM_CTRL_RST_AT_EOF);
+	writel(val, base + IMGU_REG_PM_CTRL);
+	writel(0, base + IMGU_REG_GP_BUSY);
+	if (imgu_hw_wait(base, IMGU_REG_STATE,
+			 IMGU_STATE_PWRDNM_FSM_MASK, 0)) {
+		dev_err(dev, "failed to pwrdn CSS\n");
+		goto fail;
+	}
+	val = (freq / IMGU_SYSTEM_REQ_FREQ_DIVIDER) & IMGU_SYSTEM_REQ_FREQ_MASK;
+	writel(val, base + IMGU_REG_SYSTEM_REQ);
+	writel(1, base + IMGU_REG_GP_BUSY);
+	writel(readl(base + IMGU_REG_PM_CTRL) | IMGU_PM_CTRL_FORCE_HALT,
+	       base + IMGU_REG_PM_CTRL);
+	if (imgu_hw_wait(base, IMGU_REG_STATE, IMGU_STATE_HALT_STS,
+			 IMGU_STATE_HALT_STS)) {
+		dev_err(dev, "failed to halt CSS\n");
+		goto fail;
+	}
+
+	writel(readl(base + IMGU_REG_PM_CTRL) | IMGU_PM_CTRL_START,
+	       base + IMGU_REG_PM_CTRL);
+	if (imgu_hw_wait(base, IMGU_REG_PM_CTRL, IMGU_PM_CTRL_START, 0)) {
+		dev_err(dev, "failed to start CSS\n");
+		goto fail;
+	}
+	writel(readl(base + IMGU_REG_PM_CTRL) | IMGU_PM_CTRL_FORCE_UNHALT,
+	       base + IMGU_REG_PM_CTRL);
+
+	val = readl(base + IMGU_REG_PM_CTRL);	/* get pm_ctrl */
+	val &= ~(IMGU_PM_CTRL_CSS_PWRDN | IMGU_PM_CTRL_RST_AT_EOF);
+	val |= pm_ctrl & (IMGU_PM_CTRL_CSS_PWRDN | IMGU_PM_CTRL_RST_AT_EOF);
+	writel(val, base + IMGU_REG_PM_CTRL);
+
+	return 0;
+
+fail:
+	imgu_css_set_powerdown(dev, base);
+	return -EIO;
+}
+
+void imgu_css_set_powerdown(struct device *dev, void __iomem *base)
+{
+	dev_dbg(dev, "%s\n", __func__);
+	/* wait for cio idle signal */
+	if (imgu_hw_wait(base, IMGU_REG_CIO_GATE_BURST_STATE,
+			 IMGU_CIO_GATE_BURST_MASK, 0))
+		dev_warn(dev, "wait cio gate idle timeout");
+
+	/* wait for css idle signal */
+	if (imgu_hw_wait(base, IMGU_REG_STATE, IMGU_STATE_IDLE_STS,
+			 IMGU_STATE_IDLE_STS))
+		dev_warn(dev, "wait css idle timeout\n");
+
+	/* do halt-halted handshake with css */
+	writel(1, base + IMGU_REG_GP_HALT);
+	if (imgu_hw_wait(base, IMGU_REG_STATE, IMGU_STATE_HALT_STS,
+			 IMGU_STATE_HALT_STS))
+		dev_warn(dev, "failed to halt css");
+
+	/* de-assert the busy bit */
+	writel(0, base + IMGU_REG_GP_BUSY);
+}
+
+static void imgu_css_hw_enable_irq(struct imgu_css *css)
+{
+	void __iomem *const base = css->base;
+	u32 val, i;
+
+	/* Set up interrupts */
+
+	/*
+	 * Enable IRQ on the SP which signals that SP goes to idle
+	 * (aka ready state) and set trigger to pulse
+	 */
+	val = readl(base + IMGU_REG_SP_CTRL(0)) | IMGU_CTRL_IRQ_READY;
+	writel(val, base + IMGU_REG_SP_CTRL(0));
+	writel(val | IMGU_CTRL_IRQ_CLEAR, base + IMGU_REG_SP_CTRL(0));
+
+	/* Enable IRQs from the IMGU wrapper */
+	writel(IMGU_REG_INT_CSS_IRQ, base + IMGU_REG_INT_ENABLE);
+	/* Clear */
+	writel(IMGU_REG_INT_CSS_IRQ, base + IMGU_REG_INT_STATUS);
+
+	/* Enable IRQs from main IRQ controller */
+	writel(~0, base + IMGU_REG_IRQCTRL_EDGE_NOT_PULSE(IMGU_IRQCTRL_MAIN));
+	writel(0, base + IMGU_REG_IRQCTRL_MASK(IMGU_IRQCTRL_MAIN));
+	writel(IMGU_IRQCTRL_IRQ_MASK,
+	       base + IMGU_REG_IRQCTRL_EDGE(IMGU_IRQCTRL_MAIN));
+	writel(IMGU_IRQCTRL_IRQ_MASK,
+	       base + IMGU_REG_IRQCTRL_ENABLE(IMGU_IRQCTRL_MAIN));
+	writel(IMGU_IRQCTRL_IRQ_MASK,
+	       base + IMGU_REG_IRQCTRL_CLEAR(IMGU_IRQCTRL_MAIN));
+	writel(IMGU_IRQCTRL_IRQ_MASK,
+	       base + IMGU_REG_IRQCTRL_MASK(IMGU_IRQCTRL_MAIN));
+	/* Wait for write complete */
+	readl(base + IMGU_REG_IRQCTRL_ENABLE(IMGU_IRQCTRL_MAIN));
+
+	/* Enable IRQs from SP0 and SP1 controllers */
+	for (i = IMGU_IRQCTRL_SP0; i <= IMGU_IRQCTRL_SP1; i++) {
+		writel(~0, base + IMGU_REG_IRQCTRL_EDGE_NOT_PULSE(i));
+		writel(0, base + IMGU_REG_IRQCTRL_MASK(i));
+		writel(IMGU_IRQCTRL_IRQ_MASK, base + IMGU_REG_IRQCTRL_EDGE(i));
+		writel(IMGU_IRQCTRL_IRQ_MASK,
+		       base + IMGU_REG_IRQCTRL_ENABLE(i));
+		writel(IMGU_IRQCTRL_IRQ_MASK, base + IMGU_REG_IRQCTRL_CLEAR(i));
+		writel(IMGU_IRQCTRL_IRQ_MASK, base + IMGU_REG_IRQCTRL_MASK(i));
+		/* Wait for write complete */
+		readl(base + IMGU_REG_IRQCTRL_ENABLE(i));
+	}
+}
+
+static int imgu_css_hw_init(struct imgu_css *css)
+{
+	/* For checking that streaming monitor statuses are valid */
+	static const struct {
+		u32 reg;
+		u32 mask;
+		const char *name;
+	} stream_monitors[] = {
+		{
+			IMGU_REG_GP_SP1_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_ISP_PORT_SP12ISP,
+			"ISP0 to SP0"
+		}, {
+			IMGU_REG_GP_ISP_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_SP1_PORT_ISP2SP1,
+			"SP0 to ISP0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_ISP2DMA,
+			"ISP0 to DMA0"
+		}, {
+			IMGU_REG_GP_ISP_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_ISP_PORT_DMA2ISP,
+			"DMA0 to ISP0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_CELLS2GDC,
+			"ISP0 to GDC0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_GDC2CELLS,
+			"GDC0 to ISP0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_SP12DMA,
+			"SP0 to DMA0"
+		}, {
+			IMGU_REG_GP_SP1_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_SP1_PORT_DMA2SP1,
+			"DMA0 to SP0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_CELLS2GDC,
+			"SP0 to GDC0"
+		}, {
+			IMGU_REG_GP_MOD_STRMON_STAT,
+			IMGU_GP_STRMON_STAT_MOD_PORT_GDC2CELLS,
+			"GDC0 to SP0"
+		},
+	};
+
+	struct device *dev = css->dev;
+	void __iomem *const base = css->base;
+	u32 val, i;
+
+	/* Set instruction cache address and inv bit for ISP, SP, and SP1 */
+	for (i = 0; i < IMGU_NUM_SP; i++) {
+		struct imgu_fw_info *bi =
+					&css->fwp->binary_header[css->fw_sp[i]];
+
+		writel(css->binary[css->fw_sp[i]].daddr,
+		       base + IMGU_REG_SP_ICACHE_ADDR(bi->type));
+		writel(readl(base + IMGU_REG_SP_CTRL(bi->type)) |
+		       IMGU_CTRL_ICACHE_INV,
+		       base + IMGU_REG_SP_CTRL(bi->type));
+	}
+	writel(css->binary[css->fw_bl].daddr, base + IMGU_REG_ISP_ICACHE_ADDR);
+	writel(readl(base + IMGU_REG_ISP_CTRL) | IMGU_CTRL_ICACHE_INV,
+	       base + IMGU_REG_ISP_CTRL);
+
+	/* Check that IMGU hardware is ready */
+
+	if (!(readl(base + IMGU_REG_SP_CTRL(0)) & IMGU_CTRL_IDLE)) {
+		dev_err(dev, "SP is not idle\n");
+		return -EIO;
+	}
+	if (!(readl(base + IMGU_REG_ISP_CTRL) & IMGU_CTRL_IDLE)) {
+		dev_err(dev, "ISP is not idle\n");
+		return -EIO;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(stream_monitors); i++) {
+		val = readl(base + stream_monitors[i].reg);
+		if (val & stream_monitors[i].mask) {
+			dev_err(dev, "error: Stream monitor %s is valid\n",
+				stream_monitors[i].name);
+			return -EIO;
+		}
+	}
+
+	/* Initialize GDC with default values */
+
+	for (i = 0; i < ARRAY_SIZE(imgu_css_gdc_lut[0]); i++) {
+		u32 val0 = imgu_css_gdc_lut[0][i] & IMGU_GDC_LUT_MASK;
+		u32 val1 = imgu_css_gdc_lut[1][i] & IMGU_GDC_LUT_MASK;
+		u32 val2 = imgu_css_gdc_lut[2][i] & IMGU_GDC_LUT_MASK;
+		u32 val3 = imgu_css_gdc_lut[3][i] & IMGU_GDC_LUT_MASK;
+
+		writel(val0 | (val1 << 16),
+		       base + IMGU_REG_GDC_LUT_BASE + i * 8);
+		writel(val2 | (val3 << 16),
+		       base + IMGU_REG_GDC_LUT_BASE + i * 8 + 4);
+	}
+
+	return 0;
+}
+
+/* Boot the given IPU3 CSS SP */
+static int imgu_css_hw_start_sp(struct imgu_css *css, int sp)
+{
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[sp]];
+	struct imgu_abi_sp_init_dmem_cfg dmem_cfg = {
+		.ddr_data_addr = css->binary[css->fw_sp[sp]].daddr
+			+ bi->blob.data_source,
+		.dmem_data_addr = bi->blob.data_target,
+		.dmem_bss_addr = bi->blob.bss_target,
+		.data_size = bi->blob.data_size,
+		.bss_size = bi->blob.bss_size,
+		.sp_id = sp,
+	};
+
+	writes(&dmem_cfg, sizeof(dmem_cfg), base +
+	       IMGU_REG_SP_DMEM_BASE(sp) + bi->info.sp.init_dmem_data);
+
+	writel(bi->info.sp.sp_entry, base + IMGU_REG_SP_START_ADDR(sp));
+
+	writel(readl(base + IMGU_REG_SP_CTRL(sp))
+		| IMGU_CTRL_START | IMGU_CTRL_RUN, base + IMGU_REG_SP_CTRL(sp));
+
+	if (imgu_hw_wait(css->base, IMGU_REG_SP_DMEM_BASE(sp)
+			 + bi->info.sp.sw_state,
+			 ~0, IMGU_ABI_SP_SWSTATE_INITIALIZED))
+		return -EIO;
+
+	return 0;
+}
+
+/* Start the IPU3 CSS ImgU (Imaging Unit) and all the SPs */
+static int imgu_css_hw_start(struct imgu_css *css)
+{
+	static const u32 event_mask =
+		((1 << IMGU_ABI_EVTTYPE_OUT_FRAME_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_2ND_OUT_FRAME_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_VF_OUT_FRAME_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_2ND_VF_OUT_FRAME_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_3A_STATS_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_DIS_STATS_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_PIPELINE_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_FRAME_TAGGED) |
+		(1 << IMGU_ABI_EVTTYPE_INPUT_FRAME_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_METADATA_DONE) |
+		(1 << IMGU_ABI_EVTTYPE_ACC_STAGE_COMPLETE))
+		<< IMGU_ABI_SP_COMM_EVENT_IRQ_MASK_OR_SHIFT;
+
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi, *bl = &css->fwp->binary_header[css->fw_bl];
+	unsigned int i;
+
+	writel(IMGU_TLB_INVALIDATE, base + IMGU_REG_TLB_INVALIDATE);
+
+	/* Start bootloader */
+
+	writel(IMGU_ABI_BL_SWSTATE_BUSY,
+	       base + IMGU_REG_ISP_DMEM_BASE + bl->info.bl.sw_state);
+	writel(IMGU_NUM_SP,
+	       base + IMGU_REG_ISP_DMEM_BASE + bl->info.bl.num_dma_cmds);
+
+	for (i = 0; i < IMGU_NUM_SP; i++) {
+		int j = IMGU_NUM_SP - i - 1;	/* load sp1 first, then sp0 */
+		struct imgu_fw_info *sp =
+					&css->fwp->binary_header[css->fw_sp[j]];
+		struct imgu_abi_bl_dma_cmd_entry dma_cmd = {
+			.src_addr = css->binary[css->fw_sp[j]].daddr
+				+ sp->blob.text_source,
+			.size = sp->blob.text_size,
+			.dst_type = IMGU_ABI_BL_DMACMD_TYPE_SP_PMEM,
+			.dst_addr = IMGU_SP_PMEM_BASE(j),
+		};
+
+		writes(&dma_cmd, sizeof(dma_cmd),
+		       base + IMGU_REG_ISP_DMEM_BASE + i * sizeof(dma_cmd) +
+		       bl->info.bl.dma_cmd_list);
+	}
+
+	writel(bl->info.bl.bl_entry, base + IMGU_REG_ISP_START_ADDR);
+
+	writel(readl(base + IMGU_REG_ISP_CTRL)
+		| IMGU_CTRL_START | IMGU_CTRL_RUN, base + IMGU_REG_ISP_CTRL);
+	if (imgu_hw_wait(css->base, IMGU_REG_ISP_DMEM_BASE
+			 + bl->info.bl.sw_state, ~0,
+			 IMGU_ABI_BL_SWSTATE_OK)) {
+		dev_err(css->dev, "failed to start bootloader\n");
+		return -EIO;
+	}
+
+	/* Start ISP */
+
+	memset(css->xmem_sp_group_ptrs.vaddr, 0,
+	       sizeof(struct imgu_abi_sp_group));
+
+	bi = &css->fwp->binary_header[css->fw_sp[0]];
+
+	writel(css->xmem_sp_group_ptrs.daddr,
+	       base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.per_frame_data);
+
+	writel(IMGU_ABI_SP_SWSTATE_TERMINATED,
+	       base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.sw_state);
+	writel(1, base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.invalidate_tlb);
+
+	if (imgu_css_hw_start_sp(css, 0))
+		return -EIO;
+
+	writel(0, base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.isp_started);
+	writel(0, base + IMGU_REG_SP_DMEM_BASE(0) +
+		bi->info.sp.host_sp_queues_initialized);
+	writel(0, base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.sleep_mode);
+	writel(0, base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.invalidate_tlb);
+	writel(IMGU_ABI_SP_COMM_COMMAND_READY, base + IMGU_REG_SP_DMEM_BASE(0)
+		+ bi->info.sp.host_sp_com + IMGU_ABI_SP_COMM_COMMAND);
+
+	/* Enable all events for all queues */
+
+	for (i = 0; i < IPU3_CSS_PIPE_ID_NUM; i++)
+		writel(event_mask, base + IMGU_REG_SP_DMEM_BASE(0)
+			+ bi->info.sp.host_sp_com
+			+ IMGU_ABI_SP_COMM_EVENT_IRQ_MASK(i));
+	writel(1, base + IMGU_REG_SP_DMEM_BASE(0) +
+		bi->info.sp.host_sp_queues_initialized);
+
+	/* Start SP1 */
+
+	bi = &css->fwp->binary_header[css->fw_sp[1]];
+
+	writel(IMGU_ABI_SP_SWSTATE_TERMINATED,
+	       base + IMGU_REG_SP_DMEM_BASE(1) + bi->info.sp.sw_state);
+
+	if (imgu_css_hw_start_sp(css, 1))
+		return -EIO;
+
+	writel(IMGU_ABI_SP_COMM_COMMAND_READY, base + IMGU_REG_SP_DMEM_BASE(1)
+		+ bi->info.sp.host_sp_com + IMGU_ABI_SP_COMM_COMMAND);
+
+	return 0;
+}
+
+static void imgu_css_hw_stop(struct imgu_css *css)
+{
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[0]];
+
+	/* Stop fw */
+	writel(IMGU_ABI_SP_COMM_COMMAND_TERMINATE,
+	       base + IMGU_REG_SP_DMEM_BASE(0) +
+	       bi->info.sp.host_sp_com + IMGU_ABI_SP_COMM_COMMAND);
+	if (imgu_hw_wait(css->base, IMGU_REG_SP_CTRL(0),
+			 IMGU_CTRL_IDLE, IMGU_CTRL_IDLE))
+		dev_err(css->dev, "wait sp0 idle timeout.\n");
+	if (readl(base + IMGU_REG_SP_DMEM_BASE(0) + bi->info.sp.sw_state) !=
+		  IMGU_ABI_SP_SWSTATE_TERMINATED)
+		dev_err(css->dev, "sp0 is not terminated.\n");
+	if (imgu_hw_wait(css->base, IMGU_REG_ISP_CTRL,
+			 IMGU_CTRL_IDLE, IMGU_CTRL_IDLE))
+		dev_err(css->dev, "wait isp idle timeout\n");
+}
+
+static void imgu_css_hw_cleanup(struct imgu_css *css)
+{
+	void __iomem *const base = css->base;
+
+	/** Reset CSS **/
+
+	/* Clear the CSS busy signal */
+	readl(base + IMGU_REG_GP_BUSY);
+	writel(0, base + IMGU_REG_GP_BUSY);
+
+	/* Wait for idle signal */
+	if (imgu_hw_wait(css->base, IMGU_REG_STATE, IMGU_STATE_IDLE_STS,
+			 IMGU_STATE_IDLE_STS))
+		dev_err(css->dev, "failed to shut down hw cleanly\n");
+
+	/* Reset the css */
+	writel(readl(base + IMGU_REG_PM_CTRL) | IMGU_PM_CTRL_FORCE_RESET,
+	       base + IMGU_REG_PM_CTRL);
+
+	usleep_range(200, 300);
+}
+
+static void imgu_css_pipeline_cleanup(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	unsigned int i;
+
+	imgu_css_pool_cleanup(imgu, &css_pipe->pool.parameter_set_info);
+	imgu_css_pool_cleanup(imgu, &css_pipe->pool.acc);
+	imgu_css_pool_cleanup(imgu, &css_pipe->pool.gdc);
+	imgu_css_pool_cleanup(imgu, &css_pipe->pool.obgrid);
+
+	for (i = 0; i < IMGU_ABI_NUM_MEMORIES; i++)
+		imgu_css_pool_cleanup(imgu, &css_pipe->pool.binary_params_p[i]);
+}
+
+/*
+ * This function initializes various stages of the
+ * IPU3 CSS ISP pipeline
+ */
+static int imgu_css_pipeline_init(struct imgu_css *css, unsigned int pipe)
+{
+	static const int BYPC = 2;	/* Bytes per component */
+	static const struct imgu_abi_buffer_sp buffer_sp_init = {
+		.buf_src = {.queue_id = IMGU_ABI_QUEUE_EVENT_ID},
+		.buf_type = IMGU_ABI_BUFFER_TYPE_INVALID,
+	};
+
+	struct imgu_abi_isp_iterator_config *cfg_iter;
+	struct imgu_abi_isp_ref_config *cfg_ref;
+	struct imgu_abi_isp_dvs_config *cfg_dvs;
+	struct imgu_abi_isp_tnr3_config *cfg_tnr;
+	struct imgu_abi_isp_ref_dmem_state *cfg_ref_state;
+	struct imgu_abi_isp_tnr3_dmem_state *cfg_tnr_state;
+
+	const int stage = 0;
+	unsigned int i, j;
+
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	struct imgu_css_queue *css_queue_in =
+			&css_pipe->queue[IPU3_CSS_QUEUE_IN];
+	struct imgu_css_queue *css_queue_out =
+			&css_pipe->queue[IPU3_CSS_QUEUE_OUT];
+	struct imgu_css_queue *css_queue_vf =
+			&css_pipe->queue[IPU3_CSS_QUEUE_VF];
+	const struct imgu_fw_info *bi =
+			&css->fwp->binary_header[css_pipe->bindex];
+	const unsigned int stripes = bi->info.isp.sp.iterator.num_stripes;
+
+	struct imgu_fw_config_memory_offsets *cofs = (void *)css->fwp +
+		bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_CONFIG];
+	struct imgu_fw_state_memory_offsets *sofs = (void *)css->fwp +
+		bi->blob.memory_offsets.offsets[IMGU_ABI_PARAM_CLASS_STATE];
+
+	struct imgu_abi_isp_stage *isp_stage;
+	struct imgu_abi_sp_stage *sp_stage;
+	struct imgu_abi_sp_group *sp_group;
+	struct imgu_abi_frames_sp *frames_sp;
+	struct imgu_abi_frame_sp *frame_sp;
+	struct imgu_abi_frame_sp_info *frame_sp_info;
+
+	const unsigned int bds_width_pad =
+				ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width,
+				      2 * IPU3_UAPI_ISP_VEC_ELEMS);
+
+	const enum imgu_abi_memories m0 = IMGU_ABI_MEM_ISP_DMEM0;
+	enum imgu_abi_param_class cfg = IMGU_ABI_PARAM_CLASS_CONFIG;
+	void *vaddr = css_pipe->binary_params_cs[cfg - 1][m0].vaddr;
+
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+
+	dev_dbg(css->dev, "%s for pipe %d", __func__, pipe);
+
+	/* Configure iterator */
+
+	cfg_iter = imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+					       &cofs->dmem.iterator,
+					       sizeof(*cfg_iter), vaddr);
+	if (!cfg_iter)
+		goto bad_firmware;
+
+	frame_sp_info = &cfg_iter->input_info;
+	frame_sp_info->res.width	= css_queue_in->fmt.mpix.width;
+	frame_sp_info->res.height	= css_queue_in->fmt.mpix.height;
+	frame_sp_info->padded_width	= css_queue_in->width_pad;
+	frame_sp_info->format		= css_queue_in->css_fmt->frame_format;
+	frame_sp_info->raw_bit_depth	= css_queue_in->css_fmt->bit_depth;
+	frame_sp_info->raw_bayer_order	= css_queue_in->css_fmt->bayer_order;
+	frame_sp_info->raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+
+	frame_sp_info = &cfg_iter->internal_info;
+	frame_sp_info->res.width = css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	frame_sp_info->res.height = css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	frame_sp_info->padded_width	= bds_width_pad;
+	frame_sp_info->format		= css_queue_out->css_fmt->frame_format;
+	frame_sp_info->raw_bit_depth	= css_queue_out->css_fmt->bit_depth;
+	frame_sp_info->raw_bayer_order	= css_queue_out->css_fmt->bayer_order;
+	frame_sp_info->raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+
+	frame_sp_info = &cfg_iter->output_info;
+	frame_sp_info->res.width	= css_queue_out->fmt.mpix.width;
+	frame_sp_info->res.height	= css_queue_out->fmt.mpix.height;
+	frame_sp_info->padded_width	= css_queue_out->width_pad;
+	frame_sp_info->format		= css_queue_out->css_fmt->frame_format;
+	frame_sp_info->raw_bit_depth	= css_queue_out->css_fmt->bit_depth;
+	frame_sp_info->raw_bayer_order	= css_queue_out->css_fmt->bayer_order;
+	frame_sp_info->raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+
+	frame_sp_info = &cfg_iter->vf_info;
+	frame_sp_info->res.width	= css_queue_vf->fmt.mpix.width;
+	frame_sp_info->res.height	= css_queue_vf->fmt.mpix.height;
+	frame_sp_info->padded_width	= css_queue_vf->width_pad;
+	frame_sp_info->format		= css_queue_vf->css_fmt->frame_format;
+	frame_sp_info->raw_bit_depth	= css_queue_vf->css_fmt->bit_depth;
+	frame_sp_info->raw_bayer_order	= css_queue_vf->css_fmt->bayer_order;
+	frame_sp_info->raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+
+	cfg_iter->dvs_envelope.width =
+				css_pipe->rect[IPU3_CSS_RECT_ENVELOPE].width;
+	cfg_iter->dvs_envelope.height =
+				css_pipe->rect[IPU3_CSS_RECT_ENVELOPE].height;
+
+	/* Configure reference (delay) frames */
+
+	cfg_ref = imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+					      &cofs->dmem.ref,
+					      sizeof(*cfg_ref), vaddr);
+	if (!cfg_ref)
+		goto bad_firmware;
+
+	cfg_ref->port_b.crop = 0;
+	cfg_ref->port_b.elems = IMGU_ABI_ISP_DDR_WORD_BYTES / BYPC;
+	cfg_ref->port_b.width =
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].width;
+	cfg_ref->port_b.stride =
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperline;
+	cfg_ref->width_a_over_b =
+				IPU3_UAPI_ISP_VEC_ELEMS / cfg_ref->port_b.elems;
+	cfg_ref->dvs_frame_delay = IPU3_CSS_AUX_FRAMES - 1;
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++) {
+		cfg_ref->ref_frame_addr_y[i] =
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].mem[i].daddr;
+		cfg_ref->ref_frame_addr_c[i] =
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].mem[i].daddr +
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperline *
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].height;
+	}
+	for (; i < IMGU_ABI_FRAMES_REF; i++) {
+		cfg_ref->ref_frame_addr_y[i] = 0;
+		cfg_ref->ref_frame_addr_c[i] = 0;
+	}
+
+	/* Configure DVS (digital video stabilization) */
+
+	cfg_dvs = imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+					      &cofs->dmem.dvs, sizeof(*cfg_dvs),
+					      vaddr);
+	if (!cfg_dvs)
+		goto bad_firmware;
+
+	cfg_dvs->num_horizontal_blocks =
+			ALIGN(DIV_ROUND_UP(css_pipe->rect[IPU3_CSS_RECT_GDC].width,
+					   IMGU_DVS_BLOCK_W), 2);
+	cfg_dvs->num_vertical_blocks =
+			DIV_ROUND_UP(css_pipe->rect[IPU3_CSS_RECT_GDC].height,
+				     IMGU_DVS_BLOCK_H);
+
+	/* Configure TNR (temporal noise reduction) */
+
+	if (css_pipe->pipe_id == IPU3_CSS_PIPE_ID_VIDEO) {
+		cfg_tnr = imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+						      &cofs->dmem.tnr3,
+						      sizeof(*cfg_tnr),
+						      vaddr);
+		if (!cfg_tnr)
+			goto bad_firmware;
+
+		cfg_tnr->port_b.crop = 0;
+		cfg_tnr->port_b.elems = IMGU_ABI_ISP_DDR_WORD_BYTES;
+		cfg_tnr->port_b.width =
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].width;
+		cfg_tnr->port_b.stride =
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].bytesperline;
+		cfg_tnr->width_a_over_b =
+			IPU3_UAPI_ISP_VEC_ELEMS / cfg_tnr->port_b.elems;
+		cfg_tnr->frame_height =
+			css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].height;
+		cfg_tnr->delay_frame = IPU3_CSS_AUX_FRAMES - 1;
+		for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+			cfg_tnr->frame_addr[i] =
+				css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR]
+					.mem[i].daddr;
+		for (; i < IMGU_ABI_FRAMES_TNR; i++)
+			cfg_tnr->frame_addr[i] = 0;
+	}
+
+	/* Configure ref dmem state parameters */
+
+	cfg = IMGU_ABI_PARAM_CLASS_STATE;
+	vaddr = css_pipe->binary_params_cs[cfg - 1][m0].vaddr;
+
+	cfg_ref_state = imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+						    &sofs->dmem.ref,
+						    sizeof(*cfg_ref_state),
+						    vaddr);
+	if (!cfg_ref_state)
+		goto bad_firmware;
+
+	cfg_ref_state->ref_in_buf_idx = 0;
+	cfg_ref_state->ref_out_buf_idx = 1;
+
+	/* Configure tnr dmem state parameters */
+	if (css_pipe->pipe_id == IPU3_CSS_PIPE_ID_VIDEO) {
+		cfg_tnr_state =
+			imgu_css_fw_pipeline_params(css, pipe, cfg, m0,
+						    &sofs->dmem.tnr3,
+						    sizeof(*cfg_tnr_state),
+						    vaddr);
+		if (!cfg_tnr_state)
+			goto bad_firmware;
+
+		cfg_tnr_state->in_bufidx = 0;
+		cfg_tnr_state->out_bufidx = 1;
+		cfg_tnr_state->bypass_filter = 0;
+		cfg_tnr_state->total_frame_counter = 0;
+		for (i = 0; i < IMGU_ABI_BUF_SETS_TNR; i++)
+			cfg_tnr_state->buffer_frame_counter[i] = 0;
+	}
+
+	/* Configure ISP stage */
+
+	isp_stage = css_pipe->xmem_isp_stage_ptrs[pipe][stage].vaddr;
+	memset(isp_stage, 0, sizeof(*isp_stage));
+	isp_stage->blob_info = bi->blob;
+	isp_stage->binary_info = bi->info.isp.sp;
+	strscpy(isp_stage->binary_name,
+		(char *)css->fwp + bi->blob.prog_name_offset,
+		sizeof(isp_stage->binary_name));
+	isp_stage->mem_initializers = bi->info.isp.sp.mem_initializers;
+	for (i = IMGU_ABI_PARAM_CLASS_CONFIG; i < IMGU_ABI_PARAM_CLASS_NUM; i++)
+		for (j = 0; j < IMGU_ABI_NUM_MEMORIES; j++)
+			isp_stage->mem_initializers.params[i][j].address =
+					css_pipe->binary_params_cs[i - 1][j].daddr;
+
+	/* Configure SP stage */
+
+	sp_stage = css_pipe->xmem_sp_stage_ptrs[pipe][stage].vaddr;
+	memset(sp_stage, 0, sizeof(*sp_stage));
+
+	frames_sp = &sp_stage->frames;
+	frames_sp->in.buf_attr = buffer_sp_init;
+	for (i = 0; i < IMGU_ABI_BINARY_MAX_OUTPUT_PORTS; i++)
+		frames_sp->out[i].buf_attr = buffer_sp_init;
+	frames_sp->out_vf.buf_attr = buffer_sp_init;
+	frames_sp->s3a_buf = buffer_sp_init;
+	frames_sp->dvs_buf = buffer_sp_init;
+
+	sp_stage->stage_type = IMGU_ABI_STAGE_TYPE_ISP;
+	sp_stage->num = stage;
+	sp_stage->isp_online = 0;
+	sp_stage->isp_copy_vf = 0;
+	sp_stage->isp_copy_output = 0;
+
+	sp_stage->enable.vf_output = css_pipe->vf_output_en;
+
+	frames_sp->effective_in_res.width =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].width;
+	frames_sp->effective_in_res.height =
+				css_pipe->rect[IPU3_CSS_RECT_EFFECTIVE].height;
+
+	frame_sp = &frames_sp->in;
+	frame_sp->info.res.width	= css_queue_in->fmt.mpix.width;
+	frame_sp->info.res.height	= css_queue_in->fmt.mpix.height;
+	frame_sp->info.padded_width	= css_queue_in->width_pad;
+	frame_sp->info.format		= css_queue_in->css_fmt->frame_format;
+	frame_sp->info.raw_bit_depth	= css_queue_in->css_fmt->bit_depth;
+	frame_sp->info.raw_bayer_order	= css_queue_in->css_fmt->bayer_order;
+	frame_sp->info.raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+	frame_sp->buf_attr.buf_src.queue_id = IMGU_ABI_QUEUE_C_ID;
+	frame_sp->buf_attr.buf_type	= IMGU_ABI_BUFFER_TYPE_INPUT_FRAME;
+
+	frame_sp = &frames_sp->out[0];
+	frame_sp->info.res.width	= css_queue_out->fmt.mpix.width;
+	frame_sp->info.res.height	= css_queue_out->fmt.mpix.height;
+	frame_sp->info.padded_width	= css_queue_out->width_pad;
+	frame_sp->info.format		= css_queue_out->css_fmt->frame_format;
+	frame_sp->info.raw_bit_depth	= css_queue_out->css_fmt->bit_depth;
+	frame_sp->info.raw_bayer_order	= css_queue_out->css_fmt->bayer_order;
+	frame_sp->info.raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+	frame_sp->planes.nv.uv.offset	= css_queue_out->width_pad *
+					  css_queue_out->fmt.mpix.height;
+	frame_sp->buf_attr.buf_src.queue_id = IMGU_ABI_QUEUE_D_ID;
+	frame_sp->buf_attr.buf_type	= IMGU_ABI_BUFFER_TYPE_OUTPUT_FRAME;
+
+	frame_sp = &frames_sp->out[1];
+	frame_sp->buf_attr.buf_src.queue_id = IMGU_ABI_QUEUE_EVENT_ID;
+
+	frame_sp_info = &frames_sp->internal_frame_info;
+	frame_sp_info->res.width = css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	frame_sp_info->res.height = css_pipe->rect[IPU3_CSS_RECT_BDS].height;
+	frame_sp_info->padded_width	= bds_width_pad;
+	frame_sp_info->format		= css_queue_out->css_fmt->frame_format;
+	frame_sp_info->raw_bit_depth	= css_queue_out->css_fmt->bit_depth;
+	frame_sp_info->raw_bayer_order	= css_queue_out->css_fmt->bayer_order;
+	frame_sp_info->raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+
+	frame_sp = &frames_sp->out_vf;
+	frame_sp->info.res.width	= css_queue_vf->fmt.mpix.width;
+	frame_sp->info.res.height	= css_queue_vf->fmt.mpix.height;
+	frame_sp->info.padded_width	= css_queue_vf->width_pad;
+	frame_sp->info.format		= css_queue_vf->css_fmt->frame_format;
+	frame_sp->info.raw_bit_depth	= css_queue_vf->css_fmt->bit_depth;
+	frame_sp->info.raw_bayer_order	= css_queue_vf->css_fmt->bayer_order;
+	frame_sp->info.raw_type		= IMGU_ABI_RAW_TYPE_BAYER;
+	frame_sp->planes.yuv.u.offset	= css_queue_vf->width_pad *
+					  css_queue_vf->fmt.mpix.height;
+	frame_sp->planes.yuv.v.offset	= css_queue_vf->width_pad *
+					  css_queue_vf->fmt.mpix.height * 5 / 4;
+	frame_sp->buf_attr.buf_src.queue_id = IMGU_ABI_QUEUE_E_ID;
+	frame_sp->buf_attr.buf_type	= IMGU_ABI_BUFFER_TYPE_VF_OUTPUT_FRAME;
+
+	frames_sp->s3a_buf.buf_src.queue_id = IMGU_ABI_QUEUE_F_ID;
+	frames_sp->s3a_buf.buf_type	= IMGU_ABI_BUFFER_TYPE_3A_STATISTICS;
+
+	frames_sp->dvs_buf.buf_src.queue_id = IMGU_ABI_QUEUE_G_ID;
+	frames_sp->dvs_buf.buf_type	= IMGU_ABI_BUFFER_TYPE_DIS_STATISTICS;
+
+	sp_stage->dvs_envelope.width =
+				css_pipe->rect[IPU3_CSS_RECT_ENVELOPE].width;
+	sp_stage->dvs_envelope.height =
+				css_pipe->rect[IPU3_CSS_RECT_ENVELOPE].height;
+
+	sp_stage->isp_pipe_version =
+				bi->info.isp.sp.pipeline.isp_pipe_version;
+	sp_stage->isp_deci_log_factor =
+			clamp(max(fls(css_pipe->rect[IPU3_CSS_RECT_BDS].width /
+				      IMGU_MAX_BQ_GRID_WIDTH),
+				  fls(css_pipe->rect[IPU3_CSS_RECT_BDS].height /
+				      IMGU_MAX_BQ_GRID_HEIGHT)) - 1, 3, 5);
+	sp_stage->isp_vf_downscale_bits = 0;
+	sp_stage->if_config_index = 255;
+	sp_stage->sp_enable_xnr = 0;
+	sp_stage->num_stripes = stripes;
+	sp_stage->enable.s3a = 1;
+	sp_stage->enable.dvs_stats = 0;
+
+	sp_stage->xmem_bin_addr = css->binary[css_pipe->bindex].daddr;
+	sp_stage->xmem_map_addr = css_pipe->sp_ddr_ptrs.daddr;
+	sp_stage->isp_stage_addr =
+		css_pipe->xmem_isp_stage_ptrs[pipe][stage].daddr;
+
+	/* Configure SP group */
+
+	sp_group = css->xmem_sp_group_ptrs.vaddr;
+	memset(&sp_group->pipe[pipe], 0, sizeof(struct imgu_abi_sp_pipeline));
+
+	sp_group->pipe[pipe].num_stages = 1;
+	sp_group->pipe[pipe].pipe_id = css_pipe->pipe_id;
+	sp_group->pipe[pipe].thread_id = pipe;
+	sp_group->pipe[pipe].pipe_num = pipe;
+	sp_group->pipe[pipe].num_execs = -1;
+	sp_group->pipe[pipe].pipe_qos_config = -1;
+	sp_group->pipe[pipe].required_bds_factor = 0;
+	sp_group->pipe[pipe].dvs_frame_delay = IPU3_CSS_AUX_FRAMES - 1;
+	sp_group->pipe[pipe].inout_port_config =
+					IMGU_ABI_PORT_CONFIG_TYPE_INPUT_HOST |
+					IMGU_ABI_PORT_CONFIG_TYPE_OUTPUT_HOST;
+	sp_group->pipe[pipe].scaler_pp_lut = 0;
+	sp_group->pipe[pipe].shading.internal_frame_origin_x_bqs_on_sctbl = 0;
+	sp_group->pipe[pipe].shading.internal_frame_origin_y_bqs_on_sctbl = 0;
+	sp_group->pipe[pipe].sp_stage_addr[stage] =
+			css_pipe->xmem_sp_stage_ptrs[pipe][stage].daddr;
+	sp_group->pipe[pipe].pipe_config =
+			bi->info.isp.sp.enable.params ? (1 << pipe) : 0;
+	sp_group->pipe[pipe].pipe_config |= IMGU_ABI_PIPE_CONFIG_ACQUIRE_ISP;
+
+	/* Initialize parameter pools */
+
+	if (imgu_css_pool_init(imgu, &css_pipe->pool.parameter_set_info,
+			       sizeof(struct imgu_abi_parameter_set_info)) ||
+	    imgu_css_pool_init(imgu, &css_pipe->pool.acc,
+			       sizeof(struct imgu_abi_acc_param)) ||
+	    imgu_css_pool_init(imgu, &css_pipe->pool.gdc,
+			       sizeof(struct imgu_abi_gdc_warp_param) *
+			       3 * cfg_dvs->num_horizontal_blocks / 2 *
+			       cfg_dvs->num_vertical_blocks) ||
+	    imgu_css_pool_init(imgu, &css_pipe->pool.obgrid,
+			       imgu_css_fw_obgrid_size(
+			       &css->fwp->binary_header[css_pipe->bindex])))
+		goto out_of_memory;
+
+	for (i = 0; i < IMGU_ABI_NUM_MEMORIES; i++)
+		if (imgu_css_pool_init(imgu,
+				       &css_pipe->pool.binary_params_p[i],
+				       bi->info.isp.sp.mem_initializers.params
+				       [IMGU_ABI_PARAM_CLASS_PARAM][i].size))
+			goto out_of_memory;
+
+	return 0;
+
+bad_firmware:
+	imgu_css_pipeline_cleanup(css, pipe);
+	return -EPROTO;
+
+out_of_memory:
+	imgu_css_pipeline_cleanup(css, pipe);
+	return -ENOMEM;
+}
+
+static u8 imgu_css_queue_pos(struct imgu_css *css, int queue, int thread)
+{
+	static const unsigned int sp;
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[sp]];
+	struct imgu_abi_queues __iomem *q = base + IMGU_REG_SP_DMEM_BASE(sp) +
+						bi->info.sp.host_sp_queue;
+
+	return queue >= 0 ? readb(&q->host2sp_bufq_info[thread][queue].end) :
+			    readb(&q->host2sp_evtq_info.end);
+}
+
+/* Sent data to sp using given buffer queue, or if queue < 0, event queue. */
+static int imgu_css_queue_data(struct imgu_css *css,
+			       int queue, int thread, u32 data)
+{
+	static const unsigned int sp;
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[sp]];
+	struct imgu_abi_queues __iomem *q = base + IMGU_REG_SP_DMEM_BASE(sp) +
+						bi->info.sp.host_sp_queue;
+	u8 size, start, end, end2;
+
+	if (queue >= 0) {
+		size = readb(&q->host2sp_bufq_info[thread][queue].size);
+		start = readb(&q->host2sp_bufq_info[thread][queue].start);
+		end = readb(&q->host2sp_bufq_info[thread][queue].end);
+	} else {
+		size = readb(&q->host2sp_evtq_info.size);
+		start = readb(&q->host2sp_evtq_info.start);
+		end = readb(&q->host2sp_evtq_info.end);
+	}
+
+	if (size == 0)
+		return -EIO;
+
+	end2 = (end + 1) % size;
+	if (end2 == start)
+		return -EBUSY;	/* Queue full */
+
+	if (queue >= 0) {
+		writel(data, &q->host2sp_bufq[thread][queue][end]);
+		writeb(end2, &q->host2sp_bufq_info[thread][queue].end);
+	} else {
+		writel(data, &q->host2sp_evtq[end]);
+		writeb(end2, &q->host2sp_evtq_info.end);
+	}
+
+	return 0;
+}
+
+/* Receive data using given buffer queue, or if queue < 0, event queue. */
+static int imgu_css_dequeue_data(struct imgu_css *css, int queue, u32 *data)
+{
+	static const unsigned int sp;
+	void __iomem *const base = css->base;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[sp]];
+	struct imgu_abi_queues __iomem *q = base + IMGU_REG_SP_DMEM_BASE(sp) +
+						bi->info.sp.host_sp_queue;
+	u8 size, start, end, start2;
+
+	if (queue >= 0) {
+		size = readb(&q->sp2host_bufq_info[queue].size);
+		start = readb(&q->sp2host_bufq_info[queue].start);
+		end = readb(&q->sp2host_bufq_info[queue].end);
+	} else {
+		size = readb(&q->sp2host_evtq_info.size);
+		start = readb(&q->sp2host_evtq_info.start);
+		end = readb(&q->sp2host_evtq_info.end);
+	}
+
+	if (size == 0)
+		return -EIO;
+
+	if (end == start)
+		return -EBUSY;	/* Queue empty */
+
+	start2 = (start + 1) % size;
+
+	if (queue >= 0) {
+		*data = readl(&q->sp2host_bufq[queue][start]);
+		writeb(start2, &q->sp2host_bufq_info[queue].start);
+	} else {
+		int r;
+
+		*data = readl(&q->sp2host_evtq[start]);
+		writeb(start2, &q->sp2host_evtq_info.start);
+
+		/* Acknowledge events dequeued from event queue */
+		r = imgu_css_queue_data(css, queue, 0,
+					IMGU_ABI_EVENT_EVENT_DEQUEUED);
+		if (r < 0)
+			return r;
+	}
+
+	return 0;
+}
+
+/* Free binary-specific resources */
+static void imgu_css_binary_cleanup(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	unsigned int i, j;
+
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	for (j = 0; j < IMGU_ABI_PARAM_CLASS_NUM - 1; j++)
+		for (i = 0; i < IMGU_ABI_NUM_MEMORIES; i++)
+			imgu_dmamap_free(imgu,
+					 &css_pipe->binary_params_cs[j][i]);
+
+	j = IPU3_CSS_AUX_FRAME_REF;
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		imgu_dmamap_free(imgu,
+				 &css_pipe->aux_frames[j].mem[i]);
+
+	j = IPU3_CSS_AUX_FRAME_TNR;
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		imgu_dmamap_free(imgu,
+				 &css_pipe->aux_frames[j].mem[i]);
+}
+
+static int imgu_css_binary_preallocate(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	unsigned int i, j;
+
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	for (j = IMGU_ABI_PARAM_CLASS_CONFIG;
+	     j < IMGU_ABI_PARAM_CLASS_NUM; j++)
+		for (i = 0; i < IMGU_ABI_NUM_MEMORIES; i++)
+			if (!imgu_dmamap_alloc(imgu,
+					       &css_pipe->binary_params_cs[j - 1][i],
+					       CSS_ABI_SIZE))
+				goto out_of_memory;
+
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		if (!imgu_dmamap_alloc(imgu,
+				       &css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].
+				       mem[i], CSS_BDS_SIZE))
+			goto out_of_memory;
+
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		if (!imgu_dmamap_alloc(imgu,
+				       &css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].
+				       mem[i], CSS_GDC_SIZE))
+			goto out_of_memory;
+
+	return 0;
+
+out_of_memory:
+	imgu_css_binary_cleanup(css, pipe);
+	return -ENOMEM;
+}
+
+/* allocate binary-specific resources */
+static int imgu_css_binary_setup(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css_pipe->bindex];
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	int i, j, size;
+	static const int BYPC = 2;	/* Bytes per component */
+	unsigned int w, h;
+
+	/* Allocate parameter memory blocks for this binary */
+
+	for (j = IMGU_ABI_PARAM_CLASS_CONFIG; j < IMGU_ABI_PARAM_CLASS_NUM; j++)
+		for (i = 0; i < IMGU_ABI_NUM_MEMORIES; i++) {
+			if (imgu_css_dma_buffer_resize(
+			    imgu,
+			    &css_pipe->binary_params_cs[j - 1][i],
+			    bi->info.isp.sp.mem_initializers.params[j][i].size))
+				goto out_of_memory;
+		}
+
+	/* Allocate internal frame buffers */
+
+	/* Reference frames for DVS, FRAME_FORMAT_YUV420_16 */
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperpixel = BYPC;
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].width =
+					css_pipe->rect[IPU3_CSS_RECT_BDS].width;
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].height =
+				ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].height,
+				      IMGU_DVS_BLOCK_H) + 2 * IMGU_GDC_BUF_Y;
+	h = css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].height;
+	w = ALIGN(css_pipe->rect[IPU3_CSS_RECT_BDS].width,
+		  2 * IPU3_UAPI_ISP_VEC_ELEMS) + 2 * IMGU_GDC_BUF_X;
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperline =
+		css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].bytesperpixel * w;
+	size = w * h * BYPC + (w / 2) * (h / 2) * BYPC * 2;
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		if (imgu_css_dma_buffer_resize(
+			imgu,
+			&css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_REF].mem[i],
+			size))
+			goto out_of_memory;
+
+	/* TNR frames for temporal noise reduction, FRAME_FORMAT_YUV_LINE */
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].bytesperpixel = 1;
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].width =
+			roundup(css_pipe->rect[IPU3_CSS_RECT_GDC].width,
+				bi->info.isp.sp.block.block_width *
+				IPU3_UAPI_ISP_VEC_ELEMS);
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].height =
+			roundup(css_pipe->rect[IPU3_CSS_RECT_GDC].height,
+				bi->info.isp.sp.block.output_block_height);
+
+	w = css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].width;
+	css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].bytesperline = w;
+	h = css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].height;
+	size = w * ALIGN(h * 3 / 2 + 3, 2);	/* +3 for vf_pp prefetch */
+	for (i = 0; i < IPU3_CSS_AUX_FRAMES; i++)
+		if (imgu_css_dma_buffer_resize(
+			imgu,
+			&css_pipe->aux_frames[IPU3_CSS_AUX_FRAME_TNR].mem[i],
+			size))
+			goto out_of_memory;
+
+	return 0;
+
+out_of_memory:
+	imgu_css_binary_cleanup(css, pipe);
+	return -ENOMEM;
+}
+
+int imgu_css_start_streaming(struct imgu_css *css)
+{
+	u32 data;
+	int r, pipe;
+
+	if (css->streaming)
+		return -EPROTO;
+
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_css_binary_setup(css, pipe);
+		if (r < 0)
+			return r;
+	}
+
+	r = imgu_css_hw_init(css);
+	if (r < 0)
+		return r;
+
+	r = imgu_css_hw_start(css);
+	if (r < 0)
+		goto fail;
+
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_css_pipeline_init(css, pipe);
+		if (r < 0)
+			goto fail;
+	}
+
+	css->streaming = true;
+
+	imgu_css_hw_enable_irq(css);
+
+	/* Initialize parameters to default */
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_css_set_parameters(css, pipe, NULL);
+		if (r < 0)
+			goto fail;
+	}
+
+	while (!(r = imgu_css_dequeue_data(css, IMGU_ABI_QUEUE_A_ID, &data)))
+		;
+	if (r != -EBUSY)
+		goto fail;
+
+	while (!(r = imgu_css_dequeue_data(css, IMGU_ABI_QUEUE_B_ID, &data)))
+		;
+	if (r != -EBUSY)
+		goto fail;
+
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe,
+					IMGU_ABI_EVENT_START_STREAM |
+					pipe << 16);
+		if (r < 0)
+			goto fail;
+	}
+
+	return 0;
+
+fail:
+	css->streaming = false;
+	imgu_css_hw_cleanup(css);
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		imgu_css_pipeline_cleanup(css, pipe);
+		imgu_css_binary_cleanup(css, pipe);
+	}
+
+	return r;
+}
+
+void imgu_css_stop_streaming(struct imgu_css *css)
+{
+	struct imgu_css_buffer *b, *b0;
+	int q, r, pipe;
+
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe,
+					IMGU_ABI_EVENT_STOP_STREAM);
+		if (r < 0)
+			dev_warn(css->dev, "failed on stop stream event\n");
+	}
+
+	if (!css->streaming)
+		return;
+
+	imgu_css_hw_stop(css);
+
+	imgu_css_hw_cleanup(css);
+
+	for_each_set_bit(pipe, css->enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+		imgu_css_pipeline_cleanup(css, pipe);
+
+		spin_lock(&css_pipe->qlock);
+		for (q = 0; q < IPU3_CSS_QUEUES; q++)
+			list_for_each_entry_safe(b, b0,
+						 &css_pipe->queue[q].bufs,
+						 list) {
+				b->state = IPU3_CSS_BUFFER_FAILED;
+				list_del(&b->list);
+			}
+		spin_unlock(&css_pipe->qlock);
+	}
+
+	css->streaming = false;
+}
+
+bool imgu_css_pipe_queue_empty(struct imgu_css *css, unsigned int pipe)
+{
+	int q;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	spin_lock(&css_pipe->qlock);
+	for (q = 0; q < IPU3_CSS_QUEUES; q++)
+		if (!list_empty(&css_pipe->queue[q].bufs))
+			break;
+	spin_unlock(&css_pipe->qlock);
+	return (q == IPU3_CSS_QUEUES);
+}
+
+bool imgu_css_queue_empty(struct imgu_css *css)
+{
+	unsigned int pipe;
+	bool ret = false;
+
+	for (pipe = 0; pipe < IMGU_MAX_PIPE_NUM; pipe++)
+		ret &= imgu_css_pipe_queue_empty(css, pipe);
+
+	return ret;
+}
+
+bool imgu_css_is_streaming(struct imgu_css *css)
+{
+	return css->streaming;
+}
+
+static int imgu_css_map_init(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	unsigned int p, q, i;
+
+	/* Allocate and map common structures with imgu hardware */
+	for (p = 0; p < IPU3_CSS_PIPE_ID_NUM; p++)
+		for (i = 0; i < IMGU_ABI_MAX_STAGES; i++) {
+			if (!imgu_dmamap_alloc(imgu,
+					       &css_pipe->
+					       xmem_sp_stage_ptrs[p][i],
+					       sizeof(struct imgu_abi_sp_stage)))
+				return -ENOMEM;
+			if (!imgu_dmamap_alloc(imgu,
+					       &css_pipe->
+					       xmem_isp_stage_ptrs[p][i],
+					       sizeof(struct imgu_abi_isp_stage)))
+				return -ENOMEM;
+		}
+
+	if (!imgu_dmamap_alloc(imgu, &css_pipe->sp_ddr_ptrs,
+			       ALIGN(sizeof(struct imgu_abi_ddr_address_map),
+				     IMGU_ABI_ISP_DDR_WORD_BYTES)))
+		return -ENOMEM;
+
+	for (q = 0; q < IPU3_CSS_QUEUES; q++) {
+		unsigned int abi_buf_num = ARRAY_SIZE(css_pipe->abi_buffers[q]);
+
+		for (i = 0; i < abi_buf_num; i++)
+			if (!imgu_dmamap_alloc(imgu,
+					       &css_pipe->abi_buffers[q][i],
+					       sizeof(struct imgu_abi_buffer)))
+				return -ENOMEM;
+	}
+
+	if (imgu_css_binary_preallocate(css, pipe)) {
+		imgu_css_binary_cleanup(css, pipe);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void imgu_css_pipe_cleanup(struct imgu_css *css, unsigned int pipe)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	unsigned int p, q, i, abi_buf_num;
+
+	imgu_css_binary_cleanup(css, pipe);
+
+	for (q = 0; q < IPU3_CSS_QUEUES; q++) {
+		abi_buf_num = ARRAY_SIZE(css_pipe->abi_buffers[q]);
+		for (i = 0; i < abi_buf_num; i++)
+			imgu_dmamap_free(imgu, &css_pipe->abi_buffers[q][i]);
+	}
+
+	for (p = 0; p < IPU3_CSS_PIPE_ID_NUM; p++)
+		for (i = 0; i < IMGU_ABI_MAX_STAGES; i++) {
+			imgu_dmamap_free(imgu,
+					 &css_pipe->xmem_sp_stage_ptrs[p][i]);
+			imgu_dmamap_free(imgu,
+					 &css_pipe->xmem_isp_stage_ptrs[p][i]);
+		}
+
+	imgu_dmamap_free(imgu, &css_pipe->sp_ddr_ptrs);
+}
+
+void imgu_css_cleanup(struct imgu_css *css)
+{
+	struct imgu_device *imgu = dev_get_drvdata(css->dev);
+	unsigned int pipe;
+
+	imgu_css_stop_streaming(css);
+	for (pipe = 0; pipe < IMGU_MAX_PIPE_NUM; pipe++)
+		imgu_css_pipe_cleanup(css, pipe);
+	imgu_dmamap_free(imgu, &css->xmem_sp_group_ptrs);
+	imgu_css_fw_cleanup(css);
+}
+
+int imgu_css_init(struct device *dev, struct imgu_css *css,
+		  void __iomem *base, int length)
+{
+	struct imgu_device *imgu = dev_get_drvdata(dev);
+	int r, q, pipe;
+
+	/* Initialize main data structure */
+	css->dev = dev;
+	css->base = base;
+	css->iomem_length = length;
+
+	for (pipe = 0; pipe < IMGU_MAX_PIPE_NUM; pipe++) {
+		struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+		css_pipe->vf_output_en = false;
+		spin_lock_init(&css_pipe->qlock);
+		css_pipe->bindex = IPU3_CSS_DEFAULT_BINARY;
+		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
+		for (q = 0; q < IPU3_CSS_QUEUES; q++) {
+			r = imgu_css_queue_init(&css_pipe->queue[q], NULL, 0);
+			if (r)
+				return r;
+		}
+		r = imgu_css_map_init(css, pipe);
+		if (r) {
+			imgu_css_cleanup(css);
+			return r;
+		}
+	}
+	if (!imgu_dmamap_alloc(imgu, &css->xmem_sp_group_ptrs,
+			       sizeof(struct imgu_abi_sp_group)))
+		return -ENOMEM;
+
+	r = imgu_css_fw_init(css);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static u32 imgu_css_adjust(u32 res, u32 align)
+{
+	u32 val = max_t(u32, IPU3_CSS_MIN_RES, res);
+
+	return DIV_ROUND_CLOSEST(val, align) * align;
+}
+
+/* Select a binary matching the required resolutions and formats */
+static int imgu_css_find_binary(struct imgu_css *css,
+				unsigned int pipe,
+				struct imgu_css_queue queue[IPU3_CSS_QUEUES],
+				struct v4l2_rect rects[IPU3_CSS_RECTS])
+{
+	const int binary_nr = css->fwp->file_header.binary_nr;
+	unsigned int binary_mode =
+		(css->pipes[pipe].pipe_id == IPU3_CSS_PIPE_ID_CAPTURE) ?
+		IA_CSS_BINARY_MODE_PRIMARY : IA_CSS_BINARY_MODE_VIDEO;
+	const struct v4l2_pix_format_mplane *in =
+					&queue[IPU3_CSS_QUEUE_IN].fmt.mpix;
+	const struct v4l2_pix_format_mplane *out =
+					&queue[IPU3_CSS_QUEUE_OUT].fmt.mpix;
+	const struct v4l2_pix_format_mplane *vf =
+					&queue[IPU3_CSS_QUEUE_VF].fmt.mpix;
+	u32 stripe_w = 0, stripe_h = 0;
+	const char *name;
+	int i, j;
+
+	if (!imgu_css_queue_enabled(&queue[IPU3_CSS_QUEUE_IN]))
+		return -EINVAL;
+
+	/* Find out the strip size boundary */
+	for (i = 0; i < binary_nr; i++) {
+		struct imgu_fw_info *bi = &css->fwp->binary_header[i];
+
+		u32 max_width = bi->info.isp.sp.output.max_width;
+		u32 max_height = bi->info.isp.sp.output.max_height;
+
+		if (bi->info.isp.sp.iterator.num_stripes <= 1) {
+			stripe_w = stripe_w ?
+				min(stripe_w, max_width) : max_width;
+			stripe_h = stripe_h ?
+				min(stripe_h, max_height) : max_height;
+		}
+	}
+
+	for (i = 0; i < binary_nr; i++) {
+		struct imgu_fw_info *bi = &css->fwp->binary_header[i];
+		enum imgu_abi_frame_format q_fmt;
+
+		name = (void *)css->fwp + bi->blob.prog_name_offset;
+
+		/* Check that binary supports memory-to-memory processing */
+		if (bi->info.isp.sp.input.source !=
+		    IMGU_ABI_BINARY_INPUT_SOURCE_MEMORY)
+			continue;
+
+		/* Check that binary supports raw10 input */
+		if (!bi->info.isp.sp.enable.input_feeder &&
+		    !bi->info.isp.sp.enable.input_raw)
+			continue;
+
+		/* Check binary mode */
+		if (bi->info.isp.sp.pipeline.mode != binary_mode)
+			continue;
+
+		/* Since input is RGGB bayer, need to process colors */
+		if (bi->info.isp.sp.enable.luma_only)
+			continue;
+
+		if (in->width < bi->info.isp.sp.input.min_width ||
+		    in->width > bi->info.isp.sp.input.max_width ||
+		    in->height < bi->info.isp.sp.input.min_height ||
+		    in->height > bi->info.isp.sp.input.max_height)
+			continue;
+
+		if (imgu_css_queue_enabled(&queue[IPU3_CSS_QUEUE_OUT])) {
+			if (bi->info.isp.num_output_pins <= 0)
+				continue;
+
+			q_fmt = queue[IPU3_CSS_QUEUE_OUT].css_fmt->frame_format;
+			for (j = 0; j < bi->info.isp.num_output_formats; j++)
+				if (bi->info.isp.output_formats[j] == q_fmt)
+					break;
+			if (j >= bi->info.isp.num_output_formats)
+				continue;
+
+			if (out->width < bi->info.isp.sp.output.min_width ||
+			    out->width > bi->info.isp.sp.output.max_width ||
+			    out->height < bi->info.isp.sp.output.min_height ||
+			    out->height > bi->info.isp.sp.output.max_height)
+				continue;
+
+			if (out->width > bi->info.isp.sp.internal.max_width ||
+			    out->height > bi->info.isp.sp.internal.max_height)
+				continue;
+		}
+
+		if (imgu_css_queue_enabled(&queue[IPU3_CSS_QUEUE_VF])) {
+			if (bi->info.isp.num_output_pins <= 1)
+				continue;
+
+			q_fmt = queue[IPU3_CSS_QUEUE_VF].css_fmt->frame_format;
+			for (j = 0; j < bi->info.isp.num_output_formats; j++)
+				if (bi->info.isp.output_formats[j] == q_fmt)
+					break;
+			if (j >= bi->info.isp.num_output_formats)
+				continue;
+
+			if (vf->width < bi->info.isp.sp.output.min_width ||
+			    vf->width > bi->info.isp.sp.output.max_width ||
+			    vf->height < bi->info.isp.sp.output.min_height ||
+			    vf->height > bi->info.isp.sp.output.max_height)
+				continue;
+		}
+
+		/* All checks passed, select the binary */
+		dev_dbg(css->dev, "using binary %s id = %u\n", name,
+			bi->info.isp.sp.id);
+		return i;
+	}
+
+	/* Can not find suitable binary for these parameters */
+	return -EINVAL;
+}
+
+/*
+ * Check that there is a binary matching requirements. Parameters may be
+ * NULL indicating disabled input/output. Return negative if given
+ * parameters can not be supported or on error, zero or positive indicating
+ * found binary number. May modify the given parameters if not exact match
+ * is found.
+ */
+int imgu_css_fmt_try(struct imgu_css *css,
+		     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES],
+		     struct v4l2_rect *rects[IPU3_CSS_RECTS],
+		     unsigned int pipe)
+{
+	static const u32 EFF_ALIGN_W = 2;
+	static const u32 BDS_ALIGN_W = 4;
+	static const u32 OUT_ALIGN_W = 8;
+	static const u32 OUT_ALIGN_H = 4;
+	static const u32 VF_ALIGN_W  = 2;
+	static const char *qnames[IPU3_CSS_QUEUES] = {
+		[IPU3_CSS_QUEUE_IN] = "in",
+		[IPU3_CSS_QUEUE_PARAMS]    = "params",
+		[IPU3_CSS_QUEUE_OUT] = "out",
+		[IPU3_CSS_QUEUE_VF] = "vf",
+		[IPU3_CSS_QUEUE_STAT_3A]   = "3a",
+	};
+	static const char *rnames[IPU3_CSS_RECTS] = {
+		[IPU3_CSS_RECT_EFFECTIVE] = "effective resolution",
+		[IPU3_CSS_RECT_BDS]       = "bayer-domain scaled resolution",
+		[IPU3_CSS_RECT_ENVELOPE]  = "DVS envelope size",
+		[IPU3_CSS_RECT_GDC]  = "GDC output res",
+	};
+	struct v4l2_rect r[IPU3_CSS_RECTS] = { };
+	struct v4l2_rect *const eff = &r[IPU3_CSS_RECT_EFFECTIVE];
+	struct v4l2_rect *const bds = &r[IPU3_CSS_RECT_BDS];
+	struct v4l2_rect *const env = &r[IPU3_CSS_RECT_ENVELOPE];
+	struct v4l2_rect *const gdc = &r[IPU3_CSS_RECT_GDC];
+	struct imgu_css_queue *q;
+	struct v4l2_pix_format_mplane *in, *out, *vf;
+	int i, s, ret;
+
+	q = kcalloc(IPU3_CSS_QUEUES, sizeof(struct imgu_css_queue), GFP_KERNEL);
+	if (!q)
+		return -ENOMEM;
+
+	in  = &q[IPU3_CSS_QUEUE_IN].fmt.mpix;
+	out = &q[IPU3_CSS_QUEUE_OUT].fmt.mpix;
+	vf  = &q[IPU3_CSS_QUEUE_VF].fmt.mpix;
+
+	/* Adjust all formats, get statistics buffer sizes and formats */
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		if (fmts[i])
+			dev_dbg(css->dev, "%s %s: (%i,%i) fmt 0x%x\n", __func__,
+				qnames[i], fmts[i]->width, fmts[i]->height,
+				fmts[i]->pixelformat);
+		else
+			dev_dbg(css->dev, "%s %s: (not set)\n", __func__,
+				qnames[i]);
+		if (imgu_css_queue_init(&q[i], fmts[i],
+					IPU3_CSS_QUEUE_TO_FLAGS(i))) {
+			dev_notice(css->dev, "can not initialize queue %s\n",
+				   qnames[i]);
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+	for (i = 0; i < IPU3_CSS_RECTS; i++) {
+		if (rects[i]) {
+			dev_dbg(css->dev, "%s %s: (%i,%i)\n", __func__,
+				rnames[i], rects[i]->width, rects[i]->height);
+			r[i].width  = rects[i]->width;
+			r[i].height = rects[i]->height;
+		} else {
+			dev_dbg(css->dev, "%s %s: (not set)\n", __func__,
+				rnames[i]);
+		}
+		/* For now, force known good resolutions */
+		r[i].left = 0;
+		r[i].top  = 0;
+	}
+
+	/* Always require one input and vf only if out is also enabled */
+	if (!imgu_css_queue_enabled(&q[IPU3_CSS_QUEUE_IN]) ||
+	    !imgu_css_queue_enabled(&q[IPU3_CSS_QUEUE_OUT])) {
+		dev_warn(css->dev, "required queues are disabled\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (!imgu_css_queue_enabled(&q[IPU3_CSS_QUEUE_OUT])) {
+		out->width = in->width;
+		out->height = in->height;
+	}
+	if (eff->width <= 0 || eff->height <= 0) {
+		eff->width = in->width;
+		eff->height = in->height;
+	}
+	if (bds->width <= 0 || bds->height <= 0) {
+		bds->width = out->width;
+		bds->height = out->height;
+	}
+	if (gdc->width <= 0 || gdc->height <= 0) {
+		gdc->width = out->width;
+		gdc->height = out->height;
+	}
+
+	in->width   = imgu_css_adjust(in->width, 1);
+	in->height  = imgu_css_adjust(in->height, 1);
+	eff->width  = imgu_css_adjust(eff->width, EFF_ALIGN_W);
+	eff->height = imgu_css_adjust(eff->height, 1);
+	bds->width  = imgu_css_adjust(bds->width, BDS_ALIGN_W);
+	bds->height = imgu_css_adjust(bds->height, 1);
+	gdc->width  = imgu_css_adjust(gdc->width, OUT_ALIGN_W);
+	gdc->height = imgu_css_adjust(gdc->height, OUT_ALIGN_H);
+	out->width  = imgu_css_adjust(out->width, OUT_ALIGN_W);
+	out->height = imgu_css_adjust(out->height, OUT_ALIGN_H);
+	vf->width   = imgu_css_adjust(vf->width, VF_ALIGN_W);
+	vf->height  = imgu_css_adjust(vf->height, 1);
+
+	s = (bds->width - gdc->width) / 2;
+	env->width = s < MIN_ENVELOPE ? MIN_ENVELOPE : s;
+	s = (bds->height - gdc->height) / 2;
+	env->height = s < MIN_ENVELOPE ? MIN_ENVELOPE : s;
+
+	ret = imgu_css_find_binary(css, pipe, q, r);
+	if (ret < 0) {
+		dev_err(css->dev, "failed to find suitable binary\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	css->pipes[pipe].bindex = ret;
+
+	dev_dbg(css->dev, "Binary index %d for pipe %d found.",
+		css->pipes[pipe].bindex, pipe);
+
+	/* Final adjustment and set back the queried formats */
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		if (fmts[i]) {
+			if (imgu_css_queue_init(&q[i], &q[i].fmt.mpix,
+						IPU3_CSS_QUEUE_TO_FLAGS(i))) {
+				dev_err(css->dev,
+					"final resolution adjustment failed\n");
+				ret = -EINVAL;
+				goto out;
+			}
+			*fmts[i] = q[i].fmt.mpix;
+		}
+	}
+
+	for (i = 0; i < IPU3_CSS_RECTS; i++)
+		if (rects[i])
+			*rects[i] = r[i];
+
+	dev_dbg(css->dev,
+		"in(%u,%u) if(%u,%u) ds(%u,%u) gdc(%u,%u) out(%u,%u) vf(%u,%u)",
+		 in->width, in->height, eff->width, eff->height,
+		 bds->width, bds->height, gdc->width, gdc->height,
+		 out->width, out->height, vf->width, vf->height);
+
+	ret = 0;
+out:
+	kfree(q);
+	return ret;
+}
+
+int imgu_css_fmt_set(struct imgu_css *css,
+		     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES],
+		     struct v4l2_rect *rects[IPU3_CSS_RECTS],
+		     unsigned int pipe)
+{
+	struct v4l2_rect rect_data[IPU3_CSS_RECTS];
+	struct v4l2_rect *all_rects[IPU3_CSS_RECTS];
+	int i, r;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	for (i = 0; i < IPU3_CSS_RECTS; i++) {
+		if (rects[i])
+			rect_data[i] = *rects[i];
+		else
+			memset(&rect_data[i], 0, sizeof(rect_data[i]));
+		all_rects[i] = &rect_data[i];
+	}
+	r = imgu_css_fmt_try(css, fmts, all_rects, pipe);
+	if (r < 0)
+		return r;
+
+	for (i = 0; i < IPU3_CSS_QUEUES; i++)
+		if (imgu_css_queue_init(&css_pipe->queue[i], fmts[i],
+					IPU3_CSS_QUEUE_TO_FLAGS(i)))
+			return -EINVAL;
+	for (i = 0; i < IPU3_CSS_RECTS; i++) {
+		css_pipe->rect[i] = rect_data[i];
+		if (rects[i])
+			*rects[i] = rect_data[i];
+	}
+
+	return 0;
+}
+
+int imgu_css_meta_fmt_set(struct v4l2_meta_format *fmt)
+{
+	switch (fmt->dataformat) {
+	case V4L2_META_FMT_IPU3_PARAMS:
+		fmt->buffersize = sizeof(struct ipu3_uapi_params);
+
+		/*
+		 * Sanity check for the parameter struct size. This must
+		 * not change!
+		 */
+		BUILD_BUG_ON(sizeof(struct ipu3_uapi_params) != 39328);
+
+		break;
+	case V4L2_META_FMT_IPU3_STAT_3A:
+		fmt->buffersize = sizeof(struct ipu3_uapi_stats_3a);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Queue given buffer to CSS. imgu_css_buf_prepare() must have been first
+ * called for the buffer. May be called from interrupt context.
+ * Returns 0 on success, -EBUSY if the buffer queue is full, or some other
+ * code on error conditions.
+ */
+int imgu_css_buf_queue(struct imgu_css *css, unsigned int pipe,
+		       struct imgu_css_buffer *b)
+{
+	struct imgu_abi_buffer *abi_buf;
+	struct imgu_addr_t *buf_addr;
+	u32 data;
+	int r;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+
+	if (!css->streaming)
+		return -EPROTO;	/* CSS or buffer in wrong state */
+
+	if (b->queue >= IPU3_CSS_QUEUES || !imgu_css_queues[b->queue].qid)
+		return -EINVAL;
+
+	b->queue_pos = imgu_css_queue_pos(css, imgu_css_queues[b->queue].qid,
+					  pipe);
+
+	if (b->queue_pos >= ARRAY_SIZE(css->pipes[pipe].abi_buffers[b->queue]))
+		return -EIO;
+	abi_buf = css->pipes[pipe].abi_buffers[b->queue][b->queue_pos].vaddr;
+
+	/* Fill struct abi_buffer for firmware */
+	memset(abi_buf, 0, sizeof(*abi_buf));
+
+	buf_addr = (void *)abi_buf + imgu_css_queues[b->queue].ptr_ofs;
+	*(imgu_addr_t *)buf_addr = b->daddr;
+
+	if (b->queue == IPU3_CSS_QUEUE_STAT_3A)
+		abi_buf->payload.s3a.data.dmem.s3a_tbl = b->daddr;
+
+	if (b->queue == IPU3_CSS_QUEUE_OUT)
+		abi_buf->payload.frame.padded_width =
+				css_pipe->queue[IPU3_CSS_QUEUE_OUT].width_pad;
+
+	if (b->queue == IPU3_CSS_QUEUE_VF)
+		abi_buf->payload.frame.padded_width =
+					css_pipe->queue[IPU3_CSS_QUEUE_VF].width_pad;
+
+	spin_lock(&css_pipe->qlock);
+	list_add_tail(&b->list, &css_pipe->queue[b->queue].bufs);
+	spin_unlock(&css_pipe->qlock);
+	b->state = IPU3_CSS_BUFFER_QUEUED;
+
+	data = css->pipes[pipe].abi_buffers[b->queue][b->queue_pos].daddr;
+	r = imgu_css_queue_data(css, imgu_css_queues[b->queue].qid,
+				pipe, data);
+	if (r < 0)
+		goto queueing_failed;
+
+	data = IMGU_ABI_EVENT_BUFFER_ENQUEUED(pipe,
+					      imgu_css_queues[b->queue].qid);
+	r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe, data);
+	if (r < 0)
+		goto queueing_failed;
+
+	dev_dbg(css->dev, "queued buffer %p to css queue %i in pipe %d\n",
+		b, b->queue, pipe);
+
+	return 0;
+
+queueing_failed:
+	b->state = (r == -EBUSY || r == -EAGAIN) ?
+		IPU3_CSS_BUFFER_NEW : IPU3_CSS_BUFFER_FAILED;
+	list_del(&b->list);
+
+	return r;
+}
+
+/*
+ * Get next ready CSS buffer. Returns -EAGAIN in which case the function
+ * should be called again, or -EBUSY which means that there are no more
+ * buffers available. May be called from interrupt context.
+ */
+struct imgu_css_buffer *imgu_css_buf_dequeue(struct imgu_css *css)
+{
+	static const unsigned char evtype_to_queue[] = {
+		[IMGU_ABI_EVTTYPE_INPUT_FRAME_DONE] = IPU3_CSS_QUEUE_IN,
+		[IMGU_ABI_EVTTYPE_OUT_FRAME_DONE] = IPU3_CSS_QUEUE_OUT,
+		[IMGU_ABI_EVTTYPE_VF_OUT_FRAME_DONE] = IPU3_CSS_QUEUE_VF,
+		[IMGU_ABI_EVTTYPE_3A_STATS_DONE] = IPU3_CSS_QUEUE_STAT_3A,
+	};
+	struct imgu_css_buffer *b = ERR_PTR(-EAGAIN);
+	u32 event, daddr;
+	int evtype, pipe, pipeid, queue, qid, r;
+	struct imgu_css_pipe *css_pipe;
+
+	if (!css->streaming)
+		return ERR_PTR(-EPROTO);
+
+	r = imgu_css_dequeue_data(css, IMGU_ABI_QUEUE_EVENT_ID, &event);
+	if (r < 0)
+		return ERR_PTR(r);
+
+	evtype = (event & IMGU_ABI_EVTTYPE_EVENT_MASK) >>
+		  IMGU_ABI_EVTTYPE_EVENT_SHIFT;
+
+	switch (evtype) {
+	case IMGU_ABI_EVTTYPE_OUT_FRAME_DONE:
+	case IMGU_ABI_EVTTYPE_VF_OUT_FRAME_DONE:
+	case IMGU_ABI_EVTTYPE_3A_STATS_DONE:
+	case IMGU_ABI_EVTTYPE_INPUT_FRAME_DONE:
+		pipe = (event & IMGU_ABI_EVTTYPE_PIPE_MASK) >>
+			IMGU_ABI_EVTTYPE_PIPE_SHIFT;
+		pipeid = (event & IMGU_ABI_EVTTYPE_PIPEID_MASK) >>
+			IMGU_ABI_EVTTYPE_PIPEID_SHIFT;
+		queue = evtype_to_queue[evtype];
+		qid = imgu_css_queues[queue].qid;
+
+		if (pipe >= IMGU_MAX_PIPE_NUM) {
+			dev_err(css->dev, "Invalid pipe: %i\n", pipe);
+			return ERR_PTR(-EIO);
+		}
+
+		if (qid >= IMGU_ABI_QUEUE_NUM) {
+			dev_err(css->dev, "Invalid qid: %i\n", qid);
+			return ERR_PTR(-EIO);
+		}
+		css_pipe = &css->pipes[pipe];
+		dev_dbg(css->dev,
+			"event: buffer done 0x%x queue %i pipe %i pipeid %i\n",
+			event, queue, pipe, pipeid);
+
+		r = imgu_css_dequeue_data(css, qid, &daddr);
+		if (r < 0) {
+			dev_err(css->dev, "failed to dequeue buffer\n");
+			/* Force real error, not -EBUSY */
+			return ERR_PTR(-EIO);
+		}
+
+		r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe,
+					IMGU_ABI_EVENT_BUFFER_DEQUEUED(qid));
+		if (r < 0) {
+			dev_err(css->dev, "failed to queue event\n");
+			return ERR_PTR(-EIO);
+		}
+
+		spin_lock(&css_pipe->qlock);
+		if (list_empty(&css_pipe->queue[queue].bufs)) {
+			spin_unlock(&css_pipe->qlock);
+			dev_err(css->dev, "event on empty queue\n");
+			return ERR_PTR(-EIO);
+		}
+		b = list_first_entry(&css_pipe->queue[queue].bufs,
+				     struct imgu_css_buffer, list);
+		if (queue != b->queue ||
+		    daddr != css_pipe->abi_buffers
+			[b->queue][b->queue_pos].daddr) {
+			spin_unlock(&css_pipe->qlock);
+			dev_err(css->dev, "dequeued bad buffer 0x%x\n", daddr);
+			return ERR_PTR(-EIO);
+		}
+
+		dev_dbg(css->dev, "buffer 0x%8x done from pipe %d\n", daddr, pipe);
+		b->pipe = pipe;
+		b->state = IPU3_CSS_BUFFER_DONE;
+		list_del(&b->list);
+		spin_unlock(&css_pipe->qlock);
+		break;
+	case IMGU_ABI_EVTTYPE_PIPELINE_DONE:
+		pipe = (event & IMGU_ABI_EVTTYPE_PIPE_MASK) >>
+			IMGU_ABI_EVTTYPE_PIPE_SHIFT;
+		if (pipe >= IMGU_MAX_PIPE_NUM) {
+			dev_err(css->dev, "Invalid pipe: %i\n", pipe);
+			return ERR_PTR(-EIO);
+		}
+
+		css_pipe = &css->pipes[pipe];
+		dev_dbg(css->dev, "event: pipeline done 0x%8x for pipe %d\n",
+			event, pipe);
+		break;
+	case IMGU_ABI_EVTTYPE_TIMER:
+		r = imgu_css_dequeue_data(css, IMGU_ABI_QUEUE_EVENT_ID, &event);
+		if (r < 0)
+			return ERR_PTR(r);
+
+		if ((event & IMGU_ABI_EVTTYPE_EVENT_MASK) >>
+		    IMGU_ABI_EVTTYPE_EVENT_SHIFT == IMGU_ABI_EVTTYPE_TIMER)
+			dev_dbg(css->dev, "event: timer\n");
+		else
+			dev_warn(css->dev, "half of timer event missing\n");
+		break;
+	case IMGU_ABI_EVTTYPE_FW_WARNING:
+		dev_warn(css->dev, "event: firmware warning 0x%x\n", event);
+		break;
+	case IMGU_ABI_EVTTYPE_FW_ASSERT:
+		dev_err(css->dev,
+			"event: firmware assert 0x%x module_id %i line_no %i\n",
+			event,
+			(event & IMGU_ABI_EVTTYPE_MODULEID_MASK) >>
+			IMGU_ABI_EVTTYPE_MODULEID_SHIFT,
+			swab16((event & IMGU_ABI_EVTTYPE_LINENO_MASK) >>
+			       IMGU_ABI_EVTTYPE_LINENO_SHIFT));
+		break;
+	default:
+		dev_warn(css->dev, "received unknown event 0x%x\n", event);
+	}
+
+	return b;
+}
+
+/*
+ * Get a new set of parameters from pool and initialize them based on
+ * the parameters params, gdc, and obgrid. Any of these may be NULL,
+ * in which case the previously set parameters are used.
+ * If parameters haven't been set previously, initialize from scratch.
+ *
+ * Return index to css->parameter_set_info which has the newly created
+ * parameters or negative value on error.
+ */
+int imgu_css_set_parameters(struct imgu_css *css, unsigned int pipe,
+			    struct ipu3_uapi_params *set_params)
+{
+	static const unsigned int queue_id = IMGU_ABI_QUEUE_A_ID;
+	struct imgu_css_pipe *css_pipe = &css->pipes[pipe];
+	const int stage = 0;
+	const struct imgu_fw_info *bi;
+	int obgrid_size;
+	unsigned int stripes, i;
+	struct ipu3_uapi_flags *use = set_params ? &set_params->use : NULL;
+
+	/* Destination buffers which are filled here */
+	struct imgu_abi_parameter_set_info *param_set;
+	struct imgu_abi_acc_param *acc = NULL;
+	struct imgu_abi_gdc_warp_param *gdc = NULL;
+	struct ipu3_uapi_obgrid_param *obgrid = NULL;
+	const struct imgu_css_map *map;
+	void *vmem0 = NULL;
+	void *dmem0 = NULL;
+
+	enum imgu_abi_memories m;
+	int r = -EBUSY;
+
+	if (!css->streaming)
+		return -EPROTO;
+
+	dev_dbg(css->dev, "%s for pipe %d", __func__, pipe);
+
+	bi = &css->fwp->binary_header[css_pipe->bindex];
+	obgrid_size = imgu_css_fw_obgrid_size(bi);
+	stripes = bi->info.isp.sp.iterator.num_stripes ? : 1;
+
+	imgu_css_pool_get(&css_pipe->pool.parameter_set_info);
+	param_set = imgu_css_pool_last(&css_pipe->pool.parameter_set_info,
+				       0)->vaddr;
+
+	/* Get a new acc only if new parameters given, or none yet */
+	map = imgu_css_pool_last(&css_pipe->pool.acc, 0);
+	if (set_params || !map->vaddr) {
+		imgu_css_pool_get(&css_pipe->pool.acc);
+		map = imgu_css_pool_last(&css_pipe->pool.acc, 0);
+		acc = map->vaddr;
+	}
+
+	/* Get new VMEM0 only if needed, or none yet */
+	m = IMGU_ABI_MEM_ISP_VMEM0;
+	map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 0);
+	if (!map->vaddr || (set_params && (set_params->use.lin_vmem_params ||
+					   set_params->use.tnr3_vmem_params ||
+					   set_params->use.xnr3_vmem_params))) {
+		imgu_css_pool_get(&css_pipe->pool.binary_params_p[m]);
+		map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 0);
+		vmem0 = map->vaddr;
+	}
+
+	/* Get new DMEM0 only if needed, or none yet */
+	m = IMGU_ABI_MEM_ISP_DMEM0;
+	map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 0);
+	if (!map->vaddr || (set_params && (set_params->use.tnr3_dmem_params ||
+					   set_params->use.xnr3_dmem_params))) {
+		imgu_css_pool_get(&css_pipe->pool.binary_params_p[m]);
+		map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 0);
+		dmem0 = map->vaddr;
+	}
+
+	/* Configure acc parameter cluster */
+	if (acc) {
+		/* get acc_old */
+		map = imgu_css_pool_last(&css_pipe->pool.acc, 1);
+		/* user acc */
+		r = imgu_css_cfg_acc(css, pipe, use, acc, map->vaddr,
+			set_params ? &set_params->acc_param : NULL);
+		if (r < 0)
+			goto fail;
+	}
+
+	/* Configure late binding parameters */
+	if (vmem0) {
+		m = IMGU_ABI_MEM_ISP_VMEM0;
+		map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 1);
+		r = imgu_css_cfg_vmem0(css, pipe, use, vmem0,
+				       map->vaddr, set_params);
+		if (r < 0)
+			goto fail;
+	}
+
+	if (dmem0) {
+		m = IMGU_ABI_MEM_ISP_DMEM0;
+		map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 1);
+		r = imgu_css_cfg_dmem0(css, pipe, use, dmem0,
+				       map->vaddr, set_params);
+		if (r < 0)
+			goto fail;
+	}
+
+	/* Get a new gdc only if a new gdc is given, or none yet */
+	if (bi->info.isp.sp.enable.dvs_6axis) {
+		unsigned int a = IPU3_CSS_AUX_FRAME_REF;
+		unsigned int g = IPU3_CSS_RECT_GDC;
+		unsigned int e = IPU3_CSS_RECT_ENVELOPE;
+
+		map = imgu_css_pool_last(&css_pipe->pool.gdc, 0);
+		if (!map->vaddr) {
+			imgu_css_pool_get(&css_pipe->pool.gdc);
+			map = imgu_css_pool_last(&css_pipe->pool.gdc, 0);
+			gdc = map->vaddr;
+			imgu_css_cfg_gdc_table(map->vaddr,
+				css_pipe->aux_frames[a].bytesperline /
+				css_pipe->aux_frames[a].bytesperpixel,
+				css_pipe->aux_frames[a].height,
+				css_pipe->rect[g].width,
+				css_pipe->rect[g].height,
+				css_pipe->rect[e].width,
+				css_pipe->rect[e].height);
+		}
+	}
+
+	/* Get a new obgrid only if a new obgrid is given, or none yet */
+	map = imgu_css_pool_last(&css_pipe->pool.obgrid, 0);
+	if (!map->vaddr || (set_params && set_params->use.obgrid_param)) {
+		imgu_css_pool_get(&css_pipe->pool.obgrid);
+		map = imgu_css_pool_last(&css_pipe->pool.obgrid, 0);
+		obgrid = map->vaddr;
+
+		/* Configure optical black level grid (obgrid) */
+		if (set_params && set_params->use.obgrid_param)
+			for (i = 0; i < obgrid_size / sizeof(*obgrid); i++)
+				obgrid[i] = set_params->obgrid_param;
+		else
+			memset(obgrid, 0, obgrid_size);
+	}
+
+	/* Configure parameter set info, queued to `queue_id' */
+
+	memset(param_set, 0, sizeof(*param_set));
+	map = imgu_css_pool_last(&css_pipe->pool.acc, 0);
+	param_set->mem_map.acc_cluster_params_for_sp = map->daddr;
+
+	map = imgu_css_pool_last(&css_pipe->pool.gdc, 0);
+	param_set->mem_map.dvs_6axis_params_y = map->daddr;
+
+	for (i = 0; i < stripes; i++) {
+		map = imgu_css_pool_last(&css_pipe->pool.obgrid, 0);
+		param_set->mem_map.obgrid_tbl[i] =
+			map->daddr + (obgrid_size / stripes) * i;
+	}
+
+	for (m = 0; m < IMGU_ABI_NUM_MEMORIES; m++) {
+		map = imgu_css_pool_last(&css_pipe->pool.binary_params_p[m], 0);
+		param_set->mem_map.isp_mem_param[stage][m] = map->daddr;
+	}
+
+	/* Then queue the new parameter buffer */
+	map = imgu_css_pool_last(&css_pipe->pool.parameter_set_info, 0);
+	r = imgu_css_queue_data(css, queue_id, pipe, map->daddr);
+	if (r < 0)
+		goto fail;
+
+	r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe,
+				IMGU_ABI_EVENT_BUFFER_ENQUEUED(pipe,
+							       queue_id));
+	if (r < 0)
+		goto fail_no_put;
+
+	/* Finally dequeue all old parameter buffers */
+
+	do {
+		u32 daddr;
+
+		r = imgu_css_dequeue_data(css, queue_id, &daddr);
+		if (r == -EBUSY)
+			break;
+		if (r)
+			goto fail_no_put;
+		r = imgu_css_queue_data(css, IMGU_ABI_QUEUE_EVENT_ID, pipe,
+					IMGU_ABI_EVENT_BUFFER_DEQUEUED
+					(queue_id));
+		if (r < 0) {
+			dev_err(css->dev, "failed to queue parameter event\n");
+			goto fail_no_put;
+		}
+	} while (1);
+
+	return 0;
+
+fail:
+	/*
+	 * A failure, most likely the parameter queue was full.
+	 * Return error but continue streaming. User can try submitting new
+	 * parameters again later.
+	 */
+
+	imgu_css_pool_put(&css_pipe->pool.parameter_set_info);
+	if (acc)
+		imgu_css_pool_put(&css_pipe->pool.acc);
+	if (gdc)
+		imgu_css_pool_put(&css_pipe->pool.gdc);
+	if (obgrid)
+		imgu_css_pool_put(&css_pipe->pool.obgrid);
+	if (vmem0)
+		imgu_css_pool_put(
+			&css_pipe->pool.binary_params_p
+			[IMGU_ABI_MEM_ISP_VMEM0]);
+	if (dmem0)
+		imgu_css_pool_put(
+			&css_pipe->pool.binary_params_p
+			[IMGU_ABI_MEM_ISP_DMEM0]);
+
+fail_no_put:
+	return r;
+}
+
+int imgu_css_irq_ack(struct imgu_css *css)
+{
+	static const int NUM_SWIRQS = 3;
+	struct imgu_fw_info *bi = &css->fwp->binary_header[css->fw_sp[0]];
+	void __iomem *const base = css->base;
+	u32 irq_status[IMGU_IRQCTRL_NUM];
+	int i;
+
+	u32 imgu_status = readl(base + IMGU_REG_INT_STATUS);
+
+	writel(imgu_status, base + IMGU_REG_INT_STATUS);
+	for (i = 0; i < IMGU_IRQCTRL_NUM; i++)
+		irq_status[i] = readl(base + IMGU_REG_IRQCTRL_STATUS(i));
+
+	for (i = 0; i < NUM_SWIRQS; i++) {
+		if (irq_status[IMGU_IRQCTRL_SP0] & IMGU_IRQCTRL_IRQ_SW_PIN(i)) {
+			/* SP SW interrupt */
+			u32 cnt = readl(base + IMGU_REG_SP_DMEM_BASE(0) +
+					bi->info.sp.output);
+			u32 val = readl(base + IMGU_REG_SP_DMEM_BASE(0) +
+					bi->info.sp.output + 4 + 4 * i);
+
+			dev_dbg(css->dev, "%s: swirq %i cnt %i val 0x%x\n",
+				__func__, i, cnt, val);
+		}
+	}
+
+	for (i = IMGU_IRQCTRL_NUM - 1; i >= 0; i--)
+		if (irq_status[i]) {
+			writel(irq_status[i], base + IMGU_REG_IRQCTRL_CLEAR(i));
+			/* Wait for write to complete */
+			readl(base + IMGU_REG_IRQCTRL_ENABLE(i));
+		}
+
+	dev_dbg(css->dev, "%s: imgu 0x%x main 0x%x sp0 0x%x sp1 0x%x\n",
+		__func__, imgu_status, irq_status[IMGU_IRQCTRL_MAIN],
+		irq_status[IMGU_IRQCTRL_SP0], irq_status[IMGU_IRQCTRL_SP1]);
+
+	if (!imgu_status && !irq_status[IMGU_IRQCTRL_MAIN])
+		return -ENOMSG;
+
+	return 0;
+}
diff --git a/drivers/staging/media/ipu3/ipu3-css.h b/drivers/staging/media/ipu3/ipu3-css.h
new file mode 100644
index 000000000..ab64e9521
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-css.h
@@ -0,0 +1,213 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_CSS_H
+#define __IPU3_CSS_H
+
+#include <linux/videodev2.h>
+#include <linux/types.h>
+
+#include "ipu3-abi.h"
+#include "ipu3-css-pool.h"
+
+/* 2 stages for split isp pipeline, 1 for scaling */
+#define IMGU_NUM_SP			2
+#define IMGU_MAX_PIPELINE_NUM		20
+#define IMGU_MAX_PIPE_NUM		2
+
+/* For DVS etc., format FRAME_FMT_YUV420_16 */
+#define IPU3_CSS_AUX_FRAME_REF		0
+/* For temporal noise reduction DVS etc., format FRAME_FMT_YUV_LINE */
+#define IPU3_CSS_AUX_FRAME_TNR		1
+#define IPU3_CSS_AUX_FRAME_TYPES	2	/* REF and TNR */
+#define IPU3_CSS_AUX_FRAMES		2	/* 2 for REF and 2 for TNR */
+
+#define IPU3_CSS_QUEUE_IN		0
+#define IPU3_CSS_QUEUE_PARAMS		1
+#define IPU3_CSS_QUEUE_OUT		2
+#define IPU3_CSS_QUEUE_VF		3
+#define IPU3_CSS_QUEUE_STAT_3A		4
+#define IPU3_CSS_QUEUES			5
+
+#define IPU3_CSS_RECT_EFFECTIVE		0	/* Effective resolution */
+#define IPU3_CSS_RECT_BDS		1	/* Resolution after BDS */
+#define IPU3_CSS_RECT_ENVELOPE		2	/* DVS envelope size */
+#define IPU3_CSS_RECT_GDC		3	/* gdc output res */
+#define IPU3_CSS_RECTS			4	/* number of rects */
+
+#define IA_CSS_BINARY_MODE_PRIMARY	2
+#define IA_CSS_BINARY_MODE_VIDEO	3
+#define IPU3_CSS_DEFAULT_BINARY		3	/* default binary index */
+
+/*
+ * The pipe id type, distinguishes the kind of pipes that
+ * can be run in parallel.
+ */
+enum imgu_css_pipe_id {
+	IPU3_CSS_PIPE_ID_PREVIEW,
+	IPU3_CSS_PIPE_ID_COPY,
+	IPU3_CSS_PIPE_ID_VIDEO,
+	IPU3_CSS_PIPE_ID_CAPTURE,
+	IPU3_CSS_PIPE_ID_YUVPP,
+	IPU3_CSS_PIPE_ID_ACC,
+	IPU3_CSS_PIPE_ID_NUM
+};
+
+struct imgu_css_resolution {
+	u32 w;
+	u32 h;
+};
+
+enum imgu_css_buffer_state {
+	IPU3_CSS_BUFFER_NEW,	/* Not yet queued */
+	IPU3_CSS_BUFFER_QUEUED,	/* Queued, waiting to be filled */
+	IPU3_CSS_BUFFER_DONE,	/* Finished processing, removed from queue */
+	IPU3_CSS_BUFFER_FAILED,	/* Was not processed, removed from queue */
+};
+
+struct imgu_css_buffer {
+	/* Private fields: user doesn't touch */
+	dma_addr_t daddr;
+	unsigned int queue;
+	enum imgu_css_buffer_state state;
+	struct list_head list;
+	u8 queue_pos;
+	unsigned int pipe;
+};
+
+struct imgu_css_format {
+	u32 pixelformat;
+	enum v4l2_colorspace colorspace;
+	enum imgu_abi_frame_format frame_format;
+	enum imgu_abi_bayer_order bayer_order;
+	enum imgu_abi_osys_format osys_format;
+	enum imgu_abi_osys_tiling osys_tiling;
+	u8 bit_depth;		/* Effective bits per pixel */
+	u8 chroma_decim;	/* Chroma plane decimation, 0=no chroma plane */
+	u8 width_align;		/* Alignment requirement for width_pad */
+	u8 flags;
+};
+
+struct imgu_css_queue {
+	union {
+		struct v4l2_pix_format_mplane mpix;
+		struct v4l2_meta_format	meta;
+
+	} fmt;
+	const struct imgu_css_format *css_fmt;
+	unsigned int width_pad;
+	struct list_head bufs;
+};
+
+struct imgu_css_pipe {
+	enum imgu_css_pipe_id pipe_id;
+	unsigned int bindex;
+
+	struct imgu_css_queue queue[IPU3_CSS_QUEUES];
+	struct v4l2_rect rect[IPU3_CSS_RECTS];
+
+	bool vf_output_en;
+	/* Protect access to queue[IPU3_CSS_QUEUES] */
+	spinlock_t qlock;
+
+	/* Data structures shared with IMGU and driver, always allocated */
+	struct imgu_css_map sp_ddr_ptrs;
+	struct imgu_css_map xmem_sp_stage_ptrs[IPU3_CSS_PIPE_ID_NUM]
+					    [IMGU_ABI_MAX_STAGES];
+	struct imgu_css_map xmem_isp_stage_ptrs[IPU3_CSS_PIPE_ID_NUM]
+					    [IMGU_ABI_MAX_STAGES];
+
+	/*
+	 * Data structures shared with IMGU and driver, binary specific.
+	 * PARAM_CLASS_CONFIG and PARAM_CLASS_STATE parameters.
+	 */
+	struct imgu_css_map binary_params_cs[IMGU_ABI_PARAM_CLASS_NUM - 1]
+					    [IMGU_ABI_NUM_MEMORIES];
+
+	struct {
+		struct imgu_css_map mem[IPU3_CSS_AUX_FRAMES];
+		unsigned int width;
+		unsigned int height;
+		unsigned int bytesperline;
+		unsigned int bytesperpixel;
+	} aux_frames[IPU3_CSS_AUX_FRAME_TYPES];
+
+	struct {
+		struct imgu_css_pool parameter_set_info;
+		struct imgu_css_pool acc;
+		struct imgu_css_pool gdc;
+		struct imgu_css_pool obgrid;
+		/* PARAM_CLASS_PARAM parameters for binding while streaming */
+		struct imgu_css_pool binary_params_p[IMGU_ABI_NUM_MEMORIES];
+	} pool;
+
+	struct imgu_css_map abi_buffers[IPU3_CSS_QUEUES]
+				    [IMGU_ABI_HOST2SP_BUFQ_SIZE];
+};
+
+/* IPU3 Camera Sub System structure */
+struct imgu_css {
+	struct device *dev;
+	void __iomem *base;
+	const struct firmware *fw;
+	struct imgu_fw_header *fwp;
+	int iomem_length;
+	int fw_bl, fw_sp[IMGU_NUM_SP];	/* Indices of bl and SP binaries */
+	struct imgu_css_map *binary;	/* fw binaries mapped to device */
+	bool streaming;		/* true when streaming is enabled */
+
+	struct imgu_css_pipe pipes[IMGU_MAX_PIPE_NUM];
+	struct imgu_css_map xmem_sp_group_ptrs;
+
+	/* enabled pipe(s) */
+	DECLARE_BITMAP(enabled_pipes, IMGU_MAX_PIPE_NUM);
+};
+
+/******************* css v4l *******************/
+int imgu_css_init(struct device *dev, struct imgu_css *css,
+		  void __iomem *base, int length);
+void imgu_css_cleanup(struct imgu_css *css);
+int imgu_css_fmt_try(struct imgu_css *css,
+		     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES],
+		     struct v4l2_rect *rects[IPU3_CSS_RECTS],
+		     unsigned int pipe);
+int imgu_css_fmt_set(struct imgu_css *css,
+		     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES],
+		     struct v4l2_rect *rects[IPU3_CSS_RECTS],
+		     unsigned int pipe);
+int imgu_css_meta_fmt_set(struct v4l2_meta_format *fmt);
+int imgu_css_buf_queue(struct imgu_css *css, unsigned int pipe,
+		       struct imgu_css_buffer *b);
+struct imgu_css_buffer *imgu_css_buf_dequeue(struct imgu_css *css);
+int imgu_css_start_streaming(struct imgu_css *css);
+void imgu_css_stop_streaming(struct imgu_css *css);
+bool imgu_css_queue_empty(struct imgu_css *css);
+bool imgu_css_is_streaming(struct imgu_css *css);
+bool imgu_css_pipe_queue_empty(struct imgu_css *css, unsigned int pipe);
+
+/******************* css hw *******************/
+int imgu_css_set_powerup(struct device *dev, void __iomem *base,
+			 unsigned int freq);
+void imgu_css_set_powerdown(struct device *dev, void __iomem *base);
+int imgu_css_irq_ack(struct imgu_css *css);
+
+/******************* set parameters ************/
+int imgu_css_set_parameters(struct imgu_css *css, unsigned int pipe,
+			    struct ipu3_uapi_params *set_params);
+
+/******************* auxiliary helpers *******************/
+static inline enum imgu_css_buffer_state
+imgu_css_buf_state(struct imgu_css_buffer *b)
+{
+	return b->state;
+}
+
+/* Initialize given buffer. May be called several times. */
+static inline void imgu_css_buf_init(struct imgu_css_buffer *b,
+				     unsigned int queue, dma_addr_t daddr)
+{
+	b->state = IPU3_CSS_BUFFER_NEW;
+	b->queue = queue;
+	b->daddr = daddr;
+}
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-dmamap.c b/drivers/staging/media/ipu3/ipu3-dmamap.c
new file mode 100644
index 000000000..8a19b0024
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-dmamap.c
@@ -0,0 +1,250 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2018 Intel Corporation
+ * Copyright 2018 Google LLC.
+ *
+ * Author: Tomasz Figa <tfiga@chromium.org>
+ * Author: Yong Zhi <yong.zhi@intel.com>
+ */
+
+#include <linux/vmalloc.h>
+
+#include "ipu3.h"
+#include "ipu3-css-pool.h"
+#include "ipu3-mmu.h"
+#include "ipu3-dmamap.h"
+
+/*
+ * Free a buffer allocated by imgu_dmamap_alloc_buffer()
+ */
+static void imgu_dmamap_free_buffer(struct page **pages,
+				    size_t size)
+{
+	int count = size >> PAGE_SHIFT;
+
+	while (count--)
+		__free_page(pages[count]);
+	kvfree(pages);
+}
+
+/*
+ * Based on the implementation of __iommu_dma_alloc_pages()
+ * defined in drivers/iommu/dma-iommu.c
+ */
+static struct page **imgu_dmamap_alloc_buffer(size_t size, gfp_t gfp)
+{
+	struct page **pages;
+	unsigned int i = 0, count = size >> PAGE_SHIFT;
+	unsigned int order_mask = 1;
+	const gfp_t high_order_gfp = __GFP_NOWARN | __GFP_NORETRY;
+
+	/* Allocate mem for array of page ptrs */
+	pages = kvmalloc_array(count, sizeof(*pages), GFP_KERNEL);
+
+	if (!pages)
+		return NULL;
+
+	gfp |= __GFP_HIGHMEM | __GFP_ZERO;
+
+	while (count) {
+		struct page *page = NULL;
+		unsigned int order_size;
+
+		for (order_mask &= (2U << __fls(count)) - 1;
+		     order_mask; order_mask &= ~order_size) {
+			unsigned int order = __fls(order_mask);
+
+			order_size = 1U << order;
+			page = alloc_pages((order_mask - order_size) ?
+					   gfp | high_order_gfp : gfp, order);
+			if (!page)
+				continue;
+			if (!order)
+				break;
+			if (!PageCompound(page)) {
+				split_page(page, order);
+				break;
+			}
+
+			__free_pages(page, order);
+		}
+		if (!page) {
+			imgu_dmamap_free_buffer(pages, i << PAGE_SHIFT);
+			return NULL;
+		}
+		count -= order_size;
+		while (order_size--)
+			pages[i++] = page++;
+	}
+
+	return pages;
+}
+
+/**
+ * imgu_dmamap_alloc - allocate and map a buffer into KVA
+ * @imgu: struct device pointer
+ * @map: struct to store mapping variables
+ * @len: size required
+ *
+ * Returns:
+ *  KVA on success
+ *  %NULL on failure
+ */
+void *imgu_dmamap_alloc(struct imgu_device *imgu, struct imgu_css_map *map,
+			size_t len)
+{
+	unsigned long shift = iova_shift(&imgu->iova_domain);
+	struct device *dev = &imgu->pci_dev->dev;
+	size_t size = PAGE_ALIGN(len);
+	int count = size >> PAGE_SHIFT;
+	struct page **pages;
+	dma_addr_t iovaddr;
+	struct iova *iova;
+	int i, rval;
+
+	dev_dbg(dev, "%s: allocating %zu\n", __func__, size);
+
+	iova = alloc_iova(&imgu->iova_domain, size >> shift,
+			  imgu->mmu->aperture_end >> shift, 0);
+	if (!iova)
+		return NULL;
+
+	pages = imgu_dmamap_alloc_buffer(size, GFP_KERNEL);
+	if (!pages)
+		goto out_free_iova;
+
+	/* Call IOMMU driver to setup pgt */
+	iovaddr = iova_dma_addr(&imgu->iova_domain, iova);
+	for (i = 0; i < count; ++i) {
+		rval = imgu_mmu_map(imgu->mmu, iovaddr,
+				    page_to_phys(pages[i]), PAGE_SIZE);
+		if (rval)
+			goto out_unmap;
+
+		iovaddr += PAGE_SIZE;
+	}
+
+	map->vaddr = vmap(pages, count, VM_USERMAP, PAGE_KERNEL);
+	if (!map->vaddr)
+		goto out_unmap;
+
+	map->pages = pages;
+	map->size = size;
+	map->daddr = iova_dma_addr(&imgu->iova_domain, iova);
+
+	dev_dbg(dev, "%s: allocated %zu @ IOVA %pad @ VA %p\n", __func__,
+		size, &map->daddr, map->vaddr);
+
+	return map->vaddr;
+
+out_unmap:
+	imgu_dmamap_free_buffer(pages, size);
+	imgu_mmu_unmap(imgu->mmu, iova_dma_addr(&imgu->iova_domain, iova),
+		       i * PAGE_SIZE);
+
+out_free_iova:
+	__free_iova(&imgu->iova_domain, iova);
+
+	return NULL;
+}
+
+void imgu_dmamap_unmap(struct imgu_device *imgu, struct imgu_css_map *map)
+{
+	struct iova *iova;
+
+	iova = find_iova(&imgu->iova_domain,
+			 iova_pfn(&imgu->iova_domain, map->daddr));
+	if (WARN_ON(!iova))
+		return;
+
+	imgu_mmu_unmap(imgu->mmu, iova_dma_addr(&imgu->iova_domain, iova),
+		       iova_size(iova) << iova_shift(&imgu->iova_domain));
+
+	__free_iova(&imgu->iova_domain, iova);
+}
+
+/*
+ * Counterpart of imgu_dmamap_alloc
+ */
+void imgu_dmamap_free(struct imgu_device *imgu, struct imgu_css_map *map)
+{
+	dev_dbg(&imgu->pci_dev->dev, "%s: freeing %zu @ IOVA %pad @ VA %p\n",
+		__func__, map->size, &map->daddr, map->vaddr);
+
+	if (!map->vaddr)
+		return;
+
+	imgu_dmamap_unmap(imgu, map);
+
+	vunmap(map->vaddr);
+	imgu_dmamap_free_buffer(map->pages, map->size);
+	map->vaddr = NULL;
+}
+
+int imgu_dmamap_map_sg(struct imgu_device *imgu, struct scatterlist *sglist,
+		       int nents, struct imgu_css_map *map)
+{
+	unsigned long shift = iova_shift(&imgu->iova_domain);
+	struct scatterlist *sg;
+	struct iova *iova;
+	size_t size = 0;
+	int i;
+
+	for_each_sg(sglist, sg, nents, i) {
+		if (sg->offset)
+			return -EINVAL;
+
+		if (i != nents - 1 && !PAGE_ALIGNED(sg->length))
+			return -EINVAL;
+
+		size += sg->length;
+	}
+
+	size = iova_align(&imgu->iova_domain, size);
+	dev_dbg(&imgu->pci_dev->dev, "dmamap: mapping sg %d entries, %zu pages\n",
+		nents, size >> shift);
+
+	iova = alloc_iova(&imgu->iova_domain, size >> shift,
+			  imgu->mmu->aperture_end >> shift, 0);
+	if (!iova)
+		return -ENOMEM;
+
+	dev_dbg(&imgu->pci_dev->dev, "dmamap: iova low pfn %lu, high pfn %lu\n",
+		iova->pfn_lo, iova->pfn_hi);
+
+	if (imgu_mmu_map_sg(imgu->mmu, iova_dma_addr(&imgu->iova_domain, iova),
+			    sglist, nents) < size)
+		goto out_fail;
+
+	memset(map, 0, sizeof(*map));
+	map->daddr = iova_dma_addr(&imgu->iova_domain, iova);
+	map->size = size;
+
+	return 0;
+
+out_fail:
+	__free_iova(&imgu->iova_domain, iova);
+
+	return -EFAULT;
+}
+
+int imgu_dmamap_init(struct imgu_device *imgu)
+{
+	unsigned long order, base_pfn;
+	int ret = iova_cache_get();
+
+	if (ret)
+		return ret;
+
+	order = __ffs(IPU3_PAGE_SIZE);
+	base_pfn = max_t(unsigned long, 1, imgu->mmu->aperture_start >> order);
+	init_iova_domain(&imgu->iova_domain, 1UL << order, base_pfn);
+
+	return 0;
+}
+
+void imgu_dmamap_exit(struct imgu_device *imgu)
+{
+	put_iova_domain(&imgu->iova_domain);
+	iova_cache_put();
+}
diff --git a/drivers/staging/media/ipu3/ipu3-dmamap.h b/drivers/staging/media/ipu3/ipu3-dmamap.h
new file mode 100644
index 000000000..9db513b3d
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-dmamap.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+/* Copyright 2018 Google LLC. */
+
+#ifndef __IPU3_DMAMAP_H
+#define __IPU3_DMAMAP_H
+
+struct imgu_device;
+struct scatterlist;
+
+void *imgu_dmamap_alloc(struct imgu_device *imgu, struct imgu_css_map *map,
+			size_t len);
+void imgu_dmamap_free(struct imgu_device *imgu, struct imgu_css_map *map);
+
+int imgu_dmamap_map_sg(struct imgu_device *imgu, struct scatterlist *sglist,
+		       int nents, struct imgu_css_map *map);
+void imgu_dmamap_unmap(struct imgu_device *imgu, struct imgu_css_map *map);
+
+int imgu_dmamap_init(struct imgu_device *imgu);
+void imgu_dmamap_exit(struct imgu_device *imgu);
+
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-mmu.c b/drivers/staging/media/ipu3/ipu3-mmu.c
new file mode 100644
index 000000000..cb9bf5fb2
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-mmu.c
@@ -0,0 +1,537 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2018 Intel Corporation.
+ * Copyright 2018 Google LLC.
+ *
+ * Author: Tuukka Toivonen <tuukka.toivonen@intel.com>
+ * Author: Sakari Ailus <sakari.ailus@linux.intel.com>
+ * Author: Samu Onkalo <samu.onkalo@intel.com>
+ * Author: Tomasz Figa <tfiga@chromium.org>
+ *
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/iopoll.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include <asm/set_memory.h>
+
+#include "ipu3-mmu.h"
+
+#define IPU3_PT_BITS		10
+#define IPU3_PT_PTES		(1UL << IPU3_PT_BITS)
+#define IPU3_PT_SIZE		(IPU3_PT_PTES << 2)
+#define IPU3_PT_ORDER		(IPU3_PT_SIZE >> PAGE_SHIFT)
+
+#define IPU3_ADDR2PTE(addr)	((addr) >> IPU3_PAGE_SHIFT)
+#define IPU3_PTE2ADDR(pte)	((phys_addr_t)(pte) << IPU3_PAGE_SHIFT)
+
+#define IPU3_L2PT_SHIFT		IPU3_PT_BITS
+#define IPU3_L2PT_MASK		((1UL << IPU3_L2PT_SHIFT) - 1)
+
+#define IPU3_L1PT_SHIFT		IPU3_PT_BITS
+#define IPU3_L1PT_MASK		((1UL << IPU3_L1PT_SHIFT) - 1)
+
+#define IPU3_MMU_ADDRESS_BITS	(IPU3_PAGE_SHIFT + \
+				 IPU3_L2PT_SHIFT + \
+				 IPU3_L1PT_SHIFT)
+
+#define IMGU_REG_BASE		0x4000
+#define REG_TLB_INVALIDATE	(IMGU_REG_BASE + 0x300)
+#define TLB_INVALIDATE		1
+#define REG_L1_PHYS		(IMGU_REG_BASE + 0x304) /* 27-bit pfn */
+#define REG_GP_HALT		(IMGU_REG_BASE + 0x5dc)
+#define REG_GP_HALTED		(IMGU_REG_BASE + 0x5e0)
+
+struct imgu_mmu {
+	struct device *dev;
+	void __iomem *base;
+	/* protect access to l2pts, l1pt */
+	spinlock_t lock;
+
+	void *dummy_page;
+	u32 dummy_page_pteval;
+
+	u32 *dummy_l2pt;
+	u32 dummy_l2pt_pteval;
+
+	u32 **l2pts;
+	u32 *l1pt;
+
+	struct imgu_mmu_info geometry;
+};
+
+static inline struct imgu_mmu *to_imgu_mmu(struct imgu_mmu_info *info)
+{
+	return container_of(info, struct imgu_mmu, geometry);
+}
+
+/**
+ * imgu_mmu_tlb_invalidate - invalidate translation look-aside buffer
+ * @mmu: MMU to perform the invalidate operation on
+ *
+ * This function invalidates the whole TLB. Must be called when the hardware
+ * is powered on.
+ */
+static void imgu_mmu_tlb_invalidate(struct imgu_mmu *mmu)
+{
+	writel(TLB_INVALIDATE, mmu->base + REG_TLB_INVALIDATE);
+}
+
+static void call_if_imgu_is_powered(struct imgu_mmu *mmu,
+				    void (*func)(struct imgu_mmu *mmu))
+{
+	if (!pm_runtime_get_if_in_use(mmu->dev))
+		return;
+
+	func(mmu);
+	pm_runtime_put(mmu->dev);
+}
+
+/**
+ * imgu_mmu_set_halt - set CIO gate halt bit
+ * @mmu: MMU to set the CIO gate bit in.
+ * @halt: Desired state of the gate bit.
+ *
+ * This function sets the CIO gate bit that controls whether external memory
+ * accesses are allowed. Must be called when the hardware is powered on.
+ */
+static void imgu_mmu_set_halt(struct imgu_mmu *mmu, bool halt)
+{
+	int ret;
+	u32 val;
+
+	writel(halt, mmu->base + REG_GP_HALT);
+	ret = readl_poll_timeout(mmu->base + REG_GP_HALTED,
+				 val, (val & 1) == halt, 1000, 100000);
+
+	if (ret)
+		dev_err(mmu->dev, "failed to %s CIO gate halt\n",
+			halt ? "set" : "clear");
+}
+
+/**
+ * imgu_mmu_alloc_page_table - allocate a pre-filled page table
+ * @pteval: Value to initialize for page table entries with.
+ *
+ * Return: Pointer to allocated page table or NULL on failure.
+ */
+static u32 *imgu_mmu_alloc_page_table(u32 pteval)
+{
+	u32 *pt;
+	int pte;
+
+	pt = (u32 *)__get_free_page(GFP_KERNEL);
+	if (!pt)
+		return NULL;
+
+	for (pte = 0; pte < IPU3_PT_PTES; pte++)
+		pt[pte] = pteval;
+
+	set_memory_uc((unsigned long)pt, IPU3_PT_ORDER);
+
+	return pt;
+}
+
+/**
+ * imgu_mmu_free_page_table - free page table
+ * @pt: Page table to free.
+ */
+static void imgu_mmu_free_page_table(u32 *pt)
+{
+	set_memory_wb((unsigned long)pt, IPU3_PT_ORDER);
+	free_page((unsigned long)pt);
+}
+
+/**
+ * address_to_pte_idx - split IOVA into L1 and L2 page table indices
+ * @iova: IOVA to split.
+ * @l1pt_idx: Output for the L1 page table index.
+ * @l2pt_idx: Output for the L2 page index.
+ */
+static inline void address_to_pte_idx(unsigned long iova, u32 *l1pt_idx,
+				      u32 *l2pt_idx)
+{
+	iova >>= IPU3_PAGE_SHIFT;
+
+	if (l2pt_idx)
+		*l2pt_idx = iova & IPU3_L2PT_MASK;
+
+	iova >>= IPU3_L2PT_SHIFT;
+
+	if (l1pt_idx)
+		*l1pt_idx = iova & IPU3_L1PT_MASK;
+}
+
+static u32 *imgu_mmu_get_l2pt(struct imgu_mmu *mmu, u32 l1pt_idx)
+{
+	unsigned long flags;
+	u32 *l2pt, *new_l2pt;
+	u32 pteval;
+
+	spin_lock_irqsave(&mmu->lock, flags);
+
+	l2pt = mmu->l2pts[l1pt_idx];
+	if (l2pt) {
+		spin_unlock_irqrestore(&mmu->lock, flags);
+		return l2pt;
+	}
+
+	spin_unlock_irqrestore(&mmu->lock, flags);
+
+	new_l2pt = imgu_mmu_alloc_page_table(mmu->dummy_page_pteval);
+	if (!new_l2pt)
+		return NULL;
+
+	spin_lock_irqsave(&mmu->lock, flags);
+
+	dev_dbg(mmu->dev, "allocated page table %p for l1pt_idx %u\n",
+		new_l2pt, l1pt_idx);
+
+	l2pt = mmu->l2pts[l1pt_idx];
+	if (l2pt) {
+		spin_unlock_irqrestore(&mmu->lock, flags);
+		imgu_mmu_free_page_table(new_l2pt);
+		return l2pt;
+	}
+
+	l2pt = new_l2pt;
+	mmu->l2pts[l1pt_idx] = new_l2pt;
+
+	pteval = IPU3_ADDR2PTE(virt_to_phys(new_l2pt));
+	mmu->l1pt[l1pt_idx] = pteval;
+
+	spin_unlock_irqrestore(&mmu->lock, flags);
+	return l2pt;
+}
+
+static int __imgu_mmu_map(struct imgu_mmu *mmu, unsigned long iova,
+			  phys_addr_t paddr)
+{
+	u32 l1pt_idx, l2pt_idx;
+	unsigned long flags;
+	u32 *l2pt;
+
+	if (!mmu)
+		return -ENODEV;
+
+	address_to_pte_idx(iova, &l1pt_idx, &l2pt_idx);
+
+	l2pt = imgu_mmu_get_l2pt(mmu, l1pt_idx);
+	if (!l2pt)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&mmu->lock, flags);
+
+	if (l2pt[l2pt_idx] != mmu->dummy_page_pteval) {
+		spin_unlock_irqrestore(&mmu->lock, flags);
+		return -EBUSY;
+	}
+
+	l2pt[l2pt_idx] = IPU3_ADDR2PTE(paddr);
+
+	spin_unlock_irqrestore(&mmu->lock, flags);
+
+	return 0;
+}
+
+/**
+ * imgu_mmu_map - map a buffer to a physical address
+ *
+ * @info: MMU mappable range
+ * @iova: the virtual address
+ * @paddr: the physical address
+ * @size: length of the mappable area
+ *
+ * The function has been adapted from iommu_map() in
+ * drivers/iommu/iommu.c .
+ */
+int imgu_mmu_map(struct imgu_mmu_info *info, unsigned long iova,
+		 phys_addr_t paddr, size_t size)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+	int ret = 0;
+
+	/*
+	 * both the virtual address and the physical one, as well as
+	 * the size of the mapping, must be aligned (at least) to the
+	 * size of the smallest page supported by the hardware
+	 */
+	if (!IS_ALIGNED(iova | paddr | size, IPU3_PAGE_SIZE)) {
+		dev_err(mmu->dev, "unaligned: iova 0x%lx pa %pa size 0x%zx\n",
+			iova, &paddr, size);
+		return -EINVAL;
+	}
+
+	dev_dbg(mmu->dev, "map: iova 0x%lx pa %pa size 0x%zx\n",
+		iova, &paddr, size);
+
+	while (size) {
+		dev_dbg(mmu->dev, "mapping: iova 0x%lx pa %pa\n", iova, &paddr);
+
+		ret = __imgu_mmu_map(mmu, iova, paddr);
+		if (ret)
+			break;
+
+		iova += IPU3_PAGE_SIZE;
+		paddr += IPU3_PAGE_SIZE;
+		size -= IPU3_PAGE_SIZE;
+	}
+
+	call_if_imgu_is_powered(mmu, imgu_mmu_tlb_invalidate);
+
+	return ret;
+}
+
+/**
+ * imgu_mmu_map_sg - Map a scatterlist
+ *
+ * @info: MMU mappable range
+ * @iova: the virtual address
+ * @sg: the scatterlist to map
+ * @nents: number of entries in the scatterlist
+ *
+ * The function has been adapted from default_iommu_map_sg() in
+ * drivers/iommu/iommu.c .
+ */
+size_t imgu_mmu_map_sg(struct imgu_mmu_info *info, unsigned long iova,
+		       struct scatterlist *sg, unsigned int nents)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+	struct scatterlist *s;
+	size_t s_length, mapped = 0;
+	unsigned int i;
+	int ret;
+
+	for_each_sg(sg, s, nents, i) {
+		phys_addr_t phys = page_to_phys(sg_page(s)) + s->offset;
+
+		s_length = s->length;
+
+		if (!IS_ALIGNED(s->offset, IPU3_PAGE_SIZE))
+			goto out_err;
+
+		/* must be IPU3_PAGE_SIZE aligned to be mapped singlely */
+		if (i == nents - 1 && !IS_ALIGNED(s->length, IPU3_PAGE_SIZE))
+			s_length = PAGE_ALIGN(s->length);
+
+		ret = imgu_mmu_map(info, iova + mapped, phys, s_length);
+		if (ret)
+			goto out_err;
+
+		mapped += s_length;
+	}
+
+	call_if_imgu_is_powered(mmu, imgu_mmu_tlb_invalidate);
+
+	return mapped;
+
+out_err:
+	/* undo mappings already done */
+	imgu_mmu_unmap(info, iova, mapped);
+
+	return 0;
+}
+
+static size_t __imgu_mmu_unmap(struct imgu_mmu *mmu,
+			       unsigned long iova, size_t size)
+{
+	u32 l1pt_idx, l2pt_idx;
+	unsigned long flags;
+	size_t unmap = size;
+	u32 *l2pt;
+
+	if (!mmu)
+		return 0;
+
+	address_to_pte_idx(iova, &l1pt_idx, &l2pt_idx);
+
+	spin_lock_irqsave(&mmu->lock, flags);
+
+	l2pt = mmu->l2pts[l1pt_idx];
+	if (!l2pt) {
+		spin_unlock_irqrestore(&mmu->lock, flags);
+		return 0;
+	}
+
+	if (l2pt[l2pt_idx] == mmu->dummy_page_pteval)
+		unmap = 0;
+
+	l2pt[l2pt_idx] = mmu->dummy_page_pteval;
+
+	spin_unlock_irqrestore(&mmu->lock, flags);
+
+	return unmap;
+}
+
+/**
+ * imgu_mmu_unmap - Unmap a buffer
+ *
+ * @info: MMU mappable range
+ * @iova: the virtual address
+ * @size: the length of the buffer
+ *
+ * The function has been adapted from iommu_unmap() in
+ * drivers/iommu/iommu.c .
+ */
+size_t imgu_mmu_unmap(struct imgu_mmu_info *info, unsigned long iova,
+		      size_t size)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+	size_t unmapped_page, unmapped = 0;
+
+	/*
+	 * The virtual address, as well as the size of the mapping, must be
+	 * aligned (at least) to the size of the smallest page supported
+	 * by the hardware
+	 */
+	if (!IS_ALIGNED(iova | size, IPU3_PAGE_SIZE)) {
+		dev_err(mmu->dev, "unaligned: iova 0x%lx size 0x%zx\n",
+			iova, size);
+		return -EINVAL;
+	}
+
+	dev_dbg(mmu->dev, "unmap this: iova 0x%lx size 0x%zx\n", iova, size);
+
+	/*
+	 * Keep iterating until we either unmap 'size' bytes (or more)
+	 * or we hit an area that isn't mapped.
+	 */
+	while (unmapped < size) {
+		unmapped_page = __imgu_mmu_unmap(mmu, iova, IPU3_PAGE_SIZE);
+		if (!unmapped_page)
+			break;
+
+		dev_dbg(mmu->dev, "unmapped: iova 0x%lx size 0x%zx\n",
+			iova, unmapped_page);
+
+		iova += unmapped_page;
+		unmapped += unmapped_page;
+	}
+
+	call_if_imgu_is_powered(mmu, imgu_mmu_tlb_invalidate);
+
+	return unmapped;
+}
+
+/**
+ * imgu_mmu_init() - initialize IPU3 MMU block
+ *
+ * @parent:	struct device parent
+ * @base:	IOMEM base of hardware registers.
+ *
+ * Return: Pointer to IPU3 MMU private data pointer or ERR_PTR() on error.
+ */
+struct imgu_mmu_info *imgu_mmu_init(struct device *parent, void __iomem *base)
+{
+	struct imgu_mmu *mmu;
+	u32 pteval;
+
+	mmu = kzalloc(sizeof(*mmu), GFP_KERNEL);
+	if (!mmu)
+		return ERR_PTR(-ENOMEM);
+
+	mmu->dev = parent;
+	mmu->base = base;
+	spin_lock_init(&mmu->lock);
+
+	/* Disallow external memory access when having no valid page tables. */
+	imgu_mmu_set_halt(mmu, true);
+
+	/*
+	 * The MMU does not have a "valid" bit, so we have to use a dummy
+	 * page for invalid entries.
+	 */
+	mmu->dummy_page = (void *)__get_free_page(GFP_KERNEL);
+	if (!mmu->dummy_page)
+		goto fail_group;
+	pteval = IPU3_ADDR2PTE(virt_to_phys(mmu->dummy_page));
+	mmu->dummy_page_pteval = pteval;
+
+	/*
+	 * Allocate a dummy L2 page table with all entries pointing to
+	 * the dummy page.
+	 */
+	mmu->dummy_l2pt = imgu_mmu_alloc_page_table(pteval);
+	if (!mmu->dummy_l2pt)
+		goto fail_dummy_page;
+	pteval = IPU3_ADDR2PTE(virt_to_phys(mmu->dummy_l2pt));
+	mmu->dummy_l2pt_pteval = pteval;
+
+	/*
+	 * Allocate the array of L2PT CPU pointers, initialized to zero,
+	 * which means the dummy L2PT allocated above.
+	 */
+	mmu->l2pts = vzalloc(IPU3_PT_PTES * sizeof(*mmu->l2pts));
+	if (!mmu->l2pts)
+		goto fail_l2pt;
+
+	/* Allocate the L1 page table. */
+	mmu->l1pt = imgu_mmu_alloc_page_table(mmu->dummy_l2pt_pteval);
+	if (!mmu->l1pt)
+		goto fail_l2pts;
+
+	pteval = IPU3_ADDR2PTE(virt_to_phys(mmu->l1pt));
+	writel(pteval, mmu->base + REG_L1_PHYS);
+	imgu_mmu_tlb_invalidate(mmu);
+	imgu_mmu_set_halt(mmu, false);
+
+	mmu->geometry.aperture_start = 0;
+	mmu->geometry.aperture_end = DMA_BIT_MASK(IPU3_MMU_ADDRESS_BITS);
+
+	return &mmu->geometry;
+
+fail_l2pts:
+	vfree(mmu->l2pts);
+fail_l2pt:
+	imgu_mmu_free_page_table(mmu->dummy_l2pt);
+fail_dummy_page:
+	free_page((unsigned long)mmu->dummy_page);
+fail_group:
+	kfree(mmu);
+
+	return ERR_PTR(-ENOMEM);
+}
+
+/**
+ * imgu_mmu_exit() - clean up IPU3 MMU block
+ *
+ * @info: MMU mappable range
+ */
+void imgu_mmu_exit(struct imgu_mmu_info *info)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+
+	/* We are going to free our page tables, no more memory access. */
+	imgu_mmu_set_halt(mmu, true);
+	imgu_mmu_tlb_invalidate(mmu);
+
+	imgu_mmu_free_page_table(mmu->l1pt);
+	vfree(mmu->l2pts);
+	imgu_mmu_free_page_table(mmu->dummy_l2pt);
+	free_page((unsigned long)mmu->dummy_page);
+	kfree(mmu);
+}
+
+void imgu_mmu_suspend(struct imgu_mmu_info *info)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+
+	imgu_mmu_set_halt(mmu, true);
+}
+
+void imgu_mmu_resume(struct imgu_mmu_info *info)
+{
+	struct imgu_mmu *mmu = to_imgu_mmu(info);
+	u32 pteval;
+
+	imgu_mmu_set_halt(mmu, true);
+
+	pteval = IPU3_ADDR2PTE(virt_to_phys(mmu->l1pt));
+	writel(pteval, mmu->base + REG_L1_PHYS);
+
+	imgu_mmu_tlb_invalidate(mmu);
+	imgu_mmu_set_halt(mmu, false);
+}
diff --git a/drivers/staging/media/ipu3/ipu3-mmu.h b/drivers/staging/media/ipu3/ipu3-mmu.h
new file mode 100644
index 000000000..a5f0bca7e
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-mmu.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+/* Copyright 2018 Google LLC. */
+
+#ifndef __IPU3_MMU_H
+#define __IPU3_MMU_H
+
+#define IPU3_PAGE_SHIFT		12
+#define IPU3_PAGE_SIZE		(1UL << IPU3_PAGE_SHIFT)
+
+/**
+ * struct imgu_mmu_info - Describes mmu geometry
+ *
+ * @aperture_start:	First address that can be mapped
+ * @aperture_end:	Last address that can be mapped
+ */
+struct imgu_mmu_info {
+	dma_addr_t aperture_start;
+	dma_addr_t aperture_end;
+};
+
+struct device;
+struct scatterlist;
+
+struct imgu_mmu_info *imgu_mmu_init(struct device *parent, void __iomem *base);
+void imgu_mmu_exit(struct imgu_mmu_info *info);
+void imgu_mmu_suspend(struct imgu_mmu_info *info);
+void imgu_mmu_resume(struct imgu_mmu_info *info);
+
+int imgu_mmu_map(struct imgu_mmu_info *info, unsigned long iova,
+		 phys_addr_t paddr, size_t size);
+size_t imgu_mmu_unmap(struct imgu_mmu_info *info, unsigned long iova,
+		      size_t size);
+size_t imgu_mmu_map_sg(struct imgu_mmu_info *info, unsigned long iova,
+		       struct scatterlist *sg, unsigned int nents);
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-tables.c b/drivers/staging/media/ipu3/ipu3-tables.c
new file mode 100644
index 000000000..3a3730bd4
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-tables.c
@@ -0,0 +1,9609 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include "ipu3-tables.h"
+
+#define X					0	/*  Don't care value */
+
+const struct imgu_css_bds_config
+			imgu_css_bds_configs[IMGU_BDS_CONFIG_LEN] = { {
+	/* Scale factor 32 / (32 + 0) = 1 */
+	.hor_phase_arr = {
+		.even = { { 0, 0, 64, 6, 0, 0, 0 } },
+		.odd = { { 0, 0, 64, 6, 0, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 0, 64, 6, 0, 0, 0 } },
+		.odd = { { 0, 0, 64, 6, 0, 0, 0 } } },
+	.ptrn_arr = { { 0x3 } },
+	.sample_patrn_length = 2,
+	.hor_ds_en = 0,
+	.ver_ds_en = 0
+}, {
+	/* Scale factor 32 / (32 + 1) = 0.969697 */
+	.hor_phase_arr = {
+		.even = { { 0, 3, 122, 7, 3, 0, 0 },
+			 { 0, 0, 122, 7, 7, -1, 0 },
+			 { 0, -3, 122, 7, 10, -1, 0 },
+			 { 0, -5, 121, 7, 14, -2, 0 },
+			 { 0, -7, 120, 7, 18, -3, 0 },
+			 { 0, -9, 118, 7, 23, -4, 0 },
+			 { 0, -11, 116, 7, 27, -4, 0 },
+			 { 0, -12, 113, 7, 32, -5, 0 },
+			 { 0, -13, 110, 7, 37, -6, 0 },
+			 { 0, -14, 107, 7, 42, -7, 0 },
+			 { 0, -14, 103, 7, 47, -8, 0 },
+			 { 0, -15, 100, 7, 52, -9, 0 },
+			 { 0, -15, 96, 7, 57, -10, 0 },
+			 { 0, -15, 92, 7, 62, -11, 0 },
+			 { 0, -14, 86, 7, 68, -12, 0 },
+			 { 0, -14, 82, 7, 73, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 73, 7, 82, -14, 0 },
+			 { 0, -12, 68, 7, 86, -14, 0 },
+			 { 0, -11, 62, 7, 92, -15, 0 },
+			 { 0, -10, 57, 7, 96, -15, 0 },
+			 { 0, -9, 52, 7, 100, -15, 0 },
+			 { 0, -8, 47, 7, 103, -14, 0 },
+			 { 0, -7, 42, 7, 107, -14, 0 },
+			 { 0, -6, 37, 7, 110, -13, 0 },
+			 { 0, -5, 32, 7, 113, -12, 0 },
+			 { 0, -4, 27, 7, 116, -11, 0 },
+			 { 0, -4, 23, 7, 118, -9, 0 },
+			 { 0, -3, 18, 7, 120, -7, 0 },
+			 { 0, -2, 14, 7, 121, -5, 0 },
+			 { 0, -1, 10, 7, 122, -3, 0 },
+			 { 0, -1, 7, 7, 122, 0, 0 } },
+		.odd = { { 0, 2, 122, 7, 5, -1, 0 },
+			 { 0, -1, 122, 7, 8, -1, 0 },
+			 { 0, -4, 122, 7, 12, -2, 0 },
+			 { 0, -6, 120, 7, 16, -2, 0 },
+			 { 0, -8, 118, 7, 21, -3, 0 },
+			 { 0, -10, 117, 7, 25, -4, 0 },
+			 { 0, -11, 114, 7, 30, -5, 0 },
+			 { 0, -13, 112, 7, 35, -6, 0 },
+			 { 0, -14, 109, 7, 40, -7, 0 },
+			 { 0, -14, 105, 7, 45, -8, 0 },
+			 { 0, -15, 102, 7, 50, -9, 0 },
+			 { 0, -15, 98, 7, 55, -10, 0 },
+			 { 0, -15, 94, 7, 60, -11, 0 },
+			 { 0, -15, 90, 7, 65, -12, 0 },
+			 { 0, -14, 85, 7, 70, -13, 0 },
+			 { 0, -14, 80, 7, 75, -13, 0 },
+			 { 0, -13, 75, 7, 80, -14, 0 },
+			 { 0, -13, 70, 7, 85, -14, 0 },
+			 { 0, -12, 65, 7, 90, -15, 0 },
+			 { 0, -11, 60, 7, 94, -15, 0 },
+			 { 0, -10, 55, 7, 98, -15, 0 },
+			 { 0, -9, 50, 7, 102, -15, 0 },
+			 { 0, -8, 45, 7, 105, -14, 0 },
+			 { 0, -7, 40, 7, 109, -14, 0 },
+			 { 0, -6, 35, 7, 112, -13, 0 },
+			 { 0, -5, 30, 7, 114, -11, 0 },
+			 { 0, -4, 25, 7, 117, -10, 0 },
+			 { 0, -3, 21, 7, 118, -8, 0 },
+			 { 0, -2, 16, 7, 120, -6, 0 },
+			 { 0, -2, 12, 7, 122, -4, 0 },
+			 { 0, -1, 8, 7, 122, -1, 0 },
+			 { 0, -1, 5, 7, 122, 2, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 3, 122, 7, 3, 0, 0 },
+			 { 0, 0, 122, 7, 7, -1, 0 },
+			 { 0, -3, 122, 7, 10, -1, 0 },
+			 { 0, -5, 121, 7, 14, -2, 0 },
+			 { 0, -7, 120, 7, 18, -3, 0 },
+			 { 0, -9, 118, 7, 23, -4, 0 },
+			 { 0, -11, 116, 7, 27, -4, 0 },
+			 { 0, -12, 113, 7, 32, -5, 0 },
+			 { 0, -13, 110, 7, 37, -6, 0 },
+			 { 0, -14, 107, 7, 42, -7, 0 },
+			 { 0, -14, 103, 7, 47, -8, 0 },
+			 { 0, -15, 100, 7, 52, -9, 0 },
+			 { 0, -15, 96, 7, 57, -10, 0 },
+			 { 0, -15, 92, 7, 62, -11, 0 },
+			 { 0, -14, 86, 7, 68, -12, 0 },
+			 { 0, -14, 82, 7, 73, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 73, 7, 82, -14, 0 },
+			 { 0, -12, 68, 7, 86, -14, 0 },
+			 { 0, -11, 62, 7, 92, -15, 0 },
+			 { 0, -10, 57, 7, 96, -15, 0 },
+			 { 0, -9, 52, 7, 100, -15, 0 },
+			 { 0, -8, 47, 7, 103, -14, 0 },
+			 { 0, -7, 42, 7, 107, -14, 0 },
+			 { 0, -6, 37, 7, 110, -13, 0 },
+			 { 0, -5, 32, 7, 113, -12, 0 },
+			 { 0, -4, 27, 7, 116, -11, 0 },
+			 { 0, -4, 23, 7, 118, -9, 0 },
+			 { 0, -3, 18, 7, 120, -7, 0 },
+			 { 0, -2, 14, 7, 121, -5, 0 },
+			 { 0, -1, 10, 7, 122, -3, 0 },
+			 { 0, -1, 7, 7, 122, 0, 0 } },
+		.odd = { { 0, 2, 122, 7, 5, -1, 0 },
+			 { 0, -1, 122, 7, 8, -1, 0 },
+			 { 0, -4, 122, 7, 12, -2, 0 },
+			 { 0, -6, 120, 7, 16, -2, 0 },
+			 { 0, -8, 118, 7, 21, -3, 0 },
+			 { 0, -10, 117, 7, 25, -4, 0 },
+			 { 0, -11, 114, 7, 30, -5, 0 },
+			 { 0, -13, 112, 7, 35, -6, 0 },
+			 { 0, -14, 109, 7, 40, -7, 0 },
+			 { 0, -14, 105, 7, 45, -8, 0 },
+			 { 0, -15, 102, 7, 50, -9, 0 },
+			 { 0, -15, 98, 7, 55, -10, 0 },
+			 { 0, -15, 94, 7, 60, -11, 0 },
+			 { 0, -15, 90, 7, 65, -12, 0 },
+			 { 0, -14, 85, 7, 70, -13, 0 },
+			 { 0, -14, 80, 7, 75, -13, 0 },
+			 { 0, -13, 75, 7, 80, -14, 0 },
+			 { 0, -13, 70, 7, 85, -14, 0 },
+			 { 0, -12, 65, 7, 90, -15, 0 },
+			 { 0, -11, 60, 7, 94, -15, 0 },
+			 { 0, -10, 55, 7, 98, -15, 0 },
+			 { 0, -9, 50, 7, 102, -15, 0 },
+			 { 0, -8, 45, 7, 105, -14, 0 },
+			 { 0, -7, 40, 7, 109, -14, 0 },
+			 { 0, -6, 35, 7, 112, -13, 0 },
+			 { 0, -5, 30, 7, 114, -11, 0 },
+			 { 0, -4, 25, 7, 117, -10, 0 },
+			 { 0, -3, 21, 7, 118, -8, 0 },
+			 { 0, -2, 16, 7, 120, -6, 0 },
+			 { 0, -2, 12, 7, 122, -4, 0 },
+			 { 0, -1, 8, 7, 122, -1, 0 },
+			 { 0, -1, 5, 7, 122, 2, 0 } } },
+	.ptrn_arr = { { 0xffffffff, 0xffffffff } },
+	.sample_patrn_length = 66,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 2) = 0.941176 */
+	.hor_phase_arr = {
+		.even = { { -1, 6, 118, 7, 6, -1, 0 },
+			 { 0, 0, 117, 7, 13, -2, 0 },
+			 { 0, -5, 116, 7, 21, -4, 0 },
+			 { 0, -9, 113, 7, 30, -6, 0 },
+			 { 0, -12, 109, 7, 39, -8, 0 },
+			 { 0, -13, 102, 7, 49, -10, 0 },
+			 { 0, -14, 94, 7, 59, -11, 0 },
+			 { 0, -14, 86, 7, 69, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 69, 7, 86, -14, 0 },
+			 { 0, -11, 59, 7, 94, -14, 0 },
+			 { 0, -10, 49, 7, 102, -13, 0 },
+			 { 0, -8, 39, 7, 109, -12, 0 },
+			 { 0, -6, 30, 7, 113, -9, 0 },
+			 { 0, -4, 21, 7, 116, -5, 0 },
+			 { 0, -2, 13, 7, 117, 0, 0 } },
+		.odd = { { -1, 3, 118, 7, 10, -2, 0 },
+			 { 0, -3, 117, 7, 17, -3, 0 },
+			 { 0, -7, 114, 7, 26, -5, 0 },
+			 { 0, -10, 110, 7, 35, -7, 0 },
+			 { 0, -13, 106, 7, 44, -9, 0 },
+			 { 0, -14, 99, 7, 54, -11, 0 },
+			 { 0, -14, 90, 7, 64, -12, 0 },
+			 { 0, -14, 82, 7, 73, -13, 0 },
+			 { 0, -13, 73, 7, 82, -14, 0 },
+			 { 0, -12, 64, 7, 90, -14, 0 },
+			 { 0, -11, 54, 7, 99, -14, 0 },
+			 { 0, -9, 44, 7, 106, -13, 0 },
+			 { 0, -7, 35, 7, 110, -10, 0 },
+			 { 0, -5, 26, 7, 114, -7, 0 },
+			 { 0, -3, 17, 7, 117, -3, 0 },
+			 { 0, -2, 10, 7, 118, 3, -1 } } },
+	.ver_phase_arr = {
+		.even = { { -1, 6, 118, 7, 6, -1, 0 },
+			 { 0, 0, 117, 7, 13, -2, 0 },
+			 { 0, -5, 116, 7, 21, -4, 0 },
+			 { 0, -9, 113, 7, 30, -6, 0 },
+			 { 0, -12, 109, 7, 39, -8, 0 },
+			 { 0, -13, 102, 7, 49, -10, 0 },
+			 { 0, -14, 94, 7, 59, -11, 0 },
+			 { 0, -14, 86, 7, 69, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 69, 7, 86, -14, 0 },
+			 { 0, -11, 59, 7, 94, -14, 0 },
+			 { 0, -10, 49, 7, 102, -13, 0 },
+			 { 0, -8, 39, 7, 109, -12, 0 },
+			 { 0, -6, 30, 7, 113, -9, 0 },
+			 { 0, -4, 21, 7, 116, -5, 0 },
+			 { 0, -2, 13, 7, 117, 0, 0 } },
+		.odd = { { -1, 3, 118, 7, 10, -2, 0 },
+			 { 0, -3, 117, 7, 17, -3, 0 },
+			 { 0, -7, 114, 7, 26, -5, 0 },
+			 { 0, -10, 110, 7, 35, -7, 0 },
+			 { 0, -13, 106, 7, 44, -9, 0 },
+			 { 0, -14, 99, 7, 54, -11, 0 },
+			 { 0, -14, 90, 7, 64, -12, 0 },
+			 { 0, -14, 82, 7, 73, -13, 0 },
+			 { 0, -13, 73, 7, 82, -14, 0 },
+			 { 0, -12, 64, 7, 90, -14, 0 },
+			 { 0, -11, 54, 7, 99, -14, 0 },
+			 { 0, -9, 44, 7, 106, -13, 0 },
+			 { 0, -7, 35, 7, 110, -10, 0 },
+			 { 0, -5, 26, 7, 114, -7, 0 },
+			 { 0, -3, 17, 7, 117, -3, 0 },
+			 { 0, -2, 10, 7, 118, 3, -1 } } },
+	.ptrn_arr = { { 0xffffffff } },
+	.sample_patrn_length = 34,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 3) = 0.914286 */
+	.hor_phase_arr = {
+		.even = { { -2, 9, 114, 7, 9, -2, 0 },
+			 { -1, 0, 114, 7, 20, -5, 0 },
+			 { 0, -7, 110, 7, 32, -7, 0 },
+			 { 0, -11, 103, 7, 46, -10, 0 },
+			 { 0, -13, 93, 7, 60, -12, 0 },
+			 { 0, -14, 82, 7, 74, -14, 0 },
+			 { 0, -13, 69, 7, 86, -14, 0 },
+			 { 0, -11, 55, 7, 97, -13, 0 },
+			 { 0, -9, 41, 7, 106, -10, 0 },
+			 { 0, -6, 28, 7, 111, -5, 0 },
+			 { 0, -4, 16, 7, 114, 3, -1 },
+			 { -2, 6, 115, 7, 12, -3, 0 },
+			 { 0, -2, 111, 7, 24, -5, 0 },
+			 { 0, -8, 107, 7, 37, -8, 0 },
+			 { 0, -12, 100, 7, 51, -11, 0 },
+			 { 0, -14, 90, 7, 65, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 65, 7, 90, -14, 0 },
+			 { 0, -11, 51, 7, 100, -12, 0 },
+			 { 0, -8, 37, 7, 107, -8, 0 },
+			 { 0, -5, 24, 7, 111, -2, 0 },
+			 { 0, -3, 12, 7, 115, 6, -2 },
+			 { -1, 3, 114, 7, 16, -4, 0 },
+			 { 0, -5, 111, 7, 28, -6, 0 },
+			 { 0, -10, 106, 7, 41, -9, 0 },
+			 { 0, -13, 97, 7, 55, -11, 0 },
+			 { 0, -14, 86, 7, 69, -13, 0 },
+			 { 0, -14, 74, 7, 82, -14, 0 },
+			 { 0, -12, 60, 7, 93, -13, 0 },
+			 { 0, -10, 46, 7, 103, -11, 0 },
+			 { 0, -7, 32, 7, 110, -7, 0 },
+			 { 0, -5, 20, 7, 114, 0, -1 } },
+		.odd = { { -1, 4, 114, 7, 14, -3, 0 },
+			 { 0, -4, 112, 7, 26, -6, 0 },
+			 { 0, -9, 107, 7, 39, -9, 0 },
+			 { 0, -13, 99, 7, 53, -11, 0 },
+			 { 0, -14, 88, 7, 67, -13, 0 },
+			 { 0, -14, 76, 7, 80, -14, 0 },
+			 { 0, -13, 62, 7, 93, -14, 0 },
+			 { 0, -10, 48, 7, 102, -12, 0 },
+			 { 0, -8, 35, 7, 109, -8, 0 },
+			 { 0, -5, 22, 7, 112, -1, 0 },
+			 { 0, -3, 11, 7, 115, 7, -2 },
+			 { -1, 1, 114, 7, 18, -4, 0 },
+			 { 0, -6, 111, 7, 30, -7, 0 },
+			 { 0, -10, 103, 7, 44, -9, 0 },
+			 { 0, -13, 95, 7, 58, -12, 0 },
+			 { 0, -14, 85, 7, 71, -14, 0 },
+			 { 0, -14, 71, 7, 85, -14, 0 },
+			 { 0, -12, 58, 7, 95, -13, 0 },
+			 { 0, -9, 44, 7, 103, -10, 0 },
+			 { 0, -7, 30, 7, 111, -6, 0 },
+			 { 0, -4, 18, 7, 114, 1, -1 },
+			 { -2, 7, 115, 7, 11, -3, 0 },
+			 { 0, -1, 112, 7, 22, -5, 0 },
+			 { 0, -8, 109, 7, 35, -8, 0 },
+			 { 0, -12, 102, 7, 48, -10, 0 },
+			 { 0, -14, 93, 7, 62, -13, 0 },
+			 { 0, -14, 80, 7, 76, -14, 0 },
+			 { 0, -13, 67, 7, 88, -14, 0 },
+			 { 0, -11, 53, 7, 99, -13, 0 },
+			 { 0, -9, 39, 7, 107, -9, 0 },
+			 { 0, -6, 26, 7, 112, -4, 0 },
+			 { 0, -3, 14, 7, 114, 4, -1 } } },
+	.ver_phase_arr = {
+		.even = { { -2, 9, 114, 7, 9, -2, 0 },
+			 { -1, 0, 114, 7, 20, -5, 0 },
+			 { 0, -7, 110, 7, 32, -7, 0 },
+			 { 0, -11, 103, 7, 46, -10, 0 },
+			 { 0, -13, 93, 7, 60, -12, 0 },
+			 { 0, -14, 82, 7, 74, -14, 0 },
+			 { 0, -13, 69, 7, 86, -14, 0 },
+			 { 0, -11, 55, 7, 97, -13, 0 },
+			 { 0, -9, 41, 7, 106, -10, 0 },
+			 { 0, -6, 28, 7, 111, -5, 0 },
+			 { 0, -4, 16, 7, 114, 3, -1 },
+			 { -2, 6, 115, 7, 12, -3, 0 },
+			 { 0, -2, 111, 7, 24, -5, 0 },
+			 { 0, -8, 107, 7, 37, -8, 0 },
+			 { 0, -12, 100, 7, 51, -11, 0 },
+			 { 0, -14, 90, 7, 65, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 65, 7, 90, -14, 0 },
+			 { 0, -11, 51, 7, 100, -12, 0 },
+			 { 0, -8, 37, 7, 107, -8, 0 },
+			 { 0, -5, 24, 7, 111, -2, 0 },
+			 { 0, -3, 12, 7, 115, 6, -2 },
+			 { -1, 3, 114, 7, 16, -4, 0 },
+			 { 0, -5, 111, 7, 28, -6, 0 },
+			 { 0, -10, 106, 7, 41, -9, 0 },
+			 { 0, -13, 97, 7, 55, -11, 0 },
+			 { 0, -14, 86, 7, 69, -13, 0 },
+			 { 0, -14, 74, 7, 82, -14, 0 },
+			 { 0, -12, 60, 7, 93, -13, 0 },
+			 { 0, -10, 46, 7, 103, -11, 0 },
+			 { 0, -7, 32, 7, 110, -7, 0 },
+			 { 0, -5, 20, 7, 114, 0, -1 } },
+		.odd = { { -1, 4, 114, 7, 14, -3, 0 },
+			 { 0, -4, 112, 7, 26, -6, 0 },
+			 { 0, -9, 107, 7, 39, -9, 0 },
+			 { 0, -13, 99, 7, 53, -11, 0 },
+			 { 0, -14, 88, 7, 67, -13, 0 },
+			 { 0, -14, 76, 7, 80, -14, 0 },
+			 { 0, -13, 62, 7, 93, -14, 0 },
+			 { 0, -10, 48, 7, 102, -12, 0 },
+			 { 0, -8, 35, 7, 109, -8, 0 },
+			 { 0, -5, 22, 7, 112, -1, 0 },
+			 { 0, -3, 11, 7, 115, 7, -2 },
+			 { -1, 1, 114, 7, 18, -4, 0 },
+			 { 0, -6, 111, 7, 30, -7, 0 },
+			 { 0, -10, 103, 7, 44, -9, 0 },
+			 { 0, -13, 95, 7, 58, -12, 0 },
+			 { 0, -14, 85, 7, 71, -14, 0 },
+			 { 0, -14, 71, 7, 85, -14, 0 },
+			 { 0, -12, 58, 7, 95, -13, 0 },
+			 { 0, -9, 44, 7, 103, -10, 0 },
+			 { 0, -7, 30, 7, 111, -6, 0 },
+			 { 0, -4, 18, 7, 114, 1, -1 },
+			 { -2, 7, 115, 7, 11, -3, 0 },
+			 { 0, -1, 112, 7, 22, -5, 0 },
+			 { 0, -8, 109, 7, 35, -8, 0 },
+			 { 0, -12, 102, 7, 48, -10, 0 },
+			 { 0, -14, 93, 7, 62, -13, 0 },
+			 { 0, -14, 80, 7, 76, -14, 0 },
+			 { 0, -13, 67, 7, 88, -14, 0 },
+			 { 0, -11, 53, 7, 99, -13, 0 },
+			 { 0, -9, 39, 7, 107, -9, 0 },
+			 { 0, -6, 26, 7, 112, -4, 0 },
+			 { 0, -3, 14, 7, 114, 4, -1 } } },
+	.ptrn_arr = { { 0xff3fffff, 0xffff9fff, 0xf } },
+	.sample_patrn_length = 70,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 4) = 0.888889 */
+	.hor_phase_arr = {
+		.even = { { -3, 12, 110, 7, 12, -3, 0 },
+			 { -1, 0, 110, 7, 26, -7, 0 },
+			 { 0, -8, 103, 7, 43, -10, 0 },
+			 { 0, -12, 92, 7, 61, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 61, 7, 92, -12, 0 },
+			 { 0, -10, 43, 7, 103, -8, 0 },
+			 { 0, -7, 26, 7, 110, 0, -1 } },
+		.odd = { { -2, 5, 111, 7, 19, -5, 0 },
+			 { 0, -4, 106, 7, 34, -8, 0 },
+			 { 0, -11, 98, 7, 52, -11, 0 },
+			 { 0, -13, 85, 7, 69, -13, 0 },
+			 { 0, -13, 69, 7, 85, -13, 0 },
+			 { 0, -11, 52, 7, 98, -11, 0 },
+			 { 0, -8, 34, 7, 106, -4, 0 },
+			 { 0, -5, 19, 7, 111, 5, -2 } } },
+	.ver_phase_arr = {
+		.even = { { -3, 12, 110, 7, 12, -3, 0 },
+			 { -1, 0, 110, 7, 26, -7, 0 },
+			 { 0, -8, 103, 7, 43, -10, 0 },
+			 { 0, -12, 92, 7, 61, -13, 0 },
+			 { 0, -14, 78, 7, 78, -14, 0 },
+			 { 0, -13, 61, 7, 92, -12, 0 },
+			 { 0, -10, 43, 7, 103, -8, 0 },
+			 { 0, -7, 26, 7, 110, 0, -1 } },
+		.odd = { { -2, 5, 111, 7, 19, -5, 0 },
+			 { 0, -4, 106, 7, 34, -8, 0 },
+			 { 0, -11, 98, 7, 52, -11, 0 },
+			 { 0, -13, 85, 7, 69, -13, 0 },
+			 { 0, -13, 69, 7, 85, -13, 0 },
+			 { 0, -11, 52, 7, 98, -11, 0 },
+			 { 0, -8, 34, 7, 106, -4, 0 },
+			 { 0, -5, 19, 7, 111, 5, -2 } } },
+	.ptrn_arr = { { 0xffff } },
+	.sample_patrn_length = 18,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 5) = 0.864865 */
+	.hor_phase_arr = {
+		.even = { { -5, 14, 110, 7, 14, -5, 0 },
+			 { -1, 0, 106, 7, 32, -9, 0 },
+			 { 0, -9, 96, 7, 53, -12, 0 },
+			 { 0, -13, 81, 7, 73, -13, 0 },
+			 { 0, -13, 61, 7, 91, -11, 0 },
+			 { 0, -10, 40, 7, 103, -4, -1 },
+			 { 0, -6, 21, 7, 108, 8, -3 },
+			 { -3, 5, 108, 7, 25, -7, 0 },
+			 { 0, -6, 101, 7, 44, -11, 0 },
+			 { 0, -12, 88, 7, 65, -13, 0 },
+			 { 0, -13, 69, 7, 85, -13, 0 },
+			 { 0, -11, 49, 7, 98, -8, 0 },
+			 { 0, -8, 28, 7, 108, 2, -2 },
+			 { -4, 11, 108, 7, 18, -5, 0 },
+			 { -1, -2, 104, 7, 36, -9, 0 },
+			 { 0, -10, 93, 7, 57, -12, 0 },
+			 { 0, -13, 77, 7, 77, -13, 0 },
+			 { 0, -12, 57, 7, 93, -10, 0 },
+			 { 0, -9, 36, 7, 104, -2, -1 },
+			 { 0, -5, 18, 7, 108, 11, -4 },
+			 { -2, 2, 108, 7, 28, -8, 0 },
+			 { 0, -8, 98, 7, 49, -11, 0 },
+			 { 0, -13, 85, 7, 69, -13, 0 },
+			 { 0, -13, 65, 7, 88, -12, 0 },
+			 { 0, -11, 44, 7, 101, -6, 0 },
+			 { 0, -7, 25, 7, 108, 5, -3 },
+			 { -3, 8, 108, 7, 21, -6, 0 },
+			 { -1, -4, 103, 7, 40, -10, 0 },
+			 { 0, -11, 91, 7, 61, -13, 0 },
+			 { 0, -13, 73, 7, 81, -13, 0 },
+			 { 0, -12, 53, 7, 96, -9, 0 },
+			 { 0, -9, 32, 7, 106, 0, -1 } },
+		.odd = { { -3, 7, 108, 7, 23, -7, 0 },
+			 { 0, -5, 101, 7, 42, -10, 0 },
+			 { 0, -12, 90, 7, 63, -13, 0 },
+			 { 0, -13, 71, 7, 83, -13, 0 },
+			 { 0, -12, 51, 7, 97, -8, 0 },
+			 { 0, -8, 30, 7, 107, 1, -2 },
+			 { -4, 13, 108, 7, 16, -5, 0 },
+			 { -1, -1, 105, 7, 34, -9, 0 },
+			 { 0, -10, 95, 7, 55, -12, 0 },
+			 { 0, -13, 79, 7, 75, -13, 0 },
+			 { 0, -13, 59, 7, 93, -11, 0 },
+			 { 0, -10, 38, 7, 104, -3, -1 },
+			 { 0, -6, 19, 7, 110, 9, -4 },
+			 { -2, 4, 106, 7, 27, -7, 0 },
+			 { 0, -7, 99, 7, 47, -11, 0 },
+			 { 0, -12, 86, 7, 67, -13, 0 },
+			 { 0, -13, 67, 7, 86, -12, 0 },
+			 { 0, -11, 47, 7, 99, -7, 0 },
+			 { 0, -7, 27, 7, 106, 4, -2 },
+			 { -4, 9, 110, 7, 19, -6, 0 },
+			 { -1, -3, 104, 7, 38, -10, 0 },
+			 { 0, -11, 93, 7, 59, -13, 0 },
+			 { 0, -13, 75, 7, 79, -13, 0 },
+			 { 0, -12, 55, 7, 95, -10, 0 },
+			 { 0, -9, 34, 7, 105, -1, -1 },
+			 { 0, -5, 16, 7, 108, 13, -4 },
+			 { -2, 1, 107, 7, 30, -8, 0 },
+			 { 0, -8, 97, 7, 51, -12, 0 },
+			 { 0, -13, 83, 7, 71, -13, 0 },
+			 { 0, -13, 63, 7, 90, -12, 0 },
+			 { 0, -10, 42, 7, 101, -5, 0 },
+			 { 0, -7, 23, 7, 108, 7, -3 } } },
+	.ver_phase_arr = {
+		.even = { { -5, 14, 110, 7, 14, -5, 0 },
+			 { -1, 0, 106, 7, 32, -9, 0 },
+			 { 0, -9, 96, 7, 53, -12, 0 },
+			 { 0, -13, 81, 7, 73, -13, 0 },
+			 { 0, -13, 61, 7, 91, -11, 0 },
+			 { 0, -10, 40, 7, 103, -4, -1 },
+			 { 0, -6, 21, 7, 108, 8, -3 },
+			 { -3, 5, 108, 7, 25, -7, 0 },
+			 { 0, -6, 101, 7, 44, -11, 0 },
+			 { 0, -12, 88, 7, 65, -13, 0 },
+			 { 0, -13, 69, 7, 85, -13, 0 },
+			 { 0, -11, 49, 7, 98, -8, 0 },
+			 { 0, -8, 28, 7, 108, 2, -2 },
+			 { -4, 11, 108, 7, 18, -5, 0 },
+			 { -1, -2, 104, 7, 36, -9, 0 },
+			 { 0, -10, 93, 7, 57, -12, 0 },
+			 { 0, -13, 77, 7, 77, -13, 0 },
+			 { 0, -12, 57, 7, 93, -10, 0 },
+			 { 0, -9, 36, 7, 104, -2, -1 },
+			 { 0, -5, 18, 7, 108, 11, -4 },
+			 { -2, 2, 108, 7, 28, -8, 0 },
+			 { 0, -8, 98, 7, 49, -11, 0 },
+			 { 0, -13, 85, 7, 69, -13, 0 },
+			 { 0, -13, 65, 7, 88, -12, 0 },
+			 { 0, -11, 44, 7, 101, -6, 0 },
+			 { 0, -7, 25, 7, 108, 5, -3 },
+			 { -3, 8, 108, 7, 21, -6, 0 },
+			 { -1, -4, 103, 7, 40, -10, 0 },
+			 { 0, -11, 91, 7, 61, -13, 0 },
+			 { 0, -13, 73, 7, 81, -13, 0 },
+			 { 0, -12, 53, 7, 96, -9, 0 },
+			 { 0, -9, 32, 7, 106, 0, -1 } },
+		.odd = { { -3, 7, 108, 7, 23, -7, 0 },
+			 { 0, -5, 101, 7, 42, -10, 0 },
+			 { 0, -12, 90, 7, 63, -13, 0 },
+			 { 0, -13, 71, 7, 83, -13, 0 },
+			 { 0, -12, 51, 7, 97, -8, 0 },
+			 { 0, -8, 30, 7, 107, 1, -2 },
+			 { -4, 13, 108, 7, 16, -5, 0 },
+			 { -1, -1, 105, 7, 34, -9, 0 },
+			 { 0, -10, 95, 7, 55, -12, 0 },
+			 { 0, -13, 79, 7, 75, -13, 0 },
+			 { 0, -13, 59, 7, 93, -11, 0 },
+			 { 0, -10, 38, 7, 104, -3, -1 },
+			 { 0, -6, 19, 7, 110, 9, -4 },
+			 { -2, 4, 106, 7, 27, -7, 0 },
+			 { 0, -7, 99, 7, 47, -11, 0 },
+			 { 0, -12, 86, 7, 67, -13, 0 },
+			 { 0, -13, 67, 7, 86, -12, 0 },
+			 { 0, -11, 47, 7, 99, -7, 0 },
+			 { 0, -7, 27, 7, 106, 4, -2 },
+			 { -4, 9, 110, 7, 19, -6, 0 },
+			 { -1, -3, 104, 7, 38, -10, 0 },
+			 { 0, -11, 93, 7, 59, -13, 0 },
+			 { 0, -13, 75, 7, 79, -13, 0 },
+			 { 0, -12, 55, 7, 95, -10, 0 },
+			 { 0, -9, 34, 7, 105, -1, -1 },
+			 { 0, -5, 16, 7, 108, 13, -4 },
+			 { -2, 1, 107, 7, 30, -8, 0 },
+			 { 0, -8, 97, 7, 51, -12, 0 },
+			 { 0, -13, 83, 7, 71, -13, 0 },
+			 { 0, -13, 63, 7, 90, -12, 0 },
+			 { 0, -10, 42, 7, 101, -5, 0 },
+			 { 0, -7, 23, 7, 108, 7, -3 } } },
+	.ptrn_arr = { { 0xcfff9fff, 0xf3ffe7ff, 0xff } },
+	.sample_patrn_length = 74,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 6) = 0.842105 */
+	.hor_phase_arr = {
+		.even = { { -6, 17, 106, 7, 17, -6, 0 },
+			 { -2, 0, 102, 7, 38, -10, 0 },
+			 { 0, -10, 89, 7, 62, -13, 0 },
+			 { 0, -13, 69, 7, 83, -11, 0 },
+			 { 0, -11, 46, 7, 98, -4, -1 },
+			 { 0, -7, 23, 7, 106, 10, -4 },
+			 { -3, 5, 104, 7, 31, -9, 0 },
+			 { 0, -7, 93, 7, 54, -12, 0 },
+			 { 0, -12, 76, 7, 76, -12, 0 },
+			 { 0, -12, 54, 7, 93, -7, 0 },
+			 { 0, -9, 31, 7, 104, 5, -3 },
+			 { -4, 10, 106, 7, 23, -7, 0 },
+			 { -1, -4, 98, 7, 46, -11, 0 },
+			 { 0, -11, 83, 7, 69, -13, 0 },
+			 { 0, -13, 62, 7, 89, -10, 0 },
+			 { 0, -10, 38, 7, 102, 0, -2 } },
+		.odd = { { -4, 8, 105, 7, 27, -8, 0 },
+			 { 0, -6, 96, 7, 50, -12, 0 },
+			 { 0, -12, 80, 7, 73, -13, 0 },
+			 { 0, -13, 58, 7, 92, -9, 0 },
+			 { 0, -9, 34, 7, 103, 2, -2 },
+			 { -5, 13, 107, 7, 20, -7, 0 },
+			 { -1, -2, 100, 7, 42, -11, 0 },
+			 { 0, -11, 87, 7, 65, -13, 0 },
+			 { 0, -13, 65, 7, 87, -11, 0 },
+			 { 0, -11, 42, 7, 100, -2, -1 },
+			 { 0, -7, 20, 7, 107, 13, -5 },
+			 { -2, 2, 103, 7, 34, -9, 0 },
+			 { 0, -9, 92, 7, 58, -13, 0 },
+			 { 0, -13, 73, 7, 80, -12, 0 },
+			 { 0, -12, 50, 7, 96, -6, 0 },
+			 { 0, -8, 27, 7, 105, 8, -4 } } },
+	.ver_phase_arr = {
+		.even = { { -6, 17, 106, 7, 17, -6, 0 },
+			 { -2, 0, 102, 7, 38, -10, 0 },
+			 { 0, -10, 89, 7, 62, -13, 0 },
+			 { 0, -13, 69, 7, 83, -11, 0 },
+			 { 0, -11, 46, 7, 98, -4, -1 },
+			 { 0, -7, 23, 7, 106, 10, -4 },
+			 { -3, 5, 104, 7, 31, -9, 0 },
+			 { 0, -7, 93, 7, 54, -12, 0 },
+			 { 0, -12, 76, 7, 76, -12, 0 },
+			 { 0, -12, 54, 7, 93, -7, 0 },
+			 { 0, -9, 31, 7, 104, 5, -3 },
+			 { -4, 10, 106, 7, 23, -7, 0 },
+			 { -1, -4, 98, 7, 46, -11, 0 },
+			 { 0, -11, 83, 7, 69, -13, 0 },
+			 { 0, -13, 62, 7, 89, -10, 0 },
+			 { 0, -10, 38, 7, 102, 0, -2 } },
+		.odd = { { -4, 8, 105, 7, 27, -8, 0 },
+			 { 0, -6, 96, 7, 50, -12, 0 },
+			 { 0, -12, 80, 7, 73, -13, 0 },
+			 { 0, -13, 58, 7, 92, -9, 0 },
+			 { 0, -9, 34, 7, 103, 2, -2 },
+			 { -5, 13, 107, 7, 20, -7, 0 },
+			 { -1, -2, 100, 7, 42, -11, 0 },
+			 { 0, -11, 87, 7, 65, -13, 0 },
+			 { 0, -13, 65, 7, 87, -11, 0 },
+			 { 0, -11, 42, 7, 100, -2, -1 },
+			 { 0, -7, 20, 7, 107, 13, -5 },
+			 { -2, 2, 103, 7, 34, -9, 0 },
+			 { 0, -9, 92, 7, 58, -13, 0 },
+			 { 0, -13, 73, 7, 80, -12, 0 },
+			 { 0, -12, 50, 7, 96, -6, 0 },
+			 { 0, -8, 27, 7, 105, 8, -4 } } },
+	.ptrn_arr = { { 0xfcffe7ff, 0xf } },
+	.sample_patrn_length = 38,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 7) = 0.820513 */
+	.hor_phase_arr = {
+		.even = { { -7, 19, 104, 7, 19, -7, 0 },
+			 { -2, 0, 98, 7, 43, -11, 0 },
+			 { 0, -10, 81, 7, 69, -12, 0 },
+			 { 0, -12, 58, 7, 89, -7, 0 },
+			 { 0, -10, 32, 7, 103, 7, -4 },
+			 { -5, 10, 103, 7, 29, -9, 0 },
+			 { -1, -6, 93, 7, 54, -12, 0 },
+			 { 0, -12, 72, 7, 79, -11, 0 },
+			 { 0, -12, 47, 7, 97, -2, -2 },
+			 { 0, -8, 22, 7, 104, 16, -6 },
+			 { -3, 2, 100, 7, 40, -11, 0 },
+			 { 0, -9, 84, 7, 65, -12, 0 },
+			 { 0, -13, 62, 7, 87, -8, 0 },
+			 { 0, -10, 36, 7, 100, 5, -3 },
+			 { -5, 13, 103, 7, 25, -8, 0 },
+			 { -1, -4, 94, 7, 51, -12, 0 },
+			 { 0, -12, 76, 7, 76, -12, 0 },
+			 { 0, -12, 51, 7, 94, -4, -1 },
+			 { 0, -8, 25, 7, 103, 13, -5 },
+			 { -3, 5, 100, 7, 36, -10, 0 },
+			 { 0, -8, 87, 7, 62, -13, 0 },
+			 { 0, -12, 65, 7, 84, -9, 0 },
+			 { 0, -11, 40, 7, 100, 2, -3 },
+			 { -6, 16, 104, 7, 22, -8, 0 },
+			 { -2, -2, 97, 7, 47, -12, 0 },
+			 { 0, -11, 79, 7, 72, -12, 0 },
+			 { 0, -12, 54, 7, 93, -6, -1 },
+			 { 0, -9, 29, 7, 103, 10, -5 },
+			 { -4, 7, 103, 7, 32, -10, 0 },
+			 { 0, -7, 89, 7, 58, -12, 0 },
+			 { 0, -12, 69, 7, 81, -10, 0 },
+			 { 0, -11, 43, 7, 98, 0, -2 } },
+		.odd = { { -4, 9, 101, 7, 31, -9, 0 },
+			 { -1, -6, 91, 7, 56, -12, 0 },
+			 { 0, -12, 71, 7, 80, -11, 0 },
+			 { 0, -11, 45, 7, 97, -1, -2 },
+			 { 0, -7, 20, 7, 105, 17, -7 },
+			 { -3, 1, 100, 7, 41, -11, 0 },
+			 { 0, -10, 83, 7, 67, -12, 0 },
+			 { 0, -13, 60, 7, 89, -8, 0 },
+			 { 0, -10, 34, 7, 102, 6, -4 },
+			 { -5, 11, 104, 7, 27, -9, 0 },
+			 { -1, -5, 94, 7, 52, -12, 0 },
+			 { 0, -12, 74, 7, 77, -11, 0 },
+			 { 0, -12, 49, 7, 95, -3, -1 },
+			 { 0, -8, 24, 7, 104, 14, -6 },
+			 { -3, 3, 100, 7, 38, -10, 0 },
+			 { 0, -9, 87, 7, 63, -13, 0 },
+			 { 0, -13, 63, 7, 87, -9, 0 },
+			 { 0, -10, 38, 7, 100, 3, -3 },
+			 { -6, 14, 104, 7, 24, -8, 0 },
+			 { -1, -3, 95, 7, 49, -12, 0 },
+			 { 0, -11, 77, 7, 74, -12, 0 },
+			 { 0, -12, 52, 7, 94, -5, -1 },
+			 { 0, -9, 27, 7, 104, 11, -5 },
+			 { -4, 6, 102, 7, 34, -10, 0 },
+			 { 0, -8, 89, 7, 60, -13, 0 },
+			 { 0, -12, 67, 7, 83, -10, 0 },
+			 { 0, -11, 41, 7, 100, 1, -3 },
+			 { -7, 17, 105, 7, 20, -7, 0 },
+			 { -2, -1, 97, 7, 45, -11, 0 },
+			 { 0, -11, 80, 7, 71, -12, 0 },
+			 { 0, -12, 56, 7, 91, -6, -1 },
+			 { 0, -9, 31, 7, 101, 9, -4 } } },
+	.ver_phase_arr = {
+		.even = { { -7, 19, 104, 7, 19, -7, 0 },
+			 { -2, 0, 98, 7, 43, -11, 0 },
+			 { 0, -10, 81, 7, 69, -12, 0 },
+			 { 0, -12, 58, 7, 89, -7, 0 },
+			 { 0, -10, 32, 7, 103, 7, -4 },
+			 { -5, 10, 103, 7, 29, -9, 0 },
+			 { -1, -6, 93, 7, 54, -12, 0 },
+			 { 0, -12, 72, 7, 79, -11, 0 },
+			 { 0, -12, 47, 7, 97, -2, -2 },
+			 { 0, -8, 22, 7, 104, 16, -6 },
+			 { -3, 2, 100, 7, 40, -11, 0 },
+			 { 0, -9, 84, 7, 65, -12, 0 },
+			 { 0, -13, 62, 7, 87, -8, 0 },
+			 { 0, -10, 36, 7, 100, 5, -3 },
+			 { -5, 13, 103, 7, 25, -8, 0 },
+			 { -1, -4, 94, 7, 51, -12, 0 },
+			 { 0, -12, 76, 7, 76, -12, 0 },
+			 { 0, -12, 51, 7, 94, -4, -1 },
+			 { 0, -8, 25, 7, 103, 13, -5 },
+			 { -3, 5, 100, 7, 36, -10, 0 },
+			 { 0, -8, 87, 7, 62, -13, 0 },
+			 { 0, -12, 65, 7, 84, -9, 0 },
+			 { 0, -11, 40, 7, 100, 2, -3 },
+			 { -6, 16, 104, 7, 22, -8, 0 },
+			 { -2, -2, 97, 7, 47, -12, 0 },
+			 { 0, -11, 79, 7, 72, -12, 0 },
+			 { 0, -12, 54, 7, 93, -6, -1 },
+			 { 0, -9, 29, 7, 103, 10, -5 },
+			 { -4, 7, 103, 7, 32, -10, 0 },
+			 { 0, -7, 89, 7, 58, -12, 0 },
+			 { 0, -12, 69, 7, 81, -10, 0 },
+			 { 0, -11, 43, 7, 98, 0, -2 } },
+		.odd = { { -4, 9, 101, 7, 31, -9, 0 },
+			 { -1, -6, 91, 7, 56, -12, 0 },
+			 { 0, -12, 71, 7, 80, -11, 0 },
+			 { 0, -11, 45, 7, 97, -1, -2 },
+			 { 0, -7, 20, 7, 105, 17, -7 },
+			 { -3, 1, 100, 7, 41, -11, 0 },
+			 { 0, -10, 83, 7, 67, -12, 0 },
+			 { 0, -13, 60, 7, 89, -8, 0 },
+			 { 0, -10, 34, 7, 102, 6, -4 },
+			 { -5, 11, 104, 7, 27, -9, 0 },
+			 { -1, -5, 94, 7, 52, -12, 0 },
+			 { 0, -12, 74, 7, 77, -11, 0 },
+			 { 0, -12, 49, 7, 95, -3, -1 },
+			 { 0, -8, 24, 7, 104, 14, -6 },
+			 { -3, 3, 100, 7, 38, -10, 0 },
+			 { 0, -9, 87, 7, 63, -13, 0 },
+			 { 0, -13, 63, 7, 87, -9, 0 },
+			 { 0, -10, 38, 7, 100, 3, -3 },
+			 { -6, 14, 104, 7, 24, -8, 0 },
+			 { -1, -3, 95, 7, 49, -12, 0 },
+			 { 0, -11, 77, 7, 74, -12, 0 },
+			 { 0, -12, 52, 7, 94, -5, -1 },
+			 { 0, -9, 27, 7, 104, 11, -5 },
+			 { -4, 6, 102, 7, 34, -10, 0 },
+			 { 0, -8, 89, 7, 60, -13, 0 },
+			 { 0, -12, 67, 7, 83, -10, 0 },
+			 { 0, -11, 41, 7, 100, 1, -3 },
+			 { -7, 17, 105, 7, 20, -7, 0 },
+			 { -2, -1, 97, 7, 45, -11, 0 },
+			 { 0, -11, 80, 7, 71, -12, 0 },
+			 { 0, -12, 56, 7, 91, -6, -1 },
+			 { 0, -9, 31, 7, 101, 9, -4 } } },
+	.ptrn_arr = { { 0xff9ff3ff, 0xff3fe7fc, 0xff9 } },
+	.sample_patrn_length = 78,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 8) = 0.8 */
+	.hor_phase_arr = {
+		.even = { { -8, 21, 102, 7, 21, -8, 0 },
+			 { -3, 0, 95, 7, 48, -12, 0 },
+			 { 0, -11, 75, 7, 75, -11, 0 },
+			 { 0, -12, 48, 7, 95, 0, -3 } },
+		.odd = { { -5, 9, 100, 7, 34, -10, 0 },
+			 { -1, -7, 86, 7, 62, -12, 0 },
+			 { 0, -12, 62, 7, 86, -7, -1 },
+			 { 0, -10, 34, 7, 100, 9, -5 } } },
+	.ver_phase_arr = {
+		.even = { { -8, 21, 102, 7, 21, -8, 0 },
+			 { -3, 0, 95, 7, 48, -12, 0 },
+			 { 0, -11, 75, 7, 75, -11, 0 },
+			 { 0, -12, 48, 7, 95, 0, -3 } },
+		.odd = { { -5, 9, 100, 7, 34, -10, 0 },
+			 { -1, -7, 86, 7, 62, -12, 0 },
+			 { 0, -12, 62, 7, 86, -7, -1 },
+			 { 0, -10, 34, 7, 100, 9, -5 } } },
+	.ptrn_arr = { { 0xff } },
+	.sample_patrn_length = 10,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 9) = 0.780488 */
+	.hor_phase_arr = {
+		.even = { { -9, 23, 100, 7, 23, -9, 0 },
+			 { -3, 0, 91, 7, 52, -12, 0 },
+			 { 0, -11, 68, 7, 80, -8, -1 },
+			 { 0, -11, 39, 7, 96, 9, -5 },
+			 { -6, 12, 98, 7, 35, -11, 0 },
+			 { -1, -6, 81, 7, 65, -11, 0 },
+			 { 0, -12, 55, 7, 89, -2, -2 },
+			 { 0, -9, 26, 7, 99, 20, -8 },
+			 { -4, 2, 93, 7, 49, -12, 0 },
+			 { 0, -10, 71, 7, 76, -9, 0 },
+			 { 0, -11, 42, 7, 95, 7, -5 },
+			 { -7, 14, 99, 7, 32, -10, 0 },
+			 { -1, -5, 84, 7, 62, -12, 0 },
+			 { 0, -12, 59, 7, 87, -4, -2 },
+			 { 0, -10, 29, 7, 99, 17, -7 },
+			 { -4, 4, 95, 7, 45, -12, 0 },
+			 { 0, -9, 72, 7, 74, -9, 0 },
+			 { 0, -12, 45, 7, 95, 4, -4 },
+			 { -7, 17, 99, 7, 29, -10, 0 },
+			 { -2, -4, 87, 7, 59, -12, 0 },
+			 { 0, -12, 62, 7, 84, -5, -1 },
+			 { 0, -10, 32, 7, 99, 14, -7 },
+			 { -5, 7, 95, 7, 42, -11, 0 },
+			 { 0, -9, 76, 7, 71, -10, 0 },
+			 { 0, -12, 49, 7, 93, 2, -4 },
+			 { -8, 20, 99, 7, 26, -9, 0 },
+			 { -2, -2, 89, 7, 55, -12, 0 },
+			 { 0, -11, 65, 7, 81, -6, -1 },
+			 { 0, -11, 35, 7, 98, 12, -6 },
+			 { -5, 9, 96, 7, 39, -11, 0 },
+			 { -1, -8, 80, 7, 68, -11, 0 },
+			 { 0, -12, 52, 7, 91, 0, -3 } },
+		.odd = { { -6, 10, 98, 7, 37, -11, 0 },
+			 { -1, -7, 81, 7, 66, -11, 0 },
+			 { 0, -12, 54, 7, 90, -1, -3 },
+			 { 0, -9, 24, 7, 100, 21, -8 },
+			 { -3, 1, 92, 7, 50, -12, 0 },
+			 { 0, -10, 69, 7, 78, -8, -1 },
+			 { 0, -11, 40, 7, 96, 8, -5 },
+			 { -6, 13, 97, 7, 34, -10, 0 },
+			 { -1, -6, 83, 7, 63, -11, 0 },
+			 { 0, -12, 57, 7, 88, -3, -2 },
+			 { 0, -9, 27, 7, 100, 18, -8 },
+			 { -4, 3, 94, 7, 47, -12, 0 },
+			 { 0, -10, 72, 7, 75, -9, 0 },
+			 { 0, -11, 44, 7, 95, 5, -5 },
+			 { -7, 16, 98, 7, 31, -10, 0 },
+			 { -2, -4, 86, 7, 60, -12, 0 },
+			 { 0, -12, 60, 7, 86, -4, -2 },
+			 { 0, -10, 31, 7, 98, 16, -7 },
+			 { -5, 5, 95, 7, 44, -11, 0 },
+			 { 0, -9, 75, 7, 72, -10, 0 },
+			 { 0, -12, 47, 7, 94, 3, -4 },
+			 { -8, 18, 100, 7, 27, -9, 0 },
+			 { -2, -3, 88, 7, 57, -12, 0 },
+			 { 0, -11, 63, 7, 83, -6, -1 },
+			 { 0, -10, 34, 7, 97, 13, -6 },
+			 { -5, 8, 96, 7, 40, -11, 0 },
+			 { -1, -8, 78, 7, 69, -10, 0 },
+			 { 0, -12, 50, 7, 92, 1, -3 },
+			 { -8, 21, 100, 7, 24, -9, 0 },
+			 { -3, -1, 90, 7, 54, -12, 0 },
+			 { 0, -11, 66, 7, 81, -7, -1 },
+			 { 0, -11, 37, 7, 98, 10, -6 } } },
+	.ver_phase_arr = {
+		.even = { { -9, 23, 100, 7, 23, -9, 0 },
+			 { -3, 0, 91, 7, 52, -12, 0 },
+			 { 0, -11, 68, 7, 80, -8, -1 },
+			 { 0, -11, 39, 7, 96, 9, -5 },
+			 { -6, 12, 98, 7, 35, -11, 0 },
+			 { -1, -6, 81, 7, 65, -11, 0 },
+			 { 0, -12, 55, 7, 89, -2, -2 },
+			 { 0, -9, 26, 7, 99, 20, -8 },
+			 { -4, 2, 93, 7, 49, -12, 0 },
+			 { 0, -10, 71, 7, 76, -9, 0 },
+			 { 0, -11, 42, 7, 95, 7, -5 },
+			 { -7, 14, 99, 7, 32, -10, 0 },
+			 { -1, -5, 84, 7, 62, -12, 0 },
+			 { 0, -12, 59, 7, 87, -4, -2 },
+			 { 0, -10, 29, 7, 99, 17, -7 },
+			 { -4, 4, 95, 7, 45, -12, 0 },
+			 { 0, -9, 72, 7, 74, -9, 0 },
+			 { 0, -12, 45, 7, 95, 4, -4 },
+			 { -7, 17, 99, 7, 29, -10, 0 },
+			 { -2, -4, 87, 7, 59, -12, 0 },
+			 { 0, -12, 62, 7, 84, -5, -1 },
+			 { 0, -10, 32, 7, 99, 14, -7 },
+			 { -5, 7, 95, 7, 42, -11, 0 },
+			 { 0, -9, 76, 7, 71, -10, 0 },
+			 { 0, -12, 49, 7, 93, 2, -4 },
+			 { -8, 20, 99, 7, 26, -9, 0 },
+			 { -2, -2, 89, 7, 55, -12, 0 },
+			 { 0, -11, 65, 7, 81, -6, -1 },
+			 { 0, -11, 35, 7, 98, 12, -6 },
+			 { -5, 9, 96, 7, 39, -11, 0 },
+			 { -1, -8, 80, 7, 68, -11, 0 },
+			 { 0, -12, 52, 7, 91, 0, -3 } },
+		.odd = { { -6, 10, 98, 7, 37, -11, 0 },
+			 { -1, -7, 81, 7, 66, -11, 0 },
+			 { 0, -12, 54, 7, 90, -1, -3 },
+			 { 0, -9, 24, 7, 100, 21, -8 },
+			 { -3, 1, 92, 7, 50, -12, 0 },
+			 { 0, -10, 69, 7, 78, -8, -1 },
+			 { 0, -11, 40, 7, 96, 8, -5 },
+			 { -6, 13, 97, 7, 34, -10, 0 },
+			 { -1, -6, 83, 7, 63, -11, 0 },
+			 { 0, -12, 57, 7, 88, -3, -2 },
+			 { 0, -9, 27, 7, 100, 18, -8 },
+			 { -4, 3, 94, 7, 47, -12, 0 },
+			 { 0, -10, 72, 7, 75, -9, 0 },
+			 { 0, -11, 44, 7, 95, 5, -5 },
+			 { -7, 16, 98, 7, 31, -10, 0 },
+			 { -2, -4, 86, 7, 60, -12, 0 },
+			 { 0, -12, 60, 7, 86, -4, -2 },
+			 { 0, -10, 31, 7, 98, 16, -7 },
+			 { -5, 5, 95, 7, 44, -11, 0 },
+			 { 0, -9, 75, 7, 72, -10, 0 },
+			 { 0, -12, 47, 7, 94, 3, -4 },
+			 { -8, 18, 100, 7, 27, -9, 0 },
+			 { -2, -3, 88, 7, 57, -12, 0 },
+			 { 0, -11, 63, 7, 83, -6, -1 },
+			 { 0, -10, 34, 7, 97, 13, -6 },
+			 { -5, 8, 96, 7, 40, -11, 0 },
+			 { -1, -8, 78, 7, 69, -10, 0 },
+			 { 0, -12, 50, 7, 92, 1, -3 },
+			 { -8, 21, 100, 7, 24, -9, 0 },
+			 { -3, -1, 90, 7, 54, -12, 0 },
+			 { 0, -11, 66, 7, 81, -7, -1 },
+			 { 0, -11, 37, 7, 98, 10, -6 } } },
+	.ptrn_arr = { { 0xf3f9fcff, 0x3f9fcfe7, 0xfe7f } },
+	.sample_patrn_length = 82,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 10) = 0.761905 */
+	.hor_phase_arr = {
+		.even = { { -9, 25, 96, 7, 25, -9, 0 },
+			 { -3, 0, 86, 7, 56, -11, 0 },
+			 { 0, -11, 62, 7, 82, -3, -2 },
+			 { 0, -10, 31, 7, 96, 19, -8 },
+			 { -5, 4, 92, 7, 49, -12, 0 },
+			 { 0, -10, 67, 7, 78, -6, -1 },
+			 { 0, -11, 37, 7, 95, 14, -7 },
+			 { -6, 9, 93, 7, 43, -11, 0 },
+			 { -1, -8, 73, 7, 73, -8, -1 },
+			 { 0, -11, 43, 7, 93, 9, -6 },
+			 { -7, 14, 95, 7, 37, -11, 0 },
+			 { -1, -6, 78, 7, 67, -10, 0 },
+			 { 0, -12, 49, 7, 92, 4, -5 },
+			 { -8, 19, 96, 7, 31, -10, 0 },
+			 { -2, -3, 82, 7, 62, -11, 0 },
+			 { 0, -11, 56, 7, 86, 0, -3 } },
+		.odd = { { -6, 11, 94, 7, 40, -11, 0 },
+			 { -1, -7, 75, 7, 70, -9, 0 },
+			 { 0, -12, 46, 7, 93, 6, -5 },
+			 { -8, 16, 97, 7, 34, -11, 0 },
+			 { -2, -5, 81, 7, 64, -10, 0 },
+			 { 0, -12, 53, 7, 89, 2, -4 },
+			 { -9, 22, 97, 7, 28, -10, 0 },
+			 { -3, -2, 85, 7, 59, -11, 0 },
+			 { 0, -11, 59, 7, 85, -2, -3 },
+			 { 0, -10, 28, 7, 97, 22, -9 },
+			 { -4, 2, 89, 7, 53, -12, 0 },
+			 { 0, -10, 64, 7, 81, -5, -2 },
+			 { 0, -11, 34, 7, 97, 16, -8 },
+			 { -5, 6, 93, 7, 46, -12, 0 },
+			 { 0, -9, 70, 7, 75, -7, -1 },
+			 { 0, -11, 40, 7, 94, 11, -6 } } },
+	.ver_phase_arr = {
+		.even = { { -9, 25, 96, 7, 25, -9, 0 },
+			 { -3, 0, 86, 7, 56, -11, 0 },
+			 { 0, -11, 62, 7, 82, -3, -2 },
+			 { 0, -10, 31, 7, 96, 19, -8 },
+			 { -5, 4, 92, 7, 49, -12, 0 },
+			 { 0, -10, 67, 7, 78, -6, -1 },
+			 { 0, -11, 37, 7, 95, 14, -7 },
+			 { -6, 9, 93, 7, 43, -11, 0 },
+			 { -1, -8, 73, 7, 73, -8, -1 },
+			 { 0, -11, 43, 7, 93, 9, -6 },
+			 { -7, 14, 95, 7, 37, -11, 0 },
+			 { -1, -6, 78, 7, 67, -10, 0 },
+			 { 0, -12, 49, 7, 92, 4, -5 },
+			 { -8, 19, 96, 7, 31, -10, 0 },
+			 { -2, -3, 82, 7, 62, -11, 0 },
+			 { 0, -11, 56, 7, 86, 0, -3 } },
+		.odd = { { -6, 11, 94, 7, 40, -11, 0 },
+			 { -1, -7, 75, 7, 70, -9, 0 },
+			 { 0, -12, 46, 7, 93, 6, -5 },
+			 { -8, 16, 97, 7, 34, -11, 0 },
+			 { -2, -5, 81, 7, 64, -10, 0 },
+			 { 0, -12, 53, 7, 89, 2, -4 },
+			 { -9, 22, 97, 7, 28, -10, 0 },
+			 { -3, -2, 85, 7, 59, -11, 0 },
+			 { 0, -11, 59, 7, 85, -2, -3 },
+			 { 0, -10, 28, 7, 97, 22, -9 },
+			 { -4, 2, 89, 7, 53, -12, 0 },
+			 { 0, -10, 64, 7, 81, -5, -2 },
+			 { 0, -11, 34, 7, 97, 16, -8 },
+			 { -5, 6, 93, 7, 46, -12, 0 },
+			 { 0, -9, 70, 7, 75, -7, -1 },
+			 { 0, -11, 40, 7, 94, 11, -6 } } },
+	.ptrn_arr = { { 0xfcfe7e7f, 0xfc } },
+	.sample_patrn_length = 42,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 11) = 0.744186 */
+	.hor_phase_arr = {
+		.even = { { -10, 26, 96, 7, 26, -10, 0 },
+			 { -4, 0, 83, 7, 59, -10, 0 },
+			 { 0, -11, 56, 7, 85, 2, -4 },
+			 { -9, 23, 95, 7, 29, -10, 0 },
+			 { -3, -2, 82, 7, 61, -10, 0 },
+			 { 0, -11, 53, 7, 87, 4, -5 },
+			 { -9, 21, 94, 7, 32, -10, 0 },
+			 { -3, -3, 79, 7, 64, -9, 0 },
+			 { 0, -11, 50, 7, 88, 6, -5 },
+			 { -8, 18, 94, 7, 35, -11, 0 },
+			 { -2, -5, 78, 7, 67, -9, -1 },
+			 { 0, -11, 47, 7, 90, 8, -6 },
+			 { -8, 15, 94, 7, 38, -11, 0 },
+			 { -2, -6, 75, 7, 70, -8, -1 },
+			 { 0, -11, 44, 7, 92, 10, -7 },
+			 { -7, 13, 92, 7, 41, -11, 0 },
+			 { -1, -7, 72, 7, 72, -7, -1 },
+			 { 0, -11, 41, 7, 92, 13, -7 },
+			 { -7, 10, 92, 7, 44, -11, 0 },
+			 { -1, -8, 70, 7, 75, -6, -2 },
+			 { 0, -11, 38, 7, 94, 15, -8 },
+			 { -6, 8, 90, 7, 47, -11, 0 },
+			 { -1, -9, 67, 7, 78, -5, -2 },
+			 { 0, -11, 35, 7, 94, 18, -8 },
+			 { -5, 6, 88, 7, 50, -11, 0 },
+			 { 0, -9, 64, 7, 79, -3, -3 },
+			 { 0, -10, 32, 7, 94, 21, -9 },
+			 { -5, 4, 87, 7, 53, -11, 0 },
+			 { 0, -10, 61, 7, 82, -2, -3 },
+			 { 0, -10, 29, 7, 95, 23, -9 },
+			 { -4, 2, 85, 7, 56, -11, 0 },
+			 { 0, -10, 59, 7, 83, 0, -4 } },
+		.odd = { { -7, 12, 92, 7, 42, -11, 0 },
+			 { -1, -7, 71, 7, 72, -6, -1 },
+			 { 0, -11, 39, 7, 93, 14, -7 },
+			 { -6, 9, 91, 7, 45, -11, 0 },
+			 { -1, -8, 68, 7, 76, -5, -2 },
+			 { 0, -11, 36, 7, 94, 17, -8 },
+			 { -6, 7, 90, 7, 48, -11, 0 },
+			 { 0, -9, 66, 7, 77, -4, -2 },
+			 { 0, -11, 33, 7, 96, 19, -9 },
+			 { -5, 5, 88, 7, 51, -11, 0 },
+			 { 0, -10, 63, 7, 80, -2, -3 },
+			 { 0, -10, 31, 7, 94, 22, -9 },
+			 { -5, 3, 87, 7, 54, -11, 0 },
+			 { 0, -10, 60, 7, 82, -1, -3 },
+			 { 0, -10, 28, 7, 94, 25, -9 },
+			 { -4, 1, 85, 7, 57, -11, 0 },
+			 { 0, -11, 57, 7, 85, 1, -4 },
+			 { -9, 25, 94, 7, 28, -10, 0 },
+			 { -3, -1, 82, 7, 60, -10, 0 },
+			 { 0, -11, 54, 7, 87, 3, -5 },
+			 { -9, 22, 94, 7, 31, -10, 0 },
+			 { -3, -2, 80, 7, 63, -10, 0 },
+			 { 0, -11, 51, 7, 88, 5, -5 },
+			 { -9, 19, 96, 7, 33, -11, 0 },
+			 { -2, -4, 77, 7, 66, -9, 0 },
+			 { 0, -11, 48, 7, 90, 7, -6 },
+			 { -8, 17, 94, 7, 36, -11, 0 },
+			 { -2, -5, 76, 7, 68, -8, -1 },
+			 { 0, -11, 45, 7, 91, 9, -6 },
+			 { -7, 14, 93, 7, 39, -11, 0 },
+			 { -1, -6, 72, 7, 71, -7, -1 },
+			 { 0, -11, 42, 7, 92, 12, -7 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 26, 96, 7, 26, -10, 0 },
+			 { -4, 0, 83, 7, 59, -10, 0 },
+			 { 0, -11, 56, 7, 85, 2, -4 },
+			 { -9, 23, 95, 7, 29, -10, 0 },
+			 { -3, -2, 82, 7, 61, -10, 0 },
+			 { 0, -11, 53, 7, 87, 4, -5 },
+			 { -9, 21, 94, 7, 32, -10, 0 },
+			 { -3, -3, 79, 7, 64, -9, 0 },
+			 { 0, -11, 50, 7, 88, 6, -5 },
+			 { -8, 18, 94, 7, 35, -11, 0 },
+			 { -2, -5, 78, 7, 67, -9, -1 },
+			 { 0, -11, 47, 7, 90, 8, -6 },
+			 { -8, 15, 94, 7, 38, -11, 0 },
+			 { -2, -6, 75, 7, 70, -8, -1 },
+			 { 0, -11, 44, 7, 92, 10, -7 },
+			 { -7, 13, 92, 7, 41, -11, 0 },
+			 { -1, -7, 72, 7, 72, -7, -1 },
+			 { 0, -11, 41, 7, 92, 13, -7 },
+			 { -7, 10, 92, 7, 44, -11, 0 },
+			 { -1, -8, 70, 7, 75, -6, -2 },
+			 { 0, -11, 38, 7, 94, 15, -8 },
+			 { -6, 8, 90, 7, 47, -11, 0 },
+			 { -1, -9, 67, 7, 78, -5, -2 },
+			 { 0, -11, 35, 7, 94, 18, -8 },
+			 { -5, 6, 88, 7, 50, -11, 0 },
+			 { 0, -9, 64, 7, 79, -3, -3 },
+			 { 0, -10, 32, 7, 94, 21, -9 },
+			 { -5, 4, 87, 7, 53, -11, 0 },
+			 { 0, -10, 61, 7, 82, -2, -3 },
+			 { 0, -10, 29, 7, 95, 23, -9 },
+			 { -4, 2, 85, 7, 56, -11, 0 },
+			 { 0, -10, 59, 7, 83, 0, -4 } },
+		.odd = { { -7, 12, 92, 7, 42, -11, 0 },
+			 { -1, -7, 71, 7, 72, -6, -1 },
+			 { 0, -11, 39, 7, 93, 14, -7 },
+			 { -6, 9, 91, 7, 45, -11, 0 },
+			 { -1, -8, 68, 7, 76, -5, -2 },
+			 { 0, -11, 36, 7, 94, 17, -8 },
+			 { -6, 7, 90, 7, 48, -11, 0 },
+			 { 0, -9, 66, 7, 77, -4, -2 },
+			 { 0, -11, 33, 7, 96, 19, -9 },
+			 { -5, 5, 88, 7, 51, -11, 0 },
+			 { 0, -10, 63, 7, 80, -2, -3 },
+			 { 0, -10, 31, 7, 94, 22, -9 },
+			 { -5, 3, 87, 7, 54, -11, 0 },
+			 { 0, -10, 60, 7, 82, -1, -3 },
+			 { 0, -10, 28, 7, 94, 25, -9 },
+			 { -4, 1, 85, 7, 57, -11, 0 },
+			 { 0, -11, 57, 7, 85, 1, -4 },
+			 { -9, 25, 94, 7, 28, -10, 0 },
+			 { -3, -1, 82, 7, 60, -10, 0 },
+			 { 0, -11, 54, 7, 87, 3, -5 },
+			 { -9, 22, 94, 7, 31, -10, 0 },
+			 { -3, -2, 80, 7, 63, -10, 0 },
+			 { 0, -11, 51, 7, 88, 5, -5 },
+			 { -9, 19, 96, 7, 33, -11, 0 },
+			 { -2, -4, 77, 7, 66, -9, 0 },
+			 { 0, -11, 48, 7, 90, 7, -6 },
+			 { -8, 17, 94, 7, 36, -11, 0 },
+			 { -2, -5, 76, 7, 68, -8, -1 },
+			 { 0, -11, 45, 7, 91, 9, -6 },
+			 { -7, 14, 93, 7, 39, -11, 0 },
+			 { -1, -6, 72, 7, 71, -7, -1 },
+			 { 0, -11, 42, 7, 92, 12, -7 } } },
+	.ptrn_arr = { { 0x3f3f3f3f, 0x9f9f9f3f, 0xf9f9f } },
+	.sample_patrn_length = 86,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 12) = 0.727273 */
+	.hor_phase_arr = {
+		.even = { { -10, 28, 92, 7, 28, -10, 0 },
+			 { -4, 0, 81, 7, 61, -9, -1 },
+			 { 0, -11, 50, 7, 87, 8, -6 },
+			 { -8, 17, 91, 7, 39, -11, 0 },
+			 { -2, -6, 72, 7, 72, -6, -2 },
+			 { 0, -11, 39, 7, 91, 17, -8 },
+			 { -6, 8, 87, 7, 50, -11, 0 },
+			 { -1, -9, 61, 7, 81, 0, -4 } },
+		.odd = { { -7, 12, 89, 7, 45, -11, 0 },
+			 { -1, -8, 67, 7, 76, -3, -3 },
+			 { 0, -11, 33, 7, 93, 22, -9 },
+			 { -5, 4, 83, 7, 56, -10, 0 },
+			 { 0, -10, 56, 7, 83, 4, -5 },
+			 { -9, 22, 93, 7, 33, -11, 0 },
+			 { -3, -3, 76, 7, 67, -8, -1 },
+			 { 0, -11, 45, 7, 89, 12, -7 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 28, 92, 7, 28, -10, 0 },
+			 { -4, 0, 81, 7, 61, -9, -1 },
+			 { 0, -11, 50, 7, 87, 8, -6 },
+			 { -8, 17, 91, 7, 39, -11, 0 },
+			 { -2, -6, 72, 7, 72, -6, -2 },
+			 { 0, -11, 39, 7, 91, 17, -8 },
+			 { -6, 8, 87, 7, 50, -11, 0 },
+			 { -1, -9, 61, 7, 81, 0, -4 } },
+		.odd = { { -7, 12, 89, 7, 45, -11, 0 },
+			 { -1, -8, 67, 7, 76, -3, -3 },
+			 { 0, -11, 33, 7, 93, 22, -9 },
+			 { -5, 4, 83, 7, 56, -10, 0 },
+			 { 0, -10, 56, 7, 83, 4, -5 },
+			 { -9, 22, 93, 7, 33, -11, 0 },
+			 { -3, -3, 76, 7, 67, -8, -1 },
+			 { 0, -11, 45, 7, 89, 12, -7 } } },
+	.ptrn_arr = { { 0xf9f3f } },
+	.sample_patrn_length = 22,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 13) = 0.711111 */
+	.hor_phase_arr = {
+		.even = { { -10, 29, 90, 7, 29, -10, 0 },
+			 { -4, 0, 76, 7, 64, -7, -1 },
+			 { 0, -11, 45, 7, 88, 14, -8 },
+			 { -7, 12, 85, 7, 48, -10, 0 },
+			 { -1, -8, 61, 7, 79, 2, -5 },
+			 { -10, 26, 90, 7, 32, -10, 0 },
+			 { -4, -2, 76, 7, 66, -6, -2 },
+			 { 0, -11, 42, 7, 89, 16, -8 },
+			 { -7, 10, 84, 7, 51, -10, 0 },
+			 { -1, -9, 59, 7, 81, 3, -5 },
+			 { -10, 24, 91, 7, 34, -11, 0 },
+			 { -3, -3, 72, 7, 69, -5, -2 },
+			 { 0, -11, 40, 7, 89, 19, -9 },
+			 { -6, 7, 84, 7, 53, -10, 0 },
+			 { -1, -9, 56, 7, 83, 5, -6 },
+			 { -9, 21, 90, 7, 37, -11, 0 },
+			 { -3, -4, 71, 7, 71, -4, -3 },
+			 { 0, -11, 37, 7, 90, 21, -9 },
+			 { -6, 5, 83, 7, 56, -9, -1 },
+			 { 0, -10, 53, 7, 84, 7, -6 },
+			 { -9, 19, 89, 7, 40, -11, 0 },
+			 { -2, -5, 69, 7, 72, -3, -3 },
+			 { 0, -11, 34, 7, 91, 24, -10 },
+			 { -5, 3, 81, 7, 59, -9, -1 },
+			 { 0, -10, 51, 7, 84, 10, -7 },
+			 { -8, 16, 89, 7, 42, -11, 0 },
+			 { -2, -6, 66, 7, 76, -2, -4 },
+			 { 0, -10, 32, 7, 90, 26, -10 },
+			 { -5, 2, 79, 7, 61, -8, -1 },
+			 { 0, -10, 48, 7, 85, 12, -7 },
+			 { -8, 14, 88, 7, 45, -11, 0 },
+			 { -1, -7, 64, 7, 76, 0, -4 } },
+		.odd = { { -8, 13, 88, 7, 46, -11, 0 },
+			 { -1, -8, 63, 7, 78, 1, -5 },
+			 { -10, 28, 90, 7, 30, -10, 0 },
+			 { -4, -1, 77, 7, 65, -7, -2 },
+			 { 0, -11, 44, 7, 88, 15, -8 },
+			 { -7, 11, 85, 7, 49, -10, 0 },
+			 { -1, -8, 60, 7, 79, 3, -5 },
+			 { -10, 25, 91, 7, 33, -11, 0 },
+			 { -4, -2, 74, 7, 68, -6, -2 },
+			 { 0, -11, 41, 7, 89, 18, -9 },
+			 { -7, 8, 85, 7, 52, -10, 0 },
+			 { -1, -9, 57, 7, 83, 4, -6 },
+			 { -9, 22, 90, 7, 36, -11, 0 },
+			 { -3, -4, 73, 7, 70, -5, -3 },
+			 { 0, -11, 38, 7, 90, 20, -9 },
+			 { -6, 6, 83, 7, 55, -10, 0 },
+			 { 0, -10, 55, 7, 83, 6, -6 },
+			 { -9, 20, 90, 7, 38, -11, 0 },
+			 { -3, -5, 70, 7, 73, -4, -3 },
+			 { 0, -11, 36, 7, 90, 22, -9 },
+			 { -6, 4, 83, 7, 57, -9, -1 },
+			 { 0, -10, 52, 7, 85, 8, -7 },
+			 { -9, 18, 89, 7, 41, -11, 0 },
+			 { -2, -6, 68, 7, 74, -2, -4 },
+			 { 0, -11, 33, 7, 91, 25, -10 },
+			 { -5, 3, 79, 7, 60, -8, -1 },
+			 { 0, -10, 49, 7, 85, 11, -7 },
+			 { -8, 15, 88, 7, 44, -11, 0 },
+			 { -2, -7, 65, 7, 77, -1, -4 },
+			 { 0, -10, 30, 7, 90, 28, -10 },
+			 { -5, 1, 78, 7, 63, -8, -1 },
+			 { 0, -11, 46, 7, 88, 13, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 29, 90, 7, 29, -10, 0 },
+			 { -4, 0, 76, 7, 64, -7, -1 },
+			 { 0, -11, 45, 7, 88, 14, -8 },
+			 { -7, 12, 85, 7, 48, -10, 0 },
+			 { -1, -8, 61, 7, 79, 2, -5 },
+			 { -10, 26, 90, 7, 32, -10, 0 },
+			 { -4, -2, 76, 7, 66, -6, -2 },
+			 { 0, -11, 42, 7, 89, 16, -8 },
+			 { -7, 10, 84, 7, 51, -10, 0 },
+			 { -1, -9, 59, 7, 81, 3, -5 },
+			 { -10, 24, 91, 7, 34, -11, 0 },
+			 { -3, -3, 72, 7, 69, -5, -2 },
+			 { 0, -11, 40, 7, 89, 19, -9 },
+			 { -6, 7, 84, 7, 53, -10, 0 },
+			 { -1, -9, 56, 7, 83, 5, -6 },
+			 { -9, 21, 90, 7, 37, -11, 0 },
+			 { -3, -4, 71, 7, 71, -4, -3 },
+			 { 0, -11, 37, 7, 90, 21, -9 },
+			 { -6, 5, 83, 7, 56, -9, -1 },
+			 { 0, -10, 53, 7, 84, 7, -6 },
+			 { -9, 19, 89, 7, 40, -11, 0 },
+			 { -2, -5, 69, 7, 72, -3, -3 },
+			 { 0, -11, 34, 7, 91, 24, -10 },
+			 { -5, 3, 81, 7, 59, -9, -1 },
+			 { 0, -10, 51, 7, 84, 10, -7 },
+			 { -8, 16, 89, 7, 42, -11, 0 },
+			 { -2, -6, 66, 7, 76, -2, -4 },
+			 { 0, -10, 32, 7, 90, 26, -10 },
+			 { -5, 2, 79, 7, 61, -8, -1 },
+			 { 0, -10, 48, 7, 85, 12, -7 },
+			 { -8, 14, 88, 7, 45, -11, 0 },
+			 { -1, -7, 64, 7, 76, 0, -4 } },
+		.odd = { { -8, 13, 88, 7, 46, -11, 0 },
+			 { -1, -8, 63, 7, 78, 1, -5 },
+			 { -10, 28, 90, 7, 30, -10, 0 },
+			 { -4, -1, 77, 7, 65, -7, -2 },
+			 { 0, -11, 44, 7, 88, 15, -8 },
+			 { -7, 11, 85, 7, 49, -10, 0 },
+			 { -1, -8, 60, 7, 79, 3, -5 },
+			 { -10, 25, 91, 7, 33, -11, 0 },
+			 { -4, -2, 74, 7, 68, -6, -2 },
+			 { 0, -11, 41, 7, 89, 18, -9 },
+			 { -7, 8, 85, 7, 52, -10, 0 },
+			 { -1, -9, 57, 7, 83, 4, -6 },
+			 { -9, 22, 90, 7, 36, -11, 0 },
+			 { -3, -4, 73, 7, 70, -5, -3 },
+			 { 0, -11, 38, 7, 90, 20, -9 },
+			 { -6, 6, 83, 7, 55, -10, 0 },
+			 { 0, -10, 55, 7, 83, 6, -6 },
+			 { -9, 20, 90, 7, 38, -11, 0 },
+			 { -3, -5, 70, 7, 73, -4, -3 },
+			 { 0, -11, 36, 7, 90, 22, -9 },
+			 { -6, 4, 83, 7, 57, -9, -1 },
+			 { 0, -10, 52, 7, 85, 8, -7 },
+			 { -9, 18, 89, 7, 41, -11, 0 },
+			 { -2, -6, 68, 7, 74, -2, -4 },
+			 { 0, -11, 33, 7, 91, 25, -10 },
+			 { -5, 3, 79, 7, 60, -8, -1 },
+			 { 0, -10, 49, 7, 85, 11, -7 },
+			 { -8, 15, 88, 7, 44, -11, 0 },
+			 { -2, -7, 65, 7, 77, -1, -4 },
+			 { 0, -10, 30, 7, 90, 28, -10 },
+			 { -5, 1, 78, 7, 63, -8, -1 },
+			 { 0, -11, 46, 7, 88, 13, -8 } } },
+	.ptrn_arr = { { 0xf3e7cf9f, 0x9f3e7cf9, 0xf3e7cf } },
+	.sample_patrn_length = 90,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 14) = 0.695652 */
+	.hor_phase_arr = {
+		.even = { { -10, 30, 88, 7, 30, -10, 0 },
+			 { -5, 0, 75, 7, 66, -5, -3 },
+			 { 0, -10, 40, 7, 87, 20, -9 },
+			 { -7, 7, 81, 7, 56, -8, -1 },
+			 { 0, -9, 51, 7, 83, 11, -8 },
+			 { -8, 16, 84, 7, 46, -10, 0 },
+			 { -2, -7, 61, 7, 79, 3, -6 },
+			 { -10, 25, 88, 7, 35, -10, 0 },
+			 { -4, -3, 72, 7, 70, -3, -4 },
+			 { 0, -10, 35, 7, 88, 25, -10 },
+			 { -6, 3, 79, 7, 61, -7, -2 },
+			 { 0, -10, 46, 7, 84, 16, -8 },
+			 { -8, 11, 83, 7, 51, -9, 0 },
+			 { -1, -8, 56, 7, 81, 7, -7 },
+			 { -9, 20, 87, 7, 40, -10, 0 },
+			 { -3, -5, 66, 7, 75, 0, -5 } },
+		.odd = { { -8, 13, 85, 7, 48, -10, 0 },
+			 { -1, -8, 59, 7, 79, 5, -6 },
+			 { -10, 23, 87, 7, 38, -10, 0 },
+			 { -3, -4, 68, 7, 72, -1, -4 },
+			 { 0, -10, 33, 7, 87, 28, -10 },
+			 { -5, 2, 75, 7, 64, -6, -2 },
+			 { 0, -10, 43, 7, 86, 18, -9 },
+			 { -7, 9, 83, 7, 53, -9, -1 },
+			 { -1, -9, 53, 7, 83, 9, -7 },
+			 { -9, 18, 86, 7, 43, -10, 0 },
+			 { -2, -6, 64, 7, 75, 2, -5 },
+			 { -10, 28, 87, 7, 33, -10, 0 },
+			 { -4, -1, 72, 7, 68, -4, -3 },
+			 { 0, -10, 38, 7, 87, 23, -10 },
+			 { -6, 5, 79, 7, 59, -8, -1 },
+			 { 0, -10, 48, 7, 85, 13, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 30, 88, 7, 30, -10, 0 },
+			 { -5, 0, 75, 7, 66, -5, -3 },
+			 { 0, -10, 40, 7, 87, 20, -9 },
+			 { -7, 7, 81, 7, 56, -8, -1 },
+			 { 0, -9, 51, 7, 83, 11, -8 },
+			 { -8, 16, 84, 7, 46, -10, 0 },
+			 { -2, -7, 61, 7, 79, 3, -6 },
+			 { -10, 25, 88, 7, 35, -10, 0 },
+			 { -4, -3, 72, 7, 70, -3, -4 },
+			 { 0, -10, 35, 7, 88, 25, -10 },
+			 { -6, 3, 79, 7, 61, -7, -2 },
+			 { 0, -10, 46, 7, 84, 16, -8 },
+			 { -8, 11, 83, 7, 51, -9, 0 },
+			 { -1, -8, 56, 7, 81, 7, -7 },
+			 { -9, 20, 87, 7, 40, -10, 0 },
+			 { -3, -5, 66, 7, 75, 0, -5 } },
+		.odd = { { -8, 13, 85, 7, 48, -10, 0 },
+			 { -1, -8, 59, 7, 79, 5, -6 },
+			 { -10, 23, 87, 7, 38, -10, 0 },
+			 { -3, -4, 68, 7, 72, -1, -4 },
+			 { 0, -10, 33, 7, 87, 28, -10 },
+			 { -5, 2, 75, 7, 64, -6, -2 },
+			 { 0, -10, 43, 7, 86, 18, -9 },
+			 { -7, 9, 83, 7, 53, -9, -1 },
+			 { -1, -9, 53, 7, 83, 9, -7 },
+			 { -9, 18, 86, 7, 43, -10, 0 },
+			 { -2, -6, 64, 7, 75, 2, -5 },
+			 { -10, 28, 87, 7, 33, -10, 0 },
+			 { -4, -1, 72, 7, 68, -4, -3 },
+			 { 0, -10, 38, 7, 87, 23, -10 },
+			 { -6, 5, 79, 7, 59, -8, -1 },
+			 { 0, -10, 48, 7, 85, 13, -8 } } },
+	.ptrn_arr = { { 0x79f3cf9f, 0xf3e } },
+	.sample_patrn_length = 46,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 15) = 0.680851 */
+	.hor_phase_arr = {
+		.even = { { -10, 31, 86, 7, 31, -10, 0 },
+			 { -5, 0, 72, 7, 68, -3, -4 },
+			 { 0, -10, 36, 7, 86, 26, -10 },
+			 { -6, 3, 76, 7, 63, -5, -3 },
+			 { 0, -10, 41, 7, 85, 21, -9 },
+			 { -7, 7, 78, 7, 59, -7, -2 },
+			 { 0, -10, 46, 7, 84, 17, -9 },
+			 { -8, 11, 80, 7, 54, -8, -1 },
+			 { -1, -9, 51, 7, 82, 13, -8 },
+			 { -9, 15, 83, 7, 49, -9, -1 },
+			 { -2, -8, 56, 7, 80, 9, -7 },
+			 { -9, 19, 85, 7, 43, -10, 0 },
+			 { -3, -6, 61, 7, 77, 5, -6 },
+			 { -10, 24, 86, 7, 38, -10, 0 },
+			 { -3, -4, 66, 7, 72, 2, -5 },
+			 { -10, 29, 86, 7, 33, -10, 0 },
+			 { -4, -1, 68, 7, 70, -1, -4 },
+			 { 0, -10, 33, 7, 86, 29, -10 },
+			 { -5, 2, 72, 7, 66, -4, -3 },
+			 { 0, -10, 38, 7, 86, 24, -10 },
+			 { -6, 5, 77, 7, 61, -6, -3 },
+			 { 0, -10, 43, 7, 85, 19, -9 },
+			 { -7, 9, 80, 7, 56, -8, -2 },
+			 { -1, -9, 49, 7, 83, 15, -9 },
+			 { -8, 13, 82, 7, 51, -9, -1 },
+			 { -1, -8, 54, 7, 80, 11, -8 },
+			 { -9, 17, 84, 7, 46, -10, 0 },
+			 { -2, -7, 59, 7, 78, 7, -7 },
+			 { -9, 21, 85, 7, 41, -10, 0 },
+			 { -3, -5, 63, 7, 76, 3, -6 },
+			 { -10, 26, 86, 7, 36, -10, 0 },
+			 { -4, -3, 68, 7, 72, 0, -5 } },
+		.odd = { { -8, 14, 82, 7, 50, -9, -1 },
+			 { -1, -8, 55, 7, 79, 10, -7 },
+			 { -9, 18, 84, 7, 45, -10, 0 },
+			 { -2, -6, 60, 7, 77, 6, -7 },
+			 { -10, 23, 85, 7, 40, -10, 0 },
+			 { -3, -4, 64, 7, 75, 2, -6 },
+			 { -10, 27, 86, 7, 35, -10, 0 },
+			 { -4, -2, 69, 7, 71, -1, -5 },
+			 { 0, -10, 32, 7, 86, 30, -10 },
+			 { -5, 1, 72, 7, 67, -3, -4 },
+			 { 0, -10, 37, 7, 86, 25, -10 },
+			 { -6, 4, 77, 7, 62, -6, -3 },
+			 { 0, -10, 42, 7, 85, 20, -9 },
+			 { -7, 8, 79, 7, 57, -7, -2 },
+			 { -1, -9, 47, 7, 84, 16, -9 },
+			 { -8, 12, 81, 7, 52, -8, -1 },
+			 { -1, -8, 52, 7, 81, 12, -8 },
+			 { -9, 16, 84, 7, 47, -9, -1 },
+			 { -2, -7, 57, 7, 79, 8, -7 },
+			 { -9, 20, 85, 7, 42, -10, 0 },
+			 { -3, -6, 62, 7, 77, 4, -6 },
+			 { -10, 25, 86, 7, 37, -10, 0 },
+			 { -4, -3, 67, 7, 72, 1, -5 },
+			 { -10, 30, 86, 7, 32, -10, 0 },
+			 { -5, -1, 71, 7, 69, -2, -4 },
+			 { 0, -10, 35, 7, 86, 27, -10 },
+			 { -6, 2, 75, 7, 64, -4, -3 },
+			 { 0, -10, 40, 7, 85, 23, -10 },
+			 { -7, 6, 77, 7, 60, -6, -2 },
+			 { 0, -10, 45, 7, 84, 18, -9 },
+			 { -7, 10, 79, 7, 55, -8, -1 },
+			 { -1, -9, 50, 7, 82, 14, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 31, 86, 7, 31, -10, 0 },
+			 { -5, 0, 72, 7, 68, -3, -4 },
+			 { 0, -10, 36, 7, 86, 26, -10 },
+			 { -6, 3, 76, 7, 63, -5, -3 },
+			 { 0, -10, 41, 7, 85, 21, -9 },
+			 { -7, 7, 78, 7, 59, -7, -2 },
+			 { 0, -10, 46, 7, 84, 17, -9 },
+			 { -8, 11, 80, 7, 54, -8, -1 },
+			 { -1, -9, 51, 7, 82, 13, -8 },
+			 { -9, 15, 83, 7, 49, -9, -1 },
+			 { -2, -8, 56, 7, 80, 9, -7 },
+			 { -9, 19, 85, 7, 43, -10, 0 },
+			 { -3, -6, 61, 7, 77, 5, -6 },
+			 { -10, 24, 86, 7, 38, -10, 0 },
+			 { -3, -4, 66, 7, 72, 2, -5 },
+			 { -10, 29, 86, 7, 33, -10, 0 },
+			 { -4, -1, 68, 7, 70, -1, -4 },
+			 { 0, -10, 33, 7, 86, 29, -10 },
+			 { -5, 2, 72, 7, 66, -4, -3 },
+			 { 0, -10, 38, 7, 86, 24, -10 },
+			 { -6, 5, 77, 7, 61, -6, -3 },
+			 { 0, -10, 43, 7, 85, 19, -9 },
+			 { -7, 9, 80, 7, 56, -8, -2 },
+			 { -1, -9, 49, 7, 83, 15, -9 },
+			 { -8, 13, 82, 7, 51, -9, -1 },
+			 { -1, -8, 54, 7, 80, 11, -8 },
+			 { -9, 17, 84, 7, 46, -10, 0 },
+			 { -2, -7, 59, 7, 78, 7, -7 },
+			 { -9, 21, 85, 7, 41, -10, 0 },
+			 { -3, -5, 63, 7, 76, 3, -6 },
+			 { -10, 26, 86, 7, 36, -10, 0 },
+			 { -4, -3, 68, 7, 72, 0, -5 } },
+		.odd = { { -8, 14, 82, 7, 50, -9, -1 },
+			 { -1, -8, 55, 7, 79, 10, -7 },
+			 { -9, 18, 84, 7, 45, -10, 0 },
+			 { -2, -6, 60, 7, 77, 6, -7 },
+			 { -10, 23, 85, 7, 40, -10, 0 },
+			 { -3, -4, 64, 7, 75, 2, -6 },
+			 { -10, 27, 86, 7, 35, -10, 0 },
+			 { -4, -2, 69, 7, 71, -1, -5 },
+			 { 0, -10, 32, 7, 86, 30, -10 },
+			 { -5, 1, 72, 7, 67, -3, -4 },
+			 { 0, -10, 37, 7, 86, 25, -10 },
+			 { -6, 4, 77, 7, 62, -6, -3 },
+			 { 0, -10, 42, 7, 85, 20, -9 },
+			 { -7, 8, 79, 7, 57, -7, -2 },
+			 { -1, -9, 47, 7, 84, 16, -9 },
+			 { -8, 12, 81, 7, 52, -8, -1 },
+			 { -1, -8, 52, 7, 81, 12, -8 },
+			 { -9, 16, 84, 7, 47, -9, -1 },
+			 { -2, -7, 57, 7, 79, 8, -7 },
+			 { -9, 20, 85, 7, 42, -10, 0 },
+			 { -3, -6, 62, 7, 77, 4, -6 },
+			 { -10, 25, 86, 7, 37, -10, 0 },
+			 { -4, -3, 67, 7, 72, 1, -5 },
+			 { -10, 30, 86, 7, 32, -10, 0 },
+			 { -5, -1, 71, 7, 69, -2, -4 },
+			 { 0, -10, 35, 7, 86, 27, -10 },
+			 { -6, 2, 75, 7, 64, -4, -3 },
+			 { 0, -10, 40, 7, 85, 23, -10 },
+			 { -7, 6, 77, 7, 60, -6, -2 },
+			 { 0, -10, 45, 7, 84, 18, -9 },
+			 { -7, 10, 79, 7, 55, -8, -1 },
+			 { -1, -9, 50, 7, 82, 14, -8 } } },
+	.ptrn_arr = { { 0x3cf9e79f, 0x9e79f3cf, 0xf3cf3e7 } },
+	.sample_patrn_length = 94,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 16) = 0.666667 */
+	.hor_phase_arr = {
+		.even = { { -10, 32, 84, 7, 32, -10, 0 },
+			 { -5, 0, 69, 7, 69, 0, -5 } },
+		.odd = { { -9, 14, 82, 7, 51, -8, -2 },
+			 { -2, -8, 51, 7, 82, 14, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 32, 84, 7, 32, -10, 0 },
+			 { -5, 0, 69, 7, 69, 0, -5 } },
+		.odd = { { -9, 14, 82, 7, 51, -8, -2 },
+			 { -2, -8, 51, 7, 82, 14, -9 } } },
+	.ptrn_arr = { { 0xf } },
+	.sample_patrn_length = 6,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 17) = 0.653061 */
+	.hor_phase_arr = {
+		.even = { { -10, 33, 82, 7, 33, -10, 0 },
+			 { -5, 0, 66, 7, 70, 3, -6 },
+			 { -10, 28, 82, 7, 37, -9, 0 },
+			 { -4, -3, 62, 7, 74, 6, -7 },
+			 { -10, 24, 82, 7, 42, -9, -1 },
+			 { -3, -5, 58, 7, 76, 10, -8 },
+			 { -9, 20, 79, 7, 47, -8, -1 },
+			 { -3, -6, 54, 7, 78, 14, -9 },
+			 { -9, 16, 79, 7, 51, -7, -2 },
+			 { -2, -8, 49, 7, 80, 18, -9 },
+			 { -8, 12, 77, 7, 56, -6, -3 },
+			 { -1, -9, 44, 7, 81, 22, -9 },
+			 { -7, 8, 75, 7, 60, -4, -4 },
+			 { -1, -9, 40, 7, 82, 26, -10 },
+			 { -7, 5, 71, 7, 65, -1, -5 },
+			 { 0, -10, 35, 7, 83, 30, -10 },
+			 { -6, 1, 70, 7, 68, 1, -6 },
+			 { -10, 30, 83, 7, 35, -10, 0 },
+			 { -5, -1, 65, 7, 71, 5, -7 },
+			 { -10, 26, 82, 7, 40, -9, -1 },
+			 { -4, -4, 60, 7, 75, 8, -7 },
+			 { -9, 22, 81, 7, 44, -9, -1 },
+			 { -3, -6, 56, 7, 77, 12, -8 },
+			 { -9, 18, 80, 7, 49, -8, -2 },
+			 { -2, -7, 51, 7, 79, 16, -9 },
+			 { -9, 14, 78, 7, 54, -6, -3 },
+			 { -1, -8, 47, 7, 79, 20, -9 },
+			 { -8, 10, 76, 7, 58, -5, -3 },
+			 { -1, -9, 42, 7, 82, 24, -10 },
+			 { -7, 6, 74, 7, 62, -3, -4 },
+			 { 0, -9, 37, 7, 82, 28, -10 },
+			 { -6, 3, 70, 7, 66, 0, -5 } },
+		.odd = { { -9, 15, 79, 7, 52, -7, -2 },
+			 { -2, -8, 48, 7, 80, 19, -9 },
+			 { -8, 11, 76, 7, 57, -5, -3 },
+			 { -1, -9, 43, 7, 82, 23, -10 },
+			 { -7, 7, 74, 7, 61, -3, -4 },
+			 { -1, -9, 38, 7, 83, 27, -10 },
+			 { -6, 4, 70, 7, 66, -1, -5 },
+			 { 0, -10, 34, 7, 83, 31, -10 },
+			 { -6, 1, 67, 7, 70, 2, -6 },
+			 { -10, 29, 83, 7, 36, -10, 0 },
+			 { -5, -2, 64, 7, 73, 5, -7 },
+			 { -10, 25, 82, 7, 41, -9, -1 },
+			 { -4, -4, 59, 7, 76, 9, -8 },
+			 { -9, 21, 80, 7, 45, -8, -1 },
+			 { -3, -6, 55, 7, 77, 13, -8 },
+			 { -9, 17, 79, 7, 50, -7, -2 },
+			 { -2, -7, 50, 7, 79, 17, -9 },
+			 { -8, 13, 77, 7, 55, -6, -3 },
+			 { -1, -8, 45, 7, 80, 21, -9 },
+			 { -8, 9, 76, 7, 59, -4, -4 },
+			 { -1, -9, 41, 7, 82, 25, -10 },
+			 { -7, 5, 73, 7, 64, -2, -5 },
+			 { 0, -10, 36, 7, 83, 29, -10 },
+			 { -6, 2, 70, 7, 67, 1, -6 },
+			 { -10, 31, 83, 7, 34, -10, 0 },
+			 { -5, -1, 66, 7, 70, 4, -6 },
+			 { -10, 27, 83, 7, 38, -9, -1 },
+			 { -4, -3, 61, 7, 74, 7, -7 },
+			 { -10, 23, 82, 7, 43, -9, -1 },
+			 { -3, -5, 57, 7, 76, 11, -8 },
+			 { -9, 19, 80, 7, 48, -8, -2 },
+			 { -2, -7, 52, 7, 79, 15, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -10, 33, 82, 7, 33, -10, 0 },
+			 { -5, 0, 66, 7, 70, 3, -6 },
+			 { -10, 28, 82, 7, 37, -9, 0 },
+			 { -4, -3, 62, 7, 74, 6, -7 },
+			 { -10, 24, 82, 7, 42, -9, -1 },
+			 { -3, -5, 58, 7, 76, 10, -8 },
+			 { -9, 20, 79, 7, 47, -8, -1 },
+			 { -3, -6, 54, 7, 78, 14, -9 },
+			 { -9, 16, 79, 7, 51, -7, -2 },
+			 { -2, -8, 49, 7, 80, 18, -9 },
+			 { -8, 12, 77, 7, 56, -6, -3 },
+			 { -1, -9, 44, 7, 81, 22, -9 },
+			 { -7, 8, 75, 7, 60, -4, -4 },
+			 { -1, -9, 40, 7, 82, 26, -10 },
+			 { -7, 5, 71, 7, 65, -1, -5 },
+			 { 0, -10, 35, 7, 83, 30, -10 },
+			 { -6, 1, 70, 7, 68, 1, -6 },
+			 { -10, 30, 83, 7, 35, -10, 0 },
+			 { -5, -1, 65, 7, 71, 5, -7 },
+			 { -10, 26, 82, 7, 40, -9, -1 },
+			 { -4, -4, 60, 7, 75, 8, -7 },
+			 { -9, 22, 81, 7, 44, -9, -1 },
+			 { -3, -6, 56, 7, 77, 12, -8 },
+			 { -9, 18, 80, 7, 49, -8, -2 },
+			 { -2, -7, 51, 7, 79, 16, -9 },
+			 { -9, 14, 78, 7, 54, -6, -3 },
+			 { -1, -8, 47, 7, 79, 20, -9 },
+			 { -8, 10, 76, 7, 58, -5, -3 },
+			 { -1, -9, 42, 7, 82, 24, -10 },
+			 { -7, 6, 74, 7, 62, -3, -4 },
+			 { 0, -9, 37, 7, 82, 28, -10 },
+			 { -6, 3, 70, 7, 66, 0, -5 } },
+		.odd = { { -9, 15, 79, 7, 52, -7, -2 },
+			 { -2, -8, 48, 7, 80, 19, -9 },
+			 { -8, 11, 76, 7, 57, -5, -3 },
+			 { -1, -9, 43, 7, 82, 23, -10 },
+			 { -7, 7, 74, 7, 61, -3, -4 },
+			 { -1, -9, 38, 7, 83, 27, -10 },
+			 { -6, 4, 70, 7, 66, -1, -5 },
+			 { 0, -10, 34, 7, 83, 31, -10 },
+			 { -6, 1, 67, 7, 70, 2, -6 },
+			 { -10, 29, 83, 7, 36, -10, 0 },
+			 { -5, -2, 64, 7, 73, 5, -7 },
+			 { -10, 25, 82, 7, 41, -9, -1 },
+			 { -4, -4, 59, 7, 76, 9, -8 },
+			 { -9, 21, 80, 7, 45, -8, -1 },
+			 { -3, -6, 55, 7, 77, 13, -8 },
+			 { -9, 17, 79, 7, 50, -7, -2 },
+			 { -2, -7, 50, 7, 79, 17, -9 },
+			 { -8, 13, 77, 7, 55, -6, -3 },
+			 { -1, -8, 45, 7, 80, 21, -9 },
+			 { -8, 9, 76, 7, 59, -4, -4 },
+			 { -1, -9, 41, 7, 82, 25, -10 },
+			 { -7, 5, 73, 7, 64, -2, -5 },
+			 { 0, -10, 36, 7, 83, 29, -10 },
+			 { -6, 2, 70, 7, 67, 1, -6 },
+			 { -10, 31, 83, 7, 34, -10, 0 },
+			 { -5, -1, 66, 7, 70, 4, -6 },
+			 { -10, 27, 83, 7, 38, -9, -1 },
+			 { -4, -3, 61, 7, 74, 7, -7 },
+			 { -10, 23, 82, 7, 43, -9, -1 },
+			 { -3, -5, 57, 7, 76, 11, -8 },
+			 { -9, 19, 80, 7, 48, -8, -2 },
+			 { -2, -7, 52, 7, 79, 15, -9 } } },
+	.ptrn_arr = { { 0xe73cf3cf, 0x3cf39e79, 0xe79e79cf } },
+	.sample_patrn_length = 98,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 18) = 0.64 */
+	.hor_phase_arr = {
+		.even = { { -9, 33, 80, 7, 33, -9, 0 },
+			 { -6, 0, 64, 7, 71, 6, -7 },
+			 { -10, 25, 80, 7, 42, -8, -1 },
+			 { -4, -4, 56, 7, 76, 13, -9 },
+			 { -9, 17, 78, 7, 51, -6, -3 },
+			 { -2, -7, 47, 7, 78, 21, -9 },
+			 { -8, 9, 74, 7, 60, -2, -5 },
+			 { -1, -9, 38, 7, 81, 29, -10 },
+			 { -6, 3, 66, 7, 68, 3, -6 },
+			 { -10, 29, 81, 7, 38, -9, -1 },
+			 { -5, -2, 60, 7, 74, 9, -8 },
+			 { -9, 21, 78, 7, 47, -7, -2 },
+			 { -3, -6, 51, 7, 78, 17, -9 },
+			 { -9, 13, 76, 7, 56, -4, -4 },
+			 { -1, -8, 42, 7, 80, 25, -10 },
+			 { -7, 6, 71, 7, 64, 0, -6 } },
+		.odd = { { -9, 15, 76, 7, 54, -5, -3 },
+			 { -2, -8, 45, 7, 80, 23, -10 },
+			 { -8, 8, 72, 7, 62, -1, -5 },
+			 { -1, -9, 36, 7, 80, 31, -9 },
+			 { -6, 1, 66, 7, 70, 4, -7 },
+			 { -10, 27, 81, 7, 40, -9, -1 },
+			 { -4, -4, 58, 7, 75, 11, -8 },
+			 { -9, 19, 78, 7, 49, -7, -2 },
+			 { -2, -7, 49, 7, 78, 19, -9 },
+			 { -8, 11, 75, 7, 58, -4, -4 },
+			 { -1, -9, 40, 7, 81, 27, -10 },
+			 { -7, 4, 70, 7, 66, 1, -6 },
+			 { -9, 31, 80, 7, 36, -9, -1 },
+			 { -5, -1, 62, 7, 72, 8, -8 },
+			 { -10, 23, 80, 7, 45, -8, -2 },
+			 { -3, -5, 54, 7, 76, 15, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -9, 33, 80, 7, 33, -9, 0 },
+			 { -6, 0, 64, 7, 71, 6, -7 },
+			 { -10, 25, 80, 7, 42, -8, -1 },
+			 { -4, -4, 56, 7, 76, 13, -9 },
+			 { -9, 17, 78, 7, 51, -6, -3 },
+			 { -2, -7, 47, 7, 78, 21, -9 },
+			 { -8, 9, 74, 7, 60, -2, -5 },
+			 { -1, -9, 38, 7, 81, 29, -10 },
+			 { -6, 3, 66, 7, 68, 3, -6 },
+			 { -10, 29, 81, 7, 38, -9, -1 },
+			 { -5, -2, 60, 7, 74, 9, -8 },
+			 { -9, 21, 78, 7, 47, -7, -2 },
+			 { -3, -6, 51, 7, 78, 17, -9 },
+			 { -9, 13, 76, 7, 56, -4, -4 },
+			 { -1, -8, 42, 7, 80, 25, -10 },
+			 { -7, 6, 71, 7, 64, 0, -6 } },
+		.odd = { { -9, 15, 76, 7, 54, -5, -3 },
+			 { -2, -8, 45, 7, 80, 23, -10 },
+			 { -8, 8, 72, 7, 62, -1, -5 },
+			 { -1, -9, 36, 7, 80, 31, -9 },
+			 { -6, 1, 66, 7, 70, 4, -7 },
+			 { -10, 27, 81, 7, 40, -9, -1 },
+			 { -4, -4, 58, 7, 75, 11, -8 },
+			 { -9, 19, 78, 7, 49, -7, -2 },
+			 { -2, -7, 49, 7, 78, 19, -9 },
+			 { -8, 11, 75, 7, 58, -4, -4 },
+			 { -1, -9, 40, 7, 81, 27, -10 },
+			 { -7, 4, 70, 7, 66, 1, -6 },
+			 { -9, 31, 80, 7, 36, -9, -1 },
+			 { -5, -1, 62, 7, 72, 8, -8 },
+			 { -10, 23, 80, 7, 45, -8, -2 },
+			 { -3, -5, 54, 7, 76, 15, -9 } } },
+	.ptrn_arr = { { 0xf39e73cf, 0xe79c } },
+	.sample_patrn_length = 50,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 19) = 0.627451 */
+	.hor_phase_arr = {
+		.even = { { -9, 34, 79, 7, 34, -9, -1 },
+			 { -6, 0, 61, 7, 72, 9, -8 },
+			 { -9, 22, 78, 7, 47, -7, -3 },
+			 { -3, -6, 49, 7, 77, 20, -9 },
+			 { -8, 11, 72, 7, 59, -1, -5 },
+			 { -1, -9, 36, 7, 79, 32, -9 },
+			 { -6, 1, 63, 7, 71, 7, -8 },
+			 { -9, 24, 77, 7, 45, -7, -2 },
+			 { -4, -5, 51, 7, 77, 18, -9 },
+			 { -9, 13, 73, 7, 58, -2, -5 },
+			 { -1, -8, 38, 7, 78, 30, -9 },
+			 { -6, 3, 65, 7, 67, 6, -7 },
+			 { -9, 26, 78, 7, 43, -8, -2 },
+			 { -4, -4, 53, 7, 76, 16, -9 },
+			 { -9, 14, 75, 7, 55, -3, -4 },
+			 { -2, -8, 40, 7, 79, 28, -9 },
+			 { -7, 4, 67, 7, 67, 4, -7 },
+			 { -9, 28, 79, 7, 40, -8, -2 },
+			 { -4, -3, 55, 7, 75, 14, -9 },
+			 { -9, 16, 76, 7, 53, -4, -4 },
+			 { -2, -8, 43, 7, 78, 26, -9 },
+			 { -7, 6, 67, 7, 65, 3, -6 },
+			 { -9, 30, 78, 7, 38, -8, -1 },
+			 { -5, -2, 58, 7, 73, 13, -9 },
+			 { -9, 18, 77, 7, 51, -5, -4 },
+			 { -2, -7, 45, 7, 77, 24, -9 },
+			 { -8, 7, 71, 7, 63, 1, -6 },
+			 { -9, 32, 79, 7, 36, -9, -1 },
+			 { -5, -1, 59, 7, 72, 11, -8 },
+			 { -9, 20, 77, 7, 49, -6, -3 },
+			 { -3, -7, 47, 7, 78, 22, -9 },
+			 { -8, 9, 72, 7, 61, 0, -6 } },
+		.odd = { { -9, 15, 76, 7, 54, -4, -4 },
+			 { -2, -8, 41, 7, 79, 27, -9 },
+			 { -7, 5, 68, 7, 66, 3, -7 },
+			 { -9, 29, 78, 7, 39, -8, -1 },
+			 { -5, -3, 56, 7, 76, 13, -9 },
+			 { -9, 17, 77, 7, 52, -5, -4 },
+			 { -2, -7, 44, 7, 77, 25, -9 },
+			 { -7, 7, 68, 7, 64, 2, -6 },
+			 { -9, 31, 79, 7, 37, -9, -1 },
+			 { -5, -2, 59, 7, 72, 12, -8 },
+			 { -9, 19, 77, 7, 50, -6, -3 },
+			 { -3, -7, 46, 7, 78, 23, -9 },
+			 { -8, 8, 71, 7, 62, 1, -6 },
+			 { -9, 33, 79, 7, 35, -9, -1 },
+			 { -5, -1, 60, 7, 72, 10, -8 },
+			 { -9, 21, 77, 7, 48, -6, -3 },
+			 { -3, -6, 48, 7, 77, 21, -9 },
+			 { -8, 10, 72, 7, 60, -1, -5 },
+			 { -1, -9, 35, 7, 79, 33, -9 },
+			 { -6, 1, 62, 7, 71, 8, -8 },
+			 { -9, 23, 78, 7, 46, -7, -3 },
+			 { -3, -6, 50, 7, 77, 19, -9 },
+			 { -8, 12, 72, 7, 59, -2, -5 },
+			 { -1, -9, 37, 7, 79, 31, -9 },
+			 { -6, 2, 64, 7, 68, 7, -7 },
+			 { -9, 25, 77, 7, 44, -7, -2 },
+			 { -4, -5, 52, 7, 77, 17, -9 },
+			 { -9, 13, 76, 7, 56, -3, -5 },
+			 { -1, -8, 39, 7, 78, 29, -9 },
+			 { -7, 3, 66, 7, 68, 5, -7 },
+			 { -9, 27, 79, 7, 41, -8, -2 },
+			 { -4, -4, 54, 7, 76, 15, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -9, 34, 79, 7, 34, -9, -1 },
+			 { -6, 0, 61, 7, 72, 9, -8 },
+			 { -9, 22, 78, 7, 47, -7, -3 },
+			 { -3, -6, 49, 7, 77, 20, -9 },
+			 { -8, 11, 72, 7, 59, -1, -5 },
+			 { -1, -9, 36, 7, 79, 32, -9 },
+			 { -6, 1, 63, 7, 71, 7, -8 },
+			 { -9, 24, 77, 7, 45, -7, -2 },
+			 { -4, -5, 51, 7, 77, 18, -9 },
+			 { -9, 13, 73, 7, 58, -2, -5 },
+			 { -1, -8, 38, 7, 78, 30, -9 },
+			 { -6, 3, 65, 7, 67, 6, -7 },
+			 { -9, 26, 78, 7, 43, -8, -2 },
+			 { -4, -4, 53, 7, 76, 16, -9 },
+			 { -9, 14, 75, 7, 55, -3, -4 },
+			 { -2, -8, 40, 7, 79, 28, -9 },
+			 { -7, 4, 67, 7, 67, 4, -7 },
+			 { -9, 28, 79, 7, 40, -8, -2 },
+			 { -4, -3, 55, 7, 75, 14, -9 },
+			 { -9, 16, 76, 7, 53, -4, -4 },
+			 { -2, -8, 43, 7, 78, 26, -9 },
+			 { -7, 6, 67, 7, 65, 3, -6 },
+			 { -9, 30, 78, 7, 38, -8, -1 },
+			 { -5, -2, 58, 7, 73, 13, -9 },
+			 { -9, 18, 77, 7, 51, -5, -4 },
+			 { -2, -7, 45, 7, 77, 24, -9 },
+			 { -8, 7, 71, 7, 63, 1, -6 },
+			 { -9, 32, 79, 7, 36, -9, -1 },
+			 { -5, -1, 59, 7, 72, 11, -8 },
+			 { -9, 20, 77, 7, 49, -6, -3 },
+			 { -3, -7, 47, 7, 78, 22, -9 },
+			 { -8, 9, 72, 7, 61, 0, -6 } },
+		.odd = { { -9, 15, 76, 7, 54, -4, -4 },
+			 { -2, -8, 41, 7, 79, 27, -9 },
+			 { -7, 5, 68, 7, 66, 3, -7 },
+			 { -9, 29, 78, 7, 39, -8, -1 },
+			 { -5, -3, 56, 7, 76, 13, -9 },
+			 { -9, 17, 77, 7, 52, -5, -4 },
+			 { -2, -7, 44, 7, 77, 25, -9 },
+			 { -7, 7, 68, 7, 64, 2, -6 },
+			 { -9, 31, 79, 7, 37, -9, -1 },
+			 { -5, -2, 59, 7, 72, 12, -8 },
+			 { -9, 19, 77, 7, 50, -6, -3 },
+			 { -3, -7, 46, 7, 78, 23, -9 },
+			 { -8, 8, 71, 7, 62, 1, -6 },
+			 { -9, 33, 79, 7, 35, -9, -1 },
+			 { -5, -1, 60, 7, 72, 10, -8 },
+			 { -9, 21, 77, 7, 48, -6, -3 },
+			 { -3, -6, 48, 7, 77, 21, -9 },
+			 { -8, 10, 72, 7, 60, -1, -5 },
+			 { -1, -9, 35, 7, 79, 33, -9 },
+			 { -6, 1, 62, 7, 71, 8, -8 },
+			 { -9, 23, 78, 7, 46, -7, -3 },
+			 { -3, -6, 50, 7, 77, 19, -9 },
+			 { -8, 12, 72, 7, 59, -2, -5 },
+			 { -1, -9, 37, 7, 79, 31, -9 },
+			 { -6, 2, 64, 7, 68, 7, -7 },
+			 { -9, 25, 77, 7, 44, -7, -2 },
+			 { -4, -5, 52, 7, 77, 17, -9 },
+			 { -9, 13, 76, 7, 56, -3, -5 },
+			 { -1, -8, 39, 7, 78, 29, -9 },
+			 { -7, 3, 66, 7, 68, 5, -7 },
+			 { -9, 27, 79, 7, 41, -8, -2 },
+			 { -4, -4, 54, 7, 76, 15, -9 } } },
+	.ptrn_arr = { { 0x79ce79cf, 0x73ce79ce, 0x73ce73ce, 0xe } },
+	.sample_patrn_length = 102,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 20) = 0.615385 */
+	.hor_phase_arr = {
+		.even = { { -8, 34, 77, 7, 34, -8, -1 },
+			 { -6, 0, 59, 7, 71, 12, -8 },
+			 { -9, 19, 75, 7, 51, -4, -4 },
+			 { -3, -7, 43, 7, 77, 27, -9 },
+			 { -7, 6, 64, 7, 66, 6, -7 },
+			 { -9, 27, 77, 7, 43, -7, -3 },
+			 { -4, -4, 51, 7, 75, 19, -9 },
+			 { -8, 12, 71, 7, 59, 0, -6 } },
+		.odd = { { -9, 16, 73, 7, 55, -2, -5 },
+			 { -2, -8, 39, 7, 77, 31, -9 },
+			 { -7, 3, 63, 7, 68, 9, -8 },
+			 { -9, 23, 76, 7, 47, -6, -3 },
+			 { -3, -6, 47, 7, 76, 23, -9 },
+			 { -8, 9, 68, 7, 63, 3, -7 },
+			 { -9, 31, 77, 7, 39, -8, -2 },
+			 { -5, -2, 55, 7, 73, 16, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -8, 34, 77, 7, 34, -8, -1 },
+			 { -6, 0, 59, 7, 71, 12, -8 },
+			 { -9, 19, 75, 7, 51, -4, -4 },
+			 { -3, -7, 43, 7, 77, 27, -9 },
+			 { -7, 6, 64, 7, 66, 6, -7 },
+			 { -9, 27, 77, 7, 43, -7, -3 },
+			 { -4, -4, 51, 7, 75, 19, -9 },
+			 { -8, 12, 71, 7, 59, 0, -6 } },
+		.odd = { { -9, 16, 73, 7, 55, -2, -5 },
+			 { -2, -8, 39, 7, 77, 31, -9 },
+			 { -7, 3, 63, 7, 68, 9, -8 },
+			 { -9, 23, 76, 7, 47, -6, -3 },
+			 { -3, -6, 47, 7, 76, 23, -9 },
+			 { -8, 9, 68, 7, 63, 3, -7 },
+			 { -9, 31, 77, 7, 39, -8, -2 },
+			 { -5, -2, 55, 7, 73, 16, -9 } } },
+	.ptrn_arr = { { 0xe739cf } },
+	.sample_patrn_length = 26,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 21) = 0.603774 */
+	.hor_phase_arr = {
+		.even = { { -8, 35, 76, 7, 35, -8, -2 },
+			 { -6, 0, 57, 7, 71, 15, -9 },
+			 { -9, 17, 71, 7, 55, -1, -5 },
+			 { -2, -8, 37, 7, 76, 33, -8 },
+			 { -6, 1, 58, 7, 71, 13, -9 },
+			 { -9, 18, 73, 7, 53, -2, -5 },
+			 { -2, -7, 39, 7, 75, 31, -8 },
+			 { -7, 2, 60, 7, 69, 12, -8 },
+			 { -9, 20, 74, 7, 51, -3, -5 },
+			 { -3, -7, 41, 7, 77, 29, -9 },
+			 { -7, 4, 62, 7, 67, 10, -8 },
+			 { -9, 22, 74, 7, 49, -4, -4 },
+			 { -3, -6, 43, 7, 75, 28, -9 },
+			 { -7, 5, 63, 7, 67, 8, -8 },
+			 { -9, 24, 75, 7, 47, -5, -4 },
+			 { -4, -5, 45, 7, 75, 26, -9 },
+			 { -8, 7, 65, 7, 65, 7, -8 },
+			 { -9, 26, 75, 7, 45, -5, -4 },
+			 { -4, -5, 47, 7, 75, 24, -9 },
+			 { -8, 8, 67, 7, 63, 5, -7 },
+			 { -9, 28, 75, 7, 43, -6, -3 },
+			 { -4, -4, 49, 7, 74, 22, -9 },
+			 { -8, 10, 67, 7, 62, 4, -7 },
+			 { -9, 29, 77, 7, 41, -7, -3 },
+			 { -5, -3, 51, 7, 74, 20, -9 },
+			 { -8, 12, 69, 7, 60, 2, -7 },
+			 { -8, 31, 75, 7, 39, -7, -2 },
+			 { -5, -2, 53, 7, 73, 18, -9 },
+			 { -9, 13, 71, 7, 58, 1, -6 },
+			 { -8, 33, 76, 7, 37, -8, -2 },
+			 { -5, -1, 55, 7, 71, 17, -9 },
+			 { -9, 15, 71, 7, 57, 0, -6 } },
+		.odd = { { -9, 16, 72, 7, 56, -1, -6 },
+			 { -2, -8, 36, 7, 76, 34, -8 },
+			 { -6, 1, 58, 7, 70, 14, -9 },
+			 { -9, 18, 72, 7, 54, -2, -5 },
+			 { -2, -7, 38, 7, 75, 32, -8 },
+			 { -6, 2, 59, 7, 70, 12, -9 },
+			 { -9, 19, 74, 7, 52, -3, -5 },
+			 { -3, -7, 40, 7, 77, 30, -9 },
+			 { -7, 3, 61, 7, 68, 11, -8 },
+			 { -9, 21, 75, 7, 50, -4, -5 },
+			 { -3, -6, 42, 7, 75, 29, -9 },
+			 { -7, 5, 63, 7, 66, 9, -8 },
+			 { -9, 23, 74, 7, 48, -4, -4 },
+			 { -3, -6, 44, 7, 75, 27, -9 },
+			 { -7, 6, 64, 7, 65, 8, -8 },
+			 { -9, 25, 75, 7, 46, -5, -4 },
+			 { -4, -5, 46, 7, 75, 25, -9 },
+			 { -8, 8, 65, 7, 64, 6, -7 },
+			 { -9, 27, 75, 7, 44, -6, -3 },
+			 { -4, -4, 48, 7, 74, 23, -9 },
+			 { -8, 9, 66, 7, 63, 5, -7 },
+			 { -9, 29, 75, 7, 42, -6, -3 },
+			 { -5, -4, 50, 7, 75, 21, -9 },
+			 { -8, 11, 68, 7, 61, 3, -7 },
+			 { -9, 30, 77, 7, 40, -7, -3 },
+			 { -5, -3, 52, 7, 74, 19, -9 },
+			 { -9, 12, 70, 7, 59, 2, -6 },
+			 { -8, 32, 75, 7, 38, -7, -2 },
+			 { -5, -2, 54, 7, 72, 18, -9 },
+			 { -9, 14, 70, 7, 58, 1, -6 },
+			 { -8, 34, 76, 7, 36, -8, -2 },
+			 { -6, -1, 56, 7, 72, 16, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -8, 35, 76, 7, 35, -8, -2 },
+			 { -6, 0, 57, 7, 71, 15, -9 },
+			 { -9, 17, 71, 7, 55, -1, -5 },
+			 { -2, -8, 37, 7, 76, 33, -8 },
+			 { -6, 1, 58, 7, 71, 13, -9 },
+			 { -9, 18, 73, 7, 53, -2, -5 },
+			 { -2, -7, 39, 7, 75, 31, -8 },
+			 { -7, 2, 60, 7, 69, 12, -8 },
+			 { -9, 20, 74, 7, 51, -3, -5 },
+			 { -3, -7, 41, 7, 77, 29, -9 },
+			 { -7, 4, 62, 7, 67, 10, -8 },
+			 { -9, 22, 74, 7, 49, -4, -4 },
+			 { -3, -6, 43, 7, 75, 28, -9 },
+			 { -7, 5, 63, 7, 67, 8, -8 },
+			 { -9, 24, 75, 7, 47, -5, -4 },
+			 { -4, -5, 45, 7, 75, 26, -9 },
+			 { -8, 7, 65, 7, 65, 7, -8 },
+			 { -9, 26, 75, 7, 45, -5, -4 },
+			 { -4, -5, 47, 7, 75, 24, -9 },
+			 { -8, 8, 67, 7, 63, 5, -7 },
+			 { -9, 28, 75, 7, 43, -6, -3 },
+			 { -4, -4, 49, 7, 74, 22, -9 },
+			 { -8, 10, 67, 7, 62, 4, -7 },
+			 { -9, 29, 77, 7, 41, -7, -3 },
+			 { -5, -3, 51, 7, 74, 20, -9 },
+			 { -8, 12, 69, 7, 60, 2, -7 },
+			 { -8, 31, 75, 7, 39, -7, -2 },
+			 { -5, -2, 53, 7, 73, 18, -9 },
+			 { -9, 13, 71, 7, 58, 1, -6 },
+			 { -8, 33, 76, 7, 37, -8, -2 },
+			 { -5, -1, 55, 7, 71, 17, -9 },
+			 { -9, 15, 71, 7, 57, 0, -6 } },
+		.odd = { { -9, 16, 72, 7, 56, -1, -6 },
+			 { -2, -8, 36, 7, 76, 34, -8 },
+			 { -6, 1, 58, 7, 70, 14, -9 },
+			 { -9, 18, 72, 7, 54, -2, -5 },
+			 { -2, -7, 38, 7, 75, 32, -8 },
+			 { -6, 2, 59, 7, 70, 12, -9 },
+			 { -9, 19, 74, 7, 52, -3, -5 },
+			 { -3, -7, 40, 7, 77, 30, -9 },
+			 { -7, 3, 61, 7, 68, 11, -8 },
+			 { -9, 21, 75, 7, 50, -4, -5 },
+			 { -3, -6, 42, 7, 75, 29, -9 },
+			 { -7, 5, 63, 7, 66, 9, -8 },
+			 { -9, 23, 74, 7, 48, -4, -4 },
+			 { -3, -6, 44, 7, 75, 27, -9 },
+			 { -7, 6, 64, 7, 65, 8, -8 },
+			 { -9, 25, 75, 7, 46, -5, -4 },
+			 { -4, -5, 46, 7, 75, 25, -9 },
+			 { -8, 8, 65, 7, 64, 6, -7 },
+			 { -9, 27, 75, 7, 44, -6, -3 },
+			 { -4, -4, 48, 7, 74, 23, -9 },
+			 { -8, 9, 66, 7, 63, 5, -7 },
+			 { -9, 29, 75, 7, 42, -6, -3 },
+			 { -5, -4, 50, 7, 75, 21, -9 },
+			 { -8, 11, 68, 7, 61, 3, -7 },
+			 { -9, 30, 77, 7, 40, -7, -3 },
+			 { -5, -3, 52, 7, 74, 19, -9 },
+			 { -9, 12, 70, 7, 59, 2, -6 },
+			 { -8, 32, 75, 7, 38, -7, -2 },
+			 { -5, -2, 54, 7, 72, 18, -9 },
+			 { -9, 14, 70, 7, 58, 1, -6 },
+			 { -8, 34, 76, 7, 36, -8, -2 },
+			 { -6, -1, 56, 7, 72, 16, -9 } } },
+	.ptrn_arr = { { 0x9ce739cf, 0xe739ce73, 0x39ce739c, 0xe7 } },
+	.sample_patrn_length = 106,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 22) = 0.592593 */
+	.hor_phase_arr = {
+		.even = { { -7, 35, 74, 7, 35, -7, -2 },
+			 { -6, 0, 54, 7, 71, 18, -9 },
+			 { -9, 14, 70, 7, 58, 2, -7 },
+			 { -8, 32, 74, 7, 39, -6, -3 },
+			 { -5, -2, 51, 7, 72, 21, -9 },
+			 { -8, 11, 66, 7, 61, 5, -7 },
+			 { -9, 28, 75, 7, 43, -5, -4 },
+			 { -4, -4, 47, 7, 73, 25, -9 },
+			 { -8, 8, 64, 7, 64, 8, -8 },
+			 { -9, 25, 73, 7, 47, -4, -4 },
+			 { -4, -5, 43, 7, 75, 28, -9 },
+			 { -7, 5, 61, 7, 66, 11, -8 },
+			 { -9, 21, 72, 7, 51, -2, -5 },
+			 { -3, -6, 39, 7, 74, 32, -8 },
+			 { -7, 2, 58, 7, 70, 14, -9 },
+			 { -9, 18, 71, 7, 54, 0, -6 } },
+		.odd = { { -9, 16, 70, 7, 56, 1, -6 },
+			 { -8, 34, 75, 7, 37, -7, -3 },
+			 { -6, -1, 53, 7, 72, 19, -9 },
+			 { -9, 13, 68, 7, 59, 4, -7 },
+			 { -8, 30, 74, 7, 41, -6, -3 },
+			 { -5, -3, 49, 7, 73, 23, -9 },
+			 { -8, 10, 66, 7, 62, 6, -8 },
+			 { -9, 27, 74, 7, 45, -5, -4 },
+			 { -4, -5, 45, 7, 74, 27, -9 },
+			 { -8, 6, 62, 7, 66, 10, -8 },
+			 { -9, 23, 73, 7, 49, -3, -5 },
+			 { -3, -6, 41, 7, 74, 30, -8 },
+			 { -7, 4, 59, 7, 68, 13, -9 },
+			 { -9, 19, 72, 7, 53, -1, -6 },
+			 { -3, -7, 37, 7, 75, 34, -8 },
+			 { -6, 1, 56, 7, 70, 16, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -7, 35, 74, 7, 35, -7, -2 },
+			 { -6, 0, 54, 7, 71, 18, -9 },
+			 { -9, 14, 70, 7, 58, 2, -7 },
+			 { -8, 32, 74, 7, 39, -6, -3 },
+			 { -5, -2, 51, 7, 72, 21, -9 },
+			 { -8, 11, 66, 7, 61, 5, -7 },
+			 { -9, 28, 75, 7, 43, -5, -4 },
+			 { -4, -4, 47, 7, 73, 25, -9 },
+			 { -8, 8, 64, 7, 64, 8, -8 },
+			 { -9, 25, 73, 7, 47, -4, -4 },
+			 { -4, -5, 43, 7, 75, 28, -9 },
+			 { -7, 5, 61, 7, 66, 11, -8 },
+			 { -9, 21, 72, 7, 51, -2, -5 },
+			 { -3, -6, 39, 7, 74, 32, -8 },
+			 { -7, 2, 58, 7, 70, 14, -9 },
+			 { -9, 18, 71, 7, 54, 0, -6 } },
+		.odd = { { -9, 16, 70, 7, 56, 1, -6 },
+			 { -8, 34, 75, 7, 37, -7, -3 },
+			 { -6, -1, 53, 7, 72, 19, -9 },
+			 { -9, 13, 68, 7, 59, 4, -7 },
+			 { -8, 30, 74, 7, 41, -6, -3 },
+			 { -5, -3, 49, 7, 73, 23, -9 },
+			 { -8, 10, 66, 7, 62, 6, -8 },
+			 { -9, 27, 74, 7, 45, -5, -4 },
+			 { -4, -5, 45, 7, 74, 27, -9 },
+			 { -8, 6, 62, 7, 66, 10, -8 },
+			 { -9, 23, 73, 7, 49, -3, -5 },
+			 { -3, -6, 41, 7, 74, 30, -8 },
+			 { -7, 4, 59, 7, 68, 13, -9 },
+			 { -9, 19, 72, 7, 53, -1, -6 },
+			 { -3, -7, 37, 7, 75, 34, -8 },
+			 { -6, 1, 56, 7, 70, 16, -9 } } },
+	.ptrn_arr = { { 0xce739ce7, 0xce739 } },
+	.sample_patrn_length = 54,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 23) = 0.581818 */
+	.hor_phase_arr = {
+		.even = { { -7, 36, 73, 7, 36, -7, -3 },
+			 { -6, 0, 52, 7, 71, 20, -9 },
+			 { -8, 12, 66, 7, 60, 6, -8 },
+			 { -8, 27, 73, 7, 45, -4, -5 },
+			 { -4, -4, 43, 7, 72, 29, -8 },
+			 { -7, 5, 59, 7, 66, 14, -9 },
+			 { -9, 19, 69, 7, 54, 1, -6 },
+			 { -7, 34, 72, 7, 38, -6, -3 },
+			 { -6, -1, 50, 7, 72, 22, -9 },
+			 { -8, 11, 63, 7, 62, 8, -8 },
+			 { -9, 26, 72, 7, 47, -3, -5 },
+			 { -4, -5, 41, 7, 73, 31, -8 },
+			 { -7, 4, 57, 7, 68, 15, -9 },
+			 { -9, 17, 69, 7, 56, 2, -7 },
+			 { -7, 32, 74, 7, 39, -6, -4 },
+			 { -5, -2, 49, 7, 71, 24, -9 },
+			 { -8, 9, 63, 7, 63, 9, -8 },
+			 { -9, 24, 71, 7, 49, -2, -5 },
+			 { -4, -6, 39, 7, 74, 32, -7 },
+			 { -7, 2, 56, 7, 69, 17, -9 },
+			 { -9, 15, 68, 7, 57, 4, -7 },
+			 { -8, 31, 73, 7, 41, -5, -4 },
+			 { -5, -3, 47, 7, 72, 26, -9 },
+			 { -8, 8, 62, 7, 63, 11, -8 },
+			 { -9, 22, 72, 7, 50, -1, -6 },
+			 { -3, -6, 38, 7, 72, 34, -7 },
+			 { -6, 1, 54, 7, 69, 19, -9 },
+			 { -9, 14, 66, 7, 59, 5, -7 },
+			 { -8, 29, 72, 7, 43, -4, -4 },
+			 { -5, -4, 45, 7, 73, 27, -8 },
+			 { -8, 6, 60, 7, 66, 12, -8 },
+			 { -9, 20, 71, 7, 52, 0, -6 } },
+		.odd = { { -9, 16, 69, 7, 56, 3, -7 },
+			 { -8, 31, 74, 7, 40, -5, -4 },
+			 { -5, -2, 48, 7, 71, 25, -9 },
+			 { -8, 8, 62, 7, 64, 10, -8 },
+			 { -9, 23, 72, 7, 50, -2, -6 },
+			 { -3, -6, 39, 7, 72, 33, -7 },
+			 { -7, 2, 55, 7, 69, 18, -9 },
+			 { -9, 15, 67, 7, 58, 4, -7 },
+			 { -8, 30, 73, 7, 42, -5, -4 },
+			 { -5, -3, 46, 7, 72, 26, -8 },
+			 { -8, 7, 61, 7, 65, 11, -8 },
+			 { -9, 21, 72, 7, 51, -1, -6 },
+			 { -3, -6, 37, 7, 72, 35, -7 },
+			 { -6, 1, 53, 7, 69, 20, -9 },
+			 { -9, 13, 66, 7, 59, 6, -7 },
+			 { -8, 28, 72, 7, 44, -4, -4 },
+			 { -4, -4, 44, 7, 72, 28, -8 },
+			 { -7, 6, 59, 7, 66, 13, -9 },
+			 { -9, 20, 69, 7, 53, 1, -6 },
+			 { -7, 35, 72, 7, 37, -6, -3 },
+			 { -6, -1, 51, 7, 72, 21, -9 },
+			 { -8, 11, 65, 7, 61, 7, -8 },
+			 { -8, 26, 72, 7, 46, -3, -5 },
+			 { -4, -5, 42, 7, 73, 30, -8 },
+			 { -7, 4, 58, 7, 67, 15, -9 },
+			 { -9, 18, 69, 7, 55, 2, -7 },
+			 { -7, 33, 72, 7, 39, -6, -3 },
+			 { -6, -2, 50, 7, 72, 23, -9 },
+			 { -8, 10, 64, 7, 62, 8, -8 },
+			 { -9, 25, 71, 7, 48, -2, -5 },
+			 { -4, -5, 40, 7, 74, 31, -8 },
+			 { -7, 3, 56, 7, 69, 16, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -7, 36, 73, 7, 36, -7, -3 },
+			 { -6, 0, 52, 7, 71, 20, -9 },
+			 { -8, 12, 66, 7, 60, 6, -8 },
+			 { -8, 27, 73, 7, 45, -4, -5 },
+			 { -4, -4, 43, 7, 72, 29, -8 },
+			 { -7, 5, 59, 7, 66, 14, -9 },
+			 { -9, 19, 69, 7, 54, 1, -6 },
+			 { -7, 34, 72, 7, 38, -6, -3 },
+			 { -6, -1, 50, 7, 72, 22, -9 },
+			 { -8, 11, 63, 7, 62, 8, -8 },
+			 { -9, 26, 72, 7, 47, -3, -5 },
+			 { -4, -5, 41, 7, 73, 31, -8 },
+			 { -7, 4, 57, 7, 68, 15, -9 },
+			 { -9, 17, 69, 7, 56, 2, -7 },
+			 { -7, 32, 74, 7, 39, -6, -4 },
+			 { -5, -2, 49, 7, 71, 24, -9 },
+			 { -8, 9, 63, 7, 63, 9, -8 },
+			 { -9, 24, 71, 7, 49, -2, -5 },
+			 { -4, -6, 39, 7, 74, 32, -7 },
+			 { -7, 2, 56, 7, 69, 17, -9 },
+			 { -9, 15, 68, 7, 57, 4, -7 },
+			 { -8, 31, 73, 7, 41, -5, -4 },
+			 { -5, -3, 47, 7, 72, 26, -9 },
+			 { -8, 8, 62, 7, 63, 11, -8 },
+			 { -9, 22, 72, 7, 50, -1, -6 },
+			 { -3, -6, 38, 7, 72, 34, -7 },
+			 { -6, 1, 54, 7, 69, 19, -9 },
+			 { -9, 14, 66, 7, 59, 5, -7 },
+			 { -8, 29, 72, 7, 43, -4, -4 },
+			 { -5, -4, 45, 7, 73, 27, -8 },
+			 { -8, 6, 60, 7, 66, 12, -8 },
+			 { -9, 20, 71, 7, 52, 0, -6 } },
+		.odd = { { -9, 16, 69, 7, 56, 3, -7 },
+			 { -8, 31, 74, 7, 40, -5, -4 },
+			 { -5, -2, 48, 7, 71, 25, -9 },
+			 { -8, 8, 62, 7, 64, 10, -8 },
+			 { -9, 23, 72, 7, 50, -2, -6 },
+			 { -3, -6, 39, 7, 72, 33, -7 },
+			 { -7, 2, 55, 7, 69, 18, -9 },
+			 { -9, 15, 67, 7, 58, 4, -7 },
+			 { -8, 30, 73, 7, 42, -5, -4 },
+			 { -5, -3, 46, 7, 72, 26, -8 },
+			 { -8, 7, 61, 7, 65, 11, -8 },
+			 { -9, 21, 72, 7, 51, -1, -6 },
+			 { -3, -6, 37, 7, 72, 35, -7 },
+			 { -6, 1, 53, 7, 69, 20, -9 },
+			 { -9, 13, 66, 7, 59, 6, -7 },
+			 { -8, 28, 72, 7, 44, -4, -4 },
+			 { -4, -4, 44, 7, 72, 28, -8 },
+			 { -7, 6, 59, 7, 66, 13, -9 },
+			 { -9, 20, 69, 7, 53, 1, -6 },
+			 { -7, 35, 72, 7, 37, -6, -3 },
+			 { -6, -1, 51, 7, 72, 21, -9 },
+			 { -8, 11, 65, 7, 61, 7, -8 },
+			 { -8, 26, 72, 7, 46, -3, -5 },
+			 { -4, -5, 42, 7, 73, 30, -8 },
+			 { -7, 4, 58, 7, 67, 15, -9 },
+			 { -9, 18, 69, 7, 55, 2, -7 },
+			 { -7, 33, 72, 7, 39, -6, -3 },
+			 { -6, -2, 50, 7, 72, 23, -9 },
+			 { -8, 10, 64, 7, 62, 8, -8 },
+			 { -9, 25, 71, 7, 48, -2, -5 },
+			 { -4, -5, 40, 7, 74, 31, -8 },
+			 { -7, 3, 56, 7, 69, 16, -9 } } },
+	.ptrn_arr = { { 0xe7339ce7, 0x9ce7339c, 0x399ce739, 0xce7 } },
+	.sample_patrn_length = 110,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 24) = 0.571429 */
+	.hor_phase_arr = {
+		.even = { { -6, 36, 71, 7, 36, -6, -3 },
+			 { -6, 0, 50, 7, 69, 23, -8 },
+			 { -8, 10, 62, 7, 62, 10, -8 },
+			 { -8, 23, 69, 7, 50, 0, -6 } },
+		.odd = { { -9, 16, 67, 7, 56, 5, -7 },
+			 { -8, 29, 73, 7, 43, -4, -5 },
+			 { -5, -4, 43, 7, 73, 29, -8 },
+			 { -7, 5, 56, 7, 67, 16, -9 } } },
+	.ver_phase_arr = {
+		.even = { { -6, 36, 71, 7, 36, -6, -3 },
+			 { -6, 0, 50, 7, 69, 23, -8 },
+			 { -8, 10, 62, 7, 62, 10, -8 },
+			 { -8, 23, 69, 7, 50, 0, -6 } },
+		.odd = { { -9, 16, 67, 7, 56, 5, -7 },
+			 { -8, 29, 73, 7, 43, -4, -5 },
+			 { -5, -4, 43, 7, 73, 29, -8 },
+			 { -7, 5, 56, 7, 67, 16, -9 } } },
+	.ptrn_arr = { { 0xce7 } },
+	.sample_patrn_length = 14,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 25) = 0.561404 */
+	.hor_phase_arr = {
+		.even = { { -5, 36, 70, 7, 36, -5, -4 },
+			 { -6, 0, 48, 7, 69, 25, -8 },
+			 { -8, 8, 59, 7, 63, 14, -8 },
+			 { -8, 19, 66, 7, 54, 4, -7 },
+			 { -7, 30, 70, 7, 43, -3, -5 },
+			 { -5, -3, 41, 7, 70, 32, -7 },
+			 { -7, 3, 53, 7, 67, 20, -8 },
+			 { -8, 13, 61, 7, 60, 10, -8 },
+			 { -8, 24, 67, 7, 50, 1, -6 },
+			 { -6, 35, 70, 7, 38, -5, -4 },
+			 { -6, -1, 46, 7, 70, 27, -8 },
+			 { -8, 7, 57, 7, 64, 16, -8 },
+			 { -8, 17, 64, 7, 56, 6, -7 },
+			 { -7, 28, 69, 7, 45, -2, -5 },
+			 { -4, -4, 40, 7, 69, 33, -6 },
+			 { -7, 2, 51, 7, 68, 22, -8 },
+			 { -8, 11, 61, 7, 61, 11, -8 },
+			 { -8, 22, 68, 7, 51, 2, -7 },
+			 { -6, 33, 69, 7, 40, -4, -4 },
+			 { -5, -2, 45, 7, 69, 28, -7 },
+			 { -7, 6, 56, 7, 64, 17, -8 },
+			 { -8, 16, 64, 7, 57, 7, -8 },
+			 { -8, 27, 70, 7, 46, -1, -6 },
+			 { -4, -5, 38, 7, 70, 35, -6 },
+			 { -6, 1, 50, 7, 67, 24, -8 },
+			 { -8, 10, 60, 7, 61, 13, -8 },
+			 { -8, 20, 67, 7, 53, 3, -7 },
+			 { -7, 32, 70, 7, 41, -3, -5 },
+			 { -5, -3, 43, 7, 70, 30, -7 },
+			 { -7, 4, 54, 7, 66, 19, -8 },
+			 { -8, 14, 63, 7, 59, 8, -8 },
+			 { -8, 25, 69, 7, 48, 0, -6 } },
+		.odd = { { -8, 16, 66, 7, 56, 6, -8 },
+			 { -8, 28, 69, 7, 46, -1, -6 },
+			 { -4, -4, 39, 7, 69, 34, -6 },
+			 { -7, 2, 51, 7, 67, 23, -8 },
+			 { -8, 10, 60, 7, 62, 12, -8 },
+			 { -8, 21, 67, 7, 52, 3, -7 },
+			 { -7, 32, 71, 7, 41, -4, -5 },
+			 { -5, -2, 44, 7, 69, 29, -7 },
+			 { -7, 5, 55, 7, 65, 18, -8 },
+			 { -8, 15, 63, 7, 58, 8, -8 },
+			 { -8, 26, 69, 7, 47, 0, -6 },
+			 { -4, -5, 37, 7, 71, 35, -6 },
+			 { -6, 1, 49, 7, 68, 24, -8 },
+			 { -8, 9, 59, 7, 63, 13, -8 },
+			 { -8, 20, 65, 7, 54, 4, -7 },
+			 { -7, 31, 70, 7, 42, -3, -5 },
+			 { -5, -3, 42, 7, 70, 31, -7 },
+			 { -7, 4, 54, 7, 65, 20, -8 },
+			 { -8, 13, 63, 7, 59, 9, -8 },
+			 { -8, 24, 68, 7, 49, 1, -6 },
+			 { -6, 35, 71, 7, 37, -5, -4 },
+			 { -6, 0, 47, 7, 69, 26, -8 },
+			 { -8, 8, 58, 7, 63, 15, -8 },
+			 { -8, 18, 65, 7, 55, 5, -7 },
+			 { -7, 29, 69, 7, 44, -2, -5 },
+			 { -5, -4, 41, 7, 71, 32, -7 },
+			 { -7, 3, 52, 7, 67, 21, -8 },
+			 { -8, 12, 62, 7, 60, 10, -8 },
+			 { -8, 23, 67, 7, 51, 2, -7 },
+			 { -6, 34, 69, 7, 39, -4, -4 },
+			 { -6, -1, 46, 7, 69, 28, -8 },
+			 { -8, 6, 56, 7, 66, 16, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -5, 36, 70, 7, 36, -5, -4 },
+			 { -6, 0, 48, 7, 69, 25, -8 },
+			 { -8, 8, 59, 7, 63, 14, -8 },
+			 { -8, 19, 66, 7, 54, 4, -7 },
+			 { -7, 30, 70, 7, 43, -3, -5 },
+			 { -5, -3, 41, 7, 70, 32, -7 },
+			 { -7, 3, 53, 7, 67, 20, -8 },
+			 { -8, 13, 61, 7, 60, 10, -8 },
+			 { -8, 24, 67, 7, 50, 1, -6 },
+			 { -6, 35, 70, 7, 38, -5, -4 },
+			 { -6, -1, 46, 7, 70, 27, -8 },
+			 { -8, 7, 57, 7, 64, 16, -8 },
+			 { -8, 17, 64, 7, 56, 6, -7 },
+			 { -7, 28, 69, 7, 45, -2, -5 },
+			 { -4, -4, 40, 7, 69, 33, -6 },
+			 { -7, 2, 51, 7, 68, 22, -8 },
+			 { -8, 11, 61, 7, 61, 11, -8 },
+			 { -8, 22, 68, 7, 51, 2, -7 },
+			 { -6, 33, 69, 7, 40, -4, -4 },
+			 { -5, -2, 45, 7, 69, 28, -7 },
+			 { -7, 6, 56, 7, 64, 17, -8 },
+			 { -8, 16, 64, 7, 57, 7, -8 },
+			 { -8, 27, 70, 7, 46, -1, -6 },
+			 { -4, -5, 38, 7, 70, 35, -6 },
+			 { -6, 1, 50, 7, 67, 24, -8 },
+			 { -8, 10, 60, 7, 61, 13, -8 },
+			 { -8, 20, 67, 7, 53, 3, -7 },
+			 { -7, 32, 70, 7, 41, -3, -5 },
+			 { -5, -3, 43, 7, 70, 30, -7 },
+			 { -7, 4, 54, 7, 66, 19, -8 },
+			 { -8, 14, 63, 7, 59, 8, -8 },
+			 { -8, 25, 69, 7, 48, 0, -6 } },
+		.odd = { { -8, 16, 66, 7, 56, 6, -8 },
+			 { -8, 28, 69, 7, 46, -1, -6 },
+			 { -4, -4, 39, 7, 69, 34, -6 },
+			 { -7, 2, 51, 7, 67, 23, -8 },
+			 { -8, 10, 60, 7, 62, 12, -8 },
+			 { -8, 21, 67, 7, 52, 3, -7 },
+			 { -7, 32, 71, 7, 41, -4, -5 },
+			 { -5, -2, 44, 7, 69, 29, -7 },
+			 { -7, 5, 55, 7, 65, 18, -8 },
+			 { -8, 15, 63, 7, 58, 8, -8 },
+			 { -8, 26, 69, 7, 47, 0, -6 },
+			 { -4, -5, 37, 7, 71, 35, -6 },
+			 { -6, 1, 49, 7, 68, 24, -8 },
+			 { -8, 9, 59, 7, 63, 13, -8 },
+			 { -8, 20, 65, 7, 54, 4, -7 },
+			 { -7, 31, 70, 7, 42, -3, -5 },
+			 { -5, -3, 42, 7, 70, 31, -7 },
+			 { -7, 4, 54, 7, 65, 20, -8 },
+			 { -8, 13, 63, 7, 59, 9, -8 },
+			 { -8, 24, 68, 7, 49, 1, -6 },
+			 { -6, 35, 71, 7, 37, -5, -4 },
+			 { -6, 0, 47, 7, 69, 26, -8 },
+			 { -8, 8, 58, 7, 63, 15, -8 },
+			 { -8, 18, 65, 7, 55, 5, -7 },
+			 { -7, 29, 69, 7, 44, -2, -5 },
+			 { -5, -4, 41, 7, 71, 32, -7 },
+			 { -7, 3, 52, 7, 67, 21, -8 },
+			 { -8, 12, 62, 7, 60, 10, -8 },
+			 { -8, 23, 67, 7, 51, 2, -7 },
+			 { -6, 34, 69, 7, 39, -4, -4 },
+			 { -6, -1, 46, 7, 69, 28, -8 },
+			 { -8, 6, 56, 7, 66, 16, -8 } } },
+	.ptrn_arr = { { 0x3399cce7, 0x3399cce7, 0x3399ce67, 0xce67 } },
+	.sample_patrn_length = 114,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 26) = 0.551724 */
+	.hor_phase_arr = {
+		.even = { { -5, 36, 70, 7, 36, -5, -4 },
+			 { -6, 0, 46, 7, 68, 27, -7 },
+			 { -8, 7, 55, 7, 64, 18, -8 },
+			 { -8, 15, 62, 7, 58, 9, -8 },
+			 { -8, 24, 68, 7, 49, 2, -7 },
+			 { -6, 33, 69, 7, 40, -3, -5 },
+			 { -6, -2, 43, 7, 70, 30, -7 },
+			 { -7, 4, 52, 7, 66, 21, -8 },
+			 { -8, 12, 60, 7, 60, 12, -8 },
+			 { -8, 21, 66, 7, 52, 4, -7 },
+			 { -7, 30, 70, 7, 43, -2, -6 },
+			 { -5, -3, 40, 7, 69, 33, -6 },
+			 { -7, 2, 49, 7, 68, 24, -8 },
+			 { -8, 9, 58, 7, 62, 15, -8 },
+			 { -8, 18, 64, 7, 55, 7, -8 },
+			 { -7, 27, 68, 7, 46, 0, -6 } },
+		.odd = { { -8, 17, 63, 7, 56, 8, -8 },
+			 { -8, 26, 67, 7, 48, 1, -6 },
+			 { -5, 35, 69, 7, 38, -4, -5 },
+			 { -6, -1, 45, 7, 68, 29, -7 },
+			 { -7, 5, 54, 7, 64, 20, -8 },
+			 { -8, 14, 60, 7, 59, 11, -8 },
+			 { -8, 23, 66, 7, 51, 3, -7 },
+			 { -6, 32, 69, 7, 41, -3, -5 },
+			 { -5, -3, 41, 7, 69, 32, -6 },
+			 { -7, 3, 51, 7, 66, 23, -8 },
+			 { -8, 11, 59, 7, 60, 14, -8 },
+			 { -8, 20, 64, 7, 54, 5, -7 },
+			 { -7, 29, 68, 7, 45, -1, -6 },
+			 { -5, -4, 38, 7, 69, 35, -5 },
+			 { -6, 1, 48, 7, 67, 26, -8 },
+			 { -8, 8, 56, 7, 63, 17, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -5, 36, 70, 7, 36, -5, -4 },
+			 { -6, 0, 46, 7, 68, 27, -7 },
+			 { -8, 7, 55, 7, 64, 18, -8 },
+			 { -8, 15, 62, 7, 58, 9, -8 },
+			 { -8, 24, 68, 7, 49, 2, -7 },
+			 { -6, 33, 69, 7, 40, -3, -5 },
+			 { -6, -2, 43, 7, 70, 30, -7 },
+			 { -7, 4, 52, 7, 66, 21, -8 },
+			 { -8, 12, 60, 7, 60, 12, -8 },
+			 { -8, 21, 66, 7, 52, 4, -7 },
+			 { -7, 30, 70, 7, 43, -2, -6 },
+			 { -5, -3, 40, 7, 69, 33, -6 },
+			 { -7, 2, 49, 7, 68, 24, -8 },
+			 { -8, 9, 58, 7, 62, 15, -8 },
+			 { -8, 18, 64, 7, 55, 7, -8 },
+			 { -7, 27, 68, 7, 46, 0, -6 } },
+		.odd = { { -8, 17, 63, 7, 56, 8, -8 },
+			 { -8, 26, 67, 7, 48, 1, -6 },
+			 { -5, 35, 69, 7, 38, -4, -5 },
+			 { -6, -1, 45, 7, 68, 29, -7 },
+			 { -7, 5, 54, 7, 64, 20, -8 },
+			 { -8, 14, 60, 7, 59, 11, -8 },
+			 { -8, 23, 66, 7, 51, 3, -7 },
+			 { -6, 32, 69, 7, 41, -3, -5 },
+			 { -5, -3, 41, 7, 69, 32, -6 },
+			 { -7, 3, 51, 7, 66, 23, -8 },
+			 { -8, 11, 59, 7, 60, 14, -8 },
+			 { -8, 20, 64, 7, 54, 5, -7 },
+			 { -7, 29, 68, 7, 45, -1, -6 },
+			 { -5, -4, 38, 7, 69, 35, -5 },
+			 { -6, 1, 48, 7, 67, 26, -8 },
+			 { -8, 8, 56, 7, 63, 17, -8 } } },
+	.ptrn_arr = { { 0x399cce67, 0xcce673 } },
+	.sample_patrn_length = 58,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 27) = 0.542373 */
+	.hor_phase_arr = {
+		.even = { { -4, 37, 67, 7, 37, -4, -5 },
+			 { -6, 0, 44, 7, 67, 29, -6 },
+			 { -7, 5, 52, 7, 64, 22, -8 },
+			 { -8, 12, 58, 7, 60, 14, -8 },
+			 { -8, 19, 63, 7, 54, 8, -8 },
+			 { -7, 26, 67, 7, 47, 2, -7 },
+			 { -5, 34, 66, 7, 40, -2, -5 },
+			 { -6, -2, 41, 7, 68, 32, -5 },
+			 { -7, 3, 49, 7, 65, 25, -7 },
+			 { -8, 9, 56, 7, 62, 17, -8 },
+			 { -8, 16, 61, 7, 57, 10, -8 },
+			 { -8, 23, 66, 7, 50, 4, -7 },
+			 { -6, 31, 67, 7, 43, -1, -6 },
+			 { -5, -3, 38, 7, 67, 35, -4 },
+			 { -6, 1, 46, 7, 66, 28, -7 },
+			 { -8, 6, 53, 7, 65, 20, -8 },
+			 { -8, 13, 59, 7, 59, 13, -8 },
+			 { -8, 20, 65, 7, 53, 6, -8 },
+			 { -7, 28, 66, 7, 46, 1, -6 },
+			 { -4, 35, 67, 7, 38, -3, -5 },
+			 { -6, -1, 43, 7, 67, 31, -6 },
+			 { -7, 4, 50, 7, 66, 23, -8 },
+			 { -8, 10, 57, 7, 61, 16, -8 },
+			 { -8, 17, 62, 7, 56, 9, -8 },
+			 { -7, 25, 65, 7, 49, 3, -7 },
+			 { -5, 32, 68, 7, 41, -2, -6 },
+			 { -5, -2, 40, 7, 66, 34, -5 },
+			 { -7, 2, 47, 7, 67, 26, -7 },
+			 { -8, 8, 54, 7, 63, 19, -8 },
+			 { -8, 14, 60, 7, 58, 12, -8 },
+			 { -8, 22, 64, 7, 52, 5, -7 },
+			 { -6, 29, 67, 7, 44, 0, -6 } },
+		.odd = { { -8, 17, 61, 7, 56, 10, -8 },
+			 { -7, 24, 64, 7, 50, 4, -7 },
+			 { -6, 31, 68, 7, 42, -1, -6 },
+			 { -5, -3, 39, 7, 68, 34, -5 },
+			 { -7, 1, 47, 7, 67, 27, -7 },
+			 { -8, 7, 54, 7, 64, 19, -8 },
+			 { -8, 14, 59, 7, 59, 12, -8 },
+			 { -8, 21, 64, 7, 52, 6, -7 },
+			 { -7, 28, 68, 7, 45, 0, -6 },
+			 { -4, 36, 68, 7, 37, -4, -5 },
+			 { -6, 0, 44, 7, 66, 30, -6 },
+			 { -7, 5, 51, 7, 65, 22, -8 },
+			 { -8, 11, 57, 7, 61, 15, -8 },
+			 { -8, 18, 63, 7, 55, 8, -8 },
+			 { -7, 25, 67, 7, 48, 2, -7 },
+			 { -5, 33, 66, 7, 41, -2, -5 },
+			 { -5, -2, 41, 7, 66, 33, -5 },
+			 { -7, 2, 48, 7, 67, 25, -7 },
+			 { -8, 8, 55, 7, 63, 18, -8 },
+			 { -8, 15, 61, 7, 57, 11, -8 },
+			 { -8, 22, 65, 7, 51, 5, -7 },
+			 { -6, 30, 66, 7, 44, 0, -6 },
+			 { -5, -4, 37, 7, 68, 36, -4 },
+			 { -6, 0, 45, 7, 68, 28, -7 },
+			 { -7, 6, 52, 7, 64, 21, -8 },
+			 { -8, 12, 59, 7, 59, 14, -8 },
+			 { -8, 19, 64, 7, 54, 7, -8 },
+			 { -7, 27, 67, 7, 47, 1, -7 },
+			 { -5, 34, 68, 7, 39, -3, -5 },
+			 { -6, -1, 42, 7, 68, 31, -6 },
+			 { -7, 4, 50, 7, 64, 24, -7 },
+			 { -8, 10, 56, 7, 61, 17, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -4, 37, 67, 7, 37, -4, -5 },
+			 { -6, 0, 44, 7, 67, 29, -6 },
+			 { -7, 5, 52, 7, 64, 22, -8 },
+			 { -8, 12, 58, 7, 60, 14, -8 },
+			 { -8, 19, 63, 7, 54, 8, -8 },
+			 { -7, 26, 67, 7, 47, 2, -7 },
+			 { -5, 34, 66, 7, 40, -2, -5 },
+			 { -6, -2, 41, 7, 68, 32, -5 },
+			 { -7, 3, 49, 7, 65, 25, -7 },
+			 { -8, 9, 56, 7, 62, 17, -8 },
+			 { -8, 16, 61, 7, 57, 10, -8 },
+			 { -8, 23, 66, 7, 50, 4, -7 },
+			 { -6, 31, 67, 7, 43, -1, -6 },
+			 { -5, -3, 38, 7, 67, 35, -4 },
+			 { -6, 1, 46, 7, 66, 28, -7 },
+			 { -8, 6, 53, 7, 65, 20, -8 },
+			 { -8, 13, 59, 7, 59, 13, -8 },
+			 { -8, 20, 65, 7, 53, 6, -8 },
+			 { -7, 28, 66, 7, 46, 1, -6 },
+			 { -4, 35, 67, 7, 38, -3, -5 },
+			 { -6, -1, 43, 7, 67, 31, -6 },
+			 { -7, 4, 50, 7, 66, 23, -8 },
+			 { -8, 10, 57, 7, 61, 16, -8 },
+			 { -8, 17, 62, 7, 56, 9, -8 },
+			 { -7, 25, 65, 7, 49, 3, -7 },
+			 { -5, 32, 68, 7, 41, -2, -6 },
+			 { -5, -2, 40, 7, 66, 34, -5 },
+			 { -7, 2, 47, 7, 67, 26, -7 },
+			 { -8, 8, 54, 7, 63, 19, -8 },
+			 { -8, 14, 60, 7, 58, 12, -8 },
+			 { -8, 22, 64, 7, 52, 5, -7 },
+			 { -6, 29, 67, 7, 44, 0, -6 } },
+		.odd = { { -8, 17, 61, 7, 56, 10, -8 },
+			 { -7, 24, 64, 7, 50, 4, -7 },
+			 { -6, 31, 68, 7, 42, -1, -6 },
+			 { -5, -3, 39, 7, 68, 34, -5 },
+			 { -7, 1, 47, 7, 67, 27, -7 },
+			 { -8, 7, 54, 7, 64, 19, -8 },
+			 { -8, 14, 59, 7, 59, 12, -8 },
+			 { -8, 21, 64, 7, 52, 6, -7 },
+			 { -7, 28, 68, 7, 45, 0, -6 },
+			 { -4, 36, 68, 7, 37, -4, -5 },
+			 { -6, 0, 44, 7, 66, 30, -6 },
+			 { -7, 5, 51, 7, 65, 22, -8 },
+			 { -8, 11, 57, 7, 61, 15, -8 },
+			 { -8, 18, 63, 7, 55, 8, -8 },
+			 { -7, 25, 67, 7, 48, 2, -7 },
+			 { -5, 33, 66, 7, 41, -2, -5 },
+			 { -5, -2, 41, 7, 66, 33, -5 },
+			 { -7, 2, 48, 7, 67, 25, -7 },
+			 { -8, 8, 55, 7, 63, 18, -8 },
+			 { -8, 15, 61, 7, 57, 11, -8 },
+			 { -8, 22, 65, 7, 51, 5, -7 },
+			 { -6, 30, 66, 7, 44, 0, -6 },
+			 { -5, -4, 37, 7, 68, 36, -4 },
+			 { -6, 0, 45, 7, 68, 28, -7 },
+			 { -7, 6, 52, 7, 64, 21, -8 },
+			 { -8, 12, 59, 7, 59, 14, -8 },
+			 { -8, 19, 64, 7, 54, 7, -8 },
+			 { -7, 27, 67, 7, 47, 1, -7 },
+			 { -5, 34, 68, 7, 39, -3, -5 },
+			 { -6, -1, 42, 7, 68, 31, -6 },
+			 { -7, 4, 50, 7, 64, 24, -7 },
+			 { -8, 10, 56, 7, 61, 17, -8 } } },
+	.ptrn_arr = { { 0x99ccce67, 0xce667339, 0x733399cc, 0xcce66 } },
+	.sample_patrn_length = 118,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 28) = 0.533333 */
+	.hor_phase_arr = {
+		.even = { { -3, 37, 65, 7, 37, -3, -5 },
+			 { -6, 0, 43, 7, 65, 31, -5 },
+			 { -7, 4, 48, 7, 65, 25, -7 },
+			 { -8, 9, 54, 7, 62, 19, -8 },
+			 { -8, 14, 58, 7, 58, 14, -8 },
+			 { -8, 19, 62, 7, 54, 9, -8 },
+			 { -7, 25, 65, 7, 48, 4, -7 },
+			 { -5, 31, 65, 7, 43, 0, -6 } },
+		.odd = { { -8, 17, 60, 7, 56, 11, -8 },
+			 { -7, 22, 63, 7, 51, 6, -7 },
+			 { -6, 28, 65, 7, 46, 2, -7 },
+			 { -4, 34, 66, 7, 40, -2, -6 },
+			 { -6, -2, 40, 7, 66, 34, -4 },
+			 { -7, 2, 46, 7, 65, 28, -6 },
+			 { -7, 6, 51, 7, 63, 22, -7 },
+			 { -8, 11, 56, 7, 60, 17, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -3, 37, 65, 7, 37, -3, -5 },
+			 { -6, 0, 43, 7, 65, 31, -5 },
+			 { -7, 4, 48, 7, 65, 25, -7 },
+			 { -8, 9, 54, 7, 62, 19, -8 },
+			 { -8, 14, 58, 7, 58, 14, -8 },
+			 { -8, 19, 62, 7, 54, 9, -8 },
+			 { -7, 25, 65, 7, 48, 4, -7 },
+			 { -5, 31, 65, 7, 43, 0, -6 } },
+		.odd = { { -8, 17, 60, 7, 56, 11, -8 },
+			 { -7, 22, 63, 7, 51, 6, -7 },
+			 { -6, 28, 65, 7, 46, 2, -7 },
+			 { -4, 34, 66, 7, 40, -2, -6 },
+			 { -6, -2, 40, 7, 66, 34, -4 },
+			 { -7, 2, 46, 7, 65, 28, -6 },
+			 { -7, 6, 51, 7, 63, 22, -7 },
+			 { -8, 11, 56, 7, 60, 17, -8 } } },
+	.ptrn_arr = { { 0xccce667 } },
+	.sample_patrn_length = 30,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 29) = 0.52459 */
+	.hor_phase_arr = {
+		.even = { { -2, 37, 63, 7, 37, -2, -5 },
+			 { -6, 0, 41, 7, 64, 33, -4 },
+			 { -7, 3, 45, 7, 65, 28, -6 },
+			 { -7, 6, 49, 7, 63, 24, -7 },
+			 { -8, 9, 53, 7, 61, 20, -7 },
+			 { -8, 13, 56, 7, 59, 16, -8 },
+			 { -8, 17, 60, 7, 55, 12, -8 },
+			 { -7, 21, 62, 7, 52, 8, -8 },
+			 { -6, 26, 62, 7, 48, 5, -7 },
+			 { -5, 30, 64, 7, 44, 2, -7 },
+			 { -4, 34, 65, 7, 40, -1, -6 },
+			 { -6, -2, 38, 7, 66, 35, -3 },
+			 { -6, 1, 42, 7, 65, 31, -5 },
+			 { -7, 4, 47, 7, 63, 27, -6 },
+			 { -7, 7, 50, 7, 62, 23, -7 },
+			 { -8, 11, 54, 7, 59, 19, -7 },
+			 { -8, 15, 57, 7, 57, 15, -8 },
+			 { -7, 19, 59, 7, 54, 11, -8 },
+			 { -7, 23, 62, 7, 50, 7, -7 },
+			 { -6, 27, 63, 7, 47, 4, -7 },
+			 { -5, 31, 65, 7, 42, 1, -6 },
+			 { -3, 35, 66, 7, 38, -2, -6 },
+			 { -6, -1, 40, 7, 65, 34, -4 },
+			 { -7, 2, 44, 7, 64, 30, -5 },
+			 { -7, 5, 48, 7, 62, 26, -6 },
+			 { -8, 8, 52, 7, 62, 21, -7 },
+			 { -8, 12, 55, 7, 60, 17, -8 },
+			 { -8, 16, 59, 7, 56, 13, -8 },
+			 { -7, 20, 61, 7, 53, 9, -8 },
+			 { -7, 24, 63, 7, 49, 6, -7 },
+			 { -6, 28, 65, 7, 45, 3, -7 },
+			 { -4, 33, 64, 7, 41, 0, -6 } },
+		.odd = { { -8, 17, 58, 7, 56, 13, -8 },
+			 { -7, 21, 61, 7, 52, 9, -8 },
+			 { -6, 25, 62, 7, 49, 5, -7 },
+			 { -5, 29, 64, 7, 45, 2, -7 },
+			 { -4, 33, 65, 7, 40, 0, -6 },
+			 { -6, -2, 37, 7, 66, 36, -3 },
+			 { -6, 0, 42, 7, 64, 32, -4 },
+			 { -7, 3, 46, 7, 64, 28, -6 },
+			 { -7, 7, 50, 7, 61, 24, -7 },
+			 { -8, 10, 53, 7, 61, 19, -7 },
+			 { -8, 14, 57, 7, 58, 15, -8 },
+			 { -8, 18, 60, 7, 55, 11, -8 },
+			 { -7, 22, 62, 7, 51, 8, -8 },
+			 { -6, 26, 64, 7, 47, 4, -7 },
+			 { -5, 31, 65, 7, 43, 1, -7 },
+			 { -3, 35, 64, 7, 39, -1, -6 },
+			 { -6, -1, 39, 7, 64, 35, -3 },
+			 { -7, 1, 43, 7, 65, 31, -5 },
+			 { -7, 4, 47, 7, 64, 26, -6 },
+			 { -8, 8, 51, 7, 62, 22, -7 },
+			 { -8, 11, 55, 7, 60, 18, -8 },
+			 { -8, 15, 58, 7, 57, 14, -8 },
+			 { -7, 19, 61, 7, 53, 10, -8 },
+			 { -7, 24, 61, 7, 50, 7, -7 },
+			 { -6, 28, 64, 7, 46, 3, -7 },
+			 { -4, 32, 64, 7, 42, 0, -6 },
+			 { -3, 36, 66, 7, 37, -2, -6 },
+			 { -6, 0, 40, 7, 65, 33, -4 },
+			 { -7, 2, 45, 7, 64, 29, -5 },
+			 { -7, 5, 49, 7, 62, 25, -6 },
+			 { -8, 9, 52, 7, 61, 21, -7 },
+			 { -8, 13, 56, 7, 58, 17, -8 } } },
+	.ver_phase_arr = {
+		.even = { { -2, 37, 63, 7, 37, -2, -5 },
+			 { -6, 0, 41, 7, 64, 33, -4 },
+			 { -7, 3, 45, 7, 65, 28, -6 },
+			 { -7, 6, 49, 7, 63, 24, -7 },
+			 { -8, 9, 53, 7, 61, 20, -7 },
+			 { -8, 13, 56, 7, 59, 16, -8 },
+			 { -8, 17, 60, 7, 55, 12, -8 },
+			 { -7, 21, 62, 7, 52, 8, -8 },
+			 { -6, 26, 62, 7, 48, 5, -7 },
+			 { -5, 30, 64, 7, 44, 2, -7 },
+			 { -4, 34, 65, 7, 40, -1, -6 },
+			 { -6, -2, 38, 7, 66, 35, -3 },
+			 { -6, 1, 42, 7, 65, 31, -5 },
+			 { -7, 4, 47, 7, 63, 27, -6 },
+			 { -7, 7, 50, 7, 62, 23, -7 },
+			 { -8, 11, 54, 7, 59, 19, -7 },
+			 { -8, 15, 57, 7, 57, 15, -8 },
+			 { -7, 19, 59, 7, 54, 11, -8 },
+			 { -7, 23, 62, 7, 50, 7, -7 },
+			 { -6, 27, 63, 7, 47, 4, -7 },
+			 { -5, 31, 65, 7, 42, 1, -6 },
+			 { -3, 35, 66, 7, 38, -2, -6 },
+			 { -6, -1, 40, 7, 65, 34, -4 },
+			 { -7, 2, 44, 7, 64, 30, -5 },
+			 { -7, 5, 48, 7, 62, 26, -6 },
+			 { -8, 8, 52, 7, 62, 21, -7 },
+			 { -8, 12, 55, 7, 60, 17, -8 },
+			 { -8, 16, 59, 7, 56, 13, -8 },
+			 { -7, 20, 61, 7, 53, 9, -8 },
+			 { -7, 24, 63, 7, 49, 6, -7 },
+			 { -6, 28, 65, 7, 45, 3, -7 },
+			 { -4, 33, 64, 7, 41, 0, -6 } },
+		.odd = { { -8, 17, 58, 7, 56, 13, -8 },
+			 { -7, 21, 61, 7, 52, 9, -8 },
+			 { -6, 25, 62, 7, 49, 5, -7 },
+			 { -5, 29, 64, 7, 45, 2, -7 },
+			 { -4, 33, 65, 7, 40, 0, -6 },
+			 { -6, -2, 37, 7, 66, 36, -3 },
+			 { -6, 0, 42, 7, 64, 32, -4 },
+			 { -7, 3, 46, 7, 64, 28, -6 },
+			 { -7, 7, 50, 7, 61, 24, -7 },
+			 { -8, 10, 53, 7, 61, 19, -7 },
+			 { -8, 14, 57, 7, 58, 15, -8 },
+			 { -8, 18, 60, 7, 55, 11, -8 },
+			 { -7, 22, 62, 7, 51, 8, -8 },
+			 { -6, 26, 64, 7, 47, 4, -7 },
+			 { -5, 31, 65, 7, 43, 1, -7 },
+			 { -3, 35, 64, 7, 39, -1, -6 },
+			 { -6, -1, 39, 7, 64, 35, -3 },
+			 { -7, 1, 43, 7, 65, 31, -5 },
+			 { -7, 4, 47, 7, 64, 26, -6 },
+			 { -8, 8, 51, 7, 62, 22, -7 },
+			 { -8, 11, 55, 7, 60, 18, -8 },
+			 { -8, 15, 58, 7, 57, 14, -8 },
+			 { -7, 19, 61, 7, 53, 10, -8 },
+			 { -7, 24, 61, 7, 50, 7, -7 },
+			 { -6, 28, 64, 7, 46, 3, -7 },
+			 { -4, 32, 64, 7, 42, 0, -6 },
+			 { -3, 36, 66, 7, 37, -2, -6 },
+			 { -6, 0, 40, 7, 65, 33, -4 },
+			 { -7, 2, 45, 7, 64, 29, -5 },
+			 { -7, 5, 49, 7, 62, 25, -6 },
+			 { -8, 9, 52, 7, 61, 21, -7 },
+			 { -8, 13, 56, 7, 58, 17, -8 } } },
+	.ptrn_arr = { { 0xccce6667, 0x399999cc, 0x66673333, 0xcccce6 } },
+	.sample_patrn_length = 122,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 30) = 0.516129 */
+	.hor_phase_arr = {
+		.even = { { -2, 37, 64, 7, 37, -2, -6 },
+			 { -6, 0, 39, 7, 64, 34, -3 },
+			 { -7, 2, 42, 7, 64, 31, -4 },
+			 { -7, 4, 45, 7, 62, 29, -5 },
+			 { -7, 6, 47, 7, 62, 26, -6 },
+			 { -7, 8, 50, 7, 60, 23, -6 },
+			 { -8, 10, 52, 7, 60, 21, -7 },
+			 { -8, 13, 54, 7, 58, 18, -7 },
+			 { -8, 15, 58, 7, 56, 15, -8 },
+			 { -7, 18, 58, 7, 54, 13, -8 },
+			 { -7, 21, 60, 7, 52, 10, -8 },
+			 { -6, 23, 60, 7, 50, 8, -7 },
+			 { -6, 26, 62, 7, 47, 6, -7 },
+			 { -5, 29, 62, 7, 45, 4, -7 },
+			 { -4, 31, 64, 7, 42, 2, -7 },
+			 { -3, 34, 64, 7, 39, 0, -6 } },
+		.odd = { { -7, 17, 57, 7, 55, 14, -8 },
+			 { -7, 19, 59, 7, 53, 12, -8 },
+			 { -7, 22, 61, 7, 51, 9, -8 },
+			 { -6, 25, 60, 7, 49, 7, -7 },
+			 { -5, 27, 62, 7, 46, 5, -7 },
+			 { -5, 30, 63, 7, 44, 3, -7 },
+			 { -3, 33, 62, 7, 41, 1, -6 },
+			 { -2, 35, 64, 7, 38, -1, -6 },
+			 { -6, -1, 38, 7, 64, 35, -2 },
+			 { -6, 1, 41, 7, 62, 33, -3 },
+			 { -7, 3, 44, 7, 63, 30, -5 },
+			 { -7, 5, 46, 7, 62, 27, -5 },
+			 { -7, 7, 49, 7, 60, 25, -6 },
+			 { -8, 9, 51, 7, 61, 22, -7 },
+			 { -8, 12, 53, 7, 59, 19, -7 },
+			 { -8, 14, 55, 7, 57, 17, -7 } } },
+	.ver_phase_arr = {
+		.even = { { -2, 37, 64, 7, 37, -2, -6 },
+			 { -6, 0, 39, 7, 64, 34, -3 },
+			 { -7, 2, 42, 7, 64, 31, -4 },
+			 { -7, 4, 45, 7, 62, 29, -5 },
+			 { -7, 6, 47, 7, 62, 26, -6 },
+			 { -7, 8, 50, 7, 60, 23, -6 },
+			 { -8, 10, 52, 7, 60, 21, -7 },
+			 { -8, 13, 54, 7, 58, 18, -7 },
+			 { -8, 15, 58, 7, 56, 15, -8 },
+			 { -7, 18, 58, 7, 54, 13, -8 },
+			 { -7, 21, 60, 7, 52, 10, -8 },
+			 { -6, 23, 60, 7, 50, 8, -7 },
+			 { -6, 26, 62, 7, 47, 6, -7 },
+			 { -5, 29, 62, 7, 45, 4, -7 },
+			 { -4, 31, 64, 7, 42, 2, -7 },
+			 { -3, 34, 64, 7, 39, 0, -6 } },
+		.odd = { { -7, 17, 57, 7, 55, 14, -8 },
+			 { -7, 19, 59, 7, 53, 12, -8 },
+			 { -7, 22, 61, 7, 51, 9, -8 },
+			 { -6, 25, 60, 7, 49, 7, -7 },
+			 { -5, 27, 62, 7, 46, 5, -7 },
+			 { -5, 30, 63, 7, 44, 3, -7 },
+			 { -3, 33, 62, 7, 41, 1, -6 },
+			 { -2, 35, 64, 7, 38, -1, -6 },
+			 { -6, -1, 38, 7, 64, 35, -2 },
+			 { -6, 1, 41, 7, 62, 33, -3 },
+			 { -7, 3, 44, 7, 63, 30, -5 },
+			 { -7, 5, 46, 7, 62, 27, -5 },
+			 { -7, 7, 49, 7, 60, 25, -6 },
+			 { -8, 9, 51, 7, 61, 22, -7 },
+			 { -8, 12, 53, 7, 59, 19, -7 },
+			 { -8, 14, 55, 7, 57, 17, -7 } } },
+	.ptrn_arr = { { 0xe6666667, 0xccccccc } },
+	.sample_patrn_length = 62,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 31) = 0.507937 */
+	.hor_phase_arr = {
+		.even = { { -1, 37, 62, 7, 37, -1, -6 },
+			 { -6, 0, 38, 7, 62, 35, -1 },
+			 { -6, 1, 39, 7, 62, 34, -2 },
+			 { -7, 2, 41, 7, 62, 33, -3 },
+			 { -7, 3, 42, 7, 61, 32, -3 },
+			 { -7, 4, 43, 7, 62, 30, -4 },
+			 { -7, 4, 44, 7, 62, 29, -4 },
+			 { -7, 6, 46, 7, 60, 28, -5 },
+			 { -7, 7, 47, 7, 60, 26, -5 },
+			 { -7, 8, 48, 7, 60, 25, -6 },
+			 { -7, 9, 49, 7, 59, 24, -6 },
+			 { -7, 10, 50, 7, 59, 22, -6 },
+			 { -7, 11, 51, 7, 59, 21, -7 },
+			 { -7, 12, 52, 7, 58, 20, -7 },
+			 { -7, 13, 53, 7, 57, 19, -7 },
+			 { -7, 15, 54, 7, 56, 17, -7 },
+			 { -7, 16, 55, 7, 55, 16, -7 },
+			 { -7, 17, 56, 7, 54, 15, -7 },
+			 { -7, 19, 57, 7, 53, 13, -7 },
+			 { -7, 20, 58, 7, 52, 12, -7 },
+			 { -7, 21, 59, 7, 51, 11, -7 },
+			 { -6, 22, 59, 7, 50, 10, -7 },
+			 { -6, 24, 59, 7, 49, 9, -7 },
+			 { -6, 25, 60, 7, 48, 8, -7 },
+			 { -5, 26, 60, 7, 47, 7, -7 },
+			 { -5, 28, 60, 7, 46, 6, -7 },
+			 { -4, 29, 62, 7, 44, 4, -7 },
+			 { -4, 30, 62, 7, 43, 4, -7 },
+			 { -3, 32, 61, 7, 42, 3, -7 },
+			 { -3, 33, 62, 7, 41, 2, -7 },
+			 { -2, 34, 62, 7, 39, 1, -6 },
+			 { -1, 35, 62, 7, 38, 0, -6 } },
+		.odd = { { -7, 17, 55, 7, 55, 15, -7 },
+			 { -7, 18, 56, 7, 54, 14, -7 },
+			 { -7, 19, 57, 7, 53, 13, -7 },
+			 { -7, 20, 58, 7, 52, 12, -7 },
+			 { -6, 22, 58, 7, 51, 10, -7 },
+			 { -6, 23, 59, 7, 50, 9, -7 },
+			 { -6, 24, 60, 7, 49, 8, -7 },
+			 { -5, 26, 60, 7, 47, 7, -7 },
+			 { -5, 27, 61, 7, 46, 6, -7 },
+			 { -5, 28, 62, 7, 45, 5, -7 },
+			 { -4, 30, 61, 7, 44, 4, -7 },
+			 { -4, 31, 62, 7, 43, 3, -7 },
+			 { -3, 32, 63, 7, 41, 2, -7 },
+			 { -2, 34, 61, 7, 40, 1, -6 },
+			 { -2, 35, 62, 7, 39, 0, -6 },
+			 { -1, 36, 62, 7, 37, 0, -6 },
+			 { -6, 0, 37, 7, 62, 36, -1 },
+			 { -6, 0, 39, 7, 62, 35, -2 },
+			 { -6, 1, 40, 7, 61, 34, -2 },
+			 { -7, 2, 41, 7, 63, 32, -3 },
+			 { -7, 3, 43, 7, 62, 31, -4 },
+			 { -7, 4, 44, 7, 61, 30, -4 },
+			 { -7, 5, 45, 7, 62, 28, -5 },
+			 { -7, 6, 46, 7, 61, 27, -5 },
+			 { -7, 7, 47, 7, 60, 26, -5 },
+			 { -7, 8, 49, 7, 60, 24, -6 },
+			 { -7, 9, 50, 7, 59, 23, -6 },
+			 { -7, 10, 51, 7, 58, 22, -6 },
+			 { -7, 12, 52, 7, 58, 20, -7 },
+			 { -7, 13, 53, 7, 57, 19, -7 },
+			 { -7, 14, 54, 7, 56, 18, -7 },
+			 { -7, 15, 55, 7, 55, 17, -7 } } },
+	.ver_phase_arr = {
+		.even = { { -1, 37, 62, 7, 37, -1, -6 },
+			 { -6, 0, 38, 7, 62, 35, -1 },
+			 { -6, 1, 39, 7, 62, 34, -2 },
+			 { -7, 2, 41, 7, 62, 33, -3 },
+			 { -7, 3, 42, 7, 61, 32, -3 },
+			 { -7, 4, 43, 7, 62, 30, -4 },
+			 { -7, 4, 44, 7, 62, 29, -4 },
+			 { -7, 6, 46, 7, 60, 28, -5 },
+			 { -7, 7, 47, 7, 60, 26, -5 },
+			 { -7, 8, 48, 7, 60, 25, -6 },
+			 { -7, 9, 49, 7, 59, 24, -6 },
+			 { -7, 10, 50, 7, 59, 22, -6 },
+			 { -7, 11, 51, 7, 59, 21, -7 },
+			 { -7, 12, 52, 7, 58, 20, -7 },
+			 { -7, 13, 53, 7, 57, 19, -7 },
+			 { -7, 15, 54, 7, 56, 17, -7 },
+			 { -7, 16, 55, 7, 55, 16, -7 },
+			 { -7, 17, 56, 7, 54, 15, -7 },
+			 { -7, 19, 57, 7, 53, 13, -7 },
+			 { -7, 20, 58, 7, 52, 12, -7 },
+			 { -7, 21, 59, 7, 51, 11, -7 },
+			 { -6, 22, 59, 7, 50, 10, -7 },
+			 { -6, 24, 59, 7, 49, 9, -7 },
+			 { -6, 25, 60, 7, 48, 8, -7 },
+			 { -5, 26, 60, 7, 47, 7, -7 },
+			 { -5, 28, 60, 7, 46, 6, -7 },
+			 { -4, 29, 62, 7, 44, 4, -7 },
+			 { -4, 30, 62, 7, 43, 4, -7 },
+			 { -3, 32, 61, 7, 42, 3, -7 },
+			 { -3, 33, 62, 7, 41, 2, -7 },
+			 { -2, 34, 62, 7, 39, 1, -6 },
+			 { -1, 35, 62, 7, 38, 0, -6 } },
+		.odd = { { -7, 17, 55, 7, 55, 15, -7 },
+			 { -7, 18, 56, 7, 54, 14, -7 },
+			 { -7, 19, 57, 7, 53, 13, -7 },
+			 { -7, 20, 58, 7, 52, 12, -7 },
+			 { -6, 22, 58, 7, 51, 10, -7 },
+			 { -6, 23, 59, 7, 50, 9, -7 },
+			 { -6, 24, 60, 7, 49, 8, -7 },
+			 { -5, 26, 60, 7, 47, 7, -7 },
+			 { -5, 27, 61, 7, 46, 6, -7 },
+			 { -5, 28, 62, 7, 45, 5, -7 },
+			 { -4, 30, 61, 7, 44, 4, -7 },
+			 { -4, 31, 62, 7, 43, 3, -7 },
+			 { -3, 32, 63, 7, 41, 2, -7 },
+			 { -2, 34, 61, 7, 40, 1, -6 },
+			 { -2, 35, 62, 7, 39, 0, -6 },
+			 { -1, 36, 62, 7, 37, 0, -6 },
+			 { -6, 0, 37, 7, 62, 36, -1 },
+			 { -6, 0, 39, 7, 62, 35, -2 },
+			 { -6, 1, 40, 7, 61, 34, -2 },
+			 { -7, 2, 41, 7, 63, 32, -3 },
+			 { -7, 3, 43, 7, 62, 31, -4 },
+			 { -7, 4, 44, 7, 61, 30, -4 },
+			 { -7, 5, 45, 7, 62, 28, -5 },
+			 { -7, 6, 46, 7, 61, 27, -5 },
+			 { -7, 7, 47, 7, 60, 26, -5 },
+			 { -7, 8, 49, 7, 60, 24, -6 },
+			 { -7, 9, 50, 7, 59, 23, -6 },
+			 { -7, 10, 51, 7, 58, 22, -6 },
+			 { -7, 12, 52, 7, 58, 20, -7 },
+			 { -7, 13, 53, 7, 57, 19, -7 },
+			 { -7, 14, 54, 7, 56, 18, -7 },
+			 { -7, 15, 55, 7, 55, 17, -7 } } },
+	.ptrn_arr = { { 0x66666667, 0xe6666666, 0xcccccccc, 0xccccccc } },
+	.sample_patrn_length = 126,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 32) = 0.5 */
+	.hor_phase_arr = {
+		.even = { { 0, 8, 112, 7, 8, 0, 0 } },
+		.odd = { { 0, 0, 64, 7, 64, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 8, 112, 7, 8, 0, 0 } },
+		.odd = { { 0, 0, 64, 7, 64, 0, 0 } } },
+	.ptrn_arr = { { 0x3 } },
+	.sample_patrn_length = 4,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 33) = 0.492308 */
+	.hor_phase_arr = {
+		.even = { { 0, 9, 110, 7, 9, 0, 0 },
+			 { 0, 8, 109, 7, 11, 0, 0 },
+			 { 0, 7, 109, 7, 12, 0, 0 },
+			 { 0, 6, 108, 7, 14, 0, 0 },
+			 { 0, 5, 107, 7, 16, 0, 0 },
+			 { 0, 4, 105, 7, 19, 0, 0 },
+			 { 0, 3, 103, 7, 22, 0, 0 },
+			 { 0, 3, 100, 7, 25, 0, 0 },
+			 { 0, 2, 98, 7, 28, 0, 0 },
+			 { 0, 2, 94, 7, 32, 0, 0 },
+			 { 0, 2, 90, 7, 36, 0, 0 },
+			 { 0, 1, 87, 7, 40, 0, 0 },
+			 { 0, 1, 83, 7, 44, 0, 0 },
+			 { 0, 1, 78, 7, 49, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 },
+			 { 0, 0, 64, 7, 64, 0, 0 },
+			 { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 49, 7, 78, 1, 0 },
+			 { 0, 0, 44, 7, 83, 1, 0 },
+			 { 0, 0, 40, 7, 87, 1, 0 },
+			 { 0, 0, 36, 7, 90, 2, 0 },
+			 { 0, 0, 32, 7, 94, 2, 0 },
+			 { 0, 0, 28, 7, 98, 2, 0 },
+			 { 0, 0, 25, 7, 100, 3, 0 },
+			 { 0, 0, 22, 7, 103, 3, 0 },
+			 { 0, 0, 19, 7, 105, 4, 0 },
+			 { 0, 0, 16, 7, 107, 5, 0 },
+			 { 0, 0, 14, 7, 108, 6, 0 },
+			 { 0, 0, 12, 7, 109, 7, 0 },
+			 { 0, 0, 11, 7, 109, 8, 0 } },
+		.odd = { { 0, 0, 61, 7, 67, 0, 0 },
+			 { 0, 0, 56, 7, 71, 1, 0 },
+			 { 0, 0, 51, 7, 76, 1, 0 },
+			 { 0, 0, 46, 7, 81, 1, 0 },
+			 { 0, 0, 42, 7, 85, 1, 0 },
+			 { 0, 0, 38, 7, 89, 1, 0 },
+			 { 0, 0, 34, 7, 92, 2, 0 },
+			 { 0, 0, 30, 7, 96, 2, 0 },
+			 { 0, 0, 26, 7, 99, 3, 0 },
+			 { 0, 0, 23, 7, 102, 3, 0 },
+			 { 0, 0, 20, 7, 104, 4, 0 },
+			 { 0, 0, 18, 7, 106, 4, 0 },
+			 { 0, 0, 15, 7, 108, 5, 0 },
+			 { 0, 0, 13, 7, 109, 6, 0 },
+			 { 0, 0, 11, 7, 110, 7, 0 },
+			 { 0, 0, 10, 7, 110, 8, 0 },
+			 { 0, 8, 110, 7, 10, 0, 0 },
+			 { 0, 7, 110, 7, 11, 0, 0 },
+			 { 0, 6, 109, 7, 13, 0, 0 },
+			 { 0, 5, 108, 7, 15, 0, 0 },
+			 { 0, 4, 106, 7, 18, 0, 0 },
+			 { 0, 4, 104, 7, 20, 0, 0 },
+			 { 0, 3, 102, 7, 23, 0, 0 },
+			 { 0, 3, 99, 7, 26, 0, 0 },
+			 { 0, 2, 96, 7, 30, 0, 0 },
+			 { 0, 2, 92, 7, 34, 0, 0 },
+			 { 0, 1, 89, 7, 38, 0, 0 },
+			 { 0, 1, 85, 7, 42, 0, 0 },
+			 { 0, 1, 81, 7, 46, 0, 0 },
+			 { 0, 1, 76, 7, 51, 0, 0 },
+			 { 0, 1, 71, 7, 56, 0, 0 },
+			 { 0, 0, 67, 7, 61, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 9, 110, 7, 9, 0, 0 },
+			 { 0, 8, 109, 7, 11, 0, 0 },
+			 { 0, 7, 109, 7, 12, 0, 0 },
+			 { 0, 6, 108, 7, 14, 0, 0 },
+			 { 0, 5, 107, 7, 16, 0, 0 },
+			 { 0, 4, 105, 7, 19, 0, 0 },
+			 { 0, 3, 103, 7, 22, 0, 0 },
+			 { 0, 3, 100, 7, 25, 0, 0 },
+			 { 0, 2, 98, 7, 28, 0, 0 },
+			 { 0, 2, 94, 7, 32, 0, 0 },
+			 { 0, 2, 90, 7, 36, 0, 0 },
+			 { 0, 1, 87, 7, 40, 0, 0 },
+			 { 0, 1, 83, 7, 44, 0, 0 },
+			 { 0, 1, 78, 7, 49, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 },
+			 { 0, 0, 64, 7, 64, 0, 0 },
+			 { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 49, 7, 78, 1, 0 },
+			 { 0, 0, 44, 7, 83, 1, 0 },
+			 { 0, 0, 40, 7, 87, 1, 0 },
+			 { 0, 0, 36, 7, 90, 2, 0 },
+			 { 0, 0, 32, 7, 94, 2, 0 },
+			 { 0, 0, 28, 7, 98, 2, 0 },
+			 { 0, 0, 25, 7, 100, 3, 0 },
+			 { 0, 0, 22, 7, 103, 3, 0 },
+			 { 0, 0, 19, 7, 105, 4, 0 },
+			 { 0, 0, 16, 7, 107, 5, 0 },
+			 { 0, 0, 14, 7, 108, 6, 0 },
+			 { 0, 0, 12, 7, 109, 7, 0 },
+			 { 0, 0, 11, 7, 109, 8, 0 } },
+		.odd = { { 0, 0, 61, 7, 67, 0, 0 },
+			 { 0, 0, 56, 7, 71, 1, 0 },
+			 { 0, 0, 51, 7, 76, 1, 0 },
+			 { 0, 0, 46, 7, 81, 1, 0 },
+			 { 0, 0, 42, 7, 85, 1, 0 },
+			 { 0, 0, 38, 7, 89, 1, 0 },
+			 { 0, 0, 34, 7, 92, 2, 0 },
+			 { 0, 0, 30, 7, 96, 2, 0 },
+			 { 0, 0, 26, 7, 99, 3, 0 },
+			 { 0, 0, 23, 7, 102, 3, 0 },
+			 { 0, 0, 20, 7, 104, 4, 0 },
+			 { 0, 0, 18, 7, 106, 4, 0 },
+			 { 0, 0, 15, 7, 108, 5, 0 },
+			 { 0, 0, 13, 7, 109, 6, 0 },
+			 { 0, 0, 11, 7, 110, 7, 0 },
+			 { 0, 0, 10, 7, 110, 8, 0 },
+			 { 0, 8, 110, 7, 10, 0, 0 },
+			 { 0, 7, 110, 7, 11, 0, 0 },
+			 { 0, 6, 109, 7, 13, 0, 0 },
+			 { 0, 5, 108, 7, 15, 0, 0 },
+			 { 0, 4, 106, 7, 18, 0, 0 },
+			 { 0, 4, 104, 7, 20, 0, 0 },
+			 { 0, 3, 102, 7, 23, 0, 0 },
+			 { 0, 3, 99, 7, 26, 0, 0 },
+			 { 0, 2, 96, 7, 30, 0, 0 },
+			 { 0, 2, 92, 7, 34, 0, 0 },
+			 { 0, 1, 89, 7, 38, 0, 0 },
+			 { 0, 1, 85, 7, 42, 0, 0 },
+			 { 0, 1, 81, 7, 46, 0, 0 },
+			 { 0, 1, 76, 7, 51, 0, 0 },
+			 { 0, 1, 71, 7, 56, 0, 0 },
+			 { 0, 0, 67, 7, 61, 0, 0 } } },
+	.ptrn_arr = { { 0x33333333, 0x33333333, 0x99999999, 0x99999999 } },
+	.sample_patrn_length = 130,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 34) = 0.484848 */
+	.hor_phase_arr = {
+		.even = { { 0, 10, 108, 7, 10, 0, 0 },
+			 { 0, 7, 108, 7, 13, 0, 0 },
+			 { 0, 5, 106, 7, 17, 0, 0 },
+			 { 0, 4, 102, 7, 22, 0, 0 },
+			 { 0, 3, 96, 7, 29, 0, 0 },
+			 { 0, 2, 90, 7, 36, 0, 0 },
+			 { 0, 1, 82, 7, 45, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 64, 7, 64, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 45, 7, 82, 1, 0 },
+			 { 0, 0, 36, 7, 90, 2, 0 },
+			 { 0, 0, 29, 7, 96, 3, 0 },
+			 { 0, 0, 22, 7, 102, 4, 0 },
+			 { 0, 0, 17, 7, 106, 5, 0 },
+			 { 0, 0, 13, 7, 108, 7, 0 } },
+		.odd = { { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 49, 7, 78, 1, 0 },
+			 { 0, 0, 40, 7, 87, 1, 0 },
+			 { 0, 0, 32, 7, 94, 2, 0 },
+			 { 0, 0, 25, 7, 100, 3, 0 },
+			 { 0, 0, 20, 7, 104, 4, 0 },
+			 { 0, 0, 15, 7, 107, 6, 0 },
+			 { 0, 0, 11, 7, 109, 8, 0 },
+			 { 0, 8, 109, 7, 11, 0, 0 },
+			 { 0, 6, 107, 7, 15, 0, 0 },
+			 { 0, 4, 104, 7, 20, 0, 0 },
+			 { 0, 3, 100, 7, 25, 0, 0 },
+			 { 0, 2, 94, 7, 32, 0, 0 },
+			 { 0, 1, 87, 7, 40, 0, 0 },
+			 { 0, 1, 78, 7, 49, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 10, 108, 7, 10, 0, 0 },
+			 { 0, 7, 108, 7, 13, 0, 0 },
+			 { 0, 5, 106, 7, 17, 0, 0 },
+			 { 0, 4, 102, 7, 22, 0, 0 },
+			 { 0, 3, 96, 7, 29, 0, 0 },
+			 { 0, 2, 90, 7, 36, 0, 0 },
+			 { 0, 1, 82, 7, 45, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 64, 7, 64, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 45, 7, 82, 1, 0 },
+			 { 0, 0, 36, 7, 90, 2, 0 },
+			 { 0, 0, 29, 7, 96, 3, 0 },
+			 { 0, 0, 22, 7, 102, 4, 0 },
+			 { 0, 0, 17, 7, 106, 5, 0 },
+			 { 0, 0, 13, 7, 108, 7, 0 } },
+		.odd = { { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 49, 7, 78, 1, 0 },
+			 { 0, 0, 40, 7, 87, 1, 0 },
+			 { 0, 0, 32, 7, 94, 2, 0 },
+			 { 0, 0, 25, 7, 100, 3, 0 },
+			 { 0, 0, 20, 7, 104, 4, 0 },
+			 { 0, 0, 15, 7, 107, 6, 0 },
+			 { 0, 0, 11, 7, 109, 8, 0 },
+			 { 0, 8, 109, 7, 11, 0, 0 },
+			 { 0, 6, 107, 7, 15, 0, 0 },
+			 { 0, 4, 104, 7, 20, 0, 0 },
+			 { 0, 3, 100, 7, 25, 0, 0 },
+			 { 0, 2, 94, 7, 32, 0, 0 },
+			 { 0, 1, 87, 7, 40, 0, 0 },
+			 { 0, 1, 78, 7, 49, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 } } },
+	.ptrn_arr = { { 0x33333333, 0x99999999 } },
+	.sample_patrn_length = 66,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 35) = 0.477612 */
+	.hor_phase_arr = {
+		.even = { { 0, 10, 108, 7, 10, 0, 0 },
+			 { 0, 6, 106, 7, 16, 0, 0 },
+			 { 0, 4, 101, 7, 23, 0, 0 },
+			 { 0, 2, 93, 7, 33, 0, 0 },
+			 { 0, 1, 82, 7, 45, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 41, 7, 85, 2, 0 },
+			 { 0, 0, 29, 7, 96, 3, 0 },
+			 { 0, 0, 20, 7, 103, 5, 0 },
+			 { 0, 0, 14, 7, 106, 8, 0 },
+			 { 0, 9, 107, 7, 12, 0, 0 },
+			 { 0, 5, 105, 7, 18, 0, 0 },
+			 { 0, 3, 99, 7, 26, 0, 0 },
+			 { 0, 2, 89, 7, 37, 0, 0 },
+			 { 0, 1, 77, 7, 50, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 50, 7, 77, 1, 0 },
+			 { 0, 0, 37, 7, 89, 2, 0 },
+			 { 0, 0, 26, 7, 99, 3, 0 },
+			 { 0, 0, 18, 7, 105, 5, 0 },
+			 { 0, 0, 12, 7, 107, 9, 0 },
+			 { 0, 8, 106, 7, 14, 0, 0 },
+			 { 0, 5, 103, 7, 20, 0, 0 },
+			 { 0, 3, 96, 7, 29, 0, 0 },
+			 { 0, 2, 85, 7, 41, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 45, 7, 82, 1, 0 },
+			 { 0, 0, 33, 7, 93, 2, 0 },
+			 { 0, 0, 23, 7, 101, 4, 0 },
+			 { 0, 0, 16, 7, 106, 6, 0 } },
+		.odd = { { 0, 0, 56, 7, 71, 1, 0 },
+			 { 0, 0, 43, 7, 84, 1, 0 },
+			 { 0, 0, 31, 7, 94, 3, 0 },
+			 { 0, 0, 22, 7, 102, 4, 0 },
+			 { 0, 0, 15, 7, 106, 7, 0 },
+			 { 0, 9, 108, 7, 11, 0, 0 },
+			 { 0, 6, 105, 7, 17, 0, 0 },
+			 { 0, 4, 99, 7, 25, 0, 0 },
+			 { 0, 2, 91, 7, 35, 0, 0 },
+			 { 0, 1, 80, 7, 47, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 52, 7, 75, 1, 0 },
+			 { 0, 0, 39, 7, 87, 2, 0 },
+			 { 0, 0, 28, 7, 97, 3, 0 },
+			 { 0, 0, 19, 7, 104, 5, 0 },
+			 { 0, 0, 13, 7, 107, 8, 0 },
+			 { 0, 8, 107, 7, 13, 0, 0 },
+			 { 0, 5, 104, 7, 19, 0, 0 },
+			 { 0, 3, 97, 7, 28, 0, 0 },
+			 { 0, 2, 87, 7, 39, 0, 0 },
+			 { 0, 1, 75, 7, 52, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 47, 7, 80, 1, 0 },
+			 { 0, 0, 35, 7, 91, 2, 0 },
+			 { 0, 0, 25, 7, 99, 4, 0 },
+			 { 0, 0, 17, 7, 105, 6, 0 },
+			 { 0, 0, 11, 7, 108, 9, 0 },
+			 { 0, 7, 106, 7, 15, 0, 0 },
+			 { 0, 4, 102, 7, 22, 0, 0 },
+			 { 0, 3, 94, 7, 31, 0, 0 },
+			 { 0, 1, 84, 7, 43, 0, 0 },
+			 { 0, 1, 71, 7, 56, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 10, 108, 7, 10, 0, 0 },
+			 { 0, 6, 106, 7, 16, 0, 0 },
+			 { 0, 4, 101, 7, 23, 0, 0 },
+			 { 0, 2, 93, 7, 33, 0, 0 },
+			 { 0, 1, 82, 7, 45, 0, 0 },
+			 { 0, 1, 68, 7, 59, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 41, 7, 85, 2, 0 },
+			 { 0, 0, 29, 7, 96, 3, 0 },
+			 { 0, 0, 20, 7, 103, 5, 0 },
+			 { 0, 0, 14, 7, 106, 8, 0 },
+			 { 0, 9, 107, 7, 12, 0, 0 },
+			 { 0, 5, 105, 7, 18, 0, 0 },
+			 { 0, 3, 99, 7, 26, 0, 0 },
+			 { 0, 2, 89, 7, 37, 0, 0 },
+			 { 0, 1, 77, 7, 50, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 50, 7, 77, 1, 0 },
+			 { 0, 0, 37, 7, 89, 2, 0 },
+			 { 0, 0, 26, 7, 99, 3, 0 },
+			 { 0, 0, 18, 7, 105, 5, 0 },
+			 { 0, 0, 12, 7, 107, 9, 0 },
+			 { 0, 8, 106, 7, 14, 0, 0 },
+			 { 0, 5, 103, 7, 20, 0, 0 },
+			 { 0, 3, 96, 7, 29, 0, 0 },
+			 { 0, 2, 85, 7, 41, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 59, 7, 68, 1, 0 },
+			 { 0, 0, 45, 7, 82, 1, 0 },
+			 { 0, 0, 33, 7, 93, 2, 0 },
+			 { 0, 0, 23, 7, 101, 4, 0 },
+			 { 0, 0, 16, 7, 106, 6, 0 } },
+		.odd = { { 0, 0, 56, 7, 71, 1, 0 },
+			 { 0, 0, 43, 7, 84, 1, 0 },
+			 { 0, 0, 31, 7, 94, 3, 0 },
+			 { 0, 0, 22, 7, 102, 4, 0 },
+			 { 0, 0, 15, 7, 106, 7, 0 },
+			 { 0, 9, 108, 7, 11, 0, 0 },
+			 { 0, 6, 105, 7, 17, 0, 0 },
+			 { 0, 4, 99, 7, 25, 0, 0 },
+			 { 0, 2, 91, 7, 35, 0, 0 },
+			 { 0, 1, 80, 7, 47, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 52, 7, 75, 1, 0 },
+			 { 0, 0, 39, 7, 87, 2, 0 },
+			 { 0, 0, 28, 7, 97, 3, 0 },
+			 { 0, 0, 19, 7, 104, 5, 0 },
+			 { 0, 0, 13, 7, 107, 8, 0 },
+			 { 0, 8, 107, 7, 13, 0, 0 },
+			 { 0, 5, 104, 7, 19, 0, 0 },
+			 { 0, 3, 97, 7, 28, 0, 0 },
+			 { 0, 2, 87, 7, 39, 0, 0 },
+			 { 0, 1, 75, 7, 52, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 47, 7, 80, 1, 0 },
+			 { 0, 0, 35, 7, 91, 2, 0 },
+			 { 0, 0, 25, 7, 99, 4, 0 },
+			 { 0, 0, 17, 7, 105, 6, 0 },
+			 { 0, 0, 11, 7, 108, 9, 0 },
+			 { 0, 7, 106, 7, 15, 0, 0 },
+			 { 0, 4, 102, 7, 22, 0, 0 },
+			 { 0, 3, 94, 7, 31, 0, 0 },
+			 { 0, 1, 84, 7, 43, 0, 0 },
+			 { 0, 1, 71, 7, 56, 0, 0 } } },
+	.ptrn_arr = { { 0x99933333, 0xccccc999, 0x32666664, 0x99993333,
+			 0x9 } },
+	.sample_patrn_length = 134,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 36) = 0.470588 */
+	.hor_phase_arr = {
+		.even = { { 0, 11, 106, 7, 11, 0, 0 },
+			 { 0, 6, 103, 7, 19, 0, 0 },
+			 { 0, 3, 95, 7, 30, 0, 0 },
+			 { 0, 1, 81, 7, 46, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 46, 7, 81, 1, 0 },
+			 { 0, 0, 30, 7, 95, 3, 0 },
+			 { 0, 0, 19, 7, 103, 6, 0 } },
+		.odd = { { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 37, 7, 89, 2, 0 },
+			 { 0, 0, 24, 7, 100, 4, 0 },
+			 { 0, 0, 14, 7, 106, 8, 0 },
+			 { 0, 8, 106, 7, 14, 0, 0 },
+			 { 0, 4, 100, 7, 24, 0, 0 },
+			 { 0, 2, 89, 7, 37, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 11, 106, 7, 11, 0, 0 },
+			 { 0, 6, 103, 7, 19, 0, 0 },
+			 { 0, 3, 95, 7, 30, 0, 0 },
+			 { 0, 1, 81, 7, 46, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 46, 7, 81, 1, 0 },
+			 { 0, 0, 30, 7, 95, 3, 0 },
+			 { 0, 0, 19, 7, 103, 6, 0 } },
+		.odd = { { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 37, 7, 89, 2, 0 },
+			 { 0, 0, 24, 7, 100, 4, 0 },
+			 { 0, 0, 14, 7, 106, 8, 0 },
+			 { 0, 8, 106, 7, 14, 0, 0 },
+			 { 0, 4, 100, 7, 24, 0, 0 },
+			 { 0, 2, 89, 7, 37, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 } } },
+	.ptrn_arr = { { 0x99993333 } },
+	.sample_patrn_length = 34,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 37) = 0.463768 */
+	.hor_phase_arr = {
+		.even = { { 0, 11, 106, 7, 11, 0, 0 },
+			 { 0, 5, 101, 7, 22, 0, 0 },
+			 { 0, 2, 88, 7, 38, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 46, 7, 80, 2, 0 },
+			 { 0, 0, 28, 7, 96, 4, 0 },
+			 { 0, 0, 15, 7, 104, 9, 0 },
+			 { 0, 7, 104, 7, 17, 0, 0 },
+			 { 0, 3, 94, 7, 31, 0, 0 },
+			 { 0, 1, 77, 7, 50, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 34, 7, 91, 3, 0 },
+			 { 0, 0, 19, 7, 103, 6, 0 },
+			 { 0, 10, 105, 7, 13, 0, 0 },
+			 { 0, 5, 98, 7, 25, 0, 0 },
+			 { 0, 2, 84, 7, 42, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 42, 7, 84, 2, 0 },
+			 { 0, 0, 25, 7, 98, 5, 0 },
+			 { 0, 0, 13, 7, 105, 10, 0 },
+			 { 0, 6, 103, 7, 19, 0, 0 },
+			 { 0, 3, 91, 7, 34, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 50, 7, 77, 1, 0 },
+			 { 0, 0, 31, 7, 94, 3, 0 },
+			 { 0, 0, 17, 7, 104, 7, 0 },
+			 { 0, 9, 104, 7, 15, 0, 0 },
+			 { 0, 4, 96, 7, 28, 0, 0 },
+			 { 0, 2, 80, 7, 46, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 38, 7, 88, 2, 0 },
+			 { 0, 0, 22, 7, 101, 5, 0 } },
+		.odd = { { 0, 0, 52, 7, 75, 1, 0 },
+			 { 0, 0, 33, 7, 92, 3, 0 },
+			 { 0, 0, 18, 7, 103, 7, 0 },
+			 { 0, 9, 105, 7, 14, 0, 0 },
+			 { 0, 4, 98, 7, 26, 0, 0 },
+			 { 0, 2, 82, 7, 44, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 40, 7, 86, 2, 0 },
+			 { 0, 0, 23, 7, 100, 5, 0 },
+			 { 0, 0, 12, 7, 105, 11, 0 },
+			 { 0, 6, 101, 7, 21, 0, 0 },
+			 { 0, 3, 89, 7, 36, 0, 0 },
+			 { 0, 1, 69, 7, 57, 1, 0 },
+			 { 0, 0, 48, 7, 79, 1, 0 },
+			 { 0, 0, 29, 7, 95, 4, 0 },
+			 { 0, 0, 16, 7, 104, 8, 0 },
+			 { 0, 8, 104, 7, 16, 0, 0 },
+			 { 0, 4, 95, 7, 29, 0, 0 },
+			 { 0, 1, 79, 7, 48, 0, 0 },
+			 { 0, 1, 57, 7, 69, 1, 0 },
+			 { 0, 0, 36, 7, 89, 3, 0 },
+			 { 0, 0, 21, 7, 101, 6, 0 },
+			 { 0, 11, 105, 7, 12, 0, 0 },
+			 { 0, 5, 100, 7, 23, 0, 0 },
+			 { 0, 2, 86, 7, 40, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 44, 7, 82, 2, 0 },
+			 { 0, 0, 26, 7, 98, 4, 0 },
+			 { 0, 0, 14, 7, 105, 9, 0 },
+			 { 0, 7, 103, 7, 18, 0, 0 },
+			 { 0, 3, 92, 7, 33, 0, 0 },
+			 { 0, 1, 75, 7, 52, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 11, 106, 7, 11, 0, 0 },
+			 { 0, 5, 101, 7, 22, 0, 0 },
+			 { 0, 2, 88, 7, 38, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 46, 7, 80, 2, 0 },
+			 { 0, 0, 28, 7, 96, 4, 0 },
+			 { 0, 0, 15, 7, 104, 9, 0 },
+			 { 0, 7, 104, 7, 17, 0, 0 },
+			 { 0, 3, 94, 7, 31, 0, 0 },
+			 { 0, 1, 77, 7, 50, 0, 0 },
+			 { 0, 0, 54, 7, 73, 1, 0 },
+			 { 0, 0, 34, 7, 91, 3, 0 },
+			 { 0, 0, 19, 7, 103, 6, 0 },
+			 { 0, 10, 105, 7, 13, 0, 0 },
+			 { 0, 5, 98, 7, 25, 0, 0 },
+			 { 0, 2, 84, 7, 42, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 42, 7, 84, 2, 0 },
+			 { 0, 0, 25, 7, 98, 5, 0 },
+			 { 0, 0, 13, 7, 105, 10, 0 },
+			 { 0, 6, 103, 7, 19, 0, 0 },
+			 { 0, 3, 91, 7, 34, 0, 0 },
+			 { 0, 1, 73, 7, 54, 0, 0 },
+			 { 0, 0, 50, 7, 77, 1, 0 },
+			 { 0, 0, 31, 7, 94, 3, 0 },
+			 { 0, 0, 17, 7, 104, 7, 0 },
+			 { 0, 9, 104, 7, 15, 0, 0 },
+			 { 0, 4, 96, 7, 28, 0, 0 },
+			 { 0, 2, 80, 7, 46, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 38, 7, 88, 2, 0 },
+			 { 0, 0, 22, 7, 101, 5, 0 } },
+		.odd = { { 0, 0, 52, 7, 75, 1, 0 },
+			 { 0, 0, 33, 7, 92, 3, 0 },
+			 { 0, 0, 18, 7, 103, 7, 0 },
+			 { 0, 9, 105, 7, 14, 0, 0 },
+			 { 0, 4, 98, 7, 26, 0, 0 },
+			 { 0, 2, 82, 7, 44, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 40, 7, 86, 2, 0 },
+			 { 0, 0, 23, 7, 100, 5, 0 },
+			 { 0, 0, 12, 7, 105, 11, 0 },
+			 { 0, 6, 101, 7, 21, 0, 0 },
+			 { 0, 3, 89, 7, 36, 0, 0 },
+			 { 0, 1, 69, 7, 57, 1, 0 },
+			 { 0, 0, 48, 7, 79, 1, 0 },
+			 { 0, 0, 29, 7, 95, 4, 0 },
+			 { 0, 0, 16, 7, 104, 8, 0 },
+			 { 0, 8, 104, 7, 16, 0, 0 },
+			 { 0, 4, 95, 7, 29, 0, 0 },
+			 { 0, 1, 79, 7, 48, 0, 0 },
+			 { 0, 1, 57, 7, 69, 1, 0 },
+			 { 0, 0, 36, 7, 89, 3, 0 },
+			 { 0, 0, 21, 7, 101, 6, 0 },
+			 { 0, 11, 105, 7, 12, 0, 0 },
+			 { 0, 5, 100, 7, 23, 0, 0 },
+			 { 0, 2, 86, 7, 40, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 44, 7, 82, 2, 0 },
+			 { 0, 0, 26, 7, 98, 4, 0 },
+			 { 0, 0, 14, 7, 105, 9, 0 },
+			 { 0, 7, 103, 7, 18, 0, 0 },
+			 { 0, 3, 92, 7, 33, 0, 0 },
+			 { 0, 1, 75, 7, 52, 0, 0 } } },
+	.ptrn_arr = { { 0xc9999333, 0x332664cc, 0x4cc99993, 0x93332666,
+			 0x99 } },
+	.sample_patrn_length = 138,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 38) = 0.457143 */
+	.hor_phase_arr = {
+		.even = { { 0, 12, 104, 7, 12, 0, 0 },
+			 { 0, 5, 98, 7, 25, 0, 0 },
+			 { 0, 2, 80, 7, 46, 0, 0 },
+			 { 0, 1, 55, 7, 71, 1, 0 },
+			 { 0, 0, 32, 7, 92, 4, 0 },
+			 { 0, 0, 16, 7, 103, 9, 0 },
+			 { 0, 7, 101, 7, 20, 0, 0 },
+			 { 0, 3, 86, 7, 39, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 39, 7, 86, 3, 0 },
+			 { 0, 0, 20, 7, 101, 7, 0 },
+			 { 0, 9, 103, 7, 16, 0, 0 },
+			 { 0, 4, 92, 7, 32, 0, 0 },
+			 { 0, 1, 71, 7, 55, 1, 0 },
+			 { 0, 0, 46, 7, 80, 2, 0 },
+			 { 0, 0, 25, 7, 98, 5, 0 } },
+		.odd = { { 0, 0, 50, 7, 76, 2, 0 },
+			 { 0, 0, 28, 7, 96, 4, 0 },
+			 { 0, 0, 14, 7, 104, 10, 0 },
+			 { 0, 6, 99, 7, 23, 0, 0 },
+			 { 0, 2, 84, 7, 42, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 35, 7, 90, 3, 0 },
+			 { 0, 0, 18, 7, 102, 8, 0 },
+			 { 0, 8, 102, 7, 18, 0, 0 },
+			 { 0, 3, 90, 7, 35, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 42, 7, 84, 2, 0 },
+			 { 0, 0, 23, 7, 99, 6, 0 },
+			 { 0, 10, 104, 7, 14, 0, 0 },
+			 { 0, 4, 96, 7, 28, 0, 0 },
+			 { 0, 2, 76, 7, 50, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 12, 104, 7, 12, 0, 0 },
+			 { 0, 5, 98, 7, 25, 0, 0 },
+			 { 0, 2, 80, 7, 46, 0, 0 },
+			 { 0, 1, 55, 7, 71, 1, 0 },
+			 { 0, 0, 32, 7, 92, 4, 0 },
+			 { 0, 0, 16, 7, 103, 9, 0 },
+			 { 0, 7, 101, 7, 20, 0, 0 },
+			 { 0, 3, 86, 7, 39, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 39, 7, 86, 3, 0 },
+			 { 0, 0, 20, 7, 101, 7, 0 },
+			 { 0, 9, 103, 7, 16, 0, 0 },
+			 { 0, 4, 92, 7, 32, 0, 0 },
+			 { 0, 1, 71, 7, 55, 1, 0 },
+			 { 0, 0, 46, 7, 80, 2, 0 },
+			 { 0, 0, 25, 7, 98, 5, 0 } },
+		.odd = { { 0, 0, 50, 7, 76, 2, 0 },
+			 { 0, 0, 28, 7, 96, 4, 0 },
+			 { 0, 0, 14, 7, 104, 10, 0 },
+			 { 0, 6, 99, 7, 23, 0, 0 },
+			 { 0, 2, 84, 7, 42, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 35, 7, 90, 3, 0 },
+			 { 0, 0, 18, 7, 102, 8, 0 },
+			 { 0, 8, 102, 7, 18, 0, 0 },
+			 { 0, 3, 90, 7, 35, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 42, 7, 84, 2, 0 },
+			 { 0, 0, 23, 7, 99, 6, 0 },
+			 { 0, 10, 104, 7, 14, 0, 0 },
+			 { 0, 4, 96, 7, 28, 0, 0 },
+			 { 0, 2, 76, 7, 50, 0, 0 } } },
+	.ptrn_arr = { { 0xcc999333, 0x99332664, 0x9 } },
+	.sample_patrn_length = 70,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 39) = 0.450704 */
+	.hor_phase_arr = {
+		.even = { { 0, 13, 102, 7, 13, 0, 0 },
+			 { 0, 5, 94, 7, 29, 0, 0 },
+			 { 0, 1, 71, 7, 55, 1, 0 },
+			 { 0, 0, 43, 7, 83, 2, 0 },
+			 { 0, 0, 21, 7, 100, 7, 0 },
+			 { 0, 8, 102, 7, 18, 0, 0 },
+			 { 0, 3, 86, 7, 39, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 32, 7, 92, 4, 0 },
+			 { 0, 0, 14, 7, 103, 11, 0 },
+			 { 0, 5, 97, 7, 26, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 47, 7, 79, 2, 0 },
+			 { 0, 0, 23, 7, 99, 6, 0 },
+			 { 0, 10, 102, 7, 16, 0, 0 },
+			 { 0, 3, 89, 7, 36, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 36, 7, 89, 3, 0 },
+			 { 0, 0, 16, 7, 102, 10, 0 },
+			 { 0, 6, 99, 7, 23, 0, 0 },
+			 { 0, 2, 79, 7, 47, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 26, 7, 97, 5, 0 },
+			 { 0, 11, 103, 7, 14, 0, 0 },
+			 { 0, 4, 92, 7, 32, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 39, 7, 86, 3, 0 },
+			 { 0, 0, 18, 7, 102, 8, 0 },
+			 { 0, 7, 100, 7, 21, 0, 0 },
+			 { 0, 2, 83, 7, 43, 0, 0 },
+			 { 0, 1, 55, 7, 71, 1, 0 },
+			 { 0, 0, 29, 7, 94, 5, 0 } },
+		.odd = { { 0, 0, 49, 7, 77, 2, 0 },
+			 { 0, 0, 25, 7, 97, 6, 0 },
+			 { 0, 10, 103, 7, 15, 0, 0 },
+			 { 0, 4, 90, 7, 34, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 37, 7, 88, 3, 0 },
+			 { 0, 0, 17, 7, 102, 9, 0 },
+			 { 0, 7, 99, 7, 22, 0, 0 },
+			 { 0, 2, 81, 7, 45, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 27, 7, 96, 5, 0 },
+			 { 0, 12, 103, 7, 13, 0, 0 },
+			 { 0, 4, 93, 7, 31, 0, 0 },
+			 { 0, 1, 69, 7, 57, 1, 0 },
+			 { 0, 0, 41, 7, 84, 3, 0 },
+			 { 0, 0, 20, 7, 100, 8, 0 },
+			 { 0, 8, 100, 7, 20, 0, 0 },
+			 { 0, 3, 84, 7, 41, 0, 0 },
+			 { 0, 1, 57, 7, 69, 1, 0 },
+			 { 0, 0, 31, 7, 93, 4, 0 },
+			 { 0, 0, 13, 7, 103, 12, 0 },
+			 { 0, 5, 96, 7, 27, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 45, 7, 81, 2, 0 },
+			 { 0, 0, 22, 7, 99, 7, 0 },
+			 { 0, 9, 102, 7, 17, 0, 0 },
+			 { 0, 3, 88, 7, 37, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 34, 7, 90, 4, 0 },
+			 { 0, 0, 15, 7, 103, 10, 0 },
+			 { 0, 6, 97, 7, 25, 0, 0 },
+			 { 0, 2, 77, 7, 49, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 13, 102, 7, 13, 0, 0 },
+			 { 0, 5, 94, 7, 29, 0, 0 },
+			 { 0, 1, 71, 7, 55, 1, 0 },
+			 { 0, 0, 43, 7, 83, 2, 0 },
+			 { 0, 0, 21, 7, 100, 7, 0 },
+			 { 0, 8, 102, 7, 18, 0, 0 },
+			 { 0, 3, 86, 7, 39, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 32, 7, 92, 4, 0 },
+			 { 0, 0, 14, 7, 103, 11, 0 },
+			 { 0, 5, 97, 7, 26, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 47, 7, 79, 2, 0 },
+			 { 0, 0, 23, 7, 99, 6, 0 },
+			 { 0, 10, 102, 7, 16, 0, 0 },
+			 { 0, 3, 89, 7, 36, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 36, 7, 89, 3, 0 },
+			 { 0, 0, 16, 7, 102, 10, 0 },
+			 { 0, 6, 99, 7, 23, 0, 0 },
+			 { 0, 2, 79, 7, 47, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 26, 7, 97, 5, 0 },
+			 { 0, 11, 103, 7, 14, 0, 0 },
+			 { 0, 4, 92, 7, 32, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 39, 7, 86, 3, 0 },
+			 { 0, 0, 18, 7, 102, 8, 0 },
+			 { 0, 7, 100, 7, 21, 0, 0 },
+			 { 0, 2, 83, 7, 43, 0, 0 },
+			 { 0, 1, 55, 7, 71, 1, 0 },
+			 { 0, 0, 29, 7, 94, 5, 0 } },
+		.odd = { { 0, 0, 49, 7, 77, 2, 0 },
+			 { 0, 0, 25, 7, 97, 6, 0 },
+			 { 0, 10, 103, 7, 15, 0, 0 },
+			 { 0, 4, 90, 7, 34, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 37, 7, 88, 3, 0 },
+			 { 0, 0, 17, 7, 102, 9, 0 },
+			 { 0, 7, 99, 7, 22, 0, 0 },
+			 { 0, 2, 81, 7, 45, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 27, 7, 96, 5, 0 },
+			 { 0, 12, 103, 7, 13, 0, 0 },
+			 { 0, 4, 93, 7, 31, 0, 0 },
+			 { 0, 1, 69, 7, 57, 1, 0 },
+			 { 0, 0, 41, 7, 84, 3, 0 },
+			 { 0, 0, 20, 7, 100, 8, 0 },
+			 { 0, 8, 100, 7, 20, 0, 0 },
+			 { 0, 3, 84, 7, 41, 0, 0 },
+			 { 0, 1, 57, 7, 69, 1, 0 },
+			 { 0, 0, 31, 7, 93, 4, 0 },
+			 { 0, 0, 13, 7, 103, 12, 0 },
+			 { 0, 5, 96, 7, 27, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 45, 7, 81, 2, 0 },
+			 { 0, 0, 22, 7, 99, 7, 0 },
+			 { 0, 9, 102, 7, 17, 0, 0 },
+			 { 0, 3, 88, 7, 37, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 34, 7, 90, 4, 0 },
+			 { 0, 0, 15, 7, 103, 10, 0 },
+			 { 0, 6, 97, 7, 25, 0, 0 },
+			 { 0, 2, 77, 7, 49, 0, 0 } } },
+	.ptrn_arr = { { 0x4cc99933, 0xc9993266, 0x9332664c, 0x32664cc9,
+			 0x993 } },
+	.sample_patrn_length = 142,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 40) = 0.444444 */
+	.hor_phase_arr = {
+		.even = { { 0, 13, 102, 7, 13, 0, 0 },
+			 { 0, 4, 91, 7, 33, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 33, 7, 91, 4, 0 } },
+		.odd = { { 0, 0, 47, 7, 79, 2, 0 },
+			 { 0, 0, 21, 7, 99, 8, 0 },
+			 { 0, 8, 99, 7, 21, 0, 0 },
+			 { 0, 2, 79, 7, 47, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 13, 102, 7, 13, 0, 0 },
+			 { 0, 4, 91, 7, 33, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 33, 7, 91, 4, 0 } },
+		.odd = { { 0, 0, 47, 7, 79, 2, 0 },
+			 { 0, 0, 21, 7, 99, 8, 0 },
+			 { 0, 8, 99, 7, 21, 0, 0 },
+			 { 0, 2, 79, 7, 47, 0, 0 } } },
+	.ptrn_arr = { { 0x9933 } },
+	.sample_patrn_length = 18,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 41) = 0.438356 */
+	.hor_phase_arr = {
+		.even = { { 0, 14, 100, 7, 14, 0, 0 },
+			 { 0, 4, 87, 7, 37, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 25, 7, 96, 7, 0 },
+			 { 0, 8, 98, 7, 22, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 40, 7, 85, 3, 0 },
+			 { 0, 0, 16, 7, 100, 12, 0 },
+			 { 0, 5, 90, 7, 33, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 27, 7, 95, 6, 0 },
+			 { 0, 9, 99, 7, 20, 0, 0 },
+			 { 0, 2, 78, 7, 47, 1, 0 },
+			 { 0, 0, 44, 7, 81, 3, 0 },
+			 { 0, 0, 18, 7, 99, 11, 0 },
+			 { 0, 5, 93, 7, 30, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 30, 7, 93, 5, 0 },
+			 { 0, 11, 99, 7, 18, 0, 0 },
+			 { 0, 3, 81, 7, 44, 0, 0 },
+			 { 0, 1, 47, 7, 78, 2, 0 },
+			 { 0, 0, 20, 7, 99, 9, 0 },
+			 { 0, 6, 95, 7, 27, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 33, 7, 90, 5, 0 },
+			 { 0, 12, 100, 7, 16, 0, 0 },
+			 { 0, 3, 85, 7, 40, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 22, 7, 98, 8, 0 },
+			 { 0, 7, 96, 7, 25, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 37, 7, 87, 4, 0 } },
+		.odd = { { 0, 0, 45, 7, 80, 3, 0 },
+			 { 0, 0, 19, 7, 99, 10, 0 },
+			 { 0, 6, 93, 7, 29, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 32, 7, 91, 5, 0 },
+			 { 0, 11, 100, 7, 17, 0, 0 },
+			 { 0, 3, 83, 7, 42, 0, 0 },
+			 { 0, 1, 49, 7, 76, 2, 0 },
+			 { 0, 0, 21, 7, 98, 9, 0 },
+			 { 0, 7, 95, 7, 26, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 35, 7, 89, 4, 0 },
+			 { 0, 13, 100, 7, 15, 0, 0 },
+			 { 0, 4, 86, 7, 38, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 23, 7, 97, 8, 0 },
+			 { 0, 8, 97, 7, 23, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 38, 7, 86, 4, 0 },
+			 { 0, 0, 15, 7, 100, 13, 0 },
+			 { 0, 4, 89, 7, 35, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 26, 7, 95, 7, 0 },
+			 { 0, 9, 98, 7, 21, 0, 0 },
+			 { 0, 2, 76, 7, 49, 1, 0 },
+			 { 0, 0, 42, 7, 83, 3, 0 },
+			 { 0, 0, 17, 7, 100, 11, 0 },
+			 { 0, 5, 91, 7, 32, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 29, 7, 93, 6, 0 },
+			 { 0, 10, 99, 7, 19, 0, 0 },
+			 { 0, 3, 80, 7, 45, 0, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 14, 100, 7, 14, 0, 0 },
+			 { 0, 4, 87, 7, 37, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 25, 7, 96, 7, 0 },
+			 { 0, 8, 98, 7, 22, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 40, 7, 85, 3, 0 },
+			 { 0, 0, 16, 7, 100, 12, 0 },
+			 { 0, 5, 90, 7, 33, 0, 0 },
+			 { 0, 1, 59, 7, 67, 1, 0 },
+			 { 0, 0, 27, 7, 95, 6, 0 },
+			 { 0, 9, 99, 7, 20, 0, 0 },
+			 { 0, 2, 78, 7, 47, 1, 0 },
+			 { 0, 0, 44, 7, 81, 3, 0 },
+			 { 0, 0, 18, 7, 99, 11, 0 },
+			 { 0, 5, 93, 7, 30, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 30, 7, 93, 5, 0 },
+			 { 0, 11, 99, 7, 18, 0, 0 },
+			 { 0, 3, 81, 7, 44, 0, 0 },
+			 { 0, 1, 47, 7, 78, 2, 0 },
+			 { 0, 0, 20, 7, 99, 9, 0 },
+			 { 0, 6, 95, 7, 27, 0, 0 },
+			 { 0, 1, 67, 7, 59, 1, 0 },
+			 { 0, 0, 33, 7, 90, 5, 0 },
+			 { 0, 12, 100, 7, 16, 0, 0 },
+			 { 0, 3, 85, 7, 40, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 22, 7, 98, 8, 0 },
+			 { 0, 7, 96, 7, 25, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 37, 7, 87, 4, 0 } },
+		.odd = { { 0, 0, 45, 7, 80, 3, 0 },
+			 { 0, 0, 19, 7, 99, 10, 0 },
+			 { 0, 6, 93, 7, 29, 0, 0 },
+			 { 0, 1, 65, 7, 61, 1, 0 },
+			 { 0, 0, 32, 7, 91, 5, 0 },
+			 { 0, 11, 100, 7, 17, 0, 0 },
+			 { 0, 3, 83, 7, 42, 0, 0 },
+			 { 0, 1, 49, 7, 76, 2, 0 },
+			 { 0, 0, 21, 7, 98, 9, 0 },
+			 { 0, 7, 95, 7, 26, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 35, 7, 89, 4, 0 },
+			 { 0, 13, 100, 7, 15, 0, 0 },
+			 { 0, 4, 86, 7, 38, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 23, 7, 97, 8, 0 },
+			 { 0, 8, 97, 7, 23, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 38, 7, 86, 4, 0 },
+			 { 0, 0, 15, 7, 100, 13, 0 },
+			 { 0, 4, 89, 7, 35, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 26, 7, 95, 7, 0 },
+			 { 0, 9, 98, 7, 21, 0, 0 },
+			 { 0, 2, 76, 7, 49, 1, 0 },
+			 { 0, 0, 42, 7, 83, 3, 0 },
+			 { 0, 0, 17, 7, 100, 11, 0 },
+			 { 0, 5, 91, 7, 32, 0, 0 },
+			 { 0, 1, 61, 7, 65, 1, 0 },
+			 { 0, 0, 29, 7, 93, 6, 0 },
+			 { 0, 10, 99, 7, 19, 0, 0 },
+			 { 0, 3, 80, 7, 45, 0, 0 } } },
+	.ptrn_arr = { { 0x664c9933, 0x664c9932, 0x64cc9932, 0x64cc9932,
+			 0x9932 } },
+	.sample_patrn_length = 146,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 42) = 0.432432 */
+	.hor_phase_arr = {
+		.even = { { 0, 14, 100, 7, 14, 0, 0 },
+			 { 0, 4, 84, 7, 40, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 0, 18, 7, 99, 11, 0 },
+			 { 0, 5, 89, 7, 34, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 23, 7, 96, 9, 0 },
+			 { 0, 7, 93, 7, 28, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 28, 7, 93, 7, 0 },
+			 { 0, 9, 96, 7, 23, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 34, 7, 89, 5, 0 },
+			 { 0, 11, 99, 7, 18, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 40, 7, 84, 4, 0 } },
+		.odd = { { 0, 1, 44, 7, 80, 3, 0 },
+			 { 0, 0, 16, 7, 99, 13, 0 },
+			 { 0, 4, 87, 7, 37, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 20, 7, 98, 10, 0 },
+			 { 0, 6, 91, 7, 31, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 25, 7, 95, 8, 0 },
+			 { 0, 8, 95, 7, 25, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 31, 7, 91, 6, 0 },
+			 { 0, 10, 98, 7, 20, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 37, 7, 87, 4, 0 },
+			 { 0, 13, 99, 7, 16, 0, 0 },
+			 { 0, 3, 80, 7, 44, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 14, 100, 7, 14, 0, 0 },
+			 { 0, 4, 84, 7, 40, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 0, 18, 7, 99, 11, 0 },
+			 { 0, 5, 89, 7, 34, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 23, 7, 96, 9, 0 },
+			 { 0, 7, 93, 7, 28, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 28, 7, 93, 7, 0 },
+			 { 0, 9, 96, 7, 23, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 34, 7, 89, 5, 0 },
+			 { 0, 11, 99, 7, 18, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 40, 7, 84, 4, 0 } },
+		.odd = { { 0, 1, 44, 7, 80, 3, 0 },
+			 { 0, 0, 16, 7, 99, 13, 0 },
+			 { 0, 4, 87, 7, 37, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 20, 7, 98, 10, 0 },
+			 { 0, 6, 91, 7, 31, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 25, 7, 95, 8, 0 },
+			 { 0, 8, 95, 7, 25, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 31, 7, 91, 6, 0 },
+			 { 0, 10, 98, 7, 20, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 37, 7, 87, 4, 0 },
+			 { 0, 13, 99, 7, 16, 0, 0 },
+			 { 0, 3, 80, 7, 44, 1, 0 } } },
+	.ptrn_arr = { { 0x264c9933, 0x3264c993, 0x99 } },
+	.sample_patrn_length = 74,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 43) = 0.426667 */
+	.hor_phase_arr = {
+		.even = { { 0, 15, 98, 7, 15, 0, 0 },
+			 { 0, 3, 80, 7, 44, 1, 0 },
+			 { 0, 0, 41, 7, 83, 4, 0 },
+			 { 0, 13, 98, 7, 17, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 38, 7, 85, 5, 0 },
+			 { 0, 12, 97, 7, 19, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 34, 7, 89, 5, 0 },
+			 { 0, 10, 97, 7, 21, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 31, 7, 91, 6, 0 },
+			 { 0, 9, 96, 7, 23, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 29, 7, 92, 7, 0 },
+			 { 0, 8, 94, 7, 26, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 26, 7, 94, 8, 0 },
+			 { 0, 7, 92, 7, 29, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 23, 7, 96, 9, 0 },
+			 { 0, 6, 91, 7, 31, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 21, 7, 97, 10, 0 },
+			 { 0, 5, 89, 7, 34, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 19, 7, 97, 12, 0 },
+			 { 0, 5, 85, 7, 38, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 0, 17, 7, 98, 13, 0 },
+			 { 0, 4, 83, 7, 41, 0, 0 },
+			 { 0, 1, 44, 7, 80, 3, 0 } },
+		.odd = { { 0, 1, 43, 7, 80, 4, 0 },
+			 { 0, 14, 98, 7, 16, 0, 0 },
+			 { 0, 3, 78, 7, 46, 1, 0 },
+			 { 0, 0, 39, 7, 85, 4, 0 },
+			 { 0, 12, 98, 7, 18, 0, 0 },
+			 { 0, 3, 74, 7, 50, 1, 0 },
+			 { 0, 0, 36, 7, 87, 5, 0 },
+			 { 0, 11, 97, 7, 20, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 33, 7, 89, 6, 0 },
+			 { 0, 10, 96, 7, 22, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 30, 7, 92, 6, 0 },
+			 { 0, 9, 94, 7, 25, 0, 0 },
+			 { 0, 2, 64, 7, 61, 1, 0 },
+			 { 0, 0, 27, 7, 94, 7, 0 },
+			 { 0, 7, 94, 7, 27, 0, 0 },
+			 { 0, 1, 61, 7, 64, 2, 0 },
+			 { 0, 0, 25, 7, 94, 9, 0 },
+			 { 0, 6, 92, 7, 30, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 22, 7, 96, 10, 0 },
+			 { 0, 6, 89, 7, 33, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 20, 7, 97, 11, 0 },
+			 { 0, 5, 87, 7, 36, 0, 0 },
+			 { 0, 1, 50, 7, 74, 3, 0 },
+			 { 0, 0, 18, 7, 98, 12, 0 },
+			 { 0, 4, 85, 7, 39, 0, 0 },
+			 { 0, 1, 46, 7, 78, 3, 0 },
+			 { 0, 0, 16, 7, 98, 14, 0 },
+			 { 0, 4, 80, 7, 43, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 15, 98, 7, 15, 0, 0 },
+			 { 0, 3, 80, 7, 44, 1, 0 },
+			 { 0, 0, 41, 7, 83, 4, 0 },
+			 { 0, 13, 98, 7, 17, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 38, 7, 85, 5, 0 },
+			 { 0, 12, 97, 7, 19, 0, 0 },
+			 { 0, 2, 74, 7, 51, 1, 0 },
+			 { 0, 0, 34, 7, 89, 5, 0 },
+			 { 0, 10, 97, 7, 21, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 31, 7, 91, 6, 0 },
+			 { 0, 9, 96, 7, 23, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 29, 7, 92, 7, 0 },
+			 { 0, 8, 94, 7, 26, 0, 0 },
+			 { 0, 1, 63, 7, 63, 1, 0 },
+			 { 0, 0, 26, 7, 94, 8, 0 },
+			 { 0, 7, 92, 7, 29, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 23, 7, 96, 9, 0 },
+			 { 0, 6, 91, 7, 31, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 21, 7, 97, 10, 0 },
+			 { 0, 5, 89, 7, 34, 0, 0 },
+			 { 0, 1, 51, 7, 74, 2, 0 },
+			 { 0, 0, 19, 7, 97, 12, 0 },
+			 { 0, 5, 85, 7, 38, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 0, 17, 7, 98, 13, 0 },
+			 { 0, 4, 83, 7, 41, 0, 0 },
+			 { 0, 1, 44, 7, 80, 3, 0 } },
+		.odd = { { 0, 1, 43, 7, 80, 4, 0 },
+			 { 0, 14, 98, 7, 16, 0, 0 },
+			 { 0, 3, 78, 7, 46, 1, 0 },
+			 { 0, 0, 39, 7, 85, 4, 0 },
+			 { 0, 12, 98, 7, 18, 0, 0 },
+			 { 0, 3, 74, 7, 50, 1, 0 },
+			 { 0, 0, 36, 7, 87, 5, 0 },
+			 { 0, 11, 97, 7, 20, 0, 0 },
+			 { 0, 2, 72, 7, 53, 1, 0 },
+			 { 0, 0, 33, 7, 89, 6, 0 },
+			 { 0, 10, 96, 7, 22, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 30, 7, 92, 6, 0 },
+			 { 0, 9, 94, 7, 25, 0, 0 },
+			 { 0, 2, 64, 7, 61, 1, 0 },
+			 { 0, 0, 27, 7, 94, 7, 0 },
+			 { 0, 7, 94, 7, 27, 0, 0 },
+			 { 0, 1, 61, 7, 64, 2, 0 },
+			 { 0, 0, 25, 7, 94, 9, 0 },
+			 { 0, 6, 92, 7, 30, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 22, 7, 96, 10, 0 },
+			 { 0, 6, 89, 7, 33, 0, 0 },
+			 { 0, 1, 53, 7, 72, 2, 0 },
+			 { 0, 0, 20, 7, 97, 11, 0 },
+			 { 0, 5, 87, 7, 36, 0, 0 },
+			 { 0, 1, 50, 7, 74, 3, 0 },
+			 { 0, 0, 18, 7, 98, 12, 0 },
+			 { 0, 4, 85, 7, 39, 0, 0 },
+			 { 0, 1, 46, 7, 78, 3, 0 },
+			 { 0, 0, 16, 7, 98, 14, 0 },
+			 { 0, 4, 80, 7, 43, 1, 0 } } },
+	.ptrn_arr = { { 0x3264c993, 0x93264c99, 0x993264c9, 0xc993264c,
+			 0x93264 } },
+	.sample_patrn_length = 150,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 44) = 0.421053 */
+	.hor_phase_arr = {
+		.even = { { 0, 16, 96, 7, 16, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 35, 7, 87, 6, 0 },
+			 { 0, 10, 94, 7, 24, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 24, 7, 94, 10, 0 },
+			 { 0, 6, 87, 7, 35, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 } },
+		.odd = { { 0, 1, 41, 7, 82, 4, 0 },
+			 { 0, 12, 97, 7, 19, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 29, 7, 92, 7, 0 },
+			 { 0, 7, 92, 7, 29, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 19, 7, 97, 12, 0 },
+			 { 0, 4, 82, 7, 41, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 16, 96, 7, 16, 0, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 35, 7, 87, 6, 0 },
+			 { 0, 10, 94, 7, 24, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 24, 7, 94, 10, 0 },
+			 { 0, 6, 87, 7, 35, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 } },
+		.odd = { { 0, 1, 41, 7, 82, 4, 0 },
+			 { 0, 12, 97, 7, 19, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 29, 7, 92, 7, 0 },
+			 { 0, 7, 92, 7, 29, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 19, 7, 97, 12, 0 },
+			 { 0, 4, 82, 7, 41, 1, 0 } } },
+	.ptrn_arr = { { 0x3264c993, 0x9 } },
+	.sample_patrn_length = 38,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 45) = 0.415584 */
+	.hor_phase_arr = {
+		.even = { { 0, 16, 96, 7, 16, 0, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 30, 7, 90, 8, 0 },
+			 { 0, 7, 89, 7, 32, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 14, 96, 7, 18, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 27, 7, 92, 9, 0 },
+			 { 0, 6, 87, 7, 35, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 13, 95, 7, 20, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 24, 7, 94, 10, 0 },
+			 { 0, 5, 85, 7, 38, 0, 0 },
+			 { 0, 1, 42, 7, 81, 4, 0 },
+			 { 0, 11, 95, 7, 22, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 22, 7, 95, 11, 0 },
+			 { 0, 4, 81, 7, 42, 1, 0 },
+			 { 0, 0, 38, 7, 85, 5, 0 },
+			 { 0, 10, 94, 7, 24, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 20, 7, 95, 13, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 35, 7, 87, 6, 0 },
+			 { 0, 9, 92, 7, 27, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 18, 7, 96, 14, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 32, 7, 89, 7, 0 },
+			 { 0, 8, 90, 7, 30, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 } },
+		.odd = { { 0, 1, 40, 7, 82, 5, 0 },
+			 { 0, 11, 94, 7, 23, 0, 0 },
+			 { 0, 2, 61, 7, 63, 2, 0 },
+			 { 0, 0, 21, 7, 95, 12, 0 },
+			 { 0, 4, 80, 7, 43, 1, 0 },
+			 { 0, 0, 37, 7, 85, 6, 0 },
+			 { 0, 9, 93, 7, 26, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 19, 7, 95, 14, 0 },
+			 { 0, 4, 76, 7, 47, 1, 0 },
+			 { 0, 0, 34, 7, 88, 6, 0 },
+			 { 0, 8, 92, 7, 28, 0, 0 },
+			 { 0, 1, 54, 7, 70, 3, 0 },
+			 { 0, 0, 17, 7, 96, 15, 0 },
+			 { 0, 3, 74, 7, 50, 1, 0 },
+			 { 0, 0, 31, 7, 90, 7, 0 },
+			 { 0, 7, 90, 7, 31, 0, 0 },
+			 { 0, 1, 50, 7, 74, 3, 0 },
+			 { 0, 15, 96, 7, 17, 0, 0 },
+			 { 0, 3, 70, 7, 54, 1, 0 },
+			 { 0, 0, 28, 7, 92, 8, 0 },
+			 { 0, 6, 88, 7, 34, 0, 0 },
+			 { 0, 1, 47, 7, 76, 4, 0 },
+			 { 0, 14, 95, 7, 19, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 26, 7, 93, 9, 0 },
+			 { 0, 6, 85, 7, 37, 0, 0 },
+			 { 0, 1, 43, 7, 80, 4, 0 },
+			 { 0, 12, 95, 7, 21, 0, 0 },
+			 { 0, 2, 63, 7, 61, 2, 0 },
+			 { 0, 0, 23, 7, 94, 11, 0 },
+			 { 0, 5, 82, 7, 40, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 16, 96, 7, 16, 0, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 30, 7, 90, 8, 0 },
+			 { 0, 7, 89, 7, 32, 0, 0 },
+			 { 0, 1, 48, 7, 76, 3, 0 },
+			 { 0, 14, 96, 7, 18, 0, 0 },
+			 { 0, 2, 70, 7, 55, 1, 0 },
+			 { 0, 0, 27, 7, 92, 9, 0 },
+			 { 0, 6, 87, 7, 35, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 13, 95, 7, 20, 0, 0 },
+			 { 0, 2, 66, 7, 59, 1, 0 },
+			 { 0, 0, 24, 7, 94, 10, 0 },
+			 { 0, 5, 85, 7, 38, 0, 0 },
+			 { 0, 1, 42, 7, 81, 4, 0 },
+			 { 0, 11, 95, 7, 22, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 22, 7, 95, 11, 0 },
+			 { 0, 4, 81, 7, 42, 1, 0 },
+			 { 0, 0, 38, 7, 85, 5, 0 },
+			 { 0, 10, 94, 7, 24, 0, 0 },
+			 { 0, 1, 59, 7, 66, 2, 0 },
+			 { 0, 0, 20, 7, 95, 13, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 35, 7, 87, 6, 0 },
+			 { 0, 9, 92, 7, 27, 0, 0 },
+			 { 0, 1, 55, 7, 70, 2, 0 },
+			 { 0, 0, 18, 7, 96, 14, 0 },
+			 { 0, 3, 76, 7, 48, 1, 0 },
+			 { 0, 0, 32, 7, 89, 7, 0 },
+			 { 0, 8, 90, 7, 30, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 } },
+		.odd = { { 0, 1, 40, 7, 82, 5, 0 },
+			 { 0, 11, 94, 7, 23, 0, 0 },
+			 { 0, 2, 61, 7, 63, 2, 0 },
+			 { 0, 0, 21, 7, 95, 12, 0 },
+			 { 0, 4, 80, 7, 43, 1, 0 },
+			 { 0, 0, 37, 7, 85, 6, 0 },
+			 { 0, 9, 93, 7, 26, 0, 0 },
+			 { 0, 1, 57, 7, 68, 2, 0 },
+			 { 0, 0, 19, 7, 95, 14, 0 },
+			 { 0, 4, 76, 7, 47, 1, 0 },
+			 { 0, 0, 34, 7, 88, 6, 0 },
+			 { 0, 8, 92, 7, 28, 0, 0 },
+			 { 0, 1, 54, 7, 70, 3, 0 },
+			 { 0, 0, 17, 7, 96, 15, 0 },
+			 { 0, 3, 74, 7, 50, 1, 0 },
+			 { 0, 0, 31, 7, 90, 7, 0 },
+			 { 0, 7, 90, 7, 31, 0, 0 },
+			 { 0, 1, 50, 7, 74, 3, 0 },
+			 { 0, 15, 96, 7, 17, 0, 0 },
+			 { 0, 3, 70, 7, 54, 1, 0 },
+			 { 0, 0, 28, 7, 92, 8, 0 },
+			 { 0, 6, 88, 7, 34, 0, 0 },
+			 { 0, 1, 47, 7, 76, 4, 0 },
+			 { 0, 14, 95, 7, 19, 0, 0 },
+			 { 0, 2, 68, 7, 57, 1, 0 },
+			 { 0, 0, 26, 7, 93, 9, 0 },
+			 { 0, 6, 85, 7, 37, 0, 0 },
+			 { 0, 1, 43, 7, 80, 4, 0 },
+			 { 0, 12, 95, 7, 21, 0, 0 },
+			 { 0, 2, 63, 7, 61, 2, 0 },
+			 { 0, 0, 23, 7, 94, 11, 0 },
+			 { 0, 5, 82, 7, 40, 1, 0 } } },
+	.ptrn_arr = { { 0x9324c993, 0xc99324c9, 0x26499324, 0x93264993,
+			 0x932649 } },
+	.sample_patrn_length = 154,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 46) = 0.410256 */
+	.hor_phase_arr = {
+		.even = { { 0, 17, 94, 7, 17, 0, 0 },
+			 { 0, 3, 69, 7, 55, 1, 0 },
+			 { 0, 0, 25, 7, 93, 10, 0 },
+			 { 0, 5, 80, 7, 42, 1, 0 },
+			 { 0, 0, 36, 7, 86, 6, 0 },
+			 { 0, 8, 90, 7, 30, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 13, 94, 7, 21, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 21, 7, 94, 13, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 30, 7, 90, 8, 0 },
+			 { 0, 6, 86, 7, 36, 0, 0 },
+			 { 0, 1, 42, 7, 80, 5, 0 },
+			 { 0, 10, 93, 7, 25, 0, 0 },
+			 { 0, 1, 55, 7, 69, 3, 0 } },
+		.odd = { { 0, 1, 39, 7, 83, 5, 0 },
+			 { 0, 9, 91, 7, 28, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 },
+			 { 0, 15, 94, 7, 19, 0, 0 },
+			 { 0, 2, 65, 7, 59, 2, 0 },
+			 { 0, 0, 23, 7, 93, 12, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 33, 7, 88, 7, 0 },
+			 { 0, 7, 88, 7, 33, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 12, 93, 7, 23, 0, 0 },
+			 { 0, 2, 59, 7, 65, 2, 0 },
+			 { 0, 0, 19, 7, 94, 15, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 28, 7, 91, 9, 0 },
+			 { 0, 5, 83, 7, 39, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 17, 94, 7, 17, 0, 0 },
+			 { 0, 3, 69, 7, 55, 1, 0 },
+			 { 0, 0, 25, 7, 93, 10, 0 },
+			 { 0, 5, 80, 7, 42, 1, 0 },
+			 { 0, 0, 36, 7, 86, 6, 0 },
+			 { 0, 8, 90, 7, 30, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 13, 94, 7, 21, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 21, 7, 94, 13, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 30, 7, 90, 8, 0 },
+			 { 0, 6, 86, 7, 36, 0, 0 },
+			 { 0, 1, 42, 7, 80, 5, 0 },
+			 { 0, 10, 93, 7, 25, 0, 0 },
+			 { 0, 1, 55, 7, 69, 3, 0 } },
+		.odd = { { 0, 1, 39, 7, 83, 5, 0 },
+			 { 0, 9, 91, 7, 28, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 },
+			 { 0, 15, 94, 7, 19, 0, 0 },
+			 { 0, 2, 65, 7, 59, 2, 0 },
+			 { 0, 0, 23, 7, 93, 12, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 33, 7, 88, 7, 0 },
+			 { 0, 7, 88, 7, 33, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 12, 93, 7, 23, 0, 0 },
+			 { 0, 2, 59, 7, 65, 2, 0 },
+			 { 0, 0, 19, 7, 94, 15, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 28, 7, 91, 9, 0 },
+			 { 0, 5, 83, 7, 39, 1, 0 } } },
+	.ptrn_arr = { { 0x93264993, 0x4c99264c, 0x932 } },
+	.sample_patrn_length = 78,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 47) = 0.405063 */
+	.hor_phase_arr = {
+		.even = { { 0, 17, 94, 7, 17, 0, 0 },
+			 { 0, 2, 65, 7, 59, 2, 0 },
+			 { 0, 0, 21, 7, 93, 14, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 26, 7, 91, 11, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 31, 7, 88, 9, 0 },
+			 { 0, 6, 82, 7, 39, 1, 0 },
+			 { 0, 1, 36, 7, 84, 7, 0 },
+			 { 0, 8, 87, 7, 33, 0, 0 },
+			 { 0, 1, 42, 7, 80, 5, 0 },
+			 { 0, 10, 90, 7, 28, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 12, 93, 7, 23, 0, 0 },
+			 { 0, 2, 55, 7, 68, 3, 0 },
+			 { 0, 15, 94, 7, 19, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 19, 7, 94, 15, 0 },
+			 { 0, 3, 68, 7, 55, 2, 0 },
+			 { 0, 0, 23, 7, 93, 12, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 28, 7, 90, 10, 0 },
+			 { 0, 5, 80, 7, 42, 1, 0 },
+			 { 0, 0, 33, 7, 87, 8, 0 },
+			 { 0, 7, 84, 7, 36, 1, 0 },
+			 { 0, 1, 39, 7, 82, 6, 0 },
+			 { 0, 9, 88, 7, 31, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 11, 91, 7, 26, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 },
+			 { 0, 14, 93, 7, 21, 0, 0 },
+			 { 0, 2, 59, 7, 65, 2, 0 } },
+		.odd = { { 0, 1, 38, 7, 83, 6, 0 },
+			 { 0, 8, 88, 7, 32, 0, 0 },
+			 { 0, 1, 44, 7, 78, 5, 0 },
+			 { 0, 10, 91, 7, 27, 0, 0 },
+			 { 0, 1, 50, 7, 73, 4, 0 },
+			 { 0, 13, 93, 7, 22, 0, 0 },
+			 { 0, 2, 57, 7, 66, 3, 0 },
+			 { 0, 16, 94, 7, 18, 0, 0 },
+			 { 0, 2, 64, 7, 60, 2, 0 },
+			 { 0, 0, 20, 7, 93, 15, 0 },
+			 { 0, 3, 70, 7, 54, 1, 0 },
+			 { 0, 0, 24, 7, 92, 12, 0 },
+			 { 0, 4, 76, 7, 47, 1, 0 },
+			 { 0, 0, 29, 7, 90, 9, 0 },
+			 { 0, 5, 81, 7, 41, 1, 0 },
+			 { 0, 0, 35, 7, 86, 7, 0 },
+			 { 0, 7, 86, 7, 35, 0, 0 },
+			 { 0, 1, 41, 7, 81, 5, 0 },
+			 { 0, 9, 90, 7, 29, 0, 0 },
+			 { 0, 1, 47, 7, 76, 4, 0 },
+			 { 0, 12, 92, 7, 24, 0, 0 },
+			 { 0, 1, 54, 7, 70, 3, 0 },
+			 { 0, 15, 93, 7, 20, 0, 0 },
+			 { 0, 2, 60, 7, 64, 2, 0 },
+			 { 0, 0, 18, 7, 94, 16, 0 },
+			 { 0, 3, 66, 7, 57, 2, 0 },
+			 { 0, 0, 22, 7, 93, 13, 0 },
+			 { 0, 4, 73, 7, 50, 1, 0 },
+			 { 0, 0, 27, 7, 91, 10, 0 },
+			 { 0, 5, 78, 7, 44, 1, 0 },
+			 { 0, 0, 32, 7, 88, 8, 0 },
+			 { 0, 6, 83, 7, 38, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 17, 94, 7, 17, 0, 0 },
+			 { 0, 2, 65, 7, 59, 2, 0 },
+			 { 0, 0, 21, 7, 93, 14, 0 },
+			 { 0, 3, 72, 7, 52, 1, 0 },
+			 { 0, 0, 26, 7, 91, 11, 0 },
+			 { 0, 4, 78, 7, 45, 1, 0 },
+			 { 0, 0, 31, 7, 88, 9, 0 },
+			 { 0, 6, 82, 7, 39, 1, 0 },
+			 { 0, 1, 36, 7, 84, 7, 0 },
+			 { 0, 8, 87, 7, 33, 0, 0 },
+			 { 0, 1, 42, 7, 80, 5, 0 },
+			 { 0, 10, 90, 7, 28, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 12, 93, 7, 23, 0, 0 },
+			 { 0, 2, 55, 7, 68, 3, 0 },
+			 { 0, 15, 94, 7, 19, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 0, 19, 7, 94, 15, 0 },
+			 { 0, 3, 68, 7, 55, 2, 0 },
+			 { 0, 0, 23, 7, 93, 12, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 28, 7, 90, 10, 0 },
+			 { 0, 5, 80, 7, 42, 1, 0 },
+			 { 0, 0, 33, 7, 87, 8, 0 },
+			 { 0, 7, 84, 7, 36, 1, 0 },
+			 { 0, 1, 39, 7, 82, 6, 0 },
+			 { 0, 9, 88, 7, 31, 0, 0 },
+			 { 0, 1, 45, 7, 78, 4, 0 },
+			 { 0, 11, 91, 7, 26, 0, 0 },
+			 { 0, 1, 52, 7, 72, 3, 0 },
+			 { 0, 14, 93, 7, 21, 0, 0 },
+			 { 0, 2, 59, 7, 65, 2, 0 } },
+		.odd = { { 0, 1, 38, 7, 83, 6, 0 },
+			 { 0, 8, 88, 7, 32, 0, 0 },
+			 { 0, 1, 44, 7, 78, 5, 0 },
+			 { 0, 10, 91, 7, 27, 0, 0 },
+			 { 0, 1, 50, 7, 73, 4, 0 },
+			 { 0, 13, 93, 7, 22, 0, 0 },
+			 { 0, 2, 57, 7, 66, 3, 0 },
+			 { 0, 16, 94, 7, 18, 0, 0 },
+			 { 0, 2, 64, 7, 60, 2, 0 },
+			 { 0, 0, 20, 7, 93, 15, 0 },
+			 { 0, 3, 70, 7, 54, 1, 0 },
+			 { 0, 0, 24, 7, 92, 12, 0 },
+			 { 0, 4, 76, 7, 47, 1, 0 },
+			 { 0, 0, 29, 7, 90, 9, 0 },
+			 { 0, 5, 81, 7, 41, 1, 0 },
+			 { 0, 0, 35, 7, 86, 7, 0 },
+			 { 0, 7, 86, 7, 35, 0, 0 },
+			 { 0, 1, 41, 7, 81, 5, 0 },
+			 { 0, 9, 90, 7, 29, 0, 0 },
+			 { 0, 1, 47, 7, 76, 4, 0 },
+			 { 0, 12, 92, 7, 24, 0, 0 },
+			 { 0, 1, 54, 7, 70, 3, 0 },
+			 { 0, 15, 93, 7, 20, 0, 0 },
+			 { 0, 2, 60, 7, 64, 2, 0 },
+			 { 0, 0, 18, 7, 94, 16, 0 },
+			 { 0, 3, 66, 7, 57, 2, 0 },
+			 { 0, 0, 22, 7, 93, 13, 0 },
+			 { 0, 4, 73, 7, 50, 1, 0 },
+			 { 0, 0, 27, 7, 91, 10, 0 },
+			 { 0, 5, 78, 7, 44, 1, 0 },
+			 { 0, 0, 32, 7, 88, 8, 0 },
+			 { 0, 6, 83, 7, 38, 1, 0 } } },
+	.ptrn_arr = { { 0x99264993, 0x24c93264, 0x99264c93, 0x24c99264,
+			 0x9324c93 } },
+	.sample_patrn_length = 158,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 48) = 0.4 */
+	.hor_phase_arr = {
+		.even = { { 0, 18, 92, 7, 18, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 } },
+		.odd = { { 0, 1, 37, 7, 83, 7, 0 },
+			 { 0, 7, 83, 7, 37, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 18, 92, 7, 18, 0, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 } },
+		.odd = { { 0, 1, 37, 7, 83, 7, 0 },
+			 { 0, 7, 83, 7, 37, 1, 0 } } },
+	.ptrn_arr = { { 0x93 } },
+	.sample_patrn_length = 10,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 49) = 0.395062 */
+	.hor_phase_arr = {
+		.even = { { 0, 18, 92, 7, 18, 0, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 15, 91, 7, 22, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 12, 89, 7, 27, 0, 0 },
+			 { 0, 1, 46, 7, 76, 5, 0 },
+			 { 0, 9, 87, 7, 32, 0, 0 },
+			 { 0, 1, 40, 7, 80, 7, 0 },
+			 { 0, 7, 83, 7, 37, 1, 0 },
+			 { 0, 1, 34, 7, 85, 8, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 29, 7, 88, 11, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 24, 7, 91, 13, 0 },
+			 { 0, 3, 68, 7, 55, 2, 0 },
+			 { 0, 0, 20, 7, 92, 16, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 16, 92, 7, 20, 0, 0 },
+			 { 0, 2, 55, 7, 68, 3, 0 },
+			 { 0, 13, 91, 7, 24, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 11, 88, 7, 29, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 8, 85, 7, 34, 1, 0 },
+			 { 0, 1, 37, 7, 83, 7, 0 },
+			 { 0, 7, 80, 7, 40, 1, 0 },
+			 { 0, 0, 32, 7, 87, 9, 0 },
+			 { 0, 5, 76, 7, 46, 1, 0 },
+			 { 0, 0, 27, 7, 89, 12, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 },
+			 { 0, 0, 22, 7, 91, 15, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 } },
+		.odd = { { 0, 1, 36, 7, 83, 8, 0 },
+			 { 0, 6, 80, 7, 41, 1, 0 },
+			 { 0, 0, 30, 7, 88, 10, 0 },
+			 { 0, 5, 75, 7, 47, 1, 0 },
+			 { 0, 0, 25, 7, 90, 13, 0 },
+			 { 0, 4, 68, 7, 54, 2, 0 },
+			 { 0, 0, 21, 7, 91, 16, 0 },
+			 { 0, 3, 63, 7, 60, 2, 0 },
+			 { 0, 17, 92, 7, 19, 0, 0 },
+			 { 0, 2, 57, 7, 66, 3, 0 },
+			 { 0, 14, 91, 7, 23, 0, 0 },
+			 { 0, 1, 51, 7, 72, 4, 0 },
+			 { 0, 11, 89, 7, 28, 0, 0 },
+			 { 0, 1, 44, 7, 78, 5, 0 },
+			 { 0, 9, 85, 7, 33, 1, 0 },
+			 { 0, 1, 38, 7, 82, 7, 0 },
+			 { 0, 7, 82, 7, 38, 1, 0 },
+			 { 0, 1, 33, 7, 85, 9, 0 },
+			 { 0, 5, 78, 7, 44, 1, 0 },
+			 { 0, 0, 28, 7, 89, 11, 0 },
+			 { 0, 4, 72, 7, 51, 1, 0 },
+			 { 0, 0, 23, 7, 91, 14, 0 },
+			 { 0, 3, 66, 7, 57, 2, 0 },
+			 { 0, 0, 19, 7, 92, 17, 0 },
+			 { 0, 2, 60, 7, 63, 3, 0 },
+			 { 0, 16, 91, 7, 21, 0, 0 },
+			 { 0, 2, 54, 7, 68, 4, 0 },
+			 { 0, 13, 90, 7, 25, 0, 0 },
+			 { 0, 1, 47, 7, 75, 5, 0 },
+			 { 0, 10, 88, 7, 30, 0, 0 },
+			 { 0, 1, 41, 7, 80, 6, 0 },
+			 { 0, 8, 83, 7, 36, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 18, 92, 7, 18, 0, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 15, 91, 7, 22, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 12, 89, 7, 27, 0, 0 },
+			 { 0, 1, 46, 7, 76, 5, 0 },
+			 { 0, 9, 87, 7, 32, 0, 0 },
+			 { 0, 1, 40, 7, 80, 7, 0 },
+			 { 0, 7, 83, 7, 37, 1, 0 },
+			 { 0, 1, 34, 7, 85, 8, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 29, 7, 88, 11, 0 },
+			 { 0, 4, 74, 7, 49, 1, 0 },
+			 { 0, 0, 24, 7, 91, 13, 0 },
+			 { 0, 3, 68, 7, 55, 2, 0 },
+			 { 0, 0, 20, 7, 92, 16, 0 },
+			 { 0, 2, 62, 7, 62, 2, 0 },
+			 { 0, 16, 92, 7, 20, 0, 0 },
+			 { 0, 2, 55, 7, 68, 3, 0 },
+			 { 0, 13, 91, 7, 24, 0, 0 },
+			 { 0, 1, 49, 7, 74, 4, 0 },
+			 { 0, 11, 88, 7, 29, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 8, 85, 7, 34, 1, 0 },
+			 { 0, 1, 37, 7, 83, 7, 0 },
+			 { 0, 7, 80, 7, 40, 1, 0 },
+			 { 0, 0, 32, 7, 87, 9, 0 },
+			 { 0, 5, 76, 7, 46, 1, 0 },
+			 { 0, 0, 27, 7, 89, 12, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 },
+			 { 0, 0, 22, 7, 91, 15, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 } },
+		.odd = { { 0, 1, 36, 7, 83, 8, 0 },
+			 { 0, 6, 80, 7, 41, 1, 0 },
+			 { 0, 0, 30, 7, 88, 10, 0 },
+			 { 0, 5, 75, 7, 47, 1, 0 },
+			 { 0, 0, 25, 7, 90, 13, 0 },
+			 { 0, 4, 68, 7, 54, 2, 0 },
+			 { 0, 0, 21, 7, 91, 16, 0 },
+			 { 0, 3, 63, 7, 60, 2, 0 },
+			 { 0, 17, 92, 7, 19, 0, 0 },
+			 { 0, 2, 57, 7, 66, 3, 0 },
+			 { 0, 14, 91, 7, 23, 0, 0 },
+			 { 0, 1, 51, 7, 72, 4, 0 },
+			 { 0, 11, 89, 7, 28, 0, 0 },
+			 { 0, 1, 44, 7, 78, 5, 0 },
+			 { 0, 9, 85, 7, 33, 1, 0 },
+			 { 0, 1, 38, 7, 82, 7, 0 },
+			 { 0, 7, 82, 7, 38, 1, 0 },
+			 { 0, 1, 33, 7, 85, 9, 0 },
+			 { 0, 5, 78, 7, 44, 1, 0 },
+			 { 0, 0, 28, 7, 89, 11, 0 },
+			 { 0, 4, 72, 7, 51, 1, 0 },
+			 { 0, 0, 23, 7, 91, 14, 0 },
+			 { 0, 3, 66, 7, 57, 2, 0 },
+			 { 0, 0, 19, 7, 92, 17, 0 },
+			 { 0, 2, 60, 7, 63, 3, 0 },
+			 { 0, 16, 91, 7, 21, 0, 0 },
+			 { 0, 2, 54, 7, 68, 4, 0 },
+			 { 0, 13, 90, 7, 25, 0, 0 },
+			 { 0, 1, 47, 7, 75, 5, 0 },
+			 { 0, 10, 88, 7, 30, 0, 0 },
+			 { 0, 1, 41, 7, 80, 6, 0 },
+			 { 0, 8, 83, 7, 36, 1, 0 } } },
+	.ptrn_arr = { { 0xc9324c93, 0x92649924, 0x24c92649, 0x49324c93,
+			 0x92649926 } },
+	.sample_patrn_length = 162,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 50) = 0.390244 */
+	.hor_phase_arr = {
+		.even = { { 0, 19, 90, 7, 19, 0, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 12, 89, 7, 27, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 8, 82, 7, 37, 1, 0 },
+			 { 0, 1, 32, 7, 85, 10, 0 },
+			 { 0, 5, 73, 7, 49, 1, 0 },
+			 { 0, 0, 23, 7, 90, 15, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 15, 90, 7, 23, 0, 0 },
+			 { 0, 1, 49, 7, 73, 5, 0 },
+			 { 0, 10, 85, 7, 32, 1, 0 },
+			 { 0, 1, 37, 7, 82, 8, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 27, 7, 89, 12, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 } },
+		.odd = { { 0, 1, 35, 7, 83, 9, 0 },
+			 { 0, 5, 76, 7, 46, 1, 0 },
+			 { 0, 0, 25, 7, 89, 14, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 },
+			 { 0, 17, 90, 7, 21, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 11, 88, 7, 29, 0, 0 },
+			 { 0, 1, 40, 7, 80, 7, 0 },
+			 { 0, 7, 80, 7, 40, 1, 0 },
+			 { 0, 0, 29, 7, 88, 11, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 },
+			 { 0, 0, 21, 7, 90, 17, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 14, 89, 7, 25, 0, 0 },
+			 { 0, 1, 46, 7, 76, 5, 0 },
+			 { 0, 9, 83, 7, 35, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 19, 90, 7, 19, 0, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 12, 89, 7, 27, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 8, 82, 7, 37, 1, 0 },
+			 { 0, 1, 32, 7, 85, 10, 0 },
+			 { 0, 5, 73, 7, 49, 1, 0 },
+			 { 0, 0, 23, 7, 90, 15, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 15, 90, 7, 23, 0, 0 },
+			 { 0, 1, 49, 7, 73, 5, 0 },
+			 { 0, 10, 85, 7, 32, 1, 0 },
+			 { 0, 1, 37, 7, 82, 8, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 27, 7, 89, 12, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 } },
+		.odd = { { 0, 1, 35, 7, 83, 9, 0 },
+			 { 0, 5, 76, 7, 46, 1, 0 },
+			 { 0, 0, 25, 7, 89, 14, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 },
+			 { 0, 17, 90, 7, 21, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 11, 88, 7, 29, 0, 0 },
+			 { 0, 1, 40, 7, 80, 7, 0 },
+			 { 0, 7, 80, 7, 40, 1, 0 },
+			 { 0, 0, 29, 7, 88, 11, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 },
+			 { 0, 0, 21, 7, 90, 17, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 14, 89, 7, 25, 0, 0 },
+			 { 0, 1, 46, 7, 76, 5, 0 },
+			 { 0, 9, 83, 7, 35, 1, 0 } } },
+	.ptrn_arr = { { 0x49924c93, 0x9324c926, 0x9264 } },
+	.sample_patrn_length = 82,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 51) = 0.385542 */
+	.hor_phase_arr = {
+		.even = { { 0, 19, 90, 7, 19, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 10, 85, 7, 32, 1, 0 },
+			 { 0, 1, 35, 7, 83, 9, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 },
+			 { 0, 0, 21, 7, 90, 17, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 11, 87, 7, 30, 0, 0 },
+			 { 0, 1, 38, 7, 81, 8, 0 },
+			 { 0, 6, 75, 7, 46, 1, 0 },
+			 { 0, 0, 23, 7, 89, 16, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 13, 87, 7, 28, 0, 0 },
+			 { 0, 1, 41, 7, 79, 7, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 25, 7, 89, 14, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 14, 89, 7, 25, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 7, 79, 7, 41, 1, 0 },
+			 { 0, 0, 28, 7, 87, 13, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 },
+			 { 0, 16, 89, 7, 23, 0, 0 },
+			 { 0, 1, 46, 7, 75, 6, 0 },
+			 { 0, 8, 81, 7, 38, 1, 0 },
+			 { 0, 0, 30, 7, 87, 11, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 17, 90, 7, 21, 0, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 9, 83, 7, 35, 1, 0 },
+			 { 0, 1, 32, 7, 85, 10, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 } },
+		.odd = { { 0, 1, 34, 7, 83, 10, 0 },
+			 { 0, 5, 70, 7, 51, 2, 0 },
+			 { 0, 0, 20, 7, 90, 18, 0 },
+			 { 0, 2, 54, 7, 68, 4, 0 },
+			 { 0, 11, 85, 7, 31, 1, 0 },
+			 { 0, 1, 36, 7, 82, 9, 0 },
+			 { 0, 5, 74, 7, 48, 1, 0 },
+			 { 0, 0, 22, 7, 89, 17, 0 },
+			 { 0, 2, 57, 7, 65, 4, 0 },
+			 { 0, 12, 87, 7, 29, 0, 0 },
+			 { 0, 1, 39, 7, 80, 8, 0 },
+			 { 0, 6, 76, 7, 45, 1, 0 },
+			 { 0, 0, 24, 7, 89, 15, 0 },
+			 { 0, 3, 60, 7, 62, 3, 0 },
+			 { 0, 13, 89, 7, 26, 0, 0 },
+			 { 0, 1, 42, 7, 78, 7, 0 },
+			 { 0, 7, 78, 7, 42, 1, 0 },
+			 { 0, 0, 26, 7, 89, 13, 0 },
+			 { 0, 3, 62, 7, 60, 3, 0 },
+			 { 0, 15, 89, 7, 24, 0, 0 },
+			 { 0, 1, 45, 7, 76, 6, 0 },
+			 { 0, 8, 80, 7, 39, 1, 0 },
+			 { 0, 0, 29, 7, 87, 12, 0 },
+			 { 0, 4, 65, 7, 57, 2, 0 },
+			 { 0, 17, 89, 7, 22, 0, 0 },
+			 { 0, 1, 48, 7, 74, 5, 0 },
+			 { 0, 9, 82, 7, 36, 1, 0 },
+			 { 0, 1, 31, 7, 85, 11, 0 },
+			 { 0, 4, 68, 7, 54, 2, 0 },
+			 { 0, 18, 90, 7, 20, 0, 0 },
+			 { 0, 2, 51, 7, 70, 5, 0 },
+			 { 0, 10, 83, 7, 34, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 19, 90, 7, 19, 0, 0 },
+			 { 0, 2, 52, 7, 70, 4, 0 },
+			 { 0, 10, 85, 7, 32, 1, 0 },
+			 { 0, 1, 35, 7, 83, 9, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 },
+			 { 0, 0, 21, 7, 90, 17, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 11, 87, 7, 30, 0, 0 },
+			 { 0, 1, 38, 7, 81, 8, 0 },
+			 { 0, 6, 75, 7, 46, 1, 0 },
+			 { 0, 0, 23, 7, 89, 16, 0 },
+			 { 0, 2, 58, 7, 65, 3, 0 },
+			 { 0, 13, 87, 7, 28, 0, 0 },
+			 { 0, 1, 41, 7, 79, 7, 0 },
+			 { 0, 6, 78, 7, 43, 1, 0 },
+			 { 0, 0, 25, 7, 89, 14, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 14, 89, 7, 25, 0, 0 },
+			 { 0, 1, 43, 7, 78, 6, 0 },
+			 { 0, 7, 79, 7, 41, 1, 0 },
+			 { 0, 0, 28, 7, 87, 13, 0 },
+			 { 0, 3, 65, 7, 58, 2, 0 },
+			 { 0, 16, 89, 7, 23, 0, 0 },
+			 { 0, 1, 46, 7, 75, 6, 0 },
+			 { 0, 8, 81, 7, 38, 1, 0 },
+			 { 0, 0, 30, 7, 87, 11, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 17, 90, 7, 21, 0, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 9, 83, 7, 35, 1, 0 },
+			 { 0, 1, 32, 7, 85, 10, 0 },
+			 { 0, 4, 70, 7, 52, 2, 0 } },
+		.odd = { { 0, 1, 34, 7, 83, 10, 0 },
+			 { 0, 5, 70, 7, 51, 2, 0 },
+			 { 0, 0, 20, 7, 90, 18, 0 },
+			 { 0, 2, 54, 7, 68, 4, 0 },
+			 { 0, 11, 85, 7, 31, 1, 0 },
+			 { 0, 1, 36, 7, 82, 9, 0 },
+			 { 0, 5, 74, 7, 48, 1, 0 },
+			 { 0, 0, 22, 7, 89, 17, 0 },
+			 { 0, 2, 57, 7, 65, 4, 0 },
+			 { 0, 12, 87, 7, 29, 0, 0 },
+			 { 0, 1, 39, 7, 80, 8, 0 },
+			 { 0, 6, 76, 7, 45, 1, 0 },
+			 { 0, 0, 24, 7, 89, 15, 0 },
+			 { 0, 3, 60, 7, 62, 3, 0 },
+			 { 0, 13, 89, 7, 26, 0, 0 },
+			 { 0, 1, 42, 7, 78, 7, 0 },
+			 { 0, 7, 78, 7, 42, 1, 0 },
+			 { 0, 0, 26, 7, 89, 13, 0 },
+			 { 0, 3, 62, 7, 60, 3, 0 },
+			 { 0, 15, 89, 7, 24, 0, 0 },
+			 { 0, 1, 45, 7, 76, 6, 0 },
+			 { 0, 8, 80, 7, 39, 1, 0 },
+			 { 0, 0, 29, 7, 87, 12, 0 },
+			 { 0, 4, 65, 7, 57, 2, 0 },
+			 { 0, 17, 89, 7, 22, 0, 0 },
+			 { 0, 1, 48, 7, 74, 5, 0 },
+			 { 0, 9, 82, 7, 36, 1, 0 },
+			 { 0, 1, 31, 7, 85, 11, 0 },
+			 { 0, 4, 68, 7, 54, 2, 0 },
+			 { 0, 18, 90, 7, 20, 0, 0 },
+			 { 0, 2, 51, 7, 70, 5, 0 },
+			 { 0, 10, 83, 7, 34, 1, 0 } } },
+	.ptrn_arr = { { 0x49924c93, 0xc9264932, 0x93249924, 0x924c9264,
+			 0x26493249, 0x9 } },
+	.sample_patrn_length = 166,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 52) = 0.380952 */
+	.hor_phase_arr = {
+		.even = { { 0, 20, 88, 7, 20, 0, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 8, 81, 7, 38, 1, 0 },
+			 { 0, 0, 28, 7, 87, 13, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 13, 87, 7, 28, 0, 0 },
+			 { 0, 1, 38, 7, 81, 8, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 } },
+		.odd = { { 0, 1, 33, 7, 83, 11, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 16, 88, 7, 24, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 },
+			 { 0, 0, 24, 7, 88, 16, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 11, 83, 7, 33, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 20, 88, 7, 20, 0, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 8, 81, 7, 38, 1, 0 },
+			 { 0, 0, 28, 7, 87, 13, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 13, 87, 7, 28, 0, 0 },
+			 { 0, 1, 38, 7, 81, 8, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 } },
+		.odd = { { 0, 1, 33, 7, 83, 11, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 16, 88, 7, 24, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 },
+			 { 0, 0, 24, 7, 88, 16, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 11, 83, 7, 33, 1, 0 } } },
+	.ptrn_arr = { { 0x4c926493, 0x92 } },
+	.sample_patrn_length = 42,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 53) = 0.376471 */
+	.hor_phase_arr = {
+		.even = { { 0, 20, 88, 7, 20, 0, 0 },
+			 { 0, 2, 47, 7, 73, 6, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 },
+			 { 0, 0, 22, 7, 88, 18, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 8, 78, 7, 41, 1, 0 },
+			 { 0, 0, 24, 7, 87, 17, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 9, 80, 7, 38, 1, 0 },
+			 { 0, 0, 26, 7, 87, 15, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 10, 81, 7, 36, 1, 0 },
+			 { 0, 1, 28, 7, 85, 14, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 11, 83, 7, 33, 1, 0 },
+			 { 0, 1, 31, 7, 84, 12, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 12, 84, 7, 31, 1, 0 },
+			 { 0, 1, 33, 7, 83, 11, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 14, 85, 7, 28, 1, 0 },
+			 { 0, 1, 36, 7, 81, 10, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 15, 87, 7, 26, 0, 0 },
+			 { 0, 1, 38, 7, 80, 9, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 17, 87, 7, 24, 0, 0 },
+			 { 0, 1, 41, 7, 78, 8, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 },
+			 { 0, 18, 88, 7, 22, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 6, 73, 7, 47, 2, 0 } },
+		.odd = { { 0, 1, 32, 7, 83, 12, 0 },
+			 { 0, 3, 63, 7, 59, 3, 0 },
+			 { 0, 13, 84, 7, 30, 1, 0 },
+			 { 0, 1, 34, 7, 83, 10, 0 },
+			 { 0, 4, 64, 7, 57, 3, 0 },
+			 { 0, 14, 87, 7, 27, 0, 0 },
+			 { 0, 1, 37, 7, 81, 9, 0 },
+			 { 0, 5, 67, 7, 54, 2, 0 },
+			 { 0, 16, 87, 7, 25, 0, 0 },
+			 { 0, 1, 40, 7, 79, 8, 0 },
+			 { 0, 5, 70, 7, 51, 2, 0 },
+			 { 0, 18, 87, 7, 23, 0, 0 },
+			 { 0, 1, 42, 7, 78, 7, 0 },
+			 { 0, 6, 72, 7, 48, 2, 0 },
+			 { 0, 19, 88, 7, 21, 0, 0 },
+			 { 0, 1, 45, 7, 75, 7, 0 },
+			 { 0, 7, 75, 7, 45, 1, 0 },
+			 { 0, 0, 21, 7, 88, 19, 0 },
+			 { 0, 2, 48, 7, 72, 6, 0 },
+			 { 0, 7, 78, 7, 42, 1, 0 },
+			 { 0, 0, 23, 7, 87, 18, 0 },
+			 { 0, 2, 51, 7, 70, 5, 0 },
+			 { 0, 8, 79, 7, 40, 1, 0 },
+			 { 0, 0, 25, 7, 87, 16, 0 },
+			 { 0, 2, 54, 7, 67, 5, 0 },
+			 { 0, 9, 81, 7, 37, 1, 0 },
+			 { 0, 0, 27, 7, 87, 14, 0 },
+			 { 0, 3, 57, 7, 64, 4, 0 },
+			 { 0, 10, 83, 7, 34, 1, 0 },
+			 { 0, 1, 30, 7, 84, 13, 0 },
+			 { 0, 3, 59, 7, 63, 3, 0 },
+			 { 0, 12, 83, 7, 32, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 20, 88, 7, 20, 0, 0 },
+			 { 0, 2, 47, 7, 73, 6, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 },
+			 { 0, 0, 22, 7, 88, 18, 0 },
+			 { 0, 2, 49, 7, 72, 5, 0 },
+			 { 0, 8, 78, 7, 41, 1, 0 },
+			 { 0, 0, 24, 7, 87, 17, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 9, 80, 7, 38, 1, 0 },
+			 { 0, 0, 26, 7, 87, 15, 0 },
+			 { 0, 2, 55, 7, 67, 4, 0 },
+			 { 0, 10, 81, 7, 36, 1, 0 },
+			 { 0, 1, 28, 7, 85, 14, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 11, 83, 7, 33, 1, 0 },
+			 { 0, 1, 31, 7, 84, 12, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 12, 84, 7, 31, 1, 0 },
+			 { 0, 1, 33, 7, 83, 11, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 14, 85, 7, 28, 1, 0 },
+			 { 0, 1, 36, 7, 81, 10, 0 },
+			 { 0, 4, 67, 7, 55, 2, 0 },
+			 { 0, 15, 87, 7, 26, 0, 0 },
+			 { 0, 1, 38, 7, 80, 9, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 17, 87, 7, 24, 0, 0 },
+			 { 0, 1, 41, 7, 78, 8, 0 },
+			 { 0, 5, 72, 7, 49, 2, 0 },
+			 { 0, 18, 88, 7, 22, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 6, 73, 7, 47, 2, 0 } },
+		.odd = { { 0, 1, 32, 7, 83, 12, 0 },
+			 { 0, 3, 63, 7, 59, 3, 0 },
+			 { 0, 13, 84, 7, 30, 1, 0 },
+			 { 0, 1, 34, 7, 83, 10, 0 },
+			 { 0, 4, 64, 7, 57, 3, 0 },
+			 { 0, 14, 87, 7, 27, 0, 0 },
+			 { 0, 1, 37, 7, 81, 9, 0 },
+			 { 0, 5, 67, 7, 54, 2, 0 },
+			 { 0, 16, 87, 7, 25, 0, 0 },
+			 { 0, 1, 40, 7, 79, 8, 0 },
+			 { 0, 5, 70, 7, 51, 2, 0 },
+			 { 0, 18, 87, 7, 23, 0, 0 },
+			 { 0, 1, 42, 7, 78, 7, 0 },
+			 { 0, 6, 72, 7, 48, 2, 0 },
+			 { 0, 19, 88, 7, 21, 0, 0 },
+			 { 0, 1, 45, 7, 75, 7, 0 },
+			 { 0, 7, 75, 7, 45, 1, 0 },
+			 { 0, 0, 21, 7, 88, 19, 0 },
+			 { 0, 2, 48, 7, 72, 6, 0 },
+			 { 0, 7, 78, 7, 42, 1, 0 },
+			 { 0, 0, 23, 7, 87, 18, 0 },
+			 { 0, 2, 51, 7, 70, 5, 0 },
+			 { 0, 8, 79, 7, 40, 1, 0 },
+			 { 0, 0, 25, 7, 87, 16, 0 },
+			 { 0, 2, 54, 7, 67, 5, 0 },
+			 { 0, 9, 81, 7, 37, 1, 0 },
+			 { 0, 0, 27, 7, 87, 14, 0 },
+			 { 0, 3, 57, 7, 64, 4, 0 },
+			 { 0, 10, 83, 7, 34, 1, 0 },
+			 { 0, 1, 30, 7, 84, 13, 0 },
+			 { 0, 3, 59, 7, 63, 3, 0 },
+			 { 0, 12, 83, 7, 32, 1, 0 } } },
+	.ptrn_arr = { { 0x64926493, 0x64926492, 0x4c926492, 0x4c924c92,
+			 0x4c924c92, 0x92 } },
+	.sample_patrn_length = 170,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 54) = 0.372093 */
+	.hor_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 6, 71, 7, 49, 2, 0 },
+			 { 0, 17, 86, 7, 25, 0, 0 },
+			 { 0, 1, 39, 7, 79, 9, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 14, 84, 7, 29, 1, 0 },
+			 { 0, 1, 34, 7, 82, 11, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 11, 82, 7, 34, 1, 0 },
+			 { 0, 1, 29, 7, 84, 14, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 9, 79, 7, 39, 1, 0 },
+			 { 0, 0, 25, 7, 86, 17, 0 },
+			 { 0, 2, 49, 7, 71, 6, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 } },
+		.odd = { { 0, 1, 31, 7, 83, 13, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 10, 81, 7, 36, 1, 0 },
+			 { 0, 0, 27, 7, 85, 16, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 8, 78, 7, 41, 1, 0 },
+			 { 0, 0, 23, 7, 86, 19, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 19, 86, 7, 23, 0, 0 },
+			 { 0, 1, 41, 7, 78, 8, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 16, 85, 7, 27, 0, 0 },
+			 { 0, 1, 36, 7, 81, 10, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 13, 83, 7, 31, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 },
+			 { 0, 1, 44, 7, 76, 7, 0 },
+			 { 0, 6, 71, 7, 49, 2, 0 },
+			 { 0, 17, 86, 7, 25, 0, 0 },
+			 { 0, 1, 39, 7, 79, 9, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 14, 84, 7, 29, 1, 0 },
+			 { 0, 1, 34, 7, 82, 11, 0 },
+			 { 0, 3, 61, 7, 61, 3, 0 },
+			 { 0, 11, 82, 7, 34, 1, 0 },
+			 { 0, 1, 29, 7, 84, 14, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 9, 79, 7, 39, 1, 0 },
+			 { 0, 0, 25, 7, 86, 17, 0 },
+			 { 0, 2, 49, 7, 71, 6, 0 },
+			 { 0, 7, 76, 7, 44, 1, 0 } },
+		.odd = { { 0, 1, 31, 7, 83, 13, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 10, 81, 7, 36, 1, 0 },
+			 { 0, 0, 27, 7, 85, 16, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 8, 78, 7, 41, 1, 0 },
+			 { 0, 0, 23, 7, 86, 19, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 19, 86, 7, 23, 0, 0 },
+			 { 0, 1, 41, 7, 78, 8, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 16, 85, 7, 27, 0, 0 },
+			 { 0, 1, 36, 7, 81, 10, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 13, 83, 7, 31, 1, 0 } } },
+	.ptrn_arr = { { 0x24932493, 0x24992493, 0x92499 } },
+	.sample_patrn_length = 86,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 55) = 0.367816 */
+	.hor_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 },
+			 { 0, 1, 41, 7, 77, 9, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 13, 82, 7, 32, 1, 0 },
+			 { 0, 1, 29, 7, 83, 15, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 19, 86, 7, 23, 0, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 12, 81, 7, 34, 1, 0 },
+			 { 0, 1, 27, 7, 84, 16, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 18, 85, 7, 25, 0, 0 },
+			 { 0, 1, 36, 7, 80, 11, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 11, 80, 7, 36, 1, 0 },
+			 { 0, 0, 25, 7, 85, 18, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 16, 84, 7, 27, 1, 0 },
+			 { 0, 1, 34, 7, 81, 12, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 0, 23, 7, 86, 19, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 15, 83, 7, 29, 1, 0 },
+			 { 0, 1, 32, 7, 82, 13, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 9, 77, 7, 41, 1, 0 } },
+		.odd = { { 0, 1, 30, 7, 83, 14, 0 },
+			 { 0, 3, 54, 7, 66, 5, 0 },
+			 { 0, 8, 76, 7, 43, 1, 0 },
+			 { 0, 20, 86, 7, 22, 0, 0 },
+			 { 0, 1, 40, 7, 78, 9, 0 },
+			 { 0, 4, 65, 7, 56, 3, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 1, 28, 7, 84, 15, 0 },
+			 { 0, 2, 51, 7, 69, 6, 0 },
+			 { 0, 7, 74, 7, 45, 2, 0 },
+			 { 0, 18, 86, 7, 24, 0, 0 },
+			 { 0, 1, 38, 7, 79, 10, 0 },
+			 { 0, 4, 62, 7, 59, 3, 0 },
+			 { 0, 11, 81, 7, 35, 1, 0 },
+			 { 0, 0, 26, 7, 85, 17, 0 },
+			 { 0, 2, 48, 7, 72, 6, 0 },
+			 { 0, 6, 72, 7, 48, 2, 0 },
+			 { 0, 17, 85, 7, 26, 0, 0 },
+			 { 0, 1, 35, 7, 81, 11, 0 },
+			 { 0, 3, 59, 7, 62, 4, 0 },
+			 { 0, 10, 79, 7, 38, 1, 0 },
+			 { 0, 0, 24, 7, 86, 18, 0 },
+			 { 0, 2, 45, 7, 74, 7, 0 },
+			 { 0, 6, 69, 7, 51, 2, 0 },
+			 { 0, 15, 84, 7, 28, 1, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 3, 56, 7, 65, 4, 0 },
+			 { 0, 9, 78, 7, 40, 1, 0 },
+			 { 0, 0, 22, 7, 86, 20, 0 },
+			 { 0, 1, 43, 7, 76, 8, 0 },
+			 { 0, 5, 66, 7, 54, 3, 0 },
+			 { 0, 14, 83, 7, 30, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 },
+			 { 0, 1, 41, 7, 77, 9, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 13, 82, 7, 32, 1, 0 },
+			 { 0, 1, 29, 7, 83, 15, 0 },
+			 { 0, 2, 52, 7, 69, 5, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 19, 86, 7, 23, 0, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 63, 7, 58, 3, 0 },
+			 { 0, 12, 81, 7, 34, 1, 0 },
+			 { 0, 1, 27, 7, 84, 16, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 18, 85, 7, 25, 0, 0 },
+			 { 0, 1, 36, 7, 80, 11, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 11, 80, 7, 36, 1, 0 },
+			 { 0, 0, 25, 7, 85, 18, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 16, 84, 7, 27, 1, 0 },
+			 { 0, 1, 34, 7, 81, 12, 0 },
+			 { 0, 3, 58, 7, 63, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 0, 23, 7, 86, 19, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 5, 69, 7, 52, 2, 0 },
+			 { 0, 15, 83, 7, 29, 1, 0 },
+			 { 0, 1, 32, 7, 82, 13, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 9, 77, 7, 41, 1, 0 } },
+		.odd = { { 0, 1, 30, 7, 83, 14, 0 },
+			 { 0, 3, 54, 7, 66, 5, 0 },
+			 { 0, 8, 76, 7, 43, 1, 0 },
+			 { 0, 20, 86, 7, 22, 0, 0 },
+			 { 0, 1, 40, 7, 78, 9, 0 },
+			 { 0, 4, 65, 7, 56, 3, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 1, 28, 7, 84, 15, 0 },
+			 { 0, 2, 51, 7, 69, 6, 0 },
+			 { 0, 7, 74, 7, 45, 2, 0 },
+			 { 0, 18, 86, 7, 24, 0, 0 },
+			 { 0, 1, 38, 7, 79, 10, 0 },
+			 { 0, 4, 62, 7, 59, 3, 0 },
+			 { 0, 11, 81, 7, 35, 1, 0 },
+			 { 0, 0, 26, 7, 85, 17, 0 },
+			 { 0, 2, 48, 7, 72, 6, 0 },
+			 { 0, 6, 72, 7, 48, 2, 0 },
+			 { 0, 17, 85, 7, 26, 0, 0 },
+			 { 0, 1, 35, 7, 81, 11, 0 },
+			 { 0, 3, 59, 7, 62, 4, 0 },
+			 { 0, 10, 79, 7, 38, 1, 0 },
+			 { 0, 0, 24, 7, 86, 18, 0 },
+			 { 0, 2, 45, 7, 74, 7, 0 },
+			 { 0, 6, 69, 7, 51, 2, 0 },
+			 { 0, 15, 84, 7, 28, 1, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 3, 56, 7, 65, 4, 0 },
+			 { 0, 9, 78, 7, 40, 1, 0 },
+			 { 0, 0, 22, 7, 86, 20, 0 },
+			 { 0, 1, 43, 7, 76, 8, 0 },
+			 { 0, 5, 66, 7, 54, 3, 0 },
+			 { 0, 14, 83, 7, 30, 1, 0 } } },
+	.ptrn_arr = { { 0x24992493, 0x264924c9, 0x92493249, 0x924c9249,
+			 0x93249264, 0x924 } },
+	.sample_patrn_length = 174,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 56) = 0.363636 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 } },
+		.odd = { { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 } },
+		.odd = { { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 } } },
+	.ptrn_arr = { { 0x92493 } },
+	.sample_patrn_length = 22,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 57) = 0.359551 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 37, 7, 79, 11, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 20, 84, 7, 24, 0, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 7, 69, 7, 50, 2, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 1, 26, 7, 83, 18, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 18, 83, 7, 26, 1, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 2, 50, 7, 69, 7, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 0, 24, 7, 84, 20, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 11, 79, 7, 37, 1, 0 } },
+		.odd = { { 0, 1, 29, 7, 82, 16, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 5, 64, 7, 56, 3, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 1, 36, 7, 79, 12, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 15, 81, 7, 31, 1, 0 },
+			 { 0, 1, 27, 7, 82, 18, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 4, 61, 7, 59, 4, 0 },
+			 { 0, 10, 75, 7, 41, 2, 0 },
+			 { 0, 19, 83, 7, 25, 1, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 3, 51, 7, 68, 6, 0 },
+			 { 0, 6, 68, 7, 51, 3, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 1, 25, 7, 83, 19, 0 },
+			 { 0, 2, 41, 7, 75, 10, 0 },
+			 { 0, 4, 59, 7, 61, 4, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 18, 82, 7, 27, 1, 0 },
+			 { 0, 1, 31, 7, 81, 15, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 12, 79, 7, 36, 1, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 3, 56, 7, 64, 5, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 16, 82, 7, 29, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 37, 7, 79, 11, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 20, 84, 7, 24, 0, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 7, 69, 7, 50, 2, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 1, 26, 7, 83, 18, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 18, 83, 7, 26, 1, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 2, 50, 7, 69, 7, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 0, 24, 7, 84, 20, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 11, 79, 7, 37, 1, 0 } },
+		.odd = { { 0, 1, 29, 7, 82, 16, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 5, 64, 7, 56, 3, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 1, 36, 7, 79, 12, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 15, 81, 7, 31, 1, 0 },
+			 { 0, 1, 27, 7, 82, 18, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 4, 61, 7, 59, 4, 0 },
+			 { 0, 10, 75, 7, 41, 2, 0 },
+			 { 0, 19, 83, 7, 25, 1, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 3, 51, 7, 68, 6, 0 },
+			 { 0, 6, 68, 7, 51, 3, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 1, 25, 7, 83, 19, 0 },
+			 { 0, 2, 41, 7, 75, 10, 0 },
+			 { 0, 4, 59, 7, 61, 4, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 18, 82, 7, 27, 1, 0 },
+			 { 0, 1, 31, 7, 81, 15, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 12, 79, 7, 36, 1, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 3, 56, 7, 64, 5, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 16, 82, 7, 29, 1, 0 } } },
+	.ptrn_arr = { { 0x26492493, 0x924c9249, 0x49249924, 0x64924932,
+			 0x24c92492, 0x9249 } },
+	.sample_patrn_length = 178,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 58) = 0.355556 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 6, 64, 7, 55, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 19, 82, 7, 26, 1, 0 },
+			 { 0, 1, 30, 7, 81, 16, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 16, 81, 7, 30, 1, 0 },
+			 { 0, 1, 26, 7, 82, 19, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 55, 7, 64, 6, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 } },
+		.odd = { { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 14, 80, 7, 33, 1, 0 },
+			 { 0, 1, 24, 7, 82, 21, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 21, 82, 7, 24, 1, 0 },
+			 { 0, 1, 33, 7, 80, 14, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 6, 64, 7, 55, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 19, 82, 7, 26, 1, 0 },
+			 { 0, 1, 30, 7, 81, 16, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 16, 81, 7, 30, 1, 0 },
+			 { 0, 1, 26, 7, 82, 19, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 55, 7, 64, 6, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 } },
+		.odd = { { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 14, 80, 7, 33, 1, 0 },
+			 { 0, 1, 24, 7, 82, 21, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 21, 82, 7, 24, 1, 0 },
+			 { 0, 1, 33, 7, 80, 14, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 } } },
+	.ptrn_arr = { { 0x32492493, 0x99249249, 0x924924 } },
+	.sample_patrn_length = 90,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 59) = 0.351648 */
+	.hor_phase_arr = {
+		.even = { { 0, 23, 82, 7, 23, 0, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 19, 81, 7, 27, 1, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 52, 7, 66, 7, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 16, 80, 7, 31, 1, 0 },
+			 { 0, 1, 25, 7, 81, 21, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 21, 81, 7, 25, 1, 0 },
+			 { 0, 1, 31, 7, 80, 16, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 7, 66, 7, 52, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 1, 27, 7, 81, 19, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 } },
+		.odd = { { 0, 1, 28, 7, 81, 18, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 6, 62, 7, 56, 4, 0 },
+			 { 0, 10, 73, 7, 43, 2, 0 },
+			 { 0, 15, 80, 7, 32, 1, 0 },
+			 { 0, 1, 24, 7, 81, 22, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 2, 46, 7, 71, 9, 0 },
+			 { 0, 4, 58, 7, 61, 5, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 13, 78, 7, 36, 1, 0 },
+			 { 0, 20, 81, 7, 26, 1, 0 },
+			 { 0, 1, 30, 7, 80, 17, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 17, 80, 7, 30, 1, 0 },
+			 { 0, 1, 26, 7, 81, 20, 0 },
+			 { 0, 1, 36, 7, 78, 13, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 5, 61, 7, 58, 4, 0 },
+			 { 0, 9, 71, 7, 46, 2, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 22, 81, 7, 24, 1, 0 },
+			 { 0, 1, 32, 7, 80, 15, 0 },
+			 { 0, 2, 43, 7, 73, 10, 0 },
+			 { 0, 4, 56, 7, 62, 6, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 18, 81, 7, 28, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 23, 82, 7, 23, 0, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 19, 81, 7, 27, 1, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 52, 7, 66, 7, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 16, 80, 7, 31, 1, 0 },
+			 { 0, 1, 25, 7, 81, 21, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 21, 81, 7, 25, 1, 0 },
+			 { 0, 1, 31, 7, 80, 16, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 7, 66, 7, 52, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 1, 27, 7, 81, 19, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 } },
+		.odd = { { 0, 1, 28, 7, 81, 18, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 6, 62, 7, 56, 4, 0 },
+			 { 0, 10, 73, 7, 43, 2, 0 },
+			 { 0, 15, 80, 7, 32, 1, 0 },
+			 { 0, 1, 24, 7, 81, 22, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 2, 46, 7, 71, 9, 0 },
+			 { 0, 4, 58, 7, 61, 5, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 13, 78, 7, 36, 1, 0 },
+			 { 0, 20, 81, 7, 26, 1, 0 },
+			 { 0, 1, 30, 7, 80, 17, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 17, 80, 7, 30, 1, 0 },
+			 { 0, 1, 26, 7, 81, 20, 0 },
+			 { 0, 1, 36, 7, 78, 13, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 5, 61, 7, 58, 4, 0 },
+			 { 0, 9, 71, 7, 46, 2, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 22, 81, 7, 24, 1, 0 },
+			 { 0, 1, 32, 7, 80, 15, 0 },
+			 { 0, 2, 43, 7, 73, 10, 0 },
+			 { 0, 4, 56, 7, 62, 6, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 18, 81, 7, 28, 1, 0 } } },
+	.ptrn_arr = { { 0x92492493, 0x4924924c, 0x24924992, 0x92493249,
+			 0x49264924, 0x92492 } },
+	.sample_patrn_length = 182,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 60) = 0.347826 */
+	.hor_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 } },
+		.odd = { { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 1, 35, 7, 78, 14, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 14, 78, 7, 35, 1, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 } },
+		.odd = { { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 1, 35, 7, 78, 14, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 14, 78, 7, 35, 1, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 } } },
+	.ptrn_arr = { { 0x92492493, 0x924 } },
+	.sample_patrn_length = 46,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 61) = 0.344086 */
+	.hor_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 1, 36, 7, 77, 14, 0 },
+			 { 0, 2, 42, 7, 73, 11, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 16, 78, 7, 33, 1, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 1, 25, 7, 79, 22, 1 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 2, 45, 7, 71, 10, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 10, 71, 7, 45, 2, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 1, 22, 79, 7, 25, 1, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 1, 33, 7, 78, 16, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 11, 73, 7, 42, 2, 0 },
+			 { 0, 14, 77, 7, 36, 1, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 } },
+		.odd = { { 0, 1, 26, 7, 80, 21, 0 },
+			 { 0, 1, 32, 7, 79, 16, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 3, 46, 7, 70, 9, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 8, 66, 7, 51, 3, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 18, 79, 7, 30, 1, 0 },
+			 { 1, 23, 79, 7, 24, 1, 0 },
+			 { 0, 1, 28, 7, 80, 19, 0 },
+			 { 0, 1, 35, 7, 77, 15, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 15, 77, 7, 35, 1, 0 },
+			 { 0, 19, 80, 7, 28, 1, 0 },
+			 { 0, 1, 24, 7, 79, 23, 1 },
+			 { 0, 1, 30, 7, 79, 18, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 3, 51, 7, 66, 8, 0 },
+			 { 0, 5, 58, 7, 60, 5, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 9, 70, 7, 46, 3, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 16, 79, 7, 32, 1, 0 },
+			 { 0, 21, 80, 7, 26, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 1, 36, 7, 77, 14, 0 },
+			 { 0, 2, 42, 7, 73, 11, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 16, 78, 7, 33, 1, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 1, 25, 7, 79, 22, 1 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 2, 45, 7, 71, 10, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 10, 71, 7, 45, 2, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 1, 22, 79, 7, 25, 1, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 1, 33, 7, 78, 16, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 11, 73, 7, 42, 2, 0 },
+			 { 0, 14, 77, 7, 36, 1, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 } },
+		.odd = { { 0, 1, 26, 7, 80, 21, 0 },
+			 { 0, 1, 32, 7, 79, 16, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 3, 46, 7, 70, 9, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 8, 66, 7, 51, 3, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 18, 79, 7, 30, 1, 0 },
+			 { 1, 23, 79, 7, 24, 1, 0 },
+			 { 0, 1, 28, 7, 80, 19, 0 },
+			 { 0, 1, 35, 7, 77, 15, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 15, 77, 7, 35, 1, 0 },
+			 { 0, 19, 80, 7, 28, 1, 0 },
+			 { 0, 1, 24, 7, 79, 23, 1 },
+			 { 0, 1, 30, 7, 79, 18, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 3, 51, 7, 66, 8, 0 },
+			 { 0, 5, 58, 7, 60, 5, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 9, 70, 7, 46, 3, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 16, 79, 7, 32, 1, 0 },
+			 { 0, 21, 80, 7, 26, 1, 0 } } },
+	.ptrn_arr = { { 0x92492493, 0x64924924, 0x92492492, 0x4c924924,
+			 0x92492492, 0x924924 } },
+	.sample_patrn_length = 186,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 62) = 0.340426 */
+	.hor_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 1, 28, 7, 79, 20, 0 },
+			 { 0, 1, 32, 7, 78, 17, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 17, 78, 7, 32, 1, 0 },
+			 { 0, 20, 79, 7, 28, 1, 0 } },
+		.odd = { { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 1, 28, 7, 79, 20, 0 },
+			 { 0, 1, 32, 7, 78, 17, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 17, 78, 7, 32, 1, 0 },
+			 { 0, 20, 79, 7, 28, 1, 0 } },
+		.odd = { { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 } } },
+	.ptrn_arr = { { 0x92492493, 0x24924924, 0x9249249 } },
+	.sample_patrn_length = 94,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 63) = 0.336842 */
+	.hor_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 0, 2, 40, 7, 73, 13, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 13, 73, 7, 40, 2, 0 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 } },
+		.odd = { { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 1, 27, 7, 77, 22, 1 },
+			 { 0, 1, 29, 7, 78, 20, 0 },
+			 { 0, 1, 31, 7, 78, 18, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 0, 2, 42, 7, 72, 12, 0 },
+			 { 0, 3, 44, 7, 70, 11, 0 },
+			 { 0, 3, 46, 7, 69, 10, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 10, 69, 7, 46, 3, 0 },
+			 { 0, 11, 70, 7, 44, 3, 0 },
+			 { 0, 12, 72, 7, 42, 2, 0 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 18, 78, 7, 31, 1, 0 },
+			 { 0, 20, 78, 7, 29, 1, 0 },
+			 { 1, 22, 77, 7, 27, 1, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 0, 2, 40, 7, 73, 13, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 13, 73, 7, 40, 2, 0 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 } },
+		.odd = { { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 1, 27, 7, 77, 22, 1 },
+			 { 0, 1, 29, 7, 78, 20, 0 },
+			 { 0, 1, 31, 7, 78, 18, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 0, 2, 42, 7, 72, 12, 0 },
+			 { 0, 3, 44, 7, 70, 11, 0 },
+			 { 0, 3, 46, 7, 69, 10, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 10, 69, 7, 46, 3, 0 },
+			 { 0, 11, 70, 7, 44, 3, 0 },
+			 { 0, 12, 72, 7, 42, 2, 0 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 18, 78, 7, 31, 1, 0 },
+			 { 0, 20, 78, 7, 29, 1, 0 },
+			 { 1, 22, 77, 7, 27, 1, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 } } },
+	.ptrn_arr = { { 0x92492493, 0x24924924, 0x49249249, 0x92492492,
+			 0x24924924, 0x9249249 } },
+	.sample_patrn_length = 190,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 64) = 0.333333 */
+	.hor_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 } },
+		.odd = { { 0, 4, 60, 7, 60, 4, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 21, 86, 7, 21, 0, 0 } },
+		.odd = { { 0, 4, 60, 7, 60, 4, 0 } } },
+	.ptrn_arr = { { 0x9 } },
+	.sample_patrn_length = 6,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 65) = 0.329897 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 20, 85, 7, 23, 0, 0 },
+			 { 0, 18, 84, 7, 25, 1, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 12, 81, 7, 34, 1, 0 },
+			 { 0, 11, 79, 7, 37, 1, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 4, 64, 7, 57, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 57, 7, 64, 4, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 1, 37, 7, 79, 11, 0 },
+			 { 0, 1, 34, 7, 81, 12, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 1, 25, 7, 84, 18, 0 },
+			 { 0, 0, 23, 7, 85, 20, 0 } },
+		.odd = { { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 19, 85, 7, 24, 0, 0 },
+			 { 0, 17, 84, 7, 26, 1, 0 },
+			 { 0, 16, 82, 7, 29, 1, 0 },
+			 { 0, 14, 82, 7, 31, 1, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 12, 80, 7, 35, 1, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 10, 77, 7, 40, 1, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 6, 69, 7, 51, 2, 0 },
+			 { 0, 5, 66, 7, 54, 3, 0 },
+			 { 0, 5, 64, 7, 56, 3, 0 },
+			 { 0, 4, 61, 7, 59, 4, 0 },
+			 { 0, 4, 59, 7, 61, 4, 0 },
+			 { 0, 3, 56, 7, 64, 5, 0 },
+			 { 0, 3, 54, 7, 66, 5, 0 },
+			 { 0, 2, 51, 7, 69, 6, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 1, 40, 7, 77, 10, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 1, 35, 7, 80, 12, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 1, 31, 7, 82, 14, 0 },
+			 { 0, 1, 29, 7, 82, 16, 0 },
+			 { 0, 1, 26, 7, 84, 17, 0 },
+			 { 0, 0, 24, 7, 85, 19, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 20, 85, 7, 23, 0, 0 },
+			 { 0, 18, 84, 7, 25, 1, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 12, 81, 7, 34, 1, 0 },
+			 { 0, 11, 79, 7, 37, 1, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 7, 72, 7, 47, 2, 0 },
+			 { 0, 6, 70, 7, 50, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 4, 64, 7, 57, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 57, 7, 64, 4, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 50, 7, 70, 6, 0 },
+			 { 0, 2, 47, 7, 72, 7, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 1, 37, 7, 79, 11, 0 },
+			 { 0, 1, 34, 7, 81, 12, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 1, 25, 7, 84, 18, 0 },
+			 { 0, 0, 23, 7, 85, 20, 0 } },
+		.odd = { { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 19, 85, 7, 24, 0, 0 },
+			 { 0, 17, 84, 7, 26, 1, 0 },
+			 { 0, 16, 82, 7, 29, 1, 0 },
+			 { 0, 14, 82, 7, 31, 1, 0 },
+			 { 0, 13, 81, 7, 33, 1, 0 },
+			 { 0, 12, 80, 7, 35, 1, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 10, 77, 7, 40, 1, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 6, 69, 7, 51, 2, 0 },
+			 { 0, 5, 66, 7, 54, 3, 0 },
+			 { 0, 5, 64, 7, 56, 3, 0 },
+			 { 0, 4, 61, 7, 59, 4, 0 },
+			 { 0, 4, 59, 7, 61, 4, 0 },
+			 { 0, 3, 56, 7, 64, 5, 0 },
+			 { 0, 3, 54, 7, 66, 5, 0 },
+			 { 0, 2, 51, 7, 69, 6, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 1, 40, 7, 77, 10, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 1, 35, 7, 80, 12, 0 },
+			 { 0, 1, 33, 7, 81, 13, 0 },
+			 { 0, 1, 31, 7, 82, 14, 0 },
+			 { 0, 1, 29, 7, 82, 16, 0 },
+			 { 0, 1, 26, 7, 84, 17, 0 },
+			 { 0, 0, 24, 7, 85, 19, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x92492492, 0x24924924, 0x49249249,
+			 0x92492492, 0x24924924 } },
+	.sample_patrn_length = 194,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 66) = 0.326531 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 18, 83, 7, 26, 1, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 7, 69, 7, 50, 2, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 2, 50, 7, 69, 7, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 1, 26, 7, 83, 18, 0 } },
+		.odd = { { 0, 20, 84, 7, 24, 0, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 0, 24, 7, 84, 20, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 18, 83, 7, 26, 1, 0 },
+			 { 0, 15, 82, 7, 30, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 10, 78, 7, 39, 1, 0 },
+			 { 0, 8, 74, 7, 44, 2, 0 },
+			 { 0, 7, 69, 7, 50, 2, 0 },
+			 { 0, 5, 65, 7, 55, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 55, 7, 65, 5, 0 },
+			 { 0, 2, 50, 7, 69, 7, 0 },
+			 { 0, 2, 44, 7, 74, 8, 0 },
+			 { 0, 1, 39, 7, 78, 10, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 30, 7, 82, 15, 0 },
+			 { 0, 1, 26, 7, 83, 18, 0 } },
+		.odd = { { 0, 20, 84, 7, 24, 0, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 14, 81, 7, 32, 1, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 9, 75, 7, 42, 2, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 2, 42, 7, 75, 9, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 1, 32, 7, 81, 14, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 },
+			 { 0, 0, 24, 7, 84, 20, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x92492492, 0x24924924 } },
+	.sample_patrn_length = 98,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 67) = 0.323232 */
+	.hor_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 3, 55, 7, 64, 6, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 33, 7, 80, 14, 0 },
+			 { 0, 1, 26, 7, 82, 19, 0 },
+			 { 0, 21, 82, 7, 24, 1, 0 },
+			 { 0, 16, 81, 7, 30, 1, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 1, 30, 7, 81, 16, 0 },
+			 { 0, 1, 24, 7, 82, 21, 0 },
+			 { 0, 19, 82, 7, 26, 1, 0 },
+			 { 0, 14, 80, 7, 33, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 6, 64, 7, 55, 3, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 } },
+		.odd = { { 0, 20, 82, 7, 25, 1, 0 },
+			 { 0, 15, 81, 7, 31, 1, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 4, 58, 7, 61, 5, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 1, 36, 7, 79, 12, 0 },
+			 { 0, 1, 29, 7, 81, 17, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 },
+			 { 0, 18, 82, 7, 27, 1, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 10, 75, 7, 41, 2, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 5, 63, 7, 56, 4, 0 },
+			 { 0, 4, 56, 7, 63, 5, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 2, 41, 7, 75, 10, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 1, 27, 7, 82, 18, 0 },
+			 { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 17, 81, 7, 29, 1, 0 },
+			 { 0, 12, 79, 7, 36, 1, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 5, 61, 7, 58, 4, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 1, 31, 7, 81, 15, 0 },
+			 { 0, 1, 25, 7, 82, 20, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 22, 84, 7, 22, 0, 0 },
+			 { 0, 17, 82, 7, 28, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 3, 55, 7, 64, 6, 0 },
+			 { 0, 2, 47, 7, 71, 8, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 33, 7, 80, 14, 0 },
+			 { 0, 1, 26, 7, 82, 19, 0 },
+			 { 0, 21, 82, 7, 24, 1, 0 },
+			 { 0, 16, 81, 7, 30, 1, 0 },
+			 { 0, 12, 78, 7, 37, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 6, 67, 7, 52, 3, 0 },
+			 { 0, 4, 60, 7, 60, 4, 0 },
+			 { 0, 3, 52, 7, 67, 6, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 37, 7, 78, 12, 0 },
+			 { 0, 1, 30, 7, 81, 16, 0 },
+			 { 0, 1, 24, 7, 82, 21, 0 },
+			 { 0, 19, 82, 7, 26, 1, 0 },
+			 { 0, 14, 80, 7, 33, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 8, 71, 7, 47, 2, 0 },
+			 { 0, 6, 64, 7, 55, 3, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 28, 7, 82, 17, 0 } },
+		.odd = { { 0, 20, 82, 7, 25, 1, 0 },
+			 { 0, 15, 81, 7, 31, 1, 0 },
+			 { 0, 11, 78, 7, 38, 1, 0 },
+			 { 0, 8, 72, 7, 46, 2, 0 },
+			 { 0, 6, 66, 7, 53, 3, 0 },
+			 { 0, 4, 58, 7, 61, 5, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 2, 43, 7, 74, 9, 0 },
+			 { 0, 1, 36, 7, 79, 12, 0 },
+			 { 0, 1, 29, 7, 81, 17, 0 },
+			 { 0, 0, 23, 7, 84, 21, 0 },
+			 { 0, 18, 82, 7, 27, 1, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 10, 75, 7, 41, 2, 0 },
+			 { 0, 7, 71, 7, 48, 2, 0 },
+			 { 0, 5, 63, 7, 56, 4, 0 },
+			 { 0, 4, 56, 7, 63, 5, 0 },
+			 { 0, 2, 48, 7, 71, 7, 0 },
+			 { 0, 2, 41, 7, 75, 10, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 1, 27, 7, 82, 18, 0 },
+			 { 0, 21, 84, 7, 23, 0, 0 },
+			 { 0, 17, 81, 7, 29, 1, 0 },
+			 { 0, 12, 79, 7, 36, 1, 0 },
+			 { 0, 9, 74, 7, 43, 2, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 5, 61, 7, 58, 4, 0 },
+			 { 0, 3, 53, 7, 66, 6, 0 },
+			 { 0, 2, 46, 7, 72, 8, 0 },
+			 { 0, 1, 38, 7, 78, 11, 0 },
+			 { 0, 1, 31, 7, 81, 15, 0 },
+			 { 0, 1, 25, 7, 82, 20, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x92492492, 0x92492490, 0x24924924,
+			 0x24924921, 0x49249249, 0x2 } },
+	.sample_patrn_length = 198,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 68) = 0.32 */
+	.hor_phase_arr = {
+		.even = { { 0, 23, 82, 7, 23, 0, 0 },
+			 { 0, 16, 80, 7, 31, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 31, 7, 80, 16, 0 } },
+		.odd = { { 0, 19, 81, 7, 27, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 27, 7, 81, 19, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 0, 23, 82, 7, 23, 0, 0 },
+			 { 0, 16, 80, 7, 31, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 7, 68, 7, 50, 3, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 3, 50, 7, 68, 7, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 31, 7, 80, 16, 0 } },
+		.odd = { { 0, 19, 81, 7, 27, 1, 0 },
+			 { 0, 13, 79, 7, 35, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 35, 7, 79, 13, 0 },
+			 { 0, 1, 27, 7, 81, 19, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x2492 } },
+	.sample_patrn_length = 50,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 69) = 0.316832 */
+	.hor_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 7, 66, 7, 52, 3, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 21, 81, 7, 25, 1, 0 },
+			 { 0, 14, 78, 7, 35, 1, 0 },
+			 { 0, 8, 70, 7, 47, 3, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 3, 47, 7, 70, 8, 0 },
+			 { 0, 1, 35, 7, 78, 14, 0 },
+			 { 0, 1, 25, 7, 81, 21, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 3, 52, 7, 66, 7, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 } },
+		.odd = { { 0, 19, 80, 7, 28, 1, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 4, 56, 7, 62, 6, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 1, 32, 7, 79, 16, 0 },
+			 { 0, 22, 81, 7, 24, 1, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 9, 71, 7, 46, 2, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 1, 36, 7, 78, 13, 0 },
+			 { 0, 1, 26, 7, 81, 20, 0 },
+			 { 0, 17, 80, 7, 30, 1, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 6, 65, 7, 53, 4, 0 },
+			 { 0, 4, 53, 7, 65, 6, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 1, 30, 7, 80, 17, 0 },
+			 { 0, 20, 81, 7, 26, 1, 0 },
+			 { 0, 13, 78, 7, 36, 1, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 5, 58, 7, 60, 5, 0 },
+			 { 0, 2, 46, 7, 71, 9, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 1, 24, 7, 81, 22, 0 },
+			 { 0, 16, 79, 7, 32, 1, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 6, 62, 7, 56, 4, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 1, 28, 7, 80, 19, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 },
+			 { 0, 9, 72, 7, 45, 2, 0 },
+			 { 0, 5, 62, 7, 57, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 11, 75, 7, 40, 2, 0 },
+			 { 0, 7, 66, 7, 52, 3, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 2, 42, 7, 74, 10, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 21, 81, 7, 25, 1, 0 },
+			 { 0, 14, 78, 7, 35, 1, 0 },
+			 { 0, 8, 70, 7, 47, 3, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 3, 47, 7, 70, 8, 0 },
+			 { 0, 1, 35, 7, 78, 14, 0 },
+			 { 0, 1, 25, 7, 81, 21, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 10, 74, 7, 42, 2, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 3, 52, 7, 66, 7, 0 },
+			 { 0, 2, 40, 7, 75, 11, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 57, 7, 62, 5, 0 },
+			 { 0, 2, 45, 7, 72, 9, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 } },
+		.odd = { { 0, 19, 80, 7, 28, 1, 0 },
+			 { 0, 12, 75, 7, 39, 2, 0 },
+			 { 0, 7, 67, 7, 51, 3, 0 },
+			 { 0, 4, 56, 7, 62, 6, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 1, 32, 7, 79, 16, 0 },
+			 { 0, 22, 81, 7, 24, 1, 0 },
+			 { 0, 14, 79, 7, 34, 1, 0 },
+			 { 0, 9, 71, 7, 46, 2, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 3, 48, 7, 69, 8, 0 },
+			 { 0, 1, 36, 7, 78, 13, 0 },
+			 { 0, 1, 26, 7, 81, 20, 0 },
+			 { 0, 17, 80, 7, 30, 1, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 6, 65, 7, 53, 4, 0 },
+			 { 0, 4, 53, 7, 65, 6, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 1, 30, 7, 80, 17, 0 },
+			 { 0, 20, 81, 7, 26, 1, 0 },
+			 { 0, 13, 78, 7, 36, 1, 0 },
+			 { 0, 8, 69, 7, 48, 3, 0 },
+			 { 0, 5, 58, 7, 60, 5, 0 },
+			 { 0, 2, 46, 7, 71, 9, 0 },
+			 { 0, 1, 34, 7, 79, 14, 0 },
+			 { 0, 1, 24, 7, 81, 22, 0 },
+			 { 0, 16, 79, 7, 32, 1, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 6, 62, 7, 56, 4, 0 },
+			 { 0, 3, 51, 7, 67, 7, 0 },
+			 { 0, 2, 39, 7, 75, 12, 0 },
+			 { 0, 1, 28, 7, 80, 19, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x49249212, 0x49242492, 0x48492492,
+			 0x92492492, 0x92492490, 0x24 } },
+	.sample_patrn_length = 202,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 70) = 0.313725 */
+	.hor_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 14, 77, 7, 36, 1, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 10, 71, 7, 45, 2, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 2, 45, 7, 71, 10, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 1, 36, 7, 77, 14, 0 } },
+		.odd = { { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 11, 73, 7, 42, 2, 0 },
+			 { 0, 6, 61, 7, 57, 4, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 },
+			 { 0, 22, 80, 7, 25, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 1, 25, 7, 80, 22, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 4, 57, 7, 61, 6, 0 },
+			 { 0, 2, 42, 7, 73, 11, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 23, 80, 7, 23, 1, 0 },
+			 { 0, 14, 77, 7, 36, 1, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 54, 7, 64, 6, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 10, 71, 7, 45, 2, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 2, 45, 7, 71, 10, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 6, 64, 7, 54, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 1, 36, 7, 77, 14, 0 } },
+		.odd = { { 0, 18, 80, 7, 29, 1, 0 },
+			 { 0, 11, 73, 7, 42, 2, 0 },
+			 { 0, 6, 61, 7, 57, 4, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 1, 33, 7, 79, 15, 0 },
+			 { 0, 22, 80, 7, 25, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 },
+			 { 0, 1, 25, 7, 80, 22, 0 },
+			 { 0, 15, 79, 7, 33, 1, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 4, 57, 7, 61, 6, 0 },
+			 { 0, 2, 42, 7, 73, 11, 0 },
+			 { 0, 1, 29, 7, 80, 18, 0 } } },
+	.ptrn_arr = { { 0x49249249, 0x49249248, 0x49249242, 0x2 } },
+	.sample_patrn_length = 102,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 71) = 0.31068 */
+	.hor_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 19, 79, 7, 29, 1, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 14, 76, 7, 36, 2, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 2, 36, 7, 76, 14, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 1, 29, 7, 79, 19, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 } },
+		.odd = { { 0, 18, 79, 7, 30, 1, 0 },
+			 { 0, 9, 70, 7, 46, 3, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 },
+			 { 0, 20, 79, 7, 28, 1, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 5, 58, 7, 59, 6, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 1, 26, 7, 79, 21, 1 },
+			 { 0, 15, 77, 7, 35, 1, 0 },
+			 { 0, 8, 66, 7, 51, 3, 0 },
+			 { 0, 3, 51, 7, 66, 8, 0 },
+			 { 0, 1, 35, 7, 77, 15, 0 },
+			 { 1, 21, 79, 7, 26, 1, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 6, 59, 7, 58, 5, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 1, 28, 7, 79, 20, 0 },
+			 { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 3, 46, 7, 70, 9, 0 },
+			 { 0, 1, 30, 7, 79, 18, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 13, 75, 7, 38, 2, 0 },
+			 { 0, 7, 63, 7, 54, 4, 0 },
+			 { 0, 3, 47, 7, 69, 9, 0 },
+			 { 0, 1, 31, 7, 79, 17, 0 },
+			 { 0, 19, 79, 7, 29, 1, 0 },
+			 { 0, 10, 70, 7, 45, 3, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 },
+			 { 0, 1, 26, 7, 78, 22, 1 },
+			 { 0, 14, 76, 7, 36, 2, 0 },
+			 { 0, 7, 65, 7, 52, 4, 0 },
+			 { 0, 3, 50, 7, 67, 8, 0 },
+			 { 0, 1, 34, 7, 77, 16, 0 },
+			 { 0, 20, 80, 7, 27, 1, 0 },
+			 { 0, 11, 72, 7, 43, 2, 0 },
+			 { 0, 5, 59, 7, 59, 5, 0 },
+			 { 0, 2, 43, 7, 72, 11, 0 },
+			 { 0, 1, 27, 7, 80, 20, 0 },
+			 { 0, 16, 77, 7, 34, 1, 0 },
+			 { 0, 8, 67, 7, 50, 3, 0 },
+			 { 0, 4, 52, 7, 65, 7, 0 },
+			 { 0, 2, 36, 7, 76, 14, 0 },
+			 { 1, 22, 78, 7, 26, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 3, 45, 7, 70, 10, 0 },
+			 { 0, 1, 29, 7, 79, 19, 0 },
+			 { 0, 17, 79, 7, 31, 1, 0 },
+			 { 0, 9, 69, 7, 47, 3, 0 },
+			 { 0, 4, 54, 7, 63, 7, 0 },
+			 { 0, 2, 38, 7, 75, 13, 0 } },
+		.odd = { { 0, 18, 79, 7, 30, 1, 0 },
+			 { 0, 9, 70, 7, 46, 3, 0 },
+			 { 0, 4, 55, 7, 63, 6, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 },
+			 { 0, 20, 79, 7, 28, 1, 0 },
+			 { 0, 10, 72, 7, 44, 2, 0 },
+			 { 0, 5, 58, 7, 59, 6, 0 },
+			 { 0, 2, 41, 7, 74, 11, 0 },
+			 { 0, 1, 26, 7, 79, 21, 1 },
+			 { 0, 15, 77, 7, 35, 1, 0 },
+			 { 0, 8, 66, 7, 51, 3, 0 },
+			 { 0, 3, 51, 7, 66, 8, 0 },
+			 { 0, 1, 35, 7, 77, 15, 0 },
+			 { 1, 21, 79, 7, 26, 1, 0 },
+			 { 0, 11, 74, 7, 41, 2, 0 },
+			 { 0, 6, 59, 7, 58, 5, 0 },
+			 { 0, 2, 44, 7, 72, 10, 0 },
+			 { 0, 1, 28, 7, 79, 20, 0 },
+			 { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 6, 63, 7, 55, 4, 0 },
+			 { 0, 3, 46, 7, 70, 9, 0 },
+			 { 0, 1, 30, 7, 79, 18, 0 } } },
+	.ptrn_arr = { { 0x9249249, 0x21249249, 0x24249249, 0x24849249,
+			 0x24909249, 0x24921249, 0x249 } },
+	.sample_patrn_length = 206,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 72) = 0.307692 */
+	.hor_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 } },
+		.odd = { { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 8, 68, 7, 49, 3, 0 },
+			 { 0, 3, 49, 7, 68, 8, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 12, 74, 7, 40, 2, 0 },
+			 { 0, 5, 60, 7, 58, 5, 0 },
+			 { 0, 2, 40, 7, 74, 12, 0 } },
+		.odd = { { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 8, 68, 7, 49, 3, 0 },
+			 { 0, 3, 49, 7, 68, 8, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 } } },
+	.ptrn_arr = { { 0x249249 } },
+	.sample_patrn_length = 26,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 73) = 0.304762 */
+	.hor_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 5, 54, 7, 62, 7, 0 },
+			 { 0, 2, 34, 7, 76, 16, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 0, 1, 26, 7, 77, 23, 1 },
+			 { 0, 13, 73, 7, 40, 2, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 47, 7, 68, 10, 0 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 0, 10, 68, 7, 47, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 2, 40, 7, 73, 13, 0 },
+			 { 1, 23, 77, 7, 26, 1, 0 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 16, 76, 7, 34, 2, 0 },
+			 { 0, 7, 62, 7, 54, 5, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 } },
+		.odd = { { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 3, 44, 7, 70, 11, 0 },
+			 { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 12, 72, 7, 42, 2, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 },
+			 { 0, 19, 77, 7, 31, 1, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 3, 46, 7, 69, 10, 0 },
+			 { 0, 1, 27, 7, 77, 22, 1 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 1, 20, 77, 7, 29, 1, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 1, 29, 7, 77, 20, 1 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 1, 22, 77, 7, 27, 1, 0 },
+			 { 0, 10, 69, 7, 46, 3, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 1, 31, 7, 77, 19, 0 },
+			 { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 2, 42, 7, 72, 12, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 },
+			 { 0, 11, 70, 7, 44, 3, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 24, 78, 7, 24, 1, 0 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 5, 54, 7, 62, 7, 0 },
+			 { 0, 2, 34, 7, 76, 16, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 0, 1, 26, 7, 77, 23, 1 },
+			 { 0, 13, 73, 7, 40, 2, 0 },
+			 { 0, 5, 56, 7, 61, 6, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 19, 78, 7, 30, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 47, 7, 68, 10, 0 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 0, 10, 68, 7, 47, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 30, 7, 78, 19, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 6, 61, 7, 56, 5, 0 },
+			 { 0, 2, 40, 7, 73, 13, 0 },
+			 { 1, 23, 77, 7, 26, 1, 0 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 16, 76, 7, 34, 2, 0 },
+			 { 0, 7, 62, 7, 54, 5, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 } },
+		.odd = { { 0, 17, 77, 7, 33, 1, 0 },
+			 { 0, 7, 64, 7, 53, 4, 0 },
+			 { 0, 3, 44, 7, 70, 11, 0 },
+			 { 0, 1, 25, 7, 78, 23, 1 },
+			 { 0, 12, 72, 7, 42, 2, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 },
+			 { 0, 19, 77, 7, 31, 1, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 3, 46, 7, 69, 10, 0 },
+			 { 0, 1, 27, 7, 77, 22, 1 },
+			 { 0, 13, 74, 7, 39, 2, 0 },
+			 { 0, 5, 57, 7, 60, 6, 0 },
+			 { 0, 2, 37, 7, 75, 14, 0 },
+			 { 1, 20, 77, 7, 29, 1, 0 },
+			 { 0, 9, 68, 7, 48, 3, 0 },
+			 { 0, 3, 48, 7, 68, 9, 0 },
+			 { 0, 1, 29, 7, 77, 20, 1 },
+			 { 0, 14, 75, 7, 37, 2, 0 },
+			 { 0, 6, 60, 7, 57, 5, 0 },
+			 { 0, 2, 39, 7, 74, 13, 0 },
+			 { 1, 22, 77, 7, 27, 1, 0 },
+			 { 0, 10, 69, 7, 46, 3, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 1, 31, 7, 77, 19, 0 },
+			 { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 2, 42, 7, 72, 12, 0 },
+			 { 1, 23, 78, 7, 25, 1, 0 },
+			 { 0, 11, 70, 7, 44, 3, 0 },
+			 { 0, 4, 53, 7, 64, 7, 0 },
+			 { 0, 1, 33, 7, 77, 17, 0 } } },
+	.ptrn_arr = { { 0x24249249, 0x24921249, 0x84924909, 0x92424924,
+			 0x92492124, 0x48492490, 0x2492 } },
+	.sample_patrn_length = 210,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 74) = 0.301887 */
+	.hor_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 13, 72, 7, 41, 2, 0 },
+			 { 0, 5, 54, 7, 62, 7, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 7, 62, 7, 54, 5, 0 },
+			 { 0, 2, 41, 7, 72, 13, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 } },
+		.odd = { { 0, 17, 75, 7, 34, 2, 0 },
+			 { 0, 7, 60, 7, 56, 5, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 1, 20, 76, 7, 30, 1, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 },
+			 { 1, 23, 77, 7, 26, 1, 0 },
+			 { 0, 10, 68, 7, 47, 3, 0 },
+			 { 0, 3, 47, 7, 68, 10, 0 },
+			 { 0, 1, 26, 7, 77, 23, 1 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 1, 30, 7, 76, 20, 1 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 5, 56, 7, 60, 7, 0 },
+			 { 0, 2, 34, 7, 75, 17, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 28, 7, 77, 21, 1 },
+			 { 0, 13, 72, 7, 41, 2, 0 },
+			 { 0, 5, 54, 7, 62, 7, 0 },
+			 { 0, 1, 32, 7, 77, 18, 0 },
+			 { 0, 15, 75, 7, 36, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 36, 7, 75, 15, 0 },
+			 { 0, 18, 77, 7, 32, 1, 0 },
+			 { 0, 7, 62, 7, 54, 5, 0 },
+			 { 0, 2, 41, 7, 72, 13, 0 },
+			 { 1, 21, 77, 7, 28, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 } },
+		.odd = { { 0, 17, 75, 7, 34, 2, 0 },
+			 { 0, 7, 60, 7, 56, 5, 0 },
+			 { 0, 2, 38, 7, 74, 14, 0 },
+			 { 1, 20, 76, 7, 30, 1, 0 },
+			 { 0, 8, 64, 7, 52, 4, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 },
+			 { 1, 23, 77, 7, 26, 1, 0 },
+			 { 0, 10, 68, 7, 47, 3, 0 },
+			 { 0, 3, 47, 7, 68, 10, 0 },
+			 { 0, 1, 26, 7, 77, 23, 1 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 4, 52, 7, 64, 8, 0 },
+			 { 0, 1, 30, 7, 76, 20, 1 },
+			 { 0, 14, 74, 7, 38, 2, 0 },
+			 { 0, 5, 56, 7, 60, 7, 0 },
+			 { 0, 2, 34, 7, 75, 17, 0 } } },
+	.ptrn_arr = { { 0x24849249, 0x24924849, 0x92424924, 0x24 } },
+	.sample_patrn_length = 106,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 75) = 0.299065 */
+	.hor_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 10, 67, 7, 47, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 1, 23, 76, 7, 27, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 2, 41, 7, 72, 13, 0 },
+			 { 1, 20, 76, 7, 30, 1, 0 },
+			 { 0, 8, 61, 7, 54, 5, 0 },
+			 { 0, 2, 39, 7, 72, 15, 0 },
+			 { 0, 19, 76, 7, 32, 1, 0 },
+			 { 0, 7, 59, 7, 56, 6, 0 },
+			 { 0, 2, 36, 7, 74, 16, 0 },
+			 { 0, 17, 75, 7, 34, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 34, 7, 75, 17, 0 },
+			 { 0, 16, 74, 7, 36, 2, 0 },
+			 { 0, 6, 56, 7, 59, 7, 0 },
+			 { 0, 1, 32, 7, 76, 19, 0 },
+			 { 0, 15, 72, 7, 39, 2, 0 },
+			 { 0, 5, 54, 7, 61, 8, 0 },
+			 { 0, 1, 30, 7, 76, 20, 1 },
+			 { 0, 13, 72, 7, 41, 2, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 27, 7, 76, 23, 1 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 47, 7, 67, 10, 0 } },
+		.odd = { { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 6, 57, 7, 58, 7, 0 },
+			 { 0, 2, 33, 7, 75, 18, 0 },
+			 { 0, 15, 73, 7, 38, 2, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 1, 31, 7, 76, 19, 1 },
+			 { 0, 14, 72, 7, 40, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 30, 7, 75, 21, 1 },
+			 { 0, 13, 70, 7, 42, 3, 0 },
+			 { 0, 4, 50, 7, 65, 9, 0 },
+			 { 0, 1, 28, 7, 76, 22, 1 },
+			 { 0, 12, 69, 7, 44, 3, 0 },
+			 { 0, 4, 48, 7, 66, 10, 0 },
+			 { 0, 1, 26, 7, 76, 24, 1 },
+			 { 0, 11, 68, 7, 46, 3, 0 },
+			 { 0, 3, 46, 7, 68, 11, 0 },
+			 { 1, 24, 76, 7, 26, 1, 0 },
+			 { 0, 10, 66, 7, 48, 4, 0 },
+			 { 0, 3, 44, 7, 69, 12, 0 },
+			 { 1, 22, 76, 7, 28, 1, 0 },
+			 { 0, 9, 65, 7, 50, 4, 0 },
+			 { 0, 3, 42, 7, 70, 13, 0 },
+			 { 1, 21, 75, 7, 30, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 40, 7, 72, 14, 0 },
+			 { 1, 19, 76, 7, 31, 1, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 2, 38, 7, 73, 15, 0 },
+			 { 0, 18, 75, 7, 33, 2, 0 },
+			 { 0, 7, 58, 7, 57, 6, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 10, 67, 7, 47, 4, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 1, 23, 76, 7, 27, 1, 0 },
+			 { 0, 9, 66, 7, 49, 4, 0 },
+			 { 0, 3, 43, 7, 70, 12, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 8, 65, 7, 51, 4, 0 },
+			 { 0, 2, 41, 7, 72, 13, 0 },
+			 { 1, 20, 76, 7, 30, 1, 0 },
+			 { 0, 8, 61, 7, 54, 5, 0 },
+			 { 0, 2, 39, 7, 72, 15, 0 },
+			 { 0, 19, 76, 7, 32, 1, 0 },
+			 { 0, 7, 59, 7, 56, 6, 0 },
+			 { 0, 2, 36, 7, 74, 16, 0 },
+			 { 0, 17, 75, 7, 34, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 34, 7, 75, 17, 0 },
+			 { 0, 16, 74, 7, 36, 2, 0 },
+			 { 0, 6, 56, 7, 59, 7, 0 },
+			 { 0, 1, 32, 7, 76, 19, 0 },
+			 { 0, 15, 72, 7, 39, 2, 0 },
+			 { 0, 5, 54, 7, 61, 8, 0 },
+			 { 0, 1, 30, 7, 76, 20, 1 },
+			 { 0, 13, 72, 7, 41, 2, 0 },
+			 { 0, 4, 51, 7, 65, 8, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 12, 70, 7, 43, 3, 0 },
+			 { 0, 4, 49, 7, 66, 9, 0 },
+			 { 0, 1, 27, 7, 76, 23, 1 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 4, 47, 7, 67, 10, 0 } },
+		.odd = { { 0, 16, 75, 7, 35, 2, 0 },
+			 { 0, 6, 57, 7, 58, 7, 0 },
+			 { 0, 2, 33, 7, 75, 18, 0 },
+			 { 0, 15, 73, 7, 38, 2, 0 },
+			 { 0, 5, 55, 7, 61, 7, 0 },
+			 { 0, 1, 31, 7, 76, 19, 1 },
+			 { 0, 14, 72, 7, 40, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 30, 7, 75, 21, 1 },
+			 { 0, 13, 70, 7, 42, 3, 0 },
+			 { 0, 4, 50, 7, 65, 9, 0 },
+			 { 0, 1, 28, 7, 76, 22, 1 },
+			 { 0, 12, 69, 7, 44, 3, 0 },
+			 { 0, 4, 48, 7, 66, 10, 0 },
+			 { 0, 1, 26, 7, 76, 24, 1 },
+			 { 0, 11, 68, 7, 46, 3, 0 },
+			 { 0, 3, 46, 7, 68, 11, 0 },
+			 { 1, 24, 76, 7, 26, 1, 0 },
+			 { 0, 10, 66, 7, 48, 4, 0 },
+			 { 0, 3, 44, 7, 69, 12, 0 },
+			 { 1, 22, 76, 7, 28, 1, 0 },
+			 { 0, 9, 65, 7, 50, 4, 0 },
+			 { 0, 3, 42, 7, 70, 13, 0 },
+			 { 1, 21, 75, 7, 30, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 40, 7, 72, 14, 0 },
+			 { 1, 19, 76, 7, 31, 1, 0 },
+			 { 0, 7, 61, 7, 55, 5, 0 },
+			 { 0, 2, 38, 7, 73, 15, 0 },
+			 { 0, 18, 75, 7, 33, 2, 0 },
+			 { 0, 7, 58, 7, 57, 6, 0 },
+			 { 0, 2, 35, 7, 75, 16, 0 } } },
+	.ptrn_arr = { { 0x24909249, 0x90924909, 0x92490924, 0x49212490,
+			 0x21249212, 0x24921249, 0x24921 } },
+	.sample_patrn_length = 214,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 76) = 0.296296 */
+	.hor_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 10, 65, 7, 49, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 49, 7, 65, 10, 0 } },
+		.odd = { { 0, 16, 73, 7, 37, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 37, 7, 73, 16, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 25, 76, 7, 25, 1, 0 },
+			 { 0, 10, 65, 7, 49, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 58, 7, 58, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 49, 7, 65, 10, 0 } },
+		.odd = { { 0, 16, 73, 7, 37, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 11, 69, 7, 45, 3, 0 },
+			 { 0, 3, 45, 7, 69, 11, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 37, 7, 73, 16, 0 } } },
+	.ptrn_arr = { { 0x24909249, 0x24921 } },
+	.sample_patrn_length = 54,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 77) = 0.293578 */
+	.hor_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 37, 7, 73, 16, 0 },
+			 { 0, 15, 72, 7, 39, 2, 0 },
+			 { 0, 4, 49, 7, 65, 10, 0 },
+			 { 1, 24, 75, 7, 27, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 7, 60, 7, 55, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 45, 7, 68, 12, 0 },
+			 { 1, 21, 74, 7, 31, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 31, 7, 74, 21, 1 },
+			 { 0, 12, 68, 7, 45, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 55, 7, 60, 7, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 27, 7, 75, 24, 1 },
+			 { 0, 10, 65, 7, 49, 4, 0 },
+			 { 0, 2, 39, 7, 72, 15, 0 },
+			 { 0, 16, 73, 7, 37, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 } },
+		.odd = { { 0, 16, 72, 7, 38, 2, 0 },
+			 { 0, 5, 50, 7, 64, 9, 0 },
+			 { 1, 25, 75, 7, 26, 1, 0 },
+			 { 0, 8, 63, 7, 52, 5, 0 },
+			 { 0, 2, 36, 7, 73, 17, 0 },
+			 { 0, 15, 70, 7, 40, 3, 0 },
+			 { 0, 4, 48, 7, 66, 10, 0 },
+			 { 1, 23, 75, 7, 28, 1, 0 },
+			 { 0, 8, 60, 7, 54, 6, 0 },
+			 { 0, 2, 34, 7, 73, 18, 1 },
+			 { 0, 13, 70, 7, 42, 3, 0 },
+			 { 0, 4, 46, 7, 67, 11, 0 },
+			 { 1, 21, 75, 7, 30, 1, 0 },
+			 { 0, 7, 59, 7, 56, 6, 0 },
+			 { 0, 2, 32, 7, 73, 20, 1 },
+			 { 0, 12, 69, 7, 44, 3, 0 },
+			 { 0, 3, 44, 7, 69, 12, 0 },
+			 { 1, 20, 73, 7, 32, 2, 0 },
+			 { 0, 6, 56, 7, 59, 7, 0 },
+			 { 0, 1, 30, 7, 75, 21, 1 },
+			 { 0, 11, 67, 7, 46, 4, 0 },
+			 { 0, 3, 42, 7, 70, 13, 0 },
+			 { 1, 18, 73, 7, 34, 2, 0 },
+			 { 0, 6, 54, 7, 60, 8, 0 },
+			 { 0, 1, 28, 7, 75, 23, 1 },
+			 { 0, 10, 66, 7, 48, 4, 0 },
+			 { 0, 3, 40, 7, 70, 15, 0 },
+			 { 0, 17, 73, 7, 36, 2, 0 },
+			 { 0, 5, 52, 7, 63, 8, 0 },
+			 { 0, 1, 26, 7, 75, 25, 1 },
+			 { 0, 9, 64, 7, 50, 5, 0 },
+			 { 0, 2, 38, 7, 72, 16, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 37, 7, 73, 16, 0 },
+			 { 0, 15, 72, 7, 39, 2, 0 },
+			 { 0, 4, 49, 7, 65, 10, 0 },
+			 { 1, 24, 75, 7, 27, 1, 0 },
+			 { 0, 8, 62, 7, 53, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 7, 60, 7, 55, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 45, 7, 68, 12, 0 },
+			 { 1, 21, 74, 7, 31, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 31, 7, 74, 21, 1 },
+			 { 0, 12, 68, 7, 45, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 55, 7, 60, 7, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 53, 7, 62, 8, 0 },
+			 { 0, 1, 27, 7, 75, 24, 1 },
+			 { 0, 10, 65, 7, 49, 4, 0 },
+			 { 0, 2, 39, 7, 72, 15, 0 },
+			 { 0, 16, 73, 7, 37, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 } },
+		.odd = { { 0, 16, 72, 7, 38, 2, 0 },
+			 { 0, 5, 50, 7, 64, 9, 0 },
+			 { 1, 25, 75, 7, 26, 1, 0 },
+			 { 0, 8, 63, 7, 52, 5, 0 },
+			 { 0, 2, 36, 7, 73, 17, 0 },
+			 { 0, 15, 70, 7, 40, 3, 0 },
+			 { 0, 4, 48, 7, 66, 10, 0 },
+			 { 1, 23, 75, 7, 28, 1, 0 },
+			 { 0, 8, 60, 7, 54, 6, 0 },
+			 { 0, 2, 34, 7, 73, 18, 1 },
+			 { 0, 13, 70, 7, 42, 3, 0 },
+			 { 0, 4, 46, 7, 67, 11, 0 },
+			 { 1, 21, 75, 7, 30, 1, 0 },
+			 { 0, 7, 59, 7, 56, 6, 0 },
+			 { 0, 2, 32, 7, 73, 20, 1 },
+			 { 0, 12, 69, 7, 44, 3, 0 },
+			 { 0, 3, 44, 7, 69, 12, 0 },
+			 { 1, 20, 73, 7, 32, 2, 0 },
+			 { 0, 6, 56, 7, 59, 7, 0 },
+			 { 0, 1, 30, 7, 75, 21, 1 },
+			 { 0, 11, 67, 7, 46, 4, 0 },
+			 { 0, 3, 42, 7, 70, 13, 0 },
+			 { 1, 18, 73, 7, 34, 2, 0 },
+			 { 0, 6, 54, 7, 60, 8, 0 },
+			 { 0, 1, 28, 7, 75, 23, 1 },
+			 { 0, 10, 66, 7, 48, 4, 0 },
+			 { 0, 3, 40, 7, 70, 15, 0 },
+			 { 0, 17, 73, 7, 36, 2, 0 },
+			 { 0, 5, 52, 7, 63, 8, 0 },
+			 { 0, 1, 26, 7, 75, 25, 1 },
+			 { 0, 9, 64, 7, 50, 5, 0 },
+			 { 0, 2, 38, 7, 72, 16, 0 } } },
+	.ptrn_arr = { { 0x24921249, 0x92484924, 0x49212490, 0x24849242,
+			 0x92124909, 0x48492424, 0x249092 } },
+	.sample_patrn_length = 218,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 78) = 0.290909 */
+	.hor_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 8, 61, 7, 53, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 12, 67, 7, 45, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 0, 17, 72, 7, 37, 2, 0 },
+			 { 0, 5, 49, 7, 64, 10, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 10, 64, 7, 49, 5, 0 },
+			 { 0, 2, 37, 7, 72, 17, 0 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 45, 7, 67, 12, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 53, 7, 61, 8, 0 } },
+		.odd = { { 0, 15, 71, 7, 39, 3, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 21, 73, 7, 31, 2, 0 },
+			 { 0, 6, 55, 7, 59, 8, 0 },
+			 { 0, 1, 28, 7, 74, 24, 1 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 },
+			 { 1, 24, 74, 7, 28, 1, 0 },
+			 { 0, 8, 59, 7, 55, 6, 0 },
+			 { 0, 2, 31, 7, 73, 21, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 3, 39, 7, 71, 15, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 8, 61, 7, 53, 6, 0 },
+			 { 0, 2, 33, 7, 73, 19, 1 },
+			 { 0, 12, 67, 7, 45, 4, 0 },
+			 { 0, 3, 41, 7, 70, 14, 0 },
+			 { 0, 17, 72, 7, 37, 2, 0 },
+			 { 0, 5, 49, 7, 64, 10, 0 },
+			 { 1, 22, 75, 7, 29, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 29, 7, 75, 22, 1 },
+			 { 0, 10, 64, 7, 49, 5, 0 },
+			 { 0, 2, 37, 7, 72, 17, 0 },
+			 { 0, 14, 70, 7, 41, 3, 0 },
+			 { 0, 4, 45, 7, 67, 12, 0 },
+			 { 1, 19, 73, 7, 33, 2, 0 },
+			 { 0, 6, 53, 7, 61, 8, 0 } },
+		.odd = { { 0, 15, 71, 7, 39, 3, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 21, 73, 7, 31, 2, 0 },
+			 { 0, 6, 55, 7, 59, 8, 0 },
+			 { 0, 1, 28, 7, 74, 24, 1 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 },
+			 { 1, 24, 74, 7, 28, 1, 0 },
+			 { 0, 8, 59, 7, 55, 6, 0 },
+			 { 0, 2, 31, 7, 73, 21, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 3, 39, 7, 71, 15, 0 } } },
+	.ptrn_arr = { { 0x24921249, 0x12490924, 0x9248492, 0x249 } },
+	.sample_patrn_length = 110,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 79) = 0.288288 */
+	.hor_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 8, 59, 7, 55, 6, 0 },
+			 { 0, 1, 30, 7, 73, 23, 1 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 33, 7, 72, 20, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 2, 37, 7, 71, 17, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 },
+			 { 0, 16, 70, 7, 39, 3, 0 },
+			 { 0, 4, 45, 7, 67, 12, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 49, 7, 64, 10, 0 },
+			 { 1, 21, 73, 7, 31, 2, 0 },
+			 { 0, 6, 53, 7, 60, 9, 0 },
+			 { 1, 24, 74, 7, 28, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 28, 7, 74, 24, 1 },
+			 { 0, 9, 60, 7, 53, 6, 0 },
+			 { 0, 2, 31, 7, 73, 21, 1 },
+			 { 0, 10, 64, 7, 49, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 12, 67, 7, 45, 4, 0 },
+			 { 0, 3, 39, 7, 70, 16, 0 },
+			 { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 17, 71, 7, 37, 2, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 20, 72, 7, 33, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 },
+			 { 1, 23, 73, 7, 30, 1, 0 },
+			 { 0, 6, 55, 7, 59, 8, 0 } },
+		.odd = { { 0, 15, 70, 7, 40, 3, 0 },
+			 { 0, 4, 44, 7, 67, 13, 0 },
+			 { 1, 18, 71, 7, 36, 2, 0 },
+			 { 0, 4, 48, 7, 65, 11, 0 },
+			 { 1, 20, 73, 7, 32, 2, 0 },
+			 { 0, 6, 52, 7, 61, 9, 0 },
+			 { 1, 24, 73, 7, 29, 1, 0 },
+			 { 0, 7, 56, 7, 58, 7, 0 },
+			 { 0, 1, 27, 7, 74, 25, 1 },
+			 { 0, 8, 60, 7, 54, 6, 0 },
+			 { 0, 2, 30, 7, 73, 22, 1 },
+			 { 0, 10, 63, 7, 50, 5, 0 },
+			 { 0, 2, 34, 7, 72, 19, 1 },
+			 { 0, 12, 66, 7, 46, 4, 0 },
+			 { 0, 3, 38, 7, 71, 16, 0 },
+			 { 0, 14, 69, 7, 42, 3, 0 },
+			 { 0, 3, 42, 7, 69, 14, 0 },
+			 { 0, 16, 71, 7, 38, 3, 0 },
+			 { 0, 4, 46, 7, 66, 12, 0 },
+			 { 1, 19, 72, 7, 34, 2, 0 },
+			 { 0, 5, 50, 7, 63, 10, 0 },
+			 { 1, 22, 73, 7, 30, 2, 0 },
+			 { 0, 6, 54, 7, 60, 8, 0 },
+			 { 1, 25, 74, 7, 27, 1, 0 },
+			 { 0, 7, 58, 7, 56, 7, 0 },
+			 { 0, 1, 29, 7, 73, 24, 1 },
+			 { 0, 9, 61, 7, 52, 6, 0 },
+			 { 0, 2, 32, 7, 73, 20, 1 },
+			 { 0, 11, 65, 7, 48, 4, 0 },
+			 { 0, 2, 36, 7, 71, 18, 1 },
+			 { 0, 13, 67, 7, 44, 4, 0 },
+			 { 0, 3, 40, 7, 70, 15, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 8, 59, 7, 55, 6, 0 },
+			 { 0, 1, 30, 7, 73, 23, 1 },
+			 { 0, 9, 63, 7, 51, 5, 0 },
+			 { 0, 2, 33, 7, 72, 20, 1 },
+			 { 0, 11, 66, 7, 47, 4, 0 },
+			 { 0, 2, 37, 7, 71, 17, 1 },
+			 { 0, 13, 69, 7, 43, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 },
+			 { 0, 16, 70, 7, 39, 3, 0 },
+			 { 0, 4, 45, 7, 67, 12, 0 },
+			 { 1, 18, 72, 7, 35, 2, 0 },
+			 { 0, 5, 49, 7, 64, 10, 0 },
+			 { 1, 21, 73, 7, 31, 2, 0 },
+			 { 0, 6, 53, 7, 60, 9, 0 },
+			 { 1, 24, 74, 7, 28, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 },
+			 { 0, 1, 28, 7, 74, 24, 1 },
+			 { 0, 9, 60, 7, 53, 6, 0 },
+			 { 0, 2, 31, 7, 73, 21, 1 },
+			 { 0, 10, 64, 7, 49, 5, 0 },
+			 { 0, 2, 35, 7, 72, 18, 1 },
+			 { 0, 12, 67, 7, 45, 4, 0 },
+			 { 0, 3, 39, 7, 70, 16, 0 },
+			 { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 43, 7, 69, 13, 0 },
+			 { 1, 17, 71, 7, 37, 2, 0 },
+			 { 0, 4, 47, 7, 66, 11, 0 },
+			 { 1, 20, 72, 7, 33, 2, 0 },
+			 { 0, 5, 51, 7, 63, 9, 0 },
+			 { 1, 23, 73, 7, 30, 1, 0 },
+			 { 0, 6, 55, 7, 59, 8, 0 } },
+		.odd = { { 0, 15, 70, 7, 40, 3, 0 },
+			 { 0, 4, 44, 7, 67, 13, 0 },
+			 { 1, 18, 71, 7, 36, 2, 0 },
+			 { 0, 4, 48, 7, 65, 11, 0 },
+			 { 1, 20, 73, 7, 32, 2, 0 },
+			 { 0, 6, 52, 7, 61, 9, 0 },
+			 { 1, 24, 73, 7, 29, 1, 0 },
+			 { 0, 7, 56, 7, 58, 7, 0 },
+			 { 0, 1, 27, 7, 74, 25, 1 },
+			 { 0, 8, 60, 7, 54, 6, 0 },
+			 { 0, 2, 30, 7, 73, 22, 1 },
+			 { 0, 10, 63, 7, 50, 5, 0 },
+			 { 0, 2, 34, 7, 72, 19, 1 },
+			 { 0, 12, 66, 7, 46, 4, 0 },
+			 { 0, 3, 38, 7, 71, 16, 0 },
+			 { 0, 14, 69, 7, 42, 3, 0 },
+			 { 0, 3, 42, 7, 69, 14, 0 },
+			 { 0, 16, 71, 7, 38, 3, 0 },
+			 { 0, 4, 46, 7, 66, 12, 0 },
+			 { 1, 19, 72, 7, 34, 2, 0 },
+			 { 0, 5, 50, 7, 63, 10, 0 },
+			 { 1, 22, 73, 7, 30, 2, 0 },
+			 { 0, 6, 54, 7, 60, 8, 0 },
+			 { 1, 25, 74, 7, 27, 1, 0 },
+			 { 0, 7, 58, 7, 56, 7, 0 },
+			 { 0, 1, 29, 7, 73, 24, 1 },
+			 { 0, 9, 61, 7, 52, 6, 0 },
+			 { 0, 2, 32, 7, 73, 20, 1 },
+			 { 0, 11, 65, 7, 48, 4, 0 },
+			 { 0, 2, 36, 7, 71, 18, 1 },
+			 { 0, 13, 67, 7, 44, 4, 0 },
+			 { 0, 3, 40, 7, 70, 15, 0 } } },
+	.ptrn_arr = { { 0x84921249, 0x42492124, 0x24249092, 0x92124909,
+			 0x49212484, 0x24909248, 0x2490924 } },
+	.sample_patrn_length = 222,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 80) = 0.285714 */
+	.hor_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 } },
+		.odd = { { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 26, 74, 7, 26, 1, 0 },
+			 { 0, 7, 57, 7, 57, 7, 0 } },
+		.odd = { { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 } } },
+	.ptrn_arr = { { 0x249 } },
+	.sample_patrn_length = 14,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 81) = 0.283186 */
+	.hor_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 7, 54, 7, 59, 8, 0 },
+			 { 1, 23, 72, 7, 30, 2, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 20, 71, 7, 34, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 18, 69, 7, 37, 3, 0 },
+			 { 0, 4, 43, 7, 67, 14, 0 },
+			 { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 39, 7, 69, 16, 1 },
+			 { 0, 13, 66, 7, 45, 4, 0 },
+			 { 0, 2, 35, 7, 71, 19, 1 },
+			 { 0, 11, 63, 7, 49, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 9, 60, 7, 53, 6, 0 },
+			 { 0, 1, 28, 7, 73, 25, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 25, 73, 7, 28, 1, 0 },
+			 { 0, 6, 53, 7, 60, 9, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 49, 7, 63, 11, 0 },
+			 { 1, 19, 71, 7, 35, 2, 0 },
+			 { 0, 4, 45, 7, 66, 13, 0 },
+			 { 1, 16, 69, 7, 39, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 },
+			 { 0, 14, 67, 7, 43, 4, 0 },
+			 { 0, 3, 37, 7, 69, 18, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 34, 7, 71, 20, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 0, 2, 30, 7, 72, 23, 1 },
+			 { 0, 8, 59, 7, 54, 7, 0 } },
+		.odd = { { 0, 15, 67, 7, 42, 4, 0 },
+			 { 0, 3, 38, 7, 69, 17, 1 },
+			 { 0, 12, 66, 7, 46, 4, 0 },
+			 { 0, 2, 34, 7, 71, 20, 1 },
+			 { 0, 10, 63, 7, 50, 5, 0 },
+			 { 0, 2, 31, 7, 71, 23, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 0, 1, 27, 7, 73, 26, 1 },
+			 { 0, 7, 55, 7, 58, 8, 0 },
+			 { 1, 24, 72, 7, 29, 2, 0 },
+			 { 0, 6, 52, 7, 60, 10, 0 },
+			 { 1, 21, 71, 7, 33, 2, 0 },
+			 { 0, 5, 48, 7, 64, 11, 0 },
+			 { 1, 18, 70, 7, 36, 3, 0 },
+			 { 0, 4, 44, 7, 67, 13, 0 },
+			 { 0, 16, 69, 7, 40, 3, 0 },
+			 { 0, 3, 40, 7, 69, 16, 0 },
+			 { 0, 13, 67, 7, 44, 4, 0 },
+			 { 0, 3, 36, 7, 70, 18, 1 },
+			 { 0, 11, 64, 7, 48, 5, 0 },
+			 { 0, 2, 33, 7, 71, 21, 1 },
+			 { 0, 10, 60, 7, 52, 6, 0 },
+			 { 0, 2, 29, 7, 72, 24, 1 },
+			 { 0, 8, 58, 7, 55, 7, 0 },
+			 { 1, 26, 73, 7, 27, 1, 0 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 23, 71, 7, 31, 2, 0 },
+			 { 0, 5, 50, 7, 63, 10, 0 },
+			 { 1, 20, 71, 7, 34, 2, 0 },
+			 { 0, 4, 46, 7, 66, 12, 0 },
+			 { 1, 17, 69, 7, 38, 3, 0 },
+			 { 0, 4, 42, 7, 67, 15, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 7, 54, 7, 59, 8, 0 },
+			 { 1, 23, 72, 7, 30, 2, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 20, 71, 7, 34, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 18, 69, 7, 37, 3, 0 },
+			 { 0, 4, 43, 7, 67, 14, 0 },
+			 { 0, 15, 69, 7, 41, 3, 0 },
+			 { 0, 3, 39, 7, 69, 16, 1 },
+			 { 0, 13, 66, 7, 45, 4, 0 },
+			 { 0, 2, 35, 7, 71, 19, 1 },
+			 { 0, 11, 63, 7, 49, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 9, 60, 7, 53, 6, 0 },
+			 { 0, 1, 28, 7, 73, 25, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 25, 73, 7, 28, 1, 0 },
+			 { 0, 6, 53, 7, 60, 9, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 49, 7, 63, 11, 0 },
+			 { 1, 19, 71, 7, 35, 2, 0 },
+			 { 0, 4, 45, 7, 66, 13, 0 },
+			 { 1, 16, 69, 7, 39, 3, 0 },
+			 { 0, 3, 41, 7, 69, 15, 0 },
+			 { 0, 14, 67, 7, 43, 4, 0 },
+			 { 0, 3, 37, 7, 69, 18, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 34, 7, 71, 20, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 0, 2, 30, 7, 72, 23, 1 },
+			 { 0, 8, 59, 7, 54, 7, 0 } },
+		.odd = { { 0, 15, 67, 7, 42, 4, 0 },
+			 { 0, 3, 38, 7, 69, 17, 1 },
+			 { 0, 12, 66, 7, 46, 4, 0 },
+			 { 0, 2, 34, 7, 71, 20, 1 },
+			 { 0, 10, 63, 7, 50, 5, 0 },
+			 { 0, 2, 31, 7, 71, 23, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 0, 1, 27, 7, 73, 26, 1 },
+			 { 0, 7, 55, 7, 58, 8, 0 },
+			 { 1, 24, 72, 7, 29, 2, 0 },
+			 { 0, 6, 52, 7, 60, 10, 0 },
+			 { 1, 21, 71, 7, 33, 2, 0 },
+			 { 0, 5, 48, 7, 64, 11, 0 },
+			 { 1, 18, 70, 7, 36, 3, 0 },
+			 { 0, 4, 44, 7, 67, 13, 0 },
+			 { 0, 16, 69, 7, 40, 3, 0 },
+			 { 0, 3, 40, 7, 69, 16, 0 },
+			 { 0, 13, 67, 7, 44, 4, 0 },
+			 { 0, 3, 36, 7, 70, 18, 1 },
+			 { 0, 11, 64, 7, 48, 5, 0 },
+			 { 0, 2, 33, 7, 71, 21, 1 },
+			 { 0, 10, 60, 7, 52, 6, 0 },
+			 { 0, 2, 29, 7, 72, 24, 1 },
+			 { 0, 8, 58, 7, 55, 7, 0 },
+			 { 1, 26, 73, 7, 27, 1, 0 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 23, 71, 7, 31, 2, 0 },
+			 { 0, 5, 50, 7, 63, 10, 0 },
+			 { 1, 20, 71, 7, 34, 2, 0 },
+			 { 0, 4, 46, 7, 66, 12, 0 },
+			 { 1, 17, 69, 7, 38, 3, 0 },
+			 { 0, 4, 42, 7, 67, 15, 0 } } },
+	.ptrn_arr = { { 0x90924249, 0x49092424, 0x84921248, 0x49092124,
+			 0x24909242, 0x48492124, 0x24849212 } },
+	.sample_patrn_length = 226,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 82) = 0.280702 */
+	.hor_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 6, 52, 7, 61, 9, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 45, 7, 66, 13, 0 },
+			 { 0, 15, 68, 7, 41, 4, 0 },
+			 { 0, 3, 37, 7, 69, 18, 1 },
+			 { 0, 11, 63, 7, 49, 5, 0 },
+			 { 0, 2, 30, 7, 71, 24, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 24, 71, 7, 30, 2, 0 },
+			 { 0, 5, 49, 7, 63, 11, 0 },
+			 { 1, 18, 69, 7, 37, 3, 0 },
+			 { 0, 4, 41, 7, 68, 15, 0 },
+			 { 0, 13, 66, 7, 45, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 9, 61, 7, 52, 6, 0 } },
+		.odd = { { 0, 14, 67, 7, 43, 4, 0 },
+			 { 0, 3, 36, 7, 69, 19, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 0, 2, 28, 7, 72, 25, 1 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 17, 68, 7, 39, 3, 0 },
+			 { 0, 3, 39, 7, 68, 17, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 1, 25, 72, 7, 28, 2, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 19, 69, 7, 36, 3, 0 },
+			 { 0, 4, 43, 7, 67, 14, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 6, 52, 7, 61, 9, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 45, 7, 66, 13, 0 },
+			 { 0, 15, 68, 7, 41, 4, 0 },
+			 { 0, 3, 37, 7, 69, 18, 1 },
+			 { 0, 11, 63, 7, 49, 5, 0 },
+			 { 0, 2, 30, 7, 71, 24, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 24, 71, 7, 30, 2, 0 },
+			 { 0, 5, 49, 7, 63, 11, 0 },
+			 { 1, 18, 69, 7, 37, 3, 0 },
+			 { 0, 4, 41, 7, 68, 15, 0 },
+			 { 0, 13, 66, 7, 45, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 9, 61, 7, 52, 6, 0 } },
+		.odd = { { 0, 14, 67, 7, 43, 4, 0 },
+			 { 0, 3, 36, 7, 69, 19, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 0, 2, 28, 7, 72, 25, 1 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 17, 68, 7, 39, 3, 0 },
+			 { 0, 3, 39, 7, 68, 17, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 1, 25, 72, 7, 28, 2, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 19, 69, 7, 36, 3, 0 },
+			 { 0, 4, 43, 7, 67, 14, 0 } } },
+	.ptrn_arr = { { 0x90924249, 0x9212484, 0x92124249, 0x2484 } },
+	.sample_patrn_length = 114,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 83) = 0.278261 */
+	.hor_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 39, 7, 68, 17, 1 },
+			 { 0, 11, 62, 7, 49, 6, 0 },
+			 { 0, 2, 29, 7, 71, 25, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 19, 69, 7, 36, 3, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 30, 7, 71, 24, 1 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 43, 7, 66, 15, 0 },
+			 { 0, 13, 65, 7, 45, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 45, 7, 65, 13, 0 },
+			 { 0, 15, 66, 7, 43, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 1, 24, 71, 7, 30, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 3, 36, 7, 69, 19, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 25, 71, 7, 29, 2, 0 },
+			 { 0, 6, 49, 7, 62, 11, 0 },
+			 { 1, 17, 68, 7, 39, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 } },
+		.odd = { { 0, 14, 66, 7, 44, 4, 0 },
+			 { 0, 2, 33, 7, 70, 22, 1 },
+			 { 0, 8, 57, 7, 55, 8, 0 },
+			 { 1, 23, 71, 7, 31, 2, 0 },
+			 { 0, 5, 46, 7, 64, 13, 0 },
+			 { 0, 15, 67, 7, 42, 4, 0 },
+			 { 0, 3, 35, 7, 69, 20, 1 },
+			 { 0, 9, 59, 7, 53, 7, 0 },
+			 { 1, 25, 71, 7, 29, 2, 0 },
+			 { 0, 5, 48, 7, 63, 12, 0 },
+			 { 1, 16, 68, 7, 40, 3, 0 },
+			 { 0, 3, 37, 7, 68, 19, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 1, 26, 71, 7, 28, 2, 0 },
+			 { 0, 6, 50, 7, 61, 11, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 11, 61, 7, 50, 6, 0 },
+			 { 0, 2, 28, 7, 71, 26, 1 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 19, 68, 7, 37, 3, 0 },
+			 { 0, 3, 40, 7, 68, 16, 1 },
+			 { 0, 12, 63, 7, 48, 5, 0 },
+			 { 0, 2, 29, 7, 71, 25, 1 },
+			 { 0, 7, 53, 7, 59, 9, 0 },
+			 { 1, 20, 69, 7, 35, 3, 0 },
+			 { 0, 4, 42, 7, 67, 15, 0 },
+			 { 0, 13, 64, 7, 46, 5, 0 },
+			 { 0, 2, 31, 7, 71, 23, 1 },
+			 { 0, 8, 55, 7, 57, 8, 0 },
+			 { 1, 22, 70, 7, 33, 2, 0 },
+			 { 0, 4, 44, 7, 66, 14, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 1, 27, 72, 7, 27, 1, 0 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 39, 7, 68, 17, 1 },
+			 { 0, 11, 62, 7, 49, 6, 0 },
+			 { 0, 2, 29, 7, 71, 25, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 19, 69, 7, 36, 3, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 12, 64, 7, 47, 5, 0 },
+			 { 0, 2, 30, 7, 71, 24, 1 },
+			 { 0, 7, 54, 7, 58, 9, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 43, 7, 66, 15, 0 },
+			 { 0, 13, 65, 7, 45, 5, 0 },
+			 { 0, 2, 32, 7, 71, 22, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 22, 71, 7, 32, 2, 0 },
+			 { 0, 5, 45, 7, 65, 13, 0 },
+			 { 0, 15, 66, 7, 43, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 9, 58, 7, 54, 7, 0 },
+			 { 1, 24, 71, 7, 30, 2, 0 },
+			 { 0, 5, 47, 7, 64, 12, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 3, 36, 7, 69, 19, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 25, 71, 7, 29, 2, 0 },
+			 { 0, 6, 49, 7, 62, 11, 0 },
+			 { 1, 17, 68, 7, 39, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 } },
+		.odd = { { 0, 14, 66, 7, 44, 4, 0 },
+			 { 0, 2, 33, 7, 70, 22, 1 },
+			 { 0, 8, 57, 7, 55, 8, 0 },
+			 { 1, 23, 71, 7, 31, 2, 0 },
+			 { 0, 5, 46, 7, 64, 13, 0 },
+			 { 0, 15, 67, 7, 42, 4, 0 },
+			 { 0, 3, 35, 7, 69, 20, 1 },
+			 { 0, 9, 59, 7, 53, 7, 0 },
+			 { 1, 25, 71, 7, 29, 2, 0 },
+			 { 0, 5, 48, 7, 63, 12, 0 },
+			 { 1, 16, 68, 7, 40, 3, 0 },
+			 { 0, 3, 37, 7, 68, 19, 1 },
+			 { 0, 10, 61, 7, 51, 6, 0 },
+			 { 1, 26, 71, 7, 28, 2, 0 },
+			 { 0, 6, 50, 7, 61, 11, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 11, 61, 7, 50, 6, 0 },
+			 { 0, 2, 28, 7, 71, 26, 1 },
+			 { 0, 6, 51, 7, 61, 10, 0 },
+			 { 1, 19, 68, 7, 37, 3, 0 },
+			 { 0, 3, 40, 7, 68, 16, 1 },
+			 { 0, 12, 63, 7, 48, 5, 0 },
+			 { 0, 2, 29, 7, 71, 25, 1 },
+			 { 0, 7, 53, 7, 59, 9, 0 },
+			 { 1, 20, 69, 7, 35, 3, 0 },
+			 { 0, 4, 42, 7, 67, 15, 0 },
+			 { 0, 13, 64, 7, 46, 5, 0 },
+			 { 0, 2, 31, 7, 71, 23, 1 },
+			 { 0, 8, 55, 7, 57, 8, 0 },
+			 { 1, 22, 70, 7, 33, 2, 0 },
+			 { 0, 4, 44, 7, 66, 14, 0 } } },
+	.ptrn_arr = { { 0x92124249, 0x21242484, 0x12424849, 0x24248492,
+			 0x42484909, 0x24849092, 0x48490924, 0x2 } },
+	.sample_patrn_length = 230,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 84) = 0.275862 */
+	.hor_phase_arr = {
+		.even = { { 2, 27, 70, 7, 27, 2, 0 },
+			 { 0, 6, 49, 7, 61, 12, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 12, 61, 7, 49, 6, 0 } },
+		.odd = { { 0, 14, 64, 7, 45, 5, 0 },
+			 { 0, 2, 31, 7, 70, 24, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 24, 70, 7, 31, 2, 0 },
+			 { 0, 5, 45, 7, 64, 14, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 27, 70, 7, 27, 2, 0 },
+			 { 0, 6, 49, 7, 61, 12, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 34, 7, 70, 21, 1 },
+			 { 0, 8, 56, 7, 56, 8, 0 },
+			 { 1, 21, 70, 7, 34, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 12, 61, 7, 49, 6, 0 } },
+		.odd = { { 0, 14, 64, 7, 45, 5, 0 },
+			 { 0, 2, 31, 7, 70, 24, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 18, 68, 7, 38, 3, 0 },
+			 { 0, 3, 38, 7, 68, 18, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 24, 70, 7, 31, 2, 0 },
+			 { 0, 5, 45, 7, 64, 14, 0 } } },
+	.ptrn_arr = { { 0x92124249, 0x248490 } },
+	.sample_patrn_length = 58,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 85) = 0.273504 */
+	.hor_phase_arr = {
+		.even = { { 2, 27, 70, 7, 27, 2, 0 },
+			 { 0, 5, 47, 7, 63, 13, 0 },
+			 { 0, 14, 64, 7, 45, 5, 0 },
+			 { 0, 2, 29, 7, 70, 26, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 15, 65, 7, 43, 4, 0 },
+			 { 0, 2, 31, 7, 70, 24, 1 },
+			 { 0, 6, 50, 7, 61, 11, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 32, 7, 70, 23, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 17, 67, 7, 39, 4, 0 },
+			 { 0, 3, 34, 7, 69, 21, 1 },
+			 { 0, 8, 54, 7, 57, 9, 0 },
+			 { 1, 19, 67, 7, 38, 3, 0 },
+			 { 0, 3, 36, 7, 68, 20, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 20, 68, 7, 36, 3, 0 },
+			 { 0, 3, 38, 7, 67, 19, 1 },
+			 { 0, 9, 57, 7, 54, 8, 0 },
+			 { 1, 21, 69, 7, 34, 3, 0 },
+			 { 0, 4, 39, 7, 67, 17, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 23, 70, 7, 32, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 11, 61, 7, 50, 6, 0 },
+			 { 1, 24, 70, 7, 31, 2, 0 },
+			 { 0, 4, 43, 7, 65, 15, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 26, 70, 7, 29, 2, 0 },
+			 { 0, 5, 45, 7, 64, 14, 0 },
+			 { 0, 13, 63, 7, 47, 5, 0 } },
+		.odd = { { 0, 13, 64, 7, 46, 5, 0 },
+			 { 0, 2, 28, 7, 69, 27, 2 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 14, 64, 7, 44, 5, 0 },
+			 { 0, 2, 30, 7, 70, 25, 1 },
+			 { 0, 6, 49, 7, 62, 11, 0 },
+			 { 1, 16, 65, 7, 42, 4, 0 },
+			 { 0, 2, 32, 7, 69, 24, 1 },
+			 { 0, 7, 51, 7, 59, 11, 0 },
+			 { 1, 17, 66, 7, 40, 4, 0 },
+			 { 0, 2, 33, 7, 70, 22, 1 },
+			 { 0, 7, 53, 7, 58, 10, 0 },
+			 { 1, 18, 67, 7, 39, 3, 0 },
+			 { 0, 3, 35, 7, 68, 21, 1 },
+			 { 0, 8, 54, 7, 57, 9, 0 },
+			 { 1, 19, 68, 7, 37, 3, 0 },
+			 { 0, 3, 37, 7, 68, 19, 1 },
+			 { 0, 9, 57, 7, 54, 8, 0 },
+			 { 1, 21, 68, 7, 35, 3, 0 },
+			 { 0, 3, 39, 7, 67, 18, 1 },
+			 { 0, 10, 58, 7, 53, 7, 0 },
+			 { 1, 22, 70, 7, 33, 2, 0 },
+			 { 0, 4, 40, 7, 66, 17, 1 },
+			 { 0, 11, 59, 7, 51, 7, 0 },
+			 { 1, 24, 69, 7, 32, 2, 0 },
+			 { 0, 4, 42, 7, 65, 16, 1 },
+			 { 0, 11, 62, 7, 49, 6, 0 },
+			 { 1, 25, 70, 7, 30, 2, 0 },
+			 { 0, 5, 44, 7, 64, 14, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 2, 27, 69, 7, 28, 2, 0 },
+			 { 0, 5, 46, 7, 64, 13, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 27, 70, 7, 27, 2, 0 },
+			 { 0, 5, 47, 7, 63, 13, 0 },
+			 { 0, 14, 64, 7, 45, 5, 0 },
+			 { 0, 2, 29, 7, 70, 26, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 15, 65, 7, 43, 4, 0 },
+			 { 0, 2, 31, 7, 70, 24, 1 },
+			 { 0, 6, 50, 7, 61, 11, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 32, 7, 70, 23, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 17, 67, 7, 39, 4, 0 },
+			 { 0, 3, 34, 7, 69, 21, 1 },
+			 { 0, 8, 54, 7, 57, 9, 0 },
+			 { 1, 19, 67, 7, 38, 3, 0 },
+			 { 0, 3, 36, 7, 68, 20, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 20, 68, 7, 36, 3, 0 },
+			 { 0, 3, 38, 7, 67, 19, 1 },
+			 { 0, 9, 57, 7, 54, 8, 0 },
+			 { 1, 21, 69, 7, 34, 3, 0 },
+			 { 0, 4, 39, 7, 67, 17, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 23, 70, 7, 32, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 11, 61, 7, 50, 6, 0 },
+			 { 1, 24, 70, 7, 31, 2, 0 },
+			 { 0, 4, 43, 7, 65, 15, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 26, 70, 7, 29, 2, 0 },
+			 { 0, 5, 45, 7, 64, 14, 0 },
+			 { 0, 13, 63, 7, 47, 5, 0 } },
+		.odd = { { 0, 13, 64, 7, 46, 5, 0 },
+			 { 0, 2, 28, 7, 69, 27, 2 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 14, 64, 7, 44, 5, 0 },
+			 { 0, 2, 30, 7, 70, 25, 1 },
+			 { 0, 6, 49, 7, 62, 11, 0 },
+			 { 1, 16, 65, 7, 42, 4, 0 },
+			 { 0, 2, 32, 7, 69, 24, 1 },
+			 { 0, 7, 51, 7, 59, 11, 0 },
+			 { 1, 17, 66, 7, 40, 4, 0 },
+			 { 0, 2, 33, 7, 70, 22, 1 },
+			 { 0, 7, 53, 7, 58, 10, 0 },
+			 { 1, 18, 67, 7, 39, 3, 0 },
+			 { 0, 3, 35, 7, 68, 21, 1 },
+			 { 0, 8, 54, 7, 57, 9, 0 },
+			 { 1, 19, 68, 7, 37, 3, 0 },
+			 { 0, 3, 37, 7, 68, 19, 1 },
+			 { 0, 9, 57, 7, 54, 8, 0 },
+			 { 1, 21, 68, 7, 35, 3, 0 },
+			 { 0, 3, 39, 7, 67, 18, 1 },
+			 { 0, 10, 58, 7, 53, 7, 0 },
+			 { 1, 22, 70, 7, 33, 2, 0 },
+			 { 0, 4, 40, 7, 66, 17, 1 },
+			 { 0, 11, 59, 7, 51, 7, 0 },
+			 { 1, 24, 69, 7, 32, 2, 0 },
+			 { 0, 4, 42, 7, 65, 16, 1 },
+			 { 0, 11, 62, 7, 49, 6, 0 },
+			 { 1, 25, 70, 7, 30, 2, 0 },
+			 { 0, 5, 44, 7, 64, 14, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 2, 27, 69, 7, 28, 2, 0 },
+			 { 0, 5, 46, 7, 64, 13, 0 } } },
+	.ptrn_arr = { { 0x92124249, 0x24248490, 0x48490921, 0x90921242,
+			 0x21242484, 0x42484909, 0x84909212, 0x24 } },
+	.sample_patrn_length = 234,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 86) = 0.271186 */
+	.hor_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 5, 45, 7, 63, 14, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 25, 69, 7, 31, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 38, 7, 67, 19, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 19, 67, 7, 38, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 31, 7, 69, 25, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 14, 63, 7, 45, 5, 0 } },
+		.odd = { { 0, 13, 62, 7, 47, 6, 0 },
+			 { 2, 26, 69, 7, 29, 2, 0 },
+			 { 0, 5, 43, 7, 64, 15, 1 },
+			 { 0, 11, 60, 7, 50, 7, 0 },
+			 { 1, 23, 69, 7, 33, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 10, 57, 7, 53, 8, 0 },
+			 { 1, 20, 68, 7, 36, 3, 0 },
+			 { 0, 3, 36, 7, 68, 20, 1 },
+			 { 0, 8, 53, 7, 57, 10, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 0, 2, 33, 7, 69, 23, 1 },
+			 { 0, 7, 50, 7, 60, 11, 0 },
+			 { 1, 15, 64, 7, 43, 5, 0 },
+			 { 0, 2, 29, 7, 69, 26, 2 },
+			 { 0, 6, 47, 7, 62, 13, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 5, 45, 7, 63, 14, 1 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 25, 69, 7, 31, 2, 0 },
+			 { 0, 4, 41, 7, 66, 16, 1 },
+			 { 0, 10, 59, 7, 52, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 38, 7, 67, 19, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 19, 67, 7, 38, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 52, 7, 59, 10, 0 },
+			 { 1, 16, 66, 7, 41, 4, 0 },
+			 { 0, 2, 31, 7, 69, 25, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 1, 14, 63, 7, 45, 5, 0 } },
+		.odd = { { 0, 13, 62, 7, 47, 6, 0 },
+			 { 2, 26, 69, 7, 29, 2, 0 },
+			 { 0, 5, 43, 7, 64, 15, 1 },
+			 { 0, 11, 60, 7, 50, 7, 0 },
+			 { 1, 23, 69, 7, 33, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 10, 57, 7, 53, 8, 0 },
+			 { 1, 20, 68, 7, 36, 3, 0 },
+			 { 0, 3, 36, 7, 68, 20, 1 },
+			 { 0, 8, 53, 7, 57, 10, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 0, 2, 33, 7, 69, 23, 1 },
+			 { 0, 7, 50, 7, 60, 11, 0 },
+			 { 1, 15, 64, 7, 43, 5, 0 },
+			 { 0, 2, 29, 7, 69, 26, 2 },
+			 { 0, 6, 47, 7, 62, 13, 0 } } },
+	.ptrn_arr = { { 0x12424849, 0x24849092, 0x49092124, 0x24248 } },
+	.sample_patrn_length = 118,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 87) = 0.268908 */
+	.hor_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 11, 57, 7, 52, 8, 0 },
+			 { 1, 21, 67, 7, 36, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 50, 7, 60, 11, 0 },
+			 { 1, 14, 63, 7, 45, 5, 0 },
+			 { 2, 26, 69, 7, 29, 2, 0 },
+			 { 0, 4, 41, 7, 65, 17, 1 },
+			 { 0, 10, 57, 7, 53, 8, 0 },
+			 { 1, 19, 66, 7, 38, 4, 0 },
+			 { 0, 3, 33, 7, 68, 23, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 0, 13, 62, 7, 47, 6, 0 },
+			 { 1, 25, 69, 7, 31, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 0, 2, 31, 7, 69, 25, 1 },
+			 { 0, 6, 47, 7, 62, 13, 0 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 23, 68, 7, 33, 3, 0 },
+			 { 0, 4, 38, 7, 66, 19, 1 },
+			 { 0, 8, 53, 7, 57, 10, 0 },
+			 { 1, 17, 65, 7, 41, 4, 0 },
+			 { 0, 2, 29, 7, 69, 26, 2 },
+			 { 0, 5, 45, 7, 63, 14, 1 },
+			 { 0, 11, 60, 7, 50, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 36, 7, 67, 21, 1 },
+			 { 0, 8, 52, 7, 57, 11, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 } },
+		.odd = { { 0, 13, 62, 7, 47, 6, 0 },
+			 { 1, 24, 69, 7, 32, 2, 0 },
+			 { 0, 4, 39, 7, 65, 19, 1 },
+			 { 0, 9, 54, 7, 56, 9, 0 },
+			 { 1, 17, 66, 7, 40, 4, 0 },
+			 { 0, 2, 30, 7, 69, 25, 2 },
+			 { 0, 5, 46, 7, 62, 14, 1 },
+			 { 0, 12, 60, 7, 49, 7, 0 },
+			 { 1, 23, 67, 7, 34, 3, 0 },
+			 { 0, 3, 37, 7, 67, 20, 1 },
+			 { 0, 8, 52, 7, 58, 10, 0 },
+			 { 1, 16, 64, 7, 42, 5, 0 },
+			 { 0, 2, 29, 7, 68, 27, 2 },
+			 { 0, 5, 44, 7, 63, 15, 1 },
+			 { 0, 11, 59, 7, 51, 7, 0 },
+			 { 1, 21, 68, 7, 35, 3, 0 },
+			 { 0, 3, 35, 7, 68, 21, 1 },
+			 { 0, 7, 51, 7, 59, 11, 0 },
+			 { 1, 15, 63, 7, 44, 5, 0 },
+			 { 2, 27, 68, 7, 29, 2, 0 },
+			 { 0, 5, 42, 7, 64, 16, 1 },
+			 { 0, 10, 58, 7, 52, 8, 0 },
+			 { 1, 20, 67, 7, 37, 3, 0 },
+			 { 0, 3, 34, 7, 67, 23, 1 },
+			 { 0, 7, 49, 7, 60, 12, 0 },
+			 { 1, 14, 62, 7, 46, 5, 0 },
+			 { 2, 25, 69, 7, 30, 2, 0 },
+			 { 0, 4, 40, 7, 66, 17, 1 },
+			 { 0, 9, 56, 7, 54, 9, 0 },
+			 { 1, 19, 65, 7, 39, 4, 0 },
+			 { 0, 2, 32, 7, 69, 24, 1 },
+			 { 0, 6, 47, 7, 62, 13, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 11, 57, 7, 52, 8, 0 },
+			 { 1, 21, 67, 7, 36, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 50, 7, 60, 11, 0 },
+			 { 1, 14, 63, 7, 45, 5, 0 },
+			 { 2, 26, 69, 7, 29, 2, 0 },
+			 { 0, 4, 41, 7, 65, 17, 1 },
+			 { 0, 10, 57, 7, 53, 8, 0 },
+			 { 1, 19, 66, 7, 38, 4, 0 },
+			 { 0, 3, 33, 7, 68, 23, 1 },
+			 { 0, 6, 48, 7, 62, 12, 0 },
+			 { 0, 13, 62, 7, 47, 6, 0 },
+			 { 1, 25, 69, 7, 31, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 0, 2, 31, 7, 69, 25, 1 },
+			 { 0, 6, 47, 7, 62, 13, 0 },
+			 { 0, 12, 62, 7, 48, 6, 0 },
+			 { 1, 23, 68, 7, 33, 3, 0 },
+			 { 0, 4, 38, 7, 66, 19, 1 },
+			 { 0, 8, 53, 7, 57, 10, 0 },
+			 { 1, 17, 65, 7, 41, 4, 0 },
+			 { 0, 2, 29, 7, 69, 26, 2 },
+			 { 0, 5, 45, 7, 63, 14, 1 },
+			 { 0, 11, 60, 7, 50, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 36, 7, 67, 21, 1 },
+			 { 0, 8, 52, 7, 57, 11, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 } },
+		.odd = { { 0, 13, 62, 7, 47, 6, 0 },
+			 { 1, 24, 69, 7, 32, 2, 0 },
+			 { 0, 4, 39, 7, 65, 19, 1 },
+			 { 0, 9, 54, 7, 56, 9, 0 },
+			 { 1, 17, 66, 7, 40, 4, 0 },
+			 { 0, 2, 30, 7, 69, 25, 2 },
+			 { 0, 5, 46, 7, 62, 14, 1 },
+			 { 0, 12, 60, 7, 49, 7, 0 },
+			 { 1, 23, 67, 7, 34, 3, 0 },
+			 { 0, 3, 37, 7, 67, 20, 1 },
+			 { 0, 8, 52, 7, 58, 10, 0 },
+			 { 1, 16, 64, 7, 42, 5, 0 },
+			 { 0, 2, 29, 7, 68, 27, 2 },
+			 { 0, 5, 44, 7, 63, 15, 1 },
+			 { 0, 11, 59, 7, 51, 7, 0 },
+			 { 1, 21, 68, 7, 35, 3, 0 },
+			 { 0, 3, 35, 7, 68, 21, 1 },
+			 { 0, 7, 51, 7, 59, 11, 0 },
+			 { 1, 15, 63, 7, 44, 5, 0 },
+			 { 2, 27, 68, 7, 29, 2, 0 },
+			 { 0, 5, 42, 7, 64, 16, 1 },
+			 { 0, 10, 58, 7, 52, 8, 0 },
+			 { 1, 20, 67, 7, 37, 3, 0 },
+			 { 0, 3, 34, 7, 67, 23, 1 },
+			 { 0, 7, 49, 7, 60, 12, 0 },
+			 { 1, 14, 62, 7, 46, 5, 0 },
+			 { 2, 25, 69, 7, 30, 2, 0 },
+			 { 0, 4, 40, 7, 66, 17, 1 },
+			 { 0, 9, 56, 7, 54, 9, 0 },
+			 { 1, 19, 65, 7, 39, 4, 0 },
+			 { 0, 2, 32, 7, 69, 24, 1 },
+			 { 0, 6, 47, 7, 62, 13, 0 } } },
+	.ptrn_arr = { { 0x12424849, 0x84909092, 0x9212424, 0x42484909,
+			 0x90921212, 0x21242484, 0x48490921, 0x242 } },
+	.sample_patrn_length = 238,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 88) = 0.266667 */
+	.hor_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 41, 7, 65, 17, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 17, 65, 7, 41, 4, 0 } },
+		.odd = { { 0, 13, 60, 7, 48, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 48, 7, 60, 13, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 41, 7, 65, 17, 1 },
+			 { 0, 9, 55, 7, 55, 9, 0 },
+			 { 1, 17, 65, 7, 41, 4, 0 } },
+		.odd = { { 0, 13, 60, 7, 48, 7, 0 },
+			 { 1, 22, 68, 7, 34, 3, 0 },
+			 { 0, 3, 34, 7, 68, 22, 1 },
+			 { 0, 7, 48, 7, 60, 13, 0 } } },
+	.ptrn_arr = { { 0x2424849 } },
+	.sample_patrn_length = 30,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 89) = 0.264463 */
+	.hor_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 8, 51, 7, 58, 11, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 22, 67, 7, 35, 3, 0 },
+			 { 0, 3, 33, 7, 67, 24, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 56, 7, 53, 9, 0 },
+			 { 1, 17, 64, 7, 41, 5, 0 },
+			 { 2, 27, 67, 7, 30, 2, 0 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 50, 7, 59, 12, 0 },
+			 { 0, 13, 60, 7, 48, 7, 0 },
+			 { 1, 21, 67, 7, 36, 3, 0 },
+			 { 0, 3, 31, 7, 67, 25, 2 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 9, 56, 7, 54, 9, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 },
+			 { 2, 25, 67, 7, 31, 3, 0 },
+			 { 0, 3, 36, 7, 67, 21, 1 },
+			 { 0, 7, 48, 7, 60, 13, 0 },
+			 { 0, 12, 59, 7, 50, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 0, 2, 30, 7, 67, 27, 2 },
+			 { 0, 5, 41, 7, 64, 17, 1 },
+			 { 0, 9, 53, 7, 56, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 24, 67, 7, 33, 3, 0 },
+			 { 0, 3, 35, 7, 67, 22, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 11, 58, 7, 51, 8, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 } },
+		.odd = { { 0, 12, 60, 7, 49, 7, 0 },
+			 { 1, 20, 66, 7, 37, 4, 0 },
+			 { 0, 2, 31, 7, 67, 26, 2 },
+			 { 0, 5, 42, 7, 63, 17, 1 },
+			 { 0, 9, 54, 7, 55, 10, 0 },
+			 { 1, 16, 62, 7, 44, 5, 0 },
+			 { 2, 24, 67, 7, 32, 3, 0 },
+			 { 0, 3, 35, 7, 67, 22, 1 },
+			 { 0, 6, 47, 7, 61, 13, 1 },
+			 { 0, 12, 58, 7, 50, 8, 0 },
+			 { 1, 19, 65, 7, 39, 4, 0 },
+			 { 0, 2, 29, 7, 68, 27, 2 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 8, 52, 7, 57, 11, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 23, 67, 7, 34, 3, 0 },
+			 { 0, 3, 34, 7, 67, 23, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 11, 57, 7, 52, 8, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 2, 27, 68, 7, 29, 2, 0 },
+			 { 0, 4, 39, 7, 65, 19, 1 },
+			 { 0, 8, 50, 7, 58, 12, 0 },
+			 { 1, 13, 61, 7, 47, 6, 0 },
+			 { 1, 22, 67, 7, 35, 3, 0 },
+			 { 0, 3, 32, 7, 67, 24, 2 },
+			 { 0, 5, 44, 7, 62, 16, 1 },
+			 { 0, 10, 55, 7, 54, 9, 0 },
+			 { 1, 17, 63, 7, 42, 5, 0 },
+			 { 2, 26, 67, 7, 31, 2, 0 },
+			 { 0, 4, 37, 7, 66, 20, 1 },
+			 { 0, 7, 49, 7, 60, 12, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 8, 51, 7, 58, 11, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 22, 67, 7, 35, 3, 0 },
+			 { 0, 3, 33, 7, 67, 24, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 56, 7, 53, 9, 0 },
+			 { 1, 17, 64, 7, 41, 5, 0 },
+			 { 2, 27, 67, 7, 30, 2, 0 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 50, 7, 59, 12, 0 },
+			 { 0, 13, 60, 7, 48, 7, 0 },
+			 { 1, 21, 67, 7, 36, 3, 0 },
+			 { 0, 3, 31, 7, 67, 25, 2 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 9, 56, 7, 54, 9, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 },
+			 { 2, 25, 67, 7, 31, 3, 0 },
+			 { 0, 3, 36, 7, 67, 21, 1 },
+			 { 0, 7, 48, 7, 60, 13, 0 },
+			 { 0, 12, 59, 7, 50, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 0, 2, 30, 7, 67, 27, 2 },
+			 { 0, 5, 41, 7, 64, 17, 1 },
+			 { 0, 9, 53, 7, 56, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 24, 67, 7, 33, 3, 0 },
+			 { 0, 3, 35, 7, 67, 22, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 11, 58, 7, 51, 8, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 } },
+		.odd = { { 0, 12, 60, 7, 49, 7, 0 },
+			 { 1, 20, 66, 7, 37, 4, 0 },
+			 { 0, 2, 31, 7, 67, 26, 2 },
+			 { 0, 5, 42, 7, 63, 17, 1 },
+			 { 0, 9, 54, 7, 55, 10, 0 },
+			 { 1, 16, 62, 7, 44, 5, 0 },
+			 { 2, 24, 67, 7, 32, 3, 0 },
+			 { 0, 3, 35, 7, 67, 22, 1 },
+			 { 0, 6, 47, 7, 61, 13, 1 },
+			 { 0, 12, 58, 7, 50, 8, 0 },
+			 { 1, 19, 65, 7, 39, 4, 0 },
+			 { 0, 2, 29, 7, 68, 27, 2 },
+			 { 0, 4, 40, 7, 65, 18, 1 },
+			 { 0, 8, 52, 7, 57, 11, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 23, 67, 7, 34, 3, 0 },
+			 { 0, 3, 34, 7, 67, 23, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 11, 57, 7, 52, 8, 0 },
+			 { 1, 18, 65, 7, 40, 4, 0 },
+			 { 2, 27, 68, 7, 29, 2, 0 },
+			 { 0, 4, 39, 7, 65, 19, 1 },
+			 { 0, 8, 50, 7, 58, 12, 0 },
+			 { 1, 13, 61, 7, 47, 6, 0 },
+			 { 1, 22, 67, 7, 35, 3, 0 },
+			 { 0, 3, 32, 7, 67, 24, 2 },
+			 { 0, 5, 44, 7, 62, 16, 1 },
+			 { 0, 10, 55, 7, 54, 9, 0 },
+			 { 1, 17, 63, 7, 42, 5, 0 },
+			 { 2, 26, 67, 7, 31, 2, 0 },
+			 { 0, 4, 37, 7, 66, 20, 1 },
+			 { 0, 7, 49, 7, 60, 12, 0 } } },
+	.ptrn_arr = { { 0x42424849, 0x90921212, 0x24248490, 0x9212124,
+			 0x48484909, 0x92121242, 0x84849090, 0x2424 } },
+	.sample_patrn_length = 242,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 90) = 0.262295 */
+	.hor_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 48, 7, 59, 13, 1 },
+			 { 0, 11, 58, 7, 51, 8, 0 },
+			 { 1, 17, 64, 7, 41, 5, 0 },
+			 { 2, 25, 67, 7, 31, 3, 0 },
+			 { 0, 3, 35, 7, 66, 23, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 23, 66, 7, 35, 3, 0 },
+			 { 0, 3, 31, 7, 67, 25, 2 },
+			 { 0, 5, 41, 7, 64, 17, 1 },
+			 { 0, 8, 51, 7, 58, 11, 0 },
+			 { 1, 13, 59, 7, 48, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 } },
+		.odd = { { 0, 12, 59, 7, 49, 8, 0 },
+			 { 1, 19, 64, 7, 40, 4, 0 },
+			 { 2, 27, 67, 7, 30, 2, 0 },
+			 { 0, 4, 36, 7, 66, 21, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 10, 56, 7, 53, 9, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 },
+			 { 2, 24, 66, 7, 33, 3, 0 },
+			 { 0, 3, 33, 7, 66, 24, 2 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 9, 53, 7, 56, 10, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 21, 66, 7, 36, 4, 0 },
+			 { 0, 2, 30, 7, 67, 27, 2 },
+			 { 0, 4, 40, 7, 64, 19, 1 },
+			 { 0, 8, 49, 7, 59, 12, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 28, 68, 7, 28, 2, 0 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 48, 7, 59, 13, 1 },
+			 { 0, 11, 58, 7, 51, 8, 0 },
+			 { 1, 17, 64, 7, 41, 5, 0 },
+			 { 2, 25, 67, 7, 31, 3, 0 },
+			 { 0, 3, 35, 7, 66, 23, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 23, 66, 7, 35, 3, 0 },
+			 { 0, 3, 31, 7, 67, 25, 2 },
+			 { 0, 5, 41, 7, 64, 17, 1 },
+			 { 0, 8, 51, 7, 58, 11, 0 },
+			 { 1, 13, 59, 7, 48, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 } },
+		.odd = { { 0, 12, 59, 7, 49, 8, 0 },
+			 { 1, 19, 64, 7, 40, 4, 0 },
+			 { 2, 27, 67, 7, 30, 2, 0 },
+			 { 0, 4, 36, 7, 66, 21, 1 },
+			 { 0, 6, 46, 7, 61, 14, 1 },
+			 { 0, 10, 56, 7, 53, 9, 0 },
+			 { 1, 16, 63, 7, 43, 5, 0 },
+			 { 2, 24, 66, 7, 33, 3, 0 },
+			 { 0, 3, 33, 7, 66, 24, 2 },
+			 { 0, 5, 43, 7, 63, 16, 1 },
+			 { 0, 9, 53, 7, 56, 10, 0 },
+			 { 1, 14, 61, 7, 46, 6, 0 },
+			 { 1, 21, 66, 7, 36, 4, 0 },
+			 { 0, 2, 30, 7, 67, 27, 2 },
+			 { 0, 4, 40, 7, 64, 19, 1 },
+			 { 0, 8, 49, 7, 59, 12, 0 } } },
+	.ptrn_arr = { { 0x42484849, 0x92121242, 0x84849090, 0x242424 } },
+	.sample_patrn_length = 122,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 91) = 0.260163 */
+	.hor_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 4, 36, 7, 66, 21, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 9, 52, 7, 56, 11, 0 },
+			 { 1, 13, 59, 7, 48, 7, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 33, 7, 66, 24, 2 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 8, 49, 7, 59, 12, 0 },
+			 { 0, 12, 57, 7, 51, 8, 0 },
+			 { 1, 17, 62, 7, 43, 5, 0 },
+			 { 1, 23, 66, 7, 35, 3, 0 },
+			 { 0, 3, 30, 7, 66, 27, 2 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 46, 7, 60, 14, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 14, 60, 7, 46, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 2, 27, 66, 7, 30, 3, 0 },
+			 { 0, 3, 35, 7, 66, 23, 1 },
+			 { 0, 5, 43, 7, 62, 17, 1 },
+			 { 0, 8, 51, 7, 57, 12, 0 },
+			 { 0, 12, 59, 7, 49, 8, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 2, 24, 66, 7, 33, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 7, 48, 7, 59, 13, 1 },
+			 { 0, 11, 56, 7, 52, 9, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 21, 66, 7, 36, 4, 0 } },
+		.odd = { { 0, 12, 58, 7, 50, 8, 0 },
+			 { 1, 17, 63, 7, 42, 5, 0 },
+			 { 2, 23, 66, 7, 34, 3, 0 },
+			 { 0, 3, 31, 7, 66, 26, 2 },
+			 { 0, 4, 39, 7, 64, 20, 1 },
+			 { 0, 7, 47, 7, 59, 14, 1 },
+			 { 0, 10, 55, 7, 53, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 21, 65, 7, 37, 4, 0 },
+			 { 2, 28, 67, 7, 29, 2, 0 },
+			 { 0, 4, 36, 7, 65, 22, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 9, 52, 7, 56, 11, 0 },
+			 { 1, 13, 58, 7, 49, 7, 0 },
+			 { 1, 18, 64, 7, 40, 5, 0 },
+			 { 2, 25, 66, 7, 32, 3, 0 },
+			 { 0, 3, 32, 7, 66, 25, 2 },
+			 { 0, 5, 40, 7, 64, 18, 1 },
+			 { 0, 7, 49, 7, 58, 13, 1 },
+			 { 0, 11, 56, 7, 52, 9, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 22, 65, 7, 36, 4, 0 },
+			 { 0, 2, 29, 7, 67, 28, 2 },
+			 { 0, 4, 37, 7, 65, 21, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 53, 7, 55, 10, 0 },
+			 { 1, 14, 59, 7, 47, 7, 0 },
+			 { 1, 20, 64, 7, 39, 4, 0 },
+			 { 2, 26, 66, 7, 31, 3, 0 },
+			 { 0, 3, 34, 7, 66, 23, 2 },
+			 { 0, 5, 42, 7, 63, 17, 1 },
+			 { 0, 8, 50, 7, 58, 12, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 4, 36, 7, 66, 21, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 9, 52, 7, 56, 11, 0 },
+			 { 1, 13, 59, 7, 48, 7, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 33, 7, 66, 24, 2 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 8, 49, 7, 59, 12, 0 },
+			 { 0, 12, 57, 7, 51, 8, 0 },
+			 { 1, 17, 62, 7, 43, 5, 0 },
+			 { 1, 23, 66, 7, 35, 3, 0 },
+			 { 0, 3, 30, 7, 66, 27, 2 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 7, 46, 7, 60, 14, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 14, 60, 7, 46, 7, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 2, 27, 66, 7, 30, 3, 0 },
+			 { 0, 3, 35, 7, 66, 23, 1 },
+			 { 0, 5, 43, 7, 62, 17, 1 },
+			 { 0, 8, 51, 7, 57, 12, 0 },
+			 { 0, 12, 59, 7, 49, 8, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 2, 24, 66, 7, 33, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 7, 48, 7, 59, 13, 1 },
+			 { 0, 11, 56, 7, 52, 9, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 21, 66, 7, 36, 4, 0 } },
+		.odd = { { 0, 12, 58, 7, 50, 8, 0 },
+			 { 1, 17, 63, 7, 42, 5, 0 },
+			 { 2, 23, 66, 7, 34, 3, 0 },
+			 { 0, 3, 31, 7, 66, 26, 2 },
+			 { 0, 4, 39, 7, 64, 20, 1 },
+			 { 0, 7, 47, 7, 59, 14, 1 },
+			 { 0, 10, 55, 7, 53, 10, 0 },
+			 { 1, 15, 61, 7, 45, 6, 0 },
+			 { 1, 21, 65, 7, 37, 4, 0 },
+			 { 2, 28, 67, 7, 29, 2, 0 },
+			 { 0, 4, 36, 7, 65, 22, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 9, 52, 7, 56, 11, 0 },
+			 { 1, 13, 58, 7, 49, 7, 0 },
+			 { 1, 18, 64, 7, 40, 5, 0 },
+			 { 2, 25, 66, 7, 32, 3, 0 },
+			 { 0, 3, 32, 7, 66, 25, 2 },
+			 { 0, 5, 40, 7, 64, 18, 1 },
+			 { 0, 7, 49, 7, 58, 13, 1 },
+			 { 0, 11, 56, 7, 52, 9, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 22, 65, 7, 36, 4, 0 },
+			 { 0, 2, 29, 7, 67, 28, 2 },
+			 { 0, 4, 37, 7, 65, 21, 1 },
+			 { 0, 6, 45, 7, 61, 15, 1 },
+			 { 0, 10, 53, 7, 55, 10, 0 },
+			 { 1, 14, 59, 7, 47, 7, 0 },
+			 { 1, 20, 64, 7, 39, 4, 0 },
+			 { 2, 26, 66, 7, 31, 3, 0 },
+			 { 0, 3, 34, 7, 66, 23, 2 },
+			 { 0, 5, 42, 7, 63, 17, 1 },
+			 { 0, 8, 50, 7, 58, 12, 0 } } },
+	.ptrn_arr = { { 0x42484849, 0x12124242, 0x90909212, 0x24848484,
+			 0x21242424, 0x9090921, 0x48484849, 0x24242 } },
+	.sample_patrn_length = 246,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 92) = 0.258065 */
+	.hor_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 48, 7, 58, 14, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 14, 58, 7, 48, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 } },
+		.odd = { { 0, 12, 56, 7, 51, 9, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 9, 51, 7, 56, 12, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 48, 7, 58, 14, 1 },
+			 { 0, 10, 54, 7, 54, 10, 0 },
+			 { 1, 14, 58, 7, 48, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 } },
+		.odd = { { 0, 12, 56, 7, 51, 9, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 20, 65, 7, 38, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 38, 7, 65, 20, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 9, 51, 7, 56, 12, 0 } } },
+	.ptrn_arr = { { 0x48484849, 0x2424242 } },
+	.sample_patrn_length = 62,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 93) = 0.256 */
+	.hor_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 38, 7, 64, 21, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 1, 22, 65, 7, 36, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 30, 7, 66, 27, 2 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 8, 49, 7, 57, 13, 1 },
+			 { 0, 10, 55, 7, 53, 10, 0 },
+			 { 1, 13, 57, 7, 49, 8, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 },
+			 { 2, 27, 66, 7, 30, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 36, 7, 65, 22, 1 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 21, 64, 7, 38, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 } },
+		.odd = { { 0, 12, 56, 7, 51, 9, 0 },
+			 { 1, 14, 59, 7, 47, 7, 0 },
+			 { 1, 18, 61, 7, 42, 6, 0 },
+			 { 1, 21, 65, 7, 37, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 },
+			 { 0, 3, 30, 7, 65, 28, 2 },
+			 { 0, 3, 34, 7, 65, 24, 2 },
+			 { 0, 5, 39, 7, 63, 20, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 8, 48, 7, 58, 13, 1 },
+			 { 0, 10, 53, 7, 54, 11, 0 },
+			 { 1, 12, 57, 7, 50, 8, 0 },
+			 { 1, 15, 60, 7, 45, 7, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 23, 63, 7, 36, 4, 0 },
+			 { 2, 27, 65, 7, 31, 3, 0 },
+			 { 0, 3, 31, 7, 65, 27, 2 },
+			 { 0, 4, 36, 7, 63, 23, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 7, 45, 7, 60, 15, 1 },
+			 { 0, 8, 50, 7, 57, 12, 1 },
+			 { 0, 11, 54, 7, 53, 10, 0 },
+			 { 1, 13, 58, 7, 48, 8, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 20, 63, 7, 39, 5, 0 },
+			 { 2, 24, 65, 7, 34, 3, 0 },
+			 { 2, 28, 65, 7, 30, 3, 0 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 37, 7, 65, 21, 1 },
+			 { 0, 6, 42, 7, 61, 18, 1 },
+			 { 0, 7, 47, 7, 59, 14, 1 },
+			 { 0, 9, 51, 7, 56, 12, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 2, 29, 66, 7, 29, 2, 0 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 38, 7, 64, 21, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 1, 22, 65, 7, 36, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 0, 3, 30, 7, 66, 27, 2 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 8, 49, 7, 57, 13, 1 },
+			 { 0, 10, 55, 7, 53, 10, 0 },
+			 { 1, 13, 57, 7, 49, 8, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 },
+			 { 2, 27, 66, 7, 30, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 36, 7, 65, 22, 1 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 21, 64, 7, 38, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 } },
+		.odd = { { 0, 12, 56, 7, 51, 9, 0 },
+			 { 1, 14, 59, 7, 47, 7, 0 },
+			 { 1, 18, 61, 7, 42, 6, 0 },
+			 { 1, 21, 65, 7, 37, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 },
+			 { 0, 3, 30, 7, 65, 28, 2 },
+			 { 0, 3, 34, 7, 65, 24, 2 },
+			 { 0, 5, 39, 7, 63, 20, 1 },
+			 { 0, 6, 44, 7, 61, 16, 1 },
+			 { 0, 8, 48, 7, 58, 13, 1 },
+			 { 0, 10, 53, 7, 54, 11, 0 },
+			 { 1, 12, 57, 7, 50, 8, 0 },
+			 { 1, 15, 60, 7, 45, 7, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 2, 23, 63, 7, 36, 4, 0 },
+			 { 2, 27, 65, 7, 31, 3, 0 },
+			 { 0, 3, 31, 7, 65, 27, 2 },
+			 { 0, 4, 36, 7, 63, 23, 2 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 7, 45, 7, 60, 15, 1 },
+			 { 0, 8, 50, 7, 57, 12, 1 },
+			 { 0, 11, 54, 7, 53, 10, 0 },
+			 { 1, 13, 58, 7, 48, 8, 0 },
+			 { 1, 16, 61, 7, 44, 6, 0 },
+			 { 1, 20, 63, 7, 39, 5, 0 },
+			 { 2, 24, 65, 7, 34, 3, 0 },
+			 { 2, 28, 65, 7, 30, 3, 0 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 37, 7, 65, 21, 1 },
+			 { 0, 6, 42, 7, 61, 18, 1 },
+			 { 0, 7, 47, 7, 59, 14, 1 },
+			 { 0, 9, 51, 7, 56, 12, 0 } } },
+	.ptrn_arr = { { 0x48484849, 0x42424248, 0x12124242, 0x92121212,
+			 0x90909090, 0x84848490, 0x24248484, 0x242424 } },
+	.sample_patrn_length = 250,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 94) = 0.253968 */
+	.hor_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 38, 7, 63, 21, 1 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 44, 7, 60, 16, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 11, 53, 7, 53, 11, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 16, 60, 7, 44, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 1, 21, 63, 7, 38, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 } },
+		.odd = { { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 13, 57, 7, 49, 8, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 20, 62, 7, 40, 5, 0 },
+			 { 2, 22, 63, 7, 37, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 },
+			 { 2, 28, 65, 7, 30, 3, 0 },
+			 { 0, 3, 30, 7, 65, 28, 2 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 37, 7, 63, 22, 2 },
+			 { 0, 5, 40, 7, 62, 20, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 8, 49, 7, 57, 13, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 35, 7, 64, 23, 2 },
+			 { 0, 5, 38, 7, 63, 21, 1 },
+			 { 0, 5, 41, 7, 63, 18, 1 },
+			 { 0, 7, 44, 7, 60, 16, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 11, 53, 7, 53, 11, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 16, 60, 7, 44, 7, 0 },
+			 { 1, 18, 63, 7, 41, 5, 0 },
+			 { 1, 21, 63, 7, 38, 5, 0 },
+			 { 2, 23, 64, 7, 35, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 } },
+		.odd = { { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 13, 57, 7, 49, 8, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 20, 62, 7, 40, 5, 0 },
+			 { 2, 22, 63, 7, 37, 4, 0 },
+			 { 2, 25, 65, 7, 33, 3, 0 },
+			 { 2, 28, 65, 7, 30, 3, 0 },
+			 { 0, 3, 30, 7, 65, 28, 2 },
+			 { 0, 3, 33, 7, 65, 25, 2 },
+			 { 0, 4, 37, 7, 63, 22, 2 },
+			 { 0, 5, 40, 7, 62, 20, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 8, 49, 7, 57, 13, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 } } },
+	.ptrn_arr = { { 0x48484849, 0x48484848, 0x42424242, 0x2424242 } },
+	.sample_patrn_length = 126,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 95) = 0.251969 */
+	.hor_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 },
+			 { 0, 3, 31, 7, 64, 28, 2 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 34, 7, 63, 25, 2 },
+			 { 0, 4, 35, 7, 63, 24, 2 },
+			 { 0, 4, 37, 7, 63, 22, 2 },
+			 { 0, 5, 38, 7, 63, 21, 1 },
+			 { 0, 5, 40, 7, 62, 20, 1 },
+			 { 0, 6, 41, 7, 61, 19, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 44, 7, 60, 16, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 9, 49, 7, 56, 13, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 10, 51, 7, 54, 12, 1 },
+			 { 0, 11, 53, 7, 53, 11, 0 },
+			 { 1, 12, 54, 7, 51, 10, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 13, 56, 7, 49, 9, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 16, 60, 7, 44, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 19, 61, 7, 41, 6, 0 },
+			 { 1, 20, 62, 7, 40, 5, 0 },
+			 { 1, 21, 63, 7, 38, 5, 0 },
+			 { 2, 22, 63, 7, 37, 4, 0 },
+			 { 2, 24, 63, 7, 35, 4, 0 },
+			 { 2, 25, 63, 7, 34, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 2, 28, 64, 7, 31, 3, 0 } },
+		.odd = { { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 12, 54, 7, 51, 10, 0 },
+			 { 1, 13, 56, 7, 49, 9, 0 },
+			 { 1, 14, 57, 7, 48, 8, 0 },
+			 { 1, 15, 58, 7, 46, 8, 0 },
+			 { 1, 16, 59, 7, 45, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 18, 61, 7, 42, 6, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 1, 20, 63, 7, 39, 5, 0 },
+			 { 2, 22, 62, 7, 37, 5, 0 },
+			 { 2, 23, 63, 7, 36, 4, 0 },
+			 { 2, 24, 64, 7, 34, 4, 0 },
+			 { 2, 26, 64, 7, 33, 3, 0 },
+			 { 2, 27, 65, 7, 31, 3, 0 },
+			 { 3, 28, 64, 7, 30, 3, 0 },
+			 { 0, 3, 30, 7, 64, 28, 3 },
+			 { 0, 3, 31, 7, 65, 27, 2 },
+			 { 0, 3, 33, 7, 64, 26, 2 },
+			 { 0, 4, 34, 7, 64, 24, 2 },
+			 { 0, 4, 36, 7, 63, 23, 2 },
+			 { 0, 5, 37, 7, 62, 22, 2 },
+			 { 0, 5, 39, 7, 63, 20, 1 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 6, 42, 7, 61, 18, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 45, 7, 59, 16, 1 },
+			 { 0, 8, 46, 7, 58, 15, 1 },
+			 { 0, 8, 48, 7, 57, 14, 1 },
+			 { 0, 9, 49, 7, 56, 13, 1 },
+			 { 0, 10, 51, 7, 54, 12, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 },
+			 { 0, 3, 31, 7, 64, 28, 2 },
+			 { 0, 3, 32, 7, 65, 26, 2 },
+			 { 0, 4, 34, 7, 63, 25, 2 },
+			 { 0, 4, 35, 7, 63, 24, 2 },
+			 { 0, 4, 37, 7, 63, 22, 2 },
+			 { 0, 5, 38, 7, 63, 21, 1 },
+			 { 0, 5, 40, 7, 62, 20, 1 },
+			 { 0, 6, 41, 7, 61, 19, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 44, 7, 60, 16, 1 },
+			 { 0, 7, 46, 7, 59, 15, 1 },
+			 { 0, 8, 47, 7, 58, 14, 1 },
+			 { 0, 9, 49, 7, 56, 13, 1 },
+			 { 0, 9, 50, 7, 56, 12, 1 },
+			 { 0, 10, 51, 7, 54, 12, 1 },
+			 { 0, 11, 53, 7, 53, 11, 0 },
+			 { 1, 12, 54, 7, 51, 10, 0 },
+			 { 1, 12, 56, 7, 50, 9, 0 },
+			 { 1, 13, 56, 7, 49, 9, 0 },
+			 { 1, 14, 58, 7, 47, 8, 0 },
+			 { 1, 15, 59, 7, 46, 7, 0 },
+			 { 1, 16, 60, 7, 44, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 19, 61, 7, 41, 6, 0 },
+			 { 1, 20, 62, 7, 40, 5, 0 },
+			 { 1, 21, 63, 7, 38, 5, 0 },
+			 { 2, 22, 63, 7, 37, 4, 0 },
+			 { 2, 24, 63, 7, 35, 4, 0 },
+			 { 2, 25, 63, 7, 34, 4, 0 },
+			 { 2, 26, 65, 7, 32, 3, 0 },
+			 { 2, 28, 64, 7, 31, 3, 0 } },
+		.odd = { { 0, 11, 55, 7, 52, 10, 0 },
+			 { 1, 12, 54, 7, 51, 10, 0 },
+			 { 1, 13, 56, 7, 49, 9, 0 },
+			 { 1, 14, 57, 7, 48, 8, 0 },
+			 { 1, 15, 58, 7, 46, 8, 0 },
+			 { 1, 16, 59, 7, 45, 7, 0 },
+			 { 1, 17, 61, 7, 43, 6, 0 },
+			 { 1, 18, 61, 7, 42, 6, 0 },
+			 { 1, 19, 63, 7, 40, 5, 0 },
+			 { 1, 20, 63, 7, 39, 5, 0 },
+			 { 2, 22, 62, 7, 37, 5, 0 },
+			 { 2, 23, 63, 7, 36, 4, 0 },
+			 { 2, 24, 64, 7, 34, 4, 0 },
+			 { 2, 26, 64, 7, 33, 3, 0 },
+			 { 2, 27, 65, 7, 31, 3, 0 },
+			 { 3, 28, 64, 7, 30, 3, 0 },
+			 { 0, 3, 30, 7, 64, 28, 3 },
+			 { 0, 3, 31, 7, 65, 27, 2 },
+			 { 0, 3, 33, 7, 64, 26, 2 },
+			 { 0, 4, 34, 7, 64, 24, 2 },
+			 { 0, 4, 36, 7, 63, 23, 2 },
+			 { 0, 5, 37, 7, 62, 22, 2 },
+			 { 0, 5, 39, 7, 63, 20, 1 },
+			 { 0, 5, 40, 7, 63, 19, 1 },
+			 { 0, 6, 42, 7, 61, 18, 1 },
+			 { 0, 6, 43, 7, 61, 17, 1 },
+			 { 0, 7, 45, 7, 59, 16, 1 },
+			 { 0, 8, 46, 7, 58, 15, 1 },
+			 { 0, 8, 48, 7, 57, 14, 1 },
+			 { 0, 9, 49, 7, 56, 13, 1 },
+			 { 0, 10, 51, 7, 54, 12, 1 },
+			 { 0, 10, 52, 7, 55, 11, 0 } } },
+	.ptrn_arr = { { 0x48484849, 0x48484848, 0x48484848, 0x48484848,
+			 0x42424242, 0x42424242, 0x42424242, 0x2424242 } },
+	.sample_patrn_length = 254,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+}, {
+	/* Scale factor 32 / (32 + 96) = 0.25 */
+	.hor_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 } },
+		.odd = { { 0, 11, 53, 7, 53, 11, 0 } } },
+	.ver_phase_arr = {
+		.even = { { 3, 29, 64, 7, 29, 3, 0 } },
+		.odd = { { 0, 11, 53, 7, 53, 11, 0 } } },
+	.ptrn_arr = { { 0x9 } },
+	.sample_patrn_length = 8,
+	.hor_ds_en = 1,
+	.ver_ds_en = 1
+} };
+
+const s32 imgu_css_downscale_4taps[IMGU_SCALER_DOWNSCALE_4TAPS_LEN] = {
+	IMGU_SCALER_FP * -0.000000000000000,
+	IMGU_SCALER_FP * -0.000249009327023,
+	IMGU_SCALER_FP * -0.001022241683322,
+	IMGU_SCALER_FP * -0.002352252699175,
+	IMGU_SCALER_FP * -0.004261594242362,
+	IMGU_SCALER_FP * -0.006761648795689,
+	IMGU_SCALER_FP * -0.009851589454154,
+	IMGU_SCALER_FP * -0.013517488475013,
+	IMGU_SCALER_FP * -0.017731595701026,
+	IMGU_SCALER_FP * -0.022451806160682,
+	IMGU_SCALER_FP * -0.027621333752351,
+	IMGU_SCALER_FP * -0.033168605172067,
+	IMGU_SCALER_FP * -0.039007385183627,
+	IMGU_SCALER_FP * -0.045037140997445,
+	IMGU_SCALER_FP * -0.051143649969349,
+	IMGU_SCALER_FP * -0.057199851105019,
+	IMGU_SCALER_FP * -0.063066937016941,
+	IMGU_SCALER_FP * -0.068595679088417,
+	IMGU_SCALER_FP * -0.073627974715370,
+	IMGU_SCALER_FP * -0.077998601684588,
+	IMGU_SCALER_FP * -0.081537161069780,
+	IMGU_SCALER_FP * -0.084070186546763,
+	IMGU_SCALER_FP * -0.085423394806327,
+	IMGU_SCALER_FP * -0.085424048835192,
+	IMGU_SCALER_FP * -0.083903403294908,
+	IMGU_SCALER_FP * -0.080699199103829,
+	IMGU_SCALER_FP * -0.075658172660608,
+	IMGU_SCALER_FP * -0.068638543974523,
+	IMGU_SCALER_FP * -0.059512447316781,
+	IMGU_SCALER_FP * -0.048168267897836,
+	IMGU_SCALER_FP * -0.034512848520921,
+	IMGU_SCALER_FP * -0.018473531164409,
+	IMGU_SCALER_FP * 0.000000000000000,
+	IMGU_SCALER_FP * 0.020934105554674,
+	IMGU_SCALER_FP * 0.044329836544650,
+	IMGU_SCALER_FP * 0.070161864654994,
+	IMGU_SCALER_FP * 0.098377719033862,
+	IMGU_SCALER_FP * 0.128897348012514,
+	IMGU_SCALER_FP * 0.161613019706978,
+	IMGU_SCALER_FP * 0.196389570939079,
+	IMGU_SCALER_FP * 0.233065009152522,
+	IMGU_SCALER_FP * 0.271451467092549,
+	IMGU_SCALER_FP * 0.311336505037934,
+	IMGU_SCALER_FP * 0.352484750396743,
+	IMGU_SCALER_FP * 0.394639859577736,
+	IMGU_SCALER_FP * 0.437526782302744,
+	IMGU_SCALER_FP * 0.480854304005320,
+	IMGU_SCALER_FP * 0.524317837738108,
+	IMGU_SCALER_FP * 0.567602433152471,
+	IMGU_SCALER_FP * 0.610385966680669,
+	IMGU_SCALER_FP * 0.652342474098843,
+	IMGU_SCALER_FP * 0.693145584226952,
+	IMGU_SCALER_FP * 0.732472010670320,
+	IMGU_SCALER_FP * 0.770005057258970,
+	IMGU_SCALER_FP * 0.805438092218553,
+	IMGU_SCALER_FP * 0.838477946124244,
+	IMGU_SCALER_FP * 0.868848189350256,
+	IMGU_SCALER_FP * 0.896292246026874,
+	IMGU_SCALER_FP * 0.920576303438191,
+	IMGU_SCALER_FP * 0.941491978311745,
+	IMGU_SCALER_FP * 0.958858704531378,
+	IMGU_SCALER_FP * 0.972525810403401,
+	IMGU_SCALER_FP * 0.982374257672165,
+	IMGU_SCALER_FP * 0.988318018955586,
+	IMGU_SCALER_FP * 0.990305075088925,
+	IMGU_SCALER_FP * 0.988318018955586,
+	IMGU_SCALER_FP * 0.982374257672165,
+	IMGU_SCALER_FP * 0.972525810403401,
+	IMGU_SCALER_FP * 0.958858704531378,
+	IMGU_SCALER_FP * 0.941491978311745,
+	IMGU_SCALER_FP * 0.920576303438191,
+	IMGU_SCALER_FP * 0.896292246026874,
+	IMGU_SCALER_FP * 0.868848189350256,
+	IMGU_SCALER_FP * 0.838477946124244,
+	IMGU_SCALER_FP * 0.805438092218553,
+	IMGU_SCALER_FP * 0.770005057258970,
+	IMGU_SCALER_FP * 0.732472010670320,
+	IMGU_SCALER_FP * 0.693145584226952,
+	IMGU_SCALER_FP * 0.652342474098843,
+	IMGU_SCALER_FP * 0.610385966680669,
+	IMGU_SCALER_FP * 0.567602433152471,
+	IMGU_SCALER_FP * 0.524317837738108,
+	IMGU_SCALER_FP * 0.480854304005320,
+	IMGU_SCALER_FP * 0.437526782302744,
+	IMGU_SCALER_FP * 0.394639859577736,
+	IMGU_SCALER_FP * 0.352484750396743,
+	IMGU_SCALER_FP * 0.311336505037934,
+	IMGU_SCALER_FP * 0.271451467092549,
+	IMGU_SCALER_FP * 0.233065009152522,
+	IMGU_SCALER_FP * 0.196389570939079,
+	IMGU_SCALER_FP * 0.161613019706978,
+	IMGU_SCALER_FP * 0.128897348012514,
+	IMGU_SCALER_FP * 0.098377719033862,
+	IMGU_SCALER_FP * 0.070161864654994,
+	IMGU_SCALER_FP * 0.044329836544650,
+	IMGU_SCALER_FP * 0.020934105554674,
+	IMGU_SCALER_FP * 0.000000000000000,
+	IMGU_SCALER_FP * -0.018473531164409,
+	IMGU_SCALER_FP * -0.034512848520921,
+	IMGU_SCALER_FP * -0.048168267897836,
+	IMGU_SCALER_FP * -0.059512447316781,
+	IMGU_SCALER_FP * -0.068638543974523,
+	IMGU_SCALER_FP * -0.075658172660608,
+	IMGU_SCALER_FP * -0.080699199103829,
+	IMGU_SCALER_FP * -0.083903403294908,
+	IMGU_SCALER_FP * -0.085424048835192,
+	IMGU_SCALER_FP * -0.085423394806327,
+	IMGU_SCALER_FP * -0.084070186546763,
+	IMGU_SCALER_FP * -0.081537161069780,
+	IMGU_SCALER_FP * -0.077998601684588,
+	IMGU_SCALER_FP * -0.073627974715370,
+	IMGU_SCALER_FP * -0.068595679088417,
+	IMGU_SCALER_FP * -0.063066937016941,
+	IMGU_SCALER_FP * -0.057199851105019,
+	IMGU_SCALER_FP * -0.051143649969349,
+	IMGU_SCALER_FP * -0.045037140997445,
+	IMGU_SCALER_FP * -0.039007385183627,
+	IMGU_SCALER_FP * -0.033168605172067,
+	IMGU_SCALER_FP * -0.027621333752351,
+	IMGU_SCALER_FP * -0.022451806160682,
+	IMGU_SCALER_FP * -0.017731595701026,
+	IMGU_SCALER_FP * -0.013517488475013,
+	IMGU_SCALER_FP * -0.009851589454154,
+	IMGU_SCALER_FP * -0.006761648795689,
+	IMGU_SCALER_FP * -0.004261594242362,
+	IMGU_SCALER_FP * -0.002352252699175,
+	IMGU_SCALER_FP * -0.001022241683322,
+	IMGU_SCALER_FP * -0.000249009327023
+};
+
+const s32 imgu_css_downscale_2taps[IMGU_SCALER_DOWNSCALE_2TAPS_LEN] = {
+	IMGU_SCALER_FP * 0.074300676367033,
+	IMGU_SCALER_FP * 0.094030234498392,
+	IMGU_SCALER_FP * 0.115522859526596,
+	IMGU_SCALER_FP * 0.138778551451644,
+	IMGU_SCALER_FP * 0.163629399140505,
+	IMGU_SCALER_FP * 0.190075402593178,
+	IMGU_SCALER_FP * 0.217864695110113,
+	IMGU_SCALER_FP * 0.247081232257828,
+	IMGU_SCALER_FP * 0.277389191770256,
+	IMGU_SCALER_FP * 0.308704618080881,
+	IMGU_SCALER_FP * 0.340859600056670,
+	IMGU_SCALER_FP * 0.373602270998074,
+	IMGU_SCALER_FP * 0.406848675338577,
+	IMGU_SCALER_FP * 0.440346946378629,
+	IMGU_SCALER_FP * 0.473845217418681,
+	IMGU_SCALER_FP * 0.507091621759184,
+	IMGU_SCALER_FP * 0.540002203833621,
+	IMGU_SCALER_FP * 0.572157185809410,
+	IMGU_SCALER_FP * 0.603472612120036,
+	IMGU_SCALER_FP * 0.633612660499431,
+	IMGU_SCALER_FP * 0.662493375381080,
+	IMGU_SCALER_FP * 0.689778934498917,
+	IMGU_SCALER_FP * 0.715301426719909,
+	IMGU_SCALER_FP * 0.738892940911023,
+	IMGU_SCALER_FP * 0.760385565939227,
+	IMGU_SCALER_FP * 0.779527435104971,
+	IMGU_SCALER_FP * 0.796234592841739,
+	IMGU_SCALER_FP * 0.810339128016497,
+	IMGU_SCALER_FP * 0.821841040629247,
+	IMGU_SCALER_FP * 0.830488463980438,
+	IMGU_SCALER_FP * 0.836281398070072,
+	IMGU_SCALER_FP * 0.839219842898146,
+	IMGU_SCALER_FP * 0.839219842898146,
+	IMGU_SCALER_FP * 0.836281398070072,
+	IMGU_SCALER_FP * 0.830488463980438,
+	IMGU_SCALER_FP * 0.821841040629247,
+	IMGU_SCALER_FP * 0.810339128016497,
+	IMGU_SCALER_FP * 0.796234592841739,
+	IMGU_SCALER_FP * 0.779527435104971,
+	IMGU_SCALER_FP * 0.760385565939227,
+	IMGU_SCALER_FP * 0.738892940911023,
+	IMGU_SCALER_FP * 0.715301426719909,
+	IMGU_SCALER_FP * 0.689778934498917,
+	IMGU_SCALER_FP * 0.662493375381080,
+	IMGU_SCALER_FP * 0.633612660499431,
+	IMGU_SCALER_FP * 0.603472612120036,
+	IMGU_SCALER_FP * 0.572157185809410,
+	IMGU_SCALER_FP * 0.540002203833621,
+	IMGU_SCALER_FP * 0.507091621759184,
+	IMGU_SCALER_FP * 0.473845217418681,
+	IMGU_SCALER_FP * 0.440346946378629,
+	IMGU_SCALER_FP * 0.406848675338577,
+	IMGU_SCALER_FP * 0.373602270998074,
+	IMGU_SCALER_FP * 0.340859600056670,
+	IMGU_SCALER_FP * 0.308704618080881,
+	IMGU_SCALER_FP * 0.277389191770256,
+	IMGU_SCALER_FP * 0.247081232257828,
+	IMGU_SCALER_FP * 0.217864695110113,
+	IMGU_SCALER_FP * 0.190075402593178,
+	IMGU_SCALER_FP * 0.163629399140505,
+	IMGU_SCALER_FP * 0.138778551451644,
+	IMGU_SCALER_FP * 0.115522859526596,
+	IMGU_SCALER_FP * 0.094030234498392,
+	IMGU_SCALER_FP * 0.074300676367033
+};
+
+/* settings for Geometric Distortion Correction */
+const s16 imgu_css_gdc_lut[4][256] = { {
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, -1, -1, -1, -1, -2, -2, -2,
+	-2, -3, -3, -3, -4, -4, -4, -5, -5, -5, -6, -6, -7, -7, -7, -8, -8,
+	-9, -9, -10, -10, -11, -11, -12, -12, -13, -13, -14, -14, -15, -15,
+	-16, -16, -17, -17, -18, -19, -19, -20, -20, -21, -22, -22, -23, -24,
+	-24, -25, -25, -26, -27, -27, -28, -29, -29, -30, -31, -31, -32, -33,
+	-33, -34, -35, -35, -36, -37, -37, -38, -39, -39, -40,	-41, -41, -42,
+	-43, -43, -44, -45, -45, -46, -46, -47, -48, -48, -49, -50, -50, -51,
+	-52, -52, -53, -53, -54, -55, -55, -56, -56, -57, -58, -58, -59, -59,
+	-60, -60, -61, -61, -62, -62, -63, -64, -64, -64, -65,	-65, -66, -66,
+	-67, -67, -68, -68, -68, -69, -69, -70, -70, -70, -71, -71, -71, -72,
+	-72, -72, -73, -73, -73, -73, -74, -74, -74, -74, -74, -75, -75, -75,
+	-75, -75, -75, -75, -75, -75, -75, -75, -75, -75, -75,	-75, -75, -75,
+	-75, -75, -75, -75, -74, -74, -74, -74, -74, -73, -73, -73, -73, -72,
+	-72, -72, -71, -71, -70, -70, -69, -69, -68, -68, -67, -67, -66, -66,
+	-65, -64, -64, -63, -62, -61, -61, -60, -59, -58, -57,	-56, -56, -55,
+	-54, -53, -52, -51, -50, -49, -47, -46, -45, -44, -43, -41, -40, -39,
+	-38, -36, -35, -33, -32, -31, -29, -28, -26, -25, -23, -21, -20, -18,
+	-16, -15, -13, -11, -9, -7, -5, -3, -1
+}, {
+	0, 2, 4, 6, 8, 10, 13, 15, 17, 20, 23, 25, 28, 31, 33, 36, 39, 42, 45,
+	48, 51, 54, 58, 61, 64, 68, 71, 74, 78, 82, 85, 89, 93, 96, 100, 104,
+	108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 149, 153, 157, 162,
+	166, 171, 175, 180, 184, 189, 193, 198, 203, 207, 212, 217, 222, 227,
+	232, 236, 241, 246, 251, 256, 261, 266, 271, 276, 282, 287, 292, 297,
+	302, 307, 313, 318, 323, 328, 334, 339, 344, 350, 355, 360, 366, 371,
+	377, 382, 388, 393, 399, 404, 409, 415, 420, 426, 431, 437, 443, 448,
+	454, 459, 465, 470, 476, 481, 487, 492, 498, 504, 509, 515, 520, 526,
+	531, 537, 542, 548, 553, 559, 564, 570, 576, 581, 586, 592, 597, 603,
+	608, 614, 619, 625, 630, 635, 641, 646, 651, 657, 662, 667, 673, 678,
+	683, 688, 694, 699, 704, 709, 714, 719, 724, 729, 735, 740, 745, 749,
+	754, 759, 764, 769, 774, 779, 783, 788, 793, 797, 802, 807, 811, 816,
+	820, 825, 829, 834, 838, 842, 847, 851, 855, 859, 863, 868, 872, 876,
+	880, 884, 888, 891, 895, 899, 903, 906, 910, 914, 917, 921, 924, 927,
+	931, 934, 937, 940, 944, 947, 950, 953, 956, 959, 961, 964, 967, 970,
+	972, 975, 977, 980, 982, 984, 987, 989, 991, 993, 995, 997, 999, 1001,
+	1002, 1004, 1006, 1007, 1009, 1010, 1011, 1013, 1014, 1015, 1016, 1017,
+	1018, 1019, 1020, 1020, 1021, 1022, 1022, 1023, 1023, 1023, 1023, 1023
+}, {
+	1024, 1023, 1023, 1023, 1023, 1023, 1022, 1022, 1021, 1020, 1020, 1019,
+	1018, 1017, 1016, 1015, 1014, 1013, 1011, 1010, 1009, 1007, 1006, 1004,
+	1002, 1001, 999, 997, 995, 993, 991, 989, 987, 984, 982, 980, 977, 975,
+	972, 970, 967, 964, 961, 959, 956, 953, 950, 947, 944, 940, 937, 934,
+	931, 927, 924, 921, 917, 914, 910, 906, 903, 899, 895, 891, 888, 884,
+	880, 876, 872, 868, 863, 859, 855, 851, 847, 842, 838, 834, 829, 825,
+	820, 816, 811, 807, 802, 797, 793, 788, 783, 779, 774, 769, 764, 759,
+	754, 749, 745, 740, 735, 729, 724, 719, 714, 709, 704, 699, 694, 688,
+	683, 678, 673, 667, 662, 657, 651, 646, 641, 635, 630, 625, 619, 614,
+	608, 603, 597, 592, 586, 581, 576, 570, 564, 559, 553, 548, 542, 537,
+	531, 526, 520, 515, 509, 504, 498, 492, 487, 481, 476, 470, 465, 459,
+	454, 448, 443, 437, 431, 426, 420, 415, 409, 404, 399, 393, 388, 382,
+	377, 371, 366, 360, 355, 350, 344, 339, 334, 328, 323, 318, 313, 307,
+	302, 297, 292, 287, 282, 276, 271, 266, 261, 256, 251, 246, 241, 236,
+	232, 227, 222, 217, 212, 207, 203, 198, 193, 189, 184, 180, 175, 171,
+	166, 162, 157, 153, 149, 144, 140, 136, 132, 128, 124, 120, 116, 112,
+	108, 104, 100, 96, 93, 89, 85, 82, 78, 74, 71, 68, 64, 61, 58, 54, 51,
+	48, 45, 42, 39, 36, 33, 31, 28, 25, 23, 20, 17, 15, 13, 10, 8, 6, 4, 2
+}, {
+	0, -1, -3, -5, -7, -9, -11, -13, -14, -16, -19, -20, -21, -23, -24, -26,
+	-28, -29, -30, -32, -34, -34, -37, -38, -38, -41, -42, -42, -44, -46,
+	-46, -48, -49, -49, -51, -52, -53, -54, -55, -56, -57, -57, -58, -59,
+	-60, -60, -62, -62, -63, -63, -64, -65, -66, -66, -67, -68, -67, -69,
+	-69, -69, -70, -70, -71, -71, -72, -72, -72, -73, -73, -73, -73, -73,
+	-73, -74, -75, -74, -75, -75, -74, -75, -75, -75, -75, -75, -75, -75,
+	-75, -75, -75, -75, -75, -75, -75, -74, -75, -74, -75, -75, -74, -74,
+	-73, -73, -73, -73, -73, -73, -73, -71, -72, -71, -72, -70, -70, -70,
+	-69, -70, -69, -68, -68, -68, -67, -67, -66, -66, -65, -65, -64, -64,
+	-64, -63, -62, -62, -61, -61, -60, -60, -59, -59, -58, -58, -57, -57,
+	-55, -55, -55, -53, -54, -53, -52, -51, -52, -50, -50, -49, -48, -47,
+	-46, -46, -46, -46, -45, -43, -43, -42, -42, -41, -41, -40, -39, -39,
+	-38, -37, -37, -36, -35, -35, -34, -33, -32, -32, -31, -31, -31, -29,
+	-28, -27, -27, -27, -26, -25, -25, -24, -24, -23, -22, -22, -21, -20,
+	-20, -20, -18, -19, -17, -17, -16, -16, -15, -14, -14, -14, -14, -12,
+	-12, -12, -11, -11, -11, -10, -9, -9, -8, -8, -7, -6, -7, -7, -6, -6,
+	-5, -4, -5, -5, -3, -3, -4, -2, -3, -2, -1, -2, -1, -1, 0, -1, -1, 0,
+	-1, 0, 1, 0, 0, 0, 0, 0, 0, 0
+} };
+
+const struct imgu_css_xnr3_vmem_defaults imgu_css_xnr3_vmem_defaults = {
+	.x = {
+		1024, 1164, 1320, 1492, 1680, 1884, 2108, 2352,
+		2616, 2900, 3208, 3540, 3896, 4276, 4684, 5120
+	},
+	.a = {
+		-7213, -5580, -4371, -3421, -2722, -2159, -6950, -5585,
+		-4529, -3697, -3010, -2485, -2070, -1727, -1428, 0
+	},
+	.b = {
+		4096, 3603, 3178, 2811, 2497, 2226, 1990, 1783,
+		1603, 1446, 1307, 1185, 1077, 981, 895, 819
+	},
+	.c = {
+		1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	},
+};
+
+/* settings for Bayer Noise Reduction */
+const struct ipu3_uapi_bnr_static_config imgu_css_bnr_defaults = {
+	{ 16, 16, 16, 16 },			/* wb_gains */
+	{ 16, 16, 16, 16 },			/* wb_gains_thr */
+	{ 0, X, 8, 6, X, 14 },			/* thr_coeffs */
+	{ 0, 0, 0, 0 },				/* thr_ctrl_shd */
+	{ -128, X, -128, X },			/* opt_center */
+	{					/* lut */
+		{ 17, 23, 28, 32, 36, 39, 42, 45,
+		  48, 51, 53, 55, 58, 60, 62, 64,
+		  66, 68, 70, 72, 73, 75, 77, 78,
+		  80, 82, 83, 85, 86, 88, 89, 90 }
+	},
+	{ 4, X, 1, 8, X, 8, X, 8, X },		/* bp_ctrl */
+	{ 8, 4, 4, X, 8, X, 1, 1, 1, 1 },	/* dn_detect_ctrl */
+};
+
+const struct ipu3_uapi_dm_config imgu_css_dm_defaults = {
+	1, 1, 1, X, X, 8, X, 7, X, 8, X, 8, X, 4, X
+};
+
+const struct ipu3_uapi_ccm_mat_config imgu_css_ccm_defaults = {
+	 9775, -2671,  1087, 0,
+	-1071,  8303,   815, 0,
+	  -23, -7887, 16103, 0
+};
+
+/* settings for Gamma correction */
+const struct ipu3_uapi_gamma_corr_lut imgu_css_gamma_lut = { {
+	63, 79, 95, 111, 127, 143, 159, 175, 191, 207, 223, 239, 255, 271, 287,
+	303, 319, 335, 351, 367, 383, 399, 415, 431, 447, 463, 479, 495, 511,
+	527, 543, 559, 575, 591, 607, 623, 639, 655, 671, 687, 703, 719, 735,
+	751, 767, 783, 799, 815, 831, 847, 863, 879, 895, 911, 927, 943, 959,
+	975, 991, 1007, 1023, 1039, 1055, 1071, 1087, 1103, 1119, 1135, 1151,
+	1167, 1183, 1199, 1215, 1231, 1247, 1263, 1279, 1295, 1311, 1327, 1343,
+	1359, 1375, 1391, 1407, 1423, 1439, 1455, 1471, 1487, 1503, 1519, 1535,
+	1551, 1567, 1583, 1599, 1615, 1631, 1647, 1663, 1679, 1695, 1711, 1727,
+	1743, 1759, 1775, 1791, 1807, 1823, 1839, 1855, 1871, 1887, 1903, 1919,
+	1935, 1951, 1967, 1983, 1999, 2015, 2031, 2047, 2063, 2079, 2095, 2111,
+	2143, 2175, 2207, 2239, 2271, 2303, 2335, 2367, 2399, 2431, 2463, 2495,
+	2527, 2559, 2591, 2623, 2655, 2687, 2719, 2751, 2783, 2815, 2847, 2879,
+	2911, 2943, 2975, 3007, 3039, 3071, 3103, 3135, 3167, 3199, 3231, 3263,
+	3295, 3327, 3359, 3391, 3423, 3455, 3487, 3519, 3551, 3583, 3615, 3647,
+	3679, 3711, 3743, 3775, 3807, 3839, 3871, 3903, 3935, 3967, 3999, 4031,
+	4063, 4095, 4127, 4159, 4223, 4287, 4351, 4415, 4479, 4543, 4607, 4671,
+	4735, 4799, 4863, 4927, 4991, 5055, 5119, 5183, 5247, 5311, 5375, 5439,
+	5503, 5567, 5631, 5695, 5759, 5823, 5887, 5951, 6015, 6079, 6143, 6207,
+	6271, 6335, 6399, 6463, 6527, 6591, 6655, 6719, 6783, 6847, 6911, 6975,
+	7039, 7103, 7167, 7231, 7295, 7359, 7423, 7487, 7551, 7615, 7679, 7743,
+	7807, 7871, 7935, 7999, 8063, 8127, 8191
+} };
+
+const struct ipu3_uapi_csc_mat_config imgu_css_csc_defaults = {
+	 4898,  9617,  1867, 0,
+	-2410, -4732,  7143, 0,
+	10076, -8437, -1638, 0
+};
+
+const struct ipu3_uapi_cds_params imgu_css_cds_defaults = {
+	1, 3, 3, 1,
+	1, 3, 3, 1,
+	4, X,					/* ds_nf */
+	1,					/* csc_en */
+	0, X					/* uv_bin_output */
+};
+
+const struct ipu3_uapi_shd_config_static imgu_css_shd_defaults = {
+	.grid = {
+		.width = 73,
+		.height = 55,
+		.block_width_log2 = 7,
+		.block_height_log2 = 7,
+		.x_start = 0,
+		.y_start = 0,
+	},
+	.general = {
+		.shd_enable = 1,
+		.gain_factor = 0,
+	},
+	.black_level = {
+		.bl_r = 0,
+		.bl_gr = 0 | (0 << IPU3_UAPI_SHD_BLGR_NF_SHIFT),
+		.bl_gb = 0,
+		.bl_b = 0,
+	},
+};
+
+const struct ipu3_uapi_yuvp1_iefd_config imgu_css_iefd_defaults = {
+	.units = {
+		.cu_1 = { 0, 150, 7, 0 },
+		.cu_ed = { 7, 110, 244, X, 307, 409, 511, X,
+			   184, 255, 255, X, 0, 0, X,
+			   7, 81, 255, X, 255, 255, X },
+		.cu_3 = { 148, 251, 10, 0 },
+		.cu_5 = { 25, 70, 501, X, 32, X },
+		.cu_6 = { 32, 63, 183, X, 397,
+			  33, 0, X, 0,
+			  0, 64, X, 64, X },
+		.cu_7 = { 200, 303,
+			  10, 0 },
+		.cu_unsharp = { 10, 64, 110, X, 511,
+				66, 12, X, 0,
+				0, 56, X, 64, X },
+		.cu_radial = { 6, 203, 255, 255, 255, 255, X,
+			      84, 444, 397, 288, 300, X,
+			       4, 69, 207, X, 369, 448, X },
+		.cu_vssnlm = { 61, 100, 25, 0}
+	},
+	.config = { 45, X, 0, X, 16, X, 45, X },
+	.control = { 1, 1, 1, 1, 1, X },
+	.sharp = { { 50, X, 511, X, 50, X, 50, X },
+		   { 64, X, 0, X, 0, X},
+		   { 56, X, 56, X } },
+	.unsharp = { { 36, 17, 8, X },
+		     { 13, 7, 3, X } },
+	.rad = { { -2104, X, -1559, X },
+		 { 4426816, X },
+		 { 2430481, X },
+		 { 6, X, 79, X },
+		 { 64, 0, 0, X },
+		 { 1, X, 2, X, 0, X, 0, X },
+		 { 40, X, 62, X } },
+	.vsslnm = { { 16, 32, 64, X },
+		    { 1, X, 2, X,  8, X } },
+};
+
+const struct ipu3_uapi_yuvp1_yds_config imgu_css_yds_defaults = {
+	0, 1, 1, 0, 0, 1, 1, 0, 2, X, 0, X
+};
+
+const struct ipu3_uapi_yuvp1_chnr_config imgu_css_chnr_defaults = {
+	.coring = { 0, X, 0, X },
+	.sense_gain = { 6, 6, 6, X, 4, 4, 4, X },
+	.iir_fir = { 8, X, 12, X, 0, 256 - 127, X },
+};
+
+const struct ipu3_uapi_yuvp1_y_ee_nr_config imgu_css_y_ee_nr_defaults = {
+	.lpf = { 4, X, 8, X, 16, X,  0 },
+	.sense = { 8191, X, 0, X, 8191, X, 0, X },
+	.gain = { 8, X, 0, X, 8, X, 0, X },
+	.clip = { 8, X, 0, X, 8, X, 0, X },
+	.frng = { 2, X, 200, X, 2, X, 1, 1, X },
+	.diag = { 1, X, 4, 1, 1, 4, X },
+	.fc_coring = { 0, X, 0, X, 0, X, 0, X }
+};
+
+const struct ipu3_uapi_yuvp2_tcc_gain_pcwl_lut_static_config
+					imgu_css_tcc_gain_pcwl_lut = { {
+	1024, 1032, 1040, 1048, 1057, 1065, 1073, 1081, 1089, 1097, 1105, 1113,
+	1122, 1130, 1138, 1146, 1154, 1162, 1170, 1178, 1187, 1195, 1203, 1211,
+	1219, 1227, 1235, 1243, 1252, 1260, 1268, 1276, 1284, 1292, 1300, 1308,
+	1317, 1325, 1333, 1341, 1349, 1357, 1365, 1373, 1382, 1390, 1398, 1406,
+	1414, 1422, 1430, 1438, 1447, 1455, 1463, 1471, 1479, 1487, 1495, 1503,
+	1512, 1520, 1528, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536, 1536,
+	1536, 1536, 1528, 1520, 1512, 1503, 1495, 1487, 1479, 1471, 1463, 1455,
+	1447, 1438, 1430, 1422, 1414, 1406, 1398, 1390, 1382, 1373, 1365, 1357,
+	1349, 1341, 1333, 1325, 1317, 1308, 1300, 1292, 1284, 1276, 1268, 1260,
+	1252, 1243, 1235, 1227, 1219, 1211, 1203, 1195, 1187, 1178, 1170, 1162,
+	1154, 1146, 1138, 1130, 1122, 1113, 1105, 1097, 1089, 1081, 1073, 1065,
+	1057, 1048, 1040, 1032, 1024
+} };
+
+const struct ipu3_uapi_yuvp2_tcc_r_sqr_lut_static_config
+					imgu_css_tcc_r_sqr_lut = { {
+	32, 44, 64, 92, 128, 180, 256, 364, 512, 628, 724, 808, 888,
+	956, 1024, 1088, 1144, 1200, 1256, 1304, 1356, 1404, 1448
+} };
+
+const struct imgu_abi_anr_config imgu_css_anr_defaults = {
+	.transform = {
+		.adaptive_treshhold_en = 1,
+		.alpha = { { 13, 13, 13, 13, 0, 0, 0, 0},
+			   { 11, 11, 11, 11, 0, 0, 0, 0},
+			   { 14,  14, 14, 14, 0, 0, 0, 0} },
+		.beta = { { 24, 24, 24, 24},
+			  { 21, 20, 20, 21},
+			  { 25, 25, 25, 25} },
+		.color = { { { 166, 173, 149, 166, 161, 146, 145, 173,
+			       145, 150, 141, 149, 145, 141, 142 },
+			     { 166, 173, 149, 165, 161, 145, 145, 173,
+			       145, 150, 141, 149, 145, 141, 142 },
+			     { 166, 174, 149, 166, 162, 146, 146, 173,
+			       145, 150, 141, 149, 145, 141, 142 },
+			     { 166, 173, 149, 165, 161, 145, 145, 173,
+			       146, 150, 141, 149, 145, 141, 142 } },
+			   { { 141, 131, 140, 141, 144, 143, 144, 131,
+			       143, 137, 140, 140, 144, 140, 141 },
+			     { 141, 131, 140, 141, 143, 143, 144, 131,
+			       143, 137, 140, 140, 144, 140, 141 },
+			     { 141, 131, 141, 141, 144, 144, 144, 131,
+			       143, 137, 140, 140, 144, 140, 141 },
+			     { 140, 131, 140, 141, 143, 143, 144, 131,
+			       143, 137, 140, 140, 144, 140, 141 } },
+			   { { 184, 173, 188, 184, 182, 182, 181, 173,
+			       182, 179, 182, 188, 181, 182, 180 },
+			     { 184, 173, 188, 184, 183, 182, 181, 173,
+			       182, 178, 182, 188, 181, 182, 180 },
+			     { 184, 173, 188, 184, 182, 182, 181, 173,
+			       182, 178, 182, 188, 181, 182, 181 },
+			     { 184, 172, 188, 184, 182, 182, 181, 173,
+			       182, 178, 182, 188, 182, 182, 180 } } },
+		.sqrt_lut = { 724, 768, 810, 849, 887, 923, 958, 991, 1024,
+			      1056, 1086, 1116, 1145, 1173, 1201, 1228, 1254,
+			      1280, 1305, 1330, 1355, 1379, 1402, 1425, 1448 },
+		.xreset = -1632,
+		.yreset = -1224,
+		.x_sqr_reset = 2663424,
+		.r_normfactor = 14,
+		.y_sqr_reset = 1498176,
+		.gain_scale = 115
+	},
+	.stitch = {
+		.anr_stitch_en = 1,
+		.pyramid = { { 1, 3, 5 }, { 7, 7, 5 }, { 3, 1, 3 },
+			     { 9, 15, 21 }, { 21, 15, 9 }, { 3, 5, 15 },
+			     { 25, 35, 35 }, { 25, 15, 5 }, { 7, 21, 35 },
+			     { 49, 49, 35 }, { 21, 7, 7 }, { 21, 35, 49 },
+			     { 49, 35, 21 }, { 7, 5, 15 }, { 25, 35, 35 },
+			     { 25, 15, 5 }, { 3, 9, 15 }, { 21, 21, 15 },
+			     { 9, 3, 1 }, { 3, 5, 7 }, { 7, 5, 3}, { 1 }
+		}
+	}
+};
+
+/* frame settings for Auto White Balance */
+const struct ipu3_uapi_awb_fr_config_s imgu_css_awb_fr_defaults = {
+	.grid_cfg = {
+		.width = 16,
+		.height = 16,
+		.block_width_log2 = 3,
+		.block_height_log2 = 3,
+		.x_start = 10,
+		.y_start = 2 | IPU3_UAPI_GRID_Y_START_EN,
+	},
+	.bayer_coeff = { 0, 0, 0, 0, 0, 128 },
+	.bayer_sign = 0,
+	.bayer_nf = 7
+};
+
+/* settings for Auto Exposure */
+const struct ipu3_uapi_ae_grid_config imgu_css_ae_grid_defaults = {
+	.width = 16,
+	.height = 16,
+	.block_width_log2 = 3,
+	.block_height_log2 = 3,
+	.ae_en = 1,
+	.x_start = 0,
+	.y_start = 0,
+};
+
+/* settings for Auto Exposure color correction matrix */
+const struct ipu3_uapi_ae_ccm imgu_css_ae_ccm_defaults = {
+	256, 256, 256, 256,		/* gain_gr/r/b/gb */
+	.mat = { 128, 0, 0, 0, 0, 128, 0, 0, 0, 0, 128, 0, 0, 0, 0, 128 },
+};
+
+/* settings for Auto Focus */
+const struct ipu3_uapi_af_config_s imgu_css_af_defaults = {
+	.filter_config = {
+		{ 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 128 }, 0,
+		{ 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 128 }, 0,
+		.y_calc = { 8, 8, 8, 8 },
+		.nf = { X, 7, X, 7 },
+	},
+	.grid_cfg = {
+		.width = 16,
+		.height = 16,
+		.block_width_log2 = 3,
+		.block_height_log2 = 3,
+		.x_start = 10,
+		.y_start = 2 | IPU3_UAPI_GRID_Y_START_EN,
+	},
+};
+
+/* settings for Auto White Balance */
+const struct ipu3_uapi_awb_config_s imgu_css_awb_defaults = {
+	8191, 8191, 8191, 8191 |	/* rgbs_thr_gr/r/gb/b */
+	IPU3_UAPI_AWB_RGBS_THR_B_EN | IPU3_UAPI_AWB_RGBS_THR_B_INCL_SAT,
+	.grid = {
+		.width = 16,
+		.height = 16,
+		.block_width_log2 = 3,
+		.block_height_log2 = 3,
+		.x_start = 0,
+		.y_start = 0,
+	},
+};
diff --git a/drivers/staging/media/ipu3/ipu3-tables.h b/drivers/staging/media/ipu3/ipu3-tables.h
new file mode 100644
index 000000000..9f719c48b
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-tables.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_TABLES_H
+#define __IPU3_TABLES_H
+
+#include <linux/bitops.h>
+
+#include "ipu3-abi.h"
+
+#define IMGU_BDS_GRANULARITY		32	/* Downscaling granularity */
+#define IMGU_BDS_MIN_SF_INV		IMGU_BDS_GRANULARITY
+#define IMGU_BDS_CONFIG_LEN		97
+
+#define IMGU_SCALER_DOWNSCALE_4TAPS_LEN	128
+#define IMGU_SCALER_DOWNSCALE_2TAPS_LEN	64
+#define IMGU_SCALER_FP			BIT(31) /* 1.0 in fixed point */
+
+#define IMGU_XNR3_VMEM_LUT_LEN		16
+
+#define IMGU_GDC_LUT_UNIT		4
+#define IMGU_GDC_LUT_LEN		256
+
+struct imgu_css_bds_config {
+	struct imgu_abi_bds_phase_arr hor_phase_arr;
+	struct imgu_abi_bds_phase_arr ver_phase_arr;
+	struct imgu_abi_bds_ptrn_arr ptrn_arr;
+	u16 sample_patrn_length;
+	u8 hor_ds_en;
+	u8 ver_ds_en;
+};
+
+struct imgu_css_xnr3_vmem_defaults {
+	s16 x[IMGU_XNR3_VMEM_LUT_LEN];
+	s16 a[IMGU_XNR3_VMEM_LUT_LEN];
+	s16 b[IMGU_XNR3_VMEM_LUT_LEN];
+	s16 c[IMGU_XNR3_VMEM_LUT_LEN];
+};
+
+extern const struct imgu_css_bds_config
+			imgu_css_bds_configs[IMGU_BDS_CONFIG_LEN];
+extern const s32 imgu_css_downscale_4taps[IMGU_SCALER_DOWNSCALE_4TAPS_LEN];
+extern const s32 imgu_css_downscale_2taps[IMGU_SCALER_DOWNSCALE_2TAPS_LEN];
+extern const s16 imgu_css_gdc_lut[IMGU_GDC_LUT_UNIT][IMGU_GDC_LUT_LEN];
+extern const struct imgu_css_xnr3_vmem_defaults imgu_css_xnr3_vmem_defaults;
+extern const struct ipu3_uapi_bnr_static_config imgu_css_bnr_defaults;
+extern const struct ipu3_uapi_dm_config imgu_css_dm_defaults;
+extern const struct ipu3_uapi_ccm_mat_config imgu_css_ccm_defaults;
+extern const struct ipu3_uapi_gamma_corr_lut imgu_css_gamma_lut;
+extern const struct ipu3_uapi_csc_mat_config imgu_css_csc_defaults;
+extern const struct ipu3_uapi_cds_params imgu_css_cds_defaults;
+extern const struct ipu3_uapi_shd_config_static imgu_css_shd_defaults;
+extern const struct ipu3_uapi_yuvp1_iefd_config imgu_css_iefd_defaults;
+extern const struct ipu3_uapi_yuvp1_yds_config imgu_css_yds_defaults;
+extern const struct ipu3_uapi_yuvp1_chnr_config imgu_css_chnr_defaults;
+extern const struct ipu3_uapi_yuvp1_y_ee_nr_config imgu_css_y_ee_nr_defaults;
+extern const struct ipu3_uapi_yuvp2_tcc_gain_pcwl_lut_static_config
+						imgu_css_tcc_gain_pcwl_lut;
+extern const struct ipu3_uapi_yuvp2_tcc_r_sqr_lut_static_config
+						imgu_css_tcc_r_sqr_lut;
+extern const struct imgu_abi_anr_config imgu_css_anr_defaults;
+extern const struct ipu3_uapi_awb_fr_config_s imgu_css_awb_fr_defaults;
+extern const struct ipu3_uapi_ae_grid_config imgu_css_ae_grid_defaults;
+extern const struct ipu3_uapi_ae_ccm imgu_css_ae_ccm_defaults;
+extern const struct ipu3_uapi_af_config_s imgu_css_af_defaults;
+extern const struct ipu3_uapi_awb_config_s imgu_css_awb_defaults;
+
+#endif
diff --git a/drivers/staging/media/ipu3/ipu3-v4l2.c b/drivers/staging/media/ipu3/ipu3-v4l2.c
new file mode 100644
index 000000000..e530767e8
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3-v4l2.c
@@ -0,0 +1,1398 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+
+#include "ipu3.h"
+#include "ipu3-dmamap.h"
+
+/******************** v4l2_subdev_ops ********************/
+
+#define IPU3_RUNNING_MODE_VIDEO		0
+#define IPU3_RUNNING_MODE_STILL		1
+
+static int imgu_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
+							struct imgu_v4l2_subdev,
+							subdev);
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[imgu_sd->pipe];
+	struct v4l2_rect try_crop = {
+		.top = 0,
+		.left = 0,
+	};
+	unsigned int i;
+
+	try_crop.width =
+		imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.width;
+	try_crop.height =
+		imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.height;
+
+	/* Initialize try_fmt */
+	for (i = 0; i < IMGU_NODE_NUM; i++) {
+		struct v4l2_mbus_framefmt *try_fmt =
+			v4l2_subdev_get_try_format(sd, fh->state, i);
+
+		try_fmt->width = try_crop.width;
+		try_fmt->height = try_crop.height;
+		try_fmt->code = imgu_pipe->nodes[i].pad_fmt.code;
+		try_fmt->field = V4L2_FIELD_NONE;
+	}
+
+	*v4l2_subdev_get_try_crop(sd, fh->state, IMGU_NODE_IN) = try_crop;
+	*v4l2_subdev_get_try_compose(sd, fh->state, IMGU_NODE_IN) = try_crop;
+
+	return 0;
+}
+
+static int imgu_subdev_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int i;
+	unsigned int node;
+	int r = 0;
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
+							struct imgu_v4l2_subdev,
+							subdev);
+	unsigned int pipe = imgu_sd->pipe;
+	struct device *dev = &imgu->pci_dev->dev;
+	struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
+	struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
+	struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	dev_dbg(dev, "%s %d for pipe %u", __func__, enable, pipe);
+	/* grab ctrl after streamon and return after off */
+	v4l2_ctrl_grab(imgu_sd->ctrl, enable);
+
+	if (!enable) {
+		imgu_sd->active = false;
+		return 0;
+	}
+
+	for (i = 0; i < IMGU_NODE_NUM; i++)
+		imgu_pipe->queue_enabled[i] = imgu_pipe->nodes[i].enabled;
+
+	/* This is handled specially */
+	imgu_pipe->queue_enabled[IPU3_CSS_QUEUE_PARAMS] = false;
+
+	/* Initialize CSS formats */
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		node = imgu_map_node(imgu, i);
+		/* No need to reconfig meta nodes */
+		if (node == IMGU_NODE_STAT_3A || node == IMGU_NODE_PARAMS)
+			continue;
+		fmts[i] = imgu_pipe->queue_enabled[node] ?
+			&imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp : NULL;
+	}
+
+	/* Enable VF output only when VF queue requested by user */
+	css_pipe->vf_output_en = false;
+	if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
+		css_pipe->vf_output_en = true;
+
+	if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
+		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
+	else
+		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
+
+	dev_dbg(dev, "IPU3 pipe %u pipe_id %u", pipe, css_pipe->pipe_id);
+
+	rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
+	rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
+	rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
+
+	r = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
+	if (r) {
+		dev_err(dev, "failed to set initial formats pipe %u with (%d)",
+			pipe, r);
+		return r;
+	}
+
+	imgu_sd->active = true;
+
+	return 0;
+}
+
+static int imgu_subdev_get_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf;
+	struct imgu_media_pipe *imgu_pipe;
+	u32 pad = fmt->pad;
+	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
+							struct imgu_v4l2_subdev,
+							subdev);
+	unsigned int pipe = imgu_sd->pipe;
+
+	imgu_pipe = &imgu->imgu_pipe[pipe];
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		fmt->format = imgu_pipe->nodes[pad].pad_fmt;
+	} else {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, pad);
+		fmt->format = *mf;
+	}
+
+	return 0;
+}
+
+static int imgu_subdev_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct imgu_media_pipe *imgu_pipe;
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
+							struct imgu_v4l2_subdev,
+							subdev);
+	struct v4l2_mbus_framefmt *mf;
+	u32 pad = fmt->pad;
+	unsigned int pipe = imgu_sd->pipe;
+
+	dev_dbg(&imgu->pci_dev->dev, "set subdev %u pad %u fmt to [%ux%u]",
+		pipe, pad, fmt->format.width, fmt->format.height);
+
+	imgu_pipe = &imgu->imgu_pipe[pipe];
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		mf = v4l2_subdev_get_try_format(sd, sd_state, pad);
+	else
+		mf = &imgu_pipe->nodes[pad].pad_fmt;
+
+	fmt->format.code = mf->code;
+	/* Clamp the w and h based on the hardware capabilities */
+	if (imgu_sd->subdev_pads[pad].flags & MEDIA_PAD_FL_SOURCE) {
+		fmt->format.width = clamp(fmt->format.width,
+					  IPU3_OUTPUT_MIN_WIDTH,
+					  IPU3_OUTPUT_MAX_WIDTH);
+		fmt->format.height = clamp(fmt->format.height,
+					   IPU3_OUTPUT_MIN_HEIGHT,
+					   IPU3_OUTPUT_MAX_HEIGHT);
+	} else {
+		fmt->format.width = clamp(fmt->format.width,
+					  IPU3_INPUT_MIN_WIDTH,
+					  IPU3_INPUT_MAX_WIDTH);
+		fmt->format.height = clamp(fmt->format.height,
+					   IPU3_INPUT_MIN_HEIGHT,
+					   IPU3_INPUT_MAX_HEIGHT);
+	}
+
+	*mf = fmt->format;
+
+	return 0;
+}
+
+static struct v4l2_rect *
+imgu_subdev_get_crop(struct imgu_v4l2_subdev *sd,
+		     struct v4l2_subdev_state *sd_state, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&sd->subdev, sd_state, pad);
+	else
+		return &sd->rect.eff;
+}
+
+static struct v4l2_rect *
+imgu_subdev_get_compose(struct imgu_v4l2_subdev *sd,
+			struct v4l2_subdev_state *sd_state, unsigned int pad,
+			enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_compose(&sd->subdev, sd_state, pad);
+	else
+		return &sd->rect.bds;
+}
+
+static int imgu_subdev_get_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_selection *sel)
+{
+	struct imgu_v4l2_subdev *imgu_sd =
+		container_of(sd, struct imgu_v4l2_subdev, subdev);
+
+	if (sel->pad != IMGU_NODE_IN)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *imgu_subdev_get_crop(imgu_sd, sd_state, sel->pad,
+					       sel->which);
+		return 0;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *imgu_subdev_get_compose(imgu_sd, sd_state, sel->pad,
+						  sel->which);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int imgu_subdev_set_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_selection *sel)
+{
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct imgu_v4l2_subdev *imgu_sd =
+		container_of(sd, struct imgu_v4l2_subdev, subdev);
+	struct v4l2_rect *rect;
+
+	dev_dbg(&imgu->pci_dev->dev,
+		 "set subdev %u sel which %u target 0x%4x rect [%ux%u]",
+		 imgu_sd->pipe, sel->which, sel->target,
+		 sel->r.width, sel->r.height);
+
+	if (sel->pad != IMGU_NODE_IN)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		rect = imgu_subdev_get_crop(imgu_sd, sd_state, sel->pad,
+					    sel->which);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		rect = imgu_subdev_get_compose(imgu_sd, sd_state, sel->pad,
+					       sel->which);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	*rect = sel->r;
+	return 0;
+}
+
+/******************** media_entity_operations ********************/
+
+static int imgu_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct imgu_media_pipe *imgu_pipe;
+	struct v4l2_subdev *sd = container_of(entity, struct v4l2_subdev,
+					      entity);
+	struct imgu_device *imgu = v4l2_get_subdevdata(sd);
+	struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
+							struct imgu_v4l2_subdev,
+							subdev);
+	unsigned int pipe = imgu_sd->pipe;
+	u32 pad = local->index;
+
+	WARN_ON(pad >= IMGU_NODE_NUM);
+
+	dev_dbg(&imgu->pci_dev->dev, "pipe %u pad %u is %s", pipe, pad,
+		 flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
+
+	imgu_pipe = &imgu->imgu_pipe[pipe];
+	imgu_pipe->nodes[pad].enabled = flags & MEDIA_LNK_FL_ENABLED;
+
+	/* enable input node to enable the pipe */
+	if (pad != IMGU_NODE_IN)
+		return 0;
+
+	if (flags & MEDIA_LNK_FL_ENABLED)
+		__set_bit(pipe, imgu->css.enabled_pipes);
+	else
+		__clear_bit(pipe, imgu->css.enabled_pipes);
+
+	dev_dbg(&imgu->pci_dev->dev, "pipe %u is %s", pipe,
+		 flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
+
+	return 0;
+}
+
+/******************** vb2_ops ********************/
+
+static int imgu_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
+	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
+	struct imgu_buffer *buf = container_of(vb,
+		struct imgu_buffer, vid_buf.vbb.vb2_buf);
+	struct imgu_video_device *node =
+		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
+	unsigned int queue = imgu_node_to_queue(node->id);
+
+	if (queue == IPU3_CSS_QUEUE_PARAMS)
+		return 0;
+
+	return imgu_dmamap_map_sg(imgu, sg->sgl, sg->nents, &buf->map);
+}
+
+/* Called when each buffer is freed */
+static void imgu_vb2_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
+	struct imgu_buffer *buf = container_of(vb,
+		struct imgu_buffer, vid_buf.vbb.vb2_buf);
+	struct imgu_video_device *node =
+		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
+	unsigned int queue = imgu_node_to_queue(node->id);
+
+	if (queue == IPU3_CSS_QUEUE_PARAMS)
+		return;
+
+	imgu_dmamap_unmap(imgu, &buf->map);
+}
+
+/* Transfer buffer ownership to me */
+static void imgu_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
+	struct imgu_video_device *node =
+		container_of(vb->vb2_queue, struct imgu_video_device, vbq);
+	unsigned int queue = imgu_node_to_queue(node->id);
+	struct imgu_buffer *buf = container_of(vb, struct imgu_buffer,
+					       vid_buf.vbb.vb2_buf);
+	unsigned long need_bytes;
+	unsigned long payload = vb2_get_plane_payload(vb, 0);
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_META_CAPTURE ||
+	    vb->vb2_queue->type == V4L2_BUF_TYPE_META_OUTPUT)
+		need_bytes = node->vdev_fmt.fmt.meta.buffersize;
+	else
+		need_bytes = node->vdev_fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
+
+	if (queue == IPU3_CSS_QUEUE_PARAMS && payload && payload < need_bytes) {
+		dev_err(&imgu->pci_dev->dev, "invalid data size for params.");
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		return;
+	}
+
+	mutex_lock(&imgu->lock);
+	if (queue != IPU3_CSS_QUEUE_PARAMS)
+		imgu_css_buf_init(&buf->css_buf, queue, buf->map.daddr);
+
+	list_add_tail(&buf->vid_buf.list, &node->buffers);
+	mutex_unlock(&imgu->lock);
+
+	vb2_set_plane_payload(vb, 0, need_bytes);
+
+	mutex_lock(&imgu->streaming_lock);
+	if (imgu->streaming)
+		imgu_queue_buffers(imgu, false, node->pipe);
+	mutex_unlock(&imgu->streaming_lock);
+
+	dev_dbg(&imgu->pci_dev->dev, "%s for pipe %u node %u", __func__,
+		node->pipe, node->id);
+}
+
+static int imgu_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *num_buffers,
+				unsigned int *num_planes,
+				unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct imgu_device *imgu = vb2_get_drv_priv(vq);
+	struct imgu_video_device *node =
+		container_of(vq, struct imgu_video_device, vbq);
+	const struct v4l2_format *fmt = &node->vdev_fmt;
+	unsigned int size;
+
+	*num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
+	alloc_devs[0] = &imgu->pci_dev->dev;
+
+	if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
+	    vq->type == V4L2_BUF_TYPE_META_OUTPUT)
+		size = fmt->fmt.meta.buffersize;
+	else
+		size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+	if (*num_planes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
+
+	*num_planes = 1;
+	sizes[0] = size;
+
+	/* Initialize buffer queue */
+	INIT_LIST_HEAD(&node->buffers);
+
+	return 0;
+}
+
+/* Check if all enabled video nodes are streaming, exception ignored */
+static bool imgu_all_nodes_streaming(struct imgu_device *imgu,
+				     struct imgu_video_device *except)
+{
+	unsigned int i, pipe, p;
+	struct imgu_video_device *node;
+	struct device *dev = &imgu->pci_dev->dev;
+
+	pipe = except->pipe;
+	if (!test_bit(pipe, imgu->css.enabled_pipes)) {
+		dev_warn(&imgu->pci_dev->dev,
+			 "pipe %u link is not ready yet", pipe);
+		return false;
+	}
+
+	for_each_set_bit(p, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		for (i = 0; i < IMGU_NODE_NUM; i++) {
+			node = &imgu->imgu_pipe[p].nodes[i];
+			dev_dbg(dev, "%s pipe %u queue %u name %s enabled = %u",
+				__func__, p, i, node->name, node->enabled);
+			if (node == except)
+				continue;
+			if (node->enabled && !vb2_start_streaming_called(&node->vbq))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+static void imgu_return_all_buffers(struct imgu_device *imgu,
+				    struct imgu_video_device *node,
+				    enum vb2_buffer_state state)
+{
+	struct imgu_vb2_buffer *b, *b0;
+
+	/* Return all buffers */
+	mutex_lock(&imgu->lock);
+	list_for_each_entry_safe(b, b0, &node->buffers, list) {
+		list_del(&b->list);
+		vb2_buffer_done(&b->vbb.vb2_buf, state);
+	}
+	mutex_unlock(&imgu->lock);
+}
+
+static int imgu_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct imgu_media_pipe *imgu_pipe;
+	struct imgu_device *imgu = vb2_get_drv_priv(vq);
+	struct device *dev = &imgu->pci_dev->dev;
+	struct imgu_video_device *node =
+		container_of(vq, struct imgu_video_device, vbq);
+	int r;
+	unsigned int pipe;
+
+	dev_dbg(dev, "%s node name %s pipe %u id %u", __func__,
+		node->name, node->pipe, node->id);
+
+	mutex_lock(&imgu->streaming_lock);
+	if (imgu->streaming) {
+		r = -EBUSY;
+		mutex_unlock(&imgu->streaming_lock);
+		goto fail_return_bufs;
+	}
+	mutex_unlock(&imgu->streaming_lock);
+
+	if (!node->enabled) {
+		dev_err(dev, "IMGU node is not enabled");
+		r = -EINVAL;
+		goto fail_return_bufs;
+	}
+
+	pipe = node->pipe;
+	imgu_pipe = &imgu->imgu_pipe[pipe];
+	atomic_set(&node->sequence, 0);
+	r = video_device_pipeline_start(&node->vdev, &imgu_pipe->pipeline);
+	if (r < 0)
+		goto fail_return_bufs;
+
+	if (!imgu_all_nodes_streaming(imgu, node))
+		return 0;
+
+	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = v4l2_subdev_call(&imgu->imgu_pipe[pipe].imgu_sd.subdev,
+				     video, s_stream, 1);
+		if (r < 0)
+			goto fail_stop_pipeline;
+	}
+
+	/* Start streaming of the whole pipeline now */
+	dev_dbg(dev, "IMGU streaming is ready to start");
+	mutex_lock(&imgu->streaming_lock);
+	r = imgu_s_stream(imgu, true);
+	if (!r)
+		imgu->streaming = true;
+	mutex_unlock(&imgu->streaming_lock);
+
+	return 0;
+
+fail_stop_pipeline:
+	video_device_pipeline_stop(&node->vdev);
+fail_return_bufs:
+	imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_QUEUED);
+
+	return r;
+}
+
+static void imgu_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	struct imgu_media_pipe *imgu_pipe;
+	struct imgu_device *imgu = vb2_get_drv_priv(vq);
+	struct device *dev = &imgu->pci_dev->dev;
+	struct imgu_video_device *node =
+		container_of(vq, struct imgu_video_device, vbq);
+	int r;
+	unsigned int pipe;
+
+	WARN_ON(!node->enabled);
+
+	pipe = node->pipe;
+	dev_dbg(dev, "Try to stream off node [%u][%u]", pipe, node->id);
+	imgu_pipe = &imgu->imgu_pipe[pipe];
+	r = v4l2_subdev_call(&imgu_pipe->imgu_sd.subdev, video, s_stream, 0);
+	if (r)
+		dev_err(&imgu->pci_dev->dev,
+			"failed to stop subdev streaming\n");
+
+	mutex_lock(&imgu->streaming_lock);
+	/* Was this the first node with streaming disabled? */
+	if (imgu->streaming && imgu_all_nodes_streaming(imgu, node)) {
+		/* Yes, really stop streaming now */
+		dev_dbg(dev, "IMGU streaming is ready to stop");
+		r = imgu_s_stream(imgu, false);
+		if (!r)
+			imgu->streaming = false;
+	}
+
+	imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_ERROR);
+	mutex_unlock(&imgu->streaming_lock);
+
+	video_device_pipeline_stop(&node->vdev);
+}
+
+/******************** v4l2_ioctl_ops ********************/
+
+#define VID_CAPTURE	0
+#define VID_OUTPUT	1
+#define DEF_VID_CAPTURE	0
+#define DEF_VID_OUTPUT	1
+
+struct imgu_fmt {
+	u32	fourcc;
+	u16	type; /* VID_CAPTURE or VID_OUTPUT not both */
+};
+
+/* format descriptions for capture and preview */
+static const struct imgu_fmt formats[] = {
+	{ V4L2_PIX_FMT_NV12, VID_CAPTURE },
+	{ V4L2_PIX_FMT_IPU3_SGRBG10, VID_OUTPUT },
+	{ V4L2_PIX_FMT_IPU3_SBGGR10, VID_OUTPUT },
+	{ V4L2_PIX_FMT_IPU3_SGBRG10, VID_OUTPUT },
+	{ V4L2_PIX_FMT_IPU3_SRGGB10, VID_OUTPUT },
+};
+
+/* Find the first matched format, return default if not found */
+static const struct imgu_fmt *find_format(struct v4l2_format *f, u32 type)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+		    formats[i].type == type)
+			return &formats[i];
+	}
+
+	return type == VID_CAPTURE ? &formats[DEF_VID_CAPTURE] :
+				     &formats[DEF_VID_OUTPUT];
+}
+
+static int imgu_vidioc_querycap(struct file *file, void *fh,
+				struct v4l2_capability *cap)
+{
+	struct imgu_device *imgu = video_drvdata(file);
+
+	strscpy(cap->driver, IMGU_NAME, sizeof(cap->driver));
+	strscpy(cap->card, IMGU_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(imgu->pci_dev));
+
+	return 0;
+}
+
+static int enum_fmts(struct v4l2_fmtdesc *f, u32 type)
+{
+	unsigned int i, j;
+
+	if (f->mbus_code != 0 && f->mbus_code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
+	for (i = j = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].type == type) {
+			if (j == f->index)
+				break;
+			++j;
+		}
+	}
+
+	if (i < ARRAY_SIZE(formats)) {
+		f->pixelformat = formats[i].fourcc;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	return enum_fmts(f, VID_CAPTURE);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	return enum_fmts(f, VID_OUTPUT);
+}
+
+/* Propagate forward always the format from the CIO2 subdev */
+static int imgu_vidioc_g_fmt(struct file *file, void *fh,
+			     struct v4l2_format *f)
+{
+	struct imgu_video_device *node = file_to_intel_imgu_node(file);
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+/*
+ * Set input/output format. Unless it is just a try, this also resets
+ * selections (ie. effective and BDS resolutions) to defaults.
+ */
+static int imgu_fmt(struct imgu_device *imgu, unsigned int pipe, int node,
+		    struct v4l2_format *f, bool try)
+{
+	struct device *dev = &imgu->pci_dev->dev;
+	struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
+	struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
+	struct v4l2_mbus_framefmt pad_fmt;
+	unsigned int i, css_q;
+	int ret;
+	struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+	struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
+
+	dev_dbg(dev, "set fmt node [%u][%u](try = %u)", pipe, node, try);
+
+	for (i = 0; i < IMGU_NODE_NUM; i++)
+		dev_dbg(dev, "IMGU pipe %u node %u enabled = %u",
+			pipe, i, imgu_pipe->nodes[i].enabled);
+
+	if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
+		css_pipe->vf_output_en = true;
+
+	if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
+		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
+	else
+		css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
+
+	dev_dbg(dev, "IPU3 pipe %u pipe_id = %u", pipe, css_pipe->pipe_id);
+
+	css_q = imgu_node_to_queue(node);
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		unsigned int inode = imgu_map_node(imgu, i);
+
+		/* Skip the meta node */
+		if (inode == IMGU_NODE_STAT_3A || inode == IMGU_NODE_PARAMS)
+			continue;
+
+		/* CSS expects some format on OUT queue */
+		if (i != IPU3_CSS_QUEUE_OUT &&
+		    !imgu_pipe->nodes[inode].enabled && !try) {
+			fmts[i] = NULL;
+			continue;
+		}
+
+		if (i == css_q) {
+			fmts[i] = &f->fmt.pix_mp;
+			continue;
+		}
+
+		if (try) {
+			fmts[i] = kmemdup(&imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp,
+					  sizeof(struct v4l2_pix_format_mplane),
+					  GFP_KERNEL);
+			if (!fmts[i]) {
+				ret = -ENOMEM;
+				goto out;
+			}
+		} else {
+			fmts[i] = &imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp;
+		}
+
+	}
+
+	if (!try) {
+		/* eff and bds res got by imgu_s_sel */
+		struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
+
+		rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
+		rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
+		rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
+
+		/* suppose that pad fmt was set by subdev s_fmt before */
+		pad_fmt = imgu_pipe->nodes[IMGU_NODE_IN].pad_fmt;
+		rects[IPU3_CSS_RECT_GDC]->width = pad_fmt.width;
+		rects[IPU3_CSS_RECT_GDC]->height = pad_fmt.height;
+	}
+
+	if (!fmts[css_q]) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (try)
+		ret = imgu_css_fmt_try(&imgu->css, fmts, rects, pipe);
+	else
+		ret = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
+
+	/* ret is the binary number in the firmware blob */
+	if (ret < 0)
+		goto out;
+
+	/*
+	 * imgu doesn't set the node to the value given by user
+	 * before we return success from this function, so set it here.
+	 */
+	if (!try)
+		imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp = f->fmt.pix_mp;
+
+out:
+	if (try) {
+		for (i = 0; i < IPU3_CSS_QUEUES; i++)
+			if (i != css_q)
+				kfree(fmts[i]);
+	}
+
+	return ret;
+}
+
+static int imgu_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	const struct imgu_fmt *fmt;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fmt = find_format(f, VID_CAPTURE);
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		fmt = find_format(f, VID_OUTPUT);
+	else
+		return -EINVAL;
+
+	pixm->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int imgu_vidioc_try_fmt(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct imgu_device *imgu = video_drvdata(file);
+	struct device *dev = &imgu->pci_dev->dev;
+	struct imgu_video_device *node = file_to_intel_imgu_node(file);
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	int r;
+
+	dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
+		pix_mp->width, pix_mp->height, node->id);
+
+	r = imgu_try_fmt(file, fh, f);
+	if (r)
+		return r;
+
+	return imgu_fmt(imgu, node->pipe, node->id, f, true);
+}
+
+static int imgu_vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct imgu_device *imgu = video_drvdata(file);
+	struct device *dev = &imgu->pci_dev->dev;
+	struct imgu_video_device *node = file_to_intel_imgu_node(file);
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	int r;
+
+	dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
+		pix_mp->width, pix_mp->height, node->id);
+
+	r = imgu_try_fmt(file, fh, f);
+	if (r)
+		return r;
+
+	return imgu_fmt(imgu, node->pipe, node->id, f, false);
+}
+
+struct imgu_meta_fmt {
+	__u32 fourcc;
+	char *name;
+};
+
+/* From drivers/media/v4l2-core/v4l2-ioctl.c */
+static const struct imgu_meta_fmt meta_fmts[] = {
+	{ V4L2_META_FMT_IPU3_PARAMS, "IPU3 processing parameters" },
+	{ V4L2_META_FMT_IPU3_STAT_3A, "IPU3 3A statistics" },
+};
+
+static int imgu_meta_enum_format(struct file *file, void *fh,
+				 struct v4l2_fmtdesc *fmt)
+{
+	struct imgu_video_device *node = file_to_intel_imgu_node(file);
+	unsigned int i = fmt->type == V4L2_BUF_TYPE_META_OUTPUT ? 0 : 1;
+
+	/* Each node is dedicated to only one meta format */
+	if (fmt->index > 0 || fmt->type != node->vbq.type)
+		return -EINVAL;
+
+	if (fmt->mbus_code != 0 && fmt->mbus_code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
+	strscpy(fmt->description, meta_fmts[i].name, sizeof(fmt->description));
+	fmt->pixelformat = meta_fmts[i].fourcc;
+
+	return 0;
+}
+
+static int imgu_vidioc_g_meta_fmt(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct imgu_video_device *node = file_to_intel_imgu_node(file);
+
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+/******************** function pointers ********************/
+
+static const struct v4l2_subdev_internal_ops imgu_subdev_internal_ops = {
+	.open = imgu_subdev_open,
+};
+
+static const struct v4l2_subdev_core_ops imgu_subdev_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imgu_subdev_video_ops = {
+	.s_stream = imgu_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imgu_subdev_pad_ops = {
+	.link_validate = v4l2_subdev_link_validate_default,
+	.get_fmt = imgu_subdev_get_fmt,
+	.set_fmt = imgu_subdev_set_fmt,
+	.get_selection = imgu_subdev_get_selection,
+	.set_selection = imgu_subdev_set_selection,
+};
+
+static const struct v4l2_subdev_ops imgu_subdev_ops = {
+	.core = &imgu_subdev_core_ops,
+	.video = &imgu_subdev_video_ops,
+	.pad = &imgu_subdev_pad_ops,
+};
+
+static const struct media_entity_operations imgu_media_ops = {
+	.link_setup = imgu_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/****************** vb2_ops of the Q ********************/
+
+static const struct vb2_ops imgu_vb2_ops = {
+	.buf_init = imgu_vb2_buf_init,
+	.buf_cleanup = imgu_vb2_buf_cleanup,
+	.buf_queue = imgu_vb2_buf_queue,
+	.queue_setup = imgu_vb2_queue_setup,
+	.start_streaming = imgu_vb2_start_streaming,
+	.stop_streaming = imgu_vb2_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+/****************** v4l2_file_operations *****************/
+
+static const struct v4l2_file_operations imgu_v4l2_fops = {
+	.unlocked_ioctl = video_ioctl2,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+};
+
+/******************** v4l2_ioctl_ops ********************/
+
+static const struct v4l2_ioctl_ops imgu_v4l2_ioctl_ops = {
+	.vidioc_querycap = imgu_vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap_mplane = imgu_vidioc_g_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = imgu_vidioc_s_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = imgu_vidioc_try_fmt,
+
+	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out_mplane = imgu_vidioc_g_fmt,
+	.vidioc_s_fmt_vid_out_mplane = imgu_vidioc_s_fmt,
+	.vidioc_try_fmt_vid_out_mplane = imgu_vidioc_try_fmt,
+
+	/* buffer queue management */
+	.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_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static const struct v4l2_ioctl_ops imgu_v4l2_meta_ioctl_ops = {
+	.vidioc_querycap = imgu_vidioc_querycap,
+
+	/* meta capture */
+	.vidioc_enum_fmt_meta_cap = imgu_meta_enum_format,
+	.vidioc_g_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
+	.vidioc_s_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
+	.vidioc_try_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
+
+	/* meta output */
+	.vidioc_enum_fmt_meta_out = imgu_meta_enum_format,
+	.vidioc_g_fmt_meta_out = imgu_vidioc_g_meta_fmt,
+	.vidioc_s_fmt_meta_out = imgu_vidioc_g_meta_fmt,
+	.vidioc_try_fmt_meta_out = imgu_vidioc_g_meta_fmt,
+
+	.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_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static int imgu_sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imgu_v4l2_subdev *imgu_sd =
+		container_of(ctrl->handler, struct imgu_v4l2_subdev, ctrl_handler);
+	struct imgu_device *imgu = v4l2_get_subdevdata(&imgu_sd->subdev);
+	struct device *dev = &imgu->pci_dev->dev;
+
+	dev_dbg(dev, "set val %d to ctrl 0x%8x for subdev %u",
+		ctrl->val, ctrl->id, imgu_sd->pipe);
+
+	switch (ctrl->id) {
+	case V4L2_CID_INTEL_IPU3_MODE:
+		atomic_set(&imgu_sd->running_mode, ctrl->val);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops imgu_subdev_ctrl_ops = {
+	.s_ctrl = imgu_sd_s_ctrl,
+};
+
+static const char * const imgu_ctrl_mode_strings[] = {
+	"Video mode",
+	"Still mode",
+};
+
+static const struct v4l2_ctrl_config imgu_subdev_ctrl_mode = {
+	.ops = &imgu_subdev_ctrl_ops,
+	.id = V4L2_CID_INTEL_IPU3_MODE,
+	.name = "IPU3 Pipe Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(imgu_ctrl_mode_strings) - 1,
+	.def = IPU3_RUNNING_MODE_VIDEO,
+	.qmenu = imgu_ctrl_mode_strings,
+};
+
+/******************** Framework registration ********************/
+
+/* helper function to config node's video properties */
+static void imgu_node_to_v4l2(u32 node, struct video_device *vdev,
+			      struct v4l2_format *f)
+{
+	u32 cap;
+
+	/* Should not happen */
+	WARN_ON(node >= IMGU_NODE_NUM);
+
+	switch (node) {
+	case IMGU_NODE_IN:
+		cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+		f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+		vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
+		break;
+	case IMGU_NODE_PARAMS:
+		cap = V4L2_CAP_META_OUTPUT;
+		f->type = V4L2_BUF_TYPE_META_OUTPUT;
+		f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_PARAMS;
+		vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
+		imgu_css_meta_fmt_set(&f->fmt.meta);
+		break;
+	case IMGU_NODE_STAT_3A:
+		cap = V4L2_CAP_META_CAPTURE;
+		f->type = V4L2_BUF_TYPE_META_CAPTURE;
+		f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_STAT_3A;
+		vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
+		imgu_css_meta_fmt_set(&f->fmt.meta);
+		break;
+	default:
+		cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
+		f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
+	}
+
+	vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_IO_MC | cap;
+}
+
+static int imgu_v4l2_subdev_register(struct imgu_device *imgu,
+				     struct imgu_v4l2_subdev *imgu_sd,
+				     unsigned int pipe)
+{
+	int i, r;
+	struct v4l2_ctrl_handler *hdl = &imgu_sd->ctrl_handler;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	/* Initialize subdev media entity */
+	r = media_entity_pads_init(&imgu_sd->subdev.entity, IMGU_NODE_NUM,
+				   imgu_sd->subdev_pads);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed initialize subdev media entity (%d)\n", r);
+		return r;
+	}
+	imgu_sd->subdev.entity.ops = &imgu_media_ops;
+	for (i = 0; i < IMGU_NODE_NUM; i++) {
+		imgu_sd->subdev_pads[i].flags = imgu_pipe->nodes[i].output ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&imgu_sd->subdev, &imgu_subdev_ops);
+	imgu_sd->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
+	imgu_sd->subdev.internal_ops = &imgu_subdev_internal_ops;
+	imgu_sd->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
+				V4L2_SUBDEV_FL_HAS_EVENTS;
+	snprintf(imgu_sd->subdev.name, sizeof(imgu_sd->subdev.name),
+		 "%s %u", IMGU_NAME, pipe);
+	v4l2_set_subdevdata(&imgu_sd->subdev, imgu);
+	atomic_set(&imgu_sd->running_mode, IPU3_RUNNING_MODE_VIDEO);
+	v4l2_ctrl_handler_init(hdl, 1);
+	imgu_sd->subdev.ctrl_handler = hdl;
+	imgu_sd->ctrl = v4l2_ctrl_new_custom(hdl, &imgu_subdev_ctrl_mode, NULL);
+	if (hdl->error) {
+		r = hdl->error;
+		dev_err(&imgu->pci_dev->dev,
+			"failed to create subdev v4l2 ctrl with err %d", r);
+		goto fail_subdev;
+	}
+	r = v4l2_device_register_subdev(&imgu->v4l2_dev, &imgu_sd->subdev);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed initialize subdev (%d)\n", r);
+		goto fail_subdev;
+	}
+
+	imgu_sd->pipe = pipe;
+	return 0;
+
+fail_subdev:
+	v4l2_ctrl_handler_free(imgu_sd->subdev.ctrl_handler);
+	media_entity_cleanup(&imgu_sd->subdev.entity);
+
+	return r;
+}
+
+static int imgu_v4l2_node_setup(struct imgu_device *imgu, unsigned int pipe,
+				int node_num)
+{
+	int r;
+	u32 flags;
+	struct v4l2_mbus_framefmt def_bus_fmt = { 0 };
+	struct v4l2_pix_format_mplane def_pix_fmt = { 0 };
+	struct device *dev = &imgu->pci_dev->dev;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+	struct v4l2_subdev *sd = &imgu_pipe->imgu_sd.subdev;
+	struct imgu_video_device *node = &imgu_pipe->nodes[node_num];
+	struct video_device *vdev = &node->vdev;
+	struct vb2_queue *vbq = &node->vbq;
+
+	/* Initialize formats to default values */
+	def_bus_fmt.width = 1920;
+	def_bus_fmt.height = 1080;
+	def_bus_fmt.code = MEDIA_BUS_FMT_FIXED;
+	def_bus_fmt.field = V4L2_FIELD_NONE;
+	def_bus_fmt.colorspace = V4L2_COLORSPACE_RAW;
+	def_bus_fmt.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	def_bus_fmt.quantization = V4L2_QUANTIZATION_DEFAULT;
+	def_bus_fmt.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+	def_pix_fmt.width = def_bus_fmt.width;
+	def_pix_fmt.height = def_bus_fmt.height;
+	def_pix_fmt.field = def_bus_fmt.field;
+	def_pix_fmt.num_planes = 1;
+	def_pix_fmt.plane_fmt[0].bytesperline =
+		imgu_bytesperline(def_pix_fmt.width,
+				  IMGU_ABI_FRAME_FORMAT_RAW_PACKED);
+	def_pix_fmt.plane_fmt[0].sizeimage =
+		def_pix_fmt.height * def_pix_fmt.plane_fmt[0].bytesperline;
+	def_pix_fmt.flags = 0;
+	def_pix_fmt.colorspace = def_bus_fmt.colorspace;
+	def_pix_fmt.ycbcr_enc = def_bus_fmt.ycbcr_enc;
+	def_pix_fmt.quantization = def_bus_fmt.quantization;
+	def_pix_fmt.xfer_func = def_bus_fmt.xfer_func;
+
+	/* Initialize miscellaneous variables */
+	mutex_init(&node->lock);
+	INIT_LIST_HEAD(&node->buffers);
+
+	/* Initialize formats to default values */
+	node->pad_fmt = def_bus_fmt;
+	node->id = node_num;
+	node->pipe = pipe;
+	imgu_node_to_v4l2(node_num, vdev, &node->vdev_fmt);
+	if (node->vdev_fmt.type ==
+	    V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE ||
+	    node->vdev_fmt.type ==
+	    V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		def_pix_fmt.pixelformat = node->output ?
+			V4L2_PIX_FMT_IPU3_SGRBG10 :
+			V4L2_PIX_FMT_NV12;
+		node->vdev_fmt.fmt.pix_mp = def_pix_fmt;
+	}
+
+	/* Initialize media entities */
+	r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
+	if (r) {
+		dev_err(dev, "failed initialize media entity (%d)\n", r);
+		mutex_destroy(&node->lock);
+		return r;
+	}
+	node->vdev_pad.flags = node->output ?
+		MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
+	vdev->entity.ops = NULL;
+
+	/* Initialize vbq */
+	vbq->type = node->vdev_fmt.type;
+	vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF;
+	vbq->ops = &imgu_vb2_ops;
+	vbq->mem_ops = &vb2_dma_sg_memops;
+	if (imgu->buf_struct_size <= 0)
+		imgu->buf_struct_size =
+			sizeof(struct imgu_vb2_buffer);
+	vbq->buf_struct_size = imgu->buf_struct_size;
+	vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	/* can streamon w/o buffers */
+	vbq->min_buffers_needed = 0;
+	vbq->drv_priv = imgu;
+	vbq->lock = &node->lock;
+	r = vb2_queue_init(vbq);
+	if (r) {
+		dev_err(dev, "failed to initialize video queue (%d)", r);
+		media_entity_cleanup(&vdev->entity);
+		return r;
+	}
+
+	/* Initialize vdev */
+	snprintf(vdev->name, sizeof(vdev->name), "%s %u %s",
+		 IMGU_NAME, pipe, node->name);
+	vdev->release = video_device_release_empty;
+	vdev->fops = &imgu_v4l2_fops;
+	vdev->lock = &node->lock;
+	vdev->v4l2_dev = &imgu->v4l2_dev;
+	vdev->queue = &node->vbq;
+	vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
+	video_set_drvdata(vdev, imgu);
+	r = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (r) {
+		dev_err(dev, "failed to register video device (%d)", r);
+		media_entity_cleanup(&vdev->entity);
+		return r;
+	}
+
+	/* Create link between video node and the subdev pad */
+	flags = 0;
+	if (node->enabled)
+		flags |= MEDIA_LNK_FL_ENABLED;
+	if (node->output) {
+		r = media_create_pad_link(&vdev->entity, 0, &sd->entity,
+					  node_num, flags);
+	} else {
+		if (node->id == IMGU_NODE_OUT) {
+			flags |= MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
+			node->enabled = true;
+		}
+
+		r = media_create_pad_link(&sd->entity, node_num, &vdev->entity,
+					  0, flags);
+	}
+	if (r) {
+		dev_err(dev, "failed to create pad link (%d)", r);
+		video_unregister_device(vdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void imgu_v4l2_nodes_cleanup_pipe(struct imgu_device *imgu,
+					 unsigned int pipe, int node)
+{
+	int i;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	for (i = 0; i < node; i++) {
+		video_unregister_device(&imgu_pipe->nodes[i].vdev);
+		media_entity_cleanup(&imgu_pipe->nodes[i].vdev.entity);
+		mutex_destroy(&imgu_pipe->nodes[i].lock);
+	}
+}
+
+static int imgu_v4l2_nodes_setup_pipe(struct imgu_device *imgu, int pipe)
+{
+	int i;
+
+	for (i = 0; i < IMGU_NODE_NUM; i++) {
+		int r = imgu_v4l2_node_setup(imgu, pipe, i);
+
+		if (r) {
+			imgu_v4l2_nodes_cleanup_pipe(imgu, pipe, i);
+			return r;
+		}
+	}
+	return 0;
+}
+
+static void imgu_v4l2_subdev_cleanup(struct imgu_device *imgu, unsigned int i)
+{
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[i];
+
+	v4l2_device_unregister_subdev(&imgu_pipe->imgu_sd.subdev);
+	v4l2_ctrl_handler_free(imgu_pipe->imgu_sd.subdev.ctrl_handler);
+	media_entity_cleanup(&imgu_pipe->imgu_sd.subdev.entity);
+}
+
+static void imgu_v4l2_cleanup_pipes(struct imgu_device *imgu, unsigned int pipe)
+{
+	int i;
+
+	for (i = 0; i < pipe; i++) {
+		imgu_v4l2_nodes_cleanup_pipe(imgu, i, IMGU_NODE_NUM);
+		imgu_v4l2_subdev_cleanup(imgu, i);
+	}
+}
+
+static int imgu_v4l2_register_pipes(struct imgu_device *imgu)
+{
+	struct imgu_media_pipe *imgu_pipe;
+	int i, r;
+
+	for (i = 0; i < IMGU_MAX_PIPE_NUM; i++) {
+		imgu_pipe = &imgu->imgu_pipe[i];
+		r = imgu_v4l2_subdev_register(imgu, &imgu_pipe->imgu_sd, i);
+		if (r) {
+			dev_err(&imgu->pci_dev->dev,
+				"failed to register subdev%u ret (%d)\n", i, r);
+			goto pipes_cleanup;
+		}
+		r = imgu_v4l2_nodes_setup_pipe(imgu, i);
+		if (r) {
+			imgu_v4l2_subdev_cleanup(imgu, i);
+			goto pipes_cleanup;
+		}
+	}
+
+	return 0;
+
+pipes_cleanup:
+	imgu_v4l2_cleanup_pipes(imgu, i);
+	return r;
+}
+
+int imgu_v4l2_register(struct imgu_device *imgu)
+{
+	int r;
+
+	/* Initialize miscellaneous variables */
+	imgu->streaming = false;
+
+	/* Set up media device */
+	media_device_pci_init(&imgu->media_dev, imgu->pci_dev, IMGU_NAME);
+
+	/* Set up v4l2 device */
+	imgu->v4l2_dev.mdev = &imgu->media_dev;
+	imgu->v4l2_dev.ctrl_handler = NULL;
+	r = v4l2_device_register(&imgu->pci_dev->dev, &imgu->v4l2_dev);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed to register V4L2 device (%d)\n", r);
+		goto fail_v4l2_dev;
+	}
+
+	r = imgu_v4l2_register_pipes(imgu);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed to register pipes (%d)\n", r);
+		goto fail_v4l2_pipes;
+	}
+
+	r = v4l2_device_register_subdev_nodes(&imgu->v4l2_dev);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed to register subdevs (%d)\n", r);
+		goto fail_subdevs;
+	}
+
+	r = media_device_register(&imgu->media_dev);
+	if (r) {
+		dev_err(&imgu->pci_dev->dev,
+			"failed to register media device (%d)\n", r);
+		goto fail_subdevs;
+	}
+
+	return 0;
+
+fail_subdevs:
+	imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
+fail_v4l2_pipes:
+	v4l2_device_unregister(&imgu->v4l2_dev);
+fail_v4l2_dev:
+	media_device_cleanup(&imgu->media_dev);
+
+	return r;
+}
+
+int imgu_v4l2_unregister(struct imgu_device *imgu)
+{
+	media_device_unregister(&imgu->media_dev);
+	imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
+	v4l2_device_unregister(&imgu->v4l2_dev);
+	media_device_cleanup(&imgu->media_dev);
+
+	return 0;
+}
+
+void imgu_v4l2_buffer_done(struct vb2_buffer *vb,
+			   enum vb2_buffer_state state)
+{
+	struct imgu_vb2_buffer *b =
+		container_of(vb, struct imgu_vb2_buffer, vbb.vb2_buf);
+
+	list_del(&b->list);
+	vb2_buffer_done(&b->vbb.vb2_buf, state);
+}
diff --git a/drivers/staging/media/ipu3/ipu3.c b/drivers/staging/media/ipu3/ipu3.c
new file mode 100644
index 000000000..0c453b37f
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3.c
@@ -0,0 +1,868 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2017 - 2018 Intel Corporation
+ * Copyright 2017 Google LLC
+ *
+ * Based on Intel IPU4 driver.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+
+#include "ipu3.h"
+#include "ipu3-dmamap.h"
+#include "ipu3-mmu.h"
+
+#define IMGU_PCI_ID			0x1919
+#define IMGU_PCI_BAR			0
+#define IMGU_DMA_MASK			DMA_BIT_MASK(39)
+#define IMGU_MAX_QUEUE_DEPTH		(2 + 2)
+
+/*
+ * pre-allocated buffer size for IMGU dummy buffers. Those
+ * values should be tuned to big enough to avoid buffer
+ * re-allocation when streaming to lower streaming latency.
+ */
+#define CSS_QUEUE_IN_BUF_SIZE		0
+#define CSS_QUEUE_PARAMS_BUF_SIZE	0
+#define CSS_QUEUE_OUT_BUF_SIZE		(4160 * 3120 * 12 / 8)
+#define CSS_QUEUE_VF_BUF_SIZE		(1920 * 1080 * 12 / 8)
+#define CSS_QUEUE_STAT_3A_BUF_SIZE	sizeof(struct ipu3_uapi_stats_3a)
+
+static const size_t css_queue_buf_size_map[IPU3_CSS_QUEUES] = {
+	[IPU3_CSS_QUEUE_IN] = CSS_QUEUE_IN_BUF_SIZE,
+	[IPU3_CSS_QUEUE_PARAMS] = CSS_QUEUE_PARAMS_BUF_SIZE,
+	[IPU3_CSS_QUEUE_OUT] = CSS_QUEUE_OUT_BUF_SIZE,
+	[IPU3_CSS_QUEUE_VF] = CSS_QUEUE_VF_BUF_SIZE,
+	[IPU3_CSS_QUEUE_STAT_3A] = CSS_QUEUE_STAT_3A_BUF_SIZE,
+};
+
+static const struct imgu_node_mapping imgu_node_map[IMGU_NODE_NUM] = {
+	[IMGU_NODE_IN] = {IPU3_CSS_QUEUE_IN, "input"},
+	[IMGU_NODE_PARAMS] = {IPU3_CSS_QUEUE_PARAMS, "parameters"},
+	[IMGU_NODE_OUT] = {IPU3_CSS_QUEUE_OUT, "output"},
+	[IMGU_NODE_VF] = {IPU3_CSS_QUEUE_VF, "viewfinder"},
+	[IMGU_NODE_STAT_3A] = {IPU3_CSS_QUEUE_STAT_3A, "3a stat"},
+};
+
+unsigned int imgu_node_to_queue(unsigned int node)
+{
+	return imgu_node_map[node].css_queue;
+}
+
+unsigned int imgu_map_node(struct imgu_device *imgu, unsigned int css_queue)
+{
+	unsigned int i;
+
+	for (i = 0; i < IMGU_NODE_NUM; i++)
+		if (imgu_node_map[i].css_queue == css_queue)
+			break;
+
+	return i;
+}
+
+/**************** Dummy buffers ****************/
+
+static void imgu_dummybufs_cleanup(struct imgu_device *imgu, unsigned int pipe)
+{
+	unsigned int i;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	for (i = 0; i < IPU3_CSS_QUEUES; i++)
+		imgu_dmamap_free(imgu,
+				 &imgu_pipe->queues[i].dmap);
+}
+
+static int imgu_dummybufs_preallocate(struct imgu_device *imgu,
+				      unsigned int pipe)
+{
+	unsigned int i;
+	size_t size;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		size = css_queue_buf_size_map[i];
+		/*
+		 * Do not enable dummy buffers for master queue,
+		 * always require that real buffers from user are
+		 * available.
+		 */
+		if (i == IMGU_QUEUE_MASTER || size == 0)
+			continue;
+
+		if (!imgu_dmamap_alloc(imgu,
+				       &imgu_pipe->queues[i].dmap, size)) {
+			imgu_dummybufs_cleanup(imgu, pipe);
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+static int imgu_dummybufs_init(struct imgu_device *imgu, unsigned int pipe)
+{
+	const struct v4l2_pix_format_mplane *mpix;
+	const struct v4l2_meta_format	*meta;
+	unsigned int i, k, node;
+	size_t size;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	/* Allocate a dummy buffer for each queue where buffer is optional */
+	for (i = 0; i < IPU3_CSS_QUEUES; i++) {
+		node = imgu_map_node(imgu, i);
+		if (!imgu_pipe->queue_enabled[node] || i == IMGU_QUEUE_MASTER)
+			continue;
+
+		if (!imgu_pipe->nodes[IMGU_NODE_VF].enabled &&
+		    i == IPU3_CSS_QUEUE_VF)
+			/*
+			 * Do not enable dummy buffers for VF if it is not
+			 * requested by the user.
+			 */
+			continue;
+
+		meta = &imgu_pipe->nodes[node].vdev_fmt.fmt.meta;
+		mpix = &imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp;
+
+		if (node == IMGU_NODE_STAT_3A || node == IMGU_NODE_PARAMS)
+			size = meta->buffersize;
+		else
+			size = mpix->plane_fmt[0].sizeimage;
+
+		if (imgu_css_dma_buffer_resize(imgu,
+					       &imgu_pipe->queues[i].dmap,
+					       size)) {
+			imgu_dummybufs_cleanup(imgu, pipe);
+			return -ENOMEM;
+		}
+
+		for (k = 0; k < IMGU_MAX_QUEUE_DEPTH; k++)
+			imgu_css_buf_init(&imgu_pipe->queues[i].dummybufs[k], i,
+					  imgu_pipe->queues[i].dmap.daddr);
+	}
+
+	return 0;
+}
+
+/* May be called from atomic context */
+static struct imgu_css_buffer *imgu_dummybufs_get(struct imgu_device *imgu,
+						   int queue, unsigned int pipe)
+{
+	unsigned int i;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	/* dummybufs are not allocated for master q */
+	if (queue == IPU3_CSS_QUEUE_IN)
+		return NULL;
+
+	if (WARN_ON(!imgu_pipe->queues[queue].dmap.vaddr))
+		/* Buffer should not be allocated here */
+		return NULL;
+
+	for (i = 0; i < IMGU_MAX_QUEUE_DEPTH; i++)
+		if (imgu_css_buf_state(&imgu_pipe->queues[queue].dummybufs[i]) !=
+			IPU3_CSS_BUFFER_QUEUED)
+			break;
+
+	if (i == IMGU_MAX_QUEUE_DEPTH)
+		return NULL;
+
+	imgu_css_buf_init(&imgu_pipe->queues[queue].dummybufs[i], queue,
+			  imgu_pipe->queues[queue].dmap.daddr);
+
+	return &imgu_pipe->queues[queue].dummybufs[i];
+}
+
+/* Check if given buffer is a dummy buffer */
+static bool imgu_dummybufs_check(struct imgu_device *imgu,
+				 struct imgu_css_buffer *buf,
+				 unsigned int pipe)
+{
+	unsigned int i;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	for (i = 0; i < IMGU_MAX_QUEUE_DEPTH; i++)
+		if (buf == &imgu_pipe->queues[buf->queue].dummybufs[i])
+			break;
+
+	return i < IMGU_MAX_QUEUE_DEPTH;
+}
+
+static void imgu_buffer_done(struct imgu_device *imgu, struct vb2_buffer *vb,
+			     enum vb2_buffer_state state)
+{
+	mutex_lock(&imgu->lock);
+	imgu_v4l2_buffer_done(vb, state);
+	mutex_unlock(&imgu->lock);
+}
+
+static struct imgu_css_buffer *imgu_queue_getbuf(struct imgu_device *imgu,
+						 unsigned int node,
+						 unsigned int pipe)
+{
+	struct imgu_buffer *buf;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	if (WARN_ON(node >= IMGU_NODE_NUM))
+		return NULL;
+
+	/* Find first free buffer from the node */
+	list_for_each_entry(buf, &imgu_pipe->nodes[node].buffers, vid_buf.list) {
+		if (imgu_css_buf_state(&buf->css_buf) == IPU3_CSS_BUFFER_NEW)
+			return &buf->css_buf;
+	}
+
+	/* There were no free buffers, try to return a dummy buffer */
+	return imgu_dummybufs_get(imgu, imgu_node_map[node].css_queue, pipe);
+}
+
+/*
+ * Queue as many buffers to CSS as possible. If all buffers don't fit into
+ * CSS buffer queues, they remain unqueued and will be queued later.
+ */
+int imgu_queue_buffers(struct imgu_device *imgu, bool initial, unsigned int pipe)
+{
+	unsigned int node;
+	int r = 0;
+	struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
+
+	if (!imgu_css_is_streaming(&imgu->css))
+		return 0;
+
+	dev_dbg(&imgu->pci_dev->dev, "Queue buffers to pipe %d", pipe);
+	mutex_lock(&imgu->lock);
+
+	if (!imgu_css_pipe_queue_empty(&imgu->css, pipe)) {
+		mutex_unlock(&imgu->lock);
+		return 0;
+	}
+
+	/* Buffer set is queued to FW only when input buffer is ready */
+	for (node = IMGU_NODE_NUM - 1;
+	     imgu_queue_getbuf(imgu, IMGU_NODE_IN, pipe);
+	     node = node ? node - 1 : IMGU_NODE_NUM - 1) {
+		if (node == IMGU_NODE_VF &&
+		    !imgu_pipe->nodes[IMGU_NODE_VF].enabled) {
+			dev_warn(&imgu->pci_dev->dev,
+				 "Vf not enabled, ignore queue");
+			continue;
+		} else if (node == IMGU_NODE_PARAMS &&
+			   imgu_pipe->nodes[node].enabled) {
+			struct vb2_buffer *vb;
+			struct imgu_vb2_buffer *ivb;
+
+			/* No parameters for this frame */
+			if (list_empty(&imgu_pipe->nodes[node].buffers))
+				continue;
+
+			ivb = list_first_entry(&imgu_pipe->nodes[node].buffers,
+					       struct imgu_vb2_buffer, list);
+			list_del(&ivb->list);
+			vb = &ivb->vbb.vb2_buf;
+			r = imgu_css_set_parameters(&imgu->css, pipe,
+						    vb2_plane_vaddr(vb, 0));
+			if (r) {
+				vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+				dev_warn(&imgu->pci_dev->dev,
+					 "set parameters failed.");
+				continue;
+			}
+
+			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+			dev_dbg(&imgu->pci_dev->dev,
+				"queue user parameters %d to css.", vb->index);
+		} else if (imgu_pipe->queue_enabled[node]) {
+			struct imgu_css_buffer *buf =
+				imgu_queue_getbuf(imgu, node, pipe);
+			struct imgu_buffer *ibuf = NULL;
+			bool dummy;
+
+			if (!buf)
+				break;
+
+			r = imgu_css_buf_queue(&imgu->css, pipe, buf);
+			if (r)
+				break;
+			dummy = imgu_dummybufs_check(imgu, buf, pipe);
+			if (!dummy)
+				ibuf = container_of(buf, struct imgu_buffer,
+						    css_buf);
+			dev_dbg(&imgu->pci_dev->dev,
+				"queue %s %s buffer %u to css da: 0x%08x\n",
+				dummy ? "dummy" : "user",
+				imgu_node_map[node].name,
+				dummy ? 0 : ibuf->vid_buf.vbb.vb2_buf.index,
+				(u32)buf->daddr);
+		}
+	}
+	mutex_unlock(&imgu->lock);
+
+	if (r && r != -EBUSY)
+		goto failed;
+
+	return 0;
+
+failed:
+	/*
+	 * On error, mark all buffers as failed which are not
+	 * yet queued to CSS
+	 */
+	dev_err(&imgu->pci_dev->dev,
+		"failed to queue buffer to CSS on queue %i (%d)\n",
+		node, r);
+
+	if (initial)
+		/* If we were called from streamon(), no need to finish bufs */
+		return r;
+
+	for (node = 0; node < IMGU_NODE_NUM; node++) {
+		struct imgu_buffer *buf, *buf0;
+
+		if (!imgu_pipe->queue_enabled[node])
+			continue;	/* Skip disabled queues */
+
+		mutex_lock(&imgu->lock);
+		list_for_each_entry_safe(buf, buf0,
+					 &imgu_pipe->nodes[node].buffers,
+					 vid_buf.list) {
+			if (imgu_css_buf_state(&buf->css_buf) ==
+			    IPU3_CSS_BUFFER_QUEUED)
+				continue;	/* Was already queued, skip */
+
+			imgu_v4l2_buffer_done(&buf->vid_buf.vbb.vb2_buf,
+					      VB2_BUF_STATE_ERROR);
+		}
+		mutex_unlock(&imgu->lock);
+	}
+
+	return r;
+}
+
+static int imgu_powerup(struct imgu_device *imgu)
+{
+	int r;
+	unsigned int pipe;
+	unsigned int freq = 200;
+	struct v4l2_mbus_framefmt *fmt;
+
+	/* input larger than 2048*1152, ask imgu to work on high freq */
+	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		fmt = &imgu->imgu_pipe[pipe].nodes[IMGU_NODE_IN].pad_fmt;
+		dev_dbg(&imgu->pci_dev->dev, "pipe %u input format = %ux%u",
+			pipe, fmt->width, fmt->height);
+		if ((fmt->width * fmt->height) >= (2048 * 1152))
+			freq = 450;
+	}
+
+	r = imgu_css_set_powerup(&imgu->pci_dev->dev, imgu->base, freq);
+	if (r)
+		return r;
+
+	imgu_mmu_resume(imgu->mmu);
+	return 0;
+}
+
+static void imgu_powerdown(struct imgu_device *imgu)
+{
+	imgu_mmu_suspend(imgu->mmu);
+	imgu_css_set_powerdown(&imgu->pci_dev->dev, imgu->base);
+}
+
+int imgu_s_stream(struct imgu_device *imgu, int enable)
+{
+	struct device *dev = &imgu->pci_dev->dev;
+	int r, pipe;
+
+	if (!enable) {
+		/* Stop streaming */
+		dev_dbg(dev, "stream off\n");
+		/* Block new buffers to be queued to CSS. */
+		atomic_set(&imgu->qbuf_barrier, 1);
+		imgu_css_stop_streaming(&imgu->css);
+		synchronize_irq(imgu->pci_dev->irq);
+		atomic_set(&imgu->qbuf_barrier, 0);
+		imgu_powerdown(imgu);
+		pm_runtime_put(&imgu->pci_dev->dev);
+
+		return 0;
+	}
+
+	/* Set Power */
+	r = pm_runtime_resume_and_get(dev);
+	if (r < 0) {
+		dev_err(dev, "failed to set imgu power\n");
+		return r;
+	}
+
+	r = imgu_powerup(imgu);
+	if (r) {
+		dev_err(dev, "failed to power up imgu\n");
+		pm_runtime_put(dev);
+		return r;
+	}
+
+	/* Start CSS streaming */
+	r = imgu_css_start_streaming(&imgu->css);
+	if (r) {
+		dev_err(dev, "failed to start css streaming (%d)", r);
+		goto fail_start_streaming;
+	}
+
+	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		/* Initialize dummy buffers */
+		r = imgu_dummybufs_init(imgu, pipe);
+		if (r) {
+			dev_err(dev, "failed to initialize dummy buffers (%d)", r);
+			goto fail_dummybufs;
+		}
+
+		/* Queue as many buffers from queue as possible */
+		r = imgu_queue_buffers(imgu, true, pipe);
+		if (r) {
+			dev_err(dev, "failed to queue initial buffers (%d)", r);
+			goto fail_queueing;
+		}
+	}
+
+	return 0;
+fail_queueing:
+	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM)
+		imgu_dummybufs_cleanup(imgu, pipe);
+fail_dummybufs:
+	imgu_css_stop_streaming(&imgu->css);
+fail_start_streaming:
+	pm_runtime_put(dev);
+
+	return r;
+}
+
+static void imgu_video_nodes_exit(struct imgu_device *imgu)
+{
+	int i;
+
+	for (i = 0; i < IMGU_MAX_PIPE_NUM; i++)
+		imgu_dummybufs_cleanup(imgu, i);
+
+	imgu_v4l2_unregister(imgu);
+}
+
+static int imgu_video_nodes_init(struct imgu_device *imgu)
+{
+	struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
+	struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
+	struct imgu_media_pipe *imgu_pipe;
+	unsigned int i, j;
+	int r;
+
+	imgu->buf_struct_size = sizeof(struct imgu_buffer);
+
+	for (j = 0; j < IMGU_MAX_PIPE_NUM; j++) {
+		imgu_pipe = &imgu->imgu_pipe[j];
+
+		for (i = 0; i < IMGU_NODE_NUM; i++) {
+			imgu_pipe->nodes[i].name = imgu_node_map[i].name;
+			imgu_pipe->nodes[i].output = i < IMGU_QUEUE_FIRST_INPUT;
+			imgu_pipe->nodes[i].enabled = false;
+
+			if (i != IMGU_NODE_PARAMS && i != IMGU_NODE_STAT_3A)
+				fmts[imgu_node_map[i].css_queue] =
+					&imgu_pipe->nodes[i].vdev_fmt.fmt.pix_mp;
+			atomic_set(&imgu_pipe->nodes[i].sequence, 0);
+		}
+	}
+
+	r = imgu_v4l2_register(imgu);
+	if (r)
+		return r;
+
+	/* Set initial formats and initialize formats of video nodes */
+	for (j = 0; j < IMGU_MAX_PIPE_NUM; j++) {
+		imgu_pipe = &imgu->imgu_pipe[j];
+
+		rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_pipe->imgu_sd.rect.eff;
+		rects[IPU3_CSS_RECT_BDS] = &imgu_pipe->imgu_sd.rect.bds;
+		imgu_css_fmt_set(&imgu->css, fmts, rects, j);
+
+		/* Pre-allocate dummy buffers */
+		r = imgu_dummybufs_preallocate(imgu, j);
+		if (r) {
+			dev_err(&imgu->pci_dev->dev,
+				"failed to pre-allocate dummy buffers (%d)", r);
+			goto out_cleanup;
+		}
+	}
+
+	return 0;
+
+out_cleanup:
+	imgu_video_nodes_exit(imgu);
+
+	return r;
+}
+
+/**************** PCI interface ****************/
+
+static irqreturn_t imgu_isr_threaded(int irq, void *imgu_ptr)
+{
+	struct imgu_device *imgu = imgu_ptr;
+	struct imgu_media_pipe *imgu_pipe;
+	int p;
+
+	/* Dequeue / queue buffers */
+	do {
+		u64 ns = ktime_get_ns();
+		struct imgu_css_buffer *b;
+		struct imgu_buffer *buf = NULL;
+		unsigned int node, pipe;
+		bool dummy;
+
+		do {
+			mutex_lock(&imgu->lock);
+			b = imgu_css_buf_dequeue(&imgu->css);
+			mutex_unlock(&imgu->lock);
+		} while (PTR_ERR(b) == -EAGAIN);
+
+		if (IS_ERR(b)) {
+			if (PTR_ERR(b) != -EBUSY)	/* All done */
+				dev_err(&imgu->pci_dev->dev,
+					"failed to dequeue buffers (%ld)\n",
+					PTR_ERR(b));
+			break;
+		}
+
+		node = imgu_map_node(imgu, b->queue);
+		pipe = b->pipe;
+		dummy = imgu_dummybufs_check(imgu, b, pipe);
+		if (!dummy)
+			buf = container_of(b, struct imgu_buffer, css_buf);
+		dev_dbg(&imgu->pci_dev->dev,
+			"dequeue %s %s buffer %d daddr 0x%x from css\n",
+			dummy ? "dummy" : "user",
+			imgu_node_map[node].name,
+			dummy ? 0 : buf->vid_buf.vbb.vb2_buf.index,
+			(u32)b->daddr);
+
+		if (dummy)
+			/* It was a dummy buffer, skip it */
+			continue;
+
+		/* Fill vb2 buffer entries and tell it's ready */
+		imgu_pipe = &imgu->imgu_pipe[pipe];
+		if (!imgu_pipe->nodes[node].output) {
+			buf->vid_buf.vbb.vb2_buf.timestamp = ns;
+			buf->vid_buf.vbb.field = V4L2_FIELD_NONE;
+			buf->vid_buf.vbb.sequence =
+				atomic_inc_return(
+				&imgu_pipe->nodes[node].sequence);
+			dev_dbg(&imgu->pci_dev->dev, "vb2 buffer sequence %d",
+				buf->vid_buf.vbb.sequence);
+		}
+		imgu_buffer_done(imgu, &buf->vid_buf.vbb.vb2_buf,
+				 imgu_css_buf_state(&buf->css_buf) ==
+						    IPU3_CSS_BUFFER_DONE ?
+						    VB2_BUF_STATE_DONE :
+						    VB2_BUF_STATE_ERROR);
+		mutex_lock(&imgu->lock);
+		if (imgu_css_queue_empty(&imgu->css))
+			wake_up_all(&imgu->buf_drain_wq);
+		mutex_unlock(&imgu->lock);
+	} while (1);
+
+	/*
+	 * Try to queue more buffers for CSS.
+	 * qbuf_barrier is used to disable new buffers
+	 * to be queued to CSS.
+	 */
+	if (!atomic_read(&imgu->qbuf_barrier))
+		for_each_set_bit(p, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM)
+			imgu_queue_buffers(imgu, false, p);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t imgu_isr(int irq, void *imgu_ptr)
+{
+	struct imgu_device *imgu = imgu_ptr;
+
+	/* acknowledge interruption */
+	if (imgu_css_irq_ack(&imgu->css) < 0)
+		return IRQ_NONE;
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int imgu_pci_config_setup(struct pci_dev *dev)
+{
+	u16 pci_command;
+	int r = pci_enable_msi(dev);
+
+	if (r) {
+		dev_err(&dev->dev, "failed to enable MSI (%d)\n", r);
+		return r;
+	}
+
+	pci_read_config_word(dev, PCI_COMMAND, &pci_command);
+	pci_command |= PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
+			PCI_COMMAND_INTX_DISABLE;
+	pci_write_config_word(dev, PCI_COMMAND, pci_command);
+
+	return 0;
+}
+
+static int imgu_pci_probe(struct pci_dev *pci_dev,
+			  const struct pci_device_id *id)
+{
+	struct imgu_device *imgu;
+	phys_addr_t phys;
+	unsigned long phys_len;
+	void __iomem *const *iomap;
+	int r;
+
+	imgu = devm_kzalloc(&pci_dev->dev, sizeof(*imgu), GFP_KERNEL);
+	if (!imgu)
+		return -ENOMEM;
+
+	imgu->pci_dev = pci_dev;
+
+	r = pcim_enable_device(pci_dev);
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to enable device (%d)\n", r);
+		return r;
+	}
+
+	dev_info(&pci_dev->dev, "device 0x%x (rev: 0x%x)\n",
+		 pci_dev->device, pci_dev->revision);
+
+	phys = pci_resource_start(pci_dev, IMGU_PCI_BAR);
+	phys_len = pci_resource_len(pci_dev, IMGU_PCI_BAR);
+
+	r = pcim_iomap_regions(pci_dev, 1 << IMGU_PCI_BAR, pci_name(pci_dev));
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to remap I/O memory (%d)\n", r);
+		return r;
+	}
+	dev_info(&pci_dev->dev, "physical base address %pap, %lu bytes\n",
+		 &phys, phys_len);
+
+	iomap = pcim_iomap_table(pci_dev);
+	if (!iomap) {
+		dev_err(&pci_dev->dev, "failed to iomap table\n");
+		return -ENODEV;
+	}
+
+	imgu->base = iomap[IMGU_PCI_BAR];
+
+	pci_set_drvdata(pci_dev, imgu);
+
+	pci_set_master(pci_dev);
+
+	r = dma_coerce_mask_and_coherent(&pci_dev->dev, IMGU_DMA_MASK);
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to set DMA mask (%d)\n", r);
+		return -ENODEV;
+	}
+
+	r = imgu_pci_config_setup(pci_dev);
+	if (r)
+		return r;
+
+	mutex_init(&imgu->lock);
+	mutex_init(&imgu->streaming_lock);
+	atomic_set(&imgu->qbuf_barrier, 0);
+	init_waitqueue_head(&imgu->buf_drain_wq);
+
+	r = imgu_css_set_powerup(&pci_dev->dev, imgu->base, 200);
+	if (r) {
+		dev_err(&pci_dev->dev,
+			"failed to power up CSS (%d)\n", r);
+		goto out_mutex_destroy;
+	}
+
+	imgu->mmu = imgu_mmu_init(&pci_dev->dev, imgu->base);
+	if (IS_ERR(imgu->mmu)) {
+		r = PTR_ERR(imgu->mmu);
+		dev_err(&pci_dev->dev, "failed to initialize MMU (%d)\n", r);
+		goto out_css_powerdown;
+	}
+
+	r = imgu_dmamap_init(imgu);
+	if (r) {
+		dev_err(&pci_dev->dev,
+			"failed to initialize DMA mapping (%d)\n", r);
+		goto out_mmu_exit;
+	}
+
+	/* ISP programming */
+	r = imgu_css_init(&pci_dev->dev, &imgu->css, imgu->base, phys_len);
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to initialize CSS (%d)\n", r);
+		goto out_dmamap_exit;
+	}
+
+	/* v4l2 sub-device registration */
+	r = imgu_video_nodes_init(imgu);
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to create V4L2 devices (%d)\n",
+			r);
+		goto out_css_cleanup;
+	}
+
+	r = devm_request_threaded_irq(&pci_dev->dev, pci_dev->irq,
+				      imgu_isr, imgu_isr_threaded,
+				      IRQF_SHARED, IMGU_NAME, imgu);
+	if (r) {
+		dev_err(&pci_dev->dev, "failed to request IRQ (%d)\n", r);
+		goto out_video_exit;
+	}
+
+	pm_runtime_put_noidle(&pci_dev->dev);
+	pm_runtime_allow(&pci_dev->dev);
+
+	return 0;
+
+out_video_exit:
+	imgu_video_nodes_exit(imgu);
+out_css_cleanup:
+	imgu_css_cleanup(&imgu->css);
+out_dmamap_exit:
+	imgu_dmamap_exit(imgu);
+out_mmu_exit:
+	imgu_mmu_exit(imgu->mmu);
+out_css_powerdown:
+	imgu_css_set_powerdown(&pci_dev->dev, imgu->base);
+out_mutex_destroy:
+	mutex_destroy(&imgu->streaming_lock);
+	mutex_destroy(&imgu->lock);
+
+	return r;
+}
+
+static void imgu_pci_remove(struct pci_dev *pci_dev)
+{
+	struct imgu_device *imgu = pci_get_drvdata(pci_dev);
+
+	pm_runtime_forbid(&pci_dev->dev);
+	pm_runtime_get_noresume(&pci_dev->dev);
+
+	imgu_video_nodes_exit(imgu);
+	imgu_css_cleanup(&imgu->css);
+	imgu_css_set_powerdown(&pci_dev->dev, imgu->base);
+	imgu_dmamap_exit(imgu);
+	imgu_mmu_exit(imgu->mmu);
+	mutex_destroy(&imgu->streaming_lock);
+	mutex_destroy(&imgu->lock);
+}
+
+static int __maybe_unused imgu_suspend(struct device *dev)
+{
+	struct pci_dev *pci_dev = to_pci_dev(dev);
+	struct imgu_device *imgu = pci_get_drvdata(pci_dev);
+
+	dev_dbg(dev, "enter %s\n", __func__);
+	imgu->suspend_in_stream = imgu_css_is_streaming(&imgu->css);
+	if (!imgu->suspend_in_stream)
+		goto out;
+	/* Block new buffers to be queued to CSS. */
+	atomic_set(&imgu->qbuf_barrier, 1);
+	/*
+	 * Wait for currently running irq handler to be done so that
+	 * no new buffers will be queued to fw later.
+	 */
+	synchronize_irq(pci_dev->irq);
+	/* Wait until all buffers in CSS are done. */
+	if (!wait_event_timeout(imgu->buf_drain_wq,
+	    imgu_css_queue_empty(&imgu->css), msecs_to_jiffies(1000)))
+		dev_err(dev, "wait buffer drain timeout.\n");
+
+	imgu_css_stop_streaming(&imgu->css);
+	atomic_set(&imgu->qbuf_barrier, 0);
+	imgu_powerdown(imgu);
+	pm_runtime_force_suspend(dev);
+out:
+	dev_dbg(dev, "leave %s\n", __func__);
+	return 0;
+}
+
+static int __maybe_unused imgu_resume(struct device *dev)
+{
+	struct imgu_device *imgu = dev_get_drvdata(dev);
+	int r = 0;
+	unsigned int pipe;
+
+	dev_dbg(dev, "enter %s\n", __func__);
+
+	if (!imgu->suspend_in_stream)
+		goto out;
+
+	pm_runtime_force_resume(dev);
+
+	r = imgu_powerup(imgu);
+	if (r) {
+		dev_err(dev, "failed to power up imgu\n");
+		goto out;
+	}
+
+	/* Start CSS streaming */
+	r = imgu_css_start_streaming(&imgu->css);
+	if (r) {
+		dev_err(dev, "failed to resume css streaming (%d)", r);
+		goto out;
+	}
+
+	for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
+		r = imgu_queue_buffers(imgu, true, pipe);
+		if (r)
+			dev_err(dev, "failed to queue buffers to pipe %d (%d)",
+				pipe, r);
+	}
+
+out:
+	dev_dbg(dev, "leave %s\n", __func__);
+
+	return r;
+}
+
+/*
+ * PCI rpm framework checks the existence of driver rpm callbacks.
+ * Place a dummy callback here to avoid rpm going into error state.
+ */
+static __maybe_unused int imgu_rpm_dummy_cb(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops imgu_pm_ops = {
+	SET_RUNTIME_PM_OPS(&imgu_rpm_dummy_cb, &imgu_rpm_dummy_cb, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(&imgu_suspend, &imgu_resume)
+};
+
+static const struct pci_device_id imgu_pci_tbl[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, IMGU_PCI_ID) },
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(pci, imgu_pci_tbl);
+
+static struct pci_driver imgu_pci_driver = {
+	.name = IMGU_NAME,
+	.id_table = imgu_pci_tbl,
+	.probe = imgu_pci_probe,
+	.remove = imgu_pci_remove,
+	.driver = {
+		.pm = &imgu_pm_ops,
+	},
+};
+
+module_pci_driver(imgu_pci_driver);
+
+MODULE_AUTHOR("Tuukka Toivonen <tuukka.toivonen@intel.com>");
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Jian Xu Zheng <jian.xu.zheng@intel.com>");
+MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
+MODULE_AUTHOR("Yong Zhi <yong.zhi@intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel ipu3_imgu PCI driver");
diff --git a/drivers/staging/media/ipu3/ipu3.h b/drivers/staging/media/ipu3/ipu3.h
new file mode 100644
index 000000000..d2ad0a95c
--- /dev/null
+++ b/drivers/staging/media/ipu3/ipu3.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2018 Intel Corporation */
+
+#ifndef __IPU3_H
+#define __IPU3_H
+
+#include <linux/iova.h>
+#include <linux/pci.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "ipu3-css.h"
+
+#define IMGU_NAME			"ipu3-imgu"
+
+/*
+ * The semantics of the driver is that whenever there is a buffer available in
+ * master queue, the driver queues a buffer also to all other active nodes.
+ * If user space hasn't provided a buffer to all other video nodes first,
+ * the driver gets an internal dummy buffer and queues it.
+ */
+#define IMGU_QUEUE_MASTER		IPU3_CSS_QUEUE_IN
+#define IMGU_QUEUE_FIRST_INPUT		IPU3_CSS_QUEUE_OUT
+#define IMGU_MAX_QUEUE_DEPTH		(2 + 2)
+
+#define IMGU_NODE_IN			0 /* Input RAW image */
+#define IMGU_NODE_PARAMS		1 /* Input parameters */
+#define IMGU_NODE_OUT			2 /* Main output for still or video */
+#define IMGU_NODE_VF			3 /* Preview */
+#define IMGU_NODE_STAT_3A		4 /* 3A statistics */
+#define IMGU_NODE_NUM			5
+
+#define file_to_intel_imgu_node(__file) \
+	container_of(video_devdata(__file), struct imgu_video_device, vdev)
+
+#define IPU3_INPUT_MIN_WIDTH		0U
+#define IPU3_INPUT_MIN_HEIGHT		0U
+#define IPU3_INPUT_MAX_WIDTH		5120U
+#define IPU3_INPUT_MAX_HEIGHT		38404U
+#define IPU3_OUTPUT_MIN_WIDTH		2U
+#define IPU3_OUTPUT_MIN_HEIGHT		2U
+#define IPU3_OUTPUT_MAX_WIDTH		4480U
+#define IPU3_OUTPUT_MAX_HEIGHT		34004U
+
+struct imgu_vb2_buffer {
+	/* Public fields */
+	struct vb2_v4l2_buffer vbb;	/* Must be the first field */
+
+	/* Private fields */
+	struct list_head list;
+};
+
+struct imgu_buffer {
+	struct imgu_vb2_buffer vid_buf;	/* Must be the first field */
+	struct imgu_css_buffer css_buf;
+	struct imgu_css_map map;
+};
+
+struct imgu_node_mapping {
+	unsigned int css_queue;
+	const char *name;
+};
+
+struct imgu_video_device {
+	const char *name;
+	bool output;
+	bool enabled;
+	struct v4l2_format vdev_fmt;	/* Currently set format */
+
+	/* Private fields */
+	struct video_device vdev;
+	struct media_pad vdev_pad;
+	struct v4l2_mbus_framefmt pad_fmt;
+	struct vb2_queue vbq;
+	struct list_head buffers;
+	/* Protect vb2_queue and vdev structs*/
+	struct mutex lock;
+	atomic_t sequence;
+	unsigned int id;
+	unsigned int pipe;
+};
+
+struct imgu_v4l2_subdev {
+	unsigned int pipe;
+	struct v4l2_subdev subdev;
+	struct media_pad subdev_pads[IMGU_NODE_NUM];
+	struct {
+		struct v4l2_rect eff; /* effective resolution */
+		struct v4l2_rect bds; /* bayer-domain scaled resolution*/
+		struct v4l2_rect gdc; /* gdc output resolution */
+	} rect;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *ctrl;
+	atomic_t running_mode;
+	bool active;
+};
+
+struct imgu_media_pipe {
+	unsigned int pipe;
+
+	/* Internally enabled queues */
+	struct {
+		struct imgu_css_map dmap;
+		struct imgu_css_buffer dummybufs[IMGU_MAX_QUEUE_DEPTH];
+	} queues[IPU3_CSS_QUEUES];
+	struct imgu_video_device nodes[IMGU_NODE_NUM];
+	bool queue_enabled[IMGU_NODE_NUM];
+	struct media_pipeline pipeline;
+	struct imgu_v4l2_subdev imgu_sd;
+};
+
+/*
+ * imgu_device -- ImgU (Imaging Unit) driver
+ */
+struct imgu_device {
+	struct pci_dev *pci_dev;
+	void __iomem *base;
+
+	/* Public fields, fill before registering */
+	unsigned int buf_struct_size;
+	bool streaming;		/* Public read only */
+
+	struct imgu_media_pipe imgu_pipe[IMGU_MAX_PIPE_NUM];
+
+	/* Private fields */
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct v4l2_file_operations v4l2_file_ops;
+
+	/* MMU driver for css */
+	struct imgu_mmu_info *mmu;
+	struct iova_domain iova_domain;
+
+	/* css - Camera Sub-System */
+	struct imgu_css css;
+
+	/*
+	 * Coarse-grained lock to protect
+	 * vid_buf.list and css->queue
+	 */
+	struct mutex lock;
+
+	/* Lock to protect writes to streaming flag in this struct */
+	struct mutex streaming_lock;
+
+	/* Forbid streaming and buffer queuing during system suspend. */
+	atomic_t qbuf_barrier;
+	/* Indicate if system suspend take place while imgu is streaming. */
+	bool suspend_in_stream;
+	/* Used to wait for FW buffer queue drain. */
+	wait_queue_head_t buf_drain_wq;
+};
+
+unsigned int imgu_node_to_queue(unsigned int node);
+unsigned int imgu_map_node(struct imgu_device *imgu, unsigned int css_queue);
+int imgu_queue_buffers(struct imgu_device *imgu, bool initial,
+		       unsigned int pipe);
+
+int imgu_v4l2_register(struct imgu_device *dev);
+int imgu_v4l2_unregister(struct imgu_device *dev);
+void imgu_v4l2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state);
+
+int imgu_s_stream(struct imgu_device *imgu, int enable);
+
+static inline u32 imgu_bytesperline(const unsigned int width,
+				    enum imgu_abi_frame_format frame_format)
+{
+	if (frame_format == IMGU_ABI_FRAME_FORMAT_NV12)
+		return ALIGN(width, IPU3_UAPI_ISP_VEC_ELEMS);
+	/*
+	 * 64 bytes for every 50 pixels, the line length
+	 * in bytes is multiple of 64 (line end alignment).
+	 */
+	return DIV_ROUND_UP(width, 50) * 64;
+}
+
+#endif
diff --git a/drivers/staging/media/max96712/Kconfig b/drivers/staging/media/max96712/Kconfig
new file mode 100644
index 000000000..117fadf81
--- /dev/null
+++ b/drivers/staging/media/max96712/Kconfig
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_MAX96712
+	tristate "Maxim MAX96712 Quad GMSL2 Deserializer support"
+	depends on I2C
+	depends on OF_GPIO
+	depends on VIDEO_DEV
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	help
+	  This driver supports the Maxim MAX96712 Quad GMSL2 Deserializer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max96712.
diff --git a/drivers/staging/media/max96712/Makefile b/drivers/staging/media/max96712/Makefile
new file mode 100644
index 000000000..70c1974ce
--- /dev/null
+++ b/drivers/staging/media/max96712/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_VIDEO_MAX96712) += max96712.o
diff --git a/drivers/staging/media/max96712/max96712.c b/drivers/staging/media/max96712/max96712.c
new file mode 100644
index 000000000..99b333b68
--- /dev/null
+++ b/drivers/staging/media/max96712/max96712.c
@@ -0,0 +1,438 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Maxim MAX9286 Quad GMSL2 Deserializer Driver
+ *
+ * Copyright (C) 2021 Renesas Electronics Corporation
+ * Copyright (C) 2021 Niklas Söderlund
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/regmap.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define MAX96712_ID 0x20
+
+#define MAX96712_DPLL_FREQ 1000
+
+enum max96712_pattern {
+	MAX96712_PATTERN_CHECKERBOARD = 0,
+	MAX96712_PATTERN_GRADIENT,
+};
+
+struct max96712_priv {
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct gpio_desc *gpiod_pwdn;
+
+	struct v4l2_mbus_config_mipi_csi2 mipi;
+
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct media_pad pads[1];
+
+	enum max96712_pattern pattern;
+};
+
+static int max96712_read(struct max96712_priv *priv, int reg)
+{
+	int ret, val;
+
+	ret = regmap_read(priv->regmap, reg, &val);
+	if (ret) {
+		dev_err(&priv->client->dev, "read 0x%04x failed\n", reg);
+		return ret;
+	}
+
+	return val;
+}
+
+static int max96712_write(struct max96712_priv *priv, unsigned int reg, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret)
+		dev_err(&priv->client->dev, "write 0x%04x failed\n", reg);
+
+	return ret;
+}
+
+static int max96712_update_bits(struct max96712_priv *priv, unsigned int reg,
+				u8 mask, u8 val)
+{
+	int ret;
+
+	ret = regmap_update_bits(priv->regmap, reg, mask, val);
+	if (ret)
+		dev_err(&priv->client->dev, "update 0x%04x failed\n", reg);
+
+	return ret;
+}
+
+static int max96712_write_bulk(struct max96712_priv *priv, unsigned int reg,
+			       const void *val, size_t val_count)
+{
+	int ret;
+
+	ret = regmap_bulk_write(priv->regmap, reg, val, val_count);
+	if (ret)
+		dev_err(&priv->client->dev, "bulk write 0x%04x failed\n", reg);
+
+	return ret;
+}
+
+static int max96712_write_bulk_value(struct max96712_priv *priv,
+				     unsigned int reg, unsigned int val,
+				     size_t val_count)
+{
+	unsigned int i;
+	u8 values[4];
+
+	for (i = 1; i <= val_count; i++)
+		values[i - 1] = (val >> ((val_count - i) * 8)) & 0xff;
+
+	return max96712_write_bulk(priv, reg, &values, val_count);
+}
+
+static void max96712_reset(struct max96712_priv *priv)
+{
+	max96712_update_bits(priv, 0x13, 0x40, 0x40);
+	msleep(20);
+}
+
+static void max96712_mipi_enable(struct max96712_priv *priv, bool enable)
+{
+	if (enable) {
+		max96712_update_bits(priv, 0x40b, 0x02, 0x02);
+		max96712_update_bits(priv, 0x8a0, 0x80, 0x80);
+	} else {
+		max96712_update_bits(priv, 0x8a0, 0x80, 0x00);
+		max96712_update_bits(priv, 0x40b, 0x02, 0x00);
+	}
+}
+
+static void max96712_mipi_configure(struct max96712_priv *priv)
+{
+	unsigned int i;
+	u8 phy5 = 0;
+
+	max96712_mipi_enable(priv, false);
+
+	/* Select 2x4 mode. */
+	max96712_write(priv, 0x8a0, 0x04);
+
+	/* Configure a 4-lane DPHY using PHY0 and PHY1. */
+	/* TODO: Add support for 2-lane and 1-lane configurations. */
+	/* TODO: Add support CPHY mode. */
+	max96712_write(priv, 0x94a, 0xc0);
+
+	/* Configure lane mapping for PHY0 and PHY1. */
+	/* TODO: Add support for lane swapping. */
+	max96712_write(priv, 0x8a3, 0xe4);
+
+	/* Configure lane polarity for PHY0 and PHY1. */
+	for (i = 0; i < priv->mipi.num_data_lanes + 1; i++)
+		if (priv->mipi.lane_polarities[i])
+			phy5 |= BIT(i == 0 ? 5 : i < 3 ? i - 1 : i);
+	max96712_write(priv, 0x8a5, phy5);
+
+	/* Set link frequency for PHY0 and PHY1. */
+	max96712_update_bits(priv, 0x415, 0x3f,
+			     ((MAX96712_DPLL_FREQ / 100) & 0x1f) | BIT(5));
+	max96712_update_bits(priv, 0x418, 0x3f,
+			     ((MAX96712_DPLL_FREQ / 100) & 0x1f) | BIT(5));
+
+	/* Enable PHY0 and PHY1 */
+	max96712_update_bits(priv, 0x8a2, 0xf0, 0x30);
+}
+
+static void max96712_pattern_enable(struct max96712_priv *priv, bool enable)
+{
+	const u32 h_active = 1920;
+	const u32 h_fp = 88;
+	const u32 h_sw = 44;
+	const u32 h_bp = 148;
+	const u32 h_tot = h_active + h_fp + h_sw + h_bp;
+
+	const u32 v_active = 1080;
+	const u32 v_fp = 4;
+	const u32 v_sw = 5;
+	const u32 v_bp = 36;
+	const u32 v_tot = v_active + v_fp + v_sw + v_bp;
+
+	if (!enable) {
+		max96712_write(priv, 0x1051, 0x00);
+		return;
+	}
+
+	/* PCLK 75MHz. */
+	max96712_write(priv, 0x0009, 0x01);
+
+	/* Configure Video Timing Generator for 1920x1080 @ 30 fps. */
+	max96712_write_bulk_value(priv, 0x1052, 0, 3);
+	max96712_write_bulk_value(priv, 0x1055, v_sw * h_tot, 3);
+	max96712_write_bulk_value(priv, 0x1058,
+				  (v_active + v_fp + + v_bp) * h_tot, 3);
+	max96712_write_bulk_value(priv, 0x105b, 0, 3);
+	max96712_write_bulk_value(priv, 0x105e, h_sw, 2);
+	max96712_write_bulk_value(priv, 0x1060, h_active + h_fp + h_bp, 2);
+	max96712_write_bulk_value(priv, 0x1062, v_tot, 2);
+	max96712_write_bulk_value(priv, 0x1064,
+				  h_tot * (v_sw + v_bp) + (h_sw + h_bp), 3);
+	max96712_write_bulk_value(priv, 0x1067, h_active, 2);
+	max96712_write_bulk_value(priv, 0x1069, h_fp + h_sw + h_bp, 2);
+	max96712_write_bulk_value(priv, 0x106b, v_active, 2);
+
+	/* Generate VS, HS and DE in free-running mode. */
+	max96712_write(priv, 0x1050, 0xfb);
+
+	/* Configure Video Pattern Generator. */
+	if (priv->pattern == MAX96712_PATTERN_CHECKERBOARD) {
+		/* Set checkerboard pattern size. */
+		max96712_write(priv, 0x1074, 0x3c);
+		max96712_write(priv, 0x1075, 0x3c);
+		max96712_write(priv, 0x1076, 0x3c);
+
+		/* Set checkerboard pattern colors. */
+		max96712_write_bulk_value(priv, 0x106e, 0xfecc00, 3);
+		max96712_write_bulk_value(priv, 0x1071, 0x006aa7, 3);
+
+		/* Generate checkerboard pattern. */
+		max96712_write(priv, 0x1051, 0x10);
+	} else {
+		/* Set gradient increment. */
+		max96712_write(priv, 0x106d, 0x10);
+
+		/* Generate gradient pattern. */
+		max96712_write(priv, 0x1051, 0x20);
+	}
+}
+
+static int max96712_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct max96712_priv *priv = v4l2_get_subdevdata(sd);
+
+	if (enable) {
+		max96712_pattern_enable(priv, true);
+		max96712_mipi_enable(priv, true);
+	} else {
+		max96712_mipi_enable(priv, false);
+		max96712_pattern_enable(priv, false);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops max96712_video_ops = {
+	.s_stream = max96712_s_stream,
+};
+
+static int max96712_get_pad_format(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *format)
+{
+	format->format.width = 1920;
+	format->format.height = 1080;
+	format->format.code = MEDIA_BUS_FMT_RGB888_1X24;
+	format->format.field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops max96712_pad_ops = {
+	.get_fmt = max96712_get_pad_format,
+	.set_fmt = max96712_get_pad_format,
+};
+
+static const struct v4l2_subdev_ops max96712_subdev_ops = {
+	.video = &max96712_video_ops,
+	.pad = &max96712_pad_ops,
+};
+
+static const char * const max96712_test_pattern[] = {
+	"Checkerboard",
+	"Gradient",
+};
+
+static int max96712_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max96712_priv *priv =
+		container_of(ctrl->handler, struct max96712_priv, ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		priv->pattern = ctrl->val ?
+			MAX96712_PATTERN_GRADIENT :
+			MAX96712_PATTERN_CHECKERBOARD;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops max96712_ctrl_ops = {
+	.s_ctrl = max96712_s_ctrl,
+};
+
+static int max96712_v4l2_register(struct max96712_priv *priv)
+{
+	long pixel_rate;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &max96712_subdev_ops);
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+
+	v4l2_ctrl_handler_init(&priv->ctrl_handler, 2);
+
+	/*
+	 * TODO: Once V4L2_CID_LINK_FREQ is changed from a menu control to an
+	 * INT64 control it should be used here instead of V4L2_CID_PIXEL_RATE.
+	 */
+	pixel_rate = MAX96712_DPLL_FREQ / priv->mipi.num_data_lanes * 1000000;
+	v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_PIXEL_RATE,
+			  pixel_rate, pixel_rate, 1, pixel_rate);
+
+	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler, &max96712_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(max96712_test_pattern) - 1,
+				     0, 0, max96712_test_pattern);
+
+	priv->sd.ctrl_handler = &priv->ctrl_handler;
+	ret = priv->ctrl_handler.error;
+	if (ret)
+		goto error;
+
+	priv->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&priv->sd.entity, 1, priv->pads);
+	if (ret)
+		goto error;
+
+	v4l2_set_subdevdata(&priv->sd, priv);
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret < 0) {
+		dev_err(&priv->client->dev, "Unable to register subdevice\n");
+		goto error;
+	}
+
+	return 0;
+error:
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+
+	return ret;
+}
+
+static int max96712_parse_dt(struct max96712_priv *priv)
+{
+	struct fwnode_handle *ep;
+	struct v4l2_fwnode_endpoint v4l2_ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret;
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(&priv->client->dev), 4,
+					     0, 0);
+	if (!ep) {
+		dev_err(&priv->client->dev, "Not connected to subdevice\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &v4l2_ep);
+	fwnode_handle_put(ep);
+	if (ret) {
+		dev_err(&priv->client->dev, "Could not parse v4l2 endpoint\n");
+		return -EINVAL;
+	}
+
+	if (v4l2_ep.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(&priv->client->dev, "Only 4 data lanes supported\n");
+		return -EINVAL;
+	}
+
+	priv->mipi = v4l2_ep.bus.mipi_csi2;
+
+	return 0;
+}
+
+static const struct regmap_config max96712_i2c_regmap = {
+	.reg_bits = 16,
+	.val_bits = 8,
+	.max_register = 0x1f00,
+};
+
+static int max96712_probe(struct i2c_client *client)
+{
+	struct max96712_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+	i2c_set_clientdata(client, priv);
+
+	priv->regmap = devm_regmap_init_i2c(client, &max96712_i2c_regmap);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	priv->gpiod_pwdn = devm_gpiod_get_optional(&client->dev, "enable",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->gpiod_pwdn))
+		return PTR_ERR(priv->gpiod_pwdn);
+
+	gpiod_set_consumer_name(priv->gpiod_pwdn, "max96712-pwdn");
+	gpiod_set_value_cansleep(priv->gpiod_pwdn, 1);
+
+	if (priv->gpiod_pwdn)
+		usleep_range(4000, 5000);
+
+	if (max96712_read(priv, 0x4a) != MAX96712_ID)
+		return -ENODEV;
+
+	max96712_reset(priv);
+
+	ret = max96712_parse_dt(priv);
+	if (ret)
+		return ret;
+
+	max96712_mipi_configure(priv);
+
+	return max96712_v4l2_register(priv);
+}
+
+static void max96712_remove(struct i2c_client *client)
+{
+	struct max96712_priv *priv = i2c_get_clientdata(client);
+
+	v4l2_async_unregister_subdev(&priv->sd);
+
+	gpiod_set_value_cansleep(priv->gpiod_pwdn, 0);
+}
+
+static const struct of_device_id max96712_of_table[] = {
+	{ .compatible = "maxim,max96712" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, max96712_of_table);
+
+static struct i2c_driver max96712_i2c_driver = {
+	.driver	= {
+		.name = "max96712",
+		.of_match_table	= of_match_ptr(max96712_of_table),
+	},
+	.probe_new = max96712_probe,
+	.remove = max96712_remove,
+};
+
+module_i2c_driver(max96712_i2c_driver);
+
+MODULE_DESCRIPTION("Maxim MAX96712 Quad GMSL2 Deserializer Driver");
+MODULE_AUTHOR("Niklas Söderlund <niklas.soderlund@ragnatech.se>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/meson/vdec/Kconfig b/drivers/staging/media/meson/vdec/Kconfig
new file mode 100644
index 000000000..19ffea987
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/Kconfig
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config VIDEO_MESON_VDEC
+	tristate "Amlogic video decoder driver"
+	depends on VIDEO_DEV && HAS_DMA
+	depends on ARCH_MESON || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	select MESON_CANVAS
+	help
+	Support for the video decoder found in gxbb/gxl/gxm chips.
diff --git a/drivers/staging/media/meson/vdec/Makefile b/drivers/staging/media/meson/vdec/Makefile
new file mode 100644
index 000000000..6e726af84
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+# Makefile for Amlogic meson video decoder driver
+
+meson-vdec-objs = esparser.o vdec.o vdec_helpers.o vdec_platform.o
+meson-vdec-objs += vdec_1.o vdec_hevc.o
+meson-vdec-objs += codec_mpeg12.o codec_h264.o codec_hevc_common.o codec_vp9.o
+
+obj-$(CONFIG_VIDEO_MESON_VDEC) += meson-vdec.o
diff --git a/drivers/staging/media/meson/vdec/TODO b/drivers/staging/media/meson/vdec/TODO
new file mode 100644
index 000000000..70ae990cf
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/TODO
@@ -0,0 +1,8 @@
+This driver is in staging until the V4L2 documentation about stateful video
+decoders is finalized, as well as the corresponding compliance tests.
+
+It is at the moment not guaranteed to work properly with a userspace
+stack that follows the latest version of the specification, especially
+with compression standards like MPEG1/2 where the driver does not support
+dynamic resolution switching, including the first one used to determine coded
+resolution.
diff --git a/drivers/staging/media/meson/vdec/codec_h264.c b/drivers/staging/media/meson/vdec/codec_h264.c
new file mode 100644
index 000000000..c61128fc4
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_h264.c
@@ -0,0 +1,485 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2019 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "vdec_helpers.h"
+#include "dos_regs.h"
+#include "codec_h264.h"
+
+#define SIZE_EXT_FW	(20 * SZ_1K)
+#define SIZE_WORKSPACE	0x1ee000
+#define SIZE_SEI	(8 * SZ_1K)
+
+/*
+ * Offset added by the firmware which must be substracted
+ * from the workspace phyaddr
+ */
+#define WORKSPACE_BUF_OFFSET	0x1000000
+
+/* ISR status */
+#define CMD_MASK		GENMASK(7, 0)
+#define CMD_SRC_CHANGE		1
+#define CMD_FRAMES_READY	2
+#define CMD_FATAL_ERROR		6
+#define CMD_BAD_WIDTH		7
+#define CMD_BAD_HEIGHT		8
+
+#define SEI_DATA_READY	BIT(15)
+
+/* Picture type */
+#define PIC_TOP_BOT	5
+#define PIC_BOT_TOP	6
+
+/* Size of Motion Vector per macroblock */
+#define MB_MV_SIZE	96
+
+/* Frame status data */
+#define PIC_STRUCT_BIT	5
+#define PIC_STRUCT_MASK	GENMASK(2, 0)
+#define BUF_IDX_MASK	GENMASK(4, 0)
+#define ERROR_FLAG	BIT(9)
+#define OFFSET_BIT	16
+#define OFFSET_MASK	GENMASK(15, 0)
+
+/* Bitstream parsed data */
+#define MB_TOTAL_BIT	8
+#define MB_TOTAL_MASK	GENMASK(15, 0)
+#define MB_WIDTH_MASK	GENMASK(7, 0)
+#define MAX_REF_BIT	24
+#define MAX_REF_MASK	GENMASK(6, 0)
+#define AR_IDC_BIT	16
+#define AR_IDC_MASK	GENMASK(7, 0)
+#define AR_PRESENT_FLAG	BIT(0)
+#define AR_EXTEND	0xff
+
+/*
+ * Buffer to send to the ESPARSER to signal End Of Stream for H.264.
+ * This is a 16x16 encoded picture that will trigger drain firmware-side.
+ * There is no known alternative.
+ */
+static const u8 eos_sequence[SZ_4K] = {
+	0x00, 0x00, 0x00, 0x01, 0x06, 0x05, 0xff, 0xe4, 0xdc, 0x45, 0xe9, 0xbd,
+	0xe6, 0xd9, 0x48, 0xb7,	0x96, 0x2c, 0xd8, 0x20, 0xd9, 0x23, 0xee, 0xef,
+	0x78, 0x32, 0x36, 0x34, 0x20, 0x2d, 0x20, 0x63,	0x6f, 0x72, 0x65, 0x20,
+	0x36, 0x37, 0x20, 0x72, 0x31, 0x31, 0x33, 0x30, 0x20, 0x38, 0x34, 0x37,
+	0x35, 0x39, 0x37, 0x37, 0x20, 0x2d, 0x20, 0x48, 0x2e, 0x32, 0x36, 0x34,
+	0x2f, 0x4d, 0x50, 0x45,	0x47, 0x2d, 0x34, 0x20, 0x41, 0x56, 0x43, 0x20,
+	0x63, 0x6f, 0x64, 0x65, 0x63, 0x20, 0x2d, 0x20,	0x43, 0x6f, 0x70, 0x79,
+	0x6c, 0x65, 0x66, 0x74, 0x20, 0x32, 0x30, 0x30, 0x33, 0x2d, 0x32, 0x30,
+	0x30, 0x39, 0x20, 0x2d, 0x20, 0x68, 0x74, 0x74, 0x70, 0x3a, 0x2f, 0x2f,
+	0x77, 0x77, 0x77, 0x2e,	0x76, 0x69, 0x64, 0x65, 0x6f, 0x6c, 0x61, 0x6e,
+	0x2e, 0x6f, 0x72, 0x67, 0x2f, 0x78, 0x32, 0x36,	0x34, 0x2e, 0x68, 0x74,
+	0x6d, 0x6c, 0x20, 0x2d, 0x20, 0x6f, 0x70, 0x74, 0x69, 0x6f, 0x6e, 0x73,
+	0x3a, 0x20, 0x63, 0x61, 0x62, 0x61, 0x63, 0x3d, 0x31, 0x20, 0x72, 0x65,
+	0x66, 0x3d, 0x31, 0x20,	0x64, 0x65, 0x62, 0x6c, 0x6f, 0x63, 0x6b, 0x3d,
+	0x31, 0x3a, 0x30, 0x3a, 0x30, 0x20, 0x61, 0x6e,	0x61, 0x6c, 0x79, 0x73,
+	0x65, 0x3d, 0x30, 0x78, 0x31, 0x3a, 0x30, 0x78, 0x31, 0x31, 0x31, 0x20,
+	0x6d, 0x65, 0x3d, 0x68, 0x65, 0x78, 0x20, 0x73, 0x75, 0x62, 0x6d, 0x65,
+	0x3d, 0x36, 0x20, 0x70,	0x73, 0x79, 0x5f, 0x72, 0x64, 0x3d, 0x31, 0x2e,
+	0x30, 0x3a, 0x30, 0x2e, 0x30, 0x20, 0x6d, 0x69,	0x78, 0x65, 0x64, 0x5f,
+	0x72, 0x65, 0x66, 0x3d, 0x30, 0x20, 0x6d, 0x65, 0x5f, 0x72, 0x61, 0x6e,
+	0x67, 0x65, 0x3d, 0x31, 0x36, 0x20, 0x63, 0x68, 0x72, 0x6f, 0x6d, 0x61,
+	0x5f, 0x6d, 0x65, 0x3d,	0x31, 0x20, 0x74, 0x72, 0x65, 0x6c, 0x6c, 0x69,
+	0x73, 0x3d, 0x30, 0x20, 0x38, 0x78, 0x38, 0x64,	0x63, 0x74, 0x3d, 0x30,
+	0x20, 0x63, 0x71, 0x6d, 0x3d, 0x30, 0x20, 0x64, 0x65, 0x61, 0x64, 0x7a,
+	0x6f, 0x6e, 0x65, 0x3d, 0x32, 0x31, 0x2c, 0x31, 0x31, 0x20, 0x63, 0x68,
+	0x72, 0x6f, 0x6d, 0x61,	0x5f, 0x71, 0x70, 0x5f, 0x6f, 0x66, 0x66, 0x73,
+	0x65, 0x74, 0x3d, 0x2d, 0x32, 0x20, 0x74, 0x68,	0x72, 0x65, 0x61, 0x64,
+	0x73, 0x3d, 0x31, 0x20, 0x6e, 0x72, 0x3d, 0x30, 0x20, 0x64, 0x65, 0x63,
+	0x69, 0x6d, 0x61, 0x74, 0x65, 0x3d, 0x31, 0x20, 0x6d, 0x62, 0x61, 0x66,
+	0x66, 0x3d, 0x30, 0x20,	0x62, 0x66, 0x72, 0x61, 0x6d, 0x65, 0x73, 0x3d,
+	0x30, 0x20, 0x6b, 0x65, 0x79, 0x69, 0x6e, 0x74,	0x3d, 0x32, 0x35, 0x30,
+	0x20, 0x6b, 0x65, 0x79, 0x69, 0x6e, 0x74, 0x5f, 0x6d, 0x69, 0x6e, 0x3d,
+	0x32, 0x35, 0x20, 0x73, 0x63, 0x65, 0x6e, 0x65, 0x63, 0x75, 0x74, 0x3d,
+	0x34, 0x30, 0x20, 0x72,	0x63, 0x3d, 0x61, 0x62, 0x72, 0x20, 0x62, 0x69,
+	0x74, 0x72, 0x61, 0x74, 0x65, 0x3d, 0x31, 0x30,	0x20, 0x72, 0x61, 0x74,
+	0x65, 0x74, 0x6f, 0x6c, 0x3d, 0x31, 0x2e, 0x30, 0x20, 0x71, 0x63, 0x6f,
+	0x6d, 0x70, 0x3d, 0x30, 0x2e, 0x36, 0x30, 0x20, 0x71, 0x70, 0x6d, 0x69,
+	0x6e, 0x3d, 0x31, 0x30,	0x20, 0x71, 0x70, 0x6d, 0x61, 0x78, 0x3d, 0x35,
+	0x31, 0x20, 0x71, 0x70, 0x73, 0x74, 0x65, 0x70,	0x3d, 0x34, 0x20, 0x69,
+	0x70, 0x5f, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x3d, 0x31, 0x2e, 0x34, 0x30,
+	0x20, 0x61, 0x71, 0x3d, 0x31, 0x3a, 0x31, 0x2e, 0x30, 0x30, 0x00, 0x80,
+	0x00, 0x00, 0x00, 0x01,	0x67, 0x4d, 0x40, 0x0a, 0x9a, 0x74, 0xf4, 0x20,
+	0x00, 0x00, 0x03, 0x00, 0x20, 0x00, 0x00, 0x06,	0x51, 0xe2, 0x44, 0xd4,
+	0x00, 0x00, 0x00, 0x01, 0x68, 0xee, 0x32, 0xc8, 0x00, 0x00, 0x00, 0x01,
+	0x65, 0x88, 0x80, 0x20, 0x00, 0x08, 0x7f, 0xea, 0x6a, 0xe2, 0x99, 0xb6,
+	0x57, 0xae, 0x49, 0x30,	0xf5, 0xfe, 0x5e, 0x46, 0x0b, 0x72, 0x44, 0xc4,
+	0xe1, 0xfc, 0x62, 0xda, 0xf1, 0xfb, 0xa2, 0xdb,	0xd6, 0xbe, 0x5c, 0xd7,
+	0x24, 0xa3, 0xf5, 0xb9, 0x2f, 0x57, 0x16, 0x49, 0x75, 0x47, 0x77, 0x09,
+	0x5c, 0xa1, 0xb4, 0xc3, 0x4f, 0x60, 0x2b, 0xb0, 0x0c, 0xc8, 0xd6, 0x66,
+	0xba, 0x9b, 0x82, 0x29,	0x33, 0x92, 0x26, 0x99, 0x31, 0x1c, 0x7f, 0x9b,
+	0x00, 0x00, 0x01, 0x0ff,
+};
+
+static const u8 *codec_h264_eos_sequence(u32 *len)
+{
+	*len = ARRAY_SIZE(eos_sequence);
+	return eos_sequence;
+}
+
+struct codec_h264 {
+	/* H.264 decoder requires an extended firmware */
+	void      *ext_fw_vaddr;
+	dma_addr_t ext_fw_paddr;
+
+	/* Buffer for the H.264 Workspace */
+	void      *workspace_vaddr;
+	dma_addr_t workspace_paddr;
+
+	/* Buffer for the H.264 references MV */
+	void      *ref_vaddr;
+	dma_addr_t ref_paddr;
+	u32	   ref_size;
+
+	/* Buffer for parsed SEI data */
+	void      *sei_vaddr;
+	dma_addr_t sei_paddr;
+
+	u32 mb_width;
+	u32 mb_height;
+	u32 max_refs;
+};
+
+static int codec_h264_can_recycle(struct amvdec_core *core)
+{
+	return !amvdec_read_dos(core, AV_SCRATCH_7) ||
+	       !amvdec_read_dos(core, AV_SCRATCH_8);
+}
+
+static void codec_h264_recycle(struct amvdec_core *core, u32 buf_idx)
+{
+	/*
+	 * Tell the firmware it can recycle this buffer.
+	 * AV_SCRATCH_8 serves the same purpose.
+	 */
+	if (!amvdec_read_dos(core, AV_SCRATCH_7))
+		amvdec_write_dos(core, AV_SCRATCH_7, buf_idx + 1);
+	else
+		amvdec_write_dos(core, AV_SCRATCH_8, buf_idx + 1);
+}
+
+static int codec_h264_start(struct amvdec_session *sess)
+{
+	u32 workspace_offset;
+	struct amvdec_core *core = sess->core;
+	struct codec_h264 *h264 = sess->priv;
+
+	/* Allocate some memory for the H.264 decoder's state */
+	h264->workspace_vaddr =
+		dma_alloc_coherent(core->dev, SIZE_WORKSPACE,
+				   &h264->workspace_paddr, GFP_KERNEL);
+	if (!h264->workspace_vaddr)
+		return -ENOMEM;
+
+	/* Allocate some memory for the H.264 SEI dump */
+	h264->sei_vaddr = dma_alloc_coherent(core->dev, SIZE_SEI,
+					     &h264->sei_paddr, GFP_KERNEL);
+	if (!h264->sei_vaddr)
+		return -ENOMEM;
+
+	amvdec_write_dos_bits(core, POWER_CTL_VLD, BIT(9) | BIT(6));
+
+	workspace_offset = h264->workspace_paddr - WORKSPACE_BUF_OFFSET;
+	amvdec_write_dos(core, AV_SCRATCH_1, workspace_offset);
+	amvdec_write_dos(core, AV_SCRATCH_G, h264->ext_fw_paddr);
+	amvdec_write_dos(core, AV_SCRATCH_I, h264->sei_paddr -
+					     workspace_offset);
+
+	/* Enable "error correction" */
+	amvdec_write_dos(core, AV_SCRATCH_F,
+			 (amvdec_read_dos(core, AV_SCRATCH_F) & 0xffffffc3) |
+			 BIT(4) | BIT(7));
+
+	amvdec_write_dos(core, MDEC_PIC_DC_THRESH, 0x404038aa);
+
+	return 0;
+}
+
+static int codec_h264_stop(struct amvdec_session *sess)
+{
+	struct codec_h264 *h264 = sess->priv;
+	struct amvdec_core *core = sess->core;
+
+	if (h264->ext_fw_vaddr)
+		dma_free_coherent(core->dev, SIZE_EXT_FW,
+				  h264->ext_fw_vaddr, h264->ext_fw_paddr);
+
+	if (h264->workspace_vaddr)
+		dma_free_coherent(core->dev, SIZE_WORKSPACE,
+				  h264->workspace_vaddr, h264->workspace_paddr);
+
+	if (h264->ref_vaddr)
+		dma_free_coherent(core->dev, h264->ref_size,
+				  h264->ref_vaddr, h264->ref_paddr);
+
+	if (h264->sei_vaddr)
+		dma_free_coherent(core->dev, SIZE_SEI,
+				  h264->sei_vaddr, h264->sei_paddr);
+
+	return 0;
+}
+
+static int codec_h264_load_extended_firmware(struct amvdec_session *sess,
+					     const u8 *data, u32 len)
+{
+	struct codec_h264 *h264;
+	struct amvdec_core *core = sess->core;
+
+	if (len < SIZE_EXT_FW)
+		return -EINVAL;
+
+	h264 = kzalloc(sizeof(*h264), GFP_KERNEL);
+	if (!h264)
+		return -ENOMEM;
+
+	h264->ext_fw_vaddr = dma_alloc_coherent(core->dev, SIZE_EXT_FW,
+						&h264->ext_fw_paddr,
+						GFP_KERNEL);
+	if (!h264->ext_fw_vaddr) {
+		kfree(h264);
+		return -ENOMEM;
+	}
+
+	memcpy(h264->ext_fw_vaddr, data, SIZE_EXT_FW);
+	sess->priv = h264;
+
+	return 0;
+}
+
+static const struct v4l2_fract par_table[] = {
+	{ 1, 1 },   { 1, 1 },    { 12, 11 }, { 10, 11 },
+	{ 16, 11 }, { 40, 33 },  { 24, 11 }, { 20, 11 },
+	{ 32, 11 }, { 80, 33 },  { 18, 11 }, { 15, 11 },
+	{ 64, 33 }, { 160, 99 }, { 4, 3 },   { 3, 2 },
+	{ 2, 1 }
+};
+
+static void codec_h264_set_par(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	u32 seq_info = amvdec_read_dos(core, AV_SCRATCH_2);
+	u32 ar_idc = (seq_info >> AR_IDC_BIT) & AR_IDC_MASK;
+
+	if (!(seq_info & AR_PRESENT_FLAG))
+		return;
+
+	if (ar_idc == AR_EXTEND) {
+		u32 ar_info = amvdec_read_dos(core, AV_SCRATCH_3);
+
+		sess->pixelaspect.numerator = ar_info & 0xffff;
+		sess->pixelaspect.denominator = (ar_info >> 16) & 0xffff;
+		return;
+	}
+
+	if (ar_idc >= ARRAY_SIZE(par_table))
+		return;
+
+	sess->pixelaspect = par_table[ar_idc];
+}
+
+static void codec_h264_resume(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_h264 *h264 = sess->priv;
+	u32 mb_width, mb_height, mb_total;
+
+	amvdec_set_canvases(sess,
+			    (u32[]){ ANC0_CANVAS_ADDR, 0 },
+			    (u32[]){ 24, 0 });
+
+	dev_dbg(core->dev, "max_refs = %u; actual_dpb_size = %u\n",
+		h264->max_refs, sess->num_dst_bufs);
+
+	/* Align to a multiple of 4 macroblocks */
+	mb_width = ALIGN(h264->mb_width, 4);
+	mb_height = ALIGN(h264->mb_height, 4);
+	mb_total = mb_width * mb_height;
+
+	h264->ref_size = mb_total * MB_MV_SIZE * h264->max_refs;
+	h264->ref_vaddr = dma_alloc_coherent(core->dev, h264->ref_size,
+					     &h264->ref_paddr, GFP_KERNEL);
+	if (!h264->ref_vaddr) {
+		amvdec_abort(sess);
+		return;
+	}
+
+	/* Address to store the references' MVs */
+	amvdec_write_dos(core, AV_SCRATCH_1, h264->ref_paddr);
+	/* End of ref MV */
+	amvdec_write_dos(core, AV_SCRATCH_4, h264->ref_paddr + h264->ref_size);
+
+	amvdec_write_dos(core, AV_SCRATCH_0, (h264->max_refs << 24) |
+					     (sess->num_dst_bufs << 16) |
+					     ((h264->max_refs - 1) << 8));
+}
+
+/*
+ * Configure the H.264 decoder when the parser detected a parameter set change
+ */
+static void codec_h264_src_change(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_h264 *h264 = sess->priv;
+	u32 parsed_info, mb_total;
+	u32 crop_infor, crop_bottom, crop_right;
+	u32 frame_width, frame_height;
+
+	sess->keyframe_found = 1;
+
+	parsed_info = amvdec_read_dos(core, AV_SCRATCH_1);
+
+	/* Total number of 16x16 macroblocks */
+	mb_total = (parsed_info >> MB_TOTAL_BIT) & MB_TOTAL_MASK;
+	/* Number of macroblocks per line */
+	h264->mb_width = parsed_info & MB_WIDTH_MASK;
+	/* Number of macroblock lines */
+	h264->mb_height = mb_total / h264->mb_width;
+
+	h264->max_refs = ((parsed_info >> MAX_REF_BIT) & MAX_REF_MASK) + 1;
+
+	crop_infor = amvdec_read_dos(core, AV_SCRATCH_6);
+	crop_bottom = (crop_infor & 0xff);
+	crop_right = (crop_infor >> 16) & 0xff;
+
+	frame_width = h264->mb_width * 16 - crop_right;
+	frame_height = h264->mb_height * 16 - crop_bottom;
+
+	dev_dbg(core->dev, "frame: %ux%u; crop: %u %u\n",
+		frame_width, frame_height, crop_right, crop_bottom);
+
+	codec_h264_set_par(sess);
+	amvdec_src_change(sess, frame_width, frame_height, h264->max_refs + 5);
+}
+
+/*
+ * The bitstream offset is split in half in 2 different registers.
+ * Fetch its MSB here, which location depends on the frame number.
+ */
+static u32 get_offset_msb(struct amvdec_core *core, int frame_num)
+{
+	int take_msb = frame_num % 2;
+	int reg_offset = (frame_num / 2) * 4;
+	u32 offset_msb = amvdec_read_dos(core, AV_SCRATCH_A + reg_offset);
+
+	if (take_msb)
+		return offset_msb & 0xffff0000;
+
+	return (offset_msb & 0x0000ffff) << 16;
+}
+
+static void codec_h264_frames_ready(struct amvdec_session *sess, u32 status)
+{
+	struct amvdec_core *core = sess->core;
+	int error_count;
+	int num_frames;
+	int i;
+
+	error_count = amvdec_read_dos(core, AV_SCRATCH_D);
+	num_frames = (status >> 8) & 0xff;
+	if (error_count) {
+		dev_warn(core->dev,
+			 "decoder error(s) happened, count %d\n", error_count);
+		amvdec_write_dos(core, AV_SCRATCH_D, 0);
+	}
+
+	for (i = 0; i < num_frames; i++) {
+		u32 frame_status = amvdec_read_dos(core, AV_SCRATCH_1 + i * 4);
+		u32 buffer_index = frame_status & BUF_IDX_MASK;
+		u32 pic_struct = (frame_status >> PIC_STRUCT_BIT) &
+				 PIC_STRUCT_MASK;
+		u32 offset = (frame_status >> OFFSET_BIT) & OFFSET_MASK;
+		u32 field = V4L2_FIELD_NONE;
+
+		/*
+		 * A buffer decode error means it was decoded,
+		 * but part of the picture will have artifacts.
+		 * Typical reason is a temporarily corrupted bitstream
+		 */
+		if (frame_status & ERROR_FLAG)
+			dev_dbg(core->dev, "Buffer %d decode error\n",
+				buffer_index);
+
+		if (pic_struct == PIC_TOP_BOT)
+			field = V4L2_FIELD_INTERLACED_TB;
+		else if (pic_struct == PIC_BOT_TOP)
+			field = V4L2_FIELD_INTERLACED_BT;
+
+		offset |= get_offset_msb(core, i);
+		amvdec_dst_buf_done_idx(sess, buffer_index, offset, field);
+	}
+}
+
+static irqreturn_t codec_h264_threaded_isr(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	u32 status;
+	u32 size;
+	u8 cmd;
+
+	status = amvdec_read_dos(core, AV_SCRATCH_0);
+	cmd = status & CMD_MASK;
+
+	switch (cmd) {
+	case CMD_SRC_CHANGE:
+		codec_h264_src_change(sess);
+		break;
+	case CMD_FRAMES_READY:
+		codec_h264_frames_ready(sess, status);
+		break;
+	case CMD_FATAL_ERROR:
+		dev_err(core->dev, "H.264 decoder fatal error\n");
+		goto abort;
+	case CMD_BAD_WIDTH:
+		size = (amvdec_read_dos(core, AV_SCRATCH_1) + 1) * 16;
+		dev_err(core->dev, "Unsupported video width: %u\n", size);
+		goto abort;
+	case CMD_BAD_HEIGHT:
+		size = (amvdec_read_dos(core, AV_SCRATCH_1) + 1) * 16;
+		dev_err(core->dev, "Unsupported video height: %u\n", size);
+		goto abort;
+	case 0: /* Unused but not worth printing for */
+	case 9:
+		break;
+	default:
+		dev_info(core->dev, "Unexpected H264 ISR: %08X\n", cmd);
+		break;
+	}
+
+	if (cmd && cmd != CMD_SRC_CHANGE)
+		amvdec_write_dos(core, AV_SCRATCH_0, 0);
+
+	/* Decoder has some SEI data for us ; ignore */
+	if (amvdec_read_dos(core, AV_SCRATCH_J) & SEI_DATA_READY)
+		amvdec_write_dos(core, AV_SCRATCH_J, 0);
+
+	return IRQ_HANDLED;
+abort:
+	amvdec_abort(sess);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t codec_h264_isr(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	amvdec_write_dos(core, ASSIST_MBOX1_CLR_REG, 1);
+
+	return IRQ_WAKE_THREAD;
+}
+
+struct amvdec_codec_ops codec_h264_ops = {
+	.start = codec_h264_start,
+	.stop = codec_h264_stop,
+	.load_extended_firmware = codec_h264_load_extended_firmware,
+	.isr = codec_h264_isr,
+	.threaded_isr = codec_h264_threaded_isr,
+	.can_recycle = codec_h264_can_recycle,
+	.recycle = codec_h264_recycle,
+	.eos_sequence = codec_h264_eos_sequence,
+	.resume = codec_h264_resume,
+};
diff --git a/drivers/staging/media/meson/vdec/codec_h264.h b/drivers/staging/media/meson/vdec/codec_h264.h
new file mode 100644
index 000000000..7cb4fb86f
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_h264.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2019 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_CODEC_H264_H_
+#define __MESON_VDEC_CODEC_H264_H_
+
+#include "vdec.h"
+
+extern struct amvdec_codec_ops codec_h264_ops;
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/codec_hevc_common.c b/drivers/staging/media/meson/vdec/codec_hevc_common.c
new file mode 100644
index 000000000..0315cc091
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_hevc_common.c
@@ -0,0 +1,297 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "codec_hevc_common.h"
+#include "vdec_helpers.h"
+#include "hevc_regs.h"
+
+#define MMU_COMPRESS_HEADER_SIZE 0x48000
+#define MMU_MAP_SIZE 0x4800
+
+const u16 vdec_hevc_parser_cmd[] = {
+	0x0401,	0x8401,	0x0800,	0x0402,
+	0x9002,	0x1423,	0x8CC3,	0x1423,
+	0x8804,	0x9825,	0x0800,	0x04FE,
+	0x8406,	0x8411,	0x1800,	0x8408,
+	0x8409,	0x8C2A,	0x9C2B,	0x1C00,
+	0x840F,	0x8407,	0x8000,	0x8408,
+	0x2000,	0xA800,	0x8410,	0x04DE,
+	0x840C,	0x840D,	0xAC00,	0xA000,
+	0x08C0,	0x08E0,	0xA40E,	0xFC00,
+	0x7C00
+};
+
+/* Configure decode head read mode */
+void codec_hevc_setup_decode_head(struct amvdec_session *sess, int is_10bit)
+{
+	struct amvdec_core *core = sess->core;
+	u32 body_size = amvdec_am21c_body_size(sess->width, sess->height);
+	u32 head_size = amvdec_am21c_head_size(sess->width, sess->height);
+
+	if (!codec_hevc_use_fbc(sess->pixfmt_cap, is_10bit)) {
+		/* Enable 2-plane reference read mode */
+		amvdec_write_dos(core, HEVCD_MPP_DECOMP_CTL1, BIT(31));
+		return;
+	}
+
+	if (codec_hevc_use_mmu(core->platform->revision,
+			       sess->pixfmt_cap, is_10bit))
+		amvdec_write_dos(core, HEVCD_MPP_DECOMP_CTL1, BIT(4));
+	else
+		amvdec_write_dos(core, HEVCD_MPP_DECOMP_CTL1, 0);
+
+	if (core->platform->revision < VDEC_REVISION_SM1)
+		amvdec_write_dos(core, HEVCD_MPP_DECOMP_CTL2, body_size / 32);
+	amvdec_write_dos(core, HEVC_CM_BODY_LENGTH, body_size);
+	amvdec_write_dos(core, HEVC_CM_HEADER_OFFSET, body_size);
+	amvdec_write_dos(core, HEVC_CM_HEADER_LENGTH, head_size);
+}
+EXPORT_SYMBOL_GPL(codec_hevc_setup_decode_head);
+
+static void codec_hevc_setup_buffers_gxbb(struct amvdec_session *sess,
+					  struct codec_hevc_common *comm,
+					  int is_10bit)
+{
+	struct amvdec_core *core = sess->core;
+	struct v4l2_m2m_buffer *buf;
+	u32 buf_num = v4l2_m2m_num_dst_bufs_ready(sess->m2m_ctx);
+	dma_addr_t buf_y_paddr = 0;
+	dma_addr_t buf_uv_paddr = 0;
+	u32 idx = 0;
+	u32 val;
+	int i;
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CONF_ADDR, 0);
+
+	v4l2_m2m_for_each_dst_buf(sess->m2m_ctx, buf) {
+		struct vb2_buffer *vb = &buf->vb.vb2_buf;
+
+		idx = vb->index;
+
+		if (codec_hevc_use_downsample(sess->pixfmt_cap, is_10bit))
+			buf_y_paddr = comm->fbc_buffer_paddr[idx];
+		else
+			buf_y_paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+		if (codec_hevc_use_fbc(sess->pixfmt_cap, is_10bit)) {
+			val = buf_y_paddr | (idx << 8) | 1;
+			amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CMD_ADDR,
+					 val);
+		} else {
+			buf_uv_paddr = vb2_dma_contig_plane_dma_addr(vb, 1);
+			val = buf_y_paddr | ((idx * 2) << 8) | 1;
+			amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CMD_ADDR,
+					 val);
+			val = buf_uv_paddr | ((idx * 2 + 1) << 8) | 1;
+			amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CMD_ADDR,
+					 val);
+		}
+	}
+
+	if (codec_hevc_use_fbc(sess->pixfmt_cap, is_10bit))
+		val = buf_y_paddr | (idx << 8) | 1;
+	else
+		val = buf_y_paddr | ((idx * 2) << 8) | 1;
+
+	/* Fill the remaining unused slots with the last buffer's Y addr */
+	for (i = buf_num; i < MAX_REF_PIC_NUM; ++i)
+		amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CMD_ADDR, val);
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CONF_ADDR, 1);
+	amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR, 1);
+	for (i = 0; i < 32; ++i)
+		amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_DATA_ADDR, 0);
+}
+
+static void codec_hevc_setup_buffers_gxl(struct amvdec_session *sess,
+					 struct codec_hevc_common *comm,
+					 int is_10bit)
+{
+	struct amvdec_core *core = sess->core;
+	struct v4l2_m2m_buffer *buf;
+	u32 revision = core->platform->revision;
+	u32 pixfmt_cap = sess->pixfmt_cap;
+	int i;
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CONF_ADDR,
+			 BIT(2) | BIT(1));
+
+	v4l2_m2m_for_each_dst_buf(sess->m2m_ctx, buf) {
+		struct vb2_buffer *vb = &buf->vb.vb2_buf;
+		dma_addr_t buf_y_paddr = 0;
+		dma_addr_t buf_uv_paddr = 0;
+		u32 idx = vb->index;
+
+		if (codec_hevc_use_mmu(revision, pixfmt_cap, is_10bit))
+			buf_y_paddr = comm->mmu_header_paddr[idx];
+		else if (codec_hevc_use_downsample(pixfmt_cap, is_10bit))
+			buf_y_paddr = comm->fbc_buffer_paddr[idx];
+		else
+			buf_y_paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+		amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_DATA,
+				 buf_y_paddr >> 5);
+
+		if (!codec_hevc_use_fbc(pixfmt_cap, is_10bit)) {
+			buf_uv_paddr = vb2_dma_contig_plane_dma_addr(vb, 1);
+			amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_DATA,
+					 buf_uv_paddr >> 5);
+		}
+	}
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC2AXI_TBL_CONF_ADDR, 1);
+	amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR, 1);
+	for (i = 0; i < 32; ++i)
+		amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_DATA_ADDR, 0);
+}
+
+void codec_hevc_free_fbc_buffers(struct amvdec_session *sess,
+				 struct codec_hevc_common *comm)
+{
+	struct device *dev = sess->core->dev;
+	u32 am21_size = amvdec_am21c_size(sess->width, sess->height);
+	int i;
+
+	for (i = 0; i < MAX_REF_PIC_NUM; ++i) {
+		if (comm->fbc_buffer_vaddr[i]) {
+			dma_free_coherent(dev, am21_size,
+					  comm->fbc_buffer_vaddr[i],
+					  comm->fbc_buffer_paddr[i]);
+			comm->fbc_buffer_vaddr[i] = NULL;
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(codec_hevc_free_fbc_buffers);
+
+static int codec_hevc_alloc_fbc_buffers(struct amvdec_session *sess,
+					struct codec_hevc_common *comm)
+{
+	struct device *dev = sess->core->dev;
+	struct v4l2_m2m_buffer *buf;
+	u32 am21_size = amvdec_am21c_size(sess->width, sess->height);
+
+	v4l2_m2m_for_each_dst_buf(sess->m2m_ctx, buf) {
+		u32 idx = buf->vb.vb2_buf.index;
+		dma_addr_t paddr;
+		void *vaddr = dma_alloc_coherent(dev, am21_size, &paddr,
+						 GFP_KERNEL);
+		if (!vaddr) {
+			codec_hevc_free_fbc_buffers(sess, comm);
+			return -ENOMEM;
+		}
+
+		comm->fbc_buffer_vaddr[idx] = vaddr;
+		comm->fbc_buffer_paddr[idx] = paddr;
+	}
+
+	return 0;
+}
+
+void codec_hevc_free_mmu_headers(struct amvdec_session *sess,
+				 struct codec_hevc_common *comm)
+{
+	struct device *dev = sess->core->dev;
+	int i;
+
+	for (i = 0; i < MAX_REF_PIC_NUM; ++i) {
+		if (comm->mmu_header_vaddr[i]) {
+			dma_free_coherent(dev, MMU_COMPRESS_HEADER_SIZE,
+					  comm->mmu_header_vaddr[i],
+					  comm->mmu_header_paddr[i]);
+			comm->mmu_header_vaddr[i] = NULL;
+		}
+	}
+
+	if (comm->mmu_map_vaddr) {
+		dma_free_coherent(dev, MMU_MAP_SIZE,
+				  comm->mmu_map_vaddr,
+				  comm->mmu_map_paddr);
+		comm->mmu_map_vaddr = NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(codec_hevc_free_mmu_headers);
+
+static int codec_hevc_alloc_mmu_headers(struct amvdec_session *sess,
+					struct codec_hevc_common *comm)
+{
+	struct device *dev = sess->core->dev;
+	struct v4l2_m2m_buffer *buf;
+
+	comm->mmu_map_vaddr = dma_alloc_coherent(dev, MMU_MAP_SIZE,
+						 &comm->mmu_map_paddr,
+						 GFP_KERNEL);
+	if (!comm->mmu_map_vaddr)
+		return -ENOMEM;
+
+	v4l2_m2m_for_each_dst_buf(sess->m2m_ctx, buf) {
+		u32 idx = buf->vb.vb2_buf.index;
+		dma_addr_t paddr;
+		void *vaddr = dma_alloc_coherent(dev, MMU_COMPRESS_HEADER_SIZE,
+						 &paddr, GFP_KERNEL);
+		if (!vaddr) {
+			codec_hevc_free_mmu_headers(sess, comm);
+			return -ENOMEM;
+		}
+
+		comm->mmu_header_vaddr[idx] = vaddr;
+		comm->mmu_header_paddr[idx] = paddr;
+	}
+
+	return 0;
+}
+
+int codec_hevc_setup_buffers(struct amvdec_session *sess,
+			     struct codec_hevc_common *comm,
+			     int is_10bit)
+{
+	struct amvdec_core *core = sess->core;
+	int ret;
+
+	if (codec_hevc_use_downsample(sess->pixfmt_cap, is_10bit)) {
+		ret = codec_hevc_alloc_fbc_buffers(sess, comm);
+		if (ret)
+			return ret;
+	}
+
+	if (codec_hevc_use_mmu(core->platform->revision,
+			       sess->pixfmt_cap, is_10bit)) {
+		ret = codec_hevc_alloc_mmu_headers(sess, comm);
+		if (ret) {
+			codec_hevc_free_fbc_buffers(sess, comm);
+			return ret;
+		}
+	}
+
+	if (core->platform->revision == VDEC_REVISION_GXBB)
+		codec_hevc_setup_buffers_gxbb(sess, comm, is_10bit);
+	else
+		codec_hevc_setup_buffers_gxl(sess, comm, is_10bit);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(codec_hevc_setup_buffers);
+
+void codec_hevc_fill_mmu_map(struct amvdec_session *sess,
+			     struct codec_hevc_common *comm,
+			     struct vb2_buffer *vb)
+{
+	u32 size = amvdec_am21c_size(sess->width, sess->height);
+	u32 nb_pages = size / PAGE_SIZE;
+	u32 *mmu_map = comm->mmu_map_vaddr;
+	u32 first_page;
+	u32 i;
+
+	if (sess->pixfmt_cap == V4L2_PIX_FMT_NV12M)
+		first_page = comm->fbc_buffer_paddr[vb->index] >> PAGE_SHIFT;
+	else
+		first_page = vb2_dma_contig_plane_dma_addr(vb, 0) >> PAGE_SHIFT;
+
+	for (i = 0; i < nb_pages; ++i)
+		mmu_map[i] = first_page + i;
+}
+EXPORT_SYMBOL_GPL(codec_hevc_fill_mmu_map);
diff --git a/drivers/staging/media/meson/vdec/codec_hevc_common.h b/drivers/staging/media/meson/vdec/codec_hevc_common.h
new file mode 100644
index 000000000..cf072b8a9
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_hevc_common.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_HEVC_COMMON_H_
+#define __MESON_VDEC_HEVC_COMMON_H_
+
+#include "vdec.h"
+
+#define PARSER_CMD_SKIP_CFG_0 0x0000090b
+#define PARSER_CMD_SKIP_CFG_1 0x1b14140f
+#define PARSER_CMD_SKIP_CFG_2 0x001b1910
+
+#define VDEC_HEVC_PARSER_CMD_LEN 37
+extern const u16 vdec_hevc_parser_cmd[VDEC_HEVC_PARSER_CMD_LEN];
+
+#define MAX_REF_PIC_NUM	24
+
+struct codec_hevc_common {
+	void      *fbc_buffer_vaddr[MAX_REF_PIC_NUM];
+	dma_addr_t fbc_buffer_paddr[MAX_REF_PIC_NUM];
+
+	void      *mmu_header_vaddr[MAX_REF_PIC_NUM];
+	dma_addr_t mmu_header_paddr[MAX_REF_PIC_NUM];
+
+	void      *mmu_map_vaddr;
+	dma_addr_t mmu_map_paddr;
+};
+
+/* Returns 1 if we must use framebuffer compression */
+static inline int codec_hevc_use_fbc(u32 pixfmt, int is_10bit)
+{
+	/* TOFIX: Handle Amlogic Compressed buffer for 8bit also */
+	return is_10bit;
+}
+
+/* Returns 1 if we are decoding 10-bit but outputting 8-bit NV12 */
+static inline int codec_hevc_use_downsample(u32 pixfmt, int is_10bit)
+{
+	return is_10bit;
+}
+
+/* Returns 1 if we are decoding using the IOMMU */
+static inline int codec_hevc_use_mmu(u32 revision, u32 pixfmt, int is_10bit)
+{
+	return revision >= VDEC_REVISION_G12A &&
+	       codec_hevc_use_fbc(pixfmt, is_10bit);
+}
+
+/* Configure decode head read mode */
+void codec_hevc_setup_decode_head(struct amvdec_session *sess, int is_10bit);
+
+void codec_hevc_free_fbc_buffers(struct amvdec_session *sess,
+				 struct codec_hevc_common *comm);
+
+void codec_hevc_free_mmu_headers(struct amvdec_session *sess,
+				 struct codec_hevc_common *comm);
+
+int codec_hevc_setup_buffers(struct amvdec_session *sess,
+			     struct codec_hevc_common *comm,
+			     int is_10bit);
+
+void codec_hevc_fill_mmu_map(struct amvdec_session *sess,
+			     struct codec_hevc_common *comm,
+			     struct vb2_buffer *vb);
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/codec_mpeg12.c b/drivers/staging/media/meson/vdec/codec_mpeg12.c
new file mode 100644
index 000000000..48869cc3d
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_mpeg12.c
@@ -0,0 +1,210 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "codec_mpeg12.h"
+#include "dos_regs.h"
+#include "vdec_helpers.h"
+
+#define SIZE_WORKSPACE		SZ_128K
+/* Offset substracted by the firmware from the workspace paddr */
+#define WORKSPACE_OFFSET	(5 * SZ_1K)
+
+/* map firmware registers to known MPEG1/2 functions */
+#define MREG_SEQ_INFO		AV_SCRATCH_4
+	#define MPEG2_SEQ_DAR_MASK	GENMASK(3, 0)
+	#define MPEG2_DAR_4_3		2
+	#define MPEG2_DAR_16_9		3
+	#define MPEG2_DAR_221_100	4
+#define MREG_PIC_INFO		AV_SCRATCH_5
+#define MREG_PIC_WIDTH		AV_SCRATCH_6
+#define MREG_PIC_HEIGHT		AV_SCRATCH_7
+#define MREG_BUFFERIN		AV_SCRATCH_8
+#define MREG_BUFFEROUT		AV_SCRATCH_9
+#define MREG_CMD		AV_SCRATCH_A
+#define MREG_CO_MV_START	AV_SCRATCH_B
+#define MREG_ERROR_COUNT	AV_SCRATCH_C
+#define MREG_FRAME_OFFSET	AV_SCRATCH_D
+#define MREG_WAIT_BUFFER	AV_SCRATCH_E
+#define MREG_FATAL_ERROR	AV_SCRATCH_F
+
+#define PICINFO_PROG		0x00008000
+#define PICINFO_TOP_FIRST	0x00002000
+
+struct codec_mpeg12 {
+	/* Buffer for the MPEG1/2 Workspace */
+	void	  *workspace_vaddr;
+	dma_addr_t workspace_paddr;
+};
+
+static const u8 eos_sequence[SZ_1K] = { 0x00, 0x00, 0x01, 0xB7 };
+
+static const u8 *codec_mpeg12_eos_sequence(u32 *len)
+{
+	*len = ARRAY_SIZE(eos_sequence);
+	return eos_sequence;
+}
+
+static int codec_mpeg12_can_recycle(struct amvdec_core *core)
+{
+	return !amvdec_read_dos(core, MREG_BUFFERIN);
+}
+
+static void codec_mpeg12_recycle(struct amvdec_core *core, u32 buf_idx)
+{
+	amvdec_write_dos(core, MREG_BUFFERIN, buf_idx + 1);
+}
+
+static int codec_mpeg12_start(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_mpeg12 *mpeg12;
+	int ret;
+
+	mpeg12 = kzalloc(sizeof(*mpeg12), GFP_KERNEL);
+	if (!mpeg12)
+		return -ENOMEM;
+
+	/* Allocate some memory for the MPEG1/2 decoder's state */
+	mpeg12->workspace_vaddr = dma_alloc_coherent(core->dev, SIZE_WORKSPACE,
+						     &mpeg12->workspace_paddr,
+						     GFP_KERNEL);
+	if (!mpeg12->workspace_vaddr) {
+		dev_err(core->dev, "Failed to request MPEG 1/2 Workspace\n");
+		ret = -ENOMEM;
+		goto free_mpeg12;
+	}
+
+	ret = amvdec_set_canvases(sess, (u32[]){ AV_SCRATCH_0, 0 },
+					(u32[]){ 8, 0 });
+	if (ret)
+		goto free_workspace;
+
+	amvdec_write_dos(core, POWER_CTL_VLD, BIT(4));
+	amvdec_write_dos(core, MREG_CO_MV_START,
+			 mpeg12->workspace_paddr + WORKSPACE_OFFSET);
+
+	amvdec_write_dos(core, MPEG1_2_REG, 0);
+	amvdec_write_dos(core, PSCALE_CTRL, 0);
+	amvdec_write_dos(core, PIC_HEAD_INFO, 0x380);
+	amvdec_write_dos(core, M4_CONTROL_REG, 0);
+	amvdec_write_dos(core, MREG_BUFFERIN, 0);
+	amvdec_write_dos(core, MREG_BUFFEROUT, 0);
+	amvdec_write_dos(core, MREG_CMD, (sess->width << 16) | sess->height);
+	amvdec_write_dos(core, MREG_ERROR_COUNT, 0);
+	amvdec_write_dos(core, MREG_FATAL_ERROR, 0);
+	amvdec_write_dos(core, MREG_WAIT_BUFFER, 0);
+
+	sess->keyframe_found = 1;
+	sess->priv = mpeg12;
+
+	return 0;
+
+free_workspace:
+	dma_free_coherent(core->dev, SIZE_WORKSPACE, mpeg12->workspace_vaddr,
+			  mpeg12->workspace_paddr);
+free_mpeg12:
+	kfree(mpeg12);
+
+	return ret;
+}
+
+static int codec_mpeg12_stop(struct amvdec_session *sess)
+{
+	struct codec_mpeg12 *mpeg12 = sess->priv;
+	struct amvdec_core *core = sess->core;
+
+	if (mpeg12->workspace_vaddr)
+		dma_free_coherent(core->dev, SIZE_WORKSPACE,
+				  mpeg12->workspace_vaddr,
+				  mpeg12->workspace_paddr);
+
+	return 0;
+}
+
+static void codec_mpeg12_update_dar(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	u32 seq = amvdec_read_dos(core, MREG_SEQ_INFO);
+	u32 ar = seq & MPEG2_SEQ_DAR_MASK;
+
+	switch (ar) {
+	case MPEG2_DAR_4_3:
+		amvdec_set_par_from_dar(sess, 4, 3);
+		break;
+	case MPEG2_DAR_16_9:
+		amvdec_set_par_from_dar(sess, 16, 9);
+		break;
+	case MPEG2_DAR_221_100:
+		amvdec_set_par_from_dar(sess, 221, 100);
+		break;
+	default:
+		sess->pixelaspect.numerator = 1;
+		sess->pixelaspect.denominator = 1;
+		break;
+	}
+}
+
+static irqreturn_t codec_mpeg12_threaded_isr(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	u32 reg;
+	u32 pic_info;
+	u32 is_progressive;
+	u32 buffer_index;
+	u32 field = V4L2_FIELD_NONE;
+	u32 offset;
+
+	amvdec_write_dos(core, ASSIST_MBOX1_CLR_REG, 1);
+	reg = amvdec_read_dos(core, MREG_FATAL_ERROR);
+	if (reg == 1) {
+		dev_err(core->dev, "MPEG1/2 fatal error\n");
+		amvdec_abort(sess);
+		return IRQ_HANDLED;
+	}
+
+	reg = amvdec_read_dos(core, MREG_BUFFEROUT);
+	if (!reg)
+		return IRQ_HANDLED;
+
+	/* Unclear what this means */
+	if ((reg & GENMASK(23, 17)) == GENMASK(23, 17))
+		goto end;
+
+	pic_info = amvdec_read_dos(core, MREG_PIC_INFO);
+	is_progressive = pic_info & PICINFO_PROG;
+
+	if (!is_progressive)
+		field = (pic_info & PICINFO_TOP_FIRST) ?
+			V4L2_FIELD_INTERLACED_TB :
+			V4L2_FIELD_INTERLACED_BT;
+
+	codec_mpeg12_update_dar(sess);
+	buffer_index = ((reg & 0xf) - 1) & 7;
+	offset = amvdec_read_dos(core, MREG_FRAME_OFFSET);
+	amvdec_dst_buf_done_idx(sess, buffer_index, offset, field);
+
+end:
+	amvdec_write_dos(core, MREG_BUFFEROUT, 0);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t codec_mpeg12_isr(struct amvdec_session *sess)
+{
+	return IRQ_WAKE_THREAD;
+}
+
+struct amvdec_codec_ops codec_mpeg12_ops = {
+	.start = codec_mpeg12_start,
+	.stop = codec_mpeg12_stop,
+	.isr = codec_mpeg12_isr,
+	.threaded_isr = codec_mpeg12_threaded_isr,
+	.can_recycle = codec_mpeg12_can_recycle,
+	.recycle = codec_mpeg12_recycle,
+	.eos_sequence = codec_mpeg12_eos_sequence,
+};
diff --git a/drivers/staging/media/meson/vdec/codec_mpeg12.h b/drivers/staging/media/meson/vdec/codec_mpeg12.h
new file mode 100644
index 000000000..43cab5f39
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_mpeg12.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_CODEC_MPEG12_H_
+#define __MESON_VDEC_CODEC_MPEG12_H_
+
+#include "vdec.h"
+
+extern struct amvdec_codec_ops codec_mpeg12_ops;
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/codec_vp9.c b/drivers/staging/media/meson/vdec/codec_vp9.c
new file mode 100644
index 000000000..897f5d7a6
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_vp9.c
@@ -0,0 +1,2172 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 Maxime Jourdan <mjourdan@baylibre.com>
+ * Copyright (C) 2015 Amlogic, Inc. All rights reserved.
+ */
+
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "dos_regs.h"
+#include "hevc_regs.h"
+#include "codec_vp9.h"
+#include "vdec_helpers.h"
+#include "codec_hevc_common.h"
+
+/* HEVC reg mapping */
+#define VP9_DEC_STATUS_REG	HEVC_ASSIST_SCRATCH_0
+	#define VP9_10B_DECODE_SLICE	5
+	#define VP9_HEAD_PARSER_DONE	0xf0
+#define VP9_RPM_BUFFER		HEVC_ASSIST_SCRATCH_1
+#define VP9_SHORT_TERM_RPS	HEVC_ASSIST_SCRATCH_2
+#define VP9_ADAPT_PROB_REG	HEVC_ASSIST_SCRATCH_3
+#define VP9_MMU_MAP_BUFFER	HEVC_ASSIST_SCRATCH_4
+#define VP9_PPS_BUFFER		HEVC_ASSIST_SCRATCH_5
+#define VP9_SAO_UP		HEVC_ASSIST_SCRATCH_6
+#define VP9_STREAM_SWAP_BUFFER	HEVC_ASSIST_SCRATCH_7
+#define VP9_STREAM_SWAP_BUFFER2 HEVC_ASSIST_SCRATCH_8
+#define VP9_PROB_SWAP_BUFFER	HEVC_ASSIST_SCRATCH_9
+#define VP9_COUNT_SWAP_BUFFER	HEVC_ASSIST_SCRATCH_A
+#define VP9_SEG_MAP_BUFFER	HEVC_ASSIST_SCRATCH_B
+#define VP9_SCALELUT		HEVC_ASSIST_SCRATCH_D
+#define VP9_WAIT_FLAG		HEVC_ASSIST_SCRATCH_E
+#define LMEM_DUMP_ADR		HEVC_ASSIST_SCRATCH_F
+#define NAL_SEARCH_CTL		HEVC_ASSIST_SCRATCH_I
+#define VP9_DECODE_MODE		HEVC_ASSIST_SCRATCH_J
+	#define DECODE_MODE_SINGLE 0
+#define DECODE_STOP_POS		HEVC_ASSIST_SCRATCH_K
+#define HEVC_DECODE_COUNT	HEVC_ASSIST_SCRATCH_M
+#define HEVC_DECODE_SIZE	HEVC_ASSIST_SCRATCH_N
+
+/* VP9 Constants */
+#define LCU_SIZE		64
+#define MAX_REF_PIC_NUM		24
+#define REFS_PER_FRAME		3
+#define REF_FRAMES		8
+#define MV_MEM_UNIT		0x240
+#define ADAPT_PROB_SIZE		0xf80
+
+enum FRAME_TYPE {
+	KEY_FRAME = 0,
+	INTER_FRAME = 1,
+	FRAME_TYPES,
+};
+
+/* VP9 Workspace layout */
+#define MPRED_MV_BUF_SIZE 0x120000
+
+#define IPP_SIZE	0x4000
+#define SAO_ABV_SIZE	0x30000
+#define SAO_VB_SIZE	0x30000
+#define SH_TM_RPS_SIZE	0x800
+#define VPS_SIZE	0x800
+#define SPS_SIZE	0x800
+#define PPS_SIZE	0x2000
+#define SAO_UP_SIZE	0x2800
+#define SWAP_BUF_SIZE	0x800
+#define SWAP_BUF2_SIZE	0x800
+#define SCALELUT_SIZE	0x8000
+#define DBLK_PARA_SIZE	0x80000
+#define DBLK_DATA_SIZE	0x80000
+#define SEG_MAP_SIZE	0xd800
+#define PROB_SIZE	0x5000
+#define COUNT_SIZE	0x3000
+#define MMU_VBH_SIZE	0x5000
+#define MPRED_ABV_SIZE	0x10000
+#define MPRED_MV_SIZE	(MPRED_MV_BUF_SIZE * MAX_REF_PIC_NUM)
+#define RPM_BUF_SIZE	0x100
+#define LMEM_SIZE	0x800
+
+#define IPP_OFFSET       0x00
+#define SAO_ABV_OFFSET   (IPP_OFFSET + IPP_SIZE)
+#define SAO_VB_OFFSET    (SAO_ABV_OFFSET + SAO_ABV_SIZE)
+#define SH_TM_RPS_OFFSET (SAO_VB_OFFSET + SAO_VB_SIZE)
+#define VPS_OFFSET       (SH_TM_RPS_OFFSET + SH_TM_RPS_SIZE)
+#define SPS_OFFSET       (VPS_OFFSET + VPS_SIZE)
+#define PPS_OFFSET       (SPS_OFFSET + SPS_SIZE)
+#define SAO_UP_OFFSET    (PPS_OFFSET + PPS_SIZE)
+#define SWAP_BUF_OFFSET  (SAO_UP_OFFSET + SAO_UP_SIZE)
+#define SWAP_BUF2_OFFSET (SWAP_BUF_OFFSET + SWAP_BUF_SIZE)
+#define SCALELUT_OFFSET  (SWAP_BUF2_OFFSET + SWAP_BUF2_SIZE)
+#define DBLK_PARA_OFFSET (SCALELUT_OFFSET + SCALELUT_SIZE)
+#define DBLK_DATA_OFFSET (DBLK_PARA_OFFSET + DBLK_PARA_SIZE)
+#define SEG_MAP_OFFSET   (DBLK_DATA_OFFSET + DBLK_DATA_SIZE)
+#define PROB_OFFSET      (SEG_MAP_OFFSET + SEG_MAP_SIZE)
+#define COUNT_OFFSET     (PROB_OFFSET + PROB_SIZE)
+#define MMU_VBH_OFFSET   (COUNT_OFFSET + COUNT_SIZE)
+#define MPRED_ABV_OFFSET (MMU_VBH_OFFSET + MMU_VBH_SIZE)
+#define MPRED_MV_OFFSET  (MPRED_ABV_OFFSET + MPRED_ABV_SIZE)
+#define RPM_OFFSET       (MPRED_MV_OFFSET + MPRED_MV_SIZE)
+#define LMEM_OFFSET      (RPM_OFFSET + RPM_BUF_SIZE)
+
+#define SIZE_WORKSPACE	ALIGN(LMEM_OFFSET + LMEM_SIZE, 64 * SZ_1K)
+
+#define NONE           -1
+#define INTRA_FRAME     0
+#define LAST_FRAME      1
+#define GOLDEN_FRAME    2
+#define ALTREF_FRAME    3
+#define MAX_REF_FRAMES  4
+
+/*
+ * Defines, declarations, sub-functions for vp9 de-block loop
+	filter Thr/Lvl table update
+ * - struct segmentation is for loop filter only (removed something)
+ * - function "vp9_loop_filter_init" and "vp9_loop_filter_frame_init" will
+	be instantiated in C_Entry
+ * - vp9_loop_filter_init run once before decoding start
+ * - vp9_loop_filter_frame_init run before every frame decoding start
+ * - set video format to VP9 is in vp9_loop_filter_init
+ */
+#define MAX_LOOP_FILTER		63
+#define MAX_REF_LF_DELTAS	4
+#define MAX_MODE_LF_DELTAS	2
+#define SEGMENT_DELTADATA	0
+#define SEGMENT_ABSDATA		1
+#define MAX_SEGMENTS		8
+
+/* VP9 PROB processing defines */
+#define VP9_PARTITION_START      0
+#define VP9_PARTITION_SIZE_STEP  (3 * 4)
+#define VP9_PARTITION_ONE_SIZE   (4 * VP9_PARTITION_SIZE_STEP)
+#define VP9_PARTITION_KEY_START  0
+#define VP9_PARTITION_P_START    VP9_PARTITION_ONE_SIZE
+#define VP9_PARTITION_SIZE       (2 * VP9_PARTITION_ONE_SIZE)
+#define VP9_SKIP_START           (VP9_PARTITION_START + VP9_PARTITION_SIZE)
+#define VP9_SKIP_SIZE            4 /* only use 3*/
+#define VP9_TX_MODE_START        (VP9_SKIP_START + VP9_SKIP_SIZE)
+#define VP9_TX_MODE_8_0_OFFSET   0
+#define VP9_TX_MODE_8_1_OFFSET   1
+#define VP9_TX_MODE_16_0_OFFSET  2
+#define VP9_TX_MODE_16_1_OFFSET  4
+#define VP9_TX_MODE_32_0_OFFSET  6
+#define VP9_TX_MODE_32_1_OFFSET  9
+#define VP9_TX_MODE_SIZE         12
+#define VP9_COEF_START           (VP9_TX_MODE_START + VP9_TX_MODE_SIZE)
+#define VP9_COEF_BAND_0_OFFSET   0
+#define VP9_COEF_BAND_1_OFFSET   (VP9_COEF_BAND_0_OFFSET + 3 * 3 + 1)
+#define VP9_COEF_BAND_2_OFFSET   (VP9_COEF_BAND_1_OFFSET + 6 * 3)
+#define VP9_COEF_BAND_3_OFFSET   (VP9_COEF_BAND_2_OFFSET + 6 * 3)
+#define VP9_COEF_BAND_4_OFFSET   (VP9_COEF_BAND_3_OFFSET + 6 * 3)
+#define VP9_COEF_BAND_5_OFFSET   (VP9_COEF_BAND_4_OFFSET + 6 * 3)
+#define VP9_COEF_SIZE_ONE_SET    100 /* ((3 + 5 * 6) * 3 + 1 padding)*/
+#define VP9_COEF_4X4_START       (VP9_COEF_START + 0 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_COEF_8X8_START       (VP9_COEF_START + 4 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_COEF_16X16_START     (VP9_COEF_START + 8 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_COEF_32X32_START     (VP9_COEF_START + 12 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_COEF_SIZE_PLANE      (2 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_COEF_SIZE            (4 * 2 * 2 * VP9_COEF_SIZE_ONE_SET)
+#define VP9_INTER_MODE_START     (VP9_COEF_START + VP9_COEF_SIZE)
+#define VP9_INTER_MODE_SIZE      24 /* only use 21 (# * 7)*/
+#define VP9_INTERP_START         (VP9_INTER_MODE_START + VP9_INTER_MODE_SIZE)
+#define VP9_INTERP_SIZE          8
+#define VP9_INTRA_INTER_START    (VP9_INTERP_START + VP9_INTERP_SIZE)
+#define VP9_INTRA_INTER_SIZE     4
+#define VP9_INTERP_INTRA_INTER_START  VP9_INTERP_START
+#define VP9_INTERP_INTRA_INTER_SIZE   (VP9_INTERP_SIZE + VP9_INTRA_INTER_SIZE)
+#define VP9_COMP_INTER_START     \
+		(VP9_INTERP_INTRA_INTER_START + VP9_INTERP_INTRA_INTER_SIZE)
+#define VP9_COMP_INTER_SIZE      5
+#define VP9_COMP_REF_START       (VP9_COMP_INTER_START + VP9_COMP_INTER_SIZE)
+#define VP9_COMP_REF_SIZE        5
+#define VP9_SINGLE_REF_START     (VP9_COMP_REF_START + VP9_COMP_REF_SIZE)
+#define VP9_SINGLE_REF_SIZE      10
+#define VP9_REF_MODE_START       VP9_COMP_INTER_START
+#define VP9_REF_MODE_SIZE        \
+		(VP9_COMP_INTER_SIZE + VP9_COMP_REF_SIZE + VP9_SINGLE_REF_SIZE)
+#define VP9_IF_Y_MODE_START      (VP9_REF_MODE_START + VP9_REF_MODE_SIZE)
+#define VP9_IF_Y_MODE_SIZE       36
+#define VP9_IF_UV_MODE_START     (VP9_IF_Y_MODE_START + VP9_IF_Y_MODE_SIZE)
+#define VP9_IF_UV_MODE_SIZE      92 /* only use 90*/
+#define VP9_MV_JOINTS_START      (VP9_IF_UV_MODE_START + VP9_IF_UV_MODE_SIZE)
+#define VP9_MV_JOINTS_SIZE       3
+#define VP9_MV_SIGN_0_START      (VP9_MV_JOINTS_START + VP9_MV_JOINTS_SIZE)
+#define VP9_MV_SIGN_0_SIZE       1
+#define VP9_MV_CLASSES_0_START   (VP9_MV_SIGN_0_START + VP9_MV_SIGN_0_SIZE)
+#define VP9_MV_CLASSES_0_SIZE    10
+#define VP9_MV_CLASS0_0_START    \
+		(VP9_MV_CLASSES_0_START + VP9_MV_CLASSES_0_SIZE)
+#define VP9_MV_CLASS0_0_SIZE     1
+#define VP9_MV_BITS_0_START      (VP9_MV_CLASS0_0_START + VP9_MV_CLASS0_0_SIZE)
+#define VP9_MV_BITS_0_SIZE       10
+#define VP9_MV_SIGN_1_START      (VP9_MV_BITS_0_START + VP9_MV_BITS_0_SIZE)
+#define VP9_MV_SIGN_1_SIZE       1
+#define VP9_MV_CLASSES_1_START   \
+			(VP9_MV_SIGN_1_START + VP9_MV_SIGN_1_SIZE)
+#define VP9_MV_CLASSES_1_SIZE    10
+#define VP9_MV_CLASS0_1_START    \
+			(VP9_MV_CLASSES_1_START + VP9_MV_CLASSES_1_SIZE)
+#define VP9_MV_CLASS0_1_SIZE     1
+#define VP9_MV_BITS_1_START      \
+			(VP9_MV_CLASS0_1_START + VP9_MV_CLASS0_1_SIZE)
+#define VP9_MV_BITS_1_SIZE       10
+#define VP9_MV_CLASS0_FP_0_START \
+			(VP9_MV_BITS_1_START + VP9_MV_BITS_1_SIZE)
+#define VP9_MV_CLASS0_FP_0_SIZE  9
+#define VP9_MV_CLASS0_FP_1_START \
+			(VP9_MV_CLASS0_FP_0_START + VP9_MV_CLASS0_FP_0_SIZE)
+#define VP9_MV_CLASS0_FP_1_SIZE  9
+#define VP9_MV_CLASS0_HP_0_START \
+			(VP9_MV_CLASS0_FP_1_START + VP9_MV_CLASS0_FP_1_SIZE)
+#define VP9_MV_CLASS0_HP_0_SIZE  2
+#define VP9_MV_CLASS0_HP_1_START \
+			(VP9_MV_CLASS0_HP_0_START + VP9_MV_CLASS0_HP_0_SIZE)
+#define VP9_MV_CLASS0_HP_1_SIZE  2
+#define VP9_MV_START             VP9_MV_JOINTS_START
+#define VP9_MV_SIZE              72 /*only use 69*/
+
+#define VP9_TOTAL_SIZE           (VP9_MV_START + VP9_MV_SIZE)
+
+/* VP9 COUNT mem processing defines */
+#define VP9_COEF_COUNT_START           0
+#define VP9_COEF_COUNT_BAND_0_OFFSET   0
+#define VP9_COEF_COUNT_BAND_1_OFFSET   \
+			(VP9_COEF_COUNT_BAND_0_OFFSET + 3 * 5)
+#define VP9_COEF_COUNT_BAND_2_OFFSET   \
+			(VP9_COEF_COUNT_BAND_1_OFFSET + 6 * 5)
+#define VP9_COEF_COUNT_BAND_3_OFFSET   \
+			(VP9_COEF_COUNT_BAND_2_OFFSET + 6 * 5)
+#define VP9_COEF_COUNT_BAND_4_OFFSET   \
+			(VP9_COEF_COUNT_BAND_3_OFFSET + 6 * 5)
+#define VP9_COEF_COUNT_BAND_5_OFFSET   \
+			(VP9_COEF_COUNT_BAND_4_OFFSET + 6 * 5)
+#define VP9_COEF_COUNT_SIZE_ONE_SET    165 /* ((3 + 5 * 6) * 5 */
+#define VP9_COEF_COUNT_4X4_START       \
+		(VP9_COEF_COUNT_START + 0 * VP9_COEF_COUNT_SIZE_ONE_SET)
+#define VP9_COEF_COUNT_8X8_START       \
+		(VP9_COEF_COUNT_START + 4 * VP9_COEF_COUNT_SIZE_ONE_SET)
+#define VP9_COEF_COUNT_16X16_START     \
+		(VP9_COEF_COUNT_START + 8 * VP9_COEF_COUNT_SIZE_ONE_SET)
+#define VP9_COEF_COUNT_32X32_START     \
+		(VP9_COEF_COUNT_START + 12 * VP9_COEF_COUNT_SIZE_ONE_SET)
+#define VP9_COEF_COUNT_SIZE_PLANE      (2 * VP9_COEF_COUNT_SIZE_ONE_SET)
+#define VP9_COEF_COUNT_SIZE            (4 * 2 * 2 * VP9_COEF_COUNT_SIZE_ONE_SET)
+
+#define VP9_INTRA_INTER_COUNT_START    \
+		(VP9_COEF_COUNT_START + VP9_COEF_COUNT_SIZE)
+#define VP9_INTRA_INTER_COUNT_SIZE     (4 * 2)
+#define VP9_COMP_INTER_COUNT_START     \
+		(VP9_INTRA_INTER_COUNT_START + VP9_INTRA_INTER_COUNT_SIZE)
+#define VP9_COMP_INTER_COUNT_SIZE      (5 * 2)
+#define VP9_COMP_REF_COUNT_START       \
+		(VP9_COMP_INTER_COUNT_START + VP9_COMP_INTER_COUNT_SIZE)
+#define VP9_COMP_REF_COUNT_SIZE        (5 * 2)
+#define VP9_SINGLE_REF_COUNT_START     \
+		(VP9_COMP_REF_COUNT_START + VP9_COMP_REF_COUNT_SIZE)
+#define VP9_SINGLE_REF_COUNT_SIZE      (10 * 2)
+#define VP9_TX_MODE_COUNT_START        \
+		(VP9_SINGLE_REF_COUNT_START + VP9_SINGLE_REF_COUNT_SIZE)
+#define VP9_TX_MODE_COUNT_SIZE         (12 * 2)
+#define VP9_SKIP_COUNT_START           \
+		(VP9_TX_MODE_COUNT_START + VP9_TX_MODE_COUNT_SIZE)
+#define VP9_SKIP_COUNT_SIZE            (3 * 2)
+#define VP9_MV_SIGN_0_COUNT_START      \
+		(VP9_SKIP_COUNT_START + VP9_SKIP_COUNT_SIZE)
+#define VP9_MV_SIGN_0_COUNT_SIZE       (1 * 2)
+#define VP9_MV_SIGN_1_COUNT_START      \
+		(VP9_MV_SIGN_0_COUNT_START + VP9_MV_SIGN_0_COUNT_SIZE)
+#define VP9_MV_SIGN_1_COUNT_SIZE       (1 * 2)
+#define VP9_MV_BITS_0_COUNT_START      \
+		(VP9_MV_SIGN_1_COUNT_START + VP9_MV_SIGN_1_COUNT_SIZE)
+#define VP9_MV_BITS_0_COUNT_SIZE       (10 * 2)
+#define VP9_MV_BITS_1_COUNT_START      \
+		(VP9_MV_BITS_0_COUNT_START + VP9_MV_BITS_0_COUNT_SIZE)
+#define VP9_MV_BITS_1_COUNT_SIZE       (10 * 2)
+#define VP9_MV_CLASS0_HP_0_COUNT_START \
+		(VP9_MV_BITS_1_COUNT_START + VP9_MV_BITS_1_COUNT_SIZE)
+#define VP9_MV_CLASS0_HP_0_COUNT_SIZE  (2 * 2)
+#define VP9_MV_CLASS0_HP_1_COUNT_START \
+		(VP9_MV_CLASS0_HP_0_COUNT_START + VP9_MV_CLASS0_HP_0_COUNT_SIZE)
+#define VP9_MV_CLASS0_HP_1_COUNT_SIZE  (2 * 2)
+
+/* Start merge_tree */
+#define VP9_INTER_MODE_COUNT_START     \
+		(VP9_MV_CLASS0_HP_1_COUNT_START + VP9_MV_CLASS0_HP_1_COUNT_SIZE)
+#define VP9_INTER_MODE_COUNT_SIZE      (7 * 4)
+#define VP9_IF_Y_MODE_COUNT_START      \
+		(VP9_INTER_MODE_COUNT_START + VP9_INTER_MODE_COUNT_SIZE)
+#define VP9_IF_Y_MODE_COUNT_SIZE       (10 * 4)
+#define VP9_IF_UV_MODE_COUNT_START     \
+		(VP9_IF_Y_MODE_COUNT_START + VP9_IF_Y_MODE_COUNT_SIZE)
+#define VP9_IF_UV_MODE_COUNT_SIZE      (10 * 10)
+#define VP9_PARTITION_P_COUNT_START    \
+		(VP9_IF_UV_MODE_COUNT_START + VP9_IF_UV_MODE_COUNT_SIZE)
+#define VP9_PARTITION_P_COUNT_SIZE     (4 * 4 * 4)
+#define VP9_INTERP_COUNT_START         \
+		(VP9_PARTITION_P_COUNT_START + VP9_PARTITION_P_COUNT_SIZE)
+#define VP9_INTERP_COUNT_SIZE          (4 * 3)
+#define VP9_MV_JOINTS_COUNT_START      \
+		(VP9_INTERP_COUNT_START + VP9_INTERP_COUNT_SIZE)
+#define VP9_MV_JOINTS_COUNT_SIZE       (1 * 4)
+#define VP9_MV_CLASSES_0_COUNT_START   \
+		(VP9_MV_JOINTS_COUNT_START + VP9_MV_JOINTS_COUNT_SIZE)
+#define VP9_MV_CLASSES_0_COUNT_SIZE    (1 * 11)
+#define VP9_MV_CLASS0_0_COUNT_START    \
+		(VP9_MV_CLASSES_0_COUNT_START + VP9_MV_CLASSES_0_COUNT_SIZE)
+#define VP9_MV_CLASS0_0_COUNT_SIZE     (1 * 2)
+#define VP9_MV_CLASSES_1_COUNT_START   \
+		(VP9_MV_CLASS0_0_COUNT_START + VP9_MV_CLASS0_0_COUNT_SIZE)
+#define VP9_MV_CLASSES_1_COUNT_SIZE    (1 * 11)
+#define VP9_MV_CLASS0_1_COUNT_START    \
+		(VP9_MV_CLASSES_1_COUNT_START + VP9_MV_CLASSES_1_COUNT_SIZE)
+#define VP9_MV_CLASS0_1_COUNT_SIZE     (1 * 2)
+#define VP9_MV_CLASS0_FP_0_COUNT_START \
+		(VP9_MV_CLASS0_1_COUNT_START + VP9_MV_CLASS0_1_COUNT_SIZE)
+#define VP9_MV_CLASS0_FP_0_COUNT_SIZE  (3 * 4)
+#define VP9_MV_CLASS0_FP_1_COUNT_START \
+		(VP9_MV_CLASS0_FP_0_COUNT_START + VP9_MV_CLASS0_FP_0_COUNT_SIZE)
+#define VP9_MV_CLASS0_FP_1_COUNT_SIZE  (3 * 4)
+
+#define DC_PRED    0	/* Average of above and left pixels */
+#define V_PRED     1	/* Vertical */
+#define H_PRED     2	/* Horizontal */
+#define D45_PRED   3	/* Directional 45 deg = round(arctan(1/1) * 180/pi) */
+#define D135_PRED  4	/* Directional 135 deg = 180 - 45 */
+#define D117_PRED  5	/* Directional 117 deg = 180 - 63 */
+#define D153_PRED  6	/* Directional 153 deg = 180 - 27 */
+#define D207_PRED  7	/* Directional 207 deg = 180 + 27 */
+#define D63_PRED   8	/* Directional 63 deg = round(arctan(2/1) * 180/pi) */
+#define TM_PRED    9	/* True-motion */
+
+/* Use a static inline to avoid possible side effect from num being reused */
+static inline int round_power_of_two(int value, int num)
+{
+	return (value + (1 << (num - 1))) >> num;
+}
+
+#define MODE_MV_COUNT_SAT 20
+static const int count_to_update_factor[MODE_MV_COUNT_SAT + 1] = {
+	0, 6, 12, 19, 25, 32, 38, 44, 51, 57, 64,
+	70, 76, 83, 89, 96, 102, 108, 115, 121, 128
+};
+
+union rpm_param {
+	struct {
+		u16 data[RPM_BUF_SIZE];
+	} l;
+	struct {
+		u16 profile;
+		u16 show_existing_frame;
+		u16 frame_to_show_idx;
+		u16 frame_type; /*1 bit*/
+		u16 show_frame; /*1 bit*/
+		u16 error_resilient_mode; /*1 bit*/
+		u16 intra_only; /*1 bit*/
+		u16 display_size_present; /*1 bit*/
+		u16 reset_frame_context;
+		u16 refresh_frame_flags;
+		u16 width;
+		u16 height;
+		u16 display_width;
+		u16 display_height;
+		u16 ref_info;
+		u16 same_frame_size;
+		u16 mode_ref_delta_enabled;
+		u16 ref_deltas[4];
+		u16 mode_deltas[2];
+		u16 filter_level;
+		u16 sharpness_level;
+		u16 bit_depth;
+		u16 seg_quant_info[8];
+		u16 seg_enabled;
+		u16 seg_abs_delta;
+		/* bit 15: feature enabled; bit 8, sign; bit[5:0], data */
+		u16 seg_lf_info[8];
+	} p;
+};
+
+enum SEG_LVL_FEATURES {
+	SEG_LVL_ALT_Q = 0,	/* Use alternate Quantizer */
+	SEG_LVL_ALT_LF = 1,	/* Use alternate loop filter value */
+	SEG_LVL_REF_FRAME = 2,	/* Optional Segment reference frame */
+	SEG_LVL_SKIP = 3,	/* Optional Segment (0,0) + skip mode */
+	SEG_LVL_MAX = 4		/* Number of features supported */
+};
+
+struct segmentation {
+	u8 enabled;
+	u8 update_map;
+	u8 update_data;
+	u8 abs_delta;
+	u8 temporal_update;
+	s16 feature_data[MAX_SEGMENTS][SEG_LVL_MAX];
+	unsigned int feature_mask[MAX_SEGMENTS];
+};
+
+struct loop_filter_thresh {
+	u8 mblim;
+	u8 lim;
+	u8 hev_thr;
+};
+
+struct loop_filter_info_n {
+	struct loop_filter_thresh lfthr[MAX_LOOP_FILTER + 1];
+	u8 lvl[MAX_SEGMENTS][MAX_REF_FRAMES][MAX_MODE_LF_DELTAS];
+};
+
+struct loopfilter {
+	int filter_level;
+
+	int sharpness_level;
+	int last_sharpness_level;
+
+	u8 mode_ref_delta_enabled;
+	u8 mode_ref_delta_update;
+
+	/*0 = Intra, Last, GF, ARF*/
+	signed char ref_deltas[MAX_REF_LF_DELTAS];
+	signed char last_ref_deltas[MAX_REF_LF_DELTAS];
+
+	/*0 = ZERO_MV, MV*/
+	signed char mode_deltas[MAX_MODE_LF_DELTAS];
+	signed char last_mode_deltas[MAX_MODE_LF_DELTAS];
+};
+
+struct vp9_frame {
+	struct list_head list;
+	struct vb2_v4l2_buffer *vbuf;
+	int index;
+	int intra_only;
+	int show;
+	int type;
+	int done;
+	unsigned int width;
+	unsigned int height;
+};
+
+struct codec_vp9 {
+	/* VP9 context lock */
+	struct mutex lock;
+
+	/* Common part with the HEVC decoder */
+	struct codec_hevc_common common;
+
+	/* Buffer for the VP9 Workspace */
+	void      *workspace_vaddr;
+	dma_addr_t workspace_paddr;
+
+	/* Contains many information parsed from the bitstream */
+	union rpm_param rpm_param;
+
+	/* Whether we detected the bitstream as 10-bit */
+	int is_10bit;
+
+	/* Coded resolution reported by the hardware */
+	u32 width, height;
+
+	/* All ref frames used by the HW at a given time */
+	struct list_head ref_frames_list;
+	u32 frames_num;
+
+	/* In case of downsampling (decoding with FBC but outputting in NV12M),
+	 * we need to allocate additional buffers for FBC.
+	 */
+	void      *fbc_buffer_vaddr[MAX_REF_PIC_NUM];
+	dma_addr_t fbc_buffer_paddr[MAX_REF_PIC_NUM];
+
+	int ref_frame_map[REF_FRAMES];
+	int next_ref_frame_map[REF_FRAMES];
+	struct vp9_frame *frame_refs[REFS_PER_FRAME];
+
+	u32 lcu_total;
+
+	/* loop filter */
+	int default_filt_lvl;
+	struct loop_filter_info_n lfi;
+	struct loopfilter lf;
+	struct segmentation seg_4lf;
+
+	struct vp9_frame *cur_frame;
+	struct vp9_frame *prev_frame;
+};
+
+static int div_r32(s64 m, int n)
+{
+	s64 qu = div_s64(m, n);
+
+	return (int)qu;
+}
+
+static int clip_prob(int p)
+{
+	return clamp_val(p, 1, 255);
+}
+
+static int segfeature_active(struct segmentation *seg, int segment_id,
+			     enum SEG_LVL_FEATURES feature_id)
+{
+	return seg->enabled &&
+		(seg->feature_mask[segment_id] & (1 << feature_id));
+}
+
+static int get_segdata(struct segmentation *seg, int segment_id,
+		       enum SEG_LVL_FEATURES feature_id)
+{
+	return seg->feature_data[segment_id][feature_id];
+}
+
+static void vp9_update_sharpness(struct loop_filter_info_n *lfi,
+				 int sharpness_lvl)
+{
+	int lvl;
+
+	/* For each possible value for the loop filter fill out limits*/
+	for (lvl = 0; lvl <= MAX_LOOP_FILTER; lvl++) {
+		/* Set loop filter parameters that control sharpness.*/
+		int block_inside_limit = lvl >> ((sharpness_lvl > 0) +
+					(sharpness_lvl > 4));
+
+		if (sharpness_lvl > 0) {
+			if (block_inside_limit > (9 - sharpness_lvl))
+				block_inside_limit = (9 - sharpness_lvl);
+		}
+
+		if (block_inside_limit < 1)
+			block_inside_limit = 1;
+
+		lfi->lfthr[lvl].lim = (u8)block_inside_limit;
+		lfi->lfthr[lvl].mblim = (u8)(2 * (lvl + 2) +
+				block_inside_limit);
+	}
+}
+
+/* Instantiate this function once when decode is started */
+static void
+vp9_loop_filter_init(struct amvdec_core *core, struct codec_vp9 *vp9)
+{
+	struct loop_filter_info_n *lfi = &vp9->lfi;
+	struct loopfilter *lf = &vp9->lf;
+	struct segmentation *seg_4lf = &vp9->seg_4lf;
+	int i;
+
+	memset(lfi, 0, sizeof(struct loop_filter_info_n));
+	memset(lf, 0, sizeof(struct loopfilter));
+	memset(seg_4lf, 0, sizeof(struct segmentation));
+	lf->sharpness_level = 0;
+	vp9_update_sharpness(lfi, lf->sharpness_level);
+	lf->last_sharpness_level = lf->sharpness_level;
+
+	for (i = 0; i < 32; i++) {
+		unsigned int thr;
+
+		thr = ((lfi->lfthr[i * 2 + 1].lim & 0x3f) << 8) |
+			(lfi->lfthr[i * 2 + 1].mblim & 0xff);
+		thr = (thr << 16) | ((lfi->lfthr[i * 2].lim & 0x3f) << 8) |
+			(lfi->lfthr[i * 2].mblim & 0xff);
+
+		amvdec_write_dos(core, HEVC_DBLK_CFG9, thr);
+	}
+
+	if (core->platform->revision >= VDEC_REVISION_SM1)
+		amvdec_write_dos(core, HEVC_DBLK_CFGB,
+				 (0x3 << 14) | /* dw fifo thres r and b */
+				 (0x3 << 12) | /* dw fifo thres r or b */
+				 (0x3 << 10) | /* dw fifo thres not r/b */
+				 BIT(0)); /* VP9 video format */
+	else if (core->platform->revision >= VDEC_REVISION_G12A)
+		/* VP9 video format */
+		amvdec_write_dos(core, HEVC_DBLK_CFGB, (0x54 << 8) | BIT(0));
+	else
+		amvdec_write_dos(core, HEVC_DBLK_CFGB, 0x40400001);
+}
+
+static void
+vp9_loop_filter_frame_init(struct amvdec_core *core, struct segmentation *seg,
+			   struct loop_filter_info_n *lfi,
+			   struct loopfilter *lf, int default_filt_lvl)
+{
+	int i;
+	int seg_id;
+
+	/*
+	 * n_shift is the multiplier for lf_deltas
+	 * the multiplier is:
+	 * - 1 for when filter_lvl is between 0 and 31
+	 * - 2 when filter_lvl is between 32 and 63
+	 */
+	const int scale = 1 << (default_filt_lvl >> 5);
+
+	/* update limits if sharpness has changed */
+	if (lf->last_sharpness_level != lf->sharpness_level) {
+		vp9_update_sharpness(lfi, lf->sharpness_level);
+		lf->last_sharpness_level = lf->sharpness_level;
+
+		/* Write to register */
+		for (i = 0; i < 32; i++) {
+			unsigned int thr;
+
+			thr = ((lfi->lfthr[i * 2 + 1].lim & 0x3f) << 8) |
+			      (lfi->lfthr[i * 2 + 1].mblim & 0xff);
+			thr = (thr << 16) |
+			      ((lfi->lfthr[i * 2].lim & 0x3f) << 8) |
+			      (lfi->lfthr[i * 2].mblim & 0xff);
+
+			amvdec_write_dos(core, HEVC_DBLK_CFG9, thr);
+		}
+	}
+
+	for (seg_id = 0; seg_id < MAX_SEGMENTS; seg_id++) {
+		int lvl_seg = default_filt_lvl;
+
+		if (segfeature_active(seg, seg_id, SEG_LVL_ALT_LF)) {
+			const int data = get_segdata(seg, seg_id,
+						SEG_LVL_ALT_LF);
+			lvl_seg = clamp_t(int,
+					  seg->abs_delta == SEGMENT_ABSDATA ?
+						data : default_filt_lvl + data,
+					  0, MAX_LOOP_FILTER);
+		}
+
+		if (!lf->mode_ref_delta_enabled) {
+			/*
+			 * We could get rid of this if we assume that deltas
+			 * are set to zero when not in use.
+			 * encoder always uses deltas
+			 */
+			memset(lfi->lvl[seg_id], lvl_seg,
+			       sizeof(lfi->lvl[seg_id]));
+		} else {
+			int ref, mode;
+			const int intra_lvl =
+				lvl_seg + lf->ref_deltas[INTRA_FRAME] * scale;
+			lfi->lvl[seg_id][INTRA_FRAME][0] =
+				clamp_val(intra_lvl, 0, MAX_LOOP_FILTER);
+
+			for (ref = LAST_FRAME; ref < MAX_REF_FRAMES; ++ref) {
+				for (mode = 0; mode < MAX_MODE_LF_DELTAS;
+				     ++mode) {
+					const int inter_lvl =
+						lvl_seg +
+						lf->ref_deltas[ref] * scale +
+						lf->mode_deltas[mode] * scale;
+					lfi->lvl[seg_id][ref][mode] =
+						clamp_val(inter_lvl, 0,
+							  MAX_LOOP_FILTER);
+				}
+			}
+		}
+	}
+
+	for (i = 0; i < 16; i++) {
+		unsigned int level;
+
+		level = ((lfi->lvl[i >> 1][3][i & 1] & 0x3f) << 24) |
+			((lfi->lvl[i >> 1][2][i & 1] & 0x3f) << 16) |
+			((lfi->lvl[i >> 1][1][i & 1] & 0x3f) << 8) |
+			(lfi->lvl[i >> 1][0][i & 1] & 0x3f);
+		if (!default_filt_lvl)
+			level = 0;
+
+		amvdec_write_dos(core, HEVC_DBLK_CFGA, level);
+	}
+}
+
+static void codec_vp9_flush_output(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+	struct vp9_frame *tmp, *n;
+
+	mutex_lock(&vp9->lock);
+	list_for_each_entry_safe(tmp, n, &vp9->ref_frames_list, list) {
+		if (!tmp->done) {
+			if (tmp->show)
+				amvdec_dst_buf_done(sess, tmp->vbuf,
+						    V4L2_FIELD_NONE);
+			else
+				v4l2_m2m_buf_queue(sess->m2m_ctx, tmp->vbuf);
+
+			vp9->frames_num--;
+		}
+
+		list_del(&tmp->list);
+		kfree(tmp);
+	}
+	mutex_unlock(&vp9->lock);
+}
+
+static u32 codec_vp9_num_pending_bufs(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+
+	if (!vp9)
+		return 0;
+
+	return vp9->frames_num;
+}
+
+static int codec_vp9_alloc_workspace(struct amvdec_core *core,
+				     struct codec_vp9 *vp9)
+{
+	/* Allocate some memory for the VP9 decoder's state */
+	vp9->workspace_vaddr = dma_alloc_coherent(core->dev, SIZE_WORKSPACE,
+						  &vp9->workspace_paddr,
+						  GFP_KERNEL);
+	if (!vp9->workspace_vaddr) {
+		dev_err(core->dev, "Failed to allocate VP9 Workspace\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void codec_vp9_setup_workspace(struct amvdec_session *sess,
+				      struct codec_vp9 *vp9)
+{
+	struct amvdec_core *core = sess->core;
+	u32 revision = core->platform->revision;
+	dma_addr_t wkaddr = vp9->workspace_paddr;
+
+	amvdec_write_dos(core, HEVCD_IPP_LINEBUFF_BASE, wkaddr + IPP_OFFSET);
+	amvdec_write_dos(core, VP9_RPM_BUFFER, wkaddr + RPM_OFFSET);
+	amvdec_write_dos(core, VP9_SHORT_TERM_RPS, wkaddr + SH_TM_RPS_OFFSET);
+	amvdec_write_dos(core, VP9_PPS_BUFFER, wkaddr + PPS_OFFSET);
+	amvdec_write_dos(core, VP9_SAO_UP, wkaddr + SAO_UP_OFFSET);
+
+	amvdec_write_dos(core, VP9_STREAM_SWAP_BUFFER,
+			 wkaddr + SWAP_BUF_OFFSET);
+	amvdec_write_dos(core, VP9_STREAM_SWAP_BUFFER2,
+			 wkaddr + SWAP_BUF2_OFFSET);
+	amvdec_write_dos(core, VP9_SCALELUT, wkaddr + SCALELUT_OFFSET);
+
+	if (core->platform->revision >= VDEC_REVISION_G12A)
+		amvdec_write_dos(core, HEVC_DBLK_CFGE,
+				 wkaddr + DBLK_PARA_OFFSET);
+
+	amvdec_write_dos(core, HEVC_DBLK_CFG4, wkaddr + DBLK_PARA_OFFSET);
+	amvdec_write_dos(core, HEVC_DBLK_CFG5, wkaddr + DBLK_DATA_OFFSET);
+	amvdec_write_dos(core, VP9_SEG_MAP_BUFFER, wkaddr + SEG_MAP_OFFSET);
+	amvdec_write_dos(core, VP9_PROB_SWAP_BUFFER, wkaddr + PROB_OFFSET);
+	amvdec_write_dos(core, VP9_COUNT_SWAP_BUFFER, wkaddr + COUNT_OFFSET);
+	amvdec_write_dos(core, LMEM_DUMP_ADR, wkaddr + LMEM_OFFSET);
+
+	if (codec_hevc_use_mmu(revision, sess->pixfmt_cap, vp9->is_10bit)) {
+		amvdec_write_dos(core, HEVC_SAO_MMU_VH0_ADDR,
+				 wkaddr + MMU_VBH_OFFSET);
+		amvdec_write_dos(core, HEVC_SAO_MMU_VH1_ADDR,
+				 wkaddr + MMU_VBH_OFFSET + (MMU_VBH_SIZE / 2));
+
+		if (revision >= VDEC_REVISION_G12A)
+			amvdec_write_dos(core, HEVC_ASSIST_MMU_MAP_ADDR,
+					 vp9->common.mmu_map_paddr);
+		else
+			amvdec_write_dos(core, VP9_MMU_MAP_BUFFER,
+					 vp9->common.mmu_map_paddr);
+	}
+}
+
+static int codec_vp9_start(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9;
+	u32 val;
+	int i;
+	int ret;
+
+	vp9 = kzalloc(sizeof(*vp9), GFP_KERNEL);
+	if (!vp9)
+		return -ENOMEM;
+
+	ret = codec_vp9_alloc_workspace(core, vp9);
+	if (ret)
+		goto free_vp9;
+
+	codec_vp9_setup_workspace(sess, vp9);
+	amvdec_write_dos_bits(core, HEVC_STREAM_CONTROL, BIT(0));
+	/* stream_fifo_hole */
+	if (core->platform->revision >= VDEC_REVISION_G12A)
+		amvdec_write_dos_bits(core, HEVC_STREAM_FIFO_CTL, BIT(29));
+
+	val = amvdec_read_dos(core, HEVC_PARSER_INT_CONTROL) & 0x7fffffff;
+	val |= (3 << 29) | BIT(24) | BIT(22) | BIT(7) | BIT(4) | BIT(0);
+	amvdec_write_dos(core, HEVC_PARSER_INT_CONTROL, val);
+	amvdec_write_dos_bits(core, HEVC_SHIFT_STATUS, BIT(0));
+	amvdec_write_dos(core, HEVC_SHIFT_CONTROL, BIT(10) | BIT(9) |
+			 (3 << 6) | BIT(5) | BIT(2) | BIT(1) | BIT(0));
+	amvdec_write_dos(core, HEVC_CABAC_CONTROL, BIT(0));
+	amvdec_write_dos(core, HEVC_PARSER_CORE_CONTROL, BIT(0));
+	amvdec_write_dos(core, HEVC_SHIFT_STARTCODE, 0x00000001);
+
+	amvdec_write_dos(core, VP9_DEC_STATUS_REG, 0);
+
+	amvdec_write_dos(core, HEVC_PARSER_CMD_WRITE, BIT(16));
+	for (i = 0; i < ARRAY_SIZE(vdec_hevc_parser_cmd); ++i)
+		amvdec_write_dos(core, HEVC_PARSER_CMD_WRITE,
+				 vdec_hevc_parser_cmd[i]);
+
+	amvdec_write_dos(core, HEVC_PARSER_CMD_SKIP_0, PARSER_CMD_SKIP_CFG_0);
+	amvdec_write_dos(core, HEVC_PARSER_CMD_SKIP_1, PARSER_CMD_SKIP_CFG_1);
+	amvdec_write_dos(core, HEVC_PARSER_CMD_SKIP_2, PARSER_CMD_SKIP_CFG_2);
+	amvdec_write_dos(core, HEVC_PARSER_IF_CONTROL,
+			 BIT(5) | BIT(2) | BIT(0));
+
+	amvdec_write_dos(core, HEVCD_IPP_TOP_CNTL, BIT(0));
+	amvdec_write_dos(core, HEVCD_IPP_TOP_CNTL, BIT(1));
+
+	amvdec_write_dos(core, VP9_WAIT_FLAG, 1);
+
+	/* clear mailbox interrupt */
+	amvdec_write_dos(core, HEVC_ASSIST_MBOX1_CLR_REG, 1);
+	/* enable mailbox interrupt */
+	amvdec_write_dos(core, HEVC_ASSIST_MBOX1_MASK, 1);
+	/* disable PSCALE for hardware sharing */
+	amvdec_write_dos(core, HEVC_PSCALE_CTRL, 0);
+	/* Let the uCode do all the parsing */
+	amvdec_write_dos(core, NAL_SEARCH_CTL, 0x8);
+
+	amvdec_write_dos(core, DECODE_STOP_POS, 0);
+	amvdec_write_dos(core, VP9_DECODE_MODE, DECODE_MODE_SINGLE);
+
+	pr_debug("decode_count: %u; decode_size: %u\n",
+		 amvdec_read_dos(core, HEVC_DECODE_COUNT),
+		 amvdec_read_dos(core, HEVC_DECODE_SIZE));
+
+	vp9_loop_filter_init(core, vp9);
+
+	INIT_LIST_HEAD(&vp9->ref_frames_list);
+	mutex_init(&vp9->lock);
+	memset(&vp9->ref_frame_map, -1, sizeof(vp9->ref_frame_map));
+	memset(&vp9->next_ref_frame_map, -1, sizeof(vp9->next_ref_frame_map));
+	for (i = 0; i < REFS_PER_FRAME; ++i)
+		vp9->frame_refs[i] = NULL;
+	sess->priv = vp9;
+
+	return 0;
+
+free_vp9:
+	kfree(vp9);
+	return ret;
+}
+
+static int codec_vp9_stop(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9 = sess->priv;
+
+	mutex_lock(&vp9->lock);
+	if (vp9->workspace_vaddr)
+		dma_free_coherent(core->dev, SIZE_WORKSPACE,
+				  vp9->workspace_vaddr,
+				  vp9->workspace_paddr);
+
+	codec_hevc_free_fbc_buffers(sess, &vp9->common);
+	mutex_unlock(&vp9->lock);
+
+	return 0;
+}
+
+/*
+ * Program LAST & GOLDEN frames into the motion compensation reference cache
+ * controller
+ */
+static void codec_vp9_set_mcrcc(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9 = sess->priv;
+	u32 val;
+
+	/* Reset mcrcc */
+	amvdec_write_dos(core, HEVCD_MCRCC_CTL1, 0x2);
+	/* Disable on I-frame */
+	if (vp9->cur_frame->type == KEY_FRAME || vp9->cur_frame->intra_only) {
+		amvdec_write_dos(core, HEVCD_MCRCC_CTL1, 0x0);
+		return;
+	}
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR, BIT(1));
+	val = amvdec_read_dos(core, HEVCD_MPP_ANC_CANVAS_DATA_ADDR) & 0xffff;
+	val |= (val << 16);
+	amvdec_write_dos(core, HEVCD_MCRCC_CTL2, val);
+	val = amvdec_read_dos(core, HEVCD_MPP_ANC_CANVAS_DATA_ADDR) & 0xffff;
+	val |= (val << 16);
+	amvdec_write_dos(core, HEVCD_MCRCC_CTL3, val);
+
+	/* Enable mcrcc progressive-mode */
+	amvdec_write_dos(core, HEVCD_MCRCC_CTL1, 0xff0);
+}
+
+static void codec_vp9_set_sao(struct amvdec_session *sess,
+			      struct vb2_buffer *vb)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9 = sess->priv;
+
+	dma_addr_t buf_y_paddr;
+	dma_addr_t buf_u_v_paddr;
+	u32 val;
+
+	if (codec_hevc_use_downsample(sess->pixfmt_cap, vp9->is_10bit))
+		buf_y_paddr =
+			vp9->common.fbc_buffer_paddr[vb->index];
+	else
+		buf_y_paddr =
+		       vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (codec_hevc_use_fbc(sess->pixfmt_cap, vp9->is_10bit)) {
+		val = amvdec_read_dos(core, HEVC_SAO_CTRL5) & ~0xff0200;
+		amvdec_write_dos(core, HEVC_SAO_CTRL5, val);
+		amvdec_write_dos(core, HEVC_CM_BODY_START_ADDR, buf_y_paddr);
+	}
+
+	if (sess->pixfmt_cap == V4L2_PIX_FMT_NV12M) {
+		buf_y_paddr =
+		       vb2_dma_contig_plane_dma_addr(vb, 0);
+		buf_u_v_paddr =
+		       vb2_dma_contig_plane_dma_addr(vb, 1);
+		amvdec_write_dos(core, HEVC_SAO_Y_START_ADDR, buf_y_paddr);
+		amvdec_write_dos(core, HEVC_SAO_C_START_ADDR, buf_u_v_paddr);
+		amvdec_write_dos(core, HEVC_SAO_Y_WPTR, buf_y_paddr);
+		amvdec_write_dos(core, HEVC_SAO_C_WPTR, buf_u_v_paddr);
+	}
+
+	if (codec_hevc_use_mmu(core->platform->revision, sess->pixfmt_cap,
+			       vp9->is_10bit)) {
+		amvdec_write_dos(core, HEVC_CM_HEADER_START_ADDR,
+				 vp9->common.mmu_header_paddr[vb->index]);
+		/* use HEVC_CM_HEADER_START_ADDR */
+		amvdec_write_dos_bits(core, HEVC_SAO_CTRL5, BIT(10));
+	}
+
+	amvdec_write_dos(core, HEVC_SAO_Y_LENGTH,
+			 amvdec_get_output_size(sess));
+	amvdec_write_dos(core, HEVC_SAO_C_LENGTH,
+			 (amvdec_get_output_size(sess) / 2));
+
+	if (core->platform->revision >= VDEC_REVISION_G12A) {
+		amvdec_clear_dos_bits(core, HEVC_DBLK_CFGB,
+				      BIT(4) | BIT(5) | BIT(8) | BIT(9));
+		/* enable first, compressed write */
+		if (codec_hevc_use_fbc(sess->pixfmt_cap, vp9->is_10bit))
+			amvdec_write_dos_bits(core, HEVC_DBLK_CFGB, BIT(8));
+
+		/* enable second, uncompressed write */
+		if (sess->pixfmt_cap == V4L2_PIX_FMT_NV12M)
+			amvdec_write_dos_bits(core, HEVC_DBLK_CFGB, BIT(9));
+
+		/* dblk pipeline mode=1 for performance */
+		if (sess->width >= 1280)
+			amvdec_write_dos_bits(core, HEVC_DBLK_CFGB, BIT(4));
+
+		pr_debug("HEVC_DBLK_CFGB: %08X\n",
+			 amvdec_read_dos(core, HEVC_DBLK_CFGB));
+	}
+
+	val = amvdec_read_dos(core, HEVC_SAO_CTRL1) & ~0x3ff0;
+	val |= 0xff0; /* Set endianness for 2-bytes swaps (nv12) */
+	if (core->platform->revision < VDEC_REVISION_G12A) {
+		val &= ~0x3;
+		if (!codec_hevc_use_fbc(sess->pixfmt_cap, vp9->is_10bit))
+			val |= BIT(0); /* disable cm compression */
+		/* TOFIX: Handle Amlogic Framebuffer compression */
+	}
+
+	amvdec_write_dos(core, HEVC_SAO_CTRL1, val);
+	pr_debug("HEVC_SAO_CTRL1: %08X\n", val);
+
+	/* no downscale for NV12 */
+	val = amvdec_read_dos(core, HEVC_SAO_CTRL5) & ~0xff0000;
+	amvdec_write_dos(core, HEVC_SAO_CTRL5, val);
+
+	val = amvdec_read_dos(core, HEVCD_IPP_AXIIF_CONFIG) & ~0x30;
+	val |= 0xf;
+	val &= ~BIT(12); /* NV12 */
+	amvdec_write_dos(core, HEVCD_IPP_AXIIF_CONFIG, val);
+}
+
+static dma_addr_t codec_vp9_get_frame_mv_paddr(struct codec_vp9 *vp9,
+					       struct vp9_frame *frame)
+{
+	return vp9->workspace_paddr + MPRED_MV_OFFSET +
+	       (frame->index * MPRED_MV_BUF_SIZE);
+}
+
+static void codec_vp9_set_mpred_mv(struct amvdec_core *core,
+				   struct codec_vp9 *vp9)
+{
+	int mpred_mv_rd_end_addr;
+	int use_prev_frame_mvs = vp9->prev_frame->width ==
+					vp9->cur_frame->width &&
+				 vp9->prev_frame->height ==
+					vp9->cur_frame->height &&
+				 !vp9->prev_frame->intra_only &&
+				 vp9->prev_frame->show &&
+				 vp9->prev_frame->type != KEY_FRAME;
+
+	amvdec_write_dos(core, HEVC_MPRED_CTRL3, 0x24122412);
+	amvdec_write_dos(core, HEVC_MPRED_ABV_START_ADDR,
+			 vp9->workspace_paddr + MPRED_ABV_OFFSET);
+
+	amvdec_clear_dos_bits(core, HEVC_MPRED_CTRL4, BIT(6));
+	if (use_prev_frame_mvs)
+		amvdec_write_dos_bits(core, HEVC_MPRED_CTRL4, BIT(6));
+
+	amvdec_write_dos(core, HEVC_MPRED_MV_WR_START_ADDR,
+			 codec_vp9_get_frame_mv_paddr(vp9, vp9->cur_frame));
+	amvdec_write_dos(core, HEVC_MPRED_MV_WPTR,
+			 codec_vp9_get_frame_mv_paddr(vp9, vp9->cur_frame));
+
+	amvdec_write_dos(core, HEVC_MPRED_MV_RD_START_ADDR,
+			 codec_vp9_get_frame_mv_paddr(vp9, vp9->prev_frame));
+	amvdec_write_dos(core, HEVC_MPRED_MV_RPTR,
+			 codec_vp9_get_frame_mv_paddr(vp9, vp9->prev_frame));
+
+	mpred_mv_rd_end_addr =
+			codec_vp9_get_frame_mv_paddr(vp9, vp9->prev_frame) +
+			(vp9->lcu_total * MV_MEM_UNIT);
+	amvdec_write_dos(core, HEVC_MPRED_MV_RD_END_ADDR, mpred_mv_rd_end_addr);
+}
+
+static void codec_vp9_update_next_ref(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	u32 buf_idx = vp9->cur_frame->index;
+	int ref_index = 0;
+	int refresh_frame_flags;
+	int mask;
+
+	refresh_frame_flags = vp9->cur_frame->type == KEY_FRAME ?
+				0xff : param->p.refresh_frame_flags;
+
+	for (mask = refresh_frame_flags; mask; mask >>= 1) {
+		pr_debug("mask=%08X; ref_index=%d\n", mask, ref_index);
+		if (mask & 1)
+			vp9->next_ref_frame_map[ref_index] = buf_idx;
+		else
+			vp9->next_ref_frame_map[ref_index] =
+				vp9->ref_frame_map[ref_index];
+
+		++ref_index;
+	}
+
+	for (; ref_index < REF_FRAMES; ++ref_index)
+		vp9->next_ref_frame_map[ref_index] =
+			vp9->ref_frame_map[ref_index];
+}
+
+static void codec_vp9_save_refs(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	int i;
+
+	for (i = 0; i < REFS_PER_FRAME; ++i) {
+		const int ref = (param->p.ref_info >>
+				 (((REFS_PER_FRAME - i - 1) * 4) + 1)) & 0x7;
+
+		if (vp9->ref_frame_map[ref] < 0)
+			continue;
+
+		pr_warn("%s: FIXME, would need to save ref %d\n",
+			__func__, vp9->ref_frame_map[ref]);
+	}
+}
+
+static void codec_vp9_update_ref(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	int ref_index = 0;
+	int mask;
+	int refresh_frame_flags;
+
+	if (!vp9->cur_frame)
+		return;
+
+	refresh_frame_flags = vp9->cur_frame->type == KEY_FRAME ?
+				0xff : param->p.refresh_frame_flags;
+
+	for (mask = refresh_frame_flags; mask; mask >>= 1) {
+		vp9->ref_frame_map[ref_index] =
+			vp9->next_ref_frame_map[ref_index];
+		++ref_index;
+	}
+
+	if (param->p.show_existing_frame)
+		return;
+
+	for (; ref_index < REF_FRAMES; ++ref_index)
+		vp9->ref_frame_map[ref_index] =
+			vp9->next_ref_frame_map[ref_index];
+}
+
+static struct vp9_frame *codec_vp9_get_frame_by_idx(struct codec_vp9 *vp9,
+						    int idx)
+{
+	struct vp9_frame *frame;
+
+	list_for_each_entry(frame, &vp9->ref_frames_list, list) {
+		if (frame->index == idx)
+			return frame;
+	}
+
+	return NULL;
+}
+
+static void codec_vp9_sync_ref(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	int i;
+
+	for (i = 0; i < REFS_PER_FRAME; ++i) {
+		const int ref = (param->p.ref_info >>
+				 (((REFS_PER_FRAME - i - 1) * 4) + 1)) & 0x7;
+		const int idx = vp9->ref_frame_map[ref];
+
+		vp9->frame_refs[i] = codec_vp9_get_frame_by_idx(vp9, idx);
+		if (!vp9->frame_refs[i])
+			pr_warn("%s: couldn't find VP9 ref %d\n", __func__,
+				idx);
+	}
+}
+
+static void codec_vp9_set_refs(struct amvdec_session *sess,
+			       struct codec_vp9 *vp9)
+{
+	struct amvdec_core *core = sess->core;
+	int i;
+
+	for (i = 0; i < REFS_PER_FRAME; ++i) {
+		struct vp9_frame *frame = vp9->frame_refs[i];
+		int id_y;
+		int id_u_v;
+
+		if (!frame)
+			continue;
+
+		if (codec_hevc_use_fbc(sess->pixfmt_cap, vp9->is_10bit)) {
+			id_y = frame->index;
+			id_u_v = id_y;
+		} else {
+			id_y = frame->index * 2;
+			id_u_v = id_y + 1;
+		}
+
+		amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_DATA_ADDR,
+				 (id_u_v << 16) | (id_u_v << 8) | id_y);
+	}
+}
+
+static void codec_vp9_set_mc(struct amvdec_session *sess,
+			     struct codec_vp9 *vp9)
+{
+	struct amvdec_core *core = sess->core;
+	u32 scale = 0;
+	u32 sz;
+	int i;
+
+	amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR, 1);
+	codec_vp9_set_refs(sess, vp9);
+	amvdec_write_dos(core, HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR,
+			 (16 << 8) | 1);
+	codec_vp9_set_refs(sess, vp9);
+
+	amvdec_write_dos(core, VP9D_MPP_REFINFO_TBL_ACCCONFIG, BIT(2));
+	for (i = 0; i < REFS_PER_FRAME; ++i) {
+		if (!vp9->frame_refs[i])
+			continue;
+
+		if (vp9->frame_refs[i]->width != vp9->width ||
+		    vp9->frame_refs[i]->height != vp9->height)
+			scale = 1;
+
+		sz = amvdec_am21c_body_size(vp9->frame_refs[i]->width,
+					    vp9->frame_refs[i]->height);
+
+		amvdec_write_dos(core, VP9D_MPP_REFINFO_DATA,
+				 vp9->frame_refs[i]->width);
+		amvdec_write_dos(core, VP9D_MPP_REFINFO_DATA,
+				 vp9->frame_refs[i]->height);
+		amvdec_write_dos(core, VP9D_MPP_REFINFO_DATA,
+				 (vp9->frame_refs[i]->width << 14) /
+				 vp9->width);
+		amvdec_write_dos(core, VP9D_MPP_REFINFO_DATA,
+				 (vp9->frame_refs[i]->height << 14) /
+				 vp9->height);
+		amvdec_write_dos(core, VP9D_MPP_REFINFO_DATA, sz >> 5);
+	}
+
+	amvdec_write_dos(core, VP9D_MPP_REF_SCALE_ENBL, scale);
+}
+
+static struct vp9_frame *codec_vp9_get_new_frame(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+	union rpm_param *param = &vp9->rpm_param;
+	struct vb2_v4l2_buffer *vbuf;
+	struct vp9_frame *new_frame;
+
+	new_frame = kzalloc(sizeof(*new_frame), GFP_KERNEL);
+	if (!new_frame)
+		return NULL;
+
+	vbuf = v4l2_m2m_dst_buf_remove(sess->m2m_ctx);
+	if (!vbuf) {
+		dev_err(sess->core->dev, "No dst buffer available\n");
+		kfree(new_frame);
+		return NULL;
+	}
+
+	while (codec_vp9_get_frame_by_idx(vp9, vbuf->vb2_buf.index)) {
+		struct vb2_v4l2_buffer *old_vbuf = vbuf;
+
+		vbuf = v4l2_m2m_dst_buf_remove(sess->m2m_ctx);
+		v4l2_m2m_buf_queue(sess->m2m_ctx, old_vbuf);
+		if (!vbuf) {
+			dev_err(sess->core->dev, "No dst buffer available\n");
+			kfree(new_frame);
+			return NULL;
+		}
+	}
+
+	new_frame->vbuf = vbuf;
+	new_frame->index = vbuf->vb2_buf.index;
+	new_frame->intra_only = param->p.intra_only;
+	new_frame->show = param->p.show_frame;
+	new_frame->type = param->p.frame_type;
+	new_frame->width = vp9->width;
+	new_frame->height = vp9->height;
+	list_add_tail(&new_frame->list, &vp9->ref_frames_list);
+	vp9->frames_num++;
+
+	return new_frame;
+}
+
+static void codec_vp9_show_existing_frame(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+
+	if (!param->p.show_existing_frame)
+		return;
+
+	pr_debug("showing frame %u\n", param->p.frame_to_show_idx);
+}
+
+static void codec_vp9_rm_noshow_frame(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+	struct vp9_frame *tmp;
+
+	list_for_each_entry(tmp, &vp9->ref_frames_list, list) {
+		if (tmp->show)
+			continue;
+
+		pr_debug("rm noshow: %u\n", tmp->index);
+		v4l2_m2m_buf_queue(sess->m2m_ctx, tmp->vbuf);
+		list_del(&tmp->list);
+		kfree(tmp);
+		vp9->frames_num--;
+		return;
+	}
+}
+
+static void codec_vp9_process_frame(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9 = sess->priv;
+	union rpm_param *param = &vp9->rpm_param;
+	int intra_only;
+
+	if (!param->p.show_frame)
+		codec_vp9_rm_noshow_frame(sess);
+
+	vp9->cur_frame = codec_vp9_get_new_frame(sess);
+	if (!vp9->cur_frame)
+		return;
+
+	pr_debug("frame %d: type: %08X; show_exist: %u; show: %u, intra_only: %u\n",
+		 vp9->cur_frame->index,
+		 param->p.frame_type, param->p.show_existing_frame,
+		 param->p.show_frame, param->p.intra_only);
+
+	if (param->p.frame_type != KEY_FRAME)
+		codec_vp9_sync_ref(vp9);
+	codec_vp9_update_next_ref(vp9);
+	codec_vp9_show_existing_frame(vp9);
+
+	if (codec_hevc_use_mmu(core->platform->revision, sess->pixfmt_cap,
+			       vp9->is_10bit))
+		codec_hevc_fill_mmu_map(sess, &vp9->common,
+					&vp9->cur_frame->vbuf->vb2_buf);
+
+	intra_only = param->p.show_frame ? 0 : param->p.intra_only;
+
+	/* clear mpred (for keyframe only) */
+	if (param->p.frame_type != KEY_FRAME && !intra_only) {
+		codec_vp9_set_mc(sess, vp9);
+		codec_vp9_set_mpred_mv(core, vp9);
+	} else {
+		amvdec_clear_dos_bits(core, HEVC_MPRED_CTRL4, BIT(6));
+	}
+
+	amvdec_write_dos(core, HEVC_PARSER_PICTURE_SIZE,
+			 (vp9->height << 16) | vp9->width);
+	codec_vp9_set_mcrcc(sess);
+	codec_vp9_set_sao(sess, &vp9->cur_frame->vbuf->vb2_buf);
+
+	vp9_loop_filter_frame_init(core, &vp9->seg_4lf,
+				   &vp9->lfi, &vp9->lf,
+				   vp9->default_filt_lvl);
+
+	/* ask uCode to start decoding */
+	amvdec_write_dos(core, VP9_DEC_STATUS_REG, VP9_10B_DECODE_SLICE);
+}
+
+static void codec_vp9_process_lf(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	int i;
+
+	vp9->lf.mode_ref_delta_enabled = param->p.mode_ref_delta_enabled;
+	vp9->lf.sharpness_level = param->p.sharpness_level;
+	vp9->default_filt_lvl = param->p.filter_level;
+	vp9->seg_4lf.enabled = param->p.seg_enabled;
+	vp9->seg_4lf.abs_delta = param->p.seg_abs_delta;
+
+	for (i = 0; i < 4; i++)
+		vp9->lf.ref_deltas[i] = param->p.ref_deltas[i];
+
+	for (i = 0; i < 2; i++)
+		vp9->lf.mode_deltas[i] = param->p.mode_deltas[i];
+
+	for (i = 0; i < MAX_SEGMENTS; i++)
+		vp9->seg_4lf.feature_mask[i] =
+			(param->p.seg_lf_info[i] & 0x8000) ?
+				(1 << SEG_LVL_ALT_LF) : 0;
+
+	for (i = 0; i < MAX_SEGMENTS; i++)
+		vp9->seg_4lf.feature_data[i][SEG_LVL_ALT_LF] =
+			(param->p.seg_lf_info[i] & 0x100) ?
+				-(param->p.seg_lf_info[i] & 0x3f)
+				: (param->p.seg_lf_info[i] & 0x3f);
+}
+
+static void codec_vp9_resume(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+
+	mutex_lock(&vp9->lock);
+	if (codec_hevc_setup_buffers(sess, &vp9->common, vp9->is_10bit)) {
+		mutex_unlock(&vp9->lock);
+		amvdec_abort(sess);
+		return;
+	}
+
+	codec_vp9_setup_workspace(sess, vp9);
+	codec_hevc_setup_decode_head(sess, vp9->is_10bit);
+	codec_vp9_process_lf(vp9);
+	codec_vp9_process_frame(sess);
+
+	mutex_unlock(&vp9->lock);
+}
+
+/*
+ * The RPM section within the workspace contains
+ * many information regarding the parsed bitstream
+ */
+static void codec_vp9_fetch_rpm(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+	u16 *rpm_vaddr = vp9->workspace_vaddr + RPM_OFFSET;
+	int i, j;
+
+	for (i = 0; i < RPM_BUF_SIZE; i += 4)
+		for (j = 0; j < 4; j++)
+			vp9->rpm_param.l.data[i + j] = rpm_vaddr[i + 3 - j];
+}
+
+static int codec_vp9_process_rpm(struct codec_vp9 *vp9)
+{
+	union rpm_param *param = &vp9->rpm_param;
+	int src_changed = 0;
+	int is_10bit = 0;
+	int pic_width_64 = ALIGN(param->p.width, 64);
+	int pic_height_32 = ALIGN(param->p.height, 32);
+	int pic_width_lcu  = (pic_width_64 % LCU_SIZE) ?
+				pic_width_64 / LCU_SIZE  + 1
+				: pic_width_64 / LCU_SIZE;
+	int pic_height_lcu = (pic_height_32 % LCU_SIZE) ?
+				pic_height_32 / LCU_SIZE + 1
+				: pic_height_32 / LCU_SIZE;
+	vp9->lcu_total = pic_width_lcu * pic_height_lcu;
+
+	if (param->p.bit_depth == 10)
+		is_10bit = 1;
+
+	if (vp9->width != param->p.width || vp9->height != param->p.height ||
+	    vp9->is_10bit != is_10bit)
+		src_changed = 1;
+
+	vp9->width = param->p.width;
+	vp9->height = param->p.height;
+	vp9->is_10bit = is_10bit;
+
+	pr_debug("width: %u; height: %u; is_10bit: %d; src_changed: %d\n",
+		 vp9->width, vp9->height, is_10bit, src_changed);
+
+	return src_changed;
+}
+
+static bool codec_vp9_is_ref(struct codec_vp9 *vp9, struct vp9_frame *frame)
+{
+	int i;
+
+	for (i = 0; i < REF_FRAMES; ++i)
+		if (vp9->ref_frame_map[i] == frame->index)
+			return true;
+
+	return false;
+}
+
+static void codec_vp9_show_frame(struct amvdec_session *sess)
+{
+	struct codec_vp9 *vp9 = sess->priv;
+	struct vp9_frame *tmp, *n;
+
+	list_for_each_entry_safe(tmp, n, &vp9->ref_frames_list, list) {
+		if (!tmp->show || tmp == vp9->cur_frame)
+			continue;
+
+		if (!tmp->done) {
+			pr_debug("Doning %u\n", tmp->index);
+			amvdec_dst_buf_done(sess, tmp->vbuf, V4L2_FIELD_NONE);
+			tmp->done = 1;
+			vp9->frames_num--;
+		}
+
+		if (codec_vp9_is_ref(vp9, tmp) || tmp == vp9->prev_frame)
+			continue;
+
+		pr_debug("deleting %d\n", tmp->index);
+		list_del(&tmp->list);
+		kfree(tmp);
+	}
+}
+
+static void vp9_tree_merge_probs(unsigned int *prev_prob,
+				 unsigned int *cur_prob,
+				 int coef_node_start, int tree_left,
+				 int tree_right,
+				 int tree_i, int node)
+{
+	int prob_32, prob_res, prob_shift;
+	int pre_prob, new_prob;
+	int den, m_count, get_prob, factor;
+
+	prob_32 = prev_prob[coef_node_start / 4 * 2];
+	prob_res = coef_node_start & 3;
+	prob_shift = prob_res * 8;
+	pre_prob = (prob_32 >> prob_shift) & 0xff;
+
+	den = tree_left + tree_right;
+
+	if (den == 0) {
+		new_prob = pre_prob;
+	} else {
+		m_count = den < MODE_MV_COUNT_SAT ? den : MODE_MV_COUNT_SAT;
+		get_prob =
+			clip_prob(div_r32(((int64_t)tree_left * 256 +
+					   (den >> 1)),
+					  den));
+
+		/* weighted_prob */
+		factor = count_to_update_factor[m_count];
+		new_prob = round_power_of_two(pre_prob * (256 - factor) +
+					      get_prob * factor, 8);
+	}
+
+	cur_prob[coef_node_start / 4 * 2] =
+		(cur_prob[coef_node_start / 4 * 2] & (~(0xff << prob_shift))) |
+		(new_prob << prob_shift);
+}
+
+static void adapt_coef_probs_cxt(unsigned int *prev_prob,
+				 unsigned int *cur_prob,
+				 unsigned int *count,
+				 int update_factor,
+				 int cxt_num,
+				 int coef_cxt_start,
+				 int coef_count_cxt_start)
+{
+	int prob_32, prob_res, prob_shift;
+	int pre_prob, new_prob;
+	int num, den, m_count, get_prob, factor;
+	int node, coef_node_start;
+	int count_sat = 24;
+	int cxt;
+
+	for (cxt = 0; cxt < cxt_num; cxt++) {
+		const int n0 = count[coef_count_cxt_start];
+		const int n1 = count[coef_count_cxt_start + 1];
+		const int n2 = count[coef_count_cxt_start + 2];
+		const int neob = count[coef_count_cxt_start + 3];
+		const int nneob = count[coef_count_cxt_start + 4];
+		const unsigned int branch_ct[3][2] = {
+			{ neob, nneob },
+			{ n0, n1 + n2 },
+			{ n1, n2 }
+		};
+
+		coef_node_start = coef_cxt_start;
+		for (node = 0 ; node < 3 ; node++) {
+			prob_32 = prev_prob[coef_node_start / 4 * 2];
+			prob_res = coef_node_start & 3;
+			prob_shift = prob_res * 8;
+			pre_prob = (prob_32 >> prob_shift) & 0xff;
+
+			/* get binary prob */
+			num = branch_ct[node][0];
+			den = branch_ct[node][0] + branch_ct[node][1];
+			m_count = den < count_sat ? den : count_sat;
+
+			get_prob = (den == 0) ?
+					128u :
+					clip_prob(div_r32(((int64_t)num * 256 +
+							  (den >> 1)), den));
+
+			factor = update_factor * m_count / count_sat;
+			new_prob =
+				round_power_of_two(pre_prob * (256 - factor) +
+						   get_prob * factor, 8);
+
+			cur_prob[coef_node_start / 4 * 2] =
+				(cur_prob[coef_node_start / 4 * 2] &
+				 (~(0xff << prob_shift))) |
+				(new_prob << prob_shift);
+
+			coef_node_start += 1;
+		}
+
+		coef_cxt_start = coef_cxt_start + 3;
+		coef_count_cxt_start = coef_count_cxt_start + 5;
+	}
+}
+
+static void adapt_coef_probs(int prev_kf, int cur_kf, int pre_fc,
+			     unsigned int *prev_prob, unsigned int *cur_prob,
+			     unsigned int *count)
+{
+	int tx_size, coef_tx_size_start, coef_count_tx_size_start;
+	int plane, coef_plane_start, coef_count_plane_start;
+	int type, coef_type_start, coef_count_type_start;
+	int band, coef_band_start, coef_count_band_start;
+	int cxt_num;
+	int coef_cxt_start, coef_count_cxt_start;
+	int node, coef_node_start, coef_count_node_start;
+
+	int tree_i, tree_left, tree_right;
+	int mvd_i;
+
+	int update_factor = cur_kf ? 112 : (prev_kf ? 128 : 112);
+
+	int prob_32;
+	int prob_res;
+	int prob_shift;
+	int pre_prob;
+
+	int den;
+	int get_prob;
+	int m_count;
+	int factor;
+
+	int new_prob;
+
+	for (tx_size = 0 ; tx_size < 4 ; tx_size++) {
+		coef_tx_size_start = VP9_COEF_START +
+				tx_size * 4 * VP9_COEF_SIZE_ONE_SET;
+		coef_count_tx_size_start = VP9_COEF_COUNT_START +
+				tx_size * 4 * VP9_COEF_COUNT_SIZE_ONE_SET;
+		coef_plane_start = coef_tx_size_start;
+		coef_count_plane_start = coef_count_tx_size_start;
+
+		for (plane = 0 ; plane < 2 ; plane++) {
+			coef_type_start = coef_plane_start;
+			coef_count_type_start = coef_count_plane_start;
+
+			for (type = 0 ; type < 2 ; type++) {
+				coef_band_start = coef_type_start;
+				coef_count_band_start = coef_count_type_start;
+
+				for (band = 0 ; band < 6 ; band++) {
+					if (band == 0)
+						cxt_num = 3;
+					else
+						cxt_num = 6;
+					coef_cxt_start = coef_band_start;
+					coef_count_cxt_start =
+						coef_count_band_start;
+
+					adapt_coef_probs_cxt(prev_prob,
+							     cur_prob,
+							     count,
+							     update_factor,
+							     cxt_num,
+							     coef_cxt_start,
+							coef_count_cxt_start);
+
+					if (band == 0) {
+						coef_band_start += 10;
+						coef_count_band_start += 15;
+					} else {
+						coef_band_start += 18;
+						coef_count_band_start += 30;
+					}
+				}
+				coef_type_start += VP9_COEF_SIZE_ONE_SET;
+				coef_count_type_start +=
+					VP9_COEF_COUNT_SIZE_ONE_SET;
+			}
+
+			coef_plane_start += 2 * VP9_COEF_SIZE_ONE_SET;
+			coef_count_plane_start +=
+				2 * VP9_COEF_COUNT_SIZE_ONE_SET;
+		}
+	}
+
+	if (cur_kf == 0) {
+		/* mode_mv_merge_probs - merge_intra_inter_prob */
+		for (coef_count_node_start = VP9_INTRA_INTER_COUNT_START;
+		     coef_count_node_start < (VP9_MV_CLASS0_HP_1_COUNT_START +
+					      VP9_MV_CLASS0_HP_1_COUNT_SIZE);
+		     coef_count_node_start += 2) {
+			if (coef_count_node_start ==
+					VP9_INTRA_INTER_COUNT_START)
+				coef_node_start = VP9_INTRA_INTER_START;
+			else if (coef_count_node_start ==
+					VP9_COMP_INTER_COUNT_START)
+				coef_node_start = VP9_COMP_INTER_START;
+			else if (coef_count_node_start ==
+					VP9_TX_MODE_COUNT_START)
+				coef_node_start = VP9_TX_MODE_START;
+			else if (coef_count_node_start ==
+					VP9_SKIP_COUNT_START)
+				coef_node_start = VP9_SKIP_START;
+			else if (coef_count_node_start ==
+					VP9_MV_SIGN_0_COUNT_START)
+				coef_node_start = VP9_MV_SIGN_0_START;
+			else if (coef_count_node_start ==
+					VP9_MV_SIGN_1_COUNT_START)
+				coef_node_start = VP9_MV_SIGN_1_START;
+			else if (coef_count_node_start ==
+					VP9_MV_BITS_0_COUNT_START)
+				coef_node_start = VP9_MV_BITS_0_START;
+			else if (coef_count_node_start ==
+					VP9_MV_BITS_1_COUNT_START)
+				coef_node_start = VP9_MV_BITS_1_START;
+			else if (coef_count_node_start ==
+					VP9_MV_CLASS0_HP_0_COUNT_START)
+				coef_node_start = VP9_MV_CLASS0_HP_0_START;
+
+			den = count[coef_count_node_start] +
+			      count[coef_count_node_start + 1];
+
+			prob_32 = prev_prob[coef_node_start / 4 * 2];
+			prob_res = coef_node_start & 3;
+			prob_shift = prob_res * 8;
+			pre_prob = (prob_32 >> prob_shift) & 0xff;
+
+			if (den == 0) {
+				new_prob = pre_prob;
+			} else {
+				m_count = den < MODE_MV_COUNT_SAT ?
+						den : MODE_MV_COUNT_SAT;
+				get_prob =
+				clip_prob(div_r32(((int64_t)
+					count[coef_count_node_start] * 256 +
+					(den >> 1)),
+					den));
+
+				/* weighted prob */
+				factor = count_to_update_factor[m_count];
+				new_prob =
+					round_power_of_two(pre_prob *
+							   (256 - factor) +
+							   get_prob * factor,
+							   8);
+			}
+
+			cur_prob[coef_node_start / 4 * 2] =
+				(cur_prob[coef_node_start / 4 * 2] &
+				 (~(0xff << prob_shift))) |
+				(new_prob << prob_shift);
+
+			coef_node_start = coef_node_start + 1;
+		}
+
+		coef_node_start = VP9_INTER_MODE_START;
+		coef_count_node_start = VP9_INTER_MODE_COUNT_START;
+		for (tree_i = 0 ; tree_i < 7 ; tree_i++) {
+			for (node = 0 ; node < 3 ; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 2:
+					tree_left = count[start + 1];
+					tree_right = count[start + 3];
+					break;
+				case 1:
+					tree_left = count[start + 0];
+					tree_right = count[start + 1] +
+						     count[start + 3];
+					break;
+				default:
+					tree_left = count[start + 2];
+					tree_right = count[start + 0] +
+						     count[start + 1] +
+						     count[start + 3];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+
+			coef_count_node_start = coef_count_node_start + 4;
+		}
+
+		coef_node_start = VP9_IF_Y_MODE_START;
+		coef_count_node_start = VP9_IF_Y_MODE_COUNT_START;
+		for (tree_i = 0 ; tree_i < 14 ; tree_i++) {
+			for (node = 0 ; node < 9 ; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 8:
+					tree_left =
+						count[start + D153_PRED];
+					tree_right =
+						count[start + D207_PRED];
+					break;
+				case 7:
+					tree_left =
+						count[start + D63_PRED];
+					tree_right =
+						count[start + D207_PRED] +
+						count[start + D153_PRED];
+					break;
+				case 6:
+					tree_left =
+						count[start + D45_PRED];
+					tree_right =
+						count[start + D207_PRED] +
+						count[start + D153_PRED] +
+						count[start + D63_PRED];
+					break;
+				case 5:
+					tree_left =
+						count[start + D135_PRED];
+					tree_right =
+						count[start + D117_PRED];
+					break;
+				case 4:
+					tree_left =
+						count[start + H_PRED];
+					tree_right =
+						count[start + D117_PRED] +
+						count[start + D135_PRED];
+					break;
+				case 3:
+					tree_left =
+						count[start + H_PRED] +
+						count[start + D117_PRED] +
+						count[start + D135_PRED];
+					tree_right =
+						count[start + D45_PRED] +
+						count[start + D207_PRED] +
+						count[start + D153_PRED] +
+						count[start + D63_PRED];
+					break;
+				case 2:
+					tree_left =
+						count[start + V_PRED];
+					tree_right =
+						count[start + H_PRED] +
+						count[start + D117_PRED] +
+						count[start + D135_PRED] +
+						count[start + D45_PRED] +
+						count[start + D207_PRED] +
+						count[start + D153_PRED] +
+						count[start + D63_PRED];
+					break;
+				case 1:
+					tree_left =
+						count[start + TM_PRED];
+					tree_right =
+						count[start + V_PRED] +
+						count[start + H_PRED] +
+						count[start + D117_PRED] +
+						count[start + D135_PRED] +
+						count[start + D45_PRED] +
+						count[start + D207_PRED] +
+						count[start + D153_PRED] +
+						count[start + D63_PRED];
+					break;
+				default:
+					tree_left =
+						count[start + DC_PRED];
+					tree_right =
+						count[start + TM_PRED] +
+						count[start + V_PRED] +
+						count[start + H_PRED] +
+						count[start + D117_PRED] +
+						count[start + D135_PRED] +
+						count[start + D45_PRED] +
+						count[start + D207_PRED] +
+						count[start + D153_PRED] +
+						count[start + D63_PRED];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+			coef_count_node_start = coef_count_node_start + 10;
+		}
+
+		coef_node_start = VP9_PARTITION_P_START;
+		coef_count_node_start = VP9_PARTITION_P_COUNT_START;
+		for (tree_i = 0 ; tree_i < 16 ; tree_i++) {
+			for (node = 0 ; node < 3 ; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 2:
+					tree_left = count[start + 2];
+					tree_right = count[start + 3];
+					break;
+				case 1:
+					tree_left = count[start + 1];
+					tree_right = count[start + 2] +
+						     count[start + 3];
+					break;
+				default:
+					tree_left = count[start + 0];
+					tree_right = count[start + 1] +
+						     count[start + 2] +
+						     count[start + 3];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+
+			coef_count_node_start = coef_count_node_start + 4;
+		}
+
+		coef_node_start = VP9_INTERP_START;
+		coef_count_node_start = VP9_INTERP_COUNT_START;
+		for (tree_i = 0 ; tree_i < 4 ; tree_i++) {
+			for (node = 0 ; node < 2 ; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 1:
+					tree_left = count[start + 1];
+					tree_right = count[start + 2];
+					break;
+				default:
+					tree_left = count[start + 0];
+					tree_right = count[start + 1] +
+						     count[start + 2];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+			coef_count_node_start = coef_count_node_start + 3;
+		}
+
+		coef_node_start = VP9_MV_JOINTS_START;
+		coef_count_node_start = VP9_MV_JOINTS_COUNT_START;
+		for (tree_i = 0 ; tree_i < 1 ; tree_i++) {
+			for (node = 0 ; node < 3 ; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 2:
+					tree_left = count[start + 2];
+					tree_right = count[start + 3];
+					break;
+				case 1:
+					tree_left = count[start + 1];
+					tree_right = count[start + 2] +
+						     count[start + 3];
+					break;
+				default:
+					tree_left = count[start + 0];
+					tree_right = count[start + 1] +
+						     count[start + 2] +
+						     count[start + 3];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+			coef_count_node_start = coef_count_node_start + 4;
+		}
+
+		for (mvd_i = 0 ; mvd_i < 2 ; mvd_i++) {
+			coef_node_start = mvd_i ? VP9_MV_CLASSES_1_START :
+						  VP9_MV_CLASSES_0_START;
+			coef_count_node_start = mvd_i ?
+					VP9_MV_CLASSES_1_COUNT_START :
+					VP9_MV_CLASSES_0_COUNT_START;
+			tree_i = 0;
+			for (node = 0; node < 10; node++) {
+				unsigned int start = coef_count_node_start;
+
+				switch (node) {
+				case 9:
+					tree_left = count[start + 9];
+					tree_right = count[start + 10];
+					break;
+				case 8:
+					tree_left = count[start + 7];
+					tree_right = count[start + 8];
+					break;
+				case 7:
+					tree_left = count[start + 7] +
+						     count[start + 8];
+					tree_right = count[start + 9] +
+						     count[start + 10];
+					break;
+				case 6:
+					tree_left = count[start + 6];
+					tree_right = count[start + 7] +
+						     count[start + 8] +
+						     count[start + 9] +
+						     count[start + 10];
+					break;
+				case 5:
+					tree_left = count[start + 4];
+					tree_right = count[start + 5];
+					break;
+				case 4:
+					tree_left = count[start + 4] +
+						    count[start + 5];
+					tree_right = count[start + 6] +
+						     count[start + 7] +
+						     count[start + 8] +
+						     count[start + 9] +
+						     count[start + 10];
+					break;
+				case 3:
+					tree_left = count[start + 2];
+					tree_right = count[start + 3];
+					break;
+				case 2:
+					tree_left = count[start + 2] +
+						    count[start + 3];
+					tree_right = count[start + 4] +
+						     count[start + 5] +
+						     count[start + 6] +
+						     count[start + 7] +
+						     count[start + 8] +
+						     count[start + 9] +
+						     count[start + 10];
+					break;
+				case 1:
+					tree_left = count[start + 1];
+					tree_right = count[start + 2] +
+						     count[start + 3] +
+						     count[start + 4] +
+						     count[start + 5] +
+						     count[start + 6] +
+						     count[start + 7] +
+						     count[start + 8] +
+						     count[start + 9] +
+						     count[start + 10];
+					break;
+				default:
+					tree_left = count[start + 0];
+					tree_right = count[start + 1] +
+						     count[start + 2] +
+						     count[start + 3] +
+						     count[start + 4] +
+						     count[start + 5] +
+						     count[start + 6] +
+						     count[start + 7] +
+						     count[start + 8] +
+						     count[start + 9] +
+						     count[start + 10];
+					break;
+				}
+
+				vp9_tree_merge_probs(prev_prob, cur_prob,
+						     coef_node_start,
+						     tree_left, tree_right,
+						     tree_i, node);
+
+				coef_node_start = coef_node_start + 1;
+			}
+
+			coef_node_start = mvd_i ? VP9_MV_CLASS0_1_START :
+						  VP9_MV_CLASS0_0_START;
+			coef_count_node_start =	mvd_i ?
+						VP9_MV_CLASS0_1_COUNT_START :
+						VP9_MV_CLASS0_0_COUNT_START;
+			tree_i = 0;
+			node = 0;
+			tree_left = count[coef_count_node_start + 0];
+			tree_right = count[coef_count_node_start + 1];
+
+			vp9_tree_merge_probs(prev_prob, cur_prob,
+					     coef_node_start,
+					     tree_left, tree_right,
+					     tree_i, node);
+			coef_node_start = mvd_i ? VP9_MV_CLASS0_FP_1_START :
+						  VP9_MV_CLASS0_FP_0_START;
+			coef_count_node_start =	mvd_i ?
+					VP9_MV_CLASS0_FP_1_COUNT_START :
+					VP9_MV_CLASS0_FP_0_COUNT_START;
+
+			for (tree_i = 0; tree_i < 3; tree_i++) {
+				for (node = 0; node < 3; node++) {
+					unsigned int start =
+						coef_count_node_start;
+					switch (node) {
+					case 2:
+						tree_left = count[start + 2];
+						tree_right = count[start + 3];
+						break;
+					case 1:
+						tree_left = count[start + 1];
+						tree_right = count[start + 2] +
+							     count[start + 3];
+						break;
+					default:
+						tree_left = count[start + 0];
+						tree_right = count[start + 1] +
+							     count[start + 2] +
+							     count[start + 3];
+						break;
+					}
+
+					vp9_tree_merge_probs(prev_prob,
+							     cur_prob,
+							     coef_node_start,
+							     tree_left,
+							     tree_right,
+							     tree_i, node);
+
+					coef_node_start = coef_node_start + 1;
+				}
+				coef_count_node_start =
+					coef_count_node_start + 4;
+			}
+		}
+	}
+}
+
+static irqreturn_t codec_vp9_threaded_isr(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct codec_vp9 *vp9 = sess->priv;
+	u32 dec_status = amvdec_read_dos(core, VP9_DEC_STATUS_REG);
+	u32 prob_status = amvdec_read_dos(core, VP9_ADAPT_PROB_REG);
+	int i;
+
+	if (!vp9)
+		return IRQ_HANDLED;
+
+	mutex_lock(&vp9->lock);
+	if (dec_status != VP9_HEAD_PARSER_DONE) {
+		dev_err(core->dev_dec, "Unrecognized dec_status: %08X\n",
+			dec_status);
+		amvdec_abort(sess);
+		goto unlock;
+	}
+
+	pr_debug("ISR: %08X;%08X\n", dec_status, prob_status);
+	sess->keyframe_found = 1;
+
+	if ((prob_status & 0xff) == 0xfd && vp9->cur_frame) {
+		/* VP9_REQ_ADAPT_PROB */
+		u8 *prev_prob_b = ((u8 *)vp9->workspace_vaddr +
+					 PROB_OFFSET) +
+					((prob_status >> 8) * 0x1000);
+		u8 *cur_prob_b = ((u8 *)vp9->workspace_vaddr +
+					 PROB_OFFSET) + 0x4000;
+		u8 *count_b = (u8 *)vp9->workspace_vaddr +
+				   COUNT_OFFSET;
+		int last_frame_type = vp9->prev_frame ?
+						vp9->prev_frame->type :
+						KEY_FRAME;
+
+		adapt_coef_probs(last_frame_type == KEY_FRAME,
+				 vp9->cur_frame->type == KEY_FRAME ? 1 : 0,
+				 prob_status >> 8,
+				 (unsigned int *)prev_prob_b,
+				 (unsigned int *)cur_prob_b,
+				 (unsigned int *)count_b);
+
+		memcpy(prev_prob_b, cur_prob_b, ADAPT_PROB_SIZE);
+		amvdec_write_dos(core, VP9_ADAPT_PROB_REG, 0);
+	}
+
+	/* Invalidate first 3 refs */
+	for (i = 0; i < REFS_PER_FRAME ; ++i)
+		vp9->frame_refs[i] = NULL;
+
+	vp9->prev_frame = vp9->cur_frame;
+	codec_vp9_update_ref(vp9);
+
+	codec_vp9_fetch_rpm(sess);
+	if (codec_vp9_process_rpm(vp9)) {
+		amvdec_src_change(sess, vp9->width, vp9->height, 16);
+
+		/* No frame is actually processed */
+		vp9->cur_frame = NULL;
+
+		/* Show the remaining frame */
+		codec_vp9_show_frame(sess);
+
+		/* FIXME: Save refs for resized frame */
+		if (vp9->frames_num)
+			codec_vp9_save_refs(vp9);
+
+		goto unlock;
+	}
+
+	codec_vp9_process_lf(vp9);
+	codec_vp9_process_frame(sess);
+	codec_vp9_show_frame(sess);
+
+unlock:
+	mutex_unlock(&vp9->lock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t codec_vp9_isr(struct amvdec_session *sess)
+{
+	return IRQ_WAKE_THREAD;
+}
+
+struct amvdec_codec_ops codec_vp9_ops = {
+	.start = codec_vp9_start,
+	.stop = codec_vp9_stop,
+	.isr = codec_vp9_isr,
+	.threaded_isr = codec_vp9_threaded_isr,
+	.num_pending_bufs = codec_vp9_num_pending_bufs,
+	.drain = codec_vp9_flush_output,
+	.resume = codec_vp9_resume,
+};
diff --git a/drivers/staging/media/meson/vdec/codec_vp9.h b/drivers/staging/media/meson/vdec/codec_vp9.h
new file mode 100644
index 000000000..62db65a2b
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/codec_vp9.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 Maxime Jourdan <maxi.jourdan@wanadoo.fr>
+ */
+
+#ifndef __MESON_VDEC_CODEC_VP9_H_
+#define __MESON_VDEC_CODEC_VP9_H_
+
+#include "vdec.h"
+
+extern struct amvdec_codec_ops codec_vp9_ops;
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/dos_regs.h b/drivers/staging/media/meson/vdec/dos_regs.h
new file mode 100644
index 000000000..abd810542
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/dos_regs.h
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_DOS_REGS_H_
+#define __MESON_VDEC_DOS_REGS_H_
+
+/* DOS registers */
+#define VDEC_ASSIST_AMR1_INT8	0x00b4
+
+#define ASSIST_MBOX1_CLR_REG	0x01d4
+#define ASSIST_MBOX1_MASK	0x01d8
+
+#define MPSR			0x0c04
+#define MCPU_INTR_MSK		0x0c10
+#define CPSR			0x0c84
+
+#define IMEM_DMA_CTRL		0x0d00
+#define IMEM_DMA_ADR		0x0d04
+#define IMEM_DMA_COUNT		0x0d08
+#define LMEM_DMA_CTRL		0x0d40
+
+#define MC_STATUS0		0x2424
+#define MC_CTRL1		0x242c
+
+#define PSCALE_RST		0x2440
+#define PSCALE_CTRL		0x2444
+#define PSCALE_BMEM_ADDR	0x247c
+#define PSCALE_BMEM_DAT		0x2480
+
+#define DBLK_CTRL		0x2544
+#define DBLK_STATUS		0x254c
+
+#define GCLK_EN			0x260c
+#define MDEC_PIC_DC_CTRL	0x2638
+#define MDEC_PIC_DC_STATUS	0x263c
+#define ANC0_CANVAS_ADDR	0x2640
+#define MDEC_PIC_DC_THRESH	0x26e0
+
+/* Firmware interface registers */
+#define AV_SCRATCH_0		0x2700
+#define AV_SCRATCH_1		0x2704
+#define AV_SCRATCH_2		0x2708
+#define AV_SCRATCH_3		0x270c
+#define AV_SCRATCH_4		0x2710
+#define AV_SCRATCH_5		0x2714
+#define AV_SCRATCH_6		0x2718
+#define AV_SCRATCH_7		0x271c
+#define AV_SCRATCH_8		0x2720
+#define AV_SCRATCH_9		0x2724
+#define AV_SCRATCH_A		0x2728
+#define AV_SCRATCH_B		0x272c
+#define AV_SCRATCH_C		0x2730
+#define AV_SCRATCH_D		0x2734
+#define AV_SCRATCH_E		0x2738
+#define AV_SCRATCH_F		0x273c
+#define AV_SCRATCH_G		0x2740
+#define AV_SCRATCH_H		0x2744
+#define AV_SCRATCH_I		0x2748
+#define AV_SCRATCH_J		0x274c
+#define AV_SCRATCH_K		0x2750
+#define AV_SCRATCH_L		0x2754
+
+#define MPEG1_2_REG		0x3004
+#define PIC_HEAD_INFO		0x300c
+#define POWER_CTL_VLD		0x3020
+#define M4_CONTROL_REG		0x30a4
+
+/* Stream Buffer (stbuf) regs */
+#define VLD_MEM_VIFIFO_START_PTR	0x3100
+#define VLD_MEM_VIFIFO_CURR_PTR	0x3104
+#define VLD_MEM_VIFIFO_END_PTR	0x3108
+#define VLD_MEM_VIFIFO_CONTROL	0x3110
+	#define MEM_FIFO_CNT_BIT	16
+	#define MEM_FILL_ON_LEVEL	BIT(10)
+	#define MEM_CTRL_EMPTY_EN	BIT(2)
+	#define MEM_CTRL_FILL_EN	BIT(1)
+#define VLD_MEM_VIFIFO_WP	0x3114
+#define VLD_MEM_VIFIFO_RP	0x3118
+#define VLD_MEM_VIFIFO_LEVEL	0x311c
+#define VLD_MEM_VIFIFO_BUF_CNTL	0x3120
+	#define MEM_BUFCTRL_MANUAL	BIT(1)
+#define VLD_MEM_VIFIFO_WRAP_COUNT	0x3144
+
+#define DCAC_DMA_CTRL		0x3848
+
+#define DOS_SW_RESET0		0xfc00
+#define DOS_GCLK_EN0		0xfc04
+#define DOS_GEN_CTRL0		0xfc08
+#define DOS_MEM_PD_VDEC		0xfcc0
+#define DOS_MEM_PD_HEVC		0xfccc
+#define DOS_SW_RESET3		0xfcd0
+#define DOS_GCLK_EN3		0xfcd4
+#define DOS_VDEC_MCRCC_STALL_CTRL	0xfd00
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/esparser.c b/drivers/staging/media/meson/vdec/esparser.c
new file mode 100644
index 000000000..86ccc8937
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/esparser.c
@@ -0,0 +1,459 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ *
+ * The Elementary Stream Parser is a HW bitstream parser.
+ * It reads bitstream buffers and feeds them to the VIFIFO
+ */
+
+#include <linux/init.h>
+#include <linux/ioctl.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/reset.h>
+#include <linux/interrupt.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "dos_regs.h"
+#include "esparser.h"
+#include "vdec_helpers.h"
+
+/* PARSER REGS (CBUS) */
+#define PARSER_CONTROL 0x00
+	#define ES_PACK_SIZE_BIT	8
+	#define ES_WRITE		BIT(5)
+	#define ES_SEARCH		BIT(1)
+	#define ES_PARSER_START		BIT(0)
+#define PARSER_FETCH_ADDR	0x4
+#define PARSER_FETCH_CMD	0x8
+#define PARSER_CONFIG 0x14
+	#define PS_CFG_MAX_FETCH_CYCLE_BIT	0
+	#define PS_CFG_STARTCODE_WID_24_BIT	10
+	#define PS_CFG_MAX_ES_WR_CYCLE_BIT	12
+	#define PS_CFG_PFIFO_EMPTY_CNT_BIT	16
+#define PFIFO_WR_PTR 0x18
+#define PFIFO_RD_PTR 0x1c
+#define PARSER_SEARCH_PATTERN 0x24
+	#define ES_START_CODE_PATTERN 0x00000100
+#define PARSER_SEARCH_MASK 0x28
+	#define ES_START_CODE_MASK	0xffffff00
+	#define FETCH_ENDIAN_BIT	27
+#define PARSER_INT_ENABLE	0x2c
+	#define PARSER_INT_HOST_EN_BIT	8
+#define PARSER_INT_STATUS	0x30
+	#define PARSER_INTSTAT_SC_FOUND	1
+#define PARSER_ES_CONTROL	0x5c
+#define PARSER_VIDEO_START_PTR	0x80
+#define PARSER_VIDEO_END_PTR	0x84
+#define PARSER_VIDEO_WP		0x88
+#define PARSER_VIDEO_HOLE	0x90
+
+#define SEARCH_PATTERN_LEN	512
+#define VP9_HEADER_SIZE		16
+
+static DECLARE_WAIT_QUEUE_HEAD(wq);
+static int search_done;
+
+static irqreturn_t esparser_isr(int irq, void *dev)
+{
+	int int_status;
+	struct amvdec_core *core = dev;
+
+	int_status = amvdec_read_parser(core, PARSER_INT_STATUS);
+	amvdec_write_parser(core, PARSER_INT_STATUS, int_status);
+
+	if (int_status & PARSER_INTSTAT_SC_FOUND) {
+		amvdec_write_parser(core, PFIFO_RD_PTR, 0);
+		amvdec_write_parser(core, PFIFO_WR_PTR, 0);
+		search_done = 1;
+		wake_up_interruptible(&wq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * VP9 frame headers need to be appended by a 16-byte long
+ * Amlogic custom header
+ */
+static int vp9_update_header(struct amvdec_core *core, struct vb2_buffer *buf)
+{
+	u8 *dp;
+	u8 marker;
+	int dsize;
+	int num_frames, cur_frame;
+	int cur_mag, mag, mag_ptr;
+	int frame_size[8], tot_frame_size[8];
+	int total_datasize = 0;
+	int new_frame_size;
+	unsigned char *old_header = NULL;
+
+	dp = (uint8_t *)vb2_plane_vaddr(buf, 0);
+	dsize = vb2_get_plane_payload(buf, 0);
+
+	if (dsize == vb2_plane_size(buf, 0)) {
+		dev_warn(core->dev, "%s: unable to update header\n", __func__);
+		return 0;
+	}
+
+	marker = dp[dsize - 1];
+	if ((marker & 0xe0) == 0xc0) {
+		num_frames = (marker & 0x7) + 1;
+		mag = ((marker >> 3) & 0x3) + 1;
+		mag_ptr = dsize - mag * num_frames - 2;
+		if (dp[mag_ptr] != marker)
+			return 0;
+
+		mag_ptr++;
+		for (cur_frame = 0; cur_frame < num_frames; cur_frame++) {
+			frame_size[cur_frame] = 0;
+			for (cur_mag = 0; cur_mag < mag; cur_mag++) {
+				frame_size[cur_frame] |=
+					(dp[mag_ptr] << (cur_mag * 8));
+				mag_ptr++;
+			}
+			if (cur_frame == 0)
+				tot_frame_size[cur_frame] =
+					frame_size[cur_frame];
+			else
+				tot_frame_size[cur_frame] =
+					tot_frame_size[cur_frame - 1] +
+					frame_size[cur_frame];
+			total_datasize += frame_size[cur_frame];
+		}
+	} else {
+		num_frames = 1;
+		frame_size[0] = dsize;
+		tot_frame_size[0] = dsize;
+		total_datasize = dsize;
+	}
+
+	new_frame_size = total_datasize + num_frames * VP9_HEADER_SIZE;
+
+	if (new_frame_size >= vb2_plane_size(buf, 0)) {
+		dev_warn(core->dev, "%s: unable to update header\n", __func__);
+		return 0;
+	}
+
+	for (cur_frame = num_frames - 1; cur_frame >= 0; cur_frame--) {
+		int framesize = frame_size[cur_frame];
+		int framesize_header = framesize + 4;
+		int oldframeoff = tot_frame_size[cur_frame] - framesize;
+		int outheaderoff =  oldframeoff + cur_frame * VP9_HEADER_SIZE;
+		u8 *fdata = dp + outheaderoff;
+		u8 *old_framedata = dp + oldframeoff;
+
+		memmove(fdata + VP9_HEADER_SIZE, old_framedata, framesize);
+
+		fdata[0] = (framesize_header >> 24) & 0xff;
+		fdata[1] = (framesize_header >> 16) & 0xff;
+		fdata[2] = (framesize_header >> 8) & 0xff;
+		fdata[3] = (framesize_header >> 0) & 0xff;
+		fdata[4] = ((framesize_header >> 24) & 0xff) ^ 0xff;
+		fdata[5] = ((framesize_header >> 16) & 0xff) ^ 0xff;
+		fdata[6] = ((framesize_header >> 8) & 0xff) ^ 0xff;
+		fdata[7] = ((framesize_header >> 0) & 0xff) ^ 0xff;
+		fdata[8] = 0;
+		fdata[9] = 0;
+		fdata[10] = 0;
+		fdata[11] = 1;
+		fdata[12] = 'A';
+		fdata[13] = 'M';
+		fdata[14] = 'L';
+		fdata[15] = 'V';
+
+		if (!old_header) {
+			/* nothing */
+		} else if (old_header > fdata + 16 + framesize) {
+			dev_dbg(core->dev, "%s: data has gaps, setting to 0\n",
+				__func__);
+			memset(fdata + 16 + framesize, 0,
+			       (old_header - fdata + 16 + framesize));
+		} else if (old_header < fdata + 16 + framesize) {
+			dev_err(core->dev, "%s: data overwritten\n", __func__);
+		}
+		old_header = fdata;
+	}
+
+	return new_frame_size;
+}
+
+/* Pad the packet to at least 4KiB bytes otherwise the VDEC unit won't trigger
+ * ISRs.
+ * Also append a start code 000001ff at the end to trigger
+ * the ESPARSER interrupt.
+ */
+static u32 esparser_pad_start_code(struct amvdec_core *core,
+				   struct vb2_buffer *vb,
+				   u32 payload_size)
+{
+	u32 pad_size = 0;
+	u8 *vaddr = vb2_plane_vaddr(vb, 0);
+
+	if (payload_size < ESPARSER_MIN_PACKET_SIZE) {
+		pad_size = ESPARSER_MIN_PACKET_SIZE - payload_size;
+		memset(vaddr + payload_size, 0, pad_size);
+	}
+
+	if ((payload_size + pad_size + SEARCH_PATTERN_LEN) >
+						vb2_plane_size(vb, 0)) {
+		dev_warn(core->dev, "%s: unable to pad start code\n", __func__);
+		return pad_size;
+	}
+
+	memset(vaddr + payload_size + pad_size, 0, SEARCH_PATTERN_LEN);
+	vaddr[payload_size + pad_size]     = 0x00;
+	vaddr[payload_size + pad_size + 1] = 0x00;
+	vaddr[payload_size + pad_size + 2] = 0x01;
+	vaddr[payload_size + pad_size + 3] = 0xff;
+
+	return pad_size;
+}
+
+static int
+esparser_write_data(struct amvdec_core *core, dma_addr_t addr, u32 size)
+{
+	amvdec_write_parser(core, PFIFO_RD_PTR, 0);
+	amvdec_write_parser(core, PFIFO_WR_PTR, 0);
+	amvdec_write_parser(core, PARSER_CONTROL,
+			    ES_WRITE |
+			    ES_PARSER_START |
+			    ES_SEARCH |
+			    (size << ES_PACK_SIZE_BIT));
+
+	amvdec_write_parser(core, PARSER_FETCH_ADDR, addr);
+	amvdec_write_parser(core, PARSER_FETCH_CMD,
+			    (7 << FETCH_ENDIAN_BIT) |
+			    (size + SEARCH_PATTERN_LEN));
+
+	search_done = 0;
+	return wait_event_interruptible_timeout(wq, search_done, (HZ / 5));
+}
+
+static u32 esparser_vififo_get_free_space(struct amvdec_session *sess)
+{
+	u32 vififo_usage;
+	struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
+	struct amvdec_core *core = sess->core;
+
+	vififo_usage  = vdec_ops->vififo_level(sess);
+	vififo_usage += amvdec_read_parser(core, PARSER_VIDEO_HOLE);
+	vififo_usage += (6 * SZ_1K); // 6 KiB internal fifo
+
+	if (vififo_usage > sess->vififo_size) {
+		dev_warn(sess->core->dev,
+			 "VIFIFO usage (%u) > VIFIFO size (%u)\n",
+			 vififo_usage, sess->vififo_size);
+		return 0;
+	}
+
+	return sess->vififo_size - vififo_usage;
+}
+
+int esparser_queue_eos(struct amvdec_core *core, const u8 *data, u32 len)
+{
+	struct device *dev = core->dev;
+	void *eos_vaddr;
+	dma_addr_t eos_paddr;
+	int ret;
+
+	eos_vaddr = dma_alloc_coherent(dev, len + SEARCH_PATTERN_LEN,
+				       &eos_paddr, GFP_KERNEL);
+	if (!eos_vaddr)
+		return -ENOMEM;
+
+	memcpy(eos_vaddr, data, len);
+	ret = esparser_write_data(core, eos_paddr, len);
+	dma_free_coherent(dev, len + SEARCH_PATTERN_LEN,
+			  eos_vaddr, eos_paddr);
+
+	return ret;
+}
+
+static u32 esparser_get_offset(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	u32 offset = amvdec_read_parser(core, PARSER_VIDEO_WP) -
+		     sess->vififo_paddr;
+
+	if (offset < sess->last_offset)
+		sess->wrap_count++;
+
+	sess->last_offset = offset;
+	offset += (sess->wrap_count * sess->vififo_size);
+
+	return offset;
+}
+
+static int
+esparser_queue(struct amvdec_session *sess, struct vb2_v4l2_buffer *vbuf)
+{
+	int ret;
+	struct vb2_buffer *vb = &vbuf->vb2_buf;
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	u32 payload_size = vb2_get_plane_payload(vb, 0);
+	dma_addr_t phy = vb2_dma_contig_plane_dma_addr(vb, 0);
+	u32 num_dst_bufs = 0;
+	u32 offset;
+	u32 pad_size;
+
+	/*
+	 * When max ref frame is held by VP9, this should be -= 3 to prevent a
+	 * shortage of CAPTURE buffers on the decoder side.
+	 * For the future, a good enhancement of the way this is handled could
+	 * be to notify new capture buffers to the decoding modules, so that
+	 * they could pause when there is no capture buffer available and
+	 * resume on this notification.
+	 */
+	if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) {
+		if (codec_ops->num_pending_bufs)
+			num_dst_bufs = codec_ops->num_pending_bufs(sess);
+
+		num_dst_bufs += v4l2_m2m_num_dst_bufs_ready(sess->m2m_ctx);
+		if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9)
+			num_dst_bufs -= 3;
+
+		if (esparser_vififo_get_free_space(sess) < payload_size ||
+		    atomic_read(&sess->esparser_queued_bufs) >= num_dst_bufs)
+			return -EAGAIN;
+	} else if (esparser_vififo_get_free_space(sess) < payload_size) {
+		return -EAGAIN;
+	}
+
+	v4l2_m2m_src_buf_remove_by_buf(sess->m2m_ctx, vbuf);
+
+	offset = esparser_get_offset(sess);
+
+	ret = amvdec_add_ts(sess, vb->timestamp, vbuf->timecode, offset, vbuf->flags);
+	if (ret) {
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+		return ret;
+	}
+
+	dev_dbg(core->dev, "esparser: ts = %llu pld_size = %u offset = %08X flags = %08X\n",
+		vb->timestamp, payload_size, offset, vbuf->flags);
+
+	vbuf->flags = 0;
+	vbuf->field = V4L2_FIELD_NONE;
+	vbuf->sequence = sess->sequence_out++;
+
+	if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) {
+		payload_size = vp9_update_header(core, vb);
+
+		/* If unable to alter buffer to add headers */
+		if (payload_size == 0) {
+			amvdec_remove_ts(sess, vb->timestamp);
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+
+			return 0;
+		}
+	}
+
+	pad_size = esparser_pad_start_code(core, vb, payload_size);
+	ret = esparser_write_data(core, phy, payload_size + pad_size);
+
+	if (ret <= 0) {
+		dev_warn(core->dev, "esparser: input parsing error\n");
+		amvdec_remove_ts(sess, vb->timestamp);
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+		amvdec_write_parser(core, PARSER_FETCH_CMD, 0);
+
+		return 0;
+	}
+
+	atomic_inc(&sess->esparser_queued_bufs);
+	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
+
+	return 0;
+}
+
+void esparser_queue_all_src(struct work_struct *work)
+{
+	struct v4l2_m2m_buffer *buf, *n;
+	struct amvdec_session *sess =
+		container_of(work, struct amvdec_session, esparser_queue_work);
+
+	mutex_lock(&sess->lock);
+	v4l2_m2m_for_each_src_buf_safe(sess->m2m_ctx, buf, n) {
+		if (sess->should_stop)
+			break;
+
+		if (esparser_queue(sess, &buf->vb) < 0)
+			break;
+	}
+	mutex_unlock(&sess->lock);
+}
+
+int esparser_power_up(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
+
+	reset_control_reset(core->esparser_reset);
+	amvdec_write_parser(core, PARSER_CONFIG,
+			    (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
+			    (1  << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
+			    (16 << PS_CFG_MAX_FETCH_CYCLE_BIT));
+
+	amvdec_write_parser(core, PFIFO_RD_PTR, 0);
+	amvdec_write_parser(core, PFIFO_WR_PTR, 0);
+
+	amvdec_write_parser(core, PARSER_SEARCH_PATTERN,
+			    ES_START_CODE_PATTERN);
+	amvdec_write_parser(core, PARSER_SEARCH_MASK, ES_START_CODE_MASK);
+
+	amvdec_write_parser(core, PARSER_CONFIG,
+			    (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
+			    (1  << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
+			    (16 << PS_CFG_MAX_FETCH_CYCLE_BIT) |
+			    (2  << PS_CFG_STARTCODE_WID_24_BIT));
+
+	amvdec_write_parser(core, PARSER_CONTROL,
+			    (ES_SEARCH | ES_PARSER_START));
+
+	amvdec_write_parser(core, PARSER_VIDEO_START_PTR, sess->vififo_paddr);
+	amvdec_write_parser(core, PARSER_VIDEO_END_PTR,
+			    sess->vififo_paddr + sess->vififo_size - 8);
+	amvdec_write_parser(core, PARSER_ES_CONTROL,
+			    amvdec_read_parser(core, PARSER_ES_CONTROL) & ~1);
+
+	if (vdec_ops->conf_esparser)
+		vdec_ops->conf_esparser(sess);
+
+	amvdec_write_parser(core, PARSER_INT_STATUS, 0xffff);
+	amvdec_write_parser(core, PARSER_INT_ENABLE,
+			    BIT(PARSER_INT_HOST_EN_BIT));
+
+	return 0;
+}
+
+int esparser_init(struct platform_device *pdev, struct amvdec_core *core)
+{
+	struct device *dev = &pdev->dev;
+	int ret;
+	int irq;
+
+	irq = platform_get_irq_byname(pdev, "esparser");
+	if (irq < 0)
+		return irq;
+
+	ret = devm_request_irq(dev, irq, esparser_isr, IRQF_SHARED,
+			       "esparserirq", core);
+	if (ret) {
+		dev_err(dev, "Failed requesting ESPARSER IRQ\n");
+		return ret;
+	}
+
+	core->esparser_reset =
+		devm_reset_control_get_exclusive(dev, "esparser");
+	if (IS_ERR(core->esparser_reset)) {
+		dev_err(dev, "Failed to get esparser_reset\n");
+		return PTR_ERR(core->esparser_reset);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/meson/vdec/esparser.h b/drivers/staging/media/meson/vdec/esparser.h
new file mode 100644
index 000000000..9351e62c7
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/esparser.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_ESPARSER_H_
+#define __MESON_VDEC_ESPARSER_H_
+
+#include <linux/platform_device.h>
+
+#include "vdec.h"
+
+int esparser_init(struct platform_device *pdev, struct amvdec_core *core);
+int esparser_power_up(struct amvdec_session *sess);
+
+/**
+ * esparser_queue_eos() - write End Of Stream sequence to the ESPARSER
+ *
+ * @core: vdec core struct
+ * @data: EOS sequence
+ * @len: length of EOS sequence
+ */
+int esparser_queue_eos(struct amvdec_core *core, const u8 *data, u32 len);
+
+/**
+ * esparser_queue_all_src() - work handler that writes as many src buffers
+ * as possible to the ESPARSER
+ *
+ * @work: work struct
+ */
+void esparser_queue_all_src(struct work_struct *work);
+
+#define ESPARSER_MIN_PACKET_SIZE SZ_4K
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/hevc_regs.h b/drivers/staging/media/meson/vdec/hevc_regs.h
new file mode 100644
index 000000000..0392f41a1
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/hevc_regs.h
@@ -0,0 +1,218 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2015 Amlogic, Inc. All rights reserved.
+ */
+
+#ifndef __MESON_VDEC_HEVC_REGS_H_
+#define __MESON_VDEC_HEVC_REGS_H_
+
+#define HEVC_ASSIST_MMU_MAP_ADDR 0xc024
+
+#define HEVC_ASSIST_MBOX1_CLR_REG 0xc1d4
+#define HEVC_ASSIST_MBOX1_MASK 0xc1d8
+
+#define HEVC_ASSIST_SCRATCH_0 0xc300
+#define HEVC_ASSIST_SCRATCH_1 0xc304
+#define HEVC_ASSIST_SCRATCH_2 0xc308
+#define HEVC_ASSIST_SCRATCH_3 0xc30c
+#define HEVC_ASSIST_SCRATCH_4 0xc310
+#define HEVC_ASSIST_SCRATCH_5 0xc314
+#define HEVC_ASSIST_SCRATCH_6 0xc318
+#define HEVC_ASSIST_SCRATCH_7 0xc31c
+#define HEVC_ASSIST_SCRATCH_8 0xc320
+#define HEVC_ASSIST_SCRATCH_9 0xc324
+#define HEVC_ASSIST_SCRATCH_A 0xc328
+#define HEVC_ASSIST_SCRATCH_B 0xc32c
+#define HEVC_ASSIST_SCRATCH_C 0xc330
+#define HEVC_ASSIST_SCRATCH_D 0xc334
+#define HEVC_ASSIST_SCRATCH_E 0xc338
+#define HEVC_ASSIST_SCRATCH_F 0xc33c
+#define HEVC_ASSIST_SCRATCH_G 0xc340
+#define HEVC_ASSIST_SCRATCH_H 0xc344
+#define HEVC_ASSIST_SCRATCH_I 0xc348
+#define HEVC_ASSIST_SCRATCH_J 0xc34c
+#define HEVC_ASSIST_SCRATCH_K 0xc350
+#define HEVC_ASSIST_SCRATCH_L 0xc354
+#define HEVC_ASSIST_SCRATCH_M 0xc358
+#define HEVC_ASSIST_SCRATCH_N 0xc35c
+
+#define HEVC_PARSER_VERSION 0xc400
+#define HEVC_STREAM_CONTROL 0xc404
+#define HEVC_STREAM_START_ADDR 0xc408
+#define HEVC_STREAM_END_ADDR 0xc40c
+#define HEVC_STREAM_WR_PTR 0xc410
+#define HEVC_STREAM_RD_PTR 0xc414
+#define HEVC_STREAM_LEVEL 0xc418
+#define HEVC_STREAM_FIFO_CTL 0xc41c
+#define HEVC_SHIFT_CONTROL 0xc420
+#define HEVC_SHIFT_STARTCODE 0xc424
+#define HEVC_SHIFT_EMULATECODE 0xc428
+#define HEVC_SHIFT_STATUS 0xc42c
+#define HEVC_SHIFTED_DATA 0xc430
+#define HEVC_SHIFT_BYTE_COUNT 0xc434
+#define HEVC_SHIFT_COMMAND 0xc438
+#define HEVC_ELEMENT_RESULT 0xc43c
+#define HEVC_CABAC_CONTROL 0xc440
+#define HEVC_PARSER_SLICE_INFO 0xc444
+#define HEVC_PARSER_CMD_WRITE 0xc448
+#define HEVC_PARSER_CORE_CONTROL 0xc44c
+#define HEVC_PARSER_CMD_FETCH 0xc450
+#define HEVC_PARSER_CMD_STATUS 0xc454
+#define HEVC_PARSER_LCU_INFO 0xc458
+#define HEVC_PARSER_HEADER_INFO 0xc45c
+#define HEVC_PARSER_INT_CONTROL 0xc480
+#define HEVC_PARSER_INT_STATUS 0xc484
+#define HEVC_PARSER_IF_CONTROL 0xc488
+#define HEVC_PARSER_PICTURE_SIZE 0xc48c
+#define HEVC_PARSER_LCU_START 0xc490
+#define HEVC_PARSER_HEADER_INFO2 0xc494
+#define HEVC_PARSER_QUANT_READ 0xc498
+#define HEVC_PARSER_RESERVED_27 0xc49c
+#define HEVC_PARSER_CMD_SKIP_0 0xc4a0
+#define HEVC_PARSER_CMD_SKIP_1 0xc4a4
+#define HEVC_PARSER_CMD_SKIP_2 0xc4a8
+#define HEVC_SAO_IF_STATUS 0xc4c0
+#define HEVC_SAO_IF_DATA_Y 0xc4c4
+#define HEVC_SAO_IF_DATA_U 0xc4c8
+#define HEVC_SAO_IF_DATA_V 0xc4cc
+#define HEVC_STREAM_SWAP_ADDR 0xc4d0
+#define HEVC_STREAM_SWAP_CTRL 0xc4d4
+#define HEVC_IQIT_IF_WAIT_CNT 0xc4d8
+#define HEVC_MPRED_IF_WAIT_CNT 0xc4dc
+#define HEVC_SAO_IF_WAIT_CNT 0xc4e0
+
+#define HEVC_MPRED_VERSION 0xc800
+#define HEVC_MPRED_CTRL0 0xc804
+	#define MPRED_CTRL0_NEW_PIC	BIT(2)
+	#define MPRED_CTRL0_NEW_TILE	BIT(3)
+	#define MPRED_CTRL0_NEW_SLI_SEG	BIT(4)
+	#define MPRED_CTRL0_TMVP	BIT(5)
+	#define MPRED_CTRL0_LDC		BIT(6)
+	#define MPRED_CTRL0_COL_FROM_L0	BIT(7)
+	#define MPRED_CTRL0_ABOVE_EN	BIT(9)
+	#define MPRED_CTRL0_MV_WR_EN	BIT(10)
+	#define MPRED_CTRL0_MV_RD_EN	BIT(11)
+	#define MPRED_CTRL0_BUF_LINEAR	BIT(13)
+#define HEVC_MPRED_CTRL1 0xc808
+#define HEVC_MPRED_INT_EN 0xc80c
+#define HEVC_MPRED_INT_STATUS 0xc810
+#define HEVC_MPRED_PIC_SIZE 0xc814
+#define HEVC_MPRED_PIC_SIZE_LCU 0xc818
+#define HEVC_MPRED_TILE_START 0xc81c
+#define HEVC_MPRED_TILE_SIZE_LCU 0xc820
+#define HEVC_MPRED_REF_NUM 0xc824
+#define HEVC_MPRED_REF_EN_L0 0xc830
+#define HEVC_MPRED_REF_EN_L1 0xc834
+#define HEVC_MPRED_COLREF_EN_L0 0xc838
+#define HEVC_MPRED_COLREF_EN_L1 0xc83c
+#define HEVC_MPRED_AXI_WCTRL 0xc840
+#define HEVC_MPRED_AXI_RCTRL 0xc844
+#define HEVC_MPRED_ABV_START_ADDR 0xc848
+#define HEVC_MPRED_MV_WR_START_ADDR 0xc84c
+#define HEVC_MPRED_MV_RD_START_ADDR 0xc850
+#define HEVC_MPRED_MV_WPTR 0xc854
+#define HEVC_MPRED_MV_RPTR 0xc858
+#define HEVC_MPRED_MV_WR_ROW_JUMP 0xc85c
+#define HEVC_MPRED_MV_RD_ROW_JUMP 0xc860
+#define HEVC_MPRED_CURR_LCU 0xc864
+#define HEVC_MPRED_ABV_WPTR 0xc868
+#define HEVC_MPRED_ABV_RPTR 0xc86c
+#define HEVC_MPRED_CTRL2 0xc870
+#define HEVC_MPRED_CTRL3 0xc874
+#define HEVC_MPRED_L0_REF00_POC 0xc880
+#define HEVC_MPRED_L1_REF00_POC 0xc8c0
+
+#define HEVC_MPRED_CTRL4 0xc930
+
+#define HEVC_MPRED_CUR_POC 0xc980
+#define HEVC_MPRED_COL_POC 0xc984
+#define HEVC_MPRED_MV_RD_END_ADDR 0xc988
+
+#define HEVC_MSP 0xcc00
+#define HEVC_MPSR 0xcc04
+#define HEVC_MCPU_INTR_MSK 0xcc10
+#define HEVC_MCPU_INTR_REQ 0xcc14
+#define HEVC_CPSR 0xcc84
+
+#define HEVC_IMEM_DMA_CTRL 0xcd00
+#define HEVC_IMEM_DMA_ADR 0xcd04
+#define HEVC_IMEM_DMA_COUNT 0xcd08
+
+#define HEVCD_IPP_TOP_CNTL 0xd000
+#define HEVCD_IPP_LINEBUFF_BASE 0xd024
+#define HEVCD_IPP_AXIIF_CONFIG 0xd02c
+
+#define VP9D_MPP_REF_SCALE_ENBL		0xd104
+#define VP9D_MPP_REFINFO_TBL_ACCCONFIG	0xd108
+#define VP9D_MPP_REFINFO_DATA		0xd10c
+
+#define HEVCD_MPP_ANC2AXI_TBL_CONF_ADDR 0xd180
+#define HEVCD_MPP_ANC2AXI_TBL_CMD_ADDR 0xd184
+#define HEVCD_MPP_ANC2AXI_TBL_DATA 0xd190
+
+#define HEVCD_MPP_ANC_CANVAS_ACCCONFIG_ADDR 0xd300
+#define HEVCD_MPP_ANC_CANVAS_DATA_ADDR 0xd304
+#define HEVCD_MPP_DECOMP_CTL1 0xd308
+#define HEVCD_MPP_DECOMP_CTL2 0xd30c
+#define HEVCD_MCRCC_CTL1 0xd3c0
+#define HEVCD_MCRCC_CTL2 0xd3c4
+#define HEVCD_MCRCC_CTL3 0xd3c8
+
+#define HEVC_DBLK_CFG0 0xd400
+#define HEVC_DBLK_CFG1 0xd404
+#define HEVC_DBLK_CFG2 0xd408
+#define HEVC_DBLK_CFG3 0xd40c
+#define HEVC_DBLK_CFG4 0xd410
+#define HEVC_DBLK_CFG5 0xd414
+#define HEVC_DBLK_CFG6 0xd418
+#define HEVC_DBLK_CFG7 0xd41c
+#define HEVC_DBLK_CFG8 0xd420
+#define HEVC_DBLK_CFG9 0xd424
+#define HEVC_DBLK_CFGA 0xd428
+#define HEVC_DBLK_STS0 0xd42c
+#define HEVC_DBLK_CFGB 0xd42c
+#define HEVC_DBLK_STS1 0xd430
+#define HEVC_DBLK_CFGE 0xd438
+
+#define HEVC_SAO_VERSION 0xd800
+#define HEVC_SAO_CTRL0 0xd804
+#define HEVC_SAO_CTRL1 0xd808
+#define HEVC_SAO_PIC_SIZE 0xd814
+#define HEVC_SAO_PIC_SIZE_LCU 0xd818
+#define HEVC_SAO_TILE_START 0xd81c
+#define HEVC_SAO_TILE_SIZE_LCU 0xd820
+#define HEVC_SAO_Y_START_ADDR 0xd82c
+#define HEVC_SAO_Y_LENGTH 0xd830
+#define HEVC_SAO_C_START_ADDR 0xd834
+#define HEVC_SAO_C_LENGTH 0xd838
+#define HEVC_SAO_Y_WPTR 0xd83c
+#define HEVC_SAO_C_WPTR 0xd840
+#define HEVC_SAO_ABV_START_ADDR 0xd844
+#define HEVC_SAO_VB_WR_START_ADDR 0xd848
+#define HEVC_SAO_VB_RD_START_ADDR 0xd84c
+#define HEVC_SAO_ABV_WPTR 0xd850
+#define HEVC_SAO_ABV_RPTR 0xd854
+#define HEVC_SAO_VB_WPTR 0xd858
+#define HEVC_SAO_VB_RPTR 0xd85c
+#define HEVC_SAO_CTRL2 0xd880
+#define HEVC_SAO_CTRL3 0xd884
+#define HEVC_SAO_CTRL4 0xd888
+#define HEVC_SAO_CTRL5 0xd88c
+#define HEVC_SAO_CTRL6 0xd890
+#define HEVC_SAO_CTRL7 0xd894
+#define HEVC_CM_BODY_START_ADDR 0xd898
+#define HEVC_CM_BODY_LENGTH 0xd89c
+#define HEVC_CM_HEADER_START_ADDR 0xd8a0
+#define HEVC_CM_HEADER_LENGTH 0xd8a4
+#define HEVC_CM_HEADER_OFFSET 0xd8ac
+#define HEVC_SAO_MMU_VH0_ADDR 0xd8e8
+#define HEVC_SAO_MMU_VH1_ADDR 0xd8ec
+
+#define HEVC_IQIT_CLK_RST_CTRL 0xdc00
+#define HEVC_IQIT_SCALELUT_WR_ADDR 0xdc08
+#define HEVC_IQIT_SCALELUT_RD_ADDR 0xdc0c
+#define HEVC_IQIT_SCALELUT_DATA 0xdc10
+
+#define HEVC_PSCALE_CTRL 0xe444
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/vdec.c b/drivers/staging/media/meson/vdec/vdec.c
new file mode 100644
index 000000000..52f224d8d
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec.c
@@ -0,0 +1,1131 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include <linux/of_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/syscon.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "vdec.h"
+#include "esparser.h"
+#include "vdec_helpers.h"
+
+struct dummy_buf {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+/* 16 MiB for parsed bitstream swap exchange */
+#define SIZE_VIFIFO SZ_16M
+
+static u32 get_output_size(u32 width, u32 height)
+{
+	return ALIGN(width * height, SZ_64K);
+}
+
+u32 amvdec_get_output_size(struct amvdec_session *sess)
+{
+	return get_output_size(sess->width, sess->height);
+}
+EXPORT_SYMBOL_GPL(amvdec_get_output_size);
+
+static int vdec_codec_needs_recycle(struct amvdec_session *sess)
+{
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	return codec_ops->can_recycle && codec_ops->recycle;
+}
+
+static int vdec_recycle_thread(void *data)
+{
+	struct amvdec_session *sess = data;
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	struct amvdec_buffer *tmp, *n;
+
+	while (!kthread_should_stop()) {
+		mutex_lock(&sess->bufs_recycle_lock);
+		list_for_each_entry_safe(tmp, n, &sess->bufs_recycle, list) {
+			if (!codec_ops->can_recycle(core))
+				break;
+
+			codec_ops->recycle(core, tmp->vb->index);
+			list_del(&tmp->list);
+			kfree(tmp);
+		}
+		mutex_unlock(&sess->bufs_recycle_lock);
+
+		usleep_range(5000, 10000);
+	}
+
+	return 0;
+}
+
+static int vdec_poweron(struct amvdec_session *sess)
+{
+	int ret;
+	struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
+
+	ret = clk_prepare_enable(sess->core->dos_parser_clk);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(sess->core->dos_clk);
+	if (ret)
+		goto disable_dos_parser;
+
+	ret = vdec_ops->start(sess);
+	if (ret)
+		goto disable_dos;
+
+	esparser_power_up(sess);
+
+	return 0;
+
+disable_dos:
+	clk_disable_unprepare(sess->core->dos_clk);
+disable_dos_parser:
+	clk_disable_unprepare(sess->core->dos_parser_clk);
+
+	return ret;
+}
+
+static void vdec_wait_inactive(struct amvdec_session *sess)
+{
+	/* We consider 50ms with no IRQ to be inactive. */
+	while (time_is_after_jiffies64(sess->last_irq_jiffies +
+				       msecs_to_jiffies(50)))
+		msleep(25);
+}
+
+static void vdec_poweroff(struct amvdec_session *sess)
+{
+	struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	sess->should_stop = 1;
+	vdec_wait_inactive(sess);
+	if (codec_ops->drain)
+		codec_ops->drain(sess);
+
+	vdec_ops->stop(sess);
+	clk_disable_unprepare(sess->core->dos_clk);
+	clk_disable_unprepare(sess->core->dos_parser_clk);
+}
+
+static void
+vdec_queue_recycle(struct amvdec_session *sess, struct vb2_buffer *vb)
+{
+	struct amvdec_buffer *new_buf;
+
+	new_buf = kmalloc(sizeof(*new_buf), GFP_KERNEL);
+	if (!new_buf)
+		return;
+	new_buf->vb = vb;
+
+	mutex_lock(&sess->bufs_recycle_lock);
+	list_add_tail(&new_buf->list, &sess->bufs_recycle);
+	mutex_unlock(&sess->bufs_recycle_lock);
+}
+
+static void vdec_m2m_device_run(void *priv)
+{
+	struct amvdec_session *sess = priv;
+
+	schedule_work(&sess->esparser_queue_work);
+}
+
+static void vdec_m2m_job_abort(void *priv)
+{
+	struct amvdec_session *sess = priv;
+
+	v4l2_m2m_job_finish(sess->m2m_dev, sess->m2m_ctx);
+}
+
+static const struct v4l2_m2m_ops vdec_m2m_ops = {
+	.device_run = vdec_m2m_device_run,
+	.job_abort = vdec_m2m_job_abort,
+};
+
+static void process_num_buffers(struct vb2_queue *q,
+				struct amvdec_session *sess,
+				unsigned int *num_buffers,
+				bool is_reqbufs)
+{
+	const struct amvdec_format *fmt_out = sess->fmt_out;
+	unsigned int buffers_total = q->num_buffers + *num_buffers;
+	u32 min_buf_capture = v4l2_ctrl_g_ctrl(sess->ctrl_min_buf_capture);
+
+	if (q->num_buffers + *num_buffers < min_buf_capture)
+		*num_buffers = min_buf_capture - q->num_buffers;
+	if (is_reqbufs && buffers_total < fmt_out->min_buffers)
+		*num_buffers = fmt_out->min_buffers - q->num_buffers;
+	if (buffers_total > fmt_out->max_buffers)
+		*num_buffers = fmt_out->max_buffers - q->num_buffers;
+
+	/* We need to program the complete CAPTURE buffer list
+	 * in registers during start_streaming, and the firmwares
+	 * are free to choose any of them to write frames to. As such,
+	 * we need all of them to be queued into the driver
+	 */
+	sess->num_dst_bufs = q->num_buffers + *num_buffers;
+	q->min_buffers_needed = max(fmt_out->min_buffers, sess->num_dst_bufs);
+}
+
+static int vdec_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
+			    unsigned int *num_planes, unsigned int sizes[],
+			    struct device *alloc_devs[])
+{
+	struct amvdec_session *sess = vb2_get_drv_priv(q);
+	u32 output_size = amvdec_get_output_size(sess);
+
+	if (*num_planes) {
+		switch (q->type) {
+		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+			if (*num_planes != 1 ||
+			    sizes[0] < sess->src_buffer_size)
+				return -EINVAL;
+			break;
+		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+			switch (sess->pixfmt_cap) {
+			case V4L2_PIX_FMT_NV12M:
+				if (*num_planes != 2 ||
+				    sizes[0] < output_size ||
+				    sizes[1] < output_size / 2)
+					return -EINVAL;
+				break;
+			case V4L2_PIX_FMT_YUV420M:
+				if (*num_planes != 3 ||
+				    sizes[0] < output_size ||
+				    sizes[1] < output_size / 4 ||
+				    sizes[2] < output_size / 4)
+					return -EINVAL;
+				break;
+			default:
+				return -EINVAL;
+			}
+
+			process_num_buffers(q, sess, num_buffers, false);
+			break;
+		}
+
+		return 0;
+	}
+
+	switch (q->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		sizes[0] = sess->src_buffer_size;
+		*num_planes = 1;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		switch (sess->pixfmt_cap) {
+		case V4L2_PIX_FMT_NV12M:
+			sizes[0] = output_size;
+			sizes[1] = output_size / 2;
+			*num_planes = 2;
+			break;
+		case V4L2_PIX_FMT_YUV420M:
+			sizes[0] = output_size;
+			sizes[1] = output_size / 4;
+			sizes[2] = output_size / 4;
+			*num_planes = 3;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		process_num_buffers(q, sess, num_buffers, true);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sess->changed_format = 1;
+	return 0;
+}
+
+static void vdec_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct amvdec_session *sess = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_m2m_ctx *m2m_ctx = sess->m2m_ctx;
+
+	v4l2_m2m_buf_queue(m2m_ctx, vbuf);
+
+	if (!sess->streamon_out)
+		return;
+
+	if (sess->streamon_cap &&
+	    vb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    vdec_codec_needs_recycle(sess))
+		vdec_queue_recycle(sess, vb);
+
+	schedule_work(&sess->esparser_queue_work);
+}
+
+static int vdec_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct amvdec_session *sess = vb2_get_drv_priv(q);
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	struct amvdec_core *core = sess->core;
+	struct vb2_v4l2_buffer *buf;
+	int ret;
+
+	if (core->cur_sess && core->cur_sess != sess) {
+		ret = -EBUSY;
+		goto bufs_done;
+	}
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		sess->streamon_out = 1;
+	else
+		sess->streamon_cap = 1;
+
+	if (!sess->streamon_out)
+		return 0;
+
+	if (sess->status == STATUS_NEEDS_RESUME &&
+	    q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    sess->changed_format) {
+		codec_ops->resume(sess);
+		sess->status = STATUS_RUNNING;
+		return 0;
+	}
+
+	if (sess->status == STATUS_RUNNING ||
+	    sess->status == STATUS_NEEDS_RESUME ||
+	    sess->status == STATUS_INIT)
+		return 0;
+
+	sess->vififo_size = SIZE_VIFIFO;
+	sess->vififo_vaddr =
+		dma_alloc_coherent(sess->core->dev, sess->vififo_size,
+				   &sess->vififo_paddr, GFP_KERNEL);
+	if (!sess->vififo_vaddr) {
+		dev_err(sess->core->dev, "Failed to request VIFIFO buffer\n");
+		ret = -ENOMEM;
+		goto bufs_done;
+	}
+
+	sess->should_stop = 0;
+	sess->keyframe_found = 0;
+	sess->last_offset = 0;
+	sess->wrap_count = 0;
+	sess->pixelaspect.numerator = 1;
+	sess->pixelaspect.denominator = 1;
+	atomic_set(&sess->esparser_queued_bufs, 0);
+	v4l2_ctrl_s_ctrl(sess->ctrl_min_buf_capture, 1);
+
+	ret = vdec_poweron(sess);
+	if (ret)
+		goto vififo_free;
+
+	sess->sequence_cap = 0;
+	sess->sequence_out = 0;
+	if (vdec_codec_needs_recycle(sess))
+		sess->recycle_thread = kthread_run(vdec_recycle_thread, sess,
+						   "vdec_recycle");
+
+	sess->status = STATUS_INIT;
+	core->cur_sess = sess;
+	schedule_work(&sess->esparser_queue_work);
+	return 0;
+
+vififo_free:
+	dma_free_coherent(sess->core->dev, sess->vififo_size,
+			  sess->vififo_vaddr, sess->vififo_paddr);
+bufs_done:
+	while ((buf = v4l2_m2m_src_buf_remove(sess->m2m_ctx)))
+		v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+	while ((buf = v4l2_m2m_dst_buf_remove(sess->m2m_ctx)))
+		v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		sess->streamon_out = 0;
+	else
+		sess->streamon_cap = 0;
+
+	return ret;
+}
+
+static void vdec_free_canvas(struct amvdec_session *sess)
+{
+	int i;
+
+	for (i = 0; i < sess->canvas_num; ++i)
+		meson_canvas_free(sess->core->canvas, sess->canvas_alloc[i]);
+
+	sess->canvas_num = 0;
+}
+
+static void vdec_reset_timestamps(struct amvdec_session *sess)
+{
+	struct amvdec_timestamp *tmp, *n;
+
+	list_for_each_entry_safe(tmp, n, &sess->timestamps, list) {
+		list_del(&tmp->list);
+		kfree(tmp);
+	}
+}
+
+static void vdec_reset_bufs_recycle(struct amvdec_session *sess)
+{
+	struct amvdec_buffer *tmp, *n;
+
+	list_for_each_entry_safe(tmp, n, &sess->bufs_recycle, list) {
+		list_del(&tmp->list);
+		kfree(tmp);
+	}
+}
+
+static void vdec_stop_streaming(struct vb2_queue *q)
+{
+	struct amvdec_session *sess = vb2_get_drv_priv(q);
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	struct amvdec_core *core = sess->core;
+	struct vb2_v4l2_buffer *buf;
+
+	if (sess->status == STATUS_RUNNING ||
+	    sess->status == STATUS_INIT ||
+	    (sess->status == STATUS_NEEDS_RESUME &&
+	     (!sess->streamon_out || !sess->streamon_cap))) {
+		if (vdec_codec_needs_recycle(sess))
+			kthread_stop(sess->recycle_thread);
+
+		vdec_poweroff(sess);
+		vdec_free_canvas(sess);
+		dma_free_coherent(sess->core->dev, sess->vififo_size,
+				  sess->vififo_vaddr, sess->vififo_paddr);
+		vdec_reset_timestamps(sess);
+		vdec_reset_bufs_recycle(sess);
+		kfree(sess->priv);
+		sess->priv = NULL;
+		core->cur_sess = NULL;
+		sess->status = STATUS_STOPPED;
+	}
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		while ((buf = v4l2_m2m_src_buf_remove(sess->m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+
+		sess->streamon_out = 0;
+	} else {
+		/* Drain remaining refs if was still running */
+		if (sess->status >= STATUS_RUNNING && codec_ops->drain)
+			codec_ops->drain(sess);
+
+		while ((buf = v4l2_m2m_dst_buf_remove(sess->m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+
+		sess->streamon_cap = 0;
+	}
+}
+
+static int vdec_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	vbuf->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static const struct vb2_ops vdec_vb2_ops = {
+	.queue_setup = vdec_queue_setup,
+	.start_streaming = vdec_start_streaming,
+	.stop_streaming = vdec_stop_streaming,
+	.buf_queue = vdec_vb2_buf_queue,
+	.buf_prepare = vdec_vb2_buf_prepare,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+static int
+vdec_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, "meson-vdec", sizeof(cap->driver));
+	strscpy(cap->card, "Amlogic Video Decoder", sizeof(cap->card));
+	strscpy(cap->bus_info, "platform:meson-vdec", sizeof(cap->bus_info));
+
+	return 0;
+}
+
+static const struct amvdec_format *
+find_format(const struct amvdec_format *fmts, u32 size, u32 pixfmt)
+{
+	unsigned int i;
+
+	for (i = 0; i < size; i++) {
+		if (fmts[i].pixfmt == pixfmt)
+			return &fmts[i];
+	}
+
+	return NULL;
+}
+
+static unsigned int
+vdec_supports_pixfmt_cap(const struct amvdec_format *fmt_out, u32 pixfmt_cap)
+{
+	int i;
+
+	for (i = 0; fmt_out->pixfmts_cap[i]; i++)
+		if (fmt_out->pixfmts_cap[i] == pixfmt_cap)
+			return 1;
+
+	return 0;
+}
+
+static const struct amvdec_format *
+vdec_try_fmt_common(struct amvdec_session *sess, u32 size,
+		    struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = pixmp->plane_fmt;
+	const struct amvdec_format *fmts = sess->core->platform->formats;
+	const struct amvdec_format *fmt_out = NULL;
+	u32 output_size = 0;
+
+	memset(pfmt[0].reserved, 0, sizeof(pfmt[0].reserved));
+	memset(pixmp->reserved, 0, sizeof(pixmp->reserved));
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		fmt_out = find_format(fmts, size, pixmp->pixelformat);
+		if (!fmt_out) {
+			pixmp->pixelformat = V4L2_PIX_FMT_MPEG2;
+			fmt_out = find_format(fmts, size, pixmp->pixelformat);
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		fmt_out = sess->fmt_out;
+		break;
+	default:
+		return NULL;
+	}
+
+	pixmp->width  = clamp(pixmp->width,  (u32)256, fmt_out->max_width);
+	pixmp->height = clamp(pixmp->height, (u32)144, fmt_out->max_height);
+	output_size = get_output_size(pixmp->width, pixmp->height);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (!pfmt[0].sizeimage)
+			pfmt[0].sizeimage = sess->src_buffer_size;
+		pfmt[0].bytesperline = 0;
+		pixmp->num_planes = 1;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt_out = sess->fmt_out;
+		if (!vdec_supports_pixfmt_cap(fmt_out, pixmp->pixelformat))
+			pixmp->pixelformat = fmt_out->pixfmts_cap[0];
+
+		memset(pfmt[1].reserved, 0, sizeof(pfmt[1].reserved));
+		if (pixmp->pixelformat == V4L2_PIX_FMT_NV12M) {
+			pfmt[0].sizeimage = output_size;
+			pfmt[0].bytesperline = ALIGN(pixmp->width, 32);
+
+			pfmt[1].sizeimage = output_size / 2;
+			pfmt[1].bytesperline = ALIGN(pixmp->width, 32);
+			pixmp->num_planes = 2;
+		} else if (pixmp->pixelformat == V4L2_PIX_FMT_YUV420M) {
+			pfmt[0].sizeimage = output_size;
+			pfmt[0].bytesperline = ALIGN(pixmp->width, 32);
+
+			pfmt[1].sizeimage = output_size / 4;
+			pfmt[1].bytesperline = ALIGN(pixmp->width, 32) / 2;
+
+			pfmt[2].sizeimage = output_size / 2;
+			pfmt[2].bytesperline = ALIGN(pixmp->width, 32) / 2;
+			pixmp->num_planes = 3;
+		}
+	}
+
+	if (pixmp->field == V4L2_FIELD_ANY)
+		pixmp->field = V4L2_FIELD_NONE;
+
+	return fmt_out;
+}
+
+static int vdec_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+
+	vdec_try_fmt_common(sess, sess->core->platform->num_formats, f);
+
+	return 0;
+}
+
+static int vdec_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		pixmp->pixelformat = sess->pixfmt_cap;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		pixmp->pixelformat = sess->fmt_out->pixfmt;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixmp->width = sess->width;
+		pixmp->height = sess->height;
+		pixmp->colorspace = sess->colorspace;
+		pixmp->ycbcr_enc = sess->ycbcr_enc;
+		pixmp->quantization = sess->quantization;
+		pixmp->xfer_func = sess->xfer_func;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixmp->width = sess->width;
+		pixmp->height = sess->height;
+	}
+
+	vdec_try_fmt_common(sess, sess->core->platform->num_formats, f);
+
+	return 0;
+}
+
+static int vdec_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	u32 num_formats = sess->core->platform->num_formats;
+	const struct amvdec_format *fmt_out;
+	struct v4l2_pix_format_mplane orig_pixmp;
+	struct v4l2_format format;
+	u32 pixfmt_out = 0, pixfmt_cap = 0;
+
+	orig_pixmp = *pixmp;
+
+	fmt_out = vdec_try_fmt_common(sess, num_formats, f);
+	if (!fmt_out)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixfmt_out = pixmp->pixelformat;
+		pixfmt_cap = sess->pixfmt_cap;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixfmt_cap = pixmp->pixelformat;
+		pixfmt_out = sess->fmt_out->pixfmt;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_out;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	vdec_try_fmt_common(sess, num_formats, &format);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		sess->width = format.fmt.pix_mp.width;
+		sess->height = format.fmt.pix_mp.height;
+		sess->colorspace = pixmp->colorspace;
+		sess->ycbcr_enc = pixmp->ycbcr_enc;
+		sess->quantization = pixmp->quantization;
+		sess->xfer_func = pixmp->xfer_func;
+		sess->src_buffer_size = pixmp->plane_fmt[0].sizeimage;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_cap;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	vdec_try_fmt_common(sess, num_formats, &format);
+
+	sess->width = format.fmt.pix_mp.width;
+	sess->height = format.fmt.pix_mp.height;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		sess->fmt_out = fmt_out;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		sess->pixfmt_cap = format.fmt.pix_mp.pixelformat;
+
+	return 0;
+}
+
+static int vdec_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+	const struct vdec_platform *platform = sess->core->platform;
+	const struct amvdec_format *fmt_out;
+
+	memset(f->reserved, 0, sizeof(f->reserved));
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (f->index >= platform->num_formats)
+			return -EINVAL;
+
+		fmt_out = &platform->formats[f->index];
+		f->pixelformat = fmt_out->pixfmt;
+		f->flags = fmt_out->flags;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt_out = sess->fmt_out;
+		if (f->index >= 4 || !fmt_out->pixfmts_cap[f->index])
+			return -EINVAL;
+
+		f->pixelformat = fmt_out->pixfmts_cap[f->index];
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vdec_enum_framesizes(struct file *file, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+	const struct amvdec_format *formats = sess->core->platform->formats;
+	const struct amvdec_format *fmt;
+	u32 num_formats = sess->core->platform->num_formats;
+
+	fmt = find_format(formats, num_formats, fsize->pixel_format);
+	if (!fmt || fsize->index)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+
+	fsize->stepwise.min_width = 256;
+	fsize->stepwise.max_width = fmt->max_width;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.min_height = 144;
+	fsize->stepwise.max_height = fmt->max_height;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int
+vdec_decoder_cmd(struct file *file, void *fh, struct v4l2_decoder_cmd *cmd)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	struct device *dev = sess->core->dev;
+	int ret;
+
+	ret = v4l2_m2m_ioctl_try_decoder_cmd(file, fh, cmd);
+	if (ret)
+		return ret;
+
+	if (!(sess->streamon_out & sess->streamon_cap))
+		return 0;
+
+	if (cmd->cmd == V4L2_DEC_CMD_START) {
+		v4l2_m2m_clear_state(sess->m2m_ctx);
+		sess->should_stop = 0;
+		return 0;
+	}
+
+	/* Should not happen */
+	if (cmd->cmd != V4L2_DEC_CMD_STOP)
+		return -EINVAL;
+
+	dev_dbg(dev, "Received V4L2_DEC_CMD_STOP\n");
+
+	sess->should_stop = 1;
+
+	v4l2_m2m_mark_stopped(sess->m2m_ctx);
+
+	if (codec_ops->drain) {
+		vdec_wait_inactive(sess);
+		codec_ops->drain(sess);
+	} else if (codec_ops->eos_sequence) {
+		u32 len;
+		const u8 *data = codec_ops->eos_sequence(&len);
+
+		esparser_queue_eos(sess->core, data, len);
+		vdec_wait_inactive(sess);
+	}
+
+	return ret;
+}
+
+static int vdec_subscribe_event(struct v4l2_fh *fh,
+				const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int vdec_g_pixelaspect(struct file *file, void *fh, int type,
+			      struct v4l2_fract *f)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	*f = sess->pixelaspect;
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops vdec_ioctl_ops = {
+	.vidioc_querycap = vdec_querycap,
+	.vidioc_enum_fmt_vid_cap = vdec_enum_fmt,
+	.vidioc_enum_fmt_vid_out = vdec_enum_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = vdec_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane = vdec_s_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = vdec_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane = vdec_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = vdec_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane = vdec_try_fmt,
+	.vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+	.vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+	.vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_streamon = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+	.vidioc_enum_framesizes = vdec_enum_framesizes,
+	.vidioc_subscribe_event = vdec_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_try_decoder_cmd = v4l2_m2m_ioctl_try_decoder_cmd,
+	.vidioc_decoder_cmd = vdec_decoder_cmd,
+	.vidioc_g_pixelaspect = vdec_g_pixelaspect,
+};
+
+static int m2m_queue_init(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq)
+{
+	struct amvdec_session *sess = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->ops = &vdec_vb2_ops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->drv_priv = sess;
+	src_vq->buf_struct_size = sizeof(struct dummy_buf);
+	src_vq->min_buffers_needed = 1;
+	src_vq->dev = sess->core->dev;
+	src_vq->lock = &sess->lock;
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->ops = &vdec_vb2_ops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->drv_priv = sess;
+	dst_vq->buf_struct_size = sizeof(struct dummy_buf);
+	dst_vq->min_buffers_needed = 1;
+	dst_vq->dev = sess->core->dev;
+	dst_vq->lock = &sess->lock;
+	return vb2_queue_init(dst_vq);
+}
+
+static int vdec_init_ctrls(struct amvdec_session *sess)
+{
+	struct v4l2_ctrl_handler *ctrl_handler = &sess->ctrl_handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_handler, 1);
+	if (ret)
+		return ret;
+
+	sess->ctrl_min_buf_capture =
+		v4l2_ctrl_new_std(ctrl_handler, NULL,
+				  V4L2_CID_MIN_BUFFERS_FOR_CAPTURE, 1, 32, 1,
+				  1);
+
+	ret = ctrl_handler->error;
+	if (ret) {
+		v4l2_ctrl_handler_free(ctrl_handler);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vdec_open(struct file *file)
+{
+	struct amvdec_core *core = video_drvdata(file);
+	struct device *dev = core->dev;
+	const struct amvdec_format *formats = core->platform->formats;
+	struct amvdec_session *sess;
+	int ret;
+
+	sess = kzalloc(sizeof(*sess), GFP_KERNEL);
+	if (!sess)
+		return -ENOMEM;
+
+	sess->core = core;
+
+	sess->m2m_dev = v4l2_m2m_init(&vdec_m2m_ops);
+	if (IS_ERR(sess->m2m_dev)) {
+		dev_err(dev, "Fail to v4l2_m2m_init\n");
+		ret = PTR_ERR(sess->m2m_dev);
+		goto err_free_sess;
+	}
+
+	sess->m2m_ctx = v4l2_m2m_ctx_init(sess->m2m_dev, sess, m2m_queue_init);
+	if (IS_ERR(sess->m2m_ctx)) {
+		dev_err(dev, "Fail to v4l2_m2m_ctx_init\n");
+		ret = PTR_ERR(sess->m2m_ctx);
+		goto err_m2m_release;
+	}
+
+	ret = vdec_init_ctrls(sess);
+	if (ret)
+		goto err_m2m_release;
+
+	sess->pixfmt_cap = formats[0].pixfmts_cap[0];
+	sess->fmt_out = &formats[0];
+	sess->width = 1280;
+	sess->height = 720;
+	sess->pixelaspect.numerator = 1;
+	sess->pixelaspect.denominator = 1;
+	sess->src_buffer_size = SZ_1M;
+
+	INIT_LIST_HEAD(&sess->timestamps);
+	INIT_LIST_HEAD(&sess->bufs_recycle);
+	INIT_WORK(&sess->esparser_queue_work, esparser_queue_all_src);
+	mutex_init(&sess->lock);
+	mutex_init(&sess->bufs_recycle_lock);
+	spin_lock_init(&sess->ts_spinlock);
+
+	v4l2_fh_init(&sess->fh, core->vdev_dec);
+	sess->fh.ctrl_handler = &sess->ctrl_handler;
+	v4l2_fh_add(&sess->fh);
+	sess->fh.m2m_ctx = sess->m2m_ctx;
+	file->private_data = &sess->fh;
+
+	return 0;
+
+err_m2m_release:
+	v4l2_m2m_release(sess->m2m_dev);
+err_free_sess:
+	kfree(sess);
+	return ret;
+}
+
+static int vdec_close(struct file *file)
+{
+	struct amvdec_session *sess =
+		container_of(file->private_data, struct amvdec_session, fh);
+
+	v4l2_m2m_ctx_release(sess->m2m_ctx);
+	v4l2_m2m_release(sess->m2m_dev);
+	v4l2_fh_del(&sess->fh);
+	v4l2_fh_exit(&sess->fh);
+
+	mutex_destroy(&sess->lock);
+	mutex_destroy(&sess->bufs_recycle_lock);
+
+	kfree(sess);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations vdec_fops = {
+	.owner = THIS_MODULE,
+	.open = vdec_open,
+	.release = vdec_close,
+	.unlocked_ioctl = video_ioctl2,
+	.poll = v4l2_m2m_fop_poll,
+	.mmap = v4l2_m2m_fop_mmap,
+};
+
+static irqreturn_t vdec_isr(int irq, void *data)
+{
+	struct amvdec_core *core = data;
+	struct amvdec_session *sess = core->cur_sess;
+
+	sess->last_irq_jiffies = get_jiffies_64();
+
+	return sess->fmt_out->codec_ops->isr(sess);
+}
+
+static irqreturn_t vdec_threaded_isr(int irq, void *data)
+{
+	struct amvdec_core *core = data;
+	struct amvdec_session *sess = core->cur_sess;
+
+	return sess->fmt_out->codec_ops->threaded_isr(sess);
+}
+
+static const struct of_device_id vdec_dt_match[] = {
+	{ .compatible = "amlogic,gxbb-vdec",
+	  .data = &vdec_platform_gxbb },
+	{ .compatible = "amlogic,gxm-vdec",
+	  .data = &vdec_platform_gxm },
+	{ .compatible = "amlogic,gxl-vdec",
+	  .data = &vdec_platform_gxl },
+	{ .compatible = "amlogic,g12a-vdec",
+	  .data = &vdec_platform_g12a },
+	{ .compatible = "amlogic,sm1-vdec",
+	  .data = &vdec_platform_sm1 },
+	{}
+};
+MODULE_DEVICE_TABLE(of, vdec_dt_match);
+
+static int vdec_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct video_device *vdev;
+	struct amvdec_core *core;
+	const struct of_device_id *of_id;
+	int irq;
+	int ret;
+
+	core = devm_kzalloc(dev, sizeof(*core), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+
+	core->dev = dev;
+	platform_set_drvdata(pdev, core);
+
+	core->dos_base = devm_platform_ioremap_resource_byname(pdev, "dos");
+	if (IS_ERR(core->dos_base))
+		return PTR_ERR(core->dos_base);
+
+	core->esparser_base = devm_platform_ioremap_resource_byname(pdev, "esparser");
+	if (IS_ERR(core->esparser_base))
+		return PTR_ERR(core->esparser_base);
+
+	core->regmap_ao =
+		syscon_regmap_lookup_by_phandle(dev->of_node,
+						"amlogic,ao-sysctrl");
+	if (IS_ERR(core->regmap_ao)) {
+		dev_err(dev, "Couldn't regmap AO sysctrl\n");
+		return PTR_ERR(core->regmap_ao);
+	}
+
+	core->canvas = meson_canvas_get(dev);
+	if (IS_ERR(core->canvas))
+		return PTR_ERR(core->canvas);
+
+	of_id = of_match_node(vdec_dt_match, dev->of_node);
+	core->platform = of_id->data;
+
+	if (core->platform->revision == VDEC_REVISION_G12A ||
+	    core->platform->revision == VDEC_REVISION_SM1) {
+		core->vdec_hevcf_clk = devm_clk_get(dev, "vdec_hevcf");
+		if (IS_ERR(core->vdec_hevcf_clk))
+			return -EPROBE_DEFER;
+	}
+
+	core->dos_parser_clk = devm_clk_get(dev, "dos_parser");
+	if (IS_ERR(core->dos_parser_clk))
+		return -EPROBE_DEFER;
+
+	core->dos_clk = devm_clk_get(dev, "dos");
+	if (IS_ERR(core->dos_clk))
+		return -EPROBE_DEFER;
+
+	core->vdec_1_clk = devm_clk_get(dev, "vdec_1");
+	if (IS_ERR(core->vdec_1_clk))
+		return -EPROBE_DEFER;
+
+	core->vdec_hevc_clk = devm_clk_get(dev, "vdec_hevc");
+	if (IS_ERR(core->vdec_hevc_clk))
+		return -EPROBE_DEFER;
+
+	irq = platform_get_irq_byname(pdev, "vdec");
+	if (irq < 0)
+		return irq;
+
+	ret = devm_request_threaded_irq(core->dev, irq, vdec_isr,
+					vdec_threaded_isr, IRQF_ONESHOT,
+					"vdec", core);
+	if (ret)
+		return ret;
+
+	ret = esparser_init(pdev, core);
+	if (ret)
+		return ret;
+
+	ret = v4l2_device_register(dev, &core->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "Couldn't register v4l2 device\n");
+		return -ENOMEM;
+	}
+
+	vdev = video_device_alloc();
+	if (!vdev) {
+		ret = -ENOMEM;
+		goto err_vdev_release;
+	}
+
+	core->vdev_dec = vdev;
+	core->dev_dec = dev;
+	mutex_init(&core->lock);
+
+	strscpy(vdev->name, "meson-video-decoder", sizeof(vdev->name));
+	vdev->release = video_device_release;
+	vdev->fops = &vdec_fops;
+	vdev->ioctl_ops = &vdec_ioctl_ops;
+	vdev->vfl_dir = VFL_DIR_M2M;
+	vdev->v4l2_dev = &core->v4l2_dev;
+	vdev->lock = &core->lock;
+	vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+
+	video_set_drvdata(vdev, core);
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(dev, "Failed registering video device\n");
+		goto err_vdev_release;
+	}
+
+	return 0;
+
+err_vdev_release:
+	video_device_release(vdev);
+	v4l2_device_unregister(&core->v4l2_dev);
+	return ret;
+}
+
+static int vdec_remove(struct platform_device *pdev)
+{
+	struct amvdec_core *core = platform_get_drvdata(pdev);
+
+	video_unregister_device(core->vdev_dec);
+	v4l2_device_unregister(&core->v4l2_dev);
+
+	return 0;
+}
+
+static struct platform_driver meson_vdec_driver = {
+	.probe = vdec_probe,
+	.remove = vdec_remove,
+	.driver = {
+		.name = "meson-vdec",
+		.of_match_table = vdec_dt_match,
+	},
+};
+module_platform_driver(meson_vdec_driver);
+
+MODULE_DESCRIPTION("Meson video decoder driver for GXBB/GXL/GXM/G12/SM1");
+MODULE_AUTHOR("Maxime Jourdan <mjourdan@baylibre.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/meson/vdec/vdec.h b/drivers/staging/media/meson/vdec/vdec.h
new file mode 100644
index 000000000..0906b8fb5
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec.h
@@ -0,0 +1,288 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_CORE_H_
+#define __MESON_VDEC_CORE_H_
+
+#include <linux/irqreturn.h>
+#include <linux/regmap.h>
+#include <linux/list.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <linux/soc/amlogic/meson-canvas.h>
+
+#include "vdec_platform.h"
+
+/* 32 buffers in 3-plane YUV420 */
+#define MAX_CANVAS (32 * 3)
+
+struct amvdec_buffer {
+	struct list_head list;
+	struct vb2_buffer *vb;
+};
+
+/**
+ * struct amvdec_timestamp - stores a src timestamp along with a VIFIFO offset
+ *
+ * @list: used to make lists out of this struct
+ * @tc: timecode from the v4l2 buffer
+ * @ts: timestamp from the VB2 buffer
+ * @offset: offset in the VIFIFO where the associated packet was written
+ * @flags: flags from the v4l2 buffer
+ * @used_count: times this timestamp was checked for a match with a dst buffer
+ */
+struct amvdec_timestamp {
+	struct list_head list;
+	struct v4l2_timecode tc;
+	u64 ts;
+	u32 offset;
+	u32 flags;
+	u32 used_count;
+};
+
+struct amvdec_session;
+
+/**
+ * struct amvdec_core - device parameters, singleton
+ *
+ * @dos_base: DOS memory base address
+ * @esparser_base: PARSER memory base address
+ * @regmap_ao: regmap for the AO bus
+ * @dev: core device
+ * @dev_dec: decoder device
+ * @platform: platform-specific data
+ * @canvas: canvas provider reference
+ * @dos_parser_clk: DOS_PARSER clock
+ * @dos_clk: DOS clock
+ * @vdec_1_clk: VDEC_1 clock
+ * @vdec_hevc_clk: VDEC_HEVC clock
+ * @vdec_hevcf_clk: VDEC_HEVCF clock
+ * @esparser_reset: RESET for the PARSER
+ * @vdev_dec: video device for the decoder
+ * @v4l2_dev: v4l2 device
+ * @cur_sess: current decoding session
+ * @lock: video device lock
+ */
+struct amvdec_core {
+	void __iomem *dos_base;
+	void __iomem *esparser_base;
+	struct regmap *regmap_ao;
+
+	struct device *dev;
+	struct device *dev_dec;
+	const struct vdec_platform *platform;
+
+	struct meson_canvas *canvas;
+
+	struct clk *dos_parser_clk;
+	struct clk *dos_clk;
+	struct clk *vdec_1_clk;
+	struct clk *vdec_hevc_clk;
+	struct clk *vdec_hevcf_clk;
+
+	struct reset_control *esparser_reset;
+
+	struct video_device *vdev_dec;
+	struct v4l2_device v4l2_dev;
+
+	struct amvdec_session *cur_sess;
+	struct mutex lock;
+};
+
+/**
+ * struct amvdec_ops - vdec operations
+ *
+ * @start: mandatory call when the vdec needs to initialize
+ * @stop: mandatory call when the vdec needs to stop
+ * @conf_esparser: mandatory call to let the vdec configure the ESPARSER
+ * @vififo_level: mandatory call to get the current amount of data
+ *		  in the VIFIFO
+ * @use_offsets: mandatory call. Returns 1 if the VDEC supports vififo offsets
+ */
+struct amvdec_ops {
+	int (*start)(struct amvdec_session *sess);
+	int (*stop)(struct amvdec_session *sess);
+	void (*conf_esparser)(struct amvdec_session *sess);
+	u32 (*vififo_level)(struct amvdec_session *sess);
+};
+
+/**
+ * struct amvdec_codec_ops - codec operations
+ *
+ * @start: mandatory call when the codec needs to initialize
+ * @stop: mandatory call when the codec needs to stop
+ * @load_extended_firmware: optional call to load additional firmware bits
+ * @num_pending_bufs: optional call to get the number of dst buffers on hold
+ * @can_recycle: optional call to know if the codec is ready to recycle
+ *		 a dst buffer
+ * @recycle: optional call to tell the codec to recycle a dst buffer. Must go
+ *	     in pair with @can_recycle
+ * @drain: optional call if the codec has a custom way of draining
+ * @resume: optional call to resume after a resolution change
+ * @eos_sequence: optional call to get an end sequence to send to esparser
+ *		  for flush. Mutually exclusive with @drain.
+ * @isr: mandatory call when the ISR triggers
+ * @threaded_isr: mandatory call for the threaded ISR
+ */
+struct amvdec_codec_ops {
+	int (*start)(struct amvdec_session *sess);
+	int (*stop)(struct amvdec_session *sess);
+	int (*load_extended_firmware)(struct amvdec_session *sess,
+				      const u8 *data, u32 len);
+	u32 (*num_pending_bufs)(struct amvdec_session *sess);
+	int (*can_recycle)(struct amvdec_core *core);
+	void (*recycle)(struct amvdec_core *core, u32 buf_idx);
+	void (*drain)(struct amvdec_session *sess);
+	void (*resume)(struct amvdec_session *sess);
+	const u8 * (*eos_sequence)(u32 *len);
+	irqreturn_t (*isr)(struct amvdec_session *sess);
+	irqreturn_t (*threaded_isr)(struct amvdec_session *sess);
+};
+
+/**
+ * struct amvdec_format - describes one of the OUTPUT (src) format supported
+ *
+ * @pixfmt: V4L2 pixel format
+ * @min_buffers: minimum amount of CAPTURE (dst) buffers
+ * @max_buffers: maximum amount of CAPTURE (dst) buffers
+ * @max_width: maximum picture width supported
+ * @max_height: maximum picture height supported
+ * @flags: enum flags associated with this pixfmt
+ * @vdec_ops: the VDEC operations that support this format
+ * @codec_ops: the codec operations that support this format
+ * @firmware_path: Path to the firmware that supports this format
+ * @pixfmts_cap: list of CAPTURE pixel formats available with pixfmt
+ */
+struct amvdec_format {
+	u32 pixfmt;
+	u32 min_buffers;
+	u32 max_buffers;
+	u32 max_width;
+	u32 max_height;
+	u32 flags;
+
+	struct amvdec_ops *vdec_ops;
+	struct amvdec_codec_ops *codec_ops;
+
+	char *firmware_path;
+	u32 pixfmts_cap[4];
+};
+
+enum amvdec_status {
+	STATUS_STOPPED,
+	STATUS_INIT,
+	STATUS_RUNNING,
+	STATUS_NEEDS_RESUME,
+};
+
+/**
+ * struct amvdec_session - decoding session parameters
+ *
+ * @core: reference to the vdec core struct
+ * @fh: v4l2 file handle
+ * @m2m_dev: v4l2 m2m device
+ * @m2m_ctx: v4l2 m2m context
+ * @ctrl_handler: V4L2 control handler
+ * @ctrl_min_buf_capture: V4L2 control V4L2_CID_MIN_BUFFERS_FOR_CAPTURE
+ * @lock: cap & out queues lock
+ * @fmt_out: vdec pixel format for the OUTPUT queue
+ * @pixfmt_cap: V4L2 pixel format for the CAPTURE queue
+ * @src_buffer_size: size in bytes of the OUTPUT buffers' only plane
+ * @width: current picture width
+ * @height: current picture height
+ * @colorspace: current colorspace
+ * @ycbcr_enc: current ycbcr_enc
+ * @quantization: current quantization
+ * @xfer_func: current transfer function
+ * @pixelaspect: Pixel Aspect Ratio reported by the decoder
+ * @esparser_queued_bufs: number of buffers currently queued into ESPARSER
+ * @esparser_queue_work: work struct for the ESPARSER to process src buffers
+ * @streamon_cap: stream on flag for capture queue
+ * @streamon_out: stream on flag for output queue
+ * @sequence_cap: capture sequence counter
+ * @sequence_out: output sequence counter
+ * @should_stop: flag set if userspace signaled EOS via command
+ *		 or empty buffer
+ * @keyframe_found: flag set once a keyframe has been parsed
+ * @num_dst_bufs: number of destination buffers
+ * @changed_format: the format changed
+ * @canvas_alloc: array of all the canvas IDs allocated
+ * @canvas_num: number of canvas IDs allocated
+ * @vififo_vaddr: virtual address for the VIFIFO
+ * @vififo_paddr: physical address for the VIFIFO
+ * @vififo_size: size of the VIFIFO dma alloc
+ * @bufs_recycle: list of buffers that need to be recycled
+ * @bufs_recycle_lock: lock for the bufs_recycle list
+ * @recycle_thread: task struct for the recycling thread
+ * @timestamps: chronological list of src timestamps
+ * @ts_spinlock: spinlock for the timestamps list
+ * @last_irq_jiffies: tracks last time the vdec triggered an IRQ
+ * @last_offset: tracks last offset of vififo
+ * @wrap_count: number of times the vififo wrapped around
+ * @fw_idx_to_vb2_idx: firmware buffer index to vb2 buffer index
+ * @status: current decoding status
+ * @priv: codec private data
+ */
+struct amvdec_session {
+	struct amvdec_core *core;
+
+	struct v4l2_fh fh;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct v4l2_m2m_ctx *m2m_ctx;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *ctrl_min_buf_capture;
+	struct mutex lock;
+
+	const struct amvdec_format *fmt_out;
+	u32 pixfmt_cap;
+	u32 src_buffer_size;
+
+	u32 width;
+	u32 height;
+	u32 colorspace;
+	u8 ycbcr_enc;
+	u8 quantization;
+	u8 xfer_func;
+
+	struct v4l2_fract pixelaspect;
+
+	atomic_t esparser_queued_bufs;
+	struct work_struct esparser_queue_work;
+
+	unsigned int streamon_cap, streamon_out;
+	unsigned int sequence_cap, sequence_out;
+	unsigned int should_stop;
+	unsigned int keyframe_found;
+	unsigned int num_dst_bufs;
+	unsigned int changed_format;
+
+	u8 canvas_alloc[MAX_CANVAS];
+	u32 canvas_num;
+
+	void *vififo_vaddr;
+	dma_addr_t vififo_paddr;
+	u32 vififo_size;
+
+	struct list_head bufs_recycle;
+	struct mutex bufs_recycle_lock; /* bufs_recycle list lock */
+	struct task_struct *recycle_thread;
+
+	struct list_head timestamps;
+	spinlock_t ts_spinlock; /* timestamp list lock */
+
+	u64 last_irq_jiffies;
+	u32 last_offset;
+	u32 wrap_count;
+	u32 fw_idx_to_vb2_idx[32];
+
+	enum amvdec_status status;
+	void *priv;
+};
+
+u32 amvdec_get_output_size(struct amvdec_session *sess);
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/vdec_1.c b/drivers/staging/media/meson/vdec/vdec_1.c
new file mode 100644
index 000000000..3fe2de0c9
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_1.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ *
+ * VDEC_1 is a video decoding block that allows decoding of
+ * MPEG 1/2/4, H.263, H.264, MJPEG, VC1
+ */
+
+#include <linux/firmware.h>
+#include <linux/clk.h>
+
+#include "vdec_1.h"
+#include "vdec_helpers.h"
+#include "dos_regs.h"
+
+/* AO Registers */
+#define AO_RTI_GEN_PWR_SLEEP0	0xe8
+#define AO_RTI_GEN_PWR_ISO0	0xec
+	#define GEN_PWR_VDEC_1 (BIT(3) | BIT(2))
+	#define GEN_PWR_VDEC_1_SM1 (BIT(1))
+
+#define MC_SIZE			(4096 * 4)
+
+static int
+vdec_1_load_firmware(struct amvdec_session *sess, const char *fwname)
+{
+	const struct firmware *fw;
+	struct amvdec_core *core = sess->core;
+	struct device *dev = core->dev_dec;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+	static void *mc_addr;
+	static dma_addr_t mc_addr_map;
+	int ret;
+	u32 i = 1000;
+
+	ret = request_firmware(&fw, fwname, dev);
+	if (ret < 0)
+		return -EINVAL;
+
+	if (fw->size < MC_SIZE) {
+		dev_err(dev, "Firmware size %zu is too small. Expected %u.\n",
+			fw->size, MC_SIZE);
+		ret = -EINVAL;
+		goto release_firmware;
+	}
+
+	mc_addr = dma_alloc_coherent(core->dev, MC_SIZE,
+				     &mc_addr_map, GFP_KERNEL);
+	if (!mc_addr) {
+		ret = -ENOMEM;
+		goto release_firmware;
+	}
+
+	memcpy(mc_addr, fw->data, MC_SIZE);
+
+	amvdec_write_dos(core, MPSR, 0);
+	amvdec_write_dos(core, CPSR, 0);
+
+	amvdec_clear_dos_bits(core, MDEC_PIC_DC_CTRL, BIT(31));
+
+	amvdec_write_dos(core, IMEM_DMA_ADR, mc_addr_map);
+	amvdec_write_dos(core, IMEM_DMA_COUNT, MC_SIZE / 4);
+	amvdec_write_dos(core, IMEM_DMA_CTRL, (0x8000 | (7 << 16)));
+
+	while (--i && amvdec_read_dos(core, IMEM_DMA_CTRL) & 0x8000);
+
+	if (i == 0) {
+		dev_err(dev, "Firmware load fail (DMA hang?)\n");
+		ret = -EINVAL;
+		goto free_mc;
+	}
+
+	if (codec_ops->load_extended_firmware)
+		ret = codec_ops->load_extended_firmware(sess,
+							fw->data + MC_SIZE,
+							fw->size - MC_SIZE);
+
+free_mc:
+	dma_free_coherent(core->dev, MC_SIZE, mc_addr, mc_addr_map);
+release_firmware:
+	release_firmware(fw);
+	return ret;
+}
+
+static int vdec_1_stbuf_power_up(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_CONTROL, 0);
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_WRAP_COUNT, 0);
+	amvdec_write_dos(core, POWER_CTL_VLD, BIT(4));
+
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_START_PTR, sess->vififo_paddr);
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_CURR_PTR, sess->vififo_paddr);
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_END_PTR,
+			 sess->vififo_paddr + sess->vififo_size - 8);
+
+	amvdec_write_dos_bits(core, VLD_MEM_VIFIFO_CONTROL, 1);
+	amvdec_clear_dos_bits(core, VLD_MEM_VIFIFO_CONTROL, 1);
+
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_BUF_CNTL, MEM_BUFCTRL_MANUAL);
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_WP, sess->vififo_paddr);
+
+	amvdec_write_dos_bits(core, VLD_MEM_VIFIFO_BUF_CNTL, 1);
+	amvdec_clear_dos_bits(core, VLD_MEM_VIFIFO_BUF_CNTL, 1);
+
+	amvdec_write_dos_bits(core, VLD_MEM_VIFIFO_CONTROL,
+			      (0x11 << MEM_FIFO_CNT_BIT) | MEM_FILL_ON_LEVEL |
+			      MEM_CTRL_FILL_EN | MEM_CTRL_EMPTY_EN);
+
+	return 0;
+}
+
+static void vdec_1_conf_esparser(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	/* VDEC_1 specific ESPARSER stuff */
+	amvdec_write_dos(core, DOS_GEN_CTRL0, 0);
+	amvdec_write_dos(core, VLD_MEM_VIFIFO_BUF_CNTL, 1);
+	amvdec_clear_dos_bits(core, VLD_MEM_VIFIFO_BUF_CNTL, 1);
+}
+
+static u32 vdec_1_vififo_level(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	return amvdec_read_dos(core, VLD_MEM_VIFIFO_LEVEL);
+}
+
+static int vdec_1_stop(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	amvdec_write_dos(core, MPSR, 0);
+	amvdec_write_dos(core, CPSR, 0);
+	amvdec_write_dos(core, ASSIST_MBOX1_MASK, 0);
+
+	amvdec_write_dos(core, DOS_SW_RESET0, BIT(12) | BIT(11));
+	amvdec_write_dos(core, DOS_SW_RESET0, 0);
+	amvdec_read_dos(core, DOS_SW_RESET0);
+
+	/* enable vdec1 isolation */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   GEN_PWR_VDEC_1_SM1, GEN_PWR_VDEC_1_SM1);
+	else
+		regmap_write(core->regmap_ao, AO_RTI_GEN_PWR_ISO0, 0xc0);
+	/* power off vdec1 memories */
+	amvdec_write_dos(core, DOS_MEM_PD_VDEC, 0xffffffff);
+	/* power off vdec1 */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_1_SM1, GEN_PWR_VDEC_1_SM1);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_1, GEN_PWR_VDEC_1);
+
+	clk_disable_unprepare(core->vdec_1_clk);
+
+	if (sess->priv)
+		codec_ops->stop(sess);
+
+	return 0;
+}
+
+static int vdec_1_start(struct amvdec_session *sess)
+{
+	int ret;
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	/* Configure the vdec clk to the maximum available */
+	clk_set_rate(core->vdec_1_clk, 666666666);
+	ret = clk_prepare_enable(core->vdec_1_clk);
+	if (ret)
+		return ret;
+
+	/* Enable power for VDEC_1 */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_1_SM1, 0);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_1, 0);
+	usleep_range(10, 20);
+
+	/* Reset VDEC1 */
+	amvdec_write_dos(core, DOS_SW_RESET0, 0xfffffffc);
+	amvdec_write_dos(core, DOS_SW_RESET0, 0x00000000);
+
+	amvdec_write_dos(core, DOS_GCLK_EN0, 0x3ff);
+
+	/* enable VDEC Memories */
+	amvdec_write_dos(core, DOS_MEM_PD_VDEC, 0);
+	/* Remove VDEC1 Isolation */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   GEN_PWR_VDEC_1_SM1, 0);
+	else
+		regmap_write(core->regmap_ao, AO_RTI_GEN_PWR_ISO0, 0);
+	/* Reset DOS top registers */
+	amvdec_write_dos(core, DOS_VDEC_MCRCC_STALL_CTRL, 0);
+
+	amvdec_write_dos(core, GCLK_EN, 0x3ff);
+	amvdec_clear_dos_bits(core, MDEC_PIC_DC_CTRL, BIT(31));
+
+	vdec_1_stbuf_power_up(sess);
+
+	ret = vdec_1_load_firmware(sess, sess->fmt_out->firmware_path);
+	if (ret)
+		goto stop;
+
+	ret = codec_ops->start(sess);
+	if (ret)
+		goto stop;
+
+	/* Enable IRQ */
+	amvdec_write_dos(core, ASSIST_MBOX1_CLR_REG, 1);
+	amvdec_write_dos(core, ASSIST_MBOX1_MASK, 1);
+
+	/* Enable 2-plane output */
+	if (sess->pixfmt_cap == V4L2_PIX_FMT_NV12M)
+		amvdec_write_dos_bits(core, MDEC_PIC_DC_CTRL, BIT(17));
+	else
+		amvdec_clear_dos_bits(core, MDEC_PIC_DC_CTRL, BIT(17));
+
+	/* Enable firmware processor */
+	amvdec_write_dos(core, MPSR, 1);
+	/* Let the firmware settle */
+	usleep_range(10, 20);
+
+	return 0;
+
+stop:
+	vdec_1_stop(sess);
+	return ret;
+}
+
+struct amvdec_ops vdec_1_ops = {
+	.start = vdec_1_start,
+	.stop = vdec_1_stop,
+	.conf_esparser = vdec_1_conf_esparser,
+	.vififo_level = vdec_1_vififo_level,
+};
diff --git a/drivers/staging/media/meson/vdec/vdec_1.h b/drivers/staging/media/meson/vdec/vdec_1.h
new file mode 100644
index 000000000..042d930c4
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_1.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_VDEC_1_H_
+#define __MESON_VDEC_VDEC_1_H_
+
+#include "vdec.h"
+
+extern struct amvdec_ops vdec_1_ops;
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/vdec_helpers.c b/drivers/staging/media/meson/vdec/vdec_helpers.c
new file mode 100644
index 000000000..7d2a75653
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_helpers.c
@@ -0,0 +1,480 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include <linux/gcd.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "vdec_helpers.h"
+
+#define NUM_CANVAS_NV12 2
+#define NUM_CANVAS_YUV420 3
+
+u32 amvdec_read_dos(struct amvdec_core *core, u32 reg)
+{
+	return readl_relaxed(core->dos_base + reg);
+}
+EXPORT_SYMBOL_GPL(amvdec_read_dos);
+
+void amvdec_write_dos(struct amvdec_core *core, u32 reg, u32 val)
+{
+	writel_relaxed(val, core->dos_base + reg);
+}
+EXPORT_SYMBOL_GPL(amvdec_write_dos);
+
+void amvdec_write_dos_bits(struct amvdec_core *core, u32 reg, u32 val)
+{
+	amvdec_write_dos(core, reg, amvdec_read_dos(core, reg) | val);
+}
+EXPORT_SYMBOL_GPL(amvdec_write_dos_bits);
+
+void amvdec_clear_dos_bits(struct amvdec_core *core, u32 reg, u32 val)
+{
+	amvdec_write_dos(core, reg, amvdec_read_dos(core, reg) & ~val);
+}
+EXPORT_SYMBOL_GPL(amvdec_clear_dos_bits);
+
+u32 amvdec_read_parser(struct amvdec_core *core, u32 reg)
+{
+	return readl_relaxed(core->esparser_base + reg);
+}
+EXPORT_SYMBOL_GPL(amvdec_read_parser);
+
+void amvdec_write_parser(struct amvdec_core *core, u32 reg, u32 val)
+{
+	writel_relaxed(val, core->esparser_base + reg);
+}
+EXPORT_SYMBOL_GPL(amvdec_write_parser);
+
+/* 4 KiB per 64x32 block */
+u32 amvdec_am21c_body_size(u32 width, u32 height)
+{
+	u32 width_64 = ALIGN(width, 64) / 64;
+	u32 height_32 = ALIGN(height, 32) / 32;
+
+	return SZ_4K * width_64 * height_32;
+}
+EXPORT_SYMBOL_GPL(amvdec_am21c_body_size);
+
+/* 32 bytes per 128x64 block */
+u32 amvdec_am21c_head_size(u32 width, u32 height)
+{
+	u32 width_128 = ALIGN(width, 128) / 128;
+	u32 height_64 = ALIGN(height, 64) / 64;
+
+	return 32 * width_128 * height_64;
+}
+EXPORT_SYMBOL_GPL(amvdec_am21c_head_size);
+
+u32 amvdec_am21c_size(u32 width, u32 height)
+{
+	return ALIGN(amvdec_am21c_body_size(width, height) +
+		     amvdec_am21c_head_size(width, height), SZ_64K);
+}
+EXPORT_SYMBOL_GPL(amvdec_am21c_size);
+
+static int canvas_alloc(struct amvdec_session *sess, u8 *canvas_id)
+{
+	int ret;
+
+	if (sess->canvas_num >= MAX_CANVAS) {
+		dev_err(sess->core->dev, "Reached max number of canvas\n");
+		return -ENOMEM;
+	}
+
+	ret = meson_canvas_alloc(sess->core->canvas, canvas_id);
+	if (ret)
+		return ret;
+
+	sess->canvas_alloc[sess->canvas_num++] = *canvas_id;
+	return 0;
+}
+
+static int set_canvas_yuv420m(struct amvdec_session *sess,
+			      struct vb2_buffer *vb, u32 width,
+			      u32 height, u32 reg)
+{
+	struct amvdec_core *core = sess->core;
+	u8 canvas_id[NUM_CANVAS_YUV420]; /* Y U V */
+	dma_addr_t buf_paddr[NUM_CANVAS_YUV420]; /* Y U V */
+	int ret, i;
+
+	for (i = 0; i < NUM_CANVAS_YUV420; ++i) {
+		ret = canvas_alloc(sess, &canvas_id[i]);
+		if (ret)
+			return ret;
+
+		buf_paddr[i] =
+		    vb2_dma_contig_plane_dma_addr(vb, i);
+	}
+
+	/* Y plane */
+	meson_canvas_config(core->canvas, canvas_id[0], buf_paddr[0],
+			    width, height, MESON_CANVAS_WRAP_NONE,
+			    MESON_CANVAS_BLKMODE_LINEAR,
+			    MESON_CANVAS_ENDIAN_SWAP64);
+
+	/* U plane */
+	meson_canvas_config(core->canvas, canvas_id[1], buf_paddr[1],
+			    width / 2, height / 2, MESON_CANVAS_WRAP_NONE,
+			    MESON_CANVAS_BLKMODE_LINEAR,
+			    MESON_CANVAS_ENDIAN_SWAP64);
+
+	/* V plane */
+	meson_canvas_config(core->canvas, canvas_id[2], buf_paddr[2],
+			    width / 2, height / 2, MESON_CANVAS_WRAP_NONE,
+			    MESON_CANVAS_BLKMODE_LINEAR,
+			    MESON_CANVAS_ENDIAN_SWAP64);
+
+	amvdec_write_dos(core, reg,
+			 ((canvas_id[2]) << 16) |
+			 ((canvas_id[1]) << 8)  |
+			 (canvas_id[0]));
+
+	return 0;
+}
+
+static int set_canvas_nv12m(struct amvdec_session *sess,
+			    struct vb2_buffer *vb, u32 width,
+			    u32 height, u32 reg)
+{
+	struct amvdec_core *core = sess->core;
+	u8 canvas_id[NUM_CANVAS_NV12]; /* Y U/V */
+	dma_addr_t buf_paddr[NUM_CANVAS_NV12]; /* Y U/V */
+	int ret, i;
+
+	for (i = 0; i < NUM_CANVAS_NV12; ++i) {
+		ret = canvas_alloc(sess, &canvas_id[i]);
+		if (ret)
+			return ret;
+
+		buf_paddr[i] =
+		    vb2_dma_contig_plane_dma_addr(vb, i);
+	}
+
+	/* Y plane */
+	meson_canvas_config(core->canvas, canvas_id[0], buf_paddr[0],
+			    width, height, MESON_CANVAS_WRAP_NONE,
+			    MESON_CANVAS_BLKMODE_LINEAR,
+			    MESON_CANVAS_ENDIAN_SWAP64);
+
+	/* U/V plane */
+	meson_canvas_config(core->canvas, canvas_id[1], buf_paddr[1],
+			    width, height / 2, MESON_CANVAS_WRAP_NONE,
+			    MESON_CANVAS_BLKMODE_LINEAR,
+			    MESON_CANVAS_ENDIAN_SWAP64);
+
+	amvdec_write_dos(core, reg,
+			 ((canvas_id[1]) << 16) |
+			 ((canvas_id[1]) << 8)  |
+			 (canvas_id[0]));
+
+	return 0;
+}
+
+int amvdec_set_canvases(struct amvdec_session *sess,
+			u32 reg_base[], u32 reg_num[])
+{
+	struct v4l2_m2m_buffer *buf;
+	u32 pixfmt = sess->pixfmt_cap;
+	u32 width = ALIGN(sess->width, 32);
+	u32 height = ALIGN(sess->height, 32);
+	u32 reg_cur;
+	u32 reg_num_cur = 0;
+	u32 reg_base_cur = 0;
+	int i = 0;
+	int ret;
+
+	v4l2_m2m_for_each_dst_buf(sess->m2m_ctx, buf) {
+		if (!reg_base[reg_base_cur])
+			return -EINVAL;
+
+		reg_cur = reg_base[reg_base_cur] + reg_num_cur * 4;
+
+		switch (pixfmt) {
+		case V4L2_PIX_FMT_NV12M:
+			ret = set_canvas_nv12m(sess, &buf->vb.vb2_buf, width,
+					       height, reg_cur);
+			if (ret)
+				return ret;
+			break;
+		case V4L2_PIX_FMT_YUV420M:
+			ret = set_canvas_yuv420m(sess, &buf->vb.vb2_buf, width,
+						 height, reg_cur);
+			if (ret)
+				return ret;
+			break;
+		default:
+			dev_err(sess->core->dev, "Unsupported pixfmt %08X\n",
+				pixfmt);
+			return -EINVAL;
+		}
+
+		reg_num_cur++;
+		if (reg_num_cur >= reg_num[reg_base_cur]) {
+			reg_base_cur++;
+			reg_num_cur = 0;
+		}
+
+		sess->fw_idx_to_vb2_idx[i++] = buf->vb.vb2_buf.index;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(amvdec_set_canvases);
+
+int amvdec_add_ts(struct amvdec_session *sess, u64 ts,
+		  struct v4l2_timecode tc, u32 offset, u32 vbuf_flags)
+{
+	struct amvdec_timestamp *new_ts;
+	unsigned long flags;
+
+	new_ts = kzalloc(sizeof(*new_ts), GFP_KERNEL);
+	if (!new_ts)
+		return -ENOMEM;
+
+	new_ts->ts = ts;
+	new_ts->tc = tc;
+	new_ts->offset = offset;
+	new_ts->flags = vbuf_flags;
+
+	spin_lock_irqsave(&sess->ts_spinlock, flags);
+	list_add_tail(&new_ts->list, &sess->timestamps);
+	spin_unlock_irqrestore(&sess->ts_spinlock, flags);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(amvdec_add_ts);
+
+void amvdec_remove_ts(struct amvdec_session *sess, u64 ts)
+{
+	struct amvdec_timestamp *tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sess->ts_spinlock, flags);
+	list_for_each_entry(tmp, &sess->timestamps, list) {
+		if (tmp->ts == ts) {
+			list_del(&tmp->list);
+			kfree(tmp);
+			goto unlock;
+		}
+	}
+	dev_warn(sess->core->dev_dec,
+		 "Couldn't remove buffer with timestamp %llu from list\n", ts);
+
+unlock:
+	spin_unlock_irqrestore(&sess->ts_spinlock, flags);
+}
+EXPORT_SYMBOL_GPL(amvdec_remove_ts);
+
+static void dst_buf_done(struct amvdec_session *sess,
+			 struct vb2_v4l2_buffer *vbuf,
+			 u32 field, u64 timestamp,
+			 struct v4l2_timecode timecode, u32 flags)
+{
+	struct device *dev = sess->core->dev_dec;
+	u32 output_size = amvdec_get_output_size(sess);
+
+	switch (sess->pixfmt_cap) {
+	case V4L2_PIX_FMT_NV12M:
+		vb2_set_plane_payload(&vbuf->vb2_buf, 0, output_size);
+		vb2_set_plane_payload(&vbuf->vb2_buf, 1, output_size / 2);
+		break;
+	case V4L2_PIX_FMT_YUV420M:
+		vb2_set_plane_payload(&vbuf->vb2_buf, 0, output_size);
+		vb2_set_plane_payload(&vbuf->vb2_buf, 1, output_size / 4);
+		vb2_set_plane_payload(&vbuf->vb2_buf, 2, output_size / 4);
+		break;
+	}
+
+	vbuf->vb2_buf.timestamp = timestamp;
+	vbuf->sequence = sess->sequence_cap++;
+	vbuf->flags = flags;
+	vbuf->timecode = timecode;
+
+	if (sess->should_stop &&
+	    atomic_read(&sess->esparser_queued_bufs) <= 1) {
+		const struct v4l2_event ev = { .type = V4L2_EVENT_EOS };
+
+		dev_dbg(dev, "Signaling EOS, sequence_cap = %u\n",
+			sess->sequence_cap - 1);
+		v4l2_event_queue_fh(&sess->fh, &ev);
+		vbuf->flags |= V4L2_BUF_FLAG_LAST;
+	} else if (sess->status == STATUS_NEEDS_RESUME) {
+		/* Mark LAST for drained show frames during a source change */
+		vbuf->flags |= V4L2_BUF_FLAG_LAST;
+		sess->sequence_cap = 0;
+	} else if (sess->should_stop)
+		dev_dbg(dev, "should_stop, %u bufs remain\n",
+			atomic_read(&sess->esparser_queued_bufs));
+
+	dev_dbg(dev, "Buffer %u done, ts = %llu, flags = %08X\n",
+		vbuf->vb2_buf.index, timestamp, flags);
+	vbuf->field = field;
+	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
+
+	/* Buffer done probably means the vififo got freed */
+	schedule_work(&sess->esparser_queue_work);
+}
+
+void amvdec_dst_buf_done(struct amvdec_session *sess,
+			 struct vb2_v4l2_buffer *vbuf, u32 field)
+{
+	struct device *dev = sess->core->dev_dec;
+	struct amvdec_timestamp *tmp;
+	struct list_head *timestamps = &sess->timestamps;
+	struct v4l2_timecode timecode;
+	u64 timestamp;
+	u32 vbuf_flags;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sess->ts_spinlock, flags);
+	if (list_empty(timestamps)) {
+		dev_err(dev, "Buffer %u done but list is empty\n",
+			vbuf->vb2_buf.index);
+
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&sess->ts_spinlock, flags);
+		return;
+	}
+
+	tmp = list_first_entry(timestamps, struct amvdec_timestamp, list);
+	timestamp = tmp->ts;
+	timecode = tmp->tc;
+	vbuf_flags = tmp->flags;
+	list_del(&tmp->list);
+	kfree(tmp);
+	spin_unlock_irqrestore(&sess->ts_spinlock, flags);
+
+	dst_buf_done(sess, vbuf, field, timestamp, timecode, vbuf_flags);
+	atomic_dec(&sess->esparser_queued_bufs);
+}
+EXPORT_SYMBOL_GPL(amvdec_dst_buf_done);
+
+void amvdec_dst_buf_done_offset(struct amvdec_session *sess,
+				struct vb2_v4l2_buffer *vbuf,
+				u32 offset, u32 field, bool allow_drop)
+{
+	struct device *dev = sess->core->dev_dec;
+	struct amvdec_timestamp *match = NULL;
+	struct amvdec_timestamp *tmp, *n;
+	struct v4l2_timecode timecode = { 0 };
+	u64 timestamp = 0;
+	u32 vbuf_flags = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sess->ts_spinlock, flags);
+
+	/* Look for our vififo offset to get the corresponding timestamp. */
+	list_for_each_entry_safe(tmp, n, &sess->timestamps, list) {
+		if (tmp->offset > offset) {
+			/*
+			 * Delete any record that remained unused for 32 match
+			 * checks
+			 */
+			if (tmp->used_count++ >= 32) {
+				list_del(&tmp->list);
+				kfree(tmp);
+			}
+			break;
+		}
+
+		match = tmp;
+	}
+
+	if (!match) {
+		dev_err(dev, "Buffer %u done but can't match offset (%08X)\n",
+			vbuf->vb2_buf.index, offset);
+	} else {
+		timestamp = match->ts;
+		timecode = match->tc;
+		vbuf_flags = match->flags;
+		list_del(&match->list);
+		kfree(match);
+	}
+	spin_unlock_irqrestore(&sess->ts_spinlock, flags);
+
+	dst_buf_done(sess, vbuf, field, timestamp, timecode, vbuf_flags);
+	if (match)
+		atomic_dec(&sess->esparser_queued_bufs);
+}
+EXPORT_SYMBOL_GPL(amvdec_dst_buf_done_offset);
+
+void amvdec_dst_buf_done_idx(struct amvdec_session *sess,
+			     u32 buf_idx, u32 offset, u32 field)
+{
+	struct vb2_v4l2_buffer *vbuf;
+	struct device *dev = sess->core->dev_dec;
+
+	vbuf = v4l2_m2m_dst_buf_remove_by_idx(sess->m2m_ctx,
+					      sess->fw_idx_to_vb2_idx[buf_idx]);
+
+	if (!vbuf) {
+		dev_err(dev,
+			"Buffer %u done but it doesn't exist in m2m_ctx\n",
+			buf_idx);
+		return;
+	}
+
+	if (offset != -1)
+		amvdec_dst_buf_done_offset(sess, vbuf, offset, field, true);
+	else
+		amvdec_dst_buf_done(sess, vbuf, field);
+}
+EXPORT_SYMBOL_GPL(amvdec_dst_buf_done_idx);
+
+void amvdec_set_par_from_dar(struct amvdec_session *sess,
+			     u32 dar_num, u32 dar_den)
+{
+	u32 div;
+
+	sess->pixelaspect.numerator = sess->height * dar_num;
+	sess->pixelaspect.denominator = sess->width * dar_den;
+	div = gcd(sess->pixelaspect.numerator, sess->pixelaspect.denominator);
+	sess->pixelaspect.numerator /= div;
+	sess->pixelaspect.denominator /= div;
+}
+EXPORT_SYMBOL_GPL(amvdec_set_par_from_dar);
+
+void amvdec_src_change(struct amvdec_session *sess, u32 width,
+		       u32 height, u32 dpb_size)
+{
+	static const struct v4l2_event ev = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION };
+
+	v4l2_ctrl_s_ctrl(sess->ctrl_min_buf_capture, dpb_size);
+
+	/*
+	 * Check if the capture queue is already configured well for our
+	 * usecase. If so, keep decoding with it and do not send the event
+	 */
+	if (sess->streamon_cap &&
+	    sess->width == width &&
+	    sess->height == height &&
+	    dpb_size <= sess->num_dst_bufs) {
+		sess->fmt_out->codec_ops->resume(sess);
+		return;
+	}
+
+	sess->changed_format = 0;
+	sess->width = width;
+	sess->height = height;
+	sess->status = STATUS_NEEDS_RESUME;
+
+	dev_dbg(sess->core->dev, "Res. changed (%ux%u), DPB size %u\n",
+		width, height, dpb_size);
+	v4l2_event_queue_fh(&sess->fh, &ev);
+}
+EXPORT_SYMBOL_GPL(amvdec_src_change);
+
+void amvdec_abort(struct amvdec_session *sess)
+{
+	dev_info(sess->core->dev, "Aborting decoding session!\n");
+	vb2_queue_error(&sess->m2m_ctx->cap_q_ctx.q);
+	vb2_queue_error(&sess->m2m_ctx->out_q_ctx.q);
+}
+EXPORT_SYMBOL_GPL(amvdec_abort);
diff --git a/drivers/staging/media/meson/vdec/vdec_helpers.h b/drivers/staging/media/meson/vdec/vdec_helpers.h
new file mode 100644
index 000000000..4bf3e61d0
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_helpers.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_HELPERS_H_
+#define __MESON_VDEC_HELPERS_H_
+
+#include "vdec.h"
+
+/**
+ * amvdec_set_canvases() - Map VB2 buffers to canvases
+ *
+ * @sess: current session
+ * @reg_base: Registry bases of where to write the canvas indexes
+ * @reg_num: number of contiguous registers after each reg_base (including it)
+ */
+int amvdec_set_canvases(struct amvdec_session *sess,
+			u32 reg_base[], u32 reg_num[]);
+
+/* Helpers to read/write to the various IPs (DOS, PARSER) */
+u32 amvdec_read_dos(struct amvdec_core *core, u32 reg);
+void amvdec_write_dos(struct amvdec_core *core, u32 reg, u32 val);
+void amvdec_write_dos_bits(struct amvdec_core *core, u32 reg, u32 val);
+void amvdec_clear_dos_bits(struct amvdec_core *core, u32 reg, u32 val);
+u32 amvdec_read_parser(struct amvdec_core *core, u32 reg);
+void amvdec_write_parser(struct amvdec_core *core, u32 reg, u32 val);
+
+u32 amvdec_am21c_body_size(u32 width, u32 height);
+u32 amvdec_am21c_head_size(u32 width, u32 height);
+u32 amvdec_am21c_size(u32 width, u32 height);
+
+/**
+ * amvdec_dst_buf_done_idx() - Signal that a buffer is done decoding
+ *
+ * @sess: current session
+ * @buf_idx: hardware buffer index
+ * @offset: VIFIFO bitstream offset corresponding to the buffer
+ * @field: V4L2 interlaced field
+ */
+void amvdec_dst_buf_done_idx(struct amvdec_session *sess, u32 buf_idx,
+			     u32 offset, u32 field);
+void amvdec_dst_buf_done(struct amvdec_session *sess,
+			 struct vb2_v4l2_buffer *vbuf, u32 field);
+void amvdec_dst_buf_done_offset(struct amvdec_session *sess,
+				struct vb2_v4l2_buffer *vbuf,
+				u32 offset, u32 field, bool allow_drop);
+
+/**
+ * amvdec_add_ts() - Add a timestamp to the list
+ *
+ * @sess: current session
+ * @ts: timestamp to add
+ * @tc: timecode to add
+ * @offset: offset in the VIFIFO where the associated packet was written
+ * @flags: the vb2_v4l2_buffer flags
+ */
+int amvdec_add_ts(struct amvdec_session *sess, u64 ts,
+		  struct v4l2_timecode tc, u32 offset, u32 flags);
+void amvdec_remove_ts(struct amvdec_session *sess, u64 ts);
+
+/**
+ * amvdec_set_par_from_dar() - Set Pixel Aspect Ratio from Display Aspect Ratio
+ *
+ * @sess: current session
+ * @dar_num: numerator of the DAR
+ * @dar_den: denominator of the DAR
+ */
+void amvdec_set_par_from_dar(struct amvdec_session *sess,
+			     u32 dar_num, u32 dar_den);
+
+/**
+ * amvdec_src_change() - Notify new resolution/DPB size to the core
+ *
+ * @sess: current session
+ * @width: picture width detected by the hardware
+ * @height: picture height detected by the hardware
+ * @dpb_size: Decoded Picture Buffer size (= amount of buffers for decoding)
+ */
+void amvdec_src_change(struct amvdec_session *sess, u32 width,
+		       u32 height, u32 dpb_size);
+
+/**
+ * amvdec_abort() - Abort the current decoding session
+ *
+ * @sess: current session
+ */
+void amvdec_abort(struct amvdec_session *sess);
+#endif
diff --git a/drivers/staging/media/meson/vdec/vdec_hevc.c b/drivers/staging/media/meson/vdec/vdec_hevc.c
new file mode 100644
index 000000000..afced435c
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_hevc.c
@@ -0,0 +1,235 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 Maxime Jourdan <maxi.jourdan@wanadoo.fr>
+ *
+ * VDEC_HEVC is a video decoding block that allows decoding of
+ * HEVC, VP9
+ */
+
+#include <linux/firmware.h>
+#include <linux/clk.h>
+
+#include "vdec_1.h"
+#include "vdec_helpers.h"
+#include "vdec_hevc.h"
+#include "hevc_regs.h"
+#include "dos_regs.h"
+
+/* AO Registers */
+#define AO_RTI_GEN_PWR_SLEEP0	0xe8
+#define AO_RTI_GEN_PWR_ISO0	0xec
+	#define GEN_PWR_VDEC_HEVC (BIT(7) | BIT(6))
+	#define GEN_PWR_VDEC_HEVC_SM1 (BIT(2))
+
+#define MC_SIZE	(4096 * 4)
+
+static int vdec_hevc_load_firmware(struct amvdec_session *sess,
+				   const char *fwname)
+{
+	struct amvdec_core *core = sess->core;
+	struct device *dev = core->dev_dec;
+	const struct firmware *fw;
+	static void *mc_addr;
+	static dma_addr_t mc_addr_map;
+	int ret;
+	u32 i = 100;
+
+	ret = request_firmware(&fw, fwname, dev);
+	if (ret < 0)  {
+		dev_err(dev, "Unable to request firmware %s\n", fwname);
+		return ret;
+	}
+
+	if (fw->size < MC_SIZE) {
+		dev_err(dev, "Firmware size %zu is too small. Expected %u.\n",
+			fw->size, MC_SIZE);
+		ret = -EINVAL;
+		goto release_firmware;
+	}
+
+	mc_addr = dma_alloc_coherent(core->dev, MC_SIZE, &mc_addr_map,
+				     GFP_KERNEL);
+	if (!mc_addr) {
+		ret = -ENOMEM;
+		goto release_firmware;
+	}
+
+	memcpy(mc_addr, fw->data, MC_SIZE);
+
+	amvdec_write_dos(core, HEVC_MPSR, 0);
+	amvdec_write_dos(core, HEVC_CPSR, 0);
+
+	amvdec_write_dos(core, HEVC_IMEM_DMA_ADR, mc_addr_map);
+	amvdec_write_dos(core, HEVC_IMEM_DMA_COUNT, MC_SIZE / 4);
+	amvdec_write_dos(core, HEVC_IMEM_DMA_CTRL, (0x8000 | (7 << 16)));
+
+	while (i && (readl(core->dos_base + HEVC_IMEM_DMA_CTRL) & 0x8000))
+		i--;
+
+	if (i == 0) {
+		dev_err(dev, "Firmware load fail (DMA hang?)\n");
+		ret = -ENODEV;
+	}
+
+	dma_free_coherent(core->dev, MC_SIZE, mc_addr, mc_addr_map);
+release_firmware:
+	release_firmware(fw);
+	return ret;
+}
+
+static void vdec_hevc_stbuf_init(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	amvdec_write_dos(core, HEVC_STREAM_CONTROL,
+			 amvdec_read_dos(core, HEVC_STREAM_CONTROL) & ~1);
+	amvdec_write_dos(core, HEVC_STREAM_START_ADDR, sess->vififo_paddr);
+	amvdec_write_dos(core, HEVC_STREAM_END_ADDR,
+			 sess->vififo_paddr + sess->vififo_size);
+	amvdec_write_dos(core, HEVC_STREAM_RD_PTR, sess->vififo_paddr);
+	amvdec_write_dos(core, HEVC_STREAM_WR_PTR, sess->vififo_paddr);
+}
+
+/* VDEC_HEVC specific ESPARSER configuration */
+static void vdec_hevc_conf_esparser(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+
+	/* set vififo_vbuf_rp_sel=>vdec_hevc */
+	amvdec_write_dos(core, DOS_GEN_CTRL0, 3 << 1);
+	amvdec_write_dos(core, HEVC_STREAM_CONTROL,
+			 amvdec_read_dos(core, HEVC_STREAM_CONTROL) | BIT(3));
+	amvdec_write_dos(core, HEVC_STREAM_CONTROL,
+			 amvdec_read_dos(core, HEVC_STREAM_CONTROL) | 1);
+	amvdec_write_dos(core, HEVC_STREAM_FIFO_CTL,
+			 amvdec_read_dos(core, HEVC_STREAM_FIFO_CTL) | BIT(29));
+}
+
+static u32 vdec_hevc_vififo_level(struct amvdec_session *sess)
+{
+	return readl_relaxed(sess->core->dos_base + HEVC_STREAM_LEVEL);
+}
+
+static int vdec_hevc_stop(struct amvdec_session *sess)
+{
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	/* Disable interrupt */
+	amvdec_write_dos(core, HEVC_ASSIST_MBOX1_MASK, 0);
+	/* Disable firmware processor */
+	amvdec_write_dos(core, HEVC_MPSR, 0);
+
+	if (sess->priv)
+		codec_ops->stop(sess);
+
+	/* Enable VDEC_HEVC Isolation */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   GEN_PWR_VDEC_HEVC_SM1,
+				   GEN_PWR_VDEC_HEVC_SM1);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   0xc00, 0xc00);
+
+	/* VDEC_HEVC Memories */
+	amvdec_write_dos(core, DOS_MEM_PD_HEVC, 0xffffffffUL);
+
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_HEVC_SM1,
+				   GEN_PWR_VDEC_HEVC_SM1);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_HEVC, GEN_PWR_VDEC_HEVC);
+
+	clk_disable_unprepare(core->vdec_hevc_clk);
+	if (core->platform->revision == VDEC_REVISION_G12A ||
+	    core->platform->revision == VDEC_REVISION_SM1)
+		clk_disable_unprepare(core->vdec_hevcf_clk);
+
+	return 0;
+}
+
+static int vdec_hevc_start(struct amvdec_session *sess)
+{
+	int ret;
+	struct amvdec_core *core = sess->core;
+	struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
+
+	if (core->platform->revision == VDEC_REVISION_G12A ||
+	    core->platform->revision == VDEC_REVISION_SM1) {
+		clk_set_rate(core->vdec_hevcf_clk, 666666666);
+		ret = clk_prepare_enable(core->vdec_hevcf_clk);
+		if (ret)
+			return ret;
+	}
+
+	clk_set_rate(core->vdec_hevc_clk, 666666666);
+	ret = clk_prepare_enable(core->vdec_hevc_clk);
+	if (ret) {
+		if (core->platform->revision == VDEC_REVISION_G12A ||
+		    core->platform->revision == VDEC_REVISION_SM1)
+			clk_disable_unprepare(core->vdec_hevcf_clk);
+		return ret;
+	}
+
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_HEVC_SM1, 0);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_SLEEP0,
+				   GEN_PWR_VDEC_HEVC, 0);
+	usleep_range(10, 20);
+
+	/* Reset VDEC_HEVC*/
+	amvdec_write_dos(core, DOS_SW_RESET3, 0xffffffff);
+	amvdec_write_dos(core, DOS_SW_RESET3, 0x00000000);
+
+	amvdec_write_dos(core, DOS_GCLK_EN3, 0xffffffff);
+
+	/* VDEC_HEVC Memories */
+	amvdec_write_dos(core, DOS_MEM_PD_HEVC, 0x00000000);
+
+	/* Remove VDEC_HEVC Isolation */
+	if (core->platform->revision == VDEC_REVISION_SM1)
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   GEN_PWR_VDEC_HEVC_SM1, 0);
+	else
+		regmap_update_bits(core->regmap_ao, AO_RTI_GEN_PWR_ISO0,
+				   0xc00, 0);
+
+	amvdec_write_dos(core, DOS_SW_RESET3, 0xffffffff);
+	amvdec_write_dos(core, DOS_SW_RESET3, 0x00000000);
+
+	vdec_hevc_stbuf_init(sess);
+
+	ret = vdec_hevc_load_firmware(sess, sess->fmt_out->firmware_path);
+	if (ret)
+		goto stop;
+
+	ret = codec_ops->start(sess);
+	if (ret)
+		goto stop;
+
+	amvdec_write_dos(core, DOS_SW_RESET3, BIT(12) | BIT(11));
+	amvdec_write_dos(core, DOS_SW_RESET3, 0);
+	amvdec_read_dos(core, DOS_SW_RESET3);
+
+	amvdec_write_dos(core, HEVC_MPSR, 1);
+	/* Let the firmware settle */
+	usleep_range(10, 20);
+
+	return 0;
+
+stop:
+	vdec_hevc_stop(sess);
+	return ret;
+}
+
+struct amvdec_ops vdec_hevc_ops = {
+	.start = vdec_hevc_start,
+	.stop = vdec_hevc_stop,
+	.conf_esparser = vdec_hevc_conf_esparser,
+	.vififo_level = vdec_hevc_vififo_level,
+};
diff --git a/drivers/staging/media/meson/vdec/vdec_hevc.h b/drivers/staging/media/meson/vdec/vdec_hevc.h
new file mode 100644
index 000000000..cd576a73a
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_hevc.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 Maxime Jourdan <maxi.jourdan@wanadoo.fr>
+ */
+
+#ifndef __MESON_VDEC_VDEC_HEVC_H_
+#define __MESON_VDEC_VDEC_HEVC_H_
+
+#include "vdec.h"
+
+extern struct amvdec_ops vdec_hevc_ops;
+
+#endif
diff --git a/drivers/staging/media/meson/vdec/vdec_platform.c b/drivers/staging/media/meson/vdec/vdec_platform.c
new file mode 100644
index 000000000..88c9d72e1
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_platform.c
@@ -0,0 +1,282 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#include "vdec_platform.h"
+#include "vdec.h"
+
+#include "vdec_1.h"
+#include "vdec_hevc.h"
+#include "codec_mpeg12.h"
+#include "codec_h264.h"
+#include "codec_vp9.h"
+
+static const struct amvdec_format vdec_formats_gxbb[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.min_buffers = 2,
+		.max_buffers = 24,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_h264_ops,
+		.firmware_path = "meson/vdec/gxbb_h264.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG1,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	},
+};
+
+static const struct amvdec_format vdec_formats_gxl[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.min_buffers = 16,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_hevc_ops,
+		.codec_ops = &codec_vp9_ops,
+		.firmware_path = "meson/vdec/gxl_vp9.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.min_buffers = 2,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_h264_ops,
+		.firmware_path = "meson/vdec/gxl_h264.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG1,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	},
+};
+
+static const struct amvdec_format vdec_formats_gxm[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.min_buffers = 16,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_hevc_ops,
+		.codec_ops = &codec_vp9_ops,
+		.firmware_path = "meson/vdec/gxl_vp9.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.min_buffers = 2,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_h264_ops,
+		.firmware_path = "meson/vdec/gxm_h264.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG1,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	},
+};
+
+static const struct amvdec_format vdec_formats_g12a[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.min_buffers = 16,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_hevc_ops,
+		.codec_ops = &codec_vp9_ops,
+		.firmware_path = "meson/vdec/g12a_vp9.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.min_buffers = 2,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_h264_ops,
+		.firmware_path = "meson/vdec/g12a_h264.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG1,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	},
+};
+
+static const struct amvdec_format vdec_formats_sm1[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.min_buffers = 16,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_hevc_ops,
+		.codec_ops = &codec_vp9_ops,
+		.firmware_path = "meson/vdec/sm1_vp9_mmu.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.min_buffers = 2,
+		.max_buffers = 24,
+		.max_width = 3840,
+		.max_height = 2160,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_h264_ops,
+		.firmware_path = "meson/vdec/g12a_h264.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED |
+			 V4L2_FMT_FLAG_DYN_RESOLUTION,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG1,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.min_buffers = 8,
+		.max_buffers = 8,
+		.max_width = 1920,
+		.max_height = 1080,
+		.vdec_ops = &vdec_1_ops,
+		.codec_ops = &codec_mpeg12_ops,
+		.firmware_path = "meson/vdec/gxl_mpeg12.bin",
+		.pixfmts_cap = { V4L2_PIX_FMT_NV12M, V4L2_PIX_FMT_YUV420M, 0 },
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+	},
+};
+
+const struct vdec_platform vdec_platform_gxbb = {
+	.formats = vdec_formats_gxbb,
+	.num_formats = ARRAY_SIZE(vdec_formats_gxbb),
+	.revision = VDEC_REVISION_GXBB,
+};
+
+const struct vdec_platform vdec_platform_gxl = {
+	.formats = vdec_formats_gxl,
+	.num_formats = ARRAY_SIZE(vdec_formats_gxl),
+	.revision = VDEC_REVISION_GXL,
+};
+
+const struct vdec_platform vdec_platform_gxm = {
+	.formats = vdec_formats_gxm,
+	.num_formats = ARRAY_SIZE(vdec_formats_gxm),
+	.revision = VDEC_REVISION_GXM,
+};
+
+const struct vdec_platform vdec_platform_g12a = {
+	.formats = vdec_formats_g12a,
+	.num_formats = ARRAY_SIZE(vdec_formats_g12a),
+	.revision = VDEC_REVISION_G12A,
+};
+
+const struct vdec_platform vdec_platform_sm1 = {
+	.formats = vdec_formats_sm1,
+	.num_formats = ARRAY_SIZE(vdec_formats_sm1),
+	.revision = VDEC_REVISION_SM1,
+};
diff --git a/drivers/staging/media/meson/vdec/vdec_platform.h b/drivers/staging/media/meson/vdec/vdec_platform.h
new file mode 100644
index 000000000..731877a77
--- /dev/null
+++ b/drivers/staging/media/meson/vdec/vdec_platform.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 BayLibre, SAS
+ * Author: Maxime Jourdan <mjourdan@baylibre.com>
+ */
+
+#ifndef __MESON_VDEC_PLATFORM_H_
+#define __MESON_VDEC_PLATFORM_H_
+
+#include "vdec.h"
+
+struct amvdec_format;
+
+enum vdec_revision {
+	VDEC_REVISION_GXBB,
+	VDEC_REVISION_GXL,
+	VDEC_REVISION_GXM,
+	VDEC_REVISION_G12A,
+	VDEC_REVISION_SM1,
+};
+
+struct vdec_platform {
+	const struct amvdec_format *formats;
+	const u32 num_formats;
+	enum vdec_revision revision;
+};
+
+extern const struct vdec_platform vdec_platform_gxbb;
+extern const struct vdec_platform vdec_platform_gxm;
+extern const struct vdec_platform vdec_platform_gxl;
+extern const struct vdec_platform vdec_platform_g12a;
+extern const struct vdec_platform vdec_platform_sm1;
+
+#endif
diff --git a/drivers/staging/media/omap4iss/Kconfig b/drivers/staging/media/omap4iss/Kconfig
new file mode 100644
index 000000000..6d1f55b09
--- /dev/null
+++ b/drivers/staging/media/omap4iss/Kconfig
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config VIDEO_OMAP4
+	tristate "OMAP 4 Camera support"
+	depends on VIDEO_DEV  && I2C
+	depends on ARCH_OMAP4 || COMPILE_TEST
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select MFD_SYSCON
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  Driver for an OMAP 4 ISS controller.
diff --git a/drivers/staging/media/omap4iss/Makefile b/drivers/staging/media/omap4iss/Makefile
new file mode 100644
index 000000000..e64d489a4
--- /dev/null
+++ b/drivers/staging/media/omap4iss/Makefile
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for OMAP4 ISS driver
+#
+
+omap4-iss-objs += \
+	iss.o iss_csi2.o iss_csiphy.o iss_ipipeif.o iss_ipipe.o iss_resizer.o iss_video.o
+
+obj-$(CONFIG_VIDEO_OMAP4) += omap4-iss.o
diff --git a/drivers/staging/media/omap4iss/TODO b/drivers/staging/media/omap4iss/TODO
new file mode 100644
index 000000000..4d220ef82
--- /dev/null
+++ b/drivers/staging/media/omap4iss/TODO
@@ -0,0 +1,3 @@
+* Fix FIFO/buffer overflows and underflows
+* Replace dummy resizer code with a real implementation
+* Fix checkpatch errors and warnings
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
new file mode 100644
index 000000000..fa2a36d82
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -0,0 +1,1356 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver
+ *
+ * Copyright (C) 2012, Texas Instruments
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+
+#define ISS_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISS " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_##name))
+
+static void iss_print_status(struct iss_device *iss)
+{
+	dev_dbg(iss->dev, "-------------ISS HL Register dump-------------\n");
+
+	ISS_PRINT_REGISTER(iss, HL_REVISION);
+	ISS_PRINT_REGISTER(iss, HL_SYSCONFIG);
+	ISS_PRINT_REGISTER(iss, HL_IRQSTATUS(5));
+	ISS_PRINT_REGISTER(iss, HL_IRQENABLE_SET(5));
+	ISS_PRINT_REGISTER(iss, HL_IRQENABLE_CLR(5));
+	ISS_PRINT_REGISTER(iss, CTRL);
+	ISS_PRINT_REGISTER(iss, CLKCTRL);
+	ISS_PRINT_REGISTER(iss, CLKSTAT);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * omap4iss_flush - Post pending L3 bus writes by doing a register readback
+ * @iss: OMAP4 ISS device
+ *
+ * In order to force posting of pending writes, we need to write and
+ * readback the same register, in this case the revision register.
+ *
+ * See this link for reference:
+ *   https://www.mail-archive.com/linux-omap@vger.kernel.org/msg08149.html
+ */
+static void omap4iss_flush(struct iss_device *iss)
+{
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION, 0);
+	iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION);
+}
+
+/*
+ * iss_isp_enable_interrupts - Enable ISS ISP interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void omap4iss_isp_enable_interrupts(struct iss_device *iss)
+{
+	static const u32 isp_irq = ISP5_IRQ_OCP_ERR |
+				   ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+				   ISP5_IRQ_RSZ_FIFO_OVF |
+				   ISP5_IRQ_RSZ_INT_DMA |
+				   ISP5_IRQ_ISIF_INT(0);
+
+	/* Enable ISP interrupts */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQSTATUS(0), isp_irq);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQENABLE_SET(0),
+		      isp_irq);
+}
+
+/*
+ * iss_isp_disable_interrupts - Disable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void omap4iss_isp_disable_interrupts(struct iss_device *iss)
+{
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQENABLE_CLR(0), ~0);
+}
+
+/*
+ * iss_enable_interrupts - Enable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void iss_enable_interrupts(struct iss_device *iss)
+{
+	static const u32 hl_irq = ISS_HL_IRQ_CSIA | ISS_HL_IRQ_CSIB
+				| ISS_HL_IRQ_ISP(0);
+
+	/* Enable HL interrupts */
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5), hl_irq);
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQENABLE_SET(5), hl_irq);
+
+	if (iss->regs[OMAP4_ISS_MEM_ISP_SYS1])
+		omap4iss_isp_enable_interrupts(iss);
+}
+
+/*
+ * iss_disable_interrupts - Disable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void iss_disable_interrupts(struct iss_device *iss)
+{
+	if (iss->regs[OMAP4_ISS_MEM_ISP_SYS1])
+		omap4iss_isp_disable_interrupts(iss);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQENABLE_CLR(5), ~0);
+}
+
+int omap4iss_get_external_info(struct iss_pipeline *pipe,
+			       struct media_link *link)
+{
+	struct iss_device *iss =
+		container_of(pipe, struct iss_video, pipe)->iss;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	if (!pipe->external)
+		return 0;
+
+	if (pipe->external_rate)
+		return 0;
+
+	memset(&fmt, 0, sizeof(fmt));
+
+	fmt.pad = link->source->index;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(media_entity_to_v4l2_subdev(link->sink->entity),
+			       pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return -EPIPE;
+
+	pipe->external_bpp = omap4iss_video_format_info(fmt.format.code)->bpp;
+
+	ctrl = v4l2_ctrl_find(pipe->external->ctrl_handler,
+			      V4L2_CID_PIXEL_RATE);
+	if (!ctrl) {
+		dev_warn(iss->dev, "no pixel rate control in subdev %s\n",
+			 pipe->external->name);
+		return -EPIPE;
+	}
+
+	pipe->external_rate = v4l2_ctrl_g_ctrl_int64(ctrl);
+
+	return 0;
+}
+
+/*
+ * Configure the bridge. Valid inputs are
+ *
+ * IPIPEIF_INPUT_CSI2A: CSI2a receiver
+ * IPIPEIF_INPUT_CSI2B: CSI2b receiver
+ *
+ * The bridge and lane shifter are configured according to the selected input
+ * and the ISP platform data.
+ */
+void omap4iss_configure_bridge(struct iss_device *iss,
+			       enum ipipeif_input_entity input)
+{
+	u32 issctrl_val;
+	u32 isp5ctrl_val;
+
+	issctrl_val = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_CTRL);
+	issctrl_val &= ~ISS_CTRL_INPUT_SEL_MASK;
+	issctrl_val &= ~ISS_CTRL_CLK_DIV_MASK;
+
+	isp5ctrl_val = iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL);
+
+	switch (input) {
+	case IPIPEIF_INPUT_CSI2A:
+		issctrl_val |= ISS_CTRL_INPUT_SEL_CSI2A;
+		break;
+
+	case IPIPEIF_INPUT_CSI2B:
+		issctrl_val |= ISS_CTRL_INPUT_SEL_CSI2B;
+		break;
+
+	default:
+		return;
+	}
+
+	issctrl_val |= ISS_CTRL_SYNC_DETECT_VS_RAISING;
+
+	isp5ctrl_val |= ISP5_CTRL_VD_PULSE_EXT | ISP5_CTRL_PSYNC_CLK_SEL |
+			ISP5_CTRL_SYNC_ENABLE;
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_CTRL, issctrl_val);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL, isp5ctrl_val);
+}
+
+#ifdef ISS_ISR_DEBUG
+static void iss_isr_dbg(struct iss_device *iss, u32 irqstatus)
+{
+	static const char * const name[] = {
+		"ISP_0",
+		"ISP_1",
+		"ISP_2",
+		"ISP_3",
+		"CSIA",
+		"CSIB",
+		"CCP2_0",
+		"CCP2_1",
+		"CCP2_2",
+		"CCP2_3",
+		"CBUFF",
+		"BTE",
+		"SIMCOP_0",
+		"SIMCOP_1",
+		"SIMCOP_2",
+		"SIMCOP_3",
+		"CCP2_8",
+		"HS_VS",
+		"18",
+		"19",
+		"20",
+		"21",
+		"22",
+		"23",
+		"24",
+		"25",
+		"26",
+		"27",
+		"28",
+		"29",
+		"30",
+		"31",
+	};
+	unsigned int i;
+
+	dev_dbg(iss->dev, "ISS IRQ: ");
+
+	for (i = 0; i < ARRAY_SIZE(name); i++) {
+		if ((1 << i) & irqstatus)
+			pr_cont("%s ", name[i]);
+	}
+	pr_cont("\n");
+}
+
+static void iss_isp_isr_dbg(struct iss_device *iss, u32 irqstatus)
+{
+	static const char * const name[] = {
+		"ISIF_0",
+		"ISIF_1",
+		"ISIF_2",
+		"ISIF_3",
+		"IPIPEREQ",
+		"IPIPELAST_PIX",
+		"IPIPEDMA",
+		"IPIPEBSC",
+		"IPIPEHST",
+		"IPIPEIF",
+		"AEW",
+		"AF",
+		"H3A",
+		"RSZ_REG",
+		"RSZ_LAST_PIX",
+		"RSZ_DMA",
+		"RSZ_CYC_RZA",
+		"RSZ_CYC_RZB",
+		"RSZ_FIFO_OVF",
+		"RSZ_FIFO_IN_BLK_ERR",
+		"20",
+		"21",
+		"RSZ_EOF0",
+		"RSZ_EOF1",
+		"H3A_EOF",
+		"IPIPE_EOF",
+		"26",
+		"IPIPE_DPC_INI",
+		"IPIPE_DPC_RNEW0",
+		"IPIPE_DPC_RNEW1",
+		"30",
+		"OCP_ERR",
+	};
+	unsigned int i;
+
+	dev_dbg(iss->dev, "ISP IRQ: ");
+
+	for (i = 0; i < ARRAY_SIZE(name); i++) {
+		if ((1 << i) & irqstatus)
+			pr_cont("%s ", name[i]);
+	}
+	pr_cont("\n");
+}
+#endif
+
+/*
+ * iss_isr - Interrupt Service Routine for ISS module.
+ * @irq: Not used currently.
+ * @_iss: Pointer to the OMAP4 ISS device
+ *
+ * Handles the corresponding callback if plugged in.
+ *
+ * Returns IRQ_HANDLED when IRQ was correctly handled, or IRQ_NONE when the
+ * IRQ wasn't handled.
+ */
+static irqreturn_t iss_isr(int irq, void *_iss)
+{
+	static const u32 ipipeif_events = ISP5_IRQ_IPIPEIF_IRQ |
+					  ISP5_IRQ_ISIF_INT(0);
+	static const u32 resizer_events = ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+					  ISP5_IRQ_RSZ_FIFO_OVF |
+					  ISP5_IRQ_RSZ_INT_DMA;
+	struct iss_device *iss = _iss;
+	u32 irqstatus;
+
+	irqstatus = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5));
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5), irqstatus);
+
+	if (irqstatus & ISS_HL_IRQ_CSIA)
+		omap4iss_csi2_isr(&iss->csi2a);
+
+	if (irqstatus & ISS_HL_IRQ_CSIB)
+		omap4iss_csi2_isr(&iss->csi2b);
+
+	if (irqstatus & ISS_HL_IRQ_ISP(0)) {
+		u32 isp_irqstatus = iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1,
+						 ISP5_IRQSTATUS(0));
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQSTATUS(0),
+			      isp_irqstatus);
+
+		if (isp_irqstatus & ISP5_IRQ_OCP_ERR)
+			dev_dbg(iss->dev, "ISP5 OCP Error!\n");
+
+		if (isp_irqstatus & ipipeif_events) {
+			omap4iss_ipipeif_isr(&iss->ipipeif,
+					     isp_irqstatus & ipipeif_events);
+		}
+
+		if (isp_irqstatus & resizer_events)
+			omap4iss_resizer_isr(&iss->resizer,
+					     isp_irqstatus & resizer_events);
+
+#ifdef ISS_ISR_DEBUG
+		iss_isp_isr_dbg(iss, isp_irqstatus);
+#endif
+	}
+
+	omap4iss_flush(iss);
+
+#ifdef ISS_ISR_DEBUG
+	iss_isr_dbg(iss, irqstatus);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+static const struct media_device_ops iss_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+/* -----------------------------------------------------------------------------
+ * Pipeline stream management
+ */
+
+/*
+ * iss_pipeline_disable - Disable streaming on a pipeline
+ * @pipe: ISS pipeline
+ * @until: entity at which to stop pipeline walk
+ *
+ * Walk the entities chain starting at the pipeline output video node and stop
+ * all modules in the chain. Wait synchronously for the modules to be stopped if
+ * necessary.
+ *
+ * If the until argument isn't NULL, stop the pipeline walk when reaching the
+ * until entity. This is used to disable a partially started pipeline due to a
+ * subdev start error.
+ */
+static int iss_pipeline_disable(struct iss_pipeline *pipe,
+				struct media_entity *until)
+{
+	struct iss_device *iss = pipe->output->iss;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int failure = 0;
+	int ret;
+
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		if (entity == until)
+			break;
+
+		subdev = media_entity_to_v4l2_subdev(entity);
+		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
+		if (ret < 0) {
+			dev_warn(iss->dev, "%s: module stop timeout.\n",
+				 subdev->name);
+			/* If the entity failed to stopped, assume it has
+			 * crashed. Mark it as such, the ISS will be reset when
+			 * applications will release it.
+			 */
+			media_entity_enum_set(&iss->crashed, &subdev->entity);
+			failure = -ETIMEDOUT;
+		}
+	}
+
+	return failure;
+}
+
+/*
+ * iss_pipeline_enable - Enable streaming on a pipeline
+ * @pipe: ISS pipeline
+ * @mode: Stream mode (single shot or continuous)
+ *
+ * Walk the entities chain starting at the pipeline output video node and start
+ * all modules in the chain in the given mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise.
+ */
+static int iss_pipeline_enable(struct iss_pipeline *pipe,
+			       enum iss_pipeline_stream_state mode)
+{
+	struct iss_device *iss = pipe->output->iss;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	unsigned long flags;
+	int ret;
+
+	/* If one of the entities in the pipeline has crashed it will not work
+	 * properly. Refuse to start streaming in that case. This check must be
+	 * performed before the loop below to avoid starting entities if the
+	 * pipeline won't start anyway (those entities would then likely fail to
+	 * stop, making the problem worse).
+	 */
+	if (media_entity_enum_intersects(&pipe->ent_enum, &iss->crashed))
+		return -EIO;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~(ISS_PIPELINE_IDLE_INPUT | ISS_PIPELINE_IDLE_OUTPUT);
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	pipe->do_propagation = false;
+
+	mutex_lock(&iss->media_dev.graph_mutex);
+
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, mode);
+		if (ret < 0 && ret != -ENOIOCTLCMD) {
+			iss_pipeline_disable(pipe, entity);
+			mutex_unlock(&iss->media_dev.graph_mutex);
+			return ret;
+		}
+
+		if (subdev == &iss->csi2a.subdev ||
+		    subdev == &iss->csi2b.subdev)
+			pipe->do_propagation = true;
+	}
+
+	mutex_unlock(&iss->media_dev.graph_mutex);
+	iss_print_status(pipe->output->iss);
+
+	return 0;
+}
+
+/*
+ * omap4iss_pipeline_set_stream - Enable/disable streaming on a pipeline
+ * @pipe: ISS pipeline
+ * @state: Stream state (stopped, single shot or continuous)
+ *
+ * Set the pipeline to the given stream state. Pipelines can be started in
+ * single-shot or continuous mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise. The pipeline state is not updated when the operation
+ * fails, except when stopping the pipeline.
+ */
+int omap4iss_pipeline_set_stream(struct iss_pipeline *pipe,
+				 enum iss_pipeline_stream_state state)
+{
+	int ret;
+
+	if (state == ISS_PIPELINE_STREAM_STOPPED)
+		ret = iss_pipeline_disable(pipe, NULL);
+	else
+		ret = iss_pipeline_enable(pipe, state);
+
+	if (ret == 0 || state == ISS_PIPELINE_STREAM_STOPPED)
+		pipe->stream_state = state;
+
+	return ret;
+}
+
+/*
+ * omap4iss_pipeline_cancel_stream - Cancel stream on a pipeline
+ * @pipe: ISS pipeline
+ *
+ * Cancelling a stream mark all buffers on all video nodes in the pipeline as
+ * erroneous and makes sure no new buffer can be queued. This function is called
+ * when a fatal error that prevents any further operation on the pipeline
+ * occurs.
+ */
+void omap4iss_pipeline_cancel_stream(struct iss_pipeline *pipe)
+{
+	if (pipe->input)
+		omap4iss_video_cancel_stream(pipe->input);
+	if (pipe->output)
+		omap4iss_video_cancel_stream(pipe->output);
+}
+
+/*
+ * iss_pipeline_is_last - Verify if entity has an enabled link to the output
+ *			  video node
+ * @me: ISS module's media entity
+ *
+ * Returns 1 if the entity has an enabled link to the output video node or 0
+ * otherwise. It's true only while pipeline can have no more than one output
+ * node.
+ */
+static int iss_pipeline_is_last(struct media_entity *me)
+{
+	struct iss_pipeline *pipe;
+	struct media_pad *pad;
+
+	pipe = to_iss_pipeline(me);
+	if (!pipe || pipe->stream_state == ISS_PIPELINE_STREAM_STOPPED)
+		return 0;
+	pad = media_pad_remote_pad_first(&pipe->output->pad);
+	return pad->entity == me;
+}
+
+static int iss_reset(struct iss_device *iss)
+{
+	unsigned int timeout;
+
+	iss_reg_set(iss, OMAP4_ISS_MEM_TOP, ISS_HL_SYSCONFIG,
+		    ISS_HL_SYSCONFIG_SOFTRESET);
+
+	timeout = iss_poll_condition_timeout(
+		!(iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_SYSCONFIG) &
+		ISS_HL_SYSCONFIG_SOFTRESET), 1000, 10, 100);
+	if (timeout) {
+		dev_err(iss->dev, "ISS reset timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	media_entity_enum_zero(&iss->crashed);
+
+	return 0;
+}
+
+static int iss_isp_reset(struct iss_device *iss)
+{
+	unsigned int timeout;
+
+	/* Fist, ensure that the ISP is IDLE (no transactions happening) */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG,
+		       ISP5_SYSCONFIG_STANDBYMODE_MASK,
+		       ISP5_SYSCONFIG_STANDBYMODE_SMART);
+
+	iss_reg_set(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL, ISP5_CTRL_MSTANDBY);
+
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL) &
+		ISP5_CTRL_MSTANDBY_WAIT, 1000000, 1000, 1500);
+	if (timeout) {
+		dev_err(iss->dev, "ISP5 standby timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	/* Now finally, do the reset */
+	iss_reg_set(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG,
+		    ISP5_SYSCONFIG_SOFTRESET);
+
+	timeout = iss_poll_condition_timeout(
+		!(iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG) &
+		ISP5_SYSCONFIG_SOFTRESET), 1000000, 1000, 1500);
+	if (timeout) {
+		dev_err(iss->dev, "ISP5 reset timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/*
+ * iss_module_sync_idle - Helper to sync module with its idle state
+ * @me: ISS submodule's media entity
+ * @wait: ISS submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISS submodule needs to wait for next interrupt. If
+ * yes, makes the caller to sleep while waiting for such event.
+ */
+int omap4iss_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(me);
+	struct iss_video *video = pipe->output;
+	unsigned long flags;
+
+	if (pipe->stream_state == ISS_PIPELINE_STREAM_STOPPED ||
+	    (pipe->stream_state == ISS_PIPELINE_STREAM_SINGLESHOT &&
+	     !iss_pipeline_ready(pipe)))
+		return 0;
+
+	/*
+	 * atomic_set() doesn't include memory barrier on ARM platform for SMP
+	 * scenario. We'll call it here to avoid race conditions.
+	 */
+	atomic_set(stopping, 1);
+	smp_wmb();
+
+	/*
+	 * If module is the last one, it's writing to memory. In this case,
+	 * it's necessary to check if the module is already paused due to
+	 * DMA queue underrun or if it has to wait for next interrupt to be
+	 * idle.
+	 * If it isn't the last one, the function won't sleep but *stopping
+	 * will still be set to warn next submodule caller's interrupt the
+	 * module wants to be idle.
+	 */
+	if (!iss_pipeline_is_last(me))
+		return 0;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		atomic_set(stopping, 0);
+		smp_wmb();
+		return 0;
+	}
+	spin_unlock_irqrestore(&video->qlock, flags);
+	if (!wait_event_timeout(*wait, !atomic_read(stopping),
+				msecs_to_jiffies(1000))) {
+		atomic_set(stopping, 0);
+		smp_wmb();
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/*
+ * omap4iss_module_sync_is_stopped - Helper to verify if module was stopping
+ * @wait: ISS submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISS submodule was stopping. In case of yes, it
+ * notices the caller by setting stopping to 0 and waking up the wait queue.
+ * Returns 1 if it was stopping or 0 otherwise.
+ */
+int omap4iss_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping)
+{
+	if (atomic_cmpxchg(stopping, 1, 0)) {
+		wake_up(wait);
+		return 1;
+	}
+
+	return 0;
+}
+
+/* --------------------------------------------------------------------------
+ * Clock management
+ */
+
+#define ISS_CLKCTRL_MASK	(ISS_CLKCTRL_CSI2_A |\
+				 ISS_CLKCTRL_CSI2_B |\
+				 ISS_CLKCTRL_ISP)
+
+static int __iss_subclk_update(struct iss_device *iss)
+{
+	u32 clk = 0;
+	int ret = 0, timeout = 1000;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_CSI2_A)
+		clk |= ISS_CLKCTRL_CSI2_A;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_CSI2_B)
+		clk |= ISS_CLKCTRL_CSI2_B;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_ISP)
+		clk |= ISS_CLKCTRL_ISP;
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_TOP, ISS_CLKCTRL,
+		       ISS_CLKCTRL_MASK, clk);
+
+	/* Wait for HW assertion */
+	while (--timeout > 0) {
+		udelay(1);
+		if ((iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_CLKSTAT) &
+		    ISS_CLKCTRL_MASK) == clk)
+			break;
+	}
+
+	if (!timeout)
+		ret = -EBUSY;
+
+	return ret;
+}
+
+int omap4iss_subclk_enable(struct iss_device *iss,
+			   enum iss_subclk_resource res)
+{
+	iss->subclk_resources |= res;
+
+	return __iss_subclk_update(iss);
+}
+
+int omap4iss_subclk_disable(struct iss_device *iss,
+			    enum iss_subclk_resource res)
+{
+	iss->subclk_resources &= ~res;
+
+	return __iss_subclk_update(iss);
+}
+
+#define ISS_ISP5_CLKCTRL_MASK	(ISP5_CTRL_BL_CLK_ENABLE |\
+				 ISP5_CTRL_ISIF_CLK_ENABLE |\
+				 ISP5_CTRL_H3A_CLK_ENABLE |\
+				 ISP5_CTRL_RSZ_CLK_ENABLE |\
+				 ISP5_CTRL_IPIPE_CLK_ENABLE |\
+				 ISP5_CTRL_IPIPEIF_CLK_ENABLE)
+
+static void __iss_isp_subclk_update(struct iss_device *iss)
+{
+	u32 clk = 0;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_ISIF)
+		clk |= ISP5_CTRL_ISIF_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_H3A)
+		clk |= ISP5_CTRL_H3A_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_RSZ)
+		clk |= ISP5_CTRL_RSZ_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_IPIPE)
+		clk |= ISP5_CTRL_IPIPE_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_IPIPEIF)
+		clk |= ISP5_CTRL_IPIPEIF_CLK_ENABLE;
+
+	if (clk)
+		clk |= ISP5_CTRL_BL_CLK_ENABLE;
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL,
+		       ISS_ISP5_CLKCTRL_MASK, clk);
+}
+
+void omap4iss_isp_subclk_enable(struct iss_device *iss,
+				enum iss_isp_subclk_resource res)
+{
+	iss->isp_subclk_resources |= res;
+
+	__iss_isp_subclk_update(iss);
+}
+
+void omap4iss_isp_subclk_disable(struct iss_device *iss,
+				 enum iss_isp_subclk_resource res)
+{
+	iss->isp_subclk_resources &= ~res;
+
+	__iss_isp_subclk_update(iss);
+}
+
+/*
+ * iss_enable_clocks - Enable ISS clocks
+ * @iss: OMAP4 ISS device
+ *
+ * Return 0 if successful, or clk_enable return value if any of tthem fails.
+ */
+static int iss_enable_clocks(struct iss_device *iss)
+{
+	int ret;
+
+	ret = clk_enable(iss->iss_fck);
+	if (ret) {
+		dev_err(iss->dev, "clk_enable iss_fck failed\n");
+		return ret;
+	}
+
+	ret = clk_enable(iss->iss_ctrlclk);
+	if (ret) {
+		dev_err(iss->dev, "clk_enable iss_ctrlclk failed\n");
+		clk_disable(iss->iss_fck);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * iss_disable_clocks - Disable ISS clocks
+ * @iss: OMAP4 ISS device
+ */
+static void iss_disable_clocks(struct iss_device *iss)
+{
+	clk_disable(iss->iss_ctrlclk);
+	clk_disable(iss->iss_fck);
+}
+
+static int iss_get_clocks(struct iss_device *iss)
+{
+	iss->iss_fck = devm_clk_get(iss->dev, "iss_fck");
+	if (IS_ERR(iss->iss_fck)) {
+		dev_err(iss->dev, "Unable to get iss_fck clock info\n");
+		return PTR_ERR(iss->iss_fck);
+	}
+
+	iss->iss_ctrlclk = devm_clk_get(iss->dev, "iss_ctrlclk");
+	if (IS_ERR(iss->iss_ctrlclk)) {
+		dev_err(iss->dev, "Unable to get iss_ctrlclk clock info\n");
+		return PTR_ERR(iss->iss_ctrlclk);
+	}
+
+	return 0;
+}
+
+/*
+ * omap4iss_get - Acquire the ISS resource.
+ *
+ * Initializes the clocks for the first acquire.
+ *
+ * Increment the reference count on the ISS. If the first reference is taken,
+ * enable clocks and power-up all submodules.
+ *
+ * Return a pointer to the ISS device structure, or NULL if an error occurred.
+ */
+struct iss_device *omap4iss_get(struct iss_device *iss)
+{
+	struct iss_device *__iss = iss;
+
+	if (!iss)
+		return NULL;
+
+	mutex_lock(&iss->iss_mutex);
+	if (iss->ref_count > 0)
+		goto out;
+
+	if (iss_enable_clocks(iss) < 0) {
+		__iss = NULL;
+		goto out;
+	}
+
+	iss_enable_interrupts(iss);
+
+out:
+	if (__iss)
+		iss->ref_count++;
+	mutex_unlock(&iss->iss_mutex);
+
+	return __iss;
+}
+
+/*
+ * omap4iss_put - Release the ISS
+ *
+ * Decrement the reference count on the ISS. If the last reference is released,
+ * power-down all submodules, disable clocks and free temporary buffers.
+ */
+void omap4iss_put(struct iss_device *iss)
+{
+	if (!iss)
+		return;
+
+	mutex_lock(&iss->iss_mutex);
+	WARN_ON(iss->ref_count == 0);
+	if (--iss->ref_count == 0) {
+		iss_disable_interrupts(iss);
+		/* Reset the ISS if an entity has failed to stop. This is the
+		 * only way to recover from such conditions, although it would
+		 * be worth investigating whether resetting the ISP only can't
+		 * fix the problem in some cases.
+		 */
+		if (!media_entity_enum_empty(&iss->crashed))
+			iss_reset(iss);
+		iss_disable_clocks(iss);
+	}
+	mutex_unlock(&iss->iss_mutex);
+}
+
+static int iss_map_mem_resource(struct platform_device *pdev,
+				struct iss_device *iss,
+				enum iss_mem_resources res)
+{
+	iss->regs[res] = devm_platform_ioremap_resource(pdev, res);
+
+	return PTR_ERR_OR_ZERO(iss->regs[res]);
+}
+
+static void iss_unregister_entities(struct iss_device *iss)
+{
+	omap4iss_resizer_unregister_entities(&iss->resizer);
+	omap4iss_ipipe_unregister_entities(&iss->ipipe);
+	omap4iss_ipipeif_unregister_entities(&iss->ipipeif);
+	omap4iss_csi2_unregister_entities(&iss->csi2a);
+	omap4iss_csi2_unregister_entities(&iss->csi2b);
+
+	v4l2_device_unregister(&iss->v4l2_dev);
+	media_device_unregister(&iss->media_dev);
+}
+
+/*
+ * iss_register_subdev_group - Register a group of subdevices
+ * @iss: OMAP4 ISS device
+ * @board_info: I2C subdevs board information array
+ *
+ * Register all I2C subdevices in the board_info array. The array must be
+ * terminated by a NULL entry, and the first entry must be the sensor.
+ *
+ * Return a pointer to the sensor media entity if it has been successfully
+ * registered, or NULL otherwise.
+ */
+static struct v4l2_subdev *
+iss_register_subdev_group(struct iss_device *iss,
+			  struct iss_subdev_i2c_board_info *board_info)
+{
+	struct v4l2_subdev *sensor = NULL;
+	unsigned int first;
+
+	if (!board_info->board_info)
+		return NULL;
+
+	for (first = 1; board_info->board_info; ++board_info, first = 0) {
+		struct v4l2_subdev *subdev;
+		struct i2c_adapter *adapter;
+
+		adapter = i2c_get_adapter(board_info->i2c_adapter_id);
+		if (!adapter) {
+			dev_err(iss->dev,
+				"%s: Unable to get I2C adapter %d for device %s\n",
+				__func__, board_info->i2c_adapter_id,
+				board_info->board_info->type);
+			continue;
+		}
+
+		subdev = v4l2_i2c_new_subdev_board(&iss->v4l2_dev, adapter,
+						   board_info->board_info, NULL);
+		if (!subdev) {
+			dev_err(iss->dev, "Unable to register subdev %s\n",
+				board_info->board_info->type);
+			continue;
+		}
+
+		if (first)
+			sensor = subdev;
+	}
+
+	return sensor;
+}
+
+static int iss_register_entities(struct iss_device *iss)
+{
+	struct iss_platform_data *pdata = iss->pdata;
+	struct iss_v4l2_subdevs_group *subdevs;
+	int ret;
+
+	iss->media_dev.dev = iss->dev;
+	strscpy(iss->media_dev.model, "TI OMAP4 ISS",
+		sizeof(iss->media_dev.model));
+	iss->media_dev.hw_revision = iss->revision;
+	iss->media_dev.ops = &iss_media_ops;
+	ret = media_device_register(&iss->media_dev);
+	if (ret < 0) {
+		dev_err(iss->dev, "Media device registration failed (%d)\n",
+			ret);
+		return ret;
+	}
+
+	iss->v4l2_dev.mdev = &iss->media_dev;
+	ret = v4l2_device_register(iss->dev, &iss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(iss->dev, "V4L2 device registration failed (%d)\n",
+			ret);
+		goto done;
+	}
+
+	/* Register internal entities */
+	ret = omap4iss_csi2_register_entities(&iss->csi2a, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_csi2_register_entities(&iss->csi2b, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_ipipeif_register_entities(&iss->ipipeif, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_ipipe_register_entities(&iss->ipipe, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_resizer_register_entities(&iss->resizer, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	/* Register external entities */
+	for (subdevs = pdata->subdevs; subdevs && subdevs->subdevs; ++subdevs) {
+		struct v4l2_subdev *sensor;
+		struct media_entity *input;
+		unsigned int flags;
+		unsigned int pad;
+
+		sensor = iss_register_subdev_group(iss, subdevs->subdevs);
+		if (!sensor)
+			continue;
+
+		sensor->host_priv = subdevs;
+
+		/* Connect the sensor to the correct interface module.
+		 * CSI2a receiver through CSIPHY1, or
+		 * CSI2b receiver through CSIPHY2
+		 */
+		switch (subdevs->interface) {
+		case ISS_INTERFACE_CSI2A_PHY1:
+			input = &iss->csi2a.subdev.entity;
+			pad = CSI2_PAD_SINK;
+			flags = MEDIA_LNK_FL_IMMUTABLE
+			      | MEDIA_LNK_FL_ENABLED;
+			break;
+
+		case ISS_INTERFACE_CSI2B_PHY2:
+			input = &iss->csi2b.subdev.entity;
+			pad = CSI2_PAD_SINK;
+			flags = MEDIA_LNK_FL_IMMUTABLE
+			      | MEDIA_LNK_FL_ENABLED;
+			break;
+
+		default:
+			dev_err(iss->dev, "invalid interface type %u\n",
+				subdevs->interface);
+			ret = -EINVAL;
+			goto done;
+		}
+
+		ret = media_create_pad_link(&sensor->entity, 0, input, pad,
+					    flags);
+		if (ret < 0)
+			goto done;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&iss->v4l2_dev);
+
+done:
+	if (ret < 0)
+		iss_unregister_entities(iss);
+
+	return ret;
+}
+
+/*
+ * iss_create_links() - Pads links creation for the subdevices
+ * @iss : Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+static int iss_create_links(struct iss_device *iss)
+{
+	int ret;
+
+	ret = omap4iss_csi2_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI2 pads links creation failed\n");
+		return ret;
+	}
+
+	ret = omap4iss_ipipeif_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPEIF pads links creation failed\n");
+		return ret;
+	}
+
+	ret = omap4iss_resizer_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP RESIZER pads links creation failed\n");
+		return ret;
+	}
+
+	/* Connect the submodules. */
+	ret = media_create_pad_link(
+			&iss->csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->csi2b.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+};
+
+static void iss_cleanup_modules(struct iss_device *iss)
+{
+	omap4iss_csi2_cleanup(iss);
+	omap4iss_ipipeif_cleanup(iss);
+	omap4iss_ipipe_cleanup(iss);
+	omap4iss_resizer_cleanup(iss);
+}
+
+static int iss_initialize_modules(struct iss_device *iss)
+{
+	int ret;
+
+	ret = omap4iss_csiphy_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI PHY initialization failed\n");
+		goto error_csiphy;
+	}
+
+	ret = omap4iss_csi2_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI2 initialization failed\n");
+		goto error_csi2;
+	}
+
+	ret = omap4iss_ipipeif_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPEIF initialization failed\n");
+		goto error_ipipeif;
+	}
+
+	ret = omap4iss_ipipe_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPE initialization failed\n");
+		goto error_ipipe;
+	}
+
+	ret = omap4iss_resizer_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP RESIZER initialization failed\n");
+		goto error_resizer;
+	}
+
+	return 0;
+
+error_resizer:
+	omap4iss_ipipe_cleanup(iss);
+error_ipipe:
+	omap4iss_ipipeif_cleanup(iss);
+error_ipipeif:
+	omap4iss_csi2_cleanup(iss);
+error_csi2:
+error_csiphy:
+	return ret;
+}
+
+static int iss_probe(struct platform_device *pdev)
+{
+	struct iss_platform_data *pdata = pdev->dev.platform_data;
+	struct iss_device *iss;
+	unsigned int i;
+	int ret;
+
+	if (!pdata)
+		return -EINVAL;
+
+	iss = devm_kzalloc(&pdev->dev, sizeof(*iss), GFP_KERNEL);
+	if (!iss)
+		return -ENOMEM;
+
+	mutex_init(&iss->iss_mutex);
+
+	iss->dev = &pdev->dev;
+	iss->pdata = pdata;
+
+	iss->raw_dmamask = DMA_BIT_MASK(32);
+	iss->dev->dma_mask = &iss->raw_dmamask;
+	iss->dev->coherent_dma_mask = DMA_BIT_MASK(32);
+
+	platform_set_drvdata(pdev, iss);
+
+	/*
+	 * TODO: When implementing DT support switch to syscon regmap lookup by
+	 * phandle.
+	 */
+	iss->syscon = syscon_regmap_lookup_by_compatible("syscon");
+	if (IS_ERR(iss->syscon)) {
+		ret = PTR_ERR(iss->syscon);
+		goto error;
+	}
+
+	/* Clocks */
+	ret = iss_map_mem_resource(pdev, iss, OMAP4_ISS_MEM_TOP);
+	if (ret < 0)
+		goto error;
+
+	ret = iss_get_clocks(iss);
+	if (ret < 0)
+		goto error;
+
+	if (!omap4iss_get(iss)) {
+		ret = -EINVAL;
+		goto error;
+	}
+
+	ret = iss_reset(iss);
+	if (ret < 0)
+		goto error_iss;
+
+	iss->revision = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION);
+	dev_info(iss->dev, "Revision %08x found\n", iss->revision);
+
+	for (i = 1; i < OMAP4_ISS_MEM_LAST; i++) {
+		ret = iss_map_mem_resource(pdev, iss, i);
+		if (ret)
+			goto error_iss;
+	}
+
+	/* Configure BTE BW_LIMITER field to max recommended value (1 GB) */
+	iss_reg_update(iss, OMAP4_ISS_MEM_BTE, BTE_CTRL,
+		       BTE_CTRL_BW_LIMITER_MASK,
+		       18 << BTE_CTRL_BW_LIMITER_SHIFT);
+
+	/* Perform ISP reset */
+	ret = omap4iss_subclk_enable(iss, OMAP4_ISS_SUBCLK_ISP);
+	if (ret < 0)
+		goto error_iss;
+
+	ret = iss_isp_reset(iss);
+	if (ret < 0)
+		goto error_iss;
+
+	dev_info(iss->dev, "ISP Revision %08x found\n",
+		 iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_REVISION));
+
+	/* Interrupt */
+	ret = platform_get_irq(pdev, 0);
+	if (ret <= 0) {
+		ret = -ENODEV;
+		goto error_iss;
+	}
+	iss->irq_num = ret;
+
+	if (devm_request_irq(iss->dev, iss->irq_num, iss_isr, IRQF_SHARED,
+			     "OMAP4 ISS", iss)) {
+		dev_err(iss->dev, "Unable to request IRQ\n");
+		ret = -EINVAL;
+		goto error_iss;
+	}
+
+	/* Entities */
+	ret = iss_initialize_modules(iss);
+	if (ret < 0)
+		goto error_iss;
+
+	ret = iss_register_entities(iss);
+	if (ret < 0)
+		goto error_modules;
+
+	ret = media_entity_enum_init(&iss->crashed, &iss->media_dev);
+	if (ret)
+		goto error_entities;
+
+	ret = iss_create_links(iss);
+	if (ret < 0)
+		goto error_entities;
+
+	omap4iss_put(iss);
+
+	return 0;
+
+error_entities:
+	iss_unregister_entities(iss);
+	media_entity_enum_cleanup(&iss->crashed);
+error_modules:
+	iss_cleanup_modules(iss);
+error_iss:
+	omap4iss_put(iss);
+error:
+	mutex_destroy(&iss->iss_mutex);
+
+	return ret;
+}
+
+static int iss_remove(struct platform_device *pdev)
+{
+	struct iss_device *iss = platform_get_drvdata(pdev);
+
+	iss_unregister_entities(iss);
+	media_entity_enum_cleanup(&iss->crashed);
+	iss_cleanup_modules(iss);
+
+	return 0;
+}
+
+static const struct platform_device_id omap4iss_id_table[] = {
+	{ "omap4iss", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, omap4iss_id_table);
+
+static struct platform_driver iss_driver = {
+	.probe		= iss_probe,
+	.remove		= iss_remove,
+	.id_table	= omap4iss_id_table,
+	.driver = {
+		.name	= "omap4iss",
+	},
+};
+
+module_platform_driver(iss_driver);
+
+MODULE_DESCRIPTION("TI OMAP4 ISS driver");
+MODULE_AUTHOR("Sergio Aguirre <sergio.a.aguirre@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/omap4iss/iss.h b/drivers/staging/media/omap4iss/iss.h
new file mode 100644
index 000000000..3f587e000
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss.h
@@ -0,0 +1,247 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver
+ *
+ * Copyright (C) 2012 Texas Instruments.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef _OMAP4_ISS_H_
+#define _OMAP4_ISS_H_
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-mc.h>
+
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+
+#include <linux/platform_data/media/omap4iss.h>
+
+#include "iss_regs.h"
+#include "iss_csiphy.h"
+#include "iss_csi2.h"
+#include "iss_ipipeif.h"
+#include "iss_ipipe.h"
+#include "iss_resizer.h"
+
+struct regmap;
+
+#define to_iss_device(ptr_module)				\
+	container_of(ptr_module, struct iss_device, ptr_module)
+#define to_device(ptr_module)						\
+	(to_iss_device(ptr_module)->dev)
+
+enum iss_mem_resources {
+	OMAP4_ISS_MEM_TOP,
+	OMAP4_ISS_MEM_CSI2_A_REGS1,
+	OMAP4_ISS_MEM_CAMERARX_CORE1,
+	OMAP4_ISS_MEM_CSI2_B_REGS1,
+	OMAP4_ISS_MEM_CAMERARX_CORE2,
+	OMAP4_ISS_MEM_BTE,
+	OMAP4_ISS_MEM_ISP_SYS1,
+	OMAP4_ISS_MEM_ISP_RESIZER,
+	OMAP4_ISS_MEM_ISP_IPIPE,
+	OMAP4_ISS_MEM_ISP_ISIF,
+	OMAP4_ISS_MEM_ISP_IPIPEIF,
+	OMAP4_ISS_MEM_LAST,
+};
+
+enum iss_subclk_resource {
+	OMAP4_ISS_SUBCLK_SIMCOP		= (1 << 0),
+	OMAP4_ISS_SUBCLK_ISP		= (1 << 1),
+	OMAP4_ISS_SUBCLK_CSI2_A		= (1 << 2),
+	OMAP4_ISS_SUBCLK_CSI2_B		= (1 << 3),
+	OMAP4_ISS_SUBCLK_CCP2		= (1 << 4),
+};
+
+enum iss_isp_subclk_resource {
+	OMAP4_ISS_ISP_SUBCLK_BL		= (1 << 0),
+	OMAP4_ISS_ISP_SUBCLK_ISIF	= (1 << 1),
+	OMAP4_ISS_ISP_SUBCLK_H3A	= (1 << 2),
+	OMAP4_ISS_ISP_SUBCLK_RSZ	= (1 << 3),
+	OMAP4_ISS_ISP_SUBCLK_IPIPE	= (1 << 4),
+	OMAP4_ISS_ISP_SUBCLK_IPIPEIF	= (1 << 5),
+};
+
+/*
+ * struct iss_reg - Structure for ISS register values.
+ * @reg: 32-bit Register address.
+ * @val: 32-bit Register value.
+ */
+struct iss_reg {
+	enum iss_mem_resources mmio_range;
+	u32 reg;
+	u32 val;
+};
+
+/*
+ * struct iss_device - ISS device structure.
+ * @syscon: Regmap for the syscon register space
+ * @crashed: Crashed entities
+ */
+struct iss_device {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct device *dev;
+	u32 revision;
+
+	/* platform HW resources */
+	struct iss_platform_data *pdata;
+	unsigned int irq_num;
+
+	struct resource *res[OMAP4_ISS_MEM_LAST];
+	void __iomem *regs[OMAP4_ISS_MEM_LAST];
+	struct regmap *syscon;
+
+	u64 raw_dmamask;
+
+	struct mutex iss_mutex;	/* For handling ref_count field */
+	struct media_entity_enum crashed;
+	int has_context;
+	int ref_count;
+
+	struct clk *iss_fck;
+	struct clk *iss_ctrlclk;
+
+	/* ISS modules */
+	struct iss_csi2_device csi2a;
+	struct iss_csi2_device csi2b;
+	struct iss_csiphy csiphy1;
+	struct iss_csiphy csiphy2;
+	struct iss_ipipeif_device ipipeif;
+	struct iss_ipipe_device ipipe;
+	struct iss_resizer_device resizer;
+
+	unsigned int subclk_resources;
+	unsigned int isp_subclk_resources;
+};
+
+int omap4iss_get_external_info(struct iss_pipeline *pipe,
+			       struct media_link *link);
+
+int omap4iss_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping);
+
+int omap4iss_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping);
+
+int omap4iss_pipeline_set_stream(struct iss_pipeline *pipe,
+				 enum iss_pipeline_stream_state state);
+void omap4iss_pipeline_cancel_stream(struct iss_pipeline *pipe);
+
+void omap4iss_configure_bridge(struct iss_device *iss,
+			       enum ipipeif_input_entity input);
+
+struct iss_device *omap4iss_get(struct iss_device *iss);
+void omap4iss_put(struct iss_device *iss);
+int omap4iss_subclk_enable(struct iss_device *iss,
+			   enum iss_subclk_resource res);
+int omap4iss_subclk_disable(struct iss_device *iss,
+			    enum iss_subclk_resource res);
+void omap4iss_isp_subclk_enable(struct iss_device *iss,
+				enum iss_isp_subclk_resource res);
+void omap4iss_isp_subclk_disable(struct iss_device *iss,
+				 enum iss_isp_subclk_resource res);
+
+int omap4iss_register_entities(struct platform_device *pdev,
+			       struct v4l2_device *v4l2_dev);
+void omap4iss_unregister_entities(struct platform_device *pdev);
+
+/*
+ * iss_reg_read - Read the value of an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ *
+ * Return the register value.
+ */
+static inline
+u32 iss_reg_read(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset)
+{
+	return readl(iss->regs[res] + offset);
+}
+
+/*
+ * iss_reg_write - Write a value to an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @value: value to be written
+ */
+static inline
+void iss_reg_write(struct iss_device *iss, enum iss_mem_resources res,
+		   u32 offset, u32 value)
+{
+	writel(value, iss->regs[res] + offset);
+}
+
+/*
+ * iss_reg_clr - Clear bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @clr: bit mask to be cleared
+ */
+static inline
+void iss_reg_clr(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset, u32 clr)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, v & ~clr);
+}
+
+/*
+ * iss_reg_set - Set bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @set: bit mask to be set
+ */
+static inline
+void iss_reg_set(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset, u32 set)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, v | set);
+}
+
+/*
+ * iss_reg_update - Clear and set bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @clr: bit mask to be cleared
+ * @set: bit mask to be set
+ *
+ * Clear the clr mask first and then set the set mask.
+ */
+static inline
+void iss_reg_update(struct iss_device *iss, enum iss_mem_resources res,
+		    u32 offset, u32 clr, u32 set)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, (v & ~clr) | set);
+}
+
+#define iss_poll_condition_timeout(cond, timeout, min_ival, max_ival)	\
+({									\
+	unsigned long __timeout = jiffies + usecs_to_jiffies(timeout);	\
+	unsigned int __min_ival = (min_ival);				\
+	unsigned int __max_ival = (max_ival);				\
+	bool __cond;							\
+	while (!(__cond = (cond))) {					\
+		if (time_after(jiffies, __timeout))			\
+			break;						\
+		usleep_range(__min_ival, __max_ival);			\
+	}								\
+	!__cond;							\
+})
+
+#endif /* _OMAP4_ISS_H_ */
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
new file mode 100644
index 000000000..04ce0e7eb
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -0,0 +1,1380 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/delay.h>
+#include <media/v4l2-common.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/mm.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_csi2.h"
+
+/*
+ * csi2_if_enable - Enable CSI2 Receiver interface.
+ * @enable: enable flag
+ *
+ */
+static void csi2_if_enable(struct iss_csi2_device *csi2, u8 enable)
+{
+	struct iss_csi2_ctrl_cfg *currctrl = &csi2->ctrl;
+
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_CTRL, CSI2_CTRL_IF_EN,
+		       enable ? CSI2_CTRL_IF_EN : 0);
+
+	currctrl->if_enable = enable;
+}
+
+/*
+ * csi2_recv_config - CSI2 receiver module configuration.
+ * @currctrl: iss_csi2_ctrl_cfg structure
+ *
+ */
+static void csi2_recv_config(struct iss_csi2_device *csi2,
+			     struct iss_csi2_ctrl_cfg *currctrl)
+{
+	u32 reg = 0;
+
+	if (currctrl->frame_mode)
+		reg |= CSI2_CTRL_FRAME;
+	else
+		reg &= ~CSI2_CTRL_FRAME;
+
+	if (currctrl->vp_clk_enable)
+		reg |= CSI2_CTRL_VP_CLK_EN;
+	else
+		reg &= ~CSI2_CTRL_VP_CLK_EN;
+
+	if (currctrl->vp_only_enable)
+		reg |= CSI2_CTRL_VP_ONLY_EN;
+	else
+		reg &= ~CSI2_CTRL_VP_ONLY_EN;
+
+	reg &= ~CSI2_CTRL_VP_OUT_CTRL_MASK;
+	reg |= currctrl->vp_out_ctrl << CSI2_CTRL_VP_OUT_CTRL_SHIFT;
+
+	if (currctrl->ecc_enable)
+		reg |= CSI2_CTRL_ECC_EN;
+	else
+		reg &= ~CSI2_CTRL_ECC_EN;
+
+	/*
+	 * Set MFlag assertion boundaries to:
+	 * Low: 4/8 of FIFO size
+	 * High: 6/8 of FIFO size
+	 */
+	reg &= ~(CSI2_CTRL_MFLAG_LEVH_MASK | CSI2_CTRL_MFLAG_LEVL_MASK);
+	reg |= (2 << CSI2_CTRL_MFLAG_LEVH_SHIFT) |
+	       (4 << CSI2_CTRL_MFLAG_LEVL_SHIFT);
+
+	/* Generation of 16x64-bit bursts (Recommended) */
+	reg |= CSI2_CTRL_BURST_SIZE_EXPAND;
+
+	/* Do Non-Posted writes (Recommended) */
+	reg |= CSI2_CTRL_NON_POSTED_WRITE;
+
+	/*
+	 * Enforce Little endian for all formats, including:
+	 * YUV4:2:2 8-bit and YUV4:2:0 Legacy
+	 */
+	reg |= CSI2_CTRL_ENDIANNESS;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTRL, reg);
+}
+
+static const unsigned int csi2_input_fmts[] = {
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+};
+
+/* To set the format on the CSI2 requires a mapping function that takes
+ * the following inputs:
+ * - 3 different formats (at this time)
+ * - 2 destinations (mem, vp+mem) (vp only handled separately)
+ * - 2 decompression options (on, off)
+ * Output should be CSI2 frame format code
+ * Array indices as follows: [format][dest][decompr]
+ * Not all combinations are valid. 0 means invalid.
+ */
+static const u16 __csi2_fmt_map[][2][2] = {
+	/* RAW10 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW10_EXP16,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW10_EXP16_VP,
+			/* DPCM decompression */
+			0,
+		},
+	},
+	/* RAW10 DPCM8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1,
+			/* DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1_DPCM10,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8_VP,
+			/* DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1_DPCM10_VP,
+		},
+	},
+	/* RAW8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8_VP,
+			/* DPCM decompression */
+			0,
+		},
+	},
+	/* YUV422 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_YUV422_8BIT,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_YUV422_8BIT_VP16,
+			/* DPCM decompression */
+			0,
+		},
+	},
+};
+
+/*
+ * csi2_ctx_map_format - Map CSI2 sink media bus format to CSI2 format ID
+ * @csi2: ISS CSI2 device
+ *
+ * Returns CSI2 physical format id
+ */
+static u16 csi2_ctx_map_format(struct iss_csi2_device *csi2)
+{
+	const struct v4l2_mbus_framefmt *fmt = &csi2->formats[CSI2_PAD_SINK];
+	int fmtidx, destidx;
+
+	switch (fmt->code) {
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		fmtidx = 0;
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8:
+		fmtidx = 1;
+		break;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		fmtidx = 2;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		fmtidx = 3;
+		break;
+	default:
+		WARN(1, "CSI2: pixel format %08x unsupported!\n",
+		     fmt->code);
+		return 0;
+	}
+
+	if (!(csi2->output & CSI2_OUTPUT_IPIPEIF) &&
+	    !(csi2->output & CSI2_OUTPUT_MEMORY)) {
+		/* Neither output enabled is a valid combination */
+		return CSI2_PIX_FMT_OTHERS;
+	}
+
+	/* If we need to skip frames at the beginning of the stream disable the
+	 * video port to avoid sending the skipped frames to the IPIPEIF.
+	 */
+	destidx = csi2->frame_skip ? 0 : !!(csi2->output & CSI2_OUTPUT_IPIPEIF);
+
+	return __csi2_fmt_map[fmtidx][destidx][csi2->dpcm_decompress];
+}
+
+/*
+ * csi2_set_outaddr - Set memory address to save output image
+ * @csi2: Pointer to ISS CSI2a device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ *
+ * Returns 0 if successful, or -EINVAL if the address is not in the 32 byte
+ * boundary.
+ */
+static void csi2_set_outaddr(struct iss_csi2_device *csi2, u32 addr)
+{
+	struct iss_csi2_ctx_cfg *ctx = &csi2->contexts[0];
+
+	ctx->ping_addr = addr;
+	ctx->pong_addr = addr;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PING_ADDR(ctx->ctxnum),
+		      ctx->ping_addr);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PONG_ADDR(ctx->ctxnum),
+		      ctx->pong_addr);
+}
+
+/*
+ * is_usr_def_mapping - Checks whether USER_DEF_MAPPING should
+ *			be enabled by CSI2.
+ * @format_id: mapped format id
+ *
+ */
+static inline int is_usr_def_mapping(u32 format_id)
+{
+	return (format_id & 0xf0) == 0x40 ? 1 : 0;
+}
+
+/*
+ * csi2_ctx_enable - Enable specified CSI2 context
+ * @ctxnum: Context number, valid between 0 and 7 values.
+ * @enable: enable
+ *
+ */
+static void csi2_ctx_enable(struct iss_csi2_device *csi2, u8 ctxnum, u8 enable)
+{
+	struct iss_csi2_ctx_cfg *ctx = &csi2->contexts[ctxnum];
+	u32 reg;
+
+	reg = iss_reg_read(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctxnum));
+
+	if (enable) {
+		unsigned int skip = 0;
+
+		if (csi2->frame_skip)
+			skip = csi2->frame_skip;
+		else if (csi2->output & CSI2_OUTPUT_MEMORY)
+			skip = 1;
+
+		reg &= ~CSI2_CTX_CTRL1_COUNT_MASK;
+		reg |= CSI2_CTX_CTRL1_COUNT_UNLOCK
+		    |  (skip << CSI2_CTX_CTRL1_COUNT_SHIFT)
+		    |  CSI2_CTX_CTRL1_CTX_EN;
+	} else {
+		reg &= ~CSI2_CTX_CTRL1_CTX_EN;
+	}
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctxnum), reg);
+	ctx->enabled = enable;
+}
+
+/*
+ * csi2_ctx_config - CSI2 context configuration.
+ * @ctx: context configuration
+ *
+ */
+static void csi2_ctx_config(struct iss_csi2_device *csi2,
+			    struct iss_csi2_ctx_cfg *ctx)
+{
+	u32 reg = 0;
+
+	ctx->frame = 0;
+
+	/* Set up CSI2_CTx_CTRL1 */
+	if (ctx->eof_enabled)
+		reg = CSI2_CTX_CTRL1_EOF_EN;
+
+	if (ctx->eol_enabled)
+		reg |= CSI2_CTX_CTRL1_EOL_EN;
+
+	if (ctx->checksum_enabled)
+		reg |= CSI2_CTX_CTRL1_CS_EN;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctx->ctxnum), reg);
+
+	/* Set up CSI2_CTx_CTRL2 */
+	reg = ctx->virtual_id << CSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT;
+	reg |= ctx->format_id << CSI2_CTX_CTRL2_FORMAT_SHIFT;
+
+	if (ctx->dpcm_decompress && ctx->dpcm_predictor)
+		reg |= CSI2_CTX_CTRL2_DPCM_PRED;
+
+	if (is_usr_def_mapping(ctx->format_id))
+		reg |= 2 << CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL2(ctx->ctxnum), reg);
+
+	/* Set up CSI2_CTx_CTRL3 */
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL3(ctx->ctxnum),
+		      ctx->alpha << CSI2_CTX_CTRL3_ALPHA_SHIFT);
+
+	/* Set up CSI2_CTx_DAT_OFST */
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_CTX_DAT_OFST(ctx->ctxnum),
+		       CSI2_CTX_DAT_OFST_MASK, ctx->data_offset);
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PING_ADDR(ctx->ctxnum),
+		      ctx->ping_addr);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PONG_ADDR(ctx->ctxnum),
+		      ctx->pong_addr);
+}
+
+/*
+ * csi2_timing_config - CSI2 timing configuration.
+ * @timing: csi2_timing_cfg structure
+ */
+static void csi2_timing_config(struct iss_csi2_device *csi2,
+			       struct iss_csi2_timing_cfg *timing)
+{
+	u32 reg;
+
+	reg = iss_reg_read(csi2->iss, csi2->regs1, CSI2_TIMING);
+
+	if (timing->force_rx_mode)
+		reg |= CSI2_TIMING_FORCE_RX_MODE_IO1;
+	else
+		reg &= ~CSI2_TIMING_FORCE_RX_MODE_IO1;
+
+	if (timing->stop_state_16x)
+		reg |= CSI2_TIMING_STOP_STATE_X16_IO1;
+	else
+		reg &= ~CSI2_TIMING_STOP_STATE_X16_IO1;
+
+	if (timing->stop_state_4x)
+		reg |= CSI2_TIMING_STOP_STATE_X4_IO1;
+	else
+		reg &= ~CSI2_TIMING_STOP_STATE_X4_IO1;
+
+	reg &= ~CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK;
+	reg |= timing->stop_state_counter <<
+	       CSI2_TIMING_STOP_STATE_COUNTER_IO1_SHIFT;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_TIMING, reg);
+}
+
+/*
+ * csi2_irq_ctx_set - Enables CSI2 Context IRQs.
+ * @enable: Enable/disable CSI2 Context interrupts
+ */
+static void csi2_irq_ctx_set(struct iss_csi2_device *csi2, int enable)
+{
+	const u32 mask = CSI2_CTX_IRQ_FE | CSI2_CTX_IRQ_FS;
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(i),
+			      mask);
+		if (enable)
+			iss_reg_set(csi2->iss, csi2->regs1,
+				    CSI2_CTX_IRQENABLE(i), mask);
+		else
+			iss_reg_clr(csi2->iss, csi2->regs1,
+				    CSI2_CTX_IRQENABLE(i), mask);
+	}
+}
+
+/*
+ * csi2_irq_complexio1_set - Enables CSI2 ComplexIO IRQs.
+ * @enable: Enable/disable CSI2 ComplexIO #1 interrupts
+ */
+static void csi2_irq_complexio1_set(struct iss_csi2_device *csi2, int enable)
+{
+	u32 reg;
+
+	reg = CSI2_COMPLEXIO_IRQ_STATEALLULPMEXIT |
+		CSI2_COMPLEXIO_IRQ_STATEALLULPMENTER |
+		CSI2_COMPLEXIO_IRQ_STATEULPM5 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL5 |
+		CSI2_COMPLEXIO_IRQ_ERRESC5 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS5 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS5 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM4 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL4 |
+		CSI2_COMPLEXIO_IRQ_ERRESC4 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS4 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS4 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM3 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL3 |
+		CSI2_COMPLEXIO_IRQ_ERRESC3 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS3 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS3 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM2 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL2 |
+		CSI2_COMPLEXIO_IRQ_ERRESC2 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS2 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS2 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM1 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL1 |
+		CSI2_COMPLEXIO_IRQ_ERRESC1 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS1 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS1;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQSTATUS, reg);
+	if (enable)
+		iss_reg_set(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQENABLE,
+			    reg);
+	else
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQENABLE,
+			      0);
+}
+
+/*
+ * csi2_irq_status_set - Enables CSI2 Status IRQs.
+ * @enable: Enable/disable CSI2 Status interrupts
+ */
+static void csi2_irq_status_set(struct iss_csi2_device *csi2, int enable)
+{
+	u32 reg;
+
+	reg = CSI2_IRQ_OCP_ERR |
+		CSI2_IRQ_SHORT_PACKET |
+		CSI2_IRQ_ECC_CORRECTION |
+		CSI2_IRQ_ECC_NO_CORRECTION |
+		CSI2_IRQ_COMPLEXIO_ERR |
+		CSI2_IRQ_FIFO_OVF |
+		CSI2_IRQ_CONTEXT0;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQSTATUS, reg);
+	if (enable)
+		iss_reg_set(csi2->iss, csi2->regs1, CSI2_IRQENABLE, reg);
+	else
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQENABLE, 0);
+}
+
+/*
+ * omap4iss_csi2_reset - Resets the CSI2 module.
+ *
+ * Must be called with the phy lock held.
+ *
+ * Returns 0 if successful, or -EBUSY if power command didn't respond.
+ */
+int omap4iss_csi2_reset(struct iss_csi2_device *csi2)
+{
+	unsigned int timeout;
+
+	if (!csi2->available)
+		return -ENODEV;
+
+	if (csi2->phy->phy_in_use)
+		return -EBUSY;
+
+	iss_reg_set(csi2->iss, csi2->regs1, CSI2_SYSCONFIG,
+		    CSI2_SYSCONFIG_SOFT_RESET);
+
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(csi2->iss, csi2->regs1, CSI2_SYSSTATUS) &
+		CSI2_SYSSTATUS_RESET_DONE, 500, 100, 200);
+	if (timeout) {
+		dev_err(csi2->iss->dev, "CSI2: Soft reset timeout!\n");
+		return -EBUSY;
+	}
+
+	iss_reg_set(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_CFG,
+		    CSI2_COMPLEXIO_CFG_RESET_CTRL);
+
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(csi2->iss, csi2->phy->phy_regs, REGISTER1) &
+		REGISTER1_RESET_DONE_CTRLCLK, 10000, 100, 500);
+	if (timeout) {
+		dev_err(csi2->iss->dev, "CSI2: CSI2_96M_FCLK reset timeout!\n");
+		return -EBUSY;
+	}
+
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_SYSCONFIG,
+		       CSI2_SYSCONFIG_MSTANDBY_MODE_MASK |
+		       CSI2_SYSCONFIG_AUTO_IDLE,
+		       CSI2_SYSCONFIG_MSTANDBY_MODE_NO);
+
+	return 0;
+}
+
+static int csi2_configure(struct iss_csi2_device *csi2)
+{
+	const struct iss_v4l2_subdevs_group *pdata;
+	struct iss_csi2_timing_cfg *timing = &csi2->timing[0];
+	struct v4l2_subdev *sensor;
+	struct media_pad *pad;
+
+	/*
+	 * CSI2 fields that can be updated while the context has
+	 * been enabled or the interface has been enabled are not
+	 * updated dynamically currently. So we do not allow to
+	 * reconfigure if either has been enabled
+	 */
+	if (csi2->contexts[0].enabled || csi2->ctrl.if_enable)
+		return -EBUSY;
+
+	pad = media_pad_remote_pad_first(&csi2->pads[CSI2_PAD_SINK]);
+	sensor = media_entity_to_v4l2_subdev(pad->entity);
+	pdata = sensor->host_priv;
+
+	csi2->frame_skip = 0;
+	v4l2_subdev_call(sensor, sensor, g_skip_frames, &csi2->frame_skip);
+
+	csi2->ctrl.vp_out_ctrl = pdata->bus.csi2.vpclk_div;
+	csi2->ctrl.frame_mode = ISS_CSI2_FRAME_IMMEDIATE;
+	csi2->ctrl.ecc_enable = pdata->bus.csi2.crc;
+
+	timing->force_rx_mode = 1;
+	timing->stop_state_16x = 1;
+	timing->stop_state_4x = 1;
+	timing->stop_state_counter = 0x1ff;
+
+	/*
+	 * The CSI2 receiver can't do any format conversion except DPCM
+	 * decompression, so every set_format call configures both pads
+	 * and enables DPCM decompression as a special case:
+	 */
+	if (csi2->formats[CSI2_PAD_SINK].code !=
+	    csi2->formats[CSI2_PAD_SOURCE].code)
+		csi2->dpcm_decompress = true;
+	else
+		csi2->dpcm_decompress = false;
+
+	csi2->contexts[0].format_id = csi2_ctx_map_format(csi2);
+
+	if (csi2->video_out.bpl_padding == 0)
+		csi2->contexts[0].data_offset = 0;
+	else
+		csi2->contexts[0].data_offset = csi2->video_out.bpl_value;
+
+	/*
+	 * Enable end of frame and end of line signals generation for
+	 * context 0. These signals are generated from CSI2 receiver to
+	 * qualify the last pixel of a frame and the last pixel of a line.
+	 * Without enabling the signals CSI2 receiver writes data to memory
+	 * beyond buffer size and/or data line offset is not handled correctly.
+	 */
+	csi2->contexts[0].eof_enabled = 1;
+	csi2->contexts[0].eol_enabled = 1;
+
+	csi2_irq_complexio1_set(csi2, 1);
+	csi2_irq_ctx_set(csi2, 1);
+	csi2_irq_status_set(csi2, 1);
+
+	/* Set configuration (timings, format and links) */
+	csi2_timing_config(csi2, timing);
+	csi2_recv_config(csi2, &csi2->ctrl);
+	csi2_ctx_config(csi2, &csi2->contexts[0]);
+
+	return 0;
+}
+
+/*
+ * csi2_print_status - Prints CSI2 debug information.
+ */
+#define CSI2_PRINT_REGISTER(iss, regs, name)\
+	dev_dbg(iss->dev, "###CSI2 " #name "=0x%08x\n", \
+		iss_reg_read(iss, regs, CSI2_##name))
+
+static void csi2_print_status(struct iss_csi2_device *csi2)
+{
+	struct iss_device *iss = csi2->iss;
+
+	if (!csi2->available)
+		return;
+
+	dev_dbg(iss->dev, "-------------CSI2 Register dump-------------\n");
+
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SYSCONFIG);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SYSSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, IRQENABLE);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, IRQSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTRL);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, DBG_H);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_CFG);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_IRQSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SHORT_PACKET);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_IRQENABLE);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, DBG_P);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, TIMING);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL1(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL2(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_DAT_OFST(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_PING_ADDR(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_PONG_ADDR(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_IRQENABLE(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_IRQSTATUS(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL3(0));
+
+	dev_dbg(iss->dev, "--------------------------------------------\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+/*
+ * csi2_isr_buffer - Does buffer handling at end-of-frame
+ * when writing to memory.
+ */
+static void csi2_isr_buffer(struct iss_csi2_device *csi2)
+{
+	struct iss_buffer *buffer;
+
+	csi2_ctx_enable(csi2, 0, 0);
+
+	buffer = omap4iss_video_buffer_next(&csi2->video_out);
+
+	/*
+	 * Let video queue operation restart engine if there is an underrun
+	 * condition.
+	 */
+	if (!buffer)
+		return;
+
+	csi2_set_outaddr(csi2, buffer->iss_addr);
+	csi2_ctx_enable(csi2, 0, 1);
+}
+
+static void csi2_isr_ctx(struct iss_csi2_device *csi2,
+			 struct iss_csi2_ctx_cfg *ctx)
+{
+	unsigned int n = ctx->ctxnum;
+	u32 status;
+
+	status = iss_reg_read(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n));
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n), status);
+
+	if (omap4iss_module_sync_is_stopping(&csi2->wait, &csi2->stopping))
+		return;
+
+	/* Propagate frame number */
+	if (status & CSI2_CTX_IRQ_FS) {
+		struct iss_pipeline *pipe =
+				     to_iss_pipeline(&csi2->subdev.entity);
+		u16 frame;
+		u16 delta;
+
+		frame = iss_reg_read(csi2->iss, csi2->regs1,
+				     CSI2_CTX_CTRL2(ctx->ctxnum))
+		      >> CSI2_CTX_CTRL2_FRAME_SHIFT;
+
+		if (frame == 0) {
+			/* A zero value means that the counter isn't implemented
+			 * by the source. Increment the frame number in software
+			 * in that case.
+			 */
+			atomic_inc(&pipe->frame_number);
+		} else {
+			/* Extend the 16 bit frame number to 32 bits by
+			 * computing the delta between two consecutive CSI2
+			 * frame numbers and adding it to the software frame
+			 * number. The hardware counter starts at 1 and wraps
+			 * from 0xffff to 1 without going through 0, so subtract
+			 * 1 when the counter wraps.
+			 */
+			delta = frame - ctx->frame;
+			if (frame < ctx->frame)
+				delta--;
+			ctx->frame = frame;
+
+			atomic_add(delta, &pipe->frame_number);
+		}
+	}
+
+	if (!(status & CSI2_CTX_IRQ_FE))
+		return;
+
+	/* Skip interrupts until we reach the frame skip count. The CSI2 will be
+	 * automatically disabled, as the frame skip count has been programmed
+	 * in the CSI2_CTx_CTRL1::COUNT field, so re-enable it.
+	 *
+	 * It would have been nice to rely on the FRAME_NUMBER interrupt instead
+	 * but it turned out that the interrupt is only generated when the CSI2
+	 * writes to memory (the CSI2_CTx_CTRL1::COUNT field is decreased
+	 * correctly and reaches 0 when data is forwarded to the video port only
+	 * but no interrupt arrives). Maybe a CSI2 hardware bug.
+	 */
+	if (csi2->frame_skip) {
+		csi2->frame_skip--;
+		if (csi2->frame_skip == 0) {
+			ctx->format_id = csi2_ctx_map_format(csi2);
+			csi2_ctx_config(csi2, ctx);
+			csi2_ctx_enable(csi2, n, 1);
+		}
+		return;
+	}
+
+	if (csi2->output & CSI2_OUTPUT_MEMORY)
+		csi2_isr_buffer(csi2);
+}
+
+/*
+ * omap4iss_csi2_isr - CSI2 interrupt handling.
+ */
+void omap4iss_csi2_isr(struct iss_csi2_device *csi2)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2->subdev.entity);
+	u32 csi2_irqstatus, cpxio1_irqstatus;
+	struct iss_device *iss = csi2->iss;
+
+	if (!csi2->available)
+		return;
+
+	csi2_irqstatus = iss_reg_read(csi2->iss, csi2->regs1, CSI2_IRQSTATUS);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQSTATUS, csi2_irqstatus);
+
+	/* Failure Cases */
+	if (csi2_irqstatus & CSI2_IRQ_COMPLEXIO_ERR) {
+		cpxio1_irqstatus = iss_reg_read(csi2->iss, csi2->regs1,
+						CSI2_COMPLEXIO_IRQSTATUS);
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQSTATUS,
+			      cpxio1_irqstatus);
+		dev_dbg(iss->dev, "CSI2: ComplexIO Error IRQ %x\n",
+			cpxio1_irqstatus);
+		pipe->error = true;
+	}
+
+	if (csi2_irqstatus & (CSI2_IRQ_OCP_ERR |
+			      CSI2_IRQ_SHORT_PACKET |
+			      CSI2_IRQ_ECC_NO_CORRECTION |
+			      CSI2_IRQ_COMPLEXIO_ERR |
+			      CSI2_IRQ_FIFO_OVF)) {
+		dev_dbg(iss->dev,
+			"CSI2 Err: OCP:%d SHORT:%d ECC:%d CPXIO:%d OVF:%d\n",
+			csi2_irqstatus & CSI2_IRQ_OCP_ERR ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_SHORT_PACKET ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_ECC_NO_CORRECTION ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_COMPLEXIO_ERR ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_FIFO_OVF ? 1 : 0);
+		pipe->error = true;
+	}
+
+	/* Successful cases */
+	if (csi2_irqstatus & CSI2_IRQ_CONTEXT0)
+		csi2_isr_ctx(csi2, &csi2->contexts[0]);
+
+	if (csi2_irqstatus & CSI2_IRQ_ECC_CORRECTION)
+		dev_dbg(iss->dev, "CSI2: ECC correction done\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS video operations
+ */
+
+/*
+ * csi2_queue - Queues the first buffer when using memory output
+ * @video: The video node
+ * @buffer: buffer to queue
+ */
+static int csi2_queue(struct iss_video *video, struct iss_buffer *buffer)
+{
+	struct iss_csi2_device *csi2 = container_of(video,
+				struct iss_csi2_device, video_out);
+
+	csi2_set_outaddr(csi2, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (csi2->video_out.dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		/* Enable / disable context 0 and IRQs */
+		csi2_if_enable(csi2, 1);
+		csi2_ctx_enable(csi2, 0, 1);
+		iss_video_dmaqueue_flags_clr(&csi2->video_out);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations csi2_issvideo_ops = {
+	.queue = csi2_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__csi2_get_format(struct iss_csi2_device *csi2,
+		  struct v4l2_subdev_state *sd_state,
+		  unsigned int pad,
+		  enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->subdev, sd_state,
+						  pad);
+
+	return &csi2->formats[pad];
+}
+
+static void
+csi2_try_format(struct iss_csi2_device *csi2,
+		struct v4l2_subdev_state *sd_state,
+		unsigned int pad,
+		struct v4l2_mbus_framefmt *fmt,
+		enum v4l2_subdev_format_whence which)
+{
+	u32 pixelcode;
+	struct v4l2_mbus_framefmt *format;
+	const struct iss_format_info *info;
+	unsigned int i;
+
+	switch (pad) {
+	case CSI2_PAD_SINK:
+		/* Clamp the width and height to valid range (1-8191). */
+		for (i = 0; i < ARRAY_SIZE(csi2_input_fmts); i++) {
+			if (fmt->code == csi2_input_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(csi2_input_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+		break;
+
+	case CSI2_PAD_SOURCE:
+		/* Source format same as sink format, except for DPCM
+		 * compression.
+		 */
+		pixelcode = fmt->code;
+		format = __csi2_get_format(csi2, sd_state, CSI2_PAD_SINK,
+					   which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		/*
+		 * Only Allow DPCM decompression, and check that the
+		 * pattern is preserved
+		 */
+		info = omap4iss_video_format_info(fmt->code);
+		if (info->uncompressed == pixelcode)
+			fmt->code = pixelcode;
+		break;
+	}
+
+	/* RGB, non-interlaced */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * csi2_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg    : V4L2 subdev pad config
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	const struct iss_format_info *info;
+
+	if (code->pad == CSI2_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(csi2_input_fmts))
+			return -EINVAL;
+
+		code->code = csi2_input_fmts[code->index];
+	} else {
+		format = __csi2_get_format(csi2, sd_state, CSI2_PAD_SINK,
+					   code->which);
+		switch (code->index) {
+		case 0:
+			/* Passthrough sink pad code */
+			code->code = format->code;
+			break;
+		case 1:
+			/* Uncompressed code */
+			info = omap4iss_video_format_info(format->code);
+			if (info->uncompressed == format->code)
+				return -EINVAL;
+
+			code->code = info->uncompressed;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int csi2_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	csi2_try_format(csi2, sd_state, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	csi2_try_format(csi2, sd_state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * csi2_get_format - Handle get format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad config
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_get_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * csi2_set_format - Handle set format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad config
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_set_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	csi2_try_format(csi2, sd_state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == CSI2_PAD_SINK) {
+		format = __csi2_get_format(csi2, sd_state, CSI2_PAD_SOURCE,
+					   fmt->which);
+		*format = fmt->format;
+		csi2_try_format(csi2, sd_state, CSI2_PAD_SOURCE, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+static int csi2_link_validate(struct v4l2_subdev *sd, struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2->subdev.entity);
+	int rval;
+
+	pipe->external = media_entity_to_v4l2_subdev(link->source->entity);
+	rval = omap4iss_get_external_info(pipe, link);
+	if (rval < 0)
+		return rval;
+
+	return v4l2_subdev_link_validate_default(sd, link, source_fmt,
+						 sink_fmt);
+}
+
+/*
+ * csi2_init_formats - Initialize formats on all pads
+ * @sd: ISS CSI2 V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int csi2_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = CSI2_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	csi2_set_format(sd, fh ? fh->state : NULL, &format);
+
+	return 0;
+}
+
+/*
+ * csi2_set_stream - Enable/Disable streaming on the CSI2 module
+ * @sd: ISS CSI2 V4L2 subdevice
+ * @enable: ISS pipeline stream state
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = csi2->iss;
+	struct iss_video *video_out = &csi2->video_out;
+	int ret = 0;
+
+	if (csi2->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_subclk_enable(iss, csi2->subclk);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS: {
+		ret = omap4iss_csiphy_config(iss, sd);
+		if (ret < 0)
+			return ret;
+
+		if (omap4iss_csiphy_acquire(csi2->phy) < 0)
+			return -ENODEV;
+		csi2_configure(csi2);
+		csi2_print_status(csi2);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (csi2->output & CSI2_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+		/* Enable context 0 and IRQs */
+		atomic_set(&csi2->stopping, 0);
+		csi2_ctx_enable(csi2, 0, 1);
+		csi2_if_enable(csi2, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (csi2->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &csi2->wait,
+					      &csi2->stopping))
+			ret = -ETIMEDOUT;
+		csi2_ctx_enable(csi2, 0, 0);
+		csi2_if_enable(csi2, 0);
+		csi2_irq_ctx_set(csi2, 0);
+		omap4iss_csiphy_release(csi2->phy);
+		omap4iss_subclk_disable(iss, csi2->subclk);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	csi2->state = enable;
+	return ret;
+}
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.enum_mbus_code = csi2_enum_mbus_code,
+	.enum_frame_size = csi2_enum_frame_size,
+	.get_fmt = csi2_get_format,
+	.set_fmt = csi2_set_format,
+	.link_validate = csi2_link_validate,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops csi2_ops = {
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.open = csi2_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * csi2_link_setup - Setup CSI2 connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_csi2_ctrl_cfg *ctrl = &csi2->ctrl;
+	unsigned int index = local->index;
+
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	/*
+	 * The ISS core doesn't support pipelines with multiple video outputs.
+	 * Revisit this when it will be implemented, and return -EBUSY for now.
+	 */
+
+	switch (index) {
+	case CSI2_PAD_SOURCE:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_MEMORY)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_MEMORY;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_MEMORY;
+		}
+		break;
+
+	case CSI2_PAD_SOURCE | 2 << 16:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_IPIPEIF)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_IPIPEIF;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_IPIPEIF;
+		}
+		break;
+
+	default:
+		/* Link from camera to CSI2 is fixed... */
+		return -EINVAL;
+	}
+
+	ctrl->vp_only_enable = csi2->output & CSI2_OUTPUT_MEMORY ? false : true;
+	ctrl->vp_clk_enable = !!(csi2->output & CSI2_OUTPUT_IPIPEIF);
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations csi2_media_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap4iss_csi2_unregister_entities(struct iss_csi2_device *csi2)
+{
+	v4l2_device_unregister_subdev(&csi2->subdev);
+	omap4iss_video_unregister(&csi2->video_out);
+}
+
+int omap4iss_csi2_register_entities(struct iss_csi2_device *csi2,
+				    struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&csi2->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_csi2_unregister_entities(csi2);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS CSI2 initialisation and cleanup
+ */
+
+/*
+ * csi2_init_entities - Initialize subdev and media entity.
+ * @csi2: Pointer to csi2 structure.
+ * return -ENOMEM or zero on success
+ */
+static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
+{
+	struct v4l2_subdev *sd = &csi2->subdev;
+	struct media_pad *pads = csi2->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+	char name[V4L2_SUBDEV_NAME_SIZE];
+
+	v4l2_subdev_init(sd, &csi2_ops);
+	sd->internal_ops = &csi2_internal_ops;
+	snprintf(name, sizeof(name), "CSI2%s", subname);
+	snprintf(sd->name, sizeof(sd->name), "OMAP4 ISS %s", name);
+
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, csi2);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+
+	me->ops = &csi2_media_ops;
+	ret = media_entity_pads_init(me, CSI2_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	csi2_init_formats(sd, NULL);
+
+	/* Video device node */
+	csi2->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	csi2->video_out.ops = &csi2_issvideo_ops;
+	csi2->video_out.bpl_alignment = 32;
+	csi2->video_out.bpl_zero_padding = 1;
+	csi2->video_out.bpl_max = 0x1ffe0;
+	csi2->video_out.iss = csi2->iss;
+	csi2->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+
+	ret = omap4iss_video_init(&csi2->video_out, name);
+	if (ret < 0)
+		goto error_video;
+
+	return 0;
+
+error_video:
+	media_entity_cleanup(&csi2->subdev.entity);
+	return ret;
+}
+
+/*
+ * omap4iss_csi2_init - Routine for module driver init
+ */
+int omap4iss_csi2_init(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+	int ret;
+
+	csi2a->iss = iss;
+	csi2a->available = 1;
+	csi2a->regs1 = OMAP4_ISS_MEM_CSI2_A_REGS1;
+	csi2a->phy = &iss->csiphy1;
+	csi2a->subclk = OMAP4_ISS_SUBCLK_CSI2_A;
+	csi2a->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&csi2a->wait);
+
+	ret = csi2_init_entities(csi2a, "a");
+	if (ret < 0)
+		return ret;
+
+	csi2b->iss = iss;
+	csi2b->available = 1;
+	csi2b->regs1 = OMAP4_ISS_MEM_CSI2_B_REGS1;
+	csi2b->phy = &iss->csiphy2;
+	csi2b->subclk = OMAP4_ISS_SUBCLK_CSI2_B;
+	csi2b->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&csi2b->wait);
+
+	ret = csi2_init_entities(csi2b, "b");
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * omap4iss_csi2_create_links() - CSI2 pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_csi2_create_links(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+	int ret;
+
+	/* Connect the CSI2a subdev to the video node. */
+	ret = media_create_pad_link(&csi2a->subdev.entity, CSI2_PAD_SOURCE,
+				    &csi2a->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Connect the CSI2b subdev to the video node. */
+	ret = media_create_pad_link(&csi2b->subdev.entity, CSI2_PAD_SOURCE,
+				    &csi2b->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * omap4iss_csi2_cleanup - Routine for module driver cleanup
+ */
+void omap4iss_csi2_cleanup(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+
+	omap4iss_video_cleanup(&csi2a->video_out);
+	media_entity_cleanup(&csi2a->subdev.entity);
+
+	omap4iss_video_cleanup(&csi2b->video_out);
+	media_entity_cleanup(&csi2b->subdev.entity);
+}
diff --git a/drivers/staging/media/omap4iss/iss_csi2.h b/drivers/staging/media/omap4iss/iss_csi2.h
new file mode 100644
index 000000000..3f7fd9cff
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csi2.h
@@ -0,0 +1,155 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI2 module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_CSI2_H
+#define OMAP4_ISS_CSI2_H
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include "iss_video.h"
+
+struct iss_csiphy;
+
+/* This is not an exhaustive list */
+enum iss_csi2_pix_formats {
+	CSI2_PIX_FMT_OTHERS = 0,
+	CSI2_PIX_FMT_YUV422_8BIT = 0x1e,
+	CSI2_PIX_FMT_YUV422_8BIT_VP = 0x9e,
+	CSI2_PIX_FMT_YUV422_8BIT_VP16 = 0xde,
+	CSI2_PIX_FMT_RAW10_EXP16 = 0xab,
+	CSI2_PIX_FMT_RAW10_EXP16_VP = 0x12f,
+	CSI2_PIX_FMT_RAW8 = 0x2a,
+	CSI2_PIX_FMT_RAW8_DPCM10_EXP16 = 0x2aa,
+	CSI2_PIX_FMT_RAW8_DPCM10_VP = 0x32a,
+	CSI2_PIX_FMT_RAW8_VP = 0x12a,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10_VP = 0x340,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10 = 0x2c0,
+	CSI2_USERDEF_8BIT_DATA1 = 0x40,
+};
+
+enum iss_csi2_irqevents {
+	OCP_ERR_IRQ = 0x4000,
+	SHORT_PACKET_IRQ = 0x2000,
+	ECC_CORRECTION_IRQ = 0x1000,
+	ECC_NO_CORRECTION_IRQ = 0x800,
+	COMPLEXIO2_ERR_IRQ = 0x400,
+	COMPLEXIO1_ERR_IRQ = 0x200,
+	FIFO_OVF_IRQ = 0x100,
+	CONTEXT7 = 0x80,
+	CONTEXT6 = 0x40,
+	CONTEXT5 = 0x20,
+	CONTEXT4 = 0x10,
+	CONTEXT3 = 0x8,
+	CONTEXT2 = 0x4,
+	CONTEXT1 = 0x2,
+	CONTEXT0 = 0x1,
+};
+
+enum iss_csi2_ctx_irqevents {
+	CTX_ECC_CORRECTION = 0x100,
+	CTX_LINE_NUMBER = 0x80,
+	CTX_FRAME_NUMBER = 0x40,
+	CTX_CS = 0x20,
+	CTX_LE = 0x8,
+	CTX_LS = 0x4,
+	CTX_FE = 0x2,
+	CTX_FS = 0x1,
+};
+
+enum iss_csi2_frame_mode {
+	ISS_CSI2_FRAME_IMMEDIATE,
+	ISS_CSI2_FRAME_AFTERFEC,
+};
+
+#define ISS_CSI2_MAX_CTX_NUM	7
+
+struct iss_csi2_ctx_cfg {
+	u8 ctxnum;		/* context number 0 - 7 */
+	u8 dpcm_decompress;
+
+	/* Fields in CSI2_CTx_CTRL2 - locked by CSI2_CTx_CTRL1.CTX_EN */
+	u8 virtual_id;
+	u16 format_id;		/* as in CSI2_CTx_CTRL2[9:0] */
+	u8 dpcm_predictor;	/* 1: simple, 0: advanced */
+	u16 frame;
+
+	/* Fields in CSI2_CTx_CTRL1/3 - Shadowed */
+	u16 alpha;
+	u16 data_offset;
+	u32 ping_addr;
+	u32 pong_addr;
+	u8 eof_enabled;
+	u8 eol_enabled;
+	u8 checksum_enabled;
+	u8 enabled;
+};
+
+struct iss_csi2_timing_cfg {
+	u8 ionum;			/* IO1 or IO2 as in CSI2_TIMING */
+	unsigned force_rx_mode:1;
+	unsigned stop_state_16x:1;
+	unsigned stop_state_4x:1;
+	u16 stop_state_counter;
+};
+
+struct iss_csi2_ctrl_cfg {
+	bool vp_clk_enable;
+	bool vp_only_enable;
+	u8 vp_out_ctrl;
+	enum iss_csi2_frame_mode frame_mode;
+	bool ecc_enable;
+	bool if_enable;
+};
+
+#define CSI2_PAD_SINK		0
+#define CSI2_PAD_SOURCE		1
+#define CSI2_PADS_NUM		2
+
+#define CSI2_OUTPUT_IPIPEIF	BIT(0)
+#define CSI2_OUTPUT_MEMORY	BIT(1)
+
+struct iss_csi2_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[CSI2_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[CSI2_PADS_NUM];
+
+	struct iss_video video_out;
+	struct iss_device *iss;
+
+	u8 available;		/* Is the IP present on the silicon? */
+
+	/* memory resources, as defined in enum iss_mem_resources */
+	unsigned int regs1;
+	unsigned int regs2;
+	/* ISP subclock, as defined in enum iss_isp_subclk_resource */
+	unsigned int subclk;
+
+	u32 output; /* output to IPIPEIF, memory or both? */
+	bool dpcm_decompress;
+	unsigned int frame_skip;
+
+	struct iss_csiphy *phy;
+	struct iss_csi2_ctx_cfg contexts[ISS_CSI2_MAX_CTX_NUM + 1];
+	struct iss_csi2_timing_cfg timing[2];
+	struct iss_csi2_ctrl_cfg ctrl;
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+void omap4iss_csi2_isr(struct iss_csi2_device *csi2);
+int omap4iss_csi2_reset(struct iss_csi2_device *csi2);
+int omap4iss_csi2_init(struct iss_device *iss);
+int omap4iss_csi2_create_links(struct iss_device *iss);
+void omap4iss_csi2_cleanup(struct iss_device *iss);
+void omap4iss_csi2_unregister_entities(struct iss_csi2_device *csi2);
+int omap4iss_csi2_register_entities(struct iss_csi2_device *csi2,
+				    struct v4l2_device *vdev);
+#endif	/* OMAP4_ISS_CSI2_H */
diff --git a/drivers/staging/media/omap4iss/iss_csiphy.c b/drivers/staging/media/omap4iss/iss_csiphy.c
new file mode 100644
index 000000000..96f2ce045
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csiphy.c
@@ -0,0 +1,277 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+
+#include "../../../../arch/arm/mach-omap2/control.h"
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_csiphy.h"
+
+/*
+ * csiphy_lanes_config - Configuration of CSIPHY lanes.
+ *
+ * Updates HW configuration.
+ * Called with phy->mutex taken.
+ */
+static void csiphy_lanes_config(struct iss_csiphy *phy)
+{
+	unsigned int i;
+	u32 reg;
+
+	reg = iss_reg_read(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG);
+
+	for (i = 0; i < phy->max_data_lanes; i++) {
+		reg &= ~(CSI2_COMPLEXIO_CFG_DATA_POL(i + 1) |
+			 CSI2_COMPLEXIO_CFG_DATA_POSITION_MASK(i + 1));
+		reg |= (phy->lanes.data[i].pol ?
+			CSI2_COMPLEXIO_CFG_DATA_POL(i + 1) : 0);
+		reg |= (phy->lanes.data[i].pos <<
+			CSI2_COMPLEXIO_CFG_DATA_POSITION_SHIFT(i + 1));
+	}
+
+	reg &= ~(CSI2_COMPLEXIO_CFG_CLOCK_POL |
+		 CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK);
+	reg |= phy->lanes.clk.pol ? CSI2_COMPLEXIO_CFG_CLOCK_POL : 0;
+	reg |= phy->lanes.clk.pos << CSI2_COMPLEXIO_CFG_CLOCK_POSITION_SHIFT;
+
+	iss_reg_write(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG, reg);
+}
+
+/*
+ * csiphy_set_power
+ * @power: Power state to be set.
+ *
+ * Returns 0 if successful, or -EBUSY if the retry count is exceeded.
+ */
+static int csiphy_set_power(struct iss_csiphy *phy, u32 power)
+{
+	u32 reg;
+	u8 retry_count;
+
+	iss_reg_update(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG,
+		       CSI2_COMPLEXIO_CFG_PWD_CMD_MASK,
+		       power | CSI2_COMPLEXIO_CFG_PWR_AUTO);
+
+	retry_count = 0;
+	do {
+		udelay(1);
+		reg = iss_reg_read(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG)
+		    & CSI2_COMPLEXIO_CFG_PWD_STATUS_MASK;
+
+		if (reg != power >> 2)
+			retry_count++;
+
+	} while ((reg != power >> 2) && (retry_count < 250));
+
+	if (retry_count == 250) {
+		dev_err(phy->iss->dev, "CSI2 CIO set power failed!\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_dphy_config - Configure CSI2 D-PHY parameters.
+ *
+ * Called with phy->mutex taken.
+ */
+static void csiphy_dphy_config(struct iss_csiphy *phy)
+{
+	u32 reg;
+
+	/* Set up REGISTER0 */
+	reg = phy->dphy.ths_term << REGISTER0_THS_TERM_SHIFT;
+	reg |= phy->dphy.ths_settle << REGISTER0_THS_SETTLE_SHIFT;
+
+	iss_reg_write(phy->iss, phy->phy_regs, REGISTER0, reg);
+
+	/* Set up REGISTER1 */
+	reg = phy->dphy.tclk_term << REGISTER1_TCLK_TERM_SHIFT;
+	reg |= phy->dphy.tclk_miss << REGISTER1_CTRLCLK_DIV_FACTOR_SHIFT;
+	reg |= phy->dphy.tclk_settle << REGISTER1_TCLK_SETTLE_SHIFT;
+	reg |= 0xb8 << REGISTER1_DPHY_HS_SYNC_PATTERN_SHIFT;
+
+	iss_reg_write(phy->iss, phy->phy_regs, REGISTER1, reg);
+}
+
+/*
+ * TCLK values are OK at their reset values
+ */
+#define TCLK_TERM	0
+#define TCLK_MISS	1
+#define TCLK_SETTLE	14
+
+int omap4iss_csiphy_config(struct iss_device *iss,
+			   struct v4l2_subdev *csi2_subdev)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(csi2_subdev);
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2_subdev->entity);
+	struct iss_v4l2_subdevs_group *subdevs = pipe->external->host_priv;
+	struct iss_csiphy_dphy_cfg csi2phy;
+	int csi2_ddrclk_khz;
+	struct iss_csiphy_lanes_cfg *lanes;
+	unsigned int used_lanes = 0;
+	u32 cam_rx_ctrl;
+	unsigned int i;
+
+	lanes = &subdevs->bus.csi2.lanecfg;
+
+	/*
+	 * SCM.CONTROL_CAMERA_RX
+	 * - bit [31] : CSIPHY2 lane 2 enable (4460+ only)
+	 * - bit [30:29] : CSIPHY2 per-lane enable (1 to 0)
+	 * - bit [28:24] : CSIPHY1 per-lane enable (4 to 0)
+	 * - bit [21] : CSIPHY2 CTRLCLK enable
+	 * - bit [20:19] : CSIPHY2 config: 00 d-phy, 01/10 ccp2
+	 * - bit [18] : CSIPHY1 CTRLCLK enable
+	 * - bit [17:16] : CSIPHY1 config: 00 d-phy, 01/10 ccp2
+	 */
+	/*
+	 * TODO: When implementing DT support specify the CONTROL_CAMERA_RX
+	 * register offset in the syscon property instead of hardcoding it.
+	 */
+	regmap_read(iss->syscon, 0x68, &cam_rx_ctrl);
+
+	if (subdevs->interface == ISS_INTERFACE_CSI2A_PHY1) {
+		cam_rx_ctrl &= ~(OMAP4_CAMERARX_CSI21_LANEENABLE_MASK |
+				OMAP4_CAMERARX_CSI21_CAMMODE_MASK);
+		/* NOTE: Leave CSIPHY1 config to 0x0: D-PHY mode */
+		/* Enable all lanes for now */
+		cam_rx_ctrl |=
+			0x1f << OMAP4_CAMERARX_CSI21_LANEENABLE_SHIFT;
+		/* Enable CTRLCLK */
+		cam_rx_ctrl |= OMAP4_CAMERARX_CSI21_CTRLCLKEN_MASK;
+	}
+
+	if (subdevs->interface == ISS_INTERFACE_CSI2B_PHY2) {
+		cam_rx_ctrl &= ~(OMAP4_CAMERARX_CSI22_LANEENABLE_MASK |
+				OMAP4_CAMERARX_CSI22_CAMMODE_MASK);
+		/* NOTE: Leave CSIPHY2 config to 0x0: D-PHY mode */
+		/* Enable all lanes for now */
+		cam_rx_ctrl |=
+			0x3 << OMAP4_CAMERARX_CSI22_LANEENABLE_SHIFT;
+		/* Enable CTRLCLK */
+		cam_rx_ctrl |= OMAP4_CAMERARX_CSI22_CTRLCLKEN_MASK;
+	}
+
+	regmap_write(iss->syscon, 0x68, cam_rx_ctrl);
+
+	/* Reset used lane count */
+	csi2->phy->used_data_lanes = 0;
+
+	/* Clock and data lanes verification */
+	for (i = 0; i < csi2->phy->max_data_lanes; i++) {
+		if (lanes->data[i].pos == 0)
+			continue;
+
+		if (lanes->data[i].pol > 1 ||
+		    lanes->data[i].pos > (csi2->phy->max_data_lanes + 1))
+			return -EINVAL;
+
+		if (used_lanes & (1 << lanes->data[i].pos))
+			return -EINVAL;
+
+		used_lanes |= 1 << lanes->data[i].pos;
+		csi2->phy->used_data_lanes++;
+	}
+
+	if (lanes->clk.pol > 1 ||
+	    lanes->clk.pos > (csi2->phy->max_data_lanes + 1))
+		return -EINVAL;
+
+	if (lanes->clk.pos == 0 || used_lanes & (1 << lanes->clk.pos))
+		return -EINVAL;
+
+	csi2_ddrclk_khz = pipe->external_rate / 1000
+		/ (2 * csi2->phy->used_data_lanes)
+		* pipe->external_bpp;
+
+	/*
+	 * THS_TERM: Programmed value = ceil(12.5 ns/DDRClk period) - 1.
+	 * THS_SETTLE: Programmed value = ceil(90 ns/DDRClk period) + 3.
+	 */
+	csi2phy.ths_term = DIV_ROUND_UP(25 * csi2_ddrclk_khz, 2000000) - 1;
+	csi2phy.ths_settle = DIV_ROUND_UP(90 * csi2_ddrclk_khz, 1000000) + 3;
+	csi2phy.tclk_term = TCLK_TERM;
+	csi2phy.tclk_miss = TCLK_MISS;
+	csi2phy.tclk_settle = TCLK_SETTLE;
+
+	mutex_lock(&csi2->phy->mutex);
+	csi2->phy->dphy = csi2phy;
+	csi2->phy->lanes = *lanes;
+	mutex_unlock(&csi2->phy->mutex);
+
+	return 0;
+}
+
+int omap4iss_csiphy_acquire(struct iss_csiphy *phy)
+{
+	int rval;
+
+	mutex_lock(&phy->mutex);
+
+	rval = omap4iss_csi2_reset(phy->csi2);
+	if (rval)
+		goto done;
+
+	csiphy_dphy_config(phy);
+	csiphy_lanes_config(phy);
+
+	rval = csiphy_set_power(phy, CSI2_COMPLEXIO_CFG_PWD_CMD_ON);
+	if (rval)
+		goto done;
+
+	phy->phy_in_use = 1;
+
+done:
+	mutex_unlock(&phy->mutex);
+	return rval;
+}
+
+void omap4iss_csiphy_release(struct iss_csiphy *phy)
+{
+	mutex_lock(&phy->mutex);
+	if (phy->phy_in_use) {
+		csiphy_set_power(phy, CSI2_COMPLEXIO_CFG_PWD_CMD_OFF);
+		phy->phy_in_use = 0;
+	}
+	mutex_unlock(&phy->mutex);
+}
+
+/*
+ * omap4iss_csiphy_init - Initialize the CSI PHY frontends
+ */
+int omap4iss_csiphy_init(struct iss_device *iss)
+{
+	struct iss_csiphy *phy1 = &iss->csiphy1;
+	struct iss_csiphy *phy2 = &iss->csiphy2;
+
+	phy1->iss = iss;
+	phy1->csi2 = &iss->csi2a;
+	phy1->max_data_lanes = ISS_CSIPHY1_NUM_DATA_LANES;
+	phy1->used_data_lanes = 0;
+	phy1->cfg_regs = OMAP4_ISS_MEM_CSI2_A_REGS1;
+	phy1->phy_regs = OMAP4_ISS_MEM_CAMERARX_CORE1;
+	mutex_init(&phy1->mutex);
+
+	phy2->iss = iss;
+	phy2->csi2 = &iss->csi2b;
+	phy2->max_data_lanes = ISS_CSIPHY2_NUM_DATA_LANES;
+	phy2->used_data_lanes = 0;
+	phy2->cfg_regs = OMAP4_ISS_MEM_CSI2_B_REGS1;
+	phy2->phy_regs = OMAP4_ISS_MEM_CAMERARX_CORE2;
+	mutex_init(&phy2->mutex);
+
+	return 0;
+}
diff --git a/drivers/staging/media/omap4iss/iss_csiphy.h b/drivers/staging/media/omap4iss/iss_csiphy.h
new file mode 100644
index 000000000..44408e4fc
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csiphy.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_CSI_PHY_H
+#define OMAP4_ISS_CSI_PHY_H
+
+#include <linux/platform_data/media/omap4iss.h>
+
+struct iss_csi2_device;
+
+struct iss_csiphy_dphy_cfg {
+	u8 ths_term;
+	u8 ths_settle;
+	u8 tclk_term;
+	unsigned tclk_miss:1;
+	u8 tclk_settle;
+};
+
+struct iss_csiphy {
+	struct iss_device *iss;
+	struct mutex mutex;	/* serialize csiphy configuration */
+	u8 phy_in_use;
+	struct iss_csi2_device *csi2;
+
+	/* memory resources, as defined in enum iss_mem_resources */
+	unsigned int cfg_regs;
+	unsigned int phy_regs;
+
+	u8 max_data_lanes;	/* number of CSI2 Data Lanes supported */
+	u8 used_data_lanes;	/* number of CSI2 Data Lanes used */
+	struct iss_csiphy_lanes_cfg lanes;
+	struct iss_csiphy_dphy_cfg dphy;
+};
+
+int omap4iss_csiphy_config(struct iss_device *iss,
+			   struct v4l2_subdev *csi2_subdev);
+int omap4iss_csiphy_acquire(struct iss_csiphy *phy);
+void omap4iss_csiphy_release(struct iss_csiphy *phy);
+int omap4iss_csiphy_init(struct iss_device *iss);
+
+#endif	/* OMAP4_ISS_CSI_PHY_H */
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
new file mode 100644
index 000000000..23f707cb3
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -0,0 +1,580 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPE module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_ipipe.h"
+
+static struct v4l2_mbus_framefmt *
+__ipipe_get_format(struct iss_ipipe_device *ipipe,
+		   struct v4l2_subdev_state *sd_state,
+		   unsigned int pad,
+		   enum v4l2_subdev_format_whence which);
+
+static const unsigned int ipipe_fmts[] = {
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+};
+
+/*
+ * ipipe_print_status - Print current IPIPE Module register values.
+ * @ipipe: Pointer to ISS ISP IPIPE device.
+ *
+ * Also prints other debug information stored in the IPIPE module.
+ */
+#define IPIPE_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###IPIPE " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_##name))
+
+static void ipipe_print_status(struct iss_ipipe_device *ipipe)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	dev_dbg(iss->dev, "-------------IPIPE Register dump-------------\n");
+
+	IPIPE_PRINT_REGISTER(iss, SRC_EN);
+	IPIPE_PRINT_REGISTER(iss, SRC_MODE);
+	IPIPE_PRINT_REGISTER(iss, SRC_FMT);
+	IPIPE_PRINT_REGISTER(iss, SRC_COL);
+	IPIPE_PRINT_REGISTER(iss, SRC_VPS);
+	IPIPE_PRINT_REGISTER(iss, SRC_VSZ);
+	IPIPE_PRINT_REGISTER(iss, SRC_HPS);
+	IPIPE_PRINT_REGISTER(iss, SRC_HSZ);
+	IPIPE_PRINT_REGISTER(iss, GCK_MMR);
+	IPIPE_PRINT_REGISTER(iss, YUV_PHS);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * ipipe_enable - Enable/Disable IPIPE.
+ * @enable: enable flag
+ *
+ */
+static void ipipe_enable(struct iss_ipipe_device *ipipe, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_EN,
+		       IPIPE_SRC_EN_EN, enable ? IPIPE_SRC_EN_EN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+static void ipipe_configure(struct iss_ipipe_device *ipipe)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+	struct v4l2_mbus_framefmt *format;
+
+	/* IPIPE_PAD_SINK */
+	format = &ipipe->formats[IPIPE_PAD_SINK];
+
+	/* NOTE: Currently just supporting pipeline IN: RGB, OUT: YUV422 */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_FMT,
+		      IPIPE_SRC_FMT_RAW2YUV);
+
+	/* Enable YUV444 -> YUV422 conversion */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_YUV_PHS,
+		      IPIPE_YUV_PHS_LPF);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_HPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_VSZ,
+		      (format->height - 2) & IPIPE_SRC_VSZ_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_HSZ,
+		      (format->width - 1) & IPIPE_SRC_HSZ_MASK);
+
+	/* Ignore ipipeif_wrt signal, and operate on-the-fly.  */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_MODE,
+		    IPIPE_SRC_MODE_WRT | IPIPE_SRC_MODE_OST);
+
+	/* HACK: Values tuned for Ducati SW (OV) */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_COL,
+		      IPIPE_SRC_COL_EE_B | IPIPE_SRC_COL_EO_GB |
+		      IPIPE_SRC_COL_OE_GR | IPIPE_SRC_COL_OO_R);
+
+	/* IPIPE_PAD_SOURCE_VP */
+	format = &ipipe->formats[IPIPE_PAD_SOURCE_VP];
+	/* Do nothing? */
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * ipipe_set_stream - Enable/Disable streaming on the IPIPE module
+ * @sd: ISP IPIPE V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int ipipe_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipe);
+	int ret = 0;
+
+	if (ipipe->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, OMAP4_ISS_ISP_SUBCLK_IPIPE);
+
+		/* Enable clk_arm_g0 */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_GCK_MMR,
+			      IPIPE_GCK_MMR_REG);
+
+		/* Enable clk_pix_g[3:0] */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_GCK_PIX,
+			      IPIPE_GCK_PIX_G3 | IPIPE_GCK_PIX_G2 |
+			      IPIPE_GCK_PIX_G1 | IPIPE_GCK_PIX_G0);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		ipipe_configure(ipipe);
+		ipipe_print_status(ipipe);
+
+		atomic_set(&ipipe->stopping, 0);
+		ipipe_enable(ipipe, 1);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (ipipe->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &ipipe->wait,
+					      &ipipe->stopping))
+			ret = -ETIMEDOUT;
+
+		ipipe_enable(ipipe, 0);
+		omap4iss_isp_subclk_disable(iss, OMAP4_ISS_ISP_SUBCLK_IPIPE);
+		break;
+	}
+
+	ipipe->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__ipipe_get_format(struct iss_ipipe_device *ipipe,
+		   struct v4l2_subdev_state *sd_state,
+		   unsigned int pad,
+		   enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ipipe->subdev, sd_state,
+						  pad);
+
+	return &ipipe->formats[pad];
+}
+
+/*
+ * ipipe_try_format - Try video format on a pad
+ * @ipipe: ISS IPIPE device
+ * @cfg: V4L2 subdev pad config
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+ipipe_try_format(struct iss_ipipe_device *ipipe,
+		 struct v4l2_subdev_state *sd_state,
+		 unsigned int pad,
+		 struct v4l2_mbus_framefmt *fmt,
+		 enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case IPIPE_PAD_SINK:
+		for (i = 0; i < ARRAY_SIZE(ipipe_fmts); i++) {
+			if (fmt->code == ipipe_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(ipipe_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+
+	case IPIPE_PAD_SOURCE_VP:
+		format = __ipipe_get_format(ipipe, sd_state, IPIPE_PAD_SINK,
+					    which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		fmt->width = clamp_t(u32, width, 32, fmt->width);
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		fmt->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * ipipe_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg    : V4L2 subdev pad config
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ipipe_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->pad) {
+	case IPIPE_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(ipipe_fmts))
+			return -EINVAL;
+
+		code->code = ipipe_fmts[code->index];
+		break;
+
+	case IPIPE_PAD_SOURCE_VP:
+		/* FIXME: Forced format conversion inside IPIPE ? */
+		if (code->index != 0)
+			return -EINVAL;
+
+		code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ipipe_enum_frame_size(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ipipe_try_format(ipipe, sd_state, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ipipe_try_format(ipipe, sd_state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ipipe_get_format - Retrieve the video format on a pad
+ * @sd : ISP IPIPE V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipe_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipe_get_format(ipipe, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ipipe_set_format - Set the video format on a pad
+ * @sd : ISP IPIPE V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipe_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipe_get_format(ipipe, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	ipipe_try_format(ipipe, sd_state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == IPIPE_PAD_SINK) {
+		format = __ipipe_get_format(ipipe, sd_state,
+					    IPIPE_PAD_SOURCE_VP,
+					    fmt->which);
+		*format = fmt->format;
+		ipipe_try_format(ipipe, sd_state, IPIPE_PAD_SOURCE_VP, format,
+				 fmt->which);
+	}
+
+	return 0;
+}
+
+static int ipipe_link_validate(struct v4l2_subdev *sd, struct media_link *link,
+			       struct v4l2_subdev_format *source_fmt,
+			       struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * ipipe_init_formats - Initialize formats on all pads
+ * @sd: ISP IPIPE V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ipipe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = IPIPE_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ipipe_set_format(sd, fh ? fh->state : NULL, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops ipipe_v4l2_video_ops = {
+	.s_stream = ipipe_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops ipipe_v4l2_pad_ops = {
+	.enum_mbus_code = ipipe_enum_mbus_code,
+	.enum_frame_size = ipipe_enum_frame_size,
+	.get_fmt = ipipe_get_format,
+	.set_fmt = ipipe_set_format,
+	.link_validate = ipipe_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops ipipe_v4l2_ops = {
+	.video = &ipipe_v4l2_video_ops,
+	.pad = &ipipe_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops ipipe_v4l2_internal_ops = {
+	.open = ipipe_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ipipe_link_setup - Setup IPIPE connections
+ * @entity: IPIPE media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int ipipe_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	if (!is_media_entity_v4l2_subdev(remote->entity))
+		return -EINVAL;
+
+	switch (local->index) {
+	case IPIPE_PAD_SINK:
+		/* Read from IPIPEIF. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			ipipe->input = IPIPE_INPUT_NONE;
+			break;
+		}
+
+		if (ipipe->input != IPIPE_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->ipipeif.subdev.entity)
+			ipipe->input = IPIPE_INPUT_IPIPEIF;
+
+		break;
+
+	case IPIPE_PAD_SOURCE_VP:
+		/* Send to RESIZER */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipe->output & ~IPIPE_OUTPUT_VP)
+				return -EBUSY;
+			ipipe->output |= IPIPE_OUTPUT_VP;
+		} else {
+			ipipe->output &= ~IPIPE_OUTPUT_VP;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ipipe_media_ops = {
+	.link_setup = ipipe_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * ipipe_init_entities - Initialize V4L2 subdev and media entity
+ * @ipipe: ISS ISP IPIPE module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int ipipe_init_entities(struct iss_ipipe_device *ipipe)
+{
+	struct v4l2_subdev *sd = &ipipe->subdev;
+	struct media_pad *pads = ipipe->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ipipe->input = IPIPE_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &ipipe_v4l2_ops);
+	sd->internal_ops = &ipipe_v4l2_internal_ops;
+	strscpy(sd->name, "OMAP4 ISS ISP IPIPE", sizeof(sd->name));
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, ipipe);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[IPIPE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[IPIPE_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ipipe_media_ops;
+	ret = media_entity_pads_init(me, IPIPE_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	ipipe_init_formats(sd, NULL);
+
+	return 0;
+}
+
+void omap4iss_ipipe_unregister_entities(struct iss_ipipe_device *ipipe)
+{
+	v4l2_device_unregister_subdev(&ipipe->subdev);
+}
+
+int omap4iss_ipipe_register_entities(struct iss_ipipe_device *ipipe,
+				     struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ipipe->subdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_ipipe_unregister_entities(ipipe);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP IPIPE initialisation and cleanup
+ */
+
+/*
+ * omap4iss_ipipe_init - IPIPE module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_ipipe_init(struct iss_device *iss)
+{
+	struct iss_ipipe_device *ipipe = &iss->ipipe;
+
+	ipipe->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&ipipe->wait);
+
+	return ipipe_init_entities(ipipe);
+}
+
+/*
+ * omap4iss_ipipe_cleanup - IPIPE module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_ipipe_cleanup(struct iss_device *iss)
+{
+	struct iss_ipipe_device *ipipe = &iss->ipipe;
+
+	media_entity_cleanup(&ipipe->subdev.entity);
+}
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.h b/drivers/staging/media/omap4iss/iss_ipipe.h
new file mode 100644
index 000000000..53b42aac1
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipe.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPE module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_IPIPE_H
+#define OMAP4_ISS_IPIPE_H
+
+#include "iss_video.h"
+
+enum ipipe_input_entity {
+	IPIPE_INPUT_NONE,
+	IPIPE_INPUT_IPIPEIF,
+};
+
+#define IPIPE_OUTPUT_VP				BIT(0)
+
+/* Sink and source IPIPE pads */
+#define IPIPE_PAD_SINK				0
+#define IPIPE_PAD_SOURCE_VP			1
+#define IPIPE_PADS_NUM				2
+
+/*
+ * struct iss_ipipe_device - Structure for the IPIPE module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @error: A hardware error occurred during capture
+ * @state: Streaming state
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ */
+struct iss_ipipe_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[IPIPE_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[IPIPE_PADS_NUM];
+
+	enum ipipe_input_entity input;
+	unsigned int output;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_ipipe_register_entities(struct iss_ipipe_device *ipipe,
+				     struct v4l2_device *vdev);
+void omap4iss_ipipe_unregister_entities(struct iss_ipipe_device *ipipe);
+
+int omap4iss_ipipe_init(struct iss_device *iss);
+void omap4iss_ipipe_cleanup(struct iss_device *iss);
+
+#endif	/* OMAP4_ISS_IPIPE_H */
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
new file mode 100644
index 000000000..5e7f25cd5
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -0,0 +1,845 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPEIF module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_ipipeif.h"
+
+static const unsigned int ipipeif_fmts[] = {
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+};
+
+/*
+ * ipipeif_print_status - Print current IPIPEIF Module register values.
+ * @ipipeif: Pointer to ISS ISP IPIPEIF device.
+ *
+ * Also prints other debug information stored in the IPIPEIF module.
+ */
+#define IPIPEIF_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###IPIPEIF " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_##name))
+
+#define ISIF_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISIF " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_##name))
+
+#define ISP5_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISP5 " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_##name))
+
+static void ipipeif_print_status(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	dev_dbg(iss->dev, "-------------IPIPEIF Register dump-------------\n");
+
+	IPIPEIF_PRINT_REGISTER(iss, CFG1);
+	IPIPEIF_PRINT_REGISTER(iss, CFG2);
+
+	ISIF_PRINT_REGISTER(iss, SYNCEN);
+	ISIF_PRINT_REGISTER(iss, CADU);
+	ISIF_PRINT_REGISTER(iss, CADL);
+	ISIF_PRINT_REGISTER(iss, MODESET);
+	ISIF_PRINT_REGISTER(iss, CCOLP);
+	ISIF_PRINT_REGISTER(iss, SPH);
+	ISIF_PRINT_REGISTER(iss, LNH);
+	ISIF_PRINT_REGISTER(iss, LNV);
+	ISIF_PRINT_REGISTER(iss, VDINT(0));
+	ISIF_PRINT_REGISTER(iss, HSIZE);
+
+	ISP5_PRINT_REGISTER(iss, SYSCONFIG);
+	ISP5_PRINT_REGISTER(iss, CTRL);
+	ISP5_PRINT_REGISTER(iss, IRQSTATUS(0));
+	ISP5_PRINT_REGISTER(iss, IRQENABLE_SET(0));
+	ISP5_PRINT_REGISTER(iss, IRQENABLE_CLR(0));
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+static void ipipeif_write_enable(struct iss_ipipeif_device *ipipeif, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SYNCEN,
+		       ISIF_SYNCEN_DWEN, enable ? ISIF_SYNCEN_DWEN : 0);
+}
+
+/*
+ * ipipeif_enable - Enable/Disable IPIPEIF.
+ * @enable: enable flag
+ *
+ */
+static void ipipeif_enable(struct iss_ipipeif_device *ipipeif, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SYNCEN,
+		       ISIF_SYNCEN_SYEN, enable ? ISIF_SYNCEN_SYEN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+/*
+ * ipipeif_set_outaddr - Set memory address to save output image
+ * @ipipeif: Pointer to ISP IPIPEIF device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ */
+static void ipipeif_set_outaddr(struct iss_ipipeif_device *ipipeif, u32 addr)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	/* Save address split in Base Address H & L */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CADU,
+		      (addr >> (16 + 5)) & ISIF_CADU_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CADL,
+		      (addr >> 5) & ISIF_CADL_MASK);
+}
+
+static void ipipeif_configure(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+	const struct iss_format_info *info;
+	struct v4l2_mbus_framefmt *format;
+	u32 isif_ccolp = 0;
+
+	omap4iss_configure_bridge(iss, ipipeif->input);
+
+	/* IPIPEIF_PAD_SINK */
+	format = &ipipeif->formats[IPIPEIF_PAD_SINK];
+
+	/* IPIPEIF with YUV422 input from ISIF */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG1,
+		    IPIPEIF_CFG1_INPSRC1_MASK | IPIPEIF_CFG1_INPSRC2_MASK);
+
+	/* Select ISIF/IPIPEIF input format */
+	switch (format->code) {
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_MODESET,
+			       ISIF_MODESET_CCDMD | ISIF_MODESET_INPMOD_MASK |
+			       ISIF_MODESET_CCDW_MASK,
+			       ISIF_MODESET_INPMOD_YCBCR16);
+
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG2,
+			       IPIPEIF_CFG2_YUV8, IPIPEIF_CFG2_YUV16);
+
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_GR |
+			ISIF_CCOLP_CP1_F0_R |
+			ISIF_CCOLP_CP2_F0_B |
+			ISIF_CCOLP_CP3_F0_GB;
+		goto cont_raw;
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_R |
+			ISIF_CCOLP_CP1_F0_GR |
+			ISIF_CCOLP_CP2_F0_GB |
+			ISIF_CCOLP_CP3_F0_B;
+		goto cont_raw;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_B |
+			ISIF_CCOLP_CP1_F0_GB |
+			ISIF_CCOLP_CP2_F0_GR |
+			ISIF_CCOLP_CP3_F0_R;
+		goto cont_raw;
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_GB |
+			ISIF_CCOLP_CP1_F0_B |
+			ISIF_CCOLP_CP2_F0_R |
+			ISIF_CCOLP_CP3_F0_GR;
+cont_raw:
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG2,
+			    IPIPEIF_CFG2_YUV16);
+
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_MODESET,
+			       ISIF_MODESET_CCDMD | ISIF_MODESET_INPMOD_MASK |
+			       ISIF_MODESET_CCDW_MASK, ISIF_MODESET_INPMOD_RAW |
+			       ISIF_MODESET_CCDW_2BIT);
+
+		info = omap4iss_video_format_info(format->code);
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CGAMMAWD,
+			       ISIF_CGAMMAWD_GWDI_MASK,
+			       ISIF_CGAMMAWD_GWDI(info->bpp));
+
+		/* Set RAW Bayer pattern */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CCOLP,
+			      isif_ccolp);
+		break;
+	}
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SPH, 0 & ISIF_SPH_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_LNH,
+		      (format->width - 1) & ISIF_LNH_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_LNV,
+		      (format->height - 1) & ISIF_LNV_MASK);
+
+	/* Generate ISIF0 on the last line of the image */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_VDINT(0),
+		      format->height - 1);
+
+	/* IPIPEIF_PAD_SOURCE_ISIF_SF */
+	format = &ipipeif->formats[IPIPEIF_PAD_SOURCE_ISIF_SF];
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_HSIZE,
+		      (ipipeif->video_out.bpl_value >> 5) &
+		      ISIF_HSIZE_HSIZE_MASK);
+
+	/* IPIPEIF_PAD_SOURCE_VP */
+	/* Do nothing? */
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void ipipeif_isr_buffer(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_buffer *buffer;
+
+	/* The ISIF generates VD0 interrupts even when writes are disabled.
+	 * deal with it anyway). Disabling the ISIF when no buffer is available
+	 * is thus not be enough, we need to handle the situation explicitly.
+	 */
+	if (list_empty(&ipipeif->video_out.dmaqueue))
+		return;
+
+	ipipeif_write_enable(ipipeif, 0);
+
+	buffer = omap4iss_video_buffer_next(&ipipeif->video_out);
+	if (!buffer)
+		return;
+
+	ipipeif_set_outaddr(ipipeif, buffer->iss_addr);
+
+	ipipeif_write_enable(ipipeif, 1);
+}
+
+/*
+ * omap4iss_ipipeif_isr - Configure ipipeif during interframe time.
+ * @ipipeif: Pointer to ISP IPIPEIF device.
+ * @events: IPIPEIF events
+ */
+void omap4iss_ipipeif_isr(struct iss_ipipeif_device *ipipeif, u32 events)
+{
+	if (omap4iss_module_sync_is_stopping(&ipipeif->wait,
+					     &ipipeif->stopping))
+		return;
+
+	if ((events & ISP5_IRQ_ISIF_INT(0)) &&
+	    (ipipeif->output & IPIPEIF_OUTPUT_MEMORY))
+		ipipeif_isr_buffer(ipipeif);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+static int ipipeif_video_queue(struct iss_video *video,
+			       struct iss_buffer *buffer)
+{
+	struct iss_ipipeif_device *ipipeif = container_of(video,
+				struct iss_ipipeif_device, video_out);
+
+	if (!(ipipeif->output & IPIPEIF_OUTPUT_MEMORY))
+		return -ENODEV;
+
+	ipipeif_set_outaddr(ipipeif, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 1);
+		ipipeif_enable(ipipeif, 1);
+		iss_video_dmaqueue_flags_clr(video);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations ipipeif_video_ops = {
+	.queue = ipipeif_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+#define IPIPEIF_DRV_SUBCLK_MASK	(OMAP4_ISS_ISP_SUBCLK_IPIPEIF |\
+				 OMAP4_ISS_ISP_SUBCLK_ISIF)
+/*
+ * ipipeif_set_stream - Enable/Disable streaming on the IPIPEIF module
+ * @sd: ISP IPIPEIF V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int ipipeif_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipeif);
+	struct iss_video *video_out = &ipipeif->video_out;
+	int ret = 0;
+
+	if (ipipeif->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, IPIPEIF_DRV_SUBCLK_MASK);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		ipipeif_configure(ipipeif);
+		ipipeif_print_status(ipipeif);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+
+		atomic_set(&ipipeif->stopping, 0);
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 1);
+		ipipeif_enable(ipipeif, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (ipipeif->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &ipipeif->wait,
+					      &ipipeif->stopping))
+			ret = -ETIMEDOUT;
+
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 0);
+		ipipeif_enable(ipipeif, 0);
+		omap4iss_isp_subclk_disable(iss, IPIPEIF_DRV_SUBCLK_MASK);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	ipipeif->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__ipipeif_get_format(struct iss_ipipeif_device *ipipeif,
+		     struct v4l2_subdev_state *sd_state, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ipipeif->subdev, sd_state,
+						  pad);
+	return &ipipeif->formats[pad];
+}
+
+/*
+ * ipipeif_try_format - Try video format on a pad
+ * @ipipeif: ISS IPIPEIF device
+ * @cfg: V4L2 subdev pad config
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+ipipeif_try_format(struct iss_ipipeif_device *ipipeif,
+		   struct v4l2_subdev_state *sd_state, unsigned int pad,
+		   struct v4l2_mbus_framefmt *fmt,
+		   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case IPIPEIF_PAD_SINK:
+		/* TODO: If the IPIPEIF output formatter pad is connected
+		 * directly to the resizer, only YUV formats can be used.
+		 */
+		for (i = 0; i < ARRAY_SIZE(ipipeif_fmts); i++) {
+			if (fmt->code == ipipeif_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(ipipeif_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
+		format = __ipipeif_get_format(ipipeif, sd_state,
+					      IPIPEIF_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		/* The data formatter truncates the number of horizontal output
+		 * pixels to a multiple of 16. To avoid clipping data, allow
+		 * callers to request an output size bigger than the input size
+		 * up to the nearest multiple of 16.
+		 */
+		fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+		fmt->width &= ~15;
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+
+	case IPIPEIF_PAD_SOURCE_VP:
+		format = __ipipeif_get_format(ipipeif, sd_state,
+					      IPIPEIF_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		fmt->width = clamp_t(u32, width, 32, fmt->width);
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+	}
+
+	/* Data is written to memory unpacked, each 10-bit or 12-bit pixel is
+	 * stored on 2 bytes.
+	 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * ipipeif_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg    : V4L2 subdev pad config
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ipipeif_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (code->pad) {
+	case IPIPEIF_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(ipipeif_fmts))
+			return -EINVAL;
+
+		code->code = ipipeif_fmts[code->index];
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
+	case IPIPEIF_PAD_SOURCE_VP:
+		/* No format conversion inside IPIPEIF */
+		if (code->index != 0)
+			return -EINVAL;
+
+		format = __ipipeif_get_format(ipipeif, sd_state,
+					      IPIPEIF_PAD_SINK,
+					      code->which);
+
+		code->code = format->code;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ipipeif_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ipipeif_try_format(ipipeif, sd_state, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ipipeif_try_format(ipipeif, sd_state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ipipeif_get_format - Retrieve the video format on a pad
+ * @sd : ISP IPIPEIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipeif_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipeif_get_format(ipipeif, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ipipeif_set_format - Set the video format on a pad
+ * @sd : ISP IPIPEIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipeif_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipeif_get_format(ipipeif, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	ipipeif_try_format(ipipeif, sd_state, fmt->pad, &fmt->format,
+			   fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == IPIPEIF_PAD_SINK) {
+		format = __ipipeif_get_format(ipipeif, sd_state,
+					      IPIPEIF_PAD_SOURCE_ISIF_SF,
+					      fmt->which);
+		*format = fmt->format;
+		ipipeif_try_format(ipipeif, sd_state,
+				   IPIPEIF_PAD_SOURCE_ISIF_SF,
+				   format, fmt->which);
+
+		format = __ipipeif_get_format(ipipeif, sd_state,
+					      IPIPEIF_PAD_SOURCE_VP,
+					      fmt->which);
+		*format = fmt->format;
+		ipipeif_try_format(ipipeif, sd_state, IPIPEIF_PAD_SOURCE_VP,
+				   format,
+				   fmt->which);
+	}
+
+	return 0;
+}
+
+static int ipipeif_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * ipipeif_init_formats - Initialize formats on all pads
+ * @sd: ISP IPIPEIF V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ipipeif_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = IPIPEIF_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ipipeif_set_format(sd, fh ? fh->state : NULL, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops ipipeif_v4l2_video_ops = {
+	.s_stream = ipipeif_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops ipipeif_v4l2_pad_ops = {
+	.enum_mbus_code = ipipeif_enum_mbus_code,
+	.enum_frame_size = ipipeif_enum_frame_size,
+	.get_fmt = ipipeif_get_format,
+	.set_fmt = ipipeif_set_format,
+	.link_validate = ipipeif_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops ipipeif_v4l2_ops = {
+	.video = &ipipeif_v4l2_video_ops,
+	.pad = &ipipeif_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops ipipeif_v4l2_internal_ops = {
+	.open = ipipeif_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ipipeif_link_setup - Setup IPIPEIF connections
+ * @entity: IPIPEIF media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int ipipeif_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipeif);
+	unsigned int index = local->index;
+
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case IPIPEIF_PAD_SINK | 2 << 16:
+		/* Read from the sensor CSI2a or CSI2b. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			ipipeif->input = IPIPEIF_INPUT_NONE;
+			break;
+		}
+
+		if (ipipeif->input != IPIPEIF_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->csi2a.subdev.entity)
+			ipipeif->input = IPIPEIF_INPUT_CSI2A;
+		else if (remote->entity == &iss->csi2b.subdev.entity)
+			ipipeif->input = IPIPEIF_INPUT_CSI2B;
+
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
+		/* Write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipeif->output & ~IPIPEIF_OUTPUT_MEMORY)
+				return -EBUSY;
+			ipipeif->output |= IPIPEIF_OUTPUT_MEMORY;
+		} else {
+			ipipeif->output &= ~IPIPEIF_OUTPUT_MEMORY;
+		}
+		break;
+
+	case IPIPEIF_PAD_SOURCE_VP | 2 << 16:
+		/* Send to IPIPE/RESIZER */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipeif->output & ~IPIPEIF_OUTPUT_VP)
+				return -EBUSY;
+			ipipeif->output |= IPIPEIF_OUTPUT_VP;
+		} else {
+			ipipeif->output &= ~IPIPEIF_OUTPUT_VP;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ipipeif_media_ops = {
+	.link_setup = ipipeif_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * ipipeif_init_entities - Initialize V4L2 subdev and media entity
+ * @ipipeif: ISS ISP IPIPEIF module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int ipipeif_init_entities(struct iss_ipipeif_device *ipipeif)
+{
+	struct v4l2_subdev *sd = &ipipeif->subdev;
+	struct media_pad *pads = ipipeif->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ipipeif->input = IPIPEIF_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &ipipeif_v4l2_ops);
+	sd->internal_ops = &ipipeif_v4l2_internal_ops;
+	strscpy(sd->name, "OMAP4 ISS ISP IPIPEIF", sizeof(sd->name));
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, ipipeif);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[IPIPEIF_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[IPIPEIF_PAD_SOURCE_ISIF_SF].flags = MEDIA_PAD_FL_SOURCE;
+	pads[IPIPEIF_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ipipeif_media_ops;
+	ret = media_entity_pads_init(me, IPIPEIF_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	ipipeif_init_formats(sd, NULL);
+
+	ipipeif->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ipipeif->video_out.ops = &ipipeif_video_ops;
+	ipipeif->video_out.iss = to_iss_device(ipipeif);
+	ipipeif->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+	ipipeif->video_out.bpl_alignment = 32;
+	ipipeif->video_out.bpl_zero_padding = 1;
+	ipipeif->video_out.bpl_max = 0x1ffe0;
+
+	return omap4iss_video_init(&ipipeif->video_out, "ISP IPIPEIF");
+}
+
+void omap4iss_ipipeif_unregister_entities(struct iss_ipipeif_device *ipipeif)
+{
+	v4l2_device_unregister_subdev(&ipipeif->subdev);
+	omap4iss_video_unregister(&ipipeif->video_out);
+}
+
+int omap4iss_ipipeif_register_entities(struct iss_ipipeif_device *ipipeif,
+				       struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ipipeif->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&ipipeif->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_ipipeif_unregister_entities(ipipeif);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP IPIPEIF initialisation and cleanup
+ */
+
+/*
+ * omap4iss_ipipeif_init - IPIPEIF module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_ipipeif_init(struct iss_device *iss)
+{
+	struct iss_ipipeif_device *ipipeif = &iss->ipipeif;
+
+	ipipeif->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&ipipeif->wait);
+
+	return ipipeif_init_entities(ipipeif);
+}
+
+/*
+ * omap4iss_ipipeif_create_links() - IPIPEIF pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_ipipeif_create_links(struct iss_device *iss)
+{
+	struct iss_ipipeif_device *ipipeif = &iss->ipipeif;
+
+	/* Connect the IPIPEIF subdev to the video node. */
+	return media_create_pad_link(&ipipeif->subdev.entity,
+				     IPIPEIF_PAD_SOURCE_ISIF_SF,
+				     &ipipeif->video_out.video.entity, 0, 0);
+}
+
+/*
+ * omap4iss_ipipeif_cleanup - IPIPEIF module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_ipipeif_cleanup(struct iss_device *iss)
+{
+	struct iss_ipipeif_device *ipipeif = &iss->ipipeif;
+
+	media_entity_cleanup(&ipipeif->subdev.entity);
+}
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.h b/drivers/staging/media/omap4iss/iss_ipipeif.h
new file mode 100644
index 000000000..69792333a
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.h
@@ -0,0 +1,89 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPEIF module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_IPIPEIF_H
+#define OMAP4_ISS_IPIPEIF_H
+
+#include "iss_video.h"
+
+enum ipipeif_input_entity {
+	IPIPEIF_INPUT_NONE,
+	IPIPEIF_INPUT_CSI2A,
+	IPIPEIF_INPUT_CSI2B
+};
+
+#define IPIPEIF_OUTPUT_MEMORY			BIT(0)
+#define IPIPEIF_OUTPUT_VP			BIT(1)
+
+/* Sink and source IPIPEIF pads */
+#define IPIPEIF_PAD_SINK			0
+#define IPIPEIF_PAD_SOURCE_ISIF_SF		1
+#define IPIPEIF_PAD_SOURCE_VP			2
+#define IPIPEIF_PADS_NUM			3
+
+/*
+ * struct iss_ipipeif_device - Structure for the IPIPEIF module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @video_out: Output video node
+ * @error: A hardware error occurred during capture
+ * @alaw: A-law compression enabled (1) or disabled (0)
+ * @lpf: Low pass filter enabled (1) or disabled (0)
+ * @obclamp: Optical-black clamp enabled (1) or disabled (0)
+ * @fpc_en: Faulty pixels correction enabled (1) or disabled (0)
+ * @blcomp: Black level compensation configuration
+ * @clamp: Optical-black or digital clamp configuration
+ * @fpc: Faulty pixels correction configuration
+ * @lsc: Lens shading compensation configuration
+ * @update: Bitmask of controls to update during the next interrupt
+ * @shadow_update: Controls update in progress by userspace
+ * @syncif: Interface synchronization configuration
+ * @vpcfg: Video port configuration
+ * @underrun: A buffer underrun occurred and a new buffer has been queued
+ * @state: Streaming state
+ * @lock: Serializes shadow_update with interrupt handler
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ * @ioctl_lock: Serializes ioctl calls and LSC requests freeing
+ */
+struct iss_ipipeif_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[IPIPEIF_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[IPIPEIF_PADS_NUM];
+
+	enum ipipeif_input_entity input;
+	unsigned int output;
+	struct iss_video video_out;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_ipipeif_init(struct iss_device *iss);
+int omap4iss_ipipeif_create_links(struct iss_device *iss);
+void omap4iss_ipipeif_cleanup(struct iss_device *iss);
+int omap4iss_ipipeif_register_entities(struct iss_ipipeif_device *ipipeif,
+				       struct v4l2_device *vdev);
+void omap4iss_ipipeif_unregister_entities(struct iss_ipipeif_device *ipipeif);
+
+int omap4iss_ipipeif_busy(struct iss_ipipeif_device *ipipeif);
+void omap4iss_ipipeif_isr(struct iss_ipipeif_device *ipipeif, u32 events);
+void omap4iss_ipipeif_restore_context(struct iss_device *iss);
+void omap4iss_ipipeif_max_rate(struct iss_ipipeif_device *ipipeif,
+			       unsigned int *max_rate);
+
+#endif	/* OMAP4_ISS_IPIPEIF_H */
diff --git a/drivers/staging/media/omap4iss/iss_regs.h b/drivers/staging/media/omap4iss/iss_regs.h
new file mode 100644
index 000000000..cfe0bb075
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_regs.h
@@ -0,0 +1,899 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - Register defines
+ *
+ * Copyright (C) 2012 Texas Instruments.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef _OMAP4_ISS_REGS_H_
+#define _OMAP4_ISS_REGS_H_
+
+/* ISS */
+#define ISS_HL_REVISION					0x0
+
+#define ISS_HL_SYSCONFIG				0x10
+#define ISS_HL_SYSCONFIG_IDLEMODE_SHIFT			2
+#define ISS_HL_SYSCONFIG_IDLEMODE_FORCEIDLE		0x0
+#define ISS_HL_SYSCONFIG_IDLEMODE_NOIDLE		0x1
+#define ISS_HL_SYSCONFIG_IDLEMODE_SMARTIDLE		0x2
+#define ISS_HL_SYSCONFIG_SOFTRESET			BIT(0)
+
+#define ISS_HL_IRQSTATUS_RAW(i)				(0x20 + (0x10 * (i)))
+#define ISS_HL_IRQSTATUS(i)				(0x24 + (0x10 * (i)))
+#define ISS_HL_IRQENABLE_SET(i)				(0x28 + (0x10 * (i)))
+#define ISS_HL_IRQENABLE_CLR(i)				(0x2c + (0x10 * (i)))
+
+#define ISS_HL_IRQ_HS_VS			BIT(17)
+#define ISS_HL_IRQ_SIMCOP(i)			BIT(12 + (i))
+#define ISS_HL_IRQ_BTE				BIT(11)
+#define ISS_HL_IRQ_CBUFF			BIT(10)
+#define ISS_HL_IRQ_CCP2(i)			BIT((i) > 3 ? 16 : 14 + (i))
+#define ISS_HL_IRQ_CSIB				BIT(5)
+#define ISS_HL_IRQ_CSIA				BIT(4)
+#define ISS_HL_IRQ_ISP(i)			BIT(i)
+
+#define ISS_CTRL					0x80
+#define ISS_CTRL_CLK_DIV_MASK				(3 << 4)
+#define ISS_CTRL_INPUT_SEL_MASK				(3 << 2)
+#define ISS_CTRL_INPUT_SEL_CSI2A			(0 << 2)
+#define ISS_CTRL_INPUT_SEL_CSI2B			(1 << 2)
+#define ISS_CTRL_SYNC_DETECT_VS_RAISING			(3 << 0)
+
+#define ISS_CLKCTRL					0x84
+#define ISS_CLKCTRL_VPORT2_CLK				BIT(30)
+#define ISS_CLKCTRL_VPORT1_CLK				BIT(29)
+#define ISS_CLKCTRL_VPORT0_CLK				BIT(28)
+#define ISS_CLKCTRL_CCP2				BIT(4)
+#define ISS_CLKCTRL_CSI2_B				BIT(3)
+#define ISS_CLKCTRL_CSI2_A				BIT(2)
+#define ISS_CLKCTRL_ISP					BIT(1)
+#define ISS_CLKCTRL_SIMCOP				BIT(0)
+
+#define ISS_CLKSTAT					0x88
+#define ISS_CLKSTAT_VPORT2_CLK				BIT(30)
+#define ISS_CLKSTAT_VPORT1_CLK				BIT(29)
+#define ISS_CLKSTAT_VPORT0_CLK				BIT(28)
+#define ISS_CLKSTAT_CCP2				BIT(4)
+#define ISS_CLKSTAT_CSI2_B				BIT(3)
+#define ISS_CLKSTAT_CSI2_A				BIT(2)
+#define ISS_CLKSTAT_ISP					BIT(1)
+#define ISS_CLKSTAT_SIMCOP				BIT(0)
+
+#define ISS_PM_STATUS					0x8c
+#define ISS_PM_STATUS_CBUFF_PM_MASK			(3 << 12)
+#define ISS_PM_STATUS_BTE_PM_MASK			(3 << 10)
+#define ISS_PM_STATUS_SIMCOP_PM_MASK			(3 << 8)
+#define ISS_PM_STATUS_ISP_PM_MASK			(3 << 6)
+#define ISS_PM_STATUS_CCP2_PM_MASK			(3 << 4)
+#define ISS_PM_STATUS_CSI2_B_PM_MASK			(3 << 2)
+#define ISS_PM_STATUS_CSI2_A_PM_MASK			(3 << 0)
+
+#define REGISTER0					0x0
+#define REGISTER0_HSCLOCKCONFIG				BIT(24)
+#define REGISTER0_THS_TERM_MASK				(0xff << 8)
+#define REGISTER0_THS_TERM_SHIFT			8
+#define REGISTER0_THS_SETTLE_MASK			(0xff << 0)
+#define REGISTER0_THS_SETTLE_SHIFT			0
+
+#define REGISTER1					0x4
+#define REGISTER1_RESET_DONE_CTRLCLK			BIT(29)
+#define REGISTER1_CLOCK_MISS_DETECTOR_STATUS		BIT(25)
+#define REGISTER1_TCLK_TERM_MASK			(0x3f << 18)
+#define REGISTER1_TCLK_TERM_SHIFT			18
+#define REGISTER1_DPHY_HS_SYNC_PATTERN_SHIFT		10
+#define REGISTER1_CTRLCLK_DIV_FACTOR_MASK		(0x3 << 8)
+#define REGISTER1_CTRLCLK_DIV_FACTOR_SHIFT		8
+#define REGISTER1_TCLK_SETTLE_MASK			(0xff << 0)
+#define REGISTER1_TCLK_SETTLE_SHIFT			0
+
+#define REGISTER2					0x8
+
+#define CSI2_SYSCONFIG					0x10
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_MASK		(3 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_FORCE		(0 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_NO			(1 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_SMART		(2 << 12)
+#define CSI2_SYSCONFIG_SOFT_RESET			(1 << 1)
+#define CSI2_SYSCONFIG_AUTO_IDLE			(1 << 0)
+
+#define CSI2_SYSSTATUS					0x14
+#define CSI2_SYSSTATUS_RESET_DONE			BIT(0)
+
+#define CSI2_IRQSTATUS					0x18
+#define CSI2_IRQENABLE					0x1c
+
+/* Shared bits across CSI2_IRQENABLE and IRQSTATUS */
+
+#define CSI2_IRQ_OCP_ERR				BIT(14)
+#define CSI2_IRQ_SHORT_PACKET				BIT(13)
+#define CSI2_IRQ_ECC_CORRECTION				BIT(12)
+#define CSI2_IRQ_ECC_NO_CORRECTION			BIT(11)
+#define CSI2_IRQ_COMPLEXIO_ERR				BIT(9)
+#define CSI2_IRQ_FIFO_OVF				BIT(8)
+#define CSI2_IRQ_CONTEXT0				BIT(0)
+
+#define CSI2_CTRL					0x40
+#define CSI2_CTRL_MFLAG_LEVH_MASK			(7 << 20)
+#define CSI2_CTRL_MFLAG_LEVH_SHIFT			20
+#define CSI2_CTRL_MFLAG_LEVL_MASK			(7 << 17)
+#define CSI2_CTRL_MFLAG_LEVL_SHIFT			17
+#define CSI2_CTRL_BURST_SIZE_EXPAND			(1 << 16)
+#define CSI2_CTRL_VP_CLK_EN				(1 << 15)
+#define CSI2_CTRL_NON_POSTED_WRITE			(1 << 13)
+#define CSI2_CTRL_VP_ONLY_EN				(1 << 11)
+#define CSI2_CTRL_VP_OUT_CTRL_MASK			(3 << 8)
+#define CSI2_CTRL_VP_OUT_CTRL_SHIFT			8
+#define CSI2_CTRL_DBG_EN				(1 << 7)
+#define CSI2_CTRL_BURST_SIZE_MASK			(3 << 5)
+#define CSI2_CTRL_ENDIANNESS				(1 << 4)
+#define CSI2_CTRL_FRAME					(1 << 3)
+#define CSI2_CTRL_ECC_EN				(1 << 2)
+#define CSI2_CTRL_IF_EN					(1 << 0)
+
+#define CSI2_DBG_H					0x44
+
+#define CSI2_COMPLEXIO_CFG				0x50
+#define CSI2_COMPLEXIO_CFG_RESET_CTRL			(1 << 30)
+#define CSI2_COMPLEXIO_CFG_RESET_DONE			(1 << 29)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_MASK			(3 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_OFF			(0 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_ON			(1 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_ULP			(2 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_MASK		(3 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_OFF		(0 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_ON		(1 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_ULP		(2 << 25)
+#define CSI2_COMPLEXIO_CFG_PWR_AUTO			(1 << 24)
+#define CSI2_COMPLEXIO_CFG_DATA_POL(i)			(1 << (((i) * 4) + 3))
+#define CSI2_COMPLEXIO_CFG_DATA_POSITION_MASK(i)	(7 << ((i) * 4))
+#define CSI2_COMPLEXIO_CFG_DATA_POSITION_SHIFT(i)	((i) * 4)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POL			(1 << 3)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK		(7 << 0)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POSITION_SHIFT		0
+
+#define CSI2_COMPLEXIO_IRQSTATUS			0x54
+
+#define CSI2_SHORT_PACKET				0x5c
+
+#define CSI2_COMPLEXIO_IRQENABLE			0x60
+
+/* Shared bits across CSI2_COMPLEXIO_IRQENABLE and IRQSTATUS */
+#define CSI2_COMPLEXIO_IRQ_STATEALLULPMEXIT		BIT(26)
+#define CSI2_COMPLEXIO_IRQ_STATEALLULPMENTER		BIT(25)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM5			BIT(24)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM4			BIT(23)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM3			BIT(22)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM2			BIT(21)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM1			BIT(20)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL5			BIT(19)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL4			BIT(18)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL3			BIT(17)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL2			BIT(16)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL1			BIT(15)
+#define CSI2_COMPLEXIO_IRQ_ERRESC5			BIT(14)
+#define CSI2_COMPLEXIO_IRQ_ERRESC4			BIT(13)
+#define CSI2_COMPLEXIO_IRQ_ERRESC3			BIT(12)
+#define CSI2_COMPLEXIO_IRQ_ERRESC2			BIT(11)
+#define CSI2_COMPLEXIO_IRQ_ERRESC1			BIT(10)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS5		BIT(9)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS4		BIT(8)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS3		BIT(7)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS2		BIT(6)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS1		BIT(5)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS5			BIT(4)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS4			BIT(3)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS3			BIT(2)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS2			BIT(1)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS1			BIT(0)
+
+#define CSI2_DBG_P					0x68
+
+#define CSI2_TIMING					0x6c
+#define CSI2_TIMING_FORCE_RX_MODE_IO1			BIT(15)
+#define CSI2_TIMING_STOP_STATE_X16_IO1			BIT(14)
+#define CSI2_TIMING_STOP_STATE_X4_IO1			BIT(13)
+#define CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK		(0x1fff << 0)
+#define CSI2_TIMING_STOP_STATE_COUNTER_IO1_SHIFT	0
+
+#define CSI2_CTX_CTRL1(i)				(0x70 + (0x20 * (i)))
+#define CSI2_CTX_CTRL1_GENERIC				BIT(30)
+#define CSI2_CTX_CTRL1_TRANSCODE			(0xf << 24)
+#define CSI2_CTX_CTRL1_FEC_NUMBER_MASK			(0xff << 16)
+#define CSI2_CTX_CTRL1_COUNT_MASK			(0xff << 8)
+#define CSI2_CTX_CTRL1_COUNT_SHIFT			8
+#define CSI2_CTX_CTRL1_EOF_EN				BIT(7)
+#define CSI2_CTX_CTRL1_EOL_EN				BIT(6)
+#define CSI2_CTX_CTRL1_CS_EN				BIT(5)
+#define CSI2_CTX_CTRL1_COUNT_UNLOCK			BIT(4)
+#define CSI2_CTX_CTRL1_PING_PONG			BIT(3)
+#define CSI2_CTX_CTRL1_CTX_EN				BIT(0)
+
+#define CSI2_CTX_CTRL2(i)				(0x74 + (0x20 * (i)))
+#define CSI2_CTX_CTRL2_FRAME_MASK			(0xffff << 16)
+#define CSI2_CTX_CTRL2_FRAME_SHIFT			16
+#define CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT		13
+#define CSI2_CTX_CTRL2_USER_DEF_MAP_MASK		\
+		(0x3 << CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT)
+#define CSI2_CTX_CTRL2_VIRTUAL_ID_MASK			(3 << 11)
+#define CSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT			11
+#define CSI2_CTX_CTRL2_DPCM_PRED			(1 << 10)
+#define CSI2_CTX_CTRL2_FORMAT_MASK			(0x3ff << 0)
+#define CSI2_CTX_CTRL2_FORMAT_SHIFT			0
+
+#define CSI2_CTX_DAT_OFST(i)				(0x78 + (0x20 * (i)))
+#define CSI2_CTX_DAT_OFST_MASK				(0xfff << 5)
+
+#define CSI2_CTX_PING_ADDR(i)				(0x7c + (0x20 * (i)))
+#define CSI2_CTX_PING_ADDR_MASK				0xffffffe0
+
+#define CSI2_CTX_PONG_ADDR(i)				(0x80 + (0x20 * (i)))
+#define CSI2_CTX_PONG_ADDR_MASK				CSI2_CTX_PING_ADDR_MASK
+
+#define CSI2_CTX_IRQENABLE(i)				(0x84 + (0x20 * (i)))
+#define CSI2_CTX_IRQSTATUS(i)				(0x88 + (0x20 * (i)))
+
+#define CSI2_CTX_CTRL3(i)				(0x8c + (0x20 * (i)))
+#define CSI2_CTX_CTRL3_ALPHA_SHIFT			5
+#define CSI2_CTX_CTRL3_ALPHA_MASK			\
+		(0x3fff << CSI2_CTX_CTRL3_ALPHA_SHIFT)
+
+/* Shared bits across CSI2_CTX_IRQENABLE and IRQSTATUS */
+#define CSI2_CTX_IRQ_ECC_CORRECTION			BIT(8)
+#define CSI2_CTX_IRQ_LINE_NUMBER			BIT(7)
+#define CSI2_CTX_IRQ_FRAME_NUMBER			BIT(6)
+#define CSI2_CTX_IRQ_CS					BIT(5)
+#define CSI2_CTX_IRQ_LE					BIT(3)
+#define CSI2_CTX_IRQ_LS					BIT(2)
+#define CSI2_CTX_IRQ_FE					BIT(1)
+#define CSI2_CTX_IRQ_FS					BIT(0)
+
+/* ISS BTE */
+#define BTE_CTRL					(0x0030)
+#define BTE_CTRL_BW_LIMITER_MASK			(0x3ff << 22)
+#define BTE_CTRL_BW_LIMITER_SHIFT			22
+
+/* ISS ISP_SYS1 */
+#define ISP5_REVISION					(0x0000)
+#define ISP5_SYSCONFIG					(0x0010)
+#define ISP5_SYSCONFIG_STANDBYMODE_MASK			(3 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_FORCE		(0 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_NO			(1 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_SMART		(2 << 4)
+#define ISP5_SYSCONFIG_SOFTRESET			(1 << 1)
+
+#define ISP5_IRQSTATUS(i)				(0x0028 + (0x10 * (i)))
+#define ISP5_IRQENABLE_SET(i)				(0x002c + (0x10 * (i)))
+#define ISP5_IRQENABLE_CLR(i)				(0x0030 + (0x10 * (i)))
+
+/* Bits shared for ISP5_IRQ* registers */
+#define ISP5_IRQ_OCP_ERR				BIT(31)
+#define ISP5_IRQ_IPIPE_INT_DPC_RNEW1			BIT(29)
+#define ISP5_IRQ_IPIPE_INT_DPC_RNEW0			BIT(28)
+#define ISP5_IRQ_IPIPE_INT_DPC_INIT			BIT(27)
+#define ISP5_IRQ_IPIPE_INT_EOF				BIT(25)
+#define ISP5_IRQ_H3A_INT_EOF				BIT(24)
+#define ISP5_IRQ_RSZ_INT_EOF1				BIT(23)
+#define ISP5_IRQ_RSZ_INT_EOF0				BIT(22)
+#define ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR			BIT(19)
+#define ISP5_IRQ_RSZ_FIFO_OVF				BIT(18)
+#define ISP5_IRQ_RSZ_INT_CYC_RSZB			BIT(17)
+#define ISP5_IRQ_RSZ_INT_CYC_RSZA			BIT(16)
+#define ISP5_IRQ_RSZ_INT_DMA				BIT(15)
+#define ISP5_IRQ_RSZ_INT_LAST_PIX			BIT(14)
+#define ISP5_IRQ_RSZ_INT_REG				BIT(13)
+#define ISP5_IRQ_H3A_INT				BIT(12)
+#define ISP5_IRQ_AF_INT					BIT(11)
+#define ISP5_IRQ_AEW_INT				BIT(10)
+#define ISP5_IRQ_IPIPEIF_IRQ				BIT(9)
+#define ISP5_IRQ_IPIPE_INT_HST				BIT(8)
+#define ISP5_IRQ_IPIPE_INT_BSC				BIT(7)
+#define ISP5_IRQ_IPIPE_INT_DMA				BIT(6)
+#define ISP5_IRQ_IPIPE_INT_LAST_PIX			BIT(5)
+#define ISP5_IRQ_IPIPE_INT_REG				BIT(4)
+#define ISP5_IRQ_ISIF_INT(i)				BIT(i)
+
+#define ISP5_CTRL					(0x006c)
+#define ISP5_CTRL_MSTANDBY				BIT(24)
+#define ISP5_CTRL_VD_PULSE_EXT				BIT(23)
+#define ISP5_CTRL_MSTANDBY_WAIT				BIT(20)
+#define ISP5_CTRL_BL_CLK_ENABLE				BIT(15)
+#define ISP5_CTRL_ISIF_CLK_ENABLE			BIT(14)
+#define ISP5_CTRL_H3A_CLK_ENABLE			BIT(13)
+#define ISP5_CTRL_RSZ_CLK_ENABLE			BIT(12)
+#define ISP5_CTRL_IPIPE_CLK_ENABLE			BIT(11)
+#define ISP5_CTRL_IPIPEIF_CLK_ENABLE			BIT(10)
+#define ISP5_CTRL_SYNC_ENABLE				BIT(9)
+#define ISP5_CTRL_PSYNC_CLK_SEL				BIT(8)
+
+/* ISS ISP ISIF register offsets */
+#define ISIF_SYNCEN					(0x0000)
+#define ISIF_SYNCEN_DWEN				BIT(1)
+#define ISIF_SYNCEN_SYEN				BIT(0)
+
+#define ISIF_MODESET					(0x0004)
+#define ISIF_MODESET_INPMOD_MASK			(3 << 12)
+#define ISIF_MODESET_INPMOD_RAW				(0 << 12)
+#define ISIF_MODESET_INPMOD_YCBCR16			(1 << 12)
+#define ISIF_MODESET_INPMOD_YCBCR8			(2 << 12)
+#define ISIF_MODESET_CCDW_MASK				(7 << 8)
+#define ISIF_MODESET_CCDW_2BIT				(2 << 8)
+#define ISIF_MODESET_CCDMD				(1 << 7)
+#define ISIF_MODESET_SWEN				(1 << 5)
+#define ISIF_MODESET_HDPOL				(1 << 3)
+#define ISIF_MODESET_VDPOL				(1 << 2)
+
+#define ISIF_SPH					(0x0018)
+#define ISIF_SPH_MASK					(0x7fff)
+
+#define ISIF_LNH					(0x001c)
+#define ISIF_LNH_MASK					(0x7fff)
+
+#define ISIF_LNV					(0x0028)
+#define ISIF_LNV_MASK					(0x7fff)
+
+#define ISIF_HSIZE					(0x0034)
+#define ISIF_HSIZE_ADCR					BIT(12)
+#define ISIF_HSIZE_HSIZE_MASK				(0xfff)
+
+#define ISIF_CADU					(0x003c)
+#define ISIF_CADU_MASK					(0x7ff)
+
+#define ISIF_CADL					(0x0040)
+#define ISIF_CADL_MASK					(0xffff)
+
+#define ISIF_CCOLP					(0x004c)
+#define ISIF_CCOLP_CP0_F0_R				(0 << 6)
+#define ISIF_CCOLP_CP0_F0_GR				(1 << 6)
+#define ISIF_CCOLP_CP0_F0_B				(3 << 6)
+#define ISIF_CCOLP_CP0_F0_GB				(2 << 6)
+#define ISIF_CCOLP_CP1_F0_R				(0 << 4)
+#define ISIF_CCOLP_CP1_F0_GR				(1 << 4)
+#define ISIF_CCOLP_CP1_F0_B				(3 << 4)
+#define ISIF_CCOLP_CP1_F0_GB				(2 << 4)
+#define ISIF_CCOLP_CP2_F0_R				(0 << 2)
+#define ISIF_CCOLP_CP2_F0_GR				(1 << 2)
+#define ISIF_CCOLP_CP2_F0_B				(3 << 2)
+#define ISIF_CCOLP_CP2_F0_GB				(2 << 2)
+#define ISIF_CCOLP_CP3_F0_R				(0 << 0)
+#define ISIF_CCOLP_CP3_F0_GR				(1 << 0)
+#define ISIF_CCOLP_CP3_F0_B				(3 << 0)
+#define ISIF_CCOLP_CP3_F0_GB				(2 << 0)
+
+#define ISIF_VDINT(i)					(0x0070 + (i) * 4)
+#define ISIF_VDINT_MASK					(0x7fff)
+
+#define ISIF_CGAMMAWD					(0x0080)
+#define ISIF_CGAMMAWD_GWDI_MASK				(0xf << 1)
+#define ISIF_CGAMMAWD_GWDI(bpp)				((16 - (bpp)) << 1)
+
+#define ISIF_CCDCFG					(0x0088)
+#define ISIF_CCDCFG_Y8POS				BIT(11)
+
+/* ISS ISP IPIPEIF register offsets */
+#define IPIPEIF_ENABLE					(0x0000)
+
+#define IPIPEIF_CFG1					(0x0004)
+#define IPIPEIF_CFG1_INPSRC1_MASK			(3 << 14)
+#define IPIPEIF_CFG1_INPSRC1_VPORT_RAW			(0 << 14)
+#define IPIPEIF_CFG1_INPSRC1_SDRAM_RAW			(1 << 14)
+#define IPIPEIF_CFG1_INPSRC1_ISIF_DARKFM		(2 << 14)
+#define IPIPEIF_CFG1_INPSRC1_SDRAM_YUV			(3 << 14)
+#define IPIPEIF_CFG1_INPSRC2_MASK			(3 << 2)
+#define IPIPEIF_CFG1_INPSRC2_ISIF			(0 << 2)
+#define IPIPEIF_CFG1_INPSRC2_SDRAM_RAW			(1 << 2)
+#define IPIPEIF_CFG1_INPSRC2_ISIF_DARKFM		(2 << 2)
+#define IPIPEIF_CFG1_INPSRC2_SDRAM_YUV			(3 << 2)
+
+#define IPIPEIF_CFG2					(0x0030)
+#define IPIPEIF_CFG2_YUV8P				BIT(7)
+#define IPIPEIF_CFG2_YUV8				BIT(6)
+#define IPIPEIF_CFG2_YUV16				BIT(3)
+#define IPIPEIF_CFG2_VDPOL				BIT(2)
+#define IPIPEIF_CFG2_HDPOL				BIT(1)
+#define IPIPEIF_CFG2_INTSW				BIT(0)
+
+#define IPIPEIF_CLKDIV					(0x0040)
+
+/* ISS ISP IPIPE register offsets */
+#define IPIPE_SRC_EN					(0x0000)
+#define IPIPE_SRC_EN_EN					BIT(0)
+
+#define IPIPE_SRC_MODE					(0x0004)
+#define IPIPE_SRC_MODE_WRT				BIT(1)
+#define IPIPE_SRC_MODE_OST				BIT(0)
+
+#define IPIPE_SRC_FMT					(0x0008)
+#define IPIPE_SRC_FMT_RAW2YUV				(0 << 0)
+#define IPIPE_SRC_FMT_RAW2RAW				(1 << 0)
+#define IPIPE_SRC_FMT_RAW2STATS				(2 << 0)
+#define IPIPE_SRC_FMT_YUV2YUV				(3 << 0)
+
+#define IPIPE_SRC_COL					(0x000c)
+#define IPIPE_SRC_COL_OO_R				(0 << 6)
+#define IPIPE_SRC_COL_OO_GR				(1 << 6)
+#define IPIPE_SRC_COL_OO_B				(3 << 6)
+#define IPIPE_SRC_COL_OO_GB				(2 << 6)
+#define IPIPE_SRC_COL_OE_R				(0 << 4)
+#define IPIPE_SRC_COL_OE_GR				(1 << 4)
+#define IPIPE_SRC_COL_OE_B				(3 << 4)
+#define IPIPE_SRC_COL_OE_GB				(2 << 4)
+#define IPIPE_SRC_COL_EO_R				(0 << 2)
+#define IPIPE_SRC_COL_EO_GR				(1 << 2)
+#define IPIPE_SRC_COL_EO_B				(3 << 2)
+#define IPIPE_SRC_COL_EO_GB				(2 << 2)
+#define IPIPE_SRC_COL_EE_R				(0 << 0)
+#define IPIPE_SRC_COL_EE_GR				(1 << 0)
+#define IPIPE_SRC_COL_EE_B				(3 << 0)
+#define IPIPE_SRC_COL_EE_GB				(2 << 0)
+
+#define IPIPE_SRC_VPS					(0x0010)
+#define IPIPE_SRC_VPS_MASK				(0xffff)
+
+#define IPIPE_SRC_VSZ					(0x0014)
+#define IPIPE_SRC_VSZ_MASK				(0x1fff)
+
+#define IPIPE_SRC_HPS					(0x0018)
+#define IPIPE_SRC_HPS_MASK				(0xffff)
+
+#define IPIPE_SRC_HSZ					(0x001c)
+#define IPIPE_SRC_HSZ_MASK				(0x1ffe)
+
+#define IPIPE_SEL_SBU					(0x0020)
+
+#define IPIPE_SRC_STA					(0x0024)
+
+#define IPIPE_GCK_MMR					(0x0028)
+#define IPIPE_GCK_MMR_REG				BIT(0)
+
+#define IPIPE_GCK_PIX					(0x002c)
+#define IPIPE_GCK_PIX_G3				BIT(3)
+#define IPIPE_GCK_PIX_G2				BIT(2)
+#define IPIPE_GCK_PIX_G1				BIT(1)
+#define IPIPE_GCK_PIX_G0				BIT(0)
+
+#define IPIPE_DPC_LUT_EN				(0x0034)
+#define IPIPE_DPC_LUT_SEL				(0x0038)
+#define IPIPE_DPC_LUT_ADR				(0x003c)
+#define IPIPE_DPC_LUT_SIZ				(0x0040)
+
+#define IPIPE_DPC_OTF_EN				(0x0044)
+#define IPIPE_DPC_OTF_TYP				(0x0048)
+#define IPIPE_DPC_OTF_2_D_THR_R				(0x004c)
+#define IPIPE_DPC_OTF_2_D_THR_GR			(0x0050)
+#define IPIPE_DPC_OTF_2_D_THR_GB			(0x0054)
+#define IPIPE_DPC_OTF_2_D_THR_B				(0x0058)
+#define IPIPE_DPC_OTF_2_C_THR_R				(0x005c)
+#define IPIPE_DPC_OTF_2_C_THR_GR			(0x0060)
+#define IPIPE_DPC_OTF_2_C_THR_GB			(0x0064)
+#define IPIPE_DPC_OTF_2_C_THR_B				(0x0068)
+#define IPIPE_DPC_OTF_3_SHF				(0x006c)
+#define IPIPE_DPC_OTF_3_D_THR				(0x0070)
+#define IPIPE_DPC_OTF_3_D_SPL				(0x0074)
+#define IPIPE_DPC_OTF_3_D_MIN				(0x0078)
+#define IPIPE_DPC_OTF_3_D_MAX				(0x007c)
+#define IPIPE_DPC_OTF_3_C_THR				(0x0080)
+#define IPIPE_DPC_OTF_3_C_SLP				(0x0084)
+#define IPIPE_DPC_OTF_3_C_MIN				(0x0088)
+#define IPIPE_DPC_OTF_3_C_MAX				(0x008c)
+
+#define IPIPE_LSC_VOFT					(0x0090)
+#define IPIPE_LSC_VA2					(0x0094)
+#define IPIPE_LSC_VA1					(0x0098)
+#define IPIPE_LSC_VS					(0x009c)
+#define IPIPE_LSC_HOFT					(0x00a0)
+#define IPIPE_LSC_HA2					(0x00a4)
+#define IPIPE_LSC_HA1					(0x00a8)
+#define IPIPE_LSC_HS					(0x00ac)
+#define IPIPE_LSC_GAN_R					(0x00b0)
+#define IPIPE_LSC_GAN_GR				(0x00b4)
+#define IPIPE_LSC_GAN_GB				(0x00b8)
+#define IPIPE_LSC_GAN_B					(0x00bc)
+#define IPIPE_LSC_OFT_R					(0x00c0)
+#define IPIPE_LSC_OFT_GR				(0x00c4)
+#define IPIPE_LSC_OFT_GB				(0x00c8)
+#define IPIPE_LSC_OFT_B					(0x00cc)
+#define IPIPE_LSC_SHF					(0x00d0)
+#define IPIPE_LSC_MAX					(0x00d4)
+
+#define IPIPE_D2F_1ST_EN				(0x00d8)
+#define IPIPE_D2F_1ST_TYP				(0x00dc)
+#define IPIPE_D2F_1ST_THR_00				(0x00e0)
+#define IPIPE_D2F_1ST_THR_01				(0x00e4)
+#define IPIPE_D2F_1ST_THR_02				(0x00e8)
+#define IPIPE_D2F_1ST_THR_03				(0x00ec)
+#define IPIPE_D2F_1ST_THR_04				(0x00f0)
+#define IPIPE_D2F_1ST_THR_05				(0x00f4)
+#define IPIPE_D2F_1ST_THR_06				(0x00f8)
+#define IPIPE_D2F_1ST_THR_07				(0x00fc)
+#define IPIPE_D2F_1ST_STR_00				(0x0100)
+#define IPIPE_D2F_1ST_STR_01				(0x0104)
+#define IPIPE_D2F_1ST_STR_02				(0x0108)
+#define IPIPE_D2F_1ST_STR_03				(0x010c)
+#define IPIPE_D2F_1ST_STR_04				(0x0110)
+#define IPIPE_D2F_1ST_STR_05				(0x0114)
+#define IPIPE_D2F_1ST_STR_06				(0x0118)
+#define IPIPE_D2F_1ST_STR_07				(0x011c)
+#define IPIPE_D2F_1ST_SPR_00				(0x0120)
+#define IPIPE_D2F_1ST_SPR_01				(0x0124)
+#define IPIPE_D2F_1ST_SPR_02				(0x0128)
+#define IPIPE_D2F_1ST_SPR_03				(0x012c)
+#define IPIPE_D2F_1ST_SPR_04				(0x0130)
+#define IPIPE_D2F_1ST_SPR_05				(0x0134)
+#define IPIPE_D2F_1ST_SPR_06				(0x0138)
+#define IPIPE_D2F_1ST_SPR_07				(0x013c)
+#define IPIPE_D2F_1ST_EDG_MIN				(0x0140)
+#define IPIPE_D2F_1ST_EDG_MAX				(0x0144)
+#define IPIPE_D2F_2ND_EN				(0x0148)
+#define IPIPE_D2F_2ND_TYP				(0x014c)
+#define IPIPE_D2F_2ND_THR00				(0x0150)
+#define IPIPE_D2F_2ND_THR01				(0x0154)
+#define IPIPE_D2F_2ND_THR02				(0x0158)
+#define IPIPE_D2F_2ND_THR03				(0x015c)
+#define IPIPE_D2F_2ND_THR04				(0x0160)
+#define IPIPE_D2F_2ND_THR05				(0x0164)
+#define IPIPE_D2F_2ND_THR06				(0x0168)
+#define IPIPE_D2F_2ND_THR07				(0x016c)
+#define IPIPE_D2F_2ND_STR_00				(0x0170)
+#define IPIPE_D2F_2ND_STR_01				(0x0174)
+#define IPIPE_D2F_2ND_STR_02				(0x0178)
+#define IPIPE_D2F_2ND_STR_03				(0x017c)
+#define IPIPE_D2F_2ND_STR_04				(0x0180)
+#define IPIPE_D2F_2ND_STR_05				(0x0184)
+#define IPIPE_D2F_2ND_STR_06				(0x0188)
+#define IPIPE_D2F_2ND_STR_07				(0x018c)
+#define IPIPE_D2F_2ND_SPR_00				(0x0190)
+#define IPIPE_D2F_2ND_SPR_01				(0x0194)
+#define IPIPE_D2F_2ND_SPR_02				(0x0198)
+#define IPIPE_D2F_2ND_SPR_03				(0x019c)
+#define IPIPE_D2F_2ND_SPR_04				(0x01a0)
+#define IPIPE_D2F_2ND_SPR_05				(0x01a4)
+#define IPIPE_D2F_2ND_SPR_06				(0x01a8)
+#define IPIPE_D2F_2ND_SPR_07				(0x01ac)
+#define IPIPE_D2F_2ND_EDG_MIN				(0x01b0)
+#define IPIPE_D2F_2ND_EDG_MAX				(0x01b4)
+
+#define IPIPE_GIC_EN					(0x01b8)
+#define IPIPE_GIC_TYP					(0x01bc)
+#define IPIPE_GIC_GAN					(0x01c0)
+#define IPIPE_GIC_NFGAIN				(0x01c4)
+#define IPIPE_GIC_THR					(0x01c8)
+#define IPIPE_GIC_SLP					(0x01cc)
+
+#define IPIPE_WB2_OFT_R					(0x01d0)
+#define IPIPE_WB2_OFT_GR				(0x01d4)
+#define IPIPE_WB2_OFT_GB				(0x01d8)
+#define IPIPE_WB2_OFT_B					(0x01dc)
+
+#define IPIPE_WB2_WGN_R					(0x01e0)
+#define IPIPE_WB2_WGN_GR				(0x01e4)
+#define IPIPE_WB2_WGN_GB				(0x01e8)
+#define IPIPE_WB2_WGN_B					(0x01ec)
+
+#define IPIPE_CFA_MODE					(0x01f0)
+#define IPIPE_CFA_2DIR_HPF_THR				(0x01f4)
+#define IPIPE_CFA_2DIR_HPF_SLP				(0x01f8)
+#define IPIPE_CFA_2DIR_MIX_THR				(0x01fc)
+#define IPIPE_CFA_2DIR_MIX_SLP				(0x0200)
+#define IPIPE_CFA_2DIR_DIR_TRH				(0x0204)
+#define IPIPE_CFA_2DIR_DIR_SLP				(0x0208)
+#define IPIPE_CFA_2DIR_NDWT				(0x020c)
+#define IPIPE_CFA_MONO_HUE_FRA				(0x0210)
+#define IPIPE_CFA_MONO_EDG_THR				(0x0214)
+#define IPIPE_CFA_MONO_THR_MIN				(0x0218)
+
+#define IPIPE_CFA_MONO_THR_SLP				(0x021c)
+#define IPIPE_CFA_MONO_SLP_MIN				(0x0220)
+#define IPIPE_CFA_MONO_SLP_SLP				(0x0224)
+#define IPIPE_CFA_MONO_LPWT				(0x0228)
+
+#define IPIPE_RGB1_MUL_RR				(0x022c)
+#define IPIPE_RGB1_MUL_GR				(0x0230)
+#define IPIPE_RGB1_MUL_BR				(0x0234)
+#define IPIPE_RGB1_MUL_RG				(0x0238)
+#define IPIPE_RGB1_MUL_GG				(0x023c)
+#define IPIPE_RGB1_MUL_BG				(0x0240)
+#define IPIPE_RGB1_MUL_RB				(0x0244)
+#define IPIPE_RGB1_MUL_GB				(0x0248)
+#define IPIPE_RGB1_MUL_BB				(0x024c)
+#define IPIPE_RGB1_OFT_OR				(0x0250)
+#define IPIPE_RGB1_OFT_OG				(0x0254)
+#define IPIPE_RGB1_OFT_OB				(0x0258)
+#define IPIPE_GMM_CFG					(0x025c)
+#define IPIPE_RGB2_MUL_RR				(0x0260)
+#define IPIPE_RGB2_MUL_GR				(0x0264)
+#define IPIPE_RGB2_MUL_BR				(0x0268)
+#define IPIPE_RGB2_MUL_RG				(0x026c)
+#define IPIPE_RGB2_MUL_GG				(0x0270)
+#define IPIPE_RGB2_MUL_BG				(0x0274)
+#define IPIPE_RGB2_MUL_RB				(0x0278)
+#define IPIPE_RGB2_MUL_GB				(0x027c)
+#define IPIPE_RGB2_MUL_BB				(0x0280)
+#define IPIPE_RGB2_OFT_OR				(0x0284)
+#define IPIPE_RGB2_OFT_OG				(0x0288)
+#define IPIPE_RGB2_OFT_OB				(0x028c)
+
+#define IPIPE_YUV_ADJ					(0x0294)
+#define IPIPE_YUV_MUL_RY				(0x0298)
+#define IPIPE_YUV_MUL_GY				(0x029c)
+#define IPIPE_YUV_MUL_BY				(0x02a0)
+#define IPIPE_YUV_MUL_RCB				(0x02a4)
+#define IPIPE_YUV_MUL_GCB				(0x02a8)
+#define IPIPE_YUV_MUL_BCB				(0x02ac)
+#define IPIPE_YUV_MUL_RCR				(0x02b0)
+#define IPIPE_YUV_MUL_GCR				(0x02b4)
+#define IPIPE_YUV_MUL_BCR				(0x02b8)
+#define IPIPE_YUV_OFT_Y					(0x02bc)
+#define IPIPE_YUV_OFT_CB				(0x02c0)
+#define IPIPE_YUV_OFT_CR				(0x02c4)
+
+#define IPIPE_YUV_PHS					(0x02c8)
+#define IPIPE_YUV_PHS_LPF				BIT(1)
+#define IPIPE_YUV_PHS_POS				BIT(0)
+
+#define IPIPE_YEE_EN					(0x02d4)
+#define IPIPE_YEE_TYP					(0x02d8)
+#define IPIPE_YEE_SHF					(0x02dc)
+#define IPIPE_YEE_MUL_00				(0x02e0)
+#define IPIPE_YEE_MUL_01				(0x02e4)
+#define IPIPE_YEE_MUL_02				(0x02e8)
+#define IPIPE_YEE_MUL_10				(0x02ec)
+#define IPIPE_YEE_MUL_11				(0x02f0)
+#define IPIPE_YEE_MUL_12				(0x02f4)
+#define IPIPE_YEE_MUL_20				(0x02f8)
+#define IPIPE_YEE_MUL_21				(0x02fc)
+#define IPIPE_YEE_MUL_22				(0x0300)
+#define IPIPE_YEE_THR					(0x0304)
+#define IPIPE_YEE_E_GAN					(0x0308)
+#define IPIPE_YEE_E_THR_1				(0x030c)
+#define IPIPE_YEE_E_THR_2				(0x0310)
+#define IPIPE_YEE_G_GAN					(0x0314)
+#define IPIPE_YEE_G_OFT					(0x0318)
+
+#define IPIPE_CAR_EN					(0x031c)
+#define IPIPE_CAR_TYP					(0x0320)
+#define IPIPE_CAR_SW					(0x0324)
+#define IPIPE_CAR_HPF_TYP				(0x0328)
+#define IPIPE_CAR_HPF_SHF				(0x032c)
+#define IPIPE_CAR_HPF_THR				(0x0330)
+#define IPIPE_CAR_GN1_GAN				(0x0334)
+#define IPIPE_CAR_GN1_SHF				(0x0338)
+#define IPIPE_CAR_GN1_MIN				(0x033c)
+#define IPIPE_CAR_GN2_GAN				(0x0340)
+#define IPIPE_CAR_GN2_SHF				(0x0344)
+#define IPIPE_CAR_GN2_MIN				(0x0348)
+#define IPIPE_CGS_EN					(0x034c)
+#define IPIPE_CGS_GN1_L_THR				(0x0350)
+#define IPIPE_CGS_GN1_L_GAIN				(0x0354)
+#define IPIPE_CGS_GN1_L_SHF				(0x0358)
+#define IPIPE_CGS_GN1_L_MIN				(0x035c)
+#define IPIPE_CGS_GN1_H_THR				(0x0360)
+#define IPIPE_CGS_GN1_H_GAIN				(0x0364)
+#define IPIPE_CGS_GN1_H_SHF				(0x0368)
+#define IPIPE_CGS_GN1_H_MIN				(0x036c)
+#define IPIPE_CGS_GN2_L_THR				(0x0370)
+#define IPIPE_CGS_GN2_L_GAIN				(0x0374)
+#define IPIPE_CGS_GN2_L_SHF				(0x0378)
+#define IPIPE_CGS_GN2_L_MIN				(0x037c)
+
+#define IPIPE_BOX_EN					(0x0380)
+#define IPIPE_BOX_MODE					(0x0384)
+#define IPIPE_BOX_TYP					(0x0388)
+#define IPIPE_BOX_SHF					(0x038c)
+#define IPIPE_BOX_SDR_SAD_H				(0x0390)
+#define IPIPE_BOX_SDR_SAD_L				(0x0394)
+
+#define IPIPE_HST_EN					(0x039c)
+#define IPIPE_HST_MODE					(0x03a0)
+#define IPIPE_HST_SEL					(0x03a4)
+#define IPIPE_HST_PARA					(0x03a8)
+#define IPIPE_HST_0_VPS					(0x03ac)
+#define IPIPE_HST_0_VSZ					(0x03b0)
+#define IPIPE_HST_0_HPS					(0x03b4)
+#define IPIPE_HST_0_HSZ					(0x03b8)
+#define IPIPE_HST_1_VPS					(0x03bc)
+#define IPIPE_HST_1_VSZ					(0x03c0)
+#define IPIPE_HST_1_HPS					(0x03c4)
+#define IPIPE_HST_1_HSZ					(0x03c8)
+#define IPIPE_HST_2_VPS					(0x03cc)
+#define IPIPE_HST_2_VSZ					(0x03d0)
+#define IPIPE_HST_2_HPS					(0x03d4)
+#define IPIPE_HST_2_HSZ					(0x03d8)
+#define IPIPE_HST_3_VPS					(0x03dc)
+#define IPIPE_HST_3_VSZ					(0x03e0)
+#define IPIPE_HST_3_HPS					(0x03e4)
+#define IPIPE_HST_3_HSZ					(0x03e8)
+#define IPIPE_HST_TBL					(0x03ec)
+#define IPIPE_HST_MUL_R					(0x03f0)
+#define IPIPE_HST_MUL_GR				(0x03f4)
+#define IPIPE_HST_MUL_GB				(0x03f8)
+#define IPIPE_HST_MUL_B					(0x03fc)
+
+#define IPIPE_BSC_EN					(0x0400)
+#define IPIPE_BSC_MODE					(0x0404)
+#define IPIPE_BSC_TYP					(0x0408)
+#define IPIPE_BSC_ROW_VCT				(0x040c)
+#define IPIPE_BSC_ROW_SHF				(0x0410)
+#define IPIPE_BSC_ROW_VPO				(0x0414)
+#define IPIPE_BSC_ROW_VNU				(0x0418)
+#define IPIPE_BSC_ROW_VSKIP				(0x041c)
+#define IPIPE_BSC_ROW_HPO				(0x0420)
+#define IPIPE_BSC_ROW_HNU				(0x0424)
+#define IPIPE_BSC_ROW_HSKIP				(0x0428)
+#define IPIPE_BSC_COL_VCT				(0x042c)
+#define IPIPE_BSC_COL_SHF				(0x0430)
+#define IPIPE_BSC_COL_VPO				(0x0434)
+#define IPIPE_BSC_COL_VNU				(0x0438)
+#define IPIPE_BSC_COL_VSKIP				(0x043c)
+#define IPIPE_BSC_COL_HPO				(0x0440)
+#define IPIPE_BSC_COL_HNU				(0x0444)
+#define IPIPE_BSC_COL_HSKIP				(0x0448)
+
+#define IPIPE_BSC_EN					(0x0400)
+
+/* ISS ISP Resizer register offsets */
+#define RSZ_REVISION					(0x0000)
+#define RSZ_SYSCONFIG					(0x0004)
+#define RSZ_SYSCONFIG_RSZB_CLK_EN			BIT(9)
+#define RSZ_SYSCONFIG_RSZA_CLK_EN			BIT(8)
+
+#define RSZ_IN_FIFO_CTRL				(0x000c)
+#define RSZ_IN_FIFO_CTRL_THRLD_LOW_MASK			(0x1ff << 16)
+#define RSZ_IN_FIFO_CTRL_THRLD_LOW_SHIFT		16
+#define RSZ_IN_FIFO_CTRL_THRLD_HIGH_MASK		(0x1ff << 0)
+#define RSZ_IN_FIFO_CTRL_THRLD_HIGH_SHIFT		0
+
+#define RSZ_FRACDIV					(0x0008)
+#define RSZ_FRACDIV_MASK				(0xffff)
+
+#define RSZ_SRC_EN					(0x0020)
+#define RSZ_SRC_EN_SRC_EN				BIT(0)
+
+#define RSZ_SRC_MODE					(0x0024)
+#define RSZ_SRC_MODE_OST				BIT(0)
+#define RSZ_SRC_MODE_WRT				BIT(1)
+
+#define RSZ_SRC_FMT0					(0x0028)
+#define RSZ_SRC_FMT0_BYPASS				BIT(1)
+#define RSZ_SRC_FMT0_SEL				BIT(0)
+
+#define RSZ_SRC_FMT1					(0x002c)
+#define RSZ_SRC_FMT1_IN420				BIT(1)
+
+#define RSZ_SRC_VPS					(0x0030)
+#define RSZ_SRC_VSZ					(0x0034)
+#define RSZ_SRC_HPS					(0x0038)
+#define RSZ_SRC_HSZ					(0x003c)
+#define RSZ_DMA_RZA					(0x0040)
+#define RSZ_DMA_RZB					(0x0044)
+#define RSZ_DMA_STA					(0x0048)
+#define RSZ_GCK_MMR					(0x004c)
+#define RSZ_GCK_MMR_MMR					BIT(0)
+
+#define RSZ_GCK_SDR					(0x0054)
+#define RSZ_GCK_SDR_CORE				BIT(0)
+
+#define RSZ_IRQ_RZA					(0x0058)
+#define RSZ_IRQ_RZA_MASK				(0x1fff)
+
+#define RSZ_IRQ_RZB					(0x005c)
+#define RSZ_IRQ_RZB_MASK				(0x1fff)
+
+#define RSZ_YUV_Y_MIN					(0x0060)
+#define RSZ_YUV_Y_MAX					(0x0064)
+#define RSZ_YUV_C_MIN					(0x0068)
+#define RSZ_YUV_C_MAX					(0x006c)
+
+#define RSZ_SEQ						(0x0074)
+#define RSZ_SEQ_HRVB					BIT(2)
+#define RSZ_SEQ_HRVA					BIT(0)
+
+#define RZA_EN						(0x0078)
+#define RZA_MODE					(0x007c)
+#define RZA_MODE_ONE_SHOT				BIT(0)
+
+#define RZA_420						(0x0080)
+#define RZA_I_VPS					(0x0084)
+#define RZA_I_HPS					(0x0088)
+#define RZA_O_VSZ					(0x008c)
+#define RZA_O_HSZ					(0x0090)
+#define RZA_V_PHS_Y					(0x0094)
+#define RZA_V_PHS_C					(0x0098)
+#define RZA_V_DIF					(0x009c)
+#define RZA_V_TYP					(0x00a0)
+#define RZA_V_LPF					(0x00a4)
+#define RZA_H_PHS					(0x00a8)
+#define RZA_H_DIF					(0x00b0)
+#define RZA_H_TYP					(0x00b4)
+#define RZA_H_LPF					(0x00b8)
+#define RZA_DWN_EN					(0x00bc)
+#define RZA_SDR_Y_BAD_H					(0x00d0)
+#define RZA_SDR_Y_BAD_L					(0x00d4)
+#define RZA_SDR_Y_SAD_H					(0x00d8)
+#define RZA_SDR_Y_SAD_L					(0x00dc)
+#define RZA_SDR_Y_OFT					(0x00e0)
+#define RZA_SDR_Y_PTR_S					(0x00e4)
+#define RZA_SDR_Y_PTR_E					(0x00e8)
+#define RZA_SDR_C_BAD_H					(0x00ec)
+#define RZA_SDR_C_BAD_L					(0x00f0)
+#define RZA_SDR_C_SAD_H					(0x00f4)
+#define RZA_SDR_C_SAD_L					(0x00f8)
+#define RZA_SDR_C_OFT					(0x00fc)
+#define RZA_SDR_C_PTR_S					(0x0100)
+#define RZA_SDR_C_PTR_E					(0x0104)
+
+#define RZB_EN						(0x0108)
+#define RZB_MODE					(0x010c)
+#define RZB_420						(0x0110)
+#define RZB_I_VPS					(0x0114)
+#define RZB_I_HPS					(0x0118)
+#define RZB_O_VSZ					(0x011c)
+#define RZB_O_HSZ					(0x0120)
+
+#define RZB_V_DIF					(0x012c)
+#define RZB_V_TYP					(0x0130)
+#define RZB_V_LPF					(0x0134)
+
+#define RZB_H_DIF					(0x0140)
+#define RZB_H_TYP					(0x0144)
+#define RZB_H_LPF					(0x0148)
+
+#define RZB_SDR_Y_BAD_H					(0x0160)
+#define RZB_SDR_Y_BAD_L					(0x0164)
+#define RZB_SDR_Y_SAD_H					(0x0168)
+#define RZB_SDR_Y_SAD_L					(0x016c)
+#define RZB_SDR_Y_OFT					(0x0170)
+#define RZB_SDR_Y_PTR_S					(0x0174)
+#define RZB_SDR_Y_PTR_E					(0x0178)
+#define RZB_SDR_C_BAD_H					(0x017c)
+#define RZB_SDR_C_BAD_L					(0x0180)
+#define RZB_SDR_C_SAD_H					(0x0184)
+#define RZB_SDR_C_SAD_L					(0x0188)
+
+#define RZB_SDR_C_PTR_S					(0x0190)
+#define RZB_SDR_C_PTR_E					(0x0194)
+
+/* Shared Bitmasks between RZA & RZB */
+#define RSZ_EN_EN					BIT(0)
+
+#define RSZ_420_CEN					BIT(1)
+#define RSZ_420_YEN					BIT(0)
+
+#define RSZ_I_VPS_MASK					(0x1fff)
+
+#define RSZ_I_HPS_MASK					(0x1fff)
+
+#define RSZ_O_VSZ_MASK					(0x1fff)
+
+#define RSZ_O_HSZ_MASK					(0x1ffe)
+
+#define RSZ_V_PHS_Y_MASK				(0x3fff)
+
+#define RSZ_V_PHS_C_MASK				(0x3fff)
+
+#define RSZ_V_DIF_MASK					(0x3fff)
+
+#define RSZ_V_TYP_C					BIT(1)
+#define RSZ_V_TYP_Y					BIT(0)
+
+#define RSZ_V_LPF_C_MASK				(0x3f << 6)
+#define RSZ_V_LPF_C_SHIFT				6
+#define RSZ_V_LPF_Y_MASK				(0x3f << 0)
+#define RSZ_V_LPF_Y_SHIFT				0
+
+#define RSZ_H_PHS_MASK					(0x3fff)
+
+#define RSZ_H_DIF_MASK					(0x3fff)
+
+#define RSZ_H_TYP_C					BIT(1)
+#define RSZ_H_TYP_Y					BIT(0)
+
+#define RSZ_H_LPF_C_MASK				(0x3f << 6)
+#define RSZ_H_LPF_C_SHIFT				6
+#define RSZ_H_LPF_Y_MASK				(0x3f << 0)
+#define RSZ_H_LPF_Y_SHIFT				0
+
+#define RSZ_DWN_EN_DWN_EN				BIT(0)
+
+#endif /* _OMAP4_ISS_REGS_H_ */
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
new file mode 100644
index 000000000..a5f8f9f1a
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -0,0 +1,885 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP RESIZER module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_resizer.h"
+
+static const unsigned int resizer_fmts[] = {
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+};
+
+/*
+ * resizer_print_status - Print current RESIZER Module register values.
+ * @resizer: Pointer to ISS ISP RESIZER device.
+ *
+ * Also prints other debug information stored in the RESIZER module.
+ */
+#define RSZ_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###RSZ " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_##name))
+
+#define RZA_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###RZA " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_##name))
+
+static void resizer_print_status(struct iss_resizer_device *resizer)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+
+	dev_dbg(iss->dev, "-------------RESIZER Register dump-------------\n");
+
+	RSZ_PRINT_REGISTER(iss, SYSCONFIG);
+	RSZ_PRINT_REGISTER(iss, IN_FIFO_CTRL);
+	RSZ_PRINT_REGISTER(iss, FRACDIV);
+	RSZ_PRINT_REGISTER(iss, SRC_EN);
+	RSZ_PRINT_REGISTER(iss, SRC_MODE);
+	RSZ_PRINT_REGISTER(iss, SRC_FMT0);
+	RSZ_PRINT_REGISTER(iss, SRC_FMT1);
+	RSZ_PRINT_REGISTER(iss, SRC_VPS);
+	RSZ_PRINT_REGISTER(iss, SRC_VSZ);
+	RSZ_PRINT_REGISTER(iss, SRC_HPS);
+	RSZ_PRINT_REGISTER(iss, SRC_HSZ);
+	RSZ_PRINT_REGISTER(iss, DMA_RZA);
+	RSZ_PRINT_REGISTER(iss, DMA_RZB);
+	RSZ_PRINT_REGISTER(iss, DMA_STA);
+	RSZ_PRINT_REGISTER(iss, GCK_MMR);
+	RSZ_PRINT_REGISTER(iss, GCK_SDR);
+	RSZ_PRINT_REGISTER(iss, IRQ_RZA);
+	RSZ_PRINT_REGISTER(iss, IRQ_RZB);
+	RSZ_PRINT_REGISTER(iss, YUV_Y_MIN);
+	RSZ_PRINT_REGISTER(iss, YUV_Y_MAX);
+	RSZ_PRINT_REGISTER(iss, YUV_C_MIN);
+	RSZ_PRINT_REGISTER(iss, YUV_C_MAX);
+	RSZ_PRINT_REGISTER(iss, SEQ);
+
+	RZA_PRINT_REGISTER(iss, EN);
+	RZA_PRINT_REGISTER(iss, MODE);
+	RZA_PRINT_REGISTER(iss, 420);
+	RZA_PRINT_REGISTER(iss, I_VPS);
+	RZA_PRINT_REGISTER(iss, I_HPS);
+	RZA_PRINT_REGISTER(iss, O_VSZ);
+	RZA_PRINT_REGISTER(iss, O_HSZ);
+	RZA_PRINT_REGISTER(iss, V_PHS_Y);
+	RZA_PRINT_REGISTER(iss, V_PHS_C);
+	RZA_PRINT_REGISTER(iss, V_DIF);
+	RZA_PRINT_REGISTER(iss, V_TYP);
+	RZA_PRINT_REGISTER(iss, V_LPF);
+	RZA_PRINT_REGISTER(iss, H_PHS);
+	RZA_PRINT_REGISTER(iss, H_DIF);
+	RZA_PRINT_REGISTER(iss, H_TYP);
+	RZA_PRINT_REGISTER(iss, H_LPF);
+	RZA_PRINT_REGISTER(iss, DWN_EN);
+	RZA_PRINT_REGISTER(iss, SDR_Y_BAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_Y_BAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_Y_SAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_Y_SAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_Y_OFT);
+	RZA_PRINT_REGISTER(iss, SDR_Y_PTR_S);
+	RZA_PRINT_REGISTER(iss, SDR_Y_PTR_E);
+	RZA_PRINT_REGISTER(iss, SDR_C_BAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_C_BAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_C_SAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_C_SAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_C_OFT);
+	RZA_PRINT_REGISTER(iss, SDR_C_PTR_S);
+	RZA_PRINT_REGISTER(iss, SDR_C_PTR_E);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * resizer_enable - Enable/Disable RESIZER.
+ * @enable: enable flag
+ *
+ */
+static void resizer_enable(struct iss_resizer_device *resizer, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_EN,
+		       RSZ_SRC_EN_SRC_EN, enable ? RSZ_SRC_EN_SRC_EN : 0);
+
+	/* TODO: Enable RSZB */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_EN, RSZ_EN_EN,
+		       enable ? RSZ_EN_EN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+/*
+ * resizer_set_outaddr - Set memory address to save output image
+ * @resizer: Pointer to ISP RESIZER device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ */
+static void resizer_set_outaddr(struct iss_resizer_device *resizer, u32 addr)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct v4l2_mbus_framefmt *informat, *outformat;
+
+	informat = &resizer->formats[RESIZER_PAD_SINK];
+	outformat = &resizer->formats[RESIZER_PAD_SOURCE_MEM];
+
+	/* Save address split in Base Address H & L */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_BAD_H,
+		      (addr >> 16) & 0xffff);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_BAD_L,
+		      addr & 0xffff);
+
+	/* SAD = BAD */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_SAD_H,
+		      (addr >> 16) & 0xffff);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_SAD_L,
+		      addr & 0xffff);
+
+	/* Program UV buffer address... Hardcoded to be contiguous! */
+	if ((informat->code == MEDIA_BUS_FMT_UYVY8_1X16) &&
+	    (outformat->code == MEDIA_BUS_FMT_YUYV8_1_5X8)) {
+		u32 c_addr = addr + resizer->video_out.bpl_value
+			   * outformat->height;
+
+		/* Ensure Y_BAD_L[6:0] = C_BAD_L[6:0]*/
+		if ((c_addr ^ addr) & 0x7f) {
+			c_addr &= ~0x7f;
+			c_addr += 0x80;
+			c_addr |= addr & 0x7f;
+		}
+
+		/* Save address split in Base Address H & L */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_BAD_H,
+			      (c_addr >> 16) & 0xffff);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_BAD_L,
+			      c_addr & 0xffff);
+
+		/* SAD = BAD */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_SAD_H,
+			      (c_addr >> 16) & 0xffff);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_SAD_L,
+			      c_addr & 0xffff);
+	}
+}
+
+static void resizer_configure(struct iss_resizer_device *resizer)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct v4l2_mbus_framefmt *informat, *outformat;
+
+	informat = &resizer->formats[RESIZER_PAD_SINK];
+	outformat = &resizer->formats[RESIZER_PAD_SOURCE_MEM];
+
+	/* Disable pass-through more. Despite its name, the BYPASS bit controls
+	 * pass-through mode, not bypass mode.
+	 */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_FMT0,
+		    RSZ_SRC_FMT0_BYPASS);
+
+	/* Select RSZ input */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_FMT0,
+		       RSZ_SRC_FMT0_SEL,
+		       resizer->input == RESIZER_INPUT_IPIPEIF ?
+		       RSZ_SRC_FMT0_SEL : 0);
+
+	/* RSZ ignores WEN signal from IPIPE/IPIPEIF */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_MODE,
+		    RSZ_SRC_MODE_WRT);
+
+	/* Set Resizer in free-running mode */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_MODE,
+		    RSZ_SRC_MODE_OST);
+
+	/* Init Resizer A */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_MODE,
+		    RZA_MODE_ONE_SHOT);
+
+	/* Set size related things now */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_HPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_VSZ,
+		      informat->height - 2);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_HSZ,
+		      informat->width - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_I_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_I_HPS, 0);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_O_VSZ,
+		      outformat->height - 2);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_O_HSZ,
+		      outformat->width - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_V_DIF, 0x100);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_H_DIF, 0x100);
+
+	/* Buffer output settings */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_PTR_S, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_PTR_E,
+		      outformat->height - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_OFT,
+		      resizer->video_out.bpl_value);
+
+	/* UYVY -> NV12 conversion */
+	if ((informat->code == MEDIA_BUS_FMT_UYVY8_1X16) &&
+	    (outformat->code == MEDIA_BUS_FMT_YUYV8_1_5X8)) {
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_420,
+			      RSZ_420_CEN | RSZ_420_YEN);
+
+		/* UV Buffer output settings */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_PTR_S,
+			      0);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_PTR_E,
+			      outformat->height - 1);
+
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_OFT,
+			      resizer->video_out.bpl_value);
+	} else {
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_420, 0);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void resizer_isr_buffer(struct iss_resizer_device *resizer)
+{
+	struct iss_buffer *buffer;
+
+	/* The whole resizer needs to be stopped. Disabling RZA only produces
+	 * input FIFO overflows, most probably when the next frame is received.
+	 */
+	resizer_enable(resizer, 0);
+
+	buffer = omap4iss_video_buffer_next(&resizer->video_out);
+	if (!buffer)
+		return;
+
+	resizer_set_outaddr(resizer, buffer->iss_addr);
+
+	resizer_enable(resizer, 1);
+}
+
+/*
+ * omap4iss_resizer_isr - Configure resizer during interframe time.
+ * @resizer: Pointer to ISP RESIZER device.
+ * @events: RESIZER events
+ */
+void omap4iss_resizer_isr(struct iss_resizer_device *resizer, u32 events)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct iss_pipeline *pipe =
+			     to_iss_pipeline(&resizer->subdev.entity);
+
+	if (events & (ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+		      ISP5_IRQ_RSZ_FIFO_OVF)) {
+		dev_dbg(iss->dev, "RSZ Err: FIFO_IN_BLK:%d, FIFO_OVF:%d\n",
+			events & ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR ? 1 : 0,
+			events & ISP5_IRQ_RSZ_FIFO_OVF ? 1 : 0);
+		omap4iss_pipeline_cancel_stream(pipe);
+	}
+
+	if (omap4iss_module_sync_is_stopping(&resizer->wait,
+					     &resizer->stopping))
+		return;
+
+	if (events & ISP5_IRQ_RSZ_INT_DMA)
+		resizer_isr_buffer(resizer);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS video operations
+ */
+
+static int resizer_video_queue(struct iss_video *video,
+			       struct iss_buffer *buffer)
+{
+	struct iss_resizer_device *resizer = container_of(video,
+				struct iss_resizer_device, video_out);
+
+	if (!(resizer->output & RESIZER_OUTPUT_MEMORY))
+		return -ENODEV;
+
+	resizer_set_outaddr(resizer, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		resizer_enable(resizer, 1);
+		iss_video_dmaqueue_flags_clr(video);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations resizer_video_ops = {
+	.queue = resizer_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * resizer_set_stream - Enable/Disable streaming on the RESIZER module
+ * @sd: ISP RESIZER V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int resizer_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(resizer);
+	struct iss_video *video_out = &resizer->video_out;
+	int ret = 0;
+
+	if (resizer->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, OMAP4_ISS_ISP_SUBCLK_RSZ);
+
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_MMR,
+			    RSZ_GCK_MMR_MMR);
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_SDR,
+			    RSZ_GCK_SDR_CORE);
+
+		/* FIXME: Enable RSZB also */
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SYSCONFIG,
+			    RSZ_SYSCONFIG_RSZA_CLK_EN);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		resizer_configure(resizer);
+		resizer_print_status(resizer);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (resizer->output & RESIZER_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+
+		atomic_set(&resizer->stopping, 0);
+		resizer_enable(resizer, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (resizer->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &resizer->wait,
+					      &resizer->stopping))
+			ret = -ETIMEDOUT;
+
+		resizer_enable(resizer, 0);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SYSCONFIG,
+			    RSZ_SYSCONFIG_RSZA_CLK_EN);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_SDR,
+			    RSZ_GCK_SDR_CORE);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_MMR,
+			    RSZ_GCK_MMR_MMR);
+		omap4iss_isp_subclk_disable(iss, OMAP4_ISS_ISP_SUBCLK_RSZ);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	resizer->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__resizer_get_format(struct iss_resizer_device *resizer,
+		     struct v4l2_subdev_state *sd_state, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&resizer->subdev, sd_state,
+						  pad);
+	return &resizer->formats[pad];
+}
+
+/*
+ * resizer_try_format - Try video format on a pad
+ * @resizer: ISS RESIZER device
+ * @cfg: V4L2 subdev pad config
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+resizer_try_format(struct iss_resizer_device *resizer,
+		   struct v4l2_subdev_state *sd_state, unsigned int pad,
+		   struct v4l2_mbus_framefmt *fmt,
+		   enum v4l2_subdev_format_whence which)
+{
+	u32 pixelcode;
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case RESIZER_PAD_SINK:
+		for (i = 0; i < ARRAY_SIZE(resizer_fmts); i++) {
+			if (fmt->code == resizer_fmts[i])
+				break;
+		}
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(resizer_fmts))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM:
+		pixelcode = fmt->code;
+		format = __resizer_get_format(resizer, sd_state,
+					      RESIZER_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		if ((pixelcode == MEDIA_BUS_FMT_YUYV8_1_5X8) &&
+		    (fmt->code == MEDIA_BUS_FMT_UYVY8_1X16))
+			fmt->code = pixelcode;
+
+		/* The data formatter truncates the number of horizontal output
+		 * pixels to a multiple of 16. To avoid clipping data, allow
+		 * callers to request an output size bigger than the input size
+		 * up to the nearest multiple of 16.
+		 */
+		fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+		fmt->width &= ~15;
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_JPEG;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * resizer_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad config
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int resizer_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (code->pad) {
+	case RESIZER_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(resizer_fmts))
+			return -EINVAL;
+
+		code->code = resizer_fmts[code->index];
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM:
+		format = __resizer_get_format(resizer, sd_state,
+					      RESIZER_PAD_SINK,
+					      code->which);
+
+		if (code->index == 0) {
+			code->code = format->code;
+			break;
+		}
+
+		switch (format->code) {
+		case MEDIA_BUS_FMT_UYVY8_1X16:
+			if (code->index == 1)
+				code->code = MEDIA_BUS_FMT_YUYV8_1_5X8;
+			else
+				return -EINVAL;
+			break;
+		default:
+			if (code->index != 0)
+				return -EINVAL;
+		}
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int resizer_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	resizer_try_format(resizer, sd_state, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	resizer_try_format(resizer, sd_state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * resizer_get_format - Retrieve the video format on a pad
+ * @sd : ISP RESIZER V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int resizer_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __resizer_get_format(resizer, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * resizer_set_format - Set the video format on a pad
+ * @sd : ISP RESIZER V4L2 subdevice
+ * @cfg: V4L2 subdev pad config
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int resizer_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __resizer_get_format(resizer, sd_state, fmt->pad, fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	resizer_try_format(resizer, sd_state, fmt->pad, &fmt->format,
+			   fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == RESIZER_PAD_SINK) {
+		format = __resizer_get_format(resizer, sd_state,
+					      RESIZER_PAD_SOURCE_MEM,
+					      fmt->which);
+		*format = fmt->format;
+		resizer_try_format(resizer, sd_state, RESIZER_PAD_SOURCE_MEM,
+				   format,
+				   fmt->which);
+	}
+
+	return 0;
+}
+
+static int resizer_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * resizer_init_formats - Initialize formats on all pads
+ * @sd: ISP RESIZER V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int resizer_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = RESIZER_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_1X16;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	resizer_set_format(sd, fh ? fh->state : NULL, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops resizer_v4l2_video_ops = {
+	.s_stream = resizer_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
+	.enum_mbus_code = resizer_enum_mbus_code,
+	.enum_frame_size = resizer_enum_frame_size,
+	.get_fmt = resizer_get_format,
+	.set_fmt = resizer_set_format,
+	.link_validate = resizer_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops resizer_v4l2_ops = {
+	.video = &resizer_v4l2_video_ops,
+	.pad = &resizer_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops resizer_v4l2_internal_ops = {
+	.open = resizer_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * resizer_link_setup - Setup RESIZER connections
+ * @entity: RESIZER media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int resizer_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(resizer);
+	unsigned int index = local->index;
+
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case RESIZER_PAD_SINK | 2 << 16:
+		/* Read from IPIPE or IPIPEIF. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			resizer->input = RESIZER_INPUT_NONE;
+			break;
+		}
+
+		if (resizer->input != RESIZER_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->ipipeif.subdev.entity)
+			resizer->input = RESIZER_INPUT_IPIPEIF;
+		else if (remote->entity == &iss->ipipe.subdev.entity)
+			resizer->input = RESIZER_INPUT_IPIPE;
+
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM:
+		/* Write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (resizer->output & ~RESIZER_OUTPUT_MEMORY)
+				return -EBUSY;
+			resizer->output |= RESIZER_OUTPUT_MEMORY;
+		} else {
+			resizer->output &= ~RESIZER_OUTPUT_MEMORY;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations resizer_media_ops = {
+	.link_setup = resizer_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * resizer_init_entities - Initialize V4L2 subdev and media entity
+ * @resizer: ISS ISP RESIZER module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int resizer_init_entities(struct iss_resizer_device *resizer)
+{
+	struct v4l2_subdev *sd = &resizer->subdev;
+	struct media_pad *pads = resizer->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	resizer->input = RESIZER_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &resizer_v4l2_ops);
+	sd->internal_ops = &resizer_v4l2_internal_ops;
+	strscpy(sd->name, "OMAP4 ISS ISP resizer", sizeof(sd->name));
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, resizer);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[RESIZER_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[RESIZER_PAD_SOURCE_MEM].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &resizer_media_ops;
+	ret = media_entity_pads_init(me, RESIZER_PADS_NUM, pads);
+	if (ret < 0)
+		return ret;
+
+	resizer_init_formats(sd, NULL);
+
+	resizer->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	resizer->video_out.ops = &resizer_video_ops;
+	resizer->video_out.iss = to_iss_device(resizer);
+	resizer->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+	resizer->video_out.bpl_alignment = 32;
+	resizer->video_out.bpl_zero_padding = 1;
+	resizer->video_out.bpl_max = 0x1ffe0;
+
+	return omap4iss_video_init(&resizer->video_out, "ISP resizer a");
+}
+
+void omap4iss_resizer_unregister_entities(struct iss_resizer_device *resizer)
+{
+	v4l2_device_unregister_subdev(&resizer->subdev);
+	omap4iss_video_unregister(&resizer->video_out);
+}
+
+int omap4iss_resizer_register_entities(struct iss_resizer_device *resizer,
+				       struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &resizer->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&resizer->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_resizer_unregister_entities(resizer);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP RESIZER initialisation and cleanup
+ */
+
+/*
+ * omap4iss_resizer_init - RESIZER module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_resizer_init(struct iss_device *iss)
+{
+	struct iss_resizer_device *resizer = &iss->resizer;
+
+	resizer->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&resizer->wait);
+
+	return resizer_init_entities(resizer);
+}
+
+/*
+ * omap4iss_resizer_create_links() - RESIZER pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_resizer_create_links(struct iss_device *iss)
+{
+	struct iss_resizer_device *resizer = &iss->resizer;
+
+	/* Connect the RESIZER subdev to the video node. */
+	return media_create_pad_link(&resizer->subdev.entity,
+				     RESIZER_PAD_SOURCE_MEM,
+				     &resizer->video_out.video.entity, 0, 0);
+}
+
+/*
+ * omap4iss_resizer_cleanup - RESIZER module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_resizer_cleanup(struct iss_device *iss)
+{
+	struct iss_resizer_device *resizer = &iss->resizer;
+
+	media_entity_cleanup(&resizer->subdev.entity);
+}
diff --git a/drivers/staging/media/omap4iss/iss_resizer.h b/drivers/staging/media/omap4iss/iss_resizer.h
new file mode 100644
index 000000000..cb937fccc
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_resizer.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP RESIZER module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_RESIZER_H
+#define OMAP4_ISS_RESIZER_H
+
+#include "iss_video.h"
+
+enum resizer_input_entity {
+	RESIZER_INPUT_NONE,
+	RESIZER_INPUT_IPIPE,
+	RESIZER_INPUT_IPIPEIF
+};
+
+#define RESIZER_OUTPUT_MEMORY			BIT(0)
+
+/* Sink and source RESIZER pads */
+#define RESIZER_PAD_SINK			0
+#define RESIZER_PAD_SOURCE_MEM			1
+#define RESIZER_PADS_NUM			2
+
+/*
+ * struct iss_resizer_device - Structure for the RESIZER module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @video_out: Output video node
+ * @error: A hardware error occurred during capture
+ * @state: Streaming state
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ */
+struct iss_resizer_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[RESIZER_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[RESIZER_PADS_NUM];
+
+	enum resizer_input_entity input;
+	unsigned int output;
+	struct iss_video video_out;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_resizer_init(struct iss_device *iss);
+int omap4iss_resizer_create_links(struct iss_device *iss);
+void omap4iss_resizer_cleanup(struct iss_device *iss);
+int omap4iss_resizer_register_entities(struct iss_resizer_device *resizer,
+				       struct v4l2_device *vdev);
+void omap4iss_resizer_unregister_entities(struct iss_resizer_device *resizer);
+
+int omap4iss_resizer_busy(struct iss_resizer_device *resizer);
+void omap4iss_resizer_isr(struct iss_resizer_device *resizer, u32 events);
+void omap4iss_resizer_restore_context(struct iss_device *iss);
+void omap4iss_resizer_max_rate(struct iss_resizer_device *resizer,
+			       unsigned int *max_rate);
+
+#endif	/* OMAP4_ISS_RESIZER_H */
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
new file mode 100644
index 000000000..8cc8b3eba
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -0,0 +1,1286 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * TI OMAP4 ISS V4L2 Driver - Generic video node
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/mm.h>
+#include <linux/pagemap.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+
+#include <asm/cacheflush.h>
+
+#include "iss_video.h"
+#include "iss.h"
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static struct iss_format_info formats[] = {
+	{ MEDIA_BUS_FMT_Y8_1X8, MEDIA_BUS_FMT_Y8_1X8,
+	  MEDIA_BUS_FMT_Y8_1X8, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_GREY, 8, },
+	{ MEDIA_BUS_FMT_Y10_1X10, MEDIA_BUS_FMT_Y10_1X10,
+	  MEDIA_BUS_FMT_Y10_1X10, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y10, 10, },
+	{ MEDIA_BUS_FMT_Y12_1X12, MEDIA_BUS_FMT_Y10_1X10,
+	  MEDIA_BUS_FMT_Y12_1X12, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y12, 12, },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR8, 8, },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  MEDIA_BUS_FMT_SGBRG8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG8, 8, },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG8, 8, },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  MEDIA_BUS_FMT_SRGGB8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB8, 8, },
+	{ MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8, MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	  MEDIA_BUS_FMT_SGRBG10_1X10, 0,
+	  V4L2_PIX_FMT_SGRBG10DPCM8, 8, },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR10, 10, },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG10, 10, },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG10, 10, },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB10, 10, },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR12, 12, },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  MEDIA_BUS_FMT_SGBRG12_1X12, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG12, 12, },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG12, 12, },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  MEDIA_BUS_FMT_SRGGB12_1X12, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB12, 12, },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_UYVY8_1X16,
+	  MEDIA_BUS_FMT_UYVY8_1X16, 0,
+	  V4L2_PIX_FMT_UYVY, 16, },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, MEDIA_BUS_FMT_YUYV8_1X16,
+	  MEDIA_BUS_FMT_YUYV8_1X16, 0,
+	  V4L2_PIX_FMT_YUYV, 16, },
+	{ MEDIA_BUS_FMT_YUYV8_1_5X8, MEDIA_BUS_FMT_YUYV8_1_5X8,
+	  MEDIA_BUS_FMT_YUYV8_1_5X8, 0,
+	  V4L2_PIX_FMT_NV12, 8, },
+};
+
+const struct iss_format_info *
+omap4iss_video_format_info(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * iss_video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @video: ISS video instance
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format format (output)
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ * The bytesperline and sizeimage fields are computed from the requested bytes
+ * per line value in the pix format and information from the video instance.
+ *
+ * Return the number of padding bytes at end of line.
+ */
+static unsigned int iss_video_mbus_to_pix(const struct iss_video *video,
+					  const struct v4l2_mbus_framefmt *mbus,
+					  struct v4l2_pix_format *pix)
+{
+	unsigned int bpl = pix->bytesperline;
+	unsigned int min_bpl;
+	unsigned int i;
+
+	memset(pix, 0, sizeof(*pix));
+	pix->width = mbus->width;
+	pix->height = mbus->height;
+
+	/*
+	 * Skip the last format in the loop so that it will be selected if no
+	 * match is found.
+	 */
+	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
+		if (formats[i].code == mbus->code)
+			break;
+	}
+
+	min_bpl = pix->width * ALIGN(formats[i].bpp, 8) / 8;
+
+	/*
+	 * Clamp the requested bytes per line value. If the maximum bytes per
+	 * line value is zero, the module doesn't support user configurable line
+	 * sizes. Override the requested value with the minimum in that case.
+	 */
+	if (video->bpl_max)
+		bpl = clamp(bpl, min_bpl, video->bpl_max);
+	else
+		bpl = min_bpl;
+
+	if (!video->bpl_zero_padding || bpl != min_bpl)
+		bpl = ALIGN(bpl, video->bpl_alignment);
+
+	pix->pixelformat = formats[i].pixelformat;
+	pix->bytesperline = bpl;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = mbus->colorspace;
+	pix->field = mbus->field;
+
+	/* FIXME: Special case for NV12! We should make this nicer... */
+	if (pix->pixelformat == V4L2_PIX_FMT_NV12)
+		pix->sizeimage += (pix->bytesperline * pix->height) / 2;
+
+	return bpl - min_bpl;
+}
+
+static void iss_video_pix_to_mbus(const struct v4l2_pix_format *pix,
+				  struct v4l2_mbus_framefmt *mbus)
+{
+	unsigned int i;
+
+	memset(mbus, 0, sizeof(*mbus));
+	mbus->width = pix->width;
+	mbus->height = pix->height;
+
+	/*
+	 * Skip the last format in the loop so that it will be selected if no
+	 * match is found.
+	 */
+	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
+		if (formats[i].pixelformat == pix->pixelformat)
+			break;
+	}
+
+	mbus->code = formats[i].code;
+	mbus->colorspace = pix->colorspace;
+	mbus->field = pix->field;
+}
+
+static struct v4l2_subdev *
+iss_video_remote_subdev(struct iss_video *video, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_pad_remote_pad_first(&video->pad);
+
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+/* Return a pointer to the ISS video instance at the far end of the pipeline. */
+static struct iss_video *
+iss_video_far_end(struct iss_video *video)
+{
+	struct media_graph graph;
+	struct media_entity *entity = &video->video.entity;
+	struct media_device *mdev = entity->graph_obj.mdev;
+	struct iss_video *far_end = NULL;
+
+	mutex_lock(&mdev->graph_mutex);
+
+	if (media_graph_walk_init(&graph, mdev)) {
+		mutex_unlock(&mdev->graph_mutex);
+		return NULL;
+	}
+
+	media_graph_walk_start(&graph, entity);
+
+	while ((entity = media_graph_walk_next(&graph))) {
+		if (entity == &video->video.entity)
+			continue;
+
+		if (!is_media_entity_v4l2_video_device(entity))
+			continue;
+
+		far_end = to_iss_video(media_entity_to_video_device(entity));
+		if (far_end->type != video->type)
+			break;
+
+		far_end = NULL;
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	media_graph_walk_cleanup(&graph);
+
+	return far_end;
+}
+
+static int
+__iss_video_get_format(struct iss_video *video,
+		       struct v4l2_mbus_framefmt *format)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EINVAL;
+
+	fmt.pad = pad;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	mutex_unlock(&video->mutex);
+
+	if (ret)
+		return ret;
+
+	*format = fmt.format;
+	return 0;
+}
+
+static int
+iss_video_check_format(struct iss_video *video, struct iss_video_fh *vfh)
+{
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_pix_format pixfmt;
+	int ret;
+
+	ret = __iss_video_get_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	pixfmt.bytesperline = 0;
+	ret = iss_video_mbus_to_pix(video, &format, &pixfmt);
+
+	if (vfh->format.fmt.pix.pixelformat != pixfmt.pixelformat ||
+	    vfh->format.fmt.pix.height != pixfmt.height ||
+	    vfh->format.fmt.pix.width != pixfmt.width ||
+	    vfh->format.fmt.pix.bytesperline != pixfmt.bytesperline ||
+	    vfh->format.fmt.pix.sizeimage != pixfmt.sizeimage)
+		return -EINVAL;
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int iss_video_queue_setup(struct vb2_queue *vq,
+				 unsigned int *count, unsigned int *num_planes,
+				 unsigned int sizes[],
+				 struct device *alloc_devs[])
+{
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vq);
+	struct iss_video *video = vfh->video;
+
+	/* Revisit multi-planar support for NV12 */
+	*num_planes = 1;
+
+	sizes[0] = vfh->format.fmt.pix.sizeimage;
+	if (sizes[0] == 0)
+		return -EINVAL;
+
+	*count = min(*count, video->capture_mem / PAGE_ALIGN(sizes[0]));
+
+	return 0;
+}
+
+static void iss_video_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct iss_buffer *buffer = container_of(vbuf, struct iss_buffer, vb);
+
+	if (buffer->iss_addr)
+		buffer->iss_addr = 0;
+}
+
+static int iss_video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vb->vb2_queue);
+	struct iss_buffer *buffer = container_of(vbuf, struct iss_buffer, vb);
+	struct iss_video *video = vfh->video;
+	unsigned long size = vfh->format.fmt.pix.sizeimage;
+	dma_addr_t addr;
+
+	if (vb2_plane_size(vb, 0) < size)
+		return -ENOBUFS;
+
+	addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!IS_ALIGNED(addr, 32)) {
+		dev_dbg(video->iss->dev,
+			"Buffer address must be aligned to 32 bytes boundary.\n");
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+	buffer->iss_addr = addr;
+	return 0;
+}
+
+static void iss_video_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vb->vb2_queue);
+	struct iss_video *video = vfh->video;
+	struct iss_buffer *buffer = container_of(vbuf, struct iss_buffer, vb);
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	unsigned long flags;
+	bool empty;
+
+	spin_lock_irqsave(&video->qlock, flags);
+
+	/*
+	 * Mark the buffer is faulty and give it back to the queue immediately
+	 * if the video node has registered an error. vb2 will perform the same
+	 * check when preparing the buffer, but that is inherently racy, so we
+	 * need to handle the race condition with an authoritative check here.
+	 */
+	if (unlikely(video->error)) {
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&video->qlock, flags);
+		return;
+	}
+
+	empty = list_empty(&video->dmaqueue);
+	list_add_tail(&buffer->list, &video->dmaqueue);
+
+	spin_unlock_irqrestore(&video->qlock, flags);
+
+	if (empty) {
+		enum iss_pipeline_state state;
+		unsigned int start;
+
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISS_PIPELINE_QUEUE_OUTPUT;
+		else
+			state = ISS_PIPELINE_QUEUE_INPUT;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state |= state;
+		video->ops->queue(video, buffer);
+		video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_QUEUED;
+
+		start = iss_pipeline_ready(pipe);
+		if (start)
+			pipe->state |= ISS_PIPELINE_STREAM;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+
+		if (start)
+			omap4iss_pipeline_set_stream(pipe,
+						     ISS_PIPELINE_STREAM_SINGLESHOT);
+	}
+}
+
+static const struct vb2_ops iss_video_vb2ops = {
+	.queue_setup	= iss_video_queue_setup,
+	.buf_prepare	= iss_video_buf_prepare,
+	.buf_queue	= iss_video_buf_queue,
+	.buf_cleanup	= iss_video_buf_cleanup,
+};
+
+/*
+ * omap4iss_video_buffer_next - Complete the current buffer and return the next
+ * @video: ISS video object
+ *
+ * Remove the current video buffer from the DMA queue and fill its timestamp,
+ * field count and state fields before waking up its completion handler.
+ *
+ * For capture video nodes, the buffer state is set to VB2_BUF_STATE_DONE if no
+ * error has been flagged in the pipeline, or to VB2_BUF_STATE_ERROR otherwise.
+ *
+ * The DMA queue is expected to contain at least one buffer.
+ *
+ * Return a pointer to the next buffer in the DMA queue, or NULL if the queue is
+ * empty.
+ */
+struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	enum iss_pipeline_state state;
+	struct iss_buffer *buf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (WARN_ON(list_empty(&video->dmaqueue))) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		return NULL;
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+			       list);
+	list_del(&buf->list);
+	spin_unlock_irqrestore(&video->qlock, flags);
+
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
+
+	/*
+	 * Do frame number propagation only if this is the output video node.
+	 * Frame number either comes from the CSI receivers or it gets
+	 * incremented here if H3A is not active.
+	 * Note: There is no guarantee that the output buffer will finish
+	 * first, so the input number might lag behind by 1 in some cases.
+	 */
+	if (video == pipe->output && !pipe->do_propagation)
+		buf->vb.sequence =
+			atomic_inc_return(&pipe->frame_number);
+	else
+		buf->vb.sequence = atomic_read(&pipe->frame_number);
+
+	vb2_buffer_done(&buf->vb.vb2_buf, pipe->error ?
+			VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	pipe->error = false;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (list_empty(&video->dmaqueue)) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISS_PIPELINE_QUEUE_OUTPUT
+			      | ISS_PIPELINE_STREAM;
+		else
+			state = ISS_PIPELINE_QUEUE_INPUT
+			      | ISS_PIPELINE_STREAM;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state &= ~state;
+		if (video->pipe.stream_state == ISS_PIPELINE_STREAM_CONTINUOUS)
+			video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+		return NULL;
+	}
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->input) {
+		spin_lock(&pipe->lock);
+		pipe->state &= ~ISS_PIPELINE_STREAM;
+		spin_unlock(&pipe->lock);
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+			       list);
+	spin_unlock_irqrestore(&video->qlock, flags);
+	buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
+	return buf;
+}
+
+/*
+ * omap4iss_video_cancel_stream - Cancel stream on a video node
+ * @video: ISS video object
+ *
+ * Cancelling a stream mark all buffers on the video node as erroneous and makes
+ * sure no new buffer can be queued.
+ */
+void omap4iss_video_cancel_stream(struct iss_video *video)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&video->qlock, flags);
+
+	while (!list_empty(&video->dmaqueue)) {
+		struct iss_buffer *buf;
+
+		buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+				       list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	vb2_queue_error(video->queue);
+	video->error = true;
+
+	spin_unlock_irqrestore(&video->qlock, flags);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+iss_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct iss_video *video = video_drvdata(file);
+
+	strscpy(cap->driver, ISS_VIDEO_DRIVER_NAME, sizeof(cap->driver));
+	strscpy(cap->card, video->video.name, sizeof(cap->card));
+	strscpy(cap->bus_info, "media", sizeof(cap->bus_info));
+	cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
+			  | V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
+
+	return 0;
+}
+
+static int
+iss_video_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_mbus_framefmt format;
+	unsigned int index = f->index;
+	unsigned int i;
+	int ret;
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	ret = __iss_video_get_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		const struct iss_format_info *info = &formats[i];
+
+		if (format.code != info->code)
+			continue;
+
+		if (index == 0) {
+			f->pixelformat = info->pixelformat;
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
+}
+
+static int
+iss_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	*format = vfh->format;
+	mutex_unlock(&video->mutex);
+
+	return 0;
+}
+
+static int
+iss_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_mbus_framefmt fmt;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+
+	/*
+	 * Fill the bytesperline and sizeimage fields by converting to media bus
+	 * format and back to pixel format.
+	 */
+	iss_video_pix_to_mbus(&format->fmt.pix, &fmt);
+	iss_video_mbus_to_pix(video, &fmt, &format->fmt.pix);
+
+	vfh->format = *format;
+
+	mutex_unlock(&video->mutex);
+	return 0;
+}
+
+static int
+iss_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EINVAL;
+
+	iss_video_pix_to_mbus(&format->fmt.pix, &fmt.format);
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	iss_video_mbus_to_pix(video, &fmt.format, &format->fmt.pix);
+	return 0;
+}
+
+static int
+iss_video_get_selection(struct file *file, void *fh, struct v4l2_selection *sel)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev_format format;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+	};
+	u32 pad;
+	int ret;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EINVAL;
+
+	/*
+	 * Try the get selection operation first and fallback to get format if
+	 * not implemented.
+	 */
+	sdsel.pad = pad;
+	ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
+	if (!ret)
+		sel->r = sdsel.r;
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	format.pad = pad;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &format);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+
+	sel->r.left = 0;
+	sel->r.top = 0;
+	sel->r.width = format.format.width;
+	sel->r.height = format.format.height;
+
+	return 0;
+}
+
+static int
+iss_video_set_selection(struct file *file, void *fh, struct v4l2_selection *sel)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+		.flags = sel->flags,
+		.r = sel->r,
+	};
+	u32 pad;
+	int ret;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EINVAL;
+
+	sdsel.pad = pad;
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sdsel);
+	mutex_unlock(&video->mutex);
+	if (!ret)
+		sel->r = sdsel.r;
+
+	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+}
+
+static int
+iss_video_get_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	memset(a, 0, sizeof(*a));
+	a->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.output.timeperframe = vfh->timeperframe;
+
+	return 0;
+}
+
+static int
+iss_video_set_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	if (a->parm.output.timeperframe.denominator == 0)
+		a->parm.output.timeperframe.denominator = 1;
+
+	vfh->timeperframe = a->parm.output.timeperframe;
+
+	return 0;
+}
+
+static int
+iss_video_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_reqbufs(&vfh->queue, rb);
+}
+
+static int
+iss_video_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_querybuf(&vfh->queue, b);
+}
+
+static int
+iss_video_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_qbuf(&vfh->queue, video->video.v4l2_dev->mdev, b);
+}
+
+static int
+iss_video_expbuf(struct file *file, void *fh, struct v4l2_exportbuffer *e)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_expbuf(&vfh->queue, e);
+}
+
+static int
+iss_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_dqbuf(&vfh->queue, b, file->f_flags & O_NONBLOCK);
+}
+
+/*
+ * Stream management
+ *
+ * Every ISS pipeline has a single input and a single output. The input can be
+ * either a sensor or a video node. The output is always a video node.
+ *
+ * As every pipeline has an output video node, the ISS video objects at the
+ * pipeline output stores the pipeline state. It tracks the streaming state of
+ * both the input and output, as well as the availability of buffers.
+ *
+ * In sensor-to-memory mode, frames are always available at the pipeline input.
+ * Starting the sensor usually requires I2C transfers and must be done in
+ * interruptible context. The pipeline is started and stopped synchronously
+ * to the stream on/off commands. All modules in the pipeline will get their
+ * subdev set stream handler called. The module at the end of the pipeline must
+ * delay starting the hardware until buffers are available at its output.
+ *
+ * In memory-to-memory mode, starting/stopping the stream requires
+ * synchronization between the input and output. ISS modules can't be stopped
+ * in the middle of a frame, and at least some of the modules seem to become
+ * busy as soon as they're started, even if they don't receive a frame start
+ * event. For that reason frames need to be processed in single-shot mode. The
+ * driver needs to wait until a frame is completely processed and written to
+ * memory before restarting the pipeline for the next frame. Pipelined
+ * processing might be possible but requires more testing.
+ *
+ * Stream start must be delayed until buffers are available at both the input
+ * and output. The pipeline must be started in the vb2 queue callback with
+ * the buffers queue spinlock held. The modules subdev set stream operation must
+ * not sleep.
+ */
+static int
+iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct media_graph graph;
+	struct media_entity *entity = &video->video.entity;
+	struct media_device *mdev = entity->graph_obj.mdev;
+	enum iss_pipeline_state state;
+	struct iss_pipeline *pipe;
+	struct iss_video *far_end;
+	unsigned long flags;
+	int ret;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	/*
+	 * Start streaming on the pipeline. No link touching an entity in the
+	 * pipeline can be activated or deactivated once streaming is started.
+	 */
+	pipe = to_iss_pipeline(&video->video.entity) ? : &video->pipe;
+	pipe->external = NULL;
+	pipe->external_rate = 0;
+	pipe->external_bpp = 0;
+
+	ret = media_entity_enum_init(&pipe->ent_enum, entity->graph_obj.mdev);
+	if (ret)
+		goto err_graph_walk_init;
+
+	ret = media_graph_walk_init(&graph, entity->graph_obj.mdev);
+	if (ret)
+		goto err_graph_walk_init;
+
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, true);
+
+	ret = video_device_pipeline_start(&video->video, &pipe->pipe);
+	if (ret < 0)
+		goto err_media_pipeline_start;
+
+	mutex_lock(&mdev->graph_mutex);
+	media_graph_walk_start(&graph, entity);
+	while ((entity = media_graph_walk_next(&graph)))
+		media_entity_enum_set(&pipe->ent_enum, entity);
+	mutex_unlock(&mdev->graph_mutex);
+
+	/*
+	 * Verify that the currently configured format matches the output of
+	 * the connected subdev.
+	 */
+	ret = iss_video_check_format(video, vfh);
+	if (ret < 0)
+		goto err_iss_video_check_format;
+
+	video->bpl_padding = ret;
+	video->bpl_value = vfh->format.fmt.pix.bytesperline;
+
+	/*
+	 * Find the ISS video node connected at the far end of the pipeline and
+	 * update the pipeline.
+	 */
+	far_end = iss_video_far_end(video);
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		state = ISS_PIPELINE_STREAM_OUTPUT | ISS_PIPELINE_IDLE_OUTPUT;
+		pipe->input = far_end;
+		pipe->output = video;
+	} else {
+		if (!far_end) {
+			ret = -EPIPE;
+			goto err_iss_video_check_format;
+		}
+
+		state = ISS_PIPELINE_STREAM_INPUT | ISS_PIPELINE_IDLE_INPUT;
+		pipe->input = video;
+		pipe->output = far_end;
+	}
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~ISS_PIPELINE_STREAM;
+	pipe->state |= state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/*
+	 * Set the maximum time per frame as the value requested by userspace.
+	 * This is a soft limit that can be overridden if the hardware doesn't
+	 * support the request limit.
+	 */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		pipe->max_timeperframe = vfh->timeperframe;
+
+	video->queue = &vfh->queue;
+	INIT_LIST_HEAD(&video->dmaqueue);
+	video->error = false;
+	atomic_set(&pipe->frame_number, -1);
+
+	ret = vb2_streamon(&vfh->queue, type);
+	if (ret < 0)
+		goto err_iss_video_check_format;
+
+	/*
+	 * In sensor-to-memory mode, the stream can be started synchronously
+	 * to the stream on command. In memory-to-memory mode, it will be
+	 * started when buffers are queued on both the input and output.
+	 */
+	if (!pipe->input) {
+		unsigned long flags;
+
+		ret = omap4iss_pipeline_set_stream(pipe,
+						   ISS_PIPELINE_STREAM_CONTINUOUS);
+		if (ret < 0)
+			goto err_omap4iss_set_stream;
+		spin_lock_irqsave(&video->qlock, flags);
+		if (list_empty(&video->dmaqueue))
+			video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&video->qlock, flags);
+	}
+
+	media_graph_walk_cleanup(&graph);
+
+	mutex_unlock(&video->stream_lock);
+
+	return 0;
+
+err_omap4iss_set_stream:
+	vb2_streamoff(&vfh->queue, type);
+err_iss_video_check_format:
+	video_device_pipeline_stop(&video->video);
+err_media_pipeline_start:
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, false);
+	video->queue = NULL;
+
+	media_graph_walk_cleanup(&graph);
+
+err_graph_walk_init:
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
+	mutex_unlock(&video->stream_lock);
+
+	return ret;
+}
+
+static int
+iss_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	enum iss_pipeline_state state;
+	unsigned long flags;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	if (!vb2_is_streaming(&vfh->queue))
+		goto done;
+
+	/* Update the pipeline state. */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		state = ISS_PIPELINE_STREAM_OUTPUT
+		      | ISS_PIPELINE_QUEUE_OUTPUT;
+	else
+		state = ISS_PIPELINE_STREAM_INPUT
+		      | ISS_PIPELINE_QUEUE_INPUT;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/* Stop the stream. */
+	omap4iss_pipeline_set_stream(pipe, ISS_PIPELINE_STREAM_STOPPED);
+	vb2_streamoff(&vfh->queue, type);
+	video->queue = NULL;
+
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, false);
+	video_device_pipeline_stop(&video->video);
+
+done:
+	mutex_unlock(&video->stream_lock);
+	return 0;
+}
+
+static int
+iss_video_enum_input(struct file *file, void *fh, struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strscpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int
+iss_video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+static int
+iss_video_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+static const struct v4l2_ioctl_ops iss_video_ioctl_ops = {
+	.vidioc_querycap		= iss_video_querycap,
+	.vidioc_enum_fmt_vid_cap        = iss_video_enum_format,
+	.vidioc_g_fmt_vid_cap		= iss_video_get_format,
+	.vidioc_s_fmt_vid_cap		= iss_video_set_format,
+	.vidioc_try_fmt_vid_cap		= iss_video_try_format,
+	.vidioc_g_fmt_vid_out		= iss_video_get_format,
+	.vidioc_s_fmt_vid_out		= iss_video_set_format,
+	.vidioc_try_fmt_vid_out		= iss_video_try_format,
+	.vidioc_g_selection		= iss_video_get_selection,
+	.vidioc_s_selection		= iss_video_set_selection,
+	.vidioc_g_parm			= iss_video_get_param,
+	.vidioc_s_parm			= iss_video_set_param,
+	.vidioc_reqbufs			= iss_video_reqbufs,
+	.vidioc_querybuf		= iss_video_querybuf,
+	.vidioc_qbuf			= iss_video_qbuf,
+	.vidioc_expbuf			= iss_video_expbuf,
+	.vidioc_dqbuf			= iss_video_dqbuf,
+	.vidioc_streamon		= iss_video_streamon,
+	.vidioc_streamoff		= iss_video_streamoff,
+	.vidioc_enum_input		= iss_video_enum_input,
+	.vidioc_g_input			= iss_video_g_input,
+	.vidioc_s_input			= iss_video_s_input,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static int iss_video_open(struct file *file)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct iss_video_fh *handle;
+	struct vb2_queue *q;
+	int ret = 0;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (!handle)
+		return -ENOMEM;
+
+	v4l2_fh_init(&handle->vfh, &video->video);
+	v4l2_fh_add(&handle->vfh);
+
+	/* If this is the first user, initialise the pipeline. */
+	if (!omap4iss_get(video->iss)) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	ret = v4l2_pipeline_pm_get(&video->video.entity);
+	if (ret < 0) {
+		omap4iss_put(video->iss);
+		goto done;
+	}
+
+	q = &handle->queue;
+
+	q->type = video->type;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = handle;
+	q->ops = &iss_video_vb2ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct iss_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->dev = video->iss->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		omap4iss_put(video->iss);
+		goto done;
+	}
+
+	memset(&handle->format, 0, sizeof(handle->format));
+	handle->format.type = video->type;
+	handle->timeperframe.denominator = 1;
+
+	handle->video = video;
+	file->private_data = &handle->vfh;
+
+done:
+	if (ret < 0) {
+		v4l2_fh_del(&handle->vfh);
+		v4l2_fh_exit(&handle->vfh);
+		kfree(handle);
+	}
+
+	return ret;
+}
+
+static int iss_video_release(struct file *file)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh = file->private_data;
+	struct iss_video_fh *handle = to_iss_video_fh(vfh);
+
+	/* Disable streaming and free the buffers queue resources. */
+	iss_video_streamoff(file, vfh, video->type);
+
+	v4l2_pipeline_pm_put(&video->video.entity);
+
+	/* Release the videobuf2 queue */
+	vb2_queue_release(&handle->queue);
+
+	v4l2_fh_del(vfh);
+	v4l2_fh_exit(vfh);
+	kfree(handle);
+	file->private_data = NULL;
+
+	omap4iss_put(video->iss);
+
+	return 0;
+}
+
+static __poll_t iss_video_poll(struct file *file, poll_table *wait)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(file->private_data);
+
+	return vb2_poll(&vfh->queue, file, wait);
+}
+
+static int iss_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(file->private_data);
+
+	return vb2_mmap(&vfh->queue, vma);
+}
+
+static const struct v4l2_file_operations iss_video_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open = iss_video_open,
+	.release = iss_video_release,
+	.poll = iss_video_poll,
+	.mmap = iss_video_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * ISS video core
+ */
+
+static const struct iss_video_operations iss_video_dummy_ops = {
+};
+
+int omap4iss_video_init(struct iss_video *video, const char *name)
+{
+	const char *direction;
+	int ret;
+
+	switch (video->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		direction = "output";
+		video->pad.flags = MEDIA_PAD_FL_SINK;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		direction = "input";
+		video->pad.flags = MEDIA_PAD_FL_SOURCE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ret = media_entity_pads_init(&video->video.entity, 1, &video->pad);
+	if (ret < 0)
+		return ret;
+
+	spin_lock_init(&video->qlock);
+	mutex_init(&video->mutex);
+	atomic_set(&video->active, 0);
+
+	spin_lock_init(&video->pipe.lock);
+	mutex_init(&video->stream_lock);
+
+	/* Initialize the video device. */
+	if (!video->ops)
+		video->ops = &iss_video_dummy_ops;
+
+	video->video.fops = &iss_video_fops;
+	snprintf(video->video.name, sizeof(video->video.name),
+		 "OMAP4 ISS %s %s", name, direction);
+	video->video.vfl_type = VFL_TYPE_VIDEO;
+	video->video.release = video_device_release_empty;
+	video->video.ioctl_ops = &iss_video_ioctl_ops;
+	video->pipe.stream_state = ISS_PIPELINE_STREAM_STOPPED;
+
+	video_set_drvdata(&video->video, video);
+
+	return 0;
+}
+
+void omap4iss_video_cleanup(struct iss_video *video)
+{
+	media_entity_cleanup(&video->video.entity);
+	mutex_destroy(&video->stream_lock);
+	mutex_destroy(&video->mutex);
+}
+
+int omap4iss_video_register(struct iss_video *video, struct v4l2_device *vdev)
+{
+	int ret;
+
+	video->video.v4l2_dev = vdev;
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		video->video.device_caps = V4L2_CAP_VIDEO_CAPTURE;
+	else
+		video->video.device_caps = V4L2_CAP_VIDEO_OUTPUT;
+	video->video.device_caps |= V4L2_CAP_STREAMING;
+
+	ret = video_register_device(&video->video, VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		dev_err(video->iss->dev,
+			"could not register video device (%d)\n", ret);
+
+	return ret;
+}
+
+void omap4iss_video_unregister(struct iss_video *video)
+{
+	video_unregister_device(&video->video);
+}
diff --git a/drivers/staging/media/omap4iss/iss_video.h b/drivers/staging/media/omap4iss/iss_video.h
new file mode 100644
index 000000000..ca2d5edb6
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_video.h
@@ -0,0 +1,203 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * TI OMAP4 ISS V4L2 Driver - Generic video node
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ */
+
+#ifndef OMAP4_ISS_VIDEO_H
+#define OMAP4_ISS_VIDEO_H
+
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define ISS_VIDEO_DRIVER_NAME		"issvideo"
+
+struct iss_device;
+struct iss_video;
+struct v4l2_mbus_framefmt;
+struct v4l2_pix_format;
+
+/*
+ * struct iss_format_info - ISS media bus format information
+ * @code: V4L2 media bus format code
+ * @truncated: V4L2 media bus format code for the same format truncated to 10
+ *	bits. Identical to @code if the format is 10 bits wide or less.
+ * @uncompressed: V4L2 media bus format code for the corresponding uncompressed
+ *	format. Identical to @code if the format is not DPCM compressed.
+ * @flavor: V4L2 media bus format code for the same pixel layout but
+ *	shifted to be 8 bits per pixel. =0 if format is not shiftable.
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @bpp: Bits per pixel
+ */
+struct iss_format_info {
+	u32 code;
+	u32 truncated;
+	u32 uncompressed;
+	u32 flavor;
+	u32 pixelformat;
+	unsigned int bpp;
+};
+
+enum iss_pipeline_stream_state {
+	ISS_PIPELINE_STREAM_STOPPED = 0,
+	ISS_PIPELINE_STREAM_CONTINUOUS = 1,
+	ISS_PIPELINE_STREAM_SINGLESHOT = 2,
+};
+
+enum iss_pipeline_state {
+	/* The stream has been started on the input video node. */
+	ISS_PIPELINE_STREAM_INPUT = 1,
+	/* The stream has been started on the output video node. */
+	ISS_PIPELINE_STREAM_OUTPUT = (1 << 1),
+	/* At least one buffer is queued on the input video node. */
+	ISS_PIPELINE_QUEUE_INPUT = (1 << 2),
+	/* At least one buffer is queued on the output video node. */
+	ISS_PIPELINE_QUEUE_OUTPUT = (1 << 3),
+	/* The input entity is idle, ready to be started. */
+	ISS_PIPELINE_IDLE_INPUT = (1 << 4),
+	/* The output entity is idle, ready to be started. */
+	ISS_PIPELINE_IDLE_OUTPUT = (1 << 5),
+	/* The pipeline is currently streaming. */
+	ISS_PIPELINE_STREAM = (1 << 6),
+};
+
+/*
+ * struct iss_pipeline - An OMAP4 ISS hardware pipeline
+ * @ent_enum: Entities in the pipeline
+ * @error: A hardware error occurred during capture
+ */
+struct iss_pipeline {
+	struct media_pipeline pipe;
+	spinlock_t lock;		/* Pipeline state and queue flags */
+	unsigned int state;
+	enum iss_pipeline_stream_state stream_state;
+	struct iss_video *input;
+	struct iss_video *output;
+	struct media_entity_enum ent_enum;
+	atomic_t frame_number;
+	bool do_propagation; /* of frame number */
+	bool error;
+	struct v4l2_fract max_timeperframe;
+	struct v4l2_subdev *external;
+	unsigned int external_rate;
+	int external_bpp;
+};
+
+static inline struct iss_pipeline *to_iss_pipeline(struct media_entity *entity)
+{
+	struct media_pipeline *pipe = media_entity_pipeline(entity);
+
+	if (!pipe)
+		return NULL;
+
+	return container_of(pipe, struct iss_pipeline, pipe);
+}
+
+static inline int iss_pipeline_ready(struct iss_pipeline *pipe)
+{
+	return pipe->state == (ISS_PIPELINE_STREAM_INPUT |
+			       ISS_PIPELINE_STREAM_OUTPUT |
+			       ISS_PIPELINE_QUEUE_INPUT |
+			       ISS_PIPELINE_QUEUE_OUTPUT |
+			       ISS_PIPELINE_IDLE_INPUT |
+			       ISS_PIPELINE_IDLE_OUTPUT);
+}
+
+/*
+ * struct iss_buffer - ISS buffer
+ * @buffer: ISS video buffer
+ * @iss_addr: Physical address of the buffer.
+ */
+struct iss_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer	vb;
+	struct list_head	list;
+	dma_addr_t iss_addr;
+};
+
+#define to_iss_buffer(buf)	container_of(buf, struct iss_buffer, vb)
+
+enum iss_video_dmaqueue_flags {
+	/* Set if DMA queue becomes empty when ISS_PIPELINE_STREAM_CONTINUOUS */
+	ISS_VIDEO_DMAQUEUE_UNDERRUN = (1 << 0),
+	/* Set when queuing buffer to an empty DMA queue */
+	ISS_VIDEO_DMAQUEUE_QUEUED = (1 << 1),
+};
+
+#define iss_video_dmaqueue_flags_clr(video)	\
+			({ (video)->dmaqueue_flags = 0; })
+
+/*
+ * struct iss_video_operations - ISS video operations
+ * @queue:	Resume streaming when a buffer is queued. Called on VIDIOC_QBUF
+ *		if there was no buffer previously queued.
+ */
+struct iss_video_operations {
+	int (*queue)(struct iss_video *video, struct iss_buffer *buffer);
+};
+
+struct iss_video {
+	struct video_device video;
+	enum v4l2_buf_type type;
+	struct media_pad pad;
+
+	struct mutex mutex;		/* format and crop settings */
+	atomic_t active;
+
+	struct iss_device *iss;
+
+	unsigned int capture_mem;
+	unsigned int bpl_alignment;	/* alignment value */
+	unsigned int bpl_zero_padding;	/* whether the alignment is optional */
+	unsigned int bpl_max;		/* maximum bytes per line value */
+	unsigned int bpl_value;		/* bytes per line value */
+	unsigned int bpl_padding;	/* padding at end of line */
+
+	/* Pipeline state */
+	struct iss_pipeline pipe;
+	struct mutex stream_lock;	/* pipeline and stream states */
+	bool error;
+
+	/* Video buffers queue */
+	struct vb2_queue *queue;
+	spinlock_t qlock;		/* protects dmaqueue and error */
+	struct list_head dmaqueue;
+	enum iss_video_dmaqueue_flags dmaqueue_flags;
+
+	const struct iss_video_operations *ops;
+};
+
+#define to_iss_video(vdev)	container_of(vdev, struct iss_video, video)
+
+struct iss_video_fh {
+	struct v4l2_fh vfh;
+	struct iss_video *video;
+	struct vb2_queue queue;
+	struct v4l2_format format;
+	struct v4l2_fract timeperframe;
+};
+
+#define to_iss_video_fh(fh)	container_of(fh, struct iss_video_fh, vfh)
+#define iss_video_queue_to_iss_video_fh(q) \
+				container_of(q, struct iss_video_fh, queue)
+
+int omap4iss_video_init(struct iss_video *video, const char *name);
+void omap4iss_video_cleanup(struct iss_video *video);
+int omap4iss_video_register(struct iss_video *video,
+			    struct v4l2_device *vdev);
+void omap4iss_video_unregister(struct iss_video *video);
+struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video);
+void omap4iss_video_cancel_stream(struct iss_video *video);
+struct media_pad *omap4iss_video_remote_pad(struct iss_video *video);
+
+const struct iss_format_info *
+omap4iss_video_format_info(u32 code);
+
+#endif /* OMAP4_ISS_VIDEO_H */
diff --git a/drivers/staging/media/rkvdec/Kconfig b/drivers/staging/media/rkvdec/Kconfig
new file mode 100644
index 000000000..e963d60cc
--- /dev/null
+++ b/drivers/staging/media/rkvdec/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_ROCKCHIP_VDEC
+	tristate "Rockchip Video Decoder driver"
+	depends on ARCH_ROCKCHIP || COMPILE_TEST
+	depends on VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select MEDIA_CONTROLLER_REQUEST_API
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_VMALLOC
+	select V4L2_MEM2MEM_DEV
+	select V4L2_H264
+	select V4L2_VP9
+	help
+	  Support for the Rockchip Video Decoder IP present on Rockchip SoCs,
+	  which accelerates video decoding.
+	  To compile this driver as a module, choose M here: the module
+	  will be called rockchip-vdec.
diff --git a/drivers/staging/media/rkvdec/Makefile b/drivers/staging/media/rkvdec/Makefile
new file mode 100644
index 000000000..cb86b429c
--- /dev/null
+++ b/drivers/staging/media/rkvdec/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_ROCKCHIP_VDEC) += rockchip-vdec.o
+
+rockchip-vdec-y += rkvdec.o rkvdec-h264.o rkvdec-vp9.o
diff --git a/drivers/staging/media/rkvdec/TODO b/drivers/staging/media/rkvdec/TODO
new file mode 100644
index 000000000..2c0779383
--- /dev/null
+++ b/drivers/staging/media/rkvdec/TODO
@@ -0,0 +1,11 @@
+* Support for HEVC is planned for this driver.
+
+  Given the V4L controls for that CODEC will be part of
+  the uABI, it will be required to have the driver in staging.
+
+  For this reason, we are keeping this driver in staging for now.
+
+* Evaluate introducing a helper to consolidate duplicated
+  code in rkvdec_request_validate and cedrus_request_validate.
+  The helper needs to the driver private data associated with
+  the videobuf2 queue, from a media request.
diff --git a/drivers/staging/media/rkvdec/rkvdec-h264.c b/drivers/staging/media/rkvdec/rkvdec-h264.c
new file mode 100644
index 000000000..4fc167b42
--- /dev/null
+++ b/drivers/staging/media/rkvdec/rkvdec-h264.c
@@ -0,0 +1,1192 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Rockchip Video Decoder H264 backend
+ *
+ * Copyright (C) 2019 Collabora, Ltd.
+ *	Boris Brezillon <boris.brezillon@collabora.com>
+ *
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ *	Jeffy Chen <jeffy.chen@rock-chips.com>
+ */
+
+#include <media/v4l2-h264.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "rkvdec.h"
+#include "rkvdec-regs.h"
+
+/* Size with u32 units. */
+#define RKV_CABAC_INIT_BUFFER_SIZE	(3680 + 128)
+#define RKV_RPS_SIZE			((128 + 128) / 4)
+#define RKV_ERROR_INFO_SIZE		(256 * 144 * 4)
+
+#define RKVDEC_NUM_REFLIST		3
+
+struct rkvdec_h264_scaling_list {
+	u8 scaling_list_4x4[6][16];
+	u8 scaling_list_8x8[6][64];
+	u8 padding[128];
+};
+
+struct rkvdec_sps_pps_packet {
+	u32 info[8];
+};
+
+struct rkvdec_ps_field {
+	u16 offset;
+	u8 len;
+};
+
+#define PS_FIELD(_offset, _len) \
+	((struct rkvdec_ps_field){ _offset, _len })
+
+#define SEQ_PARAMETER_SET_ID				PS_FIELD(0, 4)
+#define PROFILE_IDC					PS_FIELD(4, 8)
+#define CONSTRAINT_SET3_FLAG				PS_FIELD(12, 1)
+#define CHROMA_FORMAT_IDC				PS_FIELD(13, 2)
+#define BIT_DEPTH_LUMA					PS_FIELD(15, 3)
+#define BIT_DEPTH_CHROMA				PS_FIELD(18, 3)
+#define QPPRIME_Y_ZERO_TRANSFORM_BYPASS_FLAG		PS_FIELD(21, 1)
+#define LOG2_MAX_FRAME_NUM_MINUS4			PS_FIELD(22, 4)
+#define MAX_NUM_REF_FRAMES				PS_FIELD(26, 5)
+#define PIC_ORDER_CNT_TYPE				PS_FIELD(31, 2)
+#define LOG2_MAX_PIC_ORDER_CNT_LSB_MINUS4		PS_FIELD(33, 4)
+#define DELTA_PIC_ORDER_ALWAYS_ZERO_FLAG		PS_FIELD(37, 1)
+#define PIC_WIDTH_IN_MBS				PS_FIELD(38, 9)
+#define PIC_HEIGHT_IN_MBS				PS_FIELD(47, 9)
+#define FRAME_MBS_ONLY_FLAG				PS_FIELD(56, 1)
+#define MB_ADAPTIVE_FRAME_FIELD_FLAG			PS_FIELD(57, 1)
+#define DIRECT_8X8_INFERENCE_FLAG			PS_FIELD(58, 1)
+#define MVC_EXTENSION_ENABLE				PS_FIELD(59, 1)
+#define NUM_VIEWS					PS_FIELD(60, 2)
+#define VIEW_ID(i)					PS_FIELD(62 + ((i) * 10), 10)
+#define NUM_ANCHOR_REFS_L(i)				PS_FIELD(82 + ((i) * 11), 1)
+#define ANCHOR_REF_L(i)				PS_FIELD(83 + ((i) * 11), 10)
+#define NUM_NON_ANCHOR_REFS_L(i)			PS_FIELD(104 + ((i) * 11), 1)
+#define NON_ANCHOR_REFS_L(i)				PS_FIELD(105 + ((i) * 11), 10)
+#define PIC_PARAMETER_SET_ID				PS_FIELD(128, 8)
+#define PPS_SEQ_PARAMETER_SET_ID			PS_FIELD(136, 5)
+#define ENTROPY_CODING_MODE_FLAG			PS_FIELD(141, 1)
+#define BOTTOM_FIELD_PIC_ORDER_IN_FRAME_PRESENT_FLAG	PS_FIELD(142, 1)
+#define NUM_REF_IDX_L_DEFAULT_ACTIVE_MINUS1(i)		PS_FIELD(143 + ((i) * 5), 5)
+#define WEIGHTED_PRED_FLAG				PS_FIELD(153, 1)
+#define WEIGHTED_BIPRED_IDC				PS_FIELD(154, 2)
+#define PIC_INIT_QP_MINUS26				PS_FIELD(156, 7)
+#define PIC_INIT_QS_MINUS26				PS_FIELD(163, 6)
+#define CHROMA_QP_INDEX_OFFSET				PS_FIELD(169, 5)
+#define DEBLOCKING_FILTER_CONTROL_PRESENT_FLAG		PS_FIELD(174, 1)
+#define CONSTRAINED_INTRA_PRED_FLAG			PS_FIELD(175, 1)
+#define REDUNDANT_PIC_CNT_PRESENT			PS_FIELD(176, 1)
+#define TRANSFORM_8X8_MODE_FLAG			PS_FIELD(177, 1)
+#define SECOND_CHROMA_QP_INDEX_OFFSET			PS_FIELD(178, 5)
+#define SCALING_LIST_ENABLE_FLAG			PS_FIELD(183, 1)
+#define SCALING_LIST_ADDRESS				PS_FIELD(184, 32)
+#define IS_LONG_TERM(i)				PS_FIELD(216 + (i), 1)
+
+#define DPB_OFFS(i, j)					(288 + ((j) * 32 * 7) + ((i) * 7))
+#define DPB_INFO(i, j)					PS_FIELD(DPB_OFFS(i, j), 5)
+#define BOTTOM_FLAG(i, j)				PS_FIELD(DPB_OFFS(i, j) + 5, 1)
+#define VIEW_INDEX_OFF(i, j)				PS_FIELD(DPB_OFFS(i, j) + 6, 1)
+
+/* Data structure describing auxiliary buffer format. */
+struct rkvdec_h264_priv_tbl {
+	s8 cabac_table[4][464][2];
+	struct rkvdec_h264_scaling_list scaling_list;
+	u32 rps[RKV_RPS_SIZE];
+	struct rkvdec_sps_pps_packet param_set[256];
+	u8 err_info[RKV_ERROR_INFO_SIZE];
+};
+
+struct rkvdec_h264_reflists {
+	struct v4l2_h264_reference p[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference b0[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference b1[V4L2_H264_REF_LIST_LEN];
+};
+
+struct rkvdec_h264_run {
+	struct rkvdec_run base;
+	const struct v4l2_ctrl_h264_decode_params *decode_params;
+	const struct v4l2_ctrl_h264_sps *sps;
+	const struct v4l2_ctrl_h264_pps *pps;
+	const struct v4l2_ctrl_h264_scaling_matrix *scaling_matrix;
+	struct vb2_buffer *ref_buf[V4L2_H264_NUM_DPB_ENTRIES];
+};
+
+struct rkvdec_h264_ctx {
+	struct rkvdec_aux_buf priv_tbl;
+	struct rkvdec_h264_reflists reflists;
+};
+
+#define CABAC_ENTRY(ctxidx, idc0_m, idc0_n, idc1_m, idc1_n,		\
+	    idc2_m, idc2_n, intra_m, intra_n)			\
+	[0][(ctxidx)] = {idc0_m, idc0_n},			\
+	[1][(ctxidx)] = {idc1_m, idc1_n},			\
+	[2][(ctxidx)] = {idc2_m, idc2_n},			\
+	[3][(ctxidx)] = {intra_m, intra_n}
+
+/*
+ * Constant CABAC table.
+ * Built from the tables described in section '9.3.1.1 Initialisation process
+ * for context variables' of the H264 spec.
+ */
+static const s8 rkvdec_h264_cabac_table[4][464][2] = {
+	/* Table 9-12 – Values of variables m and n for ctxIdx from 0 to 10 */
+	CABAC_ENTRY(0, 20, -15, 20, -15, 20, -15, 20, -15),
+	CABAC_ENTRY(1, 2, 54, 2, 54, 2, 54, 2, 54),
+	CABAC_ENTRY(2, 3, 74, 3, 74, 3, 74, 3, 74),
+	CABAC_ENTRY(3, 20, -15, 20, -15, 20, -15, 20, -15),
+	CABAC_ENTRY(4, 2, 54, 2, 54, 2, 54, 2, 54),
+	CABAC_ENTRY(5, 3, 74, 3, 74, 3, 74, 3, 74),
+	CABAC_ENTRY(6, -28, 127, -28, 127, -28, 127, -28, 127),
+	CABAC_ENTRY(7, -23, 104, -23, 104, -23, 104, -23, 104),
+	CABAC_ENTRY(8, -6, 53, -6, 53, -6, 53, -6, 53),
+	CABAC_ENTRY(9, -1, 54, -1, 54, -1, 54, -1, 54),
+	CABAC_ENTRY(10, 7, 51, 7, 51, 7, 51, 7, 51),
+
+	/* Table 9-13 – Values of variables m and n for ctxIdx from 11 to 23 */
+	CABAC_ENTRY(11, 23, 33, 22, 25, 29, 16, 0, 0),
+	CABAC_ENTRY(12, 23, 2, 34, 0, 25, 0, 0, 0),
+	CABAC_ENTRY(13, 21, 0, 16, 0, 14, 0, 0, 0),
+	CABAC_ENTRY(14, 1, 9, -2, 9, -10, 51, 0, 0),
+	CABAC_ENTRY(15, 0, 49, 4, 41, -3, 62, 0, 0),
+	CABAC_ENTRY(16, -37, 118, -29, 118, -27, 99, 0, 0),
+	CABAC_ENTRY(17, 5, 57, 2, 65, 26, 16, 0, 0),
+	CABAC_ENTRY(18, -13, 78, -6, 71, -4, 85, 0, 0),
+	CABAC_ENTRY(19, -11, 65, -13, 79, -24, 102, 0, 0),
+	CABAC_ENTRY(20, 1, 62, 5, 52, 5, 57, 0, 0),
+	CABAC_ENTRY(21, 12, 49, 9, 50, 6, 57, 0, 0),
+	CABAC_ENTRY(22, -4, 73, -3, 70, -17, 73, 0, 0),
+	CABAC_ENTRY(23, 17, 50, 10, 54, 14, 57, 0, 0),
+
+	/* Table 9-14 – Values of variables m and n for ctxIdx from 24 to 39 */
+	CABAC_ENTRY(24, 18, 64, 26, 34, 20, 40, 0, 0),
+	CABAC_ENTRY(25, 9, 43, 19, 22, 20, 10, 0, 0),
+	CABAC_ENTRY(26, 29, 0, 40, 0, 29, 0, 0, 0),
+	CABAC_ENTRY(27, 26, 67, 57, 2, 54, 0, 0, 0),
+	CABAC_ENTRY(28, 16, 90, 41, 36, 37, 42, 0, 0),
+	CABAC_ENTRY(29, 9, 104, 26, 69, 12, 97, 0, 0),
+	CABAC_ENTRY(30, -46, 127, -45, 127, -32, 127, 0, 0),
+	CABAC_ENTRY(31, -20, 104, -15, 101, -22, 117, 0, 0),
+	CABAC_ENTRY(32, 1, 67, -4, 76, -2, 74, 0, 0),
+	CABAC_ENTRY(33, -13, 78, -6, 71, -4, 85, 0, 0),
+	CABAC_ENTRY(34, -11, 65, -13, 79, -24, 102, 0, 0),
+	CABAC_ENTRY(35, 1, 62, 5, 52, 5, 57, 0, 0),
+	CABAC_ENTRY(36, -6, 86, 6, 69, -6, 93, 0, 0),
+	CABAC_ENTRY(37, -17, 95, -13, 90, -14, 88, 0, 0),
+	CABAC_ENTRY(38, -6, 61, 0, 52, -6, 44, 0, 0),
+	CABAC_ENTRY(39, 9, 45, 8, 43, 4, 55, 0, 0),
+
+	/* Table 9-15 – Values of variables m and n for ctxIdx from 40 to 53 */
+	CABAC_ENTRY(40, -3, 69, -2, 69, -11, 89, 0, 0),
+	CABAC_ENTRY(41, -6, 81, -5, 82, -15, 103, 0, 0),
+	CABAC_ENTRY(42, -11, 96, -10, 96, -21, 116, 0, 0),
+	CABAC_ENTRY(43, 6, 55, 2, 59, 19, 57, 0, 0),
+	CABAC_ENTRY(44, 7, 67, 2, 75, 20, 58, 0, 0),
+	CABAC_ENTRY(45, -5, 86, -3, 87, 4, 84, 0, 0),
+	CABAC_ENTRY(46, 2, 88, -3, 100, 6, 96, 0, 0),
+	CABAC_ENTRY(47, 0, 58, 1, 56, 1, 63, 0, 0),
+	CABAC_ENTRY(48, -3, 76, -3, 74, -5, 85, 0, 0),
+	CABAC_ENTRY(49, -10, 94, -6, 85, -13, 106, 0, 0),
+	CABAC_ENTRY(50, 5, 54, 0, 59, 5, 63, 0, 0),
+	CABAC_ENTRY(51, 4, 69, -3, 81, 6, 75, 0, 0),
+	CABAC_ENTRY(52, -3, 81, -7, 86, -3, 90, 0, 0),
+	CABAC_ENTRY(53, 0, 88, -5, 95, -1, 101, 0, 0),
+
+	/* Table 9-16 – Values of variables m and n for ctxIdx from 54 to 59 */
+	CABAC_ENTRY(54, -7, 67, -1, 66, 3, 55, 0, 0),
+	CABAC_ENTRY(55, -5, 74, -1, 77, -4, 79, 0, 0),
+	CABAC_ENTRY(56, -4, 74, 1, 70, -2, 75, 0, 0),
+	CABAC_ENTRY(57, -5, 80, -2, 86, -12, 97, 0, 0),
+	CABAC_ENTRY(58, -7, 72, -5, 72, -7, 50, 0, 0),
+	CABAC_ENTRY(59, 1, 58, 0, 61, 1, 60, 0, 0),
+
+	/* Table 9-17 – Values of variables m and n for ctxIdx from 60 to 69 */
+	CABAC_ENTRY(60, 0, 41, 0, 41, 0, 41, 0, 41),
+	CABAC_ENTRY(61, 0, 63, 0, 63, 0, 63, 0, 63),
+	CABAC_ENTRY(62, 0, 63, 0, 63, 0, 63, 0, 63),
+	CABAC_ENTRY(63, 0, 63, 0, 63, 0, 63, 0, 63),
+	CABAC_ENTRY(64, -9, 83, -9, 83, -9, 83, -9, 83),
+	CABAC_ENTRY(65, 4, 86, 4, 86, 4, 86, 4, 86),
+	CABAC_ENTRY(66, 0, 97, 0, 97, 0, 97, 0, 97),
+	CABAC_ENTRY(67, -7, 72, -7, 72, -7, 72, -7, 72),
+	CABAC_ENTRY(68, 13, 41, 13, 41, 13, 41, 13, 41),
+	CABAC_ENTRY(69, 3, 62, 3, 62, 3, 62, 3, 62),
+
+	/* Table 9-18 – Values of variables m and n for ctxIdx from 70 to 104 */
+	CABAC_ENTRY(70, 0, 45, 13, 15, 7, 34, 0, 11),
+	CABAC_ENTRY(71, -4, 78, 7, 51, -9, 88, 1, 55),
+	CABAC_ENTRY(72, -3, 96, 2, 80, -20, 127, 0, 69),
+	CABAC_ENTRY(73, -27, 126, -39, 127, -36, 127, -17, 127),
+	CABAC_ENTRY(74, -28, 98, -18, 91, -17, 91, -13, 102),
+	CABAC_ENTRY(75, -25, 101, -17, 96, -14, 95, 0, 82),
+	CABAC_ENTRY(76, -23, 67, -26, 81, -25, 84, -7, 74),
+	CABAC_ENTRY(77, -28, 82, -35, 98, -25, 86, -21, 107),
+	CABAC_ENTRY(78, -20, 94, -24, 102, -12, 89, -27, 127),
+	CABAC_ENTRY(79, -16, 83, -23, 97, -17, 91, -31, 127),
+	CABAC_ENTRY(80, -22, 110, -27, 119, -31, 127, -24, 127),
+	CABAC_ENTRY(81, -21, 91, -24, 99, -14, 76, -18, 95),
+	CABAC_ENTRY(82, -18, 102, -21, 110, -18, 103, -27, 127),
+	CABAC_ENTRY(83, -13, 93, -18, 102, -13, 90, -21, 114),
+	CABAC_ENTRY(84, -29, 127, -36, 127, -37, 127, -30, 127),
+	CABAC_ENTRY(85, -7, 92, 0, 80, 11, 80, -17, 123),
+	CABAC_ENTRY(86, -5, 89, -5, 89, 5, 76, -12, 115),
+	CABAC_ENTRY(87, -7, 96, -7, 94, 2, 84, -16, 122),
+	CABAC_ENTRY(88, -13, 108, -4, 92, 5, 78, -11, 115),
+	CABAC_ENTRY(89, -3, 46, 0, 39, -6, 55, -12, 63),
+	CABAC_ENTRY(90, -1, 65, 0, 65, 4, 61, -2, 68),
+	CABAC_ENTRY(91, -1, 57, -15, 84, -14, 83, -15, 84),
+	CABAC_ENTRY(92, -9, 93, -35, 127, -37, 127, -13, 104),
+	CABAC_ENTRY(93, -3, 74, -2, 73, -5, 79, -3, 70),
+	CABAC_ENTRY(94, -9, 92, -12, 104, -11, 104, -8, 93),
+	CABAC_ENTRY(95, -8, 87, -9, 91, -11, 91, -10, 90),
+	CABAC_ENTRY(96, -23, 126, -31, 127, -30, 127, -30, 127),
+	CABAC_ENTRY(97, 5, 54, 3, 55, 0, 65, -1, 74),
+	CABAC_ENTRY(98, 6, 60, 7, 56, -2, 79, -6, 97),
+	CABAC_ENTRY(99, 6, 59, 7, 55, 0, 72, -7, 91),
+	CABAC_ENTRY(100, 6, 69, 8, 61, -4, 92, -20, 127),
+	CABAC_ENTRY(101, -1, 48, -3, 53, -6, 56, -4, 56),
+	CABAC_ENTRY(102, 0, 68, 0, 68, 3, 68, -5, 82),
+	CABAC_ENTRY(103, -4, 69, -7, 74, -8, 71, -7, 76),
+	CABAC_ENTRY(104, -8, 88, -9, 88, -13, 98, -22, 125),
+
+	/* Table 9-19 – Values of variables m and n for ctxIdx from 105 to 165 */
+	CABAC_ENTRY(105, -2, 85, -13, 103, -4, 86, -7, 93),
+	CABAC_ENTRY(106, -6, 78, -13, 91, -12, 88, -11, 87),
+	CABAC_ENTRY(107, -1, 75, -9, 89, -5, 82, -3, 77),
+	CABAC_ENTRY(108, -7, 77, -14, 92, -3, 72, -5, 71),
+	CABAC_ENTRY(109, 2, 54, -8, 76, -4, 67, -4, 63),
+	CABAC_ENTRY(110, 5, 50, -12, 87, -8, 72, -4, 68),
+	CABAC_ENTRY(111, -3, 68, -23, 110, -16, 89, -12, 84),
+	CABAC_ENTRY(112, 1, 50, -24, 105, -9, 69, -7, 62),
+	CABAC_ENTRY(113, 6, 42, -10, 78, -1, 59, -7, 65),
+	CABAC_ENTRY(114, -4, 81, -20, 112, 5, 66, 8, 61),
+	CABAC_ENTRY(115, 1, 63, -17, 99, 4, 57, 5, 56),
+	CABAC_ENTRY(116, -4, 70, -78, 127, -4, 71, -2, 66),
+	CABAC_ENTRY(117, 0, 67, -70, 127, -2, 71, 1, 64),
+	CABAC_ENTRY(118, 2, 57, -50, 127, 2, 58, 0, 61),
+	CABAC_ENTRY(119, -2, 76, -46, 127, -1, 74, -2, 78),
+	CABAC_ENTRY(120, 11, 35, -4, 66, -4, 44, 1, 50),
+	CABAC_ENTRY(121, 4, 64, -5, 78, -1, 69, 7, 52),
+	CABAC_ENTRY(122, 1, 61, -4, 71, 0, 62, 10, 35),
+	CABAC_ENTRY(123, 11, 35, -8, 72, -7, 51, 0, 44),
+	CABAC_ENTRY(124, 18, 25, 2, 59, -4, 47, 11, 38),
+	CABAC_ENTRY(125, 12, 24, -1, 55, -6, 42, 1, 45),
+	CABAC_ENTRY(126, 13, 29, -7, 70, -3, 41, 0, 46),
+	CABAC_ENTRY(127, 13, 36, -6, 75, -6, 53, 5, 44),
+	CABAC_ENTRY(128, -10, 93, -8, 89, 8, 76, 31, 17),
+	CABAC_ENTRY(129, -7, 73, -34, 119, -9, 78, 1, 51),
+	CABAC_ENTRY(130, -2, 73, -3, 75, -11, 83, 7, 50),
+	CABAC_ENTRY(131, 13, 46, 32, 20, 9, 52, 28, 19),
+	CABAC_ENTRY(132, 9, 49, 30, 22, 0, 67, 16, 33),
+	CABAC_ENTRY(133, -7, 100, -44, 127, -5, 90, 14, 62),
+	CABAC_ENTRY(134, 9, 53, 0, 54, 1, 67, -13, 108),
+	CABAC_ENTRY(135, 2, 53, -5, 61, -15, 72, -15, 100),
+	CABAC_ENTRY(136, 5, 53, 0, 58, -5, 75, -13, 101),
+	CABAC_ENTRY(137, -2, 61, -1, 60, -8, 80, -13, 91),
+	CABAC_ENTRY(138, 0, 56, -3, 61, -21, 83, -12, 94),
+	CABAC_ENTRY(139, 0, 56, -8, 67, -21, 64, -10, 88),
+	CABAC_ENTRY(140, -13, 63, -25, 84, -13, 31, -16, 84),
+	CABAC_ENTRY(141, -5, 60, -14, 74, -25, 64, -10, 86),
+	CABAC_ENTRY(142, -1, 62, -5, 65, -29, 94, -7, 83),
+	CABAC_ENTRY(143, 4, 57, 5, 52, 9, 75, -13, 87),
+	CABAC_ENTRY(144, -6, 69, 2, 57, 17, 63, -19, 94),
+	CABAC_ENTRY(145, 4, 57, 0, 61, -8, 74, 1, 70),
+	CABAC_ENTRY(146, 14, 39, -9, 69, -5, 35, 0, 72),
+	CABAC_ENTRY(147, 4, 51, -11, 70, -2, 27, -5, 74),
+	CABAC_ENTRY(148, 13, 68, 18, 55, 13, 91, 18, 59),
+	CABAC_ENTRY(149, 3, 64, -4, 71, 3, 65, -8, 102),
+	CABAC_ENTRY(150, 1, 61, 0, 58, -7, 69, -15, 100),
+	CABAC_ENTRY(151, 9, 63, 7, 61, 8, 77, 0, 95),
+	CABAC_ENTRY(152, 7, 50, 9, 41, -10, 66, -4, 75),
+	CABAC_ENTRY(153, 16, 39, 18, 25, 3, 62, 2, 72),
+	CABAC_ENTRY(154, 5, 44, 9, 32, -3, 68, -11, 75),
+	CABAC_ENTRY(155, 4, 52, 5, 43, -20, 81, -3, 71),
+	CABAC_ENTRY(156, 11, 48, 9, 47, 0, 30, 15, 46),
+	CABAC_ENTRY(157, -5, 60, 0, 44, 1, 7, -13, 69),
+	CABAC_ENTRY(158, -1, 59, 0, 51, -3, 23, 0, 62),
+	CABAC_ENTRY(159, 0, 59, 2, 46, -21, 74, 0, 65),
+	CABAC_ENTRY(160, 22, 33, 19, 38, 16, 66, 21, 37),
+	CABAC_ENTRY(161, 5, 44, -4, 66, -23, 124, -15, 72),
+	CABAC_ENTRY(162, 14, 43, 15, 38, 17, 37, 9, 57),
+	CABAC_ENTRY(163, -1, 78, 12, 42, 44, -18, 16, 54),
+	CABAC_ENTRY(164, 0, 60, 9, 34, 50, -34, 0, 62),
+	CABAC_ENTRY(165, 9, 69, 0, 89, -22, 127, 12, 72),
+
+	/* Table 9-20 – Values of variables m and n for ctxIdx from 166 to 226 */
+	CABAC_ENTRY(166, 11, 28, 4, 45, 4, 39, 24, 0),
+	CABAC_ENTRY(167, 2, 40, 10, 28, 0, 42, 15, 9),
+	CABAC_ENTRY(168, 3, 44, 10, 31, 7, 34, 8, 25),
+	CABAC_ENTRY(169, 0, 49, 33, -11, 11, 29, 13, 18),
+	CABAC_ENTRY(170, 0, 46, 52, -43, 8, 31, 15, 9),
+	CABAC_ENTRY(171, 2, 44, 18, 15, 6, 37, 13, 19),
+	CABAC_ENTRY(172, 2, 51, 28, 0, 7, 42, 10, 37),
+	CABAC_ENTRY(173, 0, 47, 35, -22, 3, 40, 12, 18),
+	CABAC_ENTRY(174, 4, 39, 38, -25, 8, 33, 6, 29),
+	CABAC_ENTRY(175, 2, 62, 34, 0, 13, 43, 20, 33),
+	CABAC_ENTRY(176, 6, 46, 39, -18, 13, 36, 15, 30),
+	CABAC_ENTRY(177, 0, 54, 32, -12, 4, 47, 4, 45),
+	CABAC_ENTRY(178, 3, 54, 102, -94, 3, 55, 1, 58),
+	CABAC_ENTRY(179, 2, 58, 0, 0, 2, 58, 0, 62),
+	CABAC_ENTRY(180, 4, 63, 56, -15, 6, 60, 7, 61),
+	CABAC_ENTRY(181, 6, 51, 33, -4, 8, 44, 12, 38),
+	CABAC_ENTRY(182, 6, 57, 29, 10, 11, 44, 11, 45),
+	CABAC_ENTRY(183, 7, 53, 37, -5, 14, 42, 15, 39),
+	CABAC_ENTRY(184, 6, 52, 51, -29, 7, 48, 11, 42),
+	CABAC_ENTRY(185, 6, 55, 39, -9, 4, 56, 13, 44),
+	CABAC_ENTRY(186, 11, 45, 52, -34, 4, 52, 16, 45),
+	CABAC_ENTRY(187, 14, 36, 69, -58, 13, 37, 12, 41),
+	CABAC_ENTRY(188, 8, 53, 67, -63, 9, 49, 10, 49),
+	CABAC_ENTRY(189, -1, 82, 44, -5, 19, 58, 30, 34),
+	CABAC_ENTRY(190, 7, 55, 32, 7, 10, 48, 18, 42),
+	CABAC_ENTRY(191, -3, 78, 55, -29, 12, 45, 10, 55),
+	CABAC_ENTRY(192, 15, 46, 32, 1, 0, 69, 17, 51),
+	CABAC_ENTRY(193, 22, 31, 0, 0, 20, 33, 17, 46),
+	CABAC_ENTRY(194, -1, 84, 27, 36, 8, 63, 0, 89),
+	CABAC_ENTRY(195, 25, 7, 33, -25, 35, -18, 26, -19),
+	CABAC_ENTRY(196, 30, -7, 34, -30, 33, -25, 22, -17),
+	CABAC_ENTRY(197, 28, 3, 36, -28, 28, -3, 26, -17),
+	CABAC_ENTRY(198, 28, 4, 38, -28, 24, 10, 30, -25),
+	CABAC_ENTRY(199, 32, 0, 38, -27, 27, 0, 28, -20),
+	CABAC_ENTRY(200, 34, -1, 34, -18, 34, -14, 33, -23),
+	CABAC_ENTRY(201, 30, 6, 35, -16, 52, -44, 37, -27),
+	CABAC_ENTRY(202, 30, 6, 34, -14, 39, -24, 33, -23),
+	CABAC_ENTRY(203, 32, 9, 32, -8, 19, 17, 40, -28),
+	CABAC_ENTRY(204, 31, 19, 37, -6, 31, 25, 38, -17),
+	CABAC_ENTRY(205, 26, 27, 35, 0, 36, 29, 33, -11),
+	CABAC_ENTRY(206, 26, 30, 30, 10, 24, 33, 40, -15),
+	CABAC_ENTRY(207, 37, 20, 28, 18, 34, 15, 41, -6),
+	CABAC_ENTRY(208, 28, 34, 26, 25, 30, 20, 38, 1),
+	CABAC_ENTRY(209, 17, 70, 29, 41, 22, 73, 41, 17),
+	CABAC_ENTRY(210, 1, 67, 0, 75, 20, 34, 30, -6),
+	CABAC_ENTRY(211, 5, 59, 2, 72, 19, 31, 27, 3),
+	CABAC_ENTRY(212, 9, 67, 8, 77, 27, 44, 26, 22),
+	CABAC_ENTRY(213, 16, 30, 14, 35, 19, 16, 37, -16),
+	CABAC_ENTRY(214, 18, 32, 18, 31, 15, 36, 35, -4),
+	CABAC_ENTRY(215, 18, 35, 17, 35, 15, 36, 38, -8),
+	CABAC_ENTRY(216, 22, 29, 21, 30, 21, 28, 38, -3),
+	CABAC_ENTRY(217, 24, 31, 17, 45, 25, 21, 37, 3),
+	CABAC_ENTRY(218, 23, 38, 20, 42, 30, 20, 38, 5),
+	CABAC_ENTRY(219, 18, 43, 18, 45, 31, 12, 42, 0),
+	CABAC_ENTRY(220, 20, 41, 27, 26, 27, 16, 35, 16),
+	CABAC_ENTRY(221, 11, 63, 16, 54, 24, 42, 39, 22),
+	CABAC_ENTRY(222, 9, 59, 7, 66, 0, 93, 14, 48),
+	CABAC_ENTRY(223, 9, 64, 16, 56, 14, 56, 27, 37),
+	CABAC_ENTRY(224, -1, 94, 11, 73, 15, 57, 21, 60),
+	CABAC_ENTRY(225, -2, 89, 10, 67, 26, 38, 12, 68),
+	CABAC_ENTRY(226, -9, 108, -10, 116, -24, 127, 2, 97),
+
+	/* Table 9-21 – Values of variables m and n for ctxIdx from 227 to 275 */
+	CABAC_ENTRY(227, -6, 76, -23, 112, -24, 115, -3, 71),
+	CABAC_ENTRY(228, -2, 44, -15, 71, -22, 82, -6, 42),
+	CABAC_ENTRY(229, 0, 45, -7, 61, -9, 62, -5, 50),
+	CABAC_ENTRY(230, 0, 52, 0, 53, 0, 53, -3, 54),
+	CABAC_ENTRY(231, -3, 64, -5, 66, 0, 59, -2, 62),
+	CABAC_ENTRY(232, -2, 59, -11, 77, -14, 85, 0, 58),
+	CABAC_ENTRY(233, -4, 70, -9, 80, -13, 89, 1, 63),
+	CABAC_ENTRY(234, -4, 75, -9, 84, -13, 94, -2, 72),
+	CABAC_ENTRY(235, -8, 82, -10, 87, -11, 92, -1, 74),
+	CABAC_ENTRY(236, -17, 102, -34, 127, -29, 127, -9, 91),
+	CABAC_ENTRY(237, -9, 77, -21, 101, -21, 100, -5, 67),
+	CABAC_ENTRY(238, 3, 24, -3, 39, -14, 57, -5, 27),
+	CABAC_ENTRY(239, 0, 42, -5, 53, -12, 67, -3, 39),
+	CABAC_ENTRY(240, 0, 48, -7, 61, -11, 71, -2, 44),
+	CABAC_ENTRY(241, 0, 55, -11, 75, -10, 77, 0, 46),
+	CABAC_ENTRY(242, -6, 59, -15, 77, -21, 85, -16, 64),
+	CABAC_ENTRY(243, -7, 71, -17, 91, -16, 88, -8, 68),
+	CABAC_ENTRY(244, -12, 83, -25, 107, -23, 104, -10, 78),
+	CABAC_ENTRY(245, -11, 87, -25, 111, -15, 98, -6, 77),
+	CABAC_ENTRY(246, -30, 119, -28, 122, -37, 127, -10, 86),
+	CABAC_ENTRY(247, 1, 58, -11, 76, -10, 82, -12, 92),
+	CABAC_ENTRY(248, -3, 29, -10, 44, -8, 48, -15, 55),
+	CABAC_ENTRY(249, -1, 36, -10, 52, -8, 61, -10, 60),
+	CABAC_ENTRY(250, 1, 38, -10, 57, -8, 66, -6, 62),
+	CABAC_ENTRY(251, 2, 43, -9, 58, -7, 70, -4, 65),
+	CABAC_ENTRY(252, -6, 55, -16, 72, -14, 75, -12, 73),
+	CABAC_ENTRY(253, 0, 58, -7, 69, -10, 79, -8, 76),
+	CABAC_ENTRY(254, 0, 64, -4, 69, -9, 83, -7, 80),
+	CABAC_ENTRY(255, -3, 74, -5, 74, -12, 92, -9, 88),
+	CABAC_ENTRY(256, -10, 90, -9, 86, -18, 108, -17, 110),
+	CABAC_ENTRY(257, 0, 70, 2, 66, -4, 79, -11, 97),
+	CABAC_ENTRY(258, -4, 29, -9, 34, -22, 69, -20, 84),
+	CABAC_ENTRY(259, 5, 31, 1, 32, -16, 75, -11, 79),
+	CABAC_ENTRY(260, 7, 42, 11, 31, -2, 58, -6, 73),
+	CABAC_ENTRY(261, 1, 59, 5, 52, 1, 58, -4, 74),
+	CABAC_ENTRY(262, -2, 58, -2, 55, -13, 78, -13, 86),
+	CABAC_ENTRY(263, -3, 72, -2, 67, -9, 83, -13, 96),
+	CABAC_ENTRY(264, -3, 81, 0, 73, -4, 81, -11, 97),
+	CABAC_ENTRY(265, -11, 97, -8, 89, -13, 99, -19, 117),
+	CABAC_ENTRY(266, 0, 58, 3, 52, -13, 81, -8, 78),
+	CABAC_ENTRY(267, 8, 5, 7, 4, -6, 38, -5, 33),
+	CABAC_ENTRY(268, 10, 14, 10, 8, -13, 62, -4, 48),
+	CABAC_ENTRY(269, 14, 18, 17, 8, -6, 58, -2, 53),
+	CABAC_ENTRY(270, 13, 27, 16, 19, -2, 59, -3, 62),
+	CABAC_ENTRY(271, 2, 40, 3, 37, -16, 73, -13, 71),
+	CABAC_ENTRY(272, 0, 58, -1, 61, -10, 76, -10, 79),
+	CABAC_ENTRY(273, -3, 70, -5, 73, -13, 86, -12, 86),
+	CABAC_ENTRY(274, -6, 79, -1, 70, -9, 83, -13, 90),
+	CABAC_ENTRY(275, -8, 85, -4, 78, -10, 87, -14, 97),
+
+	/* Table 9-22 – Values of variables m and n for ctxIdx from 277 to 337 */
+	CABAC_ENTRY(277, -13, 106, -21, 126, -22, 127, -6, 93),
+	CABAC_ENTRY(278, -16, 106, -23, 124, -25, 127, -6, 84),
+	CABAC_ENTRY(279, -10, 87, -20, 110, -25, 120, -8, 79),
+	CABAC_ENTRY(280, -21, 114, -26, 126, -27, 127, 0, 66),
+	CABAC_ENTRY(281, -18, 110, -25, 124, -19, 114, -1, 71),
+	CABAC_ENTRY(282, -14, 98, -17, 105, -23, 117, 0, 62),
+	CABAC_ENTRY(283, -22, 110, -27, 121, -25, 118, -2, 60),
+	CABAC_ENTRY(284, -21, 106, -27, 117, -26, 117, -2, 59),
+	CABAC_ENTRY(285, -18, 103, -17, 102, -24, 113, -5, 75),
+	CABAC_ENTRY(286, -21, 107, -26, 117, -28, 118, -3, 62),
+	CABAC_ENTRY(287, -23, 108, -27, 116, -31, 120, -4, 58),
+	CABAC_ENTRY(288, -26, 112, -33, 122, -37, 124, -9, 66),
+	CABAC_ENTRY(289, -10, 96, -10, 95, -10, 94, -1, 79),
+	CABAC_ENTRY(290, -12, 95, -14, 100, -15, 102, 0, 71),
+	CABAC_ENTRY(291, -5, 91, -8, 95, -10, 99, 3, 68),
+	CABAC_ENTRY(292, -9, 93, -17, 111, -13, 106, 10, 44),
+	CABAC_ENTRY(293, -22, 94, -28, 114, -50, 127, -7, 62),
+	CABAC_ENTRY(294, -5, 86, -6, 89, -5, 92, 15, 36),
+	CABAC_ENTRY(295, 9, 67, -2, 80, 17, 57, 14, 40),
+	CABAC_ENTRY(296, -4, 80, -4, 82, -5, 86, 16, 27),
+	CABAC_ENTRY(297, -10, 85, -9, 85, -13, 94, 12, 29),
+	CABAC_ENTRY(298, -1, 70, -8, 81, -12, 91, 1, 44),
+	CABAC_ENTRY(299, 7, 60, -1, 72, -2, 77, 20, 36),
+	CABAC_ENTRY(300, 9, 58, 5, 64, 0, 71, 18, 32),
+	CABAC_ENTRY(301, 5, 61, 1, 67, -1, 73, 5, 42),
+	CABAC_ENTRY(302, 12, 50, 9, 56, 4, 64, 1, 48),
+	CABAC_ENTRY(303, 15, 50, 0, 69, -7, 81, 10, 62),
+	CABAC_ENTRY(304, 18, 49, 1, 69, 5, 64, 17, 46),
+	CABAC_ENTRY(305, 17, 54, 7, 69, 15, 57, 9, 64),
+	CABAC_ENTRY(306, 10, 41, -7, 69, 1, 67, -12, 104),
+	CABAC_ENTRY(307, 7, 46, -6, 67, 0, 68, -11, 97),
+	CABAC_ENTRY(308, -1, 51, -16, 77, -10, 67, -16, 96),
+	CABAC_ENTRY(309, 7, 49, -2, 64, 1, 68, -7, 88),
+	CABAC_ENTRY(310, 8, 52, 2, 61, 0, 77, -8, 85),
+	CABAC_ENTRY(311, 9, 41, -6, 67, 2, 64, -7, 85),
+	CABAC_ENTRY(312, 6, 47, -3, 64, 0, 68, -9, 85),
+	CABAC_ENTRY(313, 2, 55, 2, 57, -5, 78, -13, 88),
+	CABAC_ENTRY(314, 13, 41, -3, 65, 7, 55, 4, 66),
+	CABAC_ENTRY(315, 10, 44, -3, 66, 5, 59, -3, 77),
+	CABAC_ENTRY(316, 6, 50, 0, 62, 2, 65, -3, 76),
+	CABAC_ENTRY(317, 5, 53, 9, 51, 14, 54, -6, 76),
+	CABAC_ENTRY(318, 13, 49, -1, 66, 15, 44, 10, 58),
+	CABAC_ENTRY(319, 4, 63, -2, 71, 5, 60, -1, 76),
+	CABAC_ENTRY(320, 6, 64, -2, 75, 2, 70, -1, 83),
+	CABAC_ENTRY(321, -2, 69, -1, 70, -2, 76, -7, 99),
+	CABAC_ENTRY(322, -2, 59, -9, 72, -18, 86, -14, 95),
+	CABAC_ENTRY(323, 6, 70, 14, 60, 12, 70, 2, 95),
+	CABAC_ENTRY(324, 10, 44, 16, 37, 5, 64, 0, 76),
+	CABAC_ENTRY(325, 9, 31, 0, 47, -12, 70, -5, 74),
+	CABAC_ENTRY(326, 12, 43, 18, 35, 11, 55, 0, 70),
+	CABAC_ENTRY(327, 3, 53, 11, 37, 5, 56, -11, 75),
+	CABAC_ENTRY(328, 14, 34, 12, 41, 0, 69, 1, 68),
+	CABAC_ENTRY(329, 10, 38, 10, 41, 2, 65, 0, 65),
+	CABAC_ENTRY(330, -3, 52, 2, 48, -6, 74, -14, 73),
+	CABAC_ENTRY(331, 13, 40, 12, 41, 5, 54, 3, 62),
+	CABAC_ENTRY(332, 17, 32, 13, 41, 7, 54, 4, 62),
+	CABAC_ENTRY(333, 7, 44, 0, 59, -6, 76, -1, 68),
+	CABAC_ENTRY(334, 7, 38, 3, 50, -11, 82, -13, 75),
+	CABAC_ENTRY(335, 13, 50, 19, 40, -2, 77, 11, 55),
+	CABAC_ENTRY(336, 10, 57, 3, 66, -2, 77, 5, 64),
+	CABAC_ENTRY(337, 26, 43, 18, 50, 25, 42, 12, 70),
+
+	/* Table 9-23 – Values of variables m and n for ctxIdx from 338 to 398 */
+	CABAC_ENTRY(338, 14, 11, 19, -6, 17, -13, 15, 6),
+	CABAC_ENTRY(339, 11, 14, 18, -6, 16, -9, 6, 19),
+	CABAC_ENTRY(340, 9, 11, 14, 0, 17, -12, 7, 16),
+	CABAC_ENTRY(341, 18, 11, 26, -12, 27, -21, 12, 14),
+	CABAC_ENTRY(342, 21, 9, 31, -16, 37, -30, 18, 13),
+	CABAC_ENTRY(343, 23, -2, 33, -25, 41, -40, 13, 11),
+	CABAC_ENTRY(344, 32, -15, 33, -22, 42, -41, 13, 15),
+	CABAC_ENTRY(345, 32, -15, 37, -28, 48, -47, 15, 16),
+	CABAC_ENTRY(346, 34, -21, 39, -30, 39, -32, 12, 23),
+	CABAC_ENTRY(347, 39, -23, 42, -30, 46, -40, 13, 23),
+	CABAC_ENTRY(348, 42, -33, 47, -42, 52, -51, 15, 20),
+	CABAC_ENTRY(349, 41, -31, 45, -36, 46, -41, 14, 26),
+	CABAC_ENTRY(350, 46, -28, 49, -34, 52, -39, 14, 44),
+	CABAC_ENTRY(351, 38, -12, 41, -17, 43, -19, 17, 40),
+	CABAC_ENTRY(352, 21, 29, 32, 9, 32, 11, 17, 47),
+	CABAC_ENTRY(353, 45, -24, 69, -71, 61, -55, 24, 17),
+	CABAC_ENTRY(354, 53, -45, 63, -63, 56, -46, 21, 21),
+	CABAC_ENTRY(355, 48, -26, 66, -64, 62, -50, 25, 22),
+	CABAC_ENTRY(356, 65, -43, 77, -74, 81, -67, 31, 27),
+	CABAC_ENTRY(357, 43, -19, 54, -39, 45, -20, 22, 29),
+	CABAC_ENTRY(358, 39, -10, 52, -35, 35, -2, 19, 35),
+	CABAC_ENTRY(359, 30, 9, 41, -10, 28, 15, 14, 50),
+	CABAC_ENTRY(360, 18, 26, 36, 0, 34, 1, 10, 57),
+	CABAC_ENTRY(361, 20, 27, 40, -1, 39, 1, 7, 63),
+	CABAC_ENTRY(362, 0, 57, 30, 14, 30, 17, -2, 77),
+	CABAC_ENTRY(363, -14, 82, 28, 26, 20, 38, -4, 82),
+	CABAC_ENTRY(364, -5, 75, 23, 37, 18, 45, -3, 94),
+	CABAC_ENTRY(365, -19, 97, 12, 55, 15, 54, 9, 69),
+	CABAC_ENTRY(366, -35, 125, 11, 65, 0, 79, -12, 109),
+	CABAC_ENTRY(367, 27, 0, 37, -33, 36, -16, 36, -35),
+	CABAC_ENTRY(368, 28, 0, 39, -36, 37, -14, 36, -34),
+	CABAC_ENTRY(369, 31, -4, 40, -37, 37, -17, 32, -26),
+	CABAC_ENTRY(370, 27, 6, 38, -30, 32, 1, 37, -30),
+	CABAC_ENTRY(371, 34, 8, 46, -33, 34, 15, 44, -32),
+	CABAC_ENTRY(372, 30, 10, 42, -30, 29, 15, 34, -18),
+	CABAC_ENTRY(373, 24, 22, 40, -24, 24, 25, 34, -15),
+	CABAC_ENTRY(374, 33, 19, 49, -29, 34, 22, 40, -15),
+	CABAC_ENTRY(375, 22, 32, 38, -12, 31, 16, 33, -7),
+	CABAC_ENTRY(376, 26, 31, 40, -10, 35, 18, 35, -5),
+	CABAC_ENTRY(377, 21, 41, 38, -3, 31, 28, 33, 0),
+	CABAC_ENTRY(378, 26, 44, 46, -5, 33, 41, 38, 2),
+	CABAC_ENTRY(379, 23, 47, 31, 20, 36, 28, 33, 13),
+	CABAC_ENTRY(380, 16, 65, 29, 30, 27, 47, 23, 35),
+	CABAC_ENTRY(381, 14, 71, 25, 44, 21, 62, 13, 58),
+	CABAC_ENTRY(382, 8, 60, 12, 48, 18, 31, 29, -3),
+	CABAC_ENTRY(383, 6, 63, 11, 49, 19, 26, 26, 0),
+	CABAC_ENTRY(384, 17, 65, 26, 45, 36, 24, 22, 30),
+	CABAC_ENTRY(385, 21, 24, 22, 22, 24, 23, 31, -7),
+	CABAC_ENTRY(386, 23, 20, 23, 22, 27, 16, 35, -15),
+	CABAC_ENTRY(387, 26, 23, 27, 21, 24, 30, 34, -3),
+	CABAC_ENTRY(388, 27, 32, 33, 20, 31, 29, 34, 3),
+	CABAC_ENTRY(389, 28, 23, 26, 28, 22, 41, 36, -1),
+	CABAC_ENTRY(390, 28, 24, 30, 24, 22, 42, 34, 5),
+	CABAC_ENTRY(391, 23, 40, 27, 34, 16, 60, 32, 11),
+	CABAC_ENTRY(392, 24, 32, 18, 42, 15, 52, 35, 5),
+	CABAC_ENTRY(393, 28, 29, 25, 39, 14, 60, 34, 12),
+	CABAC_ENTRY(394, 23, 42, 18, 50, 3, 78, 39, 11),
+	CABAC_ENTRY(395, 19, 57, 12, 70, -16, 123, 30, 29),
+	CABAC_ENTRY(396, 22, 53, 21, 54, 21, 53, 34, 26),
+	CABAC_ENTRY(397, 22, 61, 14, 71, 22, 56, 29, 39),
+	CABAC_ENTRY(398, 11, 86, 11, 83, 25, 61, 19, 66),
+
+	/* Values of variables m and n for ctxIdx from 399 to 463 (not documented) */
+	CABAC_ENTRY(399, 12, 40, 25, 32, 21, 33, 31, 21),
+	CABAC_ENTRY(400, 11, 51, 21, 49, 19, 50, 31, 31),
+	CABAC_ENTRY(401, 14, 59, 21, 54, 17, 61, 25, 50),
+	CABAC_ENTRY(402, -4, 79, -5, 85, -3, 78, -17, 120),
+	CABAC_ENTRY(403, -7, 71, -6, 81, -8, 74, -20, 112),
+	CABAC_ENTRY(404, -5, 69, -10, 77, -9, 72, -18, 114),
+	CABAC_ENTRY(405, -9, 70, -7, 81, -10, 72, -11, 85),
+	CABAC_ENTRY(406, -8, 66, -17, 80, -18, 75, -15, 92),
+	CABAC_ENTRY(407, -10, 68, -18, 73, -12, 71, -14, 89),
+	CABAC_ENTRY(408, -19, 73, -4, 74, -11, 63, -26, 71),
+	CABAC_ENTRY(409, -12, 69, -10, 83, -5, 70, -15, 81),
+	CABAC_ENTRY(410, -16, 70, -9, 71, -17, 75, -14, 80),
+	CABAC_ENTRY(411, -15, 67, -9, 67, -14, 72, 0, 68),
+	CABAC_ENTRY(412, -20, 62, -1, 61, -16, 67, -14, 70),
+	CABAC_ENTRY(413, -19, 70, -8, 66, -8, 53, -24, 56),
+	CABAC_ENTRY(414, -16, 66, -14, 66, -14, 59, -23, 68),
+	CABAC_ENTRY(415, -22, 65, 0, 59, -9, 52, -24, 50),
+	CABAC_ENTRY(416, -20, 63, 2, 59, -11, 68, -11, 74),
+	CABAC_ENTRY(417, 9, -2, 17, -10, 9, -2, 23, -13),
+	CABAC_ENTRY(418, 26, -9, 32, -13, 30, -10, 26, -13),
+	CABAC_ENTRY(419, 33, -9, 42, -9, 31, -4, 40, -15),
+	CABAC_ENTRY(420, 39, -7, 49, -5, 33, -1, 49, -14),
+	CABAC_ENTRY(421, 41, -2, 53, 0, 33, 7, 44, 3),
+	CABAC_ENTRY(422, 45, 3, 64, 3, 31, 12, 45, 6),
+	CABAC_ENTRY(423, 49, 9, 68, 10, 37, 23, 44, 34),
+	CABAC_ENTRY(424, 45, 27, 66, 27, 31, 38, 33, 54),
+	CABAC_ENTRY(425, 36, 59, 47, 57, 20, 64, 19, 82),
+	CABAC_ENTRY(426, -6, 66, -5, 71, -9, 71, -3, 75),
+	CABAC_ENTRY(427, -7, 35, 0, 24, -7, 37, -1, 23),
+	CABAC_ENTRY(428, -7, 42, -1, 36, -8, 44, 1, 34),
+	CABAC_ENTRY(429, -8, 45, -2, 42, -11, 49, 1, 43),
+	CABAC_ENTRY(430, -5, 48, -2, 52, -10, 56, 0, 54),
+	CABAC_ENTRY(431, -12, 56, -9, 57, -12, 59, -2, 55),
+	CABAC_ENTRY(432, -6, 60, -6, 63, -8, 63, 0, 61),
+	CABAC_ENTRY(433, -5, 62, -4, 65, -9, 67, 1, 64),
+	CABAC_ENTRY(434, -8, 66, -4, 67, -6, 68, 0, 68),
+	CABAC_ENTRY(435, -8, 76, -7, 82, -10, 79, -9, 92),
+	CABAC_ENTRY(436, -5, 85, -3, 81, -3, 78, -14, 106),
+	CABAC_ENTRY(437, -6, 81, -3, 76, -8, 74, -13, 97),
+	CABAC_ENTRY(438, -10, 77, -7, 72, -9, 72, -15, 90),
+	CABAC_ENTRY(439, -7, 81, -6, 78, -10, 72, -12, 90),
+	CABAC_ENTRY(440, -17, 80, -12, 72, -18, 75, -18, 88),
+	CABAC_ENTRY(441, -18, 73, -14, 68, -12, 71, -10, 73),
+	CABAC_ENTRY(442, -4, 74, -3, 70, -11, 63, -9, 79),
+	CABAC_ENTRY(443, -10, 83, -6, 76, -5, 70, -14, 86),
+	CABAC_ENTRY(444, -9, 71, -5, 66, -17, 75, -10, 73),
+	CABAC_ENTRY(445, -9, 67, -5, 62, -14, 72, -10, 70),
+	CABAC_ENTRY(446, -1, 61, 0, 57, -16, 67, -10, 69),
+	CABAC_ENTRY(447, -8, 66, -4, 61, -8, 53, -5, 66),
+	CABAC_ENTRY(448, -14, 66, -9, 60, -14, 59, -9, 64),
+	CABAC_ENTRY(449, 0, 59, 1, 54, -9, 52, -5, 58),
+	CABAC_ENTRY(450, 2, 59, 2, 58, -11, 68, 2, 59),
+	CABAC_ENTRY(451, 21, -13, 17, -10, 9, -2, 21, -10),
+	CABAC_ENTRY(452, 33, -14, 32, -13, 30, -10, 24, -11),
+	CABAC_ENTRY(453, 39, -7, 42, -9, 31, -4, 28, -8),
+	CABAC_ENTRY(454, 46, -2, 49, -5, 33, -1, 28, -1),
+	CABAC_ENTRY(455, 51, 2, 53, 0, 33, 7, 29, 3),
+	CABAC_ENTRY(456, 60, 6, 64, 3, 31, 12, 29, 9),
+	CABAC_ENTRY(457, 61, 17, 68, 10, 37, 23, 35, 20),
+	CABAC_ENTRY(458, 55, 34, 66, 27, 31, 38, 29, 36),
+	CABAC_ENTRY(459, 42, 62, 47, 57, 20, 64, 14, 67),
+};
+
+static void set_ps_field(u32 *buf, struct rkvdec_ps_field field, u32 value)
+{
+	u8 bit = field.offset % 32, word = field.offset / 32;
+	u64 mask = GENMASK_ULL(bit + field.len - 1, bit);
+	u64 val = ((u64)value << bit) & mask;
+
+	buf[word] &= ~mask;
+	buf[word] |= val;
+	if (bit + field.len > 32) {
+		buf[word + 1] &= ~(mask >> 32);
+		buf[word + 1] |= val >> 32;
+	}
+}
+
+static void assemble_hw_pps(struct rkvdec_ctx *ctx,
+			    struct rkvdec_h264_run *run)
+{
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	const struct v4l2_ctrl_h264_sps *sps = run->sps;
+	const struct v4l2_ctrl_h264_pps *pps = run->pps;
+	const struct v4l2_ctrl_h264_decode_params *dec_params = run->decode_params;
+	const struct v4l2_h264_dpb_entry *dpb = dec_params->dpb;
+	struct rkvdec_h264_priv_tbl *priv_tbl = h264_ctx->priv_tbl.cpu;
+	struct rkvdec_sps_pps_packet *hw_ps;
+	dma_addr_t scaling_list_address;
+	u32 scaling_distance;
+	u32 i;
+
+	/*
+	 * HW read the SPS/PPS information from PPS packet index by PPS id.
+	 * offset from the base can be calculated by PPS_id * 32 (size per PPS
+	 * packet unit). so the driver copy SPS/PPS information to the exact PPS
+	 * packet unit for HW accessing.
+	 */
+	hw_ps = &priv_tbl->param_set[pps->pic_parameter_set_id];
+	memset(hw_ps, 0, sizeof(*hw_ps));
+
+#define WRITE_PPS(value, field) set_ps_field(hw_ps->info, field, value)
+	/* write sps */
+	WRITE_PPS(0xf, SEQ_PARAMETER_SET_ID);
+	WRITE_PPS(0xff, PROFILE_IDC);
+	WRITE_PPS(1, CONSTRAINT_SET3_FLAG);
+	WRITE_PPS(sps->chroma_format_idc, CHROMA_FORMAT_IDC);
+	WRITE_PPS(sps->bit_depth_luma_minus8, BIT_DEPTH_LUMA);
+	WRITE_PPS(sps->bit_depth_chroma_minus8, BIT_DEPTH_CHROMA);
+	WRITE_PPS(0, QPPRIME_Y_ZERO_TRANSFORM_BYPASS_FLAG);
+	WRITE_PPS(sps->log2_max_frame_num_minus4, LOG2_MAX_FRAME_NUM_MINUS4);
+	WRITE_PPS(sps->max_num_ref_frames, MAX_NUM_REF_FRAMES);
+	WRITE_PPS(sps->pic_order_cnt_type, PIC_ORDER_CNT_TYPE);
+	WRITE_PPS(sps->log2_max_pic_order_cnt_lsb_minus4,
+		  LOG2_MAX_PIC_ORDER_CNT_LSB_MINUS4);
+	WRITE_PPS(!!(sps->flags & V4L2_H264_SPS_FLAG_DELTA_PIC_ORDER_ALWAYS_ZERO),
+		  DELTA_PIC_ORDER_ALWAYS_ZERO_FLAG);
+
+	/*
+	 * Use the SPS values since they are already in macroblocks
+	 * dimensions, height can be field height (halved) if
+	 * V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY is not set and also it allows
+	 * decoding smaller images into larger allocation which can be used
+	 * to implementing SVC spatial layer support.
+	 */
+	WRITE_PPS(sps->pic_width_in_mbs_minus1 + 1, PIC_WIDTH_IN_MBS);
+	WRITE_PPS(sps->pic_height_in_map_units_minus1 + 1, PIC_HEIGHT_IN_MBS);
+
+	WRITE_PPS(!!(sps->flags & V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY),
+		  FRAME_MBS_ONLY_FLAG);
+	WRITE_PPS(!!(sps->flags & V4L2_H264_SPS_FLAG_MB_ADAPTIVE_FRAME_FIELD),
+		  MB_ADAPTIVE_FRAME_FIELD_FLAG);
+	WRITE_PPS(!!(sps->flags & V4L2_H264_SPS_FLAG_DIRECT_8X8_INFERENCE),
+		  DIRECT_8X8_INFERENCE_FLAG);
+
+	/* write pps */
+	WRITE_PPS(0xff, PIC_PARAMETER_SET_ID);
+	WRITE_PPS(0x1f, PPS_SEQ_PARAMETER_SET_ID);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_ENTROPY_CODING_MODE),
+		  ENTROPY_CODING_MODE_FLAG);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_BOTTOM_FIELD_PIC_ORDER_IN_FRAME_PRESENT),
+		  BOTTOM_FIELD_PIC_ORDER_IN_FRAME_PRESENT_FLAG);
+	WRITE_PPS(pps->num_ref_idx_l0_default_active_minus1,
+		  NUM_REF_IDX_L_DEFAULT_ACTIVE_MINUS1(0));
+	WRITE_PPS(pps->num_ref_idx_l1_default_active_minus1,
+		  NUM_REF_IDX_L_DEFAULT_ACTIVE_MINUS1(1));
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_WEIGHTED_PRED),
+		  WEIGHTED_PRED_FLAG);
+	WRITE_PPS(pps->weighted_bipred_idc, WEIGHTED_BIPRED_IDC);
+	WRITE_PPS(pps->pic_init_qp_minus26, PIC_INIT_QP_MINUS26);
+	WRITE_PPS(pps->pic_init_qs_minus26, PIC_INIT_QS_MINUS26);
+	WRITE_PPS(pps->chroma_qp_index_offset, CHROMA_QP_INDEX_OFFSET);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_DEBLOCKING_FILTER_CONTROL_PRESENT),
+		  DEBLOCKING_FILTER_CONTROL_PRESENT_FLAG);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_CONSTRAINED_INTRA_PRED),
+		  CONSTRAINED_INTRA_PRED_FLAG);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_REDUNDANT_PIC_CNT_PRESENT),
+		  REDUNDANT_PIC_CNT_PRESENT);
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_TRANSFORM_8X8_MODE),
+		  TRANSFORM_8X8_MODE_FLAG);
+	WRITE_PPS(pps->second_chroma_qp_index_offset,
+		  SECOND_CHROMA_QP_INDEX_OFFSET);
+
+	WRITE_PPS(!!(pps->flags & V4L2_H264_PPS_FLAG_SCALING_MATRIX_PRESENT),
+		  SCALING_LIST_ENABLE_FLAG);
+	/* To be on the safe side, program the scaling matrix address */
+	scaling_distance = offsetof(struct rkvdec_h264_priv_tbl, scaling_list);
+	scaling_list_address = h264_ctx->priv_tbl.dma + scaling_distance;
+	WRITE_PPS(scaling_list_address, SCALING_LIST_ADDRESS);
+
+	for (i = 0; i < ARRAY_SIZE(dec_params->dpb); i++) {
+		u32 is_longterm = 0;
+
+		if (dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_LONG_TERM)
+			is_longterm = 1;
+
+		WRITE_PPS(is_longterm, IS_LONG_TERM(i));
+	}
+}
+
+static void lookup_ref_buf_idx(struct rkvdec_ctx *ctx,
+			       struct rkvdec_h264_run *run)
+{
+	const struct v4l2_ctrl_h264_decode_params *dec_params = run->decode_params;
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(dec_params->dpb); i++) {
+		struct v4l2_m2m_ctx *m2m_ctx = ctx->fh.m2m_ctx;
+		const struct v4l2_h264_dpb_entry *dpb = run->decode_params->dpb;
+		struct vb2_queue *cap_q = &m2m_ctx->cap_q_ctx.q;
+		struct vb2_buffer *buf = NULL;
+
+		if (dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE) {
+			buf = vb2_find_buffer(cap_q, dpb[i].reference_ts);
+			if (!buf)
+				pr_debug("No buffer for reference_ts %llu",
+					 dpb[i].reference_ts);
+		}
+
+		run->ref_buf[i] = buf;
+	}
+}
+
+static void assemble_hw_rps(struct rkvdec_ctx *ctx,
+			    struct v4l2_h264_reflist_builder *builder,
+			    struct rkvdec_h264_run *run)
+{
+	const struct v4l2_ctrl_h264_decode_params *dec_params = run->decode_params;
+	const struct v4l2_h264_dpb_entry *dpb = dec_params->dpb;
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	struct rkvdec_h264_priv_tbl *priv_tbl = h264_ctx->priv_tbl.cpu;
+
+	u32 *hw_rps = priv_tbl->rps;
+	u32 i, j;
+	u16 *p = (u16 *)hw_rps;
+
+	memset(hw_rps, 0, sizeof(priv_tbl->rps));
+
+	/*
+	 * Assign an invalid pic_num if DPB entry at that position is inactive.
+	 * If we assign 0 in that position hardware will treat that as a real
+	 * reference picture with pic_num 0, triggering output picture
+	 * corruption.
+	 */
+	for (i = 0; i < ARRAY_SIZE(dec_params->dpb); i++) {
+		if (!(dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE))
+			continue;
+
+		p[i] = builder->refs[i].frame_num;
+	}
+
+	for (j = 0; j < RKVDEC_NUM_REFLIST; j++) {
+		for (i = 0; i < builder->num_valid; i++) {
+			struct v4l2_h264_reference *ref;
+			bool dpb_valid;
+			bool bottom;
+
+			switch (j) {
+			case 0:
+				ref = &h264_ctx->reflists.p[i];
+				break;
+			case 1:
+				ref = &h264_ctx->reflists.b0[i];
+				break;
+			case 2:
+				ref = &h264_ctx->reflists.b1[i];
+				break;
+			}
+
+			if (WARN_ON(ref->index >= ARRAY_SIZE(dec_params->dpb)))
+				continue;
+
+			dpb_valid = run->ref_buf[ref->index] != NULL;
+			bottom = ref->fields == V4L2_H264_BOTTOM_FIELD_REF;
+
+			set_ps_field(hw_rps, DPB_INFO(i, j),
+				     ref->index | dpb_valid << 4);
+			set_ps_field(hw_rps, BOTTOM_FLAG(i, j), bottom);
+		}
+	}
+}
+
+static void assemble_hw_scaling_list(struct rkvdec_ctx *ctx,
+				     struct rkvdec_h264_run *run)
+{
+	const struct v4l2_ctrl_h264_scaling_matrix *scaling = run->scaling_matrix;
+	const struct v4l2_ctrl_h264_pps *pps = run->pps;
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	struct rkvdec_h264_priv_tbl *tbl = h264_ctx->priv_tbl.cpu;
+
+	if (!(pps->flags & V4L2_H264_PPS_FLAG_SCALING_MATRIX_PRESENT))
+		return;
+
+	BUILD_BUG_ON(sizeof(tbl->scaling_list.scaling_list_4x4) !=
+		     sizeof(scaling->scaling_list_4x4));
+	BUILD_BUG_ON(sizeof(tbl->scaling_list.scaling_list_8x8) !=
+		     sizeof(scaling->scaling_list_8x8));
+
+	memcpy(tbl->scaling_list.scaling_list_4x4,
+	       scaling->scaling_list_4x4,
+	       sizeof(scaling->scaling_list_4x4));
+
+	memcpy(tbl->scaling_list.scaling_list_8x8,
+	       scaling->scaling_list_8x8,
+	       sizeof(scaling->scaling_list_8x8));
+}
+
+/*
+ * dpb poc related registers table
+ */
+static const u32 poc_reg_tbl_top_field[16] = {
+	RKVDEC_REG_H264_POC_REFER0(0),
+	RKVDEC_REG_H264_POC_REFER0(2),
+	RKVDEC_REG_H264_POC_REFER0(4),
+	RKVDEC_REG_H264_POC_REFER0(6),
+	RKVDEC_REG_H264_POC_REFER0(8),
+	RKVDEC_REG_H264_POC_REFER0(10),
+	RKVDEC_REG_H264_POC_REFER0(12),
+	RKVDEC_REG_H264_POC_REFER0(14),
+	RKVDEC_REG_H264_POC_REFER1(1),
+	RKVDEC_REG_H264_POC_REFER1(3),
+	RKVDEC_REG_H264_POC_REFER1(5),
+	RKVDEC_REG_H264_POC_REFER1(7),
+	RKVDEC_REG_H264_POC_REFER1(9),
+	RKVDEC_REG_H264_POC_REFER1(11),
+	RKVDEC_REG_H264_POC_REFER1(13),
+	RKVDEC_REG_H264_POC_REFER2(0)
+};
+
+static const u32 poc_reg_tbl_bottom_field[16] = {
+	RKVDEC_REG_H264_POC_REFER0(1),
+	RKVDEC_REG_H264_POC_REFER0(3),
+	RKVDEC_REG_H264_POC_REFER0(5),
+	RKVDEC_REG_H264_POC_REFER0(7),
+	RKVDEC_REG_H264_POC_REFER0(9),
+	RKVDEC_REG_H264_POC_REFER0(11),
+	RKVDEC_REG_H264_POC_REFER0(13),
+	RKVDEC_REG_H264_POC_REFER1(0),
+	RKVDEC_REG_H264_POC_REFER1(2),
+	RKVDEC_REG_H264_POC_REFER1(4),
+	RKVDEC_REG_H264_POC_REFER1(6),
+	RKVDEC_REG_H264_POC_REFER1(8),
+	RKVDEC_REG_H264_POC_REFER1(10),
+	RKVDEC_REG_H264_POC_REFER1(12),
+	RKVDEC_REG_H264_POC_REFER1(14),
+	RKVDEC_REG_H264_POC_REFER2(1)
+};
+
+static void config_registers(struct rkvdec_ctx *ctx,
+			     struct rkvdec_h264_run *run)
+{
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	const struct v4l2_ctrl_h264_decode_params *dec_params = run->decode_params;
+	const struct v4l2_ctrl_h264_sps *sps = run->sps;
+	const struct v4l2_h264_dpb_entry *dpb = dec_params->dpb;
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	dma_addr_t priv_start_addr = h264_ctx->priv_tbl.dma;
+	const struct v4l2_pix_format_mplane *dst_fmt;
+	struct vb2_v4l2_buffer *src_buf = run->base.bufs.src;
+	struct vb2_v4l2_buffer *dst_buf = run->base.bufs.dst;
+	const struct v4l2_format *f;
+	dma_addr_t rlc_addr;
+	dma_addr_t refer_addr;
+	u32 rlc_len;
+	u32 hor_virstride = 0;
+	u32 ver_virstride = 0;
+	u32 y_virstride = 0;
+	u32 yuv_virstride = 0;
+	u32 offset;
+	dma_addr_t dst_addr;
+	u32 reg, i;
+
+	reg = RKVDEC_MODE(RKVDEC_MODE_H264);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_SYSCTRL);
+
+	f = &ctx->decoded_fmt;
+	dst_fmt = &f->fmt.pix_mp;
+	hor_virstride = (sps->bit_depth_luma_minus8 + 8) * dst_fmt->width / 8;
+	ver_virstride = round_up(dst_fmt->height, 16);
+	y_virstride = hor_virstride * ver_virstride;
+
+	if (sps->chroma_format_idc == 0)
+		yuv_virstride = y_virstride;
+	else if (sps->chroma_format_idc == 1)
+		yuv_virstride += y_virstride + y_virstride / 2;
+	else if (sps->chroma_format_idc == 2)
+		yuv_virstride += 2 * y_virstride;
+
+	reg = RKVDEC_Y_HOR_VIRSTRIDE(hor_virstride / 16) |
+	      RKVDEC_UV_HOR_VIRSTRIDE(hor_virstride / 16) |
+	      RKVDEC_SLICE_NUM_HIGHBIT |
+	      RKVDEC_SLICE_NUM_LOWBITS(0x7ff);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_PICPAR);
+
+	/* config rlc base address */
+	rlc_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+	writel_relaxed(rlc_addr, rkvdec->regs + RKVDEC_REG_STRM_RLC_BASE);
+	writel_relaxed(rlc_addr, rkvdec->regs + RKVDEC_REG_RLCWRITE_BASE);
+
+	rlc_len = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
+	reg = RKVDEC_STRM_LEN(rlc_len);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_STRM_LEN);
+
+	/* config cabac table */
+	offset = offsetof(struct rkvdec_h264_priv_tbl, cabac_table);
+	writel_relaxed(priv_start_addr + offset,
+		       rkvdec->regs + RKVDEC_REG_CABACTBL_PROB_BASE);
+
+	/* config output base address */
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+	writel_relaxed(dst_addr, rkvdec->regs + RKVDEC_REG_DECOUT_BASE);
+
+	reg = RKVDEC_Y_VIRSTRIDE(y_virstride / 16);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_Y_VIRSTRIDE);
+
+	reg = RKVDEC_YUV_VIRSTRIDE(yuv_virstride / 16);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_YUV_VIRSTRIDE);
+
+	/* config ref pic address & poc */
+	for (i = 0; i < ARRAY_SIZE(dec_params->dpb); i++) {
+		struct vb2_buffer *vb_buf = run->ref_buf[i];
+
+		/*
+		 * If a DPB entry is unused or invalid, address of current destination
+		 * buffer is returned.
+		 */
+		if (!vb_buf)
+			vb_buf = &dst_buf->vb2_buf;
+		refer_addr = vb2_dma_contig_plane_dma_addr(vb_buf, 0);
+
+		if (dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE)
+			refer_addr |= RKVDEC_COLMV_USED_FLAG_REF;
+		if (dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_FIELD)
+			refer_addr |= RKVDEC_FIELD_REF;
+
+		if (dpb[i].fields & V4L2_H264_TOP_FIELD_REF)
+			refer_addr |= RKVDEC_TOPFIELD_USED_REF;
+		if (dpb[i].fields & V4L2_H264_BOTTOM_FIELD_REF)
+			refer_addr |= RKVDEC_BOTFIELD_USED_REF;
+
+		writel_relaxed(dpb[i].top_field_order_cnt,
+			       rkvdec->regs +  poc_reg_tbl_top_field[i]);
+		writel_relaxed(dpb[i].bottom_field_order_cnt,
+			       rkvdec->regs + poc_reg_tbl_bottom_field[i]);
+
+		if (i < V4L2_H264_NUM_DPB_ENTRIES - 1)
+			writel_relaxed(refer_addr,
+				       rkvdec->regs + RKVDEC_REG_H264_BASE_REFER(i));
+		else
+			writel_relaxed(refer_addr,
+				       rkvdec->regs + RKVDEC_REG_H264_BASE_REFER15);
+	}
+
+	reg = RKVDEC_CUR_POC(dec_params->top_field_order_cnt);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_CUR_POC0);
+
+	reg = RKVDEC_CUR_POC(dec_params->bottom_field_order_cnt);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_CUR_POC1);
+
+	/* config hw pps address */
+	offset = offsetof(struct rkvdec_h264_priv_tbl, param_set);
+	writel_relaxed(priv_start_addr + offset,
+		       rkvdec->regs + RKVDEC_REG_PPS_BASE);
+
+	/* config hw rps address */
+	offset = offsetof(struct rkvdec_h264_priv_tbl, rps);
+	writel_relaxed(priv_start_addr + offset,
+		       rkvdec->regs + RKVDEC_REG_RPS_BASE);
+
+	reg = RKVDEC_AXI_DDR_RDATA(0);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_AXI_DDR_RDATA);
+
+	reg = RKVDEC_AXI_DDR_WDATA(0);
+	writel_relaxed(reg, rkvdec->regs + RKVDEC_REG_AXI_DDR_WDATA);
+
+	offset = offsetof(struct rkvdec_h264_priv_tbl, err_info);
+	writel_relaxed(priv_start_addr + offset,
+		       rkvdec->regs + RKVDEC_REG_H264_ERRINFO_BASE);
+}
+
+#define RKVDEC_H264_MAX_DEPTH_IN_BYTES		2
+
+static int rkvdec_h264_adjust_fmt(struct rkvdec_ctx *ctx,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *fmt = &f->fmt.pix_mp;
+
+	fmt->num_planes = 1;
+	if (!fmt->plane_fmt[0].sizeimage)
+		fmt->plane_fmt[0].sizeimage = fmt->width * fmt->height *
+					      RKVDEC_H264_MAX_DEPTH_IN_BYTES;
+	return 0;
+}
+
+static int rkvdec_h264_validate_sps(struct rkvdec_ctx *ctx,
+				    const struct v4l2_ctrl_h264_sps *sps)
+{
+	unsigned int width, height;
+
+	/*
+	 * TODO: The hardware supports 10-bit and 4:2:2 profiles,
+	 * but it's currently broken in the driver.
+	 * Reject them for now, until it's fixed.
+	 */
+	if (sps->chroma_format_idc > 1)
+		/* Only 4:0:0 and 4:2:0 are supported */
+		return -EINVAL;
+	if (sps->bit_depth_luma_minus8 != sps->bit_depth_chroma_minus8)
+		/* Luma and chroma bit depth mismatch */
+		return -EINVAL;
+	if (sps->bit_depth_luma_minus8 != 0)
+		/* Only 8-bit is supported */
+		return -EINVAL;
+
+	width = (sps->pic_width_in_mbs_minus1 + 1) * 16;
+	height = (sps->pic_height_in_map_units_minus1 + 1) * 16;
+
+	/*
+	 * When frame_mbs_only_flag is not set, this is field height,
+	 * which is half the final height (see (7-18) in the
+	 * specification)
+	 */
+	if (!(sps->flags & V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY))
+		height *= 2;
+
+	if (width > ctx->coded_fmt.fmt.pix_mp.width ||
+	    height > ctx->coded_fmt.fmt.pix_mp.height)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int rkvdec_h264_start(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	struct rkvdec_h264_priv_tbl *priv_tbl;
+	struct rkvdec_h264_ctx *h264_ctx;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_H264_SPS);
+	if (!ctrl)
+		return -EINVAL;
+
+	ret = rkvdec_h264_validate_sps(ctx, ctrl->p_new.p_h264_sps);
+	if (ret)
+		return ret;
+
+	h264_ctx = kzalloc(sizeof(*h264_ctx), GFP_KERNEL);
+	if (!h264_ctx)
+		return -ENOMEM;
+
+	priv_tbl = dma_alloc_coherent(rkvdec->dev, sizeof(*priv_tbl),
+				      &h264_ctx->priv_tbl.dma, GFP_KERNEL);
+	if (!priv_tbl) {
+		ret = -ENOMEM;
+		goto err_free_ctx;
+	}
+
+	h264_ctx->priv_tbl.size = sizeof(*priv_tbl);
+	h264_ctx->priv_tbl.cpu = priv_tbl;
+	memcpy(priv_tbl->cabac_table, rkvdec_h264_cabac_table,
+	       sizeof(rkvdec_h264_cabac_table));
+
+	ctx->priv = h264_ctx;
+	return 0;
+
+err_free_ctx:
+	kfree(h264_ctx);
+	return ret;
+}
+
+static void rkvdec_h264_stop(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+
+	dma_free_coherent(rkvdec->dev, h264_ctx->priv_tbl.size,
+			  h264_ctx->priv_tbl.cpu, h264_ctx->priv_tbl.dma);
+	kfree(h264_ctx);
+}
+
+static void rkvdec_h264_run_preamble(struct rkvdec_ctx *ctx,
+				     struct rkvdec_h264_run *run)
+{
+	struct v4l2_ctrl *ctrl;
+
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+	run->decode_params = ctrl ? ctrl->p_cur.p : NULL;
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_H264_SPS);
+	run->sps = ctrl ? ctrl->p_cur.p : NULL;
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_H264_PPS);
+	run->pps = ctrl ? ctrl->p_cur.p : NULL;
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_H264_SCALING_MATRIX);
+	run->scaling_matrix = ctrl ? ctrl->p_cur.p : NULL;
+
+	rkvdec_run_preamble(ctx, &run->base);
+}
+
+static int rkvdec_h264_run(struct rkvdec_ctx *ctx)
+{
+	struct v4l2_h264_reflist_builder reflist_builder;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	struct rkvdec_h264_ctx *h264_ctx = ctx->priv;
+	struct rkvdec_h264_run run;
+
+	rkvdec_h264_run_preamble(ctx, &run);
+
+	/* Build the P/B{0,1} ref lists. */
+	v4l2_h264_init_reflist_builder(&reflist_builder, run.decode_params,
+				       run.sps, run.decode_params->dpb);
+	v4l2_h264_build_p_ref_list(&reflist_builder, h264_ctx->reflists.p);
+	v4l2_h264_build_b_ref_lists(&reflist_builder, h264_ctx->reflists.b0,
+				    h264_ctx->reflists.b1);
+
+	assemble_hw_scaling_list(ctx, &run);
+	assemble_hw_pps(ctx, &run);
+	lookup_ref_buf_idx(ctx, &run);
+	assemble_hw_rps(ctx, &reflist_builder, &run);
+	config_registers(ctx, &run);
+
+	rkvdec_run_postamble(ctx, &run.base);
+
+	schedule_delayed_work(&rkvdec->watchdog_work, msecs_to_jiffies(2000));
+
+	writel(0, rkvdec->regs + RKVDEC_REG_STRMD_ERR_EN);
+	writel(0, rkvdec->regs + RKVDEC_REG_H264_ERR_E);
+	writel(1, rkvdec->regs + RKVDEC_REG_PREF_LUMA_CACHE_COMMAND);
+	writel(1, rkvdec->regs + RKVDEC_REG_PREF_CHR_CACHE_COMMAND);
+
+	/* Start decoding! */
+	writel(RKVDEC_INTERRUPT_DEC_E | RKVDEC_CONFIG_DEC_CLK_GATE_E |
+	       RKVDEC_TIMEOUT_E | RKVDEC_BUF_EMPTY_E,
+	       rkvdec->regs + RKVDEC_REG_INTERRUPT);
+
+	return 0;
+}
+
+static int rkvdec_h264_try_ctrl(struct rkvdec_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->id == V4L2_CID_STATELESS_H264_SPS)
+		return rkvdec_h264_validate_sps(ctx, ctrl->p_new.p_h264_sps);
+
+	return 0;
+}
+
+const struct rkvdec_coded_fmt_ops rkvdec_h264_fmt_ops = {
+	.adjust_fmt = rkvdec_h264_adjust_fmt,
+	.start = rkvdec_h264_start,
+	.stop = rkvdec_h264_stop,
+	.run = rkvdec_h264_run,
+	.try_ctrl = rkvdec_h264_try_ctrl,
+};
diff --git a/drivers/staging/media/rkvdec/rkvdec-regs.h b/drivers/staging/media/rkvdec/rkvdec-regs.h
new file mode 100644
index 000000000..15b9bee92
--- /dev/null
+++ b/drivers/staging/media/rkvdec/rkvdec-regs.h
@@ -0,0 +1,223 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef RKVDEC_REGS_H_
+#define RKVDEC_REGS_H_
+
+/* rkvcodec registers */
+#define RKVDEC_REG_INTERRUPT				0x004
+#define RKVDEC_INTERRUPT_DEC_E				BIT(0)
+#define RKVDEC_CONFIG_DEC_CLK_GATE_E			BIT(1)
+#define RKVDEC_E_STRMD_CLKGATE_DIS			BIT(2)
+#define RKVDEC_TIMEOUT_MODE				BIT(3)
+#define RKVDEC_IRQ_DIS					BIT(4)
+#define RKVDEC_TIMEOUT_E				BIT(5)
+#define RKVDEC_BUF_EMPTY_E				BIT(6)
+#define RKVDEC_STRM_E_WAITDECFIFO_EMPTY			BIT(7)
+#define RKVDEC_IRQ					BIT(8)
+#define RKVDEC_IRQ_RAW					BIT(9)
+#define RKVDEC_E_REWRITE_VALID				BIT(10)
+#define RKVDEC_COMMONIRQ_MODE				BIT(11)
+#define RKVDEC_RDY_STA					BIT(12)
+#define RKVDEC_BUS_STA					BIT(13)
+#define RKVDEC_ERR_STA					BIT(14)
+#define RKVDEC_TIMEOUT_STA				BIT(15)
+#define RKVDEC_BUF_EMPTY_STA				BIT(16)
+#define RKVDEC_COLMV_REF_ERR_STA			BIT(17)
+#define RKVDEC_CABU_END_STA				BIT(18)
+#define RKVDEC_H264ORVP9_ERR_MODE			BIT(19)
+#define RKVDEC_SOFTRST_EN_P				BIT(20)
+#define RKVDEC_FORCE_SOFTRESET_VALID			BIT(21)
+#define RKVDEC_SOFTRESET_RDY				BIT(22)
+
+#define RKVDEC_REG_SYSCTRL				0x008
+#define RKVDEC_IN_ENDIAN				BIT(0)
+#define RKVDEC_IN_SWAP32_E				BIT(1)
+#define RKVDEC_IN_SWAP64_E				BIT(2)
+#define RKVDEC_STR_ENDIAN				BIT(3)
+#define RKVDEC_STR_SWAP32_E				BIT(4)
+#define RKVDEC_STR_SWAP64_E				BIT(5)
+#define RKVDEC_OUT_ENDIAN				BIT(6)
+#define RKVDEC_OUT_SWAP32_E				BIT(7)
+#define RKVDEC_OUT_CBCR_SWAP				BIT(8)
+#define RKVDEC_RLC_MODE_DIRECT_WRITE			BIT(10)
+#define RKVDEC_RLC_MODE					BIT(11)
+#define RKVDEC_STRM_START_BIT(x)			(((x) & 0x7f) << 12)
+#define RKVDEC_MODE(x)					(((x) & 0x03) << 20)
+#define RKVDEC_MODE_H264				1
+#define RKVDEC_MODE_VP9					2
+#define RKVDEC_RPS_MODE					BIT(24)
+#define RKVDEC_STRM_MODE				BIT(25)
+#define RKVDEC_H264_STRM_LASTPKT			BIT(26)
+#define RKVDEC_H264_FIRSTSLICE_FLAG			BIT(27)
+#define RKVDEC_H264_FRAME_ORSLICE			BIT(28)
+#define RKVDEC_BUSPR_SLOT_DIS				BIT(29)
+
+#define RKVDEC_REG_PICPAR				0x00C
+#define RKVDEC_Y_HOR_VIRSTRIDE(x)			((x) & 0x1ff)
+#define RKVDEC_SLICE_NUM_HIGHBIT			BIT(11)
+#define RKVDEC_UV_HOR_VIRSTRIDE(x)			(((x) & 0x1ff) << 12)
+#define RKVDEC_SLICE_NUM_LOWBITS(x)			(((x) & 0x7ff) << 21)
+
+#define RKVDEC_REG_STRM_RLC_BASE			0x010
+
+#define RKVDEC_REG_STRM_LEN				0x014
+#define RKVDEC_STRM_LEN(x)				((x) & 0x7ffffff)
+
+#define RKVDEC_REG_CABACTBL_PROB_BASE			0x018
+#define RKVDEC_REG_DECOUT_BASE				0x01C
+
+#define RKVDEC_REG_Y_VIRSTRIDE				0x020
+#define RKVDEC_Y_VIRSTRIDE(x)				((x) & 0xfffff)
+
+#define RKVDEC_REG_YUV_VIRSTRIDE			0x024
+#define RKVDEC_YUV_VIRSTRIDE(x)				((x) & 0x1fffff)
+#define RKVDEC_REG_H264_BASE_REFER(i)			(((i) * 0x04) + 0x028)
+
+#define RKVDEC_REG_H264_BASE_REFER15			0x0C0
+#define RKVDEC_FIELD_REF				BIT(0)
+#define RKVDEC_TOPFIELD_USED_REF			BIT(1)
+#define RKVDEC_BOTFIELD_USED_REF			BIT(2)
+#define RKVDEC_COLMV_USED_FLAG_REF			BIT(3)
+
+#define RKVDEC_REG_VP9_LAST_FRAME_BASE			0x02c
+#define RKVDEC_REG_VP9_GOLDEN_FRAME_BASE		0x030
+#define RKVDEC_REG_VP9_ALTREF_FRAME_BASE		0x034
+
+#define RKVDEC_REG_VP9_CPRHEADER_OFFSET			0x028
+#define RKVDEC_VP9_CPRHEADER_OFFSET(x)			((x) & 0xffff)
+
+#define RKVDEC_REG_VP9_REFERLAST_BASE			0x02C
+#define RKVDEC_REG_VP9_REFERGOLDEN_BASE			0x030
+#define RKVDEC_REG_VP9_REFERALFTER_BASE			0x034
+
+#define RKVDEC_REG_VP9COUNT_BASE			0x038
+#define RKVDEC_VP9COUNT_UPDATE_EN			BIT(0)
+
+#define RKVDEC_REG_VP9_SEGIDLAST_BASE			0x03C
+#define RKVDEC_REG_VP9_SEGIDCUR_BASE			0x040
+#define RKVDEC_REG_VP9_FRAME_SIZE(i)			((i) * 0x04 + 0x044)
+#define RKVDEC_VP9_FRAMEWIDTH(x)			(((x) & 0xffff) << 0)
+#define RKVDEC_VP9_FRAMEHEIGHT(x)			(((x) & 0xffff) << 16)
+
+#define RKVDEC_VP9_SEGID_GRP(i)				((i) * 0x04 + 0x050)
+#define RKVDEC_SEGID_ABS_DELTA(x)			((x) & 0x1)
+#define RKVDEC_SEGID_FRAME_QP_DELTA_EN(x)		(((x) & 0x1) << 1)
+#define RKVDEC_SEGID_FRAME_QP_DELTA(x)			(((x) & 0x1ff) << 2)
+#define RKVDEC_SEGID_FRAME_LOOPFILTER_VALUE_EN(x)	(((x) & 0x1) << 11)
+#define RKVDEC_SEGID_FRAME_LOOPFILTER_VALUE(x)		(((x) & 0x7f) << 12)
+#define RKVDEC_SEGID_REFERINFO_EN(x)			(((x) & 0x1) << 19)
+#define RKVDEC_SEGID_REFERINFO(x)			(((x) & 0x03) << 20)
+#define RKVDEC_SEGID_FRAME_SKIP_EN(x)			(((x) & 0x1) << 22)
+
+#define RKVDEC_VP9_CPRHEADER_CONFIG			0x070
+#define RKVDEC_VP9_TX_MODE(x)				((x) & 0x07)
+#define RKVDEC_VP9_FRAME_REF_MODE(x)			(((x) & 0x03) << 3)
+
+#define RKVDEC_VP9_REF_SCALE(i)				((i) * 0x04 + 0x074)
+#define RKVDEC_VP9_REF_HOR_SCALE(x)			((x) & 0xffff)
+#define RKVDEC_VP9_REF_VER_SCALE(x)			(((x) & 0xffff) << 16)
+
+#define RKVDEC_VP9_REF_DELTAS_LASTFRAME			0x080
+#define RKVDEC_REF_DELTAS_LASTFRAME(pos, val)		(((val) & 0x7f) << ((pos) * 7))
+
+#define RKVDEC_VP9_INFO_LASTFRAME			0x084
+#define RKVDEC_MODE_DELTAS_LASTFRAME(pos, val)		(((val) & 0x7f) << ((pos) * 7))
+#define RKVDEC_SEG_EN_LASTFRAME				BIT(16)
+#define RKVDEC_LAST_SHOW_FRAME				BIT(17)
+#define RKVDEC_LAST_INTRA_ONLY				BIT(18)
+#define RKVDEC_LAST_WIDHHEIGHT_EQCUR			BIT(19)
+#define RKVDEC_COLOR_SPACE_LASTKEYFRAME(x)		(((x) & 0x07) << 20)
+
+#define RKVDEC_VP9_INTERCMD_BASE			0x088
+
+#define RKVDEC_VP9_INTERCMD_NUM				0x08C
+#define RKVDEC_INTERCMD_NUM(x)				((x) & 0xffffff)
+
+#define RKVDEC_VP9_LASTTILE_SIZE			0x090
+#define RKVDEC_LASTTILE_SIZE(x)				((x) & 0xffffff)
+
+#define RKVDEC_VP9_HOR_VIRSTRIDE(i)			((i) * 0x04 + 0x094)
+#define RKVDEC_HOR_Y_VIRSTRIDE(x)			((x) & 0x1ff)
+#define RKVDEC_HOR_UV_VIRSTRIDE(x)			(((x) & 0x1ff) << 16)
+
+#define RKVDEC_REG_H264_POC_REFER0(i)			(((i) * 0x04) + 0x064)
+#define RKVDEC_REG_H264_POC_REFER1(i)			(((i) * 0x04) + 0x0C4)
+#define RKVDEC_REG_H264_POC_REFER2(i)			(((i) * 0x04) + 0x120)
+#define RKVDEC_POC_REFER(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_CUR_POC0				0x0A0
+#define RKVDEC_REG_CUR_POC1				0x128
+#define RKVDEC_CUR_POC(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_RLCWRITE_BASE			0x0A4
+#define RKVDEC_REG_PPS_BASE				0x0A8
+#define RKVDEC_REG_RPS_BASE				0x0AC
+
+#define RKVDEC_REG_STRMD_ERR_EN				0x0B0
+#define RKVDEC_STRMD_ERR_EN(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_STRMD_ERR_STA			0x0B4
+#define RKVDEC_STRMD_ERR_STA(x)				((x) & 0xfffffff)
+#define RKVDEC_COLMV_ERR_REF_PICIDX(x)			(((x) & 0x0f) << 28)
+
+#define RKVDEC_REG_STRMD_ERR_CTU			0x0B8
+#define RKVDEC_STRMD_ERR_CTU(x)				((x) & 0xff)
+#define RKVDEC_STRMD_ERR_CTU_YOFFSET(x)			(((x) & 0xff) << 8)
+#define RKVDEC_STRMFIFO_SPACE2FULL(x)			(((x) & 0x7f) << 16)
+#define RKVDEC_VP9_ERR_EN_CTU0				BIT(24)
+
+#define RKVDEC_REG_SAO_CTU_POS				0x0BC
+#define RKVDEC_SAOWR_XOFFSET(x)				((x) & 0x1ff)
+#define RKVDEC_SAOWR_YOFFSET(x)				(((x) & 0x3ff) << 16)
+
+#define RKVDEC_VP9_LAST_FRAME_YSTRIDE			0x0C0
+#define RKVDEC_VP9_GOLDEN_FRAME_YSTRIDE			0x0C4
+#define RKVDEC_VP9_ALTREF_FRAME_YSTRIDE			0x0C8
+#define RKVDEC_VP9_REF_YSTRIDE(x)			(((x) & 0xfffff) << 0)
+
+#define RKVDEC_VP9_LAST_FRAME_YUVSTRIDE			0x0CC
+#define RKVDEC_VP9_REF_YUVSTRIDE(x)			(((x) & 0x1fffff) << 0)
+
+#define RKVDEC_VP9_REF_COLMV_BASE			0x0D0
+
+#define RKVDEC_REG_PERFORMANCE_CYCLE			0x100
+#define RKVDEC_PERFORMANCE_CYCLE(x)			((x) & 0xffffffff)
+
+#define RKVDEC_REG_AXI_DDR_RDATA			0x104
+#define RKVDEC_AXI_DDR_RDATA(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_AXI_DDR_WDATA			0x108
+#define RKVDEC_AXI_DDR_WDATA(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_FPGADEBUG_RESET			0x10C
+#define RKVDEC_BUSIFD_RESETN				BIT(0)
+#define RKVDEC_CABAC_RESETN				BIT(1)
+#define RKVDEC_DEC_CTRL_RESETN				BIT(2)
+#define RKVDEC_TRANSD_RESETN				BIT(3)
+#define RKVDEC_INTRA_RESETN				BIT(4)
+#define RKVDEC_INTER_RESETN				BIT(5)
+#define RKVDEC_RECON_RESETN				BIT(6)
+#define RKVDEC_FILER_RESETN				BIT(7)
+
+#define RKVDEC_REG_PERFORMANCE_SEL			0x110
+#define RKVDEC_PERF_SEL_CNT0(x)				((x) & 0x3f)
+#define RKVDEC_PERF_SEL_CNT1(x)				(((x) & 0x3f) << 8)
+#define RKVDEC_PERF_SEL_CNT2(x)				(((x) & 0x3f) << 16)
+
+#define RKVDEC_REG_PERFORMANCE_CNT(i)			((i) * 0x04 + 0x114)
+#define RKVDEC_PERF_CNT(x)				((x) & 0xffffffff)
+
+#define RKVDEC_REG_H264_ERRINFO_BASE			0x12C
+
+#define RKVDEC_REG_H264_ERRINFO_NUM			0x130
+#define RKVDEC_SLICEDEC_NUM(x)				((x) & 0x3fff)
+#define RKVDEC_STRMD_DECT_ERR_FLAG			BIT(15)
+#define RKVDEC_ERR_PKT_NUM(x)				(((x) & 0x3fff) << 16)
+
+#define RKVDEC_REG_H264_ERR_E				0x134
+#define RKVDEC_H264_ERR_EN_HIGHBITS(x)			((x) & 0x3fffffff)
+
+#define RKVDEC_REG_PREF_LUMA_CACHE_COMMAND		0x410
+#define RKVDEC_REG_PREF_CHR_CACHE_COMMAND		0x450
+
+#endif /* RKVDEC_REGS_H_ */
diff --git a/drivers/staging/media/rkvdec/rkvdec-vp9.c b/drivers/staging/media/rkvdec/rkvdec-vp9.c
new file mode 100644
index 000000000..cfae99b40
--- /dev/null
+++ b/drivers/staging/media/rkvdec/rkvdec-vp9.c
@@ -0,0 +1,1073 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Rockchip Video Decoder VP9 backend
+ *
+ * Copyright (C) 2019 Collabora, Ltd.
+ *	Boris Brezillon <boris.brezillon@collabora.com>
+ * Copyright (C) 2021 Collabora, Ltd.
+ *	Andrzej Pietrasiewicz <andrzej.p@collabora.com>
+ *
+ * Copyright (C) 2016 Rockchip Electronics Co., Ltd.
+ *	Alpha Lin <Alpha.Lin@rock-chips.com>
+ */
+
+/*
+ * For following the vp9 spec please start reading this driver
+ * code from rkvdec_vp9_run() followed by rkvdec_vp9_done().
+ */
+
+#include <linux/kernel.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-vp9.h>
+
+#include "rkvdec.h"
+#include "rkvdec-regs.h"
+
+#define RKVDEC_VP9_PROBE_SIZE		4864
+#define RKVDEC_VP9_COUNT_SIZE		13232
+#define RKVDEC_VP9_MAX_SEGMAP_SIZE	73728
+
+struct rkvdec_vp9_intra_mode_probs {
+	u8 y_mode[105];
+	u8 uv_mode[23];
+};
+
+struct rkvdec_vp9_intra_only_frame_probs {
+	u8 coef_intra[4][2][128];
+	struct rkvdec_vp9_intra_mode_probs intra_mode[10];
+};
+
+struct rkvdec_vp9_inter_frame_probs {
+	u8 y_mode[4][9];
+	u8 comp_mode[5];
+	u8 comp_ref[5];
+	u8 single_ref[5][2];
+	u8 inter_mode[7][3];
+	u8 interp_filter[4][2];
+	u8 padding0[11];
+	u8 coef[2][4][2][128];
+	u8 uv_mode_0_2[3][9];
+	u8 padding1[5];
+	u8 uv_mode_3_5[3][9];
+	u8 padding2[5];
+	u8 uv_mode_6_8[3][9];
+	u8 padding3[5];
+	u8 uv_mode_9[9];
+	u8 padding4[7];
+	u8 padding5[16];
+	struct {
+		u8 joint[3];
+		u8 sign[2];
+		u8 classes[2][10];
+		u8 class0_bit[2];
+		u8 bits[2][10];
+		u8 class0_fr[2][2][3];
+		u8 fr[2][3];
+		u8 class0_hp[2];
+		u8 hp[2];
+	} mv;
+};
+
+struct rkvdec_vp9_probs {
+	u8 partition[16][3];
+	u8 pred[3];
+	u8 tree[7];
+	u8 skip[3];
+	u8 tx32[2][3];
+	u8 tx16[2][2];
+	u8 tx8[2][1];
+	u8 is_inter[4];
+	/* 128 bit alignment */
+	u8 padding0[3];
+	union {
+		struct rkvdec_vp9_inter_frame_probs inter;
+		struct rkvdec_vp9_intra_only_frame_probs intra_only;
+	};
+	/* 128 bit alignment */
+	u8 padding1[11];
+};
+
+/* Data structure describing auxiliary buffer format. */
+struct rkvdec_vp9_priv_tbl {
+	struct rkvdec_vp9_probs probs;
+	u8 segmap[2][RKVDEC_VP9_MAX_SEGMAP_SIZE];
+};
+
+struct rkvdec_vp9_refs_counts {
+	u32 eob[2];
+	u32 coeff[3];
+};
+
+struct rkvdec_vp9_inter_frame_symbol_counts {
+	u32 partition[16][4];
+	u32 skip[3][2];
+	u32 inter[4][2];
+	u32 tx32p[2][4];
+	u32 tx16p[2][4];
+	u32 tx8p[2][2];
+	u32 y_mode[4][10];
+	u32 uv_mode[10][10];
+	u32 comp[5][2];
+	u32 comp_ref[5][2];
+	u32 single_ref[5][2][2];
+	u32 mv_mode[7][4];
+	u32 filter[4][3];
+	u32 mv_joint[4];
+	u32 sign[2][2];
+	/* add 1 element for align */
+	u32 classes[2][11 + 1];
+	u32 class0[2][2];
+	u32 bits[2][10][2];
+	u32 class0_fp[2][2][4];
+	u32 fp[2][4];
+	u32 class0_hp[2][2];
+	u32 hp[2][2];
+	struct rkvdec_vp9_refs_counts ref_cnt[2][4][2][6][6];
+};
+
+struct rkvdec_vp9_intra_frame_symbol_counts {
+	u32 partition[4][4][4];
+	u32 skip[3][2];
+	u32 intra[4][2];
+	u32 tx32p[2][4];
+	u32 tx16p[2][4];
+	u32 tx8p[2][2];
+	struct rkvdec_vp9_refs_counts ref_cnt[2][4][2][6][6];
+};
+
+struct rkvdec_vp9_run {
+	struct rkvdec_run base;
+	const struct v4l2_ctrl_vp9_frame *decode_params;
+};
+
+struct rkvdec_vp9_frame_info {
+	u32 valid : 1;
+	u32 segmapid : 1;
+	u32 frame_context_idx : 2;
+	u32 reference_mode : 2;
+	u32 tx_mode : 3;
+	u32 interpolation_filter : 3;
+	u32 flags;
+	u64 timestamp;
+	struct v4l2_vp9_segmentation seg;
+	struct v4l2_vp9_loop_filter lf;
+};
+
+struct rkvdec_vp9_ctx {
+	struct rkvdec_aux_buf priv_tbl;
+	struct rkvdec_aux_buf count_tbl;
+	struct v4l2_vp9_frame_symbol_counts inter_cnts;
+	struct v4l2_vp9_frame_symbol_counts intra_cnts;
+	struct v4l2_vp9_frame_context probability_tables;
+	struct v4l2_vp9_frame_context frame_context[4];
+	struct rkvdec_vp9_frame_info cur;
+	struct rkvdec_vp9_frame_info last;
+};
+
+static void write_coeff_plane(const u8 coef[6][6][3], u8 *coeff_plane)
+{
+	unsigned int idx = 0, byte_count = 0;
+	int k, m, n;
+	u8 p;
+
+	for (k = 0; k < 6; k++) {
+		for (m = 0; m < 6; m++) {
+			for (n = 0; n < 3; n++) {
+				p = coef[k][m][n];
+				coeff_plane[idx++] = p;
+				byte_count++;
+				if (byte_count == 27) {
+					idx += 5;
+					byte_count = 0;
+				}
+			}
+		}
+	}
+}
+
+static void init_intra_only_probs(struct rkvdec_ctx *ctx,
+				  const struct rkvdec_vp9_run *run)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct rkvdec_vp9_priv_tbl *tbl = vp9_ctx->priv_tbl.cpu;
+	struct rkvdec_vp9_intra_only_frame_probs *rkprobs;
+	const struct v4l2_vp9_frame_context *probs;
+	unsigned int i, j, k;
+
+	rkprobs = &tbl->probs.intra_only;
+	probs = &vp9_ctx->probability_tables;
+
+	/*
+	 * intra only 149 x 128 bits ,aligned to 152 x 128 bits coeff related
+	 * prob 64 x 128 bits
+	 */
+	for (i = 0; i < ARRAY_SIZE(probs->coef); i++) {
+		for (j = 0; j < ARRAY_SIZE(probs->coef[0]); j++)
+			write_coeff_plane(probs->coef[i][j][0],
+					  rkprobs->coef_intra[i][j]);
+	}
+
+	/* intra mode prob  80 x 128 bits */
+	for (i = 0; i < ARRAY_SIZE(v4l2_vp9_kf_y_mode_prob); i++) {
+		unsigned int byte_count = 0;
+		int idx = 0;
+
+		/* vp9_kf_y_mode_prob */
+		for (j = 0; j < ARRAY_SIZE(v4l2_vp9_kf_y_mode_prob[0]); j++) {
+			for (k = 0; k < ARRAY_SIZE(v4l2_vp9_kf_y_mode_prob[0][0]);
+			     k++) {
+				u8 val = v4l2_vp9_kf_y_mode_prob[i][j][k];
+
+				rkprobs->intra_mode[i].y_mode[idx++] = val;
+				byte_count++;
+				if (byte_count == 27) {
+					byte_count = 0;
+					idx += 5;
+				}
+			}
+		}
+
+	}
+
+	for (i = 0; i < sizeof(v4l2_vp9_kf_uv_mode_prob); ++i) {
+		const u8 *ptr = (const u8 *)v4l2_vp9_kf_uv_mode_prob;
+
+		rkprobs->intra_mode[i / 23].uv_mode[i % 23] = ptr[i];
+	}
+}
+
+static void init_inter_probs(struct rkvdec_ctx *ctx,
+			     const struct rkvdec_vp9_run *run)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct rkvdec_vp9_priv_tbl *tbl = vp9_ctx->priv_tbl.cpu;
+	struct rkvdec_vp9_inter_frame_probs *rkprobs;
+	const struct v4l2_vp9_frame_context *probs;
+	unsigned int i, j, k;
+
+	rkprobs = &tbl->probs.inter;
+	probs = &vp9_ctx->probability_tables;
+
+	/*
+	 * inter probs
+	 * 151 x 128 bits, aligned to 152 x 128 bits
+	 * inter only
+	 * intra_y_mode & inter_block info 6 x 128 bits
+	 */
+
+	memcpy(rkprobs->y_mode, probs->y_mode, sizeof(rkprobs->y_mode));
+	memcpy(rkprobs->comp_mode, probs->comp_mode,
+	       sizeof(rkprobs->comp_mode));
+	memcpy(rkprobs->comp_ref, probs->comp_ref,
+	       sizeof(rkprobs->comp_ref));
+	memcpy(rkprobs->single_ref, probs->single_ref,
+	       sizeof(rkprobs->single_ref));
+	memcpy(rkprobs->inter_mode, probs->inter_mode,
+	       sizeof(rkprobs->inter_mode));
+	memcpy(rkprobs->interp_filter, probs->interp_filter,
+	       sizeof(rkprobs->interp_filter));
+
+	/* 128 x 128 bits coeff related */
+	for (i = 0; i < ARRAY_SIZE(probs->coef); i++) {
+		for (j = 0; j < ARRAY_SIZE(probs->coef[0]); j++) {
+			for (k = 0; k < ARRAY_SIZE(probs->coef[0][0]); k++)
+				write_coeff_plane(probs->coef[i][j][k],
+						  rkprobs->coef[k][i][j]);
+		}
+	}
+
+	/* intra uv mode 6 x 128 */
+	memcpy(rkprobs->uv_mode_0_2, &probs->uv_mode[0],
+	       sizeof(rkprobs->uv_mode_0_2));
+	memcpy(rkprobs->uv_mode_3_5, &probs->uv_mode[3],
+	       sizeof(rkprobs->uv_mode_3_5));
+	memcpy(rkprobs->uv_mode_6_8, &probs->uv_mode[6],
+	       sizeof(rkprobs->uv_mode_6_8));
+	memcpy(rkprobs->uv_mode_9, &probs->uv_mode[9],
+	       sizeof(rkprobs->uv_mode_9));
+
+	/* mv related 6 x 128 */
+	memcpy(rkprobs->mv.joint, probs->mv.joint,
+	       sizeof(rkprobs->mv.joint));
+	memcpy(rkprobs->mv.sign, probs->mv.sign,
+	       sizeof(rkprobs->mv.sign));
+	memcpy(rkprobs->mv.classes, probs->mv.classes,
+	       sizeof(rkprobs->mv.classes));
+	memcpy(rkprobs->mv.class0_bit, probs->mv.class0_bit,
+	       sizeof(rkprobs->mv.class0_bit));
+	memcpy(rkprobs->mv.bits, probs->mv.bits,
+	       sizeof(rkprobs->mv.bits));
+	memcpy(rkprobs->mv.class0_fr, probs->mv.class0_fr,
+	       sizeof(rkprobs->mv.class0_fr));
+	memcpy(rkprobs->mv.fr, probs->mv.fr,
+	       sizeof(rkprobs->mv.fr));
+	memcpy(rkprobs->mv.class0_hp, probs->mv.class0_hp,
+	       sizeof(rkprobs->mv.class0_hp));
+	memcpy(rkprobs->mv.hp, probs->mv.hp,
+	       sizeof(rkprobs->mv.hp));
+}
+
+static void init_probs(struct rkvdec_ctx *ctx,
+		       const struct rkvdec_vp9_run *run)
+{
+	const struct v4l2_ctrl_vp9_frame *dec_params;
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct rkvdec_vp9_priv_tbl *tbl = vp9_ctx->priv_tbl.cpu;
+	struct rkvdec_vp9_probs *rkprobs = &tbl->probs;
+	const struct v4l2_vp9_segmentation *seg;
+	const struct v4l2_vp9_frame_context *probs;
+	bool intra_only;
+
+	dec_params = run->decode_params;
+	probs = &vp9_ctx->probability_tables;
+	seg = &dec_params->seg;
+
+	memset(rkprobs, 0, sizeof(*rkprobs));
+
+	intra_only = !!(dec_params->flags &
+			(V4L2_VP9_FRAME_FLAG_KEY_FRAME |
+			 V4L2_VP9_FRAME_FLAG_INTRA_ONLY));
+
+	/* sb info  5 x 128 bit */
+	memcpy(rkprobs->partition,
+	       intra_only ? v4l2_vp9_kf_partition_probs : probs->partition,
+	       sizeof(rkprobs->partition));
+
+	memcpy(rkprobs->pred, seg->pred_probs, sizeof(rkprobs->pred));
+	memcpy(rkprobs->tree, seg->tree_probs, sizeof(rkprobs->tree));
+	memcpy(rkprobs->skip, probs->skip, sizeof(rkprobs->skip));
+	memcpy(rkprobs->tx32, probs->tx32, sizeof(rkprobs->tx32));
+	memcpy(rkprobs->tx16, probs->tx16, sizeof(rkprobs->tx16));
+	memcpy(rkprobs->tx8, probs->tx8, sizeof(rkprobs->tx8));
+	memcpy(rkprobs->is_inter, probs->is_inter, sizeof(rkprobs->is_inter));
+
+	if (intra_only)
+		init_intra_only_probs(ctx, run);
+	else
+		init_inter_probs(ctx, run);
+}
+
+struct rkvdec_vp9_ref_reg {
+	u32 reg_frm_size;
+	u32 reg_hor_stride;
+	u32 reg_y_stride;
+	u32 reg_yuv_stride;
+	u32 reg_ref_base;
+};
+
+static struct rkvdec_vp9_ref_reg ref_regs[] = {
+	{
+		.reg_frm_size = RKVDEC_REG_VP9_FRAME_SIZE(0),
+		.reg_hor_stride = RKVDEC_VP9_HOR_VIRSTRIDE(0),
+		.reg_y_stride = RKVDEC_VP9_LAST_FRAME_YSTRIDE,
+		.reg_yuv_stride = RKVDEC_VP9_LAST_FRAME_YUVSTRIDE,
+		.reg_ref_base = RKVDEC_REG_VP9_LAST_FRAME_BASE,
+	},
+	{
+		.reg_frm_size = RKVDEC_REG_VP9_FRAME_SIZE(1),
+		.reg_hor_stride = RKVDEC_VP9_HOR_VIRSTRIDE(1),
+		.reg_y_stride = RKVDEC_VP9_GOLDEN_FRAME_YSTRIDE,
+		.reg_yuv_stride = 0,
+		.reg_ref_base = RKVDEC_REG_VP9_GOLDEN_FRAME_BASE,
+	},
+	{
+		.reg_frm_size = RKVDEC_REG_VP9_FRAME_SIZE(2),
+		.reg_hor_stride = RKVDEC_VP9_HOR_VIRSTRIDE(2),
+		.reg_y_stride = RKVDEC_VP9_ALTREF_FRAME_YSTRIDE,
+		.reg_yuv_stride = 0,
+		.reg_ref_base = RKVDEC_REG_VP9_ALTREF_FRAME_BASE,
+	}
+};
+
+static struct rkvdec_decoded_buffer *
+get_ref_buf(struct rkvdec_ctx *ctx, struct vb2_v4l2_buffer *dst, u64 timestamp)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = ctx->fh.m2m_ctx;
+	struct vb2_queue *cap_q = &m2m_ctx->cap_q_ctx.q;
+	struct vb2_buffer *buf;
+
+	/*
+	 * If a ref is unused or invalid, address of current destination
+	 * buffer is returned.
+	 */
+	buf = vb2_find_buffer(cap_q, timestamp);
+	if (!buf)
+		buf = &dst->vb2_buf;
+
+	return vb2_to_rkvdec_decoded_buf(buf);
+}
+
+static dma_addr_t get_mv_base_addr(struct rkvdec_decoded_buffer *buf)
+{
+	unsigned int aligned_pitch, aligned_height, yuv_len;
+
+	aligned_height = round_up(buf->vp9.height, 64);
+	aligned_pitch = round_up(buf->vp9.width * buf->vp9.bit_depth, 512) / 8;
+	yuv_len = (aligned_height * aligned_pitch * 3) / 2;
+
+	return vb2_dma_contig_plane_dma_addr(&buf->base.vb.vb2_buf, 0) +
+	       yuv_len;
+}
+
+static void config_ref_registers(struct rkvdec_ctx *ctx,
+				 const struct rkvdec_vp9_run *run,
+				 struct rkvdec_decoded_buffer *ref_buf,
+				 struct rkvdec_vp9_ref_reg *ref_reg)
+{
+	unsigned int aligned_pitch, aligned_height, y_len, yuv_len;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+
+	aligned_height = round_up(ref_buf->vp9.height, 64);
+	writel_relaxed(RKVDEC_VP9_FRAMEWIDTH(ref_buf->vp9.width) |
+		       RKVDEC_VP9_FRAMEHEIGHT(ref_buf->vp9.height),
+		       rkvdec->regs + ref_reg->reg_frm_size);
+
+	writel_relaxed(vb2_dma_contig_plane_dma_addr(&ref_buf->base.vb.vb2_buf, 0),
+		       rkvdec->regs + ref_reg->reg_ref_base);
+
+	if (&ref_buf->base.vb == run->base.bufs.dst)
+		return;
+
+	aligned_pitch = round_up(ref_buf->vp9.width * ref_buf->vp9.bit_depth, 512) / 8;
+	y_len = aligned_height * aligned_pitch;
+	yuv_len = (y_len * 3) / 2;
+
+	writel_relaxed(RKVDEC_HOR_Y_VIRSTRIDE(aligned_pitch / 16) |
+		       RKVDEC_HOR_UV_VIRSTRIDE(aligned_pitch / 16),
+		       rkvdec->regs + ref_reg->reg_hor_stride);
+	writel_relaxed(RKVDEC_VP9_REF_YSTRIDE(y_len / 16),
+		       rkvdec->regs + ref_reg->reg_y_stride);
+
+	if (!ref_reg->reg_yuv_stride)
+		return;
+
+	writel_relaxed(RKVDEC_VP9_REF_YUVSTRIDE(yuv_len / 16),
+		       rkvdec->regs + ref_reg->reg_yuv_stride);
+}
+
+static void config_seg_registers(struct rkvdec_ctx *ctx, unsigned int segid)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	const struct v4l2_vp9_segmentation *seg;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	s16 feature_val;
+	int feature_id;
+	u32 val = 0;
+
+	seg = vp9_ctx->last.valid ? &vp9_ctx->last.seg : &vp9_ctx->cur.seg;
+	feature_id = V4L2_VP9_SEG_LVL_ALT_Q;
+	if (v4l2_vp9_seg_feat_enabled(seg->feature_enabled, feature_id, segid)) {
+		feature_val = seg->feature_data[segid][feature_id];
+		val |= RKVDEC_SEGID_FRAME_QP_DELTA_EN(1) |
+		       RKVDEC_SEGID_FRAME_QP_DELTA(feature_val);
+	}
+
+	feature_id = V4L2_VP9_SEG_LVL_ALT_L;
+	if (v4l2_vp9_seg_feat_enabled(seg->feature_enabled, feature_id, segid)) {
+		feature_val = seg->feature_data[segid][feature_id];
+		val |= RKVDEC_SEGID_FRAME_LOOPFILTER_VALUE_EN(1) |
+		       RKVDEC_SEGID_FRAME_LOOPFILTER_VALUE(feature_val);
+	}
+
+	feature_id = V4L2_VP9_SEG_LVL_REF_FRAME;
+	if (v4l2_vp9_seg_feat_enabled(seg->feature_enabled, feature_id, segid)) {
+		feature_val = seg->feature_data[segid][feature_id];
+		val |= RKVDEC_SEGID_REFERINFO_EN(1) |
+		       RKVDEC_SEGID_REFERINFO(feature_val);
+	}
+
+	feature_id = V4L2_VP9_SEG_LVL_SKIP;
+	if (v4l2_vp9_seg_feat_enabled(seg->feature_enabled, feature_id, segid))
+		val |= RKVDEC_SEGID_FRAME_SKIP_EN(1);
+
+	if (!segid &&
+	    (seg->flags & V4L2_VP9_SEGMENTATION_FLAG_ABS_OR_DELTA_UPDATE))
+		val |= RKVDEC_SEGID_ABS_DELTA(1);
+
+	writel_relaxed(val, rkvdec->regs + RKVDEC_VP9_SEGID_GRP(segid));
+}
+
+static void update_dec_buf_info(struct rkvdec_decoded_buffer *buf,
+				const struct v4l2_ctrl_vp9_frame *dec_params)
+{
+	buf->vp9.width = dec_params->frame_width_minus_1 + 1;
+	buf->vp9.height = dec_params->frame_height_minus_1 + 1;
+	buf->vp9.bit_depth = dec_params->bit_depth;
+}
+
+static void update_ctx_cur_info(struct rkvdec_vp9_ctx *vp9_ctx,
+				struct rkvdec_decoded_buffer *buf,
+				const struct v4l2_ctrl_vp9_frame *dec_params)
+{
+	vp9_ctx->cur.valid = true;
+	vp9_ctx->cur.reference_mode = dec_params->reference_mode;
+	vp9_ctx->cur.interpolation_filter = dec_params->interpolation_filter;
+	vp9_ctx->cur.flags = dec_params->flags;
+	vp9_ctx->cur.timestamp = buf->base.vb.vb2_buf.timestamp;
+	vp9_ctx->cur.seg = dec_params->seg;
+	vp9_ctx->cur.lf = dec_params->lf;
+}
+
+static void update_ctx_last_info(struct rkvdec_vp9_ctx *vp9_ctx)
+{
+	vp9_ctx->last = vp9_ctx->cur;
+}
+
+static void config_registers(struct rkvdec_ctx *ctx,
+			     const struct rkvdec_vp9_run *run)
+{
+	unsigned int y_len, uv_len, yuv_len, bit_depth, aligned_height, aligned_pitch, stream_len;
+	const struct v4l2_ctrl_vp9_frame *dec_params;
+	struct rkvdec_decoded_buffer *ref_bufs[3];
+	struct rkvdec_decoded_buffer *dst, *last, *mv_ref;
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	u32 val, last_frame_info = 0;
+	const struct v4l2_vp9_segmentation *seg;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	dma_addr_t addr;
+	bool intra_only;
+	unsigned int i;
+
+	dec_params = run->decode_params;
+	dst = vb2_to_rkvdec_decoded_buf(&run->base.bufs.dst->vb2_buf);
+	ref_bufs[0] = get_ref_buf(ctx, &dst->base.vb, dec_params->last_frame_ts);
+	ref_bufs[1] = get_ref_buf(ctx, &dst->base.vb, dec_params->golden_frame_ts);
+	ref_bufs[2] = get_ref_buf(ctx, &dst->base.vb, dec_params->alt_frame_ts);
+
+	if (vp9_ctx->last.valid)
+		last = get_ref_buf(ctx, &dst->base.vb, vp9_ctx->last.timestamp);
+	else
+		last = dst;
+
+	update_dec_buf_info(dst, dec_params);
+	update_ctx_cur_info(vp9_ctx, dst, dec_params);
+	seg = &dec_params->seg;
+
+	intra_only = !!(dec_params->flags &
+			(V4L2_VP9_FRAME_FLAG_KEY_FRAME |
+			 V4L2_VP9_FRAME_FLAG_INTRA_ONLY));
+
+	writel_relaxed(RKVDEC_MODE(RKVDEC_MODE_VP9),
+		       rkvdec->regs + RKVDEC_REG_SYSCTRL);
+
+	bit_depth = dec_params->bit_depth;
+	aligned_height = round_up(ctx->decoded_fmt.fmt.pix_mp.height, 64);
+
+	aligned_pitch = round_up(ctx->decoded_fmt.fmt.pix_mp.width *
+				 bit_depth,
+				 512) / 8;
+	y_len = aligned_height * aligned_pitch;
+	uv_len = y_len / 2;
+	yuv_len = y_len + uv_len;
+
+	writel_relaxed(RKVDEC_Y_HOR_VIRSTRIDE(aligned_pitch / 16) |
+		       RKVDEC_UV_HOR_VIRSTRIDE(aligned_pitch / 16),
+		       rkvdec->regs + RKVDEC_REG_PICPAR);
+	writel_relaxed(RKVDEC_Y_VIRSTRIDE(y_len / 16),
+		       rkvdec->regs + RKVDEC_REG_Y_VIRSTRIDE);
+	writel_relaxed(RKVDEC_YUV_VIRSTRIDE(yuv_len / 16),
+		       rkvdec->regs + RKVDEC_REG_YUV_VIRSTRIDE);
+
+	stream_len = vb2_get_plane_payload(&run->base.bufs.src->vb2_buf, 0);
+	writel_relaxed(RKVDEC_STRM_LEN(stream_len),
+		       rkvdec->regs + RKVDEC_REG_STRM_LEN);
+
+	/*
+	 * Reset count buffer, because decoder only output intra related syntax
+	 * counts when decoding intra frame, but update entropy need to update
+	 * all the probabilities.
+	 */
+	if (intra_only)
+		memset(vp9_ctx->count_tbl.cpu, 0, vp9_ctx->count_tbl.size);
+
+	vp9_ctx->cur.segmapid = vp9_ctx->last.segmapid;
+	if (!intra_only &&
+	    !(dec_params->flags & V4L2_VP9_FRAME_FLAG_ERROR_RESILIENT) &&
+	    (!(seg->flags & V4L2_VP9_SEGMENTATION_FLAG_ENABLED) ||
+	     (seg->flags & V4L2_VP9_SEGMENTATION_FLAG_UPDATE_MAP)))
+		vp9_ctx->cur.segmapid++;
+
+	for (i = 0; i < ARRAY_SIZE(ref_bufs); i++)
+		config_ref_registers(ctx, run, ref_bufs[i], &ref_regs[i]);
+
+	for (i = 0; i < 8; i++)
+		config_seg_registers(ctx, i);
+
+	writel_relaxed(RKVDEC_VP9_TX_MODE(vp9_ctx->cur.tx_mode) |
+		       RKVDEC_VP9_FRAME_REF_MODE(dec_params->reference_mode),
+		       rkvdec->regs + RKVDEC_VP9_CPRHEADER_CONFIG);
+
+	if (!intra_only) {
+		const struct v4l2_vp9_loop_filter *lf;
+		s8 delta;
+
+		if (vp9_ctx->last.valid)
+			lf = &vp9_ctx->last.lf;
+		else
+			lf = &vp9_ctx->cur.lf;
+
+		val = 0;
+		for (i = 0; i < ARRAY_SIZE(lf->ref_deltas); i++) {
+			delta = lf->ref_deltas[i];
+			val |= RKVDEC_REF_DELTAS_LASTFRAME(i, delta);
+		}
+
+		writel_relaxed(val,
+			       rkvdec->regs + RKVDEC_VP9_REF_DELTAS_LASTFRAME);
+
+		for (i = 0; i < ARRAY_SIZE(lf->mode_deltas); i++) {
+			delta = lf->mode_deltas[i];
+			last_frame_info |= RKVDEC_MODE_DELTAS_LASTFRAME(i,
+									delta);
+		}
+	}
+
+	if (vp9_ctx->last.valid && !intra_only &&
+	    vp9_ctx->last.seg.flags & V4L2_VP9_SEGMENTATION_FLAG_ENABLED)
+		last_frame_info |= RKVDEC_SEG_EN_LASTFRAME;
+
+	if (vp9_ctx->last.valid &&
+	    vp9_ctx->last.flags & V4L2_VP9_FRAME_FLAG_SHOW_FRAME)
+		last_frame_info |= RKVDEC_LAST_SHOW_FRAME;
+
+	if (vp9_ctx->last.valid &&
+	    vp9_ctx->last.flags &
+	    (V4L2_VP9_FRAME_FLAG_KEY_FRAME | V4L2_VP9_FRAME_FLAG_INTRA_ONLY))
+		last_frame_info |= RKVDEC_LAST_INTRA_ONLY;
+
+	if (vp9_ctx->last.valid &&
+	    last->vp9.width == dst->vp9.width &&
+	    last->vp9.height == dst->vp9.height)
+		last_frame_info |= RKVDEC_LAST_WIDHHEIGHT_EQCUR;
+
+	writel_relaxed(last_frame_info,
+		       rkvdec->regs + RKVDEC_VP9_INFO_LASTFRAME);
+
+	writel_relaxed(stream_len - dec_params->compressed_header_size -
+		       dec_params->uncompressed_header_size,
+		       rkvdec->regs + RKVDEC_VP9_LASTTILE_SIZE);
+
+	for (i = 0; !intra_only && i < ARRAY_SIZE(ref_bufs); i++) {
+		unsigned int refw = ref_bufs[i]->vp9.width;
+		unsigned int refh = ref_bufs[i]->vp9.height;
+		u32 hscale, vscale;
+
+		hscale = (refw << 14) /	dst->vp9.width;
+		vscale = (refh << 14) / dst->vp9.height;
+		writel_relaxed(RKVDEC_VP9_REF_HOR_SCALE(hscale) |
+			       RKVDEC_VP9_REF_VER_SCALE(vscale),
+			       rkvdec->regs + RKVDEC_VP9_REF_SCALE(i));
+	}
+
+	addr = vb2_dma_contig_plane_dma_addr(&dst->base.vb.vb2_buf, 0);
+	writel_relaxed(addr, rkvdec->regs + RKVDEC_REG_DECOUT_BASE);
+	addr = vb2_dma_contig_plane_dma_addr(&run->base.bufs.src->vb2_buf, 0);
+	writel_relaxed(addr, rkvdec->regs + RKVDEC_REG_STRM_RLC_BASE);
+	writel_relaxed(vp9_ctx->priv_tbl.dma +
+		       offsetof(struct rkvdec_vp9_priv_tbl, probs),
+		       rkvdec->regs + RKVDEC_REG_CABACTBL_PROB_BASE);
+	writel_relaxed(vp9_ctx->count_tbl.dma,
+		       rkvdec->regs + RKVDEC_REG_VP9COUNT_BASE);
+
+	writel_relaxed(vp9_ctx->priv_tbl.dma +
+		       offsetof(struct rkvdec_vp9_priv_tbl, segmap) +
+		       (RKVDEC_VP9_MAX_SEGMAP_SIZE * vp9_ctx->cur.segmapid),
+		       rkvdec->regs + RKVDEC_REG_VP9_SEGIDCUR_BASE);
+	writel_relaxed(vp9_ctx->priv_tbl.dma +
+		       offsetof(struct rkvdec_vp9_priv_tbl, segmap) +
+		       (RKVDEC_VP9_MAX_SEGMAP_SIZE * (!vp9_ctx->cur.segmapid)),
+		       rkvdec->regs + RKVDEC_REG_VP9_SEGIDLAST_BASE);
+
+	if (!intra_only &&
+	    !(dec_params->flags & V4L2_VP9_FRAME_FLAG_ERROR_RESILIENT) &&
+	    vp9_ctx->last.valid)
+		mv_ref = last;
+	else
+		mv_ref = dst;
+
+	writel_relaxed(get_mv_base_addr(mv_ref),
+		       rkvdec->regs + RKVDEC_VP9_REF_COLMV_BASE);
+
+	writel_relaxed(ctx->decoded_fmt.fmt.pix_mp.width |
+		       (ctx->decoded_fmt.fmt.pix_mp.height << 16),
+		       rkvdec->regs + RKVDEC_REG_PERFORMANCE_CYCLE);
+}
+
+static int validate_dec_params(struct rkvdec_ctx *ctx,
+			       const struct v4l2_ctrl_vp9_frame *dec_params)
+{
+	unsigned int aligned_width, aligned_height;
+
+	/* We only support profile 0. */
+	if (dec_params->profile != 0) {
+		dev_err(ctx->dev->dev, "unsupported profile %d\n",
+			dec_params->profile);
+		return -EINVAL;
+	}
+
+	aligned_width = round_up(dec_params->frame_width_minus_1 + 1, 64);
+	aligned_height = round_up(dec_params->frame_height_minus_1 + 1, 64);
+
+	/*
+	 * Userspace should update the capture/decoded format when the
+	 * resolution changes.
+	 */
+	if (aligned_width != ctx->decoded_fmt.fmt.pix_mp.width ||
+	    aligned_height != ctx->decoded_fmt.fmt.pix_mp.height) {
+		dev_err(ctx->dev->dev,
+			"unexpected bitstream resolution %dx%d\n",
+			dec_params->frame_width_minus_1 + 1,
+			dec_params->frame_height_minus_1 + 1);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rkvdec_vp9_run_preamble(struct rkvdec_ctx *ctx,
+				   struct rkvdec_vp9_run *run)
+{
+	const struct v4l2_ctrl_vp9_frame *dec_params;
+	const struct v4l2_ctrl_vp9_compressed_hdr *prob_updates;
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct v4l2_ctrl *ctrl;
+	unsigned int fctx_idx;
+	int ret;
+
+	/* v4l2-specific stuff */
+	rkvdec_run_preamble(ctx, &run->base);
+
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl,
+			      V4L2_CID_STATELESS_VP9_FRAME);
+	if (WARN_ON(!ctrl))
+		return -EINVAL;
+	dec_params = ctrl->p_cur.p;
+
+	ret = validate_dec_params(ctx, dec_params);
+	if (ret)
+		return ret;
+
+	run->decode_params = dec_params;
+
+	ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl, V4L2_CID_STATELESS_VP9_COMPRESSED_HDR);
+	if (WARN_ON(!ctrl))
+		return -EINVAL;
+	prob_updates = ctrl->p_cur.p;
+	vp9_ctx->cur.tx_mode = prob_updates->tx_mode;
+
+	/*
+	 * vp9 stuff
+	 *
+	 * by this point the userspace has done all parts of 6.2 uncompressed_header()
+	 * except this fragment:
+	 * if ( FrameIsIntra || error_resilient_mode ) {
+	 *	setup_past_independence ( )
+	 *	if ( frame_type == KEY_FRAME || error_resilient_mode == 1 ||
+	 *	     reset_frame_context == 3 ) {
+	 *		for ( i = 0; i < 4; i ++ ) {
+	 *			save_probs( i )
+	 *		}
+	 *	} else if ( reset_frame_context == 2 ) {
+	 *		save_probs( frame_context_idx )
+	 *	}
+	 *	frame_context_idx = 0
+	 * }
+	 */
+	fctx_idx = v4l2_vp9_reset_frame_ctx(dec_params, vp9_ctx->frame_context);
+	vp9_ctx->cur.frame_context_idx = fctx_idx;
+
+	/* 6.1 frame(sz): load_probs() and load_probs2() */
+	vp9_ctx->probability_tables = vp9_ctx->frame_context[fctx_idx];
+
+	/*
+	 * The userspace has also performed 6.3 compressed_header(), but handling the
+	 * probs in a special way. All probs which need updating, except MV-related,
+	 * have been read from the bitstream and translated through inv_map_table[],
+	 * but no 6.3.6 inv_recenter_nonneg(v, m) has been performed. The values passed
+	 * by userspace are either translated values (there are no 0 values in
+	 * inv_map_table[]), or zero to indicate no update. All MV-related probs which need
+	 * updating have been read from the bitstream and (mv_prob << 1) | 1 has been
+	 * performed. The values passed by userspace are either new values
+	 * to replace old ones (the above mentioned shift and bitwise or never result in
+	 * a zero) or zero to indicate no update.
+	 * fw_update_probs() performs actual probs updates or leaves probs as-is
+	 * for values for which a zero was passed from userspace.
+	 */
+	v4l2_vp9_fw_update_probs(&vp9_ctx->probability_tables, prob_updates, dec_params);
+
+	return 0;
+}
+
+static int rkvdec_vp9_run(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	struct rkvdec_vp9_run run = { };
+	int ret;
+
+	ret = rkvdec_vp9_run_preamble(ctx, &run);
+	if (ret) {
+		rkvdec_run_postamble(ctx, &run.base);
+		return ret;
+	}
+
+	/* Prepare probs. */
+	init_probs(ctx, &run);
+
+	/* Configure hardware registers. */
+	config_registers(ctx, &run);
+
+	rkvdec_run_postamble(ctx, &run.base);
+
+	schedule_delayed_work(&rkvdec->watchdog_work, msecs_to_jiffies(2000));
+
+	writel(1, rkvdec->regs + RKVDEC_REG_PREF_LUMA_CACHE_COMMAND);
+	writel(1, rkvdec->regs + RKVDEC_REG_PREF_CHR_CACHE_COMMAND);
+
+	writel(0xe, rkvdec->regs + RKVDEC_REG_STRMD_ERR_EN);
+	/* Start decoding! */
+	writel(RKVDEC_INTERRUPT_DEC_E | RKVDEC_CONFIG_DEC_CLK_GATE_E |
+	       RKVDEC_TIMEOUT_E | RKVDEC_BUF_EMPTY_E,
+	       rkvdec->regs + RKVDEC_REG_INTERRUPT);
+
+	return 0;
+}
+
+#define copy_tx_and_skip(p1, p2)				\
+do {								\
+	memcpy((p1)->tx8, (p2)->tx8, sizeof((p1)->tx8));	\
+	memcpy((p1)->tx16, (p2)->tx16, sizeof((p1)->tx16));	\
+	memcpy((p1)->tx32, (p2)->tx32, sizeof((p1)->tx32));	\
+	memcpy((p1)->skip, (p2)->skip, sizeof((p1)->skip));	\
+} while (0)
+
+static void rkvdec_vp9_done(struct rkvdec_ctx *ctx,
+			    struct vb2_v4l2_buffer *src_buf,
+			    struct vb2_v4l2_buffer *dst_buf,
+			    enum vb2_buffer_state result)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	unsigned int fctx_idx;
+
+	/* v4l2-specific stuff */
+	if (result == VB2_BUF_STATE_ERROR)
+		goto out_update_last;
+
+	/*
+	 * vp9 stuff
+	 *
+	 * 6.1.2 refresh_probs()
+	 *
+	 * In the spec a complementary condition goes last in 6.1.2 refresh_probs(),
+	 * but it makes no sense to perform all the activities from the first "if"
+	 * there if we actually are not refreshing the frame context. On top of that,
+	 * because of 6.2 uncompressed_header() whenever error_resilient_mode == 1,
+	 * refresh_frame_context == 0. Consequently, if we don't jump to out_update_last
+	 * it means error_resilient_mode must be 0.
+	 */
+	if (!(vp9_ctx->cur.flags & V4L2_VP9_FRAME_FLAG_REFRESH_FRAME_CTX))
+		goto out_update_last;
+
+	fctx_idx = vp9_ctx->cur.frame_context_idx;
+
+	if (!(vp9_ctx->cur.flags & V4L2_VP9_FRAME_FLAG_PARALLEL_DEC_MODE)) {
+		/* error_resilient_mode == 0 && frame_parallel_decoding_mode == 0 */
+		struct v4l2_vp9_frame_context *probs = &vp9_ctx->probability_tables;
+		bool frame_is_intra = vp9_ctx->cur.flags &
+		    (V4L2_VP9_FRAME_FLAG_KEY_FRAME | V4L2_VP9_FRAME_FLAG_INTRA_ONLY);
+		struct tx_and_skip {
+			u8 tx8[2][1];
+			u8 tx16[2][2];
+			u8 tx32[2][3];
+			u8 skip[3];
+		} _tx_skip, *tx_skip = &_tx_skip;
+		struct v4l2_vp9_frame_symbol_counts *counts;
+
+		/* buffer the forward-updated TX and skip probs */
+		if (frame_is_intra)
+			copy_tx_and_skip(tx_skip, probs);
+
+		/* 6.1.2 refresh_probs(): load_probs() and load_probs2() */
+		*probs = vp9_ctx->frame_context[fctx_idx];
+
+		/* if FrameIsIntra then undo the effect of load_probs2() */
+		if (frame_is_intra)
+			copy_tx_and_skip(probs, tx_skip);
+
+		counts = frame_is_intra ? &vp9_ctx->intra_cnts : &vp9_ctx->inter_cnts;
+		v4l2_vp9_adapt_coef_probs(probs, counts,
+					  !vp9_ctx->last.valid ||
+					  vp9_ctx->last.flags & V4L2_VP9_FRAME_FLAG_KEY_FRAME,
+					  frame_is_intra);
+		if (!frame_is_intra) {
+			const struct rkvdec_vp9_inter_frame_symbol_counts *inter_cnts;
+			u32 classes[2][11];
+			int i;
+
+			inter_cnts = vp9_ctx->count_tbl.cpu;
+			for (i = 0; i < ARRAY_SIZE(classes); ++i)
+				memcpy(classes[i], inter_cnts->classes[i], sizeof(classes[0]));
+			counts->classes = &classes;
+
+			/* load_probs2() already done */
+			v4l2_vp9_adapt_noncoef_probs(&vp9_ctx->probability_tables, counts,
+						     vp9_ctx->cur.reference_mode,
+						     vp9_ctx->cur.interpolation_filter,
+						     vp9_ctx->cur.tx_mode, vp9_ctx->cur.flags);
+		}
+	}
+
+	/* 6.1.2 refresh_probs(): save_probs(fctx_idx) */
+	vp9_ctx->frame_context[fctx_idx] = vp9_ctx->probability_tables;
+
+out_update_last:
+	update_ctx_last_info(vp9_ctx);
+}
+
+static void rkvdec_init_v4l2_vp9_count_tbl(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct rkvdec_vp9_intra_frame_symbol_counts *intra_cnts = vp9_ctx->count_tbl.cpu;
+	struct rkvdec_vp9_inter_frame_symbol_counts *inter_cnts = vp9_ctx->count_tbl.cpu;
+	int i, j, k, l, m;
+
+	vp9_ctx->inter_cnts.partition = &inter_cnts->partition;
+	vp9_ctx->inter_cnts.skip = &inter_cnts->skip;
+	vp9_ctx->inter_cnts.intra_inter = &inter_cnts->inter;
+	vp9_ctx->inter_cnts.tx32p = &inter_cnts->tx32p;
+	vp9_ctx->inter_cnts.tx16p = &inter_cnts->tx16p;
+	vp9_ctx->inter_cnts.tx8p = &inter_cnts->tx8p;
+
+	vp9_ctx->intra_cnts.partition = (u32 (*)[16][4])(&intra_cnts->partition);
+	vp9_ctx->intra_cnts.skip = &intra_cnts->skip;
+	vp9_ctx->intra_cnts.intra_inter = &intra_cnts->intra;
+	vp9_ctx->intra_cnts.tx32p = &intra_cnts->tx32p;
+	vp9_ctx->intra_cnts.tx16p = &intra_cnts->tx16p;
+	vp9_ctx->intra_cnts.tx8p = &intra_cnts->tx8p;
+
+	vp9_ctx->inter_cnts.y_mode = &inter_cnts->y_mode;
+	vp9_ctx->inter_cnts.uv_mode = &inter_cnts->uv_mode;
+	vp9_ctx->inter_cnts.comp = &inter_cnts->comp;
+	vp9_ctx->inter_cnts.comp_ref = &inter_cnts->comp_ref;
+	vp9_ctx->inter_cnts.single_ref = &inter_cnts->single_ref;
+	vp9_ctx->inter_cnts.mv_mode = &inter_cnts->mv_mode;
+	vp9_ctx->inter_cnts.filter = &inter_cnts->filter;
+	vp9_ctx->inter_cnts.mv_joint = &inter_cnts->mv_joint;
+	vp9_ctx->inter_cnts.sign = &inter_cnts->sign;
+	/*
+	 * rk hardware actually uses "u32 classes[2][11 + 1];"
+	 * instead of "u32 classes[2][11];", so this must be explicitly
+	 * copied into vp9_ctx->classes when passing the data to the
+	 * vp9 library function
+	 */
+	vp9_ctx->inter_cnts.class0 = &inter_cnts->class0;
+	vp9_ctx->inter_cnts.bits = &inter_cnts->bits;
+	vp9_ctx->inter_cnts.class0_fp = &inter_cnts->class0_fp;
+	vp9_ctx->inter_cnts.fp = &inter_cnts->fp;
+	vp9_ctx->inter_cnts.class0_hp = &inter_cnts->class0_hp;
+	vp9_ctx->inter_cnts.hp = &inter_cnts->hp;
+
+#define INNERMOST_LOOP \
+	do {										\
+		for (m = 0; m < ARRAY_SIZE(vp9_ctx->inter_cnts.coeff[0][0][0][0]); ++m) {\
+			vp9_ctx->inter_cnts.coeff[i][j][k][l][m] =			\
+				&inter_cnts->ref_cnt[k][i][j][l][m].coeff;		\
+			vp9_ctx->inter_cnts.eob[i][j][k][l][m][0] =			\
+				&inter_cnts->ref_cnt[k][i][j][l][m].eob[0];		\
+			vp9_ctx->inter_cnts.eob[i][j][k][l][m][1] =			\
+				&inter_cnts->ref_cnt[k][i][j][l][m].eob[1];		\
+											\
+			vp9_ctx->intra_cnts.coeff[i][j][k][l][m] =			\
+				&intra_cnts->ref_cnt[k][i][j][l][m].coeff;		\
+			vp9_ctx->intra_cnts.eob[i][j][k][l][m][0] =			\
+				&intra_cnts->ref_cnt[k][i][j][l][m].eob[0];		\
+			vp9_ctx->intra_cnts.eob[i][j][k][l][m][1] =			\
+				&intra_cnts->ref_cnt[k][i][j][l][m].eob[1];		\
+		}									\
+	} while (0)
+
+	for (i = 0; i < ARRAY_SIZE(vp9_ctx->inter_cnts.coeff); ++i)
+		for (j = 0; j < ARRAY_SIZE(vp9_ctx->inter_cnts.coeff[0]); ++j)
+			for (k = 0; k < ARRAY_SIZE(vp9_ctx->inter_cnts.coeff[0][0]); ++k)
+				for (l = 0; l < ARRAY_SIZE(vp9_ctx->inter_cnts.coeff[0][0][0]); ++l)
+					INNERMOST_LOOP;
+#undef INNERMOST_LOOP
+}
+
+static int rkvdec_vp9_start(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	struct rkvdec_vp9_priv_tbl *priv_tbl;
+	struct rkvdec_vp9_ctx *vp9_ctx;
+	unsigned char *count_tbl;
+	int ret;
+
+	vp9_ctx = kzalloc(sizeof(*vp9_ctx), GFP_KERNEL);
+	if (!vp9_ctx)
+		return -ENOMEM;
+
+	ctx->priv = vp9_ctx;
+
+	BUILD_BUG_ON(sizeof(priv_tbl->probs) % 16); /* ensure probs size is 128-bit aligned */
+	priv_tbl = dma_alloc_coherent(rkvdec->dev, sizeof(*priv_tbl),
+				      &vp9_ctx->priv_tbl.dma, GFP_KERNEL);
+	if (!priv_tbl) {
+		ret = -ENOMEM;
+		goto err_free_ctx;
+	}
+
+	vp9_ctx->priv_tbl.size = sizeof(*priv_tbl);
+	vp9_ctx->priv_tbl.cpu = priv_tbl;
+
+	count_tbl = dma_alloc_coherent(rkvdec->dev, RKVDEC_VP9_COUNT_SIZE,
+				       &vp9_ctx->count_tbl.dma, GFP_KERNEL);
+	if (!count_tbl) {
+		ret = -ENOMEM;
+		goto err_free_priv_tbl;
+	}
+
+	vp9_ctx->count_tbl.size = RKVDEC_VP9_COUNT_SIZE;
+	vp9_ctx->count_tbl.cpu = count_tbl;
+	rkvdec_init_v4l2_vp9_count_tbl(ctx);
+
+	return 0;
+
+err_free_priv_tbl:
+	dma_free_coherent(rkvdec->dev, vp9_ctx->priv_tbl.size,
+			  vp9_ctx->priv_tbl.cpu, vp9_ctx->priv_tbl.dma);
+
+err_free_ctx:
+	kfree(vp9_ctx);
+	return ret;
+}
+
+static void rkvdec_vp9_stop(struct rkvdec_ctx *ctx)
+{
+	struct rkvdec_vp9_ctx *vp9_ctx = ctx->priv;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+
+	dma_free_coherent(rkvdec->dev, vp9_ctx->count_tbl.size,
+			  vp9_ctx->count_tbl.cpu, vp9_ctx->count_tbl.dma);
+	dma_free_coherent(rkvdec->dev, vp9_ctx->priv_tbl.size,
+			  vp9_ctx->priv_tbl.cpu, vp9_ctx->priv_tbl.dma);
+	kfree(vp9_ctx);
+}
+
+static int rkvdec_vp9_adjust_fmt(struct rkvdec_ctx *ctx,
+				 struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *fmt = &f->fmt.pix_mp;
+
+	fmt->num_planes = 1;
+	if (!fmt->plane_fmt[0].sizeimage)
+		fmt->plane_fmt[0].sizeimage = fmt->width * fmt->height * 2;
+	return 0;
+}
+
+const struct rkvdec_coded_fmt_ops rkvdec_vp9_fmt_ops = {
+	.adjust_fmt = rkvdec_vp9_adjust_fmt,
+	.start = rkvdec_vp9_start,
+	.stop = rkvdec_vp9_stop,
+	.run = rkvdec_vp9_run,
+	.done = rkvdec_vp9_done,
+};
diff --git a/drivers/staging/media/rkvdec/rkvdec.c b/drivers/staging/media/rkvdec/rkvdec.c
new file mode 100644
index 000000000..d16cf4115
--- /dev/null
+++ b/drivers/staging/media/rkvdec/rkvdec.c
@@ -0,0 +1,1118 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Rockchip Video Decoder driver
+ *
+ * Copyright (C) 2019 Collabora, Ltd.
+ *
+ * Based on rkvdec driver by Google LLC. (Tomasz Figa <tfiga@chromium.org>)
+ * Based on s5p-mfc driver by Samsung Electronics Co., Ltd.
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "rkvdec.h"
+#include "rkvdec-regs.h"
+
+static int rkvdec_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct rkvdec_ctx *ctx = container_of(ctrl->handler, struct rkvdec_ctx, ctrl_hdl);
+	const struct rkvdec_coded_fmt_desc *desc = ctx->coded_fmt_desc;
+
+	if (desc->ops->try_ctrl)
+		return desc->ops->try_ctrl(ctx, ctrl);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops rkvdec_ctrl_ops = {
+	.try_ctrl = rkvdec_try_ctrl,
+};
+
+static const struct rkvdec_ctrl_desc rkvdec_h264_ctrl_descs[] = {
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_DECODE_PARAMS,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_SPS,
+		.cfg.ops = &rkvdec_ctrl_ops,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_PPS,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_SCALING_MATRIX,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_DECODE_MODE,
+		.cfg.min = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+		.cfg.max = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+		.cfg.def = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_H264_START_CODE,
+		.cfg.min = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+		.cfg.def = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+		.cfg.max = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+	},
+	{
+		.cfg.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		.cfg.min = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+		.cfg.max = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		.cfg.menu_skip_mask =
+			BIT(V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED),
+		.cfg.def = V4L2_MPEG_VIDEO_H264_PROFILE_MAIN,
+	},
+	{
+		.cfg.id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+		.cfg.min = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+		.cfg.max = V4L2_MPEG_VIDEO_H264_LEVEL_5_1,
+	},
+};
+
+static const struct rkvdec_ctrls rkvdec_h264_ctrls = {
+	.ctrls = rkvdec_h264_ctrl_descs,
+	.num_ctrls = ARRAY_SIZE(rkvdec_h264_ctrl_descs),
+};
+
+static const u32 rkvdec_h264_vp9_decoded_fmts[] = {
+	V4L2_PIX_FMT_NV12,
+};
+
+static const struct rkvdec_ctrl_desc rkvdec_vp9_ctrl_descs[] = {
+	{
+		.cfg.id = V4L2_CID_STATELESS_VP9_FRAME,
+	},
+	{
+		.cfg.id = V4L2_CID_STATELESS_VP9_COMPRESSED_HDR,
+	},
+	{
+		.cfg.id = V4L2_CID_MPEG_VIDEO_VP9_PROFILE,
+		.cfg.min = V4L2_MPEG_VIDEO_VP9_PROFILE_0,
+		.cfg.max = V4L2_MPEG_VIDEO_VP9_PROFILE_0,
+		.cfg.def = V4L2_MPEG_VIDEO_VP9_PROFILE_0,
+	},
+};
+
+static const struct rkvdec_ctrls rkvdec_vp9_ctrls = {
+	.ctrls = rkvdec_vp9_ctrl_descs,
+	.num_ctrls = ARRAY_SIZE(rkvdec_vp9_ctrl_descs),
+};
+
+static const struct rkvdec_coded_fmt_desc rkvdec_coded_fmts[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_H264_SLICE,
+		.frmsize = {
+			.min_width = 48,
+			.max_width = 4096,
+			.step_width = 16,
+			.min_height = 48,
+			.max_height = 2560,
+			.step_height = 16,
+		},
+		.ctrls = &rkvdec_h264_ctrls,
+		.ops = &rkvdec_h264_fmt_ops,
+		.num_decoded_fmts = ARRAY_SIZE(rkvdec_h264_vp9_decoded_fmts),
+		.decoded_fmts = rkvdec_h264_vp9_decoded_fmts,
+		.subsystem_flags = VB2_V4L2_FL_SUPPORTS_M2M_HOLD_CAPTURE_BUF,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_VP9_FRAME,
+		.frmsize = {
+			.min_width = 64,
+			.max_width = 4096,
+			.step_width = 64,
+			.min_height = 64,
+			.max_height = 2304,
+			.step_height = 64,
+		},
+		.ctrls = &rkvdec_vp9_ctrls,
+		.ops = &rkvdec_vp9_fmt_ops,
+		.num_decoded_fmts = ARRAY_SIZE(rkvdec_h264_vp9_decoded_fmts),
+		.decoded_fmts = rkvdec_h264_vp9_decoded_fmts,
+	}
+};
+
+static const struct rkvdec_coded_fmt_desc *
+rkvdec_find_coded_fmt_desc(u32 fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(rkvdec_coded_fmts); i++) {
+		if (rkvdec_coded_fmts[i].fourcc == fourcc)
+			return &rkvdec_coded_fmts[i];
+	}
+
+	return NULL;
+}
+
+static void rkvdec_reset_fmt(struct rkvdec_ctx *ctx, struct v4l2_format *f,
+			     u32 fourcc)
+{
+	memset(f, 0, sizeof(*f));
+	f->fmt.pix_mp.pixelformat = fourcc;
+	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	f->fmt.pix_mp.colorspace = V4L2_COLORSPACE_REC709;
+	f->fmt.pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	f->fmt.pix_mp.quantization = V4L2_QUANTIZATION_DEFAULT;
+	f->fmt.pix_mp.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+}
+
+static void rkvdec_reset_coded_fmt(struct rkvdec_ctx *ctx)
+{
+	struct v4l2_format *f = &ctx->coded_fmt;
+
+	ctx->coded_fmt_desc = &rkvdec_coded_fmts[0];
+	rkvdec_reset_fmt(ctx, f, ctx->coded_fmt_desc->fourcc);
+
+	f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	f->fmt.pix_mp.width = ctx->coded_fmt_desc->frmsize.min_width;
+	f->fmt.pix_mp.height = ctx->coded_fmt_desc->frmsize.min_height;
+
+	if (ctx->coded_fmt_desc->ops->adjust_fmt)
+		ctx->coded_fmt_desc->ops->adjust_fmt(ctx, f);
+}
+
+static void rkvdec_reset_decoded_fmt(struct rkvdec_ctx *ctx)
+{
+	struct v4l2_format *f = &ctx->decoded_fmt;
+
+	rkvdec_reset_fmt(ctx, f, ctx->coded_fmt_desc->decoded_fmts[0]);
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	v4l2_fill_pixfmt_mp(&f->fmt.pix_mp,
+			    ctx->coded_fmt_desc->decoded_fmts[0],
+			    ctx->coded_fmt.fmt.pix_mp.width,
+			    ctx->coded_fmt.fmt.pix_mp.height);
+	f->fmt.pix_mp.plane_fmt[0].sizeimage += 128 *
+		DIV_ROUND_UP(f->fmt.pix_mp.width, 16) *
+		DIV_ROUND_UP(f->fmt.pix_mp.height, 16);
+}
+
+static int rkvdec_enum_framesizes(struct file *file, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	const struct rkvdec_coded_fmt_desc *fmt;
+
+	if (fsize->index != 0)
+		return -EINVAL;
+
+	fmt = rkvdec_find_coded_fmt_desc(fsize->pixel_format);
+	if (!fmt)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise = fmt->frmsize;
+	return 0;
+}
+
+static int rkvdec_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct rkvdec_dev *rkvdec = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	strscpy(cap->driver, rkvdec->dev->driver->name,
+		sizeof(cap->driver));
+	strscpy(cap->card, vdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 rkvdec->dev->driver->name);
+	return 0;
+}
+
+static int rkvdec_try_capture_fmt(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+	const struct rkvdec_coded_fmt_desc *coded_desc;
+	unsigned int i;
+
+	/*
+	 * The codec context should point to a coded format desc, if the format
+	 * on the coded end has not been set yet, it should point to the
+	 * default value.
+	 */
+	coded_desc = ctx->coded_fmt_desc;
+	if (WARN_ON(!coded_desc))
+		return -EINVAL;
+
+	for (i = 0; i < coded_desc->num_decoded_fmts; i++) {
+		if (coded_desc->decoded_fmts[i] == pix_mp->pixelformat)
+			break;
+	}
+
+	if (i == coded_desc->num_decoded_fmts)
+		pix_mp->pixelformat = coded_desc->decoded_fmts[0];
+
+	/* Always apply the frmsize constraint of the coded end. */
+	pix_mp->width = max(pix_mp->width, ctx->coded_fmt.fmt.pix_mp.width);
+	pix_mp->height = max(pix_mp->height, ctx->coded_fmt.fmt.pix_mp.height);
+	v4l2_apply_frmsize_constraints(&pix_mp->width,
+				       &pix_mp->height,
+				       &coded_desc->frmsize);
+
+	v4l2_fill_pixfmt_mp(pix_mp, pix_mp->pixelformat,
+			    pix_mp->width, pix_mp->height);
+	pix_mp->plane_fmt[0].sizeimage +=
+		128 *
+		DIV_ROUND_UP(pix_mp->width, 16) *
+		DIV_ROUND_UP(pix_mp->height, 16);
+	pix_mp->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int rkvdec_try_output_fmt(struct file *file, void *priv,
+				 struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+	const struct rkvdec_coded_fmt_desc *desc;
+
+	desc = rkvdec_find_coded_fmt_desc(pix_mp->pixelformat);
+	if (!desc) {
+		pix_mp->pixelformat = rkvdec_coded_fmts[0].fourcc;
+		desc = &rkvdec_coded_fmts[0];
+	}
+
+	v4l2_apply_frmsize_constraints(&pix_mp->width,
+				       &pix_mp->height,
+				       &desc->frmsize);
+
+	pix_mp->field = V4L2_FIELD_NONE;
+	/* All coded formats are considered single planar for now. */
+	pix_mp->num_planes = 1;
+
+	if (desc->ops->adjust_fmt) {
+		int ret;
+
+		ret = desc->ops->adjust_fmt(ctx, f);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int rkvdec_s_capture_fmt(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+	struct vb2_queue *vq;
+	int ret;
+
+	/* Change not allowed if queue is busy */
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+			     V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (vb2_is_busy(vq))
+		return -EBUSY;
+
+	ret = rkvdec_try_capture_fmt(file, priv, f);
+	if (ret)
+		return ret;
+
+	ctx->decoded_fmt = *f;
+	return 0;
+}
+
+static int rkvdec_s_output_fmt(struct file *file, void *priv,
+			       struct v4l2_format *f)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+	struct v4l2_m2m_ctx *m2m_ctx = ctx->fh.m2m_ctx;
+	const struct rkvdec_coded_fmt_desc *desc;
+	struct v4l2_format *cap_fmt;
+	struct vb2_queue *peer_vq, *vq;
+	int ret;
+
+	/*
+	 * In order to support dynamic resolution change, the decoder admits
+	 * a resolution change, as long as the pixelformat remains. Can't be
+	 * done if streaming.
+	 */
+	vq = v4l2_m2m_get_vq(m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (vb2_is_streaming(vq) ||
+	    (vb2_is_busy(vq) &&
+	     f->fmt.pix_mp.pixelformat != ctx->coded_fmt.fmt.pix_mp.pixelformat))
+		return -EBUSY;
+
+	/*
+	 * Since format change on the OUTPUT queue will reset the CAPTURE
+	 * queue, we can't allow doing so when the CAPTURE queue has buffers
+	 * allocated.
+	 */
+	peer_vq = v4l2_m2m_get_vq(m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (vb2_is_busy(peer_vq))
+		return -EBUSY;
+
+	ret = rkvdec_try_output_fmt(file, priv, f);
+	if (ret)
+		return ret;
+
+	desc = rkvdec_find_coded_fmt_desc(f->fmt.pix_mp.pixelformat);
+	if (!desc)
+		return -EINVAL;
+	ctx->coded_fmt_desc = desc;
+	ctx->coded_fmt = *f;
+
+	/*
+	 * Current decoded format might have become invalid with newly
+	 * selected codec, so reset it to default just to be safe and
+	 * keep internal driver state sane. User is mandated to set
+	 * the decoded format again after we return, so we don't need
+	 * anything smarter.
+	 *
+	 * Note that this will propagates any size changes to the decoded format.
+	 */
+	rkvdec_reset_decoded_fmt(ctx);
+
+	/* Propagate colorspace information to capture. */
+	cap_fmt = &ctx->decoded_fmt;
+	cap_fmt->fmt.pix_mp.colorspace = f->fmt.pix_mp.colorspace;
+	cap_fmt->fmt.pix_mp.xfer_func = f->fmt.pix_mp.xfer_func;
+	cap_fmt->fmt.pix_mp.ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+	cap_fmt->fmt.pix_mp.quantization = f->fmt.pix_mp.quantization;
+
+	/* Enable format specific queue features */
+	vq->subsystem_flags |= desc->subsystem_flags;
+
+	return 0;
+}
+
+static int rkvdec_g_output_fmt(struct file *file, void *priv,
+			       struct v4l2_format *f)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+
+	*f = ctx->coded_fmt;
+	return 0;
+}
+
+static int rkvdec_g_capture_fmt(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+
+	*f = ctx->decoded_fmt;
+	return 0;
+}
+
+static int rkvdec_enum_output_fmt(struct file *file, void *priv,
+				  struct v4l2_fmtdesc *f)
+{
+	if (f->index >= ARRAY_SIZE(rkvdec_coded_fmts))
+		return -EINVAL;
+
+	f->pixelformat = rkvdec_coded_fmts[f->index].fourcc;
+	return 0;
+}
+
+static int rkvdec_enum_capture_fmt(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(priv);
+
+	if (WARN_ON(!ctx->coded_fmt_desc))
+		return -EINVAL;
+
+	if (f->index >= ctx->coded_fmt_desc->num_decoded_fmts)
+		return -EINVAL;
+
+	f->pixelformat = ctx->coded_fmt_desc->decoded_fmts[f->index];
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops rkvdec_ioctl_ops = {
+	.vidioc_querycap = rkvdec_querycap,
+	.vidioc_enum_framesizes = rkvdec_enum_framesizes,
+
+	.vidioc_try_fmt_vid_cap_mplane = rkvdec_try_capture_fmt,
+	.vidioc_try_fmt_vid_out_mplane = rkvdec_try_output_fmt,
+	.vidioc_s_fmt_vid_out_mplane = rkvdec_s_output_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = rkvdec_s_capture_fmt,
+	.vidioc_g_fmt_vid_out_mplane = rkvdec_g_output_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = rkvdec_g_capture_fmt,
+	.vidioc_enum_fmt_vid_out = rkvdec_enum_output_fmt,
+	.vidioc_enum_fmt_vid_cap = rkvdec_enum_capture_fmt,
+
+	.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_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+
+	.vidioc_streamon = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_decoder_cmd = v4l2_m2m_ioctl_stateless_decoder_cmd,
+	.vidioc_try_decoder_cmd = v4l2_m2m_ioctl_stateless_try_decoder_cmd,
+};
+
+static int rkvdec_queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+			      unsigned int *num_planes, unsigned int sizes[],
+			      struct device *alloc_devs[])
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(vq);
+	struct v4l2_format *f;
+	unsigned int i;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type))
+		f = &ctx->coded_fmt;
+	else
+		f = &ctx->decoded_fmt;
+
+	if (*num_planes) {
+		if (*num_planes != f->fmt.pix_mp.num_planes)
+			return -EINVAL;
+
+		for (i = 0; i < f->fmt.pix_mp.num_planes; i++) {
+			if (sizes[i] < f->fmt.pix_mp.plane_fmt[i].sizeimage)
+				return -EINVAL;
+		}
+	} else {
+		*num_planes = f->fmt.pix_mp.num_planes;
+		for (i = 0; i < f->fmt.pix_mp.num_planes; i++)
+			sizes[i] = f->fmt.pix_mp.plane_fmt[i].sizeimage;
+	}
+
+	return 0;
+}
+
+static int rkvdec_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(vq);
+	struct v4l2_format *f;
+	unsigned int i;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type))
+		f = &ctx->coded_fmt;
+	else
+		f = &ctx->decoded_fmt;
+
+	for (i = 0; i < f->fmt.pix_mp.num_planes; ++i) {
+		u32 sizeimage = f->fmt.pix_mp.plane_fmt[i].sizeimage;
+
+		if (vb2_plane_size(vb, i) < sizeimage)
+			return -EINVAL;
+	}
+
+	/*
+	 * Buffer's bytesused must be written by driver for CAPTURE buffers.
+	 * (for OUTPUT buffers, if userspace passes 0 bytesused, v4l2-core sets
+	 * it to buffer length).
+	 */
+	if (V4L2_TYPE_IS_CAPTURE(vq->type))
+		vb2_set_plane_payload(vb, 0, f->fmt.pix_mp.plane_fmt[0].sizeimage);
+
+	return 0;
+}
+
+static void rkvdec_buf_queue(struct vb2_buffer *vb)
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int rkvdec_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 void rkvdec_buf_request_complete(struct vb2_buffer *vb)
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->ctrl_hdl);
+}
+
+static int rkvdec_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(q);
+	const struct rkvdec_coded_fmt_desc *desc;
+	int ret;
+
+	if (V4L2_TYPE_IS_CAPTURE(q->type))
+		return 0;
+
+	desc = ctx->coded_fmt_desc;
+	if (WARN_ON(!desc))
+		return -EINVAL;
+
+	if (desc->ops->start) {
+		ret = desc->ops->start(ctx);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void rkvdec_queue_cleanup(struct vb2_queue *vq, u32 state)
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(vq);
+
+	while (true) {
+		struct vb2_v4l2_buffer *vbuf;
+
+		if (V4L2_TYPE_IS_OUTPUT(vq->type))
+			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		else
+			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+		if (!vbuf)
+			break;
+
+		v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
+					   &ctx->ctrl_hdl);
+		v4l2_m2m_buf_done(vbuf, state);
+	}
+}
+
+static void rkvdec_stop_streaming(struct vb2_queue *q)
+{
+	struct rkvdec_ctx *ctx = vb2_get_drv_priv(q);
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		const struct rkvdec_coded_fmt_desc *desc = ctx->coded_fmt_desc;
+
+		if (WARN_ON(!desc))
+			return;
+
+		if (desc->ops->stop)
+			desc->ops->stop(ctx);
+	}
+
+	rkvdec_queue_cleanup(q, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops rkvdec_queue_ops = {
+	.queue_setup = rkvdec_queue_setup,
+	.buf_prepare = rkvdec_buf_prepare,
+	.buf_queue = rkvdec_buf_queue,
+	.buf_out_validate = rkvdec_buf_out_validate,
+	.buf_request_complete = rkvdec_buf_request_complete,
+	.start_streaming = rkvdec_start_streaming,
+	.stop_streaming = rkvdec_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+static int rkvdec_request_validate(struct media_request *req)
+{
+	unsigned int count;
+
+	count = vb2_request_buffer_cnt(req);
+	if (!count)
+		return -ENOENT;
+	else if (count > 1)
+		return -EINVAL;
+
+	return vb2_request_validate(req);
+}
+
+static const struct media_device_ops rkvdec_media_ops = {
+	.req_validate = rkvdec_request_validate,
+	.req_queue = v4l2_m2m_request_queue,
+};
+
+static void rkvdec_job_finish_no_pm(struct rkvdec_ctx *ctx,
+				    enum vb2_buffer_state result)
+{
+	if (ctx->coded_fmt_desc->ops->done) {
+		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);
+		ctx->coded_fmt_desc->ops->done(ctx, src_buf, dst_buf, result);
+	}
+
+	v4l2_m2m_buf_done_and_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx,
+					 result);
+}
+
+static void rkvdec_job_finish(struct rkvdec_ctx *ctx,
+			      enum vb2_buffer_state result)
+{
+	struct rkvdec_dev *rkvdec = ctx->dev;
+
+	pm_runtime_mark_last_busy(rkvdec->dev);
+	pm_runtime_put_autosuspend(rkvdec->dev);
+	rkvdec_job_finish_no_pm(ctx, result);
+}
+
+void rkvdec_run_preamble(struct rkvdec_ctx *ctx, struct rkvdec_run *run)
+{
+	struct media_request *src_req;
+
+	memset(run, 0, sizeof(*run));
+
+	run->bufs.src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	run->bufs.dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	/* Apply request(s) controls if needed. */
+	src_req = run->bufs.src->vb2_buf.req_obj.req;
+	if (src_req)
+		v4l2_ctrl_request_setup(src_req, &ctx->ctrl_hdl);
+
+	v4l2_m2m_buf_copy_metadata(run->bufs.src, run->bufs.dst, true);
+}
+
+void rkvdec_run_postamble(struct rkvdec_ctx *ctx, struct rkvdec_run *run)
+{
+	struct media_request *src_req = run->bufs.src->vb2_buf.req_obj.req;
+
+	if (src_req)
+		v4l2_ctrl_request_complete(src_req, &ctx->ctrl_hdl);
+}
+
+static void rkvdec_device_run(void *priv)
+{
+	struct rkvdec_ctx *ctx = priv;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	const struct rkvdec_coded_fmt_desc *desc = ctx->coded_fmt_desc;
+	int ret;
+
+	if (WARN_ON(!desc))
+		return;
+
+	ret = pm_runtime_resume_and_get(rkvdec->dev);
+	if (ret < 0) {
+		rkvdec_job_finish_no_pm(ctx, VB2_BUF_STATE_ERROR);
+		return;
+	}
+
+	ret = desc->ops->run(ctx);
+	if (ret)
+		rkvdec_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static const struct v4l2_m2m_ops rkvdec_m2m_ops = {
+	.device_run = rkvdec_device_run,
+};
+
+static int rkvdec_queue_init(void *priv,
+			     struct vb2_queue *src_vq,
+			     struct vb2_queue *dst_vq)
+{
+	struct rkvdec_ctx *ctx = priv;
+	struct rkvdec_dev *rkvdec = ctx->dev;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &rkvdec_queue_ops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+
+	/*
+	 * Driver does mostly sequential access, so sacrifice TLB efficiency
+	 * for faster allocation. Also, no CPU access on the source queue,
+	 * so no kernel mapping needed.
+	 */
+	src_vq->dma_attrs = DMA_ATTR_ALLOC_SINGLE_PAGES |
+			    DMA_ATTR_NO_KERNEL_MAPPING;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &rkvdec->vdev_lock;
+	src_vq->dev = rkvdec->v4l2_dev.dev;
+	src_vq->supports_requests = true;
+	src_vq->requires_requests = true;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->bidirectional = true;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->dma_attrs = DMA_ATTR_ALLOC_SINGLE_PAGES |
+			    DMA_ATTR_NO_KERNEL_MAPPING;
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &rkvdec_queue_ops;
+	dst_vq->buf_struct_size = sizeof(struct rkvdec_decoded_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &rkvdec->vdev_lock;
+	dst_vq->dev = rkvdec->v4l2_dev.dev;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int rkvdec_add_ctrls(struct rkvdec_ctx *ctx,
+			    const struct rkvdec_ctrls *ctrls)
+{
+	unsigned int i;
+
+	for (i = 0; i < ctrls->num_ctrls; i++) {
+		const struct v4l2_ctrl_config *cfg = &ctrls->ctrls[i].cfg;
+
+		v4l2_ctrl_new_custom(&ctx->ctrl_hdl, cfg, ctx);
+		if (ctx->ctrl_hdl.error)
+			return ctx->ctrl_hdl.error;
+	}
+
+	return 0;
+}
+
+static int rkvdec_init_ctrls(struct rkvdec_ctx *ctx)
+{
+	unsigned int i, nctrls = 0;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(rkvdec_coded_fmts); i++)
+		nctrls += rkvdec_coded_fmts[i].ctrls->num_ctrls;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_hdl, nctrls);
+
+	for (i = 0; i < ARRAY_SIZE(rkvdec_coded_fmts); i++) {
+		ret = rkvdec_add_ctrls(ctx, rkvdec_coded_fmts[i].ctrls);
+		if (ret)
+			goto err_free_handler;
+	}
+
+	ret = v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
+	if (ret)
+		goto err_free_handler;
+
+	ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+	return ret;
+}
+
+static int rkvdec_open(struct file *filp)
+{
+	struct rkvdec_dev *rkvdec = video_drvdata(filp);
+	struct rkvdec_ctx *ctx;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->dev = rkvdec;
+	rkvdec_reset_coded_fmt(ctx);
+	rkvdec_reset_decoded_fmt(ctx);
+	v4l2_fh_init(&ctx->fh, video_devdata(filp));
+
+	ret = rkvdec_init_ctrls(ctx);
+	if (ret)
+		goto err_free_ctx;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(rkvdec->m2m_dev, ctx,
+					    rkvdec_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto err_cleanup_ctrls;
+	}
+
+	filp->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	return 0;
+
+err_cleanup_ctrls:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+
+err_free_ctx:
+	kfree(ctx);
+	return ret;
+}
+
+static int rkvdec_release(struct file *filp)
+{
+	struct rkvdec_ctx *ctx = fh_to_rkvdec_ctx(filp->private_data);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations rkvdec_fops = {
+	.owner = THIS_MODULE,
+	.open = rkvdec_open,
+	.release = rkvdec_release,
+	.poll = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = v4l2_m2m_fop_mmap,
+};
+
+static int rkvdec_v4l2_init(struct rkvdec_dev *rkvdec)
+{
+	int ret;
+
+	ret = v4l2_device_register(rkvdec->dev, &rkvdec->v4l2_dev);
+	if (ret) {
+		dev_err(rkvdec->dev, "Failed to register V4L2 device\n");
+		return ret;
+	}
+
+	rkvdec->m2m_dev = v4l2_m2m_init(&rkvdec_m2m_ops);
+	if (IS_ERR(rkvdec->m2m_dev)) {
+		v4l2_err(&rkvdec->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(rkvdec->m2m_dev);
+		goto err_unregister_v4l2;
+	}
+
+	rkvdec->mdev.dev = rkvdec->dev;
+	strscpy(rkvdec->mdev.model, "rkvdec", sizeof(rkvdec->mdev.model));
+	strscpy(rkvdec->mdev.bus_info, "platform:rkvdec",
+		sizeof(rkvdec->mdev.bus_info));
+	media_device_init(&rkvdec->mdev);
+	rkvdec->mdev.ops = &rkvdec_media_ops;
+	rkvdec->v4l2_dev.mdev = &rkvdec->mdev;
+
+	rkvdec->vdev.lock = &rkvdec->vdev_lock;
+	rkvdec->vdev.v4l2_dev = &rkvdec->v4l2_dev;
+	rkvdec->vdev.fops = &rkvdec_fops;
+	rkvdec->vdev.release = video_device_release_empty;
+	rkvdec->vdev.vfl_dir = VFL_DIR_M2M;
+	rkvdec->vdev.device_caps = V4L2_CAP_STREAMING |
+				   V4L2_CAP_VIDEO_M2M_MPLANE;
+	rkvdec->vdev.ioctl_ops = &rkvdec_ioctl_ops;
+	video_set_drvdata(&rkvdec->vdev, rkvdec);
+	strscpy(rkvdec->vdev.name, "rkvdec", sizeof(rkvdec->vdev.name));
+
+	ret = video_register_device(&rkvdec->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		v4l2_err(&rkvdec->v4l2_dev, "Failed to register video device\n");
+		goto err_cleanup_mc;
+	}
+
+	ret = v4l2_m2m_register_media_controller(rkvdec->m2m_dev, &rkvdec->vdev,
+						 MEDIA_ENT_F_PROC_VIDEO_DECODER);
+	if (ret) {
+		v4l2_err(&rkvdec->v4l2_dev,
+			 "Failed to initialize V4L2 M2M media controller\n");
+		goto err_unregister_vdev;
+	}
+
+	ret = media_device_register(&rkvdec->mdev);
+	if (ret) {
+		v4l2_err(&rkvdec->v4l2_dev, "Failed to register media device\n");
+		goto err_unregister_mc;
+	}
+
+	return 0;
+
+err_unregister_mc:
+	v4l2_m2m_unregister_media_controller(rkvdec->m2m_dev);
+
+err_unregister_vdev:
+	video_unregister_device(&rkvdec->vdev);
+
+err_cleanup_mc:
+	media_device_cleanup(&rkvdec->mdev);
+	v4l2_m2m_release(rkvdec->m2m_dev);
+
+err_unregister_v4l2:
+	v4l2_device_unregister(&rkvdec->v4l2_dev);
+	return ret;
+}
+
+static void rkvdec_v4l2_cleanup(struct rkvdec_dev *rkvdec)
+{
+	media_device_unregister(&rkvdec->mdev);
+	v4l2_m2m_unregister_media_controller(rkvdec->m2m_dev);
+	video_unregister_device(&rkvdec->vdev);
+	media_device_cleanup(&rkvdec->mdev);
+	v4l2_m2m_release(rkvdec->m2m_dev);
+	v4l2_device_unregister(&rkvdec->v4l2_dev);
+}
+
+static irqreturn_t rkvdec_irq_handler(int irq, void *priv)
+{
+	struct rkvdec_dev *rkvdec = priv;
+	enum vb2_buffer_state state;
+	u32 status;
+
+	status = readl(rkvdec->regs + RKVDEC_REG_INTERRUPT);
+	state = (status & RKVDEC_RDY_STA) ?
+		VB2_BUF_STATE_DONE : VB2_BUF_STATE_ERROR;
+
+	writel(0, rkvdec->regs + RKVDEC_REG_INTERRUPT);
+	if (cancel_delayed_work(&rkvdec->watchdog_work)) {
+		struct rkvdec_ctx *ctx;
+
+		ctx = v4l2_m2m_get_curr_priv(rkvdec->m2m_dev);
+		rkvdec_job_finish(ctx, state);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void rkvdec_watchdog_func(struct work_struct *work)
+{
+	struct rkvdec_dev *rkvdec;
+	struct rkvdec_ctx *ctx;
+
+	rkvdec = container_of(to_delayed_work(work), struct rkvdec_dev,
+			      watchdog_work);
+	ctx = v4l2_m2m_get_curr_priv(rkvdec->m2m_dev);
+	if (ctx) {
+		dev_err(rkvdec->dev, "Frame processing timed out!\n");
+		writel(RKVDEC_IRQ_DIS, rkvdec->regs + RKVDEC_REG_INTERRUPT);
+		writel(0, rkvdec->regs + RKVDEC_REG_SYSCTRL);
+		rkvdec_job_finish(ctx, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static const struct of_device_id of_rkvdec_match[] = {
+	{ .compatible = "rockchip,rk3399-vdec" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, of_rkvdec_match);
+
+static const char * const rkvdec_clk_names[] = {
+	"axi", "ahb", "cabac", "core"
+};
+
+static int rkvdec_probe(struct platform_device *pdev)
+{
+	struct rkvdec_dev *rkvdec;
+	unsigned int i;
+	int ret, irq;
+
+	rkvdec = devm_kzalloc(&pdev->dev, sizeof(*rkvdec), GFP_KERNEL);
+	if (!rkvdec)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, rkvdec);
+	rkvdec->dev = &pdev->dev;
+	mutex_init(&rkvdec->vdev_lock);
+	INIT_DELAYED_WORK(&rkvdec->watchdog_work, rkvdec_watchdog_func);
+
+	rkvdec->clocks = devm_kcalloc(&pdev->dev, ARRAY_SIZE(rkvdec_clk_names),
+				      sizeof(*rkvdec->clocks), GFP_KERNEL);
+	if (!rkvdec->clocks)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(rkvdec_clk_names); i++)
+		rkvdec->clocks[i].id = rkvdec_clk_names[i];
+
+	ret = devm_clk_bulk_get(&pdev->dev, ARRAY_SIZE(rkvdec_clk_names),
+				rkvdec->clocks);
+	if (ret)
+		return ret;
+
+	rkvdec->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(rkvdec->regs))
+		return PTR_ERR(rkvdec->regs);
+
+	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+	if (ret) {
+		dev_err(&pdev->dev, "Could not set DMA coherent mask.\n");
+		return ret;
+	}
+
+	vb2_dma_contig_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0)
+		return -ENXIO;
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+					rkvdec_irq_handler, IRQF_ONESHOT,
+					dev_name(&pdev->dev), rkvdec);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not request vdec IRQ\n");
+		return ret;
+	}
+
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 100);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = rkvdec_v4l2_init(rkvdec);
+	if (ret)
+		goto err_disable_runtime_pm;
+
+	return 0;
+
+err_disable_runtime_pm:
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return ret;
+}
+
+static int rkvdec_remove(struct platform_device *pdev)
+{
+	struct rkvdec_dev *rkvdec = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&rkvdec->watchdog_work);
+
+	rkvdec_v4l2_cleanup(rkvdec);
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int rkvdec_runtime_resume(struct device *dev)
+{
+	struct rkvdec_dev *rkvdec = dev_get_drvdata(dev);
+
+	return clk_bulk_prepare_enable(ARRAY_SIZE(rkvdec_clk_names),
+				       rkvdec->clocks);
+}
+
+static int rkvdec_runtime_suspend(struct device *dev)
+{
+	struct rkvdec_dev *rkvdec = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(ARRAY_SIZE(rkvdec_clk_names),
+				   rkvdec->clocks);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops rkvdec_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(rkvdec_runtime_suspend, rkvdec_runtime_resume, NULL)
+};
+
+static struct platform_driver rkvdec_driver = {
+	.probe = rkvdec_probe,
+	.remove = rkvdec_remove,
+	.driver = {
+		   .name = "rkvdec",
+		   .of_match_table = of_rkvdec_match,
+		   .pm = &rkvdec_pm_ops,
+	},
+};
+module_platform_driver(rkvdec_driver);
+
+MODULE_AUTHOR("Boris Brezillon <boris.brezillon@collabora.com>");
+MODULE_DESCRIPTION("Rockchip Video Decoder driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/rkvdec/rkvdec.h b/drivers/staging/media/rkvdec/rkvdec.h
new file mode 100644
index 000000000..633335ebb
--- /dev/null
+++ b/drivers/staging/media/rkvdec/rkvdec.h
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Rockchip Video Decoder driver
+ *
+ * Copyright (C) 2019 Collabora, Ltd.
+ *
+ * Based on rkvdec driver by Google LLC. (Tomasz Figa <tfiga@chromium.org>)
+ * Based on s5p-mfc driver by Samsung Electronics Co., Ltd.
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ */
+#ifndef RKVDEC_H_
+#define RKVDEC_H_
+
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/wait.h>
+#include <linux/clk.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+struct rkvdec_ctx;
+
+struct rkvdec_ctrl_desc {
+	struct v4l2_ctrl_config cfg;
+};
+
+struct rkvdec_ctrls {
+	const struct rkvdec_ctrl_desc *ctrls;
+	unsigned int num_ctrls;
+};
+
+struct rkvdec_run {
+	struct {
+		struct vb2_v4l2_buffer *src;
+		struct vb2_v4l2_buffer *dst;
+	} bufs;
+};
+
+struct rkvdec_vp9_decoded_buffer_info {
+	/* Info needed when the decoded frame serves as a reference frame. */
+	unsigned short width;
+	unsigned short height;
+	unsigned int bit_depth : 4;
+};
+
+struct rkvdec_decoded_buffer {
+	/* Must be the first field in this struct. */
+	struct v4l2_m2m_buffer base;
+
+	union {
+		struct rkvdec_vp9_decoded_buffer_info vp9;
+	};
+};
+
+static inline struct rkvdec_decoded_buffer *
+vb2_to_rkvdec_decoded_buf(struct vb2_buffer *buf)
+{
+	return container_of(buf, struct rkvdec_decoded_buffer,
+			    base.vb.vb2_buf);
+}
+
+struct rkvdec_coded_fmt_ops {
+	int (*adjust_fmt)(struct rkvdec_ctx *ctx,
+			  struct v4l2_format *f);
+	int (*start)(struct rkvdec_ctx *ctx);
+	void (*stop)(struct rkvdec_ctx *ctx);
+	int (*run)(struct rkvdec_ctx *ctx);
+	void (*done)(struct rkvdec_ctx *ctx, struct vb2_v4l2_buffer *src_buf,
+		     struct vb2_v4l2_buffer *dst_buf,
+		     enum vb2_buffer_state result);
+	int (*try_ctrl)(struct rkvdec_ctx *ctx, struct v4l2_ctrl *ctrl);
+};
+
+struct rkvdec_coded_fmt_desc {
+	u32 fourcc;
+	struct v4l2_frmsize_stepwise frmsize;
+	const struct rkvdec_ctrls *ctrls;
+	const struct rkvdec_coded_fmt_ops *ops;
+	unsigned int num_decoded_fmts;
+	const u32 *decoded_fmts;
+	u32 subsystem_flags;
+};
+
+struct rkvdec_dev {
+	struct v4l2_device v4l2_dev;
+	struct media_device mdev;
+	struct video_device vdev;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct device *dev;
+	struct clk_bulk_data *clocks;
+	void __iomem *regs;
+	struct mutex vdev_lock; /* serializes ioctls */
+	struct delayed_work watchdog_work;
+};
+
+struct rkvdec_ctx {
+	struct v4l2_fh fh;
+	struct v4l2_format coded_fmt;
+	struct v4l2_format decoded_fmt;
+	const struct rkvdec_coded_fmt_desc *coded_fmt_desc;
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct rkvdec_dev *dev;
+	void *priv;
+};
+
+static inline struct rkvdec_ctx *fh_to_rkvdec_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct rkvdec_ctx, fh);
+}
+
+struct rkvdec_aux_buf {
+	void *cpu;
+	dma_addr_t dma;
+	size_t size;
+};
+
+void rkvdec_run_preamble(struct rkvdec_ctx *ctx, struct rkvdec_run *run);
+void rkvdec_run_postamble(struct rkvdec_ctx *ctx, struct rkvdec_run *run);
+
+extern const struct rkvdec_coded_fmt_ops rkvdec_h264_fmt_ops;
+extern const struct rkvdec_coded_fmt_ops rkvdec_vp9_fmt_ops;
+
+#endif /* RKVDEC_H_ */
diff --git a/drivers/staging/media/sunxi/Kconfig b/drivers/staging/media/sunxi/Kconfig
new file mode 100644
index 000000000..4549a1357
--- /dev/null
+++ b/drivers/staging/media/sunxi/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_SUNXI
+	bool "Allwinner sunXi family Video Devices"
+	depends on ARCH_SUNXI || COMPILE_TEST
+	help
+	  If you have an Allwinner SoC based on the sunXi family, say Y.
+
+	  Note that this option doesn't include new drivers in the
+	  kernel: saying N will just cause Kconfig to skip all the
+	  questions about Allwinner media devices.
+
+if VIDEO_SUNXI
+
+source "drivers/staging/media/sunxi/cedrus/Kconfig"
+
+endif
diff --git a/drivers/staging/media/sunxi/Makefile b/drivers/staging/media/sunxi/Makefile
new file mode 100644
index 000000000..b87140b0e
--- /dev/null
+++ b/drivers/staging/media/sunxi/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_VIDEO_SUNXI_CEDRUS)	+= cedrus/
diff --git a/drivers/staging/media/sunxi/cedrus/Kconfig b/drivers/staging/media/sunxi/cedrus/Kconfig
new file mode 100644
index 000000000..621944f99
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_SUNXI_CEDRUS
+	tristate "Allwinner Cedrus VPU driver"
+	depends on VIDEO_DEV
+	depends on RESET_CONTROLLER
+	depends on HAS_DMA
+	depends on OF
+	select MEDIA_CONTROLLER
+	select MEDIA_CONTROLLER_REQUEST_API
+	select SUNXI_SRAM
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  Support for the VPU found in Allwinner SoCs, also known as the Cedar
+	  video engine.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called sunxi-cedrus.
diff --git a/drivers/staging/media/sunxi/cedrus/Makefile b/drivers/staging/media/sunxi/cedrus/Makefile
new file mode 100644
index 000000000..a647b3690
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_VIDEO_SUNXI_CEDRUS) += sunxi-cedrus.o
+
+sunxi-cedrus-y = cedrus.o cedrus_video.o cedrus_hw.o cedrus_dec.o \
+		 cedrus_mpeg2.o cedrus_h264.o cedrus_h265.o \
+		 cedrus_vp8.o
diff --git a/drivers/staging/media/sunxi/cedrus/TODO b/drivers/staging/media/sunxi/cedrus/TODO
new file mode 100644
index 000000000..ec277ece4
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/TODO
@@ -0,0 +1,7 @@
+Before this stateless decoder driver can leave the staging area:
+* The Request API needs to be stabilized;
+* The codec-specific controls need to be thoroughly reviewed to ensure they
+  cover all intended uses cases;
+* Userspace support for the Request API needs to be reviewed;
+* Another stateless decoder driver should be submitted;
+* At least one stateless encoder driver should be submitted.
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus.c b/drivers/staging/media/sunxi/cedrus/cedrus.c
new file mode 100644
index 000000000..d2419319a
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus.c
@@ -0,0 +1,717 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "cedrus.h"
+#include "cedrus_video.h"
+#include "cedrus_dec.h"
+#include "cedrus_hw.h"
+
+static int cedrus_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->id == V4L2_CID_STATELESS_H264_SPS) {
+		const struct v4l2_ctrl_h264_sps *sps = ctrl->p_new.p_h264_sps;
+
+		if (sps->chroma_format_idc != 1)
+			/* Only 4:2:0 is supported */
+			return -EINVAL;
+		if (sps->bit_depth_luma_minus8 != sps->bit_depth_chroma_minus8)
+			/* Luma and chroma bit depth mismatch */
+			return -EINVAL;
+		if (sps->bit_depth_luma_minus8 != 0)
+			/* Only 8-bit is supported */
+			return -EINVAL;
+	} else if (ctrl->id == V4L2_CID_STATELESS_HEVC_SPS) {
+		const struct v4l2_ctrl_hevc_sps *sps = ctrl->p_new.p_hevc_sps;
+		struct cedrus_ctx *ctx = container_of(ctrl->handler, struct cedrus_ctx, hdl);
+
+		if (sps->chroma_format_idc != 1)
+			/* Only 4:2:0 is supported */
+			return -EINVAL;
+
+		if (sps->bit_depth_luma_minus8 != sps->bit_depth_chroma_minus8)
+			/* Luma and chroma bit depth mismatch */
+			return -EINVAL;
+
+		if (ctx->dev->capabilities & CEDRUS_CAPABILITY_H265_10_DEC) {
+			if (sps->bit_depth_luma_minus8 != 0 && sps->bit_depth_luma_minus8 != 2)
+				/* Only 8-bit and 10-bit are supported */
+				return -EINVAL;
+		} else {
+			if (sps->bit_depth_luma_minus8 != 0)
+				/* Only 8-bit is supported */
+				return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops cedrus_ctrl_ops = {
+	.try_ctrl = cedrus_try_ctrl,
+};
+
+static const struct cedrus_control cedrus_controls[] = {
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_MPEG2_SEQUENCE,
+		},
+		.codec		= CEDRUS_CODEC_MPEG2,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_MPEG2_PICTURE,
+		},
+		.codec		= CEDRUS_CODEC_MPEG2,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_MPEG2_QUANTISATION,
+		},
+		.codec		= CEDRUS_CODEC_MPEG2,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_DECODE_PARAMS,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_SLICE_PARAMS,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_SPS,
+			.ops	= &cedrus_ctrl_ops,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_PPS,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_SCALING_MATRIX,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_PRED_WEIGHTS,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_DECODE_MODE,
+			.max	= V4L2_STATELESS_H264_DECODE_MODE_SLICE_BASED,
+			.def	= V4L2_STATELESS_H264_DECODE_MODE_SLICE_BASED,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_H264_START_CODE,
+			.max	= V4L2_STATELESS_H264_START_CODE_NONE,
+			.def	= V4L2_STATELESS_H264_START_CODE_NONE,
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	/*
+	 * We only expose supported profiles information,
+	 * and not levels as it's not clear what is supported
+	 * for each hardware/core version.
+	 * In any case, TRY/S_FMT will clamp the format resolution
+	 * to the maximum supported.
+	 */
+	{
+		.cfg = {
+			.id	= V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+			.min	= V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+			.def	= V4L2_MPEG_VIDEO_H264_PROFILE_MAIN,
+			.max	= V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+			.menu_skip_mask =
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED),
+		},
+		.codec		= CEDRUS_CODEC_H264,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_SPS,
+			.ops	= &cedrus_ctrl_ops,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_PPS,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_SLICE_PARAMS,
+			/* The driver can only handle 1 entry per slice for now */
+			.dims   = { 1 },
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_SCALING_MATRIX,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_ENTRY_POINT_OFFSETS,
+			/* maximum 256 entry point offsets per slice */
+			.dims	= { 256 },
+			.max = 0xffffffff,
+			.step = 1,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_DECODE_MODE,
+			.max	= V4L2_STATELESS_HEVC_DECODE_MODE_SLICE_BASED,
+			.def	= V4L2_STATELESS_HEVC_DECODE_MODE_SLICE_BASED,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_HEVC_START_CODE,
+			.max	= V4L2_STATELESS_HEVC_START_CODE_NONE,
+			.def	= V4L2_STATELESS_HEVC_START_CODE_NONE,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+	{
+		.cfg = {
+			.id	= V4L2_CID_STATELESS_VP8_FRAME,
+		},
+		.codec		= CEDRUS_CODEC_VP8,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_HEVC_DECODE_PARAMS,
+		},
+		.codec		= CEDRUS_CODEC_H265,
+	},
+};
+
+#define CEDRUS_CONTROLS_COUNT	ARRAY_SIZE(cedrus_controls)
+
+void *cedrus_find_control_data(struct cedrus_ctx *ctx, u32 id)
+{
+	unsigned int i;
+
+	for (i = 0; ctx->ctrls[i]; i++)
+		if (ctx->ctrls[i]->id == id)
+			return ctx->ctrls[i]->p_cur.p;
+
+	return NULL;
+}
+
+u32 cedrus_get_num_of_controls(struct cedrus_ctx *ctx, u32 id)
+{
+	unsigned int i;
+
+	for (i = 0; ctx->ctrls[i]; i++)
+		if (ctx->ctrls[i]->id == id)
+			return ctx->ctrls[i]->elems;
+
+	return 0;
+}
+
+static int cedrus_init_ctrls(struct cedrus_dev *dev, struct cedrus_ctx *ctx)
+{
+	struct v4l2_ctrl_handler *hdl = &ctx->hdl;
+	struct v4l2_ctrl *ctrl;
+	unsigned int ctrl_size;
+	unsigned int i;
+
+	v4l2_ctrl_handler_init(hdl, CEDRUS_CONTROLS_COUNT);
+	if (hdl->error) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to initialize control handler: %d\n",
+			 hdl->error);
+		return hdl->error;
+	}
+
+	ctrl_size = sizeof(ctrl) * CEDRUS_CONTROLS_COUNT + 1;
+
+	ctx->ctrls = kzalloc(ctrl_size, GFP_KERNEL);
+	if (!ctx->ctrls)
+		return -ENOMEM;
+
+	for (i = 0; i < CEDRUS_CONTROLS_COUNT; i++) {
+		ctrl = v4l2_ctrl_new_custom(hdl, &cedrus_controls[i].cfg,
+					    NULL);
+		if (hdl->error) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed to create %s control: %d\n",
+				 v4l2_ctrl_get_name(cedrus_controls[i].cfg.id),
+				 hdl->error);
+
+			v4l2_ctrl_handler_free(hdl);
+			kfree(ctx->ctrls);
+			ctx->ctrls = NULL;
+			return hdl->error;
+		}
+
+		ctx->ctrls[i] = ctrl;
+	}
+
+	ctx->fh.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
+
+	return 0;
+}
+
+static int cedrus_request_validate(struct media_request *req)
+{
+	struct media_request_object *obj;
+	struct cedrus_ctx *ctx = NULL;
+	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)
+		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;
+	}
+
+	return vb2_request_validate(req);
+}
+
+static int cedrus_open(struct file *file)
+{
+	struct cedrus_dev *dev = video_drvdata(file);
+	struct cedrus_ctx *ctx = NULL;
+	int ret;
+
+	if (mutex_lock_interruptible(&dev->dev_mutex))
+		return -ERESTARTSYS;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		mutex_unlock(&dev->dev_mutex);
+		return -ENOMEM;
+	}
+
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	ctx->dev = dev;
+
+	ret = cedrus_init_ctrls(dev, ctx);
+	if (ret)
+		goto err_free;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx,
+					    &cedrus_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto err_ctrls;
+	}
+	ctx->dst_fmt.pixelformat = V4L2_PIX_FMT_NV12_32L32;
+	cedrus_prepare_format(&ctx->dst_fmt);
+	ctx->src_fmt.pixelformat = V4L2_PIX_FMT_MPEG2_SLICE;
+	/*
+	 * TILED_NV12 has more strict requirements, so copy the width and
+	 * height to src_fmt to ensure that is matches the dst_fmt resolution.
+	 */
+	ctx->src_fmt.width = ctx->dst_fmt.width;
+	ctx->src_fmt.height = ctx->dst_fmt.height;
+	cedrus_prepare_format(&ctx->src_fmt);
+
+	v4l2_fh_add(&ctx->fh);
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(&ctx->hdl);
+err_free:
+	kfree(ctx);
+	mutex_unlock(&dev->dev_mutex);
+
+	return ret;
+}
+
+static int cedrus_release(struct file *file)
+{
+	struct cedrus_dev *dev = video_drvdata(file);
+	struct cedrus_ctx *ctx = container_of(file->private_data,
+					      struct cedrus_ctx, fh);
+
+	mutex_lock(&dev->dev_mutex);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	v4l2_ctrl_handler_free(&ctx->hdl);
+	kfree(ctx->ctrls);
+
+	v4l2_fh_exit(&ctx->fh);
+
+	kfree(ctx);
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations cedrus_fops = {
+	.owner		= THIS_MODULE,
+	.open		= cedrus_open,
+	.release	= cedrus_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct video_device cedrus_video_device = {
+	.name		= CEDRUS_NAME,
+	.vfl_dir	= VFL_DIR_M2M,
+	.fops		= &cedrus_fops,
+	.ioctl_ops	= &cedrus_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
+};
+
+static const struct v4l2_m2m_ops cedrus_m2m_ops = {
+	.device_run	= cedrus_device_run,
+};
+
+static const struct media_device_ops cedrus_m2m_media_ops = {
+	.req_validate	= cedrus_request_validate,
+	.req_queue	= v4l2_m2m_request_queue,
+};
+
+static int cedrus_probe(struct platform_device *pdev)
+{
+	struct cedrus_dev *dev;
+	struct video_device *vfd;
+	int ret;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, dev);
+
+	dev->vfd = cedrus_video_device;
+	dev->dev = &pdev->dev;
+	dev->pdev = pdev;
+
+	ret = cedrus_hw_probe(dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to probe hardware\n");
+		return ret;
+	}
+
+	dev->dec_ops[CEDRUS_CODEC_MPEG2] = &cedrus_dec_ops_mpeg2;
+	dev->dec_ops[CEDRUS_CODEC_H264] = &cedrus_dec_ops_h264;
+	dev->dec_ops[CEDRUS_CODEC_H265] = &cedrus_dec_ops_h265;
+	dev->dec_ops[CEDRUS_CODEC_VP8] = &cedrus_dec_ops_vp8;
+
+	mutex_init(&dev->dev_mutex);
+
+	INIT_DELAYED_WORK(&dev->watchdog_work, cedrus_watchdog);
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register V4L2 device\n");
+		return ret;
+	}
+
+	vfd = &dev->vfd;
+	vfd->lock = &dev->dev_mutex;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	snprintf(vfd->name, sizeof(vfd->name), "%s", cedrus_video_device.name);
+	video_set_drvdata(vfd, dev);
+
+	dev->m2m_dev = v4l2_m2m_init(&cedrus_m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to initialize V4L2 M2M device\n");
+		ret = PTR_ERR(dev->m2m_dev);
+
+		goto err_v4l2;
+	}
+
+	dev->mdev.dev = &pdev->dev;
+	strscpy(dev->mdev.model, CEDRUS_NAME, sizeof(dev->mdev.model));
+	strscpy(dev->mdev.bus_info, "platform:" CEDRUS_NAME,
+		sizeof(dev->mdev.bus_info));
+
+	media_device_init(&dev->mdev);
+	dev->mdev.ops = &cedrus_m2m_media_ops;
+	dev->v4l2_dev.mdev = &dev->mdev;
+
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+		goto err_m2m;
+	}
+
+	v4l2_info(&dev->v4l2_dev,
+		  "Device registered as /dev/video%d\n", vfd->num);
+
+	ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
+						 MEDIA_ENT_F_PROC_VIDEO_DECODER);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to initialize V4L2 M2M media controller\n");
+		goto err_video;
+	}
+
+	ret = media_device_register(&dev->mdev);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register media device\n");
+		goto err_m2m_mc;
+	}
+
+	return 0;
+
+err_m2m_mc:
+	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
+err_video:
+	video_unregister_device(&dev->vfd);
+err_m2m:
+	v4l2_m2m_release(dev->m2m_dev);
+err_v4l2:
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	return ret;
+}
+
+static int cedrus_remove(struct platform_device *pdev)
+{
+	struct cedrus_dev *dev = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&dev->watchdog_work);
+	if (media_devnode_is_registered(dev->mdev.devnode)) {
+		media_device_unregister(&dev->mdev);
+		v4l2_m2m_unregister_media_controller(dev->m2m_dev);
+		media_device_cleanup(&dev->mdev);
+	}
+
+	v4l2_m2m_release(dev->m2m_dev);
+	video_unregister_device(&dev->vfd);
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	cedrus_hw_remove(dev);
+
+	return 0;
+}
+
+static const struct cedrus_variant sun4i_a10_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 320000000,
+};
+
+static const struct cedrus_variant sun5i_a13_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 320000000,
+};
+
+static const struct cedrus_variant sun7i_a20_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 320000000,
+};
+
+static const struct cedrus_variant sun8i_a33_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 320000000,
+};
+
+static const struct cedrus_variant sun8i_h3_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_H265_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 402000000,
+};
+
+static const struct cedrus_variant sun8i_v3s_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_H264_DEC,
+	.mod_rate	= 297000000,
+};
+
+static const struct cedrus_variant sun8i_r40_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 297000000,
+};
+
+static const struct cedrus_variant sun20i_d1_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_H265_DEC,
+	.mod_rate	= 432000000,
+};
+
+static const struct cedrus_variant sun50i_a64_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_H265_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 402000000,
+};
+
+static const struct cedrus_variant sun50i_h5_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_H265_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 402000000,
+};
+
+static const struct cedrus_variant sun50i_h6_cedrus_variant = {
+	.capabilities	= CEDRUS_CAPABILITY_UNTILED |
+			  CEDRUS_CAPABILITY_MPEG2_DEC |
+			  CEDRUS_CAPABILITY_H264_DEC |
+			  CEDRUS_CAPABILITY_H265_DEC |
+			  CEDRUS_CAPABILITY_H265_10_DEC |
+			  CEDRUS_CAPABILITY_VP8_DEC,
+	.mod_rate	= 600000000,
+};
+
+static const struct of_device_id cedrus_dt_match[] = {
+	{
+		.compatible = "allwinner,sun4i-a10-video-engine",
+		.data = &sun4i_a10_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun5i-a13-video-engine",
+		.data = &sun5i_a13_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun7i-a20-video-engine",
+		.data = &sun7i_a20_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun8i-a33-video-engine",
+		.data = &sun8i_a33_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun8i-h3-video-engine",
+		.data = &sun8i_h3_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun8i-v3s-video-engine",
+		.data = &sun8i_v3s_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun8i-r40-video-engine",
+		.data = &sun8i_r40_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun20i-d1-video-engine",
+		.data = &sun20i_d1_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun50i-a64-video-engine",
+		.data = &sun50i_a64_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun50i-h5-video-engine",
+		.data = &sun50i_h5_cedrus_variant,
+	},
+	{
+		.compatible = "allwinner,sun50i-h6-video-engine",
+		.data = &sun50i_h6_cedrus_variant,
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, cedrus_dt_match);
+
+static const struct dev_pm_ops cedrus_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(cedrus_hw_suspend,
+			   cedrus_hw_resume, NULL)
+};
+
+static struct platform_driver cedrus_driver = {
+	.probe		= cedrus_probe,
+	.remove		= cedrus_remove,
+	.driver		= {
+		.name		= CEDRUS_NAME,
+		.of_match_table	= of_match_ptr(cedrus_dt_match),
+		.pm		= &cedrus_dev_pm_ops,
+	},
+};
+module_platform_driver(cedrus_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Florent Revest <florent.revest@free-electrons.com>");
+MODULE_AUTHOR("Paul Kocialkowski <paul.kocialkowski@bootlin.com>");
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
+MODULE_DESCRIPTION("Cedrus VPU driver");
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus.h b/drivers/staging/media/sunxi/cedrus/cedrus.h
new file mode 100644
index 000000000..93a219600
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus.h
@@ -0,0 +1,274 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#ifndef _CEDRUS_H_
+#define _CEDRUS_H_
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include <linux/iopoll.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+
+#define CEDRUS_NAME			"cedrus"
+
+#define CEDRUS_CAPABILITY_UNTILED	BIT(0)
+#define CEDRUS_CAPABILITY_H265_DEC	BIT(1)
+#define CEDRUS_CAPABILITY_H264_DEC	BIT(2)
+#define CEDRUS_CAPABILITY_MPEG2_DEC	BIT(3)
+#define CEDRUS_CAPABILITY_VP8_DEC	BIT(4)
+#define CEDRUS_CAPABILITY_H265_10_DEC	BIT(5)
+
+enum cedrus_codec {
+	CEDRUS_CODEC_MPEG2,
+	CEDRUS_CODEC_H264,
+	CEDRUS_CODEC_H265,
+	CEDRUS_CODEC_VP8,
+	CEDRUS_CODEC_LAST,
+};
+
+enum cedrus_irq_status {
+	CEDRUS_IRQ_NONE,
+	CEDRUS_IRQ_ERROR,
+	CEDRUS_IRQ_OK,
+};
+
+enum cedrus_h264_pic_type {
+	CEDRUS_H264_PIC_TYPE_FRAME	= 0,
+	CEDRUS_H264_PIC_TYPE_FIELD,
+	CEDRUS_H264_PIC_TYPE_MBAFF,
+};
+
+struct cedrus_control {
+	struct v4l2_ctrl_config cfg;
+	enum cedrus_codec	codec;
+};
+
+struct cedrus_h264_run {
+	const struct v4l2_ctrl_h264_decode_params	*decode_params;
+	const struct v4l2_ctrl_h264_pps			*pps;
+	const struct v4l2_ctrl_h264_scaling_matrix	*scaling_matrix;
+	const struct v4l2_ctrl_h264_slice_params	*slice_params;
+	const struct v4l2_ctrl_h264_sps			*sps;
+	const struct v4l2_ctrl_h264_pred_weights	*pred_weights;
+};
+
+struct cedrus_mpeg2_run {
+	const struct v4l2_ctrl_mpeg2_sequence		*sequence;
+	const struct v4l2_ctrl_mpeg2_picture		*picture;
+	const struct v4l2_ctrl_mpeg2_quantisation	*quantisation;
+};
+
+struct cedrus_h265_run {
+	const struct v4l2_ctrl_hevc_sps			*sps;
+	const struct v4l2_ctrl_hevc_pps			*pps;
+	const struct v4l2_ctrl_hevc_slice_params	*slice_params;
+	const struct v4l2_ctrl_hevc_decode_params	*decode_params;
+	const struct v4l2_ctrl_hevc_scaling_matrix	*scaling_matrix;
+	const u32					*entry_points;
+	u32						entry_points_count;
+};
+
+struct cedrus_vp8_run {
+	const struct v4l2_ctrl_vp8_frame		*frame_params;
+};
+
+struct cedrus_run {
+	struct vb2_v4l2_buffer	*src;
+	struct vb2_v4l2_buffer	*dst;
+
+	union {
+		struct cedrus_h264_run	h264;
+		struct cedrus_mpeg2_run	mpeg2;
+		struct cedrus_h265_run	h265;
+		struct cedrus_vp8_run	vp8;
+	};
+};
+
+struct cedrus_buffer {
+	struct v4l2_m2m_buffer          m2m_buf;
+
+	union {
+		struct {
+			unsigned int			position;
+			enum cedrus_h264_pic_type	pic_type;
+		} h264;
+	} codec;
+};
+
+struct cedrus_ctx {
+	struct v4l2_fh			fh;
+	struct cedrus_dev		*dev;
+
+	struct v4l2_pix_format		src_fmt;
+	struct v4l2_pix_format		dst_fmt;
+	enum cedrus_codec		current_codec;
+
+	struct v4l2_ctrl_handler	hdl;
+	struct v4l2_ctrl		**ctrls;
+
+	union {
+		struct {
+			void		*mv_col_buf;
+			dma_addr_t	mv_col_buf_dma;
+			ssize_t		mv_col_buf_field_size;
+			ssize_t		mv_col_buf_size;
+			void		*pic_info_buf;
+			dma_addr_t	pic_info_buf_dma;
+			ssize_t		pic_info_buf_size;
+			void		*neighbor_info_buf;
+			dma_addr_t	neighbor_info_buf_dma;
+			void		*deblk_buf;
+			dma_addr_t	deblk_buf_dma;
+			ssize_t		deblk_buf_size;
+			void		*intra_pred_buf;
+			dma_addr_t	intra_pred_buf_dma;
+			ssize_t		intra_pred_buf_size;
+		} h264;
+		struct {
+			void		*mv_col_buf;
+			dma_addr_t	mv_col_buf_addr;
+			ssize_t		mv_col_buf_size;
+			ssize_t		mv_col_buf_unit_size;
+			void		*neighbor_info_buf;
+			dma_addr_t	neighbor_info_buf_addr;
+			void		*entry_points_buf;
+			dma_addr_t	entry_points_buf_addr;
+		} h265;
+		struct {
+			unsigned int	last_frame_p_type;
+			unsigned int	last_filter_type;
+			unsigned int	last_sharpness_level;
+
+			u8		*entropy_probs_buf;
+			dma_addr_t	entropy_probs_buf_dma;
+		} vp8;
+	} codec;
+};
+
+struct cedrus_dec_ops {
+	void (*irq_clear)(struct cedrus_ctx *ctx);
+	void (*irq_disable)(struct cedrus_ctx *ctx);
+	enum cedrus_irq_status (*irq_status)(struct cedrus_ctx *ctx);
+	int (*setup)(struct cedrus_ctx *ctx, struct cedrus_run *run);
+	int (*start)(struct cedrus_ctx *ctx);
+	void (*stop)(struct cedrus_ctx *ctx);
+	void (*trigger)(struct cedrus_ctx *ctx);
+};
+
+struct cedrus_variant {
+	unsigned int	capabilities;
+	unsigned int	mod_rate;
+};
+
+struct cedrus_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd;
+	struct media_device	mdev;
+	struct media_pad	pad[2];
+	struct platform_device	*pdev;
+	struct device		*dev;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct cedrus_dec_ops	*dec_ops[CEDRUS_CODEC_LAST];
+
+	/* Device file mutex */
+	struct mutex		dev_mutex;
+
+	void __iomem		*base;
+
+	struct clk		*mod_clk;
+	struct clk		*ahb_clk;
+	struct clk		*ram_clk;
+
+	struct reset_control	*rstc;
+
+	unsigned int		capabilities;
+
+	struct delayed_work	watchdog_work;
+};
+
+extern struct cedrus_dec_ops cedrus_dec_ops_mpeg2;
+extern struct cedrus_dec_ops cedrus_dec_ops_h264;
+extern struct cedrus_dec_ops cedrus_dec_ops_h265;
+extern struct cedrus_dec_ops cedrus_dec_ops_vp8;
+
+static inline void cedrus_write(struct cedrus_dev *dev, u32 reg, u32 val)
+{
+	writel(val, dev->base + reg);
+}
+
+static inline u32 cedrus_read(struct cedrus_dev *dev, u32 reg)
+{
+	return readl(dev->base + reg);
+}
+
+static inline u32 cedrus_wait_for(struct cedrus_dev *dev, u32 reg, u32 flag)
+{
+	u32 value;
+
+	return readl_poll_timeout_atomic(dev->base + reg, value,
+			(value & flag) == 0, 10, 1000);
+}
+
+static inline dma_addr_t cedrus_buf_addr(struct vb2_buffer *buf,
+					 struct v4l2_pix_format *pix_fmt,
+					 unsigned int plane)
+{
+	dma_addr_t addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+
+	return addr + (pix_fmt ? (dma_addr_t)pix_fmt->bytesperline *
+	       pix_fmt->height * plane : 0);
+}
+
+static inline dma_addr_t cedrus_dst_buf_addr(struct cedrus_ctx *ctx,
+					     struct vb2_buffer *buf,
+					     unsigned int plane)
+{
+	return buf ? cedrus_buf_addr(buf, &ctx->dst_fmt, plane) : 0;
+}
+
+static inline void cedrus_write_ref_buf_addr(struct cedrus_ctx *ctx,
+					     struct vb2_queue *q,
+					     u64 timestamp,
+					     u32 luma_reg,
+					     u32 chroma_reg)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	struct vb2_buffer *buf = vb2_find_buffer(q, timestamp);
+
+	cedrus_write(dev, luma_reg, cedrus_dst_buf_addr(ctx, buf, 0));
+	cedrus_write(dev, chroma_reg, cedrus_dst_buf_addr(ctx, buf, 1));
+}
+
+static inline struct cedrus_buffer *
+vb2_v4l2_to_cedrus_buffer(const struct vb2_v4l2_buffer *p)
+{
+	return container_of(p, struct cedrus_buffer, m2m_buf.vb);
+}
+
+static inline struct cedrus_buffer *
+vb2_to_cedrus_buffer(const struct vb2_buffer *p)
+{
+	return vb2_v4l2_to_cedrus_buffer(to_vb2_v4l2_buffer(p));
+}
+
+void *cedrus_find_control_data(struct cedrus_ctx *ctx, u32 id);
+u32 cedrus_get_num_of_controls(struct cedrus_ctx *ctx, u32 id);
+
+#endif
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_dec.c b/drivers/staging/media/sunxi/cedrus/cedrus_dec.c
new file mode 100644
index 000000000..e7f7602a5
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_dec.c
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "cedrus.h"
+#include "cedrus_dec.h"
+#include "cedrus_hw.h"
+
+void cedrus_device_run(void *priv)
+{
+	struct cedrus_ctx *ctx = priv;
+	struct cedrus_dev *dev = ctx->dev;
+	struct cedrus_run run = {};
+	struct media_request *src_req;
+	int error;
+
+	run.src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	run.dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	/* Apply request(s) controls if needed. */
+	src_req = run.src->vb2_buf.req_obj.req;
+
+	if (src_req)
+		v4l2_ctrl_request_setup(src_req, &ctx->hdl);
+
+	switch (ctx->src_fmt.pixelformat) {
+	case V4L2_PIX_FMT_MPEG2_SLICE:
+		run.mpeg2.sequence = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_MPEG2_SEQUENCE);
+		run.mpeg2.picture = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_MPEG2_PICTURE);
+		run.mpeg2.quantisation = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_MPEG2_QUANTISATION);
+		break;
+
+	case V4L2_PIX_FMT_H264_SLICE:
+		run.h264.decode_params = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+		run.h264.pps = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_PPS);
+		run.h264.scaling_matrix = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_SCALING_MATRIX);
+		run.h264.slice_params = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_SLICE_PARAMS);
+		run.h264.sps = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_SPS);
+		run.h264.pred_weights = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_H264_PRED_WEIGHTS);
+		break;
+
+	case V4L2_PIX_FMT_HEVC_SLICE:
+		run.h265.sps = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_SPS);
+		run.h265.pps = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_PPS);
+		run.h265.slice_params = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_SLICE_PARAMS);
+		run.h265.decode_params = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_DECODE_PARAMS);
+		run.h265.scaling_matrix = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_SCALING_MATRIX);
+		run.h265.entry_points = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_HEVC_ENTRY_POINT_OFFSETS);
+		run.h265.entry_points_count = cedrus_get_num_of_controls(ctx,
+			V4L2_CID_STATELESS_HEVC_ENTRY_POINT_OFFSETS);
+		break;
+
+	case V4L2_PIX_FMT_VP8_FRAME:
+		run.vp8.frame_params = cedrus_find_control_data(ctx,
+			V4L2_CID_STATELESS_VP8_FRAME);
+		break;
+
+	default:
+		break;
+	}
+
+	v4l2_m2m_buf_copy_metadata(run.src, run.dst, true);
+
+	cedrus_dst_format_set(dev, &ctx->dst_fmt);
+
+	error = dev->dec_ops[ctx->current_codec]->setup(ctx, &run);
+	if (error)
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Failed to setup decoding job: %d\n", error);
+
+	/* Complete request(s) controls if needed. */
+
+	if (src_req)
+		v4l2_ctrl_request_complete(src_req, &ctx->hdl);
+
+	/* Trigger decoding if setup went well, bail out otherwise. */
+	if (!error) {
+		/* Start the watchdog timer. */
+		schedule_delayed_work(&dev->watchdog_work,
+				      msecs_to_jiffies(2000));
+
+		dev->dec_ops[ctx->current_codec]->trigger(ctx);
+	} else {
+		v4l2_m2m_buf_done_and_job_finish(ctx->dev->m2m_dev,
+						 ctx->fh.m2m_ctx,
+						 VB2_BUF_STATE_ERROR);
+	}
+}
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_dec.h b/drivers/staging/media/sunxi/cedrus/cedrus_dec.h
new file mode 100644
index 000000000..d1ae79036
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_dec.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#ifndef _CEDRUS_DEC_H_
+#define _CEDRUS_DEC_H_
+
+void cedrus_device_run(void *priv);
+
+#endif
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_h264.c b/drivers/staging/media/sunxi/cedrus/cedrus_h264.c
new file mode 100644
index 000000000..a8b236cd3
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_h264.c
@@ -0,0 +1,708 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (c) 2013 Jens Kuske <jenskuske@gmail.com>
+ * Copyright (c) 2018 Bootlin
+ */
+
+#include <linux/delay.h>
+#include <linux/types.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cedrus.h"
+#include "cedrus_hw.h"
+#include "cedrus_regs.h"
+
+enum cedrus_h264_sram_off {
+	CEDRUS_SRAM_H264_PRED_WEIGHT_TABLE	= 0x000,
+	CEDRUS_SRAM_H264_FRAMEBUFFER_LIST	= 0x100,
+	CEDRUS_SRAM_H264_REF_LIST_0		= 0x190,
+	CEDRUS_SRAM_H264_REF_LIST_1		= 0x199,
+	CEDRUS_SRAM_H264_SCALING_LIST_8x8_0	= 0x200,
+	CEDRUS_SRAM_H264_SCALING_LIST_8x8_1	= 0x210,
+	CEDRUS_SRAM_H264_SCALING_LIST_4x4	= 0x220,
+};
+
+struct cedrus_h264_sram_ref_pic {
+	__le32	top_field_order_cnt;
+	__le32	bottom_field_order_cnt;
+	__le32	frame_info;
+	__le32	luma_ptr;
+	__le32	chroma_ptr;
+	__le32	mv_col_top_ptr;
+	__le32	mv_col_bot_ptr;
+	__le32	reserved;
+} __packed;
+
+#define CEDRUS_H264_FRAME_NUM		18
+
+#define CEDRUS_NEIGHBOR_INFO_BUF_SIZE	(32 * SZ_1K)
+#define CEDRUS_MIN_PIC_INFO_BUF_SIZE       (130 * SZ_1K)
+
+static void cedrus_h264_write_sram(struct cedrus_dev *dev,
+				   enum cedrus_h264_sram_off off,
+				   const void *data, size_t len)
+{
+	const u32 *buffer = data;
+	size_t count = DIV_ROUND_UP(len, 4);
+
+	cedrus_write(dev, VE_AVC_SRAM_PORT_OFFSET, off << 2);
+
+	while (count--)
+		cedrus_write(dev, VE_AVC_SRAM_PORT_DATA, *buffer++);
+}
+
+static dma_addr_t cedrus_h264_mv_col_buf_addr(struct cedrus_ctx *ctx,
+					      unsigned int position,
+					      unsigned int field)
+{
+	dma_addr_t addr = ctx->codec.h264.mv_col_buf_dma;
+
+	/* Adjust for the position */
+	addr += position * ctx->codec.h264.mv_col_buf_field_size * 2;
+
+	/* Adjust for the field */
+	addr += field * ctx->codec.h264.mv_col_buf_field_size;
+
+	return addr;
+}
+
+static void cedrus_fill_ref_pic(struct cedrus_ctx *ctx,
+				struct cedrus_buffer *buf,
+				unsigned int top_field_order_cnt,
+				unsigned int bottom_field_order_cnt,
+				struct cedrus_h264_sram_ref_pic *pic)
+{
+	struct vb2_buffer *vbuf = &buf->m2m_buf.vb.vb2_buf;
+	unsigned int position = buf->codec.h264.position;
+
+	pic->top_field_order_cnt = cpu_to_le32(top_field_order_cnt);
+	pic->bottom_field_order_cnt = cpu_to_le32(bottom_field_order_cnt);
+	pic->frame_info = cpu_to_le32(buf->codec.h264.pic_type << 8);
+
+	pic->luma_ptr = cpu_to_le32(cedrus_buf_addr(vbuf, &ctx->dst_fmt, 0));
+	pic->chroma_ptr = cpu_to_le32(cedrus_buf_addr(vbuf, &ctx->dst_fmt, 1));
+	pic->mv_col_top_ptr =
+		cpu_to_le32(cedrus_h264_mv_col_buf_addr(ctx, position, 0));
+	pic->mv_col_bot_ptr =
+		cpu_to_le32(cedrus_h264_mv_col_buf_addr(ctx, position, 1));
+}
+
+static void cedrus_write_frame_list(struct cedrus_ctx *ctx,
+				    struct cedrus_run *run)
+{
+	struct cedrus_h264_sram_ref_pic pic_list[CEDRUS_H264_FRAME_NUM];
+	const struct v4l2_ctrl_h264_decode_params *decode = run->h264.decode_params;
+	const struct v4l2_ctrl_h264_sps *sps = run->h264.sps;
+	struct vb2_queue *cap_q;
+	struct cedrus_buffer *output_buf;
+	struct cedrus_dev *dev = ctx->dev;
+	unsigned long used_dpbs = 0;
+	unsigned int position;
+	int output = -1;
+	unsigned int i;
+
+	cap_q = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	memset(pic_list, 0, sizeof(pic_list));
+
+	for (i = 0; i < ARRAY_SIZE(decode->dpb); i++) {
+		const struct v4l2_h264_dpb_entry *dpb = &decode->dpb[i];
+		struct cedrus_buffer *cedrus_buf;
+		struct vb2_buffer *buf;
+
+		if (!(dpb->flags & V4L2_H264_DPB_ENTRY_FLAG_VALID))
+			continue;
+
+		buf = vb2_find_buffer(cap_q, dpb->reference_ts);
+		if (!buf)
+			continue;
+
+		cedrus_buf = vb2_to_cedrus_buffer(buf);
+		position = cedrus_buf->codec.h264.position;
+		used_dpbs |= BIT(position);
+
+		if (run->dst->vb2_buf.timestamp == dpb->reference_ts) {
+			output = position;
+			continue;
+		}
+
+		if (!(dpb->flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE))
+			continue;
+
+		cedrus_fill_ref_pic(ctx, cedrus_buf,
+				    dpb->top_field_order_cnt,
+				    dpb->bottom_field_order_cnt,
+				    &pic_list[position]);
+	}
+
+	if (output >= 0)
+		position = output;
+	else
+		position = find_first_zero_bit(&used_dpbs, CEDRUS_H264_FRAME_NUM);
+
+	output_buf = vb2_to_cedrus_buffer(&run->dst->vb2_buf);
+	output_buf->codec.h264.position = position;
+
+	if (decode->flags & V4L2_H264_DECODE_PARAM_FLAG_FIELD_PIC)
+		output_buf->codec.h264.pic_type = CEDRUS_H264_PIC_TYPE_FIELD;
+	else if (sps->flags & V4L2_H264_SPS_FLAG_MB_ADAPTIVE_FRAME_FIELD)
+		output_buf->codec.h264.pic_type = CEDRUS_H264_PIC_TYPE_MBAFF;
+	else
+		output_buf->codec.h264.pic_type = CEDRUS_H264_PIC_TYPE_FRAME;
+
+	cedrus_fill_ref_pic(ctx, output_buf,
+			    decode->top_field_order_cnt,
+			    decode->bottom_field_order_cnt,
+			    &pic_list[position]);
+
+	cedrus_h264_write_sram(dev, CEDRUS_SRAM_H264_FRAMEBUFFER_LIST,
+			       pic_list, sizeof(pic_list));
+
+	cedrus_write(dev, VE_H264_OUTPUT_FRAME_IDX, position);
+}
+
+#define CEDRUS_MAX_REF_IDX	32
+
+static void _cedrus_write_ref_list(struct cedrus_ctx *ctx,
+				   struct cedrus_run *run,
+				   const struct v4l2_h264_reference *ref_list,
+				   u8 num_ref, enum cedrus_h264_sram_off sram)
+{
+	const struct v4l2_ctrl_h264_decode_params *decode = run->h264.decode_params;
+	struct vb2_queue *cap_q;
+	struct cedrus_dev *dev = ctx->dev;
+	u8 sram_array[CEDRUS_MAX_REF_IDX];
+	unsigned int i;
+	size_t size;
+
+	cap_q = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	memset(sram_array, 0, sizeof(sram_array));
+
+	for (i = 0; i < num_ref; i++) {
+		const struct v4l2_h264_dpb_entry *dpb;
+		const struct cedrus_buffer *cedrus_buf;
+		unsigned int position;
+		struct vb2_buffer *buf;
+		u8 dpb_idx;
+
+		dpb_idx = ref_list[i].index;
+		dpb = &decode->dpb[dpb_idx];
+
+		if (!(dpb->flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE))
+			continue;
+
+		buf = vb2_find_buffer(cap_q, dpb->reference_ts);
+		if (!buf)
+			continue;
+
+		cedrus_buf = vb2_to_cedrus_buffer(buf);
+		position = cedrus_buf->codec.h264.position;
+
+		sram_array[i] |= position << 1;
+		if (ref_list[i].fields == V4L2_H264_BOTTOM_FIELD_REF)
+			sram_array[i] |= BIT(0);
+	}
+
+	size = min_t(size_t, ALIGN(num_ref, 4), sizeof(sram_array));
+	cedrus_h264_write_sram(dev, sram, &sram_array, size);
+}
+
+static void cedrus_write_ref_list0(struct cedrus_ctx *ctx,
+				   struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_h264_slice_params *slice = run->h264.slice_params;
+
+	_cedrus_write_ref_list(ctx, run,
+			       slice->ref_pic_list0,
+			       slice->num_ref_idx_l0_active_minus1 + 1,
+			       CEDRUS_SRAM_H264_REF_LIST_0);
+}
+
+static void cedrus_write_ref_list1(struct cedrus_ctx *ctx,
+				   struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_h264_slice_params *slice = run->h264.slice_params;
+
+	_cedrus_write_ref_list(ctx, run,
+			       slice->ref_pic_list1,
+			       slice->num_ref_idx_l1_active_minus1 + 1,
+			       CEDRUS_SRAM_H264_REF_LIST_1);
+}
+
+static void cedrus_write_scaling_lists(struct cedrus_ctx *ctx,
+				       struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_h264_scaling_matrix *scaling =
+		run->h264.scaling_matrix;
+	const struct v4l2_ctrl_h264_pps *pps = run->h264.pps;
+	struct cedrus_dev *dev = ctx->dev;
+
+	if (!(pps->flags & V4L2_H264_PPS_FLAG_SCALING_MATRIX_PRESENT))
+		return;
+
+	cedrus_h264_write_sram(dev, CEDRUS_SRAM_H264_SCALING_LIST_8x8_0,
+			       scaling->scaling_list_8x8[0],
+			       sizeof(scaling->scaling_list_8x8[0]));
+
+	cedrus_h264_write_sram(dev, CEDRUS_SRAM_H264_SCALING_LIST_8x8_1,
+			       scaling->scaling_list_8x8[1],
+			       sizeof(scaling->scaling_list_8x8[1]));
+
+	cedrus_h264_write_sram(dev, CEDRUS_SRAM_H264_SCALING_LIST_4x4,
+			       scaling->scaling_list_4x4,
+			       sizeof(scaling->scaling_list_4x4));
+}
+
+static void cedrus_write_pred_weight_table(struct cedrus_ctx *ctx,
+					   struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_h264_pred_weights *pred_weight =
+		run->h264.pred_weights;
+	struct cedrus_dev *dev = ctx->dev;
+	int i, j, k;
+
+	cedrus_write(dev, VE_H264_SHS_WP,
+		     ((pred_weight->chroma_log2_weight_denom & 0x7) << 4) |
+		     ((pred_weight->luma_log2_weight_denom & 0x7) << 0));
+
+	cedrus_write(dev, VE_AVC_SRAM_PORT_OFFSET,
+		     CEDRUS_SRAM_H264_PRED_WEIGHT_TABLE << 2);
+
+	for (i = 0; i < ARRAY_SIZE(pred_weight->weight_factors); i++) {
+		const struct v4l2_h264_weight_factors *factors =
+			&pred_weight->weight_factors[i];
+
+		for (j = 0; j < ARRAY_SIZE(factors->luma_weight); j++) {
+			u32 val;
+
+			val = (((u32)factors->luma_offset[j] & 0x1ff) << 16) |
+				(factors->luma_weight[j] & 0x1ff);
+			cedrus_write(dev, VE_AVC_SRAM_PORT_DATA, val);
+		}
+
+		for (j = 0; j < ARRAY_SIZE(factors->chroma_weight); j++) {
+			for (k = 0; k < ARRAY_SIZE(factors->chroma_weight[0]); k++) {
+				u32 val;
+
+				val = (((u32)factors->chroma_offset[j][k] & 0x1ff) << 16) |
+					(factors->chroma_weight[j][k] & 0x1ff);
+				cedrus_write(dev, VE_AVC_SRAM_PORT_DATA, val);
+			}
+		}
+	}
+}
+
+/*
+ * It turns out that using VE_H264_VLD_OFFSET to skip bits is not reliable. In
+ * rare cases frame is not decoded correctly. However, setting offset to 0 and
+ * skipping appropriate amount of bits with flush bits trigger always works.
+ */
+static void cedrus_skip_bits(struct cedrus_dev *dev, int num)
+{
+	int count = 0;
+
+	while (count < num) {
+		int tmp = min(num - count, 32);
+
+		cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+			     VE_H264_TRIGGER_TYPE_FLUSH_BITS |
+			     VE_H264_TRIGGER_TYPE_N_BITS(tmp));
+		while (cedrus_read(dev, VE_H264_STATUS) & VE_H264_STATUS_VLD_BUSY)
+			udelay(1);
+
+		count += tmp;
+	}
+}
+
+static void cedrus_set_params(struct cedrus_ctx *ctx,
+			      struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_h264_decode_params *decode = run->h264.decode_params;
+	const struct v4l2_ctrl_h264_slice_params *slice = run->h264.slice_params;
+	const struct v4l2_ctrl_h264_pps *pps = run->h264.pps;
+	const struct v4l2_ctrl_h264_sps *sps = run->h264.sps;
+	struct vb2_buffer *src_buf = &run->src->vb2_buf;
+	struct cedrus_dev *dev = ctx->dev;
+	dma_addr_t src_buf_addr;
+	size_t slice_bytes = vb2_get_plane_payload(src_buf, 0);
+	unsigned int pic_width_in_mbs;
+	bool mbaff_pic;
+	u32 reg;
+
+	cedrus_write(dev, VE_H264_VLD_LEN, slice_bytes * 8);
+	cedrus_write(dev, VE_H264_VLD_OFFSET, 0);
+
+	src_buf_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	cedrus_write(dev, VE_H264_VLD_END, src_buf_addr + slice_bytes);
+	cedrus_write(dev, VE_H264_VLD_ADDR,
+		     VE_H264_VLD_ADDR_VAL(src_buf_addr) |
+		     VE_H264_VLD_ADDR_FIRST | VE_H264_VLD_ADDR_VALID |
+		     VE_H264_VLD_ADDR_LAST);
+
+	if (ctx->src_fmt.width > 2048) {
+		cedrus_write(dev, VE_BUF_CTRL,
+			     VE_BUF_CTRL_INTRAPRED_MIXED_RAM |
+			     VE_BUF_CTRL_DBLK_MIXED_RAM);
+		cedrus_write(dev, VE_DBLK_DRAM_BUF_ADDR,
+			     ctx->codec.h264.deblk_buf_dma);
+		cedrus_write(dev, VE_INTRAPRED_DRAM_BUF_ADDR,
+			     ctx->codec.h264.intra_pred_buf_dma);
+	} else {
+		cedrus_write(dev, VE_BUF_CTRL,
+			     VE_BUF_CTRL_INTRAPRED_INT_SRAM |
+			     VE_BUF_CTRL_DBLK_INT_SRAM);
+	}
+
+	/*
+	 * FIXME: Since the bitstream parsing is done in software, and
+	 * in userspace, this shouldn't be needed anymore. But it
+	 * turns out that removing it breaks the decoding process,
+	 * without any clear indication why.
+	 */
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+		     VE_H264_TRIGGER_TYPE_INIT_SWDEC);
+
+	cedrus_skip_bits(dev, slice->header_bit_size);
+
+	if (V4L2_H264_CTRL_PRED_WEIGHTS_REQUIRED(pps, slice))
+		cedrus_write_pred_weight_table(ctx, run);
+
+	if ((slice->slice_type == V4L2_H264_SLICE_TYPE_P) ||
+	    (slice->slice_type == V4L2_H264_SLICE_TYPE_SP) ||
+	    (slice->slice_type == V4L2_H264_SLICE_TYPE_B))
+		cedrus_write_ref_list0(ctx, run);
+
+	if (slice->slice_type == V4L2_H264_SLICE_TYPE_B)
+		cedrus_write_ref_list1(ctx, run);
+
+	// picture parameters
+	reg = 0;
+	/*
+	 * FIXME: the kernel headers are allowing the default value to
+	 * be passed, but the libva doesn't give us that.
+	 */
+	reg |= (slice->num_ref_idx_l0_active_minus1 & 0x1f) << 10;
+	reg |= (slice->num_ref_idx_l1_active_minus1 & 0x1f) << 5;
+	reg |= (pps->weighted_bipred_idc & 0x3) << 2;
+	if (pps->flags & V4L2_H264_PPS_FLAG_ENTROPY_CODING_MODE)
+		reg |= VE_H264_PPS_ENTROPY_CODING_MODE;
+	if (pps->flags & V4L2_H264_PPS_FLAG_WEIGHTED_PRED)
+		reg |= VE_H264_PPS_WEIGHTED_PRED;
+	if (pps->flags & V4L2_H264_PPS_FLAG_CONSTRAINED_INTRA_PRED)
+		reg |= VE_H264_PPS_CONSTRAINED_INTRA_PRED;
+	if (pps->flags & V4L2_H264_PPS_FLAG_TRANSFORM_8X8_MODE)
+		reg |= VE_H264_PPS_TRANSFORM_8X8_MODE;
+	cedrus_write(dev, VE_H264_PPS, reg);
+
+	// sequence parameters
+	reg = 0;
+	reg |= (sps->chroma_format_idc & 0x7) << 19;
+	reg |= (sps->pic_width_in_mbs_minus1 & 0xff) << 8;
+	reg |= sps->pic_height_in_map_units_minus1 & 0xff;
+	if (sps->flags & V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY)
+		reg |= VE_H264_SPS_MBS_ONLY;
+	if (sps->flags & V4L2_H264_SPS_FLAG_MB_ADAPTIVE_FRAME_FIELD)
+		reg |= VE_H264_SPS_MB_ADAPTIVE_FRAME_FIELD;
+	if (sps->flags & V4L2_H264_SPS_FLAG_DIRECT_8X8_INFERENCE)
+		reg |= VE_H264_SPS_DIRECT_8X8_INFERENCE;
+	cedrus_write(dev, VE_H264_SPS, reg);
+
+	mbaff_pic = !(decode->flags & V4L2_H264_DECODE_PARAM_FLAG_FIELD_PIC) &&
+		    (sps->flags & V4L2_H264_SPS_FLAG_MB_ADAPTIVE_FRAME_FIELD);
+	pic_width_in_mbs = sps->pic_width_in_mbs_minus1 + 1;
+
+	// slice parameters
+	reg = 0;
+	reg |= ((slice->first_mb_in_slice % pic_width_in_mbs) & 0xff) << 24;
+	reg |= (((slice->first_mb_in_slice / pic_width_in_mbs) *
+		 (mbaff_pic + 1)) & 0xff) << 16;
+	reg |= decode->nal_ref_idc ? BIT(12) : 0;
+	reg |= (slice->slice_type & 0xf) << 8;
+	reg |= slice->cabac_init_idc & 0x3;
+	if (ctx->fh.m2m_ctx->new_frame)
+		reg |= VE_H264_SHS_FIRST_SLICE_IN_PIC;
+	if (decode->flags & V4L2_H264_DECODE_PARAM_FLAG_FIELD_PIC)
+		reg |= VE_H264_SHS_FIELD_PIC;
+	if (decode->flags & V4L2_H264_DECODE_PARAM_FLAG_BOTTOM_FIELD)
+		reg |= VE_H264_SHS_BOTTOM_FIELD;
+	if (slice->flags & V4L2_H264_SLICE_FLAG_DIRECT_SPATIAL_MV_PRED)
+		reg |= VE_H264_SHS_DIRECT_SPATIAL_MV_PRED;
+	cedrus_write(dev, VE_H264_SHS, reg);
+
+	reg = 0;
+	reg |= VE_H264_SHS2_NUM_REF_IDX_ACTIVE_OVRD;
+	reg |= (slice->num_ref_idx_l0_active_minus1 & 0x1f) << 24;
+	reg |= (slice->num_ref_idx_l1_active_minus1 & 0x1f) << 16;
+	reg |= (slice->disable_deblocking_filter_idc & 0x3) << 8;
+	reg |= (slice->slice_alpha_c0_offset_div2 & 0xf) << 4;
+	reg |= slice->slice_beta_offset_div2 & 0xf;
+	cedrus_write(dev, VE_H264_SHS2, reg);
+
+	reg = 0;
+	reg |= (pps->second_chroma_qp_index_offset & 0x3f) << 16;
+	reg |= (pps->chroma_qp_index_offset & 0x3f) << 8;
+	reg |= (pps->pic_init_qp_minus26 + 26 + slice->slice_qp_delta) & 0x3f;
+	if (!(pps->flags & V4L2_H264_PPS_FLAG_SCALING_MATRIX_PRESENT))
+		reg |= VE_H264_SHS_QP_SCALING_MATRIX_DEFAULT;
+	cedrus_write(dev, VE_H264_SHS_QP, reg);
+
+	// clear status flags
+	cedrus_write(dev, VE_H264_STATUS, cedrus_read(dev, VE_H264_STATUS));
+
+	// enable int
+	cedrus_write(dev, VE_H264_CTRL,
+		     VE_H264_CTRL_SLICE_DECODE_INT |
+		     VE_H264_CTRL_DECODE_ERR_INT |
+		     VE_H264_CTRL_VLD_DATA_REQ_INT);
+}
+
+static enum cedrus_irq_status
+cedrus_h264_irq_status(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_H264_STATUS);
+
+	if (reg & (VE_H264_STATUS_DECODE_ERR_INT |
+		   VE_H264_STATUS_VLD_DATA_REQ_INT))
+		return CEDRUS_IRQ_ERROR;
+
+	if (reg & VE_H264_CTRL_SLICE_DECODE_INT)
+		return CEDRUS_IRQ_OK;
+
+	return CEDRUS_IRQ_NONE;
+}
+
+static void cedrus_h264_irq_clear(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_H264_STATUS,
+		     VE_H264_STATUS_INT_MASK);
+}
+
+static void cedrus_h264_irq_disable(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_H264_CTRL);
+
+	cedrus_write(dev, VE_H264_CTRL,
+		     reg & ~VE_H264_CTRL_INT_MASK);
+}
+
+static int cedrus_h264_setup(struct cedrus_ctx *ctx, struct cedrus_run *run)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_engine_enable(ctx, CEDRUS_CODEC_H264);
+
+	cedrus_write(dev, VE_H264_SDROT_CTRL, 0);
+	cedrus_write(dev, VE_H264_EXTRA_BUFFER1,
+		     ctx->codec.h264.pic_info_buf_dma);
+	cedrus_write(dev, VE_H264_EXTRA_BUFFER2,
+		     ctx->codec.h264.neighbor_info_buf_dma);
+
+	cedrus_write_scaling_lists(ctx, run);
+	cedrus_write_frame_list(ctx, run);
+
+	cedrus_set_params(ctx, run);
+
+	return 0;
+}
+
+static int cedrus_h264_start(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	unsigned int pic_info_size;
+	unsigned int field_size;
+	unsigned int mv_col_size;
+	int ret;
+
+	/*
+	 * NOTE: All buffers allocated here are only used by HW, so we
+	 * can add DMA_ATTR_NO_KERNEL_MAPPING flag when allocating them.
+	 */
+
+	/* Formula for picture buffer size is taken from CedarX source. */
+
+	if (ctx->src_fmt.width > 2048)
+		pic_info_size = CEDRUS_H264_FRAME_NUM * 0x4000;
+	else
+		pic_info_size = CEDRUS_H264_FRAME_NUM * 0x1000;
+
+	/*
+	 * FIXME: If V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY is set,
+	 * there is no need to multiply by 2.
+	 */
+	pic_info_size += ctx->src_fmt.height * 2 * 64;
+
+	if (pic_info_size < CEDRUS_MIN_PIC_INFO_BUF_SIZE)
+		pic_info_size = CEDRUS_MIN_PIC_INFO_BUF_SIZE;
+
+	ctx->codec.h264.pic_info_buf_size = pic_info_size;
+	ctx->codec.h264.pic_info_buf =
+		dma_alloc_attrs(dev->dev, ctx->codec.h264.pic_info_buf_size,
+				&ctx->codec.h264.pic_info_buf_dma,
+				GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+	if (!ctx->codec.h264.pic_info_buf)
+		return -ENOMEM;
+
+	/*
+	 * That buffer is supposed to be 16kiB in size, and be aligned
+	 * on 16kiB as well. However, dma_alloc_attrs provides the
+	 * guarantee that we'll have a DMA address aligned on the
+	 * smallest page order that is greater to the requested size,
+	 * so we don't have to overallocate.
+	 */
+	ctx->codec.h264.neighbor_info_buf =
+		dma_alloc_attrs(dev->dev, CEDRUS_NEIGHBOR_INFO_BUF_SIZE,
+				&ctx->codec.h264.neighbor_info_buf_dma,
+				GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+	if (!ctx->codec.h264.neighbor_info_buf) {
+		ret = -ENOMEM;
+		goto err_pic_buf;
+	}
+
+	field_size = DIV_ROUND_UP(ctx->src_fmt.width, 16) *
+		DIV_ROUND_UP(ctx->src_fmt.height, 16) * 16;
+
+	/*
+	 * FIXME: This is actually conditional to
+	 * V4L2_H264_SPS_FLAG_DIRECT_8X8_INFERENCE not being set, we
+	 * might have to rework this if memory efficiency ever is
+	 * something we need to work on.
+	 */
+	field_size = field_size * 2;
+
+	/*
+	 * FIXME: This is actually conditional to
+	 * V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY not being set, we might
+	 * have to rework this if memory efficiency ever is something
+	 * we need to work on.
+	 */
+	field_size = field_size * 2;
+	ctx->codec.h264.mv_col_buf_field_size = field_size;
+
+	mv_col_size = field_size * 2 * CEDRUS_H264_FRAME_NUM;
+	ctx->codec.h264.mv_col_buf_size = mv_col_size;
+	ctx->codec.h264.mv_col_buf =
+		dma_alloc_attrs(dev->dev,
+				ctx->codec.h264.mv_col_buf_size,
+				&ctx->codec.h264.mv_col_buf_dma,
+				GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+	if (!ctx->codec.h264.mv_col_buf) {
+		ret = -ENOMEM;
+		goto err_neighbor_buf;
+	}
+
+	if (ctx->src_fmt.width > 2048) {
+		/*
+		 * Formulas for deblock and intra prediction buffer sizes
+		 * are taken from CedarX source.
+		 */
+
+		ctx->codec.h264.deblk_buf_size =
+			ALIGN(ctx->src_fmt.width, 32) * 12;
+		ctx->codec.h264.deblk_buf =
+			dma_alloc_attrs(dev->dev,
+					ctx->codec.h264.deblk_buf_size,
+					&ctx->codec.h264.deblk_buf_dma,
+					GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+		if (!ctx->codec.h264.deblk_buf) {
+			ret = -ENOMEM;
+			goto err_mv_col_buf;
+		}
+
+		/*
+		 * NOTE: Multiplying by two deviates from CedarX logic, but it
+		 * is for some unknown reason needed for H264 4K decoding on H6.
+		 */
+		ctx->codec.h264.intra_pred_buf_size =
+			ALIGN(ctx->src_fmt.width, 64) * 5 * 2;
+		ctx->codec.h264.intra_pred_buf =
+			dma_alloc_attrs(dev->dev,
+					ctx->codec.h264.intra_pred_buf_size,
+					&ctx->codec.h264.intra_pred_buf_dma,
+					GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+		if (!ctx->codec.h264.intra_pred_buf) {
+			ret = -ENOMEM;
+			goto err_deblk_buf;
+		}
+	}
+
+	return 0;
+
+err_deblk_buf:
+	dma_free_attrs(dev->dev, ctx->codec.h264.deblk_buf_size,
+		       ctx->codec.h264.deblk_buf,
+		       ctx->codec.h264.deblk_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+
+err_mv_col_buf:
+	dma_free_attrs(dev->dev, ctx->codec.h264.mv_col_buf_size,
+		       ctx->codec.h264.mv_col_buf,
+		       ctx->codec.h264.mv_col_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+
+err_neighbor_buf:
+	dma_free_attrs(dev->dev, CEDRUS_NEIGHBOR_INFO_BUF_SIZE,
+		       ctx->codec.h264.neighbor_info_buf,
+		       ctx->codec.h264.neighbor_info_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+
+err_pic_buf:
+	dma_free_attrs(dev->dev, ctx->codec.h264.pic_info_buf_size,
+		       ctx->codec.h264.pic_info_buf,
+		       ctx->codec.h264.pic_info_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+	return ret;
+}
+
+static void cedrus_h264_stop(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	dma_free_attrs(dev->dev, ctx->codec.h264.mv_col_buf_size,
+		       ctx->codec.h264.mv_col_buf,
+		       ctx->codec.h264.mv_col_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+	dma_free_attrs(dev->dev, CEDRUS_NEIGHBOR_INFO_BUF_SIZE,
+		       ctx->codec.h264.neighbor_info_buf,
+		       ctx->codec.h264.neighbor_info_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+	dma_free_attrs(dev->dev, ctx->codec.h264.pic_info_buf_size,
+		       ctx->codec.h264.pic_info_buf,
+		       ctx->codec.h264.pic_info_buf_dma,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+	if (ctx->codec.h264.deblk_buf_size)
+		dma_free_attrs(dev->dev, ctx->codec.h264.deblk_buf_size,
+			       ctx->codec.h264.deblk_buf,
+			       ctx->codec.h264.deblk_buf_dma,
+			       DMA_ATTR_NO_KERNEL_MAPPING);
+	if (ctx->codec.h264.intra_pred_buf_size)
+		dma_free_attrs(dev->dev, ctx->codec.h264.intra_pred_buf_size,
+			       ctx->codec.h264.intra_pred_buf,
+			       ctx->codec.h264.intra_pred_buf_dma,
+			       DMA_ATTR_NO_KERNEL_MAPPING);
+}
+
+static void cedrus_h264_trigger(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+		     VE_H264_TRIGGER_TYPE_AVC_SLICE_DECODE);
+}
+
+struct cedrus_dec_ops cedrus_dec_ops_h264 = {
+	.irq_clear	= cedrus_h264_irq_clear,
+	.irq_disable	= cedrus_h264_irq_disable,
+	.irq_status	= cedrus_h264_irq_status,
+	.setup		= cedrus_h264_setup,
+	.start		= cedrus_h264_start,
+	.stop		= cedrus_h264_stop,
+	.trigger	= cedrus_h264_trigger,
+};
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_h265.c b/drivers/staging/media/sunxi/cedrus/cedrus_h265.c
new file mode 100644
index 000000000..625f77a8c
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_h265.c
@@ -0,0 +1,882 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2013 Jens Kuske <jenskuske@gmail.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ */
+
+#include <linux/delay.h>
+#include <linux/types.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cedrus.h"
+#include "cedrus_hw.h"
+#include "cedrus_regs.h"
+
+/*
+ * These are the sizes for side buffers required by the hardware for storing
+ * internal decoding metadata. They match the values used by the early BSP
+ * implementations, that were initially exposed in libvdpau-sunxi.
+ * Subsequent BSP implementations seem to double the neighbor info buffer size
+ * for the H6 SoC, which may be related to 10 bit H265 support.
+ */
+#define CEDRUS_H265_NEIGHBOR_INFO_BUF_SIZE	(794 * SZ_1K)
+#define CEDRUS_H265_ENTRY_POINTS_BUF_SIZE	(4 * SZ_1K)
+#define CEDRUS_H265_MV_COL_BUF_UNIT_CTB_SIZE	160
+
+struct cedrus_h265_sram_frame_info {
+	__le32	top_pic_order_cnt;
+	__le32	bottom_pic_order_cnt;
+	__le32	top_mv_col_buf_addr;
+	__le32	bottom_mv_col_buf_addr;
+	__le32	luma_addr;
+	__le32	chroma_addr;
+} __packed;
+
+struct cedrus_h265_sram_pred_weight {
+	__s8	delta_weight;
+	__s8	offset;
+} __packed;
+
+static enum cedrus_irq_status cedrus_h265_irq_status(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg;
+
+	reg = cedrus_read(dev, VE_DEC_H265_STATUS);
+	reg &= VE_DEC_H265_STATUS_CHECK_MASK;
+
+	if (reg & VE_DEC_H265_STATUS_CHECK_ERROR ||
+	    !(reg & VE_DEC_H265_STATUS_SUCCESS))
+		return CEDRUS_IRQ_ERROR;
+
+	return CEDRUS_IRQ_OK;
+}
+
+static void cedrus_h265_irq_clear(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_DEC_H265_STATUS, VE_DEC_H265_STATUS_CHECK_MASK);
+}
+
+static void cedrus_h265_irq_disable(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_DEC_H265_CTRL);
+
+	reg &= ~VE_DEC_H265_CTRL_IRQ_MASK;
+
+	cedrus_write(dev, VE_DEC_H265_CTRL, reg);
+}
+
+static void cedrus_h265_sram_write_offset(struct cedrus_dev *dev, u32 offset)
+{
+	cedrus_write(dev, VE_DEC_H265_SRAM_OFFSET, offset);
+}
+
+static void cedrus_h265_sram_write_data(struct cedrus_dev *dev, void *data,
+					unsigned int size)
+{
+	u32 *word = data;
+
+	while (size >= sizeof(u32)) {
+		cedrus_write(dev, VE_DEC_H265_SRAM_DATA, *word++);
+		size -= sizeof(u32);
+	}
+}
+
+static inline dma_addr_t
+cedrus_h265_frame_info_mv_col_buf_addr(struct cedrus_ctx *ctx,
+				       unsigned int index, unsigned int field)
+{
+	return ctx->codec.h265.mv_col_buf_addr + index *
+	       ctx->codec.h265.mv_col_buf_unit_size +
+	       field * ctx->codec.h265.mv_col_buf_unit_size / 2;
+}
+
+static void cedrus_h265_frame_info_write_single(struct cedrus_ctx *ctx,
+						unsigned int index,
+						bool field_pic,
+						u32 pic_order_cnt[],
+						struct vb2_buffer *buf)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	dma_addr_t dst_luma_addr = cedrus_dst_buf_addr(ctx, buf, 0);
+	dma_addr_t dst_chroma_addr = cedrus_dst_buf_addr(ctx, buf, 1);
+	dma_addr_t mv_col_buf_addr[2] = {
+		cedrus_h265_frame_info_mv_col_buf_addr(ctx, buf->index, 0),
+		cedrus_h265_frame_info_mv_col_buf_addr(ctx, buf->index,
+						       field_pic ? 1 : 0)
+	};
+	u32 offset = VE_DEC_H265_SRAM_OFFSET_FRAME_INFO +
+		     VE_DEC_H265_SRAM_OFFSET_FRAME_INFO_UNIT * index;
+	struct cedrus_h265_sram_frame_info frame_info = {
+		.top_pic_order_cnt = cpu_to_le32(pic_order_cnt[0]),
+		.bottom_pic_order_cnt = cpu_to_le32(field_pic ?
+						    pic_order_cnt[1] :
+						    pic_order_cnt[0]),
+		.top_mv_col_buf_addr =
+			cpu_to_le32(VE_DEC_H265_SRAM_DATA_ADDR_BASE(mv_col_buf_addr[0])),
+		.bottom_mv_col_buf_addr = cpu_to_le32(field_pic ?
+			VE_DEC_H265_SRAM_DATA_ADDR_BASE(mv_col_buf_addr[1]) :
+			VE_DEC_H265_SRAM_DATA_ADDR_BASE(mv_col_buf_addr[0])),
+		.luma_addr = cpu_to_le32(VE_DEC_H265_SRAM_DATA_ADDR_BASE(dst_luma_addr)),
+		.chroma_addr = cpu_to_le32(VE_DEC_H265_SRAM_DATA_ADDR_BASE(dst_chroma_addr)),
+	};
+
+	cedrus_h265_sram_write_offset(dev, offset);
+	cedrus_h265_sram_write_data(dev, &frame_info, sizeof(frame_info));
+}
+
+static void cedrus_h265_frame_info_write_dpb(struct cedrus_ctx *ctx,
+					     const struct v4l2_hevc_dpb_entry *dpb,
+					     u8 num_active_dpb_entries)
+{
+	struct vb2_queue *vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+					       V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	unsigned int i;
+
+	for (i = 0; i < num_active_dpb_entries; i++) {
+		struct vb2_buffer *buf = vb2_find_buffer(vq, dpb[i].timestamp);
+		u32 pic_order_cnt[2] = {
+			dpb[i].pic_order_cnt_val,
+			dpb[i].pic_order_cnt_val
+		};
+
+		if (!buf)
+			continue;
+
+		cedrus_h265_frame_info_write_single(ctx, i, dpb[i].field_pic,
+						    pic_order_cnt,
+						    buf);
+	}
+}
+
+static void cedrus_h265_ref_pic_list_write(struct cedrus_dev *dev,
+					   const struct v4l2_hevc_dpb_entry *dpb,
+					   const u8 list[],
+					   u8 num_ref_idx_active,
+					   u32 sram_offset)
+{
+	unsigned int i;
+	u32 word = 0;
+
+	cedrus_h265_sram_write_offset(dev, sram_offset);
+
+	for (i = 0; i < num_ref_idx_active; i++) {
+		unsigned int shift = (i % 4) * 8;
+		unsigned int index = list[i];
+		u8 value = list[i];
+
+		if (dpb[index].flags & V4L2_HEVC_DPB_ENTRY_LONG_TERM_REFERENCE)
+			value |= VE_DEC_H265_SRAM_REF_PIC_LIST_LT_REF;
+
+		/* Each SRAM word gathers up to 4 references. */
+		word |= value << shift;
+
+		/* Write the word to SRAM and clear it for the next batch. */
+		if ((i % 4) == 3 || i == (num_ref_idx_active - 1)) {
+			cedrus_h265_sram_write_data(dev, &word, sizeof(word));
+			word = 0;
+		}
+	}
+}
+
+static void cedrus_h265_pred_weight_write(struct cedrus_dev *dev,
+					  const s8 delta_luma_weight[],
+					  const s8 luma_offset[],
+					  const s8 delta_chroma_weight[][2],
+					  const s8 chroma_offset[][2],
+					  u8 num_ref_idx_active,
+					  u32 sram_luma_offset,
+					  u32 sram_chroma_offset)
+{
+	struct cedrus_h265_sram_pred_weight pred_weight[2] = { { 0 } };
+	unsigned int i, j;
+
+	cedrus_h265_sram_write_offset(dev, sram_luma_offset);
+
+	for (i = 0; i < num_ref_idx_active; i++) {
+		unsigned int index = i % 2;
+
+		pred_weight[index].delta_weight = delta_luma_weight[i];
+		pred_weight[index].offset = luma_offset[i];
+
+		if (index == 1 || i == (num_ref_idx_active - 1))
+			cedrus_h265_sram_write_data(dev, (u32 *)&pred_weight,
+						    sizeof(pred_weight));
+	}
+
+	cedrus_h265_sram_write_offset(dev, sram_chroma_offset);
+
+	for (i = 0; i < num_ref_idx_active; i++) {
+		for (j = 0; j < 2; j++) {
+			pred_weight[j].delta_weight = delta_chroma_weight[i][j];
+			pred_weight[j].offset = chroma_offset[i][j];
+		}
+
+		cedrus_h265_sram_write_data(dev, &pred_weight,
+					    sizeof(pred_weight));
+	}
+}
+
+static void cedrus_h265_skip_bits(struct cedrus_dev *dev, int num)
+{
+	int count = 0;
+
+	while (count < num) {
+		int tmp = min(num - count, 32);
+
+		cedrus_write(dev, VE_DEC_H265_TRIGGER,
+			     VE_DEC_H265_TRIGGER_FLUSH_BITS |
+			     VE_DEC_H265_TRIGGER_TYPE_N_BITS(tmp));
+
+		if (cedrus_wait_for(dev, VE_DEC_H265_STATUS, VE_DEC_H265_STATUS_VLD_BUSY))
+			dev_err_ratelimited(dev->dev, "timed out waiting to skip bits\n");
+
+		count += tmp;
+	}
+}
+
+static u32 cedrus_h265_show_bits(struct cedrus_dev *dev, int num)
+{
+	cedrus_write(dev, VE_DEC_H265_TRIGGER,
+		     VE_DEC_H265_TRIGGER_SHOW_BITS |
+		     VE_DEC_H265_TRIGGER_TYPE_N_BITS(num));
+
+	cedrus_wait_for(dev, VE_DEC_H265_STATUS,
+			VE_DEC_H265_STATUS_VLD_BUSY);
+
+	return cedrus_read(dev, VE_DEC_H265_BITS_READ);
+}
+
+static void cedrus_h265_write_scaling_list(struct cedrus_ctx *ctx,
+					   struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_hevc_scaling_matrix *scaling;
+	struct cedrus_dev *dev = ctx->dev;
+	u32 i, j, k, val;
+
+	scaling = run->h265.scaling_matrix;
+
+	cedrus_write(dev, VE_DEC_H265_SCALING_LIST_DC_COEF0,
+		     (scaling->scaling_list_dc_coef_32x32[1] << 24) |
+		     (scaling->scaling_list_dc_coef_32x32[0] << 16) |
+		     (scaling->scaling_list_dc_coef_16x16[1] << 8) |
+		     (scaling->scaling_list_dc_coef_16x16[0] << 0));
+
+	cedrus_write(dev, VE_DEC_H265_SCALING_LIST_DC_COEF1,
+		     (scaling->scaling_list_dc_coef_16x16[5] << 24) |
+		     (scaling->scaling_list_dc_coef_16x16[4] << 16) |
+		     (scaling->scaling_list_dc_coef_16x16[3] << 8) |
+		     (scaling->scaling_list_dc_coef_16x16[2] << 0));
+
+	cedrus_h265_sram_write_offset(dev, VE_DEC_H265_SRAM_OFFSET_SCALING_LISTS);
+
+	for (i = 0; i < 6; i++)
+		for (j = 0; j < 8; j++)
+			for (k = 0; k < 8; k += 4) {
+				val = ((u32)scaling->scaling_list_8x8[i][j + (k + 3) * 8] << 24) |
+				      ((u32)scaling->scaling_list_8x8[i][j + (k + 2) * 8] << 16) |
+				      ((u32)scaling->scaling_list_8x8[i][j + (k + 1) * 8] << 8) |
+				      scaling->scaling_list_8x8[i][j + k * 8];
+				cedrus_write(dev, VE_DEC_H265_SRAM_DATA, val);
+			}
+
+	for (i = 0; i < 2; i++)
+		for (j = 0; j < 8; j++)
+			for (k = 0; k < 8; k += 4) {
+				val = ((u32)scaling->scaling_list_32x32[i][j + (k + 3) * 8] << 24) |
+				      ((u32)scaling->scaling_list_32x32[i][j + (k + 2) * 8] << 16) |
+				      ((u32)scaling->scaling_list_32x32[i][j + (k + 1) * 8] << 8) |
+				      scaling->scaling_list_32x32[i][j + k * 8];
+				cedrus_write(dev, VE_DEC_H265_SRAM_DATA, val);
+			}
+
+	for (i = 0; i < 6; i++)
+		for (j = 0; j < 8; j++)
+			for (k = 0; k < 8; k += 4) {
+				val = ((u32)scaling->scaling_list_16x16[i][j + (k + 3) * 8] << 24) |
+				      ((u32)scaling->scaling_list_16x16[i][j + (k + 2) * 8] << 16) |
+				      ((u32)scaling->scaling_list_16x16[i][j + (k + 1) * 8] << 8) |
+				      scaling->scaling_list_16x16[i][j + k * 8];
+				cedrus_write(dev, VE_DEC_H265_SRAM_DATA, val);
+			}
+
+	for (i = 0; i < 6; i++)
+		for (j = 0; j < 4; j++) {
+			val = ((u32)scaling->scaling_list_4x4[i][j + 12] << 24) |
+			      ((u32)scaling->scaling_list_4x4[i][j + 8] << 16) |
+			      ((u32)scaling->scaling_list_4x4[i][j + 4] << 8) |
+			      scaling->scaling_list_4x4[i][j];
+			cedrus_write(dev, VE_DEC_H265_SRAM_DATA, val);
+		}
+}
+
+static int cedrus_h265_is_low_delay(struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_hevc_slice_params *slice_params;
+	const struct v4l2_hevc_dpb_entry *dpb;
+	s32 poc;
+	int i;
+
+	slice_params = run->h265.slice_params;
+	poc = run->h265.decode_params->pic_order_cnt_val;
+	dpb = run->h265.decode_params->dpb;
+
+	for (i = 0; i < slice_params->num_ref_idx_l0_active_minus1 + 1; i++)
+		if (dpb[slice_params->ref_idx_l0[i]].pic_order_cnt_val > poc)
+			return 1;
+
+	if (slice_params->slice_type != V4L2_HEVC_SLICE_TYPE_B)
+		return 0;
+
+	for (i = 0; i < slice_params->num_ref_idx_l1_active_minus1 + 1; i++)
+		if (dpb[slice_params->ref_idx_l1[i]].pic_order_cnt_val > poc)
+			return 1;
+
+	return 0;
+}
+
+static void cedrus_h265_write_tiles(struct cedrus_ctx *ctx,
+				    struct cedrus_run *run,
+				    unsigned int ctb_addr_x,
+				    unsigned int ctb_addr_y)
+{
+	const struct v4l2_ctrl_hevc_slice_params *slice_params;
+	const struct v4l2_ctrl_hevc_pps *pps;
+	struct cedrus_dev *dev = ctx->dev;
+	const u32 *entry_points;
+	u32 *entry_points_buf;
+	int i, x, tx, y, ty;
+
+	pps = run->h265.pps;
+	slice_params = run->h265.slice_params;
+	entry_points = run->h265.entry_points;
+	entry_points_buf = ctx->codec.h265.entry_points_buf;
+
+	for (x = 0, tx = 0; tx < pps->num_tile_columns_minus1 + 1; tx++) {
+		if (x + pps->column_width_minus1[tx] + 1 > ctb_addr_x)
+			break;
+
+		x += pps->column_width_minus1[tx] + 1;
+	}
+
+	for (y = 0, ty = 0; ty < pps->num_tile_rows_minus1 + 1; ty++) {
+		if (y + pps->row_height_minus1[ty] + 1 > ctb_addr_y)
+			break;
+
+		y += pps->row_height_minus1[ty] + 1;
+	}
+
+	cedrus_write(dev, VE_DEC_H265_TILE_START_CTB, (y << 16) | (x << 0));
+	cedrus_write(dev, VE_DEC_H265_TILE_END_CTB,
+		     ((y + pps->row_height_minus1[ty]) << 16) |
+		     ((x + pps->column_width_minus1[tx]) << 0));
+
+	if (pps->flags & V4L2_HEVC_PPS_FLAG_ENTROPY_CODING_SYNC_ENABLED) {
+		for (i = 0; i < slice_params->num_entry_point_offsets; i++)
+			entry_points_buf[i] = entry_points[i];
+	} else {
+		for (i = 0; i < slice_params->num_entry_point_offsets; i++) {
+			if (tx + 1 >= pps->num_tile_columns_minus1 + 1) {
+				x = 0;
+				tx = 0;
+				y += pps->row_height_minus1[ty++] + 1;
+			} else {
+				x += pps->column_width_minus1[tx++] + 1;
+			}
+
+			entry_points_buf[i * 4 + 0] = entry_points[i];
+			entry_points_buf[i * 4 + 1] = 0x0;
+			entry_points_buf[i * 4 + 2] = (y << 16) | (x << 0);
+			entry_points_buf[i * 4 + 3] =
+				((y + pps->row_height_minus1[ty]) << 16) |
+				((x + pps->column_width_minus1[tx]) << 0);
+		}
+	}
+}
+
+static int cedrus_h265_setup(struct cedrus_ctx *ctx, struct cedrus_run *run)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	const struct v4l2_ctrl_hevc_sps *sps;
+	const struct v4l2_ctrl_hevc_pps *pps;
+	const struct v4l2_ctrl_hevc_slice_params *slice_params;
+	const struct v4l2_ctrl_hevc_decode_params *decode_params;
+	const struct v4l2_hevc_pred_weight_table *pred_weight_table;
+	unsigned int width_in_ctb_luma, ctb_size_luma;
+	unsigned int log2_max_luma_coding_block_size;
+	unsigned int ctb_addr_x, ctb_addr_y;
+	dma_addr_t src_buf_addr;
+	dma_addr_t src_buf_end_addr;
+	u32 chroma_log2_weight_denom;
+	u32 num_entry_point_offsets;
+	u32 output_pic_list_index;
+	u32 pic_order_cnt[2];
+	u8 padding;
+	int count;
+	u32 reg;
+
+	sps = run->h265.sps;
+	pps = run->h265.pps;
+	slice_params = run->h265.slice_params;
+	decode_params = run->h265.decode_params;
+	pred_weight_table = &slice_params->pred_weight_table;
+	num_entry_point_offsets = slice_params->num_entry_point_offsets;
+
+	/*
+	 * If entry points offsets are present, we should get them
+	 * exactly the right amount.
+	 */
+	if (num_entry_point_offsets &&
+	    num_entry_point_offsets != run->h265.entry_points_count)
+		return -ERANGE;
+
+	log2_max_luma_coding_block_size =
+		sps->log2_min_luma_coding_block_size_minus3 + 3 +
+		sps->log2_diff_max_min_luma_coding_block_size;
+	ctb_size_luma = 1UL << log2_max_luma_coding_block_size;
+	width_in_ctb_luma =
+		DIV_ROUND_UP(sps->pic_width_in_luma_samples, ctb_size_luma);
+
+	/* MV column buffer size and allocation. */
+	if (!ctx->codec.h265.mv_col_buf_size) {
+		unsigned int num_buffers =
+			run->dst->vb2_buf.vb2_queue->num_buffers;
+
+		/*
+		 * Each CTB requires a MV col buffer with a specific unit size.
+		 * Since the address is given with missing lsb bits, 1 KiB is
+		 * added to each buffer to ensure proper alignment.
+		 */
+		ctx->codec.h265.mv_col_buf_unit_size =
+			DIV_ROUND_UP(ctx->src_fmt.width, ctb_size_luma) *
+			DIV_ROUND_UP(ctx->src_fmt.height, ctb_size_luma) *
+			CEDRUS_H265_MV_COL_BUF_UNIT_CTB_SIZE + SZ_1K;
+
+		ctx->codec.h265.mv_col_buf_size = num_buffers *
+			ctx->codec.h265.mv_col_buf_unit_size;
+
+		/* Buffer is never accessed by CPU, so we can skip kernel mapping. */
+		ctx->codec.h265.mv_col_buf =
+			dma_alloc_attrs(dev->dev,
+					ctx->codec.h265.mv_col_buf_size,
+					&ctx->codec.h265.mv_col_buf_addr,
+					GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+		if (!ctx->codec.h265.mv_col_buf) {
+			ctx->codec.h265.mv_col_buf_size = 0;
+			return -ENOMEM;
+		}
+	}
+
+	/* Activate H265 engine. */
+	cedrus_engine_enable(ctx, CEDRUS_CODEC_H265);
+
+	/* Source offset and length in bits. */
+
+	cedrus_write(dev, VE_DEC_H265_BITS_OFFSET, 0);
+
+	reg = slice_params->bit_size;
+	cedrus_write(dev, VE_DEC_H265_BITS_LEN, reg);
+
+	/* Source beginning and end addresses. */
+
+	src_buf_addr = vb2_dma_contig_plane_dma_addr(&run->src->vb2_buf, 0);
+
+	reg = VE_DEC_H265_BITS_ADDR_BASE(src_buf_addr);
+	reg |= VE_DEC_H265_BITS_ADDR_VALID_SLICE_DATA;
+	reg |= VE_DEC_H265_BITS_ADDR_LAST_SLICE_DATA;
+	reg |= VE_DEC_H265_BITS_ADDR_FIRST_SLICE_DATA;
+
+	cedrus_write(dev, VE_DEC_H265_BITS_ADDR, reg);
+
+	src_buf_end_addr = src_buf_addr +
+			   DIV_ROUND_UP(slice_params->bit_size, 8);
+
+	reg = VE_DEC_H265_BITS_END_ADDR_BASE(src_buf_end_addr);
+	cedrus_write(dev, VE_DEC_H265_BITS_END_ADDR, reg);
+
+	/* Coding tree block address */
+	ctb_addr_x = slice_params->slice_segment_addr % width_in_ctb_luma;
+	ctb_addr_y = slice_params->slice_segment_addr / width_in_ctb_luma;
+	reg = VE_DEC_H265_DEC_CTB_ADDR_X(ctb_addr_x);
+	reg |= VE_DEC_H265_DEC_CTB_ADDR_Y(ctb_addr_y);
+	cedrus_write(dev, VE_DEC_H265_DEC_CTB_ADDR, reg);
+
+	if ((pps->flags & V4L2_HEVC_PPS_FLAG_TILES_ENABLED) ||
+	    (pps->flags & V4L2_HEVC_PPS_FLAG_ENTROPY_CODING_SYNC_ENABLED)) {
+		cedrus_h265_write_tiles(ctx, run, ctb_addr_x, ctb_addr_y);
+	} else {
+		cedrus_write(dev, VE_DEC_H265_TILE_START_CTB, 0);
+		cedrus_write(dev, VE_DEC_H265_TILE_END_CTB, 0);
+	}
+
+	/* Clear the number of correctly-decoded coding tree blocks. */
+	if (ctx->fh.m2m_ctx->new_frame)
+		cedrus_write(dev, VE_DEC_H265_DEC_CTB_NUM, 0);
+
+	/* Initialize bitstream access. */
+	cedrus_write(dev, VE_DEC_H265_TRIGGER, VE_DEC_H265_TRIGGER_INIT_SWDEC);
+
+	/*
+	 * Cedrus expects that bitstream pointer is actually at the end of the slice header
+	 * instead of start of slice data. Padding is 8 bits at most (one bit set to 1 and
+	 * at most seven bits set to 0), so we have to inspect only one byte before slice data.
+	 */
+
+	if (slice_params->data_byte_offset == 0)
+		return -EOPNOTSUPP;
+
+	cedrus_h265_skip_bits(dev, (slice_params->data_byte_offset - 1) * 8);
+
+	padding = cedrus_h265_show_bits(dev, 8);
+
+	/* at least one bit must be set in that byte */
+	if (padding == 0)
+		return -EINVAL;
+
+	for (count = 0; count < 8; count++)
+		if (padding & (1 << count))
+			break;
+
+	/* Include the one bit. */
+	count++;
+
+	cedrus_h265_skip_bits(dev, 8 - count);
+
+	/* Bitstream parameters. */
+
+	reg = VE_DEC_H265_DEC_NAL_HDR_NAL_UNIT_TYPE(slice_params->nal_unit_type) |
+	      VE_DEC_H265_DEC_NAL_HDR_NUH_TEMPORAL_ID_PLUS1(slice_params->nuh_temporal_id_plus1);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_NAL_HDR, reg);
+
+	/* SPS. */
+
+	reg = VE_DEC_H265_DEC_SPS_HDR_MAX_TRANSFORM_HIERARCHY_DEPTH_INTRA(sps->max_transform_hierarchy_depth_intra) |
+	      VE_DEC_H265_DEC_SPS_HDR_MAX_TRANSFORM_HIERARCHY_DEPTH_INTER(sps->max_transform_hierarchy_depth_inter) |
+	      VE_DEC_H265_DEC_SPS_HDR_LOG2_DIFF_MAX_MIN_TRANSFORM_BLOCK_SIZE(sps->log2_diff_max_min_luma_transform_block_size) |
+	      VE_DEC_H265_DEC_SPS_HDR_LOG2_MIN_TRANSFORM_BLOCK_SIZE_MINUS2(sps->log2_min_luma_transform_block_size_minus2) |
+	      VE_DEC_H265_DEC_SPS_HDR_LOG2_DIFF_MAX_MIN_LUMA_CODING_BLOCK_SIZE(sps->log2_diff_max_min_luma_coding_block_size) |
+	      VE_DEC_H265_DEC_SPS_HDR_LOG2_MIN_LUMA_CODING_BLOCK_SIZE_MINUS3(sps->log2_min_luma_coding_block_size_minus3) |
+	      VE_DEC_H265_DEC_SPS_HDR_BIT_DEPTH_CHROMA_MINUS8(sps->bit_depth_chroma_minus8) |
+	      VE_DEC_H265_DEC_SPS_HDR_BIT_DEPTH_LUMA_MINUS8(sps->bit_depth_luma_minus8) |
+	      VE_DEC_H265_DEC_SPS_HDR_CHROMA_FORMAT_IDC(sps->chroma_format_idc);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SPS_HDR_FLAG_STRONG_INTRA_SMOOTHING_ENABLE,
+				V4L2_HEVC_SPS_FLAG_STRONG_INTRA_SMOOTHING_ENABLED,
+				sps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SPS_HDR_FLAG_SPS_TEMPORAL_MVP_ENABLED,
+				V4L2_HEVC_SPS_FLAG_SPS_TEMPORAL_MVP_ENABLED,
+				sps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SPS_HDR_FLAG_SAMPLE_ADAPTIVE_OFFSET_ENABLED,
+				V4L2_HEVC_SPS_FLAG_SAMPLE_ADAPTIVE_OFFSET,
+				sps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SPS_HDR_FLAG_AMP_ENABLED,
+				V4L2_HEVC_SPS_FLAG_AMP_ENABLED, sps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SPS_HDR_FLAG_SEPARATE_COLOUR_PLANE,
+				V4L2_HEVC_SPS_FLAG_SEPARATE_COLOUR_PLANE,
+				sps->flags);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_SPS_HDR, reg);
+
+	reg = VE_DEC_H265_DEC_PCM_CTRL_LOG2_DIFF_MAX_MIN_PCM_LUMA_CODING_BLOCK_SIZE(sps->log2_diff_max_min_pcm_luma_coding_block_size) |
+	      VE_DEC_H265_DEC_PCM_CTRL_LOG2_MIN_PCM_LUMA_CODING_BLOCK_SIZE_MINUS3(sps->log2_min_pcm_luma_coding_block_size_minus3) |
+	      VE_DEC_H265_DEC_PCM_CTRL_PCM_SAMPLE_BIT_DEPTH_CHROMA_MINUS1(sps->pcm_sample_bit_depth_chroma_minus1) |
+	      VE_DEC_H265_DEC_PCM_CTRL_PCM_SAMPLE_BIT_DEPTH_LUMA_MINUS1(sps->pcm_sample_bit_depth_luma_minus1);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PCM_CTRL_FLAG_PCM_ENABLED,
+				V4L2_HEVC_SPS_FLAG_PCM_ENABLED, sps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PCM_CTRL_FLAG_PCM_LOOP_FILTER_DISABLED,
+				V4L2_HEVC_SPS_FLAG_PCM_LOOP_FILTER_DISABLED,
+				sps->flags);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_PCM_CTRL, reg);
+
+	/* PPS. */
+
+	reg = VE_DEC_H265_DEC_PPS_CTRL0_PPS_CR_QP_OFFSET(pps->pps_cr_qp_offset) |
+	      VE_DEC_H265_DEC_PPS_CTRL0_PPS_CB_QP_OFFSET(pps->pps_cb_qp_offset) |
+	      VE_DEC_H265_DEC_PPS_CTRL0_INIT_QP_MINUS26(pps->init_qp_minus26) |
+	      VE_DEC_H265_DEC_PPS_CTRL0_DIFF_CU_QP_DELTA_DEPTH(pps->diff_cu_qp_delta_depth);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL0_FLAG_CU_QP_DELTA_ENABLED,
+				V4L2_HEVC_PPS_FLAG_CU_QP_DELTA_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL0_FLAG_TRANSFORM_SKIP_ENABLED,
+				V4L2_HEVC_PPS_FLAG_TRANSFORM_SKIP_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL0_FLAG_CONSTRAINED_INTRA_PRED,
+				V4L2_HEVC_PPS_FLAG_CONSTRAINED_INTRA_PRED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL0_FLAG_SIGN_DATA_HIDING_ENABLED,
+				V4L2_HEVC_PPS_FLAG_SIGN_DATA_HIDING_ENABLED,
+				pps->flags);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_PPS_CTRL0, reg);
+
+	reg = VE_DEC_H265_DEC_PPS_CTRL1_LOG2_PARALLEL_MERGE_LEVEL_MINUS2(pps->log2_parallel_merge_level_minus2);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_PPS_LOOP_FILTER_ACROSS_SLICES_ENABLED,
+				V4L2_HEVC_PPS_FLAG_PPS_LOOP_FILTER_ACROSS_SLICES_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_LOOP_FILTER_ACROSS_TILES_ENABLED,
+				V4L2_HEVC_PPS_FLAG_LOOP_FILTER_ACROSS_TILES_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_ENTROPY_CODING_SYNC_ENABLED,
+				V4L2_HEVC_PPS_FLAG_ENTROPY_CODING_SYNC_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_TILES_ENABLED,
+				V4L2_HEVC_PPS_FLAG_TILES_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_TRANSQUANT_BYPASS_ENABLED,
+				V4L2_HEVC_PPS_FLAG_TRANSQUANT_BYPASS_ENABLED,
+				pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_WEIGHTED_BIPRED,
+				V4L2_HEVC_PPS_FLAG_WEIGHTED_BIPRED, pps->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_PPS_CTRL1_FLAG_WEIGHTED_PRED,
+				V4L2_HEVC_PPS_FLAG_WEIGHTED_PRED, pps->flags);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_PPS_CTRL1, reg);
+
+	/* Slice Parameters. */
+
+	reg = VE_DEC_H265_DEC_SLICE_HDR_INFO0_PICTURE_TYPE(slice_params->pic_struct) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_FIVE_MINUS_MAX_NUM_MERGE_CAND(slice_params->five_minus_max_num_merge_cand) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_NUM_REF_IDX_L1_ACTIVE_MINUS1(slice_params->num_ref_idx_l1_active_minus1) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_NUM_REF_IDX_L0_ACTIVE_MINUS1(slice_params->num_ref_idx_l0_active_minus1) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_COLLOCATED_REF_IDX(slice_params->collocated_ref_idx) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_COLOUR_PLANE_ID(slice_params->colour_plane_id) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO0_SLICE_TYPE(slice_params->slice_type);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_COLLOCATED_FROM_L0,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_COLLOCATED_FROM_L0,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_CABAC_INIT,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_CABAC_INIT,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_MVD_L1_ZERO,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_MVD_L1_ZERO,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_SAO_CHROMA,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_SLICE_SAO_CHROMA,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_SAO_LUMA,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_SLICE_SAO_LUMA,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_TEMPORAL_MVP_ENABLE,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_SLICE_TEMPORAL_MVP_ENABLED,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_DEPENDENT_SLICE_SEGMENT,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_DEPENDENT_SLICE_SEGMENT,
+				slice_params->flags);
+
+	if (ctx->fh.m2m_ctx->new_frame)
+		reg |= VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_FIRST_SLICE_SEGMENT_IN_PIC;
+
+	cedrus_write(dev, VE_DEC_H265_DEC_SLICE_HDR_INFO0, reg);
+
+	reg = VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_TC_OFFSET_DIV2(slice_params->slice_tc_offset_div2) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_BETA_OFFSET_DIV2(slice_params->slice_beta_offset_div2) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_CR_QP_OFFSET(slice_params->slice_cr_qp_offset) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_CB_QP_OFFSET(slice_params->slice_cb_qp_offset) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_QP_DELTA(slice_params->slice_qp_delta);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_DEBLOCKING_FILTER_DISABLED,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_SLICE_DEBLOCKING_FILTER_DISABLED,
+				slice_params->flags);
+
+	reg |= VE_DEC_H265_FLAG(VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_LOOP_FILTER_ACROSS_SLICES_ENABLED,
+				V4L2_HEVC_SLICE_PARAMS_FLAG_SLICE_LOOP_FILTER_ACROSS_SLICES_ENABLED,
+				slice_params->flags);
+
+	if (slice_params->slice_type != V4L2_HEVC_SLICE_TYPE_I && !cedrus_h265_is_low_delay(run))
+		reg |= VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_NOT_LOW_DELAY;
+
+	cedrus_write(dev, VE_DEC_H265_DEC_SLICE_HDR_INFO1, reg);
+
+	chroma_log2_weight_denom = pred_weight_table->luma_log2_weight_denom +
+				   pred_weight_table->delta_chroma_log2_weight_denom;
+	reg = VE_DEC_H265_DEC_SLICE_HDR_INFO2_NUM_ENTRY_POINT_OFFSETS(num_entry_point_offsets) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO2_CHROMA_LOG2_WEIGHT_DENOM(chroma_log2_weight_denom) |
+	      VE_DEC_H265_DEC_SLICE_HDR_INFO2_LUMA_LOG2_WEIGHT_DENOM(pred_weight_table->luma_log2_weight_denom);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_SLICE_HDR_INFO2, reg);
+
+	cedrus_write(dev, VE_DEC_H265_ENTRY_POINT_OFFSET_ADDR,
+		     ctx->codec.h265.entry_points_buf_addr >> 8);
+
+	/* Decoded picture size. */
+
+	reg = VE_DEC_H265_DEC_PIC_SIZE_WIDTH(ctx->src_fmt.width) |
+	      VE_DEC_H265_DEC_PIC_SIZE_HEIGHT(ctx->src_fmt.height);
+
+	cedrus_write(dev, VE_DEC_H265_DEC_PIC_SIZE, reg);
+
+	/* Scaling list. */
+
+	if (sps->flags & V4L2_HEVC_SPS_FLAG_SCALING_LIST_ENABLED) {
+		cedrus_h265_write_scaling_list(ctx, run);
+		reg = VE_DEC_H265_SCALING_LIST_CTRL0_FLAG_ENABLED;
+	} else {
+		reg = VE_DEC_H265_SCALING_LIST_CTRL0_DEFAULT;
+	}
+	cedrus_write(dev, VE_DEC_H265_SCALING_LIST_CTRL0, reg);
+
+	/* Neightbor information address. */
+	reg = VE_DEC_H265_NEIGHBOR_INFO_ADDR_BASE(ctx->codec.h265.neighbor_info_buf_addr);
+	cedrus_write(dev, VE_DEC_H265_NEIGHBOR_INFO_ADDR, reg);
+
+	/* Write decoded picture buffer in pic list. */
+	cedrus_h265_frame_info_write_dpb(ctx, decode_params->dpb,
+					 decode_params->num_active_dpb_entries);
+
+	/* Output frame. */
+
+	output_pic_list_index = V4L2_HEVC_DPB_ENTRIES_NUM_MAX;
+	pic_order_cnt[0] = slice_params->slice_pic_order_cnt;
+	pic_order_cnt[1] = slice_params->slice_pic_order_cnt;
+
+	cedrus_h265_frame_info_write_single(ctx, output_pic_list_index,
+					    slice_params->pic_struct != 0,
+					    pic_order_cnt,
+					    &run->dst->vb2_buf);
+
+	cedrus_write(dev, VE_DEC_H265_OUTPUT_FRAME_IDX, output_pic_list_index);
+
+	/* Reference picture list 0 (for P/B frames). */
+	if (slice_params->slice_type != V4L2_HEVC_SLICE_TYPE_I) {
+		cedrus_h265_ref_pic_list_write(dev, decode_params->dpb,
+					       slice_params->ref_idx_l0,
+					       slice_params->num_ref_idx_l0_active_minus1 + 1,
+					       VE_DEC_H265_SRAM_OFFSET_REF_PIC_LIST0);
+
+		if ((pps->flags & V4L2_HEVC_PPS_FLAG_WEIGHTED_PRED) ||
+		    (pps->flags & V4L2_HEVC_PPS_FLAG_WEIGHTED_BIPRED))
+			cedrus_h265_pred_weight_write(dev,
+						      pred_weight_table->delta_luma_weight_l0,
+						      pred_weight_table->luma_offset_l0,
+						      pred_weight_table->delta_chroma_weight_l0,
+						      pred_weight_table->chroma_offset_l0,
+						      slice_params->num_ref_idx_l0_active_minus1 + 1,
+						      VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_LUMA_L0,
+						      VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_CHROMA_L0);
+	}
+
+	/* Reference picture list 1 (for B frames). */
+	if (slice_params->slice_type == V4L2_HEVC_SLICE_TYPE_B) {
+		cedrus_h265_ref_pic_list_write(dev, decode_params->dpb,
+					       slice_params->ref_idx_l1,
+					       slice_params->num_ref_idx_l1_active_minus1 + 1,
+					       VE_DEC_H265_SRAM_OFFSET_REF_PIC_LIST1);
+
+		if (pps->flags & V4L2_HEVC_PPS_FLAG_WEIGHTED_BIPRED)
+			cedrus_h265_pred_weight_write(dev,
+						      pred_weight_table->delta_luma_weight_l1,
+						      pred_weight_table->luma_offset_l1,
+						      pred_weight_table->delta_chroma_weight_l1,
+						      pred_weight_table->chroma_offset_l1,
+						      slice_params->num_ref_idx_l1_active_minus1 + 1,
+						      VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_LUMA_L1,
+						      VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_CHROMA_L1);
+	}
+
+	/* Enable appropriate interruptions. */
+	cedrus_write(dev, VE_DEC_H265_CTRL, VE_DEC_H265_CTRL_IRQ_MASK);
+
+	return 0;
+}
+
+static int cedrus_h265_start(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	/* The buffer size is calculated at setup time. */
+	ctx->codec.h265.mv_col_buf_size = 0;
+
+	/* Buffer is never accessed by CPU, so we can skip kernel mapping. */
+	ctx->codec.h265.neighbor_info_buf =
+		dma_alloc_attrs(dev->dev, CEDRUS_H265_NEIGHBOR_INFO_BUF_SIZE,
+				&ctx->codec.h265.neighbor_info_buf_addr,
+				GFP_KERNEL, DMA_ATTR_NO_KERNEL_MAPPING);
+	if (!ctx->codec.h265.neighbor_info_buf)
+		return -ENOMEM;
+
+	ctx->codec.h265.entry_points_buf =
+		dma_alloc_coherent(dev->dev, CEDRUS_H265_ENTRY_POINTS_BUF_SIZE,
+				   &ctx->codec.h265.entry_points_buf_addr,
+				   GFP_KERNEL);
+	if (!ctx->codec.h265.entry_points_buf) {
+		dma_free_attrs(dev->dev, CEDRUS_H265_NEIGHBOR_INFO_BUF_SIZE,
+			       ctx->codec.h265.neighbor_info_buf,
+			       ctx->codec.h265.neighbor_info_buf_addr,
+			       DMA_ATTR_NO_KERNEL_MAPPING);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void cedrus_h265_stop(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	if (ctx->codec.h265.mv_col_buf_size > 0) {
+		dma_free_attrs(dev->dev, ctx->codec.h265.mv_col_buf_size,
+			       ctx->codec.h265.mv_col_buf,
+			       ctx->codec.h265.mv_col_buf_addr,
+			       DMA_ATTR_NO_KERNEL_MAPPING);
+
+		ctx->codec.h265.mv_col_buf_size = 0;
+	}
+
+	dma_free_attrs(dev->dev, CEDRUS_H265_NEIGHBOR_INFO_BUF_SIZE,
+		       ctx->codec.h265.neighbor_info_buf,
+		       ctx->codec.h265.neighbor_info_buf_addr,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+	dma_free_coherent(dev->dev, CEDRUS_H265_ENTRY_POINTS_BUF_SIZE,
+			  ctx->codec.h265.entry_points_buf,
+			  ctx->codec.h265.entry_points_buf_addr);
+}
+
+static void cedrus_h265_trigger(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_DEC_H265_TRIGGER, VE_DEC_H265_TRIGGER_DEC_SLICE);
+}
+
+struct cedrus_dec_ops cedrus_dec_ops_h265 = {
+	.irq_clear	= cedrus_h265_irq_clear,
+	.irq_disable	= cedrus_h265_irq_disable,
+	.irq_status	= cedrus_h265_irq_status,
+	.setup		= cedrus_h265_setup,
+	.start		= cedrus_h265_start,
+	.stop		= cedrus_h265_stop,
+	.trigger	= cedrus_h265_trigger,
+};
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_hw.c b/drivers/staging/media/sunxi/cedrus/cedrus_hw.c
new file mode 100644
index 000000000..fe5fbf6cf
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_hw.c
@@ -0,0 +1,341 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#include <linux/platform_device.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/of_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/soc/sunxi/sunxi_sram.h>
+
+#include <media/videobuf2-core.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "cedrus.h"
+#include "cedrus_hw.h"
+#include "cedrus_regs.h"
+
+int cedrus_engine_enable(struct cedrus_ctx *ctx, enum cedrus_codec codec)
+{
+	u32 reg = 0;
+
+	/*
+	 * FIXME: This is only valid on 32-bits DDR's, we should test
+	 * it on the A13/A33.
+	 */
+	reg |= VE_MODE_REC_WR_MODE_2MB;
+	reg |= VE_MODE_DDR_MODE_BW_128;
+
+	switch (codec) {
+	case CEDRUS_CODEC_MPEG2:
+		reg |= VE_MODE_DEC_MPEG;
+		break;
+
+	/* H.264 and VP8 both use the same decoding mode bit. */
+	case CEDRUS_CODEC_H264:
+	case CEDRUS_CODEC_VP8:
+		reg |= VE_MODE_DEC_H264;
+		break;
+
+	case CEDRUS_CODEC_H265:
+		reg |= VE_MODE_DEC_H265;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (ctx->src_fmt.width == 4096)
+		reg |= VE_MODE_PIC_WIDTH_IS_4096;
+	if (ctx->src_fmt.width > 2048)
+		reg |= VE_MODE_PIC_WIDTH_MORE_2048;
+
+	cedrus_write(ctx->dev, VE_MODE, reg);
+
+	return 0;
+}
+
+void cedrus_engine_disable(struct cedrus_dev *dev)
+{
+	cedrus_write(dev, VE_MODE, VE_MODE_DISABLED);
+}
+
+void cedrus_dst_format_set(struct cedrus_dev *dev,
+			   struct v4l2_pix_format *fmt)
+{
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	u32 chroma_size;
+	u32 reg;
+
+	switch (fmt->pixelformat) {
+	case V4L2_PIX_FMT_NV12:
+		chroma_size = ALIGN(width, 16) * ALIGN(height, 16) / 2;
+
+		reg = VE_PRIMARY_OUT_FMT_NV12;
+		cedrus_write(dev, VE_PRIMARY_OUT_FMT, reg);
+
+		reg = chroma_size / 2;
+		cedrus_write(dev, VE_PRIMARY_CHROMA_BUF_LEN, reg);
+
+		reg = VE_PRIMARY_FB_LINE_STRIDE_LUMA(ALIGN(width, 16)) |
+		      VE_PRIMARY_FB_LINE_STRIDE_CHROMA(ALIGN(width, 16) / 2);
+		cedrus_write(dev, VE_PRIMARY_FB_LINE_STRIDE, reg);
+
+		break;
+	case V4L2_PIX_FMT_NV12_32L32:
+	default:
+		reg = VE_PRIMARY_OUT_FMT_TILED_32_NV12;
+		cedrus_write(dev, VE_PRIMARY_OUT_FMT, reg);
+
+		reg = VE_SECONDARY_OUT_FMT_TILED_32_NV12;
+		cedrus_write(dev, VE_CHROMA_BUF_LEN, reg);
+
+		break;
+	}
+}
+
+static irqreturn_t cedrus_irq(int irq, void *data)
+{
+	struct cedrus_dev *dev = data;
+	struct cedrus_ctx *ctx;
+	enum vb2_buffer_state state;
+	enum cedrus_irq_status status;
+
+	/*
+	 * If cancel_delayed_work returns false it means watchdog already
+	 * executed and finished the job.
+	 */
+	if (!cancel_delayed_work(&dev->watchdog_work))
+		return IRQ_HANDLED;
+
+	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
+	if (!ctx) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Instance released before the end of transaction\n");
+		return IRQ_NONE;
+	}
+
+	status = dev->dec_ops[ctx->current_codec]->irq_status(ctx);
+	if (status == CEDRUS_IRQ_NONE)
+		return IRQ_NONE;
+
+	dev->dec_ops[ctx->current_codec]->irq_disable(ctx);
+	dev->dec_ops[ctx->current_codec]->irq_clear(ctx);
+
+	if (status == CEDRUS_IRQ_ERROR)
+		state = VB2_BUF_STATE_ERROR;
+	else
+		state = VB2_BUF_STATE_DONE;
+
+	v4l2_m2m_buf_done_and_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx,
+					 state);
+
+	return IRQ_HANDLED;
+}
+
+void cedrus_watchdog(struct work_struct *work)
+{
+	struct cedrus_dev *dev;
+	struct cedrus_ctx *ctx;
+
+	dev = container_of(to_delayed_work(work),
+			   struct cedrus_dev, watchdog_work);
+
+	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
+	if (!ctx)
+		return;
+
+	v4l2_err(&dev->v4l2_dev, "frame processing timed out!\n");
+	reset_control_reset(dev->rstc);
+	v4l2_m2m_buf_done_and_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx,
+					 VB2_BUF_STATE_ERROR);
+}
+
+int cedrus_hw_suspend(struct device *device)
+{
+	struct cedrus_dev *dev = dev_get_drvdata(device);
+
+	clk_disable_unprepare(dev->ram_clk);
+	clk_disable_unprepare(dev->mod_clk);
+	clk_disable_unprepare(dev->ahb_clk);
+
+	reset_control_assert(dev->rstc);
+
+	return 0;
+}
+
+int cedrus_hw_resume(struct device *device)
+{
+	struct cedrus_dev *dev = dev_get_drvdata(device);
+	int ret;
+
+	ret = reset_control_reset(dev->rstc);
+	if (ret) {
+		dev_err(dev->dev, "Failed to apply reset\n");
+
+		return ret;
+	}
+
+	ret = clk_prepare_enable(dev->ahb_clk);
+	if (ret) {
+		dev_err(dev->dev, "Failed to enable AHB clock\n");
+
+		goto err_rst;
+	}
+
+	ret = clk_prepare_enable(dev->mod_clk);
+	if (ret) {
+		dev_err(dev->dev, "Failed to enable MOD clock\n");
+
+		goto err_ahb_clk;
+	}
+
+	ret = clk_prepare_enable(dev->ram_clk);
+	if (ret) {
+		dev_err(dev->dev, "Failed to enable RAM clock\n");
+
+		goto err_mod_clk;
+	}
+
+	return 0;
+
+err_mod_clk:
+	clk_disable_unprepare(dev->mod_clk);
+err_ahb_clk:
+	clk_disable_unprepare(dev->ahb_clk);
+err_rst:
+	reset_control_assert(dev->rstc);
+
+	return ret;
+}
+
+int cedrus_hw_probe(struct cedrus_dev *dev)
+{
+	const struct cedrus_variant *variant;
+	int irq_dec;
+	int ret;
+
+	variant = of_device_get_match_data(dev->dev);
+	if (!variant)
+		return -EINVAL;
+
+	dev->capabilities = variant->capabilities;
+
+	irq_dec = platform_get_irq(dev->pdev, 0);
+	if (irq_dec <= 0)
+		return irq_dec;
+	ret = devm_request_irq(dev->dev, irq_dec, cedrus_irq,
+			       0, dev_name(dev->dev), dev);
+	if (ret) {
+		dev_err(dev->dev, "Failed to request IRQ\n");
+
+		return ret;
+	}
+
+	ret = of_reserved_mem_device_init(dev->dev);
+	if (ret && ret != -ENODEV) {
+		dev_err(dev->dev, "Failed to reserve memory\n");
+
+		return ret;
+	}
+
+	ret = sunxi_sram_claim(dev->dev);
+	if (ret) {
+		dev_err(dev->dev, "Failed to claim SRAM\n");
+
+		goto err_mem;
+	}
+
+	dev->ahb_clk = devm_clk_get(dev->dev, "ahb");
+	if (IS_ERR(dev->ahb_clk)) {
+		dev_err(dev->dev, "Failed to get AHB clock\n");
+
+		ret = PTR_ERR(dev->ahb_clk);
+		goto err_sram;
+	}
+
+	dev->mod_clk = devm_clk_get(dev->dev, "mod");
+	if (IS_ERR(dev->mod_clk)) {
+		dev_err(dev->dev, "Failed to get MOD clock\n");
+
+		ret = PTR_ERR(dev->mod_clk);
+		goto err_sram;
+	}
+
+	dev->ram_clk = devm_clk_get(dev->dev, "ram");
+	if (IS_ERR(dev->ram_clk)) {
+		dev_err(dev->dev, "Failed to get RAM clock\n");
+
+		ret = PTR_ERR(dev->ram_clk);
+		goto err_sram;
+	}
+
+	dev->rstc = devm_reset_control_get(dev->dev, NULL);
+	if (IS_ERR(dev->rstc)) {
+		dev_err(dev->dev, "Failed to get reset control\n");
+
+		ret = PTR_ERR(dev->rstc);
+		goto err_sram;
+	}
+
+	dev->base = devm_platform_ioremap_resource(dev->pdev, 0);
+	if (IS_ERR(dev->base)) {
+		dev_err(dev->dev, "Failed to map registers\n");
+
+		ret = PTR_ERR(dev->base);
+		goto err_sram;
+	}
+
+	ret = clk_set_rate(dev->mod_clk, variant->mod_rate);
+	if (ret) {
+		dev_err(dev->dev, "Failed to set clock rate\n");
+
+		goto err_sram;
+	}
+
+	pm_runtime_enable(dev->dev);
+	if (!pm_runtime_enabled(dev->dev)) {
+		ret = cedrus_hw_resume(dev->dev);
+		if (ret)
+			goto err_pm;
+	}
+
+	return 0;
+
+err_pm:
+	pm_runtime_disable(dev->dev);
+err_sram:
+	sunxi_sram_release(dev->dev);
+err_mem:
+	of_reserved_mem_device_release(dev->dev);
+
+	return ret;
+}
+
+void cedrus_hw_remove(struct cedrus_dev *dev)
+{
+	pm_runtime_disable(dev->dev);
+	if (!pm_runtime_status_suspended(dev->dev))
+		cedrus_hw_suspend(dev->dev);
+
+	sunxi_sram_release(dev->dev);
+
+	of_reserved_mem_device_release(dev->dev);
+}
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_hw.h b/drivers/staging/media/sunxi/cedrus/cedrus_hw.h
new file mode 100644
index 000000000..7c92f00e3
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_hw.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#ifndef _CEDRUS_HW_H_
+#define _CEDRUS_HW_H_
+
+int cedrus_engine_enable(struct cedrus_ctx *ctx, enum cedrus_codec codec);
+void cedrus_engine_disable(struct cedrus_dev *dev);
+
+void cedrus_dst_format_set(struct cedrus_dev *dev,
+			   struct v4l2_pix_format *fmt);
+
+int cedrus_hw_suspend(struct device *device);
+int cedrus_hw_resume(struct device *device);
+
+int cedrus_hw_probe(struct cedrus_dev *dev);
+void cedrus_hw_remove(struct cedrus_dev *dev);
+
+void cedrus_watchdog(struct work_struct *work);
+
+#endif
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_mpeg2.c b/drivers/staging/media/sunxi/cedrus/cedrus_mpeg2.c
new file mode 100644
index 000000000..c1128d2cd
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_mpeg2.c
@@ -0,0 +1,198 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ */
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cedrus.h"
+#include "cedrus_hw.h"
+#include "cedrus_regs.h"
+
+static enum cedrus_irq_status cedrus_mpeg2_irq_status(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg;
+
+	reg = cedrus_read(dev, VE_DEC_MPEG_STATUS);
+	reg &= VE_DEC_MPEG_STATUS_CHECK_MASK;
+
+	if (!reg)
+		return CEDRUS_IRQ_NONE;
+
+	if (reg & VE_DEC_MPEG_STATUS_CHECK_ERROR ||
+	    !(reg & VE_DEC_MPEG_STATUS_SUCCESS))
+		return CEDRUS_IRQ_ERROR;
+
+	return CEDRUS_IRQ_OK;
+}
+
+static void cedrus_mpeg2_irq_clear(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_DEC_MPEG_STATUS, VE_DEC_MPEG_STATUS_CHECK_MASK);
+}
+
+static void cedrus_mpeg2_irq_disable(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_DEC_MPEG_CTRL);
+
+	reg &= ~VE_DEC_MPEG_CTRL_IRQ_MASK;
+
+	cedrus_write(dev, VE_DEC_MPEG_CTRL, reg);
+}
+
+static int cedrus_mpeg2_setup(struct cedrus_ctx *ctx, struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_mpeg2_sequence *seq;
+	const struct v4l2_ctrl_mpeg2_picture *pic;
+	const struct v4l2_ctrl_mpeg2_quantisation *quantisation;
+	dma_addr_t src_buf_addr, dst_luma_addr, dst_chroma_addr;
+	struct cedrus_dev *dev = ctx->dev;
+	struct vb2_queue *vq;
+	const u8 *matrix;
+	unsigned int i;
+	u32 reg;
+
+	seq = run->mpeg2.sequence;
+	pic = run->mpeg2.picture;
+
+	quantisation = run->mpeg2.quantisation;
+
+	/* Activate MPEG engine. */
+	cedrus_engine_enable(ctx, CEDRUS_CODEC_MPEG2);
+
+	/* Set intra quantisation matrix. */
+	matrix = quantisation->intra_quantiser_matrix;
+	for (i = 0; i < 64; i++) {
+		reg = VE_DEC_MPEG_IQMINPUT_WEIGHT(i, matrix[i]);
+		reg |= VE_DEC_MPEG_IQMINPUT_FLAG_INTRA;
+
+		cedrus_write(dev, VE_DEC_MPEG_IQMINPUT, reg);
+	}
+
+	/* Set non-intra quantisation matrix. */
+	matrix = quantisation->non_intra_quantiser_matrix;
+	for (i = 0; i < 64; i++) {
+		reg = VE_DEC_MPEG_IQMINPUT_WEIGHT(i, matrix[i]);
+		reg |= VE_DEC_MPEG_IQMINPUT_FLAG_NON_INTRA;
+
+		cedrus_write(dev, VE_DEC_MPEG_IQMINPUT, reg);
+	}
+
+	/* Set MPEG picture header. */
+
+	reg = VE_DEC_MPEG_MP12HDR_SLICE_TYPE(pic->picture_coding_type);
+	reg |= VE_DEC_MPEG_MP12HDR_F_CODE(0, 0, pic->f_code[0][0]);
+	reg |= VE_DEC_MPEG_MP12HDR_F_CODE(0, 1, pic->f_code[0][1]);
+	reg |= VE_DEC_MPEG_MP12HDR_F_CODE(1, 0, pic->f_code[1][0]);
+	reg |= VE_DEC_MPEG_MP12HDR_F_CODE(1, 1, pic->f_code[1][1]);
+	reg |= VE_DEC_MPEG_MP12HDR_INTRA_DC_PRECISION(pic->intra_dc_precision);
+	reg |= VE_DEC_MPEG_MP12HDR_INTRA_PICTURE_STRUCTURE(pic->picture_structure);
+	reg |= VE_DEC_MPEG_MP12HDR_TOP_FIELD_FIRST(pic->flags & V4L2_MPEG2_PIC_FLAG_TOP_FIELD_FIRST);
+	reg |= VE_DEC_MPEG_MP12HDR_FRAME_PRED_FRAME_DCT(pic->flags & V4L2_MPEG2_PIC_FLAG_FRAME_PRED_DCT);
+	reg |= VE_DEC_MPEG_MP12HDR_CONCEALMENT_MOTION_VECTORS(pic->flags & V4L2_MPEG2_PIC_FLAG_CONCEALMENT_MV);
+	reg |= VE_DEC_MPEG_MP12HDR_Q_SCALE_TYPE(pic->flags & V4L2_MPEG2_PIC_FLAG_Q_SCALE_TYPE);
+	reg |= VE_DEC_MPEG_MP12HDR_INTRA_VLC_FORMAT(pic->flags & V4L2_MPEG2_PIC_FLAG_INTRA_VLC);
+	reg |= VE_DEC_MPEG_MP12HDR_ALTERNATE_SCAN(pic->flags & V4L2_MPEG2_PIC_FLAG_ALT_SCAN);
+	reg |= VE_DEC_MPEG_MP12HDR_FULL_PEL_FORWARD_VECTOR(0);
+	reg |= VE_DEC_MPEG_MP12HDR_FULL_PEL_BACKWARD_VECTOR(0);
+
+	cedrus_write(dev, VE_DEC_MPEG_MP12HDR, reg);
+
+	/* Set frame dimensions. */
+
+	reg = VE_DEC_MPEG_PICCODEDSIZE_WIDTH(seq->horizontal_size);
+	reg |= VE_DEC_MPEG_PICCODEDSIZE_HEIGHT(seq->vertical_size);
+
+	cedrus_write(dev, VE_DEC_MPEG_PICCODEDSIZE, reg);
+
+	reg = VE_DEC_MPEG_PICBOUNDSIZE_WIDTH(ctx->src_fmt.width);
+	reg |= VE_DEC_MPEG_PICBOUNDSIZE_HEIGHT(ctx->src_fmt.height);
+
+	cedrus_write(dev, VE_DEC_MPEG_PICBOUNDSIZE, reg);
+
+	/* Forward and backward prediction reference buffers. */
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	cedrus_write_ref_buf_addr(ctx, vq, pic->forward_ref_ts,
+				  VE_DEC_MPEG_FWD_REF_LUMA_ADDR,
+				  VE_DEC_MPEG_FWD_REF_CHROMA_ADDR);
+	cedrus_write_ref_buf_addr(ctx, vq, pic->backward_ref_ts,
+				  VE_DEC_MPEG_BWD_REF_LUMA_ADDR,
+				  VE_DEC_MPEG_BWD_REF_CHROMA_ADDR);
+
+	/* Destination luma and chroma buffers. */
+
+	dst_luma_addr = cedrus_dst_buf_addr(ctx, &run->dst->vb2_buf, 0);
+	dst_chroma_addr = cedrus_dst_buf_addr(ctx, &run->dst->vb2_buf, 1);
+
+	cedrus_write(dev, VE_DEC_MPEG_REC_LUMA, dst_luma_addr);
+	cedrus_write(dev, VE_DEC_MPEG_REC_CHROMA, dst_chroma_addr);
+
+	/* Source offset and length in bits. */
+
+	cedrus_write(dev, VE_DEC_MPEG_VLD_OFFSET, 0);
+
+	reg = vb2_get_plane_payload(&run->src->vb2_buf, 0) * 8;
+	cedrus_write(dev, VE_DEC_MPEG_VLD_LEN, reg);
+
+	/* Source beginning and end addresses. */
+
+	src_buf_addr = vb2_dma_contig_plane_dma_addr(&run->src->vb2_buf, 0);
+
+	reg = VE_DEC_MPEG_VLD_ADDR_BASE(src_buf_addr);
+	reg |= VE_DEC_MPEG_VLD_ADDR_VALID_PIC_DATA;
+	reg |= VE_DEC_MPEG_VLD_ADDR_LAST_PIC_DATA;
+	reg |= VE_DEC_MPEG_VLD_ADDR_FIRST_PIC_DATA;
+
+	cedrus_write(dev, VE_DEC_MPEG_VLD_ADDR, reg);
+
+	reg = src_buf_addr + vb2_get_plane_payload(&run->src->vb2_buf, 0);
+	cedrus_write(dev, VE_DEC_MPEG_VLD_END_ADDR, reg);
+
+	/* Macroblock address: start at the beginning. */
+	reg = VE_DEC_MPEG_MBADDR_Y(0) | VE_DEC_MPEG_MBADDR_X(0);
+	cedrus_write(dev, VE_DEC_MPEG_MBADDR, reg);
+
+	/* Clear previous errors. */
+	cedrus_write(dev, VE_DEC_MPEG_ERROR, 0);
+
+	/* Clear correct macroblocks register. */
+	cedrus_write(dev, VE_DEC_MPEG_CRTMBADDR, 0);
+
+	/* Enable appropriate interruptions and components. */
+
+	reg = VE_DEC_MPEG_CTRL_IRQ_MASK | VE_DEC_MPEG_CTRL_MC_NO_WRITEBACK |
+	      VE_DEC_MPEG_CTRL_MC_CACHE_EN;
+
+	cedrus_write(dev, VE_DEC_MPEG_CTRL, reg);
+
+	return 0;
+}
+
+static void cedrus_mpeg2_trigger(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg;
+
+	/* Trigger MPEG engine. */
+	reg = VE_DEC_MPEG_TRIGGER_HW_MPEG_VLD | VE_DEC_MPEG_TRIGGER_MPEG2 |
+	      VE_DEC_MPEG_TRIGGER_MB_BOUNDARY;
+
+	cedrus_write(dev, VE_DEC_MPEG_TRIGGER, reg);
+}
+
+struct cedrus_dec_ops cedrus_dec_ops_mpeg2 = {
+	.irq_clear	= cedrus_mpeg2_irq_clear,
+	.irq_disable	= cedrus_mpeg2_irq_disable,
+	.irq_status	= cedrus_mpeg2_irq_status,
+	.setup		= cedrus_mpeg2_setup,
+	.trigger	= cedrus_mpeg2_trigger,
+};
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_regs.h b/drivers/staging/media/sunxi/cedrus/cedrus_regs.h
new file mode 100644
index 000000000..655c05b38
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_regs.h
@@ -0,0 +1,701 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (c) 2013-2016 Jens Kuske <jenskuske@gmail.com>
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ */
+
+#ifndef _CEDRUS_REGS_H_
+#define _CEDRUS_REGS_H_
+
+#define SHIFT_AND_MASK_BITS(v, h, l) \
+	(((unsigned long)(v) << (l)) & GENMASK(h, l))
+
+/*
+ * Common acronyms and contractions used in register descriptions:
+ * * VLD : Variable-Length Decoder
+ * * IQ: Inverse Quantization
+ * * IDCT: Inverse Discrete Cosine Transform
+ * * MC: Motion Compensation
+ * * STCD: Start Code Detect
+ * * SDRT: Scale Down and Rotate
+ * * WB: Writeback
+ * * BITS/BS: Bitstream
+ * * MB: Macroblock
+ * * CTU: Coding Tree Unit
+ * * CTB: Coding Tree Block
+ * * IDX: Index
+ */
+
+#define VE_ENGINE_DEC_MPEG			0x100
+#define VE_ENGINE_DEC_H264			0x200
+#define VE_ENGINE_DEC_H265			0x500
+
+#define VE_MODE					0x00
+
+#define VE_MODE_PIC_WIDTH_IS_4096		BIT(22)
+#define VE_MODE_PIC_WIDTH_MORE_2048		BIT(21)
+#define VE_MODE_REC_WR_MODE_2MB			(0x01 << 20)
+#define VE_MODE_REC_WR_MODE_1MB			(0x00 << 20)
+#define VE_MODE_DDR_MODE_BW_128			(0x03 << 16)
+#define VE_MODE_DDR_MODE_BW_256			(0x02 << 16)
+#define VE_MODE_DISABLED			(0x07 << 0)
+#define VE_MODE_DEC_H265			(0x04 << 0)
+#define VE_MODE_DEC_H264			(0x01 << 0)
+#define VE_MODE_DEC_MPEG			(0x00 << 0)
+
+#define VE_BUF_CTRL				0x50
+
+#define VE_BUF_CTRL_INTRAPRED_EXT_RAM		(0x02 << 2)
+#define VE_BUF_CTRL_INTRAPRED_MIXED_RAM		(0x01 << 2)
+#define VE_BUF_CTRL_INTRAPRED_INT_SRAM		(0x00 << 2)
+#define VE_BUF_CTRL_DBLK_EXT_RAM		(0x02 << 0)
+#define VE_BUF_CTRL_DBLK_MIXED_RAM		(0x01 << 0)
+#define VE_BUF_CTRL_DBLK_INT_SRAM		(0x00 << 0)
+
+#define VE_DBLK_DRAM_BUF_ADDR			0x54
+#define VE_INTRAPRED_DRAM_BUF_ADDR		0x58
+#define VE_PRIMARY_CHROMA_BUF_LEN		0xc4
+#define VE_PRIMARY_FB_LINE_STRIDE		0xc8
+
+#define VE_PRIMARY_FB_LINE_STRIDE_CHROMA(s)	SHIFT_AND_MASK_BITS(s, 31, 16)
+#define VE_PRIMARY_FB_LINE_STRIDE_LUMA(s)	SHIFT_AND_MASK_BITS(s, 15, 0)
+
+#define VE_CHROMA_BUF_LEN			0xe8
+
+#define VE_SECONDARY_OUT_FMT_TILED_32_NV12	(0x00 << 30)
+#define VE_SECONDARY_OUT_FMT_EXT		(0x01 << 30)
+#define VE_SECONDARY_OUT_FMT_YU12		(0x02 << 30)
+#define VE_SECONDARY_OUT_FMT_YV12		(0x03 << 30)
+#define VE_CHROMA_BUF_LEN_SDRT(l)		SHIFT_AND_MASK_BITS(l, 27, 0)
+
+#define VE_PRIMARY_OUT_FMT			0xec
+
+#define VE_PRIMARY_OUT_FMT_TILED_32_NV12	(0x00 << 4)
+#define VE_PRIMARY_OUT_FMT_TILED_128_NV12	(0x01 << 4)
+#define VE_PRIMARY_OUT_FMT_YU12			(0x02 << 4)
+#define VE_PRIMARY_OUT_FMT_YV12			(0x03 << 4)
+#define VE_PRIMARY_OUT_FMT_NV12			(0x04 << 4)
+#define VE_PRIMARY_OUT_FMT_NV21			(0x05 << 4)
+#define VE_SECONDARY_OUT_FMT_EXT_TILED_32_NV12	(0x00 << 0)
+#define VE_SECONDARY_OUT_FMT_EXT_TILED_128_NV12	(0x01 << 0)
+#define VE_SECONDARY_OUT_FMT_EXT_YU12		(0x02 << 0)
+#define VE_SECONDARY_OUT_FMT_EXT_YV12		(0x03 << 0)
+#define VE_SECONDARY_OUT_FMT_EXT_NV12		(0x04 << 0)
+#define VE_SECONDARY_OUT_FMT_EXT_NV21		(0x05 << 0)
+
+#define VE_VERSION				0xf0
+
+#define VE_VERSION_SHIFT			16
+
+#define VE_DEC_MPEG_MP12HDR			(VE_ENGINE_DEC_MPEG + 0x00)
+
+#define VE_DEC_MPEG_MP12HDR_SLICE_TYPE(t)	SHIFT_AND_MASK_BITS(t, 30, 28)
+#define VE_DEC_MPEG_MP12HDR_F_CODE_SHIFT(x, y)	(24 - 4 * (y) - 8 * (x))
+#define VE_DEC_MPEG_MP12HDR_F_CODE(__x, __y, __v) \
+	(((unsigned long)(__v) & GENMASK(3, 0)) << VE_DEC_MPEG_MP12HDR_F_CODE_SHIFT(__x, __y))
+
+#define VE_DEC_MPEG_MP12HDR_INTRA_DC_PRECISION(p) \
+	SHIFT_AND_MASK_BITS(p, 11, 10)
+#define VE_DEC_MPEG_MP12HDR_INTRA_PICTURE_STRUCTURE(s) \
+	SHIFT_AND_MASK_BITS(s, 9, 8)
+#define VE_DEC_MPEG_MP12HDR_TOP_FIELD_FIRST(v) \
+	((v) ? BIT(7) : 0)
+#define VE_DEC_MPEG_MP12HDR_FRAME_PRED_FRAME_DCT(v) \
+	((v) ? BIT(6) : 0)
+#define VE_DEC_MPEG_MP12HDR_CONCEALMENT_MOTION_VECTORS(v) \
+	((v) ? BIT(5) : 0)
+#define VE_DEC_MPEG_MP12HDR_Q_SCALE_TYPE(v) \
+	((v) ? BIT(4) : 0)
+#define VE_DEC_MPEG_MP12HDR_INTRA_VLC_FORMAT(v) \
+	((v) ? BIT(3) : 0)
+#define VE_DEC_MPEG_MP12HDR_ALTERNATE_SCAN(v) \
+	((v) ? BIT(2) : 0)
+#define VE_DEC_MPEG_MP12HDR_FULL_PEL_FORWARD_VECTOR(v) \
+	((v) ? BIT(1) : 0)
+#define VE_DEC_MPEG_MP12HDR_FULL_PEL_BACKWARD_VECTOR(v) \
+	((v) ? BIT(0) : 0)
+
+#define VE_DEC_MPEG_PICCODEDSIZE		(VE_ENGINE_DEC_MPEG + 0x08)
+
+#define VE_DEC_MPEG_PICCODEDSIZE_WIDTH(w) \
+	SHIFT_AND_MASK_BITS(DIV_ROUND_UP(w, 16), 15, 8)
+#define VE_DEC_MPEG_PICCODEDSIZE_HEIGHT(h) \
+	SHIFT_AND_MASK_BITS(DIV_ROUND_UP(h, 16), 7, 0)
+
+#define VE_DEC_MPEG_PICBOUNDSIZE		(VE_ENGINE_DEC_MPEG + 0x0c)
+
+#define VE_DEC_MPEG_PICBOUNDSIZE_WIDTH(w)	SHIFT_AND_MASK_BITS(w, 27, 16)
+#define VE_DEC_MPEG_PICBOUNDSIZE_HEIGHT(h)	SHIFT_AND_MASK_BITS(h, 11, 0)
+
+#define VE_DEC_MPEG_MBADDR			(VE_ENGINE_DEC_MPEG + 0x10)
+
+#define VE_DEC_MPEG_MBADDR_X(w)			SHIFT_AND_MASK_BITS(w, 15, 8)
+#define VE_DEC_MPEG_MBADDR_Y(h)			SHIFT_AND_MASK_BITS(h, 7, 0)
+
+#define VE_DEC_MPEG_CTRL			(VE_ENGINE_DEC_MPEG + 0x14)
+
+#define VE_DEC_MPEG_CTRL_MC_CACHE_EN		BIT(31)
+#define VE_DEC_MPEG_CTRL_SW_VLD			BIT(27)
+#define VE_DEC_MPEG_CTRL_SW_IQ_IS		BIT(17)
+#define VE_DEC_MPEG_CTRL_QP_AC_DC_OUT_EN	BIT(14)
+#define VE_DEC_MPEG_CTRL_ROTATE_SCALE_OUT_EN	BIT(8)
+#define VE_DEC_MPEG_CTRL_MC_NO_WRITEBACK	BIT(7)
+#define VE_DEC_MPEG_CTRL_ROTATE_IRQ_EN		BIT(6)
+#define VE_DEC_MPEG_CTRL_VLD_DATA_REQ_IRQ_EN	BIT(5)
+#define VE_DEC_MPEG_CTRL_ERROR_IRQ_EN		BIT(4)
+#define VE_DEC_MPEG_CTRL_FINISH_IRQ_EN		BIT(3)
+#define VE_DEC_MPEG_CTRL_IRQ_MASK \
+	(VE_DEC_MPEG_CTRL_FINISH_IRQ_EN | VE_DEC_MPEG_CTRL_ERROR_IRQ_EN | \
+	 VE_DEC_MPEG_CTRL_VLD_DATA_REQ_IRQ_EN)
+
+#define VE_DEC_MPEG_TRIGGER			(VE_ENGINE_DEC_MPEG + 0x18)
+
+#define VE_DEC_MPEG_TRIGGER_MB_BOUNDARY		BIT(31)
+
+#define VE_DEC_MPEG_TRIGGER_CHROMA_FMT_420	(0x00 << 27)
+#define VE_DEC_MPEG_TRIGGER_CHROMA_FMT_411	(0x01 << 27)
+#define VE_DEC_MPEG_TRIGGER_CHROMA_FMT_422	(0x02 << 27)
+#define VE_DEC_MPEG_TRIGGER_CHROMA_FMT_444	(0x03 << 27)
+#define VE_DEC_MPEG_TRIGGER_CHROMA_FMT_422T	(0x04 << 27)
+
+#define VE_DEC_MPEG_TRIGGER_MPEG1		(0x01 << 24)
+#define VE_DEC_MPEG_TRIGGER_MPEG2		(0x02 << 24)
+#define VE_DEC_MPEG_TRIGGER_JPEG		(0x03 << 24)
+#define VE_DEC_MPEG_TRIGGER_MPEG4		(0x04 << 24)
+#define VE_DEC_MPEG_TRIGGER_VP62		(0x05 << 24)
+
+#define VE_DEC_MPEG_TRIGGER_VP62_AC_GET_BITS	BIT(7)
+
+#define VE_DEC_MPEG_TRIGGER_STCD_VC1		(0x02 << 4)
+#define VE_DEC_MPEG_TRIGGER_STCD_MPEG2		(0x01 << 4)
+#define VE_DEC_MPEG_TRIGGER_STCD_AVC		(0x00 << 4)
+
+#define VE_DEC_MPEG_TRIGGER_HW_MPEG_VLD		(0x0f << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_JPEG_VLD		(0x0e << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_MB		(0x0d << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_ROTATE		(0x0c << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_VP6_VLD		(0x0b << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_MAF		(0x0a << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_STCD_END		(0x09 << 0)
+#define VE_DEC_MPEG_TRIGGER_HW_STCD_BEGIN	(0x08 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_MC		(0x07 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_IQ		(0x06 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_IDCT		(0x05 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_SCALE		(0x04 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_VP6		(0x03 << 0)
+#define VE_DEC_MPEG_TRIGGER_SW_VP62_AC_GET_BITS	(0x02 << 0)
+
+#define VE_DEC_MPEG_STATUS			(VE_ENGINE_DEC_MPEG + 0x1c)
+
+#define VE_DEC_MPEG_STATUS_START_DETECT_BUSY	BIT(27)
+#define VE_DEC_MPEG_STATUS_VP6_BIT		BIT(26)
+#define VE_DEC_MPEG_STATUS_VP6_BIT_BUSY		BIT(25)
+#define VE_DEC_MPEG_STATUS_MAF_BUSY		BIT(23)
+#define VE_DEC_MPEG_STATUS_VP6_MVP_BUSY		BIT(22)
+#define VE_DEC_MPEG_STATUS_JPEG_BIT_END		BIT(21)
+#define VE_DEC_MPEG_STATUS_JPEG_RESTART_ERROR	BIT(20)
+#define VE_DEC_MPEG_STATUS_JPEG_MARKER		BIT(19)
+#define VE_DEC_MPEG_STATUS_ROTATE_BUSY		BIT(18)
+#define VE_DEC_MPEG_STATUS_DEBLOCKING_BUSY	BIT(17)
+#define VE_DEC_MPEG_STATUS_SCALE_DOWN_BUSY	BIT(16)
+#define VE_DEC_MPEG_STATUS_IQIS_BUF_EMPTY	BIT(15)
+#define VE_DEC_MPEG_STATUS_IDCT_BUF_EMPTY	BIT(14)
+#define VE_DEC_MPEG_STATUS_VE_BUSY		BIT(13)
+#define VE_DEC_MPEG_STATUS_MC_BUSY		BIT(12)
+#define VE_DEC_MPEG_STATUS_IDCT_BUSY		BIT(11)
+#define VE_DEC_MPEG_STATUS_IQIS_BUSY		BIT(10)
+#define VE_DEC_MPEG_STATUS_DCAC_BUSY		BIT(9)
+#define VE_DEC_MPEG_STATUS_VLD_BUSY		BIT(8)
+#define VE_DEC_MPEG_STATUS_ROTATE_SUCCESS	BIT(3)
+#define VE_DEC_MPEG_STATUS_VLD_DATA_REQ		BIT(2)
+#define VE_DEC_MPEG_STATUS_ERROR		BIT(1)
+#define VE_DEC_MPEG_STATUS_SUCCESS		BIT(0)
+#define VE_DEC_MPEG_STATUS_CHECK_MASK \
+	(VE_DEC_MPEG_STATUS_SUCCESS | VE_DEC_MPEG_STATUS_ERROR | \
+	 VE_DEC_MPEG_STATUS_VLD_DATA_REQ)
+#define VE_DEC_MPEG_STATUS_CHECK_ERROR \
+	(VE_DEC_MPEG_STATUS_ERROR | VE_DEC_MPEG_STATUS_VLD_DATA_REQ)
+
+#define VE_DEC_MPEG_VLD_ADDR			(VE_ENGINE_DEC_MPEG + 0x28)
+
+#define VE_DEC_MPEG_VLD_ADDR_FIRST_PIC_DATA	BIT(30)
+#define VE_DEC_MPEG_VLD_ADDR_LAST_PIC_DATA	BIT(29)
+#define VE_DEC_MPEG_VLD_ADDR_VALID_PIC_DATA	BIT(28)
+#define VE_DEC_MPEG_VLD_ADDR_BASE(a)					\
+	({								\
+		u32 _tmp = (a);						\
+		u32 _lo = _tmp & GENMASK(27, 4);			\
+		u32 _hi = (_tmp >> 28) & GENMASK(3, 0);			\
+		(_lo | _hi);						\
+	})
+
+#define VE_DEC_MPEG_VLD_OFFSET			(VE_ENGINE_DEC_MPEG + 0x2c)
+#define VE_DEC_MPEG_VLD_LEN			(VE_ENGINE_DEC_MPEG + 0x30)
+#define VE_DEC_MPEG_VLD_END_ADDR		(VE_ENGINE_DEC_MPEG + 0x34)
+
+#define VE_DEC_MPEG_REC_LUMA			(VE_ENGINE_DEC_MPEG + 0x48)
+#define VE_DEC_MPEG_REC_CHROMA			(VE_ENGINE_DEC_MPEG + 0x4c)
+#define VE_DEC_MPEG_FWD_REF_LUMA_ADDR		(VE_ENGINE_DEC_MPEG + 0x50)
+#define VE_DEC_MPEG_FWD_REF_CHROMA_ADDR		(VE_ENGINE_DEC_MPEG + 0x54)
+#define VE_DEC_MPEG_BWD_REF_LUMA_ADDR		(VE_ENGINE_DEC_MPEG + 0x58)
+#define VE_DEC_MPEG_BWD_REF_CHROMA_ADDR		(VE_ENGINE_DEC_MPEG + 0x5c)
+
+#define VE_DEC_MPEG_IQMINPUT			(VE_ENGINE_DEC_MPEG + 0x80)
+
+#define VE_DEC_MPEG_IQMINPUT_FLAG_INTRA		(0x01 << 14)
+#define VE_DEC_MPEG_IQMINPUT_FLAG_NON_INTRA	(0x00 << 14)
+#define VE_DEC_MPEG_IQMINPUT_WEIGHT(i, v) \
+	(SHIFT_AND_MASK_BITS(i, 13, 8) | SHIFT_AND_MASK_BITS(v, 7, 0))
+
+#define VE_DEC_MPEG_ERROR			(VE_ENGINE_DEC_MPEG + 0xc4)
+#define VE_DEC_MPEG_CRTMBADDR			(VE_ENGINE_DEC_MPEG + 0xc8)
+#define VE_DEC_MPEG_ROT_LUMA			(VE_ENGINE_DEC_MPEG + 0xcc)
+#define VE_DEC_MPEG_ROT_CHROMA			(VE_ENGINE_DEC_MPEG + 0xd0)
+
+#define VE_DEC_H265_DEC_NAL_HDR			(VE_ENGINE_DEC_H265 + 0x00)
+
+#define VE_DEC_H265_DEC_NAL_HDR_NUH_TEMPORAL_ID_PLUS1(v) \
+	SHIFT_AND_MASK_BITS(v, 8, 6)
+#define VE_DEC_H265_DEC_NAL_HDR_NAL_UNIT_TYPE(v) \
+	SHIFT_AND_MASK_BITS(v, 5, 0)
+
+#define VE_DEC_H265_FLAG(reg_flag, ctrl_flag, flags) \
+	(((flags) & (ctrl_flag)) ? reg_flag : 0)
+
+#define VE_DEC_H265_DEC_SPS_HDR			(VE_ENGINE_DEC_H265 + 0x04)
+
+#define VE_DEC_H265_DEC_SPS_HDR_FLAG_STRONG_INTRA_SMOOTHING_ENABLE	BIT(26)
+#define VE_DEC_H265_DEC_SPS_HDR_FLAG_SPS_TEMPORAL_MVP_ENABLED		BIT(25)
+#define VE_DEC_H265_DEC_SPS_HDR_FLAG_SAMPLE_ADAPTIVE_OFFSET_ENABLED	BIT(24)
+#define VE_DEC_H265_DEC_SPS_HDR_FLAG_AMP_ENABLED			BIT(23)
+#define VE_DEC_H265_DEC_SPS_HDR_FLAG_SEPARATE_COLOUR_PLANE		BIT(2)
+
+#define VE_DEC_H265_DEC_SPS_HDR_MAX_TRANSFORM_HIERARCHY_DEPTH_INTRA(v) \
+	SHIFT_AND_MASK_BITS(v, 22, 20)
+#define VE_DEC_H265_DEC_SPS_HDR_MAX_TRANSFORM_HIERARCHY_DEPTH_INTER(v) \
+	SHIFT_AND_MASK_BITS(v, 19, 17)
+#define VE_DEC_H265_DEC_SPS_HDR_LOG2_DIFF_MAX_MIN_TRANSFORM_BLOCK_SIZE(v) \
+	SHIFT_AND_MASK_BITS(v, 16, 15)
+#define VE_DEC_H265_DEC_SPS_HDR_LOG2_MIN_TRANSFORM_BLOCK_SIZE_MINUS2(v) \
+	SHIFT_AND_MASK_BITS(v, 14, 13)
+#define VE_DEC_H265_DEC_SPS_HDR_LOG2_DIFF_MAX_MIN_LUMA_CODING_BLOCK_SIZE(v) \
+	SHIFT_AND_MASK_BITS(v, 12, 11)
+#define VE_DEC_H265_DEC_SPS_HDR_LOG2_MIN_LUMA_CODING_BLOCK_SIZE_MINUS3(v) \
+	SHIFT_AND_MASK_BITS(v, 10, 9)
+#define VE_DEC_H265_DEC_SPS_HDR_BIT_DEPTH_CHROMA_MINUS8(v) \
+	SHIFT_AND_MASK_BITS(v, 8, 6)
+#define VE_DEC_H265_DEC_SPS_HDR_BIT_DEPTH_LUMA_MINUS8(v) \
+	SHIFT_AND_MASK_BITS(v, 5, 3)
+#define VE_DEC_H265_DEC_SPS_HDR_CHROMA_FORMAT_IDC(v) \
+	SHIFT_AND_MASK_BITS(v, 1, 0)
+
+#define VE_DEC_H265_DEC_PIC_SIZE		(VE_ENGINE_DEC_H265 + 0x08)
+
+#define VE_DEC_H265_DEC_PIC_SIZE_WIDTH(w)	(((w) << 0) & GENMASK(13, 0))
+#define VE_DEC_H265_DEC_PIC_SIZE_HEIGHT(h)	(((h) << 16) & GENMASK(29, 16))
+
+#define VE_DEC_H265_DEC_PCM_CTRL		(VE_ENGINE_DEC_H265 + 0x0c)
+
+#define VE_DEC_H265_DEC_PCM_CTRL_FLAG_PCM_ENABLED		BIT(15)
+#define VE_DEC_H265_DEC_PCM_CTRL_FLAG_PCM_LOOP_FILTER_DISABLED	BIT(14)
+
+#define VE_DEC_H265_DEC_PCM_CTRL_LOG2_DIFF_MAX_MIN_PCM_LUMA_CODING_BLOCK_SIZE(v) \
+	SHIFT_AND_MASK_BITS(v, 11, 10)
+#define VE_DEC_H265_DEC_PCM_CTRL_LOG2_MIN_PCM_LUMA_CODING_BLOCK_SIZE_MINUS3(v) \
+	SHIFT_AND_MASK_BITS(v, 9, 8)
+#define VE_DEC_H265_DEC_PCM_CTRL_PCM_SAMPLE_BIT_DEPTH_CHROMA_MINUS1(v) \
+	SHIFT_AND_MASK_BITS(v, 7, 4)
+#define VE_DEC_H265_DEC_PCM_CTRL_PCM_SAMPLE_BIT_DEPTH_LUMA_MINUS1(v) \
+	SHIFT_AND_MASK_BITS(v, 3, 0)
+
+#define VE_DEC_H265_DEC_PPS_CTRL0		(VE_ENGINE_DEC_H265 + 0x10)
+
+#define VE_DEC_H265_DEC_PPS_CTRL0_FLAG_CU_QP_DELTA_ENABLED	BIT(3)
+#define VE_DEC_H265_DEC_PPS_CTRL0_FLAG_TRANSFORM_SKIP_ENABLED	BIT(2)
+#define VE_DEC_H265_DEC_PPS_CTRL0_FLAG_CONSTRAINED_INTRA_PRED	BIT(1)
+#define VE_DEC_H265_DEC_PPS_CTRL0_FLAG_SIGN_DATA_HIDING_ENABLED	BIT(0)
+
+#define VE_DEC_H265_DEC_PPS_CTRL0_PPS_CR_QP_OFFSET(v) \
+	SHIFT_AND_MASK_BITS(v, 29, 24)
+#define VE_DEC_H265_DEC_PPS_CTRL0_PPS_CB_QP_OFFSET(v) \
+	SHIFT_AND_MASK_BITS(v, 21, 16)
+#define VE_DEC_H265_DEC_PPS_CTRL0_INIT_QP_MINUS26(v) \
+	SHIFT_AND_MASK_BITS(v, 14, 8)
+#define VE_DEC_H265_DEC_PPS_CTRL0_DIFF_CU_QP_DELTA_DEPTH(v) \
+	SHIFT_AND_MASK_BITS(v, 5, 4)
+
+#define VE_DEC_H265_DEC_PPS_CTRL1		(VE_ENGINE_DEC_H265 + 0x14)
+
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_PPS_LOOP_FILTER_ACROSS_SLICES_ENABLED BIT(6)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_LOOP_FILTER_ACROSS_TILES_ENABLED	BIT(5)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_ENTROPY_CODING_SYNC_ENABLED	BIT(4)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_TILES_ENABLED			BIT(3)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_TRANSQUANT_BYPASS_ENABLED	BIT(2)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_WEIGHTED_BIPRED			BIT(1)
+#define VE_DEC_H265_DEC_PPS_CTRL1_FLAG_WEIGHTED_PRED			BIT(0)
+
+#define VE_DEC_H265_DEC_PPS_CTRL1_LOG2_PARALLEL_MERGE_LEVEL_MINUS2(v) \
+	SHIFT_AND_MASK_BITS(v, 10, 8)
+
+#define VE_DEC_H265_SCALING_LIST_CTRL0		(VE_ENGINE_DEC_H265 + 0x18)
+
+#define VE_DEC_H265_SCALING_LIST_CTRL0_FLAG_ENABLED			BIT(31)
+
+#define VE_DEC_H265_SCALING_LIST_CTRL0_SRAM	(0 << 30)
+#define VE_DEC_H265_SCALING_LIST_CTRL0_DEFAULT	(1 << 30)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0		(VE_ENGINE_DEC_H265 + 0x20)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_COLLOCATED_FROM_L0		BIT(11)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_CABAC_INIT			BIT(10)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_MVD_L1_ZERO		BIT(9)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_SAO_CHROMA		BIT(8)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_SAO_LUMA		BIT(7)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_SLICE_TEMPORAL_MVP_ENABLE	BIT(6)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_DEPENDENT_SLICE_SEGMENT	BIT(1)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FLAG_FIRST_SLICE_SEGMENT_IN_PIC	BIT(0)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_PICTURE_TYPE(v) \
+	SHIFT_AND_MASK_BITS(v, 29, 28)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_FIVE_MINUS_MAX_NUM_MERGE_CAND(v) \
+	SHIFT_AND_MASK_BITS(v, 26, 24)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_NUM_REF_IDX_L1_ACTIVE_MINUS1(v) \
+	SHIFT_AND_MASK_BITS(v, 23, 20)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_NUM_REF_IDX_L0_ACTIVE_MINUS1(v) \
+	SHIFT_AND_MASK_BITS(v, 19, 16)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_COLLOCATED_REF_IDX(v) \
+	SHIFT_AND_MASK_BITS(v, 15, 12)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_COLOUR_PLANE_ID(v) \
+	SHIFT_AND_MASK_BITS(v, 5, 4)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO0_SLICE_TYPE(v) \
+	SHIFT_AND_MASK_BITS(v, 3, 2)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1		(VE_ENGINE_DEC_H265 + 0x24)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_DEBLOCKING_FILTER_DISABLED BIT(23)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_LOOP_FILTER_ACROSS_SLICES_ENABLED BIT(22)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_FLAG_SLICE_NOT_LOW_DELAY BIT(21)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_TC_OFFSET_DIV2(v) \
+	SHIFT_AND_MASK_BITS(v, 31, 28)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_BETA_OFFSET_DIV2(v) \
+	SHIFT_AND_MASK_BITS(v, 27, 24)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_CR_QP_OFFSET(v) \
+	SHIFT_AND_MASK_BITS(v, 20, 16)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_CB_QP_OFFSET(v) \
+	SHIFT_AND_MASK_BITS(v, 12, 8)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO1_SLICE_QP_DELTA(v) \
+	SHIFT_AND_MASK_BITS(v, 6, 0)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO2		(VE_ENGINE_DEC_H265 + 0x28)
+
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO2_NUM_ENTRY_POINT_OFFSETS(v) \
+	SHIFT_AND_MASK_BITS(v, 21, 8)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO2_CHROMA_LOG2_WEIGHT_DENOM(v) \
+	SHIFT_AND_MASK_BITS(v, 6, 4)
+#define VE_DEC_H265_DEC_SLICE_HDR_INFO2_LUMA_LOG2_WEIGHT_DENOM(v) \
+	SHIFT_AND_MASK_BITS(v, 2, 0)
+
+#define VE_DEC_H265_DEC_CTB_ADDR		(VE_ENGINE_DEC_H265 + 0x2c)
+
+#define VE_DEC_H265_DEC_CTB_ADDR_Y(y)		SHIFT_AND_MASK_BITS(y, 25, 16)
+#define VE_DEC_H265_DEC_CTB_ADDR_X(x)		SHIFT_AND_MASK_BITS(x, 9, 0)
+
+#define VE_DEC_H265_CTRL			(VE_ENGINE_DEC_H265 + 0x30)
+
+#define VE_DEC_H265_CTRL_DDR_CONSISTENCY_EN	BIT(31)
+#define VE_DEC_H265_CTRL_STCD_EN		BIT(25)
+#define VE_DEC_H265_CTRL_EPTB_DEC_BYPASS_EN	BIT(24)
+#define VE_DEC_H265_CTRL_TQ_BYPASS_EN		BIT(12)
+#define VE_DEC_H265_CTRL_VLD_BYPASS_EN		BIT(11)
+#define VE_DEC_H265_CTRL_NCRI_CACHE_DISABLE	BIT(10)
+#define VE_DEC_H265_CTRL_ROTATE_SCALE_OUT_EN	BIT(9)
+#define VE_DEC_H265_CTRL_MC_NO_WRITEBACK	BIT(8)
+#define VE_DEC_H265_CTRL_VLD_DATA_REQ_IRQ_EN	BIT(2)
+#define VE_DEC_H265_CTRL_ERROR_IRQ_EN		BIT(1)
+#define VE_DEC_H265_CTRL_FINISH_IRQ_EN		BIT(0)
+#define VE_DEC_H265_CTRL_IRQ_MASK \
+	(VE_DEC_H265_CTRL_FINISH_IRQ_EN | VE_DEC_H265_CTRL_ERROR_IRQ_EN | \
+	 VE_DEC_H265_CTRL_VLD_DATA_REQ_IRQ_EN)
+
+#define VE_DEC_H265_TRIGGER			(VE_ENGINE_DEC_H265 + 0x34)
+
+#define VE_DEC_H265_TRIGGER_TYPE_N_BITS(x)	(((x) & 0x3f) << 8)
+#define VE_DEC_H265_TRIGGER_STCD_VC1		(0x02 << 4)
+#define VE_DEC_H265_TRIGGER_STCD_AVS		(0x01 << 4)
+#define VE_DEC_H265_TRIGGER_STCD_HEVC		(0x00 << 4)
+#define VE_DEC_H265_TRIGGER_DEC_SLICE		(0x08 << 0)
+#define VE_DEC_H265_TRIGGER_INIT_SWDEC		(0x07 << 0)
+#define VE_DEC_H265_TRIGGER_BYTE_ALIGN		(0x06 << 0)
+#define VE_DEC_H265_TRIGGER_GET_VLCUE		(0x05 << 0)
+#define VE_DEC_H265_TRIGGER_GET_VLCSE		(0x04 << 0)
+#define VE_DEC_H265_TRIGGER_FLUSH_BITS		(0x03 << 0)
+#define VE_DEC_H265_TRIGGER_GET_BITS		(0x02 << 0)
+#define VE_DEC_H265_TRIGGER_SHOW_BITS		(0x01 << 0)
+
+#define VE_DEC_H265_STATUS			(VE_ENGINE_DEC_H265 + 0x38)
+
+#define VE_DEC_H265_STATUS_STCD			BIT(24)
+#define VE_DEC_H265_STATUS_STCD_BUSY		BIT(21)
+#define VE_DEC_H265_STATUS_WB_BUSY		BIT(20)
+#define VE_DEC_H265_STATUS_BS_DMA_BUSY		BIT(19)
+#define VE_DEC_H265_STATUS_IT_BUSY		BIT(18)
+#define VE_DEC_H265_STATUS_INTER_BUSY		BIT(17)
+#define VE_DEC_H265_STATUS_MORE_DATA		BIT(16)
+#define VE_DEC_H265_STATUS_DBLK_BUSY		BIT(15)
+#define VE_DEC_H265_STATUS_IREC_BUSY		BIT(14)
+#define VE_DEC_H265_STATUS_INTRA_BUSY		BIT(13)
+#define VE_DEC_H265_STATUS_MCRI_BUSY		BIT(12)
+#define VE_DEC_H265_STATUS_IQIT_BUSY		BIT(11)
+#define VE_DEC_H265_STATUS_MVP_BUSY		BIT(10)
+#define VE_DEC_H265_STATUS_IS_BUSY		BIT(9)
+#define VE_DEC_H265_STATUS_VLD_BUSY		BIT(8)
+#define VE_DEC_H265_STATUS_OVER_TIME		BIT(3)
+#define VE_DEC_H265_STATUS_VLD_DATA_REQ		BIT(2)
+#define VE_DEC_H265_STATUS_ERROR		BIT(1)
+#define VE_DEC_H265_STATUS_SUCCESS		BIT(0)
+#define VE_DEC_H265_STATUS_STCD_TYPE_MASK	GENMASK(23, 22)
+#define VE_DEC_H265_STATUS_CHECK_MASK \
+	(VE_DEC_H265_STATUS_SUCCESS | VE_DEC_H265_STATUS_ERROR | \
+	 VE_DEC_H265_STATUS_VLD_DATA_REQ)
+#define VE_DEC_H265_STATUS_CHECK_ERROR \
+	(VE_DEC_H265_STATUS_ERROR | VE_DEC_H265_STATUS_VLD_DATA_REQ)
+
+#define VE_DEC_H265_DEC_CTB_NUM			(VE_ENGINE_DEC_H265 + 0x3c)
+
+#define VE_DEC_H265_BITS_ADDR			(VE_ENGINE_DEC_H265 + 0x40)
+
+#define VE_DEC_H265_BITS_ADDR_FIRST_SLICE_DATA	BIT(30)
+#define VE_DEC_H265_BITS_ADDR_LAST_SLICE_DATA	BIT(29)
+#define VE_DEC_H265_BITS_ADDR_VALID_SLICE_DATA	BIT(28)
+#define VE_DEC_H265_BITS_ADDR_BASE(a)		(((a) >> 8) & GENMASK(27, 0))
+
+#define VE_DEC_H265_BITS_OFFSET			(VE_ENGINE_DEC_H265 + 0x44)
+#define VE_DEC_H265_BITS_LEN			(VE_ENGINE_DEC_H265 + 0x48)
+
+#define VE_DEC_H265_BITS_END_ADDR		(VE_ENGINE_DEC_H265 + 0x4c)
+
+#define VE_DEC_H265_BITS_END_ADDR_BASE(a)	((a) >> 8)
+
+#define VE_DEC_H265_SDRT_CTRL			(VE_ENGINE_DEC_H265 + 0x50)
+#define VE_DEC_H265_SDRT_LUMA_ADDR		(VE_ENGINE_DEC_H265 + 0x54)
+#define VE_DEC_H265_SDRT_CHROMA_ADDR		(VE_ENGINE_DEC_H265 + 0x58)
+
+#define VE_DEC_H265_OUTPUT_FRAME_IDX		(VE_ENGINE_DEC_H265 + 0x5c)
+
+#define VE_DEC_H265_NEIGHBOR_INFO_ADDR		(VE_ENGINE_DEC_H265 + 0x60)
+
+#define VE_DEC_H265_NEIGHBOR_INFO_ADDR_BASE(a)	((a) >> 8)
+
+#define VE_DEC_H265_ENTRY_POINT_OFFSET_ADDR	(VE_ENGINE_DEC_H265 + 0x64)
+#define VE_DEC_H265_TILE_START_CTB		(VE_ENGINE_DEC_H265 + 0x68)
+#define VE_DEC_H265_TILE_END_CTB		(VE_ENGINE_DEC_H265 + 0x6c)
+#define VE_DEC_H265_SCALING_LIST_DC_COEF0	(VE_ENGINE_DEC_H265 + 0x78)
+#define VE_DEC_H265_SCALING_LIST_DC_COEF1	(VE_ENGINE_DEC_H265 + 0x7c)
+
+#define VE_DEC_H265_LOW_ADDR			(VE_ENGINE_DEC_H265 + 0x80)
+
+#define VE_DEC_H265_LOW_ADDR_PRIMARY_CHROMA(a) \
+	SHIFT_AND_MASK_BITS(a, 31, 24)
+#define VE_DEC_H265_LOW_ADDR_SECONDARY_CHROMA(a) \
+	SHIFT_AND_MASK_BITS(a, 23, 16)
+#define VE_DEC_H265_LOW_ADDR_ENTRY_POINTS_BUF(a) \
+	SHIFT_AND_MASK_BITS(a, 7, 0)
+
+#define VE_DEC_H265_BITS_READ			(VE_ENGINE_DEC_H265 + 0xdc)
+
+#define VE_DEC_H265_SRAM_OFFSET			(VE_ENGINE_DEC_H265 + 0xe0)
+
+#define VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_LUMA_L0	0x00
+#define VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_CHROMA_L0	0x20
+#define VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_LUMA_L1	0x60
+#define VE_DEC_H265_SRAM_OFFSET_PRED_WEIGHT_CHROMA_L1	0x80
+#define VE_DEC_H265_SRAM_OFFSET_FRAME_INFO		0x400
+#define VE_DEC_H265_SRAM_OFFSET_FRAME_INFO_UNIT		0x20
+#define VE_DEC_H265_SRAM_OFFSET_SCALING_LISTS		0x800
+#define VE_DEC_H265_SRAM_OFFSET_REF_PIC_LIST0		0xc00
+#define VE_DEC_H265_SRAM_OFFSET_REF_PIC_LIST1		0xc10
+
+#define VE_DEC_H265_SRAM_DATA			(VE_ENGINE_DEC_H265 + 0xe4)
+
+#define VE_DEC_H265_SRAM_DATA_ADDR_BASE(a)	((a) >> 8)
+#define VE_DEC_H265_SRAM_REF_PIC_LIST_LT_REF	BIT(7)
+
+#define VE_H264_SPS			0x200
+#define VE_H264_SPS_MBS_ONLY			BIT(18)
+#define VE_H264_SPS_MB_ADAPTIVE_FRAME_FIELD	BIT(17)
+#define VE_H264_SPS_DIRECT_8X8_INFERENCE	BIT(16)
+
+#define VE_H264_PPS			0x204
+#define VE_H264_PPS_ENTROPY_CODING_MODE		BIT(15)
+#define VE_H264_PPS_WEIGHTED_PRED		BIT(4)
+#define VE_H264_PPS_CONSTRAINED_INTRA_PRED	BIT(1)
+#define VE_H264_PPS_TRANSFORM_8X8_MODE		BIT(0)
+
+#define VE_H264_SHS			0x208
+#define VE_H264_SHS_FIRST_SLICE_IN_PIC		BIT(5)
+#define VE_H264_SHS_FIELD_PIC			BIT(4)
+#define VE_H264_SHS_BOTTOM_FIELD		BIT(3)
+#define VE_H264_SHS_DIRECT_SPATIAL_MV_PRED	BIT(2)
+
+#define VE_H264_SHS2			0x20c
+#define VE_H264_SHS2_NUM_REF_IDX_ACTIVE_OVRD	BIT(12)
+
+#define VE_H264_SHS_WP			0x210
+
+#define VE_H264_SHS_QP			0x21c
+#define VE_H264_SHS_QP_SCALING_MATRIX_DEFAULT	BIT(24)
+
+#define VE_H264_CTRL			0x220
+#define VE_H264_CTRL_VP8			BIT(29)
+#define VE_H264_CTRL_VLD_DATA_REQ_INT		BIT(2)
+#define VE_H264_CTRL_DECODE_ERR_INT		BIT(1)
+#define VE_H264_CTRL_SLICE_DECODE_INT		BIT(0)
+
+#define VE_H264_CTRL_INT_MASK		(VE_H264_CTRL_VLD_DATA_REQ_INT | \
+					 VE_H264_CTRL_DECODE_ERR_INT | \
+					 VE_H264_CTRL_SLICE_DECODE_INT)
+
+#define VE_H264_TRIGGER_TYPE		0x224
+#define VE_H264_TRIGGER_TYPE_PROBABILITY(x)	SHIFT_AND_MASK_BITS(x, 31, 24)
+#define VE_H264_TRIGGER_TYPE_BIN_LENS(x)	SHIFT_AND_MASK_BITS((x) - 1, 18, 16)
+#define VE_H264_TRIGGER_TYPE_N_BITS(x)		(((x) & 0x3f) << 8)
+#define VE_H264_TRIGGER_TYPE_VP8_GET_BITS	(15 << 0)
+#define VE_H264_TRIGGER_TYPE_VP8_UPDATE_COEF	(14 << 0)
+#define VE_H264_TRIGGER_TYPE_VP8_SLICE_DECODE	(10 << 0)
+#define VE_H264_TRIGGER_TYPE_AVC_SLICE_DECODE	(8 << 0)
+#define VE_H264_TRIGGER_TYPE_INIT_SWDEC		(7 << 0)
+#define VE_H264_TRIGGER_TYPE_FLUSH_BITS		(3 << 0)
+
+#define VE_H264_STATUS			0x228
+#define VE_H264_STATUS_VLD_DATA_REQ_INT		VE_H264_CTRL_VLD_DATA_REQ_INT
+#define VE_H264_STATUS_DECODE_ERR_INT		VE_H264_CTRL_DECODE_ERR_INT
+#define VE_H264_STATUS_SLICE_DECODE_INT		VE_H264_CTRL_SLICE_DECODE_INT
+#define VE_H264_STATUS_VLD_BUSY			BIT(8)
+#define VE_H264_STATUS_VP8_UPPROB_BUSY		BIT(17)
+
+#define VE_H264_STATUS_INT_MASK			VE_H264_CTRL_INT_MASK
+
+#define VE_H264_CUR_MB_NUM		0x22c
+
+#define VE_H264_VLD_ADDR		0x230
+#define VE_H264_VLD_ADDR_FIRST			BIT(30)
+#define VE_H264_VLD_ADDR_LAST			BIT(29)
+#define VE_H264_VLD_ADDR_VALID			BIT(28)
+#define VE_H264_VLD_ADDR_VAL(x)			(((x) & 0x0ffffff0) | ((x) >> 28))
+
+#define VE_H264_VLD_OFFSET		0x234
+#define VE_H264_VLD_LEN			0x238
+#define VE_H264_VLD_END			0x23c
+#define VE_H264_SDROT_CTRL		0x240
+#define VE_H264_OUTPUT_FRAME_IDX	0x24c
+#define VE_H264_EXTRA_BUFFER1		0x250
+#define VE_H264_EXTRA_BUFFER2		0x254
+#define VE_H264_MB_ADDR			0x260
+#define VE_H264_ERROR_CASE		0x2b8
+#define VE_H264_BASIC_BITS		0x2dc
+#define VE_AVC_SRAM_PORT_OFFSET		0x2e0
+#define VE_AVC_SRAM_PORT_DATA		0x2e4
+
+#define VE_VP8_PPS			0x214
+#define VE_VP8_PPS_PIC_TYPE_P_FRAME		BIT(31)
+#define VE_VP8_PPS_LAST_SHARPNESS_LEVEL(v)	SHIFT_AND_MASK_BITS(v, 30, 28)
+#define VE_VP8_PPS_LAST_PIC_TYPE_P_FRAME	BIT(27)
+#define VE_VP8_PPS_ALTREF_SIGN_BIAS		BIT(26)
+#define VE_VP8_PPS_GOLDEN_SIGN_BIAS		BIT(25)
+#define VE_VP8_PPS_RELOAD_ENTROPY_PROBS		BIT(24)
+#define VE_VP8_PPS_REFRESH_ENTROPY_PROBS	BIT(23)
+#define VE_VP8_PPS_MB_NO_COEFF_SKIP		BIT(22)
+#define VE_VP8_PPS_TOKEN_PARTITION(v)		SHIFT_AND_MASK_BITS(v, 21, 20)
+#define VE_VP8_PPS_MODE_REF_LF_DELTA_UPDATE	BIT(19)
+#define VE_VP8_PPS_MODE_REF_LF_DELTA_ENABLE	BIT(18)
+#define VE_VP8_PPS_LOOP_FILTER_LEVEL(v)		SHIFT_AND_MASK_BITS(v, 17, 12)
+#define VE_VP8_PPS_LOOP_FILTER_SIMPLE		BIT(11)
+#define VE_VP8_PPS_SHARPNESS_LEVEL(v)		SHIFT_AND_MASK_BITS(v, 10, 8)
+#define VE_VP8_PPS_LAST_LOOP_FILTER_SIMPLE	BIT(7)
+#define VE_VP8_PPS_SEGMENTATION_ENABLE		BIT(6)
+#define VE_VP8_PPS_MB_SEGMENT_ABS_DELTA		BIT(5)
+#define VE_VP8_PPS_UPDATE_MB_SEGMENTATION_MAP	BIT(4)
+#define VE_VP8_PPS_FULL_PIXEL			BIT(3)
+#define VE_VP8_PPS_BILINEAR_MC_FILTER		BIT(2)
+#define VE_VP8_PPS_FILTER_TYPE_SIMPLE		BIT(1)
+#define VE_VP8_PPS_LPF_DISABLE			BIT(0)
+
+#define VE_VP8_QP_INDEX_DELTA		0x218
+#define VE_VP8_QP_INDEX_DELTA_UVAC(v)		SHIFT_AND_MASK_BITS(v, 31, 27)
+#define VE_VP8_QP_INDEX_DELTA_UVDC(v)		SHIFT_AND_MASK_BITS(v, 26, 22)
+#define VE_VP8_QP_INDEX_DELTA_Y2AC(v)		SHIFT_AND_MASK_BITS(v, 21, 17)
+#define VE_VP8_QP_INDEX_DELTA_Y2DC(v)		SHIFT_AND_MASK_BITS(v, 16, 12)
+#define VE_VP8_QP_INDEX_DELTA_Y1DC(v)		SHIFT_AND_MASK_BITS(v, 11, 7)
+#define VE_VP8_QP_INDEX_DELTA_BASE_QINDEX(v)	SHIFT_AND_MASK_BITS(v, 6, 0)
+
+#define VE_VP8_PART_SIZE_OFFSET		0x21c
+#define VE_VP8_ENTROPY_PROBS_ADDR	0x250
+#define VE_VP8_FIRST_DATA_PART_LEN	0x254
+
+#define VE_VP8_FSIZE			0x258
+#define VE_VP8_FSIZE_WIDTH(w) \
+	SHIFT_AND_MASK_BITS(DIV_ROUND_UP(w, 16), 15, 8)
+#define VE_VP8_FSIZE_HEIGHT(h) \
+	SHIFT_AND_MASK_BITS(DIV_ROUND_UP(h, 16), 7, 0)
+
+#define VE_VP8_PICSIZE			0x25c
+#define VE_VP8_PICSIZE_WIDTH(w)			SHIFT_AND_MASK_BITS(w, 27, 16)
+#define VE_VP8_PICSIZE_HEIGHT(h)		SHIFT_AND_MASK_BITS(h, 11, 0)
+
+#define VE_VP8_REC_LUMA			0x2ac
+#define VE_VP8_FWD_LUMA			0x2b0
+#define VE_VP8_BWD_LUMA			0x2b4
+#define VE_VP8_REC_CHROMA		0x2d0
+#define VE_VP8_FWD_CHROMA		0x2d4
+#define VE_VP8_BWD_CHROMA		0x2d8
+#define VE_VP8_ALT_LUMA			0x2e8
+#define VE_VP8_ALT_CHROMA		0x2ec
+
+#define VE_VP8_SEGMENT_FEAT_MB_LV0	0x2f0
+#define VE_VP8_SEGMENT_FEAT_MB_LV1	0x2f4
+
+#define VE_VP8_SEGMENT3(v)			SHIFT_AND_MASK_BITS(v, 31, 24)
+#define VE_VP8_SEGMENT2(v)			SHIFT_AND_MASK_BITS(v, 23, 16)
+#define VE_VP8_SEGMENT1(v)			SHIFT_AND_MASK_BITS(v, 15, 8)
+#define VE_VP8_SEGMENT0(v)			SHIFT_AND_MASK_BITS(v, 7, 0)
+
+#define VE_VP8_REF_LF_DELTA		0x2f8
+#define VE_VP8_MODE_LF_DELTA		0x2fc
+
+#define VE_VP8_LF_DELTA3(v)			SHIFT_AND_MASK_BITS(v, 30, 24)
+#define VE_VP8_LF_DELTA2(v)			SHIFT_AND_MASK_BITS(v, 22, 16)
+#define VE_VP8_LF_DELTA1(v)			SHIFT_AND_MASK_BITS(v, 14, 8)
+#define VE_VP8_LF_DELTA0(v)			SHIFT_AND_MASK_BITS(v, 6, 0)
+
+#define VE_ISP_INPUT_SIZE		0xa00
+#define VE_ISP_INPUT_STRIDE		0xa04
+#define VE_ISP_CTRL			0xa08
+#define VE_ISP_INPUT_LUMA		0xa78
+#define VE_ISP_INPUT_CHROMA		0xa7c
+
+#define VE_AVC_PARAM			0xb04
+#define VE_AVC_QP			0xb08
+#define VE_AVC_MOTION_EST		0xb10
+#define VE_AVC_CTRL			0xb14
+#define VE_AVC_TRIGGER			0xb18
+#define VE_AVC_STATUS			0xb1c
+#define VE_AVC_BASIC_BITS		0xb20
+#define VE_AVC_UNK_BUF			0xb60
+#define VE_AVC_VLE_ADDR			0xb80
+#define VE_AVC_VLE_END			0xb84
+#define VE_AVC_VLE_OFFSET		0xb88
+#define VE_AVC_VLE_MAX			0xb8c
+#define VE_AVC_VLE_LENGTH		0xb90
+#define VE_AVC_REF_LUMA			0xba0
+#define VE_AVC_REF_CHROMA		0xba4
+#define VE_AVC_REC_LUMA			0xbb0
+#define VE_AVC_REC_CHROMA		0xbb4
+#define VE_AVC_REF_SLUMA		0xbb8
+#define VE_AVC_REC_SLUMA		0xbbc
+#define VE_AVC_MB_INFO			0xbc0
+
+#endif
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_video.c b/drivers/staging/media/sunxi/cedrus/cedrus_video.c
new file mode 100644
index 000000000..66714609b
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_video.c
@@ -0,0 +1,596 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#include <linux/pm_runtime.h>
+
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "cedrus.h"
+#include "cedrus_video.h"
+#include "cedrus_dec.h"
+#include "cedrus_hw.h"
+
+#define CEDRUS_DECODE_SRC	BIT(0)
+#define CEDRUS_DECODE_DST	BIT(1)
+
+#define CEDRUS_MIN_WIDTH	16U
+#define CEDRUS_MIN_HEIGHT	16U
+#define CEDRUS_MAX_WIDTH	4096U
+#define CEDRUS_MAX_HEIGHT	2304U
+
+static struct cedrus_format cedrus_formats[] = {
+	{
+		.pixelformat	= V4L2_PIX_FMT_MPEG2_SLICE,
+		.directions	= CEDRUS_DECODE_SRC,
+		.capabilities	= CEDRUS_CAPABILITY_MPEG2_DEC,
+	},
+	{
+		.pixelformat	= V4L2_PIX_FMT_H264_SLICE,
+		.directions	= CEDRUS_DECODE_SRC,
+		.capabilities	= CEDRUS_CAPABILITY_H264_DEC,
+	},
+	{
+		.pixelformat	= V4L2_PIX_FMT_HEVC_SLICE,
+		.directions	= CEDRUS_DECODE_SRC,
+		.capabilities	= CEDRUS_CAPABILITY_H265_DEC,
+	},
+	{
+		.pixelformat	= V4L2_PIX_FMT_VP8_FRAME,
+		.directions	= CEDRUS_DECODE_SRC,
+		.capabilities	= CEDRUS_CAPABILITY_VP8_DEC,
+	},
+	{
+		.pixelformat	= V4L2_PIX_FMT_NV12_32L32,
+		.directions	= CEDRUS_DECODE_DST,
+	},
+	{
+		.pixelformat	= V4L2_PIX_FMT_NV12,
+		.directions	= CEDRUS_DECODE_DST,
+		.capabilities	= CEDRUS_CAPABILITY_UNTILED,
+	},
+};
+
+#define CEDRUS_FORMATS_COUNT	ARRAY_SIZE(cedrus_formats)
+
+static inline struct cedrus_ctx *cedrus_file2ctx(struct file *file)
+{
+	return container_of(file->private_data, struct cedrus_ctx, fh);
+}
+
+static struct cedrus_format *cedrus_find_format(u32 pixelformat, u32 directions,
+						unsigned int capabilities)
+{
+	struct cedrus_format *first_valid_fmt = NULL;
+	struct cedrus_format *fmt;
+	unsigned int i;
+
+	for (i = 0; i < CEDRUS_FORMATS_COUNT; i++) {
+		fmt = &cedrus_formats[i];
+
+		if ((fmt->capabilities & capabilities) != fmt->capabilities ||
+		    !(fmt->directions & directions))
+			continue;
+
+		if (fmt->pixelformat == pixelformat)
+			break;
+
+		if (!first_valid_fmt)
+			first_valid_fmt = fmt;
+	}
+
+	if (i == CEDRUS_FORMATS_COUNT)
+		return first_valid_fmt;
+
+	return &cedrus_formats[i];
+}
+
+void cedrus_prepare_format(struct v4l2_pix_format *pix_fmt)
+{
+	unsigned int width = pix_fmt->width;
+	unsigned int height = pix_fmt->height;
+	unsigned int sizeimage = pix_fmt->sizeimage;
+	unsigned int bytesperline = pix_fmt->bytesperline;
+
+	pix_fmt->field = V4L2_FIELD_NONE;
+
+	/* Limit to hardware min/max. */
+	width = clamp(width, CEDRUS_MIN_WIDTH, CEDRUS_MAX_WIDTH);
+	height = clamp(height, CEDRUS_MIN_HEIGHT, CEDRUS_MAX_HEIGHT);
+
+	switch (pix_fmt->pixelformat) {
+	case V4L2_PIX_FMT_MPEG2_SLICE:
+	case V4L2_PIX_FMT_H264_SLICE:
+	case V4L2_PIX_FMT_HEVC_SLICE:
+	case V4L2_PIX_FMT_VP8_FRAME:
+		/* Zero bytes per line for encoded source. */
+		bytesperline = 0;
+		/* Choose some minimum size since this can't be 0 */
+		sizeimage = max_t(u32, SZ_1K, sizeimage);
+		break;
+
+	case V4L2_PIX_FMT_NV12_32L32:
+		/* 32-aligned stride. */
+		bytesperline = ALIGN(width, 32);
+
+		/* 32-aligned height. */
+		height = ALIGN(height, 32);
+
+		/* Luma plane size. */
+		sizeimage = bytesperline * height;
+
+		/* Chroma plane size. */
+		sizeimage += bytesperline * ALIGN(height, 64) / 2;
+
+		break;
+
+	case V4L2_PIX_FMT_NV12:
+		/* 16-aligned stride. */
+		bytesperline = ALIGN(width, 16);
+
+		/* 16-aligned height. */
+		height = ALIGN(height, 16);
+
+		/* Luma plane size. */
+		sizeimage = bytesperline * height;
+
+		/* Chroma plane size. */
+		sizeimage += bytesperline * height / 2;
+
+		break;
+	}
+
+	pix_fmt->width = width;
+	pix_fmt->height = height;
+
+	pix_fmt->bytesperline = bytesperline;
+	pix_fmt->sizeimage = sizeimage;
+}
+
+static int cedrus_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, CEDRUS_NAME, sizeof(cap->driver));
+	strscpy(cap->card, CEDRUS_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", CEDRUS_NAME);
+
+	return 0;
+}
+
+static int cedrus_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
+			   u32 direction)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+	struct cedrus_dev *dev = ctx->dev;
+	unsigned int capabilities = dev->capabilities;
+	struct cedrus_format *fmt;
+	unsigned int i, index;
+
+	/* Index among formats that match the requested direction. */
+	index = 0;
+
+	for (i = 0; i < CEDRUS_FORMATS_COUNT; i++) {
+		fmt = &cedrus_formats[i];
+
+		if (fmt->capabilities && (fmt->capabilities & capabilities) !=
+		    fmt->capabilities)
+			continue;
+
+		if (!(cedrus_formats[i].directions & direction))
+			continue;
+
+		if (index == f->index)
+			break;
+
+		index++;
+	}
+
+	/* Matched format. */
+	if (i < CEDRUS_FORMATS_COUNT) {
+		f->pixelformat = cedrus_formats[i].pixelformat;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int cedrus_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return cedrus_enum_fmt(file, f, CEDRUS_DECODE_DST);
+}
+
+static int cedrus_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return cedrus_enum_fmt(file, f, CEDRUS_DECODE_SRC);
+}
+
+static int cedrus_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+
+	f->fmt.pix = ctx->dst_fmt;
+	return 0;
+}
+
+static int cedrus_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+
+	f->fmt.pix = ctx->src_fmt;
+	return 0;
+}
+
+static int cedrus_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+	struct cedrus_dev *dev = ctx->dev;
+	struct v4l2_pix_format *pix_fmt = &f->fmt.pix;
+	struct cedrus_format *fmt =
+		cedrus_find_format(pix_fmt->pixelformat, CEDRUS_DECODE_DST,
+				   dev->capabilities);
+
+	if (!fmt)
+		return -EINVAL;
+
+	pix_fmt->pixelformat = fmt->pixelformat;
+	pix_fmt->width = ctx->src_fmt.width;
+	pix_fmt->height = ctx->src_fmt.height;
+	cedrus_prepare_format(pix_fmt);
+
+	return 0;
+}
+
+static int cedrus_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+	struct cedrus_dev *dev = ctx->dev;
+	struct v4l2_pix_format *pix_fmt = &f->fmt.pix;
+	struct cedrus_format *fmt =
+		cedrus_find_format(pix_fmt->pixelformat, CEDRUS_DECODE_SRC,
+				   dev->capabilities);
+
+	if (!fmt)
+		return -EINVAL;
+
+	pix_fmt->pixelformat = fmt->pixelformat;
+	cedrus_prepare_format(pix_fmt);
+
+	return 0;
+}
+
+static int cedrus_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+	struct vb2_queue *vq;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_busy(vq))
+		return -EBUSY;
+
+	ret = cedrus_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	ctx->dst_fmt = f->fmt.pix;
+
+	return 0;
+}
+
+static int cedrus_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct cedrus_ctx *ctx = cedrus_file2ctx(file);
+	struct vb2_queue *vq;
+	struct vb2_queue *peer_vq;
+	int ret;
+
+	ret = cedrus_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	/*
+	 * In order to support dynamic resolution change,
+	 * the decoder admits a resolution change, as long
+	 * as the pixelformat remains. Can't be done if streaming.
+	 */
+	if (vb2_is_streaming(vq) || (vb2_is_busy(vq) &&
+	    f->fmt.pix.pixelformat != ctx->src_fmt.pixelformat))
+		return -EBUSY;
+	/*
+	 * Since format change on the OUTPUT queue will reset
+	 * the CAPTURE queue, we can't allow doing so
+	 * when the CAPTURE queue has buffers allocated.
+	 */
+	peer_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+				  V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	if (vb2_is_busy(peer_vq))
+		return -EBUSY;
+
+	ret = cedrus_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	ctx->src_fmt = f->fmt.pix;
+
+	switch (ctx->src_fmt.pixelformat) {
+	case V4L2_PIX_FMT_H264_SLICE:
+	case V4L2_PIX_FMT_HEVC_SLICE:
+		vq->subsystem_flags |=
+			VB2_V4L2_FL_SUPPORTS_M2M_HOLD_CAPTURE_BUF;
+		break;
+	default:
+		vq->subsystem_flags &=
+			~VB2_V4L2_FL_SUPPORTS_M2M_HOLD_CAPTURE_BUF;
+		break;
+	}
+
+	/* Propagate format information to capture. */
+	ctx->dst_fmt.colorspace = f->fmt.pix.colorspace;
+	ctx->dst_fmt.xfer_func = f->fmt.pix.xfer_func;
+	ctx->dst_fmt.ycbcr_enc = f->fmt.pix.ycbcr_enc;
+	ctx->dst_fmt.quantization = f->fmt.pix.quantization;
+	ctx->dst_fmt.width = ctx->src_fmt.width;
+	ctx->dst_fmt.height = ctx->src_fmt.height;
+	cedrus_prepare_format(&ctx->dst_fmt);
+
+	return 0;
+}
+
+const struct v4l2_ioctl_ops cedrus_ioctl_ops = {
+	.vidioc_querycap		= cedrus_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= cedrus_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= cedrus_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= cedrus_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= cedrus_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out	= cedrus_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out		= cedrus_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= cedrus_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= cedrus_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_try_decoder_cmd		= v4l2_m2m_ioctl_stateless_try_decoder_cmd,
+	.vidioc_decoder_cmd		= v4l2_m2m_ioctl_stateless_decoder_cmd,
+
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static int cedrus_queue_setup(struct vb2_queue *vq, unsigned int *nbufs,
+			      unsigned int *nplanes, unsigned int sizes[],
+			      struct device *alloc_devs[])
+{
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix_fmt;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type))
+		pix_fmt = &ctx->src_fmt;
+	else
+		pix_fmt = &ctx->dst_fmt;
+
+	if (*nplanes) {
+		if (sizes[0] < pix_fmt->sizeimage)
+			return -EINVAL;
+	} else {
+		sizes[0] = pix_fmt->sizeimage;
+		*nplanes = 1;
+	}
+
+	return 0;
+}
+
+static void cedrus_queue_cleanup(struct vb2_queue *vq, u32 state)
+{
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vq);
+	struct vb2_v4l2_buffer *vbuf;
+
+	for (;;) {
+		if (V4L2_TYPE_IS_OUTPUT(vq->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, state);
+	}
+}
+
+static int cedrus_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 cedrus_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix_fmt;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type))
+		pix_fmt = &ctx->src_fmt;
+	else
+		pix_fmt = &ctx->dst_fmt;
+
+	if (vb2_plane_size(vb, 0) < pix_fmt->sizeimage)
+		return -EINVAL;
+
+	/*
+	 * Buffer's bytesused must be written by driver for CAPTURE buffers.
+	 * (for OUTPUT buffers, if userspace passes 0 bytesused, v4l2-core sets
+	 * it to buffer length).
+	 */
+	if (V4L2_TYPE_IS_CAPTURE(vq->type))
+		vb2_set_plane_payload(vb, 0, pix_fmt->sizeimage);
+
+	return 0;
+}
+
+static int cedrus_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vq);
+	struct cedrus_dev *dev = ctx->dev;
+	int ret = 0;
+
+	switch (ctx->src_fmt.pixelformat) {
+	case V4L2_PIX_FMT_MPEG2_SLICE:
+		ctx->current_codec = CEDRUS_CODEC_MPEG2;
+		break;
+
+	case V4L2_PIX_FMT_H264_SLICE:
+		ctx->current_codec = CEDRUS_CODEC_H264;
+		break;
+
+	case V4L2_PIX_FMT_HEVC_SLICE:
+		ctx->current_codec = CEDRUS_CODEC_H265;
+		break;
+
+	case V4L2_PIX_FMT_VP8_FRAME:
+		ctx->current_codec = CEDRUS_CODEC_VP8;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
+		ret = pm_runtime_resume_and_get(dev->dev);
+		if (ret < 0)
+			goto err_cleanup;
+
+		if (dev->dec_ops[ctx->current_codec]->start) {
+			ret = dev->dec_ops[ctx->current_codec]->start(ctx);
+			if (ret)
+				goto err_pm;
+		}
+	}
+
+	return 0;
+
+err_pm:
+	pm_runtime_put(dev->dev);
+err_cleanup:
+	cedrus_queue_cleanup(vq, VB2_BUF_STATE_QUEUED);
+
+	return ret;
+}
+
+static void cedrus_stop_streaming(struct vb2_queue *vq)
+{
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vq);
+	struct cedrus_dev *dev = ctx->dev;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
+		if (dev->dec_ops[ctx->current_codec]->stop)
+			dev->dec_ops[ctx->current_codec]->stop(ctx);
+
+		pm_runtime_put(dev->dev);
+	}
+
+	cedrus_queue_cleanup(vq, VB2_BUF_STATE_ERROR);
+}
+
+static void cedrus_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static void cedrus_buf_request_complete(struct vb2_buffer *vb)
+{
+	struct cedrus_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
+}
+
+static struct vb2_ops cedrus_qops = {
+	.queue_setup		= cedrus_queue_setup,
+	.buf_prepare		= cedrus_buf_prepare,
+	.buf_queue		= cedrus_buf_queue,
+	.buf_out_validate	= cedrus_buf_out_validate,
+	.buf_request_complete	= cedrus_buf_request_complete,
+	.start_streaming	= cedrus_start_streaming,
+	.stop_streaming		= cedrus_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+int cedrus_queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct cedrus_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct cedrus_buffer);
+	src_vq->ops = &cedrus_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->dev->dev_mutex;
+	src_vq->dev = ctx->dev->dev;
+	src_vq->supports_requests = true;
+	src_vq->requires_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_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct cedrus_buffer);
+	dst_vq->ops = &cedrus_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->dev->dev_mutex;
+	dst_vq->dev = ctx->dev->dev;
+
+	return vb2_queue_init(dst_vq);
+}
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_video.h b/drivers/staging/media/sunxi/cedrus/cedrus_video.h
new file mode 100644
index 000000000..05050c0a0
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_video.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
+ * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ * Copyright (C) 2018 Bootlin
+ *
+ * Based on the vim2m driver, that is:
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ */
+
+#ifndef _CEDRUS_VIDEO_H_
+#define _CEDRUS_VIDEO_H_
+
+struct cedrus_format {
+	u32		pixelformat;
+	u32		directions;
+	unsigned int	capabilities;
+};
+
+extern const struct v4l2_ioctl_ops cedrus_ioctl_ops;
+
+int cedrus_queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq);
+void cedrus_prepare_format(struct v4l2_pix_format *pix_fmt);
+
+#endif
diff --git a/drivers/staging/media/sunxi/cedrus/cedrus_vp8.c b/drivers/staging/media/sunxi/cedrus/cedrus_vp8.c
new file mode 100644
index 000000000..f7714baae
--- /dev/null
+++ b/drivers/staging/media/sunxi/cedrus/cedrus_vp8.c
@@ -0,0 +1,882 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Cedrus VPU driver
+ *
+ * Copyright (c) 2019 Jernej Skrabec <jernej.skrabec@siol.net>
+ */
+
+/*
+ * VP8 in Cedrus shares same engine as H264.
+ *
+ * Note that it seems necessary to call bitstream parsing functions,
+ * to parse frame header, otherwise decoded image is garbage. This is
+ * contrary to what is driver supposed to do. However, values are not
+ * really used, so this might be acceptable. It's possible that bitstream
+ * parsing functions set some internal VPU state, which is later necessary
+ * for proper decoding. Biggest suspect is "VP8 probs update" trigger.
+ */
+
+#include <linux/delay.h>
+#include <linux/types.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cedrus.h"
+#include "cedrus_hw.h"
+#include "cedrus_regs.h"
+
+#define CEDRUS_ENTROPY_PROBS_SIZE 0x2400
+#define VP8_PROB_HALF 128
+#define QUANT_DELTA_COUNT 5
+
+/*
+ * This table comes from the concatenation of k_coeff_entropy_update_probs,
+ * kf_ymode_prob, default_mv_context, etc. It is provided in this form in
+ * order to avoid computing it every time the driver is initialised, and is
+ * suitable for direct consumption by the hardware.
+ */
+static const u8 prob_table_init[] = {
+	/* k_coeff_entropy_update_probs */
+	/* block 0 */
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xB0, 0xF6, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xDF, 0xF1, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF9, 0xFD, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xF4, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEA, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xF6, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEF, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xF8, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFD, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFA, 0xFF, 0xFE, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* block 1 */
+	0xD9, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xE1, 0xFC, 0xF1, 0xFD, 0xFF, 0xFF, 0xFE, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEA, 0xFA, 0xF1, 0xFA, 0xFD, 0xFF, 0xFD, 0xFE,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xDF, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEE, 0xFD, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xF8, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF9, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF7, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFA, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* block 2 */
+	0xBA, 0xFB, 0xFA, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEA, 0xFB, 0xF4, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0xFB, 0xF3, 0xFD, 0xFE, 0xFF, 0xFE, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xEC, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0xFD, 0xFD, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* block 3 */
+	0xF8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFA, 0xFE, 0xFC, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF8, 0xFE, 0xF9, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFD, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF6, 0xFD, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFC, 0xFE, 0xFB, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFE, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF8, 0xFE, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFD, 0xFF, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFB, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF5, 0xFB, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFD, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFB, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFC, 0xFD, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xF9, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFA, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* kf_y_mode_probs */
+	0x91, 0x9C, 0xA3, 0x80, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* split_mv_probs */
+	0x6E, 0x6F, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* bmode_prob */
+	0x78, 0x5A, 0x4F, 0x85, 0x57, 0x55, 0x50, 0x6F,
+	0x97, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* sub_mv_ref_prob */
+	0x93, 0x88, 0x12, 0x00,
+	0x6A, 0x91, 0x01, 0x00,
+	0xB3, 0x79, 0x01, 0x00,
+	0xDF, 0x01, 0x22, 0x00,
+	0xD0, 0x01, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* mv_counts_to_probs */
+	0x07, 0x01, 0x01, 0x8F,
+	0x0E, 0x12, 0x0E, 0x6B,
+	0x87, 0x40, 0x39, 0x44,
+	0x3C, 0x38, 0x80, 0x41,
+	0x9F, 0x86, 0x80, 0x22,
+	0xEA, 0xBC, 0x80, 0x1C,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* kf_y_mode_tree */
+	0x84, 0x02, 0x04, 0x06, 0x80, 0x81, 0x82, 0x83,
+
+	/* y_mode_tree */
+	0x80, 0x02, 0x04, 0x06, 0x81, 0x82, 0x83, 0x84,
+
+	/* uv_mode_tree */
+	0x80, 0x02, 0x81, 0x04, 0x82, 0x83, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00,
+
+	/* small_mv_tree */
+	0x02, 0x08, 0x04, 0x06, 0x80, 0x81, 0x82, 0x83,
+	0x0A, 0x0C, 0x84, 0x85, 0x86, 0x87, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* small_mv_tree again */
+	0x02, 0x08, 0x04, 0x06, 0x80, 0x81, 0x82, 0x83,
+	0x0A, 0x0C, 0x84, 0x85, 0x86, 0x87, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* split_mv_tree */
+	0x83, 0x02, 0x82, 0x04, 0x80, 0x81, 0x00, 0x00,
+
+	/* b_mode_tree */
+	0x80, 0x02, 0x81, 0x04, 0x82, 0x06, 0x08, 0x0C,
+	0x83, 0x0A, 0x85, 0x86, 0x84, 0x0E, 0x87, 0x10,
+	0x88, 0x89, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* submv_ref_tree */
+	0x8A, 0x02, 0x8B, 0x04, 0x8C, 0x8D, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* mv_ref_tree */
+	0x87, 0x02, 0x85, 0x04, 0x86, 0x06, 0x88, 0x89,
+};
+
+/*
+ * This table is a copy of k_mv_entropy_update_probs from the VP8
+ * specification.
+ *
+ * FIXME: If any other driver uses it, we can consider moving
+ * this table so it can be shared.
+ */
+static const u8 k_mv_entropy_update_probs[2][V4L2_VP8_MV_PROB_CNT] = {
+	{ 237, 246, 253, 253, 254, 254, 254, 254, 254,
+	  254, 254, 254, 254, 254, 250, 250, 252, 254, 254 },
+	{ 231, 243, 245, 253, 254, 254, 254, 254, 254,
+	  254, 254, 254, 254, 254, 251, 251, 254, 254, 254 }
+};
+
+static uint8_t read_bits(struct cedrus_dev *dev, unsigned int bits_count,
+			 unsigned int probability)
+{
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+		     VE_H264_TRIGGER_TYPE_VP8_GET_BITS |
+		     VE_H264_TRIGGER_TYPE_BIN_LENS(bits_count) |
+		     VE_H264_TRIGGER_TYPE_PROBABILITY(probability));
+
+	cedrus_wait_for(dev, VE_H264_STATUS, VE_H264_STATUS_VLD_BUSY);
+
+	return cedrus_read(dev, VE_H264_BASIC_BITS);
+}
+
+static void get_delta_q(struct cedrus_dev *dev)
+{
+	if (read_bits(dev, 1, VP8_PROB_HALF)) {
+		read_bits(dev, 4, VP8_PROB_HALF);
+		read_bits(dev, 1, VP8_PROB_HALF);
+	}
+}
+
+static void process_segmentation_info(struct cedrus_dev *dev)
+{
+	int update, i;
+
+	update = read_bits(dev, 1, VP8_PROB_HALF);
+
+	if (read_bits(dev, 1, VP8_PROB_HALF)) {
+		read_bits(dev, 1, VP8_PROB_HALF);
+
+		for (i = 0; i < 4; i++)
+			if (read_bits(dev, 1, VP8_PROB_HALF)) {
+				read_bits(dev, 7, VP8_PROB_HALF);
+				read_bits(dev, 1, VP8_PROB_HALF);
+			}
+
+		for (i = 0; i < 4; i++)
+			if (read_bits(dev, 1, VP8_PROB_HALF)) {
+				read_bits(dev, 6, VP8_PROB_HALF);
+				read_bits(dev, 1, VP8_PROB_HALF);
+			}
+	}
+
+	if (update)
+		for (i = 0; i < 3; i++)
+			if (read_bits(dev, 1, VP8_PROB_HALF))
+				read_bits(dev, 8, VP8_PROB_HALF);
+}
+
+static void process_ref_lf_delta_info(struct cedrus_dev *dev)
+{
+	if (read_bits(dev, 1, VP8_PROB_HALF)) {
+		int i;
+
+		for (i = 0; i < 4; i++)
+			if (read_bits(dev, 1, VP8_PROB_HALF)) {
+				read_bits(dev, 6, VP8_PROB_HALF);
+				read_bits(dev, 1, VP8_PROB_HALF);
+			}
+
+		for (i = 0; i < 4; i++)
+			if (read_bits(dev, 1, VP8_PROB_HALF)) {
+				read_bits(dev, 6, VP8_PROB_HALF);
+				read_bits(dev, 1, VP8_PROB_HALF);
+			}
+	}
+}
+
+static void process_ref_frame_info(struct cedrus_dev *dev)
+{
+	u8 refresh_golden_frame = read_bits(dev, 1, VP8_PROB_HALF);
+	u8 refresh_alt_ref_frame = read_bits(dev, 1, VP8_PROB_HALF);
+
+	if (!refresh_golden_frame)
+		read_bits(dev, 2, VP8_PROB_HALF);
+
+	if (!refresh_alt_ref_frame)
+		read_bits(dev, 2, VP8_PROB_HALF);
+
+	read_bits(dev, 1, VP8_PROB_HALF);
+	read_bits(dev, 1, VP8_PROB_HALF);
+}
+
+static void cedrus_irq_clear(struct cedrus_dev *dev)
+{
+	cedrus_write(dev, VE_H264_STATUS,
+		     VE_H264_STATUS_INT_MASK);
+}
+
+static void cedrus_read_header(struct cedrus_dev *dev,
+			       const struct v4l2_ctrl_vp8_frame *slice)
+{
+	int i, j;
+
+	if (V4L2_VP8_FRAME_IS_KEY_FRAME(slice)) {
+		read_bits(dev, 1, VP8_PROB_HALF);
+		read_bits(dev, 1, VP8_PROB_HALF);
+	}
+
+	if (read_bits(dev, 1, VP8_PROB_HALF))
+		process_segmentation_info(dev);
+
+	read_bits(dev, 1, VP8_PROB_HALF);
+	read_bits(dev, 6, VP8_PROB_HALF);
+	read_bits(dev, 3, VP8_PROB_HALF);
+
+	if (read_bits(dev, 1, VP8_PROB_HALF))
+		process_ref_lf_delta_info(dev);
+
+	read_bits(dev, 2, VP8_PROB_HALF);
+
+	/* y_ac_qi */
+	read_bits(dev, 7, VP8_PROB_HALF);
+
+	/* Parses y_dc_delta, y2_dc_delta, etc. */
+	for (i = 0; i < QUANT_DELTA_COUNT; i++)
+		get_delta_q(dev);
+
+	if (!V4L2_VP8_FRAME_IS_KEY_FRAME(slice))
+		process_ref_frame_info(dev);
+
+	read_bits(dev, 1, VP8_PROB_HALF);
+
+	if (!V4L2_VP8_FRAME_IS_KEY_FRAME(slice))
+		read_bits(dev, 1, VP8_PROB_HALF);
+
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE, VE_H264_TRIGGER_TYPE_VP8_UPDATE_COEF);
+	cedrus_wait_for(dev, VE_H264_STATUS, VE_H264_STATUS_VP8_UPPROB_BUSY);
+	cedrus_irq_clear(dev);
+
+	if (read_bits(dev, 1, VP8_PROB_HALF))
+		read_bits(dev, 8, VP8_PROB_HALF);
+
+	if (!V4L2_VP8_FRAME_IS_KEY_FRAME(slice)) {
+		read_bits(dev, 8, VP8_PROB_HALF);
+		read_bits(dev, 8, VP8_PROB_HALF);
+		read_bits(dev, 8, VP8_PROB_HALF);
+
+		if (read_bits(dev, 1, VP8_PROB_HALF)) {
+			read_bits(dev, 8, VP8_PROB_HALF);
+			read_bits(dev, 8, VP8_PROB_HALF);
+			read_bits(dev, 8, VP8_PROB_HALF);
+			read_bits(dev, 8, VP8_PROB_HALF);
+		}
+
+		if (read_bits(dev, 1, VP8_PROB_HALF)) {
+			read_bits(dev, 8, VP8_PROB_HALF);
+			read_bits(dev, 8, VP8_PROB_HALF);
+			read_bits(dev, 8, VP8_PROB_HALF);
+		}
+
+		for (i = 0; i < 2; i++)
+			for (j = 0; j < V4L2_VP8_MV_PROB_CNT; j++)
+				if (read_bits(dev, 1, k_mv_entropy_update_probs[i][j]))
+					read_bits(dev, 7, VP8_PROB_HALF);
+	}
+}
+
+static void cedrus_vp8_update_probs(const struct v4l2_ctrl_vp8_frame *slice,
+				    u8 *prob_table)
+{
+	int i, j, k;
+
+	memcpy(&prob_table[0x1008], slice->entropy.y_mode_probs,
+	       sizeof(slice->entropy.y_mode_probs));
+	memcpy(&prob_table[0x1010], slice->entropy.uv_mode_probs,
+	       sizeof(slice->entropy.uv_mode_probs));
+
+	memcpy(&prob_table[0x1018], slice->segment.segment_probs,
+	       sizeof(slice->segment.segment_probs));
+
+	prob_table[0x101c] = slice->prob_skip_false;
+	prob_table[0x101d] = slice->prob_intra;
+	prob_table[0x101e] = slice->prob_last;
+	prob_table[0x101f] = slice->prob_gf;
+
+	memcpy(&prob_table[0x1020], slice->entropy.mv_probs[0],
+	       V4L2_VP8_MV_PROB_CNT);
+	memcpy(&prob_table[0x1040], slice->entropy.mv_probs[1],
+	       V4L2_VP8_MV_PROB_CNT);
+
+	for (i = 0; i < 4; ++i)
+		for (j = 0; j < 8; ++j)
+			for (k = 0; k < 3; ++k)
+				memcpy(&prob_table[i * 512 + j * 64 + k * 16],
+				       slice->entropy.coeff_probs[i][j][k], 11);
+}
+
+static enum cedrus_irq_status
+cedrus_vp8_irq_status(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_H264_STATUS);
+
+	if (reg & (VE_H264_STATUS_DECODE_ERR_INT |
+		   VE_H264_STATUS_VLD_DATA_REQ_INT))
+		return CEDRUS_IRQ_ERROR;
+
+	if (reg & VE_H264_CTRL_SLICE_DECODE_INT)
+		return CEDRUS_IRQ_OK;
+
+	return CEDRUS_IRQ_NONE;
+}
+
+static void cedrus_vp8_irq_clear(struct cedrus_ctx *ctx)
+{
+	cedrus_irq_clear(ctx->dev);
+}
+
+static void cedrus_vp8_irq_disable(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+	u32 reg = cedrus_read(dev, VE_H264_CTRL);
+
+	cedrus_write(dev, VE_H264_CTRL,
+		     reg & ~VE_H264_CTRL_INT_MASK);
+}
+
+static int cedrus_vp8_setup(struct cedrus_ctx *ctx, struct cedrus_run *run)
+{
+	const struct v4l2_ctrl_vp8_frame *slice = run->vp8.frame_params;
+	struct vb2_queue *cap_q = &ctx->fh.m2m_ctx->cap_q_ctx.q;
+	struct vb2_buffer *src_buf = &run->src->vb2_buf;
+	struct cedrus_dev *dev = ctx->dev;
+	dma_addr_t luma_addr, chroma_addr;
+	dma_addr_t src_buf_addr;
+	int header_size;
+	u32 reg;
+
+	cedrus_engine_enable(ctx, CEDRUS_CODEC_VP8);
+
+	cedrus_write(dev, VE_H264_CTRL, VE_H264_CTRL_VP8);
+
+	cedrus_vp8_update_probs(slice, ctx->codec.vp8.entropy_probs_buf);
+
+	reg = slice->first_part_size * 8;
+	cedrus_write(dev, VE_VP8_FIRST_DATA_PART_LEN, reg);
+
+	header_size = V4L2_VP8_FRAME_IS_KEY_FRAME(slice) ? 10 : 3;
+
+	reg = slice->first_part_size + header_size;
+	cedrus_write(dev, VE_VP8_PART_SIZE_OFFSET, reg);
+
+	reg = vb2_plane_size(src_buf, 0) * 8;
+	cedrus_write(dev, VE_H264_VLD_LEN, reg);
+
+	/*
+	 * FIXME: There is a problem if frame header is skipped (adding
+	 * first_part_header_bits to offset). It seems that functions
+	 * for parsing bitstreams change internal state of VPU in some
+	 * way that can't be otherwise set. Maybe this can be bypassed
+	 * by somehow fixing probability table buffer?
+	 */
+	reg = header_size * 8;
+	cedrus_write(dev, VE_H264_VLD_OFFSET, reg);
+
+	src_buf_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	cedrus_write(dev, VE_H264_VLD_END,
+		     src_buf_addr + vb2_get_plane_payload(src_buf, 0));
+	cedrus_write(dev, VE_H264_VLD_ADDR,
+		     VE_H264_VLD_ADDR_VAL(src_buf_addr) |
+		     VE_H264_VLD_ADDR_FIRST | VE_H264_VLD_ADDR_VALID |
+		     VE_H264_VLD_ADDR_LAST);
+
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+		     VE_H264_TRIGGER_TYPE_INIT_SWDEC);
+
+	cedrus_write(dev, VE_VP8_ENTROPY_PROBS_ADDR,
+		     ctx->codec.vp8.entropy_probs_buf_dma);
+
+	reg = 0;
+	switch (slice->version) {
+	case 1:
+		reg |= VE_VP8_PPS_FILTER_TYPE_SIMPLE;
+		reg |= VE_VP8_PPS_BILINEAR_MC_FILTER;
+		break;
+	case 2:
+		reg |= VE_VP8_PPS_LPF_DISABLE;
+		reg |= VE_VP8_PPS_BILINEAR_MC_FILTER;
+		break;
+	case 3:
+		reg |= VE_VP8_PPS_LPF_DISABLE;
+		reg |= VE_VP8_PPS_FULL_PIXEL;
+		break;
+	}
+	if (slice->segment.flags & V4L2_VP8_SEGMENT_FLAG_UPDATE_MAP)
+		reg |= VE_VP8_PPS_UPDATE_MB_SEGMENTATION_MAP;
+	if (!(slice->segment.flags & V4L2_VP8_SEGMENT_FLAG_DELTA_VALUE_MODE))
+		reg |= VE_VP8_PPS_MB_SEGMENT_ABS_DELTA;
+	if (slice->segment.flags & V4L2_VP8_SEGMENT_FLAG_ENABLED)
+		reg |= VE_VP8_PPS_SEGMENTATION_ENABLE;
+	if (ctx->codec.vp8.last_filter_type)
+		reg |= VE_VP8_PPS_LAST_LOOP_FILTER_SIMPLE;
+	reg |= VE_VP8_PPS_SHARPNESS_LEVEL(slice->lf.sharpness_level);
+	if (slice->lf.flags & V4L2_VP8_LF_FILTER_TYPE_SIMPLE)
+		reg |= VE_VP8_PPS_LOOP_FILTER_SIMPLE;
+	reg |= VE_VP8_PPS_LOOP_FILTER_LEVEL(slice->lf.level);
+	if (slice->lf.flags & V4L2_VP8_LF_ADJ_ENABLE)
+		reg |= VE_VP8_PPS_MODE_REF_LF_DELTA_ENABLE;
+	if (slice->lf.flags & V4L2_VP8_LF_DELTA_UPDATE)
+		reg |= VE_VP8_PPS_MODE_REF_LF_DELTA_UPDATE;
+	reg |= VE_VP8_PPS_TOKEN_PARTITION(ilog2(slice->num_dct_parts));
+	if (slice->flags & V4L2_VP8_FRAME_FLAG_MB_NO_SKIP_COEFF)
+		reg |= VE_VP8_PPS_MB_NO_COEFF_SKIP;
+	reg |= VE_VP8_PPS_RELOAD_ENTROPY_PROBS;
+	if (slice->flags & V4L2_VP8_FRAME_FLAG_SIGN_BIAS_GOLDEN)
+		reg |= VE_VP8_PPS_GOLDEN_SIGN_BIAS;
+	if (slice->flags & V4L2_VP8_FRAME_FLAG_SIGN_BIAS_ALT)
+		reg |= VE_VP8_PPS_ALTREF_SIGN_BIAS;
+	if (ctx->codec.vp8.last_frame_p_type)
+		reg |= VE_VP8_PPS_LAST_PIC_TYPE_P_FRAME;
+	reg |= VE_VP8_PPS_LAST_SHARPNESS_LEVEL(ctx->codec.vp8.last_sharpness_level);
+	if (!(slice->flags & V4L2_VP8_FRAME_FLAG_KEY_FRAME))
+		reg |= VE_VP8_PPS_PIC_TYPE_P_FRAME;
+	cedrus_write(dev, VE_VP8_PPS, reg);
+
+	cedrus_read_header(dev, slice);
+
+	/* reset registers changed by HW */
+	cedrus_write(dev, VE_H264_CUR_MB_NUM, 0);
+	cedrus_write(dev, VE_H264_MB_ADDR, 0);
+	cedrus_write(dev, VE_H264_ERROR_CASE, 0);
+
+	reg = 0;
+	reg |= VE_VP8_QP_INDEX_DELTA_UVAC(slice->quant.uv_ac_delta);
+	reg |= VE_VP8_QP_INDEX_DELTA_UVDC(slice->quant.uv_dc_delta);
+	reg |= VE_VP8_QP_INDEX_DELTA_Y2AC(slice->quant.y2_ac_delta);
+	reg |= VE_VP8_QP_INDEX_DELTA_Y2DC(slice->quant.y2_dc_delta);
+	reg |= VE_VP8_QP_INDEX_DELTA_Y1DC(slice->quant.y_dc_delta);
+	reg |= VE_VP8_QP_INDEX_DELTA_BASE_QINDEX(slice->quant.y_ac_qi);
+	cedrus_write(dev, VE_VP8_QP_INDEX_DELTA, reg);
+
+	reg = 0;
+	reg |= VE_VP8_FSIZE_WIDTH(slice->width);
+	reg |= VE_VP8_FSIZE_HEIGHT(slice->height);
+	cedrus_write(dev, VE_VP8_FSIZE, reg);
+
+	reg = 0;
+	reg |= VE_VP8_PICSIZE_WIDTH(slice->width);
+	reg |= VE_VP8_PICSIZE_HEIGHT(slice->height);
+	cedrus_write(dev, VE_VP8_PICSIZE, reg);
+
+	reg = 0;
+	reg |= VE_VP8_SEGMENT3(slice->segment.quant_update[3]);
+	reg |= VE_VP8_SEGMENT2(slice->segment.quant_update[2]);
+	reg |= VE_VP8_SEGMENT1(slice->segment.quant_update[1]);
+	reg |= VE_VP8_SEGMENT0(slice->segment.quant_update[0]);
+	cedrus_write(dev, VE_VP8_SEGMENT_FEAT_MB_LV0, reg);
+
+	reg = 0;
+	reg |= VE_VP8_SEGMENT3(slice->segment.lf_update[3]);
+	reg |= VE_VP8_SEGMENT2(slice->segment.lf_update[2]);
+	reg |= VE_VP8_SEGMENT1(slice->segment.lf_update[1]);
+	reg |= VE_VP8_SEGMENT0(slice->segment.lf_update[0]);
+	cedrus_write(dev, VE_VP8_SEGMENT_FEAT_MB_LV1, reg);
+
+	reg = 0;
+	reg |= VE_VP8_LF_DELTA3(slice->lf.ref_frm_delta[3]);
+	reg |= VE_VP8_LF_DELTA2(slice->lf.ref_frm_delta[2]);
+	reg |= VE_VP8_LF_DELTA1(slice->lf.ref_frm_delta[1]);
+	reg |= VE_VP8_LF_DELTA0(slice->lf.ref_frm_delta[0]);
+	cedrus_write(dev, VE_VP8_REF_LF_DELTA, reg);
+
+	reg = 0;
+	reg |= VE_VP8_LF_DELTA3(slice->lf.mb_mode_delta[3]);
+	reg |= VE_VP8_LF_DELTA2(slice->lf.mb_mode_delta[2]);
+	reg |= VE_VP8_LF_DELTA1(slice->lf.mb_mode_delta[1]);
+	reg |= VE_VP8_LF_DELTA0(slice->lf.mb_mode_delta[0]);
+	cedrus_write(dev, VE_VP8_MODE_LF_DELTA, reg);
+
+	luma_addr = cedrus_dst_buf_addr(ctx, &run->dst->vb2_buf, 0);
+	chroma_addr = cedrus_dst_buf_addr(ctx, &run->dst->vb2_buf, 1);
+	cedrus_write(dev, VE_VP8_REC_LUMA, luma_addr);
+	cedrus_write(dev, VE_VP8_REC_CHROMA, chroma_addr);
+
+	cedrus_write_ref_buf_addr(ctx, cap_q, slice->last_frame_ts,
+				  VE_VP8_FWD_LUMA, VE_VP8_FWD_CHROMA);
+	cedrus_write_ref_buf_addr(ctx, cap_q, slice->golden_frame_ts,
+				  VE_VP8_BWD_LUMA, VE_VP8_BWD_CHROMA);
+	cedrus_write_ref_buf_addr(ctx, cap_q, slice->alt_frame_ts,
+				  VE_VP8_ALT_LUMA, VE_VP8_ALT_CHROMA);
+
+	cedrus_write(dev, VE_H264_CTRL, VE_H264_CTRL_VP8 |
+		     VE_H264_CTRL_DECODE_ERR_INT |
+		     VE_H264_CTRL_SLICE_DECODE_INT);
+
+	if (slice->lf.level) {
+		ctx->codec.vp8.last_filter_type =
+			!!(slice->lf.flags & V4L2_VP8_LF_FILTER_TYPE_SIMPLE);
+		ctx->codec.vp8.last_frame_p_type =
+			!V4L2_VP8_FRAME_IS_KEY_FRAME(slice);
+		ctx->codec.vp8.last_sharpness_level =
+			slice->lf.sharpness_level;
+	}
+
+	return 0;
+}
+
+static int cedrus_vp8_start(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	ctx->codec.vp8.entropy_probs_buf =
+		dma_alloc_coherent(dev->dev, CEDRUS_ENTROPY_PROBS_SIZE,
+				   &ctx->codec.vp8.entropy_probs_buf_dma,
+				   GFP_KERNEL);
+	if (!ctx->codec.vp8.entropy_probs_buf)
+		return -ENOMEM;
+
+	/*
+	 * This offset has been discovered by reverse engineering, we don’t know
+	 * what it actually means.
+	 */
+	memcpy(&ctx->codec.vp8.entropy_probs_buf[2048],
+	       prob_table_init, sizeof(prob_table_init));
+
+	return 0;
+}
+
+static void cedrus_vp8_stop(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_engine_disable(dev);
+
+	dma_free_coherent(dev->dev, CEDRUS_ENTROPY_PROBS_SIZE,
+			  ctx->codec.vp8.entropy_probs_buf,
+			  ctx->codec.vp8.entropy_probs_buf_dma);
+}
+
+static void cedrus_vp8_trigger(struct cedrus_ctx *ctx)
+{
+	struct cedrus_dev *dev = ctx->dev;
+
+	cedrus_write(dev, VE_H264_TRIGGER_TYPE,
+		     VE_H264_TRIGGER_TYPE_VP8_SLICE_DECODE);
+}
+
+struct cedrus_dec_ops cedrus_dec_ops_vp8 = {
+	.irq_clear	= cedrus_vp8_irq_clear,
+	.irq_disable	= cedrus_vp8_irq_disable,
+	.irq_status	= cedrus_vp8_irq_status,
+	.setup		= cedrus_vp8_setup,
+	.start		= cedrus_vp8_start,
+	.stop		= cedrus_vp8_stop,
+	.trigger	= cedrus_vp8_trigger,
+};
diff --git a/drivers/staging/media/tegra-video/Kconfig b/drivers/staging/media/tegra-video/Kconfig
new file mode 100644
index 000000000..df1b2cff2
--- /dev/null
+++ b/drivers/staging/media/tegra-video/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_TEGRA
+	tristate "NVIDIA Tegra VI driver"
+	depends on TEGRA_HOST1X
+	depends on VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	  Choose this option if you have an NVIDIA Tegra SoC.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called tegra-video.
+
+config VIDEO_TEGRA_TPG
+	bool "NVIDIA Tegra VI driver TPG mode"
+	depends on VIDEO_TEGRA
+	help
+	  Say yes here to enable Tegra internal TPG mode
diff --git a/drivers/staging/media/tegra-video/Makefile b/drivers/staging/media/tegra-video/Makefile
new file mode 100644
index 000000000..dfa2ef8f9
--- /dev/null
+++ b/drivers/staging/media/tegra-video/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+tegra-video-objs := \
+		video.o \
+		vi.o \
+		csi.o
+
+tegra-video-$(CONFIG_ARCH_TEGRA_210_SOC) += tegra210.o
+obj-$(CONFIG_VIDEO_TEGRA) += tegra-video.o
diff --git a/drivers/staging/media/tegra-video/TODO b/drivers/staging/media/tegra-video/TODO
new file mode 100644
index 000000000..c82108166
--- /dev/null
+++ b/drivers/staging/media/tegra-video/TODO
@@ -0,0 +1,5 @@
+TODO list
+* Add support for Ganged mode.
+* Add RAW10 packed video format support to Tegra210 video formats.
+* Add support for suspend and resume.
+* Make sure v4l2-compliance tests pass with all of the above implementations.
diff --git a/drivers/staging/media/tegra-video/csi.c b/drivers/staging/media/tegra-video/csi.c
new file mode 100644
index 000000000..426e653bd
--- /dev/null
+++ b/drivers/staging/media/tegra-video/csi.c
@@ -0,0 +1,815 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk/tegra.h>
+#include <linux/device.h>
+#include <linux/host1x.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-fwnode.h>
+
+#include "csi.h"
+#include "video.h"
+
+#define MHZ			1000000
+
+static inline struct tegra_csi *
+host1x_client_to_csi(struct host1x_client *client)
+{
+	return container_of(client, struct tegra_csi, client);
+}
+
+static inline struct tegra_csi_channel *to_csi_chan(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct tegra_csi_channel, subdev);
+}
+
+/*
+ * CSI is a separate subdevice which has 6 source pads to generate
+ * test pattern. CSI subdevice pad ops are used only for TPG and
+ * allows below TPG formats.
+ */
+static const struct v4l2_mbus_framefmt tegra_csi_tpg_fmts[] = {
+	{
+		TEGRA_DEF_WIDTH,
+		TEGRA_DEF_HEIGHT,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		V4L2_FIELD_NONE,
+		V4L2_COLORSPACE_SRGB
+	},
+	{
+		TEGRA_DEF_WIDTH,
+		TEGRA_DEF_HEIGHT,
+		MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+		V4L2_FIELD_NONE,
+		V4L2_COLORSPACE_SRGB
+	},
+};
+
+static const struct v4l2_frmsize_discrete tegra_csi_tpg_sizes[] = {
+	{ 1280, 720 },
+	{ 1920, 1080 },
+	{ 3840, 2160 },
+};
+
+/*
+ * V4L2 Subdevice Pad Operations
+ */
+static int csi_enum_bus_code(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	if (code->index >= ARRAY_SIZE(tegra_csi_tpg_fmts))
+		return -EINVAL;
+
+	code->code = tegra_csi_tpg_fmts[code->index].code;
+
+	return 0;
+}
+
+static int csi_get_format(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	fmt->format = csi_chan->format;
+
+	return 0;
+}
+
+static int csi_get_frmrate_table_index(struct tegra_csi *csi, u32 code,
+				       u32 width, u32 height)
+{
+	const struct tpg_framerate *frmrate;
+	unsigned int i;
+
+	frmrate = csi->soc->tpg_frmrate_table;
+	for (i = 0; i < csi->soc->tpg_frmrate_table_size; i++) {
+		if (frmrate[i].code == code &&
+		    frmrate[i].frmsize.width == width &&
+		    frmrate[i].frmsize.height == height) {
+			return i;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void csi_chan_update_blank_intervals(struct tegra_csi_channel *csi_chan,
+					    u32 code, u32 width, u32 height)
+{
+	struct tegra_csi *csi = csi_chan->csi;
+	const struct tpg_framerate *frmrate = csi->soc->tpg_frmrate_table;
+	int index;
+
+	index = csi_get_frmrate_table_index(csi_chan->csi, code,
+					    width, height);
+	if (index >= 0) {
+		csi_chan->h_blank = frmrate[index].h_blank;
+		csi_chan->v_blank = frmrate[index].v_blank;
+		csi_chan->framerate = frmrate[index].framerate;
+	}
+}
+
+static int csi_enum_framesizes(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	unsigned int i;
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	if (fse->index >= ARRAY_SIZE(tegra_csi_tpg_sizes))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++)
+		if (fse->code == tegra_csi_tpg_fmts[i].code)
+			break;
+
+	if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
+		return -EINVAL;
+
+	fse->min_width = tegra_csi_tpg_sizes[fse->index].width;
+	fse->max_width = tegra_csi_tpg_sizes[fse->index].width;
+	fse->min_height = tegra_csi_tpg_sizes[fse->index].height;
+	fse->max_height = tegra_csi_tpg_sizes[fse->index].height;
+
+	return 0;
+}
+
+static int csi_enum_frameintervals(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+	struct tegra_csi *csi = csi_chan->csi;
+	const struct tpg_framerate *frmrate = csi->soc->tpg_frmrate_table;
+	int index;
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	/* one framerate per format and resolution */
+	if (fie->index > 0)
+		return -EINVAL;
+
+	index = csi_get_frmrate_table_index(csi_chan->csi, fie->code,
+					    fie->width, fie->height);
+	if (index < 0)
+		return -EINVAL;
+
+	fie->interval.numerator = 1;
+	fie->interval.denominator = frmrate[index].framerate;
+
+	return 0;
+}
+
+static int csi_set_format(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+	struct v4l2_mbus_framefmt *format = &fmt->format;
+	const struct v4l2_frmsize_discrete *sizes;
+	unsigned int i;
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	sizes = v4l2_find_nearest_size(tegra_csi_tpg_sizes,
+				       ARRAY_SIZE(tegra_csi_tpg_sizes),
+				       width, height,
+				       format->width, format->width);
+	format->width = sizes->width;
+	format->height = sizes->height;
+
+	for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++)
+		if (format->code == tegra_csi_tpg_fmts[i].code)
+			break;
+
+	if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
+		i = 0;
+
+	format->code = tegra_csi_tpg_fmts[i].code;
+	format->field = V4L2_FIELD_NONE;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	/* update blanking intervals from frame rate table and format */
+	csi_chan_update_blank_intervals(csi_chan, format->code,
+					format->width, format->height);
+	csi_chan->format = *format;
+
+	return 0;
+}
+
+/*
+ * V4L2 Subdevice Video Operations
+ */
+static int tegra_csi_g_frame_interval(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_frame_interval *vfi)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return -ENOIOCTLCMD;
+
+	vfi->interval.numerator = 1;
+	vfi->interval.denominator = csi_chan->framerate;
+
+	return 0;
+}
+
+static unsigned int csi_get_pixel_rate(struct tegra_csi_channel *csi_chan)
+{
+	struct tegra_vi_channel *chan;
+	struct v4l2_subdev *src_subdev;
+	struct v4l2_ctrl *ctrl;
+
+	chan = v4l2_get_subdev_hostdata(&csi_chan->subdev);
+	src_subdev = tegra_channel_get_remote_source_subdev(chan);
+	ctrl = v4l2_ctrl_find(src_subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
+	if (ctrl)
+		return v4l2_ctrl_g_ctrl_int64(ctrl);
+
+	return 0;
+}
+
+void tegra_csi_calc_settle_time(struct tegra_csi_channel *csi_chan,
+				u8 csi_port_num,
+				u8 *clk_settle_time,
+				u8 *ths_settle_time)
+{
+	struct tegra_csi *csi = csi_chan->csi;
+	unsigned int cil_clk_mhz;
+	unsigned int pix_clk_mhz;
+	int clk_idx = (csi_port_num >> 1) + 1;
+
+	cil_clk_mhz = clk_get_rate(csi->clks[clk_idx].clk) / MHZ;
+	pix_clk_mhz = csi_get_pixel_rate(csi_chan) / MHZ;
+
+	/*
+	 * CLK Settle time is the interval during which HS receiver should
+	 * ignore any clock lane HS transitions, starting from the beginning
+	 * of T-CLK-PREPARE.
+	 * Per DPHY specification, T-CLK-SETTLE should be between 95ns ~ 300ns
+	 *
+	 * 95ns < (clk-settle-programmed + 7) * lp clk period < 300ns
+	 * midpoint = 197.5 ns
+	 */
+	*clk_settle_time = ((95 + 300) * cil_clk_mhz - 14000) / 2000;
+
+	/*
+	 * THS Settle time is the interval during which HS receiver should
+	 * ignore any data lane HS transitions, starting from the beginning
+	 * of THS-PREPARE.
+	 *
+	 * Per DPHY specification, T-HS-SETTLE should be between 85ns + 6UI
+	 * and 145ns+10UI.
+	 * 85ns + 6UI < (Ths-settle-prog + 5) * lp_clk_period < 145ns + 10UI
+	 * midpoint = 115ns + 8UI
+	 */
+	if (pix_clk_mhz)
+		*ths_settle_time = (115 * cil_clk_mhz + 8000 * cil_clk_mhz
+				   / (2 * pix_clk_mhz) - 5000) / 1000;
+}
+
+static int tegra_csi_enable_stream(struct v4l2_subdev *subdev)
+{
+	struct tegra_vi_channel *chan = v4l2_get_subdev_hostdata(subdev);
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+	struct tegra_csi *csi = csi_chan->csi;
+	int ret, err;
+
+	ret = pm_runtime_resume_and_get(csi->dev);
+	if (ret < 0) {
+		dev_err(csi->dev, "failed to get runtime PM: %d\n", ret);
+		return ret;
+	}
+
+	if (csi_chan->mipi) {
+		ret = tegra_mipi_enable(csi_chan->mipi);
+		if (ret < 0) {
+			dev_err(csi->dev,
+				"failed to enable MIPI pads: %d\n", ret);
+			goto rpm_put;
+		}
+
+		/*
+		 * CSI MIPI pads PULLUP, PULLDN and TERM impedances need to
+		 * be calibrated after power on.
+		 * So, trigger the calibration start here and results will
+		 * be latched and applied to the pads when link is in LP11
+		 * state during start of sensor streaming.
+		 */
+		ret = tegra_mipi_start_calibration(csi_chan->mipi);
+		if (ret < 0) {
+			dev_err(csi->dev,
+				"failed to start MIPI calibration: %d\n", ret);
+			goto disable_mipi;
+		}
+	}
+
+	csi_chan->pg_mode = chan->pg_mode;
+	ret = csi->ops->csi_start_streaming(csi_chan);
+	if (ret < 0)
+		goto finish_calibration;
+
+	return 0;
+
+finish_calibration:
+	if (csi_chan->mipi)
+		tegra_mipi_finish_calibration(csi_chan->mipi);
+disable_mipi:
+	if (csi_chan->mipi) {
+		err = tegra_mipi_disable(csi_chan->mipi);
+		if (err < 0)
+			dev_err(csi->dev,
+				"failed to disable MIPI pads: %d\n", err);
+	}
+
+rpm_put:
+	pm_runtime_put(csi->dev);
+	return ret;
+}
+
+static int tegra_csi_disable_stream(struct v4l2_subdev *subdev)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
+	struct tegra_csi *csi = csi_chan->csi;
+	int err;
+
+	csi->ops->csi_stop_streaming(csi_chan);
+
+	if (csi_chan->mipi) {
+		err = tegra_mipi_disable(csi_chan->mipi);
+		if (err < 0)
+			dev_err(csi->dev,
+				"failed to disable MIPI pads: %d\n", err);
+	}
+
+	pm_runtime_put(csi->dev);
+
+	return 0;
+}
+
+static int tegra_csi_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	int ret;
+
+	if (enable)
+		ret = tegra_csi_enable_stream(subdev);
+	else
+		ret = tegra_csi_disable_stream(subdev);
+
+	return ret;
+}
+
+/*
+ * V4L2 Subdevice Operations
+ */
+static const struct v4l2_subdev_video_ops tegra_csi_video_ops = {
+	.s_stream = tegra_csi_s_stream,
+	.g_frame_interval = tegra_csi_g_frame_interval,
+	.s_frame_interval = tegra_csi_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops tegra_csi_pad_ops = {
+	.enum_mbus_code		= csi_enum_bus_code,
+	.enum_frame_size	= csi_enum_framesizes,
+	.enum_frame_interval	= csi_enum_frameintervals,
+	.get_fmt		= csi_get_format,
+	.set_fmt		= csi_set_format,
+};
+
+static const struct v4l2_subdev_ops tegra_csi_ops = {
+	.video  = &tegra_csi_video_ops,
+	.pad    = &tegra_csi_pad_ops,
+};
+
+static int tegra_csi_channel_alloc(struct tegra_csi *csi,
+				   struct device_node *node,
+				   unsigned int port_num, unsigned int lanes,
+				   unsigned int num_pads)
+{
+	struct tegra_csi_channel *chan;
+	int ret = 0, i;
+
+	chan = kzalloc(sizeof(*chan), GFP_KERNEL);
+	if (!chan)
+		return -ENOMEM;
+
+	list_add_tail(&chan->list, &csi->csi_chans);
+	chan->csi = csi;
+	/*
+	 * Each CSI brick has maximum of 4 lanes.
+	 * For lanes more than 4, use multiple of immediate CSI bricks as gang.
+	 */
+	if (lanes <= CSI_LANES_PER_BRICK) {
+		chan->numlanes = lanes;
+		chan->numgangports = 1;
+	} else {
+		chan->numlanes = CSI_LANES_PER_BRICK;
+		chan->numgangports = lanes / CSI_LANES_PER_BRICK;
+	}
+
+	for (i = 0; i < chan->numgangports; i++)
+		chan->csi_port_nums[i] = port_num + i * CSI_PORTS_PER_BRICK;
+
+	chan->of_node = of_node_get(node);
+	chan->numpads = num_pads;
+	if (num_pads & 0x2) {
+		chan->pads[0].flags = MEDIA_PAD_FL_SINK;
+		chan->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+	} else {
+		chan->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	}
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return 0;
+
+	chan->mipi = tegra_mipi_request(csi->dev, node);
+	if (IS_ERR(chan->mipi)) {
+		ret = PTR_ERR(chan->mipi);
+		chan->mipi = NULL;
+		dev_err(csi->dev, "failed to get mipi device: %d\n", ret);
+	}
+
+	return ret;
+}
+
+static int tegra_csi_tpg_channels_alloc(struct tegra_csi *csi)
+{
+	struct device_node *node = csi->dev->of_node;
+	unsigned int port_num;
+	unsigned int tpg_channels = csi->soc->csi_max_channels;
+	int ret;
+
+	/* allocate CSI channel for each CSI x2 ports */
+	for (port_num = 0; port_num < tpg_channels; port_num++) {
+		ret = tegra_csi_channel_alloc(csi, node, port_num, 2, 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int tegra_csi_channels_alloc(struct tegra_csi *csi)
+{
+	struct device_node *node = csi->dev->of_node;
+	struct v4l2_fwnode_endpoint v4l2_ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *fwh;
+	struct device_node *channel;
+	struct device_node *ep;
+	unsigned int lanes, portno, num_pads;
+	int ret;
+
+	for_each_child_of_node(node, channel) {
+		if (!of_node_name_eq(channel, "channel"))
+			continue;
+
+		ret = of_property_read_u32(channel, "reg", &portno);
+		if (ret < 0)
+			continue;
+
+		if (portno >= csi->soc->csi_max_channels) {
+			dev_err(csi->dev, "invalid port num %d for %pOF\n",
+				portno, channel);
+			ret = -EINVAL;
+			goto err_node_put;
+		}
+
+		ep = of_graph_get_endpoint_by_regs(channel, 0, 0);
+		if (!ep)
+			continue;
+
+		fwh = of_fwnode_handle(ep);
+		ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
+		of_node_put(ep);
+		if (ret) {
+			dev_err(csi->dev,
+				"failed to parse v4l2 endpoint for %pOF: %d\n",
+				channel, ret);
+			goto err_node_put;
+		}
+
+		lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
+		/*
+		 * Each CSI brick has maximum 4 data lanes.
+		 * For lanes more than 4, validate lanes to be multiple of 4
+		 * so multiple of consecutive CSI bricks can be ganged up for
+		 * streaming.
+		 */
+		if (!lanes || ((lanes & (lanes - 1)) != 0) ||
+		    (lanes > CSI_LANES_PER_BRICK && ((portno & 1) != 0))) {
+			dev_err(csi->dev, "invalid data-lanes %d for %pOF\n",
+				lanes, channel);
+			ret = -EINVAL;
+			goto err_node_put;
+		}
+
+		num_pads = of_graph_get_endpoint_count(channel);
+		if (num_pads == TEGRA_CSI_PADS_NUM) {
+			ret = tegra_csi_channel_alloc(csi, channel, portno,
+						      lanes, num_pads);
+			if (ret < 0)
+				goto err_node_put;
+		}
+	}
+
+	return 0;
+
+err_node_put:
+	of_node_put(channel);
+	return ret;
+}
+
+static int tegra_csi_channel_init(struct tegra_csi_channel *chan)
+{
+	struct tegra_csi *csi = chan->csi;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	/* initialize the default format */
+	chan->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	chan->format.field = V4L2_FIELD_NONE;
+	chan->format.colorspace = V4L2_COLORSPACE_SRGB;
+	chan->format.width = TEGRA_DEF_WIDTH;
+	chan->format.height = TEGRA_DEF_HEIGHT;
+	csi_chan_update_blank_intervals(chan, chan->format.code,
+					chan->format.width,
+					chan->format.height);
+	/* initialize V4L2 subdevice and media entity */
+	subdev = &chan->subdev;
+	v4l2_subdev_init(subdev, &tegra_csi_ops);
+	subdev->dev = csi->dev;
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "%s-%d", "tpg",
+			 chan->csi_port_nums[0]);
+	else
+		snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "%s",
+			 kbasename(chan->of_node->full_name));
+
+	v4l2_set_subdevdata(subdev, chan);
+	subdev->fwnode = of_fwnode_handle(chan->of_node);
+	subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+
+	/* initialize media entity pads */
+	ret = media_entity_pads_init(&subdev->entity, chan->numpads,
+				     chan->pads);
+	if (ret < 0) {
+		dev_err(csi->dev,
+			"failed to initialize media entity: %d\n", ret);
+		subdev->dev = NULL;
+		return ret;
+	}
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
+		ret = v4l2_async_register_subdev(subdev);
+		if (ret < 0) {
+			dev_err(csi->dev,
+				"failed to register subdev: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+void tegra_csi_error_recover(struct v4l2_subdev *sd)
+{
+	struct tegra_csi_channel *csi_chan = to_csi_chan(sd);
+	struct tegra_csi *csi = csi_chan->csi;
+
+	/* stop streaming during error recovery */
+	csi->ops->csi_stop_streaming(csi_chan);
+	csi->ops->csi_err_recover(csi_chan);
+	csi->ops->csi_start_streaming(csi_chan);
+}
+
+static int tegra_csi_channels_init(struct tegra_csi *csi)
+{
+	struct tegra_csi_channel *chan;
+	int ret;
+
+	list_for_each_entry(chan, &csi->csi_chans, list) {
+		ret = tegra_csi_channel_init(chan);
+		if (ret) {
+			dev_err(csi->dev,
+				"failed to initialize channel-%d: %d\n",
+				chan->csi_port_nums[0], ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void tegra_csi_channels_cleanup(struct tegra_csi *csi)
+{
+	struct v4l2_subdev *subdev;
+	struct tegra_csi_channel *chan, *tmp;
+
+	list_for_each_entry_safe(chan, tmp, &csi->csi_chans, list) {
+		if (chan->mipi)
+			tegra_mipi_free(chan->mipi);
+
+		subdev = &chan->subdev;
+		if (subdev->dev) {
+			if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+				v4l2_async_unregister_subdev(subdev);
+			media_entity_cleanup(&subdev->entity);
+		}
+
+		of_node_put(chan->of_node);
+		list_del(&chan->list);
+		kfree(chan);
+	}
+}
+
+static int __maybe_unused csi_runtime_suspend(struct device *dev)
+{
+	struct tegra_csi *csi = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(csi->soc->num_clks, csi->clks);
+
+	return 0;
+}
+
+static int __maybe_unused csi_runtime_resume(struct device *dev)
+{
+	struct tegra_csi *csi = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_bulk_prepare_enable(csi->soc->num_clks, csi->clks);
+	if (ret < 0) {
+		dev_err(csi->dev, "failed to enable clocks: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int tegra_csi_init(struct host1x_client *client)
+{
+	struct tegra_csi *csi = host1x_client_to_csi(client);
+	struct tegra_video_device *vid = dev_get_drvdata(client->host);
+	int ret;
+
+	INIT_LIST_HEAD(&csi->csi_chans);
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		ret = tegra_csi_tpg_channels_alloc(csi);
+	else
+		ret = tegra_csi_channels_alloc(csi);
+	if (ret < 0) {
+		dev_err(csi->dev,
+			"failed to allocate channels: %d\n", ret);
+		goto cleanup;
+	}
+
+	ret = tegra_csi_channels_init(csi);
+	if (ret < 0)
+		goto cleanup;
+
+	vid->csi = csi;
+
+	return 0;
+
+cleanup:
+	tegra_csi_channels_cleanup(csi);
+	return ret;
+}
+
+static int tegra_csi_exit(struct host1x_client *client)
+{
+	struct tegra_csi *csi = host1x_client_to_csi(client);
+
+	tegra_csi_channels_cleanup(csi);
+
+	return 0;
+}
+
+static const struct host1x_client_ops csi_client_ops = {
+	.init = tegra_csi_init,
+	.exit = tegra_csi_exit,
+};
+
+static int tegra_csi_probe(struct platform_device *pdev)
+{
+	struct tegra_csi *csi;
+	unsigned int i;
+	int ret;
+
+	csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
+	if (!csi)
+		return -ENOMEM;
+
+	csi->iomem = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(csi->iomem))
+		return PTR_ERR(csi->iomem);
+
+	csi->soc = of_device_get_match_data(&pdev->dev);
+
+	csi->clks = devm_kcalloc(&pdev->dev, csi->soc->num_clks,
+				 sizeof(*csi->clks), GFP_KERNEL);
+	if (!csi->clks)
+		return -ENOMEM;
+
+	for (i = 0; i < csi->soc->num_clks; i++)
+		csi->clks[i].id = csi->soc->clk_names[i];
+
+	ret = devm_clk_bulk_get(&pdev->dev, csi->soc->num_clks, csi->clks);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to get the clocks: %d\n", ret);
+		return ret;
+	}
+
+	if (!pdev->dev.pm_domain) {
+		ret = -ENOENT;
+		dev_warn(&pdev->dev, "PM domain is not attached: %d\n", ret);
+		return ret;
+	}
+
+	csi->dev = &pdev->dev;
+	csi->ops = csi->soc->ops;
+	platform_set_drvdata(pdev, csi);
+	pm_runtime_enable(&pdev->dev);
+
+	/* initialize host1x interface */
+	INIT_LIST_HEAD(&csi->client.list);
+	csi->client.ops = &csi_client_ops;
+	csi->client.dev = &pdev->dev;
+
+	ret = host1x_client_register(&csi->client);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"failed to register host1x client: %d\n", ret);
+		goto rpm_disable;
+	}
+
+	return 0;
+
+rpm_disable:
+	pm_runtime_disable(&pdev->dev);
+	return ret;
+}
+
+static int tegra_csi_remove(struct platform_device *pdev)
+{
+	struct tegra_csi *csi = platform_get_drvdata(pdev);
+	int err;
+
+	err = host1x_client_unregister(&csi->client);
+	if (err < 0) {
+		dev_err(&pdev->dev,
+			"failed to unregister host1x client: %d\n", err);
+		return err;
+	}
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id tegra_csi_of_id_table[] = {
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+	{ .compatible = "nvidia,tegra210-csi", .data = &tegra210_csi_soc },
+#endif
+	{ }
+};
+MODULE_DEVICE_TABLE(of, tegra_csi_of_id_table);
+
+static const struct dev_pm_ops tegra_csi_pm_ops = {
+	SET_RUNTIME_PM_OPS(csi_runtime_suspend, csi_runtime_resume, NULL)
+};
+
+struct platform_driver tegra_csi_driver = {
+	.driver = {
+		.name		= "tegra-csi",
+		.of_match_table	= tegra_csi_of_id_table,
+		.pm		= &tegra_csi_pm_ops,
+	},
+	.probe			= tegra_csi_probe,
+	.remove			= tegra_csi_remove,
+};
diff --git a/drivers/staging/media/tegra-video/csi.h b/drivers/staging/media/tegra-video/csi.h
new file mode 100644
index 000000000..6960ea2e3
--- /dev/null
+++ b/drivers/staging/media/tegra-video/csi.h
@@ -0,0 +1,163 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#ifndef __TEGRA_CSI_H__
+#define __TEGRA_CSI_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * Each CSI brick supports max of 4 lanes that can be used as either
+ * one x4 port using both CILA and CILB partitions of a CSI brick or can
+ * be used as two x2 ports with one x2 from CILA and the other x2 from
+ * CILB.
+ */
+#define CSI_PORTS_PER_BRICK	2
+#define CSI_LANES_PER_BRICK	4
+
+/* Maximum 2 CSI x4 ports can be ganged up for streaming */
+#define GANG_PORTS_MAX	2
+
+/* each CSI channel can have one sink and one source pads */
+#define TEGRA_CSI_PADS_NUM	2
+
+enum tegra_csi_cil_port {
+	PORT_A = 0,
+	PORT_B,
+};
+
+enum tegra_csi_block {
+	CSI_CIL_AB = 0,
+	CSI_CIL_CD,
+	CSI_CIL_EF,
+};
+
+struct tegra_csi;
+
+/**
+ * struct tegra_csi_channel - Tegra CSI channel
+ *
+ * @list: list head for this entry
+ * @subdev: V4L2 subdevice associated with this channel
+ * @pads: media pads for the subdevice entity
+ * @numpads: number of pads.
+ * @csi: Tegra CSI device structure
+ * @of_node: csi device tree node
+ * @numgangports: number of immediate ports ganged up to meet the
+ *             channel bus-width
+ * @numlanes: number of lanes used per port
+ * @csi_port_nums: CSI channel port numbers
+ * @pg_mode: test pattern generator mode for channel
+ * @format: active format of the channel
+ * @framerate: active framerate for TPG
+ * @h_blank: horizontal blanking for TPG active format
+ * @v_blank: vertical blanking for TPG active format
+ * @mipi: mipi device for corresponding csi channel pads, or NULL if not applicable (TPG, error)
+ * @pixel_rate: active pixel rate from the sensor on this channel
+ */
+struct tegra_csi_channel {
+	struct list_head list;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[TEGRA_CSI_PADS_NUM];
+	unsigned int numpads;
+	struct tegra_csi *csi;
+	struct device_node *of_node;
+	u8 numgangports;
+	unsigned int numlanes;
+	u8 csi_port_nums[GANG_PORTS_MAX];
+	u8 pg_mode;
+	struct v4l2_mbus_framefmt format;
+	unsigned int framerate;
+	unsigned int h_blank;
+	unsigned int v_blank;
+	struct tegra_mipi_device *mipi;
+	unsigned int pixel_rate;
+};
+
+/**
+ * struct tpg_framerate - Tegra CSI TPG framerate configuration
+ *
+ * @frmsize: frame resolution
+ * @code: media bus format code
+ * @h_blank: horizontal blanking used for TPG
+ * @v_blank: vertical blanking interval used for TPG
+ * @framerate: framerate achieved with the corresponding blanking intervals,
+ *		format and resolution.
+ */
+struct tpg_framerate {
+	struct v4l2_frmsize_discrete frmsize;
+	u32 code;
+	unsigned int h_blank;
+	unsigned int v_blank;
+	unsigned int framerate;
+};
+
+/**
+ * struct tegra_csi_ops - Tegra CSI operations
+ *
+ * @csi_start_streaming: programs csi hardware to enable streaming.
+ * @csi_stop_streaming: programs csi hardware to disable streaming.
+ * @csi_err_recover: csi hardware block recovery in case of any capture errors
+ *		due to missing source stream or due to improper csi input from
+ *		the external source.
+ */
+struct tegra_csi_ops {
+	int (*csi_start_streaming)(struct tegra_csi_channel *csi_chan);
+	void (*csi_stop_streaming)(struct tegra_csi_channel *csi_chan);
+	void (*csi_err_recover)(struct tegra_csi_channel *csi_chan);
+};
+
+/**
+ * struct tegra_csi_soc - NVIDIA Tegra CSI SoC structure
+ *
+ * @ops: csi hardware operations
+ * @csi_max_channels: supported max streaming channels
+ * @clk_names: csi and cil clock names
+ * @num_clks: total clocks count
+ * @tpg_frmrate_table: csi tpg frame rate table with blanking intervals
+ * @tpg_frmrate_table_size: size of frame rate table
+ */
+struct tegra_csi_soc {
+	const struct tegra_csi_ops *ops;
+	unsigned int csi_max_channels;
+	const char * const *clk_names;
+	unsigned int num_clks;
+	const struct tpg_framerate *tpg_frmrate_table;
+	unsigned int tpg_frmrate_table_size;
+};
+
+/**
+ * struct tegra_csi - NVIDIA Tegra CSI device structure
+ *
+ * @dev: device struct
+ * @client: host1x_client struct
+ * @iomem: register base
+ * @clks: clock for CSI and CIL
+ * @soc: pointer to SoC data structure
+ * @ops: csi operations
+ * @csi_chans: list head for CSI channels
+ */
+struct tegra_csi {
+	struct device *dev;
+	struct host1x_client client;
+	void __iomem *iomem;
+	struct clk_bulk_data *clks;
+	const struct tegra_csi_soc *soc;
+	const struct tegra_csi_ops *ops;
+	struct list_head csi_chans;
+};
+
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+extern const struct tegra_csi_soc tegra210_csi_soc;
+#endif
+
+void tegra_csi_error_recover(struct v4l2_subdev *subdev);
+void tegra_csi_calc_settle_time(struct tegra_csi_channel *csi_chan,
+				u8 csi_port_num,
+				u8 *clk_settle_time,
+				u8 *ths_settle_time);
+#endif
diff --git a/drivers/staging/media/tegra-video/tegra210.c b/drivers/staging/media/tegra-video/tegra210.c
new file mode 100644
index 000000000..d58370a84
--- /dev/null
+++ b/drivers/staging/media/tegra-video/tegra210.c
@@ -0,0 +1,1131 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+/*
+ * This source file contains Tegra210 supported video formats,
+ * VI and CSI SoC specific data, operations and registers accessors.
+ */
+#include <linux/bitfield.h>
+#include <linux/clk.h>
+#include <linux/clk/tegra.h>
+#include <linux/delay.h>
+#include <linux/host1x.h>
+#include <linux/kthread.h>
+
+#include "csi.h"
+#include "vi.h"
+
+#define TEGRA_VI_SYNCPT_WAIT_TIMEOUT			msecs_to_jiffies(200)
+
+/* Tegra210 VI registers */
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT			0x000
+#define   VI_CFG_VI_INCR_SYNCPT_COND(x)			(((x) & 0xff) << 8)
+#define   VI_CSI_PP_FRAME_START(port)			(5 + (port) * 4)
+#define   VI_CSI_MW_ACK_DONE(port)			(7 + (port) * 4)
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL		0x004
+#define   VI_INCR_SYNCPT_NO_STALL			BIT(8)
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR		0x008
+#define TEGRA_VI_CFG_CG_CTRL				0x0b8
+#define   VI_CG_2ND_LEVEL_EN				0x1
+
+/* Tegra210 VI CSI registers */
+#define TEGRA_VI_CSI_SW_RESET				0x000
+#define TEGRA_VI_CSI_SINGLE_SHOT			0x004
+#define   SINGLE_SHOT_CAPTURE				0x1
+#define TEGRA_VI_CSI_IMAGE_DEF				0x00c
+#define   BYPASS_PXL_TRANSFORM_OFFSET			24
+#define   IMAGE_DEF_FORMAT_OFFSET			16
+#define   IMAGE_DEF_DEST_MEM				0x1
+#define TEGRA_VI_CSI_IMAGE_SIZE				0x018
+#define   IMAGE_SIZE_HEIGHT_OFFSET			16
+#define TEGRA_VI_CSI_IMAGE_SIZE_WC			0x01c
+#define TEGRA_VI_CSI_IMAGE_DT				0x020
+#define TEGRA_VI_CSI_SURFACE0_OFFSET_MSB		0x024
+#define TEGRA_VI_CSI_SURFACE0_OFFSET_LSB		0x028
+#define TEGRA_VI_CSI_SURFACE1_OFFSET_MSB		0x02c
+#define TEGRA_VI_CSI_SURFACE1_OFFSET_LSB		0x030
+#define TEGRA_VI_CSI_SURFACE2_OFFSET_MSB		0x034
+#define TEGRA_VI_CSI_SURFACE2_OFFSET_LSB		0x038
+#define TEGRA_VI_CSI_SURFACE0_STRIDE			0x054
+#define TEGRA_VI_CSI_SURFACE1_STRIDE			0x058
+#define TEGRA_VI_CSI_SURFACE2_STRIDE			0x05c
+#define TEGRA_VI_CSI_SURFACE_HEIGHT0			0x060
+#define TEGRA_VI_CSI_ERROR_STATUS			0x084
+
+/* Tegra210 CSI Pixel Parser registers: Starts from 0x838, offset 0x0 */
+#define TEGRA_CSI_INPUT_STREAM_CONTROL                  0x000
+#define   CSI_SKIP_PACKET_THRESHOLD_OFFSET		16
+#define TEGRA_CSI_PIXEL_STREAM_CONTROL0			0x004
+#define   CSI_PP_PACKET_HEADER_SENT			BIT(4)
+#define   CSI_PP_DATA_IDENTIFIER_ENABLE			BIT(5)
+#define   CSI_PP_WORD_COUNT_SELECT_HEADER		BIT(6)
+#define   CSI_PP_CRC_CHECK_ENABLE			BIT(7)
+#define   CSI_PP_WC_CHECK				BIT(8)
+#define   CSI_PP_OUTPUT_FORMAT_STORE			(0x3 << 16)
+#define   CSI_PPA_PAD_LINE_NOPAD			(0x2 << 24)
+#define   CSI_PP_HEADER_EC_DISABLE			(0x1 << 27)
+#define   CSI_PPA_PAD_FRAME_NOPAD			(0x2 << 28)
+#define TEGRA_CSI_PIXEL_STREAM_CONTROL1                 0x008
+#define   CSI_PP_TOP_FIELD_FRAME_OFFSET			0
+#define   CSI_PP_TOP_FIELD_FRAME_MASK_OFFSET		4
+#define TEGRA_CSI_PIXEL_STREAM_GAP                      0x00c
+#define   PP_FRAME_MIN_GAP_OFFSET			16
+#define TEGRA_CSI_PIXEL_STREAM_PP_COMMAND               0x010
+#define   CSI_PP_ENABLE					0x1
+#define   CSI_PP_DISABLE				0x2
+#define   CSI_PP_RST					0x3
+#define   CSI_PP_SINGLE_SHOT_ENABLE			(0x1 << 2)
+#define   CSI_PP_START_MARKER_FRAME_MAX_OFFSET		12
+#define TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME           0x014
+#define TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK           0x018
+#define TEGRA_CSI_PIXEL_PARSER_STATUS                   0x01c
+
+/* Tegra210 CSI PHY registers */
+/* CSI_PHY_CIL_COMMAND_0 offset 0x0d0 from TEGRA_CSI_PIXEL_PARSER_0_BASE */
+#define TEGRA_CSI_PHY_CIL_COMMAND                       0x0d0
+#define   CSI_A_PHY_CIL_NOP				0x0
+#define   CSI_A_PHY_CIL_ENABLE				0x1
+#define   CSI_A_PHY_CIL_DISABLE				0x2
+#define   CSI_A_PHY_CIL_ENABLE_MASK			0x3
+#define   CSI_B_PHY_CIL_NOP				(0x0 << 8)
+#define   CSI_B_PHY_CIL_ENABLE				(0x1 << 8)
+#define   CSI_B_PHY_CIL_DISABLE				(0x2 << 8)
+#define   CSI_B_PHY_CIL_ENABLE_MASK			(0x3 << 8)
+
+#define TEGRA_CSI_CIL_PAD_CONFIG0                       0x000
+#define   BRICK_CLOCK_A_4X				(0x1 << 16)
+#define   BRICK_CLOCK_B_4X				(0x2 << 16)
+#define TEGRA_CSI_CIL_PAD_CONFIG1                       0x004
+#define TEGRA_CSI_CIL_PHY_CONTROL                       0x008
+#define   CLK_SETTLE_MASK				GENMASK(13, 8)
+#define   THS_SETTLE_MASK				GENMASK(5, 0)
+#define TEGRA_CSI_CIL_INTERRUPT_MASK                    0x00c
+#define TEGRA_CSI_CIL_STATUS                            0x010
+#define TEGRA_CSI_CILX_STATUS                           0x014
+#define TEGRA_CSI_CIL_SW_SENSOR_RESET                   0x020
+
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL		0x000
+#define   PG_MODE_OFFSET				2
+#define   PG_ENABLE					0x1
+#define   PG_DISABLE					0x0
+#define TEGRA_CSI_PG_BLANK				0x004
+#define   PG_VBLANK_OFFSET				16
+#define TEGRA_CSI_PG_PHASE				0x008
+#define TEGRA_CSI_PG_RED_FREQ				0x00c
+#define   PG_RED_VERT_INIT_FREQ_OFFSET			16
+#define   PG_RED_HOR_INIT_FREQ_OFFSET			0
+#define TEGRA_CSI_PG_RED_FREQ_RATE			0x010
+#define TEGRA_CSI_PG_GREEN_FREQ				0x014
+#define   PG_GREEN_VERT_INIT_FREQ_OFFSET		16
+#define   PG_GREEN_HOR_INIT_FREQ_OFFSET			0
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE			0x018
+#define TEGRA_CSI_PG_BLUE_FREQ				0x01c
+#define   PG_BLUE_VERT_INIT_FREQ_OFFSET			16
+#define   PG_BLUE_HOR_INIT_FREQ_OFFSET			0
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE			0x020
+#define TEGRA_CSI_PG_AOHDR				0x024
+#define TEGRA_CSI_CSI_SW_STATUS_RESET			0x214
+#define TEGRA_CSI_CLKEN_OVERRIDE			0x218
+
+#define TEGRA210_CSI_PORT_OFFSET			0x34
+#define TEGRA210_CSI_CIL_OFFSET				0x0f4
+#define TEGRA210_CSI_TPG_OFFSET				0x18c
+
+#define CSI_PP_OFFSET(block)				((block) * 0x800)
+#define TEGRA210_VI_CSI_BASE(x)				(0x100 + (x) * 0x100)
+
+/* Tegra210 VI registers accessors */
+static void tegra_vi_write(struct tegra_vi_channel *chan, unsigned int addr,
+			   u32 val)
+{
+	writel_relaxed(val, chan->vi->iomem + addr);
+}
+
+static u32 tegra_vi_read(struct tegra_vi_channel *chan, unsigned int addr)
+{
+	return readl_relaxed(chan->vi->iomem + addr);
+}
+
+/* Tegra210 VI_CSI registers accessors */
+static void vi_csi_write(struct tegra_vi_channel *chan, u8 portno,
+			 unsigned int addr, u32 val)
+{
+	void __iomem *vi_csi_base;
+
+	vi_csi_base = chan->vi->iomem + TEGRA210_VI_CSI_BASE(portno);
+
+	writel_relaxed(val, vi_csi_base + addr);
+}
+
+static u32 vi_csi_read(struct tegra_vi_channel *chan, u8 portno,
+		       unsigned int addr)
+{
+	void __iomem *vi_csi_base;
+
+	vi_csi_base = chan->vi->iomem + TEGRA210_VI_CSI_BASE(portno);
+
+	return readl_relaxed(vi_csi_base + addr);
+}
+
+/*
+ * Tegra210 VI channel capture operations
+ */
+static int tegra_channel_capture_setup(struct tegra_vi_channel *chan,
+				       u8 portno)
+{
+	u32 height = chan->format.height;
+	u32 width = chan->format.width;
+	u32 format = chan->fmtinfo->img_fmt;
+	u32 data_type = chan->fmtinfo->img_dt;
+	u32 word_count = (width * chan->fmtinfo->bit_width) / 8;
+	u32 bypass_pixel_transform = BIT(BYPASS_PXL_TRANSFORM_OFFSET);
+
+	/*
+	 * VI Pixel transformation unit converts source pixels data format
+	 * into selected destination pixel format and aligns properly while
+	 * interfacing with memory packer.
+	 * This pixel transformation should be enabled for YUV and RGB
+	 * formats and should be bypassed for RAW formats as RAW formats
+	 * only support direct to memory.
+	 */
+	if (chan->pg_mode || data_type == TEGRA_IMAGE_DT_YUV422_8 ||
+	    data_type == TEGRA_IMAGE_DT_RGB888)
+		bypass_pixel_transform = 0;
+
+	/*
+	 * For x8 source streaming, the source image is split onto two x4 ports
+	 * with left half to first x4 port and right half to second x4 port.
+	 * So, use split width and corresponding word count for each x4 port.
+	 */
+	if (chan->numgangports > 1) {
+		width = width >> 1;
+		word_count = (width * chan->fmtinfo->bit_width) / 8;
+	}
+
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_ERROR_STATUS, 0xffffffff);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_IMAGE_DEF,
+		     bypass_pixel_transform |
+		     (format << IMAGE_DEF_FORMAT_OFFSET) |
+		     IMAGE_DEF_DEST_MEM);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_IMAGE_DT, data_type);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_IMAGE_SIZE_WC, word_count);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_IMAGE_SIZE,
+		     (height << IMAGE_SIZE_HEIGHT_OFFSET) | width);
+	return 0;
+}
+
+static void tegra_channel_vi_soft_reset(struct tegra_vi_channel *chan,
+					u8 portno)
+{
+	/* disable clock gating to enable continuous clock */
+	tegra_vi_write(chan, TEGRA_VI_CFG_CG_CTRL, 0);
+	/*
+	 * Soft reset memory client interface, pixel format logic, sensor
+	 * control logic, and a shadow copy logic to bring VI to clean state.
+	 */
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SW_RESET, 0xf);
+	usleep_range(100, 200);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SW_RESET, 0x0);
+
+	/* enable back VI clock gating */
+	tegra_vi_write(chan, TEGRA_VI_CFG_CG_CTRL, VI_CG_2ND_LEVEL_EN);
+}
+
+static void tegra_channel_capture_error_recover(struct tegra_vi_channel *chan,
+						u8 portno)
+{
+	struct v4l2_subdev *subdev;
+	u32 val;
+
+	/*
+	 * Recover VI and CSI hardware blocks in case of missing frame start
+	 * events due to source not streaming or noisy csi inputs from the
+	 * external source or many outstanding frame start or MW_ACK_DONE
+	 * events which can cause CSI and VI hardware hang.
+	 * This helps to have a clean capture for next frame.
+	 */
+	val = vi_csi_read(chan, portno, TEGRA_VI_CSI_ERROR_STATUS);
+	dev_dbg(&chan->video.dev, "TEGRA_VI_CSI_ERROR_STATUS 0x%08x\n", val);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_ERROR_STATUS, val);
+
+	val = tegra_vi_read(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR);
+	dev_dbg(&chan->video.dev,
+		"TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR 0x%08x\n", val);
+	tegra_vi_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR, val);
+
+	/* recover VI by issuing software reset and re-setup for capture */
+	tegra_channel_vi_soft_reset(chan, portno);
+	tegra_channel_capture_setup(chan, portno);
+
+	/* recover CSI block */
+	subdev = tegra_channel_get_remote_csi_subdev(chan);
+	tegra_csi_error_recover(subdev);
+}
+
+static struct tegra_channel_buffer *
+dequeue_buf_done(struct tegra_vi_channel *chan)
+{
+	struct tegra_channel_buffer *buf = NULL;
+
+	spin_lock(&chan->done_lock);
+	if (list_empty(&chan->done)) {
+		spin_unlock(&chan->done_lock);
+		return NULL;
+	}
+
+	buf = list_first_entry(&chan->done,
+			       struct tegra_channel_buffer, queue);
+	if (buf)
+		list_del_init(&buf->queue);
+	spin_unlock(&chan->done_lock);
+
+	return buf;
+}
+
+static void release_buffer(struct tegra_vi_channel *chan,
+			   struct tegra_channel_buffer *buf,
+			   enum vb2_buffer_state state)
+{
+	struct vb2_v4l2_buffer *vb = &buf->buf;
+
+	vb->sequence = chan->sequence++;
+	vb->field = V4L2_FIELD_NONE;
+	vb->vb2_buf.timestamp = ktime_get_ns();
+	vb2_buffer_done(&vb->vb2_buf, state);
+}
+
+static void tegra_channel_vi_buffer_setup(struct tegra_vi_channel *chan,
+					  u8 portno, u32 buf_offset,
+					  struct tegra_channel_buffer *buf)
+{
+	int bytesperline = chan->format.bytesperline;
+	u32 sizeimage = chan->format.sizeimage;
+
+	/* program buffer address by using surface 0 */
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE0_OFFSET_MSB,
+		     ((u64)buf->addr + buf_offset) >> 32);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE0_OFFSET_LSB,
+		     buf->addr + buf_offset);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE0_STRIDE, bytesperline);
+
+	if (chan->fmtinfo->fourcc != V4L2_PIX_FMT_NV16)
+		return;
+	/*
+	 * Program surface 1 for UV plane with offset sizeimage from Y plane.
+	 */
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE1_OFFSET_MSB,
+		     (((u64)buf->addr + sizeimage / 2) + buf_offset) >> 32);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE1_OFFSET_LSB,
+		     buf->addr + sizeimage / 2 + buf_offset);
+	vi_csi_write(chan, portno, TEGRA_VI_CSI_SURFACE1_STRIDE, bytesperline);
+}
+
+static int tegra_channel_capture_frame(struct tegra_vi_channel *chan,
+				       struct tegra_channel_buffer *buf)
+{
+	u32 thresh, value, frame_start, mw_ack_done;
+	u32 fs_thresh[GANG_PORTS_MAX];
+	u8 *portnos = chan->portnos;
+	int gang_bpl = (chan->format.width >> 1) * chan->fmtinfo->bpp;
+	u32 buf_offset;
+	bool capture_timedout = false;
+	int err, i;
+
+	for (i = 0; i < chan->numgangports; i++) {
+		/*
+		 * Align buffers side-by-side for all consecutive x4 ports
+		 * in gang ports using bytes per line based on source split
+		 * width.
+		 */
+		buf_offset = i * roundup(gang_bpl, SURFACE_ALIGN_BYTES);
+		tegra_channel_vi_buffer_setup(chan, portnos[i], buf_offset,
+					      buf);
+
+		/*
+		 * Tegra VI block interacts with host1x syncpt to synchronize
+		 * programmed condition and hardware operation for capture.
+		 * Frame start and Memory write acknowledge syncpts has their
+		 * own FIFO of depth 2.
+		 *
+		 * Syncpoint trigger conditions set through VI_INCR_SYNCPT
+		 * register are added to HW syncpt FIFO and when HW triggers,
+		 * syncpt condition is removed from the FIFO and counter at
+		 * syncpoint index will be incremented by the hardware and
+		 * software can wait for counter to reach threshold to
+		 * synchronize capturing frame with hardware capture events.
+		 */
+
+		/* increase channel syncpoint threshold for FRAME_START */
+		thresh = host1x_syncpt_incr_max(chan->frame_start_sp[i], 1);
+		fs_thresh[i] = thresh;
+
+		/* Program FRAME_START trigger condition syncpt request */
+		frame_start = VI_CSI_PP_FRAME_START(portnos[i]);
+		value = VI_CFG_VI_INCR_SYNCPT_COND(frame_start) |
+			host1x_syncpt_id(chan->frame_start_sp[i]);
+		tegra_vi_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT, value);
+
+		/* increase channel syncpoint threshold for MW_ACK_DONE */
+		thresh = host1x_syncpt_incr_max(chan->mw_ack_sp[i], 1);
+		buf->mw_ack_sp_thresh[i] = thresh;
+
+		/* Program MW_ACK_DONE trigger condition syncpt request */
+		mw_ack_done = VI_CSI_MW_ACK_DONE(portnos[i]);
+		value = VI_CFG_VI_INCR_SYNCPT_COND(mw_ack_done) |
+			host1x_syncpt_id(chan->mw_ack_sp[i]);
+		tegra_vi_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT, value);
+	}
+
+	/* enable single shot capture after all ganged ports are ready */
+	for (i = 0; i < chan->numgangports; i++)
+		vi_csi_write(chan, portnos[i], TEGRA_VI_CSI_SINGLE_SHOT,
+			     SINGLE_SHOT_CAPTURE);
+
+	for (i = 0; i < chan->numgangports; i++) {
+		/*
+		 * Wait for syncpt counter to reach frame start event threshold
+		 */
+		err = host1x_syncpt_wait(chan->frame_start_sp[i], fs_thresh[i],
+					 TEGRA_VI_SYNCPT_WAIT_TIMEOUT, &value);
+		if (err) {
+			capture_timedout = true;
+			/* increment syncpoint counter for timedout events */
+			host1x_syncpt_incr(chan->frame_start_sp[i]);
+			spin_lock(&chan->sp_incr_lock[i]);
+			host1x_syncpt_incr(chan->mw_ack_sp[i]);
+			spin_unlock(&chan->sp_incr_lock[i]);
+			/* clear errors and recover */
+			tegra_channel_capture_error_recover(chan, portnos[i]);
+		}
+	}
+
+	if (capture_timedout) {
+		dev_err_ratelimited(&chan->video.dev,
+				    "frame start syncpt timeout: %d\n", err);
+		release_buffer(chan, buf, VB2_BUF_STATE_ERROR);
+		return err;
+	}
+
+	/* move buffer to capture done queue */
+	spin_lock(&chan->done_lock);
+	list_add_tail(&buf->queue, &chan->done);
+	spin_unlock(&chan->done_lock);
+
+	/* wait up kthread for capture done */
+	wake_up_interruptible(&chan->done_wait);
+
+	return 0;
+}
+
+static void tegra_channel_capture_done(struct tegra_vi_channel *chan,
+				       struct tegra_channel_buffer *buf)
+{
+	enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
+	u32 value;
+	bool capture_timedout = false;
+	int ret, i;
+
+	for (i = 0; i < chan->numgangports; i++) {
+		/*
+		 * Wait for syncpt counter to reach MW_ACK_DONE event threshold
+		 */
+		ret = host1x_syncpt_wait(chan->mw_ack_sp[i],
+					 buf->mw_ack_sp_thresh[i],
+					 TEGRA_VI_SYNCPT_WAIT_TIMEOUT, &value);
+		if (ret) {
+			capture_timedout = true;
+			state = VB2_BUF_STATE_ERROR;
+			/* increment syncpoint counter for timedout event */
+			spin_lock(&chan->sp_incr_lock[i]);
+			host1x_syncpt_incr(chan->mw_ack_sp[i]);
+			spin_unlock(&chan->sp_incr_lock[i]);
+		}
+	}
+
+	if (capture_timedout)
+		dev_err_ratelimited(&chan->video.dev,
+				    "MW_ACK_DONE syncpt timeout: %d\n", ret);
+	release_buffer(chan, buf, state);
+}
+
+static int chan_capture_kthread_start(void *data)
+{
+	struct tegra_vi_channel *chan = data;
+	struct tegra_channel_buffer *buf;
+	unsigned int retries = 0;
+	int err = 0;
+
+	while (1) {
+		/*
+		 * Source is not streaming if error is non-zero.
+		 * So, do not dequeue buffers on error and let the thread sleep
+		 * till kthread stop signal is received.
+		 */
+		wait_event_interruptible(chan->start_wait,
+					 kthread_should_stop() ||
+					 (!list_empty(&chan->capture) &&
+					 !err));
+
+		if (kthread_should_stop())
+			break;
+
+		/* dequeue the buffer and start capture */
+		spin_lock(&chan->start_lock);
+		if (list_empty(&chan->capture)) {
+			spin_unlock(&chan->start_lock);
+			continue;
+		}
+
+		buf = list_first_entry(&chan->capture,
+				       struct tegra_channel_buffer, queue);
+		list_del_init(&buf->queue);
+		spin_unlock(&chan->start_lock);
+
+		err = tegra_channel_capture_frame(chan, buf);
+		if (!err) {
+			retries = 0;
+			continue;
+		}
+
+		if (retries++ > chan->syncpt_timeout_retry)
+			vb2_queue_error(&chan->queue);
+		else
+			err = 0;
+	}
+
+	return 0;
+}
+
+static int chan_capture_kthread_finish(void *data)
+{
+	struct tegra_vi_channel *chan = data;
+	struct tegra_channel_buffer *buf;
+
+	while (1) {
+		wait_event_interruptible(chan->done_wait,
+					 !list_empty(&chan->done) ||
+					 kthread_should_stop());
+
+		/* dequeue buffers and finish capture */
+		buf = dequeue_buf_done(chan);
+		while (buf) {
+			tegra_channel_capture_done(chan, buf);
+			buf = dequeue_buf_done(chan);
+		}
+
+		if (kthread_should_stop())
+			break;
+	}
+
+	return 0;
+}
+
+static int tegra210_vi_start_streaming(struct vb2_queue *vq, u32 count)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
+	struct media_pipeline *pipe = &chan->video.pipe;
+	u32 val;
+	u8 *portnos = chan->portnos;
+	int ret, i;
+
+	tegra_vi_write(chan, TEGRA_VI_CFG_CG_CTRL, VI_CG_2ND_LEVEL_EN);
+
+	/* clear syncpt errors */
+	val = tegra_vi_read(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR);
+	tegra_vi_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR, val);
+
+	/*
+	 * Sync point FIFO full stalls the host interface.
+	 * Setting NO_STALL will drop INCR_SYNCPT methods when fifos are
+	 * full and the corresponding condition bits in INCR_SYNCPT_ERROR
+	 * register will be set.
+	 * This allows SW to process error recovery.
+	 */
+	tegra_vi_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL,
+		       VI_INCR_SYNCPT_NO_STALL);
+
+	/* start the pipeline */
+	ret = video_device_pipeline_start(&chan->video, pipe);
+	if (ret < 0)
+		goto error_pipeline_start;
+
+	/* clear csi errors and do capture setup for all ports in gang mode */
+	for (i = 0; i < chan->numgangports; i++) {
+		val = vi_csi_read(chan, portnos[i], TEGRA_VI_CSI_ERROR_STATUS);
+		vi_csi_write(chan, portnos[i], TEGRA_VI_CSI_ERROR_STATUS, val);
+
+		tegra_channel_capture_setup(chan, portnos[i]);
+	}
+
+	ret = tegra_channel_set_stream(chan, true);
+	if (ret < 0)
+		goto error_set_stream;
+
+	chan->sequence = 0;
+
+	/* start kthreads to capture data to buffer and return them */
+	chan->kthread_start_capture = kthread_run(chan_capture_kthread_start,
+						  chan, "%s:0",
+						  chan->video.name);
+	if (IS_ERR(chan->kthread_start_capture)) {
+		ret = PTR_ERR(chan->kthread_start_capture);
+		chan->kthread_start_capture = NULL;
+		dev_err(&chan->video.dev,
+			"failed to run capture start kthread: %d\n", ret);
+		goto error_kthread_start;
+	}
+
+	chan->kthread_finish_capture = kthread_run(chan_capture_kthread_finish,
+						   chan, "%s:1",
+						   chan->video.name);
+	if (IS_ERR(chan->kthread_finish_capture)) {
+		ret = PTR_ERR(chan->kthread_finish_capture);
+		chan->kthread_finish_capture = NULL;
+		dev_err(&chan->video.dev,
+			"failed to run capture finish kthread: %d\n", ret);
+		goto error_kthread_done;
+	}
+
+	return 0;
+
+error_kthread_done:
+	kthread_stop(chan->kthread_start_capture);
+error_kthread_start:
+	tegra_channel_set_stream(chan, false);
+error_set_stream:
+	video_device_pipeline_stop(&chan->video);
+error_pipeline_start:
+	tegra_channel_release_buffers(chan, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void tegra210_vi_stop_streaming(struct vb2_queue *vq)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
+
+	if (chan->kthread_start_capture) {
+		kthread_stop(chan->kthread_start_capture);
+		chan->kthread_start_capture = NULL;
+	}
+
+	if (chan->kthread_finish_capture) {
+		kthread_stop(chan->kthread_finish_capture);
+		chan->kthread_finish_capture = NULL;
+	}
+
+	tegra_channel_release_buffers(chan, VB2_BUF_STATE_ERROR);
+	tegra_channel_set_stream(chan, false);
+	video_device_pipeline_stop(&chan->video);
+}
+
+/*
+ * Tegra210 VI Pixel memory format enum.
+ * These format enum value gets programmed into corresponding Tegra VI
+ * channel register bits.
+ */
+enum tegra210_image_format {
+	TEGRA210_IMAGE_FORMAT_T_L8 = 16,
+
+	TEGRA210_IMAGE_FORMAT_T_R16_I = 32,
+	TEGRA210_IMAGE_FORMAT_T_B5G6R5,
+	TEGRA210_IMAGE_FORMAT_T_R5G6B5,
+	TEGRA210_IMAGE_FORMAT_T_A1B5G5R5,
+	TEGRA210_IMAGE_FORMAT_T_A1R5G5B5,
+	TEGRA210_IMAGE_FORMAT_T_B5G5R5A1,
+	TEGRA210_IMAGE_FORMAT_T_R5G5B5A1,
+	TEGRA210_IMAGE_FORMAT_T_A4B4G4R4,
+	TEGRA210_IMAGE_FORMAT_T_A4R4G4B4,
+	TEGRA210_IMAGE_FORMAT_T_B4G4R4A4,
+	TEGRA210_IMAGE_FORMAT_T_R4G4B4A4,
+
+	TEGRA210_IMAGE_FORMAT_T_A8B8G8R8 = 64,
+	TEGRA210_IMAGE_FORMAT_T_A8R8G8B8,
+	TEGRA210_IMAGE_FORMAT_T_B8G8R8A8,
+	TEGRA210_IMAGE_FORMAT_T_R8G8B8A8,
+	TEGRA210_IMAGE_FORMAT_T_A2B10G10R10,
+	TEGRA210_IMAGE_FORMAT_T_A2R10G10B10,
+	TEGRA210_IMAGE_FORMAT_T_B10G10R10A2,
+	TEGRA210_IMAGE_FORMAT_T_R10G10B10A2,
+
+	TEGRA210_IMAGE_FORMAT_T_A8Y8U8V8 = 193,
+	TEGRA210_IMAGE_FORMAT_T_V8U8Y8A8,
+
+	TEGRA210_IMAGE_FORMAT_T_A2Y10U10V10 = 197,
+	TEGRA210_IMAGE_FORMAT_T_V10U10Y10A2,
+	TEGRA210_IMAGE_FORMAT_T_Y8_U8__Y8_V8,
+	TEGRA210_IMAGE_FORMAT_T_Y8_V8__Y8_U8,
+	TEGRA210_IMAGE_FORMAT_T_U8_Y8__V8_Y8,
+	TEGRA210_IMAGE_FORMAT_T_V8_Y8__U8_Y8,
+
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8__V8_N444 = 224,
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8V8_N444,
+	TEGRA210_IMAGE_FORMAT_T_Y8__V8U8_N444,
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8__V8_N422,
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8V8_N422,
+	TEGRA210_IMAGE_FORMAT_T_Y8__V8U8_N422,
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8__V8_N420,
+	TEGRA210_IMAGE_FORMAT_T_Y8__U8V8_N420,
+	TEGRA210_IMAGE_FORMAT_T_Y8__V8U8_N420,
+	TEGRA210_IMAGE_FORMAT_T_X2LC10LB10LA10,
+	TEGRA210_IMAGE_FORMAT_T_A2R6R6R6R6R6,
+};
+
+#define TEGRA210_VIDEO_FMT(DATA_TYPE, BIT_WIDTH, MBUS_CODE, BPP,	\
+			   FORMAT, FOURCC)				\
+{									\
+	TEGRA_IMAGE_DT_##DATA_TYPE,					\
+	BIT_WIDTH,							\
+	MEDIA_BUS_FMT_##MBUS_CODE,					\
+	BPP,								\
+	TEGRA210_IMAGE_FORMAT_##FORMAT,					\
+	V4L2_PIX_FMT_##FOURCC,						\
+}
+
+/* Tegra210 supported video formats */
+static const struct tegra_video_format tegra210_video_formats[] = {
+	/* RAW 8 */
+	TEGRA210_VIDEO_FMT(RAW8, 8, SRGGB8_1X8, 1, T_L8, SRGGB8),
+	TEGRA210_VIDEO_FMT(RAW8, 8, SGRBG8_1X8, 1, T_L8, SGRBG8),
+	TEGRA210_VIDEO_FMT(RAW8, 8, SGBRG8_1X8, 1, T_L8, SGBRG8),
+	TEGRA210_VIDEO_FMT(RAW8, 8, SBGGR8_1X8, 1, T_L8, SBGGR8),
+	/* RAW 10 */
+	TEGRA210_VIDEO_FMT(RAW10, 10, SRGGB10_1X10, 2, T_R16_I, SRGGB10),
+	TEGRA210_VIDEO_FMT(RAW10, 10, SGRBG10_1X10, 2, T_R16_I, SGRBG10),
+	TEGRA210_VIDEO_FMT(RAW10, 10, SGBRG10_1X10, 2, T_R16_I, SGBRG10),
+	TEGRA210_VIDEO_FMT(RAW10, 10, SBGGR10_1X10, 2, T_R16_I, SBGGR10),
+	/* RAW 12 */
+	TEGRA210_VIDEO_FMT(RAW12, 12, SRGGB12_1X12, 2, T_R16_I, SRGGB12),
+	TEGRA210_VIDEO_FMT(RAW12, 12, SGRBG12_1X12, 2, T_R16_I, SGRBG12),
+	TEGRA210_VIDEO_FMT(RAW12, 12, SGBRG12_1X12, 2, T_R16_I, SGBRG12),
+	TEGRA210_VIDEO_FMT(RAW12, 12, SBGGR12_1X12, 2, T_R16_I, SBGGR12),
+	/* RGB888 */
+	TEGRA210_VIDEO_FMT(RGB888, 24, RGB888_1X24, 4, T_A8R8G8B8, XBGR32),
+	TEGRA210_VIDEO_FMT(RGB888, 24, RGB888_1X32_PADHI, 4, T_A8B8G8R8,
+			   RGBX32),
+	/* YUV422 */
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, UYVY8_1X16, 2, T_U8_Y8__V8_Y8, YVYU),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, VYUY8_1X16, 2, T_V8_Y8__U8_Y8, YUYV),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, YUYV8_1X16, 2, T_Y8_U8__Y8_V8, VYUY),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, YVYU8_1X16, 2, T_Y8_V8__Y8_U8, UYVY),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, UYVY8_1X16, 1, T_Y8__V8U8_N422, NV16),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, UYVY8_2X8, 2, T_U8_Y8__V8_Y8, YVYU),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, VYUY8_2X8, 2, T_V8_Y8__U8_Y8, YUYV),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, YUYV8_2X8, 2, T_Y8_U8__Y8_V8, VYUY),
+	TEGRA210_VIDEO_FMT(YUV422_8, 16, YVYU8_2X8, 2, T_Y8_V8__Y8_U8, UYVY),
+};
+
+/* Tegra210 VI operations */
+static const struct tegra_vi_ops tegra210_vi_ops = {
+	.vi_start_streaming = tegra210_vi_start_streaming,
+	.vi_stop_streaming = tegra210_vi_stop_streaming,
+};
+
+/* Tegra210 VI SoC data */
+const struct tegra_vi_soc tegra210_vi_soc = {
+	.video_formats = tegra210_video_formats,
+	.nformats = ARRAY_SIZE(tegra210_video_formats),
+	.ops = &tegra210_vi_ops,
+	.hw_revision = 3,
+	.vi_max_channels = 6,
+#if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
+	.vi_max_clk_hz = 499200000,
+#else
+	.vi_max_clk_hz = 998400000,
+#endif
+};
+
+/* Tegra210 CSI PHY registers accessors */
+static void csi_write(struct tegra_csi *csi, u8 portno, unsigned int addr,
+		      u32 val)
+{
+	void __iomem *csi_pp_base;
+
+	csi_pp_base = csi->iomem + CSI_PP_OFFSET(portno >> 1);
+
+	writel_relaxed(val, csi_pp_base + addr);
+}
+
+/* Tegra210 CSI Pixel parser registers accessors */
+static void pp_write(struct tegra_csi *csi, u8 portno, u32 addr, u32 val)
+{
+	void __iomem *csi_pp_base;
+	unsigned int offset;
+
+	csi_pp_base = csi->iomem + CSI_PP_OFFSET(portno >> 1);
+	offset = (portno % CSI_PORTS_PER_BRICK) * TEGRA210_CSI_PORT_OFFSET;
+
+	writel_relaxed(val, csi_pp_base + offset + addr);
+}
+
+static u32 pp_read(struct tegra_csi *csi, u8 portno, u32 addr)
+{
+	void __iomem *csi_pp_base;
+	unsigned int offset;
+
+	csi_pp_base = csi->iomem + CSI_PP_OFFSET(portno >> 1);
+	offset = (portno % CSI_PORTS_PER_BRICK) * TEGRA210_CSI_PORT_OFFSET;
+
+	return readl_relaxed(csi_pp_base + offset + addr);
+}
+
+/* Tegra210 CSI CIL A/B port registers accessors */
+static void cil_write(struct tegra_csi *csi, u8 portno, u32 addr, u32 val)
+{
+	void __iomem *csi_cil_base;
+	unsigned int offset;
+
+	csi_cil_base = csi->iomem + CSI_PP_OFFSET(portno >> 1) +
+		       TEGRA210_CSI_CIL_OFFSET;
+	offset = (portno % CSI_PORTS_PER_BRICK) * TEGRA210_CSI_PORT_OFFSET;
+
+	writel_relaxed(val, csi_cil_base + offset + addr);
+}
+
+static u32 cil_read(struct tegra_csi *csi, u8 portno, u32 addr)
+{
+	void __iomem *csi_cil_base;
+	unsigned int offset;
+
+	csi_cil_base = csi->iomem + CSI_PP_OFFSET(portno >> 1) +
+		       TEGRA210_CSI_CIL_OFFSET;
+	offset = (portno % CSI_PORTS_PER_BRICK) * TEGRA210_CSI_PORT_OFFSET;
+
+	return readl_relaxed(csi_cil_base + offset + addr);
+}
+
+/* Tegra210 CSI Test pattern generator registers accessor */
+static void tpg_write(struct tegra_csi *csi, u8 portno, unsigned int addr,
+		      u32 val)
+{
+	void __iomem *csi_pp_base;
+	unsigned int offset;
+
+	csi_pp_base = csi->iomem + CSI_PP_OFFSET(portno >> 1);
+	offset = (portno % CSI_PORTS_PER_BRICK) * TEGRA210_CSI_PORT_OFFSET +
+		 TEGRA210_CSI_TPG_OFFSET;
+
+	writel_relaxed(val, csi_pp_base + offset + addr);
+}
+
+/*
+ * Tegra210 CSI operations
+ */
+static void tegra210_csi_port_recover(struct tegra_csi_channel *csi_chan,
+				      u8 portno)
+{
+	struct tegra_csi *csi = csi_chan->csi;
+	u32 val;
+
+	/*
+	 * Recover CSI hardware in case of capture errors by issuing
+	 * software reset to CSICIL sensor, pixel parser, and clear errors
+	 * to have clean capture on  next streaming.
+	 */
+	val = pp_read(csi, portno, TEGRA_CSI_PIXEL_PARSER_STATUS);
+	dev_dbg(csi->dev, "TEGRA_CSI_PIXEL_PARSER_STATUS 0x%08x\n", val);
+
+	val = cil_read(csi, portno, TEGRA_CSI_CIL_STATUS);
+	dev_dbg(csi->dev, "TEGRA_CSI_CIL_STATUS 0x%08x\n", val);
+
+	val = cil_read(csi, portno, TEGRA_CSI_CILX_STATUS);
+	dev_dbg(csi->dev, "TEGRA_CSI_CILX_STATUS 0x%08x\n", val);
+
+	if (csi_chan->numlanes == 4) {
+		/* reset CSI CIL sensor */
+		cil_write(csi, portno, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+		/*
+		 * SW_STATUS_RESET resets all status bits of PPA, PPB, CILA,
+		 * CILB status registers and debug counters.
+		 * So, SW_STATUS_RESET can be used only when CSI brick is in
+		 * x4 mode.
+		 */
+		csi_write(csi, portno, TEGRA_CSI_CSI_SW_STATUS_RESET, 0x1);
+
+		/* sleep for 20 clock cycles to drain the FIFO */
+		usleep_range(10, 20);
+
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+		cil_write(csi, portno, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+		csi_write(csi, portno, TEGRA_CSI_CSI_SW_STATUS_RESET, 0x0);
+	} else {
+		/* reset CSICIL sensor */
+		cil_write(csi, portno, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+		usleep_range(10, 20);
+		cil_write(csi, portno, TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+
+		/* clear the errors */
+		pp_write(csi, portno, TEGRA_CSI_PIXEL_PARSER_STATUS,
+			 0xffffffff);
+		cil_write(csi, portno, TEGRA_CSI_CIL_STATUS, 0xffffffff);
+		cil_write(csi, portno, TEGRA_CSI_CILX_STATUS, 0xffffffff);
+	}
+}
+
+static void tegra210_csi_error_recover(struct tegra_csi_channel *csi_chan)
+{
+	u8 *portnos = csi_chan->csi_port_nums;
+	int i;
+
+	for (i = 0; i < csi_chan->numgangports; i++)
+		tegra210_csi_port_recover(csi_chan, portnos[i]);
+}
+
+static int
+tegra210_csi_port_start_streaming(struct tegra_csi_channel *csi_chan,
+				  u8 portno)
+{
+	struct tegra_csi *csi = csi_chan->csi;
+	u8 clk_settle_time = 0;
+	u8 ths_settle_time = 10;
+	u32 val;
+
+	if (!csi_chan->pg_mode)
+		tegra_csi_calc_settle_time(csi_chan, portno, &clk_settle_time,
+					   &ths_settle_time);
+
+	csi_write(csi, portno, TEGRA_CSI_CLKEN_OVERRIDE, 0);
+
+	/* clean up status */
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_PARSER_STATUS, 0xffffffff);
+	cil_write(csi, portno, TEGRA_CSI_CIL_STATUS, 0xffffffff);
+	cil_write(csi, portno, TEGRA_CSI_CILX_STATUS, 0xffffffff);
+	cil_write(csi, portno, TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0);
+
+	/* CIL PHY registers setup */
+	cil_write(csi, portno, TEGRA_CSI_CIL_PAD_CONFIG0, 0x0);
+	cil_write(csi, portno, TEGRA_CSI_CIL_PHY_CONTROL,
+		  FIELD_PREP(CLK_SETTLE_MASK, clk_settle_time) |
+		  FIELD_PREP(THS_SETTLE_MASK, ths_settle_time));
+
+	/*
+	 * The CSI unit provides for connection of up to six cameras in
+	 * the system and is organized as three identical instances of
+	 * two MIPI support blocks, each with a separate 4-lane
+	 * interface that can be configured as a single camera with 4
+	 * lanes or as a dual camera with 2 lanes available for each
+	 * camera.
+	 */
+	if (csi_chan->numlanes == 4) {
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_STATUS, 0xffffffff);
+		cil_write(csi, portno + 1, TEGRA_CSI_CILX_STATUS, 0xffffffff);
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0);
+
+		cil_write(csi, portno, TEGRA_CSI_CIL_PAD_CONFIG0,
+			  BRICK_CLOCK_A_4X);
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_PAD_CONFIG0, 0x0);
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0);
+		cil_write(csi, portno + 1, TEGRA_CSI_CIL_PHY_CONTROL,
+			  FIELD_PREP(CLK_SETTLE_MASK, clk_settle_time) |
+			  FIELD_PREP(THS_SETTLE_MASK, ths_settle_time));
+		csi_write(csi, portno, TEGRA_CSI_PHY_CIL_COMMAND,
+			  CSI_A_PHY_CIL_ENABLE | CSI_B_PHY_CIL_ENABLE);
+	} else {
+		val = ((portno & 1) == PORT_A) ?
+		      CSI_A_PHY_CIL_ENABLE | CSI_B_PHY_CIL_NOP :
+		      CSI_B_PHY_CIL_ENABLE | CSI_A_PHY_CIL_NOP;
+		csi_write(csi, portno, TEGRA_CSI_PHY_CIL_COMMAND, val);
+	}
+
+	/* CSI pixel parser registers setup */
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+		 (0xf << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+		 CSI_PP_SINGLE_SHOT_ENABLE | CSI_PP_RST);
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_CONTROL0,
+		 CSI_PP_PACKET_HEADER_SENT |
+		 CSI_PP_DATA_IDENTIFIER_ENABLE |
+		 CSI_PP_WORD_COUNT_SELECT_HEADER |
+		 CSI_PP_CRC_CHECK_ENABLE |  CSI_PP_WC_CHECK |
+		 CSI_PP_OUTPUT_FORMAT_STORE | CSI_PPA_PAD_LINE_NOPAD |
+		 CSI_PP_HEADER_EC_DISABLE | CSI_PPA_PAD_FRAME_NOPAD |
+		 (portno & 1));
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_CONTROL1,
+		 (0x1 << CSI_PP_TOP_FIELD_FRAME_OFFSET) |
+		 (0x1 << CSI_PP_TOP_FIELD_FRAME_MASK_OFFSET));
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_GAP,
+		 0x14 << PP_FRAME_MIN_GAP_OFFSET);
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME, 0x0);
+	pp_write(csi, portno, TEGRA_CSI_INPUT_STREAM_CONTROL,
+		 (0x3f << CSI_SKIP_PACKET_THRESHOLD_OFFSET) |
+		 (csi_chan->numlanes - 1));
+
+	/* TPG setup */
+	if (csi_chan->pg_mode) {
+		tpg_write(csi, portno, TEGRA_CSI_PATTERN_GENERATOR_CTRL,
+			  ((csi_chan->pg_mode - 1) << PG_MODE_OFFSET) |
+			  PG_ENABLE);
+		tpg_write(csi, portno, TEGRA_CSI_PG_BLANK,
+			  csi_chan->v_blank << PG_VBLANK_OFFSET |
+			  csi_chan->h_blank);
+		tpg_write(csi, portno, TEGRA_CSI_PG_PHASE, 0x0);
+		tpg_write(csi, portno, TEGRA_CSI_PG_RED_FREQ,
+			  (0x10 << PG_RED_VERT_INIT_FREQ_OFFSET) |
+			  (0x10 << PG_RED_HOR_INIT_FREQ_OFFSET));
+		tpg_write(csi, portno, TEGRA_CSI_PG_RED_FREQ_RATE, 0x0);
+		tpg_write(csi, portno, TEGRA_CSI_PG_GREEN_FREQ,
+			  (0x10 << PG_GREEN_VERT_INIT_FREQ_OFFSET) |
+			  (0x10 << PG_GREEN_HOR_INIT_FREQ_OFFSET));
+		tpg_write(csi, portno, TEGRA_CSI_PG_GREEN_FREQ_RATE, 0x0);
+		tpg_write(csi, portno, TEGRA_CSI_PG_BLUE_FREQ,
+			  (0x10 << PG_BLUE_VERT_INIT_FREQ_OFFSET) |
+			  (0x10 << PG_BLUE_HOR_INIT_FREQ_OFFSET));
+		tpg_write(csi, portno, TEGRA_CSI_PG_BLUE_FREQ_RATE, 0x0);
+	}
+
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+		 (0xf << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+		 CSI_PP_SINGLE_SHOT_ENABLE | CSI_PP_ENABLE);
+
+	return 0;
+}
+
+static void
+tegra210_csi_port_stop_streaming(struct tegra_csi_channel *csi_chan, u8 portno)
+{
+	struct tegra_csi *csi = csi_chan->csi;
+	u32 val;
+
+	val = pp_read(csi, portno, TEGRA_CSI_PIXEL_PARSER_STATUS);
+
+	dev_dbg(csi->dev, "TEGRA_CSI_PIXEL_PARSER_STATUS 0x%08x\n", val);
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_PARSER_STATUS, val);
+
+	val = cil_read(csi, portno, TEGRA_CSI_CIL_STATUS);
+	dev_dbg(csi->dev, "TEGRA_CSI_CIL_STATUS 0x%08x\n", val);
+	cil_write(csi, portno, TEGRA_CSI_CIL_STATUS, val);
+
+	val = cil_read(csi, portno, TEGRA_CSI_CILX_STATUS);
+	dev_dbg(csi->dev, "TEGRA_CSI_CILX_STATUS 0x%08x\n", val);
+	cil_write(csi, portno, TEGRA_CSI_CILX_STATUS, val);
+
+	pp_write(csi, portno, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+		 (0xf << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+		 CSI_PP_DISABLE);
+
+	if (csi_chan->pg_mode) {
+		tpg_write(csi, portno, TEGRA_CSI_PATTERN_GENERATOR_CTRL,
+			  PG_DISABLE);
+		return;
+	}
+
+	if (csi_chan->numlanes == 4) {
+		csi_write(csi, portno, TEGRA_CSI_PHY_CIL_COMMAND,
+			  CSI_A_PHY_CIL_DISABLE |
+			  CSI_B_PHY_CIL_DISABLE);
+	} else {
+		val = ((portno & 1) == PORT_A) ?
+		      CSI_A_PHY_CIL_DISABLE | CSI_B_PHY_CIL_NOP :
+		      CSI_B_PHY_CIL_DISABLE | CSI_A_PHY_CIL_NOP;
+		csi_write(csi, portno, TEGRA_CSI_PHY_CIL_COMMAND, val);
+	}
+}
+
+static int tegra210_csi_start_streaming(struct tegra_csi_channel *csi_chan)
+{
+	u8 *portnos = csi_chan->csi_port_nums;
+	int ret, i;
+
+	for (i = 0; i < csi_chan->numgangports; i++) {
+		ret = tegra210_csi_port_start_streaming(csi_chan, portnos[i]);
+		if (ret)
+			goto stream_start_fail;
+	}
+
+	return 0;
+
+stream_start_fail:
+	for (i = i - 1; i >= 0; i--)
+		tegra210_csi_port_stop_streaming(csi_chan, portnos[i]);
+
+	return ret;
+}
+
+static void tegra210_csi_stop_streaming(struct tegra_csi_channel *csi_chan)
+{
+	u8 *portnos = csi_chan->csi_port_nums;
+	int i;
+
+	for (i = 0; i < csi_chan->numgangports; i++)
+		tegra210_csi_port_stop_streaming(csi_chan, portnos[i]);
+}
+
+/*
+ * Tegra210 CSI TPG frame rate table with horizontal and vertical
+ * blanking intervals for corresponding format and resolution.
+ * Blanking intervals are tuned values from design team for max TPG
+ * clock rate.
+ */
+static const struct tpg_framerate tegra210_tpg_frmrate_table[] = {
+	{
+		.frmsize = { 1280, 720 },
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.framerate = 120,
+		.h_blank = 512,
+		.v_blank = 8,
+	},
+	{
+		.frmsize = { 1920, 1080 },
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.framerate = 60,
+		.h_blank = 512,
+		.v_blank = 8,
+	},
+	{
+		.frmsize = { 3840, 2160 },
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.framerate = 20,
+		.h_blank = 8,
+		.v_blank = 8,
+	},
+	{
+		.frmsize = { 1280, 720 },
+		.code = MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+		.framerate = 60,
+		.h_blank = 512,
+		.v_blank = 8,
+	},
+	{
+		.frmsize = { 1920, 1080 },
+		.code = MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+		.framerate = 30,
+		.h_blank = 512,
+		.v_blank = 8,
+	},
+	{
+		.frmsize = { 3840, 2160 },
+		.code = MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+		.framerate = 8,
+		.h_blank = 8,
+		.v_blank = 8,
+	},
+};
+
+static const char * const tegra210_csi_cil_clks[] = {
+	"csi",
+	"cilab",
+	"cilcd",
+	"cile",
+#if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
+	"csi_tpg",
+#endif
+};
+
+/* Tegra210 CSI operations */
+static const struct tegra_csi_ops tegra210_csi_ops = {
+	.csi_start_streaming = tegra210_csi_start_streaming,
+	.csi_stop_streaming = tegra210_csi_stop_streaming,
+	.csi_err_recover = tegra210_csi_error_recover,
+};
+
+/* Tegra210 CSI SoC data */
+const struct tegra_csi_soc tegra210_csi_soc = {
+	.ops = &tegra210_csi_ops,
+	.csi_max_channels = 6,
+	.clk_names = tegra210_csi_cil_clks,
+	.num_clks = ARRAY_SIZE(tegra210_csi_cil_clks),
+	.tpg_frmrate_table = tegra210_tpg_frmrate_table,
+	.tpg_frmrate_table_size = ARRAY_SIZE(tegra210_tpg_frmrate_table),
+};
diff --git a/drivers/staging/media/tegra-video/vi.c b/drivers/staging/media/tegra-video/vi.c
new file mode 100644
index 000000000..9d46a36cc
--- /dev/null
+++ b/drivers/staging/media/tegra-video/vi.c
@@ -0,0 +1,2078 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#include <linux/bitmap.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/host1x.h>
+#include <linux/lcm.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include <soc/tegra/pmc.h>
+
+#include "vi.h"
+#include "video.h"
+
+#define MAX_CID_CONTROLS		1
+
+static const struct tegra_video_format tegra_default_format = {
+	.img_dt = TEGRA_IMAGE_DT_RAW10,
+	.bit_width = 10,
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.bpp = 2,
+	.img_fmt = TEGRA_IMAGE_FORMAT_DEF,
+	.fourcc = V4L2_PIX_FMT_SRGGB10,
+};
+
+static inline struct tegra_vi *
+host1x_client_to_vi(struct host1x_client *client)
+{
+	return container_of(client, struct tegra_vi, client);
+}
+
+static inline struct tegra_channel_buffer *
+to_tegra_channel_buffer(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct tegra_channel_buffer, buf);
+}
+
+static inline struct tegra_vi_graph_entity *
+to_tegra_vi_graph_entity(struct v4l2_async_subdev *asd)
+{
+	return container_of(asd, struct tegra_vi_graph_entity, asd);
+}
+
+static int tegra_get_format_idx_by_code(struct tegra_vi *vi,
+					unsigned int code,
+					unsigned int offset)
+{
+	unsigned int i;
+
+	for (i = offset; i < vi->soc->nformats; ++i) {
+		if (vi->soc->video_formats[i].code == code)
+			return i;
+	}
+
+	return -1;
+}
+
+static u32 tegra_get_format_fourcc_by_idx(struct tegra_vi *vi,
+					  unsigned int index)
+{
+	if (index >= vi->soc->nformats)
+		return -EINVAL;
+
+	return vi->soc->video_formats[index].fourcc;
+}
+
+static const struct tegra_video_format *
+tegra_get_format_by_fourcc(struct tegra_vi *vi, u32 fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vi->soc->nformats; ++i) {
+		if (vi->soc->video_formats[i].fourcc == fourcc)
+			return &vi->soc->video_formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * videobuf2 queue operations
+ */
+static int tegra_channel_queue_setup(struct vb2_queue *vq,
+				     unsigned int *nbuffers,
+				     unsigned int *nplanes,
+				     unsigned int sizes[],
+				     struct device *alloc_devs[])
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
+
+	if (*nplanes)
+		return sizes[0] < chan->format.sizeimage ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = chan->format.sizeimage;
+	alloc_devs[0] = chan->vi->dev;
+
+	return 0;
+}
+
+static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
+	unsigned long size = chan->format.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		v4l2_err(chan->video.v4l2_dev,
+			 "buffer too small (%lu < %lu)\n",
+			 vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+	buf->chan = chan;
+	buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	return 0;
+}
+
+static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
+
+	/* put buffer into the capture queue */
+	spin_lock(&chan->start_lock);
+	list_add_tail(&buf->queue, &chan->capture);
+	spin_unlock(&chan->start_lock);
+
+	/* wait up kthread for capture */
+	wake_up_interruptible(&chan->start_wait);
+}
+
+struct v4l2_subdev *
+tegra_channel_get_remote_csi_subdev(struct tegra_vi_channel *chan)
+{
+	struct media_pad *pad;
+
+	pad = media_pad_remote_pad_first(&chan->pad);
+	if (!pad)
+		return NULL;
+
+	return media_entity_to_v4l2_subdev(pad->entity);
+}
+
+struct v4l2_subdev *
+tegra_channel_get_remote_source_subdev(struct tegra_vi_channel *chan)
+{
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	struct media_entity *entity;
+
+	subdev = tegra_channel_get_remote_csi_subdev(chan);
+	if (!subdev)
+		return NULL;
+
+	pad = &subdev->entity.pads[0];
+	while (!(pad->flags & MEDIA_PAD_FL_SOURCE)) {
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+		entity = pad->entity;
+		pad = &entity->pads[0];
+		subdev = media_entity_to_v4l2_subdev(entity);
+	}
+
+	return subdev;
+}
+
+static int tegra_channel_enable_stream(struct tegra_vi_channel *chan)
+{
+	struct v4l2_subdev *csi_subdev, *src_subdev;
+	struct tegra_csi_channel *csi_chan;
+	int ret, err;
+
+	/*
+	 * Tegra CSI receiver can detect the first LP to HS transition.
+	 * So, start the CSI stream-on prior to sensor stream-on and
+	 * vice-versa for stream-off.
+	 */
+	csi_subdev = tegra_channel_get_remote_csi_subdev(chan);
+	ret = v4l2_subdev_call(csi_subdev, video, s_stream, true);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return 0;
+
+	csi_chan = v4l2_get_subdevdata(csi_subdev);
+	/*
+	 * TRM has incorrectly documented to wait for done status from
+	 * calibration logic after CSI interface power on.
+	 * As per the design, calibration results are latched and applied
+	 * to the pads only when the link is in LP11 state which will happen
+	 * during the sensor stream-on.
+	 * CSI subdev stream-on triggers start of MIPI pads calibration.
+	 * Wait for calibration to finish here after sensor subdev stream-on.
+	 */
+	src_subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(src_subdev, video, s_stream, true);
+	err = tegra_mipi_finish_calibration(csi_chan->mipi);
+
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		goto err_disable_csi_stream;
+
+	if (err < 0)
+		dev_warn(csi_chan->csi->dev,
+			 "MIPI calibration failed: %d\n", err);
+
+	return 0;
+
+err_disable_csi_stream:
+	v4l2_subdev_call(csi_subdev, video, s_stream, false);
+	return ret;
+}
+
+static int tegra_channel_disable_stream(struct tegra_vi_channel *chan)
+{
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	/*
+	 * Stream-off subdevices in reverse order to stream-on.
+	 * Remote source subdev in TPG mode is same as CSI subdev.
+	 */
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(subdev, video, s_stream, false);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		return 0;
+
+	subdev = tegra_channel_get_remote_csi_subdev(chan);
+	ret = v4l2_subdev_call(subdev, video, s_stream, false);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	return 0;
+}
+
+int tegra_channel_set_stream(struct tegra_vi_channel *chan, bool on)
+{
+	int ret;
+
+	if (on)
+		ret = tegra_channel_enable_stream(chan);
+	else
+		ret = tegra_channel_disable_stream(chan);
+
+	return ret;
+}
+
+void tegra_channel_release_buffers(struct tegra_vi_channel *chan,
+				   enum vb2_buffer_state state)
+{
+	struct tegra_channel_buffer *buf, *nbuf;
+
+	spin_lock(&chan->start_lock);
+	list_for_each_entry_safe(buf, nbuf, &chan->capture, queue) {
+		vb2_buffer_done(&buf->buf.vb2_buf, state);
+		list_del(&buf->queue);
+	}
+	spin_unlock(&chan->start_lock);
+
+	spin_lock(&chan->done_lock);
+	list_for_each_entry_safe(buf, nbuf, &chan->done, queue) {
+		vb2_buffer_done(&buf->buf.vb2_buf, state);
+		list_del(&buf->queue);
+	}
+	spin_unlock(&chan->done_lock);
+}
+
+static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
+	int ret;
+
+	ret = pm_runtime_resume_and_get(chan->vi->dev);
+	if (ret < 0) {
+		dev_err(chan->vi->dev, "failed to get runtime PM: %d\n", ret);
+		return ret;
+	}
+
+	ret = chan->vi->ops->vi_start_streaming(vq, count);
+	if (ret < 0)
+		pm_runtime_put(chan->vi->dev);
+
+	return ret;
+}
+
+static void tegra_channel_stop_streaming(struct vb2_queue *vq)
+{
+	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
+
+	chan->vi->ops->vi_stop_streaming(vq);
+	pm_runtime_put(chan->vi->dev);
+}
+
+static const struct vb2_ops tegra_channel_queue_qops = {
+	.queue_setup = tegra_channel_queue_setup,
+	.buf_prepare = tegra_channel_buffer_prepare,
+	.buf_queue = tegra_channel_buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = tegra_channel_start_streaming,
+	.stop_streaming = tegra_channel_stop_streaming,
+};
+
+/*
+ * V4L2 ioctl operations
+ */
+static int tegra_channel_querycap(struct file *file, void *fh,
+				  struct v4l2_capability *cap)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+
+	strscpy(cap->driver, "tegra-video", sizeof(cap->driver));
+	strscpy(cap->card, chan->video.name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(chan->vi->dev));
+
+	return 0;
+}
+
+static int tegra_channel_g_parm(struct file *file, void *fh,
+				struct v4l2_streamparm *a)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	return v4l2_g_parm_cap(&chan->video, subdev, a);
+}
+
+static int tegra_channel_s_parm(struct file *file, void *fh,
+				struct v4l2_streamparm *a)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	return v4l2_s_parm_cap(&chan->video, subdev, a);
+}
+
+static int tegra_channel_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *sizes)
+{
+	int ret;
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	const struct tegra_video_format *fmtinfo;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = sizes->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	fmtinfo = tegra_get_format_by_fourcc(chan->vi, sizes->pixel_format);
+	if (!fmtinfo)
+		return -EINVAL;
+
+	fse.code = fmtinfo->code;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+
+	sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	sizes->discrete.width = fse.max_width;
+	sizes->discrete.height = fse.max_height;
+
+	return 0;
+}
+
+static int tegra_channel_enum_frameintervals(struct file *file, void *fh,
+					     struct v4l2_frmivalenum *ivals)
+{
+	int ret;
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	const struct tegra_video_format *fmtinfo;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = ivals->index,
+		.width = ivals->width,
+		.height = ivals->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	fmtinfo = tegra_get_format_by_fourcc(chan->vi, ivals->pixel_format);
+	if (!fmtinfo)
+		return -EINVAL;
+
+	fie.code = fmtinfo->code;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+
+	ivals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	ivals->discrete.numerator = fie.interval.numerator;
+	ivals->discrete.denominator = fie.interval.denominator;
+
+	return 0;
+}
+
+static int tegra_channel_enum_format(struct file *file, void *fh,
+				     struct v4l2_fmtdesc *f)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	unsigned int index = 0, i;
+	unsigned long *fmts_bitmap = chan->tpg_fmts_bitmap;
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		fmts_bitmap = chan->fmts_bitmap;
+
+	if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
+		return -EINVAL;
+
+	for (i = 0; i < f->index + 1; i++, index++)
+		index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
+
+	f->pixelformat = tegra_get_format_fourcc_by_idx(chan->vi, index - 1);
+
+	return 0;
+}
+
+static int tegra_channel_get_format(struct file *file, void *fh,
+				    struct v4l2_format *format)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+
+	format->fmt.pix = chan->format;
+
+	return 0;
+}
+
+static void tegra_channel_fmt_align(struct tegra_vi_channel *chan,
+				    struct v4l2_pix_format *pix,
+				    unsigned int bpp)
+{
+	unsigned int min_bpl;
+	unsigned int max_bpl;
+	unsigned int bpl;
+
+	/*
+	 * The transfer alignment requirements are expressed in bytes.
+	 * Clamp the requested width and height to the limits.
+	 */
+	pix->width = clamp(pix->width, TEGRA_MIN_WIDTH, TEGRA_MAX_WIDTH);
+	pix->height = clamp(pix->height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
+
+	/* Clamp the requested bytes per line value. If the maximum bytes per
+	 * line value is zero, the module doesn't support user configurable
+	 * line sizes. Override the requested value with the minimum in that
+	 * case.
+	 */
+	min_bpl = pix->width * bpp;
+	max_bpl = rounddown(TEGRA_MAX_WIDTH, SURFACE_ALIGN_BYTES);
+	bpl = roundup(pix->bytesperline, SURFACE_ALIGN_BYTES);
+
+	pix->bytesperline = clamp(bpl, min_bpl, max_bpl);
+	pix->sizeimage = pix->bytesperline * pix->height;
+	if (pix->pixelformat == V4L2_PIX_FMT_NV16)
+		pix->sizeimage *= 2;
+}
+
+static int __tegra_channel_try_format(struct tegra_vi_channel *chan,
+				      struct v4l2_pix_format *pix)
+{
+	const struct tegra_video_format *fmtinfo;
+	static struct lock_class_key key;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev_state *sd_state;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP_BOUNDS,
+	};
+	int ret;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!subdev)
+		return -ENODEV;
+
+	/*
+	 * FIXME: Drop this call, drivers are not supposed to use
+	 * __v4l2_subdev_state_alloc().
+	 */
+	sd_state = __v4l2_subdev_state_alloc(subdev, "tegra:state->lock",
+					     &key);
+	if (IS_ERR(sd_state))
+		return PTR_ERR(sd_state);
+	/*
+	 * Retrieve the format information and if requested format isn't
+	 * supported, keep the current format.
+	 */
+	fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
+	if (!fmtinfo) {
+		pix->pixelformat = chan->format.pixelformat;
+		pix->colorspace = chan->format.colorspace;
+		fmtinfo = tegra_get_format_by_fourcc(chan->vi,
+						     pix->pixelformat);
+	}
+
+	pix->field = V4L2_FIELD_NONE;
+	fmt.which = V4L2_SUBDEV_FORMAT_TRY;
+	fmt.pad = 0;
+	v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
+
+	/*
+	 * Attempt to obtain the format size from subdev.
+	 * If not available, try to get crop boundary from subdev.
+	 */
+	fse.code = fmtinfo->code;
+	ret = v4l2_subdev_call(subdev, pad, enum_frame_size, sd_state, &fse);
+	if (ret) {
+		if (!v4l2_subdev_has_op(subdev, pad, get_selection)) {
+			sd_state->pads->try_crop.width = 0;
+			sd_state->pads->try_crop.height = 0;
+		} else {
+			ret = v4l2_subdev_call(subdev, pad, get_selection,
+					       NULL, &sdsel);
+			if (ret)
+				return -EINVAL;
+
+			sd_state->pads->try_crop.width = sdsel.r.width;
+			sd_state->pads->try_crop.height = sdsel.r.height;
+		}
+	} else {
+		sd_state->pads->try_crop.width = fse.max_width;
+		sd_state->pads->try_crop.height = fse.max_height;
+	}
+
+	ret = v4l2_subdev_call(subdev, pad, set_fmt, sd_state, &fmt);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_pix_format(pix, &fmt.format);
+	tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
+
+	__v4l2_subdev_state_free(sd_state);
+
+	return 0;
+}
+
+static int tegra_channel_try_format(struct file *file, void *fh,
+				    struct v4l2_format *format)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+
+	return __tegra_channel_try_format(chan, &format->fmt.pix);
+}
+
+static void tegra_channel_update_gangports(struct tegra_vi_channel *chan)
+{
+	if (chan->format.width <= 1920)
+		chan->numgangports = 1;
+	else
+		chan->numgangports = chan->totalports;
+}
+
+static int tegra_channel_set_format(struct file *file, void *fh,
+				    struct v4l2_format *format)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	const struct tegra_video_format *fmtinfo;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	struct v4l2_pix_format *pix = &format->fmt.pix;
+	int ret;
+
+	if (vb2_is_busy(&chan->queue))
+		return -EBUSY;
+
+	/* get supported format by try_fmt */
+	ret = __tegra_channel_try_format(chan, pix);
+	if (ret)
+		return ret;
+
+	fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
+
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.pad = 0;
+	v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(subdev, pad, set_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_pix_format(pix, &fmt.format);
+	tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
+
+	chan->format = *pix;
+	chan->fmtinfo = fmtinfo;
+	tegra_channel_update_gangports(chan);
+
+	return 0;
+}
+
+static int tegra_channel_set_subdev_active_fmt(struct tegra_vi_channel *chan)
+{
+	int ret, index;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	/*
+	 * Initialize channel format to the sub-device active format if there
+	 * is corresponding match in the Tegra supported video formats.
+	 */
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	index = tegra_get_format_idx_by_code(chan->vi, fmt.format.code, 0);
+	if (index < 0)
+		return -EINVAL;
+
+	chan->fmtinfo = &chan->vi->soc->video_formats[index];
+	v4l2_fill_pix_format(&chan->format, &fmt.format);
+	chan->format.pixelformat = chan->fmtinfo->fourcc;
+	chan->format.bytesperline = chan->format.width * chan->fmtinfo->bpp;
+	chan->format.sizeimage = chan->format.bytesperline *
+				 chan->format.height;
+	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
+	tegra_channel_update_gangports(chan);
+
+	return 0;
+}
+
+static int
+tegra_channel_subscribe_event(struct v4l2_fh *fh,
+			      const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_event_subscribe(fh, sub, 4, NULL);
+	}
+
+	return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static int tegra_channel_g_selection(struct file *file, void *priv,
+				     struct v4l2_selection *sel)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+	};
+	int ret;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, get_selection))
+		return -ENOTTY;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	/*
+	 * Try the get selection operation and fallback to get format if not
+	 * implemented.
+	 */
+	ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
+	if (!ret)
+		sel->r = sdsel.r;
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	sel->r.left = 0;
+	sel->r.top = 0;
+	sel->r.width = fmt.format.width;
+	sel->r.height = fmt.format.height;
+
+	return 0;
+}
+
+static int tegra_channel_s_selection(struct file *file, void *fh,
+				     struct v4l2_selection *sel)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	int ret;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+		.flags = sel->flags,
+		.r = sel->r,
+	};
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, set_selection))
+		return -ENOTTY;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (vb2_is_busy(&chan->queue))
+		return -EBUSY;
+
+	ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sdsel);
+	if (!ret) {
+		sel->r = sdsel.r;
+		/*
+		 * Subdev active format resolution may have changed during
+		 * set selection operation. So, update channel format to
+		 * the sub-device active format.
+		 */
+		return tegra_channel_set_subdev_active_fmt(chan);
+	}
+
+	return ret;
+}
+
+static int tegra_channel_g_edid(struct file *file, void *fh,
+				struct v4l2_edid *edid)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, get_edid))
+		return -ENOTTY;
+
+	return v4l2_subdev_call(subdev, pad, get_edid, edid);
+}
+
+static int tegra_channel_s_edid(struct file *file, void *fh,
+				struct v4l2_edid *edid)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, set_edid))
+		return -ENOTTY;
+
+	return v4l2_subdev_call(subdev, pad, set_edid, edid);
+}
+
+static int tegra_channel_g_dv_timings(struct file *file, void *fh,
+				      struct v4l2_dv_timings *timings)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, video, g_dv_timings))
+		return -ENOTTY;
+
+	return v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
+					  video, g_dv_timings, timings);
+}
+
+static int tegra_channel_s_dv_timings(struct file *file, void *fh,
+				      struct v4l2_dv_timings *timings)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	struct v4l2_bt_timings *bt = &timings->bt;
+	struct v4l2_dv_timings curr_timings;
+	int ret;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, video, s_dv_timings))
+		return -ENOTTY;
+
+	ret = tegra_channel_g_dv_timings(file, fh, &curr_timings);
+	if (ret)
+		return ret;
+
+	if (v4l2_match_dv_timings(timings, &curr_timings, 0, false))
+		return 0;
+
+	if (vb2_is_busy(&chan->queue))
+		return -EBUSY;
+
+	ret = v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
+					 video, s_dv_timings, timings);
+	if (ret)
+		return ret;
+
+	chan->format.width = bt->width;
+	chan->format.height = bt->height;
+	chan->format.bytesperline = bt->width * chan->fmtinfo->bpp;
+	chan->format.sizeimage = chan->format.bytesperline * bt->height;
+	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
+	tegra_channel_update_gangports(chan);
+
+	return 0;
+}
+
+static int tegra_channel_query_dv_timings(struct file *file, void *fh,
+					  struct v4l2_dv_timings *timings)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, video, query_dv_timings))
+		return -ENOTTY;
+
+	return v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
+					  video, query_dv_timings, timings);
+}
+
+static int tegra_channel_enum_dv_timings(struct file *file, void *fh,
+					 struct v4l2_enum_dv_timings *timings)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, enum_dv_timings))
+		return -ENOTTY;
+
+	return v4l2_subdev_call(subdev, pad, enum_dv_timings, timings);
+}
+
+static int tegra_channel_dv_timings_cap(struct file *file, void *fh,
+					struct v4l2_dv_timings_cap *cap)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!v4l2_subdev_has_op(subdev, pad, dv_timings_cap))
+		return -ENOTTY;
+
+	return v4l2_subdev_call(subdev, pad, dv_timings_cap, cap);
+}
+
+static int tegra_channel_log_status(struct file *file, void *fh)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+
+	v4l2_device_call_all(chan->video.v4l2_dev, 0, core, log_status);
+
+	return 0;
+}
+
+static int tegra_channel_enum_input(struct file *file, void *fh,
+				    struct v4l2_input *inp)
+{
+	struct tegra_vi_channel *chan = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+
+	if (inp->index)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	strscpy(inp->name, subdev->name, sizeof(inp->name));
+	if (v4l2_subdev_has_op(subdev, pad, dv_timings_cap))
+		inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+
+	return 0;
+}
+
+static int tegra_channel_g_input(struct file *file, void *priv,
+				 unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int tegra_channel_s_input(struct file *file, void *priv,
+				 unsigned int input)
+{
+	if (input > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
+	.vidioc_querycap		= tegra_channel_querycap,
+	.vidioc_g_parm			= tegra_channel_g_parm,
+	.vidioc_s_parm			= tegra_channel_s_parm,
+	.vidioc_enum_framesizes		= tegra_channel_enum_framesizes,
+	.vidioc_enum_frameintervals	= tegra_channel_enum_frameintervals,
+	.vidioc_enum_fmt_vid_cap	= tegra_channel_enum_format,
+	.vidioc_g_fmt_vid_cap		= tegra_channel_get_format,
+	.vidioc_s_fmt_vid_cap		= tegra_channel_set_format,
+	.vidioc_try_fmt_vid_cap		= tegra_channel_try_format,
+	.vidioc_enum_input		= tegra_channel_enum_input,
+	.vidioc_g_input			= tegra_channel_g_input,
+	.vidioc_s_input			= tegra_channel_s_input,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_subscribe_event		= tegra_channel_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_g_selection		= tegra_channel_g_selection,
+	.vidioc_s_selection		= tegra_channel_s_selection,
+	.vidioc_g_edid			= tegra_channel_g_edid,
+	.vidioc_s_edid			= tegra_channel_s_edid,
+	.vidioc_g_dv_timings		= tegra_channel_g_dv_timings,
+	.vidioc_s_dv_timings		= tegra_channel_s_dv_timings,
+	.vidioc_query_dv_timings	= tegra_channel_query_dv_timings,
+	.vidioc_enum_dv_timings		= tegra_channel_enum_dv_timings,
+	.vidioc_dv_timings_cap		= tegra_channel_dv_timings_cap,
+	.vidioc_log_status		= tegra_channel_log_status,
+};
+
+/*
+ * V4L2 file operations
+ */
+static const struct v4l2_file_operations tegra_channel_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= video_ioctl2,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.read		= vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+};
+
+/*
+ * V4L2 control operations
+ */
+static int vi_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tegra_vi_channel *chan = container_of(ctrl->handler,
+						     struct tegra_vi_channel,
+						     ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		/* pattern change takes effect on next stream */
+		chan->pg_mode = ctrl->val + 1;
+		break;
+	case V4L2_CID_TEGRA_SYNCPT_TIMEOUT_RETRY:
+		chan->syncpt_timeout_retry = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vi_ctrl_ops = {
+	.s_ctrl	= vi_s_ctrl,
+};
+
+#if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
+static const char *const vi_pattern_strings[] = {
+	"Black/White Direct Mode",
+	"Color Patch Mode",
+};
+#else
+static const struct v4l2_ctrl_config syncpt_timeout_ctrl = {
+	.ops = &vi_ctrl_ops,
+	.id = V4L2_CID_TEGRA_SYNCPT_TIMEOUT_RETRY,
+	.name = "Syncpt timeout retry",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 10000,
+	.step = 1,
+	.def = 5,
+};
+#endif
+
+static int tegra_channel_setup_ctrl_handler(struct tegra_vi_channel *chan)
+{
+	int ret;
+
+#if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
+	/* add test pattern control handler to v4l2 device */
+	v4l2_ctrl_new_std_menu_items(&chan->ctrl_handler, &vi_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(vi_pattern_strings) - 1,
+				     0, 0, vi_pattern_strings);
+	if (chan->ctrl_handler.error) {
+		dev_err(chan->vi->dev, "failed to add TPG ctrl handler: %d\n",
+			chan->ctrl_handler.error);
+		v4l2_ctrl_handler_free(&chan->ctrl_handler);
+		return chan->ctrl_handler.error;
+	}
+#else
+	struct v4l2_subdev *subdev;
+
+	/* custom control */
+	v4l2_ctrl_new_custom(&chan->ctrl_handler, &syncpt_timeout_ctrl, NULL);
+	if (chan->ctrl_handler.error) {
+		dev_err(chan->vi->dev, "failed to add %s ctrl handler: %d\n",
+			syncpt_timeout_ctrl.name,
+			chan->ctrl_handler.error);
+		v4l2_ctrl_handler_free(&chan->ctrl_handler);
+		return chan->ctrl_handler.error;
+	}
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	if (!subdev)
+		return -ENODEV;
+
+	ret = v4l2_ctrl_add_handler(&chan->ctrl_handler, subdev->ctrl_handler,
+				    NULL, true);
+	if (ret < 0) {
+		dev_err(chan->vi->dev,
+			"failed to add subdev %s ctrl handler: %d\n",
+			subdev->name, ret);
+		v4l2_ctrl_handler_free(&chan->ctrl_handler);
+		return ret;
+	}
+#endif
+
+	/* setup the controls */
+	ret = v4l2_ctrl_handler_setup(&chan->ctrl_handler);
+	if (ret < 0) {
+		dev_err(chan->vi->dev,
+			"failed to setup v4l2 ctrl handler: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* VI only support 2 formats in TPG mode */
+static void vi_tpg_fmts_bitmap_init(struct tegra_vi_channel *chan)
+{
+	int index;
+
+	bitmap_zero(chan->tpg_fmts_bitmap, MAX_FORMAT_NUM);
+
+	index = tegra_get_format_idx_by_code(chan->vi,
+					     MEDIA_BUS_FMT_SRGGB10_1X10, 0);
+	bitmap_set(chan->tpg_fmts_bitmap, index, 1);
+
+	index = tegra_get_format_idx_by_code(chan->vi,
+					     MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+					     0);
+	bitmap_set(chan->tpg_fmts_bitmap, index, 1);
+}
+
+static int vi_fmts_bitmap_init(struct tegra_vi_channel *chan)
+{
+	int index, ret, match_code = 0;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
+
+	/*
+	 * Set the bitmap bits based on all the matched formats between the
+	 * available media bus formats of sub-device and the pre-defined Tegra
+	 * supported video formats.
+	 */
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	while (1) {
+		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
+				       NULL, &code);
+		if (ret < 0)
+			break;
+
+		index = tegra_get_format_idx_by_code(chan->vi, code.code, 0);
+		while (index >= 0) {
+			bitmap_set(chan->fmts_bitmap, index, 1);
+			if (!match_code)
+				match_code = code.code;
+			/* look for other formats with same mbus code */
+			index = tegra_get_format_idx_by_code(chan->vi,
+							     code.code,
+							     index + 1);
+		}
+
+		code.index++;
+	}
+
+	/*
+	 * Set the bitmap bit corresponding to default tegra video format if
+	 * there are no matched formats.
+	 */
+	if (!match_code) {
+		match_code = tegra_default_format.code;
+		index = tegra_get_format_idx_by_code(chan->vi, match_code, 0);
+		if (WARN_ON(index < 0))
+			return -EINVAL;
+
+		bitmap_set(chan->fmts_bitmap, index, 1);
+	}
+
+	/* initialize channel format to the sub-device active format */
+	tegra_channel_set_subdev_active_fmt(chan);
+
+	return 0;
+}
+
+static void tegra_channel_host1x_syncpts_free(struct tegra_vi_channel *chan)
+{
+	int i;
+
+	for (i = 0; i < chan->numgangports; i++) {
+		host1x_syncpt_put(chan->mw_ack_sp[i]);
+		host1x_syncpt_put(chan->frame_start_sp[i]);
+	}
+}
+
+static void tegra_channel_cleanup(struct tegra_vi_channel *chan)
+{
+	v4l2_ctrl_handler_free(&chan->ctrl_handler);
+	media_entity_cleanup(&chan->video.entity);
+	tegra_channel_host1x_syncpts_free(chan);
+	mutex_destroy(&chan->video_lock);
+}
+
+void tegra_channels_cleanup(struct tegra_vi *vi)
+{
+	struct tegra_vi_channel *chan, *tmp;
+
+	if (!vi)
+		return;
+
+	list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
+		tegra_channel_cleanup(chan);
+		list_del(&chan->list);
+		kfree(chan);
+	}
+}
+
+static int tegra_channel_host1x_syncpt_init(struct tegra_vi_channel *chan)
+{
+	struct tegra_vi *vi = chan->vi;
+	unsigned long flags = HOST1X_SYNCPT_CLIENT_MANAGED;
+	struct host1x_syncpt *fs_sp;
+	struct host1x_syncpt *mw_sp;
+	int ret, i;
+
+	for (i = 0; i < chan->numgangports; i++) {
+		fs_sp = host1x_syncpt_request(&vi->client, flags);
+		if (!fs_sp) {
+			dev_err(vi->dev, "failed to request frame start syncpoint\n");
+			ret = -ENOMEM;
+			goto free_syncpts;
+		}
+
+		mw_sp = host1x_syncpt_request(&vi->client, flags);
+		if (!mw_sp) {
+			dev_err(vi->dev, "failed to request memory ack syncpoint\n");
+			host1x_syncpt_put(fs_sp);
+			ret = -ENOMEM;
+			goto free_syncpts;
+		}
+
+		chan->frame_start_sp[i] = fs_sp;
+		chan->mw_ack_sp[i] = mw_sp;
+		spin_lock_init(&chan->sp_incr_lock[i]);
+	}
+
+	return 0;
+
+free_syncpts:
+	tegra_channel_host1x_syncpts_free(chan);
+	return ret;
+}
+
+static int tegra_channel_init(struct tegra_vi_channel *chan)
+{
+	struct tegra_vi *vi = chan->vi;
+	struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
+	int ret;
+
+	mutex_init(&chan->video_lock);
+	INIT_LIST_HEAD(&chan->capture);
+	INIT_LIST_HEAD(&chan->done);
+	spin_lock_init(&chan->start_lock);
+	spin_lock_init(&chan->done_lock);
+	init_waitqueue_head(&chan->start_wait);
+	init_waitqueue_head(&chan->done_wait);
+
+	/* initialize the video format */
+	chan->fmtinfo = &tegra_default_format;
+	chan->format.pixelformat = chan->fmtinfo->fourcc;
+	chan->format.colorspace = V4L2_COLORSPACE_SRGB;
+	chan->format.field = V4L2_FIELD_NONE;
+	chan->format.width = TEGRA_DEF_WIDTH;
+	chan->format.height = TEGRA_DEF_HEIGHT;
+	chan->format.bytesperline = TEGRA_DEF_WIDTH * chan->fmtinfo->bpp;
+	chan->format.sizeimage = chan->format.bytesperline * TEGRA_DEF_HEIGHT;
+	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
+
+	ret = tegra_channel_host1x_syncpt_init(chan);
+	if (ret)
+		return ret;
+
+	/* initialize the media entity */
+	chan->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&chan->video.entity, 1, &chan->pad);
+	if (ret < 0) {
+		dev_err(vi->dev,
+			"failed to initialize media entity: %d\n", ret);
+		goto free_syncpts;
+	}
+
+	ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
+	if (chan->ctrl_handler.error) {
+		dev_err(vi->dev,
+			"failed to initialize v4l2 ctrl handler: %d\n", ret);
+		goto cleanup_media;
+	}
+
+	/* initialize the video_device */
+	chan->video.fops = &tegra_channel_fops;
+	chan->video.v4l2_dev = &vid->v4l2_dev;
+	chan->video.release = video_device_release_empty;
+	chan->video.queue = &chan->queue;
+	snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
+		 dev_name(vi->dev), "output", chan->portnos[0]);
+	chan->video.vfl_type = VFL_TYPE_VIDEO;
+	chan->video.vfl_dir = VFL_DIR_RX;
+	chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
+	chan->video.ctrl_handler = &chan->ctrl_handler;
+	chan->video.lock = &chan->video_lock;
+	chan->video.device_caps = V4L2_CAP_VIDEO_CAPTURE |
+				  V4L2_CAP_STREAMING |
+				  V4L2_CAP_READWRITE;
+	video_set_drvdata(&chan->video, chan);
+
+	chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
+	chan->queue.lock = &chan->video_lock;
+	chan->queue.drv_priv = chan;
+	chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
+	chan->queue.ops = &tegra_channel_queue_qops;
+	chan->queue.mem_ops = &vb2_dma_contig_memops;
+	chan->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	chan->queue.min_buffers_needed = 2;
+	chan->queue.dev = vi->dev;
+	ret = vb2_queue_init(&chan->queue);
+	if (ret < 0) {
+		dev_err(vi->dev, "failed to initialize vb2 queue: %d\n", ret);
+		goto free_v4l2_ctrl_hdl;
+	}
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		v4l2_async_nf_init(&chan->notifier);
+
+	return 0;
+
+free_v4l2_ctrl_hdl:
+	v4l2_ctrl_handler_free(&chan->ctrl_handler);
+cleanup_media:
+	media_entity_cleanup(&chan->video.entity);
+free_syncpts:
+	tegra_channel_host1x_syncpts_free(chan);
+	return ret;
+}
+
+static int tegra_vi_channel_alloc(struct tegra_vi *vi, unsigned int port_num,
+				  struct device_node *node, unsigned int lanes)
+{
+	struct tegra_vi_channel *chan;
+	unsigned int i;
+
+	/*
+	 * Do not use devm_kzalloc as memory is freed immediately
+	 * when device instance is unbound but application might still
+	 * be holding the device node open. Channel memory allocated
+	 * with kzalloc is freed during video device release callback.
+	 */
+	chan = kzalloc(sizeof(*chan), GFP_KERNEL);
+	if (!chan)
+		return -ENOMEM;
+
+	chan->vi = vi;
+	chan->portnos[0] = port_num;
+	/*
+	 * For data lanes more than maximum csi lanes per brick, multiple of
+	 * x4 ports are used simultaneously for capture.
+	 */
+	if (lanes <= CSI_LANES_PER_BRICK)
+		chan->totalports = 1;
+	else
+		chan->totalports = lanes / CSI_LANES_PER_BRICK;
+	chan->numgangports = chan->totalports;
+
+	for (i = 1; i < chan->totalports; i++)
+		chan->portnos[i] = chan->portnos[0] + i * CSI_PORTS_PER_BRICK;
+
+	chan->of_node = node;
+	list_add_tail(&chan->list, &vi->vi_chans);
+
+	return 0;
+}
+
+static int tegra_vi_tpg_channels_alloc(struct tegra_vi *vi)
+{
+	unsigned int port_num;
+	unsigned int nchannels = vi->soc->vi_max_channels;
+	int ret;
+
+	for (port_num = 0; port_num < nchannels; port_num++) {
+		ret = tegra_vi_channel_alloc(vi, port_num,
+					     vi->dev->of_node, 2);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int tegra_vi_channels_alloc(struct tegra_vi *vi)
+{
+	struct device_node *node = vi->dev->of_node;
+	struct device_node *ep = NULL;
+	struct device_node *ports;
+	struct device_node *port;
+	unsigned int port_num;
+	struct device_node *parent;
+	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
+	unsigned int lanes;
+	int ret = 0;
+
+	ports = of_get_child_by_name(node, "ports");
+	if (!ports)
+		return -ENODEV;
+
+	for_each_child_of_node(ports, port) {
+		if (!of_node_name_eq(port, "port"))
+			continue;
+
+		ret = of_property_read_u32(port, "reg", &port_num);
+		if (ret < 0)
+			continue;
+
+		if (port_num > vi->soc->vi_max_channels) {
+			dev_err(vi->dev, "invalid port num %d for %pOF\n",
+				port_num, port);
+			ret = -EINVAL;
+			of_node_put(port);
+			goto cleanup;
+		}
+
+		ep = of_get_child_by_name(port, "endpoint");
+		if (!ep)
+			continue;
+
+		parent = of_graph_get_remote_port_parent(ep);
+		of_node_put(ep);
+		if (!parent)
+			continue;
+
+		ep = of_graph_get_endpoint_by_regs(parent, 0, 0);
+		of_node_put(parent);
+		ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep),
+						 &v4l2_ep);
+		of_node_put(ep);
+		if (ret)
+			continue;
+
+		lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
+		ret = tegra_vi_channel_alloc(vi, port_num, port, lanes);
+		if (ret < 0) {
+			of_node_put(port);
+			goto cleanup;
+		}
+	}
+
+cleanup:
+	of_node_put(ports);
+	return ret;
+}
+
+static int tegra_vi_channels_init(struct tegra_vi *vi)
+{
+	struct tegra_vi_channel *chan;
+	int ret;
+
+	list_for_each_entry(chan, &vi->vi_chans, list) {
+		ret = tegra_channel_init(chan);
+		if (ret < 0) {
+			dev_err(vi->dev,
+				"failed to initialize channel-%d: %d\n",
+				chan->portnos[0], ret);
+			goto cleanup;
+		}
+	}
+
+	return 0;
+
+cleanup:
+	list_for_each_entry_continue_reverse(chan, &vi->vi_chans, list)
+		tegra_channel_cleanup(chan);
+
+	return ret;
+}
+
+void tegra_v4l2_nodes_cleanup_tpg(struct tegra_video_device *vid)
+{
+	struct tegra_vi *vi = vid->vi;
+	struct tegra_csi *csi = vid->csi;
+	struct tegra_csi_channel *csi_chan;
+	struct tegra_vi_channel *chan;
+
+	list_for_each_entry(chan, &vi->vi_chans, list)
+		vb2_video_unregister_device(&chan->video);
+
+	list_for_each_entry(csi_chan, &csi->csi_chans, list)
+		v4l2_device_unregister_subdev(&csi_chan->subdev);
+}
+
+int tegra_v4l2_nodes_setup_tpg(struct tegra_video_device *vid)
+{
+	struct tegra_vi *vi = vid->vi;
+	struct tegra_csi *csi = vid->csi;
+	struct tegra_vi_channel *vi_chan;
+	struct tegra_csi_channel *csi_chan;
+	u32 link_flags = MEDIA_LNK_FL_ENABLED;
+	int ret;
+
+	if (!vi || !csi)
+		return -ENODEV;
+
+	csi_chan = list_first_entry(&csi->csi_chans,
+				    struct tegra_csi_channel, list);
+
+	list_for_each_entry(vi_chan, &vi->vi_chans, list) {
+		struct media_entity *source = &csi_chan->subdev.entity;
+		struct media_entity *sink = &vi_chan->video.entity;
+		struct media_pad *source_pad = csi_chan->pads;
+		struct media_pad *sink_pad = &vi_chan->pad;
+
+		ret = v4l2_device_register_subdev(&vid->v4l2_dev,
+						  &csi_chan->subdev);
+		if (ret) {
+			dev_err(vi->dev,
+				"failed to register subdev: %d\n", ret);
+			goto cleanup;
+		}
+
+		ret = video_register_device(&vi_chan->video,
+					    VFL_TYPE_VIDEO, -1);
+		if (ret < 0) {
+			dev_err(vi->dev,
+				"failed to register video device: %d\n", ret);
+			goto cleanup;
+		}
+
+		dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
+			source->name, source_pad->index,
+			sink->name, sink_pad->index);
+
+		ret = media_create_pad_link(source, source_pad->index,
+					    sink, sink_pad->index,
+					    link_flags);
+		if (ret < 0) {
+			dev_err(vi->dev,
+				"failed to create %s:%u -> %s:%u link: %d\n",
+				source->name, source_pad->index,
+				sink->name, sink_pad->index, ret);
+			goto cleanup;
+		}
+
+		ret = tegra_channel_setup_ctrl_handler(vi_chan);
+		if (ret < 0)
+			goto cleanup;
+
+		v4l2_set_subdev_hostdata(&csi_chan->subdev, vi_chan);
+		vi_tpg_fmts_bitmap_init(vi_chan);
+		csi_chan = list_next_entry(csi_chan, list);
+	}
+
+	return 0;
+
+cleanup:
+	tegra_v4l2_nodes_cleanup_tpg(vid);
+	return ret;
+}
+
+static int __maybe_unused vi_runtime_resume(struct device *dev)
+{
+	struct tegra_vi *vi = dev_get_drvdata(dev);
+	int ret;
+
+	ret = regulator_enable(vi->vdd);
+	if (ret) {
+		dev_err(dev, "failed to enable VDD supply: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_set_rate(vi->clk, vi->soc->vi_max_clk_hz);
+	if (ret) {
+		dev_err(dev, "failed to set vi clock rate: %d\n", ret);
+		goto disable_vdd;
+	}
+
+	ret = clk_prepare_enable(vi->clk);
+	if (ret) {
+		dev_err(dev, "failed to enable vi clock: %d\n", ret);
+		goto disable_vdd;
+	}
+
+	return 0;
+
+disable_vdd:
+	regulator_disable(vi->vdd);
+	return ret;
+}
+
+static int __maybe_unused vi_runtime_suspend(struct device *dev)
+{
+	struct tegra_vi *vi = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(vi->clk);
+
+	regulator_disable(vi->vdd);
+
+	return 0;
+}
+
+/*
+ * Graph Management
+ */
+static struct tegra_vi_graph_entity *
+tegra_vi_graph_find_entity(struct tegra_vi_channel *chan,
+			   const struct fwnode_handle *fwnode)
+{
+	struct tegra_vi_graph_entity *entity;
+	struct v4l2_async_subdev *asd;
+
+	list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
+		entity = to_tegra_vi_graph_entity(asd);
+		if (entity->asd.match.fwnode == fwnode)
+			return entity;
+	}
+
+	return NULL;
+}
+
+static int tegra_vi_graph_build(struct tegra_vi_channel *chan,
+				struct tegra_vi_graph_entity *entity)
+{
+	struct tegra_vi *vi = chan->vi;
+	struct tegra_vi_graph_entity *ent;
+	struct fwnode_handle *ep = NULL;
+	struct v4l2_fwnode_link link;
+	struct media_entity *local = entity->entity;
+	struct media_entity *remote;
+	struct media_pad *local_pad;
+	struct media_pad *remote_pad;
+	u32 link_flags = MEDIA_LNK_FL_ENABLED;
+	int ret = 0;
+
+	dev_dbg(vi->dev, "creating links for entity %s\n", local->name);
+
+	while (1) {
+		ep = fwnode_graph_get_next_endpoint(entity->asd.match.fwnode,
+						    ep);
+		if (!ep)
+			break;
+
+		ret = v4l2_fwnode_parse_link(ep, &link);
+		if (ret < 0) {
+			dev_err(vi->dev, "failed to parse link for %pOF: %d\n",
+				to_of_node(ep), ret);
+			continue;
+		}
+
+		if (link.local_port >= local->num_pads) {
+			dev_err(vi->dev, "invalid port number %u on %pOF\n",
+				link.local_port, to_of_node(link.local_node));
+			v4l2_fwnode_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		local_pad = &local->pads[link.local_port];
+		/* Remote node is vi node. So use channel video entity and pad
+		 * as remote/sink.
+		 */
+		if (link.remote_node == of_fwnode_handle(vi->dev->of_node)) {
+			remote = &chan->video.entity;
+			remote_pad = &chan->pad;
+			goto create_link;
+		}
+
+		/*
+		 * Skip sink ports, they will be processed from the other end
+		 * of the link.
+		 */
+		if (local_pad->flags & MEDIA_PAD_FL_SINK) {
+			dev_dbg(vi->dev, "skipping sink port %pOF:%u\n",
+				to_of_node(link.local_node), link.local_port);
+			v4l2_fwnode_put_link(&link);
+			continue;
+		}
+
+		/* find the remote entity from notifier list */
+		ent = tegra_vi_graph_find_entity(chan, link.remote_node);
+		if (!ent) {
+			dev_err(vi->dev, "no entity found for %pOF\n",
+				to_of_node(link.remote_node));
+			v4l2_fwnode_put_link(&link);
+			ret = -ENODEV;
+			break;
+		}
+
+		remote = ent->entity;
+		if (link.remote_port >= remote->num_pads) {
+			dev_err(vi->dev, "invalid port number %u on %pOF\n",
+				link.remote_port,
+				to_of_node(link.remote_node));
+			v4l2_fwnode_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		remote_pad = &remote->pads[link.remote_port];
+
+create_link:
+		dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
+			local->name, local_pad->index,
+			remote->name, remote_pad->index);
+
+		ret = media_create_pad_link(local, local_pad->index,
+					    remote, remote_pad->index,
+					    link_flags);
+		v4l2_fwnode_put_link(&link);
+		if (ret < 0) {
+			dev_err(vi->dev,
+				"failed to create %s:%u -> %s:%u link: %d\n",
+				local->name, local_pad->index,
+				remote->name, remote_pad->index, ret);
+			break;
+		}
+	}
+
+	fwnode_handle_put(ep);
+	return ret;
+}
+
+static int tegra_vi_graph_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct tegra_vi_graph_entity *entity;
+	struct v4l2_async_subdev *asd;
+	struct v4l2_subdev *subdev;
+	struct tegra_vi_channel *chan;
+	struct tegra_vi *vi;
+	int ret;
+
+	chan = container_of(notifier, struct tegra_vi_channel, notifier);
+	vi = chan->vi;
+
+	dev_dbg(vi->dev, "notify complete, all subdevs registered\n");
+
+	/*
+	 * Video device node should be created at the end of all the device
+	 * related initialization/setup.
+	 * Current video_register_device() does both initialize and register
+	 * video device in same API.
+	 *
+	 * TODO: Update v4l2-dev driver to split initialize and register into
+	 * separate APIs and then update Tegra video driver to do video device
+	 * initialize followed by all video device related setup and then
+	 * register the video device.
+	 */
+	ret = video_register_device(&chan->video, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		dev_err(vi->dev,
+			"failed to register video device: %d\n", ret);
+		goto unregister_video;
+	}
+
+	/* create links between the entities */
+	list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
+		entity = to_tegra_vi_graph_entity(asd);
+		ret = tegra_vi_graph_build(chan, entity);
+		if (ret < 0)
+			goto unregister_video;
+	}
+
+	ret = tegra_channel_setup_ctrl_handler(chan);
+	if (ret < 0) {
+		dev_err(vi->dev,
+			"failed to setup channel controls: %d\n", ret);
+		goto unregister_video;
+	}
+
+	ret = vi_fmts_bitmap_init(chan);
+	if (ret < 0) {
+		dev_err(vi->dev,
+			"failed to initialize formats bitmap: %d\n", ret);
+		goto unregister_video;
+	}
+
+	subdev = tegra_channel_get_remote_csi_subdev(chan);
+	if (!subdev) {
+		ret = -ENODEV;
+		dev_err(vi->dev,
+			"failed to get remote csi subdev: %d\n", ret);
+		goto unregister_video;
+	}
+
+	v4l2_set_subdev_hostdata(subdev, chan);
+
+	subdev = tegra_channel_get_remote_source_subdev(chan);
+	v4l2_set_subdev_hostdata(subdev, chan);
+
+	return 0;
+
+unregister_video:
+	vb2_video_unregister_device(&chan->video);
+	return ret;
+}
+
+static int tegra_vi_graph_notify_bound(struct v4l2_async_notifier *notifier,
+				       struct v4l2_subdev *subdev,
+				       struct v4l2_async_subdev *asd)
+{
+	struct tegra_vi_graph_entity *entity;
+	struct tegra_vi *vi;
+	struct tegra_vi_channel *chan;
+
+	chan = container_of(notifier, struct tegra_vi_channel, notifier);
+	vi = chan->vi;
+
+	/*
+	 * Locate the entity corresponding to the bound subdev and store the
+	 * subdev pointer.
+	 */
+	entity = tegra_vi_graph_find_entity(chan, subdev->fwnode);
+	if (!entity) {
+		dev_err(vi->dev, "no entity for subdev %s\n", subdev->name);
+		return -EINVAL;
+	}
+
+	if (entity->subdev) {
+		dev_err(vi->dev, "duplicate subdev for node %pOF\n",
+			to_of_node(entity->asd.match.fwnode));
+		return -EINVAL;
+	}
+
+	dev_dbg(vi->dev, "subdev %s bound\n", subdev->name);
+	entity->entity = &subdev->entity;
+	entity->subdev = subdev;
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations tegra_vi_async_ops = {
+	.bound = tegra_vi_graph_notify_bound,
+	.complete = tegra_vi_graph_notify_complete,
+};
+
+static int tegra_vi_graph_parse_one(struct tegra_vi_channel *chan,
+				    struct fwnode_handle *fwnode)
+{
+	struct tegra_vi *vi = chan->vi;
+	struct fwnode_handle *ep = NULL;
+	struct fwnode_handle *remote = NULL;
+	struct tegra_vi_graph_entity *tvge;
+	struct device_node *node = NULL;
+	int ret;
+
+	dev_dbg(vi->dev, "parsing node %pOF\n", to_of_node(fwnode));
+
+	/* parse all the remote entities and put them into the list */
+	for_each_endpoint_of_node(to_of_node(fwnode), node) {
+		ep = of_fwnode_handle(node);
+		remote = fwnode_graph_get_remote_port_parent(ep);
+		if (!remote) {
+			dev_err(vi->dev,
+				"remote device at %pOF not found\n", node);
+			ret = -EINVAL;
+			goto cleanup;
+		}
+
+		/* skip entities that are already processed */
+		if (remote == dev_fwnode(vi->dev) ||
+		    tegra_vi_graph_find_entity(chan, remote)) {
+			fwnode_handle_put(remote);
+			continue;
+		}
+
+		tvge = v4l2_async_nf_add_fwnode(&chan->notifier, remote,
+						struct tegra_vi_graph_entity);
+		if (IS_ERR(tvge)) {
+			ret = PTR_ERR(tvge);
+			dev_err(vi->dev,
+				"failed to add subdev to notifier: %d\n", ret);
+			fwnode_handle_put(remote);
+			goto cleanup;
+		}
+
+		ret = tegra_vi_graph_parse_one(chan, remote);
+		if (ret < 0) {
+			fwnode_handle_put(remote);
+			goto cleanup;
+		}
+
+		fwnode_handle_put(remote);
+	}
+
+	return 0;
+
+cleanup:
+	dev_err(vi->dev, "failed parsing the graph: %d\n", ret);
+	v4l2_async_nf_cleanup(&chan->notifier);
+	of_node_put(node);
+	return ret;
+}
+
+static int tegra_vi_graph_init(struct tegra_vi *vi)
+{
+	struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
+	struct tegra_vi_channel *chan;
+	struct fwnode_handle *fwnode = dev_fwnode(vi->dev);
+	int ret;
+
+	/*
+	 * Walk the links to parse the full graph. Each channel will have
+	 * one endpoint of the composite node. Start by parsing the
+	 * composite node and parse the remote entities in turn.
+	 * Each channel will register v4l2 async notifier to make the graph
+	 * independent between the channels so we can the current channel
+	 * in case of something wrong during graph parsing and continue with
+	 * next channels.
+	 */
+	list_for_each_entry(chan, &vi->vi_chans, list) {
+		struct fwnode_handle *ep, *remote;
+
+		ep = fwnode_graph_get_endpoint_by_id(fwnode,
+						     chan->portnos[0], 0, 0);
+		if (!ep)
+			continue;
+
+		remote = fwnode_graph_get_remote_port_parent(ep);
+		fwnode_handle_put(ep);
+
+		ret = tegra_vi_graph_parse_one(chan, remote);
+		fwnode_handle_put(remote);
+		if (ret < 0 || list_empty(&chan->notifier.asd_list))
+			continue;
+
+		chan->notifier.ops = &tegra_vi_async_ops;
+		ret = v4l2_async_nf_register(&vid->v4l2_dev, &chan->notifier);
+		if (ret < 0) {
+			dev_err(vi->dev,
+				"failed to register channel %d notifier: %d\n",
+				chan->portnos[0], ret);
+			v4l2_async_nf_cleanup(&chan->notifier);
+		}
+	}
+
+	return 0;
+}
+
+static void tegra_vi_graph_cleanup(struct tegra_vi *vi)
+{
+	struct tegra_vi_channel *chan;
+
+	list_for_each_entry(chan, &vi->vi_chans, list) {
+		vb2_video_unregister_device(&chan->video);
+		v4l2_async_nf_unregister(&chan->notifier);
+		v4l2_async_nf_cleanup(&chan->notifier);
+	}
+}
+
+static int tegra_vi_init(struct host1x_client *client)
+{
+	struct tegra_video_device *vid = dev_get_drvdata(client->host);
+	struct tegra_vi *vi = host1x_client_to_vi(client);
+	struct tegra_vi_channel *chan, *tmp;
+	int ret;
+
+	vid->media_dev.hw_revision = vi->soc->hw_revision;
+	snprintf(vid->media_dev.bus_info, sizeof(vid->media_dev.bus_info),
+		 "platform:%s", dev_name(vi->dev));
+
+	INIT_LIST_HEAD(&vi->vi_chans);
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		ret = tegra_vi_tpg_channels_alloc(vi);
+	else
+		ret = tegra_vi_channels_alloc(vi);
+	if (ret < 0) {
+		dev_err(vi->dev,
+			"failed to allocate vi channels: %d\n", ret);
+		goto free_chans;
+	}
+
+	ret = tegra_vi_channels_init(vi);
+	if (ret < 0)
+		goto free_chans;
+
+	vid->vi = vi;
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
+		ret = tegra_vi_graph_init(vi);
+		if (ret < 0)
+			goto cleanup_chans;
+	}
+
+	return 0;
+
+cleanup_chans:
+	list_for_each_entry(chan, &vi->vi_chans, list)
+		tegra_channel_cleanup(chan);
+free_chans:
+	list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
+		list_del(&chan->list);
+		kfree(chan);
+	}
+
+	return ret;
+}
+
+static int tegra_vi_exit(struct host1x_client *client)
+{
+	struct tegra_vi *vi = host1x_client_to_vi(client);
+
+	/*
+	 * Do not cleanup the channels here as application might still be
+	 * holding video device nodes. Channels cleanup will happen during
+	 * v4l2_device release callback which gets called after all video
+	 * device nodes are released.
+	 */
+
+	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		tegra_vi_graph_cleanup(vi);
+
+	return 0;
+}
+
+static const struct host1x_client_ops vi_client_ops = {
+	.init = tegra_vi_init,
+	.exit = tegra_vi_exit,
+};
+
+static int tegra_vi_probe(struct platform_device *pdev)
+{
+	struct tegra_vi *vi;
+	int ret;
+
+	vi = devm_kzalloc(&pdev->dev, sizeof(*vi), GFP_KERNEL);
+	if (!vi)
+		return -ENOMEM;
+
+	vi->iomem = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(vi->iomem))
+		return PTR_ERR(vi->iomem);
+
+	vi->soc = of_device_get_match_data(&pdev->dev);
+
+	vi->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(vi->clk)) {
+		ret = PTR_ERR(vi->clk);
+		dev_err(&pdev->dev, "failed to get vi clock: %d\n", ret);
+		return ret;
+	}
+
+	vi->vdd = devm_regulator_get(&pdev->dev, "avdd-dsi-csi");
+	if (IS_ERR(vi->vdd)) {
+		ret = PTR_ERR(vi->vdd);
+		dev_err(&pdev->dev, "failed to get VDD supply: %d\n", ret);
+		return ret;
+	}
+
+	if (!pdev->dev.pm_domain) {
+		ret = -ENOENT;
+		dev_warn(&pdev->dev, "PM domain is not attached: %d\n", ret);
+		return ret;
+	}
+
+	ret = devm_of_platform_populate(&pdev->dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"failed to populate vi child device: %d\n", ret);
+		return ret;
+	}
+
+	vi->dev = &pdev->dev;
+	vi->ops = vi->soc->ops;
+	platform_set_drvdata(pdev, vi);
+	pm_runtime_enable(&pdev->dev);
+
+	/* initialize host1x interface */
+	INIT_LIST_HEAD(&vi->client.list);
+	vi->client.ops = &vi_client_ops;
+	vi->client.dev = &pdev->dev;
+
+	ret = host1x_client_register(&vi->client);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"failed to register host1x client: %d\n", ret);
+		goto rpm_disable;
+	}
+
+	return 0;
+
+rpm_disable:
+	pm_runtime_disable(&pdev->dev);
+	return ret;
+}
+
+static int tegra_vi_remove(struct platform_device *pdev)
+{
+	struct tegra_vi *vi = platform_get_drvdata(pdev);
+	int err;
+
+	err = host1x_client_unregister(&vi->client);
+	if (err < 0) {
+		dev_err(&pdev->dev,
+			"failed to unregister host1x client: %d\n", err);
+		return err;
+	}
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id tegra_vi_of_id_table[] = {
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+	{ .compatible = "nvidia,tegra210-vi", .data = &tegra210_vi_soc },
+#endif
+	{ }
+};
+MODULE_DEVICE_TABLE(of, tegra_vi_of_id_table);
+
+static const struct dev_pm_ops tegra_vi_pm_ops = {
+	SET_RUNTIME_PM_OPS(vi_runtime_suspend, vi_runtime_resume, NULL)
+};
+
+struct platform_driver tegra_vi_driver = {
+	.driver = {
+		.name = "tegra-vi",
+		.of_match_table = tegra_vi_of_id_table,
+		.pm = &tegra_vi_pm_ops,
+	},
+	.probe = tegra_vi_probe,
+	.remove = tegra_vi_remove,
+};
diff --git a/drivers/staging/media/tegra-video/vi.h b/drivers/staging/media/tegra-video/vi.h
new file mode 100644
index 000000000..a68e2c02c
--- /dev/null
+++ b/drivers/staging/media/tegra-video/vi.h
@@ -0,0 +1,291 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#ifndef __TEGRA_VI_H__
+#define __TEGRA_VI_H__
+
+#include <linux/host1x.h>
+#include <linux/list.h>
+
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "csi.h"
+
+#define V4L2_CID_TEGRA_SYNCPT_TIMEOUT_RETRY	(V4L2_CTRL_CLASS_CAMERA | 0x1001)
+
+#define TEGRA_MIN_WIDTH		32U
+#define TEGRA_MAX_WIDTH		32768U
+#define TEGRA_MIN_HEIGHT	32U
+#define TEGRA_MAX_HEIGHT	32768U
+
+#define TEGRA_DEF_WIDTH		1920
+#define TEGRA_DEF_HEIGHT	1080
+#define TEGRA_IMAGE_FORMAT_DEF	32
+
+#define MAX_FORMAT_NUM		64
+#define SURFACE_ALIGN_BYTES	64
+
+enum tegra_vi_pg_mode {
+	TEGRA_VI_PG_DISABLED = 0,
+	TEGRA_VI_PG_DIRECT,
+	TEGRA_VI_PG_PATCH,
+};
+
+/**
+ * struct tegra_vi_ops - Tegra VI operations
+ * @vi_start_streaming: starts media pipeline, subdevice streaming, sets up
+ *		VI for capture and runs capture start and capture finish
+ *		kthreads for capturing frames to buffer and returns them back.
+ * @vi_stop_streaming: stops media pipeline and subdevice streaming and returns
+ *		back any queued buffers.
+ */
+struct tegra_vi_ops {
+	int (*vi_start_streaming)(struct vb2_queue *vq, u32 count);
+	void (*vi_stop_streaming)(struct vb2_queue *vq);
+};
+
+/**
+ * struct tegra_vi_soc - NVIDIA Tegra Video Input SoC structure
+ *
+ * @video_formats: supported video formats
+ * @nformats: total video formats
+ * @ops: vi operations
+ * @hw_revision: VI hw_revision
+ * @vi_max_channels: supported max streaming channels
+ * @vi_max_clk_hz: VI clock max frequency
+ */
+struct tegra_vi_soc {
+	const struct tegra_video_format *video_formats;
+	const unsigned int nformats;
+	const struct tegra_vi_ops *ops;
+	u32 hw_revision;
+	unsigned int vi_max_channels;
+	unsigned int vi_max_clk_hz;
+};
+
+/**
+ * struct tegra_vi - NVIDIA Tegra Video Input device structure
+ *
+ * @dev: device struct
+ * @client: host1x_client struct
+ * @iomem: register base
+ * @clk: main clock for VI block
+ * @vdd: vdd regulator for VI hardware, normally it is avdd_dsi_csi
+ * @soc: pointer to SoC data structure
+ * @ops: vi operations
+ * @vi_chans: list head for VI channels
+ */
+struct tegra_vi {
+	struct device *dev;
+	struct host1x_client client;
+	void __iomem *iomem;
+	struct clk *clk;
+	struct regulator *vdd;
+	const struct tegra_vi_soc *soc;
+	const struct tegra_vi_ops *ops;
+	struct list_head vi_chans;
+};
+
+/**
+ * struct tegra_vi_graph_entity - Entity in the video graph
+ *
+ * @asd: subdev asynchronous registration information
+ * @entity: media entity from the corresponding V4L2 subdev
+ * @subdev: V4L2 subdev
+ */
+struct tegra_vi_graph_entity {
+	struct v4l2_async_subdev asd;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+};
+
+/**
+ * struct tegra_vi_channel - Tegra video channel
+ *
+ * @list: list head for this entry
+ * @video: V4L2 video device associated with the video channel
+ * @video_lock: protects the @format and @queue fields
+ * @pad: media pad for the video device entity
+ *
+ * @vi: Tegra video input device structure
+ * @frame_start_sp: host1x syncpoint pointer to synchronize programmed capture
+ *		start condition with hardware frame start events through host1x
+ *		syncpoint counters.
+ * @mw_ack_sp: host1x syncpoint pointer to synchronize programmed memory write
+ *		ack trigger condition with hardware memory write done at end of
+ *		frame through host1x syncpoint counters.
+ * @sp_incr_lock: protects cpu syncpoint increment.
+ *
+ * @kthread_start_capture: kthread to start capture of single frame when
+ *		vb buffer is available. This thread programs VI CSI hardware
+ *		for single frame capture and waits for frame start event from
+ *		the hardware. On receiving frame start event, it wakes up
+ *		kthread_finish_capture thread to wait for finishing frame data
+ *		write to the memory. In case of missing frame start event, this
+ *		thread returns buffer back to vb with VB2_BUF_STATE_ERROR.
+ * @start_wait: waitqueue for starting frame capture when buffer is available.
+ * @kthread_finish_capture: kthread to finish the buffer capture and return to.
+ *		This thread is woken up by kthread_start_capture on receiving
+ *		frame start event from the hardware and this thread waits for
+ *		MW_ACK_DONE event which indicates completion of writing frame
+ *		data to the memory. On receiving MW_ACK_DONE event, buffer is
+ *		returned back to vb with VB2_BUF_STATE_DONE and in case of
+ *		missing MW_ACK_DONE event, buffer is returned back to vb with
+ *		VB2_BUF_STATE_ERROR.
+ * @done_wait: waitqueue for finishing capture data writes to memory.
+ *
+ * @format: active V4L2 pixel format
+ * @fmtinfo: format information corresponding to the active @format
+ * @queue: vb2 buffers queue
+ * @sequence: V4L2 buffers sequence number
+ *
+ * @capture: list of queued buffers for capture
+ * @start_lock: protects the capture queued list
+ * @done: list of capture done queued buffers
+ * @done_lock: protects the capture done queue list
+ *
+ * @portnos: VI channel port numbers
+ * @totalports: total number of ports used for this channel
+ * @numgangports: number of ports combined together as a gang for capture
+ * @of_node: device node of VI channel
+ *
+ * @ctrl_handler: V4L2 control handler of this video channel
+ * @syncpt_timeout_retry: syncpt timeout retry count for the capture
+ * @fmts_bitmap: a bitmap for supported formats matching v4l2 subdev formats
+ * @tpg_fmts_bitmap: a bitmap for supported TPG formats
+ * @pg_mode: test pattern generator mode (disabled/direct/patch)
+ * @notifier: V4L2 asynchronous subdevs notifier
+ */
+struct tegra_vi_channel {
+	struct list_head list;
+	struct video_device video;
+	/* protects the @format and @queue fields */
+	struct mutex video_lock;
+	struct media_pad pad;
+
+	struct tegra_vi *vi;
+	struct host1x_syncpt *frame_start_sp[GANG_PORTS_MAX];
+	struct host1x_syncpt *mw_ack_sp[GANG_PORTS_MAX];
+	/* protects the cpu syncpoint increment */
+	spinlock_t sp_incr_lock[GANG_PORTS_MAX];
+
+	struct task_struct *kthread_start_capture;
+	wait_queue_head_t start_wait;
+	struct task_struct *kthread_finish_capture;
+	wait_queue_head_t done_wait;
+
+	struct v4l2_pix_format format;
+	const struct tegra_video_format *fmtinfo;
+	struct vb2_queue queue;
+	u32 sequence;
+
+	struct list_head capture;
+	/* protects the capture queued list */
+	spinlock_t start_lock;
+	struct list_head done;
+	/* protects the capture done queue list */
+	spinlock_t done_lock;
+
+	unsigned char portnos[GANG_PORTS_MAX];
+	u8 totalports;
+	u8 numgangports;
+	struct device_node *of_node;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	unsigned int syncpt_timeout_retry;
+	DECLARE_BITMAP(fmts_bitmap, MAX_FORMAT_NUM);
+	DECLARE_BITMAP(tpg_fmts_bitmap, MAX_FORMAT_NUM);
+	enum tegra_vi_pg_mode pg_mode;
+
+	struct v4l2_async_notifier notifier;
+};
+
+/**
+ * struct tegra_channel_buffer - video channel buffer
+ *
+ * @buf: vb2 buffer base object
+ * @queue: buffer list entry in the channel queued buffers list
+ * @chan: channel that uses the buffer
+ * @addr: Tegra IOVA buffer address for VI output
+ * @mw_ack_sp_thresh: MW_ACK_DONE syncpoint threshold corresponding
+ *		      to the capture buffer.
+ */
+struct tegra_channel_buffer {
+	struct vb2_v4l2_buffer buf;
+	struct list_head queue;
+	struct tegra_vi_channel *chan;
+	dma_addr_t addr;
+	u32 mw_ack_sp_thresh[GANG_PORTS_MAX];
+};
+
+/*
+ * VI channel input data type enum.
+ * These data type enum value gets programmed into corresponding Tegra VI
+ * channel register bits.
+ */
+enum tegra_image_dt {
+	TEGRA_IMAGE_DT_YUV420_8 = 24,
+	TEGRA_IMAGE_DT_YUV420_10,
+
+	TEGRA_IMAGE_DT_YUV420CSPS_8 = 28,
+	TEGRA_IMAGE_DT_YUV420CSPS_10,
+	TEGRA_IMAGE_DT_YUV422_8,
+	TEGRA_IMAGE_DT_YUV422_10,
+	TEGRA_IMAGE_DT_RGB444,
+	TEGRA_IMAGE_DT_RGB555,
+	TEGRA_IMAGE_DT_RGB565,
+	TEGRA_IMAGE_DT_RGB666,
+	TEGRA_IMAGE_DT_RGB888,
+
+	TEGRA_IMAGE_DT_RAW6 = 40,
+	TEGRA_IMAGE_DT_RAW7,
+	TEGRA_IMAGE_DT_RAW8,
+	TEGRA_IMAGE_DT_RAW10,
+	TEGRA_IMAGE_DT_RAW12,
+	TEGRA_IMAGE_DT_RAW14,
+};
+
+/**
+ * struct tegra_video_format - Tegra video format description
+ *
+ * @img_dt: image data type
+ * @bit_width: format width in bits per component
+ * @code: media bus format code
+ * @bpp: bytes per pixel (when stored in memory)
+ * @img_fmt: image format
+ * @fourcc: V4L2 pixel format FCC identifier
+ */
+struct tegra_video_format {
+	enum tegra_image_dt img_dt;
+	unsigned int bit_width;
+	unsigned int code;
+	unsigned int bpp;
+	u32 img_fmt;
+	u32 fourcc;
+};
+
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+extern const struct tegra_vi_soc tegra210_vi_soc;
+#endif
+
+struct v4l2_subdev *
+tegra_channel_get_remote_csi_subdev(struct tegra_vi_channel *chan);
+struct v4l2_subdev *
+tegra_channel_get_remote_source_subdev(struct tegra_vi_channel *chan);
+int tegra_channel_set_stream(struct tegra_vi_channel *chan, bool on);
+void tegra_channel_release_buffers(struct tegra_vi_channel *chan,
+				   enum vb2_buffer_state state);
+void tegra_channels_cleanup(struct tegra_vi *vi);
+#endif
diff --git a/drivers/staging/media/tegra-video/video.c b/drivers/staging/media/tegra-video/video.c
new file mode 100644
index 000000000..d966b3195
--- /dev/null
+++ b/drivers/staging/media/tegra-video/video.c
@@ -0,0 +1,176 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#include <linux/host1x.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <media/v4l2-event.h>
+
+#include "video.h"
+
+static void tegra_v4l2_dev_release(struct v4l2_device *v4l2_dev)
+{
+	struct tegra_video_device *vid;
+
+	vid = container_of(v4l2_dev, struct tegra_video_device, v4l2_dev);
+
+	/* cleanup channels here as all video device nodes are released */
+	tegra_channels_cleanup(vid->vi);
+
+	v4l2_device_unregister(v4l2_dev);
+	media_device_unregister(&vid->media_dev);
+	media_device_cleanup(&vid->media_dev);
+	kfree(vid);
+}
+
+static void tegra_v4l2_dev_notify(struct v4l2_subdev *sd,
+				  unsigned int notification, void *arg)
+{
+	struct tegra_vi_channel *chan;
+	const struct v4l2_event *ev = arg;
+
+	if (notification != V4L2_DEVICE_NOTIFY_EVENT)
+		return;
+
+	chan = v4l2_get_subdev_hostdata(sd);
+	v4l2_event_queue(&chan->video, arg);
+	if (ev->type == V4L2_EVENT_SOURCE_CHANGE && vb2_is_streaming(&chan->queue))
+		vb2_queue_error(&chan->queue);
+}
+
+static int host1x_video_probe(struct host1x_device *dev)
+{
+	struct tegra_video_device *vid;
+	int ret;
+
+	vid = kzalloc(sizeof(*vid), GFP_KERNEL);
+	if (!vid)
+		return -ENOMEM;
+
+	dev_set_drvdata(&dev->dev, vid);
+
+	vid->media_dev.dev = &dev->dev;
+	strscpy(vid->media_dev.model, "NVIDIA Tegra Video Input Device",
+		sizeof(vid->media_dev.model));
+
+	media_device_init(&vid->media_dev);
+	ret = media_device_register(&vid->media_dev);
+	if (ret < 0) {
+		dev_err(&dev->dev,
+			"failed to register media device: %d\n", ret);
+		goto cleanup;
+	}
+
+	vid->v4l2_dev.mdev = &vid->media_dev;
+	vid->v4l2_dev.release = tegra_v4l2_dev_release;
+	vid->v4l2_dev.notify = tegra_v4l2_dev_notify;
+	ret = v4l2_device_register(&dev->dev, &vid->v4l2_dev);
+	if (ret < 0) {
+		dev_err(&dev->dev,
+			"V4L2 device registration failed: %d\n", ret);
+		goto unregister_media;
+	}
+
+	ret = host1x_device_init(dev);
+	if (ret < 0)
+		goto unregister_v4l2;
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
+		/*
+		 * Both vi and csi channels are available now.
+		 * Register v4l2 nodes and create media links for TPG.
+		 */
+		ret = tegra_v4l2_nodes_setup_tpg(vid);
+		if (ret < 0) {
+			dev_err(&dev->dev,
+				"failed to setup tpg graph: %d\n", ret);
+			goto device_exit;
+		}
+	}
+
+	return 0;
+
+device_exit:
+	host1x_device_exit(dev);
+	/* vi exit ops does not clean channels, so clean them here */
+	tegra_channels_cleanup(vid->vi);
+unregister_v4l2:
+	v4l2_device_unregister(&vid->v4l2_dev);
+unregister_media:
+	media_device_unregister(&vid->media_dev);
+cleanup:
+	media_device_cleanup(&vid->media_dev);
+	kfree(vid);
+	return ret;
+}
+
+static int host1x_video_remove(struct host1x_device *dev)
+{
+	struct tegra_video_device *vid = dev_get_drvdata(&dev->dev);
+
+	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
+		tegra_v4l2_nodes_cleanup_tpg(vid);
+
+	host1x_device_exit(dev);
+
+	/* This calls v4l2_dev release callback on last reference */
+	v4l2_device_put(&vid->v4l2_dev);
+
+	return 0;
+}
+
+static const struct of_device_id host1x_video_subdevs[] = {
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+	{ .compatible = "nvidia,tegra210-csi", },
+	{ .compatible = "nvidia,tegra210-vi", },
+#endif
+	{ }
+};
+
+static struct host1x_driver host1x_video_driver = {
+	.driver = {
+		.name = "tegra-video",
+	},
+	.probe = host1x_video_probe,
+	.remove = host1x_video_remove,
+	.subdevs = host1x_video_subdevs,
+};
+
+static struct platform_driver * const drivers[] = {
+	&tegra_csi_driver,
+	&tegra_vi_driver,
+};
+
+static int __init host1x_video_init(void)
+{
+	int err;
+
+	err = host1x_driver_register(&host1x_video_driver);
+	if (err < 0)
+		return err;
+
+	err = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
+	if (err < 0)
+		goto unregister_host1x;
+
+	return 0;
+
+unregister_host1x:
+	host1x_driver_unregister(&host1x_video_driver);
+	return err;
+}
+module_init(host1x_video_init);
+
+static void __exit host1x_video_exit(void)
+{
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
+	host1x_driver_unregister(&host1x_video_driver);
+}
+module_exit(host1x_video_exit);
+
+MODULE_AUTHOR("Sowjanya Komatineni <skomatineni@nvidia.com>");
+MODULE_DESCRIPTION("NVIDIA Tegra Host1x Video driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/tegra-video/video.h b/drivers/staging/media/tegra-video/video.h
new file mode 100644
index 000000000..fadaf2189
--- /dev/null
+++ b/drivers/staging/media/tegra-video/video.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#ifndef __TEGRA_VIDEO_H__
+#define __TEGRA_VIDEO_H__
+
+#include <linux/host1x.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+
+#include "vi.h"
+#include "csi.h"
+
+struct tegra_video_device {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct tegra_vi *vi;
+	struct tegra_csi *csi;
+};
+
+int tegra_v4l2_nodes_setup_tpg(struct tegra_video_device *vid);
+void tegra_v4l2_nodes_cleanup_tpg(struct tegra_video_device *vid);
+
+extern struct platform_driver tegra_vi_driver;
+extern struct platform_driver tegra_csi_driver;
+#endif
diff --git a/drivers/staging/most/Documentation/ABI/sysfs-class-most.txt b/drivers/staging/most/Documentation/ABI/sysfs-class-most.txt
new file mode 100644
index 000000000..48aa45acc
--- /dev/null
+++ b/drivers/staging/most/Documentation/ABI/sysfs-class-most.txt
@@ -0,0 +1,315 @@
+What:		/sys/class/most/mostcore/aims
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		List of AIMs that have been loaded.
+Users:
+
+What:		/sys/class/most/mostcore/aims/<aim>/add_link
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to establish a connection of a channel and the
+		current AIM.
+Users:
+
+What:		/sys/class/most/mostcore/aims/<aim>/remove_link
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to remove a connected channel from the
+		current AIM.
+Users:
+
+What:		/sys/class/most/mostcore/devices
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		List of attached MOST interfaces.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/description
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Provides information about the interface type and the physical
+		location of the device. Hardware attached via USB, for instance,
+		might return <usb_device 1-1.1:1.0>
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/interface
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the type of peripheral interface the current device
+		uses.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		If the network interface controller is attached via USB, a dci
+		directory is created that allows applications to use the
+		controller's direct communication interface (DCI) to exchange
+		information.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/arb_address
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to set an arbitrary DCI register address an
+		application wants to read from or write to.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/arb_value
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to read from or write to the arbitrary DCI register
+		whose address is stored in arb_address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_eui48_hi
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MAC address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_eui48_lo
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MAC address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_eui48_mi
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MAC address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_filter
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MEP filter address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_hash0
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MEP hash table.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_hash1
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MEP hash table.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_hash2
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MEP hash table.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/mep_hash3
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is used to check and configure the MEP hash table.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/ni_state
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the current network interface state.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/node_address
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the current node address.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/node_position
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the current node position.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/packet_bandwidth
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the configured packet bandwidth.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/dci/sync_ep
+Date:		June 2016
+KernelVersion:	4.9
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Triggers the controller's synchronization process for a certain
+		endpoint.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		For every channel of the device a directory is created, whose
+		name is dictated by the HDM. This enables an application to
+		collect information about the channel's capabilities and
+		configure it.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/available_datatypes
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the data types the current channel can transport.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/available_directions
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the directions the current channel is capable of.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/number_of_packet_buffers
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the number of packet buffers the current channel can
+		handle.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/number_of_stream_buffers
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the number of streaming buffers the current channel can
+		handle.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/size_of_packet_buffer
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the size of a packet buffer the current channel can
+		handle.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/size_of_stream_buffer
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates the size of a streaming buffer the current channel can
+		handle.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_number_of_buffers
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the number of buffers of the current channel.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_buffer_size
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the size of a buffer of the current channel.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_direction
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the direction of the current channel.
+		The following strings will be accepted:
+			'dir_tx',
+			'dir_rx'
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_datatype
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the data type of the current channel.
+		The following strings will be accepted:
+			'control',
+			'async',
+			'sync',
+			'isoc_avp'
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_subbuffer_size
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the subbuffer size of the current channel.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/set_packets_per_xact
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		This is to configure the number of packets per transaction of
+		the current channel. This is only needed network interface
+		controller is attached via USB.
+Users:
+
+What:		/sys/class/most/mostcore/devices/<mdev>/<channel>/channel_starving
+Date:		June 2015
+KernelVersion:	4.3
+Contact:	Christian Gromm <christian.gromm@microchip.com>
+Description:
+		Indicates whether current channel ran out of buffers.
+Users:
diff --git a/drivers/staging/most/Documentation/driver_usage.txt b/drivers/staging/most/Documentation/driver_usage.txt
new file mode 100644
index 000000000..2fa8dea1d
--- /dev/null
+++ b/drivers/staging/most/Documentation/driver_usage.txt
@@ -0,0 +1,237 @@
+
+		Section 1 Overview
+
+The Media Oriented Systems Transport (MOST) driver gives Linux applications
+access a MOST network: The Automotive Information Backbone and the de-facto
+standard for high-bandwidth automotive multimedia networking.
+
+MOST defines the protocol, hardware and software layers necessary to allow
+for the efficient and low-cost transport of control, real-time and packet
+data using a single medium (physical layer). Media currently in use are
+fiber optics, unshielded twisted pair cables (UTP) and coax cables. MOST
+also supports various speed grades up to 150 Mbps.
+For more information on MOST, visit the MOST Cooperation website:
+www.mostcooperation.com.
+
+Cars continue to evolve into sophisticated consumer electronics platforms,
+increasing the demand for reliable and simple solutions to support audio,
+video and data communications. MOST can be used to connect multiple
+consumer devices via optical or electrical physical layers directly to one
+another or in a network configuration. As a synchronous network, MOST
+provides excellent Quality of Service and seamless connectivity for
+audio/video streaming. Therefore, the driver perfectly fits to the mission
+of Automotive Grade Linux to create open source software solutions for
+automotive applications.
+
+The MOST driver uses module stacking to divide the associated modules into
+three layers. From bottom up these layers are: the adapter layer, the core
+layer and the application layer. The core layer implements the MOST
+subsystem and consists basically of the module core.c and its API. It
+registers the MOST bus with the kernel's device model, handles the data
+routing through all three layers, the configuration of the driver, the
+representation of the configuration interface in sysfs and the buffer
+management.
+
+For each of the other two layers a set of modules is provided. Those can be
+arbitrarily combined with the core to meet the connectivity of the desired
+system architecture.
+
+A module of the adapter layer is basically a device driver for a different
+subsystem. It is registered with the core to connect the MOST subsystem to
+the attached network interface controller hardware. Hence, a given module
+of this layer is designed to handle exactly one of the peripheral
+interfaces (e.g. USB, MediaLB, I2C) the hardware provides.
+
+A module of the application layer is referred to as a core component,
+which kind of extends the core by providing connectivity to the user space.
+Applications, then, can access a MOST network via character devices, an
+ALSA soundcard, a Network adapter or a V4L2 capture device.
+
+To physically access MOST, an Intelligent Network Interface Controller
+(INIC) is needed. For more information on available controllers visit:
+www.microchip.com
+
+
+
+		Section 1.1 Adapter Layer
+
+The adapter layer contains a pool of device drivers. For each peripheral
+interface the hardware supports there is one suitable module that handles
+the interface. Adapter drivers encapsulate the peripheral interface
+specific knowledge of the MOST driver stack and provide an easy way of
+extending the number of supported interfaces. Currently the following
+interfaces are available:
+
+	1) MediaLB (DIM2)
+	   Host wants to communicate with hardware via MediaLB.
+
+	2) I2C
+	   Host wants to communicate with the hardware via I2C.
+
+	3) USB
+	   Host wants to communicate with the hardware via USB.
+
+Once an adapter driver recognizes a MOST device being attached, it
+registers it with the core, which, in turn, assigns the necessary members
+of the embedded struct device (e.g. the bus this device belongs to and
+attribute groups) and registers it with the kernel's device model.
+
+
+		Section 1.2 Core Layer
+
+This layer implements the MOST subsystem. It contains the core module and
+the header file most.h that exposes the API of the core. When inserted in
+the kernel, it registers the MOST bus_type with the kernel's device model
+and registers itself as a device driver for this bus. Besides these meta
+tasks the core populates the configuration directory for a registered MOST
+device (represented by struct most_interface) in sysfs and processes the
+configuration of the device's interface. The core layer also handles the
+buffer management and the data/message routing.
+
+
+		Section 1.3 Application Layer
+
+This layer contains a pool of device drivers that are components of the
+core designed to make up the userspace experience of the MOST driver stack.
+Depending on how an application is meant to interface the driver, one or
+more modules of this pool can be registered with the core. Currently the
+following components are available
+
+	1) Character Device
+	   Userspace can access the driver by means of character devices.
+
+	2) Networking
+	   Standard networking applications (e.g. iperf) can by used to access
+	   the driver via the networking subsystem.
+
+	3) Video4Linux (v4l2)
+	   Standard video applications (e.g. VLC) can by used to access the
+	   driver via the V4L subsystem.
+
+	4) Advanced Linux Sound Architecture (ALSA)
+	   Standard sound applications (e.g. aplay, arecord, audacity) can by
+	   used to access the driver via the ALSA subsystem.
+
+
+		Section 2 Usage of the MOST Driver
+
+		Section 2.1 Configuration and Data Link
+
+The driver is to be configured via configfs. Each loaded component kernel
+object (see section 1.3) registers a subsystem with configfs, which is used to
+configure and establish communication pathways (links) to attached devices on
+the bus. To do so, the user has to descend into the component's configuration
+directory and create a new directory (child config itmes). The name of this
+directory will be used as a reference for the link and it will contain the
+following attributes:
+
+	- buffer_size
+	  configure the buffer size for this channel
+	- subbuffer_size
+	  configure the sub-buffer size for this channel (needed for
+	  synchronous and isochrnous data)
+	- num_buffers
+	  configure number of buffers used for this channel
+	- datatype
+	  configure type of data that will travel over this channel
+	- direction
+	  configure whether this link will be an input or output
+	- dbr_size
+	  configure DBR data buffer size (this is used for MediaLB communication
+	  only)
+	- packets_per_xact
+	  configure the number of packets that will be collected from the
+	  network before being transmitted via USB (this is used for USB
+	  communication only)
+	- device
+	  name of the device the link is to be attached to
+	- channel
+	  name of the channel the link is to be attached to
+	- comp_params
+	  pass parameters needed by some components
+	- create_link
+	  write '1' to this attribute to trigger the creation of the link. In
+	  case of speculative configuration, the creation is post-poned until
+	  a physical device is being attached to the bus.
+	- destroy_link
+	  write '1' to this attribute to destroy an already established link
+
+
+See ABI/sysfs-bus-most.txt and ABI/configfs-most.txt
+
+
+		Section 2.2 Configure a Sound Card
+
+Setting up synchronous channels to be mapped as an ALSA sound adapter is a two
+step process. Firstly, a directory (child config group) has to be created
+inside the most_sound's configuration directory. This adapter dir will
+represent the sound adapter. The name of the directory is for user reference
+only and has no further influence, as all sound adapters will be given a static
+name in ALSA. The sound adapter will have the following attribute:
+
+	- create_card
+	  write '1' to this attribute to trigger the registration of the card
+	  with the ALSA subsystem.
+	  In case of speculative configuration, the creation is post-poned
+	  until a physical device is being attached to the bus.
+
+Secondly, links will have to be created inside the adapter dir as described in
+section 2.1. These links will become the PCM devices of the sound card. The
+name of a PCM device will be inherited from the directory name. When all
+channels have been configured and created, the sound card itself can be created
+by writing '1' to the create_card attribute.
+
+The sound component needs an additional parameter to determine the audio
+resolution that is going to be used.
+The following audio formats are available:
+
+	- "1x8" (Mono)
+	- "2x16" (16-bit stereo)
+	- "2x24" (24-bit stereo)
+	- "2x32" (32-bit stereo)
+	- "6x16" (16-bit surround 5.1)
+
+The resolution string has to be written to the link directory's comp_params
+attribute.
+
+		Section 2.3 USB Padding
+
+When transceiving synchronous or isochronous data, the number of packets
+per USB transaction and the sub-buffer size need to be configured. These
+values are needed for the driver to process buffer padding, as expected by
+hardware, which is for performance optimization purposes of the USB
+transmission.
+
+When transmitting synchronous data the allocated channel width needs to be
+written to 'subbuffer_size'. Additionally, the number of MOST frames that
+should travel to the host within one USB transaction need to be written to
+'packets_per_xact'.
+
+The driver, then, calculates the synchronous threshold as follows:
+
+	frame_size = subbuffer_size * packets_per_xact
+
+In case 'packets_per_xact' is set to 0xFF the maximum number of packets,
+allocated within one MOST frame, is calculated that fit into _one_ 512 byte
+USB full packet.
+
+	frame_size = floor(MTU_USB / bandwidth_sync) * bandwidth_sync
+
+This frame_size is the number of synchronous data within an USB
+transaction, which renders MTU_USB - frame_size bytes for padding.
+
+When transmitting isochronous AVP data the desired packet size needs to be
+written to 'subbuffer_size' and hardware will always expect two isochronous
+packets within one USB transaction. This renders
+
+	MTU_USB - (2 * subbuffer_size)
+
+bytes for padding.
+
+Note that at least (2 * subbuffer_size) bytes for isochronous data or
+(subbuffer_size * packts_per_xact) bytes for synchronous data need to
+be put in the transmission buffer and passed to the driver.
+
+Since adapter drivers are allowed to change a chosen configuration to best
+fit its constraints, it is recommended to always double check the
+configuration and read back the previously written files.
diff --git a/drivers/staging/most/Kconfig b/drivers/staging/most/Kconfig
new file mode 100644
index 000000000..6f420cbcd
--- /dev/null
+++ b/drivers/staging/most/Kconfig
@@ -0,0 +1,29 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig MOST_COMPONENTS
+	tristate "MOST support"
+	depends on HAS_DMA && CONFIGFS_FS && MOST
+	default n
+	help
+	  Say Y here if you want to enable MOST support.
+	  This driver needs at least one additional component to enable the
+	  desired access from userspace (e.g. character devices) and one that
+	  matches the network controller's hardware interface (e.g. USB).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called most_core.
+
+	  If in doubt, say N here.
+
+
+
+if MOST_COMPONENTS
+
+source "drivers/staging/most/net/Kconfig"
+
+source "drivers/staging/most/video/Kconfig"
+
+source "drivers/staging/most/dim2/Kconfig"
+
+source "drivers/staging/most/i2c/Kconfig"
+
+endif
diff --git a/drivers/staging/most/Makefile b/drivers/staging/most/Makefile
new file mode 100644
index 000000000..8b3fc5a7a
--- /dev/null
+++ b/drivers/staging/most/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_MOST_NET)	+= net/
+obj-$(CONFIG_MOST_VIDEO)	+= video/
+obj-$(CONFIG_MOST_DIM2)	+= dim2/
+obj-$(CONFIG_MOST_I2C)	+= i2c/
diff --git a/drivers/staging/most/TODO b/drivers/staging/most/TODO
new file mode 100644
index 000000000..4fa11a9d2
--- /dev/null
+++ b/drivers/staging/most/TODO
@@ -0,0 +1,8 @@
+* Get through code review with Greg Kroah-Hartman
+
+Contact:
+To:
+Christian Gromm <christian.gromm@microchip.com>
+Cc:
+Michael Fabry <Michael.Fabry@microchip.com>
+Christian Gromm <chris@engineersdelight.de>
diff --git a/drivers/staging/most/dim2/Kconfig b/drivers/staging/most/dim2/Kconfig
new file mode 100644
index 000000000..c211f0d91
--- /dev/null
+++ b/drivers/staging/most/dim2/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# MediaLB configuration
+#
+
+config MOST_DIM2
+	tristate "DIM2"
+	depends on HAS_IOMEM && OF
+
+	help
+	  Say Y here if you want to connect via MediaLB to network transceiver.
+	  This device driver is platform dependent and needs an additional
+	  platform driver to be installed. For more information contact
+	  maintainer of this driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called most_dim2.
diff --git a/drivers/staging/most/dim2/Makefile b/drivers/staging/most/dim2/Makefile
new file mode 100644
index 000000000..5f9612af3
--- /dev/null
+++ b/drivers/staging/most/dim2/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_MOST_DIM2) += most_dim2.o
+
+most_dim2-objs := dim2.o hal.o
diff --git a/drivers/staging/most/dim2/dim2.c b/drivers/staging/most/dim2/dim2.c
new file mode 100644
index 000000000..97dff82b7
--- /dev/null
+++ b/drivers/staging/most/dim2/dim2.c
@@ -0,0 +1,1106 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * dim2.c - MediaLB DIM2 Hardware Dependent Module
+ *
+ * Copyright (C) 2015-2016, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/printk.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/sched.h>
+#include <linux/kthread.h>
+#include <linux/most.h>
+#include "hal.h"
+#include "errors.h"
+#include "sysfs.h"
+
+#define DMA_CHANNELS (32 - 1)  /* channel 0 is a system channel */
+
+#define MAX_BUFFERS_PACKET      32
+#define MAX_BUFFERS_STREAMING   32
+#define MAX_BUF_SIZE_PACKET     2048
+#define MAX_BUF_SIZE_STREAMING  (8 * 1024)
+
+/*
+ * The parameter representing the number of frames per sub-buffer for
+ * synchronous channels.  Valid values: [0 .. 6].
+ *
+ * The values 0, 1, 2, 3, 4, 5, 6 represent corresponding number of frames per
+ * sub-buffer 1, 2, 4, 8, 16, 32, 64.
+ */
+static u8 fcnt = 4;  /* (1 << fcnt) frames per subbuffer */
+module_param(fcnt, byte, 0000);
+MODULE_PARM_DESC(fcnt, "Num of frames per sub-buffer for sync channels as a power of 2");
+
+static DEFINE_SPINLOCK(dim_lock);
+
+/**
+ * struct hdm_channel - private structure to keep channel specific data
+ * @name: channel name
+ * @is_initialized: identifier to know whether the channel is initialized
+ * @ch: HAL specific channel data
+ * @reset_dbr_size: reset DBR data buffer size
+ * @pending_list: list to keep MBO's before starting transfer
+ * @started_list: list to keep MBO's after starting transfer
+ * @direction: channel direction (TX or RX)
+ * @data_type: channel data type
+ */
+struct hdm_channel {
+	char name[sizeof "caNNN"];
+	bool is_initialized;
+	struct dim_channel ch;
+	u16 *reset_dbr_size;
+	struct list_head pending_list;	/* before dim_enqueue_buffer() */
+	struct list_head started_list;	/* after dim_enqueue_buffer() */
+	enum most_channel_direction direction;
+	enum most_channel_data_type data_type;
+};
+
+/*
+ * struct dim2_hdm - private structure to keep interface specific data
+ * @hch: an array of channel specific data
+ * @most_iface: most interface structure
+ * @capabilities: an array of channel capability data
+ * @io_base: I/O register base address
+ * @netinfo_task: thread to deliver network status
+ * @netinfo_waitq: waitq for the thread to sleep
+ * @deliver_netinfo: to identify whether network status received
+ * @mac_addrs: INIC mac address
+ * @link_state: network link state
+ * @atx_idx: index of async tx channel
+ */
+struct dim2_hdm {
+	struct device dev;
+	struct hdm_channel hch[DMA_CHANNELS];
+	struct most_channel_capability capabilities[DMA_CHANNELS];
+	struct most_interface most_iface;
+	char name[16 + sizeof "dim2-"];
+	void __iomem *io_base;
+	u8 clk_speed;
+	struct clk *clk;
+	struct clk *clk_pll;
+	struct task_struct *netinfo_task;
+	wait_queue_head_t netinfo_waitq;
+	int deliver_netinfo;
+	unsigned char mac_addrs[6];
+	unsigned char link_state;
+	int atx_idx;
+	struct medialb_bus bus;
+	void (*on_netinfo)(struct most_interface *most_iface,
+			   unsigned char link_state, unsigned char *addrs);
+	void (*disable_platform)(struct platform_device *pdev);
+};
+
+struct dim2_platform_data {
+	int (*enable)(struct platform_device *pdev);
+	void (*disable)(struct platform_device *pdev);
+	u8 fcnt;
+};
+
+#define iface_to_hdm(iface) container_of(iface, struct dim2_hdm, most_iface)
+
+/* Macro to identify a network status message */
+#define PACKET_IS_NET_INFO(p)  \
+	(((p)[1] == 0x18) && ((p)[2] == 0x05) && ((p)[3] == 0x0C) && \
+	 ((p)[13] == 0x3C) && ((p)[14] == 0x00) && ((p)[15] == 0x0A))
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	bool state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dim_lock, flags);
+	state = dim_get_lock_state();
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	return sysfs_emit(buf, "%s\n", state ? "locked" : "");
+}
+
+static DEVICE_ATTR_RO(state);
+
+static struct attribute *dim2_attrs[] = {
+	&dev_attr_state.attr,
+	NULL,
+};
+
+ATTRIBUTE_GROUPS(dim2);
+
+/**
+ * dimcb_on_error - callback from HAL to report miscommunication between
+ * HDM and HAL
+ * @error_id: Error ID
+ * @error_message: Error message. Some text in a free format
+ */
+void dimcb_on_error(u8 error_id, const char *error_message)
+{
+	pr_err("%s: error_id - %d, error_message - %s\n", __func__, error_id,
+	       error_message);
+}
+
+/**
+ * try_start_dim_transfer - try to transfer a buffer on a channel
+ * @hdm_ch: channel specific data
+ *
+ * Transfer a buffer from pending_list if the channel is ready
+ */
+static int try_start_dim_transfer(struct hdm_channel *hdm_ch)
+{
+	u16 buf_size;
+	struct list_head *head = &hdm_ch->pending_list;
+	struct mbo *mbo;
+	unsigned long flags;
+	struct dim_ch_state_t st;
+
+	BUG_ON(!hdm_ch);
+	BUG_ON(!hdm_ch->is_initialized);
+
+	spin_lock_irqsave(&dim_lock, flags);
+	if (list_empty(head)) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		return -EAGAIN;
+	}
+
+	if (!dim_get_channel_state(&hdm_ch->ch, &st)->ready) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		return -EAGAIN;
+	}
+
+	mbo = list_first_entry(head, struct mbo, list);
+	buf_size = mbo->buffer_length;
+
+	if (dim_dbr_space(&hdm_ch->ch) < buf_size) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		return -EAGAIN;
+	}
+
+	BUG_ON(mbo->bus_address == 0);
+	if (!dim_enqueue_buffer(&hdm_ch->ch, mbo->bus_address, buf_size)) {
+		list_del(head->next);
+		spin_unlock_irqrestore(&dim_lock, flags);
+		mbo->processed_length = 0;
+		mbo->status = MBO_E_INVAL;
+		mbo->complete(mbo);
+		return -EFAULT;
+	}
+
+	list_move_tail(head->next, &hdm_ch->started_list);
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	return 0;
+}
+
+/**
+ * deliver_netinfo_thread - thread to deliver network status to mostcore
+ * @data: private data
+ *
+ * Wait for network status and deliver it to mostcore once it is received
+ */
+static int deliver_netinfo_thread(void *data)
+{
+	struct dim2_hdm *dev = data;
+
+	while (!kthread_should_stop()) {
+		wait_event_interruptible(dev->netinfo_waitq,
+					 dev->deliver_netinfo ||
+					 kthread_should_stop());
+
+		if (dev->deliver_netinfo) {
+			dev->deliver_netinfo--;
+			if (dev->on_netinfo) {
+				dev->on_netinfo(&dev->most_iface,
+						dev->link_state,
+						dev->mac_addrs);
+			}
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * retrieve_netinfo - retrieve network status from received buffer
+ * @dev: private data
+ * @mbo: received MBO
+ *
+ * Parse the message in buffer and get node address, link state, MAC address.
+ * Wake up a thread to deliver this status to mostcore
+ */
+static void retrieve_netinfo(struct dim2_hdm *dev, struct mbo *mbo)
+{
+	u8 *data = mbo->virt_address;
+
+	pr_info("Node Address: 0x%03x\n", (u16)data[16] << 8 | data[17]);
+	dev->link_state = data[18];
+	pr_info("NIState: %d\n", dev->link_state);
+	memcpy(dev->mac_addrs, data + 19, 6);
+	dev->deliver_netinfo++;
+	wake_up_interruptible(&dev->netinfo_waitq);
+}
+
+/**
+ * service_done_flag - handle completed buffers
+ * @dev: private data
+ * @ch_idx: channel index
+ *
+ * Return back the completed buffers to mostcore, using completion callback
+ */
+static void service_done_flag(struct dim2_hdm *dev, int ch_idx)
+{
+	struct hdm_channel *hdm_ch = dev->hch + ch_idx;
+	struct dim_ch_state_t st;
+	struct list_head *head;
+	struct mbo *mbo;
+	int done_buffers;
+	unsigned long flags;
+	u8 *data;
+
+	BUG_ON(!hdm_ch);
+	BUG_ON(!hdm_ch->is_initialized);
+
+	spin_lock_irqsave(&dim_lock, flags);
+
+	done_buffers = dim_get_channel_state(&hdm_ch->ch, &st)->done_buffers;
+	if (!done_buffers) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		return;
+	}
+
+	if (!dim_detach_buffers(&hdm_ch->ch, done_buffers)) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	head = &hdm_ch->started_list;
+
+	while (done_buffers) {
+		spin_lock_irqsave(&dim_lock, flags);
+		if (list_empty(head)) {
+			spin_unlock_irqrestore(&dim_lock, flags);
+			pr_crit("hard error: started_mbo list is empty whereas DIM2 has sent buffers\n");
+			break;
+		}
+
+		mbo = list_first_entry(head, struct mbo, list);
+		list_del(head->next);
+		spin_unlock_irqrestore(&dim_lock, flags);
+
+		data = mbo->virt_address;
+
+		if (hdm_ch->data_type == MOST_CH_ASYNC &&
+		    hdm_ch->direction == MOST_CH_RX &&
+		    PACKET_IS_NET_INFO(data)) {
+			retrieve_netinfo(dev, mbo);
+
+			spin_lock_irqsave(&dim_lock, flags);
+			list_add_tail(&mbo->list, &hdm_ch->pending_list);
+			spin_unlock_irqrestore(&dim_lock, flags);
+		} else {
+			if (hdm_ch->data_type == MOST_CH_CONTROL ||
+			    hdm_ch->data_type == MOST_CH_ASYNC) {
+				u32 const data_size =
+					(u32)data[0] * 256 + data[1] + 2;
+
+				mbo->processed_length =
+					min_t(u32, data_size,
+					      mbo->buffer_length);
+			} else {
+				mbo->processed_length = mbo->buffer_length;
+			}
+			mbo->status = MBO_SUCCESS;
+			mbo->complete(mbo);
+		}
+
+		done_buffers--;
+	}
+}
+
+static struct dim_channel **get_active_channels(struct dim2_hdm *dev,
+						struct dim_channel **buffer)
+{
+	int idx = 0;
+	int ch_idx;
+
+	for (ch_idx = 0; ch_idx < DMA_CHANNELS; ch_idx++) {
+		if (dev->hch[ch_idx].is_initialized)
+			buffer[idx++] = &dev->hch[ch_idx].ch;
+	}
+	buffer[idx++] = NULL;
+
+	return buffer;
+}
+
+static irqreturn_t dim2_mlb_isr(int irq, void *_dev)
+{
+	struct dim2_hdm *dev = _dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dim_lock, flags);
+	dim_service_mlb_int_irq();
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	if (dev->atx_idx >= 0 && dev->hch[dev->atx_idx].is_initialized)
+		while (!try_start_dim_transfer(dev->hch + dev->atx_idx))
+			continue;
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t dim2_task_irq(int irq, void *_dev)
+{
+	struct dim2_hdm *dev = _dev;
+	unsigned long flags;
+	int ch_idx;
+
+	for (ch_idx = 0; ch_idx < DMA_CHANNELS; ch_idx++) {
+		if (!dev->hch[ch_idx].is_initialized)
+			continue;
+
+		spin_lock_irqsave(&dim_lock, flags);
+		dim_service_channel(&dev->hch[ch_idx].ch);
+		spin_unlock_irqrestore(&dim_lock, flags);
+
+		service_done_flag(dev, ch_idx);
+		while (!try_start_dim_transfer(dev->hch + ch_idx))
+			continue;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * dim2_ahb_isr - interrupt service routine
+ * @irq: irq number
+ * @_dev: private data
+ *
+ * Acknowledge the interrupt and service each initialized channel,
+ * if needed, in task context.
+ */
+static irqreturn_t dim2_ahb_isr(int irq, void *_dev)
+{
+	struct dim2_hdm *dev = _dev;
+	struct dim_channel *buffer[DMA_CHANNELS + 1];
+	unsigned long flags;
+
+	spin_lock_irqsave(&dim_lock, flags);
+	dim_service_ahb_int_irq(get_active_channels(dev, buffer));
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	return IRQ_WAKE_THREAD;
+}
+
+/**
+ * complete_all_mbos - complete MBO's in a list
+ * @head: list head
+ *
+ * Delete all the entries in list and return back MBO's to mostcore using
+ * completion call back.
+ */
+static void complete_all_mbos(struct list_head *head)
+{
+	unsigned long flags;
+	struct mbo *mbo;
+
+	for (;;) {
+		spin_lock_irqsave(&dim_lock, flags);
+		if (list_empty(head)) {
+			spin_unlock_irqrestore(&dim_lock, flags);
+			break;
+		}
+
+		mbo = list_first_entry(head, struct mbo, list);
+		list_del(head->next);
+		spin_unlock_irqrestore(&dim_lock, flags);
+
+		mbo->processed_length = 0;
+		mbo->status = MBO_E_CLOSE;
+		mbo->complete(mbo);
+	}
+}
+
+/**
+ * configure_channel - initialize a channel
+ * @most_iface: interface the channel belongs to
+ * @ch_idx: channel index to be configured
+ * @ccfg: structure that holds the configuration information
+ *
+ * Receives configuration information from mostcore and initialize
+ * the corresponding channel. Return 0 on success, negative on failure.
+ */
+static int configure_channel(struct most_interface *most_iface, int ch_idx,
+			     struct most_channel_config *ccfg)
+{
+	struct dim2_hdm *dev = iface_to_hdm(most_iface);
+	bool const is_tx = ccfg->direction == MOST_CH_TX;
+	u16 const sub_size = ccfg->subbuffer_size;
+	u16 const buf_size = ccfg->buffer_size;
+	u16 new_size;
+	unsigned long flags;
+	u8 hal_ret;
+	int const ch_addr = ch_idx * 2 + 2;
+	struct hdm_channel *const hdm_ch = dev->hch + ch_idx;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= DMA_CHANNELS);
+
+	if (hdm_ch->is_initialized)
+		return -EPERM;
+
+	/* do not reset if the property was set by user, see poison_channel */
+	hdm_ch->reset_dbr_size = ccfg->dbr_size ? NULL : &ccfg->dbr_size;
+
+	/* zero value is default dbr_size, see dim2 hal */
+	hdm_ch->ch.dbr_size = ccfg->dbr_size;
+
+	switch (ccfg->data_type) {
+	case MOST_CH_CONTROL:
+		new_size = dim_norm_ctrl_async_buffer_size(buf_size);
+		if (new_size == 0) {
+			pr_err("%s: too small buffer size\n", hdm_ch->name);
+			return -EINVAL;
+		}
+		ccfg->buffer_size = new_size;
+		if (new_size != buf_size)
+			pr_warn("%s: fixed buffer size (%d -> %d)\n",
+				hdm_ch->name, buf_size, new_size);
+		spin_lock_irqsave(&dim_lock, flags);
+		hal_ret = dim_init_control(&hdm_ch->ch, is_tx, ch_addr,
+					   is_tx ? new_size * 2 : new_size);
+		break;
+	case MOST_CH_ASYNC:
+		new_size = dim_norm_ctrl_async_buffer_size(buf_size);
+		if (new_size == 0) {
+			pr_err("%s: too small buffer size\n", hdm_ch->name);
+			return -EINVAL;
+		}
+		ccfg->buffer_size = new_size;
+		if (new_size != buf_size)
+			pr_warn("%s: fixed buffer size (%d -> %d)\n",
+				hdm_ch->name, buf_size, new_size);
+		spin_lock_irqsave(&dim_lock, flags);
+		hal_ret = dim_init_async(&hdm_ch->ch, is_tx, ch_addr,
+					 is_tx ? new_size * 2 : new_size);
+		break;
+	case MOST_CH_ISOC:
+		new_size = dim_norm_isoc_buffer_size(buf_size, sub_size);
+		if (new_size == 0) {
+			pr_err("%s: invalid sub-buffer size or too small buffer size\n",
+			       hdm_ch->name);
+			return -EINVAL;
+		}
+		ccfg->buffer_size = new_size;
+		if (new_size != buf_size)
+			pr_warn("%s: fixed buffer size (%d -> %d)\n",
+				hdm_ch->name, buf_size, new_size);
+		spin_lock_irqsave(&dim_lock, flags);
+		hal_ret = dim_init_isoc(&hdm_ch->ch, is_tx, ch_addr, sub_size);
+		break;
+	case MOST_CH_SYNC:
+		new_size = dim_norm_sync_buffer_size(buf_size, sub_size);
+		if (new_size == 0) {
+			pr_err("%s: invalid sub-buffer size or too small buffer size\n",
+			       hdm_ch->name);
+			return -EINVAL;
+		}
+		ccfg->buffer_size = new_size;
+		if (new_size != buf_size)
+			pr_warn("%s: fixed buffer size (%d -> %d)\n",
+				hdm_ch->name, buf_size, new_size);
+		spin_lock_irqsave(&dim_lock, flags);
+		hal_ret = dim_init_sync(&hdm_ch->ch, is_tx, ch_addr, sub_size);
+		break;
+	default:
+		pr_err("%s: configure failed, bad channel type: %d\n",
+		       hdm_ch->name, ccfg->data_type);
+		return -EINVAL;
+	}
+
+	if (hal_ret != DIM_NO_ERROR) {
+		spin_unlock_irqrestore(&dim_lock, flags);
+		pr_err("%s: configure failed (%d), type: %d, is_tx: %d\n",
+		       hdm_ch->name, hal_ret, ccfg->data_type, (int)is_tx);
+		return -ENODEV;
+	}
+
+	hdm_ch->data_type = ccfg->data_type;
+	hdm_ch->direction = ccfg->direction;
+	hdm_ch->is_initialized = true;
+
+	if (hdm_ch->data_type == MOST_CH_ASYNC &&
+	    hdm_ch->direction == MOST_CH_TX &&
+	    dev->atx_idx < 0)
+		dev->atx_idx = ch_idx;
+
+	spin_unlock_irqrestore(&dim_lock, flags);
+	ccfg->dbr_size = hdm_ch->ch.dbr_size;
+
+	return 0;
+}
+
+/**
+ * enqueue - enqueue a buffer for data transfer
+ * @most_iface: intended interface
+ * @ch_idx: ID of the channel the buffer is intended for
+ * @mbo: pointer to the buffer object
+ *
+ * Push the buffer into pending_list and try to transfer one buffer from
+ * pending_list. Return 0 on success, negative on failure.
+ */
+static int enqueue(struct most_interface *most_iface, int ch_idx,
+		   struct mbo *mbo)
+{
+	struct dim2_hdm *dev = iface_to_hdm(most_iface);
+	struct hdm_channel *hdm_ch = dev->hch + ch_idx;
+	unsigned long flags;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= DMA_CHANNELS);
+
+	if (!hdm_ch->is_initialized)
+		return -EPERM;
+
+	if (mbo->bus_address == 0)
+		return -EFAULT;
+
+	spin_lock_irqsave(&dim_lock, flags);
+	list_add_tail(&mbo->list, &hdm_ch->pending_list);
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	(void)try_start_dim_transfer(hdm_ch);
+
+	return 0;
+}
+
+/**
+ * request_netinfo - triggers retrieving of network info
+ * @most_iface: pointer to the interface
+ * @ch_idx: corresponding channel ID
+ * @on_netinfo: call-back used to deliver network status to mostcore
+ *
+ * Send a command to INIC which triggers retrieving of network info by means of
+ * "Message exchange over MDP/MEP". Return 0 on success, negative on failure.
+ */
+static void request_netinfo(struct most_interface *most_iface, int ch_idx,
+			    void (*on_netinfo)(struct most_interface *,
+					       unsigned char, unsigned char *))
+{
+	struct dim2_hdm *dev = iface_to_hdm(most_iface);
+	struct mbo *mbo;
+	u8 *data;
+
+	dev->on_netinfo = on_netinfo;
+	if (!on_netinfo)
+		return;
+
+	if (dev->atx_idx < 0) {
+		pr_err("Async Tx Not initialized\n");
+		return;
+	}
+
+	mbo = most_get_mbo(&dev->most_iface, dev->atx_idx, NULL);
+	if (!mbo)
+		return;
+
+	mbo->buffer_length = 5;
+
+	data = mbo->virt_address;
+
+	data[0] = 0x00; /* PML High byte */
+	data[1] = 0x03; /* PML Low byte */
+	data[2] = 0x02; /* PMHL */
+	data[3] = 0x08; /* FPH */
+	data[4] = 0x40; /* FMF (FIFO cmd msg - Triggers NAOverMDP) */
+
+	most_submit_mbo(mbo);
+}
+
+/**
+ * poison_channel - poison buffers of a channel
+ * @most_iface: pointer to the interface the channel to be poisoned belongs to
+ * @ch_idx: corresponding channel ID
+ *
+ * Destroy a channel and complete all the buffers in both started_list &
+ * pending_list. Return 0 on success, negative on failure.
+ */
+static int poison_channel(struct most_interface *most_iface, int ch_idx)
+{
+	struct dim2_hdm *dev = iface_to_hdm(most_iface);
+	struct hdm_channel *hdm_ch = dev->hch + ch_idx;
+	unsigned long flags;
+	u8 hal_ret;
+	int ret = 0;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= DMA_CHANNELS);
+
+	if (!hdm_ch->is_initialized)
+		return -EPERM;
+
+	spin_lock_irqsave(&dim_lock, flags);
+	hal_ret = dim_destroy_channel(&hdm_ch->ch);
+	hdm_ch->is_initialized = false;
+	if (ch_idx == dev->atx_idx)
+		dev->atx_idx = -1;
+	spin_unlock_irqrestore(&dim_lock, flags);
+	if (hal_ret != DIM_NO_ERROR) {
+		pr_err("HAL Failed to close channel %s\n", hdm_ch->name);
+		ret = -EFAULT;
+	}
+
+	complete_all_mbos(&hdm_ch->started_list);
+	complete_all_mbos(&hdm_ch->pending_list);
+	if (hdm_ch->reset_dbr_size)
+		*hdm_ch->reset_dbr_size = 0;
+
+	return ret;
+}
+
+static void *dma_alloc(struct mbo *mbo, u32 size)
+{
+	struct device *dev = mbo->ifp->driver_dev;
+
+	return dma_alloc_coherent(dev, size, &mbo->bus_address, GFP_KERNEL);
+}
+
+static void dma_free(struct mbo *mbo, u32 size)
+{
+	struct device *dev = mbo->ifp->driver_dev;
+
+	dma_free_coherent(dev, size, mbo->virt_address, mbo->bus_address);
+}
+
+static const struct of_device_id dim2_of_match[];
+
+static struct {
+	const char *clock_speed;
+	u8 clk_speed;
+} clk_mt[] = {
+	{ "256fs", CLK_256FS },
+	{ "512fs", CLK_512FS },
+	{ "1024fs", CLK_1024FS },
+	{ "2048fs", CLK_2048FS },
+	{ "3072fs", CLK_3072FS },
+	{ "4096fs", CLK_4096FS },
+	{ "6144fs", CLK_6144FS },
+	{ "8192fs", CLK_8192FS },
+};
+
+/**
+ * get_dim2_clk_speed - converts string to DIM2 clock speed value
+ *
+ * @clock_speed: string in the format "{NUMBER}fs"
+ * @val: pointer to get one of the CLK_{NUMBER}FS values
+ *
+ * By success stores one of the CLK_{NUMBER}FS in the *val and returns 0,
+ * otherwise returns -EINVAL.
+ */
+static int get_dim2_clk_speed(const char *clock_speed, u8 *val)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(clk_mt); i++) {
+		if (!strcmp(clock_speed, clk_mt[i].clock_speed)) {
+			*val = clk_mt[i].clk_speed;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static void dim2_release(struct device *d)
+{
+	struct dim2_hdm *dev = container_of(d, struct dim2_hdm, dev);
+	unsigned long flags;
+
+	kthread_stop(dev->netinfo_task);
+
+	spin_lock_irqsave(&dim_lock, flags);
+	dim_shutdown();
+	spin_unlock_irqrestore(&dim_lock, flags);
+
+	if (dev->disable_platform)
+		dev->disable_platform(to_platform_device(d->parent));
+
+	kfree(dev);
+}
+
+/*
+ * dim2_probe - dim2 probe handler
+ * @pdev: platform device structure
+ *
+ * Register the dim2 interface with mostcore and initialize it.
+ * Return 0 on success, negative on failure.
+ */
+static int dim2_probe(struct platform_device *pdev)
+{
+	const struct dim2_platform_data *pdata;
+	const struct of_device_id *of_id;
+	const char *clock_speed;
+	struct dim2_hdm *dev;
+	struct resource *res;
+	int ret, i;
+	u8 hal_ret;
+	u8 dev_fcnt = fcnt;
+	int irq;
+
+	enum { MLB_INT_IDX, AHB0_INT_IDX };
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->atx_idx = -1;
+
+	platform_set_drvdata(pdev, dev);
+
+	ret = of_property_read_string(pdev->dev.of_node,
+				      "microchip,clock-speed", &clock_speed);
+	if (ret) {
+		dev_err(&pdev->dev, "missing dt property clock-speed\n");
+		goto err_free_dev;
+	}
+
+	ret = get_dim2_clk_speed(clock_speed, &dev->clk_speed);
+	if (ret) {
+		dev_err(&pdev->dev, "bad dt property clock-speed\n");
+		goto err_free_dev;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->io_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->io_base)) {
+		ret = PTR_ERR(dev->io_base);
+		goto err_free_dev;
+	}
+
+	of_id = of_match_node(dim2_of_match, pdev->dev.of_node);
+	pdata = of_id->data;
+	if (pdata) {
+		if (pdata->enable) {
+			ret = pdata->enable(pdev);
+			if (ret)
+				goto err_free_dev;
+		}
+		dev->disable_platform = pdata->disable;
+		if (pdata->fcnt)
+			dev_fcnt = pdata->fcnt;
+	}
+
+	dev_info(&pdev->dev, "sync: num of frames per sub-buffer: %u\n",
+		 dev_fcnt);
+	hal_ret = dim_startup(dev->io_base, dev->clk_speed, dev_fcnt);
+	if (hal_ret != DIM_NO_ERROR) {
+		dev_err(&pdev->dev, "dim_startup failed: %d\n", hal_ret);
+		ret = -ENODEV;
+		goto err_disable_platform;
+	}
+
+	irq = platform_get_irq(pdev, AHB0_INT_IDX);
+	if (irq < 0) {
+		ret = irq;
+		goto err_shutdown_dim;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, dim2_ahb_isr,
+					dim2_task_irq, 0, "dim2_ahb0_int", dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request ahb0_int irq %d\n", irq);
+		goto err_shutdown_dim;
+	}
+
+	irq = platform_get_irq(pdev, MLB_INT_IDX);
+	if (irq < 0) {
+		ret = irq;
+		goto err_shutdown_dim;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, dim2_mlb_isr, 0,
+			       "dim2_mlb_int", dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request mlb_int irq %d\n", irq);
+		goto err_shutdown_dim;
+	}
+
+	init_waitqueue_head(&dev->netinfo_waitq);
+	dev->deliver_netinfo = 0;
+	dev->netinfo_task = kthread_run(&deliver_netinfo_thread, dev,
+					"dim2_netinfo");
+	if (IS_ERR(dev->netinfo_task)) {
+		ret = PTR_ERR(dev->netinfo_task);
+		goto err_shutdown_dim;
+	}
+
+	for (i = 0; i < DMA_CHANNELS; i++) {
+		struct most_channel_capability *cap = dev->capabilities + i;
+		struct hdm_channel *hdm_ch = dev->hch + i;
+
+		INIT_LIST_HEAD(&hdm_ch->pending_list);
+		INIT_LIST_HEAD(&hdm_ch->started_list);
+		hdm_ch->is_initialized = false;
+		snprintf(hdm_ch->name, sizeof(hdm_ch->name), "ca%d", i * 2 + 2);
+
+		cap->name_suffix = hdm_ch->name;
+		cap->direction = MOST_CH_RX | MOST_CH_TX;
+		cap->data_type = MOST_CH_CONTROL | MOST_CH_ASYNC |
+				 MOST_CH_ISOC | MOST_CH_SYNC;
+		cap->num_buffers_packet = MAX_BUFFERS_PACKET;
+		cap->buffer_size_packet = MAX_BUF_SIZE_PACKET;
+		cap->num_buffers_streaming = MAX_BUFFERS_STREAMING;
+		cap->buffer_size_streaming = MAX_BUF_SIZE_STREAMING;
+	}
+
+	{
+		const char *fmt;
+
+		if (sizeof(res->start) == sizeof(long long))
+			fmt = "dim2-%016llx";
+		else if (sizeof(res->start) == sizeof(long))
+			fmt = "dim2-%016lx";
+		else
+			fmt = "dim2-%016x";
+
+		snprintf(dev->name, sizeof(dev->name), fmt, res->start);
+	}
+
+	dev->most_iface.interface = ITYPE_MEDIALB_DIM2;
+	dev->most_iface.description = dev->name;
+	dev->most_iface.num_channels = DMA_CHANNELS;
+	dev->most_iface.channel_vector = dev->capabilities;
+	dev->most_iface.configure = configure_channel;
+	dev->most_iface.enqueue = enqueue;
+	dev->most_iface.dma_alloc = dma_alloc;
+	dev->most_iface.dma_free = dma_free;
+	dev->most_iface.poison_channel = poison_channel;
+	dev->most_iface.request_netinfo = request_netinfo;
+	dev->most_iface.driver_dev = &pdev->dev;
+	dev->most_iface.dev = &dev->dev;
+	dev->dev.init_name = dev->name;
+	dev->dev.parent = &pdev->dev;
+	dev->dev.release = dim2_release;
+
+	return most_register_interface(&dev->most_iface);
+
+err_shutdown_dim:
+	dim_shutdown();
+err_disable_platform:
+	if (dev->disable_platform)
+		dev->disable_platform(pdev);
+err_free_dev:
+	kfree(dev);
+
+	return ret;
+}
+
+/**
+ * dim2_remove - dim2 remove handler
+ * @pdev: platform device structure
+ *
+ * Unregister the interface from mostcore
+ */
+static int dim2_remove(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+
+	most_deregister_interface(&dev->most_iface);
+
+	return 0;
+}
+
+/* platform specific functions [[ */
+
+static int fsl_mx6_enable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+	int ret;
+
+	dev->clk = devm_clk_get(&pdev->dev, "mlb");
+	if (IS_ERR_OR_NULL(dev->clk)) {
+		dev_err(&pdev->dev, "unable to get mlb clock\n");
+		return -EFAULT;
+	}
+
+	ret = clk_prepare_enable(dev->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "%s\n", "clk_prepare_enable failed");
+		return ret;
+	}
+
+	if (dev->clk_speed >= CLK_2048FS) {
+		/* enable pll */
+		dev->clk_pll = devm_clk_get(&pdev->dev, "pll8_mlb");
+		if (IS_ERR_OR_NULL(dev->clk_pll)) {
+			dev_err(&pdev->dev, "unable to get mlb pll clock\n");
+			clk_disable_unprepare(dev->clk);
+			return -EFAULT;
+		}
+
+		writel(0x888, dev->io_base + 0x38);
+		clk_prepare_enable(dev->clk_pll);
+	}
+
+	return 0;
+}
+
+static void fsl_mx6_disable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+
+	if (dev->clk_speed >= CLK_2048FS)
+		clk_disable_unprepare(dev->clk_pll);
+
+	clk_disable_unprepare(dev->clk);
+}
+
+static int rcar_gen2_enable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+	int ret;
+
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(dev->clk)) {
+		dev_err(&pdev->dev, "cannot get clock\n");
+		return PTR_ERR(dev->clk);
+	}
+
+	ret = clk_prepare_enable(dev->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "%s\n", "clk_prepare_enable failed");
+		return ret;
+	}
+
+	if (dev->clk_speed >= CLK_2048FS) {
+		/* enable MLP pll and LVDS drivers */
+		writel(0x03, dev->io_base + 0x600);
+		/* set bias */
+		writel(0x888, dev->io_base + 0x38);
+	} else {
+		/* PLL */
+		writel(0x04, dev->io_base + 0x600);
+	}
+
+
+	/* BBCR = 0b11 */
+	writel(0x03, dev->io_base + 0x500);
+	writel(0x0002FF02, dev->io_base + 0x508);
+
+	return 0;
+}
+
+static void rcar_gen2_disable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(dev->clk);
+
+	/* disable PLLs and LVDS drivers */
+	writel(0x0, dev->io_base + 0x600);
+}
+
+static int rcar_gen3_enable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+	u32 enable_512fs = dev->clk_speed == CLK_512FS;
+	int ret;
+
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(dev->clk)) {
+		dev_err(&pdev->dev, "cannot get clock\n");
+		return PTR_ERR(dev->clk);
+	}
+
+	ret = clk_prepare_enable(dev->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "%s\n", "clk_prepare_enable failed");
+		return ret;
+	}
+
+	/* PLL */
+	writel(0x04, dev->io_base + 0x600);
+
+	writel(enable_512fs, dev->io_base + 0x604);
+
+	/* BBCR = 0b11 */
+	writel(0x03, dev->io_base + 0x500);
+	writel(0x0002FF02, dev->io_base + 0x508);
+
+	return 0;
+}
+
+static void rcar_gen3_disable(struct platform_device *pdev)
+{
+	struct dim2_hdm *dev = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(dev->clk);
+
+	/* disable PLLs and LVDS drivers */
+	writel(0x0, dev->io_base + 0x600);
+}
+
+/* ]] platform specific functions */
+
+enum dim2_platforms { FSL_MX6, RCAR_GEN2, RCAR_GEN3 };
+
+static struct dim2_platform_data plat_data[] = {
+	[FSL_MX6] = {
+		.enable = fsl_mx6_enable,
+		.disable = fsl_mx6_disable,
+	},
+	[RCAR_GEN2] = {
+		.enable = rcar_gen2_enable,
+		.disable = rcar_gen2_disable,
+	},
+	[RCAR_GEN3] = {
+		.enable = rcar_gen3_enable,
+		.disable = rcar_gen3_disable,
+		.fcnt = 3,
+	},
+};
+
+static const struct of_device_id dim2_of_match[] = {
+	{
+		.compatible = "fsl,imx6q-mlb150",
+		.data = plat_data + FSL_MX6
+	},
+	{
+		.compatible = "renesas,mlp",
+		.data = plat_data + RCAR_GEN2
+	},
+	{
+		.compatible = "renesas,rcar-gen3-mlp",
+		.data = plat_data + RCAR_GEN3
+	},
+	{
+		.compatible = "xlnx,axi4-os62420_3pin-1.00.a",
+	},
+	{
+		.compatible = "xlnx,axi4-os62420_6pin-1.00.a",
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, dim2_of_match);
+
+static struct platform_driver dim2_driver = {
+	.probe = dim2_probe,
+	.remove = dim2_remove,
+	.driver = {
+		.name = "hdm_dim2",
+		.of_match_table = dim2_of_match,
+		.dev_groups = dim2_groups,
+	},
+};
+
+module_platform_driver(dim2_driver);
+
+MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
+MODULE_DESCRIPTION("MediaLB DIM2 Hardware Dependent Module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/most/dim2/errors.h b/drivers/staging/most/dim2/errors.h
new file mode 100644
index 000000000..268332e57
--- /dev/null
+++ b/drivers/staging/most/dim2/errors.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * errors.h - Definitions of errors for DIM2 HAL API
+ * (MediaLB, Device Interface Macro IP, OS62420)
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#ifndef _MOST_DIM_ERRORS_H
+#define _MOST_DIM_ERRORS_H
+
+/**
+ * MOST DIM errors.
+ */
+enum dim_errors_t {
+	/** Not an error */
+	DIM_NO_ERROR = 0,
+
+	/** Bad base address for DIM2 IP */
+	DIM_INIT_ERR_DIM_ADDR = 0x10,
+
+	/**< Bad MediaLB clock */
+	DIM_INIT_ERR_MLB_CLOCK,
+
+	/** Bad channel address */
+	DIM_INIT_ERR_CHANNEL_ADDRESS,
+
+	/** Out of DBR memory */
+	DIM_INIT_ERR_OUT_OF_MEMORY,
+
+	/** DIM API is called while DIM is not initialized successfully */
+	DIM_ERR_DRIVER_NOT_INITIALIZED = 0x20,
+
+	/**
+	 * Configuration does not respect hardware limitations
+	 * for isochronous or synchronous channels
+	 */
+	DIM_ERR_BAD_CONFIG,
+
+	/**
+	 * Buffer size does not respect hardware limitations
+	 * for isochronous or synchronous channels
+	 */
+	DIM_ERR_BAD_BUFFER_SIZE,
+
+	DIM_ERR_UNDERFLOW,
+
+	DIM_ERR_OVERFLOW,
+};
+
+#endif /* _MOST_DIM_ERRORS_H */
diff --git a/drivers/staging/most/dim2/hal.c b/drivers/staging/most/dim2/hal.c
new file mode 100644
index 000000000..65282c276
--- /dev/null
+++ b/drivers/staging/most/dim2/hal.c
@@ -0,0 +1,975 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * hal.c - DIM2 HAL implementation
+ * (MediaLB, Device Interface Macro IP, OS62420)
+ *
+ * Copyright (C) 2015-2016, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+/* Author: Andrey Shvetsov <andrey.shvetsov@k2l.de> */
+
+#include "hal.h"
+#include "errors.h"
+#include "reg.h"
+#include <linux/stddef.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+
+/*
+ * Size factor for isochronous DBR buffer.
+ * Minimal value is 3.
+ */
+#define ISOC_DBR_FACTOR 3u
+
+/*
+ * Number of 32-bit units for DBR map.
+ *
+ * 1: block size is 512, max allocation is 16K
+ * 2: block size is 256, max allocation is 8K
+ * 4: block size is 128, max allocation is 4K
+ * 8: block size is 64, max allocation is 2K
+ *
+ * Min allocated space is block size.
+ * Max possible allocated space is 32 blocks.
+ */
+#define DBR_MAP_SIZE 2
+
+/* -------------------------------------------------------------------------- */
+/* not configurable area */
+
+#define CDT 0x00
+#define ADT 0x40
+#define MLB_CAT 0x80
+#define AHB_CAT 0x88
+
+#define DBR_SIZE  (16 * 1024) /* specified by IP */
+#define DBR_BLOCK_SIZE  (DBR_SIZE / 32 / DBR_MAP_SIZE)
+
+#define ROUND_UP_TO(x, d)  (DIV_ROUND_UP(x, (d)) * (d))
+
+/* -------------------------------------------------------------------------- */
+/* generic helper functions and macros */
+
+static inline u32 bit_mask(u8 position)
+{
+	return (u32)1 << position;
+}
+
+static inline bool dim_on_error(u8 error_id, const char *error_message)
+{
+	dimcb_on_error(error_id, error_message);
+	return false;
+}
+
+/* -------------------------------------------------------------------------- */
+/* types and local variables */
+
+struct async_tx_dbr {
+	u8 ch_addr;
+	u16 rpc;
+	u16 wpc;
+	u16 rest_size;
+	u16 sz_queue[CDT0_RPC_MASK + 1];
+};
+
+struct lld_global_vars_t {
+	bool dim_is_initialized;
+	bool mcm_is_initialized;
+	struct dim2_regs __iomem *dim2; /* DIM2 core base address */
+	struct async_tx_dbr atx_dbr;
+	u32 fcnt;
+	u32 dbr_map[DBR_MAP_SIZE];
+};
+
+static struct lld_global_vars_t g = { false };
+
+/* -------------------------------------------------------------------------- */
+
+static int dbr_get_mask_size(u16 size)
+{
+	int i;
+
+	for (i = 0; i < 6; i++)
+		if (size <= (DBR_BLOCK_SIZE << i))
+			return 1 << i;
+	return 0;
+}
+
+/**
+ * alloc_dbr() - Allocates DBR memory.
+ * @size: Allocating memory size.
+ * Returns: Offset in DBR memory by success or DBR_SIZE if out of memory.
+ */
+static int alloc_dbr(u16 size)
+{
+	int mask_size;
+	int i, block_idx = 0;
+
+	if (size <= 0)
+		return DBR_SIZE; /* out of memory */
+
+	mask_size = dbr_get_mask_size(size);
+	if (mask_size == 0)
+		return DBR_SIZE; /* out of memory */
+
+	for (i = 0; i < DBR_MAP_SIZE; i++) {
+		u32 const blocks = DIV_ROUND_UP(size, DBR_BLOCK_SIZE);
+		u32 mask = ~((~(u32)0) << blocks);
+
+		do {
+			if ((g.dbr_map[i] & mask) == 0) {
+				g.dbr_map[i] |= mask;
+				return block_idx * DBR_BLOCK_SIZE;
+			}
+			block_idx += mask_size;
+			/* do shift left with 2 steps in case mask_size == 32 */
+			mask <<= mask_size - 1;
+		} while ((mask <<= 1) != 0);
+	}
+
+	return DBR_SIZE; /* out of memory */
+}
+
+static void free_dbr(int offs, int size)
+{
+	int block_idx = offs / DBR_BLOCK_SIZE;
+	u32 const blocks = DIV_ROUND_UP(size, DBR_BLOCK_SIZE);
+	u32 mask = ~((~(u32)0) << blocks);
+
+	mask <<= block_idx % 32;
+	g.dbr_map[block_idx / 32] &= ~mask;
+}
+
+/* -------------------------------------------------------------------------- */
+
+static void dim2_transfer_madr(u32 val)
+{
+	writel(val, &g.dim2->MADR);
+
+	/* wait for transfer completion */
+	while ((readl(&g.dim2->MCTL) & 1) != 1)
+		continue;
+
+	writel(0, &g.dim2->MCTL);   /* clear transfer complete */
+}
+
+static void dim2_clear_dbr(u16 addr, u16 size)
+{
+	enum { MADR_TB_BIT = 30, MADR_WNR_BIT = 31 };
+
+	u16 const end_addr = addr + size;
+	u32 const cmd = bit_mask(MADR_WNR_BIT) | bit_mask(MADR_TB_BIT);
+
+	writel(0, &g.dim2->MCTL);   /* clear transfer complete */
+	writel(0, &g.dim2->MDAT0);
+
+	for (; addr < end_addr; addr++)
+		dim2_transfer_madr(cmd | addr);
+}
+
+static u32 dim2_read_ctr(u32 ctr_addr, u16 mdat_idx)
+{
+	dim2_transfer_madr(ctr_addr);
+
+	return readl((&g.dim2->MDAT0) + mdat_idx);
+}
+
+static void dim2_write_ctr_mask(u32 ctr_addr, const u32 *mask, const u32 *value)
+{
+	enum { MADR_WNR_BIT = 31 };
+
+	writel(0, &g.dim2->MCTL);   /* clear transfer complete */
+
+	if (mask[0] != 0)
+		writel(value[0], &g.dim2->MDAT0);
+	if (mask[1] != 0)
+		writel(value[1], &g.dim2->MDAT1);
+	if (mask[2] != 0)
+		writel(value[2], &g.dim2->MDAT2);
+	if (mask[3] != 0)
+		writel(value[3], &g.dim2->MDAT3);
+
+	writel(mask[0], &g.dim2->MDWE0);
+	writel(mask[1], &g.dim2->MDWE1);
+	writel(mask[2], &g.dim2->MDWE2);
+	writel(mask[3], &g.dim2->MDWE3);
+
+	dim2_transfer_madr(bit_mask(MADR_WNR_BIT) | ctr_addr);
+}
+
+static inline void dim2_write_ctr(u32 ctr_addr, const u32 *value)
+{
+	u32 const mask[4] = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF };
+
+	dim2_write_ctr_mask(ctr_addr, mask, value);
+}
+
+static inline void dim2_clear_ctr(u32 ctr_addr)
+{
+	u32 const value[4] = { 0, 0, 0, 0 };
+
+	dim2_write_ctr(ctr_addr, value);
+}
+
+static void dim2_configure_cat(u8 cat_base, u8 ch_addr, u8 ch_type,
+			       bool read_not_write)
+{
+	bool isoc_fce = ch_type == CAT_CT_VAL_ISOC;
+	bool sync_mfe = ch_type == CAT_CT_VAL_SYNC;
+	u16 const cat =
+		(read_not_write << CAT_RNW_BIT) |
+		(ch_type << CAT_CT_SHIFT) |
+		(ch_addr << CAT_CL_SHIFT) |
+		(isoc_fce << CAT_FCE_BIT) |
+		(sync_mfe << CAT_MFE_BIT) |
+		(false << CAT_MT_BIT) |
+		(true << CAT_CE_BIT);
+	u8 const ctr_addr = cat_base + ch_addr / 8;
+	u8 const idx = (ch_addr % 8) / 2;
+	u8 const shift = (ch_addr % 2) * 16;
+	u32 mask[4] = { 0, 0, 0, 0 };
+	u32 value[4] = { 0, 0, 0, 0 };
+
+	mask[idx] = (u32)0xFFFF << shift;
+	value[idx] = cat << shift;
+	dim2_write_ctr_mask(ctr_addr, mask, value);
+}
+
+static void dim2_clear_cat(u8 cat_base, u8 ch_addr)
+{
+	u8 const ctr_addr = cat_base + ch_addr / 8;
+	u8 const idx = (ch_addr % 8) / 2;
+	u8 const shift = (ch_addr % 2) * 16;
+	u32 mask[4] = { 0, 0, 0, 0 };
+	u32 value[4] = { 0, 0, 0, 0 };
+
+	mask[idx] = (u32)0xFFFF << shift;
+	dim2_write_ctr_mask(ctr_addr, mask, value);
+}
+
+static void dim2_configure_cdt(u8 ch_addr, u16 dbr_address, u16 hw_buffer_size,
+			       u16 packet_length)
+{
+	u32 cdt[4] = { 0, 0, 0, 0 };
+
+	if (packet_length)
+		cdt[1] = ((packet_length - 1) << CDT1_BS_ISOC_SHIFT);
+
+	cdt[3] =
+		((hw_buffer_size - 1) << CDT3_BD_SHIFT) |
+		(dbr_address << CDT3_BA_SHIFT);
+	dim2_write_ctr(CDT + ch_addr, cdt);
+}
+
+static u16 dim2_rpc(u8 ch_addr)
+{
+	u32 cdt0 = dim2_read_ctr(CDT + ch_addr, 0);
+
+	return (cdt0 >> CDT0_RPC_SHIFT) & CDT0_RPC_MASK;
+}
+
+static void dim2_clear_cdt(u8 ch_addr)
+{
+	u32 cdt[4] = { 0, 0, 0, 0 };
+
+	dim2_write_ctr(CDT + ch_addr, cdt);
+}
+
+static void dim2_configure_adt(u8 ch_addr)
+{
+	u32 adt[4] = { 0, 0, 0, 0 };
+
+	adt[0] =
+		(true << ADT0_CE_BIT) |
+		(true << ADT0_LE_BIT) |
+		(0 << ADT0_PG_BIT);
+
+	dim2_write_ctr(ADT + ch_addr, adt);
+}
+
+static void dim2_clear_adt(u8 ch_addr)
+{
+	u32 adt[4] = { 0, 0, 0, 0 };
+
+	dim2_write_ctr(ADT + ch_addr, adt);
+}
+
+static void dim2_start_ctrl_async(u8 ch_addr, u8 idx, u32 buf_addr,
+				  u16 buffer_size)
+{
+	u8 const shift = idx * 16;
+
+	u32 mask[4] = { 0, 0, 0, 0 };
+	u32 adt[4] = { 0, 0, 0, 0 };
+
+	mask[1] =
+		bit_mask(ADT1_PS_BIT + shift) |
+		bit_mask(ADT1_RDY_BIT + shift) |
+		(ADT1_CTRL_ASYNC_BD_MASK << (ADT1_BD_SHIFT + shift));
+	adt[1] =
+		(true << (ADT1_PS_BIT + shift)) |
+		(true << (ADT1_RDY_BIT + shift)) |
+		((buffer_size - 1) << (ADT1_BD_SHIFT + shift));
+
+	mask[idx + 2] = 0xFFFFFFFF;
+	adt[idx + 2] = buf_addr;
+
+	dim2_write_ctr_mask(ADT + ch_addr, mask, adt);
+}
+
+static void dim2_start_isoc_sync(u8 ch_addr, u8 idx, u32 buf_addr,
+				 u16 buffer_size)
+{
+	u8 const shift = idx * 16;
+
+	u32 mask[4] = { 0, 0, 0, 0 };
+	u32 adt[4] = { 0, 0, 0, 0 };
+
+	mask[1] =
+		bit_mask(ADT1_RDY_BIT + shift) |
+		(ADT1_ISOC_SYNC_BD_MASK << (ADT1_BD_SHIFT + shift));
+	adt[1] =
+		(true << (ADT1_RDY_BIT + shift)) |
+		((buffer_size - 1) << (ADT1_BD_SHIFT + shift));
+
+	mask[idx + 2] = 0xFFFFFFFF;
+	adt[idx + 2] = buf_addr;
+
+	dim2_write_ctr_mask(ADT + ch_addr, mask, adt);
+}
+
+static void dim2_clear_ctram(void)
+{
+	u32 ctr_addr;
+
+	for (ctr_addr = 0; ctr_addr < 0x90; ctr_addr++)
+		dim2_clear_ctr(ctr_addr);
+}
+
+static void dim2_configure_channel(
+	u8 ch_addr, u8 type, u8 is_tx, u16 dbr_address, u16 hw_buffer_size,
+	u16 packet_length)
+{
+	dim2_configure_cdt(ch_addr, dbr_address, hw_buffer_size, packet_length);
+	dim2_configure_cat(MLB_CAT, ch_addr, type, is_tx ? 1 : 0);
+
+	dim2_configure_adt(ch_addr);
+	dim2_configure_cat(AHB_CAT, ch_addr, type, is_tx ? 0 : 1);
+
+	/* unmask interrupt for used channel, enable mlb_sys_int[0] interrupt */
+	writel(readl(&g.dim2->ACMR0) | bit_mask(ch_addr), &g.dim2->ACMR0);
+}
+
+static void dim2_clear_channel(u8 ch_addr)
+{
+	/* mask interrupt for used channel, disable mlb_sys_int[0] interrupt */
+	writel(readl(&g.dim2->ACMR0) & ~bit_mask(ch_addr), &g.dim2->ACMR0);
+
+	dim2_clear_cat(AHB_CAT, ch_addr);
+	dim2_clear_adt(ch_addr);
+
+	dim2_clear_cat(MLB_CAT, ch_addr);
+	dim2_clear_cdt(ch_addr);
+
+	/* clear channel status bit */
+	writel(bit_mask(ch_addr), &g.dim2->ACSR0);
+}
+
+/* -------------------------------------------------------------------------- */
+/* trace async tx dbr fill state */
+
+static inline u16 norm_pc(u16 pc)
+{
+	return pc & CDT0_RPC_MASK;
+}
+
+static void dbrcnt_init(u8 ch_addr, u16 dbr_size)
+{
+	g.atx_dbr.rest_size = dbr_size;
+	g.atx_dbr.rpc = dim2_rpc(ch_addr);
+	g.atx_dbr.wpc = g.atx_dbr.rpc;
+}
+
+static void dbrcnt_enq(int buf_sz)
+{
+	g.atx_dbr.rest_size -= buf_sz;
+	g.atx_dbr.sz_queue[norm_pc(g.atx_dbr.wpc)] = buf_sz;
+	g.atx_dbr.wpc++;
+}
+
+u16 dim_dbr_space(struct dim_channel *ch)
+{
+	u16 cur_rpc;
+	struct async_tx_dbr *dbr = &g.atx_dbr;
+
+	if (ch->addr != dbr->ch_addr)
+		return 0xFFFF;
+
+	cur_rpc = dim2_rpc(ch->addr);
+
+	while (norm_pc(dbr->rpc) != cur_rpc) {
+		dbr->rest_size += dbr->sz_queue[norm_pc(dbr->rpc)];
+		dbr->rpc++;
+	}
+
+	if ((u16)(dbr->wpc - dbr->rpc) >= CDT0_RPC_MASK)
+		return 0;
+
+	return dbr->rest_size;
+}
+
+/* -------------------------------------------------------------------------- */
+/* channel state helpers */
+
+static void state_init(struct int_ch_state *state)
+{
+	state->request_counter = 0;
+	state->service_counter = 0;
+
+	state->idx1 = 0;
+	state->idx2 = 0;
+	state->level = 0;
+}
+
+/* -------------------------------------------------------------------------- */
+/* macro helper functions */
+
+static inline bool check_channel_address(u32 ch_address)
+{
+	return ch_address > 0 && (ch_address % 2) == 0 &&
+	       (ch_address / 2) <= (u32)CAT_CL_MASK;
+}
+
+static inline bool check_packet_length(u32 packet_length)
+{
+	u16 const max_size = ((u16)CDT3_BD_ISOC_MASK + 1u) / ISOC_DBR_FACTOR;
+
+	if (packet_length <= 0)
+		return false; /* too small */
+
+	if (packet_length > max_size)
+		return false; /* too big */
+
+	if (packet_length - 1u > (u32)CDT1_BS_ISOC_MASK)
+		return false; /* too big */
+
+	return true;
+}
+
+static inline bool check_bytes_per_frame(u32 bytes_per_frame)
+{
+	u16 const bd_factor = g.fcnt + 2;
+	u16 const max_size = ((u16)CDT3_BD_MASK + 1u) >> bd_factor;
+
+	if (bytes_per_frame <= 0)
+		return false; /* too small */
+
+	if (bytes_per_frame > max_size)
+		return false; /* too big */
+
+	return true;
+}
+
+u16 dim_norm_ctrl_async_buffer_size(u16 buf_size)
+{
+	u16 const max_size = (u16)ADT1_CTRL_ASYNC_BD_MASK + 1u;
+
+	if (buf_size > max_size)
+		return max_size;
+
+	return buf_size;
+}
+
+static inline u16 norm_isoc_buffer_size(u16 buf_size, u16 packet_length)
+{
+	u16 n;
+	u16 const max_size = (u16)ADT1_ISOC_SYNC_BD_MASK + 1u;
+
+	if (buf_size > max_size)
+		buf_size = max_size;
+
+	n = buf_size / packet_length;
+
+	if (n < 2u)
+		return 0; /* too small buffer for given packet_length */
+
+	return packet_length * n;
+}
+
+static inline u16 norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame)
+{
+	u16 n;
+	u16 const max_size = (u16)ADT1_ISOC_SYNC_BD_MASK + 1u;
+	u32 const unit = bytes_per_frame << g.fcnt;
+
+	if (buf_size > max_size)
+		buf_size = max_size;
+
+	n = buf_size / unit;
+
+	if (n < 1u)
+		return 0; /* too small buffer for given bytes_per_frame */
+
+	return unit * n;
+}
+
+static void dim2_cleanup(void)
+{
+	/* disable MediaLB */
+	writel(false << MLBC0_MLBEN_BIT, &g.dim2->MLBC0);
+
+	dim2_clear_ctram();
+
+	/* disable mlb_int interrupt */
+	writel(0, &g.dim2->MIEN);
+
+	/* clear status for all dma channels */
+	writel(0xFFFFFFFF, &g.dim2->ACSR0);
+	writel(0xFFFFFFFF, &g.dim2->ACSR1);
+
+	/* mask interrupts for all channels */
+	writel(0, &g.dim2->ACMR0);
+	writel(0, &g.dim2->ACMR1);
+}
+
+static void dim2_initialize(bool enable_6pin, u8 mlb_clock)
+{
+	dim2_cleanup();
+
+	/* configure and enable MediaLB */
+	writel(enable_6pin << MLBC0_MLBPEN_BIT |
+	       mlb_clock << MLBC0_MLBCLK_SHIFT |
+	       g.fcnt << MLBC0_FCNT_SHIFT |
+	       true << MLBC0_MLBEN_BIT,
+	       &g.dim2->MLBC0);
+
+	/* activate all HBI channels */
+	writel(0xFFFFFFFF, &g.dim2->HCMR0);
+	writel(0xFFFFFFFF, &g.dim2->HCMR1);
+
+	/* enable HBI */
+	writel(bit_mask(HCTL_EN_BIT), &g.dim2->HCTL);
+
+	/* configure DMA */
+	writel(ACTL_DMA_MODE_VAL_DMA_MODE_1 << ACTL_DMA_MODE_BIT |
+	       true << ACTL_SCE_BIT, &g.dim2->ACTL);
+}
+
+static bool dim2_is_mlb_locked(void)
+{
+	u32 const mask0 = bit_mask(MLBC0_MLBLK_BIT);
+	u32 const mask1 = bit_mask(MLBC1_CLKMERR_BIT) |
+			  bit_mask(MLBC1_LOCKERR_BIT);
+	u32 const c1 = readl(&g.dim2->MLBC1);
+	u32 const nda_mask = (u32)MLBC1_NDA_MASK << MLBC1_NDA_SHIFT;
+
+	writel(c1 & nda_mask, &g.dim2->MLBC1);
+	return (readl(&g.dim2->MLBC1) & mask1) == 0 &&
+	       (readl(&g.dim2->MLBC0) & mask0) != 0;
+}
+
+/* -------------------------------------------------------------------------- */
+/* channel help routines */
+
+static inline bool service_channel(u8 ch_addr, u8 idx)
+{
+	u8 const shift = idx * 16;
+	u32 const adt1 = dim2_read_ctr(ADT + ch_addr, 1);
+	u32 mask[4] = { 0, 0, 0, 0 };
+	u32 adt_w[4] = { 0, 0, 0, 0 };
+
+	if (((adt1 >> (ADT1_DNE_BIT + shift)) & 1) == 0)
+		return false;
+
+	mask[1] =
+		bit_mask(ADT1_DNE_BIT + shift) |
+		bit_mask(ADT1_ERR_BIT + shift) |
+		bit_mask(ADT1_RDY_BIT + shift);
+	dim2_write_ctr_mask(ADT + ch_addr, mask, adt_w);
+
+	/* clear channel status bit */
+	writel(bit_mask(ch_addr), &g.dim2->ACSR0);
+
+	return true;
+}
+
+/* -------------------------------------------------------------------------- */
+/* channel init routines */
+
+static void isoc_init(struct dim_channel *ch, u8 ch_addr, u16 packet_length)
+{
+	state_init(&ch->state);
+
+	ch->addr = ch_addr;
+
+	ch->packet_length = packet_length;
+	ch->bytes_per_frame = 0;
+	ch->done_sw_buffers_number = 0;
+}
+
+static void sync_init(struct dim_channel *ch, u8 ch_addr, u16 bytes_per_frame)
+{
+	state_init(&ch->state);
+
+	ch->addr = ch_addr;
+
+	ch->packet_length = 0;
+	ch->bytes_per_frame = bytes_per_frame;
+	ch->done_sw_buffers_number = 0;
+}
+
+static void channel_init(struct dim_channel *ch, u8 ch_addr)
+{
+	state_init(&ch->state);
+
+	ch->addr = ch_addr;
+
+	ch->packet_length = 0;
+	ch->bytes_per_frame = 0;
+	ch->done_sw_buffers_number = 0;
+}
+
+/* returns true if channel interrupt state is cleared */
+static bool channel_service_interrupt(struct dim_channel *ch)
+{
+	struct int_ch_state *const state = &ch->state;
+
+	if (!service_channel(ch->addr, state->idx2))
+		return false;
+
+	state->idx2 ^= 1;
+	state->request_counter++;
+	return true;
+}
+
+static bool channel_start(struct dim_channel *ch, u32 buf_addr, u16 buf_size)
+{
+	struct int_ch_state *const state = &ch->state;
+
+	if (buf_size <= 0)
+		return dim_on_error(DIM_ERR_BAD_BUFFER_SIZE, "Bad buffer size");
+
+	if (ch->packet_length == 0 && ch->bytes_per_frame == 0 &&
+	    buf_size != dim_norm_ctrl_async_buffer_size(buf_size))
+		return dim_on_error(DIM_ERR_BAD_BUFFER_SIZE,
+				    "Bad control/async buffer size");
+
+	if (ch->packet_length &&
+	    buf_size != norm_isoc_buffer_size(buf_size, ch->packet_length))
+		return dim_on_error(DIM_ERR_BAD_BUFFER_SIZE,
+				    "Bad isochronous buffer size");
+
+	if (ch->bytes_per_frame &&
+	    buf_size != norm_sync_buffer_size(buf_size, ch->bytes_per_frame))
+		return dim_on_error(DIM_ERR_BAD_BUFFER_SIZE,
+				    "Bad synchronous buffer size");
+
+	if (state->level >= 2u)
+		return dim_on_error(DIM_ERR_OVERFLOW, "Channel overflow");
+
+	++state->level;
+
+	if (ch->addr == g.atx_dbr.ch_addr)
+		dbrcnt_enq(buf_size);
+
+	if (ch->packet_length || ch->bytes_per_frame)
+		dim2_start_isoc_sync(ch->addr, state->idx1, buf_addr, buf_size);
+	else
+		dim2_start_ctrl_async(ch->addr, state->idx1, buf_addr,
+				      buf_size);
+	state->idx1 ^= 1;
+
+	return true;
+}
+
+static u8 channel_service(struct dim_channel *ch)
+{
+	struct int_ch_state *const state = &ch->state;
+
+	if (state->service_counter != state->request_counter) {
+		state->service_counter++;
+		if (state->level == 0)
+			return DIM_ERR_UNDERFLOW;
+
+		--state->level;
+		ch->done_sw_buffers_number++;
+	}
+
+	return DIM_NO_ERROR;
+}
+
+static bool channel_detach_buffers(struct dim_channel *ch, u16 buffers_number)
+{
+	if (buffers_number > ch->done_sw_buffers_number)
+		return dim_on_error(DIM_ERR_UNDERFLOW, "Channel underflow");
+
+	ch->done_sw_buffers_number -= buffers_number;
+	return true;
+}
+
+/* -------------------------------------------------------------------------- */
+/* API */
+
+u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock,
+	       u32 fcnt)
+{
+	g.dim_is_initialized = false;
+
+	if (!dim_base_address)
+		return DIM_INIT_ERR_DIM_ADDR;
+
+	/* MediaLB clock: 0 - 256 fs, 1 - 512 fs, 2 - 1024 fs, 3 - 2048 fs */
+	/* MediaLB clock: 4 - 3072 fs, 5 - 4096 fs, 6 - 6144 fs, 7 - 8192 fs */
+	if (mlb_clock >= 8)
+		return DIM_INIT_ERR_MLB_CLOCK;
+
+	if (fcnt > MLBC0_FCNT_MAX_VAL)
+		return DIM_INIT_ERR_MLB_CLOCK;
+
+	g.dim2 = dim_base_address;
+	g.fcnt = fcnt;
+	g.dbr_map[0] = 0;
+	g.dbr_map[1] = 0;
+
+	dim2_initialize(mlb_clock >= 3, mlb_clock);
+
+	g.dim_is_initialized = true;
+
+	return DIM_NO_ERROR;
+}
+
+void dim_shutdown(void)
+{
+	g.dim_is_initialized = false;
+	dim2_cleanup();
+}
+
+bool dim_get_lock_state(void)
+{
+	return dim2_is_mlb_locked();
+}
+
+static u8 init_ctrl_async(struct dim_channel *ch, u8 type, u8 is_tx,
+			  u16 ch_address, u16 hw_buffer_size)
+{
+	if (!g.dim_is_initialized || !ch)
+		return DIM_ERR_DRIVER_NOT_INITIALIZED;
+
+	if (!check_channel_address(ch_address))
+		return DIM_INIT_ERR_CHANNEL_ADDRESS;
+
+	if (!ch->dbr_size)
+		ch->dbr_size = ROUND_UP_TO(hw_buffer_size, DBR_BLOCK_SIZE);
+	ch->dbr_addr = alloc_dbr(ch->dbr_size);
+	if (ch->dbr_addr >= DBR_SIZE)
+		return DIM_INIT_ERR_OUT_OF_MEMORY;
+
+	channel_init(ch, ch_address / 2);
+
+	dim2_configure_channel(ch->addr, type, is_tx,
+			       ch->dbr_addr, ch->dbr_size, 0);
+
+	return DIM_NO_ERROR;
+}
+
+void dim_service_mlb_int_irq(void)
+{
+	writel(0, &g.dim2->MS0);
+	writel(0, &g.dim2->MS1);
+}
+
+/*
+ * Retrieves maximal possible correct buffer size for isochronous data type
+ * conform to given packet length and not bigger than given buffer size.
+ *
+ * Returns non-zero correct buffer size or zero by error.
+ */
+u16 dim_norm_isoc_buffer_size(u16 buf_size, u16 packet_length)
+{
+	if (!check_packet_length(packet_length))
+		return 0;
+
+	return norm_isoc_buffer_size(buf_size, packet_length);
+}
+
+/*
+ * Retrieves maximal possible correct buffer size for synchronous data type
+ * conform to given bytes per frame and not bigger than given buffer size.
+ *
+ * Returns non-zero correct buffer size or zero by error.
+ */
+u16 dim_norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame)
+{
+	if (!check_bytes_per_frame(bytes_per_frame))
+		return 0;
+
+	return norm_sync_buffer_size(buf_size, bytes_per_frame);
+}
+
+u8 dim_init_control(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		    u16 max_buffer_size)
+{
+	return init_ctrl_async(ch, CAT_CT_VAL_CONTROL, is_tx, ch_address,
+			       max_buffer_size);
+}
+
+u8 dim_init_async(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		  u16 max_buffer_size)
+{
+	u8 ret = init_ctrl_async(ch, CAT_CT_VAL_ASYNC, is_tx, ch_address,
+				 max_buffer_size);
+
+	if (is_tx && !g.atx_dbr.ch_addr) {
+		g.atx_dbr.ch_addr = ch->addr;
+		dbrcnt_init(ch->addr, ch->dbr_size);
+		writel(bit_mask(20), &g.dim2->MIEN);
+	}
+
+	return ret;
+}
+
+u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 packet_length)
+{
+	if (!g.dim_is_initialized || !ch)
+		return DIM_ERR_DRIVER_NOT_INITIALIZED;
+
+	if (!check_channel_address(ch_address))
+		return DIM_INIT_ERR_CHANNEL_ADDRESS;
+
+	if (!check_packet_length(packet_length))
+		return DIM_ERR_BAD_CONFIG;
+
+	if (!ch->dbr_size)
+		ch->dbr_size = packet_length * ISOC_DBR_FACTOR;
+	ch->dbr_addr = alloc_dbr(ch->dbr_size);
+	if (ch->dbr_addr >= DBR_SIZE)
+		return DIM_INIT_ERR_OUT_OF_MEMORY;
+
+	isoc_init(ch, ch_address / 2, packet_length);
+
+	dim2_configure_channel(ch->addr, CAT_CT_VAL_ISOC, is_tx, ch->dbr_addr,
+			       ch->dbr_size, packet_length);
+
+	return DIM_NO_ERROR;
+}
+
+u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 bytes_per_frame)
+{
+	u16 bd_factor = g.fcnt + 2;
+
+	if (!g.dim_is_initialized || !ch)
+		return DIM_ERR_DRIVER_NOT_INITIALIZED;
+
+	if (!check_channel_address(ch_address))
+		return DIM_INIT_ERR_CHANNEL_ADDRESS;
+
+	if (!check_bytes_per_frame(bytes_per_frame))
+		return DIM_ERR_BAD_CONFIG;
+
+	if (!ch->dbr_size)
+		ch->dbr_size = bytes_per_frame << bd_factor;
+	ch->dbr_addr = alloc_dbr(ch->dbr_size);
+	if (ch->dbr_addr >= DBR_SIZE)
+		return DIM_INIT_ERR_OUT_OF_MEMORY;
+
+	sync_init(ch, ch_address / 2, bytes_per_frame);
+
+	dim2_clear_dbr(ch->dbr_addr, ch->dbr_size);
+	dim2_configure_channel(ch->addr, CAT_CT_VAL_SYNC, is_tx,
+			       ch->dbr_addr, ch->dbr_size, 0);
+
+	return DIM_NO_ERROR;
+}
+
+u8 dim_destroy_channel(struct dim_channel *ch)
+{
+	if (!g.dim_is_initialized || !ch)
+		return DIM_ERR_DRIVER_NOT_INITIALIZED;
+
+	if (ch->addr == g.atx_dbr.ch_addr) {
+		writel(0, &g.dim2->MIEN);
+		g.atx_dbr.ch_addr = 0;
+	}
+
+	dim2_clear_channel(ch->addr);
+	if (ch->dbr_addr < DBR_SIZE)
+		free_dbr(ch->dbr_addr, ch->dbr_size);
+	ch->dbr_addr = DBR_SIZE;
+
+	return DIM_NO_ERROR;
+}
+
+void dim_service_ahb_int_irq(struct dim_channel *const *channels)
+{
+	bool state_changed;
+
+	if (!g.dim_is_initialized) {
+		dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED,
+			     "DIM is not initialized");
+		return;
+	}
+
+	if (!channels) {
+		dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED, "Bad channels");
+		return;
+	}
+
+	/*
+	 * Use while-loop and a flag to make sure the age is changed back at
+	 * least once, otherwise the interrupt may never come if CPU generates
+	 * interrupt on changing age.
+	 * This cycle runs not more than number of channels, because
+	 * channel_service_interrupt() routine doesn't start the channel again.
+	 */
+	do {
+		struct dim_channel *const *ch = channels;
+
+		state_changed = false;
+
+		while (*ch) {
+			state_changed |= channel_service_interrupt(*ch);
+			++ch;
+		}
+	} while (state_changed);
+}
+
+u8 dim_service_channel(struct dim_channel *ch)
+{
+	if (!g.dim_is_initialized || !ch)
+		return DIM_ERR_DRIVER_NOT_INITIALIZED;
+
+	return channel_service(ch);
+}
+
+struct dim_ch_state_t *dim_get_channel_state(struct dim_channel *ch,
+					     struct dim_ch_state_t *state_ptr)
+{
+	if (!ch || !state_ptr)
+		return NULL;
+
+	state_ptr->ready = ch->state.level < 2;
+	state_ptr->done_buffers = ch->done_sw_buffers_number;
+
+	return state_ptr;
+}
+
+bool dim_enqueue_buffer(struct dim_channel *ch, u32 buffer_addr,
+			u16 buffer_size)
+{
+	if (!ch)
+		return dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED,
+				    "Bad channel");
+
+	return channel_start(ch, buffer_addr, buffer_size);
+}
+
+bool dim_detach_buffers(struct dim_channel *ch, u16 buffers_number)
+{
+	if (!ch)
+		return dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED,
+				    "Bad channel");
+
+	return channel_detach_buffers(ch, buffers_number);
+}
diff --git a/drivers/staging/most/dim2/hal.h b/drivers/staging/most/dim2/hal.h
new file mode 100644
index 000000000..20531449a
--- /dev/null
+++ b/drivers/staging/most/dim2/hal.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * hal.h - DIM2 HAL interface
+ * (MediaLB, Device Interface Macro IP, OS62420)
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#ifndef _DIM2_HAL_H
+#define _DIM2_HAL_H
+
+#include <linux/types.h>
+#include "reg.h"
+
+/*
+ * The values below are specified in the hardware specification.
+ * So, they should not be changed until the hardware specification changes.
+ */
+enum mlb_clk_speed {
+	CLK_256FS = 0,
+	CLK_512FS = 1,
+	CLK_1024FS = 2,
+	CLK_2048FS = 3,
+	CLK_3072FS = 4,
+	CLK_4096FS = 5,
+	CLK_6144FS = 6,
+	CLK_8192FS = 7,
+};
+
+struct dim_ch_state_t {
+	bool ready; /* Shows readiness to enqueue next buffer */
+	u16 done_buffers; /* Number of completed buffers */
+};
+
+struct int_ch_state {
+	/* changed only in interrupt context */
+	volatile int request_counter;
+
+	/* changed only in task context */
+	volatile int service_counter;
+
+	u8 idx1;
+	u8 idx2;
+	u8 level; /* [0..2], buffering level */
+};
+
+struct dim_channel {
+	struct int_ch_state state;
+	u8 addr;
+	u16 dbr_addr;
+	u16 dbr_size;
+	u16 packet_length; /*< Isochronous packet length in bytes. */
+	u16 bytes_per_frame; /*< Synchronous bytes per frame. */
+	u16 done_sw_buffers_number; /*< Done software buffers number. */
+};
+
+u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock,
+	       u32 fcnt);
+
+void dim_shutdown(void);
+
+bool dim_get_lock_state(void);
+
+u16 dim_norm_ctrl_async_buffer_size(u16 buf_size);
+
+u16 dim_norm_isoc_buffer_size(u16 buf_size, u16 packet_length);
+
+u16 dim_norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame);
+
+u8 dim_init_control(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		    u16 max_buffer_size);
+
+u8 dim_init_async(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		  u16 max_buffer_size);
+
+u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 packet_length);
+
+u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 bytes_per_frame);
+
+u8 dim_destroy_channel(struct dim_channel *ch);
+
+void dim_service_mlb_int_irq(void);
+
+void dim_service_ahb_int_irq(struct dim_channel *const *channels);
+
+u8 dim_service_channel(struct dim_channel *ch);
+
+struct dim_ch_state_t *dim_get_channel_state(struct dim_channel *ch,
+					     struct dim_ch_state_t *state_ptr);
+
+u16 dim_dbr_space(struct dim_channel *ch);
+
+bool dim_enqueue_buffer(struct dim_channel *ch, u32 buffer_addr,
+			u16 buffer_size);
+
+bool dim_detach_buffers(struct dim_channel *ch, u16 buffers_number);
+
+void dimcb_on_error(u8 error_id, const char *error_message);
+
+#endif /* _DIM2_HAL_H */
diff --git a/drivers/staging/most/dim2/reg.h b/drivers/staging/most/dim2/reg.h
new file mode 100644
index 000000000..b0f36c208
--- /dev/null
+++ b/drivers/staging/most/dim2/reg.h
@@ -0,0 +1,157 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * reg.h - Definitions for registers of DIM2
+ * (MediaLB, Device Interface Macro IP, OS62420)
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#ifndef DIM2_OS62420_H
+#define	DIM2_OS62420_H
+
+#include <linux/types.h>
+
+struct dim2_regs {
+	u32 MLBC0;       /* 0x00 */
+	u32 rsvd0[1];    /* 0x01 */
+	u32 MLBPC0;      /* 0x02 */
+	u32 MS0;         /* 0x03 */
+	u32 rsvd1[1];    /* 0x04 */
+	u32 MS1;         /* 0x05 */
+	u32 rsvd2[2];    /* 0x06 */
+	u32 MSS;         /* 0x08 */
+	u32 MSD;         /* 0x09 */
+	u32 rsvd3[1];    /* 0x0A */
+	u32 MIEN;        /* 0x0B */
+	u32 rsvd4[1];    /* 0x0C */
+	u32 MLBPC2;      /* 0x0D */
+	u32 MLBPC1;      /* 0x0E */
+	u32 MLBC1;       /* 0x0F */
+	u32 rsvd5[0x10]; /* 0x10 */
+	u32 HCTL;        /* 0x20 */
+	u32 rsvd6[1];    /* 0x21 */
+	u32 HCMR0;       /* 0x22 */
+	u32 HCMR1;       /* 0x23 */
+	u32 HCER0;       /* 0x24 */
+	u32 HCER1;       /* 0x25 */
+	u32 HCBR0;       /* 0x26 */
+	u32 HCBR1;       /* 0x27 */
+	u32 rsvd7[8];    /* 0x28 */
+	u32 MDAT0;       /* 0x30 */
+	u32 MDAT1;       /* 0x31 */
+	u32 MDAT2;       /* 0x32 */
+	u32 MDAT3;       /* 0x33 */
+	u32 MDWE0;       /* 0x34 */
+	u32 MDWE1;       /* 0x35 */
+	u32 MDWE2;       /* 0x36 */
+	u32 MDWE3;       /* 0x37 */
+	u32 MCTL;        /* 0x38 */
+	u32 MADR;        /* 0x39 */
+	u32 rsvd8[0xb6]; /* 0x3A */
+	u32 ACTL;        /* 0xF0 */
+	u32 rsvd9[3];    /* 0xF1 */
+	u32 ACSR0;       /* 0xF4 */
+	u32 ACSR1;       /* 0xF5 */
+	u32 ACMR0;       /* 0xF6 */
+	u32 ACMR1;       /* 0xF7 */
+};
+
+#define DIM2_MASK(n)  (~((~(u32)0) << (n)))
+
+enum {
+	MLBC0_MLBLK_BIT = 7,
+
+	MLBC0_MLBPEN_BIT = 5,
+
+	MLBC0_MLBCLK_SHIFT = 2,
+	MLBC0_MLBCLK_VAL_256FS = 0,
+	MLBC0_MLBCLK_VAL_512FS = 1,
+	MLBC0_MLBCLK_VAL_1024FS = 2,
+	MLBC0_MLBCLK_VAL_2048FS = 3,
+
+	MLBC0_FCNT_SHIFT = 15,
+	MLBC0_FCNT_MASK = 7,
+	MLBC0_FCNT_MAX_VAL = 6,
+
+	MLBC0_MLBEN_BIT = 0,
+
+	MIEN_CTX_BREAK_BIT = 29,
+	MIEN_CTX_PE_BIT = 28,
+	MIEN_CTX_DONE_BIT = 27,
+
+	MIEN_CRX_BREAK_BIT = 26,
+	MIEN_CRX_PE_BIT = 25,
+	MIEN_CRX_DONE_BIT = 24,
+
+	MIEN_ATX_BREAK_BIT = 22,
+	MIEN_ATX_PE_BIT = 21,
+	MIEN_ATX_DONE_BIT = 20,
+
+	MIEN_ARX_BREAK_BIT = 19,
+	MIEN_ARX_PE_BIT = 18,
+	MIEN_ARX_DONE_BIT = 17,
+
+	MIEN_SYNC_PE_BIT = 16,
+
+	MIEN_ISOC_BUFO_BIT = 1,
+	MIEN_ISOC_PE_BIT = 0,
+
+	MLBC1_NDA_SHIFT = 8,
+	MLBC1_NDA_MASK = 0xFF,
+
+	MLBC1_CLKMERR_BIT = 7,
+	MLBC1_LOCKERR_BIT = 6,
+
+	ACTL_DMA_MODE_BIT = 2,
+	ACTL_DMA_MODE_VAL_DMA_MODE_0 = 0,
+	ACTL_DMA_MODE_VAL_DMA_MODE_1 = 1,
+	ACTL_SCE_BIT = 0,
+
+	HCTL_EN_BIT = 15
+};
+
+enum {
+	CDT0_RPC_SHIFT = 16 + 11,
+	CDT0_RPC_MASK = DIM2_MASK(5),
+
+	CDT1_BS_ISOC_SHIFT = 0,
+	CDT1_BS_ISOC_MASK = DIM2_MASK(9),
+
+	CDT3_BD_SHIFT = 0,
+	CDT3_BD_MASK = DIM2_MASK(12),
+	CDT3_BD_ISOC_MASK = DIM2_MASK(13),
+	CDT3_BA_SHIFT = 16,
+
+	ADT0_CE_BIT = 15,
+	ADT0_LE_BIT = 14,
+	ADT0_PG_BIT = 13,
+
+	ADT1_RDY_BIT = 15,
+	ADT1_DNE_BIT = 14,
+	ADT1_ERR_BIT = 13,
+	ADT1_PS_BIT = 12,
+	ADT1_MEP_BIT = 11,
+	ADT1_BD_SHIFT = 0,
+	ADT1_CTRL_ASYNC_BD_MASK = DIM2_MASK(11),
+	ADT1_ISOC_SYNC_BD_MASK = DIM2_MASK(13),
+
+	CAT_FCE_BIT = 14,
+	CAT_MFE_BIT = 14,
+
+	CAT_MT_BIT = 13,
+
+	CAT_RNW_BIT = 12,
+
+	CAT_CE_BIT = 11,
+
+	CAT_CT_SHIFT = 8,
+	CAT_CT_VAL_SYNC = 0,
+	CAT_CT_VAL_CONTROL = 1,
+	CAT_CT_VAL_ASYNC = 2,
+	CAT_CT_VAL_ISOC = 3,
+
+	CAT_CL_SHIFT = 0,
+	CAT_CL_MASK = DIM2_MASK(6)
+};
+
+#endif	/* DIM2_OS62420_H */
diff --git a/drivers/staging/most/dim2/sysfs.h b/drivers/staging/most/dim2/sysfs.h
new file mode 100644
index 000000000..09115cf4e
--- /dev/null
+++ b/drivers/staging/most/dim2/sysfs.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * sysfs.h - MediaLB sysfs information
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+/* Author: Andrey Shvetsov <andrey.shvetsov@k2l.de> */
+
+#ifndef DIM2_SYSFS_H
+#define	DIM2_SYSFS_H
+
+#include <linux/kobject.h>
+
+struct medialb_bus {
+	struct kobject kobj_group;
+};
+
+#endif	/* DIM2_SYSFS_H */
diff --git a/drivers/staging/most/i2c/Kconfig b/drivers/staging/most/i2c/Kconfig
new file mode 100644
index 000000000..ff64283cb
--- /dev/null
+++ b/drivers/staging/most/i2c/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# MOST I2C configuration
+#
+
+config MOST_I2C
+	tristate "I2C"
+	depends on I2C
+	help
+	  Say Y here if you want to connect via I2C to network transceiver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called most_i2c.
diff --git a/drivers/staging/most/i2c/Makefile b/drivers/staging/most/i2c/Makefile
new file mode 100644
index 000000000..71099dd0f
--- /dev/null
+++ b/drivers/staging/most/i2c/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_MOST_I2C) += most_i2c.o
+
+most_i2c-objs := i2c.o
diff --git a/drivers/staging/most/i2c/i2c.c b/drivers/staging/most/i2c/i2c.c
new file mode 100644
index 000000000..285a071f0
--- /dev/null
+++ b/drivers/staging/most/i2c/i2c.c
@@ -0,0 +1,371 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * i2c.c - Hardware Dependent Module for I2C Interface
+ *
+ * Copyright (C) 2013-2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/most.h>
+
+enum { CH_RX, CH_TX, NUM_CHANNELS };
+
+#define MAX_BUFFERS_CONTROL 32
+#define MAX_BUF_SIZE_CONTROL 256
+
+/**
+ * list_first_mbo - get the first mbo from a list
+ * @ptr:	the list head to take the mbo from.
+ */
+#define list_first_mbo(ptr) \
+	list_first_entry(ptr, struct mbo, list)
+
+static unsigned int polling_rate;
+module_param(polling_rate, uint, 0644);
+MODULE_PARM_DESC(polling_rate, "Polling rate [Hz]. Default = 0 (use IRQ)");
+
+struct hdm_i2c {
+	struct most_interface most_iface;
+	struct most_channel_capability capabilities[NUM_CHANNELS];
+	struct i2c_client *client;
+	struct rx {
+		struct delayed_work dwork;
+		struct list_head list;
+		bool int_disabled;
+		unsigned int delay;
+	} rx;
+	char name[64];
+};
+
+#define to_hdm(iface) container_of(iface, struct hdm_i2c, most_iface)
+
+static irqreturn_t most_irq_handler(int, void *);
+static void pending_rx_work(struct work_struct *);
+
+/**
+ * configure_channel - called from MOST core to configure a channel
+ * @most_iface: interface the channel belongs to
+ * @ch_idx: channel to be configured
+ * @channel_config: structure that holds the configuration information
+ *
+ * Return 0 on success, negative on failure.
+ *
+ * Receives configuration information from MOST core and initialize the
+ * corresponding channel.
+ */
+static int configure_channel(struct most_interface *most_iface,
+			     int ch_idx,
+			     struct most_channel_config *channel_config)
+{
+	int ret;
+	struct hdm_i2c *dev = to_hdm(most_iface);
+	unsigned int delay, pr;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
+
+	if (channel_config->data_type != MOST_CH_CONTROL) {
+		pr_err("bad data type for channel %d\n", ch_idx);
+		return -EPERM;
+	}
+
+	if (channel_config->direction != dev->capabilities[ch_idx].direction) {
+		pr_err("bad direction for channel %d\n", ch_idx);
+		return -EPERM;
+	}
+
+	if (channel_config->direction == MOST_CH_RX) {
+		if (!polling_rate) {
+			if (dev->client->irq <= 0) {
+				pr_err("bad irq: %d\n", dev->client->irq);
+				return -ENOENT;
+			}
+			dev->rx.int_disabled = false;
+			ret = request_irq(dev->client->irq, most_irq_handler, 0,
+					  dev->client->name, dev);
+			if (ret) {
+				pr_err("request_irq(%d) failed: %d\n",
+				       dev->client->irq, ret);
+				return ret;
+			}
+		} else {
+			delay = msecs_to_jiffies(MSEC_PER_SEC / polling_rate);
+			dev->rx.delay = delay ? delay : 1;
+			pr = MSEC_PER_SEC / jiffies_to_msecs(dev->rx.delay);
+			pr_info("polling rate is %u Hz\n", pr);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * enqueue - called from MOST core to enqueue a buffer for data transfer
+ * @most_iface: intended interface
+ * @ch_idx: ID of the channel the buffer is intended for
+ * @mbo: pointer to the buffer object
+ *
+ * Return 0 on success, negative on failure.
+ *
+ * Transmit the data over I2C if it is a "write" request or push the buffer into
+ * list if it is an "read" request
+ */
+static int enqueue(struct most_interface *most_iface,
+		   int ch_idx, struct mbo *mbo)
+{
+	struct hdm_i2c *dev = to_hdm(most_iface);
+	int ret;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
+
+	if (ch_idx == CH_RX) {
+		/* RX */
+		if (!polling_rate)
+			disable_irq(dev->client->irq);
+		cancel_delayed_work_sync(&dev->rx.dwork);
+		list_add_tail(&mbo->list, &dev->rx.list);
+		if (dev->rx.int_disabled || polling_rate)
+			pending_rx_work(&dev->rx.dwork.work);
+		if (!polling_rate)
+			enable_irq(dev->client->irq);
+	} else {
+		/* TX */
+		ret = i2c_master_send(dev->client, mbo->virt_address,
+				      mbo->buffer_length);
+		if (ret <= 0) {
+			mbo->processed_length = 0;
+			mbo->status = MBO_E_INVAL;
+		} else {
+			mbo->processed_length = mbo->buffer_length;
+			mbo->status = MBO_SUCCESS;
+		}
+		mbo->complete(mbo);
+	}
+
+	return 0;
+}
+
+/**
+ * poison_channel - called from MOST core to poison buffers of a channel
+ * @most_iface: pointer to the interface the channel to be poisoned belongs to
+ * @ch_idx: corresponding channel ID
+ *
+ * Return 0 on success, negative on failure.
+ *
+ * If channel direction is RX, complete the buffers in list with
+ * status MBO_E_CLOSE
+ */
+static int poison_channel(struct most_interface *most_iface,
+			  int ch_idx)
+{
+	struct hdm_i2c *dev = to_hdm(most_iface);
+	struct mbo *mbo;
+
+	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
+
+	if (ch_idx == CH_RX) {
+		if (!polling_rate)
+			free_irq(dev->client->irq, dev);
+		cancel_delayed_work_sync(&dev->rx.dwork);
+
+		while (!list_empty(&dev->rx.list)) {
+			mbo = list_first_mbo(&dev->rx.list);
+			list_del(&mbo->list);
+
+			mbo->processed_length = 0;
+			mbo->status = MBO_E_CLOSE;
+			mbo->complete(mbo);
+		}
+	}
+
+	return 0;
+}
+
+static void do_rx_work(struct hdm_i2c *dev)
+{
+	struct mbo *mbo;
+	unsigned char msg[MAX_BUF_SIZE_CONTROL];
+	int ret;
+	u16 pml, data_size;
+
+	/* Read PML (2 bytes) */
+	ret = i2c_master_recv(dev->client, msg, 2);
+	if (ret <= 0) {
+		pr_err("Failed to receive PML\n");
+		return;
+	}
+
+	pml = (msg[0] << 8) | msg[1];
+	if (!pml)
+		return;
+
+	data_size = pml + 2;
+
+	/* Read the whole message, including PML */
+	ret = i2c_master_recv(dev->client, msg, data_size);
+	if (ret <= 0) {
+		pr_err("Failed to receive a Port Message\n");
+		return;
+	}
+
+	mbo = list_first_mbo(&dev->rx.list);
+	list_del(&mbo->list);
+
+	mbo->processed_length = min(data_size, mbo->buffer_length);
+	memcpy(mbo->virt_address, msg, mbo->processed_length);
+	mbo->status = MBO_SUCCESS;
+	mbo->complete(mbo);
+}
+
+/**
+ * pending_rx_work - Read pending messages through I2C
+ * @work: definition of this work item
+ *
+ * Invoked by the Interrupt Service Routine, most_irq_handler()
+ */
+static void pending_rx_work(struct work_struct *work)
+{
+	struct hdm_i2c *dev = container_of(work, struct hdm_i2c, rx.dwork.work);
+
+	if (list_empty(&dev->rx.list))
+		return;
+
+	do_rx_work(dev);
+
+	if (polling_rate) {
+		schedule_delayed_work(&dev->rx.dwork, dev->rx.delay);
+	} else {
+		dev->rx.int_disabled = false;
+		enable_irq(dev->client->irq);
+	}
+}
+
+/*
+ * most_irq_handler - Interrupt Service Routine
+ * @irq: irq number
+ * @_dev: private data
+ *
+ * Schedules a delayed work
+ *
+ * By default the interrupt line behavior is Active Low. Once an interrupt is
+ * generated by the device, until driver clears the interrupt (by reading
+ * the PMP message), device keeps the interrupt line in low state. Since i2c
+ * read is done in work queue, the interrupt line must be disabled temporarily
+ * to avoid ISR being called repeatedly. Re-enable the interrupt in workqueue,
+ * after reading the message.
+ *
+ * Note: If we use the interrupt line in Falling edge mode, there is a
+ * possibility to miss interrupts when ISR is getting executed.
+ *
+ */
+static irqreturn_t most_irq_handler(int irq, void *_dev)
+{
+	struct hdm_i2c *dev = _dev;
+
+	disable_irq_nosync(irq);
+	dev->rx.int_disabled = true;
+	schedule_delayed_work(&dev->rx.dwork, 0);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * i2c_probe - i2c probe handler
+ * @client: i2c client device structure
+ * @id: i2c client device id
+ *
+ * Return 0 on success, negative on failure.
+ *
+ * Register the i2c client device as a MOST interface
+ */
+static int i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct hdm_i2c *dev;
+	int ret, i;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	/* ID format: i2c-<bus>-<address> */
+	snprintf(dev->name, sizeof(dev->name), "i2c-%d-%04x",
+		 client->adapter->nr, client->addr);
+
+	for (i = 0; i < NUM_CHANNELS; i++) {
+		dev->capabilities[i].data_type = MOST_CH_CONTROL;
+		dev->capabilities[i].num_buffers_packet = MAX_BUFFERS_CONTROL;
+		dev->capabilities[i].buffer_size_packet = MAX_BUF_SIZE_CONTROL;
+	}
+	dev->capabilities[CH_RX].direction = MOST_CH_RX;
+	dev->capabilities[CH_RX].name_suffix = "rx";
+	dev->capabilities[CH_TX].direction = MOST_CH_TX;
+	dev->capabilities[CH_TX].name_suffix = "tx";
+
+	dev->most_iface.interface = ITYPE_I2C;
+	dev->most_iface.description = dev->name;
+	dev->most_iface.num_channels = NUM_CHANNELS;
+	dev->most_iface.channel_vector = dev->capabilities;
+	dev->most_iface.configure = configure_channel;
+	dev->most_iface.enqueue = enqueue;
+	dev->most_iface.poison_channel = poison_channel;
+
+	INIT_LIST_HEAD(&dev->rx.list);
+
+	INIT_DELAYED_WORK(&dev->rx.dwork, pending_rx_work);
+
+	dev->client = client;
+	i2c_set_clientdata(client, dev);
+
+	ret = most_register_interface(&dev->most_iface);
+	if (ret) {
+		pr_err("Failed to register i2c as a MOST interface\n");
+		kfree(dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * i2c_remove - i2c remove handler
+ * @client: i2c client device structure
+ *
+ * Return 0 on success.
+ *
+ * Unregister the i2c client device as a MOST interface
+ */
+static void i2c_remove(struct i2c_client *client)
+{
+	struct hdm_i2c *dev = i2c_get_clientdata(client);
+
+	most_deregister_interface(&dev->most_iface);
+	kfree(dev);
+}
+
+static const struct i2c_device_id i2c_id[] = {
+	{ "most_i2c", 0 },
+	{ }, /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(i2c, i2c_id);
+
+static struct i2c_driver i2c_driver = {
+	.driver = {
+		.name = "hdm_i2c",
+	},
+	.probe = i2c_probe,
+	.remove = i2c_remove,
+	.id_table = i2c_id,
+};
+
+module_i2c_driver(i2c_driver);
+
+MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
+MODULE_DESCRIPTION("I2C Hardware Dependent Module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/most/net/Kconfig b/drivers/staging/most/net/Kconfig
new file mode 100644
index 000000000..ed8ac7e07
--- /dev/null
+++ b/drivers/staging/most/net/Kconfig
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# MOST Networking configuration
+#
+
+config MOST_NET
+	tristate "Net"
+	depends on NET
+
+	help
+	  Say Y here if you want to commumicate via a networking device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called most_net.
diff --git a/drivers/staging/most/net/Makefile b/drivers/staging/most/net/Makefile
new file mode 100644
index 000000000..1582c97eb
--- /dev/null
+++ b/drivers/staging/most/net/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_MOST_NET) += most_net.o
+
+most_net-objs := net.o
diff --git a/drivers/staging/most/net/net.c b/drivers/staging/most/net/net.c
new file mode 100644
index 000000000..1d1fe8bff
--- /dev/null
+++ b/drivers/staging/most/net/net.c
@@ -0,0 +1,581 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * net.c - Networking component for Mostcore
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/wait.h>
+#include <linux/kobject.h>
+#include <linux/most.h>
+
+#define MEP_HDR_LEN 8
+#define MDP_HDR_LEN 16
+#define MAMAC_DATA_LEN (1024 - MDP_HDR_LEN)
+
+#define PMHL 5
+
+#define PMS_TELID_UNSEGM_MAMAC	0x0A
+#define PMS_FIFONO_MDP		0x01
+#define PMS_FIFONO_MEP		0x04
+#define PMS_MSGTYPE_DATA	0x04
+#define PMS_DEF_PRIO		0
+#define MEP_DEF_RETRY		15
+
+#define PMS_FIFONO_MASK		0x07
+#define PMS_FIFONO_SHIFT	3
+#define PMS_RETRY_SHIFT		4
+#define PMS_TELID_MASK		0x0F
+#define PMS_TELID_SHIFT		4
+
+#define HB(value)		((u8)((u16)(value) >> 8))
+#define LB(value)		((u8)(value))
+
+#define EXTRACT_BIT_SET(bitset_name, value) \
+	(((value) >> bitset_name##_SHIFT) & bitset_name##_MASK)
+
+#define PMS_IS_MEP(buf, len) \
+	((len) > MEP_HDR_LEN && \
+	 EXTRACT_BIT_SET(PMS_FIFONO, (buf)[3]) == PMS_FIFONO_MEP)
+
+static inline bool pms_is_mamac(char *buf, u32 len)
+{
+	return (len > MDP_HDR_LEN &&
+		EXTRACT_BIT_SET(PMS_FIFONO, buf[3]) == PMS_FIFONO_MDP &&
+		EXTRACT_BIT_SET(PMS_TELID, buf[14]) == PMS_TELID_UNSEGM_MAMAC);
+}
+
+struct net_dev_channel {
+	bool linked;
+	int ch_id;
+};
+
+struct net_dev_context {
+	struct most_interface *iface;
+	bool is_mamac;
+	struct net_device *dev;
+	struct net_dev_channel rx;
+	struct net_dev_channel tx;
+	struct list_head list;
+};
+
+static LIST_HEAD(net_devices);
+static DEFINE_MUTEX(probe_disc_mt); /* ch->linked = true, most_nd_open */
+static DEFINE_SPINLOCK(list_lock); /* list_head, ch->linked = false, dev_hold */
+static struct most_component comp;
+
+static int skb_to_mamac(const struct sk_buff *skb, struct mbo *mbo)
+{
+	u8 *buff = mbo->virt_address;
+	static const u8 broadcast[] = { 0x03, 0xFF };
+	const u8 *dest_addr = skb->data + 4;
+	const u8 *eth_type = skb->data + 12;
+	unsigned int payload_len = skb->len - ETH_HLEN;
+	unsigned int mdp_len = payload_len + MDP_HDR_LEN;
+
+	if (mdp_len < skb->len) {
+		pr_err("drop: too large packet! (%u)\n", skb->len);
+		return -EINVAL;
+	}
+
+	if (mbo->buffer_length < mdp_len) {
+		pr_err("drop: too small buffer! (%d for %d)\n",
+		       mbo->buffer_length, mdp_len);
+		return -EINVAL;
+	}
+
+	if (skb->len < ETH_HLEN) {
+		pr_err("drop: too small packet! (%d)\n", skb->len);
+		return -EINVAL;
+	}
+
+	if (dest_addr[0] == 0xFF && dest_addr[1] == 0xFF)
+		dest_addr = broadcast;
+
+	*buff++ = HB(mdp_len - 2);
+	*buff++ = LB(mdp_len - 2);
+
+	*buff++ = PMHL;
+	*buff++ = (PMS_FIFONO_MDP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
+	*buff++ = PMS_DEF_PRIO;
+	*buff++ = dest_addr[0];
+	*buff++ = dest_addr[1];
+	*buff++ = 0x00;
+
+	*buff++ = HB(payload_len + 6);
+	*buff++ = LB(payload_len + 6);
+
+	/* end of FPH here */
+
+	*buff++ = eth_type[0];
+	*buff++ = eth_type[1];
+	*buff++ = 0;
+	*buff++ = 0;
+
+	*buff++ = PMS_TELID_UNSEGM_MAMAC << 4 | HB(payload_len);
+	*buff++ = LB(payload_len);
+
+	memcpy(buff, skb->data + ETH_HLEN, payload_len);
+	mbo->buffer_length = mdp_len;
+	return 0;
+}
+
+static int skb_to_mep(const struct sk_buff *skb, struct mbo *mbo)
+{
+	u8 *buff = mbo->virt_address;
+	unsigned int mep_len = skb->len + MEP_HDR_LEN;
+
+	if (mep_len < skb->len) {
+		pr_err("drop: too large packet! (%u)\n", skb->len);
+		return -EINVAL;
+	}
+
+	if (mbo->buffer_length < mep_len) {
+		pr_err("drop: too small buffer! (%d for %d)\n",
+		       mbo->buffer_length, mep_len);
+		return -EINVAL;
+	}
+
+	*buff++ = HB(mep_len - 2);
+	*buff++ = LB(mep_len - 2);
+
+	*buff++ = PMHL;
+	*buff++ = (PMS_FIFONO_MEP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
+	*buff++ = (MEP_DEF_RETRY << PMS_RETRY_SHIFT) | PMS_DEF_PRIO;
+	*buff++ = 0;
+	*buff++ = 0;
+	*buff++ = 0;
+
+	memcpy(buff, skb->data, skb->len);
+	mbo->buffer_length = mep_len;
+	return 0;
+}
+
+static int most_nd_set_mac_address(struct net_device *dev, void *p)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	int err = eth_mac_addr(dev, p);
+
+	if (err)
+		return err;
+
+	nd->is_mamac =
+		(dev->dev_addr[0] == 0 && dev->dev_addr[1] == 0 &&
+		 dev->dev_addr[2] == 0 && dev->dev_addr[3] == 0);
+
+	/*
+	 * Set default MTU for the given packet type.
+	 * It is still possible to change MTU using ip tools afterwards.
+	 */
+	dev->mtu = nd->is_mamac ? MAMAC_DATA_LEN : ETH_DATA_LEN;
+
+	return 0;
+}
+
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr);
+
+static int most_nd_open(struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	int ret = 0;
+
+	mutex_lock(&probe_disc_mt);
+
+	if (most_start_channel(nd->iface, nd->rx.ch_id, &comp)) {
+		netdev_err(dev, "most_start_channel() failed\n");
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	if (most_start_channel(nd->iface, nd->tx.ch_id, &comp)) {
+		netdev_err(dev, "most_start_channel() failed\n");
+		most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	netif_carrier_off(dev);
+	if (is_valid_ether_addr(dev->dev_addr))
+		netif_dormant_off(dev);
+	else
+		netif_dormant_on(dev);
+	netif_wake_queue(dev);
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, on_netinfo);
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int most_nd_stop(struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, NULL);
+	most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
+	most_stop_channel(nd->iface, nd->tx.ch_id, &comp);
+
+	return 0;
+}
+
+static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
+				      struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	struct mbo *mbo;
+	int ret;
+
+	mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &comp);
+
+	if (!mbo) {
+		netif_stop_queue(dev);
+		dev->stats.tx_fifo_errors++;
+		return NETDEV_TX_BUSY;
+	}
+
+	if (nd->is_mamac)
+		ret = skb_to_mamac(skb, mbo);
+	else
+		ret = skb_to_mep(skb, mbo);
+
+	if (ret) {
+		most_put_mbo(mbo);
+		dev->stats.tx_dropped++;
+		kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	most_submit_mbo(mbo);
+	dev->stats.tx_packets++;
+	dev->stats.tx_bytes += skb->len;
+	kfree_skb(skb);
+	return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops most_nd_ops = {
+	.ndo_open = most_nd_open,
+	.ndo_stop = most_nd_stop,
+	.ndo_start_xmit = most_nd_start_xmit,
+	.ndo_set_mac_address = most_nd_set_mac_address,
+};
+
+static void most_nd_setup(struct net_device *dev)
+{
+	ether_setup(dev);
+	dev->netdev_ops = &most_nd_ops;
+}
+
+static struct net_dev_context *get_net_dev(struct most_interface *iface)
+{
+	struct net_dev_context *nd;
+
+	list_for_each_entry(nd, &net_devices, list)
+		if (nd->iface == iface)
+			return nd;
+	return NULL;
+}
+
+static struct net_dev_context *get_net_dev_hold(struct most_interface *iface)
+{
+	struct net_dev_context *nd;
+	unsigned long flags;
+
+	spin_lock_irqsave(&list_lock, flags);
+	nd = get_net_dev(iface);
+	if (nd && nd->rx.linked && nd->tx.linked)
+		dev_hold(nd->dev);
+	else
+		nd = NULL;
+	spin_unlock_irqrestore(&list_lock, flags);
+	return nd;
+}
+
+static int comp_probe_channel(struct most_interface *iface, int channel_idx,
+			      struct most_channel_config *ccfg, char *name,
+			      char *args)
+{
+	struct net_dev_context *nd;
+	struct net_dev_channel *ch;
+	struct net_device *dev;
+	unsigned long flags;
+	int ret = 0;
+
+	if (!iface)
+		return -EINVAL;
+
+	if (ccfg->data_type != MOST_CH_ASYNC)
+		return -EINVAL;
+
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
+	if (!nd) {
+		dev = alloc_netdev(sizeof(struct net_dev_context), "meth%d",
+				   NET_NAME_UNKNOWN, most_nd_setup);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+
+		nd = netdev_priv(dev);
+		nd->iface = iface;
+		nd->dev = dev;
+
+		spin_lock_irqsave(&list_lock, flags);
+		list_add(&nd->list, &net_devices);
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+	} else {
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+		if (ch->linked) {
+			pr_err("direction is allocated\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+
+		if (register_netdev(nd->dev)) {
+			pr_err("register_netdev() failed\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+	ch->ch_id = channel_idx;
+	ch->linked = true;
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int comp_disconnect_channel(struct most_interface *iface,
+				   int channel_idx)
+{
+	struct net_dev_context *nd;
+	struct net_dev_channel *ch;
+	unsigned long flags;
+	int ret = 0;
+
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
+	if (!nd) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (nd->rx.linked && channel_idx == nd->rx.ch_id) {
+		ch = &nd->rx;
+	} else if (nd->tx.linked && channel_idx == nd->tx.ch_id) {
+		ch = &nd->tx;
+	} else {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (nd->rx.linked && nd->tx.linked) {
+		spin_lock_irqsave(&list_lock, flags);
+		ch->linked = false;
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		/*
+		 * do not call most_stop_channel() here, because channels are
+		 * going to be closed in ndo_stop() after unregister_netdev()
+		 */
+		unregister_netdev(nd->dev);
+	} else {
+		spin_lock_irqsave(&list_lock, flags);
+		list_del(&nd->list);
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		free_netdev(nd->dev);
+	}
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int comp_resume_tx_channel(struct most_interface *iface,
+				  int channel_idx)
+{
+	struct net_dev_context *nd;
+
+	nd = get_net_dev_hold(iface);
+	if (!nd)
+		return 0;
+
+	if (nd->tx.ch_id != channel_idx)
+		goto put_nd;
+
+	netif_wake_queue(nd->dev);
+
+put_nd:
+	dev_put(nd->dev);
+	return 0;
+}
+
+static int comp_rx_data(struct mbo *mbo)
+{
+	const u32 zero = 0;
+	struct net_dev_context *nd;
+	char *buf = mbo->virt_address;
+	u32 len = mbo->processed_length;
+	struct sk_buff *skb;
+	struct net_device *dev;
+	unsigned int skb_len;
+	int ret = 0;
+
+	nd = get_net_dev_hold(mbo->ifp);
+	if (!nd)
+		return -EIO;
+
+	if (nd->rx.ch_id != mbo->hdm_channel_id) {
+		ret = -EIO;
+		goto put_nd;
+	}
+
+	dev = nd->dev;
+
+	if (nd->is_mamac) {
+		if (!pms_is_mamac(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
+
+		skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
+	} else {
+		if (!PMS_IS_MEP(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
+
+		skb = dev_alloc_skb(len - MEP_HDR_LEN);
+	}
+
+	if (!skb) {
+		dev->stats.rx_dropped++;
+		pr_err_once("drop packet: no memory for skb\n");
+		goto out;
+	}
+
+	skb->dev = dev;
+
+	if (nd->is_mamac) {
+		/* dest */
+		ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
+
+		/* src */
+		skb_put_data(skb, &zero, 4);
+		skb_put_data(skb, buf + 5, 2);
+
+		/* eth type */
+		skb_put_data(skb, buf + 10, 2);
+
+		buf += MDP_HDR_LEN;
+		len -= MDP_HDR_LEN;
+	} else {
+		buf += MEP_HDR_LEN;
+		len -= MEP_HDR_LEN;
+	}
+
+	skb_put_data(skb, buf, len);
+	skb->protocol = eth_type_trans(skb, dev);
+	skb_len = skb->len;
+	if (netif_rx(skb) == NET_RX_SUCCESS) {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += skb_len;
+	} else {
+		dev->stats.rx_dropped++;
+	}
+
+out:
+	most_put_mbo(mbo);
+
+put_nd:
+	dev_put(nd->dev);
+	return ret;
+}
+
+static struct most_component comp = {
+	.mod = THIS_MODULE,
+	.name = "net",
+	.probe_channel = comp_probe_channel,
+	.disconnect_channel = comp_disconnect_channel,
+	.tx_completion = comp_resume_tx_channel,
+	.rx_completion = comp_rx_data,
+};
+
+static int __init most_net_init(void)
+{
+	int err;
+
+	err = most_register_component(&comp);
+	if (err)
+		return err;
+	err = most_register_configfs_subsys(&comp);
+	if (err) {
+		most_deregister_component(&comp);
+		return err;
+	}
+	return 0;
+}
+
+static void __exit most_net_exit(void)
+{
+	most_deregister_configfs_subsys(&comp);
+	most_deregister_component(&comp);
+}
+
+/**
+ * on_netinfo - callback for HDM to be informed about HW's MAC
+ * @iface: most interface instance
+ * @link_stat: link status
+ * @mac_addr: MAC address
+ */
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr)
+{
+	struct net_dev_context *nd;
+	struct net_device *dev;
+	const u8 *m = mac_addr;
+
+	nd = get_net_dev_hold(iface);
+	if (!nd)
+		return;
+
+	dev = nd->dev;
+
+	if (link_stat)
+		netif_carrier_on(dev);
+	else
+		netif_carrier_off(dev);
+
+	if (m && is_valid_ether_addr(m)) {
+		if (!is_valid_ether_addr(dev->dev_addr)) {
+			netdev_info(dev, "set mac %pM\n", m);
+			eth_hw_addr_set(dev, m);
+			netif_dormant_off(dev);
+		} else if (!ether_addr_equal(dev->dev_addr, m)) {
+			netdev_warn(dev, "reject mac %pM\n", m);
+		}
+	}
+
+	dev_put(nd->dev);
+}
+
+module_init(most_net_init);
+module_exit(most_net_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
+MODULE_DESCRIPTION("Networking Component Module for Mostcore");
diff --git a/drivers/staging/most/video/Kconfig b/drivers/staging/most/video/Kconfig
new file mode 100644
index 000000000..e16cc5e10
--- /dev/null
+++ b/drivers/staging/most/video/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# MOST V4L2 configuration
+#
+
+config MOST_VIDEO
+	tristate "Video"
+	depends on VIDEO_DEV
+	help
+	  Say Y here if you want to commumicate via Video 4 Linux.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called most_video.
diff --git a/drivers/staging/most/video/Makefile b/drivers/staging/most/video/Makefile
new file mode 100644
index 000000000..856175fec
--- /dev/null
+++ b/drivers/staging/most/video/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_MOST_VIDEO) += most_video.o
+
+most_video-objs := video.o
diff --git a/drivers/staging/most/video/video.c b/drivers/staging/most/video/video.c
new file mode 100644
index 000000000..ffa97ef21
--- /dev/null
+++ b/drivers/staging/most/video/video.c
@@ -0,0 +1,584 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * video.c - V4L2 component for Mostcore
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/suspend.h>
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <linux/most.h>
+
+#define V4L2_CMP_MAX_INPUT  1
+
+static struct most_component comp;
+
+struct most_video_dev {
+	struct most_interface *iface;
+	int ch_idx;
+	struct list_head list;
+	bool mute;
+
+	struct list_head pending_mbos;
+	spinlock_t list_lock;
+
+	struct v4l2_device v4l2_dev;
+	atomic_t access_ref;
+	struct video_device *vdev;
+	unsigned int ctrl_input;
+
+	struct mutex lock;
+
+	wait_queue_head_t wait_data;
+};
+
+struct comp_fh {
+	/* must be the first field of this struct! */
+	struct v4l2_fh fh;
+	struct most_video_dev *mdev;
+	u32 offs;
+};
+
+static LIST_HEAD(video_devices);
+static DEFINE_SPINLOCK(list_lock);
+
+static inline bool data_ready(struct most_video_dev *mdev)
+{
+	return !list_empty(&mdev->pending_mbos);
+}
+
+static inline struct mbo *get_top_mbo(struct most_video_dev *mdev)
+{
+	return list_first_entry(&mdev->pending_mbos, struct mbo, list);
+}
+
+static int comp_vdev_open(struct file *filp)
+{
+	int ret;
+	struct video_device *vdev = video_devdata(filp);
+	struct most_video_dev *mdev = video_drvdata(filp);
+	struct comp_fh *fh;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_VIDEO:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+	if (!fh)
+		return -ENOMEM;
+
+	if (!atomic_inc_and_test(&mdev->access_ref)) {
+		v4l2_err(&mdev->v4l2_dev, "too many clients\n");
+		ret = -EBUSY;
+		goto err_dec;
+	}
+
+	fh->mdev = mdev;
+	v4l2_fh_init(&fh->fh, vdev);
+	filp->private_data = fh;
+
+	v4l2_fh_add(&fh->fh);
+
+	ret = most_start_channel(mdev->iface, mdev->ch_idx, &comp);
+	if (ret) {
+		v4l2_err(&mdev->v4l2_dev, "most_start_channel() failed\n");
+		goto err_rm;
+	}
+
+	return 0;
+
+err_rm:
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+
+err_dec:
+	atomic_dec(&mdev->access_ref);
+	kfree(fh);
+	return ret;
+}
+
+static int comp_vdev_close(struct file *filp)
+{
+	struct comp_fh *fh = filp->private_data;
+	struct most_video_dev *mdev = fh->mdev;
+	struct mbo *mbo, *tmp;
+
+	/*
+	 * We need to put MBOs back before we call most_stop_channel()
+	 * to deallocate MBOs.
+	 * From the other hand mostcore still calling rx_completion()
+	 * to deliver MBOs until most_stop_channel() is called.
+	 * Use mute to work around this issue.
+	 * This must be implemented in core.
+	 */
+
+	spin_lock_irq(&mdev->list_lock);
+	mdev->mute = true;
+	list_for_each_entry_safe(mbo, tmp, &mdev->pending_mbos, list) {
+		list_del(&mbo->list);
+		spin_unlock_irq(&mdev->list_lock);
+		most_put_mbo(mbo);
+		spin_lock_irq(&mdev->list_lock);
+	}
+	spin_unlock_irq(&mdev->list_lock);
+	most_stop_channel(mdev->iface, mdev->ch_idx, &comp);
+	mdev->mute = false;
+
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+
+	atomic_dec(&mdev->access_ref);
+	kfree(fh);
+	return 0;
+}
+
+static ssize_t comp_vdev_read(struct file *filp, char __user *buf,
+			      size_t count, loff_t *pos)
+{
+	struct comp_fh *fh = filp->private_data;
+	struct most_video_dev *mdev = fh->mdev;
+	int ret = 0;
+
+	if (*pos)
+		return -ESPIPE;
+
+	if (!mdev)
+		return -ENODEV;
+
+	/* wait for the first buffer */
+	if (!(filp->f_flags & O_NONBLOCK)) {
+		if (wait_event_interruptible(mdev->wait_data, data_ready(mdev)))
+			return -ERESTARTSYS;
+	}
+
+	if (!data_ready(mdev))
+		return -EAGAIN;
+
+	while (count > 0 && data_ready(mdev)) {
+		struct mbo *const mbo = get_top_mbo(mdev);
+		int const rem = mbo->processed_length - fh->offs;
+		int const cnt = rem < count ? rem : count;
+
+		if (copy_to_user(buf, mbo->virt_address + fh->offs, cnt)) {
+			v4l2_err(&mdev->v4l2_dev, "read: copy_to_user failed\n");
+			if (!ret)
+				ret = -EFAULT;
+			return ret;
+		}
+
+		fh->offs += cnt;
+		count -= cnt;
+		buf += cnt;
+		ret += cnt;
+
+		if (cnt >= rem) {
+			fh->offs = 0;
+			spin_lock_irq(&mdev->list_lock);
+			list_del(&mbo->list);
+			spin_unlock_irq(&mdev->list_lock);
+			most_put_mbo(mbo);
+		}
+	}
+	return ret;
+}
+
+static __poll_t comp_vdev_poll(struct file *filp, poll_table *wait)
+{
+	struct comp_fh *fh = filp->private_data;
+	struct most_video_dev *mdev = fh->mdev;
+	__poll_t mask = 0;
+
+	/* only wait if no data is available */
+	if (!data_ready(mdev))
+		poll_wait(filp, &mdev->wait_data, wait);
+	if (data_ready(mdev))
+		mask |= EPOLLIN | EPOLLRDNORM;
+
+	return mask;
+}
+
+static void comp_set_format_struct(struct v4l2_format *f)
+{
+	f->fmt.pix.width = 8;
+	f->fmt.pix.height = 8;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = 188 * 2;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.priv = 0;
+}
+
+static int comp_set_format(struct most_video_dev *mdev, unsigned int cmd,
+			   struct v4l2_format *format)
+{
+	if (format->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG)
+		return -EINVAL;
+
+	if (cmd == VIDIOC_TRY_FMT)
+		return 0;
+
+	comp_set_format_struct(format);
+
+	return 0;
+}
+
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+
+	strscpy(cap->driver, "v4l2_component", sizeof(cap->driver));
+	strscpy(cap->card, "MOST", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "%s", mdev->iface->description);
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+
+	strscpy(f->description, "MPEG", sizeof(f->description));
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	f->flags = V4L2_FMT_FLAG_COMPRESSED;
+	f->pixelformat = V4L2_PIX_FMT_MPEG;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	comp_set_format_struct(f);
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+
+	return comp_set_format(mdev, VIDIOC_TRY_FMT, f);
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+
+	return comp_set_format(mdev, VIDIOC_S_FMT, f);
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
+{
+	*norm = V4L2_STD_UNKNOWN;
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *input)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+
+	if (input->index >= V4L2_CMP_MAX_INPUT)
+		return -EINVAL;
+
+	strscpy(input->name, "MOST Video", sizeof(input->name));
+	input->type |= V4L2_INPUT_TYPE_CAMERA;
+	input->audioset = 0;
+
+	input->std = mdev->vdev->tvnorms;
+
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+	*i = mdev->ctrl_input;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct comp_fh *fh = priv;
+	struct most_video_dev *mdev = fh->mdev;
+
+	if (index >= V4L2_CMP_MAX_INPUT)
+		return -EINVAL;
+	mdev->ctrl_input = index;
+	return 0;
+}
+
+static const struct v4l2_file_operations comp_fops = {
+	.owner      = THIS_MODULE,
+	.open       = comp_vdev_open,
+	.release    = comp_vdev_close,
+	.read       = comp_vdev_read,
+	.poll       = comp_vdev_poll,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops video_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_std               = vidioc_g_std,
+	.vidioc_enum_input          = vidioc_enum_input,
+	.vidioc_g_input             = vidioc_g_input,
+	.vidioc_s_input             = vidioc_s_input,
+};
+
+static const struct video_device comp_videodev_template = {
+	.fops = &comp_fops,
+	.release = video_device_release,
+	.ioctl_ops = &video_ioctl_ops,
+	.tvnorms = V4L2_STD_UNKNOWN,
+	.device_caps = V4L2_CAP_READWRITE | V4L2_CAP_VIDEO_CAPTURE,
+};
+
+/**************************************************************************/
+
+static struct most_video_dev *get_comp_dev(
+	struct most_interface *iface, int channel_idx)
+{
+	struct most_video_dev *mdev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&list_lock, flags);
+	list_for_each_entry(mdev, &video_devices, list) {
+		if (mdev->iface == iface && mdev->ch_idx == channel_idx) {
+			spin_unlock_irqrestore(&list_lock, flags);
+			return mdev;
+		}
+	}
+	spin_unlock_irqrestore(&list_lock, flags);
+	return NULL;
+}
+
+static int comp_rx_data(struct mbo *mbo)
+{
+	unsigned long flags;
+	struct most_video_dev *mdev =
+		get_comp_dev(mbo->ifp, mbo->hdm_channel_id);
+
+	if (!mdev)
+		return -EIO;
+
+	spin_lock_irqsave(&mdev->list_lock, flags);
+	if (unlikely(mdev->mute)) {
+		spin_unlock_irqrestore(&mdev->list_lock, flags);
+		return -EIO;
+	}
+
+	list_add_tail(&mbo->list, &mdev->pending_mbos);
+	spin_unlock_irqrestore(&mdev->list_lock, flags);
+	wake_up_interruptible(&mdev->wait_data);
+	return 0;
+}
+
+static int comp_register_videodev(struct most_video_dev *mdev)
+{
+	int ret;
+
+	init_waitqueue_head(&mdev->wait_data);
+
+	/* allocate and fill v4l2 video struct */
+	mdev->vdev = video_device_alloc();
+	if (!mdev->vdev)
+		return -ENOMEM;
+
+	/* Fill the video capture device struct */
+	*mdev->vdev = comp_videodev_template;
+	mdev->vdev->v4l2_dev = &mdev->v4l2_dev;
+	mdev->vdev->lock = &mdev->lock;
+	snprintf(mdev->vdev->name, sizeof(mdev->vdev->name), "MOST: %s",
+		 mdev->v4l2_dev.name);
+
+	/* Register the v4l2 device */
+	video_set_drvdata(mdev->vdev, mdev);
+	ret = video_register_device(mdev->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		v4l2_err(&mdev->v4l2_dev, "video_register_device failed (%d)\n",
+			 ret);
+		video_device_release(mdev->vdev);
+	}
+
+	return ret;
+}
+
+static void comp_unregister_videodev(struct most_video_dev *mdev)
+{
+	video_unregister_device(mdev->vdev);
+}
+
+static void comp_v4l2_dev_release(struct v4l2_device *v4l2_dev)
+{
+	struct most_video_dev *mdev =
+		container_of(v4l2_dev, struct most_video_dev, v4l2_dev);
+
+	v4l2_device_unregister(v4l2_dev);
+	kfree(mdev);
+}
+
+static int comp_probe_channel(struct most_interface *iface, int channel_idx,
+			      struct most_channel_config *ccfg, char *name,
+			      char *args)
+{
+	int ret;
+	struct most_video_dev *mdev = get_comp_dev(iface, channel_idx);
+
+	if (mdev) {
+		pr_err("channel already linked\n");
+		return -EEXIST;
+	}
+
+	if (ccfg->direction != MOST_CH_RX) {
+		pr_err("wrong direction, expect rx\n");
+		return -EINVAL;
+	}
+
+	if (ccfg->data_type != MOST_CH_SYNC &&
+	    ccfg->data_type != MOST_CH_ISOC) {
+		pr_err("wrong channel type, expect sync or isoc\n");
+		return -EINVAL;
+	}
+
+	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
+	if (!mdev)
+		return -ENOMEM;
+
+	mutex_init(&mdev->lock);
+	atomic_set(&mdev->access_ref, -1);
+	spin_lock_init(&mdev->list_lock);
+	INIT_LIST_HEAD(&mdev->pending_mbos);
+	mdev->iface = iface;
+	mdev->ch_idx = channel_idx;
+	mdev->v4l2_dev.release = comp_v4l2_dev_release;
+
+	/* Create the v4l2_device */
+	strscpy(mdev->v4l2_dev.name, name, sizeof(mdev->v4l2_dev.name));
+	ret = v4l2_device_register(NULL, &mdev->v4l2_dev);
+	if (ret) {
+		pr_err("v4l2_device_register() failed\n");
+		kfree(mdev);
+		return ret;
+	}
+
+	ret = comp_register_videodev(mdev);
+	if (ret)
+		goto err_unreg;
+
+	spin_lock_irq(&list_lock);
+	list_add(&mdev->list, &video_devices);
+	spin_unlock_irq(&list_lock);
+	return 0;
+
+err_unreg:
+	v4l2_device_disconnect(&mdev->v4l2_dev);
+	v4l2_device_put(&mdev->v4l2_dev);
+	return ret;
+}
+
+static int comp_disconnect_channel(struct most_interface *iface,
+				   int channel_idx)
+{
+	struct most_video_dev *mdev = get_comp_dev(iface, channel_idx);
+
+	if (!mdev) {
+		pr_err("no such channel is linked\n");
+		return -ENOENT;
+	}
+
+	spin_lock_irq(&list_lock);
+	list_del(&mdev->list);
+	spin_unlock_irq(&list_lock);
+
+	comp_unregister_videodev(mdev);
+	v4l2_device_disconnect(&mdev->v4l2_dev);
+	v4l2_device_put(&mdev->v4l2_dev);
+	return 0;
+}
+
+static struct most_component comp = {
+	.mod = THIS_MODULE,
+	.name = "video",
+	.probe_channel = comp_probe_channel,
+	.disconnect_channel = comp_disconnect_channel,
+	.rx_completion = comp_rx_data,
+};
+
+static int __init comp_init(void)
+{
+	int err;
+
+	err = most_register_component(&comp);
+	if (err)
+		return err;
+	err = most_register_configfs_subsys(&comp);
+	if (err) {
+		most_deregister_component(&comp);
+		return err;
+	}
+	return 0;
+}
+
+static void __exit comp_exit(void)
+{
+	struct most_video_dev *mdev, *tmp;
+
+	/*
+	 * As the mostcore currently doesn't call disconnect_channel()
+	 * for linked channels while we call most_deregister_component()
+	 * we simulate this call here.
+	 * This must be fixed in core.
+	 */
+	spin_lock_irq(&list_lock);
+	list_for_each_entry_safe(mdev, tmp, &video_devices, list) {
+		list_del(&mdev->list);
+		spin_unlock_irq(&list_lock);
+
+		comp_unregister_videodev(mdev);
+		v4l2_device_disconnect(&mdev->v4l2_dev);
+		v4l2_device_put(&mdev->v4l2_dev);
+		spin_lock_irq(&list_lock);
+	}
+	spin_unlock_irq(&list_lock);
+
+	most_deregister_configfs_subsys(&comp);
+	most_deregister_component(&comp);
+	BUG_ON(!list_empty(&video_devices));
+}
+
+module_init(comp_init);
+module_exit(comp_exit);
+
+MODULE_DESCRIPTION("V4L2 Component Module for Mostcore");
+MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/nvec/Kconfig b/drivers/staging/nvec/Kconfig
new file mode 100644
index 000000000..9fa98c16f
--- /dev/null
+++ b/drivers/staging/nvec/Kconfig
@@ -0,0 +1,54 @@
+# SPDX-License-Identifier: GPL-2.0
+config MFD_NVEC
+	tristate "NV Tegra Embedded Controller SMBus Interface"
+	depends on I2C && GPIOLIB && ARCH_TEGRA
+	select MFD_CORE
+	help
+	    Say Y here to enable support for a nVidia compliant embedded
+	    controller.
+
+	    To compile this driver as a module, say M here: the module will be
+	    called mfd-nvec
+
+config KEYBOARD_NVEC
+	tristate "Keyboard on nVidia compliant EC"
+	depends on MFD_NVEC && INPUT
+	help
+	  Say Y here to enable support for a keyboard connected to
+	  a nVidia compliant embedded controller.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called keyboard-nvec
+
+config SERIO_NVEC_PS2
+	tristate "PS2 on nVidia EC"
+	depends on MFD_NVEC && SERIO
+	help
+	  Say Y here to enable support for a Touchpad / Mouse connected
+	  to a nVidia compliant embedded controller.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called serio-nvec-ps2
+
+
+config NVEC_POWER
+	tristate "NVEC charger and battery"
+	depends on MFD_NVEC && POWER_SUPPLY
+	help
+	  Say Y to enable support for battery and charger interface for
+	  nVidia compliant embedded controllers.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called nvec-power
+
+
+config NVEC_PAZ00
+	tristate "Support for OEM specific functions on Compal PAZ00 based devices"
+	depends on MFD_NVEC && LEDS_CLASS
+	help
+	  Say Y to enable control of the yellow side leds on Compal PAZ00 based
+	  devices, e.g. Toshbia AC100 and Dynabooks AZ netbooks.
+
+	  To compile this driver as a module, say M here: the module will be
+	  called nvec-paz00
+
diff --git a/drivers/staging/nvec/Makefile b/drivers/staging/nvec/Makefile
new file mode 100644
index 000000000..f0cff8f9f
--- /dev/null
+++ b/drivers/staging/nvec/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_SERIO_NVEC_PS2)	+= nvec_ps2.o
+obj-$(CONFIG_MFD_NVEC)		+= nvec.o
+obj-$(CONFIG_NVEC_POWER) 	+= nvec_power.o
+obj-$(CONFIG_KEYBOARD_NVEC) 	+= nvec_kbd.o
+obj-$(CONFIG_NVEC_PAZ00) 	+= nvec_paz00.o
diff --git a/drivers/staging/nvec/README b/drivers/staging/nvec/README
new file mode 100644
index 000000000..510e6933f
--- /dev/null
+++ b/drivers/staging/nvec/README
@@ -0,0 +1,14 @@
+NVEC: An NVidia compliant Embedded Controller Protocol Implementation
+
+This is an implementation of the NVEC protocol used to communicate with an
+embedded controller (EC) via I2C bus. The EC is an I2C master while the host
+processor is the I2C slave. Requests from the host processor to the EC are
+started by triggering a gpio line.
+
+There is no written documentation of the protocol available to the public,
+but the source code[1] of the published nvec reference drivers can be a guide.
+This driver is currently only used by the AC100 project[2], but it is likely,
+that other Tegra boards (not yet mainlined, if ever) also use it.
+
+[1] e.g. https://nv-tegra.nvidia.com/gitweb/?p=linux-2.6.git;a=tree;f=arch/arm/mach-tegra/nvec;hb=android-tegra-2.6.32
+[2] http://gitorious.org/ac100, http://launchpad.net/ac100
diff --git a/drivers/staging/nvec/TODO b/drivers/staging/nvec/TODO
new file mode 100644
index 000000000..e4d85d9b4
--- /dev/null
+++ b/drivers/staging/nvec/TODO
@@ -0,0 +1,6 @@
+ToDo list (incomplete, unordered)
+	- add compile as module support
+	- move half of the nvec init stuff to i2c-tegra.c
+	- move event handling to nvec_events
+	- finish suspend/resume support
+	- add support for more device implementations
diff --git a/drivers/staging/nvec/nvec-keytable.h b/drivers/staging/nvec/nvec-keytable.h
new file mode 100644
index 000000000..ac58e87e6
--- /dev/null
+++ b/drivers/staging/nvec/nvec-keytable.h
@@ -0,0 +1,294 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * drivers/input/keyboard/tegra-nvec.c
+ *
+ * Keyboard class input driver for keyboards connected to an NvEc compliant
+ * embedded controller
+ *
+ * Copyright (c) 2009, NVIDIA Corporation.
+ */
+
+static unsigned short code_tab_102us[] = {
+	/* 0x00 */
+	KEY_GRAVE,
+	KEY_ESC,
+	KEY_1,
+	KEY_2,
+	KEY_3,
+	KEY_4,
+	KEY_5,
+	KEY_6,
+	KEY_7,
+	KEY_8,
+	KEY_9,
+	KEY_0,
+	KEY_MINUS,
+	KEY_EQUAL,
+	KEY_BACKSPACE,
+	KEY_TAB,
+	/* 0x10 */
+	KEY_Q,
+	KEY_W,
+	KEY_E,
+	KEY_R,
+	KEY_T,
+	KEY_Y,
+	KEY_U,
+	KEY_I,
+	KEY_O,
+	KEY_P,
+	KEY_LEFTBRACE,
+	KEY_RIGHTBRACE,
+	KEY_ENTER,
+	KEY_LEFTCTRL,
+	KEY_A,
+	KEY_S,
+	/* 0x20 */
+	KEY_D,
+	KEY_F,
+	KEY_G,
+	KEY_H,
+	KEY_J,
+	KEY_K,
+	KEY_L,
+	KEY_SEMICOLON,
+	KEY_APOSTROPHE,
+	KEY_GRAVE,
+	KEY_LEFTSHIFT,
+	KEY_BACKSLASH,
+	KEY_Z,
+	KEY_X,
+	KEY_C,
+	KEY_V,
+	/* 0x30 */
+	KEY_B,
+	KEY_N,
+	KEY_M,
+	KEY_COMMA,
+	KEY_DOT,
+	KEY_SLASH,
+	KEY_RIGHTSHIFT,
+	KEY_KPASTERISK,
+	KEY_LEFTALT,
+	KEY_SPACE,
+	KEY_CAPSLOCK,
+	KEY_F1,
+	KEY_F2,
+	KEY_F3,
+	KEY_F4,
+	KEY_F5,
+	/* 0x40 */
+	KEY_F6,
+	KEY_F7,
+	KEY_F8,
+	KEY_F9,
+	KEY_F10,
+	KEY_FN,
+	/* VK_SCROLL */
+	0,
+	KEY_KP7,
+	KEY_KP8,
+	KEY_KP9,
+	KEY_KPMINUS,
+	KEY_KP4,
+	KEY_KP5,
+	KEY_KP6,
+	KEY_KPPLUS,
+	KEY_KP1,
+	/* 0x50 */
+	KEY_KP2,
+	KEY_KP3,
+	KEY_KP0,
+	KEY_KPDOT,
+	/* VK_SNAPSHOT */
+	KEY_MENU,
+	KEY_POWER,
+	/* VK_OEM_102 */
+	KEY_102ND,
+	KEY_F11,
+	KEY_F12,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x60 */
+	0,
+	0,
+	0,
+	KEY_SEARCH,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x70 */
+	0,
+	0,
+	0,
+	KEY_KP5,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	KEY_KP9,
+};
+
+static unsigned short extcode_tab_us102[] = {
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x10 */
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* VK_MEDIA_NEXT_TRACK */
+	0,
+	0,
+	0,
+	/* VK_RETURN */
+	0,
+	KEY_RIGHTCTRL,
+	0,
+	0,
+	/* 0x20 */
+	KEY_MUTE,
+	/* VK_LAUNCH_APP1 */
+	0,
+	/* VK_MEDIA_PLAY_PAUSE */
+	0,
+	0,
+	/* VK_MEDIA_STOP */
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x30 */
+	KEY_VOLUMEUP,
+	0,
+	/* VK_BROWSER_HOME */
+	0,
+	0,
+	0,
+	/* VK_DIVIDE */
+	KEY_KPSLASH,
+	0,
+	/* VK_SNAPSHOT */
+	KEY_SYSRQ,
+	/* VK_RMENU */
+	KEY_RIGHTALT,
+	/* VK_OEM_NV_BACKLIGHT_UP */
+	0,
+	/* VK_OEM_NV_BACKLIGHT_DN */
+	0,
+	/* VK_OEM_NV_BACKLIGHT_AUTOTOGGLE */
+	0,
+	/* VK_OEM_NV_POWER_INFO */
+	0,
+	/* VK_OEM_NV_WIFI_TOGGLE */
+	0,
+	/* VK_OEM_NV_DISPLAY_SELECT */
+	0,
+	/* VK_OEM_NV_AIRPLANE_TOGGLE */
+	0,
+	/* 0x40 */
+	0,
+	KEY_LEFT,
+	0,
+	0,
+	0,
+	0,
+	KEY_CANCEL,
+	KEY_HOME,
+	KEY_UP,
+	KEY_PAGEUP,
+	0,
+	KEY_LEFT,
+	0,
+	KEY_RIGHT,
+	0,
+	KEY_END,
+	/* 0x50 */
+	KEY_DOWN,
+	KEY_PAGEDOWN,
+	KEY_INSERT,
+	KEY_DELETE,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	KEY_LEFTMETA,
+	0,
+	KEY_ESC,
+	KEY_KPMINUS,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* VK_BROWSER_SEARCH */
+	0,
+	/* VK_BROWSER_FAVORITES */
+	0,
+	/* VK_BROWSER_REFRESH */
+	0,
+	/* VK_BROWSER_STOP */
+	0,
+	/* VK_BROWSER_FORWARD */
+	0,
+	/* VK_BROWSER_BACK */
+	0,
+	/* VK_LAUNCH_APP2 */
+	0,
+	/* VK_LAUNCH_MAIL */
+	0,
+	/* VK_LAUNCH_MEDIA_SELECT */
+	0,
+};
+
+static unsigned short *code_tabs[] = { code_tab_102us, extcode_tab_us102 };
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
new file mode 100644
index 000000000..b3f114cb0
--- /dev/null
+++ b/drivers/staging/nvec/nvec.c
@@ -0,0 +1,958 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * NVEC: NVIDIA compliant embedded controller interface
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.lauchpad.net>
+ *
+ * Authors:  Pierre-Hugues Husson <phhusson@free.fr>
+ *           Ilya Petrov <ilya.muromec@gmail.com>
+ *           Marc Dietrich <marvin24@gmx.de>
+ *           Julian Andres Klode <jak@jak-linux.org>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/list.h>
+#include <linux/mfd/core.h>
+#include <linux/mutex.h>
+#include <linux/notifier.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+
+#include "nvec.h"
+
+#define I2C_CNFG			0x00
+#define I2C_CNFG_PACKET_MODE_EN		BIT(10)
+#define I2C_CNFG_NEW_MASTER_SFM		BIT(11)
+#define I2C_CNFG_DEBOUNCE_CNT_SHIFT	12
+
+#define I2C_SL_CNFG		0x20
+#define I2C_SL_NEWSL		BIT(2)
+#define I2C_SL_NACK		BIT(1)
+#define I2C_SL_RESP		BIT(0)
+#define I2C_SL_IRQ		BIT(3)
+#define END_TRANS		BIT(4)
+#define RCVD			BIT(2)
+#define RNW			BIT(1)
+
+#define I2C_SL_RCVD		0x24
+#define I2C_SL_STATUS		0x28
+#define I2C_SL_ADDR1		0x2c
+#define I2C_SL_ADDR2		0x30
+#define I2C_SL_DELAY_COUNT	0x3c
+
+/**
+ * enum nvec_msg_category - Message categories for nvec_msg_alloc()
+ * @NVEC_MSG_RX: The message is an incoming message (from EC)
+ * @NVEC_MSG_TX: The message is an outgoing message (to EC)
+ */
+enum nvec_msg_category  {
+	NVEC_MSG_RX,
+	NVEC_MSG_TX,
+};
+
+enum nvec_sleep_subcmds {
+	GLOBAL_EVENTS,
+	AP_PWR_DOWN,
+	AP_SUSPEND,
+};
+
+#define CNF_EVENT_REPORTING 0x01
+#define GET_FIRMWARE_VERSION 0x15
+#define LID_SWITCH BIT(1)
+#define PWR_BUTTON BIT(15)
+
+static struct nvec_chip *nvec_power_handle;
+
+static const struct mfd_cell nvec_devices[] = {
+	{
+		.name = "nvec-kbd",
+	},
+	{
+		.name = "nvec-mouse",
+	},
+	{
+		.name = "nvec-power",
+		.id = 0,
+	},
+	{
+		.name = "nvec-power",
+		.id = 1,
+	},
+	{
+		.name = "nvec-paz00",
+	},
+};
+
+/**
+ * nvec_register_notifier - Register a notifier with nvec
+ * @nvec: A &struct nvec_chip
+ * @nb: The notifier block to register
+ * @events: Unused
+ *
+ * Registers a notifier with @nvec. The notifier will be added to an atomic
+ * notifier chain that is called for all received messages except those that
+ * correspond to a request initiated by nvec_write_sync().
+ */
+int nvec_register_notifier(struct nvec_chip *nvec, struct notifier_block *nb,
+			   unsigned int events)
+{
+	return atomic_notifier_chain_register(&nvec->notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(nvec_register_notifier);
+
+/**
+ * nvec_unregister_notifier - Unregister a notifier with nvec
+ * @nvec: A &struct nvec_chip
+ * @nb: The notifier block to unregister
+ *
+ * Unregisters a notifier with @nvec. The notifier will be removed from the
+ * atomic notifier chain.
+ */
+int nvec_unregister_notifier(struct nvec_chip *nvec, struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&nvec->notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(nvec_unregister_notifier);
+
+/*
+ * nvec_status_notifier - The final notifier
+ *
+ * Prints a message about control events not handled in the notifier
+ * chain.
+ */
+static int nvec_status_notifier(struct notifier_block *nb,
+				unsigned long event_type, void *data)
+{
+	struct nvec_chip *nvec = container_of(nb, struct nvec_chip,
+						nvec_status_notifier);
+	unsigned char *msg = data;
+
+	if (event_type != NVEC_CNTL)
+		return NOTIFY_DONE;
+
+	dev_warn(nvec->dev, "unhandled msg type %ld\n", event_type);
+	print_hex_dump(KERN_WARNING, "payload: ", DUMP_PREFIX_NONE, 16, 1,
+		       msg, msg[1] + 2, true);
+
+	return NOTIFY_OK;
+}
+
+/**
+ * nvec_msg_alloc:
+ * @nvec: A &struct nvec_chip
+ * @category: Pool category, see &enum nvec_msg_category
+ *
+ * Allocate a single &struct nvec_msg object from the message pool of
+ * @nvec. The result shall be passed to nvec_msg_free() if no longer
+ * used.
+ *
+ * Outgoing messages are placed in the upper 75% of the pool, keeping the
+ * lower 25% available for RX buffers only. The reason is to prevent a
+ * situation where all buffers are full and a message is thus endlessly
+ * retried because the response could never be processed.
+ */
+static struct nvec_msg *nvec_msg_alloc(struct nvec_chip *nvec,
+				       enum nvec_msg_category category)
+{
+	int i = (category == NVEC_MSG_TX) ? (NVEC_POOL_SIZE / 4) : 0;
+
+	for (; i < NVEC_POOL_SIZE; i++) {
+		if (atomic_xchg(&nvec->msg_pool[i].used, 1) == 0) {
+			dev_vdbg(nvec->dev, "INFO: Allocate %i\n", i);
+			return &nvec->msg_pool[i];
+		}
+	}
+
+	dev_err(nvec->dev, "could not allocate %s buffer\n",
+		(category == NVEC_MSG_TX) ? "TX" : "RX");
+
+	return NULL;
+}
+
+/**
+ * nvec_msg_free:
+ * @nvec: A &struct nvec_chip
+ * @msg:  A message (must be allocated by nvec_msg_alloc() and belong to @nvec)
+ *
+ * Free the given message
+ */
+void nvec_msg_free(struct nvec_chip *nvec, struct nvec_msg *msg)
+{
+	if (msg != &nvec->tx_scratch)
+		dev_vdbg(nvec->dev, "INFO: Free %ti\n", msg - nvec->msg_pool);
+	atomic_set(&msg->used, 0);
+}
+EXPORT_SYMBOL_GPL(nvec_msg_free);
+
+/**
+ * nvec_msg_is_event - Return %true if @msg is an event
+ * @msg: A message
+ */
+static bool nvec_msg_is_event(struct nvec_msg *msg)
+{
+	return msg->data[0] >> 7;
+}
+
+/**
+ * nvec_msg_size - Get the size of a message
+ * @msg: The message to get the size for
+ *
+ * This only works for received messages, not for outgoing messages.
+ */
+static size_t nvec_msg_size(struct nvec_msg *msg)
+{
+	bool is_event = nvec_msg_is_event(msg);
+	int event_length = (msg->data[0] & 0x60) >> 5;
+
+	/* for variable size, payload size in byte 1 + count (1) + cmd (1) */
+	if (!is_event || event_length == NVEC_VAR_SIZE)
+		return (msg->pos || msg->size) ? (msg->data[1] + 2) : 0;
+	else if (event_length == NVEC_2BYTES)
+		return 2;
+	else if (event_length == NVEC_3BYTES)
+		return 3;
+	return 0;
+}
+
+/**
+ * nvec_gpio_set_value - Set the GPIO value
+ * @nvec: A &struct nvec_chip
+ * @value: The value to write (0 or 1)
+ *
+ * Like gpio_set_value(), but generating debugging information
+ */
+static void nvec_gpio_set_value(struct nvec_chip *nvec, int value)
+{
+	dev_dbg(nvec->dev, "GPIO changed from %u to %u\n",
+		gpiod_get_value(nvec->gpiod), value);
+	gpiod_set_value(nvec->gpiod, value);
+}
+
+/**
+ * nvec_write_async - Asynchronously write a message to NVEC
+ * @nvec: An nvec_chip instance
+ * @data: The message data, starting with the request type
+ * @size: The size of @data
+ *
+ * Queue a single message to be transferred to the embedded controller
+ * and return immediately.
+ *
+ * Returns: 0 on success, a negative error code on failure. If a failure
+ * occurred, the nvec driver may print an error.
+ */
+int nvec_write_async(struct nvec_chip *nvec, const unsigned char *data,
+		     short size)
+{
+	struct nvec_msg *msg;
+	unsigned long flags;
+
+	msg = nvec_msg_alloc(nvec, NVEC_MSG_TX);
+
+	if (!msg)
+		return -ENOMEM;
+
+	msg->data[0] = size;
+	memcpy(msg->data + 1, data, size);
+	msg->size = size + 1;
+
+	spin_lock_irqsave(&nvec->tx_lock, flags);
+	list_add_tail(&msg->node, &nvec->tx_data);
+	spin_unlock_irqrestore(&nvec->tx_lock, flags);
+
+	schedule_work(&nvec->tx_work);
+
+	return 0;
+}
+EXPORT_SYMBOL(nvec_write_async);
+
+/**
+ * nvec_write_sync - Write a message to nvec and read the response
+ * @nvec: An &struct nvec_chip
+ * @data: The data to write
+ * @size: The size of @data
+ * @msg:  The response message received
+ *
+ * This is similar to nvec_write_async(), but waits for the
+ * request to be answered before returning. This function
+ * uses a mutex and can thus not be called from e.g.
+ * interrupt handlers.
+ *
+ * Returns: 0 on success, a negative error code on failure.
+ * The response message is returned in @msg. Shall be freed
+ * with nvec_msg_free() once no longer used.
+ *
+ */
+int nvec_write_sync(struct nvec_chip *nvec,
+		    const unsigned char *data, short size,
+		    struct nvec_msg **msg)
+{
+	mutex_lock(&nvec->sync_write_mutex);
+
+	*msg = NULL;
+	nvec->sync_write_pending = (data[1] << 8) + data[0];
+
+	if (nvec_write_async(nvec, data, size) < 0) {
+		mutex_unlock(&nvec->sync_write_mutex);
+		return -ENOMEM;
+	}
+
+	dev_dbg(nvec->dev, "nvec_sync_write: 0x%04x\n",
+		nvec->sync_write_pending);
+	if (!(wait_for_completion_timeout(&nvec->sync_write,
+					  msecs_to_jiffies(2000)))) {
+		dev_warn(nvec->dev,
+			 "timeout waiting for sync write to complete\n");
+		mutex_unlock(&nvec->sync_write_mutex);
+		return -ETIMEDOUT;
+	}
+
+	dev_dbg(nvec->dev, "nvec_sync_write: pong!\n");
+
+	*msg = nvec->last_sync_msg;
+
+	mutex_unlock(&nvec->sync_write_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(nvec_write_sync);
+
+/**
+ * nvec_toggle_global_events - enables or disables global event reporting
+ * @nvec: nvec handle
+ * @state: true for enable, false for disable
+ *
+ * This switches on/off global event reports by the embedded controller.
+ */
+static void nvec_toggle_global_events(struct nvec_chip *nvec, bool state)
+{
+	unsigned char global_events[] = { NVEC_SLEEP, GLOBAL_EVENTS, state };
+
+	nvec_write_async(nvec, global_events, 3);
+}
+
+/**
+ * nvec_event_mask - fill the command string with event bitfield
+ * @ev: points to event command string
+ * @mask: bit to insert into the event mask
+ *
+ * Configure event command expects a 32 bit bitfield which describes
+ * which events to enable. The bitfield has the following structure
+ * (from highest byte to lowest):
+ *	system state bits 7-0
+ *	system state bits 15-8
+ *	oem system state bits 7-0
+ *	oem system state bits 15-8
+ */
+static void nvec_event_mask(char *ev, u32 mask)
+{
+	ev[3] = mask >> 16 & 0xff;
+	ev[4] = mask >> 24 & 0xff;
+	ev[5] = mask >> 0  & 0xff;
+	ev[6] = mask >> 8  & 0xff;
+}
+
+/**
+ * nvec_request_master - Process outgoing messages
+ * @work: A &struct work_struct (the tx_worker member of &struct nvec_chip)
+ *
+ * Processes all outgoing requests by sending the request and awaiting the
+ * response, then continuing with the next request. Once a request has a
+ * matching response, it will be freed and removed from the list.
+ */
+static void nvec_request_master(struct work_struct *work)
+{
+	struct nvec_chip *nvec = container_of(work, struct nvec_chip, tx_work);
+	unsigned long flags;
+	long err;
+	struct nvec_msg *msg;
+
+	spin_lock_irqsave(&nvec->tx_lock, flags);
+	while (!list_empty(&nvec->tx_data)) {
+		msg = list_first_entry(&nvec->tx_data, struct nvec_msg, node);
+		spin_unlock_irqrestore(&nvec->tx_lock, flags);
+		nvec_gpio_set_value(nvec, 0);
+		err = wait_for_completion_interruptible_timeout(&nvec->ec_transfer,
+								msecs_to_jiffies(5000));
+
+		if (err == 0) {
+			dev_warn(nvec->dev, "timeout waiting for ec transfer\n");
+			nvec_gpio_set_value(nvec, 1);
+			msg->pos = 0;
+		}
+
+		spin_lock_irqsave(&nvec->tx_lock, flags);
+
+		if (err > 0) {
+			list_del_init(&msg->node);
+			nvec_msg_free(nvec, msg);
+		}
+	}
+	spin_unlock_irqrestore(&nvec->tx_lock, flags);
+}
+
+/**
+ * parse_msg - Print some information and call the notifiers on an RX message
+ * @nvec: A &struct nvec_chip
+ * @msg: A message received by @nvec
+ *
+ * Paarse some pieces of the message and then call the chain of notifiers
+ * registered via nvec_register_notifier.
+ */
+static int parse_msg(struct nvec_chip *nvec, struct nvec_msg *msg)
+{
+	if ((msg->data[0] & 1 << 7) == 0 && msg->data[3]) {
+		dev_err(nvec->dev, "ec responded %*ph\n", 4, msg->data);
+		return -EINVAL;
+	}
+
+	if ((msg->data[0] >> 7) == 1 && (msg->data[0] & 0x0f) == 5)
+		print_hex_dump(KERN_WARNING, "ec system event ",
+			       DUMP_PREFIX_NONE, 16, 1, msg->data,
+			       msg->data[1] + 2, true);
+
+	atomic_notifier_call_chain(&nvec->notifier_list, msg->data[0] & 0x8f,
+				   msg->data);
+
+	return 0;
+}
+
+/**
+ * nvec_dispatch - Process messages received from the EC
+ * @work: A &struct work_struct (the tx_worker member of &struct nvec_chip)
+ *
+ * Process messages previously received from the EC and put into the RX
+ * queue of the &struct nvec_chip instance associated with @work.
+ */
+static void nvec_dispatch(struct work_struct *work)
+{
+	struct nvec_chip *nvec = container_of(work, struct nvec_chip, rx_work);
+	unsigned long flags;
+	struct nvec_msg *msg;
+
+	spin_lock_irqsave(&nvec->rx_lock, flags);
+	while (!list_empty(&nvec->rx_data)) {
+		msg = list_first_entry(&nvec->rx_data, struct nvec_msg, node);
+		list_del_init(&msg->node);
+		spin_unlock_irqrestore(&nvec->rx_lock, flags);
+
+		if (nvec->sync_write_pending ==
+		      (msg->data[2] << 8) + msg->data[0]) {
+			dev_dbg(nvec->dev, "sync write completed!\n");
+			nvec->sync_write_pending = 0;
+			nvec->last_sync_msg = msg;
+			complete(&nvec->sync_write);
+		} else {
+			parse_msg(nvec, msg);
+			nvec_msg_free(nvec, msg);
+		}
+		spin_lock_irqsave(&nvec->rx_lock, flags);
+	}
+	spin_unlock_irqrestore(&nvec->rx_lock, flags);
+}
+
+/**
+ * nvec_tx_completed - Complete the current transfer
+ * @nvec: A &struct nvec_chip
+ *
+ * This is called when we have received an END_TRANS on a TX transfer.
+ */
+static void nvec_tx_completed(struct nvec_chip *nvec)
+{
+	/* We got an END_TRANS, let's skip this, maybe there's an event */
+	if (nvec->tx->pos != nvec->tx->size) {
+		dev_err(nvec->dev, "premature END_TRANS, resending\n");
+		nvec->tx->pos = 0;
+		nvec_gpio_set_value(nvec, 0);
+	} else {
+		nvec->state = 0;
+	}
+}
+
+/**
+ * nvec_rx_completed - Complete the current transfer
+ * @nvec: A &struct nvec_chip
+ *
+ * This is called when we have received an END_TRANS on a RX transfer.
+ */
+static void nvec_rx_completed(struct nvec_chip *nvec)
+{
+	if (nvec->rx->pos != nvec_msg_size(nvec->rx)) {
+		dev_err(nvec->dev, "RX incomplete: Expected %u bytes, got %u\n",
+			(uint)nvec_msg_size(nvec->rx),
+			(uint)nvec->rx->pos);
+
+		nvec_msg_free(nvec, nvec->rx);
+		nvec->state = 0;
+
+		/* Battery quirk - Often incomplete, and likes to crash */
+		if (nvec->rx->data[0] == NVEC_BAT)
+			complete(&nvec->ec_transfer);
+
+		return;
+	}
+
+	spin_lock(&nvec->rx_lock);
+
+	/*
+	 * Add the received data to the work list and move the ring buffer
+	 * pointer to the next entry.
+	 */
+	list_add_tail(&nvec->rx->node, &nvec->rx_data);
+
+	spin_unlock(&nvec->rx_lock);
+
+	nvec->state = 0;
+
+	if (!nvec_msg_is_event(nvec->rx))
+		complete(&nvec->ec_transfer);
+
+	schedule_work(&nvec->rx_work);
+}
+
+/**
+ * nvec_invalid_flags - Send an error message about invalid flags and jump
+ * @nvec: The nvec device
+ * @status: The status flags
+ * @reset: Whether we shall jump to state 0.
+ */
+static void nvec_invalid_flags(struct nvec_chip *nvec, unsigned int status,
+			       bool reset)
+{
+	dev_err(nvec->dev, "unexpected status flags 0x%02x during state %i\n",
+		status, nvec->state);
+	if (reset)
+		nvec->state = 0;
+}
+
+/**
+ * nvec_tx_set - Set the message to transfer (nvec->tx)
+ * @nvec: A &struct nvec_chip
+ *
+ * Gets the first entry from the tx_data list of @nvec and sets the
+ * tx member to it. If the tx_data list is empty, this uses the
+ * tx_scratch message to send a no operation message.
+ */
+static void nvec_tx_set(struct nvec_chip *nvec)
+{
+	spin_lock(&nvec->tx_lock);
+	if (list_empty(&nvec->tx_data)) {
+		dev_err(nvec->dev, "empty tx - sending no-op\n");
+		memcpy(nvec->tx_scratch.data, "\x02\x07\x02", 3);
+		nvec->tx_scratch.size = 3;
+		nvec->tx_scratch.pos = 0;
+		nvec->tx = &nvec->tx_scratch;
+		list_add_tail(&nvec->tx->node, &nvec->tx_data);
+	} else {
+		nvec->tx = list_first_entry(&nvec->tx_data, struct nvec_msg,
+					    node);
+		nvec->tx->pos = 0;
+	}
+	spin_unlock(&nvec->tx_lock);
+
+	dev_dbg(nvec->dev, "Sending message of length %u, command 0x%x\n",
+		(uint)nvec->tx->size, nvec->tx->data[1]);
+}
+
+/**
+ * nvec_interrupt - Interrupt handler
+ * @irq: The IRQ
+ * @dev: The nvec device
+ *
+ * Interrupt handler that fills our RX buffers and empties our TX
+ * buffers. This uses a finite state machine with ridiculous amounts
+ * of error checking, in order to be fairly reliable.
+ */
+static irqreturn_t nvec_interrupt(int irq, void *dev)
+{
+	unsigned long status;
+	unsigned int received = 0;
+	unsigned char to_send = 0xff;
+	const unsigned long irq_mask = I2C_SL_IRQ | END_TRANS | RCVD | RNW;
+	struct nvec_chip *nvec = dev;
+	unsigned int state = nvec->state;
+
+	status = readl(nvec->base + I2C_SL_STATUS);
+
+	/* Filter out some errors */
+	if ((status & irq_mask) == 0 && (status & ~irq_mask) != 0) {
+		dev_err(nvec->dev, "unexpected irq mask %lx\n", status);
+		return IRQ_HANDLED;
+	}
+	if ((status & I2C_SL_IRQ) == 0) {
+		dev_err(nvec->dev, "Spurious IRQ\n");
+		return IRQ_HANDLED;
+	}
+
+	/* The EC did not request a read, so it send us something, read it */
+	if ((status & RNW) == 0) {
+		received = readl(nvec->base + I2C_SL_RCVD);
+		if (status & RCVD)
+			writel(0, nvec->base + I2C_SL_RCVD);
+	}
+
+	if (status == (I2C_SL_IRQ | RCVD))
+		nvec->state = 0;
+
+	switch (nvec->state) {
+	case 0:		/* Verify that its a transfer start, the rest later */
+		if (status != (I2C_SL_IRQ | RCVD))
+			nvec_invalid_flags(nvec, status, false);
+		break;
+	case 1:		/* command byte */
+		if (status != I2C_SL_IRQ) {
+			nvec_invalid_flags(nvec, status, true);
+		} else {
+			nvec->rx = nvec_msg_alloc(nvec, NVEC_MSG_RX);
+			/* Should not happen in a normal world */
+			if (unlikely(!nvec->rx)) {
+				nvec->state = 0;
+				break;
+			}
+			nvec->rx->data[0] = received;
+			nvec->rx->pos = 1;
+			nvec->state = 2;
+		}
+		break;
+	case 2:		/* first byte after command */
+		if (status == (I2C_SL_IRQ | RNW | RCVD)) {
+			udelay(33);
+			if (nvec->rx->data[0] != 0x01) {
+				dev_err(nvec->dev,
+					"Read without prior read command\n");
+				nvec->state = 0;
+				break;
+			}
+			nvec_msg_free(nvec, nvec->rx);
+			nvec->state = 3;
+			nvec_tx_set(nvec);
+			to_send = nvec->tx->data[0];
+			nvec->tx->pos = 1;
+		} else if (status == (I2C_SL_IRQ)) {
+			nvec->rx->data[1] = received;
+			nvec->rx->pos = 2;
+			nvec->state = 4;
+		} else {
+			nvec_invalid_flags(nvec, status, true);
+		}
+		break;
+	case 3:		/* EC does a block read, we transmit data */
+		if (status & END_TRANS) {
+			nvec_tx_completed(nvec);
+		} else if ((status & RNW) == 0 || (status & RCVD)) {
+			nvec_invalid_flags(nvec, status, true);
+		} else if (nvec->tx && nvec->tx->pos < nvec->tx->size) {
+			to_send = nvec->tx->data[nvec->tx->pos++];
+		} else {
+			dev_err(nvec->dev,
+				"tx buffer underflow on %p (%u > %u)\n",
+				nvec->tx,
+				(uint)(nvec->tx ? nvec->tx->pos : 0),
+				(uint)(nvec->tx ? nvec->tx->size : 0));
+			nvec->state = 0;
+		}
+		break;
+	case 4:		/* EC does some write, we read the data */
+		if ((status & (END_TRANS | RNW)) == END_TRANS)
+			nvec_rx_completed(nvec);
+		else if (status & (RNW | RCVD))
+			nvec_invalid_flags(nvec, status, true);
+		else if (nvec->rx && nvec->rx->pos < NVEC_MSG_SIZE)
+			nvec->rx->data[nvec->rx->pos++] = received;
+		else
+			dev_err(nvec->dev,
+				"RX buffer overflow on %p: Trying to write byte %u of %u\n",
+				nvec->rx, nvec->rx ? nvec->rx->pos : 0,
+				NVEC_MSG_SIZE);
+		break;
+	default:
+		nvec->state = 0;
+	}
+
+	/* If we are told that a new transfer starts, verify it */
+	if ((status & (RCVD | RNW)) == RCVD) {
+		if (received != nvec->i2c_addr)
+			dev_err(nvec->dev,
+				"received address 0x%02x, expected 0x%02x\n",
+				received, nvec->i2c_addr);
+		nvec->state = 1;
+	}
+
+	/* Send data if requested, but not on end of transmission */
+	if ((status & (RNW | END_TRANS)) == RNW)
+		writel(to_send, nvec->base + I2C_SL_RCVD);
+
+	/* If we have send the first byte */
+	if (status == (I2C_SL_IRQ | RNW | RCVD))
+		nvec_gpio_set_value(nvec, 1);
+
+	dev_dbg(nvec->dev,
+		"Handled: %s 0x%02x, %s 0x%02x in state %u [%s%s%s]\n",
+		(status & RNW) == 0 ? "received" : "R=",
+		received,
+		(status & (RNW | END_TRANS)) ? "sent" : "S=",
+		to_send,
+		state,
+		status & END_TRANS ? " END_TRANS" : "",
+		status & RCVD ? " RCVD" : "",
+		status & RNW ? " RNW" : "");
+
+	/*
+	 * TODO: A correct fix needs to be found for this.
+	 *
+	 * We experience less incomplete messages with this delay than without
+	 * it, but we don't know why. Help is appreciated.
+	 */
+	udelay(100);
+
+	return IRQ_HANDLED;
+}
+
+static void tegra_init_i2c_slave(struct nvec_chip *nvec)
+{
+	u32 val;
+
+	clk_prepare_enable(nvec->i2c_clk);
+
+	reset_control_assert(nvec->rst);
+	udelay(2);
+	reset_control_deassert(nvec->rst);
+
+	val = I2C_CNFG_NEW_MASTER_SFM | I2C_CNFG_PACKET_MODE_EN |
+	    (0x2 << I2C_CNFG_DEBOUNCE_CNT_SHIFT);
+	writel(val, nvec->base + I2C_CNFG);
+
+	clk_set_rate(nvec->i2c_clk, 8 * 80000);
+
+	writel(I2C_SL_NEWSL, nvec->base + I2C_SL_CNFG);
+	writel(0x1E, nvec->base + I2C_SL_DELAY_COUNT);
+
+	writel(nvec->i2c_addr >> 1, nvec->base + I2C_SL_ADDR1);
+	writel(0, nvec->base + I2C_SL_ADDR2);
+
+	enable_irq(nvec->irq);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static void nvec_disable_i2c_slave(struct nvec_chip *nvec)
+{
+	disable_irq(nvec->irq);
+	writel(I2C_SL_NEWSL | I2C_SL_NACK, nvec->base + I2C_SL_CNFG);
+	clk_disable_unprepare(nvec->i2c_clk);
+}
+#endif
+
+static void nvec_power_off(void)
+{
+	char ap_pwr_down[] = { NVEC_SLEEP, AP_PWR_DOWN };
+
+	nvec_toggle_global_events(nvec_power_handle, false);
+	nvec_write_async(nvec_power_handle, ap_pwr_down, 2);
+}
+
+static int tegra_nvec_probe(struct platform_device *pdev)
+{
+	int err, ret;
+	struct clk *i2c_clk;
+	struct device *dev = &pdev->dev;
+	struct nvec_chip *nvec;
+	struct nvec_msg *msg;
+	void __iomem *base;
+	char	get_firmware_version[] = { NVEC_CNTL, GET_FIRMWARE_VERSION },
+		unmute_speakers[] = { NVEC_OEM0, 0x10, 0x59, 0x95 },
+		enable_event[7] = { NVEC_SYS, CNF_EVENT_REPORTING, true };
+
+	if (!dev->of_node) {
+		dev_err(dev, "must be instantiated using device tree\n");
+		return -ENODEV;
+	}
+
+	nvec = devm_kzalloc(dev, sizeof(struct nvec_chip), GFP_KERNEL);
+	if (!nvec)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, nvec);
+	nvec->dev = dev;
+
+	if (of_property_read_u32(dev->of_node, "slave-addr", &nvec->i2c_addr)) {
+		dev_err(dev, "no i2c address specified");
+		return -ENODEV;
+	}
+
+	base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	nvec->irq = platform_get_irq(pdev, 0);
+	if (nvec->irq < 0)
+		return -ENODEV;
+
+	i2c_clk = devm_clk_get(dev, "div-clk");
+	if (IS_ERR(i2c_clk)) {
+		dev_err(dev, "failed to get controller clock\n");
+		return -ENODEV;
+	}
+
+	nvec->rst = devm_reset_control_get_exclusive(dev, "i2c");
+	if (IS_ERR(nvec->rst)) {
+		dev_err(dev, "failed to get controller reset\n");
+		return PTR_ERR(nvec->rst);
+	}
+
+	nvec->base = base;
+	nvec->i2c_clk = i2c_clk;
+	nvec->rx = &nvec->msg_pool[0];
+
+	ATOMIC_INIT_NOTIFIER_HEAD(&nvec->notifier_list);
+
+	init_completion(&nvec->sync_write);
+	init_completion(&nvec->ec_transfer);
+	mutex_init(&nvec->sync_write_mutex);
+	spin_lock_init(&nvec->tx_lock);
+	spin_lock_init(&nvec->rx_lock);
+	INIT_LIST_HEAD(&nvec->rx_data);
+	INIT_LIST_HEAD(&nvec->tx_data);
+	INIT_WORK(&nvec->rx_work, nvec_dispatch);
+	INIT_WORK(&nvec->tx_work, nvec_request_master);
+
+	nvec->gpiod = devm_gpiod_get(dev, "request", GPIOD_OUT_HIGH);
+	if (IS_ERR(nvec->gpiod)) {
+		dev_err(dev, "couldn't request gpio\n");
+		return PTR_ERR(nvec->gpiod);
+	}
+
+	err = devm_request_irq(dev, nvec->irq, nvec_interrupt, 0,
+			       "nvec", nvec);
+	if (err) {
+		dev_err(dev, "couldn't request irq\n");
+		return -ENODEV;
+	}
+	disable_irq(nvec->irq);
+
+	tegra_init_i2c_slave(nvec);
+
+	/* enable event reporting */
+	nvec_toggle_global_events(nvec, true);
+
+	nvec->nvec_status_notifier.notifier_call = nvec_status_notifier;
+	nvec_register_notifier(nvec, &nvec->nvec_status_notifier, 0);
+
+	nvec_power_handle = nvec;
+	pm_power_off = nvec_power_off;
+
+	/* Get Firmware Version */
+	err = nvec_write_sync(nvec, get_firmware_version, 2, &msg);
+
+	if (!err) {
+		dev_warn(dev,
+			 "ec firmware version %02x.%02x.%02x / %02x\n",
+			 msg->data[4], msg->data[5],
+			 msg->data[6], msg->data[7]);
+
+		nvec_msg_free(nvec, msg);
+	}
+
+	ret = mfd_add_devices(dev, 0, nvec_devices,
+			      ARRAY_SIZE(nvec_devices), NULL, 0, NULL);
+	if (ret)
+		dev_err(dev, "error adding subdevices\n");
+
+	/* unmute speakers? */
+	nvec_write_async(nvec, unmute_speakers, 4);
+
+	/* enable lid switch event */
+	nvec_event_mask(enable_event, LID_SWITCH);
+	nvec_write_async(nvec, enable_event, 7);
+
+	/* enable power button event */
+	nvec_event_mask(enable_event, PWR_BUTTON);
+	nvec_write_async(nvec, enable_event, 7);
+
+	return 0;
+}
+
+static int tegra_nvec_remove(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = platform_get_drvdata(pdev);
+
+	nvec_toggle_global_events(nvec, false);
+	mfd_remove_devices(nvec->dev);
+	nvec_unregister_notifier(nvec, &nvec->nvec_status_notifier);
+	cancel_work_sync(&nvec->rx_work);
+	cancel_work_sync(&nvec->tx_work);
+	/* FIXME: needs check whether nvec is responsible for power off */
+	pm_power_off = NULL;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int nvec_suspend(struct device *dev)
+{
+	int err;
+	struct nvec_chip *nvec = dev_get_drvdata(dev);
+	struct nvec_msg *msg;
+	char ap_suspend[] = { NVEC_SLEEP, AP_SUSPEND };
+
+	dev_dbg(nvec->dev, "suspending\n");
+
+	/* keep these sync or you'll break suspend */
+	nvec_toggle_global_events(nvec, false);
+
+	err = nvec_write_sync(nvec, ap_suspend, sizeof(ap_suspend), &msg);
+	if (!err)
+		nvec_msg_free(nvec, msg);
+
+	nvec_disable_i2c_slave(nvec);
+
+	return 0;
+}
+
+static int nvec_resume(struct device *dev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(dev);
+
+	dev_dbg(nvec->dev, "resuming\n");
+	tegra_init_i2c_slave(nvec);
+	nvec_toggle_global_events(nvec, true);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(nvec_pm_ops, nvec_suspend, nvec_resume);
+
+/* Match table for of_platform binding */
+static const struct of_device_id nvidia_nvec_of_match[] = {
+	{ .compatible = "nvidia,nvec", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, nvidia_nvec_of_match);
+
+static struct platform_driver nvec_device_driver = {
+	.probe   = tegra_nvec_probe,
+	.remove  = tegra_nvec_remove,
+	.driver  = {
+		.name = "nvec",
+		.pm = &nvec_pm_ops,
+		.of_match_table = nvidia_nvec_of_match,
+	}
+};
+
+module_platform_driver(nvec_device_driver);
+
+MODULE_ALIAS("platform:nvec");
+MODULE_DESCRIPTION("NVIDIA compliant embedded controller interface");
+MODULE_AUTHOR("Marc Dietrich <marvin24@gmx.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/nvec/nvec.h b/drivers/staging/nvec/nvec.h
new file mode 100644
index 000000000..80c0353f1
--- /dev/null
+++ b/drivers/staging/nvec/nvec.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * NVEC: NVIDIA compliant embedded controller interface
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
+ *
+ * Authors:  Pierre-Hugues Husson <phhusson@free.fr>
+ *           Ilya Petrov <ilya.muromec@gmail.com>
+ *           Marc Dietrich <marvin24@gmx.de>
+ *           Julian Andres Klode <jak@jak-linux.org>
+ */
+
+#ifndef __LINUX_MFD_NVEC
+#define __LINUX_MFD_NVEC
+
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/notifier.h>
+#include <linux/reset.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+
+/* NVEC_POOL_SIZE - Size of the pool in &struct nvec_msg */
+#define NVEC_POOL_SIZE	64
+
+/*
+ * NVEC_MSG_SIZE - Maximum size of the data field of &struct nvec_msg.
+ *
+ * A message must store up to a SMBus block operation which consists of
+ * one command byte, one count byte, and up to 32 payload bytes = 34
+ * byte.
+ */
+#define NVEC_MSG_SIZE	34
+
+/**
+ * enum nvec_event_size - The size of an event message
+ * @NVEC_2BYTES: The message has one command byte and one data byte
+ * @NVEC_3BYTES: The message has one command byte and two data bytes
+ * @NVEC_VAR_SIZE: The message has one command byte, one count byte, and has
+ *                 up to as many bytes as the number in the count byte. The
+ *                 maximum is 32
+ *
+ * Events can be fixed or variable sized. This is useless on other message
+ * types, which are always variable sized.
+ */
+enum nvec_event_size {
+	NVEC_2BYTES,
+	NVEC_3BYTES,
+	NVEC_VAR_SIZE,
+};
+
+/**
+ * enum nvec_msg_type - The type of a message
+ * @NVEC_SYS: A system request/response
+ * @NVEC_BAT: A battery request/response
+ * @NVEC_KBD: A keyboard request/response
+ * @NVEC_PS2: A mouse request/response
+ * @NVEC_CNTL: A EC control request/response
+ * @NVEC_KB_EVT: An event from the keyboard
+ * @NVEC_PS2_EVT: An event from the mouse
+ *
+ * Events can be fixed or variable sized. This is useless on other message
+ * types, which are always variable sized.
+ */
+enum nvec_msg_type {
+	NVEC_SYS = 1,
+	NVEC_BAT,
+	NVEC_GPIO,
+	NVEC_SLEEP,
+	NVEC_KBD,
+	NVEC_PS2,
+	NVEC_CNTL,
+	NVEC_OEM0 = 0x0d,
+	NVEC_KB_EVT = 0x80,
+	NVEC_PS2_EVT,
+};
+
+/**
+ * struct nvec_msg - A buffer for a single message
+ * @node: Messages are part of various lists in a &struct nvec_chip
+ * @data: The data of the message
+ * @size: For TX messages, the number of bytes used in @data
+ * @pos:  For RX messages, the current position to write to. For TX messages,
+ *        the position to read from.
+ * @used: Used for the message pool to mark a message as free/allocated.
+ *
+ * This structure is used to hold outgoing and incoming messages. Outgoing
+ * messages have a different format than incoming messages, and that is not
+ * documented yet.
+ */
+struct nvec_msg {
+	struct list_head node;
+	unsigned char data[NVEC_MSG_SIZE];
+	unsigned short size;
+	unsigned short pos;
+	atomic_t used;
+};
+
+/**
+ * struct nvec_chip - A single connection to an NVIDIA Embedded controller
+ * @dev: The device
+ * @gpio: The same as for &struct nvec_platform_data
+ * @irq: The IRQ of the I2C device
+ * @i2c_addr: The address of the I2C slave
+ * @base: The base of the memory mapped region of the I2C device
+ * @i2c_clk: The clock of the I2C device
+ * @rst: The reset of the I2C device
+ * @notifier_list: Notifiers to be called on received messages, see
+ *                 nvec_register_notifier()
+ * @rx_data: Received messages that have to be processed
+ * @tx_data: Messages waiting to be sent to the controller
+ * @nvec_status_notifier: Internal notifier (see nvec_status_notifier())
+ * @rx_work: A work structure for the RX worker nvec_dispatch()
+ * @tx_work: A work structure for the TX worker nvec_request_master()
+ * @wq: The work queue in which @rx_work and @tx_work are executed
+ * @rx: The message currently being retrieved or %NULL
+ * @msg_pool: A pool of messages for allocation
+ * @tx: The message currently being transferred
+ * @tx_scratch: Used for building pseudo messages
+ * @ec_transfer: A completion that will be completed once a message has been
+ *               received (see nvec_rx_completed())
+ * @tx_lock: Spinlock for modifications on @tx_data
+ * @rx_lock: Spinlock for modifications on @rx_data
+ * @sync_write_mutex: A mutex for nvec_write_sync()
+ * @sync_write: A completion to signal that a synchronous message is complete
+ * @sync_write_pending: The first two bytes of the request (type and subtype)
+ * @last_sync_msg: The last synchronous message.
+ * @state: State of our finite state machine used in nvec_interrupt()
+ */
+struct nvec_chip {
+	struct device *dev;
+	struct gpio_desc *gpiod;
+	int irq;
+	u32 i2c_addr;
+	void __iomem *base;
+	struct clk *i2c_clk;
+	struct reset_control *rst;
+	struct atomic_notifier_head notifier_list;
+	struct list_head rx_data, tx_data;
+	struct notifier_block nvec_status_notifier;
+	struct work_struct rx_work, tx_work;
+	struct workqueue_struct *wq;
+	struct nvec_msg msg_pool[NVEC_POOL_SIZE];
+	struct nvec_msg *rx;
+
+	struct nvec_msg *tx;
+	struct nvec_msg tx_scratch;
+	struct completion ec_transfer;
+
+	spinlock_t tx_lock, rx_lock;
+
+	/* sync write stuff */
+	struct mutex sync_write_mutex;
+	struct completion sync_write;
+	u16 sync_write_pending;
+	struct nvec_msg *last_sync_msg;
+
+	int state;
+};
+
+int nvec_write_async(struct nvec_chip *nvec, const unsigned char *data,
+		     short size);
+
+int nvec_write_sync(struct nvec_chip *nvec,
+		    const unsigned char *data, short size,
+		    struct nvec_msg **msg);
+
+int nvec_register_notifier(struct nvec_chip *nvec,
+			   struct notifier_block *nb,
+			   unsigned int events);
+
+int nvec_unregister_notifier(struct nvec_chip *dev, struct notifier_block *nb);
+
+void nvec_msg_free(struct nvec_chip *nvec, struct nvec_msg *msg);
+
+#endif
diff --git a/drivers/staging/nvec/nvec_kbd.c b/drivers/staging/nvec/nvec_kbd.c
new file mode 100644
index 000000000..386d619e3
--- /dev/null
+++ b/drivers/staging/nvec/nvec_kbd.c
@@ -0,0 +1,190 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * nvec_kbd: keyboard driver for a NVIDIA compliant embedded controller
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
+ *
+ * Authors:  Pierre-Hugues Husson <phhusson@free.fr>
+ *           Marc Dietrich <marvin24@gmx.de>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include "nvec-keytable.h"
+#include "nvec.h"
+
+enum kbd_subcmds {
+	CNFG_WAKE = 3,
+	CNFG_WAKE_KEY_REPORTING,
+	SET_LEDS = 0xed,
+	ENABLE_KBD = 0xf4,
+	DISABLE_KBD,
+};
+
+static unsigned char keycodes[ARRAY_SIZE(code_tab_102us)
+			      + ARRAY_SIZE(extcode_tab_us102)];
+
+struct nvec_keys {
+	struct input_dev *input;
+	struct notifier_block notifier;
+	struct nvec_chip *nvec;
+	bool caps_lock;
+};
+
+static struct nvec_keys keys_dev;
+
+static void nvec_kbd_toggle_led(void)
+{
+	char buf[] = { NVEC_KBD, SET_LEDS, 0 };
+
+	keys_dev.caps_lock = !keys_dev.caps_lock;
+
+	if (keys_dev.caps_lock)
+		/* should be BIT(0) only, firmware bug? */
+		buf[2] = BIT(0) | BIT(1) | BIT(2);
+
+	nvec_write_async(keys_dev.nvec, buf, sizeof(buf));
+}
+
+static int nvec_keys_notifier(struct notifier_block *nb,
+			      unsigned long event_type, void *data)
+{
+	int code, state;
+	unsigned char *msg = data;
+
+	if (event_type == NVEC_KB_EVT) {
+		int _size = (msg[0] & (3 << 5)) >> 5;
+
+/* power on/off button */
+		if (_size == NVEC_VAR_SIZE)
+			return NOTIFY_STOP;
+
+		if (_size == NVEC_3BYTES)
+			msg++;
+
+		code = msg[1] & 0x7f;
+		state = msg[1] & 0x80;
+
+		if (code_tabs[_size][code] == KEY_CAPSLOCK && state)
+			nvec_kbd_toggle_led();
+
+		input_report_key(keys_dev.input, code_tabs[_size][code],
+				 !state);
+		input_sync(keys_dev.input);
+
+		return NOTIFY_STOP;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int nvec_kbd_event(struct input_dev *dev, unsigned int type,
+			  unsigned int code, int value)
+{
+	struct nvec_chip *nvec = keys_dev.nvec;
+	char buf[] = { NVEC_KBD, SET_LEDS, 0 };
+
+	if (type == EV_REP)
+		return 0;
+
+	if (type != EV_LED)
+		return -1;
+
+	if (code != LED_CAPSL)
+		return -1;
+
+	buf[2] = !!value;
+	nvec_write_async(nvec, buf, sizeof(buf));
+
+	return 0;
+}
+
+static int nvec_kbd_probe(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	int i, j, err;
+	struct input_dev *idev;
+	char	clear_leds[] = { NVEC_KBD, SET_LEDS, 0 },
+		enable_kbd[] = { NVEC_KBD, ENABLE_KBD },
+		cnfg_wake[] = { NVEC_KBD, CNFG_WAKE, true, true },
+		cnfg_wake_key_reporting[] = { NVEC_KBD, CNFG_WAKE_KEY_REPORTING,
+						true };
+
+	j = 0;
+
+	for (i = 0; i < ARRAY_SIZE(code_tab_102us); ++i)
+		keycodes[j++] = code_tab_102us[i];
+
+	for (i = 0; i < ARRAY_SIZE(extcode_tab_us102); ++i)
+		keycodes[j++] = extcode_tab_us102[i];
+
+	idev = devm_input_allocate_device(&pdev->dev);
+	if (!idev)
+		return -ENOMEM;
+	idev->name = "nvec keyboard";
+	idev->phys = "nvec";
+	idev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP) | BIT_MASK(EV_LED);
+	idev->ledbit[0] = BIT_MASK(LED_CAPSL);
+	idev->event = nvec_kbd_event;
+	idev->keycode = keycodes;
+	idev->keycodesize = sizeof(unsigned char);
+	idev->keycodemax = ARRAY_SIZE(keycodes);
+
+	for (i = 0; i < ARRAY_SIZE(keycodes); ++i)
+		set_bit(keycodes[i], idev->keybit);
+
+	clear_bit(0, idev->keybit);
+	err = input_register_device(idev);
+	if (err)
+		return err;
+
+	keys_dev.input = idev;
+	keys_dev.notifier.notifier_call = nvec_keys_notifier;
+	keys_dev.nvec = nvec;
+	nvec_register_notifier(nvec, &keys_dev.notifier, 0);
+
+	/* Enable keyboard */
+	nvec_write_async(nvec, enable_kbd, 2);
+
+	/* configures wake on special keys */
+	nvec_write_async(nvec, cnfg_wake, 4);
+	/* enable wake key reporting */
+	nvec_write_async(nvec, cnfg_wake_key_reporting, 3);
+
+	/* Disable caps lock LED */
+	nvec_write_async(nvec, clear_leds, sizeof(clear_leds));
+
+	return 0;
+}
+
+static int nvec_kbd_remove(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	char disable_kbd[] = { NVEC_KBD, DISABLE_KBD },
+	     uncnfg_wake_key_reporting[] = { NVEC_KBD, CNFG_WAKE_KEY_REPORTING,
+						false };
+	nvec_write_async(nvec, uncnfg_wake_key_reporting, 3);
+	nvec_write_async(nvec, disable_kbd, 2);
+	nvec_unregister_notifier(nvec, &keys_dev.notifier);
+
+	return 0;
+}
+
+static struct platform_driver nvec_kbd_driver = {
+	.probe  = nvec_kbd_probe,
+	.remove = nvec_kbd_remove,
+	.driver = {
+		.name = "nvec-kbd",
+	},
+};
+
+module_platform_driver(nvec_kbd_driver);
+
+MODULE_AUTHOR("Marc Dietrich <marvin24@gmx.de>");
+MODULE_DESCRIPTION("NVEC keyboard driver");
+MODULE_ALIAS("platform:nvec-kbd");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/nvec/nvec_paz00.c b/drivers/staging/nvec/nvec_paz00.c
new file mode 100644
index 000000000..8b4da9508
--- /dev/null
+++ b/drivers/staging/nvec/nvec_paz00.c
@@ -0,0 +1,83 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * nvec_paz00: OEM specific driver for Compal PAZ00 based devices
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
+ *
+ * Authors:  Ilya Petrov <ilya.muromec@gmail.com>
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/leds.h>
+#include <linux/platform_device.h>
+#include "nvec.h"
+
+#define to_nvec_led(led_cdev) \
+	container_of(led_cdev, struct nvec_led, cdev)
+
+#define NVEC_LED_REQ {'\x0d', '\x10', '\x45', '\x10', '\x00'}
+
+#define NVEC_LED_MAX 8
+
+struct nvec_led {
+	struct led_classdev cdev;
+	struct nvec_chip *nvec;
+};
+
+static void nvec_led_brightness_set(struct led_classdev *led_cdev,
+				    enum led_brightness value)
+{
+	struct nvec_led *led = to_nvec_led(led_cdev);
+	unsigned char buf[] = NVEC_LED_REQ;
+
+	buf[4] = value;
+
+	nvec_write_async(led->nvec, buf, sizeof(buf));
+
+	led->cdev.brightness = value;
+}
+
+static int nvec_paz00_probe(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	struct nvec_led *led;
+	int ret = 0;
+
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->cdev.max_brightness = NVEC_LED_MAX;
+
+	led->cdev.brightness_set = nvec_led_brightness_set;
+	led->cdev.name = "paz00-led";
+	led->cdev.flags |= LED_CORE_SUSPENDRESUME;
+	led->nvec = nvec;
+
+	platform_set_drvdata(pdev, led);
+
+	ret = devm_led_classdev_register(&pdev->dev, &led->cdev);
+	if (ret < 0)
+		return ret;
+
+	/* to expose the default value to userspace */
+	led->cdev.brightness = 0;
+
+	return 0;
+}
+
+static struct platform_driver nvec_paz00_driver = {
+	.probe  = nvec_paz00_probe,
+	.driver = {
+		.name  = "nvec-paz00",
+	},
+};
+
+module_platform_driver(nvec_paz00_driver);
+
+MODULE_AUTHOR("Ilya Petrov <ilya.muromec@gmail.com>");
+MODULE_DESCRIPTION("Tegra NVEC PAZ00 driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:nvec-paz00");
diff --git a/drivers/staging/nvec/nvec_power.c b/drivers/staging/nvec/nvec_power.c
new file mode 100644
index 000000000..b1ef196e1
--- /dev/null
+++ b/drivers/staging/nvec/nvec_power.c
@@ -0,0 +1,449 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * nvec_power: power supply driver for a NVIDIA compliant embedded controller
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
+ *
+ * Authors:  Ilya Petrov <ilya.muromec@gmail.com>
+ *           Marc Dietrich <marvin24@gmx.de>
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+
+#include "nvec.h"
+
+#define GET_SYSTEM_STATUS 0x00
+
+struct nvec_power {
+	struct notifier_block notifier;
+	struct delayed_work poller;
+	struct nvec_chip *nvec;
+	int on;
+	int bat_present;
+	int bat_status;
+	int bat_voltage_now;
+	int bat_current_now;
+	int bat_current_avg;
+	int time_remain;
+	int charge_full_design;
+	int charge_last_full;
+	int critical_capacity;
+	int capacity_remain;
+	int bat_temperature;
+	int bat_cap;
+	int bat_type_enum;
+	char bat_manu[30];
+	char bat_model[30];
+	char bat_type[30];
+};
+
+enum {
+	SLOT_STATUS,
+	VOLTAGE,
+	TIME_REMAINING,
+	CURRENT,
+	AVERAGE_CURRENT,
+	AVERAGING_TIME_INTERVAL,
+	CAPACITY_REMAINING,
+	LAST_FULL_CHARGE_CAPACITY,
+	DESIGN_CAPACITY,
+	CRITICAL_CAPACITY,
+	TEMPERATURE,
+	MANUFACTURER,
+	MODEL,
+	TYPE,
+};
+
+enum {
+	AC,
+	BAT,
+};
+
+struct bat_response {
+	u8 event_type;
+	u8 length;
+	u8 sub_type;
+	u8 status;
+	/* payload */
+	union {
+		char plc[30];
+		u16 plu;
+		s16 pls;
+	};
+};
+
+static struct power_supply *nvec_bat_psy;
+static struct power_supply *nvec_psy;
+
+static int nvec_power_notifier(struct notifier_block *nb,
+			       unsigned long event_type, void *data)
+{
+	struct nvec_power *power =
+	    container_of(nb, struct nvec_power, notifier);
+	struct bat_response *res = data;
+
+	if (event_type != NVEC_SYS)
+		return NOTIFY_DONE;
+
+	if (res->sub_type == 0) {
+		if (power->on != res->plu) {
+			power->on = res->plu;
+			power_supply_changed(nvec_psy);
+		}
+		return NOTIFY_STOP;
+	}
+	return NOTIFY_OK;
+}
+
+static const int bat_init[] = {
+	LAST_FULL_CHARGE_CAPACITY, DESIGN_CAPACITY, CRITICAL_CAPACITY,
+	MANUFACTURER, MODEL, TYPE,
+};
+
+static void get_bat_mfg_data(struct nvec_power *power)
+{
+	int i;
+	char buf[] = { NVEC_BAT, SLOT_STATUS };
+
+	for (i = 0; i < ARRAY_SIZE(bat_init); i++) {
+		buf[1] = bat_init[i];
+		nvec_write_async(power->nvec, buf, 2);
+	}
+}
+
+static int nvec_power_bat_notifier(struct notifier_block *nb,
+				   unsigned long event_type, void *data)
+{
+	struct nvec_power *power =
+	    container_of(nb, struct nvec_power, notifier);
+	struct bat_response *res = data;
+	int status_changed = 0;
+
+	if (event_type != NVEC_BAT)
+		return NOTIFY_DONE;
+
+	switch (res->sub_type) {
+	case SLOT_STATUS:
+		if (res->plc[0] & 1) {
+			if (power->bat_present == 0) {
+				status_changed = 1;
+				get_bat_mfg_data(power);
+			}
+
+			power->bat_present = 1;
+
+			switch ((res->plc[0] >> 1) & 3) {
+			case 0:
+				power->bat_status =
+				    POWER_SUPPLY_STATUS_NOT_CHARGING;
+				break;
+			case 1:
+				power->bat_status =
+				    POWER_SUPPLY_STATUS_CHARGING;
+				break;
+			case 2:
+				power->bat_status =
+				    POWER_SUPPLY_STATUS_DISCHARGING;
+				break;
+			default:
+				power->bat_status = POWER_SUPPLY_STATUS_UNKNOWN;
+			}
+		} else {
+			if (power->bat_present == 1)
+				status_changed = 1;
+
+			power->bat_present = 0;
+			power->bat_status = POWER_SUPPLY_STATUS_UNKNOWN;
+		}
+		power->bat_cap = res->plc[1];
+		if (status_changed)
+			power_supply_changed(nvec_bat_psy);
+		break;
+	case VOLTAGE:
+		power->bat_voltage_now = res->plu * 1000;
+		break;
+	case TIME_REMAINING:
+		power->time_remain = res->plu * 3600;
+		break;
+	case CURRENT:
+		power->bat_current_now = res->pls * 1000;
+		break;
+	case AVERAGE_CURRENT:
+		power->bat_current_avg = res->pls * 1000;
+		break;
+	case CAPACITY_REMAINING:
+		power->capacity_remain = res->plu * 1000;
+		break;
+	case LAST_FULL_CHARGE_CAPACITY:
+		power->charge_last_full = res->plu * 1000;
+		break;
+	case DESIGN_CAPACITY:
+		power->charge_full_design = res->plu * 1000;
+		break;
+	case CRITICAL_CAPACITY:
+		power->critical_capacity = res->plu * 1000;
+		break;
+	case TEMPERATURE:
+		power->bat_temperature = res->plu - 2732;
+		break;
+	case MANUFACTURER:
+		memcpy(power->bat_manu, &res->plc, res->length - 2);
+		power->bat_model[res->length - 2] = '\0';
+		break;
+	case MODEL:
+		memcpy(power->bat_model, &res->plc, res->length - 2);
+		power->bat_model[res->length - 2] = '\0';
+		break;
+	case TYPE:
+		memcpy(power->bat_type, &res->plc, res->length - 2);
+		power->bat_type[res->length - 2] = '\0';
+		/*
+		 * This differs a little from the spec fill in more if you find
+		 * some.
+		 */
+		if (!strncmp(power->bat_type, "Li", 30))
+			power->bat_type_enum = POWER_SUPPLY_TECHNOLOGY_LION;
+		else
+			power->bat_type_enum = POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
+		break;
+	default:
+		return NOTIFY_STOP;
+	}
+
+	return NOTIFY_STOP;
+}
+
+static int nvec_power_get_property(struct power_supply *psy,
+				   enum power_supply_property psp,
+				   union power_supply_propval *val)
+{
+	struct nvec_power *power = dev_get_drvdata(psy->dev.parent);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = power->on;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int nvec_battery_get_property(struct power_supply *psy,
+				     enum power_supply_property psp,
+				     union power_supply_propval *val)
+{
+	struct nvec_power *power = dev_get_drvdata(psy->dev.parent);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = power->bat_status;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		val->intval = power->bat_cap;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = power->bat_present;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = power->bat_voltage_now;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = power->bat_current_now;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		val->intval = power->bat_current_avg;
+		break;
+	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW:
+		val->intval = power->time_remain;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = power->charge_full_design;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		val->intval = power->charge_last_full;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_EMPTY:
+		val->intval = power->critical_capacity;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = power->capacity_remain;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = power->bat_temperature;
+		break;
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		val->strval = power->bat_manu;
+		break;
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = power->bat_model;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = power->bat_type_enum;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static enum power_supply_property nvec_power_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static enum power_supply_property nvec_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+#ifdef EC_FULL_DIAG
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW,
+#endif
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_EMPTY,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_MANUFACTURER,
+	POWER_SUPPLY_PROP_MODEL_NAME,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+};
+
+static char *nvec_power_supplied_to[] = {
+	"battery",
+};
+
+static const struct power_supply_desc nvec_bat_psy_desc = {
+	.name = "battery",
+	.type = POWER_SUPPLY_TYPE_BATTERY,
+	.properties = nvec_battery_props,
+	.num_properties = ARRAY_SIZE(nvec_battery_props),
+	.get_property = nvec_battery_get_property,
+};
+
+static const struct power_supply_desc nvec_psy_desc = {
+	.name = "ac",
+	.type = POWER_SUPPLY_TYPE_MAINS,
+	.properties = nvec_power_props,
+	.num_properties = ARRAY_SIZE(nvec_power_props),
+	.get_property = nvec_power_get_property,
+};
+
+static int counter;
+static const int bat_iter[] = {
+	SLOT_STATUS, VOLTAGE, CURRENT, CAPACITY_REMAINING,
+#ifdef EC_FULL_DIAG
+	AVERAGE_CURRENT, TEMPERATURE, TIME_REMAINING,
+#endif
+};
+
+static void nvec_power_poll(struct work_struct *work)
+{
+	char buf[] = { NVEC_SYS, GET_SYSTEM_STATUS };
+	struct nvec_power *power = container_of(work, struct nvec_power,
+						poller.work);
+
+	if (counter >= ARRAY_SIZE(bat_iter))
+		counter = 0;
+
+	/* AC status via sys req */
+	nvec_write_async(power->nvec, buf, 2);
+	msleep(100);
+
+	/*
+	 * Select a battery request function via round robin doing it all at
+	 * once seems to overload the power supply.
+	 */
+	buf[0] = NVEC_BAT;
+	buf[1] = bat_iter[counter++];
+	nvec_write_async(power->nvec, buf, 2);
+
+	schedule_delayed_work(to_delayed_work(work), msecs_to_jiffies(5000));
+};
+
+static int nvec_power_probe(struct platform_device *pdev)
+{
+	struct power_supply **psy;
+	const struct power_supply_desc *psy_desc;
+	struct nvec_power *power;
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config psy_cfg = {};
+
+	power = devm_kzalloc(&pdev->dev, sizeof(struct nvec_power), GFP_NOWAIT);
+	if (!power)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, power);
+	power->nvec = nvec;
+
+	switch (pdev->id) {
+	case AC:
+		psy = &nvec_psy;
+		psy_desc = &nvec_psy_desc;
+		psy_cfg.supplied_to = nvec_power_supplied_to;
+		psy_cfg.num_supplicants = ARRAY_SIZE(nvec_power_supplied_to);
+
+		power->notifier.notifier_call = nvec_power_notifier;
+
+		INIT_DELAYED_WORK(&power->poller, nvec_power_poll);
+		schedule_delayed_work(&power->poller, msecs_to_jiffies(5000));
+		break;
+	case BAT:
+		psy = &nvec_bat_psy;
+		psy_desc = &nvec_bat_psy_desc;
+
+		power->notifier.notifier_call = nvec_power_bat_notifier;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	nvec_register_notifier(nvec, &power->notifier, NVEC_SYS);
+
+	if (pdev->id == BAT)
+		get_bat_mfg_data(power);
+
+	*psy = power_supply_register(&pdev->dev, psy_desc, &psy_cfg);
+
+	return PTR_ERR_OR_ZERO(*psy);
+}
+
+static int nvec_power_remove(struct platform_device *pdev)
+{
+	struct nvec_power *power = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&power->poller);
+	nvec_unregister_notifier(power->nvec, &power->notifier);
+	switch (pdev->id) {
+	case AC:
+		power_supply_unregister(nvec_psy);
+		break;
+	case BAT:
+		power_supply_unregister(nvec_bat_psy);
+	}
+
+	return 0;
+}
+
+static struct platform_driver nvec_power_driver = {
+	.probe = nvec_power_probe,
+	.remove = nvec_power_remove,
+	.driver = {
+		   .name = "nvec-power",
+	}
+};
+
+module_platform_driver(nvec_power_driver);
+
+MODULE_AUTHOR("Ilya Petrov <ilya.muromec@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NVEC battery and AC driver");
+MODULE_ALIAS("platform:nvec-power");
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
new file mode 100644
index 000000000..06041c7f7
--- /dev/null
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * nvec_ps2: mouse driver for a NVIDIA compliant embedded controller
+ *
+ * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
+ *
+ * Authors:  Pierre-Hugues Husson <phhusson@free.fr>
+ *           Ilya Petrov <ilya.muromec@gmail.com>
+ *           Marc Dietrich <marvin24@gmx.de>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/serio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include "nvec.h"
+
+#define PACKET_SIZE	6
+
+#define ENABLE_MOUSE	0xf4
+#define DISABLE_MOUSE	0xf5
+#define PSMOUSE_RST	0xff
+
+#ifdef NVEC_PS2_DEBUG
+#define NVEC_PHD(str, buf, len) \
+	print_hex_dump(KERN_DEBUG, str, DUMP_PREFIX_NONE, \
+			16, 1, buf, len, false)
+#else
+#define NVEC_PHD(str, buf, len) do { } while (0)
+#endif
+
+enum ps2_subcmds {
+	SEND_COMMAND = 1,
+	RECEIVE_N,
+	AUTO_RECEIVE_N,
+	CANCEL_AUTO_RECEIVE,
+};
+
+struct nvec_ps2 {
+	struct serio *ser_dev;
+	struct notifier_block notifier;
+	struct nvec_chip *nvec;
+};
+
+static struct nvec_ps2 ps2_dev;
+
+static int ps2_startstreaming(struct serio *ser_dev)
+{
+	unsigned char buf[] = { NVEC_PS2, AUTO_RECEIVE_N, PACKET_SIZE };
+
+	return nvec_write_async(ps2_dev.nvec, buf, sizeof(buf));
+}
+
+static void ps2_stopstreaming(struct serio *ser_dev)
+{
+	unsigned char buf[] = { NVEC_PS2, CANCEL_AUTO_RECEIVE };
+
+	nvec_write_async(ps2_dev.nvec, buf, sizeof(buf));
+}
+
+static int ps2_sendcommand(struct serio *ser_dev, unsigned char cmd)
+{
+	unsigned char buf[] = { NVEC_PS2, SEND_COMMAND, ENABLE_MOUSE, 1 };
+
+	buf[2] = cmd & 0xff;
+
+	dev_dbg(&ser_dev->dev, "Sending ps2 cmd %02x\n", cmd);
+	return nvec_write_async(ps2_dev.nvec, buf, sizeof(buf));
+}
+
+static int nvec_ps2_notifier(struct notifier_block *nb,
+			     unsigned long event_type, void *data)
+{
+	int i;
+	unsigned char *msg = data;
+
+	switch (event_type) {
+	case NVEC_PS2_EVT:
+		for (i = 0; i < msg[1]; i++)
+			serio_interrupt(ps2_dev.ser_dev, msg[2 + i], 0);
+		NVEC_PHD("ps/2 mouse event: ", &msg[2], msg[1]);
+		return NOTIFY_STOP;
+
+	case NVEC_PS2:
+		if (msg[2] == 1) {
+			for (i = 0; i < (msg[1] - 2); i++)
+				serio_interrupt(ps2_dev.ser_dev, msg[i + 4], 0);
+			NVEC_PHD("ps/2 mouse reply: ", &msg[4], msg[1] - 2);
+		}
+
+		else if (msg[1] != 2) /* !ack */
+			NVEC_PHD("unhandled mouse event: ", msg, msg[1] + 2);
+		return NOTIFY_STOP;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int nvec_mouse_probe(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	struct serio *ser_dev;
+
+	ser_dev = kzalloc(sizeof(*ser_dev), GFP_KERNEL);
+	if (!ser_dev)
+		return -ENOMEM;
+
+	ser_dev->id.type = SERIO_8042;
+	ser_dev->write = ps2_sendcommand;
+	ser_dev->start = ps2_startstreaming;
+	ser_dev->stop = ps2_stopstreaming;
+
+	strscpy(ser_dev->name, "nvec mouse", sizeof(ser_dev->name));
+	strscpy(ser_dev->phys, "nvec", sizeof(ser_dev->phys));
+
+	ps2_dev.ser_dev = ser_dev;
+	ps2_dev.notifier.notifier_call = nvec_ps2_notifier;
+	ps2_dev.nvec = nvec;
+	nvec_register_notifier(nvec, &ps2_dev.notifier, 0);
+
+	serio_register_port(ser_dev);
+
+	return 0;
+}
+
+static int nvec_mouse_remove(struct platform_device *pdev)
+{
+	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+
+	ps2_sendcommand(ps2_dev.ser_dev, DISABLE_MOUSE);
+	ps2_stopstreaming(ps2_dev.ser_dev);
+	nvec_unregister_notifier(nvec, &ps2_dev.notifier);
+	serio_unregister_port(ps2_dev.ser_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int nvec_mouse_suspend(struct device *dev)
+{
+	/* disable mouse */
+	ps2_sendcommand(ps2_dev.ser_dev, DISABLE_MOUSE);
+
+	/* send cancel autoreceive */
+	ps2_stopstreaming(ps2_dev.ser_dev);
+
+	return 0;
+}
+
+static int nvec_mouse_resume(struct device *dev)
+{
+	/* start streaming */
+	ps2_startstreaming(ps2_dev.ser_dev);
+
+	/* enable mouse */
+	ps2_sendcommand(ps2_dev.ser_dev, ENABLE_MOUSE);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(nvec_mouse_pm_ops, nvec_mouse_suspend,
+			 nvec_mouse_resume);
+
+static struct platform_driver nvec_mouse_driver = {
+	.probe  = nvec_mouse_probe,
+	.remove = nvec_mouse_remove,
+	.driver = {
+		.name = "nvec-mouse",
+		.pm = &nvec_mouse_pm_ops,
+	},
+};
+
+module_platform_driver(nvec_mouse_driver);
+
+MODULE_DESCRIPTION("NVEC mouse driver");
+MODULE_AUTHOR("Marc Dietrich <marvin24@gmx.de>");
+MODULE_ALIAS("platform:nvec-mouse");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/octeon/Kconfig b/drivers/staging/octeon/Kconfig
new file mode 100644
index 000000000..5319909eb
--- /dev/null
+++ b/drivers/staging/octeon/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0
+config OCTEON_ETHERNET
+	tristate "Cavium Networks Octeon Ethernet support"
+	depends on CAVIUM_OCTEON_SOC || COMPILE_TEST
+	depends on NETDEVICES
+	select PHYLIB
+	select MDIO_OCTEON
+	help
+	  This driver supports the builtin ethernet ports on Cavium
+	  Networks' products in the Octeon family. This driver supports the
+	  CN3XXX and CN5XXX Octeon processors.
+
+	  To compile this driver as a module, choose M here.  The module
+	  will be called octeon-ethernet.
+
diff --git a/drivers/staging/octeon/Makefile b/drivers/staging/octeon/Makefile
new file mode 100644
index 000000000..3887cf5f1
--- /dev/null
+++ b/drivers/staging/octeon/Makefile
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Copyright (C) 2005-2009 Cavium Networks
+#
+
+#
+# Makefile for Cavium OCTEON on-board ethernet driver
+#
+
+obj-${CONFIG_OCTEON_ETHERNET} :=  octeon-ethernet.o
+
+octeon-ethernet-y := ethernet.o
+octeon-ethernet-y += ethernet-mdio.o
+octeon-ethernet-y += ethernet-mem.o
+octeon-ethernet-y += ethernet-rgmii.o
+octeon-ethernet-y += ethernet-rx.o
+octeon-ethernet-y += ethernet-sgmii.o
+octeon-ethernet-y += ethernet-spi.o
+octeon-ethernet-y += ethernet-tx.o
diff --git a/drivers/staging/octeon/TODO b/drivers/staging/octeon/TODO
new file mode 100644
index 000000000..67a0a1f6b
--- /dev/null
+++ b/drivers/staging/octeon/TODO
@@ -0,0 +1,9 @@
+This driver is functional and supports Ethernet on OCTEON+/OCTEON2/OCTEON3
+chips at least up to CN7030.
+
+TODO:
+	- general code review and clean up
+	- make driver self-contained instead of being split between staging and
+	  arch/mips/cavium-octeon.
+
+Contact: Aaro Koskinen <aaro.koskinen@iki.fi>
diff --git a/drivers/staging/octeon/ethernet-defines.h b/drivers/staging/octeon/ethernet-defines.h
new file mode 100644
index 000000000..c060374a3
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-defines.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+/*
+ * A few defines are used to control the operation of this driver:
+ *  USE_ASYNC_IOBDMA
+ *      Use asynchronous IO access to hardware. This uses Octeon's asynchronous
+ *      IOBDMAs to issue IO accesses without stalling. Set this to zero
+ *      to disable this. Note that IOBDMAs require CVMSEG.
+ *  REUSE_SKBUFFS_WITHOUT_FREE
+ *      Allows the TX path to free an skbuff into the FPA hardware pool. This
+ *      can significantly improve performance for forwarding and bridging, but
+ *      may be somewhat dangerous. Checks are made, but if any buffer is reused
+ *      without the proper Linux cleanup, the networking stack may have very
+ *      bizarre bugs.
+ */
+#ifndef __ETHERNET_DEFINES_H__
+#define __ETHERNET_DEFINES_H__
+
+#ifdef CONFIG_NETFILTER
+#define REUSE_SKBUFFS_WITHOUT_FREE	0
+#else
+#define REUSE_SKBUFFS_WITHOUT_FREE	1
+#endif
+
+#define USE_ASYNC_IOBDMA		(CONFIG_CAVIUM_OCTEON_CVMSEG_SIZE > 0)
+
+/* Maximum number of SKBs to try to free per xmit packet. */
+#define MAX_OUT_QUEUE_DEPTH		1000
+
+#define FAU_TOTAL_TX_TO_CLEAN (CVMX_FAU_REG_END - sizeof(u32))
+#define FAU_NUM_PACKET_BUFFERS_TO_FREE (FAU_TOTAL_TX_TO_CLEAN - sizeof(u32))
+
+#define TOTAL_NUMBER_OF_PORTS		(CVMX_PIP_NUM_INPUT_PORTS + 1)
+
+#endif /* __ETHERNET_DEFINES_H__ */
diff --git a/drivers/staging/octeon/ethernet-mdio.c b/drivers/staging/octeon/ethernet-mdio.c
new file mode 100644
index 000000000..b3049108e
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-mdio.c
@@ -0,0 +1,171 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/kernel.h>
+#include <linux/ethtool.h>
+#include <linux/phy.h>
+#include <linux/ratelimit.h>
+#include <linux/of_mdio.h>
+#include <generated/utsrelease.h>
+#include <net/dst.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-mdio.h"
+#include "ethernet-util.h"
+
+static void cvm_oct_get_drvinfo(struct net_device *dev,
+				struct ethtool_drvinfo *info)
+{
+	strscpy(info->driver, KBUILD_MODNAME, sizeof(info->driver));
+	strscpy(info->version, UTS_RELEASE, sizeof(info->version));
+	strscpy(info->bus_info, "Builtin", sizeof(info->bus_info));
+}
+
+static int cvm_oct_nway_reset(struct net_device *dev)
+{
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	if (dev->phydev)
+		return phy_start_aneg(dev->phydev);
+
+	return -EINVAL;
+}
+
+const struct ethtool_ops cvm_oct_ethtool_ops = {
+	.get_drvinfo = cvm_oct_get_drvinfo,
+	.nway_reset = cvm_oct_nway_reset,
+	.get_link = ethtool_op_get_link,
+	.get_link_ksettings = phy_ethtool_get_link_ksettings,
+	.set_link_ksettings = phy_ethtool_set_link_ksettings,
+};
+
+/**
+ * cvm_oct_ioctl - IOCTL support for PHY control
+ * @dev:    Device to change
+ * @rq:     the request
+ * @cmd:    the command
+ *
+ * Returns Zero on success
+ */
+int cvm_oct_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	if (!netif_running(dev))
+		return -EINVAL;
+
+	if (!dev->phydev)
+		return -EINVAL;
+
+	return phy_mii_ioctl(dev->phydev, rq, cmd);
+}
+
+void cvm_oct_note_carrier(struct octeon_ethernet *priv,
+			  union cvmx_helper_link_info li)
+{
+	if (li.s.link_up) {
+		pr_notice_ratelimited("%s: %u Mbps %s duplex, port %d, queue %d\n",
+				      netdev_name(priv->netdev), li.s.speed,
+				      (li.s.full_duplex) ? "Full" : "Half",
+				      priv->port, priv->queue);
+	} else {
+		pr_notice_ratelimited("%s: Link down\n",
+				      netdev_name(priv->netdev));
+	}
+}
+
+void cvm_oct_adjust_link(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	union cvmx_helper_link_info link_info;
+
+	link_info.u64		= 0;
+	link_info.s.link_up	= dev->phydev->link ? 1 : 0;
+	link_info.s.full_duplex = dev->phydev->duplex ? 1 : 0;
+	link_info.s.speed	= dev->phydev->speed;
+	priv->link_info		= link_info.u64;
+
+	/*
+	 * The polling task need to know about link status changes.
+	 */
+	if (priv->poll)
+		priv->poll(dev);
+
+	if (priv->last_link != dev->phydev->link) {
+		priv->last_link = dev->phydev->link;
+		cvmx_helper_link_set(priv->port, link_info);
+		cvm_oct_note_carrier(priv, link_info);
+	}
+}
+
+int cvm_oct_common_stop(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
+	union cvmx_helper_link_info link_info;
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	int index = INDEX(priv->port);
+
+	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+	gmx_cfg.s.en = 0;
+	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
+
+	priv->poll = NULL;
+
+	if (dev->phydev)
+		phy_disconnect(dev->phydev);
+
+	if (priv->last_link) {
+		link_info.u64 = 0;
+		priv->last_link = 0;
+
+		cvmx_helper_link_set(priv->port, link_info);
+		cvm_oct_note_carrier(priv, link_info);
+	}
+	return 0;
+}
+
+/**
+ * cvm_oct_phy_setup_device - setup the PHY
+ *
+ * @dev:    Device to setup
+ *
+ * Returns Zero on success, negative on failure
+ */
+int cvm_oct_phy_setup_device(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	struct device_node *phy_node;
+	struct phy_device *phydev = NULL;
+
+	if (!priv->of_node)
+		goto no_phy;
+
+	phy_node = of_parse_phandle(priv->of_node, "phy-handle", 0);
+	if (!phy_node && of_phy_is_fixed_link(priv->of_node))
+		phy_node = of_node_get(priv->of_node);
+	if (!phy_node)
+		goto no_phy;
+
+	phydev = of_phy_connect(dev, phy_node, cvm_oct_adjust_link, 0,
+				priv->phy_mode);
+	of_node_put(phy_node);
+
+	if (!phydev)
+		return -EPROBE_DEFER;
+
+	priv->last_link = 0;
+	phy_start(phydev);
+
+	return 0;
+no_phy:
+	/* If there is no phy, assume a direct MAC connection and that
+	 * the link is up.
+	 */
+	netif_carrier_on(dev);
+	return 0;
+}
diff --git a/drivers/staging/octeon/ethernet-mdio.h b/drivers/staging/octeon/ethernet-mdio.h
new file mode 100644
index 000000000..7f6716e3f
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-mdio.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/string.h>
+#include <linux/ethtool.h>
+#include <linux/seq_file.h>
+#include <linux/proc_fs.h>
+#include <net/dst.h>
+#ifdef CONFIG_XFRM
+#include <linux/xfrm.h>
+#include <net/xfrm.h>
+#endif /* CONFIG_XFRM */
+
+extern const struct ethtool_ops cvm_oct_ethtool_ops;
+
+int cvm_oct_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+int cvm_oct_phy_setup_device(struct net_device *dev);
diff --git a/drivers/staging/octeon/ethernet-mem.c b/drivers/staging/octeon/ethernet-mem.c
new file mode 100644
index 000000000..532594957
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-mem.c
@@ -0,0 +1,154 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2010 Cavium Networks
+ */
+
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/slab.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-mem.h"
+#include "ethernet-defines.h"
+
+/**
+ * cvm_oct_fill_hw_skbuff - fill the supplied hardware pool with skbuffs
+ * @pool:     Pool to allocate an skbuff for
+ * @size:     Size of the buffer needed for the pool
+ * @elements: Number of buffers to allocate
+ *
+ * Returns the actual number of buffers allocated.
+ */
+static int cvm_oct_fill_hw_skbuff(int pool, int size, int elements)
+{
+	int freed = elements;
+
+	while (freed) {
+		struct sk_buff *skb = dev_alloc_skb(size + 256);
+
+		if (unlikely(!skb))
+			break;
+		skb_reserve(skb, 256 - (((unsigned long)skb->data) & 0x7f));
+		*(struct sk_buff **)(skb->data - sizeof(void *)) = skb;
+		cvmx_fpa_free(skb->data, pool, size / 128);
+		freed--;
+	}
+	return elements - freed;
+}
+
+/**
+ * cvm_oct_free_hw_skbuff- free hardware pool skbuffs
+ * @pool:     Pool to allocate an skbuff for
+ * @size:     Size of the buffer needed for the pool
+ * @elements: Number of buffers to allocate
+ */
+static void cvm_oct_free_hw_skbuff(int pool, int size, int elements)
+{
+	char *memory;
+
+	do {
+		memory = cvmx_fpa_alloc(pool);
+		if (memory) {
+			struct sk_buff *skb =
+			    *(struct sk_buff **)(memory - sizeof(void *));
+			elements--;
+			dev_kfree_skb(skb);
+		}
+	} while (memory);
+
+	if (elements < 0)
+		pr_warn("Freeing of pool %u had too many skbuffs (%d)\n",
+			pool, elements);
+	else if (elements > 0)
+		pr_warn("Freeing of pool %u is missing %d skbuffs\n",
+			pool, elements);
+}
+
+/**
+ * cvm_oct_fill_hw_memory - fill a hardware pool with memory.
+ * @pool:     Pool to populate
+ * @size:     Size of each buffer in the pool
+ * @elements: Number of buffers to allocate
+ *
+ * Returns the actual number of buffers allocated.
+ */
+static int cvm_oct_fill_hw_memory(int pool, int size, int elements)
+{
+	char *memory;
+	char *fpa;
+	int freed = elements;
+
+	while (freed) {
+		/*
+		 * FPA memory must be 128 byte aligned.  Since we are
+		 * aligning we need to save the original pointer so we
+		 * can feed it to kfree when the memory is returned to
+		 * the kernel.
+		 *
+		 * We allocate an extra 256 bytes to allow for
+		 * alignment and space for the original pointer saved
+		 * just before the block.
+		 */
+		memory = kmalloc(size + 256, GFP_ATOMIC);
+		if (unlikely(!memory)) {
+			pr_warn("Unable to allocate %u bytes for FPA pool %d\n",
+				elements * size, pool);
+			break;
+		}
+		fpa = (char *)(((unsigned long)memory + 256) & ~0x7fUL);
+		*((char **)fpa - 1) = memory;
+		cvmx_fpa_free(fpa, pool, 0);
+		freed--;
+	}
+	return elements - freed;
+}
+
+/**
+ * cvm_oct_free_hw_memory - Free memory allocated by cvm_oct_fill_hw_memory
+ * @pool:     FPA pool to free
+ * @size:     Size of each buffer in the pool
+ * @elements: Number of buffers that should be in the pool
+ */
+static void cvm_oct_free_hw_memory(int pool, int size, int elements)
+{
+	char *memory;
+	char *fpa;
+
+	do {
+		fpa = cvmx_fpa_alloc(pool);
+		if (fpa) {
+			elements--;
+			fpa = (char *)phys_to_virt(cvmx_ptr_to_phys(fpa));
+			memory = *((char **)fpa - 1);
+			kfree(memory);
+		}
+	} while (fpa);
+
+	if (elements < 0)
+		pr_warn("Freeing of pool %u had too many buffers (%d)\n",
+			pool, elements);
+	else if (elements > 0)
+		pr_warn("Warning: Freeing of pool %u is missing %d buffers\n",
+			pool, elements);
+}
+
+int cvm_oct_mem_fill_fpa(int pool, int size, int elements)
+{
+	int freed;
+
+	if (pool == CVMX_FPA_PACKET_POOL)
+		freed = cvm_oct_fill_hw_skbuff(pool, size, elements);
+	else
+		freed = cvm_oct_fill_hw_memory(pool, size, elements);
+	return freed;
+}
+
+void cvm_oct_mem_empty_fpa(int pool, int size, int elements)
+{
+	if (pool == CVMX_FPA_PACKET_POOL)
+		cvm_oct_free_hw_skbuff(pool, size, elements);
+	else
+		cvm_oct_free_hw_memory(pool, size, elements);
+}
diff --git a/drivers/staging/octeon/ethernet-mem.h b/drivers/staging/octeon/ethernet-mem.h
new file mode 100644
index 000000000..692dcdb71
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-mem.h
@@ -0,0 +1,9 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+int cvm_oct_mem_fill_fpa(int pool, int size, int elements);
+void cvm_oct_mem_empty_fpa(int pool, int size, int elements);
diff --git a/drivers/staging/octeon/ethernet-rgmii.c b/drivers/staging/octeon/ethernet-rgmii.c
new file mode 100644
index 000000000..0c4fac315
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-rgmii.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/interrupt.h>
+#include <linux/phy.h>
+#include <linux/ratelimit.h>
+#include <net/dst.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-util.h"
+#include "ethernet-mdio.h"
+
+static DEFINE_SPINLOCK(global_register_lock);
+
+static void cvm_oct_set_hw_preamble(struct octeon_ethernet *priv, bool enable)
+{
+	union cvmx_gmxx_rxx_frm_ctl gmxx_rxx_frm_ctl;
+	union cvmx_ipd_sub_port_fcs ipd_sub_port_fcs;
+	union cvmx_gmxx_rxx_int_reg gmxx_rxx_int_reg;
+	int interface = INTERFACE(priv->port);
+	int index = INDEX(priv->port);
+
+	/* Set preamble checking. */
+	gmxx_rxx_frm_ctl.u64 = cvmx_read_csr(CVMX_GMXX_RXX_FRM_CTL(index,
+								   interface));
+	gmxx_rxx_frm_ctl.s.pre_chk = enable;
+	cvmx_write_csr(CVMX_GMXX_RXX_FRM_CTL(index, interface),
+		       gmxx_rxx_frm_ctl.u64);
+
+	/* Set FCS stripping. */
+	ipd_sub_port_fcs.u64 = cvmx_read_csr(CVMX_IPD_SUB_PORT_FCS);
+	if (enable)
+		ipd_sub_port_fcs.s.port_bit |= 1ull << priv->port;
+	else
+		ipd_sub_port_fcs.s.port_bit &=
+					0xffffffffull ^ (1ull << priv->port);
+	cvmx_write_csr(CVMX_IPD_SUB_PORT_FCS, ipd_sub_port_fcs.u64);
+
+	/* Clear any error bits. */
+	gmxx_rxx_int_reg.u64 = cvmx_read_csr(CVMX_GMXX_RXX_INT_REG(index,
+								   interface));
+	cvmx_write_csr(CVMX_GMXX_RXX_INT_REG(index, interface),
+		       gmxx_rxx_int_reg.u64);
+}
+
+static void cvm_oct_check_preamble_errors(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	union cvmx_helper_link_info link_info;
+	unsigned long flags;
+
+	link_info.u64 = priv->link_info;
+
+	/*
+	 * Take the global register lock since we are going to
+	 * touch registers that affect more than one port.
+	 */
+	spin_lock_irqsave(&global_register_lock, flags);
+
+	if (link_info.s.speed == 10 && priv->last_speed == 10) {
+		/*
+		 * Read the GMXX_RXX_INT_REG[PCTERR] bit and see if we are
+		 * getting preamble errors.
+		 */
+		int interface = INTERFACE(priv->port);
+		int index = INDEX(priv->port);
+		union cvmx_gmxx_rxx_int_reg gmxx_rxx_int_reg;
+
+		gmxx_rxx_int_reg.u64 = cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
+							(index, interface));
+		if (gmxx_rxx_int_reg.s.pcterr) {
+			/*
+			 * We are getting preamble errors at 10Mbps. Most
+			 * likely the PHY is giving us packets with misaligned
+			 * preambles. In order to get these packets we need to
+			 * disable preamble checking and do it in software.
+			 */
+			cvm_oct_set_hw_preamble(priv, false);
+			printk_ratelimited("%s: Using 10Mbps with software preamble removal\n",
+					   dev->name);
+		}
+	} else {
+		/*
+		 * Since the 10Mbps preamble workaround is allowed we need to
+		 * enable preamble checking, FCS stripping, and clear error
+		 * bits on every speed change. If errors occur during 10Mbps
+		 * operation the above code will change this stuff
+		 */
+		if (priv->last_speed != link_info.s.speed)
+			cvm_oct_set_hw_preamble(priv, true);
+		priv->last_speed = link_info.s.speed;
+	}
+	spin_unlock_irqrestore(&global_register_lock, flags);
+}
+
+static void cvm_oct_rgmii_poll(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	union cvmx_helper_link_info link_info;
+	bool status_change;
+
+	link_info = cvmx_helper_link_get(priv->port);
+	if (priv->link_info != link_info.u64 &&
+	    cvmx_helper_link_set(priv->port, link_info))
+		link_info.u64 = priv->link_info;
+	status_change = priv->link_info != link_info.u64;
+	priv->link_info = link_info.u64;
+
+	cvm_oct_check_preamble_errors(dev);
+
+	if (likely(!status_change))
+		return;
+
+	/* Tell core. */
+	if (link_info.s.link_up) {
+		if (!netif_carrier_ok(dev))
+			netif_carrier_on(dev);
+	} else if (netif_carrier_ok(dev)) {
+		netif_carrier_off(dev);
+	}
+	cvm_oct_note_carrier(priv, link_info);
+}
+
+int cvm_oct_rgmii_open(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int ret;
+
+	ret = cvm_oct_common_open(dev, cvm_oct_rgmii_poll);
+	if (ret)
+		return ret;
+
+	if (dev->phydev) {
+		/*
+		 * In phydev mode, we need still periodic polling for the
+		 * preamble error checking, and we also need to call this
+		 * function on every link state change.
+		 *
+		 * Only true RGMII ports need to be polled. In GMII mode, port
+		 * 0 is really a RGMII port.
+		 */
+		if ((priv->imode == CVMX_HELPER_INTERFACE_MODE_GMII &&
+		     priv->port  == 0) ||
+		    (priv->imode == CVMX_HELPER_INTERFACE_MODE_RGMII)) {
+			priv->poll = cvm_oct_check_preamble_errors;
+			cvm_oct_check_preamble_errors(dev);
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
new file mode 100644
index 000000000..965330eec
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -0,0 +1,542 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2010 Cavium Networks
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/cache.h>
+#include <linux/cpumask.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/string.h>
+#include <linux/prefetch.h>
+#include <linux/ratelimit.h>
+#include <linux/smp.h>
+#include <linux/interrupt.h>
+#include <net/dst.h>
+#ifdef CONFIG_XFRM
+#include <linux/xfrm.h>
+#include <net/xfrm.h>
+#endif /* CONFIG_XFRM */
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-mem.h"
+#include "ethernet-rx.h"
+#include "ethernet-util.h"
+
+static atomic_t oct_rx_ready = ATOMIC_INIT(0);
+
+static struct oct_rx_group {
+	int irq;
+	int group;
+	struct napi_struct napi;
+} oct_rx_group[16];
+
+/**
+ * cvm_oct_do_interrupt - interrupt handler.
+ * @irq: Interrupt number.
+ * @napi_id: Cookie to identify the NAPI instance.
+ *
+ * The interrupt occurs whenever the POW has packets in our group.
+ *
+ */
+static irqreturn_t cvm_oct_do_interrupt(int irq, void *napi_id)
+{
+	/* Disable the IRQ and start napi_poll. */
+	disable_irq_nosync(irq);
+	napi_schedule(napi_id);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * cvm_oct_check_rcv_error - process receive errors
+ * @work: Work queue entry pointing to the packet.
+ *
+ * Returns Non-zero if the packet can be dropped, zero otherwise.
+ */
+static inline int cvm_oct_check_rcv_error(struct cvmx_wqe *work)
+{
+	int port;
+
+	if (octeon_has_feature(OCTEON_FEATURE_PKND))
+		port = work->word0.pip.cn68xx.pknd;
+	else
+		port = work->word1.cn38xx.ipprt;
+
+	if ((work->word2.snoip.err_code == 10) && (work->word1.len <= 64))
+		/*
+		 * Ignore length errors on min size packets. Some
+		 * equipment incorrectly pads packets to 64+4FCS
+		 * instead of 60+4FCS.  Note these packets still get
+		 * counted as frame errors.
+		 */
+		return 0;
+
+	if (work->word2.snoip.err_code == 5 ||
+	    work->word2.snoip.err_code == 7) {
+		/*
+		 * We received a packet with either an alignment error
+		 * or a FCS error. This may be signalling that we are
+		 * running 10Mbps with GMXX_RXX_FRM_CTL[PRE_CHK]
+		 * off. If this is the case we need to parse the
+		 * packet to determine if we can remove a non spec
+		 * preamble and generate a correct packet.
+		 */
+		int interface = cvmx_helper_get_interface_num(port);
+		int index = cvmx_helper_get_interface_index_num(port);
+		union cvmx_gmxx_rxx_frm_ctl gmxx_rxx_frm_ctl;
+
+		gmxx_rxx_frm_ctl.u64 =
+		    cvmx_read_csr(CVMX_GMXX_RXX_FRM_CTL(index, interface));
+		if (gmxx_rxx_frm_ctl.s.pre_chk == 0) {
+			u8 *ptr =
+			    cvmx_phys_to_ptr(work->packet_ptr.s.addr);
+			int i = 0;
+
+			while (i < work->word1.len - 1) {
+				if (*ptr != 0x55)
+					break;
+				ptr++;
+				i++;
+			}
+
+			if (*ptr == 0xd5) {
+				/* Port received 0xd5 preamble */
+				work->packet_ptr.s.addr += i + 1;
+				work->word1.len -= i + 5;
+				return 0;
+			}
+
+			if ((*ptr & 0xf) == 0xd) {
+				/* Port received 0xd preamble */
+				work->packet_ptr.s.addr += i;
+				work->word1.len -= i + 4;
+				for (i = 0; i < work->word1.len; i++) {
+					*ptr =
+					    ((*ptr & 0xf0) >> 4) |
+					    ((*(ptr + 1) & 0xf) << 4);
+					ptr++;
+				}
+				return 0;
+			}
+
+			printk_ratelimited("Port %d unknown preamble, packet dropped\n",
+					   port);
+			cvm_oct_free_work(work);
+			return 1;
+		}
+	}
+
+	printk_ratelimited("Port %d receive error code %d, packet dropped\n",
+			   port, work->word2.snoip.err_code);
+	cvm_oct_free_work(work);
+	return 1;
+}
+
+static void copy_segments_to_skb(struct cvmx_wqe *work, struct sk_buff *skb)
+{
+	int segments = work->word2.s.bufs;
+	union cvmx_buf_ptr segment_ptr = work->packet_ptr;
+	int len = work->word1.len;
+	int segment_size;
+
+	while (segments--) {
+		union cvmx_buf_ptr next_ptr;
+
+		next_ptr = *(union cvmx_buf_ptr *)
+			cvmx_phys_to_ptr(segment_ptr.s.addr - 8);
+
+		/*
+		 * Octeon Errata PKI-100: The segment size is wrong.
+		 *
+		 * Until it is fixed, calculate the segment size based on
+		 * the packet pool buffer size.
+		 * When it is fixed, the following line should be replaced
+		 * with this one:
+		 * int segment_size = segment_ptr.s.size;
+		 */
+		segment_size =
+			CVMX_FPA_PACKET_POOL_SIZE -
+			(segment_ptr.s.addr -
+			 (((segment_ptr.s.addr >> 7) -
+			   segment_ptr.s.back) << 7));
+
+		/* Don't copy more than what is left in the packet */
+		if (segment_size > len)
+			segment_size = len;
+
+		/* Copy the data into the packet */
+		skb_put_data(skb, cvmx_phys_to_ptr(segment_ptr.s.addr),
+			     segment_size);
+		len -= segment_size;
+		segment_ptr = next_ptr;
+	}
+}
+
+static int cvm_oct_poll(struct oct_rx_group *rx_group, int budget)
+{
+	const int	coreid = cvmx_get_core_num();
+	u64	old_group_mask;
+	u64	old_scratch;
+	int		rx_count = 0;
+	int		did_work_request = 0;
+	int		packet_not_copied;
+
+	/* Prefetch cvm_oct_device since we know we need it soon */
+	prefetch(cvm_oct_device);
+
+	if (USE_ASYNC_IOBDMA) {
+		/* Save scratch in case userspace is using it */
+		CVMX_SYNCIOBDMA;
+		old_scratch = cvmx_scratch_read64(CVMX_SCR_SCRATCH);
+	}
+
+	/* Only allow work for our group (and preserve priorities) */
+	if (OCTEON_IS_MODEL(OCTEON_CN68XX)) {
+		old_group_mask = cvmx_read_csr(CVMX_SSO_PPX_GRP_MSK(coreid));
+		cvmx_write_csr(CVMX_SSO_PPX_GRP_MSK(coreid),
+			       BIT(rx_group->group));
+		cvmx_read_csr(CVMX_SSO_PPX_GRP_MSK(coreid)); /* Flush */
+	} else {
+		old_group_mask = cvmx_read_csr(CVMX_POW_PP_GRP_MSKX(coreid));
+		cvmx_write_csr(CVMX_POW_PP_GRP_MSKX(coreid),
+			       (old_group_mask & ~0xFFFFull) |
+			       BIT(rx_group->group));
+	}
+
+	if (USE_ASYNC_IOBDMA) {
+		cvmx_pow_work_request_async(CVMX_SCR_SCRATCH, CVMX_POW_NO_WAIT);
+		did_work_request = 1;
+	}
+
+	while (rx_count < budget) {
+		struct sk_buff *skb = NULL;
+		struct sk_buff **pskb = NULL;
+		int skb_in_hw;
+		struct cvmx_wqe *work;
+		int port;
+
+		if (USE_ASYNC_IOBDMA && did_work_request)
+			work = cvmx_pow_work_response_async(CVMX_SCR_SCRATCH);
+		else
+			work = cvmx_pow_work_request_sync(CVMX_POW_NO_WAIT);
+
+		prefetch(work);
+		did_work_request = 0;
+		if (!work) {
+			if (OCTEON_IS_MODEL(OCTEON_CN68XX)) {
+				cvmx_write_csr(CVMX_SSO_WQ_IQ_DIS,
+					       BIT(rx_group->group));
+				cvmx_write_csr(CVMX_SSO_WQ_INT,
+					       BIT(rx_group->group));
+			} else {
+				union cvmx_pow_wq_int wq_int;
+
+				wq_int.u64 = 0;
+				wq_int.s.iq_dis = BIT(rx_group->group);
+				wq_int.s.wq_int = BIT(rx_group->group);
+				cvmx_write_csr(CVMX_POW_WQ_INT, wq_int.u64);
+			}
+			break;
+		}
+		pskb = (struct sk_buff **)
+			(cvm_oct_get_buffer_ptr(work->packet_ptr) -
+			sizeof(void *));
+		prefetch(pskb);
+
+		if (USE_ASYNC_IOBDMA && rx_count < (budget - 1)) {
+			cvmx_pow_work_request_async_nocheck(CVMX_SCR_SCRATCH,
+							    CVMX_POW_NO_WAIT);
+			did_work_request = 1;
+		}
+		rx_count++;
+
+		skb_in_hw = work->word2.s.bufs == 1;
+		if (likely(skb_in_hw)) {
+			skb = *pskb;
+			prefetch(&skb->head);
+			prefetch(&skb->len);
+		}
+
+		if (octeon_has_feature(OCTEON_FEATURE_PKND))
+			port = work->word0.pip.cn68xx.pknd;
+		else
+			port = work->word1.cn38xx.ipprt;
+
+		prefetch(cvm_oct_device[port]);
+
+		/* Immediately throw away all packets with receive errors */
+		if (unlikely(work->word2.snoip.rcv_error)) {
+			if (cvm_oct_check_rcv_error(work))
+				continue;
+		}
+
+		/*
+		 * We can only use the zero copy path if skbuffs are
+		 * in the FPA pool and the packet fits in a single
+		 * buffer.
+		 */
+		if (likely(skb_in_hw)) {
+			skb->data = skb->head + work->packet_ptr.s.addr -
+				cvmx_ptr_to_phys(skb->head);
+			prefetch(skb->data);
+			skb->len = work->word1.len;
+			skb_set_tail_pointer(skb, skb->len);
+			packet_not_copied = 1;
+		} else {
+			/*
+			 * We have to copy the packet. First allocate
+			 * an skbuff for it.
+			 */
+			skb = dev_alloc_skb(work->word1.len);
+			if (!skb) {
+				cvm_oct_free_work(work);
+				continue;
+			}
+
+			/*
+			 * Check if we've received a packet that was
+			 * entirely stored in the work entry.
+			 */
+			if (unlikely(work->word2.s.bufs == 0)) {
+				u8 *ptr = work->packet_data;
+
+				if (likely(!work->word2.s.not_IP)) {
+					/*
+					 * The beginning of the packet
+					 * moves for IP packets.
+					 */
+					if (work->word2.s.is_v6)
+						ptr += 2;
+					else
+						ptr += 6;
+				}
+				skb_put_data(skb, ptr, work->word1.len);
+				/* No packet buffers to free */
+			} else {
+				copy_segments_to_skb(work, skb);
+			}
+			packet_not_copied = 0;
+		}
+		if (likely((port < TOTAL_NUMBER_OF_PORTS) &&
+			   cvm_oct_device[port])) {
+			struct net_device *dev = cvm_oct_device[port];
+
+			/*
+			 * Only accept packets for devices that are
+			 * currently up.
+			 */
+			if (likely(dev->flags & IFF_UP)) {
+				skb->protocol = eth_type_trans(skb, dev);
+				skb->dev = dev;
+
+				if (unlikely(work->word2.s.not_IP ||
+					     work->word2.s.IP_exc ||
+					     work->word2.s.L4_error ||
+					     !work->word2.s.tcp_or_udp))
+					skb->ip_summed = CHECKSUM_NONE;
+				else
+					skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+				/* Increment RX stats for virtual ports */
+				if (port >= CVMX_PIP_NUM_INPUT_PORTS) {
+					dev->stats.rx_packets++;
+					dev->stats.rx_bytes += skb->len;
+				}
+				netif_receive_skb(skb);
+			} else {
+				/*
+				 * Drop any packet received for a device that
+				 * isn't up.
+				 */
+				dev->stats.rx_dropped++;
+				dev_kfree_skb_irq(skb);
+			}
+		} else {
+			/*
+			 * Drop any packet received for a device that
+			 * doesn't exist.
+			 */
+			printk_ratelimited("Port %d not controlled by Linux, packet dropped\n",
+					   port);
+			dev_kfree_skb_irq(skb);
+		}
+		/*
+		 * Check to see if the skbuff and work share the same
+		 * packet buffer.
+		 */
+		if (likely(packet_not_copied)) {
+			/*
+			 * This buffer needs to be replaced, increment
+			 * the number of buffers we need to free by
+			 * one.
+			 */
+			cvmx_fau_atomic_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE,
+					      1);
+
+			cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
+		} else {
+			cvm_oct_free_work(work);
+		}
+	}
+	/* Restore the original POW group mask */
+	if (OCTEON_IS_MODEL(OCTEON_CN68XX)) {
+		cvmx_write_csr(CVMX_SSO_PPX_GRP_MSK(coreid), old_group_mask);
+		cvmx_read_csr(CVMX_SSO_PPX_GRP_MSK(coreid)); /* Flush */
+	} else {
+		cvmx_write_csr(CVMX_POW_PP_GRP_MSKX(coreid), old_group_mask);
+	}
+
+	if (USE_ASYNC_IOBDMA) {
+		/* Restore the scratch area */
+		cvmx_scratch_write64(CVMX_SCR_SCRATCH, old_scratch);
+	}
+	cvm_oct_rx_refill_pool(0);
+
+	return rx_count;
+}
+
+/**
+ * cvm_oct_napi_poll - the NAPI poll function.
+ * @napi: The NAPI instance.
+ * @budget: Maximum number of packets to receive.
+ *
+ * Returns the number of packets processed.
+ */
+static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
+{
+	struct oct_rx_group *rx_group = container_of(napi, struct oct_rx_group,
+						     napi);
+	int rx_count;
+
+	rx_count = cvm_oct_poll(rx_group, budget);
+
+	if (rx_count < budget) {
+		/* No more work */
+		napi_complete_done(napi, rx_count);
+		enable_irq(rx_group->irq);
+	}
+	return rx_count;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+/**
+ * cvm_oct_poll_controller - poll for receive packets
+ * device.
+ *
+ * @dev:    Device to poll. Unused
+ */
+void cvm_oct_poll_controller(struct net_device *dev)
+{
+	int i;
+
+	if (!atomic_read(&oct_rx_ready))
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(oct_rx_group); i++) {
+		if (!(pow_receive_groups & BIT(i)))
+			continue;
+
+		cvm_oct_poll(&oct_rx_group[i], 16);
+	}
+}
+#endif
+
+void cvm_oct_rx_initialize(void)
+{
+	int i;
+	struct net_device *dev_for_napi = NULL;
+
+	for (i = 0; i < TOTAL_NUMBER_OF_PORTS; i++) {
+		if (cvm_oct_device[i]) {
+			dev_for_napi = cvm_oct_device[i];
+			break;
+		}
+	}
+
+	if (!dev_for_napi)
+		panic("No net_devices were allocated.");
+
+	for (i = 0; i < ARRAY_SIZE(oct_rx_group); i++) {
+		int ret;
+
+		if (!(pow_receive_groups & BIT(i)))
+			continue;
+
+		netif_napi_add_weight(dev_for_napi, &oct_rx_group[i].napi,
+				      cvm_oct_napi_poll, rx_napi_weight);
+		napi_enable(&oct_rx_group[i].napi);
+
+		oct_rx_group[i].irq = OCTEON_IRQ_WORKQ0 + i;
+		oct_rx_group[i].group = i;
+
+		/* Register an IRQ handler to receive POW interrupts */
+		ret = request_irq(oct_rx_group[i].irq, cvm_oct_do_interrupt, 0,
+				  "Ethernet", &oct_rx_group[i].napi);
+		if (ret)
+			panic("Could not acquire Ethernet IRQ %d\n",
+			      oct_rx_group[i].irq);
+
+		disable_irq_nosync(oct_rx_group[i].irq);
+
+		/* Enable POW interrupt when our port has at least one packet */
+		if (OCTEON_IS_MODEL(OCTEON_CN68XX)) {
+			union cvmx_sso_wq_int_thrx int_thr;
+			union cvmx_pow_wq_int_pc int_pc;
+
+			int_thr.u64 = 0;
+			int_thr.s.tc_en = 1;
+			int_thr.s.tc_thr = 1;
+			cvmx_write_csr(CVMX_SSO_WQ_INT_THRX(i), int_thr.u64);
+
+			int_pc.u64 = 0;
+			int_pc.s.pc_thr = 5;
+			cvmx_write_csr(CVMX_SSO_WQ_INT_PC, int_pc.u64);
+		} else {
+			union cvmx_pow_wq_int_thrx int_thr;
+			union cvmx_pow_wq_int_pc int_pc;
+
+			int_thr.u64 = 0;
+			int_thr.s.tc_en = 1;
+			int_thr.s.tc_thr = 1;
+			cvmx_write_csr(CVMX_POW_WQ_INT_THRX(i), int_thr.u64);
+
+			int_pc.u64 = 0;
+			int_pc.s.pc_thr = 5;
+			cvmx_write_csr(CVMX_POW_WQ_INT_PC, int_pc.u64);
+		}
+
+		/* Schedule NAPI now. This will indirectly enable the
+		 * interrupt.
+		 */
+		napi_schedule(&oct_rx_group[i].napi);
+	}
+	atomic_inc(&oct_rx_ready);
+}
+
+void cvm_oct_rx_shutdown(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(oct_rx_group); i++) {
+		if (!(pow_receive_groups & BIT(i)))
+			continue;
+
+		/* Disable POW interrupt */
+		if (OCTEON_IS_MODEL(OCTEON_CN68XX))
+			cvmx_write_csr(CVMX_SSO_WQ_INT_THRX(i), 0);
+		else
+			cvmx_write_csr(CVMX_POW_WQ_INT_THRX(i), 0);
+
+		/* Free the interrupt handler */
+		free_irq(oct_rx_group[i].irq, cvm_oct_device);
+
+		netif_napi_del(&oct_rx_group[i].napi);
+	}
+}
diff --git a/drivers/staging/octeon/ethernet-rx.h b/drivers/staging/octeon/ethernet-rx.h
new file mode 100644
index 000000000..ff6482fa2
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-rx.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+void cvm_oct_poll_controller(struct net_device *dev);
+void cvm_oct_rx_initialize(void);
+void cvm_oct_rx_shutdown(void);
+
+static inline void cvm_oct_rx_refill_pool(int fill_threshold)
+{
+	int number_to_free;
+	int num_freed;
+	/* Refill the packet buffer pool */
+	number_to_free =
+		cvmx_fau_fetch_and_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE, 0);
+
+	if (number_to_free > fill_threshold) {
+		cvmx_fau_atomic_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE,
+				      -number_to_free);
+		num_freed = cvm_oct_mem_fill_fpa(CVMX_FPA_PACKET_POOL,
+						 CVMX_FPA_PACKET_POOL_SIZE,
+						 number_to_free);
+		if (num_freed != number_to_free) {
+			cvmx_fau_atomic_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE,
+					      number_to_free - num_freed);
+		}
+	}
+}
diff --git a/drivers/staging/octeon/ethernet-sgmii.c b/drivers/staging/octeon/ethernet-sgmii.c
new file mode 100644
index 000000000..d7fbd9159
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-sgmii.c
@@ -0,0 +1,30 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/phy.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/ratelimit.h>
+#include <net/dst.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-util.h"
+#include "ethernet-mdio.h"
+
+int cvm_oct_sgmii_open(struct net_device *dev)
+{
+	return cvm_oct_common_open(dev, cvm_oct_link_poll);
+}
+
+int cvm_oct_sgmii_init(struct net_device *dev)
+{
+	cvm_oct_common_init(dev);
+
+	/* FIXME: Need autoneg logic */
+	return 0;
+}
diff --git a/drivers/staging/octeon/ethernet-spi.c b/drivers/staging/octeon/ethernet-spi.c
new file mode 100644
index 000000000..699c98c5e
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-spi.c
@@ -0,0 +1,226 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/interrupt.h>
+#include <net/dst.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-util.h"
+
+static int number_spi_ports;
+static int need_retrain[2] = { 0, 0 };
+
+static void cvm_oct_spxx_int_pr(union cvmx_spxx_int_reg spx_int_reg, int index)
+{
+	if (spx_int_reg.s.spf)
+		pr_err("SPI%d: SRX Spi4 interface down\n", index);
+	if (spx_int_reg.s.calerr)
+		pr_err("SPI%d: SRX Spi4 Calendar table parity error\n", index);
+	if (spx_int_reg.s.syncerr)
+		pr_err("SPI%d: SRX Consecutive Spi4 DIP4 errors have exceeded SPX_ERR_CTL[ERRCNT]\n",
+		       index);
+	if (spx_int_reg.s.diperr)
+		pr_err("SPI%d: SRX Spi4 DIP4 error\n", index);
+	if (spx_int_reg.s.tpaovr)
+		pr_err("SPI%d: SRX Selected port has hit TPA overflow\n",
+		       index);
+	if (spx_int_reg.s.rsverr)
+		pr_err("SPI%d: SRX Spi4 reserved control word detected\n",
+		       index);
+	if (spx_int_reg.s.drwnng)
+		pr_err("SPI%d: SRX Spi4 receive FIFO drowning/overflow\n",
+		       index);
+	if (spx_int_reg.s.clserr)
+		pr_err("SPI%d: SRX Spi4 packet closed on non-16B alignment without EOP\n",
+		       index);
+	if (spx_int_reg.s.spiovr)
+		pr_err("SPI%d: SRX Spi4 async FIFO overflow\n", index);
+	if (spx_int_reg.s.abnorm)
+		pr_err("SPI%d: SRX Abnormal packet termination (ERR bit)\n",
+		       index);
+	if (spx_int_reg.s.prtnxa)
+		pr_err("SPI%d: SRX Port out of range\n", index);
+}
+
+static void cvm_oct_stxx_int_pr(union cvmx_stxx_int_reg stx_int_reg, int index)
+{
+	if (stx_int_reg.s.syncerr)
+		pr_err("SPI%d: STX Interface encountered a fatal error\n",
+		       index);
+	if (stx_int_reg.s.frmerr)
+		pr_err("SPI%d: STX FRMCNT has exceeded STX_DIP_CNT[MAXFRM]\n",
+		       index);
+	if (stx_int_reg.s.unxfrm)
+		pr_err("SPI%d: STX Unexpected framing sequence\n", index);
+	if (stx_int_reg.s.nosync)
+		pr_err("SPI%d: STX ERRCNT has exceeded STX_DIP_CNT[MAXDIP]\n",
+		       index);
+	if (stx_int_reg.s.diperr)
+		pr_err("SPI%d: STX DIP2 error on the Spi4 Status channel\n",
+		       index);
+	if (stx_int_reg.s.datovr)
+		pr_err("SPI%d: STX Spi4 FIFO overflow error\n", index);
+	if (stx_int_reg.s.ovrbst)
+		pr_err("SPI%d: STX Transmit packet burst too big\n", index);
+	if (stx_int_reg.s.calpar1)
+		pr_err("SPI%d: STX Calendar Table Parity Error Bank%d\n",
+		       index, 1);
+	if (stx_int_reg.s.calpar0)
+		pr_err("SPI%d: STX Calendar Table Parity Error Bank%d\n",
+		       index, 0);
+}
+
+static irqreturn_t cvm_oct_spi_spx_int(int index)
+{
+	union cvmx_spxx_int_reg spx_int_reg;
+	union cvmx_stxx_int_reg stx_int_reg;
+
+	spx_int_reg.u64 = cvmx_read_csr(CVMX_SPXX_INT_REG(index));
+	cvmx_write_csr(CVMX_SPXX_INT_REG(index), spx_int_reg.u64);
+	if (!need_retrain[index]) {
+		spx_int_reg.u64 &= cvmx_read_csr(CVMX_SPXX_INT_MSK(index));
+		cvm_oct_spxx_int_pr(spx_int_reg, index);
+	}
+
+	stx_int_reg.u64 = cvmx_read_csr(CVMX_STXX_INT_REG(index));
+	cvmx_write_csr(CVMX_STXX_INT_REG(index), stx_int_reg.u64);
+	if (!need_retrain[index]) {
+		stx_int_reg.u64 &= cvmx_read_csr(CVMX_STXX_INT_MSK(index));
+		cvm_oct_stxx_int_pr(stx_int_reg, index);
+	}
+
+	cvmx_write_csr(CVMX_SPXX_INT_MSK(index), 0);
+	cvmx_write_csr(CVMX_STXX_INT_MSK(index), 0);
+	need_retrain[index] = 1;
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cvm_oct_spi_rml_interrupt(int cpl, void *dev_id)
+{
+	irqreturn_t return_status = IRQ_NONE;
+	union cvmx_npi_rsl_int_blocks rsl_int_blocks;
+
+	/* Check and see if this interrupt was caused by the GMX block */
+	rsl_int_blocks.u64 = cvmx_read_csr(CVMX_NPI_RSL_INT_BLOCKS);
+	if (rsl_int_blocks.s.spx1) /* 19 - SPX1_INT_REG & STX1_INT_REG */
+		return_status = cvm_oct_spi_spx_int(1);
+
+	if (rsl_int_blocks.s.spx0) /* 18 - SPX0_INT_REG & STX0_INT_REG */
+		return_status = cvm_oct_spi_spx_int(0);
+
+	return return_status;
+}
+
+static void cvm_oct_spi_enable_error_reporting(int interface)
+{
+	union cvmx_spxx_int_msk spxx_int_msk;
+	union cvmx_stxx_int_msk stxx_int_msk;
+
+	spxx_int_msk.u64 = cvmx_read_csr(CVMX_SPXX_INT_MSK(interface));
+	spxx_int_msk.s.calerr = 1;
+	spxx_int_msk.s.syncerr = 1;
+	spxx_int_msk.s.diperr = 1;
+	spxx_int_msk.s.tpaovr = 1;
+	spxx_int_msk.s.rsverr = 1;
+	spxx_int_msk.s.drwnng = 1;
+	spxx_int_msk.s.clserr = 1;
+	spxx_int_msk.s.spiovr = 1;
+	spxx_int_msk.s.abnorm = 1;
+	spxx_int_msk.s.prtnxa = 1;
+	cvmx_write_csr(CVMX_SPXX_INT_MSK(interface), spxx_int_msk.u64);
+
+	stxx_int_msk.u64 = cvmx_read_csr(CVMX_STXX_INT_MSK(interface));
+	stxx_int_msk.s.frmerr = 1;
+	stxx_int_msk.s.unxfrm = 1;
+	stxx_int_msk.s.nosync = 1;
+	stxx_int_msk.s.diperr = 1;
+	stxx_int_msk.s.datovr = 1;
+	stxx_int_msk.s.ovrbst = 1;
+	stxx_int_msk.s.calpar1 = 1;
+	stxx_int_msk.s.calpar0 = 1;
+	cvmx_write_csr(CVMX_STXX_INT_MSK(interface), stxx_int_msk.u64);
+}
+
+static void cvm_oct_spi_poll(struct net_device *dev)
+{
+	static int spi4000_port;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface;
+
+	for (interface = 0; interface < 2; interface++) {
+		if ((priv->port == interface * 16) && need_retrain[interface]) {
+			if (cvmx_spi_restart_interface
+			    (interface, CVMX_SPI_MODE_DUPLEX, 10) == 0) {
+				need_retrain[interface] = 0;
+				cvm_oct_spi_enable_error_reporting(interface);
+			}
+		}
+
+		/*
+		 * The SPI4000 TWSI interface is very slow. In order
+		 * not to bring the system to a crawl, we only poll a
+		 * single port every second. This means negotiation
+		 * speed changes take up to 10 seconds, but at least
+		 * we don't waste absurd amounts of time waiting for
+		 * TWSI.
+		 */
+		if (priv->port == spi4000_port) {
+			/*
+			 * This function does nothing if it is called on an
+			 * interface without a SPI4000.
+			 */
+			cvmx_spi4000_check_speed(interface, priv->port);
+			/*
+			 * Normal ordering increments. By decrementing
+			 * we only match once per iteration.
+			 */
+			spi4000_port--;
+			if (spi4000_port < 0)
+				spi4000_port = 10;
+		}
+	}
+}
+
+int cvm_oct_spi_init(struct net_device *dev)
+{
+	int r;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+
+	if (number_spi_ports == 0) {
+		r = request_irq(OCTEON_IRQ_RML, cvm_oct_spi_rml_interrupt,
+				IRQF_SHARED, "SPI", &number_spi_ports);
+		if (r)
+			return r;
+	}
+	number_spi_ports++;
+
+	if (priv->port == 0 || priv->port == 16) {
+		cvm_oct_spi_enable_error_reporting(INTERFACE(priv->port));
+		priv->poll = cvm_oct_spi_poll;
+	}
+	cvm_oct_common_init(dev);
+	return 0;
+}
+
+void cvm_oct_spi_uninit(struct net_device *dev)
+{
+	int interface;
+
+	cvm_oct_common_uninit(dev);
+	number_spi_ports--;
+	if (number_spi_ports == 0) {
+		for (interface = 0; interface < 2; interface++) {
+			cvmx_write_csr(CVMX_SPXX_INT_MSK(interface), 0);
+			cvmx_write_csr(CVMX_STXX_INT_MSK(interface), 0);
+		}
+		free_irq(OCTEON_IRQ_RML, &number_spi_ports);
+	}
+}
diff --git a/drivers/staging/octeon/ethernet-tx.c b/drivers/staging/octeon/ethernet-tx.c
new file mode 100644
index 000000000..a36e36701
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-tx.c
@@ -0,0 +1,716 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2010 Cavium Networks
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/ratelimit.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <net/dst.h>
+#ifdef CONFIG_XFRM
+#include <linux/xfrm.h>
+#include <net/xfrm.h>
+#endif /* CONFIG_XFRM */
+
+#include <linux/atomic.h>
+#include <net/sch_generic.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-tx.h"
+#include "ethernet-util.h"
+
+#define CVM_OCT_SKB_CB(skb)	((u64 *)((skb)->cb))
+
+/*
+ * You can define GET_SKBUFF_QOS() to override how the skbuff output
+ * function determines which output queue is used. The default
+ * implementation always uses the base queue for the port. If, for
+ * example, you wanted to use the skb->priority field, define
+ * GET_SKBUFF_QOS as: #define GET_SKBUFF_QOS(skb) ((skb)->priority)
+ */
+#ifndef GET_SKBUFF_QOS
+#define GET_SKBUFF_QOS(skb) 0
+#endif
+
+static void cvm_oct_tx_do_cleanup(unsigned long arg);
+static DECLARE_TASKLET_OLD(cvm_oct_tx_cleanup_tasklet, cvm_oct_tx_do_cleanup);
+
+/* Maximum number of SKBs to try to free per xmit packet. */
+#define MAX_SKB_TO_FREE (MAX_OUT_QUEUE_DEPTH * 2)
+
+static inline int cvm_oct_adjust_skb_to_free(int skb_to_free, int fau)
+{
+	int undo;
+
+	undo = skb_to_free > 0 ? MAX_SKB_TO_FREE : skb_to_free +
+						   MAX_SKB_TO_FREE;
+	if (undo > 0)
+		cvmx_fau_atomic_add32(fau, -undo);
+	skb_to_free = -skb_to_free > MAX_SKB_TO_FREE ? MAX_SKB_TO_FREE :
+						       -skb_to_free;
+	return skb_to_free;
+}
+
+static void cvm_oct_kick_tx_poll_watchdog(void)
+{
+	union cvmx_ciu_timx ciu_timx;
+
+	ciu_timx.u64 = 0;
+	ciu_timx.s.one_shot = 1;
+	ciu_timx.s.len = cvm_oct_tx_poll_interval;
+	cvmx_write_csr(CVMX_CIU_TIMX(1), ciu_timx.u64);
+}
+
+static void cvm_oct_free_tx_skbs(struct net_device *dev)
+{
+	int skb_to_free;
+	int qos, queues_per_port;
+	int total_freed = 0;
+	int total_remaining = 0;
+	unsigned long flags;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+
+	queues_per_port = cvmx_pko_get_num_queues(priv->port);
+	/* Drain any pending packets in the free list */
+	for (qos = 0; qos < queues_per_port; qos++) {
+		if (skb_queue_len(&priv->tx_free_list[qos]) == 0)
+			continue;
+		skb_to_free = cvmx_fau_fetch_and_add32(priv->fau + qos * 4,
+						       MAX_SKB_TO_FREE);
+		skb_to_free = cvm_oct_adjust_skb_to_free(skb_to_free,
+							 priv->fau + qos * 4);
+		total_freed += skb_to_free;
+		if (skb_to_free > 0) {
+			struct sk_buff *to_free_list = NULL;
+
+			spin_lock_irqsave(&priv->tx_free_list[qos].lock, flags);
+			while (skb_to_free > 0) {
+				struct sk_buff *t;
+
+				t = __skb_dequeue(&priv->tx_free_list[qos]);
+				t->next = to_free_list;
+				to_free_list = t;
+				skb_to_free--;
+			}
+			spin_unlock_irqrestore(&priv->tx_free_list[qos].lock,
+					       flags);
+			/* Do the actual freeing outside of the lock. */
+			while (to_free_list) {
+				struct sk_buff *t = to_free_list;
+
+				to_free_list = to_free_list->next;
+				dev_kfree_skb_any(t);
+			}
+		}
+		total_remaining += skb_queue_len(&priv->tx_free_list[qos]);
+	}
+	if (total_remaining < MAX_OUT_QUEUE_DEPTH && netif_queue_stopped(dev))
+		netif_wake_queue(dev);
+	if (total_remaining)
+		cvm_oct_kick_tx_poll_watchdog();
+}
+
+/**
+ * cvm_oct_xmit - transmit a packet
+ * @skb:    Packet to send
+ * @dev:    Device info structure
+ *
+ * Returns Always returns NETDEV_TX_OK
+ */
+netdev_tx_t cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	union cvmx_pko_command_word0 pko_command;
+	union cvmx_buf_ptr hw_buffer;
+	u64 old_scratch;
+	u64 old_scratch2;
+	int qos;
+	int i;
+	enum {QUEUE_CORE, QUEUE_HW, QUEUE_DROP} queue_type;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	struct sk_buff *to_free_list;
+	int skb_to_free;
+	int buffers_to_free;
+	u32 total_to_clean;
+	unsigned long flags;
+#if REUSE_SKBUFFS_WITHOUT_FREE
+	unsigned char *fpa_head;
+#endif
+
+	/*
+	 * Prefetch the private data structure.  It is larger than the
+	 * one cache line.
+	 */
+	prefetch(priv);
+
+	/*
+	 * The check on CVMX_PKO_QUEUES_PER_PORT_* is designed to
+	 * completely remove "qos" in the event neither interface
+	 * supports multiple queues per port.
+	 */
+	if ((CVMX_PKO_QUEUES_PER_PORT_INTERFACE0 > 1) ||
+	    (CVMX_PKO_QUEUES_PER_PORT_INTERFACE1 > 1)) {
+		qos = GET_SKBUFF_QOS(skb);
+		if (qos <= 0)
+			qos = 0;
+		else if (qos >= cvmx_pko_get_num_queues(priv->port))
+			qos = 0;
+	} else {
+		qos = 0;
+	}
+
+	if (USE_ASYNC_IOBDMA) {
+		/* Save scratch in case userspace is using it */
+		CVMX_SYNCIOBDMA;
+		old_scratch = cvmx_scratch_read64(CVMX_SCR_SCRATCH);
+		old_scratch2 = cvmx_scratch_read64(CVMX_SCR_SCRATCH + 8);
+
+		/*
+		 * Fetch and increment the number of packets to be
+		 * freed.
+		 */
+		cvmx_fau_async_fetch_and_add32(CVMX_SCR_SCRATCH + 8,
+					       FAU_NUM_PACKET_BUFFERS_TO_FREE,
+					       0);
+		cvmx_fau_async_fetch_and_add32(CVMX_SCR_SCRATCH,
+					       priv->fau + qos * 4,
+					       MAX_SKB_TO_FREE);
+	}
+
+	/*
+	 * We have space for 6 segment pointers, If there will be more
+	 * than that, we must linearize.
+	 */
+	if (unlikely(skb_shinfo(skb)->nr_frags > 5)) {
+		if (unlikely(__skb_linearize(skb))) {
+			queue_type = QUEUE_DROP;
+			if (USE_ASYNC_IOBDMA) {
+				/*
+				 * Get the number of skbuffs in use
+				 * by the hardware
+				 */
+				CVMX_SYNCIOBDMA;
+				skb_to_free =
+					cvmx_scratch_read64(CVMX_SCR_SCRATCH);
+			} else {
+				/*
+				 * Get the number of skbuffs in use
+				 * by the hardware
+				 */
+				skb_to_free =
+				     cvmx_fau_fetch_and_add32(priv->fau +
+							      qos * 4,
+							      MAX_SKB_TO_FREE);
+			}
+			skb_to_free = cvm_oct_adjust_skb_to_free(skb_to_free,
+								 priv->fau +
+								 qos * 4);
+			spin_lock_irqsave(&priv->tx_free_list[qos].lock, flags);
+			goto skip_xmit;
+		}
+	}
+
+	/*
+	 * The CN3XXX series of parts has an errata (GMX-401) which
+	 * causes the GMX block to hang if a collision occurs towards
+	 * the end of a <68 byte packet. As a workaround for this, we
+	 * pad packets to be 68 bytes whenever we are in half duplex
+	 * mode. We don't handle the case of having a small packet but
+	 * no room to add the padding.  The kernel should always give
+	 * us at least a cache line
+	 */
+	if ((skb->len < 64) && OCTEON_IS_MODEL(OCTEON_CN3XXX)) {
+		union cvmx_gmxx_prtx_cfg gmx_prt_cfg;
+		int interface = INTERFACE(priv->port);
+		int index = INDEX(priv->port);
+
+		if (interface < 2) {
+			/* We only need to pad packet in half duplex mode */
+			gmx_prt_cfg.u64 =
+			    cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+			if (gmx_prt_cfg.s.duplex == 0) {
+				int add_bytes = 64 - skb->len;
+
+				if ((skb_tail_pointer(skb) + add_bytes) <=
+				    skb_end_pointer(skb))
+					__skb_put_zero(skb, add_bytes);
+			}
+		}
+	}
+
+	/* Build the PKO command */
+	pko_command.u64 = 0;
+#ifdef __LITTLE_ENDIAN
+	pko_command.s.le = 1;
+#endif
+	pko_command.s.n2 = 1;	/* Don't pollute L2 with the outgoing packet */
+	pko_command.s.segs = 1;
+	pko_command.s.total_bytes = skb->len;
+	pko_command.s.size0 = CVMX_FAU_OP_SIZE_32;
+	pko_command.s.subone0 = 1;
+
+	pko_command.s.dontfree = 1;
+
+	/* Build the PKO buffer pointer */
+	hw_buffer.u64 = 0;
+	if (skb_shinfo(skb)->nr_frags == 0) {
+		hw_buffer.s.addr = XKPHYS_TO_PHYS((uintptr_t)skb->data);
+		hw_buffer.s.pool = 0;
+		hw_buffer.s.size = skb->len;
+	} else {
+		hw_buffer.s.addr = XKPHYS_TO_PHYS((uintptr_t)skb->data);
+		hw_buffer.s.pool = 0;
+		hw_buffer.s.size = skb_headlen(skb);
+		CVM_OCT_SKB_CB(skb)[0] = hw_buffer.u64;
+		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+			skb_frag_t *fs = skb_shinfo(skb)->frags + i;
+
+			hw_buffer.s.addr =
+				XKPHYS_TO_PHYS((uintptr_t)skb_frag_address(fs));
+			hw_buffer.s.size = skb_frag_size(fs);
+			CVM_OCT_SKB_CB(skb)[i + 1] = hw_buffer.u64;
+		}
+		hw_buffer.s.addr =
+			XKPHYS_TO_PHYS((uintptr_t)CVM_OCT_SKB_CB(skb));
+		hw_buffer.s.size = skb_shinfo(skb)->nr_frags + 1;
+		pko_command.s.segs = skb_shinfo(skb)->nr_frags + 1;
+		pko_command.s.gather = 1;
+		goto dont_put_skbuff_in_hw;
+	}
+
+	/*
+	 * See if we can put this skb in the FPA pool. Any strange
+	 * behavior from the Linux networking stack will most likely
+	 * be caused by a bug in the following code. If some field is
+	 * in use by the network stack and gets carried over when a
+	 * buffer is reused, bad things may happen.  If in doubt and
+	 * you dont need the absolute best performance, disable the
+	 * define REUSE_SKBUFFS_WITHOUT_FREE. The reuse of buffers has
+	 * shown a 25% increase in performance under some loads.
+	 */
+#if REUSE_SKBUFFS_WITHOUT_FREE
+	fpa_head = skb->head + 256 - ((unsigned long)skb->head & 0x7f);
+	if (unlikely(skb->data < fpa_head)) {
+		/* TX buffer beginning can't meet FPA alignment constraints */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely
+	    ((skb_end_pointer(skb) - fpa_head) < CVMX_FPA_PACKET_POOL_SIZE)) {
+		/* TX buffer isn't large enough for the FPA */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely(skb_shared(skb))) {
+		/* TX buffer sharing data with someone else */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely(skb_cloned(skb))) {
+		/* TX buffer has been cloned */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely(skb_header_cloned(skb))) {
+		/* TX buffer header has been cloned */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely(skb->destructor)) {
+		/* TX buffer has a destructor */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely(skb_shinfo(skb)->nr_frags)) {
+		/* TX buffer has fragments */
+		goto dont_put_skbuff_in_hw;
+	}
+	if (unlikely
+	    (skb->truesize !=
+	     sizeof(*skb) + skb_end_offset(skb))) {
+		/* TX buffer truesize has been changed */
+		goto dont_put_skbuff_in_hw;
+	}
+
+	/*
+	 * We can use this buffer in the FPA.  We don't need the FAU
+	 * update anymore
+	 */
+	pko_command.s.dontfree = 0;
+
+	hw_buffer.s.back = ((unsigned long)skb->data >> 7) -
+			   ((unsigned long)fpa_head >> 7);
+
+	*(struct sk_buff **)(fpa_head - sizeof(void *)) = skb;
+
+	/*
+	 * The skbuff will be reused without ever being freed. We must
+	 * cleanup a bunch of core things.
+	 */
+	dst_release(skb_dst(skb));
+	skb_dst_set(skb, NULL);
+	skb_ext_reset(skb);
+	nf_reset_ct(skb);
+	skb_reset_redirect(skb);
+
+#ifdef CONFIG_NET_SCHED
+	skb->tc_index = 0;
+#endif /* CONFIG_NET_SCHED */
+#endif /* REUSE_SKBUFFS_WITHOUT_FREE */
+
+dont_put_skbuff_in_hw:
+
+	/* Check if we can use the hardware checksumming */
+	if ((skb->protocol == htons(ETH_P_IP)) &&
+	    (ip_hdr(skb)->version == 4) &&
+	    (ip_hdr(skb)->ihl == 5) &&
+	    ((ip_hdr(skb)->frag_off == 0) ||
+	     (ip_hdr(skb)->frag_off == htons(1 << 14))) &&
+	    ((ip_hdr(skb)->protocol == IPPROTO_TCP) ||
+	     (ip_hdr(skb)->protocol == IPPROTO_UDP))) {
+		/* Use hardware checksum calc */
+		pko_command.s.ipoffp1 = skb_network_offset(skb) + 1;
+	}
+
+	if (USE_ASYNC_IOBDMA) {
+		/* Get the number of skbuffs in use by the hardware */
+		CVMX_SYNCIOBDMA;
+		skb_to_free = cvmx_scratch_read64(CVMX_SCR_SCRATCH);
+		buffers_to_free = cvmx_scratch_read64(CVMX_SCR_SCRATCH + 8);
+	} else {
+		/* Get the number of skbuffs in use by the hardware */
+		skb_to_free = cvmx_fau_fetch_and_add32(priv->fau + qos * 4,
+						       MAX_SKB_TO_FREE);
+		buffers_to_free =
+		    cvmx_fau_fetch_and_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE, 0);
+	}
+
+	skb_to_free = cvm_oct_adjust_skb_to_free(skb_to_free,
+						 priv->fau + qos * 4);
+
+	/*
+	 * If we're sending faster than the receive can free them then
+	 * don't do the HW free.
+	 */
+	if ((buffers_to_free < -100) && !pko_command.s.dontfree)
+		pko_command.s.dontfree = 1;
+
+	if (pko_command.s.dontfree) {
+		queue_type = QUEUE_CORE;
+		pko_command.s.reg0 = priv->fau + qos * 4;
+	} else {
+		queue_type = QUEUE_HW;
+	}
+	if (USE_ASYNC_IOBDMA)
+		cvmx_fau_async_fetch_and_add32(CVMX_SCR_SCRATCH,
+					       FAU_TOTAL_TX_TO_CLEAN, 1);
+
+	spin_lock_irqsave(&priv->tx_free_list[qos].lock, flags);
+
+	/* Drop this packet if we have too many already queued to the HW */
+	if (unlikely(skb_queue_len(&priv->tx_free_list[qos]) >=
+		     MAX_OUT_QUEUE_DEPTH)) {
+		if (dev->tx_queue_len != 0) {
+			/* Drop the lock when notifying the core.  */
+			spin_unlock_irqrestore(&priv->tx_free_list[qos].lock,
+					       flags);
+			netif_stop_queue(dev);
+			spin_lock_irqsave(&priv->tx_free_list[qos].lock,
+					  flags);
+		} else {
+			/* If not using normal queueing.  */
+			queue_type = QUEUE_DROP;
+			goto skip_xmit;
+		}
+	}
+
+	cvmx_pko_send_packet_prepare(priv->port, priv->queue + qos,
+				     CVMX_PKO_LOCK_NONE);
+
+	/* Send the packet to the output queue */
+	if (unlikely(cvmx_pko_send_packet_finish(priv->port,
+						 priv->queue + qos,
+						 pko_command, hw_buffer,
+						 CVMX_PKO_LOCK_NONE))) {
+		printk_ratelimited("%s: Failed to send the packet\n",
+				   dev->name);
+		queue_type = QUEUE_DROP;
+	}
+skip_xmit:
+	to_free_list = NULL;
+
+	switch (queue_type) {
+	case QUEUE_DROP:
+		skb->next = to_free_list;
+		to_free_list = skb;
+		dev->stats.tx_dropped++;
+		break;
+	case QUEUE_HW:
+		cvmx_fau_atomic_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE, -1);
+		break;
+	case QUEUE_CORE:
+		__skb_queue_tail(&priv->tx_free_list[qos], skb);
+		break;
+	default:
+		BUG();
+	}
+
+	while (skb_to_free > 0) {
+		struct sk_buff *t = __skb_dequeue(&priv->tx_free_list[qos]);
+
+		t->next = to_free_list;
+		to_free_list = t;
+		skb_to_free--;
+	}
+
+	spin_unlock_irqrestore(&priv->tx_free_list[qos].lock, flags);
+
+	/* Do the actual freeing outside of the lock. */
+	while (to_free_list) {
+		struct sk_buff *t = to_free_list;
+
+		to_free_list = to_free_list->next;
+		dev_kfree_skb_any(t);
+	}
+
+	if (USE_ASYNC_IOBDMA) {
+		CVMX_SYNCIOBDMA;
+		total_to_clean = cvmx_scratch_read64(CVMX_SCR_SCRATCH);
+		/* Restore the scratch area */
+		cvmx_scratch_write64(CVMX_SCR_SCRATCH, old_scratch);
+		cvmx_scratch_write64(CVMX_SCR_SCRATCH + 8, old_scratch2);
+	} else {
+		total_to_clean =
+			cvmx_fau_fetch_and_add32(FAU_TOTAL_TX_TO_CLEAN, 1);
+	}
+
+	if (total_to_clean & 0x3ff) {
+		/*
+		 * Schedule the cleanup tasklet every 1024 packets for
+		 * the pathological case of high traffic on one port
+		 * delaying clean up of packets on a different port
+		 * that is blocked waiting for the cleanup.
+		 */
+		tasklet_schedule(&cvm_oct_tx_cleanup_tasklet);
+	}
+
+	cvm_oct_kick_tx_poll_watchdog();
+
+	return NETDEV_TX_OK;
+}
+
+/**
+ * cvm_oct_xmit_pow - transmit a packet to the POW
+ * @skb:    Packet to send
+ * @dev:    Device info structure
+ * Returns Always returns zero
+ */
+netdev_tx_t cvm_oct_xmit_pow(struct sk_buff *skb, struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	void *packet_buffer;
+	void *copy_location;
+
+	/* Get a work queue entry */
+	struct cvmx_wqe *work = cvmx_fpa_alloc(CVMX_FPA_WQE_POOL);
+
+	if (unlikely(!work)) {
+		printk_ratelimited("%s: Failed to allocate a work queue entry\n",
+				   dev->name);
+		dev->stats.tx_dropped++;
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	/* Get a packet buffer */
+	packet_buffer = cvmx_fpa_alloc(CVMX_FPA_PACKET_POOL);
+	if (unlikely(!packet_buffer)) {
+		printk_ratelimited("%s: Failed to allocate a packet buffer\n",
+				   dev->name);
+		cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
+		dev->stats.tx_dropped++;
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	/*
+	 * Calculate where we need to copy the data to. We need to
+	 * leave 8 bytes for a next pointer (unused). We also need to
+	 * include any configure skip. Then we need to align the IP
+	 * packet src and dest into the same 64bit word. The below
+	 * calculation may add a little extra, but that doesn't
+	 * hurt.
+	 */
+	copy_location = packet_buffer + sizeof(u64);
+	copy_location += ((CVMX_HELPER_FIRST_MBUFF_SKIP + 7) & 0xfff8) + 6;
+
+	/*
+	 * We have to copy the packet since whoever processes this
+	 * packet will free it to a hardware pool. We can't use the
+	 * trick of counting outstanding packets like in
+	 * cvm_oct_xmit.
+	 */
+	memcpy(copy_location, skb->data, skb->len);
+
+	/*
+	 * Fill in some of the work queue fields. We may need to add
+	 * more if the software at the other end needs them.
+	 */
+	if (!OCTEON_IS_MODEL(OCTEON_CN68XX))
+		work->word0.pip.cn38xx.hw_chksum = skb->csum;
+	work->word1.len = skb->len;
+	cvmx_wqe_set_port(work, priv->port);
+	cvmx_wqe_set_qos(work, priv->port & 0x7);
+	cvmx_wqe_set_grp(work, pow_send_group);
+	work->word1.tag_type = CVMX_HELPER_INPUT_TAG_TYPE;
+	work->word1.tag = pow_send_group;	/* FIXME */
+	/* Default to zero. Sets of zero later are commented out */
+	work->word2.u64 = 0;
+	work->word2.s.bufs = 1;
+	work->packet_ptr.u64 = 0;
+	work->packet_ptr.s.addr = cvmx_ptr_to_phys(copy_location);
+	work->packet_ptr.s.pool = CVMX_FPA_PACKET_POOL;
+	work->packet_ptr.s.size = CVMX_FPA_PACKET_POOL_SIZE;
+	work->packet_ptr.s.back = (copy_location - packet_buffer) >> 7;
+
+	if (skb->protocol == htons(ETH_P_IP)) {
+		work->word2.s.ip_offset = 14;
+#if 0
+		work->word2.s.vlan_valid = 0;	/* FIXME */
+		work->word2.s.vlan_cfi = 0;	/* FIXME */
+		work->word2.s.vlan_id = 0;	/* FIXME */
+		work->word2.s.dec_ipcomp = 0;	/* FIXME */
+#endif
+		work->word2.s.tcp_or_udp =
+		    (ip_hdr(skb)->protocol == IPPROTO_TCP) ||
+		    (ip_hdr(skb)->protocol == IPPROTO_UDP);
+#if 0
+		/* FIXME */
+		work->word2.s.dec_ipsec = 0;
+		/* We only support IPv4 right now */
+		work->word2.s.is_v6 = 0;
+		/* Hardware would set to zero */
+		work->word2.s.software = 0;
+		/* No error, packet is internal */
+		work->word2.s.L4_error = 0;
+#endif
+		work->word2.s.is_frag = !((ip_hdr(skb)->frag_off == 0) ||
+					  (ip_hdr(skb)->frag_off ==
+					      cpu_to_be16(1 << 14)));
+#if 0
+		/* Assume Linux is sending a good packet */
+		work->word2.s.IP_exc = 0;
+#endif
+		work->word2.s.is_bcast = (skb->pkt_type == PACKET_BROADCAST);
+		work->word2.s.is_mcast = (skb->pkt_type == PACKET_MULTICAST);
+#if 0
+		/* This is an IP packet */
+		work->word2.s.not_IP = 0;
+		/* No error, packet is internal */
+		work->word2.s.rcv_error = 0;
+		/* No error, packet is internal */
+		work->word2.s.err_code = 0;
+#endif
+
+		/*
+		 * When copying the data, include 4 bytes of the
+		 * ethernet header to align the same way hardware
+		 * does.
+		 */
+		memcpy(work->packet_data, skb->data + 10,
+		       sizeof(work->packet_data));
+	} else {
+#if 0
+		work->word2.snoip.vlan_valid = 0;	/* FIXME */
+		work->word2.snoip.vlan_cfi = 0;	/* FIXME */
+		work->word2.snoip.vlan_id = 0;	/* FIXME */
+		work->word2.snoip.software = 0;	/* Hardware would set to zero */
+#endif
+		work->word2.snoip.is_rarp = skb->protocol == htons(ETH_P_RARP);
+		work->word2.snoip.is_arp = skb->protocol == htons(ETH_P_ARP);
+		work->word2.snoip.is_bcast =
+		    (skb->pkt_type == PACKET_BROADCAST);
+		work->word2.snoip.is_mcast =
+		    (skb->pkt_type == PACKET_MULTICAST);
+		work->word2.snoip.not_IP = 1;	/* IP was done up above */
+#if 0
+		/* No error, packet is internal */
+		work->word2.snoip.rcv_error = 0;
+		/* No error, packet is internal */
+		work->word2.snoip.err_code = 0;
+#endif
+		memcpy(work->packet_data, skb->data, sizeof(work->packet_data));
+	}
+
+	/* Submit the packet to the POW */
+	cvmx_pow_work_submit(work, work->word1.tag, work->word1.tag_type,
+			     cvmx_wqe_get_qos(work), cvmx_wqe_get_grp(work));
+	dev->stats.tx_packets++;
+	dev->stats.tx_bytes += skb->len;
+	dev_consume_skb_any(skb);
+	return 0;
+}
+
+/**
+ * cvm_oct_tx_shutdown_dev - free all skb that are currently queued for TX.
+ * @dev:    Device being shutdown
+ *
+ */
+void cvm_oct_tx_shutdown_dev(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	unsigned long flags;
+	int qos;
+
+	for (qos = 0; qos < 16; qos++) {
+		spin_lock_irqsave(&priv->tx_free_list[qos].lock, flags);
+		while (skb_queue_len(&priv->tx_free_list[qos]))
+			dev_kfree_skb_any(__skb_dequeue
+					  (&priv->tx_free_list[qos]));
+		spin_unlock_irqrestore(&priv->tx_free_list[qos].lock, flags);
+	}
+}
+
+static void cvm_oct_tx_do_cleanup(unsigned long arg)
+{
+	int port;
+
+	for (port = 0; port < TOTAL_NUMBER_OF_PORTS; port++) {
+		if (cvm_oct_device[port]) {
+			struct net_device *dev = cvm_oct_device[port];
+
+			cvm_oct_free_tx_skbs(dev);
+		}
+	}
+}
+
+static irqreturn_t cvm_oct_tx_cleanup_watchdog(int cpl, void *dev_id)
+{
+	/* Disable the interrupt.  */
+	cvmx_write_csr(CVMX_CIU_TIMX(1), 0);
+	/* Do the work in the tasklet.  */
+	tasklet_schedule(&cvm_oct_tx_cleanup_tasklet);
+	return IRQ_HANDLED;
+}
+
+void cvm_oct_tx_initialize(void)
+{
+	int i;
+
+	/* Disable the interrupt.  */
+	cvmx_write_csr(CVMX_CIU_TIMX(1), 0);
+	/* Register an IRQ handler to receive CIU_TIMX(1) interrupts */
+	i = request_irq(OCTEON_IRQ_TIMER1,
+			cvm_oct_tx_cleanup_watchdog, 0,
+			"Ethernet", cvm_oct_device);
+
+	if (i)
+		panic("Could not acquire Ethernet IRQ %d\n", OCTEON_IRQ_TIMER1);
+}
+
+void cvm_oct_tx_shutdown(void)
+{
+	/* Free the interrupt handler */
+	free_irq(OCTEON_IRQ_TIMER1, cvm_oct_device);
+}
diff --git a/drivers/staging/octeon/ethernet-tx.h b/drivers/staging/octeon/ethernet-tx.h
new file mode 100644
index 000000000..6c524668f
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-tx.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+netdev_tx_t cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev);
+netdev_tx_t cvm_oct_xmit_pow(struct sk_buff *skb, struct net_device *dev);
+int cvm_oct_transmit_qos(struct net_device *dev, void *work_queue_entry,
+			 int do_free, int qos);
+void cvm_oct_tx_initialize(void);
+void cvm_oct_tx_shutdown(void);
+void cvm_oct_tx_shutdown_dev(struct net_device *dev);
diff --git a/drivers/staging/octeon/ethernet-util.h b/drivers/staging/octeon/ethernet-util.h
new file mode 100644
index 000000000..2af83a12c
--- /dev/null
+++ b/drivers/staging/octeon/ethernet-util.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+/**
+ * cvm_oct_get_buffer_ptr - convert packet data address to pointer
+ * @packet_ptr: Packet data hardware address
+ *
+ * Returns Packet buffer pointer
+ */
+static inline void *cvm_oct_get_buffer_ptr(union cvmx_buf_ptr packet_ptr)
+{
+	return cvmx_phys_to_ptr(((packet_ptr.s.addr >> 7) - packet_ptr.s.back)
+				<< 7);
+}
+
+/**
+ * INTERFACE - convert IPD port to logical interface
+ * @ipd_port: Port to check
+ *
+ * Returns Logical interface
+ */
+static inline int INTERFACE(int ipd_port)
+{
+	int interface;
+
+	if (ipd_port == CVMX_PIP_NUM_INPUT_PORTS)
+		return 10;
+	interface = cvmx_helper_get_interface_num(ipd_port);
+	if (interface >= 0)
+		return interface;
+	panic("Illegal ipd_port %d passed to %s\n", ipd_port, __func__);
+}
+
+/**
+ * INDEX - convert IPD/PKO port number to the port's interface index
+ * @ipd_port: Port to check
+ *
+ * Returns Index into interface port list
+ */
+static inline int INDEX(int ipd_port)
+{
+	return cvmx_helper_get_interface_index_num(ipd_port);
+}
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
new file mode 100644
index 000000000..f66273913
--- /dev/null
+++ b/drivers/staging/octeon/ethernet.c
@@ -0,0 +1,993 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2007 Cavium Networks
+ */
+
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/phy.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/of_mdio.h>
+#include <linux/of_net.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+
+#include <net/dst.h>
+
+#include "octeon-ethernet.h"
+#include "ethernet-defines.h"
+#include "ethernet-mem.h"
+#include "ethernet-rx.h"
+#include "ethernet-tx.h"
+#include "ethernet-mdio.h"
+#include "ethernet-util.h"
+
+#define OCTEON_MAX_MTU 65392
+
+static int num_packet_buffers = 1024;
+module_param(num_packet_buffers, int, 0444);
+MODULE_PARM_DESC(num_packet_buffers, "\n"
+	"\tNumber of packet buffers to allocate and store in the\n"
+	"\tFPA. By default, 1024 packet buffers are used.\n");
+
+static int pow_receive_group = 15;
+module_param(pow_receive_group, int, 0444);
+MODULE_PARM_DESC(pow_receive_group, "\n"
+	"\tPOW group to receive packets from. All ethernet hardware\n"
+	"\twill be configured to send incoming packets to this POW\n"
+	"\tgroup. Also any other software can submit packets to this\n"
+	"\tgroup for the kernel to process.");
+
+static int receive_group_order;
+module_param(receive_group_order, int, 0444);
+MODULE_PARM_DESC(receive_group_order, "\n"
+	"\tOrder (0..4) of receive groups to take into use. Ethernet hardware\n"
+	"\twill be configured to send incoming packets to multiple POW\n"
+	"\tgroups. pow_receive_group parameter is ignored when multiple\n"
+	"\tgroups are taken into use and groups are allocated starting\n"
+	"\tfrom 0. By default, a single group is used.\n");
+
+int pow_send_group = -1;
+module_param(pow_send_group, int, 0644);
+MODULE_PARM_DESC(pow_send_group, "\n"
+	"\tPOW group to send packets to other software on. This\n"
+	"\tcontrols the creation of the virtual device pow0.\n"
+	"\talways_use_pow also depends on this value.");
+
+int always_use_pow;
+module_param(always_use_pow, int, 0444);
+MODULE_PARM_DESC(always_use_pow, "\n"
+	"\tWhen set, always send to the pow group. This will cause\n"
+	"\tpackets sent to real ethernet devices to be sent to the\n"
+	"\tPOW group instead of the hardware. Unless some other\n"
+	"\tapplication changes the config, packets will still be\n"
+	"\treceived from the low level hardware. Use this option\n"
+	"\tto allow a CVMX app to intercept all packets from the\n"
+	"\tlinux kernel. You must specify pow_send_group along with\n"
+	"\tthis option.");
+
+char pow_send_list[128] = "";
+module_param_string(pow_send_list, pow_send_list, sizeof(pow_send_list), 0444);
+MODULE_PARM_DESC(pow_send_list, "\n"
+	"\tComma separated list of ethernet devices that should use the\n"
+	"\tPOW for transmit instead of the actual ethernet hardware. This\n"
+	"\tis a per port version of always_use_pow. always_use_pow takes\n"
+	"\tprecedence over this list. For example, setting this to\n"
+	"\t\"eth2,spi3,spi7\" would cause these three devices to transmit\n"
+	"\tusing the pow_send_group.");
+
+int rx_napi_weight = 32;
+module_param(rx_napi_weight, int, 0444);
+MODULE_PARM_DESC(rx_napi_weight, "The NAPI WEIGHT parameter.");
+
+/* Mask indicating which receive groups are in use. */
+int pow_receive_groups;
+
+/*
+ * cvm_oct_poll_queue_stopping - flag to indicate polling should stop.
+ *
+ * Set to one right before cvm_oct_poll_queue is destroyed.
+ */
+atomic_t cvm_oct_poll_queue_stopping = ATOMIC_INIT(0);
+
+/*
+ * Array of every ethernet device owned by this driver indexed by
+ * the ipd input port number.
+ */
+struct net_device *cvm_oct_device[TOTAL_NUMBER_OF_PORTS];
+
+u64 cvm_oct_tx_poll_interval;
+
+static void cvm_oct_rx_refill_worker(struct work_struct *work);
+static DECLARE_DELAYED_WORK(cvm_oct_rx_refill_work, cvm_oct_rx_refill_worker);
+
+static void cvm_oct_rx_refill_worker(struct work_struct *work)
+{
+	/*
+	 * FPA 0 may have been drained, try to refill it if we need
+	 * more than num_packet_buffers / 2, otherwise normal receive
+	 * processing will refill it.  If it were drained, no packets
+	 * could be received so cvm_oct_napi_poll would never be
+	 * invoked to do the refill.
+	 */
+	cvm_oct_rx_refill_pool(num_packet_buffers / 2);
+
+	if (!atomic_read(&cvm_oct_poll_queue_stopping))
+		schedule_delayed_work(&cvm_oct_rx_refill_work, HZ);
+}
+
+static void cvm_oct_periodic_worker(struct work_struct *work)
+{
+	struct octeon_ethernet *priv = container_of(work,
+						    struct octeon_ethernet,
+						    port_periodic_work.work);
+
+	if (priv->poll)
+		priv->poll(cvm_oct_device[priv->port]);
+
+	cvm_oct_device[priv->port]->netdev_ops->ndo_get_stats
+						(cvm_oct_device[priv->port]);
+
+	if (!atomic_read(&cvm_oct_poll_queue_stopping))
+		schedule_delayed_work(&priv->port_periodic_work, HZ);
+}
+
+static void cvm_oct_configure_common_hw(void)
+{
+	/* Setup the FPA */
+	cvmx_fpa_enable();
+	cvm_oct_mem_fill_fpa(CVMX_FPA_PACKET_POOL, CVMX_FPA_PACKET_POOL_SIZE,
+			     num_packet_buffers);
+	cvm_oct_mem_fill_fpa(CVMX_FPA_WQE_POOL, CVMX_FPA_WQE_POOL_SIZE,
+			     num_packet_buffers);
+	if (CVMX_FPA_OUTPUT_BUFFER_POOL != CVMX_FPA_PACKET_POOL)
+		cvm_oct_mem_fill_fpa(CVMX_FPA_OUTPUT_BUFFER_POOL,
+				     CVMX_FPA_OUTPUT_BUFFER_POOL_SIZE, 1024);
+
+#ifdef __LITTLE_ENDIAN
+	{
+		union cvmx_ipd_ctl_status ipd_ctl_status;
+
+		ipd_ctl_status.u64 = cvmx_read_csr(CVMX_IPD_CTL_STATUS);
+		ipd_ctl_status.s.pkt_lend = 1;
+		ipd_ctl_status.s.wqe_lend = 1;
+		cvmx_write_csr(CVMX_IPD_CTL_STATUS, ipd_ctl_status.u64);
+	}
+#endif
+
+	cvmx_helper_setup_red(num_packet_buffers / 4, num_packet_buffers / 8);
+}
+
+/**
+ * cvm_oct_free_work- Free a work queue entry
+ *
+ * @work_queue_entry: Work queue entry to free
+ *
+ * Returns Zero on success, Negative on failure.
+ */
+int cvm_oct_free_work(void *work_queue_entry)
+{
+	struct cvmx_wqe *work = work_queue_entry;
+
+	int segments = work->word2.s.bufs;
+	union cvmx_buf_ptr segment_ptr = work->packet_ptr;
+
+	while (segments--) {
+		union cvmx_buf_ptr next_ptr = *(union cvmx_buf_ptr *)
+			cvmx_phys_to_ptr(segment_ptr.s.addr - 8);
+		if (unlikely(!segment_ptr.s.i))
+			cvmx_fpa_free(cvm_oct_get_buffer_ptr(segment_ptr),
+				      segment_ptr.s.pool,
+				      CVMX_FPA_PACKET_POOL_SIZE / 128);
+		segment_ptr = next_ptr;
+	}
+	cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
+
+	return 0;
+}
+EXPORT_SYMBOL(cvm_oct_free_work);
+
+/**
+ * cvm_oct_common_get_stats - get the low level ethernet statistics
+ * @dev:    Device to get the statistics from
+ *
+ * Returns Pointer to the statistics
+ */
+static struct net_device_stats *cvm_oct_common_get_stats(struct net_device *dev)
+{
+	cvmx_pip_port_status_t rx_status;
+	cvmx_pko_port_status_t tx_status;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+
+	if (priv->port < CVMX_PIP_NUM_INPUT_PORTS) {
+		if (octeon_is_simulation()) {
+			/* The simulator doesn't support statistics */
+			memset(&rx_status, 0, sizeof(rx_status));
+			memset(&tx_status, 0, sizeof(tx_status));
+		} else {
+			cvmx_pip_get_port_status(priv->port, 1, &rx_status);
+			cvmx_pko_get_port_status(priv->port, 1, &tx_status);
+		}
+
+		dev->stats.rx_packets += rx_status.inb_packets;
+		dev->stats.tx_packets += tx_status.packets;
+		dev->stats.rx_bytes += rx_status.inb_octets;
+		dev->stats.tx_bytes += tx_status.octets;
+		dev->stats.multicast += rx_status.multicast_packets;
+		dev->stats.rx_crc_errors += rx_status.inb_errors;
+		dev->stats.rx_frame_errors += rx_status.fcs_align_err_packets;
+		dev->stats.rx_dropped += rx_status.dropped_packets;
+	}
+
+	return &dev->stats;
+}
+
+/**
+ * cvm_oct_common_change_mtu - change the link MTU
+ * @dev:     Device to change
+ * @new_mtu: The new MTU
+ *
+ * Returns Zero on success
+ */
+static int cvm_oct_common_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
+	int vlan_bytes = VLAN_HLEN;
+#else
+	int vlan_bytes = 0;
+#endif
+	int mtu_overhead = ETH_HLEN + ETH_FCS_LEN + vlan_bytes;
+
+	dev->mtu = new_mtu;
+
+	if ((interface < 2) &&
+	    (cvmx_helper_interface_get_mode(interface) !=
+		CVMX_HELPER_INTERFACE_MODE_SPI)) {
+		int index = INDEX(priv->port);
+		/* Add ethernet header and FCS, and VLAN if configured. */
+		int max_packet = new_mtu + mtu_overhead;
+
+		if (OCTEON_IS_MODEL(OCTEON_CN3XXX) ||
+		    OCTEON_IS_MODEL(OCTEON_CN58XX)) {
+			/* Signal errors on packets larger than the MTU */
+			cvmx_write_csr(CVMX_GMXX_RXX_FRM_MAX(index, interface),
+				       max_packet);
+		} else {
+			/*
+			 * Set the hardware to truncate packets larger
+			 * than the MTU and smaller the 64 bytes.
+			 */
+			union cvmx_pip_frm_len_chkx frm_len_chk;
+
+			frm_len_chk.u64 = 0;
+			frm_len_chk.s.minlen = VLAN_ETH_ZLEN;
+			frm_len_chk.s.maxlen = max_packet;
+			cvmx_write_csr(CVMX_PIP_FRM_LEN_CHKX(interface),
+				       frm_len_chk.u64);
+		}
+		/*
+		 * Set the hardware to truncate packets larger than
+		 * the MTU. The jabber register must be set to a
+		 * multiple of 8 bytes, so round up.
+		 */
+		cvmx_write_csr(CVMX_GMXX_RXX_JABBER(index, interface),
+			       (max_packet + 7) & ~7u);
+	}
+	return 0;
+}
+
+/**
+ * cvm_oct_common_set_multicast_list - set the multicast list
+ * @dev:    Device to work on
+ */
+static void cvm_oct_common_set_multicast_list(struct net_device *dev)
+{
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
+
+	if ((interface < 2) &&
+	    (cvmx_helper_interface_get_mode(interface) !=
+		CVMX_HELPER_INTERFACE_MODE_SPI)) {
+		union cvmx_gmxx_rxx_adr_ctl control;
+		int index = INDEX(priv->port);
+
+		control.u64 = 0;
+		control.s.bcst = 1;	/* Allow broadcast MAC addresses */
+
+		if (!netdev_mc_empty(dev) || (dev->flags & IFF_ALLMULTI) ||
+		    (dev->flags & IFF_PROMISC))
+			/* Force accept multicast packets */
+			control.s.mcst = 2;
+		else
+			/* Force reject multicast packets */
+			control.s.mcst = 1;
+
+		if (dev->flags & IFF_PROMISC)
+			/*
+			 * Reject matches if promisc. Since CAM is
+			 * shut off, should accept everything.
+			 */
+			control.s.cam_mode = 0;
+		else
+			/* Filter packets based on the CAM */
+			control.s.cam_mode = 1;
+
+		gmx_cfg.u64 =
+		    cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+		cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface),
+			       gmx_cfg.u64 & ~1ull);
+
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CTL(index, interface),
+			       control.u64);
+		if (dev->flags & IFF_PROMISC)
+			cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM_EN
+				       (index, interface), 0);
+		else
+			cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM_EN
+				       (index, interface), 1);
+
+		cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface),
+			       gmx_cfg.u64);
+	}
+}
+
+static int cvm_oct_set_mac_filter(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	int interface = INTERFACE(priv->port);
+
+	if ((interface < 2) &&
+	    (cvmx_helper_interface_get_mode(interface) !=
+		CVMX_HELPER_INTERFACE_MODE_SPI)) {
+		int i;
+		const u8 *ptr = dev->dev_addr;
+		u64 mac = 0;
+		int index = INDEX(priv->port);
+
+		for (i = 0; i < 6; i++)
+			mac = (mac << 8) | (u64)ptr[i];
+
+		gmx_cfg.u64 =
+		    cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+		cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface),
+			       gmx_cfg.u64 & ~1ull);
+
+		cvmx_write_csr(CVMX_GMXX_SMACX(index, interface), mac);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM0(index, interface),
+			       ptr[0]);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM1(index, interface),
+			       ptr[1]);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM2(index, interface),
+			       ptr[2]);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM3(index, interface),
+			       ptr[3]);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM4(index, interface),
+			       ptr[4]);
+		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM5(index, interface),
+			       ptr[5]);
+		cvm_oct_common_set_multicast_list(dev);
+		cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface),
+			       gmx_cfg.u64);
+	}
+	return 0;
+}
+
+/**
+ * cvm_oct_common_set_mac_address - set the hardware MAC address for a device
+ * @dev:    The device in question.
+ * @addr:   Socket address.
+ *
+ * Returns Zero on success
+ */
+static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
+{
+	int r = eth_mac_addr(dev, addr);
+
+	if (r)
+		return r;
+	return cvm_oct_set_mac_filter(dev);
+}
+
+/**
+ * cvm_oct_common_init - per network device initialization
+ * @dev:    Device to initialize
+ *
+ * Returns Zero on success
+ */
+int cvm_oct_common_init(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int ret;
+
+	ret = of_get_ethdev_address(priv->of_node, dev);
+	if (ret)
+		eth_hw_addr_random(dev);
+
+	/*
+	 * Force the interface to use the POW send if always_use_pow
+	 * was specified or it is in the pow send list.
+	 */
+	if ((pow_send_group != -1) &&
+	    (always_use_pow || strstr(pow_send_list, dev->name)))
+		priv->queue = -1;
+
+	if (priv->queue != -1)
+		dev->features |= NETIF_F_SG | NETIF_F_IP_CSUM;
+
+	/* We do our own locking, Linux doesn't need to */
+	dev->features |= NETIF_F_LLTX;
+	dev->ethtool_ops = &cvm_oct_ethtool_ops;
+
+	cvm_oct_set_mac_filter(dev);
+	dev_set_mtu(dev, dev->mtu);
+
+	/*
+	 * Zero out stats for port so we won't mistakenly show
+	 * counters from the bootloader.
+	 */
+	memset(dev->netdev_ops->ndo_get_stats(dev), 0,
+	       sizeof(struct net_device_stats));
+
+	if (dev->netdev_ops->ndo_stop)
+		dev->netdev_ops->ndo_stop(dev);
+
+	return 0;
+}
+
+void cvm_oct_common_uninit(struct net_device *dev)
+{
+	if (dev->phydev)
+		phy_disconnect(dev->phydev);
+}
+
+int cvm_oct_common_open(struct net_device *dev,
+			void (*link_poll)(struct net_device *))
+{
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
+	int index = INDEX(priv->port);
+	union cvmx_helper_link_info link_info;
+	int rv;
+
+	rv = cvm_oct_phy_setup_device(dev);
+	if (rv)
+		return rv;
+
+	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+	gmx_cfg.s.en = 1;
+	if (octeon_has_feature(OCTEON_FEATURE_PKND))
+		gmx_cfg.s.pknd = priv->port;
+	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
+
+	if (octeon_is_simulation())
+		return 0;
+
+	if (dev->phydev) {
+		int r = phy_read_status(dev->phydev);
+
+		if (r == 0 && dev->phydev->link == 0)
+			netif_carrier_off(dev);
+		cvm_oct_adjust_link(dev);
+	} else {
+		link_info = cvmx_helper_link_get(priv->port);
+		if (!link_info.s.link_up)
+			netif_carrier_off(dev);
+		priv->poll = link_poll;
+		link_poll(dev);
+	}
+
+	return 0;
+}
+
+void cvm_oct_link_poll(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	union cvmx_helper_link_info link_info;
+
+	link_info = cvmx_helper_link_get(priv->port);
+	if (link_info.u64 == priv->link_info)
+		return;
+
+	if (cvmx_helper_link_set(priv->port, link_info))
+		link_info.u64 = priv->link_info;
+	else
+		priv->link_info = link_info.u64;
+
+	if (link_info.s.link_up) {
+		if (!netif_carrier_ok(dev))
+			netif_carrier_on(dev);
+	} else if (netif_carrier_ok(dev)) {
+		netif_carrier_off(dev);
+	}
+	cvm_oct_note_carrier(priv, link_info);
+}
+
+static int cvm_oct_xaui_open(struct net_device *dev)
+{
+	return cvm_oct_common_open(dev, cvm_oct_link_poll);
+}
+
+static const struct net_device_ops cvm_oct_npi_netdev_ops = {
+	.ndo_init		= cvm_oct_common_init,
+	.ndo_uninit		= cvm_oct_common_uninit,
+	.ndo_start_xmit		= cvm_oct_xmit,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static const struct net_device_ops cvm_oct_xaui_netdev_ops = {
+	.ndo_init		= cvm_oct_common_init,
+	.ndo_uninit		= cvm_oct_common_uninit,
+	.ndo_open		= cvm_oct_xaui_open,
+	.ndo_stop		= cvm_oct_common_stop,
+	.ndo_start_xmit		= cvm_oct_xmit,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static const struct net_device_ops cvm_oct_sgmii_netdev_ops = {
+	.ndo_init		= cvm_oct_sgmii_init,
+	.ndo_uninit		= cvm_oct_common_uninit,
+	.ndo_open		= cvm_oct_sgmii_open,
+	.ndo_stop		= cvm_oct_common_stop,
+	.ndo_start_xmit		= cvm_oct_xmit,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static const struct net_device_ops cvm_oct_spi_netdev_ops = {
+	.ndo_init		= cvm_oct_spi_init,
+	.ndo_uninit		= cvm_oct_spi_uninit,
+	.ndo_start_xmit		= cvm_oct_xmit,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static const struct net_device_ops cvm_oct_rgmii_netdev_ops = {
+	.ndo_init		= cvm_oct_common_init,
+	.ndo_uninit		= cvm_oct_common_uninit,
+	.ndo_open		= cvm_oct_rgmii_open,
+	.ndo_stop		= cvm_oct_common_stop,
+	.ndo_start_xmit		= cvm_oct_xmit,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static const struct net_device_ops cvm_oct_pow_netdev_ops = {
+	.ndo_init		= cvm_oct_common_init,
+	.ndo_start_xmit		= cvm_oct_xmit_pow,
+	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
+	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
+	.ndo_eth_ioctl		= cvm_oct_ioctl,
+	.ndo_change_mtu		= cvm_oct_common_change_mtu,
+	.ndo_get_stats		= cvm_oct_common_get_stats,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= cvm_oct_poll_controller,
+#endif
+};
+
+static struct device_node *cvm_oct_of_get_child
+				(const struct device_node *parent, int reg_val)
+{
+	struct device_node *node;
+	const __be32 *addr;
+	int size;
+
+	for_each_child_of_node(parent, node) {
+		addr = of_get_property(node, "reg", &size);
+		if (addr && (be32_to_cpu(*addr) == reg_val))
+			break;
+	}
+	return node;
+}
+
+static struct device_node *cvm_oct_node_for_port(struct device_node *pip,
+						 int interface, int port)
+{
+	struct device_node *ni, *np;
+
+	ni = cvm_oct_of_get_child(pip, interface);
+	if (!ni)
+		return NULL;
+
+	np = cvm_oct_of_get_child(ni, port);
+	of_node_put(ni);
+
+	return np;
+}
+
+static void cvm_set_rgmii_delay(struct octeon_ethernet *priv, int iface,
+				int port)
+{
+	struct device_node *np = priv->of_node;
+	u32 delay_value;
+	bool rx_delay;
+	bool tx_delay;
+
+	/* By default, both RX/TX delay is enabled in
+	 * __cvmx_helper_rgmii_enable().
+	 */
+	rx_delay = true;
+	tx_delay = true;
+
+	if (!of_property_read_u32(np, "rx-delay", &delay_value)) {
+		cvmx_write_csr(CVMX_ASXX_RX_CLK_SETX(port, iface), delay_value);
+		rx_delay = delay_value > 0;
+	}
+	if (!of_property_read_u32(np, "tx-delay", &delay_value)) {
+		cvmx_write_csr(CVMX_ASXX_TX_CLK_SETX(port, iface), delay_value);
+		tx_delay = delay_value > 0;
+	}
+
+	if (!rx_delay && !tx_delay)
+		priv->phy_mode = PHY_INTERFACE_MODE_RGMII_ID;
+	else if (!rx_delay)
+		priv->phy_mode = PHY_INTERFACE_MODE_RGMII_RXID;
+	else if (!tx_delay)
+		priv->phy_mode = PHY_INTERFACE_MODE_RGMII_TXID;
+	else
+		priv->phy_mode = PHY_INTERFACE_MODE_RGMII;
+}
+
+static int cvm_oct_probe(struct platform_device *pdev)
+{
+	int num_interfaces;
+	int interface;
+	int fau = FAU_NUM_PACKET_BUFFERS_TO_FREE;
+	int qos;
+	struct device_node *pip;
+	int mtu_overhead = ETH_HLEN + ETH_FCS_LEN;
+
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
+	mtu_overhead += VLAN_HLEN;
+#endif
+
+	pip = pdev->dev.of_node;
+	if (!pip) {
+		pr_err("Error: No 'pip' in /aliases\n");
+		return -EINVAL;
+	}
+
+	cvm_oct_configure_common_hw();
+
+	cvmx_helper_initialize_packet_io_global();
+
+	if (receive_group_order) {
+		if (receive_group_order > 4)
+			receive_group_order = 4;
+		pow_receive_groups = (1 << (1 << receive_group_order)) - 1;
+	} else {
+		pow_receive_groups = BIT(pow_receive_group);
+	}
+
+	/* Change the input group for all ports before input is enabled */
+	num_interfaces = cvmx_helper_get_number_of_interfaces();
+	for (interface = 0; interface < num_interfaces; interface++) {
+		int num_ports = cvmx_helper_ports_on_interface(interface);
+		int port;
+
+		for (port = cvmx_helper_get_ipd_port(interface, 0);
+		     port < cvmx_helper_get_ipd_port(interface, num_ports);
+		     port++) {
+			union cvmx_pip_prt_tagx pip_prt_tagx;
+
+			pip_prt_tagx.u64 =
+			    cvmx_read_csr(CVMX_PIP_PRT_TAGX(port));
+
+			if (receive_group_order) {
+				int tag_mask;
+
+				/* We support only 16 groups at the moment, so
+				 * always disable the two additional "hidden"
+				 * tag_mask bits on CN68XX.
+				 */
+				if (OCTEON_IS_MODEL(OCTEON_CN68XX))
+					pip_prt_tagx.u64 |= 0x3ull << 44;
+
+				tag_mask = ~((1 << receive_group_order) - 1);
+				pip_prt_tagx.s.grptagbase	= 0;
+				pip_prt_tagx.s.grptagmask	= tag_mask;
+				pip_prt_tagx.s.grptag		= 1;
+				pip_prt_tagx.s.tag_mode		= 0;
+				pip_prt_tagx.s.inc_prt_flag	= 1;
+				pip_prt_tagx.s.ip6_dprt_flag	= 1;
+				pip_prt_tagx.s.ip4_dprt_flag	= 1;
+				pip_prt_tagx.s.ip6_sprt_flag	= 1;
+				pip_prt_tagx.s.ip4_sprt_flag	= 1;
+				pip_prt_tagx.s.ip6_dst_flag	= 1;
+				pip_prt_tagx.s.ip4_dst_flag	= 1;
+				pip_prt_tagx.s.ip6_src_flag	= 1;
+				pip_prt_tagx.s.ip4_src_flag	= 1;
+				pip_prt_tagx.s.grp		= 0;
+			} else {
+				pip_prt_tagx.s.grptag	= 0;
+				pip_prt_tagx.s.grp	= pow_receive_group;
+			}
+
+			cvmx_write_csr(CVMX_PIP_PRT_TAGX(port),
+				       pip_prt_tagx.u64);
+		}
+	}
+
+	cvmx_helper_ipd_and_packet_input_enable();
+
+	memset(cvm_oct_device, 0, sizeof(cvm_oct_device));
+
+	/*
+	 * Initialize the FAU used for counting packet buffers that
+	 * need to be freed.
+	 */
+	cvmx_fau_atomic_write32(FAU_NUM_PACKET_BUFFERS_TO_FREE, 0);
+
+	/* Initialize the FAU used for counting tx SKBs that need to be freed */
+	cvmx_fau_atomic_write32(FAU_TOTAL_TX_TO_CLEAN, 0);
+
+	if ((pow_send_group != -1)) {
+		struct net_device *dev;
+
+		dev = alloc_etherdev(sizeof(struct octeon_ethernet));
+		if (dev) {
+			/* Initialize the device private structure. */
+			struct octeon_ethernet *priv = netdev_priv(dev);
+
+			SET_NETDEV_DEV(dev, &pdev->dev);
+			dev->netdev_ops = &cvm_oct_pow_netdev_ops;
+			priv->imode = CVMX_HELPER_INTERFACE_MODE_DISABLED;
+			priv->port = CVMX_PIP_NUM_INPUT_PORTS;
+			priv->queue = -1;
+			strscpy(dev->name, "pow%d", sizeof(dev->name));
+			for (qos = 0; qos < 16; qos++)
+				skb_queue_head_init(&priv->tx_free_list[qos]);
+			dev->min_mtu = VLAN_ETH_ZLEN - mtu_overhead;
+			dev->max_mtu = OCTEON_MAX_MTU - mtu_overhead;
+
+			if (register_netdev(dev) < 0) {
+				pr_err("Failed to register ethernet device for POW\n");
+				free_netdev(dev);
+			} else {
+				cvm_oct_device[CVMX_PIP_NUM_INPUT_PORTS] = dev;
+				pr_info("%s: POW send group %d, receive group %d\n",
+					dev->name, pow_send_group,
+					pow_receive_group);
+			}
+		} else {
+			pr_err("Failed to allocate ethernet device for POW\n");
+		}
+	}
+
+	num_interfaces = cvmx_helper_get_number_of_interfaces();
+	for (interface = 0; interface < num_interfaces; interface++) {
+		cvmx_helper_interface_mode_t imode =
+		    cvmx_helper_interface_get_mode(interface);
+		int num_ports = cvmx_helper_ports_on_interface(interface);
+		int port;
+		int port_index;
+
+		for (port_index = 0,
+		     port = cvmx_helper_get_ipd_port(interface, 0);
+		     port < cvmx_helper_get_ipd_port(interface, num_ports);
+		     port_index++, port++) {
+			struct octeon_ethernet *priv;
+			struct net_device *dev =
+			    alloc_etherdev(sizeof(struct octeon_ethernet));
+			if (!dev) {
+				pr_err("Failed to allocate ethernet device for port %d\n",
+				       port);
+				continue;
+			}
+
+			/* Initialize the device private structure. */
+			SET_NETDEV_DEV(dev, &pdev->dev);
+			priv = netdev_priv(dev);
+			priv->netdev = dev;
+			priv->of_node = cvm_oct_node_for_port(pip, interface,
+							      port_index);
+
+			INIT_DELAYED_WORK(&priv->port_periodic_work,
+					  cvm_oct_periodic_worker);
+			priv->imode = imode;
+			priv->port = port;
+			priv->queue = cvmx_pko_get_base_queue(priv->port);
+			priv->fau = fau - cvmx_pko_get_num_queues(port) * 4;
+			priv->phy_mode = PHY_INTERFACE_MODE_NA;
+			for (qos = 0; qos < 16; qos++)
+				skb_queue_head_init(&priv->tx_free_list[qos]);
+			for (qos = 0; qos < cvmx_pko_get_num_queues(port);
+			     qos++)
+				cvmx_fau_atomic_write32(priv->fau + qos * 4, 0);
+			dev->min_mtu = VLAN_ETH_ZLEN - mtu_overhead;
+			dev->max_mtu = OCTEON_MAX_MTU - mtu_overhead;
+
+			switch (priv->imode) {
+			/* These types don't support ports to IPD/PKO */
+			case CVMX_HELPER_INTERFACE_MODE_DISABLED:
+			case CVMX_HELPER_INTERFACE_MODE_PCIE:
+			case CVMX_HELPER_INTERFACE_MODE_PICMG:
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_NPI:
+				dev->netdev_ops = &cvm_oct_npi_netdev_ops;
+				strscpy(dev->name, "npi%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_XAUI:
+				dev->netdev_ops = &cvm_oct_xaui_netdev_ops;
+				strscpy(dev->name, "xaui%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_LOOP:
+				dev->netdev_ops = &cvm_oct_npi_netdev_ops;
+				strscpy(dev->name, "loop%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_SGMII:
+				priv->phy_mode = PHY_INTERFACE_MODE_SGMII;
+				dev->netdev_ops = &cvm_oct_sgmii_netdev_ops;
+				strscpy(dev->name, "eth%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_SPI:
+				dev->netdev_ops = &cvm_oct_spi_netdev_ops;
+				strscpy(dev->name, "spi%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_GMII:
+				priv->phy_mode = PHY_INTERFACE_MODE_GMII;
+				dev->netdev_ops = &cvm_oct_rgmii_netdev_ops;
+				strscpy(dev->name, "eth%d", sizeof(dev->name));
+				break;
+
+			case CVMX_HELPER_INTERFACE_MODE_RGMII:
+				dev->netdev_ops = &cvm_oct_rgmii_netdev_ops;
+				strscpy(dev->name, "eth%d", sizeof(dev->name));
+				cvm_set_rgmii_delay(priv, interface,
+						    port_index);
+				break;
+			}
+
+			if (priv->of_node && of_phy_is_fixed_link(priv->of_node)) {
+				if (of_phy_register_fixed_link(priv->of_node)) {
+					netdev_err(dev, "Failed to register fixed link for interface %d, port %d\n",
+						   interface, priv->port);
+					dev->netdev_ops = NULL;
+				}
+			}
+
+			if (!dev->netdev_ops) {
+				free_netdev(dev);
+			} else if (register_netdev(dev) < 0) {
+				pr_err("Failed to register ethernet device for interface %d, port %d\n",
+				       interface, priv->port);
+				free_netdev(dev);
+			} else {
+				cvm_oct_device[priv->port] = dev;
+				fau -=
+				    cvmx_pko_get_num_queues(priv->port) *
+				    sizeof(u32);
+				schedule_delayed_work(&priv->port_periodic_work,
+						      HZ);
+			}
+		}
+	}
+
+	cvm_oct_tx_initialize();
+	cvm_oct_rx_initialize();
+
+	/*
+	 * 150 uS: about 10 1500-byte packets at 1GE.
+	 */
+	cvm_oct_tx_poll_interval = 150 * (octeon_get_clock_rate() / 1000000);
+
+	schedule_delayed_work(&cvm_oct_rx_refill_work, HZ);
+
+	return 0;
+}
+
+static int cvm_oct_remove(struct platform_device *pdev)
+{
+	int port;
+
+	cvmx_ipd_disable();
+
+	atomic_inc_return(&cvm_oct_poll_queue_stopping);
+	cancel_delayed_work_sync(&cvm_oct_rx_refill_work);
+
+	cvm_oct_rx_shutdown();
+	cvm_oct_tx_shutdown();
+
+	cvmx_pko_disable();
+
+	/* Free the ethernet devices */
+	for (port = 0; port < TOTAL_NUMBER_OF_PORTS; port++) {
+		if (cvm_oct_device[port]) {
+			struct net_device *dev = cvm_oct_device[port];
+			struct octeon_ethernet *priv = netdev_priv(dev);
+
+			cancel_delayed_work_sync(&priv->port_periodic_work);
+
+			cvm_oct_tx_shutdown_dev(dev);
+			unregister_netdev(dev);
+			free_netdev(dev);
+			cvm_oct_device[port] = NULL;
+		}
+	}
+
+	cvmx_pko_shutdown();
+
+	cvmx_ipd_free_ptr();
+
+	/* Free the HW pools */
+	cvm_oct_mem_empty_fpa(CVMX_FPA_PACKET_POOL, CVMX_FPA_PACKET_POOL_SIZE,
+			      num_packet_buffers);
+	cvm_oct_mem_empty_fpa(CVMX_FPA_WQE_POOL, CVMX_FPA_WQE_POOL_SIZE,
+			      num_packet_buffers);
+	if (CVMX_FPA_OUTPUT_BUFFER_POOL != CVMX_FPA_PACKET_POOL)
+		cvm_oct_mem_empty_fpa(CVMX_FPA_OUTPUT_BUFFER_POOL,
+				      CVMX_FPA_OUTPUT_BUFFER_POOL_SIZE, 128);
+	return 0;
+}
+
+static const struct of_device_id cvm_oct_match[] = {
+	{
+		.compatible = "cavium,octeon-3860-pip",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, cvm_oct_match);
+
+static struct platform_driver cvm_oct_driver = {
+	.probe		= cvm_oct_probe,
+	.remove		= cvm_oct_remove,
+	.driver		= {
+		.name	= KBUILD_MODNAME,
+		.of_match_table = cvm_oct_match,
+	},
+};
+
+module_platform_driver(cvm_oct_driver);
+
+MODULE_SOFTDEP("pre: mdio-cavium");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Cavium Networks <support@caviumnetworks.com>");
+MODULE_DESCRIPTION("Cavium Networks Octeon ethernet driver.");
diff --git a/drivers/staging/octeon/octeon-ethernet.h b/drivers/staging/octeon/octeon-ethernet.h
new file mode 100644
index 000000000..a61407057
--- /dev/null
+++ b/drivers/staging/octeon/octeon-ethernet.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
+ *
+ * Copyright (c) 2003-2010 Cavium Networks
+ */
+
+/*
+ * External interface for the Cavium Octeon ethernet driver.
+ */
+#ifndef OCTEON_ETHERNET_H
+#define OCTEON_ETHERNET_H
+
+#include <linux/of.h>
+#include <linux/phy.h>
+
+#ifdef CONFIG_CAVIUM_OCTEON_SOC
+
+#include <asm/octeon/octeon.h>
+
+#include <asm/octeon/cvmx-asxx-defs.h>
+#include <asm/octeon/cvmx-config.h>
+#include <asm/octeon/cvmx-fau.h>
+#include <asm/octeon/cvmx-gmxx-defs.h>
+#include <asm/octeon/cvmx-helper.h>
+#include <asm/octeon/cvmx-helper-util.h>
+#include <asm/octeon/cvmx-ipd.h>
+#include <asm/octeon/cvmx-ipd-defs.h>
+#include <asm/octeon/cvmx-npi-defs.h>
+#include <asm/octeon/cvmx-pip.h>
+#include <asm/octeon/cvmx-pko.h>
+#include <asm/octeon/cvmx-pow.h>
+#include <asm/octeon/cvmx-scratch.h>
+#include <asm/octeon/cvmx-spi.h>
+#include <asm/octeon/cvmx-spxx-defs.h>
+#include <asm/octeon/cvmx-stxx-defs.h>
+#include <asm/octeon/cvmx-wqe.h>
+
+#else
+
+#include "octeon-stubs.h"
+
+#endif
+
+/**
+ * This is the definition of the Ethernet driver's private
+ * driver state stored in netdev_priv(dev).
+ */
+struct octeon_ethernet {
+	/* PKO hardware output port */
+	int port;
+	/* PKO hardware queue for the port */
+	int queue;
+	/* Hardware fetch and add to count outstanding tx buffers */
+	int fau;
+	/* My netdev. */
+	struct net_device *netdev;
+	/*
+	 * Type of port. This is one of the enums in
+	 * cvmx_helper_interface_mode_t
+	 */
+	int imode;
+	/* PHY mode */
+	phy_interface_t phy_mode;
+	/* List of outstanding tx buffers per queue */
+	struct sk_buff_head tx_free_list[16];
+	unsigned int last_speed;
+	unsigned int last_link;
+	/* Last negotiated link state */
+	u64 link_info;
+	/* Called periodically to check link status */
+	void (*poll)(struct net_device *dev);
+	struct delayed_work	port_periodic_work;
+	struct device_node	*of_node;
+};
+
+int cvm_oct_free_work(void *work_queue_entry);
+
+int cvm_oct_rgmii_open(struct net_device *dev);
+
+int cvm_oct_sgmii_init(struct net_device *dev);
+int cvm_oct_sgmii_open(struct net_device *dev);
+
+int cvm_oct_spi_init(struct net_device *dev);
+void cvm_oct_spi_uninit(struct net_device *dev);
+
+int cvm_oct_common_init(struct net_device *dev);
+void cvm_oct_common_uninit(struct net_device *dev);
+void cvm_oct_adjust_link(struct net_device *dev);
+int cvm_oct_common_stop(struct net_device *dev);
+int cvm_oct_common_open(struct net_device *dev,
+			void (*link_poll)(struct net_device *));
+void cvm_oct_note_carrier(struct octeon_ethernet *priv,
+			  union cvmx_helper_link_info li);
+void cvm_oct_link_poll(struct net_device *dev);
+
+extern int always_use_pow;
+extern int pow_send_group;
+extern int pow_receive_groups;
+extern char pow_send_list[];
+extern struct net_device *cvm_oct_device[];
+extern atomic_t cvm_oct_poll_queue_stopping;
+extern u64 cvm_oct_tx_poll_interval;
+
+extern int rx_napi_weight;
+
+#endif
diff --git a/drivers/staging/octeon/octeon-stubs.h b/drivers/staging/octeon/octeon-stubs.h
new file mode 100644
index 000000000..3f8e5713b
--- /dev/null
+++ b/drivers/staging/octeon/octeon-stubs.h
@@ -0,0 +1,1435 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#define CONFIG_CAVIUM_OCTEON_CVMSEG_SIZE	512
+
+#ifndef XKPHYS_TO_PHYS
+# define XKPHYS_TO_PHYS(p)			(p)
+#endif
+
+#define OCTEON_IRQ_WORKQ0 0
+#define OCTEON_IRQ_RML 0
+#define OCTEON_IRQ_TIMER1 0
+#define OCTEON_IS_MODEL(x) 0
+#define octeon_has_feature(x)	0
+#define octeon_get_clock_rate()	0
+
+#define CVMX_SYNCIOBDMA		do { } while (0)
+
+#define CVMX_HELPER_INPUT_TAG_TYPE	0
+#define CVMX_HELPER_FIRST_MBUFF_SKIP	7
+#define CVMX_FAU_REG_END		(2048)
+#define CVMX_FPA_OUTPUT_BUFFER_POOL	    (2)
+#define CVMX_FPA_OUTPUT_BUFFER_POOL_SIZE    16
+#define CVMX_FPA_PACKET_POOL		    (0)
+#define CVMX_FPA_PACKET_POOL_SIZE	    16
+#define CVMX_FPA_WQE_POOL		    (1)
+#define CVMX_FPA_WQE_POOL_SIZE		    16
+#define CVMX_GMXX_RXX_ADR_CAM_EN(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CTL(a, b)	((a) + (b))
+#define CVMX_GMXX_PRTX_CFG(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_FRM_MAX(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_JABBER(a, b)	((a) + (b))
+#define CVMX_IPD_CTL_STATUS		0
+#define CVMX_PIP_FRM_LEN_CHKX(a)	(a)
+#define CVMX_PIP_NUM_INPUT_PORTS	1
+#define CVMX_SCR_SCRATCH		0
+#define CVMX_PKO_QUEUES_PER_PORT_INTERFACE0	2
+#define CVMX_PKO_QUEUES_PER_PORT_INTERFACE1	2
+#define CVMX_IPD_SUB_PORT_FCS		0
+#define CVMX_SSO_WQ_IQ_DIS		0
+#define CVMX_SSO_WQ_INT			0
+#define CVMX_POW_WQ_INT			0
+#define CVMX_SSO_WQ_INT_PC		0
+#define CVMX_NPI_RSL_INT_BLOCKS		0
+#define CVMX_POW_WQ_INT_PC		0
+
+union cvmx_pip_wqe_word2 {
+	uint64_t u64;
+	struct {
+		uint64_t bufs:8;
+		uint64_t ip_offset:8;
+		uint64_t vlan_valid:1;
+		uint64_t vlan_stacked:1;
+		uint64_t unassigned:1;
+		uint64_t vlan_cfi:1;
+		uint64_t vlan_id:12;
+		uint64_t pr:4;
+		uint64_t unassigned2:8;
+		uint64_t dec_ipcomp:1;
+		uint64_t tcp_or_udp:1;
+		uint64_t dec_ipsec:1;
+		uint64_t is_v6:1;
+		uint64_t software:1;
+		uint64_t L4_error:1;
+		uint64_t is_frag:1;
+		uint64_t IP_exc:1;
+		uint64_t is_bcast:1;
+		uint64_t is_mcast:1;
+		uint64_t not_IP:1;
+		uint64_t rcv_error:1;
+		uint64_t err_code:8;
+	} s;
+	struct {
+		uint64_t bufs:8;
+		uint64_t ip_offset:8;
+		uint64_t vlan_valid:1;
+		uint64_t vlan_stacked:1;
+		uint64_t unassigned:1;
+		uint64_t vlan_cfi:1;
+		uint64_t vlan_id:12;
+		uint64_t port:12;
+		uint64_t dec_ipcomp:1;
+		uint64_t tcp_or_udp:1;
+		uint64_t dec_ipsec:1;
+		uint64_t is_v6:1;
+		uint64_t software:1;
+		uint64_t L4_error:1;
+		uint64_t is_frag:1;
+		uint64_t IP_exc:1;
+		uint64_t is_bcast:1;
+		uint64_t is_mcast:1;
+		uint64_t not_IP:1;
+		uint64_t rcv_error:1;
+		uint64_t err_code:8;
+	} s_cn68xx;
+
+	struct {
+		uint64_t unused1:16;
+		uint64_t vlan:16;
+		uint64_t unused2:32;
+	} svlan;
+	struct {
+		uint64_t bufs:8;
+		uint64_t unused:8;
+		uint64_t vlan_valid:1;
+		uint64_t vlan_stacked:1;
+		uint64_t unassigned:1;
+		uint64_t vlan_cfi:1;
+		uint64_t vlan_id:12;
+		uint64_t pr:4;
+		uint64_t unassigned2:12;
+		uint64_t software:1;
+		uint64_t unassigned3:1;
+		uint64_t is_rarp:1;
+		uint64_t is_arp:1;
+		uint64_t is_bcast:1;
+		uint64_t is_mcast:1;
+		uint64_t not_IP:1;
+		uint64_t rcv_error:1;
+		uint64_t err_code:8;
+	} snoip;
+
+};
+
+union cvmx_pip_wqe_word0 {
+	struct {
+		uint64_t next_ptr:40;
+		uint8_t unused;
+		__wsum hw_chksum;
+	} cn38xx;
+	struct {
+		uint64_t pknd:6;        /* 0..5 */
+		uint64_t unused2:2;     /* 6..7 */
+		uint64_t bpid:6;        /* 8..13 */
+		uint64_t unused1:18;    /* 14..31 */
+		uint64_t l2ptr:8;       /* 32..39 */
+		uint64_t l3ptr:8;       /* 40..47 */
+		uint64_t unused0:8;     /* 48..55 */
+		uint64_t l4ptr:8;       /* 56..63 */
+	} cn68xx;
+};
+
+union cvmx_wqe_word0 {
+	uint64_t u64;
+	union cvmx_pip_wqe_word0 pip;
+};
+
+union cvmx_wqe_word1 {
+	uint64_t u64;
+	struct {
+		uint64_t tag:32;
+		uint64_t tag_type:2;
+		uint64_t varies:14;
+		uint64_t len:16;
+	};
+	struct {
+		uint64_t tag:32;
+		uint64_t tag_type:2;
+		uint64_t zero_2:3;
+		uint64_t grp:6;
+		uint64_t zero_1:1;
+		uint64_t qos:3;
+		uint64_t zero_0:1;
+		uint64_t len:16;
+	} cn68xx;
+	struct {
+		uint64_t tag:32;
+		uint64_t tag_type:2;
+		uint64_t zero_2:1;
+		uint64_t grp:4;
+		uint64_t qos:3;
+		uint64_t ipprt:6;
+		uint64_t len:16;
+	} cn38xx;
+};
+
+union cvmx_buf_ptr {
+	void *ptr;
+	uint64_t u64;
+	struct {
+		uint64_t i:1;
+		uint64_t back:4;
+		uint64_t pool:3;
+		uint64_t size:16;
+		uint64_t addr:40;
+	} s;
+};
+
+struct cvmx_wqe {
+	union cvmx_wqe_word0 word0;
+	union cvmx_wqe_word1 word1;
+	union cvmx_pip_wqe_word2 word2;
+	union cvmx_buf_ptr packet_ptr;
+	uint8_t packet_data[96];
+};
+
+union cvmx_helper_link_info {
+	uint64_t u64;
+	struct {
+		uint64_t reserved_20_63:44;
+		uint64_t link_up:1;	    /**< Is the physical link up? */
+		uint64_t full_duplex:1;	    /**< 1 if the link is full duplex */
+		uint64_t speed:18;	    /**< Speed of the link in Mbps */
+	} s;
+};
+
+enum cvmx_fau_reg_32 {
+	CVMX_FAU_REG_32_START	= 0,
+};
+
+enum cvmx_fau_op_size {
+	CVMX_FAU_OP_SIZE_8 = 0,
+	CVMX_FAU_OP_SIZE_16 = 1,
+	CVMX_FAU_OP_SIZE_32 = 2,
+	CVMX_FAU_OP_SIZE_64 = 3
+};
+
+typedef enum {
+	CVMX_SPI_MODE_UNKNOWN = 0,
+	CVMX_SPI_MODE_TX_HALFPLEX = 1,
+	CVMX_SPI_MODE_RX_HALFPLEX = 2,
+	CVMX_SPI_MODE_DUPLEX = 3
+} cvmx_spi_mode_t;
+
+typedef enum {
+	CVMX_HELPER_INTERFACE_MODE_DISABLED,
+	CVMX_HELPER_INTERFACE_MODE_RGMII,
+	CVMX_HELPER_INTERFACE_MODE_GMII,
+	CVMX_HELPER_INTERFACE_MODE_SPI,
+	CVMX_HELPER_INTERFACE_MODE_PCIE,
+	CVMX_HELPER_INTERFACE_MODE_XAUI,
+	CVMX_HELPER_INTERFACE_MODE_SGMII,
+	CVMX_HELPER_INTERFACE_MODE_PICMG,
+	CVMX_HELPER_INTERFACE_MODE_NPI,
+	CVMX_HELPER_INTERFACE_MODE_LOOP,
+} cvmx_helper_interface_mode_t;
+
+typedef enum {
+	CVMX_POW_WAIT = 1,
+	CVMX_POW_NO_WAIT = 0,
+} cvmx_pow_wait_t;
+
+typedef enum {
+	CVMX_PKO_LOCK_NONE = 0,
+	CVMX_PKO_LOCK_ATOMIC_TAG = 1,
+	CVMX_PKO_LOCK_CMD_QUEUE = 2,
+} cvmx_pko_lock_t;
+
+typedef enum {
+	CVMX_PKO_SUCCESS,
+	CVMX_PKO_INVALID_PORT,
+	CVMX_PKO_INVALID_QUEUE,
+	CVMX_PKO_INVALID_PRIORITY,
+	CVMX_PKO_NO_MEMORY,
+	CVMX_PKO_PORT_ALREADY_SETUP,
+	CVMX_PKO_CMD_QUEUE_INIT_ERROR
+} cvmx_pko_status_t;
+
+enum cvmx_pow_tag_type {
+	CVMX_POW_TAG_TYPE_ORDERED   = 0L,
+	CVMX_POW_TAG_TYPE_ATOMIC    = 1L,
+	CVMX_POW_TAG_TYPE_NULL	    = 2L,
+	CVMX_POW_TAG_TYPE_NULL_NULL = 3L
+};
+
+union cvmx_ipd_ctl_status {
+	uint64_t u64;
+	struct cvmx_ipd_ctl_status_s {
+		uint64_t reserved_18_63:46;
+		uint64_t use_sop:1;
+		uint64_t rst_done:1;
+		uint64_t clken:1;
+		uint64_t no_wptr:1;
+		uint64_t pq_apkt:1;
+		uint64_t pq_nabuf:1;
+		uint64_t ipd_full:1;
+		uint64_t pkt_off:1;
+		uint64_t len_m8:1;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} s;
+	struct cvmx_ipd_ctl_status_cn30xx {
+		uint64_t reserved_10_63:54;
+		uint64_t len_m8:1;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} cn30xx;
+	struct cvmx_ipd_ctl_status_cn38xxp2 {
+		uint64_t reserved_9_63:55;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} cn38xxp2;
+	struct cvmx_ipd_ctl_status_cn50xx {
+		uint64_t reserved_15_63:49;
+		uint64_t no_wptr:1;
+		uint64_t pq_apkt:1;
+		uint64_t pq_nabuf:1;
+		uint64_t ipd_full:1;
+		uint64_t pkt_off:1;
+		uint64_t len_m8:1;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} cn50xx;
+	struct cvmx_ipd_ctl_status_cn58xx {
+		uint64_t reserved_12_63:52;
+		uint64_t ipd_full:1;
+		uint64_t pkt_off:1;
+		uint64_t len_m8:1;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} cn58xx;
+	struct cvmx_ipd_ctl_status_cn63xxp1 {
+		uint64_t reserved_16_63:48;
+		uint64_t clken:1;
+		uint64_t no_wptr:1;
+		uint64_t pq_apkt:1;
+		uint64_t pq_nabuf:1;
+		uint64_t ipd_full:1;
+		uint64_t pkt_off:1;
+		uint64_t len_m8:1;
+		uint64_t reset:1;
+		uint64_t addpkt:1;
+		uint64_t naddbuf:1;
+		uint64_t pkt_lend:1;
+		uint64_t wqe_lend:1;
+		uint64_t pbp_en:1;
+		uint64_t opc_mode:2;
+		uint64_t ipd_en:1;
+	} cn63xxp1;
+};
+
+union cvmx_ipd_sub_port_fcs {
+	uint64_t u64;
+	struct cvmx_ipd_sub_port_fcs_s {
+		uint64_t port_bit:32;
+		uint64_t reserved_32_35:4;
+		uint64_t port_bit2:4;
+		uint64_t reserved_40_63:24;
+	} s;
+	struct cvmx_ipd_sub_port_fcs_cn30xx {
+		uint64_t port_bit:3;
+		uint64_t reserved_3_63:61;
+	} cn30xx;
+	struct cvmx_ipd_sub_port_fcs_cn38xx {
+		uint64_t port_bit:32;
+		uint64_t reserved_32_63:32;
+	} cn38xx;
+};
+
+union cvmx_ipd_sub_port_qos_cnt {
+	uint64_t u64;
+	struct cvmx_ipd_sub_port_qos_cnt_s {
+		uint64_t cnt:32;
+		uint64_t port_qos:9;
+		uint64_t reserved_41_63:23;
+	} s;
+};
+
+typedef struct {
+	uint32_t dropped_octets;
+	uint32_t dropped_packets;
+	uint32_t pci_raw_packets;
+	uint32_t octets;
+	uint32_t packets;
+	uint32_t multicast_packets;
+	uint32_t broadcast_packets;
+	uint32_t len_64_packets;
+	uint32_t len_65_127_packets;
+	uint32_t len_128_255_packets;
+	uint32_t len_256_511_packets;
+	uint32_t len_512_1023_packets;
+	uint32_t len_1024_1518_packets;
+	uint32_t len_1519_max_packets;
+	uint32_t fcs_align_err_packets;
+	uint32_t runt_packets;
+	uint32_t runt_crc_packets;
+	uint32_t oversize_packets;
+	uint32_t oversize_crc_packets;
+	uint32_t inb_packets;
+	uint64_t inb_octets;
+	uint16_t inb_errors;
+} cvmx_pip_port_status_t;
+
+typedef struct {
+	uint32_t packets;
+	uint64_t octets;
+	uint64_t doorbell;
+} cvmx_pko_port_status_t;
+
+union cvmx_pip_frm_len_chkx {
+	uint64_t u64;
+	struct cvmx_pip_frm_len_chkx_s {
+		uint64_t reserved_32_63:32;
+		uint64_t maxlen:16;
+		uint64_t minlen:16;
+	} s;
+};
+
+union cvmx_gmxx_rxx_frm_ctl {
+	uint64_t u64;
+	struct cvmx_gmxx_rxx_frm_ctl_s {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t vlan_len:1;
+		uint64_t pad_len:1;
+		uint64_t pre_align:1;
+		uint64_t null_dis:1;
+		uint64_t reserved_11_11:1;
+		uint64_t ptp_mode:1;
+		uint64_t reserved_13_63:51;
+	} s;
+	struct cvmx_gmxx_rxx_frm_ctl_cn30xx {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t vlan_len:1;
+		uint64_t pad_len:1;
+		uint64_t reserved_9_63:55;
+	} cn30xx;
+	struct cvmx_gmxx_rxx_frm_ctl_cn31xx {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t vlan_len:1;
+		uint64_t reserved_8_63:56;
+	} cn31xx;
+	struct cvmx_gmxx_rxx_frm_ctl_cn50xx {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t reserved_7_8:2;
+		uint64_t pre_align:1;
+		uint64_t null_dis:1;
+		uint64_t reserved_11_63:53;
+	} cn50xx;
+	struct cvmx_gmxx_rxx_frm_ctl_cn56xxp1 {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t reserved_7_8:2;
+		uint64_t pre_align:1;
+		uint64_t reserved_10_63:54;
+	} cn56xxp1;
+	struct cvmx_gmxx_rxx_frm_ctl_cn58xx {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t vlan_len:1;
+		uint64_t pad_len:1;
+		uint64_t pre_align:1;
+		uint64_t null_dis:1;
+		uint64_t reserved_11_63:53;
+	} cn58xx;
+	struct cvmx_gmxx_rxx_frm_ctl_cn61xx {
+		uint64_t pre_chk:1;
+		uint64_t pre_strp:1;
+		uint64_t ctl_drp:1;
+		uint64_t ctl_bck:1;
+		uint64_t ctl_mcst:1;
+		uint64_t ctl_smac:1;
+		uint64_t pre_free:1;
+		uint64_t reserved_7_8:2;
+		uint64_t pre_align:1;
+		uint64_t null_dis:1;
+		uint64_t reserved_11_11:1;
+		uint64_t ptp_mode:1;
+		uint64_t reserved_13_63:51;
+	} cn61xx;
+};
+
+union cvmx_gmxx_rxx_int_reg {
+	uint64_t u64;
+	struct cvmx_gmxx_rxx_int_reg_s {
+		uint64_t minerr:1;
+		uint64_t carext:1;
+		uint64_t maxerr:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t alnerr:1;
+		uint64_t lenerr:1;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t niberr:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t phy_link:1;
+		uint64_t phy_spd:1;
+		uint64_t phy_dupx:1;
+		uint64_t pause_drp:1;
+		uint64_t loc_fault:1;
+		uint64_t rem_fault:1;
+		uint64_t bad_seq:1;
+		uint64_t bad_term:1;
+		uint64_t unsop:1;
+		uint64_t uneop:1;
+		uint64_t undat:1;
+		uint64_t hg2fld:1;
+		uint64_t hg2cc:1;
+		uint64_t reserved_29_63:35;
+	} s;
+	struct cvmx_gmxx_rxx_int_reg_cn30xx {
+		uint64_t minerr:1;
+		uint64_t carext:1;
+		uint64_t maxerr:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t alnerr:1;
+		uint64_t lenerr:1;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t niberr:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t phy_link:1;
+		uint64_t phy_spd:1;
+		uint64_t phy_dupx:1;
+		uint64_t reserved_19_63:45;
+	} cn30xx;
+	struct cvmx_gmxx_rxx_int_reg_cn50xx {
+		uint64_t reserved_0_0:1;
+		uint64_t carext:1;
+		uint64_t reserved_2_2:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t alnerr:1;
+		uint64_t reserved_6_6:1;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t niberr:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t phy_link:1;
+		uint64_t phy_spd:1;
+		uint64_t phy_dupx:1;
+		uint64_t pause_drp:1;
+		uint64_t reserved_20_63:44;
+	} cn50xx;
+	struct cvmx_gmxx_rxx_int_reg_cn52xx {
+		uint64_t reserved_0_0:1;
+		uint64_t carext:1;
+		uint64_t reserved_2_2:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t reserved_5_6:2;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t reserved_9_9:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t reserved_16_18:3;
+		uint64_t pause_drp:1;
+		uint64_t loc_fault:1;
+		uint64_t rem_fault:1;
+		uint64_t bad_seq:1;
+		uint64_t bad_term:1;
+		uint64_t unsop:1;
+		uint64_t uneop:1;
+		uint64_t undat:1;
+		uint64_t hg2fld:1;
+		uint64_t hg2cc:1;
+		uint64_t reserved_29_63:35;
+	} cn52xx;
+	struct cvmx_gmxx_rxx_int_reg_cn56xxp1 {
+		uint64_t reserved_0_0:1;
+		uint64_t carext:1;
+		uint64_t reserved_2_2:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t reserved_5_6:2;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t reserved_9_9:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t reserved_16_18:3;
+		uint64_t pause_drp:1;
+		uint64_t loc_fault:1;
+		uint64_t rem_fault:1;
+		uint64_t bad_seq:1;
+		uint64_t bad_term:1;
+		uint64_t unsop:1;
+		uint64_t uneop:1;
+		uint64_t undat:1;
+		uint64_t reserved_27_63:37;
+	} cn56xxp1;
+	struct cvmx_gmxx_rxx_int_reg_cn58xx {
+		uint64_t minerr:1;
+		uint64_t carext:1;
+		uint64_t maxerr:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t alnerr:1;
+		uint64_t lenerr:1;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t niberr:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t phy_link:1;
+		uint64_t phy_spd:1;
+		uint64_t phy_dupx:1;
+		uint64_t pause_drp:1;
+		uint64_t reserved_20_63:44;
+	} cn58xx;
+	struct cvmx_gmxx_rxx_int_reg_cn61xx {
+		uint64_t minerr:1;
+		uint64_t carext:1;
+		uint64_t reserved_2_2:1;
+		uint64_t jabber:1;
+		uint64_t fcserr:1;
+		uint64_t reserved_5_6:2;
+		uint64_t rcverr:1;
+		uint64_t skperr:1;
+		uint64_t reserved_9_9:1;
+		uint64_t ovrerr:1;
+		uint64_t pcterr:1;
+		uint64_t rsverr:1;
+		uint64_t falerr:1;
+		uint64_t coldet:1;
+		uint64_t ifgerr:1;
+		uint64_t reserved_16_18:3;
+		uint64_t pause_drp:1;
+		uint64_t loc_fault:1;
+		uint64_t rem_fault:1;
+		uint64_t bad_seq:1;
+		uint64_t bad_term:1;
+		uint64_t unsop:1;
+		uint64_t uneop:1;
+		uint64_t undat:1;
+		uint64_t hg2fld:1;
+		uint64_t hg2cc:1;
+		uint64_t reserved_29_63:35;
+	} cn61xx;
+};
+
+union cvmx_gmxx_prtx_cfg {
+	uint64_t u64;
+	struct cvmx_gmxx_prtx_cfg_s {
+		uint64_t reserved_22_63:42;
+		uint64_t pknd:6;
+		uint64_t reserved_14_15:2;
+		uint64_t tx_idle:1;
+		uint64_t rx_idle:1;
+		uint64_t reserved_9_11:3;
+		uint64_t speed_msb:1;
+		uint64_t reserved_4_7:4;
+		uint64_t slottime:1;
+		uint64_t duplex:1;
+		uint64_t speed:1;
+		uint64_t en:1;
+	} s;
+	struct cvmx_gmxx_prtx_cfg_cn30xx {
+		uint64_t reserved_4_63:60;
+		uint64_t slottime:1;
+		uint64_t duplex:1;
+		uint64_t speed:1;
+		uint64_t en:1;
+	} cn30xx;
+	struct cvmx_gmxx_prtx_cfg_cn52xx {
+		uint64_t reserved_14_63:50;
+		uint64_t tx_idle:1;
+		uint64_t rx_idle:1;
+		uint64_t reserved_9_11:3;
+		uint64_t speed_msb:1;
+		uint64_t reserved_4_7:4;
+		uint64_t slottime:1;
+		uint64_t duplex:1;
+		uint64_t speed:1;
+		uint64_t en:1;
+	} cn52xx;
+};
+
+union cvmx_gmxx_rxx_adr_ctl {
+	uint64_t u64;
+	struct cvmx_gmxx_rxx_adr_ctl_s {
+		uint64_t reserved_4_63:60;
+		uint64_t cam_mode:1;
+		uint64_t mcst:2;
+		uint64_t bcst:1;
+	} s;
+};
+
+union cvmx_pip_prt_tagx {
+	uint64_t u64;
+	struct cvmx_pip_prt_tagx_s {
+		uint64_t reserved_54_63:10;
+		uint64_t portadd_en:1;
+		uint64_t inc_hwchk:1;
+		uint64_t reserved_50_51:2;
+		uint64_t grptagbase_msb:2;
+		uint64_t reserved_46_47:2;
+		uint64_t grptagmask_msb:2;
+		uint64_t reserved_42_43:2;
+		uint64_t grp_msb:2;
+		uint64_t grptagbase:4;
+		uint64_t grptagmask:4;
+		uint64_t grptag:1;
+		uint64_t grptag_mskip:1;
+		uint64_t tag_mode:2;
+		uint64_t inc_vs:2;
+		uint64_t inc_vlan:1;
+		uint64_t inc_prt_flag:1;
+		uint64_t ip6_dprt_flag:1;
+		uint64_t ip4_dprt_flag:1;
+		uint64_t ip6_sprt_flag:1;
+		uint64_t ip4_sprt_flag:1;
+		uint64_t ip6_nxth_flag:1;
+		uint64_t ip4_pctl_flag:1;
+		uint64_t ip6_dst_flag:1;
+		uint64_t ip4_dst_flag:1;
+		uint64_t ip6_src_flag:1;
+		uint64_t ip4_src_flag:1;
+		uint64_t tcp6_tag_type:2;
+		uint64_t tcp4_tag_type:2;
+		uint64_t ip6_tag_type:2;
+		uint64_t ip4_tag_type:2;
+		uint64_t non_tag_type:2;
+		uint64_t grp:4;
+	} s;
+	struct cvmx_pip_prt_tagx_cn30xx {
+		uint64_t reserved_40_63:24;
+		uint64_t grptagbase:4;
+		uint64_t grptagmask:4;
+		uint64_t grptag:1;
+		uint64_t reserved_30_30:1;
+		uint64_t tag_mode:2;
+		uint64_t inc_vs:2;
+		uint64_t inc_vlan:1;
+		uint64_t inc_prt_flag:1;
+		uint64_t ip6_dprt_flag:1;
+		uint64_t ip4_dprt_flag:1;
+		uint64_t ip6_sprt_flag:1;
+		uint64_t ip4_sprt_flag:1;
+		uint64_t ip6_nxth_flag:1;
+		uint64_t ip4_pctl_flag:1;
+		uint64_t ip6_dst_flag:1;
+		uint64_t ip4_dst_flag:1;
+		uint64_t ip6_src_flag:1;
+		uint64_t ip4_src_flag:1;
+		uint64_t tcp6_tag_type:2;
+		uint64_t tcp4_tag_type:2;
+		uint64_t ip6_tag_type:2;
+		uint64_t ip4_tag_type:2;
+		uint64_t non_tag_type:2;
+		uint64_t grp:4;
+	} cn30xx;
+	struct cvmx_pip_prt_tagx_cn50xx {
+		uint64_t reserved_40_63:24;
+		uint64_t grptagbase:4;
+		uint64_t grptagmask:4;
+		uint64_t grptag:1;
+		uint64_t grptag_mskip:1;
+		uint64_t tag_mode:2;
+		uint64_t inc_vs:2;
+		uint64_t inc_vlan:1;
+		uint64_t inc_prt_flag:1;
+		uint64_t ip6_dprt_flag:1;
+		uint64_t ip4_dprt_flag:1;
+		uint64_t ip6_sprt_flag:1;
+		uint64_t ip4_sprt_flag:1;
+		uint64_t ip6_nxth_flag:1;
+		uint64_t ip4_pctl_flag:1;
+		uint64_t ip6_dst_flag:1;
+		uint64_t ip4_dst_flag:1;
+		uint64_t ip6_src_flag:1;
+		uint64_t ip4_src_flag:1;
+		uint64_t tcp6_tag_type:2;
+		uint64_t tcp4_tag_type:2;
+		uint64_t ip6_tag_type:2;
+		uint64_t ip4_tag_type:2;
+		uint64_t non_tag_type:2;
+		uint64_t grp:4;
+	} cn50xx;
+};
+
+union cvmx_spxx_int_reg {
+	uint64_t u64;
+	struct cvmx_spxx_int_reg_s {
+		uint64_t reserved_32_63:32;
+		uint64_t spf:1;
+		uint64_t reserved_12_30:19;
+		uint64_t calerr:1;
+		uint64_t syncerr:1;
+		uint64_t diperr:1;
+		uint64_t tpaovr:1;
+		uint64_t rsverr:1;
+		uint64_t drwnng:1;
+		uint64_t clserr:1;
+		uint64_t spiovr:1;
+		uint64_t reserved_2_3:2;
+		uint64_t abnorm:1;
+		uint64_t prtnxa:1;
+	} s;
+};
+
+union cvmx_spxx_int_msk {
+	uint64_t u64;
+	struct cvmx_spxx_int_msk_s {
+		uint64_t reserved_12_63:52;
+		uint64_t calerr:1;
+		uint64_t syncerr:1;
+		uint64_t diperr:1;
+		uint64_t tpaovr:1;
+		uint64_t rsverr:1;
+		uint64_t drwnng:1;
+		uint64_t clserr:1;
+		uint64_t spiovr:1;
+		uint64_t reserved_2_3:2;
+		uint64_t abnorm:1;
+		uint64_t prtnxa:1;
+	} s;
+};
+
+union cvmx_pow_wq_int {
+	uint64_t u64;
+	struct cvmx_pow_wq_int_s {
+		uint64_t wq_int:16;
+		uint64_t iq_dis:16;
+		uint64_t reserved_32_63:32;
+	} s;
+};
+
+union cvmx_sso_wq_int_thrx {
+	uint64_t u64;
+	struct {
+		uint64_t iq_thr:12;
+		uint64_t reserved_12_13:2;
+		uint64_t ds_thr:12;
+		uint64_t reserved_26_27:2;
+		uint64_t tc_thr:4;
+		uint64_t tc_en:1;
+		uint64_t reserved_33_63:31;
+	} s;
+};
+
+union cvmx_stxx_int_reg {
+	uint64_t u64;
+	struct cvmx_stxx_int_reg_s {
+		uint64_t reserved_9_63:55;
+		uint64_t syncerr:1;
+		uint64_t frmerr:1;
+		uint64_t unxfrm:1;
+		uint64_t nosync:1;
+		uint64_t diperr:1;
+		uint64_t datovr:1;
+		uint64_t ovrbst:1;
+		uint64_t calpar1:1;
+		uint64_t calpar0:1;
+	} s;
+};
+
+union cvmx_stxx_int_msk {
+	uint64_t u64;
+	struct cvmx_stxx_int_msk_s {
+		uint64_t reserved_8_63:56;
+		uint64_t frmerr:1;
+		uint64_t unxfrm:1;
+		uint64_t nosync:1;
+		uint64_t diperr:1;
+		uint64_t datovr:1;
+		uint64_t ovrbst:1;
+		uint64_t calpar1:1;
+		uint64_t calpar0:1;
+	} s;
+};
+
+union cvmx_pow_wq_int_pc {
+	uint64_t u64;
+	struct cvmx_pow_wq_int_pc_s {
+		uint64_t reserved_0_7:8;
+		uint64_t pc_thr:20;
+		uint64_t reserved_28_31:4;
+		uint64_t pc:28;
+		uint64_t reserved_60_63:4;
+	} s;
+};
+
+union cvmx_pow_wq_int_thrx {
+	uint64_t u64;
+	struct cvmx_pow_wq_int_thrx_s {
+		uint64_t reserved_29_63:35;
+		uint64_t tc_en:1;
+		uint64_t tc_thr:4;
+		uint64_t reserved_23_23:1;
+		uint64_t ds_thr:11;
+		uint64_t reserved_11_11:1;
+		uint64_t iq_thr:11;
+	} s;
+	struct cvmx_pow_wq_int_thrx_cn30xx {
+		uint64_t reserved_29_63:35;
+		uint64_t tc_en:1;
+		uint64_t tc_thr:4;
+		uint64_t reserved_18_23:6;
+		uint64_t ds_thr:6;
+		uint64_t reserved_6_11:6;
+		uint64_t iq_thr:6;
+	} cn30xx;
+	struct cvmx_pow_wq_int_thrx_cn31xx {
+		uint64_t reserved_29_63:35;
+		uint64_t tc_en:1;
+		uint64_t tc_thr:4;
+		uint64_t reserved_20_23:4;
+		uint64_t ds_thr:8;
+		uint64_t reserved_8_11:4;
+		uint64_t iq_thr:8;
+	} cn31xx;
+	struct cvmx_pow_wq_int_thrx_cn52xx {
+		uint64_t reserved_29_63:35;
+		uint64_t tc_en:1;
+		uint64_t tc_thr:4;
+		uint64_t reserved_21_23:3;
+		uint64_t ds_thr:9;
+		uint64_t reserved_9_11:3;
+		uint64_t iq_thr:9;
+	} cn52xx;
+	struct cvmx_pow_wq_int_thrx_cn63xx {
+		uint64_t reserved_29_63:35;
+		uint64_t tc_en:1;
+		uint64_t tc_thr:4;
+		uint64_t reserved_22_23:2;
+		uint64_t ds_thr:10;
+		uint64_t reserved_10_11:2;
+		uint64_t iq_thr:10;
+	} cn63xx;
+};
+
+union cvmx_npi_rsl_int_blocks {
+	uint64_t u64;
+	struct cvmx_npi_rsl_int_blocks_s {
+		uint64_t reserved_32_63:32;
+		uint64_t rint_31:1;
+		uint64_t iob:1;
+		uint64_t reserved_28_29:2;
+		uint64_t rint_27:1;
+		uint64_t rint_26:1;
+		uint64_t rint_25:1;
+		uint64_t rint_24:1;
+		uint64_t asx1:1;
+		uint64_t asx0:1;
+		uint64_t rint_21:1;
+		uint64_t pip:1;
+		uint64_t spx1:1;
+		uint64_t spx0:1;
+		uint64_t lmc:1;
+		uint64_t l2c:1;
+		uint64_t rint_15:1;
+		uint64_t reserved_13_14:2;
+		uint64_t pow:1;
+		uint64_t tim:1;
+		uint64_t pko:1;
+		uint64_t ipd:1;
+		uint64_t rint_8:1;
+		uint64_t zip:1;
+		uint64_t dfa:1;
+		uint64_t fpa:1;
+		uint64_t key:1;
+		uint64_t npi:1;
+		uint64_t gmx1:1;
+		uint64_t gmx0:1;
+		uint64_t mio:1;
+	} s;
+	struct cvmx_npi_rsl_int_blocks_cn30xx {
+		uint64_t reserved_32_63:32;
+		uint64_t rint_31:1;
+		uint64_t iob:1;
+		uint64_t rint_29:1;
+		uint64_t rint_28:1;
+		uint64_t rint_27:1;
+		uint64_t rint_26:1;
+		uint64_t rint_25:1;
+		uint64_t rint_24:1;
+		uint64_t asx1:1;
+		uint64_t asx0:1;
+		uint64_t rint_21:1;
+		uint64_t pip:1;
+		uint64_t spx1:1;
+		uint64_t spx0:1;
+		uint64_t lmc:1;
+		uint64_t l2c:1;
+		uint64_t rint_15:1;
+		uint64_t rint_14:1;
+		uint64_t usb:1;
+		uint64_t pow:1;
+		uint64_t tim:1;
+		uint64_t pko:1;
+		uint64_t ipd:1;
+		uint64_t rint_8:1;
+		uint64_t zip:1;
+		uint64_t dfa:1;
+		uint64_t fpa:1;
+		uint64_t key:1;
+		uint64_t npi:1;
+		uint64_t gmx1:1;
+		uint64_t gmx0:1;
+		uint64_t mio:1;
+	} cn30xx;
+	struct cvmx_npi_rsl_int_blocks_cn38xx {
+		uint64_t reserved_32_63:32;
+		uint64_t rint_31:1;
+		uint64_t iob:1;
+		uint64_t rint_29:1;
+		uint64_t rint_28:1;
+		uint64_t rint_27:1;
+		uint64_t rint_26:1;
+		uint64_t rint_25:1;
+		uint64_t rint_24:1;
+		uint64_t asx1:1;
+		uint64_t asx0:1;
+		uint64_t rint_21:1;
+		uint64_t pip:1;
+		uint64_t spx1:1;
+		uint64_t spx0:1;
+		uint64_t lmc:1;
+		uint64_t l2c:1;
+		uint64_t rint_15:1;
+		uint64_t rint_14:1;
+		uint64_t rint_13:1;
+		uint64_t pow:1;
+		uint64_t tim:1;
+		uint64_t pko:1;
+		uint64_t ipd:1;
+		uint64_t rint_8:1;
+		uint64_t zip:1;
+		uint64_t dfa:1;
+		uint64_t fpa:1;
+		uint64_t key:1;
+		uint64_t npi:1;
+		uint64_t gmx1:1;
+		uint64_t gmx0:1;
+		uint64_t mio:1;
+	} cn38xx;
+	struct cvmx_npi_rsl_int_blocks_cn50xx {
+		uint64_t reserved_31_63:33;
+		uint64_t iob:1;
+		uint64_t lmc1:1;
+		uint64_t agl:1;
+		uint64_t reserved_24_27:4;
+		uint64_t asx1:1;
+		uint64_t asx0:1;
+		uint64_t reserved_21_21:1;
+		uint64_t pip:1;
+		uint64_t spx1:1;
+		uint64_t spx0:1;
+		uint64_t lmc:1;
+		uint64_t l2c:1;
+		uint64_t reserved_15_15:1;
+		uint64_t rad:1;
+		uint64_t usb:1;
+		uint64_t pow:1;
+		uint64_t tim:1;
+		uint64_t pko:1;
+		uint64_t ipd:1;
+		uint64_t reserved_8_8:1;
+		uint64_t zip:1;
+		uint64_t dfa:1;
+		uint64_t fpa:1;
+		uint64_t key:1;
+		uint64_t npi:1;
+		uint64_t gmx1:1;
+		uint64_t gmx0:1;
+		uint64_t mio:1;
+	} cn50xx;
+};
+
+union cvmx_pko_command_word0 {
+	uint64_t u64;
+	struct {
+		uint64_t total_bytes:16;
+		uint64_t segs:6;
+		uint64_t dontfree:1;
+		uint64_t ignore_i:1;
+		uint64_t ipoffp1:7;
+		uint64_t gather:1;
+		uint64_t rsp:1;
+		uint64_t wqp:1;
+		uint64_t n2:1;
+		uint64_t le:1;
+		uint64_t reg0:11;
+		uint64_t subone0:1;
+		uint64_t reg1:11;
+		uint64_t subone1:1;
+		uint64_t size0:2;
+		uint64_t size1:2;
+	} s;
+};
+
+union cvmx_ciu_timx {
+	uint64_t u64;
+	struct cvmx_ciu_timx_s {
+		uint64_t reserved_37_63:27;
+		uint64_t one_shot:1;
+		uint64_t len:36;
+	} s;
+};
+
+union cvmx_gmxx_rxx_rx_inbnd {
+	uint64_t u64;
+	struct cvmx_gmxx_rxx_rx_inbnd_s {
+		uint64_t status:1;
+		uint64_t speed:2;
+		uint64_t duplex:1;
+		uint64_t reserved_4_63:60;
+	} s;
+};
+
+static inline int32_t cvmx_fau_fetch_and_add32(enum cvmx_fau_reg_32 reg,
+					       int32_t value)
+{
+	return value;
+}
+
+static inline void cvmx_fau_atomic_add32(enum cvmx_fau_reg_32 reg,
+					 int32_t value)
+{ }
+
+static inline void cvmx_fau_atomic_write32(enum cvmx_fau_reg_32 reg,
+					   int32_t value)
+{ }
+
+static inline uint64_t cvmx_scratch_read64(uint64_t address)
+{
+	return 0;
+}
+
+static inline void cvmx_scratch_write64(uint64_t address, uint64_t value)
+{ }
+
+static inline int cvmx_wqe_get_grp(struct cvmx_wqe *work)
+{
+	return 0;
+}
+
+static inline void *cvmx_phys_to_ptr(uint64_t physical_address)
+{
+	return (void *)(uintptr_t)(physical_address);
+}
+
+static inline uint64_t cvmx_ptr_to_phys(void *ptr)
+{
+	return (unsigned long)ptr;
+}
+
+static inline int cvmx_helper_get_interface_num(int ipd_port)
+{
+	return ipd_port;
+}
+
+static inline int cvmx_helper_get_interface_index_num(int ipd_port)
+{
+	return ipd_port;
+}
+
+static inline void cvmx_fpa_enable(void)
+{ }
+
+static inline uint64_t cvmx_read_csr(uint64_t csr_addr)
+{
+	return 0;
+}
+
+static inline void cvmx_write_csr(uint64_t csr_addr, uint64_t val)
+{ }
+
+static inline int cvmx_helper_setup_red(int pass_thresh, int drop_thresh)
+{
+	return 0;
+}
+
+static inline void *cvmx_fpa_alloc(uint64_t pool)
+{
+	return NULL;
+}
+
+static inline void cvmx_fpa_free(void *ptr, uint64_t pool,
+				 uint64_t num_cache_lines)
+{ }
+
+static inline int octeon_is_simulation(void)
+{
+	return 1;
+}
+
+static inline void cvmx_pip_get_port_status(uint64_t port_num, uint64_t clear,
+					    cvmx_pip_port_status_t *status)
+{ }
+
+static inline void cvmx_pko_get_port_status(uint64_t port_num, uint64_t clear,
+					    cvmx_pko_port_status_t *status)
+{ }
+
+static inline cvmx_helper_interface_mode_t cvmx_helper_interface_get_mode(int
+								   interface)
+{
+	return 0;
+}
+
+static inline union cvmx_helper_link_info cvmx_helper_link_get(int ipd_port)
+{
+	union cvmx_helper_link_info ret = { .u64 = 0 };
+
+	return ret;
+}
+
+static inline int cvmx_helper_link_set(int ipd_port,
+				       union cvmx_helper_link_info link_info)
+{
+	return 0;
+}
+
+static inline int cvmx_helper_initialize_packet_io_global(void)
+{
+	return 0;
+}
+
+static inline int cvmx_helper_get_number_of_interfaces(void)
+{
+	return 2;
+}
+
+static inline int cvmx_helper_ports_on_interface(int interface)
+{
+	return 1;
+}
+
+static inline int cvmx_helper_get_ipd_port(int interface, int port)
+{
+	return 0;
+}
+
+static inline int cvmx_helper_ipd_and_packet_input_enable(void)
+{
+	return 0;
+}
+
+static inline void cvmx_ipd_disable(void)
+{ }
+
+static inline void cvmx_ipd_free_ptr(void)
+{ }
+
+static inline void cvmx_pko_disable(void)
+{ }
+
+static inline void cvmx_pko_shutdown(void)
+{ }
+
+static inline int cvmx_pko_get_base_queue_per_core(int port, int core)
+{
+	return port;
+}
+
+static inline int cvmx_pko_get_base_queue(int port)
+{
+	return port;
+}
+
+static inline int cvmx_pko_get_num_queues(int port)
+{
+	return port;
+}
+
+static inline unsigned int cvmx_get_core_num(void)
+{
+	return 0;
+}
+
+static inline void cvmx_pow_work_request_async_nocheck(int scr_addr,
+						       cvmx_pow_wait_t wait)
+{ }
+
+static inline void cvmx_pow_work_request_async(int scr_addr,
+					       cvmx_pow_wait_t wait)
+{ }
+
+static inline struct cvmx_wqe *cvmx_pow_work_response_async(int scr_addr)
+{
+	struct cvmx_wqe *wqe = (void *)(unsigned long)scr_addr;
+
+	return wqe;
+}
+
+static inline struct cvmx_wqe *cvmx_pow_work_request_sync(cvmx_pow_wait_t wait)
+{
+	return (void *)(unsigned long)wait;
+}
+
+static inline int cvmx_spi_restart_interface(int interface,
+					cvmx_spi_mode_t mode, int timeout)
+{
+	return 0;
+}
+
+static inline void cvmx_fau_async_fetch_and_add32(uint64_t scraddr,
+						  enum cvmx_fau_reg_32 reg,
+						  int32_t value)
+{ }
+
+static inline union cvmx_gmxx_rxx_rx_inbnd cvmx_spi4000_check_speed(
+	int interface,
+	int port)
+{
+	union cvmx_gmxx_rxx_rx_inbnd r;
+
+	r.u64 = 0;
+	return r;
+}
+
+static inline void cvmx_pko_send_packet_prepare(uint64_t port, uint64_t queue,
+						cvmx_pko_lock_t use_locking)
+{ }
+
+static inline cvmx_pko_status_t cvmx_pko_send_packet_finish(uint64_t port,
+		uint64_t queue, union cvmx_pko_command_word0 pko_command,
+		union cvmx_buf_ptr packet, cvmx_pko_lock_t use_locking)
+{
+	return 0;
+}
+
+static inline void cvmx_wqe_set_port(struct cvmx_wqe *work, int port)
+{ }
+
+static inline void cvmx_wqe_set_qos(struct cvmx_wqe *work, int qos)
+{ }
+
+static inline int cvmx_wqe_get_qos(struct cvmx_wqe *work)
+{
+	return 0;
+}
+
+static inline void cvmx_wqe_set_grp(struct cvmx_wqe *work, int grp)
+{ }
+
+static inline void cvmx_pow_work_submit(struct cvmx_wqe *wqp, uint32_t tag,
+					enum cvmx_pow_tag_type tag_type,
+					uint64_t qos, uint64_t grp)
+{ }
+
+#define CVMX_ASXX_RX_CLK_SETX(a, b)	((a) + (b))
+#define CVMX_ASXX_TX_CLK_SETX(a, b)	((a) + (b))
+#define CVMX_CIU_TIMX(a)		(a)
+#define CVMX_GMXX_RXX_ADR_CAM0(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CAM1(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CAM2(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CAM3(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CAM4(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_ADR_CAM5(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_FRM_CTL(a, b)	((a) + (b))
+#define CVMX_GMXX_RXX_INT_REG(a, b)	((a) + (b))
+#define CVMX_GMXX_SMACX(a, b)		((a) + (b))
+#define CVMX_PIP_PRT_TAGX(a)		(a)
+#define CVMX_POW_PP_GRP_MSKX(a)		(a)
+#define CVMX_POW_WQ_INT_THRX(a)		(a)
+#define CVMX_SPXX_INT_MSK(a)		(a)
+#define CVMX_SPXX_INT_REG(a)		(a)
+#define CVMX_SSO_PPX_GRP_MSK(a)		(a)
+#define CVMX_SSO_WQ_INT_THRX(a)		(a)
+#define CVMX_STXX_INT_MSK(a)		(a)
+#define CVMX_STXX_INT_REG(a)		(a)
diff --git a/drivers/staging/olpc_dcon/Kconfig b/drivers/staging/olpc_dcon/Kconfig
new file mode 100644
index 000000000..d0ba34cc3
--- /dev/null
+++ b/drivers/staging/olpc_dcon/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+config FB_OLPC_DCON
+	tristate "One Laptop Per Child Display CONtroller support"
+	depends on OLPC && FB && BROKEN
+	depends on I2C
+	depends on GPIO_CS5535 && ACPI
+	select BACKLIGHT_CLASS_DEVICE
+	help
+	  In order to support very low power operation, the XO laptop uses a
+	  secondary Display CONtroller, or DCON.  This secondary controller
+	  is present in the video pipeline between the primary display
+	  controller (integrate into the processor or chipset) and the LCD
+	  panel.  It allows the main processor/display controller to be
+	  completely powered off while still retaining an image on the display.
+	  This controller is only available on OLPC platforms.  Unless you have
+	  one of these platforms, you will want to say 'N'.
+
diff --git a/drivers/staging/olpc_dcon/Makefile b/drivers/staging/olpc_dcon/Makefile
new file mode 100644
index 000000000..734b2ce26
--- /dev/null
+++ b/drivers/staging/olpc_dcon/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0
+olpc-dcon-objs += olpc_dcon.o olpc_dcon_xo_1.o olpc_dcon_xo_1_5.o
+obj-$(CONFIG_FB_OLPC_DCON)	+= olpc-dcon.o
+
+
diff --git a/drivers/staging/olpc_dcon/TODO b/drivers/staging/olpc_dcon/TODO
new file mode 100644
index 000000000..7c263358b
--- /dev/null
+++ b/drivers/staging/olpc_dcon/TODO
@@ -0,0 +1,15 @@
+TODO:
+	- complete rewrite:
+	  1. The underlying fbdev drivers need to be converted into drm kernel
+	     modesetting drivers.
+	  2. The dcon low-power display mode can then be integrated using the
+	     drm damage tracking and self-refresh helpers.
+	  This bolted-on self-refresh support that digs around in fbdev
+	  internals, but isn't properly integrated, is not the correct solution.
+	- see if vx855 gpio API can be made similar enough to cs5535 so we can
+	  share more code
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+copy:
+	Daniel Drake <dsd@laptop.org>
+	Jens Frederich <jfrederich@gmail.com>
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
new file mode 100644
index 000000000..4fb9b9f10
--- /dev/null
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -0,0 +1,807 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Mainly by David Woodhouse, somewhat modified by Jordan Crouse
+ *
+ * Copyright © 2006-2007  Red Hat, Inc.
+ * Copyright © 2006-2007  Advanced Micro Devices, Inc.
+ * Copyright © 2009       VIA Technology, Inc.
+ * Copyright (c) 2010-2011  Andres Salomon <dilinger@queued.net>
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/fb.h>
+#include <linux/console.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/backlight.h>
+#include <linux/device.h>
+#include <linux/uaccess.h>
+#include <linux/ctype.h>
+#include <linux/panic_notifier.h>
+#include <linux/reboot.h>
+#include <linux/olpc-ec.h>
+#include <asm/tsc.h>
+#include <asm/olpc.h>
+
+#include "olpc_dcon.h"
+
+/* Module definitions */
+
+static ushort resumeline = 898;
+module_param(resumeline, ushort, 0444);
+
+static struct dcon_platform_data *pdata;
+
+/* I2C structures */
+
+/* Platform devices */
+static struct platform_device *dcon_device;
+
+static unsigned short normal_i2c[] = { 0x0d, I2C_CLIENT_END };
+
+static s32 dcon_write(struct dcon_priv *dcon, u8 reg, u16 val)
+{
+	return i2c_smbus_write_word_data(dcon->client, reg, val);
+}
+
+static s32 dcon_read(struct dcon_priv *dcon, u8 reg)
+{
+	return i2c_smbus_read_word_data(dcon->client, reg);
+}
+
+/* ===== API functions - these are called by a variety of users ==== */
+
+static int dcon_hw_init(struct dcon_priv *dcon, int is_init)
+{
+	u16 ver;
+	int rc = 0;
+
+	ver = dcon_read(dcon, DCON_REG_ID);
+	if ((ver >> 8) != 0xDC) {
+		pr_err("DCON ID not 0xDCxx: 0x%04x instead.\n", ver);
+		rc = -ENXIO;
+		goto err;
+	}
+
+	if (is_init) {
+		pr_info("Discovered DCON version %x\n", ver & 0xFF);
+		rc = pdata->init(dcon);
+		if (rc != 0) {
+			pr_err("Unable to init.\n");
+			goto err;
+		}
+	}
+
+	if (ver < 0xdc02) {
+		dev_err(&dcon->client->dev,
+			"DCON v1 is unsupported, giving up..\n");
+		rc = -ENODEV;
+		goto err;
+	}
+
+	/* SDRAM setup/hold time */
+	dcon_write(dcon, 0x3a, 0xc040);
+	dcon_write(dcon, DCON_REG_MEM_OPT_A, 0x0000);  /* clear option bits */
+	dcon_write(dcon, DCON_REG_MEM_OPT_A,
+		   MEM_DLL_CLOCK_DELAY | MEM_POWER_DOWN);
+	dcon_write(dcon, DCON_REG_MEM_OPT_B, MEM_SOFT_RESET);
+
+	/* Colour swizzle, AA, no passthrough, backlight */
+	if (is_init) {
+		dcon->disp_mode = MODE_PASSTHRU | MODE_BL_ENABLE |
+				MODE_CSWIZZLE | MODE_COL_AA;
+	}
+	dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode);
+
+	/* Set the scanline to interrupt on during resume */
+	dcon_write(dcon, DCON_REG_SCAN_INT, resumeline);
+
+err:
+	return rc;
+}
+
+/*
+ * The smbus doesn't always come back due to what is believed to be
+ * hardware (power rail) bugs.  For older models where this is known to
+ * occur, our solution is to attempt to wait for the bus to stabilize;
+ * if it doesn't happen, cut power to the dcon, repower it, and wait
+ * for the bus to stabilize.  Rinse, repeat until we have a working
+ * smbus.  For newer models, we simply BUG(); we want to know if this
+ * still happens despite the power fixes that have been made!
+ */
+static int dcon_bus_stabilize(struct dcon_priv *dcon, int is_powered_down)
+{
+	unsigned long timeout;
+	u8 pm;
+	int x;
+
+power_up:
+	if (is_powered_down) {
+		pm = 1;
+		x = olpc_ec_cmd(EC_DCON_POWER_MODE, &pm, 1, NULL, 0);
+		if (x) {
+			pr_warn("unable to force dcon to power up: %d!\n", x);
+			return x;
+		}
+		usleep_range(10000, 11000);  /* we'll be conservative */
+	}
+
+	pdata->bus_stabilize_wiggle();
+
+	for (x = -1, timeout = 50; timeout && x < 0; timeout--) {
+		usleep_range(1000, 1100);
+		x = dcon_read(dcon, DCON_REG_ID);
+	}
+	if (x < 0) {
+		pr_err("unable to stabilize dcon's smbus, reasserting power and praying.\n");
+		BUG_ON(olpc_board_at_least(olpc_board(0xc2)));
+		pm = 0;
+		olpc_ec_cmd(EC_DCON_POWER_MODE, &pm, 1, NULL, 0);
+		msleep(100);
+		is_powered_down = 1;
+		goto power_up;	/* argh, stupid hardware.. */
+	}
+
+	if (is_powered_down)
+		return dcon_hw_init(dcon, 0);
+	return 0;
+}
+
+static void dcon_set_backlight(struct dcon_priv *dcon, u8 level)
+{
+	dcon->bl_val = level;
+	dcon_write(dcon, DCON_REG_BRIGHT, dcon->bl_val);
+
+	/* Purposely turn off the backlight when we go to level 0 */
+	if (dcon->bl_val == 0) {
+		dcon->disp_mode &= ~MODE_BL_ENABLE;
+		dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode);
+	} else if (!(dcon->disp_mode & MODE_BL_ENABLE)) {
+		dcon->disp_mode |= MODE_BL_ENABLE;
+		dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode);
+	}
+}
+
+/* Set the output type to either color or mono */
+static int dcon_set_mono_mode(struct dcon_priv *dcon, bool enable_mono)
+{
+	if (dcon->mono == enable_mono)
+		return 0;
+
+	dcon->mono = enable_mono;
+
+	if (enable_mono) {
+		dcon->disp_mode &= ~(MODE_CSWIZZLE | MODE_COL_AA);
+		dcon->disp_mode |= MODE_MONO_LUMA;
+	} else {
+		dcon->disp_mode &= ~(MODE_MONO_LUMA);
+		dcon->disp_mode |= MODE_CSWIZZLE | MODE_COL_AA;
+	}
+
+	dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode);
+	return 0;
+}
+
+/* For now, this will be really stupid - we need to address how
+ * DCONLOAD works in a sleep and account for it accordingly
+ */
+
+static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
+{
+	int x;
+
+	/* Turn off the backlight and put the DCON to sleep */
+
+	if (dcon->asleep == sleep)
+		return;
+
+	if (!olpc_board_at_least(olpc_board(0xc2)))
+		return;
+
+	if (sleep) {
+		u8 pm = 0;
+
+		x = olpc_ec_cmd(EC_DCON_POWER_MODE, &pm, 1, NULL, 0);
+		if (x)
+			pr_warn("unable to force dcon to power down: %d!\n", x);
+		else
+			dcon->asleep = sleep;
+	} else {
+		/* Only re-enable the backlight if the backlight value is set */
+		if (dcon->bl_val != 0)
+			dcon->disp_mode |= MODE_BL_ENABLE;
+		x = dcon_bus_stabilize(dcon, 1);
+		if (x)
+			pr_warn("unable to reinit dcon hardware: %d!\n", x);
+		else
+			dcon->asleep = sleep;
+
+		/* Restore backlight */
+		dcon_set_backlight(dcon, dcon->bl_val);
+	}
+
+	/* We should turn off some stuff in the framebuffer - but what? */
+}
+
+/* the DCON seems to get confused if we change DCONLOAD too
+ * frequently -- i.e., approximately faster than frame time.
+ * normally we don't change it this fast, so in general we won't
+ * delay here.
+ */
+static void dcon_load_holdoff(struct dcon_priv *dcon)
+{
+	ktime_t delta_t, now;
+
+	while (1) {
+		now = ktime_get();
+		delta_t = ktime_sub(now, dcon->load_time);
+		if (ktime_to_ns(delta_t) > NSEC_PER_MSEC * 20)
+			break;
+		mdelay(4);
+	}
+}
+
+static bool dcon_blank_fb(struct dcon_priv *dcon, bool blank)
+{
+	int err;
+
+	console_lock();
+	lock_fb_info(dcon->fbinfo);
+
+	dcon->ignore_fb_events = true;
+	err = fb_blank(dcon->fbinfo,
+		       blank ? FB_BLANK_POWERDOWN : FB_BLANK_UNBLANK);
+	dcon->ignore_fb_events = false;
+	unlock_fb_info(dcon->fbinfo);
+	console_unlock();
+
+	if (err) {
+		dev_err(&dcon->client->dev, "couldn't %sblank framebuffer\n",
+			blank ? "" : "un");
+		return false;
+	}
+	return true;
+}
+
+/* Set the source of the display (CPU or DCON) */
+static void dcon_source_switch(struct work_struct *work)
+{
+	struct dcon_priv *dcon = container_of(work, struct dcon_priv,
+			switch_source);
+	int source = dcon->pending_src;
+
+	if (dcon->curr_src == source)
+		return;
+
+	dcon_load_holdoff(dcon);
+
+	dcon->switched = false;
+
+	switch (source) {
+	case DCON_SOURCE_CPU:
+		pr_info("%s to CPU\n", __func__);
+		/* Enable the scanline interrupt bit */
+		if (dcon_write(dcon, DCON_REG_MODE,
+			       dcon->disp_mode | MODE_SCAN_INT))
+			pr_err("couldn't enable scanline interrupt!\n");
+		else
+			/* Wait up to one second for the scanline interrupt */
+			wait_event_timeout(dcon->waitq, dcon->switched, HZ);
+
+		if (!dcon->switched)
+			pr_err("Timeout entering CPU mode; expect a screen glitch.\n");
+
+		/* Turn off the scanline interrupt */
+		if (dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode))
+			pr_err("couldn't disable scanline interrupt!\n");
+
+		/*
+		 * Ideally we'd like to disable interrupts here so that the
+		 * fb unblanking and DCON turn on happen at a known time value;
+		 * however, we can't do that right now with fb_blank
+		 * messing with semaphores.
+		 *
+		 * For now, we just hope..
+		 */
+		if (!dcon_blank_fb(dcon, false)) {
+			pr_err("Failed to enter CPU mode\n");
+			dcon->pending_src = DCON_SOURCE_DCON;
+			return;
+		}
+
+		/* And turn off the DCON */
+		pdata->set_dconload(1);
+		dcon->load_time = ktime_get();
+
+		pr_info("The CPU has control\n");
+		break;
+	case DCON_SOURCE_DCON:
+	{
+		ktime_t delta_t;
+
+		pr_info("%s to DCON\n", __func__);
+
+		/* Clear DCONLOAD - this implies that the DCON is in control */
+		pdata->set_dconload(0);
+		dcon->load_time = ktime_get();
+
+		wait_event_timeout(dcon->waitq, dcon->switched, HZ / 2);
+
+		if (!dcon->switched) {
+			pr_err("Timeout entering DCON mode; expect a screen glitch.\n");
+		} else {
+			/* sometimes the DCON doesn't follow its own rules,
+			 * and doesn't wait for two vsync pulses before
+			 * ack'ing the frame load with an IRQ.  the result
+			 * is that the display shows the *previously*
+			 * loaded frame.  we can detect this by looking at
+			 * the time between asserting DCONLOAD and the IRQ --
+			 * if it's less than 20msec, then the DCON couldn't
+			 * have seen two VSYNC pulses.  in that case we
+			 * deassert and reassert, and hope for the best.
+			 * see http://dev.laptop.org/ticket/9664
+			 */
+			delta_t = ktime_sub(dcon->irq_time, dcon->load_time);
+			if (dcon->switched && ktime_to_ns(delta_t)
+			    < NSEC_PER_MSEC * 20) {
+				pr_err("missed loading, retrying\n");
+				pdata->set_dconload(1);
+				mdelay(41);
+				pdata->set_dconload(0);
+				dcon->load_time = ktime_get();
+				mdelay(41);
+			}
+		}
+
+		dcon_blank_fb(dcon, true);
+		pr_info("The DCON has control\n");
+		break;
+	}
+	default:
+		BUG();
+	}
+
+	dcon->curr_src = source;
+}
+
+static void dcon_set_source(struct dcon_priv *dcon, int arg)
+{
+	if (dcon->pending_src == arg)
+		return;
+
+	dcon->pending_src = arg;
+
+	if (dcon->curr_src != arg)
+		schedule_work(&dcon->switch_source);
+}
+
+static void dcon_set_source_sync(struct dcon_priv *dcon, int arg)
+{
+	dcon_set_source(dcon, arg);
+	flush_work(&dcon->switch_source);
+}
+
+static ssize_t dcon_mode_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%4.4X\n", dcon->disp_mode);
+}
+
+static ssize_t dcon_sleep_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", dcon->asleep);
+}
+
+static ssize_t dcon_freeze_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", dcon->curr_src == DCON_SOURCE_DCON ? 1 : 0);
+}
+
+static ssize_t dcon_mono_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", dcon->mono);
+}
+
+static ssize_t dcon_resumeline_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	return sprintf(buf, "%d\n", resumeline);
+}
+
+static ssize_t dcon_mono_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	unsigned long enable_mono;
+	int rc;
+
+	rc = kstrtoul(buf, 10, &enable_mono);
+	if (rc)
+		return rc;
+
+	dcon_set_mono_mode(dev_get_drvdata(dev), enable_mono ? true : false);
+
+	return count;
+}
+
+static ssize_t dcon_freeze_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct dcon_priv *dcon = dev_get_drvdata(dev);
+	unsigned long output;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &output);
+	if (ret)
+		return ret;
+
+	switch (output) {
+	case 0:
+		dcon_set_source(dcon, DCON_SOURCE_CPU);
+		break;
+	case 1:
+		dcon_set_source_sync(dcon, DCON_SOURCE_DCON);
+		break;
+	case 2:  /* normally unused */
+		dcon_set_source(dcon, DCON_SOURCE_DCON);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static ssize_t dcon_resumeline_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	unsigned short rl;
+	int rc;
+
+	rc = kstrtou16(buf, 10, &rl);
+	if (rc)
+		return rc;
+
+	resumeline = rl;
+	dcon_write(dev_get_drvdata(dev), DCON_REG_SCAN_INT, resumeline);
+
+	return count;
+}
+
+static ssize_t dcon_sleep_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	unsigned long output;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &output);
+	if (ret)
+		return ret;
+
+	dcon_sleep(dev_get_drvdata(dev), output ? true : false);
+	return count;
+}
+
+static struct device_attribute dcon_device_files[] = {
+	__ATTR(mode, 0444, dcon_mode_show, NULL),
+	__ATTR(sleep, 0644, dcon_sleep_show, dcon_sleep_store),
+	__ATTR(freeze, 0644, dcon_freeze_show, dcon_freeze_store),
+	__ATTR(monochrome, 0644, dcon_mono_show, dcon_mono_store),
+	__ATTR(resumeline, 0644, dcon_resumeline_show, dcon_resumeline_store),
+};
+
+static int dcon_bl_update(struct backlight_device *dev)
+{
+	struct dcon_priv *dcon = bl_get_data(dev);
+	u8 level = backlight_get_brightness(dev) & 0x0F;
+
+	if (level != dcon->bl_val)
+		dcon_set_backlight(dcon, level);
+
+	/* power down the DCON when the screen is blanked */
+	if (!dcon->ignore_fb_events)
+		dcon_sleep(dcon, !!(dev->props.state & BL_CORE_FBBLANK));
+
+	return 0;
+}
+
+static int dcon_bl_get(struct backlight_device *dev)
+{
+	struct dcon_priv *dcon = bl_get_data(dev);
+
+	return dcon->bl_val;
+}
+
+static const struct backlight_ops dcon_bl_ops = {
+	.update_status = dcon_bl_update,
+	.get_brightness = dcon_bl_get,
+};
+
+static struct backlight_properties dcon_bl_props = {
+	.max_brightness = 15,
+	.type = BACKLIGHT_RAW,
+	.power = FB_BLANK_UNBLANK,
+};
+
+static int dcon_reboot_notify(struct notifier_block *nb,
+			      unsigned long foo, void *bar)
+{
+	struct dcon_priv *dcon = container_of(nb, struct dcon_priv, reboot_nb);
+
+	if (!dcon || !dcon->client)
+		return NOTIFY_DONE;
+
+	/* Turn off the DCON. Entirely. */
+	dcon_write(dcon, DCON_REG_MODE, 0x39);
+	dcon_write(dcon, DCON_REG_MODE, 0x32);
+	return NOTIFY_DONE;
+}
+
+static int unfreeze_on_panic(struct notifier_block *nb,
+			     unsigned long e, void *p)
+{
+	pdata->set_dconload(1);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block dcon_panic_nb = {
+	.notifier_call = unfreeze_on_panic,
+};
+
+static int dcon_detect(struct i2c_client *client, struct i2c_board_info *info)
+{
+	strscpy(info->type, "olpc_dcon", I2C_NAME_SIZE);
+
+	return 0;
+}
+
+static int dcon_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct dcon_priv *dcon;
+	int rc, i, j;
+
+	if (!pdata)
+		return -ENXIO;
+
+	dcon = kzalloc(sizeof(*dcon), GFP_KERNEL);
+	if (!dcon)
+		return -ENOMEM;
+
+	dcon->client = client;
+	init_waitqueue_head(&dcon->waitq);
+	INIT_WORK(&dcon->switch_source, dcon_source_switch);
+	dcon->reboot_nb.notifier_call = dcon_reboot_notify;
+	dcon->reboot_nb.priority = -1;
+
+	i2c_set_clientdata(client, dcon);
+
+	if (num_registered_fb < 1) {
+		dev_err(&client->dev, "DCON driver requires a registered fb\n");
+		rc = -EIO;
+		goto einit;
+	}
+	dcon->fbinfo = registered_fb[0];
+
+	rc = dcon_hw_init(dcon, 1);
+	if (rc)
+		goto einit;
+
+	/* Add the DCON device */
+
+	dcon_device = platform_device_alloc("dcon", -1);
+
+	if (!dcon_device) {
+		pr_err("Unable to create the DCON device\n");
+		rc = -ENOMEM;
+		goto eirq;
+	}
+	rc = platform_device_add(dcon_device);
+	platform_set_drvdata(dcon_device, dcon);
+
+	if (rc) {
+		pr_err("Unable to add the DCON device\n");
+		goto edev;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(dcon_device_files); i++) {
+		rc = device_create_file(&dcon_device->dev,
+					&dcon_device_files[i]);
+		if (rc) {
+			dev_err(&dcon_device->dev, "Cannot create sysfs file\n");
+			goto ecreate;
+		}
+	}
+
+	dcon->bl_val = dcon_read(dcon, DCON_REG_BRIGHT) & 0x0F;
+
+	/* Add the backlight device for the DCON */
+	dcon_bl_props.brightness = dcon->bl_val;
+	dcon->bl_dev = backlight_device_register("dcon-bl", &dcon_device->dev,
+						 dcon, &dcon_bl_ops,
+						 &dcon_bl_props);
+	if (IS_ERR(dcon->bl_dev)) {
+		dev_err(&client->dev, "cannot register backlight dev (%ld)\n",
+			PTR_ERR(dcon->bl_dev));
+		dcon->bl_dev = NULL;
+	}
+
+	register_reboot_notifier(&dcon->reboot_nb);
+	atomic_notifier_chain_register(&panic_notifier_list, &dcon_panic_nb);
+
+	return 0;
+
+ ecreate:
+	for (j = 0; j < i; j++)
+		device_remove_file(&dcon_device->dev, &dcon_device_files[j]);
+	platform_device_del(dcon_device);
+ edev:
+	platform_device_put(dcon_device);
+	dcon_device = NULL;
+ eirq:
+	free_irq(DCON_IRQ, dcon);
+ einit:
+	kfree(dcon);
+	return rc;
+}
+
+static void dcon_remove(struct i2c_client *client)
+{
+	struct dcon_priv *dcon = i2c_get_clientdata(client);
+
+	unregister_reboot_notifier(&dcon->reboot_nb);
+	atomic_notifier_chain_unregister(&panic_notifier_list, &dcon_panic_nb);
+
+	free_irq(DCON_IRQ, dcon);
+
+	backlight_device_unregister(dcon->bl_dev);
+
+	if (dcon_device)
+		platform_device_unregister(dcon_device);
+	cancel_work_sync(&dcon->switch_source);
+
+	kfree(dcon);
+}
+
+#ifdef CONFIG_PM
+static int dcon_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct dcon_priv *dcon = i2c_get_clientdata(client);
+
+	if (!dcon->asleep) {
+		/* Set up the DCON to have the source */
+		dcon_set_source_sync(dcon, DCON_SOURCE_DCON);
+	}
+
+	return 0;
+}
+
+static int dcon_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct dcon_priv *dcon = i2c_get_clientdata(client);
+
+	if (!dcon->asleep) {
+		dcon_bus_stabilize(dcon, 0);
+		dcon_set_source(dcon, DCON_SOURCE_CPU);
+	}
+
+	return 0;
+}
+
+#else
+
+#define dcon_suspend NULL
+#define dcon_resume NULL
+
+#endif /* CONFIG_PM */
+
+irqreturn_t dcon_interrupt(int irq, void *id)
+{
+	struct dcon_priv *dcon = id;
+	u8 status;
+
+	if (pdata->read_status(&status))
+		return IRQ_NONE;
+
+	switch (status & 3) {
+	case 3:
+		pr_debug("DCONLOAD_MISSED interrupt\n");
+		break;
+
+	case 2:	/* switch to DCON mode */
+	case 1: /* switch to CPU mode */
+		dcon->switched = true;
+		dcon->irq_time = ktime_get();
+		wake_up(&dcon->waitq);
+		break;
+
+	case 0:
+		/* workaround resume case:  the DCON (on 1.5) doesn't
+		 * ever assert status 0x01 when switching to CPU mode
+		 * during resume.  this is because DCONLOAD is de-asserted
+		 * _immediately_ upon exiting S3, so the actual release
+		 * of the DCON happened long before this point.
+		 * see http://dev.laptop.org/ticket/9869
+		 */
+		if (dcon->curr_src != dcon->pending_src && !dcon->switched) {
+			dcon->switched = true;
+			dcon->irq_time = ktime_get();
+			wake_up(&dcon->waitq);
+			pr_debug("switching w/ status 0/0\n");
+		} else {
+			pr_debug("scanline interrupt w/CPU\n");
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct dev_pm_ops dcon_pm_ops = {
+	.suspend = dcon_suspend,
+	.resume = dcon_resume,
+};
+
+static const struct i2c_device_id dcon_idtable[] = {
+	{ "olpc_dcon",  0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, dcon_idtable);
+
+static struct i2c_driver dcon_driver = {
+	.driver = {
+		.name	= "olpc_dcon",
+		.pm = &dcon_pm_ops,
+	},
+	.class = I2C_CLASS_DDC | I2C_CLASS_HWMON,
+	.id_table = dcon_idtable,
+	.probe = dcon_probe,
+	.remove = dcon_remove,
+	.detect = dcon_detect,
+	.address_list = normal_i2c,
+};
+
+static int __init olpc_dcon_init(void)
+{
+	/* XO-1.5 */
+	if (olpc_board_at_least(olpc_board(0xd0)))
+		pdata = &dcon_pdata_xo_1_5;
+	else
+		pdata = &dcon_pdata_xo_1;
+
+	return i2c_add_driver(&dcon_driver);
+}
+
+static void __exit olpc_dcon_exit(void)
+{
+	i2c_del_driver(&dcon_driver);
+}
+
+module_init(olpc_dcon_init);
+module_exit(olpc_dcon_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.h b/drivers/staging/olpc_dcon/olpc_dcon.h
new file mode 100644
index 000000000..41bd1360b
--- /dev/null
+++ b/drivers/staging/olpc_dcon/olpc_dcon.h
@@ -0,0 +1,112 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef OLPC_DCON_H_
+#define OLPC_DCON_H_
+
+#include <linux/notifier.h>
+#include <linux/workqueue.h>
+
+/* DCON registers */
+
+#define DCON_REG_ID		 0
+#define DCON_REG_MODE		 1
+
+#define MODE_PASSTHRU	BIT(0)
+#define MODE_SLEEP	BIT(1)
+#define MODE_SLEEP_AUTO	BIT(2)
+#define MODE_BL_ENABLE	BIT(3)
+#define MODE_BLANK	BIT(4)
+#define MODE_CSWIZZLE	BIT(5)
+#define MODE_COL_AA	BIT(6)
+#define MODE_MONO_LUMA	BIT(7)
+#define MODE_SCAN_INT	BIT(8)
+#define MODE_CLOCKDIV	BIT(9)
+#define MODE_DEBUG	BIT(14)
+#define MODE_SELFTEST	BIT(15)
+
+#define DCON_REG_HRES		0x2
+#define DCON_REG_HTOTAL		0x3
+#define DCON_REG_HSYNC_WIDTH	0x4
+#define DCON_REG_VRES		0x5
+#define DCON_REG_VTOTAL		0x6
+#define DCON_REG_VSYNC_WIDTH	0x7
+#define DCON_REG_TIMEOUT	0x8
+#define DCON_REG_SCAN_INT	0x9
+#define DCON_REG_BRIGHT		0xa
+#define DCON_REG_MEM_OPT_A	0x41
+#define DCON_REG_MEM_OPT_B	0x42
+
+/* Load Delay Locked Loop (DLL) settings for clock delay */
+#define MEM_DLL_CLOCK_DELAY	BIT(0)
+/* Memory controller power down function */
+#define MEM_POWER_DOWN		BIT(8)
+/* Memory controller software reset */
+#define MEM_SOFT_RESET		BIT(0)
+
+/* Status values */
+
+#define DCONSTAT_SCANINT	0
+#define DCONSTAT_SCANINT_DCON	1
+#define DCONSTAT_DISPLAYLOAD	2
+#define DCONSTAT_MISSED		3
+
+/* Source values */
+
+#define DCON_SOURCE_DCON        0
+#define DCON_SOURCE_CPU         1
+
+/* Interrupt */
+#define DCON_IRQ                6
+
+struct dcon_priv {
+	struct i2c_client *client;
+	struct fb_info *fbinfo;
+	struct backlight_device *bl_dev;
+
+	wait_queue_head_t waitq;
+	struct work_struct switch_source;
+	struct notifier_block reboot_nb;
+
+	/* Shadow register for the DCON_REG_MODE register */
+	u8 disp_mode;
+
+	/* The current backlight value - this saves us some smbus traffic */
+	u8 bl_val;
+
+	/* Current source, initialized at probe time */
+	int curr_src;
+
+	/* Desired source */
+	int pending_src;
+
+	/* Variables used during switches */
+	bool switched;
+	ktime_t irq_time;
+	ktime_t load_time;
+
+	/* Current output type; true == mono, false == color */
+	bool mono;
+	bool asleep;
+	/* This get set while controlling fb blank state from the driver */
+	bool ignore_fb_events;
+};
+
+struct dcon_platform_data {
+	int (*init)(struct dcon_priv *dcon);
+	void (*bus_stabilize_wiggle)(void);
+	void (*set_dconload)(int load);
+	int (*read_status)(u8 *status);
+};
+
+struct dcon_gpio {
+	const char *name;
+	unsigned long flags;
+};
+
+#include <linux/interrupt.h>
+
+irqreturn_t dcon_interrupt(int irq, void *id);
+
+extern struct dcon_platform_data dcon_pdata_xo_1;
+extern struct dcon_platform_data dcon_pdata_xo_1_5;
+
+#endif
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
new file mode 100644
index 000000000..02c059897
--- /dev/null
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
@@ -0,0 +1,201 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Mainly by David Woodhouse, somewhat modified by Jordan Crouse
+ *
+ * Copyright © 2006-2007  Red Hat, Inc.
+ * Copyright © 2006-2007  Advanced Micro Devices, Inc.
+ * Copyright © 2009       VIA Technology, Inc.
+ * Copyright (c) 2010  Andres Salomon <dilinger@queued.net>
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/cs5535.h>
+#include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <asm/olpc.h>
+
+#include "olpc_dcon.h"
+
+enum dcon_gpios {
+	OLPC_DCON_STAT0,
+	OLPC_DCON_STAT1,
+	OLPC_DCON_IRQ,
+	OLPC_DCON_LOAD,
+	OLPC_DCON_BLANK,
+};
+
+static const struct dcon_gpio gpios_asis[] = {
+	[OLPC_DCON_STAT0] = { .name = "dcon_stat0", .flags = GPIOD_ASIS },
+	[OLPC_DCON_STAT1] = { .name = "dcon_stat1", .flags = GPIOD_ASIS },
+	[OLPC_DCON_IRQ] = { .name = "dcon_irq", .flags = GPIOD_ASIS },
+	[OLPC_DCON_LOAD] = { .name = "dcon_load", .flags = GPIOD_ASIS },
+	[OLPC_DCON_BLANK] = { .name = "dcon_blank", .flags = GPIOD_ASIS },
+};
+
+static struct gpio_desc *gpios[5];
+
+static int dcon_init_xo_1(struct dcon_priv *dcon)
+{
+	unsigned char lob;
+	int ret, i;
+	const struct dcon_gpio *pin = &gpios_asis[0];
+
+	for (i = 0; i < ARRAY_SIZE(gpios_asis); i++) {
+		gpios[i] = devm_gpiod_get(&dcon->client->dev, pin[i].name,
+					  pin[i].flags);
+		if (IS_ERR(gpios[i])) {
+			ret = PTR_ERR(gpios[i]);
+			pr_err("failed to request %s GPIO: %d\n", pin[i].name,
+			       ret);
+			return ret;
+		}
+	}
+
+	/* Turn off the event enable for GPIO7 just to be safe */
+	cs5535_gpio_clear(OLPC_GPIO_DCON_IRQ, GPIO_EVENTS_ENABLE);
+
+	/*
+	 * Determine the current state by reading the GPIO bit; earlier
+	 * stages of the boot process have established the state.
+	 *
+	 * Note that we read GPIO_OUTPUT_VAL rather than GPIO_READ_BACK here;
+	 * this is because OFW will disable input for the pin and set a value..
+	 * READ_BACK will only contain a valid value if input is enabled and
+	 * then a value is set.  So, future readings of the pin can use
+	 * READ_BACK, but the first one cannot.  Awesome, huh?
+	 */
+	dcon->curr_src = cs5535_gpio_isset(OLPC_GPIO_DCON_LOAD, GPIO_OUTPUT_VAL)
+		? DCON_SOURCE_CPU
+		: DCON_SOURCE_DCON;
+	dcon->pending_src = dcon->curr_src;
+
+	/* Set the directions for the GPIO pins */
+	gpiod_direction_input(gpios[OLPC_DCON_STAT0]);
+	gpiod_direction_input(gpios[OLPC_DCON_STAT1]);
+	gpiod_direction_input(gpios[OLPC_DCON_IRQ]);
+	gpiod_direction_input(gpios[OLPC_DCON_BLANK]);
+	gpiod_direction_output(gpios[OLPC_DCON_LOAD],
+			       dcon->curr_src == DCON_SOURCE_CPU);
+
+	/* Set up the interrupt mappings */
+
+	/* Set the IRQ to pair 2 */
+	cs5535_gpio_setup_event(OLPC_GPIO_DCON_IRQ, 2, 0);
+
+	/* Enable group 2 to trigger the DCON interrupt */
+	cs5535_gpio_set_irq(2, DCON_IRQ);
+
+	/* Select edge level for interrupt (in PIC) */
+	lob = inb(0x4d0);
+	lob &= ~(1 << DCON_IRQ);
+	outb(lob, 0x4d0);
+
+	/* Register the interrupt handler */
+	if (request_irq(DCON_IRQ, &dcon_interrupt, 0, "DCON", dcon)) {
+		pr_err("failed to request DCON's irq\n");
+		return -EIO;
+	}
+
+	/* Clear INV_EN for GPIO7 (DCONIRQ) */
+	cs5535_gpio_clear(OLPC_GPIO_DCON_IRQ, GPIO_INPUT_INVERT);
+
+	/* Enable filter for GPIO12 (DCONBLANK) */
+	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_INPUT_FILTER);
+
+	/* Disable filter for GPIO7 */
+	cs5535_gpio_clear(OLPC_GPIO_DCON_IRQ, GPIO_INPUT_FILTER);
+
+	/* Disable event counter for GPIO7 (DCONIRQ) and GPIO12 (DCONBLANK) */
+	cs5535_gpio_clear(OLPC_GPIO_DCON_IRQ, GPIO_INPUT_EVENT_COUNT);
+	cs5535_gpio_clear(OLPC_GPIO_DCON_BLANK, GPIO_INPUT_EVENT_COUNT);
+
+	/* Add GPIO12 to the Filter Event Pair #7 */
+	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_FE7_SEL);
+
+	/* Turn off negative Edge Enable for GPIO12 */
+	cs5535_gpio_clear(OLPC_GPIO_DCON_BLANK, GPIO_NEGATIVE_EDGE_EN);
+
+	/* Enable negative Edge Enable for GPIO7 */
+	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_NEGATIVE_EDGE_EN);
+
+	/* Zero the filter amount for Filter Event Pair #7 */
+	cs5535_gpio_set(0, GPIO_FLTR7_AMOUNT);
+
+	/* Clear the negative edge status for GPIO7 and GPIO12 */
+	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_NEGATIVE_EDGE_STS);
+	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_NEGATIVE_EDGE_STS);
+
+	/* FIXME:  Clear the positive status as well, just to be sure */
+	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_POSITIVE_EDGE_STS);
+	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_POSITIVE_EDGE_STS);
+
+	/* Enable events for GPIO7 (DCONIRQ) and GPIO12 (DCONBLANK) */
+	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_EVENTS_ENABLE);
+	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_EVENTS_ENABLE);
+
+	return 0;
+}
+
+static void dcon_wiggle_xo_1(void)
+{
+	int x;
+
+	/*
+	 * According to HiMax, when powering the DCON up we should hold
+	 * SMB_DATA high for 8 SMB_CLK cycles.  This will force the DCON
+	 * state machine to reset to a (sane) initial state.  Mitch Bradley
+	 * did some testing and discovered that holding for 16 SMB_CLK cycles
+	 * worked a lot more reliably, so that's what we do here.
+	 *
+	 * According to the cs5536 spec, to set GPIO14 to SMB_CLK we must
+	 * simultaneously set AUX1 IN/OUT to GPIO14; ditto for SMB_DATA and
+	 * GPIO15.
+	 */
+	cs5535_gpio_set(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_VAL);
+	cs5535_gpio_set(OLPC_GPIO_SMB_DATA, GPIO_OUTPUT_VAL);
+	cs5535_gpio_set(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_ENABLE);
+	cs5535_gpio_set(OLPC_GPIO_SMB_DATA, GPIO_OUTPUT_ENABLE);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_AUX1);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_DATA, GPIO_OUTPUT_AUX1);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_AUX2);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_DATA, GPIO_OUTPUT_AUX2);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_CLK, GPIO_INPUT_AUX1);
+	cs5535_gpio_clear(OLPC_GPIO_SMB_DATA, GPIO_INPUT_AUX1);
+
+	for (x = 0; x < 16; x++) {
+		udelay(5);
+		cs5535_gpio_clear(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_VAL);
+		udelay(5);
+		cs5535_gpio_set(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_VAL);
+	}
+	udelay(5);
+	cs5535_gpio_set(OLPC_GPIO_SMB_CLK, GPIO_OUTPUT_AUX1);
+	cs5535_gpio_set(OLPC_GPIO_SMB_DATA, GPIO_OUTPUT_AUX1);
+	cs5535_gpio_set(OLPC_GPIO_SMB_CLK, GPIO_INPUT_AUX1);
+	cs5535_gpio_set(OLPC_GPIO_SMB_DATA, GPIO_INPUT_AUX1);
+}
+
+static void dcon_set_dconload_1(int val)
+{
+	gpiod_set_value(gpios[OLPC_DCON_LOAD], val);
+}
+
+static int dcon_read_status_xo_1(u8 *status)
+{
+	*status = gpiod_get_value(gpios[OLPC_DCON_STAT0]);
+	*status |= gpiod_get_value(gpios[OLPC_DCON_STAT1]) << 1;
+
+	/* Clear the negative edge status for GPIO7 */
+	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_NEGATIVE_EDGE_STS);
+
+	return 0;
+}
+
+struct dcon_platform_data dcon_pdata_xo_1 = {
+	.init = dcon_init_xo_1,
+	.bus_stabilize_wiggle = dcon_wiggle_xo_1,
+	.set_dconload = dcon_set_dconload_1,
+	.read_status = dcon_read_status_xo_1,
+};
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
new file mode 100644
index 000000000..52cdcd2a8
--- /dev/null
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
@@ -0,0 +1,204 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2009,2010       One Laptop per Child
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio/consumer.h>
+#include <linux/gpio/machine.h>
+#include <asm/olpc.h>
+
+/* TODO: this eventually belongs in linux/vx855.h */
+#define NR_VX855_GPI    14
+#define NR_VX855_GPO    13
+#define NR_VX855_GPIO   15
+
+#define VX855_GPI(n)    (n)
+#define VX855_GPO(n)    (NR_VX855_GPI + (n))
+#define VX855_GPIO(n)   (NR_VX855_GPI + NR_VX855_GPO + (n))
+
+#include "olpc_dcon.h"
+
+/* Hardware setup on the XO 1.5:
+ *	DCONLOAD connects to VX855_GPIO1 (not SMBCK2)
+ *	DCONBLANK connects to VX855_GPIO8 (not SSPICLK)  unused in driver
+ *	DCONSTAT0 connects to VX855_GPI10 (not SSPISDI)
+ *	DCONSTAT1 connects to VX855_GPI11 (not nSSPISS)
+ *	DCONIRQ connects to VX855_GPIO12
+ *	DCONSMBDATA connects to VX855 graphics CRTSPD
+ *	DCONSMBCLK connects to VX855 graphics CRTSPCLK
+ */
+
+#define VX855_GENL_PURPOSE_OUTPUT 0x44c /* PMIO_Rx4c-4f */
+#define VX855_GPI_STATUS_CHG 0x450  /* PMIO_Rx50 */
+#define VX855_GPI_SCI_SMI 0x452  /* PMIO_Rx52 */
+#define BIT_GPIO12 0x40
+
+#define PREFIX "OLPC DCON:"
+
+enum dcon_gpios {
+	OLPC_DCON_STAT0,
+	OLPC_DCON_STAT1,
+	OLPC_DCON_LOAD,
+};
+
+struct gpiod_lookup_table gpios_table = {
+	.dev_id = NULL,
+	.table = {
+		GPIO_LOOKUP("VX855 South Bridge", VX855_GPIO(1), "dcon_load",
+			    GPIO_ACTIVE_LOW),
+		GPIO_LOOKUP("VX855 South Bridge", VX855_GPI(10), "dcon_stat0",
+			    GPIO_ACTIVE_LOW),
+		GPIO_LOOKUP("VX855 South Bridge", VX855_GPI(11), "dcon_stat1",
+			    GPIO_ACTIVE_LOW),
+		{ },
+	},
+};
+
+static const struct dcon_gpio gpios_asis[] = {
+	[OLPC_DCON_STAT0] = { .name = "dcon_stat0", .flags = GPIOD_ASIS },
+	[OLPC_DCON_STAT1] = { .name = "dcon_stat1", .flags = GPIOD_ASIS },
+	[OLPC_DCON_LOAD] = { .name = "dcon_load", .flags = GPIOD_ASIS },
+};
+
+static struct gpio_desc *gpios[3];
+
+static void dcon_clear_irq(void)
+{
+	/* irq status will appear in PMIO_Rx50[6] (RW1C) on gpio12 */
+	outb(BIT_GPIO12, VX855_GPI_STATUS_CHG);
+}
+
+static int dcon_was_irq(void)
+{
+	u8 tmp;
+
+	/* irq status will appear in PMIO_Rx50[6] on gpio12 */
+	tmp = inb(VX855_GPI_STATUS_CHG);
+
+	return !!(tmp & BIT_GPIO12);
+}
+
+static int dcon_init_xo_1_5(struct dcon_priv *dcon)
+{
+	unsigned int irq;
+	const struct dcon_gpio *pin = &gpios_asis[0];
+	int i;
+	int ret;
+
+	/* Add GPIO look up table */
+	gpios_table.dev_id = dev_name(&dcon->client->dev);
+	gpiod_add_lookup_table(&gpios_table);
+
+	/* Get GPIO descriptor */
+	for (i = 0; i < ARRAY_SIZE(gpios_asis); i++) {
+		gpios[i] = devm_gpiod_get(&dcon->client->dev, pin[i].name,
+					  pin[i].flags);
+		if (IS_ERR(gpios[i])) {
+			ret = PTR_ERR(gpios[i]);
+			pr_err("failed to request %s GPIO: %d\n", pin[i].name,
+			       ret);
+			return ret;
+		}
+	}
+
+	dcon_clear_irq();
+
+	/* set   PMIO_Rx52[6] to enable SCI/SMI on gpio12 */
+	outb(inb(VX855_GPI_SCI_SMI) | BIT_GPIO12, VX855_GPI_SCI_SMI);
+
+	/* Determine the current state of DCONLOAD, likely set by firmware */
+	/* GPIO1 */
+	dcon->curr_src = (inl(VX855_GENL_PURPOSE_OUTPUT) & 0x1000) ?
+			DCON_SOURCE_CPU : DCON_SOURCE_DCON;
+	dcon->pending_src = dcon->curr_src;
+
+	/* we're sharing the IRQ with ACPI */
+	irq = acpi_gbl_FADT.sci_interrupt;
+	if (request_irq(irq, &dcon_interrupt, IRQF_SHARED, "DCON", dcon)) {
+		pr_err("DCON (IRQ%d) allocation failed\n", irq);
+		return 1;
+	}
+
+	return 0;
+}
+
+static void set_i2c_line(int sda, int scl)
+{
+	unsigned char tmp;
+	unsigned int port = 0x26;
+
+	/* FIXME: This directly accesses the CRT GPIO controller !!! */
+	outb(port, 0x3c4);
+	tmp = inb(0x3c5);
+
+	if (scl)
+		tmp |= 0x20;
+	else
+		tmp &= ~0x20;
+
+	if (sda)
+		tmp |= 0x10;
+	else
+		tmp &= ~0x10;
+
+	tmp |= 0x01;
+
+	outb(port, 0x3c4);
+	outb(tmp, 0x3c5);
+}
+
+static void dcon_wiggle_xo_1_5(void)
+{
+	int x;
+
+	/*
+	 * According to HiMax, when powering the DCON up we should hold
+	 * SMB_DATA high for 8 SMB_CLK cycles.  This will force the DCON
+	 * state machine to reset to a (sane) initial state.  Mitch Bradley
+	 * did some testing and discovered that holding for 16 SMB_CLK cycles
+	 * worked a lot more reliably, so that's what we do here.
+	 */
+	set_i2c_line(1, 1);
+
+	for (x = 0; x < 16; x++) {
+		udelay(5);
+		set_i2c_line(1, 0);
+		udelay(5);
+		set_i2c_line(1, 1);
+	}
+	udelay(5);
+
+	/* set   PMIO_Rx52[6] to enable SCI/SMI on gpio12 */
+	outb(inb(VX855_GPI_SCI_SMI) | BIT_GPIO12, VX855_GPI_SCI_SMI);
+}
+
+static void dcon_set_dconload_xo_1_5(int val)
+{
+	gpiod_set_value(gpios[OLPC_DCON_LOAD], val);
+}
+
+static int dcon_read_status_xo_1_5(u8 *status)
+{
+	if (!dcon_was_irq())
+		return -1;
+
+	/* i believe this is the same as "inb(0x44b) & 3" */
+	*status = gpiod_get_value(gpios[OLPC_DCON_STAT0]);
+	*status |= gpiod_get_value(gpios[OLPC_DCON_STAT1]) << 1;
+
+	dcon_clear_irq();
+
+	return 0;
+}
+
+struct dcon_platform_data dcon_pdata_xo_1_5 = {
+	.init = dcon_init_xo_1_5,
+	.bus_stabilize_wiggle = dcon_wiggle_xo_1_5,
+	.set_dconload = dcon_set_dconload_xo_1_5,
+	.read_status = dcon_read_status_xo_1_5,
+};
diff --git a/drivers/staging/pi433/Documentation/devicetree/pi433-overlay.dts b/drivers/staging/pi433/Documentation/devicetree/pi433-overlay.dts
new file mode 100644
index 000000000..096137fcd
--- /dev/null
+++ b/drivers/staging/pi433/Documentation/devicetree/pi433-overlay.dts
@@ -0,0 +1,48 @@
+// Definitions for Pi433
+/dts-v1/;
+/plugin/;
+
+/ {
+	compatible = "brcm,bcm2835", "brcm,bcm2708", "brcm,bcm2709";
+};
+
+&spi0 {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	spidev@0{
+		reg = <0>;
+		status = "disabled";
+	};
+
+	spidev@1{
+		reg = <1>;
+		status = "disabled";
+	};
+};
+
+&gpio {
+	pi433_pins: pi433_pins {
+		brcm,pins = <7 25 24>;
+		brcm,function = <0 0 0>; // in in in
+	};
+};
+
+&spi0 {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	pi433: pi433@0 {
+		compatible = "Smarthome-Wolf,pi433";
+		reg = <0>;
+		spi-max-frequency = <10000000>;
+		status = "okay";
+
+		pinctrl-0 = <&pi433_pins>;
+		DIO0-gpio = <&gpio 24 0>;
+		DIO1-gpio = <&gpio 25 0>;
+		DIO2-gpio = <&gpio  7 0>;
+	};
+};
diff --git a/drivers/staging/pi433/Documentation/devicetree/pi433.txt b/drivers/staging/pi433/Documentation/devicetree/pi433.txt
new file mode 100644
index 000000000..a810548c5
--- /dev/null
+++ b/drivers/staging/pi433/Documentation/devicetree/pi433.txt
@@ -0,0 +1,62 @@
+* Smarthome-Wolf Pi433 - a 433MHz radio module/shield for Raspberry Pi (see www.pi433.de)
+
+Required properties:
+- compatible: must be "Smarthome-Wolf,pi433"
+- reg: chip select of SPI Interface
+- DIOx-gpio must be dedicated to the GPIO, connected with DIOx of the RFM69 module
+
+
+Example:
+
+With the following lines in gpio-section, the gpio pins, connected with pi433 are
+reserved/declared.
+
+&gpio{
+	[...]
+
+	pi433_pins: pi433_pins {
+		brcm,pins = <7 25 24>;
+		brcm,function = <0 0 0>; // in in in
+	};
+
+	[...]
+}
+
+With the following lines in spi section, the device pi433 is declared.
+It consists of the three gpio pins and an spi interface (here chip select 0)
+
+&spi0{
+	[...]
+
+	pi433: pi433@0 {
+		compatible = "Smarthome-Wolf,pi433";
+		reg = <0>; /* CE 0 */
+		#address-cells = <1>;
+		#size-cells = <0>;
+		spi-max-frequency = <10000000>;
+
+		pinctrl-0 = <&pi433_pins>;
+		DIO0-gpio = <&gpio 24 0>;
+		DIO1-gpio = <&gpio 25 0>;
+		DIO2-gpio = <&gpio  7 0>;
+	};
+}
+
+
+
+For Raspbian users only
+=======================
+Since Raspbian supports device tree overlays, you may use an overlay instead
+of editing your boards device tree.
+To use the overlay, you need to compile the file pi433-overlay.dts which can
+be found alongside this documentation.
+The file needs to be compiled - either manually or by integration in your kernel
+source tree. For a manual compile, you may use a command line like the following:
+'linux/scripts/dtc/dtc -@ -I dts -O dtb -o pi433.dtbo pi433-overlay.dts'
+
+For compiling inside of the kernel tree, you need to copy pi433-overlay.dts to
+arch/arm/boot/dts/overlays and you need to add the file to the list of files
+in the Makefile over there. Execute 'make dtbs' in kernel tree root to make the
+kernel make files compile the device tree overlay for you.
+
+
diff --git a/drivers/staging/pi433/Documentation/pi433.txt b/drivers/staging/pi433/Documentation/pi433.txt
new file mode 100644
index 000000000..4a0d34b4a
--- /dev/null
+++ b/drivers/staging/pi433/Documentation/pi433.txt
@@ -0,0 +1,274 @@
+=====
+Pi433
+=====
+
+
+Introduction
+============
+This driver is for controlling pi433, a radio module for the Raspberry Pi
+(www.pi433.de). It supports transmission and reception. It can be opened
+by multiple applications for transmission and reception. While transmit
+jobs are queued and processed automatically in the background, the first
+application asking for reception will block out all other applications
+until something gets received terminates the read request.
+The driver supports on the fly reloading of the hardware fifo of the rf
+chip, thus enabling for much longer telegrams than the hardware fifo size.
+
+Description of driver operation
+===============================
+
+a) transmission
+
+Each transmission can take place with a different configuration of the rf
+module. Therefore each application can set its own set of parameters. The driver
+takes care, that each transmission takes place with the parameterset of the
+application, that requests the transmission. To allow the transmission to take
+place in the background, a tx thread is introduced.
+The transfer of data from the main thread to the tx thread is realised by a
+kfifo. With each write request of an application, the passed in data and the
+corresponding parameter set gets written to the kfifo.
+On the other "side" of the kfifo, the tx thread continuously checks, whether the
+kfifo is empty. If not, it gets one set of config and data from the kfifo. If
+there is no receive request or the receiver is still waiting for something in
+the air, the rf module is set to standby, the parameters for transmission gets
+set, the hardware fifo of the rf chip gets preloaded and the transmission gets
+started. Upon hardware fifo threshold interrupt it gets reloaded, thus enabling
+much longer telegrams than the hardware fifo size. If the telegram is sent and there
+is more data available in the kfifo, the procedure is repeated. If not the
+transmission cycle ends.
+
+b) reception
+
+Since there is only one application allowed to receive data at a time, for
+reception there is only one configuration set.
+As soon as an application sets a request for receiving a telegram, the reception
+configuration set is written to the rf module and it gets set into receiving mode.
+Now the driver is waiting, that a predefined RSSI level (signal strength at the
+receiver) is reached. Until this hasn't happened, the reception can be
+interrupted by the transmission thread at any time to insert a transmission cycle.
+As soon as the predefined RSSI level is met, a receiving cycle starts. Similar
+as described for the transmission cycle the read out of the hardware fifo is done
+dynamically. Upon each hardware fifo threshold interrupt, a portion of data gets
+read. So also for reception it is possible to receive more data than the hardware
+fifo can hold.
+
+
+Driver API
+==========
+
+The driver is currently implemented as a character device. Therefore it supports
+the calls open, ioctl, read, write and close.
+
+
+params for ioctl
+----------------
+
+There are four options:
+PI433_IOC_RD_TX_CFG - get the transmission parameters from the driver
+PI433_IOC_WR_TX_CFG - set the transmission parameters
+PI433_IOC_RD_RX_CFG - get the receiving parameters from the driver
+PI433_IOC_WR_RX_CFG - set the receiving parameters
+
+The tx configuration is transferred via struct pi433_tx_cfg, the parameterset for transmission.
+It is divided into two sections: rf parameters and packet format.
+
+rf params:
+	frequency
+		frequency used for transmission.
+		Allowed values: 433050000...434790000
+	bit_rate
+		bit rate used for transmission.
+		Allowed values: #####
+	dev_frequency
+		frequency deviation in case of FSK.
+		Allowed values: 600...500000
+	modulation
+		FSK - frequency shift key
+		OOK - On-Off-key
+	modShaping
+		shapingOff	- no shaping
+		shaping1_0	- gauss filter with BT 1 (FSK only)
+		shaping0_5	- gauss filter with BT 0.5 (FSK only)
+		shaping0_3	- gauss filter with BT 0.3 (FSK only)
+		shapingBR	- filter cut off at BR (OOK only)
+		shaping2BR	- filter cut off at 2*BR (OOK only)
+	pa_ramp (FSK only)
+		ramp3400	- amp ramps up in 3.4ms
+		ramp2000	- amp ramps up in 2.0ms
+		ramp1000	- amp ramps up in 1ms
+		ramp500		- amp ramps up in 500us
+		ramp250		- amp ramps up in 250us
+		ramp125		- amp ramps up in 125us
+		ramp100		- amp ramps up in 100us
+		ramp62		- amp ramps up in 62us
+		ramp50		- amp ramps up in 50us
+		ramp40		- amp ramps up in 40us
+		ramp31		- amp ramps up in 31us
+		ramp25		- amp ramps up in 25us
+		ramp20		- amp ramps up in 20us
+		ramp15		- amp ramps up in 15us
+		ramp12		- amp ramps up in 12us
+		ramp10		- amp ramps up in 10us
+	tx_start_condition
+		fifo_level	- transmission starts, if fifo is filled to
+				  threshold level
+		fifo_not_empty	- transmission starts, as soon as there is one
+				  byte in internal fifo
+	repetitions
+		This gives the option, to send a telegram multiple times. Default: 1
+
+packet format:
+	enable_preamble
+		optionOn	- a preamble will be automatically generated
+		optionOff	- no preamble will be generated
+	enable_sync
+		optionOn	- a sync word will be automatically added to
+				  the telegram after the preamble
+		optionOff	- no sync word will be added
+		Attention: While possible to generate sync without preamble, the
+		receiver won't be able to detect the sync without preamble.
+	enable_length_byte
+		optionOn	- the length of the telegram will be automatically
+				  added to the telegram. It's part of the payload
+		optionOff	- no length information will be automatically added
+				  to the telegram.
+		Attention: For telegram length over 255 bytes, this option can't be used
+		Attention: should be used in combination with sync, only
+	enable_address_byte
+		optionOn	- the address byte will be automatically added to the
+				  telegram. It's part of the payload
+		optionOff	- the address byte will not be added to the telegram.
+		The address byte can be used for address filtering, so the receiver
+		will only receive telegrams with a given address byte.
+		Attention: should be used in combination with sync, only
+	enable_crc
+		optionOn	- an crc will be automatically calculated over the
+				  payload of the telegram and added to the telegram
+				  after payload.
+		optionOff	- no crc will be calculated
+	preamble_length
+		length of the preamble. Allowed values: 0...65536
+	sync_length
+		length of the sync word. Allowed values: 0...8
+	fixed_message_length
+		length of the payload of the telegram. Will override the length
+		given by the buffer, passed in with the write command. Will be
+		ignored if set to zero.
+	sync_pattern[8]
+		contains up to eight values, that are used as the sync pattern
+		on sync option
+	address_byte
+		one byte, used as address byte on address byte option.
+
+
+The rx configuration is transferred via struct pi433_rx_cfg, the parameterset for receiving. It is divided into two sections: rf parameters and packet format.
+
+rf params:
+	frequency
+		frequency used for transmission.
+		Allowed values: 433050000...434790000
+	bit_rate
+		bit rate used for transmission.
+		Allowed values: #####
+	dev_frequency
+		frequency deviation in case of FSK.
+		Allowed values: 600...500000
+	modulation
+		FSK - frequency shift key
+		OOK - on off key
+	rssi_threshold
+		threshold value for the signal strength on the receiver input.
+		If this value is exceeded, a reception cycle starts
+		Allowed values: 0...255
+	threshold_decrement
+		in order to adapt to different levels of singnal strength, over
+		time the receiver gets more and more sensitive. This value
+		determs, how fast the sensitivity increases.
+		step_0_5db	- increase in 0,5dB steps
+		step_1_0db	- increase in 1 db steps
+		step_1_5db	- increase in 1,5dB steps
+		step_2_0db	- increase in 2 db steps
+		step_3_0db	- increase in 3 db steps
+		step_4_0db	- increase in 4 db steps
+		step_5_0db	- increase in 5 db steps
+		step_6_0db	- increase in 6 db steps
+	antenna_impedance
+		sets the electrical adoption of the antenna
+		fifty_ohm	- for antennas with an impedance of 50Ohm
+		two_hundred_ohm	- for antennas with an impedance of 200Ohm
+	lna_gain
+		sets the gain of the low noise amp
+		automatic	- lna gain is determined by an agc
+		max		- lna gain is set to maximum
+		max_minus_6	- lna gain is set to  6db below max
+		max_minus_12	- lna gain is set to 12db below max
+		max_minus_24	- lna gain is set to 24db below max
+		max_minus_36	- lna gain is set to 36db below max
+		max_minus_48	- lna gain is set to 48db below max
+	bw_mantisse
+		sets the bandwidth of the channel filter - part one: mantisse.
+		mantisse16	- mantisse is set to 16
+		mantisse20	- mantisse is set to 20
+		mantisse24	- mantisse is set to 24
+	bw_exponent
+		sets the bandwidth of the channel filter - part two: exponent.
+		Allowd values: 0...7
+	dagc;
+		operation mode of the digital automatic gain control
+		normal_mode
+		improve
+		improve_for_low_modulation_index
+
+ packet format:
+	enable_sync
+		optionOn  - sync detection is enabled. If configured sync pattern
+			    isn't found, telegram will be internally discarded
+		optionOff - sync detection is disabled.
+	enable_length_byte
+		optionOn   - First byte of payload will be used as a length byte,
+			     regardless of the amount of bytes that were requested
+			     by the read request.
+		optionOff  - Number of bytes to be read will be set according to
+			     amount of bytes that were requested by the read request.
+		Attention: should be used in combination with sync, only
+	enable_address_filtering;
+		filtering_off		  - no address filtering will take place
+		node_address		  - all telegrams, not matching the node
+					    address will be internally discarded
+		node_or_broadcast_address - all telegrams, neither matching the
+					    node, nor the broadcast address will
+					    be internally discarded
+		Attention: Sync option must be enabled in order to use this feature
+	enable_crc
+		optionOn	- a crc will be calculated over the payload of
+				  the telegram, that was received. If the
+				  calculated crc doesn't match to two bytes,
+				  that follow the payload, the telegram will be
+				  internally discarded.
+		Attention: This option is only operational if sync on and fixed length
+		or length byte is used
+	sync_length
+		Gives the length of the payload.
+		Attention: This setting must meet the setting of the transmitter,
+		if sync option is used.
+	fixed_message_length
+		Overrides the telegram length either given by the first byte of
+		payload or by the read request.
+	bytes_to_drop
+		gives the number of bytes, that will be dropped before transferring
+		data to the read buffer
+		This option is only useful if all packet helper are switched
+		off and the rf chip is used in raw receiving mode. This may be
+		needed, if a telegram of a third party device should be received,
+		using a protocol not compatible with the packet engine of the rf69 chip.
+	sync_pattern[8]
+		contains up to eight values, that are used as the sync pattern
+		on sync option.
+		This setting must meet the configuration of the transmitting device,
+		if sync option is enabled.
+	node_address
+		one byte, used as node address byte on address byte option.
+	broadcast_address
+		one byte, used as broadcast address byte on address byte option.
+
+
diff --git a/drivers/staging/pi433/Kconfig b/drivers/staging/pi433/Kconfig
new file mode 100644
index 000000000..dd9e4709d
--- /dev/null
+++ b/drivers/staging/pi433/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+config PI433
+	tristate "Pi433 - a 433MHz radio module for Raspberry Pi"
+	depends on SPI
+	help
+	  This option allows you to enable support for the radio module Pi433.
+
+	  Pi433 is a shield that fits onto the GPIO header of a Raspberry Pi
+	  or compatible. It extends the Raspberry Pi with the option, to
+	  send and receive data in the 433MHz ISM band - for example to
+	  communicate between two systems without using ethernet or bluetooth
+	  or for control or read sockets, actors, sensors, widely available
+	  for low price.
+
+	  For details or the option to buy, please visit https://pi433.de/en.html
+
+	  If in doubt, say N here, but saying yes most probably won't hurt
diff --git a/drivers/staging/pi433/Makefile b/drivers/staging/pi433/Makefile
new file mode 100644
index 000000000..051132fe4
--- /dev/null
+++ b/drivers/staging/pi433/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_PI433) += pi433.o
+
+pi433-objs := pi433_if.o rf69.o
diff --git a/drivers/staging/pi433/TODO b/drivers/staging/pi433/TODO
new file mode 100644
index 000000000..5cf3fd99d
--- /dev/null
+++ b/drivers/staging/pi433/TODO
@@ -0,0 +1,5 @@
+* currently the code introduces new IOCTLs. I'm afraid this is a bad idea.
+  -> Replace this with another interface, hints are welcome!
+* Some missing data (marked with ###) needs to be added in the documentation
+* Change (struct pi433_tx_cfg)->bit_rate to be a u32 so that we can support
+    bit rates up to 300kbps per the spec.
diff --git a/drivers/staging/pi433/pi433_if.c b/drivers/staging/pi433/pi433_if.c
new file mode 100644
index 000000000..b59f6a4cb
--- /dev/null
+++ b/drivers/staging/pi433/pi433_if.c
@@ -0,0 +1,1436 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * userspace interface for pi433 radio module
+ *
+ * Pi433 is a 433MHz radio module for the Raspberry Pi.
+ * It is based on the HopeRf Module RFM69CW. Therefore inside of this
+ * driver, you'll find an abstraction of the rf69 chip.
+ *
+ * If needed, this driver could be extended, to also support other
+ * devices, basing on HopeRfs rf69.
+ *
+ * The driver can also be extended, to support other modules of
+ * HopeRf with a similar interace - e. g. RFM69HCW, RFM12, RFM95, ...
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+
+#undef DEBUG
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/idr.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/err.h>
+#include <linux/kfifo.h>
+#include <linux/errno.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio/consumer.h>
+#include <linux/kthread.h>
+#include <linux/wait.h>
+#include <linux/spi/spi.h>
+#ifdef CONFIG_COMPAT
+#include <linux/compat.h>
+#endif
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#include "pi433_if.h"
+#include "rf69.h"
+
+#define N_PI433_MINORS		BIT(MINORBITS) /*32*/	/* ... up to 256 */
+#define MAX_MSG_SIZE		900	/* min: FIFO_SIZE! */
+#define MSG_FIFO_SIZE		65536   /* 65536 = 2^16  */
+#define NUM_DIO			2
+
+static dev_t pi433_dev;
+static DEFINE_IDR(pi433_idr);
+static DEFINE_MUTEX(minor_lock); /* Protect idr accesses */
+static struct dentry *root_dir;	/* debugfs root directory for the driver */
+
+static struct class *pi433_class; /* mainly for udev to create /dev/pi433 */
+
+/*
+ * tx config is instance specific
+ * so with each open a new tx config struct is needed
+ */
+/*
+ * rx config is device specific
+ * so we have just one rx config, ebedded in device struct
+ */
+struct pi433_device {
+	/* device handling related values */
+	dev_t			devt;
+	int			minor;
+	struct device		*dev;
+	struct cdev		*cdev;
+	struct spi_device	*spi;
+
+	/* irq related values */
+	struct gpio_desc	*gpiod[NUM_DIO];
+	int			irq_num[NUM_DIO];
+	u8			irq_state[NUM_DIO];
+
+	/* tx related values */
+	STRUCT_KFIFO_REC_1(MSG_FIFO_SIZE) tx_fifo;
+	struct mutex		tx_fifo_lock; /* serialize userspace writers */
+	struct task_struct	*tx_task_struct;
+	wait_queue_head_t	tx_wait_queue;
+	u8			free_in_fifo;
+	char			buffer[MAX_MSG_SIZE];
+
+	/* rx related values */
+	struct pi433_rx_cfg	rx_cfg;
+	u8			*rx_buffer;
+	unsigned int		rx_buffer_size;
+	u32			rx_bytes_to_drop;
+	u32			rx_bytes_dropped;
+	unsigned int		rx_position;
+	struct mutex		rx_lock; /* protects rx_* variable accesses */
+	wait_queue_head_t	rx_wait_queue;
+
+	/* fifo wait queue */
+	struct task_struct	*fifo_task_struct;
+	wait_queue_head_t	fifo_wait_queue;
+
+	/* flags */
+	bool			rx_active;
+	bool			tx_active;
+	bool			interrupt_rx_allowed;
+};
+
+struct pi433_instance {
+	struct pi433_device	*device;
+	struct pi433_tx_cfg	tx_cfg;
+
+	/* control flags */
+	bool			tx_cfg_initialized;
+};
+
+/*-------------------------------------------------------------------------*/
+
+/* GPIO interrupt handlers */
+static irqreturn_t DIO0_irq_handler(int irq, void *dev_id)
+{
+	struct pi433_device *device = dev_id;
+
+	if (device->irq_state[DIO0] == DIO_PACKET_SENT) {
+		device->free_in_fifo = FIFO_SIZE;
+		dev_dbg(device->dev, "DIO0 irq: Packet sent\n");
+		wake_up_interruptible(&device->fifo_wait_queue);
+	} else if (device->irq_state[DIO0] == DIO_RSSI_DIO0) {
+		dev_dbg(device->dev, "DIO0 irq: RSSI level over threshold\n");
+		wake_up_interruptible(&device->rx_wait_queue);
+	} else if (device->irq_state[DIO0] == DIO_PAYLOAD_READY) {
+		dev_dbg(device->dev, "DIO0 irq: Payload ready\n");
+		device->free_in_fifo = 0;
+		wake_up_interruptible(&device->fifo_wait_queue);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t DIO1_irq_handler(int irq, void *dev_id)
+{
+	struct pi433_device *device = dev_id;
+
+	if (device->irq_state[DIO1] == DIO_FIFO_NOT_EMPTY_DIO1) {
+		device->free_in_fifo = FIFO_SIZE;
+	} else if (device->irq_state[DIO1] == DIO_FIFO_LEVEL) {
+		if (device->rx_active)
+			device->free_in_fifo = FIFO_THRESHOLD - 1;
+		else
+			device->free_in_fifo = FIFO_SIZE - FIFO_THRESHOLD - 1;
+	}
+	dev_dbg(device->dev,
+		"DIO1 irq: %d bytes free in fifo\n", device->free_in_fifo);
+	wake_up_interruptible(&device->fifo_wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int
+rf69_set_rx_cfg(struct pi433_device *dev, struct pi433_rx_cfg *rx_cfg)
+{
+	int ret;
+	int payload_length;
+
+	/* receiver config */
+	ret = rf69_set_frequency(dev->spi, rx_cfg->frequency);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_modulation(dev->spi, rx_cfg->modulation);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_bit_rate(dev->spi, rx_cfg->bit_rate);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_antenna_impedance(dev->spi, rx_cfg->antenna_impedance);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_rssi_threshold(dev->spi, rx_cfg->rssi_threshold);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_ook_threshold_dec(dev->spi, rx_cfg->threshold_decrement);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_bandwidth(dev->spi, rx_cfg->bw_mantisse,
+				 rx_cfg->bw_exponent);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_bandwidth_during_afc(dev->spi, rx_cfg->bw_mantisse,
+					    rx_cfg->bw_exponent);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_dagc(dev->spi, rx_cfg->dagc);
+	if (ret < 0)
+		return ret;
+
+	dev->rx_bytes_to_drop = rx_cfg->bytes_to_drop;
+
+	/* packet config */
+	/* enable */
+	if (rx_cfg->enable_sync == OPTION_ON) {
+		ret = rf69_enable_sync(dev->spi);
+		if (ret < 0)
+			return ret;
+
+		ret = rf69_set_fifo_fill_condition(dev->spi,
+						   after_sync_interrupt);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_disable_sync(dev->spi);
+		if (ret < 0)
+			return ret;
+
+		ret = rf69_set_fifo_fill_condition(dev->spi, always);
+		if (ret < 0)
+			return ret;
+	}
+	if (rx_cfg->enable_length_byte == OPTION_ON) {
+		ret = rf69_set_packet_format(dev->spi, packet_length_var);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_set_packet_format(dev->spi, packet_length_fix);
+		if (ret < 0)
+			return ret;
+	}
+	ret = rf69_set_address_filtering(dev->spi,
+					 rx_cfg->enable_address_filtering);
+	if (ret < 0)
+		return ret;
+
+	if (rx_cfg->enable_crc == OPTION_ON) {
+		ret = rf69_enable_crc(dev->spi);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_disable_crc(dev->spi);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* lengths */
+	ret = rf69_set_sync_size(dev->spi, rx_cfg->sync_length);
+	if (ret < 0)
+		return ret;
+	if (rx_cfg->enable_length_byte == OPTION_ON) {
+		ret = rf69_set_payload_length(dev->spi, 0xff);
+		if (ret < 0)
+			return ret;
+	} else if (rx_cfg->fixed_message_length != 0) {
+		payload_length = rx_cfg->fixed_message_length;
+		if (rx_cfg->enable_length_byte  == OPTION_ON)
+			payload_length++;
+		if (rx_cfg->enable_address_filtering != filtering_off)
+			payload_length++;
+		ret = rf69_set_payload_length(dev->spi, payload_length);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_set_payload_length(dev->spi, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* values */
+	if (rx_cfg->enable_sync == OPTION_ON) {
+		ret = rf69_set_sync_values(dev->spi, rx_cfg->sync_pattern);
+		if (ret < 0)
+			return ret;
+	}
+	if (rx_cfg->enable_address_filtering != filtering_off) {
+		ret = rf69_set_node_address(dev->spi, rx_cfg->node_address);
+		if (ret < 0)
+			return ret;
+		ret = rf69_set_broadcast_address(dev->spi,
+						 rx_cfg->broadcast_address);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int
+rf69_set_tx_cfg(struct pi433_device *dev, struct pi433_tx_cfg *tx_cfg)
+{
+	int ret;
+
+	ret = rf69_set_frequency(dev->spi, tx_cfg->frequency);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_modulation(dev->spi, tx_cfg->modulation);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_bit_rate(dev->spi, tx_cfg->bit_rate);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_deviation(dev->spi, tx_cfg->dev_frequency);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_pa_ramp(dev->spi, tx_cfg->pa_ramp);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_modulation_shaping(dev->spi, tx_cfg->mod_shaping);
+	if (ret < 0)
+		return ret;
+	ret = rf69_set_tx_start_condition(dev->spi, tx_cfg->tx_start_condition);
+	if (ret < 0)
+		return ret;
+
+	/* packet format enable */
+	if (tx_cfg->enable_preamble == OPTION_ON) {
+		ret = rf69_set_preamble_length(dev->spi,
+					       tx_cfg->preamble_length);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_set_preamble_length(dev->spi, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (tx_cfg->enable_sync == OPTION_ON) {
+		ret = rf69_set_sync_size(dev->spi, tx_cfg->sync_length);
+		if (ret < 0)
+			return ret;
+		ret = rf69_set_sync_values(dev->spi, tx_cfg->sync_pattern);
+		if (ret < 0)
+			return ret;
+		ret = rf69_enable_sync(dev->spi);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_disable_sync(dev->spi);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (tx_cfg->enable_length_byte == OPTION_ON) {
+		ret = rf69_set_packet_format(dev->spi, packet_length_var);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_set_packet_format(dev->spi, packet_length_fix);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (tx_cfg->enable_crc == OPTION_ON) {
+		ret = rf69_enable_crc(dev->spi);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = rf69_disable_crc(dev->spi);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int pi433_start_rx(struct pi433_device *dev)
+{
+	int retval;
+
+	/* return without action, if no pending read request */
+	if (!dev->rx_active)
+		return 0;
+
+	/* setup for receiving */
+	retval = rf69_set_rx_cfg(dev, &dev->rx_cfg);
+	if (retval)
+		return retval;
+
+	/* setup rssi irq */
+	retval = rf69_set_dio_mapping(dev->spi, DIO0, DIO_RSSI_DIO0);
+	if (retval < 0)
+		return retval;
+	dev->irq_state[DIO0] = DIO_RSSI_DIO0;
+	irq_set_irq_type(dev->irq_num[DIO0], IRQ_TYPE_EDGE_RISING);
+
+	/* setup fifo level interrupt */
+	retval = rf69_set_fifo_threshold(dev->spi, FIFO_SIZE - FIFO_THRESHOLD);
+	if (retval < 0)
+		return retval;
+	retval = rf69_set_dio_mapping(dev->spi, DIO1, DIO_FIFO_LEVEL);
+	if (retval < 0)
+		return retval;
+	dev->irq_state[DIO1] = DIO_FIFO_LEVEL;
+	irq_set_irq_type(dev->irq_num[DIO1], IRQ_TYPE_EDGE_RISING);
+
+	/* set module to receiving mode */
+	retval = rf69_set_mode(dev->spi, receive);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int pi433_receive(void *data)
+{
+	struct pi433_device *dev = data;
+	struct spi_device *spi = dev->spi;
+	int bytes_to_read, bytes_total;
+	int retval;
+
+	dev->interrupt_rx_allowed = false;
+
+	/* wait for any tx to finish */
+	dev_dbg(dev->dev, "rx: going to wait for any tx to finish\n");
+	retval = wait_event_interruptible(dev->rx_wait_queue, !dev->tx_active);
+	if (retval) {
+		/* wait was interrupted */
+		dev->interrupt_rx_allowed = true;
+		wake_up_interruptible(&dev->tx_wait_queue);
+		return retval;
+	}
+
+	/* prepare status vars */
+	dev->free_in_fifo = FIFO_SIZE;
+	dev->rx_position = 0;
+	dev->rx_bytes_dropped = 0;
+
+	/* setup radio module to listen for something "in the air" */
+	retval = pi433_start_rx(dev);
+	if (retval)
+		return retval;
+
+	/* now check RSSI, if low wait for getting high (RSSI interrupt) */
+	while (!(rf69_read_reg(spi, REG_IRQFLAGS1) & MASK_IRQFLAGS1_RSSI)) {
+		/* allow tx to interrupt us while waiting for high RSSI */
+		dev->interrupt_rx_allowed = true;
+		wake_up_interruptible(&dev->tx_wait_queue);
+
+		/* wait for RSSI level to become high */
+		dev_dbg(dev->dev, "rx: going to wait for high RSSI level\n");
+		retval = wait_event_interruptible(dev->rx_wait_queue,
+						  rf69_read_reg(spi, REG_IRQFLAGS1) &
+						  MASK_IRQFLAGS1_RSSI);
+		if (retval) /* wait was interrupted */
+			goto abort;
+		dev->interrupt_rx_allowed = false;
+
+		/* cross check for ongoing tx */
+		if (!dev->tx_active)
+			break;
+	}
+
+	/* configure payload ready irq */
+	retval = rf69_set_dio_mapping(spi, DIO0, DIO_PAYLOAD_READY);
+	if (retval < 0)
+		goto abort;
+	dev->irq_state[DIO0] = DIO_PAYLOAD_READY;
+	irq_set_irq_type(dev->irq_num[DIO0], IRQ_TYPE_EDGE_RISING);
+
+	/* fixed or unlimited length? */
+	if (dev->rx_cfg.fixed_message_length != 0) {
+		if (dev->rx_cfg.fixed_message_length > dev->rx_buffer_size) {
+			retval = -1;
+			goto abort;
+		}
+		bytes_total = dev->rx_cfg.fixed_message_length;
+		dev_dbg(dev->dev, "rx: msg len set to %d by fixed length\n",
+			bytes_total);
+	} else {
+		bytes_total = dev->rx_buffer_size;
+		dev_dbg(dev->dev, "rx: msg len set to %d as requested by read\n",
+			bytes_total);
+	}
+
+	/* length byte enabled? */
+	if (dev->rx_cfg.enable_length_byte == OPTION_ON) {
+		retval = wait_event_interruptible(dev->fifo_wait_queue,
+						  dev->free_in_fifo < FIFO_SIZE);
+		if (retval) /* wait was interrupted */
+			goto abort;
+
+		rf69_read_fifo(spi, (u8 *)&bytes_total, 1);
+		if (bytes_total > dev->rx_buffer_size) {
+			retval = -1;
+			goto abort;
+		}
+		dev->free_in_fifo++;
+		dev_dbg(dev->dev, "rx: msg len reset to %d due to length byte\n",
+			bytes_total);
+	}
+
+	/* address byte enabled? */
+	if (dev->rx_cfg.enable_address_filtering != filtering_off) {
+		u8 dummy;
+
+		bytes_total--;
+
+		retval = wait_event_interruptible(dev->fifo_wait_queue,
+						  dev->free_in_fifo < FIFO_SIZE);
+		if (retval) /* wait was interrupted */
+			goto abort;
+
+		rf69_read_fifo(spi, &dummy, 1);
+		dev->free_in_fifo++;
+		dev_dbg(dev->dev, "rx: address byte stripped off\n");
+	}
+
+	/* get payload */
+	while (dev->rx_position < bytes_total) {
+		if (!(rf69_read_reg(spi, REG_IRQFLAGS2) & MASK_IRQFLAGS2_PAYLOAD_READY)) {
+			retval = wait_event_interruptible(dev->fifo_wait_queue,
+							  dev->free_in_fifo < FIFO_SIZE);
+			if (retval) /* wait was interrupted */
+				goto abort;
+		}
+
+		/* need to drop bytes or acquire? */
+		if (dev->rx_bytes_to_drop > dev->rx_bytes_dropped)
+			bytes_to_read = dev->rx_bytes_to_drop -
+					dev->rx_bytes_dropped;
+		else
+			bytes_to_read = bytes_total - dev->rx_position;
+
+		/* access the fifo */
+		if (bytes_to_read > FIFO_SIZE - dev->free_in_fifo)
+			bytes_to_read = FIFO_SIZE - dev->free_in_fifo;
+		retval = rf69_read_fifo(spi,
+					&dev->rx_buffer[dev->rx_position],
+					bytes_to_read);
+		if (retval) /* read failed */
+			goto abort;
+
+		dev->free_in_fifo += bytes_to_read;
+
+		/* adjust status vars */
+		if (dev->rx_bytes_to_drop > dev->rx_bytes_dropped)
+			dev->rx_bytes_dropped += bytes_to_read;
+		else
+			dev->rx_position += bytes_to_read;
+	}
+
+	/* rx done, wait was interrupted or error occurred */
+abort:
+	dev->interrupt_rx_allowed = true;
+	if (rf69_set_mode(dev->spi, standby))
+		pr_err("rf69_set_mode(): radio module failed to go standby\n");
+	wake_up_interruptible(&dev->tx_wait_queue);
+
+	if (retval)
+		return retval;
+	else
+		return bytes_total;
+}
+
+static int pi433_tx_thread(void *data)
+{
+	struct pi433_device *device = data;
+	struct spi_device *spi = device->spi;
+	struct pi433_tx_cfg tx_cfg;
+	size_t size;
+	bool   rx_interrupted = false;
+	int    position, repetitions;
+	int    retval;
+
+	while (1) {
+		/* wait for fifo to be populated or for request to terminate*/
+		dev_dbg(device->dev, "thread: going to wait for new messages\n");
+		wait_event_interruptible(device->tx_wait_queue,
+					 (!kfifo_is_empty(&device->tx_fifo) ||
+					  kthread_should_stop()));
+		if (kthread_should_stop())
+			return 0;
+
+		/*
+		 * get data from fifo in the following order:
+		 * - tx_cfg
+		 * - size of message
+		 * - message
+		 */
+		retval = kfifo_out(&device->tx_fifo, &tx_cfg, sizeof(tx_cfg));
+		if (retval != sizeof(tx_cfg)) {
+			dev_dbg(device->dev,
+				"reading tx_cfg from fifo failed: got %d byte(s), expected %d\n",
+				retval, (unsigned int)sizeof(tx_cfg));
+			continue;
+		}
+
+		retval = kfifo_out(&device->tx_fifo, &size, sizeof(size_t));
+		if (retval != sizeof(size_t)) {
+			dev_dbg(device->dev,
+				"reading msg size from fifo failed: got %d, expected %d\n",
+				retval, (unsigned int)sizeof(size_t));
+			continue;
+		}
+
+		/* use fixed message length, if requested */
+		if (tx_cfg.fixed_message_length != 0)
+			size = tx_cfg.fixed_message_length;
+
+		/* increase size, if len byte is requested */
+		if (tx_cfg.enable_length_byte == OPTION_ON)
+			size++;
+
+		/* increase size, if adr byte is requested */
+		if (tx_cfg.enable_address_byte == OPTION_ON)
+			size++;
+
+		/* prime buffer */
+		memset(device->buffer, 0, size);
+		position = 0;
+
+		/* add length byte, if requested */
+		if (tx_cfg.enable_length_byte  == OPTION_ON)
+			/*
+			 * according to spec, length byte itself must be
+			 * excluded from the length calculation
+			 */
+			device->buffer[position++] = size - 1;
+
+		/* add adr byte, if requested */
+		if (tx_cfg.enable_address_byte == OPTION_ON)
+			device->buffer[position++] = tx_cfg.address_byte;
+
+		/* finally get message data from fifo */
+		retval = kfifo_out(&device->tx_fifo, &device->buffer[position],
+				   sizeof(device->buffer) - position);
+		dev_dbg(device->dev,
+			"read %d message byte(s) from fifo queue.\n", retval);
+
+		/*
+		 * if rx is active, we need to interrupt the waiting for
+		 * incoming telegrams, to be able to send something.
+		 * We are only allowed, if currently no reception takes
+		 * place otherwise we need to  wait for the incoming telegram
+		 * to finish
+		 */
+		wait_event_interruptible(device->tx_wait_queue,
+					 !device->rx_active ||
+					  device->interrupt_rx_allowed);
+
+		/*
+		 * prevent race conditions
+		 * irq will be reenabled after tx config is set
+		 */
+		disable_irq(device->irq_num[DIO0]);
+		device->tx_active = true;
+
+		/* clear fifo, set fifo threshold, set payload length */
+		retval = rf69_set_mode(spi, standby); /* this clears the fifo */
+		if (retval < 0)
+			goto abort;
+
+		if (device->rx_active && !rx_interrupted) {
+			/*
+			 * rx is currently waiting for a telegram;
+			 * we need to set the radio module to standby
+			 */
+			rx_interrupted = true;
+		}
+
+		retval = rf69_set_fifo_threshold(spi, FIFO_THRESHOLD);
+		if (retval < 0)
+			goto abort;
+		if (tx_cfg.enable_length_byte == OPTION_ON) {
+			retval = rf69_set_payload_length(spi, size * tx_cfg.repetitions);
+			if (retval < 0)
+				goto abort;
+		} else {
+			retval = rf69_set_payload_length(spi, 0);
+			if (retval < 0)
+				goto abort;
+		}
+
+		/* configure the rf chip */
+		retval = rf69_set_tx_cfg(device, &tx_cfg);
+		if (retval < 0)
+			goto abort;
+
+		/* enable fifo level interrupt */
+		retval = rf69_set_dio_mapping(spi, DIO1, DIO_FIFO_LEVEL);
+		if (retval < 0)
+			goto abort;
+		device->irq_state[DIO1] = DIO_FIFO_LEVEL;
+		irq_set_irq_type(device->irq_num[DIO1], IRQ_TYPE_EDGE_FALLING);
+
+		/* enable packet sent interrupt */
+		retval = rf69_set_dio_mapping(spi, DIO0, DIO_PACKET_SENT);
+		if (retval < 0)
+			goto abort;
+		device->irq_state[DIO0] = DIO_PACKET_SENT;
+		irq_set_irq_type(device->irq_num[DIO0], IRQ_TYPE_EDGE_RISING);
+		enable_irq(device->irq_num[DIO0]); /* was disabled by rx active check */
+
+		/* enable transmission */
+		retval = rf69_set_mode(spi, transmit);
+		if (retval < 0)
+			goto abort;
+
+		/* transfer this msg (and repetitions) to chip fifo */
+		device->free_in_fifo = FIFO_SIZE;
+		position = 0;
+		repetitions = tx_cfg.repetitions;
+		while ((repetitions > 0) && (size > position)) {
+			if ((size - position) > device->free_in_fifo) {
+				/* msg to big for fifo - take a part */
+				int write_size = device->free_in_fifo;
+
+				device->free_in_fifo = 0;
+				rf69_write_fifo(spi,
+						&device->buffer[position],
+						write_size);
+				position += write_size;
+			} else {
+				/* msg fits into fifo - take all */
+				device->free_in_fifo -= size;
+				repetitions--;
+				rf69_write_fifo(spi,
+						&device->buffer[position],
+						(size - position));
+				position = 0; /* reset for next repetition */
+			}
+
+			retval = wait_event_interruptible(device->fifo_wait_queue,
+							  device->free_in_fifo > 0);
+			if (retval) {
+				dev_dbg(device->dev, "ABORT\n");
+				goto abort;
+			}
+		}
+
+		/* we are done. Wait for packet to get sent */
+		dev_dbg(device->dev,
+			"thread: wait for packet to get sent/fifo to be empty\n");
+		wait_event_interruptible(device->fifo_wait_queue,
+					 device->free_in_fifo == FIFO_SIZE ||
+					 kthread_should_stop());
+		if (kthread_should_stop())
+			return 0;
+
+		/* STOP_TRANSMISSION */
+		dev_dbg(device->dev, "thread: Packet sent. Set mode to stby.\n");
+		retval = rf69_set_mode(spi, standby);
+		if (retval < 0)
+			goto abort;
+
+		/* everything sent? */
+		if (kfifo_is_empty(&device->tx_fifo)) {
+abort:
+			if (rx_interrupted) {
+				rx_interrupted = false;
+				pi433_start_rx(device);
+			}
+			device->tx_active = false;
+			wake_up_interruptible(&device->rx_wait_queue);
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+static ssize_t
+pi433_read(struct file *filp, char __user *buf, size_t size, loff_t *f_pos)
+{
+	struct pi433_instance	*instance;
+	struct pi433_device	*device;
+	int			bytes_received;
+	ssize_t			retval;
+
+	/* check, whether internal buffer is big enough for requested size */
+	if (size > MAX_MSG_SIZE)
+		return -EMSGSIZE;
+
+	instance = filp->private_data;
+	device = instance->device;
+
+	/* just one read request at a time */
+	mutex_lock(&device->rx_lock);
+	if (device->rx_active) {
+		mutex_unlock(&device->rx_lock);
+		return -EAGAIN;
+	}
+
+	device->rx_active = true;
+	mutex_unlock(&device->rx_lock);
+
+	/* start receiving */
+	/* will block until something was received*/
+	device->rx_buffer_size = size;
+	bytes_received = pi433_receive(device);
+
+	/* release rx */
+	mutex_lock(&device->rx_lock);
+	device->rx_active = false;
+	mutex_unlock(&device->rx_lock);
+
+	/* if read was successful copy to user space*/
+	if (bytes_received > 0) {
+		retval = copy_to_user(buf, device->rx_buffer, bytes_received);
+		if (retval)
+			return -EFAULT;
+	}
+
+	return bytes_received;
+}
+
+static ssize_t
+pi433_write(struct file *filp, const char __user *buf,
+	    size_t count, loff_t *f_pos)
+{
+	struct pi433_instance	*instance;
+	struct pi433_device	*device;
+	int                     retval;
+	unsigned int		required, available, copied;
+
+	instance = filp->private_data;
+	device = instance->device;
+
+	/*
+	 * check, whether internal buffer (tx thread) is big enough
+	 * for requested size
+	 */
+	if (count > MAX_MSG_SIZE)
+		return -EMSGSIZE;
+
+	/*
+	 * check if tx_cfg has been initialized otherwise we won't be able to
+	 * config the RF trasmitter correctly due to invalid settings
+	 */
+	if (!instance->tx_cfg_initialized) {
+		dev_notice_once(device->dev,
+				"write: failed due to unconfigured tx_cfg (see PI433_IOC_WR_TX_CFG)\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * write the following sequence into fifo:
+	 * - tx_cfg
+	 * - size of message
+	 * - message
+	 */
+	mutex_lock(&device->tx_fifo_lock);
+
+	required = sizeof(instance->tx_cfg) + sizeof(size_t) + count;
+	available = kfifo_avail(&device->tx_fifo);
+	if (required > available) {
+		dev_dbg(device->dev, "write to fifo failed: %d bytes required but %d available\n",
+			required, available);
+		mutex_unlock(&device->tx_fifo_lock);
+		return -EAGAIN;
+	}
+
+	retval = kfifo_in(&device->tx_fifo, &instance->tx_cfg,
+			  sizeof(instance->tx_cfg));
+	if (retval != sizeof(instance->tx_cfg))
+		goto abort;
+
+	retval = kfifo_in(&device->tx_fifo, &count, sizeof(size_t));
+	if (retval != sizeof(size_t))
+		goto abort;
+
+	retval = kfifo_from_user(&device->tx_fifo, buf, count, &copied);
+	if (retval || copied != count)
+		goto abort;
+
+	mutex_unlock(&device->tx_fifo_lock);
+
+	/* start transfer */
+	wake_up_interruptible(&device->tx_wait_queue);
+	dev_dbg(device->dev, "write: generated new msg with %d bytes.\n", copied);
+
+	return copied;
+
+abort:
+	dev_warn(device->dev,
+		 "write to fifo failed, non recoverable: 0x%x\n", retval);
+	mutex_unlock(&device->tx_fifo_lock);
+	return -EAGAIN;
+}
+
+static long pi433_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	struct pi433_instance	*instance;
+	struct pi433_device	*device;
+	struct pi433_tx_cfg	tx_cfg;
+	void __user *argp = (void __user *)arg;
+
+	/* Check type and command number */
+	if (_IOC_TYPE(cmd) != PI433_IOC_MAGIC)
+		return -ENOTTY;
+
+	instance = filp->private_data;
+	device = instance->device;
+
+	if (!device)
+		return -ESHUTDOWN;
+
+	switch (cmd) {
+	case PI433_IOC_RD_TX_CFG:
+		if (copy_to_user(argp, &instance->tx_cfg,
+				 sizeof(struct pi433_tx_cfg)))
+			return -EFAULT;
+		break;
+	case PI433_IOC_WR_TX_CFG:
+		if (copy_from_user(&tx_cfg, argp, sizeof(struct pi433_tx_cfg)))
+			return -EFAULT;
+		mutex_lock(&device->tx_fifo_lock);
+		memcpy(&instance->tx_cfg, &tx_cfg, sizeof(struct pi433_tx_cfg));
+		instance->tx_cfg_initialized = true;
+		mutex_unlock(&device->tx_fifo_lock);
+		break;
+	case PI433_IOC_RD_RX_CFG:
+		if (copy_to_user(argp, &device->rx_cfg,
+				 sizeof(struct pi433_rx_cfg)))
+			return -EFAULT;
+		break;
+	case PI433_IOC_WR_RX_CFG:
+		mutex_lock(&device->rx_lock);
+
+		/* during pendig read request, change of config not allowed */
+		if (device->rx_active) {
+			mutex_unlock(&device->rx_lock);
+			return -EAGAIN;
+		}
+
+		if (copy_from_user(&device->rx_cfg, argp,
+				   sizeof(struct pi433_rx_cfg))) {
+			mutex_unlock(&device->rx_lock);
+			return -EFAULT;
+		}
+
+		mutex_unlock(&device->rx_lock);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int pi433_open(struct inode *inode, struct file *filp)
+{
+	struct pi433_device	*device;
+	struct pi433_instance	*instance;
+
+	mutex_lock(&minor_lock);
+	device = idr_find(&pi433_idr, iminor(inode));
+	mutex_unlock(&minor_lock);
+	if (!device) {
+		pr_debug("device: minor %d unknown.\n", iminor(inode));
+		return -ENODEV;
+	}
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance)
+		return -ENOMEM;
+
+	/* setup instance data*/
+	instance->device = device;
+
+	/* instance data as context */
+	filp->private_data = instance;
+	stream_open(inode, filp);
+
+	return 0;
+}
+
+static int pi433_release(struct inode *inode, struct file *filp)
+{
+	struct pi433_instance	*instance;
+
+	instance = filp->private_data;
+	kfree(instance);
+	filp->private_data = NULL;
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int setup_gpio(struct pi433_device *device)
+{
+	char	name[5];
+	int	retval;
+	int	i;
+	const irq_handler_t DIO_irq_handler[NUM_DIO] = {
+		DIO0_irq_handler,
+		DIO1_irq_handler
+	};
+
+	for (i = 0; i < NUM_DIO; i++) {
+		/* "construct" name and get the gpio descriptor */
+		snprintf(name, sizeof(name), "DIO%d", i);
+		device->gpiod[i] = gpiod_get(&device->spi->dev, name,
+					     0 /*GPIOD_IN*/);
+
+		if (device->gpiod[i] == ERR_PTR(-ENOENT)) {
+			dev_dbg(&device->spi->dev,
+				"Could not find entry for %s. Ignoring.\n", name);
+			continue;
+		}
+
+		if (device->gpiod[i] == ERR_PTR(-EBUSY))
+			dev_dbg(&device->spi->dev, "%s is busy.\n", name);
+
+		if (IS_ERR(device->gpiod[i])) {
+			retval = PTR_ERR(device->gpiod[i]);
+			/* release already allocated gpios */
+			for (i--; i >= 0; i--) {
+				free_irq(device->irq_num[i], device);
+				gpiod_put(device->gpiod[i]);
+			}
+			return retval;
+		}
+
+		/* configure the pin */
+		gpiod_unexport(device->gpiod[i]);
+		retval = gpiod_direction_input(device->gpiod[i]);
+		if (retval)
+			return retval;
+
+		/* configure irq */
+		device->irq_num[i] = gpiod_to_irq(device->gpiod[i]);
+		if (device->irq_num[i] < 0) {
+			device->gpiod[i] = ERR_PTR(-EINVAL);
+			return device->irq_num[i];
+		}
+		retval = request_irq(device->irq_num[i],
+				     DIO_irq_handler[i],
+				     0, /* flags */
+				     name,
+				     device);
+
+		if (retval)
+			return retval;
+
+		dev_dbg(&device->spi->dev, "%s successfully configured\n", name);
+	}
+
+	return 0;
+}
+
+static void free_gpio(struct pi433_device *device)
+{
+	int i;
+
+	for (i = 0; i < NUM_DIO; i++) {
+		/* check if gpiod is valid */
+		if (IS_ERR(device->gpiod[i]))
+			continue;
+
+		free_irq(device->irq_num[i], device);
+		gpiod_put(device->gpiod[i]);
+	}
+}
+
+static int pi433_get_minor(struct pi433_device *device)
+{
+	int retval = -ENOMEM;
+
+	mutex_lock(&minor_lock);
+	retval = idr_alloc(&pi433_idr, device, 0, N_PI433_MINORS, GFP_KERNEL);
+	if (retval >= 0) {
+		device->minor = retval;
+		retval = 0;
+	} else if (retval == -ENOSPC) {
+		dev_err(&device->spi->dev, "too many pi433 devices\n");
+		retval = -EINVAL;
+	}
+	mutex_unlock(&minor_lock);
+	return retval;
+}
+
+static void pi433_free_minor(struct pi433_device *dev)
+{
+	mutex_lock(&minor_lock);
+	idr_remove(&pi433_idr, dev->minor);
+	mutex_unlock(&minor_lock);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static const struct file_operations pi433_fops = {
+	.owner =	THIS_MODULE,
+	/*
+	 * REVISIT switch to aio primitives, so that userspace
+	 * gets more complete API coverage.  It'll simplify things
+	 * too, except for the locking.
+	 */
+	.write =	pi433_write,
+	.read =		pi433_read,
+	.unlocked_ioctl = pi433_ioctl,
+	.compat_ioctl = compat_ptr_ioctl,
+	.open =		pi433_open,
+	.release =	pi433_release,
+	.llseek =	no_llseek,
+};
+
+static int pi433_debugfs_regs_show(struct seq_file *m, void *p)
+{
+	struct pi433_device *dev;
+	u8 reg_data[114];
+	int i;
+	char *fmt = "0x%02x, 0x%02x\n";
+	int ret;
+
+	dev = m->private;
+
+	mutex_lock(&dev->tx_fifo_lock);
+	mutex_lock(&dev->rx_lock);
+
+	// wait for on-going operations to finish
+	ret = wait_event_interruptible(dev->rx_wait_queue, !dev->tx_active);
+	if (ret)
+		goto out_unlock;
+
+	ret = wait_event_interruptible(dev->tx_wait_queue, !dev->rx_active);
+	if (ret)
+		goto out_unlock;
+
+	// skip FIFO register (0x0) otherwise this can affect some of uC ops
+	for (i = 1; i < 0x50; i++)
+		reg_data[i] = rf69_read_reg(dev->spi, i);
+
+	reg_data[REG_TESTLNA] = rf69_read_reg(dev->spi, REG_TESTLNA);
+	reg_data[REG_TESTPA1] = rf69_read_reg(dev->spi, REG_TESTPA1);
+	reg_data[REG_TESTPA2] = rf69_read_reg(dev->spi, REG_TESTPA2);
+	reg_data[REG_TESTDAGC] = rf69_read_reg(dev->spi, REG_TESTDAGC);
+	reg_data[REG_TESTAFC] = rf69_read_reg(dev->spi, REG_TESTAFC);
+
+	seq_puts(m, "# reg, val\n");
+
+	for (i = 1; i < 0x50; i++)
+		seq_printf(m, fmt, i, reg_data[i]);
+
+	seq_printf(m, fmt, REG_TESTLNA, reg_data[REG_TESTLNA]);
+	seq_printf(m, fmt, REG_TESTPA1, reg_data[REG_TESTPA1]);
+	seq_printf(m, fmt, REG_TESTPA2, reg_data[REG_TESTPA2]);
+	seq_printf(m, fmt, REG_TESTDAGC, reg_data[REG_TESTDAGC]);
+	seq_printf(m, fmt, REG_TESTAFC, reg_data[REG_TESTAFC]);
+
+out_unlock:
+	mutex_unlock(&dev->rx_lock);
+	mutex_unlock(&dev->tx_fifo_lock);
+
+	return ret;
+}
+DEFINE_SHOW_ATTRIBUTE(pi433_debugfs_regs);
+
+/*-------------------------------------------------------------------------*/
+
+static int pi433_probe(struct spi_device *spi)
+{
+	struct pi433_device	*device;
+	int			retval;
+	struct dentry		*entry;
+
+	/* setup spi parameters */
+	spi->mode = 0x00;
+	spi->bits_per_word = 8;
+	/*
+	 * spi->max_speed_hz = 10000000;
+	 * 1MHz already set by device tree overlay
+	 */
+
+	retval = spi_setup(spi);
+	if (retval) {
+		dev_dbg(&spi->dev, "configuration of SPI interface failed!\n");
+		return retval;
+	}
+
+	dev_dbg(&spi->dev,
+		"spi interface setup: mode 0x%2x, %d bits per word, %dhz max speed\n",
+		spi->mode, spi->bits_per_word, spi->max_speed_hz);
+
+	/* read chip version */
+	retval = rf69_get_version(spi);
+	if (retval < 0)
+		return retval;
+
+	switch (retval) {
+	case 0x24:
+		dev_dbg(&spi->dev, "found pi433 (ver. 0x%x)\n", retval);
+		break;
+	default:
+		dev_dbg(&spi->dev, "unknown chip version: 0x%x\n", retval);
+		return -ENODEV;
+	}
+
+	/* Allocate driver data */
+	device = kzalloc(sizeof(*device), GFP_KERNEL);
+	if (!device)
+		return -ENOMEM;
+
+	/* Initialize the driver data */
+	device->spi = spi;
+	device->rx_active = false;
+	device->tx_active = false;
+	device->interrupt_rx_allowed = false;
+
+	/* init rx buffer */
+	device->rx_buffer = kmalloc(MAX_MSG_SIZE, GFP_KERNEL);
+	if (!device->rx_buffer) {
+		retval = -ENOMEM;
+		goto RX_failed;
+	}
+
+	/* init wait queues */
+	init_waitqueue_head(&device->tx_wait_queue);
+	init_waitqueue_head(&device->rx_wait_queue);
+	init_waitqueue_head(&device->fifo_wait_queue);
+
+	/* init fifo */
+	INIT_KFIFO(device->tx_fifo);
+
+	/* init mutexes and locks */
+	mutex_init(&device->tx_fifo_lock);
+	mutex_init(&device->rx_lock);
+
+	/* setup GPIO (including irq_handler) for the different DIOs */
+	retval = setup_gpio(device);
+	if (retval) {
+		dev_dbg(&spi->dev, "setup of GPIOs failed\n");
+		goto GPIO_failed;
+	}
+
+	/* setup the radio module */
+	retval = rf69_set_mode(spi, standby);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_set_data_mode(spi, DATAMODUL_MODE_PACKET);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_enable_amplifier(spi, MASK_PALEVEL_PA0);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_disable_amplifier(spi, MASK_PALEVEL_PA1);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_disable_amplifier(spi, MASK_PALEVEL_PA2);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_set_output_power_level(spi, 13);
+	if (retval < 0)
+		goto minor_failed;
+	retval = rf69_set_antenna_impedance(spi, fifty_ohm);
+	if (retval < 0)
+		goto minor_failed;
+
+	/* determ minor number */
+	retval = pi433_get_minor(device);
+	if (retval) {
+		dev_dbg(&spi->dev, "get of minor number failed\n");
+		goto minor_failed;
+	}
+
+	/* create device */
+	device->devt = MKDEV(MAJOR(pi433_dev), device->minor);
+	device->dev = device_create(pi433_class,
+				    &spi->dev,
+				    device->devt,
+				    device,
+				    "pi433.%d",
+				    device->minor);
+	if (IS_ERR(device->dev)) {
+		pr_err("pi433: device register failed\n");
+		retval = PTR_ERR(device->dev);
+		goto device_create_failed;
+	} else {
+		dev_dbg(device->dev,
+			"created device for major %d, minor %d\n",
+			MAJOR(pi433_dev),
+			device->minor);
+	}
+
+	/* start tx thread */
+	device->tx_task_struct = kthread_run(pi433_tx_thread,
+					     device,
+					     "pi433.%d_tx_task",
+					     device->minor);
+	if (IS_ERR(device->tx_task_struct)) {
+		dev_dbg(device->dev, "start of send thread failed\n");
+		retval = PTR_ERR(device->tx_task_struct);
+		goto send_thread_failed;
+	}
+
+	/* create cdev */
+	device->cdev = cdev_alloc();
+	if (!device->cdev) {
+		dev_dbg(device->dev, "allocation of cdev failed\n");
+		retval = -ENOMEM;
+		goto cdev_failed;
+	}
+	device->cdev->owner = THIS_MODULE;
+	cdev_init(device->cdev, &pi433_fops);
+	retval = cdev_add(device->cdev, device->devt, 1);
+	if (retval) {
+		dev_dbg(device->dev, "register of cdev failed\n");
+		goto del_cdev;
+	}
+
+	/* spi setup */
+	spi_set_drvdata(spi, device);
+
+	entry = debugfs_create_dir(dev_name(device->dev), root_dir);
+	debugfs_create_file("regs", 0400, entry, device, &pi433_debugfs_regs_fops);
+
+	return 0;
+
+del_cdev:
+	cdev_del(device->cdev);
+cdev_failed:
+	kthread_stop(device->tx_task_struct);
+send_thread_failed:
+	device_destroy(pi433_class, device->devt);
+device_create_failed:
+	pi433_free_minor(device);
+minor_failed:
+	free_gpio(device);
+GPIO_failed:
+	kfree(device->rx_buffer);
+RX_failed:
+	kfree(device);
+
+	return retval;
+}
+
+static void pi433_remove(struct spi_device *spi)
+{
+	struct pi433_device	*device = spi_get_drvdata(spi);
+
+	debugfs_lookup_and_remove(dev_name(device->dev), root_dir);
+
+	/* free GPIOs */
+	free_gpio(device);
+
+	/* make sure ops on existing fds can abort cleanly */
+	device->spi = NULL;
+
+	kthread_stop(device->tx_task_struct);
+
+	device_destroy(pi433_class, device->devt);
+
+	cdev_del(device->cdev);
+
+	pi433_free_minor(device);
+
+	kfree(device->rx_buffer);
+	kfree(device);
+}
+
+static const struct of_device_id pi433_dt_ids[] = {
+	{ .compatible = "Smarthome-Wolf,pi433" },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, pi433_dt_ids);
+
+static struct spi_driver pi433_spi_driver = {
+	.driver = {
+		.name =		"pi433",
+		.owner =	THIS_MODULE,
+		.of_match_table = of_match_ptr(pi433_dt_ids),
+	},
+	.probe =	pi433_probe,
+	.remove =	pi433_remove,
+
+	/*
+	 * NOTE:  suspend/resume methods are not necessary here.
+	 * We don't do anything except pass the requests to/from
+	 * the underlying controller.  The refrigerator handles
+	 * most issues; the controller driver handles the rest.
+	 */
+};
+
+/*-------------------------------------------------------------------------*/
+
+static int __init pi433_init(void)
+{
+	int status;
+
+	/*
+	 * If MAX_MSG_SIZE is smaller then FIFO_SIZE, the driver won't
+	 * work stable - risk of buffer overflow
+	 */
+	if (MAX_MSG_SIZE < FIFO_SIZE)
+		return -EINVAL;
+
+	/*
+	 * Claim device numbers.  Then register a class
+	 * that will key udev/mdev to add/remove /dev nodes.
+	 * Last, register the driver which manages those device numbers.
+	 */
+	status = alloc_chrdev_region(&pi433_dev, 0, N_PI433_MINORS, "pi433");
+	if (status < 0)
+		return status;
+
+	pi433_class = class_create(THIS_MODULE, "pi433");
+	if (IS_ERR(pi433_class)) {
+		unregister_chrdev(MAJOR(pi433_dev),
+				  pi433_spi_driver.driver.name);
+		return PTR_ERR(pi433_class);
+	}
+
+	root_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+
+	status = spi_register_driver(&pi433_spi_driver);
+	if (status < 0) {
+		class_destroy(pi433_class);
+		unregister_chrdev(MAJOR(pi433_dev),
+				  pi433_spi_driver.driver.name);
+	}
+
+	return status;
+}
+
+module_init(pi433_init);
+
+static void __exit pi433_exit(void)
+{
+	spi_unregister_driver(&pi433_spi_driver);
+	class_destroy(pi433_class);
+	unregister_chrdev(MAJOR(pi433_dev), pi433_spi_driver.driver.name);
+	debugfs_remove(root_dir);
+}
+module_exit(pi433_exit);
+
+MODULE_AUTHOR("Marcus Wolf, <linux@wolf-entwicklungen.de>");
+MODULE_DESCRIPTION("Driver for Pi433");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("spi:pi433");
diff --git a/drivers/staging/pi433/pi433_if.h b/drivers/staging/pi433/pi433_if.h
new file mode 100644
index 000000000..25ee0b77a
--- /dev/null
+++ b/drivers/staging/pi433/pi433_if.h
@@ -0,0 +1,148 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * userspace interface for pi433 radio module
+ *
+ * Pi433 is a 433MHz radio module for the Raspberry Pi.
+ * It is based on the HopeRf Module RFM69CW. Therefore, inside of this
+ * driver you'll find an abstraction of the rf69 chip.
+ *
+ * If needed this driver could also be extended to support other
+ * devices based on HopeRf rf69 as well as HopeRf modules with a similar
+ * interface such as RFM69HCW, RFM12, RFM95 and so on.
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+
+#ifndef PI433_H
+#define PI433_H
+
+#include <linux/types.h>
+#include "rf69_enum.h"
+
+/*---------------------------------------------------------------------------*/
+
+enum option_on_off {
+	OPTION_OFF,
+	OPTION_ON
+};
+
+/* IOCTL structs and commands */
+
+/**
+ * struct pi433_tx_cfg
+ * describes the configuration of the radio module for sending data
+ * @frequency:
+ * @bit_rate:
+ * @modulation:
+ * @data_mode:
+ * @preamble_length:
+ * @sync_pattern:
+ * @tx_start_condition:
+ * @payload_length:
+ * @repetitions:
+ *
+ * ATTENTION:
+ * If the contents of 'pi433_tx_cfg' ever change
+ * incompatibly, then the ioctl number (see define below) must change.
+ *
+ * NOTE: struct layout is the same in 64bit and 32bit userspace.
+ */
+#define PI433_TX_CFG_IOCTL_NR	0
+struct pi433_tx_cfg {
+	__u32			frequency;
+	__u16			bit_rate;
+	__u32			dev_frequency;
+	enum modulation		modulation;
+	enum mod_shaping	mod_shaping;
+
+	enum pa_ramp		pa_ramp;
+
+	enum tx_start_condition	tx_start_condition;
+
+	__u16			repetitions;
+
+	/* packet format */
+	enum option_on_off	enable_preamble;
+	enum option_on_off	enable_sync;
+	enum option_on_off	enable_length_byte;
+	enum option_on_off	enable_address_byte;
+	enum option_on_off	enable_crc;
+
+	__u16			preamble_length;
+	__u8			sync_length;
+	__u8			fixed_message_length;
+
+	__u8			sync_pattern[8];
+	__u8			address_byte;
+};
+
+/**
+ * struct pi433_rx_cfg
+ * describes the configuration of the radio module for receiving data
+ * @frequency:
+ * @bit_rate:
+ * @modulation:
+ * @data_mode:
+ * @preamble_length:
+ * @sync_pattern:
+ * @tx_start_condition:
+ * @payload_length:
+ * @repetitions:
+ *
+ * ATTENTION:
+ * If the contents of 'pi433_rx_cfg' ever change
+ * incompatibly, then the ioctl number (see define below) must change
+ *
+ * NOTE: struct layout is the same in 64bit and 32bit userspace.
+ */
+#define PI433_RX_CFG_IOCTL_NR	1
+struct pi433_rx_cfg {
+	__u32			frequency;
+	__u16			bit_rate;
+	__u32			dev_frequency;
+
+	enum modulation		modulation;
+
+	__u8			rssi_threshold;
+	enum threshold_decrement threshold_decrement;
+	enum antenna_impedance	antenna_impedance;
+	enum lna_gain		lna_gain;
+	enum mantisse		bw_mantisse;	/* normal: 0x50 */
+	__u8			bw_exponent;	/* during AFC: 0x8b */
+	enum dagc		dagc;
+
+	/* packet format */
+	enum option_on_off	enable_sync;
+
+	/* should be used in combination with sync, only */
+	enum option_on_off	enable_length_byte;
+
+	/* operational with sync, only */
+	enum address_filtering	enable_address_filtering;
+
+	/* only operational, if sync on and fixed length or length byte is used */
+	enum option_on_off	enable_crc;
+
+	__u8			sync_length;
+	__u8			fixed_message_length;
+	__u32			bytes_to_drop;
+
+	__u8			sync_pattern[8];
+	__u8			node_address;
+	__u8			broadcast_address;
+};
+
+#define PI433_IOC_MAGIC	'r'
+
+#define PI433_IOC_RD_TX_CFG                                             \
+	_IOR(PI433_IOC_MAGIC, PI433_TX_CFG_IOCTL_NR, char[sizeof(struct pi433_tx_cfg)])
+#define PI433_IOC_WR_TX_CFG                                             \
+	_IOW(PI433_IOC_MAGIC, PI433_TX_CFG_IOCTL_NR, char[sizeof(struct pi433_tx_cfg)])
+
+#define PI433_IOC_RD_RX_CFG                                             \
+	_IOR(PI433_IOC_MAGIC, PI433_RX_CFG_IOCTL_NR, char[sizeof(struct pi433_rx_cfg)])
+#define PI433_IOC_WR_RX_CFG                                             \
+	_IOW(PI433_IOC_MAGIC, PI433_RX_CFG_IOCTL_NR, char[sizeof(struct pi433_rx_cfg)])
+
+#endif /* PI433_H */
diff --git a/drivers/staging/pi433/rf69.c b/drivers/staging/pi433/rf69.c
new file mode 100644
index 000000000..8c7fab6a4
--- /dev/null
+++ b/drivers/staging/pi433/rf69.c
@@ -0,0 +1,832 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * abstraction of the spi interface of HopeRf rf69 radio module
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+
+#include <linux/types.h>
+#include <linux/spi/spi.h>
+
+#include "rf69.h"
+#include "rf69_registers.h"
+
+#define F_OSC	  32000000 /* in Hz */
+#define FIFO_SIZE 66	   /* in byte */
+
+/*-------------------------------------------------------------------------*/
+
+u8 rf69_read_reg(struct spi_device *spi, u8 addr)
+{
+	return spi_w8r8(spi, addr);
+}
+
+static int rf69_write_reg(struct spi_device *spi, u8 addr, u8 value)
+{
+	char buffer[2];
+
+	buffer[0] = addr | WRITE_BIT;
+	buffer[1] = value;
+
+	return spi_write(spi, &buffer, ARRAY_SIZE(buffer));
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int rf69_set_bit(struct spi_device *spi, u8 reg, u8 mask)
+{
+	u8 tmp;
+
+	tmp = rf69_read_reg(spi, reg);
+	tmp = tmp | mask;
+	return rf69_write_reg(spi, reg, tmp);
+}
+
+static int rf69_clear_bit(struct spi_device *spi, u8 reg, u8 mask)
+{
+	u8 tmp;
+
+	tmp = rf69_read_reg(spi, reg);
+	tmp = tmp & ~mask;
+	return rf69_write_reg(spi, reg, tmp);
+}
+
+static inline int rf69_read_mod_write(struct spi_device *spi, u8 reg,
+				      u8 mask, u8 value)
+{
+	u8 tmp;
+
+	tmp = rf69_read_reg(spi, reg);
+	tmp = (tmp & ~mask) | value;
+	return rf69_write_reg(spi, reg, tmp);
+}
+
+/*-------------------------------------------------------------------------*/
+
+int rf69_get_version(struct spi_device *spi)
+{
+	return rf69_read_reg(spi, REG_VERSION);
+}
+
+int rf69_set_mode(struct spi_device *spi, enum mode mode)
+{
+	static const u8 mode_map[] = {
+		[transmit] = OPMODE_MODE_TRANSMIT,
+		[receive] = OPMODE_MODE_RECEIVE,
+		[synthesizer] = OPMODE_MODE_SYNTHESIZER,
+		[standby] = OPMODE_MODE_STANDBY,
+		[mode_sleep] = OPMODE_MODE_SLEEP,
+	};
+
+	if (unlikely(mode >= ARRAY_SIZE(mode_map))) {
+		dev_dbg(&spi->dev, "set: illegal mode %u\n", mode);
+		return -EINVAL;
+	}
+
+	return rf69_read_mod_write(spi, REG_OPMODE, MASK_OPMODE_MODE,
+				   mode_map[mode]);
+
+	/*
+	 * we are using packet mode, so this check is not really needed
+	 * but waiting for mode ready is necessary when going from sleep
+	 * because the FIFO may not be immediately available from previous mode
+	 * while (_mode == RF69_MODE_SLEEP && (READ_REG(REG_IRQFLAGS1) &
+		  RF_IRQFLAGS1_MODEREADY) == 0x00); // Wait for ModeReady
+	 */
+}
+
+int rf69_set_data_mode(struct spi_device *spi, u8 data_mode)
+{
+	return rf69_read_mod_write(spi, REG_DATAMODUL, MASK_DATAMODUL_MODE,
+				   data_mode);
+}
+
+int rf69_set_modulation(struct spi_device *spi, enum modulation modulation)
+{
+	static const u8 modulation_map[] = {
+		[OOK] = DATAMODUL_MODULATION_TYPE_OOK,
+		[FSK] = DATAMODUL_MODULATION_TYPE_FSK,
+	};
+
+	if (unlikely(modulation >= ARRAY_SIZE(modulation_map))) {
+		dev_dbg(&spi->dev, "set: illegal modulation %u\n", modulation);
+		return -EINVAL;
+	}
+
+	return rf69_read_mod_write(spi, REG_DATAMODUL,
+				   MASK_DATAMODUL_MODULATION_TYPE,
+				   modulation_map[modulation]);
+}
+
+static enum modulation rf69_get_modulation(struct spi_device *spi)
+{
+	u8 modulation_reg;
+
+	modulation_reg = rf69_read_reg(spi, REG_DATAMODUL);
+
+	switch (modulation_reg & MASK_DATAMODUL_MODULATION_TYPE) {
+	case DATAMODUL_MODULATION_TYPE_OOK:
+		return OOK;
+	case DATAMODUL_MODULATION_TYPE_FSK:
+		return FSK;
+	default:
+		return UNDEF;
+	}
+}
+
+int rf69_set_modulation_shaping(struct spi_device *spi,
+				enum mod_shaping mod_shaping)
+{
+	switch (rf69_get_modulation(spi)) {
+	case FSK:
+		switch (mod_shaping) {
+		case SHAPING_OFF:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_NONE);
+		case SHAPING_1_0:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_1_0);
+		case SHAPING_0_5:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_0_5);
+		case SHAPING_0_3:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_0_3);
+		default:
+			dev_dbg(&spi->dev, "set: illegal mod shaping for FSK %u\n", mod_shaping);
+			return -EINVAL;
+		}
+	case OOK:
+		switch (mod_shaping) {
+		case SHAPING_OFF:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_NONE);
+		case SHAPING_BR:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_BR);
+		case SHAPING_2BR:
+			return rf69_read_mod_write(spi, REG_DATAMODUL,
+						   MASK_DATAMODUL_MODULATION_SHAPE,
+						   DATAMODUL_MODULATION_SHAPE_2BR);
+		default:
+			dev_dbg(&spi->dev, "set: illegal mod shaping for OOK %u\n", mod_shaping);
+			return -EINVAL;
+		}
+	default:
+		dev_dbg(&spi->dev, "set: modulation undefined\n");
+		return -EINVAL;
+	}
+}
+
+int rf69_set_bit_rate(struct spi_device *spi, u16 bit_rate)
+{
+	int retval;
+	u32 bit_rate_reg;
+	u8 msb;
+	u8 lsb;
+	enum modulation mod;
+
+	// check if modulation is configured
+	mod = rf69_get_modulation(spi);
+	if (mod == UNDEF) {
+		dev_dbg(&spi->dev, "setBitRate: modulation is undefined\n");
+		return -EINVAL;
+	}
+
+	// check input value
+	if (bit_rate < 1200 || (mod == OOK && bit_rate > 32768)) {
+		dev_dbg(&spi->dev, "setBitRate: illegal input param\n");
+		return -EINVAL;
+	}
+
+	// calculate reg settings
+	bit_rate_reg = (F_OSC / bit_rate);
+
+	msb = (bit_rate_reg & 0xff00) >> 8;
+	lsb = (bit_rate_reg & 0xff);
+
+	// transmit to RF 69
+	retval = rf69_write_reg(spi, REG_BITRATE_MSB, msb);
+	if (retval)
+		return retval;
+	retval = rf69_write_reg(spi, REG_BITRATE_LSB, lsb);
+	if (retval)
+		return retval;
+
+	return 0;
+}
+
+int rf69_set_deviation(struct spi_device *spi, u32 deviation)
+{
+	int retval;
+	u64 f_reg;
+	u64 f_step;
+	u32 bit_rate_reg;
+	u32 bit_rate;
+	u8 msb;
+	u8 lsb;
+	u64 factor = 1000000; // to improve precision of calculation
+
+	// calculate bit rate
+	bit_rate_reg = rf69_read_reg(spi, REG_BITRATE_MSB) << 8;
+	bit_rate_reg |= rf69_read_reg(spi, REG_BITRATE_LSB);
+	bit_rate = F_OSC / bit_rate_reg;
+
+	/*
+	 * frequency deviation must exceed 600 Hz but not exceed
+	 * 500kHz when taking bitrate dependency into consideration
+	 * to ensure proper modulation
+	 */
+	if (deviation < 600 || (deviation + (bit_rate / 2)) > 500000) {
+		dev_dbg(&spi->dev,
+			"set_deviation: illegal input param: %u\n", deviation);
+		return -EINVAL;
+	}
+
+	// calculat f step
+	f_step = F_OSC * factor;
+	do_div(f_step, 524288); //  524288 = 2^19
+
+	// calculate register settings
+	f_reg = deviation * factor;
+	do_div(f_reg, f_step);
+
+	msb = (f_reg & 0xff00) >> 8;
+	lsb = (f_reg & 0xff);
+
+	// check msb
+	if (msb & ~FDEVMASB_MASK) {
+		dev_dbg(&spi->dev, "set_deviation: err in calc of msb\n");
+		return -EINVAL;
+	}
+
+	// write to chip
+	retval = rf69_write_reg(spi, REG_FDEV_MSB, msb);
+	if (retval)
+		return retval;
+	retval = rf69_write_reg(spi, REG_FDEV_LSB, lsb);
+	if (retval)
+		return retval;
+
+	return 0;
+}
+
+int rf69_set_frequency(struct spi_device *spi, u32 frequency)
+{
+	int retval;
+	u32 f_max;
+	u64 f_reg;
+	u64 f_step;
+	u8 msb;
+	u8 mid;
+	u8 lsb;
+	u64 factor = 1000000; // to improve precision of calculation
+
+	// calculat f step
+	f_step = F_OSC * factor;
+	do_div(f_step, 524288); //  524288 = 2^19
+
+	// check input value
+	f_max = div_u64(f_step * 8388608, factor);
+	if (frequency > f_max) {
+		dev_dbg(&spi->dev, "setFrequency: illegal input param\n");
+		return -EINVAL;
+	}
+
+	// calculate reg settings
+	f_reg = frequency * factor;
+	do_div(f_reg, f_step);
+
+	msb = (f_reg & 0xff0000) >> 16;
+	mid = (f_reg & 0xff00)   >>  8;
+	lsb = (f_reg & 0xff);
+
+	// write to chip
+	retval = rf69_write_reg(spi, REG_FRF_MSB, msb);
+	if (retval)
+		return retval;
+	retval = rf69_write_reg(spi, REG_FRF_MID, mid);
+	if (retval)
+		return retval;
+	retval = rf69_write_reg(spi, REG_FRF_LSB, lsb);
+	if (retval)
+		return retval;
+
+	return 0;
+}
+
+int rf69_enable_amplifier(struct spi_device *spi, u8 amplifier_mask)
+{
+	return rf69_set_bit(spi, REG_PALEVEL, amplifier_mask);
+}
+
+int rf69_disable_amplifier(struct spi_device *spi, u8 amplifier_mask)
+{
+	return rf69_clear_bit(spi, REG_PALEVEL, amplifier_mask);
+}
+
+int rf69_set_output_power_level(struct spi_device *spi, u8 power_level)
+{
+	u8 pa_level, ocp, test_pa1, test_pa2;
+	bool pa0, pa1, pa2, high_power;
+	u8 min_power_level;
+
+	// check register pa_level
+	pa_level = rf69_read_reg(spi, REG_PALEVEL);
+	pa0 = pa_level & MASK_PALEVEL_PA0;
+	pa1 = pa_level & MASK_PALEVEL_PA1;
+	pa2 = pa_level & MASK_PALEVEL_PA2;
+
+	// check high power mode
+	ocp = rf69_read_reg(spi, REG_OCP);
+	test_pa1 = rf69_read_reg(spi, REG_TESTPA1);
+	test_pa2 = rf69_read_reg(spi, REG_TESTPA2);
+	high_power = (ocp == 0x0f) && (test_pa1 == 0x5d) && (test_pa2 == 0x7c);
+
+	if (pa0 && !pa1 && !pa2) {
+		power_level += 18;
+		min_power_level = 0;
+	} else if (!pa0 && pa1 && !pa2) {
+		power_level += 18;
+		min_power_level = 16;
+	} else if (!pa0 && pa1 && pa2) {
+		if (high_power)
+			power_level += 11;
+		else
+			power_level += 14;
+		min_power_level = 16;
+	} else {
+		goto failed;
+	}
+
+	// check input value
+	if (power_level > 0x1f)
+		goto failed;
+
+	if (power_level < min_power_level)
+		goto failed;
+
+	// write value
+	return rf69_read_mod_write(spi, REG_PALEVEL, MASK_PALEVEL_OUTPUT_POWER,
+				   power_level);
+failed:
+	dev_dbg(&spi->dev, "set: illegal power level %u\n", power_level);
+	return -EINVAL;
+}
+
+int rf69_set_pa_ramp(struct spi_device *spi, enum pa_ramp pa_ramp)
+{
+	static const u8 pa_ramp_map[] = {
+		[ramp3400] = PARAMP_3400,
+		[ramp2000] = PARAMP_2000,
+		[ramp1000] = PARAMP_1000,
+		[ramp500] = PARAMP_500,
+		[ramp250] = PARAMP_250,
+		[ramp125] = PARAMP_125,
+		[ramp100] = PARAMP_100,
+		[ramp62] = PARAMP_62,
+		[ramp50] = PARAMP_50,
+		[ramp40] = PARAMP_40,
+		[ramp31] = PARAMP_31,
+		[ramp25] = PARAMP_25,
+		[ramp20] = PARAMP_20,
+		[ramp15] = PARAMP_15,
+		[ramp10] = PARAMP_10,
+	};
+
+	if (unlikely(pa_ramp >= ARRAY_SIZE(pa_ramp_map))) {
+		dev_dbg(&spi->dev, "set: illegal pa_ramp %u\n", pa_ramp);
+		return -EINVAL;
+	}
+
+	return rf69_write_reg(spi, REG_PARAMP, pa_ramp_map[pa_ramp]);
+}
+
+int rf69_set_antenna_impedance(struct spi_device *spi,
+			       enum antenna_impedance antenna_impedance)
+{
+	switch (antenna_impedance) {
+	case fifty_ohm:
+		return rf69_clear_bit(spi, REG_LNA, MASK_LNA_ZIN);
+	case two_hundred_ohm:
+		return rf69_set_bit(spi, REG_LNA, MASK_LNA_ZIN);
+	default:
+		dev_dbg(&spi->dev, "set: illegal antenna impedance %u\n", antenna_impedance);
+		return -EINVAL;
+	}
+}
+
+int rf69_set_lna_gain(struct spi_device *spi, enum lna_gain lna_gain)
+{
+	static const u8 lna_gain_map[] = {
+		[automatic] = LNA_GAIN_AUTO,
+		[max] = LNA_GAIN_MAX,
+		[max_minus_6] = LNA_GAIN_MAX_MINUS_6,
+		[max_minus_12] = LNA_GAIN_MAX_MINUS_12,
+		[max_minus_24] = LNA_GAIN_MAX_MINUS_24,
+		[max_minus_36] = LNA_GAIN_MAX_MINUS_36,
+		[max_minus_48] = LNA_GAIN_MAX_MINUS_48,
+	};
+
+	if (unlikely(lna_gain >= ARRAY_SIZE(lna_gain_map))) {
+		dev_dbg(&spi->dev, "set: illegal lna gain %u\n", lna_gain);
+		return -EINVAL;
+	}
+
+	return rf69_read_mod_write(spi, REG_LNA, MASK_LNA_GAIN,
+				   lna_gain_map[lna_gain]);
+}
+
+static int rf69_set_bandwidth_intern(struct spi_device *spi, u8 reg,
+				     enum mantisse mantisse, u8 exponent)
+{
+	u8 bandwidth;
+
+	// check value for mantisse and exponent
+	if (exponent > 7) {
+		dev_dbg(&spi->dev, "set: illegal bandwidth exponent %u\n", exponent);
+		return -EINVAL;
+	}
+
+	if (mantisse != mantisse16 &&
+	    mantisse != mantisse20 &&
+	    mantisse != mantisse24) {
+		dev_dbg(&spi->dev, "set: illegal bandwidth mantisse %u\n", mantisse);
+		return -EINVAL;
+	}
+
+	// read old value
+	bandwidth = rf69_read_reg(spi, reg);
+
+	// "delete" mantisse and exponent = just keep the DCC setting
+	bandwidth = bandwidth & MASK_BW_DCC_FREQ;
+
+	// add new mantisse
+	switch (mantisse) {
+	case mantisse16:
+		bandwidth = bandwidth | BW_MANT_16;
+		break;
+	case mantisse20:
+		bandwidth = bandwidth | BW_MANT_20;
+		break;
+	case mantisse24:
+		bandwidth = bandwidth | BW_MANT_24;
+		break;
+	}
+
+	// add new exponent
+	bandwidth = bandwidth | exponent;
+
+	// write back
+	return rf69_write_reg(spi, reg, bandwidth);
+}
+
+int rf69_set_bandwidth(struct spi_device *spi, enum mantisse mantisse,
+		       u8 exponent)
+{
+	return rf69_set_bandwidth_intern(spi, REG_RXBW, mantisse, exponent);
+}
+
+int rf69_set_bandwidth_during_afc(struct spi_device *spi,
+				  enum mantisse mantisse,
+				  u8 exponent)
+{
+	return rf69_set_bandwidth_intern(spi, REG_AFCBW, mantisse, exponent);
+}
+
+int rf69_set_ook_threshold_dec(struct spi_device *spi,
+			       enum threshold_decrement threshold_decrement)
+{
+	static const u8 td_map[] = {
+		[dec_every8th] = OOKPEAK_THRESHDEC_EVERY_8TH,
+		[dec_every4th] = OOKPEAK_THRESHDEC_EVERY_4TH,
+		[dec_every2nd] = OOKPEAK_THRESHDEC_EVERY_2ND,
+		[dec_once] = OOKPEAK_THRESHDEC_ONCE,
+		[dec_twice] = OOKPEAK_THRESHDEC_TWICE,
+		[dec_4times] = OOKPEAK_THRESHDEC_4_TIMES,
+		[dec_8times] = OOKPEAK_THRESHDEC_8_TIMES,
+		[dec_16times] = OOKPEAK_THRESHDEC_16_TIMES,
+	};
+
+	if (unlikely(threshold_decrement >= ARRAY_SIZE(td_map))) {
+		dev_dbg(&spi->dev, "set: illegal OOK threshold decrement %u\n",
+			threshold_decrement);
+		return -EINVAL;
+	}
+
+	return rf69_read_mod_write(spi, REG_OOKPEAK, MASK_OOKPEAK_THRESDEC,
+				   td_map[threshold_decrement]);
+}
+
+int rf69_set_dio_mapping(struct spi_device *spi, u8 dio_number, u8 value)
+{
+	u8 mask;
+	u8 shift;
+	u8 dio_addr;
+	u8 dio_value;
+
+	switch (dio_number) {
+	case 0:
+		mask = MASK_DIO0;
+		shift = SHIFT_DIO0;
+		dio_addr = REG_DIOMAPPING1;
+		break;
+	case 1:
+		mask = MASK_DIO1;
+		shift = SHIFT_DIO1;
+		dio_addr = REG_DIOMAPPING1;
+		break;
+	case 2:
+		mask = MASK_DIO2;
+		shift = SHIFT_DIO2;
+		dio_addr = REG_DIOMAPPING1;
+		break;
+	case 3:
+		mask = MASK_DIO3;
+		shift = SHIFT_DIO3;
+		dio_addr = REG_DIOMAPPING1;
+		break;
+	case 4:
+		mask = MASK_DIO4;
+		shift = SHIFT_DIO4;
+		dio_addr = REG_DIOMAPPING2;
+		break;
+	case 5:
+		mask = MASK_DIO5;
+		shift = SHIFT_DIO5;
+		dio_addr = REG_DIOMAPPING2;
+		break;
+	default:
+		dev_dbg(&spi->dev, "set: illegal dio number %u\n", dio_number);
+		return -EINVAL;
+	}
+
+	// read reg
+	dio_value = rf69_read_reg(spi, dio_addr);
+	// delete old value
+	dio_value = dio_value & ~mask;
+	// add new value
+	dio_value = dio_value | value << shift;
+	// write back
+	return rf69_write_reg(spi, dio_addr, dio_value);
+}
+
+int rf69_set_rssi_threshold(struct spi_device *spi, u8 threshold)
+{
+	/* no value check needed - u8 exactly matches register size */
+
+	return rf69_write_reg(spi, REG_RSSITHRESH, threshold);
+}
+
+int rf69_set_preamble_length(struct spi_device *spi, u16 preamble_length)
+{
+	int retval;
+	u8 msb, lsb;
+
+	/* no value check needed - u16 exactly matches register size */
+
+	/* calculate reg settings */
+	msb = (preamble_length & 0xff00) >> 8;
+	lsb = (preamble_length & 0xff);
+
+	/* transmit to chip */
+	retval = rf69_write_reg(spi, REG_PREAMBLE_MSB, msb);
+	if (retval)
+		return retval;
+	return rf69_write_reg(spi, REG_PREAMBLE_LSB, lsb);
+}
+
+int rf69_enable_sync(struct spi_device *spi)
+{
+	return rf69_set_bit(spi, REG_SYNC_CONFIG, MASK_SYNC_CONFIG_SYNC_ON);
+}
+
+int rf69_disable_sync(struct spi_device *spi)
+{
+	return rf69_clear_bit(spi, REG_SYNC_CONFIG, MASK_SYNC_CONFIG_SYNC_ON);
+}
+
+int rf69_set_fifo_fill_condition(struct spi_device *spi,
+				 enum fifo_fill_condition fifo_fill_condition)
+{
+	switch (fifo_fill_condition) {
+	case always:
+		return rf69_set_bit(spi, REG_SYNC_CONFIG,
+				    MASK_SYNC_CONFIG_FIFO_FILL_CONDITION);
+	case after_sync_interrupt:
+		return rf69_clear_bit(spi, REG_SYNC_CONFIG,
+				      MASK_SYNC_CONFIG_FIFO_FILL_CONDITION);
+	default:
+		dev_dbg(&spi->dev, "set: illegal fifo fill condition %u\n", fifo_fill_condition);
+		return -EINVAL;
+	}
+}
+
+int rf69_set_sync_size(struct spi_device *spi, u8 sync_size)
+{
+	// check input value
+	if (sync_size > 0x07) {
+		dev_dbg(&spi->dev, "set: illegal sync size %u\n", sync_size);
+		return -EINVAL;
+	}
+
+	// write value
+	return rf69_read_mod_write(spi, REG_SYNC_CONFIG,
+				   MASK_SYNC_CONFIG_SYNC_SIZE,
+				   (sync_size << 3));
+}
+
+int rf69_set_sync_values(struct spi_device *spi, u8 sync_values[8])
+{
+	int retval = 0;
+
+	retval += rf69_write_reg(spi, REG_SYNCVALUE1, sync_values[0]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE2, sync_values[1]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE3, sync_values[2]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE4, sync_values[3]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE5, sync_values[4]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE6, sync_values[5]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE7, sync_values[6]);
+	retval += rf69_write_reg(spi, REG_SYNCVALUE8, sync_values[7]);
+
+	return retval;
+}
+
+int rf69_set_packet_format(struct spi_device *spi,
+			   enum packet_format packet_format)
+{
+	switch (packet_format) {
+	case packet_length_var:
+		return rf69_set_bit(spi, REG_PACKETCONFIG1,
+				    MASK_PACKETCONFIG1_PACKET_FORMAT_VARIABLE);
+	case packet_length_fix:
+		return rf69_clear_bit(spi, REG_PACKETCONFIG1,
+				      MASK_PACKETCONFIG1_PACKET_FORMAT_VARIABLE);
+	default:
+		dev_dbg(&spi->dev, "set: illegal packet format %u\n", packet_format);
+		return -EINVAL;
+	}
+}
+
+int rf69_enable_crc(struct spi_device *spi)
+{
+	return rf69_set_bit(spi, REG_PACKETCONFIG1, MASK_PACKETCONFIG1_CRC_ON);
+}
+
+int rf69_disable_crc(struct spi_device *spi)
+{
+	return rf69_clear_bit(spi, REG_PACKETCONFIG1, MASK_PACKETCONFIG1_CRC_ON);
+}
+
+int rf69_set_address_filtering(struct spi_device *spi,
+			       enum address_filtering address_filtering)
+{
+	static const u8 af_map[] = {
+		[filtering_off] = PACKETCONFIG1_ADDRESSFILTERING_OFF,
+		[node_address] = PACKETCONFIG1_ADDRESSFILTERING_NODE,
+		[node_or_broadcast_address] =
+			PACKETCONFIG1_ADDRESSFILTERING_NODEBROADCAST,
+	};
+
+	if (unlikely(address_filtering >= ARRAY_SIZE(af_map))) {
+		dev_dbg(&spi->dev, "set: illegal address filtering %u\n", address_filtering);
+		return -EINVAL;
+	}
+
+	return rf69_read_mod_write(spi, REG_PACKETCONFIG1,
+				   MASK_PACKETCONFIG1_ADDRESSFILTERING,
+				   af_map[address_filtering]);
+}
+
+int rf69_set_payload_length(struct spi_device *spi, u8 payload_length)
+{
+	return rf69_write_reg(spi, REG_PAYLOAD_LENGTH, payload_length);
+}
+
+int rf69_set_node_address(struct spi_device *spi, u8 node_address)
+{
+	return rf69_write_reg(spi, REG_NODEADRS, node_address);
+}
+
+int rf69_set_broadcast_address(struct spi_device *spi, u8 broadcast_address)
+{
+	return rf69_write_reg(spi, REG_BROADCASTADRS, broadcast_address);
+}
+
+int rf69_set_tx_start_condition(struct spi_device *spi,
+				enum tx_start_condition tx_start_condition)
+{
+	switch (tx_start_condition) {
+	case fifo_level:
+		return rf69_clear_bit(spi, REG_FIFO_THRESH,
+				      MASK_FIFO_THRESH_TXSTART);
+	case fifo_not_empty:
+		return rf69_set_bit(spi, REG_FIFO_THRESH,
+				    MASK_FIFO_THRESH_TXSTART);
+	default:
+		dev_dbg(&spi->dev, "set: illegal tx start condition %u\n", tx_start_condition);
+		return -EINVAL;
+	}
+}
+
+int rf69_set_fifo_threshold(struct spi_device *spi, u8 threshold)
+{
+	int retval;
+
+	/* check input value */
+	if (threshold & ~MASK_FIFO_THRESH_VALUE) {
+		dev_dbg(&spi->dev, "set: illegal fifo threshold %u\n", threshold);
+		return -EINVAL;
+	}
+
+	/* write value */
+	retval = rf69_read_mod_write(spi, REG_FIFO_THRESH,
+				     MASK_FIFO_THRESH_VALUE,
+				     threshold);
+	if (retval)
+		return retval;
+
+	/*
+	 * access the fifo to activate new threshold
+	 * retval (mis-) used as buffer here
+	 */
+	return rf69_read_fifo(spi, (u8 *)&retval, 1);
+}
+
+int rf69_set_dagc(struct spi_device *spi, enum dagc dagc)
+{
+	static const u8 dagc_map[] = {
+		[normal_mode] = DAGC_NORMAL,
+		[improve] = DAGC_IMPROVED_LOWBETA0,
+		[improve_for_low_modulation_index] = DAGC_IMPROVED_LOWBETA1,
+	};
+
+	if (unlikely(dagc >= ARRAY_SIZE(dagc_map))) {
+		dev_dbg(&spi->dev, "set: illegal dagc %u\n", dagc);
+		return -EINVAL;
+	}
+
+	return rf69_write_reg(spi, REG_TESTDAGC, dagc_map[dagc]);
+}
+
+/*-------------------------------------------------------------------------*/
+
+int rf69_read_fifo(struct spi_device *spi, u8 *buffer, unsigned int size)
+{
+	int i;
+	struct spi_transfer transfer;
+	u8 local_buffer[FIFO_SIZE + 1] = {};
+	int retval;
+
+	if (size > FIFO_SIZE) {
+		dev_dbg(&spi->dev,
+			"read fifo: passed in buffer bigger then internal buffer\n");
+		return -EMSGSIZE;
+	}
+
+	/* prepare a bidirectional transfer */
+	local_buffer[0] = REG_FIFO;
+	memset(&transfer, 0, sizeof(transfer));
+	transfer.tx_buf = local_buffer;
+	transfer.rx_buf = local_buffer;
+	transfer.len	= size + 1;
+
+	retval = spi_sync_transfer(spi, &transfer, 1);
+
+	/* print content read from fifo for debugging purposes */
+	for (i = 0; i < size; i++)
+		dev_dbg(&spi->dev, "%d - 0x%x\n", i, local_buffer[i + 1]);
+
+	memcpy(buffer, &local_buffer[1], size);
+
+	return retval;
+}
+
+int rf69_write_fifo(struct spi_device *spi, u8 *buffer, unsigned int size)
+{
+	int i;
+	u8 local_buffer[FIFO_SIZE + 1];
+
+	if (size > FIFO_SIZE) {
+		dev_dbg(&spi->dev,
+			"write fifo: passed in buffer bigger then internal buffer\n");
+		return -EMSGSIZE;
+	}
+
+	local_buffer[0] = REG_FIFO | WRITE_BIT;
+	memcpy(&local_buffer[1], buffer, size);
+
+	/* print content written from fifo for debugging purposes */
+	for (i = 0; i < size; i++)
+		dev_dbg(&spi->dev, "%d - 0x%x\n", i, buffer[i]);
+
+	return spi_write(spi, local_buffer, size + 1);
+}
+
diff --git a/drivers/staging/pi433/rf69.h b/drivers/staging/pi433/rf69.h
new file mode 100644
index 000000000..78fa0b8ba
--- /dev/null
+++ b/drivers/staging/pi433/rf69.h
@@ -0,0 +1,70 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * hardware abstraction/register access for HopeRf rf69 radio module
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+#ifndef RF69_H
+#define RF69_H
+
+#include "rf69_enum.h"
+#include "rf69_registers.h"
+
+/* NOTE: Modifying FREQUENCY value impacts CE certification */
+#define F_OSC		32000000	/* Hz */
+#define FREQUENCY	433920000	/* Hz */
+#define FIFO_SIZE	66		/* bytes */
+#define FIFO_THRESHOLD	15		/* bytes */
+
+u8 rf69_read_reg(struct spi_device *spi, u8 addr);
+int rf69_get_version(struct spi_device *spi);
+int rf69_set_mode(struct spi_device *spi, enum mode mode);
+int rf69_set_data_mode(struct spi_device *spi, u8 data_mode);
+int rf69_set_modulation(struct spi_device *spi, enum modulation modulation);
+int rf69_set_modulation_shaping(struct spi_device *spi,
+				enum mod_shaping mod_shaping);
+int rf69_set_bit_rate(struct spi_device *spi, u16 bit_rate);
+int rf69_set_deviation(struct spi_device *spi, u32 deviation);
+int rf69_set_frequency(struct spi_device *spi, u32 frequency);
+int rf69_enable_amplifier(struct spi_device *spi, u8 amplifier_mask);
+int rf69_disable_amplifier(struct spi_device *spi, u8 amplifier_mask);
+int rf69_set_output_power_level(struct spi_device *spi, u8 power_level);
+int rf69_set_pa_ramp(struct spi_device *spi, enum pa_ramp pa_ramp);
+int rf69_set_antenna_impedance(struct spi_device *spi,
+			       enum antenna_impedance antenna_impedance);
+int rf69_set_lna_gain(struct spi_device *spi, enum lna_gain lna_gain);
+int rf69_set_bandwidth(struct spi_device *spi, enum mantisse mantisse,
+		       u8 exponent);
+int rf69_set_bandwidth_during_afc(struct spi_device *spi,
+				  enum mantisse mantisse,
+				  u8 exponent);
+int rf69_set_ook_threshold_dec(struct spi_device *spi,
+			       enum threshold_decrement threshold_decrement);
+int rf69_set_dio_mapping(struct spi_device *spi, u8 dio_number, u8 value);
+int rf69_set_rssi_threshold(struct spi_device *spi, u8 threshold);
+int rf69_set_preamble_length(struct spi_device *spi, u16 preamble_length);
+int rf69_enable_sync(struct spi_device *spi);
+int rf69_disable_sync(struct spi_device *spi);
+int rf69_set_fifo_fill_condition(struct spi_device *spi,
+				 enum fifo_fill_condition fifo_fill_condition);
+int rf69_set_sync_size(struct spi_device *spi, u8 sync_size);
+int rf69_set_sync_values(struct spi_device *spi, u8 sync_values[8]);
+int rf69_set_packet_format(struct spi_device *spi,
+			   enum packet_format packet_format);
+int rf69_enable_crc(struct spi_device *spi);
+int rf69_disable_crc(struct spi_device *spi);
+int rf69_set_address_filtering(struct spi_device *spi,
+			       enum address_filtering address_filtering);
+int rf69_set_payload_length(struct spi_device *spi, u8 payload_length);
+int rf69_set_node_address(struct spi_device *spi, u8 node_address);
+int rf69_set_broadcast_address(struct spi_device *spi, u8 broadcast_address);
+int rf69_set_tx_start_condition(struct spi_device *spi,
+				enum tx_start_condition tx_start_condition);
+int rf69_set_fifo_threshold(struct spi_device *spi, u8 threshold);
+int rf69_set_dagc(struct spi_device *spi, enum dagc dagc);
+
+int rf69_read_fifo(struct spi_device *spi, u8 *buffer, unsigned int size);
+int rf69_write_fifo(struct spi_device *spi, u8 *buffer, unsigned int size);
+
+#endif
diff --git a/drivers/staging/pi433/rf69_enum.h b/drivers/staging/pi433/rf69_enum.h
new file mode 100644
index 000000000..9dc906124
--- /dev/null
+++ b/drivers/staging/pi433/rf69_enum.h
@@ -0,0 +1,126 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * enumerations for HopeRf rf69 radio module
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+
+#ifndef RF69_ENUM_H
+#define RF69_ENUM_H
+
+enum mode {
+	mode_sleep,
+	standby,
+	synthesizer,
+	transmit,
+	receive
+};
+
+enum modulation {
+	OOK,
+	FSK,
+	UNDEF
+};
+
+enum mod_shaping {
+	SHAPING_OFF,
+	SHAPING_1_0,
+	SHAPING_0_5,
+	SHAPING_0_3,
+	SHAPING_BR,
+	SHAPING_2BR
+};
+
+enum pa_ramp {
+	ramp3400,
+	ramp2000,
+	ramp1000,
+	ramp500,
+	ramp250,
+	ramp125,
+	ramp100,
+	ramp62,
+	ramp50,
+	ramp40,
+	ramp31,
+	ramp25,
+	ramp20,
+	ramp15,
+	ramp12,
+	ramp10
+};
+
+enum antenna_impedance {
+	fifty_ohm,
+	two_hundred_ohm
+};
+
+enum lna_gain {
+	automatic,
+	max,
+	max_minus_6,
+	max_minus_12,
+	max_minus_24,
+	max_minus_36,
+	max_minus_48,
+	undefined
+};
+
+enum mantisse {
+	mantisse16,
+	mantisse20,
+	mantisse24
+};
+
+enum threshold_decrement {
+	dec_every8th,
+	dec_every4th,
+	dec_every2nd,
+	dec_once,
+	dec_twice,
+	dec_4times,
+	dec_8times,
+	dec_16times
+};
+
+enum fifo_fill_condition {
+	after_sync_interrupt,
+	always
+};
+
+enum packet_format {
+	/*
+	 * Used when the size of payload is fixed in advance. This mode of
+	 * operation may be of interest to minimize RF overhead by 1 byte as
+	 * no length byte field is required
+	 */
+	packet_length_fix,
+	/*
+	 * Used when the size of payload isn't known in advance. It requires the
+	 * transmitter to send the length byte in each packet so the receiver
+	 * would know how to operate properly
+	 */
+	packet_length_var
+};
+
+enum tx_start_condition {
+	/* the number of bytes in the FIFO exceeds FIFO_THRESHOLD */
+	fifo_level,
+	/* at least one byte in the FIFO */
+	fifo_not_empty
+};
+
+enum address_filtering {
+	filtering_off,
+	node_address,
+	node_or_broadcast_address
+};
+
+enum dagc {
+	normal_mode,
+	improve,
+	improve_for_low_modulation_index
+};
+
+#endif
diff --git a/drivers/staging/pi433/rf69_registers.h b/drivers/staging/pi433/rf69_registers.h
new file mode 100644
index 000000000..0d6737738
--- /dev/null
+++ b/drivers/staging/pi433/rf69_registers.h
@@ -0,0 +1,478 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * register description for HopeRf rf69 radio module
+ *
+ * Copyright (C) 2016 Wolf-Entwicklungen
+ *	Marcus Wolf <linux@wolf-entwicklungen.de>
+ */
+
+/*******************************************/
+/* RF69 register addresses		   */
+/*******************************************/
+#define  REG_FIFO			0x00
+#define  REG_OPMODE			0x01
+#define  REG_DATAMODUL			0x02
+#define  REG_BITRATE_MSB		0x03
+#define  REG_BITRATE_LSB		0x04
+#define  REG_FDEV_MSB			0x05
+#define  REG_FDEV_LSB			0x06
+#define  REG_FRF_MSB			0x07
+#define  REG_FRF_MID			0x08
+#define  REG_FRF_LSB			0x09
+#define  REG_OSC1			0x0A
+#define  REG_AFCCTRL			0x0B
+#define  REG_LOWBAT			0x0C
+#define  REG_LISTEN1			0x0D
+#define  REG_LISTEN2			0x0E
+#define  REG_LISTEN3			0x0F
+#define  REG_VERSION			0x10
+#define  REG_PALEVEL			0x11
+#define  REG_PARAMP			0x12
+#define  REG_OCP			0x13
+#define  REG_AGCREF			0x14 /* not available on RF69 */
+#define  REG_AGCTHRESH1			0x15 /* not available on RF69 */
+#define  REG_AGCTHRESH2			0x16 /* not available on RF69 */
+#define  REG_AGCTHRESH3			0x17 /* not available on RF69 */
+#define  REG_LNA			0x18
+#define  REG_RXBW			0x19
+#define  REG_AFCBW			0x1A
+#define  REG_OOKPEAK			0x1B
+#define  REG_OOKAVG			0x1C
+#define  REG_OOKFIX			0x1D
+#define  REG_AFCFEI			0x1E
+#define  REG_AFCMSB			0x1F
+#define  REG_AFCLSB			0x20
+#define  REG_FEIMSB			0x21
+#define  REG_FEILSB			0x22
+#define  REG_RSSICONFIG			0x23
+#define  REG_RSSIVALUE			0x24
+#define  REG_DIOMAPPING1		0x25
+#define  REG_DIOMAPPING2		0x26
+#define  REG_IRQFLAGS1			0x27
+#define  REG_IRQFLAGS2			0x28
+#define  REG_RSSITHRESH			0x29
+#define  REG_RXTIMEOUT1			0x2A
+#define  REG_RXTIMEOUT2			0x2B
+#define  REG_PREAMBLE_MSB		0x2C
+#define  REG_PREAMBLE_LSB		0x2D
+#define  REG_SYNC_CONFIG		0x2E
+#define  REG_SYNCVALUE1			0x2F
+#define  REG_SYNCVALUE2			0x30
+#define  REG_SYNCVALUE3			0x31
+#define  REG_SYNCVALUE4			0x32
+#define  REG_SYNCVALUE5			0x33
+#define  REG_SYNCVALUE6			0x34
+#define  REG_SYNCVALUE7			0x35
+#define  REG_SYNCVALUE8			0x36
+#define  REG_PACKETCONFIG1		0x37
+#define  REG_PAYLOAD_LENGTH		0x38
+#define  REG_NODEADRS			0x39
+#define  REG_BROADCASTADRS		0x3A
+#define  REG_AUTOMODES			0x3B
+#define  REG_FIFO_THRESH		0x3C
+#define  REG_PACKETCONFIG2		0x3D
+#define  REG_AESKEY1			0x3E
+#define  REG_AESKEY2			0x3F
+#define  REG_AESKEY3			0x40
+#define  REG_AESKEY4			0x41
+#define  REG_AESKEY5			0x42
+#define  REG_AESKEY6			0x43
+#define  REG_AESKEY7			0x44
+#define  REG_AESKEY8			0x45
+#define  REG_AESKEY9			0x46
+#define  REG_AESKEY10			0x47
+#define  REG_AESKEY11			0x48
+#define  REG_AESKEY12			0x49
+#define  REG_AESKEY13			0x4A
+#define  REG_AESKEY14			0x4B
+#define  REG_AESKEY15			0x4C
+#define  REG_AESKEY16			0x4D
+#define  REG_TEMP1			0x4E
+#define  REG_TEMP2			0x4F
+#define  REG_TESTLNA			0x58
+#define  REG_TESTPA1			0x5A /* only present on RFM69HW */
+#define  REG_TESTPA2			0x5C /* only present on RFM69HW */
+#define  REG_TESTDAGC			0x6F
+#define  REG_TESTAFC			0x71
+
+/******************************************************/
+/* RF69/SX1231 bit definition				*/
+/******************************************************/
+/* write bit */
+#define WRITE_BIT				0x80
+
+/* RegOpMode */
+#define  MASK_OPMODE_SEQUENCER_OFF		0x80
+#define  MASK_OPMODE_LISTEN_ON			0x40
+#define  MASK_OPMODE_LISTEN_ABORT		0x20
+#define  MASK_OPMODE_MODE			0x1C
+
+#define  OPMODE_MODE_SLEEP			0x00
+#define  OPMODE_MODE_STANDBY			0x04 /* default */
+#define  OPMODE_MODE_SYNTHESIZER		0x08
+#define  OPMODE_MODE_TRANSMIT			0x0C
+#define  OPMODE_MODE_RECEIVE			0x10
+
+/* RegDataModul */
+#define  MASK_DATAMODUL_MODE			0x06
+#define  MASK_DATAMODUL_MODULATION_TYPE		0x18
+#define  MASK_DATAMODUL_MODULATION_SHAPE	0x03
+
+#define  DATAMODUL_MODE_PACKET			0x00 /* default */
+#define  DATAMODUL_MODE_CONTINUOUS		0x40
+#define  DATAMODUL_MODE_CONTINUOUS_NOSYNC	0x60
+
+#define  DATAMODUL_MODULATION_TYPE_FSK		0x00 /* default */
+#define  DATAMODUL_MODULATION_TYPE_OOK		0x08
+
+#define  DATAMODUL_MODULATION_SHAPE_NONE	0x00 /* default */
+#define  DATAMODUL_MODULATION_SHAPE_1_0		0x01
+#define  DATAMODUL_MODULATION_SHAPE_0_5		0x02
+#define  DATAMODUL_MODULATION_SHAPE_0_3		0x03
+#define  DATAMODUL_MODULATION_SHAPE_BR		0x01
+#define  DATAMODUL_MODULATION_SHAPE_2BR		0x02
+
+/* RegFDevMsb (0x05)*/
+#define FDEVMASB_MASK				0x3f
+
+/*
+ * // RegOsc1
+ * #define  OSC1_RCCAL_START			0x80
+ * #define  OSC1_RCCAL_DONE			0x40
+ *
+ * // RegLowBat
+ * #define  LOWBAT_MONITOR				0x10
+ * #define  LOWBAT_ON				0x08
+ * #define  LOWBAT_OFF				0x00  // Default
+ *
+ * #define  LOWBAT_TRIM_1695			0x00
+ * #define  LOWBAT_TRIM_1764			0x01
+ * #define  LOWBAT_TRIM_1835			0x02  // Default
+ * #define  LOWBAT_TRIM_1905			0x03
+ * #define  LOWBAT_TRIM_1976			0x04
+ * #define  LOWBAT_TRIM_2045			0x05
+ * #define  LOWBAT_TRIM_2116			0x06
+ * #define  LOWBAT_TRIM_2185			0x07
+ *
+ *
+ * // RegListen1
+ * #define  LISTEN1_RESOL_64			0x50
+ * #define  LISTEN1_RESOL_4100			0xA0  // Default
+ * #define  LISTEN1_RESOL_262000			0xF0
+ *
+ * #define  LISTEN1_CRITERIA_RSSI			0x00  // Default
+ * #define  LISTEN1_CRITERIA_RSSIANDSYNC		0x08
+ *
+ * #define  LISTEN1_END_00				0x00
+ * #define  LISTEN1_END_01				0x02  // Default
+ * #define  LISTEN1_END_10				0x04
+ *
+ *
+ * // RegListen2
+ * #define  LISTEN2_COEFIDLE_VALUE			0xF5 // Default
+ *
+ * // RegListen3
+ * #define  LISTEN3_COEFRX_VALUE			0x20 // Default
+ */
+
+// RegPaLevel
+#define  MASK_PALEVEL_PA0			0x80
+#define  MASK_PALEVEL_PA1			0x40
+#define  MASK_PALEVEL_PA2			0x20
+#define  MASK_PALEVEL_OUTPUT_POWER		0x1F
+
+// RegPaRamp
+#define  PARAMP_3400				0x00
+#define  PARAMP_2000				0x01
+#define  PARAMP_1000				0x02
+#define  PARAMP_500				0x03
+#define  PARAMP_250				0x04
+#define  PARAMP_125				0x05
+#define  PARAMP_100				0x06
+#define  PARAMP_62				0x07
+#define  PARAMP_50				0x08
+#define  PARAMP_40				0x09 /* default */
+#define  PARAMP_31				0x0A
+#define  PARAMP_25				0x0B
+#define  PARAMP_20				0x0C
+#define  PARAMP_15				0x0D
+#define  PARAMP_12				0x0E
+#define  PARAMP_10				0x0F
+
+#define  MASK_PARAMP				0x0F
+
+/*
+ * // RegOcp
+ * #define  OCP_OFF				0x0F
+ * #define  OCP_ON					0x1A  // Default
+ *
+ * #define  OCP_TRIM_45				0x00
+ * #define  OCP_TRIM_50				0x01
+ * #define  OCP_TRIM_55				0x02
+ * #define  OCP_TRIM_60				0x03
+ * #define  OCP_TRIM_65				0x04
+ * #define  OCP_TRIM_70				0x05
+ * #define  OCP_TRIM_75				0x06
+ * #define  OCP_TRIM_80				0x07
+ * #define  OCP_TRIM_85				0x08
+ * #define  OCP_TRIM_90				0x09
+ * #define  OCP_TRIM_95				0x0A
+ * #define  OCP_TRIM_100				0x0B  // Default
+ * #define  OCP_TRIM_105				0x0C
+ * #define  OCP_TRIM_110				0x0D
+ * #define  OCP_TRIM_115				0x0E
+ * #define  OCP_TRIM_120				0x0F
+ */
+
+/* RegLna (0x18) */
+#define  MASK_LNA_ZIN				0x80
+#define  MASK_LNA_CURRENT_GAIN			0x38
+#define  MASK_LNA_GAIN				0x07
+
+#define  LNA_GAIN_AUTO				0x00 /* default */
+#define  LNA_GAIN_MAX				0x01
+#define  LNA_GAIN_MAX_MINUS_6			0x02
+#define  LNA_GAIN_MAX_MINUS_12			0x03
+#define  LNA_GAIN_MAX_MINUS_24			0x04
+#define  LNA_GAIN_MAX_MINUS_36			0x05
+#define  LNA_GAIN_MAX_MINUS_48			0x06
+
+/* RegRxBw (0x19) and RegAfcBw (0x1A) */
+#define  MASK_BW_DCC_FREQ			0xE0
+#define  MASK_BW_MANTISSE			0x18
+#define  MASK_BW_EXPONENT			0x07
+
+#define  BW_DCC_16_PERCENT			0x00
+#define  BW_DCC_8_PERCENT			0x20
+#define  BW_DCC_4_PERCENT			0x40 /* default */
+#define  BW_DCC_2_PERCENT			0x60
+#define  BW_DCC_1_PERCENT			0x80
+#define  BW_DCC_0_5_PERCENT			0xA0
+#define  BW_DCC_0_25_PERCENT			0xC0
+#define  BW_DCC_0_125_PERCENT			0xE0
+
+#define  BW_MANT_16				0x00
+#define  BW_MANT_20				0x08
+#define  BW_MANT_24				0x10 /* default */
+
+/* RegOokPeak (0x1B) */
+#define  MASK_OOKPEAK_THRESTYPE			0xc0
+#define  MASK_OOKPEAK_THRESSTEP			0x38
+#define  MASK_OOKPEAK_THRESDEC			0x07
+
+#define  OOKPEAK_THRESHTYPE_FIXED		0x00
+#define  OOKPEAK_THRESHTYPE_PEAK		0x40 /* default */
+#define  OOKPEAK_THRESHTYPE_AVERAGE		0x80
+
+#define  OOKPEAK_THRESHSTEP_0_5_DB		0x00 /* default */
+#define  OOKPEAK_THRESHSTEP_1_0_DB		0x08
+#define  OOKPEAK_THRESHSTEP_1_5_DB		0x10
+#define  OOKPEAK_THRESHSTEP_2_0_DB		0x18
+#define  OOKPEAK_THRESHSTEP_3_0_DB		0x20
+#define  OOKPEAK_THRESHSTEP_4_0_DB		0x28
+#define  OOKPEAK_THRESHSTEP_5_0_DB		0x30
+#define  OOKPEAK_THRESHSTEP_6_0_DB		0x38
+
+#define  OOKPEAK_THRESHDEC_ONCE			0x00 /* default */
+#define  OOKPEAK_THRESHDEC_EVERY_2ND		0x01
+#define  OOKPEAK_THRESHDEC_EVERY_4TH		0x02
+#define  OOKPEAK_THRESHDEC_EVERY_8TH		0x03
+#define  OOKPEAK_THRESHDEC_TWICE		0x04
+#define  OOKPEAK_THRESHDEC_4_TIMES		0x05
+#define  OOKPEAK_THRESHDEC_8_TIMES		0x06
+#define  OOKPEAK_THRESHDEC_16_TIMES		0x07
+
+/*
+ * // RegOokAvg
+ * #define  OOKAVG_AVERAGETHRESHFILT_00		0x00
+ * #define  OOKAVG_AVERAGETHRESHFILT_01		0x40
+ * #define  OOKAVG_AVERAGETHRESHFILT_10		0x80  // Default
+ * #define  OOKAVG_AVERAGETHRESHFILT_11		0xC0
+ *
+ *
+ * // RegAfcFei
+ * #define  AFCFEI_FEI_DONE			0x40
+ * #define  AFCFEI_FEI_START			0x20
+ * #define  AFCFEI_AFC_DONE			0x10
+ * #define  AFCFEI_AFCAUTOCLEAR_ON			0x08
+ * #define  AFCFEI_AFCAUTOCLEAR_OFF		0x00  // Default
+ *
+ * #define  AFCFEI_AFCAUTO_ON			0x04
+ * #define  AFCFEI_AFCAUTO_OFF			0x00  // Default
+ *
+ * #define  AFCFEI_AFC_CLEAR			0x02
+ * #define  AFCFEI_AFC_START			0x01
+ *
+ * // RegRssiConfig
+ * #define  RSSI_FASTRX_ON				0x08
+ * #define  RSSI_FASTRX_OFF			0x00  // Default
+ * #define  RSSI_DONE				0x02
+ * #define  RSSI_START				0x01
+ */
+
+/* RegDioMapping1 */
+#define  MASK_DIO0				0xC0
+#define  MASK_DIO1				0x30
+#define  MASK_DIO2				0x0C
+#define  MASK_DIO3				0x03
+#define  SHIFT_DIO0				6
+#define  SHIFT_DIO1				4
+#define  SHIFT_DIO2				2
+#define  SHIFT_DIO3				0
+
+/* RegDioMapping2 */
+#define  MASK_DIO4				0xC0
+#define  MASK_DIO5				0x30
+#define  SHIFT_DIO4				6
+#define  SHIFT_DIO5				4
+
+/* DIO numbers */
+#define  DIO0					0
+#define  DIO1					1
+#define  DIO2					2
+#define  DIO3					3
+#define  DIO4					4
+#define  DIO5					5
+
+/* DIO Mapping values (packet mode) */
+#define  DIO_MODE_READY_DIO4			0x00
+#define  DIO_MODE_READY_DIO5			0x03
+#define  DIO_CLK_OUT				0x00
+#define  DIO_DATA				0x01
+#define  DIO_TIMEOUT_DIO1			0x03
+#define  DIO_TIMEOUT_DIO4			0x00
+#define  DIO_RSSI_DIO0				0x03
+#define  DIO_RSSI_DIO3_4			0x01
+#define  DIO_RX_READY				0x02
+#define  DIO_PLL_LOCK				0x03
+#define  DIO_TX_READY				0x01
+#define  DIO_FIFO_FULL_DIO1			0x01
+#define  DIO_FIFO_FULL_DIO3			0x00
+#define  DIO_SYNC_ADDRESS			0x02
+#define  DIO_FIFO_NOT_EMPTY_DIO1		0x02
+#define  DIO_FIFO_NOT_EMPTY_FIO2		0x00
+#define  DIO_AUTOMODE				0x04
+#define  DIO_FIFO_LEVEL				0x00
+#define  DIO_CRC_OK				0x00
+#define  DIO_PAYLOAD_READY			0x01
+#define  DIO_PACKET_SENT			0x00
+#define  DIO_DCLK				0x00
+
+/* RegDioMapping2 CLK_OUT part */
+#define  MASK_DIOMAPPING2_CLK_OUT		0x07
+
+#define  DIOMAPPING2_CLK_OUT_NO_DIV		0x00
+#define  DIOMAPPING2_CLK_OUT_DIV_2		0x01
+#define  DIOMAPPING2_CLK_OUT_DIV_4		0x02
+#define  DIOMAPPING2_CLK_OUT_DIV_8		0x03
+#define  DIOMAPPING2_CLK_OUT_DIV_16		0x04
+#define  DIOMAPPING2_CLK_OUT_DIV_32		0x05
+#define  DIOMAPPING2_CLK_OUT_RC			0x06
+#define  DIOMAPPING2_CLK_OUT_OFF		0x07 /* default */
+
+/* RegIrqFlags1 */
+#define  MASK_IRQFLAGS1_MODE_READY		0x80
+#define  MASK_IRQFLAGS1_RX_READY		0x40
+#define  MASK_IRQFLAGS1_TX_READY		0x20
+#define  MASK_IRQFLAGS1_PLL_LOCK		0x10
+#define  MASK_IRQFLAGS1_RSSI			0x08
+#define  MASK_IRQFLAGS1_TIMEOUT			0x04
+#define  MASK_IRQFLAGS1_AUTOMODE		0x02
+#define  MASK_IRQFLAGS1_SYNC_ADDRESS_MATCH	0x01
+
+/* RegIrqFlags2 */
+#define  MASK_IRQFLAGS2_FIFO_FULL		0x80
+#define  MASK_IRQFLAGS2_FIFO_NOT_EMPTY		0x40
+#define  MASK_IRQFLAGS2_FIFO_LEVEL		0x20
+#define  MASK_IRQFLAGS2_FIFO_OVERRUN		0x10
+#define  MASK_IRQFLAGS2_PACKET_SENT		0x08
+#define  MASK_IRQFLAGS2_PAYLOAD_READY		0x04
+#define  MASK_IRQFLAGS2_CRC_OK			0x02
+#define  MASK_IRQFLAGS2_LOW_BAT			0x01
+
+/* RegSyncConfig */
+#define  MASK_SYNC_CONFIG_SYNC_ON		0x80 /* default */
+#define  MASK_SYNC_CONFIG_FIFO_FILL_CONDITION	0x40
+#define  MASK_SYNC_CONFIG_SYNC_SIZE		0x38
+#define  MASK_SYNC_CONFIG_SYNC_TOLERANCE	0x07
+
+/* RegPacketConfig1 */
+#define  MASK_PACKETCONFIG1_PACKET_FORMAT_VARIABLE	0x80
+#define  MASK_PACKETCONFIG1_DCFREE			0x60
+#define  MASK_PACKETCONFIG1_CRC_ON			0x10 /* default */
+#define  MASK_PACKETCONFIG1_CRCAUTOCLEAR_OFF		0x08
+#define  MASK_PACKETCONFIG1_ADDRESSFILTERING		0x06
+
+#define  PACKETCONFIG1_DCFREE_OFF			0x00 /* default */
+#define  PACKETCONFIG1_DCFREE_MANCHESTER		0x20
+#define  PACKETCONFIG1_DCFREE_WHITENING			0x40
+#define  PACKETCONFIG1_ADDRESSFILTERING_OFF		0x00 /* default */
+#define  PACKETCONFIG1_ADDRESSFILTERING_NODE		0x02
+#define  PACKETCONFIG1_ADDRESSFILTERING_NODEBROADCAST	0x04
+
+/*
+ * // RegAutoModes
+ * #define  AUTOMODES_ENTER_OFF			0x00  // Default
+ * #define  AUTOMODES_ENTER_FIFONOTEMPTY		0x20
+ * #define  AUTOMODES_ENTER_FIFOLEVEL		0x40
+ * #define  AUTOMODES_ENTER_CRCOK			0x60
+ * #define  AUTOMODES_ENTER_PAYLOADREADY		0x80
+ * #define  AUTOMODES_ENTER_SYNCADRSMATCH		0xA0
+ * #define  AUTOMODES_ENTER_PACKETSENT		0xC0
+ * #define  AUTOMODES_ENTER_FIFOEMPTY		0xE0
+ *
+ * #define  AUTOMODES_EXIT_OFF			0x00  // Default
+ * #define  AUTOMODES_EXIT_FIFOEMPTY		0x04
+ * #define  AUTOMODES_EXIT_FIFOLEVEL		0x08
+ * #define  AUTOMODES_EXIT_CRCOK			0x0C
+ * #define  AUTOMODES_EXIT_PAYLOADREADY		0x10
+ * #define  AUTOMODES_EXIT_SYNCADRSMATCH		0x14
+ * #define  AUTOMODES_EXIT_PACKETSENT		0x18
+ * #define  AUTOMODES_EXIT_RXTIMEOUT		0x1C
+ *
+ * #define  AUTOMODES_INTERMEDIATE_SLEEP		0x00  // Default
+ * #define  AUTOMODES_INTERMEDIATE_STANDBY		0x01
+ * #define  AUTOMODES_INTERMEDIATE_RECEIVER	0x02
+ * #define  AUTOMODES_INTERMEDIATE_TRANSMITTER	0x03
+ *
+ */
+/* RegFifoThresh (0x3c) */
+#define  MASK_FIFO_THRESH_TXSTART		0x80
+#define  MASK_FIFO_THRESH_VALUE			0x7F
+
+/*
+ *
+ * // RegPacketConfig2
+ * #define  PACKET2_RXRESTARTDELAY_1BIT		0x00  // Default
+ * #define  PACKET2_RXRESTARTDELAY_2BITS		0x10
+ * #define  PACKET2_RXRESTARTDELAY_4BITS		0x20
+ * #define  PACKET2_RXRESTARTDELAY_8BITS		0x30
+ * #define  PACKET2_RXRESTARTDELAY_16BITS		0x40
+ * #define  PACKET2_RXRESTARTDELAY_32BITS		0x50
+ * #define  PACKET2_RXRESTARTDELAY_64BITS		0x60
+ * #define  PACKET2_RXRESTARTDELAY_128BITS		0x70
+ * #define  PACKET2_RXRESTARTDELAY_256BITS		0x80
+ * #define  PACKET2_RXRESTARTDELAY_512BITS		0x90
+ * #define  PACKET2_RXRESTARTDELAY_1024BITS	0xA0
+ * #define  PACKET2_RXRESTARTDELAY_2048BITS	0xB0
+ * #define  PACKET2_RXRESTARTDELAY_NONE		0xC0
+ * #define  PACKET2_RXRESTART			0x04
+ *
+ * #define  PACKET2_AUTORXRESTART_ON		0x02  // Default
+ * #define  PACKET2_AUTORXRESTART_OFF		0x00
+ *
+ * #define  PACKET2_AES_ON				0x01
+ * #define  PACKET2_AES_OFF			0x00  // Default
+ *
+ *
+ * // RegTemp1
+ * #define  TEMP1_MEAS_START			0x08
+ * #define  TEMP1_MEAS_RUNNING			0x04
+ * #define  TEMP1_ADCLOWPOWER_ON			0x01  // Default
+ * #define  TEMP1_ADCLOWPOWER_OFF			0x00
+ */
+
+// RegTestDagc (0x6F)
+#define  DAGC_NORMAL				0x00 /* Reset value */
+#define  DAGC_IMPROVED_LOWBETA1			0x20
+#define  DAGC_IMPROVED_LOWBETA0			0x30 /* Recommended val */
diff --git a/drivers/staging/qlge/Kconfig b/drivers/staging/qlge/Kconfig
new file mode 100644
index 000000000..6d831ed67
--- /dev/null
+++ b/drivers/staging/qlge/Kconfig
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config QLGE
+	tristate "QLogic QLGE 10Gb Ethernet Driver Support"
+	depends on ETHERNET && PCI
+	select NET_DEVLINK
+	help
+	This driver supports QLogic ISP8XXX 10Gb Ethernet cards.
+
+	To compile this driver as a module, choose M here. The module will be
+	called qlge.
diff --git a/drivers/staging/qlge/Makefile b/drivers/staging/qlge/Makefile
new file mode 100644
index 000000000..07c1898a5
--- /dev/null
+++ b/drivers/staging/qlge/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# Makefile for the Qlogic 10GbE PCI Express ethernet driver
+#
+
+obj-$(CONFIG_QLGE) += qlge.o
+
+qlge-objs := qlge_main.o qlge_dbg.o qlge_mpi.o qlge_ethtool.o qlge_devlink.o
diff --git a/drivers/staging/qlge/TODO b/drivers/staging/qlge/TODO
new file mode 100644
index 000000000..c76394b94
--- /dev/null
+++ b/drivers/staging/qlge/TODO
@@ -0,0 +1,33 @@
+* commit 7c734359d350 ("qlge: Size RX buffers based on MTU.", v2.6.33-rc1)
+  introduced dead code in the receive routines, which should be rewritten
+  anyways by the admission of the author himself, see the comment above
+  ql_build_rx_skb(). That function is now used exclusively to handle packets
+  that underwent header splitting but it still contains code to handle non
+  split cases.
+* truesize accounting is incorrect (ex: a 9000B frame has skb->truesize 10280
+  while containing two frags of order-1 allocations, ie. >16K)
+* while in that area, using two 8k buffers to store one 9k frame is a poor
+  choice of buffer size.
+* in the "chain of large buffers" case, the driver uses an skb allocated with
+  head room but only puts data in the frags.
+* rename "rx" queues to "completion" queues. Calling tx completion queues "rx
+  queues" is confusing.
+* struct rx_ring is used for rx and tx completions, with some members relevant
+  to one case only
+* the flow control implementation in firmware is buggy (sends a flood of pause
+  frames, resets the link, device and driver buffer queues become
+  desynchronized), disable it by default
+* some structures are initialized redundantly (ex. memset 0 after
+  alloc_etherdev())
+* the driver has a habit of using runtime checks where compile time checks are
+  possible (ex. ql_free_rx_buffers(), ql_alloc_rx_buffers())
+* reorder struct members to avoid holes if it doesn't impact performance
+* avoid legacy/deprecated apis (ex. replace pci_dma_*, replace pci_enable_msi,
+  use pci_iomap)
+* some "while" loops could be rewritten with simple "for", ex.
+  ql_wait_reg_rdy(), ql_start_rx_ring())
+* remove duplicate and useless comments
+* fix weird line wrapping (all over, ex. the ql_set_routing_reg() calls in
+  qlge_set_multicast_list()).
+* fix weird indentation (all over, ex. the for loops in qlge_get_stats())
+* fix checkpatch issues
diff --git a/drivers/staging/qlge/qlge.h b/drivers/staging/qlge/qlge.h
new file mode 100644
index 000000000..d0dd65983
--- /dev/null
+++ b/drivers/staging/qlge/qlge.h
@@ -0,0 +1,2293 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * QLogic QLA41xx NIC HBA Driver
+ * Copyright (c)  2003-2006 QLogic Corporation
+ */
+#ifndef _QLGE_H_
+#define _QLGE_H_
+
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/rtnetlink.h>
+#include <linux/if_vlan.h>
+
+/*
+ * General definitions...
+ */
+#define DRV_NAME	"qlge"
+#define DRV_STRING	"QLogic 10 Gigabit PCI-E Ethernet Driver "
+#define DRV_VERSION	"1.00.00.35"
+
+#define WQ_ADDR_ALIGN	0x3	/* 4 byte alignment */
+
+#define QLGE_VENDOR_ID    0x1077
+#define QLGE_DEVICE_ID_8012	0x8012
+#define QLGE_DEVICE_ID_8000	0x8000
+#define QLGE_MEZZ_SSYS_ID_068	0x0068
+#define QLGE_MEZZ_SSYS_ID_180	0x0180
+#define MAX_CPUS 8
+#define MAX_TX_RINGS MAX_CPUS
+#define MAX_RX_RINGS ((MAX_CPUS * 2) + 1)
+
+#define NUM_TX_RING_ENTRIES	256
+#define NUM_RX_RING_ENTRIES	256
+
+/* Use the same len for sbq and lbq. Note that it seems like the device might
+ * support different sizes.
+ */
+#define QLGE_BQ_SHIFT 9
+#define QLGE_BQ_LEN BIT(QLGE_BQ_SHIFT)
+#define QLGE_BQ_SIZE (QLGE_BQ_LEN * sizeof(__le64))
+
+#define DB_PAGE_SIZE 4096
+
+/* Calculate the number of (4k) pages required to
+ * contain a buffer queue of the given length.
+ */
+#define MAX_DB_PAGES_PER_BQ(x) \
+		(((x * sizeof(u64)) / DB_PAGE_SIZE) + \
+		(((x * sizeof(u64)) % DB_PAGE_SIZE) ? 1 : 0))
+
+#define RX_RING_SHADOW_SPACE	(sizeof(u64) + \
+		MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN) * sizeof(u64) + \
+		MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN) * sizeof(u64))
+#define LARGE_BUFFER_MAX_SIZE 8192
+#define LARGE_BUFFER_MIN_SIZE 2048
+
+#define MAX_CQ 128
+#define DFLT_COALESCE_WAIT 100	/* 100 usec wait for coalescing */
+#define MAX_INTER_FRAME_WAIT 10	/* 10 usec max interframe-wait for coalescing */
+#define DFLT_INTER_FRAME_WAIT (MAX_INTER_FRAME_WAIT / 2)
+#define UDELAY_COUNT 3
+#define UDELAY_DELAY 100
+
+#define TX_DESC_PER_IOCB 8
+
+#if ((MAX_SKB_FRAGS - TX_DESC_PER_IOCB) + 2) > 0
+#define TX_DESC_PER_OAL ((MAX_SKB_FRAGS - TX_DESC_PER_IOCB) + 2)
+#else /* all other page sizes */
+#define TX_DESC_PER_OAL 0
+#endif
+
+/* Word shifting for converting 64-bit
+ * address to a series of 16-bit words.
+ * This is used for some MPI firmware
+ * mailbox commands.
+ */
+#define LSW(x)  ((u16)(x))
+#define MSW(x)  ((u16)((u32)(x) >> 16))
+#define LSD(x)  ((u32)((u64)(x)))
+#define MSD(x)  ((u32)((((u64)(x)) >> 32)))
+
+/* In some cases, the device interprets a value of 0x0000 as 65536. These
+ * cases are marked using the following macro.
+ */
+#define QLGE_FIT16(value) ((u16)(value))
+
+/* MPI test register definitions. This register
+ * is used for determining alternate NIC function's
+ * PCI->func number.
+ */
+enum {
+	MPI_TEST_FUNC_PORT_CFG = 0x1002,
+	MPI_TEST_FUNC_PRB_CTL = 0x100e,
+		MPI_TEST_FUNC_PRB_EN = 0x18a20000,
+	MPI_TEST_FUNC_RST_STS = 0x100a,
+		MPI_TEST_FUNC_RST_FRC = 0x00000003,
+	MPI_TEST_NIC_FUNC_MASK = 0x00000007,
+	MPI_TEST_NIC1_FUNCTION_ENABLE = (1 << 0),
+	MPI_TEST_NIC1_FUNCTION_MASK = 0x0000000e,
+	MPI_TEST_NIC1_FUNC_SHIFT = 1,
+	MPI_TEST_NIC2_FUNCTION_ENABLE = (1 << 4),
+	MPI_TEST_NIC2_FUNCTION_MASK = 0x000000e0,
+	MPI_TEST_NIC2_FUNC_SHIFT = 5,
+	MPI_TEST_FC1_FUNCTION_ENABLE = (1 << 8),
+	MPI_TEST_FC1_FUNCTION_MASK	= 0x00000e00,
+	MPI_TEST_FC1_FUNCTION_SHIFT = 9,
+	MPI_TEST_FC2_FUNCTION_ENABLE = (1 << 12),
+	MPI_TEST_FC2_FUNCTION_MASK = 0x0000e000,
+	MPI_TEST_FC2_FUNCTION_SHIFT = 13,
+
+	MPI_NIC_READ = 0x00000000,
+	MPI_NIC_REG_BLOCK = 0x00020000,
+	MPI_NIC_FUNCTION_SHIFT = 6,
+};
+
+/*
+ * Processor Address Register (PROC_ADDR) bit definitions.
+ */
+enum {
+	/* Misc. stuff */
+	MAILBOX_COUNT = 16,
+	MAILBOX_TIMEOUT = 5,
+
+	PROC_ADDR_RDY = (1 << 31),
+	PROC_ADDR_R = (1 << 30),
+	PROC_ADDR_ERR = (1 << 29),
+	PROC_ADDR_DA = (1 << 28),
+	PROC_ADDR_FUNC0_MBI = 0x00001180,
+	PROC_ADDR_FUNC0_MBO = (PROC_ADDR_FUNC0_MBI + MAILBOX_COUNT),
+	PROC_ADDR_FUNC0_CTL = 0x000011a1,
+	PROC_ADDR_FUNC2_MBI = 0x00001280,
+	PROC_ADDR_FUNC2_MBO = (PROC_ADDR_FUNC2_MBI + MAILBOX_COUNT),
+	PROC_ADDR_FUNC2_CTL = 0x000012a1,
+	PROC_ADDR_MPI_RISC = 0x00000000,
+	PROC_ADDR_MDE = 0x00010000,
+	PROC_ADDR_REGBLOCK = 0x00020000,
+	PROC_ADDR_RISC_REG = 0x00030000,
+};
+
+/*
+ * System Register (SYS) bit definitions.
+ */
+enum {
+	SYS_EFE = (1 << 0),
+	SYS_FAE = (1 << 1),
+	SYS_MDC = (1 << 2),
+	SYS_DST = (1 << 3),
+	SYS_DWC = (1 << 4),
+	SYS_EVW = (1 << 5),
+	SYS_OMP_DLY_MASK = 0x3f000000,
+	/*
+	 * There are no values defined as of edit #15.
+	 */
+	SYS_ODI = (1 << 14),
+};
+
+/*
+ *  Reset/Failover Register (RST_FO) bit definitions.
+ */
+enum {
+	RST_FO_TFO = (1 << 0),
+	RST_FO_RR_MASK = 0x00060000,
+	RST_FO_RR_CQ_CAM = 0x00000000,
+	RST_FO_RR_DROP = 0x00000002,
+	RST_FO_RR_DQ = 0x00000004,
+	RST_FO_RR_RCV_FUNC_CQ = 0x00000006,
+	RST_FO_FRB = (1 << 12),
+	RST_FO_MOP = (1 << 13),
+	RST_FO_REG = (1 << 14),
+	RST_FO_FR = (1 << 15),
+};
+
+/*
+ * Function Specific Control Register (FSC) bit definitions.
+ */
+enum {
+	FSC_DBRST_MASK = 0x00070000,
+	FSC_DBRST_256 = 0x00000000,
+	FSC_DBRST_512 = 0x00000001,
+	FSC_DBRST_768 = 0x00000002,
+	FSC_DBRST_1024 = 0x00000003,
+	FSC_DBL_MASK = 0x00180000,
+	FSC_DBL_DBRST = 0x00000000,
+	FSC_DBL_MAX_PLD = 0x00000008,
+	FSC_DBL_MAX_BRST = 0x00000010,
+	FSC_DBL_128_BYTES = 0x00000018,
+	FSC_EC = (1 << 5),
+	FSC_EPC_MASK = 0x00c00000,
+	FSC_EPC_INBOUND = (1 << 6),
+	FSC_EPC_OUTBOUND = (1 << 7),
+	FSC_VM_PAGESIZE_MASK = 0x07000000,
+	FSC_VM_PAGE_2K = 0x00000100,
+	FSC_VM_PAGE_4K = 0x00000200,
+	FSC_VM_PAGE_8K = 0x00000300,
+	FSC_VM_PAGE_64K = 0x00000600,
+	FSC_SH = (1 << 11),
+	FSC_DSB = (1 << 12),
+	FSC_STE = (1 << 13),
+	FSC_FE = (1 << 15),
+};
+
+/*
+ *  Host Command Status Register (CSR) bit definitions.
+ */
+enum {
+	CSR_ERR_STS_MASK = 0x0000003f,
+	/*
+	 * There are no valued defined as of edit #15.
+	 */
+	CSR_RR = (1 << 8),
+	CSR_HRI = (1 << 9),
+	CSR_RP = (1 << 10),
+	CSR_CMD_PARM_SHIFT = 22,
+	CSR_CMD_NOP = 0x00000000,
+	CSR_CMD_SET_RST = 0x10000000,
+	CSR_CMD_CLR_RST = 0x20000000,
+	CSR_CMD_SET_PAUSE = 0x30000000,
+	CSR_CMD_CLR_PAUSE = 0x40000000,
+	CSR_CMD_SET_H2R_INT = 0x50000000,
+	CSR_CMD_CLR_H2R_INT = 0x60000000,
+	CSR_CMD_PAR_EN = 0x70000000,
+	CSR_CMD_SET_BAD_PAR = 0x80000000,
+	CSR_CMD_CLR_BAD_PAR = 0x90000000,
+	CSR_CMD_CLR_R2PCI_INT = 0xa0000000,
+};
+
+/*
+ *  Configuration Register (CFG) bit definitions.
+ */
+enum {
+	CFG_LRQ = (1 << 0),
+	CFG_DRQ = (1 << 1),
+	CFG_LR = (1 << 2),
+	CFG_DR = (1 << 3),
+	CFG_LE = (1 << 5),
+	CFG_LCQ = (1 << 6),
+	CFG_DCQ = (1 << 7),
+	CFG_Q_SHIFT = 8,
+	CFG_Q_MASK = 0x7f000000,
+};
+
+/*
+ *  Status Register (STS) bit definitions.
+ */
+enum {
+	STS_FE = (1 << 0),
+	STS_PI = (1 << 1),
+	STS_PL0 = (1 << 2),
+	STS_PL1 = (1 << 3),
+	STS_PI0 = (1 << 4),
+	STS_PI1 = (1 << 5),
+	STS_FUNC_ID_MASK = 0x000000c0,
+	STS_FUNC_ID_SHIFT = 6,
+	STS_F0E = (1 << 8),
+	STS_F1E = (1 << 9),
+	STS_F2E = (1 << 10),
+	STS_F3E = (1 << 11),
+	STS_NFE = (1 << 12),
+};
+
+/*
+ * Interrupt Enable Register (INTR_EN) bit definitions.
+ */
+enum {
+	INTR_EN_INTR_MASK = 0x007f0000,
+	INTR_EN_TYPE_MASK = 0x03000000,
+	INTR_EN_TYPE_ENABLE = 0x00000100,
+	INTR_EN_TYPE_DISABLE = 0x00000200,
+	INTR_EN_TYPE_READ = 0x00000300,
+	INTR_EN_IHD = (1 << 13),
+	INTR_EN_IHD_MASK = (INTR_EN_IHD << 16),
+	INTR_EN_EI = (1 << 14),
+	INTR_EN_EN = (1 << 15),
+};
+
+/*
+ * Interrupt Mask Register (INTR_MASK) bit definitions.
+ */
+enum {
+	INTR_MASK_PI = (1 << 0),
+	INTR_MASK_HL0 = (1 << 1),
+	INTR_MASK_LH0 = (1 << 2),
+	INTR_MASK_HL1 = (1 << 3),
+	INTR_MASK_LH1 = (1 << 4),
+	INTR_MASK_SE = (1 << 5),
+	INTR_MASK_LSC = (1 << 6),
+	INTR_MASK_MC = (1 << 7),
+	INTR_MASK_LINK_IRQS = INTR_MASK_LSC | INTR_MASK_SE | INTR_MASK_MC,
+};
+
+/*
+ *  Register (REV_ID) bit definitions.
+ */
+enum {
+	REV_ID_MASK = 0x0000000f,
+	REV_ID_NICROLL_SHIFT = 0,
+	REV_ID_NICREV_SHIFT = 4,
+	REV_ID_XGROLL_SHIFT = 8,
+	REV_ID_XGREV_SHIFT = 12,
+	REV_ID_CHIPREV_SHIFT = 28,
+};
+
+/*
+ *  Force ECC Error Register (FRC_ECC_ERR) bit definitions.
+ */
+enum {
+	FRC_ECC_ERR_VW = (1 << 12),
+	FRC_ECC_ERR_VB = (1 << 13),
+	FRC_ECC_ERR_NI = (1 << 14),
+	FRC_ECC_ERR_NO = (1 << 15),
+	FRC_ECC_PFE_SHIFT = 16,
+	FRC_ECC_ERR_DO = (1 << 18),
+	FRC_ECC_P14 = (1 << 19),
+};
+
+/*
+ *  Error Status Register (ERR_STS) bit definitions.
+ */
+enum {
+	ERR_STS_NOF = (1 << 0),
+	ERR_STS_NIF = (1 << 1),
+	ERR_STS_DRP = (1 << 2),
+	ERR_STS_XGP = (1 << 3),
+	ERR_STS_FOU = (1 << 4),
+	ERR_STS_FOC = (1 << 5),
+	ERR_STS_FOF = (1 << 6),
+	ERR_STS_FIU = (1 << 7),
+	ERR_STS_FIC = (1 << 8),
+	ERR_STS_FIF = (1 << 9),
+	ERR_STS_MOF = (1 << 10),
+	ERR_STS_TA = (1 << 11),
+	ERR_STS_MA = (1 << 12),
+	ERR_STS_MPE = (1 << 13),
+	ERR_STS_SCE = (1 << 14),
+	ERR_STS_STE = (1 << 15),
+	ERR_STS_FOW = (1 << 16),
+	ERR_STS_UE = (1 << 17),
+	ERR_STS_MCH = (1 << 26),
+	ERR_STS_LOC_SHIFT = 27,
+};
+
+/*
+ *  RAM Debug Address Register (RAM_DBG_ADDR) bit definitions.
+ */
+enum {
+	RAM_DBG_ADDR_FW = (1 << 30),
+	RAM_DBG_ADDR_FR = (1 << 31),
+};
+
+/*
+ * Semaphore Register (SEM) bit definitions.
+ */
+enum {
+	/*
+	 * Example:
+	 * reg = SEM_XGMAC0_MASK | (SEM_SET << SEM_XGMAC0_SHIFT)
+	 */
+	SEM_CLEAR = 0,
+	SEM_SET = 1,
+	SEM_FORCE = 3,
+	SEM_XGMAC0_SHIFT = 0,
+	SEM_XGMAC1_SHIFT = 2,
+	SEM_ICB_SHIFT = 4,
+	SEM_MAC_ADDR_SHIFT = 6,
+	SEM_FLASH_SHIFT = 8,
+	SEM_PROBE_SHIFT = 10,
+	SEM_RT_IDX_SHIFT = 12,
+	SEM_PROC_REG_SHIFT = 14,
+	SEM_XGMAC0_MASK = 0x00030000,
+	SEM_XGMAC1_MASK = 0x000c0000,
+	SEM_ICB_MASK = 0x00300000,
+	SEM_MAC_ADDR_MASK = 0x00c00000,
+	SEM_FLASH_MASK = 0x03000000,
+	SEM_PROBE_MASK = 0x0c000000,
+	SEM_RT_IDX_MASK = 0x30000000,
+	SEM_PROC_REG_MASK = 0xc0000000,
+};
+
+/*
+ *  10G MAC Address  Register (XGMAC_ADDR) bit definitions.
+ */
+enum {
+	XGMAC_ADDR_RDY = (1 << 31),
+	XGMAC_ADDR_R = (1 << 30),
+	XGMAC_ADDR_XME = (1 << 29),
+
+	/* XGMAC control registers */
+	PAUSE_SRC_LO = 0x00000100,
+	PAUSE_SRC_HI = 0x00000104,
+	GLOBAL_CFG = 0x00000108,
+	GLOBAL_CFG_RESET = (1 << 0),
+	GLOBAL_CFG_JUMBO = (1 << 6),
+	GLOBAL_CFG_TX_STAT_EN = (1 << 10),
+	GLOBAL_CFG_RX_STAT_EN = (1 << 11),
+	TX_CFG = 0x0000010c,
+	TX_CFG_RESET = (1 << 0),
+	TX_CFG_EN = (1 << 1),
+	TX_CFG_PREAM = (1 << 2),
+	RX_CFG = 0x00000110,
+	RX_CFG_RESET = (1 << 0),
+	RX_CFG_EN = (1 << 1),
+	RX_CFG_PREAM = (1 << 2),
+	FLOW_CTL = 0x0000011c,
+	PAUSE_OPCODE = 0x00000120,
+	PAUSE_TIMER = 0x00000124,
+	PAUSE_FRM_DEST_LO = 0x00000128,
+	PAUSE_FRM_DEST_HI = 0x0000012c,
+	MAC_TX_PARAMS = 0x00000134,
+	MAC_TX_PARAMS_JUMBO = (1 << 31),
+	MAC_TX_PARAMS_SIZE_SHIFT = 16,
+	MAC_RX_PARAMS = 0x00000138,
+	MAC_SYS_INT = 0x00000144,
+	MAC_SYS_INT_MASK = 0x00000148,
+	MAC_MGMT_INT = 0x0000014c,
+	MAC_MGMT_IN_MASK = 0x00000150,
+	EXT_ARB_MODE = 0x000001fc,
+
+	/* XGMAC TX statistics  registers */
+	TX_PKTS = 0x00000200,
+	TX_BYTES = 0x00000208,
+	TX_MCAST_PKTS = 0x00000210,
+	TX_BCAST_PKTS = 0x00000218,
+	TX_UCAST_PKTS = 0x00000220,
+	TX_CTL_PKTS = 0x00000228,
+	TX_PAUSE_PKTS = 0x00000230,
+	TX_64_PKT = 0x00000238,
+	TX_65_TO_127_PKT = 0x00000240,
+	TX_128_TO_255_PKT = 0x00000248,
+	TX_256_511_PKT = 0x00000250,
+	TX_512_TO_1023_PKT = 0x00000258,
+	TX_1024_TO_1518_PKT = 0x00000260,
+	TX_1519_TO_MAX_PKT = 0x00000268,
+	TX_UNDERSIZE_PKT = 0x00000270,
+	TX_OVERSIZE_PKT = 0x00000278,
+
+	/* XGMAC statistics control registers */
+	RX_HALF_FULL_DET = 0x000002a0,
+	TX_HALF_FULL_DET = 0x000002a4,
+	RX_OVERFLOW_DET = 0x000002a8,
+	TX_OVERFLOW_DET = 0x000002ac,
+	RX_HALF_FULL_MASK = 0x000002b0,
+	TX_HALF_FULL_MASK = 0x000002b4,
+	RX_OVERFLOW_MASK = 0x000002b8,
+	TX_OVERFLOW_MASK = 0x000002bc,
+	STAT_CNT_CTL = 0x000002c0,
+	STAT_CNT_CTL_CLEAR_TX = (1 << 0),
+	STAT_CNT_CTL_CLEAR_RX = (1 << 1),
+	AUX_RX_HALF_FULL_DET = 0x000002d0,
+	AUX_TX_HALF_FULL_DET = 0x000002d4,
+	AUX_RX_OVERFLOW_DET = 0x000002d8,
+	AUX_TX_OVERFLOW_DET = 0x000002dc,
+	AUX_RX_HALF_FULL_MASK = 0x000002f0,
+	AUX_TX_HALF_FULL_MASK = 0x000002f4,
+	AUX_RX_OVERFLOW_MASK = 0x000002f8,
+	AUX_TX_OVERFLOW_MASK = 0x000002fc,
+
+	/* XGMAC RX statistics  registers */
+	RX_BYTES = 0x00000300,
+	RX_BYTES_OK = 0x00000308,
+	RX_PKTS = 0x00000310,
+	RX_PKTS_OK = 0x00000318,
+	RX_BCAST_PKTS = 0x00000320,
+	RX_MCAST_PKTS = 0x00000328,
+	RX_UCAST_PKTS = 0x00000330,
+	RX_UNDERSIZE_PKTS = 0x00000338,
+	RX_OVERSIZE_PKTS = 0x00000340,
+	RX_JABBER_PKTS = 0x00000348,
+	RX_UNDERSIZE_FCERR_PKTS = 0x00000350,
+	RX_DROP_EVENTS = 0x00000358,
+	RX_FCERR_PKTS = 0x00000360,
+	RX_ALIGN_ERR = 0x00000368,
+	RX_SYMBOL_ERR = 0x00000370,
+	RX_MAC_ERR = 0x00000378,
+	RX_CTL_PKTS = 0x00000380,
+	RX_PAUSE_PKTS = 0x00000388,
+	RX_64_PKTS = 0x00000390,
+	RX_65_TO_127_PKTS = 0x00000398,
+	RX_128_255_PKTS = 0x000003a0,
+	RX_256_511_PKTS = 0x000003a8,
+	RX_512_TO_1023_PKTS = 0x000003b0,
+	RX_1024_TO_1518_PKTS = 0x000003b8,
+	RX_1519_TO_MAX_PKTS = 0x000003c0,
+	RX_LEN_ERR_PKTS = 0x000003c8,
+
+	/* XGMAC MDIO control registers */
+	MDIO_TX_DATA = 0x00000400,
+	MDIO_RX_DATA = 0x00000410,
+	MDIO_CMD = 0x00000420,
+	MDIO_PHY_ADDR = 0x00000430,
+	MDIO_PORT = 0x00000440,
+	MDIO_STATUS = 0x00000450,
+
+	XGMAC_REGISTER_END = 0x00000740,
+};
+
+/*
+ *  Enhanced Transmission Schedule Registers (NIC_ETS,CNA_ETS) bit definitions.
+ */
+enum {
+	ETS_QUEUE_SHIFT = 29,
+	ETS_REF = (1 << 26),
+	ETS_RS = (1 << 27),
+	ETS_P = (1 << 28),
+	ETS_FC_COS_SHIFT = 23,
+};
+
+/*
+ *  Flash Address Register (FLASH_ADDR) bit definitions.
+ */
+enum {
+	FLASH_ADDR_RDY = (1 << 31),
+	FLASH_ADDR_R = (1 << 30),
+	FLASH_ADDR_ERR = (1 << 29),
+};
+
+/*
+ *  Stop CQ Processing Register (CQ_STOP) bit definitions.
+ */
+enum {
+	CQ_STOP_QUEUE_MASK = (0x007f0000),
+	CQ_STOP_TYPE_MASK = (0x03000000),
+	CQ_STOP_TYPE_START = 0x00000100,
+	CQ_STOP_TYPE_STOP = 0x00000200,
+	CQ_STOP_TYPE_READ = 0x00000300,
+	CQ_STOP_EN = (1 << 15),
+};
+
+/*
+ *  MAC Protocol Address Index Register (MAC_ADDR_IDX) bit definitions.
+ */
+enum {
+	MAC_ADDR_IDX_SHIFT = 4,
+	MAC_ADDR_TYPE_SHIFT = 16,
+	MAC_ADDR_TYPE_COUNT = 10,
+	MAC_ADDR_TYPE_MASK = 0x000f0000,
+	MAC_ADDR_TYPE_CAM_MAC = 0x00000000,
+	MAC_ADDR_TYPE_MULTI_MAC = 0x00010000,
+	MAC_ADDR_TYPE_VLAN = 0x00020000,
+	MAC_ADDR_TYPE_MULTI_FLTR = 0x00030000,
+	MAC_ADDR_TYPE_FC_MAC = 0x00040000,
+	MAC_ADDR_TYPE_MGMT_MAC = 0x00050000,
+	MAC_ADDR_TYPE_MGMT_VLAN = 0x00060000,
+	MAC_ADDR_TYPE_MGMT_V4 = 0x00070000,
+	MAC_ADDR_TYPE_MGMT_V6 = 0x00080000,
+	MAC_ADDR_TYPE_MGMT_TU_DP = 0x00090000,
+	MAC_ADDR_ADR = (1 << 25),
+	MAC_ADDR_RS = (1 << 26),
+	MAC_ADDR_E = (1 << 27),
+	MAC_ADDR_MR = (1 << 30),
+	MAC_ADDR_MW = (1 << 31),
+	MAX_MULTICAST_ENTRIES = 32,
+
+	/* Entry count and words per entry
+	 * for each address type in the filter.
+	 */
+	MAC_ADDR_MAX_CAM_ENTRIES = 512,
+	MAC_ADDR_MAX_CAM_WCOUNT = 3,
+	MAC_ADDR_MAX_MULTICAST_ENTRIES = 32,
+	MAC_ADDR_MAX_MULTICAST_WCOUNT = 2,
+	MAC_ADDR_MAX_VLAN_ENTRIES = 4096,
+	MAC_ADDR_MAX_VLAN_WCOUNT = 1,
+	MAC_ADDR_MAX_MCAST_FLTR_ENTRIES = 4096,
+	MAC_ADDR_MAX_MCAST_FLTR_WCOUNT = 1,
+	MAC_ADDR_MAX_FC_MAC_ENTRIES = 4,
+	MAC_ADDR_MAX_FC_MAC_WCOUNT = 2,
+	MAC_ADDR_MAX_MGMT_MAC_ENTRIES = 8,
+	MAC_ADDR_MAX_MGMT_MAC_WCOUNT = 2,
+	MAC_ADDR_MAX_MGMT_VLAN_ENTRIES = 16,
+	MAC_ADDR_MAX_MGMT_VLAN_WCOUNT = 1,
+	MAC_ADDR_MAX_MGMT_V4_ENTRIES = 4,
+	MAC_ADDR_MAX_MGMT_V4_WCOUNT = 1,
+	MAC_ADDR_MAX_MGMT_V6_ENTRIES = 4,
+	MAC_ADDR_MAX_MGMT_V6_WCOUNT = 4,
+	MAC_ADDR_MAX_MGMT_TU_DP_ENTRIES = 4,
+	MAC_ADDR_MAX_MGMT_TU_DP_WCOUNT = 1,
+};
+
+/*
+ *  MAC Protocol Address Index Register (SPLT_HDR) bit definitions.
+ */
+enum {
+	SPLT_HDR_EP = (1 << 31),
+};
+
+/*
+ *  FCoE Receive Configuration Register (FC_RCV_CFG) bit definitions.
+ */
+enum {
+	FC_RCV_CFG_ECT = (1 << 15),
+	FC_RCV_CFG_DFH = (1 << 20),
+	FC_RCV_CFG_DVF = (1 << 21),
+	FC_RCV_CFG_RCE = (1 << 27),
+	FC_RCV_CFG_RFE = (1 << 28),
+	FC_RCV_CFG_TEE = (1 << 29),
+	FC_RCV_CFG_TCE = (1 << 30),
+	FC_RCV_CFG_TFE = (1 << 31),
+};
+
+/*
+ *  NIC Receive Configuration Register (NIC_RCV_CFG) bit definitions.
+ */
+enum {
+	NIC_RCV_CFG_PPE = (1 << 0),
+	NIC_RCV_CFG_VLAN_MASK = 0x00060000,
+	NIC_RCV_CFG_VLAN_ALL = 0x00000000,
+	NIC_RCV_CFG_VLAN_MATCH_ONLY = 0x00000002,
+	NIC_RCV_CFG_VLAN_MATCH_AND_NON = 0x00000004,
+	NIC_RCV_CFG_VLAN_NONE_AND_NON = 0x00000006,
+	NIC_RCV_CFG_RV = (1 << 3),
+	NIC_RCV_CFG_DFQ_MASK = (0x7f000000),
+	NIC_RCV_CFG_DFQ_SHIFT = 8,
+	NIC_RCV_CFG_DFQ = 0,	/* HARDCODE default queue to 0. */
+};
+
+/*
+ *   Mgmt Receive Configuration Register (MGMT_RCV_CFG) bit definitions.
+ */
+enum {
+	MGMT_RCV_CFG_ARP = (1 << 0),
+	MGMT_RCV_CFG_DHC = (1 << 1),
+	MGMT_RCV_CFG_DHS = (1 << 2),
+	MGMT_RCV_CFG_NP = (1 << 3),
+	MGMT_RCV_CFG_I6N = (1 << 4),
+	MGMT_RCV_CFG_I6R = (1 << 5),
+	MGMT_RCV_CFG_DH6 = (1 << 6),
+	MGMT_RCV_CFG_UD1 = (1 << 7),
+	MGMT_RCV_CFG_UD0 = (1 << 8),
+	MGMT_RCV_CFG_BCT = (1 << 9),
+	MGMT_RCV_CFG_MCT = (1 << 10),
+	MGMT_RCV_CFG_DM = (1 << 11),
+	MGMT_RCV_CFG_RM = (1 << 12),
+	MGMT_RCV_CFG_STL = (1 << 13),
+	MGMT_RCV_CFG_VLAN_MASK = 0xc0000000,
+	MGMT_RCV_CFG_VLAN_ALL = 0x00000000,
+	MGMT_RCV_CFG_VLAN_MATCH_ONLY = 0x00004000,
+	MGMT_RCV_CFG_VLAN_MATCH_AND_NON = 0x00008000,
+	MGMT_RCV_CFG_VLAN_NONE_AND_NON = 0x0000c000,
+};
+
+/*
+ *  Routing Index Register (RT_IDX) bit definitions.
+ */
+enum {
+	RT_IDX_IDX_SHIFT = 8,
+	RT_IDX_TYPE_MASK = 0x000f0000,
+	RT_IDX_TYPE_SHIFT = 16,
+	RT_IDX_TYPE_RT = 0x00000000,
+	RT_IDX_TYPE_RT_INV = 0x00010000,
+	RT_IDX_TYPE_NICQ = 0x00020000,
+	RT_IDX_TYPE_NICQ_INV = 0x00030000,
+	RT_IDX_DST_MASK = 0x00700000,
+	RT_IDX_DST_RSS = 0x00000000,
+	RT_IDX_DST_CAM_Q = 0x00100000,
+	RT_IDX_DST_COS_Q = 0x00200000,
+	RT_IDX_DST_DFLT_Q = 0x00300000,
+	RT_IDX_DST_DEST_Q = 0x00400000,
+	RT_IDX_RS = (1 << 26),
+	RT_IDX_E = (1 << 27),
+	RT_IDX_MR = (1 << 30),
+	RT_IDX_MW = (1 << 31),
+
+	/* Nic Queue format - type 2 bits */
+	RT_IDX_BCAST = (1 << 0),
+	RT_IDX_MCAST = (1 << 1),
+	RT_IDX_MCAST_MATCH = (1 << 2),
+	RT_IDX_MCAST_REG_MATCH = (1 << 3),
+	RT_IDX_MCAST_HASH_MATCH = (1 << 4),
+	RT_IDX_FC_MACH = (1 << 5),
+	RT_IDX_ETH_FCOE = (1 << 6),
+	RT_IDX_CAM_HIT = (1 << 7),
+	RT_IDX_CAM_BIT0 = (1 << 8),
+	RT_IDX_CAM_BIT1 = (1 << 9),
+	RT_IDX_VLAN_TAG = (1 << 10),
+	RT_IDX_VLAN_MATCH = (1 << 11),
+	RT_IDX_VLAN_FILTER = (1 << 12),
+	RT_IDX_ETH_SKIP1 = (1 << 13),
+	RT_IDX_ETH_SKIP2 = (1 << 14),
+	RT_IDX_BCAST_MCAST_MATCH = (1 << 15),
+	RT_IDX_802_3 = (1 << 16),
+	RT_IDX_LLDP = (1 << 17),
+	RT_IDX_UNUSED018 = (1 << 18),
+	RT_IDX_UNUSED019 = (1 << 19),
+	RT_IDX_UNUSED20 = (1 << 20),
+	RT_IDX_UNUSED21 = (1 << 21),
+	RT_IDX_ERR = (1 << 22),
+	RT_IDX_VALID = (1 << 23),
+	RT_IDX_TU_CSUM_ERR = (1 << 24),
+	RT_IDX_IP_CSUM_ERR = (1 << 25),
+	RT_IDX_MAC_ERR = (1 << 26),
+	RT_IDX_RSS_TCP6 = (1 << 27),
+	RT_IDX_RSS_TCP4 = (1 << 28),
+	RT_IDX_RSS_IPV6 = (1 << 29),
+	RT_IDX_RSS_IPV4 = (1 << 30),
+	RT_IDX_RSS_MATCH = (1 << 31),
+
+	/* Hierarchy for the NIC Queue Mask */
+	RT_IDX_ALL_ERR_SLOT = 0,
+	RT_IDX_MAC_ERR_SLOT = 0,
+	RT_IDX_IP_CSUM_ERR_SLOT = 1,
+	RT_IDX_TCP_UDP_CSUM_ERR_SLOT = 2,
+	RT_IDX_BCAST_SLOT = 3,
+	RT_IDX_MCAST_MATCH_SLOT = 4,
+	RT_IDX_ALLMULTI_SLOT = 5,
+	RT_IDX_UNUSED6_SLOT = 6,
+	RT_IDX_UNUSED7_SLOT = 7,
+	RT_IDX_RSS_MATCH_SLOT = 8,
+	RT_IDX_RSS_IPV4_SLOT = 8,
+	RT_IDX_RSS_IPV6_SLOT = 9,
+	RT_IDX_RSS_TCP4_SLOT = 10,
+	RT_IDX_RSS_TCP6_SLOT = 11,
+	RT_IDX_CAM_HIT_SLOT = 12,
+	RT_IDX_UNUSED013 = 13,
+	RT_IDX_UNUSED014 = 14,
+	RT_IDX_PROMISCUOUS_SLOT = 15,
+	RT_IDX_MAX_RT_SLOTS = 8,
+	RT_IDX_MAX_NIC_SLOTS = 16,
+};
+
+/*
+ * Serdes Address Register (XG_SERDES_ADDR) bit definitions.
+ */
+enum {
+	XG_SERDES_ADDR_RDY = (1 << 31),
+	XG_SERDES_ADDR_R = (1 << 30),
+
+	XG_SERDES_ADDR_STS = 0x00001E06,
+	XG_SERDES_ADDR_XFI1_PWR_UP = 0x00000005,
+	XG_SERDES_ADDR_XFI2_PWR_UP = 0x0000000a,
+	XG_SERDES_ADDR_XAUI_PWR_DOWN = 0x00000001,
+
+	/* Serdes coredump definitions. */
+	XG_SERDES_XAUI_AN_START = 0x00000000,
+	XG_SERDES_XAUI_AN_END = 0x00000034,
+	XG_SERDES_XAUI_HSS_PCS_START = 0x00000800,
+	XG_SERDES_XAUI_HSS_PCS_END = 0x0000880,
+	XG_SERDES_XFI_AN_START = 0x00001000,
+	XG_SERDES_XFI_AN_END = 0x00001034,
+	XG_SERDES_XFI_TRAIN_START = 0x10001050,
+	XG_SERDES_XFI_TRAIN_END = 0x1000107C,
+	XG_SERDES_XFI_HSS_PCS_START = 0x00001800,
+	XG_SERDES_XFI_HSS_PCS_END = 0x00001838,
+	XG_SERDES_XFI_HSS_TX_START = 0x00001c00,
+	XG_SERDES_XFI_HSS_TX_END = 0x00001c1f,
+	XG_SERDES_XFI_HSS_RX_START = 0x00001c40,
+	XG_SERDES_XFI_HSS_RX_END = 0x00001c5f,
+	XG_SERDES_XFI_HSS_PLL_START = 0x00001e00,
+	XG_SERDES_XFI_HSS_PLL_END = 0x00001e1f,
+};
+
+/*
+ *  NIC Probe Mux Address Register (PRB_MX_ADDR) bit definitions.
+ */
+enum {
+	PRB_MX_ADDR_ARE = (1 << 16),
+	PRB_MX_ADDR_UP = (1 << 15),
+	PRB_MX_ADDR_SWP = (1 << 14),
+
+	/* Module select values. */
+	PRB_MX_ADDR_MAX_MODS = 21,
+	PRB_MX_ADDR_MOD_SEL_SHIFT = 9,
+	PRB_MX_ADDR_MOD_SEL_TBD = 0,
+	PRB_MX_ADDR_MOD_SEL_IDE1 = 1,
+	PRB_MX_ADDR_MOD_SEL_IDE2 = 2,
+	PRB_MX_ADDR_MOD_SEL_FRB = 3,
+	PRB_MX_ADDR_MOD_SEL_ODE1 = 4,
+	PRB_MX_ADDR_MOD_SEL_ODE2 = 5,
+	PRB_MX_ADDR_MOD_SEL_DA1 = 6,
+	PRB_MX_ADDR_MOD_SEL_DA2 = 7,
+	PRB_MX_ADDR_MOD_SEL_IMP1 = 8,
+	PRB_MX_ADDR_MOD_SEL_IMP2 = 9,
+	PRB_MX_ADDR_MOD_SEL_OMP1 = 10,
+	PRB_MX_ADDR_MOD_SEL_OMP2 = 11,
+	PRB_MX_ADDR_MOD_SEL_ORS1 = 12,
+	PRB_MX_ADDR_MOD_SEL_ORS2 = 13,
+	PRB_MX_ADDR_MOD_SEL_REG = 14,
+	PRB_MX_ADDR_MOD_SEL_MAC1 = 16,
+	PRB_MX_ADDR_MOD_SEL_MAC2 = 17,
+	PRB_MX_ADDR_MOD_SEL_VQM1 = 18,
+	PRB_MX_ADDR_MOD_SEL_VQM2 = 19,
+	PRB_MX_ADDR_MOD_SEL_MOP = 20,
+	/* Bit fields indicating which modules
+	 * are valid for each clock domain.
+	 */
+	PRB_MX_ADDR_VALID_SYS_MOD = 0x000f7ff7,
+	PRB_MX_ADDR_VALID_PCI_MOD = 0x000040c1,
+	PRB_MX_ADDR_VALID_XGM_MOD = 0x00037309,
+	PRB_MX_ADDR_VALID_FC_MOD = 0x00003001,
+	PRB_MX_ADDR_VALID_TOTAL = 34,
+
+	/* Clock domain values. */
+	PRB_MX_ADDR_CLOCK_SHIFT = 6,
+	PRB_MX_ADDR_SYS_CLOCK = 0,
+	PRB_MX_ADDR_PCI_CLOCK = 2,
+	PRB_MX_ADDR_FC_CLOCK = 5,
+	PRB_MX_ADDR_XGM_CLOCK = 6,
+
+	PRB_MX_ADDR_MAX_MUX = 64,
+};
+
+/*
+ * Control Register Set Map
+ */
+enum {
+	PROC_ADDR = 0,		/* Use semaphore */
+	PROC_DATA = 0x04,	/* Use semaphore */
+	SYS = 0x08,
+	RST_FO = 0x0c,
+	FSC = 0x10,
+	CSR = 0x14,
+	LED = 0x18,
+	ICB_RID = 0x1c,		/* Use semaphore */
+	ICB_L = 0x20,		/* Use semaphore */
+	ICB_H = 0x24,		/* Use semaphore */
+	CFG = 0x28,
+	BIOS_ADDR = 0x2c,
+	STS = 0x30,
+	INTR_EN = 0x34,
+	INTR_MASK = 0x38,
+	ISR1 = 0x3c,
+	ISR2 = 0x40,
+	ISR3 = 0x44,
+	ISR4 = 0x48,
+	REV_ID = 0x4c,
+	FRC_ECC_ERR = 0x50,
+	ERR_STS = 0x54,
+	RAM_DBG_ADDR = 0x58,
+	RAM_DBG_DATA = 0x5c,
+	ECC_ERR_CNT = 0x60,
+	SEM = 0x64,
+	GPIO_1 = 0x68,		/* Use semaphore */
+	GPIO_2 = 0x6c,		/* Use semaphore */
+	GPIO_3 = 0x70,		/* Use semaphore */
+	RSVD2 = 0x74,
+	XGMAC_ADDR = 0x78,	/* Use semaphore */
+	XGMAC_DATA = 0x7c,	/* Use semaphore */
+	NIC_ETS = 0x80,
+	CNA_ETS = 0x84,
+	FLASH_ADDR = 0x88,	/* Use semaphore */
+	FLASH_DATA = 0x8c,	/* Use semaphore */
+	CQ_STOP = 0x90,
+	PAGE_TBL_RID = 0x94,
+	WQ_PAGE_TBL_LO = 0x98,
+	WQ_PAGE_TBL_HI = 0x9c,
+	CQ_PAGE_TBL_LO = 0xa0,
+	CQ_PAGE_TBL_HI = 0xa4,
+	MAC_ADDR_IDX = 0xa8,	/* Use semaphore */
+	MAC_ADDR_DATA = 0xac,	/* Use semaphore */
+	COS_DFLT_CQ1 = 0xb0,
+	COS_DFLT_CQ2 = 0xb4,
+	ETYPE_SKIP1 = 0xb8,
+	ETYPE_SKIP2 = 0xbc,
+	SPLT_HDR = 0xc0,
+	FC_PAUSE_THRES = 0xc4,
+	NIC_PAUSE_THRES = 0xc8,
+	FC_ETHERTYPE = 0xcc,
+	FC_RCV_CFG = 0xd0,
+	NIC_RCV_CFG = 0xd4,
+	FC_COS_TAGS = 0xd8,
+	NIC_COS_TAGS = 0xdc,
+	MGMT_RCV_CFG = 0xe0,
+	RT_IDX = 0xe4,
+	RT_DATA = 0xe8,
+	RSVD7 = 0xec,
+	XG_SERDES_ADDR = 0xf0,
+	XG_SERDES_DATA = 0xf4,
+	PRB_MX_ADDR = 0xf8,	/* Use semaphore */
+	PRB_MX_DATA = 0xfc,	/* Use semaphore */
+};
+
+#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
+#define SMALL_BUFFER_SIZE 256
+#define SMALL_BUF_MAP_SIZE SMALL_BUFFER_SIZE
+#define SPLT_SETTING  FSC_DBRST_1024
+#define SPLT_LEN 0
+#define QLGE_SB_PAD 0
+#else
+#define SMALL_BUFFER_SIZE 512
+#define SMALL_BUF_MAP_SIZE (SMALL_BUFFER_SIZE / 2)
+#define SPLT_SETTING  FSC_SH
+#define SPLT_LEN (SPLT_HDR_EP | \
+	min(SMALL_BUF_MAP_SIZE, 1023))
+#define QLGE_SB_PAD 32
+#endif
+
+/*
+ * CAM output format.
+ */
+enum {
+	CAM_OUT_ROUTE_FC = 0,
+	CAM_OUT_ROUTE_NIC = 1,
+	CAM_OUT_FUNC_SHIFT = 2,
+	CAM_OUT_RV = (1 << 4),
+	CAM_OUT_SH = (1 << 15),
+	CAM_OUT_CQ_ID_SHIFT = 5,
+};
+
+/*
+ * Mailbox  definitions
+ */
+enum {
+	/* Asynchronous Event Notifications */
+	AEN_SYS_ERR = 0x00008002,
+	AEN_LINK_UP = 0x00008011,
+	AEN_LINK_DOWN = 0x00008012,
+	AEN_IDC_CMPLT = 0x00008100,
+	AEN_IDC_REQ = 0x00008101,
+	AEN_IDC_EXT = 0x00008102,
+	AEN_DCBX_CHG = 0x00008110,
+	AEN_AEN_LOST = 0x00008120,
+	AEN_AEN_SFP_IN = 0x00008130,
+	AEN_AEN_SFP_OUT = 0x00008131,
+	AEN_FW_INIT_DONE = 0x00008400,
+	AEN_FW_INIT_FAIL = 0x00008401,
+
+	/* Mailbox Command Opcodes. */
+	MB_CMD_NOP = 0x00000000,
+	MB_CMD_EX_FW = 0x00000002,
+	MB_CMD_MB_TEST = 0x00000006,
+	MB_CMD_CSUM_TEST = 0x00000007,	/* Verify Checksum */
+	MB_CMD_ABOUT_FW = 0x00000008,
+	MB_CMD_COPY_RISC_RAM = 0x0000000a,
+	MB_CMD_LOAD_RISC_RAM = 0x0000000b,
+	MB_CMD_DUMP_RISC_RAM = 0x0000000c,
+	MB_CMD_WRITE_RAM = 0x0000000d,
+	MB_CMD_INIT_RISC_RAM = 0x0000000e,
+	MB_CMD_READ_RAM = 0x0000000f,
+	MB_CMD_STOP_FW = 0x00000014,
+	MB_CMD_MAKE_SYS_ERR = 0x0000002a,
+	MB_CMD_WRITE_SFP = 0x00000030,
+	MB_CMD_READ_SFP = 0x00000031,
+	MB_CMD_INIT_FW = 0x00000060,
+	MB_CMD_GET_IFCB = 0x00000061,
+	MB_CMD_GET_FW_STATE = 0x00000069,
+	MB_CMD_IDC_REQ = 0x00000100,	/* Inter-Driver Communication */
+	MB_CMD_IDC_ACK = 0x00000101,	/* Inter-Driver Communication */
+	MB_CMD_SET_WOL_MODE = 0x00000110,	/* Wake On Lan */
+	MB_WOL_DISABLE = 0,
+	MB_WOL_MAGIC_PKT = (1 << 1),
+	MB_WOL_FLTR = (1 << 2),
+	MB_WOL_UCAST = (1 << 3),
+	MB_WOL_MCAST = (1 << 4),
+	MB_WOL_BCAST = (1 << 5),
+	MB_WOL_LINK_UP = (1 << 6),
+	MB_WOL_LINK_DOWN = (1 << 7),
+	MB_WOL_MODE_ON = (1 << 16),		/* Wake on Lan Mode on */
+	MB_CMD_SET_WOL_FLTR = 0x00000111,	/* Wake On Lan Filter */
+	MB_CMD_CLEAR_WOL_FLTR = 0x00000112, /* Wake On Lan Filter */
+	MB_CMD_SET_WOL_MAGIC = 0x00000113,	/* Wake On Lan Magic Packet */
+	MB_CMD_CLEAR_WOL_MAGIC = 0x00000114,/* Wake On Lan Magic Packet */
+	MB_CMD_SET_WOL_IMMED = 0x00000115,
+	MB_CMD_PORT_RESET = 0x00000120,
+	MB_CMD_SET_PORT_CFG = 0x00000122,
+	MB_CMD_GET_PORT_CFG = 0x00000123,
+	MB_CMD_GET_LINK_STS = 0x00000124,
+	MB_CMD_SET_LED_CFG = 0x00000125, /* Set LED Configuration Register */
+		QL_LED_BLINK = 0x03e803e8,
+	MB_CMD_GET_LED_CFG = 0x00000126, /* Get LED Configuration Register */
+	MB_CMD_SET_MGMNT_TFK_CTL = 0x00000160, /* Set Mgmnt Traffic Control */
+	MB_SET_MPI_TFK_STOP = (1 << 0),
+	MB_SET_MPI_TFK_RESUME = (1 << 1),
+	MB_CMD_GET_MGMNT_TFK_CTL = 0x00000161, /* Get Mgmnt Traffic Control */
+	MB_GET_MPI_TFK_STOPPED = (1 << 0),
+	MB_GET_MPI_TFK_FIFO_EMPTY = (1 << 1),
+	/* Sub-commands for IDC request.
+	 * This describes the reason for the
+	 * IDC request.
+	 */
+	MB_CMD_IOP_NONE = 0x0000,
+	MB_CMD_IOP_PREP_UPDATE_MPI	= 0x0001,
+	MB_CMD_IOP_COMP_UPDATE_MPI	= 0x0002,
+	MB_CMD_IOP_PREP_LINK_DOWN	= 0x0010,
+	MB_CMD_IOP_DVR_START	 = 0x0100,
+	MB_CMD_IOP_FLASH_ACC	 = 0x0101,
+	MB_CMD_IOP_RESTART_MPI	= 0x0102,
+	MB_CMD_IOP_CORE_DUMP_MPI	= 0x0103,
+
+	/* Mailbox Command Status. */
+	MB_CMD_STS_GOOD = 0x00004000,	/* Success. */
+	MB_CMD_STS_INTRMDT = 0x00001000,	/* Intermediate Complete. */
+	MB_CMD_STS_INVLD_CMD = 0x00004001,	/* Invalid. */
+	MB_CMD_STS_XFC_ERR = 0x00004002,	/* Interface Error. */
+	MB_CMD_STS_CSUM_ERR = 0x00004003,	/* Csum Error. */
+	MB_CMD_STS_ERR = 0x00004005,	/* System Error. */
+	MB_CMD_STS_PARAM_ERR = 0x00004006,	/* Parameter Error. */
+};
+
+struct mbox_params {
+	u32 mbox_in[MAILBOX_COUNT];
+	u32 mbox_out[MAILBOX_COUNT];
+	int in_count;
+	int out_count;
+};
+
+struct flash_params_8012 {
+	u8 dev_id_str[4];
+	__le16 size;
+	__le16 csum;
+	__le16 ver;
+	__le16 sub_dev_id;
+	u8 mac_addr[6];
+	__le16 res;
+};
+
+/* 8000 device's flash is a different structure
+ * at a different offset in flash.
+ */
+#define FUNC0_FLASH_OFFSET 0x140200
+#define FUNC1_FLASH_OFFSET 0x140600
+
+/* Flash related data structures. */
+struct flash_params_8000 {
+	u8 dev_id_str[4];	/* "8000" */
+	__le16 ver;
+	__le16 size;
+	__le16 csum;
+	__le16 reserved0;
+	__le16 total_size;
+	__le16 entry_count;
+	u8 data_type0;
+	u8 data_size0;
+	u8 mac_addr[6];
+	u8 data_type1;
+	u8 data_size1;
+	u8 mac_addr1[6];
+	u8 data_type2;
+	u8 data_size2;
+	__le16 vlan_id;
+	u8 data_type3;
+	u8 data_size3;
+	__le16 last;
+	u8 reserved1[464];
+	__le16	subsys_ven_id;
+	__le16	subsys_dev_id;
+	u8 reserved2[4];
+};
+
+union flash_params {
+	struct flash_params_8012 flash_params_8012;
+	struct flash_params_8000 flash_params_8000;
+};
+
+/*
+ * doorbell space for the rx ring context
+ */
+struct rx_doorbell_context {
+	u32 cnsmr_idx;		/* 0x00 */
+	u32 valid;		/* 0x04 */
+	u32 reserved[4];	/* 0x08-0x14 */
+	u32 lbq_prod_idx;	/* 0x18 */
+	u32 sbq_prod_idx;	/* 0x1c */
+};
+
+/*
+ * doorbell space for the tx ring context
+ */
+struct tx_doorbell_context {
+	u32 prod_idx;		/* 0x00 */
+	u32 valid;		/* 0x04 */
+	u32 reserved[4];	/* 0x08-0x14 */
+	u32 lbq_prod_idx;	/* 0x18 */
+	u32 sbq_prod_idx;	/* 0x1c */
+};
+
+/* DATA STRUCTURES SHARED WITH HARDWARE. */
+struct tx_buf_desc {
+	__le64 addr;
+	__le32 len;
+#define TX_DESC_LEN_MASK	0x000fffff
+#define TX_DESC_C	0x40000000
+#define TX_DESC_E	0x80000000
+} __packed;
+
+/*
+ * IOCB Definitions...
+ */
+
+#define OPCODE_OB_MAC_IOCB		0x01
+#define OPCODE_OB_MAC_TSO_IOCB		0x02
+#define OPCODE_IB_MAC_IOCB		0x20
+#define OPCODE_IB_MPI_IOCB		0x21
+#define OPCODE_IB_AE_IOCB		0x3f
+
+struct qlge_ob_mac_iocb_req {
+	u8 opcode;
+	u8 flags1;
+#define OB_MAC_IOCB_REQ_OI	0x01
+#define OB_MAC_IOCB_REQ_I	0x02
+#define OB_MAC_IOCB_REQ_D	0x08
+#define OB_MAC_IOCB_REQ_F	0x10
+	u8 flags2;
+	u8 flags3;
+#define OB_MAC_IOCB_DFP	0x02
+#define OB_MAC_IOCB_V	0x04
+	__le32 reserved1[2];
+	__le16 frame_len;
+#define OB_MAC_IOCB_LEN_MASK 0x3ffff
+	__le16 reserved2;
+	u32 tid;
+	u32 txq_idx;
+	__le32 reserved3;
+	__le16 vlan_tci;
+	__le16 reserved4;
+	struct tx_buf_desc tbd[TX_DESC_PER_IOCB];
+} __packed;
+
+struct qlge_ob_mac_iocb_rsp {
+	u8 opcode;		/* */
+	u8 flags1;		/* */
+#define OB_MAC_IOCB_RSP_OI	0x01	/* */
+#define OB_MAC_IOCB_RSP_I	0x02	/* */
+#define OB_MAC_IOCB_RSP_E	0x08	/* */
+#define OB_MAC_IOCB_RSP_S	0x10	/* too Short */
+#define OB_MAC_IOCB_RSP_L	0x20	/* too Large */
+#define OB_MAC_IOCB_RSP_P	0x40	/* Padded */
+	u8 flags2;		/* */
+	u8 flags3;		/* */
+#define OB_MAC_IOCB_RSP_B	0x80	/* */
+	u32 tid;
+	u32 txq_idx;
+	__le32 reserved[13];
+} __packed;
+
+struct qlge_ob_mac_tso_iocb_req {
+	u8 opcode;
+	u8 flags1;
+#define OB_MAC_TSO_IOCB_OI	0x01
+#define OB_MAC_TSO_IOCB_I	0x02
+#define OB_MAC_TSO_IOCB_D	0x08
+#define OB_MAC_TSO_IOCB_IP4	0x40
+#define OB_MAC_TSO_IOCB_IP6	0x80
+	u8 flags2;
+#define OB_MAC_TSO_IOCB_LSO	0x20
+#define OB_MAC_TSO_IOCB_UC	0x40
+#define OB_MAC_TSO_IOCB_TC	0x80
+	u8 flags3;
+#define OB_MAC_TSO_IOCB_IC	0x01
+#define OB_MAC_TSO_IOCB_DFP	0x02
+#define OB_MAC_TSO_IOCB_V	0x04
+	__le32 reserved1[2];
+	__le32 frame_len;
+	u32 tid;
+	u32 txq_idx;
+	__le16 total_hdrs_len;
+	__le16 net_trans_offset;
+#define OB_MAC_TRANSPORT_HDR_SHIFT 6
+	__le16 vlan_tci;
+	__le16 mss;
+	struct tx_buf_desc tbd[TX_DESC_PER_IOCB];
+} __packed;
+
+struct qlge_ob_mac_tso_iocb_rsp {
+	u8 opcode;
+	u8 flags1;
+#define OB_MAC_TSO_IOCB_RSP_OI	0x01
+#define OB_MAC_TSO_IOCB_RSP_I	0x02
+#define OB_MAC_TSO_IOCB_RSP_E	0x08
+#define OB_MAC_TSO_IOCB_RSP_S	0x10
+#define OB_MAC_TSO_IOCB_RSP_L	0x20
+#define OB_MAC_TSO_IOCB_RSP_P	0x40
+	u8 flags2;		/* */
+	u8 flags3;		/* */
+#define OB_MAC_TSO_IOCB_RSP_B	0x8000
+	u32 tid;
+	u32 txq_idx;
+	__le32 reserved2[13];
+} __packed;
+
+struct qlge_ib_mac_iocb_rsp {
+	u8 opcode;		/* 0x20 */
+	u8 flags1;
+#define IB_MAC_IOCB_RSP_OI	0x01	/* Override intr delay */
+#define IB_MAC_IOCB_RSP_I	0x02	/* Disable Intr Generation */
+#define IB_MAC_CSUM_ERR_MASK	0x1c	/* A mask to use for csum errs */
+#define IB_MAC_IOCB_RSP_TE	0x04	/* Checksum error */
+#define IB_MAC_IOCB_RSP_NU	0x08	/* No checksum rcvd */
+#define IB_MAC_IOCB_RSP_IE	0x10	/* IPv4 checksum error */
+#define IB_MAC_IOCB_RSP_M_MASK	0x60	/* Multicast info */
+#define IB_MAC_IOCB_RSP_M_NONE	0x00	/* Not mcast frame */
+#define IB_MAC_IOCB_RSP_M_HASH	0x20	/* HASH mcast frame */
+#define IB_MAC_IOCB_RSP_M_REG	0x40	/* Registered mcast frame */
+#define IB_MAC_IOCB_RSP_M_PROM	0x60	/* Promiscuous mcast frame */
+#define IB_MAC_IOCB_RSP_B	0x80	/* Broadcast frame */
+	u8 flags2;
+#define IB_MAC_IOCB_RSP_P	0x01	/* Promiscuous frame */
+#define IB_MAC_IOCB_RSP_V	0x02	/* Vlan tag present */
+#define IB_MAC_IOCB_RSP_ERR_MASK	0x1c	/*  */
+#define IB_MAC_IOCB_RSP_ERR_CODE_ERR	0x04
+#define IB_MAC_IOCB_RSP_ERR_OVERSIZE	0x08
+#define IB_MAC_IOCB_RSP_ERR_UNDERSIZE	0x10
+#define IB_MAC_IOCB_RSP_ERR_PREAMBLE	0x14
+#define IB_MAC_IOCB_RSP_ERR_FRAME_LEN	0x18
+#define IB_MAC_IOCB_RSP_ERR_CRC		0x1c
+#define IB_MAC_IOCB_RSP_U	0x20	/* UDP packet */
+#define IB_MAC_IOCB_RSP_T	0x40	/* TCP packet */
+#define IB_MAC_IOCB_RSP_FO	0x80	/* Failover port */
+	u8 flags3;
+#define IB_MAC_IOCB_RSP_RSS_MASK	0x07	/* RSS mask */
+#define IB_MAC_IOCB_RSP_M_NONE		0x00	/* No RSS match */
+#define IB_MAC_IOCB_RSP_M_IPV4		0x04	/* IPv4 RSS match */
+#define IB_MAC_IOCB_RSP_M_IPV6		0x02	/* IPv6 RSS match */
+#define IB_MAC_IOCB_RSP_M_TCP_V4	0x05	/* TCP with IPv4 */
+#define IB_MAC_IOCB_RSP_M_TCP_V6	0x03	/* TCP with IPv6 */
+#define IB_MAC_IOCB_RSP_V4		0x08	/* IPV4 */
+#define IB_MAC_IOCB_RSP_V6		0x10	/* IPV6 */
+#define IB_MAC_IOCB_RSP_IH		0x20	/* Split after IP header */
+#define IB_MAC_IOCB_RSP_DS		0x40	/* data is in small buffer */
+#define IB_MAC_IOCB_RSP_DL		0x80	/* data is in large buffer */
+	__le32 data_len;	/* */
+	__le64 data_addr;	/* */
+	__le32 rss;		/* */
+	__le16 vlan_id;		/* 12 bits */
+#define IB_MAC_IOCB_RSP_C	0x1000	/* VLAN CFI bit */
+#define IB_MAC_IOCB_RSP_COS_SHIFT	12	/* class of service value */
+#define IB_MAC_IOCB_RSP_VLAN_MASK	0x0ffff
+
+	__le16 reserved1;
+	__le32 reserved2[6];
+	u8 reserved3[3];
+	u8 flags4;
+#define IB_MAC_IOCB_RSP_HV	0x20
+#define IB_MAC_IOCB_RSP_HS	0x40
+#define IB_MAC_IOCB_RSP_HL	0x80
+	__le32 hdr_len;		/* */
+	__le64 hdr_addr;	/* */
+} __packed;
+
+struct qlge_ib_ae_iocb_rsp {
+	u8 opcode;
+	u8 flags1;
+#define IB_AE_IOCB_RSP_OI		0x01
+#define IB_AE_IOCB_RSP_I		0x02
+	u8 event;
+#define LINK_UP_EVENT			0x00
+#define LINK_DOWN_EVENT			0x01
+#define CAM_LOOKUP_ERR_EVENT		0x06
+#define SOFT_ECC_ERROR_EVENT		0x07
+#define MGMT_ERR_EVENT			0x08
+#define TEN_GIG_MAC_EVENT		0x09
+#define GPI0_H2L_EVENT			0x10
+#define GPI0_L2H_EVENT			0x20
+#define GPI1_H2L_EVENT			0x11
+#define GPI1_L2H_EVENT			0x21
+#define PCI_ERR_ANON_BUF_RD		0x40
+	u8 q_id;
+	__le32 reserved[15];
+} __packed;
+
+/*
+ * These three structures are for generic
+ * handling of ib and ob iocbs.
+ */
+struct qlge_net_rsp_iocb {
+	u8 opcode;
+	u8 flags0;
+	__le16 length;
+	__le32 tid;
+	__le32 reserved[14];
+} __packed;
+
+struct qlge_net_req_iocb {
+	u8 opcode;
+	u8 flags0;
+	__le16 flags1;
+	__le32 tid;
+	__le32 reserved1[30];
+} __packed;
+
+/*
+ * tx ring initialization control block for chip.
+ * It is defined as:
+ * "Work Queue Initialization Control Block"
+ */
+struct wqicb {
+	__le16 len;
+#define Q_LEN_V		(1 << 4)
+#define Q_LEN_CPP_CONT	0x0000
+#define Q_LEN_CPP_16	0x0001
+#define Q_LEN_CPP_32	0x0002
+#define Q_LEN_CPP_64	0x0003
+#define Q_LEN_CPP_512	0x0006
+	__le16 flags;
+#define Q_PRI_SHIFT	1
+#define Q_FLAGS_LC	0x1000
+#define Q_FLAGS_LB	0x2000
+#define Q_FLAGS_LI	0x4000
+#define Q_FLAGS_LO	0x8000
+	__le16 cq_id_rss;
+#define Q_CQ_ID_RSS_RV 0x8000
+	__le16 rid;
+	__le64 addr;
+	__le64 cnsmr_idx_addr;
+} __packed;
+
+/*
+ * rx ring initialization control block for chip.
+ * It is defined as:
+ * "Completion Queue Initialization Control Block"
+ */
+struct cqicb {
+	u8 msix_vect;
+	u8 reserved1;
+	u8 reserved2;
+	u8 flags;
+#define FLAGS_LV	0x08
+#define FLAGS_LS	0x10
+#define FLAGS_LL	0x20
+#define FLAGS_LI	0x40
+#define FLAGS_LC	0x80
+	__le16 len;
+#define LEN_V		(1 << 4)
+#define LEN_CPP_CONT	0x0000
+#define LEN_CPP_32	0x0001
+#define LEN_CPP_64	0x0002
+#define LEN_CPP_128	0x0003
+	__le16 rid;
+	__le64 addr;
+	__le64 prod_idx_addr;
+	__le16 pkt_delay;
+	__le16 irq_delay;
+	__le64 lbq_addr;
+	__le16 lbq_buf_size;
+	__le16 lbq_len;		/* entry count */
+	__le64 sbq_addr;
+	__le16 sbq_buf_size;
+	__le16 sbq_len;		/* entry count */
+} __packed;
+
+struct ricb {
+	u8 base_cq;
+#define RSS_L4K 0x80
+	u8 flags;
+#define RSS_L6K 0x01
+#define RSS_LI  0x02
+#define RSS_LB  0x04
+#define RSS_LM  0x08
+#define RSS_RI4 0x10
+#define RSS_RT4 0x20
+#define RSS_RI6 0x40
+#define RSS_RT6 0x80
+	__le16 mask;
+	u8 hash_cq_id[1024];
+	__le32 ipv6_hash_key[10];
+	__le32 ipv4_hash_key[4];
+} __packed;
+
+/* SOFTWARE/DRIVER DATA STRUCTURES. */
+
+struct qlge_oal {
+	struct tx_buf_desc oal[TX_DESC_PER_OAL];
+};
+
+struct map_list {
+	DEFINE_DMA_UNMAP_ADDR(mapaddr);
+	DEFINE_DMA_UNMAP_LEN(maplen);
+};
+
+struct tx_ring_desc {
+	struct sk_buff *skb;
+	struct qlge_ob_mac_iocb_req *queue_entry;
+	u32 index;
+	struct qlge_oal oal;
+	struct map_list map[MAX_SKB_FRAGS + 2];
+	int map_cnt;
+	struct tx_ring_desc *next;
+};
+
+#define QL_TXQ_IDX(qdev, skb) (smp_processor_id() % (qdev->tx_ring_count))
+
+struct tx_ring {
+	/*
+	 * queue info.
+	 */
+	struct wqicb wqicb;	/* structure used to inform chip of new queue */
+	void *wq_base;		/* pci_alloc:virtual addr for tx */
+	dma_addr_t wq_base_dma;	/* pci_alloc:dma addr for tx */
+	__le32 *cnsmr_idx_sh_reg;	/* shadow copy of consumer idx */
+	dma_addr_t cnsmr_idx_sh_reg_dma;	/* dma-shadow copy of consumer */
+	u32 wq_size;		/* size in bytes of queue area */
+	u32 wq_len;		/* number of entries in queue */
+	void __iomem *prod_idx_db_reg;	/* doorbell area index reg at offset 0x00 */
+	void __iomem *valid_db_reg;	/* doorbell area valid reg at offset 0x04 */
+	u16 prod_idx;		/* current value for prod idx */
+	u16 cq_id;		/* completion (rx) queue for tx completions */
+	u8 wq_id;		/* queue id for this entry */
+	u8 reserved1[3];
+	struct tx_ring_desc *q;	/* descriptor list for the queue */
+	spinlock_t lock;
+	atomic_t tx_count;	/* counts down for every outstanding IO */
+	struct delayed_work tx_work;
+	struct qlge_adapter *qdev;
+	u64 tx_packets;
+	u64 tx_bytes;
+	u64 tx_errors;
+};
+
+struct qlge_page_chunk {
+	struct page *page;
+	void *va; /* virt addr including offset */
+	unsigned int offset;
+};
+
+struct qlge_bq_desc {
+	union {
+		/* for large buffers */
+		struct qlge_page_chunk pg_chunk;
+		/* for small buffers */
+		struct sk_buff *skb;
+	} p;
+	dma_addr_t dma_addr;
+	/* address in ring where the buffer address is written for the device */
+	__le64 *buf_ptr;
+	u32 index;
+};
+
+/* buffer queue */
+struct qlge_bq {
+	__le64 *base;
+	dma_addr_t base_dma;
+	__le64 *base_indirect;
+	dma_addr_t base_indirect_dma;
+	struct qlge_bq_desc *queue;
+	/* prod_idx is the index of the first buffer that may NOT be used by
+	 * hw, ie. one after the last. Advanced by sw.
+	 */
+	void __iomem *prod_idx_db_reg;
+	/* next index where sw should refill a buffer for hw */
+	u16 next_to_use;
+	/* next index where sw expects to find a buffer filled by hw */
+	u16 next_to_clean;
+	enum {
+		QLGE_SB,		/* small buffer */
+		QLGE_LB,		/* large buffer */
+	} type;
+};
+
+#define QLGE_BQ_CONTAINER(bq) \
+({ \
+	typeof(bq) _bq = bq; \
+	(struct rx_ring *)((char *)_bq - (_bq->type == QLGE_SB ? \
+					  offsetof(struct rx_ring, sbq) : \
+					  offsetof(struct rx_ring, lbq))); \
+})
+
+/* Experience shows that the device ignores the low 4 bits of the tail index.
+ * Refill up to a x16 multiple.
+ */
+#define QLGE_BQ_ALIGN(index) ALIGN_DOWN(index, 16)
+
+#define QLGE_BQ_WRAP(index) ((index) & (QLGE_BQ_LEN - 1))
+
+#define QLGE_BQ_HW_OWNED(bq) \
+({ \
+	typeof(bq) _bq = bq; \
+	QLGE_BQ_WRAP(QLGE_BQ_ALIGN((_bq)->next_to_use) - \
+		     (_bq)->next_to_clean); \
+})
+
+struct rx_ring {
+	struct cqicb cqicb;	/* The chip's completion queue init control block. */
+
+	/* Completion queue elements. */
+	void *cq_base;
+	dma_addr_t cq_base_dma;
+	u32 cq_size;
+	u32 cq_len;
+	u16 cq_id;
+	__le32 *prod_idx_sh_reg;	/* Shadowed producer register. */
+	dma_addr_t prod_idx_sh_reg_dma;
+	void __iomem *cnsmr_idx_db_reg;	/* PCI doorbell mem area + 0 */
+	u32 cnsmr_idx;		/* current sw idx */
+	struct qlge_net_rsp_iocb *curr_entry;	/* next entry on queue */
+	void __iomem *valid_db_reg;	/* PCI doorbell mem area + 0x04 */
+
+	/* Large buffer queue elements. */
+	struct qlge_bq lbq;
+	struct qlge_page_chunk master_chunk;
+	dma_addr_t chunk_dma_addr;
+
+	/* Small buffer queue elements. */
+	struct qlge_bq sbq;
+
+	/* Misc. handler elements. */
+	u32 irq;		/* Which vector this ring is assigned. */
+	u32 cpu;		/* Which CPU this should run on. */
+	struct delayed_work refill_work;
+	char name[IFNAMSIZ + 5];
+	struct napi_struct napi;
+	u8 reserved;
+	struct qlge_adapter *qdev;
+	u64 rx_packets;
+	u64 rx_multicast;
+	u64 rx_bytes;
+	u64 rx_dropped;
+	u64 rx_errors;
+};
+
+/*
+ * RSS Initialization Control Block
+ */
+struct hash_id {
+	u8 value[4];
+};
+
+struct nic_stats {
+	/*
+	 * These stats come from offset 200h to 278h
+	 * in the XGMAC register.
+	 */
+	u64 tx_pkts;
+	u64 tx_bytes;
+	u64 tx_mcast_pkts;
+	u64 tx_bcast_pkts;
+	u64 tx_ucast_pkts;
+	u64 tx_ctl_pkts;
+	u64 tx_pause_pkts;
+	u64 tx_64_pkt;
+	u64 tx_65_to_127_pkt;
+	u64 tx_128_to_255_pkt;
+	u64 tx_256_511_pkt;
+	u64 tx_512_to_1023_pkt;
+	u64 tx_1024_to_1518_pkt;
+	u64 tx_1519_to_max_pkt;
+	u64 tx_undersize_pkt;
+	u64 tx_oversize_pkt;
+
+	/*
+	 * These stats come from offset 300h to 3C8h
+	 * in the XGMAC register.
+	 */
+	u64 rx_bytes;
+	u64 rx_bytes_ok;
+	u64 rx_pkts;
+	u64 rx_pkts_ok;
+	u64 rx_bcast_pkts;
+	u64 rx_mcast_pkts;
+	u64 rx_ucast_pkts;
+	u64 rx_undersize_pkts;
+	u64 rx_oversize_pkts;
+	u64 rx_jabber_pkts;
+	u64 rx_undersize_fcerr_pkts;
+	u64 rx_drop_events;
+	u64 rx_fcerr_pkts;
+	u64 rx_align_err;
+	u64 rx_symbol_err;
+	u64 rx_mac_err;
+	u64 rx_ctl_pkts;
+	u64 rx_pause_pkts;
+	u64 rx_64_pkts;
+	u64 rx_65_to_127_pkts;
+	u64 rx_128_255_pkts;
+	u64 rx_256_511_pkts;
+	u64 rx_512_to_1023_pkts;
+	u64 rx_1024_to_1518_pkts;
+	u64 rx_1519_to_max_pkts;
+	u64 rx_len_err_pkts;
+	/* Receive Mac Err stats */
+	u64 rx_code_err;
+	u64 rx_oversize_err;
+	u64 rx_undersize_err;
+	u64 rx_preamble_err;
+	u64 rx_frame_len_err;
+	u64 rx_crc_err;
+	u64 rx_err_count;
+	/*
+	 * These stats come from offset 500h to 5C8h
+	 * in the XGMAC register.
+	 */
+	u64 tx_cbfc_pause_frames0;
+	u64 tx_cbfc_pause_frames1;
+	u64 tx_cbfc_pause_frames2;
+	u64 tx_cbfc_pause_frames3;
+	u64 tx_cbfc_pause_frames4;
+	u64 tx_cbfc_pause_frames5;
+	u64 tx_cbfc_pause_frames6;
+	u64 tx_cbfc_pause_frames7;
+	u64 rx_cbfc_pause_frames0;
+	u64 rx_cbfc_pause_frames1;
+	u64 rx_cbfc_pause_frames2;
+	u64 rx_cbfc_pause_frames3;
+	u64 rx_cbfc_pause_frames4;
+	u64 rx_cbfc_pause_frames5;
+	u64 rx_cbfc_pause_frames6;
+	u64 rx_cbfc_pause_frames7;
+	u64 rx_nic_fifo_drop;
+};
+
+/* Firmware coredump internal register address/length pairs. */
+enum {
+	MPI_CORE_REGS_ADDR = 0x00030000,
+	MPI_CORE_REGS_CNT = 127,
+	MPI_CORE_SH_REGS_CNT = 16,
+	TEST_REGS_ADDR = 0x00001000,
+	TEST_REGS_CNT = 23,
+	RMII_REGS_ADDR = 0x00001040,
+	RMII_REGS_CNT = 64,
+	FCMAC1_REGS_ADDR = 0x00001080,
+	FCMAC2_REGS_ADDR = 0x000010c0,
+	FCMAC_REGS_CNT = 64,
+	FC1_MBX_REGS_ADDR = 0x00001100,
+	FC2_MBX_REGS_ADDR = 0x00001240,
+	FC_MBX_REGS_CNT = 64,
+	IDE_REGS_ADDR = 0x00001140,
+	IDE_REGS_CNT = 64,
+	NIC1_MBX_REGS_ADDR = 0x00001180,
+	NIC2_MBX_REGS_ADDR = 0x00001280,
+	NIC_MBX_REGS_CNT = 64,
+	SMBUS_REGS_ADDR = 0x00001200,
+	SMBUS_REGS_CNT = 64,
+	I2C_REGS_ADDR = 0x00001fc0,
+	I2C_REGS_CNT = 64,
+	MEMC_REGS_ADDR = 0x00003000,
+	MEMC_REGS_CNT = 256,
+	PBUS_REGS_ADDR = 0x00007c00,
+	PBUS_REGS_CNT = 256,
+	MDE_REGS_ADDR = 0x00010000,
+	MDE_REGS_CNT = 6,
+	CODE_RAM_ADDR = 0x00020000,
+	CODE_RAM_CNT = 0x2000,
+	MEMC_RAM_ADDR = 0x00100000,
+	MEMC_RAM_CNT = 0x2000,
+};
+
+#define MPI_COREDUMP_COOKIE 0x5555aaaa
+struct mpi_coredump_global_header {
+	u32	cookie;
+	u8	id_string[16];
+	u32	time_lo;
+	u32	time_hi;
+	u32	image_size;
+	u32	header_size;
+	u8	info[220];
+};
+
+struct mpi_coredump_segment_header {
+	u32	cookie;
+	u32	seg_num;
+	u32	seg_size;
+	u32	extra;
+	u8	description[16];
+};
+
+/* Firmware coredump header segment numbers. */
+enum {
+	CORE_SEG_NUM = 1,
+	TEST_LOGIC_SEG_NUM = 2,
+	RMII_SEG_NUM = 3,
+	FCMAC1_SEG_NUM = 4,
+	FCMAC2_SEG_NUM = 5,
+	FC1_MBOX_SEG_NUM = 6,
+	IDE_SEG_NUM = 7,
+	NIC1_MBOX_SEG_NUM = 8,
+	SMBUS_SEG_NUM = 9,
+	FC2_MBOX_SEG_NUM = 10,
+	NIC2_MBOX_SEG_NUM = 11,
+	I2C_SEG_NUM = 12,
+	MEMC_SEG_NUM = 13,
+	PBUS_SEG_NUM = 14,
+	MDE_SEG_NUM = 15,
+	NIC1_CONTROL_SEG_NUM = 16,
+	NIC2_CONTROL_SEG_NUM = 17,
+	NIC1_XGMAC_SEG_NUM = 18,
+	NIC2_XGMAC_SEG_NUM = 19,
+	WCS_RAM_SEG_NUM = 20,
+	MEMC_RAM_SEG_NUM = 21,
+	XAUI_AN_SEG_NUM = 22,
+	XAUI_HSS_PCS_SEG_NUM = 23,
+	XFI_AN_SEG_NUM = 24,
+	XFI_TRAIN_SEG_NUM = 25,
+	XFI_HSS_PCS_SEG_NUM = 26,
+	XFI_HSS_TX_SEG_NUM = 27,
+	XFI_HSS_RX_SEG_NUM = 28,
+	XFI_HSS_PLL_SEG_NUM = 29,
+	MISC_NIC_INFO_SEG_NUM = 30,
+	INTR_STATES_SEG_NUM = 31,
+	CAM_ENTRIES_SEG_NUM = 32,
+	ROUTING_WORDS_SEG_NUM = 33,
+	ETS_SEG_NUM = 34,
+	PROBE_DUMP_SEG_NUM = 35,
+	ROUTING_INDEX_SEG_NUM = 36,
+	MAC_PROTOCOL_SEG_NUM = 37,
+	XAUI2_AN_SEG_NUM = 38,
+	XAUI2_HSS_PCS_SEG_NUM = 39,
+	XFI2_AN_SEG_NUM = 40,
+	XFI2_TRAIN_SEG_NUM = 41,
+	XFI2_HSS_PCS_SEG_NUM = 42,
+	XFI2_HSS_TX_SEG_NUM = 43,
+	XFI2_HSS_RX_SEG_NUM = 44,
+	XFI2_HSS_PLL_SEG_NUM = 45,
+	SEM_REGS_SEG_NUM = 50
+
+};
+
+/* There are 64 generic NIC registers. */
+#define NIC_REGS_DUMP_WORD_COUNT		64
+/* XGMAC word count. */
+#define XGMAC_DUMP_WORD_COUNT		(XGMAC_REGISTER_END / 4)
+/* Word counts for the SERDES blocks. */
+#define XG_SERDES_XAUI_AN_COUNT		14
+#define XG_SERDES_XAUI_HSS_PCS_COUNT	33
+#define XG_SERDES_XFI_AN_COUNT		14
+#define XG_SERDES_XFI_TRAIN_COUNT		12
+#define XG_SERDES_XFI_HSS_PCS_COUNT	15
+#define XG_SERDES_XFI_HSS_TX_COUNT		32
+#define XG_SERDES_XFI_HSS_RX_COUNT		32
+#define XG_SERDES_XFI_HSS_PLL_COUNT	32
+
+/* There are 2 CNA ETS and 8 NIC ETS registers. */
+#define ETS_REGS_DUMP_WORD_COUNT		10
+
+/* Each probe mux entry stores the probe type plus 64 entries
+ * that are each 64-bits in length. There are a total of
+ * 34 (PRB_MX_ADDR_VALID_TOTAL) valid probes.
+ */
+#define PRB_MX_ADDR_PRB_WORD_COUNT		(1 + (PRB_MX_ADDR_MAX_MUX * 2))
+#define PRB_MX_DUMP_TOT_COUNT		(PRB_MX_ADDR_PRB_WORD_COUNT * \
+							PRB_MX_ADDR_VALID_TOTAL)
+/* Each routing entry consists of 4 32-bit words.
+ * They are route type, index, index word, and result.
+ * There are 2 route blocks with 8 entries each and
+ *  2 NIC blocks with 16 entries each.
+ * The totol entries is 48 with 4 words each.
+ */
+#define RT_IDX_DUMP_ENTRIES			48
+#define RT_IDX_DUMP_WORDS_PER_ENTRY	4
+#define RT_IDX_DUMP_TOT_WORDS		(RT_IDX_DUMP_ENTRIES * \
+						RT_IDX_DUMP_WORDS_PER_ENTRY)
+/* There are 10 address blocks in filter, each with
+ * different entry counts and different word-count-per-entry.
+ */
+#define MAC_ADDR_DUMP_ENTRIES \
+	((MAC_ADDR_MAX_CAM_ENTRIES * MAC_ADDR_MAX_CAM_WCOUNT) + \
+	(MAC_ADDR_MAX_MULTICAST_ENTRIES * MAC_ADDR_MAX_MULTICAST_WCOUNT) + \
+	(MAC_ADDR_MAX_VLAN_ENTRIES * MAC_ADDR_MAX_VLAN_WCOUNT) + \
+	(MAC_ADDR_MAX_MCAST_FLTR_ENTRIES * MAC_ADDR_MAX_MCAST_FLTR_WCOUNT) + \
+	(MAC_ADDR_MAX_FC_MAC_ENTRIES * MAC_ADDR_MAX_FC_MAC_WCOUNT) + \
+	(MAC_ADDR_MAX_MGMT_MAC_ENTRIES * MAC_ADDR_MAX_MGMT_MAC_WCOUNT) + \
+	(MAC_ADDR_MAX_MGMT_VLAN_ENTRIES * MAC_ADDR_MAX_MGMT_VLAN_WCOUNT) + \
+	(MAC_ADDR_MAX_MGMT_V4_ENTRIES * MAC_ADDR_MAX_MGMT_V4_WCOUNT) + \
+	(MAC_ADDR_MAX_MGMT_V6_ENTRIES * MAC_ADDR_MAX_MGMT_V6_WCOUNT) + \
+	(MAC_ADDR_MAX_MGMT_TU_DP_ENTRIES * MAC_ADDR_MAX_MGMT_TU_DP_WCOUNT))
+#define MAC_ADDR_DUMP_WORDS_PER_ENTRY	2
+#define MAC_ADDR_DUMP_TOT_WORDS		(MAC_ADDR_DUMP_ENTRIES * \
+						MAC_ADDR_DUMP_WORDS_PER_ENTRY)
+/* Maximum of 4 functions whose semaphore registeres are
+ * in the coredump.
+ */
+#define MAX_SEMAPHORE_FUNCTIONS		4
+/* Defines for access the MPI shadow registers. */
+#define RISC_124		0x0003007c
+#define RISC_127		0x0003007f
+#define SHADOW_OFFSET	0xb0000000
+#define SHADOW_REG_SHIFT	20
+
+struct qlge_nic_misc {
+	u32 rx_ring_count;
+	u32 tx_ring_count;
+	u32 intr_count;
+	u32 function;
+};
+
+struct qlge_reg_dump {
+	/* segment 0 */
+	struct mpi_coredump_global_header mpi_global_header;
+
+	/* segment 16 */
+	struct mpi_coredump_segment_header nic_regs_seg_hdr;
+	u32 nic_regs[64];
+
+	/* segment 30 */
+	struct mpi_coredump_segment_header misc_nic_seg_hdr;
+	struct qlge_nic_misc misc_nic_info;
+
+	/* segment 31 */
+	/* one interrupt state for each CQ */
+	struct mpi_coredump_segment_header intr_states_seg_hdr;
+	u32 intr_states[MAX_CPUS];
+
+	/* segment 32 */
+	/* 3 cam words each for 16 unicast,
+	 * 2 cam words for each of 32 multicast.
+	 */
+	struct mpi_coredump_segment_header cam_entries_seg_hdr;
+	u32 cam_entries[(16 * 3) + (32 * 3)];
+
+	/* segment 33 */
+	struct mpi_coredump_segment_header nic_routing_words_seg_hdr;
+	u32 nic_routing_words[16];
+
+	/* segment 34 */
+	struct mpi_coredump_segment_header ets_seg_hdr;
+	u32 ets[8 + 2];
+};
+
+struct qlge_mpi_coredump {
+	/* segment 0 */
+	struct mpi_coredump_global_header mpi_global_header;
+
+	/* segment 1 */
+	struct mpi_coredump_segment_header core_regs_seg_hdr;
+	u32 mpi_core_regs[MPI_CORE_REGS_CNT];
+	u32 mpi_core_sh_regs[MPI_CORE_SH_REGS_CNT];
+
+	/* segment 2 */
+	struct mpi_coredump_segment_header test_logic_regs_seg_hdr;
+	u32 test_logic_regs[TEST_REGS_CNT];
+
+	/* segment 3 */
+	struct mpi_coredump_segment_header rmii_regs_seg_hdr;
+	u32 rmii_regs[RMII_REGS_CNT];
+
+	/* segment 4 */
+	struct mpi_coredump_segment_header fcmac1_regs_seg_hdr;
+	u32 fcmac1_regs[FCMAC_REGS_CNT];
+
+	/* segment 5 */
+	struct mpi_coredump_segment_header fcmac2_regs_seg_hdr;
+	u32 fcmac2_regs[FCMAC_REGS_CNT];
+
+	/* segment 6 */
+	struct mpi_coredump_segment_header fc1_mbx_regs_seg_hdr;
+	u32 fc1_mbx_regs[FC_MBX_REGS_CNT];
+
+	/* segment 7 */
+	struct mpi_coredump_segment_header ide_regs_seg_hdr;
+	u32 ide_regs[IDE_REGS_CNT];
+
+	/* segment 8 */
+	struct mpi_coredump_segment_header nic1_mbx_regs_seg_hdr;
+	u32 nic1_mbx_regs[NIC_MBX_REGS_CNT];
+
+	/* segment 9 */
+	struct mpi_coredump_segment_header smbus_regs_seg_hdr;
+	u32 smbus_regs[SMBUS_REGS_CNT];
+
+	/* segment 10 */
+	struct mpi_coredump_segment_header fc2_mbx_regs_seg_hdr;
+	u32 fc2_mbx_regs[FC_MBX_REGS_CNT];
+
+	/* segment 11 */
+	struct mpi_coredump_segment_header nic2_mbx_regs_seg_hdr;
+	u32 nic2_mbx_regs[NIC_MBX_REGS_CNT];
+
+	/* segment 12 */
+	struct mpi_coredump_segment_header i2c_regs_seg_hdr;
+	u32 i2c_regs[I2C_REGS_CNT];
+	/* segment 13 */
+	struct mpi_coredump_segment_header memc_regs_seg_hdr;
+	u32 memc_regs[MEMC_REGS_CNT];
+
+	/* segment 14 */
+	struct mpi_coredump_segment_header pbus_regs_seg_hdr;
+	u32 pbus_regs[PBUS_REGS_CNT];
+
+	/* segment 15 */
+	struct mpi_coredump_segment_header mde_regs_seg_hdr;
+	u32 mde_regs[MDE_REGS_CNT];
+
+	/* segment 16 */
+	struct mpi_coredump_segment_header nic_regs_seg_hdr;
+	u32 nic_regs[NIC_REGS_DUMP_WORD_COUNT];
+
+	/* segment 17 */
+	struct mpi_coredump_segment_header nic2_regs_seg_hdr;
+	u32 nic2_regs[NIC_REGS_DUMP_WORD_COUNT];
+
+	/* segment 18 */
+	struct mpi_coredump_segment_header xgmac1_seg_hdr;
+	u32 xgmac1[XGMAC_DUMP_WORD_COUNT];
+
+	/* segment 19 */
+	struct mpi_coredump_segment_header xgmac2_seg_hdr;
+	u32 xgmac2[XGMAC_DUMP_WORD_COUNT];
+
+	/* segment 20 */
+	struct mpi_coredump_segment_header code_ram_seg_hdr;
+	u32 code_ram[CODE_RAM_CNT];
+
+	/* segment 21 */
+	struct mpi_coredump_segment_header memc_ram_seg_hdr;
+	u32 memc_ram[MEMC_RAM_CNT];
+
+	/* segment 22 */
+	struct mpi_coredump_segment_header xaui_an_hdr;
+	u32 serdes_xaui_an[XG_SERDES_XAUI_AN_COUNT];
+
+	/* segment 23 */
+	struct mpi_coredump_segment_header xaui_hss_pcs_hdr;
+	u32 serdes_xaui_hss_pcs[XG_SERDES_XAUI_HSS_PCS_COUNT];
+
+	/* segment 24 */
+	struct mpi_coredump_segment_header xfi_an_hdr;
+	u32 serdes_xfi_an[XG_SERDES_XFI_AN_COUNT];
+
+	/* segment 25 */
+	struct mpi_coredump_segment_header xfi_train_hdr;
+	u32 serdes_xfi_train[XG_SERDES_XFI_TRAIN_COUNT];
+
+	/* segment 26 */
+	struct mpi_coredump_segment_header xfi_hss_pcs_hdr;
+	u32 serdes_xfi_hss_pcs[XG_SERDES_XFI_HSS_PCS_COUNT];
+
+	/* segment 27 */
+	struct mpi_coredump_segment_header xfi_hss_tx_hdr;
+	u32 serdes_xfi_hss_tx[XG_SERDES_XFI_HSS_TX_COUNT];
+
+	/* segment 28 */
+	struct mpi_coredump_segment_header xfi_hss_rx_hdr;
+	u32 serdes_xfi_hss_rx[XG_SERDES_XFI_HSS_RX_COUNT];
+
+	/* segment 29 */
+	struct mpi_coredump_segment_header xfi_hss_pll_hdr;
+	u32 serdes_xfi_hss_pll[XG_SERDES_XFI_HSS_PLL_COUNT];
+
+	/* segment 30 */
+	struct mpi_coredump_segment_header misc_nic_seg_hdr;
+	struct qlge_nic_misc misc_nic_info;
+
+	/* segment 31 */
+	/* one interrupt state for each CQ */
+	struct mpi_coredump_segment_header intr_states_seg_hdr;
+	u32 intr_states[MAX_RX_RINGS];
+
+	/* segment 32 */
+	/* 3 cam words each for 16 unicast,
+	 * 2 cam words for each of 32 multicast.
+	 */
+	struct mpi_coredump_segment_header cam_entries_seg_hdr;
+	u32 cam_entries[(16 * 3) + (32 * 3)];
+
+	/* segment 33 */
+	struct mpi_coredump_segment_header nic_routing_words_seg_hdr;
+	u32 nic_routing_words[16];
+	/* segment 34 */
+	struct mpi_coredump_segment_header ets_seg_hdr;
+	u32 ets[ETS_REGS_DUMP_WORD_COUNT];
+
+	/* segment 35 */
+	struct mpi_coredump_segment_header probe_dump_seg_hdr;
+	u32 probe_dump[PRB_MX_DUMP_TOT_COUNT];
+
+	/* segment 36 */
+	struct mpi_coredump_segment_header routing_reg_seg_hdr;
+	u32 routing_regs[RT_IDX_DUMP_TOT_WORDS];
+
+	/* segment 37 */
+	struct mpi_coredump_segment_header mac_prot_reg_seg_hdr;
+	u32 mac_prot_regs[MAC_ADDR_DUMP_TOT_WORDS];
+
+	/* segment 38 */
+	struct mpi_coredump_segment_header xaui2_an_hdr;
+	u32 serdes2_xaui_an[XG_SERDES_XAUI_AN_COUNT];
+
+	/* segment 39 */
+	struct mpi_coredump_segment_header xaui2_hss_pcs_hdr;
+	u32 serdes2_xaui_hss_pcs[XG_SERDES_XAUI_HSS_PCS_COUNT];
+
+	/* segment 40 */
+	struct mpi_coredump_segment_header xfi2_an_hdr;
+	u32 serdes2_xfi_an[XG_SERDES_XFI_AN_COUNT];
+
+	/* segment 41 */
+	struct mpi_coredump_segment_header xfi2_train_hdr;
+	u32 serdes2_xfi_train[XG_SERDES_XFI_TRAIN_COUNT];
+
+	/* segment 42 */
+	struct mpi_coredump_segment_header xfi2_hss_pcs_hdr;
+	u32 serdes2_xfi_hss_pcs[XG_SERDES_XFI_HSS_PCS_COUNT];
+
+	/* segment 43 */
+	struct mpi_coredump_segment_header xfi2_hss_tx_hdr;
+	u32 serdes2_xfi_hss_tx[XG_SERDES_XFI_HSS_TX_COUNT];
+
+	/* segment 44 */
+	struct mpi_coredump_segment_header xfi2_hss_rx_hdr;
+	u32 serdes2_xfi_hss_rx[XG_SERDES_XFI_HSS_RX_COUNT];
+
+	/* segment 45 */
+	struct mpi_coredump_segment_header xfi2_hss_pll_hdr;
+	u32 serdes2_xfi_hss_pll[XG_SERDES_XFI_HSS_PLL_COUNT];
+
+	/* segment 50 */
+	/* semaphore register for all 5 functions */
+	struct mpi_coredump_segment_header sem_regs_seg_hdr;
+	u32 sem_regs[MAX_SEMAPHORE_FUNCTIONS];
+};
+
+/*
+ * intr_context structure is used during initialization
+ * to hook the interrupts.  It is also used in a single
+ * irq environment as a context to the ISR.
+ */
+struct intr_context {
+	struct qlge_adapter *qdev;
+	u32 intr;
+	u32 irq_mask;		/* Mask of which rings the vector services. */
+	u32 hooked;
+	u32 intr_en_mask;	/* value/mask used to enable this intr */
+	u32 intr_dis_mask;	/* value/mask used to disable this intr */
+	u32 intr_read_mask;	/* value/mask used to read this intr */
+	char name[IFNAMSIZ * 2];
+	irq_handler_t handler;
+};
+
+/* adapter flags definitions. */
+enum {
+	QL_ADAPTER_UP = 0,	/* Adapter has been brought up. */
+	QL_LEGACY_ENABLED = 1,
+	QL_MSI_ENABLED = 2,
+	QL_MSIX_ENABLED = 3,
+	QL_DMA64 = 4,
+	QL_PROMISCUOUS = 5,
+	QL_ALLMULTI = 6,
+	QL_PORT_CFG = 7,
+	QL_CAM_RT_SET = 8,
+	QL_SELFTEST = 9,
+	QL_LB_LINK_UP = 10,
+	QL_FRC_COREDUMP = 11,
+	QL_EEH_FATAL = 12,
+	QL_ASIC_RECOVERY = 14, /* We are in ascic recovery. */
+};
+
+/* link_status bit definitions */
+enum {
+	STS_LOOPBACK_MASK = 0x00000700,
+	STS_LOOPBACK_PCS = 0x00000100,
+	STS_LOOPBACK_HSS = 0x00000200,
+	STS_LOOPBACK_EXT = 0x00000300,
+	STS_PAUSE_MASK = 0x000000c0,
+	STS_PAUSE_STD = 0x00000040,
+	STS_PAUSE_PRI = 0x00000080,
+	STS_SPEED_MASK = 0x00000038,
+	STS_SPEED_100Mb = 0x00000000,
+	STS_SPEED_1Gb = 0x00000008,
+	STS_SPEED_10Gb = 0x00000010,
+	STS_LINK_TYPE_MASK = 0x00000007,
+	STS_LINK_TYPE_XFI = 0x00000001,
+	STS_LINK_TYPE_XAUI = 0x00000002,
+	STS_LINK_TYPE_XFI_BP = 0x00000003,
+	STS_LINK_TYPE_XAUI_BP = 0x00000004,
+	STS_LINK_TYPE_10GBASET = 0x00000005,
+};
+
+/* link_config bit definitions */
+enum {
+	CFG_JUMBO_FRAME_SIZE = 0x00010000,
+	CFG_PAUSE_MASK = 0x00000060,
+	CFG_PAUSE_STD = 0x00000020,
+	CFG_PAUSE_PRI = 0x00000040,
+	CFG_DCBX = 0x00000010,
+	CFG_LOOPBACK_MASK = 0x00000007,
+	CFG_LOOPBACK_PCS = 0x00000002,
+	CFG_LOOPBACK_HSS = 0x00000004,
+	CFG_LOOPBACK_EXT = 0x00000006,
+	CFG_DEFAULT_MAX_FRAME_SIZE = 0x00002580,
+};
+
+struct nic_operations {
+	int (*get_flash)(struct qlge_adapter *qdev);
+	int (*port_initialize)(struct qlge_adapter *qdev);
+};
+
+struct qlge_netdev_priv {
+	struct qlge_adapter *qdev;
+	struct net_device *ndev;
+};
+
+static inline
+struct qlge_adapter *netdev_to_qdev(struct net_device *ndev)
+{
+	struct qlge_netdev_priv *ndev_priv = netdev_priv(ndev);
+
+	return ndev_priv->qdev;
+}
+
+/*
+ * The main Adapter structure definition.
+ * This structure has all fields relevant to the hardware.
+ */
+struct qlge_adapter {
+	struct ricb ricb;
+	unsigned long flags;
+	u32 wol;
+
+	struct nic_stats nic_stats;
+
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+
+	/* PCI Configuration information for this device */
+	struct pci_dev *pdev;
+	struct net_device *ndev;	/* Parent NET device */
+
+	struct devlink_health_reporter *reporter;
+	/* Hardware information */
+	u32 chip_rev_id;
+	u32 fw_rev_id;
+	u32 func;		/* PCI function for this adapter */
+	u32 alt_func;		/* PCI function for alternate adapter */
+	u32 port;		/* Port number this adapter */
+
+	spinlock_t adapter_lock;
+	spinlock_t stats_lock;
+
+	/* PCI Bus Relative Register Addresses */
+	void __iomem *reg_base;
+	void __iomem *doorbell_area;
+	u32 doorbell_area_size;
+
+	u32 msg_enable;
+
+	/* Page for Shadow Registers */
+	void *rx_ring_shadow_reg_area;
+	dma_addr_t rx_ring_shadow_reg_dma;
+	void *tx_ring_shadow_reg_area;
+	dma_addr_t tx_ring_shadow_reg_dma;
+
+	u32 mailbox_in;
+	u32 mailbox_out;
+	struct mbox_params idc_mbc;
+	struct mutex	mpi_mutex;
+
+	int tx_ring_size;
+	int rx_ring_size;
+	u32 intr_count;
+	struct msix_entry *msi_x_entry;
+	struct intr_context intr_context[MAX_RX_RINGS];
+
+	int tx_ring_count;	/* One per online CPU. */
+	u32 rss_ring_count;	/* One per irq vector.  */
+	/*
+	 * rx_ring_count =
+	 *  (CPU count * outbound completion rx_ring) +
+	 *  (irq_vector_cnt * inbound (RSS) completion rx_ring)
+	 */
+	int rx_ring_count;
+	int ring_mem_size;
+	void *ring_mem;
+
+	struct rx_ring rx_ring[MAX_RX_RINGS];
+	struct tx_ring tx_ring[MAX_TX_RINGS];
+	unsigned int lbq_buf_order;
+	u32 lbq_buf_size;
+
+	int rx_csum;
+	u32 default_rx_queue;
+
+	u16 rx_coalesce_usecs;	/* cqicb->int_delay */
+	u16 rx_max_coalesced_frames;	/* cqicb->pkt_int_delay */
+	u16 tx_coalesce_usecs;	/* cqicb->int_delay */
+	u16 tx_max_coalesced_frames;	/* cqicb->pkt_int_delay */
+
+	u32 xg_sem_mask;
+	u32 port_link_up;
+	u32 port_init;
+	u32 link_status;
+	struct qlge_mpi_coredump *mpi_coredump;
+	u32 link_config;
+	u32 led_config;
+	u32 max_frame_size;
+
+	union flash_params flash;
+
+	struct workqueue_struct *workqueue;
+	struct delayed_work asic_reset_work;
+	struct delayed_work mpi_reset_work;
+	struct delayed_work mpi_work;
+	struct delayed_work mpi_port_cfg_work;
+	struct delayed_work mpi_idc_work;
+	struct completion ide_completion;
+	const struct nic_operations *nic_ops;
+	u16 device_id;
+	struct timer_list timer;
+	atomic_t lb_count;
+	/* Keep local copy of current mac address. */
+	char current_mac_addr[ETH_ALEN];
+};
+
+/*
+ * Typical Register accessor for memory mapped device.
+ */
+static inline u32 qlge_read32(const struct qlge_adapter *qdev, int reg)
+{
+	return readl(qdev->reg_base + reg);
+}
+
+/*
+ * Typical Register accessor for memory mapped device.
+ */
+static inline void qlge_write32(const struct qlge_adapter *qdev, int reg, u32 val)
+{
+	writel(val, qdev->reg_base + reg);
+}
+
+/*
+ * Doorbell Registers:
+ * Doorbell registers are virtual registers in the PCI memory space.
+ * The space is allocated by the chip during PCI initialization.  The
+ * device driver finds the doorbell address in BAR 3 in PCI config space.
+ * The registers are used to control outbound and inbound queues. For
+ * example, the producer index for an outbound queue.  Each queue uses
+ * 1 4k chunk of memory.  The lower half of the space is for outbound
+ * queues. The upper half is for inbound queues.
+ */
+static inline void qlge_write_db_reg(u32 val, void __iomem *addr)
+{
+	writel(val, addr);
+}
+
+/*
+ * Doorbell Registers:
+ * Doorbell registers are virtual registers in the PCI memory space.
+ * The space is allocated by the chip during PCI initialization.  The
+ * device driver finds the doorbell address in BAR 3 in PCI config space.
+ * The registers are used to control outbound and inbound queues. For
+ * example, the producer index for an outbound queue.  Each queue uses
+ * 1 4k chunk of memory.  The lower half of the space is for outbound
+ * queues. The upper half is for inbound queues.
+ * Caller has to guarantee ordering.
+ */
+static inline void qlge_write_db_reg_relaxed(u32 val, void __iomem *addr)
+{
+	writel_relaxed(val, addr);
+}
+
+/*
+ * Shadow Registers:
+ * Outbound queues have a consumer index that is maintained by the chip.
+ * Inbound queues have a producer index that is maintained by the chip.
+ * For lower overhead, these registers are "shadowed" to host memory
+ * which allows the device driver to track the queue progress without
+ * PCI reads. When an entry is placed on an inbound queue, the chip will
+ * update the relevant index register and then copy the value to the
+ * shadow register in host memory.
+ */
+static inline u32 qlge_read_sh_reg(__le32  *addr)
+{
+	u32 reg;
+
+	reg =  le32_to_cpu(*addr);
+	rmb();
+	return reg;
+}
+
+extern char qlge_driver_name[];
+extern const char qlge_driver_version[];
+extern const struct ethtool_ops qlge_ethtool_ops;
+
+int qlge_sem_spinlock(struct qlge_adapter *qdev, u32 sem_mask);
+void qlge_sem_unlock(struct qlge_adapter *qdev, u32 sem_mask);
+int qlge_read_xgmac_reg(struct qlge_adapter *qdev, u32 reg, u32 *data);
+int qlge_get_mac_addr_reg(struct qlge_adapter *qdev, u32 type, u16 index,
+			  u32 *value);
+int qlge_get_routing_reg(struct qlge_adapter *qdev, u32 index, u32 *value);
+int qlge_write_cfg(struct qlge_adapter *qdev, void *ptr, int size, u32 bit,
+		   u16 q_id);
+void qlge_queue_fw_error(struct qlge_adapter *qdev);
+void qlge_mpi_work(struct work_struct *work);
+void qlge_mpi_reset_work(struct work_struct *work);
+int qlge_wait_reg_rdy(struct qlge_adapter *qdev, u32 reg, u32 bit, u32 ebit);
+void qlge_queue_asic_error(struct qlge_adapter *qdev);
+void qlge_set_ethtool_ops(struct net_device *ndev);
+int qlge_read_xgmac_reg64(struct qlge_adapter *qdev, u32 reg, u64 *data);
+void qlge_mpi_idc_work(struct work_struct *work);
+void qlge_mpi_port_cfg_work(struct work_struct *work);
+int qlge_mb_get_fw_state(struct qlge_adapter *qdev);
+int qlge_cam_route_initialize(struct qlge_adapter *qdev);
+int qlge_read_mpi_reg(struct qlge_adapter *qdev, u32 reg, u32 *data);
+int qlge_write_mpi_reg(struct qlge_adapter *qdev, u32 reg, u32 data);
+int qlge_unpause_mpi_risc(struct qlge_adapter *qdev);
+int qlge_pause_mpi_risc(struct qlge_adapter *qdev);
+int qlge_hard_reset_mpi_risc(struct qlge_adapter *qdev);
+int qlge_soft_reset_mpi_risc(struct qlge_adapter *qdev);
+int qlge_dump_risc_ram_area(struct qlge_adapter *qdev, void *buf, u32 ram_addr,
+			    int word_count);
+int qlge_core_dump(struct qlge_adapter *qdev, struct qlge_mpi_coredump *mpi_coredump);
+int qlge_mb_about_fw(struct qlge_adapter *qdev);
+int qlge_mb_wol_set_magic(struct qlge_adapter *qdev, u32 enable_wol);
+int qlge_mb_wol_mode(struct qlge_adapter *qdev, u32 wol);
+int qlge_mb_set_led_cfg(struct qlge_adapter *qdev, u32 led_config);
+int qlge_mb_get_led_cfg(struct qlge_adapter *qdev);
+void qlge_link_on(struct qlge_adapter *qdev);
+void qlge_link_off(struct qlge_adapter *qdev);
+int qlge_mb_set_mgmnt_traffic_ctl(struct qlge_adapter *qdev, u32 control);
+int qlge_mb_get_port_cfg(struct qlge_adapter *qdev);
+int qlge_mb_set_port_cfg(struct qlge_adapter *qdev);
+int qlge_wait_fifo_empty(struct qlge_adapter *qdev);
+void qlge_get_dump(struct qlge_adapter *qdev, void *buff);
+netdev_tx_t qlge_lb_send(struct sk_buff *skb, struct net_device *ndev);
+void qlge_check_lb_frame(struct qlge_adapter *qdev, struct sk_buff *skb);
+int qlge_own_firmware(struct qlge_adapter *qdev);
+int qlge_clean_lb_rx_ring(struct rx_ring *rx_ring, int budget);
+
+#endif /* _QLGE_H_ */
diff --git a/drivers/staging/qlge/qlge_dbg.c b/drivers/staging/qlge/qlge_dbg.c
new file mode 100644
index 000000000..66d283583
--- /dev/null
+++ b/drivers/staging/qlge/qlge_dbg.c
@@ -0,0 +1,1314 @@
+// SPDX-License-Identifier: GPL-2.0
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/slab.h>
+
+#include "qlge.h"
+
+/* Read a NIC register from the alternate function. */
+static u32 qlge_read_other_func_reg(struct qlge_adapter *qdev,
+				    u32 reg)
+{
+	u32 register_to_read;
+	u32 reg_val;
+	unsigned int status = 0;
+
+	register_to_read = MPI_NIC_REG_BLOCK
+				| MPI_NIC_READ
+				| (qdev->alt_func << MPI_NIC_FUNCTION_SHIFT)
+				| reg;
+	status = qlge_read_mpi_reg(qdev, register_to_read, &reg_val);
+	if (status != 0)
+		return 0xffffffff;
+
+	return reg_val;
+}
+
+/* Write a NIC register from the alternate function. */
+static int qlge_write_other_func_reg(struct qlge_adapter *qdev,
+				     u32 reg, u32 reg_val)
+{
+	u32 register_to_read;
+
+	register_to_read = MPI_NIC_REG_BLOCK
+				| MPI_NIC_READ
+				| (qdev->alt_func << MPI_NIC_FUNCTION_SHIFT)
+				| reg;
+
+	return qlge_write_mpi_reg(qdev, register_to_read, reg_val);
+}
+
+static int qlge_wait_other_func_reg_rdy(struct qlge_adapter *qdev, u32 reg,
+					u32 bit, u32 err_bit)
+{
+	u32 temp;
+	int count;
+
+	for (count = 10; count; count--) {
+		temp = qlge_read_other_func_reg(qdev, reg);
+
+		/* check for errors */
+		if (temp & err_bit)
+			return -1;
+		else if (temp & bit)
+			return 0;
+		mdelay(10);
+	}
+	return -1;
+}
+
+static int qlge_read_other_func_serdes_reg(struct qlge_adapter *qdev, u32 reg,
+					   u32 *data)
+{
+	int status;
+
+	/* wait for reg to come ready */
+	status = qlge_wait_other_func_reg_rdy(qdev, XG_SERDES_ADDR / 4,
+					      XG_SERDES_ADDR_RDY, 0);
+	if (status)
+		goto exit;
+
+	/* set up for reg read */
+	qlge_write_other_func_reg(qdev, XG_SERDES_ADDR / 4, reg | PROC_ADDR_R);
+
+	/* wait for reg to come ready */
+	status = qlge_wait_other_func_reg_rdy(qdev, XG_SERDES_ADDR / 4,
+					      XG_SERDES_ADDR_RDY, 0);
+	if (status)
+		goto exit;
+
+	/* get the data */
+	*data = qlge_read_other_func_reg(qdev, (XG_SERDES_DATA / 4));
+exit:
+	return status;
+}
+
+/* Read out the SERDES registers */
+static int qlge_read_serdes_reg(struct qlge_adapter *qdev, u32 reg, u32 *data)
+{
+	int status;
+
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, XG_SERDES_ADDR, XG_SERDES_ADDR_RDY, 0);
+	if (status)
+		goto exit;
+
+	/* set up for reg read */
+	qlge_write32(qdev, XG_SERDES_ADDR, reg | PROC_ADDR_R);
+
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, XG_SERDES_ADDR, XG_SERDES_ADDR_RDY, 0);
+	if (status)
+		goto exit;
+
+	/* get the data */
+	*data = qlge_read32(qdev, XG_SERDES_DATA);
+exit:
+	return status;
+}
+
+static void qlge_get_both_serdes(struct qlge_adapter *qdev, u32 addr,
+				 u32 *direct_ptr, u32 *indirect_ptr,
+				 bool direct_valid, bool indirect_valid)
+{
+	unsigned int status;
+
+	status = 1;
+	if (direct_valid)
+		status = qlge_read_serdes_reg(qdev, addr, direct_ptr);
+	/* Dead fill any failures or invalids. */
+	if (status)
+		*direct_ptr = 0xDEADBEEF;
+
+	status = 1;
+	if (indirect_valid)
+		status = qlge_read_other_func_serdes_reg(qdev, addr,
+							 indirect_ptr);
+	/* Dead fill any failures or invalids. */
+	if (status)
+		*indirect_ptr = 0xDEADBEEF;
+}
+
+static int qlge_get_serdes_regs(struct qlge_adapter *qdev,
+				struct qlge_mpi_coredump *mpi_coredump)
+{
+	int status;
+	bool xfi_direct_valid = false, xfi_indirect_valid = false;
+	bool xaui_direct_valid = true, xaui_indirect_valid = true;
+	unsigned int i;
+	u32 *direct_ptr, temp;
+	u32 *indirect_ptr;
+
+	/* The XAUI needs to be read out per port */
+	status = qlge_read_other_func_serdes_reg(qdev,
+						 XG_SERDES_XAUI_HSS_PCS_START,
+						 &temp);
+	if (status)
+		temp = XG_SERDES_ADDR_XAUI_PWR_DOWN;
+
+	if ((temp & XG_SERDES_ADDR_XAUI_PWR_DOWN) ==
+				XG_SERDES_ADDR_XAUI_PWR_DOWN)
+		xaui_indirect_valid = false;
+
+	status = qlge_read_serdes_reg(qdev, XG_SERDES_XAUI_HSS_PCS_START, &temp);
+
+	if (status)
+		temp = XG_SERDES_ADDR_XAUI_PWR_DOWN;
+
+	if ((temp & XG_SERDES_ADDR_XAUI_PWR_DOWN) ==
+				XG_SERDES_ADDR_XAUI_PWR_DOWN)
+		xaui_direct_valid = false;
+
+	/*
+	 * XFI register is shared so only need to read one
+	 * functions and then check the bits.
+	 */
+	status = qlge_read_serdes_reg(qdev, XG_SERDES_ADDR_STS, &temp);
+	if (status)
+		temp = 0;
+
+	if ((temp & XG_SERDES_ADDR_XFI1_PWR_UP) ==
+					XG_SERDES_ADDR_XFI1_PWR_UP) {
+		/* now see if i'm NIC 1 or NIC 2 */
+		if (qdev->func & 1)
+			/* I'm NIC 2, so the indirect (NIC1) xfi is up. */
+			xfi_indirect_valid = true;
+		else
+			xfi_direct_valid = true;
+	}
+	if ((temp & XG_SERDES_ADDR_XFI2_PWR_UP) ==
+					XG_SERDES_ADDR_XFI2_PWR_UP) {
+		/* now see if i'm NIC 1 or NIC 2 */
+		if (qdev->func & 1)
+			/* I'm NIC 2, so the indirect (NIC1) xfi is up. */
+			xfi_direct_valid = true;
+		else
+			xfi_indirect_valid = true;
+	}
+
+	/* Get XAUI_AN register block. */
+	if (qdev->func & 1) {
+		/* Function 2 is direct	*/
+		direct_ptr = mpi_coredump->serdes2_xaui_an;
+		indirect_ptr = mpi_coredump->serdes_xaui_an;
+	} else {
+		/* Function 1 is direct	*/
+		direct_ptr = mpi_coredump->serdes_xaui_an;
+		indirect_ptr = mpi_coredump->serdes2_xaui_an;
+	}
+
+	for (i = 0; i <= 0x000000034; i += 4, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xaui_direct_valid, xaui_indirect_valid);
+
+	/* Get XAUI_HSS_PCS register block. */
+	if (qdev->func & 1) {
+		direct_ptr =
+			mpi_coredump->serdes2_xaui_hss_pcs;
+		indirect_ptr =
+			mpi_coredump->serdes_xaui_hss_pcs;
+	} else {
+		direct_ptr =
+			mpi_coredump->serdes_xaui_hss_pcs;
+		indirect_ptr =
+			mpi_coredump->serdes2_xaui_hss_pcs;
+	}
+
+	for (i = 0x800; i <= 0x880; i += 4, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xaui_direct_valid, xaui_indirect_valid);
+
+	/* Get XAUI_XFI_AN register block. */
+	if (qdev->func & 1) {
+		direct_ptr = mpi_coredump->serdes2_xfi_an;
+		indirect_ptr = mpi_coredump->serdes_xfi_an;
+	} else {
+		direct_ptr = mpi_coredump->serdes_xfi_an;
+		indirect_ptr = mpi_coredump->serdes2_xfi_an;
+	}
+
+	for (i = 0x1000; i <= 0x1034; i += 4, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+
+	/* Get XAUI_XFI_TRAIN register block. */
+	if (qdev->func & 1) {
+		direct_ptr = mpi_coredump->serdes2_xfi_train;
+		indirect_ptr =
+			mpi_coredump->serdes_xfi_train;
+	} else {
+		direct_ptr = mpi_coredump->serdes_xfi_train;
+		indirect_ptr =
+			mpi_coredump->serdes2_xfi_train;
+	}
+
+	for (i = 0x1050; i <= 0x107c; i += 4, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+
+	/* Get XAUI_XFI_HSS_PCS register block. */
+	if (qdev->func & 1) {
+		direct_ptr =
+			mpi_coredump->serdes2_xfi_hss_pcs;
+		indirect_ptr =
+			mpi_coredump->serdes_xfi_hss_pcs;
+	} else {
+		direct_ptr =
+			mpi_coredump->serdes_xfi_hss_pcs;
+		indirect_ptr =
+			mpi_coredump->serdes2_xfi_hss_pcs;
+	}
+
+	for (i = 0x1800; i <= 0x1838; i += 4, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+
+	/* Get XAUI_XFI_HSS_TX register block. */
+	if (qdev->func & 1) {
+		direct_ptr =
+			mpi_coredump->serdes2_xfi_hss_tx;
+		indirect_ptr =
+			mpi_coredump->serdes_xfi_hss_tx;
+	} else {
+		direct_ptr = mpi_coredump->serdes_xfi_hss_tx;
+		indirect_ptr =
+			mpi_coredump->serdes2_xfi_hss_tx;
+	}
+	for (i = 0x1c00; i <= 0x1c1f; i++, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+
+	/* Get XAUI_XFI_HSS_RX register block. */
+	if (qdev->func & 1) {
+		direct_ptr =
+			mpi_coredump->serdes2_xfi_hss_rx;
+		indirect_ptr =
+			mpi_coredump->serdes_xfi_hss_rx;
+	} else {
+		direct_ptr = mpi_coredump->serdes_xfi_hss_rx;
+		indirect_ptr =
+			mpi_coredump->serdes2_xfi_hss_rx;
+	}
+
+	for (i = 0x1c40; i <= 0x1c5f; i++, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+
+	/* Get XAUI_XFI_HSS_PLL register block. */
+	if (qdev->func & 1) {
+		direct_ptr =
+			mpi_coredump->serdes2_xfi_hss_pll;
+		indirect_ptr =
+			mpi_coredump->serdes_xfi_hss_pll;
+	} else {
+		direct_ptr =
+			mpi_coredump->serdes_xfi_hss_pll;
+		indirect_ptr =
+			mpi_coredump->serdes2_xfi_hss_pll;
+	}
+	for (i = 0x1e00; i <= 0x1e1f; i++, direct_ptr++, indirect_ptr++)
+		qlge_get_both_serdes(qdev, i, direct_ptr, indirect_ptr,
+				     xfi_direct_valid, xfi_indirect_valid);
+	return 0;
+}
+
+static int qlge_read_other_func_xgmac_reg(struct qlge_adapter *qdev, u32 reg,
+					  u32 *data)
+{
+	int status = 0;
+
+	/* wait for reg to come ready */
+	status = qlge_wait_other_func_reg_rdy(qdev, XGMAC_ADDR / 4,
+					      XGMAC_ADDR_RDY, XGMAC_ADDR_XME);
+	if (status)
+		goto exit;
+
+	/* set up for reg read */
+	qlge_write_other_func_reg(qdev, XGMAC_ADDR / 4, reg | XGMAC_ADDR_R);
+
+	/* wait for reg to come ready */
+	status = qlge_wait_other_func_reg_rdy(qdev, XGMAC_ADDR / 4,
+					      XGMAC_ADDR_RDY, XGMAC_ADDR_XME);
+	if (status)
+		goto exit;
+
+	/* get the data */
+	*data = qlge_read_other_func_reg(qdev, XGMAC_DATA / 4);
+exit:
+	return status;
+}
+
+/* Read the 400 xgmac control/statistics registers
+ * skipping unused locations.
+ */
+static int qlge_get_xgmac_regs(struct qlge_adapter *qdev, u32 *buf,
+			       unsigned int other_function)
+{
+	int status = 0;
+	int i;
+
+	for (i = PAUSE_SRC_LO; i < XGMAC_REGISTER_END; i += 4, buf++) {
+		/* We're reading 400 xgmac registers, but we filter out
+		 * several locations that are non-responsive to reads.
+		 */
+		if ((i == 0x00000114) ||
+		    (i == 0x00000118) ||
+			(i == 0x0000013c) ||
+			(i == 0x00000140) ||
+			(i > 0x00000150 && i < 0x000001fc) ||
+			(i > 0x00000278 && i < 0x000002a0) ||
+			(i > 0x000002c0 && i < 0x000002cf) ||
+			(i > 0x000002dc && i < 0x000002f0) ||
+			(i > 0x000003c8 && i < 0x00000400) ||
+			(i > 0x00000400 && i < 0x00000410) ||
+			(i > 0x00000410 && i < 0x00000420) ||
+			(i > 0x00000420 && i < 0x00000430) ||
+			(i > 0x00000430 && i < 0x00000440) ||
+			(i > 0x00000440 && i < 0x00000450) ||
+			(i > 0x00000450 && i < 0x00000500) ||
+			(i > 0x0000054c && i < 0x00000568) ||
+			(i > 0x000005c8 && i < 0x00000600)) {
+			if (other_function)
+				status =
+				qlge_read_other_func_xgmac_reg(qdev, i, buf);
+			else
+				status = qlge_read_xgmac_reg(qdev, i, buf);
+
+			if (status)
+				*buf = 0xdeadbeef;
+			break;
+		}
+	}
+	return status;
+}
+
+static int qlge_get_ets_regs(struct qlge_adapter *qdev, u32 *buf)
+{
+	int i;
+
+	for (i = 0; i < 8; i++, buf++) {
+		qlge_write32(qdev, NIC_ETS, i << 29 | 0x08000000);
+		*buf = qlge_read32(qdev, NIC_ETS);
+	}
+
+	for (i = 0; i < 2; i++, buf++) {
+		qlge_write32(qdev, CNA_ETS, i << 29 | 0x08000000);
+		*buf = qlge_read32(qdev, CNA_ETS);
+	}
+
+	return 0;
+}
+
+static void qlge_get_intr_states(struct qlge_adapter *qdev, u32 *buf)
+{
+	int i;
+
+	for (i = 0; i < qdev->rx_ring_count; i++, buf++) {
+		qlge_write32(qdev, INTR_EN,
+			     qdev->intr_context[i].intr_read_mask);
+		*buf = qlge_read32(qdev, INTR_EN);
+	}
+}
+
+static int qlge_get_cam_entries(struct qlge_adapter *qdev, u32 *buf)
+{
+	int i, status;
+	u32 value[3];
+
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return status;
+
+	for (i = 0; i < 16; i++) {
+		status = qlge_get_mac_addr_reg(qdev,
+					       MAC_ADDR_TYPE_CAM_MAC, i, value);
+		if (status) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Failed read of mac index register\n");
+			goto err;
+		}
+		*buf++ = value[0];	/* lower MAC address */
+		*buf++ = value[1];	/* upper MAC address */
+		*buf++ = value[2];	/* output */
+	}
+	for (i = 0; i < 32; i++) {
+		status = qlge_get_mac_addr_reg(qdev, MAC_ADDR_TYPE_MULTI_MAC,
+					       i, value);
+		if (status) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Failed read of mac index register\n");
+			goto err;
+		}
+		*buf++ = value[0];	/* lower Mcast address */
+		*buf++ = value[1];	/* upper Mcast address */
+	}
+err:
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+	return status;
+}
+
+static int qlge_get_routing_entries(struct qlge_adapter *qdev, u32 *buf)
+{
+	int status;
+	u32 value, i;
+
+	status = qlge_sem_spinlock(qdev, SEM_RT_IDX_MASK);
+	if (status)
+		return status;
+
+	for (i = 0; i < 16; i++) {
+		status = qlge_get_routing_reg(qdev, i, &value);
+		if (status) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Failed read of routing index register\n");
+			goto err;
+		} else {
+			*buf++ = value;
+		}
+	}
+err:
+	qlge_sem_unlock(qdev, SEM_RT_IDX_MASK);
+	return status;
+}
+
+/* Read the MPI Processor shadow registers */
+static int qlge_get_mpi_shadow_regs(struct qlge_adapter *qdev, u32 *buf)
+{
+	u32 i;
+	int status;
+
+	for (i = 0; i < MPI_CORE_SH_REGS_CNT; i++, buf++) {
+		status = qlge_write_mpi_reg(qdev,
+					    RISC_124,
+					    (SHADOW_OFFSET | i << SHADOW_REG_SHIFT));
+		if (status)
+			goto end;
+		status = qlge_read_mpi_reg(qdev, RISC_127, buf);
+		if (status)
+			goto end;
+	}
+end:
+	return status;
+}
+
+/* Read the MPI Processor core registers */
+static int qlge_get_mpi_regs(struct qlge_adapter *qdev, u32 *buf,
+			     u32 offset, u32 count)
+{
+	int i, status = 0;
+
+	for (i = 0; i < count; i++, buf++) {
+		status = qlge_read_mpi_reg(qdev, offset + i, buf);
+		if (status)
+			return status;
+	}
+	return status;
+}
+
+/* Read the ASIC probe dump */
+static unsigned int *qlge_get_probe(struct qlge_adapter *qdev, u32 clock,
+				    u32 valid, u32 *buf)
+{
+	u32 module, mux_sel, probe, lo_val, hi_val;
+
+	for (module = 0; module < PRB_MX_ADDR_MAX_MODS; module++) {
+		if (!((valid >> module) & 1))
+			continue;
+		for (mux_sel = 0; mux_sel < PRB_MX_ADDR_MAX_MUX; mux_sel++) {
+			probe = clock
+				| PRB_MX_ADDR_ARE
+				| mux_sel
+				| (module << PRB_MX_ADDR_MOD_SEL_SHIFT);
+			qlge_write32(qdev, PRB_MX_ADDR, probe);
+			lo_val = qlge_read32(qdev, PRB_MX_DATA);
+			if (mux_sel == 0) {
+				*buf = probe;
+				buf++;
+			}
+			probe |= PRB_MX_ADDR_UP;
+			qlge_write32(qdev, PRB_MX_ADDR, probe);
+			hi_val = qlge_read32(qdev, PRB_MX_DATA);
+			*buf = lo_val;
+			buf++;
+			*buf = hi_val;
+			buf++;
+		}
+	}
+	return buf;
+}
+
+static int qlge_get_probe_dump(struct qlge_adapter *qdev, unsigned int *buf)
+{
+	/* First we have to enable the probe mux */
+	qlge_write_mpi_reg(qdev, MPI_TEST_FUNC_PRB_CTL, MPI_TEST_FUNC_PRB_EN);
+	buf = qlge_get_probe(qdev, PRB_MX_ADDR_SYS_CLOCK,
+			     PRB_MX_ADDR_VALID_SYS_MOD, buf);
+	buf = qlge_get_probe(qdev, PRB_MX_ADDR_PCI_CLOCK,
+			     PRB_MX_ADDR_VALID_PCI_MOD, buf);
+	buf = qlge_get_probe(qdev, PRB_MX_ADDR_XGM_CLOCK,
+			     PRB_MX_ADDR_VALID_XGM_MOD, buf);
+	buf = qlge_get_probe(qdev, PRB_MX_ADDR_FC_CLOCK,
+			     PRB_MX_ADDR_VALID_FC_MOD, buf);
+	return 0;
+}
+
+/* Read out the routing index registers */
+static int qlge_get_routing_index_registers(struct qlge_adapter *qdev, u32 *buf)
+{
+	int status;
+	u32 type, index, index_max;
+	u32 result_index;
+	u32 result_data;
+	u32 val;
+
+	status = qlge_sem_spinlock(qdev, SEM_RT_IDX_MASK);
+	if (status)
+		return status;
+
+	for (type = 0; type < 4; type++) {
+		if (type < 2)
+			index_max = 8;
+		else
+			index_max = 16;
+		for (index = 0; index < index_max; index++) {
+			val = RT_IDX_RS
+				| (type << RT_IDX_TYPE_SHIFT)
+				| (index << RT_IDX_IDX_SHIFT);
+			qlge_write32(qdev, RT_IDX, val);
+			result_index = 0;
+			while ((result_index & RT_IDX_MR) == 0)
+				result_index = qlge_read32(qdev, RT_IDX);
+			result_data = qlge_read32(qdev, RT_DATA);
+			*buf = type;
+			buf++;
+			*buf = index;
+			buf++;
+			*buf = result_index;
+			buf++;
+			*buf = result_data;
+			buf++;
+		}
+	}
+	qlge_sem_unlock(qdev, SEM_RT_IDX_MASK);
+	return status;
+}
+
+/* Read out the MAC protocol registers */
+static void qlge_get_mac_protocol_registers(struct qlge_adapter *qdev, u32 *buf)
+{
+	u32 result_index, result_data;
+	u32 type;
+	u32 index;
+	u32 offset;
+	u32 val;
+	u32 initial_val = MAC_ADDR_RS;
+	u32 max_index;
+	u32 max_offset;
+
+	for (type = 0; type < MAC_ADDR_TYPE_COUNT; type++) {
+		switch (type) {
+		case 0: /* CAM */
+			initial_val |= MAC_ADDR_ADR;
+			max_index = MAC_ADDR_MAX_CAM_ENTRIES;
+			max_offset = MAC_ADDR_MAX_CAM_WCOUNT;
+			break;
+		case 1: /* Multicast MAC Address */
+			max_index = MAC_ADDR_MAX_CAM_WCOUNT;
+			max_offset = MAC_ADDR_MAX_CAM_WCOUNT;
+			break;
+		case 2: /* VLAN filter mask */
+		case 3: /* MC filter mask */
+			max_index = MAC_ADDR_MAX_CAM_WCOUNT;
+			max_offset = MAC_ADDR_MAX_CAM_WCOUNT;
+			break;
+		case 4: /* FC MAC addresses */
+			max_index = MAC_ADDR_MAX_FC_MAC_ENTRIES;
+			max_offset = MAC_ADDR_MAX_FC_MAC_WCOUNT;
+			break;
+		case 5: /* Mgmt MAC addresses */
+			max_index = MAC_ADDR_MAX_MGMT_MAC_ENTRIES;
+			max_offset = MAC_ADDR_MAX_MGMT_MAC_WCOUNT;
+			break;
+		case 6: /* Mgmt VLAN addresses */
+			max_index = MAC_ADDR_MAX_MGMT_VLAN_ENTRIES;
+			max_offset = MAC_ADDR_MAX_MGMT_VLAN_WCOUNT;
+			break;
+		case 7: /* Mgmt IPv4 address */
+			max_index = MAC_ADDR_MAX_MGMT_V4_ENTRIES;
+			max_offset = MAC_ADDR_MAX_MGMT_V4_WCOUNT;
+			break;
+		case 8: /* Mgmt IPv6 address */
+			max_index = MAC_ADDR_MAX_MGMT_V6_ENTRIES;
+			max_offset = MAC_ADDR_MAX_MGMT_V6_WCOUNT;
+			break;
+		case 9: /* Mgmt TCP/UDP Dest port */
+			max_index = MAC_ADDR_MAX_MGMT_TU_DP_ENTRIES;
+			max_offset = MAC_ADDR_MAX_MGMT_TU_DP_WCOUNT;
+			break;
+		default:
+			netdev_err(qdev->ndev, "Bad type!!! 0x%08x\n", type);
+			max_index = 0;
+			max_offset = 0;
+			break;
+		}
+		for (index = 0; index < max_index; index++) {
+			for (offset = 0; offset < max_offset; offset++) {
+				val = initial_val
+					| (type << MAC_ADDR_TYPE_SHIFT)
+					| (index << MAC_ADDR_IDX_SHIFT)
+					| (offset);
+				qlge_write32(qdev, MAC_ADDR_IDX, val);
+				result_index = 0;
+				while ((result_index & MAC_ADDR_MR) == 0) {
+					result_index = qlge_read32(qdev,
+								   MAC_ADDR_IDX);
+				}
+				result_data = qlge_read32(qdev, MAC_ADDR_DATA);
+				*buf = result_index;
+				buf++;
+				*buf = result_data;
+				buf++;
+			}
+		}
+	}
+}
+
+static void qlge_get_sem_registers(struct qlge_adapter *qdev, u32 *buf)
+{
+	u32 func_num, reg, reg_val;
+	int status;
+
+	for (func_num = 0; func_num < MAX_SEMAPHORE_FUNCTIONS ; func_num++) {
+		reg = MPI_NIC_REG_BLOCK
+			| (func_num << MPI_NIC_FUNCTION_SHIFT)
+			| (SEM / 4);
+		status = qlge_read_mpi_reg(qdev, reg, &reg_val);
+		*buf = reg_val;
+		/* if the read failed then dead fill the element. */
+		if (!status)
+			*buf = 0xdeadbeef;
+		buf++;
+	}
+}
+
+/* Create a coredump segment header */
+static void qlge_build_coredump_seg_header(struct mpi_coredump_segment_header *seg_hdr,
+					   u32 seg_number, u32 seg_size, u8 *desc)
+{
+	memset(seg_hdr, 0, sizeof(struct mpi_coredump_segment_header));
+	seg_hdr->cookie = MPI_COREDUMP_COOKIE;
+	seg_hdr->seg_num = seg_number;
+	seg_hdr->seg_size = seg_size;
+	strncpy(seg_hdr->description, desc, (sizeof(seg_hdr->description)) - 1);
+}
+
+/*
+ * This function should be called when a coredump / probedump
+ * is to be extracted from the HBA. It is assumed there is a
+ * qdev structure that contains the base address of the register
+ * space for this function as well as a coredump structure that
+ * will contain the dump.
+ */
+int qlge_core_dump(struct qlge_adapter *qdev, struct qlge_mpi_coredump *mpi_coredump)
+{
+	int status;
+	int i;
+
+	if (!mpi_coredump) {
+		netif_err(qdev, drv, qdev->ndev, "No memory allocated\n");
+		return -EINVAL;
+	}
+
+	/* Try to get the spinlock, but dont worry if
+	 * it isn't available.  If the firmware died it
+	 * might be holding the sem.
+	 */
+	qlge_sem_spinlock(qdev, SEM_PROC_REG_MASK);
+
+	status = qlge_pause_mpi_risc(qdev);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed RISC pause. Status = 0x%.08x\n", status);
+		goto err;
+	}
+
+	/* Insert the global header */
+	memset(&mpi_coredump->mpi_global_header, 0,
+	       sizeof(struct mpi_coredump_global_header));
+	mpi_coredump->mpi_global_header.cookie = MPI_COREDUMP_COOKIE;
+	mpi_coredump->mpi_global_header.header_size =
+		sizeof(struct mpi_coredump_global_header);
+	mpi_coredump->mpi_global_header.image_size =
+		sizeof(struct qlge_mpi_coredump);
+	strncpy(mpi_coredump->mpi_global_header.id_string, "MPI Coredump",
+		sizeof(mpi_coredump->mpi_global_header.id_string));
+
+	/* Get generic NIC reg dump */
+	qlge_build_coredump_seg_header(&mpi_coredump->nic_regs_seg_hdr,
+				       NIC1_CONTROL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->nic_regs), "NIC1 Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->nic2_regs_seg_hdr,
+				       NIC2_CONTROL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->nic2_regs), "NIC2 Registers");
+
+	/* Get XGMac registers. (Segment 18, Rev C. step 21) */
+	qlge_build_coredump_seg_header(&mpi_coredump->xgmac1_seg_hdr,
+				       NIC1_XGMAC_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->xgmac1), "NIC1 XGMac Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xgmac2_seg_hdr,
+				       NIC2_XGMAC_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->xgmac2), "NIC2 XGMac Registers");
+
+	if (qdev->func & 1) {
+		/* Odd means our function is NIC 2 */
+		for (i = 0; i < NIC_REGS_DUMP_WORD_COUNT; i++)
+			mpi_coredump->nic2_regs[i] =
+				qlge_read32(qdev, i * sizeof(u32));
+
+		for (i = 0; i < NIC_REGS_DUMP_WORD_COUNT; i++)
+			mpi_coredump->nic_regs[i] =
+				qlge_read_other_func_reg(qdev, (i * sizeof(u32)) / 4);
+
+		qlge_get_xgmac_regs(qdev, &mpi_coredump->xgmac2[0], 0);
+		qlge_get_xgmac_regs(qdev, &mpi_coredump->xgmac1[0], 1);
+	} else {
+		/* Even means our function is NIC 1 */
+		for (i = 0; i < NIC_REGS_DUMP_WORD_COUNT; i++)
+			mpi_coredump->nic_regs[i] =
+				qlge_read32(qdev, i * sizeof(u32));
+		for (i = 0; i < NIC_REGS_DUMP_WORD_COUNT; i++)
+			mpi_coredump->nic2_regs[i] =
+				qlge_read_other_func_reg(qdev, (i * sizeof(u32)) / 4);
+
+		qlge_get_xgmac_regs(qdev, &mpi_coredump->xgmac1[0], 0);
+		qlge_get_xgmac_regs(qdev, &mpi_coredump->xgmac2[0], 1);
+	}
+
+	/* Rev C. Step 20a */
+	qlge_build_coredump_seg_header(&mpi_coredump->xaui_an_hdr,
+				       XAUI_AN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xaui_an),
+				       "XAUI AN Registers");
+
+	/* Rev C. Step 20b */
+	qlge_build_coredump_seg_header(&mpi_coredump->xaui_hss_pcs_hdr,
+				       XAUI_HSS_PCS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xaui_hss_pcs),
+				       "XAUI HSS PCS Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_an_hdr, XFI_AN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_an),
+				       "XFI AN Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_train_hdr,
+				       XFI_TRAIN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_train),
+				       "XFI TRAIN Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_hss_pcs_hdr,
+				       XFI_HSS_PCS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_hss_pcs),
+				       "XFI HSS PCS Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_hss_tx_hdr,
+				       XFI_HSS_TX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_hss_tx),
+				       "XFI HSS TX Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_hss_rx_hdr,
+				       XFI_HSS_RX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_hss_rx),
+				       "XFI HSS RX Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi_hss_pll_hdr,
+				       XFI_HSS_PLL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes_xfi_hss_pll),
+				       "XFI HSS PLL Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xaui2_an_hdr,
+				       XAUI2_AN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xaui_an),
+				       "XAUI2 AN Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xaui2_hss_pcs_hdr,
+				       XAUI2_HSS_PCS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xaui_hss_pcs),
+				       "XAUI2 HSS PCS Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_an_hdr,
+				       XFI2_AN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_an),
+				       "XFI2 AN Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_train_hdr,
+				       XFI2_TRAIN_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_train),
+				       "XFI2 TRAIN Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_hss_pcs_hdr,
+				       XFI2_HSS_PCS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_hss_pcs),
+				       "XFI2 HSS PCS Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_hss_tx_hdr,
+				       XFI2_HSS_TX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_hss_tx),
+				       "XFI2 HSS TX Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_hss_rx_hdr,
+				       XFI2_HSS_RX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_hss_rx),
+				       "XFI2 HSS RX Registers");
+
+	qlge_build_coredump_seg_header(&mpi_coredump->xfi2_hss_pll_hdr,
+				       XFI2_HSS_PLL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->serdes2_xfi_hss_pll),
+				       "XFI2 HSS PLL Registers");
+
+	status = qlge_get_serdes_regs(qdev, mpi_coredump);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Dump of Serdes Registers. Status = 0x%.08x\n",
+			  status);
+		goto err;
+	}
+
+	qlge_build_coredump_seg_header(&mpi_coredump->core_regs_seg_hdr,
+				       CORE_SEG_NUM,
+				       sizeof(mpi_coredump->core_regs_seg_hdr) +
+				       sizeof(mpi_coredump->mpi_core_regs) +
+				       sizeof(mpi_coredump->mpi_core_sh_regs),
+				       "Core Registers");
+
+	/* Get the MPI Core Registers */
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->mpi_core_regs[0],
+				   MPI_CORE_REGS_ADDR, MPI_CORE_REGS_CNT);
+	if (status)
+		goto err;
+	/* Get the 16 MPI shadow registers */
+	status = qlge_get_mpi_shadow_regs(qdev,
+					  &mpi_coredump->mpi_core_sh_regs[0]);
+	if (status)
+		goto err;
+
+	/* Get the Test Logic Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->test_logic_regs_seg_hdr,
+				       TEST_LOGIC_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->test_logic_regs),
+				       "Test Logic Regs");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->test_logic_regs[0],
+				   TEST_REGS_ADDR, TEST_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the RMII Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->rmii_regs_seg_hdr,
+				       RMII_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->rmii_regs),
+				       "RMII Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->rmii_regs[0],
+				   RMII_REGS_ADDR, RMII_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the FCMAC1 Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->fcmac1_regs_seg_hdr,
+				       FCMAC1_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->fcmac1_regs),
+				       "FCMAC1 Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->fcmac1_regs[0],
+				   FCMAC1_REGS_ADDR, FCMAC_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the FCMAC2 Registers */
+
+	qlge_build_coredump_seg_header(&mpi_coredump->fcmac2_regs_seg_hdr,
+				       FCMAC2_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->fcmac2_regs),
+				       "FCMAC2 Registers");
+
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->fcmac2_regs[0],
+				   FCMAC2_REGS_ADDR, FCMAC_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the FC1 MBX Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->fc1_mbx_regs_seg_hdr,
+				       FC1_MBOX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->fc1_mbx_regs),
+				       "FC1 MBox Regs");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->fc1_mbx_regs[0],
+				   FC1_MBX_REGS_ADDR, FC_MBX_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the IDE Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->ide_regs_seg_hdr,
+				       IDE_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->ide_regs),
+				       "IDE Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->ide_regs[0],
+				   IDE_REGS_ADDR, IDE_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the NIC1 MBX Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->nic1_mbx_regs_seg_hdr,
+				       NIC1_MBOX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->nic1_mbx_regs),
+				       "NIC1 MBox Regs");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->nic1_mbx_regs[0],
+				   NIC1_MBX_REGS_ADDR, NIC_MBX_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the SMBus Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->smbus_regs_seg_hdr,
+				       SMBUS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->smbus_regs),
+				       "SMBus Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->smbus_regs[0],
+				   SMBUS_REGS_ADDR, SMBUS_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the FC2 MBX Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->fc2_mbx_regs_seg_hdr,
+				       FC2_MBOX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->fc2_mbx_regs),
+				       "FC2 MBox Regs");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->fc2_mbx_regs[0],
+				   FC2_MBX_REGS_ADDR, FC_MBX_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the NIC2 MBX Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->nic2_mbx_regs_seg_hdr,
+				       NIC2_MBOX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->nic2_mbx_regs),
+				       "NIC2 MBox Regs");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->nic2_mbx_regs[0],
+				   NIC2_MBX_REGS_ADDR, NIC_MBX_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the I2C Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->i2c_regs_seg_hdr,
+				       I2C_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->i2c_regs),
+				       "I2C Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->i2c_regs[0],
+				   I2C_REGS_ADDR, I2C_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the MEMC Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->memc_regs_seg_hdr,
+				       MEMC_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->memc_regs),
+				       "MEMC Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->memc_regs[0],
+				   MEMC_REGS_ADDR, MEMC_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the PBus Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->pbus_regs_seg_hdr,
+				       PBUS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->pbus_regs),
+				       "PBUS Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->pbus_regs[0],
+				   PBUS_REGS_ADDR, PBUS_REGS_CNT);
+	if (status)
+		goto err;
+
+	/* Get the MDE Registers */
+	qlge_build_coredump_seg_header(&mpi_coredump->mde_regs_seg_hdr,
+				       MDE_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->mde_regs),
+				       "MDE Registers");
+	status = qlge_get_mpi_regs(qdev, &mpi_coredump->mde_regs[0],
+				   MDE_REGS_ADDR, MDE_REGS_CNT);
+	if (status)
+		goto err;
+
+	qlge_build_coredump_seg_header(&mpi_coredump->misc_nic_seg_hdr,
+				       MISC_NIC_INFO_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->misc_nic_info),
+				       "MISC NIC INFO");
+	mpi_coredump->misc_nic_info.rx_ring_count = qdev->rx_ring_count;
+	mpi_coredump->misc_nic_info.tx_ring_count = qdev->tx_ring_count;
+	mpi_coredump->misc_nic_info.intr_count = qdev->intr_count;
+	mpi_coredump->misc_nic_info.function = qdev->func;
+
+	/* Segment 31 */
+	/* Get indexed register values. */
+	qlge_build_coredump_seg_header(&mpi_coredump->intr_states_seg_hdr,
+				       INTR_STATES_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->intr_states),
+				       "INTR States");
+	qlge_get_intr_states(qdev, &mpi_coredump->intr_states[0]);
+
+	qlge_build_coredump_seg_header(&mpi_coredump->cam_entries_seg_hdr,
+				       CAM_ENTRIES_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->cam_entries),
+				       "CAM Entries");
+	status = qlge_get_cam_entries(qdev, &mpi_coredump->cam_entries[0]);
+	if (status)
+		goto err;
+
+	qlge_build_coredump_seg_header(&mpi_coredump->nic_routing_words_seg_hdr,
+				       ROUTING_WORDS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->nic_routing_words),
+				       "Routing Words");
+	status = qlge_get_routing_entries(qdev,
+					  &mpi_coredump->nic_routing_words[0]);
+	if (status)
+		goto err;
+
+	/* Segment 34 (Rev C. step 23) */
+	qlge_build_coredump_seg_header(&mpi_coredump->ets_seg_hdr,
+				       ETS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->ets),
+				       "ETS Registers");
+	status = qlge_get_ets_regs(qdev, &mpi_coredump->ets[0]);
+	if (status)
+		goto err;
+
+	qlge_build_coredump_seg_header(&mpi_coredump->probe_dump_seg_hdr,
+				       PROBE_DUMP_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->probe_dump),
+				       "Probe Dump");
+	qlge_get_probe_dump(qdev, &mpi_coredump->probe_dump[0]);
+
+	qlge_build_coredump_seg_header(&mpi_coredump->routing_reg_seg_hdr,
+				       ROUTING_INDEX_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->routing_regs),
+				       "Routing Regs");
+	status = qlge_get_routing_index_registers(qdev,
+						  &mpi_coredump->routing_regs[0]);
+	if (status)
+		goto err;
+
+	qlge_build_coredump_seg_header(&mpi_coredump->mac_prot_reg_seg_hdr,
+				       MAC_PROTOCOL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->mac_prot_regs),
+				       "MAC Prot Regs");
+	qlge_get_mac_protocol_registers(qdev, &mpi_coredump->mac_prot_regs[0]);
+
+	/* Get the semaphore registers for all 5 functions */
+	qlge_build_coredump_seg_header(&mpi_coredump->sem_regs_seg_hdr,
+				       SEM_REGS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header) +
+				       sizeof(mpi_coredump->sem_regs),	"Sem Registers");
+
+	qlge_get_sem_registers(qdev, &mpi_coredump->sem_regs[0]);
+
+	/* Prevent the mpi restarting while we dump the memory.*/
+	qlge_write_mpi_reg(qdev, MPI_TEST_FUNC_RST_STS, MPI_TEST_FUNC_RST_FRC);
+
+	/* clear the pause */
+	status = qlge_unpause_mpi_risc(qdev);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed RISC unpause. Status = 0x%.08x\n", status);
+		goto err;
+	}
+
+	/* Reset the RISC so we can dump RAM */
+	status = qlge_hard_reset_mpi_risc(qdev);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed RISC reset. Status = 0x%.08x\n", status);
+		goto err;
+	}
+
+	qlge_build_coredump_seg_header(&mpi_coredump->code_ram_seg_hdr,
+				       WCS_RAM_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->code_ram),
+				       "WCS RAM");
+	status = qlge_dump_risc_ram_area(qdev, &mpi_coredump->code_ram[0],
+					 CODE_RAM_ADDR, CODE_RAM_CNT);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Dump of CODE RAM. Status = 0x%.08x\n",
+			  status);
+		goto err;
+	}
+
+	/* Insert the segment header */
+	qlge_build_coredump_seg_header(&mpi_coredump->memc_ram_seg_hdr,
+				       MEMC_RAM_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->memc_ram),
+				       "MEMC RAM");
+	status = qlge_dump_risc_ram_area(qdev, &mpi_coredump->memc_ram[0],
+					 MEMC_RAM_ADDR, MEMC_RAM_CNT);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Dump of MEMC RAM. Status = 0x%.08x\n",
+			  status);
+		goto err;
+	}
+err:
+	qlge_sem_unlock(qdev, SEM_PROC_REG_MASK); /* does flush too */
+	return status;
+}
+
+static void qlge_get_core_dump(struct qlge_adapter *qdev)
+{
+	if (!qlge_own_firmware(qdev)) {
+		netif_err(qdev, drv, qdev->ndev, "Don't own firmware!\n");
+		return;
+	}
+
+	if (!netif_running(qdev->ndev)) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Force Coredump can only be done from interface that is up\n");
+		return;
+	}
+	qlge_queue_fw_error(qdev);
+}
+
+static void qlge_gen_reg_dump(struct qlge_adapter *qdev,
+			      struct qlge_reg_dump *mpi_coredump)
+{
+	int i, status;
+
+	memset(&mpi_coredump->mpi_global_header, 0,
+	       sizeof(struct mpi_coredump_global_header));
+	mpi_coredump->mpi_global_header.cookie = MPI_COREDUMP_COOKIE;
+	mpi_coredump->mpi_global_header.header_size =
+		sizeof(struct mpi_coredump_global_header);
+	mpi_coredump->mpi_global_header.image_size =
+		sizeof(struct qlge_reg_dump);
+	strncpy(mpi_coredump->mpi_global_header.id_string, "MPI Coredump",
+		sizeof(mpi_coredump->mpi_global_header.id_string));
+
+	/* segment 16 */
+	qlge_build_coredump_seg_header(&mpi_coredump->misc_nic_seg_hdr,
+				       MISC_NIC_INFO_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->misc_nic_info),
+				       "MISC NIC INFO");
+	mpi_coredump->misc_nic_info.rx_ring_count = qdev->rx_ring_count;
+	mpi_coredump->misc_nic_info.tx_ring_count = qdev->tx_ring_count;
+	mpi_coredump->misc_nic_info.intr_count = qdev->intr_count;
+	mpi_coredump->misc_nic_info.function = qdev->func;
+
+	/* Segment 16, Rev C. Step 18 */
+	qlge_build_coredump_seg_header(&mpi_coredump->nic_regs_seg_hdr,
+				       NIC1_CONTROL_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->nic_regs),
+				       "NIC Registers");
+	/* Get generic reg dump */
+	for (i = 0; i < 64; i++)
+		mpi_coredump->nic_regs[i] = qlge_read32(qdev, i * sizeof(u32));
+
+	/* Segment 31 */
+	/* Get indexed register values. */
+	qlge_build_coredump_seg_header(&mpi_coredump->intr_states_seg_hdr,
+				       INTR_STATES_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->intr_states),
+				       "INTR States");
+	qlge_get_intr_states(qdev, &mpi_coredump->intr_states[0]);
+
+	qlge_build_coredump_seg_header(&mpi_coredump->cam_entries_seg_hdr,
+				       CAM_ENTRIES_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->cam_entries),
+				       "CAM Entries");
+	status = qlge_get_cam_entries(qdev, &mpi_coredump->cam_entries[0]);
+	if (status)
+		return;
+
+	qlge_build_coredump_seg_header(&mpi_coredump->nic_routing_words_seg_hdr,
+				       ROUTING_WORDS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->nic_routing_words),
+				       "Routing Words");
+	status = qlge_get_routing_entries(qdev,
+					  &mpi_coredump->nic_routing_words[0]);
+	if (status)
+		return;
+
+	/* Segment 34 (Rev C. step 23) */
+	qlge_build_coredump_seg_header(&mpi_coredump->ets_seg_hdr,
+				       ETS_SEG_NUM,
+				       sizeof(struct mpi_coredump_segment_header)
+				       + sizeof(mpi_coredump->ets),
+				       "ETS Registers");
+	status = qlge_get_ets_regs(qdev, &mpi_coredump->ets[0]);
+	if (status)
+		return;
+}
+
+void qlge_get_dump(struct qlge_adapter *qdev, void *buff)
+{
+	/*
+	 * If the dump has already been taken and is stored
+	 * in our internal buffer and if force dump is set then
+	 * just start the spool to dump it to the log file
+	 * and also, take a snapshot of the general regs
+	 * to the user's buffer or else take complete dump
+	 * to the user's buffer if force is not set.
+	 */
+
+	if (!test_bit(QL_FRC_COREDUMP, &qdev->flags)) {
+		if (!qlge_core_dump(qdev, buff))
+			qlge_soft_reset_mpi_risc(qdev);
+		else
+			netif_err(qdev, drv, qdev->ndev, "coredump failed!\n");
+	} else {
+		qlge_gen_reg_dump(qdev, buff);
+		qlge_get_core_dump(qdev);
+	}
+}
diff --git a/drivers/staging/qlge/qlge_devlink.c b/drivers/staging/qlge/qlge_devlink.c
new file mode 100644
index 000000000..0ab02d6d3
--- /dev/null
+++ b/drivers/staging/qlge/qlge_devlink.c
@@ -0,0 +1,167 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+#include "qlge.h"
+#include "qlge_devlink.h"
+
+static int qlge_fill_seg_(struct devlink_fmsg *fmsg,
+			  struct mpi_coredump_segment_header *seg_header,
+			  u32 *reg_data)
+{
+	int regs_num = (seg_header->seg_size
+			- sizeof(struct mpi_coredump_segment_header)) / sizeof(u32);
+	int err;
+	int i;
+
+	err = devlink_fmsg_pair_nest_start(fmsg, seg_header->description);
+	if (err)
+		return err;
+	err = devlink_fmsg_obj_nest_start(fmsg);
+	if (err)
+		return err;
+	err = devlink_fmsg_u32_pair_put(fmsg, "segment", seg_header->seg_num);
+	if (err)
+		return err;
+	err = devlink_fmsg_arr_pair_nest_start(fmsg, "values");
+	if (err)
+		return err;
+	for (i = 0; i < regs_num; i++) {
+		err = devlink_fmsg_u32_put(fmsg, *reg_data);
+		if (err)
+			return err;
+		reg_data++;
+	}
+	err = devlink_fmsg_obj_nest_end(fmsg);
+	if (err)
+		return err;
+	err = devlink_fmsg_arr_pair_nest_end(fmsg);
+	if (err)
+		return err;
+	err = devlink_fmsg_pair_nest_end(fmsg);
+	return err;
+}
+
+#define FILL_SEG(seg_hdr, seg_regs)			                    \
+	do {                                                                \
+		err = qlge_fill_seg_(fmsg, &dump->seg_hdr, dump->seg_regs); \
+		if (err) {					            \
+			kvfree(dump);                                       \
+			return err;				            \
+		}                                                           \
+	} while (0)
+
+static int qlge_reporter_coredump(struct devlink_health_reporter *reporter,
+				  struct devlink_fmsg *fmsg, void *priv_ctx,
+				  struct netlink_ext_ack *extack)
+{
+	int err = 0;
+
+	struct qlge_adapter *qdev = devlink_health_reporter_priv(reporter);
+	struct qlge_mpi_coredump *dump;
+	wait_queue_head_t wait;
+
+	if (!netif_running(qdev->ndev))
+		return 0;
+
+	if (test_bit(QL_FRC_COREDUMP, &qdev->flags)) {
+		if (qlge_own_firmware(qdev)) {
+			qlge_queue_fw_error(qdev);
+			init_waitqueue_head(&wait);
+			wait_event_timeout(wait, 0, 5 * HZ);
+		} else {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Force Coredump failed because this NIC function doesn't own the firmware\n");
+			return -EPERM;
+		}
+	}
+
+	dump = kvmalloc(sizeof(*dump), GFP_KERNEL);
+	if (!dump)
+		return -ENOMEM;
+
+	err = qlge_core_dump(qdev, dump);
+	if (err) {
+		kvfree(dump);
+		return err;
+	}
+
+	qlge_soft_reset_mpi_risc(qdev);
+
+	FILL_SEG(core_regs_seg_hdr, mpi_core_regs);
+	FILL_SEG(test_logic_regs_seg_hdr, test_logic_regs);
+	FILL_SEG(rmii_regs_seg_hdr, rmii_regs);
+	FILL_SEG(fcmac1_regs_seg_hdr, fcmac1_regs);
+	FILL_SEG(fcmac2_regs_seg_hdr, fcmac2_regs);
+	FILL_SEG(fc1_mbx_regs_seg_hdr, fc1_mbx_regs);
+	FILL_SEG(ide_regs_seg_hdr, ide_regs);
+	FILL_SEG(nic1_mbx_regs_seg_hdr, nic1_mbx_regs);
+	FILL_SEG(smbus_regs_seg_hdr, smbus_regs);
+	FILL_SEG(fc2_mbx_regs_seg_hdr, fc2_mbx_regs);
+	FILL_SEG(nic2_mbx_regs_seg_hdr, nic2_mbx_regs);
+	FILL_SEG(i2c_regs_seg_hdr, i2c_regs);
+	FILL_SEG(memc_regs_seg_hdr, memc_regs);
+	FILL_SEG(pbus_regs_seg_hdr, pbus_regs);
+	FILL_SEG(mde_regs_seg_hdr, mde_regs);
+	FILL_SEG(nic_regs_seg_hdr, nic_regs);
+	FILL_SEG(nic2_regs_seg_hdr, nic2_regs);
+	FILL_SEG(xgmac1_seg_hdr, xgmac1);
+	FILL_SEG(xgmac2_seg_hdr, xgmac2);
+	FILL_SEG(code_ram_seg_hdr, code_ram);
+	FILL_SEG(memc_ram_seg_hdr, memc_ram);
+	FILL_SEG(xaui_an_hdr, serdes_xaui_an);
+	FILL_SEG(xaui_hss_pcs_hdr, serdes_xaui_hss_pcs);
+	FILL_SEG(xfi_an_hdr, serdes_xfi_an);
+	FILL_SEG(xfi_train_hdr, serdes_xfi_train);
+	FILL_SEG(xfi_hss_pcs_hdr, serdes_xfi_hss_pcs);
+	FILL_SEG(xfi_hss_tx_hdr, serdes_xfi_hss_tx);
+	FILL_SEG(xfi_hss_rx_hdr, serdes_xfi_hss_rx);
+	FILL_SEG(xfi_hss_pll_hdr, serdes_xfi_hss_pll);
+
+	err = qlge_fill_seg_(fmsg, &dump->misc_nic_seg_hdr,
+			     (u32 *)&dump->misc_nic_info);
+	if (err) {
+		kvfree(dump);
+		return err;
+	}
+
+	FILL_SEG(intr_states_seg_hdr, intr_states);
+	FILL_SEG(cam_entries_seg_hdr, cam_entries);
+	FILL_SEG(nic_routing_words_seg_hdr, nic_routing_words);
+	FILL_SEG(ets_seg_hdr, ets);
+	FILL_SEG(probe_dump_seg_hdr, probe_dump);
+	FILL_SEG(routing_reg_seg_hdr, routing_regs);
+	FILL_SEG(mac_prot_reg_seg_hdr, mac_prot_regs);
+	FILL_SEG(xaui2_an_hdr, serdes2_xaui_an);
+	FILL_SEG(xaui2_hss_pcs_hdr, serdes2_xaui_hss_pcs);
+	FILL_SEG(xfi2_an_hdr, serdes2_xfi_an);
+	FILL_SEG(xfi2_train_hdr, serdes2_xfi_train);
+	FILL_SEG(xfi2_hss_pcs_hdr, serdes2_xfi_hss_pcs);
+	FILL_SEG(xfi2_hss_tx_hdr, serdes2_xfi_hss_tx);
+	FILL_SEG(xfi2_hss_rx_hdr, serdes2_xfi_hss_rx);
+	FILL_SEG(xfi2_hss_pll_hdr, serdes2_xfi_hss_pll);
+	FILL_SEG(sem_regs_seg_hdr, sem_regs);
+
+	kvfree(dump);
+	return err;
+}
+
+static const struct devlink_health_reporter_ops qlge_reporter_ops = {
+	.name = "coredump",
+	.dump = qlge_reporter_coredump,
+};
+
+long qlge_health_create_reporters(struct qlge_adapter *priv)
+{
+	struct devlink *devlink;
+	long err = 0;
+
+	devlink = priv_to_devlink(priv);
+	priv->reporter =
+		devlink_health_reporter_create(devlink, &qlge_reporter_ops,
+					       0, priv);
+	if (IS_ERR(priv->reporter)) {
+		err = PTR_ERR(priv->reporter);
+		netdev_warn(priv->ndev,
+			    "Failed to create reporter, err = %ld\n",
+			    err);
+	}
+	return err;
+}
diff --git a/drivers/staging/qlge/qlge_devlink.h b/drivers/staging/qlge/qlge_devlink.h
new file mode 100644
index 000000000..94538e923
--- /dev/null
+++ b/drivers/staging/qlge/qlge_devlink.h
@@ -0,0 +1,9 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+#ifndef QLGE_DEVLINK_H
+#define QLGE_DEVLINK_H
+
+#include <net/devlink.h>
+
+long qlge_health_create_reporters(struct qlge_adapter *priv);
+
+#endif /* QLGE_DEVLINK_H */
diff --git a/drivers/staging/qlge/qlge_ethtool.c b/drivers/staging/qlge/qlge_ethtool.c
new file mode 100644
index 000000000..12efcd105
--- /dev/null
+++ b/drivers/staging/qlge/qlge_ethtool.c
@@ -0,0 +1,746 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/pagemap.h>
+#include <linux/sched.h>
+#include <linux/dmapool.h>
+#include <linux/mempool.h>
+#include <linux/spinlock.h>
+#include <linux/kthread.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/ioport.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <net/ipv6.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/skbuff.h>
+#include <linux/rtnetlink.h>
+#include <linux/if_vlan.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+
+#include "qlge.h"
+
+struct qlge_stats {
+	char stat_string[ETH_GSTRING_LEN];
+	int sizeof_stat;
+	int stat_offset;
+};
+
+#define QL_SIZEOF(m) sizeof_field(struct qlge_adapter, m)
+#define QL_OFF(m) offsetof(struct qlge_adapter, m)
+
+static const struct qlge_stats qlge_gstrings_stats[] = {
+	{"tx_pkts", QL_SIZEOF(nic_stats.tx_pkts), QL_OFF(nic_stats.tx_pkts)},
+	{"tx_bytes", QL_SIZEOF(nic_stats.tx_bytes), QL_OFF(nic_stats.tx_bytes)},
+	{"tx_mcast_pkts", QL_SIZEOF(nic_stats.tx_mcast_pkts),
+					QL_OFF(nic_stats.tx_mcast_pkts)},
+	{"tx_bcast_pkts", QL_SIZEOF(nic_stats.tx_bcast_pkts),
+					QL_OFF(nic_stats.tx_bcast_pkts)},
+	{"tx_ucast_pkts", QL_SIZEOF(nic_stats.tx_ucast_pkts),
+					QL_OFF(nic_stats.tx_ucast_pkts)},
+	{"tx_ctl_pkts", QL_SIZEOF(nic_stats.tx_ctl_pkts),
+					QL_OFF(nic_stats.tx_ctl_pkts)},
+	{"tx_pause_pkts", QL_SIZEOF(nic_stats.tx_pause_pkts),
+					QL_OFF(nic_stats.tx_pause_pkts)},
+	{"tx_64_pkts", QL_SIZEOF(nic_stats.tx_64_pkt),
+					QL_OFF(nic_stats.tx_64_pkt)},
+	{"tx_65_to_127_pkts", QL_SIZEOF(nic_stats.tx_65_to_127_pkt),
+					QL_OFF(nic_stats.tx_65_to_127_pkt)},
+	{"tx_128_to_255_pkts", QL_SIZEOF(nic_stats.tx_128_to_255_pkt),
+					QL_OFF(nic_stats.tx_128_to_255_pkt)},
+	{"tx_256_511_pkts", QL_SIZEOF(nic_stats.tx_256_511_pkt),
+					QL_OFF(nic_stats.tx_256_511_pkt)},
+	{"tx_512_to_1023_pkts",	QL_SIZEOF(nic_stats.tx_512_to_1023_pkt),
+					QL_OFF(nic_stats.tx_512_to_1023_pkt)},
+	{"tx_1024_to_1518_pkts", QL_SIZEOF(nic_stats.tx_1024_to_1518_pkt),
+					QL_OFF(nic_stats.tx_1024_to_1518_pkt)},
+	{"tx_1519_to_max_pkts",	QL_SIZEOF(nic_stats.tx_1519_to_max_pkt),
+					QL_OFF(nic_stats.tx_1519_to_max_pkt)},
+	{"tx_undersize_pkts", QL_SIZEOF(nic_stats.tx_undersize_pkt),
+					QL_OFF(nic_stats.tx_undersize_pkt)},
+	{"tx_oversize_pkts", QL_SIZEOF(nic_stats.tx_oversize_pkt),
+					QL_OFF(nic_stats.tx_oversize_pkt)},
+	{"rx_bytes", QL_SIZEOF(nic_stats.rx_bytes), QL_OFF(nic_stats.rx_bytes)},
+	{"rx_bytes_ok",	QL_SIZEOF(nic_stats.rx_bytes_ok),
+					QL_OFF(nic_stats.rx_bytes_ok)},
+	{"rx_pkts", QL_SIZEOF(nic_stats.rx_pkts), QL_OFF(nic_stats.rx_pkts)},
+	{"rx_pkts_ok", QL_SIZEOF(nic_stats.rx_pkts_ok),
+					QL_OFF(nic_stats.rx_pkts_ok)},
+	{"rx_bcast_pkts", QL_SIZEOF(nic_stats.rx_bcast_pkts),
+					QL_OFF(nic_stats.rx_bcast_pkts)},
+	{"rx_mcast_pkts", QL_SIZEOF(nic_stats.rx_mcast_pkts),
+					QL_OFF(nic_stats.rx_mcast_pkts)},
+	{"rx_ucast_pkts", QL_SIZEOF(nic_stats.rx_ucast_pkts),
+					QL_OFF(nic_stats.rx_ucast_pkts)},
+	{"rx_undersize_pkts", QL_SIZEOF(nic_stats.rx_undersize_pkts),
+					QL_OFF(nic_stats.rx_undersize_pkts)},
+	{"rx_oversize_pkts", QL_SIZEOF(nic_stats.rx_oversize_pkts),
+					QL_OFF(nic_stats.rx_oversize_pkts)},
+	{"rx_jabber_pkts", QL_SIZEOF(nic_stats.rx_jabber_pkts),
+					QL_OFF(nic_stats.rx_jabber_pkts)},
+	{"rx_undersize_fcerr_pkts",
+		QL_SIZEOF(nic_stats.rx_undersize_fcerr_pkts),
+				QL_OFF(nic_stats.rx_undersize_fcerr_pkts)},
+	{"rx_drop_events", QL_SIZEOF(nic_stats.rx_drop_events),
+					QL_OFF(nic_stats.rx_drop_events)},
+	{"rx_fcerr_pkts", QL_SIZEOF(nic_stats.rx_fcerr_pkts),
+					QL_OFF(nic_stats.rx_fcerr_pkts)},
+	{"rx_align_err", QL_SIZEOF(nic_stats.rx_align_err),
+					QL_OFF(nic_stats.rx_align_err)},
+	{"rx_symbol_err", QL_SIZEOF(nic_stats.rx_symbol_err),
+					QL_OFF(nic_stats.rx_symbol_err)},
+	{"rx_mac_err", QL_SIZEOF(nic_stats.rx_mac_err),
+					QL_OFF(nic_stats.rx_mac_err)},
+	{"rx_ctl_pkts",	QL_SIZEOF(nic_stats.rx_ctl_pkts),
+					QL_OFF(nic_stats.rx_ctl_pkts)},
+	{"rx_pause_pkts", QL_SIZEOF(nic_stats.rx_pause_pkts),
+					QL_OFF(nic_stats.rx_pause_pkts)},
+	{"rx_64_pkts", QL_SIZEOF(nic_stats.rx_64_pkts),
+					QL_OFF(nic_stats.rx_64_pkts)},
+	{"rx_65_to_127_pkts", QL_SIZEOF(nic_stats.rx_65_to_127_pkts),
+					QL_OFF(nic_stats.rx_65_to_127_pkts)},
+	{"rx_128_255_pkts", QL_SIZEOF(nic_stats.rx_128_255_pkts),
+					QL_OFF(nic_stats.rx_128_255_pkts)},
+	{"rx_256_511_pkts", QL_SIZEOF(nic_stats.rx_256_511_pkts),
+					QL_OFF(nic_stats.rx_256_511_pkts)},
+	{"rx_512_to_1023_pkts",	QL_SIZEOF(nic_stats.rx_512_to_1023_pkts),
+					QL_OFF(nic_stats.rx_512_to_1023_pkts)},
+	{"rx_1024_to_1518_pkts", QL_SIZEOF(nic_stats.rx_1024_to_1518_pkts),
+					QL_OFF(nic_stats.rx_1024_to_1518_pkts)},
+	{"rx_1519_to_max_pkts",	QL_SIZEOF(nic_stats.rx_1519_to_max_pkts),
+					QL_OFF(nic_stats.rx_1519_to_max_pkts)},
+	{"rx_len_err_pkts", QL_SIZEOF(nic_stats.rx_len_err_pkts),
+					QL_OFF(nic_stats.rx_len_err_pkts)},
+	{"rx_code_err",	QL_SIZEOF(nic_stats.rx_code_err),
+					QL_OFF(nic_stats.rx_code_err)},
+	{"rx_oversize_err", QL_SIZEOF(nic_stats.rx_oversize_err),
+					QL_OFF(nic_stats.rx_oversize_err)},
+	{"rx_undersize_err", QL_SIZEOF(nic_stats.rx_undersize_err),
+					QL_OFF(nic_stats.rx_undersize_err)},
+	{"rx_preamble_err", QL_SIZEOF(nic_stats.rx_preamble_err),
+					QL_OFF(nic_stats.rx_preamble_err)},
+	{"rx_frame_len_err", QL_SIZEOF(nic_stats.rx_frame_len_err),
+					QL_OFF(nic_stats.rx_frame_len_err)},
+	{"rx_crc_err", QL_SIZEOF(nic_stats.rx_crc_err),
+					QL_OFF(nic_stats.rx_crc_err)},
+	{"rx_err_count", QL_SIZEOF(nic_stats.rx_err_count),
+					QL_OFF(nic_stats.rx_err_count)},
+	{"tx_cbfc_pause_frames0", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames0),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames0)},
+	{"tx_cbfc_pause_frames1", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames1),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames1)},
+	{"tx_cbfc_pause_frames2", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames2),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames2)},
+	{"tx_cbfc_pause_frames3", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames3),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames3)},
+	{"tx_cbfc_pause_frames4", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames4),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames4)},
+	{"tx_cbfc_pause_frames5", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames5),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames5)},
+	{"tx_cbfc_pause_frames6", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames6),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames6)},
+	{"tx_cbfc_pause_frames7", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames7),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames7)},
+	{"rx_cbfc_pause_frames0", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames0),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames0)},
+	{"rx_cbfc_pause_frames1", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames1),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames1)},
+	{"rx_cbfc_pause_frames2", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames2),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames2)},
+	{"rx_cbfc_pause_frames3", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames3),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames3)},
+	{"rx_cbfc_pause_frames4", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames4),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames4)},
+	{"rx_cbfc_pause_frames5", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames5),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames5)},
+	{"rx_cbfc_pause_frames6", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames6),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames6)},
+	{"rx_cbfc_pause_frames7", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames7),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames7)},
+	{"rx_nic_fifo_drop", QL_SIZEOF(nic_stats.rx_nic_fifo_drop),
+					QL_OFF(nic_stats.rx_nic_fifo_drop)},
+};
+
+static const char qlge_gstrings_test[][ETH_GSTRING_LEN] = {
+	"Loopback test  (offline)"
+};
+
+#define QLGE_TEST_LEN (sizeof(qlge_gstrings_test) / ETH_GSTRING_LEN)
+#define QLGE_STATS_LEN ARRAY_SIZE(qlge_gstrings_stats)
+#define QLGE_RCV_MAC_ERR_STATS	7
+
+static int qlge_update_ring_coalescing(struct qlge_adapter *qdev)
+{
+	int i, status = 0;
+	struct rx_ring *rx_ring;
+	struct cqicb *cqicb;
+
+	if (!netif_running(qdev->ndev))
+		return status;
+
+	/* Skip the default queue, and update the outbound handler
+	 * queues if they changed.
+	 */
+	cqicb = (struct cqicb *)&qdev->rx_ring[qdev->rss_ring_count];
+	if (le16_to_cpu(cqicb->irq_delay) != qdev->tx_coalesce_usecs ||
+	    le16_to_cpu(cqicb->pkt_delay) != qdev->tx_max_coalesced_frames) {
+		for (i = qdev->rss_ring_count; i < qdev->rx_ring_count; i++) {
+			rx_ring = &qdev->rx_ring[i];
+			cqicb = (struct cqicb *)rx_ring;
+			cqicb->irq_delay = cpu_to_le16(qdev->tx_coalesce_usecs);
+			cqicb->pkt_delay =
+				cpu_to_le16(qdev->tx_max_coalesced_frames);
+			cqicb->flags = FLAGS_LI;
+			status = qlge_write_cfg(qdev, cqicb, sizeof(*cqicb),
+						CFG_LCQ, rx_ring->cq_id);
+			if (status) {
+				netif_err(qdev, ifup, qdev->ndev,
+					  "Failed to load CQICB.\n");
+				goto exit;
+			}
+		}
+	}
+
+	/* Update the inbound (RSS) handler queues if they changed. */
+	cqicb = (struct cqicb *)&qdev->rx_ring[0];
+	if (le16_to_cpu(cqicb->irq_delay) != qdev->rx_coalesce_usecs ||
+	    le16_to_cpu(cqicb->pkt_delay) != qdev->rx_max_coalesced_frames) {
+		for (i = 0; i < qdev->rss_ring_count; i++, rx_ring++) {
+			rx_ring = &qdev->rx_ring[i];
+			cqicb = (struct cqicb *)rx_ring;
+			cqicb->irq_delay = cpu_to_le16(qdev->rx_coalesce_usecs);
+			cqicb->pkt_delay =
+				cpu_to_le16(qdev->rx_max_coalesced_frames);
+			cqicb->flags = FLAGS_LI;
+			status = qlge_write_cfg(qdev, cqicb, sizeof(*cqicb),
+						CFG_LCQ, rx_ring->cq_id);
+			if (status) {
+				netif_err(qdev, ifup, qdev->ndev,
+					  "Failed to load CQICB.\n");
+				goto exit;
+			}
+		}
+	}
+exit:
+	return status;
+}
+
+static void qlge_update_stats(struct qlge_adapter *qdev)
+{
+	u32 i;
+	u64 data;
+	u64 *iter = &qdev->nic_stats.tx_pkts;
+
+	spin_lock(&qdev->stats_lock);
+	if (qlge_sem_spinlock(qdev, qdev->xg_sem_mask)) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Couldn't get xgmac sem.\n");
+		goto quit;
+	}
+	/*
+	 * Get TX statistics.
+	 */
+	for (i = 0x200; i < 0x280; i += 8) {
+		if (qlge_read_xgmac_reg64(qdev, i, &data)) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Error reading status register 0x%.04x.\n",
+				  i);
+			goto end;
+		} else {
+			*iter = data;
+		}
+		iter++;
+	}
+
+	/*
+	 * Get RX statistics.
+	 */
+	for (i = 0x300; i < 0x3d0; i += 8) {
+		if (qlge_read_xgmac_reg64(qdev, i, &data)) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Error reading status register 0x%.04x.\n",
+				  i);
+			goto end;
+		} else {
+			*iter = data;
+		}
+		iter++;
+	}
+
+	/* Update receive mac error statistics */
+	iter += QLGE_RCV_MAC_ERR_STATS;
+
+	/*
+	 * Get Per-priority TX pause frame counter statistics.
+	 */
+	for (i = 0x500; i < 0x540; i += 8) {
+		if (qlge_read_xgmac_reg64(qdev, i, &data)) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Error reading status register 0x%.04x.\n",
+				  i);
+			goto end;
+		} else {
+			*iter = data;
+		}
+		iter++;
+	}
+
+	/*
+	 * Get Per-priority RX pause frame counter statistics.
+	 */
+	for (i = 0x568; i < 0x5a8; i += 8) {
+		if (qlge_read_xgmac_reg64(qdev, i, &data)) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "Error reading status register 0x%.04x.\n",
+				  i);
+			goto end;
+		} else {
+			*iter = data;
+		}
+		iter++;
+	}
+
+	/*
+	 * Get RX NIC FIFO DROP statistics.
+	 */
+	if (qlge_read_xgmac_reg64(qdev, 0x5b8, &data)) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Error reading status register 0x%.04x.\n", i);
+		goto end;
+	} else {
+		*iter = data;
+	}
+end:
+	qlge_sem_unlock(qdev, qdev->xg_sem_mask);
+quit:
+	spin_unlock(&qdev->stats_lock);
+}
+
+static void qlge_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
+{
+	int index;
+
+	switch (stringset) {
+	case ETH_SS_TEST:
+		memcpy(buf, *qlge_gstrings_test, QLGE_TEST_LEN * ETH_GSTRING_LEN);
+		break;
+	case ETH_SS_STATS:
+		for (index = 0; index < QLGE_STATS_LEN; index++) {
+			memcpy(buf + index * ETH_GSTRING_LEN,
+			       qlge_gstrings_stats[index].stat_string,
+			       ETH_GSTRING_LEN);
+		}
+		break;
+	}
+}
+
+static int qlge_get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_TEST:
+		return QLGE_TEST_LEN;
+	case ETH_SS_STATS:
+		return QLGE_STATS_LEN;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void
+qlge_get_ethtool_stats(struct net_device *ndev,
+		       struct ethtool_stats *stats, u64 *data)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int index, length;
+
+	length = QLGE_STATS_LEN;
+	qlge_update_stats(qdev);
+
+	for (index = 0; index < length; index++) {
+		char *p = (char *)qdev +
+			qlge_gstrings_stats[index].stat_offset;
+		*data++ = (qlge_gstrings_stats[index].sizeof_stat ==
+			   sizeof(u64)) ? *(u64 *)p : (*(u32 *)p);
+	}
+}
+
+static int qlge_get_link_ksettings(struct net_device *ndev,
+				   struct ethtool_link_ksettings *ecmd)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	u32 supported, advertising;
+
+	supported = SUPPORTED_10000baseT_Full;
+	advertising = ADVERTISED_10000baseT_Full;
+
+	if ((qdev->link_status & STS_LINK_TYPE_MASK) ==
+	    STS_LINK_TYPE_10GBASET) {
+		supported |= (SUPPORTED_TP | SUPPORTED_Autoneg);
+		advertising |= (ADVERTISED_TP | ADVERTISED_Autoneg);
+		ecmd->base.port = PORT_TP;
+		ecmd->base.autoneg = AUTONEG_ENABLE;
+	} else {
+		supported |= SUPPORTED_FIBRE;
+		advertising |= ADVERTISED_FIBRE;
+		ecmd->base.port = PORT_FIBRE;
+	}
+
+	ecmd->base.speed = SPEED_10000;
+	ecmd->base.duplex = DUPLEX_FULL;
+
+	ethtool_convert_legacy_u32_to_link_mode(ecmd->link_modes.supported,
+						supported);
+	ethtool_convert_legacy_u32_to_link_mode(ecmd->link_modes.advertising,
+						advertising);
+
+	return 0;
+}
+
+static void qlge_get_drvinfo(struct net_device *ndev,
+			     struct ethtool_drvinfo *drvinfo)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	strscpy(drvinfo->driver, qlge_driver_name, sizeof(drvinfo->driver));
+	strscpy(drvinfo->version, qlge_driver_version,
+		sizeof(drvinfo->version));
+	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+		 "v%d.%d.%d",
+		 (qdev->fw_rev_id & 0x00ff0000) >> 16,
+		 (qdev->fw_rev_id & 0x0000ff00) >> 8,
+		 (qdev->fw_rev_id & 0x000000ff));
+	strscpy(drvinfo->bus_info, pci_name(qdev->pdev),
+		sizeof(drvinfo->bus_info));
+}
+
+static void qlge_get_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	unsigned short ssys_dev = qdev->pdev->subsystem_device;
+
+	/* WOL is only supported for mezz card. */
+	if (ssys_dev == QLGE_MEZZ_SSYS_ID_068 ||
+	    ssys_dev == QLGE_MEZZ_SSYS_ID_180) {
+		wol->supported = WAKE_MAGIC;
+		wol->wolopts = qdev->wol;
+	}
+}
+
+static int qlge_set_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	unsigned short ssys_dev = qdev->pdev->subsystem_device;
+
+	/* WOL is only supported for mezz card. */
+	if (ssys_dev != QLGE_MEZZ_SSYS_ID_068 &&
+	    ssys_dev != QLGE_MEZZ_SSYS_ID_180) {
+		netif_info(qdev, drv, qdev->ndev,
+			   "WOL is only supported for mezz card\n");
+		return -EOPNOTSUPP;
+	}
+	if (wol->wolopts & ~WAKE_MAGIC)
+		return -EINVAL;
+	qdev->wol = wol->wolopts;
+
+	netif_info(qdev, drv, qdev->ndev, "Set wol option 0x%x\n", qdev->wol);
+	return 0;
+}
+
+static int qlge_set_phys_id(struct net_device *ndev,
+			    enum ethtool_phys_id_state state)
+
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		/* Save the current LED settings */
+		if (qlge_mb_get_led_cfg(qdev))
+			return -EIO;
+
+		/* Start blinking */
+		qlge_mb_set_led_cfg(qdev, QL_LED_BLINK);
+		return 0;
+
+	case ETHTOOL_ID_INACTIVE:
+		/* Restore LED settings */
+		if (qlge_mb_set_led_cfg(qdev, qdev->led_config))
+			return -EIO;
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int qlge_start_loopback(struct qlge_adapter *qdev)
+{
+	if (netif_carrier_ok(qdev->ndev)) {
+		set_bit(QL_LB_LINK_UP, &qdev->flags);
+		netif_carrier_off(qdev->ndev);
+	} else {
+		clear_bit(QL_LB_LINK_UP, &qdev->flags);
+	}
+	qdev->link_config |= CFG_LOOPBACK_PCS;
+	return qlge_mb_set_port_cfg(qdev);
+}
+
+static void qlge_stop_loopback(struct qlge_adapter *qdev)
+{
+	qdev->link_config &= ~CFG_LOOPBACK_PCS;
+	qlge_mb_set_port_cfg(qdev);
+	if (test_bit(QL_LB_LINK_UP, &qdev->flags)) {
+		netif_carrier_on(qdev->ndev);
+		clear_bit(QL_LB_LINK_UP, &qdev->flags);
+	}
+}
+
+static void qlge_create_lb_frame(struct sk_buff *skb,
+				 unsigned int frame_size)
+{
+	memset(skb->data, 0xFF, frame_size);
+	frame_size &= ~1;
+	memset(&skb->data[frame_size / 2], 0xAA, frame_size / 2 - 1);
+	skb->data[frame_size / 2 + 10] = (unsigned char)0xBE;
+	skb->data[frame_size / 2 + 12] = (unsigned char)0xAF;
+}
+
+void qlge_check_lb_frame(struct qlge_adapter *qdev,
+			 struct sk_buff *skb)
+{
+	unsigned int frame_size = skb->len;
+
+	if ((*(skb->data + 3) == 0xFF) &&
+	    (*(skb->data + frame_size / 2 + 10) == 0xBE) &&
+	    (*(skb->data + frame_size / 2 + 12) == 0xAF)) {
+		atomic_dec(&qdev->lb_count);
+		return;
+	}
+}
+
+static int qlge_run_loopback_test(struct qlge_adapter *qdev)
+{
+	int i;
+	netdev_tx_t rc;
+	struct sk_buff *skb;
+	unsigned int size = SMALL_BUF_MAP_SIZE;
+
+	for (i = 0; i < 64; i++) {
+		skb = netdev_alloc_skb(qdev->ndev, size);
+		if (!skb)
+			return -ENOMEM;
+
+		skb->queue_mapping = 0;
+		skb_put(skb, size);
+		qlge_create_lb_frame(skb, size);
+		rc = qlge_lb_send(skb, qdev->ndev);
+		if (rc != NETDEV_TX_OK)
+			return -EPIPE;
+		atomic_inc(&qdev->lb_count);
+	}
+	/* Give queue time to settle before testing results. */
+	usleep_range(2000, 2100);
+	qlge_clean_lb_rx_ring(&qdev->rx_ring[0], 128);
+	return atomic_read(&qdev->lb_count) ? -EIO : 0;
+}
+
+static int qlge_loopback_test(struct qlge_adapter *qdev, u64 *data)
+{
+	*data = qlge_start_loopback(qdev);
+	if (*data)
+		goto out;
+	*data = qlge_run_loopback_test(qdev);
+out:
+	qlge_stop_loopback(qdev);
+	return *data;
+}
+
+static void qlge_self_test(struct net_device *ndev,
+			   struct ethtool_test *eth_test, u64 *data)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	memset(data, 0, sizeof(u64) * QLGE_TEST_LEN);
+
+	if (netif_running(ndev)) {
+		set_bit(QL_SELFTEST, &qdev->flags);
+		if (eth_test->flags == ETH_TEST_FL_OFFLINE) {
+			/* Offline tests */
+			if (qlge_loopback_test(qdev, &data[0]))
+				eth_test->flags |= ETH_TEST_FL_FAILED;
+
+		} else {
+			/* Online tests */
+			data[0] = 0;
+		}
+		clear_bit(QL_SELFTEST, &qdev->flags);
+		/* Give link time to come up after
+		 * port configuration changes.
+		 */
+		msleep_interruptible(4 * 1000);
+	} else {
+		netif_err(qdev, drv, qdev->ndev,
+			  "is down, Loopback test will fail.\n");
+		eth_test->flags |= ETH_TEST_FL_FAILED;
+	}
+}
+
+static int qlge_get_regs_len(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	if (!test_bit(QL_FRC_COREDUMP, &qdev->flags))
+		return sizeof(struct qlge_mpi_coredump);
+	else
+		return sizeof(struct qlge_reg_dump);
+}
+
+static void qlge_get_regs(struct net_device *ndev,
+			  struct ethtool_regs *regs, void *p)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	qlge_get_dump(qdev, p);
+	if (!test_bit(QL_FRC_COREDUMP, &qdev->flags))
+		regs->len = sizeof(struct qlge_mpi_coredump);
+	else
+		regs->len = sizeof(struct qlge_reg_dump);
+}
+
+static int qlge_get_coalesce(struct net_device *ndev,
+			     struct ethtool_coalesce *c,
+			     struct kernel_ethtool_coalesce *kernel_coal,
+			     struct netlink_ext_ack *extack)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	c->rx_coalesce_usecs = qdev->rx_coalesce_usecs;
+	c->tx_coalesce_usecs = qdev->tx_coalesce_usecs;
+
+	/* This chip coalesces as follows:
+	 * If a packet arrives, hold off interrupts until
+	 * cqicb->int_delay expires, but if no other packets arrive don't
+	 * wait longer than cqicb->pkt_int_delay. But ethtool doesn't use a
+	 * timer to coalesce on a frame basis.  So, we have to take ethtool's
+	 * max_coalesced_frames value and convert it to a delay in microseconds.
+	 * We do this by using a basic thoughput of 1,000,000 frames per
+	 * second @ (1024 bytes).  This means one frame per usec. So it's a
+	 * simple one to one ratio.
+	 */
+	c->rx_max_coalesced_frames = qdev->rx_max_coalesced_frames;
+	c->tx_max_coalesced_frames = qdev->tx_max_coalesced_frames;
+
+	return 0;
+}
+
+static int qlge_set_coalesce(struct net_device *ndev,
+			     struct ethtool_coalesce *c,
+			     struct kernel_ethtool_coalesce *kernel_coal,
+			     struct netlink_ext_ack *extack)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	/* Validate user parameters. */
+	if (c->rx_coalesce_usecs > qdev->rx_ring_size / 2)
+		return -EINVAL;
+	/* Don't wait more than 10 usec. */
+	if (c->rx_max_coalesced_frames > MAX_INTER_FRAME_WAIT)
+		return -EINVAL;
+	if (c->tx_coalesce_usecs > qdev->tx_ring_size / 2)
+		return -EINVAL;
+	if (c->tx_max_coalesced_frames > MAX_INTER_FRAME_WAIT)
+		return -EINVAL;
+
+	/* Verify a change took place before updating the hardware. */
+	if (qdev->rx_coalesce_usecs == c->rx_coalesce_usecs &&
+	    qdev->tx_coalesce_usecs == c->tx_coalesce_usecs &&
+	    qdev->rx_max_coalesced_frames == c->rx_max_coalesced_frames &&
+	    qdev->tx_max_coalesced_frames == c->tx_max_coalesced_frames)
+		return 0;
+
+	qdev->rx_coalesce_usecs = c->rx_coalesce_usecs;
+	qdev->tx_coalesce_usecs = c->tx_coalesce_usecs;
+	qdev->rx_max_coalesced_frames = c->rx_max_coalesced_frames;
+	qdev->tx_max_coalesced_frames = c->tx_max_coalesced_frames;
+
+	return qlge_update_ring_coalescing(qdev);
+}
+
+static void qlge_get_pauseparam(struct net_device *ndev,
+				struct ethtool_pauseparam *pause)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	qlge_mb_get_port_cfg(qdev);
+	if (qdev->link_config & CFG_PAUSE_STD) {
+		pause->rx_pause = 1;
+		pause->tx_pause = 1;
+	}
+}
+
+static int qlge_set_pauseparam(struct net_device *ndev,
+			       struct ethtool_pauseparam *pause)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	if ((pause->rx_pause) && (pause->tx_pause))
+		qdev->link_config |= CFG_PAUSE_STD;
+	else if (!pause->rx_pause && !pause->tx_pause)
+		qdev->link_config &= ~CFG_PAUSE_STD;
+	else
+		return -EINVAL;
+
+	return qlge_mb_set_port_cfg(qdev);
+}
+
+static u32 qlge_get_msglevel(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	return qdev->msg_enable;
+}
+
+static void qlge_set_msglevel(struct net_device *ndev, u32 value)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	qdev->msg_enable = value;
+}
+
+const struct ethtool_ops qlge_ethtool_ops = {
+	.supported_coalesce_params = ETHTOOL_COALESCE_USECS |
+				     ETHTOOL_COALESCE_MAX_FRAMES,
+	.get_drvinfo = qlge_get_drvinfo,
+	.get_wol = qlge_get_wol,
+	.set_wol = qlge_set_wol,
+	.get_regs_len	= qlge_get_regs_len,
+	.get_regs = qlge_get_regs,
+	.get_msglevel = qlge_get_msglevel,
+	.set_msglevel = qlge_set_msglevel,
+	.get_link = ethtool_op_get_link,
+	.set_phys_id		 = qlge_set_phys_id,
+	.self_test		 = qlge_self_test,
+	.get_pauseparam		 = qlge_get_pauseparam,
+	.set_pauseparam		 = qlge_set_pauseparam,
+	.get_coalesce = qlge_get_coalesce,
+	.set_coalesce = qlge_set_coalesce,
+	.get_sset_count = qlge_get_sset_count,
+	.get_strings = qlge_get_strings,
+	.get_ethtool_stats = qlge_get_ethtool_stats,
+	.get_link_ksettings = qlge_get_link_ksettings,
+};
+
diff --git a/drivers/staging/qlge/qlge_main.c b/drivers/staging/qlge/qlge_main.c
new file mode 100644
index 000000000..1ead77930
--- /dev/null
+++ b/drivers/staging/qlge/qlge_main.c
@@ -0,0 +1,4845 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * QLogic qlge NIC HBA Driver
+ * Copyright (c)  2003-2008 QLogic Corporation
+ * Author:     Linux qlge network device driver by
+ *                      Ron Mercer <ron.mercer@qlogic.com>
+ */
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/pagemap.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/dmapool.h>
+#include <linux/mempool.h>
+#include <linux/spinlock.h>
+#include <linux/kthread.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/ioport.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <net/ipv6.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/if_vlan.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/prefetch.h>
+#include <net/ip6_checksum.h>
+
+#include "qlge.h"
+#include "qlge_devlink.h"
+
+char qlge_driver_name[] = DRV_NAME;
+const char qlge_driver_version[] = DRV_VERSION;
+
+MODULE_AUTHOR("Ron Mercer <ron.mercer@qlogic.com>");
+MODULE_DESCRIPTION(DRV_STRING " ");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRV_VERSION);
+
+static const u32 default_msg =
+	NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK |
+	NETIF_MSG_IFDOWN |
+	NETIF_MSG_IFUP |
+	NETIF_MSG_RX_ERR |
+	NETIF_MSG_TX_ERR |
+	NETIF_MSG_HW | NETIF_MSG_WOL | 0;
+
+static int debug = -1;	/* defaults above */
+module_param(debug, int, 0664);
+MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
+
+#define MSIX_IRQ 0
+#define MSI_IRQ 1
+#define LEG_IRQ 2
+static int qlge_irq_type = MSIX_IRQ;
+module_param(qlge_irq_type, int, 0664);
+MODULE_PARM_DESC(qlge_irq_type, "0 = MSI-X, 1 = MSI, 2 = Legacy.");
+
+static int qlge_mpi_coredump;
+module_param(qlge_mpi_coredump, int, 0);
+MODULE_PARM_DESC(qlge_mpi_coredump,
+		 "Option to enable MPI firmware dump. Default is OFF - Do Not allocate memory. ");
+
+static int qlge_force_coredump;
+module_param(qlge_force_coredump, int, 0);
+MODULE_PARM_DESC(qlge_force_coredump,
+		 "Option to allow force of firmware core dump. Default is OFF - Do not allow.");
+
+static const struct pci_device_id qlge_pci_tbl[] = {
+	{PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, QLGE_DEVICE_ID_8012)},
+	{PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, QLGE_DEVICE_ID_8000)},
+	/* required last entry */
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, qlge_pci_tbl);
+
+static int qlge_wol(struct qlge_adapter *);
+static void qlge_set_multicast_list(struct net_device *);
+static int qlge_adapter_down(struct qlge_adapter *);
+static int qlge_adapter_up(struct qlge_adapter *);
+
+/* This hardware semaphore causes exclusive access to
+ * resources shared between the NIC driver, MPI firmware,
+ * FCOE firmware and the FC driver.
+ */
+static int qlge_sem_trylock(struct qlge_adapter *qdev, u32 sem_mask)
+{
+	u32 sem_bits = 0;
+
+	switch (sem_mask) {
+	case SEM_XGMAC0_MASK:
+		sem_bits = SEM_SET << SEM_XGMAC0_SHIFT;
+		break;
+	case SEM_XGMAC1_MASK:
+		sem_bits = SEM_SET << SEM_XGMAC1_SHIFT;
+		break;
+	case SEM_ICB_MASK:
+		sem_bits = SEM_SET << SEM_ICB_SHIFT;
+		break;
+	case SEM_MAC_ADDR_MASK:
+		sem_bits = SEM_SET << SEM_MAC_ADDR_SHIFT;
+		break;
+	case SEM_FLASH_MASK:
+		sem_bits = SEM_SET << SEM_FLASH_SHIFT;
+		break;
+	case SEM_PROBE_MASK:
+		sem_bits = SEM_SET << SEM_PROBE_SHIFT;
+		break;
+	case SEM_RT_IDX_MASK:
+		sem_bits = SEM_SET << SEM_RT_IDX_SHIFT;
+		break;
+	case SEM_PROC_REG_MASK:
+		sem_bits = SEM_SET << SEM_PROC_REG_SHIFT;
+		break;
+	default:
+		netif_alert(qdev, probe, qdev->ndev, "bad Semaphore mask!.\n");
+		return -EINVAL;
+	}
+
+	qlge_write32(qdev, SEM, sem_bits | sem_mask);
+	return !(qlge_read32(qdev, SEM) & sem_bits);
+}
+
+int qlge_sem_spinlock(struct qlge_adapter *qdev, u32 sem_mask)
+{
+	unsigned int wait_count = 30;
+
+	do {
+		if (!qlge_sem_trylock(qdev, sem_mask))
+			return 0;
+		udelay(100);
+	} while (--wait_count);
+	return -ETIMEDOUT;
+}
+
+void qlge_sem_unlock(struct qlge_adapter *qdev, u32 sem_mask)
+{
+	qlge_write32(qdev, SEM, sem_mask);
+	qlge_read32(qdev, SEM);	/* flush */
+}
+
+/* This function waits for a specific bit to come ready
+ * in a given register.  It is used mostly by the initialize
+ * process, but is also used in kernel thread API such as
+ * netdev->set_multi, netdev->set_mac_address, netdev->vlan_rx_add_vid.
+ */
+int qlge_wait_reg_rdy(struct qlge_adapter *qdev, u32 reg, u32 bit, u32 err_bit)
+{
+	u32 temp;
+	int count;
+
+	for (count = 0; count < UDELAY_COUNT; count++) {
+		temp = qlge_read32(qdev, reg);
+
+		/* check for errors */
+		if (temp & err_bit) {
+			netif_alert(qdev, probe, qdev->ndev,
+				    "register 0x%.08x access error, value = 0x%.08x!.\n",
+				    reg, temp);
+			return -EIO;
+		} else if (temp & bit) {
+			return 0;
+		}
+		udelay(UDELAY_DELAY);
+	}
+	netif_alert(qdev, probe, qdev->ndev,
+		    "Timed out waiting for reg %x to come ready.\n", reg);
+	return -ETIMEDOUT;
+}
+
+/* The CFG register is used to download TX and RX control blocks
+ * to the chip. This function waits for an operation to complete.
+ */
+static int qlge_wait_cfg(struct qlge_adapter *qdev, u32 bit)
+{
+	int count;
+	u32 temp;
+
+	for (count = 0; count < UDELAY_COUNT; count++) {
+		temp = qlge_read32(qdev, CFG);
+		if (temp & CFG_LE)
+			return -EIO;
+		if (!(temp & bit))
+			return 0;
+		udelay(UDELAY_DELAY);
+	}
+	return -ETIMEDOUT;
+}
+
+/* Used to issue init control blocks to hw. Maps control block,
+ * sets address, triggers download, waits for completion.
+ */
+int qlge_write_cfg(struct qlge_adapter *qdev, void *ptr, int size, u32 bit,
+		   u16 q_id)
+{
+	u64 map;
+	int status = 0;
+	int direction;
+	u32 mask;
+	u32 value;
+
+	if (bit & (CFG_LRQ | CFG_LR | CFG_LCQ))
+		direction = DMA_TO_DEVICE;
+	else
+		direction = DMA_FROM_DEVICE;
+
+	map = dma_map_single(&qdev->pdev->dev, ptr, size, direction);
+	if (dma_mapping_error(&qdev->pdev->dev, map)) {
+		netif_err(qdev, ifup, qdev->ndev, "Couldn't map DMA area.\n");
+		return -ENOMEM;
+	}
+
+	status = qlge_sem_spinlock(qdev, SEM_ICB_MASK);
+	if (status)
+		goto lock_failed;
+
+	status = qlge_wait_cfg(qdev, bit);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Timed out waiting for CFG to come ready.\n");
+		goto exit;
+	}
+
+	qlge_write32(qdev, ICB_L, (u32)map);
+	qlge_write32(qdev, ICB_H, (u32)(map >> 32));
+
+	mask = CFG_Q_MASK | (bit << 16);
+	value = bit | (q_id << CFG_Q_SHIFT);
+	qlge_write32(qdev, CFG, (mask | value));
+
+	/*
+	 * Wait for the bit to clear after signaling hw.
+	 */
+	status = qlge_wait_cfg(qdev, bit);
+exit:
+	qlge_sem_unlock(qdev, SEM_ICB_MASK);	/* does flush too */
+lock_failed:
+	dma_unmap_single(&qdev->pdev->dev, map, size, direction);
+	return status;
+}
+
+/* Get a specific MAC address from the CAM.  Used for debug and reg dump. */
+int qlge_get_mac_addr_reg(struct qlge_adapter *qdev, u32 type, u16 index,
+			  u32 *value)
+{
+	u32 offset = 0;
+	int status;
+
+	switch (type) {
+	case MAC_ADDR_TYPE_MULTI_MAC:
+	case MAC_ADDR_TYPE_CAM_MAC: {
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   MAC_ADDR_ADR | MAC_ADDR_RS |
+				   type); /* type */
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
+		if (status)
+			break;
+		*value++ = qlge_read32(qdev, MAC_ADDR_DATA);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   MAC_ADDR_ADR | MAC_ADDR_RS |
+				   type); /* type */
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
+		if (status)
+			break;
+		*value++ = qlge_read32(qdev, MAC_ADDR_DATA);
+		if (type == MAC_ADDR_TYPE_CAM_MAC) {
+			status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX,
+						   MAC_ADDR_MW, 0);
+			if (status)
+				break;
+			qlge_write32(qdev, MAC_ADDR_IDX,
+				     (offset++) | /* offset */
+					   (index
+					    << MAC_ADDR_IDX_SHIFT) | /* index */
+					   MAC_ADDR_ADR |
+					   MAC_ADDR_RS | type); /* type */
+			status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX,
+						   MAC_ADDR_MR, 0);
+			if (status)
+				break;
+			*value++ = qlge_read32(qdev, MAC_ADDR_DATA);
+		}
+		break;
+	}
+	case MAC_ADDR_TYPE_VLAN:
+	case MAC_ADDR_TYPE_MULTI_FLTR:
+	default:
+		netif_crit(qdev, ifup, qdev->ndev,
+			   "Address type %d not yet supported.\n", type);
+		status = -EPERM;
+	}
+	return status;
+}
+
+/* Set up a MAC, multicast or VLAN address for the
+ * inbound frame matching.
+ */
+static int qlge_set_mac_addr_reg(struct qlge_adapter *qdev, const u8 *addr,
+				 u32 type, u16 index)
+{
+	u32 offset = 0;
+	int status = 0;
+
+	switch (type) {
+	case MAC_ADDR_TYPE_MULTI_MAC: {
+		u32 upper = (addr[0] << 8) | addr[1];
+		u32 lower = (addr[2] << 24) | (addr[3] << 16) | (addr[4] << 8) |
+			    (addr[5]);
+
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | (index << MAC_ADDR_IDX_SHIFT) | type |
+				   MAC_ADDR_E);
+		qlge_write32(qdev, MAC_ADDR_DATA, lower);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | (index << MAC_ADDR_IDX_SHIFT) | type |
+				   MAC_ADDR_E);
+
+		qlge_write32(qdev, MAC_ADDR_DATA, upper);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		break;
+	}
+	case MAC_ADDR_TYPE_CAM_MAC: {
+		u32 cam_output;
+		u32 upper = (addr[0] << 8) | addr[1];
+		u32 lower = (addr[2] << 24) | (addr[3] << 16) | (addr[4] << 8) |
+			    (addr[5]);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type); /* type */
+		qlge_write32(qdev, MAC_ADDR_DATA, lower);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset++) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type); /* type */
+		qlge_write32(qdev, MAC_ADDR_DATA, upper);
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     (offset) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type); /* type */
+		/* This field should also include the queue id
+		 * and possibly the function id.  Right now we hardcode
+		 * the route field to NIC core.
+		 */
+		cam_output = (CAM_OUT_ROUTE_NIC |
+			      (qdev->func << CAM_OUT_FUNC_SHIFT) |
+			      (0 << CAM_OUT_CQ_ID_SHIFT));
+		if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)
+			cam_output |= CAM_OUT_RV;
+		/* route to NIC core */
+		qlge_write32(qdev, MAC_ADDR_DATA, cam_output);
+		break;
+	}
+	case MAC_ADDR_TYPE_VLAN: {
+		u32 enable_bit = *((u32 *)&addr[0]);
+		/* For VLAN, the addr actually holds a bit that
+		 * either enables or disables the vlan id we are
+		 * addressing. It's either MAC_ADDR_E on or off.
+		 * That's bit-27 we're talking about.
+		 */
+		status = qlge_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		qlge_write32(qdev, MAC_ADDR_IDX,
+			     offset | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type | /* type */
+				   enable_bit); /* enable/disable */
+		break;
+	}
+	case MAC_ADDR_TYPE_MULTI_FLTR:
+	default:
+		netif_crit(qdev, ifup, qdev->ndev,
+			   "Address type %d not yet supported.\n", type);
+		status = -EPERM;
+	}
+	return status;
+}
+
+/* Set or clear MAC address in hardware. We sometimes
+ * have to clear it to prevent wrong frame routing
+ * especially in a bonding environment.
+ */
+static int qlge_set_mac_addr(struct qlge_adapter *qdev, int set)
+{
+	int status;
+	char zero_mac_addr[ETH_ALEN];
+	char *addr;
+
+	if (set) {
+		addr = &qdev->current_mac_addr[0];
+		netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+			     "Set Mac addr %pM\n", addr);
+	} else {
+		eth_zero_addr(zero_mac_addr);
+		addr = &zero_mac_addr[0];
+		netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+			     "Clearing MAC address\n");
+	}
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return status;
+	status = qlge_set_mac_addr_reg(qdev, (const u8 *)addr,
+				       MAC_ADDR_TYPE_CAM_MAC,
+				       qdev->func * MAX_CQ);
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init mac address.\n");
+	return status;
+}
+
+void qlge_link_on(struct qlge_adapter *qdev)
+{
+	netif_err(qdev, link, qdev->ndev, "Link is up.\n");
+	netif_carrier_on(qdev->ndev);
+	qlge_set_mac_addr(qdev, 1);
+}
+
+void qlge_link_off(struct qlge_adapter *qdev)
+{
+	netif_err(qdev, link, qdev->ndev, "Link is down.\n");
+	netif_carrier_off(qdev->ndev);
+	qlge_set_mac_addr(qdev, 0);
+}
+
+/* Get a specific frame routing value from the CAM.
+ * Used for debug and reg dump.
+ */
+int qlge_get_routing_reg(struct qlge_adapter *qdev, u32 index, u32 *value)
+{
+	int status = 0;
+
+	status = qlge_wait_reg_rdy(qdev, RT_IDX, RT_IDX_MW, 0);
+	if (status)
+		goto exit;
+
+	qlge_write32(qdev, RT_IDX,
+		     RT_IDX_TYPE_NICQ | RT_IDX_RS | (index << RT_IDX_IDX_SHIFT));
+	status = qlge_wait_reg_rdy(qdev, RT_IDX, RT_IDX_MR, 0);
+	if (status)
+		goto exit;
+	*value = qlge_read32(qdev, RT_DATA);
+exit:
+	return status;
+}
+
+/* The NIC function for this chip has 16 routing indexes.  Each one can be used
+ * to route different frame types to various inbound queues.  We send broadcast/
+ * multicast/error frames to the default queue for slow handling,
+ * and CAM hit/RSS frames to the fast handling queues.
+ */
+static int qlge_set_routing_reg(struct qlge_adapter *qdev, u32 index, u32 mask,
+				int enable)
+{
+	int status = -EINVAL; /* Return error if no mask match. */
+	u32 value = 0;
+
+	switch (mask) {
+	case RT_IDX_CAM_HIT:
+		{
+			value = RT_IDX_DST_CAM_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_CAM_HIT_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_VALID:	/* Promiscuous Mode frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_PROMISCUOUS_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_ERR:	/* Pass up MAC,IP,TCP/UDP error frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_ALL_ERR_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_IP_CSUM_ERR: /* Pass up IP CSUM error frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q | /* dest */
+				RT_IDX_TYPE_NICQ | /* type */
+				(RT_IDX_IP_CSUM_ERR_SLOT <<
+				RT_IDX_IDX_SHIFT); /* index */
+			break;
+		}
+	case RT_IDX_TU_CSUM_ERR: /* Pass up TCP/UDP CSUM error frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q | /* dest */
+				RT_IDX_TYPE_NICQ | /* type */
+				(RT_IDX_TCP_UDP_CSUM_ERR_SLOT <<
+				RT_IDX_IDX_SHIFT); /* index */
+			break;
+		}
+	case RT_IDX_BCAST:	/* Pass up Broadcast frames to default Q. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_BCAST_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_MCAST:	/* Pass up All Multicast frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_ALLMULTI_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_MCAST_MATCH:	/* Pass up matched Multicast frames. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_MCAST_MATCH_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case RT_IDX_RSS_MATCH:	/* Pass up matched RSS frames. */
+		{
+			value = RT_IDX_DST_RSS |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (RT_IDX_RSS_MATCH_SLOT << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	case 0:		/* Clear the E-bit on an entry. */
+		{
+			value = RT_IDX_DST_DFLT_Q |	/* dest */
+			    RT_IDX_TYPE_NICQ |	/* type */
+			    (index << RT_IDX_IDX_SHIFT);/* index */
+			break;
+		}
+	default:
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Mask type %d not yet supported.\n", mask);
+		status = -EPERM;
+		goto exit;
+	}
+
+	if (value) {
+		status = qlge_wait_reg_rdy(qdev, RT_IDX, RT_IDX_MW, 0);
+		if (status)
+			goto exit;
+		value |= (enable ? RT_IDX_E : 0);
+		qlge_write32(qdev, RT_IDX, value);
+		qlge_write32(qdev, RT_DATA, enable ? mask : 0);
+	}
+exit:
+	return status;
+}
+
+static void qlge_enable_interrupts(struct qlge_adapter *qdev)
+{
+	qlge_write32(qdev, INTR_EN, (INTR_EN_EI << 16) | INTR_EN_EI);
+}
+
+static void qlge_disable_interrupts(struct qlge_adapter *qdev)
+{
+	qlge_write32(qdev, INTR_EN, (INTR_EN_EI << 16));
+}
+
+static void qlge_enable_completion_interrupt(struct qlge_adapter *qdev, u32 intr)
+{
+	struct intr_context *ctx = &qdev->intr_context[intr];
+
+	qlge_write32(qdev, INTR_EN, ctx->intr_en_mask);
+}
+
+static void qlge_disable_completion_interrupt(struct qlge_adapter *qdev, u32 intr)
+{
+	struct intr_context *ctx = &qdev->intr_context[intr];
+
+	qlge_write32(qdev, INTR_EN, ctx->intr_dis_mask);
+}
+
+static void qlge_enable_all_completion_interrupts(struct qlge_adapter *qdev)
+{
+	int i;
+
+	for (i = 0; i < qdev->intr_count; i++)
+		qlge_enable_completion_interrupt(qdev, i);
+}
+
+static int qlge_validate_flash(struct qlge_adapter *qdev, u32 size, const char *str)
+{
+	int status, i;
+	u16 csum = 0;
+	__le16 *flash = (__le16 *)&qdev->flash;
+
+	status = strncmp((char *)&qdev->flash, str, 4);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Invalid flash signature.\n");
+		return	status;
+	}
+
+	for (i = 0; i < size; i++)
+		csum += le16_to_cpu(*flash++);
+
+	if (csum)
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Invalid flash checksum, csum = 0x%.04x.\n", csum);
+
+	return csum;
+}
+
+static int qlge_read_flash_word(struct qlge_adapter *qdev, int offset, __le32 *data)
+{
+	int status = 0;
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev,
+				   FLASH_ADDR, FLASH_ADDR_RDY, FLASH_ADDR_ERR);
+	if (status)
+		goto exit;
+	/* set up for reg read */
+	qlge_write32(qdev, FLASH_ADDR, FLASH_ADDR_R | offset);
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev,
+				   FLASH_ADDR, FLASH_ADDR_RDY, FLASH_ADDR_ERR);
+	if (status)
+		goto exit;
+	/* This data is stored on flash as an array of
+	 * __le32.  Since qlge_read32() returns cpu endian
+	 * we need to swap it back.
+	 */
+	*data = cpu_to_le32(qlge_read32(qdev, FLASH_DATA));
+exit:
+	return status;
+}
+
+static int qlge_get_8000_flash_params(struct qlge_adapter *qdev)
+{
+	u32 i, size;
+	int status;
+	__le32 *p = (__le32 *)&qdev->flash;
+	u32 offset;
+	u8 mac_addr[6];
+
+	/* Get flash offset for function and adjust
+	 * for dword access.
+	 */
+	if (!qdev->port)
+		offset = FUNC0_FLASH_OFFSET / sizeof(u32);
+	else
+		offset = FUNC1_FLASH_OFFSET / sizeof(u32);
+
+	if (qlge_sem_spinlock(qdev, SEM_FLASH_MASK))
+		return -ETIMEDOUT;
+
+	size = sizeof(struct flash_params_8000) / sizeof(u32);
+	for (i = 0; i < size; i++, p++) {
+		status = qlge_read_flash_word(qdev, i + offset, p);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Error reading flash.\n");
+			goto exit;
+		}
+	}
+
+	status = qlge_validate_flash(qdev,
+				     sizeof(struct flash_params_8000) /
+				   sizeof(u16),
+				   "8000");
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Invalid flash.\n");
+		status = -EINVAL;
+		goto exit;
+	}
+
+	/* Extract either manufacturer or BOFM modified
+	 * MAC address.
+	 */
+	if (qdev->flash.flash_params_8000.data_type1 == 2)
+		memcpy(mac_addr,
+		       qdev->flash.flash_params_8000.mac_addr1,
+		       qdev->ndev->addr_len);
+	else
+		memcpy(mac_addr,
+		       qdev->flash.flash_params_8000.mac_addr,
+		       qdev->ndev->addr_len);
+
+	if (!is_valid_ether_addr(mac_addr)) {
+		netif_err(qdev, ifup, qdev->ndev, "Invalid MAC address.\n");
+		status = -EINVAL;
+		goto exit;
+	}
+
+	eth_hw_addr_set(qdev->ndev, mac_addr);
+
+exit:
+	qlge_sem_unlock(qdev, SEM_FLASH_MASK);
+	return status;
+}
+
+static int qlge_get_8012_flash_params(struct qlge_adapter *qdev)
+{
+	int i;
+	int status;
+	__le32 *p = (__le32 *)&qdev->flash;
+	u32 offset = 0;
+	u32 size = sizeof(struct flash_params_8012) / sizeof(u32);
+
+	/* Second function's parameters follow the first
+	 * function's.
+	 */
+	if (qdev->port)
+		offset = size;
+
+	if (qlge_sem_spinlock(qdev, SEM_FLASH_MASK))
+		return -ETIMEDOUT;
+
+	for (i = 0; i < size; i++, p++) {
+		status = qlge_read_flash_word(qdev, i + offset, p);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Error reading flash.\n");
+			goto exit;
+		}
+	}
+
+	status = qlge_validate_flash(qdev,
+				     sizeof(struct flash_params_8012) /
+				       sizeof(u16),
+				     "8012");
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Invalid flash.\n");
+		status = -EINVAL;
+		goto exit;
+	}
+
+	if (!is_valid_ether_addr(qdev->flash.flash_params_8012.mac_addr)) {
+		status = -EINVAL;
+		goto exit;
+	}
+
+	eth_hw_addr_set(qdev->ndev, qdev->flash.flash_params_8012.mac_addr);
+
+exit:
+	qlge_sem_unlock(qdev, SEM_FLASH_MASK);
+	return status;
+}
+
+/* xgmac register are located behind the xgmac_addr and xgmac_data
+ * register pair.  Each read/write requires us to wait for the ready
+ * bit before reading/writing the data.
+ */
+static int qlge_write_xgmac_reg(struct qlge_adapter *qdev, u32 reg, u32 data)
+{
+	int status;
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev,
+				   XGMAC_ADDR, XGMAC_ADDR_RDY, XGMAC_ADDR_XME);
+	if (status)
+		return status;
+	/* write the data to the data reg */
+	qlge_write32(qdev, XGMAC_DATA, data);
+	/* trigger the write */
+	qlge_write32(qdev, XGMAC_ADDR, reg);
+	return status;
+}
+
+/* xgmac register are located behind the xgmac_addr and xgmac_data
+ * register pair.  Each read/write requires us to wait for the ready
+ * bit before reading/writing the data.
+ */
+int qlge_read_xgmac_reg(struct qlge_adapter *qdev, u32 reg, u32 *data)
+{
+	int status = 0;
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev,
+				   XGMAC_ADDR, XGMAC_ADDR_RDY, XGMAC_ADDR_XME);
+	if (status)
+		goto exit;
+	/* set up for reg read */
+	qlge_write32(qdev, XGMAC_ADDR, reg | XGMAC_ADDR_R);
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev,
+				   XGMAC_ADDR, XGMAC_ADDR_RDY, XGMAC_ADDR_XME);
+	if (status)
+		goto exit;
+	/* get the data */
+	*data = qlge_read32(qdev, XGMAC_DATA);
+exit:
+	return status;
+}
+
+/* This is used for reading the 64-bit statistics regs. */
+int qlge_read_xgmac_reg64(struct qlge_adapter *qdev, u32 reg, u64 *data)
+{
+	int status = 0;
+	u32 hi = 0;
+	u32 lo = 0;
+
+	status = qlge_read_xgmac_reg(qdev, reg, &lo);
+	if (status)
+		goto exit;
+
+	status = qlge_read_xgmac_reg(qdev, reg + 4, &hi);
+	if (status)
+		goto exit;
+
+	*data = (u64)lo | ((u64)hi << 32);
+
+exit:
+	return status;
+}
+
+static int qlge_8000_port_initialize(struct qlge_adapter *qdev)
+{
+	int status;
+	/*
+	 * Get MPI firmware version for driver banner
+	 * and ethool info.
+	 */
+	status = qlge_mb_about_fw(qdev);
+	if (status)
+		goto exit;
+	status = qlge_mb_get_fw_state(qdev);
+	if (status)
+		goto exit;
+	/* Wake up a worker to get/set the TX/RX frame sizes. */
+	queue_delayed_work(qdev->workqueue, &qdev->mpi_port_cfg_work, 0);
+exit:
+	return status;
+}
+
+/* Take the MAC Core out of reset.
+ * Enable statistics counting.
+ * Take the transmitter/receiver out of reset.
+ * This functionality may be done in the MPI firmware at a
+ * later date.
+ */
+static int qlge_8012_port_initialize(struct qlge_adapter *qdev)
+{
+	int status = 0;
+	u32 data;
+
+	if (qlge_sem_trylock(qdev, qdev->xg_sem_mask)) {
+		/* Another function has the semaphore, so
+		 * wait for the port init bit to come ready.
+		 */
+		netif_info(qdev, link, qdev->ndev,
+			   "Another function has the semaphore, so wait for the port init bit to come ready.\n");
+		status = qlge_wait_reg_rdy(qdev, STS, qdev->port_init, 0);
+		if (status) {
+			netif_crit(qdev, link, qdev->ndev,
+				   "Port initialize timed out.\n");
+		}
+		return status;
+	}
+
+	netif_info(qdev, link, qdev->ndev, "Got xgmac semaphore!.\n");
+	/* Set the core reset. */
+	status = qlge_read_xgmac_reg(qdev, GLOBAL_CFG, &data);
+	if (status)
+		goto end;
+	data |= GLOBAL_CFG_RESET;
+	status = qlge_write_xgmac_reg(qdev, GLOBAL_CFG, data);
+	if (status)
+		goto end;
+
+	/* Clear the core reset and turn on jumbo for receiver. */
+	data &= ~GLOBAL_CFG_RESET;	/* Clear core reset. */
+	data |= GLOBAL_CFG_JUMBO;	/* Turn on jumbo. */
+	data |= GLOBAL_CFG_TX_STAT_EN;
+	data |= GLOBAL_CFG_RX_STAT_EN;
+	status = qlge_write_xgmac_reg(qdev, GLOBAL_CFG, data);
+	if (status)
+		goto end;
+
+	/* Enable transmitter, and clear it's reset. */
+	status = qlge_read_xgmac_reg(qdev, TX_CFG, &data);
+	if (status)
+		goto end;
+	data &= ~TX_CFG_RESET;	/* Clear the TX MAC reset. */
+	data |= TX_CFG_EN;	/* Enable the transmitter. */
+	status = qlge_write_xgmac_reg(qdev, TX_CFG, data);
+	if (status)
+		goto end;
+
+	/* Enable receiver and clear it's reset. */
+	status = qlge_read_xgmac_reg(qdev, RX_CFG, &data);
+	if (status)
+		goto end;
+	data &= ~RX_CFG_RESET;	/* Clear the RX MAC reset. */
+	data |= RX_CFG_EN;	/* Enable the receiver. */
+	status = qlge_write_xgmac_reg(qdev, RX_CFG, data);
+	if (status)
+		goto end;
+
+	/* Turn on jumbo. */
+	status =
+	    qlge_write_xgmac_reg(qdev, MAC_TX_PARAMS, MAC_TX_PARAMS_JUMBO | (0x2580 << 16));
+	if (status)
+		goto end;
+	status =
+	    qlge_write_xgmac_reg(qdev, MAC_RX_PARAMS, 0x2580);
+	if (status)
+		goto end;
+
+	/* Signal to the world that the port is enabled.        */
+	qlge_write32(qdev, STS, ((qdev->port_init << 16) | qdev->port_init));
+end:
+	qlge_sem_unlock(qdev, qdev->xg_sem_mask);
+	return status;
+}
+
+static inline unsigned int qlge_lbq_block_size(struct qlge_adapter *qdev)
+{
+	return PAGE_SIZE << qdev->lbq_buf_order;
+}
+
+static struct qlge_bq_desc *qlge_get_curr_buf(struct qlge_bq *bq)
+{
+	struct qlge_bq_desc *bq_desc;
+
+	bq_desc = &bq->queue[bq->next_to_clean];
+	bq->next_to_clean = QLGE_BQ_WRAP(bq->next_to_clean + 1);
+
+	return bq_desc;
+}
+
+static struct qlge_bq_desc *qlge_get_curr_lchunk(struct qlge_adapter *qdev,
+						 struct rx_ring *rx_ring)
+{
+	struct qlge_bq_desc *lbq_desc = qlge_get_curr_buf(&rx_ring->lbq);
+
+	dma_sync_single_for_cpu(&qdev->pdev->dev, lbq_desc->dma_addr,
+				qdev->lbq_buf_size, DMA_FROM_DEVICE);
+
+	if ((lbq_desc->p.pg_chunk.offset + qdev->lbq_buf_size) ==
+	    qlge_lbq_block_size(qdev)) {
+		/* last chunk of the master page */
+		dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
+			       qlge_lbq_block_size(qdev), DMA_FROM_DEVICE);
+	}
+
+	return lbq_desc;
+}
+
+/* Update an rx ring index. */
+static void qlge_update_cq(struct rx_ring *rx_ring)
+{
+	rx_ring->cnsmr_idx++;
+	rx_ring->curr_entry++;
+	if (unlikely(rx_ring->cnsmr_idx == rx_ring->cq_len)) {
+		rx_ring->cnsmr_idx = 0;
+		rx_ring->curr_entry = rx_ring->cq_base;
+	}
+}
+
+static void qlge_write_cq_idx(struct rx_ring *rx_ring)
+{
+	qlge_write_db_reg(rx_ring->cnsmr_idx, rx_ring->cnsmr_idx_db_reg);
+}
+
+static const char * const bq_type_name[] = {
+	[QLGE_SB] = "sbq",
+	[QLGE_LB] = "lbq",
+};
+
+/* return 0 or negative error */
+static int qlge_refill_sb(struct rx_ring *rx_ring,
+			  struct qlge_bq_desc *sbq_desc, gfp_t gfp)
+{
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct sk_buff *skb;
+
+	if (sbq_desc->p.skb)
+		return 0;
+
+	netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+		     "ring %u sbq: getting new skb for index %d.\n",
+		     rx_ring->cq_id, sbq_desc->index);
+
+	skb = __netdev_alloc_skb(qdev->ndev, SMALL_BUFFER_SIZE, gfp);
+	if (!skb)
+		return -ENOMEM;
+	skb_reserve(skb, QLGE_SB_PAD);
+
+	sbq_desc->dma_addr = dma_map_single(&qdev->pdev->dev, skb->data,
+					    SMALL_BUF_MAP_SIZE,
+					    DMA_FROM_DEVICE);
+	if (dma_mapping_error(&qdev->pdev->dev, sbq_desc->dma_addr)) {
+		netif_err(qdev, ifup, qdev->ndev, "PCI mapping failed.\n");
+		dev_kfree_skb_any(skb);
+		return -EIO;
+	}
+	*sbq_desc->buf_ptr = cpu_to_le64(sbq_desc->dma_addr);
+
+	sbq_desc->p.skb = skb;
+	return 0;
+}
+
+/* return 0 or negative error */
+static int qlge_refill_lb(struct rx_ring *rx_ring,
+			  struct qlge_bq_desc *lbq_desc, gfp_t gfp)
+{
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct qlge_page_chunk *master_chunk = &rx_ring->master_chunk;
+
+	if (!master_chunk->page) {
+		struct page *page;
+		dma_addr_t dma_addr;
+
+		page = alloc_pages(gfp | __GFP_COMP, qdev->lbq_buf_order);
+		if (unlikely(!page))
+			return -ENOMEM;
+		dma_addr = dma_map_page(&qdev->pdev->dev, page, 0,
+					qlge_lbq_block_size(qdev),
+					DMA_FROM_DEVICE);
+		if (dma_mapping_error(&qdev->pdev->dev, dma_addr)) {
+			__free_pages(page, qdev->lbq_buf_order);
+			netif_err(qdev, drv, qdev->ndev,
+				  "PCI mapping failed.\n");
+			return -EIO;
+		}
+		master_chunk->page = page;
+		master_chunk->va = page_address(page);
+		master_chunk->offset = 0;
+		rx_ring->chunk_dma_addr = dma_addr;
+	}
+
+	lbq_desc->p.pg_chunk = *master_chunk;
+	lbq_desc->dma_addr = rx_ring->chunk_dma_addr;
+	*lbq_desc->buf_ptr = cpu_to_le64(lbq_desc->dma_addr +
+					 lbq_desc->p.pg_chunk.offset);
+
+	/* Adjust the master page chunk for next
+	 * buffer get.
+	 */
+	master_chunk->offset += qdev->lbq_buf_size;
+	if (master_chunk->offset == qlge_lbq_block_size(qdev)) {
+		master_chunk->page = NULL;
+	} else {
+		master_chunk->va += qdev->lbq_buf_size;
+		get_page(master_chunk->page);
+	}
+
+	return 0;
+}
+
+/* return 0 or negative error */
+static int qlge_refill_bq(struct qlge_bq *bq, gfp_t gfp)
+{
+	struct rx_ring *rx_ring = QLGE_BQ_CONTAINER(bq);
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct qlge_bq_desc *bq_desc;
+	int refill_count;
+	int retval;
+	int i;
+
+	refill_count = QLGE_BQ_WRAP(QLGE_BQ_ALIGN(bq->next_to_clean - 1) -
+				    bq->next_to_use);
+	if (!refill_count)
+		return 0;
+
+	i = bq->next_to_use;
+	bq_desc = &bq->queue[i];
+	i -= QLGE_BQ_LEN;
+	do {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "ring %u %s: try cleaning idx %d\n",
+			     rx_ring->cq_id, bq_type_name[bq->type], i);
+
+		if (bq->type == QLGE_SB)
+			retval = qlge_refill_sb(rx_ring, bq_desc, gfp);
+		else
+			retval = qlge_refill_lb(rx_ring, bq_desc, gfp);
+		if (retval < 0) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "ring %u %s: Could not get a page chunk, idx %d\n",
+				  rx_ring->cq_id, bq_type_name[bq->type], i);
+			break;
+		}
+
+		bq_desc++;
+		i++;
+		if (unlikely(!i)) {
+			bq_desc = &bq->queue[0];
+			i -= QLGE_BQ_LEN;
+		}
+		refill_count--;
+	} while (refill_count);
+	i += QLGE_BQ_LEN;
+
+	if (bq->next_to_use != i) {
+		if (QLGE_BQ_ALIGN(bq->next_to_use) != QLGE_BQ_ALIGN(i)) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "ring %u %s: updating prod idx = %d.\n",
+				     rx_ring->cq_id, bq_type_name[bq->type],
+				     i);
+			qlge_write_db_reg(i, bq->prod_idx_db_reg);
+		}
+		bq->next_to_use = i;
+	}
+
+	return retval;
+}
+
+static void qlge_update_buffer_queues(struct rx_ring *rx_ring, gfp_t gfp,
+				      unsigned long delay)
+{
+	bool sbq_fail, lbq_fail;
+
+	sbq_fail = !!qlge_refill_bq(&rx_ring->sbq, gfp);
+	lbq_fail = !!qlge_refill_bq(&rx_ring->lbq, gfp);
+
+	/* Minimum number of buffers needed to be able to receive at least one
+	 * frame of any format:
+	 * sbq: 1 for header + 1 for data
+	 * lbq: mtu 9000 / lb size
+	 * Below this, the queue might stall.
+	 */
+	if ((sbq_fail && QLGE_BQ_HW_OWNED(&rx_ring->sbq) < 2) ||
+	    (lbq_fail && QLGE_BQ_HW_OWNED(&rx_ring->lbq) <
+	     DIV_ROUND_UP(9000, LARGE_BUFFER_MAX_SIZE)))
+		/* Allocations can take a long time in certain cases (ex.
+		 * reclaim). Therefore, use a workqueue for long-running
+		 * work items.
+		 */
+		queue_delayed_work_on(smp_processor_id(), system_long_wq,
+				      &rx_ring->refill_work, delay);
+}
+
+static void qlge_slow_refill(struct work_struct *work)
+{
+	struct rx_ring *rx_ring = container_of(work, struct rx_ring,
+					       refill_work.work);
+	struct napi_struct *napi = &rx_ring->napi;
+
+	napi_disable(napi);
+	qlge_update_buffer_queues(rx_ring, GFP_KERNEL, HZ / 2);
+	napi_enable(napi);
+
+	local_bh_disable();
+	/* napi_disable() might have prevented incomplete napi work from being
+	 * rescheduled.
+	 */
+	napi_schedule(napi);
+	/* trigger softirq processing */
+	local_bh_enable();
+}
+
+/* Unmaps tx buffers.  Can be called from send() if a pci mapping
+ * fails at some stage, or from the interrupt when a tx completes.
+ */
+static void qlge_unmap_send(struct qlge_adapter *qdev,
+			    struct tx_ring_desc *tx_ring_desc, int mapped)
+{
+	int i;
+
+	for (i = 0; i < mapped; i++) {
+		if (i == 0 || (i == 7 && mapped > 7)) {
+			/*
+			 * Unmap the skb->data area, or the
+			 * external sglist (AKA the Outbound
+			 * Address List (OAL)).
+			 * If its the zeroeth element, then it's
+			 * the skb->data area.  If it's the 7th
+			 * element and there is more than 6 frags,
+			 * then its an OAL.
+			 */
+			if (i == 7) {
+				netif_printk(qdev, tx_done, KERN_DEBUG,
+					     qdev->ndev,
+					     "unmapping OAL area.\n");
+			}
+			dma_unmap_single(&qdev->pdev->dev,
+					 dma_unmap_addr(&tx_ring_desc->map[i],
+							mapaddr),
+					 dma_unmap_len(&tx_ring_desc->map[i],
+						       maplen),
+					 DMA_TO_DEVICE);
+		} else {
+			netif_printk(qdev, tx_done, KERN_DEBUG, qdev->ndev,
+				     "unmapping frag %d.\n", i);
+			dma_unmap_page(&qdev->pdev->dev,
+				       dma_unmap_addr(&tx_ring_desc->map[i],
+						      mapaddr),
+				       dma_unmap_len(&tx_ring_desc->map[i],
+						     maplen), DMA_TO_DEVICE);
+		}
+	}
+}
+
+/* Map the buffers for this transmit.  This will return
+ * NETDEV_TX_BUSY or NETDEV_TX_OK based on success.
+ */
+static int qlge_map_send(struct qlge_adapter *qdev,
+			 struct qlge_ob_mac_iocb_req *mac_iocb_ptr,
+			 struct sk_buff *skb, struct tx_ring_desc *tx_ring_desc)
+{
+	int len = skb_headlen(skb);
+	dma_addr_t map;
+	int frag_idx, err, map_idx = 0;
+	struct tx_buf_desc *tbd = mac_iocb_ptr->tbd;
+	int frag_cnt = skb_shinfo(skb)->nr_frags;
+
+	if (frag_cnt) {
+		netif_printk(qdev, tx_queued, KERN_DEBUG, qdev->ndev,
+			     "frag_cnt = %d.\n", frag_cnt);
+	}
+	/*
+	 * Map the skb buffer first.
+	 */
+	map = dma_map_single(&qdev->pdev->dev, skb->data, len, DMA_TO_DEVICE);
+
+	err = dma_mapping_error(&qdev->pdev->dev, map);
+	if (err) {
+		netif_err(qdev, tx_queued, qdev->ndev,
+			  "PCI mapping failed with error: %d\n", err);
+
+		return NETDEV_TX_BUSY;
+	}
+
+	tbd->len = cpu_to_le32(len);
+	tbd->addr = cpu_to_le64(map);
+	dma_unmap_addr_set(&tx_ring_desc->map[map_idx], mapaddr, map);
+	dma_unmap_len_set(&tx_ring_desc->map[map_idx], maplen, len);
+	map_idx++;
+
+	/*
+	 * This loop fills the remainder of the 8 address descriptors
+	 * in the IOCB.  If there are more than 7 fragments, then the
+	 * eighth address desc will point to an external list (OAL).
+	 * When this happens, the remainder of the frags will be stored
+	 * in this list.
+	 */
+	for (frag_idx = 0; frag_idx < frag_cnt; frag_idx++, map_idx++) {
+		skb_frag_t *frag = &skb_shinfo(skb)->frags[frag_idx];
+
+		tbd++;
+		if (frag_idx == 6 && frag_cnt > 7) {
+			/* Let's tack on an sglist.
+			 * Our control block will now
+			 * look like this:
+			 * iocb->seg[0] = skb->data
+			 * iocb->seg[1] = frag[0]
+			 * iocb->seg[2] = frag[1]
+			 * iocb->seg[3] = frag[2]
+			 * iocb->seg[4] = frag[3]
+			 * iocb->seg[5] = frag[4]
+			 * iocb->seg[6] = frag[5]
+			 * iocb->seg[7] = ptr to OAL (external sglist)
+			 * oal->seg[0] = frag[6]
+			 * oal->seg[1] = frag[7]
+			 * oal->seg[2] = frag[8]
+			 * oal->seg[3] = frag[9]
+			 * oal->seg[4] = frag[10]
+			 *      etc...
+			 */
+			/* Tack on the OAL in the eighth segment of IOCB. */
+			map = dma_map_single(&qdev->pdev->dev, &tx_ring_desc->oal,
+					     sizeof(struct qlge_oal),
+					     DMA_TO_DEVICE);
+			err = dma_mapping_error(&qdev->pdev->dev, map);
+			if (err) {
+				netif_err(qdev, tx_queued, qdev->ndev,
+					  "PCI mapping outbound address list with error: %d\n",
+					  err);
+				goto map_error;
+			}
+
+			tbd->addr = cpu_to_le64(map);
+			/*
+			 * The length is the number of fragments
+			 * that remain to be mapped times the length
+			 * of our sglist (OAL).
+			 */
+			tbd->len =
+			    cpu_to_le32((sizeof(struct tx_buf_desc) *
+					 (frag_cnt - frag_idx)) | TX_DESC_C);
+			dma_unmap_addr_set(&tx_ring_desc->map[map_idx], mapaddr,
+					   map);
+			dma_unmap_len_set(&tx_ring_desc->map[map_idx], maplen,
+					  sizeof(struct qlge_oal));
+			tbd = (struct tx_buf_desc *)&tx_ring_desc->oal;
+			map_idx++;
+		}
+
+		map = skb_frag_dma_map(&qdev->pdev->dev, frag, 0, skb_frag_size(frag),
+				       DMA_TO_DEVICE);
+
+		err = dma_mapping_error(&qdev->pdev->dev, map);
+		if (err) {
+			netif_err(qdev, tx_queued, qdev->ndev,
+				  "PCI mapping frags failed with error: %d.\n",
+				  err);
+			goto map_error;
+		}
+
+		tbd->addr = cpu_to_le64(map);
+		tbd->len = cpu_to_le32(skb_frag_size(frag));
+		dma_unmap_addr_set(&tx_ring_desc->map[map_idx], mapaddr, map);
+		dma_unmap_len_set(&tx_ring_desc->map[map_idx], maplen,
+				  skb_frag_size(frag));
+	}
+	/* Save the number of segments we've mapped. */
+	tx_ring_desc->map_cnt = map_idx;
+	/* Terminate the last segment. */
+	tbd->len = cpu_to_le32(le32_to_cpu(tbd->len) | TX_DESC_E);
+	return NETDEV_TX_OK;
+
+map_error:
+	/*
+	 * If the first frag mapping failed, then i will be zero.
+	 * This causes the unmap of the skb->data area.  Otherwise
+	 * we pass in the number of frags that mapped successfully
+	 * so they can be umapped.
+	 */
+	qlge_unmap_send(qdev, tx_ring_desc, map_idx);
+	return NETDEV_TX_BUSY;
+}
+
+/* Categorizing receive firmware frame errors */
+static void qlge_categorize_rx_err(struct qlge_adapter *qdev, u8 rx_err,
+				   struct rx_ring *rx_ring)
+{
+	struct nic_stats *stats = &qdev->nic_stats;
+
+	stats->rx_err_count++;
+	rx_ring->rx_errors++;
+
+	switch (rx_err & IB_MAC_IOCB_RSP_ERR_MASK) {
+	case IB_MAC_IOCB_RSP_ERR_CODE_ERR:
+		stats->rx_code_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_OVERSIZE:
+		stats->rx_oversize_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_UNDERSIZE:
+		stats->rx_undersize_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_PREAMBLE:
+		stats->rx_preamble_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_FRAME_LEN:
+		stats->rx_frame_len_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_CRC:
+		stats->rx_crc_err++;
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * qlge_update_mac_hdr_len - helper routine to update the mac header length
+ * based on vlan tags if present
+ */
+static void qlge_update_mac_hdr_len(struct qlge_adapter *qdev,
+				    struct qlge_ib_mac_iocb_rsp *ib_mac_rsp,
+				    void *page, size_t *len)
+{
+	u16 *tags;
+
+	if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)
+		return;
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_V) {
+		tags = (u16 *)page;
+		/* Look for stacked vlan tags in ethertype field */
+		if (tags[6] == ETH_P_8021Q &&
+		    tags[8] == ETH_P_8021Q)
+			*len += 2 * VLAN_HLEN;
+		else
+			*len += VLAN_HLEN;
+	}
+}
+
+/* Process an inbound completion from an rx ring. */
+static void qlge_process_mac_rx_gro_page(struct qlge_adapter *qdev,
+					 struct rx_ring *rx_ring,
+					 struct qlge_ib_mac_iocb_rsp *ib_mac_rsp,
+					 u32 length, u16 vlan_id)
+{
+	struct sk_buff *skb;
+	struct qlge_bq_desc *lbq_desc = qlge_get_curr_lchunk(qdev, rx_ring);
+	struct napi_struct *napi = &rx_ring->napi;
+
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		qlge_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		put_page(lbq_desc->p.pg_chunk.page);
+		return;
+	}
+	napi->dev = qdev->ndev;
+
+	skb = napi_get_frags(napi);
+	if (!skb) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Couldn't get an skb, exiting.\n");
+		rx_ring->rx_dropped++;
+		put_page(lbq_desc->p.pg_chunk.page);
+		return;
+	}
+	prefetch(lbq_desc->p.pg_chunk.va);
+	__skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
+			     lbq_desc->p.pg_chunk.page,
+			     lbq_desc->p.pg_chunk.offset,
+			     length);
+
+	skb->len += length;
+	skb->data_len += length;
+	skb->truesize += length;
+	skb_shinfo(skb)->nr_frags++;
+
+	rx_ring->rx_packets++;
+	rx_ring->rx_bytes += length;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb_record_rx_queue(skb, rx_ring->cq_id);
+	if (vlan_id != 0xffff)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
+	napi_gro_frags(napi);
+}
+
+/* Process an inbound completion from an rx ring. */
+static void qlge_process_mac_rx_page(struct qlge_adapter *qdev,
+				     struct rx_ring *rx_ring,
+				     struct qlge_ib_mac_iocb_rsp *ib_mac_rsp,
+				     u32 length, u16 vlan_id)
+{
+	struct net_device *ndev = qdev->ndev;
+	struct sk_buff *skb = NULL;
+	void *addr;
+	struct qlge_bq_desc *lbq_desc = qlge_get_curr_lchunk(qdev, rx_ring);
+	struct napi_struct *napi = &rx_ring->napi;
+	size_t hlen = ETH_HLEN;
+
+	skb = netdev_alloc_skb(ndev, length);
+	if (!skb) {
+		rx_ring->rx_dropped++;
+		put_page(lbq_desc->p.pg_chunk.page);
+		return;
+	}
+
+	addr = lbq_desc->p.pg_chunk.va;
+	prefetch(addr);
+
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		qlge_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		goto err_out;
+	}
+
+	/* Update the MAC header length*/
+	qlge_update_mac_hdr_len(qdev, ib_mac_rsp, addr, &hlen);
+
+	/* The max framesize filter on this chip is set higher than
+	 * MTU since FCoE uses 2k frames.
+	 */
+	if (skb->len > ndev->mtu + hlen) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Segment too small, dropping.\n");
+		rx_ring->rx_dropped++;
+		goto err_out;
+	}
+	skb_put_data(skb, addr, hlen);
+	netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+		     "%d bytes of headers and data in large. Chain page to new skb and pull tail.\n",
+		     length);
+	skb_fill_page_desc(skb, 0, lbq_desc->p.pg_chunk.page,
+			   lbq_desc->p.pg_chunk.offset + hlen, length - hlen);
+	skb->len += length - hlen;
+	skb->data_len += length - hlen;
+	skb->truesize += length - hlen;
+
+	rx_ring->rx_packets++;
+	rx_ring->rx_bytes += skb->len;
+	skb->protocol = eth_type_trans(skb, ndev);
+	skb_checksum_none_assert(skb);
+
+	if ((ndev->features & NETIF_F_RXCSUM) &&
+	    !(ib_mac_rsp->flags1 & IB_MAC_CSUM_ERR_MASK)) {
+		/* TCP frame. */
+		if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_T) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "TCP checksum done!\n");
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+		} else if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_U) &&
+			   (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_V4)) {
+			/* Unfragmented ipv4 UDP frame. */
+			struct iphdr *iph =
+				(struct iphdr *)((u8 *)addr + hlen);
+			if (!(iph->frag_off &
+			      htons(IP_MF | IP_OFFSET))) {
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+				netif_printk(qdev, rx_status, KERN_DEBUG,
+					     qdev->ndev,
+					     "UDP checksum done!\n");
+			}
+		}
+	}
+
+	skb_record_rx_queue(skb, rx_ring->cq_id);
+	if (vlan_id != 0xffff)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
+	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
+		napi_gro_receive(napi, skb);
+	else
+		netif_receive_skb(skb);
+	return;
+err_out:
+	dev_kfree_skb_any(skb);
+	put_page(lbq_desc->p.pg_chunk.page);
+}
+
+/* Process an inbound completion from an rx ring. */
+static void qlge_process_mac_rx_skb(struct qlge_adapter *qdev,
+				    struct rx_ring *rx_ring,
+				    struct qlge_ib_mac_iocb_rsp *ib_mac_rsp,
+				    u32 length, u16 vlan_id)
+{
+	struct qlge_bq_desc *sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
+	struct net_device *ndev = qdev->ndev;
+	struct sk_buff *skb, *new_skb;
+
+	skb = sbq_desc->p.skb;
+	/* Allocate new_skb and copy */
+	new_skb = netdev_alloc_skb(qdev->ndev, length + NET_IP_ALIGN);
+	if (!new_skb) {
+		rx_ring->rx_dropped++;
+		return;
+	}
+	skb_reserve(new_skb, NET_IP_ALIGN);
+
+	dma_sync_single_for_cpu(&qdev->pdev->dev, sbq_desc->dma_addr,
+				SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
+
+	skb_put_data(new_skb, skb->data, length);
+
+	skb = new_skb;
+
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		qlge_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
+	/* loopback self test for ethtool */
+	if (test_bit(QL_SELFTEST, &qdev->flags)) {
+		qlge_check_lb_frame(qdev, skb);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
+	/* The max framesize filter on this chip is set higher than
+	 * MTU since FCoE uses 2k frames.
+	 */
+	if (skb->len > ndev->mtu + ETH_HLEN) {
+		dev_kfree_skb_any(skb);
+		rx_ring->rx_dropped++;
+		return;
+	}
+
+	prefetch(skb->data);
+	if (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "%s Multicast.\n",
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_HASH ? "Hash" :
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_REG ? "Registered" :
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_PROM ? "Promiscuous" : "");
+	}
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_P)
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "Promiscuous Packet.\n");
+
+	rx_ring->rx_packets++;
+	rx_ring->rx_bytes += skb->len;
+	skb->protocol = eth_type_trans(skb, ndev);
+	skb_checksum_none_assert(skb);
+
+	/* If rx checksum is on, and there are no
+	 * csum or frame errors.
+	 */
+	if ((ndev->features & NETIF_F_RXCSUM) &&
+	    !(ib_mac_rsp->flags1 & IB_MAC_CSUM_ERR_MASK)) {
+		/* TCP frame. */
+		if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_T) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "TCP checksum done!\n");
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+		} else if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_U) &&
+			   (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_V4)) {
+			/* Unfragmented ipv4 UDP frame. */
+			struct iphdr *iph = (struct iphdr *)skb->data;
+
+			if (!(iph->frag_off &
+			      htons(IP_MF | IP_OFFSET))) {
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+				netif_printk(qdev, rx_status, KERN_DEBUG,
+					     qdev->ndev,
+					     "UDP checksum done!\n");
+			}
+		}
+	}
+
+	skb_record_rx_queue(skb, rx_ring->cq_id);
+	if (vlan_id != 0xffff)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
+	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
+		napi_gro_receive(&rx_ring->napi, skb);
+	else
+		netif_receive_skb(skb);
+}
+
+static void qlge_realign_skb(struct sk_buff *skb, int len)
+{
+	void *temp_addr = skb->data;
+
+	/* Undo the skb_reserve(skb,32) we did before
+	 * giving to hardware, and realign data on
+	 * a 2-byte boundary.
+	 */
+	skb->data -= QLGE_SB_PAD - NET_IP_ALIGN;
+	skb->tail -= QLGE_SB_PAD - NET_IP_ALIGN;
+	memmove(skb->data, temp_addr, len);
+}
+
+/*
+ * This function builds an skb for the given inbound
+ * completion.  It will be rewritten for readability in the near
+ * future, but for not it works well.
+ */
+static struct sk_buff *qlge_build_rx_skb(struct qlge_adapter *qdev,
+					 struct rx_ring *rx_ring,
+					 struct qlge_ib_mac_iocb_rsp *ib_mac_rsp)
+{
+	u32 length = le32_to_cpu(ib_mac_rsp->data_len);
+	u32 hdr_len = le32_to_cpu(ib_mac_rsp->hdr_len);
+	struct qlge_bq_desc *lbq_desc, *sbq_desc;
+	struct sk_buff *skb = NULL;
+	size_t hlen = ETH_HLEN;
+
+	/*
+	 * Handle the header buffer if present.
+	 */
+	if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HV &&
+	    ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HS) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "Header of %d bytes in small buffer.\n", hdr_len);
+		/*
+		 * Headers fit nicely into a small buffer.
+		 */
+		sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
+		dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+				 SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
+		skb = sbq_desc->p.skb;
+		qlge_realign_skb(skb, hdr_len);
+		skb_put(skb, hdr_len);
+		sbq_desc->p.skb = NULL;
+	}
+
+	/*
+	 * Handle the data buffer(s).
+	 */
+	if (unlikely(!length)) {	/* Is there data too? */
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "No Data buffer in this packet.\n");
+		return skb;
+	}
+
+	if (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DS) {
+		if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HS) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Headers in small, data of %d bytes in small, combine them.\n",
+				     length);
+			/*
+			 * Data is less than small buffer size so it's
+			 * stuffed in a small buffer.
+			 * For this case we append the data
+			 * from the "data" small buffer to the "header" small
+			 * buffer.
+			 */
+			sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
+			dma_sync_single_for_cpu(&qdev->pdev->dev,
+						sbq_desc->dma_addr,
+						SMALL_BUF_MAP_SIZE,
+						DMA_FROM_DEVICE);
+			skb_put_data(skb, sbq_desc->p.skb->data, length);
+		} else {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "%d bytes in a single small buffer.\n",
+				     length);
+			sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
+			skb = sbq_desc->p.skb;
+			qlge_realign_skb(skb, length);
+			skb_put(skb, length);
+			dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+					 SMALL_BUF_MAP_SIZE,
+					 DMA_FROM_DEVICE);
+			sbq_desc->p.skb = NULL;
+		}
+	} else if (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DL) {
+		if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HS) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Header in small, %d bytes in large. Chain large to small!\n",
+				     length);
+			/*
+			 * The data is in a single large buffer.  We
+			 * chain it to the header buffer's skb and let
+			 * it rip.
+			 */
+			lbq_desc = qlge_get_curr_lchunk(qdev, rx_ring);
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Chaining page at offset = %d, for %d bytes  to skb.\n",
+				     lbq_desc->p.pg_chunk.offset, length);
+			skb_fill_page_desc(skb, 0, lbq_desc->p.pg_chunk.page,
+					   lbq_desc->p.pg_chunk.offset, length);
+			skb->len += length;
+			skb->data_len += length;
+			skb->truesize += length;
+		} else {
+			/*
+			 * The headers and data are in a single large buffer. We
+			 * copy it to a new skb and let it go. This can happen with
+			 * jumbo mtu on a non-TCP/UDP frame.
+			 */
+			lbq_desc = qlge_get_curr_lchunk(qdev, rx_ring);
+			skb = netdev_alloc_skb(qdev->ndev, length);
+			if (!skb) {
+				netif_printk(qdev, probe, KERN_DEBUG, qdev->ndev,
+					     "No skb available, drop the packet.\n");
+				return NULL;
+			}
+			dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
+				       qdev->lbq_buf_size,
+				       DMA_FROM_DEVICE);
+			skb_reserve(skb, NET_IP_ALIGN);
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "%d bytes of headers and data in large. Chain page to new skb and pull tail.\n",
+				     length);
+			skb_fill_page_desc(skb, 0, lbq_desc->p.pg_chunk.page,
+					   lbq_desc->p.pg_chunk.offset,
+					   length);
+			skb->len += length;
+			skb->data_len += length;
+			skb->truesize += length;
+			qlge_update_mac_hdr_len(qdev, ib_mac_rsp,
+						lbq_desc->p.pg_chunk.va,
+						&hlen);
+			__pskb_pull_tail(skb, hlen);
+		}
+	} else {
+		/*
+		 * The data is in a chain of large buffers
+		 * pointed to by a small buffer.  We loop
+		 * thru and chain them to the our small header
+		 * buffer's skb.
+		 * frags:  There are 18 max frags and our small
+		 *         buffer will hold 32 of them. The thing is,
+		 *         we'll use 3 max for our 9000 byte jumbo
+		 *         frames.  If the MTU goes up we could
+		 *          eventually be in trouble.
+		 */
+		int size, i = 0;
+
+		sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
+		dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+				 SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
+		if (!(ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HS)) {
+			/*
+			 * This is an non TCP/UDP IP frame, so
+			 * the headers aren't split into a small
+			 * buffer.  We have to use the small buffer
+			 * that contains our sg list as our skb to
+			 * send upstairs. Copy the sg list here to
+			 * a local buffer and use it to find the
+			 * pages to chain.
+			 */
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "%d bytes of headers & data in chain of large.\n",
+				     length);
+			skb = sbq_desc->p.skb;
+			sbq_desc->p.skb = NULL;
+			skb_reserve(skb, NET_IP_ALIGN);
+		}
+		do {
+			lbq_desc = qlge_get_curr_lchunk(qdev, rx_ring);
+			size = min(length, qdev->lbq_buf_size);
+
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Adding page %d to skb for %d bytes.\n",
+				     i, size);
+			skb_fill_page_desc(skb, i,
+					   lbq_desc->p.pg_chunk.page,
+					   lbq_desc->p.pg_chunk.offset, size);
+			skb->len += size;
+			skb->data_len += size;
+			skb->truesize += size;
+			length -= size;
+			i++;
+		} while (length > 0);
+		qlge_update_mac_hdr_len(qdev, ib_mac_rsp, lbq_desc->p.pg_chunk.va,
+					&hlen);
+		__pskb_pull_tail(skb, hlen);
+	}
+	return skb;
+}
+
+/* Process an inbound completion from an rx ring. */
+static void qlge_process_mac_split_rx_intr(struct qlge_adapter *qdev,
+					   struct rx_ring *rx_ring,
+					   struct qlge_ib_mac_iocb_rsp *ib_mac_rsp,
+					   u16 vlan_id)
+{
+	struct net_device *ndev = qdev->ndev;
+	struct sk_buff *skb = NULL;
+
+	skb = qlge_build_rx_skb(qdev, rx_ring, ib_mac_rsp);
+	if (unlikely(!skb)) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "No skb available, drop packet.\n");
+		rx_ring->rx_dropped++;
+		return;
+	}
+
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		qlge_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
+	/* The max framesize filter on this chip is set higher than
+	 * MTU since FCoE uses 2k frames.
+	 */
+	if (skb->len > ndev->mtu + ETH_HLEN) {
+		dev_kfree_skb_any(skb);
+		rx_ring->rx_dropped++;
+		return;
+	}
+
+	/* loopback self test for ethtool */
+	if (test_bit(QL_SELFTEST, &qdev->flags)) {
+		qlge_check_lb_frame(qdev, skb);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
+	prefetch(skb->data);
+	if (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev, "%s Multicast.\n",
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_HASH ? "Hash" :
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_REG ? "Registered" :
+			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
+			     IB_MAC_IOCB_RSP_M_PROM ? "Promiscuous" : "");
+		rx_ring->rx_multicast++;
+	}
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_P) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "Promiscuous Packet.\n");
+	}
+
+	skb->protocol = eth_type_trans(skb, ndev);
+	skb_checksum_none_assert(skb);
+
+	/* If rx checksum is on, and there are no
+	 * csum or frame errors.
+	 */
+	if ((ndev->features & NETIF_F_RXCSUM) &&
+	    !(ib_mac_rsp->flags1 & IB_MAC_CSUM_ERR_MASK)) {
+		/* TCP frame. */
+		if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_T) {
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "TCP checksum done!\n");
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+		} else if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_U) &&
+			   (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_V4)) {
+			/* Unfragmented ipv4 UDP frame. */
+			struct iphdr *iph = (struct iphdr *)skb->data;
+
+			if (!(iph->frag_off &
+			      htons(IP_MF | IP_OFFSET))) {
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+				netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+					     "TCP checksum done!\n");
+			}
+		}
+	}
+
+	rx_ring->rx_packets++;
+	rx_ring->rx_bytes += skb->len;
+	skb_record_rx_queue(skb, rx_ring->cq_id);
+	if (vlan_id != 0xffff)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
+	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
+		napi_gro_receive(&rx_ring->napi, skb);
+	else
+		netif_receive_skb(skb);
+}
+
+/* Process an inbound completion from an rx ring. */
+static unsigned long qlge_process_mac_rx_intr(struct qlge_adapter *qdev,
+					      struct rx_ring *rx_ring,
+					      struct qlge_ib_mac_iocb_rsp *ib_mac_rsp)
+{
+	u32 length = le32_to_cpu(ib_mac_rsp->data_len);
+	u16 vlan_id = ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_V) &&
+		       (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)) ?
+		((le16_to_cpu(ib_mac_rsp->vlan_id) &
+		  IB_MAC_IOCB_RSP_VLAN_MASK)) : 0xffff;
+
+	if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HV) {
+		/* The data and headers are split into
+		 * separate buffers.
+		 */
+		qlge_process_mac_split_rx_intr(qdev, rx_ring, ib_mac_rsp,
+					       vlan_id);
+	} else if (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DS) {
+		/* The data fit in a single small buffer.
+		 * Allocate a new skb, copy the data and
+		 * return the buffer to the free pool.
+		 */
+		qlge_process_mac_rx_skb(qdev, rx_ring, ib_mac_rsp, length,
+					vlan_id);
+	} else if ((ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DL) &&
+		   !(ib_mac_rsp->flags1 & IB_MAC_CSUM_ERR_MASK) &&
+		   (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_T)) {
+		/* TCP packet in a page chunk that's been checksummed.
+		 * Tack it on to our GRO skb and let it go.
+		 */
+		qlge_process_mac_rx_gro_page(qdev, rx_ring, ib_mac_rsp, length,
+					     vlan_id);
+	} else if (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DL) {
+		/* Non-TCP packet in a page chunk. Allocate an
+		 * skb, tack it on frags, and send it up.
+		 */
+		qlge_process_mac_rx_page(qdev, rx_ring, ib_mac_rsp, length,
+					 vlan_id);
+	} else {
+		/* Non-TCP/UDP large frames that span multiple buffers
+		 * can be processed correctly by the split frame logic.
+		 */
+		qlge_process_mac_split_rx_intr(qdev, rx_ring, ib_mac_rsp,
+					       vlan_id);
+	}
+
+	return (unsigned long)length;
+}
+
+/* Process an outbound completion from an rx ring. */
+static void qlge_process_mac_tx_intr(struct qlge_adapter *qdev,
+				     struct qlge_ob_mac_iocb_rsp *mac_rsp)
+{
+	struct tx_ring *tx_ring;
+	struct tx_ring_desc *tx_ring_desc;
+
+	tx_ring = &qdev->tx_ring[mac_rsp->txq_idx];
+	tx_ring_desc = &tx_ring->q[mac_rsp->tid];
+	qlge_unmap_send(qdev, tx_ring_desc, tx_ring_desc->map_cnt);
+	tx_ring->tx_bytes += (tx_ring_desc->skb)->len;
+	tx_ring->tx_packets++;
+	dev_kfree_skb(tx_ring_desc->skb);
+	tx_ring_desc->skb = NULL;
+
+	if (unlikely(mac_rsp->flags1 & (OB_MAC_IOCB_RSP_E |
+					OB_MAC_IOCB_RSP_S |
+					OB_MAC_IOCB_RSP_L |
+					OB_MAC_IOCB_RSP_P | OB_MAC_IOCB_RSP_B))) {
+		if (mac_rsp->flags1 & OB_MAC_IOCB_RSP_E) {
+			netif_warn(qdev, tx_done, qdev->ndev,
+				   "Total descriptor length did not match transfer length.\n");
+		}
+		if (mac_rsp->flags1 & OB_MAC_IOCB_RSP_S) {
+			netif_warn(qdev, tx_done, qdev->ndev,
+				   "Frame too short to be valid, not sent.\n");
+		}
+		if (mac_rsp->flags1 & OB_MAC_IOCB_RSP_L) {
+			netif_warn(qdev, tx_done, qdev->ndev,
+				   "Frame too long, but sent anyway.\n");
+		}
+		if (mac_rsp->flags1 & OB_MAC_IOCB_RSP_B) {
+			netif_warn(qdev, tx_done, qdev->ndev,
+				   "PCI backplane error. Frame not sent.\n");
+		}
+	}
+	atomic_inc(&tx_ring->tx_count);
+}
+
+/* Fire up a handler to reset the MPI processor. */
+void qlge_queue_fw_error(struct qlge_adapter *qdev)
+{
+	qlge_link_off(qdev);
+	queue_delayed_work(qdev->workqueue, &qdev->mpi_reset_work, 0);
+}
+
+void qlge_queue_asic_error(struct qlge_adapter *qdev)
+{
+	qlge_link_off(qdev);
+	qlge_disable_interrupts(qdev);
+	/* Clear adapter up bit to signal the recovery
+	 * process that it shouldn't kill the reset worker
+	 * thread
+	 */
+	clear_bit(QL_ADAPTER_UP, &qdev->flags);
+	/* Set asic recovery bit to indicate reset process that we are
+	 * in fatal error recovery process rather than normal close
+	 */
+	set_bit(QL_ASIC_RECOVERY, &qdev->flags);
+	queue_delayed_work(qdev->workqueue, &qdev->asic_reset_work, 0);
+}
+
+static void qlge_process_chip_ae_intr(struct qlge_adapter *qdev,
+				      struct qlge_ib_ae_iocb_rsp *ib_ae_rsp)
+{
+	switch (ib_ae_rsp->event) {
+	case MGMT_ERR_EVENT:
+		netif_err(qdev, rx_err, qdev->ndev,
+			  "Management Processor Fatal Error.\n");
+		qlge_queue_fw_error(qdev);
+		return;
+
+	case CAM_LOOKUP_ERR_EVENT:
+		netdev_err(qdev->ndev, "Multiple CAM hits lookup occurred.\n");
+		netdev_err(qdev->ndev, "This event shouldn't occur.\n");
+		qlge_queue_asic_error(qdev);
+		return;
+
+	case SOFT_ECC_ERROR_EVENT:
+		netdev_err(qdev->ndev, "Soft ECC error detected.\n");
+		qlge_queue_asic_error(qdev);
+		break;
+
+	case PCI_ERR_ANON_BUF_RD:
+		netdev_err(qdev->ndev,
+			   "PCI error occurred when reading anonymous buffers from rx_ring %d.\n",
+			   ib_ae_rsp->q_id);
+		qlge_queue_asic_error(qdev);
+		break;
+
+	default:
+		netif_err(qdev, drv, qdev->ndev, "Unexpected event %d.\n",
+			  ib_ae_rsp->event);
+		qlge_queue_asic_error(qdev);
+		break;
+	}
+}
+
+static int qlge_clean_outbound_rx_ring(struct rx_ring *rx_ring)
+{
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	u32 prod = qlge_read_sh_reg(rx_ring->prod_idx_sh_reg);
+	struct qlge_ob_mac_iocb_rsp *net_rsp = NULL;
+	int count = 0;
+
+	struct tx_ring *tx_ring;
+	/* While there are entries in the completion queue. */
+	while (prod != rx_ring->cnsmr_idx) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "cq_id = %d, prod = %d, cnsmr = %d\n",
+			     rx_ring->cq_id, prod, rx_ring->cnsmr_idx);
+
+		net_rsp = (struct qlge_ob_mac_iocb_rsp *)rx_ring->curr_entry;
+		rmb();
+		switch (net_rsp->opcode) {
+		case OPCODE_OB_MAC_TSO_IOCB:
+		case OPCODE_OB_MAC_IOCB:
+			qlge_process_mac_tx_intr(qdev, net_rsp);
+			break;
+		default:
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Hit default case, not handled! dropping the packet, opcode = %x.\n",
+				     net_rsp->opcode);
+		}
+		count++;
+		qlge_update_cq(rx_ring);
+		prod = qlge_read_sh_reg(rx_ring->prod_idx_sh_reg);
+	}
+	if (!net_rsp)
+		return 0;
+	qlge_write_cq_idx(rx_ring);
+	tx_ring = &qdev->tx_ring[net_rsp->txq_idx];
+	if (__netif_subqueue_stopped(qdev->ndev, tx_ring->wq_id)) {
+		if ((atomic_read(&tx_ring->tx_count) > (tx_ring->wq_len / 4)))
+			/*
+			 * The queue got stopped because the tx_ring was full.
+			 * Wake it up, because it's now at least 25% empty.
+			 */
+			netif_wake_subqueue(qdev->ndev, tx_ring->wq_id);
+	}
+
+	return count;
+}
+
+static int qlge_clean_inbound_rx_ring(struct rx_ring *rx_ring, int budget)
+{
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	u32 prod = qlge_read_sh_reg(rx_ring->prod_idx_sh_reg);
+	struct qlge_net_rsp_iocb *net_rsp;
+	int count = 0;
+
+	/* While there are entries in the completion queue. */
+	while (prod != rx_ring->cnsmr_idx) {
+		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+			     "cq_id = %d, prod = %d, cnsmr = %d\n",
+			     rx_ring->cq_id, prod, rx_ring->cnsmr_idx);
+
+		net_rsp = rx_ring->curr_entry;
+		rmb();
+		switch (net_rsp->opcode) {
+		case OPCODE_IB_MAC_IOCB:
+			qlge_process_mac_rx_intr(qdev, rx_ring,
+						 (struct qlge_ib_mac_iocb_rsp *)
+						 net_rsp);
+			break;
+
+		case OPCODE_IB_AE_IOCB:
+			qlge_process_chip_ae_intr(qdev, (struct qlge_ib_ae_iocb_rsp *)
+						  net_rsp);
+			break;
+		default:
+			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+				     "Hit default case, not handled! dropping the packet, opcode = %x.\n",
+				     net_rsp->opcode);
+			break;
+		}
+		count++;
+		qlge_update_cq(rx_ring);
+		prod = qlge_read_sh_reg(rx_ring->prod_idx_sh_reg);
+		if (count == budget)
+			break;
+	}
+	qlge_update_buffer_queues(rx_ring, GFP_ATOMIC, 0);
+	qlge_write_cq_idx(rx_ring);
+	return count;
+}
+
+static int qlge_napi_poll_msix(struct napi_struct *napi, int budget)
+{
+	struct rx_ring *rx_ring = container_of(napi, struct rx_ring, napi);
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct rx_ring *trx_ring;
+	int i, work_done = 0;
+	struct intr_context *ctx = &qdev->intr_context[rx_ring->cq_id];
+
+	netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
+		     "Enter, NAPI POLL cq_id = %d.\n", rx_ring->cq_id);
+
+	/* Service the TX rings first.  They start
+	 * right after the RSS rings.
+	 */
+	for (i = qdev->rss_ring_count; i < qdev->rx_ring_count; i++) {
+		trx_ring = &qdev->rx_ring[i];
+		/* If this TX completion ring belongs to this vector and
+		 * it's not empty then service it.
+		 */
+		if ((ctx->irq_mask & (1 << trx_ring->cq_id)) &&
+		    (qlge_read_sh_reg(trx_ring->prod_idx_sh_reg) !=
+		     trx_ring->cnsmr_idx)) {
+			netif_printk(qdev, intr, KERN_DEBUG, qdev->ndev,
+				     "%s: Servicing TX completion ring %d.\n",
+				     __func__, trx_ring->cq_id);
+			qlge_clean_outbound_rx_ring(trx_ring);
+		}
+	}
+
+	/*
+	 * Now service the RSS ring if it's active.
+	 */
+	if (qlge_read_sh_reg(rx_ring->prod_idx_sh_reg) !=
+	    rx_ring->cnsmr_idx) {
+		netif_printk(qdev, intr, KERN_DEBUG, qdev->ndev,
+			     "%s: Servicing RX completion ring %d.\n",
+			     __func__, rx_ring->cq_id);
+		work_done = qlge_clean_inbound_rx_ring(rx_ring, budget);
+	}
+
+	if (work_done < budget) {
+		napi_complete_done(napi, work_done);
+		qlge_enable_completion_interrupt(qdev, rx_ring->irq);
+	}
+	return work_done;
+}
+
+static void qlge_vlan_mode(struct net_device *ndev, netdev_features_t features)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
+		qlge_write32(qdev, NIC_RCV_CFG, NIC_RCV_CFG_VLAN_MASK |
+			     NIC_RCV_CFG_VLAN_MATCH_AND_NON);
+	} else {
+		qlge_write32(qdev, NIC_RCV_CFG, NIC_RCV_CFG_VLAN_MASK);
+	}
+}
+
+/*
+ * qlge_update_hw_vlan_features - helper routine to reinitialize the adapter
+ * based on the features to enable/disable hardware vlan accel
+ */
+static int qlge_update_hw_vlan_features(struct net_device *ndev,
+					netdev_features_t features)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	bool need_restart = netif_running(ndev);
+	int status = 0;
+
+	if (need_restart) {
+		status = qlge_adapter_down(qdev);
+		if (status) {
+			netif_err(qdev, link, qdev->ndev,
+				  "Failed to bring down the adapter\n");
+			return status;
+		}
+	}
+
+	/* update the features with resent change */
+	ndev->features = features;
+
+	if (need_restart) {
+		status = qlge_adapter_up(qdev);
+		if (status) {
+			netif_err(qdev, link, qdev->ndev,
+				  "Failed to bring up the adapter\n");
+			return status;
+		}
+	}
+
+	return status;
+}
+
+static int qlge_set_features(struct net_device *ndev,
+			     netdev_features_t features)
+{
+	netdev_features_t changed = ndev->features ^ features;
+	int err;
+
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX) {
+		/* Update the behavior of vlan accel in the adapter */
+		err = qlge_update_hw_vlan_features(ndev, features);
+		if (err)
+			return err;
+
+		qlge_vlan_mode(ndev, features);
+	}
+
+	return 0;
+}
+
+static int __qlge_vlan_rx_add_vid(struct qlge_adapter *qdev, u16 vid)
+{
+	u32 enable_bit = MAC_ADDR_E;
+	int err;
+
+	err = qlge_set_mac_addr_reg(qdev, (u8 *)&enable_bit,
+				    MAC_ADDR_TYPE_VLAN, vid);
+	if (err)
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init vlan address.\n");
+	return err;
+}
+
+static int qlge_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int status;
+	int err;
+
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return status;
+
+	err = __qlge_vlan_rx_add_vid(qdev, vid);
+	set_bit(vid, qdev->active_vlans);
+
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+
+	return err;
+}
+
+static int __qlge_vlan_rx_kill_vid(struct qlge_adapter *qdev, u16 vid)
+{
+	u32 enable_bit = 0;
+	int err;
+
+	err = qlge_set_mac_addr_reg(qdev, (u8 *)&enable_bit,
+				    MAC_ADDR_TYPE_VLAN, vid);
+	if (err)
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to clear vlan address.\n");
+	return err;
+}
+
+static int qlge_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int status;
+	int err;
+
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return status;
+
+	err = __qlge_vlan_rx_kill_vid(qdev, vid);
+	clear_bit(vid, qdev->active_vlans);
+
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+
+	return err;
+}
+
+static void qlge_restore_vlan(struct qlge_adapter *qdev)
+{
+	int status;
+	u16 vid;
+
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return;
+
+	for_each_set_bit(vid, qdev->active_vlans, VLAN_N_VID)
+		__qlge_vlan_rx_add_vid(qdev, vid);
+
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+}
+
+/* MSI-X Multiple Vector Interrupt Handler for inbound completions. */
+static irqreturn_t qlge_msix_rx_isr(int irq, void *dev_id)
+{
+	struct rx_ring *rx_ring = dev_id;
+
+	napi_schedule(&rx_ring->napi);
+	return IRQ_HANDLED;
+}
+
+/* This handles a fatal error, MPI activity, and the default
+ * rx_ring in an MSI-X multiple vector environment.
+ * In MSI/Legacy environment it also process the rest of
+ * the rx_rings.
+ */
+static irqreturn_t qlge_isr(int irq, void *dev_id)
+{
+	struct rx_ring *rx_ring = dev_id;
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct intr_context *intr_context = &qdev->intr_context[0];
+	u32 var;
+	int work_done = 0;
+
+	/* Experience shows that when using INTx interrupts, interrupts must
+	 * be masked manually.
+	 * When using MSI mode, INTR_EN_EN must be explicitly disabled
+	 * (even though it is auto-masked), otherwise a later command to
+	 * enable it is not effective.
+	 */
+	if (!test_bit(QL_MSIX_ENABLED, &qdev->flags))
+		qlge_disable_completion_interrupt(qdev, 0);
+
+	var = qlge_read32(qdev, STS);
+
+	/*
+	 * Check for fatal error.
+	 */
+	if (var & STS_FE) {
+		qlge_disable_completion_interrupt(qdev, 0);
+		qlge_queue_asic_error(qdev);
+		netdev_err(qdev->ndev, "Got fatal error, STS = %x.\n", var);
+		var = qlge_read32(qdev, ERR_STS);
+		netdev_err(qdev->ndev, "Resetting chip. Error Status Register = 0x%x\n", var);
+		return IRQ_HANDLED;
+	}
+
+	/*
+	 * Check MPI processor activity.
+	 */
+	if ((var & STS_PI) &&
+	    (qlge_read32(qdev, INTR_MASK) & INTR_MASK_PI)) {
+		/*
+		 * We've got an async event or mailbox completion.
+		 * Handle it and clear the source of the interrupt.
+		 */
+		netif_err(qdev, intr, qdev->ndev,
+			  "Got MPI processor interrupt.\n");
+		qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16));
+		queue_delayed_work_on(smp_processor_id(),
+				      qdev->workqueue, &qdev->mpi_work, 0);
+		work_done++;
+	}
+
+	/*
+	 * Get the bit-mask that shows the active queues for this
+	 * pass.  Compare it to the queues that this irq services
+	 * and call napi if there's a match.
+	 */
+	var = qlge_read32(qdev, ISR1);
+	if (var & intr_context->irq_mask) {
+		netif_info(qdev, intr, qdev->ndev,
+			   "Waking handler for rx_ring[0].\n");
+		napi_schedule(&rx_ring->napi);
+		work_done++;
+	} else {
+		/* Experience shows that the device sometimes signals an
+		 * interrupt but no work is scheduled from this function.
+		 * Nevertheless, the interrupt is auto-masked. Therefore, we
+		 * systematically re-enable the interrupt if we didn't
+		 * schedule napi.
+		 */
+		qlge_enable_completion_interrupt(qdev, 0);
+	}
+
+	return work_done ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static int qlge_tso(struct sk_buff *skb, struct qlge_ob_mac_tso_iocb_req *mac_iocb_ptr)
+{
+	if (skb_is_gso(skb)) {
+		int err;
+		__be16 l3_proto = vlan_get_protocol(skb);
+
+		err = skb_cow_head(skb, 0);
+		if (err < 0)
+			return err;
+
+		mac_iocb_ptr->opcode = OPCODE_OB_MAC_TSO_IOCB;
+		mac_iocb_ptr->flags3 |= OB_MAC_TSO_IOCB_IC;
+		mac_iocb_ptr->frame_len = cpu_to_le32((u32)skb->len);
+		mac_iocb_ptr->total_hdrs_len =
+			cpu_to_le16(skb_tcp_all_headers(skb));
+		mac_iocb_ptr->net_trans_offset =
+			cpu_to_le16(skb_network_offset(skb) |
+				    skb_transport_offset(skb)
+				    << OB_MAC_TRANSPORT_HDR_SHIFT);
+		mac_iocb_ptr->mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
+		mac_iocb_ptr->flags2 |= OB_MAC_TSO_IOCB_LSO;
+		if (likely(l3_proto == htons(ETH_P_IP))) {
+			struct iphdr *iph = ip_hdr(skb);
+
+			iph->check = 0;
+			mac_iocb_ptr->flags1 |= OB_MAC_TSO_IOCB_IP4;
+			tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr,
+								 iph->daddr, 0,
+								 IPPROTO_TCP,
+								 0);
+		} else if (l3_proto == htons(ETH_P_IPV6)) {
+			mac_iocb_ptr->flags1 |= OB_MAC_TSO_IOCB_IP6;
+			tcp_hdr(skb)->check =
+				~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
+						 &ipv6_hdr(skb)->daddr,
+						 0, IPPROTO_TCP, 0);
+		}
+		return 1;
+	}
+	return 0;
+}
+
+static void qlge_hw_csum_setup(struct sk_buff *skb,
+			       struct qlge_ob_mac_tso_iocb_req *mac_iocb_ptr)
+{
+	int len;
+	struct iphdr *iph = ip_hdr(skb);
+	__sum16 *check;
+
+	mac_iocb_ptr->opcode = OPCODE_OB_MAC_TSO_IOCB;
+	mac_iocb_ptr->frame_len = cpu_to_le32((u32)skb->len);
+	mac_iocb_ptr->net_trans_offset =
+		cpu_to_le16(skb_network_offset(skb) |
+			    skb_transport_offset(skb) << OB_MAC_TRANSPORT_HDR_SHIFT);
+
+	mac_iocb_ptr->flags1 |= OB_MAC_TSO_IOCB_IP4;
+	len = (ntohs(iph->tot_len) - (iph->ihl << 2));
+	if (likely(iph->protocol == IPPROTO_TCP)) {
+		check = &(tcp_hdr(skb)->check);
+		mac_iocb_ptr->flags2 |= OB_MAC_TSO_IOCB_TC;
+		mac_iocb_ptr->total_hdrs_len =
+			cpu_to_le16(skb_transport_offset(skb) +
+				    (tcp_hdr(skb)->doff << 2));
+	} else {
+		check = &(udp_hdr(skb)->check);
+		mac_iocb_ptr->flags2 |= OB_MAC_TSO_IOCB_UC;
+		mac_iocb_ptr->total_hdrs_len =
+			cpu_to_le16(skb_transport_offset(skb) +
+				    sizeof(struct udphdr));
+	}
+	*check = ~csum_tcpudp_magic(iph->saddr,
+				    iph->daddr, len, iph->protocol, 0);
+}
+
+static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	struct qlge_ob_mac_iocb_req *mac_iocb_ptr;
+	struct tx_ring_desc *tx_ring_desc;
+	int tso;
+	struct tx_ring *tx_ring;
+	u32 tx_ring_idx = (u32)skb->queue_mapping;
+
+	tx_ring = &qdev->tx_ring[tx_ring_idx];
+
+	if (skb_padto(skb, ETH_ZLEN))
+		return NETDEV_TX_OK;
+
+	if (unlikely(atomic_read(&tx_ring->tx_count) < 2)) {
+		netif_info(qdev, tx_queued, qdev->ndev,
+			   "%s: BUG! shutting down tx queue %d due to lack of resources.\n",
+			   __func__, tx_ring_idx);
+		netif_stop_subqueue(ndev, tx_ring->wq_id);
+		tx_ring->tx_errors++;
+		return NETDEV_TX_BUSY;
+	}
+	tx_ring_desc = &tx_ring->q[tx_ring->prod_idx];
+	mac_iocb_ptr = tx_ring_desc->queue_entry;
+	memset((void *)mac_iocb_ptr, 0, sizeof(*mac_iocb_ptr));
+
+	mac_iocb_ptr->opcode = OPCODE_OB_MAC_IOCB;
+	mac_iocb_ptr->tid = tx_ring_desc->index;
+	/* We use the upper 32-bits to store the tx queue for this IO.
+	 * When we get the completion we can use it to establish the context.
+	 */
+	mac_iocb_ptr->txq_idx = tx_ring_idx;
+	tx_ring_desc->skb = skb;
+
+	mac_iocb_ptr->frame_len = cpu_to_le16((u16)skb->len);
+
+	if (skb_vlan_tag_present(skb)) {
+		netif_printk(qdev, tx_queued, KERN_DEBUG, qdev->ndev,
+			     "Adding a vlan tag %d.\n", skb_vlan_tag_get(skb));
+		mac_iocb_ptr->flags3 |= OB_MAC_IOCB_V;
+		mac_iocb_ptr->vlan_tci = cpu_to_le16(skb_vlan_tag_get(skb));
+	}
+	tso = qlge_tso(skb, (struct qlge_ob_mac_tso_iocb_req *)mac_iocb_ptr);
+	if (tso < 0) {
+		dev_kfree_skb_any(skb);
+		return NETDEV_TX_OK;
+	} else if (unlikely(!tso) && (skb->ip_summed == CHECKSUM_PARTIAL)) {
+		qlge_hw_csum_setup(skb,
+				   (struct qlge_ob_mac_tso_iocb_req *)mac_iocb_ptr);
+	}
+	if (qlge_map_send(qdev, mac_iocb_ptr, skb, tx_ring_desc) !=
+	    NETDEV_TX_OK) {
+		netif_err(qdev, tx_queued, qdev->ndev,
+			  "Could not map the segments.\n");
+		tx_ring->tx_errors++;
+		return NETDEV_TX_BUSY;
+	}
+
+	tx_ring->prod_idx++;
+	if (tx_ring->prod_idx == tx_ring->wq_len)
+		tx_ring->prod_idx = 0;
+	wmb();
+
+	qlge_write_db_reg_relaxed(tx_ring->prod_idx, tx_ring->prod_idx_db_reg);
+	netif_printk(qdev, tx_queued, KERN_DEBUG, qdev->ndev,
+		     "tx queued, slot %d, len %d\n",
+		     tx_ring->prod_idx, skb->len);
+
+	atomic_dec(&tx_ring->tx_count);
+
+	if (unlikely(atomic_read(&tx_ring->tx_count) < 2)) {
+		netif_stop_subqueue(ndev, tx_ring->wq_id);
+		if ((atomic_read(&tx_ring->tx_count) > (tx_ring->wq_len / 4)))
+			/*
+			 * The queue got stopped because the tx_ring was full.
+			 * Wake it up, because it's now at least 25% empty.
+			 */
+			netif_wake_subqueue(qdev->ndev, tx_ring->wq_id);
+	}
+	return NETDEV_TX_OK;
+}
+
+static void qlge_free_shadow_space(struct qlge_adapter *qdev)
+{
+	if (qdev->rx_ring_shadow_reg_area) {
+		dma_free_coherent(&qdev->pdev->dev,
+				  PAGE_SIZE,
+				  qdev->rx_ring_shadow_reg_area,
+				  qdev->rx_ring_shadow_reg_dma);
+		qdev->rx_ring_shadow_reg_area = NULL;
+	}
+	if (qdev->tx_ring_shadow_reg_area) {
+		dma_free_coherent(&qdev->pdev->dev,
+				  PAGE_SIZE,
+				  qdev->tx_ring_shadow_reg_area,
+				  qdev->tx_ring_shadow_reg_dma);
+		qdev->tx_ring_shadow_reg_area = NULL;
+	}
+}
+
+static int qlge_alloc_shadow_space(struct qlge_adapter *qdev)
+{
+	qdev->rx_ring_shadow_reg_area =
+		dma_alloc_coherent(&qdev->pdev->dev, PAGE_SIZE,
+				   &qdev->rx_ring_shadow_reg_dma, GFP_ATOMIC);
+	if (!qdev->rx_ring_shadow_reg_area) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Allocation of RX shadow space failed.\n");
+		return -ENOMEM;
+	}
+
+	qdev->tx_ring_shadow_reg_area =
+		dma_alloc_coherent(&qdev->pdev->dev, PAGE_SIZE,
+				   &qdev->tx_ring_shadow_reg_dma, GFP_ATOMIC);
+	if (!qdev->tx_ring_shadow_reg_area) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Allocation of TX shadow space failed.\n");
+		goto err_wqp_sh_area;
+	}
+	return 0;
+
+err_wqp_sh_area:
+	dma_free_coherent(&qdev->pdev->dev,
+			  PAGE_SIZE,
+			  qdev->rx_ring_shadow_reg_area,
+			  qdev->rx_ring_shadow_reg_dma);
+	return -ENOMEM;
+}
+
+static void qlge_init_tx_ring(struct qlge_adapter *qdev, struct tx_ring *tx_ring)
+{
+	struct tx_ring_desc *tx_ring_desc;
+	int i;
+	struct qlge_ob_mac_iocb_req *mac_iocb_ptr;
+
+	mac_iocb_ptr = tx_ring->wq_base;
+	tx_ring_desc = tx_ring->q;
+	for (i = 0; i < tx_ring->wq_len; i++) {
+		tx_ring_desc->index = i;
+		tx_ring_desc->skb = NULL;
+		tx_ring_desc->queue_entry = mac_iocb_ptr;
+		mac_iocb_ptr++;
+		tx_ring_desc++;
+	}
+	atomic_set(&tx_ring->tx_count, tx_ring->wq_len);
+}
+
+static void qlge_free_tx_resources(struct qlge_adapter *qdev,
+				   struct tx_ring *tx_ring)
+{
+	if (tx_ring->wq_base) {
+		dma_free_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+				  tx_ring->wq_base, tx_ring->wq_base_dma);
+		tx_ring->wq_base = NULL;
+	}
+	kfree(tx_ring->q);
+	tx_ring->q = NULL;
+}
+
+static int qlge_alloc_tx_resources(struct qlge_adapter *qdev,
+				   struct tx_ring *tx_ring)
+{
+	tx_ring->wq_base =
+		dma_alloc_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+				   &tx_ring->wq_base_dma, GFP_ATOMIC);
+
+	if (!tx_ring->wq_base ||
+	    tx_ring->wq_base_dma & WQ_ADDR_ALIGN)
+		goto pci_alloc_err;
+
+	tx_ring->q =
+		kmalloc_array(tx_ring->wq_len, sizeof(struct tx_ring_desc),
+			      GFP_KERNEL);
+	if (!tx_ring->q)
+		goto err;
+
+	return 0;
+err:
+	dma_free_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+			  tx_ring->wq_base, tx_ring->wq_base_dma);
+	tx_ring->wq_base = NULL;
+pci_alloc_err:
+	netif_err(qdev, ifup, qdev->ndev, "tx_ring alloc failed.\n");
+	return -ENOMEM;
+}
+
+static void qlge_free_lbq_buffers(struct qlge_adapter *qdev, struct rx_ring *rx_ring)
+{
+	struct qlge_bq *lbq = &rx_ring->lbq;
+	unsigned int last_offset;
+
+	last_offset = qlge_lbq_block_size(qdev) - qdev->lbq_buf_size;
+	while (lbq->next_to_clean != lbq->next_to_use) {
+		struct qlge_bq_desc *lbq_desc =
+			&lbq->queue[lbq->next_to_clean];
+
+		if (lbq_desc->p.pg_chunk.offset == last_offset)
+			dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
+				       qlge_lbq_block_size(qdev),
+				       DMA_FROM_DEVICE);
+		put_page(lbq_desc->p.pg_chunk.page);
+
+		lbq->next_to_clean = QLGE_BQ_WRAP(lbq->next_to_clean + 1);
+	}
+
+	if (rx_ring->master_chunk.page) {
+		dma_unmap_page(&qdev->pdev->dev, rx_ring->chunk_dma_addr,
+			       qlge_lbq_block_size(qdev), DMA_FROM_DEVICE);
+		put_page(rx_ring->master_chunk.page);
+		rx_ring->master_chunk.page = NULL;
+	}
+}
+
+static void qlge_free_sbq_buffers(struct qlge_adapter *qdev, struct rx_ring *rx_ring)
+{
+	int i;
+
+	for (i = 0; i < QLGE_BQ_LEN; i++) {
+		struct qlge_bq_desc *sbq_desc = &rx_ring->sbq.queue[i];
+
+		if (!sbq_desc) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "sbq_desc %d is NULL.\n", i);
+			return;
+		}
+		if (sbq_desc->p.skb) {
+			dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+					 SMALL_BUF_MAP_SIZE,
+					 DMA_FROM_DEVICE);
+			dev_kfree_skb(sbq_desc->p.skb);
+			sbq_desc->p.skb = NULL;
+		}
+	}
+}
+
+/* Free all large and small rx buffers associated
+ * with the completion queues for this device.
+ */
+static void qlge_free_rx_buffers(struct qlge_adapter *qdev)
+{
+	int i;
+
+	for (i = 0; i < qdev->rx_ring_count; i++) {
+		struct rx_ring *rx_ring = &qdev->rx_ring[i];
+
+		if (rx_ring->lbq.queue)
+			qlge_free_lbq_buffers(qdev, rx_ring);
+		if (rx_ring->sbq.queue)
+			qlge_free_sbq_buffers(qdev, rx_ring);
+	}
+}
+
+static void qlge_alloc_rx_buffers(struct qlge_adapter *qdev)
+{
+	int i;
+
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		qlge_update_buffer_queues(&qdev->rx_ring[i], GFP_KERNEL,
+					  HZ / 2);
+}
+
+static int qlge_init_bq(struct qlge_bq *bq)
+{
+	struct rx_ring *rx_ring = QLGE_BQ_CONTAINER(bq);
+	struct qlge_adapter *qdev = rx_ring->qdev;
+	struct qlge_bq_desc *bq_desc;
+	__le64 *buf_ptr;
+	int i;
+
+	bq->base = dma_alloc_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				      &bq->base_dma, GFP_ATOMIC);
+	if (!bq->base)
+		return -ENOMEM;
+
+	bq->queue = kmalloc_array(QLGE_BQ_LEN, sizeof(struct qlge_bq_desc),
+				  GFP_KERNEL);
+	if (!bq->queue)
+		return -ENOMEM;
+
+	buf_ptr = bq->base;
+	bq_desc = &bq->queue[0];
+	for (i = 0; i < QLGE_BQ_LEN; i++, buf_ptr++, bq_desc++) {
+		bq_desc->p.skb = NULL;
+		bq_desc->index = i;
+		bq_desc->buf_ptr = buf_ptr;
+	}
+
+	return 0;
+}
+
+static void qlge_free_rx_resources(struct qlge_adapter *qdev,
+				   struct rx_ring *rx_ring)
+{
+	/* Free the small buffer queue. */
+	if (rx_ring->sbq.base) {
+		dma_free_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				  rx_ring->sbq.base, rx_ring->sbq.base_dma);
+		rx_ring->sbq.base = NULL;
+	}
+
+	/* Free the small buffer queue control blocks. */
+	kfree(rx_ring->sbq.queue);
+	rx_ring->sbq.queue = NULL;
+
+	/* Free the large buffer queue. */
+	if (rx_ring->lbq.base) {
+		dma_free_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				  rx_ring->lbq.base, rx_ring->lbq.base_dma);
+		rx_ring->lbq.base = NULL;
+	}
+
+	/* Free the large buffer queue control blocks. */
+	kfree(rx_ring->lbq.queue);
+	rx_ring->lbq.queue = NULL;
+
+	/* Free the rx queue. */
+	if (rx_ring->cq_base) {
+		dma_free_coherent(&qdev->pdev->dev,
+				  rx_ring->cq_size,
+				  rx_ring->cq_base, rx_ring->cq_base_dma);
+		rx_ring->cq_base = NULL;
+	}
+}
+
+/* Allocate queues and buffers for this completions queue based
+ * on the values in the parameter structure.
+ */
+static int qlge_alloc_rx_resources(struct qlge_adapter *qdev,
+				   struct rx_ring *rx_ring)
+{
+	/*
+	 * Allocate the completion queue for this rx_ring.
+	 */
+	rx_ring->cq_base =
+		dma_alloc_coherent(&qdev->pdev->dev, rx_ring->cq_size,
+				   &rx_ring->cq_base_dma, GFP_ATOMIC);
+
+	if (!rx_ring->cq_base) {
+		netif_err(qdev, ifup, qdev->ndev, "rx_ring alloc failed.\n");
+		return -ENOMEM;
+	}
+
+	if (rx_ring->cq_id < qdev->rss_ring_count &&
+	    (qlge_init_bq(&rx_ring->sbq) || qlge_init_bq(&rx_ring->lbq))) {
+		qlge_free_rx_resources(qdev, rx_ring);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void qlge_tx_ring_clean(struct qlge_adapter *qdev)
+{
+	struct tx_ring *tx_ring;
+	struct tx_ring_desc *tx_ring_desc;
+	int i, j;
+
+	/*
+	 * Loop through all queues and free
+	 * any resources.
+	 */
+	for (j = 0; j < qdev->tx_ring_count; j++) {
+		tx_ring = &qdev->tx_ring[j];
+		for (i = 0; i < tx_ring->wq_len; i++) {
+			tx_ring_desc = &tx_ring->q[i];
+			if (tx_ring_desc && tx_ring_desc->skb) {
+				netif_err(qdev, ifdown, qdev->ndev,
+					  "Freeing lost SKB %p, from queue %d, index %d.\n",
+					  tx_ring_desc->skb, j,
+					  tx_ring_desc->index);
+				qlge_unmap_send(qdev, tx_ring_desc,
+						tx_ring_desc->map_cnt);
+				dev_kfree_skb(tx_ring_desc->skb);
+				tx_ring_desc->skb = NULL;
+			}
+		}
+	}
+}
+
+static void qlge_free_mem_resources(struct qlge_adapter *qdev)
+{
+	int i;
+
+	for (i = 0; i < qdev->tx_ring_count; i++)
+		qlge_free_tx_resources(qdev, &qdev->tx_ring[i]);
+	for (i = 0; i < qdev->rx_ring_count; i++)
+		qlge_free_rx_resources(qdev, &qdev->rx_ring[i]);
+	qlge_free_shadow_space(qdev);
+}
+
+static int qlge_alloc_mem_resources(struct qlge_adapter *qdev)
+{
+	int i;
+
+	/* Allocate space for our shadow registers and such. */
+	if (qlge_alloc_shadow_space(qdev))
+		return -ENOMEM;
+
+	for (i = 0; i < qdev->rx_ring_count; i++) {
+		if (qlge_alloc_rx_resources(qdev, &qdev->rx_ring[i]) != 0) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "RX resource allocation failed.\n");
+			goto err_mem;
+		}
+	}
+	/* Allocate tx queue resources */
+	for (i = 0; i < qdev->tx_ring_count; i++) {
+		if (qlge_alloc_tx_resources(qdev, &qdev->tx_ring[i]) != 0) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "TX resource allocation failed.\n");
+			goto err_mem;
+		}
+	}
+	return 0;
+
+err_mem:
+	qlge_free_mem_resources(qdev);
+	return -ENOMEM;
+}
+
+/* Set up the rx ring control block and pass it to the chip.
+ * The control block is defined as
+ * "Completion Queue Initialization Control Block", or cqicb.
+ */
+static int qlge_start_rx_ring(struct qlge_adapter *qdev, struct rx_ring *rx_ring)
+{
+	struct cqicb *cqicb = &rx_ring->cqicb;
+	void *shadow_reg = qdev->rx_ring_shadow_reg_area +
+		(rx_ring->cq_id * RX_RING_SHADOW_SPACE);
+	u64 shadow_reg_dma = qdev->rx_ring_shadow_reg_dma +
+		(rx_ring->cq_id * RX_RING_SHADOW_SPACE);
+	void __iomem *doorbell_area =
+		qdev->doorbell_area + (DB_PAGE_SIZE * (128 + rx_ring->cq_id));
+	int err = 0;
+	u64 dma;
+	__le64 *base_indirect_ptr;
+	int page_entries;
+
+	/* Set up the shadow registers for this ring. */
+	rx_ring->prod_idx_sh_reg = shadow_reg;
+	rx_ring->prod_idx_sh_reg_dma = shadow_reg_dma;
+	*rx_ring->prod_idx_sh_reg = 0;
+	shadow_reg += sizeof(u64);
+	shadow_reg_dma += sizeof(u64);
+	rx_ring->lbq.base_indirect = shadow_reg;
+	rx_ring->lbq.base_indirect_dma = shadow_reg_dma;
+	shadow_reg += (sizeof(u64) * MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN));
+	shadow_reg_dma += (sizeof(u64) * MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN));
+	rx_ring->sbq.base_indirect = shadow_reg;
+	rx_ring->sbq.base_indirect_dma = shadow_reg_dma;
+
+	/* PCI doorbell mem area + 0x00 for consumer index register */
+	rx_ring->cnsmr_idx_db_reg = (u32 __iomem *)doorbell_area;
+	rx_ring->cnsmr_idx = 0;
+	rx_ring->curr_entry = rx_ring->cq_base;
+
+	/* PCI doorbell mem area + 0x04 for valid register */
+	rx_ring->valid_db_reg = doorbell_area + 0x04;
+
+	/* PCI doorbell mem area + 0x18 for large buffer consumer */
+	rx_ring->lbq.prod_idx_db_reg = (u32 __iomem *)(doorbell_area + 0x18);
+
+	/* PCI doorbell mem area + 0x1c */
+	rx_ring->sbq.prod_idx_db_reg = (u32 __iomem *)(doorbell_area + 0x1c);
+
+	memset((void *)cqicb, 0, sizeof(struct cqicb));
+	cqicb->msix_vect = rx_ring->irq;
+
+	cqicb->len = cpu_to_le16(QLGE_FIT16(rx_ring->cq_len) | LEN_V |
+				 LEN_CPP_CONT);
+
+	cqicb->addr = cpu_to_le64(rx_ring->cq_base_dma);
+
+	cqicb->prod_idx_addr = cpu_to_le64(rx_ring->prod_idx_sh_reg_dma);
+
+	/*
+	 * Set up the control block load flags.
+	 */
+	cqicb->flags = FLAGS_LC |	/* Load queue base address */
+		FLAGS_LV |		/* Load MSI-X vector */
+		FLAGS_LI;		/* Load irq delay values */
+	if (rx_ring->cq_id < qdev->rss_ring_count) {
+		cqicb->flags |= FLAGS_LL;	/* Load lbq values */
+		dma = (u64)rx_ring->lbq.base_dma;
+		base_indirect_ptr = rx_ring->lbq.base_indirect;
+
+		for (page_entries = 0;
+		     page_entries < MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN);
+		     page_entries++) {
+			base_indirect_ptr[page_entries] = cpu_to_le64(dma);
+			dma += DB_PAGE_SIZE;
+		}
+		cqicb->lbq_addr = cpu_to_le64(rx_ring->lbq.base_indirect_dma);
+		cqicb->lbq_buf_size =
+			cpu_to_le16(QLGE_FIT16(qdev->lbq_buf_size));
+		cqicb->lbq_len = cpu_to_le16(QLGE_FIT16(QLGE_BQ_LEN));
+		rx_ring->lbq.next_to_use = 0;
+		rx_ring->lbq.next_to_clean = 0;
+
+		cqicb->flags |= FLAGS_LS;	/* Load sbq values */
+		dma = (u64)rx_ring->sbq.base_dma;
+		base_indirect_ptr = rx_ring->sbq.base_indirect;
+
+		for (page_entries = 0;
+		     page_entries < MAX_DB_PAGES_PER_BQ(QLGE_BQ_LEN);
+		     page_entries++) {
+			base_indirect_ptr[page_entries] = cpu_to_le64(dma);
+			dma += DB_PAGE_SIZE;
+		}
+		cqicb->sbq_addr =
+			cpu_to_le64(rx_ring->sbq.base_indirect_dma);
+		cqicb->sbq_buf_size = cpu_to_le16(SMALL_BUFFER_SIZE);
+		cqicb->sbq_len = cpu_to_le16(QLGE_FIT16(QLGE_BQ_LEN));
+		rx_ring->sbq.next_to_use = 0;
+		rx_ring->sbq.next_to_clean = 0;
+	}
+	if (rx_ring->cq_id < qdev->rss_ring_count) {
+		/* Inbound completion handling rx_rings run in
+		 * separate NAPI contexts.
+		 */
+		netif_napi_add(qdev->ndev, &rx_ring->napi,
+			       qlge_napi_poll_msix);
+		cqicb->irq_delay = cpu_to_le16(qdev->rx_coalesce_usecs);
+		cqicb->pkt_delay = cpu_to_le16(qdev->rx_max_coalesced_frames);
+	} else {
+		cqicb->irq_delay = cpu_to_le16(qdev->tx_coalesce_usecs);
+		cqicb->pkt_delay = cpu_to_le16(qdev->tx_max_coalesced_frames);
+	}
+	err = qlge_write_cfg(qdev, cqicb, sizeof(struct cqicb),
+			     CFG_LCQ, rx_ring->cq_id);
+	if (err) {
+		netif_err(qdev, ifup, qdev->ndev, "Failed to load CQICB.\n");
+		return err;
+	}
+	return err;
+}
+
+static int qlge_start_tx_ring(struct qlge_adapter *qdev, struct tx_ring *tx_ring)
+{
+	struct wqicb *wqicb = (struct wqicb *)tx_ring;
+	void __iomem *doorbell_area =
+		qdev->doorbell_area + (DB_PAGE_SIZE * tx_ring->wq_id);
+	void *shadow_reg = qdev->tx_ring_shadow_reg_area +
+		(tx_ring->wq_id * sizeof(u64));
+	u64 shadow_reg_dma = qdev->tx_ring_shadow_reg_dma +
+		(tx_ring->wq_id * sizeof(u64));
+	int err = 0;
+
+	/*
+	 * Assign doorbell registers for this tx_ring.
+	 */
+	/* TX PCI doorbell mem area for tx producer index */
+	tx_ring->prod_idx_db_reg = (u32 __iomem *)doorbell_area;
+	tx_ring->prod_idx = 0;
+	/* TX PCI doorbell mem area + 0x04 */
+	tx_ring->valid_db_reg = doorbell_area + 0x04;
+
+	/*
+	 * Assign shadow registers for this tx_ring.
+	 */
+	tx_ring->cnsmr_idx_sh_reg = shadow_reg;
+	tx_ring->cnsmr_idx_sh_reg_dma = shadow_reg_dma;
+
+	wqicb->len = cpu_to_le16(tx_ring->wq_len | Q_LEN_V | Q_LEN_CPP_CONT);
+	wqicb->flags = cpu_to_le16(Q_FLAGS_LC |
+				   Q_FLAGS_LB | Q_FLAGS_LI | Q_FLAGS_LO);
+	wqicb->cq_id_rss = cpu_to_le16(tx_ring->cq_id);
+	wqicb->rid = 0;
+	wqicb->addr = cpu_to_le64(tx_ring->wq_base_dma);
+
+	wqicb->cnsmr_idx_addr = cpu_to_le64(tx_ring->cnsmr_idx_sh_reg_dma);
+
+	qlge_init_tx_ring(qdev, tx_ring);
+
+	err = qlge_write_cfg(qdev, wqicb, sizeof(*wqicb), CFG_LRQ,
+			     (u16)tx_ring->wq_id);
+	if (err) {
+		netif_err(qdev, ifup, qdev->ndev, "Failed to load tx_ring.\n");
+		return err;
+	}
+	return err;
+}
+
+static void qlge_disable_msix(struct qlge_adapter *qdev)
+{
+	if (test_bit(QL_MSIX_ENABLED, &qdev->flags)) {
+		pci_disable_msix(qdev->pdev);
+		clear_bit(QL_MSIX_ENABLED, &qdev->flags);
+		kfree(qdev->msi_x_entry);
+		qdev->msi_x_entry = NULL;
+	} else if (test_bit(QL_MSI_ENABLED, &qdev->flags)) {
+		pci_disable_msi(qdev->pdev);
+		clear_bit(QL_MSI_ENABLED, &qdev->flags);
+	}
+}
+
+/* We start by trying to get the number of vectors
+ * stored in qdev->intr_count. If we don't get that
+ * many then we reduce the count and try again.
+ */
+static void qlge_enable_msix(struct qlge_adapter *qdev)
+{
+	int i, err;
+
+	/* Get the MSIX vectors. */
+	if (qlge_irq_type == MSIX_IRQ) {
+		/* Try to alloc space for the msix struct,
+		 * if it fails then go to MSI/legacy.
+		 */
+		qdev->msi_x_entry = kcalloc(qdev->intr_count,
+					    sizeof(struct msix_entry),
+					    GFP_KERNEL);
+		if (!qdev->msi_x_entry) {
+			qlge_irq_type = MSI_IRQ;
+			goto msi;
+		}
+
+		for (i = 0; i < qdev->intr_count; i++)
+			qdev->msi_x_entry[i].entry = i;
+
+		err = pci_enable_msix_range(qdev->pdev, qdev->msi_x_entry,
+					    1, qdev->intr_count);
+		if (err < 0) {
+			kfree(qdev->msi_x_entry);
+			qdev->msi_x_entry = NULL;
+			netif_warn(qdev, ifup, qdev->ndev,
+				   "MSI-X Enable failed, trying MSI.\n");
+			qlge_irq_type = MSI_IRQ;
+		} else {
+			qdev->intr_count = err;
+			set_bit(QL_MSIX_ENABLED, &qdev->flags);
+			netif_info(qdev, ifup, qdev->ndev,
+				   "MSI-X Enabled, got %d vectors.\n",
+				   qdev->intr_count);
+			return;
+		}
+	}
+msi:
+	qdev->intr_count = 1;
+	if (qlge_irq_type == MSI_IRQ) {
+		if (pci_alloc_irq_vectors(qdev->pdev, 1, 1, PCI_IRQ_MSI) >= 0) {
+			set_bit(QL_MSI_ENABLED, &qdev->flags);
+			netif_info(qdev, ifup, qdev->ndev,
+				   "Running with MSI interrupts.\n");
+			return;
+		}
+	}
+	qlge_irq_type = LEG_IRQ;
+	set_bit(QL_LEGACY_ENABLED, &qdev->flags);
+	netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+		     "Running with legacy interrupts.\n");
+}
+
+/* Each vector services 1 RSS ring and 1 or more
+ * TX completion rings.  This function loops through
+ * the TX completion rings and assigns the vector that
+ * will service it.  An example would be if there are
+ * 2 vectors (so 2 RSS rings) and 8 TX completion rings.
+ * This would mean that vector 0 would service RSS ring 0
+ * and TX completion rings 0,1,2 and 3.  Vector 1 would
+ * service RSS ring 1 and TX completion rings 4,5,6 and 7.
+ */
+static void qlge_set_tx_vect(struct qlge_adapter *qdev)
+{
+	int i, j, vect;
+	u32 tx_rings_per_vector = qdev->tx_ring_count / qdev->intr_count;
+
+	if (likely(test_bit(QL_MSIX_ENABLED, &qdev->flags))) {
+		/* Assign irq vectors to TX rx_rings.*/
+		for (vect = 0, j = 0, i = qdev->rss_ring_count;
+		     i < qdev->rx_ring_count; i++) {
+			if (j == tx_rings_per_vector) {
+				vect++;
+				j = 0;
+			}
+			qdev->rx_ring[i].irq = vect;
+			j++;
+		}
+	} else {
+		/* For single vector all rings have an irq
+		 * of zero.
+		 */
+		for (i = 0; i < qdev->rx_ring_count; i++)
+			qdev->rx_ring[i].irq = 0;
+	}
+}
+
+/* Set the interrupt mask for this vector.  Each vector
+ * will service 1 RSS ring and 1 or more TX completion
+ * rings.  This function sets up a bit mask per vector
+ * that indicates which rings it services.
+ */
+static void qlge_set_irq_mask(struct qlge_adapter *qdev, struct intr_context *ctx)
+{
+	int j, vect = ctx->intr;
+	u32 tx_rings_per_vector = qdev->tx_ring_count / qdev->intr_count;
+
+	if (likely(test_bit(QL_MSIX_ENABLED, &qdev->flags))) {
+		/* Add the RSS ring serviced by this vector
+		 * to the mask.
+		 */
+		ctx->irq_mask = (1 << qdev->rx_ring[vect].cq_id);
+		/* Add the TX ring(s) serviced by this vector
+		 * to the mask.
+		 */
+		for (j = 0; j < tx_rings_per_vector; j++) {
+			ctx->irq_mask |=
+				(1 << qdev->rx_ring[qdev->rss_ring_count +
+				 (vect * tx_rings_per_vector) + j].cq_id);
+		}
+	} else {
+		/* For single vector we just shift each queue's
+		 * ID into the mask.
+		 */
+		for (j = 0; j < qdev->rx_ring_count; j++)
+			ctx->irq_mask |= (1 << qdev->rx_ring[j].cq_id);
+	}
+}
+
+/*
+ * Here we build the intr_context structures based on
+ * our rx_ring count and intr vector count.
+ * The intr_context structure is used to hook each vector
+ * to possibly different handlers.
+ */
+static void qlge_resolve_queues_to_irqs(struct qlge_adapter *qdev)
+{
+	int i = 0;
+	struct intr_context *intr_context = &qdev->intr_context[0];
+
+	if (likely(test_bit(QL_MSIX_ENABLED, &qdev->flags))) {
+		/* Each rx_ring has it's
+		 * own intr_context since we have separate
+		 * vectors for each queue.
+		 */
+		for (i = 0; i < qdev->intr_count; i++, intr_context++) {
+			qdev->rx_ring[i].irq = i;
+			intr_context->intr = i;
+			intr_context->qdev = qdev;
+			/* Set up this vector's bit-mask that indicates
+			 * which queues it services.
+			 */
+			qlge_set_irq_mask(qdev, intr_context);
+			/*
+			 * We set up each vectors enable/disable/read bits so
+			 * there's no bit/mask calculations in the critical path.
+			 */
+			intr_context->intr_en_mask =
+				INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK |
+				INTR_EN_TYPE_ENABLE | INTR_EN_IHD_MASK | INTR_EN_IHD
+				| i;
+			intr_context->intr_dis_mask =
+				INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK |
+				INTR_EN_TYPE_DISABLE | INTR_EN_IHD_MASK |
+				INTR_EN_IHD | i;
+			intr_context->intr_read_mask =
+				INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK |
+				INTR_EN_TYPE_READ | INTR_EN_IHD_MASK | INTR_EN_IHD |
+				i;
+			if (i == 0) {
+				/* The first vector/queue handles
+				 * broadcast/multicast, fatal errors,
+				 * and firmware events.  This in addition
+				 * to normal inbound NAPI processing.
+				 */
+				intr_context->handler = qlge_isr;
+				sprintf(intr_context->name, "%s-rx-%d",
+					qdev->ndev->name, i);
+			} else {
+				/*
+				 * Inbound queues handle unicast frames only.
+				 */
+				intr_context->handler = qlge_msix_rx_isr;
+				sprintf(intr_context->name, "%s-rx-%d",
+					qdev->ndev->name, i);
+			}
+		}
+	} else {
+		/*
+		 * All rx_rings use the same intr_context since
+		 * there is only one vector.
+		 */
+		intr_context->intr = 0;
+		intr_context->qdev = qdev;
+		/*
+		 * We set up each vectors enable/disable/read bits so
+		 * there's no bit/mask calculations in the critical path.
+		 */
+		intr_context->intr_en_mask =
+			INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK | INTR_EN_TYPE_ENABLE;
+		intr_context->intr_dis_mask =
+			INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK |
+			INTR_EN_TYPE_DISABLE;
+		if (test_bit(QL_LEGACY_ENABLED, &qdev->flags)) {
+			/* Experience shows that when using INTx interrupts,
+			 * the device does not always auto-mask INTR_EN_EN.
+			 * Moreover, masking INTR_EN_EN manually does not
+			 * immediately prevent interrupt generation.
+			 */
+			intr_context->intr_en_mask |= INTR_EN_EI << 16 |
+				INTR_EN_EI;
+			intr_context->intr_dis_mask |= INTR_EN_EI << 16;
+		}
+		intr_context->intr_read_mask =
+			INTR_EN_TYPE_MASK | INTR_EN_INTR_MASK | INTR_EN_TYPE_READ;
+		/*
+		 * Single interrupt means one handler for all rings.
+		 */
+		intr_context->handler = qlge_isr;
+		sprintf(intr_context->name, "%s-single_irq", qdev->ndev->name);
+		/* Set up this vector's bit-mask that indicates
+		 * which queues it services. In this case there is
+		 * a single vector so it will service all RSS and
+		 * TX completion rings.
+		 */
+		qlge_set_irq_mask(qdev, intr_context);
+	}
+	/* Tell the TX completion rings which MSIx vector
+	 * they will be using.
+	 */
+	qlge_set_tx_vect(qdev);
+}
+
+static void qlge_free_irq(struct qlge_adapter *qdev)
+{
+	int i;
+	struct intr_context *intr_context = &qdev->intr_context[0];
+
+	for (i = 0; i < qdev->intr_count; i++, intr_context++) {
+		if (intr_context->hooked) {
+			if (test_bit(QL_MSIX_ENABLED, &qdev->flags)) {
+				free_irq(qdev->msi_x_entry[i].vector,
+					 &qdev->rx_ring[i]);
+			} else {
+				free_irq(qdev->pdev->irq, &qdev->rx_ring[0]);
+			}
+		}
+	}
+	qlge_disable_msix(qdev);
+}
+
+static int qlge_request_irq(struct qlge_adapter *qdev)
+{
+	int i;
+	int status = 0;
+	struct pci_dev *pdev = qdev->pdev;
+	struct intr_context *intr_context = &qdev->intr_context[0];
+
+	qlge_resolve_queues_to_irqs(qdev);
+
+	for (i = 0; i < qdev->intr_count; i++, intr_context++) {
+		if (test_bit(QL_MSIX_ENABLED, &qdev->flags)) {
+			status = request_irq(qdev->msi_x_entry[i].vector,
+					     intr_context->handler,
+					     0,
+					     intr_context->name,
+					     &qdev->rx_ring[i]);
+			if (status) {
+				netif_err(qdev, ifup, qdev->ndev,
+					  "Failed request for MSIX interrupt %d.\n",
+					  i);
+				goto err_irq;
+			}
+		} else {
+			netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+				     "trying msi or legacy interrupts.\n");
+			netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+				     "%s: irq = %d.\n", __func__, pdev->irq);
+			netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+				     "%s: context->name = %s.\n", __func__,
+				     intr_context->name);
+			netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
+				     "%s: dev_id = 0x%p.\n", __func__,
+				     &qdev->rx_ring[0]);
+			status =
+				request_irq(pdev->irq, qlge_isr,
+					    test_bit(QL_MSI_ENABLED, &qdev->flags)
+					    ? 0
+					    : IRQF_SHARED,
+					    intr_context->name, &qdev->rx_ring[0]);
+			if (status)
+				goto err_irq;
+
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Hooked intr 0, queue type RX_Q, with name %s.\n",
+				  intr_context->name);
+		}
+		intr_context->hooked = 1;
+	}
+	return status;
+err_irq:
+	netif_err(qdev, ifup, qdev->ndev, "Failed to get the interrupts!!!\n");
+	qlge_free_irq(qdev);
+	return status;
+}
+
+static int qlge_start_rss(struct qlge_adapter *qdev)
+{
+	static const u8 init_hash_seed[] = {
+		0x6d, 0x5a, 0x56, 0xda, 0x25, 0x5b, 0x0e, 0xc2,
+		0x41, 0x67, 0x25, 0x3d, 0x43, 0xa3, 0x8f, 0xb0,
+		0xd0, 0xca, 0x2b, 0xcb, 0xae, 0x7b, 0x30, 0xb4,
+		0x77, 0xcb, 0x2d, 0xa3, 0x80, 0x30, 0xf2, 0x0c,
+		0x6a, 0x42, 0xb7, 0x3b, 0xbe, 0xac, 0x01, 0xfa
+	};
+	struct ricb *ricb = &qdev->ricb;
+	int status = 0;
+	int i;
+	u8 *hash_id = (u8 *)ricb->hash_cq_id;
+
+	memset((void *)ricb, 0, sizeof(*ricb));
+
+	ricb->base_cq = RSS_L4K;
+	ricb->flags =
+		(RSS_L6K | RSS_LI | RSS_LB | RSS_LM | RSS_RT4 | RSS_RT6);
+	ricb->mask = cpu_to_le16((u16)(0x3ff));
+
+	/*
+	 * Fill out the Indirection Table.
+	 */
+	for (i = 0; i < 1024; i++)
+		hash_id[i] = (i & (qdev->rss_ring_count - 1));
+
+	memcpy((void *)&ricb->ipv6_hash_key[0], init_hash_seed, 40);
+	memcpy((void *)&ricb->ipv4_hash_key[0], init_hash_seed, 16);
+
+	status = qlge_write_cfg(qdev, ricb, sizeof(*ricb), CFG_LR, 0);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Failed to load RICB.\n");
+		return status;
+	}
+	return status;
+}
+
+static int qlge_clear_routing_entries(struct qlge_adapter *qdev)
+{
+	int i, status = 0;
+
+	status = qlge_sem_spinlock(qdev, SEM_RT_IDX_MASK);
+	if (status)
+		return status;
+	/* Clear all the entries in the routing table. */
+	for (i = 0; i < 16; i++) {
+		status = qlge_set_routing_reg(qdev, i, 0, 0);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to init routing register for CAM packets.\n");
+			break;
+		}
+	}
+	qlge_sem_unlock(qdev, SEM_RT_IDX_MASK);
+	return status;
+}
+
+/* Initialize the frame-to-queue routing. */
+static int qlge_route_initialize(struct qlge_adapter *qdev)
+{
+	int status = 0;
+
+	/* Clear all the entries in the routing table. */
+	status = qlge_clear_routing_entries(qdev);
+	if (status)
+		return status;
+
+	status = qlge_sem_spinlock(qdev, SEM_RT_IDX_MASK);
+	if (status)
+		return status;
+
+	status = qlge_set_routing_reg(qdev, RT_IDX_IP_CSUM_ERR_SLOT,
+				      RT_IDX_IP_CSUM_ERR, 1);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init routing register for IP CSUM error packets.\n");
+		goto exit;
+	}
+	status = qlge_set_routing_reg(qdev, RT_IDX_TCP_UDP_CSUM_ERR_SLOT,
+				      RT_IDX_TU_CSUM_ERR, 1);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init routing register for TCP/UDP CSUM error packets.\n");
+		goto exit;
+	}
+	status = qlge_set_routing_reg(qdev, RT_IDX_BCAST_SLOT, RT_IDX_BCAST, 1);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init routing register for broadcast packets.\n");
+		goto exit;
+	}
+	/* If we have more than one inbound queue, then turn on RSS in the
+	 * routing block.
+	 */
+	if (qdev->rss_ring_count > 1) {
+		status = qlge_set_routing_reg(qdev, RT_IDX_RSS_MATCH_SLOT,
+					      RT_IDX_RSS_MATCH, 1);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to init routing register for MATCH RSS packets.\n");
+			goto exit;
+		}
+	}
+
+	status = qlge_set_routing_reg(qdev, RT_IDX_CAM_HIT_SLOT,
+				      RT_IDX_CAM_HIT, 1);
+	if (status)
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init routing register for CAM packets.\n");
+exit:
+	qlge_sem_unlock(qdev, SEM_RT_IDX_MASK);
+	return status;
+}
+
+int qlge_cam_route_initialize(struct qlge_adapter *qdev)
+{
+	int status, set;
+
+	/* If check if the link is up and use to
+	 * determine if we are setting or clearing
+	 * the MAC address in the CAM.
+	 */
+	set = qlge_read32(qdev, STS);
+	set &= qdev->port_link_up;
+	status = qlge_set_mac_addr(qdev, set);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Failed to init mac address.\n");
+		return status;
+	}
+
+	status = qlge_route_initialize(qdev);
+	if (status)
+		netif_err(qdev, ifup, qdev->ndev, "Failed to init routing table.\n");
+
+	return status;
+}
+
+static int qlge_adapter_initialize(struct qlge_adapter *qdev)
+{
+	u32 value, mask;
+	int i;
+	int status = 0;
+
+	/*
+	 * Set up the System register to halt on errors.
+	 */
+	value = SYS_EFE | SYS_FAE;
+	mask = value << 16;
+	qlge_write32(qdev, SYS, mask | value);
+
+	/* Set the default queue, and VLAN behavior. */
+	value = NIC_RCV_CFG_DFQ;
+	mask = NIC_RCV_CFG_DFQ_MASK;
+	if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX) {
+		value |= NIC_RCV_CFG_RV;
+		mask |= (NIC_RCV_CFG_RV << 16);
+	}
+	qlge_write32(qdev, NIC_RCV_CFG, (mask | value));
+
+	/* Set the MPI interrupt to enabled. */
+	qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16) | INTR_MASK_PI);
+
+	/* Enable the function, set pagesize, enable error checking. */
+	value = FSC_FE | FSC_EPC_INBOUND | FSC_EPC_OUTBOUND |
+		FSC_EC | FSC_VM_PAGE_4K;
+	value |= SPLT_SETTING;
+
+	/* Set/clear header splitting. */
+	mask = FSC_VM_PAGESIZE_MASK |
+		FSC_DBL_MASK | FSC_DBRST_MASK | (value << 16);
+	qlge_write32(qdev, FSC, mask | value);
+
+	qlge_write32(qdev, SPLT_HDR, SPLT_LEN);
+
+	/* Set RX packet routing to use port/pci function on which the
+	 * packet arrived on in addition to usual frame routing.
+	 * This is helpful on bonding where both interfaces can have
+	 * the same MAC address.
+	 */
+	qlge_write32(qdev, RST_FO, RST_FO_RR_MASK | RST_FO_RR_RCV_FUNC_CQ);
+	/* Reroute all packets to our Interface.
+	 * They may have been routed to MPI firmware
+	 * due to WOL.
+	 */
+	value = qlge_read32(qdev, MGMT_RCV_CFG);
+	value &= ~MGMT_RCV_CFG_RM;
+	mask = 0xffff0000;
+
+	/* Sticky reg needs clearing due to WOL. */
+	qlge_write32(qdev, MGMT_RCV_CFG, mask);
+	qlge_write32(qdev, MGMT_RCV_CFG, mask | value);
+
+	/* Default WOL is enable on Mezz cards */
+	if (qdev->pdev->subsystem_device == 0x0068 ||
+	    qdev->pdev->subsystem_device == 0x0180)
+		qdev->wol = WAKE_MAGIC;
+
+	/* Start up the rx queues. */
+	for (i = 0; i < qdev->rx_ring_count; i++) {
+		status = qlge_start_rx_ring(qdev, &qdev->rx_ring[i]);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to start rx ring[%d].\n", i);
+			return status;
+		}
+	}
+
+	/* If there is more than one inbound completion queue
+	 * then download a RICB to configure RSS.
+	 */
+	if (qdev->rss_ring_count > 1) {
+		status = qlge_start_rss(qdev);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev, "Failed to start RSS.\n");
+			return status;
+		}
+	}
+
+	/* Start up the tx queues. */
+	for (i = 0; i < qdev->tx_ring_count; i++) {
+		status = qlge_start_tx_ring(qdev, &qdev->tx_ring[i]);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to start tx ring[%d].\n", i);
+			return status;
+		}
+	}
+
+	/* Initialize the port and set the max framesize. */
+	status = qdev->nic_ops->port_initialize(qdev);
+	if (status)
+		netif_err(qdev, ifup, qdev->ndev, "Failed to start port.\n");
+
+	/* Set up the MAC address and frame routing filter. */
+	status = qlge_cam_route_initialize(qdev);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Failed to init CAM/Routing tables.\n");
+		return status;
+	}
+
+	/* Start NAPI for the RSS queues. */
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		napi_enable(&qdev->rx_ring[i].napi);
+
+	return status;
+}
+
+/* Issue soft reset to chip. */
+static int qlge_adapter_reset(struct qlge_adapter *qdev)
+{
+	u32 value;
+	int status = 0;
+	unsigned long end_jiffies;
+
+	/* Clear all the entries in the routing table. */
+	status = qlge_clear_routing_entries(qdev);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev, "Failed to clear routing bits.\n");
+		return status;
+	}
+
+	/* Check if bit is set then skip the mailbox command and
+	 * clear the bit, else we are in normal reset process.
+	 */
+	if (!test_bit(QL_ASIC_RECOVERY, &qdev->flags)) {
+		/* Stop management traffic. */
+		qlge_mb_set_mgmnt_traffic_ctl(qdev, MB_SET_MPI_TFK_STOP);
+
+		/* Wait for the NIC and MGMNT FIFOs to empty. */
+		qlge_wait_fifo_empty(qdev);
+	} else {
+		clear_bit(QL_ASIC_RECOVERY, &qdev->flags);
+	}
+
+	qlge_write32(qdev, RST_FO, (RST_FO_FR << 16) | RST_FO_FR);
+
+	end_jiffies = jiffies + usecs_to_jiffies(30);
+	do {
+		value = qlge_read32(qdev, RST_FO);
+		if ((value & RST_FO_FR) == 0)
+			break;
+		cpu_relax();
+	} while (time_before(jiffies, end_jiffies));
+
+	if (value & RST_FO_FR) {
+		netif_err(qdev, ifdown, qdev->ndev,
+			  "ETIMEDOUT!!! errored out of resetting the chip!\n");
+		status = -ETIMEDOUT;
+	}
+
+	/* Resume management traffic. */
+	qlge_mb_set_mgmnt_traffic_ctl(qdev, MB_SET_MPI_TFK_RESUME);
+	return status;
+}
+
+static void qlge_display_dev_info(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	netif_info(qdev, probe, qdev->ndev,
+		   "Function #%d, Port %d, NIC Roll %d, NIC Rev = %d, XG Roll = %d, XG Rev = %d.\n",
+		   qdev->func,
+		   qdev->port,
+		   qdev->chip_rev_id & 0x0000000f,
+		   qdev->chip_rev_id >> 4 & 0x0000000f,
+		   qdev->chip_rev_id >> 8 & 0x0000000f,
+		   qdev->chip_rev_id >> 12 & 0x0000000f);
+	netif_info(qdev, probe, qdev->ndev,
+		   "MAC address %pM\n", ndev->dev_addr);
+}
+
+static int qlge_wol(struct qlge_adapter *qdev)
+{
+	int status = 0;
+	u32 wol = MB_WOL_DISABLE;
+
+	/* The CAM is still intact after a reset, but if we
+	 * are doing WOL, then we may need to program the
+	 * routing regs. We would also need to issue the mailbox
+	 * commands to instruct the MPI what to do per the ethtool
+	 * settings.
+	 */
+
+	if (qdev->wol & (WAKE_ARP | WAKE_MAGICSECURE | WAKE_PHY | WAKE_UCAST |
+			 WAKE_MCAST | WAKE_BCAST)) {
+		netif_err(qdev, ifdown, qdev->ndev,
+			  "Unsupported WOL parameter. qdev->wol = 0x%x.\n",
+			  qdev->wol);
+		return -EINVAL;
+	}
+
+	if (qdev->wol & WAKE_MAGIC) {
+		status = qlge_mb_wol_set_magic(qdev, 1);
+		if (status) {
+			netif_err(qdev, ifdown, qdev->ndev,
+				  "Failed to set magic packet on %s.\n",
+				  qdev->ndev->name);
+			return status;
+		}
+		netif_info(qdev, drv, qdev->ndev,
+			   "Enabled magic packet successfully on %s.\n",
+			   qdev->ndev->name);
+
+		wol |= MB_WOL_MAGIC_PKT;
+	}
+
+	if (qdev->wol) {
+		wol |= MB_WOL_MODE_ON;
+		status = qlge_mb_wol_mode(qdev, wol);
+		netif_err(qdev, drv, qdev->ndev,
+			  "WOL %s (wol code 0x%x) on %s\n",
+			  (status == 0) ? "Successfully set" : "Failed",
+			  wol, qdev->ndev->name);
+	}
+
+	return status;
+}
+
+static void qlge_cancel_all_work_sync(struct qlge_adapter *qdev)
+{
+	/* Don't kill the reset worker thread if we
+	 * are in the process of recovery.
+	 */
+	if (test_bit(QL_ADAPTER_UP, &qdev->flags))
+		cancel_delayed_work_sync(&qdev->asic_reset_work);
+	cancel_delayed_work_sync(&qdev->mpi_reset_work);
+	cancel_delayed_work_sync(&qdev->mpi_work);
+	cancel_delayed_work_sync(&qdev->mpi_idc_work);
+	cancel_delayed_work_sync(&qdev->mpi_port_cfg_work);
+}
+
+static int qlge_adapter_down(struct qlge_adapter *qdev)
+{
+	int i, status = 0;
+
+	qlge_link_off(qdev);
+
+	qlge_cancel_all_work_sync(qdev);
+
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		napi_disable(&qdev->rx_ring[i].napi);
+
+	clear_bit(QL_ADAPTER_UP, &qdev->flags);
+
+	qlge_disable_interrupts(qdev);
+
+	qlge_tx_ring_clean(qdev);
+
+	/* Call netif_napi_del() from common point. */
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		netif_napi_del(&qdev->rx_ring[i].napi);
+
+	status = qlge_adapter_reset(qdev);
+	if (status)
+		netif_err(qdev, ifdown, qdev->ndev, "reset(func #%d) FAILED!\n",
+			  qdev->func);
+	qlge_free_rx_buffers(qdev);
+
+	return status;
+}
+
+static int qlge_adapter_up(struct qlge_adapter *qdev)
+{
+	int err = 0;
+
+	err = qlge_adapter_initialize(qdev);
+	if (err) {
+		netif_info(qdev, ifup, qdev->ndev, "Unable to initialize adapter.\n");
+		goto err_init;
+	}
+	set_bit(QL_ADAPTER_UP, &qdev->flags);
+	qlge_alloc_rx_buffers(qdev);
+	/* If the port is initialized and the
+	 * link is up the turn on the carrier.
+	 */
+	if ((qlge_read32(qdev, STS) & qdev->port_init) &&
+	    (qlge_read32(qdev, STS) & qdev->port_link_up))
+		qlge_link_on(qdev);
+	/* Restore rx mode. */
+	clear_bit(QL_ALLMULTI, &qdev->flags);
+	clear_bit(QL_PROMISCUOUS, &qdev->flags);
+	qlge_set_multicast_list(qdev->ndev);
+
+	/* Restore vlan setting. */
+	qlge_restore_vlan(qdev);
+
+	qlge_enable_interrupts(qdev);
+	qlge_enable_all_completion_interrupts(qdev);
+	netif_tx_start_all_queues(qdev->ndev);
+
+	return 0;
+err_init:
+	qlge_adapter_reset(qdev);
+	return err;
+}
+
+static void qlge_release_adapter_resources(struct qlge_adapter *qdev)
+{
+	qlge_free_mem_resources(qdev);
+	qlge_free_irq(qdev);
+}
+
+static int qlge_get_adapter_resources(struct qlge_adapter *qdev)
+{
+	if (qlge_alloc_mem_resources(qdev)) {
+		netif_err(qdev, ifup, qdev->ndev, "Unable to  allocate memory.\n");
+		return -ENOMEM;
+	}
+	return qlge_request_irq(qdev);
+}
+
+static int qlge_close(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int i;
+
+	/* If we hit pci_channel_io_perm_failure
+	 * failure condition, then we already
+	 * brought the adapter down.
+	 */
+	if (test_bit(QL_EEH_FATAL, &qdev->flags)) {
+		netif_err(qdev, drv, qdev->ndev, "EEH fatal did unload.\n");
+		clear_bit(QL_EEH_FATAL, &qdev->flags);
+		return 0;
+	}
+
+	/*
+	 * Wait for device to recover from a reset.
+	 * (Rarely happens, but possible.)
+	 */
+	while (!test_bit(QL_ADAPTER_UP, &qdev->flags))
+		msleep(1);
+
+	/* Make sure refill_work doesn't re-enable napi */
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		cancel_delayed_work_sync(&qdev->rx_ring[i].refill_work);
+
+	qlge_adapter_down(qdev);
+	qlge_release_adapter_resources(qdev);
+	return 0;
+}
+
+static void qlge_set_lb_size(struct qlge_adapter *qdev)
+{
+	if (qdev->ndev->mtu <= 1500)
+		qdev->lbq_buf_size = LARGE_BUFFER_MIN_SIZE;
+	else
+		qdev->lbq_buf_size = LARGE_BUFFER_MAX_SIZE;
+	qdev->lbq_buf_order = get_order(qdev->lbq_buf_size);
+}
+
+static int qlge_configure_rings(struct qlge_adapter *qdev)
+{
+	int i;
+	struct rx_ring *rx_ring;
+	struct tx_ring *tx_ring;
+	int cpu_cnt = min_t(int, MAX_CPUS, num_online_cpus());
+
+	/* In a perfect world we have one RSS ring for each CPU
+	 * and each has it's own vector.  To do that we ask for
+	 * cpu_cnt vectors.  qlge_enable_msix() will adjust the
+	 * vector count to what we actually get.  We then
+	 * allocate an RSS ring for each.
+	 * Essentially, we are doing min(cpu_count, msix_vector_count).
+	 */
+	qdev->intr_count = cpu_cnt;
+	qlge_enable_msix(qdev);
+	/* Adjust the RSS ring count to the actual vector count. */
+	qdev->rss_ring_count = qdev->intr_count;
+	qdev->tx_ring_count = cpu_cnt;
+	qdev->rx_ring_count = qdev->tx_ring_count + qdev->rss_ring_count;
+
+	for (i = 0; i < qdev->tx_ring_count; i++) {
+		tx_ring = &qdev->tx_ring[i];
+		memset((void *)tx_ring, 0, sizeof(*tx_ring));
+		tx_ring->qdev = qdev;
+		tx_ring->wq_id = i;
+		tx_ring->wq_len = qdev->tx_ring_size;
+		tx_ring->wq_size =
+			tx_ring->wq_len * sizeof(struct qlge_ob_mac_iocb_req);
+
+		/*
+		 * The completion queue ID for the tx rings start
+		 * immediately after the rss rings.
+		 */
+		tx_ring->cq_id = qdev->rss_ring_count + i;
+	}
+
+	for (i = 0; i < qdev->rx_ring_count; i++) {
+		rx_ring = &qdev->rx_ring[i];
+		memset((void *)rx_ring, 0, sizeof(*rx_ring));
+		rx_ring->qdev = qdev;
+		rx_ring->cq_id = i;
+		rx_ring->cpu = i % cpu_cnt;	/* CPU to run handler on. */
+		if (i < qdev->rss_ring_count) {
+			/*
+			 * Inbound (RSS) queues.
+			 */
+			rx_ring->cq_len = qdev->rx_ring_size;
+			rx_ring->cq_size =
+				rx_ring->cq_len * sizeof(struct qlge_net_rsp_iocb);
+			rx_ring->lbq.type = QLGE_LB;
+			rx_ring->sbq.type = QLGE_SB;
+			INIT_DELAYED_WORK(&rx_ring->refill_work,
+					  &qlge_slow_refill);
+		} else {
+			/*
+			 * Outbound queue handles outbound completions only.
+			 */
+			/* outbound cq is same size as tx_ring it services. */
+			rx_ring->cq_len = qdev->tx_ring_size;
+			rx_ring->cq_size =
+				rx_ring->cq_len * sizeof(struct qlge_net_rsp_iocb);
+		}
+	}
+	return 0;
+}
+
+static int qlge_open(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int err = 0;
+
+	err = qlge_adapter_reset(qdev);
+	if (err)
+		return err;
+
+	qlge_set_lb_size(qdev);
+	err = qlge_configure_rings(qdev);
+	if (err)
+		return err;
+
+	err = qlge_get_adapter_resources(qdev);
+	if (err)
+		goto error_up;
+
+	err = qlge_adapter_up(qdev);
+	if (err)
+		goto error_up;
+
+	return err;
+
+error_up:
+	qlge_release_adapter_resources(qdev);
+	return err;
+}
+
+static int qlge_change_rx_buffers(struct qlge_adapter *qdev)
+{
+	int status;
+
+	/* Wait for an outstanding reset to complete. */
+	if (!test_bit(QL_ADAPTER_UP, &qdev->flags)) {
+		int i = 4;
+
+		while (--i && !test_bit(QL_ADAPTER_UP, &qdev->flags)) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Waiting for adapter UP...\n");
+			ssleep(1);
+		}
+
+		if (!i) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Timed out waiting for adapter UP\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	status = qlge_adapter_down(qdev);
+	if (status)
+		goto error;
+
+	qlge_set_lb_size(qdev);
+
+	status = qlge_adapter_up(qdev);
+	if (status)
+		goto error;
+
+	return status;
+error:
+	netif_alert(qdev, ifup, qdev->ndev,
+		    "Driver up/down cycle failed, closing device.\n");
+	set_bit(QL_ADAPTER_UP, &qdev->flags);
+	dev_close(qdev->ndev);
+	return status;
+}
+
+static int qlge_change_mtu(struct net_device *ndev, int new_mtu)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int status;
+
+	if (ndev->mtu == 1500 && new_mtu == 9000)
+		netif_err(qdev, ifup, qdev->ndev, "Changing to jumbo MTU.\n");
+	else if (ndev->mtu == 9000 && new_mtu == 1500)
+		netif_err(qdev, ifup, qdev->ndev, "Changing to normal MTU.\n");
+	else
+		return -EINVAL;
+
+	queue_delayed_work(qdev->workqueue,
+			   &qdev->mpi_port_cfg_work, 3 * HZ);
+
+	ndev->mtu = new_mtu;
+
+	if (!netif_running(qdev->ndev))
+		return 0;
+
+	status = qlge_change_rx_buffers(qdev);
+	if (status) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Changing MTU failed.\n");
+	}
+
+	return status;
+}
+
+static struct net_device_stats *qlge_get_stats(struct net_device
+					       *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	struct rx_ring *rx_ring = &qdev->rx_ring[0];
+	struct tx_ring *tx_ring = &qdev->tx_ring[0];
+	unsigned long pkts, mcast, dropped, errors, bytes;
+	int i;
+
+	/* Get RX stats. */
+	pkts = mcast = dropped = errors = bytes = 0;
+	for (i = 0; i < qdev->rss_ring_count; i++, rx_ring++) {
+		pkts += rx_ring->rx_packets;
+		bytes += rx_ring->rx_bytes;
+		dropped += rx_ring->rx_dropped;
+		errors += rx_ring->rx_errors;
+		mcast += rx_ring->rx_multicast;
+	}
+	ndev->stats.rx_packets = pkts;
+	ndev->stats.rx_bytes = bytes;
+	ndev->stats.rx_dropped = dropped;
+	ndev->stats.rx_errors = errors;
+	ndev->stats.multicast = mcast;
+
+	/* Get TX stats. */
+	pkts = errors = bytes = 0;
+	for (i = 0; i < qdev->tx_ring_count; i++, tx_ring++) {
+		pkts += tx_ring->tx_packets;
+		bytes += tx_ring->tx_bytes;
+		errors += tx_ring->tx_errors;
+	}
+	ndev->stats.tx_packets = pkts;
+	ndev->stats.tx_bytes = bytes;
+	ndev->stats.tx_errors = errors;
+	return &ndev->stats;
+}
+
+static void qlge_set_multicast_list(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	struct netdev_hw_addr *ha;
+	int i, status;
+
+	status = qlge_sem_spinlock(qdev, SEM_RT_IDX_MASK);
+	if (status)
+		return;
+	/*
+	 * Set or clear promiscuous mode if a
+	 * transition is taking place.
+	 */
+	if (ndev->flags & IFF_PROMISC) {
+		if (!test_bit(QL_PROMISCUOUS, &qdev->flags)) {
+			if (qlge_set_routing_reg
+			    (qdev, RT_IDX_PROMISCUOUS_SLOT, RT_IDX_VALID, 1)) {
+				netif_err(qdev, hw, qdev->ndev,
+					  "Failed to set promiscuous mode.\n");
+			} else {
+				set_bit(QL_PROMISCUOUS, &qdev->flags);
+			}
+		}
+	} else {
+		if (test_bit(QL_PROMISCUOUS, &qdev->flags)) {
+			if (qlge_set_routing_reg
+			    (qdev, RT_IDX_PROMISCUOUS_SLOT, RT_IDX_VALID, 0)) {
+				netif_err(qdev, hw, qdev->ndev,
+					  "Failed to clear promiscuous mode.\n");
+			} else {
+				clear_bit(QL_PROMISCUOUS, &qdev->flags);
+			}
+		}
+	}
+
+	/*
+	 * Set or clear all multicast mode if a
+	 * transition is taking place.
+	 */
+	if ((ndev->flags & IFF_ALLMULTI) ||
+	    (netdev_mc_count(ndev) > MAX_MULTICAST_ENTRIES)) {
+		if (!test_bit(QL_ALLMULTI, &qdev->flags)) {
+			if (qlge_set_routing_reg
+			    (qdev, RT_IDX_ALLMULTI_SLOT, RT_IDX_MCAST, 1)) {
+				netif_err(qdev, hw, qdev->ndev,
+					  "Failed to set all-multi mode.\n");
+			} else {
+				set_bit(QL_ALLMULTI, &qdev->flags);
+			}
+		}
+	} else {
+		if (test_bit(QL_ALLMULTI, &qdev->flags)) {
+			if (qlge_set_routing_reg
+			    (qdev, RT_IDX_ALLMULTI_SLOT, RT_IDX_MCAST, 0)) {
+				netif_err(qdev, hw, qdev->ndev,
+					  "Failed to clear all-multi mode.\n");
+			} else {
+				clear_bit(QL_ALLMULTI, &qdev->flags);
+			}
+		}
+	}
+
+	if (!netdev_mc_empty(ndev)) {
+		status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+		if (status)
+			goto exit;
+		i = 0;
+		netdev_for_each_mc_addr(ha, ndev) {
+			if (qlge_set_mac_addr_reg(qdev, (u8 *)ha->addr,
+						  MAC_ADDR_TYPE_MULTI_MAC, i)) {
+				netif_err(qdev, hw, qdev->ndev,
+					  "Failed to loadmulticast address.\n");
+				qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+				goto exit;
+			}
+			i++;
+		}
+		qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+		if (qlge_set_routing_reg
+		    (qdev, RT_IDX_MCAST_MATCH_SLOT, RT_IDX_MCAST_MATCH, 1)) {
+			netif_err(qdev, hw, qdev->ndev,
+				  "Failed to set multicast match mode.\n");
+		} else {
+			set_bit(QL_ALLMULTI, &qdev->flags);
+		}
+	}
+exit:
+	qlge_sem_unlock(qdev, SEM_RT_IDX_MASK);
+}
+
+static int qlge_set_mac_address(struct net_device *ndev, void *p)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	struct sockaddr *addr = p;
+	int status;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+	eth_hw_addr_set(ndev, addr->sa_data);
+	/* Update local copy of current mac address. */
+	memcpy(qdev->current_mac_addr, ndev->dev_addr, ndev->addr_len);
+
+	status = qlge_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
+	if (status)
+		return status;
+	status = qlge_set_mac_addr_reg(qdev, (const u8 *)ndev->dev_addr,
+				       MAC_ADDR_TYPE_CAM_MAC,
+				       qdev->func * MAX_CQ);
+	if (status)
+		netif_err(qdev, hw, qdev->ndev, "Failed to load MAC address.\n");
+	qlge_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
+	return status;
+}
+
+static void qlge_tx_timeout(struct net_device *ndev, unsigned int txqueue)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+
+	qlge_queue_asic_error(qdev);
+}
+
+static void qlge_asic_reset_work(struct work_struct *work)
+{
+	struct qlge_adapter *qdev =
+		container_of(work, struct qlge_adapter, asic_reset_work.work);
+	int status;
+
+	rtnl_lock();
+	status = qlge_adapter_down(qdev);
+	if (status)
+		goto error;
+
+	status = qlge_adapter_up(qdev);
+	if (status)
+		goto error;
+
+	/* Restore rx mode. */
+	clear_bit(QL_ALLMULTI, &qdev->flags);
+	clear_bit(QL_PROMISCUOUS, &qdev->flags);
+	qlge_set_multicast_list(qdev->ndev);
+
+	rtnl_unlock();
+	return;
+error:
+	netif_alert(qdev, ifup, qdev->ndev,
+		    "Driver up/down cycle failed, closing device\n");
+
+	set_bit(QL_ADAPTER_UP, &qdev->flags);
+	dev_close(qdev->ndev);
+	rtnl_unlock();
+}
+
+static const struct nic_operations qla8012_nic_ops = {
+	.get_flash		= qlge_get_8012_flash_params,
+	.port_initialize	= qlge_8012_port_initialize,
+};
+
+static const struct nic_operations qla8000_nic_ops = {
+	.get_flash		= qlge_get_8000_flash_params,
+	.port_initialize	= qlge_8000_port_initialize,
+};
+
+/* Find the pcie function number for the other NIC
+ * on this chip.  Since both NIC functions share a
+ * common firmware we have the lowest enabled function
+ * do any common work.  Examples would be resetting
+ * after a fatal firmware error, or doing a firmware
+ * coredump.
+ */
+static int qlge_get_alt_pcie_func(struct qlge_adapter *qdev)
+{
+	int status = 0;
+	u32 temp;
+	u32 nic_func1, nic_func2;
+
+	status = qlge_read_mpi_reg(qdev, MPI_TEST_FUNC_PORT_CFG,
+				   &temp);
+	if (status)
+		return status;
+
+	nic_func1 = ((temp >> MPI_TEST_NIC1_FUNC_SHIFT) &
+		     MPI_TEST_NIC_FUNC_MASK);
+	nic_func2 = ((temp >> MPI_TEST_NIC2_FUNC_SHIFT) &
+		     MPI_TEST_NIC_FUNC_MASK);
+
+	if (qdev->func == nic_func1)
+		qdev->alt_func = nic_func2;
+	else if (qdev->func == nic_func2)
+		qdev->alt_func = nic_func1;
+	else
+		status = -EIO;
+
+	return status;
+}
+
+static int qlge_get_board_info(struct qlge_adapter *qdev)
+{
+	int status;
+
+	qdev->func =
+		(qlge_read32(qdev, STS) & STS_FUNC_ID_MASK) >> STS_FUNC_ID_SHIFT;
+	if (qdev->func > 3)
+		return -EIO;
+
+	status = qlge_get_alt_pcie_func(qdev);
+	if (status)
+		return status;
+
+	qdev->port = (qdev->func < qdev->alt_func) ? 0 : 1;
+	if (qdev->port) {
+		qdev->xg_sem_mask = SEM_XGMAC1_MASK;
+		qdev->port_link_up = STS_PL1;
+		qdev->port_init = STS_PI1;
+		qdev->mailbox_in = PROC_ADDR_MPI_RISC | PROC_ADDR_FUNC2_MBI;
+		qdev->mailbox_out = PROC_ADDR_MPI_RISC | PROC_ADDR_FUNC2_MBO;
+	} else {
+		qdev->xg_sem_mask = SEM_XGMAC0_MASK;
+		qdev->port_link_up = STS_PL0;
+		qdev->port_init = STS_PI0;
+		qdev->mailbox_in = PROC_ADDR_MPI_RISC | PROC_ADDR_FUNC0_MBI;
+		qdev->mailbox_out = PROC_ADDR_MPI_RISC | PROC_ADDR_FUNC0_MBO;
+	}
+	qdev->chip_rev_id = qlge_read32(qdev, REV_ID);
+	qdev->device_id = qdev->pdev->device;
+	if (qdev->device_id == QLGE_DEVICE_ID_8012)
+		qdev->nic_ops = &qla8012_nic_ops;
+	else if (qdev->device_id == QLGE_DEVICE_ID_8000)
+		qdev->nic_ops = &qla8000_nic_ops;
+	return status;
+}
+
+static void qlge_release_all(struct pci_dev *pdev)
+{
+	struct qlge_adapter *qdev = pci_get_drvdata(pdev);
+
+	if (qdev->workqueue) {
+		destroy_workqueue(qdev->workqueue);
+		qdev->workqueue = NULL;
+	}
+
+	if (qdev->reg_base)
+		iounmap(qdev->reg_base);
+	if (qdev->doorbell_area)
+		iounmap(qdev->doorbell_area);
+	vfree(qdev->mpi_coredump);
+	pci_release_regions(pdev);
+}
+
+static int qlge_init_device(struct pci_dev *pdev, struct qlge_adapter *qdev,
+			    int cards_found)
+{
+	struct net_device *ndev = qdev->ndev;
+	int err = 0;
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "PCI device enable failed.\n");
+		return err;
+	}
+
+	qdev->pdev = pdev;
+	pci_set_drvdata(pdev, qdev);
+
+	/* Set PCIe read request size */
+	err = pcie_set_readrq(pdev, 4096);
+	if (err) {
+		dev_err(&pdev->dev, "Set readrq failed.\n");
+		goto err_disable_pci;
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err) {
+		dev_err(&pdev->dev, "PCI region request failed.\n");
+		goto err_disable_pci;
+	}
+
+	pci_set_master(pdev);
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		set_bit(QL_DMA64, &qdev->flags);
+		err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
+	} else {
+		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
+		if (!err)
+			err = dma_set_coherent_mask(&pdev->dev,
+						    DMA_BIT_MASK(32));
+	}
+
+	if (err) {
+		dev_err(&pdev->dev, "No usable DMA configuration.\n");
+		goto err_release_pci;
+	}
+
+	/* Set PCIe reset type for EEH to fundamental. */
+	pdev->needs_freset = 1;
+	pci_save_state(pdev);
+	qdev->reg_base =
+		ioremap(pci_resource_start(pdev, 1), pci_resource_len(pdev, 1));
+	if (!qdev->reg_base) {
+		dev_err(&pdev->dev, "Register mapping failed.\n");
+		err = -ENOMEM;
+		goto err_release_pci;
+	}
+
+	qdev->doorbell_area_size = pci_resource_len(pdev, 3);
+	qdev->doorbell_area =
+		ioremap(pci_resource_start(pdev, 3), pci_resource_len(pdev, 3));
+	if (!qdev->doorbell_area) {
+		dev_err(&pdev->dev, "Doorbell register mapping failed.\n");
+		err = -ENOMEM;
+		goto err_iounmap_base;
+	}
+
+	err = qlge_get_board_info(qdev);
+	if (err) {
+		dev_err(&pdev->dev, "Register access failed.\n");
+		err = -EIO;
+		goto err_iounmap_doorbell;
+	}
+	qdev->msg_enable = netif_msg_init(debug, default_msg);
+	spin_lock_init(&qdev->stats_lock);
+
+	if (qlge_mpi_coredump) {
+		qdev->mpi_coredump =
+			vmalloc(sizeof(struct qlge_mpi_coredump));
+		if (!qdev->mpi_coredump) {
+			err = -ENOMEM;
+			goto err_iounmap_doorbell;
+		}
+		if (qlge_force_coredump)
+			set_bit(QL_FRC_COREDUMP, &qdev->flags);
+	}
+	/* make sure the EEPROM is good */
+	err = qdev->nic_ops->get_flash(qdev);
+	if (err) {
+		dev_err(&pdev->dev, "Invalid FLASH.\n");
+		goto err_free_mpi_coredump;
+	}
+
+	/* Keep local copy of current mac address. */
+	memcpy(qdev->current_mac_addr, ndev->dev_addr, ndev->addr_len);
+
+	/* Set up the default ring sizes. */
+	qdev->tx_ring_size = NUM_TX_RING_ENTRIES;
+	qdev->rx_ring_size = NUM_RX_RING_ENTRIES;
+
+	/* Set up the coalescing parameters. */
+	qdev->rx_coalesce_usecs = DFLT_COALESCE_WAIT;
+	qdev->tx_coalesce_usecs = DFLT_COALESCE_WAIT;
+	qdev->rx_max_coalesced_frames = DFLT_INTER_FRAME_WAIT;
+	qdev->tx_max_coalesced_frames = DFLT_INTER_FRAME_WAIT;
+
+	/*
+	 * Set up the operating parameters.
+	 */
+	qdev->workqueue = alloc_ordered_workqueue("%s", WQ_MEM_RECLAIM,
+						  ndev->name);
+	if (!qdev->workqueue) {
+		err = -ENOMEM;
+		goto err_free_mpi_coredump;
+	}
+
+	INIT_DELAYED_WORK(&qdev->asic_reset_work, qlge_asic_reset_work);
+	INIT_DELAYED_WORK(&qdev->mpi_reset_work, qlge_mpi_reset_work);
+	INIT_DELAYED_WORK(&qdev->mpi_work, qlge_mpi_work);
+	INIT_DELAYED_WORK(&qdev->mpi_port_cfg_work, qlge_mpi_port_cfg_work);
+	INIT_DELAYED_WORK(&qdev->mpi_idc_work, qlge_mpi_idc_work);
+	init_completion(&qdev->ide_completion);
+	mutex_init(&qdev->mpi_mutex);
+
+	if (!cards_found) {
+		dev_info(&pdev->dev, "%s\n", DRV_STRING);
+		dev_info(&pdev->dev, "Driver name: %s, Version: %s.\n",
+			 DRV_NAME, DRV_VERSION);
+	}
+	return 0;
+
+err_free_mpi_coredump:
+	vfree(qdev->mpi_coredump);
+err_iounmap_doorbell:
+	iounmap(qdev->doorbell_area);
+err_iounmap_base:
+	iounmap(qdev->reg_base);
+err_release_pci:
+	pci_release_regions(pdev);
+err_disable_pci:
+	pci_disable_device(pdev);
+
+	return err;
+}
+
+static const struct net_device_ops qlge_netdev_ops = {
+	.ndo_open		= qlge_open,
+	.ndo_stop		= qlge_close,
+	.ndo_start_xmit		= qlge_send,
+	.ndo_change_mtu		= qlge_change_mtu,
+	.ndo_get_stats		= qlge_get_stats,
+	.ndo_set_rx_mode	= qlge_set_multicast_list,
+	.ndo_set_mac_address	= qlge_set_mac_address,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_tx_timeout		= qlge_tx_timeout,
+	.ndo_set_features	= qlge_set_features,
+	.ndo_vlan_rx_add_vid	= qlge_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= qlge_vlan_rx_kill_vid,
+};
+
+static void qlge_timer(struct timer_list *t)
+{
+	struct qlge_adapter *qdev = from_timer(qdev, t, timer);
+	u32 var = 0;
+
+	var = qlge_read32(qdev, STS);
+	if (pci_channel_offline(qdev->pdev)) {
+		netif_err(qdev, ifup, qdev->ndev, "EEH STS = 0x%.08x.\n", var);
+		return;
+	}
+
+	mod_timer(&qdev->timer, jiffies + (5 * HZ));
+}
+
+static const struct devlink_ops qlge_devlink_ops;
+
+static int qlge_probe(struct pci_dev *pdev,
+		      const struct pci_device_id *pci_entry)
+{
+	struct qlge_netdev_priv *ndev_priv;
+	struct qlge_adapter *qdev = NULL;
+	struct net_device *ndev = NULL;
+	struct devlink *devlink;
+	static int cards_found;
+	int err;
+
+	devlink = devlink_alloc(&qlge_devlink_ops, sizeof(struct qlge_adapter),
+				&pdev->dev);
+	if (!devlink)
+		return -ENOMEM;
+
+	qdev = devlink_priv(devlink);
+
+	ndev = alloc_etherdev_mq(sizeof(struct qlge_netdev_priv),
+				 min(MAX_CPUS,
+				     netif_get_num_default_rss_queues()));
+	if (!ndev) {
+		err = -ENOMEM;
+		goto devlink_free;
+	}
+
+	ndev_priv = netdev_priv(ndev);
+	ndev_priv->qdev = qdev;
+	ndev_priv->ndev = ndev;
+	qdev->ndev = ndev;
+	err = qlge_init_device(pdev, qdev, cards_found);
+	if (err < 0)
+		goto netdev_free;
+
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+	ndev->hw_features = NETIF_F_SG |
+		NETIF_F_IP_CSUM |
+		NETIF_F_TSO |
+		NETIF_F_TSO_ECN |
+		NETIF_F_HW_VLAN_CTAG_TX |
+		NETIF_F_HW_VLAN_CTAG_RX |
+		NETIF_F_HW_VLAN_CTAG_FILTER |
+		NETIF_F_RXCSUM;
+	ndev->features = ndev->hw_features;
+	ndev->vlan_features = ndev->hw_features;
+	/* vlan gets same features (except vlan filter) */
+	ndev->vlan_features &= ~(NETIF_F_HW_VLAN_CTAG_FILTER |
+				 NETIF_F_HW_VLAN_CTAG_TX |
+				 NETIF_F_HW_VLAN_CTAG_RX);
+
+	if (test_bit(QL_DMA64, &qdev->flags))
+		ndev->features |= NETIF_F_HIGHDMA;
+
+	/*
+	 * Set up net_device structure.
+	 */
+	ndev->tx_queue_len = qdev->tx_ring_size;
+	ndev->irq = pdev->irq;
+
+	ndev->netdev_ops = &qlge_netdev_ops;
+	ndev->ethtool_ops = &qlge_ethtool_ops;
+	ndev->watchdog_timeo = 10 * HZ;
+
+	/* MTU range: this driver only supports 1500 or 9000, so this only
+	 * filters out values above or below, and we'll rely on
+	 * qlge_change_mtu to make sure only 1500 or 9000 are allowed
+	 */
+	ndev->min_mtu = ETH_DATA_LEN;
+	ndev->max_mtu = 9000;
+
+	err = register_netdev(ndev);
+	if (err) {
+		dev_err(&pdev->dev, "net device registration failed.\n");
+		goto cleanup_pdev;
+	}
+
+	err = qlge_health_create_reporters(qdev);
+	if (err)
+		goto unregister_netdev;
+
+	/* Start up the timer to trigger EEH if
+	 * the bus goes dead
+	 */
+	timer_setup(&qdev->timer, qlge_timer, TIMER_DEFERRABLE);
+	mod_timer(&qdev->timer, jiffies + (5 * HZ));
+	qlge_link_off(qdev);
+	qlge_display_dev_info(ndev);
+	atomic_set(&qdev->lb_count, 0);
+	cards_found++;
+	devlink_register(devlink);
+	return 0;
+
+unregister_netdev:
+	unregister_netdev(ndev);
+cleanup_pdev:
+	qlge_release_all(pdev);
+	pci_disable_device(pdev);
+netdev_free:
+	free_netdev(ndev);
+devlink_free:
+	devlink_free(devlink);
+
+	return err;
+}
+
+netdev_tx_t qlge_lb_send(struct sk_buff *skb, struct net_device *ndev)
+{
+	return qlge_send(skb, ndev);
+}
+
+int qlge_clean_lb_rx_ring(struct rx_ring *rx_ring, int budget)
+{
+	return qlge_clean_inbound_rx_ring(rx_ring, budget);
+}
+
+static void qlge_remove(struct pci_dev *pdev)
+{
+	struct qlge_adapter *qdev = pci_get_drvdata(pdev);
+	struct net_device *ndev = qdev->ndev;
+	struct devlink *devlink = priv_to_devlink(qdev);
+
+	devlink_unregister(devlink);
+	del_timer_sync(&qdev->timer);
+	qlge_cancel_all_work_sync(qdev);
+	unregister_netdev(ndev);
+	qlge_release_all(pdev);
+	pci_disable_device(pdev);
+	devlink_health_reporter_destroy(qdev->reporter);
+	devlink_free(devlink);
+	free_netdev(ndev);
+}
+
+/* Clean up resources without touching hardware. */
+static void qlge_eeh_close(struct net_device *ndev)
+{
+	struct qlge_adapter *qdev = netdev_to_qdev(ndev);
+	int i;
+
+	if (netif_carrier_ok(ndev)) {
+		netif_carrier_off(ndev);
+		netif_stop_queue(ndev);
+	}
+
+	/* Disabling the timer */
+	qlge_cancel_all_work_sync(qdev);
+
+	for (i = 0; i < qdev->rss_ring_count; i++)
+		netif_napi_del(&qdev->rx_ring[i].napi);
+
+	clear_bit(QL_ADAPTER_UP, &qdev->flags);
+	qlge_tx_ring_clean(qdev);
+	qlge_free_rx_buffers(qdev);
+	qlge_release_adapter_resources(qdev);
+}
+
+/*
+ * This callback is called by the PCI subsystem whenever
+ * a PCI bus error is detected.
+ */
+static pci_ers_result_t qlge_io_error_detected(struct pci_dev *pdev,
+					       pci_channel_state_t state)
+{
+	struct qlge_adapter *qdev = pci_get_drvdata(pdev);
+	struct net_device *ndev = qdev->ndev;
+
+	switch (state) {
+	case pci_channel_io_normal:
+		return PCI_ERS_RESULT_CAN_RECOVER;
+	case pci_channel_io_frozen:
+		netif_device_detach(ndev);
+		del_timer_sync(&qdev->timer);
+		if (netif_running(ndev))
+			qlge_eeh_close(ndev);
+		pci_disable_device(pdev);
+		return PCI_ERS_RESULT_NEED_RESET;
+	case pci_channel_io_perm_failure:
+		dev_err(&pdev->dev,
+			"%s: pci_channel_io_perm_failure.\n", __func__);
+		del_timer_sync(&qdev->timer);
+		qlge_eeh_close(ndev);
+		set_bit(QL_EEH_FATAL, &qdev->flags);
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	/* Request a slot reset. */
+	return PCI_ERS_RESULT_NEED_RESET;
+}
+
+/*
+ * This callback is called after the PCI buss has been reset.
+ * Basically, this tries to restart the card from scratch.
+ * This is a shortened version of the device probe/discovery code,
+ * it resembles the first-half of the () routine.
+ */
+static pci_ers_result_t qlge_io_slot_reset(struct pci_dev *pdev)
+{
+	struct qlge_adapter *qdev = pci_get_drvdata(pdev);
+
+	pdev->error_state = pci_channel_io_normal;
+
+	pci_restore_state(pdev);
+	if (pci_enable_device(pdev)) {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Cannot re-enable PCI device after reset.\n");
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+	pci_set_master(pdev);
+
+	if (qlge_adapter_reset(qdev)) {
+		netif_err(qdev, drv, qdev->ndev, "reset FAILED!\n");
+		set_bit(QL_EEH_FATAL, &qdev->flags);
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	return PCI_ERS_RESULT_RECOVERED;
+}
+
+static void qlge_io_resume(struct pci_dev *pdev)
+{
+	struct qlge_adapter *qdev = pci_get_drvdata(pdev);
+	struct net_device *ndev = qdev->ndev;
+	int err = 0;
+
+	if (netif_running(ndev)) {
+		err = qlge_open(ndev);
+		if (err) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Device initialization failed after reset.\n");
+			return;
+		}
+	} else {
+		netif_err(qdev, ifup, qdev->ndev,
+			  "Device was not running prior to EEH.\n");
+	}
+	mod_timer(&qdev->timer, jiffies + (5 * HZ));
+	netif_device_attach(ndev);
+}
+
+static const struct pci_error_handlers qlge_err_handler = {
+	.error_detected = qlge_io_error_detected,
+	.slot_reset = qlge_io_slot_reset,
+	.resume = qlge_io_resume,
+};
+
+static int __maybe_unused qlge_suspend(struct device *dev_d)
+{
+	struct pci_dev *pdev = to_pci_dev(dev_d);
+	struct qlge_adapter *qdev;
+	struct net_device *ndev;
+	int err;
+
+	qdev = pci_get_drvdata(pdev);
+	ndev = qdev->ndev;
+	netif_device_detach(ndev);
+	del_timer_sync(&qdev->timer);
+
+	if (netif_running(ndev)) {
+		err = qlge_adapter_down(qdev);
+		if (!err)
+			return err;
+	}
+
+	qlge_wol(qdev);
+
+	return 0;
+}
+
+static int __maybe_unused qlge_resume(struct device *dev_d)
+{
+	struct pci_dev *pdev = to_pci_dev(dev_d);
+	struct qlge_adapter *qdev;
+	struct net_device *ndev;
+	int err;
+
+	qdev = pci_get_drvdata(pdev);
+	ndev = qdev->ndev;
+
+	pci_set_master(pdev);
+
+	device_wakeup_disable(dev_d);
+
+	if (netif_running(ndev)) {
+		err = qlge_adapter_up(qdev);
+		if (err)
+			return err;
+	}
+
+	mod_timer(&qdev->timer, jiffies + (5 * HZ));
+	netif_device_attach(ndev);
+
+	return 0;
+}
+
+static void qlge_shutdown(struct pci_dev *pdev)
+{
+	qlge_suspend(&pdev->dev);
+}
+
+static SIMPLE_DEV_PM_OPS(qlge_pm_ops, qlge_suspend, qlge_resume);
+
+static struct pci_driver qlge_driver = {
+	.name = DRV_NAME,
+	.id_table = qlge_pci_tbl,
+	.probe = qlge_probe,
+	.remove = qlge_remove,
+	.driver.pm = &qlge_pm_ops,
+	.shutdown = qlge_shutdown,
+	.err_handler = &qlge_err_handler
+};
+
+module_pci_driver(qlge_driver);
diff --git a/drivers/staging/qlge/qlge_mpi.c b/drivers/staging/qlge/qlge_mpi.c
new file mode 100644
index 000000000..96a4de6d2
--- /dev/null
+++ b/drivers/staging/qlge/qlge_mpi.c
@@ -0,0 +1,1273 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "qlge.h"
+
+int qlge_unpause_mpi_risc(struct qlge_adapter *qdev)
+{
+	u32 tmp;
+
+	/* Un-pause the RISC */
+	tmp = qlge_read32(qdev, CSR);
+	if (!(tmp & CSR_RP))
+		return -EIO;
+
+	qlge_write32(qdev, CSR, CSR_CMD_CLR_PAUSE);
+	return 0;
+}
+
+int qlge_pause_mpi_risc(struct qlge_adapter *qdev)
+{
+	u32 tmp;
+	int count;
+
+	/* Pause the RISC */
+	qlge_write32(qdev, CSR, CSR_CMD_SET_PAUSE);
+	for (count = UDELAY_COUNT; count; count--) {
+		tmp = qlge_read32(qdev, CSR);
+		if (tmp & CSR_RP)
+			break;
+		mdelay(UDELAY_DELAY);
+	}
+	return (count == 0) ? -ETIMEDOUT : 0;
+}
+
+int qlge_hard_reset_mpi_risc(struct qlge_adapter *qdev)
+{
+	u32 tmp;
+	int count;
+
+	/* Reset the RISC */
+	qlge_write32(qdev, CSR, CSR_CMD_SET_RST);
+	for (count = UDELAY_COUNT; count; count--) {
+		tmp = qlge_read32(qdev, CSR);
+		if (tmp & CSR_RR) {
+			qlge_write32(qdev, CSR, CSR_CMD_CLR_RST);
+			break;
+		}
+		mdelay(UDELAY_DELAY);
+	}
+	return (count == 0) ? -ETIMEDOUT : 0;
+}
+
+int qlge_read_mpi_reg(struct qlge_adapter *qdev, u32 reg, u32 *data)
+{
+	int status;
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, PROC_ADDR, PROC_ADDR_RDY, PROC_ADDR_ERR);
+	if (status)
+		goto exit;
+	/* set up for reg read */
+	qlge_write32(qdev, PROC_ADDR, reg | PROC_ADDR_R);
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, PROC_ADDR, PROC_ADDR_RDY, PROC_ADDR_ERR);
+	if (status)
+		goto exit;
+	/* get the data */
+	*data = qlge_read32(qdev, PROC_DATA);
+exit:
+	return status;
+}
+
+int qlge_write_mpi_reg(struct qlge_adapter *qdev, u32 reg, u32 data)
+{
+	int status = 0;
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, PROC_ADDR, PROC_ADDR_RDY, PROC_ADDR_ERR);
+	if (status)
+		goto exit;
+	/* write the data to the data reg */
+	qlge_write32(qdev, PROC_DATA, data);
+	/* trigger the write */
+	qlge_write32(qdev, PROC_ADDR, reg);
+	/* wait for reg to come ready */
+	status = qlge_wait_reg_rdy(qdev, PROC_ADDR, PROC_ADDR_RDY, PROC_ADDR_ERR);
+	if (status)
+		goto exit;
+exit:
+	return status;
+}
+
+int qlge_soft_reset_mpi_risc(struct qlge_adapter *qdev)
+{
+	return qlge_write_mpi_reg(qdev, 0x00001010, 1);
+}
+
+/* Determine if we are in charge of the firmware. If
+ * we are the lower of the 2 NIC pcie functions, or if
+ * we are the higher function and the lower function
+ * is not enabled.
+ */
+int qlge_own_firmware(struct qlge_adapter *qdev)
+{
+	u32 temp;
+
+	/* If we are the lower of the 2 NIC functions
+	 * on the chip the we are responsible for
+	 * core dump and firmware reset after an error.
+	 */
+	if (qdev->func < qdev->alt_func)
+		return 1;
+
+	/* If we are the higher of the 2 NIC functions
+	 * on the chip and the lower function is not
+	 * enabled, then we are responsible for
+	 * core dump and firmware reset after an error.
+	 */
+	temp =  qlge_read32(qdev, STS);
+	if (!(temp & (1 << (8 + qdev->alt_func))))
+		return 1;
+
+	return 0;
+}
+
+static int qlge_get_mb_sts(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int i, status;
+
+	status = qlge_sem_spinlock(qdev, SEM_PROC_REG_MASK);
+	if (status)
+		return -EBUSY;
+	for (i = 0; i < mbcp->out_count; i++) {
+		status =
+		    qlge_read_mpi_reg(qdev, qdev->mailbox_out + i,
+				      &mbcp->mbox_out[i]);
+		if (status) {
+			netif_err(qdev, drv, qdev->ndev, "Failed mailbox read.\n");
+			break;
+		}
+	}
+	qlge_sem_unlock(qdev, SEM_PROC_REG_MASK);	/* does flush too */
+	return status;
+}
+
+/* Wait for a single mailbox command to complete.
+ * Returns zero on success.
+ */
+static int qlge_wait_mbx_cmd_cmplt(struct qlge_adapter *qdev)
+{
+	int count;
+	u32 value;
+
+	for (count = 100; count; count--) {
+		value = qlge_read32(qdev, STS);
+		if (value & STS_PI)
+			return 0;
+		mdelay(UDELAY_DELAY); /* 100ms */
+	}
+	return -ETIMEDOUT;
+}
+
+/* Execute a single mailbox command.
+ * Caller must hold PROC_ADDR semaphore.
+ */
+static int qlge_exec_mb_cmd(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int i, status;
+
+	/*
+	 * Make sure there's nothing pending.
+	 * This shouldn't happen.
+	 */
+	if (qlge_read32(qdev, CSR) & CSR_HRI)
+		return -EIO;
+
+	status = qlge_sem_spinlock(qdev, SEM_PROC_REG_MASK);
+	if (status)
+		return status;
+
+	/*
+	 * Fill the outbound mailboxes.
+	 */
+	for (i = 0; i < mbcp->in_count; i++) {
+		status = qlge_write_mpi_reg(qdev, qdev->mailbox_in + i,
+					    mbcp->mbox_in[i]);
+		if (status)
+			goto end;
+	}
+	/*
+	 * Wake up the MPI firmware.
+	 */
+	qlge_write32(qdev, CSR, CSR_CMD_SET_H2R_INT);
+end:
+	qlge_sem_unlock(qdev, SEM_PROC_REG_MASK);
+	return status;
+}
+
+/* We are being asked by firmware to accept
+ * a change to the port.  This is only
+ * a change to max frame sizes (Tx/Rx), pause
+ * parameters, or loopback mode. We wake up a worker
+ * to handler processing this since a mailbox command
+ * will need to be sent to ACK the request.
+ */
+static int qlge_idc_req_aen(struct qlge_adapter *qdev)
+{
+	int status;
+	struct mbox_params *mbcp = &qdev->idc_mbc;
+
+	netif_err(qdev, drv, qdev->ndev, "Enter!\n");
+	/* Get the status data and start up a thread to
+	 * handle the request.
+	 */
+	mbcp->out_count = 4;
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Could not read MPI, resetting ASIC!\n");
+		qlge_queue_asic_error(qdev);
+	} else	{
+		/* Begin polled mode early so
+		 * we don't get another interrupt
+		 * when we leave mpi_worker.
+		 */
+		qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16));
+		queue_delayed_work(qdev->workqueue, &qdev->mpi_idc_work, 0);
+	}
+	return status;
+}
+
+/* Process an inter-device event completion.
+ * If good, signal the caller's completion.
+ */
+static int qlge_idc_cmplt_aen(struct qlge_adapter *qdev)
+{
+	int status;
+	struct mbox_params *mbcp = &qdev->idc_mbc;
+
+	mbcp->out_count = 4;
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Could not read MPI, resetting RISC!\n");
+		qlge_queue_fw_error(qdev);
+	} else {
+		/* Wake up the sleeping mpi_idc_work thread that is
+		 * waiting for this event.
+		 */
+		complete(&qdev->ide_completion);
+	}
+	return status;
+}
+
+static void qlge_link_up(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 2;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "%s: Could not get mailbox status.\n", __func__);
+		return;
+	}
+
+	qdev->link_status = mbcp->mbox_out[1];
+	netif_err(qdev, drv, qdev->ndev, "Link Up.\n");
+
+	/* If we're coming back from an IDC event
+	 * then set up the CAM and frame routing.
+	 */
+	if (test_bit(QL_CAM_RT_SET, &qdev->flags)) {
+		status = qlge_cam_route_initialize(qdev);
+		if (status) {
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to init CAM/Routing tables.\n");
+			return;
+		}
+		clear_bit(QL_CAM_RT_SET, &qdev->flags);
+	}
+
+	/* Queue up a worker to check the frame
+	 * size information, and fix it if it's not
+	 * to our liking.
+	 */
+	if (!test_bit(QL_PORT_CFG, &qdev->flags)) {
+		netif_err(qdev, drv, qdev->ndev, "Queue Port Config Worker!\n");
+		set_bit(QL_PORT_CFG, &qdev->flags);
+		/* Begin polled mode early so
+		 * we don't get another interrupt
+		 * when we leave mpi_worker dpc.
+		 */
+		qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16));
+		queue_delayed_work(qdev->workqueue,
+				   &qdev->mpi_port_cfg_work, 0);
+	}
+
+	qlge_link_on(qdev);
+}
+
+static void qlge_link_down(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 3;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status)
+		netif_err(qdev, drv, qdev->ndev, "Link down AEN broken!\n");
+
+	qlge_link_off(qdev);
+}
+
+static int qlge_sfp_in(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 5;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status)
+		netif_err(qdev, drv, qdev->ndev, "SFP in AEN broken!\n");
+	else
+		netif_err(qdev, drv, qdev->ndev, "SFP insertion detected.\n");
+
+	return status;
+}
+
+static int qlge_sfp_out(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 1;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status)
+		netif_err(qdev, drv, qdev->ndev, "SFP out AEN broken!\n");
+	else
+		netif_err(qdev, drv, qdev->ndev, "SFP removal detected.\n");
+
+	return status;
+}
+
+static int qlge_aen_lost(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 6;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev, "Lost AEN broken!\n");
+	} else {
+		int i;
+
+		netif_err(qdev, drv, qdev->ndev, "Lost AEN detected.\n");
+		for (i = 0; i < mbcp->out_count; i++)
+			netif_err(qdev, drv, qdev->ndev, "mbox_out[%d] = 0x%.08x.\n",
+				  i, mbcp->mbox_out[i]);
+	}
+
+	return status;
+}
+
+static void qlge_init_fw_done(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+
+	mbcp->out_count = 2;
+
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev, "Firmware did not initialize!\n");
+	} else {
+		netif_err(qdev, drv, qdev->ndev, "Firmware Revision  = 0x%.08x.\n",
+			  mbcp->mbox_out[1]);
+		qdev->fw_rev_id = mbcp->mbox_out[1];
+		status = qlge_cam_route_initialize(qdev);
+		if (status)
+			netif_err(qdev, ifup, qdev->ndev,
+				  "Failed to init CAM/Routing tables.\n");
+	}
+}
+
+/* Process an async event and clear it unless it's an
+ * error condition.
+ *  This can get called iteratively from the mpi_work thread
+ *  when events arrive via an interrupt.
+ *  It also gets called when a mailbox command is polling for
+ *  it's completion.
+ */
+static int qlge_mpi_handler(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+	int orig_count = mbcp->out_count;
+
+	/* Just get mailbox zero for now. */
+	mbcp->out_count = 1;
+	status = qlge_get_mb_sts(qdev, mbcp);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Could not read MPI, resetting ASIC!\n");
+		qlge_queue_asic_error(qdev);
+		goto end;
+	}
+
+	switch (mbcp->mbox_out[0]) {
+		/* This case is only active when we arrive here
+		 * as a result of issuing a mailbox command to
+		 * the firmware.
+		 */
+	case MB_CMD_STS_INTRMDT:
+	case MB_CMD_STS_GOOD:
+	case MB_CMD_STS_INVLD_CMD:
+	case MB_CMD_STS_XFC_ERR:
+	case MB_CMD_STS_CSUM_ERR:
+	case MB_CMD_STS_ERR:
+	case MB_CMD_STS_PARAM_ERR:
+		/* We can only get mailbox status if we're polling from an
+		 * unfinished command.  Get the rest of the status data and
+		 * return back to the caller.
+		 * We only end up here when we're polling for a mailbox
+		 * command completion.
+		 */
+		mbcp->out_count = orig_count;
+		status = qlge_get_mb_sts(qdev, mbcp);
+		return status;
+
+		/* We are being asked by firmware to accept
+		 * a change to the port.  This is only
+		 * a change to max frame sizes (Tx/Rx), pause
+		 * parameters, or loopback mode.
+		 */
+	case AEN_IDC_REQ:
+		status = qlge_idc_req_aen(qdev);
+		break;
+
+		/* Process and inbound IDC event.
+		 * This will happen when we're trying to
+		 * change tx/rx max frame size, change pause
+		 * parameters or loopback mode.
+		 */
+	case AEN_IDC_CMPLT:
+	case AEN_IDC_EXT:
+		status = qlge_idc_cmplt_aen(qdev);
+		break;
+
+	case AEN_LINK_UP:
+		qlge_link_up(qdev, mbcp);
+		break;
+
+	case AEN_LINK_DOWN:
+		qlge_link_down(qdev, mbcp);
+		break;
+
+	case AEN_FW_INIT_DONE:
+		/* If we're in process on executing the firmware,
+		 * then convert the status to normal mailbox status.
+		 */
+		if (mbcp->mbox_in[0] == MB_CMD_EX_FW) {
+			mbcp->out_count = orig_count;
+			status = qlge_get_mb_sts(qdev, mbcp);
+			mbcp->mbox_out[0] = MB_CMD_STS_GOOD;
+			return status;
+		}
+		qlge_init_fw_done(qdev, mbcp);
+		break;
+
+	case AEN_AEN_SFP_IN:
+		qlge_sfp_in(qdev, mbcp);
+		break;
+
+	case AEN_AEN_SFP_OUT:
+		qlge_sfp_out(qdev, mbcp);
+		break;
+
+		/* This event can arrive at boot time or after an
+		 * MPI reset if the firmware failed to initialize.
+		 */
+	case AEN_FW_INIT_FAIL:
+		/* If we're in process on executing the firmware,
+		 * then convert the status to normal mailbox status.
+		 */
+		if (mbcp->mbox_in[0] == MB_CMD_EX_FW) {
+			mbcp->out_count = orig_count;
+			status = qlge_get_mb_sts(qdev, mbcp);
+			mbcp->mbox_out[0] = MB_CMD_STS_ERR;
+			return status;
+		}
+		netif_err(qdev, drv, qdev->ndev,
+			  "Firmware initialization failed.\n");
+		status = -EIO;
+		qlge_queue_fw_error(qdev);
+		break;
+
+	case AEN_SYS_ERR:
+		netif_err(qdev, drv, qdev->ndev, "System Error.\n");
+		qlge_queue_fw_error(qdev);
+		status = -EIO;
+		break;
+
+	case AEN_AEN_LOST:
+		qlge_aen_lost(qdev, mbcp);
+		break;
+
+	case AEN_DCBX_CHG:
+		/* Need to support AEN 8110 */
+		break;
+	default:
+		netif_err(qdev, drv, qdev->ndev,
+			  "Unsupported AE %.08x.\n", mbcp->mbox_out[0]);
+		/* Clear the MPI firmware status. */
+	}
+end:
+	qlge_write32(qdev, CSR, CSR_CMD_CLR_R2PCI_INT);
+	/* Restore the original mailbox count to
+	 * what the caller asked for.  This can get
+	 * changed when a mailbox command is waiting
+	 * for a response and an AEN arrives and
+	 * is handled.
+	 */
+	mbcp->out_count = orig_count;
+	return status;
+}
+
+/* Execute a single mailbox command.
+ * mbcp is a pointer to an array of u32.  Each
+ * element in the array contains the value for it's
+ * respective mailbox register.
+ */
+static int qlge_mailbox_command(struct qlge_adapter *qdev, struct mbox_params *mbcp)
+{
+	int status;
+	unsigned long count;
+
+	mutex_lock(&qdev->mpi_mutex);
+
+	/* Begin polled mode for MPI */
+	qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16));
+
+	/* Load the mailbox registers and wake up MPI RISC. */
+	status = qlge_exec_mb_cmd(qdev, mbcp);
+	if (status)
+		goto end;
+
+	/* If we're generating a system error, then there's nothing
+	 * to wait for.
+	 */
+	if (mbcp->mbox_in[0] == MB_CMD_MAKE_SYS_ERR)
+		goto end;
+
+	/* Wait for the command to complete. We loop
+	 * here because some AEN might arrive while
+	 * we're waiting for the mailbox command to
+	 * complete. If more than 5 seconds expire we can
+	 * assume something is wrong.
+	 */
+	count = jiffies + HZ * MAILBOX_TIMEOUT;
+	do {
+		/* Wait for the interrupt to come in. */
+		status = qlge_wait_mbx_cmd_cmplt(qdev);
+		if (status)
+			continue;
+
+		/* Process the event.  If it's an AEN, it
+		 * will be handled in-line or a worker
+		 * will be spawned. If it's our completion
+		 * we will catch it below.
+		 */
+		status = qlge_mpi_handler(qdev, mbcp);
+		if (status)
+			goto end;
+
+		/* It's either the completion for our mailbox
+		 * command complete or an AEN.  If it's our
+		 * completion then get out.
+		 */
+		if (((mbcp->mbox_out[0] & 0x0000f000) ==
+		     MB_CMD_STS_GOOD) ||
+		    ((mbcp->mbox_out[0] & 0x0000f000) ==
+		     MB_CMD_STS_INTRMDT))
+			goto done;
+	} while (time_before(jiffies, count));
+
+	netif_err(qdev, drv, qdev->ndev,
+		  "Timed out waiting for mailbox complete.\n");
+	status = -ETIMEDOUT;
+	goto end;
+
+done:
+
+	/* Now we can clear the interrupt condition
+	 * and look at our status.
+	 */
+	qlge_write32(qdev, CSR, CSR_CMD_CLR_R2PCI_INT);
+
+	if (((mbcp->mbox_out[0] & 0x0000f000) !=
+	     MB_CMD_STS_GOOD) &&
+	    ((mbcp->mbox_out[0] & 0x0000f000) !=
+	     MB_CMD_STS_INTRMDT)) {
+		status = -EIO;
+	}
+end:
+	/* End polled mode for MPI */
+	qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16) | INTR_MASK_PI);
+	mutex_unlock(&qdev->mpi_mutex);
+	return status;
+}
+
+/* Get MPI firmware version. This will be used for
+ * driver banner and for ethtool info.
+ * Returns zero on success.
+ */
+int qlge_mb_about_fw(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status = 0;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 3;
+
+	mbcp->mbox_in[0] = MB_CMD_ABOUT_FW;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed about firmware command\n");
+		status = -EIO;
+	}
+
+	/* Store the firmware version */
+	qdev->fw_rev_id = mbcp->mbox_out[1];
+
+	return status;
+}
+
+/* Get functional state for MPI firmware.
+ * Returns zero on success.
+ */
+int qlge_mb_get_fw_state(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status = 0;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 2;
+
+	mbcp->mbox_in[0] = MB_CMD_GET_FW_STATE;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Get Firmware State.\n");
+		status = -EIO;
+	}
+
+	/* If bit zero is set in mbx 1 then the firmware is
+	 * running, but not initialized.  This should never
+	 * happen.
+	 */
+	if (mbcp->mbox_out[1] & 1) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Firmware waiting for initialization.\n");
+		status = -EIO;
+	}
+
+	return status;
+}
+
+/* Send and ACK mailbox command to the firmware to
+ * let it continue with the change.
+ */
+static int qlge_mb_idc_ack(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status = 0;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 5;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_IDC_ACK;
+	mbcp->mbox_in[1] = qdev->idc_mbc.mbox_out[1];
+	mbcp->mbox_in[2] = qdev->idc_mbc.mbox_out[2];
+	mbcp->mbox_in[3] = qdev->idc_mbc.mbox_out[3];
+	mbcp->mbox_in[4] = qdev->idc_mbc.mbox_out[4];
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev, "Failed IDC ACK send.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+/* Get link settings and maximum frame size settings
+ * for the current port.
+ * Most likely will block.
+ */
+int qlge_mb_set_port_cfg(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status = 0;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 3;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_SET_PORT_CFG;
+	mbcp->mbox_in[1] = qdev->link_config;
+	mbcp->mbox_in[2] = qdev->max_frame_size;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] == MB_CMD_STS_INTRMDT) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Port Config sent, wait for IDC.\n");
+	} else	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Set Port Configuration.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+static int qlge_mb_dump_ram(struct qlge_adapter *qdev, u64 req_dma, u32 addr,
+			    u32 size)
+{
+	int status = 0;
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 9;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_DUMP_RISC_RAM;
+	mbcp->mbox_in[1] = LSW(addr);
+	mbcp->mbox_in[2] = MSW(req_dma);
+	mbcp->mbox_in[3] = LSW(req_dma);
+	mbcp->mbox_in[4] = MSW(size);
+	mbcp->mbox_in[5] = LSW(size);
+	mbcp->mbox_in[6] = MSW(MSD(req_dma));
+	mbcp->mbox_in[7] = LSW(MSD(req_dma));
+	mbcp->mbox_in[8] = MSW(addr);
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev, "Failed to dump risc RAM.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+/* Issue a mailbox command to dump RISC RAM. */
+int qlge_dump_risc_ram_area(struct qlge_adapter *qdev, void *buf,
+			    u32 ram_addr, int word_count)
+{
+	int status;
+	char *my_buf;
+	dma_addr_t buf_dma;
+
+	my_buf = dma_alloc_coherent(&qdev->pdev->dev,
+				    word_count * sizeof(u32), &buf_dma,
+				    GFP_ATOMIC);
+	if (!my_buf)
+		return -EIO;
+
+	status = qlge_mb_dump_ram(qdev, buf_dma, ram_addr, word_count);
+	if (!status)
+		memcpy(buf, my_buf, word_count * sizeof(u32));
+
+	dma_free_coherent(&qdev->pdev->dev, word_count * sizeof(u32), my_buf,
+			  buf_dma);
+	return status;
+}
+
+/* Get link settings and maximum frame size settings
+ * for the current port.
+ * Most likely will block.
+ */
+int qlge_mb_get_port_cfg(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status = 0;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 3;
+
+	mbcp->mbox_in[0] = MB_CMD_GET_PORT_CFG;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed Get Port Configuration.\n");
+		status = -EIO;
+	} else	{
+		netif_printk(qdev, drv, KERN_DEBUG, qdev->ndev,
+			     "Passed Get Port Configuration.\n");
+		qdev->link_config = mbcp->mbox_out[1];
+		qdev->max_frame_size = mbcp->mbox_out[2];
+	}
+	return status;
+}
+
+int qlge_mb_wol_mode(struct qlge_adapter *qdev, u32 wol)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 2;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_SET_WOL_MODE;
+	mbcp->mbox_in[1] = wol;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev, "Failed to set WOL mode.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+int qlge_mb_wol_set_magic(struct qlge_adapter *qdev, u32 enable_wol)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+	const u8 *addr = qdev->ndev->dev_addr;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 8;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_SET_WOL_MAGIC;
+	if (enable_wol) {
+		mbcp->mbox_in[1] = (u32)addr[0];
+		mbcp->mbox_in[2] = (u32)addr[1];
+		mbcp->mbox_in[3] = (u32)addr[2];
+		mbcp->mbox_in[4] = (u32)addr[3];
+		mbcp->mbox_in[5] = (u32)addr[4];
+		mbcp->mbox_in[6] = (u32)addr[5];
+		mbcp->mbox_in[7] = 0;
+	} else {
+		mbcp->mbox_in[1] = 0;
+		mbcp->mbox_in[2] = 1;
+		mbcp->mbox_in[3] = 1;
+		mbcp->mbox_in[4] = 1;
+		mbcp->mbox_in[5] = 1;
+		mbcp->mbox_in[6] = 1;
+		mbcp->mbox_in[7] = 0;
+	}
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev, "Failed to set WOL mode.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+/* IDC - Inter Device Communication...
+ * Some firmware commands require consent of adjacent FCOE
+ * function.  This function waits for the OK, or a
+ * counter-request for a little more time.i
+ * The firmware will complete the request if the other
+ * function doesn't respond.
+ */
+static int qlge_idc_wait(struct qlge_adapter *qdev)
+{
+	int status = -ETIMEDOUT;
+	struct mbox_params *mbcp = &qdev->idc_mbc;
+	long wait_time;
+
+	for (wait_time = 1 * HZ; wait_time;) {
+		/* Wait here for the command to complete
+		 * via the IDC process.
+		 */
+		wait_time =
+			wait_for_completion_timeout(&qdev->ide_completion,
+						    wait_time);
+		if (!wait_time) {
+			netif_err(qdev, drv, qdev->ndev, "IDC Timeout.\n");
+			break;
+		}
+		/* Now examine the response from the IDC process.
+		 * We might have a good completion or a request for
+		 * more wait time.
+		 */
+		if (mbcp->mbox_out[0] == AEN_IDC_EXT) {
+			netif_err(qdev, drv, qdev->ndev,
+				  "IDC Time Extension from function.\n");
+			wait_time += (mbcp->mbox_out[1] >> 8) & 0x0000000f;
+		} else if (mbcp->mbox_out[0] == AEN_IDC_CMPLT) {
+			netif_err(qdev, drv, qdev->ndev, "IDC Success.\n");
+			status = 0;
+			break;
+		} else {
+			netif_err(qdev, drv, qdev->ndev,
+				  "IDC: Invalid State 0x%.04x.\n",
+				  mbcp->mbox_out[0]);
+			status = -EIO;
+			break;
+		}
+	}
+
+	return status;
+}
+
+int qlge_mb_set_led_cfg(struct qlge_adapter *qdev, u32 led_config)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 2;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_SET_LED_CFG;
+	mbcp->mbox_in[1] = led_config;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed to set LED Configuration.\n");
+		status = -EIO;
+	}
+
+	return status;
+}
+
+int qlge_mb_get_led_cfg(struct qlge_adapter *qdev)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 2;
+
+	mbcp->mbox_in[0] = MB_CMD_GET_LED_CFG;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] != MB_CMD_STS_GOOD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed to get LED Configuration.\n");
+		status = -EIO;
+	} else {
+		qdev->led_config = mbcp->mbox_out[1];
+	}
+	return status;
+}
+
+int qlge_mb_set_mgmnt_traffic_ctl(struct qlge_adapter *qdev, u32 control)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 2;
+
+	mbcp->mbox_in[0] = MB_CMD_SET_MGMNT_TFK_CTL;
+	mbcp->mbox_in[1] = control;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] == MB_CMD_STS_GOOD)
+		return status;
+
+	if (mbcp->mbox_out[0] == MB_CMD_STS_INVLD_CMD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Command not supported by firmware.\n");
+		status = -EINVAL;
+	} else if (mbcp->mbox_out[0] == MB_CMD_STS_ERR) {
+		/* This indicates that the firmware is
+		 * already in the state we are trying to
+		 * change it to.
+		 */
+		netif_err(qdev, drv, qdev->ndev,
+			  "Command parameters make no change.\n");
+	}
+	return status;
+}
+
+/* Returns a negative error code or the mailbox command status. */
+static int qlge_mb_get_mgmnt_traffic_ctl(struct qlge_adapter *qdev, u32 *control)
+{
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int status;
+
+	memset(mbcp, 0, sizeof(struct mbox_params));
+	*control = 0;
+
+	mbcp->in_count = 1;
+	mbcp->out_count = 1;
+
+	mbcp->mbox_in[0] = MB_CMD_GET_MGMNT_TFK_CTL;
+
+	status = qlge_mailbox_command(qdev, mbcp);
+	if (status)
+		return status;
+
+	if (mbcp->mbox_out[0] == MB_CMD_STS_GOOD) {
+		*control = mbcp->mbox_in[1];
+		return status;
+	}
+
+	if (mbcp->mbox_out[0] == MB_CMD_STS_INVLD_CMD) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Command not supported by firmware.\n");
+		status = -EINVAL;
+	} else if (mbcp->mbox_out[0] == MB_CMD_STS_ERR) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Failed to get MPI traffic control.\n");
+		status = -EIO;
+	}
+	return status;
+}
+
+int qlge_wait_fifo_empty(struct qlge_adapter *qdev)
+{
+	int count;
+	u32 mgmnt_fifo_empty;
+	u32 nic_fifo_empty;
+
+	for (count = 6; count; count--) {
+		nic_fifo_empty = qlge_read32(qdev, STS) & STS_NFE;
+		qlge_mb_get_mgmnt_traffic_ctl(qdev, &mgmnt_fifo_empty);
+		mgmnt_fifo_empty &= MB_GET_MPI_TFK_FIFO_EMPTY;
+		if (nic_fifo_empty && mgmnt_fifo_empty)
+			return 0;
+		msleep(100);
+	}
+	return -ETIMEDOUT;
+}
+
+/* API called in work thread context to set new TX/RX
+ * maximum frame size values to match MTU.
+ */
+static int qlge_set_port_cfg(struct qlge_adapter *qdev)
+{
+	int status;
+
+	status = qlge_mb_set_port_cfg(qdev);
+	if (status)
+		return status;
+	status = qlge_idc_wait(qdev);
+	return status;
+}
+
+/* The following routines are worker threads that process
+ * events that may sleep waiting for completion.
+ */
+
+/* This thread gets the maximum TX and RX frame size values
+ * from the firmware and, if necessary, changes them to match
+ * the MTU setting.
+ */
+void qlge_mpi_port_cfg_work(struct work_struct *work)
+{
+	struct qlge_adapter *qdev =
+		container_of(work, struct qlge_adapter, mpi_port_cfg_work.work);
+	int status;
+
+	status = qlge_mb_get_port_cfg(qdev);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Bug: Failed to get port config data.\n");
+		goto err;
+	}
+
+	if (qdev->link_config & CFG_JUMBO_FRAME_SIZE &&
+	    qdev->max_frame_size == CFG_DEFAULT_MAX_FRAME_SIZE)
+		goto end;
+
+	qdev->link_config |=	CFG_JUMBO_FRAME_SIZE;
+	qdev->max_frame_size = CFG_DEFAULT_MAX_FRAME_SIZE;
+	status = qlge_set_port_cfg(qdev);
+	if (status) {
+		netif_err(qdev, drv, qdev->ndev,
+			  "Bug: Failed to set port config data.\n");
+		goto err;
+	}
+end:
+	clear_bit(QL_PORT_CFG, &qdev->flags);
+	return;
+err:
+	qlge_queue_fw_error(qdev);
+	goto end;
+}
+
+/* Process an inter-device request.  This is issues by
+ * the firmware in response to another function requesting
+ * a change to the port. We set a flag to indicate a change
+ * has been made and then send a mailbox command ACKing
+ * the change request.
+ */
+void qlge_mpi_idc_work(struct work_struct *work)
+{
+	struct qlge_adapter *qdev =
+		container_of(work, struct qlge_adapter, mpi_idc_work.work);
+	int status;
+	struct mbox_params *mbcp = &qdev->idc_mbc;
+	u32 aen;
+	int timeout;
+
+	aen = mbcp->mbox_out[1] >> 16;
+	timeout = (mbcp->mbox_out[1] >> 8) & 0xf;
+
+	switch (aen) {
+	default:
+		netif_err(qdev, drv, qdev->ndev,
+			  "Bug: Unhandled IDC action.\n");
+		break;
+	case MB_CMD_PORT_RESET:
+	case MB_CMD_STOP_FW:
+		qlge_link_off(qdev);
+		fallthrough;
+	case MB_CMD_SET_PORT_CFG:
+		/* Signal the resulting link up AEN
+		 * that the frame routing and mac addr
+		 * needs to be set.
+		 */
+		set_bit(QL_CAM_RT_SET, &qdev->flags);
+		/* Do ACK if required */
+		if (timeout) {
+			status = qlge_mb_idc_ack(qdev);
+			if (status)
+				netif_err(qdev, drv, qdev->ndev,
+					  "Bug: No pending IDC!\n");
+		} else {
+			netif_printk(qdev, drv, KERN_DEBUG, qdev->ndev,
+				     "IDC ACK not required\n");
+			status = 0; /* success */
+		}
+		break;
+
+		/* These sub-commands issued by another (FCoE)
+		 * function are requesting to do an operation
+		 * on the shared resource (MPI environment).
+		 * We currently don't issue these so we just
+		 * ACK the request.
+		 */
+	case MB_CMD_IOP_RESTART_MPI:
+	case MB_CMD_IOP_PREP_LINK_DOWN:
+		/* Drop the link, reload the routing
+		 * table when link comes up.
+		 */
+		qlge_link_off(qdev);
+		set_bit(QL_CAM_RT_SET, &qdev->flags);
+		fallthrough;
+	case MB_CMD_IOP_DVR_START:
+	case MB_CMD_IOP_FLASH_ACC:
+	case MB_CMD_IOP_CORE_DUMP_MPI:
+	case MB_CMD_IOP_PREP_UPDATE_MPI:
+	case MB_CMD_IOP_COMP_UPDATE_MPI:
+	case MB_CMD_IOP_NONE:	/*  an IDC without params */
+		/* Do ACK if required */
+		if (timeout) {
+			status = qlge_mb_idc_ack(qdev);
+			if (status)
+				netif_err(qdev, drv, qdev->ndev,
+					  "Bug: No pending IDC!\n");
+		} else {
+			netif_printk(qdev, drv, KERN_DEBUG, qdev->ndev,
+				     "IDC ACK not required\n");
+			status = 0; /* success */
+		}
+		break;
+	}
+}
+
+void qlge_mpi_work(struct work_struct *work)
+{
+	struct qlge_adapter *qdev =
+		container_of(work, struct qlge_adapter, mpi_work.work);
+	struct mbox_params mbc;
+	struct mbox_params *mbcp = &mbc;
+	int err = 0;
+
+	mutex_lock(&qdev->mpi_mutex);
+	/* Begin polled mode for MPI */
+	qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16));
+
+	while (qlge_read32(qdev, STS) & STS_PI) {
+		memset(mbcp, 0, sizeof(struct mbox_params));
+		mbcp->out_count = 1;
+		/* Don't continue if an async event
+		 * did not complete properly.
+		 */
+		err = qlge_mpi_handler(qdev, mbcp);
+		if (err)
+			break;
+	}
+
+	/* End polled mode for MPI */
+	qlge_write32(qdev, INTR_MASK, (INTR_MASK_PI << 16) | INTR_MASK_PI);
+	mutex_unlock(&qdev->mpi_mutex);
+}
+
+void qlge_mpi_reset_work(struct work_struct *work)
+{
+	struct qlge_adapter *qdev =
+		container_of(work, struct qlge_adapter, mpi_reset_work.work);
+	cancel_delayed_work_sync(&qdev->mpi_work);
+	cancel_delayed_work_sync(&qdev->mpi_port_cfg_work);
+	cancel_delayed_work_sync(&qdev->mpi_idc_work);
+	/* If we're not the dominant NIC function,
+	 * then there is nothing to do.
+	 */
+	if (!qlge_own_firmware(qdev)) {
+		netif_err(qdev, drv, qdev->ndev, "Don't own firmware!\n");
+		return;
+	}
+
+	qlge_soft_reset_mpi_risc(qdev);
+}
diff --git a/drivers/staging/r8188eu/Kconfig b/drivers/staging/r8188eu/Kconfig
new file mode 100644
index 000000000..f5fe42353
--- /dev/null
+++ b/drivers/staging/r8188eu/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+config R8188EU
+	tristate "Realtek RTL8188EU Wireless LAN NIC driver"
+	depends on WLAN && USB && CFG80211
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select LIB80211
+	select LIB80211_CRYPT_WEP
+	select LIB80211_CRYPT_CCMP
+	help
+	This option adds support for the Realtek RTL8188EU chipset, used in USB
+	devices such as the ASUS USB-N10 Nano. This newer driver is based on GitHub
+	sources for version v4.1.4_6773.20130222, and contains modifications for
+	newer kernel features. If built as a module, it will be called r8188eu.
+
diff --git a/drivers/staging/r8188eu/Makefile b/drivers/staging/r8188eu/Makefile
new file mode 100644
index 000000000..fd494c229
--- /dev/null
+++ b/drivers/staging/r8188eu/Makefile
@@ -0,0 +1,48 @@
+
+r8188eu-y = \
+		hal/HalHWImg8188E_MAC.o \
+		hal/HalHWImg8188E_BB.o \
+		hal/HalHWImg8188E_RF.o \
+		hal/HalPhyRf_8188e.o \
+		hal/HalPwrSeqCmd.o \
+		hal/Hal8188ERateAdaptive.o \
+		hal/hal_intf.o \
+		hal/hal_com.o \
+		hal/odm.o \
+		hal/odm_HWConfig.o \
+		hal/odm_RTL8188E.o \
+		hal/rtl8188e_cmd.o \
+		hal/rtl8188e_dm.o \
+		hal/rtl8188e_hal_init.o \
+		hal/rtl8188e_phycfg.o \
+		hal/rtl8188e_rf6052.o \
+		hal/rtl8188e_rxdesc.o \
+		hal/rtl8188eu_xmit.o \
+		hal/usb_halinit.o \
+		hal/usb_ops_linux.o \
+		os_dep/ioctl_linux.o \
+		os_dep/os_intfs.o \
+		os_dep/osdep_service.o \
+		os_dep/usb_intf.o \
+		os_dep/usb_ops_linux.o \
+		core/rtw_ap.o \
+		core/rtw_br_ext.o \
+		core/rtw_cmd.o \
+		core/rtw_efuse.o \
+		core/rtw_fw.o \
+		core/rtw_ieee80211.o \
+		core/rtw_ioctl_set.o \
+		core/rtw_iol.o \
+		core/rtw_led.o \
+		core/rtw_mlme.o \
+		core/rtw_mlme_ext.o \
+		core/rtw_pwrctrl.o \
+		core/rtw_p2p.o \
+		core/rtw_recv.o \
+		core/rtw_rf.o \
+		core/rtw_security.o \
+		core/rtw_sta_mgt.o \
+		core/rtw_wlan_util.o \
+		core/rtw_xmit.o
+
+obj-$(CONFIG_R8188EU) := r8188eu.o
diff --git a/drivers/staging/r8188eu/TODO b/drivers/staging/r8188eu/TODO
new file mode 100644
index 000000000..ab9d5d145
--- /dev/null
+++ b/drivers/staging/r8188eu/TODO
@@ -0,0 +1,16 @@
+To-do list:
+
+* Correct the coding style according to Linux guidelines; please read the document
+  at https://www.kernel.org/doc/html/latest/process/coding-style.html.
+* Remove unnecessary debugging/printing macros; for those that are still needed
+  use the proper kernel API (pr_debug(), dev_dbg(), netdev_dbg()).
+* Remove dead code such as unusued functions, variables, fields, etc..
+* Use in-kernel API and remove unnecessary wrappers where possible.
+* Fix bugs due to code that sleeps in atomic context.
+* Remove the HAL layer and migrate its functionality into the relevant parts of
+  the driver.
+* Switch to use LIB80211.
+* Switch to use MAC80211.
+* Switch to use CFG80211.
+* Improve the error handling of various functions, particularly those that use
+  existing kernel APIs.
diff --git a/drivers/staging/r8188eu/core/rtw_ap.c b/drivers/staging/r8188eu/core/rtw_ap.c
new file mode 100644
index 000000000..24eb8dce9
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ap.c
@@ -0,0 +1,1184 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_AP_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+#include "../include/ieee80211.h"
+#include "../include/rtl8188e_cmd.h"
+
+void init_mlme_ap_info(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	spin_lock_init(&pmlmepriv->bcn_update_lock);
+
+	/* for ACL */
+	rtw_init_queue(&pacl_list->acl_node_q);
+
+	start_ap_mode(padapter);
+}
+
+void free_mlme_ap_info(struct adapter *padapter)
+{
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = false;
+
+	rtw_sta_flush(padapter);
+
+	pmlmeinfo->state = _HW_STATE_NOLINK_;
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	/* free bc/mc sta_info */
+	psta = rtw_get_bcmc_stainfo(padapter);
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+	rtw_free_stainfo(padapter, psta);
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+static void update_BCNTIM(struct adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
+	unsigned char *pie = pnetwork_mlmeext->IEs;
+	u8 *p, *dst_ie, *premainder_ie = NULL;
+	u8 *pbackup_remainder_ie = NULL;
+	__le16 tim_bitmap_le;
+	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
+
+	/* update TIM IE */
+
+	p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen,
+		       pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+	if (p && tim_ielen > 0) {
+		tim_ielen += 2;
+		premainder_ie = p + tim_ielen;
+		tim_ie_offset = (int)(p - pie);
+		remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;
+		/* append TIM IE from dst_ie offset */
+		dst_ie = p;
+	} else {
+		tim_ielen = 0;
+
+		/* calculate head_len */
+		offset = _FIXED_IE_LENGTH_;
+		offset += pnetwork_mlmeext->Ssid.SsidLength + 2;
+
+		/*  get supported rates len */
+		p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_,
+			       &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		if (p)
+			offset += tmp_len + 2;
+
+		/* DS Parameter Set IE, len = 3 */
+		offset += 3;
+
+		premainder_ie = pie + offset;
+
+		remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;
+
+		/* append TIM IE from offset */
+		dst_ie = pie + offset;
+	}
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = kmalloc(remainder_ielen, GFP_ATOMIC);
+		if (pbackup_remainder_ie && premainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+	*dst_ie++ = _TIM_IE_;
+
+	if ((pstapriv->tim_bitmap & 0xff00) && (pstapriv->tim_bitmap & 0x00fc))
+		tim_ielen = 5;
+	else
+		tim_ielen = 4;
+
+	*dst_ie++ = tim_ielen;
+
+	*dst_ie++ = 0;/* DTIM count */
+	*dst_ie++ = 1;/* DTIM period */
+
+	if (pstapriv->tim_bitmap & BIT(0))/* for bc/mc frames */
+		*dst_ie++ = BIT(0);/* bitmap ctrl */
+	else
+		*dst_ie++ = 0;
+
+	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
+
+	if (tim_ielen == 4) {
+		*dst_ie++ = *(u8 *)&tim_bitmap_le;
+	} else if (tim_ielen == 5) {
+		memcpy(dst_ie, &tim_bitmap_le, 2);
+		dst_ie += 2;
+	}
+
+	/* copy remainder IE */
+	if (pbackup_remainder_ie) {
+		memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+		kfree(pbackup_remainder_ie);
+	}
+	offset =  (uint)(dst_ie - pie);
+	pnetwork_mlmeext->IELength = offset + remainder_ielen;
+
+	set_tx_beacon_cmd(padapter);
+}
+
+static u8 chk_sta_is_alive(struct sta_info *psta)
+{
+	u8 ret = false;
+
+	if ((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) ==
+	    (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts))
+		;
+	else
+		ret = true;
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+void	expire_timeout_chk(struct adapter *padapter)
+{
+	struct list_head *phead, *plist;
+	u8 updated = 0;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	int i;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+
+	phead = &pstapriv->auth_list;
+	plist = phead->next;
+
+	/* check auth_queue */
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, auth_list);
+		plist = plist->next;
+
+		if (psta->expire_to > 0) {
+			psta->expire_to--;
+			if (psta->expire_to == 0) {
+				list_del_init(&psta->auth_list);
+				pstapriv->auth_list_cnt--;
+
+				spin_unlock_bh(&pstapriv->auth_list_lock);
+
+				spin_lock_bh(&pstapriv->sta_hash_lock);
+				rtw_free_stainfo(padapter, psta);
+				spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+				spin_lock_bh(&pstapriv->auth_list_lock);
+			}
+		}
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	psta = NULL;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	plist = phead->next;
+
+	/* check asoc_queue */
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, asoc_list);
+		plist = plist->next;
+
+		if (chk_sta_is_alive(psta) || !psta->expire_to) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->keep_alive_trycnt = 0;
+			psta->under_exist_checking = 0;
+		} else {
+			psta->expire_to--;
+		}
+
+		if (psta->expire_to <= 0) {
+			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+			if (padapter->registrypriv.wifi_spec == 1) {
+				psta->expire_to = pstapriv->expire_to;
+				continue;
+			}
+
+			if (psta->state & WIFI_SLEEP_STATE) {
+				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+					/* to check if alive by another methods if station is at ps mode. */
+					psta->expire_to = pstapriv->expire_to;
+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+					/* to update bcn with tim_bitmap for this station */
+					pstapriv->tim_bitmap |= BIT(psta->aid);
+					update_beacon(padapter, _TIM_IE_, NULL, false);
+
+					if (!pmlmeext->active_keep_alive_check)
+						continue;
+				}
+			}
+			if (pmlmeext->active_keep_alive_check) {
+				int stainfo_offset;
+
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					chk_alive_list[chk_alive_num++] = stainfo_offset;
+				continue;
+			}
+
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+
+			updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+		} else {
+			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
+			if (psta->sleepq_len > (NR_XMITFRAME / pstapriv->asoc_list_cnt) &&
+			    padapter->xmitpriv.free_xmitframe_cnt < (NR_XMITFRAME / pstapriv->asoc_list_cnt / 2)) {
+				wakeup_sta_to_xmit(padapter, psta);
+			}
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	if (chk_alive_num) {
+		u8 backup_oper_channel = 0;
+		struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+		/* switch to correct channel of current network  before issue keep-alive frames */
+		if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+			backup_oper_channel = rtw_get_oper_ch(padapter);
+			SelectChannel(padapter, pmlmeext->cur_channel);
+		}
+
+		/* issue null data to check sta alive*/
+		for (i = 0; i < chk_alive_num; i++) {
+			int ret = _FAIL;
+
+			psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+
+			if (psta->state & WIFI_SLEEP_STATE)
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
+			else
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+
+			psta->keep_alive_trycnt++;
+			if (ret == _SUCCESS) {
+				psta->expire_to = pstapriv->expire_to;
+				psta->keep_alive_trycnt = 0;
+				continue;
+			} else if (psta->keep_alive_trycnt <= 3) {
+				psta->expire_to = 1;
+				continue;
+			}
+
+			psta->keep_alive_trycnt = 0;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+		}
+
+		if (backup_oper_channel > 0) /* back to the original operation channel */
+			SelectChannel(padapter, backup_oper_channel);
+	}
+
+	associated_clients_update(padapter, updated);
+}
+
+void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
+{
+	int i;
+	u32 init_rate = 0;
+	unsigned char sta_band = 0, raid, shortGIrate = false;
+	unsigned char limit;
+	unsigned int tx_ra_bitmap = 0;
+	struct ht_priv	*psta_ht = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+
+	if (psta)
+		psta_ht = &psta->htpriv;
+	else
+		return;
+
+	if (!(psta->state & _FW_LINKED))
+		return;
+
+	/* b/g mode ra_bitmap */
+	for (i = 0; i < sizeof(psta->bssrateset); i++) {
+		if (psta->bssrateset[i])
+			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i] & 0x7f);
+	}
+	/* n mode ra_bitmap */
+	if (psta_ht->ht_option) {
+		limit = 8; /* 1R */
+
+		for (i = 0; i < limit; i++) {
+			if (psta_ht->ht_cap.mcs.rx_mask[i / 8] & BIT(i % 8))
+				tx_ra_bitmap |= BIT(i + 12);
+		}
+
+		/* max short GI rate */
+		shortGIrate = psta_ht->sgi;
+	}
+
+	if (pcur_network->Configuration.DSConfig > 14) {
+		sta_band |= WIRELESS_INVALID;
+	} else {
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;
+		else if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11G | WIRELESS_11B;
+		else
+			sta_band |= WIRELESS_11B;
+	}
+
+	psta->wireless_mode = sta_band;
+
+	raid = networktype_to_raid(sta_band);
+	init_rate = get_highest_rate_idx(tx_ra_bitmap & 0x0fffffff) & 0x3f;
+
+	if (psta->aid < NUM_STA) {
+		u8 arg = 0;
+
+		arg = psta->mac_id & 0x1f;
+
+		arg |= BIT(7);/* support entry 2~31 */
+
+		if (shortGIrate)
+			arg |= BIT(5);
+
+		tx_ra_bitmap |= ((raid << 28) & 0xf0000000);
+
+		/* bitmap[0:27] = tx_rate_bitmap */
+		/* bitmap[28:31]= Rate Adaptive id */
+		/* arg[0:4] = macid */
+		/* arg[5] = Short GI */
+		rtl8188e_Add_RateATid(padapter, tx_ra_bitmap, arg, rssi_level);
+
+		if (shortGIrate)
+			init_rate |= BIT(6);
+
+		/* set ra_id, init_rate */
+		psta->raid = raid;
+		psta->init_rate = init_rate;
+	}
+}
+
+void update_bmc_sta(struct adapter *padapter)
+{
+	u32 init_rate = 0;
+	unsigned char	network_type, raid;
+	int i, supportRateNum = 0;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);
+
+	if (psta) {
+		psta->aid = 0;/* default set to 0 */
+		psta->mac_id = psta->aid + 1;
+
+		psta->qos_option = 0;
+		psta->htpriv.ht_option = false;
+
+		psta->ieee8021x_blocked = 0;
+
+		memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+		/* prepare for add_RATid */
+		supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates);
+		network_type = rtw_check_network_type((u8 *)&pcur_network->SupportedRates, supportRateNum, 1);
+
+		memcpy(psta->bssrateset, &pcur_network->SupportedRates, supportRateNum);
+		psta->bssratelen = supportRateNum;
+
+		/* b/g mode ra_bitmap */
+		for (i = 0; i < supportRateNum; i++) {
+			if (psta->bssrateset[i])
+				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i] & 0x7f);
+		}
+
+		if (pcur_network->Configuration.DSConfig > 14) {
+			network_type = WIRELESS_INVALID;
+		} else {
+			/* force to b mode */
+			network_type = WIRELESS_11B;
+			tx_ra_bitmap = 0xf;
+		}
+
+		raid = networktype_to_raid(network_type);
+		init_rate = get_highest_rate_idx(tx_ra_bitmap & 0x0fffffff) & 0x3f;
+
+		/* ap mode */
+		rtl8188e_SetHalODMVar(padapter, psta, true);
+
+		{
+			u8 arg = 0;
+
+			arg = psta->mac_id & 0x1f;
+			arg |= BIT(7);
+			tx_ra_bitmap |= ((raid << 28) & 0xf0000000);
+
+			/* bitmap[0:27] = tx_rate_bitmap */
+			/* bitmap[28:31]= Rate Adaptive id */
+			/* arg[0:4] = macid */
+			/* arg[5] = Short GI */
+			rtl8188e_Add_RateATid(padapter, tx_ra_bitmap, arg, 0);
+		}
+		/* set ra_id, init_rate */
+		psta->raid = raid;
+		psta->init_rate = init_rate;
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+
+		spin_lock_bh(&psta->lock);
+		psta->state = _FW_LINKED;
+		spin_unlock_bh(&psta->lock);
+	}
+}
+
+/* notes: */
+/* AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */
+/* MAC_ID = AID+1 for sta in ap/adhoc mode */
+/* MAC_ID = 1 for bc/mc for sta/ap/adhoc */
+/* MAC_ID = 0 for bssid for sta/ap/adhoc */
+/* CAM_ID = 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;
+	struct ht_priv	*phtpriv_sta = &psta->htpriv;
+	u16 sta_cap_info;
+	u16 ap_cap_info;
+
+	psta->mac_id = psta->aid + 1;
+
+	/* ap mode */
+	rtl8188e_SetHalODMVar(padapter, psta, true);
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->ieee8021x_blocked = true;
+	else
+		psta->ieee8021x_blocked = false;
+
+	/* update sta's cap */
+
+	/* ERP */
+	VCS_update(padapter, psta);
+	/* HT related cap */
+	if (phtpriv_sta->ht_option) {
+		/* check if sta supports rx ampdu */
+		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
+		sta_cap_info = le16_to_cpu(phtpriv_sta->ht_cap.cap_info);
+		ap_cap_info = le16_to_cpu(phtpriv_ap->ht_cap.cap_info);
+
+		/* check if sta support s Short GI */
+		if ((sta_cap_info & ap_cap_info) &
+		    (IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40))
+			phtpriv_sta->sgi = true;
+
+		/*  bwmode */
+		if ((sta_cap_info & ap_cap_info) & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
+			phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
+			phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+		}
+		psta->qos_option = true;
+	} else {
+		phtpriv_sta->ampdu_enable = false;
+		phtpriv_sta->sgi = false;
+		phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
+		phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	/* Rx AMPDU */
+	send_delba(padapter, 0, psta->hwaddr);/*  recipient */
+
+	/* TX AMPDU */
+	send_delba(padapter, 1, psta->hwaddr);/* originator */
+	phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */
+	phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */
+
+	/* todo: init other variables */
+
+	memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+	spin_lock_bh(&psta->lock);
+	psta->state |= _FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+}
+
+static void update_bcn_erpinfo_ie(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+	unsigned char *p, *ie = pnetwork->IEs;
+	u32 len = 0;
+
+	if (!pmlmeinfo->ERP_enable)
+		return;
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len,
+		       (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (p && len > 0) {
+		struct ndis_802_11_var_ie *pIE = (struct ndis_802_11_var_ie *)p;
+
+		if (pmlmepriv->num_sta_non_erp == 1)
+			pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION;
+		else
+			pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION);
+
+		if (pmlmepriv->num_sta_no_short_preamble > 0)
+			pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;
+		else
+			pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE);
+
+		ERP_IE_handler(padapter, pIE);
+	}
+}
+
+static void update_bcn_wps_ie(struct adapter *padapter)
+{
+	u8 *pwps_ie = NULL, *pwps_ie_src;
+	u8 *premainder_ie, *pbackup_remainder_ie = NULL;
+	uint wps_ielen = 0, wps_offset, remainder_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+	unsigned char *ie = pnetwork->IEs;
+	u32 ielen = pnetwork->IELength;
+
+	pwps_ie = rtw_get_wps_ie(ie + _FIXED_IE_LENGTH_, ielen - _FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+	if (!pwps_ie || wps_ielen == 0)
+		return;
+
+	wps_offset = (uint)(pwps_ie - ie);
+
+	premainder_ie = pwps_ie + wps_ielen;
+
+	remainder_ielen = ielen - wps_offset - wps_ielen;
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = kmalloc(remainder_ielen, GFP_ATOMIC);
+		if (pbackup_remainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (!pwps_ie_src)
+		goto exit;
+
+	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
+	if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
+		memcpy(pwps_ie, pwps_ie_src, wps_ielen + 2);
+		pwps_ie += (wps_ielen + 2);
+
+		if (pbackup_remainder_ie)
+			memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);
+
+		/* update IELength */
+		pnetwork->IELength = wps_offset + (wps_ielen + 2) + remainder_ielen;
+	}
+
+exit:
+	kfree(pbackup_remainder_ie);
+}
+
+static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
+{
+	if (!memcmp(WPS_OUI, oui, 4))
+		update_bcn_wps_ie(padapter);
+}
+
+void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
+{
+	struct mlme_priv *pmlmepriv;
+	struct mlme_ext_priv	*pmlmeext;
+
+	if (!padapter)
+		return;
+
+	pmlmepriv = &padapter->mlmepriv;
+	pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pmlmeext->bstart_bss)
+		return;
+
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	switch (ie_id) {
+	case _TIM_IE_:
+		update_BCNTIM(padapter);
+		break;
+	case _ERPINFO_IE_:
+		update_bcn_erpinfo_ie(padapter);
+		break;
+	case _VENDOR_SPECIFIC_IE_:
+		update_bcn_vendor_spec_ie(padapter, oui);
+		break;
+	default:
+		break;
+	}
+
+	pmlmepriv->update_bcn = true;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if (tx)
+		set_tx_beacon_cmd(padapter);
+}
+
+/* op_mode
+ * Set to 0 (HT pure) under the following conditions
+ *	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+ *	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+ * Set to 1 (HT non-member protection) if there may be non-HT STAs
+ *	in both the primary and the secondary channel
+ * Set to 2 if only HT STAs are associated in BSS,
+ *	however and at least one 20 MHz HT STA is associated
+ * Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+ *	(currently non-GF HT station is considered as non-HT STA also)
+ */
+static int rtw_ht_operation_update(struct adapter *padapter)
+{
+	u16 cur_op_mode, new_op_mode;
+	int op_mode_changes = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;
+
+	if (pmlmepriv->htpriv.ht_option)
+		return 0;
+
+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+	    pmlmepriv->num_sta_ht_no_gf) {
+		pmlmepriv->ht_op_mode |=
+			HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		   HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+		   pmlmepriv->num_sta_ht_no_gf == 0) {
+		pmlmepriv->ht_op_mode &=
+			~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+		op_mode_changes++;
+	}
+
+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+	    (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode &=
+			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	}
+
+	/* Note: currently we switch to the MIXED op mode if HT non-greenfield
+	 * station is associated. Probably it's a theoretical case, since
+	 * it looks like all known HT STAs support greenfield.
+	 */
+	new_op_mode = 0;
+	if (pmlmepriv->num_sta_no_ht ||
+	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))
+		new_op_mode = OP_MODE_MIXED;
+	else if ((le16_to_cpu(phtpriv_ap->ht_cap.cap_info) &
+		  IEEE80211_HT_CAP_SUP_WIDTH_20_40) &&
+		 pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+	else if (pmlmepriv->olbc_ht)
+		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+	else
+		new_op_mode = OP_MODE_PURE;
+
+	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+	if (cur_op_mode != new_op_mode) {
+		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+		pmlmepriv->ht_op_mode |= new_op_mode;
+		op_mode_changes++;
+	}
+
+	return op_mode_changes;
+}
+
+void associated_clients_update(struct adapter *padapter, u8 updated)
+{
+	/* update associated stations cap. */
+	if (updated) {
+		struct list_head *phead, *plist;
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+
+		phead = &pstapriv->asoc_list;
+		plist = phead->next;
+
+		/* check asoc_queue */
+		while (phead != plist) {
+			psta = container_of(plist, struct sta_info, asoc_list);
+
+			plist = plist->next;
+
+			VCS_update(padapter, psta);
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+	}
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) {
+		if (!psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 1;
+
+			pmlmepriv->num_sta_no_short_preamble++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	} else {
+		if (psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 0;
+
+			pmlmepriv->num_sta_no_short_preamble--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	}
+
+	if (psta->flags & WLAN_STA_NONERP) {
+		if (!psta->nonerp_set) {
+			psta->nonerp_set = 1;
+
+			pmlmepriv->num_sta_non_erp++;
+
+			if (pmlmepriv->num_sta_non_erp == 1) {
+				beacon_updated = true;
+				update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+			}
+		}
+	} else {
+		if (psta->nonerp_set) {
+			psta->nonerp_set = 0;
+
+			pmlmepriv->num_sta_non_erp--;
+
+			if (pmlmepriv->num_sta_non_erp == 0) {
+				beacon_updated = true;
+				update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+			}
+		}
+	}
+
+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT)) {
+		if (!psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 1;
+
+			pmlmepriv->num_sta_no_short_slot_time++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	} else {
+		if (psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 0;
+
+			pmlmepriv->num_sta_no_short_slot_time--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	}
+
+	if (psta->flags & WLAN_STA_HT) {
+		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
+
+		if (psta->no_ht_set) {
+			psta->no_ht_set = 0;
+			pmlmepriv->num_sta_no_ht--;
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {
+			if (!psta->no_ht_gf_set) {
+				psta->no_ht_gf_set = 1;
+				pmlmepriv->num_sta_ht_no_gf++;
+			}
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH_20_40) == 0) {
+			if (!psta->ht_20mhz_set) {
+				psta->ht_20mhz_set = 1;
+				pmlmepriv->num_sta_ht_20mhz++;
+			}
+		}
+	} else {
+		if (!psta->no_ht_set) {
+			psta->no_ht_set = 1;
+			pmlmepriv->num_sta_no_ht++;
+		}
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+	}
+
+	/* update associated stations cap. */
+	associated_clients_update(padapter,  beacon_updated);
+}
+
+u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!psta)
+		return beacon_updated;
+
+	if (psta->no_short_preamble_set) {
+		psta->no_short_preamble_set = 0;
+		pmlmepriv->num_sta_no_short_preamble--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B &&
+		    pmlmepriv->num_sta_no_short_preamble == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->nonerp_set) {
+		psta->nonerp_set = 0;
+		pmlmepriv->num_sta_non_erp--;
+		if (pmlmepriv->num_sta_non_erp == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+		}
+	}
+
+	if (psta->no_short_slot_time_set) {
+		psta->no_short_slot_time_set = 0;
+		pmlmepriv->num_sta_no_short_slot_time--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B &&
+		    pmlmepriv->num_sta_no_short_slot_time == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->no_ht_gf_set) {
+		psta->no_ht_gf_set = 0;
+		pmlmepriv->num_sta_ht_no_gf--;
+	}
+
+	if (psta->no_ht_set) {
+		psta->no_ht_set = 0;
+		pmlmepriv->num_sta_no_ht--;
+	}
+
+	if (psta->ht_20mhz_set) {
+		psta->ht_20mhz_set = 0;
+		pmlmepriv->num_sta_ht_20mhz--;
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+	}
+
+	/* update associated stations cap. */
+
+	return beacon_updated;
+}
+
+void rtw_indicate_sta_assoc_event(struct adapter *padapter, struct sta_info *psta)
+{
+	union iwreq_data wrqu;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		return;
+
+	if (psta->aid > NUM_STA)
+		return;
+
+	if (pstapriv->sta_aid[psta->aid - 1] != psta)
+		return;
+
+	wrqu.addr.sa_family = ARPHRD_ETHER;
+
+	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
+
+	wireless_send_event(padapter->pnetdev, IWEVREGISTERED, &wrqu, NULL);
+}
+
+static void rtw_indicate_sta_disassoc_event(struct adapter *padapter, struct sta_info *psta)
+{
+	union iwreq_data wrqu;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		return;
+
+	if (psta->aid > NUM_STA)
+		return;
+
+	if (pstapriv->sta_aid[psta->aid - 1] != psta)
+		return;
+
+	wrqu.addr.sa_family = ARPHRD_ETHER;
+
+	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
+
+	wireless_send_event(padapter->pnetdev, IWEVEXPIRED, &wrqu, NULL);
+}
+
+u8 ap_free_sta(struct adapter *padapter, struct sta_info *psta,
+	       bool active, u16 reason)
+{
+	u8 beacon_updated = false;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		return beacon_updated;
+
+	/* tear down Rx AMPDU */
+	send_delba(padapter, 0, psta->hwaddr);/*  recipient */
+
+	/* tear down TX AMPDU */
+	send_delba(padapter, 1, psta->hwaddr);/*  originator */
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	if (active)
+		issue_deauth(padapter, psta->hwaddr, reason);
+
+	/* clear cam entry / key */
+	rtw_clearstakey_cmd(padapter, (u8 *)psta, (u8)(psta->mac_id + 3), true);
+
+	spin_lock_bh(&psta->lock);
+	psta->state &= ~_FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+	rtw_indicate_sta_disassoc_event(padapter, psta);
+
+	report_del_sta_event(padapter, psta->hwaddr, reason);
+
+	beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+	rtw_free_stainfo(padapter, psta);
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	return beacon_updated;
+}
+
+int rtw_sta_flush(struct adapter *padapter)
+{
+	struct list_head *phead, *plist;
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return ret;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = phead->next;
+
+	/* free sta asoc_queue */
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, asoc_list);
+
+		plist = plist->next;
+
+		list_del_init(&psta->asoc_list);
+		pstapriv->asoc_list_cnt--;
+
+		ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
+
+	associated_clients_update(padapter, true);
+
+	return ret;
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void sta_info_update(struct adapter *padapter, struct sta_info *psta)
+{
+	int flags = psta->flags;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* update wmm cap. */
+	if (WLAN_STA_WME & flags)
+		psta->qos_option = 1;
+	else
+		psta->qos_option = 0;
+
+	if (pmlmepriv->qospriv.qos_option == 0)
+		psta->qos_option = 0;
+
+	/* update 802.11n ht cap. */
+	if (WLAN_STA_HT & flags) {
+		psta->htpriv.ht_option = true;
+		psta->qos_option = 1;
+	} else {
+		psta->htpriv.ht_option = false;
+	}
+
+	if (!pmlmepriv->htpriv.ht_option)
+		psta->htpriv.ht_option = false;
+
+	update_sta_info_apmode(padapter, psta);
+}
+
+void start_ap_mode(struct adapter *padapter)
+{
+	int i;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	pmlmepriv->update_bcn = false;
+
+	pmlmeext->bstart_bss = false;
+
+	pmlmepriv->num_sta_non_erp = 0;
+
+	pmlmepriv->num_sta_no_short_slot_time = 0;
+
+	pmlmepriv->num_sta_no_short_preamble = 0;
+
+	pmlmepriv->num_sta_ht_no_gf = 0;
+	pmlmepriv->num_sta_no_ht = 0;
+	pmlmepriv->num_sta_ht_20mhz = 0;
+
+	pmlmepriv->olbc = false;
+
+	pmlmepriv->olbc_ht = false;
+
+	pmlmepriv->ht_op_mode = 0;
+
+	for (i = 0; i < NUM_STA; i++)
+		pstapriv->sta_aid[i] = NULL;
+
+	pmlmepriv->wps_beacon_ie = NULL;
+	pmlmepriv->wps_probe_resp_ie = NULL;
+	pmlmepriv->wps_assoc_resp_ie = NULL;
+
+	pmlmepriv->p2p_beacon_ie = NULL;
+	pmlmepriv->p2p_probe_resp_ie = NULL;
+
+	/* for ACL */
+	INIT_LIST_HEAD(&pacl_list->acl_node_q.queue);
+	pacl_list->num = 0;
+	pacl_list->mode = 0;
+	for (i = 0; i < NUM_ACL; i++) {
+		INIT_LIST_HEAD(&pacl_list->aclnode[i].list);
+		pacl_list->aclnode[i].valid = false;
+	}
+}
+
+void stop_ap_mode(struct adapter *padapter)
+{
+	struct list_head *phead, *plist;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = false;
+
+	/* reset and init security priv , this can refine with rtw_reset_securitypriv */
+	memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv));
+	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+
+	/* for ACL */
+	spin_lock_bh(&pacl_node_q->lock);
+	phead = get_list_head(pacl_node_q);
+	plist = phead->next;
+	while (phead != plist) {
+		paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+		plist = plist->next;
+
+		if (paclnode->valid) {
+			paclnode->valid = false;
+
+			list_del_init(&paclnode->list);
+
+			pacl_list->num--;
+		}
+	}
+	spin_unlock_bh(&pacl_node_q->lock);
+
+	rtw_sta_flush(padapter);
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	psta = rtw_get_bcmc_stainfo(padapter);
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+	rtw_free_stainfo(padapter, psta);
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	rtw_init_bcmc_stainfo(padapter);
+
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+}
diff --git a/drivers/staging/r8188eu/core/rtw_br_ext.c b/drivers/staging/r8188eu/core/rtw_br_ext.c
new file mode 100644
index 000000000..4c5f30792
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_br_ext.c
@@ -0,0 +1,660 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. i*/
+
+#define _RTW_BR_EXT_C_
+
+#include "../include/linux/if_arp.h"
+#include "../include/net/ip.h"
+#include "../include/linux/atalk.h"
+#include "../include/linux/udp.h"
+#include "../include/linux/if_pppox.h"
+
+#include "../include/drv_types.h"
+#include "../include/rtw_br_ext.h"
+#include "../include/usb_osintf.h"
+
+#ifndef csum_ipv6_magic
+#include "../include/net/ip6_checksum.h"
+#endif
+
+#include "../include/linux/ipv6.h"
+#include "../include/linux/icmpv6.h"
+#include "../include/net/ndisc.h"
+#include "../include/net/checksum.h"
+
+#define NAT25_IPV4		01
+#define NAT25_IPV6		02
+#define NAT25_IPX		03
+#define NAT25_APPLE		04
+#define NAT25_PPPOE		05
+
+#define RTL_RELAY_TAG_LEN (ETH_ALEN)
+#define TAG_HDR_LEN		4
+
+#define MAGIC_CODE		0x8186
+#define MAGIC_CODE_LEN	2
+#define WAIT_TIME_PPPOE	5	/*  waiting time for pppoe server in sec */
+
+/*-----------------------------------------------------------------
+  How database records network address:
+	   0    1    2    3    4    5    6    7    8    9   10
+	|----|----|----|----|----|----|----|----|----|----|----|
+  IPv4  |type|                             |      IP addr      |
+  IPX   |type|      Net addr     |          Node addr          |
+  IPX   |type|      Net addr     |Sckt addr|
+  Apple |type| Network |node|
+  PPPoE |type|   SID   |           AC MAC            |
+-----------------------------------------------------------------*/
+
+/* Find a tag in pppoe frame and return the pointer */
+static unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type)
+{
+	unsigned char *cur_ptr, *start_ptr;
+	unsigned short tagLen, tagType;
+
+	start_ptr = (unsigned char *)ph->tag;
+	cur_ptr = (unsigned char *)ph->tag;
+	while ((cur_ptr - start_ptr) < ntohs(ph->length)) {
+		/*  prevent un-alignment access */
+		tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]);
+		tagLen  = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]);
+		if (tagType == type)
+			return cur_ptr;
+		cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen;
+	}
+	return NULL;
+}
+
+static int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag)
+{
+	struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+	int data_len;
+
+	data_len = be16_to_cpu(tag->tag_len) + TAG_HDR_LEN;
+	if (skb_tailroom(skb) < data_len)
+		return -1;
+
+	skb_put(skb, data_len);
+	/*  have a room for new tag */
+	memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length));
+	ph->length = htons(ntohs(ph->length) + data_len);
+	memcpy((unsigned char *)ph->tag, tag, data_len);
+	return data_len;
+}
+
+static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len)
+{
+	int tail_len;
+	unsigned long end, tail;
+
+	if ((src + len) > skb_tail_pointer(skb) || skb->len < len)
+		return -1;
+
+	tail = (unsigned long)skb_tail_pointer(skb);
+	end = (unsigned long)src + len;
+	if (tail < end)
+		return -1;
+
+	tail_len = (int)(tail - end);
+	if (tail_len > 0)
+		memmove(src, src + len, tail_len);
+
+	skb_trim(skb, skb->len - len);
+	return 0;
+}
+
+static int  __nat25_has_expired(struct nat25_network_db_entry *fdb)
+{
+	if (time_before_eq(fdb->ageing_timer, jiffies - NAT25_AGEING_TIME * HZ))
+		return 1;
+
+	return 0;
+}
+
+static void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr,
+				unsigned int *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV4;
+	memcpy(networkAddr + 7, (unsigned char *)ipAddr, 4);
+}
+
+static void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr,
+				unsigned char *ac_mac, __be16 *sid)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_PPPOE;
+	memcpy(networkAddr + 1, (unsigned char *)sid, 2);
+	memcpy(networkAddr + 3, (unsigned char *)ac_mac, 6);
+}
+
+static  void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr,
+				unsigned int *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV6;
+	memcpy(networkAddr + 1, (unsigned char *)ipAddr, 16);
+}
+
+static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b)
+{
+	while (len > 0) {
+		if (*data == tag && *(data + 1) == len8b && len >= len8b * 8)
+			return data + 2;
+
+		len -= (*(data + 1)) * 8;
+		data += (*(data + 1)) * 8;
+	}
+	return NULL;
+}
+
+static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac)
+{
+	struct icmp6hdr *icmphdr = (struct icmp6hdr *)data;
+	unsigned char *mac;
+
+	if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) {
+		if (len >= 8) {
+			mac = scan_tlv(&data[8], len - 8, 1, 1);
+			if (mac) {
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) {
+		if (len >= 16) {
+			mac = scan_tlv(&data[16], len - 16, 1, 1);
+			if (mac) {
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len - 24, 1, 1);
+			if (mac) {
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len - 24, 2, 1);
+			if (mac) {
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_REDIRECT) {
+		if (len >= 40) {
+			mac = scan_tlv(&data[40], len - 40, 2, 1);
+			if (mac) {
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	}
+	return 0;
+}
+
+static int __nat25_network_hash(unsigned char *networkAddr)
+{
+	if (networkAddr[0] == NAT25_IPV4) {
+		unsigned long x;
+
+		x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_IPX) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+			networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_APPLE) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_PPPOE) {
+		unsigned long x;
+
+		x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_IPV6) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+			networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^
+			networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^
+			networkAddr[16];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else {
+		unsigned long x = 0;
+		int i;
+
+		for (i = 0; i < MAX_NETWORK_ADDR_LEN; i++)
+			x ^= networkAddr[i];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+}
+
+static void __network_hash_link(struct adapter *priv,
+				struct nat25_network_db_entry *ent, int hash)
+{
+	/*  Caller must spin_lock already! */
+	ent->next_hash = priv->nethash[hash];
+	if (ent->next_hash)
+		ent->next_hash->pprev_hash = &ent->next_hash;
+	priv->nethash[hash] = ent;
+	ent->pprev_hash = &priv->nethash[hash];
+}
+
+static void __network_hash_unlink(struct nat25_network_db_entry *ent)
+{
+	/*  Caller must spin_lock already! */
+	*ent->pprev_hash = ent->next_hash;
+	if (ent->next_hash)
+		ent->next_hash->pprev_hash = ent->pprev_hash;
+	ent->next_hash = NULL;
+	ent->pprev_hash = NULL;
+}
+
+static void __nat25_db_network_insert(struct adapter *priv,
+				unsigned char *macAddr, unsigned char *networkAddr)
+{
+	struct nat25_network_db_entry *db;
+	int hash;
+
+	spin_lock_bh(&priv->br_ext_lock);
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db) {
+		if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			memcpy(db->macAddr, macAddr, ETH_ALEN);
+			db->ageing_timer = jiffies;
+			spin_unlock_bh(&priv->br_ext_lock);
+			return;
+		}
+		db = db->next_hash;
+	}
+	db = kmalloc(sizeof(*db), GFP_ATOMIC);
+	if (!db) {
+		spin_unlock_bh(&priv->br_ext_lock);
+		return;
+	}
+	memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN);
+	memcpy(db->macAddr, macAddr, ETH_ALEN);
+	atomic_set(&db->use_count, 1);
+	db->ageing_timer = jiffies;
+
+	__network_hash_link(priv, db, hash);
+
+	spin_unlock_bh(&priv->br_ext_lock);
+}
+
+/*
+ *	NAT2.5 interface
+ */
+
+void nat25_db_cleanup(struct adapter *priv)
+{
+	int i;
+
+	spin_lock_bh(&priv->br_ext_lock);
+
+	for (i = 0; i < NAT25_HASH_SIZE; i++) {
+		struct nat25_network_db_entry *f;
+
+		f = priv->nethash[i];
+		while (f) {
+			struct nat25_network_db_entry *g;
+
+			g = f->next_hash;
+			if (priv->scdb_entry == f) {
+				memset(priv->scdb_mac, 0, ETH_ALEN);
+				memset(priv->scdb_ip, 0, 4);
+				priv->scdb_entry = NULL;
+			}
+			__network_hash_unlink(f);
+			kfree(f);
+			f = g;
+		}
+	}
+	spin_unlock_bh(&priv->br_ext_lock);
+}
+
+void nat25_db_expire(struct adapter *priv)
+{
+	int i;
+
+	spin_lock_bh(&priv->br_ext_lock);
+
+	for (i = 0; i < NAT25_HASH_SIZE; i++) {
+		struct nat25_network_db_entry *f;
+
+		f = priv->nethash[i];
+		while (f) {
+			struct nat25_network_db_entry *g;
+
+			g = f->next_hash;
+			if (__nat25_has_expired(f)) {
+				if (atomic_dec_and_test(&f->use_count)) {
+					if (priv->scdb_entry == f) {
+						memset(priv->scdb_mac, 0, ETH_ALEN);
+						memset(priv->scdb_ip, 0, 4);
+						priv->scdb_entry = NULL;
+					}
+					__network_hash_unlink(f);
+					kfree(f);
+				}
+			}
+			f = g;
+		}
+	}
+	spin_unlock_bh(&priv->br_ext_lock);
+}
+
+int nat25_db_handle(struct adapter *priv, struct sk_buff *skb, int method)
+{
+	unsigned short protocol;
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+	unsigned int tmp;
+
+	if (!skb)
+		return -1;
+
+	if ((method <= NAT25_MIN) || (method >= NAT25_MAX))
+		return -1;
+
+	protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
+
+	/*---------------------------------------------------*/
+	/*                 Handle IP frame                   */
+	/*---------------------------------------------------*/
+	if (protocol == ETH_P_IP) {
+		struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+		if (((unsigned char *)(iph) + (iph->ihl << 2)) >= (skb->data + ETH_HLEN + skb->len))
+			return -1;
+
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+		case NAT25_INSERT:
+			/* some multicast with source IP is all zero, maybe other case is illegal */
+			/* in class A, B, C, host address is all zero or all one is illegal */
+			if (iph->saddr == 0)
+				return 0;
+			tmp = be32_to_cpu(iph->saddr);
+			__nat25_generate_ipv4_network_addr(networkAddr, &tmp);
+			/* record source IP address and , source mac address into db */
+			__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+			return 0;
+		default:
+			return -1;
+		}
+	} else if (protocol == ETH_P_ARP) {
+		/*---------------------------------------------------*/
+		/*                 Handle ARP frame                  */
+		/*---------------------------------------------------*/
+		struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+		unsigned char *arp_ptr = (unsigned char *)(arp + 1);
+		unsigned int *sender;
+
+		if (arp->ar_pro != htons(ETH_P_IP))
+			return -1;
+
+		switch (method) {
+		case NAT25_CHECK:
+			return 0;	/*  skb_copy for all ARP frame */
+		case NAT25_INSERT:
+			/*  change to ARP sender mac address to wlan STA address */
+			memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN);
+			arp_ptr += arp->ar_hln;
+			sender = (unsigned int *)arp_ptr;
+			__nat25_generate_ipv4_network_addr(networkAddr, sender);
+			__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+			return 0;
+		default:
+			return -1;
+		}
+	} else if ((protocol == ETH_P_PPP_DISC) ||
+		   (protocol == ETH_P_PPP_SES)) {
+		/*---------------------------------------------------*/
+		/*                Handle PPPoE frame                 */
+		/*---------------------------------------------------*/
+		struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+		__be16 *pMagic;
+
+		switch (method) {
+		case NAT25_CHECK:
+			if (ph->sid == 0)
+				return 0;
+			return 1;
+		case NAT25_INSERT:
+			if (ph->sid == 0) {	/*  Discovery phase according to tag */
+				if (ph->code == PADI_CODE || ph->code == PADR_CODE) {
+					if (priv->ethBrExtInfo.addPPPoETag) {
+						struct pppoe_tag *tag, *pOldTag;
+						unsigned char tag_buf[40];
+						int old_tag_len = 0;
+
+						tag = (struct pppoe_tag *)tag_buf;
+						pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+						if (pOldTag) { /*  if SID existed, copy old value and delete it */
+							old_tag_len = ntohs(pOldTag->tag_len);
+							if (old_tag_len +
+							    TAG_HDR_LEN +
+							    MAGIC_CODE_LEN +
+							    RTL_RELAY_TAG_LEN >
+							    sizeof(tag_buf))
+								return -1;
+
+							memcpy(tag->tag_data + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN,
+								pOldTag->tag_data, old_tag_len);
+
+							if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN + old_tag_len) < 0)
+								return -1;
+
+							ph->length = htons(ntohs(ph->length) - TAG_HDR_LEN - old_tag_len);
+						}
+
+						tag->tag_type = PTT_RELAY_SID;
+						tag->tag_len = htons(MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN + old_tag_len);
+
+						/*  insert the magic_code+client mac in relay tag */
+						pMagic = (__be16 *)tag->tag_data;
+						*pMagic = htons(MAGIC_CODE);
+						memcpy(tag->tag_data + MAGIC_CODE_LEN, skb->data + ETH_ALEN, ETH_ALEN);
+
+						/* Add relay tag */
+						if (__nat25_add_pppoe_tag(skb, tag) < 0)
+							return -1;
+					} else { /*  not add relay tag */
+						if (priv->pppoe_connection_in_progress &&
+						    memcmp(skb->data + ETH_ALEN,
+							   priv->pppoe_addr,
+							   ETH_ALEN))
+							return -2;
+
+						if (priv->pppoe_connection_in_progress == 0)
+							memcpy(priv->pppoe_addr, skb->data + ETH_ALEN, ETH_ALEN);
+
+						priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+					}
+				} else {
+					return -1;
+				}
+			} else {	/*  session phase */
+				__nat25_generate_pppoe_network_addr(networkAddr, skb->data, &ph->sid);
+
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				if (!priv->ethBrExtInfo.addPPPoETag &&
+				    priv->pppoe_connection_in_progress &&
+				    !memcmp(skb->data + ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
+					priv->pppoe_connection_in_progress = 0;
+			}
+			return 0;
+		default:
+			return -1;
+		}
+	} else if (protocol == 0x888e) {
+		/*---------------------------------------------------*/
+		/*                 Handle EAP frame                  */
+		/*---------------------------------------------------*/
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+		case NAT25_INSERT:
+			return 0;
+		default:
+			return -1;
+		}
+	} else if ((protocol == 0xe2ae) || (protocol == 0xe2af)) {
+		/*---------------------------------------------------*/
+		/*         Handle C-Media proprietary frame          */
+		/*---------------------------------------------------*/
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+		case NAT25_INSERT:
+			return 0;
+		default:
+			return -1;
+		}
+	} else if (protocol == ETH_P_IPV6) {
+		/*------------------------------------------------*/
+		/*         Handle IPV6 frame			  */
+		/*------------------------------------------------*/
+		struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+
+		if (sizeof(*iph) >= (skb->len - ETH_HLEN))
+			return -1;
+
+		switch (method) {
+		case NAT25_CHECK:
+			if (skb->data[0] & 1)
+				return 0;
+			return -1;
+		case NAT25_INSERT:
+			if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
+				__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				if (iph->nexthdr == IPPROTO_ICMPV6 &&
+						skb->len > (ETH_HLEN +  sizeof(*iph) + 4)) {
+					if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
+								      skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
+						struct icmp6hdr  *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
+						hdr->icmp6_cksum = 0;
+						hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
+										be16_to_cpu(iph->payload_len),
+										IPPROTO_ICMPV6,
+										csum_partial((__u8 *)hdr,
+										be16_to_cpu(iph->payload_len),
+										0));
+					}
+				}
+			}
+			return 0;
+		default:
+			return -1;
+		}
+	}
+	return -1;
+}
+
+#define SERVER_PORT			67
+#define CLIENT_PORT			68
+#define DHCP_MAGIC			0x63825363
+#define BROADCAST_FLAG		0x8000
+
+struct dhcpMessage {
+	u_int8_t op;
+	u_int8_t htype;
+	u_int8_t hlen;
+	u_int8_t hops;
+	u_int32_t xid;
+	__be16 secs;
+	__be16 flags;
+	__be32 ciaddr;
+	__be32 yiaddr;
+	__be32 siaddr;
+	__be32 giaddr;
+	u_int8_t chaddr[16];
+	u_int8_t sname[64];
+	u_int8_t file[128];
+	__be32 cookie;
+	u_int8_t options[308]; /* 312 - cookie */
+};
+
+void dhcp_flag_bcast(struct adapter *priv, struct sk_buff *skb)
+{
+	if (!skb)
+		return;
+
+	if (!priv->ethBrExtInfo.dhcp_bcst_disable) {
+		__be16 protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
+
+		if (protocol == __constant_htons(ETH_P_IP)) { /*  IP */
+			struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+			if (iph->protocol == IPPROTO_UDP) { /*  UDP */
+				struct udphdr *udph = (struct udphdr *)((size_t)iph + (iph->ihl << 2));
+
+				if ((udph->source == __constant_htons(CLIENT_PORT)) &&
+				    (udph->dest == __constant_htons(SERVER_PORT))) { /*  DHCP request */
+					struct dhcpMessage *dhcph =
+						(struct dhcpMessage *)((size_t)udph + sizeof(struct udphdr));
+					u32 cookie = be32_to_cpu((__be32)dhcph->cookie);
+
+					if (cookie == DHCP_MAGIC) { /*  match magic word */
+						if (!(dhcph->flags & htons(BROADCAST_FLAG))) {
+							/*  if not broadcast */
+							register int sum = 0;
+
+							/*  or BROADCAST flag */
+							dhcph->flags |= htons(BROADCAST_FLAG);
+							/*  recalculate checksum */
+							sum = ~(udph->check) & 0xffff;
+							sum += be16_to_cpu(dhcph->flags);
+							while (sum >> 16)
+								sum = (sum & 0xffff) + (sum >> 16);
+							udph->check = ~sum;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+void *scdb_findEntry(struct adapter *priv, unsigned char *ipAddr)
+{
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+	struct nat25_network_db_entry *db;
+	int hash;
+
+	__nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db) {
+		if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			return (void *)db;
+		}
+
+		db = db->next_hash;
+	}
+
+	return NULL;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_cmd.c b/drivers/staging/r8188eu/core/rtw_cmd.c
new file mode 100644
index 000000000..3fadace33
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_cmd.c
@@ -0,0 +1,1574 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_CMD_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_br_ext.h"
+#include "../include/rtw_mlme_ext.h"
+#include "../include/rtl8188e_dm.h"
+
+/* Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+ * No irqsave is necessary.
+ */
+
+static void c2h_wk_callback(struct work_struct *work);
+
+void rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	cancel_work_sync(&pevtpriv->c2h_wk);
+	while (pevtpriv->c2h_wk_alive)
+		msleep(10);
+
+	while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+		void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
+		if (c2h && c2h != (void *)pevtpriv)
+			kfree(c2h);
+	}
+}
+
+/* Calling Context:
+ *
+ * rtw_enqueue_cmd can only be called between kernel thread,
+ * since only spin_lock is used.
+ *
+ * ISR/Call-Back functions can't call this sub-function.
+ */
+
+static int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
+{
+	unsigned long flags;
+
+	if (!obj)
+		goto exit;
+
+	spin_lock_irqsave(&queue->lock, flags);
+
+	list_add_tail(&obj->list, &queue->queue);
+
+	spin_unlock_irqrestore(&queue->lock, flags);
+
+exit:
+
+	return _SUCCESS;
+}
+
+u32	rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	init_completion(&pcmdpriv->enqueue_cmd);
+	/* sema_init(&(pcmdpriv->cmd_done_sema), 0); */
+	init_completion(&pcmdpriv->start_cmd_thread);
+	init_completion(&pcmdpriv->stop_cmd_thread);
+
+	rtw_init_queue(&pcmdpriv->cmd_queue);
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+	pcmdpriv->cmd_seq = 1;
+
+	pcmdpriv->cmd_allocated_buf = kzalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
+					      GFP_KERNEL);
+
+	if (!pcmdpriv->cmd_allocated_buf)
+		return _FAIL;
+
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ((size_t)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ - 1));
+
+	pcmdpriv->rsp_allocated_buf = kzalloc(MAX_RSPSZ + 4, GFP_KERNEL);
+
+	if (!pcmdpriv->rsp_allocated_buf) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+		return _FAIL;
+	}
+
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((size_t)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_done_cnt = 0;
+	pcmdpriv->rsp_cnt = 0;
+
+	return _SUCCESS;
+}
+
+u32 rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	u32 res = _SUCCESS;
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	atomic_set(&pevtpriv->event_seq, 0);
+
+	INIT_WORK(&pevtpriv->c2h_wk, c2h_wk_callback);
+	pevtpriv->c2h_wk_alive = false;
+	pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN + 1);
+	if (!pevtpriv->c2h_queue)
+		res = _FAIL;
+
+	return res;
+}
+
+void rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	if (pcmdpriv) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+		kfree(pcmdpriv->rsp_allocated_buf);
+	}
+}
+
+static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
+
+	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+		bAllow = true;
+
+	if ((!pcmdpriv->padapter->hw_init_completed && !bAllow) ||
+	    !pcmdpriv->cmdthd_running)	/* com_thread not running */
+		return _FAIL;
+	return _SUCCESS;
+}
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	int res = _FAIL;
+	struct adapter *padapter = pcmdpriv->padapter;
+
+	if (!cmd_obj)
+		goto exit;
+
+	cmd_obj->padapter = padapter;
+
+	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+	if (res == _FAIL) {
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+	if (res == _SUCCESS)
+		complete(&pcmdpriv->enqueue_cmd);
+
+exit:
+
+	return res;
+}
+
+struct	cmd_obj	*rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+	struct cmd_obj *obj;
+	struct __queue *queue = &pcmdpriv->cmd_queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&queue->lock, flags);
+	if (list_empty(&queue->queue)) {
+		obj = NULL;
+	} else {
+		obj = container_of((&queue->queue)->next, struct cmd_obj, list);
+		list_del_init(&obj->list);
+	}
+
+	spin_unlock_irqrestore(&queue->lock, flags);
+
+	return obj;
+}
+
+void rtw_free_cmd_obj(struct cmd_obj *pcmd)
+{
+
+	if ((pcmd->cmdcode != _JoinBss_CMD_) && (pcmd->cmdcode != _CreateBss_CMD_)) {
+		/* free parmbuf in cmd_obj */
+		kfree(pcmd->parmbuf);
+	}
+
+	if (pcmd->rsp) {
+		if (pcmd->rspsz != 0) {
+			/* free rsp in cmd_obj */
+			kfree(pcmd->rsp);
+		}
+	}
+
+	/* free cmd_obj */
+	kfree(pcmd);
+
+}
+
+int rtw_cmd_thread(void *context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf;
+	u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
+	void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
+	struct adapter *padapter = (struct adapter *)context;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmdbuf = pcmdpriv->cmd_buf;
+
+	pcmdpriv->cmdthd_running = true;
+	complete(&pcmdpriv->start_cmd_thread);
+
+	while (1) {
+		wait_for_completion(&pcmdpriv->enqueue_cmd);
+
+_next:
+		if (padapter->bDriverStopped ||
+		    padapter->bSurpriseRemoved)
+			break;
+
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd)
+			continue;
+
+		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+			pcmd->res = H2C_DROPPED;
+			goto post_process;
+		}
+
+		pcmd->cmdsz = round_up(pcmd->cmdsz, 4);
+
+		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+
+		if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
+			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+
+			if (cmd_hdl) {
+				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+				pcmd->res = ret;
+			}
+
+			pcmdpriv->cmd_seq++;
+		} else {
+			pcmd->res = H2C_PARAMETERS_ERROR;
+		}
+
+		cmd_hdl = NULL;
+
+post_process:
+
+		/* call callback function for post-processed */
+		if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
+			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+			if (!pcmd_callback)
+				rtw_free_cmd_obj(pcmd);
+			else
+				/* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
+				pcmd_callback(pcmd->padapter, pcmd);/* need consider that free cmd_obj in rtw_cmd_callback */
+		} else {
+			rtw_free_cmd_obj(pcmd);
+		}
+
+		flush_signals_thread();
+
+		goto _next;
+	}
+	pcmdpriv->cmdthd_running = false;
+
+	/*  free all cmd_obj resources */
+	do {
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd)
+			break;
+
+		rtw_free_cmd_obj(pcmd);
+	} while (1);
+
+	complete(&pcmdpriv->stop_cmd_thread);
+
+	return 0;
+}
+
+/* rtw_sitesurvey_cmd(~)
+ *	### NOTE:#### (!!!!)
+ *	MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num)
+{
+	u8 res = _FAIL;
+	struct cmd_obj		*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return _FAIL;
+
+	psurveyPara = kzalloc(sizeof(*psurveyPara), GFP_ATOMIC);
+	if (!psurveyPara) {
+		kfree(ph2c);
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, false);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+	/* psurveyPara->bsslimit = 48; */
+	psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+	/* prepare ssid list */
+	if (ssid) {
+		int i;
+		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (ssid[i].SsidLength) {
+				memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
+				psurveyPara->ssid_num++;
+			}
+		}
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	if (res == _SUCCESS) {
+		pmlmepriv->scan_start_time = jiffies;
+
+		_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+
+		rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
+
+		pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+	} else {
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+
+	return res;
+}
+
+u8 rtw_setdatarate_cmd(struct adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj *ph2c;
+	struct setdatarate_parm *pbsetdataratepara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbsetdataratepara = kzalloc(sizeof(*pbsetdataratepara), GFP_ATOMIC);
+	if (!pbsetdataratepara) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
+	pbsetdataratepara->mac_id = 5;
+	memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+	return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+
+
+	kfree(pcmd->parmbuf);
+	kfree(pcmd);
+}
+
+u8 rtw_createbss_cmd(struct adapter  *padapter)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
+	u8	res = _SUCCESS;
+
+	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+	pcmd = kzalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _CreateBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)pdev_network;
+	pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	pdev_network->Length = pcmd->cmdsz;
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+
+	return res;
+}
+
+u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
+{
+	u8	res = _SUCCESS;
+	uint	t_len = 0;
+	struct wlan_bssid_ex		*psecnetwork;
+	struct cmd_obj		*pcmd;
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	enum ndis_802_11_network_infra ndis_network_mode = pnetwork->network.InfrastructureMode;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+	pcmd = kzalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+	/* for IEs is fix buf size */
+	t_len = sizeof(struct wlan_bssid_ex);
+
+	/* for hidden ap to set fw_state here */
+	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE)) {
+		switch (ndis_network_mode) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+		case Ndis802_11APMode:
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+	}
+
+	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+	if (!psecnetwork) {
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memset(psecnetwork, 0, t_len);
+
+	memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
+
+	psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength;
+
+	if (psecnetwork->IELength - 12 < 255)
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength - 12);
+	else
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], 255);
+
+	psecnetwork->IELength = 0;
+	/*  Added by Albert 2009/02/18 */
+	/*  If the driver wants to use the bssid to create the connection. */
+	/*  If not,  we have to copy the connecting AP's MAC address to it so that */
+	/*  the driver just has the bssid information for PMKIDList searching. */
+
+	if (!pmlmepriv->assoc_by_bssid)
+		memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN);
+
+	psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
+
+	pqospriv->qos_option = 0;
+
+	if (pregistrypriv->wmm_enable) {
+		u32 tmp_len;
+
+		tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
+
+		if (psecnetwork->IELength != tmp_len) {
+			psecnetwork->IELength = tmp_len;
+			pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
+		} else {
+			pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
+		}
+	}
+
+	phtpriv->ht_option = false;
+	if (pregistrypriv->ht_enable) {
+		/*	Added by Albert 2010/06/23 */
+		/*	For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
+		/*	Especially for Realtek 8192u SoftAP. */
+		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
+		    (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
+		    (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+			/* rtw_restructure_ht_ie */
+			rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0],
+									pnetwork->network.IELength, &psecnetwork->IELength);
+		}
+	}
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
+		padapter->pwrctrlpriv.smart_ps = 0;
+	else
+		padapter->pwrctrlpriv.smart_ps = padapter->registrypriv.smart_ps;
+
+	pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
+
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
+	pcmd->parmbuf = (unsigned char *)psecnetwork;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+	return res;
+}
+
+u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
+{
+	struct cmd_obj *cmdobj = NULL;
+	struct disconnect_parm *param = NULL;
+	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* prepare cmd parameter */
+	param = kzalloc(sizeof(*param), GFP_ATOMIC);
+	if (!param) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = kzalloc(sizeof(*cmdobj), GFP_ATOMIC);
+		if (!cmdobj) {
+			res = _FAIL;
+			kfree(param);
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+		res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (disconnect_hdl(padapter, (u8 *)param) != H2C_SUCCESS)
+			res = _FAIL;
+		kfree(param);
+	}
+
+exit:
+
+	return res;
+}
+
+u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infra networktype)
+{
+	struct	cmd_obj *ph2c;
+	struct	setopmode_parm *psetop;
+
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_KERNEL);
+	if (!ph2c) {
+		res = false;
+		goto exit;
+	}
+	psetop = kzalloc(sizeof(*psetop), GFP_KERNEL);
+
+	if (!psetop) {
+		kfree(ph2c);
+		res = false;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+	psetop->mode = (u8)networktype;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct sta_info *sta = (struct sta_info *)psta;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_KERNEL);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetstakey_para = kzalloc(sizeof(*psetstakey_para), GFP_KERNEL);
+	if (!psetstakey_para) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetstakey_rsp = kzalloc(sizeof(*psetstakey_rsp), GFP_KERNEL);
+	if (!psetstakey_rsp) {
+		kfree(ph2c);
+		kfree(psetstakey_para);
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+	ph2c->rsp = (u8 *)psetstakey_rsp;
+	ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+	memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		psetstakey_para->algorithm = (unsigned char)psecuritypriv->dot11PrivacyAlgrthm;
+	else
+		GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
+
+	if (unicast_key)
+		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+	else
+		memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+
+	/* jeff: set this because at least sw key is ready */
+	padapter->securitypriv.busetkipkey = true;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm	*psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	struct sta_info *sta = (struct sta_info *)psta;
+	u8	res = _SUCCESS;
+
+	if (!enqueue) {
+		clear_cam_entry(padapter, entry);
+	} else {
+		ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = kzalloc(sizeof(*psetstakey_para),
+					  GFP_ATOMIC);
+		if (!psetstakey_para) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = kzalloc(sizeof(*psetstakey_rsp),
+					 GFP_ATOMIC);
+		if (!psetstakey_rsp) {
+			kfree(ph2c);
+			kfree(psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *)psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+		memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+		psetstakey_para->algorithm = _NO_PRIVACY_;
+
+		psetstakey_para->id = entry;
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+exit:
+
+	return res;
+}
+
+u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
+{
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct addBaReq_parm *paddbareq_parm;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm = kzalloc(sizeof(*paddbareq_parm), GFP_ATOMIC);
+	if (!paddbareq_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm->tid = tid;
+	memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
+	pdrvextra_cmd_parm->type_size = 0;
+	pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+	return res;
+}
+
+u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan)
+{
+	struct	cmd_obj *pcmdobj;
+	struct	SetChannelPlan_param *setChannelPlan_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+	/* check input parameter */
+	if (!rtw_is_channel_plan_valid(chplan)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* prepare cmd parameter */
+	setChannelPlan_param = kzalloc(sizeof(*setChannelPlan_param),
+				       GFP_KERNEL);
+	if (!setChannelPlan_param) {
+		res = _FAIL;
+		goto exit;
+	}
+	setChannelPlan_param->channel_plan = chplan;
+
+	/* need enqueue, prepare cmd_obj and enqueue */
+	pcmdobj = kzalloc(sizeof(*pcmdobj), GFP_KERNEL);
+	if (!pcmdobj) {
+		kfree(setChannelPlan_param);
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+	/* do something based on res... */
+	if (res == _SUCCESS)
+		padapter->mlmepriv.ChannelPlan = chplan;
+
+exit:
+
+	return res;
+}
+
+static void traffic_status_watchdog(struct adapter *padapter)
+{
+	u8	bEnterPS;
+	u8	bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
+	u8	bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+
+	/*  */
+	/*  Determine if our traffic is busy now */
+	/*  */
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 100 ||
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 100) {
+			bBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bRxBusyTraffic = true;
+			else
+				bTxBusyTraffic = true;
+		}
+
+		/*  Higher Tx/Rx data. */
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+			bHigherBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bHigherBusyRxTraffic = true;
+			else
+				bHigherBusyTxTraffic = true;
+		}
+
+		/*  check traffic for  powersaving. */
+		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+		    (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
+			bEnterPS = false;
+		else
+			bEnterPS = true;
+
+		/*  LeisurePS only work in infra mode. */
+		if (bEnterPS)
+			LPS_Enter(padapter);
+		else
+			LPS_Leave(padapter);
+	} else {
+		LPS_Leave(padapter);
+	}
+
+	pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+}
+
+static void rtl8188e_sreset_xmit_status_check(struct adapter *padapter)
+{
+	u32 txdma_status;
+	int res;
+
+	res = rtw_read32(padapter, REG_TXDMA_STATUS, &txdma_status);
+	if (res)
+		return;
+
+	if (txdma_status != 0x00)
+		rtw_write32(padapter, REG_TXDMA_STATUS, txdma_status);
+	/* total xmit irp = 4 */
+}
+
+static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_priv *pmlmepriv;
+
+	padapter = (struct adapter *)pbuf;
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		expire_timeout_chk(padapter);
+
+	rtl8188e_sreset_xmit_status_check(padapter);
+
+	linked_status_chk(padapter);
+	traffic_status_watchdog(padapter);
+
+	rtl8188e_HalDmWatchDog(padapter);
+}
+
+static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8	mstatus;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
+		return;
+
+	switch (lps_ctrl_type) {
+	case LPS_CTRL_SCAN:
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			/* connect */
+			LPS_Leave(padapter);
+		}
+		break;
+	case LPS_CTRL_JOINBSS:
+		LPS_Leave(padapter);
+		break;
+	case LPS_CTRL_CONNECT:
+		mstatus = 1;/* connect */
+		/*  Reset LPS Setting */
+		padapter->pwrctrlpriv.LpsIdleCount = 0;
+		rtl8188e_set_FwJoinBssReport_cmd(padapter, mstatus);
+		break;
+	case LPS_CTRL_DISCONNECT:
+		mstatus = 0;/* disconnect */
+		LPS_Leave(padapter);
+		rtl8188e_set_FwJoinBssReport_cmd(padapter, mstatus);
+		break;
+	case LPS_CTRL_SPECIAL_PACKET:
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+		LPS_Leave(padapter);
+		break;
+	case LPS_CTRL_LEAVE:
+		LPS_Leave(padapter);
+		break;
+	default:
+		break;
+	}
+
+}
+
+u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	/* struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv; */
+	u8	res = _SUCCESS;
+
+	/* if (!pwrctrlpriv->bLeisurePs) */
+	/*	return res; */
+
+	if (enqueue) {
+		ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm),
+					     GFP_ATOMIC);
+		if (!pdrvextra_cmd_parm) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
+		pdrvextra_cmd_parm->type_size = lps_ctrl_type;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
+	}
+
+exit:
+
+	return res;
+}
+
+static void rpt_timer_setting_wk_hdl(struct adapter *padapter, u16 min_time)
+{
+	struct hal_data_8188e *haldata = &padapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	ODM_RA_Set_TxRPT_Time(odmpriv, min_time);
+}
+
+u8 rtw_rpt_timer_cfg_cmd(struct adapter *padapter, u16 min_time)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm),
+				     GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
+	pdrvextra_cmd_parm->type_size = min_time;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+	return res;
+}
+
+static void antenna_select_wk_hdl(struct adapter *padapter, u8 antenna)
+{
+	struct hal_data_8188e *haldata = &padapter->haldata;
+
+	/* switch current antenna to optimum antenna */
+	if (haldata->CurAntenna != antenna) {
+		ODM_UpdateRxIdleAnt_88E(&haldata->odmpriv, antenna == 2 ? MAIN_ANT : AUX_ANT);
+		haldata->CurAntenna = antenna;
+	}
+}
+
+static bool rtw_antenna_diversity(struct adapter *adapter)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+
+	return haldata->AntDivCfg != 0;
+}
+
+u8 rtw_antenna_select_cmd(struct adapter *padapter, u8 antenna, u8 enqueue)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	if (!rtw_antenna_diversity(padapter))
+		return res;
+
+	if (enqueue) {
+		ph2c = kzalloc(sizeof(*ph2c), GFP_KERNEL);
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm),
+					     GFP_KERNEL);
+		if (!pdrvextra_cmd_parm) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
+		pdrvextra_cmd_parm->type_size = antenna;
+		pdrvextra_cmd_parm->pbuf = NULL;
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		antenna_select_wk_hdl(padapter, antenna);
+	}
+exit:
+
+	return res;
+}
+
+u8 p2p_protocol_wk_cmd(struct adapter *padapter, int intCmdType)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return res;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
+	pdrvextra_cmd_parm->type_size = intCmdType;	/*	As the command type. */
+	pdrvextra_cmd_parm->pbuf = NULL;		/*	Must be NULL here */
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+u8 rtw_ps_cmd(struct adapter *padapter)
+{
+	struct cmd_obj		*ppscmd;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+	ppscmd = kzalloc(sizeof(*ppscmd), GFP_ATOMIC);
+	if (!ppscmd) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ppscmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
+
+exit:
+
+	return res;
+}
+
+static bool rtw_is_hi_queue_empty(struct adapter *adapter)
+{
+	int res;
+	u32 reg;
+
+	res = rtw_read32(adapter, REG_HGQ_INFORMATION, &reg);
+	if (res)
+		return false;
+
+	return (reg & 0x0000ff00) == 0;
+}
+
+static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
+{
+	int cnt = 0;
+	struct sta_info *psta_bmc;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return;
+
+	if (psta_bmc->sleepq_len == 0) {
+		bool val = rtw_is_hi_queue_empty(padapter);
+
+		while (!val) {
+			msleep(100);
+
+			cnt++;
+
+			if (cnt > 10)
+				break;
+
+			val = rtw_is_hi_queue_empty(padapter);
+		}
+
+		if (cnt <= 10) {
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			update_beacon(padapter, _TIM_IE_, NULL, false);
+		} else { /* re check again */
+			rtw_chk_hi_queue_cmd(padapter);
+		}
+	}
+}
+
+u8 rtw_chk_hi_queue_cmd(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
+	pdrvextra_cmd_parm->type_size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type_size = c2h_evt ? 16 : 0;
+	pdrvextra_cmd_parm->pbuf = c2h_evt;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+/* C2H event format:
+ * Field    TRIGGER      CONTENT     CMD_SEQ    CMD_LEN    CMD_ID
+ * BITS     [127:120]    [119:16]    [15:8]     [7:4]      [3:0]
+ */
+static s32 c2h_evt_read(struct adapter *adapter, u8 *buf)
+{
+	s32 ret = _FAIL;
+	struct c2h_evt_hdr *c2h_evt;
+	int i;
+	u8 trigger;
+
+	if (!buf)
+		goto exit;
+
+	ret = rtw_read8(adapter, REG_C2HEVT_CLEAR, &trigger);
+	if (ret)
+		return _FAIL;
+
+	if (trigger == C2H_EVT_HOST_CLOSE)
+		goto exit; /* Not ready */
+	else if (trigger != C2H_EVT_FW_CLOSE)
+		goto clear_evt; /* Not a valid value */
+
+	c2h_evt = (struct c2h_evt_hdr *)buf;
+
+	memset(c2h_evt, 0, 16);
+
+	ret = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL, buf);
+	if (ret) {
+		ret = _FAIL;
+		goto clear_evt;
+	}
+
+	ret = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 1, buf + 1);
+	if (ret) {
+		ret = _FAIL;
+		goto clear_evt;
+	}
+	/* Read the content */
+	for (i = 0; i < c2h_evt->plen; i++) {
+		ret = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL +
+				sizeof(*c2h_evt) + i, c2h_evt->payload + i);
+		if (ret) {
+			ret = _FAIL;
+			goto clear_evt;
+		}
+	}
+
+	ret = _SUCCESS;
+
+clear_evt:
+	/* Clear event to notify FW we have read the command.
+	 * If this field isn't clear, the FW won't update the next
+	 * command message.
+	 */
+	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+exit:
+	return ret;
+}
+
+static void c2h_evt_hdl(struct adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter)
+{
+	u8 buf[16];
+
+	if (!c2h_evt)
+		c2h_evt_read(adapter, buf);
+}
+
+static void c2h_wk_callback(struct work_struct *work)
+{
+	struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv);
+	struct c2h_evt_hdr *c2h_evt;
+
+	evtpriv->c2h_wk_alive = true;
+
+	while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+		c2h_evt = (struct c2h_evt_hdr *)rtw_cbuf_pop(evtpriv->c2h_queue);
+		if (c2h_evt) {
+			/* This C2H event is read, clear it */
+			rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+		} else {
+			c2h_evt = kmalloc(16, GFP_KERNEL);
+			if (c2h_evt) {
+				/* This C2H event is not read, read & clear now */
+				if (c2h_evt_read(adapter, (u8 *)c2h_evt) != _SUCCESS) {
+					kfree(c2h_evt);
+					continue;
+				}
+			} else {
+				return;
+			}
+		}
+
+		/* Special pointer to trigger c2h_evt_clear only */
+		if ((void *)c2h_evt == (void *)evtpriv)
+			continue;
+
+		if (!c2h_evt_exist(c2h_evt)) {
+			kfree(c2h_evt);
+			continue;
+		}
+
+		/* Enqueue into cmd_thread for others */
+		rtw_c2h_wk_cmd(adapter, (u8 *)c2h_evt);
+	}
+
+	evtpriv->c2h_wk_alive = false;
+}
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct drvextra_cmd_parm *pdrvextra_cmd;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
+
+	switch (pdrvextra_cmd->ec_id) {
+	case DYNAMIC_CHK_WK_CID:
+		dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf);
+		break;
+	case POWER_SAVING_CTRL_WK_CID:
+		rtw_ps_processor(padapter);
+		break;
+	case LPS_CTRL_WK_CID:
+		lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
+		break;
+	case RTP_TIMER_CFG_WK_CID:
+		rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type_size);
+		break;
+	case ANT_SELECT_WK_CID:
+		antenna_select_wk_hdl(padapter, pdrvextra_cmd->type_size);
+		break;
+	case P2P_PS_WK_CID:
+		p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type_size);
+		break;
+	case P2P_PROTO_WK_CID:
+		/*	Commented by Albert 2011/07/01 */
+		/*	I used the type_size as the type command */
+		p2p_protocol_wk_hdl(padapter, pdrvextra_cmd->type_size);
+		break;
+	case CHECK_HIQ_WK_CID:
+		rtw_chk_hi_queue_hdl(padapter);
+		break;
+	case C2H_WK_CID:
+		c2h_evt_hdl(padapter, (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
+		break;
+	default:
+		break;
+	}
+
+	if (pdrvextra_cmd->pbuf && pdrvextra_cmd->type_size > 0)
+		kfree(pdrvextra_cmd->pbuf);
+
+	return H2C_SUCCESS;
+}
+
+void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+	}
+
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		spin_lock_bh(&pmlmepriv->lock);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		spin_unlock_bh(&pmlmepriv->lock);
+
+		return;
+	}
+
+	/* clear bridge database */
+	nat25_db_cleanup(padapter);
+
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	if (pcmd->res != H2C_SUCCESS)
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+	del_timer_sync(&pmlmepriv->assoc_timer);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+		if (!psta) {
+			psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+			if (!psta)
+				goto createbss_cmd_fail;
+		}
+
+		rtw_indicate_connect(padapter);
+	} else {
+
+		pwlan = rtw_alloc_network(pmlmepriv);
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+		if (!pwlan) {
+			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+			if (!pwlan) {
+				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+				goto createbss_cmd_fail;
+			}
+			pwlan->last_scanned = jiffies;
+		} else {
+			list_add_tail(&pwlan->list, &pmlmepriv->scanned_queue.queue);
+		}
+
+		pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
+		memcpy(&pwlan->network, pnetwork, pnetwork->Length);
+
+		memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		/*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
+	}
+
+createbss_cmd_fail:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
+	struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
+
+	if (!psta)
+		goto exit;
+exit:
+	rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
+	struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
+	struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
+
+	if (!psta)
+		goto exit;
+
+	psta->aid = passocsta_rsp->cam_id;
+	psta->mac_id = passocsta_rsp->cam_id;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) && check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+
+}
diff --git a/drivers/staging/r8188eu/core/rtw_efuse.c b/drivers/staging/r8188eu/core/rtw_efuse.c
new file mode 100644
index 000000000..df9534dd2
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_efuse.c
@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTW_EFUSE_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_efuse.h"
+#include "../include/rtl8188e_hal.h"
+
+/*  */
+/* 	Description: */
+/* 		Execute E-Fuse read byte operation. */
+/* 		Referred from SD1 Richard. */
+/*  */
+/* 	Assumption: */
+/* 		1. Boot from E-Fuse and successfully auto-load. */
+/* 		2. PASSIVE_LEVEL (USB interface) */
+/*  */
+/* 	Created by Roger, 2008.10.21. */
+/*  */
+void
+ReadEFuseByte(
+		struct adapter *Adapter,
+		u16 _offset,
+		u8 *pbuf)
+{
+	u32 value32;
+	u8 readbyte;
+	u16 retry;
+	int res;
+
+	/* Write Address */
+	rtw_write8(Adapter, EFUSE_CTRL + 1, (_offset & 0xff));
+	res = rtw_read8(Adapter, EFUSE_CTRL + 2, &readbyte);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, EFUSE_CTRL + 2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+
+	/* Write bit 32 0 */
+	res = rtw_read8(Adapter, EFUSE_CTRL + 3, &readbyte);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, EFUSE_CTRL + 3, (readbyte & 0x7f));
+
+	/* Check bit 32 read-ready */
+	res = rtw_read32(Adapter, EFUSE_CTRL, &value32);
+	if (res)
+		return;
+
+	for (retry = 0; retry < 10000; retry++) {
+		res = rtw_read32(Adapter, EFUSE_CTRL, &value32);
+		if (res)
+			continue;
+
+		if (((value32 >> 24) & 0xff) & 0x80)
+			break;
+	}
+
+	/*  20100205 Joseph: Add delay suggested by SD1 Victor. */
+	/*  This fix the problem that Efuse read error in high temperature condition. */
+	/*  Designer says that there shall be some delay after ready bit is set, or the */
+	/*  result will always stay on last data we read. */
+	udelay(50);
+	res = rtw_read32(Adapter, EFUSE_CTRL, &value32);
+	if (res)
+		return;
+
+	*pbuf = (u8)(value32 & 0xff);
+
+	/* FIXME: return an error to caller */
+}
diff --git a/drivers/staging/r8188eu/core/rtw_fw.c b/drivers/staging/r8188eu/core/rtw_fw.c
new file mode 100644
index 000000000..682c65b1e
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_fw.c
@@ -0,0 +1,337 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include <linux/firmware.h>
+#include "../include/rtw_fw.h"
+
+#define MAX_REG_BLOCK_SIZE	196
+#define FW_8188E_START_ADDRESS	0x1000
+#define MAX_PAGE_SIZE		4096
+
+#define IS_FW_HEADER_EXIST(_fwhdr)				\
+	((le16_to_cpu(_fwhdr->signature) & 0xFFF0) == 0x92C0 ||	\
+	(le16_to_cpu(_fwhdr->signature) & 0xFFF0) == 0x88C0 ||	\
+	(le16_to_cpu(_fwhdr->signature) & 0xFFF0) == 0x2300 ||	\
+	(le16_to_cpu(_fwhdr->signature) & 0xFFF0) == 0x88E0)
+
+struct rt_firmware_hdr {
+	__le16	signature;	/* 92C0: test chip; 92C,
+				 * 88C0: test chip; 88C1: MP A-cut;
+				 * 92C1: MP A-cut */
+	u8	category;	/* AP/NIC and USB/PCI */
+	u8	function;	/* Reserved for different FW function
+				 * indcation, for further use when
+				 * driver needs to download different
+				 * FW for different conditions */
+	__le16	version;	/* FW Version */
+	u8	subversion;	/* FW Subversion, default 0x00 */
+	u8	rsvd1;
+	u8	month;		/* Release time Month field */
+	u8	date;		/* Release time Date field */
+	u8	hour;		/* Release time Hour field */
+	u8	minute;		/* Release time Minute field */
+	__le16	ramcodesize;	/* The size of RAM code */
+	u8	foundry;
+	u8	rsvd2;
+	__le32	svnidx;		/* The SVN entry index */
+	__le32	rsvd3;
+	__le32	rsvd4;
+	__le32	rsvd5;
+};
+
+static_assert(sizeof(struct rt_firmware_hdr) == 32);
+
+static void fw_download_enable(struct adapter *padapter, bool enable)
+{
+	u8 tmp;
+	int res;
+
+	if (enable) {
+		/*  MCU firmware download enable. */
+		res = rtw_read8(padapter, REG_MCUFWDL, &tmp);
+		if (res)
+			return;
+
+		rtw_write8(padapter, REG_MCUFWDL, tmp | 0x01);
+
+		/*  8051 reset */
+		res = rtw_read8(padapter, REG_MCUFWDL + 2, &tmp);
+		if (res)
+			return;
+
+		rtw_write8(padapter, REG_MCUFWDL + 2, tmp & 0xf7);
+	} else {
+		/*  MCU firmware download disable. */
+		res = rtw_read8(padapter, REG_MCUFWDL, &tmp);
+		if (res)
+			return;
+
+		rtw_write8(padapter, REG_MCUFWDL, tmp & 0xfe);
+
+		/*  Reserved for fw extension. */
+		rtw_write8(padapter, REG_MCUFWDL + 1, 0x00);
+	}
+}
+
+static int block_write(struct adapter *padapter, u8 *buffer, u32 size)
+{
+	int ret = _SUCCESS;
+	u32 blocks, block_size, remain;
+	u32 i, offset, addr;
+	u8 *data;
+
+	block_size = MAX_REG_BLOCK_SIZE;
+
+	blocks = size / block_size;
+	remain = size % block_size;
+
+	for (i = 0; i < blocks; i++) {
+		addr = FW_8188E_START_ADDRESS + i * block_size;
+		data = buffer + i * block_size;
+
+		ret = rtw_writeN(padapter, addr, block_size, data);
+		if (ret == _FAIL)
+			goto exit;
+	}
+
+	if (remain) {
+		offset = blocks * block_size;
+		block_size = 8;
+
+		blocks = remain / block_size;
+		remain = remain % block_size;
+
+		for (i = 0; i < blocks; i++) {
+			addr = FW_8188E_START_ADDRESS + offset + i * block_size;
+			data = buffer + offset + i * block_size;
+
+			ret = rtw_writeN(padapter, addr, block_size, data);
+			if (ret == _FAIL)
+				goto exit;
+		}
+	}
+
+	if (remain) {
+		offset += blocks * block_size;
+
+		/* block size 1 */
+		blocks = remain;
+
+		for (i = 0; i < blocks; i++) {
+			addr = FW_8188E_START_ADDRESS + offset + i;
+			data = buffer + offset + i;
+
+			ret = rtw_write8(padapter, addr, *data);
+			if (ret == _FAIL)
+				goto exit;
+		}
+	}
+
+exit:
+	return ret;
+}
+
+static int page_write(struct adapter *padapter, u32 page, u8 *buffer, u32 size)
+{
+	u8 value8;
+	u8 u8Page = (u8)(page & 0x07);
+	int res;
+
+	res = rtw_read8(padapter, REG_MCUFWDL + 2, &value8);
+	if (res)
+		return _FAIL;
+
+	value8 = (value8 & 0xF8) | u8Page;
+	rtw_write8(padapter, REG_MCUFWDL + 2, value8);
+
+	return block_write(padapter, buffer, size);
+}
+
+static int write_fw(struct adapter *padapter, u8 *buffer, u32 size)
+{
+	/*  Since we need dynamic decide method of dwonload fw, so we call this function to get chip version. */
+	/*  We can remove _ReadChipVersion from ReadpadapterInfo8192C later. */
+	int ret = _SUCCESS;
+	u32	pageNums, remainSize;
+	u32	page, offset;
+
+	pageNums = size / MAX_PAGE_SIZE;
+	remainSize = size % MAX_PAGE_SIZE;
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * MAX_PAGE_SIZE;
+		ret = page_write(padapter, page, buffer + offset, MAX_PAGE_SIZE);
+
+		if (ret == _FAIL)
+			goto exit;
+	}
+	if (remainSize) {
+		offset = pageNums * MAX_PAGE_SIZE;
+		page = pageNums;
+		ret = page_write(padapter, page, buffer + offset, remainSize);
+
+		if (ret == _FAIL)
+			goto exit;
+	}
+exit:
+	return ret;
+}
+
+void rtw_reset_8051(struct adapter *padapter)
+{
+	u8 val8;
+	int res;
+
+	res = rtw_read8(padapter, REG_SYS_FUNC_EN + 1, &val8);
+	if (res)
+		return;
+
+	rtw_write8(padapter, REG_SYS_FUNC_EN + 1, val8 & (~BIT(2)));
+	rtw_write8(padapter, REG_SYS_FUNC_EN + 1, val8 | (BIT(2)));
+}
+
+static int fw_free_to_go(struct adapter *padapter)
+{
+	u32	counter = 0;
+	u32	value32;
+	int res;
+
+	/*  polling CheckSum report */
+	do {
+		res = rtw_read32(padapter, REG_MCUFWDL, &value32);
+		if (res)
+			continue;
+
+		if (value32 & FWDL_CHKSUM_RPT)
+			break;
+	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
+
+	if (counter >= POLLING_READY_TIMEOUT_COUNT)
+		return _FAIL;
+
+	res = rtw_read32(padapter, REG_MCUFWDL, &value32);
+	if (res)
+		return _FAIL;
+
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtw_write32(padapter, REG_MCUFWDL, value32);
+
+	rtw_reset_8051(padapter);
+
+	/*  polling for FW ready */
+	counter = 0;
+	do {
+		res = rtw_read32(padapter, REG_MCUFWDL, &value32);
+		if (!res && value32 & WINTINI_RDY)
+			return _SUCCESS;
+
+		udelay(5);
+	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
+
+	return _FAIL;
+}
+
+static int load_firmware(struct rt_firmware *rtfw, struct device *device)
+{
+	int ret = _SUCCESS;
+	const struct firmware *fw;
+	const char *fw_name = FW_RTL8188EU;
+	int err = request_firmware(&fw, fw_name, device);
+
+	if (err) {
+		pr_err("Request firmware failed with error 0x%x\n", err);
+		ret = _FAIL;
+		goto exit;
+	}
+	if (!fw) {
+		pr_err("Firmware %s not available\n", fw_name);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	rtfw->data = kmemdup(fw->data, fw->size, GFP_KERNEL);
+	if (!rtfw->data) {
+		pr_err("Failed to allocate rtfw->data\n");
+		ret = _FAIL;
+		goto exit;
+	}
+	rtfw->size = fw->size;
+
+exit:
+	release_firmware(fw);
+	return ret;
+}
+
+int rtl8188e_firmware_download(struct adapter *padapter)
+{
+	int ret = _SUCCESS;
+	u8 reg;
+	unsigned long fwdl_timeout;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct device *device = dvobj_to_dev(dvobj);
+	struct rt_firmware_hdr *fwhdr = NULL;
+	u8 *fw_data;
+	u32 fw_size;
+
+	if (!dvobj->firmware.data)
+		ret = load_firmware(&dvobj->firmware, device);
+	if (ret == _FAIL) {
+		dvobj->firmware.data = NULL;
+		goto exit;
+	}
+	fw_data = dvobj->firmware.data;
+	fw_size = dvobj->firmware.size;
+
+	fwhdr = (struct rt_firmware_hdr *)dvobj->firmware.data;
+
+	if (IS_FW_HEADER_EXIST(fwhdr)) {
+		dev_info_once(device, "Firmware Version %d, SubVersion %d, Signature 0x%x\n",
+			      le16_to_cpu(fwhdr->version), fwhdr->subversion,
+			      le16_to_cpu(fwhdr->signature));
+
+		fw_data = fw_data + sizeof(struct rt_firmware_hdr);
+		fw_size = fw_size - sizeof(struct rt_firmware_hdr);
+	}
+
+	/*  Suggested by Filen. If 8051 is running in RAM code, driver should inform Fw to reset by itself, */
+	/*  or it will cause download Fw fail. 2010.02.01. by tynli. */
+	ret = rtw_read8(padapter, REG_MCUFWDL, &reg);
+	if (ret) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (reg & RAM_DL_SEL) { /* 8051 RAM code */
+		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		rtw_reset_8051(padapter);
+	}
+
+	fw_download_enable(padapter, true);
+	fwdl_timeout = jiffies + msecs_to_jiffies(500);
+	do {
+		/* reset the FWDL chksum */
+		ret = rtw_read8(padapter, REG_MCUFWDL, &reg);
+		if (ret) {
+			ret = _FAIL;
+			continue;
+		}
+
+		rtw_write8(padapter, REG_MCUFWDL, reg | FWDL_CHKSUM_RPT);
+
+		ret = write_fw(padapter, fw_data, fw_size);
+		if (ret == _SUCCESS)
+			break;
+	} while (!time_after(jiffies, fwdl_timeout));
+
+	fw_download_enable(padapter, false);
+	if (ret != _SUCCESS)
+		goto exit;
+
+	ret = fw_free_to_go(padapter);
+	if (ret != _SUCCESS)
+		goto exit;
+
+exit:
+	return ret;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_ieee80211.c b/drivers/staging/r8188eu/core/rtw_ieee80211.c
new file mode 100644
index 000000000..bc8543ea2
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ieee80211.c
@@ -0,0 +1,1150 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _IEEE80211_C
+
+#include "../include/drv_types.h"
+#include "../include/ieee80211.h"
+#include "../include/wifi.h"
+#include "../include/osdep_service.h"
+#include "../include/wlan_bssdef.h"
+#include "../include/usb_osintf.h"
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+/*  */
+/*  for adhoc-master to generate ie and provide supported-rate to fw */
+/*  */
+
+static u8	WIFI_CCKRATES[] = {
+	(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+	};
+
+static u8	WIFI_OFDMRATES[] = {
+	 (IEEE80211_OFDM_RATE_6MB),
+	 (IEEE80211_OFDM_RATE_9MB),
+	 (IEEE80211_OFDM_RATE_12MB),
+	 (IEEE80211_OFDM_RATE_18MB),
+	 (IEEE80211_OFDM_RATE_24MB),
+	 IEEE80211_OFDM_RATE_36MB,
+	 IEEE80211_OFDM_RATE_48MB,
+	 IEEE80211_OFDM_RATE_54MB
+	};
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+	unsigned char dot11_rate_table[] = {
+		2, 4, 11, 22, 12, 18, 24, 36, 48,
+		72, 96, 108, 0}; /*  last element must be zero!! */
+
+	int i = 0;
+	while (dot11_rate_table[i] != 0) {
+		if (dot11_rate_table[i] == val)
+			return BIT(i);
+		i++;
+	}
+	return 0;
+}
+
+bool	rtw_is_cckrates_included(u8 *rate)
+{
+	u32	i = 0;
+
+	while (rate[i] != 0) {
+		if  ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
+		     (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+			return true;
+		i++;
+	}
+	return false;
+}
+
+bool	rtw_is_cckratesonly_included(u8 *rate)
+{
+	u32 i = 0;
+
+	while (rate[i] != 0) {
+		if  ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+		     (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+			return false;
+		i++;
+	}
+
+	return true;
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+	if (channel > 14)
+		return WIRELESS_INVALID;
+	/*  could be pure B, pure G, or B/G */
+	if (rtw_is_cckratesonly_included(rate))
+		return WIRELESS_11B;
+	else if (rtw_is_cckrates_included(rate))
+		return	WIRELESS_11BG;
+	else
+		return WIRELESS_11G;
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+				unsigned int *frlen)
+{
+	memcpy((void *)pbuf, (void *)source, len);
+	*frlen = *frlen + len;
+	return pbuf + len;
+}
+
+/*  rtw_set_ie will update frame length */
+u8 *rtw_set_ie
+(
+	u8 *pbuf,
+	int index,
+	uint len,
+	u8 *source,
+	uint *frlen /* frame length */
+)
+{
+
+	*pbuf = (u8)index;
+
+	*(pbuf + 1) = (u8)len;
+
+	if (len > 0)
+		memcpy((void *)(pbuf + 2), (void *)source, len);
+
+	*frlen = *frlen + (len + 2);
+
+	return pbuf + len + 2;
+}
+
+/*----------------------------------------------------------------------------
+index: the information element id index, limit is the limit for search
+-----------------------------------------------------------------------------*/
+u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit)
+{
+	int tmp, i;
+	u8 *p;
+
+	if (limit < 1) {
+
+		return NULL;
+	}
+
+	p = pbuf;
+	i = 0;
+	*len = 0;
+	while (1) {
+		if (*p == index) {
+			*len = *(p + 1);
+			return p;
+		}
+		tmp = *(p + 1);
+		p += (tmp + 2);
+		i += (tmp + 2);
+		if (i >= limit)
+			break;
+	}
+
+	return NULL;
+}
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
+{
+
+	memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	switch (mode) {
+	case WIRELESS_11B:
+		memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		break;
+	case WIRELESS_11G:
+		memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11_24N:
+	case WIRELESS_11BG_24N:
+		memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+	}
+
+}
+
+uint	rtw_get_rateset_len(u8	*rateset)
+{
+	uint i = 0;
+
+	while (1) {
+		if ((rateset[i]) == 0)
+			break;
+		if (i > 12)
+			break;
+		i++;
+	}
+
+	return i;
+}
+
+int rtw_generate_ie(struct registry_priv *pregistrypriv)
+{
+	u8	wireless_mode;
+	int	sz = 0, rateLen;
+	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+	u8 *ie = pdev_network->IEs;
+
+	/* timestamp will be inserted by hardware */
+	sz += 8;
+	ie += sz;
+
+	/* beacon interval : 2bytes */
+	*(__le16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);/* BCN_INTERVAL; */
+	sz += 2;
+	ie += 2;
+
+	/* capability info */
+	*(u16 *)ie = 0;
+
+	*(__le16 *)ie |= cpu_to_le16(cap_IBSS);
+
+	if (pregistrypriv->preamble == PREAMBLE_SHORT)
+		*(__le16 *)ie |= cpu_to_le16(cap_ShortPremble);
+
+	if (pdev_network->Privacy)
+		*(__le16 *)ie |= cpu_to_le16(cap_Privacy);
+
+	sz += 2;
+	ie += 2;
+
+	/* SSID */
+	ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
+
+	/* supported rates */
+	wireless_mode = pregistrypriv->wireless_mode;
+
+	rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode);
+
+	rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
+
+	if (rateLen > 8) {
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
+		/* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
+	} else {
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
+	}
+
+	/* DS parameter set */
+	ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&pdev_network->Configuration.DSConfig, &sz);
+
+	/* IBSS Parameter Set */
+
+	ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&pdev_network->Configuration.ATIMWindow, &sz);
+
+	if (rateLen > 8)
+		ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
+
+	return sz;
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+	int len;
+	u16 val16;
+	__le16 le_tmp;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *pbuf = pie;
+	int limit_new = limit;
+
+	while (1) {
+		pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
+
+		if (pbuf) {
+			/* check if oui matches... */
+			if (memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)))
+				goto check_next_ie;
+
+			/* check version... */
+			memcpy((u8 *)&le_tmp, (pbuf + 6), sizeof(val16));
+
+			val16 = le16_to_cpu(le_tmp);
+			if (val16 != 0x0001)
+				goto check_next_ie;
+			*wpa_ie_len = *(pbuf + 1);
+			return pbuf;
+		}
+		*wpa_ie_len = 0;
+		return NULL;
+
+check_next_ie:
+		limit_new = limit - (pbuf - pie) - 2 - len;
+		if (limit_new <= 0)
+			break;
+		pbuf += (2 + len);
+	}
+	*wpa_ie_len = 0;
+	return NULL;
+}
+
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+{
+
+	return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
+}
+
+int rtw_get_wpa_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_get_wpa2_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+
+	if (wpa_ie_len <= 0) {
+		/* No WPA IE - fail silently */
+		return _FAIL;
+	}
+
+	if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) ||
+	    (memcmp(wpa_ie + 2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
+		return _FAIL;
+
+	pos = wpa_ie;
+
+	pos += 8;
+	left = wpa_ie_len - 8;
+
+	/* group_cipher */
+	if (left >= WPA_SELECTOR_LEN) {
+		*group_cipher = rtw_get_wpa_cipher_suite(pos);
+		pos += WPA_SELECTOR_LEN;
+		left -= WPA_SELECTOR_LEN;
+	} else if (left > 0) {
+		return _FAIL;
+	}
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		count = get_unaligned_le16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * WPA_SELECTOR_LEN)
+			return _FAIL;
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+	} else if (left == 1) {
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4))
+				*is_8021x = 1;
+		}
+	}
+
+	return ret;
+}
+
+int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return _FAIL;
+	}
+
+	if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie + 1) != (u8)(rsn_ie_len - 2)))
+		return _FAIL;
+
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;
+
+	/* group_cipher */
+	if (left >= RSN_SELECTOR_LEN) {
+		*group_cipher = rtw_get_wpa2_cipher_suite(pos);
+
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+
+	} else if (left > 0) {
+		return _FAIL;
+	}
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		count = get_unaligned_le16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN)
+			return _FAIL;
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+
+	} else if (left == 1) {
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4))
+				*is_8021x = 1;
+		}
+	}
+	return ret;
+}
+
+int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
+{
+	u8 authmode;
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint	cnt;
+
+	/* Search required WPA or WPA2 IE and copy to sec_ie[] */
+
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4))) {
+				if (wpa_ie)
+					memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+				*wpa_len = in_ie[cnt + 1] + 2;
+				cnt += in_ie[cnt + 1] + 2;  /* get next */
+		} else {
+			if (authmode == _WPA2_IE_ID_) {
+				if (rsn_ie)
+					memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+				*rsn_len = in_ie[cnt + 1] + 2;
+				cnt += in_ie[cnt + 1] + 2;  /* get next */
+			} else {
+				cnt += in_ie[cnt + 1] + 2;   /* get next */
+			}
+		}
+	}
+
+	return *rsn_len + *wpa_len;
+}
+
+u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
+{
+	u8 match = false;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (!ie_ptr)
+		return match;
+
+	eid = ie_ptr[0];
+
+	if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) {
+		*wps_ielen = ie_ptr[1] + 2;
+		match = true;
+	}
+	return match;
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	uint cnt;
+	u8 *wpsie_ptr = NULL;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (wps_ielen)
+		*wps_ielen = 0;
+
+	if (!in_ie || in_len <= 0)
+		return wpsie_ptr;
+
+	cnt = 0;
+
+	while (cnt < in_len) {
+		eid = in_ie[cnt];
+
+		if ((eid == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt + 2], wps_oui, 4))) {
+			wpsie_ptr = &in_ie[cnt];
+
+			if (wps_ie)
+				memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (wps_ielen)
+				*wps_ielen = in_ie[cnt + 1] + 2;
+
+			cnt += in_ie[cnt + 1] + 2;
+
+			break;
+		}
+		cnt += in_ie[cnt + 1] + 2; /* goto next */
+	}
+	return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+	    (memcmp(wps_ie + 2, wps_oui, 4)))
+		return attr_ptr;
+
+	/*  6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
+	attr_ptr = wps_ie + 6; /* goto first attr */
+
+	while (attr_ptr - wps_ie < wps_ielen) {
+		/*  4 = 2(Attribute ID) + 2(Length) */
+		u16 attr_id = RTW_GET_BE16(attr_ptr);
+		u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
+		u16 attr_len = attr_data_len + 4;
+
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+			if (len_attr)
+				*len_attr = attr_len;
+			break;
+		}
+		attr_ptr += attr_len; /* goto next */
+	}
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
+ *
+ * Returns: the address of the specific WPS attribute content found, or NULL
+ */
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr + 4, attr_len - 4);
+
+		if (len_content)
+			*len_content = attr_len - 4;
+
+		return attr_ptr + 4;
+	}
+
+	return NULL;
+}
+
+static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
+					    struct rtw_ieee802_11_elems *elems,
+					    int show_errors)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4)
+		return -1;
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			elems->wpa_ie = pos;
+			elems->wpa_ie_len = elen;
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5)
+				return -1;
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				elems->wme = pos;
+				elems->wme_len = elen;
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				elems->wme_tspec = pos;
+				elems->wme_tspec_len = elen;
+				break;
+			default:
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			return -1;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			elems->vendor_ht_cap = pos;
+			elems->vendor_ht_cap_len = elen;
+			break;
+		default:
+			return -1;
+		}
+		break;
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+enum parse_res rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				struct rtw_ieee802_11_elems *elems,
+				int show_errors)
+{
+	uint left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left)
+			return ParseFailed;
+
+		switch (id) {
+		case WLAN_EID_SSID:
+			elems->ssid = pos;
+			elems->ssid_len = elen;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			elems->supp_rates = pos;
+			elems->supp_rates_len = elen;
+			break;
+		case WLAN_EID_FH_PARAMS:
+			elems->fh_params = pos;
+			elems->fh_params_len = elen;
+			break;
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_CF_PARAMS:
+			elems->cf_params = pos;
+			elems->cf_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_CHALLENGE:
+			elems->challenge = pos;
+			elems->challenge_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_EXT_SUPP_RATES:
+			elems->ext_supp_rates = pos;
+			elems->ext_supp_rates_len = elen;
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_ieee802_11_parse_vendor_specific(pos, elen, elems, show_errors))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+			elems->rsn_ie = pos;
+			elems->rsn_ie_len = elen;
+			break;
+		case WLAN_EID_PWR_CAPABILITY:
+			elems->power_cap = pos;
+			elems->power_cap_len = elen;
+			break;
+		case WLAN_EID_SUPPORTED_CHANNELS:
+			elems->supp_channels = pos;
+			elems->supp_channels_len = elen;
+			break;
+		case WLAN_EID_MOBILITY_DOMAIN:
+			elems->mdie = pos;
+			elems->mdie_len = elen;
+			break;
+		case WLAN_EID_FAST_BSS_TRANSITION:
+			elems->ftie = pos;
+			elems->ftie_len = elen;
+			break;
+		case WLAN_EID_TIMEOUT_INTERVAL:
+			elems->timeout_int = pos;
+			elems->timeout_int_len = elen;
+			break;
+		case WLAN_EID_HT_CAP:
+			elems->ht_capabilities = pos;
+			elems->ht_capabilities_len = elen;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			elems->ht_operation = pos;
+			elems->ht_operation_len = elen;
+			break;
+		default:
+			unknown++;
+			break;
+		}
+		left -= elen;
+		pos += elen;
+	}
+	if (left)
+		return ParseFailed;
+	return unknown ? ParseUnknown : ParseOK;
+}
+
+u8 key_char2num(u8 ch)
+{
+	if ((ch >= '0') && (ch <= '9'))
+		return ch - '0';
+	else if ((ch >= 'a') && (ch <= 'f'))
+		return ch - 'a' + 10;
+	else if ((ch >= 'A') && (ch <= 'F'))
+		return ch - 'A' + 10;
+	else
+		return 0xff;
+}
+
+u8 str_2char2num(u8 hch, u8 lch)
+{
+    return (key_char2num(hch) * 10) + key_char2num(lch);
+}
+
+u8 key_2char2num(u8 hch, u8 lch)
+{
+    return (key_char2num(hch) << 4) | key_char2num(lch);
+}
+
+void rtw_macaddr_cfg(u8 *mac_addr)
+{
+	u8 mac[ETH_ALEN];
+
+	if (!mac_addr)
+		return;
+
+	if (rtw_initmac && mac_pton(rtw_initmac, mac)) {
+		/* Users specify the mac address */
+		ether_addr_copy(mac_addr, mac);
+	} else {
+		/* Use the mac address stored in the Efuse */
+		ether_addr_copy(mac, mac_addr);
+	}
+
+	if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac))
+		eth_random_addr(mac_addr);
+}
+
+/**
+ * rtw_get_p2p_ie - Search P2P IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
+ * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
+ *
+ * Returns: The address of the P2P IE found, or NULL
+ */
+u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
+{
+	uint cnt = 0;
+	u8 *p2p_ie_ptr;
+	u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+	if (p2p_ielen)
+		*p2p_ielen = 0;
+
+	while (cnt < in_len) {
+		eid = in_ie[cnt];
+		if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
+			dump_stack();
+			return NULL;
+		}
+		if ((eid == _VENDOR_SPECIFIC_IE_) && !memcmp(&in_ie[cnt + 2], p2p_oui, 4)) {
+			p2p_ie_ptr = in_ie + cnt;
+
+			if (p2p_ie)
+				memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+			if (p2p_ielen)
+				*p2p_ielen = in_ie[cnt + 1] + 2;
+			return p2p_ie_ptr;
+		}
+		cnt += in_ie[cnt + 1] + 2; /* goto next */
+	}
+	return NULL;
+}
+
+/**
+ * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id, u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if (!p2p_ie || (p2p_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+	    memcmp(p2p_ie + 2, p2p_oui, 4))
+		return attr_ptr;
+
+	/*  6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
+	attr_ptr = p2p_ie + 6; /* goto first attr */
+
+	while (attr_ptr - p2p_ie < p2p_ielen) {
+		/*  3 = 1(Attribute ID) + 2(Length) */
+		u8 attr_id = *attr_ptr;
+		u16 attr_data_len = get_unaligned_le16(attr_ptr + 1);
+		u16 attr_len = attr_data_len + 3;
+
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+			if (len_attr)
+				*len_attr = attr_len;
+			break;
+		}
+		attr_ptr += attr_len; /* goto next */
+	}
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
+ *
+ * Returns: the address of the specific P2P attribute content found, or NULL
+ */
+u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id, u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr + 3, attr_len - 3);
+
+		if (len_content)
+			*len_content = attr_len - 3;
+
+		return attr_ptr + 3;
+	}
+
+	return NULL;
+}
+
+u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
+{
+	u32 a_len;
+
+	*pbuf = attr_id;
+
+	/* u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
+	RTW_PUT_LE16(pbuf + 1, attr_len);
+
+	if (pdata_attr)
+		memcpy(pbuf + 3, pdata_attr, attr_len);
+
+	a_len = attr_len + 3;
+
+	return a_len;
+}
+
+static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
+{
+	u8 *target_attr;
+	u32 target_attr_len;
+	uint ielen = ielen_ori;
+
+	while (1) {
+		target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
+		if (target_attr && target_attr_len) {
+			u8 *next_attr = target_attr + target_attr_len;
+			uint remain_len = ielen - (next_attr - ie);
+
+			memset(target_attr, 0, target_attr_len);
+			memcpy(target_attr, next_attr, remain_len);
+			memset(target_attr + remain_len, 0, target_attr_len);
+			*(ie + 1) -= target_attr_len;
+			ielen -= target_attr_len;
+		} else {
+			break;
+		}
+	}
+	return ielen;
+}
+
+void rtw_wlan_bssid_ex_remove_p2p_attr(struct wlan_bssid_ex *bss_ex, u8 attr_id)
+{
+	u8 *p2p_ie;
+	uint p2p_ielen, p2p_ielen_ori;
+
+	p2p_ie = rtw_get_p2p_ie(bss_ex->IEs + _FIXED_IE_LENGTH_, bss_ex->IELength - _FIXED_IE_LENGTH_, NULL, &p2p_ielen_ori);
+	if (p2p_ie) {
+		p2p_ielen = rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
+		if (p2p_ielen != p2p_ielen_ori) {
+			u8 *next_ie_ori = p2p_ie + p2p_ielen_ori;
+			u8 *next_ie = p2p_ie + p2p_ielen;
+			uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
+
+			memcpy(next_ie, next_ie_ori, remain_len);
+			memset(next_ie + remain_len, 0, p2p_ielen_ori - p2p_ielen);
+			bss_ex->IELength -= p2p_ielen_ori - p2p_ielen;
+		}
+	}
+}
+
+static int rtw_get_cipher_info(struct wlan_network *pnetwork)
+{
+	u32 wpa_ielen;
+	unsigned char *pbuf;
+	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
+	int ret = _FAIL;
+
+	pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+
+	if (pbuf && (wpa_ielen > 0)) {
+		if (rtw_parse_wpa_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x) == _SUCCESS) {
+			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+			pnetwork->BcnInfo.group_cipher = group_cipher;
+			pnetwork->BcnInfo.is_8021x = is8021x;
+			ret = _SUCCESS;
+		}
+	} else {
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+
+		if (pbuf && (wpa_ielen > 0)) {
+			if (rtw_parse_wpa2_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x) == _SUCCESS) {
+				pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+				pnetwork->BcnInfo.group_cipher = group_cipher;
+				pnetwork->BcnInfo.is_8021x = is8021x;
+				ret = _SUCCESS;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+	unsigned short cap = 0;
+	u8 bencrypt = 0;
+	__le16 le_tmp;
+	u16 wpa_len = 0, rsn_len = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct ieee80211_ht_cap *pht_cap = NULL;
+	unsigned int		len;
+	unsigned char		*p;
+
+	memcpy(&le_tmp, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
+	cap = le16_to_cpu(le_tmp);
+	if (cap & WLAN_CAPABILITY_PRIVACY) {
+		bencrypt = 1;
+		pnetwork->network.Privacy = 1;
+	} else {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+	}
+	rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bencrypt)
+			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+	rtw_get_cipher_info(pnetwork);
+
+	/* get bwmode and ch_offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+			pnetwork->BcnInfo.ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else {
+			pnetwork->BcnInfo.ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			pnetwork->BcnInfo.ht_info_infos_0 = 0;
+	}
+}
+
+/* show MCS rate, unit: 100Kbps */
+u16 rtw_mcs_rate(u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char *MCS_rate)
+{
+	u16 max_rate = 0;
+
+	if (MCS_rate[0] & BIT(7))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 1500 : 1350) : ((short_GI_20) ? 722 : 650);
+	else if (MCS_rate[0] & BIT(6))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 1350 : 1215) : ((short_GI_20) ? 650 : 585);
+	else if (MCS_rate[0] & BIT(5))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
+	else if (MCS_rate[0] & BIT(4))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
+	else if (MCS_rate[0] & BIT(3))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
+	else if (MCS_rate[0] & BIT(2))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 450 : 405) : ((short_GI_20) ? 217 : 195);
+	else if (MCS_rate[0] & BIT(1))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
+	else if (MCS_rate[0] & BIT(0))
+		max_rate = (bw_40MHz) ? ((short_GI_40) ? 150 : 135) : ((short_GI_20) ? 72 : 65);
+
+	return max_rate;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_ioctl_set.c b/drivers/staging/r8188eu/core/rtw_ioctl_set.c
new file mode 100644
index 000000000..55e6b0f41
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ioctl_set.c
@@ -0,0 +1,481 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_IOCTL_SET_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_ioctl_set.h"
+#include "../include/hal_intf.h"
+
+#include "../include/usb_osintf.h"
+#include "../include/usb_ops.h"
+
+u8 rtw_do_join(struct adapter *padapter)
+{
+	struct list_head *plist, *phead;
+	u8 *pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	u8 ret = _SUCCESS;
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	pmlmepriv->cur_network.join_res = -2;
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	pmlmepriv->pscanned = plist;
+
+	pmlmepriv->to_join = true;
+
+	if (list_empty(&queue->queue)) {
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		/* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */
+		/* we try to issue sitesurvey firstly */
+
+		if (!pmlmepriv->LinkDetectInfo.bBusyTraffic ||
+		    pmlmepriv->to_roaming > 0) {
+			/*  submit site_survey_cmd */
+			ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1);
+			if (ret != _SUCCESS)
+				pmlmepriv->to_join = false;
+		} else {
+			pmlmepriv->to_join = false;
+			ret = _FAIL;
+		}
+
+		return ret;
+	} else {
+		int select_ret;
+
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+		if (select_ret == _SUCCESS) {
+			pmlmepriv->to_join = false;
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+		} else {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				/*  submit createbss_cmd to change to a ADHOC_MASTER */
+
+				/* pmlmepriv->lock has been acquired by caller... */
+				struct wlan_bssid_ex    *pdev_network = &padapter->registrypriv.dev_network;
+
+				pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+				pibss = padapter->registrypriv.dev_network.MacAddress;
+
+				memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+				rtw_update_registrypriv_dev_network(padapter);
+
+				rtw_generate_random_ibss(pibss);
+
+				if (rtw_createbss_cmd(padapter) != _SUCCESS)
+					return false;
+
+				pmlmepriv->to_join = false;
+			} else {
+				/*  can't associate ; reset under-linking */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+				/* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */
+				/* we try to issue sitesurvey firstly */
+				if (!pmlmepriv->LinkDetectInfo.bBusyTraffic ||
+				    pmlmepriv->to_roaming > 0) {
+					ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1);
+					if (ret != _SUCCESS)
+						pmlmepriv->to_join = false;
+				} else {
+					ret = _FAIL;
+					pmlmepriv->to_join = false;
+				}
+			}
+		}
+	}
+
+	return ret;
+}
+
+u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid)
+{
+	u8 status = _SUCCESS;
+	u32 cur_time = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 &&
+	     bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) ||
+	    (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF &&
+	     bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		goto release_mlme_lock;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN)) {
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+				goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+		} else {
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	/* should we add something here...? */
+
+	if (padapter->securitypriv.btkip_countermeasure) {
+		cur_time = jiffies;
+
+		if ((cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ) {
+			padapter->securitypriv.btkip_countermeasure = false;
+			padapter->securitypriv.btkip_countermeasure_time = 0;
+		} else {
+			status = _FAIL;
+			goto release_mlme_lock;
+		}
+	}
+
+	memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid = true;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	return status;
+}
+
+u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid)
+{
+	u8 status = _SUCCESS;
+	u32 cur_time = 0;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+	if (!padapter->hw_init_completed) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		goto release_mlme_lock;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength))) {
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				if (!rtw_is_same_ibss(padapter, pnetwork)) {
+					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
+					rtw_disassoc_cmd(padapter, 0, true);
+
+					if (check_fwstate(pmlmepriv, _FW_LINKED))
+						rtw_indicate_disconnect(padapter);
+
+					rtw_free_assoc_resources(padapter, 1);
+
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+					}
+				} else {
+					goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+				}
+			} else {
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+			}
+		} else {
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+
+	if (padapter->securitypriv.btkip_countermeasure) {
+		cur_time = jiffies;
+
+		if ((cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ) {
+			padapter->securitypriv.btkip_countermeasure = false;
+			padapter->securitypriv.btkip_countermeasure_time = 0;
+		} else {
+			status = _FAIL;
+			goto release_mlme_lock;
+		}
+	}
+
+	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
+	pmlmepriv->assoc_by_bssid = false;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		pmlmepriv->to_join = true;
+	} else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	return status;
+}
+
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter,
+	enum ndis_802_11_network_infra networktype)
+{
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
+	enum ndis_802_11_network_infra *pold_state = &cur_network->network.InfrastructureMode;
+
+	if (*pold_state != networktype) {
+		spin_lock_bh(&pmlmepriv->lock);
+
+		if (*pold_state == Ndis802_11APMode) {
+			/* change to other mode from Ndis802_11APMode */
+			cur_network->join_res = -1;
+
+			stop_ap_mode(padapter);
+		}
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED)) ||
+		    (*pold_state == Ndis802_11IBSS))
+			rtw_disassoc_cmd(padapter, 0, true);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED)) ||
+		    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+			rtw_free_assoc_resources(padapter, 1);
+
+		if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				rtw_indicate_disconnect(padapter); /* will clr Linked_state; before this function, we must have chked whether  issue dis-assoc_cmd or not */
+	       }
+
+		*pold_state = networktype;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (networktype) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+		case Ndis802_11APMode:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+			break;
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+
+	return true;
+}
+
+u8 rtw_set_802_11_disassociate(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);
+		rtw_pwr_wakeup(padapter);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	return true;
+}
+
+u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num)
+{
+	struct	mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	u8	res = true;
+
+	if (!padapter) {
+		res = false;
+		goto exit;
+	}
+	if (!padapter->hw_init_completed) {
+		res = false;
+		goto exit;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING)) ||
+	    (pmlmepriv->LinkDetectInfo.bBusyTraffic)) {
+		/*  Scan or linking is in progress, do nothing. */
+		res = true;
+	} else {
+		spin_lock_bh(&pmlmepriv->lock);
+
+		res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num);
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+exit:
+
+	return res;
+}
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *padapter, enum ndis_802_11_auth_mode authmode)
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	int res;
+	u8 ret;
+
+	psecuritypriv->ndisauthtype = authmode;
+
+	if (psecuritypriv->ndisauthtype > 3)
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+	res = rtw_set_auth(padapter, psecuritypriv);
+
+	if (res == _SUCCESS)
+		ret = true;
+	else
+		ret = false;
+
+	return ret;
+}
+
+u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct ndis_802_11_wep *wep)
+{
+	int		keyid, res;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8		ret = _SUCCESS;
+
+	keyid = wep->KeyIndex & 0x3fffffff;
+
+	if (keyid >= 4) {
+		ret = false;
+		goto exit;
+	}
+
+	switch (wep->KeyLength) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		break;
+	}
+
+	memcpy(&psecuritypriv->dot11DefKey[keyid].skey[0], &wep->KeyMaterial, wep->KeyLength);
+
+	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	res = rtw_set_key(padapter, psecuritypriv, keyid, 1);
+
+	if (res == _FAIL)
+		ret = false;
+exit:
+
+	return ret;
+}
+
+/*
+* rtw_get_cur_max_rate -
+* @adapter: pointer to struct adapter structure
+*
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(struct adapter *adapter)
+{
+	int	i = 0;
+	u8	*p;
+	u16	rate = 0, max_rate = 0;
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
+	struct ieee80211_ht_cap *pht_capie;
+	u8	bw_40MHz = 0, short_GI_20 = 0, short_GI_40 = 0;
+	u16	mcs_rate = 0;
+	u32	ht_ielen = 0;
+
+	if ((!check_fwstate(pmlmepriv, _FW_LINKED)) &&
+	    (!check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+		return 0;
+
+	if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N)) {
+		p = rtw_get_ie(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pcur_bss->IELength - 12);
+		if (p && ht_ielen > 0) {
+			pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+
+			memcpy(&mcs_rate, pht_capie->mcs.rx_mask, 2);
+
+			/* cur_bwmod is updated by beacon, pmlmeinfo is updated by association response */
+			bw_40MHz = (pmlmeext->cur_bwmode && (HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH & pmlmeinfo->HT_info.infos[0])) ? 1 : 0;
+
+			short_GI_20 = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & IEEE80211_HT_CAP_SGI_20) ? 1 : 0;
+			short_GI_40 = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & IEEE80211_HT_CAP_SGI_40) ? 1 : 0;
+
+			max_rate = rtw_mcs_rate(bw_40MHz & (pregistrypriv->cbw40_enable),
+						short_GI_20,
+						short_GI_40,
+						pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate);
+		}
+	} else {
+		while ((pcur_bss->SupportedRates[i] != 0) && (pcur_bss->SupportedRates[i] != 0xFF)) {
+			rate = pcur_bss->SupportedRates[i] & 0x7F;
+			if (rate > max_rate)
+				max_rate = rate;
+			i++;
+		}
+
+		max_rate *= 5;
+	}
+
+	return max_rate;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_iol.c b/drivers/staging/r8188eu/core/rtw_iol.c
new file mode 100644
index 000000000..31e196ccd
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_iol.c
@@ -0,0 +1,160 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/rtw_iol.h"
+
+struct xmit_frame	*rtw_IOL_accquire_xmit_frame(struct adapter  *adapter)
+{
+	struct xmit_frame	*xmit_frame;
+	struct xmit_buf	*xmitbuf;
+	struct pkt_attrib	*pattrib;
+	struct xmit_priv	*pxmitpriv = &adapter->xmitpriv;
+
+	xmit_frame = rtw_alloc_xmitframe(pxmitpriv);
+	if (!xmit_frame)
+		return NULL;
+
+	xmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+	if (!xmitbuf) {
+		rtw_free_xmitframe(pxmitpriv, xmit_frame);
+		return NULL;
+	}
+
+	xmit_frame->frame_tag = MGNT_FRAMETAG;
+	xmit_frame->pxmitbuf = xmitbuf;
+	xmit_frame->buf_addr = xmitbuf->pbuf;
+	xmitbuf->priv_data = xmit_frame;
+
+	pattrib = &xmit_frame->attrib;
+	update_mgntframe_attrib(adapter, pattrib);
+	pattrib->qsel = 0x10;/* Beacon */
+	pattrib->subtype = WIFI_BEACON;
+	pattrib->pktlen = 0;
+	pattrib->last_txcmdsz = 0;
+
+	return xmit_frame;
+}
+
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
+{
+	struct pkt_attrib	*pattrib = &xmit_frame->attrib;
+	u16 buf_offset;
+	u32 ori_len;
+
+	buf_offset = TXDESC_OFFSET;
+	ori_len = buf_offset + pattrib->pktlen;
+
+	/* check if the io_buf can accommodate new cmds */
+	if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ)
+		return _FAIL;
+
+	memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
+	pattrib->pktlen += cmd_len;
+	pattrib->last_txcmdsz += cmd_len;
+
+	return _SUCCESS;
+}
+
+bool rtw_IOL_applied(struct adapter  *adapter)
+{
+	if (adapter->registrypriv.fw_iol == 1)
+		return true;
+
+	if ((adapter->registrypriv.fw_iol == 2) &&
+	    (adapter_to_dvobj(adapter)->pusbdev->speed != USB_SPEED_HIGH))
+		return true;
+
+	return false;
+}
+
+int rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0};
+
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFF) {
+		cmd.length = 12;
+		cmd.mask = cpu_to_le32(mask);
+	}
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WW_REG, 0x0, 0x0, 0x0};
+
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFF) {
+		cmd.length = 12;
+		cmd.mask =  cpu_to_le32(mask);
+	}
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0};
+
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFFFFFF) {
+		cmd.length = 12;
+		cmd.mask =  cpu_to_le32(mask);
+	}
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0};
+
+	cmd.address = cpu_to_le16((rf_path << 8) | ((addr) & 0xFF));
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0x000FFFFF) {
+		cmd.length = 12;
+		cmd.mask =  cpu_to_le32(mask);
+	}
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+	cmd.address = cpu_to_le16(us);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+
+	cmd.address = cpu_to_le16(ms);
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_END, cpu_to_le16(0xFFFF), cpu_to_le32(0xFF), 0x0};
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
+{
+	u8 is_cmd_bndy = false;
+	if (((pxmit_frame->attrib.pktlen + 32) % 256) + 8 >= 256) {
+		rtw_IOL_append_END_cmd(pxmit_frame);
+		pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen + 32) / 256) + 1) * 256);
+
+		pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
+		is_cmd_bndy = true;
+	}
+	return is_cmd_bndy;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_led.c b/drivers/staging/r8188eu/core/rtw_led.c
new file mode 100644
index 000000000..48c5db699
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_led.c
@@ -0,0 +1,275 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+#include "../include/rtw_led.h"
+#include "../include/rtl8188e_spec.h"
+
+#define LED_BLINK_NO_LINK_INTVL			msecs_to_jiffies(1000)
+#define LED_BLINK_LINK_INTVL			msecs_to_jiffies(500)
+#define LED_BLINK_SCAN_INTVL			msecs_to_jiffies(180)
+#define LED_BLINK_FASTER_INTVL			msecs_to_jiffies(50)
+#define LED_BLINK_WPS_SUCESS_INTVL		msecs_to_jiffies(5000)
+
+#define IS_LED_WPS_BLINKING(l) \
+	((l)->CurrLedState == LED_BLINK_WPS || \
+	(l)->CurrLedState == LED_BLINK_WPS_STOP || \
+	(l)->bLedWPSBlinkInProgress)
+
+static void ResetLedStatus(struct led_priv *pLed)
+{
+	pLed->CurrLedState = RTW_LED_OFF; /*  Current LED state. */
+	pLed->bLedOn = false; /*  true if LED is ON, false if LED is OFF. */
+
+	pLed->bLedBlinkInProgress = false; /*  true if it is blinking, false o.w.. */
+	pLed->bLedWPSBlinkInProgress = false;
+
+	pLed->BlinkTimes = 0; /*  Number of times to toggle led state for blinking. */
+
+	pLed->bLedLinkBlinkInProgress = false;
+	pLed->bLedScanBlinkInProgress = false;
+}
+
+static void SwLedOn(struct adapter *padapter, struct led_priv *pLed)
+{
+	if (padapter->bDriverStopped)
+		return;
+
+	rtw_write8(padapter, REG_LEDCFG2, BIT(5)); /*  SW control led0 on. */
+	pLed->bLedOn = true;
+}
+
+static void SwLedOff(struct adapter *padapter, struct led_priv *pLed)
+{
+	if (padapter->bDriverStopped)
+		goto exit;
+
+	rtw_write8(padapter, REG_LEDCFG2, BIT(5) | BIT(3));
+exit:
+	pLed->bLedOn = false;
+}
+
+static void blink_work(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct led_priv *pLed = container_of(dwork, struct led_priv, blink_work);
+	struct adapter *padapter = pLed->padapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+		SwLedOff(padapter, pLed);
+		ResetLedStatus(pLed);
+		return;
+	}
+
+	if (pLed->bLedOn)
+		SwLedOff(padapter, pLed);
+	else
+		SwLedOn(padapter, pLed);
+
+	switch (pLed->CurrLedState) {
+	case LED_BLINK_SLOWLY:
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_NO_LINK_INTVL);
+		break;
+	case LED_BLINK_NORMAL:
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_LINK_INTVL);
+		break;
+	case LED_BLINK_SCAN:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_NORMAL;
+				schedule_delayed_work(&pLed->blink_work, LED_BLINK_LINK_INTVL);
+			} else {
+				pLed->CurrLedState = LED_BLINK_SLOWLY;
+				schedule_delayed_work(&pLed->blink_work, LED_BLINK_NO_LINK_INTVL);
+			}
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			schedule_delayed_work(&pLed->blink_work, LED_BLINK_SCAN_INTVL);
+		}
+		break;
+	case LED_BLINK_TXRX:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_NORMAL;
+				schedule_delayed_work(&pLed->blink_work, LED_BLINK_LINK_INTVL);
+			} else {
+				pLed->CurrLedState = LED_BLINK_SLOWLY;
+				schedule_delayed_work(&pLed->blink_work, LED_BLINK_NO_LINK_INTVL);
+			}
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			schedule_delayed_work(&pLed->blink_work, LED_BLINK_FASTER_INTVL);
+		}
+		break;
+	case LED_BLINK_WPS:
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_SCAN_INTVL);
+		break;
+	case LED_BLINK_WPS_STOP:	/* WPS success */
+		if (!pLed->bLedOn) {
+			pLed->bLedLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_NORMAL;
+			schedule_delayed_work(&pLed->blink_work, LED_BLINK_LINK_INTVL);
+
+			pLed->bLedWPSBlinkInProgress = false;
+		} else {
+			schedule_delayed_work(&pLed->blink_work, LED_BLINK_WPS_SUCESS_INTVL);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl8188eu_InitSwLeds(struct adapter *padapter)
+{
+	struct led_priv *pledpriv = &padapter->ledpriv;
+
+	pledpriv->padapter = padapter;
+	ResetLedStatus(pledpriv);
+	INIT_DELAYED_WORK(&pledpriv->blink_work, blink_work);
+}
+
+void rtl8188eu_DeInitSwLeds(struct adapter *padapter)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+
+	cancel_delayed_work_sync(&ledpriv->blink_work);
+	ResetLedStatus(ledpriv);
+	SwLedOff(padapter, ledpriv);
+}
+
+void rtw_led_control(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *pLed = &padapter->ledpriv;
+	struct registry_priv *registry_par;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
+	    (!padapter->hw_init_completed))
+		return;
+
+	if (!pLed->bRegUseLed)
+		return;
+
+	registry_par = &padapter->registrypriv;
+	if (!registry_par->led_enable)
+		return;
+
+	switch (LedAction) {
+	case LED_CTL_START_TO_LINK:
+	case LED_CTL_NO_LINK:
+		if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+			return;
+
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = false;
+
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_NO_LINK_INTVL);
+		break;
+	case LED_CTL_LINK:
+		if (!pLed->bLedLinkBlinkInProgress)
+			return;
+
+		if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+			return;
+
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedBlinkInProgress = false;
+		pLed->bLedLinkBlinkInProgress = true;
+
+		pLed->CurrLedState = LED_BLINK_NORMAL;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_LINK_INTVL);
+		break;
+	case LED_CTL_SITE_SURVEY:
+		if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED)))
+			return;
+
+		if (pLed->bLedScanBlinkInProgress)
+			return;
+
+		if (IS_LED_WPS_BLINKING(pLed))
+			return;
+
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = false;
+		pLed->bLedScanBlinkInProgress = true;
+
+		pLed->CurrLedState = LED_BLINK_SCAN;
+		pLed->BlinkTimes = 24;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_SCAN_INTVL);
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (pLed->bLedBlinkInProgress)
+			return;
+
+		if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+			return;
+
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = true;
+
+		pLed->CurrLedState = LED_BLINK_TXRX;
+		pLed->BlinkTimes = 2;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_FASTER_INTVL);
+		break;
+	case LED_CTL_START_WPS: /* wait until xinpin finish */
+		if (pLed->bLedWPSBlinkInProgress)
+			return;
+
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = false;
+		pLed->bLedScanBlinkInProgress = false;
+		pLed->bLedWPSBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_WPS;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_SCAN_INTVL);
+		break;
+	case LED_CTL_STOP_WPS:
+		cancel_delayed_work(&pLed->blink_work);
+
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = false;
+		pLed->bLedScanBlinkInProgress = false;
+		pLed->bLedWPSBlinkInProgress = true;
+
+		pLed->CurrLedState = LED_BLINK_WPS_STOP;
+		if (pLed->bLedOn) {
+			schedule_delayed_work(&pLed->blink_work, LED_BLINK_WPS_SUCESS_INTVL);
+		} else {
+			schedule_delayed_work(&pLed->blink_work, 0);
+		}
+		break;
+	case LED_CTL_STOP_WPS_FAIL:
+		cancel_delayed_work(&pLed->blink_work);
+		pLed->bLedWPSBlinkInProgress = false;
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		schedule_delayed_work(&pLed->blink_work, LED_BLINK_NO_LINK_INTVL);
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = RTW_LED_OFF;
+		pLed->bLedLinkBlinkInProgress = false;
+		pLed->bLedBlinkInProgress = false;
+		pLed->bLedWPSBlinkInProgress = false;
+		pLed->bLedScanBlinkInProgress = false;
+		cancel_delayed_work(&pLed->blink_work);
+		SwLedOff(padapter, pLed);
+		break;
+	default:
+		break;
+	}
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mlme.c b/drivers/staging/r8188eu/core/rtw_mlme.c
new file mode 100644
index 000000000..5ca03d6ca
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mlme.c
@@ -0,0 +1,2099 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTW_MLME_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/hal_intf.h"
+#include "../include/sta_info.h"
+#include "../include/wifi.h"
+#include "../include/wlan_bssdef.h"
+#include "../include/rtw_ioctl_set.h"
+#include "../include/usb_osintf.h"
+#include "../include/rtl8188e_dm.h"
+
+extern unsigned char	MCS_rate_1R[16];
+
+void rtw_set_roaming(struct adapter *adapter, u8 to_roaming)
+{
+	if (to_roaming == 0)
+		adapter->mlmepriv.to_join = false;
+	adapter->mlmepriv.to_roaming = to_roaming;
+}
+
+u8 rtw_to_roaming(struct adapter *adapter)
+{
+	return adapter->mlmepriv.to_roaming;
+}
+
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	kfree(*ppie);
+	*plen = 0;
+	*ppie = NULL;
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+	kfree(pmlmepriv->assoc_req);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+}
+
+void _rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall)
+{
+	u32 curr_time, delta_time;
+	u32 lifetime = SCANQUEUE_LIFETIME;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+
+	if (!pnetwork)
+		return;
+
+	if (pnetwork->fixed)
+		return;
+	curr_time = jiffies;
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+		lifetime = 1;
+	if (!isfreeall) {
+		delta_time = (curr_time - pnetwork->last_scanned) / HZ;
+		if (delta_time < lifetime)/*  unit:sec */
+			return;
+	}
+	spin_lock_bh(&free_queue->lock);
+	list_del_init(&pnetwork->list);
+	list_add_tail(&pnetwork->list, &free_queue->queue);
+	pmlmepriv->num_of_scanned--;
+	spin_unlock_bh(&free_queue->lock);
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+
+	if (!pnetwork)
+		return;
+	if (pnetwork->fixed)
+		return;
+	list_del_init(&pnetwork->list);
+	list_add_tail(&pnetwork->list, get_list_head(free_queue));
+	pmlmepriv->num_of_scanned--;
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be called under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+	struct list_head *phead, *plist;
+	struct	wlan_network *pnetwork = NULL;
+	u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+	if (!memcmp(zero_addr, addr, ETH_ALEN)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+	phead = get_list_head(scanned_queue);
+	plist = phead->next;
+
+	while (plist != phead) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
+			break;
+		plist = plist->next;
+	}
+	if (plist == phead)
+		pnetwork = NULL;
+exit:
+
+	return pnetwork;
+}
+
+void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall)
+{
+	struct list_head *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+	spin_lock_bh(&scanned_queue->lock);
+
+	phead = get_list_head(scanned_queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+
+		plist = plist->next;
+
+		_rtw_free_network(pmlmepriv, pnetwork, isfreeall);
+	}
+	spin_unlock_bh(&scanned_queue->lock);
+
+}
+
+int rtw_if_up(struct adapter *padapter)
+{
+	int res;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+	    !check_fwstate(&padapter->mlmepriv, _FW_LINKED))
+		res = false;
+	else
+		res =  true;
+
+	return res;
+}
+
+void rtw_generate_random_ibss(u8 *pibss)
+{
+	u32	curtime = jiffies;
+
+	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+	pibss[3] = (u8)(curtime & 0xff);/* p[0]; */
+	pibss[4] = (u8)((curtime >> 8) & 0xff);/* p[1]; */
+	pibss[5] = (u8)((curtime >> 16) & 0xff);/* p[2]; */
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+	return ie + 8 + 2;
+}
+
+u16 rtw_get_capability(struct wlan_bssid_ex *bss)
+{
+	__le16	val;
+
+	memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2);
+
+	return le16_to_cpu(val);
+}
+
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie)
+{
+	return ie + 8;
+}
+
+static void rtw_join_timeout_handler(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, mlmepriv.assoc_timer);
+
+	_rtw_join_timeout_handler(adapter);
+}
+
+static void _rtw_scan_timeout_handler(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, mlmepriv.scan_to_timer);
+
+	rtw_scan_timeout_handler(adapter);
+}
+
+static void _dynamic_check_timer_handlder(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, mlmepriv.dynamic_chk_timer);
+
+	rtw_dynamic_check_timer_handlder(adapter);
+	_set_timer(&adapter->mlmepriv.dynamic_chk_timer, 2000);
+}
+
+static void rtw_init_mlme_timer(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	timer_setup(&pmlmepriv->assoc_timer, rtw_join_timeout_handler, 0);
+	timer_setup(&pmlmepriv->scan_to_timer, _rtw_scan_timeout_handler, 0);
+	timer_setup(&pmlmepriv->dynamic_chk_timer, _dynamic_check_timer_handlder, 0);
+}
+
+int rtw_init_mlme_priv(struct adapter *padapter)/* struct	mlme_priv *pmlmepriv) */
+{
+	int	i;
+	u8	*pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	int	res = _SUCCESS;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+
+	pmlmepriv->pscanned = NULL;
+	pmlmepriv->fw_state = 0;
+	pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown;
+	pmlmepriv->scan_mode = SCAN_ACTIVE;/*  1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */
+
+	spin_lock_init(&pmlmepriv->lock);
+	rtw_init_queue(&pmlmepriv->free_bss_pool);
+	rtw_init_queue(&pmlmepriv->scanned_queue);
+
+	set_scanned_network_val(pmlmepriv, 0);
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+
+	pbuf = vzalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+
+	if (!pbuf) {
+		res = _FAIL;
+		goto exit;
+	}
+	pmlmepriv->free_bss_buf = pbuf;
+
+	pnetwork = (struct wlan_network *)pbuf;
+
+	for (i = 0; i < MAX_BSS_CNT; i++) {
+		INIT_LIST_HEAD(&pnetwork->list);
+
+		list_add_tail(&pnetwork->list, &pmlmepriv->free_bss_pool.queue);
+
+		pnetwork++;
+	}
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+	return res;
+}
+
+void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+	vfree(pmlmepriv->free_bss_buf);
+}
+
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
+{
+	struct	wlan_network	*pnetwork;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct list_head *plist = NULL;
+
+	spin_lock_bh(&free_queue->lock);
+
+	if (list_empty(&free_queue->queue)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+	plist = (&free_queue->queue)->next;
+
+	pnetwork = container_of(plist, struct wlan_network, list);
+
+	list_del_init(&pnetwork->list);
+
+	pnetwork->network_type = 0;
+	pnetwork->fixed = false;
+	pnetwork->last_scanned = jiffies;
+	pnetwork->aid = 0;
+	pnetwork->join_res = 0;
+
+	pmlmepriv->num_of_scanned++;
+
+exit:
+	spin_unlock_bh(&free_queue->lock);
+
+	return pnetwork;
+}
+
+static void rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+				    struct wlan_network *pnetwork)
+{
+
+	_rtw_free_network_nolock(pmlmepriv, pnetwork);
+
+}
+
+void rtw_free_network_queue(struct adapter *dev, u8 isfreeall)
+{
+
+	_rtw_free_network_queue(dev, isfreeall);
+
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be called under atomic context... to avoid possible racing condition...
+*/
+struct	wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret = true;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
+	    (pnetwork->network.Privacy == 0))
+		ret = false;
+	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
+		 (pnetwork->network.Privacy == 1))
+		ret = false;
+	else
+		ret = true;
+	return ret;
+}
+
+static int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
+{
+	return (a->Ssid.SsidLength == b->Ssid.SsidLength) &&
+	       !memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength);
+}
+
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
+{
+	u16 s_cap, d_cap;
+	__le16 le_scap, le_dcap;
+
+	memcpy((u8 *)&le_scap, rtw_get_capability_from_ie(src->IEs), 2);
+	memcpy((u8 *)&le_dcap, rtw_get_capability_from_ie(dst->IEs), 2);
+
+	s_cap = le16_to_cpu(le_scap);
+	d_cap = le16_to_cpu(le_dcap);
+
+	return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+		((!memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN))) &&
+		((!memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength))) &&
+		((s_cap & WLAN_CAPABILITY_IBSS) ==
+		(d_cap & WLAN_CAPABILITY_IBSS)) &&
+		((s_cap & WLAN_CAPABILITY_BSS) ==
+		(d_cap & WLAN_CAPABILITY_BSS)));
+}
+
+struct	wlan_network	*rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
+{
+	struct list_head *plist, *phead;
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+
+	phead = get_list_head(scanned_queue);
+
+	plist = phead->next;
+
+	while (1) {
+		if (phead == plist)
+			break;
+
+		pwlan = container_of(plist, struct wlan_network, list);
+
+		if (!pwlan->fixed) {
+			if (!oldest || time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+
+		plist = plist->next;
+	}
+
+	return oldest;
+}
+
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+	struct adapter *padapter, bool update_ie)
+{
+	long rssi_ori = dst->Rssi;
+	u8 sq_smp = src->PhyInfo.SignalQuality;
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+	AntDivCompare8188E(padapter, dst, src); /* this will update src.Rssi, need consider again */
+
+	/* The rule below is 1/5 for sample value, 4/5 for history value */
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&padapter->mlmepriv.cur_network.network, src)) {
+		/* Take the recvpriv's value for the connected AP*/
+		ss_final = padapter->recvpriv.signal_strength;
+		sq_final = padapter->recvpriv.signal_qual;
+		/* the rssi value here is undecorated, and will be used for antenna diversity */
+		if (sq_smp != 101) /* from the right channel */
+			rssi_final = dst->Rssi; //(src->Rssi+dst->Rssi*4)/5;
+		else
+			rssi_final = rssi_ori;
+	} else {
+//		if (sq_smp != 101) { /* from the right channel */
+			ss_final = (u32)dst->PhyInfo.SignalStrength; //((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
+			sq_final = (u32)dst->PhyInfo.SignalQuality; //((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
+			rssi_final = dst->Rssi; //(src->Rssi+dst->Rssi*4)/5;
+//		} else {
+//			/* bss info not receiving from the right channel, use the original RX signal infos */
+//			ss_final = dst->PhyInfo.SignalStrength;
+//			sq_final = dst->PhyInfo.SignalQuality;
+//			rssi_final = dst->Rssi;
+//		}
+	}
+	if (update_ie) {
+		dst->Reserved[0] = src->Reserved[0];
+		dst->Reserved[1] = src->Reserved[1];
+		memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src));
+	}
+	dst->PhyInfo.SignalStrength = ss_final;
+	dst->PhyInfo.SignalQuality = sq_final;
+	dst->Rssi = rssi_final;
+
+}
+
+static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    is_same_network(&pmlmepriv->cur_network.network, pnetwork)) {
+		update_network(&pmlmepriv->cur_network.network, pnetwork, adapter, true);
+		rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(struct ndis_802_11_fixed_ie),
+				      pmlmepriv->cur_network.network.IELength);
+	}
+
+}
+
+u8 rtw_current_antenna(struct adapter *adapter)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+
+	return haldata->CurAntenna;
+}
+
+/*
+Caller must hold pmlmepriv->lock first.
+*/
+void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
+{
+	struct list_head *plist, *phead;
+	u32	bssid_ex_sz;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct wlan_network	*pnetwork = NULL;
+	struct wlan_network	*oldest = NULL;
+
+	spin_lock_bh(&queue->lock);
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork	= container_of(plist, struct wlan_network, list);
+
+		if (is_same_network(&pnetwork->network, target))
+			break;
+		if ((oldest == ((struct wlan_network *)0)) ||
+		    time_after(oldest->last_scanned, pnetwork->last_scanned))
+			oldest = pnetwork;
+		plist = plist->next;
+	}
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information */
+	if (phead == plist) {
+		if (list_empty(&pmlmepriv->free_bss_pool.queue)) {
+			/* If there are no more slots, expire the oldest */
+			pnetwork = oldest;
+
+			target->PhyInfo.Optimum_antenna = rtw_current_antenna(adapter);
+
+			memcpy(&pnetwork->network, target,  get_wlan_bssid_ex_sz(target));
+			/*  variable initialize */
+			pnetwork->fixed = false;
+			pnetwork->last_scanned = jiffies;
+
+			pnetwork->network_type = 0;
+			pnetwork->aid = 0;
+			pnetwork->join_res = 0;
+
+			/* bss info not receiving from the right channel */
+			if (pnetwork->network.PhyInfo.SignalQuality == 101)
+				pnetwork->network.PhyInfo.SignalQuality = 0;
+		} else {
+			/* Otherwise just pull from the free list */
+
+			pnetwork = rtw_alloc_network(pmlmepriv); /*  will update scan_time */
+
+			if (!pnetwork)
+				goto exit;
+
+			bssid_ex_sz = get_wlan_bssid_ex_sz(target);
+			target->Length = bssid_ex_sz;
+			target->PhyInfo.Optimum_antenna = rtw_current_antenna(adapter);
+			memcpy(&pnetwork->network, target, bssid_ex_sz);
+
+			pnetwork->last_scanned = jiffies;
+
+			/* bss info not receiving from the right channel */
+			if (pnetwork->network.PhyInfo.SignalQuality == 101)
+				pnetwork->network.PhyInfo.SignalQuality = 0;
+			list_add_tail(&pnetwork->list, &queue->queue);
+		}
+	} else {
+		/* we have an entry and we are going to update it. But this entry may
+		 * be already expired. In this case we do the same as we found a new
+		 * net and call the new_net handler
+		 */
+		bool update_ie = true;
+
+		pnetwork->last_scanned = jiffies;
+
+		/* target.Reserved[0]== 1, means that scanned network is a bcn frame. */
+		/* probe resp(3) > beacon(1) > probe req(2) */
+		if ((target->Reserved[0] != 2) &&
+		    (target->Reserved[0] >= pnetwork->network.Reserved[0]))
+			update_ie = true;
+		else
+			update_ie = false;
+		update_network(&pnetwork->network, target, adapter, update_ie);
+	}
+
+exit:
+	spin_unlock_bh(&queue->lock);
+
+}
+
+static void rtw_add_network(struct adapter *adapter,
+			    struct wlan_bssid_ex *pnetwork)
+{
+
+	rtw_wlan_bssid_ex_remove_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO);
+	update_current_network(adapter, pnetwork);
+	rtw_update_scanned_network(adapter, pnetwork);
+
+}
+
+/* select the desired network based on the capability of the (i)bss. */
+/*  check items:	(1) security */
+/*			(2) network_type */
+/*			(3) WMM */
+/*			(4) HT */
+/*			(5) others */
+static bool rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u32 desired_encmode;
+	u32 privacy;
+
+	/* u8 wps_ie[512]; */
+	uint wps_ielen;
+
+	int bselected = true;
+
+	desired_encmode = psecuritypriv->ndisencryptstatus;
+	privacy = pnetwork->network.Privacy;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, pnetwork->network.IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen))
+			return true;
+		else
+			return false;
+	}
+	if (adapter->registrypriv.wifi_spec == 1) { /* for  correct flow of 8021X  to do.... */
+		u8 *p = NULL;
+		uint ie_len = 0;
+
+		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+			bselected = false;
+		if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) {
+			p = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_,
+				       _RSN_IE_2_, &ie_len,
+				       (pnetwork->network.IELength -
+				       _BEACON_IE_OFFSET_));
+			if (p && ie_len > 0)
+				bselected = true;
+			else
+				bselected = false;
+		}
+	}
+
+	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0))
+		bselected = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+			bselected = false;
+	}
+
+	return bselected;
+}
+
+void rtw_survey_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	u32 len;
+	struct wlan_bssid_ex *pnetwork;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	pnetwork = (struct wlan_bssid_ex *)pbuf;
+
+	len = get_wlan_bssid_ex_sz(pnetwork);
+	if (len > (sizeof(struct wlan_bssid_ex)))
+		return;
+	spin_lock_bh(&pmlmepriv->lock);
+
+	/*  update IBSS_network 's timestamp */
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, pnetwork->MacAddress, ETH_ALEN)) {
+			struct wlan_network *ibss_wlan = NULL;
+
+			memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+			ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue,  pnetwork->MacAddress);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.IEs, pnetwork->IEs, 8);
+				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+				goto exit;
+			}
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		}
+	}
+
+	/*  lock pmlmepriv->lock when you accessing network_q */
+	if (!check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		if (pnetwork->Ssid.Ssid[0] == 0)
+			pnetwork->Ssid.SsidLength = 0;
+		rtw_add_network(adapter, pnetwork);
+	}
+
+exit:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static void rtw_xmit_schedule(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv;
+
+	if (!padapter)
+		return;
+
+	pxmitpriv = &padapter->xmitpriv;
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	if (rtw_txframes_pending(padapter))
+		tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+
+	spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (pmlmepriv->wps_probe_req_ie) {
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		kfree(pmlmepriv->wps_probe_req_ie);
+		pmlmepriv->wps_probe_req_ie = NULL;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	del_timer_sync(&pmlmepriv->scan_to_timer);
+
+	spin_lock_bh(&pmlmepriv->lock);
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+	if (pmlmepriv->to_join) {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+				if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) {
+					_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+				} else {
+					struct wlan_bssid_ex    *pdev_network = &adapter->registrypriv.dev_network;
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+					rtw_update_registrypriv_dev_network(adapter);
+					rtw_generate_random_ibss(pibss);
+
+					pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+					rtw_createbss_cmd(adapter);
+					pmlmepriv->to_join = false;
+				}
+			}
+		} else {
+			int s_ret;
+			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+			pmlmepriv->to_join = false;
+			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			if (s_ret == _SUCCESS) {
+				_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			} else {
+				if (rtw_to_roaming(adapter) != 0) {
+					if (--pmlmepriv->to_roaming == 0 ||
+					    rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1) != _SUCCESS) {
+						rtw_set_roaming(adapter, 0);
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter);
+					} else {
+						pmlmepriv->to_join = true;
+					}
+				} else {
+					rtw_indicate_disconnect(adapter);
+				}
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			}
+		}
+	}
+
+	indicate_wx_scan_complete_event(adapter);
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0);
+
+	rtw_xmit_schedule(adapter);
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct __queue *scan_queue = &pmlmepriv->scanned_queue;
+	struct list_head *plist, *phead, *ptemp;
+
+	spin_lock_bh(&scan_queue->lock);
+	spin_lock_bh(&free_queue->lock);
+
+	phead = get_list_head(scan_queue);
+	plist = phead->next;
+
+	while (plist != phead) {
+		ptemp = plist->next;
+		list_del_init(plist);
+		list_add_tail(plist, &free_queue->queue);
+		plist = ptemp;
+		pmlmepriv->num_of_scanned--;
+	}
+
+	spin_unlock_bh(&free_queue->lock);
+	spin_unlock_bh(&scan_queue->lock);
+}
+
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
+{
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		rtw_free_stainfo(adapter,  psta);
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		rtw_free_all_stainfo(adapter);
+
+		psta = rtw_get_bcmc_stainfo(adapter);
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		rtw_free_stainfo(adapter, psta);
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+		rtw_init_bcmc_stainfo(adapter);
+	}
+
+	if (lock_scanned_queue)
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+	if (pwlan)
+		pwlan->fixed = false;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1)))
+		rtw_free_network_nolock(pmlmepriv, pwlan);
+
+	if (lock_scanned_queue)
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+	pmlmepriv->key_mask = 0;
+
+}
+
+static struct rt_pmkid_list backup_pmkid[NUM_PMKID_CACHE];
+
+static void rtw_reset_securitypriv(struct adapter *adapter)
+{
+	u8 backup_index;
+	u8 backup_counter;
+	u32 backup_time;
+
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+		/* 802.1x */
+		/*  We have to backup the PMK information for WiFi PMK Caching test item. */
+		/*  Backup the btkip_countermeasure information. */
+		/*  When the countermeasure is trigger, the driver have to disconnect with AP for 60 seconds. */
+		memcpy(&backup_pmkid[0], &adapter->securitypriv.PMKIDList[0], sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+		backup_index = adapter->securitypriv.PMKIDIndex;
+		backup_counter = adapter->securitypriv.btkip_countermeasure;
+		backup_time = adapter->securitypriv.btkip_countermeasure_time;
+		memset((unsigned char *)&adapter->securitypriv, 0, sizeof(struct security_priv));
+
+		/*  Restore the PMK information to securitypriv structure for the following connection. */
+		memcpy(&adapter->securitypriv.PMKIDList[0],
+			    &backup_pmkid[0],
+			    sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+		adapter->securitypriv.PMKIDIndex = backup_index;
+		adapter->securitypriv.btkip_countermeasure = backup_counter;
+		adapter->securitypriv.btkip_countermeasure_time = backup_time;
+		adapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+		adapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+	} else {
+		/* reset values in securitypriv */
+		struct security_priv *psec_priv = &adapter->securitypriv;
+
+		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;  /* open system */
+		psec_priv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psec_priv->dot11PrivacyKeyIndex = 0;
+		psec_priv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+		psec_priv->dot118021XGrpKeyid = 1;
+		psec_priv->ndisauthtype = Ndis802_11AuthModeOpen;
+		psec_priv->ndisencryptstatus = Ndis802_11WEPDisabled;
+	}
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(struct adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	pmlmepriv->to_join = false;
+
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_LINK);
+
+		rtw_indicate_wx_assoc_event(padapter);
+		netif_carrier_on(padapter->pnetdev);
+		if (padapter->pid[2] != 0)
+			rtw_signal_process(padapter->pid[2], SIGALRM);
+	}
+
+	pmlmepriv->to_roaming = 0;
+}
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING | WIFI_UNDER_WPS);
+
+	if (pmlmepriv->to_roaming > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) ||
+	    (pmlmepriv->to_roaming <= 0)) {
+		/*  Do it first for tx broadcast pkt after disconnection issue! */
+		netif_carrier_off(padapter->pnetdev);
+
+		rtw_indicate_wx_disassoc_event(padapter);
+		rtw_reset_securitypriv(padapter);
+
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+		rtw_led_control(padapter, LED_CTL_NO_LINK);
+	}
+	p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+
+}
+
+inline void rtw_indicate_scan_done(struct adapter *padapter)
+{
+	indicate_wx_scan_complete_event(padapter);
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	int i;
+	struct sta_info *bmc_sta, *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress);
+	if (!psta)
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
+
+	if (psta) { /* update ptarget_sta */
+		psta->aid  = pnetwork->join_res;
+		psta->mac_id = 0;
+		/* sta mode */
+		rtl8188e_SetHalODMVar(padapter, psta, true);
+		/* security related */
+		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+			padapter->securitypriv.binstallGrpkey = false;
+			padapter->securitypriv.busetkipkey = false;
+			padapter->securitypriv.bgrpkey_handshake = false;
+			psta->ieee8021x_blocked = true;
+			psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+			memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype));
+			memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype));
+			memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype));
+			memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48));
+			memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+		}
+		/*	When doing the WPS, the wps_ie_len won't equal to 0 */
+		/*	And the Wi-Fi driver shouldn't allow the data packet to be transmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
+			psta->ieee8021x_blocked = true;
+			padapter->securitypriv.wps_ie_len = 0;
+		}
+		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
+		/* if A-MPDU Rx is enabled, resetting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* todo: check if AP can send A-MPDU packets */
+		for (i = 0; i < 16; i++) {
+			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = false;
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+			preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+		}
+		bmc_sta = rtw_get_bcmc_stainfo(padapter);
+		if (bmc_sta) {
+			for (i = 0; i < 16; i++) {
+				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+				preorder_ctrl->wend_b = 0xffff;
+				preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+			}
+		}
+		/* misc. */
+		update_sta_info(padapter, psta);
+	}
+	return psta;
+}
+
+/* pnetwork: returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network  *cur_network = &pmlmepriv->cur_network;
+
+	/*  why not use ptarget_wlan?? */
+	memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+	/*  some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */
+	cur_network->network.IELength = ptarget_wlan->network.IELength;
+	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+
+	cur_network->aid = pnetwork->join_res;
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
+	/* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
+	switch (pnetwork->network.InfrastructureMode) {
+	case Ndis802_11Infrastructure:
+		if (pmlmepriv->fw_state & WIFI_UNDER_WPS)
+			pmlmepriv->fw_state = WIFI_STATION_STATE | WIFI_UNDER_WPS;
+		else
+			pmlmepriv->fw_state = WIFI_STATION_STATE;
+		break;
+	case Ndis802_11IBSS:
+		pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+		break;
+	default:
+		pmlmepriv->fw_state = WIFI_NULL_STATE;
+		break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.IEs) +
+			      sizeof(struct ndis_802_11_fixed_ie),
+			      (cur_network->network.IELength));
+	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength);
+}
+
+/* Notes: the function could be > passive_level (the same context as Rx tasklet) */
+/* pnetwork: returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+/* if join_res > 0, for (fw_state == WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist. */
+/* if join_res > 0, for (fw_state == WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */
+/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan != NULL). */
+
+void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
+{
+	struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network	*cur_network = &pmlmepriv->cur_network;
+	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
+	unsigned int		the_same_macaddr = false;
+
+	the_same_macaddr = !memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN);
+
+	pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
+	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex))
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (pnetwork->join_res > 0) {
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/* s1. find ptarget_wlan */
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr) {
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+				} else {
+					pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
+
+					pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+					if (pcur_sta) {
+						spin_lock_bh(&pstapriv->sta_hash_lock);
+						rtw_free_stainfo(adapter,  pcur_sta);
+						spin_unlock_bh(&pstapriv->sta_hash_lock);
+					}
+
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+					if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed = true;
+					}
+				}
+			} else {
+				ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+				if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
+				}
+			}
+
+			/* s2. update cur_network */
+			if (ptarget_wlan) {
+				rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+			} else {
+				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+				goto ignore_joinbss_callback;
+			}
+
+			/* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+				if (!ptarget_sta) {
+					spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+					goto ignore_joinbss_callback;
+				}
+			}
+
+			/* s4. indicate connect */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				pmlmepriv->cur_network_scanned = ptarget_wlan;
+				rtw_indicate_connect(adapter);
+			}
+
+			spin_unlock_bh(&pmlmepriv->lock);
+			/* s5. Cancel assoc_timer */
+			del_timer_sync(&pmlmepriv->assoc_timer);
+			spin_lock_bh(&pmlmepriv->lock);
+		} else {
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+			goto ignore_joinbss_callback;
+		}
+
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	} else if (pnetwork->join_res == -4) {
+		rtw_reset_securitypriv(adapter);
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	} else { /* if join_res < 0 (join fails), then try again */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	}
+
+ignore_joinbss_callback:
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+
+	mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+	rtw_xmit_schedule(adapter);
+}
+
+void rtw_set_max_rpt_macid(struct adapter *adapter, u8 macid)
+{
+	rtw_write8(adapter, REG_TX_RPT_CTRL + 1, macid + 1);
+}
+
+static u8 search_max_mac_id(struct adapter *padapter)
+{
+	u8 mac_id;
+	u8 aid;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		for (aid = (pstapriv->max_num_sta); aid > 0; aid--) {
+			if (pstapriv->sta_aid[aid - 1])
+				break;
+		}
+		mac_id = aid + 1;
+	} else {
+		/* adhoc  id =  31~2 */
+		for (mac_id = (NUM_STA - 1); mac_id >= IBSS_START_MAC_ID; mac_id--) {
+			if (pmlmeinfo->FW_sta_info[mac_id].status == 1)
+				break;
+		}
+	}
+	return mac_id;
+}
+
+/* FOR AP , AD-HOC mode */
+void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta,
+			      u32 mstatus)
+{
+	u16 media_status_rpt;
+	u8 macid;
+
+	if (!psta)
+		return;
+
+	macid = search_max_mac_id(adapter);
+	rtw_set_max_rpt_macid(adapter, macid);
+
+	/* MACID|OPMODE:1 connect */
+	media_status_rpt = (u16)((psta->mac_id << 8) | mstatus);
+	rtl8188e_set_FwMediaStatus_cmd(adapter, media_status_rpt);
+}
+
+void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	struct sta_info *psta;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct stassoc_event	*pstassoc = (struct stassoc_event *)pbuf;
+	struct wlan_network	*cur_network = &pmlmepriv->cur_network;
+	struct wlan_network	*ptarget_wlan = NULL;
+
+	if (!rtw_access_ctrl(adapter, pstassoc->macaddr))
+		return;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+		if (psta)
+			rtw_indicate_sta_assoc_event(adapter, psta);
+		return;
+	}
+	/* for AD-HOC mode */
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta)
+		/* the sta have been in sta_info_queue => do nothing */
+		return; /* between drv has received this event before and  fw have not yet to set key to CAM_ENTRY) */
+	psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (!psta)
+		return;
+	/* to do: init sta_info variable */
+	psta->qos_option = 0;
+	psta->mac_id = (uint)pstassoc->cam_id;
+
+	/* for ad-hoc mode */
+	rtl8188e_SetHalODMVar(adapter, psta, true);
+	rtw_sta_media_status_rpt(adapter, psta, 1);
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+	psta->ieee8021x_blocked = false;
+	spin_lock_bh(&pmlmepriv->lock);
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
+			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+			ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+			pmlmepriv->cur_network_scanned = ptarget_wlan;
+			if (ptarget_wlan)
+				ptarget_wlan->fixed = true;
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+			/*  a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			rtw_indicate_connect(adapter);
+		}
+	}
+	spin_unlock_bh(&pmlmepriv->lock);
+	mlmeext_sta_add_event_callback(adapter, psta);
+}
+
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	int mac_id = -1;
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	struct wlan_bssid_ex *pdev_network = NULL;
+	u8 *pibss = NULL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct	stadel_event *pstadel = (struct stadel_event *)pbuf;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+	if (psta)
+		mac_id = psta->mac_id;
+	else
+		mac_id = pstadel->mac_id;
+
+	if (mac_id >= 0) {
+		u16 media_status;
+		media_status = (mac_id << 8) | 0; /*   MACID|OPMODE:0 means disconnect */
+		/* for STA, AP, ADHOC mode, report disconnect stauts to FW */
+		rtl8188e_set_FwMediaStatus_cmd(adapter, media_status);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		return;
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		if (adapter->registrypriv.wifi_spec == 1)
+			rtw_set_roaming(adapter, 0); /* don't roam */
+		else if (rtw_to_roaming(adapter) > 0)
+			pmlmepriv->to_roaming--; /* this stadel_event is caused by roaming, decrease to_roaming */
+		else if (rtw_to_roaming(adapter) == 0)
+			rtw_set_roaming(adapter,
+					adapter->registrypriv.max_roaming_times);
+
+		if (*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+			rtw_set_roaming(adapter, 0); /* don't roam */
+
+		rtw_free_uc_swdec_pending_queue(adapter);
+
+		rtw_free_assoc_resources(adapter, 1);
+		rtw_indicate_disconnect(adapter);
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+		/*  remove the network entry in scanned_queue */
+		pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+		if (pwlan) {
+			pwlan->fixed = false;
+			rtw_free_network_nolock(pmlmepriv, pwlan);
+		}
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		_rtw_roaming(adapter, tgt_network);
+	}
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		rtw_free_stainfo(adapter,  psta);
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+		if (adapter->stapriv.asoc_sta_count == 1) { /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+			/* free old ibss network */
+			pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+			if (pwlan) {
+				pwlan->fixed = false;
+				rtw_free_network_nolock(pmlmepriv, pwlan);
+			}
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+			/* re-create ibss */
+			pdev_network = &adapter->registrypriv.dev_network;
+			pibss = adapter->registrypriv.dev_network.MacAddress;
+
+			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+
+			memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+			rtw_update_registrypriv_dev_network(adapter);
+
+			rtw_generate_random_ibss(pibss);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+
+			rtw_createbss_cmd(adapter);
+		}
+	}
+	spin_unlock_bh(&pmlmepriv->lock);
+
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	int do_join_r;
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return;
+
+	spin_lock_irq(&pmlmepriv->lock);
+
+	if (rtw_to_roaming(adapter) > 0) { /* join timeout caused by roaming */
+		while (1) {
+			pmlmepriv->to_roaming--;
+			if (rtw_to_roaming(adapter) != 0) { /* try another */
+				do_join_r = rtw_do_join(adapter);
+				if (do_join_r != _SUCCESS)
+					continue;
+				break;
+			} else {
+				rtw_indicate_disconnect(adapter);
+				break;
+			}
+		}
+	} else {
+		rtw_indicate_disconnect(adapter);
+		free_scanqueue(pmlmepriv);/*  */
+	}
+	spin_unlock_irq(&pmlmepriv->lock);
+
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Failure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	spin_unlock_bh(&pmlmepriv->lock);
+	rtw_indicate_scan_done(adapter);
+}
+
+static void rtw_auto_scan_handler(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* auto site survey per 60sec */
+	if (pmlmepriv->scan_interval > 0) {
+		pmlmepriv->scan_interval--;
+		if (pmlmepriv->scan_interval == 0) {
+			rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+			pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+		}
+	}
+}
+
+void rtw_dynamic_check_timer_handlder(struct adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+
+	if (!adapter)
+		return;
+	if (!adapter->hw_init_completed)
+		return;
+	if ((adapter->bDriverStopped) || (adapter->bSurpriseRemoved))
+		return;
+	if (adapter->net_closed)
+		return;
+	rtw_dynamic_chk_wk_cmd(adapter);
+
+	if (pregistrypriv->wifi_spec == 1) {
+		struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+			/* auto site survey */
+			rtw_auto_scan_handler(adapter);
+		}
+	}
+
+	rcu_read_lock();
+
+	if (rcu_dereference(adapter->pnetdev->rx_handler_data) &&
+	    check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE)) {
+		/*  expire NAT2.5 entry */
+		nat25_db_expire(adapter);
+
+		if (adapter->pppoe_connection_in_progress > 0)
+			adapter->pppoe_connection_in_progress--;
+
+		/*  due to rtw_dynamic_check_timer_handlder() is called every 2 seconds */
+		if (adapter->pppoe_connection_in_progress > 0)
+			adapter->pppoe_connection_in_progress--;
+	}
+
+	rcu_read_unlock();
+}
+
+#define RTW_SCAN_RESULT_EXPIRE 2000
+
+/*
+* Select a new join candidate from the original @param candidate and @param competitor
+* @return true: candidate is updated
+* @return false: candidate is not updated
+*/
+static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
+	, struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = false;
+	struct adapter *adapter = container_of(pmlmepriv, struct adapter, mlmepriv);
+	unsigned long scan_res_expire;
+
+	/* check bssid, if needed */
+	if (pmlmepriv->assoc_by_bssid) {
+		if (memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN))
+			goto exit;
+	}
+
+	/* check ssid, if needed */
+	if (pmlmepriv->assoc_ssid.SsidLength) {
+		if (competitor->network.Ssid.SsidLength != pmlmepriv->assoc_ssid.SsidLength ||
+		    memcmp(competitor->network.Ssid.Ssid, pmlmepriv->assoc_ssid.Ssid, pmlmepriv->assoc_ssid.SsidLength))
+			goto exit;
+	}
+
+	if (!rtw_is_desired_network(adapter, competitor))
+		goto exit;
+
+	scan_res_expire = competitor->last_scanned + msecs_to_jiffies(RTW_SCAN_RESULT_EXPIRE);
+	if (rtw_to_roaming(adapter) > 0) {
+		if (time_after(jiffies, scan_res_expire) ||
+		    !is_same_ess(&competitor->network, &pmlmepriv->cur_network.network))
+			goto exit;
+	}
+
+	if (!*candidate || (*candidate)->network.Rssi < competitor->network.Rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+
+exit:
+	return updated;
+}
+
+/*
+Calling context:
+The caller of the sub-routine will be in critical section...
+The caller must hold the following spinlock
+pmlmepriv->lock
+*/
+
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
+{
+	int ret;
+	struct list_head *phead;
+	struct adapter *adapter;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+	phead = get_list_head(queue);
+	adapter = (struct adapter *)pmlmepriv->nic_hdl;
+	pmlmepriv->pscanned = phead->next;
+	while (phead != pmlmepriv->pscanned) {
+		pnetwork = container_of(pmlmepriv->pscanned, struct wlan_network, list);
+		pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+	}
+	if (!candidate) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/*  check for situation of  _FW_LINKED */
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtw_disassoc_cmd(adapter, 0, true);
+		rtw_indicate_disconnect(adapter);
+		rtw_free_assoc_resources(adapter, 0);
+	}
+
+	ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	return ret;
+}
+
+int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj *pcmd;
+	struct	setauth_parm *psetauthparm;
+	struct	cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	int		res = _SUCCESS;
+
+	pcmd = kzalloc(sizeof(*pcmd), GFP_KERNEL);
+	if (!pcmd) {
+		res = _FAIL;  /* try again */
+		goto exit;
+	}
+
+	psetauthparm = kzalloc(sizeof(*psetauthparm), GFP_KERNEL);
+	if (!psetauthparm) {
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+	psetauthparm->mode = (unsigned char)psecuritypriv->dot11AuthAlgrthm;
+	pcmd->cmdcode = _SetAuth_CMD_;
+	pcmd->parmbuf = (unsigned char *)psetauthparm;
+	pcmd->cmdsz =  (sizeof(struct setauth_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	INIT_LIST_HEAD(&pcmd->list);
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+
+	return res;
+}
+
+int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, int keyid, u8 set_tx)
+{
+	u8	keylen;
+	struct cmd_obj		*pcmd;
+	struct setkey_parm	*psetkeyparm;
+	struct cmd_priv		*pcmdpriv = &adapter->cmdpriv;
+	struct mlme_priv		*pmlmepriv = &adapter->mlmepriv;
+	int	res = _SUCCESS;
+
+	pcmd = kzalloc(sizeof(*pcmd), GFP_KERNEL);
+	if (!pcmd) {
+		res = _FAIL;  /* try again */
+		goto exit;
+	}
+	psetkeyparm = kzalloc(sizeof(*psetkeyparm), GFP_KERNEL);
+	if (!psetkeyparm) {
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy;
+	else
+		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->set_tx = set_tx;
+	pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
+
+	switch (psetkeyparm->algorithm) {
+	case _WEP40_:
+		keylen = 5;
+		memcpy(&psetkeyparm->key[0], &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		break;
+	case _WEP104_:
+		keylen = 13;
+		memcpy(&psetkeyparm->key[0], &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		break;
+	case _TKIP_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	case _AES_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	default:
+		kfree(psetkeyparm);
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+	pcmd->cmdcode = _SetKey_CMD_;
+	pcmd->parmbuf = (u8 *)psetkeyparm;
+	pcmd->cmdsz =  (sizeof(struct setkey_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	INIT_LIST_HEAD(&pcmd->list);
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+	return res;
+}
+
+/* adjust IEs for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+	unsigned	int ielength = 0;
+	unsigned int i, j;
+
+	i = 12; /* after the fixed IE */
+	while (i < in_len) {
+		ielength = initial_out_len;
+
+		if (in_ie[i] == 0xDD && in_ie[i + 2] == 0x00 && in_ie[i + 3] == 0x50  && in_ie[i + 4] == 0xF2 && in_ie[i + 5] == 0x02 && i + 5 < in_len) {
+			/* WMM element ID and OUI */
+			/* Append WMM IE to the last index of out_ie */
+
+			for (j = i; j < i + 9; j++) {
+				out_ie[ielength] = in_ie[j];
+				ielength++;
+			}
+			out_ie[initial_out_len + 1] = 0x07;
+			out_ie[initial_out_len + 6] = 0x00;
+			out_ie[initial_out_len + 8] = 0x00;
+			break;
+		}
+		i += (in_ie[i + 1] + 2); /*  to the next IE element */
+	}
+	return ielength;
+}
+
+/*  */
+/*  Search by BSSID, */
+/*  Return Value: */
+/*		-1		:if there is no pre-auth key in the  table */
+/*		>= 0		:if there is pre-auth key, and   return the entry id */
+/*  */
+/*  */
+
+static int SecIsInPMKIDList(struct adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *p = &Adapter->securitypriv;
+	int i;
+
+	for (i = 0; i < NUM_PMKID_CACHE; i++)
+		if (p->PMKIDList[i].bUsed && !memcmp(p->PMKIDList[i].Bssid, bssid, ETH_ALEN))
+			return i;
+	return -1;
+}
+
+/*  */
+/*  Check the RSN IE length */
+/*  If the RSN IE length <= 20, the RSN IE didn't include the PMKID information */
+/*  0-11th element in the array are the fixed IE */
+/*  12th element in the array is the IE */
+/*  13th element in the array is the IE length */
+/*  */
+
+static int rtw_append_pmkid(struct adapter *Adapter, int iEntry, u8 *ie, uint ie_len)
+{
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+
+	if (ie[13] <= 20) {
+		/*  The RSN IE didn't include the PMK ID, append the PMK information */
+		ie[ie_len] = 1;
+		ie_len++;
+		ie[ie_len] = 0;	/* PMKID count = 0x0100 */
+		ie_len++;
+		memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+
+		ie_len += 16;
+		ie[13] += 18;/* PMKID length = 2+16 */
+	}
+	return ie_len;
+}
+
+static void rtw_report_sec_ie(struct adapter *adapter, u8 authmode, u8 *sec_ie)
+{
+	uint len;
+	u8 *buff, *p, i;
+	union iwreq_data wrqu;
+
+	buff = NULL;
+	if (authmode == _WPA_IE_ID_) {
+		buff = kzalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
+		if (!buff)
+			return;
+		p = buff;
+		p += sprintf(p, "ASSOCINFO(ReqIEs =");
+		len = sec_ie[1] + 2;
+		len =  (len < IW_CUSTOM_MAX) ? len : IW_CUSTOM_MAX;
+		for (i = 0; i < len; i++)
+			p += sprintf(p, "%02x", sec_ie[i]);
+		p += sprintf(p, ")");
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.length = p - buff;
+		wrqu.data.length = (wrqu.data.length < IW_CUSTOM_MAX) ?
+				   wrqu.data.length : IW_CUSTOM_MAX;
+		wireless_send_event(adapter->pnetdev, IWEVCUSTOM, &wrqu, buff);
+		kfree(buff);
+	}
+}
+
+int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode = 0;
+	uint	ielength;
+	int iEntry;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	uint	ndisauthmode = psecuritypriv->ndisauthtype;
+
+	/* copy fixed ie only */
+	memcpy(out_ie, in_ie, 12);
+	ielength = 12;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA) ||
+	    (ndisauthmode == Ndis802_11AuthModeWPAPSK))
+			authmode = _WPA_IE_ID_;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA2) ||
+	    (ndisauthmode == Ndis802_11AuthModeWPA2PSK))
+		authmode = _WPA2_IE_ID_;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		memcpy(out_ie + ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len);
+
+		ielength += psecuritypriv->wps_ie_len;
+	} else if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) {
+		/* copy RSN or SSN */
+		memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1] + 2);
+		ielength += psecuritypriv->supplicant_ie[1] + 2;
+		rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+	}
+
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if (iEntry < 0) {
+		return ielength;
+	} else {
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+	}
+
+	return ielength;
+}
+
+void rtw_init_registrypriv_dev_network(struct adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+
+	memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid));
+
+	pdev_network->Configuration.Length = sizeof(struct ndis_802_11_config);
+	pdev_network->Configuration.BeaconPeriod = 100;
+	pdev_network->Configuration.FHConfig.Length = 0;
+	pdev_network->Configuration.FHConfig.HopPattern = 0;
+	pdev_network->Configuration.FHConfig.HopSet = 0;
+	pdev_network->Configuration.FHConfig.DwellTime = 0;
+
+}
+
+void rtw_update_registrypriv_dev_network(struct adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+
+	pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0); /*  adhoc no 802.1x */
+
+	pdev_network->Rssi = 0;
+
+	switch (pregistrypriv->wireless_mode) {
+	case WIRELESS_11B:
+		pdev_network->NetworkTypeInUse = (Ndis802_11DS);
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11_24N:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+		break;
+	default:
+		/*  TODO */
+		break;
+	}
+
+	pdev_network->Configuration.DSConfig = (pregistrypriv->channel);
+
+	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+		pdev_network->Configuration.ATIMWindow = (0);
+
+	pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+	/*  1. Supported rates */
+	/*  2. IE */
+
+	sz = rtw_generate_ie(pregistrypriv);
+	pdev_network->IELength = sz;
+	pdev_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex  *)pdev_network);
+
+	/* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */
+	/* pdev_network->IELength = cpu_to_le32(sz); */
+
+}
+
+static void rtw_set_threshold(struct adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	struct ht_priv *htpriv = &mlmepriv->htpriv;
+
+	if (htpriv->ht_option && adapter->registrypriv.wifi_spec != 1) {
+		/* validate usb rx aggregation, use init value. */
+		rtw_write8(adapter, REG_RXDMA_AGG_PG_TH, USB_RXAGG_PAGE_COUNT);
+	} else {
+		/* invalidate usb rx aggregation */
+		rtw_write8(adapter, REG_RXDMA_AGG_PG_TH, 1);
+	}
+}
+
+/* the function is at passive_level */
+void rtw_joinbss_reset(struct adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+	/* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+	pmlmepriv->num_FortyMHzIntolerant = 0;
+
+	pmlmepriv->num_sta_no_ht = 0;
+
+	phtpriv->ampdu_enable = false;/* reset to disabled */
+
+	rtw_set_threshold(padapter);
+}
+
+/* the function is >= passive_level */
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len)
+{
+	u32 ielen, out_len;
+	unsigned char *p;
+	struct ieee80211_ht_cap ht_capie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+	phtpriv->ht_option = false;
+
+	p = rtw_get_ie(in_ie + 12, _HT_CAPABILITY_IE_, &ielen, in_len - 12);
+
+	if (p && ielen > 0) {
+		if (pqospriv->qos_option == 0) {
+			out_len = *pout_len;
+			rtw_set_ie(out_ie + out_len, _VENDOR_SPECIFIC_IE_,
+				   _WMM_IE_Length_, WMM_IE, pout_len);
+
+			pqospriv->qos_option = 1;
+		}
+
+		out_len = *pout_len;
+
+		memset(&ht_capie, 0, sizeof(struct ieee80211_ht_cap));
+
+		ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+						IEEE80211_HT_CAP_SGI_20 |
+						IEEE80211_HT_CAP_SGI_40 |
+						IEEE80211_HT_CAP_TX_STBC |
+						IEEE80211_HT_CAP_DSSSCCK40);
+
+		/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+		*/
+
+		ht_capie.ampdu_params_info = (MAX_AMPDU_FACTOR_64K & 0x03);
+
+		if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_AMPDU_PARM_DENSITY & (0x07 << 2));
+		else
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_AMPDU_PARM_DENSITY & 0x00);
+
+		rtw_set_ie(out_ie + out_len, _HT_CAPABILITY_IE_,
+			   sizeof(struct ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+		phtpriv->ht_option = true;
+
+		p = rtw_get_ie(in_ie + 12, _HT_ADD_INFO_IE_, &ielen, in_len - 12);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			out_len = *pout_len;
+			rtw_set_ie(out_ie + out_len, _HT_ADD_INFO_IE_, ielen, p + 2, pout_len);
+		}
+	}
+	return phtpriv->ht_option;
+}
+
+/* the function is > passive_level (in critical_section) */
+void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len)
+{
+	u8 *p, max_ampdu_sz;
+	int len;
+	struct ieee80211_ht_cap *pht_capie;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (!phtpriv->ht_option)
+		return;
+
+	if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable))
+		return;
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if ((!phtpriv->ampdu_enable) && (pregistrypriv->ampdu_enable == 1)) {
+		if (pregistrypriv->wifi_spec == 1)
+			phtpriv->ampdu_enable = false;
+		else
+			phtpriv->ampdu_enable = true;
+	} else if (pregistrypriv->ampdu_enable == 2) {
+		phtpriv->ampdu_enable = true;
+	}
+
+	/* check Max Rx A-MPDU Size */
+	len = 0;
+	p = rtw_get_ie(pie + sizeof(struct ndis_802_11_fixed_ie), _HT_CAPABILITY_IE_, &len, ie_len - sizeof(struct ndis_802_11_fixed_ie));
+	if (p && len > 0) {
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR);
+		max_ampdu_sz = 1 << (max_ampdu_sz + 3); /*  max_ampdu_sz (kbytes); */
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+	}
+	len = 0;
+	p = rtw_get_ie(pie + sizeof(struct ndis_802_11_fixed_ie), _HT_ADD_INFO_IE_, &len, ie_len - sizeof(struct ndis_802_11_fixed_ie));
+
+	/* update cur_bwmode & cur_ch_offset */
+	if ((pregistrypriv->cbw40_enable) &&
+	    (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & BIT(1)) &&
+	    (pmlmeinfo->HT_info.infos[0] & BIT(2))) {
+		int i;
+
+		/* update the MCS rates */
+		for (i = 0; i < 16; i++)
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+
+		/* switch to the 40M Hz mode according to the AP */
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+		switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) {
+		case HT_EXTCHNL_OFFSET_UPPER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+		case HT_EXTCHNL_OFFSET_LOWER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+		default:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+	}
+
+	/*  Config SM Power Save setting */
+	pmlmeinfo->SM_PS = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & 0x0C) >> 2;
+
+	/*  Config current HT Protection mode. */
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 issued;
+	int priority;
+	struct sta_info *psta = NULL;
+	struct ht_priv	*phtpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	if (is_multicast_ether_addr(pattrib->ra) ||
+	    padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100)
+		return;
+
+	priority = pattrib->priority;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+		issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1;
+		issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1;
+
+		if (issued == 0) {
+			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+			rtw_addbareq_cmd(padapter, (u8)priority, pattrib->ra);
+		}
+	}
+}
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+	_rtw_roaming(padapter, tgt_network);
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	int do_join_r;
+
+	struct wlan_network *pnetwork;
+
+	if (tgt_network)
+		pnetwork = tgt_network;
+	else
+		pnetwork = &pmlmepriv->cur_network;
+
+	if (rtw_to_roaming(padapter) > 0) {
+		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid, sizeof(struct ndis_802_11_ssid));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+		while (1) {
+			do_join_r = rtw_do_join(padapter);
+			if (do_join_r == _SUCCESS) {
+				break;
+			} else {
+				pmlmepriv->to_roaming--;
+
+				if (pmlmepriv->to_roaming > 0) {
+					continue;
+				} else {
+					rtw_indicate_disconnect(padapter);
+					break;
+				}
+			}
+		}
+	}
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mlme_ext.c b/drivers/staging/r8188eu/core/rtw_mlme_ext.c
new file mode 100644
index 000000000..07905e2ae
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mlme_ext.c
@@ -0,0 +1,7989 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_MLME_EXT_C_
+
+#include <linux/ieee80211.h>
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+#include "../include/rtw_mlme_ext.h"
+#include "../include/wlan_bssdef.h"
+#include "../include/rtl8188e_xmit.h"
+#include "../include/rtl8188e_dm.h"
+
+/* response function for each management frame subtype, do not reorder */
+static mlme_handler mlme_sta_tbl[] = {
+	OnAssocReq,
+	OnAssocRsp,
+	OnAssocReq,
+	OnAssocRsp,
+	OnProbeReq,
+	OnProbeRsp,
+	NULL,
+	NULL,
+	OnBeacon,
+	NULL,
+	OnDisassoc,
+	OnAuthClient,
+	OnDeAuth,
+	OnAction,
+};
+
+static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+/**************************************************
+OUI definitions for the vendor specific IE
+***************************************************/
+unsigned char	RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char	WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+unsigned char	P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};
+unsigned char	WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A};
+
+unsigned char	WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+unsigned char	WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+
+extern unsigned char REALTEK_96B_IE[];
+
+/********************************************************
+MCS rate definitions
+*********************************************************/
+unsigned char	MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+static struct rt_channel_plan RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},			/*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},	/*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
+	{{10, 11, 12, 13}, 4},					/*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
+	{{}, 0},									/*  0x05, RT_CHANNEL_DOMAIN_2G_NULL */
+};
+
+static struct rt_channel_plan_map	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	/*  0x00 ~ 0x1F , Old Define ===== */
+	{0x02},	/* 0x00, RT_CHANNEL_DOMAIN_FCC */
+	{0x02},	/* 0x01, RT_CHANNEL_DOMAIN_IC */
+	{0x01},	/* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+	{0x01},	/* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+	{0x01},	/* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+	{0x03},	/* 0x05, RT_CHANNEL_DOMAIN_MKK */
+	{0x03},	/* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+	{0x01},	/* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+	{0x03},	/* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+	{0x03},	/* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+	{0x00},	/* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+	{0x02},	/* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+	{0x01},	/* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+	{0x02},	/* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+	{0x02},	/* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+	{0x02},	/* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+	{0x01},	/* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+	{0x02},	/* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+	{0x01},	/* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x00}, /* 0x13 */
+	{0x02},	/* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+	{0x00},	/* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+	{0x00},	/* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+	{0x03},	/* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x05},	/* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+	{0x02},	/* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+	{0x00},	/* 0x1A, */
+	{0x00},	/* 0x1B, */
+	{0x00},	/* 0x1C, */
+	{0x00},	/* 0x1D, */
+	{0x00},	/* 0x1E, */
+	{0x00},	/* 0x1F, */
+	/*  0x20 ~ 0x7F , New Define ===== */
+	{0x00},	/* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+	{0x01},	/* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+	{0x02},	/* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+	{0x03},	/* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+	{0x04},	/* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+	{0x02},	/* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+	{0x00},	/* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+	{0x03},	/* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+	{0x00},	/* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+	{0x00},	/* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+	{0x00},	/* 0x2A, */
+	{0x00},	/* 0x2B, */
+	{0x00},	/* 0x2C, */
+	{0x00},	/* 0x2D, */
+	{0x00},	/* 0x2E, */
+	{0x00},	/* 0x2F, */
+	{0x00},	/* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+	{0x00},	/* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+	{0x00},	/* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+	{0x00},	/* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+	{0x02},	/* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+	{0x00},	/* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+	{0x00},	/* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+	{0x03},	/* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+	{0x03},	/* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+	{0x02},	/* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+	{0x00},	/* 0x3A, */
+	{0x00},	/* 0x3B, */
+	{0x00},	/* 0x3C, */
+	{0x00},	/* 0x3D, */
+	{0x00},	/* 0x3E, */
+	{0x00},	/* 0x3F, */
+	{0x02},	/* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+	{0x03},	/* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
+};
+
+static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03}; /* use the conbination for max channel numbers */
+
+/*
+ * Search the @param channel_num in given @param channel_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ *
+ * return the index of channel_num in channel_set, -1 if not found
+ */
+int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
+{
+	int i;
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (ch == ch_set[i].ChannelNum)
+			break;
+	}
+
+	if (i >= ch_set[i].ChannelNum)
+		return -1;
+	return i;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	return _SUCCESS;
+}
+
+static void init_mlme_ext_priv_value(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	unsigned char	mixed_datarate[NumRates] = {
+		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+		_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
+		 _48M_RATE_, _54M_RATE_, 0xff
+	};
+	unsigned char	mixed_basicrate[NumRates] = {
+		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+		_12M_RATE_, _24M_RATE_, 0xff,
+	};
+
+	atomic_set(&pmlmeext->event_seq, 0);
+	pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
+
+	pmlmeext->cur_channel = padapter->registrypriv.channel;
+	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+
+	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->scan_abort = false;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	memset(pmlmeinfo->chg_txt, 0, 128);
+
+	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+	pmlmeinfo->dialogToken = 0;
+
+	pmlmeext->action_public_rxseq = 0xffff;
+	pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(struct rt_channel_info *channel_set,
+					   u8 chanset_size,
+					   u8 chan)
+{
+	int i;
+
+	for (i = 0; i < chanset_size; i++) {
+		if (channel_set[i].ChannelNum == chan)
+			return 1;
+	}
+	return 0;
+}
+
+static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
+							  u8 chanset_size,
+							  struct p2p_channels *channel_list)
+{
+	struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch)) {
+				continue;
+			}
+
+			if ((padapter->registrypriv.ht_enable == 0) && (o->inc == 8))
+				continue;
+
+			if (((padapter->registrypriv.cbw40_enable & BIT(1)) == 0) &&
+			    ((o->bw == BW40MINUS) || (o->bw == BW40PLUS)))
+				continue;
+
+			if (!reg) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+}
+
+static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b2_4GBand = false;
+	u8 Index2G = 0;
+
+	memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+		return chanset_size;
+
+	if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
+		b2_4GBand = true;
+		if (ChannelPlan == RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+	}
+
+	if (b2_4GBand) {
+		for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+			if ((ChannelPlan == RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN) ||/* Channel 1~11 is active, and 12~14 is passive */
+			    (ChannelPlan == RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G)) {
+				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
+			} else if (ChannelPlan == RT_CHANNEL_DOMAIN_WORLD_WIDE_13 ||
+				   Index2G == RT_CHANNEL_DOMAIN_2G_WORLD) {/*  channel 12~13, passive scan */
+				if (channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			} else {
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+			}
+
+			chanset_size++;
+		}
+	}
+	return chanset_size;
+}
+
+static void _survey_timer_hdl(struct timer_list *t)
+{
+	struct adapter *padapter = from_timer(padapter, t, mlmeextpriv.survey_timer);
+
+	survey_timer_hdl(padapter);
+}
+
+static void _link_timer_hdl(struct timer_list *t)
+{
+	struct adapter *padapter = from_timer(padapter, t, mlmeextpriv.link_timer);
+
+	link_timer_hdl(padapter);
+}
+
+static void init_mlme_ext_timer(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	timer_setup(&pmlmeext->survey_timer, _survey_timer_hdl, 0);
+	timer_setup(&pmlmeext->link_timer, _link_timer_hdl, 0);
+}
+
+void init_mlme_ext_priv(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmlmeext->padapter = padapter;
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+
+	init_mlme_ext_timer(padapter);
+
+	init_mlme_ap_info(padapter);
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	pmlmeext->chan_scan_time = SURVEY_TO;
+	pmlmeext->mlmeext_init = true;
+
+	pmlmeext->active_keep_alive_check = true;
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped) {
+		_cancel_timer_ex(&pmlmeext->survey_timer);
+		_cancel_timer_ex(&pmlmeext->link_timer);
+		/* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
+	}
+}
+
+void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	int index;
+	mlme_handler fct;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, hdr->addr2);
+
+	if (!ieee80211_is_mgmt(hdr->frame_control))
+		return;
+
+	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+	if (memcmp(hdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN) &&
+	    !is_broadcast_ether_addr(hdr->addr1))
+		return;
+
+	index = (le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE) >> 4;
+	if (index >= ARRAY_SIZE(mlme_sta_tbl))
+		return;
+	fct = mlme_sta_tbl[index];
+
+	if (psta) {
+		if (ieee80211_has_retry(hdr->frame_control)) {
+			if (precv_frame->attrib.seq_num == psta->RxMgmtFrameSeqNum)
+				/* drop the duplicate management frame */
+				return;
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
+	}
+
+	if (ieee80211_is_auth(hdr->frame_control)) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			fct = OnAuth;
+		else
+			fct = OnAuthClient;
+	}
+
+	if (fct)
+		fct(padapter, precv_frame);
+}
+
+static u32 p2p_listen_state_process(struct adapter *padapter, unsigned char *da)
+{
+	bool response = true;
+
+	/*	do nothing if the device name is empty */
+	if (!padapter->wdinfo.device_name_len)
+		response = false;
+
+	if (response)
+		issue_probersp_p2p(padapter, da);
+
+	return _SUCCESS;
+}
+
+static void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe)
+{
+	u8 *pIE;
+	__le32 *pbuf;
+
+	pIE = pframe + sizeof(struct ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	pmlmeext->TSFValue = le32_to_cpu(*(pbuf + 1));
+
+	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
+
+	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
+}
+
+static void correct_TSF(struct adapter *padapter)
+{
+	u8 reg;
+	int res;
+	u64 tsf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	tsf = pmlmeext->TSFValue - do_div(pmlmeext->TSFValue,
+					  pmlmeinfo->bcn_interval * 1024) - 1024; /* us */
+
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
+	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
+		rtw_stop_tx_beacon(padapter);
+
+	/* disable related TSF function */
+	res = rtw_read8(padapter, REG_BCN_CTRL, &reg);
+	if (res)
+		return;
+
+	rtw_write8(padapter, REG_BCN_CTRL, reg & (~BIT(3)));
+
+	rtw_write32(padapter, REG_TSFTR, tsf);
+	rtw_write32(padapter, REG_TSFTR + 4, tsf >> 32);
+
+	/* enable related TSF function */
+	res = rtw_read8(padapter, REG_BCN_CTRL, &reg);
+	if (res)
+		return;
+
+	rtw_write8(padapter, REG_BCN_CTRL, reg | BIT(3));
+
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
+	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
+		rtw_resume_tx_beacon(padapter);
+}
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char	*p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+	u8 is_valid_p2p_probereq = false;
+
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
+	    !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
+		/*	mcs_rate = 0 -> CCK 1M rate */
+		/*	mcs_rate = 1 -> CCK 2M rate */
+		/*	mcs_rate = 2 -> CCK 5.5M rate */
+		/*	mcs_rate = 3 -> CCK 11M rate */
+		/*	In the P2P mode, the driver should not support the CCK rate */
+
+		/*	Commented by Kurt 2012/10/16 */
+		/*	IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
+		is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
+		if (is_valid_p2p_probereq) {
+			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+				/*  FIXME */
+				report_survey_event(padapter, precv_frame);
+				p2p_listen_state_process(padapter,  get_sa(pframe));
+
+				return _SUCCESS;
+			}
+		}
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
+		return _SUCCESS;
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+	/* check (wildcard) SSID */
+	if (p) {
+		if (is_valid_p2p_probereq)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && memcmp((void *)(p + 2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
+		    (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
+			return _SUCCESS;
+
+_issue_probersp:
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+		    (pmlmepriv->cur_network.join_res ||
+		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+	}
+	return _SUCCESS;
+}
+
+unsigned int OnProbeRsp(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 *pframe = precv_frame->rx_data;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+		if (pwdinfo->tx_prov_disc_info.benable) {
+			if (!memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+					pwdinfo->tx_prov_disc_info.benable = false;
+					issue_p2p_provision_request(padapter,
+								    pwdinfo->tx_prov_disc_info.ssid.Ssid,
+								    pwdinfo->tx_prov_disc_info.ssid.SsidLength,
+								    pwdinfo->tx_prov_disc_info.peerDevAddr);
+				} else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+					pwdinfo->tx_prov_disc_info.benable = false;
+					issue_p2p_provision_request(padapter, NULL, 0,
+								    pwdinfo->tx_prov_disc_info.peerDevAddr);
+				}
+			}
+		}
+		return _SUCCESS;
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+		if (pwdinfo->nego_req_info.benable) {
+			if (!memcmp(pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+				pwdinfo->nego_req_info.benable = false;
+				issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr);
+			}
+		}
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
+		if (pwdinfo->invitereq_info.benable) {
+			if (!memcmp(pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+				pwdinfo->invitereq_info.benable = false;
+				issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr);
+			}
+		}
+	}
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int OnBeacon(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	int cam_idx;
+	struct sta_info	*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+	struct wlan_bssid_ex *pbss;
+	int ret = _SUCCESS;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) {
+		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+			/* we should update current network before auth, or some IE is wrong */
+			pbss = kmalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					update_network(&pmlmepriv->cur_network.network, pbss, padapter, true);
+					rtw_get_bcn_info(&pmlmepriv->cur_network);
+				}
+				kfree(pbss);
+			}
+
+			/* check the vendor of the assoc AP */
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe + sizeof(struct ieee80211_hdr_3addr), len - sizeof(struct ieee80211_hdr_3addr));
+
+			/* update TSF Value */
+			update_TSF(pmlmeext, pframe);
+
+			/* start auth */
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+					receive_disconnect(padapter,
+							   pmlmeinfo->network.MacAddress, 0);
+					return _SUCCESS;
+				}
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+				process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+			}
+		} else if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta) {
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+			} else {
+				/* allocate a new CAM entry for IBSS station */
+				cam_idx = allocate_fw_sta_entry(padapter);
+				if (cam_idx == NUM_STA)
+					goto _END_ONBEACON_;
+
+				/* get supported rate */
+				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
+					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+					goto _END_ONBEACON_;
+				}
+
+				/* update TSF Value */
+				update_TSF(pmlmeext, pframe);
+
+				/* report sta add event */
+				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+}
+
+unsigned int OnAuth(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned int	auth_mode, ie_len;
+	u16 seq;
+	unsigned char	*sa, *p;
+	u16 algorithm;
+	int	status;
+	static struct sta_info stat;
+	struct	sta_info	*pstat = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	sa = GetAddr2Ptr(pframe);
+
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+	seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
+	algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
+
+	if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	/*  rx a shared-key auth but shared not enabled */
+	    (algorithm == 0 && auth_mode == 1)) {	/*  rx a open-system auth but shared-key is enabled */
+
+		status = _STATS_NO_SUPP_ALG_;
+
+		goto auth_fail;
+	}
+
+	if (!rtw_access_ctrl(padapter, sa)) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (!pstat) {
+		/*  allocate a new one */
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (!pstat) {
+			status = _STATS_UNABLE_HANDLE_STA_;
+			goto auth_fail;
+		}
+
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+	} else {
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		if (!list_empty(&pstat->asoc_list)) {
+			list_del_init(&pstat->asoc_list);
+			pstapriv->asoc_list_cnt--;
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		if (seq == 1) {
+			/* TODO: STA re_auth and auth timeout */
+		}
+	}
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (list_empty(&pstat->auth_list)) {
+		list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+		pstapriv->auth_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+	if ((pstat->auth_seq + 1) != seq) {
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		goto auth_fail;
+	}
+
+	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
+		if (seq == 1) {
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_SUCCESS;
+			pstat->expire_to = pstapriv->assoc_to;
+			pstat->authalg = algorithm;
+		} else {
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	} else { /*  shared system or auto authentication */
+		if (seq == 1) {
+			/* prepare for the challenging txt... */
+
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_STATE;
+			pstat->authalg = algorithm;
+			pstat->auth_seq = 2;
+		} else if (seq == 3) {
+			/* checking for challenging txt... */
+
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&ie_len,
+					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if (!p || ie_len <= 0) {
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+
+			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+				pstat->state &= (~WIFI_FW_AUTH_STATE);
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				/*  challenging txt is correct... */
+				pstat->expire_to =  pstapriv->assoc_to;
+			} else {
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else {
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+
+	/*  Now, we are going to issue_auth... */
+	pstat->auth_seq = seq + 1;
+
+	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+
+	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
+		pstat->auth_seq = 0;
+
+	return _SUCCESS;
+
+auth_fail:
+
+	if (pstat)
+		rtw_free_stainfo(padapter, pstat);
+
+	pstat = &stat;
+	memset((char *)pstat, '\0', sizeof(stat));
+	pstat->auth_seq = 2;
+	memcpy(pstat->hwaddr, sa, 6);
+
+	issue_auth(padapter, pstat, (unsigned short)status);
+	return _FAIL;
+}
+
+unsigned int OnAuthClient(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, offset;
+	unsigned char	*p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&padapter->eeprompriv), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+	seq	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+		}
+
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			 /*  legendary shared system */
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (!p)
+				goto authclnt_fail;
+
+			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		} else {
+			/*  open system */
+			go2asoc = 1;
+		}
+	} else if (seq == 4) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+			go2asoc = 1;
+		else
+			goto authclnt_fail;
+	} else {
+		/*  this is also illegal */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+authclnt_fail:
+	return _FAIL;
+}
+
+static void UpdateBrateTbl(u8 *mbrate)
+{
+	u8 i;
+	u8 rate;
+
+	/*  1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		rate = mbrate[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+		case IEEE80211_OFDM_RATE_6MB:
+		case IEEE80211_OFDM_RATE_12MB:
+		case IEEE80211_OFDM_RATE_24MB:
+			mbrate[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+}
+
+static void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+{
+	u8 i;
+	u8 rate;
+
+	for (i = 0; i < bssratelen; i++) {
+		rate = bssrateset[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+			bssrateset[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+}
+
+unsigned int OnAssocReq(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	u16 capab_info;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info	*pstat;
+	unsigned char		*p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	unsigned char		supportRate[16];
+	int					supportRateNum;
+	unsigned short		status = _STATS_SUCCESSFUL_;
+	unsigned short		frame_type, ie_offset = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 p2p_status_code = P2P_STATUS_SUCCESS;
+	u8 *p2pie;
+	u32 p2pielen = 0;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ)
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	else /*  WIFI_REASSOCREQ */
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset)
+		return _FAIL;
+
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (pstat == (struct sta_info *)NULL) {
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+	/*  check if this stat has been successfully authenticated/assocated */
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+			status = _RSON_CLS2_;
+			goto asoc_class2_error;
+		} else {
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;
+		}
+	} else {
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+	pstat->capability = capab_info;
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+	/*  now we should check all the fields... */
+	/*  checking SSID */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (!p)
+		status = _STATS_FAILURE_;
+
+	if (ie_len == 0) { /*  broadcast ssid, however it is not allowed in assocreq */
+		status = _STATS_FAILURE_;
+	} else {
+		/*  check if ssid match */
+		if (memcmp((void *)(p + 2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			status = _STATS_FAILURE_;
+
+		if (ie_len != cur->Ssid.SsidLength)
+			status = _STATS_FAILURE_;
+	}
+
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+	/*  check if the supported rate is ok */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (!p) {
+		/*  use our own rate set as statoin used */
+		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+		/* supportRateNum = AP_BSSRATE_LEN; */
+
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	} else {
+		memcpy(supportRate, p + 2, ie_len);
+		supportRateNum = ie_len;
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len,
+				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+		if (p) {
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate + supportRateNum, p + 2, ie_len);
+				supportRateNum += ie_len;
+			}
+		}
+	}
+
+	/* todo: mask supportRate between AP & STA -> move to update raid */
+	/* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
+
+	/* update station supportRate */
+	pstat->bssratelen = supportRateNum;
+	memcpy(pstat->bssrateset, supportRate, supportRateNum);
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	/* check RSN/WPA/WPS */
+	pstat->dot8021xalg = 0;
+	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if (rtw_parse_wpa2_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher & psecuritypriv->wpa2_group_cipher;
+			pstat->wpa2_pairwise_cipher = pairwise_cipher & psecuritypriv->wpa2_pairwise_cipher;
+
+			if (!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if (rtw_parse_wpa_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher & psecuritypriv->wpa_group_cipher;
+			pstat->wpa_pairwise_cipher = pairwise_cipher & psecuritypriv->wpa_pairwise_cipher;
+
+			if (!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	if (!wpa_ie) {
+		if (elems.wps_ie)
+			pstat->flags |= WLAN_STA_WPS;
+			/* wpabuf_free(sta->wps_ie); */
+			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+			/*				elems.wps_ie_len - 4); */
+		else
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  that the selected registrar of AP is _FLASE */
+		if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS | WLAN_STA_MAYBE_WPS))) {
+			if (pmlmepriv->wps_beacon_ie) {
+				u8 selected_registrar = 0;
+
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL);
+
+				if (!selected_registrar) {
+
+					status = _STATS_UNABLE_HANDLE_STA_;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+	} else {
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+		}
+
+		if (elems.wps_ie) {
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len = 0;
+		} else {
+			copy_len = ((wpa_ie_len + 2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len + 2);
+		}
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie - 2, copy_len);
+	}
+	/*  check if there is WMM IE & support WWM-PS */
+	pstat->flags &= ~WLAN_STA_WME;
+	pstat->qos_option = 0;
+	pstat->qos_info = 0;
+	pstat->has_legacy_ac = true;
+	pstat->uapsd_vo = 0;
+	pstat->uapsd_vi = 0;
+	pstat->uapsd_be = 0;
+	pstat->uapsd_bk = 0;
+	if (pmlmepriv->qospriv.qos_option) {
+		p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
+		for (;;) {
+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+			if (p) {
+				if (!memcmp(p + 2, WMM_IE, 6)) {
+					pstat->flags |= WLAN_STA_WME;
+
+					pstat->qos_option = 1;
+					pstat->qos_info = *(p + 8);
+
+					pstat->max_sp_len = (pstat->qos_info >> 5) & 0x3;
+
+					if ((pstat->qos_info & 0xf) != 0xf)
+						pstat->has_legacy_ac = true;
+					else
+						pstat->has_legacy_ac = false;
+
+					if (pstat->qos_info & 0xf) {
+						if (pstat->qos_info & BIT(0))
+							pstat->uapsd_vo = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_vo = 0;
+
+						if (pstat->qos_info & BIT(1))
+							pstat->uapsd_vi = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_vi = 0;
+
+						if (pstat->qos_info & BIT(2))
+							pstat->uapsd_bk = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_bk = 0;
+
+						if (pstat->qos_info & BIT(3))
+							pstat->uapsd_be = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_be = 0;
+					}
+					break;
+				}
+			} else {
+				break;
+			}
+			p = p + ie_len + 2;
+		}
+	}
+
+	/* save HT capabilities in the sta object */
+	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap)) {
+		pstat->flags |= WLAN_STA_HT;
+
+		pstat->flags |= WLAN_STA_WME;
+
+		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
+	} else {
+		pstat->flags &= ~WLAN_STA_HT;
+	}
+	if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags & WLAN_STA_HT)) {
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+	pstat->flags |= WLAN_STA_NONERP;
+	for (i = 0; i < pstat->bssratelen; i++) {
+		if ((pstat->bssrateset[i] & 0x7f) > 22) {
+			pstat->flags &= ~WLAN_STA_NONERP;
+			break;
+		}
+	}
+
+	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+	else
+		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+	pstat->is_p2p_device = false;
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset, NULL, &p2pielen);
+		if (p2pie) {
+			pstat->is_p2p_device = true;
+			p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
+			if (p2p_status_code > 0) {
+				pstat->p2p_status_code = p2p_status_code;
+				status = _STATS_CAP_FAIL_;
+				goto OnAssocReqFail;
+			}
+		}
+	}
+	pstat->p2p_status_code = p2p_status_code;
+
+	/* TODO: identify_proprietary_vendor_ie(); */
+	/*  Realtek proprietary IE */
+	/*  identify if this is Broadcom sta */
+	/*  identify if this is ralink sta */
+	/*  Customer proprietary IE */
+
+	/* get a unique AID */
+	if (pstat->aid == 0) {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+			if (!pstapriv->sta_aid[pstat->aid - 1])
+				break;
+
+		/* if (pstat->aid > NUM_STA) { */
+		if (pstat->aid > pstapriv->max_num_sta) {
+			pstat->aid = 0;
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+			goto OnAssocReqFail;
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+		}
+	}
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&pstat->auth_list)) {
+		list_del_init(&pstat->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	if (list_empty(&pstat->asoc_list)) {
+		pstat->expire_to = pstapriv->expire_to;
+		list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+		pstapriv->asoc_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	/*  now the station is qualified to join our BSS... */
+	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (status == _STATS_SUCCESSFUL_)) {
+		/* 1 bss_cap_update & sta_info_update */
+		bss_cap_update_on_sta_join(padapter, pstat);
+		sta_info_update(padapter, pstat);
+
+		/* issue assoc rsp before notify station join event. */
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+		/* 2 - report to upper layer */
+		rtw_indicate_sta_assoc_event(padapter, pstat);
+
+		/* 3-(1) report sta add event */
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+	return _FAIL;
+}
+
+unsigned int OnAssocRsp(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)precv_frame->rx_data;
+	uint i;
+	int res;
+	struct ndis_802_11_var_ie *pIE;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	/* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&padapter->eeprompriv), mgmt->da, ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	if (le16_to_cpu(mgmt->u.assoc_resp.status_code) > 0) {
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	pmlmeinfo->capability = le16_to_cpu(mgmt->u.assoc_resp.capab_info);
+
+	/* set slot time */
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+	pmlmeinfo->aid = le16_to_cpu(mgmt->u.assoc_resp.aid) & 0x3fff;
+	res = pmlmeinfo->aid;
+
+	/* following are moved to join event callback function */
+	/* to handle HT, WMM, rate adaptive, update MAC reg */
+	/* for not to handle the synchronous IO in the tasklet */
+	for (i = offsetof(struct ieee80211_mgmt, u.assoc_resp.variable); i < pkt_len;) {
+		pIE = (struct ndis_802_11_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6))	/* WMM */
+				WMM_param_handler(padapter, pIE);
+			break;
+		case _HT_CAPABILITY_IE_:	/* HT caps */
+			HT_caps_handler(padapter, pIE);
+			break;
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			HT_info_handler(padapter, pIE);
+			break;
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+	UpdateBrateTbl(pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+unsigned int OnDeAuth(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 *pframe = precv_frame->rx_data;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/* check A3 */
+	if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = 0;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (!list_empty(&psta->asoc_list)) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	} else {
+		int	ignore_received_deauth = 0;
+
+		/* Before sending the auth frame to start the STA/GC mode connection with AP/GO,
+		 *	we will send the deauth first.
+		 *	However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth.
+		 *	Added the following code to avoid this case.
+		 */
+		if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) ||
+		    (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)) {
+			if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA) {
+				ignore_received_deauth = 1;
+			} else if (reason == WLAN_REASON_PREV_AUTH_NOT_VALID) {
+				// TODO: 802.11r
+				ignore_received_deauth = 1;
+			}
+		}
+
+		if (!ignore_received_deauth)
+			receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+}
+
+unsigned int OnDisassoc(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	u16 reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 *pframe = precv_frame->rx_data;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/* check A3 */
+	if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = 0;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (!list_empty(&psta->asoc_list)) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	} else {
+		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+}
+
+unsigned int OnAction_back(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)precv_frame->rx_data;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char		*frame_body;
+	unsigned short	tid;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 *pframe = precv_frame->rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	/* check RA matches or not */
+	if (memcmp(myid(&padapter->eeprompriv), mgmt->da, ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, mgmt->sa);
+
+	if (!psta)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	if (!pmlmeinfo->HT_enable)
+		return _SUCCESS;
+	/* All union members start with an action code, it's ok to use addba_req. */
+	switch (mgmt->u.action.u.addba_req.action_code) {
+	case WLAN_ACTION_ADDBA_REQ:
+		memcpy(&pmlmeinfo->ADDBA_req, &frame_body[2], sizeof(struct ADDBA_request));
+		tid = u16_get_bits(le16_to_cpu(mgmt->u.action.u.addba_req.capab),
+				   IEEE80211_ADDBA_PARAM_TID_MASK);
+		preorder_ctrl = &psta->recvreorder_ctrl[tid];
+		preorder_ctrl->indicate_seq = 0xffff;
+		preorder_ctrl->enable = pmlmeinfo->bAcceptAddbaReq;
+
+		issue_action_BA(padapter, mgmt->sa, WLAN_ACTION_ADDBA_RESP,
+				pmlmeinfo->bAcceptAddbaReq ?
+					WLAN_STATUS_SUCCESS : WLAN_STATUS_REQUEST_DECLINED);
+		break;
+	case WLAN_ACTION_ADDBA_RESP:
+		tid = u16_get_bits(le16_to_cpu(mgmt->u.action.u.addba_resp.capab),
+				   IEEE80211_ADDBA_PARAM_TID_MASK);
+		if (mgmt->u.action.u.addba_resp.status == 0) {	/* successful */
+			psta->htpriv.agg_enable_bitmap |= BIT(tid);
+			psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+		} else {
+			psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+		}
+		break;
+	case WLAN_ACTION_DELBA:
+		tid = u16_get_bits(le16_to_cpu(mgmt->u.action.u.delba.params),
+				   IEEE80211_DELBA_PARAM_TID_MASK);
+		if (u16_get_bits(le16_to_cpu(mgmt->u.action.u.delba.params),
+				 IEEE80211_DELBA_PARAM_INITIATOR_MASK) == WLAN_BACK_RECIPIENT) {
+			psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+		} else {
+			preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+			preorder_ctrl->enable = false;
+			preorder_ctrl->indicate_seq = 0xffff;
+		}
+		/* todo: how to notify the host while receiving DELETE BA */
+		break;
+	default:
+		break;
+	}
+
+	return _SUCCESS;
+}
+
+static int get_reg_classes_full_count(struct p2p_channels *channel_list)
+{
+	int cnt = 0;
+	int i;
+
+	for (i = 0; i < channel_list->reg_classes; i++) {
+		cnt += channel_list->reg_class[i].channels;
+	}
+
+	return cnt;
+}
+
+void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_REQ;
+	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8 wpsielen = 0, p2pielen = 0;
+	u16 len_channellist_attr = 0;
+	struct xmit_frame *pmgntframe;
+	struct pkt_attrib *pattrib;
+	unsigned char *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pwdinfo->negotiation_dialog_token = 1;	/*	Initialize the dialog value */
+	pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &pattrib->pktlen);
+
+	/*	WPS Section */
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+
+	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110306 */
+	/*	According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. Group Owner Intent */
+	/*	3. Configuration Timeout */
+	/*	4. Listen Channel */
+	/*	5. Extended Listen Timing */
+	/*	6. Intended P2P Interface Address */
+	/*	7. Channel List */
+	/*	8. P2P Device Info */
+	/*	9. Operating Channel */
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	else
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+	/*	Group Owner Intent */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Todo the tie breaker bit. */
+	p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	/*	Listen Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listening channel number */
+
+	/*	Extended Listen Timing ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Intended P2P Interface Address */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/*  Length: */
+	/*  Country String(3) */
+	/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+	/*  + number of channels in all classes */
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
+	   + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_GO_response(struct adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_RESP;
+	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	uint			wpsielen = 0;
+	u16 wps_devicepassword_id = 0x0000;
+	__be16			be_tmp;
+	uint			wps_devicepassword_id_len = 0;
+	u16 len_channellist_attr = 0;
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pwdinfo->negotiation_dialog_token = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
+	pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &pattrib->pktlen);
+
+	/*	Commented by Albert 20110328 */
+	/*	Try to get the device password ID from the WPS IE of group negotiation request frame */
+	/*	WiFi Direct test plan 5.1.15 */
+	rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
+	rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
+	wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+	memset(wpsie, 0x00, 255);
+	wpsielen = 0;
+
+	/*	WPS Section */
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+	wpsielen += 2;
+
+	/*	Commented by Kurt 20120113 */
+	/*	If some device wants to do p2p handshake without sending prov_disc_req */
+	/*	We have to get peer_req_cm from here. */
+	if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
+		if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+		else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+		else
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20100908 */
+	/*	According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */
+	/*	1. Status */
+	/*	2. P2P Capability */
+	/*	3. Group Owner Intent */
+	/*	4. Configuration Timeout */
+	/*	5. Operating Channel */
+	/*	6. Intended P2P Interface Address */
+	/*	7. Channel List */
+	/*	8. Device Info */
+	/*	9. Group ID	(Only GO) */
+
+	/*	ToDo: */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		/*	Commented by Albert 2011/03/08 */
+		/*	According to the P2P specification */
+		/*	if the sending device will be client, the P2P Capability should be reserved of group negotiation response frame */
+		p2pie[p2pielen++] = 0;
+	} else {
+		/*	Be group owner or meet the error case */
+		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+	}
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported) {
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	} else {
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+	}
+
+	/*	Group Owner Intent */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	if (pwdinfo->peer_intent & 0x01) {
+		/*	Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
+		p2pie[p2pielen++] = (pwdinfo->intent << 1);
+	} else {
+		/*	Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
+		p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
+	}
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	/*	Intended P2P Interface Address */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/*  Country String(3) */
+	/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+	/*  + number of channels in all classes */
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+	   + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	Group ID Attribute */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+		p2pielen += pwdinfo->nego_ssidlen;
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_GO_confirm(struct adapter *padapter, u8 *raddr, u8 result)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_CONF;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110306 */
+	/*	According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */
+	/*	1. Status */
+	/*	2. P2P Capability */
+	/*	3. Operating Channel */
+	/*	4. Channel List */
+	/*	5. Group ID	(if this WiFi is GO) */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	else
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
+	} else {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+
+		/*	Channel Number */
+		p2pie[p2pielen++] = pwdinfo->operating_channel;		/*	Use the listen channel as the operating channel */
+	}
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(pwdinfo->channel_list_attr_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len);
+	p2pielen += pwdinfo->channel_list_attr_len;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	Group ID Attribute */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+		p2pielen += pwdinfo->nego_ssidlen;
+	}
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_p2p_invitation_request(struct adapter *padapter, u8 *raddr)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_INVIT_REQ;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	u8 dialogToken = 3;
+	u16 len_channellist_attr = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20101011 */
+	/*	According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
+	/*	1. Configuration Timeout */
+	/*	2. Invitation Flags */
+	/*	3. Operating Channel	(Only GO) */
+	/*	4. P2P Group BSSID	(Should be included if I am the GO) */
+	/*	5. Channel List */
+	/*	6. P2P Group ID */
+	/*	7. P2P Device Info */
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	/*	Invitation Flags */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;	/*	operating channel number */
+
+	if (!memcmp(myid(&padapter->eeprompriv), pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) {
+		/*	P2P Group BSSID */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	P2P Device Address for GO */
+		memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+	}
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/*	Length: */
+	/*  Country String(3) */
+	/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+	/*  + number of channels in all classes */
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+	   + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+
+	/*	P2P Group ID */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address for GO */
+	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	SSID */
+	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen);
+	p2pielen += pwdinfo->invitereq_info.ssidlen;
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen  += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_INVIT_RESP;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	u16 len_channellist_attr = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20101005 */
+	/*	According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */
+	/*	1. Status */
+	/*	2. Configuration Timeout */
+	/*	3. Operating Channel	(Only GO) */
+	/*	4. P2P Group BSSID	(Only GO) */
+	/*	5. Channel List */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16  *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
+	/*	Sent the event receiving the P2P Invitation Req frame to DMP UI. */
+	/*	DMP had to compare the MAC address to find out the profile. */
+	/*	So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
+	/*	If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */
+	/*	to NB to rebuild the persistent group. */
+	p2pie[p2pielen++] = status_code;
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	if (status_code == P2P_STATUS_SUCCESS) {
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			/*	The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */
+			/*	In this case, the P2P Invitation response frame should carry the two more P2P attributes. */
+			/*	First one is operating channel attribute. */
+			/*	Second one is P2P Group BSSID attribute. */
+
+			/*	Operating Channel */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	Country String */
+			p2pie[p2pielen++] = 'X';
+			p2pie[p2pielen++] = 'X';
+
+			/*	The third byte should be set to 0x04. */
+			/*	Described in the "Operating Channel Attribute" section. */
+			p2pie[p2pielen++] = 0x04;
+
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+			/*	Channel Number */
+			p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+			/*	P2P Group BSSID */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	P2P Device Address for GO */
+			memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+			p2pielen += ETH_ALEN;
+		}
+
+		/*	Channel List */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+		/*	Length: */
+		/*  Country String(3) */
+		/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+		/*  + number of channels in all classes */
+		len_channellist_attr = 3
+			+ (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+			+ get_reg_classes_full_count(&pmlmeext->channel_list);
+
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Country String */
+		p2pie[p2pielen++] = 'X';
+		p2pie[p2pielen++] = 'X';
+
+		/*	The third byte should be set to 0x04. */
+		/*	Described in the "Operating Channel Attribute" section. */
+		p2pie[p2pielen++] = 0x04;
+
+		/*	Channel Entry List */
+		{
+			int i, j;
+			for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+				/*	Operating Class */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+				/*	Number of Channels */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+				/*	Channel List */
+				for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+					p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+				}
+			}
+		}
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_p2p_provision_request(struct adapter *padapter, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
+{
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	u8 dialogToken = 1;
+	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
+	u8 wpsie[100] = { 0x00 };
+	u8 wpsielen = 0;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u32			p2pielen = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	p2pielen = build_prov_disc_request_p2p_ie(pwdinfo, pframe, pssid, ussidlen, pdev_raddr);
+
+	pframe += p2pielen;
+	pattrib->pktlen += p2pielen;
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Config Method */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo)
+{
+	u8 i, match_result = 0;
+
+	for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
+		if (!memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN)) {
+			match_result = 1;
+			break;
+		}
+	}
+	return match_result;
+}
+
+void issue_probersp_p2p(struct adapter *padapter, unsigned char *da)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned char					*mac;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	u16 beacon_interval = 100;
+	u16 capInfo = 0;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 wpsie[255] = { 0x00 };
+	u32					wpsielen = 0, p2pielen = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&padapter->eeprompriv);
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	/*	Use the device address for BSSID field. */
+	memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+	memcpy(pframe, (unsigned char *)&beacon_interval, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*	capability info: 2 bytes */
+	/*	ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */
+	capInfo |= cap_ShortPremble;
+	capInfo |= cap_ShortSlot;
+
+	memcpy(pframe, (unsigned char *)&capInfo, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
+
+	/*  supported rates... */
+	/*	Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
+
+	/*	Todo: WPS IE */
+	/*	Noted by Albert 20100907 */
+	/*	According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	WiFi Simple Config State */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;	/*	Not Configured. */
+
+	/*	Response Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
+
+	/*	UUID-E */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	wpsielen += 0x10;
+
+	/*	Manufacturer */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "Realtek", 7);
+	wpsielen += 7;
+
+	/*	Model Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "8188EU", 6);
+	wpsielen += 6;
+
+	/*	Model Number */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = 0x31;		/*	character 1 */
+
+	/*	Serial Number */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "123456", ETH_ALEN);
+	wpsielen += ETH_ALEN;
+
+	/*	Primary Device Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+	wpsielen += 2;
+
+	/*	Value: */
+	/*	Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	wpsielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	wpsielen += 2;
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+	wpsielen += 2;
+
+	/*	Value: */
+	if (pwdinfo->device_name_len) {
+		memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		wpsielen += pwdinfo->device_name_len;
+	}
+
+	/*	Config Method */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
+	pframe += p2pielen;
+	pattrib->pktlen += p2pielen;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static int _issue_probereq_p2p(struct adapter *padapter, u8 *da, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned char			*mac;
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u16 wpsielen = 0, p2pielen = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&padapter->eeprompriv);
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	if (da) {
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		if ((pwdinfo->p2p_info.scan_op_ch_only) || (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+			/*	This two flags will be set when this is only the P2P client mode. */
+			memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+		} else {
+			/*	broadcast probe request frame */
+			eth_broadcast_addr(pwlanhdr->addr1);
+			eth_broadcast_addr(pwlanhdr->addr3);
+		}
+	}
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &pattrib->pktlen);
+	else
+		pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
+
+	/*	Use the OFDM rate in the P2P probe request frame. (6(B), 9(B), 12(B), 24(B), 36, 48, 54) */
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+	/*	WPS IE */
+	/*	Noted by Albert 20110221 */
+	/*	According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	if (!pmlmepriv->wps_probe_req_ie) {
+		/*	UUID-E */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+		wpsielen += 0x10;
+
+		/*	Config Method */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+		wpsielen += 2;
+
+		/*	Value: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+		wpsielen += 2;
+	}
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	wpsielen += pwdinfo->device_name_len;
+
+	/*	Primary Device Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+	wpsielen += 2;
+
+	/*	Value: */
+	/*	Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
+	wpsielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
+	wpsielen += 2;
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);	/*	Registrar-specified */
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110221 */
+	/*	According to the P2P Specification, the probe request frame should contain 5 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. P2P Device ID if this probe request wants to find the specific P2P device */
+	/*	3. Listen Channel */
+	/*	4. Extended Listen Timing */
+	/*	5. Operating Channel if this WiFi is working as the group owner now */
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+	else
+		p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+	/*	Listen Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listen channel */
+
+	/*	Extended Listen Timing */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	Operating Channel (if this WiFi is working as the group owner now) */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Country String */
+		p2pie[p2pielen++] = 'X';
+		p2pie[p2pielen++] = 'X';
+
+		/*	The third byte should be set to 0x04. */
+		/*	Described in the "Operating Channel Attribute" section. */
+		p2pie[p2pielen++] = 0x04;
+
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+		/*	Channel Number */
+		p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	if (pmlmepriv->wps_probe_req_ie) {
+		/* WPS IE */
+		memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+		pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+		pframe += pmlmepriv->wps_probe_req_ie_len;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq_p2p(struct adapter *adapter, u8 *da)
+{
+	_issue_probereq_p2p(adapter, da, false);
+}
+
+static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
+{
+	struct adapter *adapter = recv_frame->adapter;
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	u8 *frame = recv_frame->rx_data;
+	u16 seq_ctrl = ((recv_frame->attrib.seq_num & 0xffff) << 4) |
+		(recv_frame->attrib.frag_num & 0xf);
+
+	if (GetRetry(frame)) {
+		if (token >= 0) {
+			if ((seq_ctrl == mlmeext->action_public_rxseq) &&
+			    (token == mlmeext->action_public_dialog_token))
+				return _FAIL;
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq)
+				return _FAIL;
+		}
+	}
+
+	mlmeext->action_public_rxseq = seq_ctrl;
+
+	if (token >= 0)
+		mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(struct recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body;
+	u8 dialogToken = 0;
+	struct adapter *padapter = precv_frame->adapter;
+	uint len = precv_frame->len;
+	u8 *p2p_ie;
+	u32	p2p_ielen;
+	struct	wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8	result = P2P_STATUS_SUCCESS;
+	u8	empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+
+	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+		return _FAIL;
+
+	_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+	/*	Do nothing if the driver doesn't enable the P2P function. */
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+		return _SUCCESS;
+
+	len -= sizeof(struct ieee80211_hdr_3addr);
+
+	switch (frame_body[6]) { /* OUI Subtype */
+	case P2P_GO_NEGO_REQ:
+		memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+			rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) {
+			/*	Commented by Albert 20110526 */
+			/*	In this case, this means the previous nego fail doesn't be reset yet. */
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			/*	Restore the previous p2p state */
+			rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+		}
+
+		/*	Commented by Kurt 20110902 */
+		/* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+			rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+		/*	Commented by Kurt 20120113 */
+		/*	Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
+		if (!memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN))
+			memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+		result = process_p2p_group_negotation_req(pwdinfo, frame_body, len);
+		issue_p2p_GO_response(padapter, GetAddr2Ptr(pframe), frame_body, len, result);
+
+		/*	Commented by Albert 20110718 */
+		/*	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
+		_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+		break;
+	case P2P_GO_NEGO_RESP:
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+			/*	Commented by Albert 20110425 */
+			/*	The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			pwdinfo->nego_req_info.benable = false;
+			result = process_p2p_group_negotation_resp(pwdinfo, frame_body, len);
+			issue_p2p_GO_confirm(pwdinfo->padapter, GetAddr2Ptr(pframe), result);
+			if (result == P2P_STATUS_SUCCESS) {
+				if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+					pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+					pwdinfo->p2p_info.scan_op_ch_only = 1;
+					_set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+				}
+			}
+			/*	Reset the dialog token for group negotiation frames. */
+			pwdinfo->negotiation_dialog_token = 1;
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+				_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+		}
+		break;
+	case P2P_GO_NEGO_CONF:
+		result = process_p2p_group_negotation_confirm(pwdinfo, frame_body, len);
+		if (result == P2P_STATUS_SUCCESS) {
+			if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+				pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+				pwdinfo->p2p_info.scan_op_ch_only = 1;
+				_set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+			}
+		}
+		break;
+	case P2P_INVIT_REQ:
+		/*	Added by Albert 2010/10/05 */
+		/*	Received the P2P Invite Request frame. */
+
+		p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+		if (p2p_ie) {
+			/*	Parse the necessary information from the P2P Invitation Request frame. */
+			/*	For example: The MAC address of sending this P2P Invitation Request frame. */
+			u32	attr_contentlen = 0;
+			u8	status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+			struct group_id_info group_id;
+			u8	invitation_flag = 0;
+
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
+			if (attr_contentlen) {
+				rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
+				/*	Commented by Albert 20120510 */
+				/*	Copy to the pwdinfo->p2p_peer_interface_addr. */
+				/*	So that the WFD UI (or Sigma) can get the peer interface address by using the following command. */
+				/*	#> iwpriv wlan0 p2p_get peer_ifa */
+				/*	After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
+
+				if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT) {
+					/*	Re-invoke the persistent group. */
+
+					memset(&group_id, 0x00, sizeof(struct group_id_info));
+					rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
+					if (attr_contentlen) {
+						if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+							/*	The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
+							rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+							status_code = P2P_STATUS_SUCCESS;
+						} else {
+							/*	The p2p device sending this p2p invitation request wants to be the persistent GO. */
+							if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[0])) {
+								u8 operatingch_info[5] = { 0x00 };
+								if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
+									if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4])) {
+										/*	The operating channel is acceptable for this device. */
+										pwdinfo->rx_invitereq_info.operation_ch[0] = operatingch_info[4];
+										pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
+										_set_timer(&pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH);
+										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+										rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+										status_code = P2P_STATUS_SUCCESS;
+									} else {
+										/*	The operating channel isn't supported by this device. */
+										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+										rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+										status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
+										_set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
+									}
+								} else {
+									/*	Commented by Albert 20121130 */
+									/*	Intel will use the different P2P IE to store the operating channel information */
+									/*	Workaround for Intel WiDi 3.5 */
+									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+									rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+									status_code = P2P_STATUS_SUCCESS;
+								}
+							} else {
+								rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+								status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+							}
+						}
+					} else {
+						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+					}
+				} else {
+					/*	Received the invitation to join a P2P group. */
+
+					memset(&group_id, 0x00, sizeof(struct group_id_info));
+					rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
+					if (attr_contentlen) {
+						if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+							/*	In this case, the GO can't be myself. */
+							rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+							status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+						} else {
+							/*	The p2p device sending this p2p invitation request wants to join an existing P2P group */
+							/*	Commented by Albert 2012/06/28 */
+							/*	In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
+							/*	The peer device address should be the destination address for the provisioning discovery request. */
+							/*	Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
+							/*	The peer interface address should be the address for WPS mac address */
+							memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr, ETH_ALEN);
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+							rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
+							status_code = P2P_STATUS_SUCCESS;
+						}
+					} else {
+						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+					}
+				}
+			} else {
+				status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+			}
+
+			pwdinfo->inviteresp_info.token = frame_body[7];
+			issue_p2p_invitation_response(padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code);
+		}
+		break;
+	case P2P_INVIT_RESP: {
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+
+		_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+		p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+		if (p2p_ie) {
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+
+			if (attr_contentlen == 1) {
+				pwdinfo->invitereq_info.benable = false;
+
+				if (attr_content == P2P_STATUS_SUCCESS) {
+					if (!memcmp(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv), ETH_ALEN)) {
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+					} else {
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+					}
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
+				} else {
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+				}
+			} else {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+			}
+		} else {
+			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+		}
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL))
+			_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+		break;
+	}
+	case P2P_DEVDISC_REQ:
+		process_p2p_devdisc_req(pwdinfo, pframe, len);
+		break;
+	case P2P_DEVDISC_RESP:
+		process_p2p_devdisc_resp(pwdinfo, pframe, len);
+		break;
+	case P2P_PROVISION_DISC_REQ:
+		process_p2p_provdisc_req(pwdinfo, pframe, len);
+		memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+		/* 20110902 Kurt */
+		/* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+			rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
+		_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+		break;
+	case P2P_PROVISION_DISC_RESP:
+		/*	Commented by Albert 20110707 */
+		/*	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
+		/*	Commented by Albert 20110426 */
+		/*	The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
+		_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
+		process_p2p_provdisc_resp(pwdinfo, pframe);
+		_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+		break;
+	}
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(struct recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+
+	if (!memcmp(frame_body + 2, P2P_OUI, 4)) {
+		ret = on_action_public_p2p(precv_frame);
+	}
+
+	return ret;
+}
+
+static unsigned int on_action_public_default(struct recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 token;
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&padapter->eeprompriv), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_p2p(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	u8 *frame_body;
+	u8 category, OUI_Subtype;
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+	struct	wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&padapter->eeprompriv), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_P2P)
+		return _SUCCESS;
+
+	if (be32_to_cpu(*((__be32 *)(frame_body + 1))) != P2POUI)
+		return _SUCCESS;
+
+	len -= sizeof(struct ieee80211_hdr_3addr);
+	OUI_Subtype = frame_body[5];
+
+	switch (OUI_Subtype) {
+	case P2P_NOTICE_OF_ABSENCE:
+		break;
+	case P2P_PRESENCE_REQUEST:
+		process_p2p_presence_req(pwdinfo, pframe, len);
+		break;
+	case P2P_PRESENCE_RESPONSE:
+		break;
+	case P2P_GO_DISC_REQUEST:
+		break;
+	default:
+		break;
+	}
+	return _SUCCESS;
+}
+
+unsigned int OnAction(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)precv_frame->rx_data;
+
+	switch (mgmt->u.action.category) {
+	case WLAN_CATEGORY_BACK:
+		OnAction_back(padapter, precv_frame);
+		break;
+	case WLAN_CATEGORY_PUBLIC:
+		on_action_public(padapter, precv_frame);
+		break;
+	case RTW_WLAN_CATEGORY_P2P:
+		OnAction_p2p(padapter, precv_frame);
+		break;
+	}
+	return _SUCCESS;
+}
+
+struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame			*pmgntframe;
+	struct xmit_buf				*pxmitbuf;
+
+	pmgntframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return NULL;
+
+	pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+	if (!pxmitbuf) {
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		return NULL;
+	}
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+	return pmgntframe;
+}
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	pmlmeext->tx_rate = rate;
+}
+
+void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+	pattrib->mac_id = 0;
+	pattrib->qsel = 0x12;
+
+	pattrib->pktlen = 0;
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		pattrib->raid = 6;/* b mode */
+	else
+		pattrib->raid = 5;/* a/g mode */
+
+	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->bswenc = false;
+
+	pattrib->qos_en = false;
+	pattrib->ht_en = false;
+	pattrib->bwmode = HT_CHANNEL_WIDTH_20;
+	pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pattrib->sgi = false;
+
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+
+	pattrib->retry_ctrl = true;
+}
+
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return;
+
+	rtl8188eu_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return ret;
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = rtl8188eu_mgnt_xmit(padapter, pmgntframe);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx);
+
+	return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;/*   500ms */
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return -1;
+
+	mutex_lock(&pxmitpriv->ack_tx_mutex);
+	pxmitpriv->ack_tx = true;
+
+	pmgntframe->ack_report = 1;
+	if (rtl8188eu_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
+		ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
+	}
+
+	pxmitpriv->ack_tx = false;
+	mutex_unlock(&pxmitpriv->ack_tx_mutex);
+
+	return ret;
+}
+
+static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+	u8 *ssid_ie;
+	int ssid_len_ori;
+	int len_diff = 0;
+
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	if (ssid_ie && ssid_len_ori > 0) {
+		switch (hidden_ssid_mode) {
+		case 1: {
+			u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
+			u32 remain_len = 0;
+
+			remain_len = ies_len - (next_ie - ies);
+
+			ssid_ie[1] = 0;
+			memcpy(ssid_ie + 2, next_ie, remain_len);
+			len_diff -= ssid_len_ori;
+
+			break;
+		}
+		case 2:
+			memset(&ssid_ie[2], 0, ssid_len_ori);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return len_diff;
+}
+
+void issue_beacon(struct adapter *padapter, int timeout_ms)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char	*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned int	rate_len;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex		*cur_network = &pmlmeinfo->network;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	eth_broadcast_addr(pwlanhdr->addr1);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		/*  for P2P : Primary Device Type & Device Name */
+		u32 wpsielen = 0, insert_len = 0;
+		u8 *wpsie = NULL;
+		wpsie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wpsielen);
+
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen > 0) {
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie, *pframe_wscie;
+
+			wps_offset = (uint)(wpsie - cur_network->IEs);
+			premainder_ie = wpsie + wpsielen;
+			remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
+			pframe_wscie = pframe + wps_offset;
+			memcpy(pframe, cur_network->IEs, wps_offset + wpsielen);
+			pframe += (wps_offset + wpsielen);
+			pattrib->pktlen += (wps_offset + wpsielen);
+
+			/* now pframe is end of wsc ie, insert Primary Device Type & Device Name */
+			/*	Primary Device Type */
+			/*	Type: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+			insert_len += 2;
+
+			/*	Length: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(0x0008);
+			insert_len += 2;
+
+			/*	Value: */
+			/*	Category ID */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+			insert_len += 2;
+
+			/*	OUI */
+			*(__be32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI);
+			insert_len += 4;
+
+			/*	Sub Category ID */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+			insert_len += 2;
+
+			/*	Device Name */
+			/*	Type: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+			insert_len += 2;
+
+			/*	Length: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len);
+			insert_len += 2;
+
+			/*	Value: */
+			memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len);
+			insert_len += pwdinfo->device_name_len;
+
+			/* update wsc ie length */
+			*(pframe_wscie + 1) = (wpsielen - 2) + insert_len;
+
+			/* pframe move to end */
+			pframe += insert_len;
+			pattrib->pktlen += insert_len;
+
+			/* copy remainder_ie to pframe */
+			memcpy(pframe, premainder_ie, remainder_ielen);
+			pframe += remainder_ielen;
+			pattrib->pktlen += remainder_ielen;
+		} else {
+			int len_diff;
+			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			len_diff = update_hidden_ssid(
+				pframe + _BEACON_IE_OFFSET_
+				, cur_network->IELength - _BEACON_IE_OFFSET_
+				, pmlmeinfo->hidden_ssid_mode
+			);
+			pframe += (cur_network->IELength + len_diff);
+			pattrib->pktlen += (cur_network->IELength + len_diff);
+		}
+
+		{
+			u8 *wps_ie;
+			uint wps_ielen;
+			u8 sr = 0;
+			wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr + TXDESC_OFFSET + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_,
+				pattrib->pktlen - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_, NULL, &wps_ielen);
+			if (wps_ie && wps_ielen > 0)
+				rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+			if (sr != 0)
+				set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			else
+				_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
+		}
+
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			u32 len;
+			len = build_beacon_p2p_ie(pwdinfo, pframe);
+
+			pframe += len;
+			pattrib->pktlen += len;
+		}
+
+		goto _issue_bcn;
+	}
+
+	/* below for ad-hoc mode */
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  capability info: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+	/*  supported rates... */
+	rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pattrib->pktlen);
+
+	{
+		u8 erpinfo = 0;
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+		/* ERP IE */
+		pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+	}
+
+	/*  EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+	/* todo:HT for adhoc */
+_issue_bcn:
+
+	pmlmepriv->update_bcn = false;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512)
+		return;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (timeout_ms > 0)
+		dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+	else
+		dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned char					*mac, *bssid;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex		*cur_network = &pmlmeinfo->network;
+	unsigned int	rate_len;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&padapter->eeprompriv);
+	bssid = cur_network->MacAddress;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		pwps_ie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+		/* inerset & update wps_probe_resp_ie */
+		if (pmlmepriv->wps_probe_resp_ie && pwps_ie && wps_ielen > 0) {
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie;
+
+			wps_offset = (uint)(pwps_ie - cur_network->IEs);
+
+			premainder_ie = pwps_ie + wps_ielen;
+
+			remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
+
+			memcpy(pframe, cur_network->IEs, wps_offset);
+			pframe += wps_offset;
+			pattrib->pktlen += wps_offset;
+
+			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
+			if ((wps_offset + wps_ielen + 2) <= MAX_IE_SZ) {
+				memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen + 2);
+				pframe += wps_ielen + 2;
+				pattrib->pktlen += wps_ielen + 2;
+			}
+
+			if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
+				memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;
+				pattrib->pktlen += remainder_ielen;
+			}
+		} else {
+			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			pframe += cur_network->IELength;
+			pattrib->pktlen += cur_network->IELength;
+		}
+	} else {
+		/* timestamp will be inserted by hardware */
+		pframe += 8;
+		pattrib->pktlen += 8;
+
+		/*  beacon interval: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/*  capability info: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/* below for ad-hoc mode */
+
+		/*  SSID */
+		pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+		/*  supported rates... */
+		rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+		/*  DS parameter set */
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pattrib->pktlen);
+
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+			u8 erpinfo = 0;
+			u32 ATIMWindow;
+			/*  IBSS Parameter Set... */
+			/* ATIMWindow = cur->Configuration.ATIMWindow; */
+			ATIMWindow = 0;
+			pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+			/* ERP IE */
+			pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+		}
+
+		/*  EXTERNDED SUPPORTED RATE */
+		if (rate_len > 8)
+			pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+		/* todo:HT for adhoc */
+	}
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
+		u32 len;
+		len = build_probe_resp_p2p_ie(pwdinfo, pframe);
+
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned char			*mac;
+	unsigned char			bssrate[NumRates];
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	int	bssrate_len = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&padapter->eeprompriv);
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	if (da) {
+		/*	unicast probe request frame */
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		/*	broadcast probe request frame */
+		eth_broadcast_addr(pwlanhdr->addr1);
+		eth_broadcast_addr(pwlanhdr->addr3);
+	}
+
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if (pssid)
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &pattrib->pktlen);
+	else
+		pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &pattrib->pktlen);
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &pattrib->pktlen);
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, bssrate_len - 8, bssrate + 8, &pattrib->pktlen);
+	} else {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &pattrib->pktlen);
+	}
+
+	/* add wps_ie for wps2.0 */
+	if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
+		memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+		pframe += pmlmepriv->wps_probe_req_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, false);
+}
+
+int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da,
+	int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_probereq(padapter, pssid, da, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+/*  if psta == NULL, indiate we are station(client) now... */
+void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+	struct xmit_frame *pmgntframe;
+	struct pkt_attrib *pattrib;
+	unsigned char *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned int val32;
+	u16 val16;
+	__le16 le_val16;
+	int use_shared_key = 0;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_AUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if (psta) {/*  for AP mode */
+		memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (u16)psta->authalg;
+
+		if (status != _STATS_SUCCESSFUL_)
+			val16 = 0;
+
+		if (val16) {
+			le_val16 = cpu_to_le16(val16);
+			use_shared_key = 1;
+		} else {
+			le_val16 = 0;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_val16, &pattrib->pktlen);
+
+		/*  setting auth seq number */
+		val16 = (u16)psta->auth_seq;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &pattrib->pktlen);
+
+		/*  setting status code... */
+		val16 = status;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_val16, &pattrib->pktlen);
+
+		/*  added challenging text... */
+		if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &pattrib->pktlen);
+	} else {
+		__le32 le_tmp32;
+		__le16 le_tmp16;
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/*  0:OPEN System, 1:Shared key */
+		if (val16)
+			use_shared_key = 1;
+
+		/* setting IV for auth seq #3 */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			le_tmp32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &pattrib->pktlen);
+
+			pattrib->iv_len = 4;
+		}
+
+		le_tmp16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp16, &pattrib->pktlen);
+
+		/*  setting auth seq number */
+		val16 = pmlmeinfo->auth_seq;
+		le_tmp16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp16, &pattrib->pktlen);
+
+		/*  setting status code... */
+		le_tmp16 = cpu_to_le16(status);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp16, &pattrib->pktlen);
+
+		/*  then checking to see if sending challenging text... */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &pattrib->pktlen);
+
+			SetPrivacy(fctrl);
+
+			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+
+			pattrib->encrypt = _WEP40_;
+
+			pattrib->icv_len = 4;
+
+			pattrib->pktlen += pattrib->icv_len;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	rtw_wep_encrypt(padapter, pmgntframe);
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+	struct xmit_frame	*pmgntframe;
+	struct ieee80211_hdr *pwlanhdr;
+	struct pkt_attrib *pattrib;
+	unsigned char	*pbuf, *pframe;
+	unsigned short val;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+	u8 *ie = pnetwork->IEs;
+	__le16 lestatus, leval;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+		SetFrameSubType(pwlanhdr, pkt_type);
+	else
+		return;
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen += pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* capability */
+	val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+	pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_, (unsigned char *)&val, &pattrib->pktlen);
+
+	lestatus = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&lestatus, &pattrib->pktlen);
+
+	leval = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_, (unsigned char *)&leval, &pattrib->pktlen);
+
+	if (pstat->bssratelen <= 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &pattrib->pktlen);
+	} else {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &pattrib->pktlen);
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, pstat->bssratelen - 8, pstat->bssrateset + 8, &pattrib->pktlen);
+	}
+
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+		uint ie_len = 0;
+
+		/* FILL HT CAP INFO IE */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len + 2);
+			pframe += (ie_len + 2);
+			pattrib->pktlen += (ie_len + 2);
+		}
+
+		/* FILL HT ADD INFO IE */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len + 2);
+			pframe += (ie_len + 2);
+			pattrib->pktlen += (ie_len + 2);
+		}
+	}
+
+	/* FILL WMM IE */
+	if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) {
+		uint ie_len = 0;
+		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+		for (pbuf = ie + _BEACON_IE_OFFSET_;; pbuf += (ie_len + 2)) {
+			pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+			if (pbuf && !memcmp(pbuf + 2, WMM_PARA_IE, 6)) {
+				memcpy(pframe, pbuf, ie_len + 2);
+				pframe += (ie_len + 2);
+				pattrib->pktlen += (ie_len + 2);
+				break;
+			}
+
+			if (!pbuf || ie_len == 0)
+				break;
+		}
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6, REALTEK_96B_IE, &pattrib->pktlen);
+
+	/* add WPS IE ie for wps 2.0 */
+	if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) {
+		memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
+
+		pframe += pmlmepriv->wps_assoc_resp_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
+	}
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device)) {
+		u32 len;
+
+		len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
+
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_assocreq(struct adapter *padapter)
+{
+	int ret = _FAIL;
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char		*pframe, *p;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	__le16		le_tmp;
+	unsigned int	i, j, ie_len, index = 0;
+	unsigned char bssrate[NumRates], sta_bssrate[NumRates];
+	struct ndis_802_11_var_ie *pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	int	bssrate_len = 0, sta_bssrate_len = 0;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 p2pie[255] = { 0x00 };
+	u16 p2pielen = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ASSOCREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* caps */
+
+	memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* listen interval */
+	/* todo: listen interval for power saving */
+	le_tmp = cpu_to_le16(3);
+	memcpy(pframe, (unsigned char *)&le_tmp, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &pattrib->pktlen);
+
+	/* supported rate & extended supported rate */
+
+	/*  Check if the AP's supported rates are also supported by STA. */
+	get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+
+	if (pmlmeext->cur_channel == 14)/*  for JAPAN, channel 14 can only uses B Mode(CCK) */
+		sta_bssrate_len = 4;
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+
+		/*  Check if the AP's supported rates are also supported by STA. */
+		for (j = 0; j < sta_bssrate_len; j++) {
+			 /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
+			if ((pmlmeinfo->network.SupportedRates[i] | IEEE80211_BASIC_RATE_MASK)
+					== (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK))
+				break;
+		}
+
+		if (j != sta_bssrate_len)
+			/*  the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+	}
+
+	bssrate_len = index;
+
+	if (bssrate_len == 0) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit; /* don't connect to AP if no joint supported rate */
+	}
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &pattrib->pktlen);
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, bssrate_len - 8, bssrate + 8, &pattrib->pktlen);
+	} else {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &pattrib->pktlen);
+	}
+
+	/* RSN */
+	p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+	if (p)
+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, p + 2, &pattrib->pktlen);
+
+	/* HT caps */
+	if (padapter->mlmepriv.htpriv.ht_option) {
+		p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+		if (p && !is_ap_in_tkip(padapter)) {
+			memcpy(&pmlmeinfo->HT_caps, p + 2, sizeof(struct HT_caps_element));
+
+			/* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
+			if (pregpriv->cbw40_enable == 0)
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &= cpu_to_le16(~(BIT(6) | BIT(1)));
+			else
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(BIT(1));
+
+			/* todo: disable SM power save mode */
+			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x000c);
+
+			if (pregpriv->rx_stbc)
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
+			memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
+
+			pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len, (u8 *)(&pmlmeinfo->HT_caps), &pattrib->pktlen);
+		}
+	}
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+		pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
+			    (!memcmp(pIE->data, WMM_OUI, 4)) ||
+			    (!memcmp(pIE->data, WPS_OUI, 4))) {
+				if (!padapter->registrypriv.wifi_spec) {
+					/* Commented by Kurt 20110629 */
+					/* In some older APs, WPS handshake */
+					/* would be fail if we append vender extensions informations to AP */
+					if (!memcmp(pIE->data, WPS_OUI, 4))
+						pIE->Length = 14;
+				}
+				pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &pattrib->pktlen);
+			}
+			break;
+		default:
+			break;
+		}
+		i += (pIE->Length + 2);
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6, REALTEK_96B_IE, &pattrib->pktlen);
+
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
+		/*	Should add the P2P IE in the association request frame. */
+		/*	P2P OUI */
+
+		p2pielen = 0;
+		p2pie[p2pielen++] = 0x50;
+		p2pie[p2pielen++] = 0x6F;
+		p2pie[p2pielen++] = 0x9A;
+		p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+		/*	Commented by Albert 20101109 */
+		/*	According to the P2P Specification, the association request frame should contain 3 P2P attributes */
+		/*	1. P2P Capability */
+		/*	2. Extended Listen Timing */
+		/*	3. Device Info */
+		/*	Commented by Albert 20110516 */
+		/*	4. P2P Interface */
+
+		/*	P2P Capability */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Device Capability Bitmap, 1 byte */
+		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+		/*	Group Capability Bitmap, 1 byte */
+		if (pwdinfo->persistent_supported)
+			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+		/*	Extended Listen Timing */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Availability Period */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+		p2pielen += 2;
+
+		/*	Availability Interval */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+		p2pielen += 2;
+
+		/*	Device Info */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+		/*	Length: */
+		/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+		/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	P2P Device Address */
+		memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	Config Method */
+		/*	This field should be big endian. Noted by P2P specification. */
+		if ((pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
+		    (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+		else
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC);
+
+		p2pielen += 2;
+
+		/*	Primary Device Type */
+		/*	Category ID */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+		p2pielen += 2;
+
+		/*	OUI */
+		*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+		p2pielen += 4;
+
+		/*	Sub Category ID */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+		p2pielen += 2;
+
+		/*	Number of Secondary Device Types */
+		p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+		/*	Device Name */
+		/*	Type: */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+		p2pielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+		p2pielen += 2;
+
+		/*	Value: */
+		memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		p2pielen += pwdinfo->device_name_len;
+
+		/*	P2P Interface */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D);
+		p2pielen += 2;
+
+		/*	Value: */
+		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);	/*	P2P Device Address */
+		p2pielen += ETH_ALEN;
+
+		p2pie[p2pielen++] = 1;	/*	P2P Interface Address Count */
+
+		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);	/*	P2P Interface Address List */
+		p2pielen += ETH_ALEN;
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+
+exit:
+	if (ret == _SUCCESS)
+		rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+	else
+		kfree(pmlmepriv->assoc_req);
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv	*pxmitpriv;
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	if (!padapter)
+		goto exit;
+
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (power_mode)
+		SetPwrMgt(fctrl);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/* when wait_ms > 0 , this function shoule be called at process context */
+/* da == NULL for station mode */
+int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (!da)
+		da = get_my_bssid(&pmlmeinfo->network);
+
+	do {
+		ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned short *qc;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	pattrib->hdrlen += 2;
+	pattrib->qos_en = true;
+	pattrib->eosp = 1;
+	pattrib->ack_policy = 0;
+	pattrib->mdata = 0;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+
+	SetPriority(qc, tid);
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_qos_hdr);
+	pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/* when wait_ms > 0 , this function shoule be called at process context */
+/* da == NULL for station mode */
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (!da)
+		da = get_my_bssid(&pmlmeinfo->network);
+
+	do {
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+static int _issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	int ret = _FAIL;
+	__le16 le_tmp;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DEAUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	le_tmp = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_, (unsigned char *)&le_tmp, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	return _issue_deauth(padapter, da, reason, false);
+}
+
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+	int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_deauth(padapter, da, reason, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, u8 action, u16 status)
+{
+	u16 start_seq;
+	u16 BA_starting_seqctrl = 0;
+	struct xmit_frame *pmgntframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct ieee80211_mgmt *mgmt;
+	u16 capab, params;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
+
+	mgmt->frame_control = cpu_to_le16(IEEE80211_STYPE_ACTION | IEEE80211_FTYPE_MGMT);
+
+	memcpy(mgmt->da, raddr, ETH_ALEN);
+	memcpy(mgmt->sa, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(mgmt->bssid, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	mgmt->seq_ctrl = cpu_to_le16(pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+
+	mgmt->u.action.category = WLAN_CATEGORY_BACK;
+
+	switch (action) {
+	case WLAN_ACTION_ADDBA_REQ:
+		mgmt->u.action.u.addba_req.action_code = WLAN_ACTION_ADDBA_REQ;
+		do {
+			pmlmeinfo->dialogToken++;
+		} while (pmlmeinfo->dialogToken == 0);
+		mgmt->u.action.u.addba_req.dialog_token = pmlmeinfo->dialogToken;
+
+		/* immediate ack & 64 buffer size */
+		capab = u16_encode_bits(64, IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK);
+		capab |= u16_encode_bits(1, IEEE80211_ADDBA_PARAM_POLICY_MASK);
+		capab |= u16_encode_bits(status, IEEE80211_ADDBA_PARAM_TID_MASK);
+		mgmt->u.action.u.addba_req.capab = cpu_to_le16(capab);
+
+		mgmt->u.action.u.addba_req.timeout = cpu_to_le16(5000); /* 5 ms */
+
+		psta = rtw_get_stainfo(pstapriv, raddr);
+		if (psta) {
+			start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07] & 0xfff) + 1;
+
+			psta->BA_starting_seqctrl[status & 0x07] = start_seq;
+
+			BA_starting_seqctrl = start_seq << 4;
+		}
+		mgmt->u.action.u.addba_req.start_seq_num = cpu_to_le16(BA_starting_seqctrl);
+
+		pattrib->pktlen = offsetofend(struct ieee80211_mgmt,
+					      u.action.u.addba_req.start_seq_num);
+		break;
+	case WLAN_ACTION_ADDBA_RESP:
+		mgmt->u.action.u.addba_resp.action_code = WLAN_ACTION_ADDBA_RESP;
+		mgmt->u.action.u.addba_resp.dialog_token = pmlmeinfo->ADDBA_req.dialog_token;
+		mgmt->u.action.u.addba_resp.status = cpu_to_le16(status);
+		capab = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f;
+		capab |= u16_encode_bits(64, IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK);
+		capab |= u16_encode_bits(pregpriv->ampdu_amsdu, IEEE80211_ADDBA_PARAM_AMSDU_MASK);
+		mgmt->u.action.u.addba_req.capab = cpu_to_le16(capab);
+		mgmt->u.action.u.addba_resp.timeout = pmlmeinfo->ADDBA_req.BA_timeout_value;
+		pattrib->pktlen = offsetofend(struct ieee80211_mgmt, u.action.u.addba_resp.timeout);
+		break;
+	case WLAN_ACTION_DELBA:
+		mgmt->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
+		mgmt->u.action.u.delba.params = cpu_to_le16((status & 0x1F) << 3);
+		params = u16_encode_bits((status & 0x1), IEEE80211_DELBA_PARAM_INITIATOR_MASK);
+		params |= u16_encode_bits((status >> 1) & 0xF, IEEE80211_DELBA_PARAM_TID_MASK);
+		mgmt->u.action.u.delba.params = cpu_to_le16(params);
+		mgmt->u.action.u.delba.reason_code = cpu_to_le16(WLAN_STATUS_REQUEST_DECLINED);
+		pattrib->pktlen = offsetofend(struct ieee80211_mgmt, u.action.u.delba.reason_code);
+		break;
+	default:
+		break;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_action_BSSCoexistPacket(struct adapter *padapter)
+{
+	struct list_head *plist, *phead;
+	unsigned char category, action;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char				*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+		return;
+
+	if (pmlmeinfo->bwmode_updated)
+		return;
+
+	category = WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+
+	/*  */
+	if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+		u8 iedata = 0;
+
+		iedata |= BIT(2);/* 20 MHz BSS Width Request */
+
+		pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &pattrib->pktlen);
+	}
+
+	/*  */
+	memset(ICS, 0, sizeof(ICS));
+	if (pmlmepriv->num_sta_no_ht > 0) {
+		int i;
+
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+		phead = get_list_head(queue);
+		plist = phead->next;
+
+		while (phead != plist) {
+			int len;
+			u8 *p;
+			struct wlan_bssid_ex *pbss_network;
+
+			pnetwork = container_of(plist, struct wlan_network, list);
+
+			plist = plist->next;
+
+			pbss_network = (struct wlan_bssid_ex *)&pnetwork->network;
+
+			p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+			if (!p || len == 0) { /* non-HT */
+				if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
+					continue;
+
+				ICS[0][pbss_network->Configuration.DSConfig] = 1;
+
+				if (ICS[0][0] == 0)
+					ICS[0][0] = 1;
+			}
+		}
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+		for (i = 0; i < 8; i++) {
+			if (ICS[i][0] == 1) {
+				int j, k = 0;
+
+				InfoContent[k] = i;
+				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+				k++;
+
+				for (j = 1; j <= 14; j++) {
+					if (ICS[i][j] == 1) {
+						if (k < 16) {
+							InfoContent[k] = j; /* channel number */
+							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
+							k++;
+						}
+					}
+				}
+
+				pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &pattrib->pktlen);
+			}
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+	/* struct recv_reorder_ctrl *preorder_ctrl; */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u16 tid;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if (!psta)
+		return _SUCCESS;
+
+	if (initiator == 0) { /*  recipient */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->recvreorder_ctrl[tid].enable) {
+				issue_action_BA(padapter, addr, WLAN_ACTION_DELBA, (((tid << 1) | initiator) & 0x1F));
+				psta->recvreorder_ctrl[tid].enable = false;
+				psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+			}
+		}
+	} else if (initiator == 1) { /*  originator */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+				issue_action_BA(padapter, addr, WLAN_ACTION_DELBA, (((tid << 1) | initiator) & 0x1F));
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			}
+		}
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int send_beacon(struct adapter *padapter)
+{
+	bool bxmitok = false;
+	int	issue = 0;
+	int poll = 0;
+
+	clear_beacon_valid_bit(padapter);
+
+	do {
+		issue_beacon(padapter, 100);
+		issue++;
+		do {
+			yield();
+			bxmitok = get_beacon_valid_bit(padapter);
+			poll++;
+		} while ((poll % 10) != 0 && !bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+	} while (!bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped || !bxmitok)
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+bool get_beacon_valid_bit(struct adapter *adapter)
+{
+	int res;
+	u8 reg;
+
+	res = rtw_read8(adapter, REG_TDECTRL + 2, &reg);
+	if (res)
+		return false;
+
+	/* BIT(16) of REG_TDECTRL = BIT(0) of REG_TDECTRL+2 */
+	return BIT(0) & reg;
+}
+
+void clear_beacon_valid_bit(struct adapter *adapter)
+{
+	int res;
+	u8 reg;
+
+	res = rtw_read8(adapter, REG_TDECTRL + 2, &reg);
+	if (res)
+		return;
+
+	/* BIT(16) of REG_TDECTRL = BIT(0) of REG_TDECTRL+2, write 1 to clear, Clear by sw */
+	rtw_write8(adapter, REG_TDECTRL + 2, reg | BIT(0));
+}
+
+void rtw_resume_tx_beacon(struct adapter *adapt)
+{
+	struct hal_data_8188e *haldata = &adapt->haldata;
+
+	/*  2010.03.01. Marked by tynli. No need to call workitem beacause we record the value */
+	/*  which should be read from register to a global variable. */
+
+	rtw_write8(adapt, REG_FWHW_TXQ_CTRL + 2, (haldata->RegFwHwTxQCtrl) | BIT(6));
+	haldata->RegFwHwTxQCtrl |= BIT(6);
+	rtw_write8(adapt, REG_TBTT_PROHIBIT + 1, 0xff);
+	haldata->RegReg542 |= BIT(0);
+	rtw_write8(adapt, REG_TBTT_PROHIBIT + 2, haldata->RegReg542);
+}
+
+void rtw_stop_tx_beacon(struct adapter *adapt)
+{
+	struct hal_data_8188e *haldata = &adapt->haldata;
+
+	/*  2010.03.01. Marked by tynli. No need to call workitem beacause we record the value */
+	/*  which should be read from register to a global variable. */
+
+	rtw_write8(adapt, REG_FWHW_TXQ_CTRL + 2, (haldata->RegFwHwTxQCtrl) & (~BIT(6)));
+	haldata->RegFwHwTxQCtrl &= (~BIT(6));
+	rtw_write8(adapt, REG_TBTT_PROHIBIT + 1, 0x64);
+	haldata->RegReg542 &= ~(BIT(0));
+	rtw_write8(adapt, REG_TBTT_PROHIBIT + 2, haldata->RegReg542);
+
+	 /* todo: CheckFwRsvdPageContent(Adapter);  2010.06.23. Added by tynli. */
+}
+
+static void rtw_set_opmode(struct adapter *adapter, u8 mode)
+{
+	u8 val8;
+	int res;
+
+	/*  disable Port0 TSF update */
+	res = rtw_read8(adapter, REG_BCN_CTRL, &val8);
+	if (res)
+		return;
+
+	rtw_write8(adapter, REG_BCN_CTRL, val8 | BIT(4));
+
+	/*  set net_type */
+	res = rtw_read8(adapter, MSR, &val8);
+	if (res)
+		return;
+
+	val8 &= 0x0c;
+	val8 |= mode;
+	rtw_write8(adapter, MSR, val8);
+
+	if ((mode == _HW_STATE_STATION_) || (mode == _HW_STATE_NOLINK_)) {
+		rtw_stop_tx_beacon(adapter);
+
+		rtw_write8(adapter, REG_BCN_CTRL, 0x19);/* disable atim wnd */
+	} else if (mode == _HW_STATE_ADHOC_) {
+		rtw_resume_tx_beacon(adapter);
+		rtw_write8(adapter, REG_BCN_CTRL, 0x1a);
+	} else if (mode == _HW_STATE_AP_) {
+		rtw_resume_tx_beacon(adapter);
+
+		rtw_write8(adapter, REG_BCN_CTRL, 0x12);
+
+		/* Set RCR */
+		rtw_write32(adapter, REG_RCR, 0x7000208e);/* CBSSID_DATA must set to 0,reject ICV_ERR packet */
+		/* enable to rx data frame */
+		rtw_write16(adapter, REG_RXFLTMAP2, 0xFFFF);
+		/* enable to rx ps-poll */
+		rtw_write16(adapter, REG_RXFLTMAP1, 0x0400);
+
+		/* Beacon Control related register for first time */
+		rtw_write8(adapter, REG_BCNDMATIM, 0x02); /*  2ms */
+
+		rtw_write8(adapter, REG_ATIMWND, 0x0a); /*  10ms */
+		rtw_write16(adapter, REG_BCNTCFG, 0x00);
+		rtw_write16(adapter, REG_TBTT_PROHIBIT, 0xff04);
+		rtw_write16(adapter, REG_TSFTR_SYN_OFFSET, 0x7fff);/*  +32767 (~32ms) */
+
+		/* reset TSF */
+		rtw_write8(adapter, REG_DUAL_TSF_RST, BIT(0));
+
+		/* BIT(3) - If set 0, hw will clr bcnq when tx becon ok/fail or port 0 */
+		res = rtw_read8(adapter, REG_MBID_NUM, &val8);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_MBID_NUM, val8 | BIT(3) | BIT(4));
+
+		/* enable BCN0 Function for if1 */
+		/* don't enable update TSF0 for if1 (due to TSF update when beacon/probe rsp are received) */
+		rtw_write8(adapter, REG_BCN_CTRL, (DIS_TSF_UDT0_NORMAL_CHIP | EN_BCN_FUNCTION | BIT(1)));
+
+		/* dis BCN1 ATIM  WND if if2 is station */
+		res = rtw_read8(adapter, REG_BCN_CTRL_1, &val8);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_BCN_CTRL_1, val8 | BIT(0));
+	}
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+static void rtw_set_initial_gain(struct adapter *adapter, u8 gain)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+	struct rtw_dig *digtable = &odmpriv->DM_DigTable;
+
+	if (gain == 0xff) {
+		/* restore rx gain */
+		ODM_Write_DIG(odmpriv, digtable->BackupIGValue);
+	} else {
+		digtable->BackupIGValue = digtable->CurIGValue;
+		ODM_Write_DIG(odmpriv, gain);
+	}
+}
+
+void rtw_mlme_under_site_survey(struct adapter *adapter)
+{
+	/* config RCR to receive different BSSID & not to receive data frame */
+
+	int res;
+	u8 reg;
+	u32 v;
+
+	res = rtw_read32(adapter, REG_RCR, &v);
+	if (res)
+		return;
+
+	v &= ~(RCR_CBSSID_BCN);
+	rtw_write32(adapter, REG_RCR, v);
+	/* reject all data frame */
+	rtw_write16(adapter, REG_RXFLTMAP2, 0x00);
+
+	/* disable update TSF */
+	res = rtw_read8(adapter, REG_BCN_CTRL, &reg);
+	if (res)
+		return;
+
+	rtw_write8(adapter, REG_BCN_CTRL, reg | BIT(4));
+}
+
+void rtw_mlme_site_survey_done(struct adapter *adapter)
+{
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u32 reg32;
+	int res;
+	u8 reg;
+
+	if ((is_client_associated_to_ap(adapter)) ||
+	    ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) {
+		/* enable to rx data frame */
+		rtw_write16(adapter, REG_RXFLTMAP2, 0xFFFF);
+
+		/* enable update TSF */
+		res = rtw_read8(adapter, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_BCN_CTRL, reg & (~BIT(4)));
+	} else if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		rtw_write16(adapter, REG_RXFLTMAP2, 0xFFFF);
+		/* enable update TSF */
+		res = rtw_read8(adapter, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_BCN_CTRL, reg & (~BIT(4)));
+	}
+
+	res = rtw_read32(adapter, REG_RCR, &reg32);
+	if (res)
+		return;
+
+	rtw_write32(adapter, REG_RCR, reg32 | RCR_CBSSID_BCN);
+}
+
+void site_survey(struct adapter *padapter)
+{
+	unsigned char survey_channel = 0;
+	enum rt_scan_type ScanType = SCAN_PASSIVE;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
+		if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+			survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+		} else {
+			survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+		}
+		ScanType = SCAN_ACTIVE;
+	} else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
+		/*	Commented by Albert 2011/06/03 */
+		/*	The driver is in the find phase, it should go through the social channel. */
+		int ch_set_idx;
+		survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
+		ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
+		if (ch_set_idx >= 0)
+			ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
+		else
+			ScanType = SCAN_ACTIVE;
+	} else {
+		struct rtw_ieee80211_channel *ch;
+		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
+			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+			survey_channel = ch->hw_value;
+			ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+		}
+	}
+
+	if (survey_channel != 0) {
+		if (pmlmeext->sitesurvey_res.channel_idx == 0)
+			set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+		else
+			SelectChannel(padapter, survey_channel);
+
+		if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
+			    rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+			} else {
+				int i;
+				for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+					if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
+						/* todo: to issue two probe req??? */
+						issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
+						/* msleep(SURVEY_TO>>1); */
+						issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
+					}
+				}
+
+				if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+					/* todo: to issue two probe req??? */
+					issue_probereq(padapter, NULL, NULL);
+					/* msleep(SURVEY_TO>>1); */
+					issue_probereq(padapter, NULL, NULL);
+				}
+			}
+		}
+
+		set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+	} else {
+		/*	channel number is 0 or this channel is not valid. */
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
+			if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
+				/*	Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
+				/*	This will let the following flow to run the scanning end. */
+				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+			}
+		}
+
+		if (rtw_p2p_findphase_ex_is_needed(pwdinfo)) {
+			/*	Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
+			set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			/* restore RX GAIN */
+			rtw_set_initial_gain(padapter, 0xff);
+			/* turn on dynamic functions */
+			Restore_DM_Func_Flag(padapter);
+			/* Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */
+
+			_set_timer(&pwdinfo->find_phase_timer, (u32)((u32)(pwdinfo->listen_dwell) * 100));
+		} else {
+			/*  20100721:Interrupt scan operation here. */
+			/*  For SW antenna diversity before link, it needs to switch to another antenna and scan again. */
+			/*  It compares the scan result and select beter one to do connection. */
+			if (AntDivBeforeLink8188E(padapter)) {
+				pmlmeext->sitesurvey_res.bss_cnt = 0;
+				pmlmeext->sitesurvey_res.channel_idx = -1;
+				pmlmeext->chan_scan_time = SURVEY_TO / 2;
+				set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+				return;
+			}
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+
+			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+			/* switch back to the original channel */
+
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN))
+				set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			else
+				set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+			/* config MSR */
+			Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+			/* restore RX GAIN */
+			rtw_set_initial_gain(padapter, 0xff);
+			/* turn on dynamic functions */
+			Restore_DM_Func_Flag(padapter);
+			/* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+
+			if (is_client_associated_to_ap(padapter))
+				issue_nulldata(padapter, NULL, 0, 3, 500);
+
+			rtw_mlme_site_survey_done(padapter);
+
+			report_surveydone_event(padapter);
+
+			pmlmeext->chan_scan_time = SURVEY_TO;
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+		}
+	}
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+{
+	int	i;
+	u32	len;
+	u8 *p;
+	u16 val16, subtype;
+	u8 *pframe = precv_frame->rx_data;
+	u32	packet_len = precv_frame->len;
+	u8 ie_offset;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	__le32 le32_tmp;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	memset(bssid, 0, sizeof(struct wlan_bssid_ex));
+
+	subtype = GetFrameSubType(pframe);
+
+	if (subtype == WIFI_BEACON) {
+		bssid->Reserved[0] = 1;
+		ie_offset = _BEACON_IE_OFFSET_;
+	} else {
+		/*  FIXME : more type */
+		if (subtype == WIFI_PROBEREQ) {
+			ie_offset = _PROBEREQ_IE_OFFSET_;
+			bssid->Reserved[0] = 2;
+		} else if (subtype == WIFI_PROBERSP) {
+			ie_offset = _PROBERSP_IE_OFFSET_;
+			bssid->Reserved[0] = 3;
+		} else {
+			bssid->Reserved[0] = 0;
+			ie_offset = _FIXED_IE_LENGTH_;
+		}
+	}
+
+	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* get the signal strength */
+	bssid->Rssi = precv_frame->attrib.phy_info.recvpower; /*  in dBM.raw data */
+	bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
+	bssid->PhyInfo.Optimum_antenna = rtw_current_antenna(padapter);
+
+	/*  checking SSID */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+	if (!p)
+		return _FAIL;
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID)
+			return _FAIL;
+		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	} else {
+		bssid->Ssid.SsidLength = 0;
+	}
+
+	memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	/* checking rate info... */
+	i = 0;
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p) {
+		if (len > NDIS_802_11_LENGTH_RATES_EX)
+			return _FAIL;
+		memcpy(bssid->SupportedRates, (p + 2), len);
+		i = len;
+	}
+
+	p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p) {
+		if (len > (NDIS_802_11_LENGTH_RATES_EX - i))
+			return _FAIL;
+		memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	/* todo: */
+	bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+
+	if (bssid->IELength < 12)
+		return _FAIL;
+
+	/*  Checking for DSConfig */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+
+	bssid->Configuration.DSConfig = 0;
+	bssid->Configuration.Length = 0;
+
+	if (p) {
+		bssid->Configuration.DSConfig = *(p + 2);
+	} else {/*  In 5G, some ap do not have DSSET IE */
+		/*  checking HT info for channel */
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+			bssid->Configuration.DSConfig = HT_info->primary_channel;
+		} else { /*  use current channel */
+			bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
+		}
+	}
+
+	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+	bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(0)) {
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+	} else {
+		bssid->InfrastructureMode = Ndis802_11IBSS;
+		memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
+	}
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	bssid->Configuration.ATIMWindow = 0;
+
+	/* 20/40 BSS Coexistence check */
+	if ((pregistrypriv->wifi_spec == 1) && (!pmlmeinfo->bwmode_updated)) {
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
+		if (p && len > 0) {
+			struct HT_caps_element	*pHT_caps;
+			pHT_caps = (struct HT_caps_element *)(p + 2);
+
+			if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & BIT(14))
+				pmlmepriv->num_FortyMHzIntolerant++;
+		} else {
+			pmlmepriv->num_sta_no_ht++;
+		}
+	}
+
+	/*  mark bss info receiving from nearby channel as SignalQuality 101 */
+	if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+		bssid->PhyInfo.SignalQuality = 101;
+	return _SUCCESS;
+}
+
+static void rtw_set_bssid(struct adapter *adapter, u8 *bssid)
+{
+	int i;
+
+	for (i = 0; i < ETH_ALEN; i++)
+		rtw_write8(adapter, REG_BSSID + i, bssid[i]);
+}
+
+static void mlme_join(struct adapter *adapter, int type)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	u8 retry_limit = 0x30, reg;
+	u32 reg32;
+	int res;
+
+	switch (type) {
+	case 0:
+		/* prepare to join */
+		/* enable to rx data frame, accept all data frame */
+		rtw_write16(adapter, REG_RXFLTMAP2, 0xFFFF);
+
+		res = rtw_read32(adapter, REG_RCR, &reg32);
+		if (res)
+			return;
+
+		rtw_write32(adapter, REG_RCR,
+			    reg32 | RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {
+			retry_limit = 48;
+		} else {
+			/* ad-hoc mode */
+			retry_limit = 0x7;
+		}
+		break;
+	case 1:
+		/* joinbss_event call back when join res < 0 */
+		rtw_write16(adapter, REG_RXFLTMAP2, 0x00);
+		break;
+	case 2:
+		/* sta add event call back */
+		/* enable update TSF */
+		res = rtw_read8(adapter, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_BCN_CTRL, reg & (~BIT(4)));
+
+		if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE))
+			retry_limit = 0x7;
+		break;
+	default:
+		break;
+	}
+
+	rtw_write16(adapter, REG_RL,
+		    retry_limit << RETRY_LIMIT_SHORT_SHIFT | retry_limit << RETRY_LIMIT_LONG_SHIFT);
+}
+
+void start_create_ibss(struct adapter *padapter)
+{
+	unsigned short	caps;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&pmlmeinfo->network);
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps & cap_IBSS) {/* adhoc master */
+		rtw_write8(padapter, REG_SECCFG, 0xcf);
+
+		/* switch channel */
+		/* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+
+		beacon_timing_control(padapter);
+
+		/* set msr to WIFI_FW_ADHOC_STATE */
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+		Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+		/* issue beacon */
+		if (send_beacon(padapter) == _FAIL) {
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else {
+			rtw_set_bssid(padapter, padapter->registrypriv.dev_network.MacAddress);
+			mlme_join(padapter, 0);
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+			rtw_indicate_connect(padapter);
+		}
+	} else {
+		return;
+	}
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+}
+
+void start_clnt_join(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&pmlmeinfo->network);
+	int beacon_timeout;
+
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps & cap_ESS) {
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+
+		rtw_write8(padapter, REG_SECCFG, val8);
+
+		/* switch channel */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		/* here wait for receiving the beacon to start auth */
+		/* and enable a timer */
+		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+		set_link_timer(pmlmeext, beacon_timeout);
+		_set_timer(&padapter->mlmepriv.assoc_timer,
+			   (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO * REASSOC_LIMIT) + beacon_timeout);
+
+		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+	} else if (caps & cap_IBSS) { /* adhoc client */
+		Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+		rtw_write8(padapter, REG_SECCFG, 0xcf);
+
+		/* switch channel */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		beacon_timing_control(padapter);
+
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+		report_join_res(padapter, 1);
+	} else {
+		return;
+	}
+}
+
+void start_clnt_auth(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
+	pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
+
+	pmlmeinfo->auth_seq = 1;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeext->retry = 0;
+
+	/*  Because of AP's not receiving deauth before */
+	/*  AP may: 1)not response auth or 2)deauth us after link is complete */
+	/*  issue deauth before issuing auth to deal with the situation */
+	/*	Commented by Albert 2012/07/21 */
+	/*	For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
+	issue_deauth(padapter, (&pmlmeinfo->network)->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
+
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+}
+
+void start_clnt_assoc(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+	issue_assocreq(padapter);
+
+	set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+void receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* check A3 */
+	if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_del_sta_event(padapter, MacAddr, reason);
+		} else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+		}
+	}
+}
+
+static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct rt_channel_info *chplan_new;
+	u8 channel;
+	u8 i;
+
+	pregistrypriv = &padapter->registrypriv;
+	pmlmeext = &padapter->mlmeextpriv;
+
+	/*  Adjust channel plan by AP Country IE */
+	if (pregistrypriv->enable80211d &&
+	    (!pmlmeext->update_channel_plan_by_ap_done)) {
+		u8 *ie, *p;
+		u32 len;
+		struct rt_channel_plan chplan_ap;
+		struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
+		u8 country[4];
+		u8 fcn; /*  first channel number */
+		u8 noc; /*  number of channel */
+		u8 j, k;
+
+		ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+		if (!ie)
+			return;
+		if (len < 6)
+			return;
+		ie += 2;
+		p = ie;
+		ie += len;
+
+		memset(country, 0, 4);
+		memcpy(country, p, 3);
+		p += 3;
+
+		i = 0;
+		while ((ie - p) >= 3) {
+			fcn = *(p++);
+			noc = *(p++);
+			p++;
+
+			for (j = 0; j < noc; j++) {
+				channel = fcn + j;
+				chplan_ap.Channel[i++] = channel;
+			}
+		}
+		chplan_ap.Len = i;
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+
+		memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = 0;
+		j = 0;
+		k = 0;
+		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+				    (chplan_sta[i].ChannelNum == 0))
+					break;
+
+				if (j == chplan_ap.Len)
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/*  change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM) &&
+			       (chplan_sta[i].ChannelNum != 0) &&
+			       (chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/*  add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else {
+			/*  keep original STA 2.4G channel plan */
+			while ((i < MAX_CHANNEL_NUM) &&
+			       (chplan_sta[i].ChannelNum != 0) &&
+			       (chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+
+			/*  skip AP 2.4G channel plan */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+				j++;
+		}
+
+		/*  keep original STA 5G channel plan */
+		while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+			chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			chplan_new[k].ScanType = chplan_sta[i].ScanType;
+			i++;
+			k++;
+		}
+
+		pmlmeext->update_channel_plan_by_ap_done = 1;
+	}
+
+	/*  If channel is used by AP, set channel scan type to active */
+	channel = bssid->Configuration.DSConfig;
+	chplan_new = pmlmeext->channel_set;
+	i = 0;
+	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
+		if (chplan_new[i].ChannelNum == channel) {
+			if (chplan_new[i].ScanType == SCAN_PASSIVE)
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+			break;
+		}
+		i++;
+	}
+}
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct survey_event	*psurvey_evt;
+	struct C2HEvent_Header *pc2h_evt_hdr;
+	struct mlme_ext_priv *pmlmeext;
+	struct cmd_priv *pcmdpriv;
+	/* u8 *pframe = precv_frame->rx_data; */
+	/* uint len = precv_frame->len; */
+
+	if (!padapter)
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = kzalloc(sizeof(*pcmd_obj), GFP_ATOMIC);
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct survey_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+
+	if (collect_bss_info(padapter, precv_frame, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
+		kfree(pcmd_obj);
+		kfree(pevtcmd);
+		return;
+	}
+
+	process_80211d(padapter, &psurvey_evt->bss);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+}
+
+void report_surveydone_event(struct adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct surveydone_event *psurveydone_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = kzalloc(sizeof(*pcmd_obj), GFP_KERNEL);
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = kzalloc(cmdsz, GFP_KERNEL);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct surveydone_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+void report_join_res(struct adapter *padapter, int res)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct joinbss_event		*pjoinbss_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = kzalloc(sizeof(*pcmd_obj), GFP_ATOMIC);
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct joinbss_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&pjoinbss_evt->network.network), &pmlmeinfo->network, sizeof(struct wlan_bssid_ex));
+	pjoinbss_evt->network.join_res	= res;
+	pjoinbss_evt->network.aid = res;
+
+	rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = kzalloc(sizeof(*pcmd_obj), GFP_ATOMIC);
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stadel_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&pdel_sta_evt->macaddr), MacAddr, ETH_ALEN);
+	memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if (psta)
+		mac_id = (int)psta->mac_id;
+	else
+		mac_id = (-1);
+
+	pdel_sta_evt->mac_id = mac_id;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct stassoc_event		*padd_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = kzalloc(sizeof(*pcmd_obj), GFP_KERNEL);
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = kzalloc(cmdsz, GFP_KERNEL);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&padd_sta_evt->macaddr), MacAddr, ETH_ALEN);
+	padd_sta_evt->cam_id = cam_idx;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* ERP */
+	VCS_update(padapter, psta);
+
+	/* HT */
+	if (pmlmepriv->htpriv.ht_option) {
+		psta->htpriv.ht_option = true;
+
+		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+		if (support_short_GI(padapter, &pmlmeinfo->HT_caps))
+			psta->htpriv.sgi = true;
+
+		psta->qos_option = true;
+	} else {
+		psta->htpriv.ht_option = false;
+
+		psta->htpriv.ampdu_enable = false;
+
+		psta->htpriv.sgi = false;
+		psta->qos_option = false;
+	}
+	psta->htpriv.bwmode = pmlmeext->cur_bwmode;
+	psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	/* QoS */
+	if (pmlmepriv->qospriv.qos_option)
+		psta->qos_option = true;
+
+	psta->state = _FW_LINKED;
+}
+
+static void rtw_reset_dm_func_flag(struct adapter *adapter)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct dm_priv *dmpriv = &haldata->dmpriv;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	odmpriv->SupportAbility = dmpriv->InitODMFlag;
+}
+
+static void rtw_clear_dm_func_flag(struct adapter *adapter)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	odmpriv->SupportAbility = 0;
+}
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
+{
+	struct sta_info		*psta, *psta_bmc;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u16 media_status;
+
+	if (join_res < 0) {
+		mlme_join(padapter, 1);
+		rtw_set_bssid(padapter, null_addr);
+
+		/* restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+		return;
+	}
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+		/* for bc/mc */
+		psta_bmc = rtw_get_bcmc_stainfo(padapter);
+		if (psta_bmc) {
+			pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
+			update_bmc_sta_support_rate(padapter, psta_bmc->mac_id);
+			Update_RA_Entry(padapter, psta_bmc->mac_id);
+		}
+	}
+
+	/* turn on dynamic functions */
+	rtw_reset_dm_func_flag(padapter);
+
+	/*  update IOT-releated issue */
+	update_IOT_info(padapter);
+
+	rtw_set_basic_rate(padapter, cur_network->SupportedRates);
+
+	/* BCN interval */
+	rtw_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
+
+	/* udpate capability */
+	update_capinfo(padapter, pmlmeinfo->capability);
+
+	/* WMM, Update EDCA param */
+	WMMOnAssocRsp(padapter);
+
+	/* HT */
+	HTOnAssocRsp(padapter);
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (psta) { /* only for infra. mode */
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+		/* set per sta rate after updating HT cap. */
+		set_sta_rate(padapter, psta);
+		rtw_set_max_rpt_macid(padapter, psta->mac_id);
+
+		media_status = (psta->mac_id << 8) | 1; /*   MACID|OPMODE: 1 means connect */
+		rtl8188e_set_FwMediaStatus_cmd(padapter, media_status);
+	}
+
+	mlme_join(padapter, 2);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+		/*  correcting TSF */
+		correct_TSF(padapter);
+	}
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+}
+
+void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {/* adhoc master or sta_count>1 */
+			/* nothing to do */
+		} else { /* adhoc client */
+			/*  correcting TSF */
+			correct_TSF(padapter);
+
+			/* start beacon */
+			if (send_beacon(padapter) == _FAIL) {
+				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
+				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
+				return;
+			}
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+		}
+		mlme_join(padapter, 2);
+	}
+
+	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+	/* rate radaptive */
+	Update_RA_Entry(padapter, psta->mac_id);
+
+	/* update adhoc sta_info */
+	update_sta_info(padapter, psta);
+}
+
+static void mlme_disconnect(struct adapter *adapter)
+{
+	int res;
+	u8 reg;
+
+	/* Set RCR to not to receive data frame when NO LINK state */
+	/* reject all data frames */
+	rtw_write16(adapter, REG_RXFLTMAP2, 0x00);
+
+	/* reset TSF */
+	rtw_write8(adapter, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+
+	/* disable update TSF */
+
+	res = rtw_read8(adapter, REG_BCN_CTRL, &reg);
+	if (res)
+		return;
+
+	rtw_write8(adapter, REG_BCN_CTRL, reg | BIT(4));
+}
+
+void mlmeext_sta_del_event_callback(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter)) {
+		mlme_disconnect(padapter);
+		rtw_set_bssid(padapter, null_addr);
+
+		/* restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+		/* switch to the 20M Hz mode after disconnect */
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+		/* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		flush_all_cam_entry(padapter);
+
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		/* set MSR to no link state -> infra. mode */
+		Set_MSR(padapter, _HW_STATE_STATION_);
+
+		_cancel_timer_ex(&pmlmeext->link_timer);
+	}
+}
+
+/****************************************************************************
+
+Following are the functions for the timer handlers
+
+*****************************************************************************/
+static u8 chk_ap_is_alive(struct sta_info *psta)
+{
+	u8 ret = false;
+
+	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) &&
+	    sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
+	    sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
+		ret = false;
+	else
+		ret = true;
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+static int rtl8188e_sreset_linked_status_check(struct adapter *padapter)
+{
+	u32 rx_dma_status;
+	int res;
+	u8 reg;
+
+	res = rtw_read32(padapter, REG_RXDMA_STATUS, &rx_dma_status);
+	if (res)
+		return res;
+
+	if (rx_dma_status != 0x00)
+		rtw_write32(padapter, REG_RXDMA_STATUS, rx_dma_status);
+
+	return rtw_read8(padapter, REG_FMETHR, &reg);
+}
+
+void linked_status_chk(struct adapter *padapter)
+{
+	u32	i;
+	struct sta_info		*psta;
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+
+	rtl8188e_sreset_linked_status_check(padapter);
+
+	if (is_client_associated_to_ap(padapter)) {
+		/* linked infrastructure client mode */
+
+		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+		int rx_chk_limit;
+
+		rx_chk_limit = 4;
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta) {
+			bool is_p2p_enable = false;
+			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
+
+			if (!chk_ap_is_alive(psta))
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+			if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
+				u8 backup_oper_channel = 0;
+
+				/* switch to correct channel of current network  before issue keep-alive frames */
+				if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+					backup_oper_channel = rtw_get_oper_ch(padapter);
+					SelectChannel(padapter, pmlmeext->cur_channel);
+				}
+
+				if (rx_chk != _SUCCESS)
+					issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
+
+				if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
+					tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
+					/* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
+					if (tx_chk == _SUCCESS && !is_p2p_enable)
+						rx_chk = _SUCCESS;
+				}
+
+				/* back to the original operation channel */
+				if (backup_oper_channel > 0)
+					SelectChannel(padapter, backup_oper_channel);
+			} else {
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+					}
+				}
+
+				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) {
+					tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress,
+							   WLAN_REASON_EXPIRATION_CHK);
+					return;
+				}
+			} else {
+				pmlmeext->retry = 0;
+			}
+
+			if (tx_chk == _FAIL) {
+				pmlmeinfo->link_count &= 0xf;
+			} else {
+				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
+				pmlmeinfo->link_count = 0;
+			}
+		} /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */
+	} else if (is_client_associated_to_ibss(padapter)) {
+		/* linked IBSS mode */
+		/* for each assoc list entry to check the rx pkt counter */
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+			if (pmlmeinfo->FW_sta_info[i].status == 1) {
+				psta = pmlmeinfo->FW_sta_info[i].psta;
+
+				if (psta == NULL)
+					continue;
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) {
+					if (pmlmeinfo->FW_sta_info[i].retry < 3) {
+						pmlmeinfo->FW_sta_info[i].retry++;
+					} else {
+						pmlmeinfo->FW_sta_info[i].retry = 0;
+						pmlmeinfo->FW_sta_info[i].status = 0;
+						report_del_sta_event(padapter, psta->hwaddr
+							, 65535/*  indicate disconnect caused by no rx */
+					);
+					}
+				} else {
+					pmlmeinfo->FW_sta_info[i].retry = 0;
+					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
+				}
+			}
+		}
+	}
+}
+
+void survey_timer_hdl(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv					*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/* issue rtw_sitesurvey_cmd */
+	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
+		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
+			pmlmeext->sitesurvey_res.channel_idx++;
+
+		if (pmlmeext->scan_abort) {
+			if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) {
+				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+				pmlmeext->sitesurvey_res.channel_idx = 3;
+			} else {
+				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+			}
+
+			pmlmeext->scan_abort = false;/* reset */
+		}
+
+		ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+		if (!ph2c)
+			goto exit_survey_timer_hdl;
+
+		psurveyPara = kzalloc(sizeof(*psurveyPara), GFP_ATOMIC);
+		if (!psurveyPara) {
+			kfree(ph2c);
+			goto exit_survey_timer_hdl;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+
+exit_survey_timer_hdl:
+	return;
+}
+
+void link_timer_hdl(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		report_join_res(padapter, -3);
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
+		/* re-auth timer */
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
+			pmlmeinfo->state = 0;
+			report_join_res(padapter, -1);
+			return;
+		}
+
+		pmlmeinfo->auth_seq = 1;
+		issue_auth(padapter, NULL, 0);
+		set_link_timer(pmlmeext, REAUTH_TO);
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
+		/* re-assoc timer */
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+			return;
+		}
+
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+	struct ht_priv	*phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+	}
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 type;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+	if (psetop->mode == Ndis802_11APMode) {
+		pmlmeinfo->state = WIFI_FW_AP_STATE;
+		type = _HW_STATE_AP_;
+	} else if (psetop->mode == Ndis802_11Infrastructure) {
+		pmlmeinfo->state &= ~(BIT(0) | BIT(1));/*  clear state */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to	STATION_STATE */
+		type = _HW_STATE_STATION_;
+	} else if (psetop->mode == Ndis802_11IBSS) {
+		type = _HW_STATE_ADHOC_;
+	} else {
+		type = _HW_STATE_NOLINK_;
+	}
+
+	rtw_set_opmode(padapter, type);
+
+	return H2C_SUCCESS;
+}
+
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&pmlmeinfo->network);
+	struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+	/* u32	initialgain; */
+
+	if (pparm->network.InfrastructureMode == Ndis802_11APMode) {
+		if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
+			/* todo: */
+			return H2C_SUCCESS;
+		}
+	}
+
+	/* below is for ad-hoc master */
+	if (pparm->network.InfrastructureMode == Ndis802_11IBSS) {
+		rtw_joinbss_reset(padapter);
+
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		pmlmeinfo->ERP_enable = 0;
+		pmlmeinfo->WMM_enable = 0;
+		pmlmeinfo->HT_enable = 0;
+		pmlmeinfo->HT_caps_enable = 0;
+		pmlmeinfo->HT_info_enable = 0;
+		pmlmeinfo->agg_enable_bitmap = 0;
+		pmlmeinfo->candidate_tid_bitmap = 0;
+
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		rtw_clear_dm_func_flag(padapter);
+
+		/* cancel link timer */
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		memcpy(pnetwork, pbuf, offsetof(struct wlan_bssid_ex, IELength));
+		pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+		if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+			return H2C_PARAMETERS_ERROR;
+
+		memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+		start_create_ibss(padapter);
+	}
+
+	return H2C_SUCCESS;
+}
+
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct ndis_802_11_var_ie *pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&pmlmeinfo->network);
+	struct joinbss_parm	*pparm = (struct joinbss_parm *)pbuf;
+	u32 i;
+
+	/* check already connecting to AP or not */
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 5, 100);
+
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+		/* set MSR to nolink -> infra. mode */
+		Set_MSR(padapter, _HW_STATE_STATION_);
+
+		mlme_disconnect(padapter);
+	}
+
+	rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, false);
+
+	rtw_joinbss_reset(padapter);
+
+	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pmlmeinfo->ERP_enable = 0;
+	pmlmeinfo->WMM_enable = 0;
+	pmlmeinfo->HT_enable = 0;
+	pmlmeinfo->HT_caps_enable = 0;
+	pmlmeinfo->HT_info_enable = 0;
+	pmlmeinfo->agg_enable_bitmap = 0;
+	pmlmeinfo->candidate_tid_bitmap = 0;
+	pmlmeinfo->bwmode_updated = false;
+
+	memcpy(pnetwork, pbuf, offsetof(struct wlan_bssid_ex, IELength));
+	pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+	if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+		return H2C_PARAMETERS_ERROR;
+
+	memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+	/* Check AP vendor to move rtw_joinbss_cmd() */
+
+	for (i = sizeof(struct ndis_802_11_fixed_ie); i < pnetwork->IELength;) {
+		pIE = (struct ndis_802_11_var_ie *)(pnetwork->IEs + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
+			if (!memcmp(pIE->data, WMM_OUI, 4))
+				pmlmeinfo->WMM_enable = 1;
+			break;
+		case _HT_CAPABILITY_IE_:	/* Get HT Cap IE. */
+			pmlmeinfo->HT_caps_enable = 1;
+			break;
+		case _HT_EXTRA_INFO_IE_:	/* Get HT Info IE. */
+			pmlmeinfo->HT_info_enable = 1;
+
+			/* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
+			{
+				struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
+
+				if ((pregpriv->cbw40_enable) &&	 (pht_info->infos[0] & BIT(2))) {
+					/* switch to the 40M Hz mode according to the AP */
+					pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+					switch (pht_info->infos[0] & 0x3) {
+					case 1:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+						break;
+					case 3:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+						break;
+					default:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+						break;
+					}
+				}
+			}
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+	/* disable dynamic functions, such as high power, DIG */
+
+	/* config the initial gain under linking, need to write the BB registers */
+
+	rtw_set_bssid(padapter, pmlmeinfo->network.MacAddress);
+	mlme_join(padapter, 0);
+
+	/* cancel link timer */
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	start_clnt_join(padapter);
+
+	return H2C_SUCCESS;
+}
+
+u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&pmlmeinfo->network);
+	u8 val8;
+	int res;
+
+	if (is_client_associated_to_ap(padapter))
+		issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms / 100, 100);
+
+	mlme_disconnect(padapter);
+	rtw_set_bssid(padapter, null_addr);
+
+	/* restore to initial setting. */
+	update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+		/* Stop BCN */
+		res = rtw_read8(padapter, REG_BCN_CTRL, &val8);
+		if (res)
+			return H2C_DROPPED;
+
+		rtw_write8(padapter, REG_BCN_CTRL, val8 & (~(EN_BCN_FUNCTION | EN_TXBCN_RPT)));
+	}
+
+	/* set MSR to no link state -> infra. mode */
+	Set_MSR(padapter, _HW_STATE_STATION_);
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+	/* switch to the 20M Hz mode after disconnect */
+	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	flush_all_cam_entry(padapter);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	return	H2C_SUCCESS;
+}
+
+static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
+	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	int set_idx;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* clear out first */
+	memset(out, 0, sizeof(struct rtw_ieee80211_channel) * out_num);
+
+	/* acquire channels from in */
+	j = 0;
+	for (i = 0; i < in_num; i++) {
+		set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+		if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED) &&
+		    set_idx >= 0) {
+			memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
+
+			if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+				out[j].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+		if (j >= out_num)
+			break;
+	}
+
+	/* if out is empty, use channel_set as default */
+	if (j == 0) {
+		for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+			out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
+
+			if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+				out[i].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+	}
+
+	return j;
+}
+
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct sitesurvey_parm	*pparm = (struct sitesurvey_parm *)pbuf;
+	u8 bdelayscan = false;
+	u32	i;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
+		/* for first time sitesurvey_cmd */
+
+		pmlmeext->sitesurvey_res.state = SCAN_START;
+		pmlmeext->sitesurvey_res.bss_cnt = 0;
+		pmlmeext->sitesurvey_res.channel_idx = 0;
+
+		for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (pparm->ssid[i].SsidLength) {
+				memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength = pparm->ssid[i].SsidLength;
+			} else {
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength = 0;
+			}
+		}
+
+		pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
+			, pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
+			, pparm->ch, pparm->ch_num
+	);
+
+		pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
+
+		/* issue null data if associating to the AP */
+		if (is_client_associated_to_ap(padapter)) {
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			issue_nulldata(padapter, NULL, 1, 3, 500);
+
+			bdelayscan = true;
+		}
+		if (bdelayscan) {
+			/* delay 50ms to protect nulldata(1). */
+			set_survey_timer(pmlmeext, 50);
+			return H2C_SUCCESS;
+		}
+	}
+
+	if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		rtw_clear_dm_func_flag(padapter);
+
+		/* config the initial gain under scanning, need to write the BB registers */
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+			rtw_set_initial_gain(padapter, 0x1e);
+		else
+			rtw_set_initial_gain(padapter, 0x28);
+
+
+		/* set MSR to no link state */
+		Set_MSR(padapter, _HW_STATE_NOLINK_);
+
+		rtw_mlme_under_site_survey(padapter);
+
+		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
+	}
+
+	site_survey(padapter);
+
+	return H2C_SUCCESS;
+}
+
+u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pparm->mode < 4)
+		pmlmeinfo->auth_algo = pparm->mode;
+	return	H2C_SUCCESS;
+}
+
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	unsigned short				ctrl;
+	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	unsigned char					null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	/* main tx key for wep. */
+	if (pparm->set_tx)
+		pmlmeinfo->key_index = pparm->keyid;
+
+	/* write cam */
+	ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+
+	write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
+
+	return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl = 0;
+	u8 cam_id;/* cam_entry */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+
+	/* cam_entry: */
+	/* 0~3 for default key */
+
+	/* for concurrent mode (ap+sta): */
+	/* default key is disable, using sw encrypt/decrypt */
+	/* cam_entry = 4 for sta mode (macid = 0) */
+	/* cam_entry(macid+3) = 5 ~ N for ap mode (aid = 1~N, macid = 2 ~N) */
+
+	/* for concurrent mode (sta+sta): */
+	/* default key is disable, using sw encrypt/decrypt */
+	/* cam_entry = 4 mapping to macid = 0 */
+	/* cam_entry = 5 mapping to macid = 2 */
+
+	cam_id = 4;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		if (pparm->algorithm == _NO_PRIVACY_)	/*  clear cam entry */ {
+			clear_cam_entry(padapter, pparm->id);
+			return H2C_SUCCESS_RSP;
+		}
+
+		psta = rtw_get_stainfo(pstapriv, pparm->addr);
+		if (psta) {
+			ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+
+			if ((psta->mac_id < 1) || (psta->mac_id > (NUM_STA - 4)))
+				return H2C_REJECTED;
+
+			cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+
+			write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+
+			return H2C_SUCCESS_RSP;
+		} else {
+			return H2C_REJECTED;
+		}
+	}
+
+	/* below for sta mode */
+
+	if (pparm->algorithm == _NO_PRIVACY_) {	/*  clear cam entry */
+		clear_cam_entry(padapter, pparm->id);
+		return H2C_SUCCESS;
+	}
+	ctrl = BIT(15) | ((pparm->algorithm) << 2);
+	write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	return H2C_SUCCESS;
+}
+
+u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+
+	if (!psta)
+		return	H2C_SUCCESS;
+
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+		issue_action_BA(padapter, pparm->addr, WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	} else {
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+	}
+	return	H2C_SUCCESS;
+}
+
+u8 set_tx_beacon_cmd(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct Tx_Beacon_param	*ptxBeacon_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 res = _SUCCESS;
+	int len_diff = 0;
+
+	ph2c = kzalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = kzalloc(sizeof(*ptxBeacon_parm), GFP_ATOMIC);
+	if (!ptxBeacon_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(&ptxBeacon_parm->network, &pmlmeinfo->network, sizeof(struct wlan_bssid_ex));
+
+	len_diff = update_hidden_ssid(ptxBeacon_parm->network.IEs + _BEACON_IE_OFFSET_,
+				      ptxBeacon_parm->network.IELength - _BEACON_IE_OFFSET_,
+				      pmlmeinfo->hidden_ssid_mode);
+	ptxBeacon_parm->network.IELength += len_diff;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &padapter->evtpriv;
+
+	peventbuf = (uint *)pbuf;
+	evt_sz = (u16)(*peventbuf & 0xffff);
+	evt_code = (u8)((*peventbuf >> 16) & 0xff);
+
+	/*  checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT)
+		goto _abort_event_;
+
+	/*  checking if event size match the event parm size */
+	if ((wlanevents[evt_code].parmsize != 0) &&
+	    (wlanevents[evt_code].parmsize != evt_sz))
+		goto _abort_event_;
+
+	atomic_inc(&pevt_priv->event_seq);
+
+	peventbuf += 2;
+
+	if (peventbuf) {
+		event_callback = wlanevents[evt_code].event_callback;
+		event_callback(padapter, (u8 *)peventbuf);
+	}
+
+_abort_event_:
+	return H2C_SUCCESS;
+}
+
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (send_beacon(padapter) == _FAIL) {
+		return H2C_PARAMETERS_ERROR;
+	} else {
+		/* tx bc/mc frames after update TIM */
+		struct sta_info *psta_bmc;
+		struct list_head *xmitframe_plist, *xmitframe_phead;
+		struct xmit_frame *pxmitframe = NULL;
+		struct sta_priv  *pstapriv = &padapter->stapriv;
+
+		/* for BC/MC Frames */
+		psta_bmc = rtw_get_bcmc_stainfo(padapter);
+		if (!psta_bmc)
+			return H2C_SUCCESS;
+
+		if ((pstapriv->tim_bitmap & BIT(0)) && (psta_bmc->sleepq_len > 0)) {
+			msleep(10);/*  10ms, ATIM(HIQ) Windows */
+			spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+			xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+			xmitframe_plist = xmitframe_phead->next;
+
+			while (xmitframe_phead != xmitframe_plist) {
+				pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+				xmitframe_plist = xmitframe_plist->next;
+
+				list_del_init(&pxmitframe->list);
+
+				psta_bmc->sleepq_len--;
+				if (psta_bmc->sleepq_len > 0)
+					pxmitframe->attrib.mdata = 1;
+				else
+					pxmitframe->attrib.mdata = 0;
+
+				pxmitframe->attrib.triggered = 1;
+
+				pxmitframe->attrib.qsel = 0x11;/* HIQ */
+
+				spin_unlock_bh(&psta_bmc->sleep_q.lock);
+				if (rtl8188eu_hal_xmit(padapter, pxmitframe))
+					rtw_xmit_complete(padapter, pxmitframe);
+				spin_lock_bh(&psta_bmc->sleep_q.lock);
+			}
+			spin_unlock_bh(&psta_bmc->sleep_q.lock);
+		}
+	}
+	return H2C_SUCCESS;
+}
+
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	pmlmeext->cur_channel = set_ch_parm->ch;
+	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+	pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+	set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	return	H2C_SUCCESS;
+}
+
+u8 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+	return	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return	H2C_REJECTED;
+}
+
+/*  TDLS_WRCR		: write RCR DATA BIT */
+/*  TDLS_SD_PTI		: issue peer traffic indication */
+/*  TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure */
+/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame */
+/*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/*  TDLS_OFF_CH		: first time set channel to off channel */
+/*  TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel */
+/*  TDLS_P_OFF_CH	: periodically go to off channel */
+/*  TDLS_P_BASE_CH	: periodically go back to base channel */
+/*  TDLS_RS_RCR		: restore RCR */
+/*  TDLS_CKALV_PH1	: check alive timer phase1 */
+/*  TDLS_CKALV_PH2	: check alive timer phase2 */
+/*  TDLS_FREE_STA	: free tdls sta */
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return H2C_REJECTED;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_p2p.c b/drivers/staging/r8188eu/core/rtw_p2p.c
new file mode 100644
index 000000000..dc159e58f
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_p2p.c
@@ -0,0 +1,1925 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTW_P2P_C_
+
+#include "../include/drv_types.h"
+#include "../include/rtw_p2p.h"
+#include "../include/wifi.h"
+
+static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8 *ch_list, u8 ch_cnt)
+{
+	int found = 0, i = 0;
+
+	for (i = 0; i < ch_cnt; i++) {
+		if (ch_list[i] == desired_ch) {
+			found = 1;
+			break;
+		}
+	}
+	return found;
+}
+
+static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	struct list_head *phead, *plist;
+	u32 len = 0;
+	u16 attr_len = 0;
+	u8 tmplen, *pdata_attr, *pstart, *pcur;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_KERNEL);
+
+	pstart = pdata_attr;
+	pcur = pdata_attr;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = phead->next;
+
+	/* look up sta asoc_queue */
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, asoc_list);
+
+		plist = plist->next;
+
+		if (psta->is_p2p_device) {
+			tmplen = 0;
+
+			pcur++;
+
+			/* P2P device address */
+			memcpy(pcur, psta->dev_addr, ETH_ALEN);
+			pcur += ETH_ALEN;
+
+			/* P2P interface address */
+			memcpy(pcur, psta->hwaddr, ETH_ALEN);
+			pcur += ETH_ALEN;
+
+			*pcur = psta->dev_cap;
+			pcur++;
+
+			/* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
+			RTW_PUT_BE16(pcur, psta->config_methods);
+			pcur += 2;
+
+			memcpy(pcur, psta->primary_dev_type, 8);
+			pcur += 8;
+
+			*pcur = psta->num_of_secdev_type;
+			pcur++;
+
+			memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type * 8);
+			pcur += psta->num_of_secdev_type * 8;
+
+			if (psta->dev_name_len > 0) {
+				/* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+				RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
+				pcur += 2;
+
+				/* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
+				RTW_PUT_BE16(pcur, psta->dev_name_len);
+				pcur += 2;
+
+				memcpy(pcur, psta->dev_name, psta->dev_name_len);
+				pcur += psta->dev_name_len;
+			}
+
+			tmplen = (u8)(pcur - pstart);
+
+			*pstart = (tmplen - 1);
+
+			attr_len += tmplen;
+
+			/* pstart += tmplen; */
+			pstart = pcur;
+		}
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	if (attr_len > 0)
+		len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
+
+	kfree(pdata_attr);
+	return len;
+}
+
+static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
+	__be32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_GO_DISC_REQUEST;
+	u8	dialogToken = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* Build P2P action frame header */
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	/* there is no IE in this P2P action frame */
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_DEVDISC_RESP;
+	u8 p2pie[8] = { 0x00 };
+	u32 p2pielen = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* Build P2P public action frame header */
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	/* Build P2P IE */
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*  P2P_ATTR_STATUS */
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8 *raddr, u8 *frame_body, u16 config_method)
+{
+	struct adapter *padapter = pwdinfo->padapter;
+	unsigned char category = WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u8			dialogToken = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
+	u8			wpsie[100] = { 0x00 };
+	u8			wpsielen = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	RTW_PUT_BE32(wpsie, WPSOUI);
+	wpsielen += 4;
+
+	/*	Config Method */
+	/*	Type: */
+	RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	RTW_PUT_BE16(wpsie + wpsielen, config_method);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
+	__be32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_PRESENCE_RESPONSE;
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u8 noa_attr_content[32] = { 0x00 };
+	u32 p2pielen = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* Build P2P action frame header */
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &oui_subtype, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &dialogToken, &pattrib->pktlen);
+
+	/* Add P2P IE header */
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/* Add Status attribute in P2P IE */
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+	/* Add NoA attribute in P2P IE */
+	noa_attr_content[0] = 0x1;/* index */
+	noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
+
+	/* todo: Notice of Absence Descriptor(s) */
+
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u16 capability = 0;
+	u32 len = 0, p2pielen = 0;
+	__le16 le_tmp;
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. P2P Device ID */
+	/*	3. Notice of Absence (NOA) */
+
+	/*	P2P Capability ATTR */
+	/*	Type: */
+	/*	Length: */
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	/*	Be able to participate in additional P2P Groups and */
+	/*	support the P2P Invitation Procedure */
+	/*	Group Capability Bitmap, 1 byte */
+	capability = P2P_DEVCAP_INVITATION_PROC | P2P_DEVCAP_CLIENT_DISCOVERABILITY;
+	capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+		capability |= (P2P_GRPCAP_GROUP_FORMATION << 8);
+
+	le_tmp = cpu_to_le16(capability);
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8 *)&le_tmp);
+
+	/*  P2P Device ID ATTR */
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
+
+	/*  Notice of Absence ATTR */
+	/*	Type: */
+	/*	Length: */
+	/*	Value: */
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+	return len;
+}
+
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len = 0, p2pielen = 0;
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20100907 */
+	/*	According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. Extended Listen Timing */
+	/*	3. Notice of Absence (NOA)	(Only GO needs this) */
+	/*	4. Device Info */
+	/*	5. Group Info	(Only GO need this) */
+
+	/*	P2P Capability ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie[p2pielen] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+			p2pie[p2pielen] |= P2P_GRPCAP_GROUP_FORMATION;
+
+		p2pielen++;
+	} else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+		/*	Group Capability Bitmap, 1 byte */
+		if (pwdinfo->persistent_supported)
+			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+	}
+
+	/*	Extended Listen Timing ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	/*	Length: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+	/*  Notice of Absence ATTR */
+	/*	Type: */
+	/*	Length: */
+	/*	Value: */
+
+	/*	Device Info ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
+	RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
+	RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	/*  Group Info ATTR */
+	/*	Type: */
+	/*	Length: */
+	/*	Value: */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+		p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+	return len;
+}
+
+u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
+{
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len = 0, p2pielen = 0;
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110301 */
+	/*	According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. Device Info */
+	/*	3. Group ID (When joining an operating P2P Group) */
+
+	/*	P2P Capability ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+	else
+		p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+	/*	Device Info ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
+	RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) {
+		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
+	} else {
+		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
+	}
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
+	RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		/*	Added by Albert 2011/05/19 */
+		/*	In this case, the pdev_raddr is the device address of the group owner. */
+
+		/*	P2P Group ID ATTR */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+		/*	Length: */
+		/* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
+		RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		memcpy(p2pie + p2pielen, pssid, ussidlen);
+		p2pielen += ussidlen;
+	}
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+	return len;
+}
+
+u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
+{
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len = 0, p2pielen = 0;
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*  According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
+	/*	1. Status */
+	/*	2. Extended Listen Timing (optional) */
+
+	/*	Status ATTR */
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
+
+	/*  Extended Listen Timing ATTR */
+	/*	Type: */
+	/*	Length: */
+	/*	Value: */
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+	return len;
+}
+
+u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *p;
+	u32 ret = false;
+	u8 *p2pie;
+	u32	p2pielen = 0;
+	int ssid_len = 0, rate_cnt = 0;
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+	if (rate_cnt <= 4) {
+		int i, g_rate = 0;
+
+		for (i = 0; i < rate_cnt; i++) {
+			if (((*(p + 2 + i) & 0xff) != 0x02) &&
+			    ((*(p + 2 + i) & 0xff) != 0x04) &&
+			    ((*(p + 2 + i) & 0xff) != 0x0B) &&
+			    ((*(p + 2 + i) & 0xff) != 0x16))
+				g_rate = 1;
+		}
+
+		if (g_rate == 0) {
+			/*	There is no OFDM rate included in SupportedRates IE of this probe request frame */
+			/*	The driver should response this probe request. */
+			return ret;
+		}
+	} else {
+		/*	rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
+		/*	We should proceed the following check for this probe request. */
+	}
+
+	/*	Added comments by Albert 20100906 */
+	/*	There are several items we should check here. */
+	/*	1. This probe request frame must contain the P2P IE. (Done) */
+	/*	2. This probe request frame must contain the wildcard SSID. (Done) */
+	/*	3. Wildcard BSSID. (Todo) */
+	/*	4. Destination Address. (Done in mgt_dispatcher function) */
+	/*	5. Requested Device Type in WSC IE. (Todo) */
+	/*	6. Device ID attribute in P2P IE. (Todo) */
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+	ssid_len &= 0xff;	/*	Just last 1 byte is valid for ssid len of the probe request */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen);
+		if (p2pie) {
+			if (p && !memcmp((void *)(p + 2), (void *)pwdinfo->p2p_wildcard_ssid, 7)) {
+				/* todo: */
+				/* Check Requested Device Type attributes in WSC IE. */
+				/* Check Device ID attribute in P2P IE */
+
+				ret = true;
+			} else if (p && ssid_len == 0) {
+				ret = true;
+			}
+		} else {
+			/* non -p2p device */
+		}
+	}
+
+	return ret;
+}
+
+u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
+{
+	u8 status_code = P2P_STATUS_SUCCESS;
+	u8 *pbuf, *pattr_content = NULL;
+	u32 attr_contentlen = 0;
+	u16 cap_attr = 0;
+	unsigned short	frame_type, ie_offset = 0;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+	__be16 be_tmp;
+	__le16 le_tmp;
+
+	if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+		return P2P_STATUS_FAIL_REQUEST_UNABLE;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ)
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	else /*  WIFI_REASSOCREQ */
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+
+	ies = pframe + WLAN_HDR_A3_LEN + ie_offset;
+	ies_len = len - WLAN_HDR_A3_LEN - ie_offset;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	if (!p2p_ie)
+		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
+
+	while (p2p_ie) {
+		/* Check P2P Capability ATTR */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
+			cap_attr = le16_to_cpu(le_tmp);
+			psta->dev_cap = cap_attr & 0xff;
+		}
+
+		/* Check Extended Listen Timing ATTR */
+
+		/* Check P2P Device Info ATTR */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint *)&attr_contentlen)) {
+			pattr_content = kzalloc(attr_contentlen, GFP_KERNEL);
+			pbuf = pattr_content;
+			if (pattr_content) {
+				u8 num_of_secdev_type;
+				u16 dev_name_len;
+
+				rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, pattr_content, (uint *)&attr_contentlen);
+
+				memcpy(psta->dev_addr,	pattr_content, ETH_ALEN);/* P2P Device Address */
+
+				pattr_content += ETH_ALEN;
+
+				memcpy(&be_tmp, pattr_content, 2);/* Config Methods */
+				psta->config_methods = be16_to_cpu(be_tmp);
+
+				pattr_content += 2;
+
+				memcpy(psta->primary_dev_type, pattr_content, 8);
+
+				pattr_content += 8;
+
+				num_of_secdev_type = *pattr_content;
+				pattr_content += 1;
+
+				if (num_of_secdev_type == 0) {
+					psta->num_of_secdev_type = 0;
+				} else {
+					u32 len;
+
+					psta->num_of_secdev_type = num_of_secdev_type;
+
+					len = (sizeof(psta->secdev_types_list) < (num_of_secdev_type * 8)) ?
+					      (sizeof(psta->secdev_types_list)) : (num_of_secdev_type * 8);
+
+					memcpy(psta->secdev_types_list, pattr_content, len);
+
+					pattr_content += (num_of_secdev_type * 8);
+				}
+
+				psta->dev_name_len = 0;
+				if (be16_to_cpu(*(__be16 *)pattr_content) == WPS_ATTR_DEVICE_NAME) {
+					dev_name_len = be16_to_cpu(*(__be16 *)(pattr_content + 2));
+
+					psta->dev_name_len = (sizeof(psta->dev_name) < dev_name_len) ? sizeof(psta->dev_name) : dev_name_len;
+
+					memcpy(psta->dev_name, pattr_content + 4, psta->dev_name_len);
+				}
+				kfree(pbuf);
+			}
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+
+	return status_code;
+}
+
+u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *frame_body;
+	u8 status, dialogToken;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+	status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+
+	p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+	if (p2p_ie) {
+		u8 groupid[38] = { 0x00 };
+		u8 dev_addr[ETH_ALEN] = { 0x00 };
+		u32	attr_contentlen = 0;
+
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+			if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
+			    !memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN, pwdinfo->p2p_group_ssid_len)) {
+				attr_contentlen = 0;
+				if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen)) {
+					struct list_head *phead, *plist;
+
+					spin_lock_bh(&pstapriv->asoc_list_lock);
+					phead = &pstapriv->asoc_list;
+					plist = phead->next;
+
+					/* look up sta asoc_queue */
+					while (phead != plist) {
+						psta = container_of(plist, struct sta_info, asoc_list);
+
+						plist = plist->next;
+
+						if (psta->is_p2p_device && (psta->dev_cap & P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
+						    !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) {
+							/* issue GO Discoverability Request */
+							issue_group_disc_req(pwdinfo, psta->hwaddr);
+							status = P2P_STATUS_SUCCESS;
+							break;
+						} else {
+							status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+						}
+					}
+					spin_unlock_bh(&pstapriv->asoc_list_lock);
+				} else {
+					status = P2P_STATUS_FAIL_INVALID_PARAM;
+				}
+			} else {
+				status = P2P_STATUS_FAIL_INVALID_PARAM;
+			}
+		}
+	}
+
+	/* issue Device Discoverability Response */
+	issue_p2p_devdisc_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+
+	return status == P2P_STATUS_SUCCESS;
+}
+
+u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	return true;
+}
+
+u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo,  u8 *pframe, uint len)
+{
+	u8 *frame_body;
+	u8 *wpsie;
+	uint	wps_ielen = 0, attr_contentlen = 0;
+	u16	uconfig_method = 0;
+	__be16 be_tmp;
+
+	frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	wpsie = rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+	if (wpsie) {
+		if (rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, (u8 *)&be_tmp, &attr_contentlen)) {
+			uconfig_method = be16_to_cpu(be_tmp);
+			switch (uconfig_method) {
+			case WPS_CM_DISPLYA:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+				break;
+			case WPS_CM_LABEL:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3);
+				break;
+			case WPS_CM_PUSH_BUTTON:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+				break;
+			case WPS_CM_KEYPAD:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+				break;
+			}
+			issue_p2p_provision_resp(pwdinfo, GetAddr2Ptr(pframe), frame_body, uconfig_method);
+		}
+	}
+	return true;
+}
+
+u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo,  u8 *pframe)
+{
+	return true;
+}
+
+static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
+{
+	u8 i = 0, j = 0;
+	u8 temp = 0;
+	u8 ch_no = 0;
+	ch_content += 3;
+	ch_cnt -= 3;
+
+	while (ch_cnt > 0) {
+		ch_content += 1;
+		ch_cnt -= 1;
+		temp = *ch_content;
+		for (i = 0 ; i < temp ; i++, j++)
+			peer_ch_list[j] = *(ch_content + 1 + i);
+		ch_content += (temp + 1);
+		ch_cnt -= (temp + 1);
+		ch_no += temp;
+	}
+
+	return ch_no;
+}
+
+static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
+{
+	int	i = 0, j = 0, temp = 0;
+	u8 ch_no = 0;
+
+	for (i = 0; i < peer_ch_num; i++) {
+		for (j = temp; j < pmlmeext->max_chan_nums; j++) {
+			if (*(peer_ch_list + i) == pmlmeext->channel_set[j].ChannelNum) {
+				ch_list_inclusioned[ch_no++] = *(peer_ch_list + i);
+				temp = j;
+				break;
+			}
+		}
+	}
+
+	return ch_no;
+}
+
+u8 process_p2p_group_negotation_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	struct adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen = 0, wps_ielen = 0;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u8 *wpsie;
+	u16		wps_devicepassword_id = 0x0000;
+	uint	wps_devicepassword_id_len = 0;
+	__be16 be_tmp;
+
+	wpsie = rtw_get_wps_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+	if (wpsie) {
+		/*	Commented by Kurt 20120113 */
+		/*	If some device wants to do p2p handshake without sending prov_disc_req */
+		/*	We have to get peer_req_cm from here. */
+		if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
+			rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
+			wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+			if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+			else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+			else
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+		}
+	} else {
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+		return result;
+	}
+
+	if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) {
+		result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
+		return result;
+	}
+
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	if (!p2p_ie) {
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+	}
+
+	while (p2p_ie) {
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+		u8	ch_content[50] = { 0x00 };
+		uint	ch_cnt = 0;
+		u8	peer_ch_list[50] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[50] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) {
+			pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
+
+			if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
+				/*	Try to match the tie breaker value */
+				if (pwdinfo->intent == P2P_MAX_INTENT) {
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+				} else {
+					if (attr_content & 0x01)
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+					else
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				}
+			} else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+			} else {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+			}
+
+			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+				/*	Store the group id information. */
+				memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+				memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+			}
+		}
+
+		attr_contentlen = 0;
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+			if (attr_contentlen != ETH_ALEN)
+				memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+		}
+
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt)) {
+			peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
+			ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+			if (ch_num_inclusioned == 0) {
+				result = P2P_STATUS_FAIL_NO_COMMON_CH;
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+				break;
+			}
+
+			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+				if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
+				    ch_list_inclusioned, ch_num_inclusioned)) {
+					u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+					attr_contentlen = 0;
+
+					if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+						peer_operating_ch = operatingch_info[4];
+
+					if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+								       ch_list_inclusioned,
+								       ch_num_inclusioned))
+						/**
+						 *	Change our operating channel as peer's for compatibility.
+						 */
+						pwdinfo->operating_channel = peer_operating_ch;
+					else
+						/*  Take first channel of ch_list_inclusioned as operating channel */
+						pwdinfo->operating_channel = ch_list_inclusioned[0];
+				}
+			}
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+	return result;
+}
+
+u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	struct adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen, wps_ielen;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	/*	Be able to know which one is the P2P GO and which one is P2P client. */
+
+	if (!rtw_get_wps_ie(ies, ies_len, NULL, &wps_ielen)) {
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+	}
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+	if (!p2p_ie) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+	} else {
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+		u8	operatingch_info[5] = { 0x00 };
+		u8	groupid[38];
+		u8	peer_ch_list[50] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[50] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+
+		while (p2p_ie) {	/*	Found the P2P IE. */
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+			if (attr_contentlen == 1) {
+				if (attr_content == P2P_STATUS_SUCCESS) {
+					/*	Do nothing. */
+				} else {
+					if (attr_content == P2P_STATUS_FAIL_INFO_UNAVAILABLE) {
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
+					} else {
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					}
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					result = attr_content;
+					break;
+				}
+			}
+
+			/*	Try to get the peer's interface address */
+			attr_contentlen = 0;
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+				if (attr_contentlen != ETH_ALEN)
+					memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+			}
+
+			/*	Try to get the peer's intent and tie breaker value. */
+			attr_content = 0x00;
+			attr_contentlen = 0;
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) {
+				pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
+
+				if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
+					/*	Try to match the tie breaker value */
+					if (pwdinfo->intent == P2P_MAX_INTENT) {
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+						result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					} else {
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+						rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+						if (attr_content & 0x01)
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+						else
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+					}
+				} else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				} else {
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+				}
+
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+					/*	Store the group id information. */
+					memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+					memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+				}
+			}
+
+			/*	Try to get the operation channel information */
+
+			attr_contentlen = 0;
+			if (rtw_get_p2p_attr_content(p2p_ie,
+						     p2p_ielen,
+						     P2P_ATTR_OPERATING_CH,
+						     operatingch_info,
+						     &attr_contentlen))
+				pwdinfo->peer_operating_ch = operatingch_info[4];
+
+			/*	Try to get the channel list information */
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) {
+
+				peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
+				ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+				if (ch_num_inclusioned == 0) {
+					result = P2P_STATUS_FAIL_NO_COMMON_CH;
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					break;
+				}
+
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+					if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
+					    ch_list_inclusioned, ch_num_inclusioned)) {
+						u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+						attr_contentlen = 0;
+
+						if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+							peer_operating_ch = operatingch_info[4];
+
+						if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+						    ch_list_inclusioned, ch_num_inclusioned))
+							/**
+							 *	Change our operating channel as peer's for compatibility.
+							 */
+							pwdinfo->operating_channel = peer_operating_ch;
+						else
+							/*  Take first channel of ch_list_inclusioned as operating channel */
+							pwdinfo->operating_channel = ch_list_inclusioned[0];
+					}
+				}
+			}
+
+			/*	Try to get the group id information if peer is GO */
+			attr_contentlen = 0;
+			memset(groupid, 0x00, 38);
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+				memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+				memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
+			}
+
+			/* Get the next P2P IE */
+			p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+		}
+	}
+	return result;
+}
+
+u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	result = P2P_STATUS_SUCCESS;
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+	while (p2p_ie) {	/*	Found the P2P IE. */
+		u8	attr_content = 0x00, operatingch_info[5] = { 0x00 };
+		u8	groupid[38] = { 0x00 };
+		u32	attr_contentlen = 0;
+
+		pwdinfo->negotiation_dialog_token = 1;
+		rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+		if (attr_contentlen == 1) {
+			result = attr_content;
+
+			if (attr_content == P2P_STATUS_SUCCESS) {
+				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
+
+				/*	Commented by Albert 20100911 */
+				/*	Todo: Need to handle the case which both Intents are the same. */
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+				rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+				if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1)) {
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				} else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1)) {
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+				} else {
+					/*	Have to compare the Tie Breaker */
+					if (pwdinfo->peer_intent & 0x01)
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+					else
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				}
+			} else {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+				break;
+			}
+		}
+
+		/*	Try to get the group id information */
+		attr_contentlen = 0;
+		memset(groupid, 0x00, 38);
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+			memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+			memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
+		}
+
+		attr_contentlen = 0;
+		if (rtw_get_p2p_attr_content(p2p_ie,
+					     p2p_ielen,
+					     P2P_ATTR_OPERATING_CH,
+					     operatingch_info,
+					     &attr_contentlen))
+			pwdinfo->peer_operating_ch = operatingch_info[4];
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+	return result;
+}
+
+u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *frame_body;
+	u8 dialogToken = 0;
+	u8 status = P2P_STATUS_SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[6];
+
+	/* todo: check NoA attribute */
+
+	issue_p2p_presence_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+
+	return true;
+}
+
+static void find_phase_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ndis_802_11_ssid	ssid;
+
+	memset((unsigned char *)&ssid, 0, sizeof(struct ndis_802_11_ssid));
+	memcpy(ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
+	ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
+
+	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
+
+	spin_lock_bh(&pmlmepriv->lock);
+	spin_unlock_bh(&pmlmepriv->lock);
+
+}
+
+void p2p_concurrent_handler(struct adapter *padapter);
+
+static void restore_p2p_state_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+		/*	In the P2P client mode, the driver should not switch back to its listen channel */
+		/*	because this P2P client should stay at the operating channel of P2P GO. */
+		set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	}
+
+}
+
+static void pre_tx_invitereq_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+
+	set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	rtw_mlme_under_site_survey(padapter);
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+static void pre_tx_provdisc_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+
+	set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	rtw_mlme_under_site_survey(padapter);
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+static void pre_tx_negoreq_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+
+	set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	rtw_mlme_under_site_survey(padapter);
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+void p2p_protocol_wk_hdl(struct adapter *padapter, int intCmdType)
+{
+
+	switch (intCmdType) {
+	case P2P_FIND_PHASE_WK:
+		find_phase_handler(padapter);
+		break;
+	case P2P_RESTORE_STATE_WK:
+		restore_p2p_state_handler(padapter);
+		break;
+	case P2P_PRE_TX_PROVDISC_PROCESS_WK:
+		pre_tx_provdisc_handler(padapter);
+		break;
+	case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
+		pre_tx_invitereq_handler(padapter);
+		break;
+	case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
+		pre_tx_negoreq_handler(padapter);
+		break;
+	}
+
+}
+
+void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
+{
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/*  NoA length should be n*(13) + 2 */
+	u32	attr_contentlen = 0;
+
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8	find_p2p = false, find_p2p_ps = false;
+	u8	noa_offset, noa_num, noa_index;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+	if (IELength <= _BEACON_IE_OFFSET_)
+		return;
+
+	ies = IEs + _BEACON_IE_OFFSET_;
+	ies_len = IELength - _BEACON_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		find_p2p = true;
+		/*  Get Notice of Absence IE. */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen)) {
+			find_p2p_ps = true;
+			noa_index = noa_attr[0];
+
+			if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
+			    (noa_index != pwdinfo->noa_index)) { /*  if index change, driver should reconfigure related setting. */
+				pwdinfo->noa_index = noa_index;
+				pwdinfo->opp_ps = noa_attr[1] >> 7;
+				pwdinfo->ctwindow = noa_attr[1] & 0x7F;
+
+				noa_offset = 2;
+				noa_num = 0;
+				/*  NoA length should be n*(13) + 2 */
+				if (attr_contentlen > 2) {
+					while (noa_offset < attr_contentlen) {
+						/* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
+						pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
+						noa_offset += 1;
+
+						memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						noa_num++;
+					}
+				}
+				pwdinfo->noa_num = noa_num;
+
+				if (pwdinfo->opp_ps == 1) {
+					pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/*  driver should wait LPS for entering CTWindow */
+					if (padapter->pwrctrlpriv.bFwCurrentInPSMode)
+						p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+				} else if (pwdinfo->noa_num > 0) {
+					pwdinfo->p2p_ps_mode = P2P_PS_NOA;
+					p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+				} else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+					p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+				}
+			}
+
+			break; /*  find target, just break. */
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+
+	if (find_p2p) {
+		if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && !find_p2p_ps)
+			p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+	}
+
+}
+
+void p2p_ps_wk_hdl(struct adapter *padapter, u8 p2p_ps_state)
+{
+	struct pwrctrl_priv		*pwrpriv = &padapter->pwrctrlpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	/*  Pre action for p2p state */
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		pwdinfo->p2p_ps_state = p2p_ps_state;
+
+		rtl8188e_set_p2p_ps_offload_cmd(padapter, p2p_ps_state);
+
+		pwdinfo->noa_index = 0;
+		pwdinfo->ctwindow = 0;
+		pwdinfo->opp_ps = 0;
+		pwdinfo->noa_num = 0;
+		pwdinfo->p2p_ps_mode = P2P_PS_NONE;
+		if (padapter->pwrctrlpriv.bFwCurrentInPSMode) {
+			if (pwrpriv->smart_ps == 0) {
+				pwrpriv->smart_ps = 2;
+				rtw_set_firmware_ps_mode(padapter, pwrpriv->pwr_mode);
+			}
+		}
+		break;
+	case P2P_PS_ENABLE:
+		if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+			pwdinfo->p2p_ps_state = p2p_ps_state;
+
+			if (pwdinfo->ctwindow > 0) {
+				if (pwrpriv->smart_ps != 0) {
+					pwrpriv->smart_ps = 0;
+					rtw_set_firmware_ps_mode(padapter, pwrpriv->pwr_mode);
+				}
+			}
+			rtl8188e_set_p2p_ps_offload_cmd(padapter, p2p_ps_state);
+		}
+		break;
+	case P2P_PS_SCAN:
+	case P2P_PS_SCAN_DONE:
+	case P2P_PS_ALLSTASLEEP:
+		if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+			pwdinfo->p2p_ps_state = p2p_ps_state;
+			rtl8188e_set_p2p_ps_offload_cmd(padapter, p2p_ps_state);
+		}
+		break;
+	default:
+		break;
+	}
+
+}
+
+u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return res;
+
+	if (enqueue) {
+		ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+		if (!pdrvextra_cmd_parm) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
+		pdrvextra_cmd_parm->type_size = p2p_ps_state;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		p2p_ps_wk_hdl(padapter, p2p_ps_state);
+	}
+
+exit:
+
+	return res;
+}
+
+static void reset_ch_sitesurvey_timer_process(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, pwrctrlpriv.pwr_state_check_timer);
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	/*	Reset the operation channel information */
+	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+}
+
+static void reset_ch_sitesurvey_timer_process2(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, pwrctrlpriv.pwr_state_check_timer);
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	/*	Reset the operation channel information */
+	pwdinfo->p2p_info.operation_ch[0] = 0;
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+static void restore_p2p_state_timer_process(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, wdinfo.restore_p2p_state_timer);
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	p2p_protocol_wk_cmd(adapter, P2P_RESTORE_STATE_WK);
+}
+
+static void pre_tx_scan_timer_process(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, wdinfo.pre_tx_scan_timer);
+	struct	wifidirect_info *pwdinfo = &adapter->wdinfo;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+		if (pwdinfo->tx_prov_disc_info.benable) {	/*	the provision discovery request frame is trigger to send or not */
+			p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
+			/* issue_probereq_p2p(adapter, NULL); */
+			/* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
+		}
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+		if (pwdinfo->nego_req_info.benable)
+			p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
+		if (pwdinfo->invitereq_info.benable)
+			p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static void find_phase_timer_process(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, wdinfo.find_phase_timer);
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	adapter->wdinfo.find_phase_state_exchange_cnt++;
+
+	p2p_protocol_wk_cmd(adapter, P2P_FIND_PHASE_WK);
+}
+
+void reset_global_wifidirect_info(struct adapter *padapter)
+{
+	struct wifidirect_info	*pwdinfo;
+
+	pwdinfo = &padapter->wdinfo;
+	pwdinfo->persistent_supported = 0;
+	pwdinfo->session_available = true;
+}
+
+void rtw_init_wifidirect_timers(struct adapter *padapter)
+{
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	timer_setup(&pwdinfo->find_phase_timer, find_phase_timer_process, 0);
+	timer_setup(&pwdinfo->restore_p2p_state_timer, restore_p2p_state_timer_process, 0);
+	timer_setup(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process, 0);
+	timer_setup(&pwdinfo->reset_ch_sitesurvey, reset_ch_sitesurvey_timer_process, 0);
+	timer_setup(&pwdinfo->reset_ch_sitesurvey2, reset_ch_sitesurvey_timer_process2, 0);
+}
+
+void rtw_init_wifidirect_addrs(struct adapter *padapter, u8 *dev_addr, u8 *iface_addr)
+{
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*init device&interface address */
+	if (dev_addr)
+		memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
+	if (iface_addr)
+		memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
+}
+
+void init_wifidirect_info(struct adapter *padapter, enum P2P_ROLE role)
+{
+	struct wifidirect_info	*pwdinfo;
+
+	pwdinfo = &padapter->wdinfo;
+	pwdinfo->padapter = padapter;
+
+	/*	1, 6, 11 are the social channel defined in the WiFi Direct specification. */
+	pwdinfo->social_chan[0] = 1;
+	pwdinfo->social_chan[1] = 6;
+	pwdinfo->social_chan[2] = 11;
+	pwdinfo->social_chan[3] = 0;	/*	channel 0 for scanning ending in site survey function. */
+
+	/*	Use the channel 11 as the listen channel */
+	pwdinfo->listen_channel = 11;
+
+	if (role == P2P_ROLE_DEVICE) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
+	} else if (role == P2P_ROLE_CLIENT) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	} else if (role == P2P_ROLE_GO) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 15;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	}
+
+/*	Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
+	pwdinfo->support_rate[0] = 0x8c;	/*	6(B) */
+	pwdinfo->support_rate[1] = 0x92;	/*	9(B) */
+	pwdinfo->support_rate[2] = 0x18;	/*	12 */
+	pwdinfo->support_rate[3] = 0x24;	/*	18 */
+	pwdinfo->support_rate[4] = 0x30;	/*	24 */
+	pwdinfo->support_rate[5] = 0x48;	/*	36 */
+	pwdinfo->support_rate[6] = 0x60;	/*	48 */
+	pwdinfo->support_rate[7] = 0x6c;	/*	54 */
+
+	memcpy(pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
+
+	memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
+	pwdinfo->device_name_len = 0;
+
+	memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
+	pwdinfo->invitereq_info.token = 3;	/*	Token used for P2P invitation request frame. */
+
+	memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
+	pwdinfo->inviteresp_info.token = 0;
+
+	pwdinfo->profileindex = 0;
+	memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
+
+	rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+
+	pwdinfo->listen_dwell = (u8)((jiffies % 3) + 1);
+
+	memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
+	pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
+
+	memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
+
+	pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
+	pwdinfo->negotiation_dialog_token = 1;
+
+	memset(pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN);
+	pwdinfo->nego_ssidlen = 0;
+
+	pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
+	pwdinfo->channel_list_attr_len = 0;
+	memset(pwdinfo->channel_list_attr, 0x00, 100);
+
+	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
+	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
+	memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+	memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+	memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
+
+	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[1] = 0;	/*	Used to indicate the scan end in site survey function */
+	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+	pwdinfo->p2p_info.operation_ch[0] = 0;
+	pwdinfo->p2p_info.operation_ch[1] = 0;			/*	Used to indicate the scan end in site survey function */
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+int rtw_p2p_enable(struct adapter *padapter, enum P2P_ROLE role)
+{
+	int ret;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) {
+		/* leave IPS/Autosuspend */
+		ret = rtw_pwr_wakeup(padapter);
+		if (ret)
+			return ret;
+
+		/*	Added by Albert 2011/03/22 */
+		/*	In the P2P mode, the driver should not support the b mode. */
+		/*	So, the Tx packet shouldn't use the CCK rate */
+		update_tx_basic_rate(padapter, (WIRELESS_11G | WIRELESS_11_24N));
+
+		/* Enable P2P function */
+		init_wifidirect_info(padapter, role);
+
+	} else if (role == P2P_ROLE_DISABLE) {
+		ret = rtw_pwr_wakeup(padapter);
+		if (ret)
+			return ret;
+
+		/* Disable P2P function */
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+			_cancel_timer_ex(&pwdinfo->find_phase_timer);
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			_cancel_timer_ex(&pwdinfo->pre_tx_scan_timer);
+			_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+			_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey2);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
+			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
+			memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
+		}
+
+		/* Restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_pwrctrl.c b/drivers/staging/r8188eu/core/rtw_pwrctrl.c
new file mode 100644
index 000000000..5290ac36f
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_pwrctrl.c
@@ -0,0 +1,448 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_PWRCTRL_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/osdep_intf.h"
+#include "../include/linux/usb.h"
+
+void ips_enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct xmit_priv *pxmit_priv = &padapter->xmitpriv;
+
+	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+	    pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF)
+		return;
+
+	mutex_lock(&pwrpriv->lock);
+
+	pwrpriv->bips_processing = true;
+
+	/*  syn ips_mode with request */
+	pwrpriv->ips_mode = pwrpriv->ips_mode_req;
+
+	pwrpriv->ips_enter_cnts++;
+	if (rf_off == pwrpriv->change_rfpwrstate) {
+		pwrpriv->bpower_saving = true;
+
+		if (pwrpriv->ips_mode == IPS_LEVEL_2)
+			pwrpriv->bkeepfwalive = true;
+
+		rtw_ips_pwr_down(padapter);
+		pwrpriv->rf_pwrstate = rf_off;
+	}
+	pwrpriv->bips_processing = false;
+
+	mutex_unlock(&pwrpriv->lock);
+}
+
+int ips_leave(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	int result = _SUCCESS;
+	int keyid;
+
+	mutex_lock(&pwrpriv->lock);
+
+	if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) {
+		pwrpriv->bips_processing = true;
+		pwrpriv->change_rfpwrstate = rf_on;
+		pwrpriv->ips_leave_cnts++;
+
+		result = rtw_ips_pwr_up(padapter);
+		if (result == _SUCCESS) {
+			pwrpriv->rf_pwrstate = rf_on;
+		}
+
+		if ((psecuritypriv->dot11PrivacyAlgrthm == _WEP40_) || (psecuritypriv->dot11PrivacyAlgrthm == _WEP104_)) {
+			set_channel_bwmode(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			for (keyid = 0; keyid < 4; keyid++) {
+				if (pmlmepriv->key_mask & BIT(keyid)) {
+					if (keyid == psecuritypriv->dot11PrivacyKeyIndex)
+						result = rtw_set_key(padapter, psecuritypriv, keyid, 1);
+					else
+						result = rtw_set_key(padapter, psecuritypriv, keyid, 0);
+				}
+			}
+		}
+
+		pwrpriv->bips_processing = false;
+
+		pwrpriv->bkeepfwalive = false;
+		pwrpriv->bpower_saving = false;
+	}
+
+	mutex_unlock(&pwrpriv->lock);
+
+	return result;
+}
+
+static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
+{
+	struct adapter *buddy = adapter->pbuddy_adapter;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wifidirect_info	*pwdinfo = &adapter->wdinfo;
+	bool ret = false;
+
+	if (time_after_eq(adapter->pwrctrlpriv.ips_deny_time, jiffies))
+		goto exit;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_SITE_MONITOR) ||
+	    check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS) ||
+	    check_fwstate(pmlmepriv, WIFI_UNDER_WPS) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE) ||
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		goto exit;
+
+	/* consider buddy, if exist */
+	if (buddy) {
+		struct mlme_priv *b_pmlmepriv = &buddy->mlmepriv;
+		struct wifidirect_info *b_pwdinfo = &buddy->wdinfo;
+
+		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE | WIFI_SITE_MONITOR) ||
+		    check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS) ||
+		    check_fwstate(b_pmlmepriv, WIFI_AP_STATE) ||
+		    check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE) ||
+		    !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE))
+			goto exit;
+	}
+	ret = true;
+
+exit:
+	return ret;
+}
+
+void rtw_ps_processor(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	pwrpriv->ps_processing = true;
+
+	if (pwrpriv->bips_processing)
+		goto exit;
+
+	if (pwrpriv->ips_mode_req == IPS_NONE)
+		goto exit;
+
+	if (!rtw_pwr_unassociated_idle(padapter))
+		goto exit;
+
+	if (pwrpriv->rf_pwrstate == rf_on) {
+		pwrpriv->change_rfpwrstate = rf_off;
+		ips_enter(padapter);
+	}
+exit:
+	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
+	pwrpriv->ps_processing = false;
+}
+
+static void pwr_state_check_handler(struct timer_list *t)
+{
+	struct adapter *padapter =
+		from_timer(padapter, t,
+			   pwrctrlpriv.pwr_state_check_timer);
+	rtw_ps_cmd(padapter);
+}
+
+static bool PS_RDY_CHECK(struct adapter *padapter)
+{
+	u32 curr_time, delta_time;
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	curr_time = jiffies;
+	delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp;
+
+	if (delta_time < LPS_DELAY_TIME)
+		return false;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) ||
+	    check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
+		return false;
+	if (pwrpriv->bInSuspend)
+		return false;
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X &&
+	    !padapter->securitypriv.binstallGrpkey)
+		return false;
+	return true;
+}
+
+void rtw_set_firmware_ps_mode(struct adapter *adapter, u8 mode)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	/* Force leave RF low power mode for 1T1R to prevent
+	 * conflicting setting in firmware power saving sequence.
+	 */
+	if (mode != PS_MODE_ACTIVE)
+		ODM_RF_Saving(odmpriv, true);
+	rtl8188e_set_FwPwrMode_cmd(adapter, mode);
+}
+
+void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	if (ps_mode > PM_Card_Disable)
+		return;
+
+	if (pwrpriv->pwr_mode == ps_mode) {
+		if (ps_mode == PS_MODE_ACTIVE)
+			return;
+
+		if ((pwrpriv->smart_ps == smart_ps) &&
+		    (pwrpriv->bcn_ant_mode == bcn_ant_mode))
+			return;
+	}
+
+	if (ps_mode == PS_MODE_ACTIVE) {
+		if (pwdinfo->opp_ps == 0) {
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_firmware_ps_mode(padapter, ps_mode);
+			pwrpriv->bFwCurrentInPSMode = false;
+		}
+	} else {
+		if (PS_RDY_CHECK(padapter)) {
+			pwrpriv->bFwCurrentInPSMode = true;
+			pwrpriv->pwr_mode = ps_mode;
+			pwrpriv->smart_ps = smart_ps;
+			pwrpriv->bcn_ant_mode = bcn_ant_mode;
+			rtw_set_firmware_ps_mode(padapter, ps_mode);
+
+			/*  Set CTWindow after LPS */
+			if (pwdinfo->opp_ps == 1)
+				p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0);
+		}
+	}
+}
+
+static bool lps_rf_on(struct adapter *adapter)
+{
+	int res;
+	u32 reg;
+
+	/* When we halt NIC, we should check if FW LPS is leave. */
+	if (adapter->pwrctrlpriv.rf_pwrstate == rf_off) {
+		/*  If it is in HW/SW Radio OFF or IPS state, we do not check Fw LPS Leave, */
+		/*  because Fw is unload. */
+		return true;
+	}
+
+	res = rtw_read32(adapter, REG_RCR, &reg);
+	if (res)
+		return false;
+
+	if (reg & 0x00070000)
+		return false;
+
+	return true;
+}
+
+/*
+ * Return:
+ *	0:	Leave OK
+ *	-1:	Timeout
+ *	-2:	Other error
+ */
+static s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(delay_ms);
+	s32 err = 0;
+
+	while (1) {
+		if (lps_rf_on(padapter))
+			break;
+
+		if (padapter->bSurpriseRemoved) {
+			err = -2;
+			break;
+		}
+
+		if (time_after(jiffies, timeout)) {
+			err = -1;
+			break;
+		}
+		mdelay(1);
+	}
+
+	return err;
+}
+
+/*  */
+/*	Description: */
+/*		Enter the leisure power save mode. */
+/*  */
+void LPS_Enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+
+	if (!PS_RDY_CHECK(padapter))
+		return;
+
+	if (pwrpriv->bLeisurePs) {
+		/*  Idle for a while if we connect to AP a while ago. */
+		if (pwrpriv->LpsIdleCount >= 2) { /*   4 Sec */
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+				pwrpriv->bpower_saving = true;
+				/* For Tenda W311R IOT issue */
+				rtw_set_ps_mode(padapter, pwrpriv->power_mgnt,
+						pwrpriv->smart_ps, 0x40);
+			}
+		} else {
+			pwrpriv->LpsIdleCount++;
+		}
+	}
+
+}
+
+#define LPS_LEAVE_TIMEOUT_MS 100
+
+/*	Description: */
+/*		Leave the leisure power save mode. */
+void LPS_Leave(struct adapter *padapter)
+{
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+
+	if (pwrpriv->bLeisurePs) {
+		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+			rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0x40);
+
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+				LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
+		}
+	}
+
+	pwrpriv->bpower_saving = false;
+
+}
+
+/*  */
+/*  Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */
+/*  Move code to function by tynli. 2010.03.26. */
+/*  */
+void LeaveAllPowerSaveMode(struct adapter *Adapter)
+{
+	struct mlme_priv	*pmlmepriv = &Adapter->mlmepriv;
+	u8	enqueue = 0;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) { /* connect */
+		p2p_ps_wk_cmd(Adapter, P2P_PS_DISABLE, enqueue);
+
+		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+	}
+
+}
+
+void rtw_init_pwrctrl_priv(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	mutex_init(&pwrctrlpriv->lock);
+	pwrctrlpriv->rf_pwrstate = rf_on;
+	pwrctrlpriv->ips_enter_cnts = 0;
+	pwrctrlpriv->ips_leave_cnts = 0;
+	pwrctrlpriv->bips_processing = false;
+
+	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+	pwrctrlpriv->bInSuspend = false;
+	pwrctrlpriv->bkeepfwalive = false;
+
+	pwrctrlpriv->LpsIdleCount = 0;
+	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
+	pwrctrlpriv->bLeisurePs = pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
+
+	pwrctrlpriv->bFwCurrentInPSMode = false;
+
+	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+	pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
+	pwrctrlpriv->bcn_ant_mode = 0;
+
+	timer_setup(&pwrctrlpriv->pwr_state_check_timer, pwr_state_check_handler, 0);
+}
+
+/* Wake the NIC up from: 1)IPS 2)USB autosuspend */
+int rtw_pwr_wakeup(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	unsigned long timeout = jiffies + msecs_to_jiffies(3000);
+	unsigned long deny_time;
+	int ret;
+
+	while (pwrpriv->ps_processing && time_before(jiffies, timeout))
+		msleep(10);
+
+	/* I think this should be check in IPS, LPS, autosuspend functions... */
+	/* Below goto is a success path taken for already linked devices */
+	ret = 0;
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		goto exit;
+
+	if (pwrpriv->rf_pwrstate == rf_off && ips_leave(padapter) == _FAIL) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	if (padapter->bDriverStopped || !padapter->bup || !padapter->hw_init_completed) {
+		ret = -EBUSY;
+		goto exit;
+	}
+
+exit:
+	deny_time = jiffies + msecs_to_jiffies(RTW_PWR_STATE_CHK_INTERVAL);
+	if (time_before(pwrpriv->ips_deny_time, deny_time))
+		pwrpriv->ips_deny_time = deny_time;
+	return ret;
+}
+
+int rtw_pm_set_lps(struct adapter *padapter, u8 mode)
+{
+	int	ret = 0;
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	if (mode < PS_MODE_NUM) {
+		if (pwrctrlpriv->power_mgnt != mode) {
+			if (mode == PS_MODE_ACTIVE)
+				LeaveAllPowerSaveMode(padapter);
+			else
+				pwrctrlpriv->LpsIdleCount = 2;
+			pwrctrlpriv->power_mgnt = mode;
+			pwrctrlpriv->bLeisurePs = pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
+		}
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int rtw_pm_set_ips(struct adapter *padapter, u8 mode)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		return 0;
+	} else if (mode == IPS_NONE) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		if ((padapter->bSurpriseRemoved == 0) && rtw_pwr_wakeup(padapter))
+			return -EFAULT;
+	} else {
+		return -EINVAL;
+	}
+	return 0;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_recv.c b/drivers/staging/r8188eu/core/rtw_recv.c
new file mode 100644
index 000000000..bb5c3b388
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_recv.c
@@ -0,0 +1,2021 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_RECV_C_
+
+#include <linux/ieee80211.h>
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/usb_ops.h"
+#include "../include/wifi.h"
+#include "../include/rtl8188e_recv.h"
+
+static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static u8 rtw_bridge_tunnel_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8
+};
+
+static u8 rtw_rfc1042_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
+};
+
+static void rtw_signal_stat_timer_hdl(struct timer_list *t);
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+
+	memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv));
+
+	spin_lock_init(&psta_recvpriv->lock);
+
+	rtw_init_queue(&psta_recvpriv->defrag_q);
+
+}
+
+static int rtl8188eu_init_recv_priv(struct adapter *padapter)
+{
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	int i, res = _SUCCESS;
+	struct recv_buf *precvbuf;
+
+	tasklet_init(&precvpriv->recv_tasklet,
+		     rtl8188eu_recv_tasklet,
+		     (unsigned long)padapter);
+
+	/* init recv_buf */
+	rtw_init_queue(&precvpriv->free_recv_buf_queue);
+
+	precvpriv->pallocated_recv_buf = kzalloc(NR_RECVBUFF * sizeof(struct recv_buf) + 4,
+						 GFP_KERNEL);
+	if (!precvpriv->pallocated_recv_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	precvpriv->precv_buf = (u8 *)ALIGN((size_t)(precvpriv->pallocated_recv_buf), 4);
+
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		precvbuf->pskb = NULL;
+		precvbuf->reuse = false;
+		precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!precvbuf->purb) {
+			res = _FAIL;
+			break;
+		}
+		precvbuf->adapter = padapter;
+		precvbuf++;
+	}
+	precvpriv->free_recv_buf_queue_cnt = NR_RECVBUFF;
+	skb_queue_head_init(&precvpriv->rx_skb_queue);
+	{
+		int i;
+		size_t tmpaddr = 0;
+		size_t alignment = 0;
+		struct sk_buff *pskb = NULL;
+
+		skb_queue_head_init(&precvpriv->free_recv_skb_queue);
+
+		for (i = 0; i < NR_PREALLOC_RECV_SKB; i++) {
+			pskb = __netdev_alloc_skb(padapter->pnetdev,
+						  MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ, GFP_KERNEL);
+			if (pskb) {
+				pskb->dev = padapter->pnetdev;
+				tmpaddr = (size_t)pskb->data;
+				alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+				skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignment));
+
+				skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+			}
+			pskb = NULL;
+		}
+	}
+exit:
+	return res;
+}
+
+int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
+{
+	int i;
+
+	struct recv_frame *precvframe;
+
+	int	res = _SUCCESS;
+
+	spin_lock_init(&precvpriv->lock);
+
+	rtw_init_queue(&precvpriv->free_recv_queue);
+	rtw_init_queue(&precvpriv->recv_pending_queue);
+	rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
+
+	precvpriv->adapter = padapter;
+
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+	precvpriv->pallocated_frame_buf = vzalloc(NR_RECVFRAME * sizeof(struct recv_frame) + RXFRAME_ALIGN_SZ);
+
+	if (!precvpriv->pallocated_frame_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	precvpriv->precv_frame_buf = (u8 *)ALIGN((size_t)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ);
+
+	precvframe = (struct recv_frame *)precvpriv->precv_frame_buf;
+
+	for (i = 0; i < NR_RECVFRAME; i++) {
+		INIT_LIST_HEAD(&precvframe->list);
+
+		list_add_tail(&precvframe->list, &precvpriv->free_recv_queue.queue);
+
+		precvframe->pkt = NULL;
+
+		precvframe->len = 0;
+
+		precvframe->adapter = padapter;
+		precvframe++;
+	}
+	precvpriv->rx_pending_cnt = 1;
+
+	res = rtl8188eu_init_recv_priv(padapter);
+
+	timer_setup(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl, 0);
+	precvpriv->signal_stat_sampling_interval = 1000; /* ms */
+
+	rtw_set_signal_stat_timer(precvpriv);
+exit:
+
+	return res;
+}
+
+static void rtl8188eu_free_recv_priv(struct adapter *padapter)
+{
+	int i;
+	struct recv_buf *precvbuf;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		usb_free_urb(precvbuf->purb);
+		precvbuf++;
+	}
+
+	kfree(precvpriv->pallocated_recv_buf);
+
+	skb_queue_purge(&precvpriv->rx_skb_queue);
+
+	skb_queue_purge(&precvpriv->free_recv_skb_queue);
+}
+
+void _rtw_free_recv_priv(struct recv_priv *precvpriv)
+{
+	struct adapter	*padapter = precvpriv->adapter;
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	vfree(precvpriv->pallocated_frame_buf);
+
+	rtl8188eu_free_recv_priv(padapter);
+	_cancel_timer_ex(&precvpriv->signal_stat_timer);
+}
+
+struct recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+	struct recv_frame *hdr;
+	struct list_head *plist, *phead;
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	if (list_empty(&pfree_recv_queue->queue)) {
+		hdr = NULL;
+	} else {
+		phead = get_list_head(pfree_recv_queue);
+
+		plist = phead->next;
+
+		hdr = container_of(plist, struct recv_frame, list);
+
+		list_del_init(&hdr->list);
+		padapter = hdr->adapter;
+		if (padapter) {
+			precvpriv = &padapter->recvpriv;
+			if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt--;
+		}
+	}
+
+	return (struct recv_frame *)hdr;
+}
+
+struct recv_frame *rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+	struct recv_frame  *precvframe;
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+	spin_unlock_bh(&pfree_recv_queue->lock);
+
+	return precvframe;
+}
+
+int rtw_free_recvframe(struct recv_frame *precvframe, struct __queue *pfree_recv_queue)
+{
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	if (!precvframe)
+		return _FAIL;
+	padapter = precvframe->adapter;
+	precvpriv = &padapter->recvpriv;
+	if (precvframe->pkt) {
+		dev_kfree_skb_any(precvframe->pkt);/* free skb by driver */
+		precvframe->pkt = NULL;
+	}
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	list_del_init(&precvframe->list);
+
+	precvframe->len = 0;
+
+	list_add_tail(&precvframe->list, get_list_head(pfree_recv_queue));
+
+	if (padapter && (pfree_recv_queue == &precvpriv->free_recv_queue))
+		precvpriv->free_recvframe_cnt++;
+
+	spin_unlock_bh(&pfree_recv_queue->lock);
+
+	return _SUCCESS;
+}
+
+int _rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue)
+{
+	struct adapter *padapter = precvframe->adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	list_del_init(&precvframe->list);
+	list_add_tail(&precvframe->list, get_list_head(queue));
+
+	if (padapter) {
+		if (queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+	}
+
+	return _SUCCESS;
+}
+
+int rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue)
+{
+	int ret;
+
+	spin_lock_bh(&queue->lock);
+	ret = _rtw_enqueue_recvframe(precvframe, queue);
+	spin_unlock_bh(&queue->lock);
+
+	return ret;
+}
+
+/*
+ * caller : defrag ; recvframe_chk_defrag in recv_thread  (passive)
+ * pframequeue: defrag_queue : will be accessed in recv_thread  (passive)
+ *
+ * using spinlock to protect
+ *
+ */
+
+void rtw_free_recvframe_queue(struct __queue *pframequeue,  struct __queue *pfree_recv_queue)
+{
+	struct recv_frame *hdr;
+	struct list_head *plist, *phead;
+
+	spin_lock(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		hdr = container_of(plist, struct recv_frame, list);
+
+		plist = plist->next;
+
+		rtw_free_recvframe((struct recv_frame *)hdr, pfree_recv_queue);
+	}
+
+	spin_unlock(&pframequeue->lock);
+
+}
+
+u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter)
+{
+	u32 cnt = 0;
+	struct recv_frame *pending_frame;
+
+	while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) {
+		rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue);
+		cnt++;
+	}
+
+	return cnt;
+}
+
+static void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
+{
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure ev;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u32 cur_time = 0;
+
+	if (psecuritypriv->last_mic_err_time == 0) {
+		psecuritypriv->last_mic_err_time = jiffies;
+	} else {
+		cur_time = jiffies;
+
+		if (cur_time - psecuritypriv->last_mic_err_time < 60 * HZ) {
+			psecuritypriv->btkip_countermeasure = true;
+			psecuritypriv->last_mic_err_time = 0;
+			psecuritypriv->btkip_countermeasure_time = cur_time;
+		} else {
+			psecuritypriv->last_mic_err_time = jiffies;
+		}
+	}
+
+	memset(&ev, 0x00, sizeof(ev));
+	if (bgroup)
+		ev.flags |= IW_MICFAILURE_GROUP;
+	else
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	memcpy(ev.src_addr.sa_data, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	memset(&wrqu, 0x00, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+	wireless_send_event(padapter->pnetdev, IWEVMICHAELMICFAILURE,
+			    &wrqu, (char *)&ev);
+}
+
+static int recvframe_chkmic(struct adapter *adapter,  struct recv_frame *precvframe)
+{
+	int	i, res = _SUCCESS;
+	u32	datalen;
+	u8	miccode[8];
+	u8	bmic_err = false, brpt_micerror = true;
+	u8	*pframe, *payload, *pframemic;
+	u8	*mickey;
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	*prxattrib = &precvframe->attrib;
+	struct	security_priv	*psecuritypriv = &adapter->securitypriv;
+
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	stainfo = rtw_get_stainfo(&adapter->stapriv, &prxattrib->ta[0]);
+
+	if (prxattrib->encrypt == _TKIP_) {
+		/* calculate mic code */
+		if (stainfo) {
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+
+				if (!psecuritypriv) {
+					res = _FAIL;
+					goto exit;
+				}
+			} else {
+				mickey = &stainfo->dot11tkiprxmickey.skey[0];
+			}
+
+			datalen = precvframe->len - prxattrib->hdrlen - prxattrib->iv_len - prxattrib->icv_len - 8;/* icv_len included the mic code */
+			pframe = precvframe->rx_data;
+			payload = pframe + prxattrib->hdrlen + prxattrib->iv_len;
+
+			rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0],
+					   (unsigned char)prxattrib->priority); /* care the length of the data */
+
+			pframemic = payload + datalen;
+
+			bmic_err = false;
+
+			for (i = 0; i < 8; i++) {
+				if (miccode[i] != *(pframemic + i))
+					bmic_err = true;
+			}
+
+			if (bmic_err) {
+				/*  double check key_index for some timing issue , */
+				/*  cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */
+				if (is_multicast_ether_addr(prxattrib->ra) && prxattrib->key_index != pmlmeinfo->key_index)
+					brpt_micerror = false;
+
+				if ((prxattrib->bdecrypted) && (brpt_micerror))
+					rtw_handle_tkip_mic_err(adapter, (u8)is_multicast_ether_addr(prxattrib->ra));
+
+				res = _FAIL;
+			} else {
+				/* mic checked ok */
+				if (!psecuritypriv->bcheck_grpkey && is_multicast_ether_addr(prxattrib->ra))
+					psecuritypriv->bcheck_grpkey = true;
+			}
+		}
+
+		recvframe_pull_tail(precvframe, 8);
+	}
+
+exit:
+
+	return res;
+}
+
+/* decrypt and set the ivlen, icvlen of the recv_frame */
+static struct recv_frame *decryptor(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *prxattrib = &precv_frame->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct recv_frame *return_packet = precv_frame;
+	u32	 res = _SUCCESS;
+
+	if (prxattrib->encrypt > 0) {
+		u8 *iv = precv_frame->rx_data + prxattrib->hdrlen;
+
+		prxattrib->key_index = (((iv[3]) >> 6) & 0x3);
+
+		if (prxattrib->key_index > WEP_KEYS) {
+			switch (prxattrib->encrypt) {
+			case _WEP40_:
+			case _WEP104_:
+				prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+				break;
+			case _TKIP_:
+			case _AES_:
+			default:
+				prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+				break;
+			}
+		}
+	}
+
+	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt))) {
+		psecuritypriv->hw_decrypted = false;
+
+		switch (prxattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_decrypt(padapter, precv_frame);
+			break;
+		case _TKIP_:
+			res = rtw_tkip_decrypt(padapter, precv_frame);
+			break;
+		case _AES_:
+			res = rtw_aes_decrypt(padapter, precv_frame);
+			break;
+		default:
+			break;
+		}
+	} else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
+		   (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_))
+			psecuritypriv->hw_decrypted = true;
+
+	if (res == _FAIL) {
+		rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue);
+		return_packet = NULL;
+	} else {
+		prxattrib->bdecrypted = true;
+	}
+
+	return return_packet;
+}
+
+/* set the security information in the recv_frame */
+static struct recv_frame *portctrl(struct adapter *adapter, struct recv_frame *precv_frame)
+{
+	u8   *psta_addr, *ptr;
+	uint  auth_alg;
+	struct recv_frame *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	struct recv_frame *prtnframe;
+	u16 ether_type = 0;
+	u16  eapol_type = 0x888e;/* for Funia BD's WPA issue */
+	struct rx_pkt_attrib *pattrib;
+	__be16 be_tmp;
+
+	pstapriv = &adapter->stapriv;
+
+	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
+
+	ptr = precv_frame->rx_data;
+	pfhdr = precv_frame;
+	pattrib = &pfhdr->attrib;
+	psta_addr = pattrib->ta;
+
+	prtnframe = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+
+	if (auth_alg == 2) {
+		if (psta && psta->ieee8021x_blocked) {
+			/* blocked */
+			/* only accept EAPOL frame */
+			prtnframe = precv_frame;
+
+			/* get ether_type */
+			ptr = ptr + pfhdr->attrib.hdrlen + pfhdr->attrib.iv_len + LLC_HEADER_SIZE;
+			memcpy(&be_tmp, ptr, 2);
+			ether_type = ntohs(be_tmp);
+
+			if (ether_type == eapol_type) {
+				prtnframe = precv_frame;
+			} else {
+				/* free this frame */
+				rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue);
+				prtnframe = NULL;
+			}
+		} else {
+			/* allowed */
+			/* check decryption status, and decrypt the frame if needed */
+			prtnframe = precv_frame;
+		}
+	} else {
+		prtnframe = precv_frame;
+	}
+
+	return prtnframe;
+}
+
+static int recv_decache(struct recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+	int tid = precv_frame->attrib.priority;
+
+	u16 seq_ctrl = ((precv_frame->attrib.seq_num & 0xffff) << 4) |
+		(precv_frame->attrib.frag_num & 0xf);
+
+	if (tid > 15)
+		return _FAIL;
+
+	if (1) {/* if (bretry) */
+		if (seq_ctrl == prxcache->tid_rxseq[tid])
+			return _FAIL;
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+	return _SUCCESS;
+}
+
+static void process_pwrbit_data(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	unsigned char pwrbit;
+	u8 *ptr = precv_frame->rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	pwrbit = GetPwrMgt(ptr);
+
+	if (psta) {
+		if (pwrbit) {
+			if (!(psta->state & WIFI_SLEEP_STATE))
+				stop_sta_xmit(padapter, psta);
+		} else {
+			if (psta->state & WIFI_SLEEP_STATE)
+				wakeup_sta_to_xmit(padapter, psta);
+		}
+	}
+}
+
+static void process_wmmps_data(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	if (!psta)
+		return;
+
+	if (!psta->qos_option)
+		return;
+
+	if (!(psta->qos_info & 0xf))
+		return;
+
+	if (psta->state & WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		switch (pattrib->priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		if (wmmps_ac) {
+			if (psta->sleepq_ac_len > 0) {
+				/* process received triggered frame */
+				xmit_delivery_enabled_frames(padapter, psta);
+			} else {
+				/* issue one qos null frame with More data bit = 0 and the EOSP bit set (= 1) */
+				issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0);
+			}
+		}
+	}
+}
+
+static void count_rx_stats(struct adapter *padapter, struct recv_frame *prframe, struct sta_info *sta)
+{
+	int	sz;
+	struct sta_info		*psta = NULL;
+	struct stainfo_stats	*pstats = NULL;
+	struct rx_pkt_attrib	*pattrib = &prframe->attrib;
+	struct recv_priv	*precvpriv = &padapter->recvpriv;
+
+	sz = get_recvframe_len(prframe);
+	precvpriv->rx_bytes += sz;
+
+	padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+	if (!is_broadcast_ether_addr(pattrib->dst) && !is_multicast_ether_addr(pattrib->dst))
+		padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+
+	if (sta)
+		psta = sta;
+	else
+		psta = prframe->psta;
+
+	if (psta) {
+		pstats = &psta->sta_stats;
+
+		pstats->rx_data_pkts++;
+		pstats->rx_bytes += sz;
+	}
+}
+
+static int sta2sta_data_frame(struct adapter *adapter,
+			      struct recv_frame *precv_frame, struct sta_info **psta)
+{
+	int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	u8 *sta_addr = NULL;
+	bool bmcast = is_multicast_ether_addr(pattrib->dst);
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		sta_addr = pattrib->src;
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		/*  For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */
+		if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+		sta_addr = pattrib->bssid;
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		if (bmcast) {
+			/*  For AP mode, if DA == MCAST, then BSSID should be also MCAST */
+			if (!is_multicast_ether_addr(pattrib->bssid)) {
+					ret = _FAIL;
+					goto exit;
+			}
+		} else { /*  not mc-frame */
+			/*  For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */
+			if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+				ret = _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->src;
+		}
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		sta_addr = mybssid;
+	} else {
+		ret  = _FAIL;
+	}
+
+	if (bmcast)
+		*psta = rtw_get_bcmc_stainfo(adapter);
+	else
+		*psta = rtw_get_stainfo(pstapriv, sta_addr); /*  get ap_info */
+
+	if (!*psta)
+		goto exit;
+
+exit:
+
+	return ret;
+}
+
+static int ap2sta_data_frame(
+	struct adapter *adapter,
+	struct recv_frame *precv_frame,
+	struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->rx_data;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	int ret = _SUCCESS;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	bool bmcast = is_multicast_ether_addr(pattrib->dst);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    (check_fwstate(pmlmepriv, _FW_LINKED) ||
+	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*  da should be for me */
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*  check BSSID */
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		     (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+			if (!bmcast)
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			*psta = rtw_get_bcmc_stainfo(adapter);
+		else
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get ap_info */
+
+		if (!*psta) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (ieee80211_is_nullfunc(hdr->frame_control)) {
+			/* We count the nullfunc frame, but we'll not pass it on to higher layers. */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+		   check_fwstate(pmlmepriv, _FW_LINKED)) {
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+
+		memcpy(pattrib->bssid,  mybssid, ETH_ALEN);
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get sta_info */
+		if (!*psta) {
+			ret = _FAIL;
+			goto exit;
+		}
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* Special case */
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	} else {
+		if (!memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) {
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get sta_info */
+			if (!*psta)
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		}
+
+		ret = _FAIL;
+	}
+
+exit:
+
+	return ret;
+}
+
+static int sta2ap_data_frame(struct adapter *adapter,
+			     struct recv_frame *precv_frame,
+			     struct sta_info **psta)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *ptr = precv_frame->rx_data;
+	__le16 fc = *(__le16 *)ptr;
+	unsigned char *mybssid  = get_bssid(pmlmepriv);
+	int ret = _SUCCESS;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* For AP mode, RA = BSSID, TX = STA(SRC_ADDR), A3 = DST_ADDR */
+		if (memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->src);
+		if (!*psta) {
+			issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+		process_pwrbit_data(adapter, precv_frame);
+
+		if (ieee80211_is_data_qos(fc))
+			process_wmmps_data(adapter, precv_frame);
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	} else {
+		u8 *myhwaddr = myid(&adapter->eeprompriv);
+
+		if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+
+	return ret;
+}
+
+static void validate_recv_ctrl_frame(struct adapter *padapter,
+				     struct recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	struct ieee80211_pspoll *pspoll = (struct ieee80211_pspoll *)hdr;
+	u8 wmmps_ac;
+	struct sta_info *psta;
+
+	/* receive the frames that ra(a1) is my address */
+	if (memcmp(hdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN))
+		return;
+
+	/* only handle ps-poll */
+	if (!ieee80211_is_pspoll(hdr->frame_control))
+		return;
+
+	psta = rtw_get_stainfo(pstapriv, hdr->addr2);
+	if (!psta || psta->aid != (le16_to_cpu(pspoll->aid) & 0x3FFF))
+		return;
+
+	/* for rx pkt statistics */
+	psta->sta_stats.rx_ctrl_pkts++;
+
+	switch (pattrib->priority) {
+	case 1:
+	case 2:
+		wmmps_ac = psta->uapsd_bk & BIT(0);
+		break;
+	case 4:
+	case 5:
+		wmmps_ac = psta->uapsd_vi & BIT(0);
+		break;
+	case 6:
+	case 7:
+		wmmps_ac = psta->uapsd_vo & BIT(0);
+		break;
+	case 0:
+	case 3:
+	default:
+		wmmps_ac = psta->uapsd_be & BIT(0);
+		break;
+	}
+
+	if (wmmps_ac)
+		return;
+
+	if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+		psta->expire_to = pstapriv->expire_to;
+		psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+	}
+
+	if ((psta->state & WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap & BIT(psta->aid))) {
+		struct list_head *xmitframe_plist, *xmitframe_phead;
+		struct xmit_frame *pxmitframe = NULL;
+		struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+		spin_lock_bh(&pxmitpriv->lock);
+
+		xmitframe_phead = get_list_head(&psta->sleep_q);
+		xmitframe_plist = xmitframe_phead->next;
+
+		if (xmitframe_phead != xmitframe_plist) {
+			pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = xmitframe_plist->next;
+
+			list_del_init(&pxmitframe->list);
+
+			psta->sleepq_len--;
+
+			if (psta->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+
+			if (psta->sleepq_len == 0) {
+				pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+				/* upate BCN for TIM IE */
+				/* update_BCNTIM(padapter); */
+				update_beacon(padapter, _TIM_IE_, NULL, false);
+			}
+		} else {
+			if (pstapriv->tim_bitmap & BIT(psta->aid)) {
+				if (psta->sleepq_len == 0)
+					/* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+					issue_nulldata(padapter, psta->hwaddr, 0, 0, 0);
+				else
+					psta->sleepq_len = 0;
+
+				pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+				/* upate BCN for TIM IE */
+				/* update_BCNTIM(padapter); */
+				update_beacon(padapter, _TIM_IE_, NULL, false);
+			}
+		}
+		spin_unlock_bh(&pxmitpriv->lock);
+	}
+}
+
+struct recv_frame *recvframe_chk_defrag(struct adapter *padapter, struct recv_frame *precv_frame);
+
+static void validate_recv_mgnt_frame(struct adapter *padapter,
+				     struct recv_frame *precv_frame)
+{
+	struct sta_info *psta;
+	struct ieee80211_hdr *hdr;
+
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (!precv_frame)
+		return;
+
+	hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	psta = rtw_get_stainfo(&padapter->stapriv, hdr->addr2);
+	if (psta) {
+		psta->sta_stats.rx_mgnt_pkts++;
+		if (ieee80211_is_beacon(hdr->frame_control))
+			psta->sta_stats.rx_beacon_pkts++;
+		else if (ieee80211_is_probe_req(hdr->frame_control))
+			psta->sta_stats.rx_probereq_pkts++;
+		else if (ieee80211_is_probe_resp(hdr->frame_control)) {
+			if (!memcmp(padapter->eeprompriv.mac_addr, hdr->addr1, ETH_ALEN))
+				psta->sta_stats.rx_probersp_pkts++;
+			else if (is_broadcast_mac_addr(hdr->addr1) || is_multicast_mac_addr(hdr->addr1))
+				psta->sta_stats.rx_probersp_bm_pkts++;
+			else
+				psta->sta_stats.rx_probersp_uo_pkts++;
+		}
+	}
+
+	mgt_dispatcher(padapter, precv_frame);
+}
+
+static int validate_recv_data_frame(struct adapter *adapter,
+				    struct recv_frame *precv_frame)
+{
+	struct sta_info *psta = NULL;
+	u8 *ptr = precv_frame->rx_data;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	struct rx_pkt_attrib	*pattrib = &precv_frame->attrib;
+	struct security_priv	*psecuritypriv = &adapter->securitypriv;
+	int ret;
+
+	memcpy(pattrib->dst, ieee80211_get_DA(hdr), ETH_ALEN);
+	memcpy(pattrib->src, ieee80211_get_SA(hdr), ETH_ALEN);
+
+	/* address4 is used only if both to_ds and from_ds are set */
+	if (ieee80211_has_a4(hdr->frame_control))
+		return _FAIL;
+
+	memcpy(pattrib->ra, hdr->addr1, ETH_ALEN);
+	memcpy(pattrib->ta, hdr->addr2, ETH_ALEN);
+
+	if (ieee80211_has_fromds(hdr->frame_control)) {
+		memcpy(pattrib->bssid, hdr->addr2, ETH_ALEN);
+		ret = ap2sta_data_frame(adapter, precv_frame, &psta);
+	} else if (ieee80211_has_tods(hdr->frame_control)) {
+		memcpy(pattrib->bssid, hdr->addr1, ETH_ALEN);
+		ret = sta2ap_data_frame(adapter, precv_frame, &psta);
+	} else {
+		memcpy(pattrib->bssid, hdr->addr3, ETH_ALEN);
+		ret = sta2sta_data_frame(adapter, precv_frame, &psta);
+	}
+
+	if (ret == _FAIL || ret == RTW_RX_HANDLED)
+		return ret;
+
+	if (!psta)
+		return _FAIL;
+
+	/* psta->rssi = prxcmd->rssi; */
+	/* psta->signal_quality = prxcmd->sq; */
+	precv_frame->psta = psta;
+
+	pattrib->amsdu = 0;
+	pattrib->ack_policy = 0;
+	/* parsing QC field */
+	if (pattrib->qos) {
+		pattrib->priority = ieee80211_get_tid(hdr);
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = 26;
+
+		if (pattrib->priority != 0 && pattrib->priority != 3)
+			adapter->recvpriv.bIsAnyNonBEPkts = true;
+	} else {
+		pattrib->priority = 0;
+		pattrib->hdrlen = 24;
+	}
+
+	if (pattrib->order)/* HT-CTRL 11n */
+		pattrib->hdrlen += 4;
+
+	precv_frame->preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+	/*  decache, drop duplicate recv packets */
+	if (recv_decache(precv_frame, ieee80211_has_retry(hdr->frame_control),
+			 &psta->sta_recvpriv.rxcache) == _FAIL)
+		return _FAIL;
+
+	if (pattrib->privacy) {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, is_multicast_ether_addr(pattrib->ra));
+
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+	} else {
+		pattrib->encrypt = 0;
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+	}
+
+	return _SUCCESS;
+}
+
+static int validate_recv_frame(struct adapter *adapter, struct recv_frame *precv_frame)
+{
+	/* shall check frame subtype, to / from ds, da, bssid */
+
+	/* then call check if rx seq/frag. duplicated. */
+
+	int retval = _FAIL;
+	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)precv_frame->rx_data;
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		int ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, rtw_get_oper_ch(adapter));
+
+		if (ch_set_idx >= 0)
+			pmlmeext->channel_set[ch_set_idx].rx_count++;
+	}
+
+	if ((hdr->frame_control & cpu_to_le16(IEEE80211_FCTL_VERS)) != 0)
+		return _FAIL;
+
+	pattrib->frag_num = le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG;
+	pattrib->seq_num = IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
+
+	pattrib->pw_save = ieee80211_has_pm(hdr->frame_control);
+	pattrib->mfrag = ieee80211_has_morefrags(hdr->frame_control);
+	pattrib->mdata = ieee80211_has_moredata(hdr->frame_control);
+	pattrib->privacy = ieee80211_has_protected(hdr->frame_control);
+	pattrib->order = ieee80211_has_order(hdr->frame_control);
+
+	/* We return _SUCCESS only for data frames. */
+	if (ieee80211_is_mgmt(hdr->frame_control))
+		validate_recv_mgnt_frame(adapter, precv_frame);
+	else if (ieee80211_is_ctl(hdr->frame_control))
+		validate_recv_ctrl_frame(adapter, precv_frame);
+	else if (ieee80211_is_data(hdr->frame_control)) {
+		rtw_led_control(adapter, LED_CTL_RX);
+		pattrib->qos = ieee80211_is_data_qos(hdr->frame_control);
+		retval = validate_recv_data_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			struct recv_priv *precvpriv = &adapter->recvpriv;
+
+			precvpriv->rx_drop++;
+		}
+	}
+
+	return retval;
+}
+
+/* remove the wlanhdr and add the eth_hdr */
+
+static int wlanhdr_to_ethhdr(struct recv_frame *precvframe)
+{
+	int	rmv_len;
+	u16	eth_type, len;
+	__be16 be_tmp;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+
+	int ret = _SUCCESS;
+	struct adapter			*adapter = precvframe->adapter;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	u8	*ptr = precvframe->rx_data; /*  point to frame_ctrl field */
+	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
+
+	if (pattrib->encrypt)
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+
+	psnap = (struct ieee80211_snap_hdr *)(ptr + pattrib->hdrlen + pattrib->iv_len);
+	psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
+	     memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) &&
+	    memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2)) ||
+	    !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+		bsnaphdr = true;
+	} else {
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = false;
+	}
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr ? SNAP_SIZE : 0);
+	len = precvframe->len - rmv_len;
+
+	memcpy(&be_tmp, ptr + rmv_len, 2);
+	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = eth_type;
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE))) {
+		ptr += rmv_len;
+		*ptr = 0x87;
+		*(ptr + 1) = 0x12;
+
+		eth_type = 0x8712;
+		/*  append rx status for mp test packets */
+		ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + 2) - 24);
+		if (!ptr)
+			return _FAIL;
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else {
+		ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + (bsnaphdr ? 2 : 0)));
+		if (!ptr)
+			return _FAIL;
+	}
+
+	memcpy(ptr, pattrib->dst, ETH_ALEN);
+	memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN);
+
+	if (!bsnaphdr) {
+		be_tmp = htons(len);
+		memcpy(ptr + 12, &be_tmp, 2);
+	}
+
+	return ret;
+}
+
+/* perform defrag */
+static struct recv_frame *recvframe_defrag(struct adapter *adapter, struct __queue *defrag_q)
+{
+	struct list_head *plist, *phead;
+	u8 wlanhdr_offset;
+	u8	curfragnum;
+	struct recv_frame *pfhdr, *pnfhdr;
+	struct recv_frame *prframe, *pnextrframe;
+	struct __queue *pfree_recv_queue;
+
+	curfragnum = 0;
+	pfree_recv_queue = &adapter->recvpriv.free_recv_queue;
+
+	phead = get_list_head(defrag_q);
+	plist = phead->next;
+	pfhdr = container_of(plist, struct recv_frame, list);
+	prframe = (struct recv_frame *)pfhdr;
+	list_del_init(&prframe->list);
+
+	if (curfragnum != pfhdr->attrib.frag_num) {
+		/* the first fragment number must be 0 */
+		/* free the whole queue */
+		rtw_free_recvframe(prframe, pfree_recv_queue);
+		rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+		return NULL;
+	}
+
+	curfragnum++;
+
+	plist = get_list_head(defrag_q);
+	plist = phead->next;
+	pfhdr = container_of(plist, struct recv_frame, list);
+	prframe = (struct recv_frame *)pfhdr;
+	list_del_init(&prframe->list);
+
+	plist = plist->next;
+
+	while (phead != plist) {
+		pnfhdr = container_of(plist, struct recv_frame, list);
+		pnextrframe = (struct recv_frame *)pnfhdr;
+
+		/* check the fragment sequence  (2nd ~n fragment frame) */
+
+		if (curfragnum != pnfhdr->attrib.frag_num) {
+			/* the fragment number must be increasing  (after decache) */
+			/* release the defrag_q & prframe */
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+			rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+			return NULL;
+		}
+
+		curfragnum++;
+
+		/* copy the 2nd~n fragment frame's payload to the first fragment */
+		/* get the 2nd~last fragment frame's payload */
+
+		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+
+		recvframe_pull(pnextrframe, wlanhdr_offset);
+
+		/* append  to first fragment frame's tail (if privacy frame, pull the ICV) */
+		recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+
+		/* memcpy */
+		memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+
+		recvframe_put(prframe, pnfhdr->len);
+
+		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		plist = plist->next;
+	}
+
+	/* free the defrag_q queue and return the prframe */
+	rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+struct recv_frame *recvframe_chk_defrag(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	u8	ismfrag;
+	u8	fragnum;
+	u8	*psta_addr;
+	struct recv_frame *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	struct list_head *phead;
+	struct recv_frame *prtnframe = NULL;
+	struct __queue *pfree_recv_queue, *pdefrag_q;
+
+	pstapriv = &padapter->stapriv;
+
+	pfhdr = precv_frame;
+
+	pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/* need to define struct of wlan header frame ctrl */
+	ismfrag = pfhdr->attrib.mfrag;
+	fragnum = pfhdr->attrib.frag_num;
+
+	psta_addr = pfhdr->attrib.ta;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+	if (!psta) {
+		__le16 fc = *(__le16 *)pfhdr->rx_data;
+
+		if (ieee80211_is_data(fc)) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+			pdefrag_q = &psta->sta_recvpriv.defrag_q;
+		} else {
+			pdefrag_q = NULL;
+		}
+	} else {
+		pdefrag_q = &psta->sta_recvpriv.defrag_q;
+	}
+
+	if ((ismfrag == 0) && (fragnum == 0))
+		prtnframe = precv_frame;/* isn't a fragment frame */
+
+	if (ismfrag == 1) {
+		/* 0~(n-1) fragment frame */
+		/* enqueue to defraf_g */
+		if (pdefrag_q) {
+			if (fragnum == 0) {
+				/* the first fragment */
+				if (!list_empty(&pdefrag_q->queue)) {
+					/* free current defrag_q */
+					rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue);
+				}
+			}
+
+			/* Then enqueue the 0~(n-1) fragment into the defrag_q */
+
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+
+			prtnframe = NULL;
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			if (precv_frame && pfree_recv_queue)
+				rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame */
+		/* enqueue the last fragment */
+		if (pdefrag_q) {
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+
+			/* call recvframe_defrag to defrag */
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe = precv_frame;
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			if (precv_frame && pfree_recv_queue)
+				rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+
+	if (prtnframe && prtnframe->attrib.privacy) {
+		/* after defrag we must check tkip mic code */
+		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
+			if (precv_frame && pfree_recv_queue)
+				rtw_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+
+	return prtnframe;
+}
+
+static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
+{
+	int	a_len, padding_len;
+	u16	eth_type, nSubframe_Length;
+	u8	nr_subframes, i;
+	unsigned char *pdata;
+	struct rx_pkt_attrib *pattrib;
+	struct sk_buff *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
+
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+	int	ret = _SUCCESS;
+
+	nr_subframes = 0;
+
+	pattrib = &prframe->attrib;
+
+	recvframe_pull(prframe, prframe->attrib.hdrlen);
+
+	if (prframe->attrib.iv_len > 0)
+		recvframe_pull(prframe, prframe->attrib.iv_len);
+
+	a_len = prframe->len;
+
+	pdata = prframe->rx_data;
+
+	while (a_len > ETH_HLEN) {
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = RTW_GET_BE16(pdata + 12);
+
+		if (a_len < ETH_HLEN + nSubframe_Length)
+			goto exit;
+
+		/* move the data point to data content */
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+
+		/* Allocate new skb for releasing to upper layer */
+		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if (sub_skb) {
+			skb_reserve(sub_skb, 12);
+			skb_put_data(sub_skb, pdata, nSubframe_Length);
+		} else {
+			sub_skb = skb_clone(prframe->pkt, GFP_ATOMIC);
+			if (sub_skb) {
+				sub_skb->data = pdata;
+				sub_skb->len = nSubframe_Length;
+				skb_set_tail_pointer(sub_skb, nSubframe_Length);
+			} else {
+				break;
+			}
+		}
+
+		subframes[nr_subframes++] = sub_skb;
+
+		if (nr_subframes >= MAX_SUBFRAME_COUNT)
+			break;
+
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if (a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4 - 1));
+			if (padding_len == 4)
+				padding_len = 0;
+
+			if (a_len < padding_len)
+				goto exit;
+
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+
+	for (i = 0; i < nr_subframes; i++) {
+		sub_skb = subframes[i];
+		/* convert hdr + possible LLC headers into Ethernet header */
+		eth_type = RTW_GET_BE16(&sub_skb->data[6]);
+		if (sub_skb->len >= 8 &&
+		    ((!memcmp(sub_skb->data, rtw_rfc1042_header, SNAP_SIZE) &&
+			  eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+			 !memcmp(sub_skb->data, rtw_bridge_tunnel_header, SNAP_SIZE))) {
+			/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+			skb_pull(sub_skb, SNAP_SIZE);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+		} else {
+			__be16 len;
+			/* Leave Ethernet header part of hdr and full payload */
+			len = htons(sub_skb->len);
+			memcpy(skb_push(sub_skb, 2), &len, 2);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+		}
+
+		/* Indicate the packets to upper layer */
+			/*  Insert NAT2.5 RX here! */
+		sub_skb->protocol = eth_type_trans(sub_skb, padapter->pnetdev);
+		sub_skb->dev = padapter->pnetdev;
+
+		sub_skb->ip_summed = CHECKSUM_NONE;
+
+		netif_rx(sub_skb);
+	}
+
+exit:
+
+	prframe->len = 0;
+	rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */
+
+	return ret;
+}
+
+static bool check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
+{
+	u8	wsize = preorder_ctrl->wsize_b;
+	u16	wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF;/*  4096; */
+
+	/*  Rx Reorder initialize condition. */
+	if (preorder_ctrl->indicate_seq == 0xFFFF)
+		preorder_ctrl->indicate_seq = seq_num;
+
+	/*  Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(seq_num, preorder_ctrl->indicate_seq))
+		return false;
+
+	/*  */
+	/*  Sliding window manipulation. Conditions includes: */
+	/*  1. Incoming SeqNum is equal to WinStart =>Window shift 1 */
+	/*  2. Incoming SeqNum is larger than the WinEnd => Window shift N */
+	/*  */
+	if (SN_EQUAL(seq_num, preorder_ctrl->indicate_seq)) {
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+	} else if (SN_LESS(wend, seq_num)) {
+		if (seq_num >= (wsize - 1))
+			preorder_ctrl->indicate_seq = seq_num + 1 - wsize;
+		else
+			preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1;
+	}
+
+	return true;
+}
+
+static bool enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, struct recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->attrib;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct list_head *phead, *plist;
+	struct recv_frame *hdr;
+	struct rx_pkt_attrib *pnextattrib;
+
+	phead = get_list_head(ppending_recvframe_queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		hdr = container_of(plist, struct recv_frame, list);
+		pnextattrib = &hdr->attrib;
+
+		if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+			plist = plist->next;
+		else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+			return false;
+		else
+			break;
+	}
+
+	list_del_init(&prframe->list);
+
+	list_add_tail(&prframe->list, plist);
+	return true;
+}
+
+static int rtw_recv_indicatepkt(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	struct recv_priv *precvpriv;
+	struct __queue *pfree_recv_queue;
+	struct sk_buff *skb;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	precvpriv = &padapter->recvpriv;
+	pfree_recv_queue = &precvpriv->free_recv_queue;
+
+	skb = precv_frame->pkt;
+	if (!skb)
+		goto _recv_indicatepkt_drop;
+
+	skb->data = precv_frame->rx_data;
+
+	skb_set_tail_pointer(skb, precv_frame->len);
+
+	skb->len = precv_frame->len;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sk_buff *pskb2 = NULL;
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+		bool bmcast = is_multicast_ether_addr(pattrib->dst);
+
+		if (memcmp(pattrib->dst, myid(&padapter->eeprompriv), ETH_ALEN)) {
+			if (bmcast) {
+				psta = rtw_get_bcmc_stainfo(padapter);
+				pskb2 = skb_clone(skb, GFP_ATOMIC);
+			} else {
+				psta = rtw_get_stainfo(pstapriv, pattrib->dst);
+			}
+
+			if (psta) {
+				struct net_device *pnetdev;
+
+				pnetdev = (struct net_device *)padapter->pnetdev;
+				skb->dev = pnetdev;
+				skb_set_queue_mapping(skb, rtw_recv_select_queue(skb));
+
+				rtw_xmit_entry(skb, pnetdev);
+
+				if (bmcast)
+					skb = pskb2;
+				else
+					goto _recv_indicatepkt_end;
+			}
+		}
+	}
+
+	rcu_read_lock();
+	rcu_dereference(padapter->pnetdev->rx_handler_data);
+	rcu_read_unlock();
+
+	skb->ip_summed = CHECKSUM_NONE;
+	skb->dev = padapter->pnetdev;
+	skb->protocol = eth_type_trans(skb, padapter->pnetdev);
+
+	netif_rx(skb);
+
+_recv_indicatepkt_end:
+
+	/*  pointers to NULL before rtw_free_recvframe() */
+	precv_frame->pkt = NULL;
+
+	rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	return _SUCCESS;
+
+_recv_indicatepkt_drop:
+
+	/* enqueue back to free_recv_queue */
+	rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	return _FAIL;
+}
+
+static bool recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+	struct list_head *phead, *plist;
+	struct recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	int bPktInBuf = false;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+	phead =		get_list_head(ppending_recvframe_queue);
+	plist = phead->next;
+
+	/*  Handling some condition for forced indicate case. */
+	if (bforced) {
+		if (list_empty(phead))
+			return true;
+
+		prframe = container_of(plist, struct recv_frame, list);
+		pattrib = &prframe->attrib;
+		preorder_ctrl->indicate_seq = pattrib->seq_num;
+	}
+
+	/*  Prepare indication list and indication. */
+	/*  Check if there is any packet need indicate. */
+	while (!list_empty(phead)) {
+		prframe = container_of(plist, struct recv_frame, list);
+		pattrib = &prframe->attrib;
+
+		if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+			plist = plist->next;
+			list_del_init(&prframe->list);
+
+			if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num))
+				preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+			/* Set this as a lock to make sure that only one thread is indicating packet. */
+
+			/* indicate this recv_frame */
+			if (!pattrib->amsdu) {
+				if ((!padapter->bDriverStopped) &&
+				    (!padapter->bSurpriseRemoved))
+					rtw_recv_indicatepkt(padapter, prframe);/* indicate this recv_frame */
+			} else if (pattrib->amsdu == 1) {
+				if (amsdu_to_msdu(padapter, prframe) != _SUCCESS)
+					rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+			} else {
+				/* error condition; */
+			}
+
+			/* Update local variables. */
+			bPktInBuf = false;
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+	}
+	return bPktInBuf;
+}
+
+static int recv_indicatepkt_reorder(struct adapter *padapter, struct recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &prframe->attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->preorder_ctrl;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+	if (!pattrib->amsdu) {
+		/* s1. */
+		wlanhdr_to_ethhdr(prframe);
+
+		if (!pattrib->qos) {
+			if (!padapter->bDriverStopped &&
+			    !padapter->bSurpriseRemoved) {
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+			}
+
+			return _FAIL;
+		}
+
+		if (!preorder_ctrl->enable) {
+			/* indicate this recv_frame */
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			rtw_recv_indicatepkt(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) % 4096;
+			return _SUCCESS;
+		}
+	} else if (pattrib->amsdu == 1) { /* temp filter -> means didn't support A-MSDUs in a A-MPDU */
+		if (!preorder_ctrl->enable) {
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			retval = amsdu_to_msdu(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) % 4096;
+			return retval;
+		}
+	}
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	/* s2. check if winstart_b(indicate_seq) needs to been updated */
+	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num))
+		goto _err_exit;
+
+	/* s3. Insert all packet into Reorder Queue to maintain its ordering. */
+	if (!enqueue_reorder_recvframe(preorder_ctrl, prframe))
+		goto _err_exit;
+
+	/* s4. */
+	/*  Indication process. */
+	/*  After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */
+	/*  with the SeqNum smaller than latest WinStart and buffer other packets. */
+	/*  */
+	/*  For Rx Reorder condition: */
+	/*  1. All packets with SeqNum smaller than WinStart => Indicate */
+	/*  2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */
+	/*  */
+
+	/* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false)) {
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+	} else {
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+	}
+
+	return _SUCCESS;
+
+_err_exit:
+
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+	return _FAIL;
+}
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+{
+	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+	struct adapter *padapter = preorder_ctrl->padapter;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+		return;
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true))
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+}
+
+static int process_recv_indicatepkts(struct adapter *padapter, struct recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	/* struct rx_pkt_attrib *pattrib = &prframe->attrib; */
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
+
+	if (phtpriv->ht_option) {  /* B/G/N Mode */
+		/* prframe->preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */
+
+		if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) {
+			/*  including perform A-MPDU Rx Ordering Buffer Control */
+			if ((!padapter->bDriverStopped) &&
+			    (!padapter->bSurpriseRemoved)) {
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	} else { /* B/G mode */
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS)
+			return retval;
+
+		if ((!padapter->bDriverStopped) &&
+		    (!padapter->bSurpriseRemoved)) {
+			/* indicate this recv_frame */
+			rtw_recv_indicatepkt(padapter, prframe);
+		} else {
+			retval = _FAIL;
+			return retval;
+		}
+	}
+
+	return retval;
+}
+
+static int recv_func_prehandle(struct adapter *padapter, struct recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/* check the frame crtl field and decache */
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS)
+		rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+
+	return ret;
+}
+
+static int recv_func_posthandle(struct adapter *padapter, struct recv_frame *prframe)
+{
+	int ret = _SUCCESS;
+	struct recv_frame *orig_prframe = prframe;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/*  DATA FRAME */
+	rtw_led_control(padapter, LED_CTL_RX);
+
+	prframe = decryptor(padapter, prframe);
+	if (!prframe) {
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if (!prframe)
+		goto _recv_data_drop;
+
+	prframe = portctrl(padapter, prframe);
+	if (!prframe) {
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+	count_rx_stats(padapter, prframe, NULL);
+
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS) {
+		rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		goto _recv_data_drop;
+	}
+	return ret;
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+static int recv_func(struct adapter *padapter, struct recv_frame *rframe)
+{
+	int ret;
+	struct rx_pkt_attrib *prxattrib = &rframe->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct recv_priv *recvpriv = &padapter->recvpriv;
+
+	/* check if need to handle uc_swdec_pending_queue*/
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+	    psecuritypriv->busetkipkey) {
+		struct recv_frame *pending_frame;
+
+		while ((pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue)))
+			recv_func_posthandle(padapter, pending_frame);
+	}
+
+	ret = recv_func_prehandle(padapter, rframe);
+
+	if (ret == _SUCCESS) {
+		/* check if need to enqueue into uc_swdec_pending_queue*/
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+		    !is_multicast_ether_addr(prxattrib->ra) && prxattrib->encrypt > 0 &&
+		    (prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt) &&
+		     psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK &&
+		     !psecuritypriv->busetkipkey) {
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+			if (recvpriv->free_recvframe_cnt < NR_RECVFRAME / 4) {
+				/*
+				 * to prevent from recvframe starvation,
+				 * get recvframe from uc_swdec_pending_queue to
+				 * free_recvframe_cnt
+				 */
+				rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue);
+				if (rframe)
+					goto do_posthandle;
+			}
+			goto exit;
+		}
+do_posthandle:
+		ret = recv_func_posthandle(padapter, rframe);
+	}
+
+exit:
+	return ret;
+}
+
+s32 rtw_recv_entry(struct recv_frame *precvframe)
+{
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+	s32 ret = _SUCCESS;
+
+	padapter = precvframe->adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL)
+		goto _recv_entry_drop;
+
+	precvpriv->rx_pkts++;
+
+	return ret;
+
+_recv_entry_drop:
+
+	return ret;
+}
+
+static void rtw_signal_stat_timer_hdl(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t, recvpriv.signal_stat_timer);
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+
+	u32 tmp_s, tmp_q;
+	u8 avg_signal_strength = 0;
+	u8 avg_signal_qual = 0;
+	u8 _alpha = 3; /*  this value is based on converging_constant = 5000 and sampling_interval = 1000 */
+
+	if (adapter->recvpriv.is_signal_dbg) {
+		/* update the user specific value, signal_strength_dbg, to signal_strength, rssi */
+		adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg;
+		adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+	} else {
+		if (recvpriv->signal_strength_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+			/*  after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_strength_data.update_req = 1;
+		}
+
+		if (recvpriv->signal_qual_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+			/*  after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_qual_data.update_req = 1;
+		}
+
+		/* update value of signal_strength, rssi, signal_qual */
+		if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
+			tmp_s = (avg_signal_strength + (_alpha - 1) * recvpriv->signal_strength);
+			if (tmp_s % _alpha)
+				tmp_s = tmp_s / _alpha + 1;
+			else
+				tmp_s = tmp_s / _alpha;
+			if (tmp_s > 100)
+				tmp_s = 100;
+
+			tmp_q = (avg_signal_qual + (_alpha - 1) * recvpriv->signal_qual);
+			if (tmp_q % _alpha)
+				tmp_q = tmp_q / _alpha + 1;
+			else
+				tmp_q = tmp_q / _alpha;
+			if (tmp_q > 100)
+				tmp_q = 100;
+
+			recvpriv->signal_strength = tmp_s;
+			recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+			recvpriv->signal_qual = tmp_q;
+		}
+	}
+	rtw_set_signal_stat_timer(recvpriv);
+}
diff --git a/drivers/staging/r8188eu/core/rtw_rf.c b/drivers/staging/r8188eu/core/rtw_rf.c
new file mode 100644
index 000000000..2d2f0fc4c
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_rf.c
@@ -0,0 +1,29 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+
+static const u32 ch_freq_map[] = {
+	2412,
+	2417,
+	2422,
+	2427,
+	2432,
+	2437,
+	2442,
+	2447,
+	2452,
+	2457,
+	2462,
+	2467,
+	2472,
+	2484
+};
+
+u32 rtw_ch2freq(u32 channel)
+{
+	if (channel == 0 || channel > ARRAY_SIZE(ch_freq_map))
+		return 2412;
+
+	return ch_freq_map[channel - 1];
+}
diff --git a/drivers/staging/r8188eu/core/rtw_security.c b/drivers/staging/r8188eu/core/rtw_security.c
new file mode 100644
index 000000000..5bba57d18
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_security.c
@@ -0,0 +1,1376 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define  _RTW_SECURITY_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+#include "../include/osdep_intf.h"
+
+/* WEP related ===== */
+
+/*
+	Need to consider the fragment  situation
+*/
+void rtw_wep_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{	/*  exclude ICV */
+	union {
+		__le32 f0;
+		u8 f1[4];
+	} crc;
+
+	int	curfragnum, length;
+	u32	keylength;
+
+	u8	*pframe, *payload, *iv;    /* wepkey */
+	u8	wepkey[16];
+	u8   hw_hdr_offset = 0;
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct arc4_ctx *ctx = &psecuritypriv->xmit_arc4_ctx;
+
+	if (!pxmitframe->buf_addr)
+		return;
+
+	hw_hdr_offset = TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ;
+	pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+	/* start to encrypt each fragment */
+	if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
+		keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
+
+		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+			iv = pframe + pattrib->hdrlen;
+			memcpy(&wepkey[0], iv, 3);
+			memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
+			payload = pframe + pattrib->iv_len + pattrib->hdrlen;
+
+			if ((curfragnum + 1) == pattrib->nr_frags) {	/* the last fragment */
+				length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+
+				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+				arc4_setkey(ctx, wepkey, 3 + keylength);
+				arc4_crypt(ctx, payload, payload, length);
+				arc4_crypt(ctx, payload + length, crc.f1, 4);
+			} else {
+				length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+				arc4_setkey(ctx, wepkey, 3 + keylength);
+				arc4_crypt(ctx, payload, payload, length);
+				arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+				pframe += pxmitpriv->frag_len;
+				pframe = PTR_ALIGN(pframe, 4);
+			}
+		}
+	}
+
+}
+
+void rtw_wep_decrypt(struct adapter *padapter, struct recv_frame *precvframe)
+{
+	/*  exclude ICV */
+	int	length;
+	u32	keylength;
+	u8	*pframe, *payload, *iv, wepkey[16];
+	u8	 keyindex;
+	struct	rx_pkt_attrib	 *prxattrib = &precvframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct arc4_ctx *ctx = &psecuritypriv->recv_arc4_ctx;
+
+	pframe = precvframe->rx_data;
+
+	/* start to decrypt recvframe */
+	if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
+		iv = pframe + prxattrib->hdrlen;
+		keyindex = prxattrib->key_index;
+		keylength = psecuritypriv->dot11DefKeylen[keyindex];
+		memcpy(&wepkey[0], iv, 3);
+		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
+		length = precvframe->len - prxattrib->hdrlen - prxattrib->iv_len;
+
+		payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+
+		/* decrypt payload include icv */
+		arc4_setkey(ctx, wepkey, 3 + keylength);
+		arc4_crypt(ctx, payload, payload, length);
+	}
+}
+
+/* 3		===== TKIP related ===== */
+
+static u32 secmicgetuint32(u8 *p)
+/*  Convert from Byte[] to Us3232 in a portable way */
+{
+	s32 i;
+	u32 res = 0;
+
+	for (i = 0; i < 4; i++)
+		res |= ((u32)(*p++)) << (8 * i);
+
+	return res;
+}
+
+static void secmicputuint32(u8 *p, u32 val)
+/*  Convert from Us3232 to Byte[] in a portable way */
+{
+	long i;
+
+	for (i = 0; i < 4; i++) {
+		*p++ = (u8)(val & 0xff);
+		val >>= 8;
+	}
+
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+/*  Reset the state to the empty message. */
+
+	pmicdata->L = pmicdata->K0;
+	pmicdata->R = pmicdata->K1;
+	pmicdata->nBytesInM = 0;
+	pmicdata->M = 0;
+
+}
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
+{
+	/*  Set the key */
+
+	pmicdata->K0 = secmicgetuint32(key);
+	pmicdata->K1 = secmicgetuint32(key + 4);
+	/*  and reset the message */
+	secmicclear(pmicdata);
+
+}
+
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
+{
+
+	/*  Append the byte to our word-sized buffer */
+	pmicdata->M |= ((unsigned long)b) << (8 * pmicdata->nBytesInM);
+	pmicdata->nBytesInM++;
+	/*  Process the word if it is full. */
+	if (pmicdata->nBytesInM >= 4) {
+		pmicdata->L ^= pmicdata->M;
+		pmicdata->R ^= ROL32(pmicdata->L, 17);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROL32(pmicdata->L, 3);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROR32(pmicdata->L, 2);
+		pmicdata->L += pmicdata->R;
+		/*  Clear the buffer */
+		pmicdata->M = 0;
+		pmicdata->nBytesInM = 0;
+	}
+
+}
+
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
+{
+
+	/*  This is simple */
+	while (nbytes > 0) {
+		rtw_secmicappendbyte(pmicdata, *src++);
+		nbytes--;
+	}
+
+}
+
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst)
+{
+
+	/*  Append the minimum padding */
+	rtw_secmicappendbyte(pmicdata, 0x5a);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	/*  and then zeroes until the length is a multiple of 4 */
+	while (pmicdata->nBytesInM != 0)
+		rtw_secmicappendbyte(pmicdata, 0);
+	/*  The appendByte function has already computed the result. */
+	secmicputuint32(dst, pmicdata->L);
+	secmicputuint32(dst + 4, pmicdata->R);
+	/*  Reset to the empty message. */
+	secmicclear(pmicdata);
+
+}
+
+void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
+{
+	struct mic_data	micdata;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+
+	rtw_secmicsetkey(&micdata, key);
+	priority[0] = pri;
+
+	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+	if (header[1] & 1) {   /* ToDS == 1 */
+		rtw_secmicappend(&micdata, &header[16], 6);  /* DA */
+		if (header[1] & 2)  /* From Ds == 1 */
+			rtw_secmicappend(&micdata, &header[24], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+	} else {	/* ToDS == 0 */
+		rtw_secmicappend(&micdata, &header[4], 6);   /* DA */
+		if (header[1] & 2)  /* From Ds == 1 */
+			rtw_secmicappend(&micdata, &header[16], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+	}
+	rtw_secmicappend(&micdata, &priority[0], 4);
+
+	rtw_secmicappend(&micdata, data, data_len);
+
+	rtw_secgetmic(&micdata, mic_code);
+
+}
+
+/* macros for extraction/creation of unsigned char/unsigned short values  */
+#define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
+#define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
+#define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
+#define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
+#define  TK16(N)     Mk16(tk[2 * (N) + 1], tk[2 * (N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
+#define TA_SIZE	   6    /*  48-bit transmitter address       */
+#define TK_SIZE	  16    /* 128-bit temporal key	      */
+#define P1K_SIZE	 10    /*  80-bit Phase1 key		*/
+#define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256] = {  /* Sbox for hash (can be in ROM)     */
+{
+   0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+   0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+   0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+   0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+   0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+   0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+   0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+   0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+   0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+   0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+   0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+   0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+   0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+   0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+   0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+   0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+   0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+   0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+   0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+   0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+   0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+   0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+   0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+   0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+   0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+   0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+   0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+   0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+   0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+   0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+   0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+   0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+  },
+
+  {  /* second half of table is unsigned char-reversed version of first! */
+   0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+   0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+   0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+   0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+   0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+   0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+   0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+   0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+   0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+   0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+   0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+   0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+   0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+   0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+   0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+   0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+   0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+   0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+   0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+   0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+   0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+   0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+   0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+   0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+   0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+   0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+   0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+   0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+   0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+   0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+   0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+   0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+  }
+};
+
+ /*
+**********************************************************************
+* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+*
+* Inputs:
+*     tk[]      = temporal key			 [128 bits]
+*     ta[]      = transmitter's MAC address	    [ 48 bits]
+*     iv32      = upper 32 bits of IV		  [ 32 bits]
+* Output:
+*     p1k[]     = Phase 1 key			  [ 80 bits]
+*
+* Note:
+*     This function only needs to be called every 2**16 packets,
+*     although in theory it could be called every packet.
+*
+**********************************************************************
+*/
+static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
+{
+	int  i;
+
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
+	p1k[0]      = Lo16(iv32);
+	p1k[1]      = Hi16(iv32);
+	p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
+	p1k[3]      = Mk16(ta[3], ta[2]);
+	p1k[4]      = Mk16(ta[5], ta[4]);
+
+	/* Now compute an unbalanced Feistel cipher with 80-bit block */
+	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+	for (i = 0; i < PHASE1_LOOP_CNT; i++) { /* Each add operation here is mod 2**16 */
+		p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0));
+		p1k[4] +=  (unsigned short)i;   /* avoid "slide attacks" */
+	}
+
+}
+
+/*
+**********************************************************************
+* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+*
+* Inputs:
+*     tk[]      = Temporal key			 [128 bits]
+*     p1k[]     = Phase 1 output key		   [ 80 bits]
+*     iv16      = low 16 bits of IV counter	    [ 16 bits]
+* Output:
+*     rc4key[]  = the key used to encrypt the packet   [128 bits]
+*
+* Note:
+*     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
+*     across all packets using the same key TK value. Then, for a
+*     given value of TK[], this TKIP48 construction guarantees that
+*     the final RC4KEY value is unique across all packets.
+*
+* Suggested implementation optimization: if PPK[] is "overlaid"
+*     appropriately on RC4KEY[], there is no need for the final
+*     for loop below that copies the PPK[] result into RC4KEY[].
+*
+**********************************************************************
+*/
+static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
+{
+	int  i;
+	u16 PPK[6];			/* temporary key for mixing    */
+
+	/* Note: all adds in the PPK[] equations below are mod 2**16	 */
+	for (i = 0; i < 5; i++)
+		PPK[i] = p1k[i];	/* first, copy P1K to PPK      */
+	PPK[5]  =  p1k[4] + iv16;	/* next,  add in IV16	  */
+
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]	   */
+	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
+	PPK[1] +=    _S_(PPK[0] ^ TK16(1));
+	PPK[2] +=    _S_(PPK[1] ^ TK16(2));
+	PPK[3] +=    _S_(PPK[2] ^ TK16(3));
+	PPK[4] +=    _S_(PPK[3] ^ TK16(4));
+	PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
+
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
+	PPK[2] +=  RotR1(PPK[1]);
+	PPK[3] +=  RotR1(PPK[2]);
+	PPK[4] +=  RotR1(PPK[3]);
+	PPK[5] +=  RotR1(PPK[4]);
+	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
+	/*       value PPK[0..5] is guaranteed to be unique, as a function   */
+	/*       of the 96-bit "input" value   {TA, IV32, IV16}. That is, P1K  */
+	/*       is now a keyed permutation of {TA, IV32, IV16}.	       */
+
+	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
+	rc4key[0] = Hi8(iv16);		/* RC4KEY[0..2] is the WEP IV  */
+	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
+	rc4key[2] = Lo8(iv16);
+	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
+	for (i = 0; i < 6; i++) {
+		rc4key[4 + 2 * i] = Lo8(PPK[i]);
+		rc4key[5 + 2 * i] = Hi8(PPK[i]);
+	}
+
+}
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{																	/*  exclude ICV */
+	u16	pnl;
+	u32	pnh;
+	u8	rc4key[16];
+	u8   ttkey[16];
+	union {
+		__le32 f0;
+		u8 f1[4];
+	} crc;
+	u8   hw_hdr_offset = 0;
+	int			curfragnum, length;
+
+	u8	*pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	struct	sta_info		*stainfo;
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct arc4_ctx *ctx = &psecuritypriv->xmit_arc4_ctx;
+	u32	res = _SUCCESS;
+
+	if (!pxmitframe->buf_addr)
+		return _FAIL;
+
+	hw_hdr_offset = TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ;
+	pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _TKIP_) {
+		if (pattrib->psta)
+			stainfo = pattrib->psta;
+		else
+			stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
+
+		if (stainfo) {
+			if (is_multicast_ether_addr(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				iv = pframe + pattrib->hdrlen;
+				payload = pframe + pattrib->iv_len + pattrib->hdrlen;
+
+				GET_TKIP_PN(iv, dot11txpn);
+
+				pnl = (u16)(dot11txpn.val);
+				pnh = (u32)(dot11txpn.val >> 16);
+				phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
+				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
+
+				if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+					arc4_setkey(ctx, rc4key, 16);
+					arc4_crypt(ctx, payload, payload, length);
+					arc4_crypt(ctx, payload + length, crc.f1, 4);
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+					arc4_setkey(ctx, rc4key, 16);
+					arc4_crypt(ctx, payload, payload, length);
+					arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+					pframe += pxmitpriv->frag_len;
+					pframe = PTR_ALIGN(pframe, 4);
+				}
+			}
+		} else {
+			res = _FAIL;
+		}
+	}
+
+	return res;
+}
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_decrypt(struct adapter *padapter, struct recv_frame *precvframe)
+{																	/*  exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8   rc4key[16];
+	u8   ttkey[16];
+	union {
+		__le32 f0;
+		u8 f1[4];
+	} crc;
+	int			length;
+
+	u8	*pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	 *prxattrib = &precvframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct arc4_ctx *ctx = &psecuritypriv->recv_arc4_ctx;
+	u32		res = _SUCCESS;
+
+	pframe = precvframe->rx_data;
+
+	/* 4 start to decrypt recvframe */
+	if (prxattrib->encrypt == _TKIP_) {
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+		if (stainfo) {
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				if (!psecuritypriv->binstallGrpkey) {
+					res = _FAIL;
+					goto exit;
+				}
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+			} else {
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+			}
+
+			iv = pframe + prxattrib->hdrlen;
+			payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+			length = precvframe->len - prxattrib->hdrlen - prxattrib->iv_len;
+
+			GET_TKIP_PN(iv, dot11txpn);
+
+			pnl = (u16)(dot11txpn.val);
+			pnh = (u32)(dot11txpn.val >> 16);
+
+			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+			/* 4 decrypt payload include icv */
+
+			arc4_setkey(ctx, rc4key, 16);
+			arc4_crypt(ctx, payload, payload, length);
+
+			crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+			if (crc.f1[3] != payload[length - 1] ||
+			    crc.f1[2] != payload[length - 2] ||
+			    crc.f1[1] != payload[length - 3] ||
+			    crc.f1[0] != payload[length - 4])
+				res = _FAIL;
+		} else {
+			res = _FAIL;
+		}
+	}
+
+exit:
+	return res;
+}
+
+/* 3			===== AES related ===== */
+
+#define MAX_MSG_SIZE	2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+static  u8 sbox_table[256] = {
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
+static void construct_mic_iv(u8 *mic_header1, int qc_exists, int a4_exists, u8 *mpdu, uint payload_length, u8 *pn_vector);
+static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu);
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists);
+static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c);
+static void xor_128(u8 *a, u8 *b, u8 *out);
+static void xor_32(u8 *a, u8 *b, u8 *out);
+static u8 sbox(u8 a);
+static void next_key(u8 *key, int round);
+static void byte_sub(u8 *in, u8 *out);
+static void shift_row(u8 *in, u8 *out);
+static void mix_column(u8 *in, u8 *out);
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
+
+/****************************************/
+/* aes128k128d()			*/
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.			*/
+/****************************************/
+static void xor_128(u8 *a, u8 *b, u8 *out)
+{
+	int i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = a[i] ^ b[i];
+
+}
+
+static void xor_32(u8 *a, u8 *b, u8 *out)
+{
+	int i;
+
+	for (i = 0; i < 4; i++)
+		out[i] = a[i] ^ b[i];
+
+}
+
+static u8 sbox(u8 a)
+{
+	return sbox_table[(int)a];
+}
+
+static void next_key(u8 *key, int round)
+{
+	u8 rcon;
+	u8 sbox_key[4];
+	u8 rcon_table[12] = {
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = sbox(key[13]);
+	sbox_key[1] = sbox(key[14]);
+	sbox_key[2] = sbox(key[15]);
+	sbox_key[3] = sbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+
+}
+
+static void byte_sub(u8 *in, u8 *out)
+{
+	int i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = sbox(in[i]);
+
+}
+
+static void shift_row(u8 *in, u8 *out)
+{
+
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+
+}
+
+static void mix_column(u8 *in, u8 *out)
+{
+	int i;
+	u8 add1b[4];
+	u8 add1bf7[4];
+	u8 rotl[4];
+	u8 swap_halfs[4];
+	u8 andf7[4];
+	u8 rotr[4];
+	u8 temp[4];
+	u8 tempb[4];
+
+	for (i = 0 ; i < 4; i++) {
+		if ((in[i] & 0x80) == 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halves */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];	/* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i > 0; i--) {    /* logical shift left 1 bit */
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i - 1] & 0x80) == 0x80)
+			andf7[i] = (andf7[i] | 0x01);
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];	 /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl, tempb);
+	xor_32(temp, tempb, out);
+
+}
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+	int round;
+	int i;
+	u8 intermediatea[16];
+	u8 intermediateb[16];
+	u8 round_key[16];
+
+	for (i = 0; i < 16; i++)
+		round_key[i] = key[i];
+	for (round = 0; round < 11; round++) {
+		if (round == 0) {
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		} else if (round == 10) {
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		} else {    /* 1 - 9 */
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+/************************************************/
+/* construct_mic_iv()			   */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
+			     uint payload_length, u8 *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC	   */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;	/* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];	/* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+	for (i = 8; i < 14; i++)
+		mic_iv[i] = pn_vector[13 - i];	/* mic_iv[8:13] = PN[5:0] */
+	mic_iv[14] = (unsigned char)(payload_length / 256);
+	mic_iv[15] = (unsigned char)(payload_length % 256);
+
+}
+
+/************************************************/
+/* construct_mic_header1()		      */
+/* Builds the first MIC header block from       */
+/* header fields.			       */
+/************************************************/
+static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
+{
+
+	mic_header1[0] = (u8)((header_length - 2) / 256);
+	mic_header1[1] = (u8)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+
+}
+
+/************************************************/
+/* construct_mic_header2()		      */
+/* Builds the last MIC header block from	*/
+/* header fields.			       */
+/************************************************/
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists)
+{
+	int i;
+
+	for (i = 0; i < 16; i++)
+		mic_header2[i] = 0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	mic_header2[6] = 0x00;
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if (!qc_exists && a4_exists) {
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
+	}
+
+	if (qc_exists && !a4_exists) {
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists) {
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+
+}
+
+/************************************************/
+/* construct_mic_header2()		      */
+/* Builds the last MIC header block from	*/
+/* header fields.			       */
+/************************************************/
+static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c)
+{
+	int i;
+
+	for (i = 0; i < 16; i++)
+		ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;				  /* flag */
+	if (qc_exists && a4_exists)
+		ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
+	if (qc_exists && !a4_exists)
+		ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];		       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] =    pn_vector[13 - i];	  /* ctr_preload[8:13] = PN[5:0] */
+	ctr_preload[14] =  (unsigned char)(c / 256); /* Ctr */
+	ctr_preload[15] =  (unsigned char)(c % 256);
+
+}
+
+/************************************/
+/* bitwise_xor()		    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+	int i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = ina[i] ^ inb[i];
+
+}
+
+static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
+{
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+		num_blocks, payload_index;
+
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype = frsubtype >> 4;
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) || (frtype == WIFI_DATA_CFACKPOLL)) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+	} else if ((frsubtype == 0x08) || (frsubtype == 0x09) || (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
+
+	construct_mic_header1(mic_header1, hdrlen, pframe);
+	construct_mic_header2(mic_header2, pframe, a4_exists, qc_exists);
+
+	payload_remainder = plen % 16;
+	num_blocks = plen / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	for (j = 0; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		pframe[payload_index + j] = mic[j]; /* message[payload_index+j] = mic[j]; */
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i + 1);
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {    /* If there is a short final block, then pad it,*/
+					/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks + 1);
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, 0);
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = pframe[j + hdrlen + 8 + plen];
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		pframe[payload_index++] = chain_buffer[j];
+
+	return _SUCCESS;
+}
+
+u32 rtw_aes_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{	/*  exclude ICV */
+
+	/*static*/
+/*	unsigned char	message[MAX_MSG_SIZE]; */
+
+	/* Intermediate Buffers */
+	int	curfragnum, length;
+	u8	*pframe, *prwskey;	/*  *payload,*iv */
+	u8   hw_hdr_offset = 0;
+	struct	sta_info		*stainfo;
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+
+/*	uint	offset = 0; */
+	u32 res = _SUCCESS;
+
+	if (!pxmitframe->buf_addr)
+		return _FAIL;
+
+	hw_hdr_offset = TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ;
+	pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _AES_) {
+		if (pattrib->psta)
+			stainfo = pattrib->psta;
+		else
+			stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
+
+		if (stainfo) {
+			if (is_multicast_ether_addr(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+
+					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+
+					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+					pframe += pxmitpriv->frag_len;
+					pframe = PTR_ALIGN(pframe, 4);
+				}
+			}
+		} else {
+			res = _FAIL;
+		}
+	}
+
+	return res;
+}
+
+static int aes_decipher(u8 *key, uint hdrlen,
+			u8 *pframe, uint plen)
+{
+	static u8	message[MAX_MSG_SIZE];
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+			num_blocks, payload_index;
+	int res = _SUCCESS;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+
+/*	uint	offset = 0; */
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype = frsubtype >> 4;
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	/* start to decrypt the payload */
+
+	num_blocks = (plen - 8) / 16; /* plen including llc, payload_length and mic) */
+
+	payload_remainder = (plen - 8) % 16;
+
+	pn_vector[0]  = pframe[hdrlen];
+	pn_vector[1]  = pframe[hdrlen + 1];
+	pn_vector[2]  = pframe[hdrlen + 4];
+	pn_vector[3]  = pframe[hdrlen + 5];
+	pn_vector[4]  = pframe[hdrlen + 6];
+	pn_vector[5]  = pframe[hdrlen + 7];
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) ||
+	    (frtype == WIFI_DATA_CFACKPOLL)) {
+			qc_exists = 1;
+			if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+				hdrlen += 2;
+	} else if ((frsubtype == 0x08) || (frsubtype == 0x09) ||
+		   (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+
+	/*  now, decrypt pframe with hdrlen offset and plen long */
+
+	payload_index = hdrlen + 8; /*  8 is for extiv */
+
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i + 1);
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+		for (j = 0; j < 16; j++)
+			 pframe[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {    /* If there is a short final block, then pad it,*/
+					/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks + 1);
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	/* start to calculate the mic */
+	if ((hdrlen + plen + 8) <= MAX_MSG_SIZE)
+		memcpy(message, pframe, (hdrlen + plen + 8)); /* 8 is for ext iv len */
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen - 8, pn_vector);
+
+	construct_mic_header1(mic_header1, hdrlen, message);
+	construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
+
+	payload_remainder = (plen - 8) % 16;
+	num_blocks = (plen - 8) / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index++];
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	for (j = 0 ; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		message[payload_index + j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i + 1);
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, num_blocks + 1);
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, 0);
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = message[j + hdrlen + 8 + plen - 8];
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		message[payload_index++] = chain_buffer[j];
+
+	/* compare the mic */
+	for (i = 0; i < 8; i++) {
+		if (pframe[hdrlen + 8 + plen - 8 + i] != message[hdrlen + 8 + plen - 8 + i])
+			res = _FAIL;
+	}
+
+	return res;
+}
+
+u32 rtw_aes_decrypt(struct adapter *padapter, struct recv_frame *precvframe)
+{	/*  exclude ICV */
+	/* Intermediate Buffers */
+	int		length;
+	u8	*pframe, *prwskey;	/*  *payload,*iv */
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	 *prxattrib = &precvframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	u32	res = _SUCCESS;
+
+	pframe = precvframe->rx_data;
+
+	/* 4 start to encrypt each fragment */
+	if (prxattrib->encrypt == _AES_) {
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+		if (stainfo) {
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				/* in concurrent we should use sw descrypt in group key, so we remove this message */
+				if (!psecuritypriv->binstallGrpkey) {
+					res = _FAIL;
+					goto exit;
+				}
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
+					res = _FAIL;
+					goto exit;
+				}
+			} else {
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+			}
+			length = precvframe->len - prxattrib->hdrlen - prxattrib->iv_len;
+			res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
+		} else {
+			res = _FAIL;
+		}
+	}
+
+exit:
+	return res;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_sta_mgt.c b/drivers/staging/r8188eu/core/rtw_sta_mgt.c
new file mode 100644
index 000000000..98eeb16ca
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_sta_mgt.c
@@ -0,0 +1,502 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTW_STA_MGT_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/sta_info.h"
+
+static void _rtw_init_stainfo(struct sta_info *psta)
+{
+
+	memset((u8 *)psta, 0, sizeof(struct sta_info));
+
+	spin_lock_init(&psta->lock);
+	INIT_LIST_HEAD(&psta->list);
+	INIT_LIST_HEAD(&psta->hash_list);
+	rtw_init_queue(&psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+	INIT_LIST_HEAD(&psta->asoc_list);
+
+	INIT_LIST_HEAD(&psta->auth_list);
+
+	psta->expire_to = 0;
+
+	psta->flags = 0;
+
+	psta->capability = 0;
+
+	psta->bpairwise_key_installed = false;
+
+	psta->nonerp_set = 0;
+	psta->no_short_slot_time_set = 0;
+	psta->no_short_preamble_set = 0;
+	psta->no_ht_gf_set = 0;
+	psta->no_ht_set = 0;
+	psta->ht_20mhz_set = 0;
+
+	psta->under_exist_checking = 0;
+
+	psta->keep_alive_trycnt = 0;
+}
+
+u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+	pstapriv->pallocated_stainfo_buf = vzalloc(sizeof(struct sta_info) * NUM_STA + 4);
+
+	if (!pstapriv->pallocated_stainfo_buf)
+		return _FAIL;
+
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+		((size_t)(pstapriv->pallocated_stainfo_buf) & 3);
+
+	rtw_init_queue(&pstapriv->free_sta_queue);
+
+	spin_lock_init(&pstapriv->sta_hash_lock);
+
+	pstapriv->asoc_sta_count = 0;
+	rtw_init_queue(&pstapriv->sleep_q);
+	rtw_init_queue(&pstapriv->wakeup_q);
+
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+	for (i = 0; i < NUM_STA; i++) {
+		_rtw_init_stainfo(psta);
+
+		INIT_LIST_HEAD(&pstapriv->sta_hash[i]);
+
+		list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+		psta++;
+	}
+
+	pstapriv->sta_dz_bitmap = 0;
+	pstapriv->tim_bitmap = 0;
+
+	INIT_LIST_HEAD(&pstapriv->asoc_list);
+	INIT_LIST_HEAD(&pstapriv->auth_list);
+	spin_lock_init(&pstapriv->asoc_list_lock);
+	spin_lock_init(&pstapriv->auth_list_lock);
+	pstapriv->asoc_list_cnt = 0;
+	pstapriv->auth_list_cnt = 0;
+
+	pstapriv->auth_to = 3; /*  3*2 = 6 sec */
+	pstapriv->assoc_to = 3;
+	pstapriv->expire_to = 3; /*  3*2 = 6 sec */
+	pstapriv->max_num_sta = NUM_STA;
+
+	return _SUCCESS;
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	return (((u8 *)sta) - stapriv->pstainfo_buf) / sizeof(struct sta_info);
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+void _rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct list_head *phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int	index;
+
+	if (pstapriv) {
+		/*	delete all reordering_ctrl_timer		*/
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		for (index = 0; index < NUM_STA; index++) {
+			phead = &pstapriv->sta_hash[index];
+			plist = phead->next;
+
+			while (phead != plist) {
+				int i;
+				psta = container_of(plist, struct sta_info, hash_list);
+				plist = plist->next;
+
+				for (i = 0; i < 16; i++) {
+					preorder_ctrl = &psta->recvreorder_ctrl[i];
+					_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+				}
+			}
+		}
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+		/*===============================*/
+
+		vfree(pstapriv->pallocated_stainfo_buf);
+	}
+}
+
+static void _rtw_reordering_ctrl_timeout_handler(struct timer_list *t)
+{
+	struct recv_reorder_ctrl *preorder_ctrl;
+
+	preorder_ctrl = from_timer(preorder_ctrl, t, reordering_ctrl_timer);
+	rtw_reordering_ctrl_timeout_handler(preorder_ctrl);
+}
+
+static void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
+{
+	timer_setup(&preorder_ctrl->reordering_ctrl_timer, _rtw_reordering_ctrl_timeout_handler, 0);
+}
+
+static void _addba_timer_hdl(struct timer_list *t)
+{
+	struct sta_info *psta = from_timer(psta, t, addba_retry_timer);
+
+	addba_timer_hdl(psta);
+}
+
+static void init_addba_retry_timer(struct adapter *padapter, struct sta_info *psta)
+{
+	timer_setup(&psta->addba_retry_timer, _addba_timer_hdl, 0);
+}
+
+struct	sta_info *rtw_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	s32	index;
+	struct list_head *phash_list;
+	struct sta_info	*psta;
+	struct __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	spin_lock_bh(&pfree_sta_queue->lock);
+
+	if (list_empty(&pfree_sta_queue->queue)) {
+		spin_unlock_bh(&pfree_sta_queue->lock);
+		psta = NULL;
+	} else {
+		psta = container_of((&pfree_sta_queue->queue)->next, struct sta_info, list);
+		list_del_init(&psta->list);
+		spin_unlock_bh(&pfree_sta_queue->lock);
+		_rtw_init_stainfo(psta);
+		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+		index = wifi_mac_hash(hwaddr);
+		if (index >= NUM_STA) {
+			psta = NULL;
+			goto exit;
+		}
+		phash_list = &pstapriv->sta_hash[index];
+
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+
+		list_add_tail(&psta->hash_list, phash_list);
+
+		pstapriv->asoc_sta_count++;
+
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+/*  Commented by Albert 2009/08/13 */
+/*  For the SMC router, the sequence number of first packet of WPS handshake will be 0. */
+/*  In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable. */
+/*  So, we initialize the tid_rxseq variable as the 0xffff. */
+
+		for (i = 0; i < 16; i++)
+			memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2);
+
+		init_addba_retry_timer(pstapriv->padapter, psta);
+
+		/* for A-MPDU Rx reordering buffer control */
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+			preorder_ctrl->padapter = pstapriv->padapter;
+
+			preorder_ctrl->enable = false;
+
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+			preorder_ctrl->wsize_b = 64;/* 64; */
+
+			rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+			rtw_init_recv_timer(preorder_ctrl);
+		}
+
+		/* init for DM */
+		psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+		psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+
+		/* init for the sequence number of received management frame */
+		psta->RxMgmtFrameSeqNum = 0xffff;
+	}
+
+exit:
+
+	return psta;
+}
+
+/*  using pstapriv->sta_hash_lock to protect */
+u32	rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	struct __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv	*pstaxmitpriv;
+	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		goto exit;
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+
+	list_del_init(&pstaxmitpriv->vo_q.tx_pending);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+
+	list_del_init(&pstaxmitpriv->vi_q.tx_pending);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+
+	list_del_init(&pstaxmitpriv->bk_q.tx_pending);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+
+	list_del_init(&pstaxmitpriv->be_q.tx_pending);
+
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	list_del_init(&psta->hash_list);
+	pstapriv->asoc_sta_count--;
+
+	/*  re-init sta_info; 20061114 */
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+	_cancel_timer_ex(&psta->addba_retry_timer);
+
+	/* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+	for (i = 0; i < 16 ; i++) {
+		struct list_head *phead, *plist;
+		struct recv_frame *prframe;
+		struct __queue *ppending_recvframe_queue;
+		struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		spin_lock_bh(&ppending_recvframe_queue->lock);
+
+		phead =		get_list_head(ppending_recvframe_queue);
+		plist = phead->next;
+
+		while (!list_empty(phead)) {
+			prframe = container_of(plist, struct recv_frame, list);
+
+			plist = plist->next;
+
+			list_del_init(&prframe->list);
+
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+		}
+
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+	}
+
+	if (!(psta->state & WIFI_AP_STATE))
+		rtl8188e_SetHalODMVar(padapter, psta, false);
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&psta->auth_list)) {
+		list_del_init(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	psta->expire_to = 0;
+
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+	psta->has_legacy_ac = 0;
+
+	pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+	if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}
+
+	psta->under_exist_checking = 0;
+
+	spin_lock_bh(&pfree_sta_queue->lock);
+	list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+	spin_unlock_bh(&pfree_sta_queue->lock);
+
+exit:
+
+	return _SUCCESS;
+}
+
+/*  free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(struct adapter *padapter)
+{
+	struct list_head *plist, *phead;
+	s32	index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
+
+	if (pstapriv->asoc_sta_count == 1)
+		return;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &pstapriv->sta_hash[index];
+		plist = phead->next;
+
+		while (phead != plist) {
+			psta = container_of(plist, struct sta_info, hash_list);
+
+			plist = plist->next;
+
+			if (pbcmc_stainfo != psta)
+				rtw_free_stainfo(padapter, psta);
+		}
+	}
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	struct list_head *plist, *phead;
+	struct sta_info *psta = NULL;
+	u32	index;
+	u8 *addr;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if (!hwaddr)
+		return NULL;
+
+	if (IS_MCAST(hwaddr))
+		addr = bc_addr;
+	else
+		addr = hwaddr;
+
+	index = wifi_mac_hash(addr);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = &pstapriv->sta_hash[index];
+	plist = phead->next;
+
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, hash_list);
+
+		if ((!memcmp(psta->hwaddr, addr, ETH_ALEN))) {
+			/*  if found the matched address */
+			break;
+		}
+		psta = NULL;
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	return psta;
+}
+
+u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
+{
+	struct sta_info		*psta;
+	u32 res = _SUCCESS;
+	unsigned char bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+	if (!psta) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/*  default broadcast & multicast use macid 1 */
+	psta->mac_id = 1;
+
+exit:
+
+	return res;
+}
+
+struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter)
+{
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	psta = rtw_get_stainfo(pstapriv, bc_addr);
+
+	return psta;
+}
+
+u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr)
+{
+	u8 res = true;
+	struct list_head *plist, *phead;
+	struct rtw_wlan_acl_node *paclnode;
+	u8 match = false;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+	spin_lock_bh(&pacl_node_q->lock);
+	phead = get_list_head(pacl_node_q);
+	plist = phead->next;
+	while (phead != plist) {
+		paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+		plist = plist->next;
+
+		if (!memcmp(paclnode->addr, mac_addr, ETH_ALEN)) {
+			if (paclnode->valid) {
+				match = true;
+				break;
+			}
+		}
+	}
+	spin_unlock_bh(&pacl_node_q->lock);
+
+	if (pacl_list->mode == 1)/* accept unless in deny list */
+		res = !match;
+	else if (pacl_list->mode == 2)/* deny unless in accept list */
+		res = match;
+	else
+		res = true;
+
+	return res;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_wlan_util.c b/drivers/staging/r8188eu/core/rtw_wlan_util.c
new file mode 100644
index 000000000..e50631848
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_wlan_util.c
@@ -0,0 +1,1555 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_WLAN_UTIL_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+
+static unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+static unsigned char ARTHEROS_OUI2[] = {0x00, 0x13, 0x74};
+
+static unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18};
+static unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7};
+
+static unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96};
+static unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43};
+static unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43};
+static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
+static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
+static unsigned char EPIGRAM_OUI[] = {0x00, 0x90, 0x4c};
+
+unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+#define R2T_PHY_DELAY	(0)
+
+/* define WAIT_FOR_BCN_TO_M	(3000) */
+#define WAIT_FOR_BCN_TO_MIN	(6000)
+#define WAIT_FOR_BCN_TO_MAX	(20000)
+
+static u8 rtw_basic_rate_cck[4] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_ofdm[3] = {
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_mix[7] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
+};
+
+bool cckrates_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+
+	for (i = 0; i < ratelen; i++) {
+		if  ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
+		     (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+			return true;
+	}
+	return false;
+}
+
+bool cckratesonly_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+
+	for (i = 0; i < ratelen; i++) {
+		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+		    (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+			return false;
+	}
+
+	return true;
+}
+
+unsigned char networktype_to_raid(unsigned char network_type)
+{
+	unsigned char raid;
+
+	switch (network_type) {
+	case WIRELESS_11B:
+		raid = RATR_INX_WIRELESS_B;
+		break;
+	case WIRELESS_11G:
+		raid = RATR_INX_WIRELESS_G;
+		break;
+	case WIRELESS_11BG:
+		raid = RATR_INX_WIRELESS_GB;
+		break;
+	case WIRELESS_11_24N:
+		raid = RATR_INX_WIRELESS_N;
+		break;
+	case WIRELESS_11G_24N:
+		raid = RATR_INX_WIRELESS_NG;
+		break;
+	case WIRELESS_11BG_24N:
+		raid = RATR_INX_WIRELESS_NGB;
+		break;
+	default:
+		raid = RATR_INX_WIRELESS_GB;
+		break;
+	}
+	return raid;
+}
+
+u8 judge_network_type(struct adapter *padapter, unsigned char *rate, int ratelen)
+{
+	u8 network_type = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pmlmeext->cur_channel > 14) {
+		network_type |= WIRELESS_INVALID;
+	} else {
+		if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_24N;
+
+		if (cckratesonly_included(rate, ratelen))
+			network_type |= WIRELESS_11B;
+		else if (cckrates_included(rate, ratelen))
+			network_type |= WIRELESS_11BG;
+		else
+			network_type |= WIRELESS_11G;
+	}
+	return	network_type;
+}
+
+static unsigned char ratetbl_val_2wifirate(unsigned char rate)
+{
+	unsigned char val = 0;
+
+	switch (rate & 0x7f) {
+	case 0:
+		val = IEEE80211_CCK_RATE_1MB;
+		break;
+	case 1:
+		val = IEEE80211_CCK_RATE_2MB;
+		break;
+	case 2:
+		val = IEEE80211_CCK_RATE_5MB;
+		break;
+	case 3:
+		val = IEEE80211_CCK_RATE_11MB;
+		break;
+	case 4:
+		val = IEEE80211_OFDM_RATE_6MB;
+		break;
+	case 5:
+		val = IEEE80211_OFDM_RATE_9MB;
+		break;
+	case 6:
+		val = IEEE80211_OFDM_RATE_12MB;
+		break;
+	case 7:
+		val = IEEE80211_OFDM_RATE_18MB;
+		break;
+	case 8:
+		val = IEEE80211_OFDM_RATE_24MB;
+		break;
+	case 9:
+		val = IEEE80211_OFDM_RATE_36MB;
+		break;
+	case 10:
+		val = IEEE80211_OFDM_RATE_48MB;
+		break;
+	case 11:
+		val = IEEE80211_OFDM_RATE_54MB;
+		break;
+	}
+	return val;
+}
+
+static bool is_basicrate(struct adapter *padapter, unsigned char rate)
+{
+	int i;
+	unsigned char val;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		val = pmlmeext->basicrate[i];
+
+		if ((val != 0xff) && (val != 0xfe)) {
+			if (rate == ratetbl_val_2wifirate(val))
+				return true;
+		}
+	}
+	return false;
+}
+
+static unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset)
+{
+	int i;
+	unsigned char rate;
+	unsigned int	len = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		rate = pmlmeext->datarate[i];
+
+		switch (rate) {
+		case 0xff:
+			return len;
+		case 0xfe:
+			continue;
+		default:
+			rate = ratetbl_val_2wifirate(rate);
+
+			if (is_basicrate(padapter, rate))
+				rate |= IEEE80211_BASIC_RATE_MASK;
+
+			rateset[len] = rate;
+			len++;
+			break;
+		}
+	}
+	return len;
+}
+
+void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+{
+	unsigned char supportedrates[NumRates];
+
+	memset(supportedrates, 0, NumRates);
+	*bssrate_len = ratetbl2rateset(padapter, supportedrates);
+	memcpy(pbssrate, supportedrates, *bssrate_len);
+}
+
+void Save_DM_Func_Flag(struct adapter *padapter)
+{
+	struct hal_data_8188e *haldata = &padapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	odmpriv->BK_SupportAbility = odmpriv->SupportAbility;
+}
+
+void Restore_DM_Func_Flag(struct adapter *padapter)
+{
+	struct hal_data_8188e *haldata = &padapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	odmpriv->SupportAbility = odmpriv->BK_SupportAbility;
+}
+
+void Set_MSR(struct adapter *padapter, u8 type)
+{
+	u8 val8;
+	int res;
+
+	res = rtw_read8(padapter, MSR, &val8);
+	if (res)
+		return;
+
+	val8 &= 0x0c;
+	val8 |= type;
+	rtw_write8(padapter, MSR, val8);
+}
+
+inline u8 rtw_get_oper_ch(struct adapter *adapter)
+{
+	return adapter->mlmeextpriv.oper_channel;
+}
+
+inline void rtw_set_oper_ch(struct adapter *adapter, u8 ch)
+{
+	adapter->mlmeextpriv.oper_channel = ch;
+}
+
+inline void rtw_set_oper_bw(struct adapter *adapter, u8 bw)
+{
+	adapter->mlmeextpriv.oper_bwmode = bw;
+}
+
+inline void rtw_set_oper_choffset(struct adapter *adapter, u8 offset)
+{
+	adapter->mlmeextpriv.oper_ch_offset = offset;
+}
+
+void SelectChannel(struct adapter *padapter, unsigned char channel)
+{
+	/* saved channel info */
+	rtw_set_oper_ch(padapter, channel);
+	PHY_SwChnl8188E(padapter, channel);
+}
+
+void SetBWMode(struct adapter *padapter, unsigned short bwmode,
+	       unsigned char channel_offset)
+{
+	/* saved bw info */
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	PHY_SetBWMode8188E(padapter, (enum ht_channel_width)bwmode, channel_offset);
+}
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+	u8 center_ch;
+
+	if ((bwmode == HT_CHANNEL_WIDTH_20) ||
+	    (channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)) {
+		/* SelectChannel(padapter, channel); */
+		center_ch = channel;
+	} else {
+		/* switch to the proper channel */
+		if (channel_offset == HAL_PRIME_CHNL_OFFSET_LOWER) {
+			/* SelectChannel(padapter, channel + 2); */
+			center_ch = channel + 2;
+		} else {
+			/* SelectChannel(padapter, channel - 2); */
+			center_ch = channel - 2;
+		}
+	}
+
+	/* set Channel */
+	/* saved channel/bw info */
+	rtw_set_oper_ch(padapter, channel);
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	PHY_SwChnl8188E(padapter, center_ch); /*  set center channel */
+	SetBWMode(padapter, bwmode, channel_offset);
+}
+
+__inline u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork)
+{
+	return pnetwork->MacAddress;
+}
+
+u16 get_beacon_interval(struct wlan_bssid_ex *bss)
+{
+	__le16 val;
+	memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2);
+
+	return le16_to_cpu(val);
+}
+
+int is_client_associated_to_ap(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	if (!padapter)
+		return _FAIL;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_client_associated_to_ibss(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_IBSS_empty(struct adapter *padapter)
+{
+	unsigned int i;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+		if (pmlmeinfo->FW_sta_info[i].status == 1)
+			return _FAIL;
+	}
+	return true;
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+	if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+		return WAIT_FOR_BCN_TO_MIN;
+	else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+		return WAIT_FOR_BCN_TO_MAX;
+	else
+		return bcn_interval << 2;
+}
+
+void invalidate_cam_all(struct adapter *padapter)
+{
+	rtw_write32(padapter, RWCAM, BIT(31) | BIT(30));
+}
+
+void write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
+{
+	unsigned int	i, val, addr;
+	int j;
+	u32	cam_val[2];
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--) {
+		switch (j) {
+		case 0:
+			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
+			break;
+		case 1:
+			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+			break;
+		default:
+			i = (j - 2) << 2;
+			val = (key[i] | (key[i + 1] << 8) | (key[i + 2] << 16) | (key[i + 3] << 24));
+			break;
+		}
+
+		cam_val[0] = val;
+		cam_val[1] = addr + (unsigned int)j;
+
+		rtw_write32(padapter, WCAMI, cam_val[0]);
+		rtw_write32(padapter, RWCAM, CAM_POLLINIG | CAM_WRITE | cam_val[1]);
+	}
+}
+
+void clear_cam_entry(struct adapter *padapter, u8 entry)
+{
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	write_cam(padapter, entry, 0, null_sta, null_key);
+}
+
+int allocate_fw_sta_entry(struct adapter *padapter)
+{
+	unsigned int mac_id;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
+		if (pmlmeinfo->FW_sta_info[mac_id].status == 0) {
+			pmlmeinfo->FW_sta_info[mac_id].status = 1;
+			pmlmeinfo->FW_sta_info[mac_id].retry = 0;
+			break;
+		}
+	}
+
+	return mac_id;
+}
+
+void flush_all_cam_entry(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	rtw_write32(padapter, RWCAM, BIT(31) | BIT(30));
+
+	memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+}
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+	/* struct registry_priv	*pregpriv = &padapter->registrypriv; */
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pmlmepriv->qospriv.qos_option == 0) {
+		pmlmeinfo->WMM_enable = 0;
+		return _FAIL;
+	}
+
+	pmlmeinfo->WMM_enable = 1;
+	memcpy(&pmlmeinfo->WMM_param, pIE->data + 6, sizeof(struct WMM_para_element));
+	return true;
+}
+
+static void set_acm_ctrl(struct adapter *adapter, u8 acm_mask)
+{
+	u8 acmctrl;
+	int res = rtw_read8(adapter, REG_ACMHWCTRL, &acmctrl);
+
+	if (res)
+		return;
+
+	if (acm_mask > 1)
+		acmctrl = acmctrl | 0x1;
+
+	if (acm_mask & BIT(3))
+		acmctrl |= ACMHW_VOQEN;
+	else
+		acmctrl &= (~ACMHW_VOQEN);
+
+	if (acm_mask & BIT(2))
+		acmctrl |= ACMHW_VIQEN;
+	else
+		acmctrl &= (~ACMHW_VIQEN);
+
+	if (acm_mask & BIT(1))
+		acmctrl |= ACMHW_BEQEN;
+	else
+		acmctrl &= (~ACMHW_BEQEN);
+
+	rtw_write8(adapter, REG_ACMHWCTRL, acmctrl);
+}
+
+void WMMOnAssocRsp(struct adapter *padapter)
+{
+	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8	acm_mask;
+	u16	TXOP;
+	u32	acParm, i;
+	u32	edca[4], inx[4];
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct hal_data_8188e *haldata = &padapter->haldata;
+
+	if (pmlmeinfo->WMM_enable == 0) {
+		padapter->mlmepriv.acm_mask = 0;
+		return;
+	}
+
+	acm_mask = 0;
+
+	if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+		aSifsTime = 10;
+	else
+		aSifsTime = 16;
+
+	for (i = 0; i < 4; i++) {
+		ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03;
+		ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
+
+		/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
+		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+
+		ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+		ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
+		TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
+
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+
+		switch (ACI) {
+		case 0x0:
+			haldata->AcParam_BE = acParm;
+			rtw_write32(padapter, REG_EDCA_BE_PARAM, acParm);
+			acm_mask |= (ACM ? BIT(1) : 0);
+			edca[XMIT_BE_QUEUE] = acParm;
+			break;
+		case 0x1:
+			rtw_write32(padapter, REG_EDCA_BK_PARAM, acParm);
+			edca[XMIT_BK_QUEUE] = acParm;
+			break;
+		case 0x2:
+			rtw_write32(padapter, REG_EDCA_VI_PARAM, acParm);
+			acm_mask |= (ACM ? BIT(2) : 0);
+			edca[XMIT_VI_QUEUE] = acParm;
+			break;
+		case 0x3:
+			rtw_write32(padapter, REG_EDCA_VO_PARAM, acParm);
+			acm_mask |= (ACM ? BIT(3) : 0);
+			edca[XMIT_VO_QUEUE] = acParm;
+			break;
+		}
+	}
+
+	if (padapter->registrypriv.acm_method == 1)
+		set_acm_ctrl(padapter, acm_mask);
+	else
+		padapter->mlmepriv.acm_mask = acm_mask;
+
+	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+	if (pregpriv->wifi_spec == 1) {
+		u32 j, change_inx = false;
+
+		/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
+		for (i = 0; i < 4; i++) {
+			for (j = i + 1; j < 4; j++) {
+				/* compare CW and AIFS */
+				if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) {
+					change_inx = true;
+				} else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+					/* compare TXOP */
+					if ((edca[j] >> 16) > (edca[i] >> 16))
+						change_inx = true;
+				}
+
+				if (change_inx) {
+					swap(edca[i], edca[j]);
+					swap(inx[i], inx[j]);
+
+					change_inx = false;
+				}
+			}
+		}
+	}
+
+	for (i = 0; i < 4; i++)
+		pxmitpriv->wmm_para_seq[i] = inx[i];
+}
+
+static void bwmode_update_check(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+	unsigned char	 new_bwmode;
+	unsigned char  new_ch_offset;
+	struct HT_info_element	 *pHT_info;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if (!pIE)
+		return;
+
+	if (!phtpriv)
+		return;
+
+	if (pIE->Length > sizeof(struct HT_info_element))
+		return;
+
+	pHT_info = (struct HT_info_element *)pIE->data;
+
+	if ((pHT_info->infos[0] & BIT(2)) && pregistrypriv->cbw40_enable) {
+		new_bwmode = HT_CHANNEL_WIDTH_40;
+
+		switch (pHT_info->infos[0] & 0x3) {
+		case 1:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+		case 3:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+		default:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+	} else {
+		new_bwmode = HT_CHANNEL_WIDTH_20;
+		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	if ((new_bwmode != pmlmeext->cur_bwmode) ||
+	    (new_ch_offset != pmlmeext->cur_ch_offset)) {
+		pmlmeinfo->bwmode_updated = true;
+
+		pmlmeext->cur_bwmode = new_bwmode;
+		pmlmeext->cur_ch_offset = new_ch_offset;
+
+		/* update HT info also */
+		HT_info_handler(padapter, pIE);
+	} else {
+		pmlmeinfo->bwmode_updated = false;
+	}
+
+	if (pmlmeinfo->bwmode_updated) {
+		struct sta_info *psta;
+		struct wlan_bssid_ex	*cur_network = &pmlmeinfo->network;
+		struct sta_priv	*pstapriv = &padapter->stapriv;
+
+		/* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+
+		/* update ap's stainfo */
+		psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+		if (psta) {
+			struct ht_priv	*phtpriv_sta = &psta->htpriv;
+
+			if (phtpriv_sta->ht_option) {
+				/*  bwmode */
+				phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
+				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+			} else {
+				phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
+				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			}
+		}
+	}
+}
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+	unsigned int	i;
+	u8	max_AMPDU_len, min_MPDU_spacing;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if (!pIE)
+		return;
+
+	if (!phtpriv->ht_option)
+		return;
+
+	pmlmeinfo->HT_caps_enable = 1;
+
+	for (i = 0; i < (pIE->Length); i++) {
+		if (i != 2) {
+			/* 	Got the endian issue here. */
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+		} else {
+			/* modify from  fw by Thomas 2010/11/17 */
+			max_AMPDU_len = min(pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3,
+					    pIE->data[i] & 0x3);
+
+			min_MPDU_spacing = max(pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c,
+					       pIE->data[i] & 0x1c);
+
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+		}
+	}
+
+	/* update the MCS rates */
+	for (i = 0; i < 16; i++)
+		pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+}
+
+void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if (!pIE)
+		return;
+
+	if (!phtpriv->ht_option)
+		return;
+
+	if (pIE->Length > sizeof(struct HT_info_element))
+		return;
+
+	pmlmeinfo->HT_info_enable = 1;
+	memcpy(&pmlmeinfo->HT_info, pIE->data, pIE->Length);
+}
+
+static void set_min_ampdu_spacing(struct adapter *adapter, u8 spacing)
+{
+	u8 sec_spacing;
+	int res;
+
+	if (spacing <= 7) {
+		switch (adapter->securitypriv.dot11PrivacyAlgrthm) {
+		case _NO_PRIVACY_:
+		case _AES_:
+			sec_spacing = 0;
+			break;
+		case _WEP40_:
+		case _WEP104_:
+		case _TKIP_:
+		case _TKIP_WTMIC_:
+			sec_spacing = 6;
+			break;
+		default:
+			sec_spacing = 7;
+			break;
+		}
+
+		if (spacing < sec_spacing)
+			spacing = sec_spacing;
+
+		res = rtw_read8(adapter, REG_AMPDU_MIN_SPACE, &sec_spacing);
+		if (res)
+			return;
+
+		rtw_write8(adapter, REG_AMPDU_MIN_SPACE,
+			   (sec_spacing & 0xf8) | spacing);
+	}
+}
+
+static void set_ampdu_factor(struct adapter *adapter, u8 factor)
+{
+	u8 RegToSet_Normal[4] = {0x41, 0xa8, 0x72, 0xb9};
+	u8 FactorToSet;
+	u8 *pRegToSet;
+	u8 index = 0;
+
+	pRegToSet = RegToSet_Normal; /*  0xb972a841; */
+	FactorToSet = factor;
+	if (FactorToSet <= 3) {
+		FactorToSet = (1 << (FactorToSet + 2));
+		if (FactorToSet > 0xf)
+			FactorToSet = 0xf;
+
+		for (index = 0; index < 4; index++) {
+			if ((pRegToSet[index] & 0xf0) > (FactorToSet << 4))
+				pRegToSet[index] = (pRegToSet[index] & 0x0f) | (FactorToSet << 4);
+
+			if ((pRegToSet[index] & 0x0f) > FactorToSet)
+				pRegToSet[index] = (pRegToSet[index] & 0xf0) | (FactorToSet);
+
+			rtw_write8(adapter, (REG_AGGLEN_LMT + index), pRegToSet[index]);
+		}
+	}
+}
+
+void HTOnAssocRsp(struct adapter *padapter)
+{
+	unsigned char		max_AMPDU_len;
+	unsigned char		min_MPDU_spacing;
+	/* struct registry_priv	 *pregpriv = &padapter->registrypriv; */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) {
+		pmlmeinfo->HT_enable = 1;
+	} else {
+		pmlmeinfo->HT_enable = 0;
+		return;
+	}
+
+	/* handle A-MPDU parameter field */
+	/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+	*/
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+	set_min_ampdu_spacing(padapter, min_MPDU_spacing);
+
+	set_ampdu_factor(padapter, max_AMPDU_len);
+}
+
+void ERP_IE_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pIE->Length > 1)
+		return;
+
+	pmlmeinfo->ERP_enable = 1;
+	memcpy(&pmlmeinfo->ERP_IE, pIE->data, pIE->Length);
+}
+
+void VCS_update(struct adapter *padapter, struct sta_info *psta)
+{
+	struct registry_priv	 *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */
+	case 0: /* off */
+		psta->rtsen = 0;
+		psta->cts2self = 0;
+		break;
+	case 1: /* on */
+		if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+			psta->rtsen = 1;
+			psta->cts2self = 0;
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 1;
+		}
+		break;
+	case 2: /* auto */
+	default:
+		if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+			if (pregpriv->vcs_type == 1) {
+				psta->rtsen = 1;
+				psta->cts2self = 0;
+			} else {
+				psta->rtsen = 0;
+				psta->cts2self = 1;
+			}
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 0;
+		}
+		break;
+	}
+}
+
+int rtw_check_bcn_info(struct adapter  *Adapter, u8 *pframe, u32 packet_len)
+{
+	unsigned int		len;
+	unsigned char		*p;
+	unsigned short	val16, subtype;
+	struct wlan_network *cur_network = &Adapter->mlmepriv.cur_network;
+	/* u8 wpa_ie[255], rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	u8 encryp_protocol = 0;
+	struct wlan_bssid_ex *bssid;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
+	unsigned char *pbuf;
+	u32 wpa_ielen = 0;
+	u8 *pbssid = GetAddr3Ptr(pframe);
+	u32 hidden_ssid = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct ieee80211_ht_cap *pht_cap = NULL;
+	u32 bcn_channel;
+	unsigned short	ht_cap_info;
+	unsigned char	ht_info_infos_0;
+
+	if (!is_client_associated_to_ap(Adapter))
+		return true;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	if (memcmp(cur_network->network.MacAddress, pbssid, 6))
+		return true;
+
+	bssid = kzalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+	if (!bssid)
+		return _FAIL;
+
+	subtype = GetFrameSubType(pframe) >> 4;
+
+	if (subtype == WIFI_BEACON)
+		bssid->Reserved[0] = 1;
+
+	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* check bw and channel offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+		ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else {
+		ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			ht_info_infos_0 = 0;
+	}
+	if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
+	    ((ht_info_infos_0 & 0x03) != (cur_network->BcnInfo.ht_info_infos_0 & 0x03))) {
+			/* bcn_info_update */
+			cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+			cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+			/* to do : need to check that whether modify related register of BB or not */
+			/* goto _mismatch; */
+	}
+
+	/* Checking for channel */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p) {
+			bcn_channel = *(p + 2);
+	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+			p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+			if (pht_info)
+				bcn_channel = pht_info->primary_channel;
+			else /* we don't find channel IE, so don't check it */
+				bcn_channel = Adapter->mlmeextpriv.cur_channel;
+	}
+	if (bcn_channel != Adapter->mlmeextpriv.cur_channel)
+		goto _mismatch;
+
+	/* checking SSID */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (!p)
+		hidden_ssid = true;
+	else
+		hidden_ssid = false;
+
+	if (p && (!hidden_ssid && (*(p + 1)))) {
+		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	} else {
+		bssid->Ssid.SsidLength = 0;
+		bssid->Ssid.Ssid[0] = '\0';
+	}
+
+	if (memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) ||
+	    bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) {
+		/* not hidden ssid */
+		if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0)
+			goto _mismatch;
+	}
+
+	/* check encryption info */
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	if (cur_network->network.Privacy != bssid->Privacy)
+		goto _mismatch;
+
+	rtw_get_sec_ie(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bssid->Privacy)
+			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+
+	if (cur_network->BcnInfo.encryp_protocol != encryp_protocol)
+		goto _mismatch;
+
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		pbuf = rtw_get_wpa_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength - 12);
+		if (pbuf && (wpa_ielen > 0)) {
+			rtw_parse_wpa_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is_8021x);
+		} else {
+			pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength - 12);
+
+			if (pbuf && (wpa_ielen > 0))
+				rtw_parse_wpa2_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is_8021x);
+		}
+
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher ||
+		    group_cipher != cur_network->BcnInfo.group_cipher)
+			goto _mismatch;
+
+		if (is_8021x != cur_network->BcnInfo.is_8021x)
+			goto _mismatch;
+	}
+
+	kfree(bssid);
+
+	return _SUCCESS;
+
+_mismatch:
+	kfree(bssid);
+
+	return _FAIL;
+}
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+	unsigned int i;
+	unsigned int len;
+	struct ndis_802_11_var_ie *pIE;
+
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;) {
+		pIE = (struct ndis_802_11_var_ie *)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+
+		switch (pIE->ElementID) {
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			/* HT_info_handler(padapter, pIE); */
+			bwmode_update_check(padapter, pIE);
+			break;
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+			VCS_update(padapter, psta);
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+}
+
+bool is_ap_in_tkip(struct adapter *padapter)
+{
+	u32 i;
+	struct ndis_802_11_var_ie *pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex		*cur_network = &pmlmeinfo->network;
+
+	if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+			pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
+
+			switch (pIE->ElementID) {
+			case _VENDOR_SPECIFIC_IE_:
+				if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4)))
+					return true;
+				break;
+			case _RSN_IE_2_:
+				if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4))
+					return true;
+				break;
+			default:
+				break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+		return false;
+	} else {
+		return false;
+	}
+}
+
+int wifirate2_ratetbl_inx(unsigned char rate)
+{
+	int	inx = 0;
+	rate = rate & 0x7f;
+
+	switch (rate) {
+	case 54 * 2:
+		inx = 11;
+		break;
+	case 48 * 2:
+		inx = 10;
+		break;
+	case 36 * 2:
+		inx = 9;
+		break;
+	case 24 * 2:
+		inx = 8;
+		break;
+	case 18 * 2:
+		inx = 7;
+		break;
+	case 12 * 2:
+		inx = 6;
+		break;
+	case 9 * 2:
+		inx = 5;
+		break;
+	case 6 * 2:
+		inx = 4;
+		break;
+	case 11 * 2:
+		inx = 3;
+		break;
+	case 11:
+		inx = 2;
+		break;
+	case 2 * 2:
+		inx = 1;
+		break;
+	case 1 * 2:
+		inx = 0;
+		break;
+	}
+	return inx;
+}
+
+unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+	for (i = 0; i < num_of_rate; i++) {
+		if ((*(ptn + i)) & 0x80)
+			mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+	}
+	return mask;
+}
+
+unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+	for (i = 0; i < num_of_rate; i++)
+		mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+	return mask;
+}
+
+unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps)
+{
+	unsigned int mask = 0;
+
+	mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+
+	return mask;
+}
+
+int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps)
+{
+	unsigned char					bit_offset;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (!(pmlmeinfo->HT_enable))
+		return _FAIL;
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK)
+		return _FAIL;
+
+	bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40) ? 6 : 5;
+
+	if (__le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & (0x1 << bit_offset))
+		return _SUCCESS;
+	else
+		return _FAIL;
+}
+
+unsigned char get_highest_rate_idx(u32 mask)
+{
+	int i;
+	unsigned char rate_idx = 0;
+
+	for (i = 27; i >= 0; i--) {
+		if (mask & BIT(i)) {
+			rate_idx = i;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+void Update_RA_Entry(struct adapter *padapter, u32 mac_id)
+{
+	rtw_hal_update_ra_mask(padapter, mac_id, 0);
+}
+
+static void enable_rate_adaptive(struct adapter *padapter, u32 mac_id)
+{
+	Update_RA_Entry(padapter, mac_id);
+}
+
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta)
+{
+	/* rate adaptive */
+	enable_rate_adaptive(padapter, psta->mac_id);
+}
+
+void rtw_set_basic_rate(struct adapter *adapter, u8 *rates)
+{
+	u16 BrateCfg = 0;
+	u8 RateIndex = 0;
+	int res;
+	u8 reg;
+
+	/*  2007.01.16, by Emily */
+	/*  Select RRSR (in Legacy-OFDM and CCK) */
+	/*  For 8190, we select only 24M, 12M, 6M, 11M, 5.5M, 2M, and 1M from the Basic rate. */
+	/*  We do not use other rates. */
+	HalSetBrateCfg(adapter, rates, &BrateCfg);
+
+	/* 2011.03.30 add by Luke Lee */
+	/* CCK 2M ACK should be disabled for some BCM and Atheros AP IOT */
+	/* because CCK 2M has poor TXEVM */
+	/* CCK 5.5M & 11M ACK should be enabled for better performance */
+
+	BrateCfg = (BrateCfg | 0xd) & 0x15d;
+
+	BrateCfg |= 0x01; /*  default enable 1M ACK rate */
+	/*  Set RRSR rate table. */
+	rtw_write8(adapter, REG_RRSR, BrateCfg & 0xff);
+	rtw_write8(adapter, REG_RRSR + 1, (BrateCfg >> 8) & 0xff);
+	res = rtw_read8(adapter, REG_RRSR + 2, &reg);
+	if (res)
+		return;
+
+	rtw_write8(adapter, REG_RRSR + 2, reg & 0xf0);
+
+	/*  Set RTS initial rate */
+	while (BrateCfg > 0x1) {
+		BrateCfg = (BrateCfg >> 1);
+		RateIndex++;
+	}
+	/*  Ziv - Check */
+	rtw_write8(adapter, REG_INIRTS_RATE_SEL, RateIndex);
+}
+
+/*  Update RRSR and Rate for USERATE */
+void update_tx_basic_rate(struct adapter *padapter, u8 wirelessmode)
+{
+	unsigned char supported_rates[NDIS_802_11_LENGTH_RATES_EX];
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/* 	Added by Albert 2011/03/22 */
+	/* 	In the P2P mode, the driver should not support the b mode. */
+	/* 	So, the Tx packet shouldn't use the CCK rate */
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+	memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B))
+		memcpy(supported_rates, rtw_basic_rate_cck, 4);
+	else if (wirelessmode & WIRELESS_11B)
+		memcpy(supported_rates, rtw_basic_rate_mix, 7);
+	else
+		memcpy(supported_rates, rtw_basic_rate_ofdm, 3);
+
+	if (wirelessmode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+
+	rtw_set_basic_rate(padapter, supported_rates);
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+	unsigned int i;
+	struct ndis_802_11_var_ie *pIE;
+	u8	epigram_vendor_flag;
+	u8	ralink_vendor_flag;
+	epigram_vendor_flag = 0;
+	ralink_vendor_flag = 0;
+
+	for (i = sizeof(struct ndis_802_11_fixed_ie); i < len;) {
+		pIE = (struct ndis_802_11_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) ||
+			    (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+				return HT_IOT_PEER_ATHEROS;
+			} else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
+				   (!memcmp(pIE->data, BROADCOM_OUI2, 3))) {
+				return HT_IOT_PEER_BROADCOM;
+			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+				return HT_IOT_PEER_MARVELL;
+			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
+				if (!ralink_vendor_flag) {
+					ralink_vendor_flag = 1;
+				} else {
+					return HT_IOT_PEER_RALINK;
+				}
+			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+				return HT_IOT_PEER_CISCO;
+			} else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+				return HT_IOT_PEER_REALTEK;
+			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+				return HT_IOT_PEER_AIRGO;
+			} else if (!memcmp(pIE->data, EPIGRAM_OUI, 3)) {
+				epigram_vendor_flag = 1;
+				if (ralink_vendor_flag)
+					return HT_IOT_PEER_TENDA;
+			} else {
+				break;
+			}
+			break;
+
+		default:
+			break;
+		}
+		i += (pIE->Length + 2);
+	}
+
+	if (ralink_vendor_flag && !epigram_vendor_flag)
+		return HT_IOT_PEER_RALINK;
+	else if (ralink_vendor_flag && epigram_vendor_flag)
+		return HT_IOT_PEER_TENDA;
+	else
+		return HT_IOT_PEER_UNKNOWN;
+}
+
+void update_IOT_info(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	switch (pmlmeinfo->assoc_AP_vendor) {
+	case HT_IOT_PEER_MARVELL:
+		pmlmeinfo->turboMode_cts2self = 1;
+		pmlmeinfo->turboMode_rtsen = 0;
+		break;
+	case HT_IOT_PEER_RALINK:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		break;
+	case HT_IOT_PEER_REALTEK:
+		/* rtw_write16(padapter, 0x4cc, 0xffff); */
+		/* rtw_write16(padapter, 0x546, 0x01c0); */
+		break;
+	default:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		break;
+	}
+}
+
+static void set_ack_preamble(struct adapter *adapter, bool short_preamble)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	u8 val8;
+
+	/*  Joseph marked out for Netgear 3500 TKIP channel 7 issue.(Temporarily) */
+	val8 = haldata->nCur40MhzPrimeSC << 5;
+	if (short_preamble)
+		val8 |= 0x80;
+
+	rtw_write8(adapter, REG_RRSR + 2, val8);
+};
+
+static void set_slot_time(struct adapter *adapter, u8 slot_time)
+{
+	u8 u1bAIFS, aSifsTime;
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	rtw_write8(adapter, REG_SLOT, slot_time);
+
+	if (pmlmeinfo->WMM_enable == 0) {
+		if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+			aSifsTime = 10;
+		else
+			aSifsTime = 16;
+
+		u1bAIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
+
+		/*  <Roger_EXP> Temporary removed, 2008.06.20. */
+		rtw_write8(adapter, REG_EDCA_VO_PARAM, u1bAIFS);
+		rtw_write8(adapter, REG_EDCA_VI_PARAM, u1bAIFS);
+		rtw_write8(adapter, REG_EDCA_BE_PARAM, u1bAIFS);
+		rtw_write8(adapter, REG_EDCA_BK_PARAM, u1bAIFS);
+	}
+}
+
+void update_capinfo(struct adapter *Adapter, u16 updateCap)
+{
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/*  Check preamble mode, 2005.01.06, by rcnjko. */
+	/*  Mark to update preamble value forever, 2008.03.18 by lanhsin */
+
+	if (updateCap & cShortPreamble) { /*  Short Preamble */
+		if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /*  PREAMBLE_LONG or PREAMBLE_AUTO */
+			pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
+			set_ack_preamble(Adapter, true);
+		}
+	} else { /*  Long Preamble */
+		if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) {  /*  PREAMBLE_SHORT or PREAMBLE_AUTO */
+			pmlmeinfo->preamble_mode = PREAMBLE_LONG;
+			set_ack_preamble(Adapter, false);
+		}
+	}
+
+	if (updateCap & cIBSS) {
+		/* Filen: See 802.11-2007 p.91 */
+		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+	} else { /* Filen: See 802.11-2007 p.90 */
+		if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11_24N)) {
+			if (updateCap & cShortSlotTime) { /*  Short Slot Time */
+				if (pmlmeinfo->slotTime != SHORT_SLOT_TIME)
+					pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+			} else { /*  Long Slot Time */
+				if (pmlmeinfo->slotTime != NON_SHORT_SLOT_TIME)
+					pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+			}
+		} else {
+			/* B Mode */
+			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+		}
+	}
+
+	set_slot_time(Adapter, pmlmeinfo->slotTime);
+}
+
+void update_wireless_mode(struct adapter *padapter)
+{
+	int ratelen, network_type = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex	*cur_network = &pmlmeinfo->network;
+	unsigned char		*rate = cur_network->SupportedRates;
+
+	ratelen = rtw_get_rateset_len(cur_network->SupportedRates);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+		pmlmeinfo->HT_enable = 1;
+
+	if (pmlmeext->cur_channel > 14) {
+		network_type |= WIRELESS_INVALID;
+	} else {
+		if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_24N;
+
+		if (cckratesonly_included(rate, ratelen))
+			network_type |= WIRELESS_11B;
+		else if (cckrates_included(rate, ratelen))
+			network_type |= WIRELESS_11BG;
+		else
+			network_type |= WIRELESS_11G;
+	}
+
+	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+
+	/* RESP_SIFS for CCK */
+	rtw_write8(padapter, REG_R2T_SIFS, 0x08);
+	rtw_write8(padapter, REG_R2T_SIFS + 1, 0x08);
+	/* RESP_SIFS for OFDM */
+	rtw_write8(padapter, REG_T2T_SIFS, 0x0a);
+	rtw_write8(padapter, REG_T2T_SIFS + 1, 0x0a);
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	 else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void update_bmc_sta_support_rate(struct adapter *padapter, u32 mac_id)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
+		/*  Only B, B/G, and B/G/N AP could use CCK rate */
+		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_cck, 4);
+	} else {
+		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_ofdm, 3);
+	}
+}
+
+int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+{
+	unsigned int	ie_len;
+	struct ndis_802_11_var_ie *pIE;
+	int	supportRateNum = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (!pIE)
+		return _FAIL;
+
+	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
+	supportRateNum = ie_len;
+
+	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE)
+		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+
+	return _SUCCESS;
+}
+
+void beacon_timing_control(struct adapter *padapter)
+{
+	SetBeaconRelatedRegisters8188EUsb(padapter);
+}
+
+static struct adapter *pbuddy_padapter;
+
+void rtw_handle_dualmac(struct adapter *adapter, bool init)
+{
+	if (init) {
+		if (!pbuddy_padapter) {
+			pbuddy_padapter = adapter;
+		} else {
+			adapter->pbuddy_adapter = pbuddy_padapter;
+			pbuddy_padapter->pbuddy_adapter = adapter;
+			/*  clear global value */
+			pbuddy_padapter = NULL;
+		}
+	} else {
+		pbuddy_padapter = NULL;
+	}
+}
diff --git a/drivers/staging/r8188eu/core/rtw_xmit.c b/drivers/staging/r8188eu/core/rtw_xmit.c
new file mode 100644
index 000000000..873d2c5c3
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_xmit.c
@@ -0,0 +1,2347 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _RTW_XMIT_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+#include "../include/osdep_intf.h"
+#include "../include/usb_ops.h"
+#include "../include/usb_osintf.h"
+#include "../include/rtl8188e_xmit.h"
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+	INIT_LIST_HEAD(&ptxservq->tx_pending);
+	rtw_init_queue(&ptxservq->sta_pending);
+	ptxservq->qcnt = 0;
+}
+
+void	_rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{
+	memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv));
+	spin_lock_init(&psta_xmitpriv->lock);
+	_init_txservq(&psta_xmitpriv->be_q);
+	_init_txservq(&psta_xmitpriv->bk_q);
+	_init_txservq(&psta_xmitpriv->vi_q);
+	_init_txservq(&psta_xmitpriv->vo_q);
+	INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
+	INIT_LIST_HEAD(&psta_xmitpriv->apsd);
+}
+
+static int rtw_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf,
+				   u32 alloc_sz)
+{
+	pxmitbuf->pallocated_buf = kzalloc(alloc_sz, GFP_KERNEL);
+	if (!pxmitbuf->pallocated_buf)
+		return _FAIL;
+
+	pxmitbuf->pbuf = (u8 *)ALIGN((size_t)(pxmitbuf->pallocated_buf), XMITBUF_ALIGN_SZ);
+	pxmitbuf->dma_transfer_addr = 0;
+
+	pxmitbuf->pxmit_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!pxmitbuf->pxmit_urb) {
+		kfree(pxmitbuf->pallocated_buf);
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+static void rtw_xmit_resource_free(struct adapter *padapter, struct xmit_buf *pxmitbuf,
+				   u32 free_sz)
+{
+	usb_free_urb(pxmitbuf->pxmit_urb);
+	kfree(pxmitbuf->pallocated_buf);
+}
+
+s32	_rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter)
+{
+	int i;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxframe;
+	int	res = _SUCCESS;
+	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+
+	spin_lock_init(&pxmitpriv->lock);
+	sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
+
+	/*
+	 * Please insert all the queue initializaiton using rtw_init_queue below
+	 */
+
+	pxmitpriv->adapter = padapter;
+
+	rtw_init_queue(&pxmitpriv->be_pending);
+	rtw_init_queue(&pxmitpriv->bk_pending);
+	rtw_init_queue(&pxmitpriv->vi_pending);
+	rtw_init_queue(&pxmitpriv->vo_pending);
+	rtw_init_queue(&pxmitpriv->bm_pending);
+
+	rtw_init_queue(&pxmitpriv->free_xmit_queue);
+
+	/*
+	 * Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
+	 * and initialize free_xmit_frame below.
+	 * Please also apply  free_txobj to link_up all the xmit_frames...
+	 */
+
+	pxmitpriv->pallocated_frame_buf = vzalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+	if (!pxmitpriv->pallocated_frame_buf) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)ALIGN((size_t)(pxmitpriv->pallocated_frame_buf), 4);
+	/* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
+	/* 						((size_t) (pxmitpriv->pallocated_frame_buf) &3); */
+
+	pxframe = (struct xmit_frame *)pxmitpriv->pxmit_frame_buf;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&pxframe->list);
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		list_add_tail(&pxframe->list, &pxmitpriv->free_xmit_queue.queue);
+
+		pxframe++;
+	}
+
+	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+
+	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+
+	/* init xmit_buf */
+	rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
+	rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
+
+	pxmitpriv->pallocated_xmitbuf = vzalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (!pxmitpriv->pallocated_xmitbuf) {
+		res = _FAIL;
+		goto free_frame_buf;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)ALIGN((size_t)(pxmitpriv->pallocated_xmitbuf), 4);
+	/* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
+	/* 						((size_t) (pxmitpriv->pallocated_xmitbuf) &3); */
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->ext_tag = false;
+
+		/* Tx buf allocation may fail sometimes, so sleep and retry. */
+		res = rtw_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+		if (res == _FAIL) {
+			msleep(10);
+			res = rtw_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+			if (res == _FAIL)
+				goto free_xmitbuf;
+		}
+
+		pxmitbuf->flags = XMIT_VO_QUEUE;
+
+		list_add_tail(&pxmitbuf->list, &pxmitpriv->free_xmitbuf_queue.queue);
+		pxmitbuf++;
+	}
+
+	pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+
+	/*  Init xmit extension buff */
+	rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+	pxmitpriv->pallocated_xmit_extbuf = vzalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+
+	if (!pxmitpriv->pallocated_xmit_extbuf) {
+		res = _FAIL;
+		goto free_xmitbuf;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)ALIGN((size_t)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < num_xmit_extbuf; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->ext_tag = true;
+
+		res = rtw_xmit_resource_alloc(padapter, pxmitbuf, max_xmit_extbuf_size + XMITBUF_ALIGN_SZ);
+		if (res == _FAIL) {
+			res = _FAIL;
+			goto free_xmit_extbuf;
+		}
+
+		list_add_tail(&pxmitbuf->list, &pxmitpriv->free_xmit_extbuf_queue.queue);
+		pxmitbuf++;
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+	if (rtw_alloc_hwxmits(padapter)) {
+		res = _FAIL;
+		goto free_xmit_extbuf;
+	}
+
+	rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+
+	for (i = 0; i < 4; i++)
+		pxmitpriv->wmm_para_seq[i] = i;
+
+	pxmitpriv->txirp_cnt = 1;
+
+	sema_init(&pxmitpriv->tx_retevt, 0);
+
+	/* per AC pending irp */
+	pxmitpriv->beq_cnt = 0;
+	pxmitpriv->bkq_cnt = 0;
+	pxmitpriv->viq_cnt = 0;
+	pxmitpriv->voq_cnt = 0;
+
+	pxmitpriv->ack_tx = false;
+	mutex_init(&pxmitpriv->ack_tx_mutex);
+	rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
+
+	rtl8188eu_init_xmit_priv(padapter);
+
+	return _SUCCESS;
+
+free_xmit_extbuf:
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	while (i--) {
+		rtw_xmit_resource_free(padapter, pxmitbuf, (max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+		pxmitbuf++;
+	}
+	vfree(pxmitpriv->pallocated_xmit_extbuf);
+	i = NR_XMITBUFF;
+free_xmitbuf:
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+	while (i--) {
+		rtw_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+		pxmitbuf++;
+	}
+	vfree(pxmitpriv->pallocated_xmitbuf);
+free_frame_buf:
+	vfree(pxmitpriv->pallocated_frame_buf);
+exit:
+	return res;
+}
+
+static void rtw_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
+{
+	u16 queue;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	queue = skb_get_queue_mapping(pkt);
+	if (padapter->registrypriv.wifi_spec) {
+		if (__netif_subqueue_stopped(padapter->pnetdev, queue) &&
+		    (pxmitpriv->hwxmits[queue].accnt < WMM_XMIT_THRESHOLD))
+			netif_wake_subqueue(padapter->pnetdev, queue);
+	} else {
+		if (__netif_subqueue_stopped(padapter->pnetdev, queue))
+			netif_wake_subqueue(padapter->pnetdev, queue);
+	}
+
+	dev_kfree_skb_any(pkt);
+}
+
+void rtw_xmit_complete(struct adapter *padapter, struct xmit_frame *pxframe)
+{
+	if (pxframe->pkt)
+		rtw_pkt_complete(padapter, pxframe->pkt);
+	pxframe->pkt = NULL;
+}
+
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+	int i;
+	struct adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+
+	if (!pxmitpriv->pxmit_frame_buf)
+		return;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		rtw_xmit_complete(padapter, pxmitframe);
+
+		pxmitframe++;
+	}
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		rtw_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+		pxmitbuf++;
+	}
+
+	vfree(pxmitpriv->pallocated_frame_buf);
+
+	vfree(pxmitpriv->pallocated_xmitbuf);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	for (i = 0; i < num_xmit_extbuf; i++) {
+		rtw_xmit_resource_free(padapter, pxmitbuf, (max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+		pxmitbuf++;
+	}
+
+	vfree(pxmitpriv->pallocated_xmit_extbuf);
+
+	rtw_free_hwxmits(padapter);
+
+	mutex_destroy(&pxmitpriv->ack_tx_mutex);
+}
+
+static void update_attrib_vcs_info(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u32	sz;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info	*psta = pattrib->psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pattrib->nr_frags != 1)
+		sz = padapter->xmitpriv.frag_len;
+	else /* no frag */
+		sz = pattrib->last_txcmdsz;
+
+	/*  (1) RTS_Threshold is compared to the MPDU, not MSDU. */
+	/*  (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame. */
+	/* 		Other fragments are protected by previous fragment. */
+	/* 		So we only need to check the length of first fragment. */
+	if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec) {
+		if (sz > padapter->registrypriv.rts_thresh) {
+			pattrib->vcs_mode = RTS_CTS;
+		} else {
+			if (psta->rtsen)
+				pattrib->vcs_mode = RTS_CTS;
+			else if (psta->cts2self)
+				pattrib->vcs_mode = CTS_TO_SELF;
+			else
+				pattrib->vcs_mode = NONE_VCS;
+		}
+	} else {
+		while (true) {
+			/* IOT action */
+			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && pattrib->ampdu_en &&
+			    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+				pattrib->vcs_mode = CTS_TO_SELF;
+				break;
+			}
+
+			/* check ERP protection */
+			if (psta->rtsen || psta->cts2self) {
+				if (psta->rtsen)
+					pattrib->vcs_mode = RTS_CTS;
+				else if (psta->cts2self)
+					pattrib->vcs_mode = CTS_TO_SELF;
+
+				break;
+			}
+
+			/* check HT op mode */
+			if (pattrib->ht_en) {
+				u8 htopmode = pmlmeinfo->HT_protection;
+
+				if ((pmlmeext->cur_bwmode && (htopmode == 2 || htopmode == 3)) ||
+				    (!pmlmeext->cur_bwmode && htopmode == 3)) {
+					pattrib->vcs_mode = RTS_CTS;
+					break;
+				}
+			}
+
+			/* check rts */
+			if (sz > padapter->registrypriv.rts_thresh) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			/* to do list: check MIMO power save condition. */
+
+			/* check AMPDU aggregation for TXOP */
+			if (pattrib->ampdu_en) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			pattrib->vcs_mode = NONE_VCS;
+			break;
+		}
+	}
+}
+
+static void update_attrib_phy_info(struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	/*if (psta->rtsen)
+		pattrib->vcs_mode = RTS_CTS;
+	else if (psta->cts2self)
+		pattrib->vcs_mode = CTS_TO_SELF;
+	else
+		pattrib->vcs_mode = NONE_VCS;*/
+
+	pattrib->mdata = 0;
+	pattrib->eosp = 0;
+	pattrib->triggered = 0;
+
+	/* qos_en, ht_en, init rate, , bw, ch_offset, sgi */
+	pattrib->qos_en = psta->qos_option;
+
+	pattrib->raid = psta->raid;
+	pattrib->ht_en = psta->htpriv.ht_option;
+	pattrib->bwmode = psta->htpriv.bwmode;
+	pattrib->ch_offset = psta->htpriv.ch_offset;
+	pattrib->sgi = psta->htpriv.sgi;
+	pattrib->ampdu_en = false;
+	pattrib->retry_ctrl = false;
+}
+
+u8	qos_acm(u8 acm_mask, u8 priority)
+{
+	u8	change_priority = priority;
+
+	switch (priority) {
+	case 0:
+	case 3:
+		if (acm_mask & BIT(1))
+			change_priority = 1;
+		break;
+	case 1:
+	case 2:
+		break;
+	case 4:
+	case 5:
+		if (acm_mask & BIT(2))
+			change_priority = 0;
+		break;
+	case 6:
+	case 7:
+		if (acm_mask & BIT(3))
+			change_priority = 5;
+		break;
+	default:
+		break;
+	}
+
+	return change_priority;
+}
+
+static void rtw_open_pktfile(struct sk_buff *pktptr, struct pkt_file *pfile)
+{
+	if (!pktptr) {
+		pr_err("8188eu: pktptr is NULL\n");
+		return;
+	}
+	if (!pfile) {
+		pr_err("8188eu: pfile is NULL\n");
+		return;
+	}
+	pfile->pkt = pktptr;
+	pfile->cur_addr = pktptr->data;
+	pfile->buf_start = pktptr->data;
+	pfile->pkt_len = pktptr->len;
+	pfile->buf_len = pktptr->len;
+
+	pfile->cur_buffer = pfile->buf_start;
+}
+
+static uint rtw_remainder_len(struct pkt_file *pfile)
+{
+	return pfile->buf_len - ((size_t)(pfile->cur_addr) -
+	       (size_t)(pfile->buf_start));
+}
+
+static uint rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen)
+{
+	uint len;
+
+	len = rtw_remainder_len(pfile);
+	len = (rlen > len) ? len : rlen;
+
+	if (rmem)
+		skb_copy_bits(pfile->pkt, pfile->buf_len - pfile->pkt_len, rmem, len);
+
+	pfile->cur_addr += len;
+	pfile->pkt_len -= len;
+
+	return len;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	s32 user_prio = 0;
+
+	rtw_open_pktfile(ppktfile->pkt, ppktfile);
+	rtw_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	/*  get user_prio from IP hdr */
+	if (pattrib->ether_type == 0x0800) {
+		rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+/* 		user_prio = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
+		user_prio = ip_hdr.tos >> 5;
+	} else if (pattrib->ether_type == 0x888e) {
+		/*  "When priority processing of data frames is supported, */
+		/*  a STA's SME should send EAPOL-Key frames at the highest priority." */
+		user_prio = 7;
+	}
+
+	pattrib->priority = user_prio;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA;
+}
+
+static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct pkt_attrib *pattrib)
+{
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	bool bmcast;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+	int res = _SUCCESS;
+
+
+
+	rtw_open_pktfile(pkt, &pktfile);
+	rtw_pktfile_read(&pktfile, (u8 *)&etherhdr, ETH_HLEN);
+
+	pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
+	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+
+	pattrib->pctrl = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	}
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	if (pattrib->ether_type == ETH_P_IP) {
+		/*  The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */
+		/*  to prevent DHCP protocol fail */
+		u8 tmp[24];
+
+		rtw_pktfile_read(&pktfile, &tmp[0], 24);
+		pattrib->dhcp_pkt = 0;
+		if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
+			if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+			    ((tmp[21] == 67) && (tmp[23] == 68))) {
+				/*  68 : UDP BOOTP client */
+				/*  67 : UDP BOOTP server */
+				/*  Use low rate to send DHCP packet. */
+				pattrib->dhcp_pkt = 1;
+			}
+		}
+	}
+
+	/*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
+	if ((pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+
+	bmcast = is_multicast_ether_addr(pattrib->ra);
+
+	/*  get sta_info */
+	if (bmcast) {
+		psta = rtw_get_bcmc_stainfo(padapter);
+	} else {
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		if (!psta) { /*  if we cannot get psta => drrp the pkt */
+			res = _FAIL;
+			goto exit;
+		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && !(psta->state & _FW_LINKED)) {
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (psta) {
+		pattrib->mac_id = psta->mac_id;
+		pattrib->psta = psta;
+	} else {
+		/*  if we cannot get psta => drop the pkt */
+		res = _FAIL;
+		goto exit;
+	}
+
+	pattrib->ack_policy = 0;
+	/*  get ether_hdr_len */
+	pattrib->pkt_hdrlen = ETH_HLEN;/* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */
+
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = IEEE80211_FTYPE_DATA;
+	pattrib->priority = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+		if (psta->qos_option)
+			set_qos(&pktfile, pattrib);
+	} else {
+		if (pqospriv->qos_option) {
+			set_qos(&pktfile, pattrib);
+
+			if (pmlmepriv->acm_mask != 0)
+				pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
+		}
+	}
+
+	if (psta->ieee8021x_blocked) {
+		pattrib->encrypt = 0;
+
+		if ((pattrib->ether_type != 0x888e) && !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+			res = _FAIL;
+			goto exit;
+		}
+	} else {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+		switch (psecuritypriv->dot11AuthAlgrthm) {
+		case dot11AuthAlgrthm_Open:
+		case dot11AuthAlgrthm_Shared:
+		case dot11AuthAlgrthm_Auto:
+			pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+			break;
+		case dot11AuthAlgrthm_8021X:
+			if (bmcast)
+				pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
+			else
+				pattrib->key_idx = 0;
+			break;
+		default:
+			pattrib->key_idx = 0;
+			break;
+		}
+	}
+
+	switch (pattrib->encrypt) {
+	case _WEP40_:
+	case _WEP104_:
+		pattrib->iv_len = 4;
+		pattrib->icv_len = 4;
+		break;
+	case _TKIP_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 4;
+
+		if (padapter->securitypriv.busetkipkey == _FAIL) {
+			res = _FAIL;
+			goto exit;
+		}
+		break;
+	case _AES_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 8;
+		break;
+	default:
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+		break;
+	}
+
+	if (pattrib->encrypt &&
+	    (padapter->securitypriv.sw_encrypt || !psecuritypriv->hw_decrypted))
+		pattrib->bswenc = true;
+	else
+		pattrib->bswenc = false;
+
+	update_attrib_phy_info(pattrib, psta);
+
+exit:
+
+	return res;
+}
+
+static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	int curfragnum, length;
+	u8	*pframe, *payload, mic[8];
+	struct	mic_data micdata;
+	struct	sta_info *stainfo;
+	struct	pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+	u8 hw_hdr_offset = 0;
+
+	if (pattrib->psta)
+		stainfo = pattrib->psta;
+	else
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
+
+	hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+
+	if (pattrib->encrypt == _TKIP_) {/* if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_PRIVACY_) */
+		/* encode mic code */
+		if (stainfo) {
+			u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+					   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+					   0x0, 0x0};
+
+			pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+			if (is_multicast_ether_addr(pattrib->ra)) {
+				if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16))
+					return _FAIL;
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
+			} else {
+				if (!memcmp(&stainfo->dot11tkiptxmickey.skey[0], null_key, 16)) {
+					/* msleep(10); */
+					return _FAIL;
+				}
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, &stainfo->dot11tkiptxmickey.skey[0]);
+			}
+
+			if (pframe[1] & 1) {   /* ToDS == 1 */
+				rtw_secmicappend(&micdata, &pframe[16], 6);  /* DA */
+				if (pframe[1] & 2)  /* From Ds == 1 */
+					rtw_secmicappend(&micdata, &pframe[24], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+			} else {	/* ToDS == 0 */
+				rtw_secmicappend(&micdata, &pframe[4], 6);   /* DA */
+				if (pframe[1] & 2)  /* From Ds == 1 */
+					rtw_secmicappend(&micdata, &pframe[16], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+			}
+
+			if (pattrib->qos_en)
+				priority[0] = (u8)pxmitframe->attrib.priority;
+
+			rtw_secmicappend(&micdata, &priority[0], 4);
+
+			payload = pframe;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				payload = PTR_ALIGN(payload, 4);
+
+				payload = payload + pattrib->hdrlen + pattrib->iv_len;
+				if ((curfragnum + 1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload + length;
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload + length + pattrib->icv_len;
+				}
+			}
+			rtw_secgetmic(&micdata, &mic[0]);
+			/* add mic code  and add the mic code length in last_txcmdsz */
+
+			memcpy(payload, &mic[0], 8);
+			pattrib->last_txcmdsz += 8;
+
+			payload = payload - pattrib->last_txcmdsz + 8;
+		}
+	}
+
+	return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+
+	if (pattrib->bswenc) {
+		switch (pattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_encrypt(padapter, pxmitframe);
+			break;
+		case _TKIP_:
+			rtw_tkip_encrypt(padapter, pxmitframe);
+			break;
+		case _AES_:
+			rtw_aes_encrypt(padapter, pxmitframe);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	u8 qos_option = false;
+
+	int res = _SUCCESS;
+	__le16 *fctrl = &pwlanhdr->frame_control;
+
+	struct sta_info *psta;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else if (is_multicast_ether_addr(pattrib->ra))
+		psta = rtw_get_bcmc_stainfo(padapter);
+	else
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+
+	SetFrameSubType(fctrl, pattrib->subtype);
+
+	if (pattrib->subtype & IEEE80211_FTYPE_DATA) {
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+			/* to_ds = 1, fr_ds = 0; */
+			/* Data transfer to AP */
+			SetToDs(fctrl);
+			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+
+			if (pqospriv->qos_option)
+				qos_option = true;
+		} else if (check_fwstate(pmlmepriv,  WIFI_AP_STATE)) {
+			/* to_ds = 0, fr_ds = 1; */
+			SetFrDs(fctrl);
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+
+			if (psta->qos_option)
+				qos_option = true;
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+			   check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+			if (psta->qos_option)
+				qos_option = true;
+		} else {
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (pattrib->mdata)
+			SetMData(fctrl);
+
+		if (pattrib->encrypt)
+			SetPrivacy(fctrl);
+
+		if (qos_option) {
+			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+			if (pattrib->priority)
+				SetPriority(qc, pattrib->priority);
+
+			SetEOSP(qc, pattrib->eosp);
+
+			SetAckpolicy(qc, pattrib->ack_policy);
+		}
+
+		/* TODO: fill HT Control Field */
+
+		/* Update Seq Num will be handled by f/w */
+		if (psta) {
+			psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+			psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+
+			pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+
+			SetSeqNum(hdr, pattrib->seqnum);
+
+			/* check if enable ampdu */
+			if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
+				if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+					pattrib->ampdu_en = true;
+			}
+
+			/* re-check if enable ampdu by BA_starting_seqctrl */
+			if (pattrib->ampdu_en) {
+				u16 tx_seq;
+
+				tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+
+				/* check BA_starting_seqctrl */
+				if (SN_LESS(pattrib->seqnum, tx_seq)) {
+					pattrib->ampdu_en = false;/* AGG BK */
+				} else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
+					psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq + 1) & 0xfff;
+
+					pattrib->ampdu_en = true;/* AGG EN */
+				} else {
+					psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum + 1) & 0xfff;
+					pattrib->ampdu_en = true;/* AGG EN */
+				}
+			}
+		}
+	}
+exit:
+
+	return res;
+}
+
+s32 rtw_txframes_pending(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return (!list_empty(&pxmitpriv->be_pending.queue) ||
+			!list_empty(&pxmitpriv->bk_pending.queue) ||
+			!list_empty(&pxmitpriv->vi_pending.queue) ||
+			!list_empty(&pxmitpriv->vo_pending.queue));
+}
+
+s32 rtw_txframes_sta_ac_pending(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+	struct sta_info *psta;
+	struct tx_servq *ptxservq;
+	int priority = pattrib->priority;
+
+	psta = pattrib->psta;
+
+	switch (priority) {
+	case 1:
+	case 2:
+		ptxservq = &psta->sta_xmitpriv.bk_q;
+		break;
+	case 4:
+	case 5:
+		ptxservq = &psta->sta_xmitpriv.vi_q;
+		break;
+	case 6:
+	case 7:
+		ptxservq = &psta->sta_xmitpriv.vo_q;
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &psta->sta_xmitpriv.be_q;
+		break;
+	}
+
+	if (ptxservq)
+		return ptxservq->qcnt;
+	return 0;
+}
+
+/*
+ * This sub-routine will perform all the following:
+ *
+ * 1. remove 802.3 header.
+ * 2. create wlan_header, based on the info in pxmitframe
+ * 3. append sta's iv/ext-iv
+ * 4. append LLC
+ * 5. move frag chunk from pframe to pxmitframe->mem
+ * 6. apply sw-encrypt, if necessary.
+ */
+s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+	u8 *pframe, *mem_start;
+	u8 hw_hdr_offset;
+	struct sta_info		*psta;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+	s32 res = _SUCCESS;
+
+	if (!pkt)
+		return _FAIL;
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+	if (!psta)
+		return _FAIL;
+
+	if (!pxmitframe->buf_addr)
+		return _FAIL;
+
+	pbuf_start = pxmitframe->buf_addr;
+
+	hw_hdr_offset =  TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+
+	mem_start = pbuf_start +	hw_hdr_offset;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	rtw_open_pktfile(pkt, &pktfile);
+	rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
+
+	while (1) {
+		llc_sz = 0;
+
+		mpdu_len = frg_len;
+
+		pframe = mem_start;
+
+		SetMFrag(mem_start);
+
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+
+		/* adding icv, if necessary... */
+		if (pattrib->iv_len) {
+			if (psta) {
+				switch (pattrib->encrypt) {
+				case _WEP40_:
+				case _WEP104_:
+					WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+					break;
+				case _TKIP_:
+					if (bmcst)
+						TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+					else
+						TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+					break;
+				case _AES_:
+					if (bmcst)
+						AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+					else
+						AES_IV(pattrib->iv, psta->dot11txpn, 0);
+					break;
+				}
+			}
+
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+			pframe += pattrib->iv_len;
+
+			mpdu_len -= pattrib->iv_len;
+		}
+
+		if (frg_inx == 0) {
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc))
+			mpdu_len -= pattrib->icv_len;
+
+		if (bmcst) {
+			/*  don't do fragment to broadcast/multicast packets */
+			mem_sz = rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+		} else {
+			mem_sz = rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+		}
+
+		pframe += mem_sz;
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			memcpy(pframe, pattrib->icv, pattrib->icv_len);
+			pframe += pattrib->icv_len;
+		}
+
+		frg_inx++;
+
+		if (bmcst || pktfile.pkt_len == 0) {
+			pattrib->nr_frags = frg_inx;
+
+			pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz : 0) +
+						((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+
+			ClearMFrag(mem_start);
+
+			break;
+		}
+
+		mem_start = PTR_ALIGN(pframe, 4) + hw_hdr_offset;
+		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	if (!bmcst)
+		update_attrib_vcs_info(padapter, pxmitframe);
+	else
+		pattrib->vcs_mode = NONE_VCS;
+
+exit:
+
+	return res;
+}
+
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ *	Organizationally Unique Identifier(OUI), 3 octets,
+ *	type, defined by that organization, 2 octets.
+ */
+s32 rtw_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len)
+{
+	uint	protection;
+	u8	*perp;
+	int	 erp_len;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	switch (pxmitpriv->vcs_setting) {
+	case DISABLE_VCS:
+		pxmitpriv->vcs = NONE_VCS;
+		break;
+	case ENABLE_VCS:
+		break;
+	case AUTO_VCS:
+	default:
+		perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
+		if (!perp) {
+			pxmitpriv->vcs = NONE_VCS;
+		} else {
+			protection = (*(perp + 2)) & BIT(1);
+			if (protection) {
+				if (pregistrypriv->vcs_type == RTS_CTS)
+					pxmitpriv->vcs = RTS_CTS;
+				else
+					pxmitpriv->vcs = CTS_TO_SELF;
+			} else {
+				pxmitpriv->vcs = NONE_VCS;
+			}
+		}
+		break;
+	}
+}
+
+void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz)
+{
+	struct sta_info *psta = NULL;
+	struct stainfo_stats *pstats = NULL;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	if ((pxmitframe->frame_tag & 0x0f) == DATA_FRAMETAG) {
+		pxmitpriv->tx_bytes += sz;
+		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pxmitframe->agg_num;
+
+		psta = pxmitframe->attrib.psta;
+		if (psta) {
+			pstats = &psta->sta_stats;
+			pstats->tx_pkts += pxmitframe->agg_num;
+			pstats->tx_bytes += sz;
+		}
+	}
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pfree_queue->lock, flags);
+
+	if (list_empty(&pfree_queue->queue)) {
+		pxmitbuf = NULL;
+	} else {
+		phead = get_list_head(pfree_queue);
+
+		plist = phead->next;
+
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+		list_del_init(&pxmitbuf->list);
+	}
+
+	if (pxmitbuf) {
+		pxmitpriv->free_xmit_extbuf_cnt--;
+
+		pxmitbuf->priv_data = NULL;
+		/* pxmitbuf->ext_tag = true; */
+
+		if (pxmitbuf->sctx)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+	}
+
+	spin_unlock_irqrestore(&pfree_queue->lock, flags);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+	unsigned long flags;
+
+	if (!pxmitbuf)
+		return _FAIL;
+
+	spin_lock_irqsave(&pfree_queue->lock, flags);
+
+	list_del_init(&pxmitbuf->list);
+
+	list_add_tail(&pxmitbuf->list, get_list_head(pfree_queue));
+	pxmitpriv->free_xmit_extbuf_cnt++;
+
+	spin_unlock_irqrestore(&pfree_queue->lock, flags);
+
+	return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pfree_xmitbuf_queue->lock, flags);
+
+	if (list_empty(&pfree_xmitbuf_queue->queue)) {
+		pxmitbuf = NULL;
+	} else {
+		phead = get_list_head(pfree_xmitbuf_queue);
+
+		plist = phead->next;
+
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+		list_del_init(&pxmitbuf->list);
+	}
+
+	if (pxmitbuf) {
+		pxmitpriv->free_xmitbuf_cnt--;
+		pxmitbuf->priv_data = NULL;
+		if (pxmitbuf->sctx)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+	}
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, flags);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+	unsigned long flags;
+
+	if (!pxmitbuf)
+		return _FAIL;
+
+	if (pxmitbuf->sctx)
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+
+	if (pxmitbuf->ext_tag) {
+		rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+	} else {
+		spin_lock_irqsave(&pfree_xmitbuf_queue->lock, flags);
+
+		list_del_init(&pxmitbuf->list);
+
+		list_add_tail(&pxmitbuf->list, get_list_head(pfree_xmitbuf_queue));
+
+		pxmitpriv->free_xmitbuf_cnt++;
+		spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, flags);
+	}
+
+	return _SUCCESS;
+}
+
+/*
+ * Calling context:
+ * 1. OS_TXENTRY
+ * 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+ *
+ * If we turn on USE_RXTHREAD, then, no need for critical section.
+ * Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+ *
+ * Must be very very cautious...
+ */
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
+{
+	/*
+	 * Please remember to use all the osdep_service api,
+	 * and lock/unlock or _enter/_exit critical to protect
+	 * pfree_xmit_queue
+	 */
+
+	struct xmit_frame *pxframe = NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+	spin_lock_bh(&pfree_xmit_queue->lock);
+
+	if (list_empty(&pfree_xmit_queue->queue)) {
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = phead->next;
+
+		pxframe = container_of(plist, struct xmit_frame, list);
+
+		list_del_init(&pxframe->list);
+	}
+
+	if (pxframe) { /* default value setting */
+		pxmitpriv->free_xmitframe_cnt--;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
+		/* pxframe->attrib.psta = NULL; */
+
+		pxframe->frame_tag = DATA_FRAMETAG;
+
+		pxframe->pkt = NULL;
+		pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */
+
+		pxframe->agg_num = 1;
+		pxframe->ack_report = 0;
+	}
+
+	spin_unlock_bh(&pfree_xmit_queue->lock);
+
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+	struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+	struct adapter *padapter = pxmitpriv->adapter;
+	struct sk_buff *pndis_pkt = NULL;
+
+	if (!pxmitframe)
+		goto exit;
+
+	spin_lock_bh(&pfree_xmit_queue->lock);
+
+	list_del_init(&pxmitframe->list);
+
+	if (pxmitframe->pkt) {
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	list_add_tail(&pxmitframe->list, get_list_head(pfree_xmit_queue));
+
+	pxmitpriv->free_xmitframe_cnt++;
+
+	spin_unlock_bh(&pfree_xmit_queue->lock);
+
+	if (pndis_pkt)
+		rtw_pkt_complete(padapter, pndis_pkt);
+
+exit:
+
+	return _SUCCESS;
+}
+
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue)
+{
+	struct list_head *plist, *phead;
+	struct	xmit_frame	*pxmitframe;
+
+	spin_lock_bh(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pxmitframe = container_of(plist, struct xmit_frame, list);
+
+		plist = plist->next;
+
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+	}
+	spin_unlock_bh(&pframequeue->lock);
+}
+
+s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
+/* 		pxmitframe->pkt = NULL; */
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, struct __queue *pframe_queue)
+{
+	struct list_head *xmitframe_plist, *xmitframe_phead;
+	struct	xmit_frame	*pxmitframe = NULL;
+
+	xmitframe_phead = get_list_head(pframe_queue);
+	xmitframe_plist = xmitframe_phead->next;
+
+	if (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = xmitframe_plist->next;
+
+		list_del_init(&pxmitframe->list);
+
+		ptxservq->qcnt--;
+	}
+	return pxmitframe;
+}
+
+struct xmit_frame *rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, int entry)
+{
+	struct list_head *sta_plist, *sta_phead;
+	struct hw_xmit *phwxmit;
+	struct tx_servq *ptxservq = NULL;
+	struct __queue *pframe_queue = NULL;
+	struct xmit_frame *pxmitframe = NULL;
+	struct adapter *padapter = pxmitpriv->adapter;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	int i, inx[4];
+
+	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+	if (pregpriv->wifi_spec == 1) {
+		int j;
+
+		for (j = 0; j < 4; j++)
+			inx[j] = pxmitpriv->wmm_para_seq[j];
+	}
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	for (i = 0; i < entry; i++) {
+		phwxmit = phwxmit_i + inx[i];
+
+		sta_phead = get_list_head(phwxmit->sta_queue);
+		sta_plist = sta_phead->next;
+
+		while (sta_phead != sta_plist) {
+			ptxservq = container_of(sta_plist, struct tx_servq, tx_pending);
+
+			pframe_queue = &ptxservq->sta_pending;
+
+			pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
+
+			if (pxmitframe) {
+				phwxmit->accnt--;
+
+				/* Remove sta node when there are no pending packets. */
+				if (list_empty(&pframe_queue->queue)) /* must be done after get_next and before break */
+					list_del_init(&ptxservq->tx_pending);
+				goto exit;
+			}
+
+			sta_plist = sta_plist->next;
+		}
+	}
+exit:
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	return pxmitframe;
+}
+
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, int up, u8 *ac)
+{
+	struct tx_servq *ptxservq;
+
+	switch (up) {
+	case 1:
+	case 2:
+		ptxservq = &psta->sta_xmitpriv.bk_q;
+		*(ac) = 3;
+		break;
+	case 4:
+	case 5:
+		ptxservq = &psta->sta_xmitpriv.vi_q;
+		*(ac) = 1;
+		break;
+	case 6:
+	case 7:
+		ptxservq = &psta->sta_xmitpriv.vo_q;
+		*(ac) = 0;
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &psta->sta_xmitpriv.be_q;
+		*(ac) = 2;
+	break;
+	}
+
+	return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8	ac_index;
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct hw_xmit	*phwxmits =  padapter->xmitpriv.hwxmits;
+	int res = _SUCCESS;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+
+	if (!psta) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	if (list_empty(&ptxservq->tx_pending))
+		list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+
+	list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+exit:
+
+	return res;
+}
+
+int rtw_alloc_hwxmits(struct adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+
+	pxmitpriv->hwxmits = kzalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry, GFP_KERNEL);
+	if (!pxmitpriv->hwxmits)
+		return -ENOMEM;
+
+	hwxmits = pxmitpriv->hwxmits;
+
+	hwxmits[0].sta_queue = &pxmitpriv->vo_pending;
+	hwxmits[1].sta_queue = &pxmitpriv->vi_pending;
+	hwxmits[2].sta_queue = &pxmitpriv->be_pending;
+	hwxmits[3].sta_queue = &pxmitpriv->bk_pending;
+
+	return 0;
+}
+
+void rtw_free_hwxmits(struct adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	hwxmits = pxmitpriv->hwxmits;
+	kfree(hwxmits);
+}
+
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, int entry)
+{
+	int i;
+
+	for (i = 0; i < entry; i++, phwxmit++)
+		phwxmit->accnt = 0;
+}
+
+static int rtw_br_client_tx(struct adapter *padapter, struct sk_buff **pskb)
+{
+	struct sk_buff *skb = *pskb;
+	int res, is_vlan_tag = 0, i, do_nat25 = 1;
+	unsigned short vlan_hdr = 0;
+	void *br_port = NULL;
+
+	rcu_read_lock();
+	br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+	rcu_read_unlock();
+	spin_lock_bh(&padapter->br_ext_lock);
+	if (!(skb->data[0] & 1) && br_port &&
+	    memcmp(skb->data + ETH_ALEN, padapter->br_mac, ETH_ALEN) &&
+	    *((__be16 *)(skb->data + ETH_ALEN * 2)) != __constant_htons(ETH_P_8021Q) &&
+	    *((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP) &&
+	    !memcmp(padapter->scdb_mac, skb->data + ETH_ALEN, ETH_ALEN) && padapter->scdb_entry) {
+		memcpy(skb->data + ETH_ALEN, GET_MY_HWADDR(padapter), ETH_ALEN);
+		padapter->scdb_entry->ageing_timer = jiffies;
+		spin_unlock_bh(&padapter->br_ext_lock);
+	} else {
+		if (*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_8021Q)) {
+			is_vlan_tag = 1;
+			vlan_hdr = *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2));
+			for (i = 0; i < 6; i++)
+				*((unsigned short *)(skb->data + ETH_ALEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + ETH_ALEN * 2 - 2 - i * 2));
+			skb_pull(skb, 4);
+		}
+		if (!memcmp(skb->data + ETH_ALEN, padapter->br_mac, ETH_ALEN) &&
+		    (*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)))
+			memcpy(padapter->br_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4);
+
+		if (*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)) {
+			if (memcmp(padapter->scdb_mac, skb->data + ETH_ALEN, ETH_ALEN)) {
+				padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter,
+							skb->data + WLAN_ETHHDR_LEN + 12);
+				if (padapter->scdb_entry) {
+					memcpy(padapter->scdb_mac, skb->data + ETH_ALEN, ETH_ALEN);
+					memcpy(padapter->scdb_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4);
+					padapter->scdb_entry->ageing_timer = jiffies;
+					do_nat25 = 0;
+				}
+			} else {
+				if (padapter->scdb_entry) {
+					padapter->scdb_entry->ageing_timer = jiffies;
+					do_nat25 = 0;
+				} else {
+					memset(padapter->scdb_mac, 0, ETH_ALEN);
+					memset(padapter->scdb_ip, 0, 4);
+				}
+			}
+		}
+		spin_unlock_bh(&padapter->br_ext_lock);
+		if (do_nat25) {
+			if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) {
+				struct sk_buff *newskb;
+
+				if (is_vlan_tag) {
+					skb_push(skb, 4);
+					for (i = 0; i < 6; i++)
+						*((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2));
+					*((__be16 *)(skb->data + ETH_ALEN * 2)) = __constant_htons(ETH_P_8021Q);
+					*((unsigned short *)(skb->data + ETH_ALEN * 2 + 2)) = vlan_hdr;
+				}
+
+				newskb = skb_copy(skb, GFP_ATOMIC);
+				if (!newskb)
+					return -1;
+				dev_kfree_skb_any(skb);
+
+				*pskb = skb = newskb;
+				if (is_vlan_tag) {
+					vlan_hdr = *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2));
+					for (i = 0; i < 6; i++)
+						*((unsigned short *)(skb->data + ETH_ALEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + ETH_ALEN * 2 - 2 - i * 2));
+					skb_pull(skb, 4);
+				}
+			}
+
+			res = skb_linearize(skb);
+			if (res < 0)
+				return -1;
+
+			res = nat25_db_handle(padapter, skb, NAT25_INSERT);
+			if (res < 0) {
+				if (res == -2)
+					return -1;
+
+				return 0;
+			}
+		}
+
+		memcpy(skb->data + ETH_ALEN, GET_MY_HWADDR(padapter), ETH_ALEN);
+
+		dhcp_flag_bcast(padapter, skb);
+
+		if (is_vlan_tag) {
+			skb_push(skb, 4);
+			for (i = 0; i < 6; i++)
+				*((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2));
+			*((__be16 *)(skb->data + ETH_ALEN * 2)) = __constant_htons(ETH_P_8021Q);
+			*((unsigned short *)(skb->data + ETH_ALEN * 2 + 2)) = vlan_hdr;
+		}
+	}
+
+	/*  check if SA is equal to our MAC */
+	if (memcmp(skb->data + ETH_ALEN, GET_MY_HWADDR(padapter), ETH_ALEN))
+		return -1;
+
+	return 0;
+}
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	switch (pattrib->qsel) {
+	case 0:
+	case 3:
+		addr = BE_QUEUE_INX;
+		break;
+	case 1:
+	case 2:
+		addr = BK_QUEUE_INX;
+		break;
+	case 4:
+	case 5:
+		addr = VI_QUEUE_INX;
+		break;
+	case 6:
+	case 7:
+		addr = VO_QUEUE_INX;
+		break;
+	case 0x10:
+		addr = BCN_QUEUE_INX;
+		break;
+	case 0x11:/* BC/MC in PS (HIQ) */
+		addr = HIGH_QUEUE_INX;
+		break;
+	case 0x12:
+	default:
+		addr = MGT_QUEUE_INX;
+		break;
+	}
+
+	return addr;
+}
+
+static void do_queue_select(struct adapter	*padapter, struct pkt_attrib *pattrib)
+{
+	u8 qsel;
+
+	qsel = pattrib->priority;
+
+	pattrib->qsel = qsel;
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *	1	enqueue
+ *	0	success, hardware will handle this xmit frame(packet)
+ *	<0	fail
+ */
+s32 rtw_xmit(struct adapter *padapter, struct sk_buff **ppkt)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	void *br_port = NULL;
+	s32 res;
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (!pxmitframe)
+		return -1;
+
+	rcu_read_lock();
+	br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+	rcu_read_unlock();
+
+	if (br_port && check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE)) {
+		res = rtw_br_client_tx(padapter, ppkt);
+		if (res == -1) {
+			rtw_free_xmitframe(pxmitpriv, pxmitframe);
+			return -1;
+		}
+	}
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+	if (res == _FAIL) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+		return -1;
+	}
+	pxmitframe->pkt = *ppkt;
+
+	rtw_led_control(padapter, LED_CTL_TX);
+
+	do_queue_select(padapter, &pxmitframe->attrib);
+
+	spin_lock_bh(&pxmitpriv->lock);
+	if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe)) {
+		spin_unlock_bh(&pxmitpriv->lock);
+		return 1;
+	}
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	if (!rtl8188eu_hal_xmit(padapter, pxmitframe))
+		return 1;
+
+	return 0;
+}
+
+int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	int ret = false;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		return ret;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+
+	if (!psta)
+		return ret;
+
+	if (pattrib->triggered == 1) {
+		if (bmcst)
+			pattrib->qsel = 0x11;/* HIQ */
+		return ret;
+	}
+
+	if (bmcst) {
+		spin_lock_bh(&psta->sleep_q.lock);
+
+		if (pstapriv->sta_dz_bitmap) {/* if any one sta is in ps mode */
+			list_del_init(&pxmitframe->list);
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			pstapriv->tim_bitmap |= BIT(0);/*  */
+			pstapriv->sta_dz_bitmap |= BIT(0);
+			/* tx bc/mc packets after update bcn */
+			update_beacon(padapter, _TIM_IE_, NULL, false);
+
+			ret = true;
+		}
+
+		spin_unlock_bh(&psta->sleep_q.lock);
+
+		return ret;
+	}
+
+	spin_lock_bh(&psta->sleep_q.lock);
+
+	if (psta->state & WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) {
+			list_del_init(&pxmitframe->list);
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			switch (pattrib->priority) {
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk & BIT(0);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi & BIT(0);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo & BIT(0);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be & BIT(0);
+				break;
+			}
+
+			if (wmmps_ac)
+				psta->sleepq_ac_len++;
+
+			if (((psta->has_legacy_ac) && (!wmmps_ac)) ||
+			    ((!psta->has_legacy_ac) && (wmmps_ac))) {
+				pstapriv->tim_bitmap |= BIT(psta->aid);
+
+				if (psta->sleepq_len == 1) {
+					/* update BCN for TIM IE */
+					update_beacon(padapter, _TIM_IE_, NULL, false);
+				}
+			}
+			ret = true;
+		}
+	}
+
+	spin_unlock_bh(&psta->sleep_q.lock);
+
+	return ret;
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue)
+{
+	struct list_head *plist, *phead;
+	u8	ac_index;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib;
+	struct xmit_frame	*pxmitframe;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+
+	phead = get_list_head(pframequeue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pxmitframe = container_of(plist, struct xmit_frame, list);
+
+		plist = plist->next;
+
+		xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
+
+		pattrib = &pxmitframe->attrib;
+
+		ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+		ptxservq->qcnt--;
+		phwxmits[ac_index].accnt--;
+	}
+}
+
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	struct sta_info *psta_bmc;
+	struct sta_xmit_priv *pstaxmitpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	psta->state |= WIFI_SLEEP_STATE;
+
+	pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&pstaxmitpriv->vo_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&pstaxmitpriv->vi_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&pstaxmitpriv->be_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&pstaxmitpriv->bk_q.tx_pending);
+
+	/* for BC/MC Frames */
+	pstaxmitpriv = &psta_bmc->sta_xmitpriv;
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&pstaxmitpriv->be_q.tx_pending);
+
+	spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 update_mask = 0, wmmps_ac = 0;
+	struct sta_info *psta_bmc;
+	struct list_head *xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	spin_lock_bh(&psta->sleep_q.lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = xmitframe_phead->next;
+
+	while (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = xmitframe_plist->next;
+
+		list_del_init(&pxmitframe->list);
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		psta->sleepq_len--;
+		if (psta->sleepq_len > 0)
+			pxmitframe->attrib.mdata = 1;
+		else
+			pxmitframe->attrib.mdata = 0;
+
+		if (wmmps_ac) {
+			psta->sleepq_ac_len--;
+			if (psta->sleepq_ac_len > 0) {
+				pxmitframe->attrib.mdata = 1;
+				pxmitframe->attrib.eosp = 0;
+			} else {
+				pxmitframe->attrib.mdata = 0;
+				pxmitframe->attrib.eosp = 1;
+			}
+		}
+
+		pxmitframe->attrib.triggered = 1;
+
+		spin_unlock_bh(&psta->sleep_q.lock);
+		if (rtl8188eu_hal_xmit(padapter, pxmitframe))
+			rtw_xmit_complete(padapter, pxmitframe);
+		spin_lock_bh(&psta->sleep_q.lock);
+	}
+
+	if (psta->sleepq_len == 0) {
+		pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+		update_mask = BIT(0);
+
+		if (psta->state & WIFI_SLEEP_STATE)
+			psta->state ^= WIFI_SLEEP_STATE;
+
+		if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	}
+
+	spin_unlock_bh(&psta->sleep_q.lock);
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return;
+
+	if ((pstapriv->sta_dz_bitmap & 0xfffe) == 0x0) { /* no any sta in ps mode */
+		spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = xmitframe_phead->next;
+
+		while (xmitframe_phead != xmitframe_plist) {
+			pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = xmitframe_plist->next;
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+
+			spin_unlock_bh(&psta_bmc->sleep_q.lock);
+			if (rtl8188eu_hal_xmit(padapter, pxmitframe))
+				rtw_xmit_complete(padapter, pxmitframe);
+			spin_lock_bh(&psta_bmc->sleep_q.lock);
+		}
+
+		if (psta_bmc->sleepq_len == 0) {
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			update_mask |= BIT(1);
+		}
+
+		spin_unlock_bh(&psta_bmc->sleep_q.lock);
+	}
+
+	if (update_mask)
+		update_beacon(padapter, _TIM_IE_, NULL, false);
+}
+
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 wmmps_ac = 0;
+	struct list_head *xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	spin_lock_bh(&psta->sleep_q.lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = xmitframe_phead->next;
+
+	while (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = xmitframe_plist->next;
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		if (!wmmps_ac)
+			continue;
+
+		list_del_init(&pxmitframe->list);
+
+		psta->sleepq_len--;
+		psta->sleepq_ac_len--;
+
+		if (psta->sleepq_ac_len > 0) {
+			pxmitframe->attrib.mdata = 1;
+			pxmitframe->attrib.eosp = 0;
+		} else {
+			pxmitframe->attrib.mdata = 0;
+			pxmitframe->attrib.eosp = 1;
+		}
+
+		pxmitframe->attrib.triggered = 1;
+
+		if (rtl8188eu_hal_xmit(padapter, pxmitframe))
+			rtw_xmit_complete(padapter, pxmitframe);
+
+		if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) {
+			pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+			/* update BCN for TIM IE */
+			update_beacon(padapter, _TIM_IE_, NULL, false);
+		}
+	}
+
+	spin_unlock_bh(&psta->sleep_q.lock);
+}
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
+{
+	sctx->timeout_ms = timeout_ms;
+	sctx->submit_time = jiffies;
+	init_completion(&sctx->done);
+	sctx->status = RTW_SCTX_SUBMITTED;
+}
+
+int rtw_sctx_wait(struct submit_ctx *sctx)
+{
+	int ret = _FAIL;
+	unsigned long expire;
+	int status = 0;
+
+	expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
+	if (!wait_for_completion_timeout(&sctx->done, expire))
+		/* timeout, do something?? */
+		status = RTW_SCTX_DONE_TIMEOUT;
+	else
+		status = sctx->status;
+
+	if (status == RTW_SCTX_DONE_SUCCESS)
+		ret = _SUCCESS;
+
+	return ret;
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+	if (*sctx) {
+		(*sctx)->status = status;
+		complete(&((*sctx)->done));
+		*sctx = NULL;
+	}
+}
+
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	pack_tx_ops->submit_time = jiffies;
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	return rtw_sctx_wait(pack_tx_ops);
+}
+
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	if (pxmitpriv->ack_tx)
+		rtw_sctx_done_err(&pack_tx_ops, status);
+}
+
+static void rtw_check_xmit_resource(struct adapter *padapter, struct sk_buff *pkt)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u16 queue;
+
+	queue = skb_get_queue_mapping(pkt);
+	if (padapter->registrypriv.wifi_spec) {
+		/* No free space for Tx, tx_worker is too slow */
+		if (pxmitpriv->hwxmits[queue].accnt > WMM_XMIT_THRESHOLD)
+			netif_stop_subqueue(padapter->pnetdev, queue);
+	} else {
+		if (pxmitpriv->free_xmitframe_cnt <= 4) {
+			if (!netif_tx_queue_stopped(netdev_get_tx_queue(padapter->pnetdev, queue)))
+				netif_stop_subqueue(padapter->pnetdev, queue);
+		}
+	}
+}
+
+static int rtw_mlcst2unicst(struct adapter *padapter, struct sk_buff *skb)
+{
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct list_head *phead, *plist;
+	struct sk_buff *newskb;
+	struct sta_info *psta = NULL;
+	s32 res;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = phead->next;
+
+	/* free sta asoc_queue */
+	while (phead != plist) {
+		psta = container_of(plist, struct sta_info, asoc_list);
+
+		plist = plist->next;
+
+		/* avoid   come from STA1 and send back STA1 */
+		if (!memcmp(psta->hwaddr, &skb->data[6], 6))
+			continue;
+
+		newskb = skb_copy(skb, GFP_ATOMIC);
+
+		if (newskb) {
+			memcpy(newskb->data, psta->hwaddr, 6);
+			res = rtw_xmit(padapter, &newskb);
+			if (res < 0) {
+				pxmitpriv->tx_drop++;
+				dev_kfree_skb_any(newskb);
+			} else {
+				pxmitpriv->tx_pkts++;
+			}
+		} else {
+			pxmitpriv->tx_drop++;
+
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+			return false;	/*  Caller shall tx this multicast frame via normal way. */
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+	dev_kfree_skb_any(skb);
+	return true;
+}
+
+netdev_tx_t rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	s32 res = 0;
+
+	if (!rtw_if_up(padapter))
+		goto drop_packet;
+
+	rtw_check_xmit_resource(padapter, pkt);
+
+	if (!rtw_mc2u_disable && check_fwstate(pmlmepriv, WIFI_AP_STATE) &&
+	    (IP_MCAST_MAC(pkt->data) || ICMPV6_MCAST_MAC(pkt->data)) &&
+	    (padapter->registrypriv.wifi_spec == 0)) {
+		if (pxmitpriv->free_xmitframe_cnt > (NR_XMITFRAME / 4)) {
+			res = rtw_mlcst2unicst(padapter, pkt);
+			if (res)
+				goto exit;
+		}
+	}
+
+	res = rtw_xmit(padapter, &pkt);
+	if (res < 0)
+		goto drop_packet;
+
+	pxmitpriv->tx_pkts++;
+	goto exit;
+
+drop_packet:
+	pxmitpriv->tx_drop++;
+	dev_kfree_skb_any(pkt);
+
+exit:
+	return NETDEV_TX_OK;
+}
diff --git a/drivers/staging/r8188eu/hal/Hal8188ERateAdaptive.c b/drivers/staging/r8188eu/hal/Hal8188ERateAdaptive.c
new file mode 100644
index 000000000..1e04de3a6
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/Hal8188ERateAdaptive.c
@@ -0,0 +1,654 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) Realtek Semiconductor Corp. */
+
+#include "../include/drv_types.h"
+
+static u8 RETRY_PENALTY[PERENTRY][RETRYSIZE + 1] = {
+		{5, 4, 3, 2, 0, 3},      /* 92 , idx = 0 */
+		{6, 5, 4, 3, 0, 4},      /* 86 , idx = 1 */
+		{6, 5, 4, 2, 0, 4},      /* 81 , idx = 2 */
+		{8, 7, 6, 4, 0, 6},      /* 75 , idx = 3 */
+		{10, 9, 8, 6, 0, 8},     /* 71	, idx = 4 */
+		{10, 9, 8, 4, 0, 8},     /* 66	, idx = 5 */
+		{10, 9, 8, 2, 0, 8},     /* 62	, idx = 6 */
+		{10, 9, 8, 0, 0, 8},     /* 59	, idx = 7 */
+		{18, 17, 16, 8, 0, 16},  /* 53 , idx = 8 */
+		{26, 25, 24, 16, 0, 24}, /* 50	, idx = 9 */
+		{34, 33, 32, 24, 0, 32}, /* 47	, idx = 0x0a */
+		{34, 31, 28, 20, 0, 32}, /* 43	, idx = 0x0b */
+		{34, 31, 27, 18, 0, 32}, /* 40 , idx = 0x0c */
+		{34, 31, 26, 16, 0, 32}, /* 37 , idx = 0x0d */
+		{34, 30, 22, 16, 0, 32}, /* 32 , idx = 0x0e */
+		{34, 30, 24, 16, 0, 32}, /* 26 , idx = 0x0f */
+		{49, 46, 40, 16, 0, 48}, /* 20	, idx = 0x10 */
+		{49, 45, 32, 0, 0, 48},  /* 17 , idx = 0x11 */
+		{49, 45, 22, 18, 0, 48}, /* 15	, idx = 0x12 */
+		{49, 40, 24, 16, 0, 48}, /* 12	, idx = 0x13 */
+		{49, 32, 18, 12, 0, 48}, /* 9 , idx = 0x14 */
+		{49, 22, 18, 14, 0, 48}, /* 6 , idx = 0x15 */
+		{49, 16, 16, 0, 0, 48}
+	}; /* 3, idx = 0x16 */
+
+static u8 PT_PENALTY[RETRYSIZE + 1] = {34, 31, 30, 24, 0, 32};
+
+/*  wilson modify */
+static u8 RETRY_PENALTY_IDX[2][RATESIZE] = {
+		{4, 4, 4, 5, 4, 4, 5, 7, 7, 7, 8, 0x0a,	       /*  SS>TH */
+		4, 4, 4, 4, 6, 0x0a, 0x0b, 0x0d,
+		5, 5, 7, 7, 8, 0x0b, 0x0d, 0x0f},			   /*  0329 R01 */
+		{0x0a, 0x0a, 0x0b, 0x0c, 0x0a,
+		0x0a, 0x0b, 0x0c, 0x0d, 0x10, 0x13, 0x14,	   /*  SS<TH */
+		0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x11, 0x13, 0x15,
+		9, 9, 9, 9, 0x0c, 0x0e, 0x11, 0x13}
+	};
+
+static u8 RETRY_PENALTY_UP_IDX[RATESIZE] = {
+		0x0c, 0x0d, 0x0d, 0x0f, 0x0d, 0x0e,
+		0x0f, 0x0f, 0x10, 0x12, 0x13, 0x14,	       /*  SS>TH */
+		0x0f, 0x10, 0x10, 0x12, 0x12, 0x13, 0x14, 0x15,
+		0x11, 0x11, 0x12, 0x13, 0x13, 0x13, 0x14, 0x15};
+
+static u8 RSSI_THRESHOLD[RATESIZE] = {
+		0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0x24, 0x26, 0x2a,
+		0x18, 0x1a, 0x1d, 0x1f, 0x21, 0x27, 0x29, 0x2a,
+		0, 0, 0, 0x1f, 0x23, 0x28, 0x2a, 0x2c};
+
+static u16 N_THRESHOLD_HIGH[RATESIZE] = {
+		4, 4, 8, 16,
+		24, 36, 48, 72, 96, 144, 192, 216,
+		60, 80, 100, 160, 240, 400, 560, 640,
+		300, 320, 480, 720, 1000, 1200, 1600, 2000};
+static u16 N_THRESHOLD_LOW[RATESIZE] = {
+		2, 2, 4, 8,
+		12, 18, 24, 36, 48, 72, 96, 108,
+		30, 40, 50, 80, 120, 200, 280, 320,
+		150, 160, 240, 360, 500, 600, 800, 1000};
+
+static u8 DROPING_NECESSARY[RATESIZE] = {
+		1, 1, 1, 1,
+		1, 2, 3, 4, 5, 6, 7, 8,
+		1, 2, 3, 4, 5, 6, 7, 8,
+		5, 6, 7, 8, 9, 10, 11, 12};
+
+static u8 PendingForRateUpFail[5] = {2, 10, 24, 40, 60};
+static u16 DynamicTxRPTTiming[6] = {
+	0x186a, 0x30d4, 0x493e, 0x61a8, 0x7a12, 0x927c}; /*  200ms-1200ms */
+
+/*  End Rate adaptive parameters */
+
+static void odm_SetTxRPTTiming_8188E(
+		struct odm_dm_struct *dm_odm,
+		struct odm_ra_info *pRaInfo,
+		u8 extend
+	)
+{
+	u8 idx = 0;
+
+	for (idx = 0; idx < 5; idx++)
+		if (DynamicTxRPTTiming[idx] == pRaInfo->RptTime)
+			break;
+
+	if (extend == 0) { /*  back to default timing */
+		idx = 0;  /* 200ms */
+	} else if (extend == 1) {/*  increase the timing */
+		idx += 1;
+		if (idx > 5)
+			idx = 5;
+	} else if (extend == 2) {/*  decrease the timing */
+		if (idx != 0)
+			idx -= 1;
+	}
+	pRaInfo->RptTime = DynamicTxRPTTiming[idx];
+}
+
+static int odm_RateDown_8188E(struct odm_dm_struct *dm_odm, struct odm_ra_info *pRaInfo)
+{
+	u8 RateID, LowestRate, HighestRate;
+	u8 i;
+
+	if (NULL == pRaInfo)
+		return -1;
+	RateID = pRaInfo->PreRate;
+	LowestRate = pRaInfo->LowestRate;
+	HighestRate = pRaInfo->HighestRate;
+
+	if (RateID > HighestRate) {
+		RateID = HighestRate;
+	} else if (pRaInfo->RateSGI) {
+		pRaInfo->RateSGI = 0;
+	} else if (RateID > LowestRate) {
+		if (RateID > 0) {
+			for (i = RateID - 1; i > LowestRate; i--) {
+				if (pRaInfo->RAUseRate & BIT(i)) {
+					RateID = i;
+					goto RateDownFinish;
+				}
+			}
+		}
+	} else if (RateID <= LowestRate) {
+		RateID = LowestRate;
+	}
+RateDownFinish:
+	if (pRaInfo->RAWaitingCounter == 1) {
+		pRaInfo->RAWaitingCounter += 1;
+		pRaInfo->RAPendingCounter += 1;
+	} else if (pRaInfo->RAWaitingCounter == 0) {
+		;
+	} else {
+		pRaInfo->RAWaitingCounter = 0;
+		pRaInfo->RAPendingCounter = 0;
+	}
+
+	if (pRaInfo->RAPendingCounter >= 4)
+		pRaInfo->RAPendingCounter = 4;
+
+	pRaInfo->DecisionRate = RateID;
+	odm_SetTxRPTTiming_8188E(dm_odm, pRaInfo, 2);
+	return 0;
+}
+
+static int odm_RateUp_8188E(
+		struct odm_dm_struct *dm_odm,
+		struct odm_ra_info *pRaInfo
+	)
+{
+	u8 RateID, HighestRate;
+	u8 i;
+
+	if (NULL == pRaInfo)
+		return -1;
+	RateID = pRaInfo->PreRate;
+	HighestRate = pRaInfo->HighestRate;
+	if (pRaInfo->RAWaitingCounter == 1) {
+		pRaInfo->RAWaitingCounter = 0;
+		pRaInfo->RAPendingCounter = 0;
+	} else if (pRaInfo->RAWaitingCounter > 1) {
+		pRaInfo->PreRssiStaRA = pRaInfo->RssiStaRA;
+		goto RateUpfinish;
+	}
+	odm_SetTxRPTTiming_8188E(dm_odm, pRaInfo, 0);
+
+	if (RateID < HighestRate) {
+		for (i = RateID + 1; i <= HighestRate; i++) {
+			if (pRaInfo->RAUseRate & BIT(i)) {
+				RateID = i;
+				goto RateUpfinish;
+			}
+		}
+	} else if (RateID == HighestRate) {
+		if (pRaInfo->SGIEnable && (pRaInfo->RateSGI != 1))
+			pRaInfo->RateSGI = 1;
+		else if ((pRaInfo->SGIEnable) != 1)
+			pRaInfo->RateSGI = 0;
+	} else {
+		RateID = HighestRate;
+	}
+RateUpfinish:
+	if (pRaInfo->RAWaitingCounter == (4 + PendingForRateUpFail[pRaInfo->RAPendingCounter]))
+		pRaInfo->RAWaitingCounter = 0;
+	else
+		pRaInfo->RAWaitingCounter++;
+
+	pRaInfo->DecisionRate = RateID;
+	return 0;
+}
+
+static void odm_ResetRaCounter_8188E(struct odm_ra_info *pRaInfo)
+{
+	u8 RateID;
+
+	RateID = pRaInfo->DecisionRate;
+	pRaInfo->NscUp = (N_THRESHOLD_HIGH[RateID] + N_THRESHOLD_LOW[RateID]) >> 1;
+	pRaInfo->NscDown = (N_THRESHOLD_HIGH[RateID] + N_THRESHOLD_LOW[RateID]) >> 1;
+}
+
+static void odm_RateDecision_8188E(struct odm_dm_struct *dm_odm,
+		struct odm_ra_info *pRaInfo
+	)
+{
+	u8 RateID = 0, RtyPtID = 0, PenaltyID1 = 0, PenaltyID2 = 0;
+	/* u32 pool_retry; */
+	static u8 DynamicTxRPTTimingCounter;
+
+	if (pRaInfo->Active && (pRaInfo->TOTAL > 0)) { /*  STA used and data packet exits */
+		if ((pRaInfo->RssiStaRA < (pRaInfo->PreRssiStaRA - 3)) ||
+		    (pRaInfo->RssiStaRA > (pRaInfo->PreRssiStaRA + 3))) {
+			pRaInfo->RAWaitingCounter = 0;
+			pRaInfo->RAPendingCounter = 0;
+		}
+		/*  Start RA decision */
+		if (pRaInfo->PreRate > pRaInfo->HighestRate)
+			RateID = pRaInfo->HighestRate;
+		else
+			RateID = pRaInfo->PreRate;
+		if (pRaInfo->RssiStaRA > RSSI_THRESHOLD[RateID])
+			RtyPtID = 0;
+		else
+			RtyPtID = 1;
+		PenaltyID1 = RETRY_PENALTY_IDX[RtyPtID][RateID]; /* TODO by page */
+
+		pRaInfo->NscDown += pRaInfo->RTY[0] * RETRY_PENALTY[PenaltyID1][0];
+		pRaInfo->NscDown += pRaInfo->RTY[1] * RETRY_PENALTY[PenaltyID1][1];
+		pRaInfo->NscDown += pRaInfo->RTY[2] * RETRY_PENALTY[PenaltyID1][2];
+		pRaInfo->NscDown += pRaInfo->RTY[3] * RETRY_PENALTY[PenaltyID1][3];
+		pRaInfo->NscDown += pRaInfo->RTY[4] * RETRY_PENALTY[PenaltyID1][4];
+		if (pRaInfo->NscDown > (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5]))
+			pRaInfo->NscDown -= pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5];
+		else
+			pRaInfo->NscDown = 0;
+
+		/*  rate up */
+		PenaltyID2 = RETRY_PENALTY_UP_IDX[RateID];
+		pRaInfo->NscUp += pRaInfo->RTY[0] * RETRY_PENALTY[PenaltyID2][0];
+		pRaInfo->NscUp += pRaInfo->RTY[1] * RETRY_PENALTY[PenaltyID2][1];
+		pRaInfo->NscUp += pRaInfo->RTY[2] * RETRY_PENALTY[PenaltyID2][2];
+		pRaInfo->NscUp += pRaInfo->RTY[3] * RETRY_PENALTY[PenaltyID2][3];
+		pRaInfo->NscUp += pRaInfo->RTY[4] * RETRY_PENALTY[PenaltyID2][4];
+		if (pRaInfo->NscUp > (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID2][5]))
+			pRaInfo->NscUp -= pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID2][5];
+		else
+			pRaInfo->NscUp = 0;
+
+		if ((pRaInfo->NscDown < N_THRESHOLD_LOW[RateID]) ||
+		    (pRaInfo->DROP > DROPING_NECESSARY[RateID]))
+			odm_RateDown_8188E(dm_odm, pRaInfo);
+		else if (pRaInfo->NscUp > N_THRESHOLD_HIGH[RateID])
+			odm_RateUp_8188E(dm_odm, pRaInfo);
+
+		if (pRaInfo->DecisionRate > pRaInfo->HighestRate)
+			pRaInfo->DecisionRate = pRaInfo->HighestRate;
+
+		if ((pRaInfo->DecisionRate) == (pRaInfo->PreRate))
+			DynamicTxRPTTimingCounter += 1;
+		else
+			DynamicTxRPTTimingCounter = 0;
+
+		if (DynamicTxRPTTimingCounter >= 4) {
+			odm_SetTxRPTTiming_8188E(dm_odm, pRaInfo, 1);
+			DynamicTxRPTTimingCounter = 0;
+		}
+
+		pRaInfo->PreRate = pRaInfo->DecisionRate;  /* YJ, add, 120120 */
+
+		odm_ResetRaCounter_8188E(pRaInfo);
+	}
+}
+
+static int odm_ARFBRefresh_8188E(struct odm_dm_struct *dm_odm, struct odm_ra_info *pRaInfo)
+{  /*  Wilson 2011/10/26 */
+	u32 MaskFromReg;
+	s8 i;
+	int res;
+
+	switch (pRaInfo->RateID) {
+	case RATR_INX_WIRELESS_NGB:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x0f8ff015;
+		break;
+	case RATR_INX_WIRELESS_NG:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x0f8ff010;
+		break;
+	case RATR_INX_WIRELESS_NB:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x0f8ff005;
+		break;
+	case RATR_INX_WIRELESS_N:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x0f8ff000;
+		break;
+	case RATR_INX_WIRELESS_GB:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x00000ff5;
+		break;
+	case RATR_INX_WIRELESS_G:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x00000ff0;
+		break;
+	case RATR_INX_WIRELESS_B:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & 0x0000000d;
+		break;
+	case 12:
+		res = rtw_read32(dm_odm->Adapter, REG_ARFR0, &MaskFromReg);
+		if (res)
+			return res;
+
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & MaskFromReg;
+		break;
+	case 13:
+		res = rtw_read32(dm_odm->Adapter, REG_ARFR1, &MaskFromReg);
+		if (res)
+			return res;
+
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & MaskFromReg;
+		break;
+	case 14:
+		res = rtw_read32(dm_odm->Adapter, REG_ARFR2, &MaskFromReg);
+		if (res)
+			return res;
+
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & MaskFromReg;
+		break;
+	case 15:
+		res = rtw_read32(dm_odm->Adapter, REG_ARFR3, &MaskFromReg);
+		if (res)
+			return res;
+
+		pRaInfo->RAUseRate = (pRaInfo->RateMask) & MaskFromReg;
+		break;
+	default:
+		pRaInfo->RAUseRate = (pRaInfo->RateMask);
+		break;
+	}
+	/*  Highest rate */
+	if (pRaInfo->RAUseRate) {
+		for (i = RATESIZE; i >= 0; i--) {
+			if ((pRaInfo->RAUseRate) & BIT(i)) {
+				pRaInfo->HighestRate = i;
+				break;
+			}
+		}
+	} else {
+		pRaInfo->HighestRate = 0;
+	}
+	/*  Lowest rate */
+	if (pRaInfo->RAUseRate) {
+		for (i = 0; i < RATESIZE; i++) {
+			if ((pRaInfo->RAUseRate) & BIT(i)) {
+				pRaInfo->LowestRate = i;
+				break;
+			}
+		}
+	} else {
+		pRaInfo->LowestRate = 0;
+	}
+	if (pRaInfo->HighestRate > 0x13)
+		pRaInfo->PTModeSS = 3;
+	else if (pRaInfo->HighestRate > 0x0b)
+		pRaInfo->PTModeSS = 2;
+	else if (pRaInfo->HighestRate > 0x03)
+		pRaInfo->PTModeSS = 1;
+	else
+		pRaInfo->PTModeSS = 0;
+
+	if (pRaInfo->DecisionRate > pRaInfo->HighestRate)
+		pRaInfo->DecisionRate = pRaInfo->HighestRate;
+
+	return 0;
+}
+
+static void odm_PTTryState_8188E(struct odm_ra_info *pRaInfo)
+{
+	pRaInfo->PTTryState = 0;
+	switch (pRaInfo->PTModeSS) {
+	case 3:
+		if (pRaInfo->DecisionRate >= 0x19)
+			pRaInfo->PTTryState = 1;
+		break;
+	case 2:
+		if (pRaInfo->DecisionRate >= 0x11)
+			pRaInfo->PTTryState = 1;
+		break;
+	case 1:
+		if (pRaInfo->DecisionRate >= 0x0a)
+			pRaInfo->PTTryState = 1;
+		break;
+	case 0:
+		if (pRaInfo->DecisionRate >= 0x03)
+			pRaInfo->PTTryState = 1;
+		break;
+	default:
+		pRaInfo->PTTryState = 0;
+		break;
+	}
+
+	if (pRaInfo->RssiStaRA < 48) {
+		pRaInfo->PTStage = 0;
+	} else if (pRaInfo->PTTryState == 1) {
+		if ((pRaInfo->PTStopCount >= 10) ||
+		    (pRaInfo->PTPreRssi > pRaInfo->RssiStaRA + 5) ||
+		    (pRaInfo->PTPreRssi < pRaInfo->RssiStaRA - 5) ||
+		    (pRaInfo->DecisionRate != pRaInfo->PTPreRate)) {
+			if (pRaInfo->PTStage == 0)
+				pRaInfo->PTStage = 1;
+			else if (pRaInfo->PTStage == 1)
+				pRaInfo->PTStage = 3;
+			else
+				pRaInfo->PTStage = 5;
+
+			pRaInfo->PTPreRssi = pRaInfo->RssiStaRA;
+			pRaInfo->PTStopCount = 0;
+		} else {
+			pRaInfo->RAstage = 0;
+			pRaInfo->PTStopCount++;
+		}
+	} else {
+		pRaInfo->PTStage = 0;
+		pRaInfo->RAstage = 0;
+	}
+	pRaInfo->PTPreRate = pRaInfo->DecisionRate;
+}
+
+static void odm_PTDecision_8188E(struct odm_ra_info *pRaInfo)
+{
+	u8 j;
+	u8 temp_stage;
+	u32 numsc;
+	u32 num_total;
+	u8 stage_id;
+
+	numsc  = 0;
+	num_total = pRaInfo->TOTAL * PT_PENALTY[5];
+	for (j = 0; j <= 4; j++) {
+		numsc += pRaInfo->RTY[j] * PT_PENALTY[j];
+		if (numsc > num_total)
+			break;
+	}
+
+	j = j >> 1;
+	temp_stage = (pRaInfo->PTStage + 1) >> 1;
+	if (temp_stage > j)
+		stage_id = temp_stage - j;
+	else
+		stage_id = 0;
+
+	pRaInfo->PTSmoothFactor = (pRaInfo->PTSmoothFactor >> 1) + (pRaInfo->PTSmoothFactor >> 2) + stage_id * 16 + 2;
+	if (pRaInfo->PTSmoothFactor > 192)
+		pRaInfo->PTSmoothFactor = 192;
+	stage_id = pRaInfo->PTSmoothFactor >> 6;
+	temp_stage = stage_id * 2;
+	if (temp_stage != 0)
+		temp_stage -= 1;
+	if (pRaInfo->DROP > 3)
+		temp_stage = 0;
+	pRaInfo->PTStage = temp_stage;
+}
+
+static void
+odm_RATxRPTTimerSetting(
+		struct odm_dm_struct *dm_odm,
+		u16 minRptTime
+)
+{
+	if (dm_odm->CurrminRptTime != minRptTime) {
+		rtw_rpt_timer_cfg_cmd(dm_odm->Adapter, minRptTime);
+		dm_odm->CurrminRptTime = minRptTime;
+	}
+}
+
+int ODM_RAInfo_Init(struct odm_dm_struct *dm_odm, u8 macid)
+{
+	struct odm_ra_info *pRaInfo = &dm_odm->RAInfo[macid];
+	u8 WirelessMode = 0xFF; /* invalid value */
+	u8 max_rate_idx = 0x13; /* MCS7 */
+	if (dm_odm->pWirelessMode)
+		WirelessMode = *dm_odm->pWirelessMode;
+
+	if (WirelessMode != 0xFF) {
+		if (WirelessMode & ODM_WM_N24G)
+			max_rate_idx = 0x13;
+		else if (WirelessMode & ODM_WM_G)
+			max_rate_idx = 0x0b;
+		else if (WirelessMode & ODM_WM_B)
+			max_rate_idx = 0x03;
+	}
+
+	pRaInfo->DecisionRate = max_rate_idx;
+	pRaInfo->PreRate = max_rate_idx;
+	pRaInfo->HighestRate = max_rate_idx;
+	pRaInfo->LowestRate = 0;
+	pRaInfo->RateID = 0;
+	pRaInfo->RateMask = 0xffffffff;
+	pRaInfo->RssiStaRA = 0;
+	pRaInfo->PreRssiStaRA = 0;
+	pRaInfo->SGIEnable = 0;
+	pRaInfo->RAUseRate = 0xffffffff;
+	pRaInfo->NscDown = (N_THRESHOLD_HIGH[0x13] + N_THRESHOLD_LOW[0x13]) / 2;
+	pRaInfo->NscUp = (N_THRESHOLD_HIGH[0x13] + N_THRESHOLD_LOW[0x13]) / 2;
+	pRaInfo->RateSGI = 0;
+	pRaInfo->Active = 1;	/* Active is not used at present. by page, 110819 */
+	pRaInfo->RptTime = 0x927c;
+	pRaInfo->DROP = 0;
+	pRaInfo->RTY[0] = 0;
+	pRaInfo->RTY[1] = 0;
+	pRaInfo->RTY[2] = 0;
+	pRaInfo->RTY[3] = 0;
+	pRaInfo->RTY[4] = 0;
+	pRaInfo->TOTAL = 0;
+	pRaInfo->RAWaitingCounter = 0;
+	pRaInfo->RAPendingCounter = 0;
+	pRaInfo->PTActive = 1;   /*  Active when this STA is use */
+	pRaInfo->PTTryState = 0;
+	pRaInfo->PTStage = 5; /*  Need to fill into HW_PWR_STATUS */
+	pRaInfo->PTSmoothFactor = 192;
+	pRaInfo->PTStopCount = 0;
+	pRaInfo->PTPreRate = 0;
+	pRaInfo->PTPreRssi = 0;
+	pRaInfo->PTModeSS = 0;
+	pRaInfo->RAstage = 0;
+	return 0;
+}
+
+int ODM_RAInfo_Init_all(struct odm_dm_struct *dm_odm)
+{
+	u8 macid = 0;
+
+	dm_odm->CurrminRptTime = 0;
+
+	for (macid = 0; macid < ODM_ASSOCIATE_ENTRY_NUM; macid++)
+		ODM_RAInfo_Init(dm_odm, macid);
+
+	return 0;
+}
+
+u8 ODM_RA_GetShortGI_8188E(struct odm_dm_struct *dm_odm, u8 macid)
+{
+	if ((NULL == dm_odm) || (macid >= ODM_ASSOCIATE_ENTRY_NUM))
+		return 0;
+	return dm_odm->RAInfo[macid].RateSGI;
+}
+
+u8 ODM_RA_GetDecisionRate_8188E(struct odm_dm_struct *dm_odm, u8 macid)
+{
+	u8 DecisionRate = 0;
+
+	if ((NULL == dm_odm) || (macid >= ODM_ASSOCIATE_ENTRY_NUM))
+		return 0;
+	DecisionRate = (dm_odm->RAInfo[macid].DecisionRate);
+	return DecisionRate;
+}
+
+u8 ODM_RA_GetHwPwrStatus_8188E(struct odm_dm_struct *dm_odm, u8 macid)
+{
+	u8 PTStage = 5;
+
+	if ((NULL == dm_odm) || (macid >= ODM_ASSOCIATE_ENTRY_NUM))
+		return 0;
+	PTStage = (dm_odm->RAInfo[macid].PTStage);
+	return PTStage;
+}
+
+void ODM_RA_UpdateRateInfo_8188E(struct odm_dm_struct *dm_odm, u8 macid, u8 RateID, u32 RateMask, u8 SGIEnable)
+{
+	struct odm_ra_info *pRaInfo = NULL;
+
+	if ((NULL == dm_odm) || (macid >= ODM_ASSOCIATE_ENTRY_NUM))
+		return;
+
+	pRaInfo = &dm_odm->RAInfo[macid];
+	pRaInfo->RateID = RateID;
+	pRaInfo->RateMask = RateMask;
+	pRaInfo->SGIEnable = SGIEnable;
+	odm_ARFBRefresh_8188E(dm_odm, pRaInfo);
+}
+
+void ODM_RA_SetRSSI_8188E(struct odm_dm_struct *dm_odm, u8 macid, u8 Rssi)
+{
+	struct odm_ra_info *pRaInfo = NULL;
+
+	if ((NULL == dm_odm) || (macid >= ODM_ASSOCIATE_ENTRY_NUM))
+		return;
+
+	pRaInfo = &dm_odm->RAInfo[macid];
+	pRaInfo->RssiStaRA = Rssi;
+}
+
+void ODM_RA_Set_TxRPT_Time(struct odm_dm_struct *dm_odm, u16 minRptTime)
+{
+	rtw_write16(dm_odm->Adapter, REG_TX_RPT_TIME, minRptTime);
+}
+
+void ODM_RA_TxRPT2Handle_8188E(struct odm_dm_struct *dm_odm, u8 *TxRPT_Buf, u16 TxRPT_Len, u32 macid_entry0, u32 macid_entry1)
+{
+	struct odm_ra_info *pRAInfo = NULL;
+	u8 MacId = 0;
+	u8 *pBuffer = NULL;
+	u32 valid = 0, ItemNum = 0;
+	u16 minRptTime = 0x927c;
+
+	ItemNum = TxRPT_Len >> 3;
+	pBuffer = TxRPT_Buf;
+
+	do {
+		if (MacId >= ODM_ASSOCIATE_ENTRY_NUM)
+			valid = 0;
+		else if (MacId >= 32)
+			valid = (1 << (MacId - 32)) & macid_entry1;
+		else
+			valid = (1 << MacId) & macid_entry0;
+
+		pRAInfo = &dm_odm->RAInfo[MacId];
+		if (valid) {
+			pRAInfo->RTY[0] = le16_to_cpup((__le16 *)pBuffer);
+			pRAInfo->RTY[1] = pBuffer[2];
+			pRAInfo->RTY[2] = pBuffer[3];
+			pRAInfo->RTY[3] = pBuffer[4];
+			pRAInfo->RTY[4] = pBuffer[5];
+			pRAInfo->DROP = pBuffer[6];
+			pRAInfo->TOTAL = pRAInfo->RTY[0] + pRAInfo->RTY[1] +
+					 pRAInfo->RTY[2] + pRAInfo->RTY[3] +
+					 pRAInfo->RTY[4] + pRAInfo->DROP;
+			if (pRAInfo->TOTAL != 0) {
+				if (pRAInfo->PTActive) {
+					if (pRAInfo->RAstage < 5)
+						odm_RateDecision_8188E(dm_odm, pRAInfo);
+					else if (pRAInfo->RAstage == 5) /*  Power training try state */
+						odm_PTTryState_8188E(pRAInfo);
+					else /*  RAstage == 6 */
+						odm_PTDecision_8188E(pRAInfo);
+
+					/*  Stage_RA counter */
+					if (pRAInfo->RAstage <= 5)
+						pRAInfo->RAstage++;
+					else
+						pRAInfo->RAstage = 0;
+				} else {
+					odm_RateDecision_8188E(dm_odm, pRAInfo);
+				}
+			}
+		}
+
+		if (minRptTime > pRAInfo->RptTime)
+			minRptTime = pRAInfo->RptTime;
+
+		pBuffer += TX_RPT2_ITEM_SIZE;
+		MacId++;
+	} while (MacId < ItemNum);
+
+	odm_RATxRPTTimerSetting(dm_odm, minRptTime);
+}
diff --git a/drivers/staging/r8188eu/hal/HalHWImg8188E_BB.c b/drivers/staging/r8188eu/hal/HalHWImg8188E_BB.c
new file mode 100644
index 000000000..23b720572
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/HalHWImg8188E_BB.c
@@ -0,0 +1,733 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/rtw_iol.h"
+
+#define read_next_pair(array, v1, v2, i)		\
+	 do {						\
+		 i += 2;				\
+		 v1 = array[i];				\
+		 v2 = array[i + 1];			\
+	 } while (0)
+
+static bool CheckCondition(const u32  condition, const u32  hex)
+{
+	u32 _interface = (hex & 0x0000FF00) >> 8;
+	u32 _platform  = (hex & 0x00FF0000) >> 16;
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0x0000FF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0x00FF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+/******************************************************************************
+*                           AGC_TAB_1T.TXT
+******************************************************************************/
+
+static u32 array_agc_tab_1t_8188e[] = {
+		0xC78, 0xFB000001,
+		0xC78, 0xFB010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFB030001,
+		0xC78, 0xFB040001,
+		0xC78, 0xFB050001,
+		0xC78, 0xFA060001,
+		0xC78, 0xF9070001,
+		0xC78, 0xF8080001,
+		0xC78, 0xF7090001,
+		0xC78, 0xF60A0001,
+		0xC78, 0xF50B0001,
+		0xC78, 0xF40C0001,
+		0xC78, 0xF30D0001,
+		0xC78, 0xF20E0001,
+		0xC78, 0xF10F0001,
+		0xC78, 0xF0100001,
+		0xC78, 0xEF110001,
+		0xC78, 0xEE120001,
+		0xC78, 0xED130001,
+		0xC78, 0xEC140001,
+		0xC78, 0xEB150001,
+		0xC78, 0xEA160001,
+		0xC78, 0xE9170001,
+		0xC78, 0xE8180001,
+		0xC78, 0xE7190001,
+		0xC78, 0xE61A0001,
+		0xC78, 0xE51B0001,
+		0xC78, 0xE41C0001,
+		0xC78, 0xE31D0001,
+		0xC78, 0xE21E0001,
+		0xC78, 0xE11F0001,
+		0xC78, 0x8A200001,
+		0xC78, 0x89210001,
+		0xC78, 0x88220001,
+		0xC78, 0x87230001,
+		0xC78, 0x86240001,
+		0xC78, 0x85250001,
+		0xC78, 0x84260001,
+		0xC78, 0x83270001,
+		0xC78, 0x82280001,
+		0xC78, 0x6B290001,
+		0xC78, 0x6A2A0001,
+		0xC78, 0x692B0001,
+		0xC78, 0x682C0001,
+		0xC78, 0x672D0001,
+		0xC78, 0x662E0001,
+		0xC78, 0x652F0001,
+		0xC78, 0x64300001,
+		0xC78, 0x63310001,
+		0xC78, 0x62320001,
+		0xC78, 0x61330001,
+		0xC78, 0x46340001,
+		0xC78, 0x45350001,
+		0xC78, 0x44360001,
+		0xC78, 0x43370001,
+		0xC78, 0x42380001,
+		0xC78, 0x41390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+		0xC78, 0xFB400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFB420001,
+		0xC78, 0xFB430001,
+		0xC78, 0xFB440001,
+		0xC78, 0xFB450001,
+		0xC78, 0xFB460001,
+		0xC78, 0xFB470001,
+		0xC78, 0xFB480001,
+		0xC78, 0xFA490001,
+		0xC78, 0xF94A0001,
+		0xC78, 0xF84B0001,
+		0xC78, 0xF74C0001,
+		0xC78, 0xF64D0001,
+		0xC78, 0xF54E0001,
+		0xC78, 0xF44F0001,
+		0xC78, 0xF3500001,
+		0xC78, 0xF2510001,
+		0xC78, 0xF1520001,
+		0xC78, 0xF0530001,
+		0xC78, 0xEF540001,
+		0xC78, 0xEE550001,
+		0xC78, 0xED560001,
+		0xC78, 0xEC570001,
+		0xC78, 0xEB580001,
+		0xC78, 0xEA590001,
+		0xC78, 0xE95A0001,
+		0xC78, 0xE85B0001,
+		0xC78, 0xE75C0001,
+		0xC78, 0xE65D0001,
+		0xC78, 0xE55E0001,
+		0xC78, 0xE45F0001,
+		0xC78, 0xE3600001,
+		0xC78, 0xE2610001,
+		0xC78, 0xC3620001,
+		0xC78, 0xC2630001,
+		0xC78, 0xC1640001,
+		0xC78, 0x8B650001,
+		0xC78, 0x8A660001,
+		0xC78, 0x89670001,
+		0xC78, 0x88680001,
+		0xC78, 0x87690001,
+		0xC78, 0x866A0001,
+		0xC78, 0x856B0001,
+		0xC78, 0x846C0001,
+		0xC78, 0x676D0001,
+		0xC78, 0x666E0001,
+		0xC78, 0x656F0001,
+		0xC78, 0x64700001,
+		0xC78, 0x63710001,
+		0xC78, 0x62720001,
+		0xC78, 0x61730001,
+		0xC78, 0x60740001,
+		0xC78, 0x46750001,
+		0xC78, 0x45760001,
+		0xC78, 0x44770001,
+		0xC78, 0x43780001,
+		0xC78, 0x42790001,
+		0xC78, 0x417A0001,
+		0xC78, 0x407B0001,
+		0xC78, 0x407C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+};
+
+static void odm_ConfigBB_AGC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Bitmask, u32 Data)
+{
+	rtl8188e_PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+	/*  Add 1us delay between BB/RF register setting. */
+	udelay(1);
+}
+
+int ODM_ReadAndConfig_AGC_TAB_1T_8188E(struct odm_dm_struct *dm_odm)
+{
+	u32     hex         = 0;
+	u32     i           = 0;
+	u32     arraylen    = ARRAY_SIZE(array_agc_tab_1t_8188e);
+	u32    *array       = array_agc_tab_1t_8188e;
+	bool		biol = false;
+	struct adapter *adapter =  dm_odm->Adapter;
+	struct xmit_frame *pxmit_frame = NULL;
+	u8 bndy_cnt = 1;
+
+	hex += ODM_ITRF_USB << 8;
+	hex += ODM_CE << 16;
+	hex += 0xFF000000;
+	biol = rtw_IOL_applied(adapter);
+
+	if (biol) {
+		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
+		if (!pxmit_frame) {
+			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < arraylen; i += 2) {
+		u32 v1 = array[i];
+		u32 v2 = array[i + 1];
+
+		/*  This (offset, data) pair meets the condition. */
+		if (v1 < 0xCDCDCDCD) {
+			if (biol) {
+				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+					bndy_cnt++;
+				rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
+			} else {
+				odm_ConfigBB_AGC_8188E(dm_odm, v1, bMaskDWord, v2);
+			}
+			continue;
+		} else {
+			/*  This line is the start line of branch. */
+			if (!CheckCondition(array[i], hex)) {
+				/*  Discard the following (offset, data) pairs. */
+				read_next_pair(array, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					read_next_pair(array, v1, v2, i);
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				read_next_pair(array, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2) {
+					if (biol) {
+						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+							bndy_cnt++;
+						rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
+					} else {
+						odm_ConfigBB_AGC_8188E(dm_odm, v1, bMaskDWord, v2);
+					}
+					read_next_pair(array, v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylen - 2)
+					read_next_pair(array, v1, v2, i);
+			}
+		}
+	}
+	if (biol) {
+		if (!rtl8188e_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
+			printk("~~~ %s IOL_exec_cmds Failed !!!\n", __func__);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+/******************************************************************************
+*                           PHY_REG_1T.TXT
+******************************************************************************/
+
+static u32 array_phy_reg_1t_8188e[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02200385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00390204,
+		0x828, 0x00000000,
+		0x82C, 0x00000000,
+		0x830, 0x00000000,
+		0x834, 0x00000000,
+		0x838, 0x00000000,
+		0x83C, 0x00000000,
+		0x840, 0x00010000,
+		0x844, 0x00000000,
+		0x848, 0x00000000,
+		0x84C, 0x00000000,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x569A11A9,
+		0x85C, 0x01000014,
+		0x860, 0x66F60110,
+		0x864, 0x061F0649,
+		0x868, 0x00000000,
+		0x86C, 0x27272700,
+		0x870, 0x07000760,
+		0x874, 0x25004000,
+		0x878, 0x00000808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCCC000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x89C, 0x00706050,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121111,
+		0x910, 0x00000002,
+		0x914, 0x00000201,
+		0xA00, 0x00D047C8,
+		0xA04, 0x80FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F120F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x218075B1,
+		0xB2C, 0x80000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05611,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC47,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+		0xC50, 0x69553420,
+		0xC54, 0x43BC0094,
+		0xC58, 0x00013169,
+		0xC5C, 0x00250492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x2C7F000D,
+		0xC74, 0x020610DB,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+		0xC80, 0x390000E4,
+		0xC84, 0x20F60000,
+		0xC88, 0x40000100,
+		0xC8C, 0x20200000,
+		0xC90, 0x00091521,
+		0xC94, 0x00000000,
+		0xC98, 0x00121820,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00000000,
+		0xCE8, 0x37644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00000740,
+		0xD04, 0x00020401,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC43,
+		0xD18, 0x7A8F5B6F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xE00, 0x2D2D2D2D,
+		0xE04, 0x2D2D2D2D,
+		0xE08, 0x0390272D,
+		0xE10, 0x2D2D2D2D,
+		0xE14, 0x2D2D2D2D,
+		0xE18, 0x2D2D2D2D,
+		0xE1C, 0x2D2D2D2D,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x001B25A4,
+		0xE6C, 0x00C00014,
+		0xE70, 0x00C00014,
+		0xE74, 0x01000014,
+		0xE78, 0x01000014,
+		0xE7C, 0x01000014,
+		0xE80, 0x01000014,
+		0xE84, 0x00C00014,
+		0xE88, 0x01000014,
+		0xE8C, 0x00C00014,
+		0xED0, 0x00C00014,
+		0xED4, 0x00C00014,
+		0xED8, 0x00C00014,
+		0xEDC, 0x00000014,
+		0xEE0, 0x00000014,
+		0xEEC, 0x01C00014,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+};
+
+static void odm_ConfigBB_PHY_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Bitmask, u32 Data)
+{
+	if (Addr == 0xfe) {
+		msleep(50);
+	} else if (Addr == 0xfd) {
+		mdelay(5);
+	} else if (Addr == 0xfc) {
+		mdelay(1);
+	} else if (Addr == 0xfb) {
+		udelay(50);
+	} else if (Addr == 0xfa) {
+		udelay(5);
+	} else if (Addr == 0xf9) {
+		udelay(1);
+	} else {
+		if (Addr == 0xa24)
+			pDM_Odm->RFCalibrateInfo.RegA24 = Data;
+		rtl8188e_PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+
+		/*  Add 1us delay between BB/RF register setting. */
+		udelay(1);
+	}
+}
+
+int ODM_ReadAndConfig_PHY_REG_1T_8188E(struct odm_dm_struct *dm_odm)
+{
+	u32     hex         = 0;
+	u32     i           = 0;
+	u32     arraylen    = ARRAY_SIZE(array_phy_reg_1t_8188e);
+	u32    *array       = array_phy_reg_1t_8188e;
+	bool	biol = false;
+	struct adapter *adapter =  dm_odm->Adapter;
+	struct xmit_frame *pxmit_frame = NULL;
+	u8 bndy_cnt = 1;
+	hex += ODM_ITRF_USB << 8;
+	hex += ODM_CE << 16;
+	hex += 0xFF000000;
+	biol = rtw_IOL_applied(adapter);
+
+	if (biol) {
+		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
+		if (!pxmit_frame) {
+			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < arraylen; i += 2) {
+		u32 v1 = array[i];
+		u32 v2 = array[i + 1];
+
+		/*  This (offset, data) pair meets the condition. */
+		if (v1 < 0xCDCDCDCD) {
+			if (biol) {
+				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+					bndy_cnt++;
+				if (v1 == 0xfe) {
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
+				} else if (v1 == 0xfd) {
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
+				} else if (v1 == 0xfc) {
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
+				} else if (v1 == 0xfb) {
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
+				} else if (v1 == 0xfa) {
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
+				} else if (v1 == 0xf9) {
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
+				} else {
+					if (v1 == 0xa24)
+						dm_odm->RFCalibrateInfo.RegA24 = v2;
+					rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
+				}
+			} else {
+				odm_ConfigBB_PHY_8188E(dm_odm, v1, bMaskDWord, v2);
+			}
+			continue;
+		} else { /*  This line is the start line of branch. */
+			if (!CheckCondition(array[i], hex)) {
+				/*  Discard the following (offset, data) pairs. */
+				read_next_pair(array, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					read_next_pair(array, v1, v2, i);
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				read_next_pair(array, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2) {
+					if (biol) {
+						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+							bndy_cnt++;
+						if (v1 == 0xfe) {
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
+						} else if (v1 == 0xfd) {
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
+						} else if (v1 == 0xfc) {
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
+						} else if (v1 == 0xfb) {
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
+						} else if (v1 == 0xfa) {
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
+						} else if (v1 == 0xf9) {
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
+						} else {
+							if (v1 == 0xa24)
+								dm_odm->RFCalibrateInfo.RegA24 = v2;
+
+							rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
+						}
+					} else {
+						odm_ConfigBB_PHY_8188E(dm_odm, v1, bMaskDWord, v2);
+					}
+					read_next_pair(array, v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylen - 2)
+					read_next_pair(array, v1, v2, i);
+			}
+		}
+	}
+	if (biol) {
+		if (!rtl8188e_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
+			pr_info("~~~ IOL Config %s Failed !!!\n", __func__);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+static u32 array_phy_reg_pg_8188e[] = {
+		0xE00, 0xFFFFFFFF, 0x06070809,
+		0xE04, 0xFFFFFFFF, 0x02020405,
+		0xE08, 0x0000FF00, 0x00000006,
+		0x86C, 0xFFFFFF00, 0x00020400,
+		0xE10, 0xFFFFFFFF, 0x08090A0B,
+		0xE14, 0xFFFFFFFF, 0x01030607,
+		0xE18, 0xFFFFFFFF, 0x08090A0B,
+		0xE1C, 0xFFFFFFFF, 0x01030607,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+
+};
+
+static void odm_ConfigBB_PHY_REG_PG_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Bitmask,
+					  u32 Data)
+{
+	if (Addr == 0xfe)
+		msleep(50);
+	else if (Addr == 0xfd)
+		mdelay(5);
+	else if (Addr == 0xfc)
+		mdelay(1);
+	else if (Addr == 0xfb)
+		udelay(50);
+	else if (Addr == 0xfa)
+		udelay(5);
+	else if (Addr == 0xf9)
+		udelay(1);
+	else
+		storePwrIndexDiffRateOffset(pDM_Odm->Adapter, Addr, Bitmask, Data);
+}
+
+void ODM_ReadAndConfig_PHY_REG_PG_8188E(struct odm_dm_struct *dm_odm)
+{
+	u32  hex;
+	u32  i           = 0;
+	u32  arraylen    = ARRAY_SIZE(array_phy_reg_pg_8188e);
+	u32 *array       = array_phy_reg_pg_8188e;
+
+	hex = ODM_ITRF_USB << 8;
+	hex += (ODM_CE << 16) + 0xFF000000;
+
+	for (i = 0; i < arraylen; i += 3) {
+		u32 v1 = array[i];
+		u32 v2 = array[i + 1];
+		u32 v3 = array[i + 2];
+
+		/*  this line is a line of pure_body */
+		if (v1 < 0xCDCDCDCD) {
+			odm_ConfigBB_PHY_REG_PG_8188E(dm_odm, v1, v2, v3);
+			continue;
+		} else { /*  this line is the start of branch */
+			if (!CheckCondition(array[i], hex)) {
+				/*  don't need the hw_body */
+				i += 2; /*  skip the pair of expression */
+				v1 = array[i];
+				v2 = array[i + 1];
+				v3 = array[i + 2];
+				while (v2 != 0xDEAD) {
+					i += 3;
+					v1 = array[i];
+					v2 = array[i + 1];
+					v3 = array[i + 1];
+				}
+			}
+		}
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/HalHWImg8188E_MAC.c b/drivers/staging/r8188eu/hal/HalHWImg8188E_MAC.c
new file mode 100644
index 000000000..da71867bc
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/HalHWImg8188E_MAC.c
@@ -0,0 +1,212 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/rtw_iol.h"
+
+static bool Checkcondition(const u32  condition, const u32  hex)
+{
+	u32 _board     = (hex & 0x000000FF);
+	u32 _interface = (hex & 0x0000FF00) >> 8;
+	u32 _platform  = (hex & 0x00FF0000) >> 16;
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0x000000FF;
+	if ((_board == cond) && cond != 0x00)
+		return false;
+
+	cond = condition & 0x0000FF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0x00FF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+
+static u32 array_MAC_REG_8188E[] = {
+		0x026, 0x00000041,
+		0x027, 0x00000035,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000001,
+		0x432, 0x00000002,
+		0x433, 0x00000004,
+		0x434, 0x00000005,
+		0x435, 0x00000006,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x438, 0x00000000,
+		0x439, 0x00000000,
+		0x43A, 0x00000001,
+		0x43B, 0x00000002,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000006,
+		0x43F, 0x00000007,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000015,
+		0x445, 0x000000F0,
+		0x446, 0x0000000F,
+		0x447, 0x00000000,
+		0x458, 0x00000041,
+		0x459, 0x000000A8,
+		0x45A, 0x00000072,
+		0x45B, 0x000000B9,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x480, 0x00000008,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x4D3, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x652, 0x00000020,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+};
+
+static void odm_ConfigMAC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u8 Data)
+{
+	rtw_write8(pDM_Odm->Adapter, Addr, Data);
+}
+
+int ODM_ReadAndConfig_MAC_REG_8188E(struct odm_dm_struct *dm_odm)
+{
+	#define READ_NEXT_PAIR(v1, v2, i) do { i += 2; v1 = array[i]; v2 = array[i + 1]; } while (0)
+
+	u32     hex         = 0;
+	u32     i;
+	u32     array_len    = ARRAY_SIZE(array_MAC_REG_8188E);
+	u32    *array       = array_MAC_REG_8188E;
+	bool	biol = false;
+
+	struct adapter *adapt =  dm_odm->Adapter;
+	struct xmit_frame	*pxmit_frame = NULL;
+	u8 bndy_cnt = 1;
+	hex += ODM_ITRF_USB << 8;
+	hex += ODM_CE << 16;
+	hex += 0xFF000000;
+
+	biol = rtw_IOL_applied(adapt);
+
+	if (biol) {
+		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapt);
+		if (!pxmit_frame) {
+			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < array_len; i += 2) {
+		u32 v1 = array[i];
+		u32 v2 = array[i + 1];
+
+		/*  This (offset, data) pair meets the condition. */
+		if (v1 < 0xCDCDCDCD) {
+				if (biol) {
+					if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+						bndy_cnt++;
+					rtw_IOL_append_WB_cmd(pxmit_frame, (u16)v1, (u8)v2, 0xFF);
+				} else {
+					odm_ConfigMAC_8188E(dm_odm, v1, (u8)v2);
+				}
+				continue;
+		} else { /*  This line is the start line of branch. */
+			if (!Checkcondition(array[i], hex)) {
+				/*  Discard the following (offset, data) pairs. */
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < array_len - 2) {
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < array_len - 2) {
+					if (biol) {
+						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+							bndy_cnt++;
+						rtw_IOL_append_WB_cmd(pxmit_frame, (u16)v1, (u8)v2, 0xFF);
+					} else {
+						odm_ConfigMAC_8188E(dm_odm, v1, (u8)v2);
+					}
+
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+				while (v2 != 0xDEAD && i < array_len - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+			}
+		}
+	}
+	if (biol) {
+		if (!rtl8188e_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
+			pr_info("~~~ MAC IOL_exec_cmds Failed !!!\n");
+			return -1;
+		}
+	}
+	return 0;
+}
diff --git a/drivers/staging/r8188eu/hal/HalHWImg8188E_RF.c b/drivers/staging/r8188eu/hal/HalHWImg8188E_RF.c
new file mode 100644
index 000000000..a4c3d3d14
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/HalHWImg8188E_RF.c
@@ -0,0 +1,269 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/rtw_iol.h"
+
+static bool CheckCondition(const u32  Condition, const u32  Hex)
+{
+	u32 _interface = (Hex & 0x0000FF00) >> 8;
+	u32 _platform  = (Hex & 0x00FF0000) >> 16;
+	u32 cond = Condition;
+
+	if (Condition == 0xCDCDCDCD)
+		return true;
+
+	cond = Condition & 0x0000FF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = Condition & 0x00FF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+/******************************************************************************
+*                           RadioA_1T.TXT
+******************************************************************************/
+
+static u32 Array_RadioA_1T_8188E[] = {
+		0x000, 0x00030000,
+		0x008, 0x00084000,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x083, 0x00000000,
+		0x0B0, 0x000FF8FC,
+		0x0B1, 0x00054400,
+		0x0B2, 0x000CCC19,
+		0x0B4, 0x00043003,
+		0x0B6, 0x0004953E,
+		0x0B7, 0x0001C718,
+		0x0B8, 0x000060FF,
+		0x0B9, 0x00080001,
+		0x0BA, 0x00040000,
+		0x0BB, 0x00000400,
+		0x0BF, 0x000C0000,
+		0x0C2, 0x00002400,
+		0x0C3, 0x00000009,
+		0x0C4, 0x00040C91,
+		0x0C5, 0x00099999,
+		0x0C6, 0x000000A3,
+		0x0C7, 0x00088820,
+		0x0C8, 0x00076C06,
+		0x0C9, 0x00000000,
+		0x0CA, 0x00080000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x0006B27D,
+		0xFF0F041F, 0xABCD,
+		0x052, 0x0007E4DD,
+		0xCDCDCDCD, 0xCDCD,
+		0x052, 0x0007E49D,
+		0xFF0F041F, 0xDEAD,
+		0x053, 0x00000073,
+		0x056, 0x00051FF3,
+		0x035, 0x00000086,
+		0x035, 0x00000186,
+		0x035, 0x00000286,
+		0x036, 0x00001C25,
+		0x036, 0x00009C25,
+		0x036, 0x00011C25,
+		0x036, 0x00019C25,
+		0x0B6, 0x00048538,
+		0x018, 0x00000C07,
+		0x05A, 0x0004BD00,
+		0x019, 0x000739D0,
+		0x034, 0x0000ADF3,
+		0x034, 0x00009DF0,
+		0x034, 0x00008DED,
+		0x034, 0x00007DEA,
+		0x034, 0x00006DE7,
+		0x034, 0x000054EE,
+		0x034, 0x000044EB,
+		0x034, 0x000034E8,
+		0x034, 0x0000246B,
+		0x034, 0x00001468,
+		0x034, 0x0000006D,
+		0x000, 0x00030159,
+		0x084, 0x00068200,
+		0x086, 0x000000CE,
+		0x087, 0x00048A00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+		0x03B, 0x000F02B0,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+		0x03B, 0x000722B0,
+		0x03B, 0x0006F7B0,
+		0x03B, 0x00054FB0,
+		0x03B, 0x0004F060,
+		0x03B, 0x00030090,
+		0x03B, 0x00020080,
+		0x03B, 0x00010080,
+		0x03B, 0x0000F780,
+		0x0EF, 0x000000A0,
+		0x000, 0x00010159,
+		0x018, 0x0000F407,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01F, 0x00080003,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E60,
+};
+
+static void odm_ConfigRFReg_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
+				  u32 Data, u32 RegAddr)
+{
+	if (Addr == 0xffe) {
+		msleep(50);
+	} else if (Addr == 0xfd) {
+		mdelay(5);
+	} else if (Addr == 0xfc) {
+		mdelay(1);
+	} else if (Addr == 0xfb) {
+		udelay(50);
+	} else if (Addr == 0xfa) {
+		udelay(5);
+	} else if (Addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl8188e_PHY_SetRFReg(pDM_Odm->Adapter, RegAddr, bRFRegOffsetMask, Data);
+		/*  Add 1us delay between BB/RF register setting. */
+		udelay(1);
+	}
+}
+
+static void odm_ConfigRF_RadioA_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Data)
+{
+	u32  content = 0x1000; /*  RF_Content: radioa_txt */
+	u32 maskforPhySet = (u32)(content & 0xE000);
+
+	odm_ConfigRFReg_8188E(pDM_Odm, Addr, Data, Addr | maskforPhySet);
+}
+
+int ODM_ReadAndConfig_RadioA_1T_8188E(struct odm_dm_struct *pDM_Odm)
+{
+	#define READ_NEXT_PAIR(v1, v2, i) do	\
+		 { i += 2; v1 = Array[i];	\
+		 v2 = Array[i + 1]; } while (0)
+
+	u32     hex         = 0;
+	u32     i           = 0;
+	u32     ArrayLen    = ARRAY_SIZE(Array_RadioA_1T_8188E);
+	u32    *Array       = Array_RadioA_1T_8188E;
+	bool		biol = false;
+	struct adapter *Adapter =  pDM_Odm->Adapter;
+	struct xmit_frame *pxmit_frame = NULL;
+	u8 bndy_cnt = 1;
+
+	hex += ODM_ITRF_USB << 8;
+	hex += ODM_CE << 16;
+	hex += 0xFF000000;
+	biol = rtw_IOL_applied(Adapter);
+
+	if (biol) {
+		pxmit_frame = rtw_IOL_accquire_xmit_frame(Adapter);
+		if (!pxmit_frame) {
+			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i + 1];
+
+		/*  This (offset, data) pair meets the condition. */
+		if (v1 < 0xCDCDCDCD) {
+			if (biol) {
+				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+					bndy_cnt++;
+
+				if (v1 == 0xffe)
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
+				else if (v1 == 0xfd)
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
+				else if (v1 == 0xfc)
+					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
+				else if (v1 == 0xfb)
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
+				else if (v1 == 0xfa)
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
+				else if (v1 == 0xf9)
+					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
+				else
+					rtw_IOL_append_WRF_cmd(pxmit_frame, RF_PATH_A, (u16)v1, v2, bRFRegOffsetMask);
+			} else {
+				odm_ConfigRF_RadioA_8188E(pDM_Odm, v1, v2);
+			}
+			continue;
+		} else { /*  This line is the start line of branch. */
+			if (!CheckCondition(Array[i], hex)) {
+				/*  Discard the following (offset, data) pairs. */
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < ArrayLen - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+			READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < ArrayLen - 2) {
+					if (biol) {
+						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
+							bndy_cnt++;
+
+						if (v1 == 0xffe)
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
+						else if (v1 == 0xfd)
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
+						else if (v1 == 0xfc)
+							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
+						else if (v1 == 0xfb)
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
+						else if (v1 == 0xfa)
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
+						else if (v1 == 0xf9)
+							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
+						else
+							rtw_IOL_append_WRF_cmd(pxmit_frame, RF_PATH_A, (u16)v1, v2, bRFRegOffsetMask);
+					} else {
+						odm_ConfigRF_RadioA_8188E(pDM_Odm, v1, v2);
+					}
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < ArrayLen - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+			}
+		}
+	}
+	if (biol) {
+		if (!rtl8188e_IOL_exec_cmds_sync(pDM_Odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
+			pr_info("~~~ IOL Config %s Failed !!!\n", __func__);
+			return -1;
+		}
+	}
+	return 0;
+}
diff --git a/drivers/staging/r8188eu/hal/HalPhyRf_8188e.c b/drivers/staging/r8188eu/hal/HalPhyRf_8188e.c
new file mode 100644
index 000000000..525deab10
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/HalPhyRf_8188e.c
@@ -0,0 +1,908 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+
+/*---------------------------Define Local Constant---------------------------*/
+/*  2010/04/25 MH Define the max tx power tracking tx agc power. */
+#define		ODM_TXPWRTRACK_MAX_IDX_88E		6
+
+/*---------------------------Define Local Constant---------------------------*/
+
+/* 3============================================================ */
+/* 3 Tx Power Tracking */
+/* 3============================================================ */
+/*-----------------------------------------------------------------------------
+ * Function:	ODM_TxPwrTrackAdjust88E()
+ *
+ * Overview:	88E we can not write 0xc80/c94/c4c/ 0xa2x. Instead of write TX agc.
+ *				No matter OFDM & CCK use the same method.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *	04/23/2012	MHC		Adjust TX agc directly not throughput BB digital.
+ *
+ *---------------------------------------------------------------------------*/
+void ODM_TxPwrTrackAdjust88E(struct odm_dm_struct *dm_odm, u8 Type,/*  0 = OFDM, 1 = CCK */
+	u8 *pDirection, 		/*  1 = +(increase) 2 = -(decrease) */
+	u32 *pOutWriteVal		/*  Tx tracking CCK/OFDM BB swing index adjust */
+	)
+{
+	u8 pwr_value = 0;
+	/*  Tx power tracking BB swing table. */
+	/*  The base index = 12. +((12-n)/2)dB 13~?? = decrease tx pwr by -((n-12)/2)dB */
+	if (Type == 0) {		/*  For OFDM afjust */
+		if (dm_odm->BbSwingIdxOfdm <= dm_odm->BbSwingIdxOfdmBase) {
+			*pDirection	= 1;
+			pwr_value		= (dm_odm->BbSwingIdxOfdmBase - dm_odm->BbSwingIdxOfdm);
+		} else {
+			*pDirection	= 2;
+			pwr_value		= (dm_odm->BbSwingIdxOfdm - dm_odm->BbSwingIdxOfdmBase);
+		}
+	} else if (Type == 1) {	/*  For CCK adjust. */
+		if (dm_odm->BbSwingIdxCck <= dm_odm->BbSwingIdxCckBase) {
+			*pDirection	= 1;
+			pwr_value		= (dm_odm->BbSwingIdxCckBase - dm_odm->BbSwingIdxCck);
+		} else {
+			*pDirection	= 2;
+			pwr_value		= (dm_odm->BbSwingIdxCck - dm_odm->BbSwingIdxCckBase);
+		}
+	}
+
+	/*  */
+	/*  2012/04/25 MH According to Ed/Luke.Lees estimate for EVM the max tx power tracking */
+	/*  need to be less than 6 power index for 88E. */
+	/*  */
+	if (pwr_value >= ODM_TXPWRTRACK_MAX_IDX_88E && *pDirection == 1)
+		pwr_value = ODM_TXPWRTRACK_MAX_IDX_88E;
+
+	*pOutWriteVal = pwr_value | (pwr_value << 8) | (pwr_value << 16) | (pwr_value << 24);
+}	/*  ODM_TxPwrTrackAdjust88E */
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *				OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static void odm_TxPwrTrackSetPwr88E(struct odm_dm_struct *dm_odm)
+{
+	if (dm_odm->BbSwingFlagOfdm || dm_odm->BbSwingFlagCck) {
+		PHY_SetTxPowerLevel8188E(dm_odm->Adapter, *dm_odm->pChannel);
+		dm_odm->BbSwingFlagOfdm = false;
+		dm_odm->BbSwingFlagCck	= false;
+	}
+}	/*  odm_TxPwrTrackSetPwr88E */
+
+/* 091212 chiyokolin */
+void
+odm_TXPowerTrackingCallback_ThermalMeter_8188E(
+	struct adapter *Adapter
+	)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u8 ThermalValue = 0, delta, delta_LCK, delta_IQK, offset;
+	u8 ThermalValue_AVG_count = 0;
+	u32 ThermalValue_AVG = 0;
+	s32 ele_D, TempCCk;
+	s8 OFDM_index, CCK_index = 0;
+	s8 OFDM_index_old = 0, CCK_index_old = 0;
+	u32 i = 0, j = 0;
+
+	u8 OFDM_min_index = 6; /* OFDM BB Swing should be less than +3.0dB, which is required by Arthur */
+	s8 OFDM_index_mapping[2][index_mapping_NUM_88E] = {
+		{0, 0, 2, 3, 4, 4, 		/* 2.4G, decrease power */
+		5, 6, 7, 7, 8, 9,
+		10, 10, 11}, /*  For lower temperature, 20120220 updated on 20120220. */
+		{0, 0, -1, -2, -3, -4, 		/* 2.4G, increase power */
+		-4, -4, -4, -5, -7, -8,
+		-9, -9, -10},
+	};
+	u8 Thermal_mapping[2][index_mapping_NUM_88E] = {
+		{0, 2, 4, 6, 8, 10, 		/* 2.4G, decrease power */
+		12, 14, 16, 18, 20, 22,
+		24, 26, 27},
+		{0, 2, 4, 6, 8, 10, 		/* 2.4G,, increase power */
+		12, 14, 16, 18, 20, 22,
+		25, 25, 25},
+	};
+	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
+
+	/*  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E. */
+	odm_TxPwrTrackSetPwr88E(dm_odm);
+
+	/*  <Kordan> RFCalibrateInfo.RegA24 will be initialized when ODM HW configuring, but MP configures with para files. */
+	dm_odm->RFCalibrateInfo.RegA24 = 0x090e1317;
+
+	ThermalValue = (u8)rtl8188e_PHY_QueryRFReg(Adapter, RF_T_METER_88E, 0xfc00); /* 0x42: RF Reg[15:10] 88E */
+
+	if (ThermalValue) {
+		/* Query OFDM path A default setting */
+		ele_D = rtl8188e_PHY_QueryBBReg(Adapter, rOFDM0_XATxIQImbalance, bMaskDWord) & bMaskOFDM_D;
+		for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {	/* find the index */
+			if (ele_D == (OFDMSwingTable[i] & bMaskOFDM_D)) {
+				OFDM_index_old = (u8)i;
+				dm_odm->BbSwingIdxOfdmBase = (u8)i;
+				break;
+			}
+		}
+
+		/* Query CCK default setting From 0xa24 */
+		TempCCk = dm_odm->RFCalibrateInfo.RegA24;
+
+		for (i = 0; i < CCK_TABLE_SIZE; i++) {
+			if (memcmp((void *)&TempCCk, (void *)&cck_swing_table[i][2], 4)) {
+				CCK_index_old = (u8)i;
+				dm_odm->BbSwingIdxCckBase = (u8)i;
+				break;
+			}
+		}
+
+		if (!dm_odm->RFCalibrateInfo.ThermalValue) {
+			dm_odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
+			dm_odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
+			dm_odm->RFCalibrateInfo.ThermalValue_IQK = ThermalValue;
+
+			dm_odm->RFCalibrateInfo.OFDM_index = OFDM_index_old;
+			dm_odm->RFCalibrateInfo.CCK_index = CCK_index_old;
+		}
+
+		/* calculate average thermal meter */
+		dm_odm->RFCalibrateInfo.ThermalValue_AVG[dm_odm->RFCalibrateInfo.ThermalValue_AVG_index] = ThermalValue;
+		dm_odm->RFCalibrateInfo.ThermalValue_AVG_index++;
+		if (dm_odm->RFCalibrateInfo.ThermalValue_AVG_index == AVG_THERMAL_NUM_88E)
+			dm_odm->RFCalibrateInfo.ThermalValue_AVG_index = 0;
+
+		for (i = 0; i < AVG_THERMAL_NUM_88E; i++) {
+			if (dm_odm->RFCalibrateInfo.ThermalValue_AVG[i]) {
+				ThermalValue_AVG += dm_odm->RFCalibrateInfo.ThermalValue_AVG[i];
+				ThermalValue_AVG_count++;
+			}
+		}
+
+		if (ThermalValue_AVG_count)
+			ThermalValue = (u8)(ThermalValue_AVG / ThermalValue_AVG_count);
+
+		if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex) {
+			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
+				(ThermalValue - pHalData->EEPROMThermalMeter) :
+				(pHalData->EEPROMThermalMeter - ThermalValue);
+			dm_odm->RFCalibrateInfo.bReloadtxpowerindex = false;
+			dm_odm->RFCalibrateInfo.bDoneTxpower = false;
+		} else if (dm_odm->RFCalibrateInfo.bDoneTxpower) {
+			delta = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue) ?
+				(ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue) :
+				(dm_odm->RFCalibrateInfo.ThermalValue - ThermalValue);
+		} else {
+			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
+				(ThermalValue - pHalData->EEPROMThermalMeter) :
+				(pHalData->EEPROMThermalMeter - ThermalValue);
+		}
+		delta_LCK = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue_LCK) ?
+			    (ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue_LCK) :
+			    (dm_odm->RFCalibrateInfo.ThermalValue_LCK - ThermalValue);
+		delta_IQK = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue_IQK) ?
+			    (ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue_IQK) :
+			    (dm_odm->RFCalibrateInfo.ThermalValue_IQK - ThermalValue);
+
+		if ((delta_LCK >= 8)) { /*  Delta temperature is equal to or larger than 20 centigrade. */
+			dm_odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
+			PHY_LCCalibrate_8188E(Adapter);
+		}
+
+		if (delta > 0 && dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
+			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
+				(ThermalValue - pHalData->EEPROMThermalMeter) :
+				(pHalData->EEPROMThermalMeter - ThermalValue);
+			/* calculate new OFDM / CCK offset */
+			if (ThermalValue > pHalData->EEPROMThermalMeter)
+				j = 1;
+			else
+				j = 0;
+			for (offset = 0; offset < index_mapping_NUM_88E; offset++) {
+				if (delta < Thermal_mapping[j][offset]) {
+					if (offset != 0)
+						offset--;
+					break;
+				}
+			}
+			if (offset >= index_mapping_NUM_88E)
+				offset = index_mapping_NUM_88E - 1;
+			OFDM_index = dm_odm->RFCalibrateInfo.OFDM_index + OFDM_index_mapping[j][offset];
+			CCK_index = dm_odm->RFCalibrateInfo.CCK_index + OFDM_index_mapping[j][offset];
+
+			if (OFDM_index > OFDM_TABLE_SIZE_92D - 1)
+				OFDM_index = OFDM_TABLE_SIZE_92D - 1;
+			else if (OFDM_index < OFDM_min_index)
+				OFDM_index = OFDM_min_index;
+
+			if (CCK_index > CCK_TABLE_SIZE - 1)
+				CCK_index = CCK_TABLE_SIZE - 1;
+			else if (CCK_index < 0)
+				CCK_index = 0;
+
+			/* 2 temporarily remove bNOPG */
+			/* Config by SwingTable */
+			if (dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
+				dm_odm->RFCalibrateInfo.bDoneTxpower = true;
+
+				/*  Revse TX power table. */
+				dm_odm->BbSwingIdxOfdm		= (u8)OFDM_index;
+				dm_odm->BbSwingIdxCck		= (u8)CCK_index;
+
+				if (dm_odm->BbSwingIdxOfdmCurrent != dm_odm->BbSwingIdxOfdm) {
+					dm_odm->BbSwingIdxOfdmCurrent = dm_odm->BbSwingIdxOfdm;
+					dm_odm->BbSwingFlagOfdm = true;
+				}
+
+				if (dm_odm->BbSwingIdxCckCurrent != dm_odm->BbSwingIdxCck) {
+					dm_odm->BbSwingIdxCckCurrent = dm_odm->BbSwingIdxCck;
+					dm_odm->BbSwingFlagCck = true;
+				}
+			}
+		}
+
+		if (delta_IQK >= 8) { /*  Delta temperature is equal to or larger than 20 centigrade. */
+			dm_odm->RFCalibrateInfo.ThermalValue_IQK = ThermalValue;
+			PHY_IQCalibrate_8188E(Adapter, false);
+		}
+		/* update thermal meter value */
+		if (dm_odm->RFCalibrateInfo.TxPowerTrackControl)
+			dm_odm->RFCalibrateInfo.ThermalValue = ThermalValue;
+	}
+}
+
+/* 1 7.	IQK */
+#define MAX_TOLERANCE		5
+#define IQK_DELAY_TIME		1		/* ms */
+
+static u8 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+phy_PathA_IQK_8188E(struct adapter *adapt)
+{
+	u32 regeac, regE94, regE9C;
+	u8 result = 0x00;
+
+	/* 1 Tx IQK */
+	/* path-A IQK setting */
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x8214032a);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
+
+	/* LO calibration setting */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* One shot, path A LOK & IQK */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_88E);
+
+	/*  Check failed */
+	regeac = rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+
+	if (!(regeac & BIT(28)) &&
+	    (((regE94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((regE9C & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	return result;
+}
+
+static u8 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+phy_PathA_RxIQK(struct adapter *adapt)
+{
+	u32 regeac, regE94, regE9C, regEA4, u4tmp;
+	u8 result = 0x00;
+
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	rtl8188e_PHY_SetRFReg(adapt, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
+	rtl8188e_PHY_SetRFReg(adapt, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	rtl8188e_PHY_SetRFReg(adapt, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
+	rtl8188e_PHY_SetRFReg(adapt, RF_TXPA_G2, bRFRegOffsetMask, 0xf117B);
+
+	/* PA,PAD off */
+	rtl8188e_PHY_SetRFReg(adapt, 0xdf, bRFRegOffsetMask, 0x980);
+	rtl8188e_PHY_SetRFReg(adapt, 0x56, bRFRegOffsetMask, 0x51000);
+
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+
+	/* IQK setting */
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
+
+	/* path-A IQK setting */
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
+
+	/* LO calibration setting */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	/* One shot, path A LOK & IQK */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	mdelay(IQK_DELAY_TIME_88E);
+
+	/*  Check failed */
+	regeac = rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+
+	if (!(regeac & BIT(28)) &&
+	    (((regE94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((regE9C & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else							/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (regE94 & 0x3FF0000)  | ((regE9C & 0x3FF0000) >> 16);
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, u4tmp);
+
+	/* 1 RX IQK */
+	/* modify RXIQK mode table */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	rtl8188e_PHY_SetRFReg(adapt, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
+	rtl8188e_PHY_SetRFReg(adapt, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	rtl8188e_PHY_SetRFReg(adapt, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
+	rtl8188e_PHY_SetRFReg(adapt, RF_TXPA_G2, bRFRegOffsetMask, 0xf7ffa);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+
+	/* IQK setting */
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c05);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160c1f);
+
+	/* LO calibration setting */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	/* One shot, path A LOK & IQK */
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	rtl8188e_PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_88E);
+
+	/*  Check failed */
+	regeac = rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+	regEA4 = rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
+
+	/* reload RF 0xdf */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	rtl8188e_PHY_SetRFReg(adapt, 0xdf, bRFRegOffsetMask, 0x180);
+
+	if (!(regeac & BIT(27)) &&		/* if Tx is OK, check whether Rx is OK */
+	    (((regEA4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((regeac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+
+	return result;
+}
+
+static void patha_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8], u8 final_candidate, bool txonly)
+{
+	u32 Oldval_0, X, TX0_A, reg;
+	s32 Y, TX0_C;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (iqkok) {
+		Oldval_0 = (rtl8188e_PHY_QueryBBReg(adapt, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		X = result[final_candidate][0];
+		if ((X & 0x00000200) != 0)
+			X = X | 0xFFFFFC00;
+		TX0_A = (X * Oldval_0) >> 8;
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_XATxIQImbalance, 0x3FF, TX0_A);
+
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(31), ((X * Oldval_0 >> 7) & 0x1));
+
+		Y = result[final_candidate][1];
+		if ((Y & 0x00000200) != 0)
+			Y = Y | 0xFFFFFC00;
+
+		TX0_C = (Y * Oldval_0) >> 8;
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_XCTxAFE, 0xF0000000, ((TX0_C & 0x3C0) >> 6));
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_XATxIQImbalance, 0x003F0000, (TX0_C & 0x3F));
+
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(29), ((Y * Oldval_0 >> 7) & 0x1));
+
+		if (txonly)
+			return;
+
+		reg = result[final_candidate][2];
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_XARxIQImbalance, 0x3FF, reg);
+
+		reg = result[final_candidate][3] & 0x3F;
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_XARxIQImbalance, 0xFC00, reg);
+
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		rtl8188e_PHY_SetBBReg(adapt, rOFDM0_RxIQExtAnta, 0xF0000000, reg);
+	}
+}
+
+void _PHY_SaveADDARegisters(struct adapter *adapt, u32 *ADDAReg, u32 *ADDABackup, u32 RegisterNum)
+{
+	u32 i;
+
+	for (i = 0; i < RegisterNum; i++) {
+		ADDABackup[i] = rtl8188e_PHY_QueryBBReg(adapt, ADDAReg[i], bMaskDWord);
+	}
+}
+
+/* FIXME: return an error to caller */
+static void _PHY_SaveMACRegisters(
+		struct adapter *adapt,
+		u32 *MACReg,
+		u32 *MACBackup
+	)
+{
+	u32 i;
+	int res;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++) {
+		u8 reg;
+
+		res = rtw_read8(adapt, MACReg[i], &reg);
+		if (res)
+			return;
+
+		MACBackup[i] = reg;
+	}
+
+	res = rtw_read32(adapt, MACReg[i], MACBackup + i);
+	(void)res;
+}
+
+static void reload_adda_reg(struct adapter *adapt, u32 *ADDAReg, u32 *ADDABackup, u32 RegiesterNum)
+{
+	u32 i;
+
+	for (i = 0; i < RegiesterNum; i++)
+		rtl8188e_PHY_SetBBReg(adapt, ADDAReg[i], bMaskDWord, ADDABackup[i]);
+}
+
+static void
+_PHY_ReloadMACRegisters(
+		struct adapter *adapt,
+		u32 *MACReg,
+		u32 *MACBackup
+	)
+{
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtw_write8(adapt, MACReg[i], (u8)MACBackup[i]);
+
+	rtw_write32(adapt, MACReg[i], MACBackup[i]);
+}
+
+static void
+_PHY_PathADDAOn(
+		struct adapter *adapt,
+		u32 *ADDAReg)
+{
+	u32 i;
+
+	rtl8188e_PHY_SetBBReg(adapt, ADDAReg[0], bMaskDWord, 0x0b1b25a0);
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl8188e_PHY_SetBBReg(adapt, ADDAReg[i], bMaskDWord, 0x0bdb25a0);
+}
+
+void
+_PHY_MACSettingCalibration(
+		struct adapter *adapt,
+		u32 *MACReg,
+		u32 *MACBackup
+	)
+{
+	u32 i = 0;
+
+	rtw_write8(adapt, MACReg[i], 0x3F);
+
+	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtw_write8(adapt, MACReg[i], (u8)(MACBackup[i] & (~BIT(3))));
+
+	rtw_write8(adapt, MACReg[i], (u8)(MACBackup[i] & (~BIT(5))));
+}
+
+static void _PHY_PIModeSwitch(
+		struct adapter *adapt,
+		bool PIMode
+	)
+{
+	u32 mode;
+
+	mode = PIMode ? 0x01000100 : 0x01000000;
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XA_HSSIParameter1, bMaskDWord, mode);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XB_HSSIParameter1, bMaskDWord, mode);
+}
+
+static bool phy_SimularityCompare_8188E(
+		struct adapter *adapt,
+		s32 resulta[][8],
+		u8  c1,
+		u8  c2
+	)
+{
+	u32 i, j, diff, sim_bitmap, bound = 0;
+	u8 final_candidate[2] = {0xFF, 0xFF};	/* for path A and path B */
+	bool result = true;
+	s32 tmp1 = 0, tmp2 = 0;
+
+	bound = 4;
+	sim_bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((resulta[c1][i] & 0x00000200) != 0)
+				tmp1 = resulta[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = resulta[c1][i];
+
+			if ((resulta[c2][i] & 0x00000200) != 0)
+				tmp2 = resulta[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = resulta[c2][i];
+		} else {
+			tmp1 = resulta[c1][i];
+			tmp2 = resulta[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !sim_bitmap) {
+				if (resulta[c1][i] + resulta[c1][i + 1] == 0)
+					final_candidate[(i / 4)] = c2;
+				else if (resulta[c2][i] + resulta[c2][i + 1] == 0)
+					final_candidate[(i / 4)] = c1;
+				else
+					sim_bitmap = sim_bitmap | (1 << i);
+			} else {
+				sim_bitmap = sim_bitmap | (1 << i);
+			}
+		}
+	}
+
+	if (sim_bitmap == 0) {
+		for (i = 0; i < (bound / 4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
+					resulta[3][j] = resulta[final_candidate[i]][j];
+				result = false;
+			}
+		}
+		return result;
+	} else {
+		if (!(sim_bitmap & 0x03)) {		   /* path A TX OK */
+			for (i = 0; i < 2; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+		if (!(sim_bitmap & 0x0c)) {		   /* path A RX OK */
+			for (i = 2; i < 4; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+
+		if (!(sim_bitmap & 0x30)) { /* path B TX OK */
+			for (i = 4; i < 6; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+
+		if (!(sim_bitmap & 0xc0)) { /* path B RX OK */
+			for (i = 6; i < 8; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+		return false;
+	}
+}
+
+static void phy_IQCalibrate_8188E(struct adapter *adapt, s32 result[][8], u8 t)
+{
+	struct hal_data_8188e *pHalData = &adapt->haldata;
+	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
+	u32 i;
+	u8 PathAOK;
+	u32 ADDA_REG[IQK_ADDA_REG_NUM] = {
+						rFPGA0_XCD_SwitchControl, rBlue_Tooth,
+						rRx_Wait_CCA, 	rTx_CCK_RFON,
+						rTx_CCK_BBON, rTx_OFDM_RFON,
+						rTx_OFDM_BBON, rTx_To_Rx,
+						rTx_To_Tx, 	rRx_CCK,
+						rRx_OFDM, 	rRx_Wait_RIFS,
+						rRx_TO_Rx, 	rStandby,
+						rSleep, 			rPMPD_ANAEN };
+	u32 IQK_MAC_REG[IQK_MAC_REG_NUM] = {
+						REG_TXPAUSE, 	REG_BCN_CTRL,
+						REG_BCN_CTRL_1, REG_GPIO_MUXCFG};
+
+	/* since 92C & 92D have the different define in IQK_BB_REG */
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+							rOFDM0_TRxPathEnable, 	rOFDM0_TRMuxPar,
+							rFPGA0_XCD_RFInterfaceSW, rConfig_AntA, rConfig_AntB,
+							rFPGA0_XAB_RFInterfaceSW, rFPGA0_XA_RFInterfaceOE,
+							rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD
+							};
+	u32 retryCount = 2;
+	/*  Note: IQ calibration must be performed after loading */
+	/* 		PHY_REG.txt , and radio_a, radio_b.txt */
+
+	if (t == 0) {
+		/*  Save ADDA parameters, turn Path A ADDA on */
+		_PHY_SaveADDARegisters(adapt, ADDA_REG, dm_odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+		_PHY_SaveMACRegisters(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+		_PHY_SaveADDARegisters(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+	}
+
+	_PHY_PathADDAOn(adapt, ADDA_REG);
+	if (t == 0)
+		dm_odm->RFCalibrateInfo.bRfPiEnable = (u8)rtl8188e_PHY_QueryBBReg(adapt, rFPGA0_XA_HSSIParameter1, BIT(8));
+
+	if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
+		/*  Switch BB to PI mode to do IQ Calibration. */
+		_PHY_PIModeSwitch(adapt, true);
+	}
+
+	/* BB setting */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_RFMOD, BIT(24), 0x00);
+	rtl8188e_PHY_SetBBReg(adapt, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
+	rtl8188e_PHY_SetBBReg(adapt, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
+
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT(10), 0x01);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT(26), 0x01);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XA_RFInterfaceOE, BIT(10), 0x00);
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XB_RFInterfaceOE, BIT(10), 0x00);
+
+	/* MAC settings */
+	_PHY_MACSettingCalibration(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+
+	/* Page B init */
+	/* AP or IQK */
+	rtl8188e_PHY_SetBBReg(adapt, rConfig_AntA, bMaskDWord, 0x0f600000);
+
+
+	/*  IQ calibration setting */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+	rtl8188e_PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
+	rtl8188e_PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
+
+	for (i = 0; i < retryCount; i++) {
+		PathAOK = phy_PathA_IQK_8188E(adapt);
+		if (PathAOK == 0x01) {
+			result[t][0] = (rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord) & 0x3FF0000) >> 16;
+			result[t][1] = (rtl8188e_PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord) & 0x3FF0000) >> 16;
+			break;
+		}
+	}
+
+	for (i = 0; i < retryCount; i++) {
+		PathAOK = phy_PathA_RxIQK(adapt);
+		if (PathAOK == 0x03) {
+			result[t][2] = (rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord) & 0x3FF0000) >> 16;
+			result[t][3] = (rtl8188e_PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord) & 0x3FF0000) >> 16;
+			break;
+		}
+	}
+
+	/* Back to BB mode, load original value */
+	rtl8188e_PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0);
+
+	if (t != 0) {
+		if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
+			/*  Switch back BB to SI mode after finish IQ Calibration. */
+			_PHY_PIModeSwitch(adapt, false);
+		}
+
+		/*  Reload ADDA power saving parameters */
+		reload_adda_reg(adapt, ADDA_REG, dm_odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+
+		/*  Reload MAC parameters */
+		_PHY_ReloadMACRegisters(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+
+		reload_adda_reg(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+
+		/*  Restore RX initial gain */
+		rtl8188e_PHY_SetBBReg(adapt, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00032ed3);
+
+		/* load 0xe30 IQC default value */
+		rtl8188e_PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+		rtl8188e_PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+	}
+}
+
+static void phy_LCCalibrate_8188E(struct adapter *adapt)
+{
+	u8 tmpreg;
+	u32 RF_Amode = 0, LC_Cal;
+	int res;
+
+	/* Check continuous TX and Packet TX */
+	res = rtw_read8(adapt, 0xd03, &tmpreg);
+	if (res)
+		return;
+
+	if ((tmpreg & 0x70) != 0)			/* Deal with contisuous TX case */
+		rtw_write8(adapt, 0xd03, tmpreg & 0x8F);	/* disable all continuous TX */
+	else							/*  Deal with Packet TX case */
+		rtw_write8(adapt, REG_TXPAUSE, 0xFF);		/*  block all queues */
+
+	if ((tmpreg & 0x70) != 0) {
+		/* 1. Read original RF mode */
+		/* Path-A */
+		RF_Amode = rtl8188e_PHY_QueryRFReg(adapt, RF_AC, bMask12Bits);
+
+		/* 2. Set RF mode = standby mode */
+		/* Path-A */
+		rtl8188e_PHY_SetRFReg(adapt, RF_AC, bMask12Bits, (RF_Amode & 0x8FFFF) | 0x10000);
+	}
+
+	/* 3. Read RF reg18 */
+	LC_Cal = rtl8188e_PHY_QueryRFReg(adapt, RF_CHNLBW, bMask12Bits);
+
+	/* 4. Set LC calibration begin	bit15 */
+	rtl8188e_PHY_SetRFReg(adapt, RF_CHNLBW, bMask12Bits, LC_Cal | 0x08000);
+
+	msleep(100);
+
+	/* Restore original situation */
+	if ((tmpreg & 0x70) != 0) {
+		/* Deal with continuous TX case */
+		/* Path-A */
+		rtw_write8(adapt, 0xd03, tmpreg);
+		rtl8188e_PHY_SetRFReg(adapt, RF_AC, bMask12Bits, RF_Amode);
+	} else {
+		/*  Deal with Packet TX case */
+		rtw_write8(adapt, REG_TXPAUSE, 0x00);
+	}
+}
+
+void PHY_IQCalibrate_8188E(struct adapter *adapt, bool recovery)
+{
+	struct hal_data_8188e *pHalData = &adapt->haldata;
+	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
+	s32 result[4][8];	/* last is final result */
+	u8 i, final_candidate;
+	bool pathaok;
+	s32 RegE94, RegE9C, RegEA4, RegEB4, RegEBC;
+	bool is12simular, is13simular, is23simular;
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+		rOFDM0_XARxIQImbalance, rOFDM0_XBRxIQImbalance,
+		rOFDM0_ECCAThreshold, rOFDM0_AGCRSSITable,
+		rOFDM0_XATxIQImbalance, rOFDM0_XBTxIQImbalance,
+		rOFDM0_XCTxAFE, rOFDM0_XDTxAFE,
+		rOFDM0_RxIQExtAnta};
+
+	if (recovery) {
+		reload_adda_reg(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+		return;
+	}
+
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		if ((i == 0) || (i == 2) || (i == 4)  || (i == 6))
+			result[3][i] = 0x100;
+		else
+			result[3][i] = 0;
+	}
+	final_candidate = 0xff;
+	pathaok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+
+	for (i = 0; i < 3; i++) {
+		phy_IQCalibrate_8188E(adapt, result, i);
+
+		if (i == 1) {
+			is12simular = phy_SimularityCompare_8188E(adapt, result, 0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = phy_SimularityCompare_8188E(adapt, result, 0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+
+				break;
+			}
+			is23simular = phy_SimularityCompare_8188E(adapt, result, 1, 2);
+			if (is23simular) {
+				final_candidate = 1;
+			} else {
+				final_candidate = 3;
+			}
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		RegE94 = result[i][0];
+		RegE9C = result[i][1];
+		RegEA4 = result[i][2];
+		RegEB4 = result[i][4];
+		RegEBC = result[i][5];
+	}
+
+	if (final_candidate != 0xff) {
+		RegE94 = result[final_candidate][0];
+		RegE9C = result[final_candidate][1];
+		RegEA4 = result[final_candidate][2];
+		RegEB4 = result[final_candidate][4];
+		RegEBC = result[final_candidate][5];
+		dm_odm->RFCalibrateInfo.RegE94 = RegE94;
+		dm_odm->RFCalibrateInfo.RegE9C = RegE9C;
+		dm_odm->RFCalibrateInfo.RegEB4 = RegEB4;
+		dm_odm->RFCalibrateInfo.RegEBC = RegEBC;
+		pathaok = true;
+	} else {
+		dm_odm->RFCalibrateInfo.RegE94 = 0x100;
+		dm_odm->RFCalibrateInfo.RegEB4 = 0x100;	/* X default value */
+		dm_odm->RFCalibrateInfo.RegE9C = 0x0;
+		dm_odm->RFCalibrateInfo.RegEBC = 0x0;	/* Y default value */
+	}
+	if (RegE94 != 0)
+		patha_fill_iqk(adapt, pathaok, result, final_candidate, (RegEA4 == 0));
+
+/* To Fix BSOD when final_candidate is 0xff */
+/* by sherry 20120321 */
+	if (final_candidate < 4) {
+		for (i = 0; i < IQK_Matrix_REG_NUM; i++)
+			dm_odm->RFCalibrateInfo.IQKMatrixRegSetting.Value[0][i] = result[final_candidate][i];
+		dm_odm->RFCalibrateInfo.IQKMatrixRegSetting.bIQKDone = true;
+	}
+
+	_PHY_SaveADDARegisters(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+}
+
+void PHY_LCCalibrate_8188E(struct adapter *adapt)
+{
+	u32 timeout = 2000, timecount = 0;
+	struct hal_data_8188e *pHalData = &adapt->haldata;
+	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
+
+	while (*dm_odm->pbScanInProcess && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	phy_LCCalibrate_8188E(adapt);
+}
diff --git a/drivers/staging/r8188eu/hal/HalPwrSeqCmd.c b/drivers/staging/r8188eu/hal/HalPwrSeqCmd.c
new file mode 100644
index 000000000..6c0b13683
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/HalPwrSeqCmd.c
@@ -0,0 +1,149 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/HalPwrSeqCmd.h"
+
+#define PWR_CMD_WRITE			0x01
+     /*  offset: the read register offset */
+     /*  msk: the mask of the write bits */
+     /*  value: write value */
+     /*  note: driver shall implement this cmd by read & msk after write */
+
+#define PWR_CMD_POLLING			0x02
+     /*  offset: the read register offset */
+     /*  msk: the mask of the polled value */
+     /*  value: the value to be polled, masked by the msd field. */
+     /*  note: driver shall implement this cmd by */
+     /*  do{ */
+     /*  if ( (Read(offset) & msk) == (value & msk) ) */
+     /*  break; */
+     /*  } while (not timeout); */
+
+#define PWR_CMD_DELAY			0x03
+     /*  offset: the value to delay (in us) */
+     /*  msk: N/A */
+     /*  value: N/A */
+
+struct wl_pwr_cfg {
+	u16 offset;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+#define GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
+#define GET_PWR_CFG_CMD(__PWR_CMD)		__PWR_CMD.cmd
+#define GET_PWR_CFG_MASK(__PWR_CMD)		__PWR_CMD.msk
+#define GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
+
+static struct wl_pwr_cfg rtl8188E_power_on_flow[] = {
+	{ 0x0006, PWR_CMD_POLLING, BIT(1), BIT(1) },
+	{ 0x0002, PWR_CMD_WRITE, BIT(0) | BIT(1), 0 }, /* reset BB */
+	{ 0x0026, PWR_CMD_WRITE, BIT(7), BIT(7) }, /* schmitt trigger */
+	{ 0x0005, PWR_CMD_WRITE, BIT(7), 0 }, /* disable HWPDN (control by DRV)*/
+	{ 0x0005, PWR_CMD_WRITE, BIT(4) | BIT(3), 0 }, /* disable WL suspend*/
+	{ 0x0005, PWR_CMD_WRITE, BIT(0), BIT(0) },
+	{ 0x0005, PWR_CMD_POLLING, BIT(0), 0 },
+	{ 0x0023, PWR_CMD_WRITE, BIT(4), 0 },
+};
+
+static struct wl_pwr_cfg rtl8188E_card_disable_flow[] = {
+	{ 0x001F, PWR_CMD_WRITE, 0xFF, 0 }, /* turn off RF */
+	{ 0x0023, PWR_CMD_WRITE, BIT(4), BIT(4) }, /* LDO Sleep mode */
+	{ 0x0005, PWR_CMD_WRITE, BIT(1), BIT(1) }, /* turn off MAC by HW state machine */
+	{ 0x0005, PWR_CMD_POLLING, BIT(1), 0 },
+	{ 0x0026, PWR_CMD_WRITE, BIT(7), BIT(7) }, /* schmitt trigger */
+	{ 0x0005, PWR_CMD_WRITE, BIT(3) | BIT(4), BIT(3) }, /* enable WL suspend */
+	{ 0x0007, PWR_CMD_WRITE, 0xFF, 0 }, /* enable bandgap mbias in suspend */
+	{ 0x0041, PWR_CMD_WRITE, BIT(4), 0 }, /* Clear SIC_EN register */
+	{ 0xfe10, PWR_CMD_WRITE, BIT(4), BIT(4) }, /* Set USB suspend enable local register */
+};
+
+/* This is used by driver for LPSRadioOff Procedure, not for FW LPS Step */
+static struct wl_pwr_cfg rtl8188E_enter_lps_flow[] = {
+	{ 0x0522, PWR_CMD_WRITE, 0xFF, 0x7F },/* Tx Pause */
+	{ 0x05F8, PWR_CMD_POLLING, 0xFF, 0 }, /* Should be zero if no packet is transmitted */
+	{ 0x05F9, PWR_CMD_POLLING, 0xFF, 0 }, /* Should be zero if no packet is transmitted */
+	{ 0x05FA, PWR_CMD_POLLING, 0xFF, 0 }, /* Should be zero if no packet is transmitted */
+	{ 0x05FB, PWR_CMD_POLLING, 0xFF, 0 }, /* Should be zero if no packet is transmitted */
+	{ 0x0002, PWR_CMD_WRITE, BIT(0), 0 }, /* CCK and OFDM are disabled, clocks are gated */
+	{ 0x0002, PWR_CMD_DELAY, 0, 0 },
+	{ 0x0100, PWR_CMD_WRITE, 0xFF, 0x3F }, /* Reset MAC TRX */
+	{ 0x0101, PWR_CMD_WRITE, BIT(1), 0 }, /* check if removed later */
+	{ 0x0553, PWR_CMD_WRITE, BIT(5), BIT(5) }, /* Respond TxOK to scheduler */
+};
+
+u8 HalPwrSeqCmdParsing(struct adapter *padapter, enum r8188eu_pwr_seq seq)
+{
+	struct wl_pwr_cfg pwrcfgcmd = {0};
+	struct wl_pwr_cfg *pwrseqcmd;
+	u8 poll_bit = false;
+	u8 idx, num_steps;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 poll_count = 0; /*  polling autoload done. */
+	u32 max_poll_count = 5000;
+	int res;
+
+	switch (seq) {
+	case PWR_ON_FLOW:
+		pwrseqcmd = rtl8188E_power_on_flow;
+		num_steps = ARRAY_SIZE(rtl8188E_power_on_flow);
+		break;
+	case DISABLE_FLOW:
+		pwrseqcmd = rtl8188E_card_disable_flow;
+		num_steps = ARRAY_SIZE(rtl8188E_card_disable_flow);
+		break;
+	case LPS_ENTER_FLOW:
+		pwrseqcmd = rtl8188E_enter_lps_flow;
+		num_steps = ARRAY_SIZE(rtl8188E_enter_lps_flow);
+		break;
+	default:
+		return false;
+	}
+
+	for (idx = 0; idx < num_steps; idx++) {
+		pwrcfgcmd = pwrseqcmd[idx];
+
+		switch (GET_PWR_CFG_CMD(pwrcfgcmd)) {
+		case PWR_CMD_WRITE:
+			offset = GET_PWR_CFG_OFFSET(pwrcfgcmd);
+
+			/*  Read the value from system register */
+			res = rtw_read8(padapter, offset, &value);
+			if (res)
+				return false;
+
+			value &= ~(GET_PWR_CFG_MASK(pwrcfgcmd));
+			value |= (GET_PWR_CFG_VALUE(pwrcfgcmd) & GET_PWR_CFG_MASK(pwrcfgcmd));
+
+			/*  Write the value back to system register */
+			rtw_write8(padapter, offset, value);
+			break;
+		case PWR_CMD_POLLING:
+			poll_bit = false;
+			offset = GET_PWR_CFG_OFFSET(pwrcfgcmd);
+			do {
+				res = rtw_read8(padapter, offset, &value);
+				if (res)
+					return false;
+
+				value &= GET_PWR_CFG_MASK(pwrcfgcmd);
+				if (value == (GET_PWR_CFG_VALUE(pwrcfgcmd) & GET_PWR_CFG_MASK(pwrcfgcmd)))
+					poll_bit = true;
+				else
+					udelay(10);
+
+				if (poll_count++ > max_poll_count)
+					return false;
+			} while (!poll_bit);
+			break;
+		case PWR_CMD_DELAY:
+			udelay(GET_PWR_CFG_OFFSET(pwrcfgcmd));
+			break;
+		default:
+			break;
+		}
+	}
+	return true;
+}
diff --git a/drivers/staging/r8188eu/hal/hal_com.c b/drivers/staging/r8188eu/hal/hal_com.c
new file mode 100644
index 000000000..33967eb3c
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/hal_com.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+
+#include "../include/hal_intf.h"
+#include "../include/hal_com.h"
+#include "../include/rtl8188e_hal.h"
+
+#define _HAL_INIT_C_
+
+#define	CHAN_PLAN_HW	0x80
+
+u8 /* return the final channel plan decision */
+hal_com_get_channel_plan(struct adapter *padapter, u8 hw_channel_plan,
+			 u8 sw_channel_plan, u8 def_channel_plan,
+			 bool load_fail)
+{
+	u8 sw_cfg;
+	u8 chnlplan;
+
+	sw_cfg = true;
+	if (!load_fail) {
+		if (!rtw_is_channel_plan_valid(sw_channel_plan))
+			sw_cfg = false;
+		if (hw_channel_plan & CHAN_PLAN_HW)
+			sw_cfg = false;
+	}
+
+	if (sw_cfg)
+		chnlplan = sw_channel_plan;
+	else
+		chnlplan = hw_channel_plan & (~CHAN_PLAN_HW);
+
+	if (!rtw_is_channel_plan_valid(chnlplan))
+		chnlplan = def_channel_plan;
+
+	return chnlplan;
+}
+
+u8 MRateToHwRate(u8 rate)
+{
+	u8 ret = DESC_RATE1M;
+
+	switch (rate) {
+		/*  CCK and OFDM non-HT rates */
+	case IEEE80211_CCK_RATE_1MB:
+		ret = DESC_RATE1M;
+		break;
+	case IEEE80211_CCK_RATE_2MB:
+		ret = DESC_RATE2M;
+		break;
+	case IEEE80211_CCK_RATE_5MB:
+		ret = DESC_RATE5_5M;
+		break;
+	case IEEE80211_CCK_RATE_11MB:
+		ret = DESC_RATE11M;
+		break;
+	case IEEE80211_OFDM_RATE_6MB:
+		ret = DESC_RATE6M;
+		break;
+	case IEEE80211_OFDM_RATE_9MB:
+		ret = DESC_RATE9M;
+		break;
+	case IEEE80211_OFDM_RATE_12MB:
+		ret = DESC_RATE12M;
+		break;
+	case IEEE80211_OFDM_RATE_18MB:
+		ret = DESC_RATE18M;
+		break;
+	case IEEE80211_OFDM_RATE_24MB:
+		ret = DESC_RATE24M;
+		break;
+	case IEEE80211_OFDM_RATE_36MB:
+		ret = DESC_RATE36M;
+		break;
+	case IEEE80211_OFDM_RATE_48MB:
+		ret = DESC_RATE48M;
+		break;
+	case IEEE80211_OFDM_RATE_54MB:
+		ret = DESC_RATE54M;
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+void HalSetBrateCfg(struct adapter *adapt, u8 *brates, u16 *rate_cfg)
+{
+	u8 i, is_brate, brate;
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		is_brate = brates[i] & IEEE80211_BASIC_RATE_MASK;
+		brate = brates[i] & 0x7f;
+
+		if (is_brate) {
+			switch (brate) {
+			case IEEE80211_CCK_RATE_1MB:
+				*rate_cfg |= RATE_1M;
+				break;
+			case IEEE80211_CCK_RATE_2MB:
+				*rate_cfg |= RATE_2M;
+				break;
+			case IEEE80211_CCK_RATE_5MB:
+				*rate_cfg |= RATE_5_5M;
+				break;
+			case IEEE80211_CCK_RATE_11MB:
+				*rate_cfg |= RATE_11M;
+				break;
+			case IEEE80211_OFDM_RATE_6MB:
+				*rate_cfg |= RATE_6M;
+				break;
+			case IEEE80211_OFDM_RATE_9MB:
+				*rate_cfg |= RATE_9M;
+				break;
+			case IEEE80211_OFDM_RATE_12MB:
+				*rate_cfg |= RATE_12M;
+				break;
+			case IEEE80211_OFDM_RATE_18MB:
+				*rate_cfg |= RATE_18M;
+				break;
+			case IEEE80211_OFDM_RATE_24MB:
+				*rate_cfg |= RATE_24M;
+				break;
+			case IEEE80211_OFDM_RATE_36MB:
+				*rate_cfg |= RATE_36M;
+				break;
+			case IEEE80211_OFDM_RATE_48MB:
+				*rate_cfg |= RATE_48M;
+				break;
+			case IEEE80211_OFDM_RATE_54MB:
+				*rate_cfg |= RATE_54M;
+				break;
+			}
+		}
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/hal_intf.c b/drivers/staging/r8188eu/hal/hal_intf.c
new file mode 100644
index 000000000..37935aef7
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/hal_intf.c
@@ -0,0 +1,55 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _HAL_INTF_C_
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/hal_intf.h"
+
+uint	 rtw_hal_init(struct adapter *adapt)
+{
+	uint	status = _SUCCESS;
+
+	adapt->hw_init_completed = false;
+
+	status = rtl8188eu_hal_init(adapt);
+
+	if (status == _SUCCESS) {
+		adapt->hw_init_completed = true;
+
+		if (adapt->registrypriv.notch_filter == 1)
+			hal_notch_filter_8188e(adapt, 1);
+	} else {
+		adapt->hw_init_completed = false;
+	}
+
+	return status;
+}
+
+uint rtw_hal_deinit(struct adapter *adapt)
+{
+	uint	status = _SUCCESS;
+
+	status = rtl8188eu_hal_deinit(adapt);
+
+	if (status == _SUCCESS)
+		adapt->hw_init_completed = false;
+
+	return status;
+}
+
+void rtw_hal_update_ra_mask(struct adapter *adapt, u32 mac_id, u8 rssi_level)
+{
+	struct mlme_priv *pmlmepriv = &adapt->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &adapt->stapriv;
+		if (mac_id >= 2)
+			psta = pstapriv->sta_aid[(mac_id - 1) - 1];
+		if (psta)
+			add_RATid(adapt, psta, 0);/* todo: based on rssi_level*/
+	} else {
+		UpdateHalRAMask8188EUsb(adapt, mac_id, rssi_level);
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/odm.c b/drivers/staging/r8188eu/hal/odm.c
new file mode 100644
index 000000000..94f9b125d
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/odm.c
@@ -0,0 +1,821 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+
+/* avoid to warn in FreeBSD ==> To DO modify */
+static u32 EDCAParam[HT_IOT_PEER_MAX][3] = {
+	/*  UL			DL */
+	{0x5ea42b, 0x5ea42b, 0x5ea42b}, /* 0:unknown AP */
+	{0xa44f, 0x5ea44f, 0x5e431c}, /*  1:realtek AP */
+	{0x5ea42b, 0x5ea42b, 0x5ea42b}, /*  2:unknown AP => realtek_92SE */
+	{0x5ea32b, 0x5ea42b, 0x5e4322}, /*  3:broadcom AP */
+	{0x5ea422, 0x00a44f, 0x00a44f}, /*  4:ralink AP */
+	{0x5ea322, 0x00a630, 0x00a44f}, /*  5:atheros AP */
+	{0x5e4322, 0x5e4322, 0x5e4322},/*  6:cisco AP */
+	{0x5ea44f, 0x00a44f, 0x5ea42b}, /*  8:marvell AP */
+	{0x5ea42b, 0x5ea42b, 0x5ea42b}, /*  10:unknown AP=> 92U AP */
+	{0x5ea42b, 0xa630, 0x5e431c}, /*  11:airgocap AP */
+};
+
+/*  Global var */
+u32 OFDMSwingTable[OFDM_TABLE_SIZE_92D] = {
+	0x7f8001fe, /*  0, +6.0dB */
+	0x788001e2, /*  1, +5.5dB */
+	0x71c001c7, /*  2, +5.0dB */
+	0x6b8001ae, /*  3, +4.5dB */
+	0x65400195, /*  4, +4.0dB */
+	0x5fc0017f, /*  5, +3.5dB */
+	0x5a400169, /*  6, +3.0dB */
+	0x55400155, /*  7, +2.5dB */
+	0x50800142, /*  8, +2.0dB */
+	0x4c000130, /*  9, +1.5dB */
+	0x47c0011f, /*  10, +1.0dB */
+	0x43c0010f, /*  11, +0.5dB */
+	0x40000100, /*  12, +0dB */
+	0x3c8000f2, /*  13, -0.5dB */
+	0x390000e4, /*  14, -1.0dB */
+	0x35c000d7, /*  15, -1.5dB */
+	0x32c000cb, /*  16, -2.0dB */
+	0x300000c0, /*  17, -2.5dB */
+	0x2d4000b5, /*  18, -3.0dB */
+	0x2ac000ab, /*  19, -3.5dB */
+	0x288000a2, /*  20, -4.0dB */
+	0x26000098, /*  21, -4.5dB */
+	0x24000090, /*  22, -5.0dB */
+	0x22000088, /*  23, -5.5dB */
+	0x20000080, /*  24, -6.0dB */
+	0x1e400079, /*  25, -6.5dB */
+	0x1c800072, /*  26, -7.0dB */
+	0x1b00006c, /*  27. -7.5dB */
+	0x19800066, /*  28, -8.0dB */
+	0x18000060, /*  29, -8.5dB */
+	0x16c0005b, /*  30, -9.0dB */
+	0x15800056, /*  31, -9.5dB */
+	0x14400051, /*  32, -10.0dB */
+	0x1300004c, /*  33, -10.5dB */
+	0x12000048, /*  34, -11.0dB */
+	0x11000044, /*  35, -11.5dB */
+	0x10000040, /*  36, -12.0dB */
+	0x0f00003c,/*  37, -12.5dB */
+	0x0e400039,/*  38, -13.0dB */
+	0x0d800036,/*  39, -13.5dB */
+	0x0cc00033,/*  40, -14.0dB */
+	0x0c000030,/*  41, -14.5dB */
+	0x0b40002d,/*  42, -15.0dB */
+};
+
+u8 cck_swing_table[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}, /*  0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /*  1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /*  2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /*  3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /*  4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /*  5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /*  6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /*  7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /*  8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /*  9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /*  10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /*  11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /*  12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /*  13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /*  14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /*  15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /*  16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /*  17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /*  18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*  23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*  24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*  25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*  26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*  31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}	/*  32, -16.0dB */
+};
+
+#define		RxDefaultAnt1		0x65a9
+#define	RxDefaultAnt2		0x569a
+
+static void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
+{
+	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct adapter *adapter = pDM_Odm->Adapter;
+
+	pDM_DigTable->CurIGValue = (u8)rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
+	pDM_DigTable->rx_gain_range_max = DM_DIG_MAX_NIC;
+	pDM_DigTable->rx_gain_range_min = DM_DIG_MIN_NIC;
+	pDM_DigTable->CurCCK_CCAThres = 0x83;
+	pDM_DigTable->ForbiddenIGI = DM_DIG_MIN_NIC;
+	pDM_DigTable->LargeFAHit = 0;
+	pDM_DigTable->Recover_cnt = 0;
+	pDM_DigTable->DIG_Dynamic_MIN_0 = DM_DIG_MIN_NIC;
+	pDM_DigTable->bMediaConnect_0 = false;
+
+	/* To Initialize pDM_Odm->bDMInitialGainEnable == false to avoid DIG error */
+	pDM_Odm->bDMInitialGainEnable = true;
+}
+
+static void odm_DIG(struct odm_dm_struct *pDM_Odm)
+{
+	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct false_alarm_stats *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	u8 DIG_Dynamic_MIN;
+	u8 DIG_MaxOfMin;
+	bool FirstConnect, FirstDisConnect;
+	u8 dm_dig_max, dm_dig_min;
+	u8 CurrentIGI = pDM_DigTable->CurIGValue;
+
+	if (*pDM_Odm->pbScanInProcess)
+		return;
+
+	/* add by Neil Chen to avoid PSD is processing */
+	if (!pDM_Odm->bDMInitialGainEnable)
+		return;
+
+	DIG_Dynamic_MIN = pDM_DigTable->DIG_Dynamic_MIN_0;
+	FirstConnect = (pDM_Odm->bLinked) && (!pDM_DigTable->bMediaConnect_0);
+	FirstDisConnect = (!pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0);
+
+	/* 1 Boundary Decision */
+	dm_dig_max = DM_DIG_MAX_NIC;
+	dm_dig_min = DM_DIG_MIN_NIC;
+	DIG_MaxOfMin = DM_DIG_MAX_AP;
+
+	if (pDM_Odm->bLinked) {
+		/* 2 8723A Series, offset need to be 10 */
+		/* 2 Modify DIG upper bound */
+		if ((pDM_Odm->RSSI_Min + 20) > dm_dig_max)
+			pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		else if ((pDM_Odm->RSSI_Min + 20) < dm_dig_min)
+			pDM_DigTable->rx_gain_range_max = dm_dig_min;
+		else
+			pDM_DigTable->rx_gain_range_max = pDM_Odm->RSSI_Min + 20;
+		/* 2 Modify DIG lower bound */
+		if (pDM_Odm->bOneEntryOnly) {
+			if (pDM_Odm->RSSI_Min < dm_dig_min)
+				DIG_Dynamic_MIN = dm_dig_min;
+			else if (pDM_Odm->RSSI_Min > DIG_MaxOfMin)
+				DIG_Dynamic_MIN = DIG_MaxOfMin;
+			else
+				DIG_Dynamic_MIN = pDM_Odm->RSSI_Min;
+		} else if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
+			/* 1 Lower Bound for 88E AntDiv */
+			if (pDM_Odm->AntDivType == CG_TRX_HW_ANTDIV)
+				DIG_Dynamic_MIN = (u8)pDM_DigTable->AntDiv_RSSI_max;
+		} else {
+			DIG_Dynamic_MIN = dm_dig_min;
+		}
+	} else {
+		pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		DIG_Dynamic_MIN = dm_dig_min;
+	}
+
+	/* 1 Modify DIG lower bound, deal with abnormally large false alarm */
+	if (pFalseAlmCnt->Cnt_all > 10000) {
+		if (pDM_DigTable->LargeFAHit != 3)
+			pDM_DigTable->LargeFAHit++;
+		if (pDM_DigTable->ForbiddenIGI < CurrentIGI) {
+			pDM_DigTable->ForbiddenIGI = CurrentIGI;
+			pDM_DigTable->LargeFAHit = 1;
+		}
+
+		if (pDM_DigTable->LargeFAHit >= 3) {
+			if ((pDM_DigTable->ForbiddenIGI + 1) > pDM_DigTable->rx_gain_range_max)
+				pDM_DigTable->rx_gain_range_min = pDM_DigTable->rx_gain_range_max;
+			else
+				pDM_DigTable->rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 1);
+			pDM_DigTable->Recover_cnt = 3600; /* 3600=2hr */
+		}
+
+	} else {
+		/* Recovery mechanism for IGI lower bound */
+		if (pDM_DigTable->Recover_cnt != 0) {
+			pDM_DigTable->Recover_cnt--;
+		} else {
+			if (pDM_DigTable->LargeFAHit < 3) {
+				if ((pDM_DigTable->ForbiddenIGI - 1) < DIG_Dynamic_MIN) { /* DM_DIG_MIN) */
+					pDM_DigTable->ForbiddenIGI = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+					pDM_DigTable->rx_gain_range_min = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+				} else {
+					pDM_DigTable->ForbiddenIGI--;
+					pDM_DigTable->rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 1);
+				}
+			} else {
+				pDM_DigTable->LargeFAHit = 0;
+			}
+		}
+	}
+
+	/* 1 Adjust initial gain by false alarm */
+	if (pDM_Odm->bLinked) {
+		if (FirstConnect) {
+			CurrentIGI = pDM_Odm->RSSI_Min;
+		} else {
+			if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2)
+				CurrentIGI = CurrentIGI + 4;/* pDM_DigTable->CurIGValue = pDM_DigTable->PreIGValue+2; */
+			else if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH1)
+				CurrentIGI = CurrentIGI + 2;/* pDM_DigTable->CurIGValue = pDM_DigTable->PreIGValue+1; */
+			else if (pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH0)
+				CurrentIGI = CurrentIGI - 2;/* pDM_DigTable->CurIGValue =pDM_DigTable->PreIGValue-1; */
+		}
+	} else {
+		if (FirstDisConnect) {
+			CurrentIGI = pDM_DigTable->rx_gain_range_min;
+		} else {
+			/* 2012.03.30 LukeLee: enable DIG before link but with very high thresholds */
+			if (pFalseAlmCnt->Cnt_all > 10000)
+				CurrentIGI = CurrentIGI + 2;/* pDM_DigTable->CurIGValue = pDM_DigTable->PreIGValue+2; */
+			else if (pFalseAlmCnt->Cnt_all > 8000)
+				CurrentIGI = CurrentIGI + 1;/* pDM_DigTable->CurIGValue = pDM_DigTable->PreIGValue+1; */
+			else if (pFalseAlmCnt->Cnt_all < 500)
+				CurrentIGI = CurrentIGI - 1;/* pDM_DigTable->CurIGValue =pDM_DigTable->PreIGValue-1; */
+		}
+	}
+	/* 1 Check initial gain by upper/lower bound */
+	if (CurrentIGI > pDM_DigTable->rx_gain_range_max)
+		CurrentIGI = pDM_DigTable->rx_gain_range_max;
+	if (CurrentIGI < pDM_DigTable->rx_gain_range_min)
+		CurrentIGI = pDM_DigTable->rx_gain_range_min;
+
+	/* 2 High power RSSI threshold */
+
+	ODM_Write_DIG(pDM_Odm, CurrentIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+	pDM_DigTable->bMediaConnect_0 = pDM_Odm->bLinked;
+	pDM_DigTable->DIG_Dynamic_MIN_0 = DIG_Dynamic_MIN;
+}
+
+static void odm_CommonInfoSelfInit(struct odm_dm_struct *pDM_Odm)
+{
+	struct adapter *adapter = pDM_Odm->Adapter;
+
+	pDM_Odm->bCckHighPower = (bool)rtl8188e_PHY_QueryBBReg(adapter, 0x824, BIT(9));
+	pDM_Odm->RFPathRxEnable = (u8)rtl8188e_PHY_QueryBBReg(adapter, 0xc04, 0x0F);
+}
+
+static void odm_CommonInfoSelfUpdate(struct odm_dm_struct *pDM_Odm)
+{
+	u8 EntryCnt = 0;
+	u8 i;
+	struct sta_info *pEntry;
+
+	if (*pDM_Odm->pBandWidth == HT_CHANNEL_WIDTH_40) {
+		if (*pDM_Odm->pSecChOffset == 1)
+			pDM_Odm->ControlChannel = *pDM_Odm->pChannel - 2;
+		else if (*pDM_Odm->pSecChOffset == 2)
+			pDM_Odm->ControlChannel = *pDM_Odm->pChannel + 2;
+	} else {
+		pDM_Odm->ControlChannel = *pDM_Odm->pChannel;
+	}
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pEntry = pDM_Odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(pEntry))
+			EntryCnt++;
+	}
+	if (EntryCnt == 1)
+		pDM_Odm->bOneEntryOnly = true;
+	else
+		pDM_Odm->bOneEntryOnly = false;
+}
+
+static void odm_RateAdaptiveMaskInit(struct odm_dm_struct *pDM_Odm)
+{
+	struct odm_rate_adapt *pOdmRA = &pDM_Odm->RateAdaptive;
+
+	pOdmRA->RATRState = DM_RATR_STA_INIT;
+	pOdmRA->HighRSSIThresh = 50;
+	pOdmRA->LowRSSIThresh = 20;
+}
+
+static void odm_RefreshRateAdaptiveMask(struct odm_dm_struct *pDM_Odm)
+{
+	u8 i;
+	struct adapter *pAdapter = pDM_Odm->Adapter;
+
+	if (pAdapter->bDriverStopped)
+		return;
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		struct sta_info *pstat = pDM_Odm->pODM_StaInfo[i];
+
+		if (IS_STA_VALID(pstat)) {
+			if (ODM_RAStateCheck(pDM_Odm, pstat->rssi_stat.UndecoratedSmoothedPWDB, false, &pstat->rssi_level))
+				rtw_hal_update_ra_mask(pAdapter, i, pstat->rssi_level);
+		}
+	}
+}
+
+static void odm_DynamicBBPowerSavingInit(struct odm_dm_struct *pDM_Odm)
+{
+	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
+
+	pDM_PSTable->pre_rf_state = RF_MAX;
+	pDM_PSTable->cur_rf_state = RF_MAX;
+	pDM_PSTable->initialize = 0;
+}
+
+static void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
+{
+	u32 ret_value;
+	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	struct adapter *adapter = pDM_Odm->Adapter;
+
+	/* hold ofdm counter */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1); /* hold page C counter */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 1); /* hold page D counter */
+
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value & 0xffff);
+	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value & 0xffff0000) >> 16);
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value & 0xffff);
+	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value & 0xffff0000) >> 16);
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value & 0xffff);
+	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value & 0xffff0000) >> 16);
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_Mcs_fail = (ret_value & 0xffff);
+
+	FalseAlmCnt->Cnt_Ofdm_fail = FalseAlmCnt->Cnt_Parity_Fail + FalseAlmCnt->Cnt_Rate_Illegal +
+				     FalseAlmCnt->Cnt_Crc8_fail + FalseAlmCnt->Cnt_Mcs_fail +
+				     FalseAlmCnt->Cnt_Fast_Fsync + FalseAlmCnt->Cnt_SB_Search_fail;
+
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_SC_CNT_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_BW_LSC = (ret_value & 0xffff);
+	FalseAlmCnt->Cnt_BW_USC = ((ret_value & 0xffff0000) >> 16);
+
+	/* hold cck counter */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_FA_RST_11N, BIT(12), 1);
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_FA_RST_11N, BIT(14), 1);
+
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
+	FalseAlmCnt->Cnt_Cck_fail = ret_value;
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_CCK_FA_MSB_11N, bMaskByte3);
+	FalseAlmCnt->Cnt_Cck_fail +=  (ret_value & 0xff) << 8;
+
+	ret_value = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_CCK_CCA = ((ret_value & 0xFF) << 8) | ((ret_value & 0xFF00) >> 8);
+
+	FalseAlmCnt->Cnt_all = (FalseAlmCnt->Cnt_Fast_Fsync +
+				FalseAlmCnt->Cnt_SB_Search_fail +
+				FalseAlmCnt->Cnt_Parity_Fail +
+				FalseAlmCnt->Cnt_Rate_Illegal +
+				FalseAlmCnt->Cnt_Crc8_fail +
+				FalseAlmCnt->Cnt_Mcs_fail +
+				FalseAlmCnt->Cnt_Cck_fail);
+
+	FalseAlmCnt->Cnt_CCA_all = FalseAlmCnt->Cnt_OFDM_CCA + FalseAlmCnt->Cnt_CCK_CCA;
+}
+
+static void odm_CCKPacketDetectionThresh(struct odm_dm_struct *pDM_Odm)
+{
+	u8 CurCCK_CCAThres;
+	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+
+	if (pDM_Odm->bLinked) {
+		if (pDM_Odm->RSSI_Min > 25) {
+			CurCCK_CCAThres = 0xcd;
+		} else if ((pDM_Odm->RSSI_Min <= 25) && (pDM_Odm->RSSI_Min > 10)) {
+			CurCCK_CCAThres = 0x83;
+		} else {
+			if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+				CurCCK_CCAThres = 0x83;
+			else
+				CurCCK_CCAThres = 0x40;
+		}
+	} else {
+		if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+			CurCCK_CCAThres = 0x83;
+		else
+			CurCCK_CCAThres = 0x40;
+	}
+	ODM_Write_CCK_CCA_Thres(pDM_Odm, CurCCK_CCAThres);
+}
+
+static void FindMinimumRSSI(struct adapter *pAdapter)
+{
+	struct hal_data_8188e *pHalData = &pAdapter->haldata;
+	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
+	struct mlme_priv	*pmlmepriv = &pAdapter->mlmepriv;
+
+	/* 1 1.Determine the minimum RSSI */
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    pdmpriv->EntryMinUndecoratedSmoothedPWDB == 0)
+		pdmpriv->MinUndecoratedPWDBForDM = 0;
+
+	pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+}
+
+static void odm_RSSIMonitorCheck(struct odm_dm_struct *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
+	int	i;
+	int	tmpEntryMaxPWDB = 0, tmpEntryMinPWDB = 0xff;
+	u8	sta_cnt = 0;
+	u32 PWDB_rssi[NUM_STA] = {0};/* 0~15]:MACID, [16~31]:PWDB_rssi */
+	struct sta_info *psta;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		return;
+
+	if (!check_fwstate(&Adapter->mlmepriv, _FW_LINKED))
+		return;
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		psta = pDM_Odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(psta) &&
+		    (psta->state & WIFI_ASOC_STATE) &&
+		    !is_broadcast_ether_addr(psta->hwaddr) &&
+		    memcmp(psta->hwaddr, myid(&Adapter->eeprompriv), ETH_ALEN)) {
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB < tmpEntryMinPWDB)
+				tmpEntryMinPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB > tmpEntryMaxPWDB)
+				tmpEntryMaxPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB != (-1))
+				PWDB_rssi[sta_cnt++] = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB << 16));
+		}
+	}
+
+	for (i = 0; i < sta_cnt; i++) {
+		if (PWDB_rssi[i] != (0)) {
+			if (pHalData->fw_ractrl) {
+				/*  Report every sta's RSSI to FW */
+			} else {
+				ODM_RA_SetRSSI_8188E(
+				&pHalData->odmpriv, (PWDB_rssi[i] & 0xFF), (u8)((PWDB_rssi[i] >> 16) & 0xFF));
+			}
+		}
+	}
+
+	if (tmpEntryMinPWDB != 0xff) /*  If associated entry is found */
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = tmpEntryMinPWDB;
+	else
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = 0;
+
+	FindMinimumRSSI(Adapter);
+	pHalData->odmpriv.RSSI_Min = pdmpriv->MinUndecoratedPWDBForDM;
+}
+
+static void odm_TXPowerTrackingThermalMeterInit(struct odm_dm_struct *pDM_Odm)
+{
+	pDM_Odm->RFCalibrateInfo.TxPowerTrackControl = true;
+}
+
+static void odm_InitHybridAntDiv(struct odm_dm_struct *pDM_Odm)
+{
+	if (!(pDM_Odm->SupportAbility & ODM_BB_ANT_DIV))
+		return;
+
+	ODM_AntennaDiversityInit_88E(pDM_Odm);
+}
+
+static void odm_HwAntDiv(struct odm_dm_struct *pDM_Odm)
+{
+	if (!(pDM_Odm->SupportAbility & ODM_BB_ANT_DIV))
+		return;
+
+	ODM_AntennaDiversity_88E(pDM_Odm);
+}
+
+static void ODM_EdcaTurboInit(struct odm_dm_struct *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+	pDM_Odm->DM_EDCA_Table.bIsCurRDLState = false;
+	Adapter->recvpriv.bIsAnyNonBEPkts = false;
+}
+
+static void odm_EdcaTurboCheck(struct odm_dm_struct *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	u32	trafficIndex;
+	u32	edca_param;
+	u64	cur_tx_bytes = 0;
+	u64	cur_rx_bytes = 0;
+	u8	bbtchange = false;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct xmit_priv		*pxmitpriv = &Adapter->xmitpriv;
+	struct recv_priv		*precvpriv = &Adapter->recvpriv;
+	struct registry_priv	*pregpriv = &Adapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pregpriv->wifi_spec == 1)
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	if (pmlmeinfo->assoc_AP_vendor >=  HT_IOT_PEER_MAX)
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	/*  Check if the status needs to be changed. */
+	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
+		cur_tx_bytes = pxmitpriv->tx_bytes - pxmitpriv->last_tx_bytes;
+		cur_rx_bytes = precvpriv->rx_bytes - precvpriv->last_rx_bytes;
+
+		/* traffic, TX or RX */
+		if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK) ||
+		    (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS)) {
+			if (cur_tx_bytes > (cur_rx_bytes << 2)) {
+				/*  Uplink TP is present. */
+				trafficIndex = UP_LINK;
+			} else {
+				/*  Balance TP is present. */
+				trafficIndex = DOWN_LINK;
+			}
+		} else {
+			if (cur_rx_bytes > (cur_tx_bytes << 2)) {
+				/*  Downlink TP is present. */
+				trafficIndex = DOWN_LINK;
+			} else {
+				/*  Balance TP is present. */
+				trafficIndex = UP_LINK;
+			}
+		}
+
+		if ((pDM_Odm->DM_EDCA_Table.prv_traffic_idx != trafficIndex) || (!pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA)) {
+			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_CISCO) && (pmlmeext->cur_wireless_mode & WIRELESS_11_24N))
+				edca_param = EDCAParam[pmlmeinfo->assoc_AP_vendor][trafficIndex];
+			else
+				edca_param = EDCAParam[HT_IOT_PEER_UNKNOWN][trafficIndex];
+
+			rtw_write32(Adapter, REG_EDCA_BE_PARAM, edca_param);
+
+			pDM_Odm->DM_EDCA_Table.prv_traffic_idx = trafficIndex;
+		}
+
+		pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = true;
+	} else {
+		/*  Turn Off EDCA turbo here. */
+		/*  Restore original EDCA according to the declaration of AP. */
+		if (pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA) {
+			rtw_write32(Adapter, REG_EDCA_BE_PARAM, pHalData->AcParam_BE);
+			pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+		}
+	}
+
+dm_CheckEdcaTurbo_EXIT:
+	/*  Set variables for next time. */
+	precvpriv->bIsAnyNonBEPkts = false;
+	pxmitpriv->last_tx_bytes = pxmitpriv->tx_bytes;
+	precvpriv->last_rx_bytes = precvpriv->rx_bytes;
+}
+
+/*  2011/09/21 MH Add to describe different team necessary resource allocate?? */
+void ODM_DMInit(struct odm_dm_struct *pDM_Odm)
+{
+	/* 2012.05.03 Luke: For all IC series */
+	odm_CommonInfoSelfInit(pDM_Odm);
+	odm_DIGInit(pDM_Odm);
+	odm_RateAdaptiveMaskInit(pDM_Odm);
+
+	odm_DynamicBBPowerSavingInit(pDM_Odm);
+	odm_TXPowerTrackingThermalMeterInit(pDM_Odm);
+	ODM_EdcaTurboInit(pDM_Odm);
+	ODM_RAInfo_Init_all(pDM_Odm);
+	if ((pDM_Odm->AntDivType == CG_TRX_HW_ANTDIV)	||
+	    (pDM_Odm->AntDivType == CGCS_RX_HW_ANTDIV) ||
+	    (pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV))
+		odm_InitHybridAntDiv(pDM_Odm);
+}
+
+/*  2011/09/20 MH This is the entry pointer for all team to execute HW out source DM. */
+/*  You can not add any dummy function here, be care, you can only use DM structure */
+/*  to perform any new ODM_DM. */
+void ODM_DMWatchdog(struct odm_dm_struct *pDM_Odm)
+{
+	/* 2012.05.03 Luke: For all IC series */
+	odm_CommonInfoSelfUpdate(pDM_Odm);
+	odm_FalseAlarmCounterStatistics(pDM_Odm);
+	odm_RSSIMonitorCheck(pDM_Odm);
+
+	odm_DIG(pDM_Odm);
+	odm_CCKPacketDetectionThresh(pDM_Odm);
+
+	if (*pDM_Odm->pbPowerSaving)
+		return;
+
+	odm_RefreshRateAdaptiveMask(pDM_Odm);
+
+	if ((pDM_Odm->AntDivType ==  CG_TRX_HW_ANTDIV)	||
+	    (pDM_Odm->AntDivType == CGCS_RX_HW_ANTDIV)	||
+	    (pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV))
+		odm_HwAntDiv(pDM_Odm);
+
+	ODM_TXPowerTrackingCheck(pDM_Odm);
+	odm_EdcaTurboCheck(pDM_Odm);
+}
+
+/*  Init /.. Fixed HW value. Only init time. */
+void ODM_CmnInfoInit(struct odm_dm_struct *pDM_Odm, enum odm_common_info_def CmnInfo, u32 Value)
+{
+	/*  This section is used for init value */
+	switch	(CmnInfo) {
+	/*  Fixed ODM value. */
+	case	ODM_CMNINFO_MP_TEST_CHIP:
+		pDM_Odm->bIsMPChip = (u8)Value;
+		break;
+	case    ODM_CMNINFO_RF_ANTENNA_TYPE:
+		pDM_Odm->AntDivType = (u8)Value;
+		break;
+	default:
+		/* do nothing */
+		break;
+	}
+
+	/*  Tx power tracking BB swing table. */
+	/*  The base index = 12. +((12-n)/2)dB 13~?? = decrease tx pwr by -((n-12)/2)dB */
+	pDM_Odm->BbSwingIdxOfdm			= 12; /*  Set defalut value as index 12. */
+	pDM_Odm->BbSwingIdxOfdmCurrent	= 12;
+	pDM_Odm->BbSwingFlagOfdm		= false;
+}
+
+void ODM_Write_DIG(struct odm_dm_struct *pDM_Odm, u8 CurrentIGI)
+{
+	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct adapter *adapter = pDM_Odm->Adapter;
+
+	if (pDM_DigTable->CurIGValue != CurrentIGI) {
+		rtl8188e_PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N, CurrentIGI);
+		pDM_DigTable->CurIGValue = CurrentIGI;
+	}
+}
+
+void ODM_Write_CCK_CCA_Thres(struct odm_dm_struct *pDM_Odm, u8 CurCCK_CCAThres)
+{
+	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)		/* modify by Guo.Mingzhi 2012-01-03 */
+		rtw_write8(pDM_Odm->Adapter, ODM_REG_CCK_CCA_11N, CurCCK_CCAThres);
+	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
+}
+
+void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal)
+{
+	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
+	struct adapter *adapter = pDM_Odm->Adapter;
+	u8 Rssi_Up_bound = 30;
+	u8 Rssi_Low_bound = 25;
+
+	if (pDM_PSTable->initialize == 0) {
+		pDM_PSTable->reg_874 = (rtl8188e_PHY_QueryBBReg(adapter, 0x874, bMaskDWord) & 0x1CC000) >> 14;
+		pDM_PSTable->reg_c70 = (rtl8188e_PHY_QueryBBReg(adapter, 0xc70, bMaskDWord) & BIT(3)) >> 3;
+		pDM_PSTable->reg_85c = (rtl8188e_PHY_QueryBBReg(adapter, 0x85c, bMaskDWord) & 0xFF000000) >> 24;
+		pDM_PSTable->reg_a74 = (rtl8188e_PHY_QueryBBReg(adapter, 0xa74, bMaskDWord) & 0xF000) >> 12;
+		pDM_PSTable->initialize = 1;
+	}
+
+	if (!bForceInNormal) {
+		if (pDM_Odm->RSSI_Min != 0xFF) {
+			if (pDM_PSTable->pre_rf_state == RF_Normal) {
+				if (pDM_Odm->RSSI_Min >= Rssi_Up_bound)
+					pDM_PSTable->cur_rf_state = RF_Save;
+				else
+					pDM_PSTable->cur_rf_state = RF_Normal;
+			} else {
+				if (pDM_Odm->RSSI_Min <= Rssi_Low_bound)
+					pDM_PSTable->cur_rf_state = RF_Normal;
+				else
+					pDM_PSTable->cur_rf_state = RF_Save;
+			}
+		} else {
+			pDM_PSTable->cur_rf_state = RF_MAX;
+		}
+	} else {
+		pDM_PSTable->cur_rf_state = RF_Normal;
+	}
+
+	if (pDM_PSTable->pre_rf_state != pDM_PSTable->cur_rf_state) {
+		if (pDM_PSTable->cur_rf_state == RF_Save) {
+			rtl8188e_PHY_SetBBReg(adapter, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]=3'b010 */
+			rtl8188e_PHY_SetBBReg(adapter, 0xc70, BIT(3), 0); /* RegC70[3]=1'b0 */
+			rtl8188e_PHY_SetBBReg(adapter, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]=0x63 */
+			rtl8188e_PHY_SetBBReg(adapter, 0x874, 0xC000, 0x2); /* Reg874[15:14]=2'b10 */
+			rtl8188e_PHY_SetBBReg(adapter, 0xa74, 0xF000, 0x3); /* RegA75[7:4]=0x3 */
+			rtl8188e_PHY_SetBBReg(adapter, 0x818, BIT(28), 0x0); /* Reg818[28]=1'b0 */
+			rtl8188e_PHY_SetBBReg(adapter, 0x818, BIT(28), 0x1); /* Reg818[28]=1'b1 */
+		} else {
+			rtl8188e_PHY_SetBBReg(adapter, 0x874, 0x1CC000, pDM_PSTable->reg_874);
+			rtl8188e_PHY_SetBBReg(adapter, 0xc70, BIT(3), pDM_PSTable->reg_c70);
+			rtl8188e_PHY_SetBBReg(adapter, 0x85c, 0xFF000000, pDM_PSTable->reg_85c);
+			rtl8188e_PHY_SetBBReg(adapter, 0xa74, 0xF000, pDM_PSTable->reg_a74);
+			rtl8188e_PHY_SetBBReg(adapter, 0x818, BIT(28), 0x0);
+		}
+		pDM_PSTable->pre_rf_state = pDM_PSTable->cur_rf_state;
+	}
+}
+
+u32 ODM_Get_Rate_Bitmap(struct odm_dm_struct *pDM_Odm, u32 macid, u32 ra_mask, u8 rssi_level)
+{
+	struct sta_info *pEntry;
+	u32 rate_bitmap = 0x0fffffff;
+	u8 WirelessMode;
+
+	pEntry = pDM_Odm->pODM_StaInfo[macid];
+	if (!IS_STA_VALID(pEntry))
+		return ra_mask;
+
+	WirelessMode = pEntry->wireless_mode;
+
+	switch (WirelessMode) {
+	case ODM_WM_B:
+		if (ra_mask & 0x0000000c)		/* 11M or 5.5M enable */
+			rate_bitmap = 0x0000000d;
+		else
+			rate_bitmap = 0x0000000f;
+		break;
+	case (ODM_WM_B | ODM_WM_G):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x00000f00;
+		else if (rssi_level == DM_RATR_STA_MIDDLE)
+			rate_bitmap = 0x00000ff0;
+		else
+			rate_bitmap = 0x00000ff5;
+		break;
+	case (ODM_WM_B | ODM_WM_G | ODM_WM_N24G):
+		if (rssi_level == DM_RATR_STA_HIGH) {
+			rate_bitmap = 0x000f0000;
+		} else if (rssi_level == DM_RATR_STA_MIDDLE) {
+			rate_bitmap = 0x000ff000;
+		} else {
+			if (*pDM_Odm->pBandWidth == HT_CHANNEL_WIDTH_40)
+				rate_bitmap = 0x000ff015;
+			else
+				rate_bitmap = 0x000ff005;
+		}
+		break;
+	default:
+		/* case WIRELESS_11_24N: */
+		rate_bitmap = 0x0fffffff;
+		break;
+	}
+
+	return rate_bitmap;
+}
+
+/*  Return Value: bool */
+/*  - true: RATRState is changed. */
+bool ODM_RAStateCheck(struct odm_dm_struct *pDM_Odm, s32 RSSI, bool bForceUpdate, u8 *pRATRState)
+{
+	struct odm_rate_adapt *pRA = &pDM_Odm->RateAdaptive;
+	const u8 GoUpGap = 5;
+	u8 HighRSSIThreshForRA = pRA->HighRSSIThresh;
+	u8 LowRSSIThreshForRA = pRA->LowRSSIThresh;
+	u8 RATRState;
+
+	/*  Threshold Adjustment: */
+	/*  when RSSI state trends to go up one or two levels, make sure RSSI is high enough. */
+	/*  Here GoUpGap is added to solve the boundary's level alternation issue. */
+	switch (*pRATRState) {
+	case DM_RATR_STA_INIT:
+	case DM_RATR_STA_HIGH:
+		break;
+	case DM_RATR_STA_MIDDLE:
+		HighRSSIThreshForRA += GoUpGap;
+		break;
+	case DM_RATR_STA_LOW:
+		HighRSSIThreshForRA += GoUpGap;
+		LowRSSIThreshForRA += GoUpGap;
+		break;
+	default:
+		break;
+	}
+
+	/*  Decide RATRState by RSSI. */
+	if (RSSI > HighRSSIThreshForRA)
+		RATRState = DM_RATR_STA_HIGH;
+	else if (RSSI > LowRSSIThreshForRA)
+		RATRState = DM_RATR_STA_MIDDLE;
+	else
+		RATRState = DM_RATR_STA_LOW;
+
+	if (*pRATRState != RATRState || bForceUpdate) {
+		*pRATRState = RATRState;
+		return true;
+	}
+	return false;
+}
+
+void ODM_TXPowerTrackingCheck(struct odm_dm_struct *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+
+	if (!pDM_Odm->RFCalibrateInfo.TM_Trigger) {		/* at least delay 1 sec */
+		rtl8188e_PHY_SetRFReg(Adapter, RF_T_METER_88E, BIT(17) | BIT(16), 0x03);
+
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 1;
+		return;
+	} else {
+		odm_TXPowerTrackingCallback_ThermalMeter_8188E(Adapter);
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 0;
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/odm_HWConfig.c b/drivers/staging/r8188eu/hal/odm_HWConfig.c
new file mode 100644
index 000000000..38f357e8a
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/odm_HWConfig.c
@@ -0,0 +1,349 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+
+static u8 odm_query_rxpwrpercentage(s8 antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return 0;
+	else if (antpower >= 0)
+		return 100;
+	else
+		return 100 + antpower;
+}
+
+static s32 odm_signal_scale_mapping(struct odm_dm_struct *dm_odm, s32 currsig)
+{
+	s32 retsig;
+
+	if (currsig >= 51 && currsig <= 100)
+		retsig = 100;
+	else if (currsig >= 41 && currsig <= 50)
+		retsig = 80 + ((currsig - 40) * 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 10 && currsig <= 20)
+		retsig = 42 + (((currsig - 10) * 2) / 3);
+	else if (currsig >= 5 && currsig <= 9)
+		retsig = 22 + (((currsig - 5) * 3) / 2);
+	else if (currsig >= 1 && currsig <= 4)
+		retsig = 6 + (((currsig - 1) * 3) / 2);
+	else
+		retsig = currsig;
+
+	return retsig;
+}
+
+static u8 odm_evm_db_to_percentage(s8 value)
+{
+	/*  -33dB~0dB to 0%~99% */
+	s8 ret_val = clamp(-value, 0, 33) * 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+static void odm_RxPhyStatus92CSeries_Parsing(struct odm_dm_struct *dm_odm,
+			struct phy_info *pPhyInfo,
+			u8 *pPhyStatus,
+			struct odm_per_pkt_info *pPktinfo,
+			struct adapter *adapt)
+{
+	u8 i, Max_spatial_stream;
+	s8 rx_pwr[4], rx_pwr_all = 0;
+	u8 EVM, PWDB_ALL = 0;
+	u8 RSSI, total_rssi = 0;
+	u8 isCCKrate = 0;
+	u8 rf_rx_num = 0;
+	u8 cck_highpwr = 0;
+	u8 LNA_idx, VGA_idx;
+
+	struct phy_status_rpt *pPhyStaRpt = (struct phy_status_rpt *)pPhyStatus;
+
+	isCCKrate = pPktinfo->Rate >= DESC92C_RATE1M && pPktinfo->Rate <= DESC92C_RATE11M;
+
+	if (isCCKrate) {
+		u8 cck_agc_rpt;
+
+		/*  (1)Hardware does not provide RSSI for CCK */
+		/*  (2)PWDB, Average PWDB calculated by hardware (for rate adaptive) */
+
+		cck_highpwr = dm_odm->bCckHighPower;
+
+		cck_agc_rpt =  pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a;
+
+		/* 2011.11.28 LukeLee: 88E use different LNA & VGA gain table */
+		/* The RSSI formula should be modified according to the gain table */
+		/* In 88E, cck_highpwr is always set to 1 */
+		LNA_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		VGA_idx = (cck_agc_rpt & 0x1F);
+		switch (LNA_idx) {
+		case 7:
+			if (VGA_idx <= 27)
+				rx_pwr_all = -100 + 2 * (27 - VGA_idx); /* VGA_idx = 27~2 */
+			else
+				rx_pwr_all = -100;
+			break;
+		case 6:
+			rx_pwr_all = -48 + 2 * (2 - VGA_idx); /* VGA_idx = 2~0 */
+			break;
+		case 5:
+			rx_pwr_all = -42 + 2 * (7 - VGA_idx); /* VGA_idx = 7~5 */
+			break;
+		case 4:
+			rx_pwr_all = -36 + 2 * (7 - VGA_idx); /* VGA_idx = 7~4 */
+			break;
+		case 3:
+			rx_pwr_all = -24 + 2 * (7 - VGA_idx); /* VGA_idx = 7~0 */
+			break;
+		case 2:
+			if (cck_highpwr)
+				rx_pwr_all = -12 + 2 * (5 - VGA_idx); /* VGA_idx = 5~0 */
+			else
+				rx_pwr_all = -6 + 2 * (5 - VGA_idx);
+			break;
+		case 1:
+				rx_pwr_all = 8 - 2 * VGA_idx;
+			break;
+		case 0:
+				rx_pwr_all = 14 - 2 * VGA_idx;
+			break;
+		default:
+			break;
+		}
+		rx_pwr_all += 6;
+		PWDB_ALL = odm_query_rxpwrpercentage(rx_pwr_all);
+		if (!cck_highpwr) {
+			if (PWDB_ALL >= 80)
+				PWDB_ALL = ((PWDB_ALL - 80) << 1) + ((PWDB_ALL - 80) >> 1) + 80;
+			else if ((PWDB_ALL <= 78) && (PWDB_ALL >= 20))
+				PWDB_ALL += 3;
+			if (PWDB_ALL > 100)
+				PWDB_ALL = 100;
+		}
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		pPhyInfo->recvpower = rx_pwr_all;
+		/*  (3) Get Signal Quality (EVM) */
+		if (pPktinfo->bPacketMatchBSSID) {
+			u8 SQ, SQ_rpt;
+
+			if (pPhyInfo->RxPWDBAll > 40) {
+				SQ = 100;
+			} else {
+				SQ_rpt = pPhyStaRpt->cck_sig_qual_ofdm_pwdb_all;
+
+				if (SQ_rpt > 64)
+					SQ = 0;
+				else if (SQ_rpt < 20)
+					SQ = 100;
+				else
+					SQ = ((64 - SQ_rpt) * 100) / 44;
+			}
+			pPhyInfo->SignalQuality = SQ;
+		}
+	} else { /* is OFDM rate */
+		/*  (1)Get RSSI for HT rate */
+
+		for (i = RF_PATH_A; i < RF_PATH_MAX; i++) {
+			/*  2008/01/30 MH we will judge RF RX path now. */
+			if (dm_odm->RFPathRxEnable & BIT(i))
+				rf_rx_num++;
+
+			rx_pwr[i] = ((pPhyStaRpt->path_agc[i].gain & 0x3F) * 2) - 110;
+			if (i == RF_PATH_A)
+				adapt->signal_strength = rx_pwr[i];
+
+			pPhyInfo->RxPwr[i] = rx_pwr[i];
+
+			/* Translate DBM to percentage. */
+			RSSI = odm_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += RSSI;
+
+			pPhyInfo->RxMIMOSignalStrength[i] = (u8)RSSI;
+
+			/* Get Rx snr value in DB */
+			dm_odm->PhyDbgInfo.RxSNRdB[i] = (s32)(pPhyStaRpt->path_rxsnr[i] / 2);
+		}
+		/*  (2)PWDB, Average PWDB calculated by hardware (for rate adaptive) */
+		rx_pwr_all = (((pPhyStaRpt->cck_sig_qual_ofdm_pwdb_all) >> 1) & 0x7f) - 110;
+
+		PWDB_ALL = odm_query_rxpwrpercentage(rx_pwr_all);
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		pPhyInfo->RxPower = rx_pwr_all;
+		pPhyInfo->recvpower = rx_pwr_all;
+
+		/*  (3)EVM of HT rate */
+		if (pPktinfo->Rate >= DESC92C_RATEMCS8 && pPktinfo->Rate <= DESC92C_RATEMCS15)
+			Max_spatial_stream = 2; /* both spatial stream make sense */
+		else
+			Max_spatial_stream = 1; /* only spatial stream 1 makes sense */
+
+		for (i = 0; i < Max_spatial_stream; i++) {
+			/*  Do not use shift operation like "rx_evmX >>= 1" because the compilor of free build environment */
+			/*  fill most significant bit to "zero" when doing shifting operation which may change a negative */
+			/*  value to positive one, then the dbm value (which is supposed to be negative)  is not correct anymore. */
+			EVM = odm_evm_db_to_percentage((pPhyStaRpt->stream_rxevm[i]));	/* dbm */
+
+			if (pPktinfo->bPacketMatchBSSID) {
+				if (i == RF_PATH_A) /*  Fill value in RFD, Get the first spatial stream only */
+					pPhyInfo->SignalQuality = (u8)(EVM & 0xff);
+			}
+		}
+	}
+	/* UI BSS List signal strength(in percentage), make it good looking, from 0~100. */
+	/* It is assigned to the BSS List in GetValueFromBeaconOrProbeRsp(). */
+	if (isCCKrate) {
+		pPhyInfo->SignalStrength = (u8)(odm_signal_scale_mapping(dm_odm, PWDB_ALL));/* PWDB_ALL; */
+	} else {
+		if (rf_rx_num != 0)
+			pPhyInfo->SignalStrength = (u8)(odm_signal_scale_mapping(dm_odm, total_rssi /= rf_rx_num));
+	}
+
+	/* For 88E HW Antenna Diversity */
+	dm_odm->DM_FatTable.antsel_rx_keep_0 = pPhyStaRpt->ant_sel;
+	dm_odm->DM_FatTable.antsel_rx_keep_1 = pPhyStaRpt->ant_sel_b;
+	dm_odm->DM_FatTable.antsel_rx_keep_2 = pPhyStaRpt->antsel_rx_keep_2;
+}
+
+static void odm_Process_RSSIForDM(struct odm_dm_struct *dm_odm,
+				  struct phy_info *pPhyInfo,
+				  struct odm_per_pkt_info *pPktinfo)
+{
+	s32 UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK;
+	s32 UndecoratedSmoothedOFDM, RSSI_Ave;
+	u8 isCCKrate = 0;
+	u8 RSSI_max, RSSI_min, i;
+	u32 OFDM_pkt = 0;
+	u32 Weighting = 0;
+	struct sta_info *pEntry;
+	u8 antsel_tr_mux;
+	struct fast_ant_train *pDM_FatTable = &dm_odm->DM_FatTable;
+
+	if (pPktinfo->StationID == 0xFF)
+		return;
+	pEntry = dm_odm->pODM_StaInfo[pPktinfo->StationID];
+	if (!IS_STA_VALID(pEntry))
+		return;
+	if ((!pPktinfo->bPacketMatchBSSID))
+		return;
+
+	isCCKrate = pPktinfo->Rate >= DESC92C_RATE1M && pPktinfo->Rate <= DESC92C_RATE11M;
+
+	/* Smart Antenna Debug Message------------------  */
+	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) || (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV)) {
+		if (pPktinfo->bPacketToSelf || pPktinfo->bPacketBeacon) {
+			antsel_tr_mux = (pDM_FatTable->antsel_rx_keep_2 << 2) |
+					(pDM_FatTable->antsel_rx_keep_1 << 1) | pDM_FatTable->antsel_rx_keep_0;
+			ODM_AntselStatistics_88E(dm_odm, antsel_tr_mux, pPktinfo->StationID, pPhyInfo->RxPWDBAll);
+		}
+	}
+
+	/* Smart Antenna Debug Message------------------ */
+
+	UndecoratedSmoothedCCK =  pEntry->rssi_stat.UndecoratedSmoothedCCK;
+	UndecoratedSmoothedOFDM = pEntry->rssi_stat.UndecoratedSmoothedOFDM;
+	UndecoratedSmoothedPWDB = pEntry->rssi_stat.UndecoratedSmoothedPWDB;
+
+	if (pPktinfo->bPacketToSelf || pPktinfo->bPacketBeacon) {
+		if (!isCCKrate) { /* ofdm rate */
+			if (pPhyInfo->RxMIMOSignalStrength[RF_PATH_B] == 0) {
+				RSSI_Ave = pPhyInfo->RxMIMOSignalStrength[RF_PATH_A];
+			} else {
+				if (pPhyInfo->RxMIMOSignalStrength[RF_PATH_A] > pPhyInfo->RxMIMOSignalStrength[RF_PATH_B]) {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[RF_PATH_A];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[RF_PATH_B];
+				} else {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[RF_PATH_B];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[RF_PATH_A];
+				}
+				if ((RSSI_max - RSSI_min) < 3)
+					RSSI_Ave = RSSI_max;
+				else if ((RSSI_max - RSSI_min) < 6)
+					RSSI_Ave = RSSI_max - 1;
+				else if ((RSSI_max - RSSI_min) < 10)
+					RSSI_Ave = RSSI_max - 2;
+				else
+					RSSI_Ave = RSSI_max - 3;
+			}
+
+			/* 1 Process OFDM RSSI */
+			if (UndecoratedSmoothedOFDM <= 0) {	/*  initialize */
+				UndecoratedSmoothedOFDM = pPhyInfo->RxPWDBAll;
+			} else {
+				if (pPhyInfo->RxPWDBAll > (u32)UndecoratedSmoothedOFDM) {
+					UndecoratedSmoothedOFDM =
+							(((UndecoratedSmoothedOFDM) * (Rx_Smooth_Factor - 1)) +
+							(RSSI_Ave)) / (Rx_Smooth_Factor);
+					UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM + 1;
+				} else {
+					UndecoratedSmoothedOFDM =
+							(((UndecoratedSmoothedOFDM) * (Rx_Smooth_Factor - 1)) +
+							(RSSI_Ave)) / (Rx_Smooth_Factor);
+				}
+			}
+
+			pEntry->rssi_stat.PacketMap = (pEntry->rssi_stat.PacketMap << 1) | BIT(0);
+
+		} else {
+			RSSI_Ave = pPhyInfo->RxPWDBAll;
+
+			/* 1 Process CCK RSSI */
+			if (UndecoratedSmoothedCCK <= 0) {	/*  initialize */
+				UndecoratedSmoothedCCK = pPhyInfo->RxPWDBAll;
+			} else {
+				if (pPhyInfo->RxPWDBAll > (u32)UndecoratedSmoothedCCK) {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK * (Rx_Smooth_Factor - 1)) +
+							pPhyInfo->RxPWDBAll) / Rx_Smooth_Factor;
+					UndecoratedSmoothedCCK = UndecoratedSmoothedCCK + 1;
+				} else {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK * (Rx_Smooth_Factor - 1)) +
+							pPhyInfo->RxPWDBAll) / Rx_Smooth_Factor;
+				}
+			}
+			pEntry->rssi_stat.PacketMap = pEntry->rssi_stat.PacketMap << 1;
+		}
+		/* 2011.07.28 LukeLee: modified to prevent unstable CCK RSSI */
+		if (pEntry->rssi_stat.ValidBit >= 64)
+			pEntry->rssi_stat.ValidBit = 64;
+		else
+			pEntry->rssi_stat.ValidBit++;
+
+		for (i = 0; i < pEntry->rssi_stat.ValidBit; i++)
+			OFDM_pkt += (u8)(pEntry->rssi_stat.PacketMap >> i) & BIT(0);
+
+		if (pEntry->rssi_stat.ValidBit == 64) {
+			Weighting = ((OFDM_pkt << 4) > 64) ? 64 : (OFDM_pkt << 4);
+			UndecoratedSmoothedPWDB = (Weighting * UndecoratedSmoothedOFDM + (64 - Weighting) * UndecoratedSmoothedCCK) >> 6;
+		} else {
+			if (pEntry->rssi_stat.ValidBit != 0)
+				UndecoratedSmoothedPWDB = (OFDM_pkt * UndecoratedSmoothedOFDM +
+							  (pEntry->rssi_stat.ValidBit - OFDM_pkt) *
+							  UndecoratedSmoothedCCK) / pEntry->rssi_stat.ValidBit;
+			else
+				UndecoratedSmoothedPWDB = 0;
+		}
+		pEntry->rssi_stat.UndecoratedSmoothedCCK = UndecoratedSmoothedCCK;
+		pEntry->rssi_stat.UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM;
+		pEntry->rssi_stat.UndecoratedSmoothedPWDB = UndecoratedSmoothedPWDB;
+	}
+}
+
+/*  Endianness before calling this API */
+void ODM_PhyStatusQuery(struct odm_dm_struct *dm_odm,
+			struct phy_info *pPhyInfo,
+			u8 *pPhyStatus,
+			struct odm_per_pkt_info *pPktinfo,
+			struct adapter *adapt)
+{
+	odm_RxPhyStatus92CSeries_Parsing(dm_odm, pPhyInfo, pPhyStatus, pPktinfo, adapt);
+	odm_Process_RSSIForDM(dm_odm, pPhyInfo, pPktinfo);
+}
diff --git a/drivers/staging/r8188eu/hal/odm_RTL8188E.c b/drivers/staging/r8188eu/hal/odm_RTL8188E.c
new file mode 100644
index 000000000..c8a3c521b
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/odm_RTL8188E.c
@@ -0,0 +1,264 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/drv_types.h"
+
+static void odm_RX_HWAntDivInit(struct odm_dm_struct *dm_odm)
+{
+	struct adapter *adapter = dm_odm->Adapter;
+	u32	value32;
+
+	/* MAC Setting */
+	value32 = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord, value32 | (BIT(23) | BIT(25))); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
+	/* Pin Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0	antsel antselb by HW */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(10), 0);	/* Reg864[10]=1'b0	antsel2 by HW */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT(22), 1);	/* Regb2c[22]=1'b0	disable CS/CG switch */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT(31), 1);	/* Regb2c[31]=1'b1	output at CG only */
+	/* OFDM Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord, 0x000000a0);
+	/* CCK Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT(7), 1); /* Fix CCK PHY status report issue */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT(4), 1); /* CCK complete HW AntDiv within 64 samples */
+	ODM_UpdateRxIdleAnt_88E(dm_odm, MAIN_ANT);
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANT_MAPPING1_11N, 0xFFFF, 0x0201);	/* antenna mapping table */
+}
+
+static void odm_TRX_HWAntDivInit(struct odm_dm_struct *dm_odm)
+{
+	struct adapter *adapter = dm_odm->Adapter;
+	u32	value32;
+
+	/* MAC Setting */
+	value32 = rtl8188e_PHY_QueryBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord, value32 | (BIT(23) | BIT(25))); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
+	/* Pin Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0		antsel antselb by HW */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(10), 0);	/* Reg864[10]=1'b0	antsel2 by HW */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT(22), 0);	/* Regb2c[22]=1'b0	disable CS/CG switch */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT(31), 1);	/* Regb2c[31]=1'b1	output at CG only */
+	/* OFDM Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord, 0x000000a0);
+	/* CCK Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT(7), 1); /* Fix CCK PHY status report issue */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT(4), 1); /* CCK complete HW AntDiv within 64 samples */
+	/* Tx Settings */
+	rtl8188e_PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT(21), 0); /* Reg80c[21]=1'b0		from TX Reg */
+	ODM_UpdateRxIdleAnt_88E(dm_odm, MAIN_ANT);
+
+	/* antenna mapping table */
+	if (!dm_odm->bIsMPChip) { /* testchip */
+		rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_DEFUALT_A_11N, BIT(10) | BIT(9) | BIT(8), 1);	/* Reg858[10:8]=3'b001 */
+		rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_DEFUALT_A_11N, BIT(13) | BIT(12) | BIT(11), 2);	/* Reg858[13:11]=3'b010 */
+	} else { /* MPchip */
+		rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANT_MAPPING1_11N, bMaskDWord, 0x0201);	/* Reg914=3'b010, Reg915=3'b001 */
+	}
+}
+
+static void odm_FastAntTrainingInit(struct odm_dm_struct *dm_odm)
+{
+	struct adapter *adapter = dm_odm->Adapter;
+	u32	value32;
+
+	/* MAC Setting */
+	value32 = rtl8188e_PHY_QueryBBReg(adapter, 0x4c, bMaskDWord);
+	rtl8188e_PHY_SetBBReg(adapter, 0x4c, bMaskDWord, value32 | (BIT(23) | BIT(25))); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
+	value32 = rtl8188e_PHY_QueryBBReg(adapter,  0x7B4, bMaskDWord);
+	rtl8188e_PHY_SetBBReg(adapter, 0x7b4, bMaskDWord, value32 | (BIT(16) | BIT(17))); /* Reg7B4[16]=1 enable antenna training, Reg7B4[17]=1 enable A2 match */
+
+	/* Match MAC ADDR */
+	rtl8188e_PHY_SetBBReg(adapter, 0x7b4, 0xFFFF, 0);
+	rtl8188e_PHY_SetBBReg(adapter, 0x7b0, bMaskDWord, 0);
+
+	rtl8188e_PHY_SetBBReg(adapter, 0x870, BIT(9) | BIT(8), 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0		antsel antselb by HW */
+	rtl8188e_PHY_SetBBReg(adapter, 0x864, BIT(10), 0);	/* Reg864[10]=1'b0	antsel2 by HW */
+	rtl8188e_PHY_SetBBReg(adapter, 0xb2c, BIT(22), 0);	/* Regb2c[22]=1'b0	disable CS/CG switch */
+	rtl8188e_PHY_SetBBReg(adapter, 0xb2c, BIT(31), 1);	/* Regb2c[31]=1'b1	output at CG only */
+	rtl8188e_PHY_SetBBReg(adapter, 0xca4, bMaskDWord, 0x000000a0);
+
+	if (!dm_odm->bIsMPChip) { /* testchip */
+		rtl8188e_PHY_SetBBReg(adapter, 0x858, BIT(10) | BIT(9) | BIT(8), 1);	/* Reg858[10:8]=3'b001 */
+		rtl8188e_PHY_SetBBReg(adapter, 0x858, BIT(13) | BIT(12) | BIT(11), 2);	/* Reg858[13:11]=3'b010 */
+	} else { /* MPchip */
+		rtl8188e_PHY_SetBBReg(adapter, 0x914, bMaskByte0, 1);
+		rtl8188e_PHY_SetBBReg(adapter, 0x914, bMaskByte1, 2);
+	}
+
+	/* Default Ant Setting when no fast training */
+	rtl8188e_PHY_SetBBReg(adapter, 0x80c, BIT(21), 1); /* Reg80c[21]=1'b1		from TX Info */
+	rtl8188e_PHY_SetBBReg(adapter, 0x864, BIT(5) | BIT(4) | BIT(3), 0);	/* Default RX */
+	rtl8188e_PHY_SetBBReg(adapter, 0x864, BIT(8) | BIT(7) | BIT(6), 1);	/* Optional RX */
+
+	/* Enter Training state */
+	rtl8188e_PHY_SetBBReg(adapter, 0x864, BIT(2) | BIT(1) | BIT(0), 1);
+	rtl8188e_PHY_SetBBReg(adapter, 0xc50, BIT(7), 1);	/* RegC50[7]=1'b1		enable HW AntDiv */
+}
+
+void ODM_AntennaDiversityInit_88E(struct odm_dm_struct *dm_odm)
+{
+	if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV)
+		odm_RX_HWAntDivInit(dm_odm);
+	else if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
+		odm_TRX_HWAntDivInit(dm_odm);
+	else if (dm_odm->AntDivType == CG_TRX_SMART_ANTDIV)
+		odm_FastAntTrainingInit(dm_odm);
+}
+
+void ODM_UpdateRxIdleAnt_88E(struct odm_dm_struct *dm_odm, u8 Ant)
+{
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+	struct adapter *adapter = dm_odm->Adapter;
+	u32	DefaultAnt, OptionalAnt;
+
+	if (dm_fat_tbl->RxIdleAnt != Ant) {
+		if (Ant == MAIN_ANT) {
+			DefaultAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			OptionalAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+		} else {
+			DefaultAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+			OptionalAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+		}
+
+		if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(5) | BIT(4) | BIT(3), DefaultAnt);	/* Default RX */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(8) | BIT(7) | BIT(6), OptionalAnt);		/* Optional RX */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_ANTSEL_CTRL_11N, BIT(14) | BIT(13) | BIT(12), DefaultAnt);	/* Default TX */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RESP_TX_11N, BIT(6) | BIT(7), DefaultAnt);	/* Resp Tx */
+		} else if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV) {
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(5) | BIT(4) | BIT(3), DefaultAnt);	/* Default RX */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT(8) | BIT(7) | BIT(6), OptionalAnt);		/* Optional RX */
+		}
+	}
+	dm_fat_tbl->RxIdleAnt = Ant;
+	if (Ant != MAIN_ANT)
+		pr_info("RxIdleAnt=AUX_ANT\n");
+}
+
+static void odm_UpdateTxAnt_88E(struct odm_dm_struct *dm_odm, u8 Ant, u32 MacId)
+{
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+	u8	TargetAnt;
+
+	if (Ant == MAIN_ANT)
+		TargetAnt = MAIN_ANT_CG_TRX;
+	else
+		TargetAnt = AUX_ANT_CG_TRX;
+	dm_fat_tbl->antsel_a[MacId] = TargetAnt & BIT(0);
+	dm_fat_tbl->antsel_b[MacId] = (TargetAnt & BIT(1)) >> 1;
+	dm_fat_tbl->antsel_c[MacId] = (TargetAnt & BIT(2)) >> 2;
+}
+
+void ODM_SetTxAntByTxInfo_88E(struct odm_dm_struct *dm_odm, u8 *pDesc, u8 macId)
+{
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+
+	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) || (dm_odm->AntDivType == CG_TRX_SMART_ANTDIV)) {
+		SET_TX_DESC_ANTSEL_A_88E(pDesc, dm_fat_tbl->antsel_a[macId]);
+		SET_TX_DESC_ANTSEL_B_88E(pDesc, dm_fat_tbl->antsel_b[macId]);
+		SET_TX_DESC_ANTSEL_C_88E(pDesc, dm_fat_tbl->antsel_c[macId]);
+	}
+}
+
+void ODM_AntselStatistics_88E(struct odm_dm_struct *dm_odm, u8 antsel_tr_mux, u32 MacId, u8 RxPWDBAll)
+{
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+	if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
+		if (antsel_tr_mux == MAIN_ANT_CG_TRX) {
+			dm_fat_tbl->MainAnt_Sum[MacId] += RxPWDBAll;
+			dm_fat_tbl->MainAnt_Cnt[MacId]++;
+		} else {
+			dm_fat_tbl->AuxAnt_Sum[MacId] += RxPWDBAll;
+			dm_fat_tbl->AuxAnt_Cnt[MacId]++;
+		}
+	} else if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV) {
+		if (antsel_tr_mux == MAIN_ANT_CGCS_RX) {
+			dm_fat_tbl->MainAnt_Sum[MacId] += RxPWDBAll;
+			dm_fat_tbl->MainAnt_Cnt[MacId]++;
+		} else {
+			dm_fat_tbl->AuxAnt_Sum[MacId] += RxPWDBAll;
+			dm_fat_tbl->AuxAnt_Cnt[MacId]++;
+		}
+	}
+}
+
+static void odm_HWAntDiv(struct odm_dm_struct *dm_odm)
+{
+	u32	i, MinRSSI = 0xFF, AntDivMaxRSSI = 0, MaxRSSI = 0, LocalMinRSSI, LocalMaxRSSI;
+	u32	Main_RSSI, Aux_RSSI;
+	u8	RxIdleAnt = 0, TargetAnt = 7;
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+	struct rtw_dig *pDM_DigTable = &dm_odm->DM_DigTable;
+	struct sta_info *pEntry;
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pEntry = dm_odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(pEntry)) {
+			/* 2 Caculate RSSI per Antenna */
+			Main_RSSI = (dm_fat_tbl->MainAnt_Cnt[i] != 0) ? (dm_fat_tbl->MainAnt_Sum[i] / dm_fat_tbl->MainAnt_Cnt[i]) : 0;
+			Aux_RSSI = (dm_fat_tbl->AuxAnt_Cnt[i] != 0) ? (dm_fat_tbl->AuxAnt_Sum[i] / dm_fat_tbl->AuxAnt_Cnt[i]) : 0;
+			TargetAnt = (Main_RSSI >= Aux_RSSI) ? MAIN_ANT : AUX_ANT;
+			/* 2 Select MaxRSSI for DIG */
+			LocalMaxRSSI = (Main_RSSI > Aux_RSSI) ? Main_RSSI : Aux_RSSI;
+			if ((LocalMaxRSSI > AntDivMaxRSSI) && (LocalMaxRSSI < 40))
+				AntDivMaxRSSI = LocalMaxRSSI;
+			if (LocalMaxRSSI > MaxRSSI)
+				MaxRSSI = LocalMaxRSSI;
+
+			/* 2 Select RX Idle Antenna */
+			if ((dm_fat_tbl->RxIdleAnt == MAIN_ANT) && (Main_RSSI == 0))
+				Main_RSSI = Aux_RSSI;
+			else if ((dm_fat_tbl->RxIdleAnt == AUX_ANT) && (Aux_RSSI == 0))
+				Aux_RSSI = Main_RSSI;
+
+			LocalMinRSSI = (Main_RSSI > Aux_RSSI) ? Aux_RSSI : Main_RSSI;
+			if (LocalMinRSSI < MinRSSI) {
+				MinRSSI = LocalMinRSSI;
+				RxIdleAnt = TargetAnt;
+			}
+			/* 2 Select TRX Antenna */
+			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
+				odm_UpdateTxAnt_88E(dm_odm, TargetAnt, i);
+		}
+		dm_fat_tbl->MainAnt_Sum[i] = 0;
+		dm_fat_tbl->AuxAnt_Sum[i] = 0;
+		dm_fat_tbl->MainAnt_Cnt[i] = 0;
+		dm_fat_tbl->AuxAnt_Cnt[i] = 0;
+	}
+
+	/* 2 Set RX Idle Antenna */
+	ODM_UpdateRxIdleAnt_88E(dm_odm, RxIdleAnt);
+
+	pDM_DigTable->AntDiv_RSSI_max = AntDivMaxRSSI;
+	pDM_DigTable->RSSI_max = MaxRSSI;
+}
+
+void ODM_AntennaDiversity_88E(struct odm_dm_struct *dm_odm)
+{
+	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
+	struct adapter *adapter = dm_odm->Adapter;
+
+	if (!(dm_odm->SupportAbility & ODM_BB_ANT_DIV))
+		return;
+	if (!dm_odm->bLinked) {
+		if (dm_fat_tbl->bBecomeLinked) {
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT(7), 0);	/* RegC50[7]=1'b1		enable HW AntDiv */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N, BIT(15), 0); /* Enable CCK AntDiv */
+			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
+				rtl8188e_PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT(21), 0); /* Reg80c[21]=1'b0		from TX Reg */
+			dm_fat_tbl->bBecomeLinked = dm_odm->bLinked;
+		}
+		return;
+	} else {
+		if (!dm_fat_tbl->bBecomeLinked) {
+			/* Because HW AntDiv is disabled before Link, we enable HW AntDiv after link */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT(7), 1);	/* RegC50[7]=1'b1		enable HW AntDiv */
+			rtl8188e_PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N, BIT(15), 1); /* Enable CCK AntDiv */
+			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
+				rtl8188e_PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT(21), 1); /* Reg80c[21]=1'b1		from TX Info */
+			dm_fat_tbl->bBecomeLinked = dm_odm->bLinked;
+		}
+	}
+	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) || (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV))
+		odm_HWAntDiv(dm_odm);
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_cmd.c b/drivers/staging/r8188eu/hal/rtl8188e_cmd.c
new file mode 100644
index 000000000..8310d7f53
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_cmd.c
@@ -0,0 +1,694 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTL8188E_CMD_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_ioctl_set.h"
+
+#include "../include/rtl8188e_hal.h"
+
+#define RTL88E_MAX_H2C_BOX_NUMS		4
+#define RTL88E_MAX_CMD_LEN		7
+#define RTL88E_MESSAGE_BOX_SIZE		4
+#define RTL88E_EX_MESSAGE_BOX_SIZE	4
+
+static u8 _is_fw_read_cmd_down(struct adapter *adapt, u8 msgbox_num)
+{
+	u8 read_down = false, reg;
+	int	retry_cnts = 100;
+	int res;
+
+	u8 valid;
+
+	do {
+		res = rtw_read8(adapt, REG_HMETFR, &reg);
+		if (res)
+			continue;
+
+		valid = reg & BIT(msgbox_num);
+		if (0 == valid)
+			read_down = true;
+	} while ((!read_down) && (retry_cnts--));
+
+	return read_down;
+}
+
+/*****************************************
+* H2C Msg format :
+* 0x1DF - 0x1D0
+*| 31 - 8	| 7-5	 4 - 0	|
+*| h2c_msg	|Class_ID CMD_ID	|
+*
+* Extend 0x1FF - 0x1F0
+*|31 - 0	  |
+*|ext_msg|
+******************************************/
+static s32 FillH2CCmd_88E(struct adapter *adapt, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+{
+	u8 bcmd_down = false;
+	s32 retry_cnts = 100;
+	u8 h2c_box_num;
+	u32 msgbox_addr;
+	u32 msgbox_ex_addr;
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	u8 cmd_idx, ext_cmd_len;
+	u32 h2c_cmd = 0;
+	u32 h2c_cmd_ex = 0;
+
+	if (!adapt->bFWReady)
+		return _FAIL;
+
+	if (!pCmdBuffer || CmdLen > RTL88E_MAX_CMD_LEN || adapt->bSurpriseRemoved)
+		return _FAIL;
+
+	/* pay attention to if  race condition happened in  H2C cmd setting. */
+	do {
+		h2c_box_num = haldata->LastHMEBoxNum;
+
+		if (!_is_fw_read_cmd_down(adapt, h2c_box_num))
+			return _FAIL;
+
+		*(u8 *)(&h2c_cmd) = ElementID;
+
+		if (CmdLen <= 3) {
+			memcpy((u8 *)(&h2c_cmd) + 1, pCmdBuffer, CmdLen);
+		} else {
+			memcpy((u8 *)(&h2c_cmd) + 1, pCmdBuffer, 3);
+			ext_cmd_len = CmdLen - 3;
+			memcpy((u8 *)(&h2c_cmd_ex), pCmdBuffer + 3, ext_cmd_len);
+
+			/* Write Ext command */
+			msgbox_ex_addr = REG_HMEBOX_EXT_0 + (h2c_box_num * RTL88E_EX_MESSAGE_BOX_SIZE);
+			for (cmd_idx = 0; cmd_idx < ext_cmd_len; cmd_idx++) {
+				rtw_write8(adapt, msgbox_ex_addr + cmd_idx, *((u8 *)(&h2c_cmd_ex) + cmd_idx));
+			}
+		}
+		/*  Write command */
+		msgbox_addr = REG_HMEBOX_0 + (h2c_box_num * RTL88E_MESSAGE_BOX_SIZE);
+		for (cmd_idx = 0; cmd_idx < RTL88E_MESSAGE_BOX_SIZE; cmd_idx++) {
+			rtw_write8(adapt, msgbox_addr + cmd_idx, *((u8 *)(&h2c_cmd) + cmd_idx));
+		}
+		bcmd_down = true;
+
+		haldata->LastHMEBoxNum = (h2c_box_num + 1) % RTL88E_MAX_H2C_BOX_NUMS;
+
+	} while ((!bcmd_down) && (retry_cnts--));
+
+	return _SUCCESS;
+}
+
+u8 rtl8188e_set_raid_cmd(struct adapter *adapt, u32 mask)
+{
+	u8 buf[3];
+	u8 res = _SUCCESS;
+	struct hal_data_8188e *haldata = &adapt->haldata;
+
+	if (haldata->fw_ractrl) {
+		__le32 lmask;
+
+		memset(buf, 0, 3);
+		lmask = cpu_to_le32(mask);
+		memcpy(buf, &lmask, 3);
+
+		FillH2CCmd_88E(adapt, H2C_DM_MACID_CFG, 3, buf);
+	} else {
+		res = _FAIL;
+	}
+
+	return res;
+}
+
+/* bitmap[0:27] = tx_rate_bitmap */
+/* bitmap[28:31]= Rate Adaptive id */
+/* arg[0:4] = macid */
+/* arg[5] = Short GI */
+void rtl8188e_Add_RateATid(struct adapter *pAdapter, u32 bitmap, u8 arg, u8 rssi_level)
+{
+	struct hal_data_8188e *haldata = &pAdapter->haldata;
+
+	u8 macid, raid, short_gi_rate = false;
+
+	macid = arg & 0x1f;
+
+	raid = (bitmap >> 28) & 0x0f;
+	bitmap &= 0x0fffffff;
+
+	if (rssi_level != DM_RATR_STA_INIT)
+		bitmap = ODM_Get_Rate_Bitmap(&haldata->odmpriv, macid, bitmap, rssi_level);
+
+	bitmap |= ((raid << 28) & 0xf0000000);
+
+	short_gi_rate = (arg & BIT(5)) ? true : false;
+
+	raid = (bitmap >> 28) & 0x0f;
+
+	bitmap &= 0x0fffffff;
+
+	ODM_RA_UpdateRateInfo_8188E(&haldata->odmpriv, macid, raid, bitmap, short_gi_rate);
+}
+
+void rtl8188e_set_FwPwrMode_cmd(struct adapter *adapt, u8 Mode)
+{
+	struct setpwrmode_parm H2CSetPwrMode;
+	struct pwrctrl_priv *pwrpriv = &adapt->pwrctrlpriv;
+	u8 RLBM = 0; /*  0:Min, 1:Max, 2:User define */
+
+	switch (Mode) {
+	case PS_MODE_ACTIVE:
+		H2CSetPwrMode.Mode = 0;
+		break;
+	case PS_MODE_MIN:
+		H2CSetPwrMode.Mode = 1;
+		break;
+	case PS_MODE_MAX:
+		RLBM = 1;
+		H2CSetPwrMode.Mode = 1;
+		break;
+	case PS_MODE_DTIM:
+		RLBM = 2;
+		H2CSetPwrMode.Mode = 1;
+		break;
+	case PS_MODE_UAPSD_WMM:
+		H2CSetPwrMode.Mode = 2;
+		break;
+	default:
+		H2CSetPwrMode.Mode = 0;
+		break;
+	}
+
+	H2CSetPwrMode.SmartPS_RLBM = (((pwrpriv->smart_ps << 4) & 0xf0) | (RLBM & 0x0f));
+
+	H2CSetPwrMode.AwakeInterval = 1;
+
+	H2CSetPwrMode.bAllQueueUAPSD = adapt->registrypriv.uapsd_enable;
+
+	if (Mode > 0)
+		H2CSetPwrMode.PwrState = 0x00;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+	else
+		H2CSetPwrMode.PwrState = 0x0C;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+
+	FillH2CCmd_88E(adapt, H2C_PS_PWR_MODE, sizeof(H2CSetPwrMode), (u8 *)&H2CSetPwrMode);
+
+}
+
+void rtl8188e_set_FwMediaStatus_cmd(struct adapter *adapt, __le16 mstatus_rpt)
+{
+	u16 mst_rpt = le16_to_cpu(mstatus_rpt);
+
+	FillH2CCmd_88E(adapt, H2C_COM_MEDIA_STATUS_RPT, sizeof(mst_rpt), (u8 *)&mst_rpt);
+}
+
+static void ConstructBeacon(struct adapter *adapt, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 rate_len, pktlen;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex		*cur_network = &pmlmeinfo->network;
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	eth_broadcast_addr(pwlanhdr->addr1);
+	memcpy(pwlanhdr->addr2, myid(&adapt->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	/*  capability info: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		pktlen += cur_network->IELength - sizeof(struct ndis_802_11_fixed_ie);
+		memcpy(pframe, cur_network->IEs + sizeof(struct ndis_802_11_fixed_ie), pktlen);
+
+		goto _ConstructBeacon;
+	}
+
+	/* below for ad-hoc mode */
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pktlen);
+
+	/*  supported rates... */
+	rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pktlen);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pktlen);
+	}
+
+	/* todo: ERP IE */
+
+	/*  EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pktlen);
+
+	/* todo:HT for adhoc */
+
+_ConstructBeacon:
+
+	if ((pktlen + TXDESC_SIZE) > 512)
+		return;
+
+	*pLength = pktlen;
+}
+
+static void ConstructPSPoll(struct adapter *adapt, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	__le16 *fctrl;
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	/*  Frame control. */
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	SetPwrMgt(fctrl);
+	SetFrameSubType(pframe, WIFI_PSPOLL);
+
+	/*  AID. */
+	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
+
+	/*  BSSID. */
+	memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	/*  TA. */
+	memcpy(pwlanhdr->addr2, myid(&adapt->eeprompriv), ETH_ALEN);
+
+	*pLength = 16;
+}
+
+static void ConstructNullFunctionData(struct adapter *adapt, u8 *pframe,
+	u32 *pLength,
+	u8 *StaAddr,
+	u8 bQoS,
+	u8 AC,
+	u8 bEosp,
+	u8 bForcePowerSave)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 pktlen;
+	struct mlme_priv *pmlmepriv = &adapt->mlmepriv;
+	struct wlan_network		*cur_network = &pmlmepriv->cur_network;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	if (bForcePowerSave)
+		SetPwrMgt(fctrl);
+
+	switch (cur_network->network.InfrastructureMode) {
+	case Ndis802_11Infrastructure:
+		SetToDs(fctrl);
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&adapt->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
+		break;
+	case Ndis802_11APMode:
+		SetFrDs(fctrl);
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&adapt->eeprompriv), ETH_ALEN);
+		break;
+	case Ndis802_11IBSS:
+	default:
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&adapt->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		break;
+	}
+
+	SetSeqNum(pwlanhdr, 0);
+
+	if (bQoS) {
+		struct ieee80211_qos_hdr *pwlanqoshdr;
+
+		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
+		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
+		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	} else {
+		SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	}
+
+	*pLength = pktlen;
+}
+
+static void ConstructProbeRsp(struct adapter *adapt, u8 *pframe, u32 *pLength, u8 *StaAddr, bool bHideSSID)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u8 *mac, *bssid;
+	u32 pktlen;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex	*cur_network = &pmlmeinfo->network;
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&adapt->eeprompriv);
+	bssid = cur_network->MacAddress;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0);
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+	pframe += pktlen;
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+	memcpy(pframe, cur_network->IEs, cur_network->IELength);
+	pframe += cur_network->IELength;
+	pktlen += cur_network->IELength;
+
+	*pLength = pktlen;
+}
+
+/*  To check if reserved page content is destroyed by beacon because beacon is too large. */
+/*  2010.06.23. Added by tynli. */
+void CheckFwRsvdPageContent(struct adapter *Adapter)
+{
+}
+
+/*  */
+/*  Description: Fill the reserved packets that FW will use to RSVD page. */
+/*			Now we just send 4 types packet to rsvd page. */
+/*			(1)Beacon, (2)Ps-poll, (3)Null data, (4)ProbeRsp. */
+/*	Input: */
+/*	    bDLFinished - false: At the first time we will send all the packets as a large packet to Hw, */
+/*						so we need to set the packet length to total length. */
+/*			      true: At the second time, we should send the first packet (default:beacon) */
+/*						to Hw again and set the length in descriptor to the real beacon length. */
+/*  2009.10.15 by tynli. */
+static void SetFwRsvdPagePkt(struct adapter *adapt, bool bDLFinished)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+	u32 BeaconLength = 0, ProbeRspLength = 0, PSPollLength;
+	u32 NullDataLength, QosNullLength;
+	u8 *ReservedPagePacket;
+	u8 PageNum, PageNeed, TxDescLen;
+	u16 BufIndex;
+	u32 TotalPacketLen;
+	struct rsvdpage_loc RsvdPageLoc;
+
+	ReservedPagePacket = kzalloc(1000, GFP_KERNEL);
+	if (!ReservedPagePacket)
+		return;
+
+	pxmitpriv = &adapt->xmitpriv;
+	pmlmeext = &adapt->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	TxDescLen = TXDESC_SIZE;
+	PageNum = 0;
+
+	/* 3 (1) beacon * 2 pages */
+	BufIndex = TXDESC_OFFSET;
+	ConstructBeacon(adapt, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	PageNeed = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/*  To reserved 2 pages for beacon buffer. 2010.06.24. */
+	if (PageNeed == 1)
+		PageNeed += 1;
+	PageNum += PageNeed;
+
+	BufIndex += PageNeed * 128;
+
+	/* 3 (2) ps-poll *1 page */
+	RsvdPageLoc.LocPsPoll = PageNum;
+	ConstructPSPoll(adapt, &ReservedPagePacket[BufIndex], &PSPollLength);
+	rtl8188e_fill_fake_txdesc(adapt, &ReservedPagePacket[BufIndex - TxDescLen], PSPollLength, true, false);
+
+	PageNeed = (u8)PageNum_128(TxDescLen + PSPollLength);
+	PageNum += PageNeed;
+
+	BufIndex += PageNeed * 128;
+
+	/* 3 (3) null data * 1 page */
+	RsvdPageLoc.LocNullData = PageNum;
+	ConstructNullFunctionData(adapt, &ReservedPagePacket[BufIndex], &NullDataLength, get_my_bssid(&pmlmeinfo->network), false, 0, 0, false);
+	rtl8188e_fill_fake_txdesc(adapt, &ReservedPagePacket[BufIndex - TxDescLen], NullDataLength, false, false);
+
+	PageNeed = (u8)PageNum_128(TxDescLen + NullDataLength);
+	PageNum += PageNeed;
+
+	BufIndex += PageNeed * 128;
+
+	/* 3 (4) probe response * 1page */
+	RsvdPageLoc.LocProbeRsp = PageNum;
+	ConstructProbeRsp(adapt, &ReservedPagePacket[BufIndex], &ProbeRspLength, get_my_bssid(&pmlmeinfo->network), false);
+	rtl8188e_fill_fake_txdesc(adapt, &ReservedPagePacket[BufIndex - TxDescLen], ProbeRspLength, false, false);
+
+	PageNeed = (u8)PageNum_128(TxDescLen + ProbeRspLength);
+	PageNum += PageNeed;
+
+	BufIndex += PageNeed * 128;
+
+	/* 3 (5) Qos null data */
+	RsvdPageLoc.LocQosNull = PageNum;
+	ConstructNullFunctionData(adapt, &ReservedPagePacket[BufIndex],
+				  &QosNullLength, get_my_bssid(&pmlmeinfo->network), true, 0, 0, false);
+	rtl8188e_fill_fake_txdesc(adapt, &ReservedPagePacket[BufIndex - TxDescLen], QosNullLength, false, false);
+
+	PageNeed = (u8)PageNum_128(TxDescLen + QosNullLength);
+	PageNum += PageNeed;
+
+	TotalPacketLen = BufIndex + QosNullLength;
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/*  update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapt, pattrib);
+	pattrib->qsel = 0x10;
+	pattrib->last_txcmdsz = TotalPacketLen - TXDESC_OFFSET;
+	pattrib->pktlen = pattrib->last_txcmdsz;
+	memcpy(pmgntframe->buf_addr, ReservedPagePacket, TotalPacketLen);
+
+	rtl8188eu_mgnt_xmit(adapt, pmgntframe);
+
+	FillH2CCmd_88E(adapt, H2C_COM_RSVD_PAGE, sizeof(RsvdPageLoc), (u8 *)&RsvdPageLoc);
+
+exit:
+	kfree(ReservedPagePacket);
+}
+
+void rtl8188e_set_FwJoinBssReport_cmd(struct adapter *adapt, u8 mstatus)
+{
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	bool	bSendBeacon = false;
+	bool	bcn_valid = false;
+	u8 DLBcnCount = 0;
+	u32 poll = 0;
+	u8 reg;
+	int res;
+
+	if (mstatus == 1) {
+		/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
+		/*  Suggested by filen. Added by tynli. */
+		rtw_write16(adapt, REG_BCN_PSR_RPT, (0xC000 | pmlmeinfo->aid));
+		/*  Do not set TSF again here or vWiFi beacon DMA INT will not work. */
+
+		/* Set REG_CR bit 8. DMA beacon by SW. */
+		haldata->RegCR_1 |= BIT(0);
+		rtw_write8(adapt,  REG_CR + 1, haldata->RegCR_1);
+
+		/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
+		/*  Fix download reserved page packet fail that access collision with the protection time. */
+		/*  2010.05.11. Added by tynli. */
+		res = rtw_read8(adapt, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapt, REG_BCN_CTRL, reg & (~BIT(3)));
+
+		res = rtw_read8(adapt, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapt, REG_BCN_CTRL, reg | BIT(4));
+
+		if (haldata->RegFwHwTxQCtrl & BIT(6))
+			bSendBeacon = true;
+
+		/*  Set FWHW_TXQ_CTRL 0x422[6]=0 to tell Hw the packet is not a real beacon frame. */
+		rtw_write8(adapt, REG_FWHW_TXQ_CTRL + 2, (haldata->RegFwHwTxQCtrl & (~BIT(6))));
+		haldata->RegFwHwTxQCtrl &= (~BIT(6));
+
+		clear_beacon_valid_bit(adapt);
+		DLBcnCount = 0;
+		poll = 0;
+		do {
+			/*  download rsvd page. */
+			SetFwRsvdPagePkt(adapt, false);
+			DLBcnCount++;
+			do {
+				yield();
+				/* mdelay(10); */
+				/*  check rsvd page download OK. */
+				bcn_valid = get_beacon_valid_bit(adapt);
+				poll++;
+			} while (!bcn_valid && (poll % 10) != 0 && !adapt->bSurpriseRemoved && !adapt->bDriverStopped);
+		} while (!bcn_valid && DLBcnCount <= 100 && !adapt->bSurpriseRemoved && !adapt->bDriverStopped);
+
+		/*  */
+		/*  We just can send the reserved page twice during the time that Tx thread is stopped (e.g. pnpsetpower) */
+		/*  because we need to free the Tx BCN Desc which is used by the first reserved page packet. */
+		/*  At run time, we cannot get the Tx Desc until it is released in TxHandleInterrupt() so we will return */
+		/*  the beacon TCB in the following code. 2011.11.23. by tynli. */
+		/*  */
+
+		/*  Enable Bcn */
+		res = rtw_read8(adapt, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapt, REG_BCN_CTRL, reg | BIT(3));
+
+		res = rtw_read8(adapt, REG_BCN_CTRL, &reg);
+		if (res)
+			return;
+
+		rtw_write8(adapt, REG_BCN_CTRL, reg & (~BIT(4)));
+
+		/*  To make sure that if there exists an adapter which would like to send beacon. */
+		/*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
+		/*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
+		/*  the beacon cannot be sent by HW. */
+		/*  2010.06.23. Added by tynli. */
+		if (bSendBeacon) {
+			rtw_write8(adapt, REG_FWHW_TXQ_CTRL + 2, (haldata->RegFwHwTxQCtrl | BIT(6)));
+			haldata->RegFwHwTxQCtrl |= BIT(6);
+		}
+
+		/*  Update RSVD page location H2C to Fw. */
+		if (bcn_valid)
+			clear_beacon_valid_bit(adapt);
+
+		/*  Do not enable HW DMA BCN or it will cause Pcie interface hang by timing issue. 2011.11.24. by tynli. */
+		/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
+		haldata->RegCR_1 &= (~BIT(0));
+		rtw_write8(adapt,  REG_CR + 1, haldata->RegCR_1);
+	}
+
+}
+
+void rtl8188e_set_p2p_ps_offload_cmd(struct adapter *adapt, u8 p2p_ps_state)
+{
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct wifidirect_info	*pwdinfo = &adapt->wdinfo;
+	struct P2P_PS_Offload_t	*p2p_ps_offload = &haldata->p2p_ps_offload;
+	u8 i;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		memset(p2p_ps_offload, 0, 1);
+		break;
+	case P2P_PS_ENABLE:
+		/*  update CTWindow value. */
+		if (pwdinfo->ctwindow > 0) {
+			p2p_ps_offload->CTWindow_En = 1;
+			rtw_write8(adapt, REG_P2P_CTWIN, pwdinfo->ctwindow);
+		}
+
+		/*  hw only support 2 set of NoA */
+		for (i = 0; i < pwdinfo->noa_num; i++) {
+			/*  To control the register setting for which NOA */
+			rtw_write8(adapt, REG_NOA_DESC_SEL, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->NoA0_En = 1;
+			else
+				p2p_ps_offload->NoA1_En = 1;
+
+			/*  config P2P NoA Descriptor Register */
+			rtw_write32(adapt, REG_NOA_DESC_DURATION, pwdinfo->noa_duration[i]);
+			rtw_write32(adapt, REG_NOA_DESC_INTERVAL, pwdinfo->noa_interval[i]);
+			rtw_write32(adapt, REG_NOA_DESC_START, pwdinfo->noa_start_time[i]);
+			rtw_write8(adapt, REG_NOA_DESC_COUNT, pwdinfo->noa_count[i]);
+		}
+
+		if ((pwdinfo->opp_ps == 1) || (pwdinfo->noa_num > 0)) {
+			/*  rst p2p circuit */
+			rtw_write8(adapt, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->Offload_En = 1;
+
+			if (pwdinfo->role == P2P_ROLE_GO) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->AllStaSleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		p2p_ps_offload->discovery = 0;
+		pwdinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+
+	FillH2CCmd_88E(adapt, H2C_PS_P2P_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_dm.c b/drivers/staging/r8188eu/hal/rtl8188e_dm.c
new file mode 100644
index 000000000..0399872c4
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_dm.c
@@ -0,0 +1,146 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+/*  This file is for 92CE/92CU dynamic mechanism only */
+#define _RTL8188E_DM_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtl8188e_hal.h"
+
+/*  Initialize GPIO setting registers */
+static void dm_InitGPIOSetting(struct adapter *Adapter)
+{
+	u8	tmp1byte;
+	int res;
+
+	res = rtw_read8(Adapter, REG_GPIO_MUXCFG, &tmp1byte);
+	if (res)
+		return;
+
+	tmp1byte &= (GPIOSEL_GPIO | ~GPIOSEL_ENBT);
+
+	rtw_write8(Adapter, REG_GPIO_MUXCFG, tmp1byte);
+}
+
+/*  */
+/*  functions */
+/*  */
+static void Update_ODM_ComInfo_88E(struct adapter *Adapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_priv	*pmlmepriv = &Adapter->mlmepriv;
+	struct pwrctrl_priv *pwrctrlpriv = &Adapter->pwrctrlpriv;
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+	struct dm_priv	*pdmpriv = &hal_data->dmpriv;
+	int i;
+
+	pdmpriv->InitODMFlag = ODM_BB_RSSI_MONITOR;
+	if (hal_data->AntDivCfg)
+		pdmpriv->InitODMFlag |= ODM_BB_ANT_DIV;
+
+	dm_odm->SupportAbility = pdmpriv->InitODMFlag;
+
+	dm_odm->pWirelessMode = &pmlmeext->cur_wireless_mode;
+	dm_odm->pSecChOffset = &hal_data->nCur40MhzPrimeSC;
+	dm_odm->pBandWidth = &hal_data->CurrentChannelBW;
+	dm_odm->pChannel = &hal_data->CurrentChannel;
+	dm_odm->pbScanInProcess = &pmlmepriv->bScanInProcess;
+	dm_odm->pbPowerSaving = &pwrctrlpriv->bpower_saving;
+
+	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_RF_ANTENNA_TYPE, hal_data->TRxAntDivType);
+
+	for (i = 0; i < NUM_STA; i++)
+		dm_odm->pODM_StaInfo[i] = NULL;
+}
+
+void rtl8188e_InitHalDm(struct adapter *Adapter)
+{
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	dm_InitGPIOSetting(Adapter);
+	Update_ODM_ComInfo_88E(Adapter);
+	ODM_DMInit(dm_odm);
+}
+
+void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
+{
+	u8 hw_init_completed = Adapter->hw_init_completed;
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	u8 bLinked = false;
+
+	if (!hw_init_completed)
+		return;
+
+	if ((check_fwstate(pmlmepriv, WIFI_AP_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE))) {
+		if (Adapter->stapriv.asoc_sta_count > 2)
+			bLinked = true;
+	} else {/* Station mode */
+		if (check_fwstate(pmlmepriv, _FW_LINKED))
+			bLinked = true;
+	}
+
+	hal_data->odmpriv.bLinked = bLinked;
+	ODM_DMWatchdog(&hal_data->odmpriv);
+}
+
+void rtl8188e_init_dm_priv(struct adapter *Adapter)
+{
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+	struct dm_priv	*pdmpriv = &hal_data->dmpriv;
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	memset(pdmpriv, 0, sizeof(struct dm_priv));
+	memset(dm_odm, 0, sizeof(*dm_odm));
+
+	dm_odm->Adapter = Adapter;
+	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_MP_TEST_CHIP, IS_NORMAL_CHIP(hal_data->VersionID));
+	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_RF_ANTENNA_TYPE, hal_data->TRxAntDivType);
+}
+
+/*  Add new function to reset the state of antenna diversity before link. */
+/*  Compare RSSI for deciding antenna */
+void AntDivCompare8188E(struct adapter *Adapter, struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src)
+{
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+
+	if (0 != hal_data->AntDivCfg) {
+		/* select optimum_antenna for before linked =>For antenna diversity */
+		if (dst->Rssi >=  src->Rssi) {/* keep org parameter */
+			src->Rssi = dst->Rssi;
+			src->PhyInfo.Optimum_antenna = dst->PhyInfo.Optimum_antenna;
+		}
+	}
+}
+
+/*  Add new function to reset the state of antenna diversity before link. */
+u8 AntDivBeforeLink8188E(struct adapter *Adapter)
+{
+	struct hal_data_8188e *hal_data = &Adapter->haldata;
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+	struct sw_ant_switch *dm_swat_tbl = &dm_odm->DM_SWAT_Table;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+
+	/*  Condition that does not need to use antenna diversity. */
+	if (hal_data->AntDivCfg == 0)
+		return false;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		return false;
+
+	if (dm_swat_tbl->SWAS_NoLink_State == 0) {
+		/* switch channel */
+		dm_swat_tbl->SWAS_NoLink_State = 1;
+		dm_swat_tbl->CurAntenna = (dm_swat_tbl->CurAntenna == Antenna_A) ? Antenna_B : Antenna_A;
+
+		rtw_antenna_select_cmd(Adapter, dm_swat_tbl->CurAntenna, false);
+		return true;
+	} else {
+		dm_swat_tbl->SWAS_NoLink_State = 0;
+		return false;
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/r8188eu/hal/rtl8188e_hal_init.c
new file mode 100644
index 000000000..158260547
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_hal_init.c
@@ -0,0 +1,926 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _HAL_INIT_C_
+
+#include "../include/drv_types.h"
+#include "../include/rtw_efuse.h"
+#include "../include/rtl8188e_hal.h"
+#include "../include/rtw_iol.h"
+#include "../include/usb_ops.h"
+#include "../include/rtw_fw.h"
+
+static void iol_mode_enable(struct adapter *padapter, u8 enable)
+{
+	u8 reg_0xf0 = 0;
+	int res;
+
+	if (enable) {
+		/* Enable initial offload */
+		res = rtw_read8(padapter, REG_SYS_CFG, &reg_0xf0);
+		if (res)
+			return;
+
+		rtw_write8(padapter, REG_SYS_CFG, reg_0xf0 | SW_OFFLOAD_EN);
+
+		if (!padapter->bFWReady)
+			rtw_reset_8051(padapter);
+
+	} else {
+		/* disable initial offload */
+		res = rtw_read8(padapter, REG_SYS_CFG, &reg_0xf0);
+		if (res)
+			return;
+
+		rtw_write8(padapter, REG_SYS_CFG, reg_0xf0 & ~SW_OFFLOAD_EN);
+	}
+}
+
+static s32 iol_execute(struct adapter *padapter, u8 control)
+{
+	s32 status = _FAIL;
+	u8 reg_0x88 = 0;
+	unsigned long timeout;
+	int res;
+
+	control = control & 0x0f;
+	res = rtw_read8(padapter, REG_HMEBOX_E0, &reg_0x88);
+	if (res)
+		return _FAIL;
+
+	rtw_write8(padapter, REG_HMEBOX_E0,  reg_0x88 | control);
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+
+	do {
+		res = rtw_read8(padapter, REG_HMEBOX_E0, &reg_0x88);
+		if (res)
+			continue;
+
+		if (!(reg_0x88 & control))
+			break;
+
+	} while (time_before(jiffies, timeout));
+
+	res = rtw_read8(padapter, REG_HMEBOX_E0, &reg_0x88);
+	if (res)
+		return _FAIL;
+
+	status = (reg_0x88 & control) ? _FAIL : _SUCCESS;
+	if (reg_0x88 & control << 4)
+		status = _FAIL;
+	return status;
+}
+
+static s32 iol_InitLLTTable(struct adapter *padapter, u8 txpktbuf_bndy)
+{
+	s32 rst = _SUCCESS;
+	iol_mode_enable(padapter, 1);
+	rtw_write8(padapter, REG_TDECTRL + 1, txpktbuf_bndy);
+	rst = iol_execute(padapter, CMD_INIT_LLT);
+	iol_mode_enable(padapter, 0);
+	return rst;
+}
+
+static void
+efuse_phymap_to_logical(u8 *phymap, u16 _size_byte, u8  *pbuf)
+{
+	u8 *efuseTbl = NULL;
+	u8 rtemp8;
+	u16	eFuse_Addr = 0;
+	u8 offset, wren;
+	u16	i, j;
+	u16	**eFuseWord = NULL;
+	u8 u1temp = 0;
+
+	efuseTbl = kzalloc(EFUSE_MAP_LEN_88E, GFP_KERNEL);
+	if (!efuseTbl)
+		goto exit;
+
+	eFuseWord = rtw_malloc2d(EFUSE_MAX_SECTION_88E, EFUSE_MAX_WORD_UNIT, sizeof(u16));
+	if (!eFuseWord)
+		goto exit;
+
+	/*  0. Refresh efuse init map as all oxFF. */
+	for (i = 0; i < EFUSE_MAX_SECTION_88E; i++)
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
+			eFuseWord[i][j] = 0xFFFF;
+
+	/*  */
+	/*  1. Read the first byte to check if efuse is empty!!! */
+	/*  */
+	/*  */
+	rtemp8 = *(phymap + eFuse_Addr);
+	if (rtemp8 != 0xFF) {
+		eFuse_Addr++;
+	} else {
+		goto exit;
+	}
+
+	/*  */
+	/*  2. Read real efuse content. Filter PG header and every section data. */
+	/*  */
+	while ((rtemp8 != 0xFF) && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E)) {
+		/*  Check PG header for section num. */
+		if ((rtemp8 & 0x1F) == 0x0F) {		/* extended header */
+			u1temp = ((rtemp8 & 0xE0) >> 5);
+			rtemp8 = *(phymap + eFuse_Addr);
+			if ((rtemp8 & 0x0F) == 0x0F) {
+				eFuse_Addr++;
+				rtemp8 = *(phymap + eFuse_Addr);
+
+				if (rtemp8 != 0xFF && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E))
+					eFuse_Addr++;
+				continue;
+			} else {
+				offset = ((rtemp8 & 0xF0) >> 1) | u1temp;
+				wren = (rtemp8 & 0x0F);
+				eFuse_Addr++;
+			}
+		} else {
+			offset = ((rtemp8 >> 4) & 0x0f);
+			wren = (rtemp8 & 0x0f);
+		}
+
+		if (offset < EFUSE_MAX_SECTION_88E) {
+			/*  Get word enable value from PG header */
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				/*  Check word enable condition in the section */
+				if (!(wren & 0x01)) {
+					rtemp8 = *(phymap + eFuse_Addr);
+					eFuse_Addr++;
+					eFuseWord[offset][i] = (rtemp8 & 0xff);
+					if (eFuse_Addr >= EFUSE_REAL_CONTENT_LEN_88E)
+						break;
+					rtemp8 = *(phymap + eFuse_Addr);
+					eFuse_Addr++;
+					eFuseWord[offset][i] |= (((u16)rtemp8 << 8) & 0xff00);
+
+					if (eFuse_Addr >= EFUSE_REAL_CONTENT_LEN_88E)
+						break;
+				}
+				wren >>= 1;
+			}
+		}
+		/*  Read next PG header */
+		rtemp8 = *(phymap + eFuse_Addr);
+
+		if (rtemp8 != 0xFF && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E)) {
+			eFuse_Addr++;
+		}
+	}
+
+	/*  */
+	/*  3. Collect 16 sections and 4 word unit into Efuse map. */
+	/*  */
+	for (i = 0; i < EFUSE_MAX_SECTION_88E; i++) {
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
+			efuseTbl[(i * 8) + (j * 2)] = (eFuseWord[i][j] & 0xff);
+			efuseTbl[(i * 8) + ((j * 2) + 1)] = ((eFuseWord[i][j] >> 8) & 0xff);
+		}
+	}
+
+	/*  */
+	/*  4. Copy from Efuse map to output pointer memory!!! */
+	/*  */
+	memcpy(pbuf, efuseTbl, _size_byte);
+
+exit:
+	kfree(efuseTbl);
+	kfree(eFuseWord);
+}
+
+/* FIXME: add error handling in callers */
+static int efuse_read_phymap_from_txpktbuf(
+	struct adapter  *adapter,
+	u8 *content,	/* buffer to store efuse physical map */
+	u16 *size	/* for efuse content: the max byte to read. will update to byte read */
+	)
+{
+	unsigned long timeout;
+	__le32 lo32 = 0, hi32 = 0;
+	u16 len = 0, count = 0;
+	int i = 0, res;
+	u16 limit = *size;
+	u8 reg;
+	u8 *pos = content;
+	u32 reg32;
+
+	rtw_write8(adapter, REG_PKT_BUFF_ACCESS_CTRL, TXPKT_BUF_SELECT);
+
+	while (1) {
+		rtw_write16(adapter, REG_PKTBUF_DBG_ADDR, i);
+
+		rtw_write8(adapter, REG_TXPKTBUF_DBG, 0);
+		timeout = jiffies + msecs_to_jiffies(1000);
+		do {
+			res = rtw_read8(adapter, REG_TXPKTBUF_DBG, &reg);
+			if (res)
+				continue;
+
+			if (reg)
+				break;
+
+			msleep(1);
+		} while (time_before(jiffies, timeout));
+
+		/* data from EEPROM needs to be in LE */
+		res = rtw_read32(adapter, REG_PKTBUF_DBG_DATA_L, &reg32);
+		if (res)
+			return res;
+
+		lo32 = cpu_to_le32(reg32);
+
+		res = rtw_read32(adapter, REG_PKTBUF_DBG_DATA_H, &reg32);
+		if (res)
+			return res;
+
+		hi32 = cpu_to_le32(reg32);
+
+		if (i == 0) {
+			u16 reg;
+
+			/* Although lenc is only used in a debug statement,
+			 * do not remove it as the rtw_read16() call consumes
+			 * 2 bytes from the EEPROM source.
+			 */
+			res = rtw_read16(adapter, REG_PKTBUF_DBG_DATA_L, &reg);
+			if (res)
+				return res;
+
+			len = le32_to_cpu(lo32) & 0x0000ffff;
+
+			limit = (len - 2 < limit) ? len - 2 : limit;
+
+			memcpy(pos, ((u8 *)&lo32) + 2, (limit >= count + 2) ? 2 : limit - count);
+			count += (limit >= count + 2) ? 2 : limit - count;
+			pos = content + count;
+		} else {
+			memcpy(pos, ((u8 *)&lo32), (limit >= count + 4) ? 4 : limit - count);
+			count += (limit >= count + 4) ? 4 : limit - count;
+			pos = content + count;
+		}
+
+		if (limit > count && len - 2 > count) {
+			memcpy(pos, (u8 *)&hi32, (limit >= count + 4) ? 4 : limit - count);
+			count += (limit >= count + 4) ? 4 : limit - count;
+			pos = content + count;
+		}
+
+		if (limit <= count || len - 2 <= count)
+			break;
+		i++;
+	}
+	rtw_write8(adapter, REG_PKT_BUFF_ACCESS_CTRL, DISABLE_TRXPKT_BUF_ACCESS);
+	*size = count;
+
+	return 0;
+}
+
+static s32 iol_read_efuse(struct adapter *padapter, u16 size_byte, u8 *logical_map)
+{
+	s32 status = _FAIL;
+	u8 physical_map[512];
+	u16 size = 512;
+
+	rtw_write8(padapter, REG_TDECTRL + 1, 0);
+	memset(physical_map, 0xFF, 512);
+	rtw_write8(padapter, REG_PKT_BUFF_ACCESS_CTRL, TXPKT_BUF_SELECT);
+	status = iol_execute(padapter, CMD_READ_EFUSE_MAP);
+	if (status == _SUCCESS)
+		efuse_read_phymap_from_txpktbuf(padapter, physical_map, &size);
+	efuse_phymap_to_logical(physical_map, size_byte, logical_map);
+	return status;
+}
+
+s32 rtl8188e_iol_efuse_patch(struct adapter *padapter)
+{
+	s32	result = _SUCCESS;
+
+	if (rtw_IOL_applied(padapter)) {
+		iol_mode_enable(padapter, 1);
+		result = iol_execute(padapter, CMD_READ_EFUSE_MAP);
+		if (result == _SUCCESS)
+			result = iol_execute(padapter, CMD_EFUSE_PATCH);
+
+		iol_mode_enable(padapter, 0);
+	}
+	return result;
+}
+
+static s32 iol_ioconfig(struct adapter *padapter, u8 iocfg_bndy)
+{
+	s32 rst = _SUCCESS;
+
+	rtw_write8(padapter, REG_TDECTRL + 1, iocfg_bndy);
+	rst = iol_execute(padapter, CMD_IOCONFIG);
+	return rst;
+}
+
+int rtl8188e_IOL_exec_cmds_sync(struct adapter *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
+{
+	struct pkt_attrib *pattrib = &xmit_frame->attrib;
+	u8 i;
+	int ret = _FAIL;
+
+	if (rtw_IOL_append_END_cmd(xmit_frame) != _SUCCESS)
+		goto exit;
+	if (rtw_usb_bulk_size_boundary(adapter, TXDESC_SIZE + pattrib->last_txcmdsz)) {
+		if (rtw_IOL_append_END_cmd(xmit_frame) != _SUCCESS)
+			goto exit;
+	}
+
+	dump_mgntframe_and_wait(adapter, xmit_frame, max_wating_ms);
+
+	iol_mode_enable(adapter, 1);
+	for (i = 0; i < bndy_cnt; i++) {
+		u8 page_no = 0;
+		page_no = i * 2;
+		ret = iol_ioconfig(adapter, page_no);
+		if (ret != _SUCCESS)
+			break;
+	}
+	iol_mode_enable(adapter, 0);
+exit:
+	/* restore BCN_HEAD */
+	rtw_write8(adapter, REG_TDECTRL + 1, 0);
+	return ret;
+}
+
+void rtl8188e_EfusePowerSwitch(struct adapter *pAdapter, u8 PwrState)
+{
+	u16	tmpV16;
+	int res;
+
+	if (PwrState) {
+		rtw_write8(pAdapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON);
+
+		/*  1.2V Power: From VDDON with Power Cut(0x0000h[15]), defualt valid */
+		res = rtw_read16(pAdapter, REG_SYS_ISO_CTRL, &tmpV16);
+		if (res)
+			return;
+
+		if (!(tmpV16 & PWC_EV12V)) {
+			tmpV16 |= PWC_EV12V;
+			rtw_write16(pAdapter, REG_SYS_ISO_CTRL, tmpV16);
+		}
+		/*  Reset: 0x0000h[28], default valid */
+		res = rtw_read16(pAdapter, REG_SYS_FUNC_EN, &tmpV16);
+		if (res)
+			return;
+
+		if (!(tmpV16 & FEN_ELDR)) {
+			tmpV16 |= FEN_ELDR;
+			rtw_write16(pAdapter, REG_SYS_FUNC_EN, tmpV16);
+		}
+
+		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock from ANA, default valid */
+		res = rtw_read16(pAdapter, REG_SYS_CLKR, &tmpV16);
+		if (res)
+			return;
+
+		if ((!(tmpV16 & LOADER_CLK_EN))  || (!(tmpV16 & ANA8M))) {
+			tmpV16 |= (LOADER_CLK_EN | ANA8M);
+			rtw_write16(pAdapter, REG_SYS_CLKR, tmpV16);
+		}
+	} else {
+		rtw_write8(pAdapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
+	}
+}
+
+static void Hal_EfuseReadEFuse88E(struct adapter *Adapter,
+	u16			_offset,
+	u16			_size_byte,
+	u8 *pbuf)
+{
+	u8 *efuseTbl = NULL;
+	u8 rtemp8[1];
+	u16	eFuse_Addr = 0;
+	u8 offset, wren;
+	u16	i, j;
+	u16	**eFuseWord = NULL;
+	u16	efuse_utilized = 0;
+	u8 u1temp = 0;
+
+	/*  */
+	/*  Do NOT excess total size of EFuse table. Added by Roger, 2008.11.10. */
+	/*  */
+	if ((_offset + _size_byte) > EFUSE_MAP_LEN_88E) /*  total E-Fuse table is 512bytes */
+		goto exit;
+
+	efuseTbl = kzalloc(EFUSE_MAP_LEN_88E, GFP_KERNEL);
+	if (!efuseTbl)
+		goto exit;
+
+	eFuseWord = rtw_malloc2d(EFUSE_MAX_SECTION_88E, EFUSE_MAX_WORD_UNIT, sizeof(u16));
+	if (!eFuseWord)
+		goto exit;
+
+	/*  0. Refresh efuse init map as all oxFF. */
+	for (i = 0; i < EFUSE_MAX_SECTION_88E; i++)
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
+			eFuseWord[i][j] = 0xFFFF;
+
+	/*  */
+	/*  1. Read the first byte to check if efuse is empty!!! */
+	/*  */
+	/*  */
+	ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+	if (*rtemp8 != 0xFF) {
+		efuse_utilized++;
+		eFuse_Addr++;
+	} else {
+		goto exit;
+	}
+
+	/*  */
+	/*  2. Read real efuse content. Filter PG header and every section data. */
+	/*  */
+	while ((*rtemp8 != 0xFF) && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E)) {
+		/*  Check PG header for section num. */
+		if ((*rtemp8 & 0x1F) == 0x0F) {		/* extended header */
+			u1temp = ((*rtemp8 & 0xE0) >> 5);
+
+			ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+
+			if ((*rtemp8 & 0x0F) == 0x0F) {
+				eFuse_Addr++;
+				ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+
+				if (*rtemp8 != 0xFF && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E))
+					eFuse_Addr++;
+				continue;
+			} else {
+				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
+				wren = (*rtemp8 & 0x0F);
+				eFuse_Addr++;
+			}
+		} else {
+			offset = ((*rtemp8 >> 4) & 0x0f);
+			wren = (*rtemp8 & 0x0f);
+		}
+
+		if (offset < EFUSE_MAX_SECTION_88E) {
+			/*  Get word enable value from PG header */
+
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				/*  Check word enable condition in the section */
+				if (!(wren & 0x01)) {
+					ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+					eFuse_Addr++;
+					efuse_utilized++;
+					eFuseWord[offset][i] = (*rtemp8 & 0xff);
+					if (eFuse_Addr >= EFUSE_REAL_CONTENT_LEN_88E)
+						break;
+					ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+					eFuse_Addr++;
+					efuse_utilized++;
+					eFuseWord[offset][i] |= (((u16)*rtemp8 << 8) & 0xff00);
+					if (eFuse_Addr >= EFUSE_REAL_CONTENT_LEN_88E)
+						break;
+				}
+				wren >>= 1;
+			}
+		}
+
+		/*  Read next PG header */
+		ReadEFuseByte(Adapter, eFuse_Addr, rtemp8);
+
+		if (*rtemp8 != 0xFF && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E)) {
+			efuse_utilized++;
+			eFuse_Addr++;
+		}
+	}
+
+	/*  3. Collect 16 sections and 4 word unit into Efuse map. */
+	for (i = 0; i < EFUSE_MAX_SECTION_88E; i++) {
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
+			efuseTbl[(i * 8) + (j * 2)] = (eFuseWord[i][j] & 0xff);
+			efuseTbl[(i * 8) + ((j * 2) + 1)] = ((eFuseWord[i][j] >> 8) & 0xff);
+		}
+	}
+
+	/*  4. Copy from Efuse map to output pointer memory!!! */
+	for (i = 0; i < _size_byte; i++)
+		pbuf[i] = efuseTbl[_offset + i];
+
+exit:
+	kfree(efuseTbl);
+	kfree(eFuseWord);
+}
+
+void rtl8188e_ReadEFuse(struct adapter *Adapter, u16 _size_byte, u8 *pbuf)
+{
+	int ret = _FAIL;
+	if (rtw_IOL_applied(Adapter)) {
+		rtl8188eu_InitPowerOn(Adapter);
+
+		iol_mode_enable(Adapter, 1);
+		ret = iol_read_efuse(Adapter, _size_byte, pbuf);
+		iol_mode_enable(Adapter, 0);
+
+		if (_SUCCESS == ret)
+			return;
+	}
+
+	Hal_EfuseReadEFuse88E(Adapter, 0, _size_byte, pbuf);
+}
+
+static void dump_chip_info(struct adapter *adapter, struct HAL_VERSION chip_vers)
+{
+	struct net_device *netdev = adapter->pnetdev;
+	char *cut = NULL;
+	char buf[25];
+
+	switch (chip_vers.CUTVersion) {
+	case A_CUT_VERSION:
+		cut = "A_CUT";
+		break;
+	case B_CUT_VERSION:
+		cut = "B_CUT";
+		break;
+	case C_CUT_VERSION:
+		cut = "C_CUT";
+		break;
+	case D_CUT_VERSION:
+		cut = "D_CUT";
+		break;
+	case E_CUT_VERSION:
+		cut = "E_CUT";
+		break;
+	default:
+		snprintf(buf, sizeof(buf), "UNKNOWN_CUT(%d)", chip_vers.CUTVersion);
+		cut = buf;
+		break;
+	}
+
+	netdev_dbg(netdev, "Chip Version Info: CHIP_8188E_%s_%s_%s_1T1R_RomVer(%d)\n",
+		   IS_NORMAL_CHIP(chip_vers) ? "Normal_Chip" : "Test_Chip",
+		   IS_CHIP_VENDOR_TSMC(chip_vers) ? "TSMC" : "UMC",
+		   cut, 0);
+}
+
+void rtl8188e_read_chip_version(struct adapter *padapter)
+{
+	u32				value32;
+	struct HAL_VERSION		ChipVersion;
+	struct hal_data_8188e *pHalData = &padapter->haldata;
+	int res;
+
+	res = rtw_read32(padapter, REG_SYS_CFG, &value32);
+	if (res)
+		return;
+
+	ChipVersion.ChipType = ((value32 & RTL_ID) ? TEST_CHIP : NORMAL_CHIP);
+
+	ChipVersion.VendorType = ((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : CHIP_VENDOR_TSMC);
+	ChipVersion.CUTVersion = (value32 & CHIP_VER_RTL_MASK) >> CHIP_VER_RTL_SHIFT; /*  IC version (CUT) */
+
+	dump_chip_info(padapter, ChipVersion);
+
+	pHalData->VersionID = ChipVersion;
+}
+
+void rtl8188e_SetHalODMVar(struct adapter *Adapter, void *pValue1, bool bSet)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct odm_dm_struct *podmpriv = &pHalData->odmpriv;
+	struct sta_info *psta = (struct sta_info *)pValue1;
+
+	if (bSet) {
+		podmpriv->pODM_StaInfo[psta->mac_id] = psta;
+		ODM_RAInfo_Init(podmpriv, psta->mac_id);
+	} else {
+		podmpriv->pODM_StaInfo[psta->mac_id] = NULL;
+	}
+}
+
+void hal_notch_filter_8188e(struct adapter *adapter, bool enable)
+{
+	int res;
+	u8 reg;
+
+	res = rtw_read8(adapter, rOFDM0_RxDSP + 1, &reg);
+	if (res)
+		return;
+
+	if (enable)
+		rtw_write8(adapter, rOFDM0_RxDSP + 1, reg | BIT(1));
+	else
+		rtw_write8(adapter, rOFDM0_RxDSP + 1, reg & ~BIT(1));
+}
+
+/*  */
+/*  */
+/*  LLT R/W/Init function */
+/*  */
+/*  */
+static s32 _LLTWrite(struct adapter *padapter, u32 address, u32 data)
+{
+	s32	count;
+	u32	value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) | _LLT_OP(_LLT_WRITE_ACCESS);
+	u16	LLTReg = REG_LLT_INIT;
+	int res;
+
+	rtw_write32(padapter, LLTReg, value);
+
+	/* polling */
+	for (count = 0; count <= POLLING_LLT_THRESHOLD; count++) {
+		res = rtw_read32(padapter, LLTReg, &value);
+		if (res)
+			continue;
+
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+	}
+
+	return count > POLLING_LLT_THRESHOLD ? _FAIL : _SUCCESS;
+}
+
+s32 InitLLTTable(struct adapter *padapter, u8 txpktbuf_bndy)
+{
+	s32	status = _FAIL;
+	u32	i;
+	u32	Last_Entry_Of_TxPktBuf = LAST_ENTRY_OF_TX_PKT_BUFFER;/*  176, 22k */
+
+	if (rtw_IOL_applied(padapter)) {
+		status = iol_InitLLTTable(padapter, txpktbuf_bndy);
+	} else {
+		for (i = 0; i < (txpktbuf_bndy - 1); i++) {
+			status = _LLTWrite(padapter, i, i + 1);
+			if (_SUCCESS != status)
+				return status;
+		}
+
+		/*  end of list */
+		status = _LLTWrite(padapter, (txpktbuf_bndy - 1), 0xFF);
+		if (_SUCCESS != status)
+			return status;
+
+		/*  Make the other pages as ring buffer */
+		/*  This ring buffer is used as beacon buffer if we config this MAC as two MAC transfer. */
+		/*  Otherwise used as local loopback buffer. */
+		for (i = txpktbuf_bndy; i < Last_Entry_Of_TxPktBuf; i++) {
+			status = _LLTWrite(padapter, i, (i + 1));
+			if (_SUCCESS != status)
+				return status;
+		}
+
+		/*  Let last entry point to the start entry of ring buffer */
+		status = _LLTWrite(padapter, Last_Entry_Of_TxPktBuf, txpktbuf_bndy);
+		if (_SUCCESS != status) {
+			return status;
+		}
+	}
+
+	return status;
+}
+
+void
+Hal_EfuseParseIDCode88E(
+		struct adapter *padapter,
+		u8 *hwinfo
+	)
+{
+	struct eeprom_priv *pEEPROM = &padapter->eeprompriv;
+	struct net_device *netdev = padapter->pnetdev;
+	u16			EEPROMId;
+
+	/*  Check 0x8129 again for making sure autoload status!! */
+	EEPROMId = le16_to_cpu(*((__le16 *)hwinfo));
+	if (EEPROMId != RTL_EEPROM_ID) {
+		pr_err("EEPROM ID(%#x) is invalid!!\n", EEPROMId);
+		pEEPROM->bautoload_fail_flag = true;
+	} else {
+		pEEPROM->bautoload_fail_flag = false;
+	}
+
+	netdev_dbg(netdev, "EEPROM ID = 0x%04x\n", EEPROMId);
+}
+
+static void Hal_ReadPowerValueFromPROM_8188E(struct txpowerinfo24g *pwrInfo24G, u8 *PROMContent, bool AutoLoadFail)
+{
+	u32 rfPath, eeAddr = EEPROM_TX_PWR_INX_88E, group, TxCount = 0;
+
+	memset(pwrInfo24G, 0, sizeof(struct txpowerinfo24g));
+
+	if (AutoLoadFail) {
+		for (rfPath = 0; rfPath < RF_PATH_MAX; rfPath++) {
+			/* 2.4G default value */
+			for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+				pwrInfo24G->IndexCCK_Base[rfPath][group] =	EEPROM_DEFAULT_24G_INDEX;
+				pwrInfo24G->IndexBW40_Base[rfPath][group] =	EEPROM_DEFAULT_24G_INDEX;
+			}
+			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+				if (TxCount == 0) {
+					pwrInfo24G->BW20_Diff[rfPath][0] = EEPROM_DEFAULT_24G_HT20_DIFF;
+					pwrInfo24G->OFDM_Diff[rfPath][0] = EEPROM_DEFAULT_24G_OFDM_DIFF;
+				} else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				}
+			}
+		}
+		return;
+	}
+
+	for (rfPath = 0; rfPath < RF_PATH_MAX; rfPath++) {
+		/* 2.4G default value */
+		for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+			pwrInfo24G->IndexCCK_Base[rfPath][group] =	PROMContent[eeAddr++];
+			if (pwrInfo24G->IndexCCK_Base[rfPath][group] == 0xFF)
+				pwrInfo24G->IndexCCK_Base[rfPath][group] = EEPROM_DEFAULT_24G_INDEX;
+		}
+		for (group = 0; group < MAX_CHNL_GROUP_24G - 1; group++) {
+			pwrInfo24G->IndexBW40_Base[rfPath][group] =	PROMContent[eeAddr++];
+			if (pwrInfo24G->IndexBW40_Base[rfPath][group] == 0xFF)
+				pwrInfo24G->IndexBW40_Base[rfPath][group] =	EEPROM_DEFAULT_24G_INDEX;
+		}
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (TxCount == 0) {
+				pwrInfo24G->BW40_Diff[rfPath][TxCount] = 0;
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = EEPROM_DEFAULT_24G_HT20_DIFF;
+				} else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = (PROMContent[eeAddr] & 0xf0) >> 4;
+					if (pwrInfo24G->BW20_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW20_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_24G_OFDM_DIFF;
+				} else {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] =	(PROMContent[eeAddr] & 0x0f);
+					if (pwrInfo24G->OFDM_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->OFDM_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				pwrInfo24G->CCK_Diff[rfPath][TxCount] = 0;
+				eeAddr++;
+			} else {
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_DIFF;
+				} else {
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] =	(PROMContent[eeAddr] & 0xf0) >> 4;
+					if (pwrInfo24G->BW40_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW40_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_DIFF;
+				} else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] =	(PROMContent[eeAddr] & 0x0f);
+					if (pwrInfo24G->BW20_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW20_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				} else {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] =	(PROMContent[eeAddr] & 0xf0) >> 4;
+					if (pwrInfo24G->OFDM_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->OFDM_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF) {
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_DIFF;
+				} else {
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] =	(PROMContent[eeAddr] & 0x0f);
+					if (pwrInfo24G->CCK_Diff[rfPath][TxCount] & BIT(3))		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->CCK_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+			}
+		}
+	}
+}
+
+static void hal_get_chnl_group_88e(u8 chnl, u8 *group)
+{
+	if (chnl < 3)			/*  Channel 1-2 */
+		*group = 0;
+	else if (chnl < 6)		/*  Channel 3-5 */
+		*group = 1;
+	else if (chnl < 9)		/*  Channel 6-8 */
+		*group = 2;
+	else if (chnl < 12)		/*  Channel 9-11 */
+		*group = 3;
+	else if (chnl < 14)		/*  Channel 12-13 */
+		*group = 4;
+	else if (chnl == 14)		/*  Channel 14 */
+		*group = 5;
+}
+
+void Hal_ReadPowerSavingMode88E(struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail)
+{
+	if (AutoLoadFail)
+		padapter->pwrctrlpriv.bSupportRemoteWakeup = false;
+	else
+		/* hw power down mode selection , 0:rf-off / 1:power down */
+
+		/*  decide hw if support remote wakeup function */
+		/*  if hw supported, 8051 (SIE) will generate WeakUP signal(D+/D- toggle) when autoresume */
+		padapter->pwrctrlpriv.bSupportRemoteWakeup = (hwinfo[EEPROM_USB_OPTIONAL_FUNCTION0] & BIT(1)) ? true : false;
+}
+
+void Hal_ReadTxPowerInfo88E(struct adapter *padapter, u8 *PROMContent, bool AutoLoadFail)
+{
+	struct hal_data_8188e *pHalData = &padapter->haldata;
+	struct txpowerinfo24g pwrInfo24G;
+	u8 ch, group;
+	u8 TxCount;
+
+	Hal_ReadPowerValueFromPROM_8188E(&pwrInfo24G, PROMContent, AutoLoadFail);
+
+	for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++) {
+		hal_get_chnl_group_88e(ch, &group);
+
+		pHalData->Index24G_CCK_Base[ch] = pwrInfo24G.IndexCCK_Base[0][group];
+		if (ch == 14)
+			pHalData->Index24G_BW40_Base[ch] = pwrInfo24G.IndexBW40_Base[0][4];
+		else
+			pHalData->Index24G_BW40_Base[ch] = pwrInfo24G.IndexBW40_Base[0][group];
+	}
+	for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+		pHalData->OFDM_24G_Diff[TxCount] = pwrInfo24G.OFDM_Diff[0][TxCount];
+		pHalData->BW20_24G_Diff[TxCount] = pwrInfo24G.BW20_Diff[0][TxCount];
+	}
+
+	/*  2010/10/19 MH Add Regulator recognize for CU. */
+	if (!AutoLoadFail) {
+		pHalData->EEPROMRegulatory = (PROMContent[EEPROM_RF_BOARD_OPTION_88E] & 0x7);	/* bit0~2 */
+		if (PROMContent[EEPROM_RF_BOARD_OPTION_88E] == 0xFF)
+			pHalData->EEPROMRegulatory = (EEPROM_DEFAULT_BOARD_OPTION & 0x7);	/* bit0~2 */
+	} else {
+		pHalData->EEPROMRegulatory = 0;
+	}
+}
+
+void Hal_EfuseParseXtal_8188E(struct adapter *pAdapter, u8 *hwinfo, bool AutoLoadFail)
+{
+	struct hal_data_8188e *pHalData = &pAdapter->haldata;
+
+	if (!AutoLoadFail) {
+		pHalData->CrystalCap = hwinfo[EEPROM_XTAL_88E];
+		if (pHalData->CrystalCap == 0xFF)
+			pHalData->CrystalCap = EEPROM_Default_CrystalCap_88E;
+	} else {
+		pHalData->CrystalCap = EEPROM_Default_CrystalCap_88E;
+	}
+}
+
+void rtl8188e_EfuseParseChnlPlan(struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail)
+{
+	padapter->mlmepriv.ChannelPlan =
+		 hal_com_get_channel_plan(padapter,
+					  hwinfo ? hwinfo[EEPROM_ChannelPlan_88E] : 0xFF,
+					  padapter->registrypriv.channel_plan,
+					  RT_CHANNEL_DOMAIN_WORLD_WIDE_13, AutoLoadFail);
+}
+
+void Hal_ReadAntennaDiversity88E(struct adapter *pAdapter, u8 *PROMContent, bool AutoLoadFail)
+{
+	struct hal_data_8188e *pHalData = &pAdapter->haldata;
+	struct registry_priv	*registry_par = &pAdapter->registrypriv;
+
+	if (!AutoLoadFail) {
+		/*  Antenna Diversity setting. */
+		if (registry_par->antdiv_cfg == 2) { /*  2:By EFUSE */
+			pHalData->AntDivCfg = (PROMContent[EEPROM_RF_BOARD_OPTION_88E] & 0x18) >> 3;
+			if (PROMContent[EEPROM_RF_BOARD_OPTION_88E] == 0xFF)
+				pHalData->AntDivCfg = (EEPROM_DEFAULT_BOARD_OPTION & 0x18) >> 3;
+		} else {
+			pHalData->AntDivCfg = registry_par->antdiv_cfg;  /*  0:OFF , 1:ON, 2:By EFUSE */
+		}
+
+		if (registry_par->antdiv_type == 0) {
+			/* If TRxAntDivType is AUTO in advanced setting, use EFUSE value instead. */
+			pHalData->TRxAntDivType = PROMContent[EEPROM_RF_ANTENNA_OPT_88E];
+			if (pHalData->TRxAntDivType == 0xFF)
+				pHalData->TRxAntDivType = CG_TRX_HW_ANTDIV; /*  For 88EE, 1Tx and 1RxCG are fixed.(1Ant, Tx and RxCG are both on aux port) */
+		} else {
+			pHalData->TRxAntDivType = registry_par->antdiv_type;
+		}
+
+		if (pHalData->TRxAntDivType == CG_TRX_HW_ANTDIV || pHalData->TRxAntDivType == CGCS_RX_HW_ANTDIV)
+			pHalData->AntDivCfg = 1; /*  0xC1[3] is ignored. */
+	} else {
+		pHalData->AntDivCfg = 0;
+	}
+}
+
+void Hal_ReadThermalMeter_88E(struct adapter *Adapter, u8 *PROMContent, bool AutoloadFail)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	/*  ThermalMeter from EEPROM */
+	if (!AutoloadFail)
+		pHalData->EEPROMThermalMeter = PROMContent[EEPROM_THERMAL_METER_88E];
+	else
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_88E;
+
+	if (pHalData->EEPROMThermalMeter == 0xff || AutoloadFail)
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_88E;
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_phycfg.c b/drivers/staging/r8188eu/hal/rtl8188e_phycfg.c
new file mode 100644
index 000000000..532c63bce
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_phycfg.c
@@ -0,0 +1,704 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTL8188E_PHYCFG_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_iol.h"
+#include "../include/rtl8188e_hal.h"
+
+/*  */
+/*  1. BB register R/W API */
+/*  */
+
+/* Get shifted position of the bit mask */
+static u32 phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i = ffs(bitmask);
+
+	return i ? i - 1 : 32;
+}
+
+/**
+* Function:	PHY_QueryBBReg
+*
+* OverView:	Read "sepcific bits" from BB register
+*
+* Input:
+*			struct adapter *Adapter,
+*			u32			RegAddr,	The target address to be readback
+*			u32			BitMask		The target bit position in the target address
+*								to be readback
+* Output:	None
+* Return:		u32			Data		The readback register value
+* Note:		This function is equal to "GetRegSetting" in PHY programming guide
+*/
+u32
+rtl8188e_PHY_QueryBBReg(
+		struct adapter *Adapter,
+		u32 RegAddr,
+		u32 BitMask
+	)
+{
+	u32 ReturnValue = 0, OriginalValue, BitShift;
+	int res;
+
+	res = rtw_read32(Adapter, RegAddr, &OriginalValue);
+	if (res)
+		return 0;
+
+	BitShift = phy_calculate_bit_shift(BitMask);
+	ReturnValue = (OriginalValue & BitMask) >> BitShift;
+	return ReturnValue;
+}
+
+/**
+* Function:	PHY_SetBBReg
+*
+* OverView:	Write "Specific bits" to BB register (page 8~)
+*
+* Input:
+*			struct adapter *Adapter,
+*			u32			RegAddr,	The target address to be modified
+*			u32			BitMask		The target bit position in the target address
+*									to be modified
+*			u32			Data		The new register value in the target bit position
+*									of the target address
+*
+* Output:	None
+* Return:		None
+* Note:		This function is equal to "PutRegSetting" in PHY programming guide
+*/
+
+void rtl8188e_PHY_SetBBReg(struct adapter *Adapter, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	u32 OriginalValue, BitShift;
+	int res;
+
+	if (BitMask != bMaskDWord) { /* if not "double word" write */
+		res = rtw_read32(Adapter, RegAddr, &OriginalValue);
+		if (res)
+			return;
+
+		BitShift = phy_calculate_bit_shift(BitMask);
+		Data = ((OriginalValue & (~BitMask)) | (Data << BitShift));
+	}
+
+	rtw_write32(Adapter, RegAddr, Data);
+}
+
+/*  */
+/*  2. RF register R/W API */
+/*  */
+/**
+* Function:	phy_RFSerialRead
+*
+* OverView:	Read regster from RF chips
+*
+* Input:
+*			struct adapter *Adapter,
+*			u32			Offset,		The target address to be read
+*
+* Output:	None
+* Return:		u32			reback value
+* Note:		Threre are three types of serial operations:
+*			1. Software serial write
+*			2. Hardware LSSI-Low Speed Serial Interface
+*			3. Hardware HSSI-High speed
+*			serial write. Driver need to implement (1) and (2).
+*			This function is equal to the combination of RF_ReadReg() and  RFLSSIRead()
+*/
+static	u32
+phy_RFSerialRead(
+		struct adapter *Adapter,
+		u32 Offset
+	)
+{
+	u32 retValue = 0;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct bb_reg_def *pPhyReg = &pHalData->PHYRegDef;
+	u32 NewOffset;
+	u32 tmplong, tmplong2;
+	u8 	RfPiEnable = 0;
+	/*  */
+	/*  Make sure RF register offset is correct */
+	/*  */
+	Offset &= 0xff;
+
+	/*  */
+	/*  Switch page for 8256 RF IC */
+	/*  */
+	NewOffset = Offset;
+
+	/*  For 92S LSSI Read RFLSSIRead */
+	/*  For RF A/B write 0x824/82c(does not work in the future) */
+	/*  We must use 0x824 for RF A and B to execute read trigger */
+	tmplong = rtl8188e_PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord);
+	tmplong2 = tmplong;
+
+	tmplong2 = (tmplong2 & (~bLSSIReadAddress)) | (NewOffset << 23) | bLSSIReadEdge;	/* T65 RF */
+
+	rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord, tmplong & (~bLSSIReadEdge));
+	udelay(10);/*  PlatformStallExecution(10); */
+
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, bMaskDWord, tmplong2);
+	udelay(100);/* PlatformStallExecution(100); */
+
+	udelay(10);/* PlatformStallExecution(10); */
+
+	RfPiEnable = (u8)rtl8188e_PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter1, BIT(8));
+
+	if (RfPiEnable) {	/*  Read from BBreg8b8, 12 bits for 8190, 20bits for T65 RF */
+		retValue = rtl8188e_PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBackPi, bLSSIReadBackData);
+	} else {	/* Read from BBreg8a0, 12 bits for 8190, 20 bits for T65 RF */
+		retValue = rtl8188e_PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBack, bLSSIReadBackData);
+	}
+	return retValue;
+}
+
+/**
+* Function:	phy_RFSerialWrite
+*
+* OverView:	Write data to RF register (page 8~)
+*
+* Input:
+*			struct adapter *Adapter,
+*			enum rf_radio_path eRFPath,	Radio path of A/B/C/D
+*			u32			Offset,		The target address to be read
+*			u32			Data		The new register Data in the target bit position
+*									of the target to be read
+*
+* Output:	None
+* Return:		None
+* Note:		Threre are three types of serial operations:
+*			1. Software serial write
+*			2. Hardware LSSI-Low Speed Serial Interface
+*			3. Hardware HSSI-High speed
+*			serial write. Driver need to implement (1) and (2).
+*			This function is equal to the combination of RF_ReadReg() and  RFLSSIRead()
+ *
+ * Note:		  For RF8256 only
+ *			 The total count of RTL8256(Zebra4) register is around 36 bit it only employs
+ *			 4-bit RF address. RTL8256 uses "register mode control bit" (Reg00[12], Reg00[10])
+ *			 to access register address bigger than 0xf. See "Appendix-4 in PHY Configuration
+ *			 programming guide" for more details.
+ *			 Thus, we define a sub-finction for RTL8526 register address conversion
+ *		       ===========================================================
+ *			 Register Mode		RegCTL[1]		RegCTL[0]		Note
+ *								(Reg00[12])		(Reg00[10])
+ *		       ===========================================================
+ *			 Reg_Mode0				0				x			Reg 0 ~15(0x0 ~ 0xf)
+ *		       ------------------------------------------------------------------
+ *			 Reg_Mode1				1				0			Reg 16 ~30(0x1 ~ 0xf)
+ *		       ------------------------------------------------------------------
+ *			 Reg_Mode2				1				1			Reg 31 ~ 45(0x1 ~ 0xf)
+ *		       ------------------------------------------------------------------
+ *
+ *	2008/09/02	MH	Add 92S RF definition
+ *
+ *
+ *
+*/
+static	void
+phy_RFSerialWrite(
+		struct adapter *Adapter,
+		u32 Offset,
+		u32 Data
+	)
+{
+	u32 DataAndAddr = 0;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct bb_reg_def *pPhyReg = &pHalData->PHYRegDef;
+	u32 NewOffset;
+
+	/*  2009/06/17 MH We can not execute IO for power save or other accident mode. */
+
+	Offset &= 0xff;
+
+	/*  */
+	/*  Switch page for 8256 RF IC */
+	/*  */
+	NewOffset = Offset;
+
+	/*  */
+	/*  Put write addr in [5:0]  and write data in [31:16] */
+	/*  */
+	DataAndAddr = ((NewOffset << 20) | (Data & 0x000fffff)) & 0x0fffffff;	/*  T65 RF */
+
+	/*  */
+	/*  Write Operation */
+	/*  */
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
+}
+
+/**
+* Function:	PHY_QueryRFReg
+*
+* OverView:	Query "Specific bits" to RF register (page 8~)
+*
+* Input:
+*			struct adapter *Adapter,
+*			u32			RegAddr,	The target address to be read
+*			u32			BitMask		The target bit position in the target address
+*									to be read
+*
+* Output:	None
+* Return:		u32			Readback value
+* Note:		This function is equal to "GetRFRegSetting" in PHY programming guide
+*/
+u32 rtl8188e_PHY_QueryRFReg(struct adapter *Adapter, u32 RegAddr, u32 BitMask)
+{
+	u32 Original_Value, Readback_Value, BitShift;
+
+	Original_Value = phy_RFSerialRead(Adapter, RegAddr);
+
+	BitShift =  phy_calculate_bit_shift(BitMask);
+	Readback_Value = (Original_Value & BitMask) >> BitShift;
+	return Readback_Value;
+}
+
+/**
+* Function:	PHY_SetRFReg
+*
+* OverView:	Write "Specific bits" to RF register (page 8~)
+*
+* Input:
+*			struct adapter *Adapter,
+*			u32			RegAddr,	The target address to be modified
+*			u32			BitMask		The target bit position in the target address
+*									to be modified
+*			u32			Data		The new register Data in the target bit position
+*									of the target address
+*
+* Output:	None
+* Return:		None
+* Note:		This function is equal to "PutRFRegSetting" in PHY programming guide
+*/
+void
+rtl8188e_PHY_SetRFReg(
+		struct adapter *Adapter,
+		u32 RegAddr,
+		u32 BitMask,
+		u32 Data
+	)
+{
+	u32 Original_Value, BitShift;
+
+	/*  RF data is 12 bits only */
+	if (BitMask != bRFRegOffsetMask) {
+		Original_Value = phy_RFSerialRead(Adapter, RegAddr);
+		BitShift =  phy_calculate_bit_shift(BitMask);
+		Data = ((Original_Value & (~BitMask)) | (Data << BitShift));
+	}
+
+	phy_RFSerialWrite(Adapter, RegAddr, Data);
+}
+
+/*  */
+/*  3. Initial MAC/BB/RF config by reading MAC/BB/RF txt. */
+/*  */
+
+/*-----------------------------------------------------------------------------
+ * Function:    PHY_MACConfig8192C
+ *
+ * Overview:	Condig MAC by header file or parameter file.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ *  When		Who		Remark
+ *  08/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+s32 PHY_MACConfig8188E(struct adapter *Adapter)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	int rtStatus = _SUCCESS;
+
+	/*  */
+	/*  Config MAC */
+	/*  */
+	if (ODM_ReadAndConfig_MAC_REG_8188E(&pHalData->odmpriv))
+		rtStatus = _FAIL;
+
+	/*  2010.07.13 AMPDU aggregation number B */
+	rtw_write16(Adapter, REG_MAX_AGGR_NUM, MAX_AGGR_NUM);
+
+	return rtStatus;
+}
+
+/**
+* Function:	phy_InitBBRFRegisterDefinition
+*
+* OverView:	Initialize Register definition offset for Radio Path A/B/C/D
+*
+* Input:
+*			struct adapter *Adapter,
+*
+* Output:	None
+* Return:		None
+* Note:		The initialization value is constant and it should never be changes
+*/
+static	void
+phy_InitBBRFRegisterDefinition(
+		struct adapter *Adapter
+)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	/*  RF Interface Sowrtware Control */
+	pHalData->PHYRegDef.rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 LSBs if read 32-bit from 0x870 */
+
+	/*  RF Interface Readback Value */
+	pHalData->PHYRegDef.rfintfi = rFPGA0_XAB_RFInterfaceRB; /*  16 LSBs if read 32-bit from 0x8E0 */
+
+	/*  RF Interface Output (and Enable) */
+	pHalData->PHYRegDef.rfintfo = rFPGA0_XA_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x860 */
+
+	/*  RF Interface (Output and)  Enable */
+	pHalData->PHYRegDef.rfintfe = rFPGA0_XA_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x860 (16-bit for 0x862) */
+
+	/* Addr of LSSI. Wirte RF register by driver */
+	pHalData->PHYRegDef.rf3wireOffset = rFPGA0_XA_LSSIParameter; /* LSSI Parameter */
+
+	/*  RF parameter */
+	pHalData->PHYRegDef.rfLSSI_Select = rFPGA0_XAB_RFParameter;  /* BB Band Select */
+
+	/*  Tx AGC Gain Stage (same for all path. Should we remove this?) */
+	pHalData->PHYRegDef.rfTxGainStage = rFPGA0_TxGainStage; /* Tx gain stage */
+
+	/*  Transceiver A~D HSSI Parameter-1 */
+	pHalData->PHYRegDef.rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;  /* wire control parameter1 */
+
+	/*  Transceiver A~D HSSI Parameter-2 */
+	pHalData->PHYRegDef.rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;  /* wire control parameter2 */
+
+	/*  RF switch Control */
+	pHalData->PHYRegDef.rfSwitchControl = rFPGA0_XAB_SwitchControl; /* TR/Ant switch control */
+
+	/*  AGC control 1 */
+	pHalData->PHYRegDef.rfAGCControl1 = rOFDM0_XAAGCCore1;
+
+	/*  AGC control 2 */
+	pHalData->PHYRegDef.rfAGCControl2 = rOFDM0_XAAGCCore2;
+
+	/*  RX AFE control 1 */
+	pHalData->PHYRegDef.rfRxIQImbalance = rOFDM0_XARxIQImbalance;
+
+	/*  RX AFE control 1 */
+	pHalData->PHYRegDef.rfRxAFE = rOFDM0_XARxAFE;
+
+	/*  Tx AFE control 1 */
+	pHalData->PHYRegDef.rfTxIQImbalance = rOFDM0_XATxIQImbalance;
+
+	/*  Tx AFE control 2 */
+	pHalData->PHYRegDef.rfTxAFE = rOFDM0_XATxAFE;
+
+	/*  Transceiver LSSI Readback SI mode */
+	pHalData->PHYRegDef.rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+
+	/*  Transceiver LSSI Readback PI mode */
+	pHalData->PHYRegDef.rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
+}
+
+void storePwrIndexDiffRateOffset(struct adapter *Adapter, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	if (RegAddr == rTxAGC_A_Rate18_06)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][0] = Data;
+	if (RegAddr == rTxAGC_A_Rate54_24)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][1] = Data;
+	if (RegAddr == rTxAGC_A_CCK1_Mcs32)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][6] = Data;
+	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0xffffff00)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][7] = Data;
+	if (RegAddr == rTxAGC_A_Mcs03_Mcs00)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][2] = Data;
+	if (RegAddr == rTxAGC_A_Mcs07_Mcs04)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][3] = Data;
+	if (RegAddr == rTxAGC_A_Mcs11_Mcs08)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][4] = Data;
+	if (RegAddr == rTxAGC_A_Mcs15_Mcs12) {
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][5] = Data;
+		pHalData->pwrGroupCnt++;
+	}
+	if (RegAddr == rTxAGC_B_Rate18_06)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][8] = Data;
+	if (RegAddr == rTxAGC_B_Rate54_24)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][9] = Data;
+	if (RegAddr == rTxAGC_B_CCK1_55_Mcs32)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][14] = Data;
+	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0x000000ff)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][15] = Data;
+	if (RegAddr == rTxAGC_B_Mcs03_Mcs00)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][10] = Data;
+	if (RegAddr == rTxAGC_B_Mcs07_Mcs04)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][11] = Data;
+	if (RegAddr == rTxAGC_B_Mcs11_Mcs08)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][12] = Data;
+	if (RegAddr == rTxAGC_B_Mcs15_Mcs12)
+		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][13] = Data;
+}
+
+static	int phy_BB8188E_Config_ParaFile(struct adapter *Adapter)
+{
+	struct eeprom_priv *pEEPROM = &Adapter->eeprompriv;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	/*  */
+	/*  1. Read PHY_REG.TXT BB INIT!! */
+	/*  We will separate as 88C / 92C according to chip version */
+	/*  */
+	if (ODM_ReadAndConfig_PHY_REG_1T_8188E(&pHalData->odmpriv))
+		return _FAIL;
+
+	/*  2. If EEPROM or EFUSE autoload OK, We must config by PHY_REG_PG.txt */
+	if (!pEEPROM->bautoload_fail_flag) {
+		pHalData->pwrGroupCnt = 0;
+		ODM_ReadAndConfig_PHY_REG_PG_8188E(&pHalData->odmpriv);
+	}
+
+	/*  3. BB AGC table Initialization */
+	if (ODM_ReadAndConfig_AGC_TAB_1T_8188E(&pHalData->odmpriv))
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+int
+PHY_BBConfig8188E(
+		struct adapter *Adapter
+	)
+{
+	int	rtStatus = _SUCCESS;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u16 RegVal;
+	u8 CrystalCap;
+	int res;
+
+	phy_InitBBRFRegisterDefinition(Adapter);
+
+	/*  Enable BB and RF */
+	res = rtw_read16(Adapter, REG_SYS_FUNC_EN, &RegVal);
+	if (res)
+		return _FAIL;
+
+	rtw_write16(Adapter, REG_SYS_FUNC_EN, (u16)(RegVal | BIT(13) | BIT(0) | BIT(1)));
+
+	/*  20090923 Joseph: Advised by Steven and Jenyu. Power sequence before init RF. */
+
+	rtw_write8(Adapter, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
+
+	rtw_write8(Adapter, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD | FEN_BB_GLB_RSTn | FEN_BBRSTB);
+
+	/*  Config BB and AGC */
+	rtStatus = phy_BB8188E_Config_ParaFile(Adapter);
+
+	/*  write 0x24[16:11] = 0x24[22:17] = CrystalCap */
+	CrystalCap = pHalData->CrystalCap & 0x3F;
+	rtl8188e_PHY_SetBBReg(Adapter, REG_AFE_XTAL_CTRL, 0x7ff800, (CrystalCap | (CrystalCap << 6)));
+
+	return rtStatus;
+}
+
+static void getTxPowerIndex88E(struct adapter *Adapter, u8 channel, u8 *cckPowerLevel,
+			       u8 *ofdmPowerLevel, u8 *BW20PowerLevel,
+			       u8 *BW40PowerLevel)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u8 index = (channel - 1);
+
+	/*  1. CCK */
+	cckPowerLevel[RF_PATH_A] = pHalData->Index24G_CCK_Base[index];
+	/* 2. OFDM */
+	ofdmPowerLevel[RF_PATH_A] = pHalData->Index24G_BW40_Base[index] +
+		pHalData->OFDM_24G_Diff[RF_PATH_A];
+	/*  1. BW20 */
+	BW20PowerLevel[RF_PATH_A] = pHalData->Index24G_BW40_Base[index] +
+		pHalData->BW20_24G_Diff[RF_PATH_A];
+	/* 2. BW40 */
+	BW40PowerLevel[RF_PATH_A] = pHalData->Index24G_BW40_Base[index];
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:    SetTxPowerLevel8190()
+ *
+ * Overview:    This function is export to "HalCommon" moudule
+ *			We must consider RF path later!!!!!!!
+ *
+ * Input:       struct adapter *Adapter
+ *			u8		channel
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *	2008/11/04	MHC		We remove EEPROM_93C56.
+ *						We need to move CCX relative code to independet file.
+ *	2009/01/21	MHC		Support new EEPROM format from SD3 requirement.
+ *
+ *---------------------------------------------------------------------------*/
+void
+PHY_SetTxPowerLevel8188E(
+		struct adapter *Adapter,
+		u8 channel
+	)
+{
+	u8 cckPowerLevel[MAX_TX_COUNT] = {0};
+	u8 ofdmPowerLevel[MAX_TX_COUNT] = {0};/*  [0]:RF-A, [1]:RF-B */
+	u8 BW20PowerLevel[MAX_TX_COUNT] = {0};
+	u8 BW40PowerLevel[MAX_TX_COUNT] = {0};
+
+	getTxPowerIndex88E(Adapter, channel, &cckPowerLevel[0], &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0]);
+
+	rtl8188e_PHY_RF6052SetCckTxPower(Adapter, &cckPowerLevel[0]);
+	rtl8188e_PHY_RF6052SetOFDMTxPower(Adapter, &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0], channel);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:    PHY_SetBWModeCallback8192C()
+ *
+ * Overview:    Timer callback function for SetSetBWMode
+ *
+ * Input:		PRT_TIMER		pTimer
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Note:		(1) We do not take j mode into consideration now
+ *			(2) Will two workitem of "switch channel" and "switch channel bandwidth" run
+ *			     concurrently?
+ *---------------------------------------------------------------------------*/
+static void
+_PHY_SetBWMode92C(
+		struct adapter *Adapter
+)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u8 regBwOpMode;
+	u8 regRRSR_RSC;
+	int res;
+
+	if (Adapter->bDriverStopped)
+		return;
+
+	/* 3 */
+	/* 3<1>Set MAC register */
+	/* 3 */
+
+	res = rtw_read8(Adapter, REG_BWOPMODE, &regBwOpMode);
+	if (res)
+		return;
+
+	res = rtw_read8(Adapter, REG_RRSR + 2, &regRRSR_RSC);
+	if (res)
+		return;
+
+	switch (pHalData->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		regBwOpMode |= BW_OPMODE_20MHZ;
+		/*  2007/02/07 Mark by Emily because we have not verify whether this register works */
+		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		regBwOpMode &= ~BW_OPMODE_20MHZ;
+		/*  2007/02/07 Mark by Emily because we have not verify whether this register works */
+		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		regRRSR_RSC = (regRRSR_RSC & 0x90) | (pHalData->nCur40MhzPrimeSC << 5);
+		rtw_write8(Adapter, REG_RRSR + 2, regRRSR_RSC);
+		break;
+	default:
+		break;
+	}
+
+	/* 3  */
+	/* 3 <2>Set PHY related register */
+	/* 3 */
+	switch (pHalData->CurrentChannelBW) {
+	/* 20 MHz channel*/
+	case HT_CHANNEL_WIDTH_20:
+		rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x0);
+		rtl8188e_PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x0);
+		break;
+	/* 40 MHz channel*/
+	case HT_CHANNEL_WIDTH_40:
+		rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x1);
+		rtl8188e_PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x1);
+		/*  Set Control channel to upper or lower. These settings are required only for 40MHz */
+		rtl8188e_PHY_SetBBReg(Adapter, rCCK0_System, bCCKSideBand, (pHalData->nCur40MhzPrimeSC >> 1));
+		rtl8188e_PHY_SetBBReg(Adapter, rOFDM1_LSTF, 0xC00, pHalData->nCur40MhzPrimeSC);
+		rtl8188e_PHY_SetBBReg(Adapter, 0x818, (BIT(26) | BIT(27)),
+			     (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		break;
+	}
+	/* Skip over setting of J-mode in BB register here. Default value is "None J mode". Emily 20070315 */
+
+	rtl8188e_PHY_RF6052SetBandwidth(Adapter, pHalData->CurrentChannelBW);
+}
+
+ /*-----------------------------------------------------------------------------
+ * Function:   SetBWMode8190Pci()
+ *
+ * Overview:  This function is export to "HalCommon" moudule
+ *
+ * Input:		struct adapter *Adapter
+ *			enum ht_channel_width Bandwidth	20M or 40M
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Note:		We do not take j mode into consideration now
+ *---------------------------------------------------------------------------*/
+void PHY_SetBWMode8188E(struct adapter *Adapter, enum ht_channel_width Bandwidth,	/*  20M or 40M */
+			unsigned char	Offset)		/*  Upper, Lower, or Don't care */
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	enum ht_channel_width tmpBW = pHalData->CurrentChannelBW;
+
+	pHalData->CurrentChannelBW = Bandwidth;
+
+	pHalData->nCur40MhzPrimeSC = Offset;
+
+	if ((!Adapter->bDriverStopped) && (!Adapter->bSurpriseRemoved))
+		_PHY_SetBWMode92C(Adapter);
+	else
+		pHalData->CurrentChannelBW = tmpBW;
+}
+
+static void _PHY_SwChnl8192C(struct adapter *Adapter, u8 channel)
+{
+	u32 param1, param2;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	/* s1. pre common command - CmdID_SetTxPowerLevel */
+	PHY_SetTxPowerLevel8188E(Adapter, channel);
+
+	/* s2. RF dependent command - CmdID_RF_WriteReg, param1=RF_CHNLBW, param2=channel */
+	param1 = RF_CHNLBW;
+	param2 = channel;
+	pHalData->RfRegChnlVal = ((pHalData->RfRegChnlVal & 0xfffffc00) | param2);
+	rtl8188e_PHY_SetRFReg(Adapter, param1, bRFRegOffsetMask, pHalData->RfRegChnlVal);
+}
+
+void PHY_SwChnl8188E(struct adapter *Adapter, u8 channel)
+{
+	/*  Call after initialization */
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	if (channel == 0)
+		channel = 1;
+
+	if ((!Adapter->bDriverStopped) && (!Adapter->bSurpriseRemoved)) {
+		pHalData->CurrentChannel = channel;
+		_PHY_SwChnl8192C(Adapter, channel);
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_rf6052.c b/drivers/staging/r8188eu/hal/rtl8188e_rf6052.c
new file mode 100644
index 000000000..e5ec6e563
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_rf6052.c
@@ -0,0 +1,406 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+/******************************************************************************
+ *
+ *
+ * Module:	rtl8192c_rf6052.c	( Source C File)
+ *
+ * Note:	Provide RF 6052 series relative API.
+ *
+ * Function:
+ *
+ * Export:
+ *
+ * Abbrev:
+ *
+ * History:
+ * Data			Who		Remark
+ *
+ * 09/25/2008	MHC		Create initial version.
+ * 11/05/2008	MHC		Add API for tw power setting.
+ *
+ *
+******************************************************************************/
+
+#define _RTL8188E_RF6052_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtl8188e_hal.h"
+
+/*-----------------------------------------------------------------------------
+ * Function:    PHY_RF6052SetBandwidth()
+ *
+ * Overview:    This function is called by SetBWModeCallback8190Pci() only
+ *
+ * Input:       struct adapter *Adapter
+ *			WIRELESS_BANDWIDTH_E	Bandwidth	20M or 40M
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Note:		For RF type 0222D
+ *---------------------------------------------------------------------------*/
+void rtl8188e_PHY_RF6052SetBandwidth(struct adapter *Adapter,
+				     enum ht_channel_width Bandwidth)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+
+	switch (Bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		pHalData->RfRegChnlVal = ((pHalData->RfRegChnlVal & 0xfffff3ff) | BIT(10) | BIT(11));
+		rtl8188e_PHY_SetRFReg(Adapter, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		pHalData->RfRegChnlVal = ((pHalData->RfRegChnlVal & 0xfffff3ff) | BIT(10));
+		rtl8188e_PHY_SetRFReg(Adapter, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal);
+		break;
+	default:
+		break;
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	PHY_RF6052SetCckTxPower
+ *
+ * Overview:
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/05/2008	MHC		Simulate 8192series..
+ *
+ *---------------------------------------------------------------------------*/
+
+void
+rtl8188e_PHY_RF6052SetCckTxPower(
+		struct adapter *Adapter,
+		u8 *pPowerlevel)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
+	u32 TxAGC[2] = {0, 0}, tmpval = 0, pwrtrac_value;
+	u8 idx1, idx2;
+	u8 *ptr;
+	u8 direction;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		TxAGC[RF_PATH_A] = 0x3f3f3f3f;
+		TxAGC[RF_PATH_B] = 0x3f3f3f3f;
+
+		for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+			TxAGC[idx1] =
+				pPowerlevel[idx1] | (pPowerlevel[idx1] << 8) |
+				(pPowerlevel[idx1] << 16) | (pPowerlevel[idx1] << 24);
+		}
+	} else {
+		for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+			TxAGC[idx1] =
+				pPowerlevel[idx1] | (pPowerlevel[idx1] << 8) |
+				(pPowerlevel[idx1] << 16) | (pPowerlevel[idx1] << 24);
+		}
+		if (pHalData->EEPROMRegulatory == 0) {
+			tmpval = (pHalData->MCSTxPowerLevelOriginalOffset[0][6]) +
+					(pHalData->MCSTxPowerLevelOriginalOffset[0][7] << 8);
+			TxAGC[RF_PATH_A] += tmpval;
+
+			tmpval = (pHalData->MCSTxPowerLevelOriginalOffset[0][14]) +
+					(pHalData->MCSTxPowerLevelOriginalOffset[0][15] << 24);
+			TxAGC[RF_PATH_B] += tmpval;
+		}
+	}
+	for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+		ptr = (u8 *)(&TxAGC[idx1]);
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	ODM_TxPwrTrackAdjust88E(&pHalData->odmpriv, 1, &direction, &pwrtrac_value);
+
+	if (direction == 1) {
+		/*  Increase TX power */
+		TxAGC[0] += pwrtrac_value;
+		TxAGC[1] += pwrtrac_value;
+	} else if (direction == 2) {
+		/*  Decrease TX power */
+		TxAGC[0] -=  pwrtrac_value;
+		TxAGC[1] -=  pwrtrac_value;
+	}
+
+	/*  rf-A cck tx power */
+	tmpval = TxAGC[RF_PATH_A] & 0xff;
+	rtl8188e_PHY_SetBBReg(Adapter, rTxAGC_A_CCK1_Mcs32, bMaskByte1, tmpval);
+	tmpval = TxAGC[RF_PATH_A] >> 8;
+	rtl8188e_PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
+
+	/*  rf-B cck tx power */
+	tmpval = TxAGC[RF_PATH_B] >> 24;
+	rtl8188e_PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte0, tmpval);
+	tmpval = TxAGC[RF_PATH_B] & 0x00ffffff;
+	rtl8188e_PHY_SetBBReg(Adapter, rTxAGC_B_CCK1_55_Mcs32, 0xffffff00, tmpval);
+}	/* PHY_RF6052SetCckTxPower */
+
+/*  */
+/*  powerbase0 for OFDM rates */
+/*  powerbase1 for HT MCS rates */
+/*  */
+static void getpowerbase88e(struct adapter *Adapter, u8 *pPowerLevelOFDM,
+			    u8 *pPowerLevelBW20, u8 *pPowerLevelBW40, u8 Channel, u32 *OfdmBase, u32 *MCSBase)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u32 powerBase0, powerBase1;
+	u8 i;
+
+	for (i = 0; i < 2; i++) {
+		powerBase0 = pPowerLevelOFDM[i];
+
+		powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) | (powerBase0 << 8) | powerBase0;
+		*(OfdmBase + i) = powerBase0;
+	}
+
+	/* Check HT20 to HT40 diff */
+	if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+		powerBase1 = pPowerLevelBW20[0];
+	else
+		powerBase1 = pPowerLevelBW40[0];
+	powerBase1 = (powerBase1 << 24) | (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
+	*MCSBase = powerBase1;
+}
+
+static void get_rx_power_val_by_reg(struct adapter *Adapter, u8 Channel,
+				    u8 index, u32 *powerBase0, u32 *powerBase1,
+				    u32 *pOutWriteVal)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u8	i, chnlGroup = 0, pwr_diff_limit[4], customer_pwr_limit;
+	s8	pwr_diff = 0;
+	u32	writeVal, customer_limit, rf;
+	u8	Regulatory = pHalData->EEPROMRegulatory;
+
+	/*  Index 0 & 1= legacy OFDM, 2-5=HT_MCS rate */
+
+	for (rf = 0; rf < 2; rf++) {
+		switch (Regulatory) {
+		case 0:	/*  Realtek better performance */
+				/*  increase power diff defined by Realtek for large power */
+			chnlGroup = 0;
+			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index + (rf ? 8 : 0)] +
+				((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			break;
+		case 1:	/*  Realtek regulatory */
+			/*  increase power diff defined by Realtek for regulatory */
+			if (pHalData->pwrGroupCnt == 1)
+				chnlGroup = 0;
+			if (pHalData->pwrGroupCnt >= MAX_PG_GROUP) {
+				if (Channel < 3)			/*  Channel 1-2 */
+					chnlGroup = 0;
+				else if (Channel < 6)		/*  Channel 3-5 */
+					chnlGroup = 1;
+				else	 if (Channel < 9)		/*  Channel 6-8 */
+					chnlGroup = 2;
+				else if (Channel < 12)		/*  Channel 9-11 */
+					chnlGroup = 3;
+				else if (Channel < 14)		/*  Channel 12-13 */
+					chnlGroup = 4;
+				else if (Channel == 14)		/*  Channel 14 */
+					chnlGroup = 5;
+			}
+			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index + (rf ? 8 : 0)] +
+					((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			break;
+		case 2:	/*  Better regulatory */
+				/*  don't increase any power diff */
+			writeVal = ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			break;
+		case 3:	/*  Customer defined power diff. */
+				/*  increase power diff defined by customer. */
+			chnlGroup = 0;
+
+			if (index < 2)
+				pwr_diff = pHalData->TxPwrLegacyHtDiff[rf][Channel - 1];
+			else if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				pwr_diff = pHalData->TxPwrHt20Diff[rf][Channel - 1];
+
+			if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_40)
+				customer_pwr_limit = pHalData->PwrGroupHT40[rf][Channel - 1];
+			else
+				customer_pwr_limit = pHalData->PwrGroupHT20[rf][Channel - 1];
+
+			if (pwr_diff >= customer_pwr_limit)
+				pwr_diff = 0;
+			else
+				pwr_diff = customer_pwr_limit - pwr_diff;
+
+			for (i = 0; i < 4; i++) {
+				pwr_diff_limit[i] = (u8)((pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index + (rf ? 8 : 0)] & (0x7f << (i * 8))) >> (i * 8));
+
+				if (pwr_diff_limit[i] > pwr_diff)
+					pwr_diff_limit[i] = pwr_diff;
+			}
+			customer_limit = (pwr_diff_limit[3] << 24) | (pwr_diff_limit[2] << 16) |
+					 (pwr_diff_limit[1] << 8) | (pwr_diff_limit[0]);
+			writeVal = customer_limit + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			break;
+		default:
+			chnlGroup = 0;
+			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index + (rf ? 8 : 0)] +
+					((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			break;
+		}
+
+		*(pOutWriteVal + rf) = writeVal;
+	}
+}
+static void writeOFDMPowerReg88E(struct adapter *Adapter, u8 index, u32 *pValue)
+{
+	u16 regoffset_a[6] = {
+		rTxAGC_A_Rate18_06, rTxAGC_A_Rate54_24,
+		rTxAGC_A_Mcs03_Mcs00, rTxAGC_A_Mcs07_Mcs04,
+		rTxAGC_A_Mcs11_Mcs08, rTxAGC_A_Mcs15_Mcs12};
+	u16 regoffset_b[6] = {
+		rTxAGC_B_Rate18_06, rTxAGC_B_Rate54_24,
+		rTxAGC_B_Mcs03_Mcs00, rTxAGC_B_Mcs07_Mcs04,
+		rTxAGC_B_Mcs11_Mcs08, rTxAGC_B_Mcs15_Mcs12};
+	u8 i, rf, pwr_val[4];
+	u32 writeVal;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		writeVal = pValue[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8)((writeVal & (0x7f << (i * 8))) >> (i * 8));
+			if (pwr_val[i]  > RF6052_MAX_TX_PWR)
+				pwr_val[i]  = RF6052_MAX_TX_PWR;
+		}
+		writeVal = (pwr_val[3] << 24) | (pwr_val[2] << 16) | (pwr_val[1] << 8) | pwr_val[0];
+
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+
+		rtl8188e_PHY_SetBBReg(Adapter, regoffset, bMaskDWord, writeVal);
+
+		/*  201005115 Joseph: Set Tx Power diff for Tx power training mechanism. */
+		if (regoffset == rTxAGC_A_Mcs07_Mcs04 || regoffset == rTxAGC_B_Mcs07_Mcs04) {
+			writeVal = pwr_val[3];
+			if (regoffset == rTxAGC_A_Mcs15_Mcs12 || regoffset == rTxAGC_A_Mcs07_Mcs04)
+				regoffset = 0xc90;
+			if (regoffset == rTxAGC_B_Mcs15_Mcs12 || regoffset == rTxAGC_B_Mcs07_Mcs04)
+				regoffset = 0xc98;
+			for (i = 0; i < 3; i++) {
+				if (i != 2)
+					writeVal = (writeVal > 8) ? (writeVal - 8) : 0;
+				else
+					writeVal = (writeVal > 6) ? (writeVal - 6) : 0;
+				rtw_write8(Adapter, (u32)(regoffset + i), (u8)writeVal);
+			}
+		}
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	PHY_RF6052SetOFDMTxPower
+ *
+ * Overview:	For legacy and HY OFDM, we must read EEPROM TX power index for
+ *			different channel and read original value in TX power register area from
+ *			0xe00. We increase offset and original value to be correct tx pwr.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/05/2008	MHC		Simulate 8192 series method.
+ * 01/06/2009	MHC		1. Prevent Path B tx power overflow or underflow dure to
+ *						A/B pwr difference or legacy/HT pwr diff.
+ *						2. We concern with path B legacy/HT OFDM difference.
+ * 01/22/2009	MHC		Support new EPRO format from SD3.
+ *
+ *---------------------------------------------------------------------------*/
+
+void
+rtl8188e_PHY_RF6052SetOFDMTxPower(
+		struct adapter *Adapter,
+		u8 *pPowerLevelOFDM,
+		u8 *pPowerLevelBW20,
+		u8 *pPowerLevelBW40,
+		u8 Channel)
+{
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u32 writeVal[2], powerBase0[2], powerBase1[2], pwrtrac_value;
+	u8 direction;
+	u8 index = 0;
+
+	getpowerbase88e(Adapter, pPowerLevelOFDM, pPowerLevelBW20, pPowerLevelBW40, Channel, &powerBase0[0], &powerBase1[0]);
+
+	/*  2012/04/23 MH According to power tracking value, we need to revise OFDM tx power. */
+	/*  This is ued to fix unstable power tracking mode. */
+	ODM_TxPwrTrackAdjust88E(&pHalData->odmpriv, 0, &direction, &pwrtrac_value);
+
+	for (index = 0; index < 6; index++) {
+		get_rx_power_val_by_reg(Adapter, Channel, index,
+					&powerBase0[0], &powerBase1[0],
+					&writeVal[0]);
+
+		if (direction == 1) {
+			writeVal[0] += pwrtrac_value;
+			writeVal[1] += pwrtrac_value;
+		} else if (direction == 2) {
+			writeVal[0] -= pwrtrac_value;
+			writeVal[1] -= pwrtrac_value;
+		}
+		writeOFDMPowerReg88E(Adapter, index, &writeVal[0]);
+	}
+}
+
+int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
+{
+	struct bb_reg_def *pPhyReg;
+	struct hal_data_8188e *pHalData = &Adapter->haldata;
+	u32 u4RegValue = 0;
+	int rtStatus = _SUCCESS;
+
+	/* Initialize RF */
+
+	pPhyReg = &pHalData->PHYRegDef;
+
+	/*----Store original RFENV control type----*/
+	u4RegValue = rtl8188e_PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV);
+
+	/*----Set RF_ENV enable----*/
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfintfe, bRFSI_RFENV << 16, 0x1);
+	udelay(1);/* PlatformStallExecution(1); */
+
+	/*----Set RF_ENV output high----*/
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
+	udelay(1);/* PlatformStallExecution(1); */
+
+	/* Set bit number of Address and Data for RF register */
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/*  Set 1 to 4 bits for 8255 */
+	udelay(1);/* PlatformStallExecution(1); */
+
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/*  Set 0 to 12  bits for 8255 */
+	udelay(1);/* PlatformStallExecution(1); */
+
+	/*----Initialize RF fom connfiguration file----*/
+	if (ODM_ReadAndConfig_RadioA_1T_8188E(&pHalData->odmpriv))
+		rtStatus = _FAIL;
+
+	/*----Restore RFENV control type----*/;
+	rtl8188e_PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
+
+	return rtStatus;
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188e_rxdesc.c b/drivers/staging/r8188eu/hal/rtl8188e_rxdesc.c
new file mode 100644
index 000000000..dff0cba75
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188e_rxdesc.c
@@ -0,0 +1,163 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTL8188E_REDESC_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtl8188e_hal.h"
+
+static void process_rssi(struct adapter *padapter, struct recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->attrib;
+	struct signal_stat *signal_stat = &padapter->recvpriv.signal_strength_data;
+
+	if (signal_stat->update_req) {
+		signal_stat->total_num = 0;
+		signal_stat->total_val = 0;
+		signal_stat->update_req = 0;
+	}
+
+	signal_stat->total_num++;
+	signal_stat->total_val  += pattrib->phy_info.SignalStrength;
+	signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+} /*  Process_UI_RSSI_8192C */
+
+static void process_link_qual(struct adapter *padapter, struct recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib;
+	struct signal_stat *signal_stat;
+
+	if (!prframe || !padapter)
+		return;
+
+	pattrib = &prframe->attrib;
+	signal_stat = &padapter->recvpriv.signal_qual_data;
+
+	if (signal_stat->update_req) {
+		signal_stat->total_num = 0;
+		signal_stat->total_val = 0;
+		signal_stat->update_req = 0;
+	}
+
+	signal_stat->total_num++;
+	signal_stat->total_val  += pattrib->phy_info.SignalQuality;
+	signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+}
+
+static void rtl8188e_process_phy_info(struct adapter *padapter, void *prframe)
+{
+	struct recv_frame *precvframe = (struct recv_frame *)prframe;
+
+	/*  Check RSSI */
+	process_rssi(padapter, precvframe);
+	/*  Check EVM */
+	process_link_qual(padapter,  precvframe);
+}
+
+void update_recvframe_attrib_88e(struct recv_frame *precvframe, struct recv_stat *prxstat)
+{
+	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
+	memset(pattrib, 0, sizeof(struct rx_pkt_attrib));
+
+	pattrib->crc_err = (le32_to_cpu(prxstat->rxdw0) >> 14) & 0x1;
+
+	pattrib->pkt_rpt_type = (le32_to_cpu(prxstat->rxdw3) >> 14) & 0x3;
+
+	if (pattrib->pkt_rpt_type == NORMAL_RX) {
+		pattrib->pkt_len = le32_to_cpu(prxstat->rxdw0) & 0x00003fff;
+		pattrib->drvinfo_sz = ((le32_to_cpu(prxstat->rxdw0) >> 16) & 0xf) * 8;
+
+		pattrib->physt = (le32_to_cpu(prxstat->rxdw0) >> 26) & 0x1;
+
+		pattrib->bdecrypted = (le32_to_cpu(prxstat->rxdw0) & BIT(27)) ? 0 : 1;
+		pattrib->encrypt = (le32_to_cpu(prxstat->rxdw0) >> 20) & 0x7;
+
+		pattrib->qos = (le32_to_cpu(prxstat->rxdw0) >> 23) & 0x1;
+		pattrib->priority = (le32_to_cpu(prxstat->rxdw1) >> 8) & 0xf;
+
+		pattrib->amsdu = (le32_to_cpu(prxstat->rxdw1) >> 13) & 0x1;
+
+		pattrib->seq_num = le32_to_cpu(prxstat->rxdw2) & 0x00000fff;
+		pattrib->frag_num = (le32_to_cpu(prxstat->rxdw2) >> 12) & 0xf;
+		pattrib->mfrag = (le32_to_cpu(prxstat->rxdw1) >> 27) & 0x1;
+		pattrib->mdata = (le32_to_cpu(prxstat->rxdw1) >> 26) & 0x1;
+
+		pattrib->mcs_rate = le32_to_cpu(prxstat->rxdw3) & 0x3f;
+		pattrib->rxht = (le32_to_cpu(prxstat->rxdw3) >> 6) & 0x1;
+
+		pattrib->icv_err = (le32_to_cpu(prxstat->rxdw0) >> 15) & 0x1;
+		pattrib->shift_sz = (le32_to_cpu(prxstat->rxdw0) >> 24) & 0x3;
+	} else if (pattrib->pkt_rpt_type == TX_REPORT1) { /* CCX */
+		pattrib->pkt_len = TX_RPT1_PKT_LEN;
+	} else if (pattrib->pkt_rpt_type == TX_REPORT2) {
+		pattrib->pkt_len = le32_to_cpu(prxstat->rxdw0) & 0x3FF;
+
+		pattrib->MacIDValidEntry[0] = le32_to_cpu(prxstat->rxdw4);
+		pattrib->MacIDValidEntry[1] = le32_to_cpu(prxstat->rxdw5);
+
+	} else if (pattrib->pkt_rpt_type == HIS_REPORT) {
+		pattrib->pkt_len = le32_to_cpu(prxstat->rxdw0) & 0x00003fff;
+	}
+}
+
+/*
+ * Notice:
+ *	Before calling this function,
+ *	precvframe->rx_data should be ready!
+ */
+void update_recvframe_phyinfo_88e(struct recv_frame *precvframe, struct phy_stat *pphy_status)
+{
+	struct adapter *padapter = precvframe->adapter;
+	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
+	struct hal_data_8188e *pHalData = &padapter->haldata;
+	struct phy_info *pPHYInfo  = &pattrib->phy_info;
+	u8 *wlanhdr = precvframe->rx_data;
+	__le16 fc = *(__le16 *)wlanhdr;
+	struct odm_per_pkt_info	pkt_info;
+	u8 *sa = NULL;
+	struct sta_priv *pstapriv;
+	struct sta_info *psta;
+
+	pkt_info.bPacketMatchBSSID = ((!ieee80211_is_ctl(fc)) &&
+		!pattrib->icv_err && !pattrib->crc_err &&
+		!memcmp(get_hdr_bssid(wlanhdr),
+		 get_bssid(&padapter->mlmepriv), ETH_ALEN));
+
+	pkt_info.bPacketToSelf = pkt_info.bPacketMatchBSSID &&
+				 (!memcmp(get_da(wlanhdr),
+				  myid(&padapter->eeprompriv), ETH_ALEN));
+
+	pkt_info.bPacketBeacon = pkt_info.bPacketMatchBSSID && ieee80211_is_beacon(fc);
+	if (pkt_info.bPacketBeacon) {
+		if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE))
+			sa = padapter->mlmepriv.cur_network.network.MacAddress;
+		/* to do Ad-hoc */
+	} else {
+		sa = get_sa(wlanhdr);
+	}
+
+	pstapriv = &padapter->stapriv;
+	pkt_info.StationID = 0xFF;
+	psta = rtw_get_stainfo(pstapriv, sa);
+	if (psta)
+		pkt_info.StationID = psta->mac_id;
+	pkt_info.Rate = pattrib->mcs_rate;
+
+	ODM_PhyStatusQuery(&pHalData->odmpriv, pPHYInfo, (u8 *)pphy_status, &(pkt_info), padapter);
+
+	precvframe->psta = NULL;
+	if (pkt_info.bPacketMatchBSSID &&
+	    (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))) {
+		if (psta) {
+			precvframe->psta = psta;
+			rtl8188e_process_phy_info(padapter, precvframe);
+		}
+	} else if (pkt_info.bPacketToSelf || pkt_info.bPacketBeacon) {
+		if (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+			if (psta)
+				precvframe->psta = psta;
+		}
+		rtl8188e_process_phy_info(padapter, precvframe);
+	}
+}
diff --git a/drivers/staging/r8188eu/hal/rtl8188eu_xmit.c b/drivers/staging/r8188eu/hal/rtl8188eu_xmit.c
new file mode 100644
index 000000000..8e4a5acc0
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/rtl8188eu_xmit.c
@@ -0,0 +1,642 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _RTL8188E_XMIT_C_
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wifi.h"
+#include "../include/osdep_intf.h"
+#include "../include/usb_ops.h"
+#include "../include/rtl8188e_hal.h"
+
+s32	rtl8188eu_init_xmit_priv(struct adapter *adapt)
+{
+	struct xmit_priv	*pxmitpriv = &adapt->xmitpriv;
+
+	tasklet_init(&pxmitpriv->xmit_tasklet,
+		     rtl8188eu_xmit_tasklet,
+		     (unsigned long)adapt);
+	return _SUCCESS;
+}
+
+static void rtl8188eu_cal_txdesc_chksum(struct tx_desc	*ptxdesc)
+{
+	u16	*usptr = (u16 *)ptxdesc;
+	u32 count = 16;		/*  (32 bytes / 2 bytes per XOR) => 16 times */
+	u32 index;
+	u16 checksum = 0;
+
+	/* Clear first */
+	ptxdesc->txdw7 &= cpu_to_le32(0xffff0000);
+
+	for (index = 0; index < count; index++)
+		checksum = checksum ^ le16_to_cpu(*(__le16 *)(usptr + index));
+	ptxdesc->txdw7 |= cpu_to_le32(0x0000ffff & checksum);
+}
+
+/*  Description: In normal chip, we should send some packet to Hw which will be used by Fw */
+/*			in FW LPS mode. The function is to fill the Tx descriptor of this packets, then */
+/*			Fw can tell Hw to send these packet derectly. */
+void rtl8188e_fill_fake_txdesc(struct adapter *adapt, u8 *desc, u32 BufferLen, u8  ispspoll, u8  is_btqosnull)
+{
+	struct tx_desc *ptxdesc;
+
+	/*  Clear all status */
+	ptxdesc = (struct tx_desc *)desc;
+	memset(desc, 0, TXDESC_SIZE);
+
+	/* offset 0 */
+	ptxdesc->txdw0 |= cpu_to_le32(OWN | FSG | LSG); /* own, bFirstSeg, bLastSeg; */
+
+	ptxdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00ff0000); /* 32 bytes for TX Desc */
+
+	ptxdesc->txdw0 |= cpu_to_le32(BufferLen & 0x0000ffff); /*  Buffer size + command header */
+
+	/* offset 4 */
+	ptxdesc->txdw1 |= cpu_to_le32((QSLT_MGNT << QSEL_SHT) & 0x00001f00); /*  Fixed queue of Mgnt queue */
+
+	/* Set NAVUSEHDR to prevent Ps-poll AId filed to be changed to error vlaue by Hw. */
+	if (ispspoll) {
+		ptxdesc->txdw1 |= cpu_to_le32(NAVUSEHDR);
+	} else {
+		ptxdesc->txdw4 |= cpu_to_le32(BIT(7)); /*  Hw set sequence number */
+		ptxdesc->txdw3 |= cpu_to_le32((8 << 28)); /* set bit3 to 1. Suugested by TimChen. 2009.12.29. */
+	}
+
+	if (is_btqosnull)
+		ptxdesc->txdw2 |= cpu_to_le32(BIT(23)); /*  BT NULL */
+
+	/* offset 16 */
+	ptxdesc->txdw4 |= cpu_to_le32(BIT(8));/* driver uses rate */
+
+	/*  USB interface drop packet if the checksum of descriptor isn't correct. */
+	/*  Using this checksum can let hardware recovery from packet bulk out error (e.g. Cancel URC, Bulk out error.). */
+	rtl8188eu_cal_txdesc_chksum(ptxdesc);
+}
+
+static void fill_txdesc_sectype(struct pkt_attrib *pattrib, struct tx_desc *ptxdesc)
+{
+	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
+		switch (pattrib->encrypt) {
+		/* SEC_TYPE : 0:NO_ENC,1:WEP40/TKIP,2:WAPI,3:AES */
+		case _WEP40_:
+		case _WEP104_:
+			ptxdesc->txdw1 |= cpu_to_le32((0x01 << SEC_TYPE_SHT) & 0x00c00000);
+			ptxdesc->txdw2 |= cpu_to_le32(0x7 << AMPDU_DENSITY_SHT);
+			break;
+		case _TKIP_:
+		case _TKIP_WTMIC_:
+			ptxdesc->txdw1 |= cpu_to_le32((0x01 << SEC_TYPE_SHT) & 0x00c00000);
+			ptxdesc->txdw2 |= cpu_to_le32(0x7 << AMPDU_DENSITY_SHT);
+			break;
+		case _AES_:
+			ptxdesc->txdw1 |= cpu_to_le32((0x03 << SEC_TYPE_SHT) & 0x00c00000);
+			ptxdesc->txdw2 |= cpu_to_le32(0x7 << AMPDU_DENSITY_SHT);
+			break;
+		case _NO_PRIVACY_:
+		default:
+			break;
+		}
+	}
+}
+
+static void fill_txdesc_vcs(struct pkt_attrib *pattrib, __le32 *pdw)
+{
+	switch (pattrib->vcs_mode) {
+	case RTS_CTS:
+		*pdw |= cpu_to_le32(RTS_EN);
+		break;
+	case CTS_TO_SELF:
+		*pdw |= cpu_to_le32(CTS_2_SELF);
+		break;
+	case NONE_VCS:
+	default:
+		break;
+	}
+	if (pattrib->vcs_mode) {
+		*pdw |= cpu_to_le32(HW_RTS_EN);
+		/*  Set RTS BW */
+		if (pattrib->ht_en) {
+			*pdw |= (pattrib->bwmode & HT_CHANNEL_WIDTH_40) ? cpu_to_le32(BIT(27)) : 0;
+
+			if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+				*pdw |= cpu_to_le32((0x01 << 28) & 0x30000000);
+			else if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+				*pdw |= cpu_to_le32((0x02 << 28) & 0x30000000);
+			else if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
+				*pdw |= 0;
+			else
+				*pdw |= cpu_to_le32((0x03 << 28) & 0x30000000);
+		}
+	}
+}
+
+static void fill_txdesc_phy(struct pkt_attrib *pattrib, __le32 *pdw)
+{
+	if (pattrib->ht_en) {
+		*pdw |= (pattrib->bwmode & HT_CHANNEL_WIDTH_40) ? cpu_to_le32(BIT(25)) : 0;
+
+		if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+			*pdw |= cpu_to_le32((0x01 << DATA_SC_SHT) & 0x003f0000);
+		else if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+			*pdw |= cpu_to_le32((0x02 << DATA_SC_SHT) & 0x003f0000);
+		else if (pattrib->ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
+			*pdw |= 0;
+		else
+			*pdw |= cpu_to_le32((0x03 << DATA_SC_SHT) & 0x003f0000);
+	}
+}
+
+static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bagg_pkt)
+{
+	int	pull = 0;
+	uint	qsel;
+	u8 data_rate, pwr_status, offset;
+	struct adapter		*adapt = pxmitframe->padapter;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct tx_desc	*ptxdesc = (struct tx_desc *)pmem;
+	struct mlme_ext_priv	*pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+
+	memset(ptxdesc, 0, sizeof(struct tx_desc));
+
+	/* 4 offset 0 */
+	ptxdesc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+	ptxdesc->txdw0 |= cpu_to_le32(sz & 0x0000ffff);/* update TXPKTSIZE */
+
+	offset = TXDESC_SIZE + OFFSET_SZ;
+
+	ptxdesc->txdw0 |= cpu_to_le32(((offset) << OFFSET_SHT) & 0x00ff0000);/* 32 bytes for TX Desc */
+
+	if (is_multicast_ether_addr(pattrib->ra))
+		ptxdesc->txdw0 |= cpu_to_le32(BMC);
+
+	/*  pkt_offset, unit:8 bytes padding */
+	if (pxmitframe->pkt_offset > 0)
+		ptxdesc->txdw1 |= cpu_to_le32((pxmitframe->pkt_offset << 26) & 0x7c000000);
+
+	/* driver uses rate */
+	ptxdesc->txdw4 |= cpu_to_le32(USERATE);/* rate control always by driver */
+
+	if ((pxmitframe->frame_tag & 0x0f) == DATA_FRAMETAG) {
+		/* offset 4 */
+		ptxdesc->txdw1 |= cpu_to_le32(pattrib->mac_id & 0x3F);
+
+		qsel = (uint)(pattrib->qsel & 0x0000001f);
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+
+		ptxdesc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000F0000);
+
+		fill_txdesc_sectype(pattrib, ptxdesc);
+
+		if (pattrib->ampdu_en) {
+			ptxdesc->txdw2 |= cpu_to_le32(AGG_EN);/* AGG EN */
+			ptxdesc->txdw6 = cpu_to_le32(0x6666f800);
+		} else {
+			ptxdesc->txdw2 |= cpu_to_le32(AGG_BK);/* AGG BK */
+		}
+
+		/* offset 8 */
+
+		/* offset 12 */
+		ptxdesc->txdw3 |= cpu_to_le32((pattrib->seqnum << SEQ_SHT) & 0x0FFF0000);
+
+		/* offset 16 , offset 20 */
+		if (pattrib->qos_en)
+			ptxdesc->txdw4 |= cpu_to_le32(QOS);/* QoS */
+
+		/* offset 20 */
+		if (pxmitframe->agg_num > 1)
+			ptxdesc->txdw5 |= cpu_to_le32((pxmitframe->agg_num << USB_TXAGG_NUM_SHT) & 0xFF000000);
+
+		if ((pattrib->ether_type != 0x888e) &&
+		    (pattrib->ether_type != 0x0806) &&
+		    (pattrib->ether_type != 0x88b4) &&
+		    (pattrib->dhcp_pkt != 1)) {
+			/* Non EAP & ARP & DHCP type data packet */
+
+			fill_txdesc_vcs(pattrib, &ptxdesc->txdw4);
+			fill_txdesc_phy(pattrib, &ptxdesc->txdw4);
+
+			ptxdesc->txdw4 |= cpu_to_le32(0x00000008);/* RTS Rate=24M */
+			ptxdesc->txdw5 |= cpu_to_le32(0x0001ff00);/* DATA/RTS  Rate FB LMT */
+
+			if (pattrib->ht_en) {
+				if (ODM_RA_GetShortGI_8188E(&haldata->odmpriv, pattrib->mac_id))
+					ptxdesc->txdw5 |= cpu_to_le32(SGI);/* SGI */
+			}
+			data_rate = ODM_RA_GetDecisionRate_8188E(&haldata->odmpriv, pattrib->mac_id);
+			ptxdesc->txdw5 |= cpu_to_le32(data_rate & 0x3F);
+			pwr_status = ODM_RA_GetHwPwrStatus_8188E(&haldata->odmpriv, pattrib->mac_id);
+			ptxdesc->txdw4 |= cpu_to_le32((pwr_status & 0x7) << PWR_STATUS_SHT);
+		} else {
+			/*  EAP data packet and ARP packet and DHCP. */
+			/*  Use the 1M data rate to send the EAP/ARP packet. */
+			/*  This will maybe make the handshake smooth. */
+			ptxdesc->txdw2 |= cpu_to_le32(AGG_BK);/* AGG BK */
+			if (pmlmeinfo->preamble_mode == PREAMBLE_SHORT)
+				ptxdesc->txdw4 |= cpu_to_le32(BIT(24));/*  DATA_SHORT */
+			ptxdesc->txdw5 |= cpu_to_le32(MRateToHwRate(pmlmeext->tx_rate));
+		}
+	} else if ((pxmitframe->frame_tag & 0x0f) == MGNT_FRAMETAG) {
+		/* offset 4 */
+		ptxdesc->txdw1 |= cpu_to_le32(pattrib->mac_id & 0x3f);
+
+		qsel = (uint)(pattrib->qsel & 0x0000001f);
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+
+		ptxdesc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000f0000);
+
+		/* offset 8 */
+		/* CCX-TXRPT ack for xmit mgmt frames. */
+		if (pxmitframe->ack_report)
+			ptxdesc->txdw2 |= cpu_to_le32(BIT(19));
+
+		/* offset 12 */
+		ptxdesc->txdw3 |= cpu_to_le32((pattrib->seqnum << SEQ_SHT) & 0x0FFF0000);
+
+		/* offset 20 */
+		ptxdesc->txdw5 |= cpu_to_le32(RTY_LMT_EN);/* retry limit enable */
+		if (pattrib->retry_ctrl)
+			ptxdesc->txdw5 |= cpu_to_le32(0x00180000);/* retry limit = 6 */
+		else
+			ptxdesc->txdw5 |= cpu_to_le32(0x00300000);/* retry limit = 12 */
+
+		ptxdesc->txdw5 |= cpu_to_le32(MRateToHwRate(pmlmeext->tx_rate));
+	} else if ((pxmitframe->frame_tag & 0x0f) != TXAGG_FRAMETAG) {
+		/* offset 4 */
+		ptxdesc->txdw1 |= cpu_to_le32((4) & 0x3f);/* CAM_ID(MAC_ID) */
+
+		ptxdesc->txdw1 |= cpu_to_le32((6 << RATE_ID_SHT) & 0x000f0000);/* raid */
+
+		/* offset 8 */
+
+		/* offset 12 */
+		ptxdesc->txdw3 |= cpu_to_le32((pattrib->seqnum << SEQ_SHT) & 0x0fff0000);
+
+		/* offset 20 */
+		ptxdesc->txdw5 |= cpu_to_le32(MRateToHwRate(pmlmeext->tx_rate));
+	}
+
+	/*  2009.11.05. tynli_test. Suggested by SD4 Filen for FW LPS. */
+	/*  (1) The sequence number of each non-Qos frame / broadcast / multicast / */
+	/*  mgnt frame should be controlled by Hw because Fw will also send null data */
+	/*  which we cannot control when Fw LPS enable. */
+	/*  --> default enable non-Qos data sequense number. 2010.06.23. by tynli. */
+	/*  (2) Enable HW SEQ control for beacon packet, because we use Hw beacon. */
+	/*  (3) Use HW Qos SEQ to control the seq num of Ext port non-Qos packets. */
+	/*  2010.06.23. Added by tynli. */
+	if (!pattrib->qos_en) {
+		ptxdesc->txdw3 |= cpu_to_le32(EN_HWSEQ); /*  Hw set sequence number */
+		ptxdesc->txdw4 |= cpu_to_le32(HW_SSN);	/*  Hw set sequence number */
+	}
+
+	ODM_SetTxAntByTxInfo_88E(&haldata->odmpriv, pmem, pattrib->mac_id);
+
+	rtl8188eu_cal_txdesc_chksum(ptxdesc);
+	return pull;
+}
+
+/* for non-agg data frame or  management frame */
+static s32 rtw_dump_xframe(struct adapter *adapt, struct xmit_frame *pxmitframe)
+{
+	s32 ret = _SUCCESS;
+	s32 inner_ret = _SUCCESS;
+	int t, sz, w_sz, pull = 0;
+	u8 *mem_addr;
+	u32 ff_hwaddr;
+	struct xmit_buf *pxmitbuf = pxmitframe->pxmitbuf;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct xmit_priv *pxmitpriv = &adapt->xmitpriv;
+	struct security_priv *psecuritypriv = &adapt->securitypriv;
+	if ((pxmitframe->frame_tag == DATA_FRAMETAG) &&
+	    (pxmitframe->attrib.ether_type != 0x0806) &&
+	    (pxmitframe->attrib.ether_type != 0x888e) &&
+	    (pxmitframe->attrib.ether_type != 0x88b4) &&
+	    (pxmitframe->attrib.dhcp_pkt != 1))
+		rtw_issue_addbareq_cmd(adapt, pxmitframe);
+	mem_addr = pxmitframe->buf_addr;
+
+	for (t = 0; t < pattrib->nr_frags; t++) {
+		if (inner_ret != _SUCCESS && ret == _SUCCESS)
+			ret = _FAIL;
+
+		if (t != (pattrib->nr_frags - 1)) {
+			sz = pxmitpriv->frag_len;
+			sz = sz - 4 - (psecuritypriv->sw_encrypt ? 0 : pattrib->icv_len);
+		} else {
+			/* no frag */
+			sz = pattrib->last_txcmdsz;
+		}
+
+		pull = update_txdesc(pxmitframe, mem_addr, sz, false);
+
+		if (pull) {
+			mem_addr += PACKET_OFFSET_SZ; /* pull txdesc head */
+			pxmitframe->buf_addr = mem_addr;
+			w_sz = sz + TXDESC_SIZE;
+		} else {
+			w_sz = sz + TXDESC_SIZE + PACKET_OFFSET_SZ;
+		}
+		ff_hwaddr = rtw_get_ff_hwaddr(pxmitframe);
+
+		inner_ret = rtw_write_port(adapt, ff_hwaddr, w_sz, (unsigned char *)pxmitbuf);
+
+		rtw_count_tx_stats(adapt, pxmitframe, sz);
+
+		mem_addr += w_sz;
+
+		mem_addr = PTR_ALIGN(mem_addr, 4);
+	}
+
+	rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+	if  (ret != _SUCCESS)
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_UNKNOWN);
+
+	return ret;
+}
+
+static u32 xmitframe_need_length(struct xmit_frame *pxmitframe)
+{
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	u32 len = 0;
+
+	/*  no consider fragement */
+	len = pattrib->hdrlen + pattrib->iv_len +
+		SNAP_SIZE + sizeof(u16) +
+		pattrib->pktlen +
+		((pattrib->bswenc) ? pattrib->icv_len : 0);
+
+	if (pattrib->encrypt == _TKIP_)
+		len += 8;
+
+	return len;
+}
+
+bool rtl8188eu_xmitframe_complete(struct adapter *adapt, struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(adapt);
+	struct xmit_frame *pxmitframe = NULL;
+	struct xmit_frame *pfirstframe = NULL;
+
+	/*  aggregate variable */
+	struct hw_xmit *phwxmit;
+	struct sta_info *psta = NULL;
+	struct tx_servq *ptxservq = NULL;
+	struct list_head *xmitframe_plist = NULL, *xmitframe_phead = NULL;
+
+	u32 pbuf;	/*  next pkt address */
+	u32 pbuf_tail;	/*  last pkt tail */
+	u32 len;	/*  packet length, except TXDESC_SIZE and PKT_OFFSET */
+
+	u32 bulksize;
+	u8 desc_cnt;
+	u32 bulkptr;
+
+	/*  dump frame variable */
+	u32 ff_hwaddr;
+
+	if (pdvobjpriv->pusbdev->speed == USB_SPEED_HIGH)
+		bulksize = USB_HIGH_SPEED_BULK_SIZE;
+	else
+		bulksize = USB_FULL_SPEED_BULK_SIZE;
+
+	/*  check xmitbuffer is ok */
+	if (!pxmitbuf) {
+		pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+		if (!pxmitbuf)
+			return false;
+	}
+
+	/* 3 1. pick up first frame */
+	rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+	pxmitframe = rtw_dequeue_xframe(pxmitpriv, pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+	if (!pxmitframe) {
+		/*  no more xmit frame, release xmit buffer */
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+		return false;
+	}
+
+	pxmitframe->pxmitbuf = pxmitbuf;
+	pxmitframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pxmitframe;
+
+	pxmitframe->agg_num = 1; /*  alloc xmitframe should assign to 1. */
+	pxmitframe->pkt_offset = 1; /*  first frame of aggregation, reserve offset */
+
+	rtw_xmitframe_coalesce(adapt, pxmitframe->pkt, pxmitframe);
+
+	/*  always return ndis_packet after rtw_xmitframe_coalesce */
+	rtw_xmit_complete(adapt, pxmitframe);
+
+	/* 3 2. aggregate same priority and same DA(AP or STA) frames */
+	pfirstframe = pxmitframe;
+	len = xmitframe_need_length(pfirstframe) + TXDESC_SIZE + (pfirstframe->pkt_offset * PACKET_OFFSET_SZ);
+	pbuf_tail = len;
+	pbuf = round_up(pbuf_tail, 8);
+
+	/*  check pkt amount in one bulk */
+	desc_cnt = 0;
+	bulkptr = bulksize;
+	if (pbuf < bulkptr) {
+		desc_cnt++;
+	} else {
+		desc_cnt = 0;
+		bulkptr = ((pbuf / bulksize) + 1) * bulksize; /*  round to next bulksize */
+	}
+
+	/*  dequeue same priority packet from station tx queue */
+	psta = pfirstframe->attrib.psta;
+	switch (pfirstframe->attrib.priority) {
+	case 1:
+	case 2:
+		ptxservq = &psta->sta_xmitpriv.bk_q;
+		phwxmit = pxmitpriv->hwxmits + 3;
+		break;
+	case 4:
+	case 5:
+		ptxservq = &psta->sta_xmitpriv.vi_q;
+		phwxmit = pxmitpriv->hwxmits + 1;
+		break;
+	case 6:
+	case 7:
+		ptxservq = &psta->sta_xmitpriv.vo_q;
+		phwxmit = pxmitpriv->hwxmits;
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &psta->sta_xmitpriv.be_q;
+		phwxmit = pxmitpriv->hwxmits + 2;
+		break;
+	}
+	spin_lock_bh(&pxmitpriv->lock);
+
+	xmitframe_phead = get_list_head(&ptxservq->sta_pending);
+	xmitframe_plist = xmitframe_phead->next;
+
+	while (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+		xmitframe_plist = xmitframe_plist->next;
+
+		pxmitframe->agg_num = 0; /*  not first frame of aggregation */
+		pxmitframe->pkt_offset = 0; /*  not first frame of aggregation, no need to reserve offset */
+
+		len = xmitframe_need_length(pxmitframe) + TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+
+		if (pbuf + len > MAX_XMITBUF_SZ) {
+			pxmitframe->agg_num = 1;
+			pxmitframe->pkt_offset = 1;
+			break;
+		}
+		list_del_init(&pxmitframe->list);
+		ptxservq->qcnt--;
+		phwxmit->accnt--;
+
+		pxmitframe->buf_addr = pxmitbuf->pbuf + pbuf;
+
+		rtw_xmitframe_coalesce(adapt, pxmitframe->pkt, pxmitframe);
+		/*  always return ndis_packet after rtw_xmitframe_coalesce */
+		rtw_xmit_complete(adapt, pxmitframe);
+
+		/*  (len - TXDESC_SIZE) == pxmitframe->attrib.last_txcmdsz */
+		update_txdesc(pxmitframe, pxmitframe->buf_addr, pxmitframe->attrib.last_txcmdsz, true);
+
+		/*  don't need xmitframe any more */
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		/*  handle pointer and stop condition */
+		pbuf_tail = pbuf + len;
+		pbuf = round_up(pbuf_tail, 8);
+
+		pfirstframe->agg_num++;
+		if (MAX_TX_AGG_PACKET_NUMBER == pfirstframe->agg_num)
+			break;
+
+		if (pbuf < bulkptr) {
+			desc_cnt++;
+			if (desc_cnt == USB_TXAGG_DESC_NUM)
+				break;
+		} else {
+			desc_cnt = 0;
+			bulkptr = ((pbuf / bulksize) + 1) * bulksize;
+		}
+	} /* end while (aggregate same priority and same DA(AP or STA) frames) */
+
+	if (list_empty(&ptxservq->sta_pending.queue))
+		list_del_init(&ptxservq->tx_pending);
+
+	spin_unlock_bh(&pxmitpriv->lock);
+	if ((pfirstframe->attrib.ether_type != 0x0806) &&
+	    (pfirstframe->attrib.ether_type != 0x888e) &&
+	    (pfirstframe->attrib.ether_type != 0x88b4) &&
+	    (pfirstframe->attrib.dhcp_pkt != 1))
+		rtw_issue_addbareq_cmd(adapt, pfirstframe);
+	/* 3 3. update first frame txdesc */
+	if ((pbuf_tail % bulksize) == 0) {
+		/*  remove pkt_offset */
+		pbuf_tail -= PACKET_OFFSET_SZ;
+		pfirstframe->buf_addr += PACKET_OFFSET_SZ;
+		pfirstframe->pkt_offset--;
+	}
+
+	update_txdesc(pfirstframe, pfirstframe->buf_addr, pfirstframe->attrib.last_txcmdsz, true);
+
+	/* 3 4. write xmit buffer to USB FIFO */
+	ff_hwaddr = rtw_get_ff_hwaddr(pfirstframe);
+	rtw_write_port(adapt, ff_hwaddr, pbuf_tail, (u8 *)pxmitbuf);
+
+	/* 3 5. update statisitc */
+	pbuf_tail -= (pfirstframe->agg_num * TXDESC_SIZE);
+	pbuf_tail -= (pfirstframe->pkt_offset * PACKET_OFFSET_SZ);
+
+	rtw_count_tx_stats(adapt, pfirstframe, pbuf_tail);
+
+	rtw_free_xmitframe(pxmitpriv, pfirstframe);
+
+	return true;
+}
+
+static s32 xmitframe_direct(struct adapter *adapt, struct xmit_frame *pxmitframe)
+{
+	s32 res = _SUCCESS;
+
+	res = rtw_xmitframe_coalesce(adapt, pxmitframe->pkt, pxmitframe);
+	if (res == _SUCCESS)
+		rtw_dump_xframe(adapt, pxmitframe);
+
+	return res;
+}
+
+/*
+ * Return
+ *	true	dump packet directly
+ *	false	enqueue packet
+ */
+static s32 pre_xmitframe(struct adapter *adapt, struct xmit_frame *pxmitframe)
+{
+	s32 res;
+	struct xmit_buf *pxmitbuf = NULL;
+	struct xmit_priv *pxmitpriv = &adapt->xmitpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_priv *pmlmepriv = &adapt->mlmepriv;
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	if (rtw_txframes_sta_ac_pending(adapt, pattrib) > 0)
+		goto enqueue;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING))
+		goto enqueue;
+
+	pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+	if (!pxmitbuf)
+		goto enqueue;
+
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	pxmitframe->pxmitbuf = pxmitbuf;
+	pxmitframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pxmitframe;
+
+	if (xmitframe_direct(adapt, pxmitframe) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+	}
+
+	return true;
+
+enqueue:
+	res = rtw_xmitframe_enqueue(adapt, pxmitframe);
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	if (res != _SUCCESS) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		/*  Trick, make the statistics correct */
+		pxmitpriv->tx_pkts--;
+		pxmitpriv->tx_drop++;
+		return true;
+	}
+
+	return false;
+}
+
+s32 rtl8188eu_mgnt_xmit(struct adapter *adapt, struct xmit_frame *pmgntframe)
+{
+	return rtw_dump_xframe(adapt, pmgntframe);
+}
+
+/*
+ * Return
+ *	true	dump packet directly ok
+ *	false	temporary can't transmit packets to hardware
+ */
+s32 rtl8188eu_hal_xmit(struct adapter *adapt, struct xmit_frame *pxmitframe)
+{
+	return pre_xmitframe(adapt, pxmitframe);
+}
diff --git a/drivers/staging/r8188eu/hal/usb_halinit.c b/drivers/staging/r8188eu/hal/usb_halinit.c
new file mode 100644
index 000000000..d28b4dc2a
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/usb_halinit.c
@@ -0,0 +1,1076 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _HCI_HAL_INIT_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_efuse.h"
+#include "../include/rtw_fw.h"
+#include "../include/rtl8188e_hal.h"
+#include "../include/rtw_iol.h"
+#include "../include/usb_ops.h"
+#include "../include/usb_osintf.h"
+#include "../include/HalPwrSeqCmd.h"
+
+static void one_out_pipe(struct adapter *adapter)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(adapter);
+
+	pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+	pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+	pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[0];/* BE */
+	pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+}
+
+static void two_out_pipe(struct adapter *adapter, bool wifi_cfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(adapter);
+
+	/* 0:H, 1:L */
+
+	pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+	pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[1];/* BE */
+
+	if (wifi_cfg) {
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[1];/* VO */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+	} else {
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[1];/* BK */
+	}
+}
+
+static void three_out_pipe(struct adapter *adapter, bool wifi_cfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(adapter);
+
+	/* 0:H, 1:N, 2:L */
+
+	pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+	pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[1];/* VI */
+	pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[2];/* BE */
+
+	pdvobjpriv->Queue2Pipe[3] = wifi_cfg ?
+		pdvobjpriv->RtOutPipe[1] : pdvobjpriv->RtOutPipe[2];/* BK */
+}
+
+int rtl8188eu_interface_configure(struct adapter *adapt)
+{
+	struct registry_priv *pregistrypriv = &adapt->registrypriv;
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(adapt);
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	bool wifi_cfg = pregistrypriv->wifi_spec;
+
+	pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+	pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+	pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+	pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	switch (pdvobjpriv->RtNumOutPipes) {
+	case 3:
+		haldata->out_ep_extra_queues = TX_SELE_LQ | TX_SELE_NQ;
+		three_out_pipe(adapt, wifi_cfg);
+		break;
+	case 2:
+		haldata->out_ep_extra_queues = TX_SELE_NQ;
+		two_out_pipe(adapt, wifi_cfg);
+		break;
+	case 1:
+		one_out_pipe(adapt);
+		break;
+	default:
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+u32 rtl8188eu_InitPowerOn(struct adapter *adapt)
+{
+	u16 value16;
+	int res;
+
+	/*  HW Power on sequence */
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	if (haldata->bMacPwrCtrlOn)
+		return _SUCCESS;
+
+	if (!HalPwrSeqCmdParsing(adapt, PWR_ON_FLOW))
+		return _FAIL;
+
+	/*  Enable MAC DMA/WMAC/SCHEDULE/SEC block */
+	/*  Set CR bit10 to enable 32k calibration. Suggested by SD1 Gimmy. Added by tynli. 2011.08.31. */
+	rtw_write16(adapt, REG_CR, 0x00);  /* suggseted by zhouzhou, by page, 20111230 */
+
+		/*  Enable MAC DMA/WMAC/SCHEDULE/SEC block */
+	res = rtw_read16(adapt, REG_CR, &value16);
+	if (res)
+		return _FAIL;
+
+	value16 |= (HCI_TXDMA_EN | HCI_RXDMA_EN | TXDMA_EN | RXDMA_EN
+				| PROTOCOL_EN | SCHEDULE_EN | ENSEC | CALTMR_EN);
+	/*  for SDIO - Set CR bit10 to enable 32k calibration. Suggested by SD1 Gimmy. Added by tynli. 2011.08.31. */
+
+	rtw_write16(adapt, REG_CR, value16);
+	haldata->bMacPwrCtrlOn = true;
+
+	return _SUCCESS;
+}
+
+/*  Shall USB interface init this? */
+static void _InitInterrupt(struct adapter *Adapter)
+{
+	u32 imr, imr_ex;
+	u8  usb_opt;
+	int res;
+
+	/* HISR write one to clear */
+	rtw_write32(Adapter, REG_HISR_88E, 0xFFFFFFFF);
+	/*  HIMR - */
+	imr = IMR_PSTIMEOUT_88E | IMR_TBDER_88E | IMR_CPWM_88E | IMR_CPWM2_88E;
+	rtw_write32(Adapter, REG_HIMR_88E, imr);
+
+	imr_ex = IMR_TXERR_88E | IMR_RXERR_88E | IMR_TXFOVW_88E | IMR_RXFOVW_88E;
+	rtw_write32(Adapter, REG_HIMRE_88E, imr_ex);
+
+	/*  REG_USB_SPECIAL_OPTION - BIT(4) */
+	/*  0; Use interrupt endpoint to upload interrupt pkt */
+	/*  1; Use bulk endpoint to upload interrupt pkt, */
+	res = rtw_read8(Adapter, REG_USB_SPECIAL_OPTION, &usb_opt);
+	if (res)
+		return;
+
+	if (adapter_to_dvobj(Adapter)->pusbdev->speed == USB_SPEED_HIGH)
+		usb_opt = usb_opt | (INT_BULK_SEL);
+	else
+		usb_opt = usb_opt & (~INT_BULK_SEL);
+
+	rtw_write8(Adapter, REG_USB_SPECIAL_OPTION, usb_opt);
+}
+
+static void _InitQueueReservedPage(struct adapter *Adapter)
+{
+	struct hal_data_8188e *haldata = &Adapter->haldata;
+	struct registry_priv	*pregistrypriv = &Adapter->registrypriv;
+	u8 numLQ = 0;
+	u8 numNQ = 0;
+	u8 numPubQ;
+
+	if (pregistrypriv->wifi_spec) {
+		if (haldata->out_ep_extra_queues & TX_SELE_LQ)
+			numLQ = 0x1C;
+
+		/*  NOTE: This step shall be proceed before writing REG_RQPN. */
+		if (haldata->out_ep_extra_queues & TX_SELE_NQ)
+			numNQ = 0x1C;
+
+		rtw_write8(Adapter, REG_RQPN_NPQ, numNQ);
+
+		numPubQ = 0xA8 - NUM_HQ - numLQ - numNQ;
+
+		/*  TX DMA */
+		rtw_write32(Adapter, REG_RQPN, LD_RQPN | numPubQ << 16 | numLQ << 8 | NUM_HQ);
+	} else {
+		rtw_write16(Adapter, REG_RQPN_NPQ, 0x0000);/* Just follow MP Team,??? Georgia 03/28 */
+		rtw_write16(Adapter, REG_RQPN_NPQ, 0x0d);
+		rtw_write32(Adapter, REG_RQPN, 0x808E000d);/* reserve 7 page for LPS */
+	}
+}
+
+static void _InitTxBufferBoundary(struct adapter *Adapter, u8 txpktbuf_bndy)
+{
+	rtw_write8(Adapter, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtw_write8(Adapter, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+	rtw_write8(Adapter, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
+	rtw_write8(Adapter, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtw_write8(Adapter, REG_TDECTRL + 1, txpktbuf_bndy);
+}
+
+static void _InitPageBoundary(struct adapter *Adapter)
+{
+	/*  RX Page Boundary */
+	/*  */
+	u16 rxff_bndy = MAX_RX_DMA_BUFFER_SIZE_88E - 1;
+
+	rtw_write16(Adapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
+}
+
+static void _InitNormalChipRegPriority(struct adapter *Adapter, u16 beQ,
+				       u16 bkQ, u16 viQ, u16 voQ, u16 mgtQ,
+				       u16 hiQ)
+{
+	u16 value16;
+	int res;
+
+	res = rtw_read16(Adapter, REG_TRXDMA_CTRL, &value16);
+	if (res)
+		return;
+
+	value16 &= 0x7;
+
+	value16 |= _TXDMA_BEQ_MAP(beQ)	| _TXDMA_BKQ_MAP(bkQ) |
+		   _TXDMA_VIQ_MAP(viQ)	| _TXDMA_VOQ_MAP(voQ) |
+		   _TXDMA_MGQ_MAP(mgtQ) | _TXDMA_HIQ_MAP(hiQ);
+
+	rtw_write16(Adapter, REG_TRXDMA_CTRL, value16);
+}
+
+static void _InitNormalChipTwoOutEpPriority(struct adapter *Adapter)
+{
+	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
+	u16 bkQ, voQ;
+
+	if (!pregistrypriv->wifi_spec) {
+		bkQ	= QUEUE_NORMAL;
+		voQ	= QUEUE_HIGH;
+	} else {/* for WMM ,CONFIG_OUT_EP_WIFI_MODE */
+		bkQ	= QUEUE_HIGH;
+		voQ	= QUEUE_NORMAL;
+	}
+	_InitNormalChipRegPriority(Adapter, QUEUE_NORMAL, bkQ, QUEUE_HIGH,
+				   voQ, QUEUE_HIGH, QUEUE_HIGH);
+}
+
+static void _InitNormalChipThreeOutEpPriority(struct adapter *Adapter)
+{
+	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
+	u16 beQ, bkQ, viQ, voQ, mgtQ, hiQ;
+
+	if (!pregistrypriv->wifi_spec) {/*  typical setting */
+		beQ	= QUEUE_LOW;
+		bkQ	= QUEUE_LOW;
+		viQ	= QUEUE_NORMAL;
+		voQ	= QUEUE_HIGH;
+		mgtQ	= QUEUE_HIGH;
+		hiQ	= QUEUE_HIGH;
+	} else {/*  for WMM */
+		beQ	= QUEUE_LOW;
+		bkQ	= QUEUE_NORMAL;
+		viQ	= QUEUE_NORMAL;
+		voQ	= QUEUE_HIGH;
+		mgtQ	= QUEUE_HIGH;
+		hiQ	= QUEUE_HIGH;
+	}
+	_InitNormalChipRegPriority(Adapter, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
+}
+
+static void _InitQueuePriority(struct adapter *Adapter)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(Adapter);
+
+	switch (pdvobjpriv->RtNumOutPipes) {
+	case 1:
+		_InitNormalChipRegPriority(Adapter, QUEUE_HIGH, QUEUE_HIGH, QUEUE_HIGH,
+					   QUEUE_HIGH, QUEUE_HIGH, QUEUE_HIGH);
+		break;
+	case 2:
+		_InitNormalChipTwoOutEpPriority(Adapter);
+		break;
+	case 3:
+		_InitNormalChipThreeOutEpPriority(Adapter);
+		break;
+	default:
+		break;
+	}
+}
+
+static void _InitNetworkType(struct adapter *Adapter)
+{
+	u32 value32;
+	int res;
+
+	res = rtw_read32(Adapter, REG_CR, &value32);
+	if (res)
+		return;
+
+	/*  TODO: use the other function to set network type */
+	value32 = (value32 & ~MASK_NETTYPE) | _NETTYPE(NT_LINK_AP);
+
+	rtw_write32(Adapter, REG_CR, value32);
+}
+
+static void _InitTransferPageSize(struct adapter *Adapter)
+{
+	/*  Tx page size is always 128. */
+
+	u8 value8;
+	value8 = _PSRX(PBP_128) | _PSTX(PBP_128);
+	rtw_write8(Adapter, REG_PBP, value8);
+}
+
+static void _InitDriverInfoSize(struct adapter *Adapter, u8 drvInfoSize)
+{
+	rtw_write8(Adapter, REG_RX_DRVINFO_SZ, drvInfoSize);
+}
+
+static void _InitWMACSetting(struct adapter *Adapter)
+{
+	u32 receive_config = RCR_AAP | RCR_APM | RCR_AM | RCR_AB |
+			     RCR_CBSSID_DATA | RCR_CBSSID_BCN |
+			     RCR_APP_ICV | RCR_AMF | RCR_HTC_LOC_CTRL |
+			     RCR_APP_MIC | RCR_APP_PHYSTS;
+
+	/*  some REG_RCR will be modified later by phy_ConfigMACWithHeaderFile() */
+	rtw_write32(Adapter, REG_RCR, receive_config);
+
+	/*  Accept all multicast address */
+	rtw_write32(Adapter, REG_MAR, 0xFFFFFFFF);
+	rtw_write32(Adapter, REG_MAR + 4, 0xFFFFFFFF);
+}
+
+static void _InitAdaptiveCtrl(struct adapter *Adapter)
+{
+	u16 value16;
+	u32 value32;
+	int res;
+
+	/*  Response Rate Set */
+	res = rtw_read32(Adapter, REG_RRSR, &value32);
+	if (res)
+		return;
+
+	value32 &= ~RATE_BITMAP_ALL;
+	value32 |= RATE_RRSR_CCK_ONLY_1M;
+	rtw_write32(Adapter, REG_RRSR, value32);
+
+	/*  CF-END Threshold */
+
+	/*  SIFS (used in NAV) */
+	value16 = _SPEC_SIFS_CCK(0x10) | _SPEC_SIFS_OFDM(0x10);
+	rtw_write16(Adapter, REG_SPEC_SIFS, value16);
+
+	/*  Retry Limit */
+	value16 = _LRL(0x30) | _SRL(0x30);
+	rtw_write16(Adapter, REG_RL, value16);
+}
+
+static void _InitEDCA(struct adapter *Adapter)
+{
+	/*  Set Spec SIFS (used in NAV) */
+	rtw_write16(Adapter, REG_SPEC_SIFS, 0x100a);
+	rtw_write16(Adapter, REG_MAC_SPEC_SIFS, 0x100a);
+
+	/*  Set SIFS for CCK */
+	rtw_write16(Adapter, REG_SIFS_CTX, 0x100a);
+
+	/*  Set SIFS for OFDM */
+	rtw_write16(Adapter, REG_SIFS_TRX, 0x100a);
+
+	/*  TXOP */
+	rtw_write32(Adapter, REG_EDCA_BE_PARAM, 0x005EA42B);
+	rtw_write32(Adapter, REG_EDCA_BK_PARAM, 0x0000A44F);
+	rtw_write32(Adapter, REG_EDCA_VI_PARAM, 0x005EA324);
+	rtw_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
+}
+
+static void _InitRetryFunction(struct adapter *Adapter)
+{
+	u8 value8;
+	int res;
+
+	res = rtw_read8(Adapter, REG_FWHW_TXQ_CTRL, &value8);
+	if (res)
+		return;
+
+	value8 |= EN_AMPDU_RTY_NEW;
+	rtw_write8(Adapter, REG_FWHW_TXQ_CTRL, value8);
+
+	/*  Set ACK timeout */
+	rtw_write8(Adapter, REG_ACKTO, 0x40);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	usb_AggSettingTxUpdate()
+ *
+ * Overview:	Separate TX/RX parameters update independent for TP detection and
+ *			dynamic TX/RX aggreagtion parameters update.
+ *
+ * Input:			struct adapter *
+ *
+ * Output/Return:	NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	12/10/2010	MHC		Separate to smaller function.
+ *
+ *---------------------------------------------------------------------------*/
+static void usb_AggSettingTxUpdate(struct adapter *Adapter)
+{
+	u32 value32;
+	int res;
+
+	if (Adapter->registrypriv.wifi_spec)
+		return;
+
+	res = rtw_read32(Adapter, REG_TDECTRL, &value32);
+	if (res)
+		return;
+
+	value32 = value32 & ~(BLK_DESC_NUM_MASK << BLK_DESC_NUM_SHIFT);
+	value32 |= ((USB_TXAGG_DESC_NUM & BLK_DESC_NUM_MASK) << BLK_DESC_NUM_SHIFT);
+
+	rtw_write32(Adapter, REG_TDECTRL, value32);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	usb_AggSettingRxUpdate()
+ *
+ * Overview:	Separate TX/RX parameters update independent for TP detection and
+ *			dynamic TX/RX aggreagtion parameters update.
+ *
+ * Input:			struct adapter *
+ *
+ * Output/Return:	NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	12/10/2010	MHC		Separate to smaller function.
+ *
+ *---------------------------------------------------------------------------*/
+static void
+usb_AggSettingRxUpdate(
+		struct adapter *Adapter
+	)
+{
+	u8 valueDMA;
+	u8 valueUSB;
+	int res;
+
+	res = rtw_read8(Adapter, REG_TRXDMA_CTRL, &valueDMA);
+	if (res)
+		return;
+
+	res = rtw_read8(Adapter, REG_USB_SPECIAL_OPTION, &valueUSB);
+	if (res)
+		return;
+
+	valueDMA |= RXDMA_AGG_EN;
+	valueUSB &= ~USB_AGG_EN;
+
+	rtw_write8(Adapter, REG_TRXDMA_CTRL, valueDMA);
+	rtw_write8(Adapter, REG_USB_SPECIAL_OPTION, valueUSB);
+
+	rtw_write8(Adapter, REG_RXDMA_AGG_PG_TH, USB_RXAGG_PAGE_COUNT);
+	rtw_write8(Adapter, REG_RXDMA_AGG_PG_TH + 1, USB_RXAGG_PAGE_TIMEOUT);
+}
+
+static void InitUsbAggregationSetting(struct adapter *Adapter)
+{
+	/*  Tx aggregation setting */
+	usb_AggSettingTxUpdate(Adapter);
+
+	/*  Rx aggregation setting */
+	usb_AggSettingRxUpdate(Adapter);
+}
+
+/* FIXME: add error handling in callers */
+static int _InitBeaconParameters(struct adapter *Adapter)
+{
+	struct hal_data_8188e *haldata = &Adapter->haldata;
+	int res;
+
+	rtw_write16(Adapter, REG_BCN_CTRL, 0x1010);
+
+	/*  TODO: Remove these magic number */
+	rtw_write16(Adapter, REG_TBTT_PROHIBIT, 0x6404);/*  ms */
+	rtw_write8(Adapter, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);/*  5ms */
+	rtw_write8(Adapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME); /*  2ms */
+
+	/*  Suggested by designer timchen. Change beacon AIFS to the largest number */
+	/*  beacause test chip does not contension before sending beacon. by tynli. 2009.11.03 */
+	rtw_write16(Adapter, REG_BCNTCFG, 0x660F);
+
+	res = rtw_read8(Adapter, REG_FWHW_TXQ_CTRL + 2, &haldata->RegFwHwTxQCtrl);
+	if (res)
+		return res;
+
+	res = rtw_read8(Adapter, REG_TBTT_PROHIBIT + 2, &haldata->RegReg542);
+	if (res)
+		return res;
+
+	res = rtw_read8(Adapter, REG_CR + 1, &haldata->RegCR_1);
+	if (res)
+		return res;
+
+	return 0;
+}
+
+static void _BeaconFunctionEnable(struct adapter *Adapter)
+{
+	rtw_write8(Adapter, REG_BCN_CTRL, (BIT(4) | BIT(3) | BIT(1)));
+
+	rtw_write8(Adapter, REG_RD_CTRL + 1, 0x6F);
+}
+
+/*  Set CCK and OFDM Block "ON" */
+static void _BBTurnOnBlock(struct adapter *Adapter)
+{
+	rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bCCKEn, 0x1);
+	rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bOFDMEn, 0x1);
+}
+
+static void _InitAntenna_Selection(struct adapter *Adapter)
+{
+	struct hal_data_8188e *haldata = &Adapter->haldata;
+	int res;
+	u32 reg;
+
+	if (haldata->AntDivCfg == 0)
+		return;
+
+	res = rtw_read32(Adapter, REG_LEDCFG0, &reg);
+	if (res)
+		return;
+
+	rtw_write32(Adapter, REG_LEDCFG0, reg | BIT(23));
+	rtl8188e_PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter, BIT(13), 0x01);
+
+	if (rtl8188e_PHY_QueryBBReg(Adapter, rFPGA0_XA_RFInterfaceOE, 0x300) == Antenna_A)
+		haldata->CurAntenna = Antenna_A;
+	else
+		haldata->CurAntenna = Antenna_B;
+}
+
+static void hw_var_set_macaddr(struct adapter *Adapter, u8 *val)
+{
+	u8 idx = 0;
+	u32 reg_macid;
+
+	reg_macid = REG_MACID;
+
+	for (idx = 0; idx < 6; idx++)
+		rtw_write8(Adapter, (reg_macid + idx), val[idx]);
+}
+
+u32 rtl8188eu_hal_init(struct adapter *Adapter)
+{
+	u8 value8 = 0;
+	u16  value16;
+	u32 status = _SUCCESS;
+	int res;
+	struct hal_data_8188e *haldata = &Adapter->haldata;
+	struct pwrctrl_priv		*pwrctrlpriv = &Adapter->pwrctrlpriv;
+	struct registry_priv	*pregistrypriv = &Adapter->registrypriv;
+	u32 reg;
+
+	if (Adapter->pwrctrlpriv.bkeepfwalive) {
+		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
+			PHY_IQCalibrate_8188E(Adapter, true);
+		} else {
+			PHY_IQCalibrate_8188E(Adapter, false);
+			haldata->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
+		}
+
+		ODM_TXPowerTrackingCheck(&haldata->odmpriv);
+		PHY_LCCalibrate_8188E(Adapter);
+
+		goto exit;
+	}
+
+	status = rtl8188eu_InitPowerOn(Adapter);
+	if (status == _FAIL)
+		goto exit;
+
+	/*  Save target channel */
+	haldata->CurrentChannel = 6;/* default set to 6 */
+
+	/*  2010/08/09 MH We need to check if we need to turnon or off RF after detecting */
+	/*  HW GPIO pin. Before PHY_RFConfig8192C. */
+	/*  2010/08/26 MH If Efuse does not support sective suspend then disable the function. */
+
+	_InitQueueReservedPage(Adapter);
+	_InitQueuePriority(Adapter);
+	_InitPageBoundary(Adapter);
+	_InitTransferPageSize(Adapter);
+
+	_InitTxBufferBoundary(Adapter, 0);
+
+	status = rtl8188e_firmware_download(Adapter);
+
+	if (status != _SUCCESS) {
+		Adapter->bFWReady = false;
+		haldata->fw_ractrl = false;
+		return status;
+	} else {
+		Adapter->bFWReady = true;
+		haldata->fw_ractrl = false;
+	}
+	/* Initialize firmware vars */
+	Adapter->pwrctrlpriv.bFwCurrentInPSMode = false;
+	haldata->LastHMEBoxNum = 0;
+
+	status = PHY_MACConfig8188E(Adapter);
+	if (status == _FAIL)
+		goto exit;
+
+	/*  */
+	/* d. Initialize BB related configurations. */
+	/*  */
+	status = PHY_BBConfig8188E(Adapter);
+	if (status == _FAIL)
+		goto exit;
+
+	status = phy_RF6052_Config_ParaFile(Adapter);
+	if (status == _FAIL)
+		goto exit;
+
+	status = rtl8188e_iol_efuse_patch(Adapter);
+	if (status == _FAIL)
+		goto exit;
+
+	_InitTxBufferBoundary(Adapter, TX_PAGE_BOUNDARY_88E);
+
+	status =  InitLLTTable(Adapter, TX_PAGE_BOUNDARY_88E);
+	if (status == _FAIL)
+		goto exit;
+
+	/*  Get Rx PHY status in order to report RSSI and others. */
+	_InitDriverInfoSize(Adapter, DRVINFO_SZ);
+
+	_InitInterrupt(Adapter);
+	hw_var_set_macaddr(Adapter, Adapter->eeprompriv.mac_addr);
+	_InitNetworkType(Adapter);/* set msr */
+	_InitWMACSetting(Adapter);
+	_InitAdaptiveCtrl(Adapter);
+	_InitEDCA(Adapter);
+	_InitRetryFunction(Adapter);
+	InitUsbAggregationSetting(Adapter);
+	_InitBeaconParameters(Adapter);
+
+	/*  */
+	/*  Init CR MACTXEN, MACRXEN after setting RxFF boundary REG_TRXFF_BNDY to patch */
+	/*  Hw bug which Hw initials RxFF boundary size to a value which is larger than the real Rx buffer size in 88E. */
+	/*  */
+	/*  Enable MACTXEN/MACRXEN block */
+	res = rtw_read16(Adapter, REG_CR, &value16);
+	if (res)
+		return _FAIL;
+
+	value16 |= (MACTXEN | MACRXEN);
+	rtw_write8(Adapter, REG_CR, value16);
+
+	/* Enable TX Report */
+	/* Enable Tx Report Timer */
+	res = rtw_read8(Adapter, REG_TX_RPT_CTRL, &value8);
+	if (res)
+		return _FAIL;
+
+	rtw_write8(Adapter,  REG_TX_RPT_CTRL, (value8 | BIT(1) | BIT(0)));
+	/* Set MAX RPT MACID */
+	rtw_write8(Adapter,  REG_TX_RPT_CTRL + 1, 2);/* FOR sta mode ,0: bc/mc ,1:AP */
+	/* Tx RPT Timer. Unit: 32us */
+	rtw_write16(Adapter, REG_TX_RPT_TIME, 0xCdf0);
+
+	rtw_write8(Adapter, REG_EARLY_MODE_CONTROL, 0);
+
+	rtw_write16(Adapter, REG_PKT_VO_VI_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
+	rtw_write16(Adapter, REG_PKT_BE_BK_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
+
+	/* Keep RfRegChnlVal for later use. */
+	haldata->RfRegChnlVal = rtl8188e_PHY_QueryRFReg(Adapter, RF_CHNLBW, bRFRegOffsetMask);
+
+	_BBTurnOnBlock(Adapter);
+
+	invalidate_cam_all(Adapter);
+
+	/*  2010/12/17 MH We need to set TX power according to EFUSE content at first. */
+	PHY_SetTxPowerLevel8188E(Adapter, haldata->CurrentChannel);
+
+/*  Move by Neo for USB SS to below setp */
+/* _RfPowerSave(Adapter); */
+
+	_InitAntenna_Selection(Adapter);
+
+	/*  */
+	/*  Disable BAR, suggested by Scott */
+	/*  2010.04.09 add by hpfan */
+	/*  */
+	rtw_write32(Adapter, REG_BAR_MODE_CTRL, 0x0201ffff);
+
+	/*  HW SEQ CTRL */
+	/* set 0x0 to 0xFF by tynli. Default enable HW SEQ NUM. */
+	rtw_write8(Adapter, REG_HWSEQ_CTRL, 0xFF);
+
+	if (pregistrypriv->wifi_spec)
+		rtw_write16(Adapter, REG_FAST_EDCA_CTRL, 0);
+
+	/* Nav limit , suggest by scott */
+	rtw_write8(Adapter, 0x652, 0x0);
+
+	rtl8188e_InitHalDm(Adapter);
+
+	/*  2010/08/11 MH Merge from 8192SE for Minicard init. We need to confirm current radio status */
+	/*  and then decide to enable RF or not.!!!??? For Selective suspend mode. We may not */
+	/*  call initstruct adapter. May cause some problem?? */
+	/*  Fix the bug that Hw/Sw radio off before S3/S4, the RF off action will not be executed */
+	/*  in MgntActSet_RF_State() after wake up, because the value of haldata->eRFPowerState */
+	/*  is the same as eRfOff, we should change it to eRfOn after we config RF parameters. */
+	/*  Added by tynli. 2010.03.30. */
+	pwrctrlpriv->rf_pwrstate = rf_on;
+
+	/*  enable Tx report. */
+	rtw_write8(Adapter,  REG_FWHW_TXQ_CTRL + 1, 0x0F);
+
+	/*  Suggested by SD1 pisa. Added by tynli. 2011.10.21. */
+	rtw_write8(Adapter, REG_EARLY_MODE_CONTROL + 3, 0x01);/* Pretx_en, for WEP/TKIP SEC */
+
+	/* tynli_test_tx_report. */
+	rtw_write16(Adapter, REG_TX_RPT_TIME, 0x3DF0);
+
+	/* enable tx DMA to drop the redundate data of packet */
+	res = rtw_read16(Adapter, REG_TXDMA_OFFSET_CHK, &value16);
+	if (res)
+		return _FAIL;
+
+	rtw_write16(Adapter, REG_TXDMA_OFFSET_CHK, (value16 | DROP_DATA_EN));
+
+	/*  2010/08/26 MH Merge from 8192CE. */
+	if (pwrctrlpriv->rf_pwrstate == rf_on) {
+		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
+			PHY_IQCalibrate_8188E(Adapter, true);
+		} else {
+			PHY_IQCalibrate_8188E(Adapter, false);
+			haldata->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
+		}
+
+		ODM_TXPowerTrackingCheck(&haldata->odmpriv);
+
+		PHY_LCCalibrate_8188E(Adapter);
+	}
+
+/*	_InitPABias(Adapter); */
+	rtw_write8(Adapter, REG_USB_HRPWM, 0);
+
+	/* ack for xmit mgmt frames. */
+	res = rtw_read32(Adapter, REG_FWHW_TXQ_CTRL, &reg);
+	if (res)
+		return _FAIL;
+
+	rtw_write32(Adapter, REG_FWHW_TXQ_CTRL, reg | BIT(12));
+
+exit:
+	return status;
+}
+
+static void CardDisableRTL8188EU(struct adapter *Adapter)
+{
+	u8 val8;
+	struct hal_data_8188e *haldata = &Adapter->haldata;
+	int res;
+
+	/* Stop Tx Report Timer. 0x4EC[Bit1]=b'0 */
+	res = rtw_read8(Adapter, REG_TX_RPT_CTRL, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_TX_RPT_CTRL, val8 & (~BIT(1)));
+
+	/*  stop rx */
+	rtw_write8(Adapter, REG_CR, 0x0);
+
+	/*  Run LPS WL RFOFF flow */
+	HalPwrSeqCmdParsing(Adapter, LPS_ENTER_FLOW);
+
+	/*  2. 0x1F[7:0] = 0		turn off RF */
+
+	res = rtw_read8(Adapter, REG_MCUFWDL, &val8);
+	if (res)
+		return;
+
+	if ((val8 & RAM_DL_SEL) && Adapter->bFWReady) { /* 8051 RAM code */
+		/*  Reset MCU 0x2[10]=0. */
+		res = rtw_read8(Adapter, REG_SYS_FUNC_EN + 1, &val8);
+		if (res)
+			return;
+
+		val8 &= ~BIT(2);	/*  0x2[10], FEN_CPUEN */
+		rtw_write8(Adapter, REG_SYS_FUNC_EN + 1, val8);
+	}
+
+	/*  reset MCU ready status */
+	rtw_write8(Adapter, REG_MCUFWDL, 0);
+
+	/* YJ,add,111212 */
+	/* Disable 32k */
+	res = rtw_read8(Adapter, REG_32K_CTRL, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_32K_CTRL, val8 & (~BIT(0)));
+
+	/*  Card disable power action flow */
+	HalPwrSeqCmdParsing(Adapter, DISABLE_FLOW);
+
+	/*  Reset MCU IO Wrapper */
+	res = rtw_read8(Adapter, REG_RSV_CTRL + 1, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_RSV_CTRL + 1, (val8 & (~BIT(3))));
+
+	res = rtw_read8(Adapter, REG_RSV_CTRL + 1, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_RSV_CTRL + 1, val8 | BIT(3));
+
+	/* YJ,test add, 111207. For Power Consumption. */
+	res = rtw_read8(Adapter, GPIO_IN, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, GPIO_OUT, val8);
+	rtw_write8(Adapter, GPIO_IO_SEL, 0xFF);/* Reg0x46 */
+
+	res = rtw_read8(Adapter, REG_GPIO_IO_SEL, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_GPIO_IO_SEL, (val8 << 4));
+	res = rtw_read8(Adapter, REG_GPIO_IO_SEL + 1, &val8);
+	if (res)
+		return;
+
+	rtw_write8(Adapter, REG_GPIO_IO_SEL + 1, val8 | 0x0F);/* Reg0x43 */
+	rtw_write32(Adapter, REG_BB_PAD_CTRL, 0x00080808);/* set LNA ,TRSW,EX_PA Pin to output mode */
+	haldata->bMacPwrCtrlOn = false;
+	Adapter->bFWReady = false;
+}
+
+u32 rtl8188eu_hal_deinit(struct adapter *Adapter)
+{
+	rtw_write32(Adapter, REG_HIMR_88E, IMR_DISABLED_88E);
+	rtw_write32(Adapter, REG_HIMRE_88E, IMR_DISABLED_88E);
+
+	if (!Adapter->pwrctrlpriv.bkeepfwalive) {
+		if (Adapter->hw_init_completed) {
+			CardDisableRTL8188EU(Adapter);
+		}
+	}
+	return _SUCCESS;
+ }
+
+unsigned int rtl8188eu_inirp_init(struct adapter *Adapter)
+{
+	u8 i;
+	struct recv_buf *precvbuf;
+	uint	status;
+	struct recv_priv *precvpriv = &Adapter->recvpriv;
+
+	status = _SUCCESS;
+
+	/* issue Rx irp to receive data */
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		if (!rtw_read_port(Adapter, (unsigned char *)precvbuf)) {
+			status = _FAIL;
+			goto exit;
+		}
+
+		precvbuf++;
+		precvpriv->free_recv_buf_queue_cnt--;
+	}
+
+exit:
+	return status;
+}
+
+/*  */
+/*  */
+/*	EEPROM/EFUSE Content Parsing */
+/*  */
+/*  */
+
+static void Hal_EfuseParseMACAddr_8188EU(struct adapter *adapt, u8 *hwinfo, bool AutoLoadFail)
+{
+	struct eeprom_priv *eeprom = &adapt->eeprompriv;
+
+	if (AutoLoadFail) {
+		eth_random_addr(eeprom->mac_addr);
+	} else {
+		/* Read Permanent MAC address */
+		memcpy(eeprom->mac_addr, &hwinfo[EEPROM_MAC_ADDR_88EU], ETH_ALEN);
+	}
+}
+
+int ReadAdapterInfo8188EU(struct adapter *Adapter)
+{
+	struct eeprom_priv *eeprom = &Adapter->eeprompriv;
+	struct led_priv *ledpriv = &Adapter->ledpriv;
+	u8 *efuse_buf;
+	u8 eeValue;
+	int res;
+
+	/* check system boot selection */
+	res = rtw_read8(Adapter, REG_9346CR, &eeValue);
+	if (res)
+		return res;
+
+	eeprom->bautoload_fail_flag	= !(eeValue & EEPROM_EN);
+
+	efuse_buf = kmalloc(EFUSE_MAP_LEN_88E, GFP_KERNEL);
+	if (!efuse_buf)
+		return -ENOMEM;
+	memset(efuse_buf, 0xFF, EFUSE_MAP_LEN_88E);
+
+	if (!(eeValue & BOOT_FROM_EEPROM) && !eeprom->bautoload_fail_flag) {
+		rtl8188e_EfusePowerSwitch(Adapter, true);
+		rtl8188e_ReadEFuse(Adapter, EFUSE_MAP_LEN_88E, efuse_buf);
+		rtl8188e_EfusePowerSwitch(Adapter, false);
+	}
+
+	/* parse the eeprom/efuse content */
+	Hal_EfuseParseIDCode88E(Adapter, efuse_buf);
+	Hal_EfuseParseMACAddr_8188EU(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+
+	Hal_ReadPowerSavingMode88E(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+	Hal_ReadTxPowerInfo88E(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+	rtl8188e_EfuseParseChnlPlan(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+	Hal_EfuseParseXtal_8188E(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+	Hal_ReadAntennaDiversity88E(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+	Hal_ReadThermalMeter_88E(Adapter, efuse_buf, eeprom->bautoload_fail_flag);
+
+	ledpriv->bRegUseLed = true;
+	kfree(efuse_buf);
+	return 0;
+}
+
+void UpdateHalRAMask8188EUsb(struct adapter *adapt, u32 mac_id, u8 rssi_level)
+{
+	u8 init_rate = 0;
+	u8 networkType, raid;
+	u32 mask, rate_bitmap;
+	u8 shortGIrate = false;
+	int	supportRateNum = 0;
+	struct sta_info	*psta;
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct mlme_ext_priv	*pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex	*cur_network = &pmlmeinfo->network;
+
+	if (mac_id >= NUM_STA) /* CAM_SIZE */
+		return;
+	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
+	if (!psta)
+		return;
+	switch (mac_id) {
+	case 0:/*  for infra mode */
+		supportRateNum = rtw_get_rateset_len(cur_network->SupportedRates);
+		networkType = judge_network_type(adapt, cur_network->SupportedRates, supportRateNum) & 0xf;
+		raid = networktype_to_raid(networkType);
+		mask = update_supported_rate(cur_network->SupportedRates, supportRateNum);
+		mask |= (pmlmeinfo->HT_enable) ? update_MSC_rate(&pmlmeinfo->HT_caps) : 0;
+		if (support_short_GI(adapt, &pmlmeinfo->HT_caps))
+			shortGIrate = true;
+		break;
+	case 1:/* for broadcast/multicast */
+		supportRateNum = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+			networkType = WIRELESS_11B;
+		else
+			networkType = WIRELESS_11G;
+		raid = networktype_to_raid(networkType);
+		mask = update_basic_rate(cur_network->SupportedRates, supportRateNum);
+		break;
+	default: /* for each sta in IBSS */
+		supportRateNum = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		networkType = judge_network_type(adapt, pmlmeinfo->FW_sta_info[mac_id].SupportedRates, supportRateNum) & 0xf;
+		raid = networktype_to_raid(networkType);
+		mask = update_supported_rate(cur_network->SupportedRates, supportRateNum);
+
+		/* todo: support HT in IBSS */
+		break;
+	}
+
+	rate_bitmap = 0x0fffffff;
+	rate_bitmap = ODM_Get_Rate_Bitmap(&haldata->odmpriv, mac_id, mask, rssi_level);
+
+	mask &= rate_bitmap;
+
+	init_rate = get_highest_rate_idx(mask) & 0x3f;
+
+	if (haldata->fw_ractrl) {
+		mask |= ((raid << 28) & 0xf0000000);
+		psta->ra_mask = mask;
+		mask |= ((raid << 28) & 0xf0000000);
+
+		/* to do ,for 8188E-SMIC */
+		rtl8188e_set_raid_cmd(adapt, mask);
+	} else {
+		ODM_RA_UpdateRateInfo_8188E(&haldata->odmpriv,
+				mac_id,
+				raid,
+				mask,
+				shortGIrate
+				);
+	}
+	/* set ra_id */
+	psta->raid = raid;
+	psta->init_rate = init_rate;
+}
+
+void SetBeaconRelatedRegisters8188EUsb(struct adapter *adapt)
+{
+	u32 value32;
+	struct mlme_ext_priv	*pmlmeext = &adapt->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u32 bcn_ctrl_reg			= REG_BCN_CTRL;
+	int res;
+	u8 reg;
+	/* reset TSF, enable update TSF, correcting TSF On Beacon */
+
+	/* BCN interval */
+	rtw_write16(adapt, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
+	rtw_write8(adapt, REG_ATIMWND, 0x02);/*  2ms */
+
+	_InitBeaconParameters(adapt);
+
+	rtw_write8(adapt, REG_SLOT, 0x09);
+
+	res = rtw_read32(adapt, REG_TCR, &value32);
+	if (res)
+		return;
+
+	value32 &= ~TSFRST;
+	rtw_write32(adapt,  REG_TCR, value32);
+
+	value32 |= TSFRST;
+	rtw_write32(adapt, REG_TCR, value32);
+
+	/*  NOTE: Fix test chip's bug (about contention windows's randomness) */
+	rtw_write8(adapt,  REG_RXTSF_OFFSET_CCK, 0x50);
+	rtw_write8(adapt, REG_RXTSF_OFFSET_OFDM, 0x50);
+
+	_BeaconFunctionEnable(adapt);
+
+	rtw_resume_tx_beacon(adapt);
+
+	res = rtw_read8(adapt, bcn_ctrl_reg, &reg);
+	if (res)
+		return;
+
+	rtw_write8(adapt, bcn_ctrl_reg, reg | BIT(1));
+}
+
+void rtl8188eu_init_default_value(struct adapter *adapt)
+{
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct pwrctrl_priv *pwrctrlpriv;
+	u8 i;
+
+	pwrctrlpriv = &adapt->pwrctrlpriv;
+
+	/* init default value */
+	haldata->fw_ractrl = false;
+	if (!pwrctrlpriv->bkeepfwalive)
+		haldata->LastHMEBoxNum = 0;
+
+	/* init dm default value */
+	haldata->odmpriv.RFCalibrateInfo.bIQKInitialized = false;
+	haldata->odmpriv.RFCalibrateInfo.TM_Trigger = 0;/* for IQK */
+	haldata->pwrGroupCnt = 0;
+	haldata->odmpriv.RFCalibrateInfo.ThermalValue_HP_index = 0;
+	for (i = 0; i < HP_THERMAL_NUM; i++)
+		haldata->odmpriv.RFCalibrateInfo.ThermalValue_HP[i] = 0;
+}
diff --git a/drivers/staging/r8188eu/hal/usb_ops_linux.c b/drivers/staging/r8188eu/hal/usb_ops_linux.c
new file mode 100644
index 000000000..7c72f5e04
--- /dev/null
+++ b/drivers/staging/r8188eu/hal/usb_ops_linux.c
@@ -0,0 +1,502 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/osdep_intf.h"
+#include "../include/usb_ops.h"
+#include "../include/rtl8188e_hal.h"
+
+static int usb_read(struct intf_hdl *intf, u16 value, void *data, u8 size)
+{
+	struct adapter *adapt = intf->padapter;
+	struct dvobj_priv *dvobjpriv = adapter_to_dvobj(adapt);
+	struct usb_device *udev = dvobjpriv->pusbdev;
+	int status;
+	u8 io_buf[4];
+
+	if (adapt->bSurpriseRemoved)
+		return -EPERM;
+
+	status = usb_control_msg_recv(udev, 0, REALTEK_USB_VENQT_CMD_REQ,
+				      REALTEK_USB_VENQT_READ, value,
+				      REALTEK_USB_VENQT_CMD_IDX, io_buf,
+				      size, RTW_USB_CONTROL_MSG_TIMEOUT,
+				      GFP_KERNEL);
+
+	if (status == -ESHUTDOWN ||
+	    status == -ENODEV ||
+	    status == -ENOENT) {
+		/*
+		 * device or controller has been disabled due to
+		 * some problem that could not be worked around,
+		 * device or bus doesn’t exist, endpoint does not
+		 * exist or is not enabled.
+		 */
+		adapt->bSurpriseRemoved = true;
+		return status;
+	}
+
+	if (status < 0) {
+		if (rtw_inc_and_chk_continual_urb_error(dvobjpriv))
+			adapt->bSurpriseRemoved = true;
+
+		return status;
+	}
+
+	rtw_reset_continual_urb_error(dvobjpriv);
+	memcpy(data, io_buf, size);
+
+	return status;
+}
+
+static int usb_write(struct intf_hdl *intf, u16 value, void *data, u8 size)
+{
+	struct adapter *adapt = intf->padapter;
+	struct dvobj_priv *dvobjpriv = adapter_to_dvobj(adapt);
+	struct usb_device *udev = dvobjpriv->pusbdev;
+	int status;
+	u8 io_buf[VENDOR_CMD_MAX_DATA_LEN];
+
+	if (adapt->bSurpriseRemoved)
+		return -EPERM;
+
+	memcpy(io_buf, data, size);
+	status = usb_control_msg_send(udev, 0, REALTEK_USB_VENQT_CMD_REQ,
+				      REALTEK_USB_VENQT_WRITE, value,
+				      REALTEK_USB_VENQT_CMD_IDX, io_buf,
+				      size, RTW_USB_CONTROL_MSG_TIMEOUT,
+				      GFP_KERNEL);
+
+	if (status == -ESHUTDOWN ||
+	    status == -ENODEV ||
+	    status == -ENOENT) {
+		/*
+		 * device or controller has been disabled due to
+		 * some problem that could not be worked around,
+		 * device or bus doesn’t exist, endpoint does not
+		 * exist or is not enabled.
+		 */
+		adapt->bSurpriseRemoved = true;
+		return status;
+	}
+
+	if (status < 0) {
+		if (rtw_inc_and_chk_continual_urb_error(dvobjpriv))
+			adapt->bSurpriseRemoved = true;
+
+		return status;
+	}
+
+	rtw_reset_continual_urb_error(dvobjpriv);
+
+	return status;
+}
+
+int __must_check rtw_read8(struct adapter *adapter, u32 addr, u8 *data)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+
+	return usb_read(intf, value, data, 1);
+}
+
+int __must_check rtw_read16(struct adapter *adapter, u32 addr, u16 *data)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	__le16 le_data;
+	int res;
+
+	res = usb_read(intf, value, &le_data, 2);
+	if (res)
+		return res;
+
+	*data = le16_to_cpu(le_data);
+
+	return 0;
+}
+
+int __must_check rtw_read32(struct adapter *adapter, u32 addr, u32 *data)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	__le32 le_data;
+	int res;
+
+	res = usb_read(intf, value, &le_data, 4);
+	if (res)
+		return res;
+
+	*data = le32_to_cpu(le_data);
+
+	return 0;
+}
+
+int rtw_write8(struct adapter *adapter, u32 addr, u8 val)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	int ret;
+
+	ret = usb_write(intf, value, &val, 1);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int rtw_write16(struct adapter *adapter, u32 addr, u16 val)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	__le16 data = cpu_to_le16(val);
+	int ret;
+
+	ret = usb_write(intf, value, &data, 2);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int rtw_write32(struct adapter *adapter, u32 addr, u32 val)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	__le32 data = cpu_to_le32(val);
+	int ret;
+
+	ret = usb_write(intf, value, &data, 4);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int rtw_writeN(struct adapter *adapter, u32 addr, u32 length, u8 *data)
+{
+	struct io_priv *io_priv = &adapter->iopriv;
+	struct intf_hdl *intf = &io_priv->intf;
+	u16 value = addr & 0xffff;
+	int ret;
+
+	if (length > VENDOR_CMD_MAX_DATA_LEN)
+		return _FAIL;
+
+	ret = usb_write(intf, value, data, length);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+static void handle_txrpt_ccx_88e(struct adapter *adapter, u8 *buf)
+{
+	struct txrpt_ccx_88e *txrpt_ccx = (struct txrpt_ccx_88e *)buf;
+
+	if (txrpt_ccx->int_ccx) {
+		if (txrpt_ccx->pkt_ok)
+			rtw_ack_tx_done(&adapter->xmitpriv,
+					RTW_SCTX_DONE_SUCCESS);
+		else
+			rtw_ack_tx_done(&adapter->xmitpriv,
+					RTW_SCTX_DONE_CCX_PKT_FAIL);
+	}
+}
+
+static int recvbuf2recvframe(struct adapter *adapt, struct sk_buff *pskb)
+{
+	u8	*pbuf;
+	u8	shift_sz = 0;
+	u16	pkt_cnt;
+	u32	pkt_offset, skb_len, alloc_sz;
+	s32	transfer_len;
+	struct recv_stat	*prxstat;
+	struct phy_stat	*pphy_status = NULL;
+	struct sk_buff *pkt_copy = NULL;
+	struct recv_frame	*precvframe = NULL;
+	struct rx_pkt_attrib	*pattrib = NULL;
+	struct hal_data_8188e *haldata = &adapt->haldata;
+	struct recv_priv	*precvpriv = &adapt->recvpriv;
+	struct __queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+
+	transfer_len = (s32)pskb->len;
+	pbuf = pskb->data;
+
+	prxstat = (struct recv_stat *)pbuf;
+	pkt_cnt = (le32_to_cpu(prxstat->rxdw2) >> 16) & 0xff;
+
+	do {
+		prxstat = (struct recv_stat *)pbuf;
+
+		precvframe = rtw_alloc_recvframe(pfree_recv_queue);
+		if (!precvframe)
+			goto _exit_recvbuf2recvframe;
+
+		INIT_LIST_HEAD(&precvframe->list);
+		precvframe->precvbuf = NULL;	/* can't access the precvbuf for new arch. */
+		precvframe->len = 0;
+
+		update_recvframe_attrib_88e(precvframe, prxstat);
+
+		pattrib = &precvframe->attrib;
+
+		if ((pattrib->crc_err) || (pattrib->icv_err)) {
+			rtw_free_recvframe(precvframe, pfree_recv_queue);
+			goto _exit_recvbuf2recvframe;
+		}
+
+		if ((pattrib->physt) && (pattrib->pkt_rpt_type == NORMAL_RX))
+			pphy_status = (struct phy_stat *)(pbuf + RXDESC_OFFSET);
+
+		pkt_offset = RXDESC_SIZE + pattrib->drvinfo_sz + pattrib->shift_sz + pattrib->pkt_len;
+
+		if ((pattrib->pkt_len <= 0) || (pkt_offset > transfer_len)) {
+			rtw_free_recvframe(precvframe, pfree_recv_queue);
+			goto _exit_recvbuf2recvframe;
+		}
+
+		/*	Modified by Albert 20101213 */
+		/*	For 8 bytes IP header alignment. */
+		if (pattrib->qos)	/*	Qos data, wireless lan header length is 26 */
+			shift_sz = 6;
+		else
+			shift_sz = 0;
+
+		skb_len = pattrib->pkt_len;
+
+		/*  for first fragment packet, driver need allocate 1536+drvinfo_sz+RXDESC_SIZE to defrag packet. */
+		/*  modify alloc_sz for recvive crc error packet by thomas 2011-06-02 */
+		if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+			if (skb_len <= 1650)
+				alloc_sz = 1664;
+			else
+				alloc_sz = skb_len + 14;
+		} else {
+			alloc_sz = skb_len;
+			/*	6 is for IP header 8 bytes alignment in QoS packet case. */
+			/*	8 is for skb->data 4 bytes alignment. */
+			alloc_sz += 14;
+		}
+
+		pkt_copy = netdev_alloc_skb(adapt->pnetdev, alloc_sz);
+		if (pkt_copy) {
+			precvframe->pkt = pkt_copy;
+			precvframe->rx_head = pkt_copy->data;
+			precvframe->rx_end = pkt_copy->data + alloc_sz;
+			skb_reserve(pkt_copy, 8 - ((size_t)(pkt_copy->data) & 7));/* force pkt_copy->data at 8-byte alignment address */
+			skb_reserve(pkt_copy, shift_sz);/* force ip_hdr at 8-byte alignment address according to shift_sz. */
+			memcpy(pkt_copy->data, (pbuf + pattrib->drvinfo_sz + RXDESC_SIZE), skb_len);
+			precvframe->rx_tail = pkt_copy->data;
+			precvframe->rx_data = pkt_copy->data;
+		} else {
+			if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+				rtw_free_recvframe(precvframe, pfree_recv_queue);
+				goto _exit_recvbuf2recvframe;
+			}
+			precvframe->pkt = skb_clone(pskb, GFP_ATOMIC);
+			if (precvframe->pkt) {
+				precvframe->rx_tail = pbuf + pattrib->drvinfo_sz + RXDESC_SIZE;
+				precvframe->rx_head = precvframe->rx_tail;
+				precvframe->rx_data = precvframe->rx_tail;
+				precvframe->rx_end =  pbuf + pattrib->drvinfo_sz + RXDESC_SIZE + alloc_sz;
+			} else {
+				rtw_free_recvframe(precvframe, pfree_recv_queue);
+				goto _exit_recvbuf2recvframe;
+			}
+		}
+
+		recvframe_put(precvframe, skb_len);
+
+		pkt_offset = (u16)round_up(pkt_offset, 128);
+
+		if (pattrib->pkt_rpt_type == NORMAL_RX) { /* Normal rx packet */
+			if (pattrib->physt)
+				update_recvframe_phyinfo_88e(precvframe, (struct phy_stat *)pphy_status);
+			rtw_recv_entry(precvframe);
+		} else {
+			/* enqueue recvframe to txrtp queue */
+			if (pattrib->pkt_rpt_type == TX_REPORT1) {
+				/* CCX-TXRPT ack for xmit mgmt frames. */
+				handle_txrpt_ccx_88e(adapt, precvframe->rx_data);
+			} else if (pattrib->pkt_rpt_type == TX_REPORT2) {
+				ODM_RA_TxRPT2Handle_8188E(
+							&haldata->odmpriv,
+							precvframe->rx_data,
+							pattrib->pkt_len,
+							pattrib->MacIDValidEntry[0],
+							pattrib->MacIDValidEntry[1]
+							);
+			}
+			rtw_free_recvframe(precvframe, pfree_recv_queue);
+		}
+		pkt_cnt--;
+		transfer_len -= pkt_offset;
+		pbuf += pkt_offset;
+		precvframe = NULL;
+		pkt_copy = NULL;
+
+		if (transfer_len > 0 && pkt_cnt == 0)
+			pkt_cnt = (le32_to_cpu(prxstat->rxdw2) >> 16) & 0xff;
+
+	} while ((transfer_len > 0) && (pkt_cnt > 0));
+
+_exit_recvbuf2recvframe:
+
+	return _SUCCESS;
+}
+
+void rtl8188eu_recv_tasklet(unsigned long priv)
+{
+	struct sk_buff *pskb;
+	struct adapter *adapt = (struct adapter *)priv;
+	struct recv_priv *precvpriv = &adapt->recvpriv;
+
+	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
+		if ((adapt->bDriverStopped) || (adapt->bSurpriseRemoved)) {
+			dev_kfree_skb_any(pskb);
+			break;
+		}
+		recvbuf2recvframe(adapt, pskb);
+		skb_reset_tail_pointer(pskb);
+		pskb->len = 0;
+		skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+	}
+}
+
+static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
+{
+	struct recv_buf	*precvbuf = (struct recv_buf *)purb->context;
+	struct adapter	*adapt = (struct adapter *)precvbuf->adapter;
+	struct recv_priv *precvpriv = &adapt->recvpriv;
+
+	precvpriv->rx_pending_cnt--;
+
+	if (adapt->bSurpriseRemoved || adapt->bDriverStopped || adapt->bReadPortCancel) {
+		precvbuf->reuse = true;
+		return;
+	}
+
+	if (purb->status == 0) { /* SUCCESS */
+		if ((purb->actual_length > MAX_RECVBUF_SZ) || (purb->actual_length < RXDESC_SIZE)) {
+			precvbuf->reuse = true;
+			rtw_read_port(adapt, (unsigned char *)precvbuf);
+		} else {
+			rtw_reset_continual_urb_error(adapter_to_dvobj(adapt));
+
+			skb_put(precvbuf->pskb, purb->actual_length);
+			skb_queue_tail(&precvpriv->rx_skb_queue, precvbuf->pskb);
+
+			if (skb_queue_len(&precvpriv->rx_skb_queue) <= 1)
+				tasklet_schedule(&precvpriv->recv_tasklet);
+
+			precvbuf->pskb = NULL;
+			precvbuf->reuse = false;
+			rtw_read_port(adapt, (unsigned char *)precvbuf);
+		}
+	} else {
+		skb_put(precvbuf->pskb, purb->actual_length);
+		precvbuf->pskb = NULL;
+
+		if (rtw_inc_and_chk_continual_urb_error(adapter_to_dvobj(adapt)))
+			adapt->bSurpriseRemoved = true;
+
+		switch (purb->status) {
+		case -EINVAL:
+		case -EPIPE:
+		case -ENODEV:
+		case -ESHUTDOWN:
+		case -ENOENT:
+			adapt->bDriverStopped = true;
+			break;
+		case -EPROTO:
+		case -EOVERFLOW:
+			precvbuf->reuse = true;
+			rtw_read_port(adapt, (unsigned char *)precvbuf);
+			break;
+		case -EINPROGRESS:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+u32 rtw_read_port(struct adapter *adapter, u8 *rmem)
+{
+	struct urb *purb = NULL;
+	struct recv_buf	*precvbuf = (struct recv_buf *)rmem;
+	struct dvobj_priv	*pdvobj = adapter_to_dvobj(adapter);
+	struct recv_priv	*precvpriv = &adapter->recvpriv;
+	struct usb_device	*pusbd = pdvobj->pusbdev;
+	int err;
+	unsigned int pipe;
+	size_t tmpaddr = 0;
+	size_t alignment = 0;
+	u32 ret = _SUCCESS;
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return _FAIL;
+
+	if (!precvbuf)
+		return _FAIL;
+
+	if (!precvbuf->reuse || !precvbuf->pskb) {
+		precvbuf->pskb = skb_dequeue(&precvpriv->free_recv_skb_queue);
+		if (precvbuf->pskb)
+			precvbuf->reuse = true;
+	}
+
+	/* re-assign for linux based on skb */
+	if (!precvbuf->reuse || !precvbuf->pskb) {
+		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
+		if (!precvbuf->pskb)
+			return _FAIL;
+
+		tmpaddr = (size_t)precvbuf->pskb->data;
+		alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+		skb_reserve(precvbuf->pskb, (RECVBUFF_ALIGN_SZ - alignment));
+	} else { /* reuse skb */
+		precvbuf->reuse = false;
+	}
+
+	precvpriv->rx_pending_cnt++;
+
+	purb = precvbuf->purb;
+
+	/* translate DMA FIFO addr to pipehandle */
+	pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe);
+
+	usb_fill_bulk_urb(purb, pusbd, pipe,
+			  precvbuf->pskb->data,
+			  MAX_RECVBUF_SZ,
+			  usb_read_port_complete,
+			  precvbuf);/* context is precvbuf */
+
+	err = usb_submit_urb(purb, GFP_ATOMIC);
+	if ((err) && (err != (-EPERM)))
+		ret = _FAIL;
+
+	return ret;
+}
+
+void rtl8188eu_xmit_tasklet(unsigned long priv)
+{
+	int ret = false;
+	struct adapter *adapt = (struct adapter *)priv;
+	struct xmit_priv *pxmitpriv = &adapt->xmitpriv;
+
+	if (check_fwstate(&adapt->mlmepriv, _FW_UNDER_SURVEY))
+		return;
+
+	while (1) {
+		if ((adapt->bDriverStopped) ||
+		    (adapt->bSurpriseRemoved) ||
+		    (adapt->bWritePortCancel))
+			break;
+
+		ret = rtl8188eu_xmitframe_complete(adapt, pxmitpriv, NULL);
+
+		if (!ret)
+			break;
+	}
+}
diff --git a/drivers/staging/r8188eu/include/Hal8188EPhyCfg.h b/drivers/staging/r8188eu/include/Hal8188EPhyCfg.h
new file mode 100644
index 000000000..4a0b782c3
--- /dev/null
+++ b/drivers/staging/r8188eu/include/Hal8188EPhyCfg.h
@@ -0,0 +1,97 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __INC_HAL8188EPHYCFG_H__
+#define __INC_HAL8188EPHYCFG_H__
+
+#define MAX_AGGR_NUM			0x07
+
+enum rf_radio_path {
+	RF_PATH_A = 0,			/* Radio Path A */
+	RF_PATH_B = 1,			/* Radio Path B */
+};
+
+#define MAX_PG_GROUP 13
+
+#define	RF_PATH_MAX			3
+#define		MAX_TX_COUNT		4 /* path numbers */
+
+#define CHANNEL_MAX_NUMBER		14	/*  14 is the max chnl number */
+#define MAX_CHNL_GROUP_24G		6	/*  ch1~2, ch3~5, ch6~8,
+						 *ch9~11, ch12~13, CH 14
+						 * total three groups */
+
+struct bb_reg_def {
+	u32 rfintfs;		/*  set software control: */
+				/*	0x870~0x877[8 bytes] */
+	u32 rfintfi;		/*  readback data: */
+				/*	0x8e0~0x8e7[8 bytes] */
+	u32 rfintfo;		/*  output data: */
+				/*	0x860~0x86f [16 bytes] */
+	u32 rfintfe;		/*  output enable: */
+				/*	0x860~0x86f [16 bytes] */
+	u32 rf3wireOffset;	/*  LSSI data: */
+				/*	0x840~0x84f [16 bytes] */
+	u32 rfLSSI_Select;	/*  BB Band Select: */
+				/*	0x878~0x87f [8 bytes] */
+	u32 rfTxGainStage;	/*  Tx gain stage: */
+				/*	0x80c~0x80f [4 bytes] */
+	u32 rfHSSIPara1;	/*  wire parameter control1 : */
+				/*	0x820~0x823,0x828~0x82b,
+				 *	0x830~0x833, 0x838~0x83b [16 bytes] */
+	u32 rfHSSIPara2;	/*  wire parameter control2 : */
+				/*	0x824~0x827,0x82c~0x82f, 0x834~0x837,
+				 *	0x83c~0x83f [16 bytes] */
+	u32 rfSwitchControl;	/* Tx Rx antenna control : */
+				/*	0x858~0x85f [16 bytes] */
+	u32 rfAGCControl1;	/* AGC parameter control1 : */
+				/*	0xc50~0xc53,0xc58~0xc5b, 0xc60~0xc63,
+				 * 0xc68~0xc6b [16 bytes] */
+	u32 rfAGCControl2;	/* AGC parameter control2 : */
+				/*	0xc54~0xc57,0xc5c~0xc5f, 0xc64~0xc67,
+				 *	0xc6c~0xc6f [16 bytes] */
+	u32 rfRxIQImbalance;	/* OFDM Rx IQ imbalance matrix : */
+				/*	0xc14~0xc17,0xc1c~0xc1f, 0xc24~0xc27,
+				 *	0xc2c~0xc2f [16 bytes] */
+	u32 rfRxAFE;		/* Rx IQ DC ofset and Rx digital filter,
+				 * Rx DC notch filter : */
+				/*	0xc10~0xc13,0xc18~0xc1b, 0xc20~0xc23,
+				 *	0xc28~0xc2b [16 bytes] */
+	u32 rfTxIQImbalance;	/* OFDM Tx IQ imbalance matrix */
+				/*	0xc80~0xc83,0xc88~0xc8b, 0xc90~0xc93,
+				 *	 0xc98~0xc9b [16 bytes] */
+	u32 rfTxAFE;		/* Tx IQ DC Offset and Tx DFIR type */
+				/*	0xc84~0xc87,0xc8c~0xc8f, 0xc94~0xc97,
+				 *	0xc9c~0xc9f [16 bytes] */
+	u32 rfLSSIReadBack;	/* LSSI RF readback data SI mode */
+				/*	0x8a0~0x8af [16 bytes] */
+	u32 rfLSSIReadBackPi;	/* LSSI RF readback data PI mode 0x8b8-8bc for
+				 * Path A and B */
+};
+
+/*  BB and RF register read/write */
+u32 rtl8188e_PHY_QueryBBReg(struct adapter *adapter, u32 regaddr, u32 mask);
+void rtl8188e_PHY_SetBBReg(struct adapter *Adapter, u32 RegAddr,
+			   u32 mask, u32 data);
+u32 rtl8188e_PHY_QueryRFReg(struct adapter *adapter, u32 regaddr, u32 mask);
+void rtl8188e_PHY_SetRFReg(struct adapter *adapter, u32 regaddr, u32 mask, u32 data);
+
+/*  Initialization related function */
+/* MAC/BB/RF HAL config */
+int PHY_MACConfig8188E(struct adapter *adapter);
+int PHY_BBConfig8188E(struct adapter *adapter);
+
+/*  BB TX Power R/W */
+void PHY_SetTxPowerLevel8188E(struct adapter *adapter, u8 channel);
+
+/*  Switch bandwidth for 8192S */
+void PHY_SetBWMode8188E(struct adapter *adapter,
+			enum ht_channel_width chnlwidth, unsigned char offset);
+
+/*  channel switch related funciton */
+void PHY_SwChnl8188E(struct adapter *adapter, u8 channel);
+
+void storePwrIndexDiffRateOffset(struct adapter *adapter, u32 regaddr,
+				 u32 mask, u32 data);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/Hal8188EPhyReg.h b/drivers/staging/r8188eu/include/Hal8188EPhyReg.h
new file mode 100644
index 000000000..8b8c75a1f
--- /dev/null
+++ b/drivers/staging/r8188eu/include/Hal8188EPhyReg.h
@@ -0,0 +1,1072 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __INC_HAL8188EPHYREG_H__
+#define __INC_HAL8188EPHYREG_H__
+/*--------------------------Define Parameters-------------------------------*/
+/*  */
+/*  BB-PHY register PMAC 0x100 PHY 0x800 - 0xEFF */
+/*  1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF */
+/*  2. 0x800/0x900/0xA00/0xC00/0xD00/0xE00 */
+/*  3. RF register 0x00-2E */
+/*  4. Bit Mask for BB/RF register */
+/*  5. Other definition for BB/RF R/W */
+/*  */
+
+/*  */
+/*  1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF */
+/*  1. Page1(0x100) */
+/*  */
+#define	rPMAC_Reset		0x100
+#define	rPMAC_TxStart		0x104
+#define	rPMAC_TxLegacySIG	0x108
+#define	rPMAC_TxHTSIG1		0x10c
+#define	rPMAC_TxHTSIG2		0x110
+#define	rPMAC_PHYDebug		0x114
+#define	rPMAC_TxPacketNum	0x118
+#define	rPMAC_TxIdle		0x11c
+#define	rPMAC_TxMACHeader0	0x120
+#define	rPMAC_TxMACHeader1	0x124
+#define	rPMAC_TxMACHeader2	0x128
+#define	rPMAC_TxMACHeader3	0x12c
+#define	rPMAC_TxMACHeader4	0x130
+#define	rPMAC_TxMACHeader5	0x134
+#define	rPMAC_TxDataType	0x138
+#define	rPMAC_TxRandomSeed	0x13c
+#define	rPMAC_CCKPLCPPreamble	0x140
+#define	rPMAC_CCKPLCPHeader	0x144
+#define	rPMAC_CCKCRC16		0x148
+#define	rPMAC_OFDMRxCRC32OK	0x170
+#define	rPMAC_OFDMRxCRC32Er	0x174
+#define	rPMAC_OFDMRxParityEr	0x178
+#define	rPMAC_OFDMRxCRC8Er	0x17c
+#define	rPMAC_CCKCRxRC16Er	0x180
+#define	rPMAC_CCKCRxRC32Er	0x184
+#define	rPMAC_CCKCRxRC32OK	0x188
+#define	rPMAC_TxStatus		0x18c
+
+/*  2. Page2(0x200) */
+/*  The following two definition are only used for USB interface. */
+#define	RF_BB_CMD_ADDR		0x02c0	/*  RF/BB r/w cmd address. */
+#define	RF_BB_CMD_DATA		0x02c4	/*  RF/BB r/w cmd data. */
+
+/*  3. Page8(0x800) */
+#define	rFPGA0_RFMOD		0x800	/* RF mode & CCK TxSC RF BW Setting */
+
+#define	rFPGA0_TxInfo		0x804	/*  Status report?? */
+#define	rFPGA0_PSDFunction	0x808
+
+#define	rFPGA0_TxGainStage	0x80c	/*  Set TX PWR init gain? */
+
+#define	rFPGA0_RFTiming1	0x810	/*  Useless now */
+#define	rFPGA0_RFTiming2	0x814
+
+#define	rFPGA0_XA_HSSIParameter1	0x820	/*  RF 3 wire register */
+#define	rFPGA0_XA_HSSIParameter2	0x824
+#define	rFPGA0_XB_HSSIParameter1	0x828
+#define	rFPGA0_XB_HSSIParameter2	0x82c
+
+#define	rFPGA0_XA_LSSIParameter		0x840
+#define	rFPGA0_XB_LSSIParameter		0x844
+
+#define	rFPGA0_RFWakeUpParameter	0x850	/*  Useless now */
+#define	rFPGA0_RFSleepUpParameter	0x854
+
+#define	rFPGA0_XAB_SwitchControl	0x858	/*  RF Channel switch */
+#define	rFPGA0_XCD_SwitchControl	0x85c
+
+#define	rFPGA0_XA_RFInterfaceOE		0x860	/*  RF Channel switch */
+#define	rFPGA0_XB_RFInterfaceOE		0x864
+
+#define	rFPGA0_XAB_RFInterfaceSW	0x870	/*  RF Iface Software Control */
+#define	rFPGA0_XCD_RFInterfaceSW	0x874
+
+#define	rFPGA0_XAB_RFParameter		0x878	/*  RF Parameter */
+#define	rFPGA0_XCD_RFParameter		0x87c
+
+/* Crystal cap setting RF-R/W protection for parameter4?? */
+#define	rFPGA0_AnalogParameter1		0x880
+#define	rFPGA0_AnalogParameter2		0x884
+#define	rFPGA0_AnalogParameter3		0x888
+/*  enable ad/da clock1 for dual-phy */
+#define	rFPGA0_AdDaClockEn		0x888
+#define	rFPGA0_AnalogParameter4		0x88c
+
+#define	rFPGA0_XA_LSSIReadBack		0x8a0	/*  Tranceiver LSSI Readback */
+#define	rFPGA0_XB_LSSIReadBack		0x8a4
+#define	rFPGA0_XC_LSSIReadBack		0x8a8
+#define	rFPGA0_XD_LSSIReadBack		0x8ac
+
+#define	rFPGA0_PSDReport		0x8b4	/*  Useless now */
+/*  Transceiver A HSPI Readback */
+#define	TransceiverA_HSPI_Readback	0x8b8
+/*  Transceiver B HSPI Readback */
+#define	TransceiverB_HSPI_Readback	0x8bc
+/*  Useless now RF Interface Readback Value */
+#define	rFPGA0_XAB_RFInterfaceRB	0x8e0
+#define	rFPGA0_XCD_RFInterfaceRB	0x8e4	/*  Useless now */
+
+/*  4. Page9(0x900) */
+/* RF mode & OFDM TxSC RF BW Setting?? */
+#define	rFPGA1_RFMOD			0x900
+
+#define	rFPGA1_TxBlock			0x904	/*  Useless now */
+#define	rFPGA1_DebugSelect		0x908	/*  Useless now */
+#define	rFPGA1_TxInfo			0x90c	/*  Useless now Status report */
+
+/*  5. PageA(0xA00) */
+/*  Set Control channel to upper or lower - required only for 40MHz */
+#define	rCCK0_System			0xa00
+
+/*  Disable init gain now Select RX path by RSSI */
+#define	rCCK0_AFESetting		0xa04
+/*  Disable init gain now Init gain */
+#define	rCCK0_CCA			0xa08
+
+/* AGC default value, saturation level Antenna Diversity, RX AGC, LNA Threshold,
+ * RX LNA Threshold useless now. Not the same as 90 series */
+#define	rCCK0_RxAGC1			0xa0c
+#define	rCCK0_RxAGC2			0xa10	/* AGC & DAGC */
+
+#define	rCCK0_RxHP			0xa14
+
+/* Timing recovery & Channel estimation threshold */
+#define	rCCK0_DSPParameter1		0xa18
+#define	rCCK0_DSPParameter2		0xa1c	/* SQ threshold */
+
+#define	rCCK0_TxFilter1			0xa20
+#define	rCCK0_TxFilter2			0xa24
+#define	rCCK0_DebugPort			0xa28	/* debug port and Tx filter3 */
+#define	rCCK0_FalseAlarmReport		0xa2c	/* 0xa2d useless now */
+#define	rCCK0_TRSSIReport		0xa50
+#define	rCCK0_RxReport			0xa54  /* 0xa57 */
+#define	rCCK0_FACounterLower		0xa5c  /* 0xa5b */
+#define	rCCK0_FACounterUpper		0xa58  /* 0xa5c */
+
+/*  */
+/*  PageB(0xB00) */
+/*  */
+#define	rPdp_AntA			0xb00
+#define	rPdp_AntA_4			0xb04
+#define	rConfig_Pmpd_AntA		0xb28
+#define	rConfig_AntA			0xb68
+#define	rConfig_AntB			0xb6c
+#define	rPdp_AntB			0xb70
+#define	rPdp_AntB_4			0xb74
+#define	rConfig_Pmpd_AntB		0xb98
+#define	rAPK				0xbd8
+
+/*  */
+/*  6. PageC(0xC00) */
+/*  */
+#define	rOFDM0_LSTF			0xc00
+
+#define	rOFDM0_TRxPathEnable		0xc04
+#define	rOFDM0_TRMuxPar			0xc08
+#define	rOFDM0_TRSWIsolation		0xc0c
+
+/* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define	rOFDM0_XARxAFE			0xc10
+#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imblance matrix */
+#define	rOFDM0_XBRxAFE			0xc18
+#define	rOFDM0_XBRxIQImbalance		0xc1c
+#define	rOFDM0_XCRxAFE			0xc20
+#define	rOFDM0_XCRxIQImbalance		0xc24
+#define	rOFDM0_XDRxAFE			0xc28
+#define	rOFDM0_XDRxIQImbalance		0xc2c
+
+#define	rOFDM0_RxDetector1		0xc30  /*PD,BW & SBD DM tune init gain*/
+#define	rOFDM0_RxDetector2		0xc34  /* SBD & Fame Sync. */
+#define	rOFDM0_RxDetector3		0xc38  /* Frame Sync. */
+#define	rOFDM0_RxDetector4		0xc3c  /* PD, SBD, Frame Sync & Short-GI */
+
+#define	rOFDM0_RxDSP			0xc40  /* Rx Sync Path */
+#define	rOFDM0_CFOandDAGC		0xc44  /* CFO & DAGC */
+#define	rOFDM0_CCADropThreshold		0xc48 /* CCA Drop threshold */
+#define	rOFDM0_ECCAThreshold		0xc4c /*  energy CCA */
+
+#define	rOFDM0_XAAGCCore1		0xc50	/*  DIG */
+#define	rOFDM0_XAAGCCore2		0xc54
+#define	rOFDM0_XBAGCCore1		0xc58
+#define	rOFDM0_XBAGCCore2		0xc5c
+#define	rOFDM0_XCAGCCore1		0xc60
+#define	rOFDM0_XCAGCCore2		0xc64
+#define	rOFDM0_XDAGCCore1		0xc68
+#define	rOFDM0_XDAGCCore2		0xc6c
+
+#define	rOFDM0_AGCParameter1		0xc70
+#define	rOFDM0_AGCParameter2		0xc74
+#define	rOFDM0_AGCRSSITable		0xc78
+#define	rOFDM0_HTSTFAGC			0xc7c
+
+#define	rOFDM0_XATxIQImbalance		0xc80	/*  TX PWR TRACK and DIG */
+#define	rOFDM0_XATxAFE			0xc84
+#define	rOFDM0_XBTxIQImbalance		0xc88
+#define	rOFDM0_XBTxAFE			0xc8c
+#define	rOFDM0_XCTxIQImbalance		0xc90
+#define	rOFDM0_XCTxAFE			0xc94
+#define	rOFDM0_XDTxIQImbalance		0xc98
+#define	rOFDM0_XDTxAFE			0xc9c
+
+#define	rOFDM0_RxIQExtAnta		0xca0
+#define	rOFDM0_TxCoeff1			0xca4
+#define	rOFDM0_TxCoeff2			0xca8
+#define	rOFDM0_TxCoeff3			0xcac
+#define	rOFDM0_TxCoeff4			0xcb0
+#define	rOFDM0_TxCoeff5			0xcb4
+#define	rOFDM0_TxCoeff6			0xcb8
+#define	rOFDM0_RxHPParameter		0xce0
+#define	rOFDM0_TxPseudoNoiseWgt		0xce4
+#define	rOFDM0_FrameSync		0xcf0
+#define	rOFDM0_DFSReport		0xcf4
+
+/*  */
+/*  7. PageD(0xD00) */
+/*  */
+#define	rOFDM1_LSTF			0xd00
+#define	rOFDM1_TRxPathEnable		0xd04
+
+#define	rOFDM1_CFO			0xd08	/*  No setting now */
+#define	rOFDM1_CSI1			0xd10
+#define	rOFDM1_SBD			0xd14
+#define	rOFDM1_CSI2			0xd18
+#define	rOFDM1_CFOTracking		0xd2c
+#define	rOFDM1_TRxMesaure1		0xd34
+#define	rOFDM1_IntfDet			0xd3c
+#define	rOFDM1_PseudoNoiseStateAB	0xd50
+#define	rOFDM1_PseudoNoiseStateCD	0xd54
+#define	rOFDM1_RxPseudoNoiseWgt		0xd58
+
+#define	rOFDM_PHYCounter1		0xda0  /* cca, parity fail */
+#define	rOFDM_PHYCounter2		0xda4  /* rate illegal, crc8 fail */
+#define	rOFDM_PHYCounter3		0xda8  /* MCS not support */
+
+#define	rOFDM_ShortCFOAB		0xdac	/*  No setting now */
+#define	rOFDM_ShortCFOCD		0xdb0
+#define	rOFDM_LongCFOAB			0xdb4
+#define	rOFDM_LongCFOCD			0xdb8
+#define	rOFDM_TailCFOAB			0xdbc
+#define	rOFDM_TailCFOCD			0xdc0
+#define	rOFDM_PWMeasure1		0xdc4
+#define	rOFDM_PWMeasure2		0xdc8
+#define	rOFDM_BWReport			0xdcc
+#define	rOFDM_AGCReport			0xdd0
+#define	rOFDM_RxSNR			0xdd4
+#define	rOFDM_RxEVMCSI			0xdd8
+#define	rOFDM_SIGReport			0xddc
+
+/*  */
+/*  8. PageE(0xE00) */
+/*  */
+#define	rTxAGC_A_Rate18_06		0xe00
+#define	rTxAGC_A_Rate54_24		0xe04
+#define	rTxAGC_A_CCK1_Mcs32		0xe08
+#define	rTxAGC_A_Mcs03_Mcs00		0xe10
+#define	rTxAGC_A_Mcs07_Mcs04		0xe14
+#define	rTxAGC_A_Mcs11_Mcs08		0xe18
+#define	rTxAGC_A_Mcs15_Mcs12		0xe1c
+
+#define	rTxAGC_B_Rate18_06		0x830
+#define	rTxAGC_B_Rate54_24		0x834
+#define	rTxAGC_B_CCK1_55_Mcs32		0x838
+#define	rTxAGC_B_Mcs03_Mcs00		0x83c
+#define	rTxAGC_B_Mcs07_Mcs04		0x848
+#define	rTxAGC_B_Mcs11_Mcs08		0x84c
+#define	rTxAGC_B_Mcs15_Mcs12		0x868
+#define	rTxAGC_B_CCK11_A_CCK2_11	0x86c
+
+#define	rFPGA0_IQK			0xe28
+#define	rTx_IQK_Tone_A			0xe30
+#define	rRx_IQK_Tone_A			0xe34
+#define	rTx_IQK_PI_A			0xe38
+#define	rRx_IQK_PI_A			0xe3c
+
+#define	rTx_IQK				0xe40
+#define	rRx_IQK				0xe44
+#define	rIQK_AGC_Pts			0xe48
+#define	rIQK_AGC_Rsp			0xe4c
+#define	rTx_IQK_Tone_B			0xe50
+#define	rRx_IQK_Tone_B			0xe54
+#define	rTx_IQK_PI_B			0xe58
+#define	rRx_IQK_PI_B			0xe5c
+#define	rIQK_AGC_Cont			0xe60
+
+#define	rBlue_Tooth			0xe6c
+#define	rRx_Wait_CCA			0xe70
+#define	rTx_CCK_RFON			0xe74
+#define	rTx_CCK_BBON			0xe78
+#define	rTx_OFDM_RFON			0xe7c
+#define	rTx_OFDM_BBON			0xe80
+#define	rTx_To_Rx			0xe84
+#define	rTx_To_Tx			0xe88
+#define	rRx_CCK				0xe8c
+
+#define	rTx_Power_Before_IQK_A		0xe94
+#define	rTx_Power_After_IQK_A		0xe9c
+
+#define	rRx_Power_Before_IQK_A		0xea0
+#define	rRx_Power_Before_IQK_A_2	0xea4
+#define	rRx_Power_After_IQK_A		0xea8
+#define	rRx_Power_After_IQK_A_2		0xeac
+
+#define	rTx_Power_Before_IQK_B		0xeb4
+#define	rTx_Power_After_IQK_B		0xebc
+
+#define	rRx_Power_Before_IQK_B		0xec0
+#define	rRx_Power_Before_IQK_B_2	0xec4
+#define	rRx_Power_After_IQK_B		0xec8
+#define	rRx_Power_After_IQK_B_2		0xecc
+
+#define	rRx_OFDM			0xed0
+#define	rRx_Wait_RIFS			0xed4
+#define	rRx_TO_Rx			0xed8
+#define	rStandby			0xedc
+#define	rSleep				0xee0
+#define	rPMPD_ANAEN			0xeec
+
+/*  */
+/*  7. RF Register 0x00-0x2E (RF 8256) */
+/*     RF-0222D 0x00-3F */
+/*  */
+/* Zebra1 */
+#define	rZebra1_HSSIEnable		0x0	/*  Useless now */
+#define	rZebra1_TRxEnable1		0x1
+#define	rZebra1_TRxEnable2		0x2
+#define	rZebra1_AGC			0x4
+#define	rZebra1_ChargePump		0x5
+#define	rZebra1_Channel			0x7	/*  RF channel switch */
+
+/* endif */
+#define	rZebra1_TxGain			0x8	/*  Useless now */
+#define	rZebra1_TxLPF			0x9
+#define	rZebra1_RxLPF			0xb
+#define	rZebra1_RxHPFCorner		0xc
+
+/* Zebra4 */
+#define	rGlobalCtrl		0	/*  Useless now */
+#define	rRTL8256_TxLPF		19
+#define	rRTL8256_RxLPF		11
+
+/* RTL8258 */
+#define	rRTL8258_TxLPF		0x11	/*  Useless now */
+#define	rRTL8258_RxLPF		0x13
+#define	rRTL8258_RSSILPF	0xa
+
+/*  */
+/*  RL6052 Register definition */
+/*  */
+#define	RF_AC			0x00	/*  */
+
+#define	RF_IQADJ_G1		0x01	/*  */
+#define	RF_IQADJ_G2		0x02	/*  */
+
+#define	RF_POW_TRSW		0x05	/*  */
+
+#define	RF_GAIN_RX		0x06	/*  */
+#define	RF_GAIN_TX		0x07	/*  */
+
+#define	RF_TXM_IDAC		0x08	/*  */
+#define	RF_IPA_G		0x09	/*  */
+#define	RF_TXBIAS_G		0x0A
+#define	RF_TXPA_AG		0x0B
+#define	RF_IPA_A		0x0C	/*  */
+#define	RF_TXBIAS_A		0x0D
+#define	RF_BS_PA_APSET_G9_G11	0x0E
+#define	RF_BS_IQGEN		0x0F	/*  */
+
+#define	RF_MODE1		0x10	/*  */
+#define	RF_MODE2		0x11	/*  */
+
+#define	RF_RX_AGC_HP		0x12	/*  */
+#define	RF_TX_AGC		0x13	/*  */
+#define	RF_BIAS			0x14	/*  */
+#define	RF_IPA			0x15	/*  */
+#define	RF_TXBIAS		0x16
+#define	RF_POW_ABILITY		0x17	/*  */
+#define	RF_CHNLBW		0x18	/*  RF channel and BW switch */
+#define	RF_TOP			0x19	/*  */
+
+#define	RF_RX_G1		0x1A	/*  */
+#define	RF_RX_G2		0x1B	/*  */
+
+#define	RF_RX_BB2		0x1C	/*  */
+#define	RF_RX_BB1		0x1D	/*  */
+
+#define	RF_RCK1			0x1E	/*  */
+#define	RF_RCK2			0x1F	/*  */
+
+#define	RF_TX_G1		0x20	/*  */
+#define	RF_TX_G2		0x21	/*  */
+#define	RF_TX_G3		0x22	/*  */
+
+#define	RF_TX_BB1		0x23	/*  */
+
+#define	RF_T_METER_92D		0x42	/*  */
+#define	RF_T_METER_88E		0x42	/*  */
+#define	RF_T_METER		0x24	/*  */
+
+#define	RF_SYN_G1		0x25	/*  RF TX Power control */
+#define	RF_SYN_G2		0x26	/*  RF TX Power control */
+#define	RF_SYN_G3		0x27	/*  RF TX Power control */
+#define	RF_SYN_G4		0x28	/*  RF TX Power control */
+#define	RF_SYN_G5		0x29	/*  RF TX Power control */
+#define	RF_SYN_G6		0x2A	/*  RF TX Power control */
+#define	RF_SYN_G7		0x2B	/*  RF TX Power control */
+#define	RF_SYN_G8		0x2C	/*  RF TX Power control */
+
+#define	RF_RCK_OS		0x30	/*  RF TX PA control */
+#define	RF_TXPA_G1		0x31	/*  RF TX PA control */
+#define	RF_TXPA_G2		0x32	/*  RF TX PA control */
+#define	RF_TXPA_G3		0x33	/*  RF TX PA control */
+#define	RF_TX_BIAS_A		0x35
+#define	RF_TX_BIAS_D		0x36
+#define	RF_LOBF_9		0x38
+#define	RF_RXRF_A3		0x3C	/*  */
+#define	RF_TRSW			0x3F
+
+#define	RF_TXRF_A2		0x41
+#define	RF_TXPA_G4		0x46
+#define	RF_TXPA_A4		0x4B
+#define	RF_0x52			0x52
+#define	RF_WE_LUT		0xEF
+
+/*  */
+/* Bit Mask */
+/*  */
+/*  1. Page1(0x100) */
+#define	bBBResetB		0x100	/*  Useless now? */
+#define	bGlobalResetB		0x200
+#define	bOFDMTxStart		0x4
+#define	bCCKTxStart		0x8
+#define	bCRC32Debug		0x100
+#define	bPMACLoopback		0x10
+#define	bTxLSIG			0xffffff
+#define	bOFDMTxRate		0xf
+#define	bOFDMTxReserved		0x10
+#define	bOFDMTxLength		0x1ffe0
+#define	bOFDMTxParity		0x20000
+#define	bTxHTSIG1		0xffffff
+#define	bTxHTMCSRate		0x7f
+#define	bTxHTBW			0x80
+#define	bTxHTLength		0xffff00
+#define	bTxHTSIG2		0xffffff
+#define	bTxHTSmoothing		0x1
+#define	bTxHTSounding		0x2
+#define	bTxHTReserved		0x4
+#define	bTxHTAggreation		0x8
+#define	bTxHTSTBC		0x30
+#define	bTxHTAdvanceCoding	0x40
+#define	bTxHTShortGI		0x80
+#define	bTxHTNumberHT_LTF	0x300
+#define	bTxHTCRC8		0x3fc00
+#define	bCounterReset		0x10000
+#define	bNumOfOFDMTx		0xffff
+#define	bNumOfCCKTx		0xffff0000
+#define	bTxIdleInterval		0xffff
+#define	bOFDMService		0xffff0000
+#define	bTxMACHeader		0xffffffff
+#define	bTxDataInit		0xff
+#define	bTxHTMode		0x100
+#define	bTxDataType		0x30000
+#define	bTxRandomSeed		0xffffffff
+#define	bCCKTxPreamble		0x1
+#define	bCCKTxSFD		0xffff0000
+#define	bCCKTxSIG		0xff
+#define	bCCKTxService		0xff00
+#define	bCCKLengthExt		0x8000
+#define	bCCKTxLength		0xffff0000
+#define	bCCKTxCRC16		0xffff
+#define	bCCKTxStatus		0x1
+#define	bOFDMTxStatus		0x2
+
+#define	IS_BB_REG_OFFSET_92S(_Offset)			\
+	((_Offset >= 0x800) && (_Offset <= 0xfff))
+
+/*  2. Page8(0x800) */
+#define	bRFMOD			0x1	/*  Reg 0x800 rFPGA0_RFMOD */
+#define	bJapanMode		0x2
+#define	bCCKTxSC		0x30
+#define	bCCKEn			0x1000000
+#define	bOFDMEn			0x2000000
+
+#define	bOFDMRxADCPhase		0x10000	/*  Useless now */
+#define	bOFDMTxDACPhase		0x40000
+#define	bXATxAGC		0x3f
+
+#define	bAntennaSelect		0x0300
+
+#define	bXBTxAGC		0xf00	/*  Reg 80c rFPGA0_TxGainStage */
+#define	bXCTxAGC		0xf000
+#define	bXDTxAGC		0xf0000
+
+#define	bPAStart		0xf0000000	/*  Useless now */
+#define	bTRStart		0x00f00000
+#define	bRFStart		0x0000f000
+#define	bBBStart		0x000000f0
+#define	bBBCCKStart		0x0000000f
+#define	bPAEnd			0xf          /* Reg0x814 */
+#define	bTREnd			0x0f000000
+#define	bRFEnd			0x000f0000
+#define	bCCAMask		0x000000f0   /* T2R */
+#define	bR2RCCAMask		0x00000f00
+#define	bHSSI_R2TDelay		0xf8000000
+#define	bHSSI_T2RDelay		0xf80000
+#define	bContTxHSSI		0x400     /* change gain at continue Tx */
+#define	bIGFromCCK		0x200
+#define	bAGCAddress		0x3f
+#define	bRxHPTx			0x7000
+#define	bRxHPT2R		0x38000
+#define	bRxHPCCKIni		0xc0000
+#define	bAGCTxCode		0xc00000
+#define	bAGCRxCode		0x300000
+
+/* Reg 0x820~84f rFPGA0_XA_HSSIParameter1 */
+#define	b3WireDataLength	0x800
+#define	b3WireAddressLength	0x400
+
+#define	b3WireRFPowerDown	0x1	/*  Useless now */
+#define	b5GPAPEPolarity		0x40000000
+#define	b2GPAPEPolarity		0x80000000
+#define	bRFSW_TxDefaultAnt	0x3
+#define	bRFSW_TxOptionAnt	0x30
+#define	bRFSW_RxDefaultAnt	0x300
+#define	bRFSW_RxOptionAnt	0x3000
+#define	bRFSI_3WireData		0x1
+#define	bRFSI_3WireClock	0x2
+#define	bRFSI_3WireLoad		0x4
+#define	bRFSI_3WireRW		0x8
+#define	bRFSI_3Wire		0xf
+
+#define	bRFSI_RFENV		0x10	/* Reg 0x870 rFPGA0_XAB_RFInterfaceSW */
+
+#define	bRFSI_TRSW		0x20	/*  Useless now */
+#define	bRFSI_TRSWB		0x40
+#define	bRFSI_ANTSW		0x100
+#define	bRFSI_ANTSWB		0x200
+#define	bRFSI_PAPE		0x400
+#define	bRFSI_PAPE5G		0x800
+#define	bBandSelect		0x1
+#define	bHTSIG2_GI		0x80
+#define	bHTSIG2_Smoothing	0x01
+#define	bHTSIG2_Sounding	0x02
+#define	bHTSIG2_Aggreaton	0x08
+#define	bHTSIG2_STBC		0x30
+#define	bHTSIG2_AdvCoding	0x40
+#define	bHTSIG2_NumOfHTLTF	0x300
+#define	bHTSIG2_CRC8		0x3fc
+#define	bHTSIG1_MCS		0x7f
+#define	bHTSIG1_BandWidth	0x80
+#define	bHTSIG1_HTLength	0xffff
+#define	bLSIG_Rate		0xf
+#define	bLSIG_Reserved		0x10
+#define	bLSIG_Length		0x1fffe
+#define	bLSIG_Parity		0x20
+#define	bCCKRxPhase		0x4
+
+#define	bLSSIReadAddress	0x7f800000   /*  T65 RF */
+
+#define	bLSSIReadEdge		0x80000000   /* LSSI "Read" edge signal */
+
+#define	bLSSIReadBackData	0xfffff		/*  T65 RF */
+
+#define	bLSSIReadOKFlag		0x1000	/*  Useless now */
+#define	bCCKSampleRate		0x8       /* 0: 44MHz, 1:88MHz */
+#define	bRegulator0Standby	0x1
+#define	bRegulatorPLLStandby	0x2
+#define	bRegulator1Standby	0x4
+#define	bPLLPowerUp		0x8
+#define	bDPLLPowerUp		0x10
+#define	bDA10PowerUp		0x20
+#define	bAD7PowerUp		0x200
+#define	bDA6PowerUp		0x2000
+#define	bXtalPowerUp		0x4000
+#define	b40MDClkPowerUP		0x8000
+#define	bDA6DebugMode		0x20000
+#define	bDA6Swing		0x380000
+
+/*  Reg 0x880 rFPGA0_AnalogParameter1 20/40 CCK support switch 40/80 BB MHZ */
+#define	bADClkPhase		0x4000000
+
+#define	b80MClkDelay		0x18000000	/*  Useless */
+#define	bAFEWatchDogEnable	0x20000000
+
+/*  Reg 0x884 rFPGA0_AnalogParameter2 Crystal cap */
+#define	bXtalCap01		0xc0000000
+#define	bXtalCap23		0x3
+#define	bXtalCap92x		0x0f000000
+#define	bXtalCap		0x0f000000
+
+#define	bIntDifClkEnable	0x400	/*  Useless */
+#define	bExtSigClkEnable	0x800
+#define	bBandgapMbiasPowerUp	0x10000
+#define	bAD11SHGain		0xc0000
+#define	bAD11InputRange		0x700000
+#define	bAD11OPCurrent		0x3800000
+#define	bIPathLoopback		0x4000000
+#define	bQPathLoopback		0x8000000
+#define	bAFELoopback		0x10000000
+#define	bDA10Swing		0x7e0
+#define	bDA10Reverse		0x800
+#define	bDAClkSource		0x1000
+#define	bAD7InputRange		0x6000
+#define	bAD7Gain		0x38000
+#define	bAD7OutputCMMode	0x40000
+#define	bAD7InputCMMode		0x380000
+#define	bAD7Current		0xc00000
+#define	bRegulatorAdjust	0x7000000
+#define	bAD11PowerUpAtTx	0x1
+#define	bDA10PSAtTx		0x10
+#define	bAD11PowerUpAtRx	0x100
+#define	bDA10PSAtRx		0x1000
+#define	bCCKRxAGCFormat		0x200
+#define	bPSDFFTSamplepPoint	0xc000
+#define	bPSDAverageNum		0x3000
+#define	bIQPathControl		0xc00
+#define	bPSDFreq		0x3ff
+#define	bPSDAntennaPath		0x30
+#define	bPSDIQSwitch		0x40
+#define	bPSDRxTrigger		0x400000
+#define	bPSDTxTrigger		0x80000000
+#define	bPSDSineToneScale	0x7f000000
+#define	bPSDReport		0xffff
+
+/*  3. Page9(0x900) */
+#define	bOFDMTxSC		0x30000000	/*  Useless */
+#define	bCCKTxOn		0x1
+#define	bOFDMTxOn		0x2
+#define	bDebugPage		0xfff  /* reset debug page and HWord, LWord */
+#define	bDebugItem		0xff   /* reset debug page and LWord */
+#define	bAntL			0x10
+#define	bAntNonHT		0x100
+#define	bAntHT1			0x1000
+#define	bAntHT2			0x10000
+#define	bAntHT1S1		0x100000
+#define	bAntNonHTS1		0x1000000
+
+/*  4. PageA(0xA00) */
+#define	bCCKBBMode		0x3	/*  Useless */
+#define	bCCKTxPowerSaving	0x80
+#define	bCCKRxPowerSaving	0x40
+
+#define	bCCKSideBand		0x10	/*  Reg 0xa00 rCCK0_System 20/40 */
+
+#define	bCCKScramble		0x8	/*  Useless */
+#define	bCCKAntDiversity	0x8000
+#define	bCCKCarrierRecovery	0x4000
+#define	bCCKTxRate		0x3000
+#define	bCCKDCCancel		0x0800
+#define	bCCKISICancel		0x0400
+#define	bCCKMatchFilter		0x0200
+#define	bCCKEqualizer		0x0100
+#define	bCCKPreambleDetect	0x800000
+#define	bCCKFastFalseCCA	0x400000
+#define	bCCKChEstStart		0x300000
+#define	bCCKCCACount		0x080000
+#define	bCCKcs_lim		0x070000
+#define	bCCKBistMode		0x80000000
+#define	bCCKCCAMask		0x40000000
+#define	bCCKTxDACPhase		0x4
+#define	bCCKRxADCPhase		0x20000000   /* r_rx_clk */
+#define	bCCKr_cp_mode0		0x0100
+#define	bCCKTxDCOffset		0xf0
+#define	bCCKRxDCOffset		0xf
+#define	bCCKCCAMode		0xc000
+#define	bCCKFalseCS_lim		0x3f00
+#define	bCCKCS_ratio		0xc00000
+#define	bCCKCorgBit_sel		0x300000
+#define	bCCKPD_lim		0x0f0000
+#define	bCCKNewCCA		0x80000000
+#define	bCCKRxHPofIG		0x8000
+#define	bCCKRxIG		0x7f00
+#define	bCCKLNAPolarity		0x800000
+#define	bCCKRx1stGain		0x7f0000
+#define	bCCKRFExtend		0x20000000 /* CCK Rx Iinital gain polarity */
+#define	bCCKRxAGCSatLevel	0x1f000000
+#define	bCCKRxAGCSatCount	0xe0
+#define	bCCKRxRFSettle		0x1f       /* AGCsamp_dly */
+#define	bCCKFixedRxAGC		0x8000
+#define	bCCKAntennaPolarity	0x2000
+#define	bCCKTxFilterType	0x0c00
+#define	bCCKRxAGCReportType	0x0300
+#define	bCCKRxDAGCEn		0x80000000
+#define	bCCKRxDAGCPeriod	0x20000000
+#define	bCCKRxDAGCSatLevel	0x1f000000
+#define	bCCKTimingRecovery	0x800000
+#define	bCCKTxC0		0x3f0000
+#define	bCCKTxC1		0x3f000000
+#define	bCCKTxC2		0x3f
+#define	bCCKTxC3		0x3f00
+#define	bCCKTxC4		0x3f0000
+#define	bCCKTxC5		0x3f000000
+#define	bCCKTxC6		0x3f
+#define	bCCKTxC7		0x3f00
+#define	bCCKDebugPort		0xff0000
+#define	bCCKDACDebug		0x0f000000
+#define	bCCKFalseAlarmEnable	0x8000
+#define	bCCKFalseAlarmRead	0x4000
+#define	bCCKTRSSI		0x7f
+#define	bCCKRxAGCReport		0xfe
+#define	bCCKRxReport_AntSel	0x80000000
+#define	bCCKRxReport_MFOff	0x40000000
+#define	bCCKRxRxReport_SQLoss	0x20000000
+#define	bCCKRxReport_Pktloss	0x10000000
+#define	bCCKRxReport_Lockedbit	0x08000000
+#define	bCCKRxReport_RateError	0x04000000
+#define	bCCKRxReport_RxRate	0x03000000
+#define	bCCKRxFACounterLower	0xff
+#define	bCCKRxFACounterUpper	0xff000000
+#define	bCCKRxHPAGCStart	0xe000
+#define	bCCKRxHPAGCFinal	0x1c00
+#define	bCCKRxFalseAlarmEnable	0x8000
+#define	bCCKFACounterFreeze	0x4000
+#define	bCCKTxPathSel		0x10000000
+#define	bCCKDefaultRxPath	0xc000000
+#define	bCCKOptionRxPath	0x3000000
+
+/*  5. PageC(0xC00) */
+#define	bNumOfSTF		0x3	/*  Useless */
+#define	bShift_L		0xc0
+#define	bGI_TH			0xc
+#define	bRxPathA		0x1
+#define	bRxPathB		0x2
+#define	bRxPathC		0x4
+#define	bRxPathD		0x8
+#define	bTxPathA		0x1
+#define	bTxPathB		0x2
+#define	bTxPathC		0x4
+#define	bTxPathD		0x8
+#define	bTRSSIFreq		0x200
+#define	bADCBackoff		0x3000
+#define	bDFIRBackoff		0xc000
+#define	bTRSSILatchPhase	0x10000
+#define	bRxIDCOffset		0xff
+#define	bRxQDCOffset		0xff00
+#define	bRxDFIRMode		0x1800000
+#define	bRxDCNFType		0xe000000
+#define	bRXIQImb_A		0x3ff
+#define	bRXIQImb_B		0xfc00
+#define	bRXIQImb_C		0x3f0000
+#define	bRXIQImb_D		0xffc00000
+#define	bDC_dc_Notch		0x60000
+#define	bRxNBINotch		0x1f000000
+#define	bPD_TH			0xf
+#define	bPD_TH_Opt2		0xc000
+#define	bPWED_TH		0x700
+#define	bIfMF_Win_L		0x800
+#define	bPD_Option		0x1000
+#define	bMF_Win_L		0xe000
+#define	bBW_Search_L		0x30000
+#define	bwin_enh_L		0xc0000
+#define	bBW_TH			0x700000
+#define	bED_TH2			0x3800000
+#define	bBW_option		0x4000000
+#define	bRatio_TH		0x18000000
+#define	bWindow_L		0xe0000000
+#define	bSBD_Option		0x1
+#define	bFrame_TH		0x1c
+#define	bFS_Option		0x60
+#define	bDC_Slope_check		0x80
+#define	bFGuard_Counter_DC_L	0xe00
+#define	bFrame_Weight_Short	0x7000
+#define	bSub_Tune		0xe00000
+#define	bFrame_DC_Length	0xe000000
+#define	bSBD_start_offset	0x30000000
+#define	bFrame_TH_2		0x7
+#define	bFrame_GI2_TH		0x38
+#define	bGI2_Sync_en		0x40
+#define	bSarch_Short_Early	0x300
+#define	bSarch_Short_Late	0xc00
+#define	bSarch_GI2_Late		0x70000
+#define	bCFOAntSum		0x1
+#define	bCFOAcc			0x2
+#define	bCFOStartOffset		0xc
+#define	bCFOLookBack		0x70
+#define	bCFOSumWeight		0x80
+#define	bDAGCEnable		0x10000
+#define	bTXIQImb_A		0x3ff
+#define	bTXIQImb_B		0xfc00
+#define	bTXIQImb_C		0x3f0000
+#define	bTXIQImb_D		0xffc00000
+#define	bTxIDCOffset		0xff
+#define	bTxQDCOffset		0xff00
+#define	bTxDFIRMode		0x10000
+#define	bTxPesudoNoiseOn	0x4000000
+#define	bTxPesudoNoise_A	0xff
+#define	bTxPesudoNoise_B	0xff00
+#define	bTxPesudoNoise_C	0xff0000
+#define	bTxPesudoNoise_D	0xff000000
+#define	bCCADropOption		0x20000
+#define	bCCADropThres		0xfff00000
+#define	bEDCCA_H		0xf
+#define	bEDCCA_L		0xf0
+#define	bLambda_ED		0x300
+#define	bRxInitialGain		0x7f
+#define	bRxAntDivEn		0x80
+#define	bRxAGCAddressForLNA	0x7f00
+#define	bRxHighPowerFlow	0x8000
+#define	bRxAGCFreezeThres	0xc0000
+#define	bRxFreezeStep_AGC1	0x300000
+#define	bRxFreezeStep_AGC2	0xc00000
+#define	bRxFreezeStep_AGC3	0x3000000
+#define	bRxFreezeStep_AGC0	0xc000000
+#define	bRxRssi_Cmp_En		0x10000000
+#define	bRxQuickAGCEn		0x20000000
+#define	bRxAGCFreezeThresMode	0x40000000
+#define	bRxOverFlowCheckType	0x80000000
+#define	bRxAGCShift		0x7f
+#define	bTRSW_Tri_Only		0x80
+#define	bPowerThres		0x300
+#define	bRxAGCEn		0x1
+#define	bRxAGCTogetherEn	0x2
+#define	bRxAGCMin		0x4
+#define	bRxHP_Ini		0x7
+#define	bRxHP_TRLNA		0x70
+#define	bRxHP_RSSI		0x700
+#define	bRxHP_BBP1		0x7000
+#define	bRxHP_BBP2		0x70000
+#define	bRxHP_BBP3		0x700000
+#define	bRSSI_H			0x7f0000     /* threshold for high power */
+#define	bRSSI_Gen		0x7f000000   /* threshold for ant diversity */
+#define	bRxSettle_TRSW		0x7
+#define	bRxSettle_LNA		0x38
+#define	bRxSettle_RSSI		0x1c0
+#define	bRxSettle_BBP		0xe00
+#define	bRxSettle_RxHP		0x7000
+#define	bRxSettle_AntSW_RSSI	0x38000
+#define	bRxSettle_AntSW		0xc0000
+#define	bRxProcessTime_DAGC	0x300000
+#define	bRxSettle_HSSI		0x400000
+#define	bRxProcessTime_BBPPW	0x800000
+#define	bRxAntennaPowerShift	0x3000000
+#define	bRSSITableSelect	0xc000000
+#define	bRxHP_Final		0x7000000
+#define	bRxHTSettle_BBP		0x7
+#define	bRxHTSettle_HSSI	0x8
+#define	bRxHTSettle_RxHP	0x70
+#define	bRxHTSettle_BBPPW	0x80
+#define	bRxHTSettle_Idle	0x300
+#define	bRxHTSettle_Reserved	0x1c00
+#define	bRxHTRxHPEn		0x8000
+#define	bRxHTAGCFreezeThres	0x30000
+#define	bRxHTAGCTogetherEn	0x40000
+#define	bRxHTAGCMin		0x80000
+#define	bRxHTAGCEn		0x100000
+#define	bRxHTDAGCEn		0x200000
+#define	bRxHTRxHP_BBP		0x1c00000
+#define	bRxHTRxHP_Final		0xe0000000
+#define	bRxPWRatioTH		0x3
+#define	bRxPWRatioEn		0x4
+#define	bRxMFHold		0x3800
+#define	bRxPD_Delay_TH1		0x38
+#define	bRxPD_Delay_TH2		0x1c0
+#define	bRxPD_DC_COUNT_MAX	0x600
+#define	bRxPD_Delay_TH		0x8000
+#define	bRxProcess_Delay	0xf0000
+#define	bRxSearchrange_GI2_Early	0x700000
+#define	bRxFrame_Guard_Counter_L	0x3800000
+#define	bRxSGI_Guard_L		0xc000000
+#define	bRxSGI_Search_L		0x30000000
+#define	bRxSGI_TH		0xc0000000
+#define	bDFSCnt0		0xff
+#define	bDFSCnt1		0xff00
+#define	bDFSFlag		0xf0000
+#define	bMFWeightSum		0x300000
+#define	bMinIdxTH		0x7f000000
+#define	bDAFormat		0x40000
+#define	bTxChEmuEnable		0x01000000
+#define	bTRSWIsolation_A	0x7f
+#define	bTRSWIsolation_B	0x7f00
+#define	bTRSWIsolation_C	0x7f0000
+#define	bTRSWIsolation_D	0x7f000000
+#define	bExtLNAGain		0x7c00
+
+/*  6. PageE(0xE00) */
+#define	bSTBCEn			0x4	/*  Useless */
+#define	bAntennaMapping		0x10
+#define	bNss			0x20
+#define	bCFOAntSumD		0x200
+#define	bPHYCounterReset	0x8000000
+#define	bCFOReportGet		0x4000000
+#define	bOFDMContinueTx		0x10000000
+#define	bOFDMSingleCarrier	0x20000000
+#define	bOFDMSingleTone		0x40000000
+#define	bHTDetect		0x100
+#define	bCFOEn			0x10000
+#define	bCFOValue		0xfff00000
+#define	bSigTone_Re		0x3f
+#define	bSigTone_Im		0x7f00
+#define	bCounter_CCA		0xffff
+#define	bCounter_ParityFail	0xffff0000
+#define	bCounter_RateIllegal	0xffff
+#define	bCounter_CRC8Fail	0xffff0000
+#define	bCounter_MCSNoSupport	0xffff
+#define	bCounter_FastSync	0xffff
+#define	bShortCFO		0xfff
+#define	bShortCFOTLength	12   /* total */
+#define	bShortCFOFLength	11   /* fraction */
+#define	bLongCFO		0x7ff
+#define	bLongCFOTLength		11
+#define	bLongCFOFLength		11
+#define	bTailCFO		0x1fff
+#define	bTailCFOTLength		13
+#define	bTailCFOFLength		12
+#define	bmax_en_pwdB		0xffff
+#define	bCC_power_dB		0xffff0000
+#define	bnoise_pwdB		0xffff
+#define	bPowerMeasTLength	10
+#define	bPowerMeasFLength	3
+#define	bRx_HT_BW		0x1
+#define	bRxSC			0x6
+#define	bRx_HT			0x8
+#define	bNB_intf_det_on		0x1
+#define	bIntf_win_len_cfg	0x30
+#define	bNB_Intf_TH_cfg		0x1c0
+#define	bRFGain			0x3f
+#define	bTableSel		0x40
+#define	bTRSW			0x80
+#define	bRxSNR_A		0xff
+#define	bRxSNR_B		0xff00
+#define	bRxSNR_C		0xff0000
+#define	bRxSNR_D		0xff000000
+#define	bSNREVMTLength		8
+#define	bSNREVMFLength		1
+#define	bCSI1st			0xff
+#define	bCSI2nd			0xff00
+#define	bRxEVM1st		0xff0000
+#define	bRxEVM2nd		0xff000000
+#define	bSIGEVM			0xff
+#define	bPWDB			0xff00
+#define	bSGIEN			0x10000
+
+#define	bSFactorQAM1		0xf	/*  Useless */
+#define	bSFactorQAM2		0xf0
+#define	bSFactorQAM3		0xf00
+#define	bSFactorQAM4		0xf000
+#define	bSFactorQAM5		0xf0000
+#define	bSFactorQAM6		0xf0000
+#define	bSFactorQAM7		0xf00000
+#define	bSFactorQAM8		0xf000000
+#define	bSFactorQAM9		0xf0000000
+#define	bCSIScheme		0x100000
+
+#define	bNoiseLvlTopSet		0x3	/*  Useless */
+#define	bChSmooth		0x4
+#define	bChSmoothCfg1		0x38
+#define	bChSmoothCfg2		0x1c0
+#define	bChSmoothCfg3		0xe00
+#define	bChSmoothCfg4		0x7000
+#define	bMRCMode		0x800000
+#define	bTHEVMCfg		0x7000000
+
+#define	bLoopFitType		0x1	/*  Useless */
+#define	bUpdCFO			0x40
+#define	bUpdCFOOffData		0x80
+#define	bAdvUpdCFO		0x100
+#define	bAdvTimeCtrl		0x800
+#define	bUpdClko		0x1000
+#define	bFC			0x6000
+#define	bTrackingMode		0x8000
+#define	bPhCmpEnable		0x10000
+#define	bUpdClkoLTF		0x20000
+#define	bComChCFO		0x40000
+#define	bCSIEstiMode		0x80000
+#define	bAdvUpdEqz		0x100000
+#define	bUChCfg			0x7000000
+#define	bUpdEqz			0x8000000
+
+/* Rx Pseduo noise */
+#define	bRxPesudoNoiseOn	0x20000000	/*  Useless */
+#define	bRxPesudoNoise_A	0xff
+#define	bRxPesudoNoise_B	0xff00
+#define	bRxPesudoNoise_C	0xff0000
+#define	bRxPesudoNoise_D	0xff000000
+#define	bPesudoNoiseState_A	0xffff
+#define	bPesudoNoiseState_B	0xffff0000
+#define	bPesudoNoiseState_C	0xffff
+#define	bPesudoNoiseState_D	0xffff0000
+
+/* 7. RF Register */
+/* Zebra1 */
+#define	bZebra1_HSSIEnable	0x8		/*  Useless */
+#define	bZebra1_TRxControl	0xc00
+#define	bZebra1_TRxGainSetting	0x07f
+#define	bZebra1_RxCorner	0xc00
+#define	bZebra1_TxChargePump	0x38
+#define	bZebra1_RxChargePump	0x7
+#define	bZebra1_ChannelNum	0xf80
+#define	bZebra1_TxLPFBW		0x400
+#define	bZebra1_RxLPFBW		0x600
+
+/* Zebra4 */
+#define	bRTL8256RegModeCtrl1	0x100	/*  Useless */
+#define	bRTL8256RegModeCtrl0	0x40
+#define	bRTL8256_TxLPFBW	0x18
+#define	bRTL8256_RxLPFBW	0x600
+
+/* RTL8258 */
+#define	bRTL8258_TxLPFBW	0xc	/*  Useless */
+#define	bRTL8258_RxLPFBW	0xc00
+#define	bRTL8258_RSSILPFBW	0xc0
+
+/*  */
+/*  Other Definition */
+/*  */
+
+/* byte endable for sb_write */
+#define	bByte0			0x1	/*  Useless */
+#define	bByte1			0x2
+#define	bByte2			0x4
+#define	bByte3			0x8
+#define	bWord0			0x3
+#define	bWord1			0xc
+#define	bDWord			0xf
+
+/* for PutRegsetting & GetRegSetting BitMask */
+#define	bMaskByte0		0xff	/*  Reg 0xc50 rOFDM0_XAAGCCore~0xC6f */
+#define	bMaskByte1		0xff00
+#define	bMaskByte2		0xff0000
+#define	bMaskByte3		0xff000000
+#define	bMaskHWord		0xffff0000
+#define	bMaskLWord		0x0000ffff
+#define	bMaskDWord		0xffffffff
+#define	bMask12Bits		0xfff
+#define	bMaskH4Bits		0xf0000000
+#define	bMaskOFDM_D		0xffc00000
+#define	bMaskCCK		0x3f3f3f3f
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+#define	bRFRegOffsetMask	0xfffff
+
+#define	bEnable                 0x1	/*  Useless */
+#define	bDisable                0x0
+
+#define	LeftAntenna		0x0	/*  Useless */
+#define	RightAntenna		0x1
+
+#define	tCheckTxStatus		500   /* 500ms Useless */
+#define	tUpdateRxCounter	100   /* 100ms */
+
+#define	rateCCK			0	/*  Useless */
+#define	rateOFDM		1
+#define	rateHT			2
+
+/* define Register-End */
+#define	bPMAC_End		0x1ff	/*  Useless */
+#define	bFPGAPHY0_End		0x8ff
+#define	bFPGAPHY1_End		0x9ff
+#define	bCCKPHY0_End		0xaff
+#define	bOFDMPHY0_End		0xcff
+#define	bOFDMPHY1_End		0xdff
+
+#define	bPMACControl		0x0	/*  Useless */
+#define	bWMACControl		0x1
+#define	bWNICControl		0x2
+
+#define	PathA			0x0	/*  Useless */
+#define	PathB			0x1
+#define	PathC			0x2
+#define	PathD			0x3
+
+/*--------------------------Define Parameters-------------------------------*/
+
+#endif
diff --git a/drivers/staging/r8188eu/include/Hal8188ERateAdaptive.h b/drivers/staging/r8188eu/include/Hal8188ERateAdaptive.h
new file mode 100644
index 000000000..c571ad947
--- /dev/null
+++ b/drivers/staging/r8188eu/include/Hal8188ERateAdaptive.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2011 Realtek Semiconductor Corp. */
+
+#ifndef __INC_RA_H
+#define __INC_RA_H
+/* Module Name: RateAdaptive.h
+ * Abstract: Prototype of RA and related data structure.
+ */
+
+#include <linux/bitfield.h>
+
+/*  Rate adaptive define */
+#define	PERENTRY	23
+#define	RETRYSIZE	5
+#define	RATESIZE	28
+#define	TX_RPT2_ITEM_SIZE	8
+
+/*  TX report 2 format in Rx desc */
+#define GET_TX_RPT2_DESC_PKT_LEN_88E(__rxstatusdesc)		\
+	le32_get_bits(*(__le32 *)__rxstatusdesc, GENMASK(8, 0))
+#define GET_TX_RPT2_DESC_MACID_VALID_1_88E(__rxstatusdesc)	\
+	le32_to_cpu((*(__le32 *)(__rxstatusdesc + 16))
+#define GET_TX_RPT2_DESC_MACID_VALID_2_88E(__rxstatusdesc)	\
+	le32_to_cpu((*(__le32 *)(__rxstatusdesc + 20))
+/*  End rate adaptive define */
+
+int ODM_RAInfo_Init_all(struct odm_dm_struct *dm_odm);
+
+int ODM_RAInfo_Init(struct odm_dm_struct *dm_odm, u8 MacID);
+
+u8 ODM_RA_GetShortGI_8188E(struct odm_dm_struct *dm_odm, u8 MacID);
+
+u8 ODM_RA_GetDecisionRate_8188E(struct odm_dm_struct *dm_odm, u8 MacID);
+
+u8 ODM_RA_GetHwPwrStatus_8188E(struct odm_dm_struct *dm_odm, u8 MacID);
+void ODM_RA_UpdateRateInfo_8188E(struct odm_dm_struct *dm_odm, u8 MacID,
+				 u8 RateID, u32 RateMask,
+				 u8 SGIEnable);
+
+void ODM_RA_SetRSSI_8188E(struct odm_dm_struct *dm_odm, u8 macid,
+			  u8 rssi);
+
+void ODM_RA_TxRPT2Handle_8188E(struct odm_dm_struct *dm_odm,
+			       u8 *txrpt_buf, u16 txrpt_len,
+			       u32 validentry0, u32 validentry1);
+
+void ODM_RA_Set_TxRPT_Time(struct odm_dm_struct *dm_odm, u16 minRptTime);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/HalHWImg8188E_BB.h b/drivers/staging/r8188eu/include/HalHWImg8188E_BB.h
new file mode 100644
index 000000000..0a290bc31
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalHWImg8188E_BB.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __INC_BB_8188E_HW_IMG_H
+#define __INC_BB_8188E_HW_IMG_H
+
+/* static bool CheckCondition(const u32 Condition, const u32 Hex); */
+
+/******************************************************************************
+*                           AGC_TAB_1T.TXT
+******************************************************************************/
+
+int ODM_ReadAndConfig_AGC_TAB_1T_8188E(struct odm_dm_struct *odm);
+
+/******************************************************************************
+*                           PHY_REG_1T.TXT
+******************************************************************************/
+
+int ODM_ReadAndConfig_PHY_REG_1T_8188E(struct odm_dm_struct *odm);
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+void ODM_ReadAndConfig_PHY_REG_PG_8188E(struct odm_dm_struct *dm_odm);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/HalHWImg8188E_MAC.h b/drivers/staging/r8188eu/include/HalHWImg8188E_MAC.h
new file mode 100644
index 000000000..b3d67c1a8
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalHWImg8188E_MAC.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __INC_MAC_8188E_HW_IMG_H
+#define __INC_MAC_8188E_HW_IMG_H
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+int ODM_ReadAndConfig_MAC_REG_8188E(struct odm_dm_struct *pDM_Odm);
+
+#endif /*  end of HWIMG_SUPPORT */
diff --git a/drivers/staging/r8188eu/include/HalHWImg8188E_RF.h b/drivers/staging/r8188eu/include/HalHWImg8188E_RF.h
new file mode 100644
index 000000000..880feadb4
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalHWImg8188E_RF.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __INC_RF_8188E_HW_IMG_H
+#define __INC_RF_8188E_HW_IMG_H
+
+/******************************************************************************
+ *                           RadioA_1T.TXT
+ ******************************************************************************/
+
+int ODM_ReadAndConfig_RadioA_1T_8188E(struct odm_dm_struct *odm);
+
+#endif /*  end of HWIMG_SUPPORT */
diff --git a/drivers/staging/r8188eu/include/HalPhyRf_8188e.h b/drivers/staging/r8188eu/include/HalPhyRf_8188e.h
new file mode 100644
index 000000000..b75a5d869
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalPhyRf_8188e.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __HAL_PHY_RF_8188E_H__
+#define __HAL_PHY_RF_8188E_H__
+
+/*--------------------------Define Parameters-------------------------------*/
+#define	IQK_DELAY_TIME_88E		10		/* ms */
+#define	index_mapping_NUM_88E	15
+#define AVG_THERMAL_NUM_88E	4
+
+void ODM_TxPwrTrackAdjust88E(struct odm_dm_struct *pDM_Odm,
+			     u8 Type,	/* 0 = OFDM, 1 = CCK */
+			     u8 *pDirection,/* 1 = +(incr) 2 = -(decr) */
+			     u32 *pOutWriteVal); /* Tx tracking CCK/OFDM BB
+						     * swing index adjust */
+
+void odm_TXPowerTrackingCallback_ThermalMeter_8188E(struct adapter *Adapter);
+
+/* 1 7.	IQK */
+
+void PHY_IQCalibrate_8188E(struct adapter *Adapter, bool ReCovery);
+
+/*  LC calibrate */
+void PHY_LCCalibrate_8188E(struct adapter *pAdapter);
+
+/*  AP calibrate */
+void PHY_DigitalPredistortion_8188E(struct adapter *pAdapter);
+
+void _PHY_SaveADDARegisters(struct adapter *pAdapter, u32 *ADDAReg,
+			    u32 *ADDABackup, u32 RegisterNum);
+
+void _PHY_MACSettingCalibration(struct adapter *pAdapter, u32 *MACReg,
+				u32 *MACBackup);
+
+#endif	/*  #ifndef __HAL_PHY_RF_8188E_H__ */
diff --git a/drivers/staging/r8188eu/include/HalPwrSeqCmd.h b/drivers/staging/r8188eu/include/HalPwrSeqCmd.h
new file mode 100644
index 000000000..0886300d2
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalPwrSeqCmd.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __HALPWRSEQCMD_H__
+#define __HALPWRSEQCMD_H__
+
+#include "drv_types.h"
+
+enum r8188eu_pwr_seq {
+	PWR_ON_FLOW,
+	DISABLE_FLOW,
+	LPS_ENTER_FLOW,
+};
+
+/*	Prototype of protected function. */
+u8 HalPwrSeqCmdParsing(struct adapter *padapter, enum r8188eu_pwr_seq seq);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/HalVerDef.h b/drivers/staging/r8188eu/include/HalVerDef.h
new file mode 100644
index 000000000..7a530c7d5
--- /dev/null
+++ b/drivers/staging/r8188eu/include/HalVerDef.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+#ifndef __HAL_VERSION_DEF_H__
+#define __HAL_VERSION_DEF_H__
+
+enum HAL_CHIP_TYPE {
+	TEST_CHIP	=	0,
+	NORMAL_CHIP	=	1,
+};
+
+enum HAL_CUT_VERSION {
+	A_CUT_VERSION	=	0,
+	B_CUT_VERSION	=	1,
+	C_CUT_VERSION	=	2,
+	D_CUT_VERSION	=	3,
+	E_CUT_VERSION	=	4,
+};
+
+enum HAL_VENDOR {
+	CHIP_VENDOR_TSMC	=	0,
+	CHIP_VENDOR_UMC		=	1,
+};
+
+struct HAL_VERSION {
+	enum HAL_CHIP_TYPE	ChipType;
+	enum HAL_CUT_VERSION	CUTVersion;
+	enum HAL_VENDOR		VendorType;
+};
+
+/*  Get element */
+#define GET_CVID_CHIP_TYPE(version)	(((version).ChipType))
+#define GET_CVID_MANUFACTUER(version)	(((version).VendorType))
+
+/* HAL_CHIP_TYPE_E */
+#define IS_NORMAL_CHIP(version)				\
+	(GET_CVID_CHIP_TYPE(version) == NORMAL_CHIP)
+
+/* HAL_VENDOR_E */
+#define IS_CHIP_VENDOR_TSMC(version)			\
+	(GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_TSMC)
+
+#endif
diff --git a/drivers/staging/r8188eu/include/drv_types.h b/drivers/staging/r8188eu/include/drv_types.h
new file mode 100644
index 000000000..1bd0c8f3a
--- /dev/null
+++ b/drivers/staging/r8188eu/include/drv_types.h
@@ -0,0 +1,229 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+/*-----------------------------------------------------------------------------
+
+	For type defines and data structure defines
+
+------------------------------------------------------------------------------*/
+
+#ifndef __DRV_TYPES_H__
+#define __DRV_TYPES_H__
+
+#include "osdep_service.h"
+#include "wlan_bssdef.h"
+#include "rtw_ht.h"
+#include "rtw_cmd.h"
+#include "rtw_xmit.h"
+#include "rtw_recv.h"
+#include "hal_intf.h"
+#include "hal_com.h"
+#include "rtw_security.h"
+#include "rtw_pwrctrl.h"
+#include "rtw_io.h"
+#include "rtw_eeprom.h"
+#include "sta_info.h"
+#include "rtw_mlme.h"
+#include "rtw_rf.h"
+#include "rtw_event.h"
+#include "rtw_led.h"
+#include "rtw_mlme_ext.h"
+#include "rtw_p2p.h"
+#include "rtw_ap.h"
+#include "rtw_br_ext.h"
+#include "rtl8188e_hal.h"
+#include "rtw_fw.h"
+
+#define FW_RTL8188EU	"rtlwifi/rtl8188eufw.bin"
+
+struct registry_priv {
+	u8	rfintfs;
+	u8	lbkmode;
+	u8	hci;
+	struct ndis_802_11_ssid	ssid;
+	u8	network_mode;	/* infra, ad-hoc, auto */
+	u8	channel;/* ad-hoc support requirement */
+	u8	wireless_mode;/* A, B, G, auto */
+	u8	scan_mode;/* active, passive */
+	u8	radio_enable;
+	u8	preamble;/* long, short, auto */
+	u8	vrtl_carrier_sense;/* Enable, Disable, Auto */
+	u8	vcs_type;/* RTS/CTS, CTS-to-self */
+	u16	rts_thresh;
+	u16	frag_thresh;
+	u8	adhoc_tx_pwr;
+	u8	soft_ap;
+	u8	power_mgnt;
+	u8	ips_mode;
+	u8	smart_ps;
+	u8	long_retry_lmt;
+	u8	short_retry_lmt;
+	u16	busy_thresh;
+	u8	ack_policy;
+	u8	software_encrypt;
+	u8	software_decrypt;
+	u8	acm_method;
+	  /* UAPSD */
+	u8	wmm_enable;
+	u8	uapsd_enable;
+	u8	uapsd_max_sp;
+	u8	uapsd_acbk_en;
+	u8	uapsd_acbe_en;
+	u8	uapsd_acvi_en;
+	u8	uapsd_acvo_en;
+
+	u8	led_enable;
+
+	struct wlan_bssid_ex    dev_network;
+
+	u8	ht_enable;
+	u8	cbw40_enable;
+	u8	ampdu_enable;/* for tx */
+	u8	rx_stbc;
+	u8	ampdu_amsdu;/* A-MPDU Supports A-MSDU is permitted */
+	u8	lowrate_two_xmit;
+
+	u8	low_power;
+
+	u8	wifi_spec;/*  !turbo_mode */
+
+	u8	channel_plan;
+	bool	bAcceptAddbaReq;
+
+	u8	antdiv_cfg;
+	u8	antdiv_type;
+
+	u8	usbss_enable;/* 0:disable,1:enable */
+	u8	hwpdn_mode;/* 0:disable,1:enable,2:decide by EFUSE config */
+	u8	hwpwrp_detect;/* 0:disable,1:enable */
+
+	u8	hw_wps_pbc;/* 0:disable,1:enable */
+
+	u8	max_roaming_times; /*  the max number driver will try */
+
+	u8	fw_iol; /* enable iol without other concern */
+
+	u8	enable80211d;
+
+	u8	ifname[16];
+	u8	if2name[16];
+
+	u8	notch_filter;
+};
+
+#define MAX_CONTINUAL_URB_ERR		4
+
+struct dvobj_priv {
+	struct adapter *if1;
+
+	/* For 92D, DMDP have 2 interface. */
+	u8	InterfaceNumber;
+	u8	NumInterfaces;
+
+	/* In /Out Pipe information */
+	int	RtInPipe;
+	int	RtOutPipe[3];
+	u8	Queue2Pipe[HW_QUEUE_ENTRY];/* for out pipe mapping */
+
+	struct rt_firmware firmware;
+
+/*-------- below is for USB INTERFACE --------*/
+
+	u8	RtNumOutPipes;
+
+	struct usb_interface *pusbintf;
+	struct usb_device *pusbdev;
+
+	atomic_t continual_urb_error;
+};
+
+static inline struct device *dvobj_to_dev(struct dvobj_priv *dvobj)
+{
+	/* todo: get interface type from dvobj and the return
+	 * the dev accordingly */
+	return &dvobj->pusbintf->dev;
+};
+
+struct adapter {
+	int	pid[3];/* process id from UI, 0:wps, 1:hostapd, 2:dhcpcd */
+
+	struct dvobj_priv *dvobj;
+	struct	mlme_priv mlmepriv;
+	struct	mlme_ext_priv mlmeextpriv;
+	struct	cmd_priv	cmdpriv;
+	struct	evt_priv	evtpriv;
+	struct	io_priv	iopriv;
+	struct	xmit_priv	xmitpriv;
+	struct	recv_priv	recvpriv;
+	struct	sta_priv	stapriv;
+	struct	security_priv	securitypriv;
+	struct	registry_priv	registrypriv;
+	struct	pwrctrl_priv	pwrctrlpriv;
+	struct	eeprom_priv eeprompriv;
+	struct	led_priv	ledpriv;
+	struct wifidirect_info	wdinfo;
+
+	struct hal_data_8188e haldata;
+
+	s32	bDriverStopped;
+	s32	bSurpriseRemoved;
+	s32	bCardDisableWOHSM;
+
+	u8	hw_init_completed;
+	s8	signal_strength;
+
+	void *cmdThread;
+	void (*intf_start)(struct adapter *adapter);
+	void (*intf_stop)(struct adapter *adapter);
+	struct  net_device *pnetdev;
+
+	/*  used by rtw_rereg_nd_name related function */
+	struct rereg_nd_name_data {
+		struct  net_device *old_pnetdev;
+		char old_ifname[IFNAMSIZ];
+		u8 old_ips_mode;
+		u8 old_bRegUseLed;
+	} rereg_nd_name_priv;
+
+	int bup;
+	struct net_device_stats stats;
+	struct iw_statistics iwstats;
+	struct proc_dir_entry *dir_dev;/*  for proc directory */
+
+	int net_closed;
+	u8 bFWReady;
+	u8 bReadPortCancel;
+	u8 bWritePortCancel;
+	u8 bRxRSSIDisplay;
+	/* The driver will show up the desired channel number
+	 * when this flag is 1. */
+	u8 bNotifyChannelChange;
+	/* The driver will show the current P2P status when the
+	 * upper application reads it. */
+	u8 bShowGetP2PState;
+	struct adapter *pbuddy_adapter;
+
+	struct mutex *hw_init_mutex;
+
+	spinlock_t br_ext_lock;
+	struct nat25_network_db_entry	*nethash[NAT25_HASH_SIZE];
+	int				pppoe_connection_in_progress;
+	unsigned char			pppoe_addr[ETH_ALEN];
+	unsigned char			scdb_mac[ETH_ALEN];
+	unsigned char			scdb_ip[4];
+	struct nat25_network_db_entry	*scdb_entry;
+	unsigned char			br_mac[ETH_ALEN];
+	unsigned char			br_ip[4];
+	struct br_ext_info		ethBrExtInfo;
+};
+
+#define adapter_to_dvobj(adapter) (adapter->dvobj)
+
+void rtw_handle_dualmac(struct adapter *adapter, bool init);
+
+static inline u8 *myid(struct eeprom_priv *peepriv)
+{
+	return peepriv->mac_addr;
+}
+
+#endif /* __DRV_TYPES_H__ */
diff --git a/drivers/staging/r8188eu/include/hal_com.h b/drivers/staging/r8188eu/include/hal_com.h
new file mode 100644
index 000000000..cd3f845e1
--- /dev/null
+++ b/drivers/staging/r8188eu/include/hal_com.h
@@ -0,0 +1,146 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __HAL_COMMON_H__
+#define __HAL_COMMON_H__
+
+/*  */
+/*        Rate Definition */
+/*  */
+/* CCK */
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M				0x00000004
+#define	RATR_11M				0x00000008
+/* OFDM */
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M				0x00000040
+#define	RATR_18M				0x00000080
+#define	RATR_24M				0x00000100
+#define	RATR_36M				0x00000200
+#define	RATR_48M				0x00000400
+#define	RATR_54M				0x00000800
+/* MCS 1 Spatial Stream */
+#define	RATR_MCS0				0x00001000
+#define	RATR_MCS1				0x00002000
+#define	RATR_MCS2				0x00004000
+#define	RATR_MCS3				0x00008000
+#define	RATR_MCS4				0x00010000
+#define	RATR_MCS5				0x00020000
+#define	RATR_MCS6				0x00040000
+#define	RATR_MCS7				0x00080000
+/* MCS 2 Spatial Stream */
+#define	RATR_MCS8				0x00100000
+#define	RATR_MCS9				0x00200000
+#define	RATR_MCS10				0x00400000
+#define	RATR_MCS11				0x00800000
+#define	RATR_MCS12				0x01000000
+#define	RATR_MCS13				0x02000000
+#define	RATR_MCS14				0x04000000
+#define	RATR_MCS15				0x08000000
+
+/* CCK */
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+/* OFDM */
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+/* MCS 1 Spatial Stream */
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+/* MCS 2 Spatial Stream */
+#define RATE_MCS8				BIT(20)
+#define RATE_MCS9				BIT(21)
+#define RATE_MCS10				BIT(22)
+#define RATE_MCS11				BIT(23)
+#define RATE_MCS12				BIT(24)
+#define RATE_MCS13				BIT(25)
+#define RATE_MCS14				BIT(26)
+#define RATE_MCS15				BIT(27)
+
+/*  ALL CCK Rate */
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M | \
+				 RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |	\
+				 RATR_MCS3 | RATR_MCS4 | RATR_MCS5|RATR_MCS6 | \
+				 RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 | \
+				 RATR_MCS11 | RATR_MCS12 | RATR_MCS13 | \
+				 RATR_MCS14 | RATR_MCS15)
+
+/*------------------------------ Tx Desc definition Macro --------------------*/
+/* pragma mark -- Tx Desc related definition. -- */
+/*	Rate */
+/*  CCK Rates, TxHT = 0 */
+#define DESC_RATE1M				0x00
+#define DESC_RATE2M				0x01
+#define DESC_RATE5_5M				0x02
+#define DESC_RATE11M				0x03
+
+/*  OFDM Rates, TxHT = 0 */
+#define DESC_RATE6M				0x04
+#define DESC_RATE9M				0x05
+#define DESC_RATE12M				0x06
+#define DESC_RATE18M				0x07
+#define DESC_RATE24M				0x08
+#define DESC_RATE36M				0x09
+#define DESC_RATE48M				0x0a
+#define DESC_RATE54M				0x0b
+
+/*  MCS Rates, TxHT = 1 */
+#define DESC_RATEMCS0				0x0c
+#define DESC_RATEMCS1				0x0d
+#define DESC_RATEMCS2				0x0e
+#define DESC_RATEMCS3				0x0f
+#define DESC_RATEMCS4				0x10
+#define DESC_RATEMCS5				0x11
+#define DESC_RATEMCS6				0x12
+#define DESC_RATEMCS7				0x13
+#define DESC_RATEMCS8				0x14
+#define DESC_RATEMCS9				0x15
+#define DESC_RATEMCS10				0x16
+#define DESC_RATEMCS11				0x17
+#define DESC_RATEMCS12				0x18
+#define DESC_RATEMCS13				0x19
+#define DESC_RATEMCS14				0x1a
+#define DESC_RATEMCS15				0x1b
+#define DESC_RATEMCS15_SG			0x1c
+#define DESC_RATEMCS32				0x20
+
+/*  1 Byte long (in unit of TU) */
+#define REG_P2P_CTWIN				0x0572
+#define REG_NOA_DESC_SEL			0x05CF
+#define REG_NOA_DESC_DURATION			0x05E0
+#define REG_NOA_DESC_INTERVAL			0x05E4
+#define REG_NOA_DESC_START			0x05E8
+#define REG_NOA_DESC_COUNT			0x05EC
+
+/* return the final channel plan decision */
+u8 hal_com_get_channel_plan(struct adapter *padapter,
+			    u8 hw_channel_plan,
+			    u8 sw_channel_plan,
+			    u8 def_channel_plan,
+			    bool AutoLoadFail
+);
+
+u8 MRateToHwRate(u8 rate);
+
+void HalSetBrateCfg(struct adapter *Adapter, u8 *mBratesOS, u16 *pBrateCfg);
+
+#endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/r8188eu/include/hal_intf.h b/drivers/staging/r8188eu/include/hal_intf.h
new file mode 100644
index 000000000..ac6e3f95c
--- /dev/null
+++ b/drivers/staging/r8188eu/include/hal_intf.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#ifndef __HAL_INTF_H__
+#define __HAL_INTF_H__
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "Hal8188EPhyCfg.h"
+
+typedef s32 (*c2h_id_filter)(u8 id);
+
+int rtl8188eu_interface_configure(struct adapter *adapt);
+int ReadAdapterInfo8188EU(struct adapter *Adapter);
+void rtl8188eu_init_default_value(struct adapter *adapt);
+void rtl8188e_SetHalODMVar(struct adapter *Adapter, void *pValue1, bool bSet);
+u32 rtl8188eu_InitPowerOn(struct adapter *adapt);
+void rtl8188e_EfusePowerSwitch(struct adapter *pAdapter, u8 PwrState);
+void rtl8188e_ReadEFuse(struct adapter *Adapter, u16 _size_byte, u8 *pbuf);
+
+void hal_notch_filter_8188e(struct adapter *adapter, bool enable);
+
+void SetBeaconRelatedRegisters8188EUsb(struct adapter *adapt);
+void UpdateHalRAMask8188EUsb(struct adapter *adapt, u32 mac_id, u8 rssi_level);
+
+int rtl8188e_IOL_exec_cmds_sync(struct adapter *adapter,
+				struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt);
+
+unsigned int rtl8188eu_inirp_init(struct adapter *Adapter);
+
+uint rtw_hal_init(struct adapter *padapter);
+uint rtw_hal_deinit(struct adapter *padapter);
+void rtw_hal_stop(struct adapter *padapter);
+
+u32 rtl8188eu_hal_init(struct adapter *Adapter);
+u32 rtl8188eu_hal_deinit(struct adapter *Adapter);
+
+void rtw_hal_update_ra_mask(struct adapter *padapter, u32 mac_id, u8 level);
+void	rtw_hal_clone_data(struct adapter *dst_adapt,
+			   struct adapter *src_adapt);
+
+u8 rtw_do_join(struct adapter *padapter);
+
+#endif /* __HAL_INTF_H__ */
diff --git a/drivers/staging/r8188eu/include/ieee80211.h b/drivers/staging/r8188eu/include/ieee80211.h
new file mode 100644
index 000000000..e7a4f8af4
--- /dev/null
+++ b/drivers/staging/r8188eu/include/ieee80211.h
@@ -0,0 +1,817 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __IEEE80211_H
+#define __IEEE80211_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wifi.h"
+#include <linux/wireless.h>
+
+#define MGMT_QUEUE_NUM 5
+
+#define ETH_TYPE_LEN		2
+#define PAYLOAD_TYPE_LEN	1
+
+#define RTL_IOCTL_HOSTAPD (SIOCIWFIRSTPRIV + 28)
+
+/* STA flags */
+#define WLAN_STA_AUTH BIT(0)
+#define WLAN_STA_ASSOC BIT(1)
+#define WLAN_STA_PS BIT(2)
+#define WLAN_STA_TIM BIT(3)
+#define WLAN_STA_PERM BIT(4)
+#define WLAN_STA_AUTHORIZED BIT(5)
+#define WLAN_STA_PENDING_POLL BIT(6) /* pending activity poll not ACKed */
+#define WLAN_STA_SHORT_PREAMBLE BIT(7)
+#define WLAN_STA_PREAUTH BIT(8)
+#define WLAN_STA_WME BIT(9)
+#define WLAN_STA_MFP BIT(10)
+#define WLAN_STA_HT BIT(11)
+#define WLAN_STA_WPS BIT(12)
+#define WLAN_STA_MAYBE_WPS BIT(13)
+#define WLAN_STA_NONERP BIT(31)
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define IEEE_CMD_SET_WPA_IE				2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME						4
+
+#define IEEE_PARAM_WPA_ENABLED				1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED			3
+#define IEEE_PARAM_PRIVACY_INVOKED			4
+#define IEEE_PARAM_AUTH_ALGS					5
+#define IEEE_PARAM_IEEE_802_1X				6
+#define IEEE_PARAM_WPAX_SELECT				7
+
+#define AUTH_ALG_OPEN_SYSTEM			0x1
+#define AUTH_ALG_SHARED_KEY			0x2
+#define AUTH_ALG_LEAP				0x00000004
+
+#define IEEE_MLME_STA_DEAUTH				1
+#define IEEE_MLME_STA_DISASSOC			2
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG			2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR			3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED		4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED			5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED		6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#define WPA_CIPHER_NONE		BIT(0)
+#define WPA_CIPHER_WEP40	BIT(1)
+#define WPA_CIPHER_WEP104 BIT(2)
+#define WPA_CIPHER_TKIP		BIT(3)
+#define WPA_CIPHER_CCMP		BIT(4)
+
+
+#define WPA_SELECTOR_LEN 4
+extern u8 RTW_WPA_OUI_TYPE[];
+extern u16 RTW_WPA_VERSION;
+extern u8 WPA_AUTH_KEY_MGMT_NONE[];
+extern u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[];
+extern u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[];
+extern u8 WPA_CIPHER_SUITE_NONE[];
+extern u8 WPA_CIPHER_SUITE_WEP40[];
+extern u8 WPA_CIPHER_SUITE_TKIP[];
+extern u8 WPA_CIPHER_SUITE_WRAP[];
+extern u8 WPA_CIPHER_SUITE_CCMP[];
+extern u8 WPA_CIPHER_SUITE_WEP104[];
+
+#define RSN_HEADER_LEN 4
+#define RSN_SELECTOR_LEN 4
+
+extern u16 RSN_VERSION_BSD;
+extern u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[];
+extern u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[];
+extern u8 RSN_CIPHER_SUITE_NONE[];
+extern u8 RSN_CIPHER_SUITE_WEP40[];
+extern u8 RSN_CIPHER_SUITE_TKIP[];
+extern u8 RSN_CIPHER_SUITE_WRAP[];
+extern u8 RSN_CIPHER_SUITE_CCMP[];
+extern u8 RSN_CIPHER_SUITE_WEP104[];
+
+enum ratr_table_mode {
+	RATR_INX_WIRELESS_NGB = 0,	/*  BGN 40 Mhz 2SS 1SS */
+	RATR_INX_WIRELESS_NG = 1,	/*  GN or N */
+	RATR_INX_WIRELESS_NB = 2,	/*  BGN 20 Mhz 2SS 1SS  or BN */
+	RATR_INX_WIRELESS_N = 3,
+	RATR_INX_WIRELESS_GB = 4,
+	RATR_INX_WIRELESS_G = 5,
+	RATR_INX_WIRELESS_B = 6,
+	RATR_INX_WIRELESS_MC = 7,
+	RATR_INX_WIRELESS_AC_N = 8,
+};
+
+enum NETWORK_TYPE {
+	WIRELESS_INVALID = 0,
+	/* Sub-Element */
+	WIRELESS_11B = BIT(0), /* tx:cck only, rx:cck only, hw: cck */
+	WIRELESS_11G = BIT(1), /* tx:ofdm only, rx:ofdm & cck, hw:cck & ofdm*/
+	WIRELESS_11_24N = BIT(3), /* tx:MCS only, rx:MCS & cck, hw:MCS & cck */
+
+	/* Combination */
+	/*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
+	WIRELESS_11BG = (WIRELESS_11B | WIRELESS_11G),
+	/*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
+	WIRELESS_11G_24N = (WIRELESS_11G | WIRELESS_11_24N),
+	/*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
+	WIRELESS_11BG_24N = (WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N),
+};
+
+struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[];
+		} wpa_ie;
+		struct {
+			int command;
+			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[];
+		} crypt;
+		struct {
+			u16 aid;
+			u16 capability;
+			int flags;
+			u8 tx_supp_rates[16];
+			struct ieee80211_ht_cap ht_cap;
+		} add_sta;
+		struct {
+			u8	reserved[2];/* for set max_num_sta */
+			u8	buf[];
+		} bcn_ie;
+	} u;
+};
+
+#define IEEE80211_DATA_LEN		2304
+/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
+   6.2.1.1.2.
+
+   The figure in section 7.1.2 suggests a body size of up to 2312
+   bytes is allowed, which is a bit confusing, I suspect this
+   represents the 2304 bytes of real data, plus a possible 8 bytes of
+   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro) */
+
+#define IEEE80211_HLEN			30
+#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define RTW_IEEE80211_FCTL_VERS		0x0003
+#define RTW_IEEE80211_FCTL_FTYPE	0x000c
+#define RTW_IEEE80211_FCTL_STYPE	0x00f0
+#define RTW_IEEE80211_FCTL_TODS		0x0100
+#define RTW_IEEE80211_FCTL_FROMDS	0x0200
+#define RTW_IEEE80211_FCTL_MOREFRAGS	0x0400
+#define RTW_IEEE80211_FCTL_RETRY	0x0800
+#define RTW_IEEE80211_FCTL_PM		0x1000
+#define RTW_IEEE80211_FCTL_MOREDATA	0x2000
+#define RTW_IEEE80211_FCTL_PROTECTED	0x4000
+#define RTW_IEEE80211_FCTL_ORDER	0x8000
+#define RTW_IEEE80211_FCTL_CTL_EXT	0x0f00
+
+#define RTW_IEEE80211_FTYPE_MGMT	0x0000
+#define RTW_IEEE80211_FTYPE_CTL		0x0004
+#define RTW_IEEE80211_FTYPE_DATA	0x0008
+#define RTW_IEEE80211_FTYPE_EXT		0x000c
+
+/* management */
+#define RTW_IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define RTW_IEEE80211_STYPE_ASSOC_RESP	0x0010
+#define RTW_IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define RTW_IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define RTW_IEEE80211_STYPE_PROBE_REQ	0x0040
+#define RTW_IEEE80211_STYPE_PROBE_RESP	0x0050
+#define RTW_IEEE80211_STYPE_BEACON	0x0080
+#define RTW_IEEE80211_STYPE_ATIM	0x0090
+#define RTW_IEEE80211_STYPE_DISASSOC	0x00A0
+#define RTW_IEEE80211_STYPE_AUTH	0x00B0
+#define RTW_IEEE80211_STYPE_DEAUTH	0x00C0
+#define RTW_IEEE80211_STYPE_ACTION	0x00D0
+
+/* control */
+#define RTW_IEEE80211_STYPE_CTL_EXT	0x0060
+#define RTW_IEEE80211_STYPE_BACK_REQ	0x0080
+#define RTW_IEEE80211_STYPE_BACK	0x0090
+#define RTW_IEEE80211_STYPE_PSPOLL	0x00A0
+#define RTW_IEEE80211_STYPE_RTS		0x00B0
+#define RTW_IEEE80211_STYPE_CTS		0x00C0
+#define RTW_IEEE80211_STYPE_ACK		0x00D0
+#define RTW_IEEE80211_STYPE_CFEND	0x00E0
+#define RTW_IEEE80211_STYPE_CFENDACK	0x00F0
+
+/* data */
+#define RTW_IEEE80211_STYPE_DATA	0x0000
+#define RTW_IEEE80211_STYPE_DATA_CFACK	0x0010
+#define RTW_IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define RTW_IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define RTW_IEEE80211_STYPE_NULLFUNC	0x0040
+#define RTW_IEEE80211_STYPE_CFACK	0x0050
+#define RTW_IEEE80211_STYPE_CFPOLL	0x0060
+#define RTW_IEEE80211_STYPE_CFACKPOLL	0x0070
+#define RTW_IEEE80211_STYPE_QOS_DATA	0x0080
+#define RTW_IEEE80211_STYPE_QOS_DATA_CFACK	0x0090
+#define RTW_IEEE80211_STYPE_QOS_DATA_CFPOLL	0x00A0
+#define RTW_IEEE80211_STYPE_QOS_DATA_CFACKPOLL	0x00B0
+#define RTW_IEEE80211_STYPE_QOS_NULLFUNC	0x00C0
+#define RTW_IEEE80211_STYPE_QOS_CFACK		0x00D0
+#define RTW_IEEE80211_STYPE_QOS_CFPOLL		0x00E0
+#define RTW_IEEE80211_STYPE_QOS_CFACKPOLL	0x00F0
+
+/* sequence control field */
+#define RTW_IEEE80211_SCTL_FRAG	0x000F
+#define RTW_IEEE80211_SCTL_SEQ	0xFFF0
+
+#define RTW_ERP_INFO_NON_ERP_PRESENT BIT(0)
+#define RTW_ERP_INFO_USE_PROTECTION BIT(1)
+#define RTW_ERP_INFO_BARKER_PREAMBLE_MODE BIT(2)
+
+/* QoS, QOS */
+#define NORMAL_ACK			0
+#define NO_ACK				1
+#define NON_EXPLICIT_ACK		2
+#define BLOCK_ACK			3
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
+#endif /* ETH_P_PAE */
+
+#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
+
+#define ETH_P_ECONET	0x0018
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+	u8    dsap;   /* always 0xAA */
+	u8    ssap;   /* always 0xAA */
+	u8    ctrl;   /* always 0x03 */
+	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+} __packed;
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define WLAN_FC_GET_TYPE(fc) ((fc) & RTW_IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & RTW_IEEE80211_FCTL_STYPE)
+
+#define WLAN_QC_GET_TID(qc) ((qc) & 0x0f)
+
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTW_IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & RTW_IEEE80211_SCTL_SEQ)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+
+#define WLAN_AUTH_CHALLENGE_LEN 128
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+
+/* Status codes */
+#define WLAN_STATUS_SUCCESS 0
+#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
+#define WLAN_STATUS_CAPS_UNSUPPORTED 10
+#define WLAN_STATUS_REASSOC_NO_ASSOC 11
+#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
+#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
+#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
+#define WLAN_STATUS_CHALLENGE_FAIL 15
+#define WLAN_STATUS_AUTH_TIMEOUT 16
+#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
+#define WLAN_STATUS_ASSOC_DENIED_RATES 18
+/* 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+
+/* Reason codes */
+#define WLAN_REASON_UNSPECIFIED 1
+#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
+#define WLAN_REASON_DEAUTH_LEAVING 3
+#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
+#define WLAN_REASON_DISASSOC_AP_BUSY 5
+#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
+#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
+#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
+#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
+#define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
+#define WLAN_REASON_EXPIRATION_CHK 65535
+
+/* Information Element IDs */
+#define WLAN_EID_SSID 0
+#define WLAN_EID_SUPP_RATES 1
+#define WLAN_EID_FH_PARAMS 2
+#define WLAN_EID_DS_PARAMS 3
+#define WLAN_EID_CF_PARAMS 4
+#define WLAN_EID_TIM 5
+#define WLAN_EID_IBSS_PARAMS 6
+#define WLAN_EID_CHALLENGE 16
+/* EIDs defined by IEEE 802.11h - START */
+#define WLAN_EID_PWR_CONSTRAINT 32
+#define WLAN_EID_PWR_CAPABILITY 33
+#define WLAN_EID_TPC_REQUEST 34
+#define WLAN_EID_TPC_REPORT 35
+#define WLAN_EID_SUPPORTED_CHANNELS 36
+#define WLAN_EID_CHANNEL_SWITCH 37
+#define WLAN_EID_MEASURE_REQUEST 38
+#define WLAN_EID_MEASURE_REPORT 39
+#define WLAN_EID_QUITE 40
+#define WLAN_EID_IBSS_DFS 41
+/* EIDs defined by IEEE 802.11h - END */
+#define WLAN_EID_ERP_INFO 42
+#define WLAN_EID_HT_CAP 45
+#define WLAN_EID_RSN 48
+#define WLAN_EID_EXT_SUPP_RATES 50
+#define WLAN_EID_MOBILITY_DOMAIN 54
+#define WLAN_EID_FAST_BSS_TRANSITION 55
+#define WLAN_EID_TIMEOUT_INTERVAL 56
+#define WLAN_EID_RIC_DATA 57
+#define WLAN_EID_HT_OPERATION 61
+#define WLAN_EID_SECONDARY_CHANNEL_OFFSET 62
+#define WLAN_EID_20_40_BSS_COEXISTENCE 72
+#define WLAN_EID_20_40_BSS_INTOLERANT 73
+#define WLAN_EID_OVERLAPPING_BSS_SCAN_PARAMS 74
+#define WLAN_EID_MMIE 76
+#define WLAN_EID_VENDOR_SPECIFIC 221
+#define WLAN_EID_GENERIC (WLAN_EID_VENDOR_SPECIFIC)
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+#define IEEE80211_STATMASK_SIGNAL (1<<0)
+#define IEEE80211_STATMASK_RSSI (1<<1)
+#define IEEE80211_STATMASK_NOISE (1<<2)
+#define IEEE80211_STATMASK_RATE (1<<3)
+#define IEEE80211_STATMASK_WEMASK 0x7
+
+#define IEEE80211_CCK_MODULATION    (1<<0)
+#define IEEE80211_OFDM_MODULATION   (1<<1)
+
+#define IEEE80211_24GHZ_BAND     (1<<0)
+#define IEEE80211_52GHZ_BAND     (1<<1)
+
+#define IEEE80211_CCK_RATE_LEN			4
+#define IEEE80211_NUM_OFDM_RATESLEN	8
+
+#define IEEE80211_CCK_RATE_1MB			0x02
+#define IEEE80211_CCK_RATE_2MB			0x04
+#define IEEE80211_CCK_RATE_5MB			0x0B
+#define IEEE80211_CCK_RATE_11MB			0x16
+#define IEEE80211_OFDM_RATE_LEN			8
+#define IEEE80211_OFDM_RATE_6MB			0x0C
+#define IEEE80211_OFDM_RATE_9MB			0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
+#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
+#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
+#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
+#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
+#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
+#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
+#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
+#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
+#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
+#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
+#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
+
+#define IEEE80211_CCK_RATES_MASK		0x0000000F
+#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
+	IEEE80211_CCK_RATE_2MB_MASK)
+#define IEEE80211_CCK_DEFAULT_RATES_MASK				\
+	(IEEE80211_CCK_BASIC_RATES_MASK |				\
+	IEEE80211_CCK_RATE_5MB_MASK |					\
+	IEEE80211_CCK_RATE_11MB_MASK)
+
+#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
+#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
+	IEEE80211_OFDM_RATE_12MB_MASK |					\
+	IEEE80211_OFDM_RATE_24MB_MASK)
+#define IEEE80211_OFDM_DEFAULT_RATES_MASK				\
+	(IEEE80211_OFDM_BASIC_RATES_MASK |				\
+	IEEE80211_OFDM_RATE_9MB_MASK  |					\
+	IEEE80211_OFDM_RATE_18MB_MASK |					\
+	IEEE80211_OFDM_RATE_36MB_MASK |					\
+	IEEE80211_OFDM_RATE_48MB_MASK |					\
+	IEEE80211_OFDM_RATE_54MB_MASK)
+#define IEEE80211_DEFAULT_RATES_MASK					\
+	(IEEE80211_OFDM_DEFAULT_RATES_MASK |				\
+	 IEEE80211_CCK_DEFAULT_RATES_MASK)
+
+#define IEEE80211_NUM_OFDM_RATES	8
+#define IEEE80211_NUM_CCK_RATES		4
+#define IEEE80211_OFDM_SHIFT_MASK_A	4
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
+#define IEEE80211_FRAG_CACHE_LEN 4
+
+#define SEC_KEY_1	(1<<0)
+#define SEC_KEY_2	(1<<1)
+#define SEC_KEY_3	(1<<2)
+#define SEC_KEY_4	(1<<3)
+#define SEC_ACTIVE_KEY  (1<<4)
+#define SEC_AUTH_MODE   (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL	(1<<7)
+#define SEC_ENABLED     (1<<8)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define WEP_KEYS 4
+#define WEP_KEY_LEN 13
+
+/*
+
+ 802.11 data frame from AP
+
+      ,-------------------------------------------------------------------.
+Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+      |------|------|---------|---------|---------|------|---------|------|
+Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+      |      | tion | (BSSID) |	 |	 | ence |  data   |      |
+      `-------------------------------------------------------------------'
+
+Total: 28-2340 bytes
+
+*/
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID		0
+#define MFIE_TYPE_RATES		1
+#define MFIE_TYPE_FH_SET	2
+#define MFIE_TYPE_DS_SET	3
+#define MFIE_TYPE_CF_SET	4
+#define MFIE_TYPE_TIM		5
+#define MFIE_TYPE_IBSS_SET	6
+#define MFIE_TYPE_CHALLENGE	16
+#define MFIE_TYPE_ERP		42
+#define MFIE_TYPE_RSN		48
+#define MFIE_TYPE_RATES_EX	50
+#define MFIE_TYPE_GENERIC	221
+
+/*
+ * These are the data types that can make up management packets
+ *
+	u16 auth_algorithm;
+	u16 auth_sequence;
+	u16 beacon_interval;
+	u16 capability;
+	u8 current_ap[ETH_ALEN];
+	u16 listen_interval;
+	struct {
+		u16 association_id:14, reserved:2;
+	} __packed;
+	u32 time_stamp[2];
+	u16 reason;
+	u16 status;
+*/
+
+#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
+#define IEEE80211_DEFAULT_BASIC_RATE 10
+
+/* SWEEP TABLE ENTRIES NUMBER*/
+#define MAX_SWEEP_TAB_ENTRIES		  42
+#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element... */
+#define MAX_RATES_LENGTH		((u8)12)
+#define MAX_RATES_EX_LENGTH		((u8)16)
+#define MAX_NETWORK_COUNT		128
+#define MAX_CHANNEL_NUMBER		161
+#define IEEE80211_SOFTMAC_SCAN_TIME	400
+/* HZ / 2) */
+#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define CRC_LENGTH		 4U
+
+#define MAX_WPA_IE_LEN (256)
+#define MAX_WPS_IE_LEN (512)
+#define MAX_P2P_IE_LEN (256)
+#define MAX_WFD_IE_LEN (128)
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+#define IEEE80211_DTIM_MBCAST 4
+#define IEEE80211_DTIM_UCAST 2
+#define IEEE80211_DTIM_VALID 1
+#define IEEE80211_DTIM_INVALID 0
+
+#define IEEE80211_PS_DISABLED 0
+#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
+#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
+#define IW_ESSID_MAX_SIZE 32
+/*
+join_res:
+-1: authentication fail
+-2: association fail
+> 0: TID
+*/
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+
+static inline int is_multicast_mac_addr(const u8 *addr)
+{
+	return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+
+static inline int is_broadcast_mac_addr(const u8 *addr)
+{
+	return (addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&
+	       (addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff);
+}
+
+#define CFG_IEEE80211_RESERVE_FCS (1<<0)
+#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
+
+#define MAXTID	16
+
+/* Action category code */
+enum rtw_ieee80211_category {
+	RTW_WLAN_CATEGORY_P2P = 0x7f,/* P2P action frames */
+};
+
+/* SPECTRUM_MGMT action code */
+enum rtw_ieee80211_spectrum_mgmt_actioncode {
+	RTW_WLAN_ACTION_SPCT_MSR_REQ = 0,
+	RTW_WLAN_ACTION_SPCT_MSR_RPRT = 1,
+	RTW_WLAN_ACTION_SPCT_TPC_REQ = 2,
+	RTW_WLAN_ACTION_SPCT_TPC_RPRT = 3,
+	RTW_WLAN_ACTION_SPCT_CHL_SWITCH = 4,
+	RTW_WLAN_ACTION_SPCT_EXT_CHL_SWITCH = 5,
+};
+
+enum _PUBLIC_ACTION {
+	ACT_PUBLIC_BSSCOEXIST = 0, /*  20/40 BSS Coexistence */
+	ACT_PUBLIC_DSE_ENABLE = 1,
+	ACT_PUBLIC_DSE_DEENABLE = 2,
+	ACT_PUBLIC_DSE_REG_LOCATION = 3,
+	ACT_PUBLIC_EXT_CHL_SWITCH = 4,
+	ACT_PUBLIC_DSE_MSR_REQ = 5,
+	ACT_PUBLIC_DSE_MSR_RPRT = 6,
+	ACT_PUBLIC_MP = 7, /*  Measurement Pilot */
+	ACT_PUBLIC_DSE_PWR_CONSTRAINT = 8,
+	ACT_PUBLIC_VENDOR = 9, /*  for WIFI_DIRECT */
+	ACT_PUBLIC_GAS_INITIAL_REQ = 10,
+	ACT_PUBLIC_GAS_INITIAL_RSP = 11,
+	ACT_PUBLIC_GAS_COMEBACK_REQ = 12,
+	ACT_PUBLIC_GAS_COMEBACK_RSP = 13,
+	ACT_PUBLIC_TDLS_DISCOVERY_RSP = 14,
+	ACT_PUBLIC_LOCATION_TRACK = 15,
+	ACT_PUBLIC_MAX
+};
+
+#define OUI_MICROSOFT 0x0050f2 /* Microsoft (also used in Wi-Fi specs)
+				* 00:50:F2 */
+#define WME_OUI_TYPE 2
+#define WME_OUI_SUBTYPE_INFORMATION_ELEMENT 0
+#define WME_OUI_SUBTYPE_PARAMETER_ELEMENT 1
+#define WME_OUI_SUBTYPE_TSPEC_ELEMENT 2
+#define WME_VERSION 1
+
+#define WME_ACTION_CODE_SETUP_REQUEST 0
+#define WME_ACTION_CODE_SETUP_RESPONSE 1
+#define WME_ACTION_CODE_TEARDOWN 2
+
+#define WME_SETUP_RESPONSE_STATUS_ADMISSION_ACCEPTED 0
+#define WME_SETUP_RESPONSE_STATUS_INVALID_PARAMETERS 1
+#define WME_SETUP_RESPONSE_STATUS_REFUSED 3
+
+#define WME_TSPEC_DIRECTION_UPLINK 0
+#define WME_TSPEC_DIRECTION_DOWNLINK 1
+#define WME_TSPEC_DIRECTION_BI_DIRECTIONAL 3
+
+#define OUI_BROADCOM 0x00904c /* Broadcom (Epigram) */
+
+#define VENDOR_HT_CAPAB_OUI_TYPE 0x33 /* 00-90-4c:0x33 */
+
+/**
+ * enum rtw_ieee80211_channel_flags - channel flags
+ *
+ * Channel flags set by the regulatory control code.
+ *
+ * @RTW_IEEE80211_CHAN_DISABLED: This channel is disabled.
+ * @RTW_IEEE80211_CHAN_PASSIVE_SCAN: Only passive scanning is permitted
+ *      on this channel.
+ * @RTW_IEEE80211_CHAN_NO_IBSS: IBSS is not allowed on this channel.
+ * @RTW_IEEE80211_CHAN_RADAR: Radar detection is required on this channel.
+ * @RTW_IEEE80211_CHAN_NO_HT40PLUS: extension channel above this channel
+ *      is not permitted.
+ * @RTW_IEEE80211_CHAN_NO_HT40MINUS: extension channel below this channel
+ *      is not permitted.
+ */
+enum rtw_ieee80211_channel_flags {
+	RTW_IEEE80211_CHAN_DISABLED	 = 1<<0,
+	RTW_IEEE80211_CHAN_PASSIVE_SCAN     = 1<<1,
+	RTW_IEEE80211_CHAN_NO_IBSS	  = 1<<2,
+	RTW_IEEE80211_CHAN_RADAR	    = 1<<3,
+	RTW_IEEE80211_CHAN_NO_HT40PLUS      = 1<<4,
+	RTW_IEEE80211_CHAN_NO_HT40MINUS     = 1<<5,
+};
+
+#define RTW_IEEE80211_CHAN_NO_HT40 \
+	  (RTW_IEEE80211_CHAN_NO_HT40PLUS | RTW_IEEE80211_CHAN_NO_HT40MINUS)
+
+/* Represent channel details, subset of ieee80211_channel */
+struct rtw_ieee80211_channel {
+	u16 hw_value;
+	u32 flags;
+};
+
+#define CHAN_FMT \
+	"hw_value:%u, " \
+	"flags:0x%08x" \
+
+#define CHAN_ARG(channel) \
+	(channel)->hw_value \
+	, (channel)->flags \
+
+/* Parsed Information Elements */
+struct rtw_ieee802_11_elems {
+	u8 *ssid;
+	u8 ssid_len;
+	u8 *supp_rates;
+	u8 supp_rates_len;
+	u8 *fh_params;
+	u8 fh_params_len;
+	u8 *ds_params;
+	u8 ds_params_len;
+	u8 *cf_params;
+	u8 cf_params_len;
+	u8 *tim;
+	u8 tim_len;
+	u8 *ibss_params;
+	u8 ibss_params_len;
+	u8 *challenge;
+	u8 challenge_len;
+	u8 *erp_info;
+	u8 erp_info_len;
+	u8 *ext_supp_rates;
+	u8 ext_supp_rates_len;
+	u8 *wpa_ie;
+	u8 wpa_ie_len;
+	u8 *rsn_ie;
+	u8 rsn_ie_len;
+	u8 *wme;
+	u8 wme_len;
+	u8 *wme_tspec;
+	u8 wme_tspec_len;
+	u8 *wps_ie;
+	u8 wps_ie_len;
+	u8 *power_cap;
+	u8 power_cap_len;
+	u8 *supp_channels;
+	u8 supp_channels_len;
+	u8 *mdie;
+	u8 mdie_len;
+	u8 *ftie;
+	u8 ftie_len;
+	u8 *timeout_int;
+	u8 timeout_int_len;
+	u8 *ht_capabilities;
+	u8 ht_capabilities_len;
+	u8 *ht_operation;
+	u8 ht_operation_len;
+	u8 *vendor_ht_cap;
+	u8 vendor_ht_cap_len;
+};
+
+enum parse_res {
+	ParseOK = 0,
+	ParseUnknown = 1,
+	ParseFailed = -1
+};
+
+enum parse_res rtw_ieee802_11_parse_elems(u8 *start, uint len,
+					  struct rtw_ieee802_11_elems *elems,
+					  int show_errors);
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len,
+		     unsigned char *source, unsigned int *frlen);
+u8 *rtw_set_ie(u8 *pbuf, int index, uint len, u8 *source, uint *frlen);
+u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit);
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode);
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit);
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit);
+int rtw_get_wpa_cipher_suite(u8 *s);
+int rtw_get_wpa2_cipher_suite(u8 *s);
+int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len);
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
+		     int *pairwise_cipher, int *is_8021x);
+int rtw_parse_wpa2_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
+		      int *pairwise_cipher, int *is_8021x);
+
+int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
+		   u8 *wpa_ie, u16 *wpa_len);
+
+u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen);
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen);
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
+		     u8 *buf_attr, u32 *len_attr);
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
+			     u8 *buf_content, uint *len_content);
+
+/**
+ * for_each_ie - iterate over continuous IEs
+ * @ie:
+ * @buf:
+ * @buf_len:
+ */
+#define for_each_ie(ie, buf, buf_len) \
+	for (ie = (void *)buf; (((u8 *)ie) - ((u8 *)buf) + 1) < buf_len;	\
+		ie = (void *)(((u8 *)ie) + *(((u8 *)ie)+1) + 2))
+
+u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen);
+u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
+		     u8 *buf_attr, u32 *len_attr);
+u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
+			     u8 *buf_content, uint *len_content);
+u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len,
+			     u8 *pdata_attr);
+void rtw_wlan_bssid_ex_remove_p2p_attr(struct wlan_bssid_ex *bss_ex,
+				       u8 attr_id);
+uint	rtw_get_rateset_len(u8	*rateset);
+
+struct registry_priv;
+int rtw_generate_ie(struct registry_priv *pregistrypriv);
+
+int rtw_get_bit_value_from_ieee_value(u8 val);
+
+bool	rtw_is_cckrates_included(u8 *rate);
+
+bool	rtw_is_cckratesonly_included(u8 *rate);
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel);
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork);
+
+void rtw_macaddr_cfg(u8 *mac_addr);
+
+u16 rtw_mcs_rate(u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char *MCS_rate);
+
+#endif /* IEEE80211_H */
diff --git a/drivers/staging/r8188eu/include/odm.h b/drivers/staging/r8188eu/include/odm.h
new file mode 100644
index 000000000..f131e1716
--- /dev/null
+++ b/drivers/staging/r8188eu/include/odm.h
@@ -0,0 +1,422 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef	__HALDMOUTSRC_H__
+#define __HALDMOUTSRC_H__
+
+struct rtw_dig {
+	u8		PreIGValue;
+	u8		CurIGValue;
+	u8		BackupIGValue;
+
+	u8		rx_gain_range_max;
+	u8		rx_gain_range_min;
+
+	u8		CurCCK_CCAThres;
+
+	u8		LargeFAHit;
+	u8		ForbiddenIGI;
+	u32		Recover_cnt;
+
+	u8		DIG_Dynamic_MIN_0;
+	bool		bMediaConnect_0;
+
+	u32		AntDiv_RSSI_max;
+	u32		RSSI_max;
+};
+
+struct rtl_ps {
+	u8		pre_rf_state;
+	u8		cur_rf_state;
+	u8		initialize;
+	u32		reg_874;
+	u32		reg_c70;
+	u32		reg_85c;
+	u32		reg_a74;
+
+};
+
+struct false_alarm_stats {
+	u32	Cnt_Parity_Fail;
+	u32	Cnt_Rate_Illegal;
+	u32	Cnt_Crc8_fail;
+	u32	Cnt_Mcs_fail;
+	u32	Cnt_Ofdm_fail;
+	u32	Cnt_Cck_fail;
+	u32	Cnt_all;
+	u32	Cnt_Fast_Fsync;
+	u32	Cnt_SB_Search_fail;
+	u32	Cnt_OFDM_CCA;
+	u32	Cnt_CCK_CCA;
+	u32	Cnt_CCA_all;
+	u32	Cnt_BW_USC;	/* Gary */
+	u32	Cnt_BW_LSC;	/* Gary */
+};
+
+#define ODM_ASSOCIATE_ENTRY_NUM	32 /*  Max size of AsocEntry[]. */
+
+struct sw_ant_switch {
+	u8	CurAntenna;
+	u8	SWAS_NoLink_State; /* Before link Antenna Switch check */
+	u8	RxIdleAnt;
+};
+
+struct edca_turbo {
+	bool bCurrentTurboEDCA;
+	bool bIsCurRDLState;
+	u32	prv_traffic_idx; /*  edca turbo */
+};
+
+struct odm_rate_adapt {
+	u8	HighRSSIThresh;	/*  if RSSI > HighRSSIThresh	=> RATRState is DM_RATR_STA_HIGH */
+	u8	LowRSSIThresh;	/*  if RSSI <= LowRSSIThresh	=> RATRState is DM_RATR_STA_LOW */
+	u8	RATRState;	/*  Current RSSI level, DM_RATR_STA_HIGH/DM_RATR_STA_MIDDLE/DM_RATR_STA_LOW */
+	u32	LastRATR;	/*  RATR Register Content */
+};
+
+#define IQK_MAC_REG_NUM		4
+#define IQK_ADDA_REG_NUM	16
+#define IQK_BB_REG_NUM		9
+#define HP_THERMAL_NUM		8
+
+#define AVG_THERMAL_NUM		8
+#define IQK_Matrix_REG_NUM	8
+
+struct odm_phy_dbg_info {
+	/* ODM Write,debug info */
+	s8	RxSNRdB[MAX_PATH_NUM_92CS];
+	u64	NumQryPhyStatus;
+	/* Others */
+	s32	RxEVM[MAX_PATH_NUM_92CS];
+};
+
+struct odm_per_pkt_info {
+	s8	Rate;
+	u8	StationID;
+	bool	bPacketMatchBSSID;
+	bool	bPacketToSelf;
+	bool	bPacketBeacon;
+};
+
+/*  2011/10/20 MH Define Common info enum for all team. */
+
+enum odm_common_info_def {
+	/*  Fixed value: */
+
+	/* HOOK BEFORE REG INIT----------- */
+	ODM_CMNINFO_MP_TEST_CHIP,
+	/* HOOK BEFORE REG INIT-----------  */
+
+/* CALL BY VALUE------------- */
+	ODM_CMNINFO_RF_ANTENNA_TYPE,		/*  u8 */
+/* CALL BY VALUE-------------*/
+};
+
+enum odm_ability_def {
+	/*  BB ODM section BIT 0-15 */
+	ODM_BB_RSSI_MONITOR		= BIT(4),
+	ODM_BB_ANT_DIV			= BIT(6),
+	ODM_BB_PWR_TRA			= BIT(8),
+};
+
+# define ODM_ITRF_USB 0x2
+
+/*  ODM_CMNINFO_WM_MODE */
+enum odm_wireless_mode {
+	ODM_WM_UNKNOW	= 0x0,
+	ODM_WM_B	= BIT(0),
+	ODM_WM_G	= BIT(1),
+	ODM_WM_N24G	= BIT(3),
+	ODM_WM_AUTO	= BIT(5),
+};
+
+struct odm_ra_info {
+	u8 RateID;
+	u32 RateMask;
+	u32 RAUseRate;
+	u8 RateSGI;
+	u8 RssiStaRA;
+	u8 PreRssiStaRA;
+	u8 SGIEnable;
+	u8 DecisionRate;
+	u8 PreRate;
+	u8 HighestRate;
+	u8 LowestRate;
+	u32 NscUp;
+	u32 NscDown;
+	u16 RTY[5];
+	u32 TOTAL;
+	u16 DROP;
+	u8 Active;
+	u16 RptTime;
+	u8 RAWaitingCounter;
+	u8 RAPendingCounter;
+	u8 PTActive;	/*  on or off */
+	u8 PTTryState;	/*  0 trying state, 1 for decision state */
+	u8 PTStage;	/*  0~6 */
+	u8 PTStopCount;	/* Stop PT counter */
+	u8 PTPreRate;	/*  if rate change do PT */
+	u8 PTPreRssi;	/*  if RSSI change 5% do PT */
+	u8 PTModeSS;	/*  decide whitch rate should do PT */
+	u8 RAstage;	/*  StageRA, decide how many times RA will be done
+			 * between PT */
+	u8 PTSmoothFactor;
+};
+
+struct ijk_matrix_regs_set {
+	bool	bIQKDone;
+	s32	Value[1][IQK_Matrix_REG_NUM];
+};
+
+struct odm_rf_cal {
+	/* for tx power tracking */
+	u32	RegA24; /*  for TempCCK */
+	s32	RegE94;
+	s32	RegE9C;
+	s32	RegEB4;
+	s32	RegEBC;
+
+	u8	TxPowerTrackControl; /* for mp mode, turn off txpwrtracking
+				      * as default */
+	u8	TM_Trigger;
+	u8	InternalPA5G[2];	/* pathA / pathB */
+
+	u8	ThermalMeter[2];    /* ThermalMeter, index 0 for RFIC0,
+				     * and 1 for RFIC1 */
+	u8	ThermalValue;
+	u8	ThermalValue_LCK;
+	u8	ThermalValue_IQK;
+	u8	ThermalValue_DPK;
+	u8	ThermalValue_AVG[AVG_THERMAL_NUM];
+	u8	ThermalValue_AVG_index;
+	u8	ThermalValue_RxGain;
+	u8	ThermalValue_Crystal;
+	u8	ThermalValue_DPKstore;
+	u8	ThermalValue_DPKtrack;
+	bool	TxPowerTrackingInProgress;
+	bool	bDPKenable;
+
+	bool	bReloadtxpowerindex;
+	u8	bRfPiEnable;
+
+	u8	CCK_index;
+	u8	OFDM_index;
+	bool bDoneTxpower;
+
+	u8	ThermalValue_HP[HP_THERMAL_NUM];
+	u8	ThermalValue_HP_index;
+	struct ijk_matrix_regs_set IQKMatrixRegSetting;
+
+	u8	Delta_IQK;
+	u8	Delta_LCK;
+
+	/* for IQK */
+	u32	RegC04;
+	u32	Reg874;
+	u32	RegC08;
+	u32	RegB68;
+	u32	RegB6C;
+	u32	Reg870;
+	u32	Reg860;
+	u32	Reg864;
+
+	bool	bIQKInitialized;
+	bool	bAntennaDetected;
+	u32	ADDA_backup[IQK_ADDA_REG_NUM];
+	u32	IQK_MAC_backup[IQK_MAC_REG_NUM];
+	u32	IQK_BB_backup_recover[9];
+	u32	IQK_BB_backup[IQK_BB_REG_NUM];
+
+	/* for APK */
+	u32	APKoutput[2][2]; /* path A/B; output1_1a/output1_2a */
+	u8	bAPKdone;
+	u8	bAPKThermalMeterIgnore;
+	u8	bDPdone;
+	u8	bDPPathAOK;
+	u8	bDPPathBOK;
+};
+
+/*  ODM Dynamic common info value definition */
+
+struct fast_ant_train {
+	u8	antsel_rx_keep_0;
+	u8	antsel_rx_keep_1;
+	u8	antsel_rx_keep_2;
+	u8	antsel_a[ODM_ASSOCIATE_ENTRY_NUM];
+	u8	antsel_b[ODM_ASSOCIATE_ENTRY_NUM];
+	u8	antsel_c[ODM_ASSOCIATE_ENTRY_NUM];
+	u32	MainAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32	AuxAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32	MainAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u32	AuxAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u8	RxIdleAnt;
+	bool	bBecomeLinked;
+};
+
+enum ant_div_type {
+	NO_ANTDIV			= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV		= 0x02,
+	FIXED_HW_ANTDIV			= 0x03,
+	CG_TRX_SMART_ANTDIV		= 0x04,
+};
+
+/* Copy from SD4 defined structure. We use to support PHY DM integration. */
+struct odm_dm_struct {
+	struct adapter *Adapter;	/*  For CE/NIC team */
+
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+	bool	bCckHighPower;
+	u8	RFPathRxEnable;		/*  ODM_CMNINFO_RFPATH_ENABLE */
+	u8	ControlChannel;
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+
+/* 1  COMMON INFORMATION */
+	/*  Init Value */
+/* HOOK BEFORE REG INIT----------- */
+	/*  ODM Support Ability DIG/RATR/TX_PWR_TRACK/ �K�K = 1/2/3/�K */
+	u32	SupportAbility;
+
+	u32	BK_SupportAbility;
+	u8	AntDivType;
+/* HOOK BEFORE REG INIT----------- */
+
+	/*  Dynamic Value */
+/*  POINTER REFERENCE----------- */
+	/*  Wireless mode B/G/A/N = BIT(0)/BIT(1)/BIT(2)/BIT(3) */
+	u8	*pWirelessMode; /* ODM_WIRELESS_MODE_E */
+	/*  Secondary channel offset don't_care/below/above = 0/1/2 */
+	u8	*pSecChOffset;
+	/*  BW info 20M/40M/80M = 0/1/2 */
+	enum ht_channel_width *pBandWidth;
+	/*  Central channel location Ch1/Ch2/.... */
+	u8	*pChannel;	/* central channel number */
+
+	/*  Common info for Status */
+	bool	*pbScanInProcess;
+	bool	*pbPowerSaving;
+/*  POINTER REFERENCE----------- */
+	/*  */
+/* CALL BY VALUE------------- */
+	bool	bLinked;
+	u8	RSSI_Min;
+	bool	bIsMPChip;
+	bool	bOneEntryOnly;
+/* CALL BY VALUE------------- */
+
+	/* 2 Define STA info. */
+	/*  _ODM_STA_INFO */
+	/*  For MP, we need to reduce one array pointer for default port.?? */
+	struct sta_info *pODM_StaInfo[ODM_ASSOCIATE_ENTRY_NUM];
+
+	u16	CurrminRptTime;
+	struct odm_ra_info RAInfo[ODM_ASSOCIATE_ENTRY_NUM]; /* Use MacID as
+			* array index. STA MacID=0,
+			* VWiFi Client MacID={1, ODM_ASSOCIATE_ENTRY_NUM-1} */
+
+	/*  Latest packet phy info (ODM write) */
+	struct odm_phy_dbg_info PhyDbgInfo;
+
+	/* ODM Structure */
+	struct fast_ant_train DM_FatTable;
+	struct rtw_dig	DM_DigTable;
+	struct rtl_ps	DM_PSTable;
+	struct false_alarm_stats FalseAlmCnt;
+	struct sw_ant_switch DM_SWAT_Table;
+
+	struct edca_turbo DM_EDCA_Table;
+
+	/* PSD */
+	bool	bDMInitialGainEnable;
+
+	struct odm_rate_adapt RateAdaptive;
+
+	struct odm_rf_cal RFCalibrateInfo;
+
+	/*  TX power tracking */
+	u8	BbSwingIdxOfdm;
+	u8	BbSwingIdxOfdmCurrent;
+	u8	BbSwingIdxOfdmBase;
+	bool	BbSwingFlagOfdm;
+	u8	BbSwingIdxCck;
+	u8	BbSwingIdxCckCurrent;
+	u8	BbSwingIdxCckBase;
+	bool	BbSwingFlagCck;
+};
+
+enum odm_bb_config_type {
+    CONFIG_BB_PHY_REG,
+    CONFIG_BB_AGC_TAB,
+    CONFIG_BB_AGC_TAB_2G,
+    CONFIG_BB_PHY_REG_PG,
+};
+
+#define		DM_DIG_MAX_NIC			0x4e
+#define		DM_DIG_MIN_NIC			0x1e /* 0x22/0x1c */
+
+#define		DM_DIG_MAX_AP			0x32
+
+/* vivi 92c&92d has different definition, 20110504 */
+/* this is for 92c */
+#define		DM_DIG_FA_TH0			0x200/* 0x20 */
+#define		DM_DIG_FA_TH1			0x300/* 0x100 */
+#define		DM_DIG_FA_TH2			0x400/* 0x200 */
+
+/* 3=========================================================== */
+/* 3 Rate Adaptive */
+/* 3=========================================================== */
+#define		DM_RATR_STA_INIT		0
+#define		DM_RATR_STA_HIGH		1
+#define		DM_RATR_STA_MIDDLE		2
+#define		DM_RATR_STA_LOW			3
+
+/* 3=========================================================== */
+/* 3 BB Power Save */
+/* 3=========================================================== */
+
+enum dm_rf {
+	RF_Save = 0,
+	RF_Normal = 1,
+	RF_MAX = 2,
+};
+
+/* 3=========================================================== */
+/* 3 Antenna Diversity */
+/* 3=========================================================== */
+enum dm_swas {
+	Antenna_A = 1,
+	Antenna_B = 2,
+	Antenna_MAX = 3,
+};
+
+/*  Extern Global Variables. */
+#define	OFDM_TABLE_SIZE_92D	43
+#define	CCK_TABLE_SIZE		33
+
+extern	u32 OFDMSwingTable[OFDM_TABLE_SIZE_92D];
+extern u8 cck_swing_table[CCK_TABLE_SIZE][8];
+
+/*  check Sta pointer valid or not */
+#define IS_STA_VALID(pSta)		(pSta)
+
+void ODM_Write_DIG(struct odm_dm_struct *pDM_Odm, u8 CurrentIGI);
+void ODM_Write_CCK_CCA_Thres(struct odm_dm_struct *pDM_Odm, u8 CurCCK_CCAThres);
+
+void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal);
+
+void ODM_TXPowerTrackingCheck(struct odm_dm_struct *pDM_Odm);
+
+bool ODM_RAStateCheck(struct odm_dm_struct *pDM_Odm, s32 RSSI,
+		      bool bForceUpdate, u8 *pRATRState);
+
+u32 ODM_Get_Rate_Bitmap(struct odm_dm_struct *pDM_Odm, u32 macid,
+			u32 ra_mask, u8 rssi_level);
+
+void ODM_DMInit(struct odm_dm_struct *pDM_Odm);
+
+void ODM_DMWatchdog(struct odm_dm_struct *pDM_Odm);
+
+void ODM_CmnInfoInit(struct odm_dm_struct *pDM_Odm,
+		     enum odm_common_info_def CmnInfo, u32 Value);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/odm_HWConfig.h b/drivers/staging/r8188eu/include/odm_HWConfig.h
new file mode 100644
index 000000000..3f7185780
--- /dev/null
+++ b/drivers/staging/r8188eu/include/odm_HWConfig.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef	__HALHWOUTSRC_H__
+#define __HALHWOUTSRC_H__
+
+/*  CCK Rates, TxHT = 0 */
+#define DESC92C_RATE1M				0x00
+#define DESC92C_RATE11M				0x03
+
+/*  MCS Rates, TxHT = 1 */
+#define DESC92C_RATEMCS8			0x14
+#define DESC92C_RATEMCS15			0x1b
+
+/*  structure and define */
+
+struct phy_rx_agc_info {
+	#ifdef __LITTLE_ENDIAN
+		u8	gain:7, trsw:1;
+	#else
+		u8	trsw:1, gain:7;
+	#endif
+};
+
+struct phy_status_rpt {
+	struct phy_rx_agc_info path_agc[3];
+	u8	ch_corr[2];
+	u8	cck_sig_qual_ofdm_pwdb_all;
+	u8	cck_agc_rpt_ofdm_cfosho_a;
+	u8	cck_rpt_b_ofdm_cfosho_b;
+	u8	rsvd_1;/* ch_corr_msb; */
+	u8	noise_power_db_msb;
+	u8	path_cfotail[2];
+	u8	pcts_mask[2];
+	s8	stream_rxevm[2];
+	u8	path_rxsnr[3];
+	u8	noise_power_db_lsb;
+	u8	rsvd_2[3];
+	u8	stream_csi[2];
+	u8	stream_target_csi[2];
+	s8	sig_evm;
+	u8	rsvd_3;
+
+#ifdef __LITTLE_ENDIAN
+	u8	antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+	u8	sgi_en:1;
+	u8	rxsc:2;
+	u8	idle_long:1;
+	u8	r_ant_train_en:1;
+	u8	ant_sel_b:1;
+	u8	ant_sel:1;
+#else	/*  _BIG_ENDIAN_ */
+	u8	ant_sel:1;
+	u8	ant_sel_b:1;
+	u8	r_ant_train_en:1;
+	u8	idle_long:1;
+	u8	rxsc:2;
+	u8	sgi_en:1;
+	u8	antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+#endif
+};
+
+void ODM_PhyStatusQuery(struct odm_dm_struct *pDM_Odm,
+			struct phy_info *pPhyInfo,
+			u8 *pPhyStatus,
+			struct odm_per_pkt_info *pPktinfo,
+			struct adapter *adapt);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/odm_RTL8188E.h b/drivers/staging/r8188eu/include/odm_RTL8188E.h
new file mode 100644
index 000000000..3c6471f1a
--- /dev/null
+++ b/drivers/staging/r8188eu/include/odm_RTL8188E.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef	__ODM_RTL8188E_H__
+#define __ODM_RTL8188E_H__
+
+#define	MAIN_ANT	0
+#define	AUX_ANT	1
+#define	MAIN_ANT_CG_TRX	1
+#define	AUX_ANT_CG_TRX	0
+#define	MAIN_ANT_CGCS_RX	0
+#define	AUX_ANT_CGCS_RX	1
+
+void ODM_AntennaDiversityInit_88E(struct odm_dm_struct *pDM_Odm);
+
+void ODM_AntennaDiversity_88E(struct odm_dm_struct *pDM_Odm);
+
+void ODM_SetTxAntByTxInfo_88E(struct odm_dm_struct *pDM_Odm, u8 *pDesc,
+			      u8 macId);
+
+void ODM_UpdateRxIdleAnt_88E(struct odm_dm_struct *pDM_Odm, u8 Ant);
+
+void ODM_AntselStatistics_88E(struct odm_dm_struct *pDM_Odm, u8	antsel_tr_mux,
+			      u32 MacId, u8 RxPWDBAll);
+
+void odm_FastAntTraining(struct odm_dm_struct *pDM_Odm);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/odm_RegDefine11N.h b/drivers/staging/r8188eu/include/odm_RegDefine11N.h
new file mode 100644
index 000000000..82a602b39
--- /dev/null
+++ b/drivers/staging/r8188eu/include/odm_RegDefine11N.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef	__ODM_REGDEFINE11N_H__
+#define __ODM_REGDEFINE11N_H__
+
+/* 2 BB REG LIST */
+/* PAGE 8 */
+#define	ODM_REG_TX_ANT_CTRL_11N			0x80C
+#define	ODM_REG_RX_DEFUALT_A_11N		0x858
+#define	ODM_REG_ANTSEL_CTRL_11N			0x860
+#define	ODM_REG_RX_ANT_CTRL_11N			0x864
+#define	ODM_REG_PIN_CTRL_11N			0x870
+#define	ODM_REG_SC_CNT_11N			0x8C4
+/* PAGE 9 */
+#define	ODM_REG_ANT_MAPPING1_11N		0x914
+/* PAGE A */
+#define	ODM_REG_CCK_ANTDIV_PARA1_11N		0xA00
+#define	ODM_REG_CCK_CCA_11N			0xA0A
+#define	ODM_REG_CCK_ANTDIV_PARA2_11N		0xA0C
+#define	ODM_REG_CCK_FA_RST_11N			0xA2C
+#define	ODM_REG_CCK_FA_MSB_11N			0xA58
+#define	ODM_REG_CCK_FA_LSB_11N			0xA5C
+#define	ODM_REG_CCK_CCA_CNT_11N			0xA60
+#define	ODM_REG_BB_PWR_SAV4_11N			0xA74
+/* PAGE B */
+#define	ODM_REG_LNA_SWITCH_11N			0xB2C
+/* PAGE C */
+#define	ODM_REG_OFDM_FA_HOLDC_11N		0xC00
+#define	ODM_REG_OFDM_FA_RSTC_11N		0xC0C
+#define	ODM_REG_IGI_A_11N			0xC50
+#define	ODM_REG_ANTDIV_PARA1_11N		0xCA4
+#define	ODM_REG_OFDM_FA_TYPE1_11N		0xCF0
+/* PAGE D */
+#define	ODM_REG_OFDM_FA_RSTD_11N		0xD00
+#define	ODM_REG_OFDM_FA_TYPE2_11N		0xDA0
+#define	ODM_REG_OFDM_FA_TYPE3_11N		0xDA4
+#define	ODM_REG_OFDM_FA_TYPE4_11N		0xDA8
+
+/* 2 MAC REG LIST */
+#define	ODM_REG_ANTSEL_PIN_11N			0x4C
+#define	ODM_REG_RESP_TX_11N			0x6D8
+
+/* DIG Related */
+#define	ODM_BIT_IGI_11N				0x0000007F
+
+#endif
diff --git a/drivers/staging/r8188eu/include/odm_types.h b/drivers/staging/r8188eu/include/odm_types.h
new file mode 100644
index 000000000..76302df4b
--- /dev/null
+++ b/drivers/staging/r8188eu/include/odm_types.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __ODM_TYPES_H__
+#define __ODM_TYPES_H__
+
+#define	ODM_CE			0x04	/* BIT(2) */
+
+#define SET_TX_DESC_ANTSEL_A_88E(__ptxdesc, __value)			\
+	le32p_replace_bits((__le32 *)(__ptxdesc + 8), __value, BIT(24))
+#define SET_TX_DESC_ANTSEL_B_88E(__ptxdesc, __value)			\
+	le32p_replace_bits((__le32 *)(__ptxdesc + 8), __value, BIT(25))
+#define SET_TX_DESC_ANTSEL_C_88E(__ptxdesc, __value)			\
+	le32p_replace_bits((__le32 *)(__ptxdesc + 28), __value, BIT(29))
+
+#endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/r8188eu/include/osdep_intf.h b/drivers/staging/r8188eu/include/osdep_intf.h
new file mode 100644
index 000000000..36511c469
--- /dev/null
+++ b/drivers/staging/r8188eu/include/osdep_intf.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __OSDEP_INTF_H_
+#define __OSDEP_INTF_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+struct intf_priv {
+	u8 *intf_dev;
+	u32	max_iosz;	/* USB2.0: 128, USB1.1: 64, SDIO:64 */
+	u32	max_xmitsz; /* USB2.0: unlimited, SDIO:512 */
+	u32	max_recvsz; /* USB2.0: unlimited, SDIO:512 */
+
+	u8 *io_rwmem;
+	u8 *allocated_io_rwmem;
+	u32	io_wsz; /* unit: 4bytes */
+	u32	io_rsz;/* unit: 4bytes */
+	u8 intf_status;
+
+	void (*_bus_io)(u8 *priv);
+
+/*
+Under Sync. IRP (SDIO/USB)
+A protection mechanism is necessary for the io_rwmem(read/write protocol)
+
+Under Async. IRP (SDIO/USB)
+The protection mechanism is through the pending queue.
+*/
+	struct mutex ioctl_mutex;
+	/*  when in USB, IO is through interrupt in/out endpoints */
+	struct usb_device	*udev;
+	struct urb *piorw_urb;
+	u8 io_irp_cnt;
+	u8 bio_irp_pending;
+	struct timer_list io_timer;
+	u8 bio_irp_timeout;
+	u8 bio_timer_cancel;
+};
+
+int netdev_open(struct net_device *pnetdev);
+int netdev_close(struct net_device *pnetdev);
+
+u8 rtw_init_drv_sw(struct adapter *padapter);
+u8 rtw_free_drv_sw(struct adapter *padapter);
+u8 rtw_reset_drv_sw(struct adapter *padapter);
+
+u32 rtw_start_drv_threads(struct adapter *padapter);
+void rtw_stop_drv_threads (struct adapter *padapter);
+void rtw_cancel_all_timer(struct adapter *padapter);
+
+int rtw_init_netdev_name(struct net_device *pnetdev, const char *ifname);
+struct net_device *rtw_init_netdev(struct adapter *padapter);
+u16 rtw_recv_select_queue(struct sk_buff *skb);
+
+void rtw_ips_dev_unload(struct adapter *padapter);
+
+int rtw_ips_pwr_up(struct adapter *padapter);
+void rtw_ips_pwr_down(struct adapter *padapter);
+
+#endif	/* _OSDEP_INTF_H_ */
diff --git a/drivers/staging/r8188eu/include/osdep_service.h b/drivers/staging/r8188eu/include/osdep_service.h
new file mode 100644
index 000000000..72990a1cd
--- /dev/null
+++ b/drivers/staging/r8188eu/include/osdep_service.h
@@ -0,0 +1,153 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __OSDEP_SERVICE_H_
+#define __OSDEP_SERVICE_H_
+
+#include <linux/sched/signal.h>
+
+#define _FAIL		0
+#define _SUCCESS	1
+#define RTW_RX_HANDLED 2
+
+#include <linux/spinlock.h>
+#include <linux/compiler.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/circ_buf.h>
+#include <linux/uaccess.h>
+#include <asm/byteorder.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/sem.h>
+#include <linux/sched.h>
+#include <linux/etherdevice.h>
+#include <linux/wireless.h>
+#include <net/iw_handler.h>
+#include <linux/if_arp.h>
+#include <linux/rtnetlink.h>
+#include <linux/delay.h>
+#include <linux/proc_fs.h>	/*  Necessary because we use the proc fs */
+#include <linux/interrupt.h>	/*  for struct tasklet_struct */
+#include <linux/ip.h>
+#include <linux/kthread.h>
+#include <linux/vmalloc.h>
+
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+
+struct	__queue	{
+	struct	list_head	queue;
+	spinlock_t lock;
+};
+
+static inline struct list_head *get_list_head(struct __queue *queue)
+{
+	return (&(queue->queue));
+}
+
+static inline void _set_timer(struct timer_list *ptimer,u32 delay_time)
+{
+	mod_timer(ptimer, jiffies + msecs_to_jiffies(delay_time));
+}
+
+static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
+{
+	return  netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 0)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 1)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 2)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3));
+}
+
+extern int RTW_STATUS_CODE(int error_code);
+
+void *rtw_malloc2d(int h, int w, int size);
+
+#define rtw_init_queue(q)					\
+	do {							\
+		INIT_LIST_HEAD(&((q)->queue));			\
+		spin_lock_init(&((q)->lock));			\
+	} while (0)
+
+static inline unsigned char _cancel_timer_ex(struct timer_list *ptimer)
+{
+	return del_timer_sync(ptimer);
+}
+
+static inline void flush_signals_thread(void)
+{
+	if (signal_pending (current))
+		flush_signals(current);
+}
+
+struct rtw_netdev_priv_indicator {
+	void *priv;
+	u32 sizeof_priv;
+};
+struct net_device *rtw_alloc_etherdev_with_old_priv(int sizeof_priv,
+						    void *old_priv);
+struct net_device *rtw_alloc_etherdev(int sizeof_priv);
+
+#define rtw_netdev_priv(netdev)					\
+	(((struct rtw_netdev_priv_indicator *)netdev_priv(netdev))->priv)
+void rtw_free_netdev(struct net_device *netdev);
+
+#define NDEV_FMT "%s"
+#define NDEV_ARG(ndev) ndev->name
+#define ADPT_FMT "%s"
+#define ADPT_ARG(adapter) adapter->pnetdev->name
+#define FUNC_NDEV_FMT "%s(%s)"
+#define FUNC_NDEV_ARG(ndev) __func__, ndev->name
+#define FUNC_ADPT_FMT "%s(%s)"
+#define FUNC_ADPT_ARG(adapter) __func__, adapter->pnetdev->name
+
+#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)),(sig), 1)
+
+/* Macros for handling unaligned memory accesses */
+
+#define RTW_GET_BE16(a) ((u16) (((a)[0] << 8) | (a)[1]))
+#define RTW_PUT_BE16(a, val)			\
+	do {					\
+		(a)[0] = ((u16) (val)) >> 8;	\
+		(a)[1] = ((u16) (val)) & 0xff;	\
+	} while (0)
+
+#define RTW_PUT_LE16(a, val)			\
+	do {					\
+		(a)[1] = ((u16) (val)) >> 8;	\
+		(a)[0] = ((u16) (val)) & 0xff;	\
+	} while (0)
+
+#define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
+			 ((u32) (a)[2]))
+
+#define RTW_PUT_BE32(a, val)					\
+	do {							\
+		(a)[0] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
+		(a)[1] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
+		(a)[2] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
+		(a)[3] = (u8) (((u32) (val)) & 0xff);		\
+	} while (0)
+
+void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len);
+
+struct rtw_cbuf {
+	u32 write;
+	u32 read;
+	u32 size;
+	void *bufs[];
+};
+
+bool rtw_cbuf_empty(struct rtw_cbuf *cbuf);
+void *rtw_cbuf_pop(struct rtw_cbuf *cbuf);
+struct rtw_cbuf *rtw_cbuf_alloc(u32 size);
+int wifirate2_ratetbl_inx(unsigned char rate);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtl8188e_cmd.h b/drivers/staging/r8188eu/include/rtl8188e_cmd.h
new file mode 100644
index 000000000..1e01c1662
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_cmd.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_CMD_H__
+#define __RTL8188E_CMD_H__
+
+enum RTL8188E_H2C_CMD_ID {
+	/* Class Common */
+	H2C_COM_RSVD_PAGE		= 0x00,
+	H2C_COM_MEDIA_STATUS_RPT	= 0x01,
+	H2C_COM_SCAN			= 0x02,
+	H2C_COM_KEEP_ALIVE		= 0x03,
+	H2C_COM_DISCNT_DECISION		= 0x04,
+	H2C_COM_INIT_OFFLOAD		= 0x06,
+	H2C_COM_REMOTE_WAKE_CTL		= 0x07,
+	H2C_COM_AP_OFFLOAD		= 0x08,
+	H2C_COM_BCN_RSVD_PAGE		= 0x09,
+	H2C_COM_PROB_RSP_RSVD_PAGE	= 0x0A,
+
+	/* Class PS */
+	H2C_PS_PWR_MODE			= 0x20,
+	H2C_PS_TUNE_PARA		= 0x21,
+	H2C_PS_TUNE_PARA_2		= 0x22,
+	H2C_PS_LPS_PARA			= 0x23,
+	H2C_PS_P2P_OFFLOAD		= 0x24,
+
+	/* Class DM */
+	H2C_DM_MACID_CFG		= 0x40,
+	H2C_DM_TXBF			= 0x41,
+};
+
+struct cmd_msg_parm {
+	u8 eid; /* element id */
+	u8 sz; /*  sz */
+	u8 buf[6];
+};
+
+struct setpwrmode_parm {
+	u8 Mode;/* 0:Active,1:LPS,2:WMMPS */
+	u8 SmartPS_RLBM;/* LPS= 0:PS_Poll,1:PS_Poll,2:NullData,WMM= 0:PS_Poll,1:NullData */
+	u8 AwakeInterval;	/*  unit: beacon interval */
+	u8 bAllQueueUAPSD;
+	u8 PwrState;/* AllON(0x0c),RFON(0x04),RFOFF(0x00) */
+};
+
+struct H2C_SS_RFOFF_PARAM {
+	u8 ROFOn; /*  1: on, 0:off */
+	u16 gpio_period; /*  unit: 1024 us */
+} __packed;
+
+struct joinbssrpt_parm {
+	u8 OpMode;	/*  RT_MEDIA_STATUS */
+};
+
+struct rsvdpage_loc {
+	u8 LocProbeRsp;
+	u8 LocPsPoll;
+	u8 LocNullData;
+	u8 LocQosNull;
+	u8 LocBTQosNull;
+};
+
+struct P2P_PS_Offload_t {
+	u8 Offload_En:1;
+	u8 role:1; /*  1: Owner, 0: Client */
+	u8 CTWindow_En:1;
+	u8 NoA0_En:1;
+	u8 NoA1_En:1;
+	u8 AllStaSleep:1; /*  Only valid in Owner */
+	u8 discovery:1;
+	u8 rsvd:1;
+};
+
+struct P2P_PS_CTWPeriod_t {
+	u8 CTWPeriod;	/* TU */
+};
+
+/*  host message to firmware cmd */
+void rtl8188e_set_FwPwrMode_cmd(struct adapter *padapter, u8 Mode);
+void rtl8188e_set_FwJoinBssReport_cmd(struct adapter *padapter, u8 mstatus);
+u8 rtl8188e_set_raid_cmd(struct adapter *padapter, u32 mask);
+void rtl8188e_Add_RateATid(struct adapter *padapter, u32 bitmap, u8 arg,
+			   u8 rssi_level);
+
+void rtl8188e_set_p2p_ps_offload_cmd(struct adapter *adapt, u8 p2p_ps_state);
+
+void CheckFwRsvdPageContent(struct adapter *adapt);
+void rtl8188e_set_FwMediaStatus_cmd(struct adapter *adapt, __le16 mstatus_rpt);
+
+#endif/* __RTL8188E_CMD_H__ */
diff --git a/drivers/staging/r8188eu/include/rtl8188e_dm.h b/drivers/staging/r8188eu/include/rtl8188e_dm.h
new file mode 100644
index 000000000..d62cdfc2d
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_dm.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_DM_H__
+#define __RTL8188E_DM_H__
+
+enum{
+	UP_LINK,
+	DOWN_LINK,
+};
+
+struct	dm_priv {
+	u32	InitODMFlag;
+
+	/* Lower Signal threshold for Rate Adaptive */
+	int	EntryMinUndecoratedSmoothedPWDB;
+	int	MinUndecoratedPWDBForDM;
+};
+
+void rtl8188e_init_dm_priv(struct adapter *adapt);
+void rtl8188e_InitHalDm(struct adapter *adapt);
+void rtl8188e_HalDmWatchDog(struct adapter *adapt);
+
+void AntDivCompare8188E(struct adapter *adapt, struct wlan_bssid_ex *dst,
+			struct wlan_bssid_ex *src);
+u8 AntDivBeforeLink8188E(struct adapter *adapt);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtl8188e_hal.h b/drivers/staging/r8188eu/include/rtl8188e_hal.h
new file mode 100644
index 000000000..ed4091e7c
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_hal.h
@@ -0,0 +1,182 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_HAL_H__
+#define __RTL8188E_HAL_H__
+
+/* include HAL Related header after HAL Related compiling flags */
+#include "rtl8188e_spec.h"
+#include "Hal8188EPhyReg.h"
+#include "Hal8188EPhyCfg.h"
+#include "rtl8188e_rf.h"
+#include "rtl8188e_dm.h"
+#include "rtl8188e_recv.h"
+#include "rtl8188e_xmit.h"
+#include "rtl8188e_cmd.h"
+#include "rtw_efuse.h"
+#include "odm_types.h"
+#include "odm.h"
+#include "odm_HWConfig.h"
+#include "odm_RegDefine11N.h"
+#include "HalPhyRf_8188e.h"
+#include "Hal8188ERateAdaptive.h"
+#include "HalHWImg8188E_MAC.h"
+#include "HalHWImg8188E_RF.h"
+#include "HalHWImg8188E_BB.h"
+#include "odm_RTL8188E.h"
+
+#define DRVINFO_SZ	4 /*  unit is 8bytes */
+#define PageNum_128(_Len)	(u32)(((_Len)>>7) + ((_Len) & 0x7F ? 1 : 0))
+
+#define DRIVER_EARLY_INT_TIME		0x05
+#define BCN_DMA_ATIME_INT_TIME		0x02
+
+#define MAX_RX_DMA_BUFFER_SIZE_88E				\
+      0x2400 /* 9k for 88E nornal chip , MaxRxBuff=10k-max(TxReportSize(64*8),
+	      * WOLPattern(16*24)) */
+
+#define TX_SELE_LQ			BIT(1)		/*  Low Queue */
+#define TX_SELE_NQ			BIT(2)		/*  Normal Queue */
+
+/*  Note: We will divide number of page equally for each queue other
+ *  than public queue! */
+/*  22k = 22528 bytes = 176 pages (@page =  128 bytes) */
+/*  must reserved about 7 pages for LPS =>  176-7 = 169 (0xA9) */
+/*  2*BCN / 1*ps-poll / 1*null-data /1*prob_rsp /1*QOS null-data /1*BT QOS
+ *  null-data */
+
+#define TX_TOTAL_PAGE_NUMBER_88E		0xA9/*   169 (21632=> 21k) */
+
+#define TX_PAGE_BOUNDARY_88E (TX_TOTAL_PAGE_NUMBER_88E + 1)
+
+#include "HalVerDef.h"
+#include "hal_com.h"
+
+/* 	Channel Plan */
+enum ChannelPlan {
+	CHPL_FCC	= 0,
+	CHPL_IC		= 1,
+	CHPL_ETSI	= 2,
+	CHPL_SPA	= 3,
+	CHPL_FRANCE	= 4,
+	CHPL_MKK	= 5,
+	CHPL_MKK1	= 6,
+	CHPL_ISRAEL	= 7,
+	CHPL_TELEC	= 8,
+	CHPL_GLOBAL	= 9,
+	CHPL_WORLD	= 10,
+};
+
+struct txpowerinfo24g {
+	u8 IndexCCK_Base[RF_PATH_MAX][MAX_CHNL_GROUP_24G];
+	u8 IndexBW40_Base[RF_PATH_MAX][MAX_CHNL_GROUP_24G];
+	/* If only one tx, only BW20 and OFDM are used. */
+	s8 CCK_Diff[RF_PATH_MAX][MAX_TX_COUNT];
+	s8 OFDM_Diff[RF_PATH_MAX][MAX_TX_COUNT];
+	s8 BW20_Diff[RF_PATH_MAX][MAX_TX_COUNT];
+	s8 BW40_Diff[RF_PATH_MAX][MAX_TX_COUNT];
+};
+
+#define EFUSE_REAL_CONTENT_LEN		512
+#define AVAILABLE_EFUSE_ADDR(addr)	(addr < EFUSE_REAL_CONTENT_LEN)
+
+#define		EFUSE_REAL_CONTENT_LEN_88E	256
+#define		EFUSE_MAP_LEN_88E		512
+#define		EFUSE_MAX_SECTION_88E		64
+/*  To prevent out of boundary programming case, leave 1byte and program
+ *  full section */
+/*  9bytes + 1byt + 5bytes and pre 1byte. */
+/*  For worst case: */
+/*  | 2byte|----8bytes----|1byte|--7bytes--| 92D */
+/*  PG data exclude header, dummy 7 bytes frome CP test and reserved 1byte. */
+#define		EFUSE_OOB_PROTECT_BYTES_88E	18
+
+#define EFUSE_PROTECT_BYTES_BANK	16
+
+#define USB_RXAGG_PAGE_COUNT	48
+#define USB_RXAGG_PAGE_TIMEOUT	0x4
+
+struct hal_data_8188e {
+	struct HAL_VERSION	VersionID;
+	/* current WIFI_PHY values */
+	enum ht_channel_width CurrentChannelBW;
+	u8	CurrentChannel;
+	u8	nCur40MhzPrimeSC;/*  Control channel sub-carrier */
+
+	u8	EEPROMRegulatory;
+	u8	EEPROMThermalMeter;
+
+	u8	Index24G_CCK_Base[CHANNEL_MAX_NUMBER];
+	u8	Index24G_BW40_Base[CHANNEL_MAX_NUMBER];
+	/* If only one tx, only BW20 and OFDM are used. */
+	s8	OFDM_24G_Diff[MAX_TX_COUNT];
+	s8	BW20_24G_Diff[MAX_TX_COUNT];
+
+	/*  HT 20<->40 Pwr diff */
+	u8	TxPwrHt20Diff[RF_PATH_MAX][CHANNEL_MAX_NUMBER];
+	/*  For HT<->legacy pwr diff */
+	u8	TxPwrLegacyHtDiff[RF_PATH_MAX][CHANNEL_MAX_NUMBER];
+	/*  For power group */
+	u8	PwrGroupHT20[RF_PATH_MAX][CHANNEL_MAX_NUMBER];
+	u8	PwrGroupHT40[RF_PATH_MAX][CHANNEL_MAX_NUMBER];
+
+	/*  Read/write are allow for following hardware information variables */
+	u8	pwrGroupCnt;
+	u32	MCSTxPowerLevelOriginalOffset[MAX_PG_GROUP][16];
+
+	u8	CrystalCap;
+
+	u32	AcParam_BE; /* Original parameter for BE, use for EDCA turbo. */
+
+	struct bb_reg_def PHYRegDef;
+
+	u32	RfRegChnlVal;
+
+	/* for host message to fw */
+	u8	LastHMEBoxNum;
+
+	u8	fw_ractrl;
+	u8	RegFwHwTxQCtrl;
+	u8	RegReg542;
+	u8	RegCR_1;
+
+	struct dm_priv	dmpriv;
+	struct odm_dm_struct odmpriv;
+
+	u8	CurAntenna;
+	u8	AntDivCfg;
+	u8	TRxAntDivType;
+
+	u8	out_ep_extra_queues;
+
+	struct P2P_PS_Offload_t	p2p_ps_offload;
+
+	/*  Auto FSM to Turn On, include clock, isolation, power control
+	 *  for MAC only */
+	u8	bMacPwrCtrlOn;
+};
+
+s32 InitLLTTable(struct adapter *padapter, u8 txpktbuf_bndy);
+
+/*  EFuse */
+void Hal_EfuseParseIDCode88E(struct adapter *padapter, u8 *hwinfo);
+void Hal_ReadTxPowerInfo88E(struct adapter *padapter, u8 *hwinfo,
+			    bool AutoLoadFail);
+
+void rtl8188e_EfuseParseChnlPlan(struct adapter *padapter, u8 *hwinfo,
+				 bool AutoLoadFail);
+void Hal_ReadAntennaDiversity88E(struct adapter *pAdapter,u8 *PROMContent,
+				 bool AutoLoadFail);
+void Hal_ReadThermalMeter_88E(struct adapter *	dapter, u8 *PROMContent,
+			      bool AutoloadFail);
+void Hal_EfuseParseXtal_8188E(struct adapter *pAdapter, u8 *hwinfo,
+			      bool AutoLoadFail);
+void Hal_ReadPowerSavingMode88E(struct adapter *pAdapter, u8 *hwinfo,
+				bool AutoLoadFail);
+
+void rtl8188e_read_chip_version(struct adapter *padapter);
+
+s32 rtl8188e_iol_efuse_patch(struct adapter *padapter);
+void rtw_cancel_all_timer(struct adapter *padapter);
+
+#endif /* __RTL8188E_HAL_H__ */
diff --git a/drivers/staging/r8188eu/include/rtl8188e_recv.h b/drivers/staging/r8188eu/include/rtl8188e_recv.h
new file mode 100644
index 000000000..dc4f358f6
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_recv.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_RECV_H__
+#define __RTL8188E_RECV_H__
+
+#define TX_RPT1_PKT_LEN 8
+
+#define NR_PREALLOC_RECV_SKB (8)
+
+#define NR_RECVBUFF (4)
+
+#define MAX_RECVBUF_SZ (15360) /*  15k < 16k */
+
+struct phy_stat {
+	unsigned int phydw0;
+	unsigned int phydw1;
+	unsigned int phydw2;
+	unsigned int phydw3;
+	unsigned int phydw4;
+	unsigned int phydw5;
+	unsigned int phydw6;
+	unsigned int phydw7;
+};
+
+/*  Rx smooth factor */
+#define	Rx_Smooth_Factor (20)
+
+enum rx_packet_type {
+	NORMAL_RX,/* Normal rx packet */
+	TX_REPORT1,/* CCX */
+	TX_REPORT2,/* TX RPT */
+	HIS_REPORT,/*  USB HISR RPT */
+};
+
+void rtl8188eu_recv_tasklet(unsigned long priv);
+void update_recvframe_phyinfo_88e(struct recv_frame *fra, struct phy_stat *phy);
+void update_recvframe_attrib_88e(struct recv_frame *fra, struct recv_stat *stat);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtl8188e_rf.h b/drivers/staging/r8188eu/include/rtl8188e_rf.h
new file mode 100644
index 000000000..63ac0acc6
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_rf.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_RF_H__
+#define __RTL8188E_RF_H__
+
+#define		RF6052_MAX_TX_PWR		0x3F
+#define		RF6052_MAX_REG			0x3F
+#define		RF6052_MAX_PATH			2
+
+int phy_RF6052_Config_ParaFile(struct adapter *Adapter);
+void rtl8188e_PHY_RF6052SetBandwidth(struct adapter *Adapter,
+				     enum ht_channel_width Bandwidth);
+void	rtl8188e_PHY_RF6052SetCckTxPower(struct adapter *Adapter, u8 *level);
+void	rtl8188e_PHY_RF6052SetOFDMTxPower(struct adapter *Adapter, u8 *ofdm,
+					  u8 *pwrbw20, u8 *pwrbw40, u8 channel);
+
+#endif/* __RTL8188E_RF_H__ */
diff --git a/drivers/staging/r8188eu/include/rtl8188e_spec.h b/drivers/staging/r8188eu/include/rtl8188e_spec.h
new file mode 100644
index 000000000..e34619140
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_spec.h
@@ -0,0 +1,1163 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_SPEC_H__
+#define __RTL8188E_SPEC_H__
+
+/*        8192C Regsiter offset definition */
+
+#define		HAL_PS_TIMER_INT_DELAY	50	/*   50 microseconds */
+#define		HAL_92C_NAV_UPPER_UNIT	128	/*  micro-second */
+
+/*  8188E PKT_BUFF_ACCESS_CTRL value */
+#define TXPKT_BUF_SELECT		0x69
+#define RXPKT_BUF_SELECT		0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS	0x0
+
+/* 	0x0000h ~ 0x00FFh	System Configuration */
+#define REG_SYS_ISO_CTRL		0x0000
+#define REG_SYS_FUNC_EN			0x0002
+#define REG_APS_FSMCO			0x0004
+#define REG_SYS_CLKR			0x0008
+#define REG_9346CR			0x000A
+#define REG_EE_VPD			0x000C
+#define REG_AFE_MISC			0x0010
+#define REG_SPS0_CTRL			0x0011
+#define REG_SPS_OCP_CFG			0x0018
+#define REG_RSV_CTRL			0x001C
+#define REG_RF_CTRL			0x001F
+#define REG_LDOA15_CTRL			0x0020
+#define REG_LDOV12D_CTRL		0x0021
+#define REG_LDOHCI12_CTRL		0x0022
+#define REG_LPLDO_CTRL			0x0023
+#define REG_AFE_XTAL_CTRL		0x0024
+#define REG_AFE_PLL_CTRL		0x0028
+#define REG_APE_PLL_CTRL_EXT		0x002c
+#define REG_EFUSE_CTRL			0x0030
+#define REG_EFUSE_TEST			0x0034
+#define REG_GPIO_MUXCFG			0x0040
+#define REG_GPIO_IO_SEL			0x0042
+#define REG_MAC_PINMUX_CFG		0x0043
+#define REG_GPIO_PIN_CTRL		0x0044
+#define REG_GPIO_INTM			0x0048
+#define REG_LEDCFG0			0x004C
+#define REG_LEDCFG1			0x004D
+#define REG_LEDCFG2			0x004E
+#define REG_LEDCFG3			0x004F
+#define REG_FSIMR			0x0050
+#define REG_FSISR			0x0054
+#define REG_HSIMR			0x0058
+#define REG_HSISR			0x005c
+#define REG_GPIO_PIN_CTRL_2		0x0060 /*  RTL8723 WIFI/BT/GPS
+				 * Multi-Function GPIO Pin Control. */
+#define REG_GPIO_IO_SEL_2		0x0062 /*  RTL8723 WIFI/BT/GPS
+				 * Multi-Function GPIO Select. */
+#define REG_BB_PAD_CTRL			0x0064
+#define REG_MULTI_FUNC_CTRL		0x0068 /*  RTL8723 WIFI/BT/GPS
+				 * Multi-Function control source. */
+#define REG_GPIO_OUTPUT			0x006c
+#define REG_AFE_XTAL_CTRL_EXT		0x0078 /* RTL8188E */
+#define REG_XCK_OUT_CTRL		0x007c /* RTL8188E */
+#define REG_MCUFWDL			0x0080
+#define REG_WOL_EVENT			0x0081 /* RTL8188E */
+#define REG_MCUTSTCFG			0x0084
+#define REG_HMEBOX_E0			0x0088
+#define REG_HMEBOX_E1			0x008A
+#define REG_HMEBOX_E2			0x008C
+#define REG_HMEBOX_E3			0x008E
+#define REG_HMEBOX_EXT_0		0x01F0
+#define REG_HMEBOX_EXT_1		0x01F4
+#define REG_HMEBOX_EXT_2		0x01F8
+#define REG_HMEBOX_EXT_3		0x01FC
+#define REG_HIMR_88E			0x00B0
+#define REG_HISR_88E			0x00B4
+#define REG_HIMRE_88E			0x00B8
+#define REG_HISRE_88E			0x00BC
+#define REG_EFUSE_ACCESS		0x00CF	/*  Efuse access protection
+						 * for RTL8723 */
+#define REG_BIST_SCAN			0x00D0
+#define REG_BIST_RPT			0x00D4
+#define REG_BIST_ROM_RPT		0x00D8
+#define REG_USB_SIE_INTF		0x00E0
+#define REG_PCIE_MIO_INTF		0x00E4
+#define REG_PCIE_MIO_INTD		0x00E8
+#define REG_HPON_FSM			0x00EC
+#define REG_SYS_CFG			0x00F0
+#define REG_GPIO_OUTSTS			0x00F4	/*  For RTL8723 only. */
+#define REG_TYPE_ID			0x00FC
+
+#define REG_MAC_PHY_CTRL_NORMAL		0x00f8
+
+/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+#define REG_CR				0x0100
+#define REG_PBP				0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL	0x0106
+#define REG_TRXDMA_CTRL			0x010C
+#define REG_TRXFF_BNDY			0x0114
+#define REG_TRXFF_STATUS		0x0118
+#define REG_RXFF_PTR			0x011C
+/* define REG_HIMR			0x0120 */
+/* define REG_HISR			0x0124 */
+#define REG_HIMRE			0x0128
+#define REG_HISRE			0x012C
+#define REG_CPWM			0x012F
+#define REG_FWIMR			0x0130
+#define REG_FTIMR			0x0138
+#define REG_FWISR			0x0134
+#define REG_PKTBUF_DBG_CTRL		0x0140
+#define REG_PKTBUF_DBG_ADDR		(REG_PKTBUF_DBG_CTRL)
+#define REG_RXPKTBUF_DBG		(REG_PKTBUF_DBG_CTRL+2)
+#define REG_TXPKTBUF_DBG		(REG_PKTBUF_DBG_CTRL+3)
+#define REG_RXPKTBUF_CTRL		(REG_PKTBUF_DBG_CTRL+2)
+#define REG_PKTBUF_DBG_DATA_L		0x0144
+#define REG_PKTBUF_DBG_DATA_H		0x0148
+
+#define REG_TC0_CTRL			0x0150
+#define REG_TC1_CTRL			0x0154
+#define REG_TC2_CTRL			0x0158
+#define REG_TC3_CTRL			0x015C
+#define REG_TC4_CTRL			0x0160
+#define REG_TCUNIT_BASE			0x0164
+#define REG_MBIST_START			0x0174
+#define REG_MBIST_DONE			0x0178
+#define REG_MBIST_FAIL			0x017C
+#define REG_32K_CTRL			0x0194 /* RTL8188E */
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
+#define REG_C2HEVT_CLEAR		0x01AF
+#define REG_MCUTST_1			0x01c0
+#define REG_FMETHR			0x01C8
+#define REG_HMETFR			0x01CC
+#define REG_HMEBOX_0			0x01D0
+#define REG_HMEBOX_1			0x01D4
+#define REG_HMEBOX_2			0x01D8
+#define REG_HMEBOX_3			0x01DC
+
+#define REG_LLT_INIT			0x01E0
+
+/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+#define REG_RQPN			0x0200
+#define REG_FIFOPAGE			0x0204
+#define REG_TDECTRL			0x0208
+#define REG_TXDMA_OFFSET_CHK		0x020C
+#define REG_TXDMA_STATUS		0x0210
+#define REG_RQPN_NPQ			0x0214
+
+/* 	0x0280h ~ 0x02FFh	RXDMA Configuration */
+#define		REG_RXDMA_AGG_PG_TH	0x0280
+#define	REG_RXPKT_NUM			0x0284
+#define		REG_RXDMA_STATUS	0x0288
+
+/* 	0x0300h ~ 0x03FFh	PCIe */
+#define	REG_PCIE_CTRL_REG		0x0300
+#define	REG_INT_MIG			0x0304	/*  Interrupt Migration */
+#define	REG_BCNQ_DESA			0x0308	/*  TX Beacon Descr Address */
+#define	REG_HQ_DESA			0x0310	/*  TX High Queue Descr Addr */
+#define	REG_MGQ_DESA			0x0318	/*  TX Manage Queue Descr Addr*/
+#define	REG_VOQ_DESA			0x0320	/*  TX VO Queue Descr Addr */
+#define	REG_VIQ_DESA			0x0328	/*  TX VI Queue Descr Addr */
+#define	REG_BEQ_DESA			0x0330	/*  TX BE Queue Descr Addr */
+#define	REG_BKQ_DESA			0x0338	/*  TX BK Queue Descr Addr */
+#define	REG_RX_DESA			0x0340	/*  RX Queue Descr Addr */
+#define	REG_MDIO			0x0354	/*  MDIO for Access PCIE PHY */
+#define	REG_DBG_SEL			0x0360	/*  Debug Selection Register */
+#define	REG_PCIE_HRPWM			0x0361	/* PCIe RPWM */
+#define	REG_PCIE_HCPWM			0x0363	/* PCIe CPWM */
+#define	REG_WATCH_DOG			0x0368
+
+/*  RTL8723 series ------------------------------ */
+#define	REG_PCIE_HISR			0x03A0
+
+/*  spec version 11 */
+/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+#define REG_VOQ_INFORMATION		0x0400
+#define REG_VIQ_INFORMATION		0x0404
+#define REG_BEQ_INFORMATION		0x0408
+#define REG_BKQ_INFORMATION		0x040C
+#define REG_MGQ_INFORMATION		0x0410
+#define REG_HGQ_INFORMATION		0x0414
+#define REG_BCNQ_INFORMATION		0x0418
+#define REG_TXPKT_EMPTY			0x041A
+
+#define REG_CPU_MGQ_INFORMATION		0x041C
+#define REG_FWHW_TXQ_CTRL		0x0420
+#define REG_HWSEQ_CTRL			0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY		0x0424
+#define REG_TXPKTBUF_MGQ_BDNY		0x0425
+#define REG_LIFETIME_EN			0x0426
+#define REG_MULTI_BCNQ_OFFSET		0x0427
+#define REG_SPEC_SIFS			0x0428
+#define REG_RL				0x042A
+#define REG_DARFRC			0x0430
+#define REG_RARFRC			0x0438
+#define REG_RRSR			0x0440
+#define REG_ARFR0			0x0444
+#define REG_ARFR1			0x0448
+#define REG_ARFR2			0x044C
+#define REG_ARFR3			0x0450
+#define REG_AGGLEN_LMT			0x0458
+#define REG_AMPDU_MIN_SPACE		0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD	0x045D
+#define REG_FAST_EDCA_CTRL		0x0460
+#define REG_RD_RESP_PKT_TH		0x0463
+#define REG_INIRTS_RATE_SEL		0x0480
+/* define REG_INIDATA_RATE_SEL		0x0484 */
+#define REG_POWER_STATUS		0x04A4
+#define REG_POWER_STAGE1		0x04B4
+#define REG_POWER_STAGE2		0x04B8
+#define REG_PKT_VO_VI_LIFE_TIME		0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME		0x04C2
+#define REG_STBC_SETTING		0x04C4
+#define REG_PROT_MODE_CTRL		0x04C8
+#define REG_MAX_AGGR_NUM		0x04CA
+#define REG_RTS_MAX_AGGR_NUM		0x04CB
+#define REG_BAR_MODE_CTRL		0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
+#define REG_EARLY_MODE_CONTROL		0x4D0
+#define REG_NQOS_SEQ			0x04DC
+#define REG_QOS_SEQ			0x04DE
+#define REG_NEED_CPU_HANDLE		0x04E0
+#define REG_PKT_LOSE_RPT		0x04E1
+#define REG_PTCL_ERR_STATUS		0x04E2
+#define REG_TX_RPT_CTRL			0x04EC
+#define REG_TX_RPT_TIME			0x04F0	/*  2 byte */
+#define REG_DUMMY			0x04FC
+
+/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+#define REG_EDCA_VO_PARAM		0x0500
+#define REG_EDCA_VI_PARAM		0x0504
+#define REG_EDCA_BE_PARAM		0x0508
+#define REG_EDCA_BK_PARAM		0x050C
+#define REG_BCNTCFG			0x0510
+#define REG_PIFS			0x0512
+#define REG_RDG_PIFS			0x0513
+#define REG_SIFS_CTX			0x0514
+#define REG_SIFS_TRX			0x0516
+#define REG_TSFTR_SYN_OFFSET		0x0518
+#define REG_AGGR_BREAK_TIME		0x051A
+#define REG_SLOT			0x051B
+#define REG_TX_PTCL_CTRL		0x0520
+#define REG_TXPAUSE			0x0522
+#define REG_DIS_TXREQ_CLR		0x0523
+#define REG_RD_CTRL			0x0524
+/*  Format for offset 540h-542h: */
+/* 	[3:0]:   TBTT prohibit setup in unit of 32us. The time for HW getting
+ *		 beacon content before TBTT. */
+/* 	[7:4]:   Reserved. */
+/* 	[19:8]:  TBTT prohibit hold in unit of 32us. The time for HW holding
+ *		 to send the beacon packet. */
+/* 	[23:20]: Reserved */
+/*  Description: */
+/* 	              | */
+/*      |<--Setup--|--Hold------------>| */
+/* 	--------------|---------------------- */
+/*                 | */
+/*                TBTT */
+/*  Note: We cannot update beacon content to HW or send any AC packets during
+ *	  the time between Setup and Hold. */
+#define REG_TBTT_PROHIBIT		0x0540
+#define REG_RD_NAV_NXT			0x0544
+#define REG_NAV_PROT_LEN		0x0546
+#define REG_BCN_CTRL			0x0550
+#define REG_BCN_CTRL_1			0x0551
+#define REG_MBID_NUM			0x0552
+#define REG_DUAL_TSF_RST		0x0553
+#define REG_BCN_INTERVAL		0x0554
+#define REG_DRVERLYINT			0x0558
+#define REG_BCNDMATIM			0x0559
+#define REG_ATIMWND			0x055A
+#define REG_BCN_MAX_ERR			0x055D
+#define REG_RXTSF_OFFSET_CCK		0x055E
+#define REG_RXTSF_OFFSET_OFDM		0x055F
+#define REG_TSFTR			0x0560
+#define REG_TSFTR1			0x0568
+#define REG_ATIMWND_1			0x0570
+#define REG_PSTIMER			0x0580
+#define REG_TIMER0			0x0584
+#define REG_TIMER1			0x0588
+#define REG_ACMHWCTRL			0x05C0
+
+/* define REG_FW_TSF_SYNC_CNT		0x04A0 */
+#define REG_FW_RESET_TSF_CNT_1		0x05FC
+#define REG_FW_RESET_TSF_CNT_0		0x05FD
+#define REG_FW_BCN_DIS_CNT		0x05FE
+
+/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+#define REG_APSD_CTRL			0x0600
+#define REG_BWOPMODE			0x0603
+#define REG_TCR				0x0604
+#define REG_RCR				0x0608
+#define REG_RX_PKT_LIMIT		0x060C
+#define REG_RX_DLK_TIME			0x060D
+#define REG_RX_DRVINFO_SZ		0x060F
+
+#define REG_MACID			0x0610
+#define REG_BSSID			0x0618
+#define REG_MAR				0x0620
+#define REG_MBIDCAMCFG			0x0628
+
+#define REG_USTIME_EDCA			0x0638
+#define REG_MAC_SPEC_SIFS		0x063A
+
+/*  20100719 Joseph: Hardware register definition change. (HW datasheet v54) */
+/*  [15:8]SIFS_R2T_OFDM, [7:0]SIFS_R2T_CCK */
+#define REG_R2T_SIFS			0x063C
+/*  [15:8]SIFS_T2T_OFDM, [7:0]SIFS_T2T_CCK */
+#define REG_T2T_SIFS			0x063E
+#define REG_ACKTO			0x0640
+#define REG_CTS2TO			0x0641
+#define REG_EIFS			0x0642
+
+/* RXERR_RPT */
+#define RXERR_TYPE_OFDM_PPDU		0
+#define RXERR_TYPE_OFDM_false_ALARM	1
+#define RXERR_TYPE_OFDM_MPDU_OK		2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
+#define RXERR_TYPE_CCK_PPDU		4
+#define RXERR_TYPE_CCK_false_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL	7
+#define RXERR_TYPE_HT_PPDU		8
+#define RXERR_TYPE_HT_false_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL	10
+#define RXERR_TYPE_HT_MPDU_OK		11
+#define RXERR_TYPE_HT_MPDU_FAIL		12
+#define RXERR_TYPE_RX_FULL_DROP		15
+
+#define RXERR_COUNTER_MASK		0xFFFFF
+#define RXERR_RPT_RST			BIT(27)
+#define _RXERR_RPT_SEL(type)		((type) << 28)
+
+/*  Note: */
+/* 	The NAV upper value is very important to WiFi 11n 5.2.3 NAV test.
+ *	The default value is always too small, but the WiFi TestPlan test
+ *	by 25,000 microseconds of NAV through sending CTS in the air.
+ *	We must update this value greater than 25,000 microseconds to pass
+ *	the item. The offset of NAV_UPPER in 8192C Spec is incorrect, and
+ *	the offset should be 0x0652. */
+#define REG_NAV_UPPER			0x0652	/*  unit of 128 */
+
+/* WMA, BA, CCX */
+/* define REG_NAV_CTRL			0x0650 */
+#define REG_BACAMCMD			0x0654
+#define REG_BACAMCONTENT		0x0658
+#define REG_LBDLY			0x0660
+#define REG_FWDLY			0x0661
+#define REG_RXERR_RPT			0x0664
+#define REG_WMAC_TRXPTCL_CTL		0x0668
+
+/*  Security */
+#define REG_CAMCMD			0x0670
+#define REG_CAMWRITE			0x0674
+#define REG_CAMREAD			0x0678
+#define REG_CAMDBG			0x067C
+#define REG_SECCFG			0x0680
+
+/*  Power */
+#define REG_WOW_CTRL			0x0690
+#define REG_PS_RX_INFO			0x0692
+#define REG_UAPSD_TID			0x0693
+#define REG_WKFMCAM_CMD			0x0698
+#define REG_WKFMCAM_NUM_88E		0x698
+#define REG_RXFLTMAP0			0x06A0
+#define REG_RXFLTMAP1			0x06A2
+#define REG_RXFLTMAP2			0x06A4
+#define REG_BCN_PSR_RPT			0x06A8
+#define REG_BT_COEX_TABLE		0x06C0
+
+/*  Hardware Port 2 */
+#define REG_MACID1			0x0700
+#define REG_BSSID1			0x0708
+
+/* 	0xFE00h ~ 0xFE55h	USB Configuration */
+#define REG_USB_INFO			0xFE17
+#define REG_USB_SPECIAL_OPTION		0xFE55
+#define REG_USB_DMA_AGG_TO		0xFE5B
+#define REG_USB_AGG_TO			0xFE5C
+#define REG_USB_AGG_TH			0xFE5D
+
+/*  For normal chip */
+#define REG_NORMAL_SIE_VID		0xFE60		/*  0xFE60~0xFE61 */
+#define REG_NORMAL_SIE_PID		0xFE62		/*  0xFE62~0xFE63 */
+#define REG_NORMAL_SIE_OPTIONAL		0xFE64
+#define REG_NORMAL_SIE_EP		0xFE65		/*  0xFE65~0xFE67 */
+#define REG_NORMAL_SIE_PHY		0xFE68		/*  0xFE68~0xFE6B */
+#define REG_NORMAL_SIE_OPTIONAL2	0xFE6C
+#define REG_NORMAL_SIE_GPS_EP		0xFE6D	/*  0xFE6D, for RTL8723 only. */
+#define REG_NORMAL_SIE_MAC_ADDR		0xFE70		/*  0xFE70~0xFE75 */
+#define REG_NORMAL_SIE_STRING		0xFE80		/*  0xFE80~0xFEDF */
+
+/*  TODO: use these definition when using REG_xxx naming rule. */
+/*  NOTE: DO NOT Remove these definition. Use later. */
+
+#define	EFUSE_CTRL			REG_EFUSE_CTRL	/*  E-Fuse Control. */
+#define	EFUSE_TEST			REG_EFUSE_TEST	/*  E-Fuse Test. */
+#define	MSR				(REG_CR + 2)	/*  Media Status reg */
+#define	ISR				REG_HISR_88E
+/*  Timing Sync Function Timer Register. */
+#define	TSFR				REG_TSFTR
+
+#define		PBP			REG_PBP
+
+/*  Redifine MACID register, to compatible prior ICs. */
+/*  MAC ID Register, Offset 0x0050-0x0053 */
+#define	IDR0				REG_MACID
+/*  MAC ID Register, Offset 0x0054-0x0055 */
+#define	IDR4				(REG_MACID + 4)
+
+/*  9. Security Control Registers	(Offset: ) */
+/* IN 8190 Data Sheet is called CAMcmd */
+#define	RWCAM				REG_CAMCMD
+/*  Software write CAM input content */
+#define	WCAMI				REG_CAMWRITE
+/*  Software read/write CAM config */
+#define	RCAMO				REG_CAMREAD
+#define	CAMDBG				REG_CAMDBG
+/* Security Configuration Register */
+#define	SECR				REG_SECCFG
+
+/*  Unused register */
+#define	UnusedRegister			0x1BF
+#define	DCAM				UnusedRegister
+#define	PSR				UnusedRegister
+#define	BBAddr				UnusedRegister
+#define	PhyDataR			UnusedRegister
+
+/*  Min Spacing related settings. */
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
+
+/*        8192C GPIO MUX Configuration Register (offset 0x40, 4 byte) */
+#define	GPIOSEL_GPIO			0
+#define	GPIOSEL_ENBT			BIT(5)
+
+/*        8192C GPIO PIN Control Register (offset 0x44, 4 byte) */
+/*  GPIO pins input value */
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+/*  GPIO pins output value */
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
+/*  GPIO pins output enable when a bit is set to "1"; otherwise,
+ *  input is configured. */
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
+
+/* 8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+#define	HSIMR_GPIO12_0_INT_EN		BIT(0)
+#define	HSIMR_SPS_OCP_INT_EN		BIT(5)
+#define	HSIMR_RON_INT_EN		BIT(6)
+#define	HSIMR_PDN_INT_EN		BIT(7)
+#define	HSIMR_GPIO9_INT_EN		BIT(25)
+
+/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+#define	HSISR_GPIO12_0_INT		BIT(0)
+#define	HSISR_SPS_OCP_INT		BIT(5)
+#define	HSISR_RON_INT_EN		BIT(6)
+#define	HSISR_PDNINT			BIT(7)
+#define	HSISR_GPIO9_INT			BIT(25)
+
+/*   8192C (MSR) Media Status Register	(Offset 0x4C, 8 bits) */
+/*
+Network Type
+00: No link
+01: Link in ad hoc network
+10: Link in infrastructure network
+11: AP mode
+Default: 00b.
+*/
+#define	MSR_NOLINK			0x00
+#define	MSR_ADHOC			0x01
+#define	MSR_INFRA			0x02
+#define	MSR_AP				0x03
+
+/*  88E Driver Initialization Offload REG_FDHM0(Offset 0x88, 8 bits) */
+/* IOL config for REG_FDHM0(Reg0x88) */
+#define CMD_INIT_LLT			BIT(0)
+#define CMD_READ_EFUSE_MAP		BIT(1)
+#define CMD_EFUSE_PATCH			BIT(2)
+#define CMD_IOCONFIG			BIT(3)
+#define CMD_INIT_LLT_ERR		BIT(4)
+#define CMD_READ_EFUSE_MAP_ERR		BIT(5)
+#define CMD_EFUSE_PATCH_ERR		BIT(6)
+#define CMD_IOCONFIG_ERR		BIT(7)
+
+/*  6. Adaptive Control Registers  (Offset: 0x0160 - 0x01CF) */
+/*  8192C Response Rate Set Register	(offset 0x181, 24bits) */
+#define	RRSR_1M				BIT(0)
+#define	RRSR_2M				BIT(1)
+#define	RRSR_5_5M			BIT(2)
+#define	RRSR_11M			BIT(3)
+#define	RRSR_6M				BIT(4)
+#define	RRSR_9M				BIT(5)
+#define	RRSR_12M			BIT(6)
+#define	RRSR_18M			BIT(7)
+#define	RRSR_24M			BIT(8)
+#define	RRSR_36M			BIT(9)
+#define	RRSR_48M			BIT(10)
+#define	RRSR_54M			BIT(11)
+#define	RRSR_MCS0			BIT(12)
+#define	RRSR_MCS1			BIT(13)
+#define	RRSR_MCS2			BIT(14)
+#define	RRSR_MCS3			BIT(15)
+#define	RRSR_MCS4			BIT(16)
+#define	RRSR_MCS5			BIT(17)
+#define	RRSR_MCS6			BIT(18)
+#define	RRSR_MCS7			BIT(19)
+
+/*  8192C Response Rate Set Register	(offset 0x1BF, 8bits) */
+/*  WOL bit information */
+#define	HAL92C_WOL_PTK_UPDATE_EVENT	BIT(0)
+#define	HAL92C_WOL_GTK_UPDATE_EVENT	BIT(1)
+
+/*        8192C BW_OPMODE bits		(Offset 0x203, 8bit) */
+#define	BW_OPMODE_20MHZ			BIT(2)
+
+/*        8192C CAM Config Setting (offset 0x250, 1 byte) */
+#define	CAM_VALID			BIT(15)
+#define	CAM_NOTVALID			0x0000
+#define	CAM_USEDK			BIT(5)
+
+#define	CAM_CONTENT_COUNT		8
+
+#define	CAM_NONE			0x0
+#define	CAM_WEP40			0x01
+#define	CAM_TKIP			0x02
+#define	CAM_AES				0x04
+#define	CAM_WEP104			0x05
+#define	CAM_SMS4			0x6
+
+#define	TOTAL_CAM_ENTRY			32
+#define	HALF_CAM_ENTRY			16
+
+#define	CAM_CONFIG_USEDK		true
+#define	CAM_CONFIG_NO_USEDK		false
+
+#define	CAM_WRITE			BIT(16)
+#define	CAM_READ			0x00000000
+#define	CAM_POLLINIG			BIT(31)
+
+#define	SCR_UseDK			0x01
+#define	SCR_TxSecEnable			0x02
+#define	SCR_RxSecEnable			0x04
+
+/*  10. Power Save Control Registers	 (Offset: 0x0260 - 0x02DF) */
+#define	WOW_PMEN			BIT(0) /*  Power management Enable. */
+#define	WOW_WOMEN			BIT(1) /*  WoW function on or off. */
+#define	WOW_MAGIC			BIT(2) /*  Magic packet */
+#define	WOW_UWF				BIT(3) /*  Unicast Wakeup frame. */
+
+/*  12. Host Interrupt Status Registers	 (Offset: 0x0300 - 0x030F) */
+/*        8188 IMR/ISR bits */
+#define	IMR_DISABLED_88E		0x0
+/*  IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TXCCK_88E			BIT(30)	/*  TXRPT interrupt when CCX bit of the packet is set */
+#define	IMR_PSTIMEOUT_88E		BIT(29)	/*  Power Save Time Out Interrupt */
+#define	IMR_GTINT4_88E			BIT(28)	/*  When GTIMER4 expires, this bit is set to 1 */
+#define	IMR_GTINT3_88E			BIT(27)	/*  When GTIMER3 expires, this bit is set to 1 */
+#define	IMR_TBDER_88E			BIT(26)	/*  Transmit Beacon0 Error */
+#define	IMR_TBDOK_88E			BIT(25)	/*  Transmit Beacon0 OK */
+#define	IMR_TSF_BIT32_TOGGLE_88E	BIT(24)	/*  TSF Timer BIT32 toggle indication interrupt */
+#define	IMR_BCNDMAINT0_88E		BIT(20)	/*  Beacon DMA Interrupt 0 */
+#define	IMR_BCNDERR0_88E		BIT(16)	/*  Beacon Queue DMA Error 0 */
+#define	IMR_HSISR_IND_ON_INT_88E	BIT(15)	/*  HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define	IMR_BCNDMAINT_E_88E		BIT(14)	/*  Beacon DMA Interrupt Extension for Win7 */
+#define	IMR_ATIMEND_88E			BIT(12)	/*  CTWidnow End or ATIM Window End */
+#define	IMR_HISR1_IND_INT_88E		BIT(11)	/*  HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1) */
+#define	IMR_C2HCMD_88E			BIT(10)	/*  CPU to Host Command INT Status, Write 1 clear */
+#define	IMR_CPWM2_88E			BIT(9)	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_CPWM_88E			BIT(8)	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_HIGHDOK_88E			BIT(7)	/*  High Queue DMA OK */
+#define	IMR_MGNTDOK_88E			BIT(6)	/*  Management Queue DMA OK */
+#define	IMR_BKDOK_88E			BIT(5)	/*  AC_BK DMA OK */
+#define	IMR_BEDOK_88E			BIT(4)	/*  AC_BE DMA OK */
+#define	IMR_VIDOK_88E			BIT(3)	/*  AC_VI DMA OK */
+#define	IMR_VODOK_88E			BIT(2)	/*  AC_VO DMA OK */
+#define	IMR_RDU_88E			BIT(1)	/*  Rx Descriptor Unavailable */
+#define	IMR_ROK_88E			BIT(0)	/*  Receive DMA OK */
+
+/*  IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7_88E		BIT(27)	/*  Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6_88E		BIT(26)	/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5_88E		BIT(25)	/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4_88E		BIT(24)	/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3_88E		BIT(23)	/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2_88E		BIT(22)	/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1_88E		BIT(21)	/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDERR7_88E		BIT(20)	/*  Beacon DMA Error Int 7 */
+#define	IMR_BCNDERR6_88E		BIT(19)	/*  Beacon DMA Error Int 6 */
+#define	IMR_BCNDERR5_88E		BIT(18)	/*  Beacon DMA Error Int 5 */
+#define	IMR_BCNDERR4_88E		BIT(17)	/*  Beacon DMA Error Int 4 */
+#define	IMR_BCNDERR3_88E		BIT(16)	/*  Beacon DMA Error Int 3 */
+#define	IMR_BCNDERR2_88E		BIT(15)	/*  Beacon DMA Error Int 2 */
+#define	IMR_BCNDERR1_88E		BIT(14)	/*  Beacon DMA Error Int 1 */
+#define	IMR_ATIMEND_E_88E		BIT(13)	/*  ATIM Window End Ext for Win7 */
+#define	IMR_TXERR_88E			BIT(11)	/*  Tx Err Flag Int Status, write 1 clear. */
+#define	IMR_RXERR_88E			BIT(10)	/*  Rx Err Flag INT Status, Write 1 clear */
+#define	IMR_TXFOVW_88E			BIT(9)	/*  Transmit FIFO Overflow */
+#define	IMR_RXFOVW_88E			BIT(8)	/*  Receive FIFO Overflow */
+
+#define	HAL_NIC_UNPLUG_ISR		0xFFFFFFFF	/*  The value when the NIC is unplugged for PCI. */
+
+/*  8192C EFUSE */
+#define		HWSET_MAX_SIZE			256
+#define		HWSET_MAX_SIZE_88E		512
+
+/*===================================================================
+=====================================================================
+Here the register defines are for 92C. When the define is as same with 92C,
+we will use the 92C's define for the consistency
+So the following defines for 92C is not entire!!!!!!
+=====================================================================
+=====================================================================*/
+/*
+Based on Datasheet V33---090401
+Register Summary
+Current IOREG MAP
+0x0000h ~ 0x00FFh   System Configuration (256 Bytes)
+0x0100h ~ 0x01FFh   MACTOP General Configuration (256 Bytes)
+0x0200h ~ 0x027Fh   TXDMA Configuration (128 Bytes)
+0x0280h ~ 0x02FFh   RXDMA Configuration (128 Bytes)
+0x0300h ~ 0x03FFh   PCIE EMAC Reserved Region (256 Bytes)
+0x0400h ~ 0x04FFh   Protocol Configuration (256 Bytes)
+0x0500h ~ 0x05FFh   EDCA Configuration (256 Bytes)
+0x0600h ~ 0x07FFh   WMAC Configuration (512 Bytes)
+0x2000h ~ 0x3FFFh   8051 FW Download Region (8196 Bytes)
+*/
+/* 		 8192C (TXPAUSE) transmission pause (Offset 0x522, 8 bits) */
+/*  Note: */
+/* 	The bits of stopping AC(VO/VI/BE/BK) queue in datasheet
+ *	RTL8192S/RTL8192C are wrong, */
+/* 	the correct arragement is VO - Bit0, VI - Bit1, BE - Bit2,
+ *	and BK - Bit3. */
+/* 	8723 and 88E may be not correct either in the earlier version. */
+#define		StopBecon			BIT(6)
+#define		StopHigh			BIT(5)
+#define		StopMgt				BIT(4)
+#define		StopBK				BIT(3)
+#define		StopBE				BIT(2)
+#define		StopVI				BIT(1)
+#define		StopVO				BIT(0)
+
+/*        8192C (RCR) Receive Configuration Register(Offset 0x608, 32 bits) */
+#define	RCR_APPFCS		BIT(31)	/* WMAC append FCS after payload */
+#define	RCR_APP_MIC		BIT(30)
+#define	RCR_APP_PHYSTS		BIT(28)
+#define	RCR_APP_ICV		BIT(29)
+#define	RCR_APP_PHYST_RXFF	BIT(28)
+#define	RCR_APP_BA_SSN		BIT(27)	/* Accept BA SSN */
+#define	RCR_ENMBID		BIT(24)	/* Enable Multiple BssId. */
+#define	RCR_LSIGEN		BIT(23)
+#define	RCR_MFBEN		BIT(22)
+#define	RCR_HTC_LOC_CTRL	BIT(14)   /* MFC<--HTC=1 MFC-->HTC=0 */
+#define	RCR_AMF			BIT(13)	/* Accept management type frame */
+#define	RCR_ACF			BIT(12)	/* Accept control type frame */
+#define	RCR_ADF			BIT(11)	/* Accept data type frame */
+#define	RCR_AICV		BIT(9)	/* Accept ICV error packet */
+#define	RCR_ACRC32		BIT(8)	/* Accept CRC32 error packet */
+#define	RCR_CBSSID_BCN		BIT(7)	/* Accept BSSID match packet
+					 * (Rx beacon, probe rsp) */
+#define	RCR_CBSSID_DATA		BIT(6)	/* Accept BSSID match (Data)*/
+#define	RCR_CBSSID		RCR_CBSSID_DATA	/* Accept BSSID match */
+#define	RCR_APWRMGT		BIT(5)	/* Accept power management pkt*/
+#define	RCR_ADD3		BIT(4)	/* Accept address 3 match pkt */
+#define	RCR_AB			BIT(3)	/* Accept broadcast packet */
+#define	RCR_AM			BIT(2)	/* Accept multicast packet */
+#define	RCR_APM			BIT(1)	/* Accept physical match pkt */
+#define	RCR_AAP			BIT(0)	/* Accept all unicast packet */
+#define	RCR_MXDMA_OFFSET	8
+#define	RCR_FIFO_OFFSET		13
+
+/* 	0xFE00h ~ 0xFE55h	USB Configuration */
+#define REG_USB_INFO			0xFE17
+#define REG_USB_SPECIAL_OPTION		0xFE55
+#define REG_USB_DMA_AGG_TO		0xFE5B
+#define REG_USB_AGG_TO			0xFE5C
+#define REG_USB_AGG_TH			0xFE5D
+
+#define REG_USB_HRPWM			0xFE58
+#define REG_USB_HCPWM			0xFE57
+/*        8192C Regsiter Bit and Content definition */
+/* 	0x0000h ~ 0x00FFh	System Configuration */
+
+/* 2 SYS_ISO_CTRL */
+#define ISO_MD2PP			BIT(0)
+#define ISO_UA2USB			BIT(1)
+#define ISO_UD2CORE			BIT(2)
+#define ISO_PA2PCIE			BIT(3)
+#define ISO_PD2CORE			BIT(4)
+#define ISO_IP2MAC			BIT(5)
+#define ISO_DIOP			BIT(6)
+#define ISO_DIOE			BIT(7)
+#define ISO_EB2CORE			BIT(8)
+#define ISO_DIOR			BIT(9)
+#define PWC_EV12V			BIT(15)
+
+/* 2 SYS_FUNC_EN */
+#define FEN_BBRSTB			BIT(0)
+#define FEN_BB_GLB_RSTn			BIT(1)
+#define FEN_USBA			BIT(2)
+#define FEN_UPLL			BIT(3)
+#define FEN_USBD			BIT(4)
+#define FEN_DIO_PCIE			BIT(5)
+#define FEN_PCIEA			BIT(6)
+#define FEN_PPLL			BIT(7)
+#define FEN_PCIED			BIT(8)
+#define FEN_DIOE			BIT(9)
+#define FEN_CPUEN			BIT(10)
+#define FEN_DCORE			BIT(11)
+#define FEN_ELDR			BIT(12)
+#define FEN_DIO_RF			BIT(13)
+#define FEN_HWPDN			BIT(14)
+#define FEN_MREGEN			BIT(15)
+
+/* 2 APS_FSMCO */
+#define PFM_LDALL			BIT(0)
+#define PFM_ALDN			BIT(1)
+#define PFM_LDKP			BIT(2)
+#define PFM_WOWL			BIT(3)
+#define EnPDN				BIT(4)
+#define PDN_PL				BIT(5)
+#define APFM_ONMAC			BIT(8)
+#define APFM_OFF			BIT(9)
+#define APFM_RSM			BIT(10)
+#define AFSM_HSUS			BIT(11)
+#define AFSM_PCIE			BIT(12)
+#define APDM_MAC			BIT(13)
+#define APDM_HOST			BIT(14)
+#define APDM_HPDN			BIT(15)
+#define RDY_MACON			BIT(16)
+#define SUS_HOST			BIT(17)
+#define ROP_ALD				BIT(20)
+#define ROP_PWR				BIT(21)
+#define ROP_SPS				BIT(22)
+#define SOP_MRST			BIT(25)
+#define SOP_FUSE			BIT(26)
+#define SOP_ABG				BIT(27)
+#define SOP_AMB				BIT(28)
+#define SOP_RCK				BIT(29)
+#define SOP_A8M				BIT(30)
+#define XOP_BTCK			BIT(31)
+
+/* 2 SYS_CLKR */
+#define ANAD16V_EN			BIT(0)
+#define ANA8M				BIT(1)
+#define MACSLP				BIT(4)
+#define LOADER_CLK_EN			BIT(5)
+
+/* 2 9346CR */
+
+#define		BOOT_FROM_EEPROM	BIT(4)
+#define		EEPROM_EN		BIT(5)
+
+/* 2 SPS0_CTRL */
+
+/* 2 SPS_OCP_CFG */
+
+/* 2 RF_CTRL */
+#define RF_EN				BIT(0)
+#define RF_RSTB				BIT(1)
+#define RF_SDMRSTB			BIT(2)
+
+/* 2 LDOV12D_CTRL */
+#define LDV12_EN			BIT(0)
+#define LDV12_SDBY			BIT(1)
+#define LPLDO_HSM			BIT(2)
+#define LPLDO_LSM_DIS			BIT(3)
+#define _LDV12_VADJ(x)			(((x) & 0xF) << 4)
+
+/* 2EFUSE_CTRL */
+#define ALD_EN				BIT(18)
+#define EF_PD				BIT(19)
+#define EF_FLAG				BIT(31)
+
+/* 2 EFUSE_TEST (For RTL8723 partially) */
+#define EF_TRPT				BIT(7)
+/*  00: Wifi Efuse, 01: BT Efuse0, 10: BT Efuse1, 11: BT Efuse2 */
+#define EF_CELL_SEL			(BIT(8)|BIT(9))
+#define LDOE25_EN			BIT(31)
+#define EFUSE_SEL(x)			(((x) & 0x3) << 8)
+#define EFUSE_SEL_MASK			0x300
+#define EFUSE_WIFI_SEL_0		0x0
+#define EFUSE_BT_SEL_0			0x1
+#define EFUSE_BT_SEL_1			0x2
+#define EFUSE_BT_SEL_2			0x3
+
+#define EFUSE_ACCESS_ON			0x69	/*  For RTL8723 only. */
+#define EFUSE_ACCESS_OFF		0x00	/*  For RTL8723 only. */
+
+/* 2 8051FWDL */
+/* 2 MCUFWDL */
+#define MCUFWDL_EN			BIT(0)
+#define MCUFWDL_RDY			BIT(1)
+#define FWDL_CHKSUM_RPT			BIT(2)
+#define MACINI_RDY			BIT(3)
+#define BBINI_RDY			BIT(4)
+#define RFINI_RDY			BIT(5)
+#define WINTINI_RDY			BIT(6)
+#define RAM_DL_SEL			BIT(7) /*  1:RAM, 0:ROM */
+#define ROM_DLEN			BIT(19)
+#define CPRST				BIT(23)
+
+/* 2 REG_SYS_CFG */
+#define XCLK_VLD			BIT(0)
+#define ACLK_VLD			BIT(1)
+#define UCLK_VLD			BIT(2)
+#define PCLK_VLD			BIT(3)
+#define PCIRSTB				BIT(4)
+#define V15_VLD				BIT(5)
+#define SW_OFFLOAD_EN			BIT(7)
+#define SIC_IDLE			BIT(8)
+#define BD_MAC2				BIT(9)
+#define BD_MAC1				BIT(10)
+#define IC_MACPHY_MODE			BIT(11)
+#define CHIP_VER			(BIT(12)|BIT(13)|BIT(14)|BIT(15))
+#define BT_FUNC				BIT(16)
+#define VENDOR_ID			BIT(19)
+#define PAD_HWPD_IDN			BIT(22)
+#define TRP_VAUX_EN			BIT(23)	/*  RTL ID */
+#define TRP_BT_EN			BIT(24)
+#define BD_PKG_SEL			BIT(25)
+#define BD_HCI_SEL			BIT(26)
+#define TYPE_ID				BIT(27)
+
+#define CHIP_VER_RTL_MASK		0xF000	/* Bit 12 ~ 15 */
+#define CHIP_VER_RTL_SHIFT		12
+
+/* 2REG_GPIO_OUTSTS (For RTL8723 only) */
+#define	EFS_HCI_SEL			(BIT(0)|BIT(1))
+#define	PAD_HCI_SEL			(BIT(2)|BIT(3))
+#define	HCI_SEL				(BIT(4)|BIT(5))
+#define	PKG_SEL_HCI			BIT(6)
+#define	FEN_GPS				BIT(7)
+#define	FEN_BT				BIT(8)
+#define	FEN_WL				BIT(9)
+#define	FEN_PCI				BIT(10)
+#define	FEN_USB				BIT(11)
+#define	BTRF_HWPDN_N			BIT(12)
+#define	WLRF_HWPDN_N			BIT(13)
+#define	PDN_BT_N			BIT(14)
+#define	PDN_GPS_N			BIT(15)
+#define	BT_CTL_HWPDN			BIT(16)
+#define	GPS_CTL_HWPDN			BIT(17)
+#define	PPHY_SUSB			BIT(20)
+#define	UPHY_SUSB			BIT(21)
+#define	PCI_SUSEN			BIT(22)
+#define	USB_SUSEN			BIT(23)
+#define	RF_RL_ID			(BIT(31)|BIT(30)|BIT(29)|BIT(28))
+
+/* 2SYS_CFG */
+#define RTL_ID				BIT(23)	/*  TestChip ID, 1:Test(RLE); 0:MP(RL) */
+
+/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+
+/* 2 Function Enable Registers */
+/* 2 CR */
+
+#define HCI_TXDMA_EN			BIT(0)
+#define HCI_RXDMA_EN			BIT(1)
+#define TXDMA_EN			BIT(2)
+#define RXDMA_EN			BIT(3)
+#define PROTOCOL_EN			BIT(4)
+#define SCHEDULE_EN			BIT(5)
+#define MACTXEN				BIT(6)
+#define MACRXEN				BIT(7)
+#define ENSWBCN				BIT(8)
+#define ENSEC				BIT(9)
+#define CALTMR_EN			BIT(10)	/*  32k CAL TMR enable */
+
+/*  Network type */
+#define _NETTYPE(x)			(((x) & 0x3) << 16)
+#define MASK_NETTYPE			0x30000
+#define NT_NO_LINK			0x0
+#define NT_LINK_AD_HOC			0x1
+#define NT_LINK_AP			0x2
+#define NT_AS_AP			0x3
+
+/* 2 PBP - Page Size Register */
+#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
+#define _PSRX_MASK			0xF
+#define _PSTX_MASK			0xF0
+#define _PSRX(x)			(x)
+#define _PSTX(x)			((x) << 4)
+
+#define PBP_128				0x1
+
+/* 2 TX/RXDMA */
+#define RXDMA_ARBBW_EN			BIT(0)
+#define RXSHFT_EN			BIT(1)
+#define RXDMA_AGG_EN			BIT(2)
+#define QS_VO_QUEUE			BIT(8)
+#define QS_VI_QUEUE			BIT(9)
+#define QS_BE_QUEUE			BIT(10)
+#define QS_BK_QUEUE			BIT(11)
+#define QS_MANAGER_QUEUE		BIT(12)
+#define QS_HIGH_QUEUE			BIT(13)
+
+#define HQSEL_VOQ			BIT(0)
+#define HQSEL_VIQ			BIT(1)
+#define HQSEL_BEQ			BIT(2)
+#define HQSEL_BKQ			BIT(3)
+#define HQSEL_MGTQ			BIT(4)
+#define HQSEL_HIQ			BIT(5)
+
+/*  For normal driver, 0x10C */
+#define _TXDMA_HIQ_MAP(x)		(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x)		(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x)		(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x)		(((x)&0x3) << 8 )
+#define _TXDMA_VIQ_MAP(x)		(((x)&0x3) << 6 )
+#define _TXDMA_VOQ_MAP(x)		(((x)&0x3) << 4 )
+
+#define QUEUE_LOW			1
+#define QUEUE_NORMAL			2
+#define QUEUE_HIGH			3
+
+/* 2 TRXFF_BNDY */
+
+/* 2 LLT_INIT */
+#define _LLT_NO_ACTIVE			0x0
+#define _LLT_WRITE_ACCESS		0x1
+#define _LLT_READ_ACCESS		0x2
+
+#define _LLT_INIT_DATA(x)		((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)		(((x) & 0xFF) << 8)
+#define _LLT_OP(x)			(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)		(((x) >> 30) & 0x3)
+
+/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+
+#define NUM_HQ 0x29
+
+#define LD_RQPN				BIT(31)
+
+/* 2TDECTRL */
+#define BCN_VALID			BIT(16)
+#define BCN_HEAD(x)			(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK			0xFF00
+
+/* 2 TDECTL */
+#define BLK_DESC_NUM_SHIFT		4
+#define BLK_DESC_NUM_MASK		0xF
+
+/* 2 TXDMA_OFFSET_CHK */
+#define DROP_DATA_EN			BIT(9)
+
+/* 	0x0280h ~ 0x028Bh	RX DMA Configuration */
+
+/*     REG_RXDMA_CONTROL, 0x0286h */
+
+/* 2 REG_RXPKT_NUM, 0x0284 */
+#define		RXPKT_RELEASE_POLL	BIT(16)
+#define	RXDMA_IDLE			BIT(17)
+#define	RW_RELEASE_EN			BIT(18)
+
+/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/* 2 FWHW_TXQ_CTRL */
+#define EN_AMPDU_RTY_NEW		BIT(7)
+
+/* 2 SPEC SIFS */
+#define _SPEC_SIFS_CCK(x)		((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)		(((x) & 0xFF) << 8)
+
+/* 2 RL */
+#define	RETRY_LIMIT_SHORT_SHIFT		8
+#define	RETRY_LIMIT_LONG_SHIFT		0
+
+/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+
+/* 2 EDCA setting */
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
+
+#define _LRL(x)			((x) & 0x3F)
+#define _SRL(x)			(((x) & 0x3F) << 8)
+
+/* 2 BCN_CTRL */
+#define EN_MBSSID		BIT(1)
+#define EN_TXBCN_RPT		BIT(2)
+#define EN_BCN_FUNCTION		BIT(3)
+#define DIS_TSF_UPDATE		BIT(3)
+
+/*  The same function but different bit field. */
+#define DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define DIS_TSF_UDT0_TEST_CHIP	BIT(5)
+#define STOP_BCNQ		BIT(6)
+
+/* 2 ACMHWCTRL */
+#define ACMHW_BEQEN		BIT(1)
+#define ACMHW_VIQEN		BIT(2)
+#define ACMHW_VOQEN		BIT(3)
+
+/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+/* 2APSD_CTRL */
+#define APSDOFF			BIT(6)
+#define APSDOFF_STATUS		BIT(7)
+
+#define RATE_BITMAP_ALL		0xFFFFF
+
+/*  Only use CCK 1M rate for ACK */
+#define RATE_RRSR_CCK_ONLY_1M	0xFFFF1
+
+/* 2 TCR */
+#define TSFRST			BIT(0)
+#define DIS_GCLK		BIT(1)
+#define PAD_SEL			BIT(2)
+#define PWR_ST			BIT(6)
+#define PWRBIT_OW_EN		BIT(7)
+#define ACRC			BIT(8)
+#define CFENDFORM		BIT(9)
+#define ICV			BIT(10)
+
+/* 2 RCR */
+#define AAP			BIT(0)
+#define APM			BIT(1)
+#define AM			BIT(2)
+#define AB			BIT(3)
+#define ADD3			BIT(4)
+#define APWRMGT			BIT(5)
+#define CBSSID			BIT(6)
+#define CBSSID_DATA		BIT(6)
+#define CBSSID_BCN		BIT(7)
+#define ACRC32			BIT(8)
+#define AICV			BIT(9)
+#define ADF			BIT(11)
+#define ACF			BIT(12)
+#define AMF			BIT(13)
+#define HTC_LOC_CTRL		BIT(14)
+#define UC_DATA_EN		BIT(16)
+#define BM_DATA_EN		BIT(17)
+#define MFBEN			BIT(22)
+#define LSIGEN			BIT(23)
+#define EnMBID			BIT(24)
+#define APP_BASSN		BIT(27)
+#define APP_PHYSTS		BIT(28)
+#define APP_ICV			BIT(29)
+#define APP_MIC			BIT(30)
+#define APP_FCS			BIT(31)
+
+/* 2 SECCFG */
+#define	SCR_TxUseDK		BIT(0)	/* Force Tx Use Default Key */
+#define	SCR_RxUseDK		BIT(1)	/* Force Rx Use Default Key */
+#define	SCR_TxEncEnable		BIT(2)	/* Enable Tx Encryption */
+#define	SCR_RxDecEnable		BIT(3)	/* Enable Rx Decryption */
+#define	SCR_SKByA2		BIT(4)	/* Search kEY BY A2 */
+#define	SCR_NoSKMC		BIT(5)	/* No Key Search Multicast */
+#define SCR_TXBCUSEDK		BIT(6)	/* Force Tx Bcast pkt Use Default Key */
+#define SCR_RXBCUSEDK		BIT(7)	/* Force Rx Bcast pkt Use Default Key */
+
+/* 	0xFE00h ~ 0xFE55h	USB Configuration */
+
+/* 2 USB Information (0xFE17) */
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK			0xF
+#define USB_NORMAL_SIE_EP_SHIFT			4
+
+/* 2 Special Option */
+#define USB_AGG_EN				BIT(3)
+
+/*  0; Use interrupt endpoint to upload interrupt pkt */
+/*  1; Use bulk endpoint to upload interrupt pkt, */
+#define INT_BULK_SEL				BIT(4)
+
+/* 2REG_C2HEVT_CLEAR */
+/*  Set by driver and notify FW that the driver has read
+ *  the C2H command message */
+#define	C2H_EVT_HOST_CLOSE	0x00
+/*  Set by FW indicating that FW had set the C2H command
+ *  message and it's not yet read by driver. */
+#define C2H_EVT_FW_CLOSE	0xFF
+
+/* 2REG_MULTI_FUNC_CTRL(For RTL8723 Only) */
+/*  Enable GPIO[9] as WiFi HW PDn source */
+#define	WL_HWPDN_EN				BIT(0)
+/*  WiFi HW PDn polarity control */
+#define	WL_HWPDN_SL				BIT(1)
+/*  WiFi function enable */
+#define	WL_FUNC_EN				BIT(2)
+/*  Enable GPIO[9] as WiFi RF HW PDn source */
+#define	WL_HWROF_EN				BIT(3)
+/*  Enable GPIO[11] as BT HW PDn source */
+#define	BT_HWPDN_EN				BIT(16)
+/*  BT HW PDn polarity control */
+#define	BT_HWPDN_SL				BIT(17)
+/*  BT function enable */
+#define	BT_FUNC_EN				BIT(18)
+/*  Enable GPIO[11] as BT/GPS RF HW PDn source */
+#define	BT_HWROF_EN				BIT(19)
+/*  Enable GPIO[10] as GPS HW PDn source */
+#define	GPS_HWPDN_EN				BIT(20)
+/*  GPS HW PDn polarity control */
+#define	GPS_HWPDN_SL				BIT(21)
+/*  GPS function enable */
+#define	GPS_FUNC_EN				BIT(22)
+
+/* 3 REG_LIFECTRL_CTRL */
+#define	HAL92C_EN_PKT_LIFE_TIME_BK		BIT(3)
+#define	HAL92C_EN_PKT_LIFE_TIME_BE		BIT(2)
+#define	HAL92C_EN_PKT_LIFE_TIME_VI		BIT(1)
+#define	HAL92C_EN_PKT_LIFE_TIME_VO		BIT(0)
+
+#define	HAL92C_MSDU_LIFE_TIME_UNIT		128	/*  in us */
+
+/*  General definitions */
+#define LAST_ENTRY_OF_TX_PKT_BUFFER		176 /*  22k 22528 bytes */
+
+#define POLLING_LLT_THRESHOLD			20
+#define POLLING_READY_TIMEOUT_COUNT		1000
+/*  GPIO BIT */
+#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+
+/*	8192C EEPROM/EFUSE share register definition. */
+
+/* 	EEPROM/Efuse PG Offset for 88EE/88EU/88ES */
+#define	EEPROM_TX_PWR_INX_88E			0x10
+
+#define	EEPROM_ChannelPlan_88E			0xB8
+#define	EEPROM_XTAL_88E				0xB9
+#define	EEPROM_THERMAL_METER_88E		0xBA
+#define	EEPROM_IQK_LCK_88E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION_88E		0xC1
+#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
+#define	EEPROM_RF_ANTENNA_OPT_88E		0xC9
+
+/* RTL88EU */
+#define	EEPROM_MAC_ADDR_88EU			0xD7
+#define EEPROM_USB_OPTIONAL_FUNCTION0		0xD4
+
+/*  RTL88ES */
+#define	EEPROM_MAC_ADDR_88ES			0x11A
+
+#define EEPROM_Default_CrystalCap_88E		0x20
+#define	EEPROM_Default_ThermalMeter_88E		0x18
+
+/* New EFUSE deafult value */
+#define		EEPROM_DEFAULT_24G_INDEX	0x2D
+#define		EEPROM_DEFAULT_24G_HT20_DIFF	0X02
+#define		EEPROM_DEFAULT_24G_OFDM_DIFF	0X04
+
+#define		EEPROM_DEFAULT_DIFF		0XFE
+#define	EEPROM_DEFAULT_BOARD_OPTION		0x00
+
+#define EEPROM_CHANNEL_PLAN_FCC			0x0
+#define EEPROM_CHANNEL_PLAN_IC			0x1
+#define EEPROM_CHANNEL_PLAN_ETSI		0x2
+#define EEPROM_CHANNEL_PLAN_SPA			0x3
+#define EEPROM_CHANNEL_PLAN_FRANCE		0x4
+#define EEPROM_CHANNEL_PLAN_MKK			0x5
+#define EEPROM_CHANNEL_PLAN_MKK1		0x6
+#define EEPROM_CHANNEL_PLAN_ISRAEL		0x7
+#define EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define EEPROM_CHANNEL_PLAN_GLOBAL_DOMA		0x9
+#define EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define EEPROM_CHANNEL_PLAN_NCC			0xB
+#define EEPROM_USB_OPTIONAL1			0xE
+#define EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define	RTL_EEPROM_ID			0x8129
+
+#endif /* __RTL8188E_SPEC_H__ */
diff --git a/drivers/staging/r8188eu/include/rtl8188e_xmit.h b/drivers/staging/r8188eu/include/rtl8188e_xmit.h
new file mode 100644
index 000000000..6db7fabeb
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtl8188e_xmit.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTL8188E_XMIT_H__
+#define __RTL8188E_XMIT_H__
+
+#define		MAX_TX_AGG_PACKET_NUMBER	0xFF
+/*  */
+/*  Queue Select Value in TxDesc */
+/*  */
+#define QSLT_BK							0x2/* 0x01 */
+#define QSLT_BE							0x0
+#define QSLT_VI							0x5/* 0x4 */
+#define QSLT_VO							0x7/* 0x6 */
+#define QSLT_BEACON						0x10
+#define QSLT_HIGH						0x11
+#define QSLT_MGNT						0x12
+#define QSLT_CMD						0x13
+
+/* For 88e early mode */
+#define SET_EARLYMODE_PKTNUM(__paddr, __value)			\
+	le32p_replace_bits((__le32 *)__paddr, __value, GENMASK(2, 0))
+#define SET_EARLYMODE_LEN0(__pAddr, __Value)			\
+	le32p_replace_bits((__le32 *)__paddr, __value, GENMASK(15, 4))
+#define SET_EARLYMODE_LEN1(__paddr, __value)			\
+	le32p_replace_bits((__le32 *)__paddr, __value, GENMASK(27, 16))
+#define SET_EARLYMODE_LEN2_1(__pdr, __vValue)			\
+	le32p_replace_bits((__le32 *)__paddr, __value, GENMASK(31, 28))
+#define SET_EARLYMODE_LEN2_2(__paddr, __value)			\
+	le32p_replace_bits((__le32 *)(__paddr + 4), __value, GENMASK(7, 0))
+#define SET_EARLYMODE_LEN3(__pAddr, __Value)			\
+	le32p_replace_bits((__le32 *)(__paddr + 4), __value, GENMASK(19, 8))
+#define SET_EARLYMODE_LEN4(__paAddr, __vValue)			\
+	le32p_replace_bits((__le32 *)(__paddr + 4), __value, GENMASK(31, 20))
+
+/* defined for TX DESC Operation */
+
+#define MAX_TID (15)
+
+/* OFFSET 0 */
+#define OFFSET_SZ	0
+#define OFFSET_SHT	16
+#define BMC		BIT(24)
+#define LSG		BIT(26)
+#define FSG		BIT(27)
+#define OWN		BIT(31)
+
+/* OFFSET 4 */
+#define PKT_OFFSET_SZ		0
+#define QSEL_SHT		8
+#define RATE_ID_SHT		16
+#define NAVUSEHDR		BIT(20)
+#define SEC_TYPE_SHT		22
+#define PKT_OFFSET_SHT		26
+
+/* OFFSET 8 */
+#define AGG_EN			BIT(12)
+#define AGG_BK			BIT(16)
+#define AMPDU_DENSITY_SHT	20
+#define ANTSEL_A		BIT(24)
+#define ANTSEL_B		BIT(25)
+#define TX_ANT_CCK_SHT		26
+#define TX_ANTL_SHT		28
+#define TX_ANT_HT_SHT		30
+
+/* OFFSET 12 */
+#define SEQ_SHT			16
+#define EN_HWSEQ		BIT(31)
+
+/* OFFSET 16 */
+#define QOS			BIT(6)
+#define	HW_SSN			BIT(7)
+#define USERATE			BIT(8)
+#define DISDATAFB		BIT(10)
+#define CTS_2_SELF		BIT(11)
+#define	RTS_EN			BIT(12)
+#define	HW_RTS_EN		BIT(13)
+#define DATA_SHORT		BIT(24)
+#define PWR_STATUS_SHT		15
+#define DATA_SC_SHT		20
+#define DATA_BW			BIT(25)
+
+/* OFFSET 20 */
+#define	RTY_LMT_EN		BIT(17)
+
+/* OFFSET 20 */
+#define SGI			BIT(6)
+#define USB_TXAGG_NUM_SHT	24
+
+#define USB_TXAGG_DESC_NUM	0x6
+
+#define txdesc_set_ccx_sw_88e(txdesc, value) \
+	do { \
+		((struct txdesc_88e *)(txdesc))->sw1 = (((value)>>8) & 0x0f); \
+		((struct txdesc_88e *)(txdesc))->sw0 = ((value) & 0xff); \
+	} while (0)
+
+struct txrpt_ccx_88e {
+	/* offset 0 */
+	u8 tag1:1;
+	u8 pkt_num:3;
+	u8 txdma_underflow:1;
+	u8 int_bt:1;
+	u8 int_tri:1;
+	u8 int_ccx:1;
+
+	/* offset 1 */
+	u8 mac_id:6;
+	u8 pkt_ok:1;
+	u8 bmc:1;
+
+	/* offset 2 */
+	u8 retry_cnt:6;
+	u8 lifetime_over:1;
+	u8 retry_over:1;
+
+	/* offset 3 */
+	u8 ccx_qtime0;
+	u8 ccx_qtime1;
+
+	/* offset 5 */
+	u8 final_data_rate;
+
+	/* offset 6 */
+	u8 sw1:4;
+	u8 qsel:4;
+
+	/* offset 7 */
+	u8 sw0;
+};
+
+void rtl8188e_fill_fake_txdesc(struct adapter *padapter, u8 *pDesc,
+			       u32 BufferLen, u8 IsPsPoll, u8 IsBTQosNull);
+s32 rtl8188eu_init_xmit_priv(struct adapter *padapter);
+s32 rtl8188eu_hal_xmit(struct adapter *padapter, struct xmit_frame *frame);
+s32 rtl8188eu_mgnt_xmit(struct adapter *padapter, struct xmit_frame *frame);
+s32 rtl8188eu_xmit_buf_handler(struct adapter *padapter);
+#define hal_xmit_handler rtl8188eu_xmit_buf_handler
+void rtl8188eu_xmit_tasklet(unsigned long priv);
+bool rtl8188eu_xmitframe_complete(struct adapter *padapter,
+				 struct xmit_priv *pxmitpriv,
+				 struct xmit_buf *pxmitbuf);
+
+#endif /* __RTL8188E_XMIT_H__ */
diff --git a/drivers/staging/r8188eu/include/rtw_ap.h b/drivers/staging/r8188eu/include/rtw_ap.h
new file mode 100644
index 000000000..8b4134eb3
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_ap.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#ifndef __RTW_AP_H_
+#define __RTW_AP_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+/* external function */
+void rtw_indicate_sta_assoc_event(struct adapter *padapter,
+				  struct sta_info *psta);
+void init_mlme_ap_info(struct adapter *padapter);
+void free_mlme_ap_info(struct adapter *padapter);
+void update_beacon(struct adapter *padapter, u8 ie_id,
+		   u8 *oui, u8 tx);
+void add_RATid(struct adapter *padapter, struct sta_info *psta,
+	       u8 rssi_level);
+void expire_timeout_chk(struct adapter *padapter);
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta);
+void rtw_ap_restore_network(struct adapter *padapter);
+
+void associated_clients_update(struct adapter *padapter, u8 updated);
+void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta);
+u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta);
+void sta_info_update(struct adapter *padapter, struct sta_info *psta);
+u8 ap_free_sta(struct adapter *padapter, struct sta_info *psta,
+	       bool active, u16 reason);
+int rtw_sta_flush(struct adapter *padapter);
+void start_ap_mode(struct adapter *padapter);
+void stop_ap_mode(struct adapter *padapter);
+void update_bmc_sta(struct adapter *padapter);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_br_ext.h b/drivers/staging/r8188eu/include/rtw_br_ext.h
new file mode 100644
index 000000000..56772af3b
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_br_ext.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_BR_EXT_H_
+#define _RTW_BR_EXT_H_
+
+#define GET_MY_HWADDR(padapter)		((padapter)->eeprompriv.mac_addr)
+
+#define NAT25_HASH_BITS		4
+#define NAT25_HASH_SIZE		(1 << NAT25_HASH_BITS)
+#define NAT25_AGEING_TIME	300
+
+#define MAX_NETWORK_ADDR_LEN	17
+
+struct nat25_network_db_entry {
+	struct nat25_network_db_entry	*next_hash;
+	struct nat25_network_db_entry	**pprev_hash;
+	atomic_t	use_count;
+	unsigned char	macAddr[6];
+	unsigned long	ageing_timer;
+	unsigned char	networkAddr[MAX_NETWORK_ADDR_LEN];
+};
+
+enum NAT25_METHOD {
+	NAT25_MIN,
+	NAT25_CHECK,
+	NAT25_INSERT,
+	NAT25_PARSE,
+	NAT25_MAX
+};
+
+struct br_ext_info {
+	unsigned int	nat25_disable;
+	unsigned int	macclone_enable;
+	unsigned int	dhcp_bcst_disable;
+	int	addPPPoETag;		/* 1: Add PPPoE relay-SID, 0: disable */
+	unsigned char	nat25_dmzMac[ETH_ALEN];
+	unsigned int	nat25sc_disable;
+};
+
+void nat25_db_cleanup(struct adapter *priv);
+
+#endif /*  _RTW_BR_EXT_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_cmd.h b/drivers/staging/r8188eu/include/rtw_cmd.h
new file mode 100644
index 000000000..9a76aa85d
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_cmd.h
@@ -0,0 +1,937 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_CMD_H_
+#define __RTW_CMD_H_
+
+#include "wlan_bssdef.h"
+#include "rtw_rf.h"
+#include "rtw_led.h"
+
+#define C2H_MEM_SZ (16*1024)
+
+#include "osdep_service.h"
+#include "ieee80211.h" /*  <ieee80211/ieee80211.h> */
+
+#define FREE_CMDOBJ_SZ	128
+
+#define MAX_CMDSZ	1024
+#define MAX_RSPSZ	512
+#define MAX_EVTSZ	1024
+
+#define CMDBUFF_ALIGN_SZ 512
+
+struct cmd_obj {
+	struct adapter *padapter;
+	u16	cmdcode;
+	u8	res;
+	u8	*parmbuf;
+	u32	cmdsz;
+	u8	*rsp;
+	u32	rspsz;
+	struct list_head list;
+};
+
+struct cmd_priv {
+	struct completion enqueue_cmd;
+	struct completion start_cmd_thread;
+	struct completion stop_cmd_thread;
+	struct __queue cmd_queue;
+	u8	cmd_seq;
+	u8	*cmd_buf;	/* shall be non-paged, and 4 bytes aligned */
+	u8	*cmd_allocated_buf;
+	u8	*rsp_buf;	/* shall be non-paged, and 4 bytes aligned */
+	u8	*rsp_allocated_buf;
+	u32	cmd_done_cnt;
+	u32	rsp_cnt;
+	u8 cmdthd_running;
+	struct adapter *padapter;
+};
+
+struct	evt_priv {
+	struct work_struct c2h_wk;
+	bool c2h_wk_alive;
+	struct rtw_cbuf *c2h_queue;
+	#define C2H_QUEUE_MAX_LEN 10
+	atomic_t event_seq;
+	u8	*evt_buf;	/* shall be non-paged, and 4 bytes aligned */
+};
+
+#define init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code) \
+do {\
+	INIT_LIST_HEAD(&pcmd->list);\
+	pcmd->cmdcode = code;\
+	pcmd->parmbuf = (u8 *)(pparm);\
+	pcmd->cmdsz = sizeof(*pparm);\
+	pcmd->rsp = NULL;\
+	pcmd->rspsz = 0;\
+} while (0)
+
+struct c2h_evt_hdr {
+	u8 id:4;
+	u8 plen:4;
+	u8 seq;
+	u8 payload[];
+};
+
+#define c2h_evt_exist(c2h_evt) ((c2h_evt)->id || (c2h_evt)->plen)
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv);
+void rtw_free_cmd_obj(struct cmd_obj *pcmd);
+
+int rtw_cmd_thread(void *context);
+
+u32 rtw_init_cmd_priv(struct cmd_priv *pcmdpriv);
+void rtw_free_cmd_priv(struct cmd_priv *pcmdpriv);
+
+u32 rtw_init_evt_priv(struct evt_priv *pevtpriv);
+void rtw_free_evt_priv(struct evt_priv *pevtpriv);
+void rtw_evt_notify_isr(struct evt_priv *pevtpriv);
+u8 p2p_protocol_wk_cmd(struct adapter *padapter, int intCmdType);
+
+enum rtw_drvextra_cmd_id {
+	NONE_WK_CID,
+	DYNAMIC_CHK_WK_CID,
+	DM_CTRL_WK_CID,
+	PBC_POLLING_WK_CID,
+	POWER_SAVING_CTRL_WK_CID,/* IPS,AUTOSuspend */
+	LPS_CTRL_WK_CID,
+	ANT_SELECT_WK_CID,
+	P2P_PS_WK_CID,
+	P2P_PROTO_WK_CID,
+	CHECK_HIQ_WK_CID,/* for softap mode, check hi queue if empty */
+	INTEl_WIDI_WK_CID,
+	C2H_WK_CID,
+	RTP_TIMER_CFG_WK_CID,
+	MAX_WK_CID
+};
+
+enum LPS_CTRL_TYPE {
+	LPS_CTRL_SCAN = 0,
+	LPS_CTRL_JOINBSS = 1,
+	LPS_CTRL_CONNECT = 2,
+	LPS_CTRL_DISCONNECT = 3,
+	LPS_CTRL_SPECIAL_PACKET = 4,
+	LPS_CTRL_LEAVE = 5,
+};
+
+enum RFINTFS {
+	SWSI,
+	HWSI,
+	HWPI,
+};
+
+/*
+Caller Mode: Infra, Ad-HoC
+
+Notes: To join a known BSS.
+
+Command-Event Mode
+
+*/
+
+/*
+Caller Mode: Infra, Ad-Hoc
+
+Notes: To join the specified bss
+
+Command Event Mode
+
+*/
+struct joinbss_parm {
+	struct wlan_bssid_ex network;
+};
+
+/*
+Caller Mode: Infra, Ad-HoC(C)
+
+Notes: To disconnect the current associated BSS
+
+Command Mode
+
+*/
+struct disconnect_parm {
+	u32 deauth_timeout_ms;
+};
+
+/*
+Caller Mode: AP, Ad-HoC(M)
+
+Notes: To create a BSS
+
+Command Mode
+*/
+struct createbss_parm {
+	struct wlan_bssid_ex network;
+};
+
+struct	setopmode_parm {
+	u8	mode;
+	u8	rsvd[3];
+};
+
+/*
+Caller Mode: AP, Ad-HoC, Infra
+
+Notes: To ask RTL8711 performing site-survey
+
+Command-Event Mode
+
+*/
+
+#define RTW_SSID_SCAN_AMOUNT 9 /*  for WEXT_CSCAN_AMOUNT 9 */
+#define RTW_CHANNEL_SCAN_AMOUNT (14+37)
+struct sitesurvey_parm {
+	int scan_mode;	/* active: 1, passive: 0 */
+	u8 ssid_num;
+	u8 ch_num;
+	struct ndis_802_11_ssid ssid[RTW_SSID_SCAN_AMOUNT];
+	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To set the auth type of RTL8711. open/shared/802.1x
+
+Command Mode
+
+*/
+struct setauth_parm {
+	u8 mode;  /* 0: legacy open, 1: legacy shared 2: 802.1x */
+	u8 _1x;   /* 0: PSK, 1: TLS */
+	u8 rsvd[2];
+};
+
+/*
+Caller Mode: Infra
+
+a. algorithm: wep40, wep104, tkip & aes
+b. keytype: grp key/unicast key
+c. key contents
+
+when shared key ==> keyid is the camid
+when 802.1x ==> keyid [0:1] ==> grp key
+when 802.1x ==> keyid > 2 ==> unicast key
+
+*/
+struct setkey_parm {
+	u8	algorithm;	/* could be none, wep40, TKIP, CCMP, wep104 */
+	u8	keyid;
+	u8	grpkey;		/* 1: this is the grpkey for 802.1x.
+				 * 0: this is the unicast key for 802.1x */
+	u8	set_tx;		/* 1: main tx key for wep. 0: other key. */
+	u8	key[16];	/* this could be 40 or 104 */
+};
+
+/*
+When in AP or Ad-Hoc mode, this is used to
+allocate an sw/hw entry for a newly associated sta.
+
+Command
+
+when shared key ==> algorithm/keyid
+
+*/
+struct set_stakey_parm {
+	u8	addr[ETH_ALEN];
+	u8	algorithm;
+	u8	id;/* currently for erasing cam entry if
+		    * algorithm == _NO_PRIVACY_ */
+	u8	key[16];
+};
+
+struct set_stakey_rsp {
+	u8	addr[ETH_ALEN];
+	u8	keyid;
+	u8	rsvd;
+};
+
+/*
+Caller Ad-Hoc/AP
+
+Command -Rsp(AID == CAMID) mode
+
+This is to force fw to add an sta_data entry per driver's request.
+
+FW will write an cam entry associated with it.
+
+*/
+struct set_assocsta_parm {
+	u8	addr[ETH_ALEN];
+};
+
+struct set_assocsta_rsp {
+	u8	cam_id;
+	u8	rsvd[3];
+};
+
+/*
+	Caller Ad-Hoc/AP
+
+	Command mode
+
+	This is to force fw to del an sta_data entry per driver's request
+
+	FW will invalidate the cam entry associated with it.
+
+*/
+struct del_assocsta_parm {
+	u8	addr[ETH_ALEN];
+};
+
+/*
+Caller Mode: AP/Ad-HoC(M)
+
+Notes: To notify fw that given staid has changed its power state
+
+Command Mode
+
+*/
+struct setstapwrstate_parm {
+	u8	staid;
+	u8	status;
+	u8	hwaddr[6];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To setup the basic rate of RTL8711
+
+Command Mode
+
+*/
+struct	setbasicrate_parm {
+	u8	basicrates[NumRates];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To read the current basic rate
+
+Command-Rsp Mode
+
+*/
+struct getbasicrate_parm {
+	u32 rsvd;
+};
+
+struct getbasicrate_rsp {
+	u8 basicrates[NumRates];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To setup the data rate of RTL8711
+
+Command Mode
+
+*/
+struct setdatarate_parm {
+	u8	mac_id;
+	u8	datarates[NumRates];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To read the current data rate
+
+Command-Rsp Mode
+
+*/
+struct getdatarate_parm {
+	u32 rsvd;
+
+};
+struct getdatarate_rsp {
+	u8 datarates[NumRates];
+};
+
+/*
+Caller Mode: Any
+AP: AP can use the info for the contents of beacon frame
+Infra: STA can use the info when sitesurveying
+Ad-HoC(M): Like AP
+Ad-HoC(C): Like STA
+
+Notes: To set the phy capability of the NIC
+
+Command Mode
+
+*/
+
+struct	setphyinfo_parm {
+	struct regulatory_class class_sets[NUM_REGULATORYS];
+	u8	status;
+};
+
+struct	getphyinfo_parm {
+	u32 rsvd;
+};
+
+struct	getphyinfo_rsp {
+	struct regulatory_class class_sets[NUM_REGULATORYS];
+	u8	status;
+};
+
+/*
+Caller Mode: Any
+
+Notes: To set the channel/modem/band
+This command will be used when channel/modem/band is changed.
+
+Command Mode
+
+*/
+struct	setphy_parm {
+	u8	rfchannel;
+	u8	modem;
+};
+
+/*
+Caller Mode: Any
+
+Notes: To get the current setting of channel/modem/band
+
+Command-Rsp Mode
+
+*/
+struct	getphy_parm {
+	u32 rsvd;
+
+};
+struct	getphy_rsp {
+	u8	rfchannel;
+	u8	modem;
+};
+
+struct readBB_parm {
+	u8	offset;
+};
+struct readBB_rsp {
+	u8	value;
+};
+
+struct readTSSI_parm {
+	u8	offset;
+};
+struct readTSSI_rsp {
+	u8	value;
+};
+
+struct writeBB_parm {
+	u8	offset;
+	u8	value;
+};
+
+struct readRF_parm {
+	u8	offset;
+};
+struct readRF_rsp {
+	u32	value;
+};
+
+struct writeRF_parm {
+	u32	offset;
+	u32	value;
+};
+
+struct getrfintfs_parm {
+	u8	rfintfs;
+};
+
+struct Tx_Beacon_param
+{
+	struct wlan_bssid_ex network;
+};
+
+/*
+	Notes: This command is used for H2C/C2H loopback testing
+
+	mac[0] == 0
+	==> CMD mode, return H2C_SUCCESS.
+	The following condition must be ture under CMD mode
+		mac[1] == mac[4], mac[2] == mac[3], mac[0]=mac[5]= 0;
+		s0 == 0x1234, s1 == 0xabcd, w0 == 0x78563412, w1 == 0x5aa5def7;
+		s2 == (b1 << 8 | b0);
+
+	mac[0] == 1
+	==> CMD_RSP mode, return H2C_SUCCESS_RSP
+
+	The rsp layout shall be:
+	rsp:			parm:
+		mac[0]  =   mac[5];
+		mac[1]  =   mac[4];
+		mac[2]  =   mac[3];
+		mac[3]  =   mac[2];
+		mac[4]  =   mac[1];
+		mac[5]  =   mac[0];
+		s0		=   s1;
+		s1		=   swap16(s0);
+		w0		=	swap32(w1);
+		b0		=	b1
+		s2		=	s0 + s1
+		b1		=	b0
+		w1		=	w0
+
+	mac[0] ==	2
+	==> CMD_EVENT mode, return	H2C_SUCCESS
+	The event layout shall be:
+	event:			parm:
+		mac[0]  =   mac[5];
+		mac[1]  =   mac[4];
+		mac[2]  =   event's seq no, starting from 1 to parm's marc[3]
+		mac[3]  =   mac[2];
+		mac[4]  =   mac[1];
+		mac[5]  =   mac[0];
+		s0		=   swap16(s0) - event.mac[2];
+		s1		=   s1 + event.mac[2];
+		w0		=	swap32(w0);
+		b0		=	b1
+		s2		=	s0 + event.mac[2]
+		b1		=	b0
+		w1		=	swap32(w1) - event.mac[2];
+
+		parm->mac[3] is the total event counts that host requested.
+	event will be the same with the cmd's param.
+*/
+
+/*  CMD param Format for driver extra cmd handler */
+struct drvextra_cmd_parm {
+	int ec_id; /* extra cmd id */
+	int type_size; /*  Can use this field as the type id or command size */
+	unsigned char *pbuf;
+};
+
+/*------------------- Below are used for RF/BB tunning ---------------------*/
+
+struct	setantenna_parm {
+	u8	tx_antset;
+	u8	rx_antset;
+	u8	tx_antenna;
+	u8	rx_antenna;
+};
+
+struct	enrateadaptive_parm {
+	u32	en;
+};
+
+struct settxagctbl_parm {
+	u32	txagc[MAX_RATES_LENGTH];
+};
+
+struct gettxagctbl_parm {
+	u32 rsvd;
+};
+struct gettxagctbl_rsp {
+	u32	txagc[MAX_RATES_LENGTH];
+};
+
+struct setagcctrl_parm {
+	u32	agcctrl;		/*  0: pure hw, 1: fw */
+};
+
+struct setssup_parm	{
+	u32	ss_ForceUp[MAX_RATES_LENGTH];
+};
+
+struct getssup_parm	{
+	u32 rsvd;
+};
+
+struct getssup_rsp	{
+	u8	ss_ForceUp[MAX_RATES_LENGTH];
+};
+
+struct setssdlevel_parm	{
+	u8	ss_DLevel[MAX_RATES_LENGTH];
+};
+
+struct getssdlevel_parm	{
+	u32 rsvd;
+};
+
+struct getssdlevel_rsp	{
+	u8	ss_DLevel[MAX_RATES_LENGTH];
+};
+
+struct setssulevel_parm	{
+	u8	ss_ULevel[MAX_RATES_LENGTH];
+};
+
+struct getssulevel_parm	{
+	u32 rsvd;
+};
+
+struct getssulevel_rsp	{
+	u8	ss_ULevel[MAX_RATES_LENGTH];
+};
+
+struct	setcountjudge_parm {
+	u8	count_judge[MAX_RATES_LENGTH];
+};
+
+struct	getcountjudge_parm {
+	u32 rsvd;
+};
+
+struct	getcountjudge_rsp {
+	u8	count_judge[MAX_RATES_LENGTH];
+};
+
+struct setratable_parm {
+	u8 ss_ForceUp[NumRates];
+	u8 ss_ULevel[NumRates];
+	u8 ss_DLevel[NumRates];
+	u8 count_judge[NumRates];
+};
+
+struct getratable_parm {
+                uint rsvd;
+};
+
+struct getratable_rsp {
+        u8 ss_ForceUp[NumRates];
+        u8 ss_ULevel[NumRates];
+        u8 ss_DLevel[NumRates];
+        u8 count_judge[NumRates];
+};
+
+/* to get TX,RX retry count */
+
+struct gettxretrycnt_parm {
+	unsigned int rsvd;
+};
+
+struct gettxretrycnt_rsp {
+	unsigned long tx_retrycnt;
+};
+
+struct getrxretrycnt_parm {
+	unsigned int rsvd;
+};
+
+struct getrxretrycnt_rsp {
+	unsigned long rx_retrycnt;
+};
+
+/* to get BCNOK,BCNERR count */
+struct getbcnokcnt_parm {
+	unsigned int rsvd;
+};
+
+struct getbcnokcnt_rsp {
+	unsigned long  bcnokcnt;
+};
+
+struct getbcnerrcnt_parm {
+	unsigned int rsvd;
+};
+
+struct getbcnerrcnt_rsp {
+	unsigned long bcnerrcnt;
+};
+
+/*  to get current TX power level */
+struct getcurtxpwrlevel_parm {
+	unsigned int rsvd;
+};
+struct getcurtxpwrlevel_rspi {
+	unsigned short tx_power;
+};
+
+struct setprobereqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setassocreqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setproberspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setassocrspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct addBaReq_parm {
+	unsigned int tid;
+	u8	addr[ETH_ALEN];
+};
+
+/*H2C Handler index: 46 */
+struct set_ch_parm {
+	u8 ch;
+	u8 bw;
+	u8 ch_offset;
+};
+
+/*H2C Handler index: 59 */
+struct SetChannelPlan_param
+{
+	u8 channel_plan;
+};
+
+/*H2C Handler index: 60 */
+struct LedBlink_param
+{
+	struct LED_871x *pLed;
+};
+
+/*H2C Handler index: 61 */
+struct SetChannelSwitch_param
+{
+	u8 new_ch_no;
+};
+
+/*H2C Handler index: 62 */
+struct TDLSoption_param
+{
+	u8 addr[ETH_ALEN];
+	u8 option;
+};
+
+#define GEN_CMD_CODE(cmd)	cmd ## _CMD_
+
+/*
+
+Result:
+0x00: success
+0x01: success, and check Response.
+0x02: cmd ignored due to duplicated sequcne number
+0x03: cmd dropped due to invalid cmd code
+0x04: reserved.
+
+*/
+
+#define H2C_RSP_OFFSET		512
+
+#define H2C_SUCCESS		0x00
+#define H2C_SUCCESS_RSP		0x01
+#define H2C_DUPLICATED		0x02
+#define H2C_DROPPED		0x03
+#define H2C_PARAMETERS_ERROR	0x04
+#define H2C_REJECTED		0x05
+#define H2C_CMD_OVERFLOW	0x06
+#define H2C_RESERVED		0x07
+
+u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num);
+u8 rtw_createbss_cmd(struct adapter  *padapter);
+u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key);
+u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue);
+u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network* pnetwork);
+u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
+u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infra networktype);
+u8 rtw_setdatarate_cmd(struct adapter  *padapter, u8 *rateset);
+u8 rtw_setrfintfs_cmd(struct adapter  *padapter, u8 mode);
+
+u8 rtw_gettssi_cmd(struct adapter  *padapter, u8 offset,u8 *pval);
+u8 rtw_setfwdig_cmd(struct adapter*padapter, u8 type);
+u8 rtw_setfwra_cmd(struct adapter*padapter, u8 type);
+
+u8 rtw_addbareq_cmd(struct adapter*padapter, u8 tid, u8 *addr);
+
+u8 rtw_dynamic_chk_wk_cmd(struct adapter *adapter);
+
+u8 rtw_lps_ctrl_wk_cmd(struct adapter*padapter, u8 lps_ctrl_type, u8 enqueue);
+u8 rtw_rpt_timer_cfg_cmd(struct adapter*padapter, u16 minRptTime);
+
+ u8 rtw_antenna_select_cmd(struct adapter*padapter, u8 antenna,u8 enqueue);
+u8 rtw_ps_cmd(struct adapter*padapter);
+
+u8 rtw_chk_hi_queue_cmd(struct adapter*padapter);
+
+u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan);
+
+u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt);
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf);
+
+void rtw_survey_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd);
+void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd);
+void rtw_joinbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd);
+void rtw_createbss_cmd_callback(struct adapter *adapt, struct cmd_obj *pcmd);
+void rtw_getbbrfreg_cmdrsp_callback(struct adapter *adapt, struct cmd_obj *cmd);
+
+void rtw_setstaKey_cmdrsp_callback(struct adapter *adapt, struct cmd_obj *cmd);
+void rtw_setassocsta_cmdrsp_callback(struct adapter *adapt, struct cmd_obj *cm);
+void rtw_getrttbl_cmdrsp_callback(struct adapter *adapt, struct cmd_obj *cmd);
+
+struct _cmd_callback {
+	u32	cmd_code;
+	void (*callback)(struct adapter  *padapter, struct cmd_obj *cmd);
+};
+
+enum rtw_h2c_cmd {
+	GEN_CMD_CODE(_Read_MACREG),	/*0*/
+	GEN_CMD_CODE(_Write_MACREG),
+	GEN_CMD_CODE(_Read_BBREG),
+	GEN_CMD_CODE(_Write_BBREG),
+	GEN_CMD_CODE(_Read_RFREG),
+	GEN_CMD_CODE(_Write_RFREG), /*5*/
+	GEN_CMD_CODE(_Read_EEPROM),
+	GEN_CMD_CODE(_Write_EEPROM),
+	GEN_CMD_CODE(_Read_EFUSE),
+	GEN_CMD_CODE(_Write_EFUSE),
+
+	GEN_CMD_CODE(_Read_CAM),	/*10*/
+	GEN_CMD_CODE(_Write_CAM),
+	GEN_CMD_CODE(_setBCNITV),
+	GEN_CMD_CODE(_setMBIDCFG),
+	GEN_CMD_CODE(_JoinBss),   /*14*/
+	GEN_CMD_CODE(_DisConnect), /*15*/
+	GEN_CMD_CODE(_CreateBss),
+	GEN_CMD_CODE(_SetOpMode),
+	GEN_CMD_CODE(_SiteSurvey),  /*18*/
+	GEN_CMD_CODE(_SetAuth),
+
+	GEN_CMD_CODE(_SetKey),	/*20*/
+	GEN_CMD_CODE(_SetStaKey),
+	GEN_CMD_CODE(_SetAssocSta),
+	GEN_CMD_CODE(_DelAssocSta),
+	GEN_CMD_CODE(_SetStaPwrState),
+	GEN_CMD_CODE(_SetBasicRate), /*25*/
+	GEN_CMD_CODE(_GetBasicRate),
+	GEN_CMD_CODE(_SetDataRate),
+	GEN_CMD_CODE(_GetDataRate),
+	GEN_CMD_CODE(_SetPhyInfo),
+
+	GEN_CMD_CODE(_GetPhyInfo),	/*30*/
+	GEN_CMD_CODE(_SetPhy),
+	GEN_CMD_CODE(_GetPhy),
+	GEN_CMD_CODE(_readRssi),
+	GEN_CMD_CODE(_readGain),
+	GEN_CMD_CODE(_SetAtim), /*35*/
+	GEN_CMD_CODE(_SetPwrMode),
+	GEN_CMD_CODE(_JoinbssRpt),
+	GEN_CMD_CODE(_SetRaTable),
+	GEN_CMD_CODE(_GetRaTable),
+
+	GEN_CMD_CODE(_GetCCXReport), /*40*/
+	GEN_CMD_CODE(_GetDTMReport),
+	GEN_CMD_CODE(_GetTXRateStatistics),
+	GEN_CMD_CODE(_SetUsbSuspend),
+	GEN_CMD_CODE(_SetH2cLbk),
+	GEN_CMD_CODE(_AddBAReq), /*45*/
+	GEN_CMD_CODE(_SetChannel), /*46*/
+	GEN_CMD_CODE(_SetTxPower),
+	GEN_CMD_CODE(_SwitchAntenna),
+	GEN_CMD_CODE(_SetCrystalCap),
+	GEN_CMD_CODE(_SetSingleCarrierTx), /*50*/
+
+	GEN_CMD_CODE(_SetSingleToneTx),/*51*/
+	GEN_CMD_CODE(_SetCarrierSuppressionTx),
+	GEN_CMD_CODE(_SetContinuousTx),
+	GEN_CMD_CODE(_SwitchBandwidth), /*54*/
+	GEN_CMD_CODE(_TX_Beacon), /*55*/
+
+	GEN_CMD_CODE(_Set_MLME_EVT), /*56*/
+	GEN_CMD_CODE(_Set_Drv_Extra), /*57*/
+	GEN_CMD_CODE(_Set_H2C_MSG), /*58*/
+
+	GEN_CMD_CODE(_SetChannelPlan), /*59*/
+	GEN_CMD_CODE(_LedBlink), /*60*/
+
+	GEN_CMD_CODE(_SetChannelSwitch), /*61*/
+	GEN_CMD_CODE(_TDLS), /*62*/
+
+	MAX_H2CCMD
+};
+
+#define _GetBBReg_CMD_		_Read_BBREG_CMD_
+#define _SetBBReg_CMD_		_Write_BBREG_CMD_
+#define _GetRFReg_CMD_		_Read_RFREG_CMD_
+#define _SetRFReg_CMD_		_Write_RFREG_CMD_
+
+#ifdef _RTW_CMD_C_
+static struct _cmd_callback	rtw_cmd_callback[] =
+{
+	{GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/
+	{GEN_CMD_CODE(_Write_MACREG), NULL},
+	{GEN_CMD_CODE(_Read_BBREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_BBREG), NULL},
+	{GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/
+	{GEN_CMD_CODE(_Read_EEPROM), NULL},
+	{GEN_CMD_CODE(_Write_EEPROM), NULL},
+	{GEN_CMD_CODE(_Read_EFUSE), NULL},
+	{GEN_CMD_CODE(_Write_EFUSE), NULL},
+
+	{GEN_CMD_CODE(_Read_CAM),	NULL},	/*10*/
+	{GEN_CMD_CODE(_Write_CAM),	 NULL},
+	{GEN_CMD_CODE(_setBCNITV), NULL},
+	{GEN_CMD_CODE(_setMBIDCFG), NULL},
+	{GEN_CMD_CODE(_JoinBss), &rtw_joinbss_cmd_callback},  /*14*/
+	{GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd_callback}, /*15*/
+	{GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd_callback},
+	{GEN_CMD_CODE(_SetOpMode), NULL},
+	{GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback}, /*18*/
+	{GEN_CMD_CODE(_SetAuth), NULL},
+
+	{GEN_CMD_CODE(_SetKey), NULL},	/*20*/
+	{GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback},
+	{GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback},
+	{GEN_CMD_CODE(_DelAssocSta), NULL},
+	{GEN_CMD_CODE(_SetStaPwrState), NULL},
+	{GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/
+	{GEN_CMD_CODE(_GetBasicRate), NULL},
+	{GEN_CMD_CODE(_SetDataRate), NULL},
+	{GEN_CMD_CODE(_GetDataRate), NULL},
+	{GEN_CMD_CODE(_SetPhyInfo), NULL},
+
+	{GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/
+	{GEN_CMD_CODE(_SetPhy), NULL},
+	{GEN_CMD_CODE(_GetPhy), NULL},
+	{GEN_CMD_CODE(_readRssi), NULL},
+	{GEN_CMD_CODE(_readGain), NULL},
+	{GEN_CMD_CODE(_SetAtim), NULL}, /*35*/
+	{GEN_CMD_CODE(_SetPwrMode), NULL},
+	{GEN_CMD_CODE(_JoinbssRpt), NULL},
+	{GEN_CMD_CODE(_SetRaTable), NULL},
+	{GEN_CMD_CODE(_GetRaTable), NULL},
+
+	{GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/
+	{GEN_CMD_CODE(_GetDTMReport),	NULL},
+	{GEN_CMD_CODE(_GetTXRateStatistics), NULL},
+	{GEN_CMD_CODE(_SetUsbSuspend), NULL},
+	{GEN_CMD_CODE(_SetH2cLbk), NULL},
+	{GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/
+	{GEN_CMD_CODE(_SetChannel), NULL},		/*46*/
+	{GEN_CMD_CODE(_SetTxPower), NULL},
+	{GEN_CMD_CODE(_SwitchAntenna), NULL},
+	{GEN_CMD_CODE(_SetCrystalCap), NULL},
+	{GEN_CMD_CODE(_SetSingleCarrierTx), NULL},	/*50*/
+
+	{GEN_CMD_CODE(_SetSingleToneTx), NULL}, /*51*/
+	{GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL},
+	{GEN_CMD_CODE(_SetContinuousTx), NULL},
+	{GEN_CMD_CODE(_SwitchBandwidth), NULL},		/*54*/
+	{GEN_CMD_CODE(_TX_Beacon), NULL},/*55*/
+
+	{GEN_CMD_CODE(_Set_MLME_EVT), NULL},/*56*/
+	{GEN_CMD_CODE(_Set_Drv_Extra), NULL},/*57*/
+	{GEN_CMD_CODE(_Set_H2C_MSG), NULL},/*58*/
+	{GEN_CMD_CODE(_SetChannelPlan), NULL},/*59*/
+	{GEN_CMD_CODE(_LedBlink), NULL},/*60*/
+
+	{GEN_CMD_CODE(_SetChannelSwitch), NULL},/*61*/
+	{GEN_CMD_CODE(_TDLS), NULL},/*62*/
+};
+#endif
+
+#endif /*  _CMD_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_eeprom.h b/drivers/staging/r8188eu/include/rtw_eeprom.h
new file mode 100644
index 000000000..94d735b1d
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_eeprom.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_EEPROM_H__
+#define __RTW_EEPROM_H__
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+struct eeprom_priv {
+	u8		bautoload_fail_flag;
+	u8		mac_addr[ETH_ALEN] __aligned(2); /* PermanentAddress */
+};
+
+#endif  /* __RTL871X_EEPROM_H__ */
diff --git a/drivers/staging/r8188eu/include/rtw_efuse.h b/drivers/staging/r8188eu/include/rtw_efuse.h
new file mode 100644
index 000000000..3d688a0e6
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_efuse.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_EFUSE_H__
+#define __RTW_EFUSE_H__
+
+#define		EFUSE_MAX_WORD_UNIT			4
+
+void ReadEFuseByte(struct adapter *adapter, u16 _offset, u8 *pbuf);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_event.h b/drivers/staging/r8188eu/include/rtw_event.h
new file mode 100644
index 000000000..54dc1ea43
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_event.h
@@ -0,0 +1,97 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_EVENT_H_
+#define _RTW_EVENT_H_
+
+#include "osdep_service.h"
+
+#include "wlan_bssdef.h"
+#include <linux/semaphore.h>
+#include <linux/sem.h>
+
+/*
+Used to report a bss has been scanned
+*/
+struct survey_event	{
+	struct wlan_bssid_ex bss;
+};
+
+/*
+Used to report that the requested site survey has been done.
+
+bss_cnt indicates the number of bss that has been reported.
+
+*/
+struct surveydone_event {
+	unsigned int	bss_cnt;
+
+};
+
+/*
+Used to report the link result of joinning the given bss
+
+join_res:
+-1: authentication fail
+-2: association fail
+> 0: TID
+
+*/
+struct joinbss_event {
+	struct	wlan_network	network;
+};
+
+/*
+Used to report a given STA has joinned the created BSS.
+It is used in AP/Ad-HoC(M) mode.
+*/
+
+struct stassoc_event {
+	unsigned char macaddr[6];
+	unsigned char rsvd[2];
+	int    cam_id;
+};
+
+struct stadel_event {
+	unsigned char macaddr[6];
+	unsigned char rsvd[2]; /* for reason */
+	int mac_id;
+};
+
+struct addba_event {
+	unsigned int tid;
+};
+
+#define GEN_EVT_CODE(event)	event ## _EVT_
+
+struct fwevent {
+	u32	parmsize;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+};
+
+#define C2HEVENT_SZ			32
+
+struct event_node {
+	unsigned char *node;
+	unsigned char evt_code;
+	unsigned short evt_sz;
+	int	*caller_ff_tail;
+	int	caller_ff_sz;
+};
+
+struct c2hevent_queue {
+	int	head;
+	int	tail;
+	struct	event_node	nodes[C2HEVENT_SZ];
+	unsigned char	seq;
+};
+
+#define NETWORK_QUEUE_SZ	4
+
+struct network_queue {
+	int	head;
+	int	tail;
+	struct wlan_bssid_ex networks[NETWORK_QUEUE_SZ];
+};
+
+#endif /*  _WLANEVENT_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_fw.h b/drivers/staging/r8188eu/include/rtw_fw.h
new file mode 100644
index 000000000..8f74157ee
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_fw.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_FW_H__
+#define __RTW_FW_H__
+
+struct rt_firmware {
+	u8 *data;
+	u32 size;
+};
+
+#include "drv_types.h"
+
+int rtl8188e_firmware_download(struct adapter *padapter);
+void rtw_reset_8051(struct adapter *padapter);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_ht.h b/drivers/staging/r8188eu/include/rtw_ht.h
new file mode 100644
index 000000000..2b56b7c38
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_ht.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_HT_H_
+#define _RTW_HT_H_
+
+#include "osdep_service.h"
+#include "wifi.h"
+
+struct ht_priv {
+	u32	ht_option;
+	u32	ampdu_enable;/* for enable Tx A-MPDU */
+	u32	tx_amsdu_enable;/* for enable Tx A-MSDU */
+	u32	tx_amdsu_maxlen; /*  1: 8k, 0:4k ; default:8k, for tx */
+	u32	rx_ampdu_maxlen; /* for rx reordering ctrl win_sz,
+				  * updated when join_callback. */
+	u8	bwmode;/*  */
+	u8	ch_offset;/* PRIME_CHNL_OFFSET */
+	u8	sgi;/* short GI */
+
+	/* for processing Tx A-MPDU */
+	u8	agg_enable_bitmap;
+	u8	candidate_tid_bitmap;
+
+	struct ieee80211_ht_cap ht_cap;
+};
+
+#endif	/* _RTL871X_HT_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_io.h b/drivers/staging/r8188eu/include/rtw_io.h
new file mode 100644
index 000000000..925c7967a
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_io.h
@@ -0,0 +1,302 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_IO_H_
+#define _RTW_IO_H_
+
+#include "osdep_service.h"
+#include "osdep_intf.h"
+
+#include <asm/byteorder.h>
+#include <linux/semaphore.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <asm/atomic.h>
+
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+
+#define rtw_usb_buffer_alloc(dev, size, dma)				\
+	usb_alloc_coherent((dev), (size), (in_interrupt() ?		\
+			   GFP_ATOMIC : GFP_KERNEL), (dma))
+#define rtw_usb_buffer_free(dev, size, addr, dma)			\
+	usb_free_coherent((dev), (size), (addr), (dma))
+
+#define NUM_IOREQ		8
+
+#define MAX_PROT_SZ	(64-16)
+
+#define _IOREADY		0
+#define _IO_WAIT_COMPLETE	1
+#define _IO_WAIT_RSP		2
+
+/*  IO COMMAND TYPE */
+#define _IOSZ_MASK_		(0x7F)
+#define _IO_WRITE_		BIT(7)
+#define _IO_FIXED_		BIT(8)
+#define _IO_BURST_		BIT(9)
+#define _IO_BYTE_		BIT(10)
+#define _IO_HW_			BIT(11)
+#define _IO_WORD_		BIT(12)
+#define _IO_SYNC_		BIT(13)
+#define _IO_CMDMASK_		(0x1F80)
+
+/*
+	For prompt mode accessing, caller shall free io_req
+	Otherwise, io_handler will free io_req
+*/
+
+/*  IO STATUS TYPE */
+#define _IO_ERR_		BIT(2)
+#define _IO_SUCCESS_		BIT(1)
+#define _IO_DONE_		BIT(0)
+
+#define IO_RD32			(_IO_SYNC_ | _IO_WORD_)
+#define IO_RD16			(_IO_SYNC_ | _IO_HW_)
+#define IO_RD8			(_IO_SYNC_ | _IO_BYTE_)
+
+#define IO_RD32_ASYNC		(_IO_WORD_)
+#define IO_RD16_ASYNC		(_IO_HW_)
+#define IO_RD8_ASYNC		(_IO_BYTE_)
+
+#define IO_WR32			(_IO_WRITE_ | _IO_SYNC_ | _IO_WORD_)
+#define IO_WR16			(_IO_WRITE_ | _IO_SYNC_ | _IO_HW_)
+#define IO_WR8			(_IO_WRITE_ | _IO_SYNC_ | _IO_BYTE_)
+
+#define IO_WR32_ASYNC		(_IO_WRITE_ | _IO_WORD_)
+#define IO_WR16_ASYNC		(_IO_WRITE_ | _IO_HW_)
+#define IO_WR8_ASYNC		(_IO_WRITE_ | _IO_BYTE_)
+
+/*
+	Only Sync. burst accessing is provided.
+*/
+
+#define IO_WR_BURST(x)						\
+	(_IO_WRITE_ | _IO_SYNC_ | _IO_BURST_ | ((x) & _IOSZ_MASK_))
+#define IO_RD_BURST(x)						\
+	(_IO_SYNC_ | _IO_BURST_ | ((x) & _IOSZ_MASK_))
+
+/* below is for the intf_option bit defition... */
+
+#define _INTF_ASYNC_	BIT(0)	/* support async io */
+
+struct intf_priv;
+struct intf_hdl;
+struct io_queue;
+
+struct io_req {
+	struct list_head list;
+	u32	addr;
+	u32	val;
+	u32	command;
+	u32	status;
+	u8	*pbuf;
+	struct semaphore sema;
+
+	void (*_async_io_callback)(struct adapter *padater,
+				   struct io_req *pio_req, u8 *cnxt);
+	u8 *cnxt;
+};
+
+struct	intf_hdl {
+	struct adapter *padapter;
+	struct dvobj_priv *pintf_dev;
+};
+
+struct reg_protocol_rd {
+#ifdef __LITTLE_ENDIAN
+	/* DW1 */
+	u32		NumOfTrans:4;
+	u32		Reserved1:4;
+	u32		Reserved2:24;
+	/* DW2 */
+	u32		ByteCount:7;
+	u32		WriteEnable:1;		/* 0:read, 1:write */
+	u32		FixOrContinuous:1;	/* 0:continuous, 1: Fix */
+	u32		BurstMode:1;
+	u32		Byte1Access:1;
+	u32		Byte2Access:1;
+	u32		Byte4Access:1;
+	u32		Reserved3:3;
+	u32		Reserved4:16;
+	/* DW3 */
+	u32		BusAddress;
+	/* DW4 */
+	/* u32		Value; */
+#else
+/* DW1 */
+	u32 Reserved1:4;
+	u32 NumOfTrans:4;
+	u32 Reserved2:24;
+	/* DW2 */
+	u32 WriteEnable:1;
+	u32 ByteCount:7;
+	u32 Reserved3:3;
+	u32 Byte4Access:1;
+
+	u32 Byte2Access:1;
+	u32 Byte1Access:1;
+	u32 BurstMode:1;
+	u32 FixOrContinuous:1;
+	u32 Reserved4:16;
+	/* DW3 */
+	u32	BusAddress;
+
+	/* DW4 */
+#endif
+};
+
+struct reg_protocol_wt {
+#ifdef __LITTLE_ENDIAN
+	/* DW1 */
+	u32	NumOfTrans:4;
+	u32	Reserved1:4;
+	u32	Reserved2:24;
+	/* DW2 */
+	u32	ByteCount:7;
+	u32	WriteEnable:1;		/* 0:read, 1:write */
+	u32	FixOrContinuous:1;	/* 0:continuous, 1: Fix */
+	u32	BurstMode:1;
+	u32	Byte1Access:1;
+	u32	Byte2Access:1;
+	u32	Byte4Access:1;
+	u32	Reserved3:3;
+	u32	Reserved4:16;
+	/* DW3 */
+	u32	BusAddress;
+	/* DW4 */
+	u32	Value;
+#else
+	/* DW1 */
+	u32 Reserved1 :4;
+	u32 NumOfTrans:4;
+	u32 Reserved2:24;
+	/* DW2 */
+	u32 WriteEnable:1;
+	u32 ByteCount:7;
+	u32 Reserved3:3;
+	u32 Byte4Access:1;
+	u32 Byte2Access:1;
+	u32 Byte1Access:1;
+	u32 BurstMode:1;
+	u32 FixOrContinuous:1;
+	u32 Reserved4:16;
+	/* DW3 */
+	u32	BusAddress;
+	/* DW4 */
+	u32	Value;
+#endif
+};
+
+/*
+Below is the data structure used by _io_handler
+*/
+
+struct io_queue {
+	spinlock_t lock;
+	struct list_head free_ioreqs;
+	struct list_head pending;	/* The io_req list that will be served
+					 * in the single protocol read/write.*/
+	struct list_head processing;
+	u8	*free_ioreqs_buf; /*  4-byte aligned */
+	u8	*pallocated_free_ioreqs_buf;
+	struct	intf_hdl	intf;
+};
+
+struct io_priv {
+	struct adapter *padapter;
+	struct intf_hdl intf;
+};
+
+uint ioreq_flush(struct adapter *adapter, struct io_queue *ioqueue);
+void sync_ioreq_enqueue(struct io_req *preq,struct io_queue *ioqueue);
+uint sync_ioreq_flush(struct adapter *adapter, struct io_queue *ioqueue);
+uint free_ioreq(struct io_req *preq, struct io_queue *pio_queue);
+struct io_req *alloc_ioreq(struct io_queue *pio_q);
+
+uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl);
+void unregister_intf_hdl(struct intf_hdl *pintfhdl);
+
+void _rtw_attrib_read(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void _rtw_attrib_write(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+int __must_check rtw_read8(struct adapter *adapter, u32 addr, u8 *data);
+int __must_check rtw_read16(struct adapter *adapter, u32 addr, u16 *data);
+int __must_check rtw_read32(struct adapter *adapter, u32 addr, u32 *data);
+void _rtw_read_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+u32 rtw_read_port(struct adapter *adapter, u8 *pmem);
+void rtw_read_port_cancel(struct adapter *adapter);
+
+int rtw_write8(struct adapter *adapter, u32 addr, u8 val);
+int rtw_write16(struct adapter *adapter, u32 addr, u16 val);
+int rtw_write32(struct adapter *adapter, u32 addr, u32 val);
+int rtw_writeN(struct adapter *adapter, u32 addr, u32 length, u8 *pdata);
+
+void _rtw_write_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+u32 rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void rtw_write_port_cancel(struct adapter *adapter);
+
+void rtw_write_scsi(struct adapter *adapter, u32 cnt, u8 *pmem);
+
+/* ioreq */
+void ioreq_read8(struct adapter *adapter, u32 addr, u8 *pval);
+void ioreq_read16(struct adapter *adapter, u32 addr, u16 *pval);
+void ioreq_read32(struct adapter *adapter, u32 addr, u32 *pval);
+void ioreq_write8(struct adapter *adapter, u32 addr, u8 val);
+void ioreq_write16(struct adapter *adapter, u32 addr, u16 val);
+void ioreq_write32(struct adapter *adapter, u32 addr, u32 val);
+
+uint async_read8(struct adapter *adapter, u32 addr, u8 *pbuff,
+		 void (*_async_io_callback)(struct adapter *padater,
+					    struct io_req *pio_req,
+					    u8 *cnxt), u8 *cnxt);
+uint async_read16(struct adapter *adapter, u32 addr,  u8 *pbuff,
+		  void (*_async_io_callback)(struct adapter *padater,
+					     struct io_req *pio_req,
+					     u8 *cnxt), u8 *cnxt);
+uint async_read32(struct adapter *adapter, u32 addr,  u8 *pbuff,
+		  void (*_async_io_callback)(struct adapter *padater,
+					     struct io_req *pio_req,
+					     u8 *cnxt), u8 *cnxt);
+
+void async_read_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void async_read_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+void async_write8(struct adapter *adapter, u32 addr, u8 val,
+		  void (*_async_io_callback)(struct adapter *padater,
+					     struct io_req *pio_req,
+					     u8 *cnxt), u8 *cnxt);
+void async_write16(struct adapter *adapter, u32 addr, u16 val,
+		   void (*_async_io_callback)(struct adapter *padater,
+					      struct io_req *pio_req,
+					      u8 *cnxt), u8 *cnxt);
+void async_write32(struct adapter *adapter, u32 addr, u32 val,
+		   void (*_async_io_callback)(struct adapter *padater,
+					      struct io_req *pio_req,
+					      u8 *cnxt), u8 *cnxt);
+
+void async_write_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void async_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+uint alloc_io_queue(struct adapter *adapter);
+void free_io_queue(struct adapter *adapter);
+void async_bus_io(struct io_queue *pio_q);
+void bus_sync_io(struct io_queue *pio_q);
+u32 _ioreq2rwmem(struct io_queue *pio_q);
+void dev_power_down(struct adapter *Adapter, u8 bpwrup);
+
+#define PlatformEFIOWrite1Byte(_a,_b,_c)		\
+	rtw_write8(_a,_b,_c)
+#define PlatformEFIOWrite2Byte(_a,_b,_c)		\
+	rtw_write16(_a,_b,_c)
+#define PlatformEFIOWrite4Byte(_a,_b,_c)		\
+	rtw_write32(_a,_b,_c)
+
+#define PlatformEFIORead1Byte(_a,_b)		\
+		rtw_read8(_a,_b)
+#define PlatformEFIORead2Byte(_a,_b)		\
+		rtw_read16(_a,_b)
+#define PlatformEFIORead4Byte(_a,_b)		\
+		rtw_read32(_a,_b)
+
+#endif	/* _RTL8711_IO_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_ioctl.h b/drivers/staging/r8188eu/include/rtw_ioctl.h
new file mode 100644
index 000000000..c704f3040
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_ioctl.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_IOCTL_H_
+#define _RTW_IOCTL_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+extern struct iw_handler_def  rtw_handlers_def;
+extern int ui_pid[3];
+
+#endif /*  #ifndef __INC_CEINFO_ */
diff --git a/drivers/staging/r8188eu/include/rtw_ioctl_set.h b/drivers/staging/r8188eu/include/rtw_ioctl_set.h
new file mode 100644
index 000000000..7365079c7
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_ioctl_set.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_IOCTL_SET_H_
+#define __RTW_IOCTL_SET_H_
+
+#include "drv_types.h"
+
+typedef u8 NDIS_802_11_PMKID_VALUE[16];
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *adapt,
+				      enum ndis_802_11_auth_mode authmode);
+u8 rtw_set_802_11_bssid(struct adapter*adapter, u8 *bssid);
+u8 rtw_set_802_11_add_wep(struct adapter *adapter, struct ndis_802_11_wep *wep);
+u8 rtw_set_802_11_disassociate(struct adapter *adapter);
+u8 rtw_set_802_11_bssid_list_scan(struct adapter*adapter,
+				  struct ndis_802_11_ssid *pssid,
+				  int ssid_max_num);
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *adapter,
+				      enum ndis_802_11_network_infra type);
+u8 rtw_set_802_11_ssid(struct adapter *adapt, struct ndis_802_11_ssid *ssid);
+u16 rtw_get_cur_max_rate(struct adapter *adapter);
+int rtw_change_ifname(struct adapter *padapter, const char *ifname);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_iol.h b/drivers/staging/r8188eu/include/rtw_iol.h
new file mode 100644
index 000000000..099f5a075
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_iol.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_IOL_H_
+#define __RTW_IOL_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define IOREG_CMD_END_LEN	4
+
+struct ioreg_cfg {
+	u8	length;
+	u8	cmd_id;
+	__le16	address;
+	__le32	data;
+	__le32  mask;
+};
+
+enum ioreg_cmd {
+	IOREG_CMD_LLT		= 0x01,
+	IOREG_CMD_REFUSE	= 0x02,
+	IOREG_CMD_EFUSE_PATH	= 0x03,
+	IOREG_CMD_WB_REG	= 0x04,
+	IOREG_CMD_WW_REG	= 0x05,
+	IOREG_CMD_WD_REG	= 0x06,
+	IOREG_CMD_W_RF		= 0x07,
+	IOREG_CMD_DELAY_US	= 0x10,
+	IOREG_CMD_DELAY_MS	= 0x11,
+	IOREG_CMD_END		= 0xFF,
+};
+
+struct xmit_frame *rtw_IOL_accquire_xmit_frame(struct adapter *adapter);
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds,
+			u32 cmd_len);
+bool rtw_IOL_applied(struct adapter  *adapter);
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us);
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms);
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame);
+
+void read_efuse_from_txpktbuf(struct adapter *adapter, int bcnhead,
+			      u8 *content, u16 *size);
+
+int rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr,
+			  u8 value, u8 mask);
+int rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr,
+			  u16 value, u16 mask);
+int rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr,
+			  u32 value, u32 mask);
+int rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path,
+			   u16 addr, u32 value, u32 mask);
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame);
+
+#endif /* __RTW_IOL_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_led.h b/drivers/staging/r8188eu/include/rtw_led.h
new file mode 100644
index 000000000..8520f022a
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_led.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_LED_H_
+#define __RTW_LED_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+enum LED_CTL_MODE {
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7,
+	LED_CTL_START_TO_LINK = 8,
+	LED_CTL_START_WPS = 9,
+	LED_CTL_STOP_WPS = 10,
+	LED_CTL_STOP_WPS_FAIL = 12,
+};
+
+enum LED_STATE_871x {
+	RTW_LED_OFF = 2,
+	LED_BLINK_NORMAL = 3,
+	LED_BLINK_SLOWLY = 4,
+	LED_BLINK_SCAN = 6, /*  LED is blinking during scanning period,
+			     * the # of times to blink is depend on time
+			     * for scanning. */
+	LED_BLINK_TXRX = 9,
+	LED_BLINK_WPS = 10,	/*  LED is blinkg during WPS communication */
+	LED_BLINK_WPS_STOP = 11,
+};
+
+struct led_priv {
+	struct adapter *padapter;
+
+	bool bRegUseLed;
+
+	enum LED_STATE_871x	CurrLedState; /*  Current LED state. */
+
+	bool bLedOn; /*  true if LED is ON, false if LED is OFF. */
+
+	bool bLedBlinkInProgress; /*  true if it is blinking, false o.w.. */
+
+	bool bLedWPSBlinkInProgress;
+
+	u32 BlinkTimes; /*  Number of times to toggle led state for blinking. */
+
+	bool bLedLinkBlinkInProgress;
+	bool bLedScanBlinkInProgress;
+	struct delayed_work blink_work;
+};
+
+void rtl8188eu_InitSwLeds(struct adapter *padapter);
+void rtl8188eu_DeInitSwLeds(struct adapter *padapter);
+
+void rtw_led_control(struct adapter *padapter, enum LED_CTL_MODE LedAction);
+
+#endif /* __RTW_LED_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_mlme.h b/drivers/staging/r8188eu/include/rtw_mlme.h
new file mode 100644
index 000000000..b69989cba
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_mlme.h
@@ -0,0 +1,581 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_MLME_H_
+#define __RTW_MLME_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wlan_bssdef.h"
+
+#define	MAX_BSS_CNT	128
+#define   MAX_JOIN_TIMEOUT	6500
+
+/* Increase the scanning timeout because of increasing the SURVEY_TO value. */
+
+#define		SCANNING_TIMEOUT	8000
+
+#define	SCAN_INTERVAL	(30) /*  unit:2sec, 30*2=60sec */
+
+#define	SCANQUEUE_LIFETIME 20 /*  unit:sec */
+
+#define	WIFI_NULL_STATE			0x00000000
+
+#define	WIFI_ASOC_STATE			0x00000001	/* Under Linked state */
+#define	WIFI_REASOC_STATE		0x00000002
+#define	WIFI_SLEEP_STATE		0x00000004
+#define	WIFI_STATION_STATE		0x00000008
+
+#define	WIFI_AP_STATE			0x00000010
+#define	WIFI_ADHOC_STATE		0x00000020
+#define WIFI_ADHOC_MASTER_STATE		0x00000040
+#define WIFI_UNDER_LINKING		0x00000080
+
+#define	WIFI_UNDER_WPS			0x00000100
+#define	WIFI_STA_ALIVE_CHK_STATE	0x00000400
+#define	WIFI_SITE_MONITOR		0x00000800	/* to indicate the station is under site surveying */
+
+#define	WIFI_MP_STATE			0x00010000
+#define	WIFI_MP_CTX_BACKGROUND		0x00020000	/*  in continuous tx background */
+#define	WIFI_MP_CTX_ST			0x00040000	/*  in continuous tx with single-tone */
+#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000	/*  pending in continuous tx background due to out of skb */
+#define	WIFI_MP_CTX_CCK_HW		0x00100000	/*  in continuous tx */
+#define	WIFI_MP_CTX_CCK_CS		0x00200000	/*  in continuous tx with carrier suppression */
+#define WIFI_MP_LPBK_STATE		0x00400000
+
+#define _FW_UNDER_LINKING	WIFI_UNDER_LINKING
+#define _FW_LINKED			WIFI_ASOC_STATE
+#define _FW_UNDER_SURVEY	WIFI_SITE_MONITOR
+
+enum dot11AuthAlgrthmNum {
+	dot11AuthAlgrthm_Open = 0,
+	dot11AuthAlgrthm_Shared,
+	dot11AuthAlgrthm_8021X,
+	dot11AuthAlgrthm_Auto,
+	dot11AuthAlgrthm_WAPI,
+	dot11AuthAlgrthm_MaxNum
+};
+
+/*  Scan type including active and passive scan. */
+enum rt_scan_type {
+	SCAN_PASSIVE,
+	SCAN_ACTIVE,
+	SCAN_MIX,
+};
+
+/*
+there are several "locks" in mlme_priv,
+since mlme_priv is a shared resource between many threads,
+like ISR/Call-Back functions, the OID handlers, and even timer functions.
+
+Each _queue has its own locks, already.
+Other items are protected by mlme_priv.lock.
+
+To avoid possible dead lock, any thread trying to modifiying mlme_priv
+SHALL not lock up more than one lock at a time!
+*/
+
+#define traffic_threshold	10
+#define	traffic_scan_period	500
+
+struct sitesurvey_ctrl {
+	u64	last_tx_pkts;
+	uint	last_rx_pkts;
+	int	traffic_busy;
+	struct timer_list sitesurvey_ctrl_timer;
+};
+
+struct rt_link_detect {
+	u32	NumTxOkInPeriod;
+	u32	NumRxOkInPeriod;
+	u32	NumRxUnicastOkInPeriod;
+	bool	bBusyTraffic;
+	bool	bTxBusyTraffic;
+	bool	bRxBusyTraffic;
+	bool	bHigherBusyTraffic; /*  For interrupt migration purpose. */
+	bool	bHigherBusyRxTraffic; /* We may disable Tx interrupt according
+				       * to Rx traffic. */
+	bool	bHigherBusyTxTraffic; /* We may disable Tx interrupt according
+				       * to Tx traffic. */
+};
+
+struct profile_info {
+	u8	ssidlen;
+	u8	ssid[ WLAN_SSID_MAXLEN ];
+	u8	peermac[ ETH_ALEN ];
+};
+
+struct tx_invite_req_info {
+	u8	token;
+	u8	benable;
+	u8	go_ssid[ WLAN_SSID_MAXLEN ];
+	u8	ssidlen;
+	u8	go_bssid[ ETH_ALEN ];
+	u8	peer_macaddr[ ETH_ALEN ];
+	u8	operating_ch;	/* This information will be set by using the
+				 * p2p_set op_ch=x */
+	u8	peer_ch;	/* The listen channel for peer P2P device */
+};
+
+struct tx_invite_resp_info {
+	u8	token;	/* Used to record the dialog token of p2p invitation
+			 * request frame. */
+};
+
+struct tx_provdisc_req_info {
+	u16	wps_config_method_request;	/* Used when sending the
+						 * provisioning request frame*/
+	u16	peer_channel_num[2];		/* The channel number which the
+						 * receiver stands. */
+	struct ndis_802_11_ssid	ssid;
+	u8	peerDevAddr[ETH_ALEN];		/* Peer device address */
+	u8	peerIFAddr[ETH_ALEN];		/* Peer interface address */
+	u8	benable;			/* This provision discovery
+						 * request frame is trigger
+						 * to send or not */
+};
+
+/* When peer device issue prov_disc_req first, we should store the following
+ * information */
+/* The UI must know this information to know which config method the
+ * remote p2p device needs. */
+struct rx_provdisc_req_info {
+	u8	peerDevAddr[ETH_ALEN];		/* Peer device address */
+	u8	strconfig_method_desc_of_prov_disc_req[4];	/* description
+			* for the config method located in the provisioning
+			* discovery request frame. */
+};
+
+struct tx_nego_req_info {
+	u16	peer_channel_num[2];	/* The channel number. */
+	u8	peerDevAddr[ETH_ALEN];	/* Peer device address */
+	u8	benable;		/* This negotiation request frame is
+					 * trigger to send or not */
+};
+
+struct group_id_info {
+	u8	go_device_addr[ ETH_ALEN ];	/* The GO's device address of
+						 * this P2P group */
+	u8	ssid[ WLAN_SSID_MAXLEN ];	/* The SSID of this P2P group */
+};
+
+struct scan_limit_info {
+	u8	scan_op_ch_only;	/* When this flag is set, the driver
+					 * should only scan the op. channel */
+	u8	operation_ch[2];	/* Store the op. chan of invitation */
+};
+
+struct wifidirect_info {
+	struct adapter *padapter;
+	struct timer_list find_phase_timer;
+	struct timer_list restore_p2p_state_timer;
+
+	/* Used to do the scanning. After confirming the peer is availalble,
+	 * the driver transmits the P2P frame to peer. */
+	struct timer_list pre_tx_scan_timer;
+	struct timer_list reset_ch_sitesurvey;
+	struct timer_list reset_ch_sitesurvey2;	/* Just for resetting the scan
+					 * limit function by using p2p nego */
+	struct tx_provdisc_req_info	tx_prov_disc_info;
+	struct rx_provdisc_req_info rx_prov_disc_info;
+	struct tx_invite_req_info	invitereq_info;
+	/* Store the profile information of persistent group */
+	struct profile_info profileinfo[P2P_MAX_PERSISTENT_GROUP_NUM];
+	struct tx_invite_resp_info	inviteresp_info;
+	struct tx_nego_req_info	nego_req_info;
+	/* Store the group id info when doing the group negot handshake. */
+	struct group_id_info groupid_info;
+	/* Used for get the limit scan channel from the Invitation procedure */
+	struct scan_limit_info rx_invitereq_info;
+	/* Used for get the limit scan chan from the P2P negotiation handshake*/
+	struct scan_limit_info p2p_info;
+	enum P2P_ROLE role;
+	enum P2P_STATE pre_p2p_state;
+	enum P2P_STATE p2p_state;
+	/* The device address should be the mac address of this device. */
+	u8 device_addr[ETH_ALEN];
+	u8 interface_addr[ETH_ALEN];
+	u8 social_chan[4];
+	u8 listen_channel;
+	u8 operating_channel;
+	u8 listen_dwell;	/* This value should be between 1 and 3 */
+	u8 support_rate[8];
+	u8 p2p_wildcard_ssid[P2P_WILDCARD_SSID_LEN];
+	u8 intent;	/* should only include the intent value. */
+	u8 p2p_peer_interface_addr[ETH_ALEN];
+	u8 p2p_peer_device_addr[ETH_ALEN];
+	u8 peer_intent;	/* Included the intent value and tie breaker value. */
+	/* Device name for displaying on searching device screen */
+	u8 device_name[WPS_MAX_DEVICE_NAME_LEN];
+	u8 device_name_len;
+	u8 profileindex; /* Used to point to the index of profileinfo array */
+	u8 peer_operating_ch;
+	u8 find_phase_state_exchange_cnt;
+	/* The device password ID for group negotiation */
+	u16 device_password_id_for_nego;
+	u8 negotiation_dialog_token;
+	/* SSID information for group negotitation */
+	u8 nego_ssid[WLAN_SSID_MAXLEN];
+	u8 nego_ssidlen;
+	u8 p2p_group_ssid[WLAN_SSID_MAXLEN];
+	u8 p2p_group_ssid_len;
+	/* Flag to know if the persistent function should be supported or not.*/
+	u8 persistent_supported;
+	/* In the Sigma test, the Sigma will provide this enable from the
+	 * sta_set_p2p CAPI. */
+	/*	0: disable */
+	/*	1: enable */
+	u8 session_available;	/* Flag to set the WFD session available to
+				 * enable or disable "by Sigma" */
+	/* In the Sigma test, the Sigma will disable the session available
+	 * by using the sta_preset CAPI. */
+	/*	0: disable */
+	/*	1: enable */
+
+	/* This field will store the WPS value (PIN value or PBC) that UI had
+	 * got from the user. */
+	enum	P2P_WPSINFO ui_got_wps_info;
+	u16 supported_wps_cm;	/* This field describes the WPS config method
+				 * which this driver supported. */
+				/* The value should be the combination of config
+				 * method defined in page104 of WPS v2.0 spec.*/
+	/* This field will contain the length of body of P2P Channel List
+	 * attribute of group negotiation response frame. */
+	uint channel_list_attr_len;
+	/* This field will contain the body of P2P Channel List attribute of
+	 * group negotitation response frame. */
+	/* We will use the channel_cnt and channel_list fields when constructing
+	 * the group negotiation confirm frame. */
+	u8 channel_list_attr[100];
+	enum P2P_PS_MODE p2p_ps_mode; /*  indicate p2p ps mode */
+	enum P2P_PS_STATE p2p_ps_state; /*  indicate p2p ps state */
+	u8 noa_index; /*  Identifies and instance of Notice of Absence timing. */
+	u8 ctwindow; /*  Client traffic window. A period of time in TU after TBTT. */
+	u8 opp_ps; /*  opportunistic power save. */
+	u8 noa_num; /*  number of NoA descriptor in P2P IE. */
+	u8 noa_count[P2P_MAX_NOA_NUM]; /*  Count for owner, Type of client. */
+	/* Max duration for owner, preferred or min acceptable duration for
+	 * client. */
+	u32 noa_duration[P2P_MAX_NOA_NUM];
+	/* Length of interval for owner, preferred or max acceptable interval
+	 * of client. */
+	u32 noa_interval[P2P_MAX_NOA_NUM];
+	/* schedule expressed in terms of the lower 4 bytes of the TSF timer. */
+	u32 noa_start_time[P2P_MAX_NOA_NUM];
+};
+
+struct tdls_ss_record {	/* signal strength record */
+	u8 macaddr[ETH_ALEN];
+	u8 RxPWDBAll;
+	u8 is_tdls_sta;	/*  true: direct link sta, false: else */
+};
+
+struct tdls_info {
+	u8 ap_prohibited;
+	uint setup_state;
+	u8 sta_cnt;
+	u8 sta_maximum;	/*  1:tdls sta is equal (NUM_STA-1), reach max direct link number; 0: else; */
+	struct tdls_ss_record	ss_record;
+	u8 macid_index;	/* macid entry that is ready to write */
+	u8 clear_cam;	/* cam entry that is trying to clear, using it in direct link teardown */
+	u8 ch_sensing;
+	u8 cur_channel;
+	u8 candidate_ch;
+	u8 collect_pkt_num[MAX_CHANNEL_NUM];
+	spinlock_t cmd_lock;
+	spinlock_t hdl_lock;
+	u8 watchdog_count;
+	u8 dev_discovered;		/* WFD_TDLS: for sigma test */
+	u8 enable;
+};
+
+struct qos_priv {
+	/* bit mask option: u-apsd,
+	 * s-apsd, ts, block ack... */
+	unsigned int qos_option;
+};
+
+struct mlme_priv {
+	spinlock_t lock;
+	int fw_state;	/* shall we protect this variable? maybe not necessarily... */
+	bool bScanInProcess;
+	u8 to_join; /* flag */
+	u8 to_roaming; /*  roaming trying times */
+
+	u8 *nic_hdl;
+
+	struct list_head *pscanned;
+	struct __queue free_bss_pool;
+	struct __queue scanned_queue;
+	u8 *free_bss_buf;
+	u8	key_mask; /* use to restore wep key after hal_init */
+	u32	num_of_scanned;
+
+	struct ndis_802_11_ssid	assoc_ssid;
+	u8	assoc_bssid[6];
+
+	struct wlan_network	cur_network;
+	struct wlan_network *cur_network_scanned;
+
+	u32	scan_interval;
+
+	struct timer_list assoc_timer;
+
+	uint assoc_by_bssid;
+	uint assoc_by_rssi;
+
+	struct timer_list scan_to_timer; /*  driver itself handles scan_timeout status. */
+	u32 scan_start_time; /*  used to evaluate the time spent in scanning */
+
+	struct qos_priv qospriv;
+
+	/* Number of non-HT AP/stations */
+	int num_sta_no_ht;
+
+	/* Number of HT AP/stations 20 MHz */
+	/* int num_sta_ht_20mhz; */
+
+	int num_FortyMHzIntolerant;
+	struct ht_priv	htpriv;
+	struct rt_link_detect LinkDetectInfo;
+	struct timer_list dynamic_chk_timer; /* dynamic/periodic check timer */
+
+	u8	acm_mask; /*  for wmm acm mask */
+	u8	ChannelPlan;
+	enum rt_scan_type scan_mode; /*  active: 1, passive: 0 */
+
+	/* u8 probereq_wpsie[MAX_WPS_IE_LEN];added in probe req */
+	/* int probereq_wpsie_len; */
+	u8 *wps_probe_req_ie;
+	u32 wps_probe_req_ie_len;
+
+	u8 *assoc_req;
+	u32 assoc_req_len;
+
+	/* Number of associated Non-ERP stations (i.e., stations using 802.11b
+	 * in 802.11g BSS) */
+	int num_sta_non_erp;
+
+	/* Number of associated stations that do not support Short Slot Time */
+	int num_sta_no_short_slot_time;
+
+	/* Number of associated stations that do not support Short Preamble */
+	int num_sta_no_short_preamble;
+
+	int olbc; /* Overlapping Legacy BSS Condition */
+
+	/* Number of HT assoc sta that do not support greenfield */
+	int num_sta_ht_no_gf;
+
+	/* Number of associated non-HT stations */
+	/* int num_sta_no_ht; */
+
+	/* Number of HT associated stations 20 MHz */
+	int num_sta_ht_20mhz;
+
+	/* Overlapping BSS information */
+	int olbc_ht;
+
+	u16 ht_op_mode;
+
+	u8 *wps_beacon_ie;
+	/* u8 *wps_probe_req_ie; */
+	u8 *wps_probe_resp_ie;
+	u8 *wps_assoc_resp_ie;
+
+	u32 wps_beacon_ie_len;
+	u32 wps_probe_resp_ie_len;
+	u32 wps_assoc_resp_ie_len;
+
+	u8 *p2p_beacon_ie;
+	u8 *p2p_probe_req_ie;
+	u8 *p2p_probe_resp_ie;
+	u8 *p2p_go_probe_resp_ie; /* for GO */
+	u8 *p2p_assoc_req_ie;
+
+	u32 p2p_beacon_ie_len;
+	u32 p2p_probe_req_ie_len;
+	u32 p2p_probe_resp_ie_len;
+	u32 p2p_go_probe_resp_ie_len; /* for GO */
+	u32 p2p_assoc_req_ie_len;
+	spinlock_t bcn_update_lock;
+	u8		update_bcn;
+};
+
+int hostapd_mode_init(struct adapter *padapter);
+void hostapd_mode_unload(struct adapter *padapter);
+
+extern unsigned char WPA_TKIP_CIPHER[4];
+extern unsigned char RSN_TKIP_CIPHER[4];
+extern unsigned char REALTEK_96B_IE[];
+extern unsigned char	MCS_rate_2R[16];
+extern unsigned char	MCS_rate_1R[16];
+
+void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf);
+void rtw_survey_event_callback(struct adapter *adapter, u8 *pbuf);
+void rtw_surveydone_event_callback(struct adapter *adapter, u8 *pbuf);
+void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf);
+void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf);
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf);
+void indicate_wx_scan_complete_event(struct adapter *padapter);
+void rtw_indicate_wx_assoc_event(struct adapter *padapter);
+void rtw_indicate_wx_disassoc_event(struct adapter *padapter);
+int event_thread(void *context);
+void rtw_free_network_queue(struct adapter *adapter, u8 isfreeall);
+int rtw_init_mlme_priv(struct adapter *adapter);
+void rtw_free_mlme_priv (struct mlme_priv *pmlmepriv);
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv);
+int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv,
+		int keyid, u8 set_tx);
+int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv);
+
+static inline u8 *get_bssid(struct mlme_priv *pmlmepriv)
+{	/* if sta_mode:pmlmepriv->cur_network.network.MacAddress=> bssid */
+	/*  if adhoc_mode:pmlmepriv->cur_network.network.MacAddress=> ibss mac address */
+	return pmlmepriv->cur_network.network.MacAddress;
+}
+
+static inline bool check_fwstate(struct mlme_priv *pmlmepriv, int state)
+{
+	if (pmlmepriv->fw_state & state)
+		return true;
+
+	return false;
+}
+
+static inline int get_fwstate(struct mlme_priv *pmlmepriv)
+{
+	return pmlmepriv->fw_state;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ *
+ * ### NOTE:#### (!!!!)
+ * MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+static inline void set_fwstate(struct mlme_priv *pmlmepriv, int state)
+{
+	pmlmepriv->fw_state |= state;
+	/* FOR HW integration */
+	if (_FW_UNDER_SURVEY==state)
+		pmlmepriv->bScanInProcess = true;
+}
+
+static inline void _clr_fwstate_(struct mlme_priv *pmlmepriv, int state)
+{
+	pmlmepriv->fw_state &= ~state;
+	/* FOR HW integration */
+	if (_FW_UNDER_SURVEY==state)
+		pmlmepriv->bScanInProcess = false;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ */
+static inline void clr_fwstate(struct mlme_priv *pmlmepriv, int state)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	if (check_fwstate(pmlmepriv, state))
+		pmlmepriv->fw_state ^= state;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static inline void clr_fwstate_ex(struct mlme_priv *pmlmepriv, int state)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	_clr_fwstate_(pmlmepriv, state);
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static inline void up_scanned_network(struct mlme_priv *pmlmepriv)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	pmlmepriv->num_of_scanned++;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static inline void down_scanned_network(struct mlme_priv *pmlmepriv)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	pmlmepriv->num_of_scanned--;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static inline void set_scanned_network_val(struct mlme_priv *pmlmepriv, int val)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	pmlmepriv->num_of_scanned = val;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+u16 rtw_get_capability(struct wlan_bssid_ex *bss);
+void rtw_update_scanned_network(struct adapter *adapter,
+				struct wlan_bssid_ex *target);
+void rtw_disconnect_hdl_under_linked(struct adapter *adapter,
+				     struct sta_info *psta, u8 free_assoc);
+void rtw_generate_random_ibss(u8 *pibss);
+struct wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr);
+struct wlan_network *rtw_get_oldest_wlan_network(struct __queue *scanned_queue);
+
+void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue);
+void rtw_indicate_disconnect(struct adapter *adapter);
+void rtw_indicate_connect(struct adapter *adapter);
+void rtw_indicate_scan_done(struct adapter *padapter);
+
+int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie,
+			uint in_len);
+int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie,
+		        uint in_len, uint initial_out_len);
+void rtw_init_registrypriv_dev_network(struct adapter *adapter);
+
+void rtw_update_registrypriv_dev_network(struct adapter *adapter);
+
+void _rtw_join_timeout_handler(struct adapter *adapter);
+void rtw_scan_timeout_handler(struct adapter *adapter);
+
+ void rtw_dynamic_check_timer_handlder(struct adapter *adapter);
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv);
+
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv);
+
+void _rtw_free_network(struct mlme_priv *pmlmepriv,
+		       struct wlan_network *pnetwork, u8 isfreeall);
+void _rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+			      struct wlan_network *pnetwork);
+
+struct wlan_network* _rtw_find_network(struct __queue *scanned_queue, u8 *addr);
+
+void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall);
+
+int rtw_if_up(struct adapter *padapter);
+
+u8 *rtw_get_capability_from_ie(u8 *ie);
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie);
+
+void rtw_joinbss_reset(struct adapter *padapter);
+
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie,
+				   u8 *out_ie, uint in_len, uint *pout_len);
+void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len);
+void rtw_issue_addbareq_cmd(struct adapter *padapter,
+			    struct xmit_frame *pxmitframe);
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork);
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst);
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network);
+void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network);
+void rtw_set_roaming(struct adapter *adapter, u8 to_roaming);
+u8 rtw_to_roaming(struct adapter *adapter);
+
+void rtw_set_max_rpt_macid(struct adapter *adapter, u8 macid);
+void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta,
+			      u32 mstatus);
+
+u8 rtw_current_antenna(struct adapter *adapter);
+
+#endif /* __RTL871X_MLME_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_mlme_ext.h b/drivers/staging/r8188eu/include/rtw_mlme_ext.h
new file mode 100644
index 000000000..b322d0848
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_mlme_ext.h
@@ -0,0 +1,783 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_MLME_EXT_H_
+#define __RTW_MLME_EXT_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wlan_bssdef.h"
+
+/*	Commented by Albert 20101105 */
+/*	Increase the SURVEY_TO value from 100 to 150  ( 100ms to 150ms ) */
+/*	The Realtek 8188CE SoftAP will spend around 100ms to send the probe response after receiving the probe request. */
+/*	So, this driver tried to extend the dwell time for each scanning channel. */
+/*	This will increase the chance to receive the probe response from SoftAP. */
+
+#define SURVEY_TO		(100)
+#define REAUTH_TO		(300) /* 50) */
+#define REASSOC_TO		(300) /* 50) */
+/* define DISCONNECT_TO	(3000) */
+#define ADDBA_TO			(2000)
+
+#define LINKED_TO (1) /* unit:2 sec, 1x2=2 sec */
+
+#define REAUTH_LIMIT	(4)
+#define REASSOC_LIMIT	(4)
+
+#define	DYNAMIC_FUNC_DISABLE			(0x0)
+
+/*  ====== ODM_ABILITY_E ======== */
+/*  BB ODM section BIT 0-15 */
+#define	DYNAMIC_BB_DIG				BIT(0)
+
+#define		DYNAMIC_ALL_FUNC_ENABLE		0xFFFFFFF
+
+#define _HW_STATE_NOLINK_		0x00
+#define _HW_STATE_ADHOC_		0x01
+#define _HW_STATE_STATION_	0x02
+#define _HW_STATE_AP_			0x03
+
+#define		_1M_RATE_	0
+#define		_2M_RATE_	1
+#define		_5M_RATE_	2
+#define		_11M_RATE_	3
+#define		_6M_RATE_	4
+#define		_9M_RATE_	5
+#define		_12M_RATE_	6
+#define		_18M_RATE_	7
+#define		_24M_RATE_	8
+#define		_36M_RATE_	9
+#define		_48M_RATE_	10
+#define		_54M_RATE_	11
+
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WMM_OUI[];
+extern unsigned char WPS_OUI[];
+extern unsigned char WFD_OUI[];
+extern unsigned char P2P_OUI[];
+
+extern unsigned char WMM_INFO_OUI[];
+extern unsigned char WMM_PARA_OUI[];
+
+/*  Channel Plan Type. */
+/*  Note: */
+/*	We just add new channel plan when the new channel plan is different
+ *      from any of the following channel plan. */
+/*	If you just want to customize the actions(scan period or join actions)
+ *      about one of the channel plan, */
+/*	customize them in struct rt_channel_info in the RT_CHANNEL_LIST. */
+enum RT_CHANNEL_DOMAIN {
+	/*  old channel plan mapping =====  */
+	RT_CHANNEL_DOMAIN_FCC = 0x00,
+	RT_CHANNEL_DOMAIN_IC = 0x01,
+	RT_CHANNEL_DOMAIN_ETSI = 0x02,
+	RT_CHANNEL_DOMAIN_SPAIN = 0x03,
+	RT_CHANNEL_DOMAIN_FRANCE = 0x04,
+	RT_CHANNEL_DOMAIN_MKK = 0x05,
+	RT_CHANNEL_DOMAIN_MKK1 = 0x06,
+	RT_CHANNEL_DOMAIN_ISRAEL = 0x07,
+	RT_CHANNEL_DOMAIN_TELEC = 0x08,
+	RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN = 0x09,
+	RT_CHANNEL_DOMAIN_WORLD_WIDE_13 = 0x0A,
+	RT_CHANNEL_DOMAIN_TAIWAN = 0x0B,
+	RT_CHANNEL_DOMAIN_CHINA = 0x0C,
+	RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO = 0x0D,
+	RT_CHANNEL_DOMAIN_KOREA = 0x0E,
+	RT_CHANNEL_DOMAIN_TURKEY = 0x0F,
+	RT_CHANNEL_DOMAIN_JAPAN = 0x10,
+	RT_CHANNEL_DOMAIN_FCC_NO_DFS = 0x11,
+	RT_CHANNEL_DOMAIN_JAPAN_NO_DFS = 0x12,
+	RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS = 0x14,
+
+	/*  new channel plan mapping, (2GDOMAIN_5GDOMAIN) ===== */
+	RT_CHANNEL_DOMAIN_WORLD_NULL = 0x20,
+	RT_CHANNEL_DOMAIN_ETSI1_NULL = 0x21,
+	RT_CHANNEL_DOMAIN_FCC1_NULL = 0x22,
+	RT_CHANNEL_DOMAIN_MKK1_NULL = 0x23,
+	RT_CHANNEL_DOMAIN_ETSI2_NULL = 0x24,
+	RT_CHANNEL_DOMAIN_FCC1_FCC1 = 0x25,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI1 = 0x26,
+	RT_CHANNEL_DOMAIN_MKK1_MKK1 = 0x27,
+	RT_CHANNEL_DOMAIN_WORLD_KCC1 = 0x28,
+	RT_CHANNEL_DOMAIN_WORLD_FCC2 = 0x29,
+	RT_CHANNEL_DOMAIN_WORLD_FCC3 = 0x30,
+	RT_CHANNEL_DOMAIN_WORLD_FCC4 = 0x31,
+	RT_CHANNEL_DOMAIN_WORLD_FCC5 = 0x32,
+	RT_CHANNEL_DOMAIN_WORLD_FCC6 = 0x33,
+	RT_CHANNEL_DOMAIN_FCC1_FCC7 = 0x34,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI2 = 0x35,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI3 = 0x36,
+	RT_CHANNEL_DOMAIN_MKK1_MKK2 = 0x37,
+	RT_CHANNEL_DOMAIN_MKK1_MKK3 = 0x38,
+	RT_CHANNEL_DOMAIN_FCC1_NCC1 = 0x39,
+	RT_CHANNEL_DOMAIN_FCC1_NCC2 = 0x40,
+	RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G = 0x41,
+	/*  Add new channel plan above this line=============== */
+	RT_CHANNEL_DOMAIN_MAX,
+	RT_CHANNEL_DOMAIN_REALTEK_DEFINE = 0x7F,
+};
+
+enum RT_CHANNEL_DOMAIN_2G {
+	RT_CHANNEL_DOMAIN_2G_WORLD = 0x00,		/* Worldwide 13 */
+	RT_CHANNEL_DOMAIN_2G_ETSI1 = 0x01,		/* Europe */
+	RT_CHANNEL_DOMAIN_2G_FCC1 = 0x02,		/* US */
+	RT_CHANNEL_DOMAIN_2G_MKK1 = 0x03,		/* Japan */
+	RT_CHANNEL_DOMAIN_2G_ETSI2 = 0x04,		/* France */
+	RT_CHANNEL_DOMAIN_2G_NULL = 0x05,
+	/*  Add new channel plan above this line=============== */
+	RT_CHANNEL_DOMAIN_2G_MAX,
+};
+
+#define rtw_is_channel_plan_valid(chplan)			\
+	(chplan < RT_CHANNEL_DOMAIN_MAX ||			\
+	 chplan == RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+
+struct rt_channel_plan {
+	unsigned char	Channel[MAX_CHANNEL_NUM];
+	unsigned char	Len;
+};
+
+struct rt_channel_plan_map {
+	unsigned char	Index2G;
+};
+
+enum Associated_AP {
+	atherosAP	= 0,
+	broadcomAP	= 1,
+	ciscoAP		= 2,
+	marvellAP	= 3,
+	ralinkAP	= 4,
+	realtekAP	= 5,
+	airgocapAP	= 6,
+	unknownAP	= 7,
+	maxAP,
+};
+
+enum HT_IOT_PEER {
+	HT_IOT_PEER_UNKNOWN		= 0,
+	HT_IOT_PEER_REALTEK		= 1,
+	HT_IOT_PEER_REALTEK_92SE	= 2,
+	HT_IOT_PEER_BROADCOM		= 3,
+	HT_IOT_PEER_RALINK		= 4,
+	HT_IOT_PEER_ATHEROS		= 5,
+	HT_IOT_PEER_CISCO		= 6,
+	HT_IOT_PEER_MERU		= 7,
+	HT_IOT_PEER_MARVELL		= 8,
+	HT_IOT_PEER_REALTEK_SOFTAP	= 9,/*  peer is RealTek SOFT_AP */
+	HT_IOT_PEER_SELF_SOFTAP		= 10, /*  Self is SoftAP */
+	HT_IOT_PEER_AIRGO		= 11,
+	HT_IOT_PEER_INTEL		= 12,
+	HT_IOT_PEER_RTK_APCLIENT	= 13,
+	HT_IOT_PEER_REALTEK_81XX	= 14,
+	HT_IOT_PEER_REALTEK_WOW		= 15,
+	HT_IOT_PEER_TENDA		= 16,
+	HT_IOT_PEER_MAX			= 17
+};
+
+enum SCAN_STATE {
+	SCAN_DISABLE = 0,
+	SCAN_START = 1,
+	SCAN_TXNULL = 2,
+	SCAN_PROCESS = 3,
+	SCAN_COMPLETE = 4,
+	SCAN_STATE_MAX,
+};
+
+typedef unsigned int (*mlme_handler)(struct adapter *adapt, struct recv_frame *frame);
+
+struct	ss_res {
+	int	state;
+	int	bss_cnt;
+	int	channel_idx;
+	int	scan_mode;
+	u8 ssid_num;
+	u8 ch_num;
+	struct ndis_802_11_ssid ssid[RTW_SSID_SCAN_AMOUNT];
+	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
+};
+
+/* define AP_MODE				0x0C */
+/* define STATION_MODE	0x08 */
+/* define AD_HOC_MODE		0x04 */
+/* define NO_LINK_MODE	0x00 */
+
+#define WIFI_FW_NULL_STATE		_HW_STATE_NOLINK_
+#define	WIFI_FW_STATION_STATE		_HW_STATE_STATION_
+#define	WIFI_FW_AP_STATE		_HW_STATE_AP_
+#define	WIFI_FW_ADHOC_STATE		_HW_STATE_ADHOC_
+
+#define	WIFI_FW_AUTH_NULL		0x00000100
+#define	WIFI_FW_AUTH_STATE		0x00000200
+#define	WIFI_FW_AUTH_SUCCESS		0x00000400
+
+#define	WIFI_FW_ASSOC_STATE		0x00002000
+#define	WIFI_FW_ASSOC_SUCCESS		0x00004000
+
+#define	WIFI_FW_LINKING_STATE		(WIFI_FW_AUTH_NULL |		\
+					WIFI_FW_AUTH_STATE |		\
+					WIFI_FW_AUTH_SUCCESS |		\
+					WIFI_FW_ASSOC_STATE)
+
+struct FW_Sta_Info {
+	struct sta_info	*psta;
+	u32	status;
+	u32	rx_pkt;
+	u32	retry;
+	unsigned char SupportedRates[NDIS_802_11_LENGTH_RATES_EX];
+};
+
+/*
+ * Usage:
+ * When one iface acted as AP mode and the other iface is STA mode and scanning,
+ * it should switch back to AP's operating channel periodically.
+ * Parameters info:
+ * When the driver scanned RTW_SCAN_NUM_OF_CH channels, it would switch back to
+ * AP's operating channel for
+ * RTW_STAY_AP_CH_MILLISECOND * SURVEY_TO milliseconds.
+ * Example:
+ * For chip supports 2.4G + 5GHz and AP mode is operating in channel 1,
+ * RTW_SCAN_NUM_OF_CH is 8, RTW_STAY_AP_CH_MS is 3 and SURVEY_TO is 100.
+ * When it's STA mode gets set_scan command,
+ * it would
+ * 1. Doing the scan on channel 1.2.3.4.5.6.7.8
+ * 2. Back to channel 1 for 300 milliseconds
+ * 3. Go through doing site survey on channel 9.10.11.36.40.44.48.52
+ * 4. Back to channel 1 for 300 milliseconds
+ * 5. ... and so on, till survey done.
+ */
+
+struct mlme_ext_info {
+	u32	state;
+	u32	reauth_count;
+	u32	reassoc_count;
+	u32	link_count;
+	u32	auth_seq;
+	u32	auth_algo;	/*  802.11 auth, could be open, shared, auto */
+	u32	authModeToggle;
+	u32	enc_algo;/* encrypt algorithm; */
+	u32	key_index;	/*  this is only valid for legacy wep,
+				 *  0~3 for key id. */
+	u32	iv;
+	u8	chg_txt[128];
+	u16	aid;
+	u16	bcn_interval;
+	u16	capability;
+	u8	assoc_AP_vendor;
+	u8	slotTime;
+	u8	preamble_mode;
+	u8	WMM_enable;
+	u8	ERP_enable;
+	u8	ERP_IE;
+	u8	HT_enable;
+	u8	HT_caps_enable;
+	u8	HT_info_enable;
+	u8	HT_protection;
+	u8	turboMode_cts2self;
+	u8	turboMode_rtsen;
+	u8	SM_PS;
+	u8	agg_enable_bitmap;
+	u8	ADDBA_retry_count;
+	u8	candidate_tid_bitmap;
+	u8	dialogToken;
+	/*  Accept ADDBA Request */
+	bool bAcceptAddbaReq;
+	u8	bwmode_updated;
+	u8	hidden_ssid_mode;
+
+	struct ADDBA_request	ADDBA_req;
+	struct WMM_para_element	WMM_param;
+	struct HT_caps_element	HT_caps;
+	struct HT_info_element	HT_info;
+	struct wlan_bssid_ex	network;/* join network or bss_network,
+					 * if in ap mode, it is the same
+					 * as cur_network.network */
+	struct FW_Sta_Info	FW_sta_info[NUM_STA];
+};
+
+/*  The channel information about this channel including joining,
+ *  scanning, and power constraints. */
+struct rt_channel_info {
+	u8	ChannelNum;	/*  The channel number. */
+	enum rt_scan_type ScanType;	/*  Scan type such as passive
+					 *  or active scan. */
+	u32	rx_count;
+};
+
+int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch);
+
+/*  P2P_MAX_REG_CLASSES - Maximum number of regulatory classes */
+#define P2P_MAX_REG_CLASSES 10
+
+/*  P2P_MAX_REG_CLASS_CHANNELS - Maximum number of chan per regulatory class */
+#define P2P_MAX_REG_CLASS_CHANNELS 20
+
+/*   struct p2p_channels - List of supported channels */
+struct p2p_channels {
+	/*  struct p2p_reg_class - Supported regulatory class */
+	struct p2p_reg_class {
+		/*  reg_class - Regulatory class (IEEE 802.11-2007, Annex J) */
+		u8 reg_class;
+
+		/*  channel - Supported channels */
+		u8 channel[P2P_MAX_REG_CLASS_CHANNELS];
+
+		/*  channels - Number of channel entries in use */
+		size_t channels;
+	} reg_class[P2P_MAX_REG_CLASSES];
+
+	/*  reg_classes - Number of reg_class entries in use */
+	size_t reg_classes;
+};
+
+struct p2p_oper_class_map {
+	enum hw_mode {IEEE80211G} mode;
+	u8 op_class;
+	u8 min_chan;
+	u8 max_chan;
+	u8 inc;
+	enum {BW20, BW40PLUS, BW40MINUS} bw;
+};
+
+struct mlme_ext_priv {
+	struct adapter	*padapter;
+	u8	mlmeext_init;
+	atomic_t event_seq;
+	u16	mgnt_seq;
+
+	unsigned char	cur_channel;
+	unsigned char	cur_bwmode;
+	unsigned char	cur_ch_offset;/* PRIME_CHNL_OFFSET */
+	unsigned char	cur_wireless_mode;	/*  NETWORK_TYPE */
+
+	unsigned char	oper_channel; /* saved chan info when call
+				       * set_channel_bw */
+	unsigned char	oper_bwmode;
+	unsigned char	oper_ch_offset;/* PRIME_CHNL_OFFSET */
+
+	unsigned char	max_chan_nums;
+	struct rt_channel_info channel_set[MAX_CHANNEL_NUM];
+	struct p2p_channels channel_list;
+	unsigned char	basicrate[NumRates];
+	unsigned char	datarate[NumRates];
+
+	struct ss_res		sitesurvey_res;
+	struct mlme_ext_info	mlmext_info;/* for sta/adhoc mode, including
+					     * current scan/connecting/connected
+					     * related info. For ap mode,
+					     * network includes ap's cap_info*/
+	struct timer_list survey_timer;
+	struct timer_list link_timer;
+	u16	chan_scan_time;
+
+	u8	scan_abort;
+	u8	tx_rate; /*  TXRATE when USERATE is set. */
+
+	u32	retry; /* retry for issue probereq */
+
+	u64 TSFValue;
+
+	unsigned char bstart_bss;
+	u8 update_channel_plan_by_ap_done;
+	/* recv_decache check for Action_public frame */
+	u8 action_public_dialog_token;
+	u16	 action_public_rxseq;
+	u8 active_keep_alive_check;
+};
+
+void init_mlme_ext_priv(struct adapter *adapter);
+int init_hw_mlme_ext(struct adapter *padapter);
+void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext);
+extern struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv);
+
+unsigned char networktype_to_raid(unsigned char network_type);
+u8 judge_network_type(struct adapter *padapter, unsigned char *rate, int len);
+void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *len);
+
+void Save_DM_Func_Flag(struct adapter *padapter);
+void Restore_DM_Func_Flag(struct adapter *padapter);
+
+void Set_MSR(struct adapter *padapter, u8 type);
+
+u8 rtw_get_oper_ch(struct adapter *adapter);
+void rtw_set_oper_ch(struct adapter *adapter, u8 ch);
+void rtw_set_oper_bw(struct adapter *adapter, u8 bw);
+void rtw_set_oper_choffset(struct adapter *adapter, u8 offset);
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel,
+			unsigned char channel_offset, unsigned short bwmode);
+void SelectChannel(struct adapter *padapter, unsigned char channel);
+void SetBWMode(struct adapter *padapter, unsigned short bwmode,
+	       unsigned char channel_offset);
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval);
+
+void write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key);
+void clear_cam_entry(struct adapter *padapter, u8 entry);
+
+void invalidate_cam_all(struct adapter *padapter);
+
+int allocate_fw_sta_entry(struct adapter *padapter);
+void flush_all_cam_entry(struct adapter *padapter);
+
+void rtw_mlme_under_site_survey(struct adapter *adapter);
+void rtw_mlme_site_survey_done(struct adapter *adapter);
+
+void site_survey(struct adapter *padapter);
+u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame,
+		    struct wlan_bssid_ex *bssid);
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+		    struct adapter *adapter, bool update_ie);
+
+u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork);
+u16 get_beacon_interval(struct wlan_bssid_ex *bss);
+
+int is_client_associated_to_ap(struct adapter *padapter);
+int is_client_associated_to_ibss(struct adapter *padapter);
+int is_IBSS_empty(struct adapter *padapter);
+
+unsigned char check_assoc_AP(u8 *pframe, uint len);
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE);
+void WMMOnAssocRsp(struct adapter *padapter);
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE);
+void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE);
+void HTOnAssocRsp(struct adapter *padapter);
+
+void ERP_IE_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE);
+void VCS_update(struct adapter *padapter, struct sta_info *psta);
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint len,
+		        struct sta_info *psta);
+int rtw_check_bcn_info(struct adapter  *Adapter, u8 *pframe, u32 packet_len);
+void update_IOT_info(struct adapter *padapter);
+void update_capinfo(struct adapter *adapter, u16 updatecap);
+void update_wireless_mode(struct adapter *padapter);
+void rtw_set_basic_rate(struct adapter *adapter, u8 *rates);
+void update_tx_basic_rate(struct adapter *padapter, u8 modulation);
+void update_bmc_sta_support_rate(struct adapter *padapter, u32 mac_id);
+int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie,
+			    uint var_ie_len, int cam_idx);
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta);
+unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz);
+unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz);
+unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps);
+void Update_RA_Entry(struct adapter *padapter, u32 mac_id);
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta);
+
+void receive_disconnect(struct adapter *padapter, unsigned char *macaddr, unsigned short reason);
+
+unsigned char get_highest_rate_idx(u32 mask);
+int support_short_GI(struct adapter *padapter, struct HT_caps_element *caps);
+bool is_ap_in_tkip(struct adapter *padapter);
+
+void report_join_res(struct adapter *padapter, int res);
+void report_survey_event(struct adapter *padapter, struct recv_frame *precv_frame);
+void report_surveydone_event(struct adapter *padapter);
+void report_del_sta_event(struct adapter *padapter,
+			  unsigned char *addr, unsigned short reason);
+void report_add_sta_event(struct adapter *padapter, unsigned char* addr,
+			  int cam_idx);
+
+void beacon_timing_control(struct adapter *padapter);
+extern u8 set_tx_beacon_cmd(struct adapter*padapter);
+unsigned int setup_beacon_frame(struct adapter *padapter,
+				unsigned char *beacon_frame);
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate);
+void update_mgntframe_attrib(struct adapter *padapter,
+			     struct pkt_attrib *pattrib);
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe);
+s32 dump_mgntframe_and_wait(struct adapter *padapter,
+			    struct xmit_frame *pmgntframe, int timeout_ms);
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter,
+				struct xmit_frame *pmgntframe);
+
+void issue_probersp_p2p(struct adapter *padapter, unsigned char *da);
+void issue_p2p_provision_request(struct adapter *padapter, u8 *pssid,
+				 u8 ussidlen, u8 *pdev_raddr);
+void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr);
+void issue_probereq_p2p(struct adapter *padapter, u8 *da);
+void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr,
+				   u8 dialogToken, u8 success);
+void issue_p2p_invitation_request(struct adapter *padapter, u8* raddr);
+void issue_beacon(struct adapter *padapter, int timeout_ms);
+void issue_probersp(struct adapter *padapter, unsigned char *da,
+		    u8 is_valid_p2p_probereq);
+void issue_assocreq(struct adapter *padapter);
+void issue_asocrsp(struct adapter *padapter, unsigned short status,
+		   struct sta_info *pstat, int pkt_type);
+void issue_auth(struct adapter *padapter, struct sta_info *psta,
+		unsigned short status);
+void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid,
+		    u8 *da);
+s32 issue_probereq_ex(struct adapter *adapter, struct ndis_802_11_ssid *pssid,
+		      u8* da, int try_cnt, int wait_ms);
+int issue_nulldata(struct adapter *padapter, unsigned char *da,
+		   unsigned int power_mode, int try_cnt, int wait_ms);
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da,
+		       u16 tid, int try_cnt, int wait_ms);
+int issue_deauth(struct adapter *padapter, unsigned char *da,
+		 unsigned short reason);
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason,
+		    int try_cnt, int wait_ms);
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, u8 action, u16 status);
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr);
+unsigned int send_beacon(struct adapter *padapter);
+bool get_beacon_valid_bit(struct adapter *adapter);
+void clear_beacon_valid_bit(struct adapter *adapter);
+void rtw_resume_tx_beacon(struct adapter *adapt);
+void rtw_stop_tx_beacon(struct adapter *adapt);
+
+void start_clnt_assoc(struct adapter *padapter);
+void start_clnt_auth(struct adapter *padapter);
+void start_clnt_join(struct adapter *padapter);
+void start_create_ibss(struct adapter *padapter);
+
+unsigned int OnAssocReq(struct adapter *padapter,
+			struct recv_frame *precv_frame);
+unsigned int OnAssocRsp(struct adapter *padapter,
+			struct recv_frame *precv_frame);
+unsigned int OnProbeReq(struct adapter *padapter,
+			struct recv_frame *precv_frame);
+unsigned int OnProbeRsp(struct adapter *padapter,
+			struct recv_frame *precv_frame);
+unsigned int OnBeacon(struct adapter *padapter,
+		      struct recv_frame *precv_frame);
+unsigned int OnDisassoc(struct adapter *padapter,
+			struct recv_frame *precv_frame);
+unsigned int OnAuth(struct adapter *padapter,
+		    struct recv_frame *precv_frame);
+unsigned int OnAuthClient(struct adapter *padapter,
+			  struct recv_frame *precv_frame);
+unsigned int OnDeAuth(struct adapter *padapter,
+		      struct recv_frame *precv_frame);
+unsigned int OnAction(struct adapter *padapter,
+		      struct recv_frame *precv_frame);
+
+unsigned int OnAction_back(struct adapter *padapter,
+			   struct recv_frame *precv_frame);
+unsigned int on_action_public(struct adapter *padapter,
+			      struct recv_frame *precv_frame);
+unsigned int OnAction_p2p(struct adapter *padapter,
+			  struct recv_frame *precv_frame);
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res);
+void mlmeext_sta_del_event_callback(struct adapter *padapter);
+void mlmeext_sta_add_event_callback(struct adapter *padapter,
+				    struct sta_info *psta);
+
+void linked_status_chk(struct adapter *padapter);
+
+void survey_timer_hdl (struct adapter *padapter);
+void link_timer_hdl (struct adapter *padapter);
+void addba_timer_hdl(struct sta_info *psta);
+
+#define set_survey_timer(mlmeext, ms) \
+	do { \
+		_set_timer(&(mlmeext)->survey_timer, (ms)); \
+	} while (0)
+
+#define set_link_timer(mlmeext, ms) \
+	do { \
+		_set_timer(&(mlmeext)->link_timer, (ms)); \
+	} while (0)
+
+bool cckrates_included(unsigned char *rate, int ratelen);
+bool cckratesonly_included(unsigned char *rate, int ratelen);
+
+struct cmd_hdl {
+	uint	parmsize;
+	u8 (*h2cfuns)(struct adapter  *padapter, u8 *pbuf);
+};
+
+u8 read_macreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_macreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 read_bbreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_bbreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 read_rfreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_rfreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf);
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf);
+u8 disconnect_hdl(struct adapter *padapter, u8 *pbuf);
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf);
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setauth_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_assocsta_hdl(struct adapter *padapter, u8 *pbuf);
+u8 del_assocsta_hdl(struct adapter *padapter, u8 *pbuf);
+u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf);
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf);
+/* Handling DFS channel switch announcement ie. */
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf);
+
+#define GEN_DRV_CMD_HANDLER(size, cmd)	{size, &cmd ## _hdl},
+#define GEN_MLME_EXT_HANDLER(size, cmd)	{size, cmd},
+
+#ifdef _RTW_CMD_C_
+
+static struct cmd_hdl wlancmds[] = {
+	GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*10*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct joinbss_parm), join_cmd_hdl) /*14*/
+	GEN_MLME_EXT_HANDLER(sizeof (struct disconnect_parm), disconnect_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof (struct createbss_parm), createbss_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setopmode_parm), setopmode_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof (struct sitesurvey_parm),
+			     sitesurvey_cmd_hdl) /*18*/
+	GEN_MLME_EXT_HANDLER(sizeof (struct setauth_parm), setauth_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setkey_parm), setkey_hdl) /*20*/
+	GEN_MLME_EXT_HANDLER(sizeof (struct set_stakey_parm), set_stakey_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof (struct set_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct del_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setstapwrstate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct getbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct getdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct setphyinfo_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct getphyinfo_parm), NULL)  /*30*/
+	GEN_MLME_EXT_HANDLER(sizeof (struct setphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof (struct getphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)	/*40*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl) /* 46 */
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*50*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct Tx_Beacon_param),
+			     tx_beacon_hdl) /*55*/
+
+	GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl) /*56*/
+	GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl) /*57*/
+
+	GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl) /*58*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param),
+			     set_chplan_hdl) /*59*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct LedBlink_param),
+			     led_blink_hdl) /*60*/
+
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param),
+			     set_csa_hdl) /*61*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param),
+			     tdls_hdl) /*62*/
+};
+
+#endif
+
+struct C2HEvent_Header {
+#ifdef __LITTLE_ENDIAN
+	unsigned int len:16;
+	unsigned int ID:8;
+	unsigned int seq:8;
+#elif defined(__BIG_ENDIAN)
+	unsigned int seq:8;
+	unsigned int ID:8;
+	unsigned int len:16;
+#endif
+	unsigned int rsvd;
+};
+
+enum rtw_c2h_event {
+	GEN_EVT_CODE(_Read_MACREG) = 0, /*0*/
+	GEN_EVT_CODE(_Read_BBREG),
+	GEN_EVT_CODE(_Read_RFREG),
+	GEN_EVT_CODE(_Read_EEPROM),
+	GEN_EVT_CODE(_Read_EFUSE),
+	GEN_EVT_CODE(_Read_CAM),	/*5*/
+	GEN_EVT_CODE(_Get_BasicRate),
+	GEN_EVT_CODE(_Get_DataRate),
+	GEN_EVT_CODE(_Survey),	 /*8*/
+	GEN_EVT_CODE(_SurveyDone),	 /*9*/
+
+	GEN_EVT_CODE(_JoinBss) , /*10*/
+	GEN_EVT_CODE(_AddSTA),
+	GEN_EVT_CODE(_DelSTA),
+	GEN_EVT_CODE(_AtimDone),
+	GEN_EVT_CODE(_TX_Report),
+	GEN_EVT_CODE(_CCX_Report),		/*15*/
+	GEN_EVT_CODE(_DTM_Report),
+	GEN_EVT_CODE(_TX_Rate_Statistics),
+	GEN_EVT_CODE(_C2HLBK),
+	GEN_EVT_CODE(_FWDBG),
+	GEN_EVT_CODE(_C2HFEEDBACK),             /*20*/
+	GEN_EVT_CODE(_ADDBA),
+	GEN_EVT_CODE(_C2HBCN),
+	GEN_EVT_CODE(_ReportPwrState),	/* filen: only for PCIE, USB */
+	GEN_EVT_CODE(_CloseRF),		/* filen: only for PCIE,
+					 * work around ASPM */
+	MAX_C2HEVT
+};
+
+#ifdef _RTW_MLME_EXT_C_
+
+static struct fwevent wlanevents[] = {
+	{0, NULL},	/*0*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, &rtw_survey_event_callback},		/*8*/
+	{sizeof (struct surveydone_event), &rtw_surveydone_event_callback},/*9*/
+	{0, &rtw_joinbss_event_callback},		/*10*/
+	{sizeof(struct stassoc_event), &rtw_stassoc_event_callback},
+	{sizeof(struct stadel_event), &rtw_stadel_event_callback},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},	/*15*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},	 /*20*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+};
+
+#endif/* _RTL_MLME_EXT_C_ */
+
+#endif /* __RTW_MLME_EXT_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_p2p.h b/drivers/staging/r8188eu/include/rtw_p2p.h
new file mode 100644
index 000000000..b91322a1f
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_p2p.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_P2P_H_
+#define __RTW_P2P_H_
+
+#include "drv_types.h"
+
+u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
+u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo,
+				   u8 *pbuf, u8 *pssid, u8 ussidlen,
+				   u8 *pdev_raddr);
+u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo,
+			    u8 *pbuf, u8 status_code);
+u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo,
+			     u8 *pframe, uint len);
+u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo,
+			     u8 *pframe, uint len, struct sta_info *psta);
+u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo,
+			    u8 *pframe, uint len);
+u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo,
+			     u8 *pframe, uint len);
+u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo,
+			    u8 *pframe, uint len);
+u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo,  u8 *pframe);
+u8 process_p2p_group_negotation_req(struct wifidirect_info *pwdinfo,
+				    u8 *pframe, uint len);
+u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo,
+				     u8 *pframe, uint len);
+u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo,
+					u8 *pframe, uint len);
+u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe,
+			    uint len);
+void p2p_protocol_wk_hdl(struct adapter *padapter, int intcmdtype);
+void process_p2p_ps_ie(struct adapter *padapter, u8 *ies, u32 ielength);
+void p2p_ps_wk_hdl(struct adapter *padapter, u8 p2p_ps_state);
+u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue);
+void reset_global_wifidirect_info(struct adapter *padapter);
+int rtw_init_wifi_display_info(struct adapter *padapter);
+void rtw_init_wifidirect_timers(struct adapter *padapter);
+void rtw_init_wifidirect_addrs(struct adapter *padapter, u8 *dev_addr,
+			       u8 *iface_addr);
+void init_wifidirect_info(struct adapter *padapter, enum P2P_ROLE role);
+int rtw_p2p_enable(struct adapter *padapter, enum P2P_ROLE role);
+
+static inline void _rtw_p2p_set_state(struct wifidirect_info *wdinfo,
+				      enum P2P_STATE state)
+{
+	if (wdinfo->p2p_state != state)
+		wdinfo->p2p_state = state;
+}
+
+static inline void _rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo,
+					  enum P2P_STATE state)
+{
+	if (wdinfo->pre_p2p_state != state)
+		wdinfo->pre_p2p_state = state;
+}
+
+static inline void _rtw_p2p_set_role(struct wifidirect_info *wdinfo,
+				     enum P2P_ROLE role)
+{
+	if (wdinfo->role != role)
+		wdinfo->role = role;
+}
+
+static inline int _rtw_p2p_state(struct wifidirect_info *wdinfo)
+{
+	return wdinfo->p2p_state;
+}
+
+static inline int _rtw_p2p_pre_state(struct wifidirect_info *wdinfo)
+{
+	return wdinfo->pre_p2p_state;
+}
+
+static inline int _rtw_p2p_role(struct wifidirect_info *wdinfo)
+{
+	return wdinfo->role;
+}
+
+static inline bool _rtw_p2p_chk_state(struct wifidirect_info *wdinfo,
+				      enum P2P_STATE state)
+{
+	return wdinfo->p2p_state == state;
+}
+
+static inline bool _rtw_p2p_chk_role(struct wifidirect_info *wdinfo,
+				     enum P2P_ROLE role)
+{
+	return wdinfo->role == role;
+}
+
+#define rtw_p2p_set_state(wdinfo, state) _rtw_p2p_set_state(wdinfo, state)
+#define rtw_p2p_set_pre_state(wdinfo, state)				\
+	 _rtw_p2p_set_pre_state(wdinfo, state)
+#define rtw_p2p_set_role(wdinfo, role) _rtw_p2p_set_role(wdinfo, role)
+
+#define rtw_p2p_state(wdinfo) _rtw_p2p_state(wdinfo)
+#define rtw_p2p_pre_state(wdinfo) _rtw_p2p_pre_state(wdinfo)
+#define rtw_p2p_role(wdinfo) _rtw_p2p_role(wdinfo)
+#define rtw_p2p_chk_state(wdinfo, state) _rtw_p2p_chk_state(wdinfo, state)
+#define rtw_p2p_chk_role(wdinfo, role) _rtw_p2p_chk_role(wdinfo, role)
+
+#define rtw_p2p_findphase_ex_set(wdinfo, value) \
+	((wdinfo)->find_phase_state_exchange_cnt = (value))
+
+/* is this find phase exchange for social channel scan? */
+#define rtw_p2p_findphase_ex_is_social(wdinfo)   \
+((wdinfo)->find_phase_state_exchange_cnt >= P2P_FINDPHASE_EX_SOCIAL_FIRST)
+
+/* should we need find phase exchange anymore? */
+#define rtw_p2p_findphase_ex_is_needed(wdinfo) \
+	((wdinfo)->find_phase_state_exchange_cnt < P2P_FINDPHASE_EX_MAX && \
+	(wdinfo)->find_phase_state_exchange_cnt != P2P_FINDPHASE_EX_NONE)
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_pwrctrl.h b/drivers/staging/r8188eu/include/rtw_pwrctrl.h
new file mode 100644
index 000000000..6e9fdd66f
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_pwrctrl.h
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#ifndef __RTW_PWRCTRL_H_
+#define __RTW_PWRCTRL_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define XMIT_ALIVE	BIT(0)
+#define RECV_ALIVE	BIT(1)
+#define CMD_ALIVE	BIT(2)
+#define EVT_ALIVE	BIT(3)
+
+enum power_mgnt {
+	PS_MODE_ACTIVE = 0,
+	PS_MODE_MIN,
+	PS_MODE_MAX,
+	PS_MODE_DTIM,
+	PS_MODE_VOIP,
+	PS_MODE_UAPSD_WMM,
+	PM_Card_Disable,
+	PS_MODE_NUM
+};
+
+#define LPS_DELAY_TIME	1*HZ /*  1 sec */
+
+/*  RF state. */
+enum rt_rf_power_state {
+	rf_on,		/*  RF is on after RFSleep or RFOff */
+	rf_sleep,	/*  802.11 Power Save mode */
+	rf_off,		/*  HW/SW Radio OFF or Inactive Power Save */
+	/* Add the new RF state above this line===== */
+	rf_max
+};
+
+enum { /*  for ips_mode */
+	IPS_NONE = 0,
+	IPS_NORMAL,
+	IPS_LEVEL_2,
+};
+
+struct pwrctrl_priv {
+	struct mutex lock; /* Mutex used to protect struct pwrctrl_priv */
+
+	u8	pwr_mode;
+	u8	smart_ps;
+	u8	bcn_ant_mode;
+
+	bool	bpower_saving;
+
+	uint	ips_enter_cnts;
+	uint	ips_leave_cnts;
+
+	u8	ips_mode;
+	u8	ips_mode_req;	/*  used to accept the mode setting request,
+				 *  will update to ipsmode later */
+	uint bips_processing;
+	unsigned long ips_deny_time; /* will deny IPS when system time less than this */
+	u8 ps_processing; /* temp used to mark whether in rtw_ps_processor */
+
+	u8	bLeisurePs;
+	u8	LpsIdleCount;
+	u8	power_mgnt;
+	u8	bFwCurrentInPSMode;
+	u32	DelayLPSLastTimeStamp;
+
+	u8		bInSuspend;
+	u8		bSupportRemoteWakeup;
+	struct timer_list pwr_state_check_timer;
+	int		pwr_state_check_interval;
+
+	enum rt_rf_power_state	rf_pwrstate;/* cur power state */
+	enum rt_rf_power_state	change_rfpwrstate;
+
+	u8		bkeepfwalive;
+};
+
+#define rtw_get_ips_mode_req(pwrctrlpriv) \
+	(pwrctrlpriv)->ips_mode_req
+
+#define rtw_ips_mode_req(pwrctrlpriv, ips_mode) \
+	((pwrctrlpriv)->ips_mode_req = (ips_mode))
+
+#define RTW_PWR_STATE_CHK_INTERVAL 2000
+
+#define _rtw_set_pwr_state_check_timer(pwrctrlpriv, ms) \
+	do { \
+		_set_timer(&(pwrctrlpriv)->pwr_state_check_timer, (ms)); \
+	} while (0)
+
+#define rtw_set_pwr_state_check_timer(pwrctrl)			\
+	_rtw_set_pwr_state_check_timer((pwrctrl),		\
+				       (pwrctrl)->pwr_state_check_interval)
+
+void rtw_init_pwrctrl_priv(struct adapter *adapter);
+
+void rtw_set_firmware_ps_mode(struct adapter *adapter, u8 mode);
+void rtw_set_ps_mode(struct adapter *adapter, u8 ps_mode, u8 smart_ps,
+		     u8 bcn_ant_mode);
+void LeaveAllPowerSaveMode(struct adapter *adapter);
+void ips_enter(struct adapter *padapter);
+int ips_leave(struct adapter *padapter);
+
+void rtw_ps_processor(struct adapter *padapter);
+
+void LPS_Enter(struct adapter *adapter);
+void LPS_Leave(struct adapter *adapter);
+
+int rtw_pwr_wakeup(struct adapter *adapter);
+int rtw_pm_set_ips(struct adapter *adapter, u8 mode);
+int rtw_pm_set_lps(struct adapter *adapter, u8 mode);
+
+#endif  /* __RTL871X_PWRCTRL_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_recv.h b/drivers/staging/r8188eu/include/rtw_recv.h
new file mode 100644
index 000000000..7768b0c59
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_recv.h
@@ -0,0 +1,347 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#ifndef _RTW_RECV_H_
+#define _RTW_RECV_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define NR_RECVFRAME 256
+
+#define RXFRAME_ALIGN	8
+#define RXFRAME_ALIGN_SZ	(1<<RXFRAME_ALIGN)
+
+#define MAX_RXFRAME_CNT	512
+#define MAX_RX_NUMBLKS		(32)
+#define RECVFRAME_HDR_ALIGN 128
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define MAX_SUBFRAME_COUNT	64
+
+#define LLC_HEADER_SIZE 6
+
+/* for Rx reordering buffer control */
+struct recv_reorder_ctrl {
+	struct adapter	*padapter;
+	u8 enable;
+	u16 indicate_seq;/* wstart_b, init_value=0xffff */
+	u16 wend_b;
+	u8 wsize_b;
+	struct __queue pending_recvframe_queue;
+	struct timer_list reordering_ctrl_timer;
+};
+
+struct	stainfo_rxcache	{
+	u16	tid_rxseq[16];
+/*
+	unsigned short	tid0_rxseq;
+	unsigned short	tid1_rxseq;
+	unsigned short	tid2_rxseq;
+	unsigned short	tid3_rxseq;
+	unsigned short	tid4_rxseq;
+	unsigned short	tid5_rxseq;
+	unsigned short	tid6_rxseq;
+	unsigned short	tid7_rxseq;
+	unsigned short	tid8_rxseq;
+	unsigned short	tid9_rxseq;
+	unsigned short	tid10_rxseq;
+	unsigned short	tid11_rxseq;
+	unsigned short	tid12_rxseq;
+	unsigned short	tid13_rxseq;
+	unsigned short	tid14_rxseq;
+	unsigned short	tid15_rxseq;
+*/
+};
+
+struct signal_stat {
+	u8	update_req;		/* used to indicate */
+	u8	avg_val;		/* avg of valid elements */
+	u32	total_num;		/* num of valid elements */
+	u32	total_val;		/* sum of valid elements */
+};
+#define MAX_PATH_NUM_92CS		3
+struct phy_info {
+	u8	RxPWDBAll;
+	u8	SignalQuality;	 /*  in 0-100 index. */
+	u8	RxMIMOSignalStrength[MAX_PATH_NUM_92CS];/*  in 0~100 index */
+	s8	RxPower; /*  in dBm Translate from PWdB */
+/*  Real power in dBm for this packet, no beautification and aggregation.
+ * Keep this raw info to be used for the other procedures. */
+	s8	recvpower;
+	u8	SignalStrength; /*  in 0-100 index. */
+	u8	RxPwr[MAX_PATH_NUM_92CS];/* per-path's pwdb */
+};
+
+struct rx_pkt_attrib {
+	u16	pkt_len;
+	u8	physt;
+	u8	drvinfo_sz;
+	u8	shift_sz;
+	u8	hdrlen; /* the WLAN Header Len */
+	u8	amsdu;
+	bool	qos;
+	u8	priority;
+	u8	pw_save;
+	u8	mdata;
+	u16	seq_num;
+	u8	frag_num;
+	u8	mfrag;
+	u8	order;
+	u8	privacy; /* in frame_ctrl field */
+	u8	bdecrypted;
+	u8	encrypt; /* when 0 indicate no encrypt. when non-zero,
+			  * indicate the encrypt algorith */
+	u8	iv_len;
+	u8	icv_len;
+	u8	crc_err;
+	u8	icv_err;
+
+	u16 eth_type;
+
+	u8	dst[ETH_ALEN] __aligned(2);
+	u8	src[ETH_ALEN] __aligned(2);
+	u8	ta[ETH_ALEN] __aligned(2);
+	u8	ra[ETH_ALEN] __aligned(2);
+	u8	bssid[ETH_ALEN] __aligned(2);
+
+	u8 ack_policy;
+
+	u8	key_index;
+
+	u8	mcs_rate;
+	u8	rxht;
+	u8	sgi;
+	u8	pkt_rpt_type;
+	u32	MacIDValidEntry[2];	/*  64 bits present 64 entry. */
+
+	struct phy_info phy_info;
+};
+
+/* These definition is used for Rx packet reordering. */
+#define SN_LESS(a, b)		(((a - b) & 0x800) != 0)
+#define SN_EQUAL(a, b)	(a == b)
+#define REORDER_WAIT_TIME	(50) /*  (ms) */
+
+#define RECVBUFF_ALIGN_SZ 8
+
+#define RXDESC_SIZE	24
+#define RXDESC_OFFSET RXDESC_SIZE
+
+struct recv_stat {
+	__le32 rxdw0;
+	__le32 rxdw1;
+	__le32 rxdw2;
+	__le32 rxdw3;
+	__le32 rxdw4;
+	__le32 rxdw5;
+};
+
+#define EOR BIT(30)
+
+/*
+accesser of recv_priv: rtw_recv_entry(dispatch / passive level);
+recv_thread(passive) ; returnpkt(dispatch)
+; halt(passive) ;
+
+using enter_critical section to protect
+*/
+struct recv_priv {
+	spinlock_t lock;
+	struct __queue free_recv_queue;
+	struct __queue recv_pending_queue;
+	struct __queue uc_swdec_pending_queue;
+	u8 *pallocated_frame_buf;
+	u8 *precv_frame_buf;
+	uint free_recvframe_cnt;
+	struct adapter	*adapter;
+	u32	bIsAnyNonBEPkts;
+	u64	rx_bytes;
+	u64	rx_pkts;
+	u64	rx_drop;
+	u64	last_rx_bytes;
+
+	uint  rx_icv_err;
+	uint  rx_largepacket_crcerr;
+	uint  rx_smallpacket_crcerr;
+	uint  rx_middlepacket_crcerr;
+	u8	rx_pending_cnt;
+
+	struct tasklet_struct recv_tasklet;
+	struct sk_buff_head free_recv_skb_queue;
+	struct sk_buff_head rx_skb_queue;
+	u8 *pallocated_recv_buf;
+	u8 *precv_buf;    /*  4 alignment */
+	struct __queue free_recv_buf_queue;
+	u32	free_recv_buf_queue_cnt;
+	/* For display the phy informatiom */
+	u8 is_signal_dbg;	/*  for debug */
+	u8 signal_strength_dbg;	/*  for debug */
+	s8 rssi;
+	s8 rxpwdb;
+	u8 signal_strength;
+	u8 signal_qual;
+	u8 noise;
+	int RxSNRdB[2];
+	s8 RxRssi[2];
+	int FalseAlmCnt_all;
+
+	struct timer_list signal_stat_timer;
+	u32 signal_stat_sampling_interval;
+	struct signal_stat signal_qual_data;
+	struct signal_stat signal_strength_data;
+};
+
+#define rtw_set_signal_stat_timer(recvpriv)			\
+	_set_timer(&(recvpriv)->signal_stat_timer,		\
+		   (recvpriv)->signal_stat_sampling_interval)
+
+struct sta_recv_priv {
+	spinlock_t lock;
+	int	option;
+	struct __queue defrag_q; /* keeping the fragment frame until defrag */
+	struct	stainfo_rxcache rxcache;
+};
+
+struct recv_buf {
+	struct adapter *adapter;
+	struct urb *purb;
+	struct sk_buff *pskb;
+	u8	reuse;
+};
+
+/*
+	head  ----->
+
+		data  ----->
+
+			payload
+
+		tail  ----->
+
+	end   ----->
+
+	len = (unsigned int )(tail - data);
+
+*/
+struct recv_frame {
+	struct list_head list;
+	struct sk_buff	 *pkt;
+	struct adapter  *adapter;
+	u8 fragcnt;
+	int frame_tag;
+	struct rx_pkt_attrib attrib;
+	uint  len;
+	u8 *rx_head;
+	u8 *rx_data;
+	u8 *rx_tail;
+	u8 *rx_end;
+	void *precvbuf;
+	struct sta_info *psta;
+	/* for A-MPDU Rx reordering buffer control */
+	struct recv_reorder_ctrl *preorder_ctrl;
+};
+
+int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter);
+void _rtw_free_recv_priv(struct recv_priv *precvpriv);
+s32 rtw_recv_entry(struct recv_frame *precv_frame);
+struct recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue);
+struct recv_frame *rtw_alloc_recvframe(struct __queue *pfree_recv_queue);
+int  rtw_free_recvframe(struct recv_frame *precvframe,
+			struct __queue *pfree_recv_queue);
+int _rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue);
+int rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue);
+void rtw_free_recvframe_queue(struct __queue *pframequeue,
+			      struct __queue *pfree_recv_queue);
+u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter);
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext);
+
+static inline u8 *get_rxmem(struct recv_frame *precvframe)
+{
+	/* always return rx_head... */
+	if (precvframe == NULL)
+		return NULL;
+	return precvframe->rx_head;
+}
+
+static inline u8 *recvframe_pull(struct recv_frame *precvframe, int sz)
+{
+	/*  rx_data += sz; move rx_data sz bytes  hereafter */
+
+	/* used for extract sz bytes from rx_data, update rx_data and return
+	 * the updated rx_data to the caller */
+
+	if (precvframe == NULL)
+		return NULL;
+	precvframe->rx_data += sz;
+	if (precvframe->rx_data > precvframe->rx_tail) {
+		precvframe->rx_data -= sz;
+		return NULL;
+	}
+	precvframe->len -= sz;
+	return precvframe->rx_data;
+}
+
+static inline u8 *recvframe_put(struct recv_frame *precvframe, int sz)
+{
+	/* used for append sz bytes from ptr to rx_tail, update rx_tail
+	 * and return the updated rx_tail to the caller */
+	/* after putting, rx_tail must be still larger than rx_end. */
+
+	if (precvframe == NULL)
+		return NULL;
+
+	precvframe->rx_tail += sz;
+
+	if (precvframe->rx_tail > precvframe->rx_end) {
+		precvframe->rx_tail -= sz;
+		return NULL;
+	}
+	precvframe->len += sz;
+	return precvframe->rx_tail;
+}
+
+static inline u8 *recvframe_pull_tail(struct recv_frame *precvframe, int sz)
+{
+	/*  rmv data from rx_tail (by yitsen) */
+
+	/* used for extract sz bytes from rx_end, update rx_end and return
+	 * the updated rx_end to the caller */
+	/* after pulling, rx_end must be still larger than rx_data. */
+
+	if (precvframe == NULL)
+		return NULL;
+	precvframe->rx_tail -= sz;
+	if (precvframe->rx_tail < precvframe->rx_data) {
+		precvframe->rx_tail += sz;
+		return NULL;
+	}
+	precvframe->len -= sz;
+	return precvframe->rx_tail;
+}
+
+static inline int get_recvframe_len(struct recv_frame *precvframe)
+{
+	return precvframe->len;
+}
+
+static inline s32 translate_percentage_to_dbm(u32 sig_stren_index)
+{
+	s32	power; /*  in dBm. */
+
+	/*  Translate to dBm (x=0.5y-95). */
+	power = (s32)((sig_stren_index + 1) >> 1);
+	power -= 95;
+
+	return power;
+}
+
+struct sta_info;
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv);
+
+void  mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/rtw_rf.h b/drivers/staging/r8188eu/include/rtw_rf.h
new file mode 100644
index 000000000..b7267e753
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_rf.h
@@ -0,0 +1,80 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef	__RTW_RF_H_
+#define __RTW_RF_H_
+
+#include "rtw_cmd.h"
+
+#define NumRates	(13)
+
+/*  slot time for 11g */
+#define SHORT_SLOT_TIME			9
+#define NON_SHORT_SLOT_TIME		20
+
+#define	MAX_CHANNEL_NUM			14	/* 2.4 GHz only */
+
+#define NUM_REGULATORYS	1
+
+struct	regulatory_class {
+	u32	starting_freq;				/* MHz, */
+	u8	channel_set[MAX_CHANNEL_NUM];
+	u8	channel_cck_power[MAX_CHANNEL_NUM];	/* dbm */
+	u8	channel_ofdm_power[MAX_CHANNEL_NUM];	/* dbm */
+	u8	txpower_limit;				/* dbm */
+	u8	channel_spacing;			/* MHz */
+	u8	modem;
+};
+
+enum capability {
+	cESS		= 0x0001,
+	cIBSS		= 0x0002,
+	cPollable	= 0x0004,
+	cPollReq	= 0x0008,
+	cPrivacy	= 0x0010,
+	cShortPreamble	= 0x0020,
+	cPBCC		= 0x0040,
+	cChannelAgility	= 0x0080,
+	cSpectrumMgnt	= 0x0100,
+	cQos		= 0x0200,	/* For HCCA, use with CF-Pollable
+					 * and CF-PollReq */
+	cShortSlotTime	= 0x0400,
+	cAPSD		= 0x0800,
+	cRM		= 0x1000,	/*  RRM (Radio Request Measurement) */
+	cDSSS_OFDM	= 0x2000,
+	cDelayedBA	= 0x4000,
+	cImmediateBA	= 0x8000,
+};
+
+enum	_REG_PREAMBLE_MODE {
+	PREAMBLE_LONG	= 1,
+	PREAMBLE_AUTO	= 2,
+	PREAMBLE_SHORT	= 3,
+};
+
+/*  Bandwidth Offset */
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE	0
+#define HAL_PRIME_CHNL_OFFSET_LOWER	1
+#define HAL_PRIME_CHNL_OFFSET_UPPER	2
+
+/*  Represent Channel Width in HT Capabilities */
+/*  */
+enum ht_channel_width {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_40 = 1,
+};
+
+/*  */
+/*  Represent Extension Channel Offset in HT Capabilities */
+/*  This is available only in 40Mhz mode. */
+/*  */
+enum ht_extchnl_offset {
+	HT_EXTCHNL_OFFSET_NO_EXT = 0,
+	HT_EXTCHNL_OFFSET_UPPER = 1,
+	HT_EXTCHNL_OFFSET_NO_DEF = 2,
+	HT_EXTCHNL_OFFSET_LOWER = 3,
+};
+
+u32 rtw_ch2freq(u32 ch);
+
+#endif /* _RTL8711_RF_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_security.h b/drivers/staging/r8188eu/include/rtw_security.h
new file mode 100644
index 000000000..783ae18a1
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_security.h
@@ -0,0 +1,231 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __RTW_SECURITY_H_
+#define __RTW_SECURITY_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include <crypto/arc4.h>
+
+#define _NO_PRIVACY_			0x0
+#define _WEP40_				0x1
+#define _TKIP_				0x2
+#define _TKIP_WTMIC_			0x3
+#define _AES_				0x4
+#define _WEP104_			0x5
+#define _SMS4_				0x06
+
+#define _WPA_IE_ID_	0xdd
+#define _WPA2_IE_ID_	0x30
+
+enum {
+	ENCRYP_PROTOCOL_OPENSYS,   /* open system */
+	ENCRYP_PROTOCOL_WEP,       /* WEP */
+	ENCRYP_PROTOCOL_WPA,       /* WPA */
+	ENCRYP_PROTOCOL_WPA2,      /* WPA2 */
+	ENCRYP_PROTOCOL_WAPI,      /* WAPI: Not support in this version */
+	ENCRYP_PROTOCOL_MAX
+};
+
+#ifndef Ndis802_11AuthModeWPA2
+#define Ndis802_11AuthModeWPA2 (Ndis802_11AuthModeWPANone + 1)
+#endif
+
+#ifndef Ndis802_11AuthModeWPA2PSK
+#define Ndis802_11AuthModeWPA2PSK (Ndis802_11AuthModeWPANone + 2)
+#endif
+
+union pn48	{
+	u64	val;
+
+#ifdef __LITTLE_ENDIAN
+	struct {
+		u8 TSC0;
+		u8 TSC1;
+		u8 TSC2;
+		u8 TSC3;
+		u8 TSC4;
+		u8 TSC5;
+		u8 TSC6;
+		u8 TSC7;
+	} _byte_;
+
+#elif defined(__BIG_ENDIAN)
+
+	struct {
+		u8 TSC7;
+		u8 TSC6;
+		u8 TSC5;
+		u8 TSC4;
+		u8 TSC3;
+		u8 TSC2;
+		u8 TSC1;
+		u8 TSC0;
+	} _byte_;
+#endif
+};
+
+union Keytype {
+	u8   skey[16];
+	u32    lkey[4];
+};
+
+struct rt_pmkid_list {
+	u8	bUsed;
+	u8	Bssid[6];
+	u8	PMKID[16];
+	u8	SsidBuf[33];
+	u8	*ssid_octet;
+	u16	ssid_length;
+};
+
+struct security_priv {
+	u32	  dot11AuthAlgrthm;	/*  802.11 auth, could be open,
+					 * shared, 8021x and authswitch */
+	u32	  dot11PrivacyAlgrthm;	/*  This specify the privacy for
+					 * shared auth. algorithm. */
+	/* WEP */
+	u32	  dot11PrivacyKeyIndex;	/*  this is only valid for legendary
+					 * wep, 0~3 for key id.(tx key index) */
+	union Keytype dot11DefKey[4];	/*  this is only valid for def. key */
+	u32	dot11DefKeylen[4];
+	u32 dot118021XGrpPrivacy;	/*  This specify the privacy algthm.
+					 * used for Grp key */
+	u32	dot118021XGrpKeyid;	/*  key id used for Grp Key
+					 * ( tx key index) */
+	union Keytype	dot118021XGrpKey[4];	/*  802.1x Group Key,
+						 * for inx0 and inx1 */
+	union Keytype	dot118021XGrptxmickey[4];
+	union Keytype	dot118021XGrprxmickey[4];
+	union pn48	dot11Grptxpn;		/* PN48 used for Grp Key xmit.*/
+	union pn48	dot11Grprxpn;		/* PN48 used for Grp Key recv.*/
+
+	struct arc4_ctx xmit_arc4_ctx;
+	struct arc4_ctx recv_arc4_ctx;
+
+	/* extend security capabilities for AP_MODE */
+	unsigned int dot8021xalg;/* 0:disable, 1:psk, 2:802.1x */
+	unsigned int wpa_psk;/* 0:disable, bit(0): WPA, bit(1):WPA2 */
+	unsigned int wpa_group_cipher;
+	unsigned int wpa2_group_cipher;
+	unsigned int wpa_pairwise_cipher;
+	unsigned int wpa2_pairwise_cipher;
+	u8 wps_ie[MAX_WPS_IE_LEN];/* added in assoc req */
+	int wps_ie_len;
+	u8	binstallGrpkey;
+	u8	busetkipkey;
+	u8	bcheck_grpkey;
+	u8	bgrpkey_handshake;
+	s32	sw_encrypt;/* from registry_priv */
+	s32	sw_decrypt;/* from registry_priv */
+	s32	hw_decrypted;/* if the rx packets is hw_decrypted==false,i
+			      * it means the hw has not been ready. */
+
+	/* keeps the auth_type & enc_status from upper layer
+	 * ioctl(wpa_supplicant or wzc) */
+	u32 ndisauthtype;	/*  NDIS_802_11_AUTHENTICATION_MODE */
+	u32 ndisencryptstatus;	/*  NDIS_802_11_ENCRYPTION_STATUS */
+	struct wlan_bssid_ex sec_bss;  /* for joinbss (h2c buffer) usage */
+	struct ndis_802_11_wep ndiswep;
+	u8 assoc_info[600];
+	u8 szofcapability[256]; /* for wpa2 usage */
+	u8 oidassociation[512]; /* for wpa/wpa2 usage */
+	u8 authenticator_ie[256];  /* store ap security information element */
+	u8 supplicant_ie[256];  /* store sta security information element */
+
+	/* for tkip countermeasure */
+	u32 last_mic_err_time;
+	u8	btkip_countermeasure;
+	u8	btkip_wait_report;
+	u32 btkip_countermeasure_time;
+
+	/*  */
+	/*  For WPA2 Pre-Authentication. */
+	/*  */
+	struct rt_pmkid_list PMKIDList[NUM_PMKID_CACHE];
+	u8	PMKIDIndex;
+	u8 bWepDefaultKeyIdxSet;
+};
+
+#define GET_ENCRY_ALGO(psecuritypriv, psta, encry_algo, bmcst)		\
+do {									\
+	switch (psecuritypriv->dot11AuthAlgrthm) {			\
+	case dot11AuthAlgrthm_Open:					\
+	case dot11AuthAlgrthm_Shared:					\
+	case dot11AuthAlgrthm_Auto:					\
+		encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;	\
+		break;							\
+	case dot11AuthAlgrthm_8021X:					\
+		if (bmcst)						\
+			encry_algo = (u8)psecuritypriv->dot118021XGrpPrivacy;\
+		else							\
+			encry_algo = (u8)psta->dot118021XPrivacy;	\
+		break;							\
+	case dot11AuthAlgrthm_WAPI:					\
+		encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;	\
+		break;							\
+	}								\
+} while (0)
+
+#define SET_ICE_IV_LEN(iv_len, icv_len, encrypt)			\
+do {									\
+	switch (encrypt) {						\
+	case _WEP40_:							\
+	case _WEP104_:							\
+		iv_len = 4;						\
+		icv_len = 4;						\
+		break;							\
+	case _TKIP_:							\
+		iv_len = 8;						\
+		icv_len = 4;						\
+		break;							\
+	case _AES_:							\
+		iv_len = 8;						\
+		icv_len = 8;						\
+		break;							\
+	case _SMS4_:							\
+		iv_len = 18;						\
+		icv_len = 16;						\
+		break;							\
+	default:							\
+		iv_len = 0;						\
+		icv_len = 0;						\
+		break;							\
+	}								\
+} while (0)
+
+#define GET_TKIP_PN(iv, dot11txpn)					\
+do {									\
+	dot11txpn._byte_.TSC0 = iv[2];					\
+	dot11txpn._byte_.TSC1 = iv[0];					\
+	dot11txpn._byte_.TSC2 = iv[4];					\
+	dot11txpn._byte_.TSC3 = iv[5];					\
+	dot11txpn._byte_.TSC4 = iv[6];					\
+	dot11txpn._byte_.TSC5 = iv[7];					\
+} while (0)
+
+#define ROL32(A, n)	(((A) << (n)) | (((A)>>(32-(n)))  & ((1UL << (n)) - 1)))
+#define ROR32(A, n)	ROL32((A), 32-(n))
+
+struct mic_data {
+	u32  K0, K1;         /*  Key */
+	u32  L, R;           /*  Current state */
+	u32  M;              /*  Message accumulator (single word) */
+	u32  nBytesInM;      /*  # bytes in M */
+};
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key);
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b);
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nBytes);
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst);
+void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len,
+			u8 *Miccode, u8   priority);
+u32 rtw_aes_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe);
+u32 rtw_tkip_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe);
+void rtw_wep_encrypt(struct adapter *padapter, struct xmit_frame *pxmitframe);
+u32 rtw_aes_decrypt(struct adapter *padapter, struct recv_frame *precvframe);
+u32 rtw_tkip_decrypt(struct adapter *padapter, struct recv_frame *precvframe);
+void rtw_wep_decrypt(struct adapter *padapter, struct recv_frame *precvframe);
+
+#endif	/* __RTL871X_SECURITY_H_ */
diff --git a/drivers/staging/r8188eu/include/rtw_xmit.h b/drivers/staging/r8188eu/include/rtw_xmit.h
new file mode 100644
index 000000000..82efcd54a
--- /dev/null
+++ b/drivers/staging/r8188eu/include/rtw_xmit.h
@@ -0,0 +1,373 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef _RTW_XMIT_H_
+#define _RTW_XMIT_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define NR_XMITFRAME		256
+#define WMM_XMIT_THRESHOLD	(NR_XMITFRAME * 2 / 5)
+
+#define MAX_XMITBUF_SZ	(20480)	/*  20k */
+#define NR_XMITBUFF		(4)
+
+#define XMITBUF_ALIGN_SZ	4
+
+/*  xmit extension buff defination */
+#define MAX_XMIT_EXTBUF_SZ	(1536)
+#define NR_XMIT_EXTBUFF		(32)
+
+#define MAX_NUMBLKS		(1)
+
+#define XMIT_VO_QUEUE		(0)
+#define XMIT_VI_QUEUE		(1)
+#define XMIT_BE_QUEUE		(2)
+#define XMIT_BK_QUEUE		(3)
+
+#define VO_QUEUE_INX		0
+#define VI_QUEUE_INX		1
+#define BE_QUEUE_INX		2
+#define BK_QUEUE_INX		3
+#define BCN_QUEUE_INX		4
+#define MGT_QUEUE_INX		5
+#define HIGH_QUEUE_INX		6
+#define TXCMD_QUEUE_INX		7
+
+#define HW_QUEUE_ENTRY		8
+
+#define WEP_IV(pattrib_iv, dot11txpn, keyidx)\
+do {\
+	pattrib_iv[0] = dot11txpn._byte_.TSC0;\
+	pattrib_iv[1] = dot11txpn._byte_.TSC1;\
+	pattrib_iv[2] = dot11txpn._byte_.TSC2;\
+	pattrib_iv[3] = ((keyidx & 0x3)<<6);\
+	dot11txpn.val = (dot11txpn.val == 0xffffff) ? 0 : (dot11txpn.val+1);\
+} while (0)
+
+#define TKIP_IV(pattrib_iv, dot11txpn, keyidx)\
+do {\
+	pattrib_iv[0] = dot11txpn._byte_.TSC1;\
+	pattrib_iv[1] = (dot11txpn._byte_.TSC1 | 0x20) & 0x7f;\
+	pattrib_iv[2] = dot11txpn._byte_.TSC0;\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3)<<6);\
+	pattrib_iv[4] = dot11txpn._byte_.TSC2;\
+	pattrib_iv[5] = dot11txpn._byte_.TSC3;\
+	pattrib_iv[6] = dot11txpn._byte_.TSC4;\
+	pattrib_iv[7] = dot11txpn._byte_.TSC5;\
+	dot11txpn.val = dot11txpn.val == 0xffffffffffffULL ? 0 : (dot11txpn.val+1);\
+} while (0)
+
+#define AES_IV(pattrib_iv, dot11txpn, keyidx)\
+do {							\
+	pattrib_iv[0] = dot11txpn._byte_.TSC0;		\
+	pattrib_iv[1] = dot11txpn._byte_.TSC1;		\
+	pattrib_iv[2] = 0;				\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3)<<6);	\
+	pattrib_iv[4] = dot11txpn._byte_.TSC2;		\
+	pattrib_iv[5] = dot11txpn._byte_.TSC3;		\
+	pattrib_iv[6] = dot11txpn._byte_.TSC4;		\
+	pattrib_iv[7] = dot11txpn._byte_.TSC5;		\
+	dot11txpn.val = dot11txpn.val == 0xffffffffffffULL ? 0 : (dot11txpn.val+1);\
+} while (0)
+
+#define HWXMIT_ENTRY	4
+
+#define TXDESC_SIZE 32
+
+#define PACKET_OFFSET_SZ (8)
+#define TXDESC_OFFSET (TXDESC_SIZE + PACKET_OFFSET_SZ)
+
+struct tx_desc {
+	/* DWORD 0 */
+	__le32 txdw0;
+	__le32 txdw1;
+	__le32 txdw2;
+	__le32 txdw3;
+	__le32 txdw4;
+	__le32 txdw5;
+	__le32 txdw6;
+	__le32 txdw7;
+};
+
+union txdesc {
+	struct tx_desc txdesc;
+	unsigned int value[TXDESC_SIZE>>2];
+};
+
+struct	hw_xmit	{
+	struct __queue *sta_queue;
+	int	accnt;
+};
+
+/* reduce size */
+struct pkt_attrib {
+	u8	type;
+	u8	subtype;
+	u8	bswenc;
+	u8	dhcp_pkt;
+	u16	ether_type;
+	u16	seqnum;
+	u16	pkt_hdrlen;	/* the original 802.3 pkt header len */
+	u16	hdrlen;		/* the WLAN Header Len */
+	u32	pktlen;		/* the original 802.3 pkt raw_data len (not include
+				 * ether_hdr data) */
+	u32	last_txcmdsz;
+	u8	nr_frags;
+	u8	encrypt;	/* when 0 indicate no encrypt. when non-zero,
+				 * indicate the encrypt algorith */
+	u8	iv_len;
+	u8	icv_len;
+	u8	iv[18];
+	u8	icv[16];
+	u8	priority;
+	u8	ack_policy;
+	u8	mac_id;
+	u8	vcs_mode;	/* virtual carrier sense method */
+	u8	dst[ETH_ALEN] __aligned(2);
+	u8	src[ETH_ALEN] __aligned(2);
+	u8	ta[ETH_ALEN] __aligned(2);
+	u8	ra[ETH_ALEN] __aligned(2);
+	u8	key_idx;
+	u8	qos_en;
+	u8	ht_en;
+	u8	raid;/* rate adpative id */
+	u8	bwmode;
+	u8	ch_offset;/* PRIME_CHNL_OFFSET */
+	u8	sgi;/* short GI */
+	u8	ampdu_en;/* tx ampdu enable */
+	u8	mdata;/* more data bit */
+	u8	pctrl;/* per packet txdesc control enable */
+	u8	triggered;/* for ap mode handling Power Saving sta */
+	u8	qsel;
+	u8	eosp;
+	u8	rate;
+	u8	intel_proxim;
+	u8	retry_ctrl;
+	struct sta_info *psta;
+};
+
+#define WLANHDR_OFFSET	64
+
+#define NULL_FRAMETAG		(0x0)
+#define DATA_FRAMETAG		0x01
+#define L2_FRAMETAG		0x02
+#define MGNT_FRAMETAG		0x03
+#define AMSDU_FRAMETAG	0x04
+
+#define EII_FRAMETAG		0x05
+#define IEEE8023_FRAMETAG  0x06
+
+#define MP_FRAMETAG		0x07
+
+#define TXAGG_FRAMETAG	0x08
+
+struct  submit_ctx {
+	u32 submit_time; /* */
+	u32 timeout_ms; /* <0: not synchronous, 0: wait forever, >0: up to ms waiting */
+	int status; /* status for operation */
+	struct completion done;
+};
+
+enum {
+	RTW_SCTX_SUBMITTED = -1,
+	RTW_SCTX_DONE_SUCCESS = 0,
+	RTW_SCTX_DONE_UNKNOWN,
+	RTW_SCTX_DONE_TIMEOUT,
+	RTW_SCTX_DONE_BUF_ALLOC,
+	RTW_SCTX_DONE_BUF_FREE,
+	RTW_SCTX_DONE_WRITE_PORT_ERR,
+	RTW_SCTX_DONE_TX_DESC_NA,
+	RTW_SCTX_DONE_TX_DENY,
+	RTW_SCTX_DONE_CCX_PKT_FAIL,
+	RTW_SCTX_DONE_DRV_STOP,
+	RTW_SCTX_DONE_DEV_REMOVE,
+};
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms);
+int rtw_sctx_wait(struct submit_ctx *sctx);
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status);
+
+struct xmit_buf {
+	struct list_head list;
+	struct adapter *padapter;
+	u8 *pallocated_buf;
+	u8 *pbuf;
+	void *priv_data;
+	u16 ext_tag; /*  0: Normal xmitbuf, 1: extension xmitbuf. */
+	u16 flags;
+	u32 alloc_sz;
+	u32  len;
+	struct submit_ctx *sctx;
+	u32	ff_hwaddr;
+	struct urb *pxmit_urb;
+	dma_addr_t dma_transfer_addr;	/* (in) dma addr for transfer_buffer */
+	u8 bpending[8];
+	int last[8];
+};
+
+struct xmit_frame {
+	struct list_head list;
+	struct pkt_attrib attrib;
+	struct sk_buff *pkt;
+	int	frame_tag;
+	struct adapter *padapter;
+	u8	*buf_addr;
+	struct xmit_buf *pxmitbuf;
+
+	u8	agg_num;
+	s8	pkt_offset;
+	u8 ack_report;
+};
+
+struct tx_servq {
+	struct list_head tx_pending;
+	struct __queue sta_pending;
+	int qcnt;
+};
+
+struct sta_xmit_priv {
+	spinlock_t lock;
+	int	option;
+	int	apsd_setting;	/* When bit mask is on, the associated edca
+				 * queue supports APSD. */
+	struct tx_servq	be_q;			/* priority == 0,3 */
+	struct tx_servq	bk_q;			/* priority == 1,2 */
+	struct tx_servq	vi_q;			/* priority == 4,5 */
+	struct tx_servq	vo_q;			/* priority == 6,7 */
+	struct list_head legacy_dz;
+	struct list_head apsd;
+	u16 txseq_tid[16];
+};
+
+struct	hw_txqueue {
+	volatile int	head;
+	volatile int	tail;
+	volatile int	free_sz;	/* in units of 64 bytes */
+	volatile int      free_cmdsz;
+	volatile int	 txsz[8];
+	uint	ff_hwaddr;
+	uint	cmd_hwaddr;
+	int	ac_tag;
+};
+
+struct agg_pkt_info {
+	u16 offset;
+	u16 pkt_len;
+};
+
+struct	xmit_priv {
+	spinlock_t lock;
+	struct semaphore terminate_xmitthread_sema;
+	struct __queue be_pending;
+	struct __queue bk_pending;
+	struct __queue vi_pending;
+	struct __queue vo_pending;
+	struct __queue bm_pending;
+	u8 *pallocated_frame_buf;
+	u8 *pxmit_frame_buf;
+	uint free_xmitframe_cnt;
+	struct __queue free_xmit_queue;
+	uint	frag_len;
+	struct adapter	*adapter;
+	u8   vcs_setting;
+	u8	vcs;
+	u8	vcs_type;
+	u64	tx_bytes;
+	u64	tx_pkts;
+	u64	tx_drop;
+	u64	last_tx_bytes;
+	u64	last_tx_pkts;
+	struct hw_xmit *hwxmits;
+	u8	hwxmit_entry;
+	u8	wmm_para_seq[4];/* sequence for wmm ac parameter strength
+				 * from large to small. it's value is 0->vo,
+				 * 1->vi, 2->be, 3->bk. */
+	struct semaphore tx_retevt;/* all tx return event; */
+	u8		txirp_cnt;/*  */
+	struct tasklet_struct xmit_tasklet;
+	/* per AC pending irp */
+	int beq_cnt;
+	int bkq_cnt;
+	int viq_cnt;
+	int voq_cnt;
+	struct __queue free_xmitbuf_queue;
+	struct __queue pending_xmitbuf_queue;
+	u8 *pallocated_xmitbuf;
+	u8 *pxmitbuf;
+	uint free_xmitbuf_cnt;
+	struct __queue free_xmit_extbuf_queue;
+	u8 *pallocated_xmit_extbuf;
+	u8 *pxmit_extbuf;
+	uint free_xmit_extbuf_cnt;
+	u16	nqos_ssn;
+	int	ack_tx;
+	struct mutex ack_tx_mutex;
+	struct submit_ctx ack_tx_ops;
+};
+
+struct pkt_file {
+	struct sk_buff *pkt;
+	size_t pkt_len;	 /* the remainder length of the open_file */
+	unsigned char *cur_buffer;
+	u8 *buf_start;
+	u8 *cur_addr;
+	size_t buf_len;
+};
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv);
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv,
+			 struct xmit_buf *pxmitbuf);
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv);
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv,
+		     struct xmit_buf *pxmitbuf);
+void rtw_count_tx_stats(struct adapter *padapter,
+			struct xmit_frame *pxmitframe, int sz);
+void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len);
+s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr,
+		     struct pkt_attrib *pattrib);
+s32 rtw_put_snap(u8 *data, u16 h_proto);
+
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv);
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv,
+		       struct xmit_frame *pxmitframe);
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv,
+			      struct __queue *pframequeue);
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter,
+				     struct sta_info *psta, int up, u8 *ac);
+s32 rtw_xmitframe_enqueue(struct adapter *padapter,
+			  struct xmit_frame *pxmitframe);
+struct xmit_frame *rtw_dequeue_xframe(struct xmit_priv *pxmitpriv,
+				      struct hw_xmit *phwxmit_i, int entry);
+
+s32 rtw_xmit_classifier(struct adapter *padapter,
+			struct xmit_frame *pxmitframe);
+s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt,
+			   struct xmit_frame *pxmitframe);
+s32 _rtw_init_hw_txqueue(struct hw_txqueue *phw_txqueue, u8 ac_tag);
+void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv);
+s32 rtw_txframes_pending(struct adapter *padapter);
+s32 rtw_txframes_sta_ac_pending(struct adapter *padapter,
+				struct pkt_attrib *pattrib);
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, int entry);
+s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter);
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv);
+int rtw_alloc_hwxmits(struct adapter *padapter);
+void rtw_free_hwxmits(struct adapter *padapter);
+s32 rtw_xmit(struct adapter *padapter, struct sk_buff **pkt);
+
+int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe);
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta);
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta);
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta);
+
+u8	qos_acm(u8 acm_mask, u8 priority);
+u32	rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe);
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms);
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status);
+
+void rtw_xmit_complete(struct adapter *padapter, struct xmit_frame *pxframe);
+netdev_tx_t rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev);
+
+#endif	/* _RTL871X_XMIT_H_ */
diff --git a/drivers/staging/r8188eu/include/sta_info.h b/drivers/staging/r8188eu/include/sta_info.h
new file mode 100644
index 000000000..4112c837b
--- /dev/null
+++ b/drivers/staging/r8188eu/include/sta_info.h
@@ -0,0 +1,313 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __STA_INFO_H_
+#define __STA_INFO_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wifi.h"
+
+#define IBSS_START_MAC_ID	2
+#define NUM_STA 32
+#define NUM_ACL 16
+
+/* if mode ==0, then the sta is allowed once the addr is hit. */
+/* if mode ==1, then the sta is rejected once the addr is non-hit. */
+struct rtw_wlan_acl_node {
+	struct list_head list;
+	u8       addr[ETH_ALEN];
+	u8       valid;
+};
+
+/* mode=0, disable */
+/* mode=1, accept unless in deny list */
+/* mode=2, deny unless in accept list */
+struct wlan_acl_pool {
+	int mode;
+	int num;
+	struct rtw_wlan_acl_node aclnode[NUM_ACL];
+	struct __queue acl_node_q;
+};
+
+struct rssi_sta {
+	s32	UndecoratedSmoothedPWDB;
+	s32	UndecoratedSmoothedCCK;
+	s32	UndecoratedSmoothedOFDM;
+	u64	PacketMap;
+	u8	ValidBit;
+};
+
+struct	stainfo_stats	{
+	u64 rx_mgnt_pkts;
+	u64 rx_beacon_pkts;
+	u64 rx_probereq_pkts;
+	u64 rx_probersp_pkts;
+	u64 rx_probersp_bm_pkts;
+	u64 rx_probersp_uo_pkts;
+	u64 rx_ctrl_pkts;
+	u64 rx_data_pkts;
+
+	u64 last_rx_beacon_pkts;
+	u64 last_rx_probereq_pkts;
+	u64 last_rx_probersp_pkts;
+	u64 last_rx_probersp_bm_pkts;
+	u64 last_rx_probersp_uo_pkts;
+	u64	last_rx_ctrl_pkts;
+	u64	last_rx_data_pkts;
+	u64	rx_bytes;
+	u64	rx_drops;
+	u64	tx_pkts;
+	u64	tx_bytes;
+	u64  tx_drops;
+};
+
+struct sta_info {
+	spinlock_t lock;
+	struct list_head list; /* free_sta_queue */
+	struct list_head hash_list; /* sta_hash */
+
+	struct sta_xmit_priv sta_xmitpriv;
+	struct sta_recv_priv sta_recvpriv;
+
+	struct __queue sleep_q;
+	unsigned int sleepq_len;
+
+	uint state;
+	uint aid;
+	uint mac_id;
+	uint qos_option;
+	u8	hwaddr[ETH_ALEN];
+
+	uint	ieee8021x_blocked;	/* 0: allowed, 1:blocked */
+	uint	dot118021XPrivacy; /* aes, tkip... */
+	union Keytype	dot11tkiptxmickey;
+	union Keytype	dot11tkiprxmickey;
+	union Keytype	dot118021x_UncstKey;
+	union pn48		dot11txpn;			/*  PN48 used for Unicast xmit. */
+	union pn48		dot11rxpn;			/*  PN48 used for Unicast recv. */
+	u8	bssrateset[16];
+	u32	bssratelen;
+	s32  rssi;
+	s32	signal_quality;
+
+	u8	cts2self;
+	u8	rtsen;
+
+	u8	raid;
+	u8	init_rate;
+	u32	ra_mask;
+	u8	wireless_mode;	/*  NETWORK_TYPE */
+	struct stainfo_stats sta_stats;
+
+	/* for A-MPDU TX, ADDBA timeout check */
+	struct timer_list addba_retry_timer;
+
+	/* for A-MPDU Rx reordering buffer control */
+	struct recv_reorder_ctrl recvreorder_ctrl[16];
+
+	/* for A-MPDU Tx */
+	/* unsigned char		ampdu_txen_bitmap; */
+	u16	BA_starting_seqctrl[16];
+
+	struct ht_priv	htpriv;
+
+	/* Notes: */
+	/* STA_Mode: */
+	/* curr_network(mlme_priv/security_priv/qos/ht) +
+	 * sta_info: (STA & AP) CAP/INFO */
+	/* scan_q: AP CAP/INFO */
+
+	/* AP_Mode: */
+	/* curr_network(mlme_priv/security_priv/qos/ht) : AP CAP/INFO */
+	/* sta_info: (AP & STA) CAP/INFO */
+
+	struct list_head asoc_list;
+	struct list_head auth_list;
+
+	unsigned int expire_to;
+	unsigned int auth_seq;
+	unsigned int authalg;
+	unsigned char chg_txt[128];
+
+	u16 capability;
+	int flags;
+
+	int dot8021xalg;/* 0:disable, 1:psk, 2:802.1x */
+	int wpa_psk;/* 0:disable, bit(0): WPA, bit(1):WPA2 */
+	int wpa_group_cipher;
+	int wpa2_group_cipher;
+	int wpa_pairwise_cipher;
+	int wpa2_pairwise_cipher;
+
+	u8 bpairwise_key_installed;
+
+	u8 wpa_ie[32];
+
+	u8 nonerp_set;
+	u8 no_short_slot_time_set;
+	u8 no_short_preamble_set;
+	u8 no_ht_gf_set;
+	u8 no_ht_set;
+	u8 ht_20mhz_set;
+
+	unsigned int tx_ra_bitmap;
+	u8 qos_info;
+
+	u8 max_sp_len;
+	u8 uapsd_bk;/* BIT(0): Delivery enabled, BIT(1): Trigger enabled */
+	u8 uapsd_be;
+	u8 uapsd_vi;
+	u8 uapsd_vo;
+
+	u8 has_legacy_ac;
+	unsigned int sleepq_ac_len;
+
+	/* p2p priv data */
+	u8 is_p2p_device;
+	u8 p2p_status_code;
+
+	/* p2p client info */
+	u8 dev_addr[ETH_ALEN];
+	u8 dev_cap;
+	u16 config_methods;
+	u8 primary_dev_type[8];
+	u8 num_of_secdev_type;
+	u8 secdev_types_list[32];/*  32/8 == 4; */
+	u16 dev_name_len;
+	u8 dev_name[32];
+	u8 under_exist_checking;
+	u8 keep_alive_trycnt;
+
+	/* for DM */
+	struct rssi_sta rssi_stat;
+
+	/*  ================ODM Relative Info======================= */
+	/*  Please be careful, don't declare too much structure here.
+	 *  It will cost memory * STA support num. */
+	/*  2011/10/20 MH Add for ODM STA info. */
+	/*  Driver Write */
+	u8	bValid;		/*  record the sta status link or not? */
+	u8	IOTPeer;	/*  Enum value.	HT_IOT_PEER_E */
+	u8	rssi_level;	/* for Refresh RA mask */
+	/*  ODM Write */
+	/* 1 PHY_STATUS_INFO */
+	u8		RSSI_Path[4];		/*  */
+	u8		RSSI_Ave;
+	u8		RXEVM[4];
+	u8		RXSNR[4];
+
+	/*  ================ODM Relative Info======================= */
+	/*  */
+
+	/* To store the sequence number of received management frame */
+	u16 RxMgmtFrameSeqNum;
+};
+
+#define sta_rx_pkts(sta) \
+	(sta->sta_stats.rx_mgnt_pkts \
+	+ sta->sta_stats.rx_ctrl_pkts \
+	+ sta->sta_stats.rx_data_pkts)
+
+#define sta_rx_data_pkts(sta) \
+	(sta->sta_stats.rx_data_pkts)
+
+#define sta_last_rx_data_pkts(sta) \
+	(sta->sta_stats.last_rx_data_pkts)
+
+#define sta_rx_beacon_pkts(sta) \
+	(sta->sta_stats.rx_beacon_pkts)
+
+#define sta_last_rx_beacon_pkts(sta) \
+	(sta->sta_stats.last_rx_beacon_pkts)
+
+#define sta_rx_probersp_pkts(sta) \
+	(sta->sta_stats.rx_probersp_pkts)
+
+#define sta_last_rx_probersp_pkts(sta) \
+	(sta->sta_stats.last_rx_probersp_pkts)
+
+#define sta_update_last_rx_pkts(sta) \
+do { \
+	sta->sta_stats.last_rx_beacon_pkts = sta->sta_stats.rx_beacon_pkts; \
+	sta->sta_stats.last_rx_probereq_pkts = sta->sta_stats.rx_probereq_pkts; \
+	sta->sta_stats.last_rx_probersp_pkts = sta->sta_stats.rx_probersp_pkts; \
+	sta->sta_stats.last_rx_probersp_bm_pkts = sta->sta_stats.rx_probersp_bm_pkts; \
+	sta->sta_stats.last_rx_probersp_uo_pkts = sta->sta_stats.rx_probersp_uo_pkts; \
+	sta->sta_stats.last_rx_ctrl_pkts = sta->sta_stats.rx_ctrl_pkts; \
+	sta->sta_stats.last_rx_data_pkts = sta->sta_stats.rx_data_pkts; \
+} while (0)
+
+struct	sta_priv {
+	u8 *pallocated_stainfo_buf;
+	u8 *pstainfo_buf;
+	struct __queue free_sta_queue;
+
+	spinlock_t sta_hash_lock;
+	struct list_head sta_hash[NUM_STA];
+	int asoc_sta_count;
+	struct __queue sleep_q;
+	struct __queue wakeup_q;
+
+	struct adapter *padapter;
+
+	spinlock_t asoc_list_lock;
+	struct list_head asoc_list;
+
+	struct list_head auth_list;
+	spinlock_t auth_list_lock;
+	u8 asoc_list_cnt;
+	u8 auth_list_cnt;
+
+	unsigned int auth_to;  /* sec, time to expire in authenticating. */
+	unsigned int assoc_to; /* sec, time to expire before associating. */
+	unsigned int expire_to; /* sec , time to expire after associated. */
+
+	/* pointers to STA info; based on allocated AID or NULL if AID free
+	 * AID is in the range 1-2007, so sta_aid[0] corresponders to AID 1
+	 * and so on
+	 */
+	struct sta_info *sta_aid[NUM_STA];
+
+	u16 sta_dz_bitmap;/* only support 15 stations, station aid bitmap
+			   * for sleeping sta. */
+	u16 tim_bitmap;	/* only support 15 stations, aid=0~15 mapping
+			 * bit0~bit15 */
+
+	u16 max_num_sta;
+
+	struct wlan_acl_pool acl_list;
+};
+
+static inline u32 wifi_mac_hash(u8 *mac)
+{
+	u32 x;
+
+	x = mac[0];
+	x = (x << 2) ^ mac[1];
+	x = (x << 2) ^ mac[2];
+	x = (x << 2) ^ mac[3];
+	x = (x << 2) ^ mac[4];
+	x = (x << 2) ^ mac[5];
+
+	x ^= x >> 8;
+	x  = x & (NUM_STA - 1);
+	return x;
+}
+
+extern u32	_rtw_init_sta_priv(struct sta_priv *pstapriv);
+extern void _rtw_free_sta_priv(struct sta_priv *pstapriv);
+
+#define stainfo_offset_valid(offset) (offset < NUM_STA && offset >= 0)
+int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta);
+struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int off);
+
+extern struct sta_info *rtw_alloc_stainfo(struct sta_priv *stapriv, u8 *hwaddr);
+extern u32	rtw_free_stainfo(struct adapter *adapt, struct sta_info *psta);
+extern void rtw_free_all_stainfo(struct adapter *adapt);
+extern struct sta_info *rtw_get_stainfo(struct sta_priv *stapriv, u8 *hwaddr);
+extern u32 rtw_init_bcmc_stainfo(struct adapter *adapt);
+extern struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter);
+extern u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr);
+
+#endif /* _STA_INFO_H_ */
diff --git a/drivers/staging/r8188eu/include/usb_ops.h b/drivers/staging/r8188eu/include/usb_ops.h
new file mode 100644
index 000000000..ddc46cb44
--- /dev/null
+++ b/drivers/staging/r8188eu/include/usb_ops.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __USB_OPS_H_
+#define __USB_OPS_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+
+#define REALTEK_USB_VENQT_READ		(USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE)
+#define REALTEK_USB_VENQT_WRITE		(USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE)
+#define REALTEK_USB_VENQT_CMD_REQ	0x05
+#define REALTEK_USB_VENQT_CMD_IDX	0x00
+
+#define ALIGNMENT_UNIT			16
+#define MAX_VENDOR_REQ_CMD_SIZE	254	/* 8188cu SIE Support */
+#define MAX_USB_IO_CTL_SIZE	(MAX_VENDOR_REQ_CMD_SIZE + ALIGNMENT_UNIT)
+
+#include "usb_ops_linux.h"
+
+/*
+ * Increase and check if the continual_urb_error of this @param dvobjprivei
+ * is larger than MAX_CONTINUAL_URB_ERR
+ * @return true:
+ * @return false:
+ */
+static inline bool rtw_inc_and_chk_continual_urb_error(struct dvobj_priv *dvobj)
+{
+	int value = atomic_inc_return(&dvobj->continual_urb_error);
+
+	if (value > MAX_CONTINUAL_URB_ERR)
+		return true;
+
+	return false;
+}
+
+/*
+* Set the continual_urb_error of this @param dvobjprive to 0
+*/
+static inline void rtw_reset_continual_urb_error(struct dvobj_priv *dvobj)
+{
+	atomic_set(&dvobj->continual_urb_error, 0);
+}
+
+#define USB_HIGH_SPEED_BULK_SIZE	512
+#define USB_FULL_SPEED_BULK_SIZE	64
+
+static inline bool rtw_usb_bulk_size_boundary(struct adapter *padapter, int buf_len)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	if (pdvobjpriv->pusbdev->speed == USB_SPEED_HIGH)
+		return buf_len % USB_HIGH_SPEED_BULK_SIZE == 0;
+	else
+		return buf_len % USB_FULL_SPEED_BULK_SIZE == 0;
+}
+
+#endif /* __USB_OPS_H_ */
diff --git a/drivers/staging/r8188eu/include/usb_ops_linux.h b/drivers/staging/r8188eu/include/usb_ops_linux.h
new file mode 100644
index 000000000..966688eed
--- /dev/null
+++ b/drivers/staging/r8188eu/include/usb_ops_linux.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __USB_OPS_LINUX_H__
+#define __USB_OPS_LINUX_H__
+
+#define VENDOR_CMD_MAX_DATA_LEN	254
+
+#define RTW_USB_CONTROL_MSG_TIMEOUT_TEST	10/* ms */
+#define RTW_USB_CONTROL_MSG_TIMEOUT	500/* ms */
+
+#define MAX_USBCTRL_VENDORREQ_TIMES	10
+
+#define RTW_USB_BULKOUT_TIME	5000/* ms */
+
+#define _usbctrl_vendorreq_async_callback(urb, regs)	\
+	_usbctrl_vendorreq_async_callback(urb)
+#define usb_bulkout_zero_complete(purb, regs)		\
+	usb_bulkout_zero_complete(purb)
+#define usb_write_mem_complete(purb, regs)		\
+	usb_write_mem_complete(purb)
+#define usb_write_port_complete(purb, regs)		\
+	usb_write_port_complete(purb)
+#define usb_read_port_complete(purb, regs)		\
+	usb_read_port_complete(purb)
+#define usb_read_interrupt_complete(purb, regs)		\
+	usb_read_interrupt_complete(purb)
+
+#endif
diff --git a/drivers/staging/r8188eu/include/usb_osintf.h b/drivers/staging/r8188eu/include/usb_osintf.h
new file mode 100644
index 000000000..f271e93e9
--- /dev/null
+++ b/drivers/staging/r8188eu/include/usb_osintf.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __USB_OSINTF_H
+#define __USB_OSINTF_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+extern char *rtw_initmac;
+extern int rtw_mc2u_disable;
+
+#define USBD_HALTED(Status) ((u32)(Status) >> 30 == 3)
+
+void netdev_br_init(struct net_device *netdev);
+void dhcp_flag_bcast(struct adapter *priv, struct sk_buff *skb);
+void *scdb_findEntry(struct adapter *priv, unsigned char *ipAddr);
+void nat25_db_expire(struct adapter *priv);
+int nat25_db_handle(struct adapter *priv, struct sk_buff *skb, int method);
+
+#endif
diff --git a/drivers/staging/r8188eu/include/wifi.h b/drivers/staging/r8188eu/include/wifi.h
new file mode 100644
index 000000000..0254310bd
--- /dev/null
+++ b/drivers/staging/r8188eu/include/wifi.h
@@ -0,0 +1,838 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#ifndef _WIFI_H_
+#define _WIFI_H_
+
+#include <linux/bits.h>
+#include <linux/ieee80211.h>
+
+#define WLAN_ETHHDR_LEN		14
+#define WLAN_HDR_A3_LEN		24
+#define WLAN_HDR_A3_QOS_LEN	26
+#define WLAN_SSID_MAXLEN	32
+
+enum WIFI_FRAME_SUBTYPE {
+	/*  below is for mgt frame */
+	WIFI_ASSOCREQ       = (0 | IEEE80211_FTYPE_MGMT),
+	WIFI_ASSOCRSP       = (BIT(4) | IEEE80211_FTYPE_MGMT),
+	WIFI_REASSOCREQ     = (BIT(5) | IEEE80211_FTYPE_MGMT),
+	WIFI_REASSOCRSP     = (BIT(5) | BIT(4) | IEEE80211_FTYPE_MGMT),
+	WIFI_PROBEREQ       = (BIT(6) | IEEE80211_FTYPE_MGMT),
+	WIFI_PROBERSP       = (BIT(6) | BIT(4) | IEEE80211_FTYPE_MGMT),
+	WIFI_BEACON         = (BIT(7) | IEEE80211_FTYPE_MGMT),
+	WIFI_ATIM           = (BIT(7) | BIT(4) | IEEE80211_FTYPE_MGMT),
+	WIFI_DISASSOC       = (BIT(7) | BIT(5) | IEEE80211_FTYPE_MGMT),
+	WIFI_AUTH           = (BIT(7) | BIT(5) | BIT(4) | IEEE80211_FTYPE_MGMT),
+	WIFI_DEAUTH         = (BIT(7) | BIT(6) | IEEE80211_FTYPE_MGMT),
+	WIFI_ACTION         = (BIT(7) | BIT(6) | BIT(4) | IEEE80211_FTYPE_MGMT),
+
+	/*  below is for control frame */
+	WIFI_PSPOLL         = (BIT(7) | BIT(5) | IEEE80211_FTYPE_CTL),
+
+	/*  below is for data frame */
+	WIFI_DATA           = (0 | IEEE80211_FTYPE_DATA),
+	WIFI_DATA_CFACK     = (BIT(4) | IEEE80211_FTYPE_DATA),
+	WIFI_DATA_CFPOLL    = (BIT(5) | IEEE80211_FTYPE_DATA),
+	WIFI_DATA_CFACKPOLL = (BIT(5) | BIT(4) | IEEE80211_FTYPE_DATA),
+	WIFI_DATA_NULL      = (BIT(6) | IEEE80211_FTYPE_DATA),
+	WIFI_QOS_DATA_NULL	= (BIT(6) | IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA),
+};
+
+enum WIFI_REASON_CODE	{
+	_RSON_RESERVED_			= 0,
+	_RSON_UNSPECIFIED_		= 1,
+	_RSON_AUTH_NO_LONGER_VALID_	= 2,
+	_RSON_DEAUTH_STA_LEAVING_	= 3,
+	_RSON_INACTIVITY_		= 4,
+	_RSON_UNABLE_HANDLE_		= 5,
+	_RSON_CLS2_			= 6,
+	_RSON_CLS3_			= 7,
+	_RSON_DISAOC_STA_LEAVING_	= 8,
+	_RSON_ASOC_NOT_AUTH_		= 9,
+
+	/*  WPA reason */
+	_RSON_INVALID_IE_		= 13,
+	_RSON_MIC_FAILURE_		= 14,
+	_RSON_4WAY_HNDSHK_TIMEOUT_	= 15,
+	_RSON_GROUP_KEY_UPDATE_TIMEOUT_	= 16,
+	_RSON_DIFF_IE_			= 17,
+	_RSON_MLTCST_CIPHER_NOT_VALID_	= 18,
+	_RSON_UNICST_CIPHER_NOT_VALID_	= 19,
+	_RSON_AKMP_NOT_VALID_		= 20,
+	_RSON_UNSUPPORT_RSNE_VER_	= 21,
+	_RSON_INVALID_RSNE_CAP_		= 22,
+	_RSON_IEEE_802DOT1X_AUTH_FAIL_	= 23,
+
+	/* belowing are Realtek definition */
+	_RSON_PMK_NOT_AVAILABLE_	= 24,
+	_RSON_TDLS_TEAR_TOOFAR_		= 25,
+	_RSON_TDLS_TEAR_UN_RSN_		= 26,
+};
+
+enum WIFI_STATUS_CODE {
+	_STATS_SUCCESSFUL_		= 0,
+	_STATS_FAILURE_			= 1,
+	_STATS_CAP_FAIL_		= 10,
+	_STATS_NO_ASOC_			= 11,
+	_STATS_OTHER_			= 12,
+	_STATS_NO_SUPP_ALG_		= 13,
+	_STATS_OUT_OF_AUTH_SEQ_		= 14,
+	_STATS_CHALLENGE_FAIL_		= 15,
+	_STATS_AUTH_TIMEOUT_		= 16,
+	_STATS_UNABLE_HANDLE_STA_	= 17,
+	_STATS_RATE_FAIL_		= 18,
+};
+
+/* entended */
+/* IEEE 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+/* IEEE 802.11h */
+#define WLAN_STATUS_SPEC_MGMT_REQUIRED 22
+#define WLAN_STATUS_PWR_CAPABILITY_NOT_VALID 23
+#define WLAN_STATUS_SUPPORTED_CHANNEL_NOT_VALID 24
+/* IEEE 802.11g */
+#define WLAN_STATUS_ASSOC_DENIED_NO_SHORT_SLOT_TIME 25
+#define WLAN_STATUS_ASSOC_DENIED_NO_ER_PBCC 26
+#define WLAN_STATUS_ASSOC_DENIED_NO_DSSS_OFDM 27
+/* IEEE 802.11w */
+#define WLAN_STATUS_ASSOC_REJECTED_TEMPORARILY 30
+#define WLAN_STATUS_ROBUST_MGMT_FRAME_POLICY_VIOLATION 31
+/* IEEE 802.11i */
+#define WLAN_STATUS_INVALID_IE 40
+#define WLAN_STATUS_GROUP_CIPHER_NOT_VALID 41
+#define WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID 42
+#define WLAN_STATUS_AKMP_NOT_VALID 43
+#define WLAN_STATUS_UNSUPPORTED_RSN_IE_VERSION 44
+#define WLAN_STATUS_INVALID_RSN_IE_CAPAB 45
+#define WLAN_STATUS_CIPHER_REJECTED_PER_POLICY 46
+#define WLAN_STATUS_TS_NOT_CREATED 47
+#define WLAN_STATUS_DIRECT_LINK_NOT_ALLOWED 48
+#define WLAN_STATUS_DEST_STA_NOT_PRESENT 49
+#define WLAN_STATUS_DEST_STA_NOT_QOS_STA 50
+#define WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE 51
+/* IEEE 802.11r */
+#define WLAN_STATUS_INVALID_FT_ACTION_FRAME_COUNT 52
+#define WLAN_STATUS_INVALID_PMKID 53
+#define WLAN_STATUS_INVALID_MDIE 54
+#define WLAN_STATUS_INVALID_FTIE 55
+
+enum WIFI_REG_DOMAIN {
+	DOMAIN_FCC	= 1,
+	DOMAIN_IC	= 2,
+	DOMAIN_ETSI	= 3,
+	DOMAIN_SPA	= 4,
+	DOMAIN_FRANCE	= 5,
+	DOMAIN_MKK	= 6,
+	DOMAIN_ISRAEL	= 7,
+	DOMAIN_MKK1	= 8,
+	DOMAIN_MKK2	= 9,
+	DOMAIN_MKK3	= 10,
+	DOMAIN_MAX
+};
+
+#define _TO_DS_		BIT(8)
+#define _FROM_DS_	BIT(9)
+#define _MORE_FRAG_	BIT(10)
+#define _RETRY_		BIT(11)
+#define _PWRMGT_	BIT(12)
+#define _MORE_DATA_	BIT(13)
+#define _PRIVACY_	BIT(14)
+#define _ORDER_		BIT(15)
+
+#define SetToDs(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_TO_DS_)
+
+#define GetToDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_TO_DS_)) != 0)
+
+#define SetFrDs(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_FROM_DS_)
+
+#define GetFrDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_FROM_DS_)) != 0)
+
+#define SetMFrag(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_MORE_FRAG_)
+
+#define ClearMFrag(pbuf)	\
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(_MORE_FRAG_))
+
+#define GetRetry(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_RETRY_)) != 0)
+
+#define SetPwrMgt(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_PWRMGT_)
+
+#define GetPwrMgt(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_PWRMGT_)) != 0)
+
+#define SetMData(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_MORE_DATA_)
+
+#define SetPrivacy(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_PRIVACY_)
+
+#define GetPrivacy(pbuf)					\
+	(((*(__le16 *)(pbuf)) & cpu_to_le16(_PRIVACY_)) != 0)
+
+#define GetFrameType(pbuf)				\
+	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(3) | BIT(2)))
+
+#define GetFrameSubType(pbuf)	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(7) |\
+	 BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2)))
+
+#define SetFrameSubType(pbuf, type) \
+	do {    \
+		*(__le16 *)(pbuf) &= cpu_to_le16(~(BIT(7) | BIT(6) |	\
+		 BIT(5) | BIT(4) | BIT(3) | BIT(2))); \
+		*(__le16 *)(pbuf) |= cpu_to_le16(type); \
+	} while (0)
+
+#define GetTupleCache(pbuf)			\
+	(cpu_to_le16(*(unsigned short *)((size_t)(pbuf) + 22)))
+
+#define SetFragNum(pbuf, num) \
+	do {    \
+		*(unsigned short *)((size_t)(pbuf) + 22) = \
+			((*(unsigned short *)((size_t)(pbuf) + 22)) &	\
+			le16_to_cpu(~(0x000f))) | \
+			cpu_to_le16(0x0f & (num));     \
+	} while (0)
+
+#define SetSeqNum(pbuf, num) \
+	do {    \
+		*(__le16 *)((size_t)(pbuf) + 22) = \
+			((*(__le16 *)((size_t)(pbuf) + 22)) & cpu_to_le16((unsigned short)0x000f)) | \
+			cpu_to_le16((unsigned short)(0xfff0 & (num << 4))); \
+	} while (0)
+
+#define SetDuration(pbuf, dur) \
+	*(__le16 *)((size_t)(pbuf) + 2) = cpu_to_le16(0xffff & (dur))
+
+#define SetPriority(pbuf, tid)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(tid & 0xf)
+
+#define SetEOSP(pbuf, eosp)	\
+		*(__le16 *)(pbuf) |= cpu_to_le16((eosp & 1) << 4)
+
+#define SetAckpolicy(pbuf, ack)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16((ack & 3) << 5)
+
+#define GetAckpolicy(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 5) & 0x3)
+
+#define GetAMsdu(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 7) & 0x1)
+
+#define SetAMsdu(pbuf, amsdu)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7)
+
+#define GetTid(pbuf)	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) +	\
+			(((GetToDs(pbuf)<<1) | GetFrDs(pbuf)) == 3 ?	\
+			30 : 24))) & 0x000f)
+
+#define GetAddr1Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 4))
+
+#define GetAddr2Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 10))
+
+#define GetAddr3Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 16))
+
+#define GetAddr4Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 24))
+
+static inline bool IS_MCAST(unsigned char *da)
+{
+	return (*da) & 0x01;
+}
+
+static inline unsigned char *get_da(unsigned char *pframe)
+{
+	unsigned char	*da;
+	unsigned int to_fr_ds = (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs=0, FromDs=0 */
+		da = GetAddr1Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs=0, FromDs=1 */
+		da = GetAddr1Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs=1, FromDs=0 */
+		da = GetAddr3Ptr(pframe);
+		break;
+	default:	/*  ToDs=1, FromDs=1 */
+		da = GetAddr3Ptr(pframe);
+		break;
+	}
+	return da;
+}
+
+static inline unsigned char *get_sa(unsigned char *pframe)
+{
+	unsigned char	*sa;
+	unsigned int	to_fr_ds = (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs=0, FromDs=0 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs=0, FromDs=1 */
+		sa = GetAddr3Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs=1, FromDs=0 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	default:	/*  ToDs=1, FromDs=1 */
+		sa = GetAddr4Ptr(pframe);
+		break;
+	}
+	return sa;
+}
+
+static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
+{
+	unsigned char	*sa;
+	unsigned int	to_fr_ds = (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs=0, FromDs=0 */
+		sa = GetAddr3Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs=0, FromDs=1 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs=1, FromDs=0 */
+		sa = GetAddr1Ptr(pframe);
+		break;
+	case 0x03:	/*  ToDs=1, FromDs=1 */
+		sa = GetAddr1Ptr(pframe);
+		break;
+	default:
+		sa = NULL; /*  */
+		break;
+	}
+	return sa;
+}
+
+/*-----------------------------------------------------------------------------
+			Below is for the security related definition
+------------------------------------------------------------------------------*/
+#define _RESERVED_FRAME_TYPE_		0
+#define _SKB_FRAME_TYPE_		2
+#define _PRE_ALLOCMEM_			1
+#define _PRE_ALLOCHDR_			3
+#define _PRE_ALLOCLLCHDR_		4
+#define _PRE_ALLOCICVHDR_		5
+#define _PRE_ALLOCMICHDR_		6
+
+#define _SIFSTIME_				\
+	(priv->pmib->dot11BssType.net_work_type = 10)
+#define _ACKCTSLNG_		14	/* 14 bytes long, including crclng */
+#define _CRCLNG_		4
+
+#define _ASOCREQ_IE_OFFSET_	4	/*  excluding wlan_hdr */
+#define	_ASOCRSP_IE_OFFSET_	6
+#define _REASOCREQ_IE_OFFSET_	10
+#define _REASOCRSP_IE_OFFSET_	6
+#define _PROBEREQ_IE_OFFSET_	0
+#define	_PROBERSP_IE_OFFSET_	12
+#define _AUTH_IE_OFFSET_	6
+#define _DEAUTH_IE_OFFSET_	0
+#define _BEACON_IE_OFFSET_	12
+#define _PUBLIC_ACTION_IE_OFFSET_	8
+
+#define _FIXED_IE_LENGTH_	_BEACON_IE_OFFSET_
+
+#define _SSID_IE_		0
+#define _SUPPORTEDRATES_IE_	1
+#define _DSSET_IE_		3
+#define _TIM_IE_		5
+#define _IBSS_PARA_IE_		6
+#define _COUNTRY_IE_		7
+#define _CHLGETXT_IE_		16
+#define _SUPPORTED_CH_IE_	36
+#define _CH_SWTICH_ANNOUNCE_	37	/* Secondary Channel Offset */
+#define _RSN_IE_2_		48
+#define _SSN_IE_1_		221
+#define _ERPINFO_IE_		42
+#define _EXT_SUPPORTEDRATES_IE_	50
+
+#define _HT_CAPABILITY_IE_	45
+#define _FTIE_			55
+#define _TIMEOUT_ITVL_IE_	56
+#define _SRC_IE_		59
+#define _HT_EXTRA_INFO_IE_	61
+#define _HT_ADD_INFO_IE_	61 /* _HT_EXTRA_INFO_IE_ */
+#define _WAPI_IE_		68
+
+#define	EID_BSSCoexistence	72 /*  20/40 BSS Coexistence */
+#define	EID_BSSIntolerantChlReport	73
+#define _RIC_Descriptor_IE_	75
+
+#define _LINK_ID_IE_		101
+#define _CH_SWITCH_TIMING_	104
+#define _PTI_BUFFER_STATUS_	106
+#define _EXT_CAP_IE_		127
+#define _VENDOR_SPECIFIC_IE_	221
+
+#define	_RESERVED47_		47
+
+/* ---------------------------------------------------------------------------
+					Below is the fixed elements...
+-----------------------------------------------------------------------------*/
+#define _AUTH_ALGM_NUM_		2
+#define _AUTH_SEQ_NUM_		2
+#define _BEACON_ITERVAL_	2
+#define _CAPABILITY_		2
+#define _CURRENT_APADDR_	6
+#define _LISTEN_INTERVAL_	2
+#define _RSON_CODE_		2
+#define _ASOC_ID_		2
+#define _STATUS_CODE_		2
+#define _TIMESTAMP_		8
+
+#define cap_ESS		BIT(0)
+#define cap_IBSS	BIT(1)
+#define cap_CFPollable	BIT(2)
+#define cap_CFRequest	BIT(3)
+#define cap_Privacy	BIT(4)
+#define cap_ShortPremble BIT(5)
+#define cap_PBCC	BIT(6)
+#define cap_ChAgility	BIT(7)
+#define cap_SpecMgmt	BIT(8)
+#define cap_QoSi	BIT(9)
+#define cap_ShortSlot	BIT(10)
+
+/*-----------------------------------------------------------------------------
+				Below is the definition for 802.11i / 802.1x
+------------------------------------------------------------------------------*/
+#define _IEEE8021X_MGT_			1	/*  WPA */
+#define _IEEE8021X_PSK_			2	/*  WPA with pre-shared key */
+
+/*-----------------------------------------------------------------------------
+				Below is the definition for WMM
+------------------------------------------------------------------------------*/
+#define _WMM_IE_Length_				7  /*  for WMM STA */
+#define _WMM_Para_Element_Length_		24
+
+/*-----------------------------------------------------------------------------
+				Below is the definition for 802.11n
+------------------------------------------------------------------------------*/
+
+#define SetOrderBit(pbuf)	\
+	do	{	\
+		*(unsigned short *)(pbuf) |= cpu_to_le16(_ORDER_); \
+	} while (0)
+
+#define GetOrderBit(pbuf)			\
+	(((*(unsigned short *)(pbuf)) & le16_to_cpu(_ORDER_)) != 0)
+
+/**
+ * struct rtw_ieee80211_bar - HT Block Ack Request
+ *
+ * This structure refers to "HT BlockAckReq" as
+ * described in 802.11n draft section 7.2.1.7.1
+ */
+struct rtw_ieee80211_bar {
+	__le16 frame_control;
+	__le16 duration;
+	unsigned char ra[ETH_ALEN];
+	unsigned char ta[ETH_ALEN];
+	__le16 control;
+	__le16 start_seq_num;
+} __packed;
+
+/**
+ * struct ieee80211_ht_cap - HT additional information
+ *
+ * This structure refers to "HT information element" as
+ * described in 802.11n draft section 7.3.2.53
+ */
+struct ieee80211_ht_addt_info {
+	unsigned char	control_chan;
+	unsigned char	ht_param;
+	__le16	operation_mode;
+	__le16	stbc_param;
+	unsigned char	basic_set[16];
+} __packed;
+
+struct HT_caps_element {
+	union {
+		struct {
+			__le16	HT_caps_info;
+			unsigned char	AMPDU_para;
+			unsigned char	MCS_rate[16];
+			__le16	HT_ext_caps;
+			__le16	Beamforming_caps;
+			unsigned char	ASEL_caps;
+		} HT_cap_element;
+		unsigned char HT_cap[26];
+	} u;
+} __packed;
+
+struct HT_info_element {
+	unsigned char	primary_channel;
+	unsigned char	infos[5];
+	unsigned char	MCS_rate[16];
+} __packed;
+
+struct AC_param {
+	unsigned char		ACI_AIFSN;
+	unsigned char		CW;
+	__le16	TXOP_limit;
+} __packed;
+
+struct WMM_para_element {
+	unsigned char		QoS_info;
+	unsigned char		reserved;
+	struct AC_param	ac_param[4];
+} __packed;
+
+struct ADDBA_request {
+	unsigned char	action_code;
+	unsigned char	dialog_token;
+	__le16	BA_para_set;
+	__le16	BA_timeout_value;
+	__le16	BA_starting_seqctrl;
+} __packed;
+
+#define MAX_AMPDU_FACTOR_64K	3
+
+/* Spatial Multiplexing Power Save Modes */
+#define WLAN_HT_CAP_SM_PS_STATIC	0
+#define WLAN_HT_CAP_SM_PS_DYNAMIC	1
+#define WLAN_HT_CAP_SM_PS_INVALID	2
+#define WLAN_HT_CAP_SM_PS_DISABLED	3
+
+#define OP_MODE_PURE                    0
+#define OP_MODE_MAY_BE_LEGACY_STAS      1
+#define OP_MODE_20MHZ_HT_STA_ASSOCED    2
+#define OP_MODE_MIXED                   3
+
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK	((u8) BIT(0) | BIT(1))
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE		((u8) BIT(0))
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW		((u8) BIT(0) | BIT(1))
+#define HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH		((u8) BIT(2))
+#define HT_INFO_HT_PARAM_RIFS_MODE			((u8) BIT(3))
+#define HT_INFO_HT_PARAM_CTRL_ACCESS_ONLY		((u8) BIT(4))
+#define HT_INFO_HT_PARAM_SRV_INTERVAL_GRANULARITY	((u8) BIT(5))
+
+#define HT_INFO_OPERATION_MODE_OP_MODE_MASK	\
+		((u16) (0x0001 | 0x0002))
+#define HT_INFO_OPERATION_MODE_OP_MODE_OFFSET		0
+#define HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT	((u8) BIT(2))
+#define HT_INFO_OPERATION_MODE_TRANSMIT_BURST_LIMIT	((u8) BIT(3))
+#define HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT	((u8) BIT(4))
+
+/*	===============WPS Section=============== */
+/*	For WPSv1.0 */
+#define WPSOUI					0x0050f204
+/*	WPS attribute ID */
+#define WPS_ATTR_VER1				0x104A
+#define WPS_ATTR_SIMPLE_CONF_STATE		0x1044
+#define WPS_ATTR_RESP_TYPE			0x103B
+#define WPS_ATTR_UUID_E				0x1047
+#define WPS_ATTR_MANUFACTURER			0x1021
+#define WPS_ATTR_MODEL_NAME			0x1023
+#define WPS_ATTR_MODEL_NUMBER			0x1024
+#define WPS_ATTR_SERIAL_NUMBER			0x1042
+#define WPS_ATTR_PRIMARY_DEV_TYPE		0x1054
+#define WPS_ATTR_SEC_DEV_TYPE_LIST		0x1055
+#define WPS_ATTR_DEVICE_NAME			0x1011
+#define WPS_ATTR_CONF_METHOD			0x1008
+#define WPS_ATTR_RF_BANDS			0x103C
+#define WPS_ATTR_DEVICE_PWID			0x1012
+#define WPS_ATTR_REQUEST_TYPE			0x103A
+#define WPS_ATTR_ASSOCIATION_STATE		0x1002
+#define WPS_ATTR_CONFIG_ERROR			0x1009
+#define WPS_ATTR_VENDOR_EXT			0x1049
+#define WPS_ATTR_SELECTED_REGISTRAR		0x1041
+
+/*	Value of WPS attribute "WPS_ATTR_DEVICE_NAME */
+#define WPS_MAX_DEVICE_NAME_LEN			32
+
+/*	Value of WPS Request Type Attribute */
+#define WPS_REQ_TYPE_ENROLLEE_INFO_ONLY		0x00
+#define WPS_REQ_TYPE_ENROLLEE_OPEN_8021X	0x01
+#define WPS_REQ_TYPE_REGISTRAR			0x02
+#define WPS_REQ_TYPE_WLAN_MANAGER_REGISTRAR	0x03
+
+/*	Value of WPS Response Type Attribute */
+#define WPS_RESPONSE_TYPE_INFO_ONLY	0x00
+#define WPS_RESPONSE_TYPE_8021X		0x01
+#define WPS_RESPONSE_TYPE_REGISTRAR	0x02
+#define WPS_RESPONSE_TYPE_AP		0x03
+
+/*	Value of WPS WiFi Simple Configuration State Attribute */
+#define WPS_WSC_STATE_NOT_CONFIG	0x01
+#define WPS_WSC_STATE_CONFIG		0x02
+
+/*	Value of WPS Version Attribute */
+#define WPS_VERSION_1			0x10
+
+/*	Value of WPS Configuration Method Attribute */
+#define WPS_CONFIG_METHOD_FLASH		0x0001
+#define WPS_CONFIG_METHOD_ETHERNET	0x0002
+#define WPS_CONFIG_METHOD_LABEL		0x0004
+#define WPS_CONFIG_METHOD_DISPLAY	0x0008
+#define WPS_CONFIG_METHOD_E_NFC		0x0010
+#define WPS_CONFIG_METHOD_I_NFC		0x0020
+#define WPS_CONFIG_METHOD_NFC		0x0040
+#define WPS_CONFIG_METHOD_PBC		0x0080
+#define WPS_CONFIG_METHOD_KEYPAD	0x0100
+#define WPS_CONFIG_METHOD_VPBC		0x0280
+#define WPS_CONFIG_METHOD_PPBC		0x0480
+#define WPS_CONFIG_METHOD_VDISPLAY	0x2008
+#define WPS_CONFIG_METHOD_PDISPLAY	0x4008
+
+/*	Value of Category ID of WPS Primary Device Type Attribute */
+#define WPS_PDT_CID_DISPLAYS		0x0007
+#define WPS_PDT_CID_MULIT_MEDIA		0x0008
+#define WPS_PDT_CID_RTK_WIDI		WPS_PDT_CID_MULIT_MEDIA
+
+/*	Value of Sub Category ID of WPS Primary Device Type Attribute */
+#define WPS_PDT_SCID_MEDIA_SERVER	0x0005
+#define WPS_PDT_SCID_RTK_DMP		WPS_PDT_SCID_MEDIA_SERVER
+
+/*	Value of Device Password ID */
+#define WPS_DPID_P			0x0000
+#define WPS_DPID_USER_SPEC		0x0001
+#define WPS_DPID_MACHINE_SPEC		0x0002
+#define WPS_DPID_REKEY			0x0003
+#define WPS_DPID_PBC			0x0004
+#define WPS_DPID_REGISTRAR_SPEC		0x0005
+
+/*	Value of WPS RF Bands Attribute */
+#define WPS_RF_BANDS_2_4_GHZ		0x01
+#define WPS_RF_BANDS_5_GHZ		0x02
+
+/*	Value of WPS Association State Attribute */
+#define WPS_ASSOC_STATE_NOT_ASSOCIATED		0x00
+#define WPS_ASSOC_STATE_CONNECTION_SUCCESS	0x01
+#define WPS_ASSOC_STATE_CONFIGURATION_FAILURE	0x02
+#define WPS_ASSOC_STATE_ASSOCIATION_FAILURE	0x03
+#define WPS_ASSOC_STATE_IP_FAILURE		0x04
+
+/*	=====================P2P Section===================== */
+/*	For P2P */
+#define	P2POUI					0x506F9A09
+
+/*	P2P Attribute ID */
+#define	P2P_ATTR_STATUS				0x00
+#define	P2P_ATTR_MINOR_REASON_CODE		0x01
+#define	P2P_ATTR_CAPABILITY			0x02
+#define	P2P_ATTR_DEVICE_ID			0x03
+#define	P2P_ATTR_GO_INTENT			0x04
+#define	P2P_ATTR_CONF_TIMEOUT			0x05
+#define	P2P_ATTR_LISTEN_CH			0x06
+#define	P2P_ATTR_GROUP_BSSID			0x07
+#define	P2P_ATTR_EX_LISTEN_TIMING		0x08
+#define	P2P_ATTR_INTENTED_IF_ADDR		0x09
+#define	P2P_ATTR_MANAGEABILITY			0x0A
+#define	P2P_ATTR_CH_LIST			0x0B
+#define	P2P_ATTR_NOA				0x0C
+#define	P2P_ATTR_DEVICE_INFO			0x0D
+#define	P2P_ATTR_GROUP_INFO			0x0E
+#define	P2P_ATTR_GROUP_ID			0x0F
+#define	P2P_ATTR_INTERFACE			0x10
+#define	P2P_ATTR_OPERATING_CH			0x11
+#define	P2P_ATTR_INVITATION_FLAGS		0x12
+
+/*	Value of Status Attribute */
+#define	P2P_STATUS_SUCCESS				0x00
+#define	P2P_STATUS_FAIL_INFO_UNAVAILABLE		0x01
+#define	P2P_STATUS_FAIL_INCOMPATIBLE_PARAM		0x02
+#define	P2P_STATUS_FAIL_LIMIT_REACHED			0x03
+#define	P2P_STATUS_FAIL_INVALID_PARAM			0x04
+#define	P2P_STATUS_FAIL_REQUEST_UNABLE			0x05
+#define	P2P_STATUS_FAIL_PREVOUS_PROTO_ERR		0x06
+#define	P2P_STATUS_FAIL_NO_COMMON_CH			0x07
+#define	P2P_STATUS_FAIL_UNKNOWN_P2PGROUP		0x08
+#define	P2P_STATUS_FAIL_BOTH_GOINTENT_15		0x09
+#define	P2P_STATUS_FAIL_INCOMPATIBLE_PROVSION		0x0A
+#define	P2P_STATUS_FAIL_USER_REJECT			0x0B
+
+/*	Value of Inviation Flags Attribute */
+#define	P2P_INVITATION_FLAGS_PERSISTENT			BIT(0)
+
+#define	DMP_P2P_DEVCAP_SUPPORT	(P2P_DEVCAP_SERVICE_DISCOVERY | \
+				P2P_DEVCAP_CLIENT_DISCOVERABILITY | \
+				P2P_DEVCAP_CONCURRENT_OPERATION | \
+				P2P_DEVCAP_INVITATION_PROC)
+
+#define	DMP_P2P_GRPCAP_SUPPORT	(P2P_GRPCAP_INTRABSS)
+
+/*	Value of Device Capability Bitmap */
+#define	P2P_DEVCAP_SERVICE_DISCOVERY		BIT(0)
+#define	P2P_DEVCAP_CLIENT_DISCOVERABILITY	BIT(1)
+#define	P2P_DEVCAP_CONCURRENT_OPERATION		BIT(2)
+#define	P2P_DEVCAP_INFRA_MANAGED		BIT(3)
+#define	P2P_DEVCAP_DEVICE_LIMIT			BIT(4)
+#define	P2P_DEVCAP_INVITATION_PROC		BIT(5)
+
+/*	Value of Group Capability Bitmap */
+#define	P2P_GRPCAP_GO				BIT(0)
+#define	P2P_GRPCAP_PERSISTENT_GROUP		BIT(1)
+#define	P2P_GRPCAP_GROUP_LIMIT			BIT(2)
+#define	P2P_GRPCAP_INTRABSS			BIT(3)
+#define	P2P_GRPCAP_CROSS_CONN			BIT(4)
+#define	P2P_GRPCAP_PERSISTENT_RECONN		BIT(5)
+#define	P2P_GRPCAP_GROUP_FORMATION		BIT(6)
+
+/*	P2P Public Action Frame (Management Frame) */
+#define	P2P_PUB_ACTION_ACTION			0x09
+
+/*	P2P Public Action Frame Type */
+#define	P2P_GO_NEGO_REQ				0
+#define	P2P_GO_NEGO_RESP			1
+#define	P2P_GO_NEGO_CONF			2
+#define	P2P_INVIT_REQ				3
+#define	P2P_INVIT_RESP				4
+#define	P2P_DEVDISC_REQ				5
+#define	P2P_DEVDISC_RESP			6
+#define	P2P_PROVISION_DISC_REQ			7
+#define	P2P_PROVISION_DISC_RESP			8
+
+/*	P2P Action Frame Type */
+#define	P2P_NOTICE_OF_ABSENCE			0
+#define	P2P_PRESENCE_REQUEST			1
+#define	P2P_PRESENCE_RESPONSE			2
+#define	P2P_GO_DISC_REQUEST			3
+
+#define	P2P_MAX_PERSISTENT_GROUP_NUM		10
+
+#define	P2P_PROVISIONING_SCAN_CNT		3
+
+#define	P2P_WILDCARD_SSID_LEN			7
+
+/* default value, used when: (1)p2p disabed or (2)p2p enabled
+ * but only do 1 scan phase */
+#define	P2P_FINDPHASE_EX_NONE		0
+/*  used when p2p enabled and want to do 1 scan phase and
+ *  P2P_FINDPHASE_EX_MAX-1 find phase */
+#define	P2P_FINDPHASE_EX_FULL		1
+#define	P2P_FINDPHASE_EX_SOCIAL_FIRST	(P2P_FINDPHASE_EX_FULL+1)
+#define	P2P_FINDPHASE_EX_MAX		4
+#define	P2P_FINDPHASE_EX_SOCIAL_LAST	P2P_FINDPHASE_EX_MAX
+
+/* 5 seconds timeout for sending the provision discovery request */
+#define	P2P_PROVISION_TIMEOUT		5000
+/* 3 seconds timeout for sending the prov disc request concurrent mode */
+#define	P2P_CONCURRENT_PROVISION_TIME	3000
+/* 5 seconds timeout for receiving the group negotiation response */
+#define	P2P_GO_NEGO_TIMEOUT		5000
+/* 3 seconds timeout for sending the negotiation request under concurrent mode */
+#define	P2P_CONCURRENT_GO_NEGO_TIME	3000
+/* 100ms */
+#define	P2P_TX_PRESCAN_TIMEOUT		100
+/* 5 seconds timeout for sending the invitation request */
+#define	P2P_INVITE_TIMEOUT		5000
+/* 3 seconds timeout for sending the invitation request under concurrent mode */
+#define	P2P_CONCURRENT_INVITE_TIME	3000
+/* 25 seconds timeout to reset the scan channel (based on channel plan) */
+#define	P2P_RESET_SCAN_CH		25000
+#define	P2P_MAX_INTENT			15
+
+#define	P2P_MAX_NOA_NUM			2
+
+/*	WPS Configuration Method */
+#define	WPS_CM_NONE			0x0000
+#define	WPS_CM_LABEL			0x0004
+#define	WPS_CM_DISPLYA			0x0008
+#define	WPS_CM_EXTERNAL_NFC_TOKEN	0x0010
+#define	WPS_CM_INTEGRATED_NFC_TOKEN	0x0020
+#define	WPS_CM_NFC_INTERFACE		0x0040
+#define	WPS_CM_PUSH_BUTTON		0x0080
+#define	WPS_CM_KEYPAD			0x0100
+#define	WPS_CM_SW_PUHS_BUTTON		0x0280
+#define	WPS_CM_HW_PUHS_BUTTON		0x0480
+#define	WPS_CM_SW_DISPLAY_P		0x2008
+#define	WPS_CM_LCD_DISPLAY_P		0x4008
+
+enum P2P_ROLE {
+	P2P_ROLE_DISABLE = 0,
+	P2P_ROLE_DEVICE = 1,
+	P2P_ROLE_CLIENT = 2,
+	P2P_ROLE_GO = 3
+};
+
+enum P2P_STATE {
+	P2P_STATE_NONE = 0,			/* P2P disable */
+	/* P2P had enabled and do nothing */
+	P2P_STATE_IDLE = 1,
+	P2P_STATE_LISTEN = 2,			/* In pure listen state */
+	P2P_STATE_SCAN = 3,			/* In scan phase */
+	/* In the listen state of find phase */
+	P2P_STATE_FIND_PHASE_LISTEN = 4,
+	/* In the search state of find phase */
+	P2P_STATE_FIND_PHASE_SEARCH = 5,
+	/* In P2P provisioning discovery */
+	P2P_STATE_TX_PROVISION_DIS_REQ = 6,
+	P2P_STATE_RX_PROVISION_DIS_RSP = 7,
+	P2P_STATE_RX_PROVISION_DIS_REQ = 8,
+	/* Doing the group owner negoitation handshake */
+	P2P_STATE_GONEGO_ING = 9,
+	/* finish the group negoitation handshake with success */
+	P2P_STATE_GONEGO_OK = 10,
+	/* finish the group negoitation handshake with failure */
+	P2P_STATE_GONEGO_FAIL = 11,
+	/* receiving the P2P Inviation request and match with the profile. */
+	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,
+	/* Doing the P2P WPS */
+	P2P_STATE_PROVISIONING_ING = 13,
+	/* Finish the P2P WPS */
+	P2P_STATE_PROVISIONING_DONE = 14,
+	/* Transmit the P2P Invitation request */
+	P2P_STATE_TX_INVITE_REQ = 15,
+	/* Receiving the P2P Invitation response */
+	P2P_STATE_RX_INVITE_RESP_OK = 16,
+	/* receiving the P2P Inviation request and dismatch with the profile. */
+	P2P_STATE_RECV_INVITE_REQ_DISMATCH = 17,
+	/* receiving the P2P Inviation request and this wifi is GO. */
+	P2P_STATE_RECV_INVITE_REQ_GO = 18,
+	/* receiving the P2P Inviation request to join an existing P2P Group. */
+	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,
+	/* recveing the P2P Inviation response with failure */
+	P2P_STATE_RX_INVITE_RESP_FAIL = 20,
+	/* receiving p2p negoitation response with information is not available */
+	P2P_STATE_RX_INFOR_NOREADY = 21,
+	/* sending p2p negoitation response with information is not available */
+	P2P_STATE_TX_INFOR_NOREADY = 22,
+};
+
+enum P2P_WPSINFO {
+	P2P_NO_WPSINFO				= 0,
+	P2P_GOT_WPSINFO_PEER_DISPLAY_PIN	= 1,
+	P2P_GOT_WPSINFO_SELF_DISPLAY_PIN	= 2,
+	P2P_GOT_WPSINFO_PBC			= 3,
+};
+
+#define	P2P_PRIVATE_IOCTL_SET_LEN		64
+
+enum P2P_PROTO_WK_ID {
+	P2P_FIND_PHASE_WK = 0,
+	P2P_RESTORE_STATE_WK = 1,
+	P2P_PRE_TX_PROVDISC_PROCESS_WK = 2,
+	P2P_PRE_TX_NEGOREQ_PROCESS_WK = 3,
+	P2P_PRE_TX_INVITEREQ_PROCESS_WK = 4,
+	P2P_AP_P2P_CH_SWITCH_PROCESS_WK = 5,
+	P2P_RO_CH_WK = 6,
+};
+
+enum P2P_PS_STATE {
+	P2P_PS_DISABLE = 0,
+	P2P_PS_ENABLE = 1,
+	P2P_PS_SCAN = 2,
+	P2P_PS_SCAN_DONE = 3,
+	P2P_PS_ALLSTASLEEP = 4, /*  for P2P GO */
+};
+
+enum P2P_PS_MODE {
+	P2P_PS_NONE = 0,
+	P2P_PS_CTWINDOW = 1,
+	P2P_PS_NOA	 = 2,
+	P2P_PS_MIX = 3, /*  CTWindow and NoA */
+};
+
+#define IP_MCAST_MAC(mac)				\
+	((mac[0] == 0x01) && (mac[1] == 0x00) && (mac[2] == 0x5e))
+#define ICMPV6_MCAST_MAC(mac)				\
+	((mac[0] == 0x33) && (mac[1] == 0x33) && (mac[2] != 0xff))
+
+#endif /*  _WIFI_H_ */
diff --git a/drivers/staging/r8188eu/include/wlan_bssdef.h b/drivers/staging/r8188eu/include/wlan_bssdef.h
new file mode 100644
index 000000000..81bda91a4
--- /dev/null
+++ b/drivers/staging/r8188eu/include/wlan_bssdef.h
@@ -0,0 +1,323 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#ifndef __WLAN_BSSDEF_H__
+#define __WLAN_BSSDEF_H__
+
+#define MAX_IE_SZ			768
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+
+#define NDIS_802_11_RSSI long           /*  in dBm */
+
+struct ndis_802_11_ssid {
+	u32  SsidLength;
+	u8  Ssid[32];
+};
+
+enum NDIS_802_11_NETWORK_TYPE {
+	Ndis802_11FH,
+	Ndis802_11DS,
+	Ndis802_11OFDM5,
+	Ndis802_11OFDM24,
+	Ndis802_11NetworkTypeMax    /*  dummy upper bound */
+};
+
+struct ndis_802_11_config_fh {
+	u32           Length;		/*  Length of structure */
+	u32           HopPattern;	/*  As defined by 802.11, MSB set */
+	u32           HopSet;		/*  to one if non-802.11 */
+	u32           DwellTime;	/*  units are Kusec */
+};
+
+/*
+ *	FW will only save the channel number in DSConfig.
+ *	ODI Handler will convert the channel number to freq. number.
+ */
+struct ndis_802_11_config {
+	u32           Length;             /*  Length of structure */
+	u32           BeaconPeriod;       /*  units are Kusec */
+	u32           ATIMWindow;         /*  units are Kusec */
+	u32           DSConfig;           /*  Frequency, units are kHz */
+	struct ndis_802_11_config_fh    FHConfig;
+};
+
+enum ndis_802_11_network_infra {
+	Ndis802_11IBSS,
+	Ndis802_11Infrastructure,
+	Ndis802_11AutoUnknown,
+	Ndis802_11InfrastructureMax,     /*  dummy upper bound */
+	Ndis802_11APMode
+};
+
+struct ndis_802_11_fixed_ie {
+	u8  Timestamp[8];
+	u16  BeaconInterval;
+	u16  Capabilities;
+};
+
+struct ndis_802_11_var_ie {
+	u8  ElementID;
+	u8  Length;
+	u8  data[];
+};
+
+/*
+ * Length is the 4 bytes multiples of the sume of
+ *	[ETH_ALEN] + 2 + sizeof (struct ndis_802_11_ssid) + sizeof (u32)
+ *	+ sizeof (NDIS_802_11_RSSI) + sizeof (enum NDIS_802_11_NETWORK_TYPE)
+ *	+ sizeof (struct ndis_802_11_config)
+ *	+ NDIS_802_11_LENGTH_RATES_EX + IELength
+ *
+ * Except the IELength, all other fields are fixed length.
+ * Therefore, we can define a macro to represent the partial sum. */
+
+enum ndis_802_11_auth_mode {
+	Ndis802_11AuthModeOpen,
+	Ndis802_11AuthModeShared,
+	Ndis802_11AuthModeAutoSwitch,
+	Ndis802_11AuthModeWPA,
+	Ndis802_11AuthModeWPAPSK,
+	Ndis802_11AuthModeWPANone,
+	Ndis802_11AuthModeWAPI,
+	Ndis802_11AuthModeMax	/*  Not a real mode, upper bound */
+};
+
+enum ndis_802_11_wep_status {
+	Ndis802_11WEPEnabled,
+	Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+	Ndis802_11WEPDisabled,
+	Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+	Ndis802_11WEPKeyAbsent,
+	Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+	Ndis802_11WEPNotSupported,
+	Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+	Ndis802_11Encryption2Enabled,
+	Ndis802_11Encryption2KeyAbsent,
+	Ndis802_11Encryption3Enabled,
+	Ndis802_11Encryption3KeyAbsent,
+	Ndis802_11_EncryptionWAPI
+};
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+struct ndis_802_11_ai_reqfi {
+    u16 Capabilities;
+    u16 ListenInterval;
+    unsigned char CurrentAPAddress[ETH_ALEN];
+};
+
+struct ndis_802_11_ai_resfi {
+    u16 Capabilities;
+    u16 StatusCode;
+    u16 AssociationId;
+};
+
+struct ndis_802_11_assoc_info {
+	u32  Length;
+	u16 AvailableRequestFixedIEs;
+	struct ndis_802_11_ai_reqfi    RequestFixedIEs;
+	u32  RequestIELength;
+	u32  OffsetRequestIEs;
+	u16 AvailableResponseFixedIEs;
+	struct ndis_802_11_ai_resfi    ResponseFixedIEs;
+	u32  ResponseIELength;
+	u32  OffsetResponseIEs;
+};
+
+/*  Key mapping keys require a BSSID */
+struct ndis_802_11_key {
+	u32           Length;             /*  Length of this structure */
+	u32           KeyIndex;
+	u32           KeyLength;          /*  length of key in bytes */
+	unsigned char BSSID[ETH_ALEN];
+	unsigned long long KeyRSC;
+	u8           KeyMaterial[32];  /* var len depending on above field */
+};
+
+struct ndis_802_11_remove_key {
+	u32                   Length;        /*  Length */
+	u32                   KeyIndex;
+	unsigned char BSSID[ETH_ALEN];
+};
+
+struct ndis_802_11_wep {
+	u32     Length;        /*  Length of this structure */
+	u32     KeyIndex;      /*  0 is the per-client key,
+				  * 1-N are the global keys */
+	u32     KeyLength;     /*  length of key in bytes */
+	u8     KeyMaterial[16];/*  variable len depending on above field */
+};
+
+struct ndis_802_11_auth_req {
+	u32 Length;            /*  Length of structure */
+	unsigned char Bssid[ETH_ALEN];
+	u32 Flags;
+};
+
+enum ndis_802_11_status_type {
+	Ndis802_11StatusType_Authentication,
+	Ndis802_11StatusType_MediaStreamMode,
+	Ndis802_11StatusType_PMKID_CandidateList,
+	Ndis802_11StatusTypeMax    /*  not a real type, defined as
+				    * an upper bound */
+};
+
+struct ndis_802_11_status_ind {
+	enum ndis_802_11_status_type StatusType;
+};
+
+/*  mask for authentication/integrity fields */
+#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        	0x0f
+#define NDIS_802_11_AUTH_REQUEST_REAUTH			0x01
+#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE		0x02
+#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR		0x06
+#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR		0x0E
+
+/*  MIC check time, 60 seconds. */
+#define MIC_CHECK_TIME	60000000
+
+struct ndis_802_11_auth_evt {
+	struct ndis_802_11_status_ind       Status;
+	struct ndis_802_11_auth_req  Request[1];
+};
+
+struct ndis_802_11_test {
+	u32 Length;
+	u32 Type;
+	union {
+		struct ndis_802_11_auth_evt AuthenticationEvent;
+		NDIS_802_11_RSSI RssiTrigger;
+	} tt;
+};
+
+#ifndef Ndis802_11APMode
+#define Ndis802_11APMode (Ndis802_11InfrastructureMax+1)
+#endif
+
+struct wlan_phy_info {
+	u8	SignalStrength;/* in percentage) */
+	u8	SignalQuality;/* in percentage) */
+	u8	Optimum_antenna;  /* for Antenna diversity */
+	u8	Reserved_0;
+};
+
+struct wlan_bcn_info {
+	/* these infor get from rtw_get_encrypt_info when
+	 *	 * translate scan to UI */
+	u8 encryp_protocol;/* ENCRYP_PROTOCOL_E: OPEN/WEP/WPA/WPA2/WAPI */
+	int group_cipher; /* WPA/WPA2 group cipher */
+	int pairwise_cipher;/* WPA/WPA2/WEP pairwise cipher */
+	int is_8021x;
+
+	/* bwmode 20/40 and ch_offset UP/LOW */
+	unsigned short	ht_cap_info;
+	unsigned char	ht_info_infos_0;
+};
+
+/* temporally add #pragma pack for structure alignment issue of
+*   struct wlan_bssid_ex and get_struct wlan_bssid_ex_sz()
+*/
+struct wlan_bssid_ex {
+	u32  Length;
+	unsigned char MacAddress[ETH_ALEN];
+	u8  Reserved[2];/* 0]: IS beacon frame */
+	struct ndis_802_11_ssid  Ssid;
+	u32  Privacy;
+	NDIS_802_11_RSSI  Rssi;/* in dBM,raw data ,get from PHY) */
+	enum  NDIS_802_11_NETWORK_TYPE  NetworkTypeInUse;
+	struct ndis_802_11_config  Configuration;
+	enum ndis_802_11_network_infra  InfrastructureMode;
+	unsigned char SupportedRates[NDIS_802_11_LENGTH_RATES_EX];
+	struct wlan_phy_info	PhyInfo;
+	u32  IELength;
+	u8  IEs[MAX_IE_SZ];	/* timestamp, beacon interval, and
+				 * capability information) */
+} __packed;
+
+static inline uint get_wlan_bssid_ex_sz(struct wlan_bssid_ex *bss)
+{
+	return sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + bss->IELength;
+}
+
+struct	wlan_network {
+	struct list_head list;
+	int	network_type;	/* refer to ieee80211.h for WIRELESS_11B/G */
+	int	fixed;		/*  set fixed when not to be removed
+				 *  in site-surveying */
+	unsigned long	last_scanned; /* timestamp for the network */
+	int	aid;		/* will only be valid when a BSS is joinned. */
+	int	join_res;
+	struct wlan_bssid_ex	network; /* must be the last item */
+	struct wlan_bcn_info	BcnInfo;
+};
+
+enum VRTL_CARRIER_SENSE {
+	DISABLE_VCS,
+	ENABLE_VCS,
+	AUTO_VCS
+};
+
+enum VCS_TYPE {
+	NONE_VCS,
+	RTS_CTS,
+	CTS_TO_SELF
+};
+
+#define PWR_CAM 0
+#define PWR_MINPS 1
+#define PWR_MAXPS 2
+#define PWR_UAPSD 3
+#define PWR_VOIP 4
+
+enum UAPSD_MAX_SP {
+	NO_LIMIT,
+	TWO_MSDU,
+	FOUR_MSDU,
+	SIX_MSDU
+};
+
+#define NUM_PRE_AUTH_KEY 16
+#define NUM_PMKID_CACHE NUM_PRE_AUTH_KEY
+
+/*
+*	WPA2
+*/
+
+struct pmkid_candidate {
+	unsigned char BSSID[ETH_ALEN];
+	u32 Flags;
+};
+
+struct ndis_802_11_pmkid_list {
+	u32 Version;       /*  Version of the structure */
+	u32 NumCandidates; /*  No. of pmkid candidates */
+	struct pmkid_candidate CandidateList[1];
+};
+
+struct ndis_802_11_auth_encrypt {
+	enum ndis_802_11_auth_mode AuthModeSupported;
+	enum ndis_802_11_wep_status EncryptStatusSupported;
+};
+
+struct ndis_802_11_cap {
+	u32  Length;
+	u32  Version;
+	u32  NoOfPMKIDs;
+	u32  NoOfAuthEncryptPairsSupported;
+	struct ndis_802_11_auth_encrypt AuthenticationEncryptionSupported[1];
+};
+
+u8 key_2char2num(u8 hch, u8 lch);
+u8 key_char2num(u8 ch);
+u8 str_2char2num(u8 hch, u8 lch);
+
+#endif /* ifndef WLAN_BSSDEF_H_ */
diff --git a/drivers/staging/r8188eu/os_dep/ioctl_linux.c b/drivers/staging/r8188eu/os_dep/ioctl_linux.c
new file mode 100644
index 000000000..2de2e1e32
--- /dev/null
+++ b/drivers/staging/r8188eu/os_dep/ioctl_linux.c
@@ -0,0 +1,3785 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/wlan_bssdef.h"
+#include "../include/wifi.h"
+#include "../include/rtw_mlme.h"
+#include "../include/rtw_mlme_ext.h"
+#include "../include/rtw_ioctl.h"
+#include "../include/rtw_ioctl_set.h"
+#include "../include/usb_ops.h"
+#include "../include/rtl8188e_hal.h"
+#include "../include/rtw_led.h"
+
+#include "../include/rtw_iol.h"
+
+#define RTL_IOCTL_WPA_SUPPLICANT	(SIOCIWFIRSTPRIV + 30)
+
+#define SCAN_ITEM_SIZE 768
+#define MAX_CUSTOM_LEN 64
+#define RATE_COUNT 4
+
+/*  combo scan */
+#define WEXT_CSCAN_AMOUNT 9
+#define WEXT_CSCAN_BUF_LEN		360
+#define WEXT_CSCAN_HEADER		"CSCAN S\x01\x00\x00S\x00"
+#define WEXT_CSCAN_HEADER_SIZE		12
+#define WEXT_CSCAN_SSID_SECTION		'S'
+#define WEXT_CSCAN_CHANNEL_SECTION	'C'
+#define WEXT_CSCAN_NPROBE_SECTION	'N'
+#define WEXT_CSCAN_ACTV_DWELL_SECTION	'A'
+#define WEXT_CSCAN_PASV_DWELL_SECTION	'P'
+#define WEXT_CSCAN_HOME_DWELL_SECTION	'H'
+#define WEXT_CSCAN_TYPE_SECTION		'T'
+
+static u32 rtw_rates[] = {1000000, 2000000, 5500000, 11000000,
+	6000000, 9000000, 12000000, 18000000, 24000000, 36000000,
+	48000000, 54000000};
+
+void indicate_wx_scan_complete_event(struct adapter *padapter)
+{
+	union iwreq_data wrqu;
+
+	memset(&wrqu, 0, sizeof(union iwreq_data));
+	wireless_send_event(padapter->pnetdev, SIOCGIWSCAN, &wrqu, NULL);
+}
+
+void rtw_indicate_wx_assoc_event(struct adapter *padapter)
+{
+	union iwreq_data wrqu;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	memset(&wrqu, 0, sizeof(union iwreq_data));
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+
+	memcpy(wrqu.ap_addr.sa_data, pmlmepriv->cur_network.network.MacAddress, ETH_ALEN);
+
+	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
+}
+
+void rtw_indicate_wx_disassoc_event(struct adapter *padapter)
+{
+	union iwreq_data wrqu;
+
+	memset(&wrqu, 0, sizeof(union iwreq_data));
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+
+	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
+}
+
+static char *translate_scan(struct adapter *padapter,
+			    struct iw_request_info *info,
+			    struct wlan_network *pnetwork,
+			    char *start, char *stop)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct iw_event iwe;
+	u16 cap;
+	__le16 le_tmp;
+	u32 ht_ielen = 0;
+	char *custom;
+	char *p;
+	u16 max_rate = 0, rate, ht_cap = false;
+	u32 i = 0;
+	u8 bw_40MHz = 0, short_GI = 0;
+	u16 mcs_rate = 0;
+	u8 ss, sq;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+		u32	blnGotP2PIE = false;
+
+		/*	User is doing the P2P device discovery */
+		/*	The prefix of SSID should be "DIRECT-" and the IE should contains the P2P IE. */
+		/*	If not, the driver should ignore this AP and go to the next AP. */
+
+		/*	Verifying the SSID */
+		if (!memcmp(pnetwork->network.Ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN)) {
+			u32	p2pielen = 0;
+
+			if (pnetwork->network.Reserved[0] == 2) {/*  Probe Request */
+				/*	Verifying the P2P IE */
+				if (rtw_get_p2p_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &p2pielen))
+					blnGotP2PIE = true;
+			} else {/*  Beacon or Probe Respones */
+				/*	Verifying the P2P IE */
+				if (rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &p2pielen))
+					blnGotP2PIE = true;
+			}
+		}
+
+		if (!blnGotP2PIE)
+			return start;
+	}
+
+	/*  AP MAC address  */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+
+	memcpy(iwe.u.ap_addr.sa_data, pnetwork->network.MacAddress, ETH_ALEN);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
+
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	iwe.u.data.length = min_t(u16, pnetwork->network.Ssid.SsidLength, 32);
+	start = iwe_stream_add_point(info, start, stop, &iwe, pnetwork->network.Ssid.Ssid);
+
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pnetwork->network.IELength - 12);
+
+	if (p && ht_ielen > 0) {
+		struct ieee80211_ht_cap *pht_capie;
+
+		ht_cap = true;
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		memcpy(&mcs_rate, pht_capie->mcs.rx_mask, 2);
+		bw_40MHz = (le16_to_cpu(pht_capie->cap_info) &
+			    IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+		short_GI = (le16_to_cpu(pht_capie->cap_info) &
+			    (IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40)) ? 1 : 0;
+	}
+
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	if ((rtw_is_cckratesonly_included((u8 *)&pnetwork->network.SupportedRates))) {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bn");
+		else
+		snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11b");
+	} else if ((rtw_is_cckrates_included((u8 *)&pnetwork->network.SupportedRates))) {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bgn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bg");
+	} else {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11gn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11g");
+	}
+
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_CHAR_LEN);
+
+	  /* Add mode */
+	iwe.cmd = SIOCGIWMODE;
+	memcpy(&le_tmp, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
+
+	cap = le16_to_cpu(le_tmp);
+
+	if (cap & (WLAN_CAPABILITY_IBSS | WLAN_CAPABILITY_BSS)) {
+		if (cap & WLAN_CAPABILITY_BSS)
+			iwe.u.mode = IW_MODE_MASTER;
+		else
+			iwe.u.mode = IW_MODE_ADHOC;
+
+		start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_UINT_LEN);
+	}
+
+	if (pnetwork->network.Configuration.DSConfig < 1)
+		pnetwork->network.Configuration.DSConfig = 1;
+
+	 /* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+	iwe.u.freq.m = rtw_ch2freq(pnetwork->network.Configuration.DSConfig) * 100000;
+	iwe.u.freq.e = 1;
+	iwe.u.freq.i = pnetwork->network.Configuration.DSConfig;
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_FREQ_LEN);
+
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	if (cap & WLAN_CAPABILITY_PRIVACY)
+		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+	else
+		iwe.u.data.flags = IW_ENCODE_DISABLED;
+	iwe.u.data.length = 0;
+	start = iwe_stream_add_point(info, start, stop, &iwe, pnetwork->network.Ssid.Ssid);
+
+	/*Add basic and extended rates */
+	max_rate = 0;
+	custom = kzalloc(MAX_CUSTOM_LEN, GFP_ATOMIC);
+	if (!custom)
+		return start;
+	p = custom;
+	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom), " Rates (Mb/s): ");
+	while (pnetwork->network.SupportedRates[i] != 0) {
+		rate = pnetwork->network.SupportedRates[i] & 0x7F;
+		if (rate > max_rate)
+			max_rate = rate;
+		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+		i++;
+	}
+
+	if (ht_cap) {
+		if (mcs_rate & 0x8000)/* MCS15 */
+			max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
+		else if (mcs_rate & 0x0080)/* MCS7 */
+			;
+		else/* default MCS7 */
+			max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) : ((short_GI) ? 72 : 65);
+
+		max_rate = max_rate * 2;/* Mbps/2; */
+	}
+
+	iwe.cmd = SIOCGIWRATE;
+	iwe.u.bitrate.fixed = 0;
+	iwe.u.bitrate.disabled = 0;
+	iwe.u.bitrate.value = max_rate * 500000;
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_PARAM_LEN);
+
+	/* parsing WPA/WPA2 IE */
+	{
+		u8 *buf;
+		u8 *wpa_ie, *rsn_ie;
+		u16 wpa_len = 0, rsn_len = 0;
+		u8 *p;
+
+		buf = kzalloc(MAX_WPA_IE_LEN, GFP_ATOMIC);
+		if (!buf)
+			goto exit;
+		wpa_ie = kzalloc(255, GFP_ATOMIC);
+		if (!wpa_ie) {
+			kfree(buf);
+			goto exit;
+		}
+		rsn_ie = kzalloc(255, GFP_ATOMIC);
+		if (!rsn_ie) {
+			kfree(buf);
+			kfree(wpa_ie);
+			goto exit;
+		}
+		rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, rsn_ie, &rsn_len, wpa_ie, &wpa_len);
+
+		if (wpa_len > 0) {
+			p = buf;
+			memset(buf, 0, MAX_WPA_IE_LEN);
+			p += sprintf(p, "wpa_ie =");
+			for (i = 0; i < wpa_len; i++)
+				p += sprintf(p, "%02x", wpa_ie[i]);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVCUSTOM;
+			iwe.u.data.length = strlen(buf);
+			start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = wpa_len;
+			start = iwe_stream_add_point(info, start, stop, &iwe, wpa_ie);
+		}
+		if (rsn_len > 0) {
+			p = buf;
+			memset(buf, 0, MAX_WPA_IE_LEN);
+			p += sprintf(p, "rsn_ie =");
+			for (i = 0; i < rsn_len; i++)
+				p += sprintf(p, "%02x", rsn_ie[i]);
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVCUSTOM;
+			iwe.u.data.length = strlen(buf);
+			start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = rsn_len;
+			start = iwe_stream_add_point(info, start, stop, &iwe, rsn_ie);
+		}
+		kfree(buf);
+		kfree(wpa_ie);
+		kfree(rsn_ie);
+	}
+
+	{/* parsing WPS IE */
+		uint cnt = 0, total_ielen;
+		u8 *wpsie_ptr = NULL;
+		uint wps_ielen = 0;
+
+		u8 *ie_ptr = pnetwork->network.IEs + _FIXED_IE_LENGTH_;
+		total_ielen = pnetwork->network.IELength - _FIXED_IE_LENGTH_;
+
+		while (cnt < total_ielen) {
+			if (rtw_is_wps_ie(&ie_ptr[cnt], &wps_ielen) && (wps_ielen > 2)) {
+				wpsie_ptr = &ie_ptr[cnt];
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = (u16)wps_ielen;
+				start = iwe_stream_add_point(info, start, stop, &iwe, wpsie_ptr);
+			}
+			cnt += ie_ptr[cnt + 1] + 2; /* goto next */
+		}
+	}
+
+	/* Add quality statistics */
+	iwe.cmd = IWEVQUAL;
+	iwe.u.qual.updated = IW_QUAL_QUAL_UPDATED | IW_QUAL_LEVEL_UPDATED | IW_QUAL_NOISE_INVALID;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network)) {
+		ss = padapter->recvpriv.signal_strength;
+		sq = padapter->recvpriv.signal_qual;
+	} else {
+		ss = pnetwork->network.PhyInfo.SignalStrength;
+		sq = pnetwork->network.PhyInfo.SignalQuality;
+	}
+
+	iwe.u.qual.level = (u8)ss;
+	iwe.u.qual.qual = (u8)sq;   /*  signal quality */
+	iwe.u.qual.noise = 0; /*  noise level */
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+exit:
+	kfree(custom);
+	return start;
+}
+
+static int wpa_set_auth_algs(struct net_device *dev, u32 value)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	int ret = 0;
+
+	if ((value & AUTH_ALG_SHARED_KEY) && (value & AUTH_ALG_OPEN_SYSTEM)) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeAutoSwitch;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+	} else if (value & AUTH_ALG_SHARED_KEY) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeShared;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Shared;
+	} else if (value & AUTH_ALG_OPEN_SYSTEM) {
+		if (padapter->securitypriv.ndisauthtype < Ndis802_11AuthModeWPAPSK) {
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		}
+	} else if (!(value & AUTH_ALG_LEAP)) {
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param, u32 param_len)
+{
+	int ret = 0;
+	u32 wep_key_idx, wep_key_len, wep_total_len;
+	struct ndis_802_11_wep	 *pwep = NULL;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len < (u32)((u8 *)param->u.crypt.key - (u8 *)param) + param->u.crypt.key_len) {
+		ret =  -EINVAL;
+		goto exit;
+	}
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		if (param->u.crypt.idx >= WEP_KEYS) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	} else {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		if (wep_key_idx > WEP_KEYS)
+			return -EINVAL;
+
+		if (wep_key_len > 0) {
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+			wep_total_len = wep_key_len + sizeof(*pwep);
+			pwep = kzalloc(wep_total_len, GFP_KERNEL);
+			if (!pwep)
+				goto exit;
+
+			pwep->KeyLength = wep_key_len;
+			pwep->Length = wep_total_len;
+			if (wep_key_len == 13) {
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+				padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+			}
+		} else {
+			ret = -EINVAL;
+			goto exit;
+		}
+		pwep->KeyIndex = wep_key_idx;
+		pwep->KeyIndex |= 0x80000000;
+		memcpy(pwep->KeyMaterial,  param->u.crypt.key, pwep->KeyLength);
+		if (param->u.crypt.set_tx) {
+			if (rtw_set_802_11_add_wep(padapter, pwep) == (u8)_FAIL)
+				ret = -EOPNOTSUPP;
+		} else {
+			if (wep_key_idx >= WEP_KEYS) {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			}
+			memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0], pwep->KeyMaterial, pwep->KeyLength);
+			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->KeyLength;
+			rtw_set_key(padapter, psecuritypriv, wep_key_idx, 0);
+		}
+		goto exit;
+	}
+
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { /*  802_1x */
+		struct sta_info *psta, *pbcmc_sta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_MP_STATE)) { /* sta mode */
+			psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+			if (!psta) {
+				;
+			} else {
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					psta->ieee8021x_blocked = false;
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+				    (padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled))
+					psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+
+				if (param->u.crypt.set_tx == 1) { /* pairwise key */
+					memcpy(psta->dot118021x_UncstKey.skey,  param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					if (strcmp(param->u.crypt.alg, "TKIP") == 0) { /* set mic key */
+						memcpy(psta->dot11tkiptxmickey.skey, &param->u.crypt.key[16], 8);
+						memcpy(psta->dot11tkiprxmickey.skey, &param->u.crypt.key[24], 8);
+						padapter->securitypriv.busetkipkey = false;
+					}
+
+					rtw_setstakey_cmd(padapter, (unsigned char *)psta, true);
+				} else { /* group key */
+					memcpy(padapter->securitypriv.dot118021XGrpKey[param->u.crypt.idx].skey,  param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+					memcpy(padapter->securitypriv.dot118021XGrptxmickey[param->u.crypt.idx].skey, &param->u.crypt.key[16], 8);
+					memcpy(padapter->securitypriv.dot118021XGrprxmickey[param->u.crypt.idx].skey, &param->u.crypt.key[24], 8);
+					padapter->securitypriv.binstallGrpkey = true;
+
+					padapter->securitypriv.dot118021XGrpKeyid = param->u.crypt.idx;
+
+					rtw_set_key(padapter, &padapter->securitypriv, param->u.crypt.idx, 1);
+					if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_PROVISIONING_DONE);
+				}
+			}
+			pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+			if (!pbcmc_sta) {
+				;
+			} else {
+				/* Jeff: don't disable ieee8021x_blocked while clearing key */
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					pbcmc_sta->ieee8021x_blocked = false;
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+				    (padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled))
+					pbcmc_sta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+			}
+		}
+	}
+
+exit:
+
+	kfree(pwep);
+
+	return ret;
+}
+
+static int rtw_set_wpa_ie(struct adapter *padapter, char *pie, unsigned short ielen)
+{
+	u8 *buf = NULL;
+	int group_cipher = 0, pairwise_cipher = 0;
+	int ret = 0;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (ielen > MAX_WPA_IE_LEN || !pie) {
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		if (!pie)
+			return ret;
+		else
+			return -EINVAL;
+	}
+
+	if (ielen) {
+		buf = kmemdup(pie, ielen, GFP_KERNEL);
+		if (!buf) {
+			ret =  -ENOMEM;
+			goto exit;
+		}
+
+		if (ielen < RSN_HEADER_LEN) {
+			ret  = -1;
+			goto exit;
+		}
+
+		if (rtw_parse_wpa_ie(buf, ielen, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPAPSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &buf[0], ielen);
+		}
+
+		if (rtw_parse_wpa2_ie(buf, ielen, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPA2PSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &buf[0], ielen);
+		}
+
+		switch (group_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.dot118021XGrpPrivacy = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.dot118021XGrpPrivacy = _AES_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		}
+
+		switch (pairwise_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _AES_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		}
+
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		{/* set wps_ie */
+			u16 cnt = 0;
+			u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+			while (cnt < ielen) {
+				eid = buf[cnt];
+				if ((eid == _VENDOR_SPECIFIC_IE_) && (!memcmp(&buf[cnt + 2], wps_oui, 4))) {
+					padapter->securitypriv.wps_ie_len = ((buf[cnt + 1] + 2) < (MAX_WPA_IE_LEN << 2)) ? (buf[cnt + 1] + 2) : (MAX_WPA_IE_LEN << 2);
+
+					memcpy(padapter->securitypriv.wps_ie, &buf[cnt], padapter->securitypriv.wps_ie_len);
+
+					set_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS);
+					if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_OK))
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_PROVISIONING_ING);
+					cnt += buf[cnt + 1] + 2;
+					break;
+				} else {
+					cnt += buf[cnt + 1] + 2; /* goto next */
+				}
+			}
+		}
+	}
+
+exit:
+	kfree(buf);
+	return ret;
+}
+
+typedef unsigned char   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];
+
+static int rtw_wx_get_name(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u32 ht_ielen = 0;
+	char *p;
+	u8 ht_cap = false;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
+	NDIS_802_11_RATES_EX *prates = NULL;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		/* parsing HT_CAP_IE */
+		p = rtw_get_ie(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pcur_bss->IELength - 12);
+		if (p && ht_ielen > 0)
+			ht_cap = true;
+
+		prates = &pcur_bss->SupportedRates;
+
+		if (rtw_is_cckratesonly_included((u8 *)prates)) {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11b");
+		} else if (rtw_is_cckrates_included((u8 *)prates)) {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bgn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bg");
+		} else {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11gn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11g");
+		}
+	} else {
+		snprintf(wrqu->name, IFNAMSIZ, "unassociated");
+	}
+
+
+
+	return 0;
+}
+
+static int rtw_wx_get_freq(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		/* wrqu->freq.m = ieee80211_wlan_frequencies[pcur_bss->Configuration.DSConfig-1] * 100000; */
+		wrqu->freq.m = rtw_ch2freq(pcur_bss->Configuration.DSConfig) * 100000;
+		wrqu->freq.e = 1;
+		wrqu->freq.i = pcur_bss->Configuration.DSConfig;
+	} else {
+		wrqu->freq.m = rtw_ch2freq(padapter->mlmeextpriv.cur_channel) * 100000;
+		wrqu->freq.e = 1;
+		wrqu->freq.i = padapter->mlmeextpriv.cur_channel;
+	}
+
+	return 0;
+}
+
+static int rtw_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	enum ndis_802_11_network_infra networkType;
+	int ret = 0;
+
+	ret = rtw_pwr_wakeup(padapter);
+	if (ret)
+		goto exit;
+
+	if (!padapter->hw_init_completed) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	switch (wrqu->mode) {
+	case IW_MODE_AUTO:
+		networkType = Ndis802_11AutoUnknown;
+		break;
+	case IW_MODE_ADHOC:
+		networkType = Ndis802_11IBSS;
+		break;
+	case IW_MODE_MASTER:
+		networkType = Ndis802_11APMode;
+		break;
+	case IW_MODE_INFRA:
+		networkType = Ndis802_11Infrastructure;
+		break;
+	default:
+		ret = -EINVAL;
+		goto exit;
+	}
+	if (!rtw_set_802_11_infrastructure_mode(padapter, networkType)) {
+		ret = -EPERM;
+		goto exit;
+	}
+	rtw_setopmode_cmd(padapter, networkType);
+exit:
+
+	return ret;
+}
+
+static int rtw_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		wrqu->mode = IW_MODE_INFRA;
+	else if  ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+		  (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+		wrqu->mode = IW_MODE_ADHOC;
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		wrqu->mode = IW_MODE_MASTER;
+	else
+		wrqu->mode = IW_MODE_AUTO;
+
+
+
+	return 0;
+}
+
+static int rtw_wx_set_pmkid(struct net_device *dev,
+			    struct iw_request_info *a,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8   j, blInserted = false;
+	int  ret = false;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct iw_pmksa *pPMK = (struct iw_pmksa *)extra;
+	u8     strZeroMacAddress[ETH_ALEN] = {0x00};
+	u8     strIssueBssid[ETH_ALEN] = {0x00};
+
+	memcpy(strIssueBssid, pPMK->bssid.sa_data, ETH_ALEN);
+	if (pPMK->cmd == IW_PMKSA_ADD) {
+		if (!memcmp(strIssueBssid, strZeroMacAddress, ETH_ALEN))
+			return ret;
+		else
+			ret = true;
+		blInserted = false;
+
+		/* overwrite PMKID */
+		for (j = 0; j < NUM_PMKID_CACHE; j++) {
+			if (!memcmp(psecuritypriv->PMKIDList[j].Bssid, strIssueBssid, ETH_ALEN)) {
+				/*  BSSID is matched, the same AP => rewrite with new PMKID. */
+				memcpy(psecuritypriv->PMKIDList[j].PMKID, pPMK->pmkid, IW_PMKID_LEN);
+				psecuritypriv->PMKIDList[j].bUsed = true;
+				psecuritypriv->PMKIDIndex = j + 1;
+				blInserted = true;
+				break;
+			}
+		}
+
+		if (!blInserted) {
+			/*  Find a new entry */
+			memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].Bssid, strIssueBssid, ETH_ALEN);
+			memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].PMKID, pPMK->pmkid, IW_PMKID_LEN);
+
+			psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].bUsed = true;
+			psecuritypriv->PMKIDIndex++;
+			if (psecuritypriv->PMKIDIndex == 16)
+				psecuritypriv->PMKIDIndex = 0;
+		}
+	} else if (pPMK->cmd == IW_PMKSA_REMOVE) {
+		ret = true;
+		for (j = 0; j < NUM_PMKID_CACHE; j++) {
+			if (!memcmp(psecuritypriv->PMKIDList[j].Bssid, strIssueBssid, ETH_ALEN)) {
+				/*  BSSID is matched, the same AP => Remove this PMKID information and reset it. */
+				memset(psecuritypriv->PMKIDList[j].Bssid, 0x00, ETH_ALEN);
+				psecuritypriv->PMKIDList[j].bUsed = false;
+				break;
+			}
+	       }
+	} else if (pPMK->cmd == IW_PMKSA_FLUSH) {
+		memset(&psecuritypriv->PMKIDList[0], 0x00, sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+		psecuritypriv->PMKIDIndex = 0;
+		ret = true;
+	}
+	return ret;
+}
+
+static int rtw_wx_get_sens(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->sens.value = 0;
+	wrqu->sens.fixed = 0;	/* no auto select */
+	wrqu->sens.disabled = 1;
+	return 0;
+}
+
+static int rtw_wx_get_range(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct iw_range *range = (struct iw_range *)extra;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	u16 val;
+	int i;
+
+	wrqu->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+
+	/* Let's try to keep this struct in the same order as in
+	 * linux/include/wireless.h
+	 */
+
+	/* TODO: See what values we can set, and remove the ones we can't
+	 * set, or fill them with some default data.
+	 */
+
+	/* ~5 Mb/s real (802.11b) */
+	range->throughput = 5 * 1000 * 1000;
+
+	/* signal level threshold range */
+
+	/* percent values between 0 and 100. */
+	range->max_qual.qual = 100;
+	range->max_qual.level = 100;
+	range->max_qual.noise = 100;
+	range->max_qual.updated = 7; /* Updated all three */
+
+	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
+	/* TODO: Find real 'good' to 'bad' threshol value for RSSI */
+	range->avg_qual.level = 178; /* -78 dBm */
+	range->avg_qual.noise = 0;
+	range->avg_qual.updated = 7; /* Updated all three */
+
+	range->num_bitrates = RATE_COUNT;
+
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
+		range->bitrate[i] = rtw_rates[i];
+
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+
+	range->pm_capa = 0;
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 16;
+
+	for (i = 0, val = 0; i < MAX_CHANNEL_NUM; i++) {
+		/*  Include only legal frequencies for some countries */
+		if (pmlmeext->channel_set[i].ChannelNum != 0) {
+			range->freq[val].i = pmlmeext->channel_set[i].ChannelNum;
+			range->freq[val].m = rtw_ch2freq(pmlmeext->channel_set[i].ChannelNum) * 100000;
+			range->freq[val].e = 1;
+			val++;
+		}
+
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+
+	range->num_channels = val;
+	range->num_frequency = val;
+
+/*  The following code will proivde the security capability to network manager. */
+/*  If the driver doesn't provide this capability to network manager, */
+/*  the WPA/WPA2 routers can't be chosen in the network manager. */
+
+/*
+#define IW_SCAN_CAPA_NONE		0x00
+#define IW_SCAN_CAPA_ESSID		0x01
+#define IW_SCAN_CAPA_BSSID		0x02
+#define IW_SCAN_CAPA_CHANNEL		0x04
+#define IW_SCAN_CAPA_MODE		0x08
+#define IW_SCAN_CAPA_RATE		0x10
+#define IW_SCAN_CAPA_TYPE		0x20
+#define IW_SCAN_CAPA_TIME		0x40
+*/
+
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+			  IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+
+	range->scan_capa = IW_SCAN_CAPA_ESSID | IW_SCAN_CAPA_TYPE |
+			   IW_SCAN_CAPA_BSSID | IW_SCAN_CAPA_CHANNEL |
+			   IW_SCAN_CAPA_MODE | IW_SCAN_CAPA_RATE;
+
+
+	return 0;
+}
+
+/* set bssid flow */
+/* s1. rtw_set_802_11_infrastructure_mode() */
+/* s2. rtw_set_802_11_authentication_mode() */
+/* s3. set_802_11_encryption_mode() */
+/* s4. rtw_set_802_11_bssid() */
+static int rtw_wx_set_wap(struct net_device *dev,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+	uint ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct sockaddr *temp = (struct sockaddr *)awrq;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *phead;
+	u8 *dst_bssid, *src_bssid;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	enum ndis_802_11_auth_mode	authmode;
+
+	ret = rtw_pwr_wakeup(padapter);
+	if (ret)
+		goto exit;
+
+	if (!padapter->bup) {
+		ret = -1;
+		goto exit;
+	}
+
+	if (temp->sa_family != ARPHRD_ETHER) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	authmode = padapter->securitypriv.ndisauthtype;
+	spin_lock_bh(&queue->lock);
+	phead = get_list_head(queue);
+	pmlmepriv->pscanned = phead->next;
+
+	while (phead != pmlmepriv->pscanned) {
+
+		pnetwork = container_of(pmlmepriv->pscanned, struct wlan_network, list);
+
+		pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+
+		dst_bssid = pnetwork->network.MacAddress;
+
+		src_bssid = temp->sa_data;
+
+		if ((!memcmp(dst_bssid, src_bssid, ETH_ALEN))) {
+			if (!rtw_set_802_11_infrastructure_mode(padapter, pnetwork->network.InfrastructureMode)) {
+				ret = -1;
+				spin_unlock_bh(&queue->lock);
+				goto exit;
+			}
+
+			break;
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+
+	rtw_set_802_11_authentication_mode(padapter, authmode);
+	/* set_802_11_encryption_mode(padapter, padapter->securitypriv.ndisencryptstatus); */
+	if (!rtw_set_802_11_bssid(padapter, temp->sa_data)) {
+		ret = -1;
+		goto exit;
+	}
+
+exit:
+
+
+
+	return ret;
+}
+
+static int rtw_wx_get_wap(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
+
+	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+
+	memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		memcpy(wrqu->ap_addr.sa_data, pcur_bss->MacAddress, ETH_ALEN);
+	else
+		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+
+
+
+	return 0;
+}
+
+static int rtw_wx_set_mlme(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_mlme *mlme = (struct iw_mlme *)extra;
+
+	if (!mlme)
+		return -1;
+
+	switch (mlme->cmd) {
+	case IW_MLME_DEAUTH:
+		if (!rtw_set_802_11_disassociate(padapter))
+			ret = -1;
+		break;
+	case IW_MLME_DISASSOC:
+		if (!rtw_set_802_11_disassociate(padapter))
+			ret = -1;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return ret;
+}
+
+static int rtw_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *extra)
+{
+	u8 _status = false;
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ndis_802_11_ssid ssid[RTW_SSID_SCAN_AMOUNT];
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	ret = rtw_pwr_wakeup(padapter);
+	if (ret)
+		goto exit;
+
+	if (padapter->bDriverStopped) {
+		ret = -1;
+		goto exit;
+	}
+
+	if (!padapter->bup) {
+		ret = -1;
+		goto exit;
+	}
+
+	if (!padapter->hw_init_completed) {
+		ret = -1;
+		goto exit;
+	}
+
+	/*  When Busy Traffic, driver do not site survey. So driver return success. */
+	/*  wpa_supplicant will not issue SIOCSIWSCAN cmd again after scan timeout. */
+	/*  modify by thomas 2011-02-22. */
+	if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
+		indicate_wx_scan_complete_event(padapter);
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING)) {
+		indicate_wx_scan_complete_event(padapter);
+		goto exit;
+	}
+
+/*	For the DMP WiFi Display project, the driver won't to scan because */
+/*	the pmlmepriv->scan_interval is always equal to 3. */
+/*	So, the wpa_supplicant won't find out the WPS SoftAP. */
+
+	if (pwdinfo->p2p_state != P2P_STATE_NONE) {
+		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
+		rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_FULL);
+		rtw_free_network_queue(padapter, true);
+	}
+
+	memset(ssid, 0, sizeof(struct ndis_802_11_ssid) * RTW_SSID_SCAN_AMOUNT);
+
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		struct iw_scan_req *req = (struct iw_scan_req *)extra;
+
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			int len = min((int)req->essid_len, IW_ESSID_MAX_SIZE);
+
+			memcpy(ssid[0].Ssid, req->essid, len);
+			ssid[0].SsidLength = len;
+
+			spin_lock_bh(&pmlmepriv->lock);
+
+			_status = rtw_sitesurvey_cmd(padapter, ssid, 1);
+
+			spin_unlock_bh(&pmlmepriv->lock);
+		}
+	} else {
+		if (wrqu->data.length >= WEXT_CSCAN_HEADER_SIZE &&
+		    !memcmp(extra, WEXT_CSCAN_HEADER, WEXT_CSCAN_HEADER_SIZE)) {
+			int len = wrqu->data.length - WEXT_CSCAN_HEADER_SIZE;
+			char *pos = extra + WEXT_CSCAN_HEADER_SIZE;
+			char section;
+			char sec_len;
+			int ssid_index = 0;
+
+			while (len >= 1) {
+				section = *(pos++);
+				len -= 1;
+
+				switch (section) {
+				case WEXT_CSCAN_SSID_SECTION:
+					if (len < 1) {
+						len = 0;
+						break;
+					}
+					sec_len = *(pos++); len -= 1;
+					if (sec_len > 0 &&
+					    sec_len <= len &&
+					    sec_len <= 32) {
+						ssid[ssid_index].SsidLength = sec_len;
+						memcpy(ssid[ssid_index].Ssid, pos, sec_len);
+						ssid_index++;
+					}
+					pos += sec_len;
+					len -= sec_len;
+					break;
+				case WEXT_CSCAN_TYPE_SECTION:
+				case WEXT_CSCAN_CHANNEL_SECTION:
+					pos += 1;
+					len -= 1;
+					break;
+				case WEXT_CSCAN_PASV_DWELL_SECTION:
+				case WEXT_CSCAN_HOME_DWELL_SECTION:
+				case WEXT_CSCAN_ACTV_DWELL_SECTION:
+					pos += 2;
+					len -= 2;
+					break;
+				default:
+					len = 0; /*  stop parsing */
+				}
+			}
+
+			/* it has still some scan parameter to parse, we only do this now... */
+			_status = rtw_set_802_11_bssid_list_scan(padapter, ssid, RTW_SSID_SCAN_AMOUNT);
+		} else {
+			_status = rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+		}
+	}
+
+	if (!_status)
+		ret = -1;
+
+exit:
+
+	return ret;
+}
+
+static int rtw_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct list_head *plist, *phead;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	char *ev = extra;
+	char *stop = ev + wrqu->data.length;
+	u32 ret = 0;
+	u32 cnt = 0;
+	u32 wait_for_surveydone;
+	int wait_status;
+	struct	wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+		/*	P2P is enabled */
+		wait_for_surveydone = 200;
+	} else {
+		/*	P2P is disabled */
+		wait_for_surveydone = 100;
+	}
+
+	wait_status = _FW_UNDER_SURVEY | _FW_UNDER_LINKING;
+
+	while (check_fwstate(pmlmepriv, wait_status)) {
+		msleep(30);
+		cnt++;
+		if (cnt > wait_for_surveydone)
+			break;
+	}
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		if ((stop - ev) < SCAN_ITEM_SIZE) {
+			ret = -E2BIG;
+			break;
+		}
+
+		pnetwork = container_of(plist, struct wlan_network, list);
+
+		/* report network only if the current channel set contains the channel to which this network belongs */
+		if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, pnetwork->network.Configuration.DSConfig) >= 0)
+			ev = translate_scan(padapter, a, pnetwork, ev, stop);
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	wrqu->data.length = ev - extra;
+	wrqu->data.flags = 0;
+
+	return ret;
+}
+
+/* set ssid flow */
+/* s1. rtw_set_802_11_infrastructure_mode() */
+/* s2. set_802_11_authenticaion_mode() */
+/* s3. set_802_11_encryption_mode() */
+/* s4. rtw_set_802_11_ssid() */
+static int rtw_wx_set_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __queue *queue = &pmlmepriv->scanned_queue;
+	struct list_head *phead;
+	struct wlan_network *pnetwork = NULL;
+	enum ndis_802_11_auth_mode authmode;
+	struct ndis_802_11_ssid ndis_ssid;
+	u8 *dst_ssid, *src_ssid;
+
+	uint ret = 0, len;
+
+	ret = rtw_pwr_wakeup(padapter);
+	if (ret)
+		goto exit;
+
+	if (!padapter->bup) {
+		ret = -1;
+		goto exit;
+	}
+
+	if (wrqu->essid.length > IW_ESSID_MAX_SIZE) {
+		ret = -E2BIG;
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		ret = -1;
+		goto exit;
+	}
+
+	authmode = padapter->securitypriv.ndisauthtype;
+	if (wrqu->essid.flags && wrqu->essid.length) {
+		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? wrqu->essid.length : IW_ESSID_MAX_SIZE;
+
+		memset(&ndis_ssid, 0, sizeof(struct ndis_802_11_ssid));
+		ndis_ssid.SsidLength = len;
+		memcpy(ndis_ssid.Ssid, extra, len);
+		src_ssid = ndis_ssid.Ssid;
+
+		spin_lock_bh(&queue->lock);
+		phead = get_list_head(queue);
+		pmlmepriv->pscanned = phead->next;
+
+		while (phead != pmlmepriv->pscanned) {
+			pnetwork = container_of(pmlmepriv->pscanned, struct wlan_network, list);
+
+			pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+
+			dst_ssid = pnetwork->network.Ssid.Ssid;
+
+			if ((!memcmp(dst_ssid, src_ssid, ndis_ssid.SsidLength)) &&
+			    (pnetwork->network.Ssid.SsidLength == ndis_ssid.SsidLength)) {
+
+				if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+					if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+						continue;
+				}
+
+				if (!rtw_set_802_11_infrastructure_mode(padapter, pnetwork->network.InfrastructureMode)) {
+					ret = -1;
+					spin_unlock_bh(&queue->lock);
+					goto exit;
+				}
+
+				break;
+			}
+		}
+		spin_unlock_bh(&queue->lock);
+		rtw_set_802_11_authentication_mode(padapter, authmode);
+		if (!rtw_set_802_11_ssid(padapter, &ndis_ssid)) {
+			ret = -1;
+			goto exit;
+		}
+	}
+
+exit:
+	return ret;
+}
+
+static int rtw_wx_get_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra)
+{
+	u32 len;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))) {
+		len = pcur_bss->Ssid.SsidLength;
+		memcpy(extra, pcur_bss->Ssid.Ssid, len);
+	} else {
+		len = 0;
+		*extra = 0;
+	}
+	wrqu->essid.length = len;
+	wrqu->essid.flags = 1;
+
+	return 0;
+}
+
+static int rtw_wx_set_rate(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra)
+{
+	int i, ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 datarates[NumRates];
+	u32	target_rate = wrqu->bitrate.value;
+	u32	fixed = wrqu->bitrate.fixed;
+	u32	ratevalue = 0;
+	u8 mpdatarate[NumRates] = {11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 0xff};
+
+	if (target_rate == -1) {
+		ratevalue = 11;
+		goto set_rate;
+	}
+	target_rate = target_rate / 100000;
+
+	switch (target_rate) {
+	case 10:
+		ratevalue = 0;
+		break;
+	case 20:
+		ratevalue = 1;
+		break;
+	case 55:
+		ratevalue = 2;
+		break;
+	case 60:
+		ratevalue = 3;
+		break;
+	case 90:
+		ratevalue = 4;
+		break;
+	case 110:
+		ratevalue = 5;
+		break;
+	case 120:
+		ratevalue = 6;
+		break;
+	case 180:
+		ratevalue = 7;
+		break;
+	case 240:
+		ratevalue = 8;
+		break;
+	case 360:
+		ratevalue = 9;
+		break;
+	case 480:
+		ratevalue = 10;
+		break;
+	case 540:
+		ratevalue = 11;
+		break;
+	default:
+		ratevalue = 11;
+		break;
+	}
+
+set_rate:
+
+	for (i = 0; i < NumRates; i++) {
+		if (ratevalue == mpdatarate[i]) {
+			datarates[i] = mpdatarate[i];
+			if (fixed == 0)
+				break;
+		} else {
+			datarates[i] = 0xff;
+		}
+	}
+
+	if (rtw_setdatarate_cmd(padapter, datarates) != _SUCCESS)
+		ret = -1;
+
+	return ret;
+}
+
+static int rtw_wx_get_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	u16 max_rate = 0;
+
+	max_rate = rtw_get_cur_max_rate((struct adapter *)rtw_netdev_priv(dev));
+
+	if (max_rate == 0)
+		return -EPERM;
+
+	wrqu->bitrate.fixed = 0;	/* no auto select */
+	wrqu->bitrate.value = max_rate * 100000;
+
+	return 0;
+}
+
+static int rtw_wx_set_rts(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+
+
+	if (wrqu->rts.disabled) {
+		padapter->registrypriv.rts_thresh = 2347;
+	} else {
+		if (wrqu->rts.value < 0 ||
+		    wrqu->rts.value > 2347)
+			return -EINVAL;
+
+		padapter->registrypriv.rts_thresh = wrqu->rts.value;
+	}
+
+	return 0;
+}
+
+static int rtw_wx_get_rts(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+
+
+	wrqu->rts.value = padapter->registrypriv.rts_thresh;
+	wrqu->rts.fixed = 0;	/* no auto select */
+	/* wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD); */
+
+
+
+	return 0;
+}
+
+static int rtw_wx_set_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+
+
+	if (wrqu->frag.disabled) {
+		padapter->xmitpriv.frag_len = MAX_FRAG_THRESHOLD;
+	} else {
+		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
+		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
+			return -EINVAL;
+
+		padapter->xmitpriv.frag_len = wrqu->frag.value & ~0x1;
+	}
+
+	return 0;
+}
+
+static int rtw_wx_get_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+
+
+	wrqu->frag.value = padapter->xmitpriv.frag_len;
+	wrqu->frag.fixed = 0;	/* no auto select */
+
+
+
+	return 0;
+}
+
+static int rtw_wx_get_retry(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->retry.value = 7;
+	wrqu->retry.fixed = 0;	/* no auto select */
+	wrqu->retry.disabled = 1;
+
+	return 0;
+}
+
+static int rtw_wx_set_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	u32 key, ret = 0;
+	u32 keyindex_provided;
+	struct ndis_802_11_wep	 wep;
+	enum ndis_802_11_auth_mode authmode;
+
+	struct iw_point *erq = &wrqu->encoding;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	memset(&wep, 0, sizeof(struct ndis_802_11_wep));
+
+	key = erq->flags & IW_ENCODE_INDEX;
+
+
+
+	if (erq->flags & IW_ENCODE_DISABLED) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+
+		goto exit;
+	}
+
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+		keyindex_provided = 1;
+	} else {
+		keyindex_provided = 0;
+		key = padapter->securitypriv.dot11PrivacyKeyIndex;
+	}
+
+	/* set authentication mode */
+	if (erq->flags & IW_ENCODE_OPEN) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;/* Ndis802_11EncryptionDisabled; */
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+	} else if (erq->flags & IW_ENCODE_RESTRICTED) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Shared;
+		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+		authmode = Ndis802_11AuthModeShared;
+		padapter->securitypriv.ndisauthtype = authmode;
+	} else {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;/* Ndis802_11EncryptionDisabled; */
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+	}
+
+	wep.KeyIndex = key;
+	if (erq->length > 0) {
+		wep.KeyLength = erq->length <= 5 ? 5 : 13;
+
+		wep.Length = wep.KeyLength + offsetof(struct ndis_802_11_wep, KeyMaterial);
+	} else {
+		wep.KeyLength = 0;
+
+		if (keyindex_provided == 1) {
+			/*  set key_id only, no given KeyMaterial(erq->length == 0). */
+			padapter->securitypriv.dot11PrivacyKeyIndex = key;
+
+			switch (padapter->securitypriv.dot11DefKeylen[key]) {
+			case 5:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+				break;
+			case 13:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+				break;
+			default:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+				break;
+			}
+
+			goto exit;
+		}
+	}
+
+	wep.KeyIndex |= 0x80000000;
+
+	memcpy(wep.KeyMaterial, keybuf, wep.KeyLength);
+
+	if (!rtw_set_802_11_add_wep(padapter, &wep)) {
+		if (rf_on == pwrpriv->rf_pwrstate)
+			ret = -EOPNOTSUPP;
+		goto exit;
+	}
+
+exit:
+
+
+
+	return ret;
+}
+
+static int rtw_wx_get_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	uint key;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_point *erq = &wrqu->encoding;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) != true) {
+		if (!check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			erq->length = 0;
+			erq->flags |= IW_ENCODE_DISABLED;
+			return 0;
+		}
+	}
+
+	key = erq->flags & IW_ENCODE_INDEX;
+
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+	} else {
+		key = padapter->securitypriv.dot11PrivacyKeyIndex;
+	}
+
+	erq->flags = key + 1;
+
+	switch (padapter->securitypriv.ndisencryptstatus) {
+	case Ndis802_11EncryptionNotSupported:
+	case Ndis802_11EncryptionDisabled:
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		break;
+	case Ndis802_11Encryption1Enabled:
+		erq->length = padapter->securitypriv.dot11DefKeylen[key];
+		if (erq->length) {
+			memcpy(keybuf, padapter->securitypriv.dot11DefKey[key].skey, padapter->securitypriv.dot11DefKeylen[key]);
+
+			erq->flags |= IW_ENCODE_ENABLED;
+
+			if (padapter->securitypriv.ndisauthtype == Ndis802_11AuthModeOpen)
+				erq->flags |= IW_ENCODE_OPEN;
+			else if (padapter->securitypriv.ndisauthtype == Ndis802_11AuthModeShared)
+				erq->flags |= IW_ENCODE_RESTRICTED;
+		} else {
+			erq->length = 0;
+			erq->flags |= IW_ENCODE_DISABLED;
+		}
+		break;
+	case Ndis802_11Encryption2Enabled:
+	case Ndis802_11Encryption3Enabled:
+		erq->length = 16;
+		erq->flags |= (IW_ENCODE_ENABLED | IW_ENCODE_OPEN | IW_ENCODE_NOKEY);
+		break;
+	default:
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		break;
+	}
+
+
+	return 0;
+}
+
+static int rtw_wx_get_power(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->power.value = 0;
+	wrqu->power.fixed = 0;	/* no auto select */
+	wrqu->power.disabled = 1;
+
+	return 0;
+}
+
+static int rtw_wx_set_gen_ie(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	ret = rtw_set_wpa_ie(padapter, extra, wrqu->data.length);
+	return ret;
+}
+
+static int rtw_wx_set_auth(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_param *param = (struct iw_param *)&wrqu->param;
+	int ret = 0;
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_WPA_VERSION:
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+
+		break;
+	case IW_AUTH_CIPHER_GROUP:
+
+		break;
+	case IW_AUTH_KEY_MGMT:
+		/*
+		 *  ??? does not use these parameters
+		 */
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		if (param->value) {
+			/*  wpa_supplicant is enabling the tkip countermeasure. */
+			padapter->securitypriv.btkip_countermeasure = true;
+		} else {
+			/*  wpa_supplicant is disabling the tkip countermeasure. */
+			padapter->securitypriv.btkip_countermeasure = false;
+		}
+		break;
+	case IW_AUTH_DROP_UNENCRYPTED:
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+
+		if (padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption1Enabled)
+			break;/* it means init value, or using wep, ndisencryptstatus = Ndis802_11Encryption1Enabled, */
+					/*  then it needn't reset it; */
+
+		if (param->value) {
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+			padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+			padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+		}
+
+		break;
+	case IW_AUTH_80211_AUTH_ALG:
+		/*
+		 *  It's the starting point of a link layer connection using wpa_supplicant
+		*/
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+			LeaveAllPowerSaveMode(padapter);
+			rtw_disassoc_cmd(padapter, 500, false);
+			rtw_indicate_disconnect(padapter);
+			rtw_free_assoc_resources(padapter, 1);
+		}
+		ret = wpa_set_auth_algs(dev, (u32)param->value);
+		break;
+	case IW_AUTH_WPA_ENABLED:
+		break;
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+static int rtw_wx_set_enc_ext(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	char *alg_name;
+	u32 param_len;
+	struct ieee_param *param = NULL;
+	struct iw_point *pencoding = &wrqu->encoding;
+	struct iw_encode_ext *pext = (struct iw_encode_ext *)extra;
+	int ret = -1;
+
+	param_len = sizeof(struct ieee_param) + pext->key_len;
+	param = kzalloc(param_len, GFP_KERNEL);
+	if (!param)
+		return -ENOMEM;
+
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	memset(param->sta_addr, 0xff, ETH_ALEN);
+
+	switch (pext->alg) {
+	case IW_ENCODE_ALG_NONE:
+		/* todo: remove key */
+		/* remove = 1; */
+		alg_name = "none";
+		break;
+	case IW_ENCODE_ALG_WEP:
+		alg_name = "WEP";
+		break;
+	case IW_ENCODE_ALG_TKIP:
+		alg_name = "TKIP";
+		break;
+	case IW_ENCODE_ALG_CCMP:
+		alg_name = "CCMP";
+		break;
+	default:
+		goto out;
+	}
+
+	strscpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
+
+	if (pext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		param->u.crypt.set_tx = 1;
+
+	/* cliW: WEP does not have group key
+	 * just not checking GROUP key setting
+	 */
+	if ((pext->alg != IW_ENCODE_ALG_WEP) &&
+	    (pext->ext_flags & IW_ENCODE_EXT_GROUP_KEY))
+		param->u.crypt.set_tx = 0;
+
+	param->u.crypt.idx = (pencoding->flags & 0x00FF) - 1;
+
+	if (pext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID)
+		memcpy(param->u.crypt.seq, pext->rx_seq, 8);
+
+	if (pext->key_len) {
+		param->u.crypt.key_len = pext->key_len;
+		memcpy(param->u.crypt.key, pext + 1, pext->key_len);
+	}
+
+	ret =  wpa_set_encryption(dev, param, param_len);
+
+out:
+	kfree(param);
+	return ret;
+}
+
+static int rtw_wx_get_nick(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu, char *extra)
+{
+	if (extra) {
+		wrqu->data.length = 14;
+		wrqu->data.flags = 1;
+		memcpy(extra, "<WIFI@REALTEK>", 14);
+	}
+
+	/* dump debug info here */
+	return 0;
+}
+
+static int rtw_wx_read_rf(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u32 path, addr, data32;
+
+	path = *(u32 *)extra;
+	if (path != RF_PATH_A)
+		return -EINVAL;
+
+	addr = *((u32 *)extra + 1);
+	data32 = rtl8188e_PHY_QueryRFReg(padapter, addr, 0xFFFFF);
+	/*
+	 * IMPORTANT!!
+	 * Only when wireless private ioctl is at odd order,
+	 * "extra" would be copied to user space.
+	 */
+	sprintf(extra, "0x%05x", data32);
+
+	return 0;
+}
+
+static int rtw_wx_write_rf(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u32 path, addr, data32;
+
+	path = *(u32 *)extra;
+	if (path != RF_PATH_A)
+		return -EINVAL;
+
+	addr = *((u32 *)extra + 1);
+	data32 = *((u32 *)extra + 2);
+	rtl8188e_PHY_SetRFReg(padapter, addr, 0xFFFFF, data32);
+
+	return 0;
+}
+
+static int rtw_wx_set_channel_plan(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 channel_plan_req = (u8)(*((int *)wrqu));
+
+	if (rtw_set_chplan_cmd(padapter, channel_plan_req) != _SUCCESS)
+		return -EPERM;
+
+	return 0;
+}
+
+static int rtw_get_ap_info(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	u32 cnt = 0, wpa_ielen;
+	struct list_head *plist, *phead;
+	unsigned char *pbuf;
+	u8 bssid[ETH_ALEN];
+	char data[32];
+	struct wlan_network *pnetwork = NULL;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __queue *queue = &pmlmepriv->scanned_queue;
+	struct iw_point *pdata = &wrqu->data;
+
+	if (padapter->bDriverStopped || !pdata) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	while ((check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY | _FW_UNDER_LINKING)))) {
+		msleep(30);
+		cnt++;
+		if (cnt > 100)
+			break;
+	}
+	pdata->flags = 0;
+	if (pdata->length >= 32) {
+		if (copy_from_user(data, pdata->pointer, 32)) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	} else {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+
+		if (!mac_pton(data, bssid)) {
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+			return -EINVAL;
+		}
+
+		if (!memcmp(bssid, pnetwork->network.MacAddress, ETH_ALEN)) {
+			/* BSSID match, then check if supporting wpa/wpa2 */
+			pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+			if (pbuf && (wpa_ielen > 0)) {
+				pdata->flags = 1;
+				break;
+			}
+
+			pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+			if (pbuf && (wpa_ielen > 0)) {
+				pdata->flags = 2;
+				break;
+			}
+		}
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (pdata->length >= 34) {
+		if (copy_to_user(pdata->pointer + 32, (u8 *)&pdata->flags, 1)) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	}
+
+exit:
+
+	return ret;
+}
+
+static int rtw_set_pid(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	int *pdata = (int *)wrqu;
+	int selector;
+
+	if (padapter->bDriverStopped || !pdata) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	selector = *pdata;
+	if (selector < 3 && selector >= 0) {
+		padapter->pid[selector] = *(pdata + 1);
+		ui_pid[selector] = *(pdata + 1);
+	}
+exit:
+	return ret;
+}
+
+static int rtw_wps_start(struct net_device *dev,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_point *pdata = &wrqu->data;
+	u32   u32wps_start = 0;
+
+	if (!pdata)
+		return -EINVAL;
+	ret = copy_from_user((void *)&u32wps_start, pdata->pointer, 4);
+	if (ret) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (padapter->bDriverStopped) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (u32wps_start == 0)
+		u32wps_start = *extra;
+
+	if (u32wps_start == 1) /*  WPS Start */
+		rtw_led_control(padapter, LED_CTL_START_WPS);
+	else if (u32wps_start == 2) /*  WPS Stop because of wps success */
+		rtw_led_control(padapter, LED_CTL_STOP_WPS);
+	else if (u32wps_start == 3) /*  WPS Stop because of wps fail */
+		rtw_led_control(padapter, LED_CTL_STOP_WPS_FAIL);
+
+exit:
+	return ret;
+}
+
+static int rtw_wext_p2p_enable(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	enum P2P_ROLE init_role = P2P_ROLE_DISABLE;
+
+	if (*extra == '0')
+		init_role = P2P_ROLE_DISABLE;
+	else if (*extra == '1')
+		init_role = P2P_ROLE_DEVICE;
+	else if (*extra == '2')
+		init_role = P2P_ROLE_CLIENT;
+	else if (*extra == '3')
+		init_role = P2P_ROLE_GO;
+
+	ret = rtw_p2p_enable(padapter, init_role);
+	if (ret)
+		return ret;
+
+	/* set channel/bandwidth */
+	if (init_role != P2P_ROLE_DISABLE) {
+		u8 channel, ch_offset;
+		u16 bwmode;
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)) {
+			/*	Stay at the listen state and wait for discovery. */
+			channel = pwdinfo->listen_channel;
+			pwdinfo->operating_channel = pwdinfo->listen_channel;
+			ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			bwmode = HT_CHANNEL_WIDTH_20;
+		} else {
+			pwdinfo->operating_channel = pmlmeext->cur_channel;
+
+			channel = pwdinfo->operating_channel;
+			ch_offset = pmlmeext->cur_ch_offset;
+			bwmode = pmlmeext->cur_bwmode;
+		}
+
+		set_channel_bwmode(padapter, channel, ch_offset, bwmode);
+	}
+
+	return 0;
+}
+
+static void rtw_p2p_set_go_nego_ssid(struct net_device *dev,
+				     struct iw_request_info *info,
+				     union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	memcpy(pwdinfo->nego_ssid, extra, strlen(extra));
+	pwdinfo->nego_ssidlen = strlen(extra);
+}
+
+static int rtw_p2p_set_intent(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 intent = pwdinfo->intent;
+
+	switch (wrqu->data.length) {
+	case 1:
+		intent = extra[0] - '0';
+		break;
+	case 2:
+		intent = str_2char2num(extra[0], extra[1]);
+		break;
+	}
+	if (intent <= 15)
+		pwdinfo->intent = intent;
+	else
+		ret = -1;
+	return ret;
+}
+
+static int rtw_p2p_set_listen_ch(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 listen_ch = pwdinfo->listen_channel;	/*	Listen channel number */
+
+	switch (wrqu->data.length) {
+	case 1:
+		listen_ch = extra[0] - '0';
+		break;
+	case 2:
+		listen_ch = str_2char2num(extra[0], extra[1]);
+		break;
+	}
+
+	if ((listen_ch == 1) || (listen_ch == 6) || (listen_ch == 11)) {
+		pwdinfo->listen_channel = listen_ch;
+		set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	} else {
+		ret = -1;
+	}
+
+	return ret;
+}
+
+static int rtw_p2p_set_op_ch(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+/*	Commented by Albert 20110524 */
+/*	This function is used to set the operating channel if the driver will become the group owner */
+
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 op_ch = pwdinfo->operating_channel;	/*	Operating channel number */
+
+	switch (wrqu->data.length) {
+	case 1:
+		op_ch = extra[0] - '0';
+		break;
+	case 2:
+		op_ch = str_2char2num(extra[0], extra[1]);
+		break;
+	}
+
+	if (op_ch > 0)
+		pwdinfo->operating_channel = op_ch;
+	else
+		ret = -1;
+
+	return ret;
+}
+
+static int rtw_p2p_profilefound(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*	Comment by Albert 2010/10/13 */
+	/*	Input data format: */
+	/*	Ex:  0 */
+	/*	Ex:  1XX:XX:XX:XX:XX:XXYYSSID */
+	/*	0 => Reflush the profile record list. */
+	/*	1 => Add the profile list */
+	/*	XX:XX:XX:XX:XX:XX => peer's MAC Address (ex: 00:E0:4C:00:00:01) */
+	/*	YY => SSID Length */
+	/*	SSID => SSID for persistence group */
+
+	/*	The upper application should pass the SSID to driver by using this rtw_p2p_profilefound function. */
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+		if (extra[0] == '0') {
+			/*	Remove all the profile information of wifidirect_info structure. */
+			memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
+			pwdinfo->profileindex = 0;
+		} else {
+			if (pwdinfo->profileindex >= P2P_MAX_PERSISTENT_GROUP_NUM) {
+				ret = -1;
+			} else {
+				int jj, kk;
+
+				/*	Add this profile information into pwdinfo->profileinfo */
+				/*	Ex:  1XX:XX:XX:XX:XX:XXYYSSID */
+				for (jj = 0, kk = 1; jj < ETH_ALEN; jj++, kk += 3)
+					pwdinfo->profileinfo[pwdinfo->profileindex].peermac[jj] = key_2char2num(extra[kk], extra[kk + 1]);
+
+				pwdinfo->profileinfo[pwdinfo->profileindex].ssidlen = (extra[18] - '0') * 10 + (extra[19] - '0');
+				memcpy(pwdinfo->profileinfo[pwdinfo->profileindex].ssid, &extra[20], pwdinfo->profileinfo[pwdinfo->profileindex].ssidlen);
+				pwdinfo->profileindex++;
+			}
+		}
+	}
+
+	return ret;
+}
+
+static void rtw_p2p_setDN(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
+	memcpy(pwdinfo->device_name, extra, wrqu->data.length - 1);
+	pwdinfo->device_name_len = wrqu->data.length - 1;
+}
+
+static int rtw_p2p_get_wps_configmethod(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	u8 peerMACStr[17] = {0x00};
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u8 blnMatch = 0;
+	u16	attr_content = 0;
+	uint attr_contentlen = 0;
+	/* 6 is the string "wpsCM =", 17 is the MAC addr, we have to clear it at wrqu->data.pointer */
+	u8 attr_content_str[6 + 17] = {0x00};
+
+	/*	Commented by Albert 20110727 */
+	/*	The input data is the MAC address which the application wants to know its WPS config method. */
+	/*	After knowing its WPS config method, the application can decide the config method for provisioning discovery. */
+	/*	Format: iwpriv wlanx p2p_get_wpsCM 00:E0:4C:00:00:05 */
+
+	if (copy_from_user(peerMACStr, wrqu->data.pointer + 6, 17))
+		return -EFAULT;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(peerMACStr[kk], peerMACStr[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			u8 *wpsie;
+			uint wpsie_len = 0;
+			__be16 be_tmp;
+
+			/*  The mac address is matched. */
+			wpsie = rtw_get_wps_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &wpsie_len);
+			if (wpsie) {
+				rtw_get_wps_attr_content(wpsie, wpsie_len, WPS_ATTR_CONF_METHOD, (u8 *)&be_tmp, &attr_contentlen);
+				if (attr_contentlen) {
+					attr_content = be16_to_cpu(be_tmp);
+					sprintf(attr_content_str, "\n\nM =%.4d", attr_content);
+					blnMatch = 1;
+				}
+			}
+			break;
+		}
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (!blnMatch)
+		sprintf(attr_content_str, "\n\nM = 0000");
+
+	if (copy_to_user(wrqu->data.pointer, attr_content_str, 6 + 17))
+		return -EFAULT;
+	return 0;
+}
+
+static int rtw_p2p_get_go_device_address(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	u8 peerMACStr[17] = {0x00};
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u8 blnMatch = 0;
+	u8 *p2pie;
+	uint p2pielen = 0, attr_contentlen = 0;
+	u8 attr_content[100] = {0x00};
+
+	u8 go_devadd_str[100 + 10] = {0x00};
+	/*  +10 is for the str "go_devadd =", we have to clear it at wrqu->data.pointer */
+
+	/*	Commented by Albert 20121209 */
+	/*	The input data is the GO's interface address which the application wants to know its device address. */
+	/*	Format: iwpriv wlanx p2p_get2 go_devadd = 00:E0:4C:00:00:05 */
+
+	if (copy_from_user(peerMACStr, wrqu->data.pointer + 10, 17))
+		return -EFAULT;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(peerMACStr[kk], peerMACStr[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			/*	Commented by Albert 2011/05/18 */
+			/*	Match the device address located in the P2P IE */
+			/*	This is for the case that the P2P device address is not the same as the P2P interface address. */
+
+			p2pie = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &p2pielen);
+			if (p2pie) {
+				while (p2pie) {
+					/*	The P2P Device ID attribute is included in the Beacon frame. */
+					/*	The P2P Device Info attribute is included in the probe response frame. */
+
+					memset(attr_content, 0x00, 100);
+					if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_ID, attr_content, &attr_contentlen)) {
+						/*	Handle the P2P Device ID attribute of Beacon first */
+						blnMatch = 1;
+						break;
+					} else if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_INFO, attr_content, &attr_contentlen)) {
+						/*	Handle the P2P Device Info attribute of probe response */
+						blnMatch = 1;
+						break;
+					}
+
+					/* Get the next P2P IE */
+					p2pie = rtw_get_p2p_ie(p2pie + p2pielen, pnetwork->network.IELength - 12 - (p2pie - &pnetwork->network.IEs[12] + p2pielen), NULL, &p2pielen);
+				}
+			}
+	     }
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (!blnMatch)
+		sprintf(go_devadd_str, "\n\ndev_add = NULL");
+	else
+		sprintf(go_devadd_str, "\ndev_add =%.2X:%.2X:%.2X:%.2X:%.2X:%.2X",
+			attr_content[0], attr_content[1], attr_content[2], attr_content[3], attr_content[4], attr_content[5]);
+
+	if (copy_to_user(wrqu->data.pointer, go_devadd_str, 10 + 17))
+		return -EFAULT;
+	return 0;
+}
+
+static int rtw_p2p_get_device_type(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	u8 peerMACStr[17] = {0x00};
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u8 blnMatch = 0;
+	u8 dev_type[8] = {0x00};
+	uint dev_type_len = 0;
+	u8 dev_type_str[17 + 9] = {0x00};	/*  +9 is for the str "dev_type =", we have to clear it at wrqu->data.pointer */
+
+	/*	Commented by Albert 20121209 */
+	/*	The input data is the MAC address which the application wants to know its device type. */
+	/*	Such user interface could know the device type. */
+	/*	Format: iwpriv wlanx p2p_get2 dev_type = 00:E0:4C:00:00:05 */
+
+	if (copy_from_user(peerMACStr, wrqu->data.pointer + 9, 17))
+		return -EFAULT;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(peerMACStr[kk], peerMACStr[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			u8 *wpsie;
+			uint wpsie_len = 0;
+
+		/*	The mac address is matched. */
+
+			wpsie = rtw_get_wps_ie(&pnetwork->network.IEs[12],
+					       pnetwork->network.IELength - 12,
+					       NULL, &wpsie_len);
+			if (wpsie) {
+				rtw_get_wps_attr_content(wpsie, wpsie_len, WPS_ATTR_PRIMARY_DEV_TYPE, dev_type, &dev_type_len);
+				if (dev_type_len) {
+					u16	type = 0;
+					__be16 be_tmp;
+
+					memcpy(&be_tmp, dev_type, 2);
+					type = be16_to_cpu(be_tmp);
+					sprintf(dev_type_str, "\n\nN =%.2d", type);
+					blnMatch = 1;
+				}
+			}
+			break;
+	     }
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (!blnMatch)
+		sprintf(dev_type_str, "\n\nN = 00");
+
+	if (copy_to_user(wrqu->data.pointer, dev_type_str, 9 + 17)) {
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int rtw_p2p_get_device_name(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	u8 peerMACStr[17] = {0x00};
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u8 blnMatch = 0;
+	u8 dev_name[WPS_MAX_DEVICE_NAME_LEN] = {0x00};
+	uint dev_len = 0;
+	u8 dev_name_str[WPS_MAX_DEVICE_NAME_LEN + 5] = {0x00};	/*  +5 is for the str "devN =", we have to clear it at wrqu->data.pointer */
+
+	/*	Commented by Albert 20121225 */
+	/*	The input data is the MAC address which the application wants to know its device name. */
+	/*	Such user interface could show peer device's device name instead of ssid. */
+	/*	Format: iwpriv wlanx p2p_get2 devN = 00:E0:4C:00:00:05 */
+
+	if (copy_from_user(peerMACStr, wrqu->data.pointer + 5, 17))
+		return -EFAULT;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(peerMACStr[kk], peerMACStr[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			u8 *wpsie;
+			uint wpsie_len = 0;
+
+			/*	The mac address is matched. */
+			wpsie = rtw_get_wps_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &wpsie_len);
+			if (wpsie) {
+				rtw_get_wps_attr_content(wpsie, wpsie_len, WPS_ATTR_DEVICE_NAME, dev_name, &dev_len);
+				if (dev_len) {
+					sprintf(dev_name_str, "\n\nN =%s", dev_name);
+					blnMatch = 1;
+				}
+			}
+			break;
+		}
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (!blnMatch)
+		sprintf(dev_name_str, "\n\nN = 0000");
+
+	if (copy_to_user(wrqu->data.pointer, dev_name_str, 5 + ((dev_len > 17) ? dev_len : 17)))
+		return -EFAULT;
+	return 0;
+}
+
+static int rtw_p2p_get_invitation_procedure(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	u8 peerMACStr[17] = {0x00};
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u8 blnMatch = 0;
+	u8 *p2pie;
+	uint p2pielen = 0, attr_contentlen = 0;
+	u8 attr_content[2] = {0x00};
+
+	u8 inv_proc_str[17 + 8] = {0x00};
+	/*  +8 is for the str "InvProc =", we have to clear it at wrqu->data.pointer */
+
+	/*	Commented by Ouden 20121226 */
+	/*	The application wants to know P2P initiation procedure is supported or not. */
+	/*	Format: iwpriv wlanx p2p_get2 InvProc = 00:E0:4C:00:00:05 */
+
+	if (copy_from_user(peerMACStr, wrqu->data.pointer + 8, 17))
+		return -EFAULT;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(peerMACStr[kk], peerMACStr[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			/*	Commented by Albert 20121226 */
+			/*	Match the device address located in the P2P IE */
+			/*	This is for the case that the P2P device address is not the same as the P2P interface address. */
+
+			p2pie = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &p2pielen);
+			if (p2pie) {
+				while (p2pie) {
+					if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_CAPABILITY, attr_content, &attr_contentlen)) {
+						/*	Handle the P2P capability attribute */
+						blnMatch = 1;
+						break;
+					}
+
+					/* Get the next P2P IE */
+					p2pie = rtw_get_p2p_ie(p2pie + p2pielen, pnetwork->network.IELength - 12 - (p2pie - &pnetwork->network.IEs[12] + p2pielen), NULL, &p2pielen);
+				}
+			}
+		}
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (!blnMatch) {
+		sprintf(inv_proc_str, "\nIP =-1");
+	} else {
+		if (attr_content[0] & 0x20)
+			sprintf(inv_proc_str, "\nIP = 1");
+		else
+			sprintf(inv_proc_str, "\nIP = 0");
+	}
+	if (copy_to_user(wrqu->data.pointer, inv_proc_str, 8 + 17))
+		return -EFAULT;
+	return 0;
+}
+
+static int rtw_p2p_connect(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	u32 peer_channel = 0;
+
+	/*	Commented by Albert 20110304 */
+	/*	The input data contains two informations. */
+	/*	1. First information is the MAC address which wants to formate with */
+	/*	2. Second information is the WPS PINCode or "pbc" string for push button method */
+	/*	Format: 00:E0:4C:00:00:05 */
+	/*	Format: 00:E0:4C:00:00:05 */
+
+	if (pwdinfo->p2p_state == P2P_STATE_NONE)
+		return ret;
+
+	if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO)
+		return -1;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(extra[kk], extra[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!memcmp(pnetwork->network.MacAddress, peerMAC, ETH_ALEN)) {
+			peer_channel = pnetwork->network.Configuration.DSConfig;
+			break;
+		}
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (peer_channel) {
+		memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
+		memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+
+		pwdinfo->nego_req_info.peer_channel_num[0] = peer_channel;
+		memcpy(pwdinfo->nego_req_info.peerDevAddr, pnetwork->network.MacAddress, ETH_ALEN);
+		pwdinfo->nego_req_info.benable = true;
+
+		_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+		if (rtw_p2p_state(pwdinfo) != P2P_STATE_GONEGO_OK) {
+			/*	Restore to the listen state if the current p2p state is not nego OK */
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+		}
+
+		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+		_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+		_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_GO_NEGO_TIMEOUT);
+	} else {
+		ret = -1;
+	}
+	return ret;
+}
+
+static void rtw_p2p_invite_req(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	int jj, kk;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	uint peer_channel = 0;
+	u8 attr_content[50] = {0x00};
+	u8 *p2pie;
+	uint p2pielen = 0, attr_contentlen = 0;
+	struct tx_invite_req_info *pinvite_req_info = &pwdinfo->invitereq_info;
+
+	/*	The input data contains two informations. */
+	/*	1. First information is the P2P device address which you want to send to. */
+	/*	2. Second information is the group id which combines with GO's mac address, space and GO's ssid. */
+	/*	Command line sample: iwpriv wlan0 p2p_set invite ="00:11:22:33:44:55 00:E0:4C:00:00:05 DIRECT-xy" */
+	/*	Format: 00:11:22:33:44:55 00:E0:4C:00:00:05 DIRECT-xy */
+
+	if (wrqu->data.length <=  37)
+		return;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+		return;
+	} else {
+		/*	Reset the content of struct tx_invite_req_info */
+		pinvite_req_info->benable = false;
+		memset(pinvite_req_info->go_bssid, 0x00, ETH_ALEN);
+		memset(pinvite_req_info->go_ssid, 0x00, WLAN_SSID_MAXLEN);
+		pinvite_req_info->ssidlen = 0x00;
+		pinvite_req_info->operating_ch = pwdinfo->operating_channel;
+		memset(pinvite_req_info->peer_macaddr, 0x00, ETH_ALEN);
+		pinvite_req_info->token = 3;
+	}
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		pinvite_req_info->peer_macaddr[jj] = key_2char2num(extra[kk], extra[kk + 1]);
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+
+		/*	Commented by Albert 2011/05/18 */
+		/*	Match the device address located in the P2P IE */
+		/*	This is for the case that the P2P device address is not the same as the P2P interface address. */
+
+		p2pie = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &p2pielen);
+		if (p2pie) {
+			/*	The P2P Device ID attribute is included in the Beacon frame. */
+			/*	The P2P Device Info attribute is included in the probe response frame. */
+
+			if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_ID, attr_content, &attr_contentlen)) {
+				/*	Handle the P2P Device ID attribute of Beacon first */
+				if (!memcmp(attr_content, pinvite_req_info->peer_macaddr, ETH_ALEN)) {
+					peer_channel = pnetwork->network.Configuration.DSConfig;
+					break;
+				}
+			} else if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_INFO, attr_content, &attr_contentlen)) {
+				/*	Handle the P2P Device Info attribute of probe response */
+				if (!memcmp(attr_content, pinvite_req_info->peer_macaddr, ETH_ALEN)) {
+					peer_channel = pnetwork->network.Configuration.DSConfig;
+					break;
+				}
+			}
+		}
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (peer_channel) {
+		/*	Store the GO's bssid */
+		for (jj = 0, kk = 18; jj < ETH_ALEN; jj++, kk += 3)
+			pinvite_req_info->go_bssid[jj] = key_2char2num(extra[kk], extra[kk + 1]);
+
+		/*	Store the GO's ssid */
+		pinvite_req_info->ssidlen = wrqu->data.length - 36;
+		memcpy(pinvite_req_info->go_ssid, &extra[36], (u32)pinvite_req_info->ssidlen);
+		pinvite_req_info->benable = true;
+		pinvite_req_info->peer_ch = peer_channel;
+
+		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INVITE_REQ);
+
+		set_channel_bwmode(padapter, peer_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+
+		_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+		_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_INVITE_TIMEOUT);
+	}
+}
+
+static void rtw_p2p_set_persistent(struct net_device *dev,
+				   struct iw_request_info *info,
+				   union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*	The input data is 0 or 1 */
+	/*	0: disable persistent group functionality */
+	/*	1: enable persistent group founctionality */
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+		return;
+	} else {
+		if (extra[0] == '0')	/*	Disable the persistent group function. */
+			pwdinfo->persistent_supported = false;
+		else if (extra[0] == '1')	/*	Enable the persistent group function. */
+			pwdinfo->persistent_supported = true;
+		else
+			pwdinfo->persistent_supported = false;
+	}
+	pr_info("[%s] persistent_supported = %d\n", __func__, pwdinfo->persistent_supported);
+}
+
+static void rtw_p2p_prov_disc(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 peerMAC[ETH_ALEN] = {0x00};
+	int jj, kk;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct list_head *plist, *phead;
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
+	struct	wlan_network	*pnetwork = NULL;
+	uint peer_channel = 0;
+	u8 attr_content[100] = {0x00};
+	u8 *p2pie;
+	uint p2pielen = 0, attr_contentlen = 0;
+
+	/*	The input data contains two informations. */
+	/*	1. First information is the MAC address which wants to issue the provisioning discovery request frame. */
+	/*	2. Second information is the WPS configuration method which wants to discovery */
+	/*	Format: 00:E0:4C:00:00:05_display */
+	/*	Format: 00:E0:4C:00:00:05_keypad */
+	/*	Format: 00:E0:4C:00:00:05_pbc */
+	/*	Format: 00:E0:4C:00:00:05_label */
+
+	if (pwdinfo->p2p_state == P2P_STATE_NONE) {
+		return;
+	} else {
+		/*	Reset the content of struct tx_provdisc_req_info excluded the wps_config_method_request. */
+		memset(pwdinfo->tx_prov_disc_info.peerDevAddr, 0x00, ETH_ALEN);
+		memset(pwdinfo->tx_prov_disc_info.peerIFAddr, 0x00, ETH_ALEN);
+		memset(&pwdinfo->tx_prov_disc_info.ssid, 0x00, sizeof(struct ndis_802_11_ssid));
+		pwdinfo->tx_prov_disc_info.peer_channel_num[0] = 0;
+		pwdinfo->tx_prov_disc_info.peer_channel_num[1] = 0;
+		pwdinfo->tx_prov_disc_info.benable = false;
+	}
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		peerMAC[jj] = key_2char2num(extra[kk], extra[kk + 1]);
+
+	if (!memcmp(&extra[18], "display", 7))
+		pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_DISPLYA;
+	else if (!memcmp(&extra[18], "keypad", 7))
+		pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_KEYPAD;
+	else if (!memcmp(&extra[18], "pbc", 3))
+		pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_PUSH_BUTTON;
+	else if (!memcmp(&extra[18], "label", 5))
+		pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_LABEL;
+	else
+		return;
+
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+	phead = get_list_head(queue);
+	plist = phead->next;
+
+	while (phead != plist) {
+		if (peer_channel != 0)
+			break;
+
+		pnetwork = container_of(plist, struct wlan_network, list);
+
+		/*	Commented by Albert 2011/05/18 */
+		/*	Match the device address located in the P2P IE */
+		/*	This is for the case that the P2P device address is not the same as the P2P interface address. */
+
+		p2pie = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &p2pielen);
+		if (p2pie) {
+			while (p2pie) {
+				/*	The P2P Device ID attribute is included in the Beacon frame. */
+				/*	The P2P Device Info attribute is included in the probe response frame. */
+
+				if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_ID, attr_content, &attr_contentlen)) {
+					/*	Handle the P2P Device ID attribute of Beacon first */
+					if (!memcmp(attr_content, peerMAC, ETH_ALEN)) {
+						peer_channel = pnetwork->network.Configuration.DSConfig;
+						break;
+					}
+				} else if (rtw_get_p2p_attr_content(p2pie, p2pielen, P2P_ATTR_DEVICE_INFO, attr_content, &attr_contentlen)) {
+					/*	Handle the P2P Device Info attribute of probe response */
+					if (!memcmp(attr_content, peerMAC, ETH_ALEN)) {
+						peer_channel = pnetwork->network.Configuration.DSConfig;
+						break;
+					}
+				}
+
+				/* Get the next P2P IE */
+				p2pie = rtw_get_p2p_ie(p2pie + p2pielen, pnetwork->network.IELength - 12 - (p2pie - &pnetwork->network.IEs[12] + p2pielen), NULL, &p2pielen);
+			}
+		}
+
+		plist = plist->next;
+	}
+
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+	if (peer_channel) {
+		memcpy(pwdinfo->tx_prov_disc_info.peerIFAddr, pnetwork->network.MacAddress, ETH_ALEN);
+		memcpy(pwdinfo->tx_prov_disc_info.peerDevAddr, peerMAC, ETH_ALEN);
+		pwdinfo->tx_prov_disc_info.peer_channel_num[0] = (u16)peer_channel;
+		pwdinfo->tx_prov_disc_info.benable = true;
+		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ);
+
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+			memcpy(&pwdinfo->tx_prov_disc_info.ssid, &pnetwork->network.Ssid, sizeof(struct ndis_802_11_ssid));
+		} else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			memcpy(pwdinfo->tx_prov_disc_info.ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
+			pwdinfo->tx_prov_disc_info.ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
+		}
+
+		set_channel_bwmode(padapter, peer_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+
+		_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+		_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+	}
+}
+
+/*	This function is used to inform the driver the user had specified the pin code value or pbc */
+/*	to application. */
+
+static void rtw_p2p_got_wpsinfo(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*	Added by Albert 20110328 */
+	/*	if the input data is P2P_NO_WPSINFO -> reset the wpsinfo */
+	/*	if the input data is P2P_GOT_WPSINFO_PEER_DISPLAY_PIN -> the utility just input the PIN code got from the peer P2P device. */
+	/*	if the input data is P2P_GOT_WPSINFO_SELF_DISPLAY_PIN -> the utility just got the PIN code from itself. */
+	/*	if the input data is P2P_GOT_WPSINFO_PBC -> the utility just determine to use the PBC */
+
+	if (*extra == '0')
+		pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+	else if (*extra == '1')
+		pwdinfo->ui_got_wps_info = P2P_GOT_WPSINFO_PEER_DISPLAY_PIN;
+	else if (*extra == '2')
+		pwdinfo->ui_got_wps_info = P2P_GOT_WPSINFO_SELF_DISPLAY_PIN;
+	else if (*extra == '3')
+		pwdinfo->ui_got_wps_info = P2P_GOT_WPSINFO_PBC;
+	else
+		pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+}
+
+static int rtw_p2p_set(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	if (!memcmp(extra, "enable =", 7)) {
+		rtw_wext_p2p_enable(dev, info, wrqu, &extra[7]);
+	} else if (!memcmp(extra, "setDN =", 6)) {
+		wrqu->data.length -= 6;
+		rtw_p2p_setDN(dev, info, wrqu, &extra[6]);
+	} else if (!memcmp(extra, "profilefound =", 13)) {
+		wrqu->data.length -= 13;
+		rtw_p2p_profilefound(dev, info, wrqu, &extra[13]);
+	} else if (!memcmp(extra, "prov_disc =", 10)) {
+		wrqu->data.length -= 10;
+		rtw_p2p_prov_disc(dev, info, wrqu, &extra[10]);
+	} else if (!memcmp(extra, "nego =", 5)) {
+		wrqu->data.length -= 5;
+		rtw_p2p_connect(dev, info, wrqu, &extra[5]);
+	} else if (!memcmp(extra, "intent =", 7)) {
+		/*	Commented by Albert 2011/03/23 */
+		/*	The wrqu->data.length will include the null character */
+		/*	So, we will decrease 7 + 1 */
+		wrqu->data.length -= 8;
+		rtw_p2p_set_intent(dev, info, wrqu, &extra[7]);
+	} else if (!memcmp(extra, "ssid =", 5)) {
+		wrqu->data.length -= 5;
+		rtw_p2p_set_go_nego_ssid(dev, info, wrqu, &extra[5]);
+	} else if (!memcmp(extra, "got_wpsinfo =", 12)) {
+		wrqu->data.length -= 12;
+		rtw_p2p_got_wpsinfo(dev, info, wrqu, &extra[12]);
+	} else if (!memcmp(extra, "listen_ch =", 10)) {
+		/*	Commented by Albert 2011/05/24 */
+		/*	The wrqu->data.length will include the null character */
+		/*	So, we will decrease (10 + 1) */
+		wrqu->data.length -= 11;
+		rtw_p2p_set_listen_ch(dev, info, wrqu, &extra[10]);
+	} else if (!memcmp(extra, "op_ch =", 6)) {
+		/*	Commented by Albert 2011/05/24 */
+		/*	The wrqu->data.length will include the null character */
+		/*	So, we will decrease (6 + 1) */
+		wrqu->data.length -= 7;
+		rtw_p2p_set_op_ch(dev, info, wrqu, &extra[6]);
+	} else if (!memcmp(extra, "invite =", 7)) {
+		wrqu->data.length -= 8;
+		rtw_p2p_invite_req(dev, info, wrqu, &extra[7]);
+	} else if (!memcmp(extra, "persistent =", 11)) {
+		wrqu->data.length -= 11;
+		rtw_p2p_set_persistent(dev, info, wrqu, &extra[11]);
+	}
+
+	return ret;
+}
+
+static int rtw_p2p_get2(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	if (!memcmp(extra, "wpsCM =", 6)) {
+		wrqu->data.length -= 6;
+		ret = rtw_p2p_get_wps_configmethod(dev, info, wrqu,  &extra[6]);
+	} else if (!memcmp(extra, "devN =", 5)) {
+		wrqu->data.length -= 5;
+		ret = rtw_p2p_get_device_name(dev, info, wrqu, &extra[5]);
+	} else if (!memcmp(extra, "dev_type =", 9)) {
+		wrqu->data.length -= 9;
+		ret = rtw_p2p_get_device_type(dev, info, wrqu, &extra[9]);
+	} else if (!memcmp(extra, "go_devadd =", 10)) {
+		wrqu->data.length -= 10;
+		ret = rtw_p2p_get_go_device_address(dev, info, wrqu, &extra[10]);
+	} else if (!memcmp(extra, "InvProc =", 8)) {
+		wrqu->data.length -= 8;
+		ret = rtw_p2p_get_invitation_procedure(dev, info, wrqu, &extra[8]);
+	}
+
+	return ret;
+}
+
+static int rtw_rereg_nd_name(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct rereg_nd_name_data *rereg_priv = &padapter->rereg_nd_name_priv;
+	char new_ifname[IFNAMSIZ];
+
+	if (rereg_priv->old_ifname[0] == 0) {
+		char *reg_ifname;
+		reg_ifname = padapter->registrypriv.if2name;
+
+		strncpy(rereg_priv->old_ifname, reg_ifname, IFNAMSIZ);
+		rereg_priv->old_ifname[IFNAMSIZ - 1] = 0;
+	}
+
+	if (wrqu->data.length > IFNAMSIZ)
+		return -EFAULT;
+
+	if (copy_from_user(new_ifname, wrqu->data.pointer, IFNAMSIZ))
+		return -EFAULT;
+
+	if (0 == strcmp(rereg_priv->old_ifname, new_ifname))
+		return ret;
+
+	ret = rtw_change_ifname(padapter, new_ifname);
+	if (0 != ret)
+		goto exit;
+
+	if (!memcmp(rereg_priv->old_ifname, "disable%d", 9)) {
+		padapter->ledpriv.bRegUseLed = rereg_priv->old_bRegUseLed;
+		rtl8188eu_InitSwLeds(padapter);
+		rtw_ips_mode_req(&padapter->pwrctrlpriv, rereg_priv->old_ips_mode);
+	}
+
+	strncpy(rereg_priv->old_ifname, new_ifname, IFNAMSIZ);
+	rereg_priv->old_ifname[IFNAMSIZ - 1] = 0;
+
+	if (!memcmp(new_ifname, "disable%d", 9)) {
+		/*  free network queue for Android's timming issue */
+		rtw_free_network_queue(padapter, true);
+
+		/*  close led */
+		rtw_led_control(padapter, LED_CTL_POWER_OFF);
+		rereg_priv->old_bRegUseLed = padapter->ledpriv.bRegUseLed;
+		padapter->ledpriv.bRegUseLed = false;
+		rtl8188eu_DeInitSwLeds(padapter);
+
+		/*  the interface is being "disabled", we can do deeper IPS */
+		rereg_priv->old_ips_mode = rtw_get_ips_mode_req(&padapter->pwrctrlpriv);
+		rtw_ips_mode_req(&padapter->pwrctrlpriv, IPS_NORMAL);
+	}
+exit:
+	return ret;
+}
+
+static void mac_reg_dump(struct adapter *padapter)
+{
+	int i, j = 1;
+	u32 reg;
+	int res;
+
+	pr_info("\n ======= MAC REG =======\n");
+	for (i = 0x0; i < 0x300; i += 4) {
+		if (j % 4 == 1)
+			pr_info("0x%02x", i);
+
+		res = rtw_read32(padapter, i, &reg);
+		if (!res)
+			pr_info(" 0x%08x ", reg);
+
+		if ((j++) % 4 == 0)
+			pr_info("\n");
+	}
+	for (i = 0x400; i < 0x800; i += 4) {
+		if (j % 4 == 1)
+			pr_info("0x%02x", i);
+
+		res = rtw_read32(padapter, i, &reg);
+		if (!res)
+			pr_info(" 0x%08x ", reg);
+
+		if ((j++) % 4 == 0)
+			pr_info("\n");
+	}
+}
+
+static void bb_reg_dump(struct adapter *padapter)
+{
+	int i, j = 1, res;
+	u32 reg;
+
+	pr_info("\n ======= BB REG =======\n");
+	for (i = 0x800; i < 0x1000; i += 4) {
+		if (j % 4 == 1)
+			pr_info("0x%02x", i);
+
+		res = rtw_read32(padapter, i, &reg);
+		if (!res)
+			pr_info(" 0x%08x ", reg);
+
+		if ((j++) % 4 == 0)
+			pr_info("\n");
+	}
+}
+
+static void rf_reg_dump(struct adapter *padapter)
+{
+	int i, j = 1;
+	u32 value;
+
+	pr_info("\n ======= RF REG =======\n");
+	pr_info("\nRF_Path(%x)\n", RF_PATH_A);
+	for (i = 0; i < 0x100; i++) {
+		value = rtl8188e_PHY_QueryRFReg(padapter, i, 0xffffffff);
+		if (j % 4 == 1)
+			pr_info("0x%02x ", i);
+		pr_info(" 0x%08x ", value);
+		if ((j++) % 4 == 0)
+			pr_info("\n");
+	}
+}
+
+static void rtw_set_dynamic_functions(struct adapter *adapter, u8 dm_func)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+	int res;
+
+	switch (dm_func) {
+	case 0:
+		/* disable all dynamic func */
+		odmpriv->SupportAbility = DYNAMIC_FUNC_DISABLE;
+		break;
+	case 1:
+		/* disable DIG */
+		odmpriv->SupportAbility &= (~DYNAMIC_BB_DIG);
+		break;
+	case 6:
+		/* turn on all dynamic func */
+		if (!(odmpriv->SupportAbility & DYNAMIC_BB_DIG)) {
+			struct rtw_dig *digtable = &odmpriv->DM_DigTable;
+
+			res = rtw_read8(adapter, 0xc50, &digtable->CurIGValue);
+			(void)res;
+			/* FIXME: return an error to caller */
+		}
+		odmpriv->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
+		break;
+	default:
+		break;
+	}
+}
+
+static void rtw_set_dm_func_flag(struct adapter *adapter, u32 odm_flag)
+{
+	struct hal_data_8188e *haldata = &adapter->haldata;
+	struct odm_dm_struct *odmpriv = &haldata->odmpriv;
+
+	odmpriv->SupportAbility = odm_flag;
+}
+
+static int rtw_dbg_port(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	u8 major_cmd, minor_cmd;
+	u16 arg;
+	s32 extra_arg;
+	u32 *pdata, val32;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	pdata = (u32 *)&wrqu->data;
+
+	val32 = *pdata;
+	arg = (u16)(val32 & 0x0000ffff);
+	major_cmd = (u8)(val32 >> 24);
+	minor_cmd = (u8)((val32 >> 16) & 0x00ff);
+
+	extra_arg = *(pdata + 1);
+
+	switch (major_cmd) {
+	case 0x70:/* read_reg */
+		switch (minor_cmd) {
+		case 1:
+			break;
+		case 2:
+			break;
+		case 4:
+			break;
+		}
+		break;
+	case 0x71:/* write_reg */
+		switch (minor_cmd) {
+		case 1:
+			rtw_write8(padapter, arg, extra_arg);
+			break;
+		case 2:
+			rtw_write16(padapter, arg, extra_arg);
+			break;
+		case 4:
+			rtw_write32(padapter, arg, extra_arg);
+			break;
+		}
+		break;
+	case 0x72:/* read_bb */
+		break;
+	case 0x73:/* write_bb */
+		rtl8188e_PHY_SetBBReg(padapter, arg, 0xffffffff, extra_arg);
+		break;
+	case 0x74:/* read_rf */
+		if (minor_cmd != RF_PATH_A) {
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	case 0x75:/* write_rf */
+		if (minor_cmd != RF_PATH_A) {
+			ret = -EINVAL;
+			break;
+		}
+		rtl8188e_PHY_SetRFReg(padapter, arg, 0xffffffff, extra_arg);
+		break;
+
+	case 0x76:
+		switch (minor_cmd) {
+		case 0x00: /* normal mode, */
+			padapter->recvpriv.is_signal_dbg = 0;
+			break;
+		case 0x01: /* dbg mode */
+			padapter->recvpriv.is_signal_dbg = 1;
+			extra_arg = extra_arg > 100 ? 100 : extra_arg;
+			extra_arg = extra_arg < 0 ? 0 : extra_arg;
+			padapter->recvpriv.signal_strength_dbg = extra_arg;
+			break;
+		}
+		break;
+	case 0x78: /* IOL test */
+		switch (minor_cmd) {
+		case 0x04: /* LLT table initialization test */
+		{
+			struct xmit_frame	*xmit_frame;
+
+			xmit_frame = rtw_IOL_accquire_xmit_frame(padapter);
+			if (!xmit_frame) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			if (rtl8188e_IOL_exec_cmds_sync(padapter, xmit_frame, 500, 0) != _SUCCESS)
+				ret = -EPERM;
+		}
+			break;
+		case 0x05: /* blink LED test */
+		{
+			u16 reg = 0x4c;
+			u32 blink_num = 50;
+			u32 blink_delay_ms = 200;
+			int i;
+			struct xmit_frame	*xmit_frame;
+
+			xmit_frame = rtw_IOL_accquire_xmit_frame(padapter);
+			if (!xmit_frame) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			for (i = 0; i < blink_num; i++) {
+				rtw_IOL_append_WB_cmd(xmit_frame, reg, 0x00, 0xff);
+				rtw_IOL_append_DELAY_MS_cmd(xmit_frame, blink_delay_ms);
+				rtw_IOL_append_WB_cmd(xmit_frame, reg, 0x08, 0xff);
+				rtw_IOL_append_DELAY_MS_cmd(xmit_frame, blink_delay_ms);
+			}
+			if (rtl8188e_IOL_exec_cmds_sync(padapter, xmit_frame, (blink_delay_ms * blink_num * 2) + 200, 0) != _SUCCESS)
+				ret = -EPERM;
+		}
+			break;
+
+		case 0x06: /* continuous write byte test */
+		{
+			u16 reg = arg;
+			u16 start_value = 0;
+			u32 write_num = extra_arg;
+			int i, res;
+			struct xmit_frame	*xmit_frame;
+			u8 val8;
+
+			xmit_frame = rtw_IOL_accquire_xmit_frame(padapter);
+			if (!xmit_frame) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			for (i = 0; i < write_num; i++)
+				rtw_IOL_append_WB_cmd(xmit_frame, reg, i + start_value, 0xFF);
+			if (rtl8188e_IOL_exec_cmds_sync(padapter, xmit_frame, 5000, 0) != _SUCCESS)
+				ret = -EPERM;
+
+			/* FIXME: is this read necessary? */
+			res = rtw_read8(padapter, reg, &val8);
+			(void)res;
+		}
+			break;
+
+		case 0x07: /* continuous write word test */
+		{
+			u16 reg = arg;
+			u16 start_value = 200;
+			u32 write_num = extra_arg;
+			u16 val16;
+			int i, res;
+			struct xmit_frame	*xmit_frame;
+
+			xmit_frame = rtw_IOL_accquire_xmit_frame(padapter);
+			if (!xmit_frame) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			for (i = 0; i < write_num; i++)
+				rtw_IOL_append_WW_cmd(xmit_frame, reg, i + start_value, 0xFFFF);
+			if (rtl8188e_IOL_exec_cmds_sync(padapter, xmit_frame, 5000, 0) != _SUCCESS)
+				ret = -EPERM;
+
+			/* FIXME: is this read necessary? */
+			res = rtw_read16(padapter, reg, &val16);
+			(void)res;
+		}
+			break;
+		case 0x08: /* continuous write dword test */
+		{
+			u16 reg = arg;
+			u32 start_value = 0x110000c7;
+			u32 write_num = extra_arg;
+
+			int i;
+			struct xmit_frame	*xmit_frame;
+
+			xmit_frame = rtw_IOL_accquire_xmit_frame(padapter);
+			if (!xmit_frame) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			for (i = 0; i < write_num; i++)
+				rtw_IOL_append_WD_cmd(xmit_frame, reg, i + start_value, 0xFFFFFFFF);
+			if (rtl8188e_IOL_exec_cmds_sync(padapter, xmit_frame, 5000, 0) != _SUCCESS)
+				ret = -EPERM;
+
+			/* FIXME: is this read necessary? */
+			ret = rtw_read32(padapter, reg, &write_num);
+		}
+			break;
+		}
+		break;
+	case 0x79:
+		{
+			/*
+			* dbg 0x79000000 [value], set RESP_TXAGC to + value, value:0~15
+			* dbg 0x79010000 [value], set RESP_TXAGC to - value, value:0~15
+			*/
+			u8 value =  extra_arg & 0x0f;
+			u8 sign = minor_cmd;
+			u16 write_value = 0;
+
+			if (sign)
+				value = value | 0x10;
+
+			write_value = value | (value << 5);
+			rtw_write16(padapter, 0x6d9, write_value);
+		}
+		break;
+	case 0x7a:
+		receive_disconnect(padapter, pmlmeinfo->network.MacAddress
+			, WLAN_REASON_EXPIRATION_CHK);
+		break;
+	case 0x7F:
+		switch (minor_cmd) {
+		case 0x0:
+			break;
+		case 0x01:
+			break;
+		case 0x02:
+			break;
+		case 0x03:
+			break;
+		case 0x04:
+			break;
+		case 0x05:
+			rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+			break;
+		case 0x06:
+			{
+				u32 ODMFlag = (u32)(0x0f & arg);
+				rtw_set_dm_func_flag(padapter, ODMFlag);
+			}
+			break;
+		case 0x07:
+			break;
+		case 0x08:
+			break;
+		case 0x09:
+			break;
+		case 0x15:
+			break;
+		case 0x10:/*  driver version display */
+			break;
+		case 0x11:
+			padapter->bRxRSSIDisplay = extra_arg;
+			break;
+		case 0x12: /* set rx_stbc */
+		{
+			struct registry_priv	*pregpriv = &padapter->registrypriv;
+			/*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, 0x3: enable both 2.4g and 5g */
+			/* default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ */
+			if (extra_arg == 0 ||
+			    extra_arg == 1 ||
+			    extra_arg == 2 ||
+			    extra_arg == 3)
+				pregpriv->rx_stbc = extra_arg;
+		}
+			break;
+		case 0x13: /* set ampdu_enable */
+		{
+			struct registry_priv	*pregpriv = &padapter->registrypriv;
+			/*  0: disable, 0x1:enable (but wifi_spec should be 0), 0x2: force enable (don't care wifi_spec) */
+			if (extra_arg >= 0 && extra_arg < 3)
+				pregpriv->ampdu_enable = extra_arg;
+		}
+			break;
+		case 0x14: /* get wifi_spec */
+			break;
+		case 0x23:
+			padapter->bNotifyChannelChange = extra_arg;
+			break;
+		case 0x24:
+			padapter->bShowGetP2PState = extra_arg;
+			break;
+		case 0xdd:/* registers dump, 0 for mac reg, 1 for bb reg, 2 for rf reg */
+			if (extra_arg == 0)
+				mac_reg_dump(padapter);
+			else if (extra_arg == 1)
+				bb_reg_dump(padapter);
+			else if (extra_arg == 2)
+				rf_reg_dump(padapter);
+			break;
+		case 0xee:/* turn on/off dynamic funcs */
+			if (extra_arg != 0xf) {
+				/* extra_arg = 0  - disable all dynamic func
+				 * extra_arg = 1  - disable DIG
+				 * extra_arg = 6  - turn on all dynamic func
+				 */
+				rtw_set_dynamic_functions(padapter, extra_arg);
+			}
+			break;
+		case 0xfd:
+			rtw_write8(padapter, 0xc50, arg);
+			rtw_write8(padapter, 0xc58, arg);
+			break;
+		case 0xfe:
+			break;
+		case 0xff:
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int rtw_wx_set_priv(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *awrq,
+				char *extra)
+{
+	int ret = 0;
+	int len = 0;
+	char *ext;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_point *dwrq = (struct iw_point *)awrq;
+
+	if (dwrq->length == 0)
+		return -EFAULT;
+
+	len = dwrq->length;
+	ext = vmalloc(len);
+	if (!ext)
+		return -ENOMEM;
+
+	if (copy_from_user(ext, dwrq->pointer, len)) {
+		vfree(ext);
+		return -EFAULT;
+	}
+
+	/* added for wps2.0 @20110524 */
+	if (dwrq->flags == 0x8766 && len > 8) {
+		u32 cp_sz;
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		u8 *probereq_wpsie = ext;
+		int probereq_wpsie_len = len;
+		u8 wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+		if ((_VENDOR_SPECIFIC_IE_ == probereq_wpsie[0]) &&
+		    (!memcmp(&probereq_wpsie[2], wps_oui, 4))) {
+			cp_sz = probereq_wpsie_len > MAX_WPS_IE_LEN ? MAX_WPS_IE_LEN : probereq_wpsie_len;
+
+			pmlmepriv->wps_probe_req_ie_len = 0;
+			kfree(pmlmepriv->wps_probe_req_ie);
+			pmlmepriv->wps_probe_req_ie = NULL;
+
+			pmlmepriv->wps_probe_req_ie = kmemdup(probereq_wpsie, cp_sz, GFP_KERNEL);
+			if (!pmlmepriv->wps_probe_req_ie) {
+				ret =  -EINVAL;
+				goto FREE_EXT;
+			}
+			pmlmepriv->wps_probe_req_ie_len = cp_sz;
+		}
+		goto FREE_EXT;
+	}
+
+	if (len >= WEXT_CSCAN_HEADER_SIZE &&
+	    !memcmp(ext, WEXT_CSCAN_HEADER, WEXT_CSCAN_HEADER_SIZE)) {
+		ret = rtw_wx_set_scan(dev, info, awrq, ext);
+		goto FREE_EXT;
+	}
+
+FREE_EXT:
+
+	vfree(ext);
+
+	return ret;
+}
+
+static int rtw_pm_set(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	unsigned	mode = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (!memcmp(extra, "lps =", 4)) {
+		sscanf(extra + 4, "%u", &mode);
+		ret = rtw_pm_set_lps(padapter, mode);
+	} else if (!memcmp(extra, "ips =", 4)) {
+		sscanf(extra + 4, "%u", &mode);
+		ret = rtw_pm_set_ips(padapter, mode);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static iw_handler rtw_handlers[] = {
+	IW_HANDLER(SIOCGIWNAME, rtw_wx_get_name),
+	IW_HANDLER(SIOCGIWFREQ, rtw_wx_get_freq),
+	IW_HANDLER(SIOCSIWMODE, rtw_wx_set_mode),
+	IW_HANDLER(SIOCGIWMODE, rtw_wx_get_mode),
+	IW_HANDLER(SIOCGIWSENS, rtw_wx_get_sens),
+	IW_HANDLER(SIOCGIWRANGE, rtw_wx_get_range),
+	IW_HANDLER(SIOCSIWPRIV, rtw_wx_set_priv),
+	IW_HANDLER(SIOCSIWAP, rtw_wx_set_wap),
+	IW_HANDLER(SIOCGIWAP, rtw_wx_get_wap),
+	IW_HANDLER(SIOCSIWMLME, rtw_wx_set_mlme),
+	IW_HANDLER(SIOCSIWSCAN, rtw_wx_set_scan),
+	IW_HANDLER(SIOCGIWSCAN, rtw_wx_get_scan),
+	IW_HANDLER(SIOCSIWESSID, rtw_wx_set_essid),
+	IW_HANDLER(SIOCGIWESSID, rtw_wx_get_essid),
+	IW_HANDLER(SIOCGIWNICKN, rtw_wx_get_nick),
+	IW_HANDLER(SIOCSIWRATE, rtw_wx_set_rate),
+	IW_HANDLER(SIOCGIWRATE, rtw_wx_get_rate),
+	IW_HANDLER(SIOCSIWRTS, rtw_wx_set_rts),
+	IW_HANDLER(SIOCGIWRTS, rtw_wx_get_rts),
+	IW_HANDLER(SIOCSIWFRAG, rtw_wx_set_frag),
+	IW_HANDLER(SIOCGIWFRAG, rtw_wx_get_frag),
+	IW_HANDLER(SIOCGIWRETRY, rtw_wx_get_retry),
+	IW_HANDLER(SIOCSIWENCODE, rtw_wx_set_enc),
+	IW_HANDLER(SIOCGIWENCODE, rtw_wx_get_enc),
+	IW_HANDLER(SIOCGIWPOWER, rtw_wx_get_power),
+	IW_HANDLER(SIOCSIWGENIE, rtw_wx_set_gen_ie),
+	IW_HANDLER(SIOCSIWAUTH, rtw_wx_set_auth),
+	IW_HANDLER(SIOCSIWENCODEEXT, rtw_wx_set_enc_ext),
+	IW_HANDLER(SIOCSIWPMKSA, rtw_wx_set_pmkid),
+};
+
+static const struct iw_priv_args rtw_private_args[] = {
+	{
+		SIOCIWFIRSTPRIV + 0x0,
+		IW_PRIV_TYPE_CHAR | 0x7FF, 0, "write"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x1,
+		IW_PRIV_TYPE_CHAR | 0x7FF,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | IFNAMSIZ, "read"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x2, 0, 0, "driver_ext"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x4,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "apinfo"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x5,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, 0, "setpid"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x6,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wps_start"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xA,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "channel_plan"
+	},
+
+	{
+		SIOCIWFIRSTPRIV + 0xB,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, 0, "dbg"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xC,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 3, 0, "rfw"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xD,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | IFNAMSIZ, "rfr"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x10,
+		IW_PRIV_TYPE_CHAR | P2P_PRIVATE_IOCTL_SET_LEN, 0, "p2p_set"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x11,
+		IW_PRIV_TYPE_CHAR | P2P_PRIVATE_IOCTL_SET_LEN, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | P2P_PRIVATE_IOCTL_SET_LEN, "p2p_get"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x12,
+		IW_PRIV_TYPE_CHAR | P2P_PRIVATE_IOCTL_SET_LEN, IW_PRIV_TYPE_CHAR | IFNAMSIZ, "p2p_get2"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x16,
+		IW_PRIV_TYPE_CHAR | 64, 0, "pm_set"
+	},
+
+	{SIOCIWFIRSTPRIV + 0x18, IW_PRIV_TYPE_CHAR | IFNAMSIZ, 0, "rereg_nd_name"},
+};
+
+static iw_handler rtw_private_handler[] = {
+	NULL,				/* 0x00 */
+	NULL,				/* 0x01 */
+	NULL,				/* 0x02 */
+NULL,					/* 0x03 */
+/*  for MM DTV platform */
+	rtw_get_ap_info,		/* 0x04 */
+
+	rtw_set_pid,			/* 0x05 */
+	rtw_wps_start,			/* 0x06 */
+
+	NULL,				/* 0x07 */
+	NULL,				/* 0x08 */
+	NULL,				/* 0x09 */
+
+/*  Set Channel depend on the country code */
+	rtw_wx_set_channel_plan,	/* 0x0A */
+
+	rtw_dbg_port,			/* 0x0B */
+	rtw_wx_write_rf,		/* 0x0C */
+	rtw_wx_read_rf,			/* 0x0D */
+	NULL,				/* 0x0E */
+	NULL,				/* 0x0F */
+
+	rtw_p2p_set,			/* 0x10 */
+	NULL,				/* 0x11 */
+	rtw_p2p_get2,			/* 0x12 */
+
+	NULL,				/* 0x13 */
+	NULL,				/* 0x14 */
+	NULL,				/* 0x15 */
+
+	rtw_pm_set,			/* 0x16 */
+	NULL,				/* 0x17 */
+	rtw_rereg_nd_name,		/* 0x18 */
+};
+
+static struct iw_statistics *rtw_get_wireless_stats(struct net_device *dev)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_statistics *piwstats = &padapter->iwstats;
+	int tmp_noise = 0;
+	int tmp;
+
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		piwstats->qual.qual = 0;
+		piwstats->qual.level = 0;
+		piwstats->qual.noise = 0;
+	} else {
+		tmp_noise = padapter->recvpriv.noise;
+
+		piwstats->qual.level = padapter->signal_strength;
+		tmp = 219 + 3 * padapter->signal_strength;
+		tmp = min(100, tmp);
+		tmp = max(0, tmp);
+		piwstats->qual.qual = tmp;
+		piwstats->qual.noise = tmp_noise;
+	}
+	piwstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+	return &padapter->iwstats;
+}
+
+struct iw_handler_def rtw_handlers_def = {
+	.standard = rtw_handlers,
+	.num_standard = ARRAY_SIZE(rtw_handlers),
+	.private = rtw_private_handler,
+	.private_args = (struct iw_priv_args *)rtw_private_args,
+	.num_private = ARRAY_SIZE(rtw_private_handler),
+	.num_private_args = ARRAY_SIZE(rtw_private_args),
+	.get_wireless_stats = rtw_get_wireless_stats,
+};
diff --git a/drivers/staging/r8188eu/os_dep/os_intfs.c b/drivers/staging/r8188eu/os_dep/os_intfs.c
new file mode 100644
index 000000000..6a45315d0
--- /dev/null
+++ b/drivers/staging/r8188eu/os_dep/os_intfs.c
@@ -0,0 +1,821 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#define _OS_INTFS_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/hal_intf.h"
+#include "../include/rtw_ioctl.h"
+#include "../include/usb_osintf.h"
+#include "../include/rtw_br_ext.h"
+#include "../include/rtw_led.h"
+#include "../include/rtl8188e_dm.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek Wireless Lan Driver");
+MODULE_AUTHOR("Realtek Semiconductor Corp.");
+MODULE_FIRMWARE(FW_RTL8188EU);
+
+#define CONFIG_BR_EXT_BRNAME "br0"
+#define RTW_NOTCH_FILTER 0 /* 0:Disable, 1:Enable, */
+
+/* module param defaults */
+static int rtw_rfintfs = HWPI;
+static int rtw_lbkmode;/* RTL8712_AIR_TRX; */
+static int rtw_network_mode = Ndis802_11IBSS;/* Ndis802_11Infrastructure; infra, ad-hoc, auto */
+static int rtw_channel = 1;/* ad-hoc support requirement */
+static int rtw_wireless_mode = WIRELESS_11BG_24N;
+static int rtw_vrtl_carrier_sense = AUTO_VCS;
+static int rtw_vcs_type = RTS_CTS;/*  */
+static int rtw_rts_thresh = 2347;/*  */
+static int rtw_frag_thresh = 2346;/*  */
+static int rtw_preamble = PREAMBLE_LONG;/* long, short, auto */
+static int rtw_scan_mode = 1;/* active, passive */
+static int rtw_adhoc_tx_pwr = 1;
+static int rtw_soft_ap;
+static int rtw_power_mgnt = 1;
+static int rtw_ips_mode = IPS_NORMAL;
+
+static int rtw_smart_ps = 2;
+
+module_param(rtw_ips_mode, int, 0644);
+MODULE_PARM_DESC(rtw_ips_mode, "The default IPS mode");
+
+static int rtw_radio_enable = 1;
+static int rtw_long_retry_lmt = 7;
+static int rtw_short_retry_lmt = 7;
+static int rtw_busy_thresh = 40;
+static int rtw_ack_policy = NORMAL_ACK;
+
+static int rtw_software_encrypt;
+static int rtw_software_decrypt;
+
+static int rtw_acm_method;/*  0:By SW 1:By HW. */
+
+static int rtw_wmm_enable = 1;/*  default is set to enable the wmm. */
+static int rtw_uapsd_enable;
+static int rtw_uapsd_max_sp = NO_LIMIT;
+static int rtw_uapsd_acbk_en;
+static int rtw_uapsd_acbe_en;
+static int rtw_uapsd_acvi_en;
+static int rtw_uapsd_acvo_en;
+
+static int rtw_led_enable = 1;
+
+static int rtw_ht_enable = 1;
+static int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
+static int rtw_rx_stbc = 1;/*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ */
+static int rtw_ampdu_amsdu;/*  0: disabled, 1:enabled, 2:auto */
+
+static int rtw_lowrate_two_xmit = 1;/* Use 2 path Tx to transmit MCS0~7 and legacy mode */
+
+static int rtw_low_power;
+static int rtw_wifi_spec;
+static int rtw_channel_plan = RT_CHANNEL_DOMAIN_MAX;
+static int rtw_AcceptAddbaReq = true;/*  0:Reject AP's Add BA req, 1:Accept AP's Add BA req. */
+
+static int rtw_antdiv_cfg = 2; /*  0:OFF , 1:ON, 2:decide by Efuse config */
+static int rtw_antdiv_type; /* 0:decide by efuse  1: for 88EE, 1Tx and 1RxCG are diversity.(2 Ant with SPDT), 2:  for 88EE, 1Tx and 2Rx are diversity.(2 Ant, Tx and RxCG are both on aux port, RxCS is on main port), 3: for 88EE, 1Tx and 1RxCG are fixed.(1Ant, Tx and RxCG are both on aux port) */
+
+
+static int rtw_hwpdn_mode = 2;/* 0:disable, 1:enable, 2: by EFUSE config */
+
+static int rtw_hwpwrp_detect; /* HW power  ping detect 0:disable , 1:enable */
+
+static int rtw_hw_wps_pbc = 1;
+
+int rtw_mc2u_disable;
+
+static int rtw_80211d;
+
+static char *ifname = "wlan%d";
+module_param(ifname, charp, 0644);
+MODULE_PARM_DESC(ifname, "The default name to allocate for first interface");
+
+static char *if2name = "wlan%d";
+module_param(if2name, charp, 0644);
+MODULE_PARM_DESC(if2name, "The default name to allocate for second interface");
+
+char *rtw_initmac;  /*  temp mac address if users want to use instead of the mac address in Efuse */
+
+module_param(rtw_initmac, charp, 0644);
+module_param(rtw_channel_plan, int, 0644);
+module_param(rtw_rfintfs, int, 0644);
+module_param(rtw_lbkmode, int, 0644);
+module_param(rtw_network_mode, int, 0644);
+module_param(rtw_channel, int, 0644);
+module_param(rtw_wmm_enable, int, 0644);
+module_param(rtw_vrtl_carrier_sense, int, 0644);
+module_param(rtw_vcs_type, int, 0644);
+module_param(rtw_busy_thresh, int, 0644);
+module_param(rtw_led_enable, int, 0644);
+module_param(rtw_ht_enable, int, 0644);
+module_param(rtw_cbw40_enable, int, 0644);
+module_param(rtw_ampdu_enable, int, 0644);
+module_param(rtw_rx_stbc, int, 0644);
+module_param(rtw_ampdu_amsdu, int, 0644);
+module_param(rtw_lowrate_two_xmit, int, 0644);
+module_param(rtw_power_mgnt, int, 0644);
+module_param(rtw_smart_ps, int, 0644);
+module_param(rtw_low_power, int, 0644);
+module_param(rtw_wifi_spec, int, 0644);
+module_param(rtw_antdiv_cfg, int, 0644);
+module_param(rtw_antdiv_type, int, 0644);
+module_param(rtw_hwpdn_mode, int, 0644);
+module_param(rtw_hwpwrp_detect, int, 0644);
+module_param(rtw_hw_wps_pbc, int, 0644);
+
+static uint rtw_max_roaming_times = 2;
+module_param(rtw_max_roaming_times, uint, 0644);
+MODULE_PARM_DESC(rtw_max_roaming_times, "The max roaming times to try");
+
+static int rtw_fw_iol = 1;/*  0:Disable, 1:enable, 2:by usb speed */
+module_param(rtw_fw_iol, int, 0644);
+MODULE_PARM_DESC(rtw_fw_iol, "FW IOL");
+
+module_param(rtw_mc2u_disable, int, 0644);
+
+module_param(rtw_80211d, int, 0644);
+MODULE_PARM_DESC(rtw_80211d, "Enable 802.11d mechanism");
+
+static uint rtw_notch_filter = RTW_NOTCH_FILTER;
+module_param(rtw_notch_filter, uint, 0644);
+MODULE_PARM_DESC(rtw_notch_filter, "0:Disable, 1:Enable, 2:Enable only for P2P");
+
+static uint loadparam(struct adapter *padapter)
+{
+	struct registry_priv  *registry_par = &padapter->registrypriv;
+
+	registry_par->rfintfs = (u8)rtw_rfintfs;
+	registry_par->lbkmode = (u8)rtw_lbkmode;
+	registry_par->network_mode  = (u8)rtw_network_mode;
+
+	memcpy(registry_par->ssid.Ssid, "ANY", 3);
+	registry_par->ssid.SsidLength = 3;
+
+	registry_par->channel = (u8)rtw_channel;
+	registry_par->wireless_mode = (u8)rtw_wireless_mode;
+	registry_par->vrtl_carrier_sense = (u8)rtw_vrtl_carrier_sense;
+	registry_par->vcs_type = (u8)rtw_vcs_type;
+	registry_par->rts_thresh = (u16)rtw_rts_thresh;
+	registry_par->frag_thresh = (u16)rtw_frag_thresh;
+	registry_par->preamble = (u8)rtw_preamble;
+	registry_par->scan_mode = (u8)rtw_scan_mode;
+	registry_par->adhoc_tx_pwr = (u8)rtw_adhoc_tx_pwr;
+	registry_par->soft_ap =  (u8)rtw_soft_ap;
+	registry_par->smart_ps =  (u8)rtw_smart_ps;
+	registry_par->power_mgnt = (u8)rtw_power_mgnt;
+	registry_par->ips_mode = (u8)rtw_ips_mode;
+	registry_par->radio_enable = (u8)rtw_radio_enable;
+	registry_par->long_retry_lmt = (u8)rtw_long_retry_lmt;
+	registry_par->short_retry_lmt = (u8)rtw_short_retry_lmt;
+	registry_par->busy_thresh = (u16)rtw_busy_thresh;
+	registry_par->ack_policy = (u8)rtw_ack_policy;
+	registry_par->software_encrypt = (u8)rtw_software_encrypt;
+	registry_par->software_decrypt = (u8)rtw_software_decrypt;
+	registry_par->acm_method = (u8)rtw_acm_method;
+
+	 /* UAPSD */
+	registry_par->wmm_enable = (u8)rtw_wmm_enable;
+	registry_par->uapsd_enable = (u8)rtw_uapsd_enable;
+	registry_par->uapsd_max_sp = (u8)rtw_uapsd_max_sp;
+	registry_par->uapsd_acbk_en = (u8)rtw_uapsd_acbk_en;
+	registry_par->uapsd_acbe_en = (u8)rtw_uapsd_acbe_en;
+	registry_par->uapsd_acvi_en = (u8)rtw_uapsd_acvi_en;
+	registry_par->uapsd_acvo_en = (u8)rtw_uapsd_acvo_en;
+
+	registry_par->led_enable = (u8)rtw_led_enable;
+
+	registry_par->ht_enable = (u8)rtw_ht_enable;
+	registry_par->cbw40_enable = (u8)rtw_cbw40_enable;
+	registry_par->ampdu_enable = (u8)rtw_ampdu_enable;
+	registry_par->rx_stbc = (u8)rtw_rx_stbc;
+	registry_par->ampdu_amsdu = (u8)rtw_ampdu_amsdu;
+	registry_par->lowrate_two_xmit = (u8)rtw_lowrate_two_xmit;
+	registry_par->low_power = (u8)rtw_low_power;
+	registry_par->wifi_spec = (u8)rtw_wifi_spec;
+	registry_par->channel_plan = (u8)rtw_channel_plan;
+	registry_par->bAcceptAddbaReq = (u8)rtw_AcceptAddbaReq;
+	registry_par->antdiv_cfg = (u8)rtw_antdiv_cfg;
+	registry_par->antdiv_type = (u8)rtw_antdiv_type;
+	registry_par->hwpdn_mode = (u8)rtw_hwpdn_mode;/* 0:disable, 1:enable, 2:by EFUSE config */
+	registry_par->hwpwrp_detect = (u8)rtw_hwpwrp_detect;/* 0:disable, 1:enable */
+	registry_par->hw_wps_pbc = (u8)rtw_hw_wps_pbc;
+
+	registry_par->max_roaming_times = (u8)rtw_max_roaming_times;
+
+	registry_par->fw_iol = rtw_fw_iol;
+
+	registry_par->enable80211d = (u8)rtw_80211d;
+	snprintf(registry_par->ifname, 16, "%s", ifname);
+	snprintf(registry_par->if2name, 16, "%s", if2name);
+	registry_par->notch_filter = (u8)rtw_notch_filter;
+
+	return _SUCCESS;
+}
+
+static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct sockaddr *addr = p;
+
+	if (!padapter->bup)
+		memcpy(padapter->eeprompriv.mac_addr, addr->sa_data, ETH_ALEN);
+
+	return 0;
+}
+
+static struct net_device_stats *rtw_net_get_stats(struct net_device *pnetdev)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	padapter->stats.tx_packets = pxmitpriv->tx_pkts;/* pxmitpriv->tx_pkts++; */
+	padapter->stats.rx_packets = precvpriv->rx_pkts;/* precvpriv->rx_pkts++; */
+	padapter->stats.tx_dropped = pxmitpriv->tx_drop;
+	padapter->stats.rx_dropped = precvpriv->rx_drop;
+	padapter->stats.tx_bytes = pxmitpriv->tx_bytes;
+	padapter->stats.rx_bytes = precvpriv->rx_bytes;
+	return &padapter->stats;
+}
+
+/*
+ * AC to queue mapping
+ *
+ * AC_VO -> queue 0
+ * AC_VI -> queue 1
+ * AC_BE -> queue 2
+ * AC_BK -> queue 3
+ */
+static const u16 rtw_1d_to_queue[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
+
+/* Given a data frame determine the 802.1p/1d tag to use. */
+static unsigned int rtw_classify8021d(struct sk_buff *skb)
+{
+	unsigned int dscp;
+
+	/* skb->priority values from 256->263 are magic values to
+	 * directly indicate a specific 802.1d priority.  This is used
+	 * to allow 802.1d priority to be passed directly in from VLAN
+	 * tags, etc.
+	 */
+	if (skb->priority >= 256 && skb->priority <= 263)
+		return skb->priority - 256;
+
+	switch (skb->protocol) {
+	case htons(ETH_P_IP):
+		dscp = ip_hdr(skb)->tos & 0xfc;
+		break;
+	default:
+		return 0;
+	}
+
+	return dscp >> 5;
+}
+
+static u16 rtw_select_queue(struct net_device *dev, struct sk_buff *skb, struct net_device *sb_dev)
+{
+	struct adapter	*padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	skb->priority = rtw_classify8021d(skb);
+
+	if (pmlmepriv->acm_mask != 0)
+		skb->priority = qos_acm(pmlmepriv->acm_mask, skb->priority);
+
+	return rtw_1d_to_queue[skb->priority];
+}
+
+u16 rtw_recv_select_queue(struct sk_buff *skb)
+{
+	struct iphdr *piphdr;
+	unsigned int dscp;
+	__be16	eth_type;
+	u32 priority;
+	u8 *pdata = skb->data;
+
+	memcpy(&eth_type, pdata + (ETH_ALEN << 1), 2);
+
+	switch (eth_type) {
+	case htons(ETH_P_IP):
+		piphdr = (struct iphdr *)(pdata + ETH_HLEN);
+		dscp = piphdr->tos & 0xfc;
+		priority = dscp >> 5;
+		break;
+	default:
+		priority = 0;
+	}
+
+	return rtw_1d_to_queue[priority];
+}
+
+static const struct net_device_ops rtw_netdev_ops = {
+	.ndo_open = netdev_open,
+	.ndo_stop = netdev_close,
+	.ndo_start_xmit = rtw_xmit_entry,
+	.ndo_select_queue	= rtw_select_queue,
+	.ndo_set_mac_address = rtw_net_set_mac_address,
+	.ndo_get_stats = rtw_net_get_stats,
+};
+
+int rtw_init_netdev_name(struct net_device *pnetdev, const char *ifname)
+{
+	int err;
+
+	err = dev_alloc_name(pnetdev, ifname);
+	if (err < 0)
+		return err;
+
+	netif_carrier_off(pnetdev);
+	return 0;
+}
+
+static const struct device_type wlan_type = {
+	.name = "wlan",
+};
+
+struct net_device *rtw_init_netdev(struct adapter *old_padapter)
+{
+	struct adapter *padapter;
+	struct net_device *pnetdev;
+
+	if (old_padapter)
+		pnetdev = rtw_alloc_etherdev_with_old_priv(sizeof(struct adapter), (void *)old_padapter);
+	else
+		pnetdev = rtw_alloc_etherdev(sizeof(struct adapter));
+
+	if (!pnetdev)
+		return NULL;
+
+	pnetdev->dev.type = &wlan_type;
+	padapter = rtw_netdev_priv(pnetdev);
+	padapter->pnetdev = pnetdev;
+	pnetdev->netdev_ops = &rtw_netdev_ops;
+	pnetdev->watchdog_timeo = HZ * 3; /* 3 second timeout */
+	pnetdev->wireless_handlers = (struct iw_handler_def *)&rtw_handlers_def;
+
+	/* step 2. */
+	loadparam(padapter);
+
+	return pnetdev;
+}
+
+u32 rtw_start_drv_threads(struct adapter *padapter)
+{
+	u32 _status = _SUCCESS;
+
+	padapter->cmdThread = kthread_run(rtw_cmd_thread, padapter, "RTW_CMD_THREAD");
+	if (IS_ERR(padapter->cmdThread))
+		_status = _FAIL;
+	else
+		/* wait for rtw_cmd_thread() to start running */
+		wait_for_completion(&padapter->cmdpriv.start_cmd_thread);
+
+	return _status;
+}
+
+void rtw_stop_drv_threads(struct adapter *padapter)
+{
+	/* Below is to termindate rtw_cmd_thread & event_thread... */
+	complete(&padapter->cmdpriv.enqueue_cmd);
+	if (padapter->cmdThread)
+		/* wait for rtw_cmd_thread() to stop running */
+		wait_for_completion(&padapter->cmdpriv.stop_cmd_thread);
+}
+
+static void rtw_init_default_value(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	/* xmit_priv */
+	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
+	pxmitpriv->vcs = pregistrypriv->vcs_type;
+	pxmitpriv->vcs_type = pregistrypriv->vcs_type;
+	pxmitpriv->frag_len = pregistrypriv->frag_thresh;
+
+	/* mlme_priv */
+	pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+	pmlmepriv->scan_mode = SCAN_ACTIVE;
+
+	/* ht_priv */
+	pmlmepriv->htpriv.ampdu_enable = false;/* set to disabled */
+
+	/* security_priv */
+	psecuritypriv->binstallGrpkey = _FAIL;
+	psecuritypriv->sw_encrypt = pregistrypriv->software_encrypt;
+	psecuritypriv->sw_decrypt = pregistrypriv->software_decrypt;
+	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+	psecuritypriv->dot11PrivacyKeyIndex = 0;
+	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpKeyid = 1;
+	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
+	psecuritypriv->ndisencryptstatus = Ndis802_11WEPDisabled;
+
+	/* registry_priv */
+	rtw_init_registrypriv_dev_network(padapter);
+	rtw_update_registrypriv_dev_network(padapter);
+
+	/* hal_priv */
+	rtl8188eu_init_default_value(padapter);
+
+	/* misc. */
+	padapter->bReadPortCancel = false;
+	padapter->bWritePortCancel = false;
+	padapter->bRxRSSIDisplay = 0;
+	padapter->bNotifyChannelChange = 0;
+	padapter->bShowGetP2PState = 1;
+}
+
+u8 rtw_reset_drv_sw(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* hal_priv */
+	rtl8188eu_init_default_value(padapter);
+	padapter->bReadPortCancel = false;
+	padapter->bWritePortCancel = false;
+	padapter->bRxRSSIDisplay = 0;
+	pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+
+	padapter->xmitpriv.tx_pkts = 0;
+	padapter->recvpriv.rx_pkts = 0;
+
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING);
+
+	/* mlmeextpriv */
+	padapter->mlmeextpriv.sitesurvey_res.state = SCAN_DISABLE;
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	return _SUCCESS;
+}
+
+u8 rtw_init_drv_sw(struct adapter *padapter)
+{
+	if ((rtw_init_cmd_priv(&padapter->cmdpriv)) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "rtw_init_cmd_priv failed\n");
+		return _FAIL;
+	}
+
+	padapter->cmdpriv.padapter = padapter;
+
+	if ((rtw_init_evt_priv(&padapter->evtpriv)) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "rtw_init_evt_priv failed\n");
+		goto free_cmd_priv;
+	}
+
+	if (rtw_init_mlme_priv(padapter) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "rtw_init_mlme_priv failed\n");
+		goto free_evt_priv;
+	}
+
+	rtw_init_wifidirect_timers(padapter);
+	init_wifidirect_info(padapter, P2P_ROLE_DISABLE);
+	reset_global_wifidirect_info(padapter);
+
+	init_mlme_ext_priv(padapter);
+
+	if (_rtw_init_xmit_priv(&padapter->xmitpriv, padapter) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "_rtw_init_xmit_priv failed\n");
+		goto free_mlme_ext;
+	}
+
+	if (_rtw_init_recv_priv(&padapter->recvpriv, padapter) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "_rtw_init_recv_priv failed\n");
+		goto free_xmit_priv;
+	}
+
+	if (_rtw_init_sta_priv(&padapter->stapriv) == _FAIL) {
+		dev_err(dvobj_to_dev(padapter->dvobj), "_rtw_init_sta_priv failed\n");
+		goto free_recv_priv;
+	}
+
+	padapter->stapriv.padapter = padapter;
+
+	rtw_init_bcmc_stainfo(padapter);
+
+	rtw_init_pwrctrl_priv(padapter);
+
+	rtw_init_default_value(padapter);
+
+	rtl8188e_init_dm_priv(padapter);
+	rtl8188eu_InitSwLeds(padapter);
+
+	spin_lock_init(&padapter->br_ext_lock);
+
+	return _SUCCESS;
+
+free_recv_priv:
+	_rtw_free_recv_priv(&padapter->recvpriv);
+
+free_xmit_priv:
+	_rtw_free_xmit_priv(&padapter->xmitpriv);
+
+free_mlme_ext:
+	free_mlme_ext_priv(&padapter->mlmeextpriv);
+
+	rtw_free_mlme_priv(&padapter->mlmepriv);
+
+free_evt_priv:
+	rtw_free_evt_priv(&padapter->evtpriv);
+
+free_cmd_priv:
+	rtw_free_cmd_priv(&padapter->cmdpriv);
+
+	return _FAIL;
+}
+
+void rtw_cancel_all_timer(struct adapter *padapter)
+{
+	_cancel_timer_ex(&padapter->mlmepriv.assoc_timer);
+
+	_cancel_timer_ex(&padapter->mlmepriv.scan_to_timer);
+
+	_cancel_timer_ex(&padapter->mlmepriv.dynamic_chk_timer);
+
+	/*  cancel sw led timer */
+	rtl8188eu_DeInitSwLeds(padapter);
+
+	_cancel_timer_ex(&padapter->pwrctrlpriv.pwr_state_check_timer);
+
+	_cancel_timer_ex(&padapter->recvpriv.signal_stat_timer);
+}
+
+u8 rtw_free_drv_sw(struct adapter *padapter)
+{
+	/* we can call rtw_p2p_enable here, but: */
+	/*  1. rtw_p2p_enable may have IO operation */
+	/*  2. rtw_p2p_enable is bundled with wext interface */
+	{
+		struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+			_cancel_timer_ex(&pwdinfo->find_phase_timer);
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			_cancel_timer_ex(&pwdinfo->pre_tx_scan_timer);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
+		}
+	}
+
+	free_mlme_ext_priv(&padapter->mlmeextpriv);
+
+	rtw_free_cmd_priv(&padapter->cmdpriv);
+
+	rtw_free_evt_priv(&padapter->evtpriv);
+
+	rtw_free_mlme_priv(&padapter->mlmepriv);
+	_rtw_free_xmit_priv(&padapter->xmitpriv);
+
+	_rtw_free_sta_priv(&padapter->stapriv); /* will free bcmc_stainfo here */
+
+	_rtw_free_recv_priv(&padapter->recvpriv);
+
+	/* free the old_pnetdev */
+	if (padapter->rereg_nd_name_priv.old_pnetdev) {
+		free_netdev(padapter->rereg_nd_name_priv.old_pnetdev);
+		padapter->rereg_nd_name_priv.old_pnetdev = NULL;
+	}
+
+	/*  clear pbuddystruct adapter to avoid access wrong pointer. */
+	if (padapter->pbuddy_adapter)
+		padapter->pbuddy_adapter->pbuddy_adapter = NULL;
+
+	return _SUCCESS;
+}
+
+void netdev_br_init(struct net_device *netdev)
+{
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+
+	rcu_read_lock();
+
+	if (rcu_dereference(adapter->pnetdev->rx_handler_data)) {
+		struct net_device *br_netdev;
+		struct net *devnet = NULL;
+
+		devnet = dev_net(netdev);
+		br_netdev = dev_get_by_name(devnet, CONFIG_BR_EXT_BRNAME);
+		if (br_netdev) {
+			memcpy(adapter->br_mac, br_netdev->dev_addr, ETH_ALEN);
+			dev_put(br_netdev);
+		} else {
+			pr_info("%s()-%d: dev_get_by_name(%s) failed!",
+				__func__, __LINE__, CONFIG_BR_EXT_BRNAME);
+		}
+	}
+	adapter->ethBrExtInfo.addPPPoETag = 1;
+
+	rcu_read_unlock();
+}
+
+static int _netdev_open(struct net_device *pnetdev)
+{
+	uint status;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+
+	if (!padapter->bup) {
+		padapter->bDriverStopped = false;
+		padapter->bSurpriseRemoved = false;
+		padapter->bCardDisableWOHSM = false;
+
+		status = rtw_hal_init(padapter);
+		if (status == _FAIL)
+			goto netdev_open_error;
+
+		netdev_dbg(pnetdev, "MAC Address = %pM\n", pnetdev->dev_addr);
+
+		status = rtw_start_drv_threads(padapter);
+		if (status == _FAIL) {
+			pr_info("Initialize driver software resource Failed!\n");
+			goto netdev_open_error;
+		}
+
+		if (init_hw_mlme_ext(padapter) == _FAIL) {
+			pr_info("can't init mlme_ext_priv\n");
+			goto netdev_open_error;
+		}
+		if (padapter->intf_start)
+			padapter->intf_start(padapter);
+
+		rtw_led_control(padapter, LED_CTL_NO_LINK);
+
+		padapter->bup = true;
+	}
+	padapter->net_closed = false;
+
+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+
+	padapter->pwrctrlpriv.bips_processing = false;
+	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
+
+	if (!rtw_netif_queue_stopped(pnetdev))
+		netif_tx_start_all_queues(pnetdev);
+	else
+		netif_tx_wake_all_queues(pnetdev);
+
+	netdev_br_init(pnetdev);
+
+	return 0;
+
+netdev_open_error:
+	padapter->bup = false;
+	netif_carrier_off(pnetdev);
+	netif_tx_stop_all_queues(pnetdev);
+	return -1;
+}
+
+int netdev_open(struct net_device *pnetdev)
+{
+	int ret;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+
+	mutex_lock(padapter->hw_init_mutex);
+	ret = _netdev_open(pnetdev);
+	mutex_unlock(padapter->hw_init_mutex);
+	return ret;
+}
+
+static int  ips_netdrv_open(struct adapter *padapter)
+{
+	int status = _SUCCESS;
+	padapter->net_closed = false;
+
+	padapter->bDriverStopped = false;
+	padapter->bSurpriseRemoved = false;
+	padapter->bCardDisableWOHSM = false;
+
+	status = rtw_hal_init(padapter);
+	if (status == _FAIL)
+		goto netdev_open_error;
+
+	if (padapter->intf_start)
+		padapter->intf_start(padapter);
+
+	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 5000);
+
+	return _SUCCESS;
+
+netdev_open_error:
+	return _FAIL;
+}
+
+int rtw_ips_pwr_up(struct adapter *padapter)
+{
+	int result;
+	rtw_reset_drv_sw(padapter);
+
+	result = ips_netdrv_open(padapter);
+
+	rtw_led_control(padapter, LED_CTL_NO_LINK);
+
+	return result;
+}
+
+void rtw_ips_pwr_down(struct adapter *padapter)
+{
+	padapter->bCardDisableWOHSM = true;
+	padapter->net_closed = true;
+
+	rtw_led_control(padapter, LED_CTL_POWER_OFF);
+
+	rtw_ips_dev_unload(padapter);
+	padapter->bCardDisableWOHSM = false;
+}
+
+static void rtw_fifo_cleanup(struct adapter *adapter)
+{
+	struct pwrctrl_priv *pwrpriv = &adapter->pwrctrlpriv;
+	u8 trycnt = 100;
+	int res;
+	u32 reg;
+
+	/* pause tx */
+	rtw_write8(adapter, REG_TXPAUSE, 0xff);
+
+	/* keep sn */
+	/* FIXME: return an error to caller */
+	res = rtw_read16(adapter, REG_NQOS_SEQ, &adapter->xmitpriv.nqos_ssn);
+	if (res)
+		return;
+
+	if (!pwrpriv->bkeepfwalive) {
+		/* RX DMA stop */
+		res = rtw_read32(adapter, REG_RXPKT_NUM, &reg);
+		if (res)
+			return;
+
+		rtw_write32(adapter, REG_RXPKT_NUM,
+			    (reg | RW_RELEASE_EN));
+		do {
+			res = rtw_read32(adapter, REG_RXPKT_NUM, &reg);
+			if (res)
+				continue;
+
+			if (!(reg & RXDMA_IDLE))
+				break;
+		} while (trycnt--);
+
+		/* RQPN Load 0 */
+		rtw_write16(adapter, REG_RQPN_NPQ, 0x0);
+		rtw_write32(adapter, REG_RQPN, 0x80000000);
+		mdelay(10);
+	}
+}
+
+void rtw_ips_dev_unload(struct adapter *padapter)
+{
+	rtw_fifo_cleanup(padapter);
+
+	if (padapter->intf_stop)
+		padapter->intf_stop(padapter);
+
+	/* s5. */
+	if (!padapter->bSurpriseRemoved)
+		rtw_hal_deinit(padapter);
+}
+
+int netdev_close(struct net_device *pnetdev)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	padapter->net_closed = true;
+
+	if (padapter->pwrctrlpriv.rf_pwrstate == rf_on) {
+		/* s1. */
+		if (pnetdev) {
+			if (!rtw_netif_queue_stopped(pnetdev))
+				netif_tx_stop_all_queues(pnetdev);
+		}
+
+		/* s2. */
+		LeaveAllPowerSaveMode(padapter);
+		rtw_disassoc_cmd(padapter, 500, false);
+		/* s2-2.  indicate disconnect to os */
+		rtw_indicate_disconnect(padapter);
+		/* s2-3. */
+		rtw_free_assoc_resources(padapter, 1);
+		/* s2-4. */
+		rtw_free_network_queue(padapter, true);
+		/*  Close LED */
+		rtw_led_control(padapter, LED_CTL_POWER_OFF);
+	}
+
+	nat25_db_cleanup(padapter);
+
+	rtw_p2p_enable(padapter, P2P_ROLE_DISABLE);
+
+	kfree(dvobj->firmware.data);
+	dvobj->firmware.data = NULL;
+
+	return 0;
+}
diff --git a/drivers/staging/r8188eu/os_dep/osdep_service.c b/drivers/staging/r8188eu/os_dep/osdep_service.c
new file mode 100644
index 000000000..88271f956
--- /dev/null
+++ b/drivers/staging/r8188eu/os_dep/osdep_service.c
@@ -0,0 +1,227 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _OSDEP_SERVICE_C_
+
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/rtw_ioctl_set.h"
+
+/*
+* Translate the OS dependent @param error_code to OS independent RTW_STATUS_CODE
+* @return: one of RTW_STATUS_CODE
+*/
+inline int RTW_STATUS_CODE(int error_code)
+{
+	if (error_code >= 0)
+		return _SUCCESS;
+	return _FAIL;
+}
+
+void *rtw_malloc2d(int h, int w, int size)
+{
+	int j;
+
+	void **a = kzalloc(h * sizeof(void *) + h * w * size, GFP_KERNEL);
+	if (!a)
+		return NULL;
+
+	for (j = 0; j < h; j++)
+		a[j] = ((char *)(a + h)) + j * w * size;
+
+	return a;
+}
+
+/*
+For the following list_xxx operations,
+caller must guarantee the atomic context.
+Otherwise, there will be racing condition.
+*/
+/*
+Caller must check if the list is empty before calling rtw_list_delete
+*/
+
+static const struct device_type wlan_type = {
+	.name = "wlan",
+};
+
+struct net_device *rtw_alloc_etherdev_with_old_priv(int sizeof_priv,
+						    void *old_priv)
+{
+	struct net_device *pnetdev;
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	pnetdev = alloc_etherdev_mq(sizeof(struct rtw_netdev_priv_indicator), 4);
+	if (!pnetdev)
+		return NULL;
+
+	pnetdev->dev.type = &wlan_type;
+	pnpi = netdev_priv(pnetdev);
+	pnpi->priv = old_priv;
+	pnpi->sizeof_priv = sizeof_priv;
+
+	return pnetdev;
+}
+
+struct net_device *rtw_alloc_etherdev(int sizeof_priv)
+{
+	struct net_device *pnetdev;
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	pnetdev = alloc_etherdev_mq(sizeof(struct rtw_netdev_priv_indicator), 4);
+	if (!pnetdev)
+		return NULL;
+
+	pnpi = netdev_priv(pnetdev);
+
+	pnpi->priv = vzalloc(sizeof_priv);
+	if (!pnpi->priv) {
+		free_netdev(pnetdev);
+		return NULL;
+	}
+
+	pnpi->sizeof_priv = sizeof_priv;
+
+	return pnetdev;
+}
+
+void rtw_free_netdev(struct net_device *netdev)
+{
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	pnpi = netdev_priv(netdev);
+
+	vfree(pnpi->priv);
+	free_netdev(netdev);
+}
+
+int rtw_change_ifname(struct adapter *padapter, const char *ifname)
+{
+	struct net_device *pnetdev;
+	struct net_device *cur_pnetdev;
+	struct rereg_nd_name_data *rereg_priv;
+	int ret;
+
+	if (!padapter)
+		goto error;
+
+	cur_pnetdev = padapter->pnetdev;
+	rereg_priv = &padapter->rereg_nd_name_priv;
+
+	/* free the old_pnetdev */
+	if (rereg_priv->old_pnetdev) {
+		free_netdev(rereg_priv->old_pnetdev);
+		rereg_priv->old_pnetdev = NULL;
+	}
+
+	if (!rtnl_is_locked())
+		unregister_netdev(cur_pnetdev);
+	else
+		unregister_netdevice(cur_pnetdev);
+
+	rereg_priv->old_pnetdev = cur_pnetdev;
+
+	pnetdev = rtw_init_netdev(padapter);
+	if (!pnetdev)  {
+		ret = -1;
+		goto error;
+	}
+
+	SET_NETDEV_DEV(pnetdev, dvobj_to_dev(adapter_to_dvobj(padapter)));
+
+	rtw_init_netdev_name(pnetdev, ifname);
+
+	eth_hw_addr_set(pnetdev, padapter->eeprompriv.mac_addr);
+
+	if (!rtnl_is_locked())
+		ret = register_netdev(pnetdev);
+	else
+		ret = register_netdevice(pnetdev);
+	if (ret != 0)
+		goto error;
+
+	return 0;
+error:
+	return -1;
+}
+
+void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len)
+{
+	u32 dup_len = 0;
+	u8 *ori = NULL;
+	u8 *dup = NULL;
+
+	if (!buf || !buf_len)
+		return;
+
+	if (!src || !src_len)
+		goto keep_ori;
+
+	/* duplicate src */
+	dup = kmalloc(src_len, GFP_ATOMIC);
+	if (dup) {
+		dup_len = src_len;
+		memcpy(dup, src, dup_len);
+	}
+
+keep_ori:
+	ori = *buf;
+
+	/* replace buf with dup */
+	*buf_len = 0;
+	*buf = dup;
+	*buf_len = dup_len;
+
+	/* free ori */
+	kfree(ori);
+}
+
+/**
+ * rtw_cbuf_empty - test if cbuf is empty
+ * @cbuf: pointer of struct rtw_cbuf
+ *
+ * Returns: true if cbuf is empty
+ */
+inline bool rtw_cbuf_empty(struct rtw_cbuf *cbuf)
+{
+	return cbuf->write == cbuf->read;
+}
+
+/**
+ * rtw_cbuf_pop - pop a pointer from cbuf
+ * @cbuf: pointer of struct rtw_cbuf
+ *
+ * Lock free operation, be careful of the use scheme
+ * Returns: pointer popped out
+ */
+void *rtw_cbuf_pop(struct rtw_cbuf *cbuf)
+{
+	void *buf;
+	if (rtw_cbuf_empty(cbuf))
+		return NULL;
+
+	buf = cbuf->bufs[cbuf->read];
+	cbuf->read = (cbuf->read + 1) % cbuf->size;
+
+	return buf;
+}
+
+/**
+ * rtw_cbuf_alloc - allocate a rtw_cbuf with given size and do initialization
+ * @size: size of pointer
+ *
+ * Returns: pointer of srtuct rtw_cbuf, NULL for allocation failure
+ */
+struct rtw_cbuf *rtw_cbuf_alloc(u32 size)
+{
+	struct rtw_cbuf *cbuf;
+
+	cbuf = kmalloc(struct_size(cbuf, bufs, size), GFP_KERNEL);
+
+	if (cbuf) {
+		cbuf->write = 0;
+		cbuf->read = 0;
+		cbuf->size = size;
+	}
+	return cbuf;
+}
diff --git a/drivers/staging/r8188eu/os_dep/usb_intf.c b/drivers/staging/r8188eu/os_dep/usb_intf.c
new file mode 100644
index 000000000..5fbfbcd95
--- /dev/null
+++ b/drivers/staging/r8188eu/os_dep/usb_intf.c
@@ -0,0 +1,472 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2011 Realtek Corporation. */
+
+#include <linux/usb.h>
+#include "../include/osdep_service.h"
+#include "../include/drv_types.h"
+#include "../include/hal_intf.h"
+#include "../include/osdep_intf.h"
+#include "../include/usb_ops.h"
+#include "../include/usb_osintf.h"
+#include "../include/rtw_ioctl.h"
+#include "../include/rtl8188e_hal.h"
+
+int ui_pid[3] = {0, 0, 0};
+
+static int rtw_suspend(struct usb_interface *intf, pm_message_t message);
+static int rtw_resume(struct usb_interface *intf);
+
+static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device_id *pdid);
+static void rtw_dev_remove(struct usb_interface *pusb_intf);
+
+#define USB_VENDER_ID_REALTEK		0x0bda
+
+/* DID_USB_v916_20130116 */
+static struct usb_device_id rtw_usb_id_tbl[] = {
+	/*=== Realtek demoboard ===*/
+	{USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8179)}, /* 8188EUS */
+	{USB_DEVICE(USB_VENDER_ID_REALTEK, 0x0179)}, /* 8188ETV */
+	{USB_DEVICE(USB_VENDER_ID_REALTEK, 0xffef)}, /* Rosewill USB-N150 Nano */
+	/*=== Customer ID ===*/
+	/****** 8188EUS ********/
+	{USB_DEVICE(0x07B8, 0x8179)}, /* Abocom - Abocom */
+	{USB_DEVICE(0x0DF6, 0x0076)}, /* Sitecom N150 v2 */
+	{USB_DEVICE(0x2001, 0x330F)}, /* DLink DWA-125 REV D1 */
+	{USB_DEVICE(0x2001, 0x3310)}, /* Dlink DWA-123 REV D1 */
+	{USB_DEVICE(0x2001, 0x3311)}, /* DLink GO-USB-N150 REV B1 */
+	{USB_DEVICE(0x2001, 0x331B)}, /* D-Link DWA-121 rev B1 */
+	{USB_DEVICE(0x056E, 0x4008)}, /* Elecom WDC-150SU2M */
+	{USB_DEVICE(0x2357, 0x010c)}, /* TP-Link TL-WN722N v2 */
+	{USB_DEVICE(0x2357, 0x0111)}, /* TP-Link TL-WN727N v5.21 */
+	{USB_DEVICE(0x2C4E, 0x0102)}, /* MERCUSYS MW150US v2 */
+	{USB_DEVICE(0x0B05, 0x18F0)}, /* ASUS USB-N10 Nano B1 */
+	{USB_DEVICE(0x7392, 0xb811)}, /* Edimax EW-7811Un V2 */
+	{}	/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, rtw_usb_id_tbl);
+
+struct rtw_usb_drv {
+	struct usb_driver usbdrv;
+	int drv_registered;
+	struct mutex hw_init_mutex;
+};
+
+static struct rtw_usb_drv rtl8188e_usb_drv = {
+	.usbdrv.name = KBUILD_MODNAME,
+	.usbdrv.probe = rtw_drv_init,
+	.usbdrv.disconnect = rtw_dev_remove,
+	.usbdrv.id_table = rtw_usb_id_tbl,
+	.usbdrv.suspend =  rtw_suspend,
+	.usbdrv.resume = rtw_resume,
+	.usbdrv.reset_resume   = rtw_resume,
+};
+
+static struct rtw_usb_drv *usb_drv = &rtl8188e_usb_drv;
+
+static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
+{
+	int	i;
+	u8	rt_num_in_pipes = 0;
+	struct dvobj_priv *pdvobjpriv;
+	struct usb_host_config		*phost_conf;
+	struct usb_config_descriptor	*pconf_desc;
+	struct usb_host_interface	*phost_iface;
+	struct usb_interface_descriptor	*piface_desc;
+	struct usb_endpoint_descriptor	*pendp_desc;
+	struct usb_device	*pusbd;
+
+	pdvobjpriv = kzalloc(sizeof(*pdvobjpriv), GFP_KERNEL);
+	if (!pdvobjpriv)
+		goto err;
+
+	pdvobjpriv->pusbintf = usb_intf;
+	pusbd = interface_to_usbdev(usb_intf);
+	pdvobjpriv->pusbdev = pusbd;
+	usb_set_intfdata(usb_intf, pdvobjpriv);
+
+	pdvobjpriv->RtNumOutPipes = 0;
+
+	phost_conf = pusbd->actconfig;
+	pconf_desc = &phost_conf->desc;
+
+	phost_iface = &usb_intf->altsetting[0];
+	piface_desc = &phost_iface->desc;
+
+	pdvobjpriv->NumInterfaces = pconf_desc->bNumInterfaces;
+	pdvobjpriv->InterfaceNumber = piface_desc->bInterfaceNumber;
+
+	for (i = 0; i < piface_desc->bNumEndpoints; i++) {
+		int ep_num;
+		pendp_desc = &phost_iface->endpoint[i].desc;
+
+		ep_num = usb_endpoint_num(pendp_desc);
+
+		if (usb_endpoint_is_bulk_in(pendp_desc)) {
+			pdvobjpriv->RtInPipe = ep_num;
+			rt_num_in_pipes++;
+		} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
+			pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] =
+				ep_num;
+			pdvobjpriv->RtNumOutPipes++;
+		}
+	}
+
+	if (rt_num_in_pipes != 1)
+		goto err;
+
+	/* 3 misc */
+	rtw_reset_continual_urb_error(pdvobjpriv);
+
+	usb_get_dev(pusbd);
+	return pdvobjpriv;
+
+err:
+	kfree(pdvobjpriv);
+	return NULL;
+}
+
+static void usb_dvobj_deinit(struct usb_interface *usb_intf)
+{
+	struct dvobj_priv *dvobj = usb_get_intfdata(usb_intf);
+
+	usb_set_intfdata(usb_intf, NULL);
+	if (dvobj) {
+		/* Modify condition for 92DU DMDP 2010.11.18, by Thomas */
+		if ((dvobj->NumInterfaces != 2 &&
+		    dvobj->NumInterfaces != 3) ||
+	    (dvobj->InterfaceNumber == 1)) {
+			if (interface_to_usbdev(usb_intf)->state !=
+			    USB_STATE_NOTATTACHED)
+				/* If we didn't unplug usb dongle and
+				 * remove/insert module, driver fails
+				 * on sitesurvey for the first time when
+				 * device is up . Reset usb port for sitesurvey
+				 * fail issue. */
+				usb_reset_device(interface_to_usbdev(usb_intf));
+		}
+		kfree(dvobj);
+	}
+
+	usb_put_dev(interface_to_usbdev(usb_intf));
+
+}
+
+static void usb_intf_start(struct adapter *padapter)
+{
+	rtl8188eu_inirp_init(padapter);
+}
+
+static void usb_intf_stop(struct adapter *padapter)
+{
+	/* cancel in irp */
+	rtw_read_port_cancel(padapter);
+
+	/* cancel out irp */
+	rtw_write_port_cancel(padapter);
+
+	/* todo:cancel other irps */
+}
+
+static void rtw_dev_unload(struct adapter *padapter)
+{
+	if (padapter->bup) {
+		padapter->bDriverStopped = true;
+		if (padapter->xmitpriv.ack_tx)
+			rtw_ack_tx_done(&padapter->xmitpriv, RTW_SCTX_DONE_DRV_STOP);
+		/* s3. */
+		if (padapter->intf_stop)
+			padapter->intf_stop(padapter);
+		/* s4. */
+		rtw_stop_drv_threads(padapter);
+
+		/* s5. */
+		if (!padapter->bSurpriseRemoved) {
+			rtw_hal_deinit(padapter);
+			padapter->bSurpriseRemoved = true;
+		}
+
+		padapter->bup = false;
+	}
+}
+
+static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
+{
+	struct dvobj_priv *dvobj = usb_get_intfdata(pusb_intf);
+	struct adapter *padapter = dvobj->if1;
+	struct net_device *pnetdev = padapter->pnetdev;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	if ((!padapter->bup) || (padapter->bDriverStopped) ||
+	    (padapter->bSurpriseRemoved))
+		goto exit;
+
+	pwrpriv->bInSuspend = true;
+	rtw_cancel_all_timer(padapter);
+	LeaveAllPowerSaveMode(padapter);
+
+	mutex_lock(&pwrpriv->lock);
+	/* s1. */
+	if (pnetdev) {
+		netif_carrier_off(pnetdev);
+		netif_tx_stop_all_queues(pnetdev);
+	}
+
+	/* s2. */
+	rtw_disassoc_cmd(padapter, 0, false);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_LINKED))
+		pmlmepriv->to_roaming = 1;
+	/* s2-2.  indicate disconnect to os */
+	rtw_indicate_disconnect(padapter);
+	/* s2-3. */
+	rtw_free_assoc_resources(padapter, 1);
+	/* s2-4. */
+	rtw_free_network_queue(padapter, true);
+
+	rtw_dev_unload(padapter);
+	mutex_unlock(&pwrpriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		rtw_indicate_scan_done(padapter);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		rtw_indicate_disconnect(padapter);
+
+exit:
+		return 0;
+}
+
+static int rtw_resume(struct usb_interface *pusb_intf)
+{
+	struct dvobj_priv *dvobj = usb_get_intfdata(pusb_intf);
+	struct adapter *padapter = dvobj->if1;
+	struct net_device *pnetdev;
+	struct pwrctrl_priv *pwrpriv = NULL;
+	int ret = -1;
+
+	pnetdev = padapter->pnetdev;
+	pwrpriv = &padapter->pwrctrlpriv;
+
+	mutex_lock(&pwrpriv->lock);
+	rtw_reset_drv_sw(padapter);
+	if (pwrpriv)
+		pwrpriv->bkeepfwalive = false;
+
+	if (netdev_open(pnetdev) != 0) {
+		mutex_unlock(&pwrpriv->lock);
+		goto exit;
+	}
+
+	netif_device_attach(pnetdev);
+	netif_carrier_on(pnetdev);
+
+	mutex_unlock(&pwrpriv->lock);
+
+	if (padapter->pid[1] != 0)
+		rtw_signal_process(padapter->pid[1], SIGUSR2);
+
+	rtw_roaming(padapter, NULL);
+
+	ret = 0;
+exit:
+	if (pwrpriv)
+		pwrpriv->bInSuspend = false;
+
+	return ret;
+}
+
+/*
+ * drv_init() - a device potentially for us
+ *
+ * notes: drv_init() is called when the bus driver has located
+ * a card for us to support.
+ *        We accept the new device by returning 0.
+ */
+
+static int rtw_usb_if1_init(struct dvobj_priv *dvobj, struct usb_interface *pusb_intf)
+{
+	struct adapter *padapter = NULL;
+	struct net_device *pnetdev = NULL;
+	struct io_priv *piopriv;
+	struct intf_hdl *pintf;
+	int ret;
+
+	padapter = vzalloc(sizeof(*padapter));
+	if (!padapter)
+		return -ENOMEM;
+
+	padapter->dvobj = dvobj;
+	dvobj->if1 = padapter;
+
+	padapter->bDriverStopped = true;
+
+	padapter->hw_init_mutex = &usb_drv->hw_init_mutex;
+
+	rtw_handle_dualmac(padapter, 1);
+
+	pnetdev = rtw_init_netdev(padapter);
+	if (!pnetdev) {
+		ret = -ENODEV;
+		goto handle_dualmac;
+	}
+	SET_NETDEV_DEV(pnetdev, dvobj_to_dev(dvobj));
+	padapter = rtw_netdev_priv(pnetdev);
+
+	padapter->intf_start = &usb_intf_start;
+	padapter->intf_stop = &usb_intf_stop;
+
+	/* step init_io_priv */
+	piopriv = &padapter->iopriv;
+	pintf = &piopriv->intf;
+	piopriv->padapter = padapter;
+	pintf->padapter = padapter;
+	pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+	/* step read_chip_version */
+	rtl8188e_read_chip_version(padapter);
+
+	/* step usb endpoint mapping */
+	ret = rtl8188eu_interface_configure(padapter);
+	if (ret)
+		goto handle_dualmac;
+
+	/* step read efuse/eeprom data and get mac_addr */
+	ret = ReadAdapterInfo8188EU(padapter);
+	if (ret)
+		goto handle_dualmac;
+
+	/* step 5. */
+	if (rtw_init_drv_sw(padapter) == _FAIL) {
+		ret = -ENODEV;
+		goto handle_dualmac;
+	}
+
+#ifdef CONFIG_PM
+	if (padapter->pwrctrlpriv.bSupportRemoteWakeup) {
+		dvobj->pusbdev->do_remote_wakeup = 1;
+		pusb_intf->needs_remote_wakeup = 1;
+		device_init_wakeup(&pusb_intf->dev, 1);
+	}
+#endif
+
+	/* 2012-07-11 Move here to prevent the 8723AS-VAU BT auto
+	 * suspend influence */
+	usb_autopm_get_interface(pusb_intf);
+
+	/*  alloc dev name after read efuse. */
+	ret = rtw_init_netdev_name(pnetdev, padapter->registrypriv.ifname);
+	if (ret)
+		goto free_drv_sw;
+	rtw_macaddr_cfg(padapter->eeprompriv.mac_addr);
+	rtw_init_wifidirect_addrs(padapter, padapter->eeprompriv.mac_addr,
+				  padapter->eeprompriv.mac_addr);
+	eth_hw_addr_set(pnetdev, padapter->eeprompriv.mac_addr);
+
+	/* step 6. Tell the network stack we exist */
+	ret = register_netdev(pnetdev);
+	if (ret)
+		goto free_drv_sw;
+
+	return 0;
+
+free_drv_sw:
+	rtw_cancel_all_timer(padapter);
+	rtw_free_drv_sw(padapter);
+handle_dualmac:
+	rtw_handle_dualmac(padapter, 0);
+	if (pnetdev)
+		rtw_free_netdev(pnetdev);
+	else
+		vfree(padapter);
+
+	return ret;
+}
+
+static void rtw_usb_if1_deinit(struct adapter *if1)
+{
+	struct net_device *pnetdev = if1->pnetdev;
+	struct mlme_priv *pmlmepriv = &if1->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		rtw_disassoc_cmd(if1, 0, false);
+
+	free_mlme_ap_info(if1);
+
+	if (pnetdev) {
+		/* will call netdev_close() */
+		unregister_netdev(pnetdev);
+	}
+	rtw_cancel_all_timer(if1);
+
+	rtw_dev_unload(if1);
+	rtw_handle_dualmac(if1, 0);
+	rtw_free_drv_sw(if1);
+	if (pnetdev)
+		rtw_free_netdev(pnetdev);
+}
+
+static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device_id *pdid)
+{
+	struct dvobj_priv *dvobj;
+	int ret;
+
+	/* Initialize dvobj_priv */
+	dvobj = usb_dvobj_init(pusb_intf);
+	if (!dvobj)
+		return -ENODEV;
+
+	ret = rtw_usb_if1_init(dvobj, pusb_intf);
+	if (ret) {
+		usb_dvobj_deinit(pusb_intf);
+		return ret;
+	}
+
+	if (ui_pid[1] != 0)
+		rtw_signal_process(ui_pid[1], SIGUSR2);
+
+	return 0;
+}
+
+/*
+ * dev_remove() - our device is being removed
+*/
+/* rmmod module & unplug(SurpriseRemoved) will call r871xu_dev_remove() => how to recognize both */
+static void rtw_dev_remove(struct usb_interface *pusb_intf)
+{
+	struct dvobj_priv *dvobj = usb_get_intfdata(pusb_intf);
+	struct adapter *padapter = dvobj->if1;
+
+	if (usb_drv->drv_registered)
+		padapter->bSurpriseRemoved = true;
+
+	rtw_pm_set_ips(padapter, IPS_NONE);
+	rtw_pm_set_lps(padapter, PS_MODE_ACTIVE);
+
+	LeaveAllPowerSaveMode(padapter);
+
+	rtw_usb_if1_deinit(padapter);
+
+	usb_dvobj_deinit(pusb_intf);
+}
+
+static int __init rtw_drv_entry(void)
+{
+	mutex_init(&usb_drv->hw_init_mutex);
+
+	usb_drv->drv_registered = true;
+	return usb_register(&usb_drv->usbdrv);
+}
+
+static void __exit rtw_drv_halt(void)
+{
+	usb_drv->drv_registered = false;
+	usb_deregister(&usb_drv->usbdrv);
+
+	mutex_destroy(&usb_drv->hw_init_mutex);
+}
+
+module_init(rtw_drv_entry);
+module_exit(rtw_drv_halt);
diff --git a/drivers/staging/r8188eu/os_dep/usb_ops_linux.c b/drivers/staging/r8188eu/os_dep/usb_ops_linux.c
new file mode 100644
index 000000000..220e592b7
--- /dev/null
+++ b/drivers/staging/r8188eu/os_dep/usb_ops_linux.c
@@ -0,0 +1,198 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2012 Realtek Corporation. */
+
+#define _USB_OPS_LINUX_C_
+
+#include "../include/drv_types.h"
+#include "../include/usb_ops_linux.h"
+#include "../include/rtl8188e_recv.h"
+
+static unsigned int ffaddr2pipehdl(struct dvobj_priv *pdvobj, u32 addr)
+{
+	unsigned int pipe = 0, ep_num = 0;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+
+	if (addr < HW_QUEUE_ENTRY) {
+		ep_num = pdvobj->Queue2Pipe[addr];
+		pipe = usb_sndbulkpipe(pusbd, ep_num);
+	}
+
+	return pipe;
+}
+
+void rtw_read_port_cancel(struct adapter *padapter)
+{
+	int i;
+	struct recv_buf *precvbuf = (struct recv_buf *)padapter->recvpriv.precv_buf;
+
+	padapter->bReadPortCancel = true;
+
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		precvbuf->reuse = true;
+		if (precvbuf->purb)
+			usb_kill_urb(precvbuf->purb);
+		precvbuf++;
+	}
+}
+
+static void usb_write_port_complete(struct urb *purb, struct pt_regs *regs)
+{
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)purb->context;
+	struct adapter	*padapter = pxmitbuf->padapter;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	switch (pxmitbuf->flags) {
+	case VO_QUEUE_INX:
+		pxmitpriv->voq_cnt--;
+		break;
+	case VI_QUEUE_INX:
+		pxmitpriv->viq_cnt--;
+		break;
+	case BE_QUEUE_INX:
+		pxmitpriv->beq_cnt--;
+		break;
+	case BK_QUEUE_INX:
+		pxmitpriv->bkq_cnt--;
+		break;
+	case HIGH_QUEUE_INX:
+		rtw_chk_hi_queue_cmd(padapter);
+		break;
+	default:
+		break;
+	}
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped ||
+	    padapter->bWritePortCancel)
+		goto check_completion;
+
+	if (purb->status) {
+		if (purb->status == -EINPROGRESS) {
+			goto check_completion;
+		} else if (purb->status == -ENOENT) {
+			goto check_completion;
+		} else if (purb->status == -ECONNRESET) {
+			goto check_completion;
+		} else if (purb->status == -ESHUTDOWN) {
+			padapter->bDriverStopped = true;
+			goto check_completion;
+		} else if ((purb->status != -EPIPE) && (purb->status != -EPROTO)) {
+			padapter->bSurpriseRemoved = true;
+
+			goto check_completion;
+		}
+	}
+
+check_completion:
+	rtw_sctx_done_err(&pxmitbuf->sctx,
+			  purb->status ? RTW_SCTX_DONE_WRITE_PORT_ERR :
+			  RTW_SCTX_DONE_SUCCESS);
+
+	rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+
+}
+
+u32 rtw_write_port(struct adapter *padapter, u32 addr, u32 cnt, u8 *wmem)
+{
+	unsigned long irqL;
+	unsigned int pipe;
+	int status;
+	u32 ret = _FAIL;
+	struct urb *purb = NULL;
+	struct dvobj_priv	*pdvobj = adapter_to_dvobj(padapter);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)wmem;
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_TX_DENY);
+		goto exit;
+	}
+
+	spin_lock_irqsave(&pxmitpriv->lock, irqL);
+
+	switch (addr) {
+	case VO_QUEUE_INX:
+		pxmitpriv->voq_cnt++;
+		pxmitbuf->flags = VO_QUEUE_INX;
+		break;
+	case VI_QUEUE_INX:
+		pxmitpriv->viq_cnt++;
+		pxmitbuf->flags = VI_QUEUE_INX;
+		break;
+	case BE_QUEUE_INX:
+		pxmitpriv->beq_cnt++;
+		pxmitbuf->flags = BE_QUEUE_INX;
+		break;
+	case BK_QUEUE_INX:
+		pxmitpriv->bkq_cnt++;
+		pxmitbuf->flags = BK_QUEUE_INX;
+		break;
+	case HIGH_QUEUE_INX:
+		pxmitbuf->flags = HIGH_QUEUE_INX;
+		break;
+	default:
+		pxmitbuf->flags = MGT_QUEUE_INX;
+		break;
+	}
+
+	spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
+
+	purb	= pxmitbuf->pxmit_urb;
+
+	/* translate DMA FIFO addr to pipehandle */
+	pipe = ffaddr2pipehdl(pdvobj, addr);
+
+	usb_fill_bulk_urb(purb, pusbd, pipe,
+			  pxmitframe->buf_addr, /*  pxmitbuf->pbuf */
+			  cnt,
+			  usb_write_port_complete,
+			  pxmitbuf);/* context is pxmitbuf */
+
+	status = usb_submit_urb(purb, GFP_ATOMIC);
+	if (status) {
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_WRITE_PORT_ERR);
+
+		switch (status) {
+		case -ENODEV:
+			padapter->bDriverStopped = true;
+			break;
+		default:
+			break;
+		}
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+/*    We add the URB_ZERO_PACKET flag to urb so that the host will send the zero packet automatically. */
+
+exit:
+	if (ret != _SUCCESS)
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+
+	return ret;
+}
+
+void rtw_write_port_cancel(struct adapter *padapter)
+{
+	int i;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)padapter->xmitpriv.pxmitbuf;
+
+	padapter->bWritePortCancel = true;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		if (pxmitbuf->pxmit_urb)
+			usb_kill_urb(pxmitbuf->pxmit_urb);
+		pxmitbuf++;
+	}
+
+	pxmitbuf = (struct xmit_buf *)padapter->xmitpriv.pxmit_extbuf;
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		if (pxmitbuf->pxmit_urb)
+			usb_kill_urb(pxmitbuf->pxmit_urb);
+		pxmitbuf++;
+	}
+}
diff --git a/drivers/staging/rtl8192e/Kconfig b/drivers/staging/rtl8192e/Kconfig
new file mode 100644
index 000000000..e06c189b4
--- /dev/null
+++ b/drivers/staging/rtl8192e/Kconfig
@@ -0,0 +1,61 @@
+# SPDX-License-Identifier: GPL-2.0
+config RTLLIB
+	tristate "Support for rtllib wireless devices"
+	depends on WLAN && m
+	select LIB80211
+	select CRC32
+	help
+	  If you have a wireless card that uses rtllib, say
+	  Y. Currently the only card is the rtl8192e.
+
+	  If unsure, say N.
+
+	  This driver adds support for rtllib wireless cards.
+	  Only the rtl8192e is supported as of now.
+
+if RTLLIB
+
+config RTLLIB_CRYPTO_CCMP
+	tristate "Support for rtllib CCMP crypto"
+	depends on RTLLIB
+	select CRYPTO
+	select CRYPTO_AES
+	select CRYPTO_CCM
+	default y
+	help
+	  CCMP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+	  Adds support for the CCM mode Protocol crypto driver for
+	  use in wireless cards (including rtllib cards).
+
+config RTLLIB_CRYPTO_TKIP
+	tristate "Support for rtllib TKIP crypto"
+	depends on RTLLIB
+	select CRYPTO
+	select CRYPTO_LIB_ARC4
+	select CRYPTO_MICHAEL_MIC
+	default y
+	help
+	  TKIP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+	  Adds support for the Temporal Key Integrity Protocol for
+	  the IEEE 802.11i standard for use on wireless cards.
+
+config RTLLIB_CRYPTO_WEP
+	tristate "Support for rtllib WEP crypto"
+	select CRYPTO_LIB_ARC4
+	depends on RTLLIB
+	default y
+	help
+	  WEP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+	  Adds support for the (now weak) Wired Equivalent Privacy
+	  (WEP) crypto protocol for wireless cards.
+	  NOTE: This protocol is now considered insecure.
+
+source "drivers/staging/rtl8192e/rtl8192e/Kconfig"
+
+endif
diff --git a/drivers/staging/rtl8192e/Makefile b/drivers/staging/rtl8192e/Makefile
new file mode 100644
index 000000000..6af519938
--- /dev/null
+++ b/drivers/staging/rtl8192e/Makefile
@@ -0,0 +1,20 @@
+# SPDX-License-Identifier: GPL-2.0
+rtllib-objs :=			\
+	dot11d.o		\
+	rtllib_module.o		\
+	rtllib_rx.o		\
+	rtllib_tx.o		\
+	rtllib_wx.o		\
+	rtllib_softmac.o	\
+	rtllib_softmac_wx.o	\
+	rtl819x_BAProc.o	\
+	rtl819x_HTProc.o	\
+	rtl819x_TSProc.o
+
+obj-$(CONFIG_RTLLIB) += rtllib.o
+
+obj-$(CONFIG_RTLLIB_CRYPTO_CCMP) += rtllib_crypt_ccmp.o
+obj-$(CONFIG_RTLLIB_CRYPTO_TKIP) += rtllib_crypt_tkip.o
+obj-$(CONFIG_RTLLIB_CRYPTO_WEP) += rtllib_crypt_wep.o
+
+obj-$(CONFIG_RTL8192E) += rtl8192e/
diff --git a/drivers/staging/rtl8192e/TODO b/drivers/staging/rtl8192e/TODO
new file mode 100644
index 000000000..d51f159d1
--- /dev/null
+++ b/drivers/staging/rtl8192e/TODO
@@ -0,0 +1,2 @@
+* merge into drivers/net/wireless/rtllib/rtl8192e
+* clean up function naming
diff --git a/drivers/staging/rtl8192e/dot11d.c b/drivers/staging/rtl8192e/dot11d.c
new file mode 100644
index 000000000..82c11caee
--- /dev/null
+++ b/drivers/staging/rtl8192e/dot11d.c
@@ -0,0 +1,168 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ ******************************************************************************/
+#include "dot11d.h"
+
+struct channel_list {
+	u8      channel[32];
+	u8      len;
+};
+
+static struct channel_list channel_array[] = {
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64,
+	  149, 153, 157, 161, 165}, 24},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56,
+	  60, 64}, 21},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52,
+	  56, 60, 64}, 22},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52,
+	  56, 60, 64}, 22},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52,
+	  56, 60, 64}, 22},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52,
+	 56, 60, 64}, 22},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52,
+	  56, 60, 64}, 21}
+};
+
+void dot11d_init(struct rtllib_device *ieee)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(ieee);
+
+	dot11d_info->enabled = false;
+
+	dot11d_info->state = DOT11D_STATE_NONE;
+	dot11d_info->country_len = 0;
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_power_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	RESET_CIE_WATCHDOG(ieee);
+}
+EXPORT_SYMBOL(dot11d_init);
+
+void dot11d_channel_map(u8 channel_plan, struct rtllib_device *ieee)
+{
+	int i, max_chan = 14, min_chan = 1;
+
+	ieee->global_domain = false;
+
+	if (channel_array[channel_plan].len != 0) {
+		memset(GET_DOT11D_INFO(ieee)->channel_map, 0,
+		       sizeof(GET_DOT11D_INFO(ieee)->channel_map));
+		for (i = 0; i < channel_array[channel_plan].len; i++) {
+			if (channel_array[channel_plan].channel[i] < min_chan ||
+			    channel_array[channel_plan].channel[i] > max_chan)
+				break;
+			GET_DOT11D_INFO(ieee)->channel_map[channel_array
+					[channel_plan].channel[i]] = 1;
+		}
+	}
+
+	switch (channel_plan) {
+	case COUNTRY_CODE_GLOBAL_DOMAIN:
+		ieee->global_domain = true;
+		for (i = 12; i <= 14; i++)
+			GET_DOT11D_INFO(ieee)->channel_map[i] = 2;
+		ieee->bss_start_channel = 10;
+		ieee->ibss_maxjoin_chal = 11;
+		break;
+
+	case COUNTRY_CODE_WORLD_WIDE_13:
+		for (i = 12; i <= 13; i++)
+			GET_DOT11D_INFO(ieee)->channel_map[i] = 2;
+		ieee->bss_start_channel = 10;
+		ieee->ibss_maxjoin_chal = 11;
+		break;
+
+	default:
+		ieee->bss_start_channel = 1;
+		ieee->ibss_maxjoin_chal = 14;
+		break;
+	}
+}
+EXPORT_SYMBOL(dot11d_channel_map);
+
+void dot11d_reset(struct rtllib_device *ieee)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(ieee);
+	u32 i;
+
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_power_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	for (i = 1; i <= 11; i++)
+		(dot11d_info->channel_map)[i] = 1;
+	for (i = 12; i <= 14; i++)
+		(dot11d_info->channel_map)[i] = 2;
+	dot11d_info->state = DOT11D_STATE_NONE;
+	dot11d_info->country_len = 0;
+	RESET_CIE_WATCHDOG(ieee);
+}
+
+void dot11d_update_country(struct rtllib_device *dev, u8 *address,
+			   u16 country_len, u8 *country)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+	u8 i, j, number_of_triples, max_channel_number;
+	struct chnl_txpow_triple *triple;
+
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_power_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	max_channel_number = 0;
+	number_of_triples = (country_len - 3) / 3;
+	triple = (struct chnl_txpow_triple *)(country + 3);
+	for (i = 0; i < number_of_triples; i++) {
+		if (max_channel_number >= triple->first_channel) {
+			netdev_info(dev->dev,
+				    "%s: Invalid country IE, skip it......1\n",
+				    __func__);
+			return;
+		}
+		if (MAX_CHANNEL_NUMBER < (triple->first_channel +
+		    triple->num_channels)) {
+			netdev_info(dev->dev,
+				    "%s: Invalid country IE, skip it......2\n",
+				    __func__);
+			return;
+		}
+
+		for (j = 0; j < triple->num_channels; j++) {
+			dot11d_info->channel_map[triple->first_channel + j] = 1;
+			dot11d_info->max_tx_power_list[triple->first_channel + j] =
+						 triple->max_tx_power;
+			max_channel_number = triple->first_channel + j;
+		}
+
+		triple = (struct chnl_txpow_triple *)((u8 *)triple + 3);
+	}
+
+	UPDATE_CIE_SRC(dev, address);
+
+	dot11d_info->country_len = country_len;
+	memcpy(dot11d_info->country_buffer, country, country_len);
+	dot11d_info->state = DOT11D_STATE_LEARNED;
+}
+
+void dot11d_scan_complete(struct rtllib_device *dev)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+
+	switch (dot11d_info->state) {
+	case DOT11D_STATE_LEARNED:
+		dot11d_info->state = DOT11D_STATE_DONE;
+		break;
+	case DOT11D_STATE_DONE:
+		dot11d_reset(dev);
+		break;
+	case DOT11D_STATE_NONE:
+		break;
+	}
+}
diff --git a/drivers/staging/rtl8192e/dot11d.h b/drivers/staging/rtl8192e/dot11d.h
new file mode 100644
index 000000000..6d2b93acf
--- /dev/null
+++ b/drivers/staging/rtl8192e/dot11d.h
@@ -0,0 +1,84 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ ******************************************************************************/
+#ifndef __INC_DOT11D_H
+#define __INC_DOT11D_H
+
+#include "rtllib.h"
+
+struct chnl_txpow_triple {
+	u8 first_channel;
+	u8  num_channels;
+	u8  max_tx_power;
+};
+
+enum dot11d_state {
+	DOT11D_STATE_NONE = 0,
+	DOT11D_STATE_LEARNED,
+	DOT11D_STATE_DONE,
+};
+
+/**
+ * struct rt_dot11d_info * @country_len: value greater than 0 if
+ *		  @country_buffer contains valid country information element.
+ * @channel_map: holds channel values
+ *		0 - invalid,
+ *		1 - valid (active scan),
+ *		2 - valid (passive scan)
+ * @country_src_addr - Source AP of the country IE
+ */
+
+struct rt_dot11d_info {
+	bool enabled;
+
+	u16 country_len;
+	u8  country_buffer[MAX_IE_LEN];
+	u8  country_src_addr[6];
+	u8  country_watchdog;
+
+	u8  channel_map[MAX_CHANNEL_NUMBER + 1];
+	u8  max_tx_power_list[MAX_CHANNEL_NUMBER + 1];
+
+	enum dot11d_state state;
+};
+
+static inline void copy_mac_addr(unsigned char *des, unsigned char *src)
+{
+	memcpy(des, src, 6);
+}
+
+#define GET_DOT11D_INFO(__ieee_dev)			\
+	 ((struct rt_dot11d_info *)((__ieee_dev)->dot11d_info))
+
+#define IS_DOT11D_ENABLE(__ieee_dev)			\
+	 (GET_DOT11D_INFO(__ieee_dev)->enabled)
+#define IS_COUNTRY_IE_VALID(__ieee_dev)			\
+	(GET_DOT11D_INFO(__ieee_dev)->country_len > 0)
+
+#define IS_EQUAL_CIE_SRC(__ieee_dev, __address)		\
+	 ether_addr_equal_unaligned( \
+		GET_DOT11D_INFO(__ieee_dev)->country_src_addr, __address)
+#define UPDATE_CIE_SRC(__ieee_dev, __address)		\
+	copy_mac_addr(GET_DOT11D_INFO(__ieee_dev)->country_src_addr, __address)
+
+#define GET_CIE_WATCHDOG(__ieee_dev)				\
+	 (GET_DOT11D_INFO(__ieee_dev)->country_watchdog)
+static inline void RESET_CIE_WATCHDOG(struct rtllib_device *__ieee_dev)
+{
+	GET_CIE_WATCHDOG(__ieee_dev) = 0;
+}
+
+#define UPDATE_CIE_WATCHDOG(__ieee_dev) (++GET_CIE_WATCHDOG(__ieee_dev))
+
+void dot11d_init(struct rtllib_device *dev);
+void dot11d_channel_map(u8 channel_plan, struct rtllib_device *ieee);
+void dot11d_reset(struct rtllib_device *dev);
+void dot11d_update_country(struct rtllib_device *dev, u8 *address,
+			   u16 country_len, u8 *country);
+void dot11d_scan_complete(struct rtllib_device *dev);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/Kconfig b/drivers/staging/rtl8192e/rtl8192e/Kconfig
new file mode 100644
index 000000000..eae8167f7
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/Kconfig
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+config RTL8192E
+	tristate "RealTek RTL8192E Wireless LAN NIC driver"
+	depends on PCI && WLAN && RTLLIB
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select CRYPTO
+	select FW_LOADER
diff --git a/drivers/staging/rtl8192e/rtl8192e/Makefile b/drivers/staging/rtl8192e/rtl8192e/Makefile
new file mode 100644
index 000000000..75e6ec510
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/Makefile
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0
+r8192e_pci-objs :=		\
+	r8192E_dev.o		\
+	r8192E_phy.o		\
+	r8192E_firmware.o	\
+	r8192E_cmdpkt.o		\
+	r8192E_hwimg.o		\
+	r8190P_rtl8256.o	\
+	rtl_cam.o		\
+	rtl_core.o		\
+	rtl_dm.o		\
+	rtl_eeprom.o		\
+	rtl_ethtool.o		\
+	rtl_pci.o		\
+	rtl_pm.o		\
+	rtl_ps.o		\
+	rtl_wx.o		\
+
+obj-$(CONFIG_RTL8192E) += r8192e_pci.o
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
new file mode 100644
index 000000000..53fd79a28
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
@@ -0,0 +1,310 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef R8190P_DEF_H
+#define R8190P_DEF_H
+
+#include <linux/types.h>
+
+#define		MAX_SILENT_RESET_RX_SLOT_NUM	10
+
+#define RX_MPDU_QUEUE				0
+
+enum rtl819x_loopback {
+	RTL819X_NO_LOOPBACK = 0,
+	RTL819X_MAC_LOOPBACK = 1,
+	RTL819X_DMA_LOOPBACK = 2,
+	RTL819X_CCK_LOOPBACK = 3,
+};
+
+#define DESC90_RATE1M				0x00
+#define DESC90_RATE2M				0x01
+#define DESC90_RATE5_5M				0x02
+#define DESC90_RATE11M				0x03
+#define DESC90_RATE6M				0x04
+#define DESC90_RATE9M				0x05
+#define DESC90_RATE12M				0x06
+#define DESC90_RATE18M				0x07
+#define DESC90_RATE24M				0x08
+#define DESC90_RATE36M				0x09
+#define DESC90_RATE48M				0x0a
+#define DESC90_RATE54M				0x0b
+#define DESC90_RATEMCS0				0x00
+#define DESC90_RATEMCS1				0x01
+#define DESC90_RATEMCS2				0x02
+#define DESC90_RATEMCS3				0x03
+#define DESC90_RATEMCS4				0x04
+#define DESC90_RATEMCS5				0x05
+#define DESC90_RATEMCS6				0x06
+#define DESC90_RATEMCS7				0x07
+#define DESC90_RATEMCS8				0x08
+#define DESC90_RATEMCS9				0x09
+#define DESC90_RATEMCS10			0x0a
+#define DESC90_RATEMCS11			0x0b
+#define DESC90_RATEMCS12			0x0c
+#define DESC90_RATEMCS13			0x0d
+#define DESC90_RATEMCS14			0x0e
+#define DESC90_RATEMCS15			0x0f
+#define DESC90_RATEMCS32			0x20
+
+#define SHORT_SLOT_TIME				9
+#define NON_SHORT_SLOT_TIME		20
+
+#define	RX_SMOOTH				20
+
+#define QSLT_BK					0x1
+#define QSLT_BE					0x0
+#define QSLT_VI					0x4
+#define QSLT_VO					0x6
+#define	QSLT_BEACON			0x10
+#define	QSLT_HIGH				0x11
+#define	QSLT_MGNT				0x12
+#define	QSLT_CMD				0x13
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK		0x007
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE		0x0aa
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI		0x024
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO		0x007
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x10
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN		0x4
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB		0xd
+
+#define APPLIED_RESERVED_QUEUE_IN_FW		0x80000000
+#define RSVD_FW_QUEUE_PAGE_BK_SHIFT		0x00
+#define RSVD_FW_QUEUE_PAGE_BE_SHIFT		0x08
+#define RSVD_FW_QUEUE_PAGE_VI_SHIFT		0x10
+#define RSVD_FW_QUEUE_PAGE_VO_SHIFT		0x18
+#define RSVD_FW_QUEUE_PAGE_MGNT_SHIFT	0x10
+#define RSVD_FW_QUEUE_PAGE_BCN_SHIFT		0x00
+#define RSVD_FW_QUEUE_PAGE_PUB_SHIFT		0x08
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE	0
+#define HAL_PRIME_CHNL_OFFSET_LOWER		1
+#define HAL_PRIME_CHNL_OFFSET_UPPER		2
+
+
+enum version_8190_loopback {
+	VERSION_8190_BD = 0x3,
+	VERSION_8190_BE
+};
+
+#define IC_VersionCut_C	0x2
+#define IC_VersionCut_D	0x3
+#define IC_VersionCut_E	0x4
+
+enum rf_optype {
+	RF_OP_By_SW_3wire = 0,
+	RF_OP_By_FW,
+	RF_OP_MAX
+};
+
+struct bb_reg_definition {
+	u32 rfintfs;
+	u32 rfintfi;
+	u32 rfintfo;
+	u32 rfintfe;
+	u32 rf3wireOffset;
+	u32 rfLSSI_Select;
+	u32 rfTxGainStage;
+	u32 rfHSSIPara1;
+	u32 rfHSSIPara2;
+	u32 rfSwitchControl;
+	u32 rfAGCControl1;
+	u32 rfAGCControl2;
+	u32 rfRxIQImbalance;
+	u32 rfRxAFE;
+	u32 rfTxIQImbalance;
+	u32 rfTxAFE;
+	u32 rfLSSIReadBack;
+	u32 rfLSSIReadBackPi;
+};
+
+struct tx_fwinfo_8190pci {
+	u8			TxRate:7;
+	u8			CtsEnable:1;
+	u8			RtsRate:7;
+	u8			RtsEnable:1;
+	u8			TxHT:1;
+	u8			Short:1;
+	u8			TxBandwidth:1;
+	u8			TxSubCarrier:2;
+	u8			STBC:2;
+	u8			AllowAggregation:1;
+	u8			RtsHT:1;
+	u8			RtsShort:1;
+	u8			RtsBandwidth:1;
+	u8			RtsSubcarrier:2;
+	u8			RtsSTBC:2;
+	u8			EnableCPUDur:1;
+
+	u32			RxMF:2;
+	u32			RxAMD:3;
+	u32			TxPerPktInfoFeedback:1;
+	u32			Reserved1:2;
+	u32			TxAGCOffset:4;
+	u32			TxAGCSign:1;
+	u32			RAW_TXD:1;
+	u32			Retry_Limit:4;
+	u32			Reserved2:1;
+	u32			PacketID:13;
+
+
+};
+
+struct log_int_8190 {
+	u32	nIMR_COMDOK;
+	u32	nIMR_MGNTDOK;
+	u32	nIMR_HIGH;
+	u32	nIMR_VODOK;
+	u32	nIMR_VIDOK;
+	u32	nIMR_BEDOK;
+	u32	nIMR_BKDOK;
+	u32	nIMR_ROK;
+	u32	nIMR_RCOK;
+	u32	nIMR_TBDOK;
+	u32	nIMR_BDOK;
+	u32	nIMR_RXFOVW;
+};
+
+struct phy_ofdm_rx_status_rxsc_sgien_exintfflag {
+	u8			reserved:4;
+	u8			rxsc:2;
+	u8			sgi_en:1;
+	u8			ex_intf_flag:1;
+};
+
+struct phy_sts_ofdm_819xpci {
+	u8	trsw_gain_X[4];
+	u8	pwdb_all;
+	u8	cfosho_X[4];
+	u8	cfotail_X[4];
+	u8	rxevm_X[2];
+	u8	rxsnr_X[4];
+	u8	pdsnr_X[2];
+	u8	csi_current_X[2];
+	u8	csi_target_X[2];
+	u8	sigevm;
+	u8	max_ex_pwr;
+	u8	sgi_en;
+	u8	rxsc_sgien_exflg;
+};
+
+struct phy_sts_cck_819xpci {
+	u8	adc_pwdb_X[4];
+	u8	sq_rpt;
+	u8	cck_agc_rpt;
+};
+
+
+#define		PHY_RSSI_SLID_WIN_MAX				100
+#define		PHY_Beacon_RSSI_SLID_WIN_MAX		10
+
+struct tx_desc {
+	u16	PktSize;
+	u8	Offset;
+	u8	Reserved1:3;
+	u8	CmdInit:1;
+	u8	LastSeg:1;
+	u8	FirstSeg:1;
+	u8	LINIP:1;
+	u8	OWN:1;
+
+	u8	TxFWInfoSize;
+	u8	RATid:3;
+	u8	DISFB:1;
+	u8	USERATE:1;
+	u8	MOREFRAG:1;
+	u8	NoEnc:1;
+	u8	PIFS:1;
+	u8	QueueSelect:5;
+	u8	NoACM:1;
+	u8	Resv:2;
+	u8	SecCAMID:5;
+	u8	SecDescAssign:1;
+	u8	SecType:2;
+
+	u16	TxBufferSize;
+	u8	PktId:7;
+	u8	Resv1:1;
+	u8	Reserved2;
+
+	u32	TxBuffAddr;
+
+	u32	NextDescAddress;
+
+	u32	Reserved5;
+	u32	Reserved6;
+	u32	Reserved7;
+};
+
+
+struct tx_desc_cmd {
+	u16	PktSize;
+	u8	Reserved1;
+	u8	CmdType:3;
+	u8	CmdInit:1;
+	u8	LastSeg:1;
+	u8	FirstSeg:1;
+	u8	LINIP:1;
+	u8	OWN:1;
+
+	u16	ElementReport;
+	u16	Reserved2;
+
+	u16	TxBufferSize;
+	u16	Reserved3;
+
+	u32	TxBuffAddr;
+	u32	NextDescAddress;
+	u32	Reserved4;
+	u32	Reserved5;
+	u32	Reserved6;
+};
+
+struct rx_desc {
+	u16			Length:14;
+	u16			CRC32:1;
+	u16			ICV:1;
+	u8			RxDrvInfoSize;
+	u8			Shift:2;
+	u8			PHYStatus:1;
+	u8			SWDec:1;
+	u8			LastSeg:1;
+	u8			FirstSeg:1;
+	u8			EOR:1;
+	u8			OWN:1;
+
+	u32			Reserved2;
+
+	u32			Reserved3;
+
+	u32	BufferAddress;
+
+};
+
+
+struct rx_fwinfo {
+	u16			Reserved1:12;
+	u16			PartAggr:1;
+	u16			FirstAGGR:1;
+	u16			Reserved2:2;
+
+	u8			RxRate:7;
+	u8			RxHT:1;
+
+	u8			BW:1;
+	u8			SPLCP:1;
+	u8			Reserved3:2;
+	u8			PAM:1;
+	u8			Mcast:1;
+	u8			Bcast:1;
+	u8			Reserved4:1;
+
+	u32			TSFL;
+
+};
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
new file mode 100644
index 000000000..ab2e9b729
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
@@ -0,0 +1,208 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_phyreg.h"
+#include "r8192E_phy.h"
+#include "r8190P_rtl8256.h"
+
+void rtl92e_set_bandwidth(struct net_device *dev,
+			  enum ht_channel_width bandwidth)
+{
+	u8	eRFPath;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->card_8192_version != VERSION_8190_BD &&
+	    priv->card_8192_version != VERSION_8190_BE) {
+		netdev_warn(dev, "%s(): Unknown HW version.\n", __func__);
+		return;
+	}
+
+	for (eRFPath = 0; eRFPath < priv->NumTotalRFPath; eRFPath++) {
+		if (!rtl92e_is_legal_rf_path(dev, eRFPath))
+			continue;
+
+		switch (bandwidth) {
+		case HT_CHANNEL_WIDTH_20:
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x0b, bMask12Bits, 0x100);
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x2c, bMask12Bits, 0x3d7);
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x0e, bMask12Bits, 0x021);
+			break;
+		case HT_CHANNEL_WIDTH_20_40:
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x0b, bMask12Bits, 0x300);
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x2c, bMask12Bits, 0x3ff);
+			rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath,
+					  0x0e, bMask12Bits, 0x0e1);
+			break;
+		default:
+			netdev_err(dev, "%s(): Unknown bandwidth: %#X\n",
+				   __func__, bandwidth);
+			break;
+		}
+	}
+}
+
+bool rtl92e_config_rf(struct net_device *dev)
+{
+	u32	u4RegValue = 0;
+	u8	eRFPath;
+	bool rtStatus = true;
+	struct bb_reg_definition *pPhyReg;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32	RegOffSetToBeCheck = 0x3;
+	u32	RegValueToBeCheck = 0x7f1;
+	u32	RF3_Final_Value = 0;
+	u8	ConstRetryTimes = 5, RetryTimes = 5;
+	u8 ret = 0;
+
+	priv->NumTotalRFPath = RTL819X_TOTAL_RF_PATH;
+
+	for (eRFPath = (enum rf90_radio_path)RF90_PATH_A;
+	     eRFPath < priv->NumTotalRFPath; eRFPath++) {
+		if (!rtl92e_is_legal_rf_path(dev, eRFPath))
+			continue;
+
+		pPhyReg = &priv->PHYRegDef[eRFPath];
+
+		switch (eRFPath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4RegValue = rtl92e_get_bb_reg(dev, pPhyReg->rfintfs,
+						       bRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4RegValue = rtl92e_get_bb_reg(dev, pPhyReg->rfintfs,
+						       bRFSI_RFENV<<16);
+			break;
+		}
+
+		rtl92e_set_bb_reg(dev, pPhyReg->rfintfe, bRFSI_RFENV<<16, 0x1);
+
+		rtl92e_set_bb_reg(dev, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
+
+		rtl92e_set_bb_reg(dev, pPhyReg->rfHSSIPara2,
+				  b3WireAddressLength, 0x0);
+		rtl92e_set_bb_reg(dev, pPhyReg->rfHSSIPara2,
+				  b3WireDataLength, 0x0);
+
+		rtl92e_set_rf_reg(dev, (enum rf90_radio_path)eRFPath, 0x0,
+				  bMask12Bits, 0xbf);
+
+		rtStatus = rtl92e_check_bb_and_rf(dev, HW90_BLOCK_RF,
+						  (enum rf90_radio_path)eRFPath);
+		if (!rtStatus) {
+			netdev_err(dev, "%s(): Failed to check RF Path %d.\n",
+				   __func__, eRFPath);
+			goto fail;
+		}
+
+		RetryTimes = ConstRetryTimes;
+		RF3_Final_Value = 0;
+		while (RF3_Final_Value != RegValueToBeCheck &&
+		       RetryTimes != 0) {
+			ret = rtl92e_config_rf_path(dev,
+						(enum rf90_radio_path)eRFPath);
+			RF3_Final_Value = rtl92e_get_rf_reg(dev,
+						(enum rf90_radio_path)eRFPath,
+						RegOffSetToBeCheck,
+						bMask12Bits);
+			RetryTimes--;
+		}
+
+		switch (eRFPath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			rtl92e_set_bb_reg(dev, pPhyReg->rfintfs, bRFSI_RFENV,
+					  u4RegValue);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			rtl92e_set_bb_reg(dev, pPhyReg->rfintfs,
+					  bRFSI_RFENV<<16, u4RegValue);
+			break;
+		}
+
+		if (ret) {
+			netdev_err(dev,
+				   "%s(): Failed to initialize RF Path %d.\n",
+				   __func__, eRFPath);
+			goto fail;
+		}
+	}
+	return true;
+
+fail:
+	return false;
+}
+
+void rtl92e_set_cck_tx_power(struct net_device *dev, u8 powerlevel)
+{
+	u32	TxAGC = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	TxAGC = powerlevel;
+	if (priv->bDynamicTxLowPower) {
+		if (priv->CustomerID == RT_CID_819x_Netcore)
+			TxAGC = 0x22;
+		else
+			TxAGC += priv->CckPwEnl;
+	}
+	if (TxAGC > 0x24)
+		TxAGC = 0x24;
+	rtl92e_set_bb_reg(dev, rTxAGC_CCK_Mcs32, bTxAGCRateCCK, TxAGC);
+}
+
+void rtl92e_set_ofdm_tx_power(struct net_device *dev, u8 powerlevel)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 writeVal, powerBase0, powerBase1, writeVal_tmp;
+	u8 index = 0;
+	u16 RegOffset[6] = {0xe00, 0xe04, 0xe10, 0xe14, 0xe18, 0xe1c};
+	u8 byte0, byte1, byte2, byte3;
+
+	powerBase0 = powerlevel + priv->LegacyHTTxPowerDiff;
+	powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) |
+		     (powerBase0 << 8) | powerBase0;
+	powerBase1 = powerlevel;
+	powerBase1 = (powerBase1 << 24) | (powerBase1 << 16) |
+		     (powerBase1 << 8) | powerBase1;
+
+	for (index = 0; index < 6; index++) {
+		writeVal = (u32)(priv->MCSTxPowerLevelOriginalOffset[index] +
+			   ((index < 2) ? powerBase0 : powerBase1));
+		byte0 = writeVal & 0x7f;
+		byte1 = (writeVal & 0x7f00) >> 8;
+		byte2 = (writeVal & 0x7f0000) >> 16;
+		byte3 = (writeVal & 0x7f000000) >> 24;
+		if (byte0 > 0x24)
+			byte0 = 0x24;
+		if (byte1 > 0x24)
+			byte1 = 0x24;
+		if (byte2 > 0x24)
+			byte2 = 0x24;
+		if (byte3 > 0x24)
+			byte3 = 0x24;
+
+		if (index == 3) {
+			writeVal_tmp = (byte3 << 24) | (byte2 << 16) |
+				       (byte1 << 8) | byte0;
+			priv->Pwr_Track = writeVal_tmp;
+		}
+
+		if (priv->bDynamicTxHighPower)
+			writeVal = 0x03030303;
+		else
+			writeVal = (byte3 << 24) | (byte2 << 16) |
+				   (byte1 << 8) | byte0;
+		rtl92e_set_bb_reg(dev, RegOffset[index], 0x7f7f7f7f, writeVal);
+	}
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
new file mode 100644
index 000000000..3c52e2b43
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef RTL8225H
+#define RTL8225H
+
+#define RTL819X_TOTAL_RF_PATH 2
+void rtl92e_set_bandwidth(struct net_device *dev,
+			  enum ht_channel_width bandwidth);
+bool rtl92e_config_rf(struct net_device *dev);
+void rtl92e_set_cck_tx_power(struct net_device *dev, u8	powerlevel);
+void rtl92e_set_ofdm_tx_power(struct net_device *dev, u8 powerlevel);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
new file mode 100644
index 000000000..8bf06f736
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_hw.h"
+#include "r8192E_cmdpkt.h"
+
+bool rtl92e_send_cmd_pkt(struct net_device *dev, u32 type, const void *data,
+			 u32 len)
+{
+	bool				rt_status = true;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u16				frag_length = 0, frag_offset = 0;
+	struct sk_buff		*skb;
+	unsigned char		*seg_ptr;
+	struct cb_desc *tcb_desc;
+	u8				bLastIniPkt;
+
+	struct tx_fwinfo_8190pci *pTxFwInfo = NULL;
+
+	do {
+		if ((len - frag_offset) > CMDPACKET_FRAG_SIZE) {
+			frag_length = CMDPACKET_FRAG_SIZE;
+			bLastIniPkt = 0;
+
+		} else {
+			frag_length = (u16)(len - frag_offset);
+			bLastIniPkt = 1;
+		}
+
+		if (type == DESC_PACKET_TYPE_NORMAL)
+			skb = dev_alloc_skb(frag_length +
+					    priv->rtllib->tx_headroom + 4);
+		else
+			skb = dev_alloc_skb(frag_length + 4);
+
+		if (!skb) {
+			rt_status = false;
+			goto Failed;
+		}
+
+		memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+		tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->queue_index = TXCMD_QUEUE;
+		tcb_desc->bCmdOrInit = type;
+		tcb_desc->bLastIniPkt = bLastIniPkt;
+
+		if (type == DESC_PACKET_TYPE_NORMAL) {
+			tcb_desc->pkt_size = frag_length;
+
+			seg_ptr = skb_put(skb, priv->rtllib->tx_headroom);
+			pTxFwInfo = (struct tx_fwinfo_8190pci *)seg_ptr;
+			memset(pTxFwInfo, 0, sizeof(struct tx_fwinfo_8190pci));
+			memset(pTxFwInfo, 0x12, 8);
+		} else {
+			tcb_desc->txbuf_size = frag_length;
+		}
+
+		skb_put_data(skb, data, frag_length);
+
+		if (type == DESC_PACKET_TYPE_INIT &&
+		    (!priv->rtllib->check_nic_enough_desc(dev, TXCMD_QUEUE) ||
+		     (!skb_queue_empty(&priv->rtllib->skb_waitQ[TXCMD_QUEUE])) ||
+		     (priv->rtllib->queue_stop))) {
+			skb_queue_tail(&priv->rtllib->skb_waitQ[TXCMD_QUEUE],
+				       skb);
+		} else {
+			priv->rtllib->softmac_hard_start_xmit(skb, dev);
+		}
+
+		data += frag_length;
+		frag_offset += frag_length;
+
+	} while (frag_offset < len);
+
+	rtl92e_writeb(dev, TPPoll, TPPoll_CQ);
+Failed:
+	return rt_status;
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
new file mode 100644
index 000000000..c63909199
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef R819XUSB_CMDPKT_H
+#define R819XUSB_CMDPKT_H
+
+bool rtl92e_send_cmd_pkt(struct net_device *dev, u32 type, const void *data,
+			 u32 len);
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
new file mode 100644
index 000000000..18e4e5d84
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -0,0 +1,2196 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_phy.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h"
+#include "r8192E_cmdpkt.h"
+#include "rtl_dm.h"
+#include "rtl_wx.h"
+
+static int WDCAPARA_ADD[] = {EDCAPARA_BE, EDCAPARA_BK, EDCAPARA_VI,
+			     EDCAPARA_VO};
+
+void rtl92e_start_beacon(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	struct rtllib_network *net = &priv->rtllib->current_network;
+	u16 BcnTimeCfg = 0;
+	u16 BcnCW = 6;
+	u16 BcnIFS = 0xf;
+
+	rtl92e_irq_disable(dev);
+
+	rtl92e_writew(dev, ATIMWND, 2);
+
+	rtl92e_writew(dev, BCN_INTERVAL, net->beacon_interval);
+	rtl92e_writew(dev, BCN_DRV_EARLY_INT, 10);
+	rtl92e_writew(dev, BCN_DMATIME, 256);
+
+	rtl92e_writeb(dev, BCN_ERR_THRESH, 100);
+
+	BcnTimeCfg |= BcnCW<<BCN_TCFG_CW_SHIFT;
+	BcnTimeCfg |= BcnIFS<<BCN_TCFG_IFS;
+	rtl92e_writew(dev, BCN_TCFG, BcnTimeCfg);
+	rtl92e_irq_enable(dev);
+}
+
+static void _rtl92e_update_msr(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 msr;
+	enum led_ctl_mode LedAction = LED_CTL_NO_LINK;
+
+	msr  = rtl92e_readb(dev, MSR);
+	msr &= ~MSR_LINK_MASK;
+
+	switch (priv->rtllib->iw_mode) {
+	case IW_MODE_INFRA:
+		if (priv->rtllib->state == RTLLIB_LINKED)
+			msr |= (MSR_LINK_MANAGED << MSR_LINK_SHIFT);
+		else
+			msr |= (MSR_LINK_NONE << MSR_LINK_SHIFT);
+		LedAction = LED_CTL_LINK;
+		break;
+	case IW_MODE_ADHOC:
+		if (priv->rtllib->state == RTLLIB_LINKED)
+			msr |= (MSR_LINK_ADHOC << MSR_LINK_SHIFT);
+		else
+			msr |= (MSR_LINK_NONE << MSR_LINK_SHIFT);
+		break;
+	case IW_MODE_MASTER:
+		if (priv->rtllib->state == RTLLIB_LINKED)
+			msr |= (MSR_LINK_MASTER << MSR_LINK_SHIFT);
+		else
+			msr |= (MSR_LINK_NONE << MSR_LINK_SHIFT);
+		break;
+	default:
+		break;
+	}
+
+	rtl92e_writeb(dev, MSR, msr);
+	if (priv->rtllib->LedControlHandler)
+		priv->rtllib->LedControlHandler(dev, LedAction);
+}
+
+void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	switch (variable) {
+	case HW_VAR_BSSID:
+		/* BSSIDR 2 byte alignment */
+		rtl92e_writew(dev, BSSIDR, *(u16 *)val);
+		rtl92e_writel(dev, BSSIDR + 2, *(u32 *)(val + 2));
+		break;
+
+	case HW_VAR_MEDIA_STATUS:
+	{
+		enum rt_op_mode OpMode = *((enum rt_op_mode *)(val));
+		u8 btMsr = rtl92e_readb(dev, MSR);
+
+		btMsr &= 0xfc;
+
+		switch (OpMode) {
+		case RT_OP_MODE_INFRASTRUCTURE:
+			btMsr |= MSR_INFRA;
+			break;
+
+		case RT_OP_MODE_IBSS:
+			btMsr |= MSR_ADHOC;
+			break;
+
+		case RT_OP_MODE_AP:
+			btMsr |= MSR_AP;
+			break;
+
+		default:
+			btMsr |= MSR_NOLINK;
+			break;
+		}
+
+		rtl92e_writeb(dev, MSR, btMsr);
+
+	}
+	break;
+
+	case HW_VAR_CECHK_BSSID:
+	{
+		u32	RegRCR, Type;
+
+		Type = val[0];
+		RegRCR = rtl92e_readl(dev, RCR);
+		priv->ReceiveConfig = RegRCR;
+
+		if (Type)
+			RegRCR |= (RCR_CBSSID);
+		else
+			RegRCR &= (~RCR_CBSSID);
+
+		rtl92e_writel(dev, RCR, RegRCR);
+		priv->ReceiveConfig = RegRCR;
+
+	}
+	break;
+
+	case HW_VAR_SLOT_TIME:
+
+		priv->slot_time = val[0];
+		rtl92e_writeb(dev, SLOT_TIME, val[0]);
+
+		break;
+
+	case HW_VAR_ACK_PREAMBLE:
+	{
+		u32 regTmp;
+
+		priv->short_preamble = (bool)*val;
+		regTmp = priv->basic_rate;
+		if (priv->short_preamble)
+			regTmp |= BRSR_AckShortPmb;
+		rtl92e_writel(dev, RRSR, regTmp);
+		break;
+	}
+
+	case HW_VAR_CPU_RST:
+		rtl92e_writel(dev, CPU_GEN, ((u32 *)(val))[0]);
+		break;
+
+	case HW_VAR_AC_PARAM:
+	{
+		u8	pAcParam = *val;
+		u32	eACI = pAcParam;
+		u8		u1bAIFS;
+		u32		u4bAcParam;
+		u8 mode = priv->rtllib->mode;
+		struct rtllib_qos_parameters *qop =
+			 &priv->rtllib->current_network.qos_data.parameters;
+
+		u1bAIFS = qop->aifs[pAcParam] *
+			  ((mode&(IEEE_G|IEEE_N_24G)) ? 9 : 20) + aSifsTime;
+
+		rtl92e_dm_init_edca_turbo(dev);
+
+		u4bAcParam = (le16_to_cpu(qop->tx_op_limit[pAcParam]) <<
+			      AC_PARAM_TXOP_LIMIT_OFFSET) |
+				((le16_to_cpu(qop->cw_max[pAcParam])) <<
+				 AC_PARAM_ECW_MAX_OFFSET) |
+				((le16_to_cpu(qop->cw_min[pAcParam])) <<
+				 AC_PARAM_ECW_MIN_OFFSET) |
+				(((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET);
+
+		switch (eACI) {
+		case AC1_BK:
+			rtl92e_writel(dev, EDCAPARA_BK, u4bAcParam);
+			break;
+
+		case AC0_BE:
+			rtl92e_writel(dev, EDCAPARA_BE, u4bAcParam);
+			break;
+
+		case AC2_VI:
+			rtl92e_writel(dev, EDCAPARA_VI, u4bAcParam);
+			break;
+
+		case AC3_VO:
+			rtl92e_writel(dev, EDCAPARA_VO, u4bAcParam);
+			break;
+
+		default:
+			netdev_info(dev, "SetHwReg8185(): invalid ACI: %d !\n",
+				    eACI);
+			break;
+		}
+		priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACM_CTRL,
+					      &pAcParam);
+		break;
+	}
+
+	case HW_VAR_ACM_CTRL:
+	{
+		struct rtllib_qos_parameters *qos_parameters =
+			 &priv->rtllib->current_network.qos_data.parameters;
+		u8 pAcParam = *val;
+		u32 eACI = pAcParam;
+		union aci_aifsn *pAciAifsn = (union aci_aifsn *)&
+					      (qos_parameters->aifs[0]);
+		u8 acm = pAciAifsn->f.acm;
+		u8 AcmCtrl = rtl92e_readb(dev, AcmHwCtrl);
+
+		AcmCtrl = AcmCtrl | ((priv->AcmMethod == 2) ? 0x0 : 0x1);
+
+		if (acm) {
+			switch (eACI) {
+			case AC0_BE:
+				AcmCtrl |= AcmHw_BeqEn;
+				break;
+
+			case AC2_VI:
+				AcmCtrl |= AcmHw_ViqEn;
+				break;
+
+			case AC3_VO:
+				AcmCtrl |= AcmHw_VoqEn;
+				break;
+			}
+		} else {
+			switch (eACI) {
+			case AC0_BE:
+				AcmCtrl &= (~AcmHw_BeqEn);
+				break;
+
+			case AC2_VI:
+				AcmCtrl &= (~AcmHw_ViqEn);
+				break;
+
+			case AC3_VO:
+				AcmCtrl &= (~AcmHw_BeqEn);
+				break;
+
+			default:
+				break;
+			}
+		}
+		rtl92e_writeb(dev, AcmHwCtrl, AcmCtrl);
+		break;
+	}
+
+	case HW_VAR_SIFS:
+		rtl92e_writeb(dev, SIFS, val[0]);
+		rtl92e_writeb(dev, SIFS+1, val[0]);
+		break;
+
+	case HW_VAR_RF_TIMING:
+	{
+		u8 Rf_Timing = *val;
+
+		rtl92e_writeb(dev, rFPGA0_RFTiming1, Rf_Timing);
+		break;
+	}
+
+	default:
+		break;
+	}
+
+}
+
+static void _rtl92e_read_eeprom_info(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	const u8 bMac_Tmp_Addr[ETH_ALEN] = {0x00, 0xe0, 0x4c, 0x00, 0x00, 0x01};
+	u8 tempval;
+	u8 ICVer8192, ICVer8256;
+	u16 i, usValue, IC_Version;
+	u16 EEPROMId;
+
+	EEPROMId = rtl92e_eeprom_read(dev, 0);
+	if (EEPROMId != RTL8190_EEPROM_ID) {
+		netdev_err(dev, "%s(): Invalid EEPROM ID: %x\n", __func__,
+			   EEPROMId);
+		priv->AutoloadFailFlag = true;
+	} else {
+		priv->AutoloadFailFlag = false;
+	}
+
+	if (!priv->AutoloadFailFlag) {
+		priv->eeprom_vid = rtl92e_eeprom_read(dev, EEPROM_VID >> 1);
+		priv->eeprom_did = rtl92e_eeprom_read(dev, EEPROM_DID >> 1);
+
+		usValue = rtl92e_eeprom_read(dev,
+					     (EEPROM_Customer_ID >> 1)) >> 8;
+		priv->eeprom_CustomerID = usValue & 0xff;
+		usValue = rtl92e_eeprom_read(dev,
+					     EEPROM_ICVersion_ChannelPlan>>1);
+		priv->eeprom_ChannelPlan = usValue&0xff;
+		IC_Version = (usValue & 0xff00)>>8;
+
+		ICVer8192 = IC_Version & 0xf;
+		ICVer8256 = (IC_Version & 0xf0)>>4;
+		if (ICVer8192 == 0x2) {
+			if (ICVer8256 == 0x5)
+				priv->card_8192_version = VERSION_8190_BE;
+		}
+		switch (priv->card_8192_version) {
+		case VERSION_8190_BD:
+		case VERSION_8190_BE:
+			break;
+		default:
+			priv->card_8192_version = VERSION_8190_BD;
+			break;
+		}
+	} else {
+		priv->card_8192_version = VERSION_8190_BD;
+		priv->eeprom_vid = 0;
+		priv->eeprom_did = 0;
+		priv->eeprom_CustomerID = 0;
+		priv->eeprom_ChannelPlan = 0;
+	}
+
+	if (!priv->AutoloadFailFlag) {
+		u8 addr[ETH_ALEN];
+
+		for (i = 0; i < 6; i += 2) {
+			usValue = rtl92e_eeprom_read(dev,
+				 (EEPROM_NODE_ADDRESS_BYTE_0 + i) >> 1);
+			*(u16 *)(&addr[i]) = usValue;
+		}
+		eth_hw_addr_set(dev, addr);
+	} else {
+		eth_hw_addr_set(dev, bMac_Tmp_Addr);
+	}
+
+	if (priv->card_8192_version > VERSION_8190_BD)
+		priv->bTXPowerDataReadFromEEPORM = true;
+	else
+		priv->bTXPowerDataReadFromEEPORM = false;
+
+	priv->rf_type = RTL819X_DEFAULT_RF_TYPE;
+
+	if (priv->card_8192_version > VERSION_8190_BD) {
+		if (!priv->AutoloadFailFlag) {
+			tempval = (rtl92e_eeprom_read(dev,
+						      (EEPROM_RFInd_PowerDiff >> 1))) & 0xff;
+			priv->EEPROMLegacyHTTxPowerDiff = tempval & 0xf;
+
+			if (tempval&0x80)
+				priv->rf_type = RF_1T2R;
+			else
+				priv->rf_type = RF_2T4R;
+		} else {
+			priv->EEPROMLegacyHTTxPowerDiff = 0x04;
+		}
+
+		if (!priv->AutoloadFailFlag)
+			priv->EEPROMThermalMeter = ((rtl92e_eeprom_read(dev,
+						   (EEPROM_ThermalMeter>>1))) &
+						   0xff00) >> 8;
+		else
+			priv->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
+		priv->TSSI_13dBm = priv->EEPROMThermalMeter * 100;
+
+		if (priv->epromtype == EEPROM_93C46) {
+			if (!priv->AutoloadFailFlag) {
+				usValue = rtl92e_eeprom_read(dev,
+					  EEPROM_TxPwDiff_CrystalCap >> 1);
+				priv->EEPROMAntPwDiff = usValue & 0x0fff;
+				priv->EEPROMCrystalCap = (usValue & 0xf000)
+							 >> 12;
+			} else {
+				priv->EEPROMAntPwDiff =
+					 EEPROM_Default_AntTxPowerDiff;
+				priv->EEPROMCrystalCap =
+					 EEPROM_Default_TxPwDiff_CrystalCap;
+			}
+
+			for (i = 0; i < 14; i += 2) {
+				if (!priv->AutoloadFailFlag)
+					usValue = rtl92e_eeprom_read(dev,
+						  (EEPROM_TxPwIndex_CCK + i) >> 1);
+				else
+					usValue = EEPROM_Default_TxPower;
+				*((u16 *)(&priv->EEPROMTxPowerLevelCCK[i])) =
+								 usValue;
+			}
+			for (i = 0; i < 14; i += 2) {
+				if (!priv->AutoloadFailFlag)
+					usValue = rtl92e_eeprom_read(dev,
+						(EEPROM_TxPwIndex_OFDM_24G + i) >> 1);
+				else
+					usValue = EEPROM_Default_TxPower;
+				*((u16 *)(&priv->EEPROMTxPowerLevelOFDM24G[i]))
+							 = usValue;
+			}
+		}
+		if (priv->epromtype == EEPROM_93C46) {
+			for (i = 0; i < 14; i++) {
+				priv->TxPowerLevelCCK[i] =
+					 priv->EEPROMTxPowerLevelCCK[i];
+				priv->TxPowerLevelOFDM24G[i] =
+					 priv->EEPROMTxPowerLevelOFDM24G[i];
+			}
+			priv->LegacyHTTxPowerDiff =
+					 priv->EEPROMLegacyHTTxPowerDiff;
+			priv->AntennaTxPwDiff[0] = priv->EEPROMAntPwDiff & 0xf;
+			priv->AntennaTxPwDiff[1] = (priv->EEPROMAntPwDiff &
+							0xf0) >> 4;
+			priv->AntennaTxPwDiff[2] = (priv->EEPROMAntPwDiff &
+							0xf00) >> 8;
+			priv->CrystalCap = priv->EEPROMCrystalCap;
+			priv->ThermalMeter[0] = priv->EEPROMThermalMeter & 0xf;
+			priv->ThermalMeter[1] = (priv->EEPROMThermalMeter &
+						     0xf0) >> 4;
+		} else if (priv->epromtype == EEPROM_93C56) {
+
+			for (i = 0; i < 3; i++) {
+				priv->TxPowerLevelCCK_A[i] =
+					 priv->EEPROMRfACCKChnl1TxPwLevel[0];
+				priv->TxPowerLevelOFDM24G_A[i] =
+					 priv->EEPROMRfAOfdmChnlTxPwLevel[0];
+				priv->TxPowerLevelCCK_C[i] =
+					 priv->EEPROMRfCCCKChnl1TxPwLevel[0];
+				priv->TxPowerLevelOFDM24G_C[i] =
+					 priv->EEPROMRfCOfdmChnlTxPwLevel[0];
+			}
+			for (i = 3; i < 9; i++) {
+				priv->TxPowerLevelCCK_A[i]  =
+					 priv->EEPROMRfACCKChnl1TxPwLevel[1];
+				priv->TxPowerLevelOFDM24G_A[i] =
+					 priv->EEPROMRfAOfdmChnlTxPwLevel[1];
+				priv->TxPowerLevelCCK_C[i] =
+					 priv->EEPROMRfCCCKChnl1TxPwLevel[1];
+				priv->TxPowerLevelOFDM24G_C[i] =
+					 priv->EEPROMRfCOfdmChnlTxPwLevel[1];
+			}
+			for (i = 9; i < 14; i++) {
+				priv->TxPowerLevelCCK_A[i]  =
+					 priv->EEPROMRfACCKChnl1TxPwLevel[2];
+				priv->TxPowerLevelOFDM24G_A[i] =
+					 priv->EEPROMRfAOfdmChnlTxPwLevel[2];
+				priv->TxPowerLevelCCK_C[i] =
+					 priv->EEPROMRfCCCKChnl1TxPwLevel[2];
+				priv->TxPowerLevelOFDM24G_C[i] =
+					 priv->EEPROMRfCOfdmChnlTxPwLevel[2];
+			}
+			priv->LegacyHTTxPowerDiff =
+				 priv->EEPROMLegacyHTTxPowerDiff;
+			priv->AntennaTxPwDiff[0] = 0;
+			priv->AntennaTxPwDiff[1] = 0;
+			priv->AntennaTxPwDiff[2] = 0;
+			priv->CrystalCap = priv->EEPROMCrystalCap;
+			priv->ThermalMeter[0] = priv->EEPROMThermalMeter & 0xf;
+			priv->ThermalMeter[1] = (priv->EEPROMThermalMeter &
+						     0xf0) >> 4;
+		}
+	}
+
+	rtl92e_init_adaptive_rate(dev);
+
+	priv->rf_chip = RF_8256;
+
+	if (priv->RegChannelPlan == 0xf)
+		priv->ChannelPlan = priv->eeprom_ChannelPlan;
+	else
+		priv->ChannelPlan = priv->RegChannelPlan;
+
+	if (priv->eeprom_vid == 0x1186 &&  priv->eeprom_did == 0x3304)
+		priv->CustomerID =  RT_CID_DLINK;
+
+	switch (priv->eeprom_CustomerID) {
+	case EEPROM_CID_DEFAULT:
+		priv->CustomerID = RT_CID_DEFAULT;
+		break;
+	case EEPROM_CID_CAMEO:
+		priv->CustomerID = RT_CID_819x_CAMEO;
+		break;
+	case  EEPROM_CID_RUNTOP:
+		priv->CustomerID = RT_CID_819x_RUNTOP;
+		break;
+	case EEPROM_CID_NetCore:
+		priv->CustomerID = RT_CID_819x_Netcore;
+		break;
+	case EEPROM_CID_TOSHIBA:
+		priv->CustomerID = RT_CID_TOSHIBA;
+		if (priv->eeprom_ChannelPlan&0x80)
+			priv->ChannelPlan = priv->eeprom_ChannelPlan&0x7f;
+		else
+			priv->ChannelPlan = 0x0;
+		break;
+	case EEPROM_CID_Nettronix:
+		priv->ScanDelay = 100;
+		priv->CustomerID = RT_CID_Nettronix;
+		break;
+	case EEPROM_CID_Pronet:
+		priv->CustomerID = RT_CID_PRONET;
+		break;
+	case EEPROM_CID_DLINK:
+		priv->CustomerID = RT_CID_DLINK;
+		break;
+
+	case EEPROM_CID_WHQL:
+		break;
+	default:
+		break;
+	}
+
+	if (priv->ChannelPlan > CHANNEL_PLAN_LEN - 1)
+		priv->ChannelPlan = 0;
+	priv->ChannelPlan = COUNTRY_CODE_WORLD_WIDE_13;
+
+	if (priv->eeprom_vid == 0x1186 &&  priv->eeprom_did == 0x3304)
+		priv->rtllib->bSupportRemoteWakeUp = true;
+	else
+		priv->rtllib->bSupportRemoteWakeUp = false;
+}
+
+void rtl92e_get_eeprom_size(struct net_device *dev)
+{
+	u16 curCR;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	curCR = rtl92e_readw(dev, EPROM_CMD);
+	priv->epromtype = (curCR & EPROM_CMD_9356SEL) ? EEPROM_93C56 :
+			  EEPROM_93C46;
+	_rtl92e_read_eeprom_info(dev);
+}
+
+static void _rtl92e_hwconfig(struct net_device *dev)
+{
+	u32 regRATR = 0, regRRSR = 0;
+	u8 regBwOpMode = 0, regTmp = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	switch (priv->rtllib->mode) {
+	case WIRELESS_MODE_B:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK;
+		regRRSR = RATE_ALL_CCK;
+		break;
+	case WIRELESS_MODE_A:
+		regBwOpMode = BW_OPMODE_5G | BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_OFDM_AG;
+		regRRSR = RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_G:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_AUTO:
+	case WIRELESS_MODE_N_24G:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+			  RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_N_5G:
+		regBwOpMode = BW_OPMODE_5G;
+		regRATR = RATE_ALL_OFDM_AG | RATE_ALL_OFDM_1SS |
+			  RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_OFDM_AG;
+		break;
+	default:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	}
+
+	rtl92e_writeb(dev, BW_OPMODE, regBwOpMode);
+	{
+		u32 ratr_value;
+
+		ratr_value = regRATR;
+		if (priv->rf_type == RF_1T2R)
+			ratr_value &= ~(RATE_ALL_OFDM_2SS);
+		rtl92e_writel(dev, RATR0, ratr_value);
+		rtl92e_writeb(dev, UFWP, 1);
+	}
+	regTmp = rtl92e_readb(dev, 0x313);
+	regRRSR = ((regTmp) << 24) | (regRRSR & 0x00ffffff);
+	rtl92e_writel(dev, RRSR, regRRSR);
+
+	rtl92e_writew(dev, RETRY_LIMIT,
+		      priv->ShortRetryLimit << RETRY_LIMIT_SHORT_SHIFT |
+		      priv->LongRetryLimit << RETRY_LIMIT_LONG_SHIFT);
+}
+
+bool rtl92e_start_adapter(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 ulRegRead;
+	bool rtStatus = true;
+	u8 tmpvalue;
+	u8 ICVersion, SwitchingRegulatorOutput;
+	bool bfirmwareok = true;
+	u32 tmpRegA, TempCCk;
+	int i = 0;
+	u32 retry_times = 0;
+
+	priv->being_init_adapter = true;
+
+start:
+	rtl92e_reset_desc_ring(dev);
+	priv->Rf_Mode = RF_OP_By_SW_3wire;
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		rtl92e_writeb(dev, ANAPAR, 0x37);
+		mdelay(500);
+	}
+	priv->pFirmware->status = FW_STATUS_0_INIT;
+
+	if (priv->RegRfOff)
+		priv->rtllib->rf_power_state = rf_off;
+
+	ulRegRead = rtl92e_readl(dev, CPU_GEN);
+	if (priv->pFirmware->status == FW_STATUS_0_INIT)
+		ulRegRead |= CPU_GEN_SYSTEM_RESET;
+	else if (priv->pFirmware->status == FW_STATUS_5_READY)
+		ulRegRead |= CPU_GEN_FIRMWARE_RESET;
+	else
+		netdev_err(dev, "%s(): undefined firmware state: %d.\n",
+			   __func__, priv->pFirmware->status);
+
+	rtl92e_writel(dev, CPU_GEN, ulRegRead);
+
+	ICVersion = rtl92e_readb(dev, IC_VERRSION);
+	if (ICVersion >= 0x4) {
+		SwitchingRegulatorOutput = rtl92e_readb(dev, SWREGULATOR);
+		if (SwitchingRegulatorOutput  != 0xb8) {
+			rtl92e_writeb(dev, SWREGULATOR, 0xa8);
+			mdelay(1);
+			rtl92e_writeb(dev, SWREGULATOR, 0xb8);
+		}
+	}
+	rtStatus = rtl92e_config_bb(dev);
+	if (!rtStatus) {
+		netdev_warn(dev, "%s(): Failed to configure BB\n", __func__);
+		return rtStatus;
+	}
+
+	priv->LoopbackMode = RTL819X_NO_LOOPBACK;
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		ulRegRead = rtl92e_readl(dev, CPU_GEN);
+		if (priv->LoopbackMode == RTL819X_NO_LOOPBACK)
+			ulRegRead = (ulRegRead & CPU_GEN_NO_LOOPBACK_MSK) |
+				    CPU_GEN_NO_LOOPBACK_SET;
+		else if (priv->LoopbackMode == RTL819X_MAC_LOOPBACK)
+			ulRegRead |= CPU_CCK_LOOPBACK;
+		else
+			netdev_err(dev, "%s: Invalid loopback mode setting.\n",
+				   __func__);
+
+		rtl92e_writel(dev, CPU_GEN, ulRegRead);
+
+		udelay(500);
+	}
+	_rtl92e_hwconfig(dev);
+	rtl92e_writeb(dev, CMDR, CR_RE | CR_TE);
+
+	rtl92e_writeb(dev, PCIF, ((MXDMA2_NoLimit<<MXDMA2_RX_SHIFT) |
+				  (MXDMA2_NoLimit<<MXDMA2_TX_SHIFT)));
+	rtl92e_writel(dev, MAC0, ((u32 *)dev->dev_addr)[0]);
+	rtl92e_writew(dev, MAC4, ((u16 *)(dev->dev_addr + 4))[0]);
+	rtl92e_writel(dev, RCR, priv->ReceiveConfig);
+
+	rtl92e_writel(dev, RQPN1, NUM_OF_PAGE_IN_FW_QUEUE_BK <<
+		      RSVD_FW_QUEUE_PAGE_BK_SHIFT |
+		      NUM_OF_PAGE_IN_FW_QUEUE_BE <<
+		      RSVD_FW_QUEUE_PAGE_BE_SHIFT |
+		      NUM_OF_PAGE_IN_FW_QUEUE_VI <<
+		      RSVD_FW_QUEUE_PAGE_VI_SHIFT |
+		      NUM_OF_PAGE_IN_FW_QUEUE_VO <<
+		      RSVD_FW_QUEUE_PAGE_VO_SHIFT);
+	rtl92e_writel(dev, RQPN2, NUM_OF_PAGE_IN_FW_QUEUE_MGNT <<
+		      RSVD_FW_QUEUE_PAGE_MGNT_SHIFT);
+	rtl92e_writel(dev, RQPN3, APPLIED_RESERVED_QUEUE_IN_FW |
+		      NUM_OF_PAGE_IN_FW_QUEUE_BCN <<
+		      RSVD_FW_QUEUE_PAGE_BCN_SHIFT|
+		      NUM_OF_PAGE_IN_FW_QUEUE_PUB <<
+		      RSVD_FW_QUEUE_PAGE_PUB_SHIFT);
+
+	rtl92e_tx_enable(dev);
+	rtl92e_rx_enable(dev);
+	ulRegRead = (0xFFF00000 & rtl92e_readl(dev, RRSR))  |
+		     RATE_ALL_OFDM_AG | RATE_ALL_CCK;
+	rtl92e_writel(dev, RRSR, ulRegRead);
+	rtl92e_writel(dev, RATR0+4*7, (RATE_ALL_OFDM_AG | RATE_ALL_CCK));
+
+	rtl92e_writeb(dev, ACK_TIMEOUT, 0x30);
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET)
+		rtl92e_set_wireless_mode(dev, priv->rtllib->mode);
+	rtl92e_cam_reset(dev);
+	{
+		u8 SECR_value = 0x0;
+
+		SECR_value |= SCR_TxEncEnable;
+		SECR_value |= SCR_RxDecEnable;
+		SECR_value |= SCR_NoSKMC;
+		rtl92e_writeb(dev, SECR, SECR_value);
+	}
+	rtl92e_writew(dev, ATIMWND, 2);
+	rtl92e_writew(dev, BCN_INTERVAL, 100);
+
+	for (i = 0; i < QOS_QUEUE_NUM; i++)
+		rtl92e_writel(dev, WDCAPARA_ADD[i], 0x005e4332);
+
+	rtl92e_writeb(dev, 0xbe, 0xc0);
+
+	rtl92e_config_mac(dev);
+
+	if (priv->card_8192_version > VERSION_8190_BD) {
+		rtl92e_get_tx_power(dev);
+		rtl92e_set_tx_power(dev, priv->chan);
+	}
+
+	tmpvalue = rtl92e_readb(dev, IC_VERRSION);
+	priv->IC_Cut = tmpvalue;
+
+	bfirmwareok = rtl92e_init_fw(dev);
+	if (!bfirmwareok) {
+		if (retry_times < 10) {
+			retry_times++;
+			goto start;
+		} else {
+			rtStatus = false;
+			goto end;
+		}
+	}
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		rtStatus = rtl92e_config_phy(dev);
+		if (!rtStatus) {
+			netdev_info(dev, "RF Config failed\n");
+			return rtStatus;
+		}
+	}
+
+	rtl92e_set_bb_reg(dev, rFPGA0_RFMOD, bCCKEn, 0x1);
+	rtl92e_set_bb_reg(dev, rFPGA0_RFMOD, bOFDMEn, 0x1);
+
+	rtl92e_writeb(dev, 0x87, 0x0);
+
+	if (priv->RegRfOff) {
+		rtl92e_set_rf_state(dev, rf_off, RF_CHANGE_BY_SW);
+	} else if (priv->rtllib->rf_off_reason > RF_CHANGE_BY_PS) {
+		rtl92e_set_rf_state(dev, rf_off, priv->rtllib->rf_off_reason);
+	} else if (priv->rtllib->rf_off_reason >= RF_CHANGE_BY_IPS) {
+		rtl92e_set_rf_state(dev, rf_off, priv->rtllib->rf_off_reason);
+	} else {
+		priv->rtllib->rf_power_state = rf_on;
+		priv->rtllib->rf_off_reason = 0;
+	}
+
+	if (priv->rtllib->FwRWRF)
+		priv->Rf_Mode = RF_OP_By_FW;
+	else
+		priv->Rf_Mode = RF_OP_By_SW_3wire;
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		rtl92e_dm_init_txpower_tracking(dev);
+
+		if (priv->IC_Cut >= IC_VersionCut_D) {
+			tmpRegA = rtl92e_get_bb_reg(dev, rOFDM0_XATxIQImbalance,
+						    bMaskDWord);
+			rtl92e_get_bb_reg(dev, rOFDM0_XCTxIQImbalance, bMaskDWord);
+
+			for (i = 0; i < TxBBGainTableLength; i++) {
+				if (tmpRegA == dm_tx_bb_gain[i]) {
+					priv->rfa_txpowertrackingindex = i;
+					priv->rfa_txpowertrackingindex_real = i;
+					priv->rfa_txpowertracking_default =
+						 priv->rfa_txpowertrackingindex;
+					break;
+				}
+			}
+
+			TempCCk = rtl92e_get_bb_reg(dev, rCCK0_TxFilter1,
+						    bMaskByte2);
+
+			for (i = 0; i < CCKTxBBGainTableLength; i++) {
+				if (TempCCk == dm_cck_tx_bb_gain[i][0]) {
+					priv->CCKPresentAttentuation_20Mdefault = i;
+					break;
+				}
+			}
+			priv->CCKPresentAttentuation_40Mdefault = 0;
+			priv->CCKPresentAttentuation_difference = 0;
+			priv->CCKPresentAttentuation =
+				  priv->CCKPresentAttentuation_20Mdefault;
+			priv->btxpower_tracking = false;
+		}
+	}
+	rtl92e_irq_enable(dev);
+end:
+	priv->being_init_adapter = false;
+	return rtStatus;
+}
+
+static void _rtl92e_net_update(struct net_device *dev)
+{
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_network *net;
+	u16 BcnTimeCfg = 0, BcnCW = 6, BcnIFS = 0xf;
+	u16 rate_config = 0;
+
+	net = &priv->rtllib->current_network;
+	rtl92e_config_rate(dev, &rate_config);
+	priv->dot11_current_preamble_mode = PREAMBLE_AUTO;
+	priv->basic_rate = rate_config &= 0x15f;
+	rtl92e_writew(dev, BSSIDR, *(u16 *)net->bssid);
+	rtl92e_writel(dev, BSSIDR + 2, *(u32 *)(net->bssid + 2));
+
+	if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
+		rtl92e_writew(dev, ATIMWND, 2);
+		rtl92e_writew(dev, BCN_DMATIME, 256);
+		rtl92e_writew(dev, BCN_INTERVAL, net->beacon_interval);
+		rtl92e_writew(dev, BCN_DRV_EARLY_INT, 10);
+		rtl92e_writeb(dev, BCN_ERR_THRESH, 100);
+
+		BcnTimeCfg |= (BcnCW<<BCN_TCFG_CW_SHIFT);
+		BcnTimeCfg |= BcnIFS<<BCN_TCFG_IFS;
+
+		rtl92e_writew(dev, BCN_TCFG, BcnTimeCfg);
+	}
+}
+
+void rtl92e_link_change(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (!priv->up)
+		return;
+
+	if (ieee->state == RTLLIB_LINKED) {
+		_rtl92e_net_update(dev);
+		priv->ops->update_ratr_table(dev);
+		if ((ieee->pairwise_key_type == KEY_TYPE_WEP40) ||
+		    (ieee->pairwise_key_type == KEY_TYPE_WEP104))
+			rtl92e_enable_hw_security_config(dev);
+	} else {
+		rtl92e_writeb(dev, 0x173, 0);
+	}
+	_rtl92e_update_msr(dev);
+
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
+		u32 reg;
+
+		reg = rtl92e_readl(dev, RCR);
+		if (priv->rtllib->state == RTLLIB_LINKED) {
+			if (ieee->IntelPromiscuousModeInfo.bPromiscuousOn)
+				;
+			else
+				priv->ReceiveConfig = reg |= RCR_CBSSID;
+		} else
+			priv->ReceiveConfig = reg &= ~RCR_CBSSID;
+
+		rtl92e_writel(dev, RCR, reg);
+	}
+}
+
+void rtl92e_set_monitor_mode(struct net_device *dev, bool bAllowAllDA,
+			     bool WriteIntoReg)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (bAllowAllDA)
+		priv->ReceiveConfig |= RCR_AAP;
+	else
+		priv->ReceiveConfig &= ~RCR_AAP;
+
+	if (WriteIntoReg)
+		rtl92e_writel(dev, RCR, priv->ReceiveConfig);
+}
+
+static u8 _rtl92e_rate_mgn_to_hw(u8 rate)
+{
+	u8  ret = DESC90_RATE1M;
+
+	switch (rate) {
+	case MGN_1M:
+		ret = DESC90_RATE1M;
+		break;
+	case MGN_2M:
+		ret = DESC90_RATE2M;
+		break;
+	case MGN_5_5M:
+		ret = DESC90_RATE5_5M;
+		break;
+	case MGN_11M:
+		ret = DESC90_RATE11M;
+		break;
+	case MGN_6M:
+		ret = DESC90_RATE6M;
+		break;
+	case MGN_9M:
+		ret = DESC90_RATE9M;
+		break;
+	case MGN_12M:
+		ret = DESC90_RATE12M;
+		break;
+	case MGN_18M:
+		ret = DESC90_RATE18M;
+		break;
+	case MGN_24M:
+		ret = DESC90_RATE24M;
+		break;
+	case MGN_36M:
+		ret = DESC90_RATE36M;
+		break;
+	case MGN_48M:
+		ret = DESC90_RATE48M;
+		break;
+	case MGN_54M:
+		ret = DESC90_RATE54M;
+		break;
+	case MGN_MCS0:
+		ret = DESC90_RATEMCS0;
+		break;
+	case MGN_MCS1:
+		ret = DESC90_RATEMCS1;
+		break;
+	case MGN_MCS2:
+		ret = DESC90_RATEMCS2;
+		break;
+	case MGN_MCS3:
+		ret = DESC90_RATEMCS3;
+		break;
+	case MGN_MCS4:
+		ret = DESC90_RATEMCS4;
+		break;
+	case MGN_MCS5:
+		ret = DESC90_RATEMCS5;
+		break;
+	case MGN_MCS6:
+		ret = DESC90_RATEMCS6;
+		break;
+	case MGN_MCS7:
+		ret = DESC90_RATEMCS7;
+		break;
+	case MGN_MCS8:
+		ret = DESC90_RATEMCS8;
+		break;
+	case MGN_MCS9:
+		ret = DESC90_RATEMCS9;
+		break;
+	case MGN_MCS10:
+		ret = DESC90_RATEMCS10;
+		break;
+	case MGN_MCS11:
+		ret = DESC90_RATEMCS11;
+		break;
+	case MGN_MCS12:
+		ret = DESC90_RATEMCS12;
+		break;
+	case MGN_MCS13:
+		ret = DESC90_RATEMCS13;
+		break;
+	case MGN_MCS14:
+		ret = DESC90_RATEMCS14;
+		break;
+	case MGN_MCS15:
+		ret = DESC90_RATEMCS15;
+		break;
+	case (0x80|0x20):
+		ret = DESC90_RATEMCS32;
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static u8 _rtl92e_hw_queue_to_fw_queue(struct net_device *dev, u8 QueueID,
+				       u8 priority)
+{
+	u8 QueueSelect = 0x0;
+
+	switch (QueueID) {
+	case BE_QUEUE:
+		QueueSelect = QSLT_BE;
+		break;
+
+	case BK_QUEUE:
+		QueueSelect = QSLT_BK;
+		break;
+
+	case VO_QUEUE:
+		QueueSelect = QSLT_VO;
+		break;
+
+	case VI_QUEUE:
+		QueueSelect = QSLT_VI;
+		break;
+	case MGNT_QUEUE:
+		QueueSelect = QSLT_MGNT;
+		break;
+	case BEACON_QUEUE:
+		QueueSelect = QSLT_BEACON;
+		break;
+	case TXCMD_QUEUE:
+		QueueSelect = QSLT_CMD;
+		break;
+	case HIGH_QUEUE:
+		QueueSelect = QSLT_HIGH;
+		break;
+	default:
+		netdev_warn(dev, "%s(): Impossible Queue Selection: %d\n",
+			    __func__, QueueID);
+		break;
+	}
+	return QueueSelect;
+}
+
+static u8 _rtl92e_query_is_short(u8 TxHT, u8 TxRate, struct cb_desc *tcb_desc)
+{
+	u8   tmp_Short;
+
+	tmp_Short = (TxHT == 1) ? ((tcb_desc->bUseShortGI) ? 1 : 0) :
+			((tcb_desc->bUseShortPreamble) ? 1 : 0);
+	if (TxHT == 1 && TxRate != DESC90_RATEMCS15)
+		tmp_Short = 0;
+
+	return tmp_Short;
+}
+
+void  rtl92e_fill_tx_desc(struct net_device *dev, struct tx_desc *pdesc,
+			  struct cb_desc *cb_desc, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	dma_addr_t mapping;
+	struct tx_fwinfo_8190pci *pTxFwInfo;
+
+	pTxFwInfo = (struct tx_fwinfo_8190pci *)skb->data;
+	memset(pTxFwInfo, 0, sizeof(struct tx_fwinfo_8190pci));
+	pTxFwInfo->TxHT = (cb_desc->data_rate & 0x80) ? 1 : 0;
+	pTxFwInfo->TxRate = _rtl92e_rate_mgn_to_hw(cb_desc->data_rate);
+	pTxFwInfo->EnableCPUDur = cb_desc->bTxEnableFwCalcDur;
+	pTxFwInfo->Short = _rtl92e_query_is_short(pTxFwInfo->TxHT,
+						  pTxFwInfo->TxRate, cb_desc);
+
+	if (cb_desc->bAMPDUEnable) {
+		pTxFwInfo->AllowAggregation = 1;
+		pTxFwInfo->RxMF = cb_desc->ampdu_factor;
+		pTxFwInfo->RxAMD = cb_desc->ampdu_density;
+	} else {
+		pTxFwInfo->AllowAggregation = 0;
+		pTxFwInfo->RxMF = 0;
+		pTxFwInfo->RxAMD = 0;
+	}
+
+	pTxFwInfo->RtsEnable =	(cb_desc->bRTSEnable) ? 1 : 0;
+	pTxFwInfo->CtsEnable = (cb_desc->bCTSEnable) ? 1 : 0;
+	pTxFwInfo->RtsSTBC = (cb_desc->bRTSSTBC) ? 1 : 0;
+	pTxFwInfo->RtsHT = (cb_desc->rts_rate&0x80) ? 1 : 0;
+	pTxFwInfo->RtsRate = _rtl92e_rate_mgn_to_hw(cb_desc->rts_rate);
+	pTxFwInfo->RtsBandwidth = 0;
+	pTxFwInfo->RtsSubcarrier = cb_desc->RTSSC;
+	pTxFwInfo->RtsShort = (pTxFwInfo->RtsHT == 0) ?
+			  (cb_desc->bRTSUseShortPreamble ? 1 : 0) :
+			  (cb_desc->bRTSUseShortGI ? 1 : 0);
+	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20_40) {
+		if (cb_desc->bPacketBW) {
+			pTxFwInfo->TxBandwidth = 1;
+			pTxFwInfo->TxSubCarrier = 0;
+		} else {
+			pTxFwInfo->TxBandwidth = 0;
+			pTxFwInfo->TxSubCarrier = priv->nCur40MhzPrimeSC;
+		}
+	} else {
+		pTxFwInfo->TxBandwidth = 0;
+		pTxFwInfo->TxSubCarrier = 0;
+	}
+
+	memset((u8 *)pdesc, 0, 12);
+
+	mapping = dma_map_single(&priv->pdev->dev, skb->data, skb->len,
+				 DMA_TO_DEVICE);
+	if (dma_mapping_error(&priv->pdev->dev, mapping)) {
+		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
+		return;
+	}
+
+	pdesc->LINIP = 0;
+	pdesc->CmdInit = 1;
+	pdesc->Offset = sizeof(struct tx_fwinfo_8190pci) + 8;
+	pdesc->PktSize = skb->len - sizeof(struct tx_fwinfo_8190pci);
+
+	pdesc->SecCAMID = 0;
+	pdesc->RATid = cb_desc->RATRIndex;
+
+
+	pdesc->NoEnc = 1;
+	pdesc->SecType = 0x0;
+	if (cb_desc->bHwSec) {
+		static u8 tmp;
+
+		if (!tmp) {
+			tmp = 1;
+		}
+		switch (priv->rtllib->pairwise_key_type) {
+		case KEY_TYPE_WEP40:
+		case KEY_TYPE_WEP104:
+			pdesc->SecType = 0x1;
+			pdesc->NoEnc = 0;
+			break;
+		case KEY_TYPE_TKIP:
+			pdesc->SecType = 0x2;
+			pdesc->NoEnc = 0;
+			break;
+		case KEY_TYPE_CCMP:
+			pdesc->SecType = 0x3;
+			pdesc->NoEnc = 0;
+			break;
+		case KEY_TYPE_NA:
+			pdesc->SecType = 0x0;
+			pdesc->NoEnc = 1;
+			break;
+		}
+	}
+
+	pdesc->PktId = 0x0;
+
+	pdesc->QueueSelect = _rtl92e_hw_queue_to_fw_queue(dev,
+							  cb_desc->queue_index,
+							  cb_desc->priority);
+	pdesc->TxFWInfoSize = sizeof(struct tx_fwinfo_8190pci);
+
+	pdesc->DISFB = cb_desc->bTxDisableRateFallBack;
+	pdesc->USERATE = cb_desc->bTxUseDriverAssingedRate;
+
+	pdesc->FirstSeg = 1;
+	pdesc->LastSeg = 1;
+	pdesc->TxBufferSize = skb->len;
+
+	pdesc->TxBuffAddr = mapping;
+}
+
+void  rtl92e_fill_tx_cmd_desc(struct net_device *dev, struct tx_desc_cmd *entry,
+			      struct cb_desc *cb_desc, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	dma_addr_t mapping = dma_map_single(&priv->pdev->dev, skb->data,
+					    skb->len, DMA_TO_DEVICE);
+
+	if (dma_mapping_error(&priv->pdev->dev, mapping))
+		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
+	memset(entry, 0, 12);
+	entry->LINIP = cb_desc->bLastIniPkt;
+	entry->FirstSeg = 1;
+	entry->LastSeg = 1;
+	if (cb_desc->bCmdOrInit == DESC_PACKET_TYPE_INIT) {
+		entry->CmdInit = DESC_PACKET_TYPE_INIT;
+	} else {
+		struct tx_desc *entry_tmp = (struct tx_desc *)entry;
+
+		entry_tmp->CmdInit = DESC_PACKET_TYPE_NORMAL;
+		entry_tmp->Offset = sizeof(struct tx_fwinfo_8190pci) + 8;
+		entry_tmp->PktSize = cb_desc->pkt_size + entry_tmp->Offset;
+		entry_tmp->QueueSelect = QSLT_CMD;
+		entry_tmp->TxFWInfoSize = 0x08;
+		entry_tmp->RATid = DESC_PACKET_TYPE_INIT;
+	}
+	entry->TxBufferSize = skb->len;
+	entry->TxBuffAddr = mapping;
+	entry->OWN = 1;
+}
+
+static u8 _rtl92e_rate_hw_to_mgn(bool bIsHT, u8 rate)
+{
+	u8  ret_rate = 0x02;
+
+	if (!bIsHT) {
+		switch (rate) {
+		case DESC90_RATE1M:
+			ret_rate = MGN_1M;
+			break;
+		case DESC90_RATE2M:
+			ret_rate = MGN_2M;
+			break;
+		case DESC90_RATE5_5M:
+			ret_rate = MGN_5_5M;
+			break;
+		case DESC90_RATE11M:
+			ret_rate = MGN_11M;
+			break;
+		case DESC90_RATE6M:
+			ret_rate = MGN_6M;
+			break;
+		case DESC90_RATE9M:
+			ret_rate = MGN_9M;
+			break;
+		case DESC90_RATE12M:
+			ret_rate = MGN_12M;
+			break;
+		case DESC90_RATE18M:
+			ret_rate = MGN_18M;
+			break;
+		case DESC90_RATE24M:
+			ret_rate = MGN_24M;
+			break;
+		case DESC90_RATE36M:
+			ret_rate = MGN_36M;
+			break;
+		case DESC90_RATE48M:
+			ret_rate = MGN_48M;
+			break;
+		case DESC90_RATE54M:
+			ret_rate = MGN_54M;
+			break;
+		}
+
+	} else {
+		switch (rate) {
+		case DESC90_RATEMCS0:
+			ret_rate = MGN_MCS0;
+			break;
+		case DESC90_RATEMCS1:
+			ret_rate = MGN_MCS1;
+			break;
+		case DESC90_RATEMCS2:
+			ret_rate = MGN_MCS2;
+			break;
+		case DESC90_RATEMCS3:
+			ret_rate = MGN_MCS3;
+			break;
+		case DESC90_RATEMCS4:
+			ret_rate = MGN_MCS4;
+			break;
+		case DESC90_RATEMCS5:
+			ret_rate = MGN_MCS5;
+			break;
+		case DESC90_RATEMCS6:
+			ret_rate = MGN_MCS6;
+			break;
+		case DESC90_RATEMCS7:
+			ret_rate = MGN_MCS7;
+			break;
+		case DESC90_RATEMCS8:
+			ret_rate = MGN_MCS8;
+			break;
+		case DESC90_RATEMCS9:
+			ret_rate = MGN_MCS9;
+			break;
+		case DESC90_RATEMCS10:
+			ret_rate = MGN_MCS10;
+			break;
+		case DESC90_RATEMCS11:
+			ret_rate = MGN_MCS11;
+			break;
+		case DESC90_RATEMCS12:
+			ret_rate = MGN_MCS12;
+			break;
+		case DESC90_RATEMCS13:
+			ret_rate = MGN_MCS13;
+			break;
+		case DESC90_RATEMCS14:
+			ret_rate = MGN_MCS14;
+			break;
+		case DESC90_RATEMCS15:
+			ret_rate = MGN_MCS15;
+			break;
+		case DESC90_RATEMCS32:
+			ret_rate = 0x80 | 0x20;
+			break;
+		}
+	}
+
+	return ret_rate;
+}
+
+static long _rtl92e_signal_scale_mapping(struct r8192_priv *priv, long currsig)
+{
+	long retsig;
+
+	if (currsig >= 61 && currsig <= 100)
+		retsig = 90 + ((currsig - 60) / 4);
+	else if (currsig >= 41 && currsig <= 60)
+		retsig = 78 + ((currsig - 40) / 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 5 && currsig <= 20)
+		retsig = 42 + (((currsig - 5) * 2) / 3);
+	else if (currsig == 4)
+		retsig = 36;
+	else if (currsig == 3)
+		retsig = 27;
+	else if (currsig == 2)
+		retsig = 18;
+	else if (currsig == 1)
+		retsig = 9;
+	else
+		retsig = currsig;
+
+	return retsig;
+}
+
+
+#define	 rx_hal_is_cck_rate(_pdrvinfo)\
+			((_pdrvinfo->RxRate == DESC90_RATE1M ||\
+			_pdrvinfo->RxRate == DESC90_RATE2M ||\
+			_pdrvinfo->RxRate == DESC90_RATE5_5M ||\
+			_pdrvinfo->RxRate == DESC90_RATE11M) &&\
+			!_pdrvinfo->RxHT)
+
+static void _rtl92e_query_rxphystatus(
+	struct r8192_priv *priv,
+	struct rtllib_rx_stats *pstats,
+	struct rx_desc  *pdesc,
+	struct rx_fwinfo   *pdrvinfo,
+	struct rtllib_rx_stats *precord_stats,
+	bool bpacket_match_bssid,
+	bool bpacket_toself,
+	bool bPacketBeacon,
+	bool bToSelfBA
+	)
+{
+	struct phy_sts_ofdm_819xpci *pofdm_buf;
+	struct phy_sts_cck_819xpci *pcck_buf;
+	struct phy_ofdm_rx_status_rxsc_sgien_exintfflag *prxsc;
+	u8 *prxpkt;
+	u8 i, max_spatial_stream, tmp_rxsnr, tmp_rxevm, rxsc_sgien_exflg;
+	s8 rx_pwr[4], rx_pwr_all = 0;
+	s8 rx_snrX, rx_evmX;
+	u8 evm, pwdb_all;
+	u32 RSSI, total_rssi = 0;
+	u8 is_cck_rate = 0;
+	u8 rf_rx_num = 0;
+	static	u8 check_reg824;
+	static	u32 reg824_bit9;
+
+	priv->stats.numqry_phystatus++;
+
+	is_cck_rate = rx_hal_is_cck_rate(pdrvinfo);
+	memset(precord_stats, 0, sizeof(struct rtllib_rx_stats));
+	pstats->bPacketMatchBSSID = precord_stats->bPacketMatchBSSID =
+				    bpacket_match_bssid;
+	pstats->bPacketToSelf = precord_stats->bPacketToSelf = bpacket_toself;
+	pstats->bIsCCK = precord_stats->bIsCCK = is_cck_rate;
+	pstats->bPacketBeacon = precord_stats->bPacketBeacon = bPacketBeacon;
+	pstats->bToSelfBA = precord_stats->bToSelfBA = bToSelfBA;
+	if (check_reg824 == 0) {
+		reg824_bit9 = rtl92e_get_bb_reg(priv->rtllib->dev,
+						rFPGA0_XA_HSSIParameter2,
+						0x200);
+		check_reg824 = 1;
+	}
+
+
+	prxpkt = (u8 *)pdrvinfo;
+
+	prxpkt += sizeof(struct rx_fwinfo);
+
+	pcck_buf = (struct phy_sts_cck_819xpci *)prxpkt;
+	pofdm_buf = (struct phy_sts_ofdm_819xpci *)prxpkt;
+
+	pstats->RxMIMOSignalQuality[0] = -1;
+	pstats->RxMIMOSignalQuality[1] = -1;
+	precord_stats->RxMIMOSignalQuality[0] = -1;
+	precord_stats->RxMIMOSignalQuality[1] = -1;
+
+	if (is_cck_rate) {
+		u8 report;
+
+		priv->stats.numqry_phystatusCCK++;
+		if (!reg824_bit9) {
+			report = pcck_buf->cck_agc_rpt & 0xc0;
+			report >>= 6;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -35 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x2:
+				rx_pwr_all = -23 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x1:
+				rx_pwr_all = -11 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x0:
+				rx_pwr_all = 8 - (pcck_buf->cck_agc_rpt & 0x3e);
+				break;
+			}
+		} else {
+			report = pcck_buf->cck_agc_rpt & 0x60;
+			report >>= 5;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -35 -
+					((pcck_buf->cck_agc_rpt &
+					0x1f) << 1);
+				break;
+			case 0x2:
+				rx_pwr_all = -23 -
+					((pcck_buf->cck_agc_rpt &
+					 0x1f) << 1);
+				break;
+			case 0x1:
+				rx_pwr_all = -11 -
+					 ((pcck_buf->cck_agc_rpt &
+					 0x1f) << 1);
+				break;
+			case 0x0:
+				rx_pwr_all = -8 -
+					 ((pcck_buf->cck_agc_rpt &
+					 0x1f) << 1);
+				break;
+			}
+		}
+
+		pwdb_all = rtl92e_rx_db_to_percent(rx_pwr_all);
+		pstats->RxPWDBAll = precord_stats->RxPWDBAll = pwdb_all;
+		pstats->RecvSignalPower = rx_pwr_all;
+
+		if (bpacket_match_bssid) {
+			u8	sq;
+
+			if (pstats->RxPWDBAll > 40) {
+				sq = 100;
+			} else {
+				sq = pcck_buf->sq_rpt;
+
+				if (pcck_buf->sq_rpt > 64)
+					sq = 0;
+				else if (pcck_buf->sq_rpt < 20)
+					sq = 100;
+				else
+					sq = ((64-sq) * 100) / 44;
+			}
+			pstats->SignalQuality = sq;
+			precord_stats->SignalQuality = sq;
+			pstats->RxMIMOSignalQuality[0] = sq;
+			precord_stats->RxMIMOSignalQuality[0] = sq;
+			pstats->RxMIMOSignalQuality[1] = -1;
+			precord_stats->RxMIMOSignalQuality[1] = -1;
+		}
+	} else {
+		priv->stats.numqry_phystatusHT++;
+		for (i = RF90_PATH_A; i < RF90_PATH_MAX; i++) {
+			if (priv->brfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((pofdm_buf->trsw_gain_X[i] & 0x3F) *
+				     2) - 110;
+
+			tmp_rxsnr = pofdm_buf->rxsnr_X[i];
+			rx_snrX = (s8)(tmp_rxsnr);
+			rx_snrX /= 2;
+			priv->stats.rxSNRdB[i] = (long)rx_snrX;
+
+			RSSI = rtl92e_rx_db_to_percent(rx_pwr[i]);
+			if (priv->brfpath_rxenable[i])
+				total_rssi += RSSI;
+
+			if (bpacket_match_bssid) {
+				pstats->RxMIMOSignalStrength[i] = RSSI;
+				precord_stats->RxMIMOSignalStrength[i] = RSSI;
+			}
+		}
+
+
+		rx_pwr_all = (((pofdm_buf->pwdb_all) >> 1) & 0x7f) - 106;
+		pwdb_all = rtl92e_rx_db_to_percent(rx_pwr_all);
+
+		pstats->RxPWDBAll = precord_stats->RxPWDBAll = pwdb_all;
+		pstats->RxPower = precord_stats->RxPower =	rx_pwr_all;
+		pstats->RecvSignalPower = rx_pwr_all;
+		if (pdrvinfo->RxHT && pdrvinfo->RxRate >= DESC90_RATEMCS8 &&
+		    pdrvinfo->RxRate <= DESC90_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			tmp_rxevm = pofdm_buf->rxevm_X[i];
+			rx_evmX = (s8)(tmp_rxevm);
+
+			rx_evmX /= 2;
+
+			evm = rtl92e_evm_db_to_percent(rx_evmX);
+			if (bpacket_match_bssid) {
+				if (i == 0) {
+					pstats->SignalQuality = evm & 0xff;
+					precord_stats->SignalQuality = evm & 0xff;
+				}
+				pstats->RxMIMOSignalQuality[i] = evm & 0xff;
+				precord_stats->RxMIMOSignalQuality[i] = evm & 0xff;
+			}
+		}
+
+
+		rxsc_sgien_exflg = pofdm_buf->rxsc_sgien_exflg;
+		prxsc = (struct phy_ofdm_rx_status_rxsc_sgien_exintfflag *)
+			&rxsc_sgien_exflg;
+		if (pdrvinfo->BW)
+			priv->stats.received_bwtype[1+prxsc->rxsc]++;
+		else
+			priv->stats.received_bwtype[0]++;
+	}
+
+	if (is_cck_rate) {
+		pstats->SignalStrength = precord_stats->SignalStrength =
+					 _rtl92e_signal_scale_mapping(priv,
+					 (long)pwdb_all);
+
+	} else {
+		if (rf_rx_num != 0)
+			pstats->SignalStrength = precord_stats->SignalStrength =
+					 _rtl92e_signal_scale_mapping(priv,
+					 (long)(total_rssi /= rf_rx_num));
+	}
+}
+
+static void _rtl92e_process_phyinfo(struct r8192_priv *priv, u8 *buffer,
+				    struct rtllib_rx_stats *prev_st,
+				    struct rtllib_rx_stats *curr_st)
+{
+	bool bcheck = false;
+	u8	rfpath;
+	u32 ij, tmp_val;
+	static u32 slide_rssi_index, slide_rssi_statistics;
+	static u32 slide_evm_index, slide_evm_statistics;
+	static u32 last_rssi, last_evm;
+	static u32 slide_beacon_adc_pwdb_index;
+	static u32 slide_beacon_adc_pwdb_statistics;
+	static u32 last_beacon_adc_pwdb;
+	struct rtllib_hdr_3addr *hdr;
+	u16 sc;
+	unsigned int seq;
+
+	hdr = (struct rtllib_hdr_3addr *)buffer;
+	sc = le16_to_cpu(hdr->seq_ctl);
+	seq = WLAN_GET_SEQ_SEQ(sc);
+	curr_st->Seq_Num = seq;
+	if (!prev_st->bIsAMPDU)
+		bcheck = true;
+
+	if (slide_rssi_statistics++ >= PHY_RSSI_SLID_WIN_MAX) {
+		slide_rssi_statistics = PHY_RSSI_SLID_WIN_MAX;
+		last_rssi = priv->stats.slide_signal_strength[slide_rssi_index];
+		priv->stats.slide_rssi_total -= last_rssi;
+	}
+	priv->stats.slide_rssi_total += prev_st->SignalStrength;
+
+	priv->stats.slide_signal_strength[slide_rssi_index++] =
+					 prev_st->SignalStrength;
+	if (slide_rssi_index >= PHY_RSSI_SLID_WIN_MAX)
+		slide_rssi_index = 0;
+
+	tmp_val = priv->stats.slide_rssi_total/slide_rssi_statistics;
+	priv->stats.signal_strength = rtl92e_translate_to_dbm(priv, tmp_val);
+	curr_st->rssi = priv->stats.signal_strength;
+	if (!prev_st->bPacketMatchBSSID) {
+		if (!prev_st->bToSelfBA)
+			return;
+	}
+
+	if (!bcheck)
+		return;
+
+	priv->stats.num_process_phyinfo++;
+	if (!prev_st->bIsCCK && prev_st->bPacketToSelf) {
+		for (rfpath = RF90_PATH_A; rfpath < RF90_PATH_C; rfpath++) {
+			if (!rtl92e_is_legal_rf_path(priv->rtllib->dev, rfpath))
+				continue;
+			if (priv->stats.rx_rssi_percentage[rfpath] == 0) {
+				priv->stats.rx_rssi_percentage[rfpath] =
+					 prev_st->RxMIMOSignalStrength[rfpath];
+			}
+			if (prev_st->RxMIMOSignalStrength[rfpath]  >
+			    priv->stats.rx_rssi_percentage[rfpath]) {
+				priv->stats.rx_rssi_percentage[rfpath] =
+					((priv->stats.rx_rssi_percentage[rfpath]
+					* (RX_SMOOTH - 1)) +
+					(prev_st->RxMIMOSignalStrength
+					[rfpath])) / (RX_SMOOTH);
+				priv->stats.rx_rssi_percentage[rfpath] =
+					 priv->stats.rx_rssi_percentage[rfpath]
+					 + 1;
+			} else {
+				priv->stats.rx_rssi_percentage[rfpath] =
+				   ((priv->stats.rx_rssi_percentage[rfpath] *
+				   (RX_SMOOTH-1)) +
+				   (prev_st->RxMIMOSignalStrength[rfpath])) /
+				   (RX_SMOOTH);
+			}
+		}
+	}
+
+
+	if (prev_st->bPacketBeacon) {
+		if (slide_beacon_adc_pwdb_statistics++ >=
+		    PHY_Beacon_RSSI_SLID_WIN_MAX) {
+			slide_beacon_adc_pwdb_statistics =
+					 PHY_Beacon_RSSI_SLID_WIN_MAX;
+			last_beacon_adc_pwdb = priv->stats.Slide_Beacon_pwdb
+					       [slide_beacon_adc_pwdb_index];
+			priv->stats.Slide_Beacon_Total -= last_beacon_adc_pwdb;
+		}
+		priv->stats.Slide_Beacon_Total += prev_st->RxPWDBAll;
+		priv->stats.Slide_Beacon_pwdb[slide_beacon_adc_pwdb_index] =
+							 prev_st->RxPWDBAll;
+		slide_beacon_adc_pwdb_index++;
+		if (slide_beacon_adc_pwdb_index >= PHY_Beacon_RSSI_SLID_WIN_MAX)
+			slide_beacon_adc_pwdb_index = 0;
+		prev_st->RxPWDBAll = priv->stats.Slide_Beacon_Total /
+				     slide_beacon_adc_pwdb_statistics;
+		if (prev_st->RxPWDBAll >= 3)
+			prev_st->RxPWDBAll -= 3;
+	}
+	if (prev_st->bPacketToSelf || prev_st->bPacketBeacon ||
+	    prev_st->bToSelfBA) {
+		if (priv->undecorated_smoothed_pwdb < 0)
+			priv->undecorated_smoothed_pwdb = prev_st->RxPWDBAll;
+		if (prev_st->RxPWDBAll > (u32)priv->undecorated_smoothed_pwdb) {
+			priv->undecorated_smoothed_pwdb =
+					(((priv->undecorated_smoothed_pwdb) *
+					(RX_SMOOTH-1)) +
+					(prev_st->RxPWDBAll)) / (RX_SMOOTH);
+			priv->undecorated_smoothed_pwdb =
+					 priv->undecorated_smoothed_pwdb + 1;
+		} else {
+			priv->undecorated_smoothed_pwdb =
+					(((priv->undecorated_smoothed_pwdb) *
+					(RX_SMOOTH-1)) +
+					(prev_st->RxPWDBAll)) / (RX_SMOOTH);
+		}
+		rtl92e_update_rx_statistics(priv, prev_st);
+	}
+
+	if (prev_st->SignalQuality != 0) {
+		if (prev_st->bPacketToSelf || prev_st->bPacketBeacon ||
+		    prev_st->bToSelfBA) {
+			if (slide_evm_statistics++ >= PHY_RSSI_SLID_WIN_MAX) {
+				slide_evm_statistics = PHY_RSSI_SLID_WIN_MAX;
+				last_evm =
+					 priv->stats.slide_evm[slide_evm_index];
+				priv->stats.slide_evm_total -= last_evm;
+			}
+
+			priv->stats.slide_evm_total += prev_st->SignalQuality;
+
+			priv->stats.slide_evm[slide_evm_index++] =
+						 prev_st->SignalQuality;
+			if (slide_evm_index >= PHY_RSSI_SLID_WIN_MAX)
+				slide_evm_index = 0;
+
+			tmp_val = priv->stats.slide_evm_total /
+				  slide_evm_statistics;
+			priv->stats.signal_quality = tmp_val;
+			priv->stats.last_signal_strength_inpercent = tmp_val;
+		}
+
+		if (prev_st->bPacketToSelf ||
+		    prev_st->bPacketBeacon ||
+		    prev_st->bToSelfBA) {
+			for (ij = 0; ij < 2; ij++) {
+				if (prev_st->RxMIMOSignalQuality[ij] != -1) {
+					if (priv->stats.rx_evm_percentage[ij] == 0)
+						priv->stats.rx_evm_percentage[ij] =
+						   prev_st->RxMIMOSignalQuality[ij];
+					priv->stats.rx_evm_percentage[ij] =
+					  ((priv->stats.rx_evm_percentage[ij] *
+					  (RX_SMOOTH - 1)) +
+					  (prev_st->RxMIMOSignalQuality[ij])) /
+					  (RX_SMOOTH);
+				}
+			}
+		}
+	}
+}
+
+static void _rtl92e_translate_rx_signal_stats(struct net_device *dev,
+					      struct sk_buff *skb,
+					      struct rtllib_rx_stats *pstats,
+					      struct rx_desc *pdesc,
+					      struct rx_fwinfo *pdrvinfo)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	bool bpacket_match_bssid, bpacket_toself;
+	bool bPacketBeacon = false;
+	struct rtllib_hdr_3addr *hdr;
+	bool bToSelfBA = false;
+	static struct rtllib_rx_stats  previous_stats;
+	u16 fc, type;
+	u8 *tmp_buf;
+	u8 *praddr;
+
+	tmp_buf = skb->data + pstats->RxDrvInfoSize + pstats->RxBufShift;
+
+	hdr = (struct rtllib_hdr_3addr *)tmp_buf;
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+
+	bpacket_match_bssid =
+		((type != RTLLIB_FTYPE_CTL) &&
+		 ether_addr_equal(priv->rtllib->current_network.bssid,
+				  (fc & RTLLIB_FCTL_TODS) ? hdr->addr1 :
+				  (fc & RTLLIB_FCTL_FROMDS) ? hdr->addr2 :
+				  hdr->addr3) &&
+		 (!pstats->bHwError) && (!pstats->bCRC) && (!pstats->bICV));
+	bpacket_toself = bpacket_match_bssid &&		/* check this */
+			 ether_addr_equal(praddr, priv->rtllib->dev->dev_addr);
+	if (WLAN_FC_GET_FRAMETYPE(fc) == RTLLIB_STYPE_BEACON)
+		bPacketBeacon = true;
+	if (bpacket_match_bssid)
+		priv->stats.numpacket_matchbssid++;
+	if (bpacket_toself)
+		priv->stats.numpacket_toself++;
+	_rtl92e_process_phyinfo(priv, tmp_buf, &previous_stats, pstats);
+	_rtl92e_query_rxphystatus(priv, pstats, pdesc, pdrvinfo,
+				  &previous_stats, bpacket_match_bssid,
+				  bpacket_toself, bPacketBeacon, bToSelfBA);
+	rtl92e_copy_mpdu_stats(pstats, &previous_stats);
+}
+
+static void _rtl92e_update_received_rate_histogram_stats(
+					   struct net_device *dev,
+					   struct rtllib_rx_stats *pstats)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	u32 rcvType = 1;
+	u32 rateIndex;
+	u32 preamble_guardinterval;
+
+	if (pstats->bCRC)
+		rcvType = 2;
+	else if (pstats->bICV)
+		rcvType = 3;
+
+	if (pstats->bShortPreamble)
+		preamble_guardinterval = 1;
+	else
+		preamble_guardinterval = 0;
+
+	switch (pstats->rate) {
+	case MGN_1M:
+		rateIndex = 0;
+		break;
+	case MGN_2M:
+		rateIndex = 1;
+		break;
+	case MGN_5_5M:
+		rateIndex = 2;
+		break;
+	case MGN_11M:
+		rateIndex = 3;
+		break;
+	case MGN_6M:
+		rateIndex = 4;
+		break;
+	case MGN_9M:
+		rateIndex = 5;
+		break;
+	case MGN_12M:
+		rateIndex = 6;
+		break;
+	case MGN_18M:
+		rateIndex = 7;
+		break;
+	case MGN_24M:
+		rateIndex = 8;
+		break;
+	case MGN_36M:
+		rateIndex = 9;
+		break;
+	case MGN_48M:
+		rateIndex = 10;
+		break;
+	case MGN_54M:
+		rateIndex = 11;
+		break;
+	case MGN_MCS0:
+		rateIndex = 12;
+		break;
+	case MGN_MCS1:
+		rateIndex = 13;
+		break;
+	case MGN_MCS2:
+		rateIndex = 14;
+		break;
+	case MGN_MCS3:
+		rateIndex = 15;
+		break;
+	case MGN_MCS4:
+		rateIndex = 16;
+		break;
+	case MGN_MCS5:
+		rateIndex = 17;
+		break;
+	case MGN_MCS6:
+		rateIndex = 18;
+		break;
+	case MGN_MCS7:
+		rateIndex = 19;
+		break;
+	case MGN_MCS8:
+		rateIndex = 20;
+		break;
+	case MGN_MCS9:
+		rateIndex = 21;
+		break;
+	case MGN_MCS10:
+		rateIndex = 22;
+		break;
+	case MGN_MCS11:
+		rateIndex = 23;
+		break;
+	case MGN_MCS12:
+		rateIndex = 24;
+		break;
+	case MGN_MCS13:
+		rateIndex = 25;
+		break;
+	case MGN_MCS14:
+		rateIndex = 26;
+		break;
+	case MGN_MCS15:
+		rateIndex = 27;
+		break;
+	default:
+		rateIndex = 28;
+		break;
+	}
+	priv->stats.received_preamble_GI[preamble_guardinterval][rateIndex]++;
+	priv->stats.received_rate_histogram[0][rateIndex]++;
+	priv->stats.received_rate_histogram[rcvType][rateIndex]++;
+}
+
+bool rtl92e_get_rx_stats(struct net_device *dev, struct rtllib_rx_stats *stats,
+			 struct rx_desc *pdesc, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rx_fwinfo *pDrvInfo = NULL;
+
+	stats->bICV = pdesc->ICV;
+	stats->bCRC = pdesc->CRC32;
+	stats->bHwError = pdesc->CRC32 | pdesc->ICV;
+
+	stats->Length = pdesc->Length;
+	if (stats->Length < 24)
+		stats->bHwError |= 1;
+
+	if (stats->bHwError) {
+		stats->bShift = false;
+
+		if (pdesc->CRC32) {
+			if (pdesc->Length < 500)
+				priv->stats.rxcrcerrmin++;
+			else if (pdesc->Length > 1000)
+				priv->stats.rxcrcerrmax++;
+			else
+				priv->stats.rxcrcerrmid++;
+		}
+		return false;
+	}
+
+	stats->RxDrvInfoSize = pdesc->RxDrvInfoSize;
+	stats->RxBufShift = (pdesc->Shift) & 0x03;
+	stats->Decrypted = !pdesc->SWDec;
+
+	pDrvInfo = (struct rx_fwinfo *)(skb->data + stats->RxBufShift);
+
+	stats->rate = _rtl92e_rate_hw_to_mgn((bool)pDrvInfo->RxHT,
+					     pDrvInfo->RxRate);
+	stats->bShortPreamble = pDrvInfo->SPLCP;
+
+	_rtl92e_update_received_rate_histogram_stats(dev, stats);
+
+	stats->bIsAMPDU = (pDrvInfo->PartAggr == 1);
+	stats->bFirstMPDU = (pDrvInfo->PartAggr == 1) &&
+			    (pDrvInfo->FirstAGGR == 1);
+
+	stats->TimeStampLow = pDrvInfo->TSFL;
+	stats->TimeStampHigh = rtl92e_readl(dev, TSFR+4);
+
+	rtl92e_update_rx_pkt_timestamp(dev, stats);
+
+	if ((stats->RxBufShift + stats->RxDrvInfoSize) > 0)
+		stats->bShift = 1;
+
+	stats->RxIs40MHzPacket = pDrvInfo->BW;
+
+	_rtl92e_translate_rx_signal_stats(dev, skb, stats, pdesc, pDrvInfo);
+	skb_trim(skb, skb->len - 4/*sCrcLng*/);
+
+
+	stats->packetlength = stats->Length-4;
+	stats->fraglength = stats->packetlength;
+	stats->fragoffset = 0;
+	stats->ntotalfrag = 1;
+	return true;
+}
+
+void rtl92e_stop_adapter(struct net_device *dev, bool reset)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int i;
+	u8	OpMode;
+	u8	u1bTmp;
+	u32	ulRegRead;
+
+	OpMode = RT_OP_MODE_NO_LINK;
+	priv->rtllib->SetHwRegHandler(dev, HW_VAR_MEDIA_STATUS, &OpMode);
+
+	if (!priv->rtllib->bSupportRemoteWakeUp) {
+		u1bTmp = 0x0;
+		rtl92e_writeb(dev, CMDR, u1bTmp);
+	}
+
+	mdelay(20);
+
+	if (!reset) {
+		mdelay(150);
+
+		priv->bHwRfOffAction = 2;
+
+		if (!priv->rtllib->bSupportRemoteWakeUp) {
+			rtl92e_set_rf_off(dev);
+			ulRegRead = rtl92e_readl(dev, CPU_GEN);
+			ulRegRead |= CPU_GEN_SYSTEM_RESET;
+			rtl92e_writel(dev, CPU_GEN, ulRegRead);
+		} else {
+			rtl92e_writel(dev, WFCRC0, 0xffffffff);
+			rtl92e_writel(dev, WFCRC1, 0xffffffff);
+			rtl92e_writel(dev, WFCRC2, 0xffffffff);
+
+
+			rtl92e_writeb(dev, PMR, 0x5);
+			rtl92e_writeb(dev, MacBlkCtrl, 0xa);
+		}
+	}
+
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_purge(&priv->rtllib->skb_waitQ[i]);
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_purge(&priv->rtllib->skb_aggQ[i]);
+
+	skb_queue_purge(&priv->skb_queue);
+}
+
+void rtl92e_update_ratr_table(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	u8 *pMcsRate = ieee->dot11HTOperationalRateSet;
+	u32 ratr_value = 0;
+	u16 rate_config = 0;
+	u8 rate_index = 0;
+
+	rtl92e_config_rate(dev, &rate_config);
+	ratr_value = rate_config | *pMcsRate << 12;
+	switch (ieee->mode) {
+	case IEEE_A:
+		ratr_value &= 0x00000FF0;
+		break;
+	case IEEE_B:
+		ratr_value &= 0x0000000F;
+		break;
+	case IEEE_G:
+	case IEEE_G|IEEE_B:
+		ratr_value &= 0x00000FF7;
+		break;
+	case IEEE_N_24G:
+	case IEEE_N_5G:
+		if (ieee->pHTInfo->peer_mimo_ps == 0) {
+			ratr_value &= 0x0007F007;
+		} else {
+			if (priv->rf_type == RF_1T2R)
+				ratr_value &= 0x000FF007;
+			else
+				ratr_value &= 0x0F81F007;
+		}
+		break;
+	default:
+		break;
+	}
+	ratr_value &= 0x0FFFFFFF;
+	if (ieee->pHTInfo->cur_tx_bw40mhz &&
+	    ieee->pHTInfo->bCurShortGI40MHz)
+		ratr_value |= 0x80000000;
+	else if (!ieee->pHTInfo->cur_tx_bw40mhz &&
+		  ieee->pHTInfo->bCurShortGI20MHz)
+		ratr_value |= 0x80000000;
+	rtl92e_writel(dev, RATR0+rate_index*4, ratr_value);
+	rtl92e_writeb(dev, UFWP, 1);
+}
+
+void
+rtl92e_init_variables(struct net_device  *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	strscpy(priv->nick, "rtl8192E", sizeof(priv->nick));
+
+	priv->rtllib->softmac_features  = IEEE_SOFTMAC_SCAN |
+		IEEE_SOFTMAC_ASSOCIATE | IEEE_SOFTMAC_PROBERQ |
+		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE;
+
+	priv->rtllib->tx_headroom = sizeof(struct tx_fwinfo_8190pci);
+
+	priv->ShortRetryLimit = 0x30;
+	priv->LongRetryLimit = 0x30;
+
+	priv->ReceiveConfig = RCR_ADD3	|
+		RCR_AMF | RCR_ADF |
+		RCR_AICV |
+		RCR_AB | RCR_AM | RCR_APM |
+		RCR_AAP | ((u32)7<<RCR_MXDMA_OFFSET) |
+		((u32)7 << RCR_FIFO_OFFSET) | RCR_ONLYERLPKT;
+
+	priv->irq_mask[0] = (u32)(IMR_ROK | IMR_VODOK | IMR_VIDOK |
+			    IMR_BEDOK | IMR_BKDOK | IMR_HCCADOK |
+			    IMR_MGNTDOK | IMR_COMDOK | IMR_HIGHDOK |
+			    IMR_BDOK | IMR_RXCMDOK | IMR_TIMEOUT0 |
+			    IMR_RDU | IMR_RXFOVW | IMR_TXFOVW |
+			    IMR_BcnInt | IMR_TBDOK | IMR_TBDER);
+
+	priv->PwrDomainProtect = false;
+
+	priv->bfirst_after_down = false;
+}
+
+void rtl92e_enable_irq(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	priv->irq_enabled = 1;
+
+	rtl92e_writel(dev, INTA_MASK, priv->irq_mask[0]);
+
+}
+
+void rtl92e_disable_irq(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	rtl92e_writel(dev, INTA_MASK, 0);
+
+	priv->irq_enabled = 0;
+}
+
+void rtl92e_clear_irq(struct net_device *dev)
+{
+	u32 tmp;
+
+	tmp = rtl92e_readl(dev, ISR);
+	rtl92e_writel(dev, ISR, tmp);
+}
+
+
+void rtl92e_enable_rx(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	rtl92e_writel(dev, RDQDA, priv->rx_ring_dma[RX_MPDU_QUEUE]);
+}
+
+static const u32 TX_DESC_BASE[] = {
+	BKQDA, BEQDA, VIQDA, VOQDA, HCCAQDA, CQDA, MQDA, HQDA, BQDA
+};
+
+void rtl92e_enable_tx(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	u32 i;
+
+	for (i = 0; i < MAX_TX_QUEUE_COUNT; i++)
+		rtl92e_writel(dev, TX_DESC_BASE[i], priv->tx_ring[i].dma);
+}
+
+
+void rtl92e_ack_irq(struct net_device *dev, u32 *p_inta, u32 *p_intb)
+{
+	*p_inta = rtl92e_readl(dev, ISR);
+	rtl92e_writel(dev, ISR, *p_inta);
+}
+
+bool rtl92e_is_rx_stuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u16		  RegRxCounter = rtl92e_readw(dev, 0x130);
+	bool		  bStuck = false;
+	static u8	  rx_chk_cnt;
+	u32		SlotIndex = 0, TotalRxStuckCount = 0;
+	u8		i;
+	u8		SilentResetRxSoltNum = 4;
+
+	rx_chk_cnt++;
+	if (priv->undecorated_smoothed_pwdb >= (RateAdaptiveTH_High+5)) {
+		rx_chk_cnt = 0;
+	} else if ((priv->undecorated_smoothed_pwdb < (RateAdaptiveTH_High + 5))
+	  && (((priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) &&
+	  (priv->undecorated_smoothed_pwdb >= RateAdaptiveTH_Low_40M))
+	  || ((priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20) &&
+	  (priv->undecorated_smoothed_pwdb >= RateAdaptiveTH_Low_20M)))) {
+		if (rx_chk_cnt < 2)
+			return bStuck;
+		rx_chk_cnt = 0;
+	} else if ((((priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) &&
+		  (priv->undecorated_smoothed_pwdb < RateAdaptiveTH_Low_40M)) ||
+		((priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20) &&
+		 (priv->undecorated_smoothed_pwdb < RateAdaptiveTH_Low_20M))) &&
+		priv->undecorated_smoothed_pwdb >= VeryLowRSSI) {
+		if (rx_chk_cnt < 4)
+			return bStuck;
+		rx_chk_cnt = 0;
+	} else {
+		if (rx_chk_cnt < 8)
+			return bStuck;
+		rx_chk_cnt = 0;
+	}
+
+
+	SlotIndex = (priv->SilentResetRxSlotIndex++)%SilentResetRxSoltNum;
+
+	if (priv->RxCounter == RegRxCounter) {
+		priv->SilentResetRxStuckEvent[SlotIndex] = 1;
+
+		for (i = 0; i < SilentResetRxSoltNum; i++)
+			TotalRxStuckCount += priv->SilentResetRxStuckEvent[i];
+
+		if (TotalRxStuckCount == SilentResetRxSoltNum) {
+			bStuck = true;
+			for (i = 0; i < SilentResetRxSoltNum; i++)
+				TotalRxStuckCount +=
+					 priv->SilentResetRxStuckEvent[i];
+		}
+
+
+	} else {
+		priv->SilentResetRxStuckEvent[SlotIndex] = 0;
+	}
+
+	priv->RxCounter = RegRxCounter;
+
+	return bStuck;
+}
+
+bool rtl92e_is_tx_stuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	bool	bStuck = false;
+	u16	RegTxCounter = rtl92e_readw(dev, 0x128);
+
+	if (priv->TxCounter == RegTxCounter)
+		bStuck = true;
+
+	priv->TxCounter = RegTxCounter;
+
+	return bStuck;
+}
+
+bool rtl92e_get_nmode_support_by_sec(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (ieee->rtllib_ap_sec_type &&
+	   (ieee->rtllib_ap_sec_type(priv->rtllib)&(SEC_ALG_WEP |
+				     SEC_ALG_TKIP))) {
+		return false;
+	} else {
+		return true;
+	}
+}
+
+bool rtl92e_is_halfn_supported_by_ap(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	return ieee->bHalfWirelessN24GMode;
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
new file mode 100644
index 000000000..1713381dc
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL8192E_H
+#define _RTL8192E_H
+
+#include "r8190P_def.h"
+
+bool rtl92e_is_halfn_supported_by_ap(struct net_device *dev);
+bool rtl92e_get_nmode_support_by_sec(struct net_device *dev);
+bool rtl92e_is_tx_stuck(struct net_device *dev);
+bool rtl92e_is_rx_stuck(struct net_device *dev);
+void rtl92e_ack_irq(struct net_device *dev, u32 *p_inta, u32 *p_intb);
+void rtl92e_enable_rx(struct net_device *dev);
+void rtl92e_enable_tx(struct net_device *dev);
+void rtl92e_enable_irq(struct net_device *dev);
+void rtl92e_disable_irq(struct net_device *dev);
+void rtl92e_clear_irq(struct net_device *dev);
+void rtl92e_init_variables(struct net_device  *dev);
+void rtl92e_start_beacon(struct net_device *dev);
+void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val);
+void rtl92e_get_eeprom_size(struct net_device *dev);
+bool rtl92e_start_adapter(struct net_device *dev);
+void rtl92e_link_change(struct net_device *dev);
+void rtl92e_set_monitor_mode(struct net_device *dev, bool bAllowAllDA,
+			     bool WriteIntoReg);
+void  rtl92e_fill_tx_desc(struct net_device *dev, struct tx_desc *pdesc,
+			  struct cb_desc *cb_desc, struct sk_buff *skb);
+void  rtl92e_fill_tx_cmd_desc(struct net_device *dev, struct tx_desc_cmd *entry,
+			      struct cb_desc *cb_desc, struct sk_buff *skb);
+bool rtl92e_get_rx_stats(struct net_device *dev, struct rtllib_rx_stats *stats,
+			 struct rx_desc *pdesc, struct sk_buff *skb);
+void rtl92e_stop_adapter(struct net_device *dev, bool reset);
+void rtl92e_update_ratr_table(struct net_device *dev);
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
new file mode 100644
index 000000000..789d288d7
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -0,0 +1,189 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_hw.h"
+#include "r8192E_hwimg.h"
+#include "r8192E_firmware.h"
+#include "r8192E_cmdpkt.h"
+#include <linux/firmware.h>
+
+static bool _rtl92e_wait_for_fw(struct net_device *dev, u32 mask, u32 timeout)
+{
+	unsigned long deadline = jiffies + msecs_to_jiffies(timeout);
+
+	while (time_before(jiffies, deadline)) {
+		if (rtl92e_readl(dev, CPU_GEN) & mask)
+			return true;
+		mdelay(2);
+	}
+	return false;
+}
+
+static bool _rtl92e_fw_boot_cpu(struct net_device *dev)
+{
+	u32		CPU_status = 0;
+
+	if (!_rtl92e_wait_for_fw(dev, CPU_GEN_PUT_CODE_OK, 200)) {
+		netdev_err(dev, "Firmware download failed.\n");
+		return false;
+	}
+	netdev_dbg(dev, "Download Firmware: Put code ok!\n");
+
+	CPU_status = rtl92e_readl(dev, CPU_GEN);
+	rtl92e_writeb(dev, CPU_GEN, (CPU_status | CPU_GEN_PWR_STB_CPU) & 0xff);
+	mdelay(1);
+
+	if (!_rtl92e_wait_for_fw(dev, CPU_GEN_BOOT_RDY, 200)) {
+		netdev_err(dev, "Firmware boot failed.\n");
+		return false;
+	}
+
+	netdev_dbg(dev, "Download Firmware: Boot ready!\n");
+
+	return true;
+}
+
+static bool _rtl92e_fw_check_ready(struct net_device *dev,
+				   u8 load_fw_status)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_firmware *pfirmware = priv->pFirmware;
+	bool rt_status  = true;
+
+	switch (load_fw_status) {
+	case FW_INIT_STEP0_BOOT:
+		pfirmware->status = FW_STATUS_1_MOVE_BOOT_CODE;
+		break;
+
+	case FW_INIT_STEP1_MAIN:
+		pfirmware->status = FW_STATUS_2_MOVE_MAIN_CODE;
+
+		rt_status = _rtl92e_fw_boot_cpu(dev);
+		if (rt_status)
+			pfirmware->status = FW_STATUS_3_TURNON_CPU;
+		else
+			netdev_dbg(dev, "_rtl92e_fw_boot_cpu fail!\n");
+
+		break;
+
+	case FW_INIT_STEP2_DATA:
+		pfirmware->status = FW_STATUS_4_MOVE_DATA_CODE;
+		mdelay(1);
+
+		rt_status = _rtl92e_wait_for_fw(dev, CPU_GEN_FIRM_RDY, 20);
+		if (rt_status)
+			pfirmware->status = FW_STATUS_5_READY;
+		break;
+	default:
+		rt_status = false;
+		netdev_dbg(dev, "Unknown firmware status");
+		break;
+	}
+
+	return rt_status;
+}
+
+static bool _rtl92e_fw_prepare(struct net_device *dev, struct rt_fw_blob *blob,
+			       const char *name, u8 padding)
+{
+	const struct firmware *fw;
+	int rc, i;
+	bool ret = true;
+
+	rc = request_firmware(&fw, name, &dev->dev);
+	if (rc < 0)
+		return false;
+
+	if (round_up(fw->size, 4) > MAX_FW_SIZE - padding) {
+		netdev_err(dev, "Firmware image %s too big for the device.\n",
+			   name);
+		ret = false;
+		goto out;
+	}
+
+	if (padding)
+		memset(blob->data, 0, padding);
+	if (fw->size % 4)
+		memset(blob->data + padding + fw->size, 0, 4);
+	memcpy(blob->data + padding, fw->data, fw->size);
+
+	blob->size = round_up(fw->size, 4) + padding;
+
+	/* Swap endian - firmware is packaged in invalid endiannes*/
+	for (i = padding; i < blob->size; i += 4) {
+		u32 *data = (u32 *)(blob->data + i);
+		*data = swab32p(data);
+	}
+out:
+	release_firmware(fw);
+	return ret;
+}
+
+bool rtl92e_init_fw(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	bool			rt_status = true;
+
+	u32	file_length = 0;
+	u8	*mapped_file = NULL;
+	u8	i = 0;
+	enum opt_rst_type rst_opt = OPT_SYSTEM_RESET;
+	enum firmware_init_step starting_state = FW_INIT_STEP0_BOOT;
+
+	struct rt_firmware *pfirmware = priv->pFirmware;
+
+	netdev_dbg(dev, " PlatformInitFirmware()==>\n");
+
+	if (pfirmware->status == FW_STATUS_0_INIT) {
+		rst_opt = OPT_SYSTEM_RESET;
+		starting_state = FW_INIT_STEP0_BOOT;
+
+	} else if (pfirmware->status == FW_STATUS_5_READY) {
+		rst_opt = OPT_FIRMWARE_RESET;
+		starting_state = FW_INIT_STEP2_DATA;
+	}
+
+	for (i = starting_state; i <= FW_INIT_STEP2_DATA; i++) {
+		if (rst_opt == OPT_SYSTEM_RESET) {
+			if (pfirmware->blobs[i].size == 0) {
+				const char *fw_name[3] = {
+					RTL8192E_BOOT_IMG_FW,
+					RTL8192E_MAIN_IMG_FW,
+					RTL8192E_DATA_IMG_FW
+				};
+				int pad = 0;
+
+				if (i == FW_INIT_STEP1_MAIN)
+					pad = 128;
+
+				if (!_rtl92e_fw_prepare(dev,
+							&pfirmware->blobs[i],
+							fw_name[i],
+							pad))
+					goto download_firmware_fail;
+			}
+		}
+
+		mapped_file = pfirmware->blobs[i].data;
+		file_length = pfirmware->blobs[i].size;
+
+		rt_status = rtl92e_send_cmd_pkt(dev, DESC_PACKET_TYPE_INIT,
+						mapped_file, file_length);
+		if (!rt_status)
+			goto download_firmware_fail;
+
+		if (!_rtl92e_fw_check_ready(dev, i))
+			goto download_firmware_fail;
+	}
+
+	netdev_dbg(dev, "Firmware Download Success\n");
+	return rt_status;
+
+download_firmware_fail:
+	netdev_err(dev, "%s: Failed to initialize firmware.\n", __func__);
+	return false;
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
new file mode 100644
index 000000000..b9059abc9
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef __INC_FIRMWARE_H
+#define __INC_FIRMWARE_H
+
+#define RTL8192E_BOOT_IMG_FW	"RTL8192E/boot.img"
+#define RTL8192E_MAIN_IMG_FW	"RTL8192E/main.img"
+#define RTL8192E_DATA_IMG_FW	"RTL8192E/data.img"
+
+enum firmware_init_step {
+	FW_INIT_STEP0_BOOT = 0,
+	FW_INIT_STEP1_MAIN = 1,
+	FW_INIT_STEP2_DATA = 2,
+};
+
+enum opt_rst_type {
+	OPT_SYSTEM_RESET = 0,
+	OPT_FIRMWARE_RESET = 1,
+};
+
+enum desc_packet_type {
+	DESC_PACKET_TYPE_INIT = 0,
+	DESC_PACKET_TYPE_NORMAL = 1,
+};
+
+enum firmware_status {
+	FW_STATUS_0_INIT = 0,
+	FW_STATUS_1_MOVE_BOOT_CODE = 1,
+	FW_STATUS_2_MOVE_MAIN_CODE = 2,
+	FW_STATUS_3_TURNON_CPU = 3,
+	FW_STATUS_4_MOVE_DATA_CODE = 4,
+	FW_STATUS_5_READY = 5,
+};
+
+#define MAX_FW_SIZE 64000
+struct rt_fw_blob {
+	u16 size;
+	u8 data[MAX_FW_SIZE];
+};
+
+#define FW_BLOBS 3
+struct rt_firmware {
+	enum firmware_status status;
+	struct rt_fw_blob blobs[FW_BLOBS];
+};
+
+bool rtl92e_init_fw(struct net_device *dev);
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
new file mode 100644
index 000000000..3e223151d
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
@@ -0,0 +1,439 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef R8180_HW
+#define R8180_HW
+
+enum baseband_config {
+	BaseBand_Config_PHY_REG = 0,
+	BaseBand_Config_AGC_TAB = 1,
+};
+
+#define	RTL8187_REQT_READ	0xc0
+#define	RTL8187_REQT_WRITE	0x40
+#define	RTL8187_REQ_GET_REGS	0x05
+#define	RTL8187_REQ_SET_REGS	0x05
+
+#define MAX_TX_URB 5
+#define MAX_RX_URB 16
+#define RX_URB_SIZE 9100
+
+#define BB_ANTATTEN_CHAN14	0x0c
+#define BB_ANTENNA_B 0x40
+
+#define BB_HOST_BANG (1<<30)
+#define BB_HOST_BANG_EN (1<<2)
+#define BB_HOST_BANG_CLK (1<<1)
+#define BB_HOST_BANG_RW (1<<3)
+#define BB_HOST_BANG_DATA	 1
+
+#define RTL8190_EEPROM_ID	0x8129
+#define EEPROM_VID		0x02
+#define EEPROM_DID		0x04
+#define EEPROM_NODE_ADDRESS_BYTE_0	0x0C
+
+#define EEPROM_TxPowerDiff	0x1F
+
+
+#define EEPROM_PwDiff		0x21
+#define EEPROM_CrystalCap	0x22
+
+
+
+#define EEPROM_TxPwIndex_CCK_V1		0x29
+#define EEPROM_TxPwIndex_OFDM_24G_V1	0x2C
+#define EEPROM_TxPwIndex_Ver		0x27
+
+#define EEPROM_Default_TxPowerDiff		0x0
+#define EEPROM_Default_ThermalMeter		0x77
+#define EEPROM_Default_AntTxPowerDiff		0x0
+#define EEPROM_Default_TxPwDiff_CrystalCap	0x5
+#define EEPROM_Default_PwDiff			0x4
+#define EEPROM_Default_CrystalCap		0x5
+#define EEPROM_Default_TxPower			0x1010
+#define EEPROM_ICVersion_ChannelPlan	0x7C
+#define EEPROM_Customer_ID			0x7B
+#define EEPROM_RFInd_PowerDiff			0x28
+#define EEPROM_ThermalMeter			0x29
+#define EEPROM_TxPwDiff_CrystalCap		0x2A
+#define EEPROM_TxPwIndex_CCK			0x2C
+#define EEPROM_TxPwIndex_OFDM_24G	0x3A
+#define EEPROM_Default_TxPowerLevel		0x10
+#define EEPROM_IC_VER				0x7d
+#define EEPROM_CRC				0x7e
+
+#define EEPROM_CID_DEFAULT			0x0
+#define EEPROM_CID_CAMEO				0x1
+#define EEPROM_CID_RUNTOP				0x2
+#define EEPROM_CID_Senao				0x3
+#define EEPROM_CID_TOSHIBA				0x4
+#define EEPROM_CID_NetCore				0x5
+#define EEPROM_CID_Nettronix			0x6
+#define EEPROM_CID_Pronet				0x7
+#define EEPROM_CID_DLINK				0x8
+#define EEPROM_CID_WHQL					0xFE
+enum _RTL8192Pci_HW {
+	MAC0			= 0x000,
+	MAC1			= 0x001,
+	MAC2			= 0x002,
+	MAC3			= 0x003,
+	MAC4			= 0x004,
+	MAC5			= 0x005,
+	PCIF			= 0x009,
+#define MXDMA2_16bytes		0x000
+#define MXDMA2_32bytes		0x001
+#define MXDMA2_64bytes		0x010
+#define MXDMA2_128bytes		0x011
+#define MXDMA2_256bytes		0x100
+#define MXDMA2_512bytes		0x101
+#define MXDMA2_1024bytes	0x110
+#define MXDMA2_NoLimit		0x7
+
+#define	MULRW_SHIFT		3
+#define	MXDMA2_RX_SHIFT		4
+#define	MXDMA2_TX_SHIFT		0
+	PMR			= 0x00c,
+	EPROM_CMD		= 0x00e,
+#define EPROM_CMD_RESERVED_MASK BIT5
+#define EPROM_CMD_9356SEL	BIT4
+#define EPROM_CMD_OPERATING_MODE_SHIFT 6
+#define EPROM_CMD_OPERATING_MODE_MASK ((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG 0x3
+#define EPROM_CMD_NORMAL 0
+#define EPROM_CMD_LOAD 1
+#define EPROM_CMD_PROGRAM 2
+#define EPROM_CS_BIT 3
+#define EPROM_CK_BIT 2
+#define EPROM_W_BIT 1
+#define EPROM_R_BIT 0
+
+	AFR			 = 0x010,
+#define AFR_CardBEn		(1<<0)
+#define AFR_CLKRUN_SEL		(1<<1)
+#define AFR_FuncRegEn		(1<<2)
+
+	ANAPAR			= 0x17,
+#define	BB_GLOBAL_RESET_BIT	0x1
+	BB_GLOBAL_RESET		= 0x020,
+	BSSIDR			= 0x02E,
+	CMDR			= 0x037,
+#define		CR_RST					0x10
+#define		CR_RE					0x08
+#define		CR_TE					0x04
+#define		CR_MulRW				0x01
+	SIFS		= 0x03E,
+	TCR			= 0x040,
+	RCR			= 0x044,
+#define RCR_FILTER_MASK (BIT0 | BIT1 | BIT2 | BIT3 | BIT5 | BIT12 |	\
+			BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23)
+#define RCR_ONLYERLPKT		BIT31
+#define RCR_ENCS2		BIT30
+#define RCR_ENCS1		BIT29
+#define RCR_ENMBID		BIT27
+#define RCR_ACKTXBW		(BIT24|BIT25)
+#define RCR_CBSSID		BIT23
+#define RCR_APWRMGT		BIT22
+#define	RCR_ADD3		BIT21
+#define RCR_AMF			BIT20
+#define RCR_ACF			BIT19
+#define RCR_ADF			BIT18
+#define RCR_RXFTH		BIT13
+#define RCR_AICV		BIT12
+#define	RCR_ACRC32		BIT5
+#define	RCR_AB			BIT3
+#define	RCR_AM			BIT2
+#define	RCR_APM			BIT1
+#define	RCR_AAP			BIT0
+#define RCR_MXDMA_OFFSET	8
+#define RCR_FIFO_OFFSET		13
+	SLOT_TIME		= 0x049,
+	ACK_TIMEOUT		= 0x04c,
+	PIFS_TIME		= 0x04d,
+	USTIME			= 0x04e,
+	EDCAPARA_BE		= 0x050,
+	EDCAPARA_BK		= 0x054,
+	EDCAPARA_VO		= 0x058,
+	EDCAPARA_VI		= 0x05C,
+#define	AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define	AC_PARAM_ECW_MAX_OFFSET		12
+#define	AC_PARAM_ECW_MIN_OFFSET			8
+#define	AC_PARAM_AIFS_OFFSET				0
+	RFPC			= 0x05F,
+	CWRR			= 0x060,
+	BCN_TCFG		= 0x062,
+#define BCN_TCFG_CW_SHIFT		8
+#define BCN_TCFG_IFS			0
+	BCN_INTERVAL		= 0x070,
+	ATIMWND			= 0x072,
+	BCN_DRV_EARLY_INT	= 0x074,
+#define	BCN_DRV_EARLY_INT_SWBCN_SHIFT	8
+#define	BCN_DRV_EARLY_INT_TIME_SHIFT	0
+	BCN_DMATIME		= 0x076,
+	BCN_ERR_THRESH		= 0x078,
+	RWCAM			= 0x0A0,
+#define   CAM_CM_SecCAMPolling		BIT31
+#define   CAM_CM_SecCAMClr			BIT30
+#define   CAM_CM_SecCAMWE			BIT16
+#define   CAM_VALID			       BIT15
+#define   CAM_NOTVALID			0x0000
+#define   CAM_USEDK				BIT5
+
+#define   CAM_NONE				0x0
+#define   CAM_WEP40				0x01
+#define   CAM_TKIP				0x02
+#define   CAM_AES				0x04
+#define   CAM_WEP104			0x05
+
+#define   TOTAL_CAM_ENTRY				32
+
+#define   CAM_CONFIG_USEDK	true
+#define   CAM_CONFIG_NO_USEDK	false
+#define   CAM_WRITE		BIT16
+#define   CAM_READ		0x00000000
+#define   CAM_POLLINIG		BIT31
+#define   SCR_UseDK		0x01
+	WCAMI			= 0x0A4,
+	RCAMO			= 0x0A8,
+	SECR			= 0x0B0,
+#define	SCR_TxUseDK			BIT0
+#define   SCR_RxUseDK			BIT1
+#define   SCR_TxEncEnable		BIT2
+#define   SCR_RxDecEnable		BIT3
+#define   SCR_SKByA2				BIT4
+#define   SCR_NoSKMC				BIT5
+	SWREGULATOR	= 0x0BD,
+	INTA_MASK		= 0x0f4,
+#define IMR8190_DISABLED		0x0
+#define IMR_ATIMEND			BIT28
+#define IMR_TBDOK			BIT27
+#define IMR_TBDER			BIT26
+#define IMR_TXFOVW			BIT15
+#define IMR_TIMEOUT0			BIT14
+#define IMR_BcnInt			BIT13
+#define	IMR_RXFOVW			BIT12
+#define IMR_RDU				BIT11
+#define IMR_RXCMDOK			BIT10
+#define IMR_BDOK			BIT9
+#define IMR_HIGHDOK			BIT8
+#define	IMR_COMDOK			BIT7
+#define IMR_MGNTDOK			BIT6
+#define IMR_HCCADOK			BIT5
+#define	IMR_BKDOK			BIT4
+#define	IMR_BEDOK			BIT3
+#define	IMR_VIDOK			BIT2
+#define	IMR_VODOK			BIT1
+#define	IMR_ROK				BIT0
+	ISR			= 0x0f8,
+	TPPoll			= 0x0fd,
+#define TPPoll_BKQ		BIT0
+#define TPPoll_BEQ		BIT1
+#define TPPoll_VIQ		BIT2
+#define TPPoll_VOQ		BIT3
+#define TPPoll_BQ		BIT4
+#define TPPoll_CQ		BIT5
+#define TPPoll_MQ		BIT6
+#define TPPoll_HQ		BIT7
+#define TPPoll_HCCAQ		BIT8
+#define TPPoll_StopBK	BIT9
+#define TPPoll_StopBE	BIT10
+#define TPPoll_StopVI		BIT11
+#define TPPoll_StopVO	BIT12
+#define TPPoll_StopMgt	BIT13
+#define TPPoll_StopHigh	BIT14
+#define TPPoll_StopHCCA	BIT15
+#define TPPoll_SHIFT		8
+
+	PSR			= 0x0ff,
+#define PSR_GEN			0x0
+#define PSR_CPU			0x1
+	CPU_GEN			= 0x100,
+	BB_RESET			= 0x101,
+#define	CPU_CCK_LOOPBACK	0x00030000
+#define	CPU_GEN_SYSTEM_RESET	0x00000001
+#define	CPU_GEN_FIRMWARE_RESET	0x00000008
+#define	CPU_GEN_BOOT_RDY	0x00000010
+#define	CPU_GEN_FIRM_RDY	0x00000020
+#define	CPU_GEN_PUT_CODE_OK	0x00000080
+#define	CPU_GEN_BB_RST		0x00000100
+#define	CPU_GEN_PWR_STB_CPU	0x00000004
+#define CPU_GEN_NO_LOOPBACK_MSK	0xFFF8FFFF
+#define CPU_GEN_NO_LOOPBACK_SET	0x00080000
+#define	CPU_GEN_GPIO_UART		0x00007000
+
+	LED1Cfg			= 0x154,
+	LED0Cfg			= 0x155,
+
+	AcmAvg			= 0x170,
+	AcmHwCtrl		= 0x171,
+#define	AcmHw_HwEn		BIT0
+#define	AcmHw_BeqEn		BIT1
+#define	AcmHw_ViqEn		BIT2
+#define	AcmHw_VoqEn		BIT3
+#define	AcmHw_BeqStatus		BIT4
+#define	AcmHw_ViqStatus		BIT5
+#define	AcmHw_VoqStatus		BIT6
+	AcmFwCtrl		= 0x172,
+#define	AcmFw_BeqStatus		BIT0
+#define	AcmFw_ViqStatus		BIT1
+#define	AcmFw_VoqStatus		BIT2
+	VOAdmTime		= 0x174,
+	VIAdmTime		= 0x178,
+	BEAdmTime		= 0x17C,
+	RQPN1			= 0x180,
+	RQPN2			= 0x184,
+	RQPN3			= 0x188,
+	QPRR			= 0x1E0,
+	QPNR			= 0x1F0,
+	BQDA			= 0x200,
+	HQDA			= 0x204,
+	CQDA			= 0x208,
+	MQDA			= 0x20C,
+	HCCAQDA			= 0x210,
+	VOQDA			= 0x214,
+	VIQDA			= 0x218,
+	BEQDA			= 0x21C,
+	BKQDA			= 0x220,
+	RCQDA			= 0x224,
+	RDQDA			= 0x228,
+
+	MAR0			= 0x240,
+	MAR4			= 0x244,
+
+	CCX_PERIOD		= 0x250,
+	CLM_RESULT		= 0x251,
+	NHM_PERIOD		= 0x252,
+
+	NHM_THRESHOLD0		= 0x253,
+	NHM_THRESHOLD1		= 0x254,
+	NHM_THRESHOLD2		= 0x255,
+	NHM_THRESHOLD3		= 0x256,
+	NHM_THRESHOLD4		= 0x257,
+	NHM_THRESHOLD5		= 0x258,
+	NHM_THRESHOLD6		= 0x259,
+
+	MCTRL			= 0x25A,
+
+	NHM_RPI_COUNTER0	= 0x264,
+	NHM_RPI_COUNTER1	= 0x265,
+	NHM_RPI_COUNTER2	= 0x266,
+	NHM_RPI_COUNTER3	= 0x267,
+	NHM_RPI_COUNTER4	= 0x268,
+	NHM_RPI_COUNTER5	= 0x269,
+	NHM_RPI_COUNTER6	= 0x26A,
+	NHM_RPI_COUNTER7	= 0x26B,
+	WFCRC0		  = 0x2f0,
+	WFCRC1		  = 0x2f4,
+	WFCRC2		  = 0x2f8,
+
+	BW_OPMODE		= 0x300,
+#define	BW_OPMODE_11J			BIT0
+#define	BW_OPMODE_5G			BIT1
+#define	BW_OPMODE_20MHZ			BIT2
+	IC_VERRSION		= 0x301,
+	MSR			= 0x303,
+#define MSR_LINK_MASK      ((1<<0)|(1<<1))
+#define MSR_LINK_MANAGED   2
+#define MSR_LINK_NONE      0
+#define MSR_LINK_SHIFT     0
+#define MSR_LINK_ADHOC     1
+#define MSR_LINK_MASTER    3
+#define MSR_LINK_ENEDCA	   (1<<4)
+
+#define	MSR_NOLINK					0x00
+#define	MSR_ADHOC					0x01
+#define	MSR_INFRA					0x02
+#define	MSR_AP						0x03
+
+	RETRY_LIMIT		= 0x304,
+#define RETRY_LIMIT_SHORT_SHIFT 8
+#define RETRY_LIMIT_LONG_SHIFT 0
+	TSFR			= 0x308,
+	RRSR			= 0x310,
+#define RRSR_RSC_OFFSET				21
+#define RRSR_SHORT_OFFSET			23
+#define RRSR_RSC_DUPLICATE			0x600000
+#define RRSR_RSC_UPSUBCHNL			0x400000
+#define RRSR_RSC_LOWSUBCHNL			0x200000
+#define RRSR_SHORT				0x800000
+#define RRSR_1M					BIT0
+#define RRSR_2M					BIT1
+#define RRSR_5_5M				BIT2
+#define RRSR_11M				BIT3
+#define RRSR_6M					BIT4
+#define RRSR_9M					BIT5
+#define RRSR_12M				BIT6
+#define RRSR_18M				BIT7
+#define RRSR_24M				BIT8
+#define RRSR_36M				BIT9
+#define RRSR_48M				BIT10
+#define RRSR_54M				BIT11
+#define RRSR_MCS0				BIT12
+#define RRSR_MCS1				BIT13
+#define RRSR_MCS2				BIT14
+#define RRSR_MCS3				BIT15
+#define RRSR_MCS4				BIT16
+#define RRSR_MCS5				BIT17
+#define RRSR_MCS6				BIT18
+#define RRSR_MCS7				BIT19
+#define BRSR_AckShortPmb			BIT23
+	UFWP			= 0x318,
+	RATR0			= 0x320,
+#define	RATR_1M			0x00000001
+#define	RATR_2M			0x00000002
+#define	RATR_55M		0x00000004
+#define	RATR_11M		0x00000008
+#define	RATR_6M			0x00000010
+#define	RATR_9M			0x00000020
+#define	RATR_12M		0x00000040
+#define	RATR_18M		0x00000080
+#define	RATR_24M		0x00000100
+#define	RATR_36M		0x00000200
+#define	RATR_48M		0x00000400
+#define	RATR_54M		0x00000800
+#define	RATR_MCS0		0x00001000
+#define	RATR_MCS1		0x00002000
+#define	RATR_MCS2		0x00004000
+#define	RATR_MCS3		0x00008000
+#define	RATR_MCS4		0x00010000
+#define	RATR_MCS5		0x00020000
+#define	RATR_MCS6		0x00040000
+#define	RATR_MCS7		0x00080000
+#define	RATR_MCS8		0x00100000
+#define	RATR_MCS9		0x00200000
+#define	RATR_MCS10		0x00400000
+#define	RATR_MCS11		0x00800000
+#define	RATR_MCS12		0x01000000
+#define	RATR_MCS13		0x02000000
+#define	RATR_MCS14		0x04000000
+#define	RATR_MCS15		0x08000000
+#define RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M | \
+				RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |	\
+				RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |	\
+				RATR_MCS6 | RATR_MCS7)
+#define RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |	\
+				RATR_MCS11 | RATR_MCS12 | RATR_MCS13 |	\
+				RATR_MCS14|RATR_MCS15)
+
+
+	DRIVER_RSSI		= 0x32c,
+	MCS_TXAGC		= 0x340,
+	CCK_TXAGC		= 0x348,
+	MacBlkCtrl		= 0x403,
+
+}
+;
+
+#define GPI 0x108
+#define GPO 0x109
+#define GPE 0x10a
+
+#define	 HWSET_MAX_SIZE_92S				128
+
+#define	ANAPAR_FOR_8192PciE				0x17
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
new file mode 100644
index 000000000..e6fce749e
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
@@ -0,0 +1,551 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "r8192E_hwimg.h"
+
+u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE] = {0x0,};
+
+u32 Rtl8192PciEPHY_REG_1T2RArray[PHY_REG_1T2RArrayLengthPciE] = {
+	0x800, 0x00000000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000001c,
+	0x810, 0x801010aa,
+	0x814, 0x008514d0,
+	0x818, 0x00000040,
+	0x81c, 0x00000000,
+	0x820, 0x00000004,
+	0x824, 0x00690000,
+	0x828, 0x00000004,
+	0x82c, 0x00e90000,
+	0x830, 0x00000004,
+	0x834, 0x00690000,
+	0x838, 0x00000004,
+	0x83c, 0x00e90000,
+	0x840, 0x00000000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x65a965a9,
+	0x85c, 0x65a965a9,
+	0x860, 0x001f0010,
+	0x864, 0x007f0010,
+	0x868, 0x001f0010,
+	0x86c, 0x007f0010,
+	0x870, 0x0f100f70,
+	0x874, 0x0f100f70,
+	0x878, 0x00000000,
+	0x87c, 0x00000000,
+	0x880, 0x6870e36c,
+	0x884, 0xe3573600,
+	0x888, 0x4260c340,
+	0x88c, 0x0000ff00,
+	0x890, 0x00000000,
+	0x894, 0xfffffffe,
+	0x898, 0x4c42382f,
+	0x89c, 0x00656056,
+	0x8b0, 0x00000000,
+	0x8e0, 0x00000000,
+	0x8e4, 0x00000000,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x31121311,
+	0xa00, 0x00d0c7d8,
+	0xa04, 0x811f0008,
+	0xa08, 0x80cd8300,
+	0xa0c, 0x2e62740f,
+	0xa10, 0x95009b78,
+	0xa14, 0x11145008,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140fa0,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00000000,
+	0xc00, 0x00000040,
+	0xc04, 0x00005433,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08000000,
+	0xc1c, 0x40000100,
+	0xc20, 0x08000000,
+	0xc24, 0x40000100,
+	0xc28, 0x08000000,
+	0xc2c, 0x40000100,
+	0xc30, 0x6de9ac44,
+	0xc34, 0x465c52cd,
+	0xc38, 0x497f5994,
+	0xc3c, 0x0a969764,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020000,
+	0xc4c, 0x00000300,
+	0xc50, 0x69543420,
+	0xc54, 0x433c0094,
+	0xc58, 0x69543420,
+	0xc5c, 0x433c0094,
+	0xc60, 0x69543420,
+	0xc64, 0x433c0094,
+	0xc68, 0x69543420,
+	0xc6c, 0x433c0094,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x0186175b,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x40000100,
+	0xc84, 0x20000000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x40000100,
+	0xc94, 0x00000000,
+	0xc98, 0x40000100,
+	0xc9c, 0x00000000,
+	0xca0, 0x00492492,
+	0xca4, 0x00000000,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x00492492,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xd00, 0x00000750,
+	0xd04, 0x00000403,
+	0xd08, 0x0000907f,
+	0xd0c, 0x00000001,
+	0xd10, 0xa0633333,
+	0xd14, 0x33333c63,
+	0xd18, 0x6a8f5b6b,
+	0xd1c, 0x00000000,
+	0xd20, 0x00000000,
+	0xd24, 0x00000000,
+	0xd28, 0x00000000,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x00000000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x024dbd02,
+	0xd58, 0x00000000,
+	0xd5c, 0x04032064,
+	0xe00, 0x161a1a1a,
+	0xe04, 0x12121416,
+	0xe08, 0x00001800,
+	0xe0c, 0x00000000,
+	0xe10, 0x161a1a1a,
+	0xe14, 0x12121416,
+	0xe18, 0x161a1a1a,
+	0xe1c, 0x12121416,
+};
+
+u32 Rtl8192PciERadioA_Array[RadioA_ArrayLengthPciE] = {
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x00000ee0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x009, 0x000007f0,
+	0x00a, 0x000009d0,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x00f, 0x00000990,
+	0x012, 0x00000806,
+	0x014, 0x000005ab,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000f80,
+	0x01b, 0x00000f5e,
+	0x01c, 0x00000008,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x01f, 0x00000000,
+	0x020, 0x000001a5,
+	0x01f, 0x00000001,
+	0x020, 0x00000165,
+	0x01f, 0x00000002,
+	0x020, 0x000000c6,
+	0x01f, 0x00000003,
+	0x020, 0x00000086,
+	0x01f, 0x00000004,
+	0x020, 0x00000046,
+	0x01f, 0x00000005,
+	0x020, 0x000001e6,
+	0x01f, 0x00000006,
+	0x020, 0x000001a6,
+	0x01f, 0x00000007,
+	0x020, 0x00000166,
+	0x01f, 0x00000008,
+	0x020, 0x000000c7,
+	0x01f, 0x00000009,
+	0x020, 0x00000087,
+	0x01f, 0x0000000a,
+	0x020, 0x000000f7,
+	0x01f, 0x0000000b,
+	0x020, 0x000000d7,
+	0x01f, 0x0000000c,
+	0x020, 0x000000b7,
+	0x01f, 0x0000000d,
+	0x020, 0x00000097,
+	0x01f, 0x0000000e,
+	0x020, 0x00000077,
+	0x01f, 0x0000000f,
+	0x020, 0x00000057,
+	0x01f, 0x00000010,
+	0x020, 0x00000037,
+	0x01f, 0x00000011,
+	0x020, 0x000000fb,
+	0x01f, 0x00000012,
+	0x020, 0x000000db,
+	0x01f, 0x00000013,
+	0x020, 0x000000bb,
+	0x01f, 0x00000014,
+	0x020, 0x000000ff,
+	0x01f, 0x00000015,
+	0x020, 0x000000e3,
+	0x01f, 0x00000016,
+	0x020, 0x000000c3,
+	0x01f, 0x00000017,
+	0x020, 0x000000a3,
+	0x01f, 0x00000018,
+	0x020, 0x00000083,
+	0x01f, 0x00000019,
+	0x020, 0x00000063,
+	0x01f, 0x0000001a,
+	0x020, 0x00000043,
+	0x01f, 0x0000001b,
+	0x020, 0x00000023,
+	0x01f, 0x0000001c,
+	0x020, 0x00000003,
+	0x01f, 0x0000001d,
+	0x020, 0x000001e3,
+	0x01f, 0x0000001e,
+	0x020, 0x000001c3,
+	0x01f, 0x0000001f,
+	0x020, 0x000001a3,
+	0x01f, 0x00000020,
+	0x020, 0x00000183,
+	0x01f, 0x00000021,
+	0x020, 0x00000163,
+	0x01f, 0x00000022,
+	0x020, 0x00000143,
+	0x01f, 0x00000023,
+	0x020, 0x00000123,
+	0x01f, 0x00000024,
+	0x020, 0x00000103,
+	0x023, 0x00000203,
+	0x024, 0x00000100,
+	0x00b, 0x000001ba,
+	0x02c, 0x000003d7,
+	0x02d, 0x00000ff0,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x007, 0x00000080,
+	0x002, 0x0000088d,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x000, 0x000000bf,
+	0x00d, 0x0000001f,
+	0x00d, 0x00000c9f,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
+};
+
+u32 Rtl8192PciERadioB_Array[RadioB_ArrayLengthPciE] = {
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x000006e0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x000, 0x000000b7,
+	0x00a, 0x00000850,
+	0x000, 0x000000bf,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000e00,
+	0x01b, 0x00000f5e,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x00b, 0x000001ba,
+	0x023, 0x00000203,
+	0x024, 0x00000100,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x00d, 0x00000ccc,
+	0x000, 0x000000bf,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
+};
+
+u32 Rtl8192PciERadioC_Array[RadioC_ArrayLengthPciE] = {
+	0x0,  };
+
+u32 Rtl8192PciERadioD_Array[RadioD_ArrayLengthPciE] = {
+	0x0, };
+
+u32 Rtl8192PciEMACPHY_Array[] = {
+	0x03c, 0xffff0000, 0x00000f0f,
+	0x340, 0xffffffff, 0x161a1a1a,
+	0x344, 0xffffffff, 0x12121416,
+	0x348, 0x0000ffff, 0x00001818,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000100,
+};
+
+u32 Rtl8192PciEMACPHY_Array_PG[] = {
+	0x03c, 0xffff0000, 0x00000f0f,
+	0xe00, 0xffffffff, 0x06090909,
+	0xe04, 0xffffffff, 0x00030306,
+	0xe08, 0x0000ff00, 0x00000000,
+	0xe10, 0xffffffff, 0x0a0c0d0f,
+	0xe14, 0xffffffff, 0x06070809,
+	0xe18, 0xffffffff, 0x0a0c0d0f,
+	0xe1c, 0xffffffff, 0x06070809,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000800,
+};
+
+u32 Rtl8192PciEAGCTAB_Array[AGCTAB_ArrayLengthPciE] = {
+	0xc78, 0x7d000001,
+	0xc78, 0x7d010001,
+	0xc78, 0x7d020001,
+	0xc78, 0x7d030001,
+	0xc78, 0x7d040001,
+	0xc78, 0x7d050001,
+	0xc78, 0x7c060001,
+	0xc78, 0x7b070001,
+	0xc78, 0x7a080001,
+	0xc78, 0x79090001,
+	0xc78, 0x780a0001,
+	0xc78, 0x770b0001,
+	0xc78, 0x760c0001,
+	0xc78, 0x750d0001,
+	0xc78, 0x740e0001,
+	0xc78, 0x730f0001,
+	0xc78, 0x72100001,
+	0xc78, 0x71110001,
+	0xc78, 0x70120001,
+	0xc78, 0x6f130001,
+	0xc78, 0x6e140001,
+	0xc78, 0x6d150001,
+	0xc78, 0x6c160001,
+	0xc78, 0x6b170001,
+	0xc78, 0x6a180001,
+	0xc78, 0x69190001,
+	0xc78, 0x681a0001,
+	0xc78, 0x671b0001,
+	0xc78, 0x661c0001,
+	0xc78, 0x651d0001,
+	0xc78, 0x641e0001,
+	0xc78, 0x491f0001,
+	0xc78, 0x48200001,
+	0xc78, 0x47210001,
+	0xc78, 0x46220001,
+	0xc78, 0x45230001,
+	0xc78, 0x44240001,
+	0xc78, 0x43250001,
+	0xc78, 0x28260001,
+	0xc78, 0x27270001,
+	0xc78, 0x26280001,
+	0xc78, 0x25290001,
+	0xc78, 0x242a0001,
+	0xc78, 0x232b0001,
+	0xc78, 0x222c0001,
+	0xc78, 0x212d0001,
+	0xc78, 0x202e0001,
+	0xc78, 0x0a2f0001,
+	0xc78, 0x08300001,
+	0xc78, 0x06310001,
+	0xc78, 0x05320001,
+	0xc78, 0x04330001,
+	0xc78, 0x03340001,
+	0xc78, 0x02350001,
+	0xc78, 0x01360001,
+	0xc78, 0x00370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7d400001,
+	0xc78, 0x7d410001,
+	0xc78, 0x7d420001,
+	0xc78, 0x7d430001,
+	0xc78, 0x7d440001,
+	0xc78, 0x7d450001,
+	0xc78, 0x7c460001,
+	0xc78, 0x7b470001,
+	0xc78, 0x7a480001,
+	0xc78, 0x79490001,
+	0xc78, 0x784a0001,
+	0xc78, 0x774b0001,
+	0xc78, 0x764c0001,
+	0xc78, 0x754d0001,
+	0xc78, 0x744e0001,
+	0xc78, 0x734f0001,
+	0xc78, 0x72500001,
+	0xc78, 0x71510001,
+	0xc78, 0x70520001,
+	0xc78, 0x6f530001,
+	0xc78, 0x6e540001,
+	0xc78, 0x6d550001,
+	0xc78, 0x6c560001,
+	0xc78, 0x6b570001,
+	0xc78, 0x6a580001,
+	0xc78, 0x69590001,
+	0xc78, 0x685a0001,
+	0xc78, 0x675b0001,
+	0xc78, 0x665c0001,
+	0xc78, 0x655d0001,
+	0xc78, 0x645e0001,
+	0xc78, 0x495f0001,
+	0xc78, 0x48600001,
+	0xc78, 0x47610001,
+	0xc78, 0x46620001,
+	0xc78, 0x45630001,
+	0xc78, 0x44640001,
+	0xc78, 0x43650001,
+	0xc78, 0x28660001,
+	0xc78, 0x27670001,
+	0xc78, 0x26680001,
+	0xc78, 0x25690001,
+	0xc78, 0x246a0001,
+	0xc78, 0x236b0001,
+	0xc78, 0x226c0001,
+	0xc78, 0x216d0001,
+	0xc78, 0x206e0001,
+	0xc78, 0x0a6f0001,
+	0xc78, 0x08700001,
+	0xc78, 0x06710001,
+	0xc78, 0x05720001,
+	0xc78, 0x04730001,
+	0xc78, 0x03740001,
+	0xc78, 0x02750001,
+	0xc78, 0x01760001,
+	0xc78, 0x00770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x2e00001e,
+	0xc78, 0x2e01001e,
+	0xc78, 0x2e02001e,
+	0xc78, 0x2e03001e,
+	0xc78, 0x2e04001e,
+	0xc78, 0x2e05001e,
+	0xc78, 0x3006001e,
+	0xc78, 0x3407001e,
+	0xc78, 0x3908001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3f0a001e,
+	0xc78, 0x420b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x450d001e,
+	0xc78, 0x460e001e,
+	0xc78, 0x460f001e,
+	0xc78, 0x4710001e,
+	0xc78, 0x4811001e,
+	0xc78, 0x4912001e,
+	0xc78, 0x4a13001e,
+	0xc78, 0x4b14001e,
+	0xc78, 0x4b15001e,
+	0xc78, 0x4c16001e,
+	0xc78, 0x4d17001e,
+	0xc78, 0x4e18001e,
+	0xc78, 0x4f19001e,
+	0xc78, 0x4f1a001e,
+	0xc78, 0x501b001e,
+	0xc78, 0x511c001e,
+	0xc78, 0x521d001e,
+	0xc78, 0x521e001e,
+	0xc78, 0x531f001e,
+	0xc78, 0x5320001e,
+	0xc78, 0x5421001e,
+	0xc78, 0x5522001e,
+	0xc78, 0x5523001e,
+	0xc78, 0x5624001e,
+	0xc78, 0x5725001e,
+	0xc78, 0x5726001e,
+	0xc78, 0x5827001e,
+	0xc78, 0x5828001e,
+	0xc78, 0x5929001e,
+	0xc78, 0x592a001e,
+	0xc78, 0x5a2b001e,
+	0xc78, 0x5b2c001e,
+	0xc78, 0x5c2d001e,
+	0xc78, 0x5c2e001e,
+	0xc78, 0x5d2f001e,
+	0xc78, 0x5e30001e,
+	0xc78, 0x5f31001e,
+	0xc78, 0x6032001e,
+	0xc78, 0x6033001e,
+	0xc78, 0x6134001e,
+	0xc78, 0x6235001e,
+	0xc78, 0x6336001e,
+	0xc78, 0x6437001e,
+	0xc78, 0x6438001e,
+	0xc78, 0x6539001e,
+	0xc78, 0x663a001e,
+	0xc78, 0x673b001e,
+	0xc78, 0x673c001e,
+	0xc78, 0x683d001e,
+	0xc78, 0x693e001e,
+	0xc78, 0x6a3f001e,
+};
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
new file mode 100644
index 000000000..7d63f5a5c
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef __INC_HAL8192PciE_FW_IMG_H
+#define __INC_HAL8192PciE_FW_IMG_H
+
+/*Created on  2008/11/18,  3: 7*/
+
+#include <linux/types.h>
+
+#define PHY_REGArrayLengthPciE 1
+extern u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE];
+#define PHY_REG_1T2RArrayLengthPciE 296
+extern u32 Rtl8192PciEPHY_REG_1T2RArray[PHY_REG_1T2RArrayLengthPciE];
+#define RadioA_ArrayLengthPciE 246
+extern u32 Rtl8192PciERadioA_Array[RadioA_ArrayLengthPciE];
+#define RadioB_ArrayLengthPciE 78
+extern u32 Rtl8192PciERadioB_Array[RadioB_ArrayLengthPciE];
+#define RadioC_ArrayLengthPciE 2
+extern u32 Rtl8192PciERadioC_Array[RadioC_ArrayLengthPciE];
+#define RadioD_ArrayLengthPciE 2
+extern u32 Rtl8192PciERadioD_Array[RadioD_ArrayLengthPciE];
+#define MACPHY_ArrayLengthPciE 18
+extern u32 Rtl8192PciEMACPHY_Array[MACPHY_ArrayLengthPciE];
+#define MACPHY_Array_PGLengthPciE 30
+extern u32 Rtl8192PciEMACPHY_Array_PG[MACPHY_Array_PGLengthPciE];
+#define AGCTAB_ArrayLengthPciE 384
+extern u32 Rtl8192PciEAGCTAB_Array[AGCTAB_ArrayLengthPciE];
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
new file mode 100644
index 000000000..1b592258e
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -0,0 +1,1480 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include <linux/bitops.h>
+#include "rtl_core.h"
+#include "r8192E_hw.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h"
+#include "r8192E_phy.h"
+#include "rtl_dm.h"
+
+#include "r8192E_hwimg.h"
+
+static u32 RF_CHANNEL_TABLE_ZEBRA[] = {
+	0,
+	0x085c,
+	0x08dc,
+	0x095c,
+	0x09dc,
+	0x0a5c,
+	0x0adc,
+	0x0b5c,
+	0x0bdc,
+	0x0c5c,
+	0x0cdc,
+	0x0d5c,
+	0x0ddc,
+	0x0e5c,
+	0x0f72,
+};
+
+/*************************Define local function prototype**********************/
+
+static u32 _rtl92e_phy_rf_fw_read(struct net_device *dev,
+				  enum rf90_radio_path eRFPath, u32 Offset);
+static void _rtl92e_phy_rf_fw_write(struct net_device *dev,
+				    enum rf90_radio_path eRFPath, u32 Offset,
+				    u32 Data);
+
+static u32 _rtl92e_calculate_bit_shift(u32 dwBitMask)
+{
+	if (!dwBitMask)
+		return 32;
+	return ffs(dwBitMask) - 1;
+}
+
+u8 rtl92e_is_legal_rf_path(struct net_device *dev, u32 eRFPath)
+{
+	u8 ret = 1;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->rf_type == RF_2T4R)
+		ret = 0;
+	else if (priv->rf_type == RF_1T2R) {
+		if (eRFPath == RF90_PATH_A || eRFPath == RF90_PATH_B)
+			ret = 1;
+		else if (eRFPath == RF90_PATH_C || eRFPath == RF90_PATH_D)
+			ret = 0;
+	}
+	return ret;
+}
+
+void rtl92e_set_bb_reg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask,
+		       u32 dwData)
+{
+
+	u32 OriginalValue, BitShift, NewValue;
+
+	if (dwBitMask != bMaskDWord) {
+		OriginalValue = rtl92e_readl(dev, dwRegAddr);
+		BitShift = _rtl92e_calculate_bit_shift(dwBitMask);
+		NewValue = (OriginalValue & ~dwBitMask) | (dwData << BitShift);
+		rtl92e_writel(dev, dwRegAddr, NewValue);
+	} else
+		rtl92e_writel(dev, dwRegAddr, dwData);
+}
+
+u32 rtl92e_get_bb_reg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask)
+{
+	u32 OriginalValue, BitShift;
+
+	OriginalValue = rtl92e_readl(dev, dwRegAddr);
+	BitShift = _rtl92e_calculate_bit_shift(dwBitMask);
+
+	return (OriginalValue & dwBitMask) >> BitShift;
+}
+
+static u32 _rtl92e_phy_rf_read(struct net_device *dev,
+			       enum rf90_radio_path eRFPath, u32 Offset)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 ret = 0;
+	u32 NewOffset = 0;
+	struct bb_reg_definition *pPhyReg = &priv->PHYRegDef[eRFPath];
+
+	Offset &= 0x3f;
+
+	if (priv->rf_chip == RF_8256) {
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4, 0xf00, 0x0);
+		if (Offset >= 31) {
+			priv->RfReg0Value[eRFPath] |= 0x140;
+			rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset,
+					  bMaskDWord,
+					  (priv->RfReg0Value[eRFPath]<<16));
+			NewOffset = Offset - 30;
+		} else if (Offset >= 16) {
+			priv->RfReg0Value[eRFPath] |= 0x100;
+			priv->RfReg0Value[eRFPath] &= (~0x40);
+			rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset,
+					  bMaskDWord,
+					  (priv->RfReg0Value[eRFPath]<<16));
+
+			NewOffset = Offset - 15;
+		} else
+			NewOffset = Offset;
+	} else {
+		NewOffset = Offset;
+	}
+	rtl92e_set_bb_reg(dev, pPhyReg->rfHSSIPara2, bLSSIReadAddress,
+			  NewOffset);
+	rtl92e_set_bb_reg(dev, pPhyReg->rfHSSIPara2,  bLSSIReadEdge, 0x0);
+	rtl92e_set_bb_reg(dev, pPhyReg->rfHSSIPara2,  bLSSIReadEdge, 0x1);
+
+	mdelay(1);
+
+	ret = rtl92e_get_bb_reg(dev, pPhyReg->rfLSSIReadBack,
+				bLSSIReadBackData);
+
+	if (priv->rf_chip == RF_8256) {
+		priv->RfReg0Value[eRFPath] &= 0xebf;
+
+		rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset, bMaskDWord,
+				  (priv->RfReg0Value[eRFPath] << 16));
+
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4, 0x300, 0x3);
+	}
+
+
+	return ret;
+
+}
+
+static void _rtl92e_phy_rf_write(struct net_device *dev,
+				 enum rf90_radio_path eRFPath, u32 Offset,
+				 u32 Data)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 DataAndAddr = 0, NewOffset = 0;
+	struct bb_reg_definition *pPhyReg = &priv->PHYRegDef[eRFPath];
+
+	Offset &= 0x3f;
+	if (priv->rf_chip == RF_8256) {
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4, 0xf00, 0x0);
+
+		if (Offset >= 31) {
+			priv->RfReg0Value[eRFPath] |= 0x140;
+			rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset,
+					  bMaskDWord,
+					  (priv->RfReg0Value[eRFPath] << 16));
+			NewOffset = Offset - 30;
+		} else if (Offset >= 16) {
+			priv->RfReg0Value[eRFPath] |= 0x100;
+			priv->RfReg0Value[eRFPath] &= (~0x40);
+			rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset,
+					  bMaskDWord,
+					  (priv->RfReg0Value[eRFPath] << 16));
+			NewOffset = Offset - 15;
+		} else
+			NewOffset = Offset;
+	} else {
+		NewOffset = Offset;
+	}
+
+	DataAndAddr = (NewOffset & 0x3f) | (Data << 16);
+
+	rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
+
+	if (Offset == 0x0)
+		priv->RfReg0Value[eRFPath] = Data;
+
+	if (priv->rf_chip == RF_8256) {
+		if (Offset != 0) {
+			priv->RfReg0Value[eRFPath] &= 0xebf;
+			rtl92e_set_bb_reg(dev, pPhyReg->rf3wireOffset,
+					  bMaskDWord,
+					  (priv->RfReg0Value[eRFPath] << 16));
+		}
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4, 0x300, 0x3);
+	}
+}
+
+void rtl92e_set_rf_reg(struct net_device *dev, enum rf90_radio_path eRFPath,
+		       u32 RegAddr, u32 BitMask, u32 Data)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 Original_Value, BitShift, New_Value;
+
+	if (!rtl92e_is_legal_rf_path(dev, eRFPath))
+		return;
+	if (priv->rtllib->rf_power_state != rf_on && !priv->being_init_adapter)
+		return;
+
+	if (priv->Rf_Mode == RF_OP_By_FW) {
+		if (BitMask != bMask12Bits) {
+			Original_Value = _rtl92e_phy_rf_fw_read(dev, eRFPath,
+								RegAddr);
+			BitShift =  _rtl92e_calculate_bit_shift(BitMask);
+			New_Value = (Original_Value & ~BitMask) | (Data << BitShift);
+
+			_rtl92e_phy_rf_fw_write(dev, eRFPath, RegAddr,
+						New_Value);
+		} else
+			_rtl92e_phy_rf_fw_write(dev, eRFPath, RegAddr, Data);
+		udelay(200);
+
+	} else {
+		if (BitMask != bMask12Bits) {
+			Original_Value = _rtl92e_phy_rf_read(dev, eRFPath,
+							     RegAddr);
+			BitShift =  _rtl92e_calculate_bit_shift(BitMask);
+			New_Value = (Original_Value & ~BitMask) | (Data << BitShift);
+
+			_rtl92e_phy_rf_write(dev, eRFPath, RegAddr, New_Value);
+		} else
+			_rtl92e_phy_rf_write(dev, eRFPath, RegAddr, Data);
+	}
+}
+
+u32 rtl92e_get_rf_reg(struct net_device *dev, enum rf90_radio_path eRFPath,
+		      u32 RegAddr, u32 BitMask)
+{
+	u32 Original_Value, Readback_Value, BitShift;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (!rtl92e_is_legal_rf_path(dev, eRFPath))
+		return 0;
+	if (priv->rtllib->rf_power_state != rf_on && !priv->being_init_adapter)
+		return	0;
+	mutex_lock(&priv->rf_mutex);
+	if (priv->Rf_Mode == RF_OP_By_FW) {
+		Original_Value = _rtl92e_phy_rf_fw_read(dev, eRFPath, RegAddr);
+		udelay(200);
+	} else {
+		Original_Value = _rtl92e_phy_rf_read(dev, eRFPath, RegAddr);
+	}
+	BitShift =  _rtl92e_calculate_bit_shift(BitMask);
+	Readback_Value = (Original_Value & BitMask) >> BitShift;
+	mutex_unlock(&priv->rf_mutex);
+	return Readback_Value;
+}
+
+static u32 _rtl92e_phy_rf_fw_read(struct net_device *dev,
+				  enum rf90_radio_path eRFPath, u32 Offset)
+{
+	u32		Data = 0;
+	u8		time = 0;
+
+	Data |= ((Offset & 0xFF) << 12);
+	Data |= ((eRFPath & 0x3) << 20);
+	Data |= 0x80000000;
+	while (rtl92e_readl(dev, QPNR) & 0x80000000) {
+		if (time++ < 100)
+			udelay(10);
+		else
+			break;
+	}
+	rtl92e_writel(dev, QPNR, Data);
+	while (rtl92e_readl(dev, QPNR) & 0x80000000) {
+		if (time++ < 100)
+			udelay(10);
+		else
+			return 0;
+	}
+	return rtl92e_readl(dev, RF_DATA);
+
+}
+
+static void _rtl92e_phy_rf_fw_write(struct net_device *dev,
+				    enum rf90_radio_path eRFPath, u32 Offset,
+				    u32 Data)
+{
+	u8	time = 0;
+
+	Data |= ((Offset & 0xFF) << 12);
+	Data |= ((eRFPath & 0x3) << 20);
+	Data |= 0x400000;
+	Data |= 0x80000000;
+
+	while (rtl92e_readl(dev, QPNR) & 0x80000000) {
+		if (time++ < 100)
+			udelay(10);
+		else
+			break;
+	}
+	rtl92e_writel(dev, QPNR, Data);
+
+}
+
+
+void rtl92e_config_mac(struct net_device *dev)
+{
+	u32 dwArrayLen = 0, i = 0;
+	u32 *pdwArray = NULL;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->bTXPowerDataReadFromEEPORM) {
+		dwArrayLen = MACPHY_Array_PGLength;
+		pdwArray = Rtl819XMACPHY_Array_PG;
+
+	} else {
+		dwArrayLen = MACPHY_ArrayLength;
+		pdwArray = Rtl819XMACPHY_Array;
+	}
+	for (i = 0; i < dwArrayLen; i += 3) {
+		if (pdwArray[i] == 0x318)
+			pdwArray[i+2] = 0x00000800;
+		rtl92e_set_bb_reg(dev, pdwArray[i], pdwArray[i+1],
+				  pdwArray[i+2]);
+	}
+	return;
+
+}
+
+static void _rtl92e_phy_config_bb(struct net_device *dev, u8 ConfigType)
+{
+	int i;
+	u32 *Rtl819XPHY_REGArray_Table = NULL;
+	u32 *Rtl819XAGCTAB_Array_Table = NULL;
+	u16 AGCTAB_ArrayLen, PHY_REGArrayLen = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	AGCTAB_ArrayLen = AGCTAB_ArrayLength;
+	Rtl819XAGCTAB_Array_Table = Rtl819XAGCTAB_Array;
+	if (priv->rf_type == RF_2T4R) {
+		PHY_REGArrayLen = PHY_REGArrayLength;
+		Rtl819XPHY_REGArray_Table = Rtl819XPHY_REGArray;
+	} else if (priv->rf_type == RF_1T2R) {
+		PHY_REGArrayLen = PHY_REG_1T2RArrayLength;
+		Rtl819XPHY_REGArray_Table = Rtl819XPHY_REG_1T2RArray;
+	}
+
+	if (ConfigType == BaseBand_Config_PHY_REG) {
+		for (i = 0; i < PHY_REGArrayLen; i += 2) {
+			rtl92e_set_bb_reg(dev, Rtl819XPHY_REGArray_Table[i],
+					  bMaskDWord,
+					  Rtl819XPHY_REGArray_Table[i+1]);
+		}
+	} else if (ConfigType == BaseBand_Config_AGC_TAB) {
+		for (i = 0; i < AGCTAB_ArrayLen; i += 2) {
+			rtl92e_set_bb_reg(dev, Rtl819XAGCTAB_Array_Table[i],
+					  bMaskDWord,
+					  Rtl819XAGCTAB_Array_Table[i+1]);
+		}
+	}
+}
+
+static void _rtl92e_init_bb_rf_reg_def(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->PHYRegDef[RF90_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	priv->PHYRegDef[RF90_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	priv->PHYRegDef[RF90_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+	priv->PHYRegDef[RF90_PATH_D].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+
+	priv->PHYRegDef[RF90_PATH_A].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	priv->PHYRegDef[RF90_PATH_B].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	priv->PHYRegDef[RF90_PATH_C].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+	priv->PHYRegDef[RF90_PATH_D].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+
+	priv->PHYRegDef[RF90_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_C].rfintfo = rFPGA0_XC_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_D].rfintfo = rFPGA0_XD_RFInterfaceOE;
+
+	priv->PHYRegDef[RF90_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_C].rfintfe = rFPGA0_XC_RFInterfaceOE;
+	priv->PHYRegDef[RF90_PATH_D].rfintfe = rFPGA0_XD_RFInterfaceOE;
+
+	priv->PHYRegDef[RF90_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_C].rf3wireOffset = rFPGA0_XC_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_D].rf3wireOffset = rFPGA0_XD_LSSIParameter;
+
+	priv->PHYRegDef[RF90_PATH_A].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	priv->PHYRegDef[RF90_PATH_B].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	priv->PHYRegDef[RF90_PATH_C].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+	priv->PHYRegDef[RF90_PATH_D].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+
+	priv->PHYRegDef[RF90_PATH_A].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_B].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_C].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_D].rfTxGainStage = rFPGA0_TxGainStage;
+
+	priv->PHYRegDef[RF90_PATH_A].rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_B].rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_C].rfHSSIPara1 = rFPGA0_XC_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_D].rfHSSIPara1 = rFPGA0_XD_HSSIParameter1;
+
+	priv->PHYRegDef[RF90_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_C].rfHSSIPara2 = rFPGA0_XC_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_D].rfHSSIPara2 = rFPGA0_XD_HSSIParameter2;
+
+	priv->PHYRegDef[RF90_PATH_A].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_B].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_C].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_D].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+
+	priv->PHYRegDef[RF90_PATH_A].rfAGCControl1 = rOFDM0_XAAGCCore1;
+	priv->PHYRegDef[RF90_PATH_B].rfAGCControl1 = rOFDM0_XBAGCCore1;
+	priv->PHYRegDef[RF90_PATH_C].rfAGCControl1 = rOFDM0_XCAGCCore1;
+	priv->PHYRegDef[RF90_PATH_D].rfAGCControl1 = rOFDM0_XDAGCCore1;
+
+	priv->PHYRegDef[RF90_PATH_A].rfAGCControl2 = rOFDM0_XAAGCCore2;
+	priv->PHYRegDef[RF90_PATH_B].rfAGCControl2 = rOFDM0_XBAGCCore2;
+	priv->PHYRegDef[RF90_PATH_C].rfAGCControl2 = rOFDM0_XCAGCCore2;
+	priv->PHYRegDef[RF90_PATH_D].rfAGCControl2 = rOFDM0_XDAGCCore2;
+
+	priv->PHYRegDef[RF90_PATH_A].rfRxIQImbalance = rOFDM0_XARxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_B].rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_C].rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_D].rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
+
+	priv->PHYRegDef[RF90_PATH_A].rfRxAFE = rOFDM0_XARxAFE;
+	priv->PHYRegDef[RF90_PATH_B].rfRxAFE = rOFDM0_XBRxAFE;
+	priv->PHYRegDef[RF90_PATH_C].rfRxAFE = rOFDM0_XCRxAFE;
+	priv->PHYRegDef[RF90_PATH_D].rfRxAFE = rOFDM0_XDRxAFE;
+
+	priv->PHYRegDef[RF90_PATH_A].rfTxIQImbalance = rOFDM0_XATxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_B].rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_C].rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_D].rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
+
+	priv->PHYRegDef[RF90_PATH_A].rfTxAFE = rOFDM0_XATxAFE;
+	priv->PHYRegDef[RF90_PATH_B].rfTxAFE = rOFDM0_XBTxAFE;
+	priv->PHYRegDef[RF90_PATH_C].rfTxAFE = rOFDM0_XCTxAFE;
+	priv->PHYRegDef[RF90_PATH_D].rfTxAFE = rOFDM0_XDTxAFE;
+
+	priv->PHYRegDef[RF90_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_C].rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_D].rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
+
+}
+
+bool rtl92e_check_bb_and_rf(struct net_device *dev, enum hw90_block CheckBlock,
+			    enum rf90_radio_path eRFPath)
+{
+	bool ret = true;
+	u32 i, CheckTimes = 4, dwRegRead = 0;
+	u32 WriteAddr[4];
+	u32 WriteData[] = {0xfffff027, 0xaa55a02f, 0x00000027, 0x55aa502f};
+
+	WriteAddr[HW90_BLOCK_MAC] = 0x100;
+	WriteAddr[HW90_BLOCK_PHY0] = 0x900;
+	WriteAddr[HW90_BLOCK_PHY1] = 0x800;
+	WriteAddr[HW90_BLOCK_RF] = 0x3;
+
+	if (CheckBlock == HW90_BLOCK_MAC) {
+		netdev_warn(dev, "%s(): No checks available for MAC block.\n",
+			    __func__);
+		return ret;
+	}
+
+	for (i = 0; i < CheckTimes; i++) {
+		switch (CheckBlock) {
+		case HW90_BLOCK_PHY0:
+		case HW90_BLOCK_PHY1:
+			rtl92e_writel(dev, WriteAddr[CheckBlock],
+				      WriteData[i]);
+			dwRegRead = rtl92e_readl(dev, WriteAddr[CheckBlock]);
+			break;
+
+		case HW90_BLOCK_RF:
+			WriteData[i] &= 0xfff;
+			rtl92e_set_rf_reg(dev, eRFPath,
+					  WriteAddr[HW90_BLOCK_RF],
+					  bMask12Bits, WriteData[i]);
+			mdelay(10);
+			dwRegRead = rtl92e_get_rf_reg(dev, eRFPath,
+						      WriteAddr[HW90_BLOCK_RF],
+						      bMaskDWord);
+			mdelay(10);
+			break;
+
+		default:
+			ret = false;
+			break;
+		}
+
+
+		if (dwRegRead != WriteData[i]) {
+			netdev_warn(dev, "%s(): Check failed.\n", __func__);
+			ret = false;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static bool _rtl92e_bb_config_para_file(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	bool rtStatus = true;
+	u8 bRegValue = 0, eCheckItem = 0;
+	u32 dwRegValue = 0;
+
+	bRegValue = rtl92e_readb(dev, BB_GLOBAL_RESET);
+	rtl92e_writeb(dev, BB_GLOBAL_RESET, (bRegValue|BB_GLOBAL_RESET_BIT));
+
+	dwRegValue = rtl92e_readl(dev, CPU_GEN);
+	rtl92e_writel(dev, CPU_GEN, (dwRegValue&(~CPU_GEN_BB_RST)));
+
+	for (eCheckItem = (enum hw90_block)HW90_BLOCK_PHY0;
+	     eCheckItem <= HW90_BLOCK_PHY1; eCheckItem++) {
+		rtStatus  = rtl92e_check_bb_and_rf(dev,
+						   (enum hw90_block)eCheckItem,
+						   (enum rf90_radio_path)0);
+		if (!rtStatus) {
+			return rtStatus;
+		}
+	}
+	rtl92e_set_bb_reg(dev, rFPGA0_RFMOD, bCCKEn|bOFDMEn, 0x0);
+	_rtl92e_phy_config_bb(dev, BaseBand_Config_PHY_REG);
+
+	dwRegValue = rtl92e_readl(dev, CPU_GEN);
+	rtl92e_writel(dev, CPU_GEN, (dwRegValue|CPU_GEN_BB_RST));
+
+	_rtl92e_phy_config_bb(dev, BaseBand_Config_AGC_TAB);
+
+	if (priv->IC_Cut  > VERSION_8190_BD) {
+		if (priv->rf_type == RF_2T4R)
+			dwRegValue = priv->AntennaTxPwDiff[2]<<8 |
+				      priv->AntennaTxPwDiff[1]<<4 |
+				      priv->AntennaTxPwDiff[0];
+		else
+			dwRegValue = 0x0;
+		rtl92e_set_bb_reg(dev, rFPGA0_TxGainStage,
+				  (bXBTxAGC|bXCTxAGC|bXDTxAGC), dwRegValue);
+
+
+		dwRegValue = priv->CrystalCap;
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, bXtalCap92x,
+				  dwRegValue);
+	}
+
+	return rtStatus;
+}
+bool rtl92e_config_bb(struct net_device *dev)
+{
+	_rtl92e_init_bb_rf_reg_def(dev);
+	return _rtl92e_bb_config_para_file(dev);
+}
+
+void rtl92e_get_tx_power(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->MCSTxPowerLevelOriginalOffset[0] =
+		rtl92e_readl(dev, rTxAGC_Rate18_06);
+	priv->MCSTxPowerLevelOriginalOffset[1] =
+		rtl92e_readl(dev, rTxAGC_Rate54_24);
+	priv->MCSTxPowerLevelOriginalOffset[2] =
+		rtl92e_readl(dev, rTxAGC_Mcs03_Mcs00);
+	priv->MCSTxPowerLevelOriginalOffset[3] =
+		rtl92e_readl(dev, rTxAGC_Mcs07_Mcs04);
+	priv->MCSTxPowerLevelOriginalOffset[4] =
+		rtl92e_readl(dev, rTxAGC_Mcs11_Mcs08);
+	priv->MCSTxPowerLevelOriginalOffset[5] =
+		rtl92e_readl(dev, rTxAGC_Mcs15_Mcs12);
+
+	priv->DefaultInitialGain[0] = rtl92e_readb(dev, rOFDM0_XAAGCCore1);
+	priv->DefaultInitialGain[1] = rtl92e_readb(dev, rOFDM0_XBAGCCore1);
+	priv->DefaultInitialGain[2] = rtl92e_readb(dev, rOFDM0_XCAGCCore1);
+	priv->DefaultInitialGain[3] = rtl92e_readb(dev, rOFDM0_XDAGCCore1);
+
+	priv->framesync = rtl92e_readb(dev, rOFDM0_RxDetector3);
+	priv->framesyncC34 = rtl92e_readl(dev, rOFDM0_RxDetector2);
+	priv->SifsTime = rtl92e_readw(dev, SIFS);
+}
+
+void rtl92e_set_tx_power(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8	powerlevel = 0, powerlevelOFDM24G = 0;
+	s8	ant_pwr_diff;
+	u32	u4RegValue;
+
+	if (priv->epromtype == EEPROM_93C46) {
+		powerlevel = priv->TxPowerLevelCCK[channel-1];
+		powerlevelOFDM24G = priv->TxPowerLevelOFDM24G[channel-1];
+	} else if (priv->epromtype == EEPROM_93C56) {
+		if (priv->rf_type == RF_1T2R) {
+			powerlevel = priv->TxPowerLevelCCK_C[channel-1];
+			powerlevelOFDM24G = priv->TxPowerLevelOFDM24G_C[channel-1];
+		} else if (priv->rf_type == RF_2T4R) {
+			powerlevel = priv->TxPowerLevelCCK_A[channel-1];
+			powerlevelOFDM24G = priv->TxPowerLevelOFDM24G_A[channel-1];
+
+			ant_pwr_diff = priv->TxPowerLevelOFDM24G_C[channel-1]
+				       - priv->TxPowerLevelOFDM24G_A[channel-1];
+
+			priv->RF_C_TxPwDiff = ant_pwr_diff;
+
+			ant_pwr_diff &= 0xf;
+
+			priv->AntennaTxPwDiff[2] = 0;
+			priv->AntennaTxPwDiff[1] = (u8)(ant_pwr_diff);
+			priv->AntennaTxPwDiff[0] = 0;
+
+			u4RegValue = priv->AntennaTxPwDiff[2]<<8 |
+				      priv->AntennaTxPwDiff[1]<<4 |
+				      priv->AntennaTxPwDiff[0];
+
+			rtl92e_set_bb_reg(dev, rFPGA0_TxGainStage,
+					  (bXBTxAGC|bXCTxAGC|bXDTxAGC),
+					  u4RegValue);
+		}
+	}
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+	case RF_8256:
+		rtl92e_set_cck_tx_power(dev, powerlevel);
+		rtl92e_set_ofdm_tx_power(dev, powerlevelOFDM24G);
+		break;
+	case RF_8258:
+		break;
+	default:
+		netdev_err(dev, "Invalid RF Chip ID.\n");
+		break;
+	}
+}
+
+bool rtl92e_config_phy(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	bool rtStatus = true;
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+	case RF_8256:
+		rtStatus = rtl92e_config_rf(dev);
+		break;
+
+	case RF_8258:
+		break;
+	case RF_PSEUDO_11N:
+		break;
+
+	default:
+		netdev_err(dev, "Invalid RF Chip ID.\n");
+		break;
+	}
+	return rtStatus;
+}
+
+u8 rtl92e_config_rf_path(struct net_device *dev, enum rf90_radio_path eRFPath)
+{
+
+	int i;
+
+	switch (eRFPath) {
+	case RF90_PATH_A:
+		for (i = 0; i < RadioA_ArrayLength; i += 2) {
+			if (Rtl819XRadioA_Array[i] == 0xfe) {
+				msleep(100);
+				continue;
+			}
+			rtl92e_set_rf_reg(dev, eRFPath, Rtl819XRadioA_Array[i],
+					  bMask12Bits,
+					  Rtl819XRadioA_Array[i+1]);
+
+		}
+		break;
+	case RF90_PATH_B:
+		for (i = 0; i < RadioB_ArrayLength; i += 2) {
+			if (Rtl819XRadioB_Array[i] == 0xfe) {
+				msleep(100);
+				continue;
+			}
+			rtl92e_set_rf_reg(dev, eRFPath, Rtl819XRadioB_Array[i],
+					  bMask12Bits,
+					  Rtl819XRadioB_Array[i+1]);
+
+		}
+		break;
+	case RF90_PATH_C:
+		for (i = 0; i < RadioC_ArrayLength; i += 2) {
+			if (Rtl819XRadioC_Array[i] == 0xfe) {
+				msleep(100);
+				continue;
+			}
+			rtl92e_set_rf_reg(dev, eRFPath, Rtl819XRadioC_Array[i],
+					  bMask12Bits,
+					  Rtl819XRadioC_Array[i+1]);
+
+		}
+		break;
+	case RF90_PATH_D:
+		for (i = 0; i < RadioD_ArrayLength; i += 2) {
+			if (Rtl819XRadioD_Array[i] == 0xfe) {
+				msleep(100);
+				continue;
+			}
+			rtl92e_set_rf_reg(dev, eRFPath, Rtl819XRadioD_Array[i],
+					  bMask12Bits,
+					  Rtl819XRadioD_Array[i+1]);
+
+		}
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+
+}
+
+static void _rtl92e_set_tx_power_level(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8	powerlevel = priv->TxPowerLevelCCK[channel-1];
+	u8	powerlevelOFDM24G = priv->TxPowerLevelOFDM24G[channel-1];
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+
+	case RF_8256:
+		rtl92e_set_cck_tx_power(dev, powerlevel);
+		rtl92e_set_ofdm_tx_power(dev, powerlevelOFDM24G);
+		break;
+
+	case RF_8258:
+		break;
+	default:
+		netdev_warn(dev, "%s(): Invalid RF Chip ID\n", __func__);
+		break;
+	}
+}
+
+static u8 _rtl92e_phy_set_sw_chnl_cmd_array(struct net_device *dev,
+					    struct sw_chnl_cmd *CmdTable,
+					    u32 CmdTableIdx, u32 CmdTableSz,
+					    enum sw_chnl_cmd_id CmdID,
+					    u32 Para1, u32 Para2, u32 msDelay)
+{
+	struct sw_chnl_cmd *pCmd;
+
+	if (CmdTable == NULL) {
+		netdev_err(dev, "%s(): CmdTable cannot be NULL.\n", __func__);
+		return false;
+	}
+	if (CmdTableIdx >= CmdTableSz) {
+		netdev_err(dev, "%s(): Invalid index requested.\n", __func__);
+		return false;
+	}
+
+	pCmd = CmdTable + CmdTableIdx;
+	pCmd->CmdID = CmdID;
+	pCmd->Para1 = Para1;
+	pCmd->Para2 = Para2;
+	pCmd->msDelay = msDelay;
+
+	return true;
+}
+
+static u8 _rtl92e_phy_switch_channel_step(struct net_device *dev, u8 channel,
+					  u8 *stage, u8 *step, u32 *delay)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	u32					PreCommonCmdCnt;
+	u32					PostCommonCmdCnt;
+	u32					RfDependCmdCnt;
+	struct sw_chnl_cmd *CurrentCmd = NULL;
+	u8		eRFPath;
+
+	if (!rtllib_legal_channel(priv->rtllib, channel)) {
+		netdev_err(dev, "Invalid channel requested: %d\n", channel);
+		return true;
+	}
+
+	{
+		PreCommonCmdCnt = 0;
+		_rtl92e_phy_set_sw_chnl_cmd_array(dev, ieee->PreCommonCmd,
+						  PreCommonCmdCnt++,
+						  MAX_PRECMD_CNT,
+						  CmdID_SetTxPowerLevel,
+						  0, 0, 0);
+		_rtl92e_phy_set_sw_chnl_cmd_array(dev, ieee->PreCommonCmd,
+						  PreCommonCmdCnt++,
+						  MAX_PRECMD_CNT, CmdID_End,
+						  0, 0, 0);
+
+		PostCommonCmdCnt = 0;
+
+		_rtl92e_phy_set_sw_chnl_cmd_array(dev, ieee->PostCommonCmd,
+						  PostCommonCmdCnt++,
+						  MAX_POSTCMD_CNT, CmdID_End,
+						  0, 0, 0);
+
+		RfDependCmdCnt = 0;
+		switch (priv->rf_chip) {
+		case RF_8225:
+			if (!(channel >= 1 && channel <= 14)) {
+				netdev_err(dev,
+					   "Invalid channel requested for 8225: %d\n",
+					   channel);
+				return false;
+			}
+			_rtl92e_phy_set_sw_chnl_cmd_array(dev,
+							  ieee->RfDependCmd,
+							  RfDependCmdCnt++,
+							  MAX_RFDEPENDCMD_CNT,
+							  CmdID_RF_WriteReg,
+							  rZebra1_Channel,
+							  RF_CHANNEL_TABLE_ZEBRA[channel],
+							  10);
+			_rtl92e_phy_set_sw_chnl_cmd_array(dev,
+							  ieee->RfDependCmd,
+							  RfDependCmdCnt++,
+							  MAX_RFDEPENDCMD_CNT,
+							  CmdID_End, 0, 0, 0);
+			break;
+
+		case RF_8256:
+			if (!(channel >= 1 && channel <= 14)) {
+				netdev_err(dev,
+					   "Invalid channel requested for 8256: %d\n",
+					   channel);
+				return false;
+			}
+			_rtl92e_phy_set_sw_chnl_cmd_array(dev,
+							  ieee->RfDependCmd,
+							  RfDependCmdCnt++,
+							  MAX_RFDEPENDCMD_CNT,
+							  CmdID_RF_WriteReg,
+							  rZebra1_Channel,
+							  channel, 10);
+			_rtl92e_phy_set_sw_chnl_cmd_array(dev,
+							  ieee->RfDependCmd,
+							  RfDependCmdCnt++,
+							  MAX_RFDEPENDCMD_CNT,
+							  CmdID_End, 0, 0, 0);
+			break;
+
+		case RF_8258:
+			break;
+
+		default:
+			netdev_warn(dev, "Unknown RF Chip ID\n");
+			return false;
+		}
+
+
+		do {
+			switch (*stage) {
+			case 0:
+				CurrentCmd = &ieee->PreCommonCmd[*step];
+				break;
+			case 1:
+				CurrentCmd = &ieee->RfDependCmd[*step];
+				break;
+			case 2:
+				CurrentCmd = &ieee->PostCommonCmd[*step];
+				break;
+			}
+
+			if (CurrentCmd && CurrentCmd->CmdID == CmdID_End) {
+				if ((*stage) == 2)
+					return true;
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+
+			if (!CurrentCmd)
+				continue;
+			switch (CurrentCmd->CmdID) {
+			case CmdID_SetTxPowerLevel:
+				if (priv->IC_Cut > VERSION_8190_BD)
+					_rtl92e_set_tx_power_level(dev,
+								   channel);
+				break;
+			case CmdID_WritePortUlong:
+				rtl92e_writel(dev, CurrentCmd->Para1,
+					      CurrentCmd->Para2);
+				break;
+			case CmdID_WritePortUshort:
+				rtl92e_writew(dev, CurrentCmd->Para1,
+					      CurrentCmd->Para2);
+				break;
+			case CmdID_WritePortUchar:
+				rtl92e_writeb(dev, CurrentCmd->Para1,
+					      CurrentCmd->Para2);
+				break;
+			case CmdID_RF_WriteReg:
+				for (eRFPath = 0; eRFPath <
+				     priv->NumTotalRFPath; eRFPath++)
+					rtl92e_set_rf_reg(dev,
+						 (enum rf90_radio_path)eRFPath,
+						 CurrentCmd->Para1, bMask12Bits,
+						 CurrentCmd->Para2<<7);
+				break;
+			default:
+				break;
+			}
+
+			break;
+		} while (true);
+	} /*for (Number of RF paths)*/
+
+	(*delay) = CurrentCmd->msDelay;
+	(*step)++;
+	return false;
+}
+
+static void _rtl92e_phy_switch_channel(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 delay = 0;
+
+	while (!_rtl92e_phy_switch_channel_step(dev, channel,
+						&priv->SwChnlStage,
+						&priv->SwChnlStep, &delay)) {
+		if (delay > 0)
+			msleep(delay);
+		if (!priv->up)
+			break;
+	}
+}
+
+static void _rtl92e_phy_switch_channel_work_item(struct net_device *dev)
+{
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	_rtl92e_phy_switch_channel(dev, priv->chan);
+}
+
+u8 rtl92e_set_channel(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (!priv->up) {
+		netdev_err(dev, "%s(): Driver is not initialized\n", __func__);
+		return false;
+	}
+	if (priv->SwChnlInProgress)
+		return false;
+
+
+	switch (priv->rtllib->mode) {
+	case WIRELESS_MODE_A:
+	case WIRELESS_MODE_N_5G:
+		if (channel <= 14) {
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11a.\n",
+				    channel);
+			return false;
+		}
+		break;
+	case WIRELESS_MODE_B:
+		if (channel > 14) {
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11b.\n",
+				    channel);
+			return false;
+		}
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		if (channel > 14) {
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11g.\n",
+				    channel);
+			return false;
+		}
+		break;
+	}
+
+	priv->SwChnlInProgress = true;
+	if (channel == 0)
+		channel = 1;
+
+	priv->chan = channel;
+
+	priv->SwChnlStage = 0;
+	priv->SwChnlStep = 0;
+
+	if (priv->up)
+		_rtl92e_phy_switch_channel_work_item(dev);
+	priv->SwChnlInProgress = false;
+	return true;
+}
+
+static void _rtl92e_cck_tx_power_track_bw_switch_tssi(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		priv->CCKPresentAttentuation =
+			priv->CCKPresentAttentuation_20Mdefault +
+			    priv->CCKPresentAttentuation_difference;
+
+		if (priv->CCKPresentAttentuation >
+		    (CCKTxBBGainTableLength-1))
+			priv->CCKPresentAttentuation =
+					 CCKTxBBGainTableLength-1;
+		if (priv->CCKPresentAttentuation < 0)
+			priv->CCKPresentAttentuation = 0;
+
+		if (priv->rtllib->current_network.channel == 14 &&
+		    !priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = true;
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else if (priv->rtllib->current_network.channel !=
+			   14 && priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = false;
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else {
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		}
+		break;
+
+	case HT_CHANNEL_WIDTH_20_40:
+		priv->CCKPresentAttentuation =
+			priv->CCKPresentAttentuation_40Mdefault +
+			priv->CCKPresentAttentuation_difference;
+
+		if (priv->CCKPresentAttentuation >
+		    (CCKTxBBGainTableLength - 1))
+			priv->CCKPresentAttentuation =
+					 CCKTxBBGainTableLength-1;
+		if (priv->CCKPresentAttentuation < 0)
+			priv->CCKPresentAttentuation = 0;
+
+		if (priv->rtllib->current_network.channel == 14 &&
+		    !priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = true;
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else if (priv->rtllib->current_network.channel != 14
+			   && priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = false;
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else {
+			rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		}
+		break;
+	}
+}
+
+static void _rtl92e_cck_tx_power_track_bw_switch_thermal(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->rtllib->current_network.channel == 14 &&
+	    !priv->bcck_in_ch14)
+		priv->bcck_in_ch14 = true;
+	else if (priv->rtllib->current_network.channel != 14 &&
+		 priv->bcck_in_ch14)
+		priv->bcck_in_ch14 = false;
+
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		if (priv->Record_CCK_20Mindex == 0)
+			priv->Record_CCK_20Mindex = 6;
+		priv->CCK_index = priv->Record_CCK_20Mindex;
+	break;
+
+	case HT_CHANNEL_WIDTH_20_40:
+		priv->CCK_index = priv->Record_CCK_40Mindex;
+	break;
+	}
+	rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+}
+
+static void _rtl92e_cck_tx_power_track_bw_switch(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->IC_Cut >= IC_VersionCut_D)
+		_rtl92e_cck_tx_power_track_bw_switch_tssi(dev);
+	else
+		_rtl92e_cck_tx_power_track_bw_switch_thermal(dev);
+}
+
+static void _rtl92e_set_bw_mode_work_item(struct net_device *dev)
+{
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 regBwOpMode;
+
+	if (priv->rf_chip == RF_PSEUDO_11N) {
+		priv->SetBWModeInProgress = false;
+		return;
+	}
+	if (!priv->up) {
+		netdev_err(dev, "%s(): Driver is not initialized\n", __func__);
+		return;
+	}
+	regBwOpMode = rtl92e_readb(dev, BW_OPMODE);
+
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		regBwOpMode |= BW_OPMODE_20MHZ;
+		rtl92e_writeb(dev, BW_OPMODE, regBwOpMode);
+		break;
+
+	case HT_CHANNEL_WIDTH_20_40:
+		regBwOpMode &= ~BW_OPMODE_20MHZ;
+		rtl92e_writeb(dev, BW_OPMODE, regBwOpMode);
+		break;
+
+	default:
+		netdev_err(dev, "%s(): unknown Bandwidth: %#X\n", __func__,
+			   priv->CurrentChannelBW);
+		break;
+	}
+
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl92e_set_bb_reg(dev, rFPGA0_RFMOD, bRFMOD, 0x0);
+		rtl92e_set_bb_reg(dev, rFPGA1_RFMOD, bRFMOD, 0x0);
+
+		if (!priv->btxpower_tracking) {
+			rtl92e_writel(dev, rCCK0_TxFilter1, 0x1a1b0000);
+			rtl92e_writel(dev, rCCK0_TxFilter2, 0x090e1317);
+			rtl92e_writel(dev, rCCK0_DebugPort, 0x00000204);
+		} else {
+			_rtl92e_cck_tx_power_track_bw_switch(dev);
+		}
+
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, 0x00100000, 1);
+
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl92e_set_bb_reg(dev, rFPGA0_RFMOD, bRFMOD, 0x1);
+		rtl92e_set_bb_reg(dev, rFPGA1_RFMOD, bRFMOD, 0x1);
+
+		if (!priv->btxpower_tracking) {
+			rtl92e_writel(dev, rCCK0_TxFilter1, 0x35360000);
+			rtl92e_writel(dev, rCCK0_TxFilter2, 0x121c252e);
+			rtl92e_writel(dev, rCCK0_DebugPort, 0x00000409);
+		} else {
+			_rtl92e_cck_tx_power_track_bw_switch(dev);
+		}
+
+		rtl92e_set_bb_reg(dev, rCCK0_System, bCCKSideBand,
+				  (priv->nCur40MhzPrimeSC>>1));
+		rtl92e_set_bb_reg(dev, rOFDM1_LSTF, 0xC00,
+				  priv->nCur40MhzPrimeSC);
+
+		rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, 0x00100000, 0);
+		break;
+	default:
+		netdev_err(dev, "%s(): unknown Bandwidth: %#X\n", __func__,
+			   priv->CurrentChannelBW);
+		break;
+
+	}
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+
+	case RF_8256:
+		rtl92e_set_bandwidth(dev, priv->CurrentChannelBW);
+		break;
+
+	case RF_8258:
+		break;
+
+	case RF_PSEUDO_11N:
+		break;
+
+	default:
+		netdev_info(dev, "%s(): Unknown RFChipID: %d\n", __func__,
+			    priv->rf_chip);
+		break;
+	}
+
+	atomic_dec(&(priv->rtllib->atm_swbw));
+	priv->SetBWModeInProgress = false;
+}
+
+void rtl92e_set_bw_mode(struct net_device *dev, enum ht_channel_width bandwidth,
+			enum ht_extchnl_offset Offset)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+
+	if (priv->SetBWModeInProgress)
+		return;
+
+	atomic_inc(&(priv->rtllib->atm_swbw));
+	priv->SetBWModeInProgress = true;
+
+	priv->CurrentChannelBW = bandwidth;
+
+	if (Offset == HT_EXTCHNL_OFFSET_LOWER)
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_UPPER;
+	else if (Offset == HT_EXTCHNL_OFFSET_UPPER)
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_LOWER;
+	else
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	_rtl92e_set_bw_mode_work_item(dev);
+
+}
+
+void rtl92e_init_gain(struct net_device *dev, u8 Operation)
+{
+#define SCAN_RX_INITIAL_GAIN	0x17
+#define POWER_DETECTION_TH	0x08
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 BitMask;
+	u8 initial_gain;
+
+	if (priv->up) {
+		switch (Operation) {
+		case IG_Backup:
+			initial_gain = SCAN_RX_INITIAL_GAIN;
+			BitMask = bMaskByte0;
+			if (dm_digtable.dig_algorithm ==
+			    DIG_ALGO_BY_FALSE_ALARM)
+				rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+			priv->initgain_backup.xaagccore1 =
+				 rtl92e_get_bb_reg(dev, rOFDM0_XAAGCCore1,
+						   BitMask);
+			priv->initgain_backup.xbagccore1 =
+				 rtl92e_get_bb_reg(dev, rOFDM0_XBAGCCore1,
+						   BitMask);
+			priv->initgain_backup.xcagccore1 =
+				 rtl92e_get_bb_reg(dev, rOFDM0_XCAGCCore1,
+						   BitMask);
+			priv->initgain_backup.xdagccore1 =
+				 rtl92e_get_bb_reg(dev, rOFDM0_XDAGCCore1,
+						   BitMask);
+			BitMask = bMaskByte2;
+			priv->initgain_backup.cca = (u8)rtl92e_get_bb_reg(dev,
+						    rCCK0_CCA, BitMask);
+
+			rtl92e_writeb(dev, rOFDM0_XAAGCCore1, initial_gain);
+			rtl92e_writeb(dev, rOFDM0_XBAGCCore1, initial_gain);
+			rtl92e_writeb(dev, rOFDM0_XCAGCCore1, initial_gain);
+			rtl92e_writeb(dev, rOFDM0_XDAGCCore1, initial_gain);
+			rtl92e_writeb(dev, 0xa0a, POWER_DETECTION_TH);
+			break;
+		case IG_Restore:
+			BitMask = 0x7f;
+			if (dm_digtable.dig_algorithm ==
+			    DIG_ALGO_BY_FALSE_ALARM)
+				rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+
+			rtl92e_set_bb_reg(dev, rOFDM0_XAAGCCore1, BitMask,
+					 (u32)priv->initgain_backup.xaagccore1);
+			rtl92e_set_bb_reg(dev, rOFDM0_XBAGCCore1, BitMask,
+					 (u32)priv->initgain_backup.xbagccore1);
+			rtl92e_set_bb_reg(dev, rOFDM0_XCAGCCore1, BitMask,
+					 (u32)priv->initgain_backup.xcagccore1);
+			rtl92e_set_bb_reg(dev, rOFDM0_XDAGCCore1, BitMask,
+					 (u32)priv->initgain_backup.xdagccore1);
+			BitMask  = bMaskByte2;
+			rtl92e_set_bb_reg(dev, rCCK0_CCA, BitMask,
+					 (u32)priv->initgain_backup.cca);
+
+			rtl92e_set_tx_power(dev,
+					 priv->rtllib->current_network.channel);
+
+			if (dm_digtable.dig_algorithm ==
+			    DIG_ALGO_BY_FALSE_ALARM)
+				rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x1);
+			break;
+		}
+	}
+}
+
+void rtl92e_set_rf_off(struct net_device *dev)
+{
+
+	rtl92e_set_bb_reg(dev, rFPGA0_XA_RFInterfaceOE, BIT4, 0x0);
+	rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4, 0x300, 0x0);
+	rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, 0x18, 0x0);
+	rtl92e_set_bb_reg(dev, rOFDM0_TRxPathEnable, 0xf, 0x0);
+	rtl92e_set_bb_reg(dev, rOFDM1_TRxPathEnable, 0xf, 0x0);
+	rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, 0x60, 0x0);
+	rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1, 0x4, 0x0);
+	rtl92e_writeb(dev, ANAPAR_FOR_8192PciE, 0x07);
+
+}
+
+static bool _rtl92e_set_rf_power_state(struct net_device *dev,
+				       enum rt_rf_power_state rf_power_state)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&(priv->rtllib->PowerSaveControl));
+	bool bResult = true;
+	u8	i = 0, QueueID = 0;
+	struct rtl8192_tx_ring  *ring = NULL;
+
+	if (priv->SetRFPowerStateInProgress)
+		return false;
+	priv->SetRFPowerStateInProgress = true;
+
+	switch (priv->rf_chip) {
+	case RF_8256:
+		switch (rf_power_state) {
+		case rf_on:
+			if ((priv->rtllib->rf_power_state == rf_off) &&
+			     RT_IN_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC)) {
+				bool rtstatus;
+				u32 InitilizeCount = 3;
+
+				do {
+					InitilizeCount--;
+					priv->RegRfOff = false;
+					rtstatus = rtl92e_enable_nic(dev);
+				} while (!rtstatus && (InitilizeCount > 0));
+
+				if (!rtstatus) {
+					netdev_err(dev,
+						   "%s(): Failed to initialize Adapter.\n",
+						   __func__);
+					priv->SetRFPowerStateInProgress = false;
+					return false;
+				}
+
+				RT_CLEAR_PS_LEVEL(pPSC,
+						  RT_RF_OFF_LEVL_HALT_NIC);
+			} else {
+				rtl92e_writeb(dev, ANAPAR, 0x37);
+				mdelay(1);
+				rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1,
+						 0x4, 0x1);
+				priv->bHwRfOffAction = 0;
+
+				rtl92e_set_bb_reg(dev, rFPGA0_XA_RFInterfaceOE,
+						  BIT4, 0x1);
+				rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter4,
+						  0x300, 0x3);
+				rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1,
+						  0x18, 0x3);
+				rtl92e_set_bb_reg(dev, rOFDM0_TRxPathEnable,
+						  0x3, 0x3);
+				rtl92e_set_bb_reg(dev, rOFDM1_TRxPathEnable,
+						  0x3, 0x3);
+				rtl92e_set_bb_reg(dev, rFPGA0_AnalogParameter1,
+						  0x60, 0x3);
+
+			}
+
+			break;
+
+		case rf_sleep:
+			if (priv->rtllib->rf_power_state == rf_off)
+				break;
+
+
+			for (QueueID = 0, i = 0; QueueID < MAX_TX_QUEUE; ) {
+				ring = &priv->tx_ring[QueueID];
+
+				if (skb_queue_len(&ring->queue) == 0) {
+					QueueID++;
+					continue;
+				} else {
+					udelay(10);
+					i++;
+				}
+
+				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+					break;
+				}
+			}
+			rtl92e_set_rf_off(dev);
+			break;
+
+		case rf_off:
+			for (QueueID = 0, i = 0; QueueID < MAX_TX_QUEUE; ) {
+				ring = &priv->tx_ring[QueueID];
+
+				if (skb_queue_len(&ring->queue) == 0) {
+					QueueID++;
+					continue;
+				} else {
+					udelay(10);
+					i++;
+				}
+
+				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+					break;
+				}
+			}
+
+			if (pPSC->RegRfPsLevel & RT_RF_OFF_LEVL_HALT_NIC &&
+			    !RT_IN_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC)) {
+				rtl92e_disable_nic(dev);
+				RT_SET_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC);
+			} else if (!(pPSC->RegRfPsLevel &
+				   RT_RF_OFF_LEVL_HALT_NIC)) {
+				rtl92e_set_rf_off(dev);
+			}
+
+			break;
+
+		default:
+			bResult = false;
+			netdev_warn(dev,
+				    "%s(): Unknown state requested: 0x%X.\n",
+				    __func__, rf_power_state);
+			break;
+		}
+
+		break;
+
+	default:
+		netdev_warn(dev, "%s(): Unknown RF type\n", __func__);
+		break;
+	}
+
+	if (bResult) {
+		priv->rtllib->rf_power_state = rf_power_state;
+
+		switch (priv->rf_chip) {
+		case RF_8256:
+			break;
+
+		default:
+			netdev_warn(dev, "%s(): Unknown RF type\n", __func__);
+			break;
+		}
+	}
+
+	priv->SetRFPowerStateInProgress = false;
+	return bResult;
+}
+
+bool rtl92e_set_rf_power_state(struct net_device *dev,
+			       enum rt_rf_power_state rf_power_state)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	bool bResult = false;
+
+	if (rf_power_state == priv->rtllib->rf_power_state &&
+	    priv->bHwRfOffAction == 0) {
+		return bResult;
+	}
+
+	bResult = _rtl92e_set_rf_power_state(dev, rf_power_state);
+	return bResult;
+}
+
+void rtl92e_scan_op_backup(struct net_device *dev, u8 Operation)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->up) {
+		switch (Operation) {
+		case SCAN_OPT_BACKUP:
+			priv->rtllib->InitialGainHandler(dev, IG_Backup);
+			break;
+
+		case SCAN_OPT_RESTORE:
+			priv->rtllib->InitialGainHandler(dev, IG_Restore);
+			break;
+		}
+	}
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
new file mode 100644
index 000000000..75629f5df
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
@@ -0,0 +1,89 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _R819XU_PHY_H
+#define _R819XU_PHY_H
+
+#define MAX_DOZE_WAITING_TIMES_9x 64
+
+#define AGCTAB_ArrayLength			AGCTAB_ArrayLengthPciE
+#define MACPHY_ArrayLength			MACPHY_ArrayLengthPciE
+#define RadioA_ArrayLength			RadioA_ArrayLengthPciE
+#define RadioB_ArrayLength			RadioB_ArrayLengthPciE
+#define MACPHY_Array_PGLength			MACPHY_Array_PGLengthPciE
+#define RadioC_ArrayLength			RadioC_ArrayLengthPciE
+#define RadioD_ArrayLength			RadioD_ArrayLengthPciE
+#define PHY_REGArrayLength			PHY_REGArrayLengthPciE
+#define PHY_REG_1T2RArrayLength			PHY_REG_1T2RArrayLengthPciE
+
+#define Rtl819XMACPHY_Array_PG			Rtl8192PciEMACPHY_Array_PG
+#define Rtl819XMACPHY_Array			Rtl8192PciEMACPHY_Array
+#define Rtl819XRadioA_Array			Rtl8192PciERadioA_Array
+#define Rtl819XRadioB_Array			Rtl8192PciERadioB_Array
+#define Rtl819XRadioC_Array			Rtl8192PciERadioC_Array
+#define Rtl819XRadioD_Array			Rtl8192PciERadioD_Array
+#define Rtl819XAGCTAB_Array			Rtl8192PciEAGCTAB_Array
+#define Rtl819XPHY_REGArray			Rtl8192PciEPHY_REGArray
+#define Rtl819XPHY_REG_1T2RArray		Rtl8192PciEPHY_REG_1T2RArray
+
+extern u32 rtl819XAGCTAB_Array[];
+
+enum hw90_block {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4,
+};
+
+enum rf90_radio_path {
+	RF90_PATH_A = 0,
+	RF90_PATH_B = 1,
+	RF90_PATH_C = 2,
+	RF90_PATH_D = 3,
+	RF90_PATH_MAX
+};
+
+#define bMaskByte0                0xff
+#define bMaskByte1                0xff00
+#define bMaskByte2                0xff0000
+#define bMaskByte3                0xff000000
+#define bMaskHWord                0xffff0000
+#define bMaskLWord                0x0000ffff
+#define bMaskDWord                0xffffffff
+
+u8 rtl92e_is_legal_rf_path(struct net_device *dev, u32 eRFPath);
+void rtl92e_set_bb_reg(struct net_device *dev, u32 dwRegAddr,
+		       u32 dwBitMask, u32 dwData);
+u32 rtl92e_get_bb_reg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask);
+void rtl92e_set_rf_reg(struct net_device *dev, enum rf90_radio_path eRFPath,
+		       u32 RegAddr, u32 BitMask, u32 Data);
+u32 rtl92e_get_rf_reg(struct net_device *dev, enum rf90_radio_path eRFPath,
+		      u32 RegAddr, u32 BitMask);
+void rtl92e_config_mac(struct net_device *dev);
+bool rtl92e_check_bb_and_rf(struct net_device *dev,
+			    enum hw90_block CheckBlock,
+			    enum rf90_radio_path eRFPath);
+bool rtl92e_config_bb(struct net_device *dev);
+void rtl92e_get_tx_power(struct net_device *dev);
+void rtl92e_set_tx_power(struct net_device *dev, u8 channel);
+bool rtl92e_config_phy(struct net_device *dev);
+u8 rtl92e_config_rf_path(struct net_device *dev, enum rf90_radio_path eRFPath);
+
+u8 rtl92e_set_channel(struct net_device *dev, u8 channel);
+void rtl92e_set_bw_mode(struct net_device *dev,
+			enum ht_channel_width bandwidth,
+			enum ht_extchnl_offset Offset);
+void rtl92e_init_gain(struct net_device *dev, u8 Operation);
+
+void rtl92e_set_rf_off(struct net_device *dev);
+
+bool rtl92e_set_rf_power_state(struct net_device *dev,
+			       enum rt_rf_power_state rf_power_state);
+
+void rtl92e_scan_op_backup(struct net_device *dev, u8 Operation);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
new file mode 100644
index 000000000..433272a2a
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
@@ -0,0 +1,878 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _R819XU_PHYREG_H
+#define _R819XU_PHYREG_H
+
+
+#define RF_DATA			0x1d4
+
+#define rPMAC_Reset		0x100
+#define rPMAC_TxStart		0x104
+#define rPMAC_TxLegacySIG	0x108
+#define rPMAC_TxHTSIG1		0x10c
+#define rPMAC_TxHTSIG2		0x110
+#define rPMAC_PHYDebug		0x114
+#define rPMAC_TxPacketNum	0x118
+#define rPMAC_TxIdle		0x11c
+#define rPMAC_TxMACHeader0	0x120
+#define rPMAC_TxMACHeader1	0x124
+#define rPMAC_TxMACHeader2	0x128
+#define rPMAC_TxMACHeader3	0x12c
+#define rPMAC_TxMACHeader4	0x130
+#define rPMAC_TxMACHeader5	0x134
+#define rPMAC_TxDataType	0x138
+#define rPMAC_TxRandomSeed	0x13c
+#define rPMAC_CCKPLCPPreamble	0x140
+#define rPMAC_CCKPLCPHeader	0x144
+#define rPMAC_CCKCRC16		0x148
+#define rPMAC_OFDMRxCRC32OK	0x170
+#define rPMAC_OFDMRxCRC32Er	0x174
+#define rPMAC_OFDMRxParityEr	0x178
+#define rPMAC_OFDMRxCRC8Er	0x17c
+#define rPMAC_CCKCRxRC16Er	0x180
+#define rPMAC_CCKCRxRC32Er	0x184
+#define rPMAC_CCKCRxRC32OK	0x188
+#define rPMAC_TxStatus		0x18c
+
+#define MCS_TXAGC		0x340
+#define CCK_TXAGC		0x348
+
+/* Mac block on/off control register */
+#define MacBlkCtrl			0x403
+
+#define rFPGA0_RFMOD			0x800 /* RF mode & CCK TxSC */
+#define rFPGA0_TxInfo			0x804
+#define rFPGA0_PSDFunction		0x808
+#define rFPGA0_TxGainStage		0x80c
+#define rFPGA0_RFTiming1		0x810
+#define rFPGA0_RFTiming2		0x814
+#define rFPGA0_XA_HSSIParameter1	0x820
+#define rFPGA0_XA_HSSIParameter2	0x824
+#define rFPGA0_XB_HSSIParameter1	0x828
+#define rFPGA0_XB_HSSIParameter2	0x82c
+#define rFPGA0_XC_HSSIParameter1	0x830
+#define rFPGA0_XC_HSSIParameter2	0x834
+#define rFPGA0_XD_HSSIParameter1	0x838
+#define rFPGA0_XD_HSSIParameter2	0x83c
+#define rFPGA0_XA_LSSIParameter		0x840
+#define rFPGA0_XB_LSSIParameter		0x844
+#define rFPGA0_XC_LSSIParameter		0x848
+#define rFPGA0_XD_LSSIParameter		0x84c
+#define rFPGA0_RFWakeUpParameter	0x850
+#define rFPGA0_RFSleepUpParameter	0x854
+#define rFPGA0_XAB_SwitchControl	0x858
+#define rFPGA0_XCD_SwitchControl	0x85c
+#define rFPGA0_XA_RFInterfaceOE		0x860
+#define rFPGA0_XB_RFInterfaceOE		0x864
+#define rFPGA0_XC_RFInterfaceOE		0x868
+#define rFPGA0_XD_RFInterfaceOE		0x86c
+#define rFPGA0_XAB_RFInterfaceSW	0x870
+#define rFPGA0_XCD_RFInterfaceSW	0x874
+#define rFPGA0_XAB_RFParameter		0x878
+#define rFPGA0_XCD_RFParameter		0x87c
+#define rFPGA0_AnalogParameter1		0x880
+#define rFPGA0_AnalogParameter2		0x884
+#define rFPGA0_AnalogParameter3		0x888
+#define rFPGA0_AnalogParameter4		0x88c
+#define rFPGA0_XA_LSSIReadBack		0x8a0
+#define rFPGA0_XB_LSSIReadBack		0x8a4
+#define rFPGA0_XC_LSSIReadBack		0x8a8
+#define rFPGA0_XD_LSSIReadBack		0x8ac
+#define rFPGA0_PSDReport		0x8b4
+#define rFPGA0_XAB_RFInterfaceRB	0x8e0
+#define rFPGA0_XCD_RFInterfaceRB	0x8e4
+
+/* Page 9 - RF mode & OFDM TxSC */
+#define rFPGA1_RFMOD			0x900
+#define rFPGA1_TxBlock			0x904
+#define rFPGA1_DebugSelect		0x908
+#define rFPGA1_TxInfo			0x90c
+
+#define rCCK0_System			0xa00
+#define rCCK0_AFESetting		0xa04
+#define rCCK0_CCA			0xa08
+/* AGC default value, saturation level */
+#define rCCK0_RxAGC1			0xa0c
+#define rCCK0_RxAGC2			0xa10 /* AGC & DAGC */
+#define rCCK0_RxHP			0xa14
+/* Timing recovery & channel estimation threshold */
+#define rCCK0_DSPParameter1		0xa18
+#define rCCK0_DSPParameter2		0xa1c /* SQ threshold */
+#define rCCK0_TxFilter1			0xa20
+#define rCCK0_TxFilter2			0xa24
+#define rCCK0_DebugPort			0xa28 /* Debug port and TX filter 3 */
+#define rCCK0_FalseAlarmReport		0xa2c
+#define rCCK0_TRSSIReport		0xa50
+#define rCCK0_RxReport			0xa54
+#define rCCK0_FACounterLower		0xa5c
+#define rCCK0_FACounterUpper		0xa58
+
+#define rOFDM0_LSTF			0xc00
+#define rOFDM0_TRxPathEnable		0xc04
+#define rOFDM0_TRMuxPar			0xc08
+#define rOFDM0_TRSWIsolation		0xc0c
+/* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define rOFDM0_XARxAFE			0xc10
+#define rOFDM0_XARxIQImbalance		0xc14 /* RxIQ imbalance matrix */
+#define rOFDM0_XBRxAFE			0xc18
+#define rOFDM0_XBRxIQImbalance		0xc1c
+#define rOFDM0_XCRxAFE			0xc20
+#define rOFDM0_XCRxIQImbalance		0xc24
+#define rOFDM0_XDRxAFE			0xc28
+#define rOFDM0_XDRxIQImbalance		0xc2c
+#define rOFDM0_RxDetector1		0xc30 /* PD, BW & SBD */
+#define rOFDM0_RxDetector2		0xc34 /* SBD */
+#define rOFDM0_RxDetector3		0xc38 /* Frame Sync */
+/* PD, SBD, Frame Sync & Short-GI */
+#define rOFDM0_RxDetector4		0xc3c
+#define rOFDM0_RxDSP			0xc40 /* Rx Sync Path */
+#define rOFDM0_CFOandDAGC		0xc44 /* CFO & DAGC */
+#define rOFDM0_CCADropThreshold		0xc48
+#define rOFDM0_ECCAThreshold		0xc4c /* Energy CCA */
+#define rOFDM0_XAAGCCore1		0xc50
+#define rOFDM0_XAAGCCore2		0xc54
+#define rOFDM0_XBAGCCore1		0xc58
+#define rOFDM0_XBAGCCore2		0xc5c
+#define rOFDM0_XCAGCCore1		0xc60
+#define rOFDM0_XCAGCCore2		0xc64
+#define rOFDM0_XDAGCCore1		0xc68
+#define rOFDM0_XDAGCCore2		0xc6c
+#define rOFDM0_AGCParameter1		0xc70
+#define rOFDM0_AGCParameter2		0xc74
+#define rOFDM0_AGCRSSITable		0xc78
+#define rOFDM0_HTSTFAGC			0xc7c
+#define rOFDM0_XATxIQImbalance		0xc80
+#define rOFDM0_XATxAFE			0xc84
+#define rOFDM0_XBTxIQImbalance		0xc88
+#define rOFDM0_XBTxAFE			0xc8c
+#define rOFDM0_XCTxIQImbalance		0xc90
+#define rOFDM0_XCTxAFE			0xc94
+#define rOFDM0_XDTxIQImbalance		0xc98
+#define rOFDM0_XDTxAFE			0xc9c
+#define rOFDM0_RxHPParameter		0xce0
+#define rOFDM0_TxPseudoNoiseWgt		0xce4
+#define rOFDM0_FrameSync		0xcf0
+#define rOFDM0_DFSReport		0xcf4
+#define rOFDM0_TxCoeff1			0xca4
+#define rOFDM0_TxCoeff2			0xca8
+#define rOFDM0_TxCoeff3			0xcac
+#define rOFDM0_TxCoeff4			0xcb0
+#define rOFDM0_TxCoeff5			0xcb4
+#define rOFDM0_TxCoeff6			0xcb8
+
+
+#define rOFDM1_LSTF			0xd00
+#define rOFDM1_TRxPathEnable		0xd04
+#define rOFDM1_CFO			0xd08
+#define rOFDM1_CSI1			0xd10
+#define rOFDM1_SBD			0xd14
+#define rOFDM1_CSI2			0xd18
+#define rOFDM1_CFOTracking		0xd2c
+#define rOFDM1_TRxMesaure1		0xd34
+#define rOFDM1_IntfDet			0xd3c
+#define rOFDM1_PseudoNoiseStateAB	0xd50
+#define rOFDM1_PseudoNoiseStateCD	0xd54
+#define rOFDM1_RxPseudoNoiseWgt		0xd58
+#define rOFDM_PHYCounter1		0xda0 /* cca, parity fail */
+#define rOFDM_PHYCounter2		0xda4 /* rate illegal, crc8 fail */
+#define rOFDM_PHYCounter3		0xda8 /* MCS not supported */
+#define rOFDM_ShortCFOAB		0xdac
+#define rOFDM_ShortCFOCD		0xdb0
+#define rOFDM_LongCFOAB			0xdb4
+#define rOFDM_LongCFOCD			0xdb8
+#define rOFDM_TailCFOAB			0xdbc
+#define rOFDM_TailCFOCD			0xdc0
+#define rOFDM_PWMeasure1		0xdc4
+#define rOFDM_PWMeasure2		0xdc8
+#define rOFDM_BWReport			0xdcc
+#define rOFDM_AGCReport			0xdd0
+#define rOFDM_RxSNR			0xdd4
+#define rOFDM_RxEVMCSI			0xdd8
+#define rOFDM_SIGReport			0xddc
+
+#define rTxAGC_Rate18_06		0xe00
+#define rTxAGC_Rate54_24		0xe04
+#define rTxAGC_CCK_Mcs32		0xe08
+#define rTxAGC_Mcs03_Mcs00		0xe10
+#define rTxAGC_Mcs07_Mcs04		0xe14
+#define rTxAGC_Mcs11_Mcs08		0xe18
+#define rTxAGC_Mcs15_Mcs12		0xe1c
+
+
+#define rZebra1_HSSIEnable		0x0
+#define rZebra1_TRxEnable1		0x1
+#define rZebra1_TRxEnable2		0x2
+#define rZebra1_AGC			0x4
+#define rZebra1_ChargePump		0x5
+#define rZebra1_Channel			0x7
+#define rZebra1_TxGain			0x8
+#define rZebra1_TxLPF			0x9
+#define rZebra1_RxLPF			0xb
+#define rZebra1_RxHPFCorner		0xc
+
+/* Zebra 4 */
+#define rGlobalCtrl			0
+#define rRTL8256_TxLPF			19
+#define rRTL8256_RxLPF			11
+
+/* RTL8258 */
+#define rRTL8258_TxLPF			0x11
+#define rRTL8258_RxLPF			0x13
+#define rRTL8258_RSSILPF		0xa
+
+/* Bit Mask - Page 1*/
+#define bBBResetB			0x100
+#define bGlobalResetB			0x200
+#define bOFDMTxStart			0x4
+#define bCCKTxStart			0x8
+#define bCRC32Debug			0x100
+#define bPMACLoopback			0x10
+#define bTxLSIG				0xffffff
+#define bOFDMTxRate			0xf
+#define bOFDMTxReserved			0x10
+#define bOFDMTxLength			0x1ffe0
+#define bOFDMTxParity			0x20000
+#define bTxHTSIG1			0xffffff
+#define bTxHTMCSRate			0x7f
+#define bTxHTBW	0x80
+#define bTxHTLength			0xffff00
+#define bTxHTSIG2			0xffffff
+#define bTxHTSmoothing			0x1
+#define bTxHTSounding			0x2
+#define bTxHTReserved			0x4
+#define bTxHTAggreation			0x8
+#define bTxHTSTBC			0x30
+#define bTxHTAdvanceCoding		0x40
+#define bTxHTShortGI			0x80
+#define bTxHTNumberHT_LTF		0x300
+#define bTxHTCRC8			0x3fc00
+#define bCounterReset			0x10000
+#define bNumOfOFDMTx			0xffff
+#define bNumOfCCKTx			0xffff0000
+#define bTxIdleInterval			0xffff
+#define bOFDMService			0xffff0000
+#define bTxMACHeader			0xffffffff
+#define bTxDataInit			0xff
+#define bTxHTMode			0x100
+#define bTxDataType			0x30000
+#define bTxRandomSeed			0xffffffff
+#define bCCKTxPreamble			0x1
+#define bCCKTxSFD			0xffff0000
+#define bCCKTxSIG			0xff
+#define bCCKTxService			0xff00
+#define bCCKLengthExt			0x8000
+#define bCCKTxLength			0xffff0000
+#define bCCKTxCRC16			0xffff
+#define bCCKTxStatus			0x1
+#define bOFDMTxStatus			0x2
+/* Bit Mask - Page 8 */
+#define bRFMOD				0x1
+#define bJapanMode			0x2
+#define bCCKTxSC			0x30
+#define bCCKEn				0x1000000
+#define bOFDMEn				0x2000000
+#define bOFDMRxADCPhase			0x10000
+#define bOFDMTxDACPhase			0x40000
+#define bXATxAGC			0x3f
+#define bXBTxAGC			0xf00
+#define bXCTxAGC			0xf000
+#define bXDTxAGC			0xf0000
+#define bPAStart			0xf0000000
+#define bTRStart			0x00f00000
+#define bRFStart			0x0000f000
+#define bBBStart			0x000000f0
+#define bBBCCKStart			0x0000000f
+/* Bit Mask - rFPGA0_RFTiming2 */
+#define bPAEnd				0xf
+#define bTREnd				0x0f000000
+#define bRFEnd				0x000f0000
+/* T2R */
+#define bCCAMask			0x000000f0
+#define bR2RCCAMask			0x00000f00
+#define bHSSI_R2TDelay			0xf8000000
+#define bHSSI_T2RDelay			0xf80000
+/* Channel gain at continue TX. */
+#define bContTxHSSI			0x400
+#define bIGFromCCK			0x200
+#define bAGCAddress			0x3f
+#define bRxHPTx				0x7000
+#define bRxHPT2R			0x38000
+#define bRxHPCCKIni			0xc0000
+#define bAGCTxCode			0xc00000
+#define bAGCRxCode			0x300000
+#define b3WireDataLength		0x800
+#define b3WireAddressLength		0x400
+#define b3WireRFPowerDown		0x1
+/*#define bHWSISelect			0x8 */
+#define b5GPAPEPolarity			0x40000000
+#define b2GPAPEPolarity			0x80000000
+#define bRFSW_TxDefaultAnt		0x3
+#define bRFSW_TxOptionAnt		0x30
+#define bRFSW_RxDefaultAnt		0x300
+#define bRFSW_RxOptionAnt		0x3000
+#define bRFSI_3WireData			0x1
+#define bRFSI_3WireClock		0x2
+#define bRFSI_3WireLoad			0x4
+#define bRFSI_3WireRW			0x8
+/* 3-wire total control */
+#define bRFSI_3Wire			0xf
+#define bRFSI_RFENV			0x10
+#define bRFSI_TRSW			0x20
+#define bRFSI_TRSWB			0x40
+#define bRFSI_ANTSW			0x100
+#define bRFSI_ANTSWB			0x200
+#define bRFSI_PAPE			0x400
+#define bRFSI_PAPE5G			0x800
+#define bBandSelect			0x1
+#define bHTSIG2_GI			0x80
+#define bHTSIG2_Smoothing		0x01
+#define bHTSIG2_Sounding		0x02
+#define bHTSIG2_Aggreaton		0x08
+#define bHTSIG2_STBC			0x30
+#define bHTSIG2_AdvCoding		0x40
+#define bHTSIG2_NumOfHTLTF		0x300
+#define bHTSIG2_CRC8			0x3fc
+#define bHTSIG1_MCS			0x7f
+#define bHTSIG1_BandWidth		0x80
+#define bHTSIG1_HTLength		0xffff
+#define bLSIG_Rate			0xf
+#define bLSIG_Reserved			0x10
+#define bLSIG_Length			0x1fffe
+#define bLSIG_Parity			0x20
+#define bCCKRxPhase			0x4
+#define bLSSIReadAddress		0x3f000000 /* LSSI "read" address */
+#define bLSSIReadEdge			0x80000000 /* LSSI "read" edge signal */
+#define bLSSIReadBackData		0xfff
+#define bLSSIReadOKFlag			0x1000
+#define bCCKSampleRate			0x8 /* 0: 44 MHz, 1: 88MHz */
+
+#define bRegulator0Standby		0x1
+#define bRegulatorPLLStandby		0x2
+#define bRegulator1Standby		0x4
+#define bPLLPowerUp			0x8
+#define bDPLLPowerUp			0x10
+#define bDA10PowerUp			0x20
+#define bAD7PowerUp			0x200
+#define bDA6PowerUp			0x2000
+#define bXtalPowerUp			0x4000
+#define b40MDClkPowerUP			0x8000
+#define bDA6DebugMode			0x20000
+#define bDA6Swing			0x380000
+#define bADClkPhase			0x4000000
+#define b80MClkDelay			0x18000000
+#define bAFEWatchDogEnable		0x20000000
+#define bXtalCap			0x0f000000
+#define bXtalCap01			0xc0000000
+#define bXtalCap23			0x3
+#define bXtalCap92x			0x0f000000
+#define bIntDifClkEnable		0x400
+#define bExtSigClkEnable		0x800
+#define bBandgapMbiasPowerUp		0x10000
+#define bAD11SHGain			0xc0000
+#define bAD11InputRange			0x700000
+#define bAD11OPCurrent			0x3800000
+#define bIPathLoopback			0x4000000
+#define bQPathLoopback			0x8000000
+#define bAFELoopback			0x10000000
+#define bDA10Swing			0x7e0
+#define bDA10Reverse			0x800
+#define bDAClkSource			0x1000
+#define bAD7InputRange			0x6000
+#define bAD7Gain			0x38000
+#define bAD7OutputCMMode		0x40000
+#define bAD7InputCMMode			0x380000
+#define bAD7Current			0xc00000
+#define bRegulatorAdjust		0x7000000
+#define bAD11PowerUpAtTx		0x1
+#define bDA10PSAtTx			0x10
+#define bAD11PowerUpAtRx		0x100
+#define bDA10PSAtRx			0x1000
+
+#define bCCKRxAGCFormat			0x200
+
+#define bPSDFFTSamplepPoint		0xc000
+#define bPSDAverageNum			0x3000
+#define bIQPathControl			0xc00
+#define bPSDFreq			0x3ff
+#define bPSDAntennaPath			0x30
+#define bPSDIQSwitch			0x40
+#define bPSDRxTrigger			0x400000
+#define bPSDTxTrigger			0x80000000
+#define bPSDSineToneScale		0x7f000000
+#define bPSDReport			0xffff
+
+/* Page 8 */
+#define bOFDMTxSC			0x30000000
+#define bCCKTxOn			0x1
+#define bOFDMTxOn			0x2
+/* Reset debug page and also HWord, LWord */
+#define bDebugPage			0xfff
+/* Reset debug page and LWord */
+#define bDebugItem			0xff
+#define bAntL				0x10
+#define bAntNonHT			0x100
+#define bAntHT1				0x1000
+#define bAntHT2				0x10000
+#define bAntHT1S1			0x100000
+#define bAntNonHTS1			0x1000000
+
+/* Page a */
+#define bCCKBBMode			0x3
+#define bCCKTxPowerSaving		0x80
+#define bCCKRxPowerSaving		0x40
+#define bCCKSideBand			0x10
+#define bCCKScramble			0x8
+#define bCCKAntDiversity		0x8000
+#define bCCKCarrierRecovery		0x4000
+#define bCCKTxRate			0x3000
+#define bCCKDCCancel			0x0800
+#define bCCKISICancel			0x0400
+#define bCCKMatchFilter			0x0200
+#define bCCKEqualizer			0x0100
+#define bCCKPreambleDetect		0x800000
+#define bCCKFastFalseCCA		0x400000
+#define bCCKChEstStart			0x300000
+#define bCCKCCACount			0x080000
+#define bCCKcs_lim			0x070000
+#define bCCKBistMode			0x80000000
+#define bCCKCCAMask			0x40000000
+#define bCCKTxDACPhase			0x4
+#define bCCKRxADCPhase			0x20000000 /* r_rx_clk */
+#define bCCKr_cp_mode0			0x0100
+#define bCCKTxDCOffset			0xf0
+#define bCCKRxDCOffset			0xf
+#define bCCKCCAMode			0xc000
+#define bCCKFalseCS_lim			0x3f00
+#define bCCKCS_ratio			0xc00000
+#define bCCKCorgBit_sel			0x300000
+#define bCCKPD_lim			0x0f0000
+#define bCCKNewCCA			0x80000000
+#define bCCKRxHPofIG			0x8000
+#define bCCKRxIG			0x7f00
+#define bCCKLNAPolarity			0x800000
+#define bCCKRx1stGain			0x7f0000
+/* CCK Rx Initial gain polarity */
+#define bCCKRFExtend			0x20000000
+#define bCCKRxAGCSatLevel		0x1f000000
+#define bCCKRxAGCSatCount		0xe0
+/* AGCSAmp_dly */
+#define bCCKRxRFSettle			0x1f
+#define bCCKFixedRxAGC			0x8000
+/*#define bCCKRxAGCFormat		0x4000  remove to HSSI register 0x824 */
+#define bCCKAntennaPolarity		0x2000
+#define bCCKTxFilterType		0x0c00
+#define bCCKRxAGCReportType		0x0300
+#define bCCKRxDAGCEn			0x80000000
+#define bCCKRxDAGCPeriod		0x20000000
+#define bCCKRxDAGCSatLevel		0x1f000000
+#define bCCKTimingRecovery		0x800000
+#define bCCKTxC0			0x3f0000
+#define bCCKTxC1			0x3f000000
+#define bCCKTxC2			0x3f
+#define bCCKTxC3			0x3f00
+#define bCCKTxC4			0x3f0000
+#define bCCKTxC5			0x3f000000
+#define bCCKTxC6			0x3f
+#define bCCKTxC7			0x3f00
+#define bCCKDebugPort			0xff0000
+#define bCCKDACDebug			0x0f000000
+#define bCCKFalseAlarmEnable		0x8000
+#define bCCKFalseAlarmRead		0x4000
+#define bCCKTRSSI			0x7f
+#define bCCKRxAGCReport			0xfe
+#define bCCKRxReport_AntSel		0x80000000
+#define bCCKRxReport_MFOff		0x40000000
+#define bCCKRxRxReport_SQLoss		0x20000000
+#define bCCKRxReport_Pktloss		0x10000000
+#define bCCKRxReport_Lockedbit		0x08000000
+#define bCCKRxReport_RateError		0x04000000
+#define bCCKRxReport_RxRate		0x03000000
+#define bCCKRxFACounterLower		0xff
+#define bCCKRxFACounterUpper		0xff000000
+#define bCCKRxHPAGCStart		0xe000
+#define bCCKRxHPAGCFinal		0x1c00
+
+#define bCCKRxFalseAlarmEnable		0x8000
+#define bCCKFACounterFreeze		0x4000
+
+#define bCCKTxPathSel			0x10000000
+#define bCCKDefaultRxPath		0xc000000
+#define bCCKOptionRxPath		0x3000000
+
+/* Page c */
+#define bNumOfSTF			0x3
+#define bShift_L			0xc0
+#define bGI_TH				0xc
+#define bRxPathA			0x1
+#define bRxPathB			0x2
+#define bRxPathC			0x4
+#define bRxPathD			0x8
+#define bTxPathA			0x1
+#define bTxPathB			0x2
+#define bTxPathC			0x4
+#define bTxPathD			0x8
+#define bTRSSIFreq			0x200
+#define bADCBackoff			0x3000
+#define bDFIRBackoff			0xc000
+#define bTRSSILatchPhase		0x10000
+#define bRxIDCOffset			0xff
+#define bRxQDCOffset			0xff00
+#define bRxDFIRMode			0x1800000
+#define bRxDCNFType			0xe000000
+#define bRXIQImb_A			0x3ff
+#define bRXIQImb_B			0xfc00
+#define bRXIQImb_C			0x3f0000
+#define bRXIQImb_D			0xffc00000
+#define bDC_dc_Notch			0x60000
+#define bRxNBINotch			0x1f000000
+#define bPD_TH				0xf
+#define bPD_TH_Opt2			0xc000
+#define bPWED_TH			0x700
+#define bIfMF_Win_L			0x800
+#define bPD_Option			0x1000
+#define bMF_Win_L			0xe000
+#define bBW_Search_L			0x30000
+#define bwin_enh_L			0xc0000
+#define bBW_TH				0x700000
+#define bED_TH2				0x3800000
+#define bBW_option			0x4000000
+#define bRatio_TH			0x18000000
+#define bWindow_L			0xe0000000
+#define bSBD_Option			0x1
+#define bFrame_TH			0x1c
+#define bFS_Option			0x60
+#define bDC_Slope_check			0x80
+#define bFGuard_Counter_DC_L		0xe00
+#define bFrame_Weight_Short		0x7000
+#define bSub_Tune			0xe00000
+#define bFrame_DC_Length		0xe000000
+#define bSBD_start_offset		0x30000000
+#define bFrame_TH_2			0x7
+#define bFrame_GI2_TH			0x38
+#define bGI2_Sync_en			0x40
+#define bSarch_Short_Early		0x300
+#define bSarch_Short_Late		0xc00
+#define bSarch_GI2_Late			0x70000
+#define bCFOAntSum			0x1
+#define bCFOAcc				0x2
+#define bCFOStartOffset			0xc
+#define bCFOLookBack			0x70
+#define bCFOSumWeight			0x80
+#define bDAGCEnable			0x10000
+#define bTXIQImb_A			0x3ff
+#define bTXIQImb_B			0xfc00
+#define bTXIQImb_C			0x3f0000
+#define bTXIQImb_D			0xffc00000
+#define bTxIDCOffset			0xff
+#define bTxQDCOffset			0xff00
+#define bTxDFIRMode			0x10000
+#define bTxPesudoNoiseOn		0x4000000
+#define bTxPesudoNoise_A		0xff
+#define bTxPesudoNoise_B		0xff00
+#define bTxPesudoNoise_C		0xff0000
+#define bTxPesudoNoise_D		0xff000000
+#define bCCADropOption			0x20000
+#define bCCADropThres			0xfff00000
+#define bEDCCA_H			0xf
+#define bEDCCA_L			0xf0
+#define bLambda_ED			0x300
+#define bRxInitialGain			0x7f
+#define bRxAntDivEn			0x80
+#define bRxAGCAddressForLNA		0x7f00
+#define bRxHighPowerFlow		0x8000
+#define bRxAGCFreezeThres		0xc0000
+#define bRxFreezeStep_AGC1		0x300000
+#define bRxFreezeStep_AGC2		0xc00000
+#define bRxFreezeStep_AGC3		0x3000000
+#define bRxFreezeStep_AGC0		0xc000000
+#define bRxRssi_Cmp_En			0x10000000
+#define bRxQuickAGCEn			0x20000000
+#define bRxAGCFreezeThresMode		0x40000000
+#define bRxOverFlowCheckType		0x80000000
+#define bRxAGCShift			0x7f
+#define bTRSW_Tri_Only			0x80
+#define bPowerThres			0x300
+#define bRxAGCEn			0x1
+#define bRxAGCTogetherEn		0x2
+#define bRxAGCMin			0x4
+#define bRxHP_Ini			0x7
+#define bRxHP_TRLNA			0x70
+#define bRxHP_RSSI			0x700
+#define bRxHP_BBP1			0x7000
+#define bRxHP_BBP2			0x70000
+#define bRxHP_BBP3			0x700000
+/* The threshold for high power */
+#define bRSSI_H				0x7f0000
+/* The threshold for ant diversity */
+#define bRSSI_Gen			0x7f000000
+#define bRxSettle_TRSW			0x7
+#define bRxSettle_LNA			0x38
+#define bRxSettle_RSSI			0x1c0
+#define bRxSettle_BBP			0xe00
+#define bRxSettle_RxHP			0x7000
+#define bRxSettle_AntSW_RSSI		0x38000
+#define bRxSettle_AntSW			0xc0000
+#define bRxProcessTime_DAGC		0x300000
+#define bRxSettle_HSSI			0x400000
+#define bRxProcessTime_BBPPW		0x800000
+#define bRxAntennaPowerShift		0x3000000
+#define bRSSITableSelect		0xc000000
+#define bRxHP_Final			0x7000000
+#define bRxHTSettle_BBP			0x7
+#define bRxHTSettle_HSSI		0x8
+#define bRxHTSettle_RxHP		0x70
+#define bRxHTSettle_BBPPW		0x80
+#define bRxHTSettle_Idle		0x300
+#define bRxHTSettle_Reserved		0x1c00
+#define bRxHTRxHPEn			0x8000
+#define bRxHTAGCFreezeThres		0x30000
+#define bRxHTAGCTogetherEn		0x40000
+#define bRxHTAGCMin			0x80000
+#define bRxHTAGCEn			0x100000
+#define bRxHTDAGCEn			0x200000
+#define bRxHTRxHP_BBP			0x1c00000
+#define bRxHTRxHP_Final			0xe0000000
+#define bRxPWRatioTH			0x3
+#define bRxPWRatioEn			0x4
+#define bRxMFHold			0x3800
+#define bRxPD_Delay_TH1			0x38
+#define bRxPD_Delay_TH2			0x1c0
+#define bRxPD_DC_COUNT_MAX		0x600
+/*#define bRxMF_Hold			0x3800*/
+#define bRxPD_Delay_TH			0x8000
+#define bRxProcess_Delay		0xf0000
+#define bRxSearchrange_GI2_Early	0x700000
+#define bRxFrame_Guard_Counter_L	0x3800000
+#define bRxSGI_Guard_L			0xc000000
+#define bRxSGI_Search_L			0x30000000
+#define bRxSGI_TH			0xc0000000
+#define bDFSCnt0			0xff
+#define bDFSCnt1			0xff00
+#define bDFSFlag			0xf0000
+
+#define bMFWeightSum		0x300000
+#define bMinIdxTH		0x7f000000
+
+#define bDAFormat		0x40000
+
+#define bTxChEmuEnable		0x01000000
+
+#define bTRSWIsolation_A	0x7f
+#define bTRSWIsolation_B	0x7f00
+#define bTRSWIsolation_C	0x7f0000
+#define bTRSWIsolation_D	0x7f000000
+
+#define bExtLNAGain		0x7c00
+
+/* Page d */
+#define bSTBCEn			0x4
+#define bAntennaMapping		0x10
+#define bNss			0x20
+#define bCFOAntSumD		0x200
+#define bPHYCounterReset	0x8000000
+#define bCFOReportGet		0x4000000
+#define bOFDMContinueTx		0x10000000
+#define bOFDMSingleCarrier	0x20000000
+#define bOFDMSingleTone		0x40000000
+/* #define bRxPath1		0x01
+ * #define bRxPath2		0x02
+ * #define bRxPath3		0x04
+ * #define bRxPath4		0x08
+ * #define bTxPath1		0x10
+ * #define bTxPath2		0x20
+ */
+#define bHTDetect		0x100
+#define bCFOEn			0x10000
+#define bCFOValue		0xfff00000
+#define bSigTone_Re		0x3f
+#define bSigTone_Im		0x7f00
+#define bCounter_CCA		0xffff
+#define bCounter_ParityFail	0xffff0000
+#define bCounter_RateIllegal	0xffff
+#define bCounter_CRC8Fail	0xffff0000
+#define bCounter_MCSNoSupport	0xffff
+#define bCounter_FastSync	0xffff
+#define bShortCFO		0xfff
+#define bShortCFOTLength	12 /* total */
+#define bShortCFOFLength	11 /* fraction */
+#define bLongCFO		0x7ff
+#define bLongCFOTLength		11
+#define bLongCFOFLength		11
+#define bTailCFO		0x1fff
+#define bTailCFOTLength		13
+#define bTailCFOFLength		12
+
+#define bmax_en_pwdB		0xffff
+#define bCC_power_dB		0xffff0000
+#define bnoise_pwdB		0xffff
+#define bPowerMeasTLength	10
+#define bPowerMeasFLength	3
+#define bRx_HT_BW		0x1
+#define bRxSC			0x6
+#define bRx_HT			0x8
+
+#define bNB_intf_det_on		0x1
+#define bIntf_win_len_cfg	0x30
+#define bNB_Intf_TH_cfg		0x1c0
+
+#define bRFGain			0x3f
+#define bTableSel		0x40
+#define bTRSW			0x80
+
+#define bRxSNR_A		0xff
+#define bRxSNR_B		0xff00
+#define bRxSNR_C		0xff0000
+#define bRxSNR_D		0xff000000
+#define bSNREVMTLength		8
+#define bSNREVMFLength		1
+
+#define bCSI1st			0xff
+#define bCSI2nd			0xff00
+#define bRxEVM1st		0xff0000
+#define bRxEVM2nd		0xff000000
+
+#define bSIGEVM			0xff
+#define bPWDB			0xff00
+#define bSGIEN			0x10000
+
+#define bSFactorQAM1		0xf
+#define bSFactorQAM2		0xf0
+#define bSFactorQAM3		0xf00
+#define bSFactorQAM4		0xf000
+#define bSFactorQAM5		0xf0000
+#define bSFactorQAM6		0xf0000
+#define bSFactorQAM7		0xf00000
+#define bSFactorQAM8		0xf000000
+#define bSFactorQAM9		0xf0000000
+#define bCSIScheme		0x100000
+
+#define bNoiseLvlTopSet		0x3
+#define bChSmooth		0x4
+#define bChSmoothCfg1		0x38
+#define bChSmoothCfg2		0x1c0
+#define bChSmoothCfg3		0xe00
+#define bChSmoothCfg4		0x7000
+#define bMRCMode		0x800000
+#define bTHEVMCfg		0x7000000
+
+#define bLoopFitType		0x1
+#define bUpdCFO			0x40
+#define bUpdCFOOffData		0x80
+#define bAdvUpdCFO		0x100
+#define bAdvTimeCtrl		0x800
+#define bUpdClko		0x1000
+#define bFC			0x6000
+#define bTrackingMode		0x8000
+#define bPhCmpEnable		0x10000
+#define bUpdClkoLTF		0x20000
+#define bComChCFO		0x40000
+#define bCSIEstiMode		0x80000
+#define bAdvUpdEqz		0x100000
+#define bUChCfg			0x7000000
+#define bUpdEqz			0x8000000
+
+/* Page e */
+#define bTxAGCRate18_06		0x7f7f7f7f
+#define bTxAGCRate54_24		0x7f7f7f7f
+#define bTxAGCRateMCS32		0x7f
+#define bTxAGCRateCCK		0x7f00
+#define bTxAGCRateMCS3_MCS0	0x7f7f7f7f
+#define bTxAGCRateMCS7_MCS4	0x7f7f7f7f
+#define bTxAGCRateMCS11_MCS8	0x7f7f7f7f
+#define bTxAGCRateMCS15_MCS12	0x7f7f7f7f
+
+#define bRxPesudoNoiseOn	0x20000000 /* Rx Pseduo noise */
+#define bRxPesudoNoise_A	0xff
+#define bRxPesudoNoise_B	0xff00
+#define bRxPesudoNoise_C	0xff0000
+#define bRxPesudoNoise_D	0xff000000
+#define bPesudoNoiseState_A	0xffff
+#define bPesudoNoiseState_B	0xffff0000
+#define bPesudoNoiseState_C	0xffff
+#define bPesudoNoiseState_D	0xffff0000
+
+/* RF Zebra 1 */
+#define bZebra1_HSSIEnable	0x8
+#define bZebra1_TRxControl	0xc00
+#define bZebra1_TRxGainSetting	0x07f
+#define bZebra1_RxCorner	0xc00
+#define bZebra1_TxChargePump	0x38
+#define bZebra1_RxChargePump	0x7
+#define bZebra1_ChannelNum	0xf80
+#define bZebra1_TxLPFBW	0x400
+#define bZebra1_RxLPFBW	0x600
+
+/* Zebra4 */
+#define bRTL8256RegModeCtrl1	0x100
+#define bRTL8256RegModeCtrl0	0x40
+#define bRTL8256_TxLPFBW	0x18
+#define bRTL8256_RxLPFBW	0x600
+
+/* RTL8258 */
+#define bRTL8258_TxLPFBW	0xc
+#define bRTL8258_RxLPFBW	0xc00
+#define bRTL8258_RSSILPFBW	0xc0
+
+/* byte enable for sb_write */
+#define bByte0	0x1
+#define bByte1	0x2
+#define bByte2	0x4
+#define bByte3	0x8
+#define bWord0	0x3
+#define bWord1	0xc
+#define bDWord	0xf
+
+/* for PutRegsetting & GetRegSetting BitMask */
+#define bMaskByte0	0xff
+#define bMaskByte1	0xff00
+#define bMaskByte2	0xff0000
+#define bMaskByte3	0xff000000
+#define bMaskHWord	0xffff0000
+#define bMaskLWord	0x0000ffff
+#define bMaskDWord	0xffffffff
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+#define bMask12Bits	0xfff
+
+#define bEnable		0x1
+#define bDisable	0x0
+
+#define LeftAntenna	0x0
+#define RightAntenna	0x1
+
+#define tCheckTxStatus		500 /* 500 ms */
+#define tUpdateRxCounter	100 /* 100 ms */
+
+#define rateCCK		0
+#define rateOFDM	1
+#define rateHT		2
+
+#define bPMAC_End	0x1ff /* define Register-End */
+#define bFPGAPHY0_End	0x8ff
+#define bFPGAPHY1_End	0x9ff
+#define bCCKPHY0_End	0xaff
+#define bOFDMPHY0_End	0xcff
+#define bOFDMPHY1_End	0xdff
+
+
+#define bPMACControl	0x0
+#define bWMACControl	0x1
+#define bWNICControl	0x2
+
+#define PathA	0x0
+#define PathB	0x1
+#define PathC	0x2
+#define PathD	0x3
+
+#define rRTL8256RxMixerPole	0xb
+#define bZebraRxMixerPole	0x6
+#define rRTL8256TxBBOPBias	0x9
+#define bRTL8256TxBBOPBias	0x400
+#define rRTL8256TxBBBW		19
+#define bRTL8256TxBBBW		0x18
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
new file mode 100644
index 000000000..41faeb4b9
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
@@ -0,0 +1,241 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_phy.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h" /* RTL8225 Radio frontend */
+#include "r8192E_cmdpkt.h"
+
+void rtl92e_cam_reset(struct net_device *dev)
+{
+	u32 ulcommand = 0;
+
+	ulcommand |= BIT31|BIT30;
+	rtl92e_writel(dev, RWCAM, ulcommand);
+}
+
+void rtl92e_enable_hw_security_config(struct net_device *dev)
+{
+	u8 SECR_value = 0x0;
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	SECR_value = SCR_TxEncEnable | SCR_RxDecEnable;
+	if (((ieee->pairwise_key_type == KEY_TYPE_WEP40) ||
+	     (ieee->pairwise_key_type == KEY_TYPE_WEP104)) &&
+	     (priv->rtllib->auth_mode != 2)) {
+		SECR_value |= SCR_RxUseDK;
+		SECR_value |= SCR_TxUseDK;
+	} else if ((ieee->iw_mode == IW_MODE_ADHOC) &&
+		   (ieee->pairwise_key_type & (KEY_TYPE_CCMP |
+		   KEY_TYPE_TKIP))) {
+		SECR_value |= SCR_RxUseDK;
+		SECR_value |= SCR_TxUseDK;
+	}
+
+
+	ieee->hwsec_active = 1;
+	if ((ieee->pHTInfo->iot_action & HT_IOT_ACT_PURE_N_MODE) || !hwwep) {
+		ieee->hwsec_active = 0;
+		SECR_value &= ~SCR_RxDecEnable;
+	}
+	rtl92e_writeb(dev, SECR, SECR_value);
+}
+
+void rtl92e_set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex,
+		      u16 KeyType, const u8 *MacAddr, u8 DefaultKey,
+		      u32 *KeyContent, u8 is_mesh)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (EntryNo >= TOTAL_CAM_ENTRY)
+		return;
+
+	if (!is_mesh) {
+		ieee->swcamtable[EntryNo].bused = true;
+		ieee->swcamtable[EntryNo].key_index = KeyIndex;
+		ieee->swcamtable[EntryNo].key_type = KeyType;
+		memcpy(ieee->swcamtable[EntryNo].macaddr, MacAddr, 6);
+		ieee->swcamtable[EntryNo].useDK = DefaultKey;
+		memcpy(ieee->swcamtable[EntryNo].key_buf, (u8 *)KeyContent, 16);
+	}
+}
+
+void rtl92e_set_key(struct net_device *dev, u8 EntryNo, u8 KeyIndex,
+		    u16 KeyType, const u8 *MacAddr, u8 DefaultKey,
+		    u32 *KeyContent)
+{
+	u32 TargetCommand = 0;
+	u32 TargetContent = 0;
+	u16 usConfig = 0;
+	u8 i;
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	enum rt_rf_power_state rt_state;
+
+	rt_state = priv->rtllib->rf_power_state;
+	if (priv->rtllib->PowerSaveControl.bInactivePs) {
+		if (rt_state == rf_off) {
+			if (priv->rtllib->rf_off_reason > RF_CHANGE_BY_IPS) {
+				netdev_warn(dev, "%s(): RF is OFF.\n",
+					    __func__);
+				return;
+			}
+			mutex_lock(&priv->rtllib->ips_mutex);
+			rtl92e_ips_leave(dev);
+			mutex_unlock(&priv->rtllib->ips_mutex);
+		}
+	}
+	priv->rtllib->is_set_key = true;
+	if (EntryNo >= TOTAL_CAM_ENTRY) {
+		netdev_info(dev, "%s(): Invalid CAM entry\n", __func__);
+		return;
+	}
+
+	if (DefaultKey)
+		usConfig |= BIT15 | (KeyType<<2);
+	else
+		usConfig |= BIT15 | (KeyType<<2) | KeyIndex;
+
+
+	for (i = 0; i < CAM_CONTENT_COUNT; i++) {
+		TargetCommand  = i + CAM_CONTENT_COUNT * EntryNo;
+		TargetCommand |= BIT31|BIT16;
+
+		if (i == 0) {
+			TargetContent = (u32)(*(MacAddr+0)) << 16 |
+				(u32)(*(MacAddr+1)) << 24 |
+				(u32)usConfig;
+
+			rtl92e_writel(dev, WCAMI, TargetContent);
+			rtl92e_writel(dev, RWCAM, TargetCommand);
+		} else if (i == 1) {
+			TargetContent = (u32)(*(MacAddr+2)) |
+				(u32)(*(MacAddr+3)) <<  8 |
+				(u32)(*(MacAddr+4)) << 16 |
+				(u32)(*(MacAddr+5)) << 24;
+			rtl92e_writel(dev, WCAMI, TargetContent);
+			rtl92e_writel(dev, RWCAM, TargetCommand);
+		} else {
+			if (KeyContent != NULL) {
+				rtl92e_writel(dev, WCAMI,
+					      (u32)(*(KeyContent+i-2)));
+				rtl92e_writel(dev, RWCAM, TargetCommand);
+				udelay(100);
+			}
+		}
+	}
+}
+
+void rtl92e_cam_restore(struct net_device *dev)
+{
+	u8 EntryId = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 *MacAddr = priv->rtllib->current_network.bssid;
+
+	static u8	CAM_CONST_ADDR[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8	CAM_CONST_BROAD[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if ((priv->rtllib->pairwise_key_type == KEY_TYPE_WEP40) ||
+	    (priv->rtllib->pairwise_key_type == KEY_TYPE_WEP104)) {
+
+		for (EntryId = 0; EntryId < 4; EntryId++) {
+			MacAddr = CAM_CONST_ADDR[EntryId];
+			if (priv->rtllib->swcamtable[EntryId].bused) {
+				rtl92e_set_key(dev, EntryId, EntryId,
+					       priv->rtllib->pairwise_key_type,
+					       MacAddr, 0,
+					       (u32 *)(&priv->rtllib->swcamtable
+						       [EntryId].key_buf[0]));
+			}
+		}
+
+	} else if (priv->rtllib->pairwise_key_type == KEY_TYPE_TKIP) {
+		if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
+			rtl92e_set_key(dev, 4, 0,
+				       priv->rtllib->pairwise_key_type,
+				       (const u8 *)dev->dev_addr, 0,
+				       (u32 *)(&priv->rtllib->swcamtable[4].key_buf[0]));
+		} else {
+			rtl92e_set_key(dev, 4, 0,
+				       priv->rtllib->pairwise_key_type,
+				       MacAddr, 0,
+				       (u32 *)(&priv->rtllib->swcamtable[4].key_buf[0]));
+		}
+
+	} else if (priv->rtllib->pairwise_key_type == KEY_TYPE_CCMP) {
+		if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
+			rtl92e_set_key(dev, 4, 0,
+				       priv->rtllib->pairwise_key_type,
+				       (const u8 *)dev->dev_addr, 0,
+				       (u32 *)(&priv->rtllib->swcamtable[4].key_buf[0]));
+		} else {
+			rtl92e_set_key(dev, 4, 0,
+				       priv->rtllib->pairwise_key_type, MacAddr,
+				       0, (u32 *)(&priv->rtllib->swcamtable[4].key_buf[0]));
+			}
+	}
+
+	if (priv->rtllib->group_key_type == KEY_TYPE_TKIP) {
+		MacAddr = CAM_CONST_BROAD;
+		for (EntryId = 1; EntryId < 4; EntryId++) {
+			if (priv->rtllib->swcamtable[EntryId].bused) {
+				rtl92e_set_key(dev, EntryId, EntryId,
+					       priv->rtllib->group_key_type,
+					       MacAddr, 0,
+					       (u32 *)(&priv->rtllib->swcamtable[EntryId].key_buf[0]));
+			}
+		}
+		if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
+			if (priv->rtllib->swcamtable[0].bused) {
+				rtl92e_set_key(dev, 0, 0,
+					       priv->rtllib->group_key_type,
+					       CAM_CONST_ADDR[0], 0,
+					       (u32 *)(&priv->rtllib->swcamtable[0].key_buf[0]));
+			} else {
+				netdev_warn(dev,
+					    "%s(): ADHOC TKIP: missing key entry.\n",
+					    __func__);
+				return;
+			}
+		}
+	} else if (priv->rtllib->group_key_type == KEY_TYPE_CCMP) {
+		MacAddr = CAM_CONST_BROAD;
+		for (EntryId = 1; EntryId < 4; EntryId++) {
+			if (priv->rtllib->swcamtable[EntryId].bused) {
+				rtl92e_set_key(dev, EntryId, EntryId,
+					       priv->rtllib->group_key_type,
+					       MacAddr, 0,
+					       (u32 *)(&priv->rtllib->swcamtable[EntryId].key_buf[0]));
+			}
+		}
+
+		if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
+			if (priv->rtllib->swcamtable[0].bused) {
+				rtl92e_set_key(dev, 0, 0,
+					       priv->rtllib->group_key_type,
+					       CAM_CONST_ADDR[0], 0,
+					       (u32 *)(&priv->rtllib->swcamtable[0].key_buf[0]));
+			} else {
+				netdev_warn(dev,
+					    "%s(): ADHOC CCMP: missing key entry.\n",
+					    __func__);
+				return;
+			}
+		}
+	}
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
new file mode 100644
index 000000000..1ebd92e27
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL_CAM_H
+#define _RTL_CAM_H
+
+#include <linux/types.h>
+struct net_device;
+
+void rtl92e_cam_reset(struct net_device *dev);
+void rtl92e_enable_hw_security_config(struct net_device *dev);
+void rtl92e_set_key(struct net_device *dev, u8 EntryNo, u8 KeyIndex,
+		    u16 KeyType, const u8 *MacAddr, u8 DefaultKey,
+		    u32 *KeyContent);
+void rtl92e_set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex,
+		      u16 KeyType, const u8 *MacAddr, u8 DefaultKey,
+		      u32 *KeyContent, u8 is_mesh);
+void rtl92e_cam_restore(struct net_device *dev);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
new file mode 100644
index 000000000..c1e500841
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -0,0 +1,2510 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include <linux/uaccess.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/ieee80211.h>
+#include "rtl_core.h"
+#include "r8192E_phy.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h"
+#include "r8192E_cmdpkt.h"
+
+#include "rtl_wx.h"
+#include "rtl_dm.h"
+
+#include "rtl_pm.h"
+
+int hwwep = 1;
+static char *ifname = "wlan%d";
+
+static const struct rtl819x_ops rtl819xp_ops = {
+	.nic_type			= NIC_8192E,
+	.get_eeprom_size		= rtl92e_get_eeprom_size,
+	.init_adapter_variable		= rtl92e_init_variables,
+	.initialize_adapter		= rtl92e_start_adapter,
+	.link_change			= rtl92e_link_change,
+	.tx_fill_descriptor		= rtl92e_fill_tx_desc,
+	.tx_fill_cmd_descriptor		= rtl92e_fill_tx_cmd_desc,
+	.rx_query_status_descriptor	= rtl92e_get_rx_stats,
+	.rx_command_packet_handler = NULL,
+	.stop_adapter			= rtl92e_stop_adapter,
+	.update_ratr_table		= rtl92e_update_ratr_table,
+	.irq_enable			= rtl92e_enable_irq,
+	.irq_disable			= rtl92e_disable_irq,
+	.irq_clear			= rtl92e_clear_irq,
+	.rx_enable			= rtl92e_enable_rx,
+	.tx_enable			= rtl92e_enable_tx,
+	.interrupt_recognized		= rtl92e_ack_irq,
+	.tx_check_stuck_handler	= rtl92e_is_tx_stuck,
+	.rx_check_stuck_handler	= rtl92e_is_rx_stuck,
+};
+
+static struct pci_device_id rtl8192_pci_id_tbl[] = {
+	{RTL_PCI_DEVICE(0x10ec, 0x8192, rtl819xp_ops)},
+	{RTL_PCI_DEVICE(0x07aa, 0x0044, rtl819xp_ops)},
+	{RTL_PCI_DEVICE(0x07aa, 0x0047, rtl819xp_ops)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(pci, rtl8192_pci_id_tbl);
+
+static int _rtl92e_pci_probe(struct pci_dev *pdev,
+			     const struct pci_device_id *id);
+static void _rtl92e_pci_disconnect(struct pci_dev *pdev);
+static irqreturn_t _rtl92e_irq(int irq, void *netdev);
+
+static SIMPLE_DEV_PM_OPS(rtl92e_pm_ops, rtl92e_suspend, rtl92e_resume);
+
+static struct pci_driver rtl8192_pci_driver = {
+	.name = DRV_NAME,	/* Driver name   */
+	.id_table = rtl8192_pci_id_tbl,	/* PCI_ID table  */
+	.probe	= _rtl92e_pci_probe,	/* probe fn      */
+	.remove	 = _rtl92e_pci_disconnect,	/* remove fn */
+	.driver.pm = &rtl92e_pm_ops,
+};
+
+static short _rtl92e_is_tx_queue_empty(struct net_device *dev);
+static void _rtl92e_watchdog_wq_cb(void *data);
+static void _rtl92e_watchdog_timer_cb(struct timer_list *t);
+static void _rtl92e_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
+				   int rate);
+static int _rtl92e_hard_start_xmit(struct sk_buff *skb, struct net_device *dev);
+static void _rtl92e_tx_cmd(struct net_device *dev, struct sk_buff *skb);
+static short _rtl92e_tx(struct net_device *dev, struct sk_buff *skb);
+static short _rtl92e_pci_initdescring(struct net_device *dev);
+static void _rtl92e_irq_tx_tasklet(struct tasklet_struct *t);
+static void _rtl92e_irq_rx_tasklet(struct tasklet_struct *t);
+static void _rtl92e_cancel_deferred_work(struct r8192_priv *priv);
+static int _rtl92e_up(struct net_device *dev, bool is_silent_reset);
+static int _rtl92e_try_up(struct net_device *dev);
+static int _rtl92e_down(struct net_device *dev, bool shutdownrf);
+static void _rtl92e_restart(void *data);
+
+/****************************************************************************
+ *  -----------------------------IO STUFF-------------------------
+ ****************************************************************************/
+
+u8 rtl92e_readb(struct net_device *dev, int x)
+{
+	return 0xff & readb((u8 __iomem *)dev->mem_start + x);
+}
+
+u32 rtl92e_readl(struct net_device *dev, int x)
+{
+	return readl((u8 __iomem *)dev->mem_start + x);
+}
+
+u16 rtl92e_readw(struct net_device *dev, int x)
+{
+	return readw((u8 __iomem *)dev->mem_start + x);
+}
+
+void rtl92e_writeb(struct net_device *dev, int x, u8 y)
+{
+	writeb(y, (u8 __iomem *)dev->mem_start + x);
+
+	udelay(20);
+}
+
+void rtl92e_writel(struct net_device *dev, int x, u32 y)
+{
+	writel(y, (u8 __iomem *)dev->mem_start + x);
+
+	udelay(20);
+}
+
+void rtl92e_writew(struct net_device *dev, int x, u16 y)
+{
+	writew(y, (u8 __iomem *)dev->mem_start + x);
+
+	udelay(20);
+}
+
+/****************************************************************************
+ *  -----------------------------GENERAL FUNCTION-------------------------
+ ****************************************************************************/
+bool rtl92e_set_rf_state(struct net_device *dev,
+			 enum rt_rf_power_state state_to_set,
+			 RT_RF_CHANGE_SOURCE change_source)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	bool action_allowed = false;
+	bool connect_by_ssid = false;
+	enum rt_rf_power_state rt_state;
+	u16 rf_wait_counter = 0;
+	unsigned long flag;
+
+	while (true) {
+		spin_lock_irqsave(&priv->rf_ps_lock, flag);
+		if (priv->rf_change_in_progress) {
+			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+
+			while (priv->rf_change_in_progress) {
+				rf_wait_counter++;
+				mdelay(1);
+
+				if (rf_wait_counter > 100) {
+					netdev_warn(dev,
+						    "%s(): Timeout waiting for RF change.\n",
+						    __func__);
+					return false;
+				}
+			}
+		} else {
+			priv->rf_change_in_progress = true;
+			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+			break;
+		}
+	}
+
+	rt_state = priv->rtllib->rf_power_state;
+
+	switch (state_to_set) {
+	case rf_on:
+		priv->rtllib->rf_off_reason &= (~change_source);
+
+		if ((change_source == RF_CHANGE_BY_HW) && priv->hw_radio_off)
+			priv->hw_radio_off = false;
+
+		if (!priv->rtllib->rf_off_reason) {
+			priv->rtllib->rf_off_reason = 0;
+			action_allowed = true;
+
+			if (rt_state == rf_off &&
+			    change_source >= RF_CHANGE_BY_HW)
+				connect_by_ssid = true;
+		}
+		break;
+
+	case rf_off:
+
+		if ((priv->rtllib->iw_mode == IW_MODE_INFRA) ||
+		    (priv->rtllib->iw_mode == IW_MODE_ADHOC)) {
+			if ((priv->rtllib->rf_off_reason > RF_CHANGE_BY_IPS) ||
+			    (change_source > RF_CHANGE_BY_IPS)) {
+				if (ieee->state == RTLLIB_LINKED)
+					priv->blinked_ingpio = true;
+				else
+					priv->blinked_ingpio = false;
+				rtllib_MgntDisconnect(priv->rtllib,
+						      WLAN_REASON_DISASSOC_STA_HAS_LEFT);
+			}
+		}
+		if ((change_source == RF_CHANGE_BY_HW) && !priv->hw_radio_off)
+			priv->hw_radio_off = true;
+		priv->rtllib->rf_off_reason |= change_source;
+		action_allowed = true;
+		break;
+
+	case rf_sleep:
+		priv->rtllib->rf_off_reason |= change_source;
+		action_allowed = true;
+		break;
+
+	default:
+		break;
+	}
+
+	if (action_allowed) {
+		rtl92e_set_rf_power_state(dev, state_to_set);
+		if (state_to_set == rf_on) {
+			if (connect_by_ssid && priv->blinked_ingpio) {
+				schedule_delayed_work(
+					 &ieee->associate_procedure_wq, 0);
+				priv->blinked_ingpio = false;
+			}
+		}
+	}
+
+	spin_lock_irqsave(&priv->rf_ps_lock, flag);
+	priv->rf_change_in_progress = false;
+	spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+	return action_allowed;
+}
+
+static short _rtl92e_check_nic_enough_desc(struct net_device *dev, int prio)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtl8192_tx_ring *ring = &priv->tx_ring[prio];
+
+	if (ring->entries - skb_queue_len(&ring->queue) >= 2)
+		return 1;
+	return 0;
+}
+
+static void _rtl92e_tx_timeout(struct net_device *dev, unsigned int txqueue)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	schedule_work(&priv->reset_wq);
+	netdev_info(dev, "TXTIMEOUT");
+}
+
+void rtl92e_irq_enable(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->irq_enabled = 1;
+
+	priv->ops->irq_enable(dev);
+}
+
+void rtl92e_irq_disable(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->ops->irq_disable(dev);
+
+	priv->irq_enabled = 0;
+}
+
+static void _rtl92e_set_chan(struct net_device *dev, short ch)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->chan_forced)
+		return;
+
+	priv->chan = ch;
+
+	if (priv->rf_set_chan)
+		priv->rf_set_chan(dev, priv->chan);
+}
+
+static void _rtl92e_update_cap(struct net_device *dev, u16 cap)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_network *net = &priv->rtllib->current_network;
+	bool		ShortPreamble;
+
+	if (cap & WLAN_CAPABILITY_SHORT_PREAMBLE) {
+		if (priv->dot11_current_preamble_mode != PREAMBLE_SHORT) {
+			ShortPreamble = true;
+			priv->dot11_current_preamble_mode = PREAMBLE_SHORT;
+			priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACK_PREAMBLE,
+					(unsigned char *)&ShortPreamble);
+		}
+	} else {
+		if (priv->dot11_current_preamble_mode != PREAMBLE_LONG) {
+			ShortPreamble = false;
+			priv->dot11_current_preamble_mode = PREAMBLE_LONG;
+			priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACK_PREAMBLE,
+					      (unsigned char *)&ShortPreamble);
+		}
+	}
+
+	if (net->mode & (IEEE_G | IEEE_N_24G)) {
+		u8	slot_time_val;
+		u8	cur_slot_time = priv->slot_time;
+
+		if ((cap & WLAN_CAPABILITY_SHORT_SLOT_TIME) &&
+		   (!priv->rtllib->pHTInfo->current_rt2rt_long_slot_time)) {
+			if (cur_slot_time != SHORT_SLOT_TIME) {
+				slot_time_val = SHORT_SLOT_TIME;
+				priv->rtllib->SetHwRegHandler(dev,
+					 HW_VAR_SLOT_TIME, &slot_time_val);
+			}
+		} else {
+			if (cur_slot_time != NON_SHORT_SLOT_TIME) {
+				slot_time_val = NON_SHORT_SLOT_TIME;
+				priv->rtllib->SetHwRegHandler(dev,
+					 HW_VAR_SLOT_TIME, &slot_time_val);
+			}
+		}
+	}
+}
+
+static const struct rtllib_qos_parameters def_qos_parameters = {
+	{cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3)},
+	{cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7)},
+	{2, 2, 2, 2},
+	{0, 0, 0, 0},
+	{0, 0, 0, 0}
+};
+
+static void _rtl92e_update_beacon(void *data)
+{
+	struct r8192_priv *priv = container_of_work_rsl(data, struct r8192_priv,
+				  update_beacon_wq.work);
+	struct net_device *dev = priv->rtllib->dev;
+	struct rtllib_device *ieee = priv->rtllib;
+	struct rtllib_network *net = &ieee->current_network;
+
+	if (ieee->pHTInfo->bCurrentHTSupport)
+		HT_update_self_and_peer_setting(ieee, net);
+	ieee->pHTInfo->current_rt2rt_long_slot_time = net->bssht.bd_rt2rt_long_slot_time;
+	ieee->pHTInfo->RT2RT_HT_Mode = net->bssht.rt2rt_ht_mode;
+	_rtl92e_update_cap(dev, net->capability);
+}
+
+static void _rtl92e_qos_activate(void *data)
+{
+	struct r8192_priv *priv = container_of_work_rsl(data, struct r8192_priv,
+				  qos_activate);
+	struct net_device *dev = priv->rtllib->dev;
+	int i;
+
+	mutex_lock(&priv->mutex);
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		goto success;
+
+	for (i = 0; i <  QOS_QUEUE_NUM; i++)
+		priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM, (u8 *)(&i));
+
+success:
+	mutex_unlock(&priv->mutex);
+}
+
+static int _rtl92e_qos_handle_probe_response(struct r8192_priv *priv,
+					     int active_network,
+					     struct rtllib_network *network)
+{
+	int ret = 0;
+	u32 size = sizeof(struct rtllib_qos_parameters);
+
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		return ret;
+
+	if (priv->rtllib->iw_mode != IW_MODE_INFRA)
+		return ret;
+
+	if (network->flags & NETWORK_HAS_QOS_MASK) {
+		if (active_network &&
+				(network->flags & NETWORK_HAS_QOS_PARAMETERS))
+			network->qos_data.active = network->qos_data.supported;
+
+		if ((network->qos_data.active == 1) && (active_network == 1) &&
+				(network->flags & NETWORK_HAS_QOS_PARAMETERS) &&
+				(network->qos_data.old_param_count !=
+				network->qos_data.param_count)) {
+			network->qos_data.old_param_count =
+				network->qos_data.param_count;
+			priv->rtllib->wmm_acm = network->qos_data.wmm_acm;
+			schedule_work(&priv->qos_activate);
+		}
+	} else {
+		memcpy(&priv->rtllib->current_network.qos_data.parameters,
+		       &def_qos_parameters, size);
+
+		if ((network->qos_data.active == 1) && (active_network == 1))
+			schedule_work(&priv->qos_activate);
+
+		network->qos_data.active = 0;
+		network->qos_data.supported = 0;
+	}
+
+	return 0;
+}
+
+static int _rtl92e_handle_beacon(struct net_device *dev,
+				 struct rtllib_beacon *beacon,
+				 struct rtllib_network *network)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	_rtl92e_qos_handle_probe_response(priv, 1, network);
+
+	schedule_delayed_work(&priv->update_beacon_wq, 0);
+	return 0;
+}
+
+static int _rtl92e_qos_assoc_resp(struct r8192_priv *priv,
+				  struct rtllib_network *network)
+{
+	unsigned long flags;
+	u32 size = sizeof(struct rtllib_qos_parameters);
+	int set_qos_param = 0;
+
+	if (!priv || !network)
+		return 0;
+
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		return 0;
+
+	if (priv->rtllib->iw_mode != IW_MODE_INFRA)
+		return 0;
+
+	spin_lock_irqsave(&priv->rtllib->lock, flags);
+	if (network->flags & NETWORK_HAS_QOS_PARAMETERS) {
+		memcpy(&priv->rtllib->current_network.qos_data.parameters,
+		       &network->qos_data.parameters,
+		       sizeof(struct rtllib_qos_parameters));
+		priv->rtllib->current_network.qos_data.active = 1;
+		priv->rtllib->wmm_acm = network->qos_data.wmm_acm;
+		set_qos_param = 1;
+		priv->rtllib->current_network.qos_data.old_param_count =
+			priv->rtllib->current_network.qos_data.param_count;
+		priv->rtllib->current_network.qos_data.param_count =
+			network->qos_data.param_count;
+	} else {
+		memcpy(&priv->rtllib->current_network.qos_data.parameters,
+		&def_qos_parameters, size);
+		priv->rtllib->current_network.qos_data.active = 0;
+		priv->rtllib->current_network.qos_data.supported = 0;
+		set_qos_param = 1;
+	}
+
+	spin_unlock_irqrestore(&priv->rtllib->lock, flags);
+
+	if (set_qos_param == 1) {
+		rtl92e_dm_init_edca_turbo(priv->rtllib->dev);
+		schedule_work(&priv->qos_activate);
+	}
+	return 0;
+}
+
+static int _rtl92e_handle_assoc_response(struct net_device *dev,
+				 struct rtllib_assoc_response_frame *resp,
+				 struct rtllib_network *network)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	_rtl92e_qos_assoc_resp(priv, network);
+	return 0;
+}
+
+static void _rtl92e_prepare_beacon(struct tasklet_struct *t)
+{
+	struct r8192_priv *priv = from_tasklet(priv, t,
+					       irq_prepare_beacon_tasklet);
+	struct net_device *dev = priv->rtllib->dev;
+	struct sk_buff *pskb = NULL, *pnewskb = NULL;
+	struct cb_desc *tcb_desc = NULL;
+	struct rtl8192_tx_ring *ring = NULL;
+	struct tx_desc *pdesc = NULL;
+
+	ring = &priv->tx_ring[BEACON_QUEUE];
+	pskb = __skb_dequeue(&ring->queue);
+	kfree_skb(pskb);
+
+	pnewskb = rtllib_get_beacon(priv->rtllib);
+	if (!pnewskb)
+		return;
+
+	tcb_desc = (struct cb_desc *)(pnewskb->cb + 8);
+	tcb_desc->queue_index = BEACON_QUEUE;
+	tcb_desc->data_rate = 2;
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+	skb_push(pnewskb, priv->rtllib->tx_headroom);
+
+	pdesc = &ring->desc[0];
+	priv->ops->tx_fill_descriptor(dev, pdesc, tcb_desc, pnewskb);
+	__skb_queue_tail(&ring->queue, pnewskb);
+	pdesc->OWN = 1;
+}
+
+static void _rtl92e_stop_beacon(struct net_device *dev)
+{
+}
+
+void rtl92e_config_rate(struct net_device *dev, u16 *rate_config)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_network *net;
+	u8 i = 0, basic_rate = 0;
+
+	net = &priv->rtllib->current_network;
+
+	for (i = 0; i < net->rates_len; i++) {
+		basic_rate = net->rates[i] & 0x7f;
+		switch (basic_rate) {
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
+		}
+	}
+
+	for (i = 0; i < net->rates_ex_len; i++) {
+		basic_rate = net->rates_ex[i] & 0x7f;
+		switch (basic_rate) {
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
+		}
+	}
+}
+
+static void _rtl92e_refresh_support_rate(struct r8192_priv *priv)
+{
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (ieee->mode == WIRELESS_MODE_N_24G ||
+	    ieee->mode == WIRELESS_MODE_N_5G) {
+		memcpy(ieee->Regdot11HTOperationalRateSet,
+		       ieee->RegHTSuppRateSet, 16);
+		memcpy(ieee->Regdot11TxHTOperationalRateSet,
+		       ieee->RegHTSuppRateSet, 16);
+
+	} else {
+		memset(ieee->Regdot11HTOperationalRateSet, 0, 16);
+	}
+}
+
+static u8 _rtl92e_get_supported_wireless_mode(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 ret = 0;
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+	case RF_8256:
+	case RF_6052:
+	case RF_PSEUDO_11N:
+		ret = (WIRELESS_MODE_N_24G | WIRELESS_MODE_G | WIRELESS_MODE_B);
+		break;
+	case RF_8258:
+		ret = (WIRELESS_MODE_A | WIRELESS_MODE_N_5G);
+		break;
+	default:
+		ret = WIRELESS_MODE_B;
+		break;
+	}
+	return ret;
+}
+
+void rtl92e_set_wireless_mode(struct net_device *dev, u8 wireless_mode)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 bSupportMode = _rtl92e_get_supported_wireless_mode(dev);
+
+	if ((wireless_mode == WIRELESS_MODE_AUTO) ||
+	    ((wireless_mode & bSupportMode) == 0)) {
+		if (bSupportMode & WIRELESS_MODE_N_24G) {
+			wireless_mode = WIRELESS_MODE_N_24G;
+		} else if (bSupportMode & WIRELESS_MODE_N_5G) {
+			wireless_mode = WIRELESS_MODE_N_5G;
+		} else if ((bSupportMode & WIRELESS_MODE_A)) {
+			wireless_mode = WIRELESS_MODE_A;
+		} else if ((bSupportMode & WIRELESS_MODE_G)) {
+			wireless_mode = WIRELESS_MODE_G;
+		} else if ((bSupportMode & WIRELESS_MODE_B)) {
+			wireless_mode = WIRELESS_MODE_B;
+		} else {
+			netdev_info(dev,
+				    "%s(): Unsupported mode requested. Fallback to 802.11b\n",
+				    __func__);
+			wireless_mode = WIRELESS_MODE_B;
+		}
+	}
+
+	if ((wireless_mode & (WIRELESS_MODE_B | WIRELESS_MODE_G)) ==
+	    (WIRELESS_MODE_G | WIRELESS_MODE_B))
+		wireless_mode = WIRELESS_MODE_G;
+
+	priv->rtllib->mode = wireless_mode;
+
+	if ((wireless_mode == WIRELESS_MODE_N_24G) ||
+	    (wireless_mode == WIRELESS_MODE_N_5G)) {
+		priv->rtllib->pHTInfo->bEnableHT = 1;
+	} else {
+		priv->rtllib->pHTInfo->bEnableHT = 0;
+	}
+	_rtl92e_refresh_support_rate(priv);
+}
+
+static int _rtl92e_sta_up(struct net_device *dev, bool is_silent_reset)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&priv->rtllib->PowerSaveControl);
+	bool init_status;
+
+	priv->bDriverIsGoingToUnload = false;
+	priv->bdisable_nic = false;
+
+	priv->up = 1;
+	priv->rtllib->ieee_up = 1;
+
+	priv->up_first_time = 0;
+	priv->bfirst_init = true;
+	init_status = priv->ops->initialize_adapter(dev);
+	if (!init_status) {
+		netdev_err(dev, "%s(): Initialization failed!\n", __func__);
+		priv->bfirst_init = false;
+		return -1;
+	}
+
+	RT_CLEAR_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC);
+	priv->bfirst_init = false;
+
+	if (priv->polling_timer_on == 0)
+		rtl92e_check_rfctrl_gpio_timer(&priv->gpio_polling_timer);
+
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		rtllib_softmac_start_protocol(priv->rtllib, 0);
+	rtllib_reset_queue(priv->rtllib);
+	_rtl92e_watchdog_timer_cb(&priv->watch_dog_timer);
+
+	if (!netif_queue_stopped(dev))
+		netif_start_queue(dev);
+	else
+		netif_wake_queue(dev);
+
+	priv->bfirst_after_down = false;
+	return 0;
+}
+
+static int _rtl92e_sta_down(struct net_device *dev, bool shutdownrf)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	unsigned long flags = 0;
+	u8 RFInProgressTimeOut = 0;
+
+	if (priv->up == 0)
+		return -1;
+
+	if (priv->rtllib->rtllib_ips_leave)
+		priv->rtllib->rtllib_ips_leave(dev);
+
+	if (priv->rtllib->state == RTLLIB_LINKED)
+		rtl92e_leisure_ps_leave(dev);
+
+	priv->bDriverIsGoingToUnload = true;
+	priv->up = 0;
+	priv->rtllib->ieee_up = 0;
+	priv->bfirst_after_down = true;
+	if (!netif_queue_stopped(dev))
+		netif_stop_queue(dev);
+
+	priv->rtllib->wpa_ie_len = 0;
+	kfree(priv->rtllib->wpa_ie);
+	priv->rtllib->wpa_ie = NULL;
+	rtl92e_cam_reset(dev);
+	memset(priv->rtllib->swcamtable, 0, sizeof(struct sw_cam_table) * 32);
+	rtl92e_irq_disable(dev);
+
+	del_timer_sync(&priv->watch_dog_timer);
+	_rtl92e_cancel_deferred_work(priv);
+	cancel_delayed_work(&priv->rtllib->hw_wakeup_wq);
+
+	rtllib_softmac_stop_protocol(priv->rtllib, 0, true);
+	spin_lock_irqsave(&priv->rf_ps_lock, flags);
+	while (priv->rf_change_in_progress) {
+		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+		if (RFInProgressTimeOut > 100) {
+			spin_lock_irqsave(&priv->rf_ps_lock, flags);
+			break;
+		}
+		mdelay(1);
+		RFInProgressTimeOut++;
+		spin_lock_irqsave(&priv->rf_ps_lock, flags);
+	}
+	priv->rf_change_in_progress = true;
+	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+	priv->ops->stop_adapter(dev, false);
+	spin_lock_irqsave(&priv->rf_ps_lock, flags);
+	priv->rf_change_in_progress = false;
+	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+	udelay(100);
+	memset(&priv->rtllib->current_network, 0,
+	       offsetof(struct rtllib_network, list));
+
+	return 0;
+}
+
+static void _rtl92e_init_priv_handler(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->rtllib->softmac_hard_start_xmit	= _rtl92e_hard_start_xmit;
+	priv->rtllib->set_chan			= _rtl92e_set_chan;
+	priv->rtllib->link_change		= priv->ops->link_change;
+	priv->rtllib->softmac_data_hard_start_xmit = _rtl92e_hard_data_xmit;
+	priv->rtllib->check_nic_enough_desc	= _rtl92e_check_nic_enough_desc;
+	priv->rtllib->handle_assoc_response	= _rtl92e_handle_assoc_response;
+	priv->rtllib->handle_beacon		= _rtl92e_handle_beacon;
+	priv->rtllib->SetWirelessMode		= rtl92e_set_wireless_mode;
+	priv->rtllib->LeisurePSLeave		= rtl92e_leisure_ps_leave;
+	priv->rtllib->SetBWModeHandler		= rtl92e_set_bw_mode;
+	priv->rf_set_chan			= rtl92e_set_channel;
+
+	priv->rtllib->start_send_beacons = rtl92e_start_beacon;
+	priv->rtllib->stop_send_beacons = _rtl92e_stop_beacon;
+
+	priv->rtllib->sta_wake_up = rtl92e_hw_wakeup;
+	priv->rtllib->enter_sleep_state = rtl92e_enter_sleep;
+	priv->rtllib->ps_is_queue_empty = _rtl92e_is_tx_queue_empty;
+
+	priv->rtllib->GetNmodeSupportBySecCfg = rtl92e_get_nmode_support_by_sec;
+	priv->rtllib->GetHalfNmodeSupportByAPsHandler =
+						rtl92e_is_halfn_supported_by_ap;
+
+	priv->rtllib->SetHwRegHandler = rtl92e_set_reg;
+	priv->rtllib->AllowAllDestAddrHandler = rtl92e_set_monitor_mode;
+	priv->rtllib->SetFwCmdHandler = NULL;
+	priv->rtllib->InitialGainHandler = rtl92e_init_gain;
+	priv->rtllib->rtllib_ips_leave_wq = rtl92e_rtllib_ips_leave_wq;
+	priv->rtllib->rtllib_ips_leave = rtl92e_rtllib_ips_leave;
+
+	priv->rtllib->LedControlHandler = NULL;
+	priv->rtllib->UpdateBeaconInterruptHandler = NULL;
+
+	priv->rtllib->ScanOperationBackupHandler = rtl92e_scan_op_backup;
+}
+
+static void _rtl92e_init_priv_constant(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&priv->rtllib->PowerSaveControl;
+
+	pPSC->RegMaxLPSAwakeIntvl = 5;
+}
+
+static void _rtl92e_init_priv_variable(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 i;
+
+	priv->AcmMethod = eAcmWay2_SW;
+	priv->dot11_current_preamble_mode = PREAMBLE_AUTO;
+	priv->rtllib->status = 0;
+	priv->polling_timer_on = 0;
+	priv->up_first_time = 1;
+	priv->blinked_ingpio = false;
+	priv->bDriverIsGoingToUnload = false;
+	priv->being_init_adapter = false;
+	priv->initialized_at_probe = false;
+	priv->bdisable_nic = false;
+	priv->bfirst_init = false;
+	priv->txringcount = 64;
+	priv->rxbuffersize = 9100;
+	priv->rxringcount = MAX_RX_COUNT;
+	priv->irq_enabled = 0;
+	priv->chan = 1;
+	priv->RegChannelPlan = 0xf;
+	priv->rtllib->mode = WIRELESS_MODE_AUTO;
+	priv->rtllib->iw_mode = IW_MODE_INFRA;
+	priv->rtllib->bNetPromiscuousMode = false;
+	priv->rtllib->IntelPromiscuousModeInfo.bPromiscuousOn = false;
+	priv->rtllib->IntelPromiscuousModeInfo.bFilterSourceStationFrame =
+								 false;
+	priv->rtllib->ieee_up = 0;
+	priv->retry_rts = DEFAULT_RETRY_RTS;
+	priv->retry_data = DEFAULT_RETRY_DATA;
+	priv->rtllib->rts = DEFAULT_RTS_THRESHOLD;
+	priv->rtllib->rate = 110;
+	priv->rtllib->short_slot = 1;
+	priv->promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
+	priv->bcck_in_ch14 = false;
+	priv->bfsync_processing  = false;
+	priv->CCKPresentAttentuation = 0;
+	priv->rfa_txpowertrackingindex = 0;
+	priv->rfc_txpowertrackingindex = 0;
+	priv->CckPwEnl = 6;
+	priv->ScanDelay = 50;
+	priv->ResetProgress = RESET_TYPE_NORESET;
+	priv->bForcedSilentReset = false;
+	priv->bDisableNormalResetCheck = false;
+	priv->force_reset = false;
+	memset(priv->rtllib->swcamtable, 0, sizeof(struct sw_cam_table) * 32);
+
+	memset(&priv->InterruptLog, 0, sizeof(struct log_int_8190));
+	priv->RxCounter = 0;
+	priv->rtllib->wx_set_enc = 0;
+	priv->hw_radio_off = false;
+	priv->RegRfOff = false;
+	priv->isRFOff = false;
+	priv->bInPowerSaveMode = false;
+	priv->rtllib->rf_off_reason = 0;
+	priv->rf_change_in_progress = false;
+	priv->bHwRfOffAction = 0;
+	priv->SetRFPowerStateInProgress = false;
+	priv->rtllib->PowerSaveControl.bInactivePs = true;
+	priv->rtllib->PowerSaveControl.bIPSModeBackup = false;
+	priv->rtllib->PowerSaveControl.bLeisurePs = true;
+	priv->rtllib->PowerSaveControl.bFwCtrlLPS = false;
+	priv->rtllib->LPSDelayCnt = 0;
+	priv->rtllib->sta_sleep = LPS_IS_WAKE;
+	priv->rtllib->rf_power_state = rf_on;
+
+	priv->rtllib->current_network.beacon_interval = DEFAULT_BEACONINTERVAL;
+	priv->rtllib->iw_mode = IW_MODE_INFRA;
+	priv->rtllib->active_scan = 1;
+	priv->rtllib->be_scan_inprogress = false;
+	priv->rtllib->modulation = RTLLIB_CCK_MODULATION |
+				   RTLLIB_OFDM_MODULATION;
+	priv->rtllib->host_encrypt = 1;
+	priv->rtllib->host_decrypt = 1;
+
+	priv->rtllib->fts = DEFAULT_FRAG_THRESHOLD;
+
+	priv->card_type = PCI;
+
+	priv->pFirmware = vzalloc(sizeof(struct rt_firmware));
+	if (!priv->pFirmware)
+		netdev_err(dev,
+			   "rtl8192e: Unable to allocate space for firmware\n");
+
+	skb_queue_head_init(&priv->skb_queue);
+
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_head_init(&priv->rtllib->skb_waitQ[i]);
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_head_init(&priv->rtllib->skb_aggQ[i]);
+}
+
+static void _rtl92e_init_priv_lock(struct r8192_priv *priv)
+{
+	spin_lock_init(&priv->tx_lock);
+	spin_lock_init(&priv->irq_th_lock);
+	spin_lock_init(&priv->rf_ps_lock);
+	spin_lock_init(&priv->ps_lock);
+	mutex_init(&priv->wx_mutex);
+	mutex_init(&priv->rf_mutex);
+	mutex_init(&priv->mutex);
+}
+
+static void _rtl92e_init_priv_task(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	INIT_WORK_RSL(&priv->reset_wq, (void *)_rtl92e_restart, dev);
+	INIT_WORK_RSL(&priv->rtllib->ips_leave_wq, (void *)rtl92e_ips_leave_wq,
+		      dev);
+	INIT_DELAYED_WORK_RSL(&priv->watch_dog_wq,
+			      (void *)_rtl92e_watchdog_wq_cb, dev);
+	INIT_DELAYED_WORK_RSL(&priv->txpower_tracking_wq,
+			      (void *)rtl92e_dm_txpower_tracking_wq, dev);
+	INIT_DELAYED_WORK_RSL(&priv->rfpath_check_wq,
+			      (void *)rtl92e_dm_rf_pathcheck_wq, dev);
+	INIT_DELAYED_WORK_RSL(&priv->update_beacon_wq,
+			      (void *)_rtl92e_update_beacon, dev);
+	INIT_WORK_RSL(&priv->qos_activate, (void *)_rtl92e_qos_activate, dev);
+	INIT_DELAYED_WORK_RSL(&priv->rtllib->hw_wakeup_wq,
+			      (void *)rtl92e_hw_wakeup_wq, dev);
+	INIT_DELAYED_WORK_RSL(&priv->rtllib->hw_sleep_wq,
+			      (void *)rtl92e_hw_sleep_wq, dev);
+	tasklet_setup(&priv->irq_rx_tasklet, _rtl92e_irq_rx_tasklet);
+	tasklet_setup(&priv->irq_tx_tasklet, _rtl92e_irq_tx_tasklet);
+	tasklet_setup(&priv->irq_prepare_beacon_tasklet,
+		      _rtl92e_prepare_beacon);
+}
+
+static short _rtl92e_get_channel_map(struct net_device *dev)
+{
+	int i;
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if ((priv->rf_chip != RF_8225) && (priv->rf_chip != RF_8256) &&
+						(priv->rf_chip != RF_6052)) {
+		netdev_err(dev, "%s: unknown rf chip, can't set channel map\n",
+			   __func__);
+		return -1;
+	}
+
+	if (priv->ChannelPlan >= COUNTRY_CODE_MAX) {
+		netdev_info(dev,
+			    "rtl819x_init:Error channel plan! Set to default.\n");
+		priv->ChannelPlan = COUNTRY_CODE_FCC;
+	}
+	dot11d_init(priv->rtllib);
+	dot11d_channel_map(priv->ChannelPlan, priv->rtllib);
+	for (i = 1; i <= 11; i++)
+		(priv->rtllib->active_channel_map)[i] = 1;
+	(priv->rtllib->active_channel_map)[12] = 2;
+	(priv->rtllib->active_channel_map)[13] = 2;
+
+	return 0;
+}
+
+static short _rtl92e_init(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	memset(&priv->stats, 0, sizeof(struct rt_stats));
+
+	_rtl92e_init_priv_handler(dev);
+	_rtl92e_init_priv_constant(dev);
+	_rtl92e_init_priv_variable(dev);
+	_rtl92e_init_priv_lock(priv);
+	_rtl92e_init_priv_task(dev);
+	priv->ops->get_eeprom_size(dev);
+	priv->ops->init_adapter_variable(dev);
+	_rtl92e_get_channel_map(dev);
+
+	rtl92e_dm_init(dev);
+
+	timer_setup(&priv->watch_dog_timer, _rtl92e_watchdog_timer_cb, 0);
+
+	timer_setup(&priv->gpio_polling_timer, rtl92e_check_rfctrl_gpio_timer,
+		    0);
+
+	rtl92e_irq_disable(dev);
+	if (request_irq(dev->irq, _rtl92e_irq, IRQF_SHARED, dev->name, dev)) {
+		netdev_err(dev, "Error allocating IRQ %d", dev->irq);
+		return -1;
+	}
+
+	priv->irq = dev->irq;
+
+	if (_rtl92e_pci_initdescring(dev) != 0) {
+		netdev_err(dev, "Endopoints initialization failed");
+		free_irq(dev->irq, dev);
+		return -1;
+	}
+
+	return 0;
+}
+
+/***************************************************************************
+ * -------------------------------WATCHDOG STUFF---------------------------
+ **************************************************************************/
+static short _rtl92e_is_tx_queue_empty(struct net_device *dev)
+{
+	int i = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	for (i = 0; i <= MGNT_QUEUE; i++) {
+		if ((i == TXCMD_QUEUE) || (i == HCCA_QUEUE))
+			continue;
+		if (skb_queue_len(&(&priv->tx_ring[i])->queue) > 0) {
+			netdev_info(dev, "===>tx queue is not empty:%d, %d\n",
+			       i, skb_queue_len(&(&priv->tx_ring[i])->queue));
+			return 0;
+		}
+	}
+	return 1;
+}
+
+static enum reset_type _rtl92e_tx_check_stuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8	QueueID;
+	bool	bCheckFwTxCnt = false;
+	struct rtl8192_tx_ring  *ring = NULL;
+	struct sk_buff *skb = NULL;
+	struct cb_desc *tcb_desc = NULL;
+	unsigned long flags = 0;
+
+	switch (priv->rtllib->ps) {
+	case RTLLIB_PS_DISABLED:
+		break;
+	case (RTLLIB_PS_MBCAST | RTLLIB_PS_UNICAST):
+		break;
+	default:
+		break;
+	}
+	spin_lock_irqsave(&priv->irq_th_lock, flags);
+	for (QueueID = 0; QueueID < MAX_TX_QUEUE; QueueID++) {
+		if (QueueID == TXCMD_QUEUE)
+			continue;
+
+		if (QueueID == BEACON_QUEUE)
+			continue;
+
+		ring = &priv->tx_ring[QueueID];
+
+		if (skb_queue_len(&ring->queue) == 0) {
+			continue;
+		} else {
+			skb = __skb_peek(&ring->queue);
+			tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+			tcb_desc->nStuckCount++;
+			bCheckFwTxCnt = true;
+			if (tcb_desc->nStuckCount > 1)
+				netdev_info(dev,
+					    "%s: QueueID=%d tcb_desc->nStuckCount=%d\n",
+					    __func__, QueueID,
+					    tcb_desc->nStuckCount);
+		}
+	}
+	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+
+	if (bCheckFwTxCnt) {
+		if (priv->ops->tx_check_stuck_handler(dev))
+			return RESET_TYPE_SILENT;
+	}
+
+	return RESET_TYPE_NORESET;
+}
+
+static enum reset_type _rtl92e_rx_check_stuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->ops->rx_check_stuck_handler(dev))
+		return RESET_TYPE_SILENT;
+
+	return RESET_TYPE_NORESET;
+}
+
+static enum reset_type _rtl92e_if_check_reset(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	enum reset_type TxResetType = RESET_TYPE_NORESET;
+	enum reset_type RxResetType = RESET_TYPE_NORESET;
+	enum rt_rf_power_state rfState;
+
+	rfState = priv->rtllib->rf_power_state;
+
+	if (rfState == rf_on)
+		TxResetType = _rtl92e_tx_check_stuck(dev);
+
+	if (rfState == rf_on &&
+	    (priv->rtllib->iw_mode == IW_MODE_INFRA) &&
+	    (priv->rtllib->state == RTLLIB_LINKED))
+		RxResetType = _rtl92e_rx_check_stuck(dev);
+
+	if (TxResetType == RESET_TYPE_NORMAL ||
+	    RxResetType == RESET_TYPE_NORMAL) {
+		netdev_info(dev, "%s(): TxResetType is %d, RxResetType is %d\n",
+			    __func__, TxResetType, RxResetType);
+		return RESET_TYPE_NORMAL;
+	} else if (TxResetType == RESET_TYPE_SILENT ||
+		   RxResetType == RESET_TYPE_SILENT) {
+		netdev_info(dev, "%s(): TxResetType is %d, RxResetType is %d\n",
+			    __func__, TxResetType, RxResetType);
+		return RESET_TYPE_SILENT;
+	} else {
+		return RESET_TYPE_NORESET;
+	}
+}
+
+static void _rtl92e_if_silent_reset(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8	reset_times = 0;
+	int reset_status = 0;
+	struct rtllib_device *ieee = priv->rtllib;
+	unsigned long flag;
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		priv->ResetProgress = RESET_TYPE_SILENT;
+
+		spin_lock_irqsave(&priv->rf_ps_lock, flag);
+		if (priv->rf_change_in_progress) {
+			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+			goto END;
+		}
+		priv->rf_change_in_progress = true;
+		priv->bResetInProgress = true;
+		spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+
+RESET_START:
+
+		mutex_lock(&priv->wx_mutex);
+
+		if (priv->rtllib->state == RTLLIB_LINKED)
+			rtl92e_leisure_ps_leave(dev);
+
+		if (priv->up) {
+			netdev_info(dev, "%s():the driver is not up.\n",
+				    __func__);
+			mutex_unlock(&priv->wx_mutex);
+			return;
+		}
+		priv->up = 0;
+
+		mdelay(1000);
+
+		if (!netif_queue_stopped(dev))
+			netif_stop_queue(dev);
+
+		rtl92e_irq_disable(dev);
+		del_timer_sync(&priv->watch_dog_timer);
+		_rtl92e_cancel_deferred_work(priv);
+		rtl92e_dm_deinit(dev);
+		rtllib_stop_scan_syncro(ieee);
+
+		if (ieee->state == RTLLIB_LINKED) {
+			mutex_lock(&ieee->wx_mutex);
+			netdev_info(dev, "ieee->state is RTLLIB_LINKED\n");
+			rtllib_stop_send_beacons(priv->rtllib);
+			del_timer_sync(&ieee->associate_timer);
+			cancel_delayed_work(&ieee->associate_retry_wq);
+			rtllib_stop_scan(ieee);
+			netif_carrier_off(dev);
+			mutex_unlock(&ieee->wx_mutex);
+		} else {
+			netdev_info(dev, "ieee->state is NOT LINKED\n");
+			rtllib_softmac_stop_protocol(priv->rtllib, 0, true);
+		}
+
+		rtl92e_dm_backup_state(dev);
+
+		mutex_unlock(&priv->wx_mutex);
+		reset_status = _rtl92e_up(dev, true);
+
+		if (reset_status == -1) {
+			if (reset_times < 3) {
+				reset_times++;
+				goto RESET_START;
+			} else {
+				netdev_warn(dev, "%s():	Reset Failed\n",
+					    __func__);
+			}
+		}
+
+		ieee->is_silent_reset = 1;
+
+		spin_lock_irqsave(&priv->rf_ps_lock, flag);
+		priv->rf_change_in_progress = false;
+		spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
+
+		rtl92e_enable_hw_security_config(dev);
+
+		if (ieee->state == RTLLIB_LINKED && ieee->iw_mode ==
+		    IW_MODE_INFRA) {
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel);
+
+			schedule_work(&ieee->associate_complete_wq);
+
+		} else if (ieee->state == RTLLIB_LINKED && ieee->iw_mode ==
+			   IW_MODE_ADHOC) {
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel);
+			ieee->link_change(ieee->dev);
+
+			notify_wx_assoc_event(ieee);
+
+			rtllib_start_send_beacons(ieee);
+
+			netif_carrier_on(ieee->dev);
+		}
+
+		rtl92e_cam_restore(dev);
+		rtl92e_dm_restore_state(dev);
+END:
+		priv->ResetProgress = RESET_TYPE_NORESET;
+		priv->reset_count++;
+
+		priv->bForcedSilentReset = false;
+		priv->bResetInProgress = false;
+
+		rtl92e_writeb(dev, UFWP, 1);
+	}
+}
+
+static void _rtl92e_update_rxcounts(struct r8192_priv *priv, u32 *TotalRxBcnNum,
+				    u32 *TotalRxDataNum)
+{
+	u16	SlotIndex;
+	u8	i;
+
+	*TotalRxBcnNum = 0;
+	*TotalRxDataNum = 0;
+
+	SlotIndex = (priv->rtllib->LinkDetectInfo.SlotIndex++) %
+			(priv->rtllib->LinkDetectInfo.SlotNum);
+	priv->rtllib->LinkDetectInfo.RxBcnNum[SlotIndex] =
+			priv->rtllib->LinkDetectInfo.NumRecvBcnInPeriod;
+	priv->rtllib->LinkDetectInfo.RxDataNum[SlotIndex] =
+			priv->rtllib->LinkDetectInfo.NumRecvDataInPeriod;
+	for (i = 0; i < priv->rtllib->LinkDetectInfo.SlotNum; i++) {
+		*TotalRxBcnNum += priv->rtllib->LinkDetectInfo.RxBcnNum[i];
+		*TotalRxDataNum += priv->rtllib->LinkDetectInfo.RxDataNum[i];
+	}
+}
+
+static void _rtl92e_watchdog_wq_cb(void *data)
+{
+	struct r8192_priv *priv = container_of_dwork_rsl(data,
+				  struct r8192_priv, watch_dog_wq);
+	struct net_device *dev = priv->rtllib->dev;
+	struct rtllib_device *ieee = priv->rtllib;
+	enum reset_type ResetType = RESET_TYPE_NORESET;
+	static u8 check_reset_cnt;
+	unsigned long flags;
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&priv->rtllib->PowerSaveControl);
+	bool bBusyTraffic = false;
+	bool	bHigherBusyTraffic = false;
+	bool	bHigherBusyRxTraffic = false;
+	bool bEnterPS = false;
+
+	if (!priv->up || priv->hw_radio_off)
+		return;
+
+	if (priv->rtllib->state >= RTLLIB_LINKED) {
+		if (priv->rtllib->CntAfterLink < 2)
+			priv->rtllib->CntAfterLink++;
+	} else {
+		priv->rtllib->CntAfterLink = 0;
+	}
+
+	rtl92e_dm_watchdog(dev);
+
+	if (!rtllib_act_scanning(priv->rtllib, false)) {
+		if ((ieee->iw_mode == IW_MODE_INFRA) && (ieee->state ==
+		     RTLLIB_NOLINK) &&
+		     (ieee->rf_power_state == rf_on) && !ieee->is_set_key &&
+		     (!ieee->proto_stoppping) && !ieee->wx_set_enc) {
+			if ((ieee->PowerSaveControl.ReturnPoint ==
+			     IPS_CALLBACK_NONE) &&
+			     (!ieee->bNetPromiscuousMode)) {
+				rtl92e_ips_enter(dev);
+			}
+		}
+	}
+	if ((ieee->state == RTLLIB_LINKED) && (ieee->iw_mode ==
+	     IW_MODE_INFRA) && (!ieee->bNetPromiscuousMode)) {
+		if (ieee->LinkDetectInfo.NumRxOkInPeriod > 100 ||
+		ieee->LinkDetectInfo.NumTxOkInPeriod > 100)
+			bBusyTraffic = true;
+
+		if (ieee->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+		    ieee->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+			bHigherBusyTraffic = true;
+			if (ieee->LinkDetectInfo.NumRxOkInPeriod > 5000)
+				bHigherBusyRxTraffic = true;
+			else
+				bHigherBusyRxTraffic = false;
+		}
+
+		if (((ieee->LinkDetectInfo.NumRxUnicastOkInPeriod +
+		    ieee->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+		    (ieee->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
+			bEnterPS = false;
+		else
+			bEnterPS = true;
+
+		if (ieee->current_network.beacon_interval < 95)
+			bEnterPS = false;
+
+		if (bEnterPS)
+			rtl92e_leisure_ps_enter(dev);
+		else
+			rtl92e_leisure_ps_leave(dev);
+
+	} else {
+		rtl92e_leisure_ps_leave(dev);
+	}
+
+	ieee->LinkDetectInfo.NumRxOkInPeriod = 0;
+	ieee->LinkDetectInfo.NumTxOkInPeriod = 0;
+	ieee->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	ieee->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+
+	ieee->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+	ieee->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+
+	if (ieee->state == RTLLIB_LINKED && ieee->iw_mode == IW_MODE_INFRA) {
+		u32	TotalRxBcnNum = 0;
+		u32	TotalRxDataNum = 0;
+
+		_rtl92e_update_rxcounts(priv, &TotalRxBcnNum, &TotalRxDataNum);
+
+		if ((TotalRxBcnNum + TotalRxDataNum) == 0)
+			priv->check_roaming_cnt++;
+		else
+			priv->check_roaming_cnt = 0;
+
+		if (priv->check_roaming_cnt > 0) {
+			if (ieee->rf_power_state == rf_off)
+				netdev_info(dev, "%s(): RF is off\n", __func__);
+
+			netdev_info(dev,
+				    "===>%s(): AP is power off, chan:%d, connect another one\n",
+				    __func__, priv->chan);
+
+			ieee->state = RTLLIB_ASSOCIATING;
+
+			RemovePeerTS(priv->rtllib,
+				     priv->rtllib->current_network.bssid);
+			ieee->is_roaming = true;
+			ieee->is_set_key = false;
+			ieee->link_change(dev);
+			if (ieee->LedControlHandler)
+				ieee->LedControlHandler(ieee->dev,
+							LED_CTL_START_TO_LINK);
+
+			notify_wx_assoc_event(ieee);
+
+			if (!(ieee->rtllib_ap_sec_type(ieee) &
+			     (SEC_ALG_CCMP | SEC_ALG_TKIP)))
+				schedule_delayed_work(
+					&ieee->associate_procedure_wq, 0);
+
+			priv->check_roaming_cnt = 0;
+		}
+		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 0;
+		ieee->LinkDetectInfo.NumRecvDataInPeriod = 0;
+	}
+
+	spin_lock_irqsave(&priv->tx_lock, flags);
+	if ((check_reset_cnt++ >= 3) && (!ieee->is_roaming) &&
+	    (!priv->rf_change_in_progress) && (!pPSC->bSwRfProcessing)) {
+		ResetType = _rtl92e_if_check_reset(dev);
+		check_reset_cnt = 3;
+	}
+	spin_unlock_irqrestore(&priv->tx_lock, flags);
+
+	if (!priv->bDisableNormalResetCheck && ResetType == RESET_TYPE_NORMAL) {
+		priv->ResetProgress = RESET_TYPE_NORMAL;
+		return;
+	}
+
+	if (((priv->force_reset) || (!priv->bDisableNormalResetCheck &&
+	      ResetType == RESET_TYPE_SILENT)))
+		_rtl92e_if_silent_reset(dev);
+	priv->force_reset = false;
+	priv->bForcedSilentReset = false;
+	priv->bResetInProgress = false;
+}
+
+static void _rtl92e_watchdog_timer_cb(struct timer_list *t)
+{
+	struct r8192_priv *priv = from_timer(priv, t, watch_dog_timer);
+
+	schedule_delayed_work(&priv->watch_dog_wq, 0);
+	mod_timer(&priv->watch_dog_timer, jiffies +
+		  msecs_to_jiffies(RTLLIB_WATCH_DOG_TIME));
+}
+
+/****************************************************************************
+ * ---------------------------- NIC TX/RX STUFF---------------------------
+ ****************************************************************************/
+void rtl92e_rx_enable(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->ops->rx_enable(dev);
+}
+
+void rtl92e_tx_enable(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->ops->tx_enable(dev);
+
+	rtllib_reset_queue(priv->rtllib);
+}
+
+static void _rtl92e_free_rx_ring(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int i, rx_queue_idx;
+
+	for (rx_queue_idx = 0; rx_queue_idx < MAX_RX_QUEUE;
+	     rx_queue_idx++) {
+		for (i = 0; i < priv->rxringcount; i++) {
+			struct sk_buff *skb = priv->rx_buf[rx_queue_idx][i];
+
+			if (!skb)
+				continue;
+
+			dma_unmap_single(&priv->pdev->dev,
+					 *((dma_addr_t *)skb->cb),
+					 priv->rxbuffersize, DMA_FROM_DEVICE);
+			kfree_skb(skb);
+		}
+
+		dma_free_coherent(&priv->pdev->dev,
+				  sizeof(*priv->rx_ring[rx_queue_idx]) * priv->rxringcount,
+				  priv->rx_ring[rx_queue_idx],
+				  priv->rx_ring_dma[rx_queue_idx]);
+		priv->rx_ring[rx_queue_idx] = NULL;
+	}
+}
+
+static void _rtl92e_free_tx_ring(struct net_device *dev, unsigned int prio)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtl8192_tx_ring *ring = &priv->tx_ring[prio];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		dma_unmap_single(&priv->pdev->dev, entry->TxBuffAddr,
+				 skb->len, DMA_TO_DEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+
+	dma_free_coherent(&priv->pdev->dev,
+			  sizeof(*ring->desc) * ring->entries, ring->desc,
+			  ring->dma);
+	ring->desc = NULL;
+}
+
+static void _rtl92e_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
+				   int rate)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u8 queue_index = tcb_desc->queue_index;
+
+	if ((priv->rtllib->rf_power_state == rf_off) || !priv->up ||
+	     priv->bResetInProgress) {
+		kfree_skb(skb);
+		return;
+	}
+
+	if (queue_index == TXCMD_QUEUE)
+		netdev_warn(dev, "%s(): queue index == TXCMD_QUEUE\n",
+			    __func__);
+
+	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+	skb_push(skb, priv->rtllib->tx_headroom);
+	ret = _rtl92e_tx(dev, skb);
+
+	if (queue_index != MGNT_QUEUE) {
+		priv->rtllib->stats.tx_bytes += (skb->len -
+						 priv->rtllib->tx_headroom);
+		priv->rtllib->stats.tx_packets++;
+	}
+
+	if (ret != 0)
+		kfree_skb(skb);
+}
+
+static int _rtl92e_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u8 queue_index = tcb_desc->queue_index;
+
+	if (queue_index != TXCMD_QUEUE) {
+		if ((priv->rtllib->rf_power_state == rf_off) ||
+		     !priv->up || priv->bResetInProgress) {
+			kfree_skb(skb);
+			return 0;
+		}
+	}
+
+	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+	if (queue_index == TXCMD_QUEUE) {
+		_rtl92e_tx_cmd(dev, skb);
+		return 0;
+	}
+
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+	tcb_desc->bTxEnableFwCalcDur = 1;
+	skb_push(skb, priv->rtllib->tx_headroom);
+	ret = _rtl92e_tx(dev, skb);
+	if (ret != 0)
+		kfree_skb(skb);
+	return ret;
+}
+
+static void _rtl92e_tx_isr(struct net_device *dev, int prio)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	struct rtl8192_tx_ring *ring = &priv->tx_ring[prio];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb;
+
+		if (prio != BEACON_QUEUE) {
+			if (entry->OWN)
+				return;
+			ring->idx = (ring->idx + 1) % ring->entries;
+		}
+
+		skb = __skb_dequeue(&ring->queue);
+		dma_unmap_single(&priv->pdev->dev, entry->TxBuffAddr,
+				 skb->len, DMA_TO_DEVICE);
+
+		kfree_skb(skb);
+	}
+	if (prio != BEACON_QUEUE)
+		tasklet_schedule(&priv->irq_tx_tasklet);
+}
+
+static void _rtl92e_tx_cmd(struct net_device *dev, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtl8192_tx_ring *ring;
+	struct tx_desc_cmd *entry;
+	unsigned int idx;
+	struct cb_desc *tcb_desc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_th_lock, flags);
+	ring = &priv->tx_ring[TXCMD_QUEUE];
+
+	idx = (ring->idx + skb_queue_len(&ring->queue)) % ring->entries;
+	entry = (struct tx_desc_cmd *)&ring->desc[idx];
+
+	tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+
+	priv->ops->tx_fill_cmd_descriptor(dev, entry, tcb_desc, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+}
+
+static short _rtl92e_tx(struct net_device *dev, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtl8192_tx_ring  *ring;
+	unsigned long flags;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	struct tx_desc *pdesc = NULL;
+	struct rtllib_hdr_1addr *header = NULL;
+	u16 fc = 0, type = 0;
+	u8 *pda_addr = NULL;
+	int   idx;
+	u32 fwinfo_size = 0;
+
+	if (priv->bdisable_nic) {
+		netdev_warn(dev, "%s: Nic is disabled! Can't tx packet.\n",
+			    __func__);
+		return skb->len;
+	}
+
+	priv->rtllib->bAwakePktSent = true;
+
+	fwinfo_size = sizeof(struct tx_fwinfo_8190pci);
+
+	header = (struct rtllib_hdr_1addr *)(((u8 *)skb->data) + fwinfo_size);
+	fc = le16_to_cpu(header->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	pda_addr = header->addr1;
+
+	if (is_broadcast_ether_addr(pda_addr))
+		priv->stats.txbytesbroadcast += skb->len - fwinfo_size;
+	else if (is_multicast_ether_addr(pda_addr))
+		priv->stats.txbytesmulticast += skb->len - fwinfo_size;
+	else
+		priv->stats.txbytesunicast += skb->len - fwinfo_size;
+
+	spin_lock_irqsave(&priv->irq_th_lock, flags);
+	ring = &priv->tx_ring[tcb_desc->queue_index];
+	if (tcb_desc->queue_index != BEACON_QUEUE)
+		idx = (ring->idx + skb_queue_len(&ring->queue)) % ring->entries;
+	else
+		idx = 0;
+
+	pdesc = &ring->desc[idx];
+	if ((pdesc->OWN == 1) && (tcb_desc->queue_index != BEACON_QUEUE)) {
+		netdev_warn(dev,
+			    "No more TX desc@%d, ring->idx = %d, idx = %d, skblen = 0x%x queuelen=%d",
+			    tcb_desc->queue_index, ring->idx, idx, skb->len,
+			    skb_queue_len(&ring->queue));
+		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+		return skb->len;
+	}
+
+	if (type == RTLLIB_FTYPE_DATA) {
+		if (priv->rtllib->LedControlHandler)
+			priv->rtllib->LedControlHandler(dev, LED_CTL_TX);
+	}
+	priv->ops->tx_fill_descriptor(dev, pdesc, tcb_desc, skb);
+	__skb_queue_tail(&ring->queue, skb);
+	pdesc->OWN = 1;
+	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+	netif_trans_update(dev);
+
+	rtl92e_writew(dev, TPPoll, 0x01 << tcb_desc->queue_index);
+	return 0;
+}
+
+static short _rtl92e_alloc_rx_ring(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rx_desc *entry = NULL;
+	int i, rx_queue_idx;
+
+	for (rx_queue_idx = 0; rx_queue_idx < MAX_RX_QUEUE; rx_queue_idx++) {
+		priv->rx_ring[rx_queue_idx] = dma_alloc_coherent(&priv->pdev->dev,
+								 sizeof(*priv->rx_ring[rx_queue_idx]) * priv->rxringcount,
+								 &priv->rx_ring_dma[rx_queue_idx],
+								 GFP_ATOMIC);
+		if (!priv->rx_ring[rx_queue_idx] ||
+		    (unsigned long)priv->rx_ring[rx_queue_idx] & 0xFF) {
+			netdev_warn(dev, "Cannot allocate RX ring\n");
+			return -ENOMEM;
+		}
+
+		priv->rx_idx[rx_queue_idx] = 0;
+
+		for (i = 0; i < priv->rxringcount; i++) {
+			struct sk_buff *skb = dev_alloc_skb(priv->rxbuffersize);
+			dma_addr_t *mapping;
+
+			entry = &priv->rx_ring[rx_queue_idx][i];
+			if (!skb)
+				return 0;
+			skb->dev = dev;
+			priv->rx_buf[rx_queue_idx][i] = skb;
+			mapping = (dma_addr_t *)skb->cb;
+			*mapping = dma_map_single(&priv->pdev->dev,
+						  skb_tail_pointer_rsl(skb),
+						  priv->rxbuffersize, DMA_FROM_DEVICE);
+			if (dma_mapping_error(&priv->pdev->dev, *mapping)) {
+				dev_kfree_skb_any(skb);
+				return -1;
+			}
+			entry->BufferAddress = *mapping;
+
+			entry->Length = priv->rxbuffersize;
+			entry->OWN = 1;
+		}
+
+		if (entry)
+			entry->EOR = 1;
+	}
+	return 0;
+}
+
+static int _rtl92e_alloc_tx_ring(struct net_device *dev, unsigned int prio,
+				 unsigned int entries)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct tx_desc *ring;
+	dma_addr_t dma;
+	int i;
+
+	ring = dma_alloc_coherent(&priv->pdev->dev, sizeof(*ring) * entries,
+				  &dma, GFP_ATOMIC);
+	if (!ring || (unsigned long)ring & 0xFF) {
+		netdev_warn(dev, "Cannot allocate TX ring (prio = %d)\n", prio);
+		return -ENOMEM;
+	}
+
+	priv->tx_ring[prio].desc = ring;
+	priv->tx_ring[prio].dma = dma;
+	priv->tx_ring[prio].idx = 0;
+	priv->tx_ring[prio].entries = entries;
+	skb_queue_head_init(&priv->tx_ring[prio].queue);
+
+	for (i = 0; i < entries; i++)
+		ring[i].NextDescAddress =
+			(u32)dma + ((i + 1) % entries) *
+			sizeof(*ring);
+
+	return 0;
+}
+
+static short _rtl92e_pci_initdescring(struct net_device *dev)
+{
+	u32 ret;
+	int i;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	ret = _rtl92e_alloc_rx_ring(dev);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < MAX_TX_QUEUE_COUNT; i++) {
+		ret = _rtl92e_alloc_tx_ring(dev, i, priv->txringcount);
+		if (ret)
+			goto err_free_rings;
+	}
+
+	return 0;
+
+err_free_rings:
+	_rtl92e_free_rx_ring(dev);
+	for (i = 0; i < MAX_TX_QUEUE_COUNT; i++)
+		if (priv->tx_ring[i].desc)
+			_rtl92e_free_tx_ring(dev, i);
+	return 1;
+}
+
+void rtl92e_reset_desc_ring(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int i, rx_queue_idx;
+	unsigned long flags = 0;
+
+	for (rx_queue_idx = 0; rx_queue_idx < MAX_RX_QUEUE; rx_queue_idx++) {
+		if (priv->rx_ring[rx_queue_idx]) {
+			struct rx_desc *entry = NULL;
+
+			for (i = 0; i < priv->rxringcount; i++) {
+				entry = &priv->rx_ring[rx_queue_idx][i];
+				entry->OWN = 1;
+			}
+			priv->rx_idx[rx_queue_idx] = 0;
+		}
+	}
+
+	spin_lock_irqsave(&priv->irq_th_lock, flags);
+	for (i = 0; i < MAX_TX_QUEUE_COUNT; i++) {
+		if (priv->tx_ring[i].desc) {
+			struct rtl8192_tx_ring *ring = &priv->tx_ring[i];
+
+			while (skb_queue_len(&ring->queue)) {
+				struct tx_desc *entry = &ring->desc[ring->idx];
+				struct sk_buff *skb =
+						 __skb_dequeue(&ring->queue);
+
+				dma_unmap_single(&priv->pdev->dev,
+						 entry->TxBuffAddr, skb->len,
+						 DMA_TO_DEVICE);
+				kfree_skb(skb);
+				ring->idx = (ring->idx + 1) % ring->entries;
+			}
+			ring->idx = 0;
+		}
+	}
+	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+}
+
+void rtl92e_update_rx_pkt_timestamp(struct net_device *dev,
+				    struct rtllib_rx_stats *stats)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (stats->bIsAMPDU && !stats->bFirstMPDU)
+		stats->mac_time = priv->LastRxDescTSF;
+	else
+		priv->LastRxDescTSF = stats->mac_time;
+}
+
+long rtl92e_translate_to_dbm(struct r8192_priv *priv, u8 signal_strength_index)
+{
+	long	signal_power;
+
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+
+	return signal_power;
+}
+
+void rtl92e_update_rx_statistics(struct r8192_priv *priv,
+				 struct rtllib_rx_stats *pprevious_stats)
+{
+	int weighting = 0;
+
+	if (priv->stats.recv_signal_power == 0)
+		priv->stats.recv_signal_power =
+					 pprevious_stats->RecvSignalPower;
+
+	if (pprevious_stats->RecvSignalPower > priv->stats.recv_signal_power)
+		weighting = 5;
+	else if (pprevious_stats->RecvSignalPower <
+		 priv->stats.recv_signal_power)
+		weighting = (-5);
+	priv->stats.recv_signal_power = (priv->stats.recv_signal_power * 5 +
+					pprevious_stats->RecvSignalPower +
+					weighting) / 6;
+}
+
+u8 rtl92e_rx_db_to_percent(s8 antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return	0;
+	else if (antpower >= 0)
+		return	100;
+	else
+		return	100 + antpower;
+
+}	/* QueryRxPwrPercentage */
+
+u8 rtl92e_evm_db_to_percent(s8 value)
+{
+	s8 ret_val = clamp(-value, 0, 33) * 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+void rtl92e_copy_mpdu_stats(struct rtllib_rx_stats *psrc_stats,
+			    struct rtllib_rx_stats *ptarget_stats)
+{
+	ptarget_stats->bIsAMPDU = psrc_stats->bIsAMPDU;
+	ptarget_stats->bFirstMPDU = psrc_stats->bFirstMPDU;
+}
+
+static void _rtl92e_rx_normal(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_hdr_1addr *rtllib_hdr = NULL;
+	bool unicast_packet = false;
+	bool bLedBlinking = true;
+	u16 fc = 0, type = 0;
+	u32 skb_len = 0;
+	int rx_queue_idx = RX_MPDU_QUEUE;
+
+	struct rtllib_rx_stats stats = {
+		.signal = 0,
+		.noise = (u8)-98,
+		.rate = 0,
+		.freq = RTLLIB_24GHZ_BAND,
+	};
+	unsigned int count = priv->rxringcount;
+
+	stats.nic_type = NIC_8192E;
+
+	while (count--) {
+		struct rx_desc *pdesc = &priv->rx_ring[rx_queue_idx]
+					[priv->rx_idx[rx_queue_idx]];
+		struct sk_buff *skb = priv->rx_buf[rx_queue_idx]
+				      [priv->rx_idx[rx_queue_idx]];
+		struct sk_buff *new_skb;
+
+		if (pdesc->OWN)
+			return;
+		if (!priv->ops->rx_query_status_descriptor(dev, &stats,
+		pdesc, skb))
+			goto done;
+		new_skb = dev_alloc_skb(priv->rxbuffersize);
+		/* if allocation of new skb failed - drop current packet
+		 * and reuse skb
+		 */
+		if (unlikely(!new_skb))
+			goto done;
+
+		dma_unmap_single(&priv->pdev->dev, *((dma_addr_t *)skb->cb),
+				 priv->rxbuffersize, DMA_FROM_DEVICE);
+
+		skb_put(skb, pdesc->Length);
+		skb_reserve(skb, stats.RxDrvInfoSize +
+			stats.RxBufShift);
+		skb_trim(skb, skb->len - 4/*sCrcLng*/);
+		rtllib_hdr = (struct rtllib_hdr_1addr *)skb->data;
+		if (!is_multicast_ether_addr(rtllib_hdr->addr1)) {
+			/* unicast packet */
+			unicast_packet = true;
+		}
+		fc = le16_to_cpu(rtllib_hdr->frame_ctl);
+		type = WLAN_FC_GET_TYPE(fc);
+		if (type == RTLLIB_FTYPE_MGMT)
+			bLedBlinking = false;
+
+		if (bLedBlinking)
+			if (priv->rtllib->LedControlHandler)
+				priv->rtllib->LedControlHandler(dev,
+							LED_CTL_RX);
+
+		if (stats.bCRC) {
+			if (type != RTLLIB_FTYPE_MGMT)
+				priv->stats.rxdatacrcerr++;
+			else
+				priv->stats.rxmgmtcrcerr++;
+		}
+
+		skb_len = skb->len;
+
+		if (!rtllib_rx(priv->rtllib, skb, &stats)) {
+			dev_kfree_skb_any(skb);
+		} else {
+			priv->stats.rxok++;
+			if (unicast_packet)
+				priv->stats.rxbytesunicast += skb_len;
+		}
+
+		skb = new_skb;
+		skb->dev = dev;
+
+		priv->rx_buf[rx_queue_idx][priv->rx_idx[rx_queue_idx]] =
+								 skb;
+		*((dma_addr_t *)skb->cb) = dma_map_single(&priv->pdev->dev,
+							  skb_tail_pointer_rsl(skb),
+							  priv->rxbuffersize, DMA_FROM_DEVICE);
+		if (dma_mapping_error(&priv->pdev->dev, *((dma_addr_t *)skb->cb))) {
+			dev_kfree_skb_any(skb);
+			return;
+		}
+done:
+		pdesc->BufferAddress = *((dma_addr_t *)skb->cb);
+		pdesc->OWN = 1;
+		pdesc->Length = priv->rxbuffersize;
+		if (priv->rx_idx[rx_queue_idx] == priv->rxringcount - 1)
+			pdesc->EOR = 1;
+		priv->rx_idx[rx_queue_idx] = (priv->rx_idx[rx_queue_idx] + 1) %
+					      priv->rxringcount;
+	}
+}
+
+static void _rtl92e_tx_resume(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	struct sk_buff *skb;
+	int queue_index;
+
+	for (queue_index = BK_QUEUE;
+	     queue_index < MAX_QUEUE_SIZE; queue_index++) {
+		while ((!skb_queue_empty(&ieee->skb_waitQ[queue_index])) &&
+		(priv->rtllib->check_nic_enough_desc(dev, queue_index) > 0)) {
+			skb = skb_dequeue(&ieee->skb_waitQ[queue_index]);
+			ieee->softmac_data_hard_start_xmit(skb, dev, 0);
+		}
+	}
+}
+
+static void _rtl92e_irq_tx_tasklet(struct tasklet_struct *t)
+{
+	struct r8192_priv *priv = from_tasklet(priv, t, irq_tx_tasklet);
+
+	_rtl92e_tx_resume(priv->rtllib->dev);
+}
+
+static void _rtl92e_irq_rx_tasklet(struct tasklet_struct *t)
+{
+	struct r8192_priv *priv = from_tasklet(priv, t, irq_rx_tasklet);
+
+	_rtl92e_rx_normal(priv->rtllib->dev);
+
+	rtl92e_writel(priv->rtllib->dev, INTA_MASK,
+		      rtl92e_readl(priv->rtllib->dev, INTA_MASK) | IMR_RDU);
+}
+
+/****************************************************************************
+ * ---------------------------- NIC START/CLOSE STUFF---------------------------
+ ****************************************************************************/
+static void _rtl92e_cancel_deferred_work(struct r8192_priv *priv)
+{
+	cancel_delayed_work_sync(&priv->watch_dog_wq);
+	cancel_delayed_work_sync(&priv->update_beacon_wq);
+	cancel_delayed_work(&priv->rtllib->hw_sleep_wq);
+	cancel_work_sync(&priv->reset_wq);
+	cancel_work_sync(&priv->qos_activate);
+}
+
+static int _rtl92e_up(struct net_device *dev, bool is_silent_reset)
+{
+	if (_rtl92e_sta_up(dev, is_silent_reset) == -1)
+		return -1;
+	return 0;
+}
+
+static int _rtl92e_open(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = _rtl92e_try_up(dev);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_try_up(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->up == 1)
+		return -1;
+	return _rtl92e_up(dev, false);
+}
+
+static int _rtl92e_close(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+
+	if ((rtllib_act_scanning(priv->rtllib, false)) &&
+		!(priv->rtllib->softmac_features & IEEE_SOFTMAC_SCAN)) {
+		rtllib_stop_scan(priv->rtllib);
+	}
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = _rtl92e_down(dev, true);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_down(struct net_device *dev, bool shutdownrf)
+{
+	if (_rtl92e_sta_down(dev, shutdownrf) == -1)
+		return -1;
+
+	return 0;
+}
+
+void rtl92e_commit(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->up == 0)
+		return;
+	rtllib_softmac_stop_protocol(priv->rtllib, 0, true);
+	rtl92e_irq_disable(dev);
+	priv->ops->stop_adapter(dev, true);
+	_rtl92e_up(dev, false);
+}
+
+static void _rtl92e_restart(void *data)
+{
+	struct r8192_priv *priv = container_of_work_rsl(data, struct r8192_priv,
+				  reset_wq);
+	struct net_device *dev = priv->rtllib->dev;
+
+	mutex_lock(&priv->wx_mutex);
+
+	rtl92e_commit(dev);
+
+	mutex_unlock(&priv->wx_mutex);
+}
+
+static void _rtl92e_set_multicast(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	short promisc;
+
+	promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
+	priv->promisc = promisc;
+}
+
+static int _rtl92e_set_mac_adr(struct net_device *dev, void *mac)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct sockaddr *addr = mac;
+
+	mutex_lock(&priv->wx_mutex);
+
+	eth_hw_addr_set(dev, addr->sa_data);
+
+	schedule_work(&priv->reset_wq);
+	mutex_unlock(&priv->wx_mutex);
+
+	return 0;
+}
+
+static irqreturn_t _rtl92e_irq(int irq, void *netdev)
+{
+	struct net_device *dev = netdev;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	unsigned long flags;
+	u32 inta;
+	u32 intb;
+
+	intb = 0;
+
+	if (priv->irq_enabled == 0)
+		goto done;
+
+	spin_lock_irqsave(&priv->irq_th_lock, flags);
+
+	priv->ops->interrupt_recognized(dev, &inta, &intb);
+	priv->stats.shints++;
+
+	if (!inta) {
+		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+		goto done;
+	}
+
+	if (inta == 0xffff) {
+		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+		goto done;
+	}
+
+	priv->stats.ints++;
+
+	if (!netif_running(dev)) {
+		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+		goto done;
+	}
+
+	if (inta & IMR_TBDOK)
+		priv->stats.txbeaconokint++;
+
+	if (inta & IMR_TBDER)
+		priv->stats.txbeaconerr++;
+
+	if (inta  & IMR_MGNTDOK) {
+		priv->stats.txmanageokint++;
+		_rtl92e_tx_isr(dev, MGNT_QUEUE);
+		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+		if (priv->rtllib->ack_tx_to_ieee) {
+			if (_rtl92e_is_tx_queue_empty(dev)) {
+				priv->rtllib->ack_tx_to_ieee = 0;
+				rtllib_ps_tx_ack(priv->rtllib, 1);
+			}
+		}
+		spin_lock_irqsave(&priv->irq_th_lock, flags);
+	}
+
+	if (inta & IMR_COMDOK) {
+		priv->stats.txcmdpktokint++;
+		_rtl92e_tx_isr(dev, TXCMD_QUEUE);
+	}
+
+	if (inta & IMR_HIGHDOK)
+		_rtl92e_tx_isr(dev, HIGH_QUEUE);
+
+	if (inta & IMR_ROK) {
+		priv->stats.rxint++;
+		priv->InterruptLog.nIMR_ROK++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+
+	if (inta & IMR_BcnInt)
+		tasklet_schedule(&priv->irq_prepare_beacon_tasklet);
+
+	if (inta & IMR_RDU) {
+		priv->stats.rxrdu++;
+		rtl92e_writel(dev, INTA_MASK,
+			      rtl92e_readl(dev, INTA_MASK) & ~IMR_RDU);
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+
+	if (inta & IMR_RXFOVW) {
+		priv->stats.rxoverflow++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+
+	if (inta & IMR_TXFOVW)
+		priv->stats.txoverflow++;
+
+	if (inta & IMR_BKDOK) {
+		priv->stats.txbkokint++;
+		priv->rtllib->LinkDetectInfo.NumTxOkInPeriod++;
+		_rtl92e_tx_isr(dev, BK_QUEUE);
+	}
+
+	if (inta & IMR_BEDOK) {
+		priv->stats.txbeokint++;
+		priv->rtllib->LinkDetectInfo.NumTxOkInPeriod++;
+		_rtl92e_tx_isr(dev, BE_QUEUE);
+	}
+
+	if (inta & IMR_VIDOK) {
+		priv->stats.txviokint++;
+		priv->rtllib->LinkDetectInfo.NumTxOkInPeriod++;
+		_rtl92e_tx_isr(dev, VI_QUEUE);
+	}
+
+	if (inta & IMR_VODOK) {
+		priv->stats.txvookint++;
+		priv->rtllib->LinkDetectInfo.NumTxOkInPeriod++;
+		_rtl92e_tx_isr(dev, VO_QUEUE);
+	}
+
+	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
+
+done:
+
+	return IRQ_HANDLED;
+}
+
+/****************************************************************************
+ * ---------------------------- PCI_STUFF---------------------------
+ ****************************************************************************/
+static const struct net_device_ops rtl8192_netdev_ops = {
+	.ndo_open = _rtl92e_open,
+	.ndo_stop = _rtl92e_close,
+	.ndo_tx_timeout = _rtl92e_tx_timeout,
+	.ndo_set_rx_mode = _rtl92e_set_multicast,
+	.ndo_set_mac_address = _rtl92e_set_mac_adr,
+	.ndo_validate_addr = eth_validate_addr,
+	.ndo_start_xmit = rtllib_xmit,
+};
+
+static int _rtl92e_pci_probe(struct pci_dev *pdev,
+			     const struct pci_device_id *id)
+{
+	unsigned long ioaddr = 0;
+	struct net_device *dev = NULL;
+	struct r8192_priv *priv = NULL;
+	struct rtl819x_ops *ops = (struct rtl819x_ops *)(id->driver_data);
+	unsigned long pmem_start, pmem_len, pmem_flags;
+	int err = -ENOMEM;
+	u8 revision_id;
+
+	if (pci_enable_device(pdev)) {
+		dev_err(&pdev->dev, "Failed to enable PCI device");
+		return -EIO;
+	}
+
+	pci_set_master(pdev);
+
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(32))) {
+		if (dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32))) {
+			dev_info(&pdev->dev,
+				 "Unable to obtain 32bit DMA for consistent allocations\n");
+			goto err_pci_disable;
+		}
+	}
+	dev = alloc_rtllib(sizeof(struct r8192_priv));
+	if (!dev)
+		goto err_pci_disable;
+
+	err = -ENODEV;
+
+	pci_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+	priv = rtllib_priv(dev);
+	priv->rtllib = (struct rtllib_device *)netdev_priv_rsl(dev);
+	priv->pdev = pdev;
+	priv->rtllib->pdev = pdev;
+	if ((pdev->subsystem_vendor == PCI_VENDOR_ID_DLINK) &&
+	    (pdev->subsystem_device == 0x3304))
+		priv->rtllib->bSupportRemoteWakeUp = 1;
+	else
+		priv->rtllib->bSupportRemoteWakeUp = 0;
+
+	pmem_start = pci_resource_start(pdev, 1);
+	pmem_len = pci_resource_len(pdev, 1);
+	pmem_flags = pci_resource_flags(pdev, 1);
+
+	if (!(pmem_flags & IORESOURCE_MEM)) {
+		netdev_err(dev, "region #1 not a MMIO resource, aborting");
+		goto err_rel_rtllib;
+	}
+
+	dev_info(&pdev->dev, "Memory mapped space start: 0x%08lx\n",
+		 pmem_start);
+	if (!request_mem_region(pmem_start, pmem_len, DRV_NAME)) {
+		netdev_err(dev, "request_mem_region failed!");
+		goto err_rel_rtllib;
+	}
+
+	ioaddr = (unsigned long)ioremap(pmem_start, pmem_len);
+	if (ioaddr == (unsigned long)NULL) {
+		netdev_err(dev, "ioremap failed!");
+		goto err_rel_mem;
+	}
+
+	dev->mem_start = ioaddr;
+	dev->mem_end = ioaddr + pci_resource_len(pdev, 0);
+
+	pci_read_config_byte(pdev, 0x08, &revision_id);
+	/* If the revisionid is 0x10, the device uses rtl8192se. */
+	if (pdev->device == 0x8192 && revision_id == 0x10)
+		goto err_unmap;
+
+	priv->ops = ops;
+
+	if (!rtl92e_check_adapter(pdev, dev))
+		goto err_unmap;
+
+	dev->irq = pdev->irq;
+	priv->irq = 0;
+
+	dev->netdev_ops = &rtl8192_netdev_ops;
+
+	dev->wireless_handlers = &r8192_wx_handlers_def;
+	dev->ethtool_ops = &rtl819x_ethtool_ops;
+
+	dev->type = ARPHRD_ETHER;
+	dev->watchdog_timeo = HZ * 3;
+
+	if (dev_alloc_name(dev, ifname) < 0)
+		dev_alloc_name(dev, ifname);
+
+	if (_rtl92e_init(dev) != 0) {
+		netdev_warn(dev, "Initialization failed");
+		goto err_free_irq;
+	}
+
+	netif_carrier_off(dev);
+	netif_stop_queue(dev);
+
+	if (register_netdev(dev))
+		goto err_free_irq;
+
+	if (priv->polling_timer_on == 0)
+		rtl92e_check_rfctrl_gpio_timer(&priv->gpio_polling_timer);
+
+	return 0;
+
+err_free_irq:
+	free_irq(dev->irq, dev);
+	priv->irq = 0;
+err_unmap:
+	iounmap((void __iomem *)ioaddr);
+err_rel_mem:
+	release_mem_region(pmem_start, pmem_len);
+err_rel_rtllib:
+	free_rtllib(dev);
+err_pci_disable:
+	pci_disable_device(pdev);
+	return err;
+}
+
+static void _rtl92e_pci_disconnect(struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct r8192_priv *priv;
+	u32 i;
+
+	if (dev) {
+		unregister_netdev(dev);
+
+		priv = rtllib_priv(dev);
+
+		del_timer_sync(&priv->gpio_polling_timer);
+		cancel_delayed_work_sync(&priv->gpio_change_rf_wq);
+		priv->polling_timer_on = 0;
+		_rtl92e_down(dev, true);
+		rtl92e_dm_deinit(dev);
+		vfree(priv->pFirmware);
+		priv->pFirmware = NULL;
+		_rtl92e_free_rx_ring(dev);
+		for (i = 0; i < MAX_TX_QUEUE_COUNT; i++)
+			_rtl92e_free_tx_ring(dev, i);
+
+		if (priv->irq) {
+			dev_info(&pdev->dev, "Freeing irq %d\n", dev->irq);
+			free_irq(dev->irq, dev);
+			priv->irq = 0;
+		}
+
+		if (dev->mem_start != 0) {
+			iounmap((void __iomem *)dev->mem_start);
+			release_mem_region(pci_resource_start(pdev, 1),
+					pci_resource_len(pdev, 1));
+		}
+
+		free_rtllib(dev);
+	}
+
+	pci_disable_device(pdev);
+}
+
+bool rtl92e_enable_nic(struct net_device *dev)
+{
+	bool init_status = true;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&priv->rtllib->PowerSaveControl);
+
+	if (!priv->up) {
+		netdev_warn(dev, "%s(): Driver is already down!\n", __func__);
+		priv->bdisable_nic = false;
+		return false;
+	}
+
+	priv->bfirst_init = true;
+	init_status = priv->ops->initialize_adapter(dev);
+	if (!init_status) {
+		netdev_warn(dev, "%s(): Initialization failed!\n", __func__);
+		priv->bdisable_nic = false;
+		return false;
+	}
+	RT_CLEAR_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC);
+	priv->bfirst_init = false;
+
+	rtl92e_irq_enable(dev);
+	priv->bdisable_nic = false;
+	return init_status;
+}
+
+bool rtl92e_disable_nic(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 tmp_state = 0;
+
+	priv->bdisable_nic = true;
+	tmp_state = priv->rtllib->state;
+	rtllib_softmac_stop_protocol(priv->rtllib, 0, false);
+	priv->rtllib->state = tmp_state;
+	_rtl92e_cancel_deferred_work(priv);
+	rtl92e_irq_disable(dev);
+
+	priv->ops->stop_adapter(dev, false);
+	return true;
+}
+
+module_pci_driver(rtl8192_pci_driver);
+
+void rtl92e_check_rfctrl_gpio_timer(struct timer_list *t)
+{
+	struct r8192_priv *priv = from_timer(priv, t, gpio_polling_timer);
+
+	priv->polling_timer_on = 1;
+
+	schedule_delayed_work(&priv->gpio_change_rf_wq, 0);
+
+	mod_timer(&priv->gpio_polling_timer, jiffies +
+		  msecs_to_jiffies(RTLLIB_WATCH_DOG_TIME));
+}
+
+/***************************************************************************
+ * ------------------- module init / exit stubs ----------------
+ ***************************************************************************/
+MODULE_DESCRIPTION("Linux driver for Realtek RTL819x WiFi cards");
+MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
+MODULE_VERSION(DRV_VERSION);
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(RTL8192E_BOOT_IMG_FW);
+MODULE_FIRMWARE(RTL8192E_MAIN_IMG_FW);
+MODULE_FIRMWARE(RTL8192E_DATA_IMG_FW);
+
+module_param(ifname, charp, 0644);
+module_param(hwwep, int, 0644);
+
+MODULE_PARM_DESC(ifname, " Net interface name, wlan%d=default");
+MODULE_PARM_DESC(hwwep, " Try to use hardware WEP support(default use hw. set 0 to use software security)");
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
new file mode 100644
index 000000000..7021f9c43
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -0,0 +1,603 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL_CORE_H
+#define _RTL_CORE_H
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+#include <linux/rtnetlink.h>
+#include <linux/wireless.h>
+#include <linux/timer.h>
+#include <linux/proc_fs.h>
+#include <linux/if_arp.h>
+#include <linux/random.h>
+#include <linux/io.h>
+
+/* Need this defined before including local include files */
+#define DRV_NAME "rtl819xE"
+
+#include "../rtllib.h"
+
+#include "../dot11d.h"
+
+#include "r8192E_firmware.h"
+#include "r8192E_hw.h"
+
+#include "r8190P_def.h"
+#include "r8192E_dev.h"
+
+#include "rtl_eeprom.h"
+#include "rtl_ps.h"
+#include "rtl_pci.h"
+#include "rtl_cam.h"
+
+#define DRV_COPYRIGHT		\
+	"Copyright(c) 2008 - 2010 Realsil Semiconductor Corporation"
+#define DRV_AUTHOR  "<wlanfae@realtek.com>"
+#define DRV_VERSION  "0014.0401.2010"
+
+#define IS_HARDWARE_TYPE_8192SE(_priv)		\
+	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192SE)
+
+#define RTL_PCI_DEVICE(vend, dev, cfg) \
+	.vendor = (vend), .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+#define TOTAL_CAM_ENTRY		32
+#define CAM_CONTENT_COUNT	8
+
+#define HAL_HW_PCI_REVISION_ID_8192PCIE		0x01
+#define HAL_HW_PCI_REVISION_ID_8192SE	0x10
+
+#define RTL819X_DEFAULT_RF_TYPE		RF_1T2R
+
+#define RTLLIB_WATCH_DOG_TIME		2000
+
+#define MAX_DEV_ADDR_SIZE		8  /*support till 64 bit bus width OS*/
+#define MAX_FIRMWARE_INFORMATION_SIZE   32
+#define MAX_802_11_HEADER_LENGTH	(40 + MAX_FIRMWARE_INFORMATION_SIZE)
+#define ENCRYPTION_MAX_OVERHEAD		128
+#define MAX_FRAGMENT_COUNT		8
+#define MAX_TRANSMIT_BUFFER_SIZE	\
+	(1600 + (MAX_802_11_HEADER_LENGTH + ENCRYPTION_MAX_OVERHEAD) *	\
+	 MAX_FRAGMENT_COUNT)
+
+#define CMDPACKET_FRAG_SIZE (4 * (MAX_TRANSMIT_BUFFER_SIZE / 4) - 8)
+
+#define DEFAULT_FRAG_THRESHOLD	2342U
+#define MIN_FRAG_THRESHOLD	256U
+#define DEFAULT_BEACONINTERVAL	0x64U
+
+#define DEFAULT_RETRY_RTS	7
+#define DEFAULT_RETRY_DATA	7
+
+#define	PHY_RSSI_SLID_WIN_MAX			100
+
+#define TxBBGainTableLength			37
+#define CCKTxBBGainTableLength			23
+
+#define CHANNEL_PLAN_LEN			10
+#define sCrcLng					4
+
+#define NIC_SEND_HANG_THRESHOLD_NORMAL		4
+#define NIC_SEND_HANG_THRESHOLD_POWERSAVE	8
+
+#define MAX_TX_QUEUE				9
+
+#define MAX_RX_QUEUE				1
+
+#define MAX_RX_COUNT				64
+#define MAX_TX_QUEUE_COUNT			9
+
+extern int hwwep;
+
+enum nic_t {
+	NIC_UNKNOWN     = 0,
+	NIC_8192E       = 1,
+	NIC_8190P       = 2,
+	NIC_8192SE      = 4,
+	NIC_8192CE	= 5,
+	NIC_8192CU	= 6,
+	NIC_8192DE	= 7,
+	NIC_8192DU	= 8,
+};
+
+enum rt_eeprom_type {
+	EEPROM_93C46,
+	EEPROM_93C56,
+};
+
+enum dcmg_txcmd_op {
+	TXCMD_TXRA_HISTORY_CTRL		= 0xFF900000,
+	TXCMD_RESET_TX_PKT_BUFF		= 0xFF900001,
+	TXCMD_RESET_RX_PKT_BUFF		= 0xFF900002,
+	TXCMD_SET_TX_DURATION		= 0xFF900003,
+	TXCMD_SET_RX_RSSI		= 0xFF900004,
+	TXCMD_SET_TX_PWR_TRACKING	= 0xFF900005,
+	TXCMD_XXXX_CTRL,
+};
+
+enum rt_rf_type_819xu {
+	RF_TYPE_MIN = 0,
+	RF_8225,
+	RF_8256,
+	RF_8258,
+	RF_6052 = 4,
+	RF_PSEUDO_11N = 5,
+};
+
+enum rt_customer_id {
+	RT_CID_DEFAULT	  = 0,
+	RT_CID_8187_ALPHA0      = 1,
+	RT_CID_8187_SERCOMM_PS  = 2,
+	RT_CID_8187_HW_LED      = 3,
+	RT_CID_8187_NETGEAR     = 4,
+	RT_CID_WHQL	     = 5,
+	RT_CID_819x_CAMEO       = 6,
+	RT_CID_819x_RUNTOP      = 7,
+	RT_CID_819x_Senao       = 8,
+	RT_CID_TOSHIBA	  = 9,
+	RT_CID_819x_Netcore     = 10,
+	RT_CID_Nettronix	= 11,
+	RT_CID_DLINK	    = 12,
+	RT_CID_PRONET	   = 13,
+	RT_CID_COREGA	   = 14,
+	RT_CID_819x_ALPHA       = 15,
+	RT_CID_819x_Sitecom     = 16,
+	RT_CID_CCX	      = 17,
+	RT_CID_819x_Lenovo      = 18,
+	RT_CID_819x_QMI	 = 19,
+	RT_CID_819x_Edimax_Belkin = 20,
+	RT_CID_819x_Sercomm_Belkin = 21,
+	RT_CID_819x_CAMEO1 = 22,
+	RT_CID_819x_MSI = 23,
+	RT_CID_819x_Acer = 24,
+	RT_CID_819x_HP	= 27,
+	RT_CID_819x_CLEVO = 28,
+	RT_CID_819x_Arcadyan_Belkin = 29,
+	RT_CID_819x_SAMSUNG = 30,
+	RT_CID_819x_WNC_COREGA = 31,
+};
+
+enum reset_type {
+	RESET_TYPE_NORESET = 0x00,
+	RESET_TYPE_NORMAL = 0x01,
+	RESET_TYPE_SILENT = 0x02
+};
+
+struct rt_stats {
+	unsigned long rxrdu;
+	unsigned long rxok;
+	unsigned long rxdatacrcerr;
+	unsigned long rxmgmtcrcerr;
+	unsigned long rxcrcerrmin;
+	unsigned long rxcrcerrmid;
+	unsigned long rxcrcerrmax;
+	unsigned long received_rate_histogram[4][32];
+	unsigned long received_preamble_GI[2][32];
+	unsigned long numpacket_matchbssid;
+	unsigned long numpacket_toself;
+	unsigned long num_process_phyinfo;
+	unsigned long numqry_phystatus;
+	unsigned long numqry_phystatusCCK;
+	unsigned long numqry_phystatusHT;
+	unsigned long received_bwtype[5];
+	unsigned long rxoverflow;
+	unsigned long rxint;
+	unsigned long ints;
+	unsigned long shints;
+	unsigned long txoverflow;
+	unsigned long txbeokint;
+	unsigned long txbkokint;
+	unsigned long txviokint;
+	unsigned long txvookint;
+	unsigned long txbeaconokint;
+	unsigned long txbeaconerr;
+	unsigned long txmanageokint;
+	unsigned long txcmdpktokint;
+	unsigned long txbytesmulticast;
+	unsigned long txbytesbroadcast;
+	unsigned long txbytesunicast;
+	unsigned long rxbytesunicast;
+	unsigned long txretrycount;
+	u8	last_packet_rate;
+	unsigned long slide_signal_strength[100];
+	unsigned long slide_evm[100];
+	unsigned long	slide_rssi_total;
+	unsigned long slide_evm_total;
+	long signal_strength;
+	long signal_quality;
+	long last_signal_strength_inpercent;
+	long	recv_signal_power;
+	u8 rx_rssi_percentage[4];
+	u8 rx_evm_percentage[2];
+	long rxSNRdB[4];
+	u32 Slide_Beacon_pwdb[100];
+	u32 Slide_Beacon_Total;
+	u32	CurrentShowTxate;
+};
+
+struct channel_access_setting {
+	u16 SIFS_Timer;
+	u16 DIFS_Timer;
+	u16 SlotTimeTimer;
+	u16 EIFS_Timer;
+	u16 CWminIndex;
+	u16 CWmaxIndex;
+};
+
+struct init_gain {
+	u8	xaagccore1;
+	u8	xbagccore1;
+	u8	xcagccore1;
+	u8	xdagccore1;
+	u8	cca;
+
+};
+
+struct tx_ring {
+	u32 *desc;
+	u8 nStuckCount;
+	struct tx_ring *next;
+} __packed;
+
+struct rtl8192_tx_ring {
+	struct tx_desc *desc;
+	dma_addr_t dma;
+	unsigned int idx;
+	unsigned int entries;
+	struct sk_buff_head queue;
+};
+
+
+
+struct rtl819x_ops {
+	enum nic_t nic_type;
+	void (*get_eeprom_size)(struct net_device *dev);
+	void (*init_adapter_variable)(struct net_device *dev);
+	void (*init_before_adapter_start)(struct net_device *dev);
+	bool (*initialize_adapter)(struct net_device *dev);
+	void (*link_change)(struct net_device *dev);
+	void (*tx_fill_descriptor)(struct net_device *dev,
+				   struct tx_desc *tx_desc,
+				   struct cb_desc *cb_desc,
+				   struct sk_buff *skb);
+	void (*tx_fill_cmd_descriptor)(struct net_device *dev,
+				       struct tx_desc_cmd *entry,
+				       struct cb_desc *cb_desc,
+				       struct sk_buff *skb);
+	bool (*rx_query_status_descriptor)(struct net_device *dev,
+					   struct rtllib_rx_stats *stats,
+					   struct rx_desc *pdesc,
+					   struct sk_buff *skb);
+	bool (*rx_command_packet_handler)(struct net_device *dev,
+					  struct sk_buff *skb,
+					  struct rx_desc *pdesc);
+	void (*stop_adapter)(struct net_device *dev, bool reset);
+	void (*update_ratr_table)(struct net_device *dev);
+	void (*irq_enable)(struct net_device *dev);
+	void (*irq_disable)(struct net_device *dev);
+	void (*irq_clear)(struct net_device *dev);
+	void (*rx_enable)(struct net_device *dev);
+	void (*tx_enable)(struct net_device *dev);
+	void (*interrupt_recognized)(struct net_device *dev,
+				     u32 *p_inta, u32 *p_intb);
+	bool (*tx_check_stuck_handler)(struct net_device *dev);
+	bool (*rx_check_stuck_handler)(struct net_device *dev);
+};
+
+struct r8192_priv {
+	struct pci_dev *pdev;
+	struct pci_dev *bridge_pdev;
+
+	bool		bfirst_init;
+	bool		bfirst_after_down;
+	bool		initialized_at_probe;
+	bool		being_init_adapter;
+	bool		bDriverIsGoingToUnload;
+
+	int		irq;
+	short	irq_enabled;
+
+	short	up;
+	short	up_first_time;
+	struct delayed_work		update_beacon_wq;
+	struct delayed_work		watch_dog_wq;
+	struct delayed_work		txpower_tracking_wq;
+	struct delayed_work		rfpath_check_wq;
+	struct delayed_work		gpio_change_rf_wq;
+
+	struct channel_access_setting ChannelAccessSetting;
+
+	struct rtl819x_ops			*ops;
+	struct rtllib_device			*rtllib;
+
+	struct work_struct				reset_wq;
+
+	struct log_int_8190 InterruptLog;
+
+	enum rt_customer_id CustomerID;
+
+
+	enum rt_rf_type_819xu rf_chip;
+	enum ht_channel_width CurrentChannelBW;
+	struct bb_reg_definition PHYRegDef[4];
+	struct rate_adaptive rate_adaptive;
+
+	enum acm_method AcmMethod;
+
+	struct rt_firmware			*pFirmware;
+	enum rtl819x_loopback LoopbackMode;
+
+	struct timer_list			watch_dog_timer;
+	struct timer_list			fsync_timer;
+	struct timer_list			gpio_polling_timer;
+
+	spinlock_t				irq_th_lock;
+	spinlock_t				tx_lock;
+	spinlock_t				rf_ps_lock;
+	spinlock_t				ps_lock;
+
+	struct sk_buff_head		skb_queue;
+
+	struct tasklet_struct		irq_rx_tasklet;
+	struct tasklet_struct		irq_tx_tasklet;
+	struct tasklet_struct		irq_prepare_beacon_tasklet;
+
+	struct mutex				wx_mutex;
+	struct mutex				rf_mutex;
+	struct mutex				mutex;
+
+	struct rt_stats stats;
+	struct iw_statistics			wstats;
+
+	short (*rf_set_sens)(struct net_device *dev, short sens);
+	u8 (*rf_set_chan)(struct net_device *dev, u8 ch);
+
+	struct rx_desc *rx_ring[MAX_RX_QUEUE];
+	struct sk_buff	*rx_buf[MAX_RX_QUEUE][MAX_RX_COUNT];
+	dma_addr_t	rx_ring_dma[MAX_RX_QUEUE];
+	unsigned int	rx_idx[MAX_RX_QUEUE];
+	int		rxringcount;
+	u16		rxbuffersize;
+
+	u64		LastRxDescTSF;
+
+	u32		ReceiveConfig;
+	u8		retry_data;
+	u8		retry_rts;
+	u16		rts;
+
+	struct rtl8192_tx_ring tx_ring[MAX_TX_QUEUE_COUNT];
+	int		 txringcount;
+	atomic_t	tx_pending[0x10];
+
+	u16		ShortRetryLimit;
+	u16		LongRetryLimit;
+
+	bool		hw_radio_off;
+	bool		blinked_ingpio;
+	u8		polling_timer_on;
+
+	/**********************************************************/
+
+	enum card_type {
+		PCI, MINIPCI,
+		CARDBUS, USB
+	} card_type;
+
+	struct work_struct qos_activate;
+
+	short	promisc;
+
+	short	chan;
+	short	sens;
+	short	max_sens;
+
+	u8 ScanDelay;
+	bool ps_force;
+
+	u32 irq_mask[2];
+
+	u8 Rf_Mode;
+	enum nic_t card_8192;
+	u8 card_8192_version;
+
+	u8 rf_type;
+	u8 IC_Cut;
+	char nick[IW_ESSID_MAX_SIZE + 1];
+	u8 check_roaming_cnt;
+
+	u32 SilentResetRxSlotIndex;
+	u32 SilentResetRxStuckEvent[MAX_SILENT_RESET_RX_SLOT_NUM];
+
+	u16 basic_rate;
+	u8 short_preamble;
+	u8 dot11_current_preamble_mode;
+	u8 slot_time;
+	u16 SifsTime;
+
+	bool AutoloadFailFlag;
+
+	short	epromtype;
+	u16 eeprom_vid;
+	u16 eeprom_did;
+	u8 eeprom_CustomerID;
+	u16 eeprom_ChannelPlan;
+
+	u8 EEPROMTxPowerLevelCCK[14];
+	u8 EEPROMTxPowerLevelOFDM24G[14];
+	u8 EEPROMRfACCKChnl1TxPwLevel[3];
+	u8 EEPROMRfAOfdmChnlTxPwLevel[3];
+	u8 EEPROMRfCCCKChnl1TxPwLevel[3];
+	u8 EEPROMRfCOfdmChnlTxPwLevel[3];
+	u16 EEPROMAntPwDiff;
+	u8 EEPROMThermalMeter;
+	u8 EEPROMCrystalCap;
+
+	u8 EEPROMLegacyHTTxPowerDiff;
+
+	u8 CrystalCap;
+	u8 ThermalMeter[2];
+
+	u8 SwChnlInProgress;
+	u8 SwChnlStage;
+	u8 SwChnlStep;
+	u8 SetBWModeInProgress;
+
+	u8 nCur40MhzPrimeSC;
+
+	u32 RfReg0Value[4];
+	u8 NumTotalRFPath;
+	bool brfpath_rxenable[4];
+
+	bool bTXPowerDataReadFromEEPORM;
+
+	u16 RegChannelPlan;
+	u16 ChannelPlan;
+
+	bool RegRfOff;
+	bool isRFOff;
+	bool bInPowerSaveMode;
+	u8 bHwRfOffAction;
+
+	bool rf_change_in_progress;
+	bool SetRFPowerStateInProgress;
+	bool bdisable_nic;
+
+	u8 DM_Type;
+
+	u8 CckPwEnl;
+	u16 TSSI_13dBm;
+	u32 Pwr_Track;
+	u8 CCKPresentAttentuation_20Mdefault;
+	u8 CCKPresentAttentuation_40Mdefault;
+	s8 CCKPresentAttentuation_difference;
+	s8 CCKPresentAttentuation;
+	long undecorated_smoothed_pwdb;
+
+	u32 MCSTxPowerLevelOriginalOffset[6];
+	u8 TxPowerLevelCCK[14];
+	u8 TxPowerLevelCCK_A[14];
+	u8 TxPowerLevelCCK_C[14];
+	u8		TxPowerLevelOFDM24G[14];
+	u8		TxPowerLevelOFDM24G_A[14];
+	u8		TxPowerLevelOFDM24G_C[14];
+	u8		LegacyHTTxPowerDiff;
+	s8		RF_C_TxPwDiff;
+	u8		AntennaTxPwDiff[3];
+
+	bool		bDynamicTxHighPower;
+	bool		bDynamicTxLowPower;
+	bool		bLastDTPFlag_High;
+	bool		bLastDTPFlag_Low;
+
+	u8		rfa_txpowertrackingindex;
+	u8		rfa_txpowertrackingindex_real;
+	u8		rfa_txpowertracking_default;
+	u8		rfc_txpowertrackingindex;
+	u8		rfc_txpowertrackingindex_real;
+	bool		btxpower_tracking;
+	bool		bcck_in_ch14;
+
+	u8		txpower_count;
+	bool		btxpower_trackingInit;
+
+	u8		OFDM_index[2];
+	u8		CCK_index;
+
+	u8		Record_CCK_20Mindex;
+	u8		Record_CCK_40Mindex;
+
+	struct init_gain initgain_backup;
+	u8		DefaultInitialGain[4];
+	bool		bis_any_nonbepkts;
+	bool		bcurrent_turbo_EDCA;
+	bool		bis_cur_rdlstate;
+
+	bool		bfsync_processing;
+	u32		rate_record;
+	u32		rateCountDiffRecord;
+	u32		ContinueDiffCount;
+	bool		bswitch_fsync;
+	u8		framesync;
+	u32		framesyncC34;
+	u8		framesyncMonitor;
+
+	u32		reset_count;
+
+	enum reset_type ResetProgress;
+	bool		bForcedSilentReset;
+	bool		bDisableNormalResetCheck;
+	u16		TxCounter;
+	u16		RxCounter;
+	bool		bResetInProgress;
+	bool		force_reset;
+	bool		force_lps;
+
+	bool		chan_forced;
+
+	u8		PwrDomainProtect;
+	u8		H2CTxCmdSeq;
+};
+
+extern const struct ethtool_ops rtl819x_ethtool_ops;
+
+u8 rtl92e_readb(struct net_device *dev, int x);
+u32 rtl92e_readl(struct net_device *dev, int x);
+u16 rtl92e_readw(struct net_device *dev, int x);
+void rtl92e_writeb(struct net_device *dev, int x, u8 y);
+void rtl92e_writew(struct net_device *dev, int x, u16 y);
+void rtl92e_writel(struct net_device *dev, int x, u32 y);
+
+void force_pci_posting(struct net_device *dev);
+
+void rtl92e_rx_enable(struct net_device *dev);
+void rtl92e_tx_enable(struct net_device *dev);
+
+void rtl92e_hw_sleep_wq(void *data);
+void rtl92e_commit(struct net_device *dev);
+
+void rtl92e_check_rfctrl_gpio_timer(struct timer_list *t);
+
+void rtl92e_hw_wakeup_wq(void *data);
+
+void rtl92e_reset_desc_ring(struct net_device *dev);
+void rtl92e_set_wireless_mode(struct net_device *dev, u8 wireless_mode);
+void rtl92e_irq_enable(struct net_device *dev);
+void rtl92e_config_rate(struct net_device *dev, u16 *rate_config);
+void rtl92e_irq_disable(struct net_device *dev);
+
+void rtl92e_update_rx_pkt_timestamp(struct net_device *dev,
+				    struct rtllib_rx_stats *stats);
+long rtl92e_translate_to_dbm(struct r8192_priv *priv, u8 signal_strength_index);
+void rtl92e_update_rx_statistics(struct r8192_priv *priv,
+				 struct rtllib_rx_stats *pprevious_stats);
+u8 rtl92e_evm_db_to_percent(s8 value);
+u8 rtl92e_rx_db_to_percent(s8 antpower);
+void rtl92e_copy_mpdu_stats(struct rtllib_rx_stats *psrc_stats,
+			    struct rtllib_rx_stats *ptarget_stats);
+bool rtl92e_enable_nic(struct net_device *dev);
+bool rtl92e_disable_nic(struct net_device *dev);
+
+bool rtl92e_set_rf_state(struct net_device *dev,
+			 enum rt_rf_power_state state_to_set,
+			 RT_RF_CHANGE_SOURCE change_source);
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
new file mode 100644
index 000000000..f660f947a
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -0,0 +1,2310 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "rtl_dm.h"
+#include "r8192E_hw.h"
+#include "r8192E_phy.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h"
+#include "r8192E_cmdpkt.h"
+
+/*---------------------------Define Local Constant---------------------------*/
+static u32 edca_setting_DL[HT_IOT_PEER_MAX] = {
+	0x5e4322,
+	0x5e4322,
+	0x5ea44f,
+	0x5e4322,
+	0x604322,
+	0xa44f,
+	0x5e4322,
+	0x5e4332
+};
+
+static u32 edca_setting_DL_GMode[HT_IOT_PEER_MAX] = {
+	0x5e4322,
+	0x5e4322,
+	0x5e4322,
+	0x5e4322,
+	0x604322,
+	0xa44f,
+	0x5e4322,
+	0x5e4322
+};
+
+static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
+	0x5e4322,
+	0xa44f,
+	0x5ea44f,
+	0x5e4322,
+	0x604322,
+	0x5e4322,
+	0x5e4322,
+	0x5e4332
+};
+
+const u32 dm_tx_bb_gain[TxBBGainTableLength] = {
+	0x7f8001fe, /* 12 dB */
+	0x788001e2, /* 11 dB */
+	0x71c001c7,
+	0x6b8001ae,
+	0x65400195,
+	0x5fc0017f,
+	0x5a400169,
+	0x55400155,
+	0x50800142,
+	0x4c000130,
+	0x47c0011f,
+	0x43c0010f,
+	0x40000100,
+	0x3c8000f2,
+	0x390000e4,
+	0x35c000d7,
+	0x32c000cb,
+	0x300000c0,
+	0x2d4000b5,
+	0x2ac000ab,
+	0x288000a2,
+	0x26000098,
+	0x24000090,
+	0x22000088,
+	0x20000080,
+	0x1a00006c,
+	0x1c800072,
+	0x18000060,
+	0x19800066,
+	0x15800056,
+	0x26c0005b,
+	0x14400051,
+	0x24400051,
+	0x1300004c,
+	0x12000048,
+	0x11000044,
+	0x10000040, /* -24 dB */
+};
+
+const u8 dm_cck_tx_bb_gain[CCKTxBBGainTableLength][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}
+};
+
+const u8 dm_cck_tx_bb_gain_ch14[CCKTxBBGainTableLength][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},
+	{0x2d, 0x2d, 0x27, 0x17, 0x00, 0x00, 0x00, 0x00},
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},
+	{0x28, 0x28, 0x22, 0x14, 0x00, 0x00, 0x00, 0x00},
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}
+};
+
+/*---------------------------Define Local Constant---------------------------*/
+
+
+/*------------------------Define global variable-----------------------------*/
+struct dig_t dm_digtable;
+
+struct drx_path_sel DM_RxPathSelTable;
+/*------------------------Define global variable-----------------------------*/
+
+
+/*------------------------Define local variable------------------------------*/
+/*------------------------Define local variable------------------------------*/
+
+
+
+/*---------------------Define local function prototype-----------------------*/
+static void _rtl92e_dm_check_rate_adaptive(struct net_device *dev);
+
+static void _rtl92e_dm_init_bandwidth_autoswitch(struct net_device *dev);
+static	void	_rtl92e_dm_bandwidth_autoswitch(struct net_device *dev);
+
+
+static	void	_rtl92e_dm_check_tx_power_tracking(struct net_device *dev);
+
+static void _rtl92e_dm_bb_initialgain_restore(struct net_device *dev);
+static void _rtl92e_dm_dig_init(struct net_device *dev);
+static void _rtl92e_dm_ctrl_initgain_byrssi(struct net_device *dev);
+static void _rtl92e_dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev);
+static void _rtl92e_dm_ctrl_initgain_byrssi_driver(struct net_device *dev);
+static void _rtl92e_dm_ctrl_initgain_byrssi_false_alarm(struct net_device *dev);
+static void _rtl92e_dm_initial_gain(struct net_device *dev);
+static void _rtl92e_dm_pd_th(struct net_device *dev);
+static void _rtl92e_dm_cs_ratio(struct net_device *dev);
+
+static	void _rtl92e_dm_init_cts_to_self(struct net_device *dev);
+static void _rtl92e_dm_init_wa_broadcom_iot(struct net_device *dev);
+
+static void _rtl92e_dm_check_edca_turbo(struct net_device *dev);
+static void _rtl92e_dm_check_rx_path_selection(struct net_device *dev);
+static void _rtl92e_dm_init_rx_path_selection(struct net_device *dev);
+static void _rtl92e_dm_rx_path_sel_byrssi(struct net_device *dev);
+
+
+static void _rtl92e_dm_init_fsync(struct net_device *dev);
+static void _rtl92e_dm_deinit_fsync(struct net_device *dev);
+
+static	void _rtl92e_dm_check_txrateandretrycount(struct net_device *dev);
+static void _rtl92e_dm_check_fsync(struct net_device *dev);
+static void _rtl92e_dm_check_rf_ctrl_gpio(void *data);
+static void _rtl92e_dm_fsync_timer_callback(struct timer_list *t);
+
+/*---------------------Define local function prototype-----------------------*/
+
+static	void	_rtl92e_dm_init_dynamic_tx_power(struct net_device *dev);
+static void _rtl92e_dm_dynamic_tx_power(struct net_device *dev);
+
+static void _rtl92e_dm_send_rssi_to_fw(struct net_device *dev);
+static void _rtl92e_dm_cts_to_self(struct net_device *dev);
+/*---------------------------Define function prototype------------------------*/
+
+void rtl92e_dm_init(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->DM_Type = DM_Type_ByDriver;
+
+	priv->undecorated_smoothed_pwdb = -1;
+
+	_rtl92e_dm_init_dynamic_tx_power(dev);
+
+	rtl92e_init_adaptive_rate(dev);
+
+	_rtl92e_dm_dig_init(dev);
+	rtl92e_dm_init_edca_turbo(dev);
+	_rtl92e_dm_init_bandwidth_autoswitch(dev);
+	_rtl92e_dm_init_fsync(dev);
+	_rtl92e_dm_init_rx_path_selection(dev);
+	_rtl92e_dm_init_cts_to_self(dev);
+	if (IS_HARDWARE_TYPE_8192SE(dev))
+		_rtl92e_dm_init_wa_broadcom_iot(dev);
+
+	INIT_DELAYED_WORK_RSL(&priv->gpio_change_rf_wq,
+			      (void *)_rtl92e_dm_check_rf_ctrl_gpio, dev);
+}
+
+void rtl92e_dm_deinit(struct net_device *dev)
+{
+
+	_rtl92e_dm_deinit_fsync(dev);
+
+}
+
+void rtl92e_dm_watchdog(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->being_init_adapter)
+		return;
+
+	_rtl92e_dm_check_txrateandretrycount(dev);
+	_rtl92e_dm_check_edca_turbo(dev);
+
+	_rtl92e_dm_check_rate_adaptive(dev);
+	_rtl92e_dm_dynamic_tx_power(dev);
+	_rtl92e_dm_check_tx_power_tracking(dev);
+
+	_rtl92e_dm_ctrl_initgain_byrssi(dev);
+	_rtl92e_dm_bandwidth_autoswitch(dev);
+
+	_rtl92e_dm_check_rx_path_selection(dev);
+	_rtl92e_dm_check_fsync(dev);
+
+	_rtl92e_dm_send_rssi_to_fw(dev);
+	_rtl92e_dm_cts_to_self(dev);
+}
+
+void rtl92e_init_adaptive_rate(struct net_device *dev)
+{
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rate_adaptive *pra = &priv->rate_adaptive;
+
+	pra->ratr_state = DM_RATR_STA_MAX;
+	pra->high2low_rssi_thresh_for_ra = RateAdaptiveTH_High;
+	pra->low2high_rssi_thresh_for_ra20M = RateAdaptiveTH_Low_20M+5;
+	pra->low2high_rssi_thresh_for_ra40M = RateAdaptiveTH_Low_40M+5;
+
+	pra->high_rssi_thresh_for_ra = RateAdaptiveTH_High+5;
+	pra->low_rssi_thresh_for_ra20M = RateAdaptiveTH_Low_20M;
+	pra->low_rssi_thresh_for_ra40M = RateAdaptiveTH_Low_40M;
+
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		pra->ping_rssi_enable = 1;
+	else
+		pra->ping_rssi_enable = 0;
+	pra->ping_rssi_thresh_for_ra = 15;
+
+
+	if (priv->rf_type == RF_2T4R) {
+		pra->upper_rssi_threshold_ratr		=	0x8f0f0000;
+		pra->middle_rssi_threshold_ratr		=	0x8f0ff000;
+		pra->low_rssi_threshold_ratr		=	0x8f0ff001;
+		pra->low_rssi_threshold_ratr_40M	=	0x8f0ff005;
+		pra->low_rssi_threshold_ratr_20M	=	0x8f0ff001;
+		pra->ping_rssi_ratr	=	0x0000000d;
+	} else if (priv->rf_type == RF_1T2R) {
+		pra->upper_rssi_threshold_ratr		=	0x000fc000;
+		pra->middle_rssi_threshold_ratr		=	0x000ff000;
+		pra->low_rssi_threshold_ratr		=	0x000ff001;
+		pra->low_rssi_threshold_ratr_40M	=	0x000ff005;
+		pra->low_rssi_threshold_ratr_20M	=	0x000ff001;
+		pra->ping_rssi_ratr	=	0x0000000d;
+	}
+
+}
+
+
+static void _rtl92e_dm_check_rate_adaptive(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_hi_throughput *pHTInfo = priv->rtllib->pHTInfo;
+	struct rate_adaptive *pra = &priv->rate_adaptive;
+	u32 currentRATR, targetRATR = 0;
+	u32 LowRSSIThreshForRA = 0, HighRSSIThreshForRA = 0;
+	bool bshort_gi_enabled = false;
+	static u8 ping_rssi_state;
+
+	if (!priv->up) {
+		return;
+	}
+
+	if (pra->rate_adaptive_disabled)
+		return;
+
+	if (!(priv->rtllib->mode == WIRELESS_MODE_N_24G ||
+	    priv->rtllib->mode == WIRELESS_MODE_N_5G))
+		return;
+
+	if (priv->rtllib->state == RTLLIB_LINKED) {
+
+		bshort_gi_enabled = (pHTInfo->cur_tx_bw40mhz &&
+				     pHTInfo->bCurShortGI40MHz) ||
+				    (!pHTInfo->cur_tx_bw40mhz &&
+				     pHTInfo->bCurShortGI20MHz);
+
+		pra->upper_rssi_threshold_ratr =
+				(pra->upper_rssi_threshold_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
+
+		pra->middle_rssi_threshold_ratr =
+				(pra->middle_rssi_threshold_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
+
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+			pra->low_rssi_threshold_ratr =
+				(pra->low_rssi_threshold_ratr_40M & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
+		} else {
+			pra->low_rssi_threshold_ratr =
+				(pra->low_rssi_threshold_ratr_20M & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
+		}
+		pra->ping_rssi_ratr =
+				(pra->ping_rssi_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
+
+		if (pra->ratr_state == DM_RATR_STA_HIGH) {
+			HighRSSIThreshForRA = pra->high2low_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low_rssi_thresh_for_ra40M) : (pra->low_rssi_thresh_for_ra20M);
+		} else if (pra->ratr_state == DM_RATR_STA_LOW) {
+			HighRSSIThreshForRA = pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low2high_rssi_thresh_for_ra40M) : (pra->low2high_rssi_thresh_for_ra20M);
+		} else {
+			HighRSSIThreshForRA = pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low_rssi_thresh_for_ra40M) : (pra->low_rssi_thresh_for_ra20M);
+		}
+
+		if (priv->undecorated_smoothed_pwdb >=
+		    (long)HighRSSIThreshForRA) {
+			pra->ratr_state = DM_RATR_STA_HIGH;
+			targetRATR = pra->upper_rssi_threshold_ratr;
+		} else if (priv->undecorated_smoothed_pwdb >=
+			   (long)LowRSSIThreshForRA) {
+			pra->ratr_state = DM_RATR_STA_MIDDLE;
+			targetRATR = pra->middle_rssi_threshold_ratr;
+		} else {
+			pra->ratr_state = DM_RATR_STA_LOW;
+			targetRATR = pra->low_rssi_threshold_ratr;
+		}
+
+		if (pra->ping_rssi_enable) {
+			if (priv->undecorated_smoothed_pwdb <
+			    (long)(pra->ping_rssi_thresh_for_ra+5)) {
+				if ((priv->undecorated_smoothed_pwdb <
+				     (long)pra->ping_rssi_thresh_for_ra) ||
+				    ping_rssi_state) {
+					pra->ratr_state = DM_RATR_STA_LOW;
+					targetRATR = pra->ping_rssi_ratr;
+					ping_rssi_state = 1;
+				}
+			} else {
+				ping_rssi_state = 0;
+			}
+		}
+
+		if (priv->rtllib->GetHalfNmodeSupportByAPsHandler(dev))
+			targetRATR &=  0xf00fffff;
+
+		currentRATR = rtl92e_readl(dev, RATR0);
+		if (targetRATR !=  currentRATR) {
+			u32 ratr_value;
+
+			ratr_value = targetRATR;
+			if (priv->rf_type == RF_1T2R)
+				ratr_value &= ~(RATE_ALL_OFDM_2SS);
+			rtl92e_writel(dev, RATR0, ratr_value);
+			rtl92e_writeb(dev, UFWP, 1);
+
+			pra->last_ratr = targetRATR;
+		}
+
+	} else {
+		pra->ratr_state = DM_RATR_STA_MAX;
+	}
+}
+
+static void _rtl92e_dm_init_bandwidth_autoswitch(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->rtllib->bandwidth_auto_switch.threshold_20Mhzto40Mhz = BW_AUTO_SWITCH_LOW_HIGH;
+	priv->rtllib->bandwidth_auto_switch.threshold_40Mhzto20Mhz = BW_AUTO_SWITCH_HIGH_LOW;
+	priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = false;
+	priv->rtllib->bandwidth_auto_switch.bautoswitch_enable = false;
+}
+
+static void _rtl92e_dm_bandwidth_autoswitch(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 ||
+	   !priv->rtllib->bandwidth_auto_switch.bautoswitch_enable)
+		return;
+	if (!priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz) {
+		if (priv->undecorated_smoothed_pwdb <=
+		    priv->rtllib->bandwidth_auto_switch.threshold_40Mhzto20Mhz)
+			priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = true;
+	} else {
+		if (priv->undecorated_smoothed_pwdb >=
+		    priv->rtllib->bandwidth_auto_switch.threshold_20Mhzto40Mhz)
+			priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = false;
+	}
+}
+
+static u32 OFDMSwingTable[OFDM_Table_Length] = {
+	0x7f8001fe,
+	0x71c001c7,
+	0x65400195,
+	0x5a400169,
+	0x50800142,
+	0x47c0011f,
+	0x40000100,
+	0x390000e4,
+	0x32c000cb,
+	0x2d4000b5,
+	0x288000a2,
+	0x24000090,
+	0x20000080,
+	0x1c800072,
+	0x19800066,
+	0x26c0005b,
+	0x24400051,
+	0x12000048,
+	0x10000040
+};
+
+static u8	CCKSwingTable_Ch1_Ch13[CCK_Table_length][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}
+};
+
+static u8	CCKSwingTable_Ch14[CCK_Table_length][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}
+};
+
+#define		Pw_Track_Flag				0x11d
+#define		Tssi_Mea_Value				0x13c
+#define		Tssi_Report_Value1			0x134
+#define		Tssi_Report_Value2			0x13e
+#define		FW_Busy_Flag				0x13f
+
+static void _rtl92e_dm_tx_update_tssi_weak_signal(struct net_device *dev,
+						  u8 RF_Type)
+{
+	struct r8192_priv *p = rtllib_priv(dev);
+
+	if (RF_Type == RF_2T4R) {
+		if ((p->rfa_txpowertrackingindex > 0) &&
+		    (p->rfc_txpowertrackingindex > 0)) {
+			p->rfa_txpowertrackingindex--;
+			if (p->rfa_txpowertrackingindex_real > 4) {
+				p->rfa_txpowertrackingindex_real--;
+				rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+						  bMaskDWord,
+						  dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			}
+
+			p->rfc_txpowertrackingindex--;
+			if (p->rfc_txpowertrackingindex_real > 4) {
+				p->rfc_txpowertrackingindex_real--;
+				rtl92e_set_bb_reg(dev,
+						  rOFDM0_XCTxIQImbalance,
+						  bMaskDWord,
+						  dm_tx_bb_gain[p->rfc_txpowertrackingindex_real]);
+			}
+		} else {
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[4]);
+			rtl92e_set_bb_reg(dev,
+					  rOFDM0_XCTxIQImbalance,
+					  bMaskDWord, dm_tx_bb_gain[4]);
+		}
+	} else {
+		if (p->rfa_txpowertrackingindex > 0) {
+			p->rfa_txpowertrackingindex--;
+			if (p->rfa_txpowertrackingindex_real > 4) {
+				p->rfa_txpowertrackingindex_real--;
+				rtl92e_set_bb_reg(dev,
+						  rOFDM0_XATxIQImbalance,
+						  bMaskDWord,
+						  dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			}
+		} else {
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord, dm_tx_bb_gain[4]);
+		}
+	}
+}
+
+static void _rtl92e_dm_tx_update_tssi_strong_signal(struct net_device *dev,
+						    u8 RF_Type)
+{
+	struct r8192_priv *p = rtllib_priv(dev);
+
+	if (RF_Type == RF_2T4R) {
+		if ((p->rfa_txpowertrackingindex < TxBBGainTableLength - 1) &&
+		    (p->rfc_txpowertrackingindex < TxBBGainTableLength - 1)) {
+			p->rfa_txpowertrackingindex++;
+			p->rfa_txpowertrackingindex_real++;
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			p->rfc_txpowertrackingindex++;
+			p->rfc_txpowertrackingindex_real++;
+			rtl92e_set_bb_reg(dev, rOFDM0_XCTxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[p->rfc_txpowertrackingindex_real]);
+		} else {
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[TxBBGainTableLength - 1]);
+			rtl92e_set_bb_reg(dev, rOFDM0_XCTxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[TxBBGainTableLength - 1]);
+		}
+	} else {
+		if (p->rfa_txpowertrackingindex < (TxBBGainTableLength - 1)) {
+			p->rfa_txpowertrackingindex++;
+			p->rfa_txpowertrackingindex_real++;
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+		} else {
+			rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					  bMaskDWord,
+					  dm_tx_bb_gain[TxBBGainTableLength - 1]);
+		}
+	}
+}
+
+static void _rtl92e_dm_tx_power_tracking_callback_tssi(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	bool	viviflag = false;
+	struct dcmd_txcmd tx_cmd;
+	u8	powerlevelOFDM24G;
+	int	i = 0, j = 0, k = 0;
+	u8	RF_Type, tmp_report[5] = {0, 0, 0, 0, 0};
+	u32	Value;
+	u8	Pwr_Flag;
+	u16	Avg_TSSI_Meas, TSSI_13dBm, Avg_TSSI_Meas_from_driver = 0;
+	u32	delta = 0;
+
+	rtl92e_writeb(dev, Pw_Track_Flag, 0);
+	rtl92e_writeb(dev, FW_Busy_Flag, 0);
+	priv->rtllib->bdynamic_txpower_enable = false;
+
+	powerlevelOFDM24G = priv->Pwr_Track >> 24;
+	RF_Type = priv->rf_type;
+	Value = (RF_Type<<8) | powerlevelOFDM24G;
+
+	for (j = 0; j <= 30; j++) {
+
+		tx_cmd.Op		= TXCMD_SET_TX_PWR_TRACKING;
+		tx_cmd.Length	= 4;
+		tx_cmd.Value		= Value;
+		rtl92e_send_cmd_pkt(dev, DESC_PACKET_TYPE_NORMAL, (u8 *)&tx_cmd,
+				    sizeof(struct dcmd_txcmd));
+		mdelay(1);
+		for (i = 0; i <= 30; i++) {
+			Pwr_Flag = rtl92e_readb(dev, Pw_Track_Flag);
+
+			if (Pwr_Flag == 0) {
+				mdelay(1);
+
+				if (priv->bResetInProgress) {
+					rtl92e_writeb(dev, Pw_Track_Flag, 0);
+					rtl92e_writeb(dev, FW_Busy_Flag, 0);
+					return;
+				}
+				if (priv->rtllib->rf_power_state != rf_on) {
+					rtl92e_writeb(dev, Pw_Track_Flag, 0);
+					rtl92e_writeb(dev, FW_Busy_Flag, 0);
+					return;
+				}
+
+				continue;
+			}
+
+			Avg_TSSI_Meas = rtl92e_readw(dev, Tssi_Mea_Value);
+
+			if (Avg_TSSI_Meas == 0) {
+				rtl92e_writeb(dev, Pw_Track_Flag, 0);
+				rtl92e_writeb(dev, FW_Busy_Flag, 0);
+				return;
+			}
+
+			for (k = 0; k < 5; k++) {
+				if (k != 4)
+					tmp_report[k] = rtl92e_readb(dev,
+							 Tssi_Report_Value1+k);
+				else
+					tmp_report[k] = rtl92e_readb(dev,
+							 Tssi_Report_Value2);
+
+				if (tmp_report[k] <= 20) {
+					viviflag = true;
+					break;
+				}
+			}
+
+			if (viviflag) {
+				rtl92e_writeb(dev, Pw_Track_Flag, 0);
+				viviflag = false;
+				for (k = 0; k < 5; k++)
+					tmp_report[k] = 0;
+				break;
+			}
+
+			for (k = 0; k < 5; k++)
+				Avg_TSSI_Meas_from_driver += tmp_report[k];
+
+			Avg_TSSI_Meas_from_driver *= 100 / 5;
+			TSSI_13dBm = priv->TSSI_13dBm;
+
+			if (Avg_TSSI_Meas_from_driver > TSSI_13dBm)
+				delta = Avg_TSSI_Meas_from_driver - TSSI_13dBm;
+			else
+				delta = TSSI_13dBm - Avg_TSSI_Meas_from_driver;
+
+			if (delta <= E_FOR_TX_POWER_TRACK) {
+				priv->rtllib->bdynamic_txpower_enable = true;
+				rtl92e_writeb(dev, Pw_Track_Flag, 0);
+				rtl92e_writeb(dev, FW_Busy_Flag, 0);
+				return;
+			}
+			if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK)
+				_rtl92e_dm_tx_update_tssi_weak_signal(dev,
+								      RF_Type);
+			else
+				_rtl92e_dm_tx_update_tssi_strong_signal(dev, RF_Type);
+
+			if (RF_Type == RF_2T4R) {
+				priv->CCKPresentAttentuation_difference
+					= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
+			} else {
+				priv->CCKPresentAttentuation_difference
+					= priv->rfa_txpowertrackingindex_real - priv->rfa_txpowertracking_default;
+			}
+
+			if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				priv->CCKPresentAttentuation =
+					 priv->CCKPresentAttentuation_20Mdefault +
+					 priv->CCKPresentAttentuation_difference;
+			else
+				priv->CCKPresentAttentuation =
+					 priv->CCKPresentAttentuation_40Mdefault +
+					 priv->CCKPresentAttentuation_difference;
+
+			if (priv->CCKPresentAttentuation > (CCKTxBBGainTableLength-1))
+				priv->CCKPresentAttentuation = CCKTxBBGainTableLength-1;
+			if (priv->CCKPresentAttentuation < 0)
+				priv->CCKPresentAttentuation = 0;
+
+			if (priv->CCKPresentAttentuation > -1 &&
+			    priv->CCKPresentAttentuation < CCKTxBBGainTableLength) {
+				if (priv->rtllib->current_network.channel == 14 &&
+				    !priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = true;
+					rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else if (priv->rtllib->current_network.channel != 14 && priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = false;
+					rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else
+					rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+			}
+
+			if (priv->CCKPresentAttentuation_difference <= -12 ||
+			    priv->CCKPresentAttentuation_difference >= 24) {
+				priv->rtllib->bdynamic_txpower_enable = true;
+				rtl92e_writeb(dev, Pw_Track_Flag, 0);
+				rtl92e_writeb(dev, FW_Busy_Flag, 0);
+				return;
+			}
+
+			rtl92e_writeb(dev, Pw_Track_Flag, 0);
+			Avg_TSSI_Meas_from_driver = 0;
+			for (k = 0; k < 5; k++)
+				tmp_report[k] = 0;
+			break;
+		}
+		rtl92e_writeb(dev, FW_Busy_Flag, 0);
+	}
+	priv->rtllib->bdynamic_txpower_enable = true;
+	rtl92e_writeb(dev, Pw_Track_Flag, 0);
+}
+
+static void _rtl92e_dm_tx_power_tracking_cb_thermal(struct net_device *dev)
+{
+#define ThermalMeterVal	9
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 tmpRegA, TempCCk;
+	u8 tmpOFDMindex, tmpCCKindex, tmpCCK20Mindex, tmpCCK40Mindex, tmpval;
+	int i = 0, CCKSwingNeedUpdate = 0;
+
+	if (!priv->btxpower_trackingInit) {
+		tmpRegA = rtl92e_get_bb_reg(dev, rOFDM0_XATxIQImbalance,
+					    bMaskDWord);
+		for (i = 0; i < OFDM_Table_Length; i++) {
+			if (tmpRegA == OFDMSwingTable[i]) {
+				priv->OFDM_index[0] = i;
+			}
+		}
+
+		TempCCk = rtl92e_get_bb_reg(dev, rCCK0_TxFilter1, bMaskByte2);
+		for (i = 0; i < CCK_Table_length; i++) {
+			if (TempCCk == (u32)CCKSwingTable_Ch1_Ch13[i][0]) {
+				priv->CCK_index = i;
+				break;
+			}
+		}
+		priv->btxpower_trackingInit = true;
+		return;
+	}
+
+	tmpRegA = rtl92e_get_rf_reg(dev, RF90_PATH_A, 0x12, 0x078);
+	if (tmpRegA < 3 || tmpRegA > 13)
+		return;
+	if (tmpRegA >= 12)
+		tmpRegA = 12;
+	priv->ThermalMeter[0] = ThermalMeterVal;
+	priv->ThermalMeter[1] = ThermalMeterVal;
+
+	if (priv->ThermalMeter[0] >= (u8)tmpRegA) {
+		tmpOFDMindex = tmpCCK20Mindex = 6+(priv->ThermalMeter[0] -
+			      (u8)tmpRegA);
+		tmpCCK40Mindex = tmpCCK20Mindex - 6;
+		if (tmpOFDMindex >= OFDM_Table_Length)
+			tmpOFDMindex = OFDM_Table_Length-1;
+		if (tmpCCK20Mindex >= CCK_Table_length)
+			tmpCCK20Mindex = CCK_Table_length-1;
+		if (tmpCCK40Mindex >= CCK_Table_length)
+			tmpCCK40Mindex = CCK_Table_length-1;
+	} else {
+		tmpval = (u8)tmpRegA - priv->ThermalMeter[0];
+		if (tmpval >= 6) {
+			tmpOFDMindex = 0;
+			tmpCCK20Mindex = 0;
+		} else {
+			tmpOFDMindex = 6 - tmpval;
+			tmpCCK20Mindex = 6 - tmpval;
+		}
+		tmpCCK40Mindex = 0;
+	}
+	if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+		tmpCCKindex = tmpCCK40Mindex;
+	else
+		tmpCCKindex = tmpCCK20Mindex;
+
+	priv->Record_CCK_20Mindex = tmpCCK20Mindex;
+	priv->Record_CCK_40Mindex = tmpCCK40Mindex;
+
+	if (priv->rtllib->current_network.channel == 14 &&
+	    !priv->bcck_in_ch14) {
+		priv->bcck_in_ch14 = true;
+		CCKSwingNeedUpdate = 1;
+	} else if (priv->rtllib->current_network.channel != 14 &&
+		   priv->bcck_in_ch14) {
+		priv->bcck_in_ch14 = false;
+		CCKSwingNeedUpdate = 1;
+	}
+
+	if (priv->CCK_index != tmpCCKindex) {
+		priv->CCK_index = tmpCCKindex;
+		CCKSwingNeedUpdate = 1;
+	}
+
+	if (CCKSwingNeedUpdate)
+		rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+	if (priv->OFDM_index[0] != tmpOFDMindex) {
+		priv->OFDM_index[0] = tmpOFDMindex;
+		rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance, bMaskDWord,
+				  OFDMSwingTable[priv->OFDM_index[0]]);
+	}
+	priv->txpower_count = 0;
+}
+
+void rtl92e_dm_txpower_tracking_wq(void *data)
+{
+	struct r8192_priv *priv = container_of_dwork_rsl(data,
+				  struct r8192_priv, txpower_tracking_wq);
+	struct net_device *dev = priv->rtllib->dev;
+
+	if (priv->IC_Cut >= IC_VersionCut_D)
+		_rtl92e_dm_tx_power_tracking_callback_tssi(dev);
+	else
+		_rtl92e_dm_tx_power_tracking_cb_thermal(dev);
+}
+
+static void _rtl92e_dm_initialize_tx_power_tracking_tssi(struct net_device *dev)
+{
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->btxpower_tracking = true;
+	priv->txpower_count       = 0;
+	priv->btxpower_trackingInit = false;
+
+}
+
+static void _rtl92e_dm_init_tx_power_tracking_thermal(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+
+	if (priv->rtllib->FwRWRF)
+		priv->btxpower_tracking = true;
+	else
+		priv->btxpower_tracking = false;
+	priv->txpower_count       = 0;
+	priv->btxpower_trackingInit = false;
+}
+
+void rtl92e_dm_init_txpower_tracking(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->IC_Cut >= IC_VersionCut_D)
+		_rtl92e_dm_initialize_tx_power_tracking_tssi(dev);
+	else
+		_rtl92e_dm_init_tx_power_tracking_thermal(dev);
+}
+
+static void _rtl92e_dm_check_tx_power_tracking_tssi(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u32 tx_power_track_counter;
+
+	if (rtl92e_readb(dev, 0x11e) == 1)
+		return;
+	if (!priv->btxpower_tracking)
+		return;
+	tx_power_track_counter++;
+
+
+	if (tx_power_track_counter >= 180) {
+		schedule_delayed_work(&priv->txpower_tracking_wq, 0);
+		tx_power_track_counter = 0;
+	}
+
+}
+
+static void _rtl92e_dm_check_tx_power_tracking_thermal(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u8	TM_Trigger;
+	u8		TxPowerCheckCnt = 0;
+
+	if (IS_HARDWARE_TYPE_8192SE(dev))
+		TxPowerCheckCnt = 5;
+	else
+		TxPowerCheckCnt = 2;
+	if (!priv->btxpower_tracking)
+		return;
+
+	if (priv->txpower_count  <= TxPowerCheckCnt) {
+		priv->txpower_count++;
+		return;
+	}
+
+	if (!TM_Trigger) {
+		rtl92e_set_rf_reg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4d);
+		rtl92e_set_rf_reg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4f);
+		rtl92e_set_rf_reg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4d);
+		rtl92e_set_rf_reg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4f);
+		TM_Trigger = 1;
+		return;
+	}
+	netdev_info(dev, "===============>Schedule TxPowerTrackingWorkItem\n");
+	schedule_delayed_work(&priv->txpower_tracking_wq, 0);
+	TM_Trigger = 0;
+
+}
+
+static void _rtl92e_dm_check_tx_power_tracking(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->IC_Cut >= IC_VersionCut_D)
+		_rtl92e_dm_check_tx_power_tracking_tssi(dev);
+	else
+		_rtl92e_dm_check_tx_power_tracking_thermal(dev);
+}
+
+static void _rtl92e_dm_cck_tx_power_adjust_tssi(struct net_device *dev,
+						bool bInCH14)
+{
+	u32 TempVal;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 attenuation = priv->CCKPresentAttentuation;
+
+	TempVal = 0;
+	if (!bInCH14) {
+		TempVal = (u32)(dm_cck_tx_bb_gain[attenuation][0] +
+			  (dm_cck_tx_bb_gain[attenuation][1] << 8));
+
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		TempVal = (u32)((dm_cck_tx_bb_gain[attenuation][2]) +
+			  (dm_cck_tx_bb_gain[attenuation][3] << 8) +
+			  (dm_cck_tx_bb_gain[attenuation][4] << 16)+
+			  (dm_cck_tx_bb_gain[attenuation][5] << 24));
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		TempVal = (u32)(dm_cck_tx_bb_gain[attenuation][6] +
+			  (dm_cck_tx_bb_gain[attenuation][7] << 8));
+
+		rtl92e_set_bb_reg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	} else {
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][0]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][1] << 8));
+
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][2]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][3] << 8) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][4] << 16)+
+			  (dm_cck_tx_bb_gain_ch14[attenuation][5] << 24));
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][6]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][7] << 8));
+
+		rtl92e_set_bb_reg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	}
+}
+
+static void _rtl92e_dm_cck_tx_power_adjust_thermal_meter(struct net_device *dev,
+							 bool bInCH14)
+{
+	u32 TempVal;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	TempVal = 0;
+	if (!bInCH14) {
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][0] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][1] << 8);
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][3] << 8) +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][4] << 16)+
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][5] << 24);
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][7] << 8);
+
+		rtl92e_set_bb_reg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	} else {
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][0] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][1] << 8);
+
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][2] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][3] << 8) +
+			  (CCKSwingTable_Ch14[priv->CCK_index][4] << 16)+
+			  (CCKSwingTable_Ch14[priv->CCK_index][5] << 24);
+		rtl92e_set_bb_reg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][6] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][7]<<8);
+
+		rtl92e_set_bb_reg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	}
+}
+
+void rtl92e_dm_cck_txpower_adjust(struct net_device *dev, bool binch14)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->IC_Cut >= IC_VersionCut_D)
+		_rtl92e_dm_cck_tx_power_adjust_tssi(dev, binch14);
+	else
+		_rtl92e_dm_cck_tx_power_adjust_thermal_meter(dev, binch14);
+}
+
+static void _rtl92e_dm_tx_power_reset_recovery(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	rtl92e_set_bb_reg(dev, rOFDM0_XATxIQImbalance, bMaskDWord,
+			  dm_tx_bb_gain[priv->rfa_txpowertrackingindex]);
+	rtl92e_dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+
+	rtl92e_set_bb_reg(dev, rOFDM0_XCTxIQImbalance, bMaskDWord,
+			  dm_tx_bb_gain[priv->rfc_txpowertrackingindex]);
+}
+
+void rtl92e_dm_restore_state(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32	reg_ratr = priv->rate_adaptive.last_ratr;
+	u32 ratr_value;
+
+	if (!priv->up) {
+		return;
+	}
+
+	if (priv->rate_adaptive.rate_adaptive_disabled)
+		return;
+	if (!(priv->rtllib->mode == WIRELESS_MODE_N_24G ||
+	      priv->rtllib->mode == WIRELESS_MODE_N_5G))
+		return;
+	ratr_value = reg_ratr;
+	if (priv->rf_type == RF_1T2R)
+		ratr_value &= ~(RATE_ALL_OFDM_2SS);
+	rtl92e_writel(dev, RATR0, ratr_value);
+	rtl92e_writeb(dev, UFWP, 1);
+	if (priv->btxpower_trackingInit && priv->btxpower_tracking)
+		_rtl92e_dm_tx_power_reset_recovery(dev);
+
+	_rtl92e_dm_bb_initialgain_restore(dev);
+
+}
+
+static void _rtl92e_dm_bb_initialgain_restore(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 bit_mask = 0x7f;
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		return;
+
+	rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+	rtl92e_set_bb_reg(dev, rOFDM0_XAAGCCore1, bit_mask,
+			  (u32)priv->initgain_backup.xaagccore1);
+	rtl92e_set_bb_reg(dev, rOFDM0_XBAGCCore1, bit_mask,
+			  (u32)priv->initgain_backup.xbagccore1);
+	rtl92e_set_bb_reg(dev, rOFDM0_XCAGCCore1, bit_mask,
+			  (u32)priv->initgain_backup.xcagccore1);
+	rtl92e_set_bb_reg(dev, rOFDM0_XDAGCCore1, bit_mask,
+			  (u32)priv->initgain_backup.xdagccore1);
+	bit_mask  = bMaskByte2;
+	rtl92e_set_bb_reg(dev, rCCK0_CCA, bit_mask,
+			  (u32)priv->initgain_backup.cca);
+	rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x1);
+
+}
+
+void rtl92e_dm_backup_state(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 bit_mask = bMaskByte0;
+
+	priv->bswitch_fsync  = false;
+	priv->bfsync_processing = false;
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		return;
+
+	rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+	priv->initgain_backup.xaagccore1 = rtl92e_get_bb_reg(dev, rOFDM0_XAAGCCore1, bit_mask);
+	priv->initgain_backup.xbagccore1 = rtl92e_get_bb_reg(dev, rOFDM0_XBAGCCore1, bit_mask);
+	priv->initgain_backup.xcagccore1 = rtl92e_get_bb_reg(dev, rOFDM0_XCAGCCore1, bit_mask);
+	priv->initgain_backup.xdagccore1 = rtl92e_get_bb_reg(dev, rOFDM0_XDAGCCore1, bit_mask);
+	bit_mask  = bMaskByte2;
+	priv->initgain_backup.cca = (u8)rtl92e_get_bb_reg(dev, rCCK0_CCA, bit_mask);
+}
+
+static void _rtl92e_dm_dig_init(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	dm_digtable.dig_enable_flag	= true;
+
+	dm_digtable.dig_algorithm = DIG_ALGO_BY_RSSI;
+
+	dm_digtable.dig_algorithm_switch = 0;
+
+	dm_digtable.dig_state		= DM_STA_DIG_MAX;
+	dm_digtable.dig_highpwr_state	= DM_STA_DIG_MAX;
+	dm_digtable.CurSTAConnectState = DIG_STA_DISCONNECT;
+	dm_digtable.PreSTAConnectState = DIG_STA_DISCONNECT;
+
+	dm_digtable.rssi_low_thresh	= DM_DIG_THRESH_LOW;
+	dm_digtable.rssi_high_thresh	= DM_DIG_THRESH_HIGH;
+
+	dm_digtable.rssi_high_power_lowthresh = DM_DIG_HIGH_PWR_THRESH_LOW;
+	dm_digtable.rssi_high_power_highthresh = DM_DIG_HIGH_PWR_THRESH_HIGH;
+
+	dm_digtable.rssi_val = 50;
+	dm_digtable.backoff_val = DM_DIG_BACKOFF;
+	dm_digtable.rx_gain_range_max = DM_DIG_MAX;
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		dm_digtable.rx_gain_range_min = DM_DIG_MIN_Netcore;
+	else
+		dm_digtable.rx_gain_range_min = DM_DIG_MIN;
+}
+
+static void _rtl92e_dm_ctrl_initgain_byrssi(struct net_device *dev)
+{
+
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
+		_rtl92e_dm_ctrl_initgain_byrssi_false_alarm(dev);
+	else if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		_rtl92e_dm_ctrl_initgain_byrssi_driver(dev);
+	else
+		return;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_CtrlInitGainBeforeConnectByRssiAndFalseAlarm()
+ *
+ * Overview:	Driver monitor RSSI and False Alarm to change initial gain.
+			Only change initial gain during link in progress.
+ *
+ * Input:		IN	PADAPTER	pAdapter
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	03/04/2009	hpfan	Create Version 0.
+ *
+ ******************************************************************************/
+
+static void _rtl92e_dm_ctrl_initgain_byrssi_driver(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 i;
+	static u8	fw_dig;
+
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	if (dm_digtable.dig_algorithm_switch)
+		fw_dig = 0;
+	if (fw_dig <= 3) {
+		for (i = 0; i < 3; i++)
+			rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+		fw_dig++;
+		dm_digtable.dig_state = DM_STA_DIG_OFF;
+	}
+
+	if (priv->rtllib->state == RTLLIB_LINKED)
+		dm_digtable.CurSTAConnectState = DIG_STA_CONNECT;
+	else
+		dm_digtable.CurSTAConnectState = DIG_STA_DISCONNECT;
+
+
+	dm_digtable.rssi_val = priv->undecorated_smoothed_pwdb;
+	_rtl92e_dm_initial_gain(dev);
+	_rtl92e_dm_pd_th(dev);
+	_rtl92e_dm_cs_ratio(dev);
+	if (dm_digtable.dig_algorithm_switch)
+		dm_digtable.dig_algorithm_switch = 0;
+	dm_digtable.PreSTAConnectState = dm_digtable.CurSTAConnectState;
+
+}
+
+static void _rtl92e_dm_ctrl_initgain_byrssi_false_alarm(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u32 reset_cnt;
+	u8 i;
+
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		dm_digtable.dig_state = DM_STA_DIG_MAX;
+		for (i = 0; i < 3; i++)
+			rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x1);
+		dm_digtable.dig_algorithm_switch = 0;
+	}
+
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		return;
+
+	if ((priv->undecorated_smoothed_pwdb > dm_digtable.rssi_low_thresh) &&
+		(priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_thresh))
+		return;
+	if (priv->undecorated_smoothed_pwdb <= dm_digtable.rssi_low_thresh) {
+		if (dm_digtable.dig_state == DM_STA_DIG_OFF &&
+			(priv->reset_count == reset_cnt))
+			return;
+		reset_cnt = priv->reset_count;
+
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_MAX;
+		dm_digtable.dig_state = DM_STA_DIG_OFF;
+
+		rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x8);
+
+		rtl92e_writeb(dev, rOFDM0_XAAGCCore1, 0x17);
+		rtl92e_writeb(dev, rOFDM0_XBAGCCore1, 0x17);
+		rtl92e_writeb(dev, rOFDM0_XCAGCCore1, 0x17);
+		rtl92e_writeb(dev, rOFDM0_XDAGCCore1, 0x17);
+
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+			rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x00);
+		else
+			rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x42);
+
+		rtl92e_writeb(dev, 0xa0a, 0x08);
+
+		return;
+	}
+
+	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
+		u8 reset_flag = 0;
+
+		if (dm_digtable.dig_state == DM_STA_DIG_ON &&
+		    (priv->reset_count == reset_cnt)) {
+			_rtl92e_dm_ctrl_initgain_byrssi_highpwr(dev);
+			return;
+		}
+		if (priv->reset_count != reset_cnt)
+			reset_flag = 1;
+
+		reset_cnt = priv->reset_count;
+
+		dm_digtable.dig_state = DM_STA_DIG_ON;
+
+		if (reset_flag == 1) {
+			rtl92e_writeb(dev, rOFDM0_XAAGCCore1, 0x2c);
+			rtl92e_writeb(dev, rOFDM0_XBAGCCore1, 0x2c);
+			rtl92e_writeb(dev, rOFDM0_XCAGCCore1, 0x2c);
+			rtl92e_writeb(dev, rOFDM0_XDAGCCore1, 0x2c);
+		} else {
+			rtl92e_writeb(dev, rOFDM0_XAAGCCore1, 0x20);
+			rtl92e_writeb(dev, rOFDM0_XBAGCCore1, 0x20);
+			rtl92e_writeb(dev, rOFDM0_XCAGCCore1, 0x20);
+			rtl92e_writeb(dev, rOFDM0_XDAGCCore1, 0x20);
+		}
+
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+			rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x20);
+		else
+			rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x44);
+
+		rtl92e_writeb(dev, 0xa0a, 0xcd);
+
+		rtl92e_set_bb_reg(dev, UFWP, bMaskByte1, 0x1);
+	}
+	_rtl92e_dm_ctrl_initgain_byrssi_highpwr(dev);
+}
+
+
+static void _rtl92e_dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u32 reset_cnt_highpwr;
+
+	if ((priv->undecorated_smoothed_pwdb >
+	     dm_digtable.rssi_high_power_lowthresh) &&
+	    (priv->undecorated_smoothed_pwdb <
+	     dm_digtable.rssi_high_power_highthresh))
+		return;
+
+	if (priv->undecorated_smoothed_pwdb >=
+	    dm_digtable.rssi_high_power_highthresh) {
+		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_ON &&
+			(priv->reset_count == reset_cnt_highpwr))
+			return;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_ON;
+
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+			rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x10);
+		else
+			rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x43);
+	} else {
+		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_OFF &&
+			(priv->reset_count == reset_cnt_highpwr))
+			return;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_OFF;
+
+		if ((priv->undecorated_smoothed_pwdb <
+		     dm_digtable.rssi_high_power_lowthresh) &&
+		    (priv->undecorated_smoothed_pwdb >=
+		    dm_digtable.rssi_high_thresh)) {
+			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+				rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x20);
+			else
+				rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x44);
+		}
+	}
+	reset_cnt_highpwr = priv->reset_count;
+}
+
+static void _rtl92e_dm_initial_gain(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 initial_gain = 0;
+	static u8 initialized, force_write;
+	static u32 reset_cnt;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (rtllib_act_scanning(priv->rtllib, true)) {
+		force_write = 1;
+		return;
+	}
+
+	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
+		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
+			long gain_range = dm_digtable.rssi_val + 10 -
+					  dm_digtable.backoff_val;
+			gain_range = clamp_t(long, gain_range,
+					     dm_digtable.rx_gain_range_min,
+					     dm_digtable.rx_gain_range_max);
+			dm_digtable.cur_ig_value = gain_range;
+		} else {
+			if (dm_digtable.cur_ig_value == 0)
+				dm_digtable.cur_ig_value = priv->DefaultInitialGain[0];
+			else
+				dm_digtable.cur_ig_value = dm_digtable.pre_ig_value;
+		}
+	} else {
+		dm_digtable.cur_ig_value = priv->DefaultInitialGain[0];
+		dm_digtable.pre_ig_value = 0;
+	}
+
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+	if (dm_digtable.pre_ig_value != rtl92e_readb(dev, rOFDM0_XAAGCCore1))
+		force_write = 1;
+
+	if ((dm_digtable.pre_ig_value != dm_digtable.cur_ig_value)
+	    || !initialized || force_write) {
+		initial_gain = dm_digtable.cur_ig_value;
+		rtl92e_writeb(dev, rOFDM0_XAAGCCore1, initial_gain);
+		rtl92e_writeb(dev, rOFDM0_XBAGCCore1, initial_gain);
+		rtl92e_writeb(dev, rOFDM0_XCAGCCore1, initial_gain);
+		rtl92e_writeb(dev, rOFDM0_XDAGCCore1, initial_gain);
+		dm_digtable.pre_ig_value = dm_digtable.cur_ig_value;
+		initialized = 1;
+		force_write = 0;
+	}
+}
+
+static void _rtl92e_dm_pd_th(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u8 initialized, force_write;
+	static u32 reset_cnt;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
+		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
+			if (dm_digtable.rssi_val >=
+			    dm_digtable.rssi_high_power_highthresh)
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_HIGH_POWER;
+			else if (dm_digtable.rssi_val <=
+				 dm_digtable.rssi_low_thresh)
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_LOW_POWER;
+			else if ((dm_digtable.rssi_val >=
+				  dm_digtable.rssi_high_thresh) &&
+				 (dm_digtable.rssi_val <
+				  dm_digtable.rssi_high_power_lowthresh))
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_NORMAL_POWER;
+			else
+				dm_digtable.curpd_thstate =
+						dm_digtable.prepd_thstate;
+		} else {
+			dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
+		}
+	} else {
+		dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
+	}
+
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+	if ((dm_digtable.prepd_thstate != dm_digtable.curpd_thstate) ||
+	    (initialized <= 3) || force_write) {
+		if (dm_digtable.curpd_thstate == DIG_PD_AT_LOW_POWER) {
+			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+				rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x00);
+			else
+				rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x42);
+		} else if (dm_digtable.curpd_thstate ==
+			   DIG_PD_AT_NORMAL_POWER) {
+			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+				rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x20);
+			else
+				rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x44);
+		} else if (dm_digtable.curpd_thstate == DIG_PD_AT_HIGH_POWER) {
+			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
+				rtl92e_writeb(dev, (rOFDM0_XATxAFE+3), 0x10);
+			else
+				rtl92e_writeb(dev, rOFDM0_RxDetector1, 0x43);
+		}
+		dm_digtable.prepd_thstate = dm_digtable.curpd_thstate;
+		if (initialized <= 3)
+			initialized++;
+		force_write = 0;
+	}
+}
+
+static void _rtl92e_dm_cs_ratio(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u8 initialized, force_write;
+	static u32 reset_cnt;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
+		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
+			if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
+				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+			else if (dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh)
+				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_HIGHER;
+			else
+				dm_digtable.curcs_ratio_state = dm_digtable.precs_ratio_state;
+		} else {
+			dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+		}
+	} else {
+		dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+	}
+
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+
+	if ((dm_digtable.precs_ratio_state != dm_digtable.curcs_ratio_state) ||
+	    !initialized || force_write) {
+		if (dm_digtable.curcs_ratio_state == DIG_CS_RATIO_LOWER)
+			rtl92e_writeb(dev, 0xa0a, 0x08);
+		else if (dm_digtable.curcs_ratio_state == DIG_CS_RATIO_HIGHER)
+			rtl92e_writeb(dev, 0xa0a, 0xcd);
+		dm_digtable.precs_ratio_state = dm_digtable.curcs_ratio_state;
+		initialized = 1;
+		force_write = 0;
+	}
+}
+
+void rtl92e_dm_init_edca_turbo(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->bcurrent_turbo_EDCA = false;
+	priv->rtllib->bis_any_nonbepkts = false;
+	priv->bis_cur_rdlstate = false;
+}
+
+static void _rtl92e_dm_check_edca_turbo(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_hi_throughput *pHTInfo = priv->rtllib->pHTInfo;
+
+	static unsigned long lastTxOkCnt;
+	static unsigned long lastRxOkCnt;
+	unsigned long curTxOkCnt = 0;
+	unsigned long curRxOkCnt = 0;
+
+	if (priv->rtllib->iw_mode == IW_MODE_ADHOC)
+		goto dm_CheckEdcaTurbo_EXIT;
+	if (priv->rtllib->state != RTLLIB_LINKED)
+		goto dm_CheckEdcaTurbo_EXIT;
+	if (priv->rtllib->pHTInfo->iot_action & HT_IOT_ACT_DISABLE_EDCA_TURBO)
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	if (!priv->rtllib->bis_any_nonbepkts) {
+		curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
+		curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
+		if (pHTInfo->iot_action & HT_IOT_ACT_EDCA_BIAS_ON_RX) {
+			if (curTxOkCnt > 4*curRxOkCnt) {
+				if (priv->bis_cur_rdlstate ||
+				    !priv->bcurrent_turbo_EDCA) {
+					rtl92e_writel(dev, EDCAPARA_BE,
+						      edca_setting_UL[pHTInfo->IOTPeer]);
+					priv->bis_cur_rdlstate = false;
+				}
+			} else {
+				if (!priv->bis_cur_rdlstate ||
+				    !priv->bcurrent_turbo_EDCA) {
+					if (priv->rtllib->mode == WIRELESS_MODE_G)
+						rtl92e_writel(dev, EDCAPARA_BE,
+							      edca_setting_DL_GMode[pHTInfo->IOTPeer]);
+					else
+						rtl92e_writel(dev, EDCAPARA_BE,
+							      edca_setting_DL[pHTInfo->IOTPeer]);
+					priv->bis_cur_rdlstate = true;
+				}
+			}
+			priv->bcurrent_turbo_EDCA = true;
+		} else {
+			if (curRxOkCnt > 4*curTxOkCnt) {
+				if (!priv->bis_cur_rdlstate ||
+				    !priv->bcurrent_turbo_EDCA) {
+					if (priv->rtllib->mode == WIRELESS_MODE_G)
+						rtl92e_writel(dev, EDCAPARA_BE,
+							      edca_setting_DL_GMode[pHTInfo->IOTPeer]);
+					else
+						rtl92e_writel(dev, EDCAPARA_BE,
+							      edca_setting_DL[pHTInfo->IOTPeer]);
+					priv->bis_cur_rdlstate = true;
+				}
+			} else {
+				if (priv->bis_cur_rdlstate ||
+				    !priv->bcurrent_turbo_EDCA) {
+					rtl92e_writel(dev, EDCAPARA_BE,
+						      edca_setting_UL[pHTInfo->IOTPeer]);
+					priv->bis_cur_rdlstate = false;
+				}
+
+			}
+
+			priv->bcurrent_turbo_EDCA = true;
+		}
+	} else {
+		if (priv->bcurrent_turbo_EDCA) {
+			u8 tmp = AC0_BE;
+
+			priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
+			priv->bcurrent_turbo_EDCA = false;
+		}
+	}
+
+
+dm_CheckEdcaTurbo_EXIT:
+	priv->rtllib->bis_any_nonbepkts = false;
+	lastTxOkCnt = priv->stats.txbytesunicast;
+	lastRxOkCnt = priv->stats.rxbytesunicast;
+}
+
+static void _rtl92e_dm_init_cts_to_self(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv((struct net_device *)dev);
+
+	priv->rtllib->bCTSToSelfEnable = true;
+}
+
+static void _rtl92e_dm_cts_to_self(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv((struct net_device *)dev);
+	struct rt_hi_throughput *pHTInfo = priv->rtllib->pHTInfo;
+	static unsigned long lastTxOkCnt;
+	static unsigned long lastRxOkCnt;
+	unsigned long curTxOkCnt = 0;
+	unsigned long curRxOkCnt = 0;
+
+	if (!priv->rtllib->bCTSToSelfEnable) {
+		pHTInfo->iot_action &= ~HT_IOT_ACT_FORCED_CTS2SELF;
+		return;
+	}
+	if (pHTInfo->IOTPeer == HT_IOT_PEER_BROADCOM) {
+		curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
+		curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
+		if (curRxOkCnt > 4*curTxOkCnt)
+			pHTInfo->iot_action &= ~HT_IOT_ACT_FORCED_CTS2SELF;
+		else
+			pHTInfo->iot_action |= HT_IOT_ACT_FORCED_CTS2SELF;
+
+		lastTxOkCnt = priv->stats.txbytesunicast;
+		lastRxOkCnt = priv->stats.rxbytesunicast;
+	}
+}
+
+
+static void _rtl92e_dm_init_wa_broadcom_iot(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv((struct net_device *)dev);
+	struct rt_hi_throughput *pHTInfo = priv->rtllib->pHTInfo;
+
+	pHTInfo->bWAIotBroadcom = false;
+	pHTInfo->WAIotTH = WAIotTHVal;
+}
+
+static void _rtl92e_dm_check_rf_ctrl_gpio(void *data)
+{
+	struct r8192_priv *priv = container_of_dwork_rsl(data,
+				  struct r8192_priv, gpio_change_rf_wq);
+	struct net_device *dev = priv->rtllib->dev;
+	u8 tmp1byte;
+	enum rt_rf_power_state rf_power_state_to_set;
+	bool bActuallySet = false;
+
+	bActuallySet = false;
+
+	if ((priv->up_first_time == 1) || (priv->being_init_adapter))
+		return;
+
+	if (priv->bfirst_after_down) {
+		priv->bfirst_after_down = true;
+		return;
+	}
+
+	tmp1byte = rtl92e_readb(dev, GPI);
+
+	rf_power_state_to_set = (tmp1byte&BIT1) ?  rf_on : rf_off;
+
+	if (priv->hw_radio_off && (rf_power_state_to_set == rf_on)) {
+		netdev_info(dev, "gpiochangeRF  - HW Radio ON\n");
+		priv->hw_radio_off = false;
+		bActuallySet = true;
+	} else if (!priv->hw_radio_off && (rf_power_state_to_set == rf_off)) {
+		netdev_info(dev, "gpiochangeRF  - HW Radio OFF\n");
+		priv->hw_radio_off = true;
+		bActuallySet = true;
+	}
+
+	if (bActuallySet) {
+		mdelay(1000);
+		priv->bHwRfOffAction = 1;
+		rtl92e_set_rf_state(dev, rf_power_state_to_set, RF_CHANGE_BY_HW);
+	}
+}
+
+void rtl92e_dm_rf_pathcheck_wq(void *data)
+{
+	struct r8192_priv *priv = container_of_dwork_rsl(data,
+				  struct r8192_priv,
+				  rfpath_check_wq);
+	struct net_device *dev = priv->rtllib->dev;
+	u8 rfpath, i;
+
+	rfpath = rtl92e_readb(dev, 0xc04);
+
+	for (i = 0; i < RF90_PATH_MAX; i++) {
+		if (rfpath & (0x01<<i))
+			priv->brfpath_rxenable[i] = true;
+		else
+			priv->brfpath_rxenable[i] = false;
+	}
+	if (!DM_RxPathSelTable.Enable)
+		return;
+
+	_rtl92e_dm_rx_path_sel_byrssi(dev);
+}
+
+static void _rtl92e_dm_init_rx_path_selection(struct net_device *dev)
+{
+	u8 i;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	DM_RxPathSelTable.Enable = 1;
+	DM_RxPathSelTable.SS_TH_low = RxPathSelection_SS_TH_low;
+	DM_RxPathSelTable.diff_TH = RxPathSelection_diff_TH;
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		DM_RxPathSelTable.cck_method = CCK_Rx_Version_2;
+	else
+		DM_RxPathSelTable.cck_method = CCK_Rx_Version_1;
+	DM_RxPathSelTable.disabledRF = 0;
+	for (i = 0; i < 4; i++) {
+		DM_RxPathSelTable.rf_rssi[i] = 50;
+		DM_RxPathSelTable.cck_pwdb_sta[i] = -64;
+		DM_RxPathSelTable.rf_enable_rssi_th[i] = 100;
+	}
+}
+
+#define PWDB_IN_RANGE	((cur_cck_pwdb < tmp_cck_max_pwdb) &&	\
+			(cur_cck_pwdb > tmp_cck_sec_pwdb))
+
+static void _rtl92e_dm_rx_path_sel_byrssi(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 i, max_rssi_index = 0, min_rssi_index = 0;
+	u8 sec_rssi_index = 0, rf_num = 0;
+	u8 tmp_max_rssi = 0, tmp_min_rssi = 0, tmp_sec_rssi = 0;
+	u8 cck_default_Rx = 0x2;
+	u8 cck_optional_Rx = 0x3;
+	long tmp_cck_max_pwdb = 0, tmp_cck_min_pwdb = 0, tmp_cck_sec_pwdb = 0;
+	u8 cck_rx_ver2_max_index = 0;
+	u8 cck_rx_ver2_sec_index = 0;
+	u8 cur_rf_rssi;
+	long cur_cck_pwdb;
+	static u8 disabled_rf_cnt, cck_Rx_Path_initialized;
+	u8 update_cck_rx_path;
+
+	if (priv->rf_type != RF_2T4R)
+		return;
+
+	if (!cck_Rx_Path_initialized) {
+		DM_RxPathSelTable.cck_Rx_path = (rtl92e_readb(dev, 0xa07)&0xf);
+		cck_Rx_Path_initialized = 1;
+	}
+
+	DM_RxPathSelTable.disabledRF = 0xf;
+	DM_RxPathSelTable.disabledRF &= ~(rtl92e_readb(dev, 0xc04));
+
+	if (priv->rtllib->mode == WIRELESS_MODE_B)
+		DM_RxPathSelTable.cck_method = CCK_Rx_Version_2;
+
+	for (i = 0; i < RF90_PATH_MAX; i++) {
+		DM_RxPathSelTable.rf_rssi[i] = priv->stats.rx_rssi_percentage[i];
+
+		if (priv->brfpath_rxenable[i]) {
+			rf_num++;
+			cur_rf_rssi = DM_RxPathSelTable.rf_rssi[i];
+
+			if (rf_num == 1) {
+				max_rssi_index = min_rssi_index = sec_rssi_index = i;
+				tmp_max_rssi = tmp_min_rssi = tmp_sec_rssi = cur_rf_rssi;
+			} else if (rf_num == 2) {
+				if (cur_rf_rssi >= tmp_max_rssi) {
+					tmp_max_rssi = cur_rf_rssi;
+					max_rssi_index = i;
+				} else {
+					tmp_sec_rssi = tmp_min_rssi = cur_rf_rssi;
+					sec_rssi_index = min_rssi_index = i;
+				}
+			} else {
+				if (cur_rf_rssi > tmp_max_rssi) {
+					tmp_sec_rssi = tmp_max_rssi;
+					sec_rssi_index = max_rssi_index;
+					tmp_max_rssi = cur_rf_rssi;
+					max_rssi_index = i;
+				} else if (cur_rf_rssi == tmp_max_rssi) {
+					tmp_sec_rssi = cur_rf_rssi;
+					sec_rssi_index = i;
+				} else if ((cur_rf_rssi < tmp_max_rssi) &&
+					   (cur_rf_rssi > tmp_sec_rssi)) {
+					tmp_sec_rssi = cur_rf_rssi;
+					sec_rssi_index = i;
+				} else if (cur_rf_rssi == tmp_sec_rssi) {
+					if (tmp_sec_rssi == tmp_min_rssi) {
+						tmp_sec_rssi = cur_rf_rssi;
+						sec_rssi_index = i;
+					}
+				} else if ((cur_rf_rssi < tmp_sec_rssi) &&
+					   (cur_rf_rssi > tmp_min_rssi)) {
+					;
+				} else if (cur_rf_rssi == tmp_min_rssi) {
+					if (tmp_sec_rssi == tmp_min_rssi) {
+						tmp_min_rssi = cur_rf_rssi;
+						min_rssi_index = i;
+					}
+				} else if (cur_rf_rssi < tmp_min_rssi) {
+					tmp_min_rssi = cur_rf_rssi;
+					min_rssi_index = i;
+				}
+			}
+		}
+	}
+
+	rf_num = 0;
+	if (DM_RxPathSelTable.cck_method == CCK_Rx_Version_2) {
+		for (i = 0; i < RF90_PATH_MAX; i++) {
+			if (priv->brfpath_rxenable[i]) {
+				rf_num++;
+				cur_cck_pwdb =
+					 DM_RxPathSelTable.cck_pwdb_sta[i];
+
+				if (rf_num == 1) {
+					cck_rx_ver2_max_index = i;
+					cck_rx_ver2_sec_index = i;
+					tmp_cck_max_pwdb = cur_cck_pwdb;
+					tmp_cck_min_pwdb = cur_cck_pwdb;
+					tmp_cck_sec_pwdb = cur_cck_pwdb;
+				} else if (rf_num == 2) {
+					if (cur_cck_pwdb >= tmp_cck_max_pwdb) {
+						tmp_cck_max_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_max_index = i;
+					} else {
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						tmp_cck_min_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+					}
+				} else {
+					if (cur_cck_pwdb > tmp_cck_max_pwdb) {
+						tmp_cck_sec_pwdb =
+							 tmp_cck_max_pwdb;
+						cck_rx_ver2_sec_index =
+							 cck_rx_ver2_max_index;
+						tmp_cck_max_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_max_index = i;
+					} else if (cur_cck_pwdb ==
+						   tmp_cck_max_pwdb) {
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+					} else if (PWDB_IN_RANGE) {
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+					} else if (cur_cck_pwdb ==
+						   tmp_cck_sec_pwdb) {
+						if (tmp_cck_sec_pwdb ==
+						    tmp_cck_min_pwdb) {
+							tmp_cck_sec_pwdb =
+								 cur_cck_pwdb;
+							cck_rx_ver2_sec_index =
+								 i;
+						}
+					} else if ((cur_cck_pwdb < tmp_cck_sec_pwdb) &&
+						   (cur_cck_pwdb > tmp_cck_min_pwdb)) {
+						;
+					} else if (cur_cck_pwdb == tmp_cck_min_pwdb) {
+						if (tmp_cck_sec_pwdb == tmp_cck_min_pwdb)
+							tmp_cck_min_pwdb = cur_cck_pwdb;
+					} else if (cur_cck_pwdb < tmp_cck_min_pwdb) {
+						tmp_cck_min_pwdb = cur_cck_pwdb;
+					}
+				}
+
+			}
+		}
+	}
+
+	update_cck_rx_path = 0;
+	if (DM_RxPathSelTable.cck_method == CCK_Rx_Version_2) {
+		cck_default_Rx = cck_rx_ver2_max_index;
+		cck_optional_Rx = cck_rx_ver2_sec_index;
+		if (tmp_cck_max_pwdb != -64)
+			update_cck_rx_path = 1;
+	}
+
+	if (tmp_min_rssi < DM_RxPathSelTable.SS_TH_low && disabled_rf_cnt < 2) {
+		if ((tmp_max_rssi - tmp_min_rssi) >=
+		     DM_RxPathSelTable.diff_TH) {
+			DM_RxPathSelTable.rf_enable_rssi_th[min_rssi_index] =
+				 tmp_max_rssi+5;
+			rtl92e_set_bb_reg(dev, rOFDM0_TRxPathEnable,
+					  0x1<<min_rssi_index, 0x0);
+			rtl92e_set_bb_reg(dev, rOFDM1_TRxPathEnable,
+					  0x1<<min_rssi_index, 0x0);
+			disabled_rf_cnt++;
+		}
+		if (DM_RxPathSelTable.cck_method == CCK_Rx_Version_1) {
+			cck_default_Rx = max_rssi_index;
+			cck_optional_Rx = sec_rssi_index;
+			if (tmp_max_rssi)
+				update_cck_rx_path = 1;
+		}
+	}
+
+	if (update_cck_rx_path) {
+		DM_RxPathSelTable.cck_Rx_path = (cck_default_Rx<<2) |
+						(cck_optional_Rx);
+		rtl92e_set_bb_reg(dev, rCCK0_AFESetting, 0x0f000000,
+				  DM_RxPathSelTable.cck_Rx_path);
+	}
+
+	if (DM_RxPathSelTable.disabledRF) {
+		for (i = 0; i < 4; i++) {
+			if ((DM_RxPathSelTable.disabledRF>>i) & 0x1) {
+				if (tmp_max_rssi >=
+				    DM_RxPathSelTable.rf_enable_rssi_th[i]) {
+					rtl92e_set_bb_reg(dev,
+							  rOFDM0_TRxPathEnable,
+							  0x1 << i, 0x1);
+					rtl92e_set_bb_reg(dev,
+							  rOFDM1_TRxPathEnable,
+							  0x1 << i, 0x1);
+					DM_RxPathSelTable.rf_enable_rssi_th[i]
+						 = 100;
+					disabled_rf_cnt--;
+				}
+			}
+		}
+	}
+}
+
+static void _rtl92e_dm_check_rx_path_selection(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	schedule_delayed_work(&priv->rfpath_check_wq, 0);
+}
+
+
+static void _rtl92e_dm_init_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->rtllib->fsync_time_interval = 500;
+	priv->rtllib->fsync_rate_bitmap = 0x0f000800;
+	priv->rtllib->fsync_rssi_threshold = 30;
+	priv->rtllib->bfsync_enable = false;
+	priv->rtllib->fsync_multiple_timeinterval = 3;
+	priv->rtllib->fsync_firstdiff_ratethreshold = 100;
+	priv->rtllib->fsync_seconddiff_ratethreshold = 200;
+	priv->rtllib->fsync_state = Default_Fsync;
+	priv->framesyncMonitor = 1;
+
+	timer_setup(&priv->fsync_timer, _rtl92e_dm_fsync_timer_callback, 0);
+}
+
+
+static void _rtl92e_dm_deinit_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	del_timer_sync(&priv->fsync_timer);
+}
+
+static void _rtl92e_dm_fsync_timer_callback(struct timer_list *t)
+{
+	struct r8192_priv *priv = from_timer(priv, t, fsync_timer);
+	struct net_device *dev = priv->rtllib->dev;
+	u32 rate_index, rate_count = 0, rate_count_diff = 0;
+	bool		bSwitchFromCountDiff = false;
+	bool		bDoubleTimeInterval = false;
+
+	if (priv->rtllib->state == RTLLIB_LINKED &&
+	    priv->rtllib->bfsync_enable &&
+	    (priv->rtllib->pHTInfo->iot_action & HT_IOT_ACT_CDD_FSYNC)) {
+		u32 rate_bitmap;
+
+		for (rate_index = 0; rate_index <= 27; rate_index++) {
+			rate_bitmap  = 1 << rate_index;
+			if (priv->rtllib->fsync_rate_bitmap &  rate_bitmap)
+				rate_count +=
+				   priv->stats.received_rate_histogram[1]
+				   [rate_index];
+		}
+
+		if (rate_count < priv->rate_record)
+			rate_count_diff = 0xffffffff - rate_count +
+					  priv->rate_record;
+		else
+			rate_count_diff = rate_count - priv->rate_record;
+		if (rate_count_diff < priv->rateCountDiffRecord) {
+
+			u32 DiffNum = priv->rateCountDiffRecord -
+				      rate_count_diff;
+			if (DiffNum >=
+			    priv->rtllib->fsync_seconddiff_ratethreshold)
+				priv->ContinueDiffCount++;
+			else
+				priv->ContinueDiffCount = 0;
+
+			if (priv->ContinueDiffCount >= 2) {
+				bSwitchFromCountDiff = true;
+				priv->ContinueDiffCount = 0;
+			}
+		} else {
+			priv->ContinueDiffCount = 0;
+		}
+
+		if (rate_count_diff <=
+		    priv->rtllib->fsync_firstdiff_ratethreshold) {
+			bSwitchFromCountDiff = true;
+			priv->ContinueDiffCount = 0;
+		}
+		priv->rate_record = rate_count;
+		priv->rateCountDiffRecord = rate_count_diff;
+		if (priv->undecorated_smoothed_pwdb >
+		    priv->rtllib->fsync_rssi_threshold &&
+		    bSwitchFromCountDiff) {
+			bDoubleTimeInterval = true;
+			priv->bswitch_fsync = !priv->bswitch_fsync;
+			if (priv->bswitch_fsync) {
+				rtl92e_writeb(dev, 0xC36, 0x1c);
+				rtl92e_writeb(dev, 0xC3e, 0x90);
+			} else {
+				rtl92e_writeb(dev, 0xC36, 0x5c);
+				rtl92e_writeb(dev, 0xC3e, 0x96);
+			}
+		} else if (priv->undecorated_smoothed_pwdb <=
+			   priv->rtllib->fsync_rssi_threshold) {
+			if (priv->bswitch_fsync) {
+				priv->bswitch_fsync  = false;
+				rtl92e_writeb(dev, 0xC36, 0x5c);
+				rtl92e_writeb(dev, 0xC3e, 0x96);
+			}
+		}
+		if (bDoubleTimeInterval) {
+			if (timer_pending(&priv->fsync_timer))
+				del_timer_sync(&priv->fsync_timer);
+			priv->fsync_timer.expires = jiffies +
+				 msecs_to_jiffies(priv->rtllib->fsync_time_interval *
+				 priv->rtllib->fsync_multiple_timeinterval);
+			add_timer(&priv->fsync_timer);
+		} else {
+			if (timer_pending(&priv->fsync_timer))
+				del_timer_sync(&priv->fsync_timer);
+			priv->fsync_timer.expires = jiffies +
+				 msecs_to_jiffies(priv->rtllib->fsync_time_interval);
+			add_timer(&priv->fsync_timer);
+		}
+	} else {
+		if (priv->bswitch_fsync) {
+			priv->bswitch_fsync  = false;
+			rtl92e_writeb(dev, 0xC36, 0x5c);
+			rtl92e_writeb(dev, 0xC3e, 0x96);
+		}
+		priv->ContinueDiffCount = 0;
+		rtl92e_writel(dev, rOFDM0_RxDetector2, 0x465c52cd);
+	}
+}
+
+static void _rtl92e_dm_start_hw_fsync(struct net_device *dev)
+{
+	u8 rf_timing = 0x77;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	rtl92e_writel(dev, rOFDM0_RxDetector2, 0x465c12cf);
+	priv->rtllib->SetHwRegHandler(dev, HW_VAR_RF_TIMING,
+				      (u8 *)(&rf_timing));
+	rtl92e_writeb(dev, 0xc3b, 0x41);
+}
+
+static void _rtl92e_dm_end_hw_fsync(struct net_device *dev)
+{
+	u8 rf_timing = 0xaa;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	rtl92e_writel(dev, rOFDM0_RxDetector2, 0x465c52cd);
+	priv->rtllib->SetHwRegHandler(dev, HW_VAR_RF_TIMING, (u8 *)
+				     (&rf_timing));
+	rtl92e_writeb(dev, 0xc3b, 0x49);
+}
+
+static void _rtl92e_dm_end_sw_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	del_timer_sync(&(priv->fsync_timer));
+
+	if (priv->bswitch_fsync) {
+		priv->bswitch_fsync  = false;
+
+		rtl92e_writeb(dev, 0xC36, 0x5c);
+
+		rtl92e_writeb(dev, 0xC3e, 0x96);
+	}
+
+	priv->ContinueDiffCount = 0;
+	rtl92e_writel(dev, rOFDM0_RxDetector2, 0x465c52cd);
+}
+
+static void _rtl92e_dm_start_sw_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32			rateIndex;
+	u32			rateBitmap;
+
+	priv->rate_record = 0;
+	priv->ContinueDiffCount = 0;
+	priv->rateCountDiffRecord = 0;
+	priv->bswitch_fsync  = false;
+
+	if (priv->rtllib->mode == WIRELESS_MODE_N_24G) {
+		priv->rtllib->fsync_firstdiff_ratethreshold = 600;
+		priv->rtllib->fsync_seconddiff_ratethreshold = 0xffff;
+	} else {
+		priv->rtllib->fsync_firstdiff_ratethreshold = 200;
+		priv->rtllib->fsync_seconddiff_ratethreshold = 200;
+	}
+	for (rateIndex = 0; rateIndex <= 27; rateIndex++) {
+		rateBitmap  = 1 << rateIndex;
+		if (priv->rtllib->fsync_rate_bitmap & rateBitmap)
+			priv->rate_record +=
+				 priv->stats.received_rate_histogram[1]
+				[rateIndex];
+	}
+	if (timer_pending(&priv->fsync_timer))
+		del_timer_sync(&priv->fsync_timer);
+	priv->fsync_timer.expires = jiffies +
+				    msecs_to_jiffies(priv->rtllib->fsync_time_interval);
+	add_timer(&priv->fsync_timer);
+
+	rtl92e_writel(dev, rOFDM0_RxDetector2, 0x465c12cd);
+
+}
+
+static void _rtl92e_dm_check_fsync(struct net_device *dev)
+{
+#define	RegC38_Default			0
+#define	RegC38_NonFsync_Other_AP	1
+#define	RegC38_Fsync_AP_BCM		2
+	struct r8192_priv *priv = rtllib_priv(dev);
+	static u8 reg_c38_State = RegC38_Default;
+	static u32 reset_cnt;
+
+	if (priv->rtllib->state == RTLLIB_LINKED &&
+	    priv->rtllib->pHTInfo->IOTPeer == HT_IOT_PEER_BROADCOM) {
+		if (priv->rtllib->bfsync_enable == 0) {
+			switch (priv->rtllib->fsync_state) {
+			case Default_Fsync:
+				_rtl92e_dm_start_hw_fsync(dev);
+				priv->rtllib->fsync_state = HW_Fsync;
+				break;
+			case SW_Fsync:
+				_rtl92e_dm_end_sw_fsync(dev);
+				_rtl92e_dm_start_hw_fsync(dev);
+				priv->rtllib->fsync_state = HW_Fsync;
+				break;
+			case HW_Fsync:
+			default:
+				break;
+			}
+		} else {
+			switch (priv->rtllib->fsync_state) {
+			case Default_Fsync:
+				_rtl92e_dm_start_sw_fsync(dev);
+				priv->rtllib->fsync_state = SW_Fsync;
+				break;
+			case HW_Fsync:
+				_rtl92e_dm_end_hw_fsync(dev);
+				_rtl92e_dm_start_sw_fsync(dev);
+				priv->rtllib->fsync_state = SW_Fsync;
+				break;
+			case SW_Fsync:
+			default:
+				break;
+
+			}
+		}
+		if (priv->framesyncMonitor) {
+			if (reg_c38_State != RegC38_Fsync_AP_BCM) {
+				rtl92e_writeb(dev, rOFDM0_RxDetector3, 0x95);
+
+				reg_c38_State = RegC38_Fsync_AP_BCM;
+			}
+		}
+	} else {
+		switch (priv->rtllib->fsync_state) {
+		case HW_Fsync:
+			_rtl92e_dm_end_hw_fsync(dev);
+			priv->rtllib->fsync_state = Default_Fsync;
+			break;
+		case SW_Fsync:
+			_rtl92e_dm_end_sw_fsync(dev);
+			priv->rtllib->fsync_state = Default_Fsync;
+			break;
+		case Default_Fsync:
+		default:
+			break;
+		}
+
+		if (priv->framesyncMonitor) {
+			if (priv->rtllib->state == RTLLIB_LINKED) {
+				if (priv->undecorated_smoothed_pwdb <=
+				    RegC38_TH) {
+					if (reg_c38_State !=
+					    RegC38_NonFsync_Other_AP) {
+						rtl92e_writeb(dev,
+							      rOFDM0_RxDetector3,
+							      0x90);
+
+						reg_c38_State =
+						     RegC38_NonFsync_Other_AP;
+					}
+				} else if (priv->undecorated_smoothed_pwdb >=
+					   (RegC38_TH+5)) {
+					if (reg_c38_State) {
+						rtl92e_writeb(dev,
+							rOFDM0_RxDetector3,
+							priv->framesync);
+						reg_c38_State = RegC38_Default;
+					}
+				}
+			} else {
+				if (reg_c38_State) {
+					rtl92e_writeb(dev, rOFDM0_RxDetector3,
+						      priv->framesync);
+					reg_c38_State = RegC38_Default;
+				}
+			}
+		}
+	}
+	if (priv->framesyncMonitor) {
+		if (priv->reset_count != reset_cnt) {
+			rtl92e_writeb(dev, rOFDM0_RxDetector3,
+				       priv->framesync);
+			reg_c38_State = RegC38_Default;
+			reset_cnt = priv->reset_count;
+		}
+	} else {
+		if (reg_c38_State) {
+			rtl92e_writeb(dev, rOFDM0_RxDetector3,
+				       priv->framesync);
+			reg_c38_State = RegC38_Default;
+		}
+	}
+}
+
+/*---------------------------Define function prototype------------------------*/
+static void _rtl92e_dm_init_dynamic_tx_power(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	priv->rtllib->bdynamic_txpower_enable = true;
+	priv->bLastDTPFlag_High = false;
+	priv->bLastDTPFlag_Low = false;
+	priv->bDynamicTxHighPower = false;
+	priv->bDynamicTxLowPower = false;
+}
+
+static void _rtl92e_dm_dynamic_tx_power(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	unsigned int txhipower_threshold = 0;
+	unsigned int txlowpower_threshold = 0;
+
+	if (!priv->rtllib->bdynamic_txpower_enable) {
+		priv->bDynamicTxHighPower = false;
+		priv->bDynamicTxLowPower = false;
+		return;
+	}
+	if ((priv->rtllib->pHTInfo->IOTPeer == HT_IOT_PEER_ATHEROS) &&
+	    (priv->rtllib->mode == IEEE_G)) {
+		txhipower_threshold = TX_POWER_ATHEROAP_THRESH_HIGH;
+		txlowpower_threshold = TX_POWER_ATHEROAP_THRESH_LOW;
+	} else {
+		txhipower_threshold = TX_POWER_NEAR_FIELD_THRESH_HIGH;
+		txlowpower_threshold = TX_POWER_NEAR_FIELD_THRESH_LOW;
+	}
+
+	if (priv->rtllib->state == RTLLIB_LINKED) {
+		if (priv->undecorated_smoothed_pwdb >= txhipower_threshold) {
+			priv->bDynamicTxHighPower = true;
+			priv->bDynamicTxLowPower = false;
+		} else {
+			if (priv->undecorated_smoothed_pwdb <
+			    txlowpower_threshold && priv->bDynamicTxHighPower)
+				priv->bDynamicTxHighPower = false;
+			if (priv->undecorated_smoothed_pwdb < 35)
+				priv->bDynamicTxLowPower = true;
+			else if (priv->undecorated_smoothed_pwdb >= 40)
+				priv->bDynamicTxLowPower = false;
+		}
+	} else {
+		priv->bDynamicTxHighPower = false;
+		priv->bDynamicTxLowPower = false;
+	}
+
+	if ((priv->bDynamicTxHighPower != priv->bLastDTPFlag_High) ||
+	    (priv->bDynamicTxLowPower != priv->bLastDTPFlag_Low)) {
+		rtl92e_set_tx_power(dev, priv->rtllib->current_network.channel);
+	}
+	priv->bLastDTPFlag_High = priv->bDynamicTxHighPower;
+	priv->bLastDTPFlag_Low = priv->bDynamicTxLowPower;
+
+}
+
+static void _rtl92e_dm_check_txrateandretrycount(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	ieee->softmac_stats.CurrentShowTxate = rtl92e_readb(dev, CURRENT_TX_RATE_REG);
+	ieee->softmac_stats.last_packet_rate = rtl92e_readb(dev, INITIAL_TX_RATE_REG);
+	ieee->softmac_stats.txretrycount = rtl92e_readl(dev, TX_RETRY_COUNT_REG);
+}
+
+static void _rtl92e_dm_send_rssi_to_fw(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	rtl92e_writeb(dev, DRIVER_RSSI, priv->undecorated_smoothed_pwdb);
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
new file mode 100644
index 000000000..51e295d38
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
@@ -0,0 +1,195 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef	__R8192UDM_H__
+#define __R8192UDM_H__
+
+
+/*--------------------------Define Parameters-------------------------------*/
+#define			OFDM_Table_Length	19
+#define		CCK_Table_length	12
+
+#define		DM_DIG_THRESH_HIGH					40
+#define		DM_DIG_THRESH_LOW					35
+
+#define		DM_DIG_HIGH_PWR_THRESH_HIGH		75
+#define		DM_DIG_HIGH_PWR_THRESH_LOW		70
+
+#define		BW_AUTO_SWITCH_HIGH_LOW			25
+#define		BW_AUTO_SWITCH_LOW_HIGH			30
+
+#define		DM_DIG_BACKOFF				12
+#define		DM_DIG_MAX					0x36
+#define		DM_DIG_MIN					0x1c
+#define		DM_DIG_MIN_Netcore			0x12
+
+#define		RxPathSelection_SS_TH_low		30
+#define		RxPathSelection_diff_TH			18
+
+#define		RateAdaptiveTH_High			50
+#define		RateAdaptiveTH_Low_20M		30
+#define		RateAdaptiveTH_Low_40M		10
+#define		VeryLowRSSI					15
+
+#define		WAIotTHVal						25
+
+#define		E_FOR_TX_POWER_TRACK	       300
+#define		TX_POWER_NEAR_FIELD_THRESH_HIGH		68
+#define		TX_POWER_NEAR_FIELD_THRESH_LOW		62
+#define	 TX_POWER_ATHEROAP_THRESH_HIGH	   78
+#define		TX_POWER_ATHEROAP_THRESH_LOW		72
+
+#define		CURRENT_TX_RATE_REG		0x1e0
+#define		INITIAL_TX_RATE_REG		0x1e1
+#define		TX_RETRY_COUNT_REG		0x1ac
+#define		RegC38_TH				 20
+
+#define		DM_Type_ByDriver		1
+
+/*--------------------------Define Parameters-------------------------------*/
+
+
+/*------------------------------Define structure----------------------------*/
+struct dig_t {
+	u8		dig_enable_flag;
+	u8		dig_algorithm;
+	u8		dig_algorithm_switch;
+
+	long		rssi_low_thresh;
+	long		rssi_high_thresh;
+
+	long		rssi_high_power_lowthresh;
+	long		rssi_high_power_highthresh;
+
+	u8		dig_state;
+	u8		dig_highpwr_state;
+	u8		CurSTAConnectState;
+	u8		PreSTAConnectState;
+
+	u8		curpd_thstate;
+	u8		prepd_thstate;
+	u8		curcs_ratio_state;
+	u8		precs_ratio_state;
+
+	u32		pre_ig_value;
+	u32		cur_ig_value;
+
+	u8		backoff_val;
+	u8		rx_gain_range_max;
+	u8		rx_gain_range_min;
+
+	long		rssi_val;
+};
+
+enum dm_dig_sta {
+	DM_STA_DIG_OFF = 0,
+	DM_STA_DIG_ON,
+	DM_STA_DIG_MAX
+};
+
+
+enum dm_ratr_sta {
+	DM_RATR_STA_HIGH = 0,
+	DM_RATR_STA_MIDDLE = 1,
+	DM_RATR_STA_LOW = 2,
+	DM_RATR_STA_MAX
+};
+
+enum dm_dig_alg {
+	DIG_ALGO_BY_FALSE_ALARM = 0,
+	DIG_ALGO_BY_RSSI	= 1,
+	DIG_ALGO_BEFORE_CONNECT_BY_RSSI_AND_ALARM = 2,
+	DIG_ALGO_BY_TOW_PORT = 3,
+	DIG_ALGO_MAX
+};
+
+enum dm_dig_connect {
+	DIG_STA_DISCONNECT = 0,
+	DIG_STA_CONNECT = 1,
+	DIG_STA_BEFORE_CONNECT = 2,
+	DIG_AP_DISCONNECT = 3,
+	DIG_AP_CONNECT = 4,
+	DIG_AP_ADD_STATION = 5,
+	DIG_CONNECT_MAX
+};
+
+enum dm_dig_pd_th {
+	DIG_PD_AT_LOW_POWER = 0,
+	DIG_PD_AT_NORMAL_POWER = 1,
+	DIG_PD_AT_HIGH_POWER = 2,
+	DIG_PD_MAX
+};
+
+enum dm_dig_cs_ratio {
+	DIG_CS_RATIO_LOWER = 0,
+	DIG_CS_RATIO_HIGHER = 1,
+	DIG_CS_MAX
+};
+
+struct drx_path_sel {
+	u8		Enable;
+	u8		cck_method;
+	u8		cck_Rx_path;
+
+	u8		SS_TH_low;
+	u8		diff_TH;
+	u8		disabledRF;
+	u8		reserved;
+
+	u8		rf_rssi[4];
+	u8		rf_enable_rssi_th[4];
+	long		cck_pwdb_sta[4];
+};
+
+enum dm_cck_rx_path_method {
+	CCK_Rx_Version_1 = 0,
+	CCK_Rx_Version_2 = 1,
+	CCK_Rx_Version_MAX
+};
+
+
+struct dcmd_txcmd {
+	u32	Op;
+	u32	Length;
+	u32	Value;
+};
+/*------------------------------Define structure----------------------------*/
+
+
+/*------------------------Export global variable----------------------------*/
+extern	struct dig_t dm_digtable;
+extern struct drx_path_sel DM_RxPathSelTable;
+
+/* Pre-calculated gain tables */
+extern const u32 dm_tx_bb_gain[TxBBGainTableLength];
+extern const u8 dm_cck_tx_bb_gain[CCKTxBBGainTableLength][8];
+extern const u8 dm_cck_tx_bb_gain_ch14[CCKTxBBGainTableLength][8];
+/* Maps table index to iq amplify gain (dB, 12 to -24dB) */
+#define dm_tx_bb_gain_idx_to_amplify(idx) (-idx + 12)
+
+/*------------------------Export global variable----------------------------*/
+
+
+/*--------------------------Exported Function prototype---------------------*/
+/*--------------------------Exported Function prototype---------------------*/
+
+void rtl92e_dm_init(struct net_device *dev);
+void rtl92e_dm_deinit(struct net_device *dev);
+
+void rtl92e_dm_watchdog(struct net_device *dev);
+
+
+void    rtl92e_init_adaptive_rate(struct net_device *dev);
+void    rtl92e_dm_txpower_tracking_wq(void *data);
+
+void rtl92e_dm_cck_txpower_adjust(struct net_device *dev, bool binch14);
+
+void    rtl92e_dm_restore_state(struct net_device *dev);
+void    rtl92e_dm_backup_state(struct net_device *dev);
+void    rtl92e_dm_init_edca_turbo(struct net_device *dev);
+void    rtl92e_dm_rf_pathcheck_wq(void *data);
+void rtl92e_dm_init_txpower_tracking(struct net_device *dev);
+#endif	/*__R8192UDM_H__ */
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
new file mode 100644
index 000000000..59532ed21
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
@@ -0,0 +1,84 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "rtl_eeprom.h"
+
+static void _rtl92e_gpio_write_bit(struct net_device *dev, int no, bool val)
+{
+	u8 reg = rtl92e_readb(dev, EPROM_CMD);
+
+	if (val)
+		reg |= 1 << no;
+	else
+		reg &= ~(1 << no);
+
+	rtl92e_writeb(dev, EPROM_CMD, reg);
+	udelay(EPROM_DELAY);
+}
+
+static bool _rtl92e_gpio_get_bit(struct net_device *dev, int no)
+{
+	u8 reg = rtl92e_readb(dev, EPROM_CMD);
+
+	return (reg >> no) & 0x1;
+}
+
+static void _rtl92e_eeprom_ck_cycle(struct net_device *dev)
+{
+	_rtl92e_gpio_write_bit(dev, EPROM_CK_BIT, 1);
+	_rtl92e_gpio_write_bit(dev, EPROM_CK_BIT, 0);
+}
+
+static u16 _rtl92e_eeprom_xfer(struct net_device *dev, u16 data, int tx_len)
+{
+	u16 ret = 0;
+	int rx_len = 16;
+
+	_rtl92e_gpio_write_bit(dev, EPROM_CS_BIT, 1);
+	_rtl92e_eeprom_ck_cycle(dev);
+
+	while (tx_len--) {
+		_rtl92e_gpio_write_bit(dev, EPROM_W_BIT,
+				       (data >> tx_len) & 0x1);
+		_rtl92e_eeprom_ck_cycle(dev);
+	}
+
+	_rtl92e_gpio_write_bit(dev, EPROM_W_BIT, 0);
+
+	while (rx_len--) {
+		_rtl92e_eeprom_ck_cycle(dev);
+		ret |= _rtl92e_gpio_get_bit(dev, EPROM_R_BIT) << rx_len;
+	}
+
+	_rtl92e_gpio_write_bit(dev, EPROM_CS_BIT, 0);
+	_rtl92e_eeprom_ck_cycle(dev);
+
+	return ret;
+}
+
+u32 rtl92e_eeprom_read(struct net_device *dev, u32 addr)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 ret = 0;
+
+	rtl92e_writeb(dev, EPROM_CMD,
+		      (EPROM_CMD_PROGRAM << EPROM_CMD_OPERATING_MODE_SHIFT));
+	udelay(EPROM_DELAY);
+
+	/* EEPROM is configured as x16 */
+	if (priv->epromtype == EEPROM_93C56)
+		ret = _rtl92e_eeprom_xfer(dev, (addr & 0xFF) | (0x6 << 8), 11);
+	else
+		ret = _rtl92e_eeprom_xfer(dev, (addr & 0x3F) | (0x6 << 6), 9);
+
+	rtl92e_writeb(dev, EPROM_CMD,
+		      (EPROM_CMD_NORMAL<<EPROM_CMD_OPERATING_MODE_SHIFT));
+	return ret;
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
new file mode 100644
index 000000000..66f1979bb
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#define EPROM_DELAY 10
+
+u32 rtl92e_eeprom_read(struct net_device *dev, u32 addr);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ethtool.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ethtool.c
new file mode 100644
index 000000000..f4f7b74c8
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ethtool.c
@@ -0,0 +1,37 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include <linux/netdevice.h>
+#include <linux/ethtool.h>
+#include <linux/delay.h>
+
+#include "rtl_core.h"
+
+static void _rtl92e_ethtool_get_drvinfo(struct net_device *dev,
+					struct ethtool_drvinfo *info)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	strscpy(info->driver, DRV_NAME, sizeof(info->driver));
+	strscpy(info->version, DRV_VERSION, sizeof(info->version));
+	strscpy(info->bus_info, pci_name(priv->pdev), sizeof(info->bus_info));
+}
+
+static u32 _rtl92e_ethtool_get_link(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return ((priv->rtllib->state == RTLLIB_LINKED) ||
+		(priv->rtllib->state == RTLLIB_LINKED_SCANNING));
+}
+
+const struct ethtool_ops rtl819x_ethtool_ops = {
+	.get_drvinfo = _rtl92e_ethtool_get_drvinfo,
+	.get_link = _rtl92e_ethtool_get_link,
+};
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
new file mode 100644
index 000000000..81e1bb856
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
@@ -0,0 +1,79 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_pci.h"
+#include "rtl_core.h"
+
+static void _rtl92e_parse_pci_configuration(struct pci_dev *pdev,
+					    struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	u8 tmp;
+	u16 link_ctrl_reg;
+
+	pcie_capability_read_word(priv->pdev, PCI_EXP_LNKCTL, &link_ctrl_reg);
+
+	pci_read_config_byte(pdev, 0x98, &tmp);
+	tmp |= BIT4;
+	pci_write_config_byte(pdev, 0x98, tmp);
+
+	tmp = 0x17;
+	pci_write_config_byte(pdev, 0x70f, tmp);
+}
+
+bool rtl92e_check_adapter(struct pci_dev *pdev, struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	u16 device_id;
+	u8  revision_id;
+	u16 irq_line;
+
+	device_id = pdev->device;
+	revision_id = pdev->revision;
+	pci_read_config_word(pdev, 0x3C, &irq_line);
+
+	priv->card_8192 = priv->ops->nic_type;
+
+	if (device_id == 0x8192) {
+		switch (revision_id) {
+		case HAL_HW_PCI_REVISION_ID_8192PCIE:
+			dev_info(&pdev->dev,
+				 "Adapter(8192 PCI-E) is found - DeviceID=%x\n",
+				 device_id);
+			priv->card_8192 = NIC_8192E;
+			break;
+		case HAL_HW_PCI_REVISION_ID_8192SE:
+			dev_info(&pdev->dev,
+				 "Adapter(8192SE) is found - DeviceID=%x\n",
+				 device_id);
+			priv->card_8192 = NIC_8192SE;
+			break;
+		default:
+			dev_info(&pdev->dev,
+				 "UNKNOWN nic type(%4x:%4x)\n",
+				 pdev->vendor, pdev->device);
+			priv->card_8192 = NIC_UNKNOWN;
+			return false;
+		}
+	}
+
+	if (priv->ops->nic_type != priv->card_8192) {
+		dev_info(&pdev->dev,
+			 "Detect info(%x) and hardware info(%x) not match!\n",
+			 priv->ops->nic_type, priv->card_8192);
+		dev_info(&pdev->dev,
+			 "Please select proper driver before install!!!!\n");
+		return false;
+	}
+
+	_rtl92e_parse_pci_configuration(pdev, dev);
+
+	return true;
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
new file mode 100644
index 000000000..866e0efbc
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL_PCI_H
+#define _RTL_PCI_H
+
+#include <linux/types.h>
+#include <linux/pci.h>
+
+struct net_device;
+bool rtl92e_check_adapter(struct pci_dev *pdev, struct net_device *dev);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
new file mode 100644
index 000000000..82b45c61a
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_core.h"
+#include "r8192E_hw.h"
+#include "r8190P_rtl8256.h"
+#include "rtl_pm.h"
+
+
+int rtl92e_suspend(struct device *dev_d)
+{
+	struct net_device *dev = dev_get_drvdata(dev_d);
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32	ulRegRead;
+
+	netdev_info(dev, "============> r8192E suspend call.\n");
+	del_timer_sync(&priv->gpio_polling_timer);
+	cancel_delayed_work_sync(&priv->gpio_change_rf_wq);
+	priv->polling_timer_on = 0;
+
+	if (!netif_running(dev)) {
+		netdev_info(dev,
+			    "RTL819XE:UI is open out of suspend function\n");
+		goto out_pci_suspend;
+	}
+
+	if (dev->netdev_ops->ndo_stop)
+		dev->netdev_ops->ndo_stop(dev);
+	netif_device_detach(dev);
+
+	if (!priv->rtllib->bSupportRemoteWakeUp) {
+		rtl92e_set_rf_state(dev, rf_off, RF_CHANGE_BY_INIT);
+		ulRegRead = rtl92e_readl(dev, CPU_GEN);
+		ulRegRead |= CPU_GEN_SYSTEM_RESET;
+		rtl92e_writel(dev, CPU_GEN, ulRegRead);
+	} else {
+		rtl92e_writel(dev, WFCRC0, 0xffffffff);
+		rtl92e_writel(dev, WFCRC1, 0xffffffff);
+		rtl92e_writel(dev, WFCRC2, 0xffffffff);
+		rtl92e_writeb(dev, PMR, 0x5);
+		rtl92e_writeb(dev, MacBlkCtrl, 0xa);
+	}
+out_pci_suspend:
+	netdev_info(dev, "WOL is %s\n", priv->rtllib->bSupportRemoteWakeUp ?
+			    "Supported" : "Not supported");
+	device_set_wakeup_enable(dev_d, priv->rtllib->bSupportRemoteWakeUp);
+
+	mdelay(20);
+
+	return 0;
+}
+
+int rtl92e_resume(struct device *dev_d)
+{
+	struct pci_dev *pdev = to_pci_dev(dev_d);
+	struct net_device *dev = dev_get_drvdata(dev_d);
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u32 val;
+
+	netdev_info(dev, "================>r8192E resume call.\n");
+
+
+	pci_read_config_dword(pdev, 0x40, &val);
+	if ((val & 0x0000ff00) != 0)
+		pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
+
+	device_wakeup_disable(dev_d);
+
+	if (priv->polling_timer_on == 0)
+		rtl92e_check_rfctrl_gpio_timer(&priv->gpio_polling_timer);
+
+	if (!netif_running(dev)) {
+		netdev_info(dev,
+			    "RTL819XE:UI is open out of resume function\n");
+		goto out;
+	}
+
+	netif_device_attach(dev);
+	if (dev->netdev_ops->ndo_open)
+		dev->netdev_ops->ndo_open(dev);
+
+	if (!priv->rtllib->bSupportRemoteWakeUp)
+		rtl92e_set_rf_state(dev, rf_on, RF_CHANGE_BY_INIT);
+
+out:
+	return 0;
+}
+
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
new file mode 100644
index 000000000..fd8611495
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef R8192E_PM_H
+#define R8192E_PM_H
+
+#include <linux/types.h>
+#include <linux/pci.h>
+
+int rtl92e_suspend(struct device *dev_d);
+int rtl92e_resume(struct device *dev_d);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
new file mode 100644
index 000000000..8c00b111d
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
@@ -0,0 +1,262 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtl_ps.h"
+#include "rtl_core.h"
+#include "r8192E_phy.h"
+#include "r8192E_phyreg.h"
+#include "r8190P_rtl8256.h" /* RTL8225 Radio frontend */
+#include "r8192E_cmdpkt.h"
+#include <linux/jiffies.h>
+
+static void _rtl92e_hw_sleep(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&priv->rf_ps_lock, flags);
+	if (priv->rf_change_in_progress) {
+		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+	rtl92e_set_rf_state(dev, rf_sleep, RF_CHANGE_BY_PS);
+}
+
+void rtl92e_hw_sleep_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, hw_sleep_wq);
+	struct net_device *dev = ieee->dev;
+
+	_rtl92e_hw_sleep(dev);
+}
+
+void rtl92e_hw_wakeup(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&priv->rf_ps_lock, flags);
+	if (priv->rf_change_in_progress) {
+		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+		schedule_delayed_work(&priv->rtllib->hw_wakeup_wq,
+				      msecs_to_jiffies(10));
+		return;
+	}
+	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
+	rtl92e_set_rf_state(dev, rf_on, RF_CHANGE_BY_PS);
+}
+
+void rtl92e_hw_wakeup_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, hw_wakeup_wq);
+	struct net_device *dev = ieee->dev;
+
+	rtl92e_hw_wakeup(dev);
+}
+
+#define MIN_SLEEP_TIME 50
+#define MAX_SLEEP_TIME 10000
+void rtl92e_enter_sleep(struct net_device *dev, u64 time)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	u32 tmp;
+	unsigned long flags;
+	unsigned long timeout;
+
+	spin_lock_irqsave(&priv->ps_lock, flags);
+
+	time -= msecs_to_jiffies(8 + 16 + 7);
+
+	timeout = jiffies + msecs_to_jiffies(MIN_SLEEP_TIME);
+	if (time_before((unsigned long)time, timeout)) {
+		spin_unlock_irqrestore(&priv->ps_lock, flags);
+		netdev_info(dev, "too short to sleep::%lld < %ld\n",
+			    time - jiffies, msecs_to_jiffies(MIN_SLEEP_TIME));
+		return;
+	}
+	timeout = jiffies + msecs_to_jiffies(MAX_SLEEP_TIME);
+	if (time_after((unsigned long)time, timeout)) {
+		netdev_info(dev, "========>too long to sleep:%lld > %ld\n",
+			    time - jiffies, msecs_to_jiffies(MAX_SLEEP_TIME));
+		spin_unlock_irqrestore(&priv->ps_lock, flags);
+		return;
+	}
+	tmp = time - jiffies;
+	schedule_delayed_work(&priv->rtllib->hw_wakeup_wq, tmp);
+	schedule_delayed_work(&priv->rtllib->hw_sleep_wq, 0);
+	spin_unlock_irqrestore(&priv->ps_lock, flags);
+}
+
+static void _rtl92e_ps_update_rf_state(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&(priv->rtllib->PowerSaveControl);
+
+	pPSC->bSwRfProcessing = true;
+	rtl92e_set_rf_state(dev, pPSC->eInactivePowerState, RF_CHANGE_BY_IPS);
+
+	pPSC->bSwRfProcessing = false;
+}
+
+void rtl92e_ips_enter(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&(priv->rtllib->PowerSaveControl);
+	enum rt_rf_power_state rt_state;
+
+	if (pPSC->bInactivePs) {
+		rt_state = priv->rtllib->rf_power_state;
+		if (rt_state == rf_on && !pPSC->bSwRfProcessing &&
+			(priv->rtllib->state != RTLLIB_LINKED) &&
+			(priv->rtllib->iw_mode != IW_MODE_MASTER)) {
+			pPSC->eInactivePowerState = rf_off;
+			priv->isRFOff = true;
+			priv->bInPowerSaveMode = true;
+			_rtl92e_ps_update_rf_state(dev);
+		}
+	}
+}
+
+void rtl92e_ips_leave(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&(priv->rtllib->PowerSaveControl);
+	enum rt_rf_power_state rt_state;
+
+	if (pPSC->bInactivePs) {
+		rt_state = priv->rtllib->rf_power_state;
+		if (rt_state != rf_on  && !pPSC->bSwRfProcessing &&
+		    priv->rtllib->rf_off_reason <= RF_CHANGE_BY_IPS) {
+			pPSC->eInactivePowerState = rf_on;
+			priv->bInPowerSaveMode = false;
+			_rtl92e_ps_update_rf_state(dev);
+		}
+	}
+}
+
+void rtl92e_ips_leave_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_work_rsl(data,
+				     struct rtllib_device, ips_leave_wq);
+	struct net_device *dev = ieee->dev;
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	mutex_lock(&priv->rtllib->ips_mutex);
+	rtl92e_ips_leave(dev);
+	mutex_unlock(&priv->rtllib->ips_mutex);
+}
+
+void rtl92e_rtllib_ips_leave_wq(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+	enum rt_rf_power_state rt_state;
+
+	rt_state = priv->rtllib->rf_power_state;
+
+	if (priv->rtllib->PowerSaveControl.bInactivePs) {
+		if (rt_state == rf_off) {
+			if (priv->rtllib->rf_off_reason > RF_CHANGE_BY_IPS) {
+				netdev_warn(dev, "%s(): RF is OFF.\n",
+					    __func__);
+				return;
+			}
+			netdev_info(dev, "=========>%s(): rtl92e_ips_leave\n",
+				    __func__);
+			schedule_work(&priv->rtllib->ips_leave_wq);
+		}
+	}
+}
+
+void rtl92e_rtllib_ips_leave(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
+	mutex_lock(&priv->rtllib->ips_mutex);
+	rtl92e_ips_leave(dev);
+	mutex_unlock(&priv->rtllib->ips_mutex);
+}
+
+static bool _rtl92e_ps_set_mode(struct net_device *dev, u8 rtPsMode)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->rtllib->iw_mode == IW_MODE_ADHOC)
+		return false;
+
+	if (!priv->ps_force)
+		priv->rtllib->ps = rtPsMode;
+	if (priv->rtllib->sta_sleep != LPS_IS_WAKE &&
+	    rtPsMode == RTLLIB_PS_DISABLED) {
+		unsigned long flags;
+
+		rtl92e_hw_wakeup(dev);
+		priv->rtllib->sta_sleep = LPS_IS_WAKE;
+
+		spin_lock_irqsave(&(priv->rtllib->mgmt_tx_lock), flags);
+		rtllib_sta_ps_send_null_frame(priv->rtllib, 0);
+		spin_unlock_irqrestore(&(priv->rtllib->mgmt_tx_lock), flags);
+	}
+
+	return true;
+}
+
+void rtl92e_leisure_ps_enter(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&(priv->rtllib->PowerSaveControl);
+
+	if (!((priv->rtllib->iw_mode == IW_MODE_INFRA) &&
+	    (priv->rtllib->state == RTLLIB_LINKED))
+	    || (priv->rtllib->iw_mode == IW_MODE_ADHOC) ||
+	    (priv->rtllib->iw_mode == IW_MODE_MASTER))
+		return;
+
+	if (pPSC->bLeisurePs) {
+		if (pPSC->LpsIdleCount >= RT_CHECK_FOR_HANG_PERIOD) {
+
+			if (priv->rtllib->ps == RTLLIB_PS_DISABLED) {
+				if (!pPSC->bFwCtrlLPS) {
+					if (priv->rtllib->SetFwCmdHandler)
+						priv->rtllib->SetFwCmdHandler(
+							dev, FW_CMD_LPS_ENTER);
+				}
+				_rtl92e_ps_set_mode(dev, RTLLIB_PS_MBCAST |
+							 RTLLIB_PS_UNICAST);
+			}
+		} else
+			pPSC->LpsIdleCount++;
+	}
+}
+
+void rtl92e_leisure_ps_leave(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					&(priv->rtllib->PowerSaveControl);
+
+	if (pPSC->bLeisurePs) {
+		if (priv->rtllib->ps != RTLLIB_PS_DISABLED) {
+			_rtl92e_ps_set_mode(dev, RTLLIB_PS_DISABLED);
+
+			if (!pPSC->bFwCtrlLPS) {
+				if (priv->rtllib->SetFwCmdHandler)
+					priv->rtllib->SetFwCmdHandler(dev,
+							 FW_CMD_LPS_LEAVE);
+			}
+		}
+	}
+}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.h b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.h
new file mode 100644
index 000000000..70fe5d39b
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL_PS_H
+#define _RTL_PS_H
+
+#include <linux/types.h>
+
+struct net_device;
+
+#define RT_CHECK_FOR_HANG_PERIOD 2
+
+void rtl92e_hw_wakeup(struct net_device *dev);
+void rtl92e_enter_sleep(struct net_device *dev, u64 time);
+void rtl92e_rtllib_ips_leave_wq(struct net_device *dev);
+void rtl92e_rtllib_ips_leave(struct net_device *dev);
+void rtl92e_ips_leave_wq(void *data);
+
+void rtl92e_ips_enter(struct net_device *dev);
+void rtl92e_ips_leave(struct net_device *dev);
+
+void rtl92e_leisure_ps_enter(struct net_device *dev);
+void rtl92e_leisure_ps_leave(struct net_device *dev);
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
new file mode 100644
index 000000000..4920cb49e
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -0,0 +1,1210 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include <linux/string.h>
+#include "rtl_core.h"
+#include "rtl_wx.h"
+
+#define RATE_COUNT 12
+static u32 rtl8192_rates[] = {
+	1000000, 2000000, 5500000, 11000000, 6000000, 9000000, 12000000,
+	18000000, 24000000, 36000000, 48000000, 54000000
+};
+
+#ifndef ENETDOWN
+#define ENETDOWN 1
+#endif
+
+static int _rtl92e_wx_get_freq(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_freq(priv->rtllib, a, wrqu, b);
+}
+
+static int _rtl92e_wx_get_mode(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_mode(priv->rtllib, a, wrqu, b);
+}
+
+static int _rtl92e_wx_get_rate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_rate(priv->rtllib, info, wrqu, extra);
+}
+
+static int _rtl92e_wx_set_rate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_rate(priv->rtllib, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_set_rts(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_rts(priv->rtllib, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_get_rts(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_rts(priv->rtllib, info, wrqu, extra);
+}
+
+static int _rtl92e_wx_set_power(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off) {
+		netdev_warn(dev, "%s(): Can't set Power: Radio is Off.\n",
+			    __func__);
+		return 0;
+	}
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_power(priv->rtllib, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_get_power(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_power(priv->rtllib, info, wrqu, extra);
+}
+
+static int _rtl92e_wx_set_rawtx(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_rawtx(priv->rtllib, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_force_reset(struct net_device *dev,
+				  struct iw_request_info *info,
+				  union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	priv->force_reset = *extra;
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int _rtl92e_wx_adapter_power_status(struct net_device *dev,
+					   struct iw_request_info *info,
+					   union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&priv->rtllib->PowerSaveControl);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	mutex_lock(&priv->wx_mutex);
+
+	if (*extra || priv->force_lps) {
+		priv->ps_force = false;
+		pPSC->bLeisurePs = true;
+	} else {
+		if (priv->rtllib->state == RTLLIB_LINKED)
+			rtl92e_leisure_ps_leave(dev);
+
+		priv->ps_force = true;
+		pPSC->bLeisurePs = false;
+		ieee->ps = *extra;
+	}
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return 0;
+}
+
+static int _rtl92e_wx_set_lps_awake_interval(struct net_device *dev,
+					     struct iw_request_info *info,
+					     union iwreq_data *wrqu,
+					     char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
+					(&priv->rtllib->PowerSaveControl);
+
+	mutex_lock(&priv->wx_mutex);
+
+	netdev_info(dev, "%s(): set lps awake interval ! extra is %d\n",
+		    __func__, *extra);
+
+	pPSC->RegMaxLPSAwakeIntvl = *extra;
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int _rtl92e_wx_set_force_lps(struct net_device *dev,
+				    struct iw_request_info *info,
+				    union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	netdev_info(dev,
+		    "%s(): force LPS ! extra is %d (1 is open 0 is close)\n",
+		    __func__, *extra);
+	priv->force_lps = *extra;
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int _rtl92e_wx_set_debug(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 c = *extra;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	netdev_info(dev, "=====>%s(), *extra:%x, debugflag:%x\n", __func__,
+		    *extra, rt_global_debug_component);
+	if (c > 0)
+		rt_global_debug_component |= (1 << c);
+	else
+		rt_global_debug_component &= BIT31;
+	return 0;
+}
+
+static int _rtl92e_wx_set_mode(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = netdev_priv_rsl(dev);
+
+	enum rt_rf_power_state rt_state;
+	int ret;
+
+	if (priv->hw_radio_off)
+		return 0;
+	rt_state = priv->rtllib->rf_power_state;
+	mutex_lock(&priv->wx_mutex);
+	if (wrqu->mode == IW_MODE_ADHOC || wrqu->mode == IW_MODE_MONITOR ||
+	    ieee->bNetPromiscuousMode) {
+		if (priv->rtllib->PowerSaveControl.bInactivePs) {
+			if (rt_state == rf_off) {
+				if (priv->rtllib->rf_off_reason >
+				    RF_CHANGE_BY_IPS) {
+					netdev_warn(dev, "%s(): RF is OFF.\n",
+						    __func__);
+					mutex_unlock(&priv->wx_mutex);
+					return -1;
+				}
+				netdev_info(dev,
+					    "=========>%s(): rtl92e_ips_leave\n",
+					    __func__);
+				mutex_lock(&priv->rtllib->ips_mutex);
+				rtl92e_ips_leave(dev);
+				mutex_unlock(&priv->rtllib->ips_mutex);
+			}
+		}
+	}
+	ret = rtllib_wx_set_mode(priv->rtllib, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+struct  iw_range_with_scan_capa {
+	/* Informative stuff (to choose between different interface) */
+	__u32	   throughput;     /* To give an idea... */
+	/* In theory this value should be the maximum benchmarked
+	 * TCP/IP throughput, because with most of these devices the
+	 * bit rate is meaningless (overhead an co) to estimate how
+	 * fast the connection will go and pick the fastest one.
+	 * I suggest people to play with Netperf or any benchmark...
+	 */
+
+	/* NWID (or domain id) */
+	__u32	   min_nwid;	/* Minimal NWID we are able to set */
+	__u32	   max_nwid;	/* Maximal NWID we are able to set */
+
+	/* Old Frequency (backward compat - moved lower ) */
+	__u16	   old_num_channels;
+	__u8	    old_num_frequency;
+
+	/* Scan capabilities */
+	__u8	    scan_capa;
+};
+
+static int _rtl92e_wx_get_range(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct iw_range *range = (struct iw_range *)extra;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	u16 val;
+	int i;
+
+	wrqu->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+
+	/* ~130 Mb/s real (802.11n) */
+	range->throughput = 130 * 1000 * 1000;
+
+	if (priv->rf_set_sens != NULL)
+		/* signal level threshold range */
+		range->sensitivity = priv->max_sens;
+
+	range->max_qual.qual = 100;
+	range->max_qual.level = 0;
+	range->max_qual.noise = 0;
+	range->max_qual.updated = 7; /* Updated all three */
+
+	range->avg_qual.qual = 70; /* > 8% missed beacons is 'bad' */
+	range->avg_qual.level = 0;
+	range->avg_qual.noise = 0;
+	range->avg_qual.updated = 7; /* Updated all three */
+
+	range->num_bitrates = min(RATE_COUNT, IW_MAX_BITRATES);
+
+	for (i = 0; i < range->num_bitrates; i++)
+		range->bitrate[i] = rtl8192_rates[i];
+
+	range->max_rts = DEFAULT_RTS_THRESHOLD;
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+
+	range->min_pmp = 0;
+	range->max_pmp = 5000000;
+	range->min_pmt = 0;
+	range->max_pmt = 65535 * 1000;
+	range->pmp_flags = IW_POWER_PERIOD;
+	range->pmt_flags = IW_POWER_TIMEOUT;
+	range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 18;
+
+	for (i = 0, val = 0; i < 14; i++) {
+		if ((priv->rtllib->active_channel_map)[i + 1]) {
+			range->freq[val].i = i + 1;
+			range->freq[val].m = rtllib_wlan_frequencies[i] *
+					     100000;
+			range->freq[val].e = 1;
+			val++;
+		}
+
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+	range->num_channels = val;
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+			  IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+	range->scan_capa = IW_SCAN_CAPA_ESSID | IW_SCAN_CAPA_TYPE;
+
+	/* Event capability (kernel + driver) */
+
+	return 0;
+}
+
+static int _rtl92e_wx_set_scan(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	enum rt_rf_power_state rt_state;
+	int ret;
+
+	if (!(ieee->softmac_features & IEEE_SOFTMAC_SCAN)) {
+		if ((ieee->state >= RTLLIB_ASSOCIATING) &&
+		    (ieee->state <= RTLLIB_ASSOCIATING_AUTHENTICATED))
+			return 0;
+		if ((priv->rtllib->state == RTLLIB_LINKED) &&
+		    (priv->rtllib->CntAfterLink < 2))
+			return 0;
+	}
+
+	if (priv->hw_radio_off) {
+		netdev_info(dev, "================>%s(): hwradio off\n",
+			    __func__);
+		return 0;
+	}
+	rt_state = priv->rtllib->rf_power_state;
+	if (!priv->up)
+		return -ENETDOWN;
+	if (priv->rtllib->LinkDetectInfo.bBusyTraffic == true)
+		return -EAGAIN;
+
+	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+		struct iw_scan_req *req = (struct iw_scan_req *)b;
+
+		if (req->essid_len) {
+			int len = min_t(int, req->essid_len, IW_ESSID_MAX_SIZE);
+
+			ieee->current_network.ssid_len = len;
+			memcpy(ieee->current_network.ssid, req->essid, len);
+		}
+	}
+
+	mutex_lock(&priv->wx_mutex);
+
+	priv->rtllib->FirstIe_InScan = true;
+
+	if (priv->rtllib->state != RTLLIB_LINKED) {
+		if (priv->rtllib->PowerSaveControl.bInactivePs) {
+			if (rt_state == rf_off) {
+				if (priv->rtllib->rf_off_reason >
+				    RF_CHANGE_BY_IPS) {
+					netdev_warn(dev, "%s(): RF is OFF.\n",
+						    __func__);
+					mutex_unlock(&priv->wx_mutex);
+					return -1;
+				}
+				mutex_lock(&priv->rtllib->ips_mutex);
+				rtl92e_ips_leave(dev);
+				mutex_unlock(&priv->rtllib->ips_mutex);
+			}
+		}
+		rtllib_stop_scan(priv->rtllib);
+		if (priv->rtllib->LedControlHandler)
+			priv->rtllib->LedControlHandler(dev,
+							 LED_CTL_SITE_SURVEY);
+
+		if (priv->rtllib->rf_power_state != rf_off) {
+			priv->rtllib->actscanning = true;
+
+			if (ieee->ScanOperationBackupHandler)
+				ieee->ScanOperationBackupHandler(ieee->dev,
+							 SCAN_OPT_BACKUP);
+
+			rtllib_start_scan_syncro(priv->rtllib, 0);
+
+			if (ieee->ScanOperationBackupHandler)
+				ieee->ScanOperationBackupHandler(ieee->dev,
+							 SCAN_OPT_RESTORE);
+		}
+		ret = 0;
+	} else {
+		priv->rtllib->actscanning = true;
+		ret = rtllib_wx_set_scan(priv->rtllib, a, wrqu, b);
+	}
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_get_scan(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (!priv->up)
+		return -ENETDOWN;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_get_scan(priv->rtllib, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_set_essid(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+
+	if (priv->hw_radio_off) {
+		netdev_info(dev,
+			    "=========>%s():hw radio off,or Rf state is rf_off, return\n",
+			    __func__);
+		return 0;
+	}
+	mutex_lock(&priv->wx_mutex);
+	ret = rtllib_wx_set_essid(priv->rtllib, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_get_essid(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_get_essid(priv->rtllib, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_set_nick(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (wrqu->data.length > IW_ESSID_MAX_SIZE)
+		return -E2BIG;
+	mutex_lock(&priv->wx_mutex);
+	wrqu->data.length = min_t(size_t, wrqu->data.length,
+				  sizeof(priv->nick));
+	memset(priv->nick, 0, sizeof(priv->nick));
+	memcpy(priv->nick, extra, wrqu->data.length);
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int _rtl92e_wx_get_nick(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+	wrqu->data.length = strlen(priv->nick);
+	memcpy(extra, priv->nick, wrqu->data.length);
+	wrqu->data.flags = 1;   /* active */
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int _rtl92e_wx_set_freq(struct net_device *dev,
+			       struct iw_request_info *a,
+			       union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_freq(priv->rtllib, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_get_name(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_name(priv->rtllib, info, wrqu, extra);
+}
+
+static int _rtl92e_wx_set_frag(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	if (wrqu->frag.disabled)
+		priv->rtllib->fts = DEFAULT_FRAG_THRESHOLD;
+	else {
+		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
+		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
+			return -EINVAL;
+
+		priv->rtllib->fts = wrqu->frag.value & ~0x1;
+	}
+
+	return 0;
+}
+
+static int _rtl92e_wx_get_frag(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	wrqu->frag.value = priv->rtllib->fts;
+	wrqu->frag.fixed = 0;	/* no auto select */
+	wrqu->frag.disabled = (wrqu->frag.value == DEFAULT_FRAG_THRESHOLD);
+
+	return 0;
+}
+
+static int _rtl92e_wx_set_wap(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *awrq, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_wap(priv->rtllib, info, awrq, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int _rtl92e_wx_get_wap(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_wap(priv->rtllib, info, wrqu, extra);
+}
+
+static int _rtl92e_wx_get_enc(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *key)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	return rtllib_wx_get_encode(priv->rtllib, info, wrqu, key);
+}
+
+static int _rtl92e_wx_set_enc(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *key)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int ret;
+
+	struct rtllib_device *ieee = priv->rtllib;
+	u32 hwkey[4] = {0, 0, 0, 0};
+	u8 mask = 0xff;
+	u32 key_idx = 0;
+	u8 zero_addr[4][6] = {{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+			     {0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+			     {0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+			     {0x00, 0x00, 0x00, 0x00, 0x00, 0x03} };
+	int i;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	if (!priv->up)
+		return -ENETDOWN;
+
+	priv->rtllib->wx_set_enc = 1;
+	mutex_lock(&priv->rtllib->ips_mutex);
+	rtl92e_ips_leave(dev);
+	mutex_unlock(&priv->rtllib->ips_mutex);
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtllib_wx_set_encode(priv->rtllib, info, wrqu, key);
+	mutex_unlock(&priv->wx_mutex);
+
+	if (wrqu->encoding.flags & IW_ENCODE_DISABLED) {
+		ieee->pairwise_key_type = ieee->group_key_type = KEY_TYPE_NA;
+		rtl92e_cam_reset(dev);
+		memset(priv->rtllib->swcamtable, 0,
+		       sizeof(struct sw_cam_table) * 32);
+		goto end_hw_sec;
+	}
+	if (wrqu->encoding.length != 0) {
+		for (i = 0; i < 4; i++) {
+			hwkey[i] |=  key[4 * i + 0] & mask;
+			if (i == 1 && (4 * i + 1) == wrqu->encoding.length)
+				mask = 0x00;
+			if (i == 3 && (4 * i + 1) == wrqu->encoding.length)
+				mask = 0x00;
+			hwkey[i] |= (key[4 * i + 1] & mask) << 8;
+			hwkey[i] |= (key[4 * i + 2] & mask) << 16;
+			hwkey[i] |= (key[4 * i + 3] & mask) << 24;
+		}
+
+		switch (wrqu->encoding.flags & IW_ENCODE_INDEX) {
+		case 0:
+			key_idx = ieee->crypt_info.tx_keyidx;
+			break;
+		case 1:
+			key_idx = 0;
+			break;
+		case 2:
+			key_idx = 1;
+			break;
+		case 3:
+			key_idx = 2;
+			break;
+		case 4:
+			key_idx	= 3;
+			break;
+		default:
+			break;
+		}
+		if (wrqu->encoding.length == 0x5) {
+			ieee->pairwise_key_type = KEY_TYPE_WEP40;
+			rtl92e_enable_hw_security_config(dev);
+		}
+
+		else if (wrqu->encoding.length == 0xd) {
+			ieee->pairwise_key_type = KEY_TYPE_WEP104;
+			rtl92e_enable_hw_security_config(dev);
+			rtl92e_set_key(dev, key_idx, key_idx, KEY_TYPE_WEP104,
+				       zero_addr[key_idx], 0, hwkey);
+			rtl92e_set_swcam(dev, key_idx, key_idx, KEY_TYPE_WEP104,
+					 zero_addr[key_idx], 0, hwkey, 0);
+		} else {
+			netdev_info(dev,
+				    "wrong type in WEP, not WEP40 and WEP104\n");
+		}
+	}
+
+end_hw_sec:
+	priv->rtllib->wx_set_enc = 0;
+	return ret;
+}
+
+static int _rtl92e_wx_set_scan_type(struct net_device *dev,
+				    struct iw_request_info *aa,
+				    union iwreq_data *wrqu, char *p)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int *parms = (int *)p;
+	int mode = parms[0];
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	priv->rtllib->active_scan = mode;
+
+	return 1;
+}
+
+#define R8192_MAX_RETRY 255
+static int _rtl92e_wx_set_retry(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	int err = 0;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	if (wrqu->retry.flags & IW_RETRY_LIFETIME ||
+	    wrqu->retry.disabled) {
+		err = -EINVAL;
+		goto exit;
+	}
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)) {
+		err = -EINVAL;
+		goto exit;
+	}
+
+	if (wrqu->retry.value > R8192_MAX_RETRY) {
+		err = -EINVAL;
+		goto exit;
+	}
+	if (wrqu->retry.flags & IW_RETRY_MAX)
+		priv->retry_rts = wrqu->retry.value;
+	else
+		priv->retry_data = wrqu->retry.value;
+
+	rtl92e_commit(dev);
+exit:
+	mutex_unlock(&priv->wx_mutex);
+
+	return err;
+}
+
+static int _rtl92e_wx_get_retry(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	wrqu->retry.disabled = 0; /* can't be disabled */
+
+	if ((wrqu->retry.flags & IW_RETRY_TYPE) ==
+	    IW_RETRY_LIFETIME)
+		return -EINVAL;
+
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+		wrqu->retry.value = priv->retry_rts;
+	} else {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
+		wrqu->retry.value = priv->retry_data;
+	}
+	return 0;
+}
+
+static int _rtl92e_wx_get_sens(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->rf_set_sens == NULL)
+		return -1; /* we have not this support for this radio */
+	wrqu->sens.value = priv->sens;
+	return 0;
+}
+
+static int _rtl92e_wx_set_sens(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	short err = 0;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+	if (priv->rf_set_sens == NULL) {
+		err = -1; /* we have not this support for this radio */
+		goto exit;
+	}
+	if (priv->rf_set_sens(dev, wrqu->sens.value) == 0)
+		priv->sens = wrqu->sens.value;
+	else
+		err = -EINVAL;
+
+exit:
+	mutex_unlock(&priv->wx_mutex);
+
+	return err;
+}
+
+static int _rtl92e_wx_set_encode_ext(struct net_device *dev,
+				     struct iw_request_info *info,
+				     union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	priv->rtllib->wx_set_enc = 1;
+	mutex_lock(&priv->rtllib->ips_mutex);
+	rtl92e_ips_leave(dev);
+	mutex_unlock(&priv->rtllib->ips_mutex);
+
+	ret = rtllib_wx_set_encode_ext(ieee, info, wrqu, extra);
+	{
+		const u8 broadcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		const u8 zero[ETH_ALEN] = {0};
+		u32 key[4] = {0};
+		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+		struct iw_point *encoding = &wrqu->encoding;
+		u8 idx = 0, alg = 0, group = 0;
+
+		if ((encoding->flags & IW_ENCODE_DISABLED) ||
+		     ext->alg == IW_ENCODE_ALG_NONE) {
+			ieee->pairwise_key_type = ieee->group_key_type
+						= KEY_TYPE_NA;
+			rtl92e_cam_reset(dev);
+			memset(priv->rtllib->swcamtable, 0,
+			       sizeof(struct sw_cam_table) * 32);
+			goto end_hw_sec;
+		}
+		alg = (ext->alg == IW_ENCODE_ALG_CCMP) ? KEY_TYPE_CCMP :
+		      ext->alg;
+		idx = encoding->flags & IW_ENCODE_INDEX;
+		if (idx)
+			idx--;
+		group = ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY;
+
+		if ((!group) || (ieee->iw_mode == IW_MODE_ADHOC) ||
+		    (alg ==  KEY_TYPE_WEP40)) {
+			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40))
+				alg = KEY_TYPE_WEP104;
+			ieee->pairwise_key_type = alg;
+			rtl92e_enable_hw_security_config(dev);
+		}
+		memcpy((u8 *)key, ext->key, 16);
+
+		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode != 2)) {
+			if (ext->key_len == 13)
+				ieee->pairwise_key_type = alg = KEY_TYPE_WEP104;
+			rtl92e_set_key(dev, idx, idx, alg, zero, 0, key);
+			rtl92e_set_swcam(dev, idx, idx, alg, zero, 0, key, 0);
+		} else if (group) {
+			ieee->group_key_type = alg;
+			rtl92e_set_key(dev, idx, idx, alg, broadcast_addr, 0,
+				       key);
+			rtl92e_set_swcam(dev, idx, idx, alg, broadcast_addr, 0,
+					 key, 0);
+		} else {
+			if ((ieee->pairwise_key_type == KEY_TYPE_CCMP) &&
+			     ieee->pHTInfo->bCurrentHTSupport)
+				rtl92e_writeb(dev, 0x173, 1);
+			rtl92e_set_key(dev, 4, idx, alg,
+				       (u8 *)ieee->ap_mac_addr, 0, key);
+			rtl92e_set_swcam(dev, 4, idx, alg,
+					 (u8 *)ieee->ap_mac_addr, 0, key, 0);
+		}
+	}
+
+end_hw_sec:
+	priv->rtllib->wx_set_enc = 0;
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_set_auth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *data, char *extra)
+{
+	int ret = 0;
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = rtllib_wx_set_auth(priv->rtllib, info, &data->param, extra);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_set_mlme(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = rtllib_wx_set_mlme(priv->rtllib, info, wrqu, extra);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_set_gen_ie(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *data, char *extra)
+{
+	int ret = 0;
+
+	struct r8192_priv *priv = rtllib_priv(dev);
+
+	if (priv->hw_radio_off)
+		return 0;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = rtllib_wx_set_gen_ie(priv->rtllib, extra, data->data.length);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int _rtl92e_wx_get_gen_ie(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *data, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	if (ieee->wpa_ie_len == 0 || ieee->wpa_ie == NULL) {
+		data->data.length = 0;
+		return 0;
+	}
+
+	if (data->data.length < ieee->wpa_ie_len)
+		return -E2BIG;
+
+	data->data.length = ieee->wpa_ie_len;
+	memcpy(extra, ieee->wpa_ie, ieee->wpa_ie_len);
+	return ret;
+}
+
+#define OID_RT_INTEL_PROMISCUOUS_MODE	0xFF0101F6
+
+static int _rtl92e_wx_set_promisc_mode(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	u32 info_buf[3];
+
+	u32 oid;
+	u32 bPromiscuousOn;
+	u32 bFilterSourceStationFrame;
+
+	if (copy_from_user(info_buf, wrqu->data.pointer, sizeof(info_buf)))
+		return -EFAULT;
+
+	oid = info_buf[0];
+	bPromiscuousOn = info_buf[1];
+	bFilterSourceStationFrame = info_buf[2];
+
+	if (oid == OID_RT_INTEL_PROMISCUOUS_MODE) {
+		ieee->IntelPromiscuousModeInfo.bPromiscuousOn =
+					(bPromiscuousOn) ? (true) : (false);
+		ieee->IntelPromiscuousModeInfo.bFilterSourceStationFrame =
+			(bFilterSourceStationFrame) ? (true) : (false);
+		(bPromiscuousOn) ?
+		(rtllib_EnableIntelPromiscuousMode(dev, false)) :
+		(rtllib_DisableIntelPromiscuousMode(dev, false));
+
+		netdev_info(dev,
+			    "=======>%s(), on = %d, filter src sta = %d\n",
+			    __func__, bPromiscuousOn,
+			    bFilterSourceStationFrame);
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int _rtl92e_wx_get_promisc_mode(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+
+	mutex_lock(&priv->wx_mutex);
+
+	snprintf(extra, 45, "PromiscuousMode:%d, FilterSrcSTAFrame:%d",
+		 ieee->IntelPromiscuousModeInfo.bPromiscuousOn,
+		 ieee->IntelPromiscuousModeInfo.bFilterSourceStationFrame);
+	wrqu->data.length = strlen(extra) + 1;
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return 0;
+}
+
+#define IW_IOCTL(x) ((x) - SIOCSIWCOMMIT)
+static iw_handler r8192_wx_handlers[] = {
+	[IW_IOCTL(SIOCGIWNAME)] = _rtl92e_wx_get_name,
+	[IW_IOCTL(SIOCSIWFREQ)] = _rtl92e_wx_set_freq,
+	[IW_IOCTL(SIOCGIWFREQ)] = _rtl92e_wx_get_freq,
+	[IW_IOCTL(SIOCSIWMODE)] = _rtl92e_wx_set_mode,
+	[IW_IOCTL(SIOCGIWMODE)] = _rtl92e_wx_get_mode,
+	[IW_IOCTL(SIOCSIWSENS)] = _rtl92e_wx_set_sens,
+	[IW_IOCTL(SIOCGIWSENS)] = _rtl92e_wx_get_sens,
+	[IW_IOCTL(SIOCGIWRANGE)] = _rtl92e_wx_get_range,
+	[IW_IOCTL(SIOCSIWAP)] = _rtl92e_wx_set_wap,
+	[IW_IOCTL(SIOCGIWAP)] = _rtl92e_wx_get_wap,
+	[IW_IOCTL(SIOCSIWSCAN)] = _rtl92e_wx_set_scan,
+	[IW_IOCTL(SIOCGIWSCAN)] = _rtl92e_wx_get_scan,
+	[IW_IOCTL(SIOCSIWESSID)] = _rtl92e_wx_set_essid,
+	[IW_IOCTL(SIOCGIWESSID)] = _rtl92e_wx_get_essid,
+	[IW_IOCTL(SIOCSIWNICKN)] = _rtl92e_wx_set_nick,
+	[IW_IOCTL(SIOCGIWNICKN)] = _rtl92e_wx_get_nick,
+	[IW_IOCTL(SIOCSIWRATE)] = _rtl92e_wx_set_rate,
+	[IW_IOCTL(SIOCGIWRATE)] = _rtl92e_wx_get_rate,
+	[IW_IOCTL(SIOCSIWRTS)] = _rtl92e_wx_set_rts,
+	[IW_IOCTL(SIOCGIWRTS)] = _rtl92e_wx_get_rts,
+	[IW_IOCTL(SIOCSIWFRAG)] = _rtl92e_wx_set_frag,
+	[IW_IOCTL(SIOCGIWFRAG)] = _rtl92e_wx_get_frag,
+	[IW_IOCTL(SIOCSIWRETRY)] = _rtl92e_wx_set_retry,
+	[IW_IOCTL(SIOCGIWRETRY)] = _rtl92e_wx_get_retry,
+	[IW_IOCTL(SIOCSIWENCODE)] = _rtl92e_wx_set_enc,
+	[IW_IOCTL(SIOCGIWENCODE)] = _rtl92e_wx_get_enc,
+	[IW_IOCTL(SIOCSIWPOWER)] = _rtl92e_wx_set_power,
+	[IW_IOCTL(SIOCGIWPOWER)] = _rtl92e_wx_get_power,
+	[IW_IOCTL(SIOCSIWGENIE)] = _rtl92e_wx_set_gen_ie,
+	[IW_IOCTL(SIOCGIWGENIE)] = _rtl92e_wx_get_gen_ie,
+	[IW_IOCTL(SIOCSIWMLME)] = _rtl92e_wx_set_mlme,
+	[IW_IOCTL(SIOCSIWAUTH)] = _rtl92e_wx_set_auth,
+	[IW_IOCTL(SIOCSIWENCODEEXT)] = _rtl92e_wx_set_encode_ext,
+};
+
+/* the following rule need to be following,
+ * Odd : get (world access),
+ * even : set (root access)
+ */
+static const struct iw_priv_args r8192_private_args[] = {
+	{
+		SIOCIWFIRSTPRIV + 0x0,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_debugflag"
+	}, {
+		SIOCIWFIRSTPRIV + 0x1,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "activescan"
+	}, {
+		SIOCIWFIRSTPRIV + 0x2,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "rawtx"
+	}, {
+		SIOCIWFIRSTPRIV + 0x3,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "forcereset"
+	}, {
+		SIOCIWFIRSTPRIV + 0x6,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, IW_PRIV_TYPE_NONE,
+		"set_power"
+	}, {
+		SIOCIWFIRSTPRIV + 0xa,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, IW_PRIV_TYPE_NONE,
+		"lps_interv"
+	}, {
+		SIOCIWFIRSTPRIV + 0xb,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, IW_PRIV_TYPE_NONE,
+		"lps_force"
+	}, {
+		SIOCIWFIRSTPRIV + 0x16,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 3, 0, "setpromisc"
+	}, {
+		SIOCIWFIRSTPRIV + 0x17,
+		0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 45, "getpromisc"
+	}
+
+};
+
+static iw_handler r8192_private_handler[] = {
+	(iw_handler)_rtl92e_wx_set_debug,   /*SIOCIWSECONDPRIV*/
+	(iw_handler)_rtl92e_wx_set_scan_type,
+	(iw_handler)_rtl92e_wx_set_rawtx,
+	(iw_handler)_rtl92e_wx_force_reset,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)_rtl92e_wx_adapter_power_status,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)_rtl92e_wx_set_lps_awake_interval,
+	(iw_handler)_rtl92e_wx_set_force_lps,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
+	(iw_handler)_rtl92e_wx_set_promisc_mode,
+	(iw_handler)_rtl92e_wx_get_promisc_mode,
+};
+
+static struct iw_statistics *_rtl92e_get_wireless_stats(struct net_device *dev)
+{
+	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rtllib_device *ieee = priv->rtllib;
+	struct iw_statistics *wstats = &priv->wstats;
+	int tmp_level = 0;
+	int tmp_qual = 0;
+	int tmp_noise = 0;
+
+	if (ieee->state < RTLLIB_LINKED) {
+		wstats->qual.qual = 10;
+		wstats->qual.level = 0;
+		wstats->qual.noise = 0x100 - 100;	/* -100 dBm */
+		wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+		return wstats;
+	}
+
+	tmp_level = (&ieee->current_network)->stats.rssi;
+	tmp_qual = (&ieee->current_network)->stats.signal;
+	tmp_noise = (&ieee->current_network)->stats.noise;
+
+	wstats->qual.level = tmp_level;
+	wstats->qual.qual = tmp_qual;
+	wstats->qual.noise = tmp_noise;
+	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+	return wstats;
+}
+
+const struct iw_handler_def r8192_wx_handlers_def = {
+	.standard = r8192_wx_handlers,
+	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
+	.private = r8192_private_handler,
+	.num_private = ARRAY_SIZE(r8192_private_handler),
+	.num_private_args = sizeof(r8192_private_args) /
+			    sizeof(struct iw_priv_args),
+	.get_wireless_stats = _rtl92e_get_wireless_stats,
+	.private_args = (struct iw_priv_args *)r8192_private_args,
+};
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
new file mode 100644
index 000000000..d70a747ac
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef R819x_WX_H
+#define R819x_WX_H
+
+struct iw_handler_def;
+
+extern const struct iw_handler_def r8192_wx_handlers_def;
+#endif
diff --git a/drivers/staging/rtl8192e/rtl819x_BA.h b/drivers/staging/rtl8192e/rtl819x_BA.h
new file mode 100644
index 000000000..8a35d7a3e
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_BA.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _BATYPE_H_
+#define _BATYPE_H_
+
+#define	BA_SETUP_TIMEOUT	200
+
+#define	BA_POLICY_DELAYED		0
+#define	BA_POLICY_IMMEDIATE	1
+
+#define	ADDBA_STATUS_SUCCESS			0
+#define	ADDBA_STATUS_REFUSED		37
+#define	ADDBA_STATUS_INVALID_PARAM	38
+
+#define	DELBA_REASON_END_BA			37
+#define	DELBA_REASON_UNKNOWN_BA	38
+#define	DELBA_REASON_TIMEOUT			39
+union sequence_control {
+	u16 short_data;
+	struct {
+		u16	frag_num:4;
+		u16	seq_num:12;
+	} field;
+};
+
+union ba_param_set {
+	u8 char_data[2];
+	u16 short_data;
+	struct {
+		u16 amsdu_support:1;
+		u16 ba_policy:1;
+		u16 tid:4;
+		u16 buffer_size:10;
+	} field;
+};
+
+union delba_param_set {
+	u8 char_data[2];
+	u16 short_data;
+	struct {
+		u16 reserved:11;
+		u16 initiator:1;
+		u16 tid:4;
+	} field;
+};
+
+struct ba_record {
+	struct timer_list timer;
+	u8 b_valid;
+	u8 dialog_token;
+	union ba_param_set ba_param_set;
+	u16 ba_timeout_value;
+	union sequence_control ba_start_seq_ctrl;
+};
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
new file mode 100644
index 000000000..19d13b3fc
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -0,0 +1,546 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include <asm/byteorder.h>
+#include <asm/unaligned.h>
+#include <linux/etherdevice.h>
+#include "rtllib.h"
+#include "rtl819x_BA.h"
+
+static void ActivateBAEntry(struct ba_record *pBA, u16 Time)
+{
+	pBA->b_valid = true;
+	if (Time != 0)
+		mod_timer(&pBA->timer, jiffies + msecs_to_jiffies(Time));
+}
+
+static void DeActivateBAEntry(struct rtllib_device *ieee, struct ba_record *pBA)
+{
+	pBA->b_valid = false;
+	del_timer_sync(&pBA->timer);
+}
+
+static u8 TxTsDeleteBA(struct rtllib_device *ieee, struct tx_ts_record *pTxTs)
+{
+	struct ba_record *pAdmittedBa = &pTxTs->TxAdmittedBARecord;
+	struct ba_record *pPendingBa = &pTxTs->TxPendingBARecord;
+	u8 bSendDELBA = false;
+
+	if (pPendingBa->b_valid) {
+		DeActivateBAEntry(ieee, pPendingBa);
+		bSendDELBA = true;
+	}
+
+	if (pAdmittedBa->b_valid) {
+		DeActivateBAEntry(ieee, pAdmittedBa);
+		bSendDELBA = true;
+	}
+	return bSendDELBA;
+}
+
+static u8 RxTsDeleteBA(struct rtllib_device *ieee, struct rx_ts_record *pRxTs)
+{
+	struct ba_record *pBa = &pRxTs->rx_admitted_ba_record;
+	u8			bSendDELBA = false;
+
+	if (pBa->b_valid) {
+		DeActivateBAEntry(ieee, pBa);
+		bSendDELBA = true;
+	}
+
+	return bSendDELBA;
+}
+
+void ResetBaEntry(struct ba_record *pBA)
+{
+	pBA->b_valid			  = false;
+	pBA->ba_param_set.short_data	  = 0;
+	pBA->ba_timeout_value		  = 0;
+	pBA->dialog_token		  = 0;
+	pBA->ba_start_seq_ctrl.short_data = 0;
+}
+static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
+				    struct ba_record *pBA,
+				    u16 StatusCode, u8 type)
+{
+	struct sk_buff *skb = NULL;
+	struct rtllib_hdr_3addr *BAReq = NULL;
+	u8 *tag = NULL;
+	u16 len = ieee->tx_headroom + 9;
+
+	netdev_dbg(ieee->dev, "%s(): frame(%d) sentd to: %pM, ieee->dev:%p\n",
+		   __func__, type, Dst, ieee->dev);
+
+	if (!pBA) {
+		netdev_warn(ieee->dev, "pBA is NULL\n");
+		return NULL;
+	}
+	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
+	if (!skb)
+		return NULL;
+
+	memset(skb->data, 0, sizeof(struct rtllib_hdr_3addr));
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	BAReq = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
+
+	ether_addr_copy(BAReq->addr1, Dst);
+	ether_addr_copy(BAReq->addr2, ieee->dev->dev_addr);
+
+	ether_addr_copy(BAReq->addr3, ieee->current_network.bssid);
+	BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
+
+	tag = skb_put(skb, 9);
+	*tag++ = ACT_CAT_BA;
+	*tag++ = type;
+	*tag++ = pBA->dialog_token;
+
+	if (type == ACT_ADDBARSP) {
+		put_unaligned_le16(StatusCode, tag);
+		tag += 2;
+	}
+
+	put_unaligned_le16(pBA->ba_param_set.short_data, tag);
+	tag += 2;
+
+	put_unaligned_le16(pBA->ba_timeout_value, tag);
+	tag += 2;
+
+	if (type == ACT_ADDBAREQ) {
+		memcpy(tag, (u8 *)&(pBA->ba_start_seq_ctrl), 2);
+		tag += 2;
+	}
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", DUMP_PREFIX_NONE, skb->data,
+			     __func__, skb->len);
+#endif
+	return skb;
+}
+
+static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
+				    struct ba_record *pBA,
+				    enum tr_select TxRxSelect, u16 ReasonCode)
+{
+	union delba_param_set DelbaParamSet;
+	struct sk_buff *skb = NULL;
+	struct rtllib_hdr_3addr *Delba = NULL;
+	u8 *tag = NULL;
+	u16 len = 6 + ieee->tx_headroom;
+
+	if (net_ratelimit())
+		netdev_dbg(ieee->dev, "%s(): ReasonCode(%d) sentd to: %pM\n",
+			   __func__, ReasonCode, dst);
+
+	memset(&DelbaParamSet, 0, 2);
+
+	DelbaParamSet.field.initiator = (TxRxSelect == TX_DIR) ? 1 : 0;
+	DelbaParamSet.field.tid	= pBA->ba_param_set.field.tid;
+
+	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	Delba = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
+
+	ether_addr_copy(Delba->addr1, dst);
+	ether_addr_copy(Delba->addr2, ieee->dev->dev_addr);
+	ether_addr_copy(Delba->addr3, ieee->current_network.bssid);
+	Delba->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
+
+	tag = skb_put(skb, 6);
+
+	*tag++ = ACT_CAT_BA;
+	*tag++ = ACT_DELBA;
+
+
+	put_unaligned_le16(DelbaParamSet.short_data, tag);
+	tag += 2;
+
+	put_unaligned_le16(ReasonCode, tag);
+	tag += 2;
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", DUMP_PREFIX_NONE, skb->data,
+			     __func__, skb->len);
+#endif
+	return skb;
+}
+
+static void rtllib_send_ADDBAReq(struct rtllib_device *ieee, u8 *dst,
+				 struct ba_record *pBA)
+{
+	struct sk_buff *skb;
+
+	skb = rtllib_ADDBA(ieee, dst, pBA, 0, ACT_ADDBAREQ);
+
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+	} else {
+		netdev_dbg(ieee->dev, "Failed to generate ADDBAReq packet.\n");
+	}
+}
+
+static void rtllib_send_ADDBARsp(struct rtllib_device *ieee, u8 *dst,
+				 struct ba_record *pBA, u16 StatusCode)
+{
+	struct sk_buff *skb;
+
+	skb = rtllib_ADDBA(ieee, dst, pBA, StatusCode, ACT_ADDBARSP);
+	if (skb)
+		softmac_mgmt_xmit(skb, ieee);
+	else
+		netdev_dbg(ieee->dev, "Failed to generate ADDBARsp packet.\n");
+}
+
+static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
+			      struct ba_record *pBA, enum tr_select TxRxSelect,
+			      u16 ReasonCode)
+{
+	struct sk_buff *skb;
+
+	skb = rtllib_DELBA(ieee, dst, pBA, TxRxSelect, ReasonCode);
+	if (skb)
+		softmac_mgmt_xmit(skb, ieee);
+	else
+		netdev_dbg(ieee->dev, "Failed to generate DELBA packet.\n");
+}
+
+int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	struct rtllib_hdr_3addr *req = NULL;
+	u16 rc = 0;
+	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
+	struct ba_record *pBA = NULL;
+	union ba_param_set *pBaParamSet = NULL;
+	u16 *pBaTimeoutVal = NULL;
+	union sequence_control *pBaStartSeqCtrl = NULL;
+	struct rx_ts_record *pTS = NULL;
+
+	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
+		netdev_warn(ieee->dev, "Invalid skb len in BAREQ(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 9));
+		return -1;
+	}
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", DUMP_PREFIX_NONE, __func__,
+			     skb->data, skb->len);
+#endif
+
+	req = (struct rtllib_hdr_3addr *)skb->data;
+	tag = (u8 *)req;
+	dst = (u8 *)(&req->addr2[0]);
+	tag += sizeof(struct rtllib_hdr_3addr);
+	pDialogToken = tag + 2;
+	pBaParamSet = (union ba_param_set *)(tag + 3);
+	pBaTimeoutVal = (u16 *)(tag + 5);
+	pBaStartSeqCtrl = (union sequence_control *)(req + 7);
+
+	if (!ieee->current_network.qos_data.active ||
+	    !ieee->pHTInfo->bCurrentHTSupport ||
+	    (ieee->pHTInfo->iot_action & HT_IOT_ACT_REJECT_ADDBA_REQ)) {
+		rc = ADDBA_STATUS_REFUSED;
+		netdev_warn(ieee->dev,
+			    "Failed to reply on ADDBA_REQ as some capability is not ready(%d, %d)\n",
+			    ieee->current_network.qos_data.active,
+			    ieee->pHTInfo->bCurrentHTSupport);
+		goto OnADDBAReq_Fail;
+	}
+	if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
+	    (u8)(pBaParamSet->field.tid), RX_DIR, true)) {
+		rc = ADDBA_STATUS_REFUSED;
+		netdev_warn(ieee->dev, "%s(): can't get TS\n", __func__);
+		goto OnADDBAReq_Fail;
+	}
+	pBA = &pTS->rx_admitted_ba_record;
+
+	if (pBaParamSet->field.ba_policy == BA_POLICY_DELAYED) {
+		rc = ADDBA_STATUS_INVALID_PARAM;
+		netdev_warn(ieee->dev, "%s(): BA Policy is not correct\n",
+			    __func__);
+		goto OnADDBAReq_Fail;
+	}
+
+	rtllib_FlushRxTsPendingPkts(ieee, pTS);
+
+	DeActivateBAEntry(ieee, pBA);
+	pBA->dialog_token = *pDialogToken;
+	pBA->ba_param_set = *pBaParamSet;
+	pBA->ba_timeout_value = *pBaTimeoutVal;
+	pBA->ba_start_seq_ctrl = *pBaStartSeqCtrl;
+
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev) ||
+	   (ieee->pHTInfo->iot_action & HT_IOT_ACT_ALLOW_PEER_AGG_ONE_PKT))
+		pBA->ba_param_set.field.buffer_size = 1;
+	else
+		pBA->ba_param_set.field.buffer_size = 32;
+
+	ActivateBAEntry(pBA, 0);
+	rtllib_send_ADDBARsp(ieee, dst, pBA, ADDBA_STATUS_SUCCESS);
+
+	return 0;
+
+OnADDBAReq_Fail:
+	{
+		struct ba_record BA;
+
+		BA.ba_param_set = *pBaParamSet;
+		BA.ba_timeout_value = *pBaTimeoutVal;
+		BA.dialog_token = *pDialogToken;
+		BA.ba_param_set.field.ba_policy = BA_POLICY_IMMEDIATE;
+		rtllib_send_ADDBARsp(ieee, dst, &BA, rc);
+		return 0;
+	}
+}
+
+int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	struct rtllib_hdr_3addr *rsp = NULL;
+	struct ba_record *pPendingBA, *pAdmittedBA;
+	struct tx_ts_record *pTS = NULL;
+	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
+	u16 *pStatusCode = NULL, *pBaTimeoutVal = NULL;
+	union ba_param_set *pBaParamSet = NULL;
+	u16			ReasonCode;
+
+	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
+		netdev_warn(ieee->dev, "Invalid skb len in BARSP(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 9));
+		return -1;
+	}
+	rsp = (struct rtllib_hdr_3addr *)skb->data;
+	tag = (u8 *)rsp;
+	dst = (u8 *)(&rsp->addr2[0]);
+	tag += sizeof(struct rtllib_hdr_3addr);
+	pDialogToken = tag + 2;
+	pStatusCode = (u16 *)(tag + 3);
+	pBaParamSet = (union ba_param_set *)(tag + 5);
+	pBaTimeoutVal = (u16 *)(tag + 7);
+
+	if (!ieee->current_network.qos_data.active ||
+	    !ieee->pHTInfo->bCurrentHTSupport ||
+	    !ieee->pHTInfo->bCurrentAMPDUEnable) {
+		netdev_warn(ieee->dev,
+			    "reject to ADDBA_RSP as some capability is not ready(%d, %d, %d)\n",
+			    ieee->current_network.qos_data.active,
+			    ieee->pHTInfo->bCurrentHTSupport,
+			    ieee->pHTInfo->bCurrentAMPDUEnable);
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	}
+
+	if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
+		   (u8)(pBaParamSet->field.tid), TX_DIR, false)) {
+		netdev_warn(ieee->dev, "%s(): can't get TS\n", __func__);
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	}
+
+	pTS->bAddBaReqInProgress = false;
+	pPendingBA = &pTS->TxPendingBARecord;
+	pAdmittedBA = &pTS->TxAdmittedBARecord;
+
+	if (pAdmittedBA->b_valid) {
+		netdev_dbg(ieee->dev, "%s(): ADDBA response already admitted\n",
+			   __func__);
+		return -1;
+	} else if (!pPendingBA->b_valid ||
+		   (*pDialogToken != pPendingBA->dialog_token)) {
+		netdev_warn(ieee->dev,
+			    "%s(): ADDBA Rsp. BA invalid, DELBA!\n",
+			    __func__);
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	} else {
+		netdev_dbg(ieee->dev,
+			   "%s(): Recv ADDBA Rsp. BA is admitted! Status code:%X\n",
+			   __func__, *pStatusCode);
+		DeActivateBAEntry(ieee, pPendingBA);
+	}
+
+	if (*pStatusCode == ADDBA_STATUS_SUCCESS) {
+		if (pBaParamSet->field.ba_policy == BA_POLICY_DELAYED) {
+			pTS->bAddBaReqDelayed = true;
+			DeActivateBAEntry(ieee, pAdmittedBA);
+			ReasonCode = DELBA_REASON_END_BA;
+			goto OnADDBARsp_Reject;
+		}
+
+
+		pAdmittedBA->dialog_token = *pDialogToken;
+		pAdmittedBA->ba_timeout_value = *pBaTimeoutVal;
+		pAdmittedBA->ba_start_seq_ctrl = pPendingBA->ba_start_seq_ctrl;
+		pAdmittedBA->ba_param_set = *pBaParamSet;
+		DeActivateBAEntry(ieee, pAdmittedBA);
+		ActivateBAEntry(pAdmittedBA, *pBaTimeoutVal);
+	} else {
+		pTS->bAddBaReqDelayed = true;
+		pTS->bDisable_AddBa = true;
+		ReasonCode = DELBA_REASON_END_BA;
+		goto OnADDBARsp_Reject;
+	}
+
+	return 0;
+
+OnADDBARsp_Reject:
+	{
+		struct ba_record BA;
+
+		BA.ba_param_set = *pBaParamSet;
+		rtllib_send_DELBA(ieee, dst, &BA, TX_DIR, ReasonCode);
+		return 0;
+	}
+}
+
+int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	struct rtllib_hdr_3addr *delba = NULL;
+	union delba_param_set *pDelBaParamSet = NULL;
+	u8 *dst = NULL;
+
+	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 6) {
+		netdev_warn(ieee->dev, "Invalid skb len in DELBA(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 6));
+		return -1;
+	}
+
+	if (!ieee->current_network.qos_data.active ||
+		!ieee->pHTInfo->bCurrentHTSupport) {
+		netdev_warn(ieee->dev,
+			    "received DELBA while QOS or HT is not supported(%d, %d)\n",
+			    ieee->current_network. qos_data.active,
+			    ieee->pHTInfo->bCurrentHTSupport);
+		return -1;
+	}
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", DUMP_PREFIX_NONE, skb->data,
+			     __func__, skb->len);
+#endif
+	delba = (struct rtllib_hdr_3addr *)skb->data;
+	dst = (u8 *)(&delba->addr2[0]);
+	pDelBaParamSet = (union delba_param_set *)&delba->payload[2];
+
+	if (pDelBaParamSet->field.initiator == 1) {
+		struct rx_ts_record *pRxTs;
+
+		if (!GetTs(ieee, (struct ts_common_info **)&pRxTs, dst,
+		    (u8)pDelBaParamSet->field.tid, RX_DIR, false)) {
+			netdev_warn(ieee->dev,
+				    "%s(): can't get TS for RXTS. dst:%pM TID:%d\n",
+				    __func__, dst,
+				    (u8)pDelBaParamSet->field.tid);
+			return -1;
+		}
+
+		RxTsDeleteBA(ieee, pRxTs);
+	} else {
+		struct tx_ts_record *pTxTs;
+
+		if (!GetTs(ieee, (struct ts_common_info **)&pTxTs, dst,
+			   (u8)pDelBaParamSet->field.tid, TX_DIR, false)) {
+			netdev_warn(ieee->dev, "%s(): can't get TS for TXTS\n",
+				    __func__);
+			return -1;
+		}
+
+		pTxTs->bUsingBa = false;
+		pTxTs->bAddBaReqInProgress = false;
+		pTxTs->bAddBaReqDelayed = false;
+		del_timer_sync(&pTxTs->TsAddBaTimer);
+		TxTsDeleteBA(ieee, pTxTs);
+	}
+	return 0;
+}
+
+void TsInitAddBA(struct rtllib_device *ieee, struct tx_ts_record *pTS,
+		 u8 Policy, u8	bOverwritePending)
+{
+	struct ba_record *pBA = &pTS->TxPendingBARecord;
+
+	if (pBA->b_valid && !bOverwritePending)
+		return;
+
+	DeActivateBAEntry(ieee, pBA);
+
+	pBA->dialog_token++;
+	pBA->ba_param_set.field.amsdu_support = 0;
+	pBA->ba_param_set.field.ba_policy = Policy;
+	pBA->ba_param_set.field.tid = pTS->TsCommonInfo.TSpec.f.TSInfo.field.ucTSID;
+	pBA->ba_param_set.field.buffer_size = 32;
+	pBA->ba_timeout_value = 0;
+	pBA->ba_start_seq_ctrl.field.seq_num = (pTS->TxCurSeq + 3) % 4096;
+
+	ActivateBAEntry(pBA, BA_SETUP_TIMEOUT);
+
+	rtllib_send_ADDBAReq(ieee, pTS->TsCommonInfo.Addr, pBA);
+}
+
+void TsInitDelBA(struct rtllib_device *ieee,
+		 struct ts_common_info *pTsCommonInfo,
+		 enum tr_select TxRxSelect)
+{
+	if (TxRxSelect == TX_DIR) {
+		struct tx_ts_record *pTxTs =
+			 (struct tx_ts_record *)pTsCommonInfo;
+
+		if (TxTsDeleteBA(ieee, pTxTs))
+			rtllib_send_DELBA(ieee, pTsCommonInfo->Addr,
+					  (pTxTs->TxAdmittedBARecord.b_valid) ?
+					 (&pTxTs->TxAdmittedBARecord) :
+					(&pTxTs->TxPendingBARecord),
+					 TxRxSelect, DELBA_REASON_END_BA);
+	} else if (TxRxSelect == RX_DIR) {
+		struct rx_ts_record *pRxTs =
+				 (struct rx_ts_record *)pTsCommonInfo;
+		if (RxTsDeleteBA(ieee, pRxTs))
+			rtllib_send_DELBA(ieee, pTsCommonInfo->Addr,
+					  &pRxTs->rx_admitted_ba_record,
+					  TxRxSelect, DELBA_REASON_END_BA);
+	}
+}
+
+void BaSetupTimeOut(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t,
+					      TxPendingBARecord.timer);
+
+	pTxTs->bAddBaReqInProgress = false;
+	pTxTs->bAddBaReqDelayed = true;
+	pTxTs->TxPendingBARecord.b_valid = false;
+}
+
+void TxBaInactTimeout(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t,
+					      TxAdmittedBARecord.timer);
+	struct rtllib_device *ieee = container_of(pTxTs, struct rtllib_device,
+				     TxTsRecord[pTxTs->num]);
+	TxTsDeleteBA(ieee, pTxTs);
+	rtllib_send_DELBA(ieee, pTxTs->TsCommonInfo.Addr,
+			  &pTxTs->TxAdmittedBARecord, TX_DIR,
+			  DELBA_REASON_TIMEOUT);
+}
+
+void RxBaInactTimeout(struct timer_list *t)
+{
+	struct rx_ts_record *pRxTs = from_timer(pRxTs, t,
+					      rx_admitted_ba_record.timer);
+	struct rtllib_device *ieee = container_of(pRxTs, struct rtllib_device,
+				     RxTsRecord[pRxTs->num]);
+
+	RxTsDeleteBA(ieee, pRxTs);
+	rtllib_send_DELBA(ieee, pRxTs->ts_common_info.Addr,
+			  &pRxTs->rx_admitted_ba_record, RX_DIR,
+			  DELBA_REASON_TIMEOUT);
+}
diff --git a/drivers/staging/rtl8192e/rtl819x_HT.h b/drivers/staging/rtl8192e/rtl819x_HT.h
new file mode 100644
index 000000000..76bc9c5a6
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_HT.h
@@ -0,0 +1,266 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL819XU_HTTYPE_H_
+#define _RTL819XU_HTTYPE_H_
+
+#define MIMO_PS_STATIC				0
+
+#define sHTCLng	4
+
+enum ht_channel_width {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_20_40 = 1,
+};
+
+enum ht_extchnl_offset {
+	HT_EXTCHNL_OFFSET_NO_EXT = 0,
+	HT_EXTCHNL_OFFSET_UPPER = 1,
+	HT_EXTCHNL_OFFSET_NO_DEF = 2,
+	HT_EXTCHNL_OFFSET_LOWER = 3,
+};
+
+struct ht_capab_ele {
+
+	u8	AdvCoding:1;
+	u8	ChlWidth:1;
+	u8	MimoPwrSave:2;
+	u8	GreenField:1;
+	u8	ShortGI20Mhz:1;
+	u8	ShortGI40Mhz:1;
+	u8	TxSTBC:1;
+	u8	RxSTBC:2;
+	u8	DelayBA:1;
+	u8	MaxAMSDUSize:1;
+	u8	DssCCk:1;
+	u8	PSMP:1;
+	u8	Rsvd1:1;
+	u8	LSigTxopProtect:1;
+
+	u8	MaxRxAMPDUFactor:2;
+	u8	MPDUDensity:3;
+	u8	Rsvd2:3;
+
+	u8	MCS[16];
+
+
+	u16	ExtHTCapInfo;
+
+	u8	TxBFCap[4];
+
+	u8	ASCap;
+
+} __packed;
+
+
+struct ht_info_ele {
+	u8	ControlChl;
+
+	u8	ExtChlOffset:2;
+	u8	RecommemdedTxWidth:1;
+	u8	RIFS:1;
+	u8	PSMPAccessOnly:1;
+	u8	SrvIntGranularity:3;
+
+	u8	OptMode:2;
+	u8	NonGFDevPresent:1;
+	u8	Revd1:5;
+	u8	Revd2:8;
+
+	u8	Rsvd3:6;
+	u8	DualBeacon:1;
+	u8	DualCTSProtect:1;
+
+	u8	SecondaryBeacon:1;
+	u8	LSigTxopProtectFull:1;
+	u8	PcoActive:1;
+	u8	PcoPhase:1;
+	u8	Rsvd4:4;
+
+	u8	BasicMSC[16];
+} __packed;
+
+enum ht_spec_ver {
+	HT_SPEC_VER_IEEE = 0,
+	HT_SPEC_VER_EWC = 1,
+};
+
+enum ht_aggre_mode {
+	HT_AGG_AUTO = 0,
+	HT_AGG_FORCE_ENABLE = 1,
+	HT_AGG_FORCE_DISABLE = 2,
+};
+
+
+struct rt_hi_throughput {
+	u8				bEnableHT;
+	u8				bCurrentHTSupport;
+
+	u8				bRegBW40MHz;
+	u8				bCurBW40MHz;
+
+	u8				bRegShortGI40MHz;
+	u8				bCurShortGI40MHz;
+
+	u8				bRegShortGI20MHz;
+	u8				bCurShortGI20MHz;
+
+	u8				bRegSuppCCK;
+	u8				bCurSuppCCK;
+
+	enum ht_spec_ver ePeerHTSpecVer;
+
+
+	struct ht_capab_ele SelfHTCap;
+	struct ht_info_ele SelfHTInfo;
+
+	u8				PeerHTCapBuf[32];
+	u8				PeerHTInfoBuf[32];
+
+
+	u8				bAMSDU_Support;
+	u16				nAMSDU_MaxSize;
+	u8				bCurrent_AMSDU_Support;
+	u16				nCurrent_AMSDU_MaxSize;
+
+	u8				bAMPDUEnable;
+	u8				bCurrentAMPDUEnable;
+	u8				AMPDU_Factor;
+	u8				CurrentAMPDUFactor;
+	u8				MPDU_Density;
+	u8 current_mpdu_density;
+
+	enum ht_aggre_mode ForcedAMPDUMode;
+	u8 forced_ampdu_factor;
+	u8 forced_mpdu_density;
+
+	enum ht_aggre_mode ForcedAMSDUMode;
+	u8 forced_short_gi;
+
+	u8 current_op_mode;
+
+	u8 self_mimo_ps;
+	u8 peer_mimo_ps;
+
+	enum ht_extchnl_offset CurSTAExtChnlOffset;
+	u8 cur_tx_bw40mhz;
+	u8 sw_bw_in_progress;
+	u8 reg_rt2rt_aggregation;
+	u8				RT2RT_HT_Mode;
+	u8 current_rt2rt_aggregation;
+	u8 current_rt2rt_long_slot_time;
+	u8 sz_rt2rt_agg_buf[10];
+
+	u8 reg_rx_reorder_enable;
+	u8 cur_rx_reorder_enable;
+	u8 rx_reorder_win_size;
+	u8 rx_reorder_pending_time;
+	u16 rx_reorder_drop_counter;
+	u8				IOTPeer;
+	u32 iot_action;
+	u8 iot_ra_func;
+
+	u8	bWAIotBroadcom;
+	u8	WAIotTH;
+} __packed;
+
+struct bss_ht {
+	u8 bd_support_ht;
+
+	u8 bd_ht_cap_buf[32];
+	u16 bd_ht_cap_len;
+	u8 bd_ht_info_buf[32];
+	u16 bd_ht_info_len;
+
+	enum ht_spec_ver bd_ht_spec_ver;
+	enum ht_channel_width bd_bandwidth;
+
+	u8 bd_rt2rt_aggregation;
+	u8 bd_rt2rt_long_slot_time;
+	u8 rt2rt_ht_mode;
+	u8 bd_ht_1r;
+};
+
+extern u8 MCS_FILTER_ALL[16];
+extern u8 MCS_FILTER_1SS[16];
+
+#define	RATE_ADPT_1SS_MASK		0xFF
+#define	RATE_ADPT_2SS_MASK		0xF0
+#define	RATE_ADPT_MCS32_MASK		0x01
+
+enum ht_aggre_size {
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+};
+
+enum ht_iot_peer {
+	HT_IOT_PEER_UNKNOWN = 0,
+	HT_IOT_PEER_REALTEK = 1,
+	HT_IOT_PEER_REALTEK_92SE = 2,
+	HT_IOT_PEER_BROADCOM = 3,
+	HT_IOT_PEER_RALINK = 4,
+	HT_IOT_PEER_ATHEROS = 5,
+	HT_IOT_PEER_CISCO = 6,
+	HT_IOT_PEER_MARVELL = 7,
+	HT_IOT_PEER_92U_SOFTAP = 8,
+	HT_IOT_PEER_SELF_SOFTAP = 9,
+	HT_IOT_PEER_AIRGO = 10,
+	HT_IOT_PEER_MAX = 11,
+};
+
+enum ht_iot_action {
+	HT_IOT_ACT_TX_USE_AMSDU_4K = 0x00000001,
+	HT_IOT_ACT_TX_USE_AMSDU_8K = 0x00000002,
+	HT_IOT_ACT_DISABLE_MCS14 = 0x00000004,
+	HT_IOT_ACT_DISABLE_MCS15 = 0x00000008,
+	HT_IOT_ACT_DISABLE_ALL_2SS = 0x00000010,
+	HT_IOT_ACT_DISABLE_EDCA_TURBO = 0x00000020,
+	HT_IOT_ACT_MGNT_USE_CCK_6M = 0x00000040,
+	HT_IOT_ACT_CDD_FSYNC = 0x00000080,
+	HT_IOT_ACT_PURE_N_MODE = 0x00000100,
+	HT_IOT_ACT_FORCED_CTS2SELF = 0x00000200,
+	HT_IOT_ACT_FORCED_RTS = 0x00000400,
+	HT_IOT_ACT_AMSDU_ENABLE = 0x00000800,
+	HT_IOT_ACT_REJECT_ADDBA_REQ = 0x00001000,
+	HT_IOT_ACT_ALLOW_PEER_AGG_ONE_PKT = 0x00002000,
+	HT_IOT_ACT_EDCA_BIAS_ON_RX = 0x00004000,
+
+	HT_IOT_ACT_HYBRID_AGGREGATION = 0x00010000,
+	HT_IOT_ACT_DISABLE_SHORT_GI = 0x00020000,
+	HT_IOT_ACT_DISABLE_HIGH_POWER = 0x00040000,
+	HT_IOT_ACT_DISABLE_TX_40_MHZ = 0x00080000,
+	HT_IOT_ACT_TX_NO_AGGREGATION = 0x00100000,
+	HT_IOT_ACT_DISABLE_TX_2SS = 0x00200000,
+
+	HT_IOT_ACT_MID_HIGHPOWER = 0x00400000,
+	HT_IOT_ACT_NULL_DATA_POWER_SAVING = 0x00800000,
+
+	HT_IOT_ACT_DISABLE_CCK_RATE = 0x01000000,
+	HT_IOT_ACT_FORCED_ENABLE_BE_TXOP = 0x02000000,
+	HT_IOT_ACT_WA_IOT_Broadcom = 0x04000000,
+
+	HT_IOT_ACT_DISABLE_RX_40MHZ_SHORT_GI = 0x08000000,
+
+};
+
+enum ht_iot_rafunc {
+	HT_IOT_RAFUNC_DISABLE_ALL = 0x00,
+	HT_IOT_RAFUNC_PEER_1R = 0x01,
+	HT_IOT_RAFUNC_TX_AMSDU = 0x02,
+};
+
+enum rt_ht_capability {
+	RT_HT_CAP_USE_TURBO_AGGR = 0x01,
+	RT_HT_CAP_USE_LONG_PREAMBLE = 0x02,
+	RT_HT_CAP_USE_AMPDU = 0x04,
+	RT_HT_CAP_USE_WOW = 0x8,
+	RT_HT_CAP_USE_SOFTAP = 0x10,
+	RT_HT_CAP_USE_92SE = 0x20,
+};
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
new file mode 100644
index 000000000..ef3dca51c
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -0,0 +1,891 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtllib.h"
+#include "rtl819x_HT.h"
+u8 MCS_FILTER_ALL[16] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+u8 MCS_FILTER_1SS[16] = {
+	0xff, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+;
+
+u16 MCS_DATA_RATE[2][2][77] = {
+	{{13, 26, 39, 52, 78, 104, 117, 130, 26, 52, 78, 104, 156, 208, 234,
+	 260, 39, 78, 117, 234, 312, 351, 390, 52, 104, 156, 208, 312, 416,
+	 468, 520, 0, 78, 104, 130, 117, 156, 195, 104, 130, 130, 156, 182,
+	 182, 208, 156, 195, 195, 234, 273, 273, 312, 130, 156, 181, 156,
+	 181, 208, 234, 208, 234, 260, 260, 286, 195, 234, 273, 234, 273,
+	 312, 351, 312, 351, 390, 390, 429},
+	{14, 29, 43, 58, 87, 116, 130, 144, 29, 58, 87, 116, 173, 231, 260, 289,
+	 43, 87, 130, 173, 260, 347, 390, 433, 58, 116, 173, 231, 347, 462, 520,
+	 578, 0, 87, 116, 144, 130, 173, 217, 116, 144, 144, 173, 202, 202, 231,
+	 173, 217, 217, 260, 303, 303, 347, 144, 173, 202, 173, 202, 231, 260,
+	 231, 260, 289, 289, 318, 217, 260, 303, 260, 303, 347, 390, 347, 390,
+	 433, 433, 477} },
+	{{27, 54, 81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486,
+	 540, 81, 162, 243, 324, 486, 648, 729, 810, 108, 216, 324, 432, 648,
+	 864, 972, 1080, 12, 162, 216, 270, 243, 324, 405, 216, 270, 270, 324,
+	 378, 378, 432, 324, 405, 405, 486, 567, 567, 648, 270, 324, 378, 324,
+	 378, 432, 486, 432, 486, 540, 540, 594, 405, 486, 567, 486, 567, 648,
+	 729, 648, 729, 810, 810, 891},
+	{30, 60, 90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540,
+	 600, 90, 180, 270, 360, 540, 720, 810, 900, 120, 240, 360, 480, 720,
+	 960, 1080, 1200, 13, 180, 240, 300, 270, 360, 450, 240, 300, 300, 360,
+	 420, 420, 480, 360, 450, 450, 540, 630, 630, 720, 300, 360, 420, 360,
+	 420, 480, 540, 480, 540, 600, 600, 660, 450, 540, 630, 540, 630, 720,
+	 810, 720, 810, 900, 900, 990} }
+};
+
+static u8 UNKNOWN_BORADCOM[3] = {0x00, 0x14, 0xbf};
+
+static u8 LINKSYSWRT330_LINKSYSWRT300_BROADCOM[3] = {0x00, 0x1a, 0x70};
+
+static u8 LINKSYSWRT350_LINKSYSWRT150_BROADCOM[3] = {0x00, 0x1d, 0x7e};
+
+static u8 BELKINF5D8233V1_RALINK[3] = {0x00, 0x17, 0x3f};
+
+static u8 BELKINF5D82334V3_RALINK[3] = {0x00, 0x1c, 0xdf};
+
+static u8 PCI_RALINK[3] = {0x00, 0x90, 0xcc};
+
+static u8 EDIMAX_RALINK[3] = {0x00, 0x0e, 0x2e};
+
+static u8 AIRLINK_RALINK[3] = {0x00, 0x18, 0x02};
+
+static u8 DLINK_ATHEROS_1[3] = {0x00, 0x1c, 0xf0};
+
+static u8 DLINK_ATHEROS_2[3] = {0x00, 0x21, 0x91};
+
+static u8 CISCO_BROADCOM[3] = {0x00, 0x17, 0x94};
+
+static u8 LINKSYS_MARVELL_4400N[3] = {0x00, 0x14, 0xa4};
+
+void HTUpdateDefaultSetting(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	pHTInfo->bRegShortGI20MHz = 1;
+	pHTInfo->bRegShortGI40MHz = 1;
+
+	pHTInfo->bRegBW40MHz = 1;
+
+	if (pHTInfo->bRegBW40MHz)
+		pHTInfo->bRegSuppCCK = 1;
+	else
+		pHTInfo->bRegSuppCCK = true;
+
+	pHTInfo->nAMSDU_MaxSize = 7935UL;
+	pHTInfo->bAMSDU_Support = 0;
+
+	pHTInfo->bAMPDUEnable = 1;
+	pHTInfo->AMPDU_Factor = 2;
+	pHTInfo->MPDU_Density = 0;
+
+	pHTInfo->self_mimo_ps = 3;
+	if (pHTInfo->self_mimo_ps == 2)
+		pHTInfo->self_mimo_ps = 3;
+	ieee->bTxDisableRateFallBack = 0;
+	ieee->bTxUseDriverAssingedRate = 0;
+
+	ieee->bTxEnableFwCalcDur = 1;
+
+	pHTInfo->reg_rt2rt_aggregation = 1;
+
+	pHTInfo->reg_rx_reorder_enable = 1;
+	pHTInfo->rx_reorder_win_size = 64;
+	pHTInfo->rx_reorder_pending_time = 30;
+}
+
+static u16 HTMcsToDataRate(struct rtllib_device *ieee, u8 nMcsRate)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	u8	is40MHz = (pHTInfo->bCurBW40MHz) ? 1 : 0;
+	u8	isShortGI = (pHTInfo->bCurBW40MHz) ?
+			    ((pHTInfo->bCurShortGI40MHz) ? 1 : 0) :
+			    ((pHTInfo->bCurShortGI20MHz) ? 1 : 0);
+	return MCS_DATA_RATE[is40MHz][isShortGI][(nMcsRate & 0x7f)];
+}
+
+u16  TxCountToDataRate(struct rtllib_device *ieee, u8 nDataRate)
+{
+	u16	CCKOFDMRate[12] = {0x02, 0x04, 0x0b, 0x16, 0x0c, 0x12, 0x18,
+				   0x24, 0x30, 0x48, 0x60, 0x6c};
+	u8	is40MHz = 0;
+	u8	isShortGI = 0;
+
+	if (nDataRate < 12)
+		return CCKOFDMRate[nDataRate];
+	if (nDataRate >= 0x10 && nDataRate <= 0x1f) {
+		is40MHz = 0;
+		isShortGI = 0;
+	} else if (nDataRate >= 0x20  && nDataRate <= 0x2f) {
+		is40MHz = 1;
+		isShortGI = 0;
+	} else if (nDataRate >= 0x30  && nDataRate <= 0x3f) {
+		is40MHz = 0;
+		isShortGI = 1;
+	} else if (nDataRate >= 0x40  && nDataRate <= 0x4f) {
+		is40MHz = 1;
+		isShortGI = 1;
+	}
+	return MCS_DATA_RATE[is40MHz][isShortGI][nDataRate & 0xf];
+}
+
+bool IsHTHalfNmodeAPs(struct rtllib_device *ieee)
+{
+	bool			retValue = false;
+	struct rtllib_network *net = &ieee->current_network;
+
+	if ((memcmp(net->bssid, BELKINF5D8233V1_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, BELKINF5D82334V3_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, PCI_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, EDIMAX_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, AIRLINK_RALINK, 3) == 0) ||
+	    (net->ralink_cap_exist))
+		retValue = true;
+	else if (!memcmp(net->bssid, UNKNOWN_BORADCOM, 3) ||
+		!memcmp(net->bssid, LINKSYSWRT330_LINKSYSWRT300_BROADCOM, 3) ||
+		!memcmp(net->bssid, LINKSYSWRT350_LINKSYSWRT150_BROADCOM, 3) ||
+		(net->broadcom_cap_exist))
+		retValue = true;
+	else if (net->bssht.bd_rt2rt_aggregation)
+		retValue = true;
+	else
+		retValue = false;
+
+	return retValue;
+}
+
+static void HTIOTPeerDetermine(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	struct rtllib_network *net = &ieee->current_network;
+
+	if (net->bssht.bd_rt2rt_aggregation) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_REALTEK;
+		if (net->bssht.rt2rt_ht_mode & RT_HT_CAP_USE_92SE)
+			pHTInfo->IOTPeer = HT_IOT_PEER_REALTEK_92SE;
+		if (net->bssht.rt2rt_ht_mode & RT_HT_CAP_USE_SOFTAP)
+			pHTInfo->IOTPeer = HT_IOT_PEER_92U_SOFTAP;
+	} else if (net->broadcom_cap_exist) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_BROADCOM;
+	} else if (!memcmp(net->bssid, UNKNOWN_BORADCOM, 3) ||
+		 !memcmp(net->bssid, LINKSYSWRT330_LINKSYSWRT300_BROADCOM, 3) ||
+		 !memcmp(net->bssid, LINKSYSWRT350_LINKSYSWRT150_BROADCOM, 3)) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_BROADCOM;
+	} else if ((memcmp(net->bssid, BELKINF5D8233V1_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, BELKINF5D82334V3_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, PCI_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, EDIMAX_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, AIRLINK_RALINK, 3) == 0) ||
+		  net->ralink_cap_exist) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_RALINK;
+	} else if ((net->atheros_cap_exist) ||
+		(memcmp(net->bssid, DLINK_ATHEROS_1, 3) == 0) ||
+		(memcmp(net->bssid, DLINK_ATHEROS_2, 3) == 0)) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_ATHEROS;
+	} else if ((memcmp(net->bssid, CISCO_BROADCOM, 3) == 0) ||
+		  net->cisco_cap_exist) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_CISCO;
+	} else if ((memcmp(net->bssid, LINKSYS_MARVELL_4400N, 3) == 0) ||
+		  net->marvell_cap_exist) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_MARVELL;
+	} else if (net->airgo_cap_exist) {
+		pHTInfo->IOTPeer = HT_IOT_PEER_AIRGO;
+	} else {
+		pHTInfo->IOTPeer = HT_IOT_PEER_UNKNOWN;
+	}
+
+	netdev_dbg(ieee->dev, "IOTPEER: %x\n", pHTInfo->IOTPeer);
+}
+
+static u8 HTIOTActIsDisableMCS14(struct rtllib_device *ieee, u8 *PeerMacAddr)
+{
+	return 0;
+}
+
+static bool HTIOTActIsDisableMCS15(struct rtllib_device *ieee)
+{
+	return false;
+}
+
+static bool HTIOTActIsDisableMCSTwoSpatialStream(struct rtllib_device *ieee)
+{
+	return false;
+}
+
+static u8 HTIOTActIsDisableEDCATurbo(struct rtllib_device *ieee,
+				     u8 *PeerMacAddr)
+{
+	return false;
+}
+
+static u8 HTIOTActIsMgntUseCCK6M(struct rtllib_device *ieee,
+				 struct rtllib_network *network)
+{
+	u8	retValue = 0;
+
+	if (ieee->pHTInfo->IOTPeer == HT_IOT_PEER_BROADCOM)
+		retValue = 1;
+
+	return retValue;
+}
+
+static u8 HTIOTActIsCCDFsync(struct rtllib_device *ieee)
+{
+	u8	retValue = 0;
+
+	if (ieee->pHTInfo->IOTPeer == HT_IOT_PEER_BROADCOM)
+		retValue = 1;
+	return retValue;
+}
+
+static void HTIOTActDetermineRaFunc(struct rtllib_device *ieee, bool bPeerRx2ss)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	pHTInfo->iot_ra_func &= HT_IOT_RAFUNC_DISABLE_ALL;
+
+	if (pHTInfo->IOTPeer == HT_IOT_PEER_RALINK && !bPeerRx2ss)
+		pHTInfo->iot_ra_func |= HT_IOT_RAFUNC_PEER_1R;
+
+	if (pHTInfo->iot_action & HT_IOT_ACT_AMSDU_ENABLE)
+		pHTInfo->iot_ra_func |= HT_IOT_RAFUNC_TX_AMSDU;
+}
+
+void HTResetIOTSetting(struct rt_hi_throughput *pHTInfo)
+{
+	pHTInfo->iot_action = 0;
+	pHTInfo->IOTPeer = HT_IOT_PEER_UNKNOWN;
+	pHTInfo->iot_ra_func = 0;
+}
+
+void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
+				  u8 *len, u8 IsEncrypt, bool bAssoc)
+{
+	struct rt_hi_throughput *pHT = ieee->pHTInfo;
+	struct ht_capab_ele *pCapELE = NULL;
+
+	if (!posHTCap || !pHT) {
+		netdev_warn(ieee->dev,
+			    "%s(): posHTCap and pHTInfo are null\n", __func__);
+		return;
+	}
+	memset(posHTCap, 0, *len);
+
+	if ((bAssoc) && (pHT->ePeerHTSpecVer == HT_SPEC_VER_EWC)) {
+		u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+
+		memcpy(posHTCap, EWC11NHTCap, sizeof(EWC11NHTCap));
+		pCapELE = (struct ht_capab_ele *)&(posHTCap[4]);
+		*len = 30 + 2;
+	} else {
+		pCapELE = (struct ht_capab_ele *)posHTCap;
+		*len = 26 + 2;
+	}
+
+	pCapELE->AdvCoding		= 0;
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		pCapELE->ChlWidth = 0;
+	else
+		pCapELE->ChlWidth = (pHT->bRegBW40MHz ? 1 : 0);
+
+	pCapELE->MimoPwrSave		= pHT->self_mimo_ps;
+	pCapELE->GreenField		= 0;
+	pCapELE->ShortGI20Mhz		= 1;
+	pCapELE->ShortGI40Mhz		= 1;
+
+	pCapELE->TxSTBC			= 1;
+	pCapELE->RxSTBC			= 0;
+	pCapELE->DelayBA		= 0;
+	pCapELE->MaxAMSDUSize = (MAX_RECEIVE_BUFFER_SIZE >= 7935) ? 1 : 0;
+	pCapELE->DssCCk = ((pHT->bRegBW40MHz) ? (pHT->bRegSuppCCK ? 1 : 0) : 0);
+	pCapELE->PSMP = 0;
+	pCapELE->LSigTxopProtect = 0;
+
+	netdev_dbg(ieee->dev,
+		   "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n",
+		   pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
+
+	if (IsEncrypt) {
+		pCapELE->MPDUDensity	= 7;
+		pCapELE->MaxRxAMPDUFactor	= 2;
+	} else {
+		pCapELE->MaxRxAMPDUFactor	= 3;
+		pCapELE->MPDUDensity	= 0;
+	}
+
+	memcpy(pCapELE->MCS, ieee->Regdot11HTOperationalRateSet, 16);
+	memset(&pCapELE->ExtHTCapInfo, 0, 2);
+	memset(pCapELE->TxBFCap, 0, 4);
+
+	pCapELE->ASCap = 0;
+
+	if (bAssoc) {
+		if (pHT->iot_action & HT_IOT_ACT_DISABLE_MCS15)
+			pCapELE->MCS[1] &= 0x7f;
+
+		if (pHT->iot_action & HT_IOT_ACT_DISABLE_MCS14)
+			pCapELE->MCS[1] &= 0xbf;
+
+		if (pHT->iot_action & HT_IOT_ACT_DISABLE_ALL_2SS)
+			pCapELE->MCS[1] &= 0x00;
+
+		if (pHT->iot_action & HT_IOT_ACT_DISABLE_RX_40MHZ_SHORT_GI)
+			pCapELE->ShortGI40Mhz		= 0;
+
+		if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev)) {
+			pCapELE->ChlWidth = 0;
+			pCapELE->MCS[1] = 0;
+		}
+	}
+}
+
+void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
+			    u8 *len, u8 IsEncrypt)
+{
+	struct rt_hi_throughput *pHT = ieee->pHTInfo;
+	struct ht_info_ele *pHTInfoEle = (struct ht_info_ele *)posHTInfo;
+
+	if (!posHTInfo || !pHTInfoEle) {
+		netdev_warn(ieee->dev,
+			    "%s(): posHTInfo and pHTInfoEle are null\n",
+			    __func__);
+		return;
+	}
+
+	memset(posHTInfo, 0, *len);
+	if ((ieee->iw_mode == IW_MODE_ADHOC) ||
+	    (ieee->iw_mode == IW_MODE_MASTER)) {
+		pHTInfoEle->ControlChl	= ieee->current_network.channel;
+		pHTInfoEle->ExtChlOffset = ((!pHT->bRegBW40MHz) ?
+					    HT_EXTCHNL_OFFSET_NO_EXT :
+					    (ieee->current_network.channel <= 6)
+					    ? HT_EXTCHNL_OFFSET_UPPER :
+					    HT_EXTCHNL_OFFSET_LOWER);
+		pHTInfoEle->RecommemdedTxWidth	= pHT->bRegBW40MHz;
+		pHTInfoEle->RIFS			= 0;
+		pHTInfoEle->PSMPAccessOnly		= 0;
+		pHTInfoEle->SrvIntGranularity		= 0;
+		pHTInfoEle->OptMode			= pHT->current_op_mode;
+		pHTInfoEle->NonGFDevPresent		= 0;
+		pHTInfoEle->DualBeacon			= 0;
+		pHTInfoEle->SecondaryBeacon		= 0;
+		pHTInfoEle->LSigTxopProtectFull		= 0;
+		pHTInfoEle->PcoActive			= 0;
+		pHTInfoEle->PcoPhase			= 0;
+
+		memset(pHTInfoEle->BasicMSC, 0, 16);
+
+		*len = 22 + 2;
+
+	} else {
+		*len = 0;
+	}
+}
+
+void HTConstructRT2RTAggElement(struct rtllib_device *ieee, u8 *posRT2RTAgg,
+				u8 *len)
+{
+	if (!posRT2RTAgg) {
+		netdev_warn(ieee->dev, "%s(): posRT2RTAgg is null\n", __func__);
+		return;
+	}
+	memset(posRT2RTAgg, 0, *len);
+	*posRT2RTAgg++ = 0x00;
+	*posRT2RTAgg++ = 0xe0;
+	*posRT2RTAgg++ = 0x4c;
+	*posRT2RTAgg++ = 0x02;
+	*posRT2RTAgg++ = 0x01;
+
+	*posRT2RTAgg = 0x30;
+
+	if (ieee->bSupportRemoteWakeUp)
+		*posRT2RTAgg |= RT_HT_CAP_USE_WOW;
+
+	*len = 6 + 2;
+}
+
+static u8 HT_PickMCSRate(struct rtllib_device *ieee, u8 *pOperateMCS)
+{
+	u8 i;
+
+	if (!pOperateMCS) {
+		netdev_warn(ieee->dev, "%s(): pOperateMCS is null\n", __func__);
+		return false;
+	}
+
+	switch (ieee->mode) {
+	case IEEE_A:
+	case IEEE_B:
+	case IEEE_G:
+		for (i = 0; i <= 15; i++)
+			pOperateMCS[i] = 0;
+		break;
+	case IEEE_N_24G:
+	case IEEE_N_5G:
+		pOperateMCS[0] &= RATE_ADPT_1SS_MASK;
+		pOperateMCS[1] &= RATE_ADPT_2SS_MASK;
+		pOperateMCS[3] &= RATE_ADPT_MCS32_MASK;
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
+		       u8 *pMCSFilter)
+{
+	u8		i, j;
+	u8		bitMap;
+	u8		mcsRate = 0;
+	u8		availableMcsRate[16];
+
+	if (!pMCSRateSet || !pMCSFilter) {
+		netdev_warn(ieee->dev,
+			    "%s(): pMCSRateSet and pMCSFilter are null\n",
+			    __func__);
+		return false;
+	}
+	for (i = 0; i < 16; i++)
+		availableMcsRate[i] = pMCSRateSet[i] & pMCSFilter[i];
+
+	for (i = 0; i < 16; i++) {
+		if (availableMcsRate[i] != 0)
+			break;
+	}
+	if (i == 16)
+		return false;
+
+	for (i = 0; i < 16; i++) {
+		if (availableMcsRate[i] != 0) {
+			bitMap = availableMcsRate[i];
+			for (j = 0; j < 8; j++) {
+				if ((bitMap % 2) != 0) {
+					if (HTMcsToDataRate(ieee, (8 * i + j)) >
+					    HTMcsToDataRate(ieee, mcsRate))
+						mcsRate = 8 * i + j;
+				}
+				bitMap >>= 1;
+			}
+		}
+	}
+	return mcsRate | 0x80;
+}
+
+static u8 HTFilterMCSRate(struct rtllib_device *ieee, u8 *pSupportMCS,
+			  u8 *pOperateMCS)
+{
+	u8 i;
+
+	for (i = 0; i <= 15; i++)
+		pOperateMCS[i] = ieee->Regdot11TxHTOperationalRateSet[i] &
+				 pSupportMCS[i];
+
+	HT_PickMCSRate(ieee, pOperateMCS);
+
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		pOperateMCS[1] = 0;
+
+	for (i = 2; i <= 15; i++)
+		pOperateMCS[i] = 0;
+
+	return true;
+}
+
+void HTSetConnectBwMode(struct rtllib_device *ieee,
+			enum ht_channel_width bandwidth,
+			enum ht_extchnl_offset Offset);
+
+void HTOnAssocRsp(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	struct ht_capab_ele *pPeerHTCap = NULL;
+	struct ht_info_ele *pPeerHTInfo = NULL;
+	u16 nMaxAMSDUSize = 0;
+	u8 *pMcsFilter = NULL;
+
+	static u8 EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+	static u8 EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};
+
+	if (!pHTInfo->bCurrentHTSupport) {
+		netdev_warn(ieee->dev, "%s(): HT_DISABLE\n", __func__);
+		return;
+	}
+	netdev_dbg(ieee->dev, "%s(): HT_ENABLE\n", __func__);
+
+	if (!memcmp(pHTInfo->PeerHTCapBuf, EWC11NHTCap, sizeof(EWC11NHTCap)))
+		pPeerHTCap = (struct ht_capab_ele *)(&pHTInfo->PeerHTCapBuf[4]);
+	else
+		pPeerHTCap = (struct ht_capab_ele *)(pHTInfo->PeerHTCapBuf);
+
+	if (!memcmp(pHTInfo->PeerHTInfoBuf, EWC11NHTInfo, sizeof(EWC11NHTInfo)))
+		pPeerHTInfo = (struct ht_info_ele *)
+			     (&pHTInfo->PeerHTInfoBuf[4]);
+	else
+		pPeerHTInfo = (struct ht_info_ele *)(pHTInfo->PeerHTInfoBuf);
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", __func__, DUMP_PREFIX_NONE,
+			     pPeerHTCap, sizeof(struct ht_capab_ele));
+#endif
+	HTSetConnectBwMode(ieee, (enum ht_channel_width)(pPeerHTCap->ChlWidth),
+			  (enum ht_extchnl_offset)(pPeerHTInfo->ExtChlOffset));
+	pHTInfo->cur_tx_bw40mhz = ((pPeerHTInfo->RecommemdedTxWidth == 1) ?
+				 true : false);
+
+	pHTInfo->bCurShortGI20MHz = ((pHTInfo->bRegShortGI20MHz) ?
+				    ((pPeerHTCap->ShortGI20Mhz == 1) ?
+				    true : false) : false);
+	pHTInfo->bCurShortGI40MHz = ((pHTInfo->bRegShortGI40MHz) ?
+				     ((pPeerHTCap->ShortGI40Mhz == 1) ?
+				     true : false) : false);
+
+	pHTInfo->bCurSuppCCK = ((pHTInfo->bRegSuppCCK) ?
+			       ((pPeerHTCap->DssCCk == 1) ? true :
+			       false) : false);
+
+	pHTInfo->bCurrent_AMSDU_Support = pHTInfo->bAMSDU_Support;
+
+	nMaxAMSDUSize = (pPeerHTCap->MaxAMSDUSize == 0) ? 3839 : 7935;
+
+	if (pHTInfo->nAMSDU_MaxSize > nMaxAMSDUSize)
+		pHTInfo->nCurrent_AMSDU_MaxSize = nMaxAMSDUSize;
+	else
+		pHTInfo->nCurrent_AMSDU_MaxSize = pHTInfo->nAMSDU_MaxSize;
+
+	pHTInfo->bCurrentAMPDUEnable = pHTInfo->bAMPDUEnable;
+	if (ieee->rtllib_ap_sec_type &&
+	   (ieee->rtllib_ap_sec_type(ieee) & (SEC_ALG_WEP | SEC_ALG_TKIP))) {
+		if ((pHTInfo->IOTPeer == HT_IOT_PEER_ATHEROS) ||
+				(pHTInfo->IOTPeer == HT_IOT_PEER_UNKNOWN))
+			pHTInfo->bCurrentAMPDUEnable = false;
+	}
+
+	if (!pHTInfo->reg_rt2rt_aggregation) {
+		if (pHTInfo->AMPDU_Factor > pPeerHTCap->MaxRxAMPDUFactor)
+			pHTInfo->CurrentAMPDUFactor =
+						 pPeerHTCap->MaxRxAMPDUFactor;
+		else
+			pHTInfo->CurrentAMPDUFactor = pHTInfo->AMPDU_Factor;
+
+	} else {
+		if (ieee->current_network.bssht.bd_rt2rt_aggregation) {
+			if (ieee->pairwise_key_type != KEY_TYPE_NA)
+				pHTInfo->CurrentAMPDUFactor =
+						 pPeerHTCap->MaxRxAMPDUFactor;
+			else
+				pHTInfo->CurrentAMPDUFactor = HT_AGG_SIZE_64K;
+		} else {
+			if (pPeerHTCap->MaxRxAMPDUFactor < HT_AGG_SIZE_32K)
+				pHTInfo->CurrentAMPDUFactor =
+						 pPeerHTCap->MaxRxAMPDUFactor;
+			else
+				pHTInfo->CurrentAMPDUFactor = HT_AGG_SIZE_32K;
+		}
+	}
+	if (pHTInfo->MPDU_Density > pPeerHTCap->MPDUDensity)
+		pHTInfo->current_mpdu_density = pHTInfo->MPDU_Density;
+	else
+		pHTInfo->current_mpdu_density = pPeerHTCap->MPDUDensity;
+	if (pHTInfo->iot_action & HT_IOT_ACT_TX_USE_AMSDU_8K) {
+		pHTInfo->bCurrentAMPDUEnable = false;
+		pHTInfo->ForcedAMSDUMode = HT_AGG_FORCE_ENABLE;
+	}
+	pHTInfo->cur_rx_reorder_enable = pHTInfo->reg_rx_reorder_enable;
+
+	if (pPeerHTCap->MCS[0] == 0)
+		pPeerHTCap->MCS[0] = 0xff;
+
+	HTIOTActDetermineRaFunc(ieee, ((pPeerHTCap->MCS[1]) != 0));
+
+	HTFilterMCSRate(ieee, pPeerHTCap->MCS, ieee->dot11HTOperationalRateSet);
+
+	pHTInfo->peer_mimo_ps = pPeerHTCap->MimoPwrSave;
+	if (pHTInfo->peer_mimo_ps == MIMO_PS_STATIC)
+		pMcsFilter = MCS_FILTER_1SS;
+	else
+		pMcsFilter = MCS_FILTER_ALL;
+	ieee->HTHighestOperaRate = HTGetHighestMCSRate(ieee,
+				   ieee->dot11HTOperationalRateSet, pMcsFilter);
+	ieee->HTCurrentOperaRate = ieee->HTHighestOperaRate;
+
+	pHTInfo->current_op_mode = pPeerHTInfo->OptMode;
+}
+
+void HTInitializeHTInfo(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	pHTInfo->bCurrentHTSupport = false;
+
+	pHTInfo->bCurBW40MHz = false;
+	pHTInfo->cur_tx_bw40mhz = false;
+
+	pHTInfo->bCurShortGI20MHz = false;
+	pHTInfo->bCurShortGI40MHz = false;
+	pHTInfo->forced_short_gi = false;
+
+	pHTInfo->bCurSuppCCK = true;
+
+	pHTInfo->bCurrent_AMSDU_Support = false;
+	pHTInfo->nCurrent_AMSDU_MaxSize = pHTInfo->nAMSDU_MaxSize;
+	pHTInfo->current_mpdu_density = pHTInfo->MPDU_Density;
+	pHTInfo->CurrentAMPDUFactor = pHTInfo->AMPDU_Factor;
+
+	memset((void *)(&(pHTInfo->SelfHTCap)), 0,
+		sizeof(pHTInfo->SelfHTCap));
+	memset((void *)(&(pHTInfo->SelfHTInfo)), 0,
+		sizeof(pHTInfo->SelfHTInfo));
+	memset((void *)(&(pHTInfo->PeerHTCapBuf)), 0,
+		sizeof(pHTInfo->PeerHTCapBuf));
+	memset((void *)(&(pHTInfo->PeerHTInfoBuf)), 0,
+		sizeof(pHTInfo->PeerHTInfoBuf));
+
+	pHTInfo->sw_bw_in_progress = false;
+
+	pHTInfo->ePeerHTSpecVer = HT_SPEC_VER_IEEE;
+
+	pHTInfo->current_rt2rt_aggregation = false;
+	pHTInfo->current_rt2rt_long_slot_time = false;
+	pHTInfo->RT2RT_HT_Mode = (enum rt_ht_capability)0;
+
+	pHTInfo->IOTPeer = 0;
+	pHTInfo->iot_action = 0;
+	pHTInfo->iot_ra_func = 0;
+
+	{
+		u8 *RegHTSuppRateSets = &(ieee->RegHTSuppRateSet[0]);
+
+		RegHTSuppRateSets[0] = 0xFF;
+		RegHTSuppRateSets[1] = 0xFF;
+		RegHTSuppRateSets[4] = 0x01;
+	}
+}
+
+void HTInitializeBssDesc(struct bss_ht *pBssHT)
+{
+	pBssHT->bd_support_ht = false;
+	memset(pBssHT->bd_ht_cap_buf, 0, sizeof(pBssHT->bd_ht_cap_buf));
+	pBssHT->bd_ht_cap_len = 0;
+	memset(pBssHT->bd_ht_info_buf, 0, sizeof(pBssHT->bd_ht_info_buf));
+	pBssHT->bd_ht_info_len = 0;
+
+	pBssHT->bd_ht_spec_ver = HT_SPEC_VER_IEEE;
+
+	pBssHT->bd_rt2rt_aggregation = false;
+	pBssHT->bd_rt2rt_long_slot_time = false;
+	pBssHT->rt2rt_ht_mode = (enum rt_ht_capability)0;
+}
+
+void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
+				   struct rtllib_network *pNetwork)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	u8	bIOTAction = 0;
+
+	/* unmark bEnableHT flag here is the same reason why unmarked in
+	 * function rtllib_softmac_new_net. WB 2008.09.10
+	 */
+	if (pNetwork->bssht.bd_support_ht) {
+		pHTInfo->bCurrentHTSupport = true;
+		pHTInfo->ePeerHTSpecVer = pNetwork->bssht.bd_ht_spec_ver;
+
+		if (pNetwork->bssht.bd_ht_cap_len > 0 &&
+		    pNetwork->bssht.bd_ht_cap_len <= sizeof(pHTInfo->PeerHTCapBuf))
+			memcpy(pHTInfo->PeerHTCapBuf,
+			       pNetwork->bssht.bd_ht_cap_buf,
+			       pNetwork->bssht.bd_ht_cap_len);
+
+		if (pNetwork->bssht.bd_ht_info_len > 0 &&
+		    pNetwork->bssht.bd_ht_info_len <=
+		    sizeof(pHTInfo->PeerHTInfoBuf))
+			memcpy(pHTInfo->PeerHTInfoBuf,
+			       pNetwork->bssht.bd_ht_info_buf,
+			       pNetwork->bssht.bd_ht_info_len);
+
+		if (pHTInfo->reg_rt2rt_aggregation) {
+			pHTInfo->current_rt2rt_aggregation =
+				 pNetwork->bssht.bd_rt2rt_aggregation;
+			pHTInfo->current_rt2rt_long_slot_time =
+				 pNetwork->bssht.bd_rt2rt_long_slot_time;
+			pHTInfo->RT2RT_HT_Mode = pNetwork->bssht.rt2rt_ht_mode;
+		} else {
+			pHTInfo->current_rt2rt_aggregation = false;
+			pHTInfo->current_rt2rt_long_slot_time = false;
+			pHTInfo->RT2RT_HT_Mode = (enum rt_ht_capability)0;
+		}
+
+		HTIOTPeerDetermine(ieee);
+
+		pHTInfo->iot_action = 0;
+		bIOTAction = HTIOTActIsDisableMCS14(ieee, pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_DISABLE_MCS14;
+
+		bIOTAction = HTIOTActIsDisableMCS15(ieee);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_DISABLE_MCS15;
+
+		bIOTAction = HTIOTActIsDisableMCSTwoSpatialStream(ieee);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_DISABLE_ALL_2SS;
+
+		bIOTAction = HTIOTActIsDisableEDCATurbo(ieee, pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_DISABLE_EDCA_TURBO;
+
+		bIOTAction = HTIOTActIsMgntUseCCK6M(ieee, pNetwork);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_MGNT_USE_CCK_6M;
+		bIOTAction = HTIOTActIsCCDFsync(ieee);
+		if (bIOTAction)
+			pHTInfo->iot_action |= HT_IOT_ACT_CDD_FSYNC;
+	} else {
+		pHTInfo->bCurrentHTSupport = false;
+		pHTInfo->current_rt2rt_aggregation = false;
+		pHTInfo->current_rt2rt_long_slot_time = false;
+		pHTInfo->RT2RT_HT_Mode = (enum rt_ht_capability)0;
+
+		pHTInfo->iot_action = 0;
+		pHTInfo->iot_ra_func = 0;
+	}
+}
+
+void HT_update_self_and_peer_setting(struct rtllib_device *ieee,
+				     struct rtllib_network *pNetwork)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	struct ht_info_ele *pPeerHTInfo =
+		 (struct ht_info_ele *)pNetwork->bssht.bd_ht_info_buf;
+
+	if (pHTInfo->bCurrentHTSupport) {
+		if (pNetwork->bssht.bd_ht_info_len != 0)
+			pHTInfo->current_op_mode = pPeerHTInfo->OptMode;
+	}
+}
+EXPORT_SYMBOL(HT_update_self_and_peer_setting);
+
+void HTUseDefaultSetting(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	if (pHTInfo->bEnableHT) {
+		pHTInfo->bCurrentHTSupport = true;
+		pHTInfo->bCurSuppCCK = pHTInfo->bRegSuppCCK;
+
+		pHTInfo->bCurBW40MHz = pHTInfo->bRegBW40MHz;
+		pHTInfo->bCurShortGI20MHz = pHTInfo->bRegShortGI20MHz;
+
+		pHTInfo->bCurShortGI40MHz = pHTInfo->bRegShortGI40MHz;
+
+		if (ieee->iw_mode == IW_MODE_ADHOC)
+			ieee->current_network.qos_data.active =
+				 ieee->current_network.qos_data.supported;
+		pHTInfo->bCurrent_AMSDU_Support = pHTInfo->bAMSDU_Support;
+		pHTInfo->nCurrent_AMSDU_MaxSize = pHTInfo->nAMSDU_MaxSize;
+
+		pHTInfo->bCurrentAMPDUEnable = pHTInfo->bAMPDUEnable;
+		pHTInfo->CurrentAMPDUFactor = pHTInfo->AMPDU_Factor;
+
+		pHTInfo->current_mpdu_density = pHTInfo->current_mpdu_density;
+
+		HTFilterMCSRate(ieee, ieee->Regdot11TxHTOperationalRateSet,
+				ieee->dot11HTOperationalRateSet);
+		ieee->HTHighestOperaRate = HTGetHighestMCSRate(ieee,
+					   ieee->dot11HTOperationalRateSet,
+					   MCS_FILTER_ALL);
+		ieee->HTCurrentOperaRate = ieee->HTHighestOperaRate;
+
+	} else {
+		pHTInfo->bCurrentHTSupport = false;
+	}
+}
+
+u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame)
+{
+	if (ieee->pHTInfo->bCurrentHTSupport) {
+		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
+			netdev_dbg(ieee->dev, "HT CONTROL FILED EXIST!!\n");
+			return true;
+		}
+	}
+	return false;
+}
+
+static void HTSetConnectBwModeCallback(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	if (pHTInfo->bCurBW40MHz) {
+		if (pHTInfo->CurSTAExtChnlOffset == HT_EXTCHNL_OFFSET_UPPER)
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel + 2);
+		else if (pHTInfo->CurSTAExtChnlOffset ==
+			 HT_EXTCHNL_OFFSET_LOWER)
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel - 2);
+		else
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel);
+
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20_40,
+				       pHTInfo->CurSTAExtChnlOffset);
+	} else {
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20,
+				       HT_EXTCHNL_OFFSET_NO_EXT);
+	}
+
+	pHTInfo->sw_bw_in_progress = false;
+}
+
+void HTSetConnectBwMode(struct rtllib_device *ieee,
+			enum ht_channel_width bandwidth,
+			enum ht_extchnl_offset Offset)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	if (!pHTInfo->bRegBW40MHz)
+		return;
+
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		bandwidth = HT_CHANNEL_WIDTH_20;
+
+	if (pHTInfo->sw_bw_in_progress) {
+		pr_info("%s: sw_bw_in_progress!!\n", __func__);
+		return;
+	}
+	if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+		if (ieee->current_network.channel < 2 &&
+		    Offset == HT_EXTCHNL_OFFSET_LOWER)
+			Offset = HT_EXTCHNL_OFFSET_NO_EXT;
+		if (Offset == HT_EXTCHNL_OFFSET_UPPER ||
+		    Offset == HT_EXTCHNL_OFFSET_LOWER) {
+			pHTInfo->bCurBW40MHz = true;
+			pHTInfo->CurSTAExtChnlOffset = Offset;
+		} else {
+			pHTInfo->bCurBW40MHz = false;
+			pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
+		}
+	} else {
+		pHTInfo->bCurBW40MHz = false;
+		pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
+	}
+
+	netdev_dbg(ieee->dev, "%s():pHTInfo->bCurBW40MHz:%x\n", __func__,
+		   pHTInfo->bCurBW40MHz);
+
+	pHTInfo->sw_bw_in_progress = true;
+
+	HTSetConnectBwModeCallback(ieee);
+}
diff --git a/drivers/staging/rtl8192e/rtl819x_Qos.h b/drivers/staging/rtl8192e/rtl819x_Qos.h
new file mode 100644
index 000000000..5073f9f40
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_Qos.h
@@ -0,0 +1,175 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef __INC_QOS_TYPE_H
+#define __INC_QOS_TYPE_H
+
+#define BIT0		    0x00000001
+#define BIT1		    0x00000002
+#define BIT2		    0x00000004
+#define BIT3		    0x00000008
+#define BIT4		    0x00000010
+#define BIT5		    0x00000020
+#define BIT6		    0x00000040
+#define BIT7		    0x00000080
+#define BIT8		    0x00000100
+#define BIT9		    0x00000200
+#define BIT10		   0x00000400
+#define BIT11		   0x00000800
+#define BIT12		   0x00001000
+#define BIT13		   0x00002000
+#define BIT14		   0x00004000
+#define BIT15		   0x00008000
+#define BIT16		   0x00010000
+#define BIT17		   0x00020000
+#define BIT18		   0x00040000
+#define BIT19		   0x00080000
+#define BIT20		   0x00100000
+#define BIT21		   0x00200000
+#define BIT22		   0x00400000
+#define BIT23		   0x00800000
+#define BIT24		   0x01000000
+#define BIT25		   0x02000000
+#define BIT26		   0x04000000
+#define BIT27		   0x08000000
+#define BIT28		   0x10000000
+#define BIT29		   0x20000000
+#define BIT30		   0x40000000
+#define BIT31		   0x80000000
+
+union qos_tsinfo {
+	u8		charData[3];
+	struct {
+		u8		ucTrafficType:1;
+		u8		ucTSID:4;
+		u8		ucDirection:2;
+		u8		ucAccessPolicy:2;
+		u8		ucAggregation:1;
+		u8		ucPSB:1;
+		u8		ucUP:3;
+		u8		ucTSInfoAckPolicy:2;
+		u8		ucSchedule:1;
+		u8		ucReserved:7;
+	} field;
+};
+
+union tspec_body {
+	u8		charData[55];
+
+	struct {
+		union qos_tsinfo TSInfo;
+		u16	NominalMSDUsize;
+		u16	MaxMSDUsize;
+		u32	MinServiceItv;
+		u32	MaxServiceItv;
+		u32	InactivityItv;
+		u32	SuspenItv;
+		u32	ServiceStartTime;
+		u32	MinDataRate;
+		u32	MeanDataRate;
+		u32	PeakDataRate;
+		u32	MaxBurstSize;
+		u32	DelayBound;
+		u32	MinPhyRate;
+		u16	SurplusBandwidthAllowance;
+		u16	MediumTime;
+	} f;
+};
+
+struct octet_string {
+	u8 *Octet;
+	u16 Length;
+};
+
+#define AC0_BE	0
+#define AC1_BK	1
+#define AC2_VI	2
+#define AC3_VO	3
+#define AC_MAX	4
+
+enum direction_value {
+	DIR_UP			= 0,
+	DIR_DOWN		= 1,
+	DIR_DIRECT		= 2,
+	DIR_BI_DIR		= 3,
+};
+
+enum acm_method {
+	eAcmWay0_SwAndHw		= 0,
+	eAcmWay1_HW			= 1,
+	eAcmWay2_SW			= 2,
+};
+
+
+struct acm {
+	u64		UsedTime;
+	u64		MediumTime;
+	u8		HwAcmCtl;
+};
+
+union qos_tclas {
+
+	struct _TYPE_GENERAL {
+		u8		Priority;
+		u8		ClassifierType;
+		u8		Mask;
+	} TYPE_GENERAL;
+
+	struct _TYPE0_ETH {
+		u8		Priority;
+		u8		ClassifierType;
+		u8		Mask;
+		u8		SrcAddr[ETH_ALEN];
+		u8		DstAddr[ETH_ALEN];
+		u16		Type;
+	} TYPE0_ETH;
+
+	struct _TYPE1_IPV4 {
+		u8		Priority;
+		u8		ClassifierType;
+		u8		Mask;
+		u8		Version;
+		u8		SrcIP[4];
+		u8		DstIP[4];
+		u16		SrcPort;
+		u16		DstPort;
+		u8		DSCP;
+		u8		Protocol;
+		u8		Reserved;
+	} TYPE1_IPV4;
+
+	struct _TYPE1_IPV6 {
+		u8		Priority;
+		u8		ClassifierType;
+		u8		Mask;
+		u8		Version;
+		u8		SrcIP[16];
+		u8		DstIP[16];
+		u16		SrcPort;
+		u16		DstPort;
+		u8		FlowLabel[3];
+	} TYPE1_IPV6;
+
+	struct _TYPE2_8021Q {
+		u8		Priority;
+		u8		ClassifierType;
+		u8		Mask;
+		u16		TagType;
+	} TYPE2_8021Q;
+};
+
+union aci_aifsn {
+	u8	charData;
+
+	struct {
+		u8	AIFSN:4;
+		u8	acm:1;
+		u8	ACI:2;
+		u8	Reserved:1;
+	} f;
+};
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl819x_TS.h b/drivers/staging/rtl8192e/rtl819x_TS.h
new file mode 100644
index 000000000..4aa9b12a2
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_TS.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _TSTYPE_H_
+#define _TSTYPE_H_
+#include "rtl819x_Qos.h"
+#define TS_ADDBA_DELAY		60
+
+#define TOTAL_TS_NUM		16
+#define TCLAS_NUM		4
+
+enum tr_select {
+	TX_DIR = 0,
+	RX_DIR = 1,
+};
+
+struct ts_common_info {
+	struct list_head		List;
+	struct timer_list		SetupTimer;
+	struct timer_list		InactTimer;
+	u8				Addr[ETH_ALEN];
+	union tspec_body TSpec;
+	union qos_tclas TClass[TCLAS_NUM];
+	u8				TClasProc;
+	u8				TClasNum;
+};
+
+struct tx_ts_record {
+	struct ts_common_info TsCommonInfo;
+	u16				TxCurSeq;
+	struct ba_record TxPendingBARecord;
+	struct ba_record TxAdmittedBARecord;
+	u8				bAddBaReqInProgress;
+	u8				bAddBaReqDelayed;
+	u8				bUsingBa;
+	u8				bDisable_AddBa;
+	struct timer_list		TsAddBaTimer;
+	u8				num;
+};
+
+struct rx_ts_record {
+	struct ts_common_info ts_common_info;
+	u16 rx_indicate_seq;
+	u16 rx_timeout_indicate_seq;
+	struct list_head rx_pending_pkt_list;
+	struct timer_list rx_pkt_pending_timer;
+	struct ba_record rx_admitted_ba_record;
+	u16 rx_last_seq_num;
+	u8 rx_last_frag_num;
+	u8 num;
+};
+
+
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
new file mode 100644
index 000000000..05c7e822f
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -0,0 +1,522 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#include "rtllib.h"
+#include <linux/etherdevice.h>
+#include "rtl819x_TS.h"
+
+static void TsSetupTimeOut(struct timer_list *unused)
+{
+}
+
+static void TsInactTimeout(struct timer_list *unused)
+{
+}
+
+static void RxPktPendingTimeout(struct timer_list *t)
+{
+	struct rx_ts_record *pRxTs = from_timer(pRxTs, t,
+						     rx_pkt_pending_timer);
+	struct rtllib_device *ieee = container_of(pRxTs, struct rtllib_device,
+						  RxTsRecord[pRxTs->num]);
+
+	struct rx_reorder_entry *pReorderEntry = NULL;
+
+	unsigned long flags = 0;
+	u8 index = 0;
+	bool bPktInBuf = false;
+
+	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
+	if (pRxTs->rx_timeout_indicate_seq != 0xffff) {
+		while (!list_empty(&pRxTs->rx_pending_pkt_list)) {
+			pReorderEntry = (struct rx_reorder_entry *)
+					list_entry(pRxTs->rx_pending_pkt_list.prev,
+					struct rx_reorder_entry, List);
+			if (index == 0)
+				pRxTs->rx_indicate_seq = pReorderEntry->SeqNum;
+
+			if (SN_LESS(pReorderEntry->SeqNum,
+				    pRxTs->rx_indicate_seq) ||
+			    SN_EQUAL(pReorderEntry->SeqNum,
+				     pRxTs->rx_indicate_seq)) {
+				list_del_init(&pReorderEntry->List);
+
+				if (SN_EQUAL(pReorderEntry->SeqNum,
+				    pRxTs->rx_indicate_seq))
+					pRxTs->rx_indicate_seq =
+					      (pRxTs->rx_indicate_seq + 1) % 4096;
+
+				netdev_dbg(ieee->dev,
+					   "%s(): Indicate SeqNum: %d\n",
+					   __func__, pReorderEntry->SeqNum);
+				ieee->stats_IndicateArray[index] =
+							 pReorderEntry->prxb;
+				index++;
+
+				list_add_tail(&pReorderEntry->List,
+					      &ieee->RxReorder_Unused_List);
+			} else {
+				bPktInBuf = true;
+				break;
+			}
+		}
+	}
+
+	if (index > 0) {
+		pRxTs->rx_timeout_indicate_seq = 0xffff;
+
+		if (index > REORDER_WIN_SIZE) {
+			netdev_warn(ieee->dev,
+				    "%s(): Rx Reorder struct buffer full\n",
+				    __func__);
+			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
+					       flags);
+			return;
+		}
+		rtllib_indicate_packets(ieee, ieee->stats_IndicateArray, index);
+		bPktInBuf = false;
+	}
+
+	if (bPktInBuf && (pRxTs->rx_timeout_indicate_seq == 0xffff)) {
+		pRxTs->rx_timeout_indicate_seq = pRxTs->rx_indicate_seq;
+		mod_timer(&pRxTs->rx_pkt_pending_timer,  jiffies +
+			  msecs_to_jiffies(ieee->pHTInfo->rx_reorder_pending_time)
+			  );
+	}
+	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+}
+
+static void TsAddBaProcess(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t, TsAddBaTimer);
+	u8 num = pTxTs->num;
+	struct rtllib_device *ieee = container_of(pTxTs, struct rtllib_device,
+				     TxTsRecord[num]);
+
+	TsInitAddBA(ieee, pTxTs, BA_POLICY_IMMEDIATE, false);
+	netdev_dbg(ieee->dev, "%s(): ADDBA Req is started\n", __func__);
+}
+
+static void ResetTsCommonInfo(struct ts_common_info *pTsCommonInfo)
+{
+	eth_zero_addr(pTsCommonInfo->Addr);
+	memset(&pTsCommonInfo->TSpec, 0, sizeof(union tspec_body));
+	memset(&pTsCommonInfo->TClass, 0, sizeof(union qos_tclas) * TCLAS_NUM);
+	pTsCommonInfo->TClasProc = 0;
+	pTsCommonInfo->TClasNum = 0;
+}
+
+static void ResetTxTsEntry(struct tx_ts_record *pTS)
+{
+	ResetTsCommonInfo(&pTS->TsCommonInfo);
+	pTS->TxCurSeq = 0;
+	pTS->bAddBaReqInProgress = false;
+	pTS->bAddBaReqDelayed = false;
+	pTS->bUsingBa = false;
+	pTS->bDisable_AddBa = false;
+	ResetBaEntry(&pTS->TxAdmittedBARecord);
+	ResetBaEntry(&pTS->TxPendingBARecord);
+}
+
+static void ResetRxTsEntry(struct rx_ts_record *pTS)
+{
+	ResetTsCommonInfo(&pTS->ts_common_info);
+	pTS->rx_indicate_seq = 0xffff;
+	pTS->rx_timeout_indicate_seq = 0xffff;
+	ResetBaEntry(&pTS->rx_admitted_ba_record);
+}
+
+void TSInitialize(struct rtllib_device *ieee)
+{
+	struct tx_ts_record *pTxTS  = ieee->TxTsRecord;
+	struct rx_ts_record *pRxTS  = ieee->RxTsRecord;
+	struct rx_reorder_entry *pRxReorderEntry = ieee->RxReorderEntry;
+	u8				count = 0;
+
+	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
+	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
+	INIT_LIST_HEAD(&ieee->Tx_TS_Unused_List);
+
+	for (count = 0; count < TOTAL_TS_NUM; count++) {
+		pTxTS->num = count;
+		timer_setup(&pTxTS->TsCommonInfo.SetupTimer, TsSetupTimeOut,
+			    0);
+
+		timer_setup(&pTxTS->TsCommonInfo.InactTimer, TsInactTimeout,
+			    0);
+
+		timer_setup(&pTxTS->TsAddBaTimer, TsAddBaProcess, 0);
+
+		timer_setup(&pTxTS->TxPendingBARecord.timer, BaSetupTimeOut,
+			    0);
+		timer_setup(&pTxTS->TxAdmittedBARecord.timer,
+			    TxBaInactTimeout, 0);
+
+		ResetTxTsEntry(pTxTS);
+		list_add_tail(&pTxTS->TsCommonInfo.List,
+				&ieee->Tx_TS_Unused_List);
+		pTxTS++;
+	}
+
+	INIT_LIST_HEAD(&ieee->Rx_TS_Admit_List);
+	INIT_LIST_HEAD(&ieee->Rx_TS_Pending_List);
+	INIT_LIST_HEAD(&ieee->Rx_TS_Unused_List);
+	for (count = 0; count < TOTAL_TS_NUM; count++) {
+		pRxTS->num = count;
+		INIT_LIST_HEAD(&pRxTS->rx_pending_pkt_list);
+
+		timer_setup(&pRxTS->ts_common_info.SetupTimer, TsSetupTimeOut,
+			    0);
+
+		timer_setup(&pRxTS->ts_common_info.InactTimer, TsInactTimeout,
+			    0);
+
+		timer_setup(&pRxTS->rx_admitted_ba_record.timer,
+			    RxBaInactTimeout, 0);
+
+		timer_setup(&pRxTS->rx_pkt_pending_timer, RxPktPendingTimeout, 0);
+
+		ResetRxTsEntry(pRxTS);
+		list_add_tail(&pRxTS->ts_common_info.List,
+			      &ieee->Rx_TS_Unused_List);
+		pRxTS++;
+	}
+	INIT_LIST_HEAD(&ieee->RxReorder_Unused_List);
+	for (count = 0; count < REORDER_ENTRY_NUM; count++) {
+		list_add_tail(&pRxReorderEntry->List,
+			      &ieee->RxReorder_Unused_List);
+		if (count == (REORDER_ENTRY_NUM - 1))
+			break;
+		pRxReorderEntry = &ieee->RxReorderEntry[count + 1];
+	}
+}
+
+static void AdmitTS(struct rtllib_device *ieee,
+		    struct ts_common_info *pTsCommonInfo, u32 InactTime)
+{
+	del_timer_sync(&pTsCommonInfo->SetupTimer);
+	del_timer_sync(&pTsCommonInfo->InactTimer);
+
+	if (InactTime != 0)
+		mod_timer(&pTsCommonInfo->InactTimer, jiffies +
+			  msecs_to_jiffies(InactTime));
+}
+
+static struct ts_common_info *SearchAdmitTRStream(struct rtllib_device *ieee,
+						  u8 *Addr, u8 TID,
+						  enum tr_select TxRxSelect)
+{
+	u8	dir;
+	bool	search_dir[4] = {0};
+	struct list_head *psearch_list;
+	struct ts_common_info *pRet = NULL;
+
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		if (TxRxSelect == TX_DIR) {
+			search_dir[DIR_DOWN] = true;
+			search_dir[DIR_BI_DIR] = true;
+		} else {
+			search_dir[DIR_UP] = true;
+			search_dir[DIR_BI_DIR] = true;
+		}
+	} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+		if (TxRxSelect == TX_DIR)
+			search_dir[DIR_UP] = true;
+		else
+			search_dir[DIR_DOWN] = true;
+	} else {
+		if (TxRxSelect == TX_DIR) {
+			search_dir[DIR_UP] = true;
+			search_dir[DIR_BI_DIR] = true;
+			search_dir[DIR_DIRECT] = true;
+		} else {
+			search_dir[DIR_DOWN] = true;
+			search_dir[DIR_BI_DIR] = true;
+			search_dir[DIR_DIRECT] = true;
+		}
+	}
+
+	if (TxRxSelect == TX_DIR)
+		psearch_list = &ieee->Tx_TS_Admit_List;
+	else
+		psearch_list = &ieee->Rx_TS_Admit_List;
+
+	for (dir = 0; dir <= DIR_BI_DIR; dir++) {
+		if (!search_dir[dir])
+			continue;
+		list_for_each_entry(pRet, psearch_list, List) {
+			if (memcmp(pRet->Addr, Addr, 6) == 0 &&
+			    pRet->TSpec.f.TSInfo.field.ucTSID == TID &&
+			    pRet->TSpec.f.TSInfo.field.ucDirection == dir)
+				break;
+		}
+		if (&pRet->List  != psearch_list)
+			break;
+	}
+
+	if (pRet && &pRet->List  != psearch_list)
+		return pRet;
+	return NULL;
+}
+
+static void MakeTSEntry(struct ts_common_info *pTsCommonInfo, u8 *Addr,
+			union tspec_body *pTSPEC, union qos_tclas *pTCLAS,
+			u8 TCLAS_Num, u8 TCLAS_Proc)
+{
+	u8	count;
+
+	if (!pTsCommonInfo)
+		return;
+
+	memcpy(pTsCommonInfo->Addr, Addr, 6);
+
+	if (pTSPEC)
+		memcpy((u8 *)(&(pTsCommonInfo->TSpec)), (u8 *)pTSPEC,
+			sizeof(union tspec_body));
+
+	for (count = 0; count < TCLAS_Num; count++)
+		memcpy((u8 *)(&(pTsCommonInfo->TClass[count])),
+		       (u8 *)pTCLAS, sizeof(union qos_tclas));
+
+	pTsCommonInfo->TClasProc = TCLAS_Proc;
+	pTsCommonInfo->TClasNum = TCLAS_Num;
+}
+
+bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
+	   u8 *Addr, u8 TID, enum tr_select TxRxSelect, bool bAddNewTs)
+{
+	u8	UP = 0;
+	union tspec_body TSpec;
+	union qos_tsinfo *pTSInfo = &TSpec.f.TSInfo;
+	struct list_head *pUnusedList;
+	struct list_head *pAddmitList;
+	enum direction_value Dir;
+
+	if (is_multicast_ether_addr(Addr)) {
+		netdev_warn(ieee->dev, "Get TS for Broadcast or Multicast\n");
+		return false;
+	}
+	if (ieee->current_network.qos_data.supported == 0) {
+		UP = 0;
+	} else {
+		switch (TID) {
+		case 0:
+		case 3:
+			UP = 0;
+			break;
+		case 1:
+		case 2:
+			UP = 2;
+			break;
+		case 4:
+		case 5:
+			UP = 5;
+			break;
+		case 6:
+		case 7:
+			UP = 7;
+			break;
+		default:
+			netdev_warn(ieee->dev, "%s(): TID(%d) is not valid\n",
+				    __func__, TID);
+			return false;
+		}
+	}
+
+	*ppTS = SearchAdmitTRStream(ieee, Addr, UP, TxRxSelect);
+	if (*ppTS)
+		return true;
+
+	if (!bAddNewTs) {
+		netdev_dbg(ieee->dev, "add new TS failed(tid:%d)\n", UP);
+		return false;
+	}
+
+	pUnusedList = (TxRxSelect == TX_DIR) ?
+				(&ieee->Tx_TS_Unused_List) :
+				(&ieee->Rx_TS_Unused_List);
+
+	pAddmitList = (TxRxSelect == TX_DIR) ?
+				(&ieee->Tx_TS_Admit_List) :
+				(&ieee->Rx_TS_Admit_List);
+
+	Dir = (ieee->iw_mode == IW_MODE_MASTER) ?
+				((TxRxSelect == TX_DIR) ? DIR_DOWN : DIR_UP) :
+				((TxRxSelect == TX_DIR) ? DIR_UP : DIR_DOWN);
+
+	if (!list_empty(pUnusedList)) {
+		(*ppTS) = list_entry(pUnusedList->next,
+			  struct ts_common_info, List);
+		list_del_init(&(*ppTS)->List);
+		if (TxRxSelect == TX_DIR) {
+			struct tx_ts_record *tmp =
+				container_of(*ppTS,
+				struct tx_ts_record,
+				TsCommonInfo);
+			ResetTxTsEntry(tmp);
+		} else {
+			struct rx_ts_record *tmp =
+				 container_of(*ppTS,
+				 struct rx_ts_record,
+				 ts_common_info);
+			ResetRxTsEntry(tmp);
+		}
+
+		netdev_dbg(ieee->dev,
+			   "to init current TS, UP:%d, Dir:%d, addr: %pM ppTs=%p\n",
+			   UP, Dir, Addr, *ppTS);
+		pTSInfo->field.ucTrafficType = 0;
+		pTSInfo->field.ucTSID = UP;
+		pTSInfo->field.ucDirection = Dir;
+		pTSInfo->field.ucAccessPolicy = 1;
+		pTSInfo->field.ucAggregation = 0;
+		pTSInfo->field.ucPSB = 0;
+		pTSInfo->field.ucUP = UP;
+		pTSInfo->field.ucTSInfoAckPolicy = 0;
+		pTSInfo->field.ucSchedule = 0;
+
+		MakeTSEntry(*ppTS, Addr, &TSpec, NULL, 0, 0);
+		AdmitTS(ieee, *ppTS, 0);
+		list_add_tail(&((*ppTS)->List), pAddmitList);
+
+		return true;
+	}
+
+	netdev_warn(ieee->dev,
+		    "There is not enough dir=%d(0=up down=1) TS record to be used!",
+		    Dir);
+	return false;
+}
+
+static void RemoveTsEntry(struct rtllib_device *ieee,
+			  struct ts_common_info *pTs, enum tr_select TxRxSelect)
+{
+	del_timer_sync(&pTs->SetupTimer);
+	del_timer_sync(&pTs->InactTimer);
+	TsInitDelBA(ieee, pTs, TxRxSelect);
+
+	if (TxRxSelect == RX_DIR) {
+		struct rx_reorder_entry *pRxReorderEntry;
+		struct rx_ts_record *pRxTS = (struct rx_ts_record *)pTs;
+
+		if (timer_pending(&pRxTS->rx_pkt_pending_timer))
+			del_timer_sync(&pRxTS->rx_pkt_pending_timer);
+
+		while (!list_empty(&pRxTS->rx_pending_pkt_list)) {
+			pRxReorderEntry = (struct rx_reorder_entry *)
+					list_entry(pRxTS->rx_pending_pkt_list.prev,
+					struct rx_reorder_entry, List);
+			netdev_dbg(ieee->dev,  "%s(): Delete SeqNum %d!\n",
+				   __func__, pRxReorderEntry->SeqNum);
+			list_del_init(&pRxReorderEntry->List);
+			{
+				int i = 0;
+				struct rtllib_rxb *prxb = pRxReorderEntry->prxb;
+
+				if (unlikely(!prxb))
+					return;
+				for (i = 0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+				kfree(prxb);
+				prxb = NULL;
+			}
+			list_add_tail(&pRxReorderEntry->List,
+				      &ieee->RxReorder_Unused_List);
+		}
+	} else {
+		struct tx_ts_record *pTxTS = (struct tx_ts_record *)pTs;
+
+		del_timer_sync(&pTxTS->TsAddBaTimer);
+	}
+}
+
+void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr)
+{
+	struct ts_common_info *pTS, *pTmpTS;
+
+	netdev_info(ieee->dev, "===========>%s, %pM\n", __func__, Addr);
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, TX_DIR);
+			list_del_init(&pTS->List);
+			list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
+			netdev_info(ieee->dev,
+				    "====>remove Tx_TS_admin_list\n");
+			RemoveTsEntry(ieee, pTS, TX_DIR);
+			list_del_init(&pTS->List);
+			list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, RX_DIR);
+			list_del_init(&pTS->List);
+			list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, RX_DIR);
+			list_del_init(&pTS->List);
+			list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
+		}
+	}
+}
+EXPORT_SYMBOL(RemovePeerTS);
+
+void RemoveAllTS(struct rtllib_device *ieee)
+{
+	struct ts_common_info *pTS, *pTmpTS;
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
+		RemoveTsEntry(ieee, pTS, TX_DIR);
+		list_del_init(&pTS->List);
+		list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List) {
+		RemoveTsEntry(ieee, pTS, TX_DIR);
+		list_del_init(&pTS->List);
+		list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List) {
+		RemoveTsEntry(ieee, pTS, RX_DIR);
+		list_del_init(&pTS->List);
+		list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List) {
+		RemoveTsEntry(ieee, pTS, RX_DIR);
+		list_del_init(&pTS->List);
+		list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
+	}
+}
+
+void TsStartAddBaProcess(struct rtllib_device *ieee, struct tx_ts_record *pTxTS)
+{
+	if (pTxTS->bAddBaReqInProgress == false) {
+		pTxTS->bAddBaReqInProgress = true;
+
+		if (pTxTS->bAddBaReqDelayed) {
+			netdev_dbg(ieee->dev, "Start ADDBA after 60 sec!!\n");
+			mod_timer(&pTxTS->TsAddBaTimer, jiffies +
+				  msecs_to_jiffies(TS_ADDBA_DELAY));
+		} else {
+			netdev_dbg(ieee->dev, "Immediately Start ADDBA\n");
+			mod_timer(&pTxTS->TsAddBaTimer, jiffies + 10);
+		}
+	} else
+		netdev_dbg(ieee->dev, "BA timer is already added\n");
+}
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
new file mode 100644
index 000000000..3c72ed2a3
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -0,0 +1,2157 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Merged with mainline rtllib.h in Aug 2004.  Original ieee802_11
+ * remains copyright by the original authors
+ *
+ * Portions of the merged code are based on Host AP (software wireless
+ * LAN access point) driver for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * Modified for Realtek's wi-fi cards by Andrea Merello
+ * <andrea.merello@gmail.com>
+ */
+#ifndef RTLLIB_H
+#define RTLLIB_H
+#include <linux/if_ether.h> /* ETH_ALEN */
+#include <linux/kernel.h>   /* ARRAY_SIZE */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/mutex.h>
+
+#include <linux/delay.h>
+#include <linux/wireless.h>
+
+#include "rtllib_debug.h"
+#include "rtl819x_HT.h"
+#include "rtl819x_BA.h"
+#include "rtl819x_TS.h"
+
+#include <linux/netdevice.h>
+#include <linux/if_arp.h> /* ARPHRD_ETHER */
+#include <net/lib80211.h>
+
+#define MAX_PRECMD_CNT 16
+#define MAX_RFDEPENDCMD_CNT 16
+#define MAX_POSTCMD_CNT 16
+
+#ifndef WIRELESS_SPY
+#define WIRELESS_SPY
+#endif
+#include <net/iw_handler.h>
+
+#ifndef IW_MODE_MONITOR
+#define IW_MODE_MONITOR 6
+#endif
+
+#ifndef IWEVCUSTOM
+#define IWEVCUSTOM 0x8c02
+#endif
+
+#ifndef IW_CUSTOM_MAX
+/* Max number of char in custom event - use multiple of them if needed */
+#define IW_CUSTOM_MAX	256	/* In bytes */
+#endif
+
+#define skb_tail_pointer_rsl(skb) skb_tail_pointer(skb)
+
+#define queue_delayed_work_rsl(x, y, z) queue_delayed_work(x, y, z)
+#define INIT_DELAYED_WORK_RSL(x, y, z) INIT_DELAYED_WORK(x, y)
+
+#define queue_work_rsl(x, y) queue_work(x, y)
+#define INIT_WORK_RSL(x, y, z) INIT_WORK(x, y)
+
+#define container_of_work_rsl(x, y, z) container_of(x, y, z)
+#define container_of_dwork_rsl(x, y, z)				\
+	container_of(to_delayed_work(x), y, z)
+
+#define iwe_stream_add_event_rsl(info, start, stop, iwe, len)	\
+	iwe_stream_add_event(info, start, stop, iwe, len)
+
+#define iwe_stream_add_point_rsl(info, start, stop, iwe, p)	\
+	iwe_stream_add_point(info, start, stop, iwe, p)
+
+static inline void *netdev_priv_rsl(struct net_device *dev)
+{
+	return netdev_priv(dev);
+}
+
+#define KEY_TYPE_NA		0x0
+#define KEY_TYPE_WEP40		0x1
+#define KEY_TYPE_TKIP		0x2
+#define KEY_TYPE_CCMP		0x4
+#define KEY_TYPE_WEP104		0x5
+/* added for rtl819x tx procedure */
+#define MAX_QUEUE_SIZE		0x10
+
+#define BK_QUEUE			       0
+#define BE_QUEUE			       1
+#define VI_QUEUE			       2
+#define VO_QUEUE			       3
+#define HCCA_QUEUE			     4
+#define TXCMD_QUEUE			    5
+#define MGNT_QUEUE			     6
+#define HIGH_QUEUE			     7
+#define BEACON_QUEUE			   8
+
+#ifndef IW_MODE_MESH
+#define IW_MODE_MESH			7
+#endif
+
+#define IE_CISCO_FLAG_POSITION		0x08
+#define SUPPORT_CKIP_MIC			0x08
+#define SUPPORT_CKIP_PK			0x10
+#define	RT_RF_OFF_LEVL_HALT_NIC		BIT3
+#define	RT_IN_PS_LEVEL(pPSC, _PS_FLAG)		\
+	((pPSC->CurPsLevel & _PS_FLAG) ? true : false)
+#define	RT_CLEAR_PS_LEVEL(pPSC, _PS_FLAG)	\
+	(pPSC->CurPsLevel &= (~(_PS_FLAG)))
+#define	RT_SET_PS_LEVEL(pPSC, _PS_FLAG)	(pPSC->CurPsLevel |= _PS_FLAG)
+
+/* defined for skb cb field */
+/* At most 28 byte */
+struct cb_desc {
+	/* Tx Desc Related flags (8-9) */
+	u8 bLastIniPkt:1;
+	u8 bCmdOrInit:1;
+	u8 bFirstSeg:1;
+	u8 bLastSeg:1;
+	u8 bEncrypt:1;
+	u8 bTxDisableRateFallBack:1;
+	u8 bTxUseDriverAssingedRate:1;
+	u8 bHwSec:1;
+
+	u8 nStuckCount;
+
+	/* Tx Firmware Related flags (10-11)*/
+	u8 bCTSEnable:1;
+	u8 bRTSEnable:1;
+	u8 bUseShortGI:1;
+	u8 bUseShortPreamble:1;
+	u8 bTxEnableFwCalcDur:1;
+	u8 bAMPDUEnable:1;
+	u8 bRTSSTBC:1;
+	u8 RTSSC:1;
+
+	u8 bRTSBW:1;
+	u8 bPacketBW:1;
+	u8 bRTSUseShortPreamble:1;
+	u8 bRTSUseShortGI:1;
+	u8 bMulticast:1;
+	u8 bBroadcast:1;
+	u8 drv_agg_enable:1;
+	u8 reserved2:1;
+
+	/* Tx Desc related element(12-19) */
+	u8 rata_index;
+	u8 queue_index;
+	u16 txbuf_size;
+	u8 RATRIndex;
+	u8 bAMSDU:1;
+	u8 bFromAggrQ:1;
+	u8 reserved6:6;
+	u8 macId;
+	u8 priority;
+
+	/* Tx firmware related element(20-27) */
+	u8 data_rate;
+	u8 rts_rate;
+	u8 ampdu_factor;
+	u8 ampdu_density;
+	u8 DrvAggrNum;
+	u8 bdhcp;
+	u16 pkt_size;
+	u8 bIsSpecialDataFrame;
+
+	u8 bBTTxPacket;
+	u8 bIsBTProbRsp;
+};
+
+enum sw_chnl_cmd_id {
+	CmdID_End,
+	CmdID_SetTxPowerLevel,
+	CmdID_BBRegWrite10,
+	CmdID_WritePortUlong,
+	CmdID_WritePortUshort,
+	CmdID_WritePortUchar,
+	CmdID_RF_WriteReg,
+};
+
+struct sw_chnl_cmd {
+	enum sw_chnl_cmd_id CmdID;
+	u32			Para1;
+	u32			Para2;
+	u32			msDelay;
+} __packed;
+
+/*--------------------------Define -------------------------------------------*/
+#define MGN_1M		  0x02
+#define MGN_2M		  0x04
+#define MGN_5_5M		0x0b
+#define MGN_11M		 0x16
+
+#define MGN_6M		  0x0c
+#define MGN_9M		  0x12
+#define MGN_12M		 0x18
+#define MGN_18M		 0x24
+#define MGN_24M		 0x30
+#define MGN_36M		 0x48
+#define MGN_48M		 0x60
+#define MGN_54M		 0x6c
+
+#define MGN_MCS0		0x80
+#define MGN_MCS1		0x81
+#define MGN_MCS2		0x82
+#define MGN_MCS3		0x83
+#define MGN_MCS4		0x84
+#define MGN_MCS5		0x85
+#define MGN_MCS6		0x86
+#define MGN_MCS7		0x87
+#define MGN_MCS8		0x88
+#define MGN_MCS9		0x89
+#define MGN_MCS10	       0x8a
+#define MGN_MCS11	       0x8b
+#define MGN_MCS12	       0x8c
+#define MGN_MCS13	       0x8d
+#define MGN_MCS14	       0x8e
+#define MGN_MCS15	       0x8f
+
+enum hw_variables {
+	HW_VAR_ETHER_ADDR,
+	HW_VAR_MULTICAST_REG,
+	HW_VAR_BASIC_RATE,
+	HW_VAR_BSSID,
+	HW_VAR_MEDIA_STATUS,
+	HW_VAR_SECURITY_CONF,
+	HW_VAR_BEACON_INTERVAL,
+	HW_VAR_ATIM_WINDOW,
+	HW_VAR_LISTEN_INTERVAL,
+	HW_VAR_CS_COUNTER,
+	HW_VAR_DEFAULTKEY0,
+	HW_VAR_DEFAULTKEY1,
+	HW_VAR_DEFAULTKEY2,
+	HW_VAR_DEFAULTKEY3,
+	HW_VAR_SIFS,
+	HW_VAR_DIFS,
+	HW_VAR_EIFS,
+	HW_VAR_SLOT_TIME,
+	HW_VAR_ACK_PREAMBLE,
+	HW_VAR_CW_CONFIG,
+	HW_VAR_CW_VALUES,
+	HW_VAR_RATE_FALLBACK_CONTROL,
+	HW_VAR_CONTENTION_WINDOW,
+	HW_VAR_RETRY_COUNT,
+	HW_VAR_TR_SWITCH,
+	HW_VAR_COMMAND,
+	HW_VAR_WPA_CONFIG,
+	HW_VAR_AMPDU_MIN_SPACE,
+	HW_VAR_SHORTGI_DENSITY,
+	HW_VAR_AMPDU_FACTOR,
+	HW_VAR_MCS_RATE_AVAILABLE,
+	HW_VAR_AC_PARAM,
+	HW_VAR_ACM_CTRL,
+	HW_VAR_DIS_Req_Qsize,
+	HW_VAR_CCX_CHNL_LOAD,
+	HW_VAR_CCX_NOISE_HISTOGRAM,
+	HW_VAR_CCX_CLM_NHM,
+	HW_VAR_TxOPLimit,
+	HW_VAR_TURBO_MODE,
+	HW_VAR_RF_STATE,
+	HW_VAR_RF_OFF_BY_HW,
+	HW_VAR_BUS_SPEED,
+	HW_VAR_SET_DEV_POWER,
+
+	HW_VAR_RCR,
+	HW_VAR_RATR_0,
+	HW_VAR_RRSR,
+	HW_VAR_CPU_RST,
+	HW_VAR_CECHK_BSSID,
+	HW_VAR_LBK_MODE,
+	HW_VAR_AES_11N_FIX,
+	HW_VAR_USB_RX_AGGR,
+	HW_VAR_USER_CONTROL_TURBO_MODE,
+	HW_VAR_RETRY_LIMIT,
+	HW_VAR_INIT_TX_RATE,
+	HW_VAR_TX_RATE_REG,
+	HW_VAR_EFUSE_USAGE,
+	HW_VAR_EFUSE_BYTES,
+	HW_VAR_AUTOLOAD_STATUS,
+	HW_VAR_RF_2R_DISABLE,
+	HW_VAR_SET_RPWM,
+	HW_VAR_H2C_FW_PWRMODE,
+	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_1X1_RECV_COMBINE,
+	HW_VAR_STOP_SEND_BEACON,
+	HW_VAR_TSF_TIMER,
+	HW_VAR_IO_CMD,
+
+	HW_VAR_RF_RECOVERY,
+	HW_VAR_H2C_FW_UPDATE_GTK,
+	HW_VAR_WF_MASK,
+	HW_VAR_WF_CRC,
+	HW_VAR_WF_IS_MAC_ADDR,
+	HW_VAR_H2C_FW_OFFLOAD,
+	HW_VAR_RESET_WFCRC,
+
+	HW_VAR_HANDLE_FW_C2H,
+	HW_VAR_DL_FW_RSVD_PAGE,
+	HW_VAR_AID,
+	HW_VAR_HW_SEQ_ENABLE,
+	HW_VAR_CORRECT_TSF,
+	HW_VAR_BCN_VALID,
+	HW_VAR_FWLPS_RF_ON,
+	HW_VAR_DUAL_TSF_RST,
+	HW_VAR_SWITCH_EPHY_WoWLAN,
+	HW_VAR_INT_MIGRATION,
+	HW_VAR_INT_AC,
+	HW_VAR_RF_TIMING,
+};
+
+enum rt_op_mode {
+	RT_OP_MODE_AP,
+	RT_OP_MODE_INFRASTRUCTURE,
+	RT_OP_MODE_IBSS,
+	RT_OP_MODE_NO_LINK,
+};
+
+
+#define aSifsTime						\
+	 (((priv->rtllib->current_network.mode == IEEE_A)	\
+	|| (priv->rtllib->current_network.mode == IEEE_N_24G)	\
+	|| (priv->rtllib->current_network.mode == IEEE_N_5G)) ? 16 : 10)
+
+#define MGMT_QUEUE_NUM 5
+
+#define MAX_IE_LEN  0xff
+
+#define msleep_interruptible_rsl  msleep_interruptible
+
+/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
+ * 6.2.1.1.2.
+ *
+ * The figure in section 7.1.2 suggests a body size of up to 2312
+ * bytes is allowed, which is a bit confusing, I suspect this
+ * represents the 2304 bytes of real data, plus a possible 8 bytes of
+ * WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro)
+ */
+#define RTLLIB_1ADDR_LEN 10
+#define RTLLIB_2ADDR_LEN 16
+#define RTLLIB_3ADDR_LEN 24
+#define RTLLIB_4ADDR_LEN 30
+#define RTLLIB_FCS_LEN    4
+
+#define RTLLIB_SKBBUFFER_SIZE 2500
+
+#define MIN_FRAG_THRESHOLD     256U
+#define MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define RTLLIB_FCTL_FTYPE		0x000c
+#define RTLLIB_FCTL_STYPE		0x00f0
+#define RTLLIB_FCTL_FRAMETYPE	0x00fc
+#define RTLLIB_FCTL_TODS		0x0100
+#define RTLLIB_FCTL_FROMDS		0x0200
+#define RTLLIB_FCTL_DSTODS		0x0300
+#define RTLLIB_FCTL_MOREFRAGS	0x0400
+#define RTLLIB_FCTL_RETRY		0x0800
+#define RTLLIB_FCTL_PM		0x1000
+#define RTLLIB_FCTL_MOREDATA		0x2000
+#define RTLLIB_FCTL_WEP		0x4000
+#define RTLLIB_FCTL_ORDER		0x8000
+
+#define RTLLIB_FTYPE_MGMT		0x0000
+#define RTLLIB_FTYPE_CTL		0x0004
+#define RTLLIB_FTYPE_DATA		0x0008
+
+/* management */
+#define RTLLIB_STYPE_ASSOC_REQ	0x0000
+#define RTLLIB_STYPE_ASSOC_RESP		0x0010
+#define RTLLIB_STYPE_REASSOC_REQ	0x0020
+#define RTLLIB_STYPE_REASSOC_RESP	0x0030
+#define RTLLIB_STYPE_PROBE_REQ	0x0040
+#define RTLLIB_STYPE_PROBE_RESP	0x0050
+#define RTLLIB_STYPE_BEACON		0x0080
+#define RTLLIB_STYPE_ATIM		0x0090
+#define RTLLIB_STYPE_DISASSOC	0x00A0
+#define RTLLIB_STYPE_AUTH		0x00B0
+#define RTLLIB_STYPE_DEAUTH		0x00C0
+#define RTLLIB_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define RTLLIB_STYPE_PSPOLL		0x00A0
+#define RTLLIB_STYPE_RTS		0x00B0
+#define RTLLIB_STYPE_CTS		0x00C0
+#define RTLLIB_STYPE_ACK		0x00D0
+
+/* data */
+#define RTLLIB_STYPE_DATA		0x0000
+#define RTLLIB_STYPE_DATA_CFACK	0x0010
+#define RTLLIB_STYPE_DATA_CFPOLL	0x0020
+#define RTLLIB_STYPE_DATA_CFACKPOLL	0x0030
+#define RTLLIB_STYPE_NULLFUNC	0x0040
+#define RTLLIB_STYPE_QOS_DATA	0x0080
+#define RTLLIB_STYPE_QOS_NULL	0x00C0
+
+#define RTLLIB_SCTL_FRAG		0x000F
+#define RTLLIB_SCTL_SEQ		0xFFF0
+
+/* QOS control */
+#define RTLLIB_QCTL_TID	      0x000F
+
+#define	FC_QOS_BIT					BIT7
+#define IsDataFrame(pdu)	(((pdu[0] & 0x0C) == 0x08) ? true : false)
+#define	IsLegacyDataFrame(pdu)	(IsDataFrame(pdu) && (!(pdu[0]&FC_QOS_BIT)))
+#define IsQoSDataFrame(pframe)			\
+	((*(u16 *)pframe&(RTLLIB_STYPE_QOS_DATA|RTLLIB_FTYPE_DATA)) ==	\
+	(RTLLIB_STYPE_QOS_DATA|RTLLIB_FTYPE_DATA))
+#define Frame_Order(pframe)     (*(u16 *)pframe&RTLLIB_FCTL_ORDER)
+#define SN_LESS(a, b)		(((a-b)&0x800) != 0)
+#define SN_EQUAL(a, b)	(a == b)
+#define MAX_DEV_ADDR_SIZE 8
+
+enum act_category {
+	ACT_CAT_QOS = 1,
+	ACT_CAT_DLS = 2,
+	ACT_CAT_BA  = 3,
+	ACT_CAT_HT  = 7,
+	ACT_CAT_WMM = 17,
+};
+
+enum ba_action {
+	ACT_ADDBAREQ = 0,
+	ACT_ADDBARSP = 1,
+	ACT_DELBA    = 2,
+};
+
+enum init_gain_op_type {
+	IG_Backup = 0,
+	IG_Restore,
+	IG_Max
+};
+
+enum led_ctl_mode {
+	LED_CTL_POWER_ON = 1,
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7,
+	LED_CTL_START_TO_LINK = 8,
+};
+
+enum rt_rf_type_def {
+	RF_1T2R = 0,
+	RF_2T4R,
+};
+
+enum wireless_mode {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_A = 0x01,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+	WIRELESS_MODE_N_24G = 0x10,
+	WIRELESS_MODE_N_5G = 0x20
+};
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE	0x888E		/* Port Access Entity (IEEE 802.1X) */
+#define ETH_P_IP	0x0800		/* Internet Protocol packet	*/
+#define ETH_P_ARP	0x0806		/* Address Resolution packet	*/
+#endif /* ETH_P_PAE */
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct rtllib_snap_hdr {
+	u8    dsap;   /* always 0xAA */
+	u8    ssap;   /* always 0xAA */
+	u8    ctrl;   /* always 0x03 */
+	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+
+} __packed;
+
+enum _REG_PREAMBLE_MODE {
+	PREAMBLE_LONG = 1,
+	PREAMBLE_AUTO = 2,
+	PREAMBLE_SHORT = 3,
+};
+
+#define SNAP_SIZE sizeof(struct rtllib_snap_hdr)
+
+#define WLAN_FC_GET_TYPE(fc) ((fc) & RTLLIB_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & RTLLIB_FCTL_STYPE)
+#define WLAN_FC_MORE_DATA(fc) ((fc) & RTLLIB_FCTL_MOREDATA)
+
+#define WLAN_FC_GET_FRAMETYPE(fc) ((fc) & RTLLIB_FCTL_FRAMETYPE)
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTLLIB_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  (((seq) & RTLLIB_SCTL_SEQ) >> 4)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+#define WLAN_AUTH_LEAP 128
+
+#define WLAN_CAPABILITY_ESS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_SHORT_SLOT_TIME (1<<10)
+
+#define RTLLIB_STATMASK_SIGNAL (1<<0)
+#define RTLLIB_STATMASK_RSSI (1<<1)
+#define RTLLIB_STATMASK_NOISE (1<<2)
+#define RTLLIB_STATMASK_WEMASK 0x7
+
+#define RTLLIB_CCK_MODULATION    (1<<0)
+#define RTLLIB_OFDM_MODULATION   (1<<1)
+
+#define RTLLIB_24GHZ_BAND     (1<<0)
+#define RTLLIB_52GHZ_BAND     (1<<1)
+
+#define RTLLIB_CCK_RATE_LEN		4
+#define RTLLIB_CCK_RATE_1MB			0x02
+#define RTLLIB_CCK_RATE_2MB			0x04
+#define RTLLIB_CCK_RATE_5MB			0x0B
+#define RTLLIB_CCK_RATE_11MB			0x16
+#define RTLLIB_OFDM_RATE_LEN		8
+#define RTLLIB_OFDM_RATE_6MB			0x0C
+#define RTLLIB_OFDM_RATE_9MB			0x12
+#define RTLLIB_OFDM_RATE_12MB		0x18
+#define RTLLIB_OFDM_RATE_18MB		0x24
+#define RTLLIB_OFDM_RATE_24MB		0x30
+#define RTLLIB_OFDM_RATE_36MB		0x48
+#define RTLLIB_OFDM_RATE_48MB		0x60
+#define RTLLIB_OFDM_RATE_54MB		0x6C
+#define RTLLIB_BASIC_RATE_MASK		0x80
+
+/* this is stolen and modified from the madwifi driver*/
+#define RTLLIB_FC0_TYPE_MASK		0x0c
+#define RTLLIB_FC0_TYPE_DATA		0x08
+#define RTLLIB_FC0_SUBTYPE_MASK	0xB0
+#define RTLLIB_FC0_SUBTYPE_QOS	0x80
+
+#define RTLLIB_QOS_HAS_SEQ(fc) \
+	(((fc) & (RTLLIB_FC0_TYPE_MASK | RTLLIB_FC0_SUBTYPE_MASK)) == \
+	 (RTLLIB_FC0_TYPE_DATA | RTLLIB_FC0_SUBTYPE_QOS))
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+struct ieee_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num[17];
+	u16 frag_num[17];
+	unsigned long packet_time[17];
+	struct list_head list;
+};
+
+/* NOTE: This data is for statistical purposes; not all hardware provides this
+ *       information for frames received.  Not setting these will not cause
+ *       any adverse affects.
+ */
+struct rtllib_rx_stats {
+	u64 mac_time;
+	s8  rssi;
+	u8  signal;
+	u8  noise;
+	u16 rate; /* in 100 kbps */
+	u8  received_channel;
+	u8  control;
+	u8  mask;
+	u8  freq;
+	u16 len;
+	u64 tsf;
+	u32 beacon_time;
+	u8  nic_type;
+	u16 Length;
+	u8  SignalQuality;
+	s32 RecvSignalPower;
+	s8  RxPower;
+	u8  SignalStrength;
+	u16 bHwError:1;
+	u16 bCRC:1;
+	u16 bICV:1;
+	u16 bShortPreamble:1;
+	u16 Antenna:1;
+	u16 Decrypted:1;
+	u16 Wakeup:1;
+	u16 Reserved0:1;
+	u8  AGC;
+	u32 TimeStampLow;
+	u32 TimeStampHigh;
+	bool bShift;
+	bool bIsQosData;
+
+	u8    RxDrvInfoSize;
+	u8    RxBufShift;
+	bool  bIsAMPDU;
+	bool  bFirstMPDU;
+	bool  bContainHTC;
+	bool  RxIs40MHzPacket;
+	u32   RxPWDBAll;
+	u8    RxMIMOSignalStrength[4];
+	s8    RxMIMOSignalQuality[2];
+	bool  bPacketMatchBSSID;
+	bool  bIsCCK;
+	bool  bPacketToSelf;
+	u16    packetlength;
+	u16    fraglength;
+	u16    fragoffset;
+	u16    ntotalfrag;
+	bool   bPacketBeacon;
+	bool   bToSelfBA;
+	u16    Seq_Num;
+};
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly.
+ */
+#define RTLLIB_FRAG_CACHE_LEN 4
+
+struct rtllib_frag_entry {
+	unsigned long first_frag_time;
+	unsigned int seq;
+	unsigned int last_frag;
+	struct sk_buff *skb;
+	u8 src_addr[ETH_ALEN];
+	u8 dst_addr[ETH_ALEN];
+};
+
+struct rtllib_device;
+
+#define SEC_ACTIVE_KEY    (1<<4)
+#define SEC_AUTH_MODE     (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL	 (1<<7)
+#define SEC_ENABLED       (1<<8)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define SEC_ALG_NONE		0
+#define SEC_ALG_WEP		1
+#define SEC_ALG_TKIP		2
+#define SEC_ALG_CCMP		4
+
+#define WEP_KEY_LEN		13
+#define SCM_KEY_LEN		32
+
+struct rtllib_security {
+	u16 active_key:2,
+	    enabled:1,
+	    auth_mode:2,
+	    auth_algo:4,
+	    unicast_uses_group:1,
+	    encrypt:1;
+	u8 key_sizes[NUM_WEP_KEYS];
+	u8 keys[NUM_WEP_KEYS][SCM_KEY_LEN];
+	u8 level;
+	u16 flags;
+} __packed;
+
+
+/* 802.11 data frame from AP
+ *       ,-------------------------------------------------------------------.
+ * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *       |------|------|---------|---------|---------|------|---------|------|
+ * Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+ *       |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *       `-------------------------------------------------------------------'
+ * Total: 28-2340 bytes
+ */
+
+/* Management Frame Information Element Types */
+enum rtllib_mfie {
+	MFIE_TYPE_SSID = 0,
+	MFIE_TYPE_RATES = 1,
+	MFIE_TYPE_FH_SET = 2,
+	MFIE_TYPE_DS_SET = 3,
+	MFIE_TYPE_CF_SET = 4,
+	MFIE_TYPE_TIM = 5,
+	MFIE_TYPE_IBSS_SET = 6,
+	MFIE_TYPE_COUNTRY = 7,
+	MFIE_TYPE_HOP_PARAMS = 8,
+	MFIE_TYPE_HOP_TABLE = 9,
+	MFIE_TYPE_REQUEST = 10,
+	MFIE_TYPE_CHALLENGE = 16,
+	MFIE_TYPE_POWER_CONSTRAINT = 32,
+	MFIE_TYPE_POWER_CAPABILITY = 33,
+	MFIE_TYPE_TPC_REQUEST = 34,
+	MFIE_TYPE_TPC_REPORT = 35,
+	MFIE_TYPE_SUPP_CHANNELS = 36,
+	MFIE_TYPE_CSA = 37,
+	MFIE_TYPE_MEASURE_REQUEST = 38,
+	MFIE_TYPE_MEASURE_REPORT = 39,
+	MFIE_TYPE_QUIET = 40,
+	MFIE_TYPE_IBSS_DFS = 41,
+	MFIE_TYPE_ERP = 42,
+	MFIE_TYPE_HT_CAP = 45,
+	MFIE_TYPE_RSN = 48,
+	MFIE_TYPE_RATES_EX = 50,
+	MFIE_TYPE_HT_INFO = 61,
+	MFIE_TYPE_AIRONET = 133,
+	MFIE_TYPE_GENERIC = 221,
+	MFIE_TYPE_QOS_PARAMETER = 222,
+};
+
+/* Minimal header; can be used for passing 802.11 frames with sufficient
+ * information to determine what type of underlying data type is actually
+ * stored in the data.
+ */
+struct rtllib_pspoll_hdr {
+	__le16 frame_ctl;
+	__le16 aid;
+	u8 bssid[ETH_ALEN];
+	u8 ta[ETH_ALEN];
+} __packed;
+
+struct rtllib_hdr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_1addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_2addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_3addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_4addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_3addrqos {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	__le16 qos_ctl;
+	u8 payload[];
+} __packed;
+
+struct rtllib_hdr_4addrqos {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	__le16 qos_ctl;
+	u8 payload[];
+} __packed;
+
+struct rtllib_info_element {
+	u8 id;
+	u8 len;
+	u8 data[];
+} __packed;
+
+struct rtllib_authentication {
+	struct rtllib_hdr_3addr header;
+	__le16 algorithm;
+	__le16 transaction;
+	__le16 status;
+	/*challenge*/
+	struct rtllib_info_element info_element[];
+} __packed;
+
+struct rtllib_disauth {
+	struct rtllib_hdr_3addr header;
+	__le16 reason;
+} __packed;
+
+struct rtllib_disassoc {
+	struct rtllib_hdr_3addr header;
+	__le16 reason;
+} __packed;
+
+struct rtllib_probe_request {
+	struct rtllib_hdr_3addr header;
+	/* SSID, supported rates */
+	struct rtllib_info_element info_element[];
+} __packed;
+
+struct rtllib_probe_response {
+	struct rtllib_hdr_3addr header;
+	u32 time_stamp[2];
+	__le16 beacon_interval;
+	__le16 capability;
+	/* SSID, supported rates, FH params, DS params,
+	 * CF params, IBSS params, TIM (if beacon), RSN
+	 */
+	struct rtllib_info_element info_element[];
+} __packed;
+
+/* Alias beacon for probe_response */
+#define rtllib_beacon rtllib_probe_response
+
+struct rtllib_assoc_request_frame {
+	struct rtllib_hdr_3addr header;
+	__le16 capability;
+	__le16 listen_interval;
+	/* SSID, supported rates, RSN */
+	struct rtllib_info_element info_element[];
+} __packed;
+
+struct rtllib_assoc_response_frame {
+	struct rtllib_hdr_3addr header;
+	__le16 capability;
+	__le16 status;
+	__le16 aid;
+	struct rtllib_info_element info_element[]; /* supported rates */
+} __packed;
+
+struct rtllib_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u8 queue_index;
+	u8 rts_included;
+	u16 reserved;
+	__le16 frag_size;
+	__le16 payload_size;
+	struct sk_buff *fragments[];
+};
+
+#define MAX_SUBFRAME_COUNT		  64
+struct rtllib_rxb {
+	u8 nr_subframes;
+	struct sk_buff *subframes[MAX_SUBFRAME_COUNT];
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+} __packed;
+
+union frameqos {
+	u16 shortdata;
+	u8  chardata[2];
+	struct {
+		u16 tid:4;
+		u16 eosp:1;
+		u16 ack_policy:2;
+		u16 reserved:1;
+		u16 txop:8;
+	} field;
+};
+
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element...
+ */
+#define MAX_RATES_LENGTH		  ((u8)12)
+#define MAX_RATES_EX_LENGTH	       ((u8)16)
+#define MAX_NETWORK_COUNT		  96
+
+#define MAX_CHANNEL_NUMBER		 161
+#define RTLLIB_SOFTMAC_SCAN_TIME	   100
+#define RTLLIB_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define MAX_WPA_IE_LEN 64
+#define MAX_WZC_IE_LEN 256
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+/* QoS structure */
+#define NETWORK_HAS_QOS_PARAMETERS      (1<<3)
+#define NETWORK_HAS_QOS_INFORMATION     (1<<4)
+#define NETWORK_HAS_QOS_MASK	    (NETWORK_HAS_QOS_PARAMETERS | \
+					 NETWORK_HAS_QOS_INFORMATION)
+/* 802.11h */
+#define NETWORK_HAS_ERP_VALUE	   (1<<10)
+
+#define QOS_QUEUE_NUM		   4
+#define QOS_OUI_LEN		     3
+#define QOS_OUI_TYPE		    2
+#define QOS_ELEMENT_ID		  221
+#define QOS_OUI_INFO_SUB_TYPE	   0
+#define QOS_OUI_PARAM_SUB_TYPE	  1
+#define QOS_VERSION_1		   1
+
+struct rtllib_qos_information_element {
+	u8 elementID;
+	u8 length;
+	u8 qui[QOS_OUI_LEN];
+	u8 qui_type;
+	u8 qui_subtype;
+	u8 version;
+	u8 ac_info;
+} __packed;
+
+struct rtllib_qos_ac_parameter {
+	u8 aci_aifsn;
+	u8 ecw_min_max;
+	__le16 tx_op_limit;
+} __packed;
+
+struct rtllib_qos_parameter_info {
+	struct rtllib_qos_information_element info_element;
+	u8 reserved;
+	struct rtllib_qos_ac_parameter ac_params_record[QOS_QUEUE_NUM];
+} __packed;
+
+struct rtllib_qos_parameters {
+	__le16 cw_min[QOS_QUEUE_NUM];
+	__le16 cw_max[QOS_QUEUE_NUM];
+	u8 aifs[QOS_QUEUE_NUM];
+	u8 flag[QOS_QUEUE_NUM];
+	__le16 tx_op_limit[QOS_QUEUE_NUM];
+} __packed;
+
+struct rtllib_qos_data {
+	struct rtllib_qos_parameters parameters;
+	unsigned int wmm_acm;
+	int active;
+	int supported;
+	u8 param_count;
+	u8 old_param_count;
+};
+
+struct rtllib_tim_parameters {
+	u8 tim_count;
+	u8 tim_period;
+} __packed;
+
+struct rtllib_wmm_ac_param {
+	u8 ac_aci_acm_aifsn;
+	u8 ac_ecwmin_ecwmax;
+	u16 ac_txop_limit;
+};
+
+enum eap_type {
+	EAP_PACKET = 0,
+	EAPOL_START,
+	EAPOL_LOGOFF,
+	EAPOL_KEY,
+	EAPOL_ENCAP_ASF_ALERT
+};
+
+static const char * const eap_types[] = {
+	[EAP_PACKET]		= "EAP-Packet",
+	[EAPOL_START]		= "EAPOL-Start",
+	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
+	[EAPOL_KEY]		= "EAPOL-Key",
+	[EAPOL_ENCAP_ASF_ALERT]	= "EAPOL-Encap-ASF-Alert"
+};
+
+static inline const char *eap_get_type(int type)
+{
+	return ((u32)type >= ARRAY_SIZE(eap_types)) ? "Unknown" :
+		 eap_types[type];
+}
+static inline u8 Frame_QoSTID(u8 *buf)
+{
+	struct rtllib_hdr_3addr *hdr;
+	u16 fc;
+
+	hdr = (struct rtllib_hdr_3addr *)buf;
+	fc = le16_to_cpu(hdr->frame_ctl);
+	return (u8)((union frameqos *)(buf + (((fc & RTLLIB_FCTL_TODS) &&
+		    (fc & RTLLIB_FCTL_FROMDS)) ? 30 : 24)))->field.tid;
+}
+
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __packed;
+
+struct rtllib_softmac_stats {
+	unsigned int rx_ass_ok;
+	unsigned int rx_ass_err;
+	unsigned int rx_probe_rq;
+	unsigned int tx_probe_rs;
+	unsigned int tx_beacons;
+	unsigned int rx_auth_rq;
+	unsigned int rx_auth_rs_ok;
+	unsigned int rx_auth_rs_err;
+	unsigned int tx_auth_rq;
+	unsigned int no_auth_rs;
+	unsigned int no_ass_rs;
+	unsigned int tx_ass_rq;
+	unsigned int rx_ass_rq;
+	unsigned int tx_probe_rq;
+	unsigned int reassoc;
+	unsigned int swtxstop;
+	unsigned int swtxawake;
+	unsigned char CurrentShowTxate;
+	unsigned char last_packet_rate;
+	unsigned int txretrycount;
+};
+
+/* These are the data types that can make up management packets
+ *
+ * u16 auth_algorithm;
+ * u16 auth_sequence;
+ * u16 beacon_interval;
+ * u16 capability;
+ * u8 current_ap[ETH_ALEN];
+ * u16 listen_interval;
+ * struct {
+ *   u16 association_id:14, reserved:2;
+ * } __packed;
+ * u32 time_stamp[2];
+ * u16 reason;
+ * u16 status;
+ */
+
+#define RTLLIB_DEFAULT_TX_ESSID "Penguin"
+#define RTLLIB_DEFAULT_BASIC_RATE 2
+
+enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
+#define MAX_SP_Len  (WMM_all_frame << 4)
+#define RTLLIB_QOS_TID 0x0f
+#define QOS_CTL_NOTCONTAIN_ACK (0x01 << 5)
+
+#define RTLLIB_DTIM_MBCAST 4
+#define RTLLIB_DTIM_UCAST 2
+#define RTLLIB_DTIM_VALID 1
+#define RTLLIB_DTIM_INVALID 0
+
+#define RTLLIB_PS_DISABLED 0
+#define RTLLIB_PS_UNICAST RTLLIB_DTIM_UCAST
+#define RTLLIB_PS_MBCAST RTLLIB_DTIM_MBCAST
+
+#define WME_AC_BK   0x00
+#define WME_AC_BE   0x01
+#define WME_AC_VI   0x02
+#define WME_AC_VO   0x03
+#define WME_AC_PRAM_LEN 16
+
+#define MAX_RECEIVE_BUFFER_SIZE 9100
+
+#define UP2AC(up) (		   \
+	((up) < 1) ? WME_AC_BE : \
+	((up) < 3) ? WME_AC_BK : \
+	((up) < 4) ? WME_AC_BE : \
+	((up) < 6) ? WME_AC_VI : \
+	WME_AC_VO)
+
+#define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
+#define ETHERNET_HEADER_SIZE    14      /* length of two Ethernet address
+					 * plus ether type
+					 */
+
+enum erp_t {
+	ERP_NonERPpresent	= 0x01,
+	ERP_UseProtection	= 0x02,
+	ERP_BarkerPreambleMode = 0x04,
+};
+
+struct rtllib_network {
+	/* These entries are used to identify a unique network */
+	u8 bssid[ETH_ALEN];
+	u8 channel;
+	/* Ensure null-terminated for any debug msgs */
+	u8 ssid[IW_ESSID_MAX_SIZE + 1];
+	u8 ssid_len;
+	u8 hidden_ssid[IW_ESSID_MAX_SIZE + 1];
+	u8 hidden_ssid_len;
+	struct rtllib_qos_data qos_data;
+
+	bool	bWithAironetIE;
+	bool	bCkipSupported;
+	bool	bCcxRmEnable;
+	u8	CcxRmState[2];
+	bool	bMBssidValid;
+	u8	MBssidMask;
+	u8	MBssid[ETH_ALEN];
+	bool	bWithCcxVerNum;
+	u8	BssCcxVerNumber;
+	/* These are network statistics */
+	struct rtllib_rx_stats stats;
+	u16 capability;
+	u8  rates[MAX_RATES_LENGTH];
+	u8  rates_len;
+	u8  rates_ex[MAX_RATES_EX_LENGTH];
+	u8  rates_ex_len;
+	unsigned long last_scanned;
+	u8  mode;
+	u32 flags;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 listen_interval;
+	u16 atim_window;
+	u8  erp_value;
+	u8  wpa_ie[MAX_WPA_IE_LEN];
+	size_t wpa_ie_len;
+	u8  rsn_ie[MAX_WPA_IE_LEN];
+	size_t rsn_ie_len;
+	u8  wzc_ie[MAX_WZC_IE_LEN];
+	size_t wzc_ie_len;
+
+	struct rtllib_tim_parameters tim;
+	u8  dtim_period;
+	u8  dtim_data;
+	u64 last_dtim_sta_time;
+
+	u8 wmm_info;
+	struct rtllib_wmm_ac_param wmm_param[4];
+	u8 Turbo_Enable;
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+	struct bss_ht bssht;
+	bool broadcom_cap_exist;
+	bool realtek_cap_exit;
+	bool marvell_cap_exist;
+	bool ralink_cap_exist;
+	bool atheros_cap_exist;
+	bool cisco_cap_exist;
+	bool airgo_cap_exist;
+	bool unknown_cap_exist;
+	bool	berp_info_valid;
+	bool buseprotection;
+	u8 SignalStrength;
+	u8 RSSI;
+	struct list_head list;
+};
+
+enum rtllib_state {
+
+	/* the card is not linked at all */
+	RTLLIB_NOLINK = 0,
+
+	/* RTLLIB_ASSOCIATING* are for BSS client mode
+	 * the driver shall not perform RX filtering unless
+	 * the state is LINKED.
+	 * The driver shall just check for the state LINKED and
+	 * defaults to NOLINK for ALL the other states (including
+	 * LINKED_SCANNING)
+	 */
+
+	/* the association procedure will start (wq scheduling)*/
+	RTLLIB_ASSOCIATING,
+	RTLLIB_ASSOCIATING_RETRY,
+
+	/* the association procedure is sending AUTH request*/
+	RTLLIB_ASSOCIATING_AUTHENTICATING,
+
+	/* the association procedure has successfully authenticated
+	 * and is sending association request
+	 */
+	RTLLIB_ASSOCIATING_AUTHENTICATED,
+
+	/* the link is ok. the card associated to a BSS or linked
+	 * to a ibss cell or acting as an AP and creating the bss
+	 */
+	RTLLIB_LINKED,
+
+	/* same as LINKED, but the driver shall apply RX filter
+	 * rules as we are in NO_LINK mode. As the card is still
+	 * logically linked, but it is doing a syncro site survey
+	 * then it will be back to LINKED state.
+	 */
+	RTLLIB_LINKED_SCANNING,
+};
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+
+#define CFG_RTLLIB_RESERVE_FCS (1<<0)
+#define CFG_RTLLIB_COMPUTE_FCS (1<<1)
+
+struct tx_pending {
+	int frag;
+	struct rtllib_txb *txb;
+};
+
+struct bandwidth_autoswitch {
+	long threshold_20Mhzto40Mhz;
+	long	threshold_40Mhzto20Mhz;
+	bool bforced_tx20Mhz;
+	bool bautoswitch_enable;
+};
+
+
+
+#define REORDER_WIN_SIZE	128
+#define REORDER_ENTRY_NUM	128
+struct rx_reorder_entry {
+	struct list_head	List;
+	u16			SeqNum;
+	struct rtllib_rxb *prxb;
+};
+enum fsync_state {
+	Default_Fsync,
+	HW_Fsync,
+	SW_Fsync
+};
+
+enum rt_ps_mode {
+	eActive,
+	eMaxPs,
+	eFastPs,
+	eAutoPs,
+};
+
+enum ips_callback_function {
+	IPS_CALLBACK_NONE = 0,
+	IPS_CALLBACK_MGNT_LINK_REQUEST = 1,
+	IPS_CALLBACK_JOIN_REQUEST = 2,
+};
+
+enum rt_rf_power_state {
+	rf_on,
+	rf_sleep,
+	rf_off
+};
+
+struct rt_pwr_save_ctrl {
+
+	bool				bInactivePs;
+	bool				bIPSModeBackup;
+	bool				bSwRfProcessing;
+	enum rt_rf_power_state eInactivePowerState;
+	enum ips_callback_function ReturnPoint;
+
+	bool				bLeisurePs;
+	u8				LpsIdleCount;
+	u8				RegMaxLPSAwakeIntvl;
+	u8				LPSAwakeIntvl;
+
+	u32				CurPsLevel;
+	u32				RegRfPsLevel;
+
+	bool				bFwCtrlLPS;
+
+};
+
+#define RT_RF_CHANGE_SOURCE u32
+
+#define RF_CHANGE_BY_SW BIT31
+#define RF_CHANGE_BY_HW BIT30
+#define RF_CHANGE_BY_PS BIT29
+#define RF_CHANGE_BY_IPS BIT28
+#define RF_CHANGE_BY_INIT	0
+
+enum country_code_type {
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC = 8,
+	COUNTRY_CODE_MIC = 9,
+	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
+	COUNTRY_CODE_WORLD_WIDE_13 = 11,
+	COUNTRY_CODE_TELEC_NETGEAR = 12,
+	COUNTRY_CODE_MAX
+};
+
+enum scan_op_backup_opt {
+	SCAN_OPT_BACKUP = 0,
+	SCAN_OPT_RESTORE,
+	SCAN_OPT_MAX
+};
+
+enum fw_cmd_io_type {
+	FW_CMD_DIG_ENABLE = 0,
+	FW_CMD_DIG_DISABLE = 1,
+	FW_CMD_DIG_HALT = 2,
+	FW_CMD_DIG_RESUME = 3,
+	FW_CMD_HIGH_PWR_ENABLE = 4,
+	FW_CMD_HIGH_PWR_DISABLE = 5,
+	FW_CMD_RA_RESET = 6,
+	FW_CMD_RA_ACTIVE = 7,
+	FW_CMD_RA_REFRESH_N = 8,
+	FW_CMD_RA_REFRESH_BG = 9,
+	FW_CMD_RA_INIT = 10,
+	FW_CMD_IQK_ENABLE = 11,
+	FW_CMD_TXPWR_TRACK_ENABLE = 12,
+	FW_CMD_TXPWR_TRACK_DISABLE = 13,
+	FW_CMD_TXPWR_TRACK_THERMAL = 14,
+	FW_CMD_PAUSE_DM_BY_SCAN = 15,
+	FW_CMD_RESUME_DM_BY_SCAN = 16,
+	FW_CMD_RA_REFRESH_N_COMB = 17,
+	FW_CMD_RA_REFRESH_BG_COMB = 18,
+	FW_CMD_ANTENNA_SW_ENABLE = 19,
+	FW_CMD_ANTENNA_SW_DISABLE = 20,
+	FW_CMD_TX_FEEDBACK_CCX_ENABLE = 21,
+	FW_CMD_LPS_ENTER = 22,
+	FW_CMD_LPS_LEAVE = 23,
+};
+
+#define RT_MAX_LD_SLOT_NUM	10
+struct rt_link_detect {
+
+	u32				NumRecvBcnInPeriod;
+	u32				NumRecvDataInPeriod;
+
+	u32				RxBcnNum[RT_MAX_LD_SLOT_NUM];
+	u32				RxDataNum[RT_MAX_LD_SLOT_NUM];
+	u16				SlotNum;
+	u16				SlotIndex;
+
+	u32				NumTxOkInPeriod;
+	u32				NumRxOkInPeriod;
+	u32				NumRxUnicastOkInPeriod;
+	bool				bBusyTraffic;
+	bool				bHigherBusyTraffic;
+	bool				bHigherBusyRxTraffic;
+};
+
+struct sw_cam_table {
+
+	u8				macaddr[ETH_ALEN];
+	bool				bused;
+	u8				key_buf[16];
+	u16				key_type;
+	u8				useDK;
+	u8				key_index;
+
+};
+#define   TOTAL_CAM_ENTRY				32
+struct rate_adaptive {
+	u8				rate_adaptive_disabled;
+	u8				ratr_state;
+	u16				reserve;
+
+	u32				high_rssi_thresh_for_ra;
+	u32				high2low_rssi_thresh_for_ra;
+	u8				low2high_rssi_thresh_for_ra40M;
+	u32				low_rssi_thresh_for_ra40M;
+	u8				low2high_rssi_thresh_for_ra20M;
+	u32				low_rssi_thresh_for_ra20M;
+	u32				upper_rssi_threshold_ratr;
+	u32				middle_rssi_threshold_ratr;
+	u32				low_rssi_threshold_ratr;
+	u32				low_rssi_threshold_ratr_40M;
+	u32				low_rssi_threshold_ratr_20M;
+	u8				ping_rssi_enable;
+	u32				ping_rssi_ratr;
+	u32				ping_rssi_thresh_for_ra;
+	u32				last_ratr;
+	u8				PreRATRState;
+
+};
+
+#define	NUM_PMKID_CACHE		16
+struct rt_pmkid_list {
+	u8 Bssid[ETH_ALEN];
+	u8 PMKID[16];
+	u8 SsidBuf[33];
+	u8 bUsed;
+};
+
+struct rt_intel_promisc_mode {
+	bool bPromiscuousOn;
+	bool bFilterSourceStationFrame;
+};
+
+
+/*************** DRIVER STATUS   *****/
+#define STATUS_SCANNING			0
+/*************** DRIVER STATUS   *****/
+
+enum {
+	LPS_IS_WAKE = 0,
+	LPS_IS_SLEEP = 1,
+	LPS_WAIT_NULL_DATA_SEND = 2,
+};
+
+struct rtllib_device {
+	struct pci_dev *pdev;
+	struct net_device *dev;
+	struct rtllib_security sec;
+
+	bool disable_mgnt_queue;
+
+	unsigned long status;
+	u8	CntAfterLink;
+
+	enum rt_op_mode OpMode;
+
+	/* The last AssocReq/Resp IEs */
+	u8 *assocreq_ies, *assocresp_ies;
+	size_t assocreq_ies_len, assocresp_ies_len;
+
+	bool	bForcedBgMode;
+	u8 RF_Type;
+
+	u8 hwsec_active;
+	bool is_silent_reset;
+	bool is_roaming;
+	bool ieee_up;
+	bool cannot_notify;
+	bool bSupportRemoteWakeUp;
+	bool actscanning;
+	bool FirstIe_InScan;
+	bool be_scan_inprogress;
+	bool beinretry;
+	enum rt_rf_power_state rf_power_state;
+	RT_RF_CHANGE_SOURCE rf_off_reason;
+	bool is_set_key;
+	bool wx_set_enc;
+	struct rt_hi_throughput *pHTInfo;
+
+	spinlock_t reorder_spinlock;
+	u8	Regdot11HTOperationalRateSet[16];
+	u8	Regdot11TxHTOperationalRateSet[16];
+	u8	dot11HTOperationalRateSet[16];
+	u8	RegHTSuppRateSet[16];
+	u8	HTCurrentOperaRate;
+	u8	HTHighestOperaRate;
+	u8	bTxDisableRateFallBack;
+	u8	bTxUseDriverAssingedRate;
+	u8	bTxEnableFwCalcDur;
+	atomic_t	atm_swbw;
+
+	struct list_head		Tx_TS_Admit_List;
+	struct list_head		Tx_TS_Pending_List;
+	struct list_head		Tx_TS_Unused_List;
+	struct tx_ts_record TxTsRecord[TOTAL_TS_NUM];
+	struct list_head		Rx_TS_Admit_List;
+	struct list_head		Rx_TS_Pending_List;
+	struct list_head		Rx_TS_Unused_List;
+	struct rx_ts_record RxTsRecord[TOTAL_TS_NUM];
+	struct rx_reorder_entry RxReorderEntry[128];
+	struct list_head		RxReorder_Unused_List;
+
+
+	/* Bookkeeping structures */
+	struct net_device_stats stats;
+	struct rtllib_softmac_stats softmac_stats;
+
+	/* Probe / Beacon management */
+	struct list_head network_free_list;
+	struct list_head network_list;
+	struct rtllib_network *networks;
+	int scans;
+	int scan_age;
+
+	int iw_mode; /* operating mode (IW_MODE_*) */
+	bool bNetPromiscuousMode;
+	struct rt_intel_promisc_mode IntelPromiscuousModeInfo;
+
+	spinlock_t lock;
+	spinlock_t wpax_suitlist_lock;
+
+	int tx_headroom; /* Set to size of any additional room needed at front
+			  * of allocated Tx SKBs
+			  */
+	u32 config;
+
+	/* WEP and other encryption related settings at the device level */
+	int open_wep; /* Set to 1 to allow unencrypted frames */
+	int auth_mode;
+	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
+				 * WEP key changes
+				 */
+
+	/* If the host performs {en,de}cryption, then set to 1 */
+	int host_encrypt;
+	int host_decrypt;
+
+	int ieee802_1x; /* is IEEE 802.1X used */
+
+	/* WPA data */
+	bool bHalfWirelessN24GMode;
+	int wpa_enabled;
+	int drop_unencrypted;
+	int tkip_countermeasures;
+	int privacy_invoked;
+	size_t wpa_ie_len;
+	u8 *wpa_ie;
+	size_t wps_ie_len;
+	u8 *wps_ie;
+	u8 ap_mac_addr[ETH_ALEN];
+	u16 pairwise_key_type;
+	u16 group_key_type;
+
+	struct lib80211_crypt_info crypt_info;
+
+	struct sw_cam_table swcamtable[TOTAL_CAM_ENTRY];
+
+	struct rt_pmkid_list PMKIDList[NUM_PMKID_CACHE];
+
+	/* Fragmentation structures */
+	struct rtllib_frag_entry frag_cache[17][RTLLIB_FRAG_CACHE_LEN];
+	unsigned int frag_next_idx[17];
+	u16 fts; /* Fragmentation Threshold */
+#define DEFAULT_RTS_THRESHOLD 2346U
+#define MIN_RTS_THRESHOLD 1
+#define MAX_RTS_THRESHOLD 2346U
+	u16 rts; /* RTS threshold */
+
+	/* Association info */
+	u8 bssid[ETH_ALEN];
+
+	/* This stores infos for the current network.
+	 * Either the network we are associated in INFRASTRUCTURE
+	 * or the network that we are creating in MASTER mode.
+	 * ad-hoc is a mixture ;-).
+	 * Note that in infrastructure mode, even when not associated,
+	 * fields bssid and essid may be valid (if wpa_set and essid_set
+	 * are true) as thy carry the value set by the user via iwconfig
+	 */
+	struct rtllib_network current_network;
+
+	enum rtllib_state state;
+
+	int short_slot;
+	int mode;       /* A, B, G */
+	int modulation; /* CCK, OFDM */
+
+	/* used for forcing the ibss workqueue to terminate
+	 * without wait for the syncro scan to terminate
+	 */
+	short sync_scan_hurryup;
+	u16 scan_watch_dog;
+
+	/* map of allowed channels. 0 is dummy */
+	void *dot11d_info;
+	bool global_domain;
+	u8 active_channel_map[MAX_CHANNEL_NUMBER+1];
+
+	u8   bss_start_channel;
+	u8   ibss_maxjoin_chal;
+
+	int rate;       /* current rate */
+	int basic_rate;
+
+	short active_scan;
+
+	/* this contains flags for selectively enable softmac support */
+	u16 softmac_features;
+
+	/* if the sequence control field is not filled by HW */
+	u16 seq_ctrl[5];
+
+	/* association procedure transaction sequence number */
+	u16 associate_seq;
+
+	/* AID for RTXed association responses */
+	u16 assoc_id;
+
+	/* power save mode related*/
+	u8 ack_tx_to_ieee;
+	short ps;
+	short sta_sleep;
+	int ps_timeout;
+	int ps_period;
+	struct work_struct ps_task;
+	u64 ps_time;
+	bool polling;
+
+	short raw_tx;
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	short queue_stop;
+	short scanning_continue;
+	short proto_started;
+	short proto_stoppping;
+
+	struct mutex wx_mutex;
+	struct mutex scan_mutex;
+	struct mutex ips_mutex;
+
+	spinlock_t mgmt_tx_lock;
+	spinlock_t beacon_lock;
+
+	short beacon_txing;
+
+	short wap_set;
+	short ssid_set;
+
+	/* set on initialization */
+	unsigned int wmm_acm;
+
+	/* for discarding duplicated packets in IBSS */
+	struct list_head ibss_mac_hash[IEEE_IBSS_MAC_HASH_SIZE];
+
+	/* for discarding duplicated packets in BSS */
+	u16 last_rxseq_num[17]; /* rx seq previous per-tid */
+	u16 last_rxfrag_num[17];/* tx frag previous per-tid */
+	unsigned long last_packet_time[17];
+
+	/* for PS mode */
+	unsigned long last_rx_ps_time;
+	bool			bAwakePktSent;
+	u8			LPSDelayCnt;
+
+	/* used if IEEE_SOFTMAC_SINGLE_QUEUE is set */
+	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
+	int mgmt_queue_head;
+	int mgmt_queue_tail;
+	u8 AsocRetryCount;
+	struct sk_buff_head skb_waitQ[MAX_QUEUE_SIZE];
+	struct sk_buff_head  skb_aggQ[MAX_QUEUE_SIZE];
+
+	bool	bdynamic_txpower_enable;
+
+	bool bCTSToSelfEnable;
+
+	u32	fsync_time_interval;
+	u32	fsync_rate_bitmap;
+	u8	fsync_rssi_threshold;
+	bool	bfsync_enable;
+
+	u8	fsync_multiple_timeinterval;
+	u32	fsync_firstdiff_ratethreshold;
+	u32	fsync_seconddiff_ratethreshold;
+	enum fsync_state fsync_state;
+	bool		bis_any_nonbepkts;
+	struct bandwidth_autoswitch bandwidth_auto_switch;
+	bool FwRWRF;
+
+	struct rt_link_detect LinkDetectInfo;
+	bool bIsAggregateFrame;
+	struct rt_pwr_save_ctrl PowerSaveControl;
+
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	struct tx_pending tx_pending;
+
+	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
+	struct timer_list associate_timer;
+
+	/* used if IEEE_SOFTMAC_BEACONS is set */
+	struct timer_list beacon_timer;
+	u8 need_sw_enc;
+	struct work_struct associate_complete_wq;
+	struct work_struct ips_leave_wq;
+	struct delayed_work associate_procedure_wq;
+	struct delayed_work softmac_scan_wq;
+	struct delayed_work associate_retry_wq;
+	struct delayed_work start_ibss_wq;
+	struct delayed_work hw_wakeup_wq;
+	struct delayed_work hw_sleep_wq;
+	struct delayed_work link_change_wq;
+	struct work_struct wx_sync_scan_wq;
+
+	union {
+		struct rtllib_rxb *RfdArray[REORDER_WIN_SIZE];
+		struct rtllib_rxb *stats_IndicateArray[REORDER_WIN_SIZE];
+		struct rtllib_rxb *prxbIndicateArray[REORDER_WIN_SIZE];
+		struct {
+			struct sw_chnl_cmd PreCommonCmd[MAX_PRECMD_CNT];
+			struct sw_chnl_cmd PostCommonCmd[MAX_POSTCMD_CNT];
+			struct sw_chnl_cmd RfDependCmd[MAX_RFDEPENDCMD_CNT];
+		};
+	};
+
+	/* Callback functions */
+	void (*set_security)(struct net_device *dev,
+			     struct rtllib_security *sec);
+
+	/* Used to TX data frame by using txb structs.
+	 * this is not used if in the softmac_features
+	 * is set the flag IEEE_SOFTMAC_TX_QUEUE
+	 */
+	int (*hard_start_xmit)(struct rtllib_txb *txb,
+			       struct net_device *dev);
+
+	int (*reset_port)(struct net_device *dev);
+
+	/* Softmac-generated frames (management) are TXed via this
+	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
+	 * not set. As some cards may have different HW queues that
+	 * one might want to use for data and management frames
+	 * the option to have two callbacks might be useful.
+	 * This function can't sleep.
+	 */
+	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev);
+
+	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
+	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
+	 * frames. If the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * then also management frames are sent via this callback.
+	 * This function can't sleep.
+	 */
+	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev, int rate);
+
+	/* stops the HW queue for DATA frames. Useful to avoid
+	 * waste time to TX data frame when we are reassociating
+	 * This function can sleep.
+	 */
+	void (*data_hard_stop)(struct net_device *dev);
+
+	/* OK this is complementing to data_poll_hard_stop */
+	void (*data_hard_resume)(struct net_device *dev);
+
+	/* ask to the driver to retune the radio.
+	 * This function can sleep. the driver should ensure
+	 * the radio has been switched before return.
+	 */
+	void (*set_chan)(struct net_device *dev, short ch);
+
+	void (*rtllib_start_hw_scan)(struct net_device *dev);
+	void (*rtllib_stop_hw_scan)(struct net_device *dev);
+
+	/* indicate the driver that the link state is changed
+	 * for example it may indicate the card is associated now.
+	 * Driver might be interested in this to apply RX filter
+	 * rules or simply light the LINK led
+	 */
+	void (*link_change)(struct net_device *dev);
+
+	/* these two function indicates to the HW when to start
+	 * and stop to send beacons. This is used when the
+	 * IEEE_SOFTMAC_BEACONS is not set. For now the
+	 * stop_send_bacons is NOT guaranteed to be called only
+	 * after start_send_beacons.
+	 */
+	void (*start_send_beacons)(struct net_device *dev);
+	void (*stop_send_beacons)(struct net_device *dev);
+
+	/* power save mode related */
+	void (*sta_wake_up)(struct net_device *dev);
+	void (*enter_sleep_state)(struct net_device *dev, u64 time);
+	short (*ps_is_queue_empty)(struct net_device *dev);
+	int (*handle_beacon)(struct net_device *dev,
+			     struct rtllib_beacon *beacon,
+			     struct rtllib_network *network);
+	int (*handle_assoc_response)(struct net_device *dev,
+				     struct rtllib_assoc_response_frame *resp,
+				     struct rtllib_network *network);
+
+
+	/* check whether Tx hw resource available */
+	short (*check_nic_enough_desc)(struct net_device *dev, int queue_index);
+	void (*SetBWModeHandler)(struct net_device *dev,
+				 enum ht_channel_width bandwidth,
+				 enum ht_extchnl_offset Offset);
+	bool (*GetNmodeSupportBySecCfg)(struct net_device *dev);
+	void (*SetWirelessMode)(struct net_device *dev, u8 wireless_mode);
+	bool (*GetHalfNmodeSupportByAPsHandler)(struct net_device *dev);
+	u8   (*rtllib_ap_sec_type)(struct rtllib_device *ieee);
+	void (*InitialGainHandler)(struct net_device *dev, u8 Operation);
+	bool (*SetFwCmdHandler)(struct net_device *dev,
+				enum fw_cmd_io_type FwCmdIO);
+	void (*UpdateBeaconInterruptHandler)(struct net_device *dev,
+					     bool start);
+	void (*ScanOperationBackupHandler)(struct net_device *dev,
+					   u8 Operation);
+	void (*LedControlHandler)(struct net_device *dev,
+				  enum led_ctl_mode LedAction);
+	void (*SetHwRegHandler)(struct net_device *dev, u8 variable, u8 *val);
+
+	void (*AllowAllDestAddrHandler)(struct net_device *dev,
+					bool bAllowAllDA, bool WriteIntoReg);
+
+	void (*rtllib_ips_leave_wq)(struct net_device *dev);
+	void (*rtllib_ips_leave)(struct net_device *dev);
+	void (*LeisurePSLeave)(struct net_device *dev);
+
+	/* This must be the last item so that it points to the data
+	 * allocated beyond this structure by alloc_rtllib
+	 */
+	u8 priv[];
+};
+
+#define IEEE_A	    (1<<0)
+#define IEEE_B	    (1<<1)
+#define IEEE_G	    (1<<2)
+#define IEEE_N_24G		  (1<<4)
+#define	IEEE_N_5G		  (1<<5)
+#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
+
+/* Generate a 802.11 header */
+
+/* Uses the channel change callback directly
+ * instead of [start/stop] scan callbacks
+ */
+#define IEEE_SOFTMAC_SCAN (1<<2)
+
+/* Perform authentication and association handshake */
+#define IEEE_SOFTMAC_ASSOCIATE (1<<3)
+
+/* Generate probe requests */
+#define IEEE_SOFTMAC_PROBERQ (1<<4)
+
+/* Generate response to probe requests */
+#define IEEE_SOFTMAC_PROBERS (1<<5)
+
+/* The ieee802.11 stack will manage the netif queue
+ * wake/stop for the driver, taking care of 802.11
+ * fragmentation. See softmac.c for details.
+ */
+#define IEEE_SOFTMAC_TX_QUEUE (1<<7)
+
+/* Uses only the softmac_data_hard_start_xmit
+ * even for TX management frames.
+ */
+#define IEEE_SOFTMAC_SINGLE_QUEUE (1<<8)
+
+/* Generate beacons.  The stack will enqueue beacons
+ * to the card
+ */
+#define IEEE_SOFTMAC_BEACONS (1<<6)
+
+
+static inline void *rtllib_priv(struct net_device *dev)
+{
+	return ((struct rtllib_device *)netdev_priv(dev))->priv;
+}
+
+static inline int rtllib_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+static inline int rtllib_get_hdrlen(u16 fc)
+{
+	int hdrlen = RTLLIB_3ADDR_LEN;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case RTLLIB_FTYPE_DATA:
+		if ((fc & RTLLIB_FCTL_FROMDS) && (fc & RTLLIB_FCTL_TODS))
+			hdrlen = RTLLIB_4ADDR_LEN; /* Addr4 */
+		if (RTLLIB_QOS_HAS_SEQ(fc))
+			hdrlen += 2; /* QOS ctrl*/
+		break;
+	case RTLLIB_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case RTLLIB_STYPE_CTS:
+		case RTLLIB_STYPE_ACK:
+			hdrlen = RTLLIB_1ADDR_LEN;
+			break;
+		default:
+			hdrlen = RTLLIB_2ADDR_LEN;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+static inline u8 *rtllib_get_payload(struct rtllib_hdr *hdr)
+{
+	switch (rtllib_get_hdrlen(le16_to_cpu(hdr->frame_ctl))) {
+	case RTLLIB_1ADDR_LEN:
+		return ((struct rtllib_hdr_1addr *)hdr)->payload;
+	case RTLLIB_2ADDR_LEN:
+		return ((struct rtllib_hdr_2addr *)hdr)->payload;
+	case RTLLIB_3ADDR_LEN:
+		return ((struct rtllib_hdr_3addr *)hdr)->payload;
+	case RTLLIB_4ADDR_LEN:
+		return ((struct rtllib_hdr_4addr *)hdr)->payload;
+	}
+	return NULL;
+}
+
+static inline int rtllib_is_ofdm_rate(u8 rate)
+{
+	switch (rate & ~RTLLIB_BASIC_RATE_MASK) {
+	case RTLLIB_OFDM_RATE_6MB:
+	case RTLLIB_OFDM_RATE_9MB:
+	case RTLLIB_OFDM_RATE_12MB:
+	case RTLLIB_OFDM_RATE_18MB:
+	case RTLLIB_OFDM_RATE_24MB:
+	case RTLLIB_OFDM_RATE_36MB:
+	case RTLLIB_OFDM_RATE_48MB:
+	case RTLLIB_OFDM_RATE_54MB:
+		return 1;
+	}
+	return 0;
+}
+
+static inline int rtllib_is_cck_rate(u8 rate)
+{
+	switch (rate & ~RTLLIB_BASIC_RATE_MASK) {
+	case RTLLIB_CCK_RATE_1MB:
+	case RTLLIB_CCK_RATE_2MB:
+	case RTLLIB_CCK_RATE_5MB:
+	case RTLLIB_CCK_RATE_11MB:
+		return 1;
+	}
+	return 0;
+}
+
+
+/* rtllib.c */
+void free_rtllib(struct net_device *dev);
+struct net_device *alloc_rtllib(int sizeof_priv);
+
+/* rtllib_tx.c */
+
+int rtllib_encrypt_fragment(
+	struct rtllib_device *ieee,
+	struct sk_buff *frag,
+	int hdr_len);
+
+netdev_tx_t rtllib_xmit(struct sk_buff *skb,  struct net_device *dev);
+void rtllib_txb_free(struct rtllib_txb *txb);
+
+/* rtllib_rx.c */
+int rtllib_rx(struct rtllib_device *ieee, struct sk_buff *skb,
+	      struct rtllib_rx_stats *rx_stats);
+void rtllib_rx_probe_rq(struct rtllib_device *ieee,
+			struct sk_buff *skb);
+int rtllib_legal_channel(struct rtllib_device *rtllib, u8 channel);
+
+/* rtllib_wx.c */
+int rtllib_wx_get_scan(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *key);
+int rtllib_wx_set_encode(struct rtllib_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *key);
+int rtllib_wx_get_encode(struct rtllib_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *key);
+int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+int rtllib_wx_set_auth(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       struct iw_param *data, char *extra);
+int rtllib_wx_set_mlme(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra);
+int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len);
+
+/* rtllib_softmac.c */
+int rtllib_rx_frame_softmac(struct rtllib_device *ieee, struct sk_buff *skb,
+			    struct rtllib_rx_stats *rx_stats, u16 type,
+			    u16 stype);
+void rtllib_softmac_new_net(struct rtllib_device *ieee,
+			    struct rtllib_network *net);
+
+void SendDisassociation(struct rtllib_device *ieee, bool deauth, u16 asRsn);
+void rtllib_softmac_xmit(struct rtllib_txb *txb, struct rtllib_device *ieee);
+
+void rtllib_start_ibss(struct rtllib_device *ieee);
+int rtllib_softmac_init(struct rtllib_device *ieee);
+void rtllib_softmac_free(struct rtllib_device *ieee);
+void rtllib_disassociate(struct rtllib_device *ieee);
+void rtllib_stop_scan(struct rtllib_device *ieee);
+bool rtllib_act_scanning(struct rtllib_device *ieee, bool sync_scan);
+void rtllib_stop_scan_syncro(struct rtllib_device *ieee);
+void rtllib_start_scan_syncro(struct rtllib_device *ieee, u8 is_mesh);
+void rtllib_sta_ps_send_null_frame(struct rtllib_device *ieee, short pwr);
+void rtllib_sta_ps_send_pspoll_frame(struct rtllib_device *ieee);
+void rtllib_start_protocol(struct rtllib_device *ieee);
+void rtllib_stop_protocol(struct rtllib_device *ieee, u8 shutdown);
+
+void rtllib_EnableNetMonitorMode(struct net_device *dev, bool bInitState);
+void rtllib_DisableNetMonitorMode(struct net_device *dev, bool bInitState);
+void rtllib_EnableIntelPromiscuousMode(struct net_device *dev, bool bInitState);
+void rtllib_DisableIntelPromiscuousMode(struct net_device *dev,
+					bool bInitState);
+void rtllib_softmac_stop_protocol(struct rtllib_device *ieee,
+				  u8 mesh_flag, u8 shutdown);
+void rtllib_softmac_start_protocol(struct rtllib_device *ieee, u8 mesh_flag);
+
+void rtllib_reset_queue(struct rtllib_device *ieee);
+void rtllib_wake_all_queues(struct rtllib_device *ieee);
+void rtllib_stop_all_queues(struct rtllib_device *ieee);
+struct sk_buff *rtllib_get_beacon(struct rtllib_device *ieee);
+void rtllib_start_send_beacons(struct rtllib_device *ieee);
+void rtllib_stop_send_beacons(struct rtllib_device *ieee);
+
+void notify_wx_assoc_event(struct rtllib_device *ieee);
+void rtllib_ps_tx_ack(struct rtllib_device *ieee, short success);
+
+void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee);
+u8 rtllib_ap_sec_type(struct rtllib_device *ieee);
+
+/* rtllib_softmac_wx.c */
+
+int rtllib_wx_get_wap(struct rtllib_device *ieee, struct iw_request_info *info,
+		      union iwreq_data *wrqu, char *ext);
+
+int rtllib_wx_set_wap(struct rtllib_device *ieee, struct iw_request_info *info,
+		      union iwreq_data *awrq, char *extra);
+
+int rtllib_wx_get_essid(struct rtllib_device *ieee, struct iw_request_info *a,
+			union iwreq_data *wrqu, char *b);
+
+int rtllib_wx_set_rate(struct rtllib_device *ieee, struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_get_rate(struct rtllib_device *ieee, struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_set_mode(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b);
+
+int rtllib_wx_set_scan(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b);
+
+int rtllib_wx_set_essid(struct rtllib_device *ieee, struct iw_request_info *a,
+			union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_get_mode(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b);
+
+int rtllib_wx_set_freq(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b);
+
+int rtllib_wx_get_freq(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b);
+void rtllib_wx_sync_scan_wq(void *data);
+
+int rtllib_wx_set_rawtx(struct rtllib_device *ieee,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_get_name(struct rtllib_device *ieee, struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_set_power(struct rtllib_device *ieee,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_get_power(struct rtllib_device *ieee,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_set_rts(struct rtllib_device *ieee, struct iw_request_info *info,
+		      union iwreq_data *wrqu, char *extra);
+
+int rtllib_wx_get_rts(struct rtllib_device *ieee, struct iw_request_info *info,
+		      union iwreq_data *wrqu, char *extra);
+#define MAX_RECEIVE_BUFFER_SIZE 9100
+
+void HTSetConnectBwMode(struct rtllib_device *ieee,
+			enum ht_channel_width bandwidth,
+			enum ht_extchnl_offset Offset);
+void HTUpdateDefaultSetting(struct rtllib_device *ieee);
+void HTConstructCapabilityElement(struct rtllib_device *ieee,
+				  u8 *posHTCap, u8 *len,
+				  u8 isEncrypt, bool bAssoc);
+void HTConstructInfoElement(struct rtllib_device *ieee,
+			    u8 *posHTInfo, u8 *len, u8 isEncrypt);
+void HTConstructRT2RTAggElement(struct rtllib_device *ieee,
+				u8 *posRT2RTAgg, u8 *len);
+void HTOnAssocRsp(struct rtllib_device *ieee);
+void HTInitializeHTInfo(struct rtllib_device *ieee);
+void HTInitializeBssDesc(struct bss_ht *pBssHT);
+void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
+				   struct rtllib_network *pNetwork);
+void HT_update_self_and_peer_setting(struct rtllib_device *ieee,
+				     struct rtllib_network *pNetwork);
+u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
+		       u8 *pMCSFilter);
+extern u8 MCS_FILTER_ALL[];
+extern u16 MCS_DATA_RATE[2][2][77];
+u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame);
+void HTResetIOTSetting(struct rt_hi_throughput *pHTInfo);
+bool IsHTHalfNmodeAPs(struct rtllib_device *ieee);
+u16  TxCountToDataRate(struct rtllib_device *ieee, u8 nDataRate);
+int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb);
+int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb);
+int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb);
+void TsInitAddBA(struct rtllib_device *ieee, struct tx_ts_record *pTS,
+		 u8 Policy, u8 bOverwritePending);
+void TsInitDelBA(struct rtllib_device *ieee,
+		 struct ts_common_info *pTsCommonInfo,
+		 enum tr_select TxRxSelect);
+void BaSetupTimeOut(struct timer_list *t);
+void TxBaInactTimeout(struct timer_list *t);
+void RxBaInactTimeout(struct timer_list *t);
+void ResetBaEntry(struct ba_record *pBA);
+bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS, u8 *Addr,
+	   u8 TID, enum tr_select TxRxSelect, bool bAddNewTs);
+void TSInitialize(struct rtllib_device *ieee);
+void TsStartAddBaProcess(struct rtllib_device *ieee,
+			 struct tx_ts_record *pTxTS);
+void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr);
+void RemoveAllTS(struct rtllib_device *ieee);
+
+extern const long rtllib_wlan_frequencies[];
+
+static inline const char *escape_essid(const char *essid, u8 essid_len)
+{
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+
+	if (rtllib_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	snprintf(escaped, sizeof(escaped), "%*pE", essid_len, essid);
+	return escaped;
+}
+
+/* fun with the built-in rtllib stack... */
+bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn);
+
+
+/* For the function is more related to hardware setting, it's better to use the
+ * ieee handler to refer to it.
+ */
+void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,
+				 struct rx_ts_record *pTS);
+int rtllib_parse_info_param(struct rtllib_device *ieee,
+			    struct rtllib_info_element *info_element,
+			    u16 length,
+			    struct rtllib_network *network,
+			    struct rtllib_rx_stats *stats);
+
+void rtllib_indicate_packets(struct rtllib_device *ieee,
+			     struct rtllib_rxb **prxbIndicateArray, u8  index);
+void HTUseDefaultSetting(struct rtllib_device *ieee);
+#define RT_ASOC_RETRY_LIMIT	5
+u8 MgntQuery_TxRateExcludeCCKRates(struct rtllib_device *ieee);
+
+#endif /* RTLLIB_H */
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
new file mode 100644
index 000000000..a8d22da8b
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -0,0 +1,421 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Host AP crypt: host-based CCMP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/string.h>
+#include <linux/wireless.h>
+#include "rtllib.h"
+
+#include <linux/crypto.h>
+#include <crypto/aead.h>
+
+#include <linux/scatterlist.h>
+
+#define AES_BLOCK_LEN 16
+#define CCMP_HDR_LEN 8
+#define CCMP_MIC_LEN 8
+#define CCMP_TK_LEN 16
+#define CCMP_PN_LEN 6
+
+struct rtllib_ccmp_data {
+	u8 key[CCMP_TK_LEN];
+	int key_set;
+
+	u8 tx_pn[CCMP_PN_LEN];
+	u8 rx_pn[CCMP_PN_LEN];
+
+	u32 dot11RSNAStatsCCMPFormatErrors;
+	u32 dot11RSNAStatsCCMPReplays;
+	u32 dot11RSNAStatsCCMPDecryptErrors;
+
+	int key_idx;
+
+	struct crypto_aead *tfm;
+
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 tx_aad[2 * AES_BLOCK_LEN];
+	u8 rx_aad[2 * AES_BLOCK_LEN];
+};
+
+static void *rtllib_ccmp_init(int key_idx)
+{
+	struct rtllib_ccmp_data *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	priv->key_idx = key_idx;
+
+	priv->tfm = crypto_alloc_aead("ccm(aes)", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tfm)) {
+		pr_debug("Could not allocate crypto API aes\n");
+		priv->tfm = NULL;
+		goto fail;
+	}
+	return priv;
+
+fail:
+	if (priv) {
+		if (priv->tfm)
+			crypto_free_aead(priv->tfm);
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void rtllib_ccmp_deinit(void *priv)
+{
+	struct rtllib_ccmp_data *_priv = priv;
+
+	if (_priv && _priv->tfm)
+		crypto_free_aead(_priv->tfm);
+	kfree(priv);
+}
+
+
+static int ccmp_init_iv_and_aad(struct rtllib_hdr_4addr *hdr,
+				u8 *pn, u8 *iv, u8 *aad)
+{
+	u8 *pos, qc = 0;
+	size_t aad_len;
+	u16 fc;
+	int a4_included, qc_included;
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	a4_included = ((fc & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
+		       (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS));
+
+	qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_FTYPE_DATA) &&
+		       (WLAN_FC_GET_STYPE(fc) & 0x80));
+	aad_len = 22;
+	if (a4_included)
+		aad_len += 6;
+	if (qc_included) {
+		pos = (u8 *)&hdr->addr4;
+		if (a4_included)
+			pos += 6;
+		qc = *pos & 0x0f;
+		aad_len += 2;
+	}
+	/* In CCM, the initial vectors (IV) used for CTR mode encryption and CBC
+	 * mode authentication are not allowed to collide, yet both are derived
+	 * from the same vector. We only set L := 1 here to indicate that the
+	 * data size can be represented in (L+1) bytes. The CCM layer will take
+	 * care of storing the data length in the top (L+1) bytes and setting
+	 * and clearing the other bits as is required to derive the two IVs.
+	 */
+	iv[0] = 0x1;
+
+	/* Nonce: QC | A2 | PN */
+	iv[1] = qc;
+	memcpy(iv + 2, hdr->addr2, ETH_ALEN);
+	memcpy(iv + 8, pn, CCMP_PN_LEN);
+
+	/* AAD:
+	 * FC with bits 4..6 and 11..13 masked to zero; 14 is always one
+	 * A1 | A2 | A3
+	 * SC with bits 4..15 (seq#) masked to zero
+	 * A4 (if present)
+	 * QC (if present)
+	 */
+	pos = (u8 *)hdr;
+	aad[0] = pos[0] & 0x8f;
+	aad[1] = pos[1] & 0xc7;
+	memcpy(&aad[2], &hdr->addr1, ETH_ALEN);
+	memcpy(&aad[8], &hdr->addr2, ETH_ALEN);
+	memcpy(&aad[14], &hdr->addr3, ETH_ALEN);
+	pos = (u8 *)&hdr->seq_ctl;
+	aad[20] = pos[0] & 0x0f;
+	aad[21] = 0; /* all bits masked */
+	memset(aad + 22, 0, 8);
+	if (a4_included)
+		memcpy(aad + 22, hdr->addr4, ETH_ALEN);
+	if (qc_included) {
+		aad[a4_included ? 28 : 22] = qc;
+		/* rest of QC masked */
+	}
+
+	return aad_len;
+}
+
+
+
+static int rtllib_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct rtllib_ccmp_data *key = priv;
+	int i;
+	u8 *pos;
+	struct rtllib_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	if (skb_headroom(skb) < CCMP_HDR_LEN ||
+	    skb_tailroom(skb) < CCMP_MIC_LEN ||
+	    skb->len < hdr_len)
+		return -1;
+
+	pos = skb_push(skb, CCMP_HDR_LEN);
+	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
+	pos += hdr_len;
+
+	i = CCMP_PN_LEN - 1;
+	while (i >= 0) {
+		key->tx_pn[i]++;
+		if (key->tx_pn[i] != 0)
+			break;
+		i--;
+	}
+
+	*pos++ = key->tx_pn[5];
+	*pos++ = key->tx_pn[4];
+	*pos++ = 0;
+	*pos++ = (key->key_idx << 6) | (1 << 5) /* Ext IV included */;
+	*pos++ = key->tx_pn[3];
+	*pos++ = key->tx_pn[2];
+	*pos++ = key->tx_pn[1];
+	*pos++ = key->tx_pn[0];
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	if (!tcb_desc->bHwSec) {
+		struct aead_request *req;
+		struct scatterlist sg[2];
+		u8 *aad = key->tx_aad;
+		u8 iv[AES_BLOCK_LEN];
+		int aad_len, ret;
+		int data_len = skb->len - hdr_len - CCMP_HDR_LEN;
+
+		req = aead_request_alloc(key->tfm, GFP_ATOMIC);
+		if (!req)
+			return -ENOMEM;
+
+		aad_len = ccmp_init_iv_and_aad(hdr, key->tx_pn, iv, aad);
+
+		skb_put(skb, CCMP_MIC_LEN);
+		sg_init_table(sg, 2);
+		sg_set_buf(&sg[0], aad, aad_len);
+		sg_set_buf(&sg[1], skb->data + hdr_len + CCMP_HDR_LEN,
+			   data_len + CCMP_MIC_LEN);
+
+		aead_request_set_callback(req, 0, NULL, NULL);
+		aead_request_set_ad(req, aad_len);
+		aead_request_set_crypt(req, sg, sg, data_len, iv);
+
+		ret = crypto_aead_encrypt(req);
+		aead_request_free(req);
+
+		return ret;
+	}
+
+	return 0;
+}
+
+
+static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct rtllib_ccmp_data *key = priv;
+	u8 keyidx, *pos;
+	struct rtllib_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u8 pn[6];
+
+	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -1;
+	}
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & (1 << 5))) {
+		if (net_ratelimit()) {
+			pr_debug("CCMP: received packet without ExtIV flag from %pM\n",
+				 hdr->addr2);
+		}
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -2;
+	}
+	keyidx >>= 6;
+	if (key->key_idx != keyidx) {
+		pr_debug("CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+			 key->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!key->key_set) {
+		if (net_ratelimit()) {
+			pr_debug("CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
+				 hdr->addr2, keyidx);
+		}
+		return -3;
+	}
+
+	pn[0] = pos[7];
+	pn[1] = pos[6];
+	pn[2] = pos[5];
+	pn[3] = pos[4];
+	pn[4] = pos[1];
+	pn[5] = pos[0];
+	pos += 8;
+	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
+		key->dot11RSNAStatsCCMPReplays++;
+		return -4;
+	}
+	if (!tcb_desc->bHwSec) {
+		size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN;
+		struct aead_request *req;
+		struct scatterlist sg[2];
+		u8 *aad = key->rx_aad;
+		u8 iv[AES_BLOCK_LEN];
+		int aad_len, ret;
+
+		req = aead_request_alloc(key->tfm, GFP_ATOMIC);
+		if (!req)
+			return -ENOMEM;
+
+		aad_len = ccmp_init_iv_and_aad(hdr, pn, iv, aad);
+
+		sg_init_table(sg, 2);
+		sg_set_buf(&sg[0], aad, aad_len);
+		sg_set_buf(&sg[1], pos, data_len);
+
+		aead_request_set_callback(req, 0, NULL, NULL);
+		aead_request_set_ad(req, aad_len);
+		aead_request_set_crypt(req, sg, sg, data_len, iv);
+
+		ret = crypto_aead_decrypt(req);
+		aead_request_free(req);
+
+		if (ret) {
+			if (net_ratelimit()) {
+				pr_debug("CCMP: decrypt failed: STA= %pM\n",
+					 hdr->addr2);
+			}
+			key->dot11RSNAStatsCCMPDecryptErrors++;
+			return -5;
+		}
+
+		memcpy(key->rx_pn, pn, CCMP_PN_LEN);
+	}
+	/* Remove hdr and MIC */
+	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
+	skb_pull(skb, CCMP_HDR_LEN);
+	skb_trim(skb, skb->len - CCMP_MIC_LEN);
+
+	return keyidx;
+}
+
+
+static int rtllib_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct rtllib_ccmp_data *data = priv;
+	int keyidx;
+	struct crypto_aead *tfm = data->tfm;
+
+	keyidx = data->key_idx;
+	memset(data, 0, sizeof(*data));
+	data->key_idx = keyidx;
+	data->tfm = tfm;
+	if (len == CCMP_TK_LEN) {
+		memcpy(data->key, key, CCMP_TK_LEN);
+		data->key_set = 1;
+		if (seq) {
+			data->rx_pn[0] = seq[5];
+			data->rx_pn[1] = seq[4];
+			data->rx_pn[2] = seq[3];
+			data->rx_pn[3] = seq[2];
+			data->rx_pn[4] = seq[1];
+			data->rx_pn[5] = seq[0];
+		}
+		if (crypto_aead_setauthsize(data->tfm, CCMP_MIC_LEN) ||
+			crypto_aead_setkey(data->tfm, data->key, CCMP_TK_LEN))
+			return -1;
+	} else if (len == 0) {
+		data->key_set = 0;
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+
+static int rtllib_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct rtllib_ccmp_data *data = priv;
+
+	if (len < CCMP_TK_LEN)
+		return -1;
+
+	if (!data->key_set)
+		return 0;
+	memcpy(key, data->key, CCMP_TK_LEN);
+
+	if (seq) {
+		seq[0] = data->tx_pn[5];
+		seq[1] = data->tx_pn[4];
+		seq[2] = data->tx_pn[3];
+		seq[3] = data->tx_pn[2];
+		seq[4] = data->tx_pn[1];
+		seq[5] = data->tx_pn[0];
+	}
+
+	return CCMP_TK_LEN;
+}
+
+
+static void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
+{
+	struct rtllib_ccmp_data *ccmp = priv;
+
+	seq_printf(m,
+		   "key[%d] alg=CCMP key_set=%d tx_pn=%pM rx_pn=%pM format_errors=%d replays=%d decrypt_errors=%d\n",
+		   ccmp->key_idx, ccmp->key_set,
+		   ccmp->tx_pn, ccmp->rx_pn,
+		   ccmp->dot11RSNAStatsCCMPFormatErrors,
+		   ccmp->dot11RSNAStatsCCMPReplays,
+		   ccmp->dot11RSNAStatsCCMPDecryptErrors);
+}
+
+static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
+	.name			= "R-CCMP",
+	.init			= rtllib_ccmp_init,
+	.deinit			= rtllib_ccmp_deinit,
+	.encrypt_mpdu		= rtllib_ccmp_encrypt,
+	.decrypt_mpdu		= rtllib_ccmp_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= rtllib_ccmp_set_key,
+	.get_key		= rtllib_ccmp_get_key,
+	.print_stats		= rtllib_ccmp_print_stats,
+	.extra_mpdu_prefix_len = CCMP_HDR_LEN,
+	.extra_mpdu_postfix_len = CCMP_MIC_LEN,
+	.owner			= THIS_MODULE,
+};
+
+
+static int __init rtllib_crypto_ccmp_init(void)
+{
+	return lib80211_register_crypto_ops(&rtllib_crypt_ccmp);
+}
+
+
+static void __exit rtllib_crypto_ccmp_exit(void)
+{
+	lib80211_unregister_crypto_ops(&rtllib_crypt_ccmp);
+}
+
+module_init(rtllib_crypto_ccmp_init);
+module_exit(rtllib_crypto_ccmp_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
new file mode 100644
index 000000000..8bc95651e
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -0,0 +1,731 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypt: host-based TKIP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <crypto/arc4.h>
+#include <crypto/hash.h>
+#include <linux/fips.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/string.h>
+#include <linux/crc32.h>
+#include <linux/etherdevice.h>
+
+#include "rtllib.h"
+
+struct rtllib_tkip_data {
+#define TKIP_KEY_LEN 32
+	u8 key[TKIP_KEY_LEN];
+	int key_set;
+
+	u32 tx_iv32;
+	u16 tx_iv16;
+	u16 tx_ttak[5];
+	int tx_phase1_done;
+
+	u32 rx_iv32;
+	u16 rx_iv16;
+	bool initialized;
+	u16 rx_ttak[5];
+	int rx_phase1_done;
+	u32 rx_iv32_new;
+	u16 rx_iv16_new;
+
+	u32 dot11RSNAStatsTKIPReplays;
+	u32 dot11RSNAStatsTKIPICVErrors;
+	u32 dot11RSNAStatsTKIPLocalMICFailures;
+
+	int key_idx;
+	struct arc4_ctx rx_ctx_arc4;
+	struct arc4_ctx tx_ctx_arc4;
+	struct crypto_shash *rx_tfm_michael;
+	struct crypto_shash *tx_tfm_michael;
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 rx_hdr[16];
+	u8 tx_hdr[16];
+};
+
+static void *rtllib_tkip_init(int key_idx)
+{
+	struct rtllib_tkip_data *priv;
+
+	if (fips_enabled)
+		return NULL;
+
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	priv->key_idx = key_idx;
+
+	priv->tx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
+	if (IS_ERR(priv->tx_tfm_michael)) {
+		pr_debug("Could not allocate crypto API michael_mic\n");
+		priv->tx_tfm_michael = NULL;
+		goto fail;
+	}
+
+	priv->rx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
+	if (IS_ERR(priv->rx_tfm_michael)) {
+		pr_debug("Could not allocate crypto API michael_mic\n");
+		priv->rx_tfm_michael = NULL;
+		goto fail;
+	}
+	return priv;
+
+fail:
+	if (priv) {
+		crypto_free_shash(priv->tx_tfm_michael);
+		crypto_free_shash(priv->rx_tfm_michael);
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void rtllib_tkip_deinit(void *priv)
+{
+	struct rtllib_tkip_data *_priv = priv;
+
+	if (_priv) {
+		crypto_free_shash(_priv->tx_tfm_michael);
+		crypto_free_shash(_priv->rx_tfm_michael);
+	}
+	kfree_sensitive(priv);
+}
+
+
+static inline u16 RotR1(u16 val)
+{
+	return (val >> 1) | (val << 15);
+}
+
+
+static inline u8 Lo8(u16 val)
+{
+	return val & 0xff;
+}
+
+
+static inline u8 Hi8(u16 val)
+{
+	return val >> 8;
+}
+
+
+static inline u16 Lo16(u32 val)
+{
+	return val & 0xffff;
+}
+
+
+static inline u16 Hi16(u32 val)
+{
+	return val >> 16;
+}
+
+
+static inline u16 Mk16(u8 hi, u8 lo)
+{
+	return lo | (hi << 8);
+}
+
+
+static inline u16 Mk16_le(u16 *v)
+{
+	return *v;
+}
+
+
+static const u16 Sbox[256] = {
+	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+};
+
+
+static inline u16 _S_(u16 v)
+{
+	u16 t = Sbox[Hi8(v)];
+	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
+}
+
+
+#define PHASE1_LOOP_COUNT 8
+
+
+static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
+{
+	int i, j;
+
+	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
+	TTAK[0] = Lo16(IV32);
+	TTAK[1] = Hi16(IV32);
+	TTAK[2] = Mk16(TA[1], TA[0]);
+	TTAK[3] = Mk16(TA[3], TA[2]);
+	TTAK[4] = Mk16(TA[5], TA[4]);
+
+	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
+		j = 2 * (i & 1);
+		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
+		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
+		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
+		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
+		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
+	}
+}
+
+
+static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
+			       u16 IV16)
+{
+	/* Make temporary area overlap WEP seed so that the final copy can be
+	 * avoided on little endian hosts.
+	 */
+	u16 *PPK = (u16 *)&WEPSeed[4];
+
+	/* Step 1 - make copy of TTAK and bring in TSC */
+	PPK[0] = TTAK[0];
+	PPK[1] = TTAK[1];
+	PPK[2] = TTAK[2];
+	PPK[3] = TTAK[3];
+	PPK[4] = TTAK[4];
+	PPK[5] = TTAK[4] + IV16;
+
+	/* Step 2 - 96-bit bijective mixing using S-box */
+	PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *)&TK[0]));
+	PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *)&TK[2]));
+	PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *)&TK[4]));
+	PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *)&TK[6]));
+	PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *)&TK[8]));
+	PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *)&TK[10]));
+
+	PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *)&TK[12]));
+	PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *)&TK[14]));
+	PPK[2] += RotR1(PPK[1]);
+	PPK[3] += RotR1(PPK[2]);
+	PPK[4] += RotR1(PPK[3]);
+	PPK[5] += RotR1(PPK[4]);
+
+	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
+	 * WEPSeed[0..2] is transmitted as WEP IV
+	 */
+	WEPSeed[0] = Hi8(IV16);
+	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
+	WEPSeed[2] = Lo8(IV16);
+	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *)&TK[0])) >> 1);
+
+#ifdef __BIG_ENDIAN
+	{
+		int i;
+
+		for (i = 0; i < 6; i++)
+			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
+	}
+#endif
+}
+
+
+static int rtllib_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+	int len;
+	u8 *pos;
+	struct rtllib_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	int ret = 0;
+	u8 rc4key[16],  *icv;
+	u32 crc;
+
+	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+
+	if (!tcb_desc->bHwSec) {
+		if (!tkey->tx_phase1_done) {
+			tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
+					tkey->tx_iv32);
+			tkey->tx_phase1_done = 1;
+		}
+		tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak,
+				   tkey->tx_iv16);
+	} else
+		tkey->tx_phase1_done = 1;
+
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 8);
+	memmove(pos, pos + 8, hdr_len);
+	pos += hdr_len;
+
+	if (tcb_desc->bHwSec) {
+		*pos++ = Hi8(tkey->tx_iv16);
+		*pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
+		*pos++ = Lo8(tkey->tx_iv16);
+	} else {
+		*pos++ = rc4key[0];
+		*pos++ = rc4key[1];
+		*pos++ = rc4key[2];
+	}
+
+	*pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
+	*pos++ = tkey->tx_iv32 & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
+
+	if (!tcb_desc->bHwSec) {
+		icv = skb_put(skb, 4);
+		crc = ~crc32_le(~0, pos, len);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		arc4_setkey(&tkey->tx_ctx_arc4, rc4key, 16);
+		arc4_crypt(&tkey->tx_ctx_arc4, pos, pos, len + 4);
+	}
+
+	tkey->tx_iv16++;
+	if (tkey->tx_iv16 == 0) {
+		tkey->tx_phase1_done = 0;
+		tkey->tx_iv32++;
+	}
+
+	if (!tcb_desc->bHwSec)
+		return ret;
+	return 0;
+
+
+}
+
+static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+	u8 keyidx, *pos;
+	u32 iv32;
+	u16 iv16;
+	struct rtllib_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u8 rc4key[16];
+	u8 icv[4];
+	u32 crc;
+	int plen;
+
+	if (skb->len < hdr_len + 8 + 4)
+		return -1;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & (1 << 5))) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev,
+				   "Received packet without ExtIV flag from %pM\n",
+				   hdr->addr2);
+		}
+		return -2;
+	}
+	keyidx >>= 6;
+	if (tkey->key_idx != keyidx) {
+		netdev_dbg(skb->dev,
+			   "RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+			   tkey->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!tkey->key_set) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev,
+				   "Received packet from %pM with keyid=%d that does not have a configured key\n",
+				   hdr->addr2, keyidx);
+		}
+		return -3;
+	}
+	iv16 = (pos[0] << 8) | pos[2];
+	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
+	pos += 8;
+
+	if (!tcb_desc->bHwSec || (skb->cb[0] == 1)) {
+		if ((iv32 < tkey->rx_iv32 ||
+		    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) &&
+		    tkey->initialized) {
+			if (net_ratelimit()) {
+				netdev_dbg(skb->dev,
+					   "Replay detected: STA= %pM previous TSC %08x%04x received TSC %08x%04x\n",
+					   hdr->addr2, tkey->rx_iv32,
+					   tkey->rx_iv16, iv32, iv16);
+			}
+			tkey->dot11RSNAStatsTKIPReplays++;
+			return -4;
+		}
+		tkey->initialized = true;
+
+		if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
+			tkip_mixing_phase1(tkey->rx_ttak, tkey->key,
+					   hdr->addr2, iv32);
+			tkey->rx_phase1_done = 1;
+		}
+		tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
+
+		plen = skb->len - hdr_len - 12;
+
+		arc4_setkey(&tkey->rx_ctx_arc4, rc4key, 16);
+		arc4_crypt(&tkey->rx_ctx_arc4, pos, pos, plen + 4);
+
+		crc = ~crc32_le(~0, pos, plen);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		if (memcmp(icv, pos + plen, 4) != 0) {
+			if (iv32 != tkey->rx_iv32) {
+				/* Previously cached Phase1 result was already
+				 * lost, so it needs to be recalculated for the
+				 * next packet.
+				 */
+				tkey->rx_phase1_done = 0;
+			}
+			if (net_ratelimit()) {
+				netdev_dbg(skb->dev,
+					   "ICV error detected: STA= %pM\n",
+					   hdr->addr2);
+			}
+			tkey->dot11RSNAStatsTKIPICVErrors++;
+			return -5;
+		}
+
+	}
+
+	/* Update real counters only after Michael MIC verification has
+	 * completed
+	 */
+	tkey->rx_iv32_new = iv32;
+	tkey->rx_iv16_new = iv16;
+
+	/* Remove IV and ICV */
+	memmove(skb->data + 8, skb->data, hdr_len);
+	skb_pull(skb, 8);
+	skb_trim(skb, skb->len - 4);
+
+	return keyidx;
+}
+
+
+static int michael_mic(struct crypto_shash *tfm_michael, u8 *key, u8 *hdr,
+		       u8 *data, size_t data_len, u8 *mic)
+{
+	SHASH_DESC_ON_STACK(desc, tfm_michael);
+	int err;
+
+	desc->tfm = tfm_michael;
+
+	if (crypto_shash_setkey(tfm_michael, key, 8))
+		return -1;
+
+	err = crypto_shash_init(desc);
+	if (err)
+		goto out;
+	err = crypto_shash_update(desc, hdr, 16);
+	if (err)
+		goto out;
+	err = crypto_shash_update(desc, data, data_len);
+	if (err)
+		goto out;
+	err = crypto_shash_final(desc, mic);
+
+out:
+	shash_desc_zero(desc);
+	return err;
+}
+
+static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
+{
+	struct rtllib_hdr_4addr *hdr11;
+
+	hdr11 = (struct rtllib_hdr_4addr *)skb->data;
+	switch (le16_to_cpu(hdr11->frame_ctl) &
+		(RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS)) {
+	case RTLLIB_FCTL_TODS:
+		ether_addr_copy(hdr, hdr11->addr3); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr2); /* SA */
+		break;
+	case RTLLIB_FCTL_FROMDS:
+		ether_addr_copy(hdr, hdr11->addr1); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr3); /* SA */
+		break;
+	case RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS:
+		ether_addr_copy(hdr, hdr11->addr3); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr4); /* SA */
+		break;
+	case 0:
+		ether_addr_copy(hdr, hdr11->addr1); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr2); /* SA */
+		break;
+	}
+
+	hdr[12] = 0; /* priority */
+
+	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
+}
+
+
+static int rtllib_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+	u8 *pos;
+	struct rtllib_hdr_4addr *hdr;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+
+	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
+		netdev_dbg(skb->dev,
+			   "Invalid packet for Michael MIC add (tailroom=%d hdr_len=%d skb->len=%d)\n",
+			   skb_tailroom(skb), hdr_len, skb->len);
+		return -1;
+	}
+
+	michael_mic_hdr(skb, tkey->tx_hdr);
+
+	if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
+		tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	pos = skb_put(skb, 8);
+	if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
+	    skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+		return -1;
+
+	return 0;
+}
+
+
+static void rtllib_michael_mic_failure(struct net_device *dev,
+				       struct rtllib_hdr_4addr *hdr,
+				       int keyidx)
+{
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure ev;
+
+	/* TODO: needed parameters: count, keyid, key type, TSC */
+	memset(&ev, 0, sizeof(ev));
+	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
+	if (hdr->addr1[0] & 0x01)
+		ev.flags |= IW_MICFAILURE_GROUP;
+	else
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	ether_addr_copy(ev.src_addr.sa_data, hdr->addr2);
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
+}
+
+static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
+				     int hdr_len, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+	u8 mic[8];
+	struct rtllib_hdr_4addr *hdr;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+
+	if (!tkey->key_set)
+		return -1;
+
+	michael_mic_hdr(skb, tkey->rx_hdr);
+	if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
+		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+
+	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+		return -1;
+
+	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
+		struct rtllib_hdr_4addr *hdr;
+
+		hdr = (struct rtllib_hdr_4addr *)skb->data;
+		netdev_dbg(skb->dev,
+			   "Michael MIC verification failed for MSDU from %pM keyidx=%d\n",
+			   hdr->addr2, keyidx);
+		netdev_dbg(skb->dev, "%d\n",
+			   memcmp(mic, skb->data + skb->len - 8, 8) != 0);
+		if (skb->dev) {
+			pr_info("skb->dev != NULL\n");
+			rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
+		}
+		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
+		return -1;
+	}
+
+	/* Update TSC counters for RX now that the packet verification has
+	 * completed.
+	 */
+	tkey->rx_iv32 = tkey->rx_iv32_new;
+	tkey->rx_iv16 = tkey->rx_iv16_new;
+
+	skb_trim(skb, skb->len - 8);
+
+	return 0;
+}
+
+
+static int rtllib_tkip_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+	int keyidx;
+	struct crypto_shash *tfm = tkey->tx_tfm_michael;
+	struct crypto_shash *tfm3 = tkey->rx_tfm_michael;
+
+	keyidx = tkey->key_idx;
+	memset(tkey, 0, sizeof(*tkey));
+	tkey->key_idx = keyidx;
+	tkey->tx_tfm_michael = tfm;
+	tkey->rx_tfm_michael = tfm3;
+
+	if (len == TKIP_KEY_LEN) {
+		memcpy(tkey->key, key, TKIP_KEY_LEN);
+		tkey->key_set = 1;
+		tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
+		if (seq) {
+			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
+				(seq[3] << 8) | seq[2];
+			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
+		}
+	} else if (len == 0)
+		tkey->key_set = 0;
+	else
+		return -1;
+
+	return 0;
+}
+
+
+static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct rtllib_tkip_data *tkey = priv;
+
+	if (len < TKIP_KEY_LEN)
+		return -1;
+
+	if (!tkey->key_set)
+		return 0;
+	memcpy(key, tkey->key, TKIP_KEY_LEN);
+
+	if (seq) {
+		/* Return the sequence number of the last transmitted frame. */
+		u16 iv16 = tkey->tx_iv16;
+		u32 iv32 = tkey->tx_iv32;
+
+		if (iv16 == 0)
+			iv32--;
+		iv16--;
+		seq[0] = tkey->tx_iv16;
+		seq[1] = tkey->tx_iv16 >> 8;
+		seq[2] = tkey->tx_iv32;
+		seq[3] = tkey->tx_iv32 >> 8;
+		seq[4] = tkey->tx_iv32 >> 16;
+		seq[5] = tkey->tx_iv32 >> 24;
+	}
+
+	return TKIP_KEY_LEN;
+}
+
+
+static void rtllib_tkip_print_stats(struct seq_file *m, void *priv)
+{
+	struct rtllib_tkip_data *tkip = priv;
+
+	seq_printf(m,
+		   "key[%d] alg=TKIP key_set=%d tx_pn=%02x%02x%02x%02x%02x%02x rx_pn=%02x%02x%02x%02x%02x%02x replays=%d icv_errors=%d local_mic_failures=%d\n",
+		   tkip->key_idx, tkip->key_set,
+		   (tkip->tx_iv32 >> 24) & 0xff,
+		   (tkip->tx_iv32 >> 16) & 0xff,
+		   (tkip->tx_iv32 >> 8) & 0xff,
+		   tkip->tx_iv32 & 0xff,
+		   (tkip->tx_iv16 >> 8) & 0xff,
+		   tkip->tx_iv16 & 0xff,
+		   (tkip->rx_iv32 >> 24) & 0xff,
+		   (tkip->rx_iv32 >> 16) & 0xff,
+		   (tkip->rx_iv32 >> 8) & 0xff,
+		   tkip->rx_iv32 & 0xff,
+		   (tkip->rx_iv16 >> 8) & 0xff,
+		   tkip->rx_iv16 & 0xff,
+		   tkip->dot11RSNAStatsTKIPReplays,
+		   tkip->dot11RSNAStatsTKIPICVErrors,
+		   tkip->dot11RSNAStatsTKIPLocalMICFailures);
+}
+
+static struct lib80211_crypto_ops rtllib_crypt_tkip = {
+	.name			= "R-TKIP",
+	.init			= rtllib_tkip_init,
+	.deinit			= rtllib_tkip_deinit,
+	.encrypt_mpdu		= rtllib_tkip_encrypt,
+	.decrypt_mpdu		= rtllib_tkip_decrypt,
+	.encrypt_msdu		= rtllib_michael_mic_add,
+	.decrypt_msdu		= rtllib_michael_mic_verify,
+	.set_key		= rtllib_tkip_set_key,
+	.get_key		= rtllib_tkip_get_key,
+	.print_stats		= rtllib_tkip_print_stats,
+	.extra_mpdu_prefix_len = 4 + 4,	/* IV + ExtIV */
+	.extra_mpdu_postfix_len = 4,	/* ICV */
+	.extra_msdu_postfix_len = 8,	/* MIC */
+	.owner			= THIS_MODULE,
+};
+
+
+static int __init rtllib_crypto_tkip_init(void)
+{
+	return lib80211_register_crypto_ops(&rtllib_crypt_tkip);
+}
+
+
+static void __exit rtllib_crypto_tkip_exit(void)
+{
+	lib80211_unregister_crypto_ops(&rtllib_crypt_tkip);
+}
+
+module_init(rtllib_crypto_tkip_init);
+module_exit(rtllib_crypto_tkip_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_wep.c b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
new file mode 100644
index 000000000..7790271a6
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
@@ -0,0 +1,249 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypt: host-based WEP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2002-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <crypto/arc4.h>
+#include <linux/fips.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/string.h>
+#include "rtllib.h"
+
+#include <linux/crc32.h>
+
+struct prism2_wep_data {
+	u32 iv;
+#define WEP_KEY_LEN 13
+	u8 key[WEP_KEY_LEN + 1];
+	u8 key_len;
+	u8 key_idx;
+	struct arc4_ctx rx_ctx_arc4;
+	struct arc4_ctx tx_ctx_arc4;
+};
+
+
+static void *prism2_wep_init(int keyidx)
+{
+	struct prism2_wep_data *priv;
+
+	if (fips_enabled)
+		return NULL;
+
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		return NULL;
+	priv->key_idx = keyidx;
+
+	/* start WEP IV from a random value */
+	get_random_bytes(&priv->iv, 4);
+
+	return priv;
+}
+
+
+static void prism2_wep_deinit(void *priv)
+{
+	kfree_sensitive(priv);
+}
+
+/* Perform WEP encryption on given skb that has at least 4 bytes of headroom
+ * for IV and 4 bytes of tailroom for ICV. Both IV and ICV will be transmitted,
+ * so the payload length increases with 8 bytes.
+ *
+ * WEP frame payload: IV + TX key idx, RC4(data), ICV = RC4(CRC32(data))
+ */
+static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+	u32 klen, len;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 *pos;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u32 crc;
+	u8 *icv;
+
+	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len){
+		pr_err("Error!!! headroom=%d tailroom=%d skblen=%d hdr_len=%d\n",
+		       skb_headroom(skb), skb_tailroom(skb), skb->len, hdr_len);
+		return -1;
+	}
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 4);
+	memmove(pos, pos + 4, hdr_len);
+	pos += hdr_len;
+
+	klen = 3 + wep->key_len;
+
+	wep->iv++;
+
+	/* Fluhrer, Mantin, and Shamir have reported weaknesses in the key
+	 * scheduling algorithm of RC4. At least IVs (KeyByte + 3, 0xff, N)
+	 * can be used to speedup attacks, so avoid using them.
+	 */
+	if ((wep->iv & 0xff00) == 0xff00) {
+		u8 B = (wep->iv >> 16) & 0xff;
+
+		if (B >= 3 && B < klen)
+			wep->iv += 0x0100;
+	}
+
+	/* Prepend 24-bit IV to RC4 key and TX frame */
+	*pos++ = key[0] = (wep->iv >> 16) & 0xff;
+	*pos++ = key[1] = (wep->iv >> 8) & 0xff;
+	*pos++ = key[2] = wep->iv & 0xff;
+	*pos++ = wep->key_idx << 6;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+	if (!tcb_desc->bHwSec) {
+		/* Append little-endian CRC32 and encrypt it to produce ICV */
+		crc = ~crc32_le(~0, pos, len);
+		icv = skb_put(skb, 4);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		arc4_setkey(&wep->tx_ctx_arc4, key, klen);
+		arc4_crypt(&wep->tx_ctx_arc4, pos, pos, len + 4);
+	}
+
+	return 0;
+}
+
+
+/* Perform WEP decryption on given struct buffer. Buffer includes whole WEP
+ * part of the frame: IV (4 bytes), encrypted payload (including SNAP header),
+ * ICV (4 bytes). len includes both IV and ICV.
+ *
+ * Returns 0 if frame was decrypted successfully and ICV was correct and -1 on
+ * failure. If frame is OK, IV and ICV will be removed.
+ */
+static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+	u32  klen, plen;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 keyidx, *pos;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
+				    MAX_DEV_ADDR_SIZE);
+	u32 crc;
+	u8 icv[4];
+
+	if (skb->len < hdr_len + 8)
+		return -1;
+
+	pos = skb->data + hdr_len;
+	key[0] = *pos++;
+	key[1] = *pos++;
+	key[2] = *pos++;
+	keyidx = *pos++ >> 6;
+	if (keyidx != wep->key_idx)
+		return -1;
+
+	klen = 3 + wep->key_len;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+	/* Apply RC4 to data and compute CRC32 over decrypted data */
+	plen = skb->len - hdr_len - 8;
+
+	if (!tcb_desc->bHwSec) {
+		arc4_setkey(&wep->rx_ctx_arc4, key, klen);
+		arc4_crypt(&wep->rx_ctx_arc4, pos, pos, plen + 4);
+
+		crc = ~crc32_le(~0, pos, plen);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+		if (memcmp(icv, pos + plen, 4) != 0) {
+			/* ICV mismatch - drop frame */
+			return -2;
+		}
+	}
+	/* Remove IV and ICV */
+	memmove(skb->data + 4, skb->data, hdr_len);
+	skb_pull(skb, 4);
+	skb_trim(skb, skb->len - 4);
+
+	return 0;
+}
+
+
+static int prism2_wep_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < 0 || len > WEP_KEY_LEN)
+		return -1;
+
+	memcpy(wep->key, key, len);
+	wep->key_len = len;
+
+	return 0;
+}
+
+
+static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < wep->key_len)
+		return -1;
+
+	memcpy(key, wep->key, wep->key_len);
+
+	return wep->key_len;
+}
+
+
+static void prism2_wep_print_stats(struct seq_file *m, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	seq_printf(m, "key[%d] alg=WEP len=%d\n", wep->key_idx, wep->key_len);
+}
+
+static struct lib80211_crypto_ops rtllib_crypt_wep = {
+	.name			= "R-WEP",
+	.init			= prism2_wep_init,
+	.deinit			= prism2_wep_deinit,
+	.encrypt_mpdu		= prism2_wep_encrypt,
+	.decrypt_mpdu		= prism2_wep_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= prism2_wep_set_key,
+	.get_key		= prism2_wep_get_key,
+	.print_stats		= prism2_wep_print_stats,
+	.extra_mpdu_prefix_len  = 4,	/* IV */
+	.extra_mpdu_postfix_len = 4,	/* ICV */
+	.owner			= THIS_MODULE,
+};
+
+
+static int __init rtllib_crypto_wep_init(void)
+{
+	return lib80211_register_crypto_ops(&rtllib_crypt_wep);
+}
+
+
+static void __exit rtllib_crypto_wep_exit(void)
+{
+	lib80211_unregister_crypto_ops(&rtllib_crypt_wep);
+}
+
+module_init(rtllib_crypto_wep_init);
+module_exit(rtllib_crypto_wep_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_debug.h b/drivers/staging/rtl8192e/rtllib_debug.h
new file mode 100644
index 000000000..f6b23defe
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_debug.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Contact Information: wlanfae <wlanfae@realtek.com>
+ */
+#ifndef _RTL_DEBUG_H
+#define _RTL_DEBUG_H
+
+#include <linux/bits.h>
+
+/* Allow files to override DRV_NAME */
+#ifndef DRV_NAME
+#define DRV_NAME "rtllib_92e"
+#endif
+
+extern u32 rt_global_debug_component;
+
+/* These are the defines for rt_global_debug_component */
+enum RTL_DEBUG {
+	COMP_TRACE		= BIT(0),
+	COMP_DBG		= BIT(1),
+	COMP_INIT		= BIT(2),
+	COMP_RECV		= BIT(3),
+	COMP_POWER		= BIT(6),
+	COMP_SWBW		= BIT(8),
+	COMP_SEC		= BIT(9),
+	COMP_LPS		= BIT(10),
+	COMP_QOS		= BIT(11),
+	COMP_RATE		= BIT(12),
+	COMP_RXDESC		= BIT(13),
+	COMP_PHY		= BIT(14),
+	COMP_DIG		= BIT(15),
+	COMP_TXAGC		= BIT(16),
+	COMP_HALDM		= BIT(17),
+	COMP_POWER_TRACKING	= BIT(18),
+	COMP_CH			= BIT(19),
+	COMP_RF			= BIT(20),
+	COMP_FIRMWARE		= BIT(21),
+	COMP_RESET		= BIT(23),
+	COMP_CMDPKT		= BIT(24),
+	COMP_SCAN		= BIT(25),
+	COMP_PS			= BIT(26),
+	COMP_DOWN		= BIT(27),
+	COMP_INTR		= BIT(28),
+	COMP_ERR		= BIT(31)
+};
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
new file mode 100644
index 000000000..41697ef55
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -0,0 +1,185 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2004 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are based on the WEP enablement code provided by the
+ * Host AP project hostap-drivers v0.1.3
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <net/arp.h>
+#include "rtllib.h"
+
+u32 rt_global_debug_component = COMP_ERR;
+EXPORT_SYMBOL(rt_global_debug_component);
+
+static inline int rtllib_networks_allocate(struct rtllib_device *ieee)
+{
+	if (ieee->networks)
+		return 0;
+
+	ieee->networks = kcalloc(MAX_NETWORK_COUNT,
+				 sizeof(struct rtllib_network), GFP_KERNEL);
+	if (!ieee->networks)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static inline void rtllib_networks_free(struct rtllib_device *ieee)
+{
+	if (!ieee->networks)
+		return;
+	kfree(ieee->networks);
+	ieee->networks = NULL;
+}
+
+static inline void rtllib_networks_initialize(struct rtllib_device *ieee)
+{
+	int i;
+
+	INIT_LIST_HEAD(&ieee->network_free_list);
+	INIT_LIST_HEAD(&ieee->network_list);
+	for (i = 0; i < MAX_NETWORK_COUNT; i++)
+		list_add_tail(&ieee->networks[i].list,
+			      &ieee->network_free_list);
+}
+
+struct net_device *alloc_rtllib(int sizeof_priv)
+{
+	struct rtllib_device *ieee = NULL;
+	struct net_device *dev;
+	int i, err;
+
+	pr_debug("rtllib: Initializing...\n");
+
+	dev = alloc_etherdev(sizeof(struct rtllib_device) + sizeof_priv);
+	if (!dev) {
+		pr_err("Unable to allocate net_device.\n");
+		return NULL;
+	}
+	ieee = (struct rtllib_device *)netdev_priv_rsl(dev);
+	ieee->dev = dev;
+
+	err = rtllib_networks_allocate(ieee);
+	if (err) {
+		pr_err("Unable to allocate beacon storage: %d\n", err);
+		goto free_netdev;
+	}
+	rtllib_networks_initialize(ieee);
+
+	/* Default fragmentation threshold is maximum payload size */
+	ieee->fts = DEFAULT_FTS;
+	ieee->scan_age = DEFAULT_MAX_SCAN_AGE;
+	ieee->open_wep = 1;
+
+	/* Default to enabling full open WEP with host based encrypt/decrypt */
+	ieee->host_encrypt = 1;
+	ieee->host_decrypt = 1;
+	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
+
+	ieee->rtllib_ap_sec_type = rtllib_ap_sec_type;
+
+	spin_lock_init(&ieee->lock);
+	spin_lock_init(&ieee->wpax_suitlist_lock);
+	spin_lock_init(&ieee->reorder_spinlock);
+	atomic_set(&(ieee->atm_swbw), 0);
+
+	/* SAM FIXME */
+	lib80211_crypt_info_init(&ieee->crypt_info, "RTLLIB", &ieee->lock);
+
+	ieee->wpa_enabled = 0;
+	ieee->tkip_countermeasures = 0;
+	ieee->drop_unencrypted = 0;
+	ieee->privacy_invoked = 0;
+	ieee->ieee802_1x = 1;
+	ieee->raw_tx = 0;
+	ieee->hwsec_active = 0;
+
+	memset(ieee->swcamtable, 0, sizeof(struct sw_cam_table) * 32);
+	err = rtllib_softmac_init(ieee);
+	if (err)
+		goto free_crypt_info;
+
+	ieee->pHTInfo = kzalloc(sizeof(struct rt_hi_throughput), GFP_KERNEL);
+	if (!ieee->pHTInfo)
+		goto free_softmac;
+
+	HTUpdateDefaultSetting(ieee);
+	HTInitializeHTInfo(ieee);
+	TSInitialize(ieee);
+	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&ieee->ibss_mac_hash[i]);
+
+	for (i = 0; i < 17; i++) {
+		ieee->last_rxseq_num[i] = -1;
+		ieee->last_rxfrag_num[i] = -1;
+		ieee->last_packet_time[i] = 0;
+	}
+
+	return dev;
+
+free_softmac:
+	rtllib_softmac_free(ieee);
+free_crypt_info:
+	lib80211_crypt_info_free(&ieee->crypt_info);
+	rtllib_networks_free(ieee);
+free_netdev:
+	free_netdev(dev);
+
+	return NULL;
+}
+EXPORT_SYMBOL(alloc_rtllib);
+
+void free_rtllib(struct net_device *dev)
+{
+	struct rtllib_device *ieee = (struct rtllib_device *)
+				      netdev_priv_rsl(dev);
+
+	kfree(ieee->pHTInfo);
+	rtllib_softmac_free(ieee);
+
+	lib80211_crypt_info_free(&ieee->crypt_info);
+
+	rtllib_networks_free(ieee);
+	free_netdev(dev);
+}
+EXPORT_SYMBOL(free_rtllib);
+
+static int __init rtllib_init(void)
+{
+	return 0;
+}
+
+static void __exit rtllib_exit(void)
+{
+}
+
+module_init(rtllib_init);
+module_exit(rtllib_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
new file mode 100644
index 000000000..f710eb2a9
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -0,0 +1,2744 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Original code based Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3 - hostap.o module, common routines
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * Few modifications for Realtek's Wi-Fi drivers by
+ * Andrea Merello <andrea.merello@gmail.com>
+ *
+ * A special thanks goes to Realtek for their support !
+ */
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <linux/ctype.h>
+
+#include "rtllib.h"
+#include "dot11d.h"
+
+static void rtllib_rx_mgt(struct rtllib_device *ieee, struct sk_buff *skb,
+			  struct rtllib_rx_stats *stats);
+
+static inline void rtllib_monitor_rx(struct rtllib_device *ieee,
+				     struct sk_buff *skb,
+				     struct rtllib_rx_stats *rx_status,
+				     size_t hdr_length)
+{
+	skb->dev = ieee->dev;
+	skb_reset_mac_header(skb);
+	skb_pull(skb, hdr_length);
+	skb->pkt_type = PACKET_OTHERHOST;
+	skb->protocol = htons(ETH_P_80211_RAW);
+	memset(skb->cb, 0, sizeof(skb->cb));
+	netif_rx(skb);
+}
+
+/* Called only as a tasklet (software IRQ) */
+static struct rtllib_frag_entry *
+rtllib_frag_cache_find(struct rtllib_device *ieee, unsigned int seq,
+			  unsigned int frag, u8 tid, u8 *src, u8 *dst)
+{
+	struct rtllib_frag_entry *entry;
+	int i;
+
+	for (i = 0; i < RTLLIB_FRAG_CACHE_LEN; i++) {
+		entry = &ieee->frag_cache[tid][i];
+		if (entry->skb != NULL &&
+		    time_after(jiffies, entry->first_frag_time + 2 * HZ)) {
+			netdev_dbg(ieee->dev,
+				   "expiring fragment cache entry seq=%u last_frag=%u\n",
+				   entry->seq, entry->last_frag);
+			dev_kfree_skb_any(entry->skb);
+			entry->skb = NULL;
+		}
+
+		if (entry->skb != NULL && entry->seq == seq &&
+		    (entry->last_frag + 1 == frag || frag == -1) &&
+		    memcmp(entry->src_addr, src, ETH_ALEN) == 0 &&
+		    memcmp(entry->dst_addr, dst, ETH_ALEN) == 0)
+			return entry;
+	}
+
+	return NULL;
+}
+
+/* Called only as a tasklet (software IRQ) */
+static struct sk_buff *
+rtllib_frag_cache_get(struct rtllib_device *ieee,
+			 struct rtllib_hdr_4addr *hdr)
+{
+	struct sk_buff *skb = NULL;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int frag = WLAN_GET_SEQ_FRAG(sc);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct rtllib_frag_entry *entry;
+	struct rtllib_hdr_3addrqos *hdr_3addrqos;
+	struct rtllib_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)hdr;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_3addrqos = (struct rtllib_hdr_3addrqos *)hdr;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else {
+		tid = 0;
+	}
+
+	if (frag == 0) {
+		/* Reserve enough space to fit maximum frame length */
+		skb = dev_alloc_skb(ieee->dev->mtu +
+				    sizeof(struct rtllib_hdr_4addr) +
+				    8 /* LLC */ +
+				    2 /* alignment */ +
+				    8 /* WEP */ +
+				    ETH_ALEN /* WDS */ +
+				    /* QOS Control */
+				    (RTLLIB_QOS_HAS_SEQ(fc) ? 2 : 0));
+		if (!skb)
+			return NULL;
+
+		entry = &ieee->frag_cache[tid][ieee->frag_next_idx[tid]];
+		ieee->frag_next_idx[tid]++;
+		if (ieee->frag_next_idx[tid] >= RTLLIB_FRAG_CACHE_LEN)
+			ieee->frag_next_idx[tid] = 0;
+
+		if (entry->skb != NULL)
+			dev_kfree_skb_any(entry->skb);
+
+		entry->first_frag_time = jiffies;
+		entry->seq = seq;
+		entry->last_frag = frag;
+		entry->skb = skb;
+		ether_addr_copy(entry->src_addr, hdr->addr2);
+		ether_addr_copy(entry->dst_addr, hdr->addr1);
+	} else {
+		/* received a fragment of a frame for which the head fragment
+		 * should have already been received
+		 */
+		entry = rtllib_frag_cache_find(ieee, seq, frag, tid, hdr->addr2,
+						  hdr->addr1);
+		if (entry != NULL) {
+			entry->last_frag = frag;
+			skb = entry->skb;
+		}
+	}
+
+	return skb;
+}
+
+
+/* Called only as a tasklet (software IRQ) */
+static int rtllib_frag_cache_invalidate(struct rtllib_device *ieee,
+					   struct rtllib_hdr_4addr *hdr)
+{
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct rtllib_frag_entry *entry;
+	struct rtllib_hdr_3addrqos *hdr_3addrqos;
+	struct rtllib_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)hdr;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_3addrqos = (struct rtllib_hdr_3addrqos *)hdr;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else {
+		tid = 0;
+	}
+
+	entry = rtllib_frag_cache_find(ieee, seq, -1, tid, hdr->addr2,
+					  hdr->addr1);
+
+	if (entry == NULL) {
+		netdev_dbg(ieee->dev,
+			   "Couldn't invalidate fragment cache entry (seq=%u)\n",
+			   seq);
+		return -1;
+	}
+
+	entry->skb = NULL;
+	return 0;
+}
+
+/* rtllib_rx_frame_mgtmt
+ *
+ * Responsible for handling management control frames
+ *
+ * Called by rtllib_rx
+ */
+static inline int
+rtllib_rx_frame_mgmt(struct rtllib_device *ieee, struct sk_buff *skb,
+			struct rtllib_rx_stats *rx_stats, u16 type,
+			u16 stype)
+{
+	/* On the struct stats definition there is written that
+	 * this is not mandatory.... but seems that the probe
+	 * response parser uses it
+	 */
+	struct rtllib_hdr_3addr *hdr = (struct rtllib_hdr_3addr *)skb->data;
+
+	rx_stats->len = skb->len;
+	rtllib_rx_mgt(ieee, skb, rx_stats);
+	if ((memcmp(hdr->addr1, ieee->dev->dev_addr, ETH_ALEN))) {
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+	rtllib_rx_frame_softmac(ieee, skb, rx_stats, type, stype);
+
+	dev_kfree_skb_any(skb);
+
+	return 0;
+}
+
+/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation
+ * Ethernet-II snap header (RFC1042 for most EtherTypes)
+ */
+static unsigned char rfc1042_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
+};
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static unsigned char bridge_tunnel_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8
+};
+/* No encapsulation header if EtherType < 0x600 (=length) */
+
+/* Called by rtllib_rx_frame_decrypt */
+static int rtllib_is_eapol_frame(struct rtllib_device *ieee,
+				    struct sk_buff *skb, size_t hdrlen)
+{
+	struct net_device *dev = ieee->dev;
+	u16 fc, ethertype;
+	struct rtllib_hdr_4addr *hdr;
+	u8 *pos;
+
+	if (skb->len < 24)
+		return 0;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	fc = le16_to_cpu(hdr->frame_ctl);
+
+	/* check that the frame is unicast frame to us */
+	if ((fc & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
+	    RTLLIB_FCTL_TODS &&
+	    memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0 &&
+	    memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN) == 0) {
+		/* ToDS frame with own addr BSSID and DA */
+	} else if ((fc & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
+		   RTLLIB_FCTL_FROMDS &&
+		   memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0) {
+		/* FromDS frame with own addr as DA */
+	} else
+		return 0;
+
+	if (skb->len < 24 + 8)
+		return 0;
+
+	/* check for port access entity Ethernet type */
+	pos = skb->data + hdrlen;
+	ethertype = (pos[6] << 8) | pos[7];
+	if (ethertype == ETH_P_PAE)
+		return 1;
+
+	return 0;
+}
+
+/* Called only as a tasklet (software IRQ), by rtllib_rx */
+static inline int
+rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
+			struct lib80211_crypt_data *crypt)
+{
+	struct rtllib_hdr_4addr *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
+		return 0;
+
+	if (ieee->hwsec_active) {
+		struct cb_desc *tcb_desc = (struct cb_desc *)
+						(skb->cb + MAX_DEV_ADDR_SIZE);
+
+		tcb_desc->bHwSec = 1;
+
+		if (ieee->need_sw_enc)
+			tcb_desc->bHwSec = 0;
+	}
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	hdrlen = rtllib_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		netdev_dbg(ieee->dev, "decryption failed (SA= %pM) res=%d\n",
+			   hdr->addr2, res);
+		if (res == -2)
+			netdev_dbg(ieee->dev,
+				   "Decryption failed ICV mismatch (key %d)\n",
+				   skb->data[hdrlen + 3] >> 6);
+		return -1;
+	}
+
+	return res;
+}
+
+
+/* Called only as a tasklet (software IRQ), by rtllib_rx */
+static inline int
+rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
+			     int keyidx, struct lib80211_crypt_data *crypt)
+{
+	struct rtllib_hdr_4addr *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
+		return 0;
+	if (ieee->hwsec_active) {
+		struct cb_desc *tcb_desc = (struct cb_desc *)
+						(skb->cb + MAX_DEV_ADDR_SIZE);
+
+		tcb_desc->bHwSec = 1;
+
+		if (ieee->need_sw_enc)
+			tcb_desc->bHwSec = 0;
+	}
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	hdrlen = rtllib_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		netdev_dbg(ieee->dev,
+			   "MSDU decryption/MIC verification failed (SA= %pM keyidx=%d)\n",
+			   hdr->addr2, keyidx);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+/* this function is stolen from ipw2200 driver*/
+#define IEEE_PACKET_RETRY_TIME (5*HZ)
+static int is_duplicate_packet(struct rtllib_device *ieee,
+				      struct rtllib_hdr_4addr *header)
+{
+	u16 fc = le16_to_cpu(header->frame_ctl);
+	u16 sc = le16_to_cpu(header->seq_ctl);
+	u16 seq = WLAN_GET_SEQ_SEQ(sc);
+	u16 frag = WLAN_GET_SEQ_FRAG(sc);
+	u16 *last_seq, *last_frag;
+	unsigned long *last_time;
+	struct rtllib_hdr_3addrqos *hdr_3addrqos;
+	struct rtllib_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)header;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (RTLLIB_QOS_HAS_SEQ(fc)) {
+		hdr_3addrqos = (struct rtllib_hdr_3addrqos *)header;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & RTLLIB_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else {
+		tid = 0;
+	}
+
+	switch (ieee->iw_mode) {
+	case IW_MODE_ADHOC:
+	{
+		struct list_head *p;
+		struct ieee_ibss_seq *entry = NULL;
+		u8 *mac = header->addr2;
+		int index = mac[5] % IEEE_IBSS_MAC_HASH_SIZE;
+
+		list_for_each(p, &ieee->ibss_mac_hash[index]) {
+			entry = list_entry(p, struct ieee_ibss_seq, list);
+			if (!memcmp(entry->mac, mac, ETH_ALEN))
+				break;
+		}
+		if (p == &ieee->ibss_mac_hash[index]) {
+			entry = kmalloc(sizeof(struct ieee_ibss_seq),
+					GFP_ATOMIC);
+			if (!entry)
+				return 0;
+
+			ether_addr_copy(entry->mac, mac);
+			entry->seq_num[tid] = seq;
+			entry->frag_num[tid] = frag;
+			entry->packet_time[tid] = jiffies;
+			list_add(&entry->list, &ieee->ibss_mac_hash[index]);
+			return 0;
+		}
+		last_seq = &entry->seq_num[tid];
+		last_frag = &entry->frag_num[tid];
+		last_time = &entry->packet_time[tid];
+		break;
+	}
+
+	case IW_MODE_INFRA:
+		last_seq = &ieee->last_rxseq_num[tid];
+		last_frag = &ieee->last_rxfrag_num[tid];
+		last_time = &ieee->last_packet_time[tid];
+		break;
+	default:
+		return 0;
+	}
+
+	if ((*last_seq == seq) &&
+	    time_after(*last_time + IEEE_PACKET_RETRY_TIME, jiffies)) {
+		if (*last_frag == frag)
+			goto drop;
+		if (*last_frag + 1 != frag)
+			/* out-of-order fragment */
+			goto drop;
+	} else
+		*last_seq = seq;
+
+	*last_frag = frag;
+	*last_time = jiffies;
+	return 0;
+
+drop:
+
+	return 1;
+}
+
+static bool AddReorderEntry(struct rx_ts_record *pTS,
+			    struct rx_reorder_entry *pReorderEntry)
+{
+	struct list_head *pList = &pTS->rx_pending_pkt_list;
+
+	while (pList->next != &pTS->rx_pending_pkt_list) {
+		if (SN_LESS(pReorderEntry->SeqNum, ((struct rx_reorder_entry *)
+		    list_entry(pList->next, struct rx_reorder_entry,
+		    List))->SeqNum))
+			pList = pList->next;
+		else if (SN_EQUAL(pReorderEntry->SeqNum,
+			((struct rx_reorder_entry *)list_entry(pList->next,
+			struct rx_reorder_entry, List))->SeqNum))
+			return false;
+		else
+			break;
+	}
+	pReorderEntry->List.next = pList->next;
+	pReorderEntry->List.next->prev = &pReorderEntry->List;
+	pReorderEntry->List.prev = pList;
+	pList->next = &pReorderEntry->List;
+
+	return true;
+}
+
+void rtllib_indicate_packets(struct rtllib_device *ieee,
+			     struct rtllib_rxb **prxbIndicateArray, u8 index)
+{
+	struct net_device_stats *stats = &ieee->stats;
+	u8 i = 0, j = 0;
+	u16 ethertype;
+
+	for (j = 0; j < index; j++) {
+		struct rtllib_rxb *prxb = prxbIndicateArray[j];
+
+		for (i = 0; i < prxb->nr_subframes; i++) {
+			struct sk_buff *sub_skb = prxb->subframes[i];
+
+		/* convert hdr + possible LLC headers into Ethernet header */
+			ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
+			if (sub_skb->len >= 8 &&
+			    ((memcmp(sub_skb->data, rfc1042_header,
+				     SNAP_SIZE) == 0 &&
+			      ethertype != ETH_P_AARP &&
+			      ethertype != ETH_P_IPX) ||
+			    memcmp(sub_skb->data, bridge_tunnel_header,
+				   SNAP_SIZE) == 0)) {
+				/* remove RFC1042 or Bridge-Tunnel encapsulation
+				 * and replace EtherType
+				 */
+				skb_pull(sub_skb, SNAP_SIZE);
+				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
+				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
+			} else {
+				u16 len;
+			/* Leave Ethernet header part of hdr and full payload */
+				len = sub_skb->len;
+				memcpy(skb_push(sub_skb, 2), &len, 2);
+				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
+				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
+			}
+
+			/* Indicate the packets to upper layer */
+			if (sub_skb) {
+				stats->rx_packets++;
+				stats->rx_bytes += sub_skb->len;
+
+				memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
+				sub_skb->protocol = eth_type_trans(sub_skb,
+								   ieee->dev);
+				sub_skb->dev = ieee->dev;
+				sub_skb->dev->stats.rx_packets++;
+				sub_skb->dev->stats.rx_bytes += sub_skb->len;
+				/* 802.11 crc not sufficient */
+				sub_skb->ip_summed = CHECKSUM_NONE;
+				ieee->last_rx_ps_time = jiffies;
+				netif_rx(sub_skb);
+			}
+		}
+		kfree(prxb);
+		prxb = NULL;
+	}
+}
+
+void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,
+				 struct rx_ts_record *pTS)
+{
+	struct rx_reorder_entry *pRxReorderEntry;
+	u8 RfdCnt = 0;
+
+	del_timer_sync(&pTS->rx_pkt_pending_timer);
+	while (!list_empty(&pTS->rx_pending_pkt_list)) {
+		if (RfdCnt >= REORDER_WIN_SIZE) {
+			netdev_info(ieee->dev,
+				    "-------------->%s() error! RfdCnt >= REORDER_WIN_SIZE\n",
+				    __func__);
+			break;
+		}
+
+		pRxReorderEntry = (struct rx_reorder_entry *)
+				  list_entry(pTS->rx_pending_pkt_list.prev,
+					     struct rx_reorder_entry, List);
+		netdev_dbg(ieee->dev, "%s(): Indicate SeqNum %d!\n", __func__,
+			   pRxReorderEntry->SeqNum);
+		list_del_init(&pRxReorderEntry->List);
+
+		ieee->RfdArray[RfdCnt] = pRxReorderEntry->prxb;
+
+		RfdCnt = RfdCnt + 1;
+		list_add_tail(&pRxReorderEntry->List,
+			      &ieee->RxReorder_Unused_List);
+	}
+	rtllib_indicate_packets(ieee, ieee->RfdArray, RfdCnt);
+
+	pTS->rx_indicate_seq = 0xffff;
+}
+
+static void RxReorderIndicatePacket(struct rtllib_device *ieee,
+				    struct rtllib_rxb *prxb,
+				    struct rx_ts_record *pTS, u16 SeqNum)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	struct rx_reorder_entry *pReorderEntry = NULL;
+	u8 WinSize = pHTInfo->rx_reorder_win_size;
+	u16 WinEnd = 0;
+	u8 index = 0;
+	bool bMatchWinStart = false, bPktInBuf = false;
+	unsigned long flags;
+
+	netdev_dbg(ieee->dev,
+		   "%s(): Seq is %d, pTS->rx_indicate_seq is %d, WinSize is %d\n",
+		   __func__, SeqNum, pTS->rx_indicate_seq, WinSize);
+
+	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
+
+	WinEnd = (pTS->rx_indicate_seq + WinSize - 1) % 4096;
+	/* Rx Reorder initialize condition.*/
+	if (pTS->rx_indicate_seq == 0xffff)
+		pTS->rx_indicate_seq = SeqNum;
+
+	/* Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(SeqNum, pTS->rx_indicate_seq)) {
+		netdev_dbg(ieee->dev,
+			   "Packet Drop! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->rx_indicate_seq, SeqNum);
+		pHTInfo->rx_reorder_drop_counter++;
+		{
+			int i;
+
+			for (i = 0; i < prxb->nr_subframes; i++)
+				dev_kfree_skb(prxb->subframes[i]);
+			kfree(prxb);
+			prxb = NULL;
+		}
+		spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+		return;
+	}
+
+	/* Sliding window manipulation. Conditions includes:
+	 * 1. Incoming SeqNum is equal to WinStart =>Window shift 1
+	 * 2. Incoming SeqNum is larger than the WinEnd => Window shift N
+	 */
+	if (SN_EQUAL(SeqNum, pTS->rx_indicate_seq)) {
+		pTS->rx_indicate_seq = (pTS->rx_indicate_seq + 1) % 4096;
+		bMatchWinStart = true;
+	} else if (SN_LESS(WinEnd, SeqNum)) {
+		if (SeqNum >= (WinSize - 1))
+			pTS->rx_indicate_seq = SeqNum + 1 - WinSize;
+		else
+			pTS->rx_indicate_seq = 4095 -
+					     (WinSize - (SeqNum + 1)) + 1;
+		netdev_dbg(ieee->dev,
+			   "Window Shift! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->rx_indicate_seq, SeqNum);
+	}
+
+	/* Indication process.
+	 * After Packet dropping and Sliding Window shifting as above, we can
+	 * now just indicate the packets with the SeqNum smaller than latest
+	 * WinStart and struct buffer other packets.
+	 *
+	 * For Rx Reorder condition:
+	 * 1. All packets with SeqNum smaller than WinStart => Indicate
+	 * 2. All packets with SeqNum larger than or equal to
+	 *	 WinStart => Buffer it.
+	 */
+	if (bMatchWinStart) {
+		/* Current packet is going to be indicated.*/
+		netdev_dbg(ieee->dev,
+			   "Packets indication! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->rx_indicate_seq, SeqNum);
+		ieee->prxbIndicateArray[0] = prxb;
+		index = 1;
+	} else {
+		/* Current packet is going to be inserted into pending list.*/
+		if (!list_empty(&ieee->RxReorder_Unused_List)) {
+			pReorderEntry = (struct rx_reorder_entry *)
+					list_entry(ieee->RxReorder_Unused_List.next,
+					struct rx_reorder_entry, List);
+			list_del_init(&pReorderEntry->List);
+
+			/* Make a reorder entry and insert
+			 * into a the packet list.
+			 */
+			pReorderEntry->SeqNum = SeqNum;
+			pReorderEntry->prxb = prxb;
+
+			if (!AddReorderEntry(pTS, pReorderEntry)) {
+				int i;
+
+				netdev_dbg(ieee->dev,
+					   "%s(): Duplicate packet is dropped. IndicateSeq: %d, NewSeq: %d\n",
+					   __func__, pTS->rx_indicate_seq,
+					   SeqNum);
+				list_add_tail(&pReorderEntry->List,
+					      &ieee->RxReorder_Unused_List);
+
+				for (i = 0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+				kfree(prxb);
+				prxb = NULL;
+			} else {
+				netdev_dbg(ieee->dev,
+					   "Pkt insert into struct buffer. IndicateSeq: %d, NewSeq: %d\n",
+					   pTS->rx_indicate_seq, SeqNum);
+			}
+		} else {
+			/* Packets are dropped if there are not enough reorder
+			 * entries. This part should be modified!! We can just
+			 * indicate all the packets in struct buffer and get
+			 * reorder entries.
+			 */
+			netdev_err(ieee->dev,
+				   "%s(): There is no reorder entry! Packet is dropped!\n",
+				   __func__);
+			{
+				int i;
+
+				for (i = 0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+				kfree(prxb);
+				prxb = NULL;
+			}
+		}
+	}
+
+	/* Check if there is any packet need indicate.*/
+	while (!list_empty(&pTS->rx_pending_pkt_list)) {
+		netdev_dbg(ieee->dev, "%s(): start RREORDER indicate\n",
+			   __func__);
+
+		pReorderEntry = (struct rx_reorder_entry *)
+					list_entry(pTS->rx_pending_pkt_list.prev,
+						   struct rx_reorder_entry,
+						   List);
+		if (SN_LESS(pReorderEntry->SeqNum, pTS->rx_indicate_seq) ||
+		    SN_EQUAL(pReorderEntry->SeqNum, pTS->rx_indicate_seq)) {
+			/* This protect struct buffer from overflow. */
+			if (index >= REORDER_WIN_SIZE) {
+				netdev_err(ieee->dev,
+					   "%s(): Buffer overflow!\n",
+					   __func__);
+				bPktInBuf = true;
+				break;
+			}
+
+			list_del_init(&pReorderEntry->List);
+
+			if (SN_EQUAL(pReorderEntry->SeqNum, pTS->rx_indicate_seq))
+				pTS->rx_indicate_seq = (pTS->rx_indicate_seq + 1) %
+						     4096;
+
+			ieee->prxbIndicateArray[index] = pReorderEntry->prxb;
+			netdev_dbg(ieee->dev, "%s(): Indicate SeqNum %d!\n",
+				   __func__, pReorderEntry->SeqNum);
+			index++;
+
+			list_add_tail(&pReorderEntry->List,
+				      &ieee->RxReorder_Unused_List);
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+	}
+
+	/* Handling pending timer. Set this timer to prevent from long time
+	 * Rx buffering.
+	 */
+	if (index > 0) {
+		if (timer_pending(&pTS->rx_pkt_pending_timer))
+			del_timer_sync(&pTS->rx_pkt_pending_timer);
+		pTS->rx_timeout_indicate_seq = 0xffff;
+
+		if (index > REORDER_WIN_SIZE) {
+			netdev_err(ieee->dev,
+				   "%s(): Rx Reorder struct buffer full!\n",
+				   __func__);
+			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
+					       flags);
+			return;
+		}
+		rtllib_indicate_packets(ieee, ieee->prxbIndicateArray, index);
+		bPktInBuf = false;
+	}
+
+	if (bPktInBuf && pTS->rx_timeout_indicate_seq == 0xffff) {
+		netdev_dbg(ieee->dev, "%s(): SET rx timeout timer\n", __func__);
+		pTS->rx_timeout_indicate_seq = pTS->rx_indicate_seq;
+		mod_timer(&pTS->rx_pkt_pending_timer, jiffies +
+			  msecs_to_jiffies(pHTInfo->rx_reorder_pending_time));
+	}
+	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+}
+
+static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
+			 struct rtllib_rx_stats *rx_stats,
+			 struct rtllib_rxb *rxb, u8 *src, u8 *dst)
+{
+	struct rtllib_hdr_3addr  *hdr = (struct rtllib_hdr_3addr *)skb->data;
+	u16		fc = le16_to_cpu(hdr->frame_ctl);
+
+	u16		LLCOffset = sizeof(struct rtllib_hdr_3addr);
+	u16		ChkLength;
+	bool		bIsAggregateFrame = false;
+	u16		nSubframe_Length;
+	u8		nPadding_Length = 0;
+	u16		SeqNum = 0;
+	struct sk_buff *sub_skb;
+	/* just for debug purpose */
+	SeqNum = WLAN_GET_SEQ_SEQ(le16_to_cpu(hdr->seq_ctl));
+	if ((RTLLIB_QOS_HAS_SEQ(fc)) &&
+	   (((union frameqos *)(skb->data + RTLLIB_3ADDR_LEN))->field.reserved))
+		bIsAggregateFrame = true;
+
+	if (RTLLIB_QOS_HAS_SEQ(fc))
+		LLCOffset += 2;
+	if (rx_stats->bContainHTC)
+		LLCOffset += sHTCLng;
+
+	ChkLength = LLCOffset;
+
+	if (skb->len <= ChkLength)
+		return 0;
+
+	skb_pull(skb, LLCOffset);
+	ieee->bIsAggregateFrame = bIsAggregateFrame;
+	if (!bIsAggregateFrame) {
+		rxb->nr_subframes = 1;
+
+		/* altered by clark 3/30/2010
+		 * The struct buffer size of the skb indicated to upper layer
+		 * must be less than 5000, or the defraged IP datagram
+		 * in the IP layer will exceed "ipfrag_high_tresh" and be
+		 * discarded. so there must not use the function
+		 * "skb_copy" and "skb_clone" for "skb".
+		 */
+
+		/* Allocate new skb for releasing to upper layer */
+		sub_skb = dev_alloc_skb(RTLLIB_SKBBUFFER_SIZE);
+		if (!sub_skb)
+			return 0;
+		skb_reserve(sub_skb, 12);
+		skb_put_data(sub_skb, skb->data, skb->len);
+		sub_skb->dev = ieee->dev;
+
+		rxb->subframes[0] = sub_skb;
+
+		memcpy(rxb->src, src, ETH_ALEN);
+		memcpy(rxb->dst, dst, ETH_ALEN);
+		rxb->subframes[0]->dev = ieee->dev;
+		return 1;
+	}
+
+	rxb->nr_subframes = 0;
+	memcpy(rxb->src, src, ETH_ALEN);
+	memcpy(rxb->dst, dst, ETH_ALEN);
+	while (skb->len > ETHERNET_HEADER_SIZE) {
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = *((u16 *)(skb->data + 12));
+		nSubframe_Length = (nSubframe_Length >> 8) +
+				   (nSubframe_Length << 8);
+
+		if (skb->len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			netdev_info(ieee->dev,
+				    "%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",
+				    __func__, rxb->nr_subframes);
+			netdev_info(ieee->dev,
+				    "%s: A-MSDU parse error!! Subframe Length: %d\n",
+				    __func__, nSubframe_Length);
+			netdev_info(ieee->dev,
+				    "nRemain_Length is %d and nSubframe_Length is : %d\n",
+				    skb->len, nSubframe_Length);
+			netdev_info(ieee->dev,
+				    "The Packet SeqNum is %d\n",
+				    SeqNum);
+			return 0;
+		}
+
+		/* move the data point to data content */
+		skb_pull(skb, ETHERNET_HEADER_SIZE);
+
+		/* altered by clark 3/30/2010
+		 * The struct buffer size of the skb indicated to upper layer
+		 * must be less than 5000, or the defraged IP datagram
+		 * in the IP layer will exceed "ipfrag_high_tresh" and be
+		 * discarded. so there must not use the function
+		 * "skb_copy" and "skb_clone" for "skb".
+		 */
+
+		/* Allocate new skb for releasing to upper layer */
+		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if (!sub_skb)
+			return 0;
+		skb_reserve(sub_skb, 12);
+		skb_put_data(sub_skb, skb->data, nSubframe_Length);
+
+		sub_skb->dev = ieee->dev;
+		rxb->subframes[rxb->nr_subframes++] = sub_skb;
+		if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
+			netdev_dbg(ieee->dev,
+				   "ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
+		}
+		skb_pull(skb, nSubframe_Length);
+
+		if (skb->len != 0) {
+			nPadding_Length = 4 - ((nSubframe_Length +
+					  ETHERNET_HEADER_SIZE) % 4);
+			if (nPadding_Length == 4)
+				nPadding_Length = 0;
+
+			if (skb->len < nPadding_Length)
+				return 0;
+
+			skb_pull(skb, nPadding_Length);
+		}
+	}
+
+	return rxb->nr_subframes;
+}
+
+
+static size_t rtllib_rx_get_hdrlen(struct rtllib_device *ieee,
+				   struct sk_buff *skb,
+				   struct rtllib_rx_stats *rx_stats)
+{
+	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	size_t hdrlen;
+
+	hdrlen = rtllib_get_hdrlen(fc);
+	if (HTCCheck(ieee, skb->data)) {
+		if (net_ratelimit())
+			netdev_info(ieee->dev, "%s: find HTCControl!\n",
+				    __func__);
+		hdrlen += 4;
+		rx_stats->bContainHTC = true;
+	}
+
+	if (RTLLIB_QOS_HAS_SEQ(fc))
+		rx_stats->bIsQosData = true;
+
+	return hdrlen;
+}
+
+static int rtllib_rx_check_duplicate(struct rtllib_device *ieee,
+				     struct sk_buff *skb, u8 multicast)
+{
+	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
+	u16 fc, sc;
+	u8 frag, type, stype;
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+	sc = le16_to_cpu(hdr->seq_ctl);
+	frag = WLAN_GET_SEQ_FRAG(sc);
+
+	if (!ieee->pHTInfo->cur_rx_reorder_enable ||
+		!ieee->current_network.qos_data.active ||
+		!IsDataFrame(skb->data) ||
+		IsLegacyDataFrame(skb->data)) {
+		if (!((type == RTLLIB_FTYPE_MGMT) &&
+		      (stype == RTLLIB_STYPE_BEACON))) {
+			if (is_duplicate_packet(ieee, hdr))
+				return -1;
+		}
+	} else {
+		struct rx_ts_record *pRxTS = NULL;
+
+		if (GetTs(ieee, (struct ts_common_info **)&pRxTS, hdr->addr2,
+			(u8)Frame_QoSTID((u8 *)(skb->data)), RX_DIR, true)) {
+			if ((fc & (1<<11)) && (frag == pRxTS->rx_last_frag_num) &&
+			    (WLAN_GET_SEQ_SEQ(sc) == pRxTS->rx_last_seq_num))
+				return -1;
+			pRxTS->rx_last_frag_num = frag;
+			pRxTS->rx_last_seq_num = WLAN_GET_SEQ_SEQ(sc);
+		} else {
+			netdev_warn(ieee->dev, "%s(): No TS! Skip the check!\n",
+				    __func__);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static void rtllib_rx_extract_addr(struct rtllib_device *ieee,
+				   struct rtllib_hdr_4addr *hdr, u8 *dst,
+				   u8 *src, u8 *bssid)
+{
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+
+	switch (fc & (RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS)) {
+	case RTLLIB_FCTL_FROMDS:
+		ether_addr_copy(dst, hdr->addr1);
+		ether_addr_copy(src, hdr->addr3);
+		ether_addr_copy(bssid, hdr->addr2);
+		break;
+	case RTLLIB_FCTL_TODS:
+		ether_addr_copy(dst, hdr->addr3);
+		ether_addr_copy(src, hdr->addr2);
+		ether_addr_copy(bssid, hdr->addr1);
+		break;
+	case RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS:
+		ether_addr_copy(dst, hdr->addr3);
+		ether_addr_copy(src, hdr->addr4);
+		ether_addr_copy(bssid, ieee->current_network.bssid);
+		break;
+	default:
+		ether_addr_copy(dst, hdr->addr1);
+		ether_addr_copy(src, hdr->addr2);
+		ether_addr_copy(bssid, hdr->addr3);
+		break;
+	}
+}
+
+static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
+				 u8 *dst, u8 *src, u8 *bssid, u8 *addr2)
+{
+	u8 type, stype;
+
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+
+	/* Filter frames from different BSS */
+	if (((fc & RTLLIB_FCTL_DSTODS) != RTLLIB_FCTL_DSTODS) &&
+	    !ether_addr_equal(ieee->current_network.bssid, bssid) &&
+	    !is_zero_ether_addr(ieee->current_network.bssid)) {
+		return -1;
+	}
+
+	/* Filter packets sent by an STA that will be forwarded by AP */
+	if (ieee->IntelPromiscuousModeInfo.bPromiscuousOn  &&
+		ieee->IntelPromiscuousModeInfo.bFilterSourceStationFrame) {
+		if ((fc & RTLLIB_FCTL_TODS) && !(fc & RTLLIB_FCTL_FROMDS) &&
+		    !ether_addr_equal(dst, ieee->current_network.bssid) &&
+		    ether_addr_equal(bssid, ieee->current_network.bssid)) {
+			return -1;
+		}
+	}
+
+	/* Nullfunc frames may have PS-bit set, so they must be passed to
+	 * hostap_handle_sta_rx() before being dropped here.
+	 */
+	if (!ieee->IntelPromiscuousModeInfo.bPromiscuousOn) {
+		if (stype != RTLLIB_STYPE_DATA &&
+		    stype != RTLLIB_STYPE_DATA_CFACK &&
+		    stype != RTLLIB_STYPE_DATA_CFPOLL &&
+		    stype != RTLLIB_STYPE_DATA_CFACKPOLL &&
+		    stype != RTLLIB_STYPE_QOS_DATA) {
+			if (stype != RTLLIB_STYPE_NULLFUNC)
+				netdev_dbg(ieee->dev,
+					   "RX: dropped data frame with no data (type=0x%02x, subtype=0x%02x)\n",
+					   type, stype);
+			return -1;
+		}
+	}
+
+	if (ieee->iw_mode != IW_MODE_MESH) {
+		/* packets from our adapter are dropped (echo) */
+		if (!memcmp(src, ieee->dev->dev_addr, ETH_ALEN))
+			return -1;
+
+		/* {broad,multi}cast packets to our BSS go through */
+		if (is_multicast_ether_addr(dst)) {
+			if (memcmp(bssid, ieee->current_network.bssid,
+				   ETH_ALEN))
+				return -1;
+		}
+	}
+	return 0;
+}
+
+static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
+			struct lib80211_crypt_data **crypt, size_t hdrlen)
+{
+	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	int idx = 0;
+
+	if (ieee->host_decrypt) {
+		if (skb->len >= hdrlen + 3)
+			idx = skb->data[hdrlen + 3] >> 6;
+
+		*crypt = ieee->crypt_info.crypt[idx];
+		/* allow NULL decrypt to indicate an station specific override
+		 * for default encryption
+		 */
+		if (*crypt && ((*crypt)->ops == NULL ||
+			      (*crypt)->ops->decrypt_mpdu == NULL))
+			*crypt = NULL;
+
+		if (!*crypt && (fc & RTLLIB_FCTL_WEP)) {
+			/* This seems to be triggered by some (multicast?)
+			 * frames from other than current BSS, so just drop the
+			 * frames silently instead of filling system log with
+			 * these reports.
+			 */
+			netdev_dbg(ieee->dev,
+				   "Decryption failed (not set) (SA= %pM)\n",
+				   hdr->addr2);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
+		      struct rtllib_rx_stats *rx_stats,
+		      struct lib80211_crypt_data *crypt, size_t hdrlen)
+{
+	struct rtllib_hdr_4addr *hdr;
+	int keyidx = 0;
+	u16 fc, sc;
+	u8 frag;
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	fc = le16_to_cpu(hdr->frame_ctl);
+	sc = le16_to_cpu(hdr->seq_ctl);
+	frag = WLAN_GET_SEQ_FRAG(sc);
+
+	if ((!rx_stats->Decrypted))
+		ieee->need_sw_enc = 1;
+	else
+		ieee->need_sw_enc = 0;
+
+	keyidx = rtllib_rx_frame_decrypt(ieee, skb, crypt);
+	if (ieee->host_decrypt && (fc & RTLLIB_FCTL_WEP) && (keyidx < 0)) {
+		netdev_info(ieee->dev, "%s: decrypt frame error\n", __func__);
+		return -1;
+	}
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	if ((frag != 0 || (fc & RTLLIB_FCTL_MOREFRAGS))) {
+		int flen;
+		struct sk_buff *frag_skb = rtllib_frag_cache_get(ieee, hdr);
+
+		netdev_dbg(ieee->dev, "Rx Fragment received (%u)\n", frag);
+
+		if (!frag_skb) {
+			netdev_dbg(ieee->dev,
+				   "Rx cannot get skb from fragment cache (morefrag=%d seq=%u frag=%u)\n",
+				   (fc & RTLLIB_FCTL_MOREFRAGS) != 0,
+				   WLAN_GET_SEQ_SEQ(sc), frag);
+			return -1;
+		}
+		flen = skb->len;
+		if (frag != 0)
+			flen -= hdrlen;
+
+		if (frag_skb->tail + flen > frag_skb->end) {
+			netdev_warn(ieee->dev,
+				    "%s: host decrypted and reassembled frame did not fit skb\n",
+				    __func__);
+			rtllib_frag_cache_invalidate(ieee, hdr);
+			return -1;
+		}
+
+		if (frag == 0) {
+			/* copy first fragment (including full headers) into
+			 * beginning of the fragment cache skb
+			 */
+			skb_put_data(frag_skb, skb->data, flen);
+		} else {
+			/* append frame payload to the end of the fragment
+			 * cache skb
+			 */
+			skb_put_data(frag_skb, skb->data + hdrlen, flen);
+		}
+		dev_kfree_skb_any(skb);
+		skb = NULL;
+
+		if (fc & RTLLIB_FCTL_MOREFRAGS) {
+			/* more fragments expected - leave the skb in fragment
+			 * cache for now; it will be delivered to upper layers
+			 * after all fragments have been received
+			 */
+			return -2;
+		}
+
+		/* this was the last fragment and the frame will be
+		 * delivered, so remove skb from fragment cache
+		 */
+		skb = frag_skb;
+		hdr = (struct rtllib_hdr_4addr *)skb->data;
+		rtllib_frag_cache_invalidate(ieee, hdr);
+	}
+
+	/* skb: hdr + (possible reassembled) full MSDU payload; possibly still
+	 * encrypted/authenticated
+	 */
+	if (ieee->host_decrypt && (fc & RTLLIB_FCTL_WEP) &&
+		rtllib_rx_frame_decrypt_msdu(ieee, skb, keyidx, crypt)) {
+		netdev_info(ieee->dev, "%s: ==>decrypt msdu error\n", __func__);
+		return -1;
+	}
+
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	if (crypt && !(fc & RTLLIB_FCTL_WEP) && !ieee->open_wep) {
+		if (/*ieee->ieee802_1x &&*/
+		    rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
+
+			/* pass unencrypted EAPOL frames even if encryption is
+			 * configured
+			 */
+			struct eapol *eap = (struct eapol *)(skb->data +
+				24);
+			netdev_dbg(ieee->dev,
+				   "RX: IEEE 802.1X EAPOL frame: %s\n",
+				   eap_get_type(eap->type));
+		} else {
+			netdev_dbg(ieee->dev,
+				   "encryption configured, but RX frame not encrypted (SA= %pM)\n",
+				   hdr->addr2);
+			return -1;
+		}
+	}
+
+	if (crypt && !(fc & RTLLIB_FCTL_WEP) &&
+	    rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
+		struct eapol *eap = (struct eapol *)(skb->data + 24);
+
+		netdev_dbg(ieee->dev, "RX: IEEE 802.1X EAPOL frame: %s\n",
+			   eap_get_type(eap->type));
+	}
+
+	if (crypt && !(fc & RTLLIB_FCTL_WEP) && !ieee->open_wep &&
+	    !rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
+		netdev_dbg(ieee->dev,
+			   "dropped unencrypted RX data frame from %pM (drop_unencrypted=1)\n",
+			   hdr->addr2);
+		return -1;
+	}
+
+	return 0;
+}
+
+static void rtllib_rx_check_leave_lps(struct rtllib_device *ieee, u8 unicast,
+				      u8 nr_subframes)
+{
+	if (unicast) {
+
+		if (ieee->state == RTLLIB_LINKED) {
+			if (((ieee->LinkDetectInfo.NumRxUnicastOkInPeriod +
+			    ieee->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+			    (ieee->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)) {
+				if (ieee->LeisurePSLeave)
+					ieee->LeisurePSLeave(ieee->dev);
+			}
+		}
+	}
+	ieee->last_rx_ps_time = jiffies;
+}
+
+static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
+		struct rtllib_rx_stats *rx_stats,
+		struct rtllib_rxb *rxb,
+		u8 *dst,
+		u8 *src)
+{
+	struct net_device *dev = ieee->dev;
+	u16 ethertype;
+	int i = 0;
+
+	if (rxb == NULL) {
+		netdev_info(dev, "%s: rxb is NULL!!\n", __func__);
+		return;
+	}
+
+	for (i = 0; i < rxb->nr_subframes; i++) {
+		struct sk_buff *sub_skb = rxb->subframes[i];
+
+		if (sub_skb) {
+			/* convert hdr + possible LLC headers
+			 * into Ethernet header
+			 */
+			ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
+			if (sub_skb->len >= 8 &&
+				((memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) == 0 &&
+				ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
+				memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
+				/* remove RFC1042 or Bridge-Tunnel encapsulation
+				 * and replace EtherType
+				 */
+				skb_pull(sub_skb, SNAP_SIZE);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						src);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						dst);
+			} else {
+				u16 len;
+				/* Leave Ethernet header part of hdr
+				 * and full payload
+				 */
+				len = sub_skb->len;
+				memcpy(skb_push(sub_skb, 2), &len, 2);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						src);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						dst);
+			}
+
+			ieee->stats.rx_packets++;
+			ieee->stats.rx_bytes += sub_skb->len;
+
+			if (is_multicast_ether_addr(dst))
+				ieee->stats.multicast++;
+
+			/* Indicate the packets to upper layer */
+			memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
+			sub_skb->protocol = eth_type_trans(sub_skb, dev);
+			sub_skb->dev = dev;
+			sub_skb->dev->stats.rx_packets++;
+			sub_skb->dev->stats.rx_bytes += sub_skb->len;
+			/* 802.11 crc not sufficient */
+			sub_skb->ip_summed = CHECKSUM_NONE;
+			netif_rx(sub_skb);
+		}
+	}
+	kfree(rxb);
+}
+
+static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
+		 struct rtllib_rx_stats *rx_stats)
+{
+	struct net_device *dev = ieee->dev;
+	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
+	struct lib80211_crypt_data *crypt = NULL;
+	struct rtllib_rxb *rxb = NULL;
+	struct rx_ts_record *pTS = NULL;
+	u16 fc, sc, SeqNum = 0;
+	u8 type, stype, multicast = 0, unicast = 0, nr_subframes = 0, TID = 0;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 bssid[ETH_ALEN] = {0};
+
+	size_t hdrlen = 0;
+	bool bToOtherSTA = false;
+	int ret = 0, i = 0;
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+	sc = le16_to_cpu(hdr->seq_ctl);
+
+	/*Filter pkt not to me*/
+	multicast = is_multicast_ether_addr(hdr->addr1);
+	unicast = !multicast;
+	if (unicast && !ether_addr_equal(dev->dev_addr, hdr->addr1)) {
+		if (ieee->bNetPromiscuousMode)
+			bToOtherSTA = true;
+		else
+			goto rx_dropped;
+	}
+
+	/*Filter pkt has too small length */
+	hdrlen = rtllib_rx_get_hdrlen(ieee, skb, rx_stats);
+	if (skb->len < hdrlen) {
+		netdev_info(dev,
+			    "%s():ERR!!! skb->len is smaller than hdrlen\n",
+			    __func__);
+		goto rx_dropped;
+	}
+
+	/* Filter Duplicate pkt */
+	ret = rtllib_rx_check_duplicate(ieee, skb, multicast);
+	if (ret < 0)
+		goto rx_dropped;
+
+	/* Filter CTRL Frame */
+	if (type == RTLLIB_FTYPE_CTL)
+		goto rx_dropped;
+
+	/* Filter MGNT Frame */
+	if (type == RTLLIB_FTYPE_MGMT) {
+		if (bToOtherSTA)
+			goto rx_dropped;
+		if (rtllib_rx_frame_mgmt(ieee, skb, rx_stats, type, stype))
+			goto rx_dropped;
+		else
+			goto rx_exit;
+	}
+
+	/* Filter WAPI DATA Frame */
+
+	/* Update statstics for AP roaming */
+	if (!bToOtherSTA) {
+		ieee->LinkDetectInfo.NumRecvDataInPeriod++;
+		ieee->LinkDetectInfo.NumRxOkInPeriod++;
+	}
+
+	/* Data frame - extract src/dst addresses */
+	rtllib_rx_extract_addr(ieee, hdr, dst, src, bssid);
+
+	/* Filter Data frames */
+	ret = rtllib_rx_data_filter(ieee, fc, dst, src, bssid, hdr->addr2);
+	if (ret < 0)
+		goto rx_dropped;
+
+	if (skb->len == hdrlen)
+		goto rx_dropped;
+
+	/* Send pspoll based on moredata */
+	if ((ieee->iw_mode == IW_MODE_INFRA)  &&
+	    (ieee->sta_sleep == LPS_IS_SLEEP) &&
+	    (ieee->polling) && (!bToOtherSTA)) {
+		if (WLAN_FC_MORE_DATA(fc)) {
+			/* more data bit is set, let's request a new frame
+			 * from the AP
+			 */
+			rtllib_sta_ps_send_pspoll_frame(ieee);
+		} else {
+			ieee->polling =  false;
+		}
+	}
+
+	/* Get crypt if encrypted */
+	ret = rtllib_rx_get_crypt(ieee, skb, &crypt, hdrlen);
+	if (ret == -1)
+		goto rx_dropped;
+
+	/* Decrypt data frame (including reassemble) */
+	ret = rtllib_rx_decrypt(ieee, skb, rx_stats, crypt, hdrlen);
+	if (ret == -1)
+		goto rx_dropped;
+	else if (ret == -2)
+		goto rx_exit;
+
+	/* Get TS for Rx Reorder  */
+	hdr = (struct rtllib_hdr_4addr *)skb->data;
+	if (ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
+		&& !is_multicast_ether_addr(hdr->addr1)
+		&& (!bToOtherSTA)) {
+		TID = Frame_QoSTID(skb->data);
+		SeqNum = WLAN_GET_SEQ_SEQ(sc);
+		GetTs(ieee, (struct ts_common_info **)&pTS, hdr->addr2, TID,
+		      RX_DIR, true);
+		if (TID != 0 && TID != 3)
+			ieee->bis_any_nonbepkts = true;
+	}
+
+	/* Parse rx data frame (For AMSDU) */
+	/* skb: hdr + (possible reassembled) full plaintext payload */
+	rxb = kmalloc(sizeof(struct rtllib_rxb), GFP_ATOMIC);
+	if (!rxb)
+		goto rx_dropped;
+
+	/* to parse amsdu packets */
+	/* qos data packets & reserved bit is 1 */
+	if (parse_subframe(ieee, skb, rx_stats, rxb, src, dst) == 0) {
+		/* only to free rxb, and not submit the packets
+		 * to upper layer
+		 */
+		for (i = 0; i < rxb->nr_subframes; i++)
+			dev_kfree_skb(rxb->subframes[i]);
+		kfree(rxb);
+		rxb = NULL;
+		goto rx_dropped;
+	}
+
+	/* Update WAPI PN */
+
+	/* Check if leave LPS */
+	if (!bToOtherSTA) {
+		if (ieee->bIsAggregateFrame)
+			nr_subframes = rxb->nr_subframes;
+		else
+			nr_subframes = 1;
+		if (unicast)
+			ieee->LinkDetectInfo.NumRxUnicastOkInPeriod += nr_subframes;
+		rtllib_rx_check_leave_lps(ieee, unicast, nr_subframes);
+	}
+
+	/* Indicate packets to upper layer or Rx Reorder */
+	if (!ieee->pHTInfo->cur_rx_reorder_enable || pTS == NULL || bToOtherSTA)
+		rtllib_rx_indicate_pkt_legacy(ieee, rx_stats, rxb, dst, src);
+	else
+		RxReorderIndicatePacket(ieee, rxb, pTS, SeqNum);
+
+	dev_kfree_skb(skb);
+
+ rx_exit:
+	return 1;
+
+ rx_dropped:
+	ieee->stats.rx_dropped++;
+
+	/* Returning 0 indicates to caller that we have not handled the SKB--
+	 * so it is still allocated and can be used again by underlying
+	 * hardware as a DMA target
+	 */
+	return 0;
+}
+
+static int rtllib_rx_Master(struct rtllib_device *ieee, struct sk_buff *skb,
+		 struct rtllib_rx_stats *rx_stats)
+{
+	return 0;
+}
+
+static int rtllib_rx_Monitor(struct rtllib_device *ieee, struct sk_buff *skb,
+		 struct rtllib_rx_stats *rx_stats)
+{
+	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	size_t hdrlen = rtllib_get_hdrlen(fc);
+
+	if (skb->len < hdrlen) {
+		netdev_info(ieee->dev,
+			    "%s():ERR!!! skb->len is smaller than hdrlen\n",
+			    __func__);
+		return 0;
+	}
+
+	if (HTCCheck(ieee, skb->data)) {
+		if (net_ratelimit())
+			netdev_info(ieee->dev, "%s: Find HTCControl!\n",
+				    __func__);
+		hdrlen += 4;
+	}
+
+	ieee->stats.rx_packets++;
+	ieee->stats.rx_bytes += skb->len;
+	rtllib_monitor_rx(ieee, skb, rx_stats, hdrlen);
+
+	return 1;
+}
+
+static int rtllib_rx_Mesh(struct rtllib_device *ieee, struct sk_buff *skb,
+		 struct rtllib_rx_stats *rx_stats)
+{
+	return 0;
+}
+
+/* All received frames are sent to this function. @skb contains the frame in
+ * IEEE 802.11 format, i.e., in the format it was sent over air.
+ * This function is called only as a tasklet (software IRQ).
+ */
+int rtllib_rx(struct rtllib_device *ieee, struct sk_buff *skb,
+		 struct rtllib_rx_stats *rx_stats)
+{
+	int ret = 0;
+
+	if (!ieee || !skb || !rx_stats) {
+		pr_info("%s: Input parameters NULL!\n", __func__);
+		goto rx_dropped;
+	}
+	if (skb->len < 10) {
+		netdev_info(ieee->dev, "%s: SKB length < 10\n", __func__);
+		goto rx_dropped;
+	}
+
+	switch (ieee->iw_mode) {
+	case IW_MODE_ADHOC:
+	case IW_MODE_INFRA:
+		ret = rtllib_rx_InfraAdhoc(ieee, skb, rx_stats);
+		break;
+	case IW_MODE_MASTER:
+	case IW_MODE_REPEAT:
+		ret = rtllib_rx_Master(ieee, skb, rx_stats);
+		break;
+	case IW_MODE_MONITOR:
+		ret = rtllib_rx_Monitor(ieee, skb, rx_stats);
+		break;
+	case IW_MODE_MESH:
+		ret = rtllib_rx_Mesh(ieee, skb, rx_stats);
+		break;
+	default:
+		netdev_info(ieee->dev, "%s: ERR iw mode!!!\n", __func__);
+		break;
+	}
+
+	return ret;
+
+ rx_dropped:
+	if (ieee)
+		ieee->stats.rx_dropped++;
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_rx);
+
+static u8 qos_oui[QOS_OUI_LEN] = { 0x00, 0x50, 0xF2 };
+
+/* Make ther structure we read from the beacon packet has the right values */
+static int rtllib_verify_qos_info(struct rtllib_qos_information_element
+				     *info_element, int sub_type)
+{
+
+	if (info_element->elementID != QOS_ELEMENT_ID)
+		return -1;
+	if (info_element->qui_subtype != sub_type)
+		return -1;
+	if (memcmp(info_element->qui, qos_oui, QOS_OUI_LEN))
+		return -1;
+	if (info_element->qui_type != QOS_OUI_TYPE)
+		return -1;
+	if (info_element->version != QOS_VERSION_1)
+		return -1;
+
+	return 0;
+}
+
+
+/* Parse a QoS parameter element */
+static int rtllib_read_qos_param_element(
+			struct rtllib_qos_parameter_info *element_param,
+			struct rtllib_info_element *info_element)
+{
+	size_t size = sizeof(*element_param);
+
+	if (!element_param || !info_element || info_element->len != size - 2)
+		return -1;
+
+	memcpy(element_param, info_element, size);
+	return rtllib_verify_qos_info(&element_param->info_element,
+				      QOS_OUI_PARAM_SUB_TYPE);
+}
+
+/* Parse a QoS information element */
+static int rtllib_read_qos_info_element(
+			struct rtllib_qos_information_element *element_info,
+			struct rtllib_info_element *info_element)
+{
+	size_t size = sizeof(*element_info);
+
+	if (!element_info || !info_element || info_element->len != size - 2)
+		return -1;
+
+	memcpy(element_info, info_element, size);
+	return rtllib_verify_qos_info(element_info, QOS_OUI_INFO_SUB_TYPE);
+}
+
+
+/* Write QoS parameters from the ac parameters. */
+static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info *param_elm,
+					       struct rtllib_qos_data *qos_data)
+{
+	struct rtllib_qos_ac_parameter *ac_params;
+	struct rtllib_qos_parameters *qos_param = &(qos_data->parameters);
+	int i;
+	u8 aci;
+	u8 acm;
+
+	qos_data->wmm_acm = 0;
+	for (i = 0; i < QOS_QUEUE_NUM; i++) {
+		ac_params = &(param_elm->ac_params_record[i]);
+
+		aci = (ac_params->aci_aifsn & 0x60) >> 5;
+		acm = (ac_params->aci_aifsn & 0x10) >> 4;
+
+		if (aci >= QOS_QUEUE_NUM)
+			continue;
+		switch (aci) {
+		case 1:
+			/* BIT(0) | BIT(3) */
+			if (acm)
+				qos_data->wmm_acm |= (0x01<<0)|(0x01<<3);
+			break;
+		case 2:
+			/* BIT(4) | BIT(5) */
+			if (acm)
+				qos_data->wmm_acm |= (0x01<<4)|(0x01<<5);
+			break;
+		case 3:
+			/* BIT(6) | BIT(7) */
+			if (acm)
+				qos_data->wmm_acm |= (0x01<<6)|(0x01<<7);
+			break;
+		case 0:
+		default:
+			/* BIT(1) | BIT(2) */
+			if (acm)
+				qos_data->wmm_acm |= (0x01<<1)|(0x01<<2);
+			break;
+		}
+
+		qos_param->aifs[aci] = (ac_params->aci_aifsn) & 0x0f;
+
+		/* WMM spec P.11: The minimum value for AIFSN shall be 2 */
+		qos_param->aifs[aci] = max_t(u8, qos_param->aifs[aci], 2);
+
+		qos_param->cw_min[aci] = cpu_to_le16(ac_params->ecw_min_max &
+						     0x0F);
+
+		qos_param->cw_max[aci] = cpu_to_le16((ac_params->ecw_min_max &
+						      0xF0) >> 4);
+
+		qos_param->flag[aci] =
+		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
+		qos_param->tx_op_limit[aci] = ac_params->tx_op_limit;
+	}
+	return 0;
+}
+
+/* we have a generic data element which it may contain QoS information or
+ * parameters element. check the information element length to decide
+ * which type to read
+ */
+static int rtllib_parse_qos_info_param_IE(struct rtllib_device *ieee,
+					  struct rtllib_info_element
+					     *info_element,
+					  struct rtllib_network *network)
+{
+	int rc = 0;
+	struct rtllib_qos_information_element qos_info_element;
+
+	rc = rtllib_read_qos_info_element(&qos_info_element, info_element);
+
+	if (rc == 0) {
+		network->qos_data.param_count = qos_info_element.ac_info & 0x0F;
+		network->flags |= NETWORK_HAS_QOS_INFORMATION;
+	} else {
+		struct rtllib_qos_parameter_info param_element;
+
+		rc = rtllib_read_qos_param_element(&param_element,
+						      info_element);
+		if (rc == 0) {
+			rtllib_qos_convert_ac_to_parameters(&param_element,
+							       &(network->qos_data));
+			network->flags |= NETWORK_HAS_QOS_PARAMETERS;
+			network->qos_data.param_count =
+			    param_element.info_element.ac_info & 0x0F;
+		}
+	}
+
+	if (rc == 0) {
+		netdev_dbg(ieee->dev, "QoS is supported\n");
+		network->qos_data.supported = 1;
+	}
+	return rc;
+}
+
+static const char *get_info_element_string(u16 id)
+{
+	switch (id) {
+	case MFIE_TYPE_SSID:
+		return "SSID";
+	case MFIE_TYPE_RATES:
+		return "RATES";
+	case MFIE_TYPE_FH_SET:
+		return "FH_SET";
+	case MFIE_TYPE_DS_SET:
+		return "DS_SET";
+	case MFIE_TYPE_CF_SET:
+		return "CF_SET";
+	case MFIE_TYPE_TIM:
+		return "TIM";
+	case MFIE_TYPE_IBSS_SET:
+		return "IBSS_SET";
+	case MFIE_TYPE_COUNTRY:
+		return "COUNTRY";
+	case MFIE_TYPE_HOP_PARAMS:
+		return "HOP_PARAMS";
+	case MFIE_TYPE_HOP_TABLE:
+		return "HOP_TABLE";
+	case MFIE_TYPE_REQUEST:
+		return "REQUEST";
+	case MFIE_TYPE_CHALLENGE:
+		return "CHALLENGE";
+	case MFIE_TYPE_POWER_CONSTRAINT:
+		return "POWER_CONSTRAINT";
+	case MFIE_TYPE_POWER_CAPABILITY:
+		return "POWER_CAPABILITY";
+	case MFIE_TYPE_TPC_REQUEST:
+		return "TPC_REQUEST";
+	case MFIE_TYPE_TPC_REPORT:
+		return "TPC_REPORT";
+	case MFIE_TYPE_SUPP_CHANNELS:
+		return "SUPP_CHANNELS";
+	case MFIE_TYPE_CSA:
+		return "CSA";
+	case MFIE_TYPE_MEASURE_REQUEST:
+		return "MEASURE_REQUEST";
+	case MFIE_TYPE_MEASURE_REPORT:
+		return "MEASURE_REPORT";
+	case MFIE_TYPE_QUIET:
+		return "QUIET";
+	case MFIE_TYPE_IBSS_DFS:
+		return "IBSS_DFS";
+	case MFIE_TYPE_RSN:
+		return "RSN";
+	case MFIE_TYPE_RATES_EX:
+		return "RATES_EX";
+	case MFIE_TYPE_GENERIC:
+		return "GENERIC";
+	case MFIE_TYPE_QOS_PARAMETER:
+		return "QOS_PARAMETER";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+static inline void rtllib_extract_country_ie(
+	struct rtllib_device *ieee,
+	struct rtllib_info_element *info_element,
+	struct rtllib_network *network,
+	u8 *addr2)
+{
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (info_element->len != 0) {
+			memcpy(network->CountryIeBuf, info_element->data,
+			       info_element->len);
+			network->CountryIeLen = info_element->len;
+
+			if (!IS_COUNTRY_IE_VALID(ieee)) {
+				if (rtllib_act_scanning(ieee, false) &&
+				    ieee->FirstIe_InScan)
+					netdev_info(ieee->dev,
+						    "Received beacon ContryIE, SSID: <%s>\n",
+						    network->ssid);
+				dot11d_update_country(ieee, addr2,
+						       info_element->len,
+						       info_element->data);
+			}
+		}
+
+		if (IS_EQUAL_CIE_SRC(ieee, addr2))
+			UPDATE_CIE_WATCHDOG(ieee);
+	}
+}
+
+static void rtllib_parse_mife_generic(struct rtllib_device *ieee,
+				      struct rtllib_info_element *info_element,
+				      struct rtllib_network *network,
+				      u16 *tmp_htcap_len,
+				      u16 *tmp_htinfo_len)
+{
+	u16 ht_realtek_agg_len = 0;
+	u8  ht_realtek_agg_buf[MAX_IE_LEN];
+
+	if (!rtllib_parse_qos_info_param_IE(ieee, info_element, network))
+		return;
+	if (info_element->len >= 4 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x50 &&
+	    info_element->data[2] == 0xf2 &&
+	    info_element->data[3] == 0x01) {
+		network->wpa_ie_len = min(info_element->len + 2,
+					  MAX_WPA_IE_LEN);
+		memcpy(network->wpa_ie, info_element, network->wpa_ie_len);
+		return;
+	}
+	if (info_element->len == 7 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0xe0 &&
+	    info_element->data[2] == 0x4c &&
+	    info_element->data[3] == 0x01 &&
+	    info_element->data[4] == 0x02)
+		network->Turbo_Enable = 1;
+
+	if (*tmp_htcap_len == 0) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0x90 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x033) {
+			*tmp_htcap_len = min_t(u8, info_element->len,
+					       MAX_IE_LEN);
+			if (*tmp_htcap_len != 0) {
+				network->bssht.bd_ht_spec_ver = HT_SPEC_VER_EWC;
+				network->bssht.bd_ht_cap_len = min_t(u16, *tmp_htcap_len,
+								  sizeof(network->bssht.bd_ht_cap_buf));
+				memcpy(network->bssht.bd_ht_cap_buf,
+				       info_element->data,
+				       network->bssht.bd_ht_cap_len);
+			}
+		}
+		if (*tmp_htcap_len != 0) {
+			network->bssht.bd_support_ht = true;
+			network->bssht.bd_ht_1r = ((((struct ht_capab_ele *)(network->bssht.bd_ht_cap_buf))->MCS[1]) == 0);
+		} else {
+			network->bssht.bd_support_ht = false;
+			network->bssht.bd_ht_1r = false;
+		}
+	}
+
+
+	if (*tmp_htinfo_len == 0) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0x90 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x034) {
+			*tmp_htinfo_len = min_t(u8, info_element->len,
+						MAX_IE_LEN);
+			if (*tmp_htinfo_len != 0) {
+				network->bssht.bd_ht_spec_ver = HT_SPEC_VER_EWC;
+				network->bssht.bd_ht_info_len = min_t(u16, *tmp_htinfo_len,
+								      sizeof(network->bssht.bd_ht_info_buf));
+				memcpy(network->bssht.bd_ht_info_buf,
+				       info_element->data,
+				       network->bssht.bd_ht_info_len);
+			}
+		}
+	}
+
+	if (network->bssht.bd_support_ht) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0xe0 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x02) {
+			ht_realtek_agg_len = min_t(u8, info_element->len,
+						   MAX_IE_LEN);
+			memcpy(ht_realtek_agg_buf, info_element->data,
+			       info_element->len);
+		}
+		if (ht_realtek_agg_len >= 5) {
+			network->realtek_cap_exit = true;
+			network->bssht.bd_rt2rt_aggregation = true;
+
+			if ((ht_realtek_agg_buf[4] == 1) &&
+			    (ht_realtek_agg_buf[5] & 0x02))
+				network->bssht.bd_rt2rt_long_slot_time = true;
+
+			if ((ht_realtek_agg_buf[4] == 1) &&
+			    (ht_realtek_agg_buf[5] & RT_HT_CAP_USE_92SE))
+				network->bssht.rt2rt_ht_mode |= RT_HT_CAP_USE_92SE;
+		}
+	}
+	if (ht_realtek_agg_len >= 5) {
+		if ((ht_realtek_agg_buf[5] & RT_HT_CAP_USE_SOFTAP))
+			network->bssht.rt2rt_ht_mode |= RT_HT_CAP_USE_SOFTAP;
+	}
+
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x05 &&
+	     info_element->data[2] == 0xb5) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x0a &&
+	     info_element->data[2] == 0xf7) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x10 &&
+	     info_element->data[2] == 0x18)) {
+		network->broadcom_cap_exist = true;
+	}
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x0c &&
+	    info_element->data[2] == 0x43)
+		network->ralink_cap_exist = true;
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x03 &&
+	     info_element->data[2] == 0x7f) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x13 &&
+	     info_element->data[2] == 0x74))
+		network->atheros_cap_exist = true;
+
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x50 &&
+	     info_element->data[2] == 0x43))
+		network->marvell_cap_exist = true;
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96)
+		network->cisco_cap_exist = true;
+
+
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x0a &&
+	    info_element->data[2] == 0xf5)
+		network->airgo_cap_exist = true;
+
+	if (info_element->len > 4 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96 &&
+	    info_element->data[3] == 0x01) {
+		if (info_element->len == 6) {
+			memcpy(network->CcxRmState, &info_element->data[4], 2);
+			if (network->CcxRmState[0] != 0)
+				network->bCcxRmEnable = true;
+			else
+				network->bCcxRmEnable = false;
+			network->MBssidMask = network->CcxRmState[1] & 0x07;
+			if (network->MBssidMask != 0) {
+				network->bMBssidValid = true;
+				network->MBssidMask = 0xff <<
+						      (network->MBssidMask);
+				ether_addr_copy(network->MBssid,
+						network->bssid);
+				network->MBssid[5] &= network->MBssidMask;
+			} else {
+				network->bMBssidValid = false;
+			}
+		} else {
+			network->bCcxRmEnable = false;
+		}
+	}
+	if (info_element->len > 4  &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96 &&
+	    info_element->data[3] == 0x03) {
+		if (info_element->len == 5) {
+			network->bWithCcxVerNum = true;
+			network->BssCcxVerNumber = info_element->data[4];
+		} else {
+			network->bWithCcxVerNum = false;
+			network->BssCcxVerNumber = 0;
+		}
+	}
+	if (info_element->len > 4  &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x50 &&
+	    info_element->data[2] == 0xf2 &&
+	    info_element->data[3] == 0x04) {
+		netdev_dbg(ieee->dev, "MFIE_TYPE_WZC: %d bytes\n",
+			   info_element->len);
+		network->wzc_ie_len = min(info_element->len+2, MAX_WZC_IE_LEN);
+		memcpy(network->wzc_ie, info_element, network->wzc_ie_len);
+	}
+}
+
+static void rtllib_parse_mfie_ht_cap(struct rtllib_info_element *info_element,
+				     struct rtllib_network *network,
+				     u16 *tmp_htcap_len)
+{
+	struct bss_ht *ht = &network->bssht;
+
+	*tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
+	if (*tmp_htcap_len != 0) {
+		ht->bd_ht_spec_ver = HT_SPEC_VER_EWC;
+		ht->bd_ht_cap_len = min_t(u16, *tmp_htcap_len,
+				       sizeof(ht->bd_ht_cap_buf));
+		memcpy(ht->bd_ht_cap_buf, info_element->data, ht->bd_ht_cap_len);
+
+		ht->bd_support_ht = true;
+		ht->bd_ht_1r = ((((struct ht_capab_ele *)
+				ht->bd_ht_cap_buf))->MCS[1]) == 0;
+
+		ht->bd_bandwidth = (enum ht_channel_width)
+					     (((struct ht_capab_ele *)
+					     (ht->bd_ht_cap_buf))->ChlWidth);
+	} else {
+		ht->bd_support_ht = false;
+		ht->bd_ht_1r = false;
+		ht->bd_bandwidth = HT_CHANNEL_WIDTH_20;
+	}
+}
+
+int rtllib_parse_info_param(struct rtllib_device *ieee,
+		struct rtllib_info_element *info_element,
+		u16 length,
+		struct rtllib_network *network,
+		struct rtllib_rx_stats *stats)
+{
+	u8 i;
+	short offset;
+	u16	tmp_htcap_len = 0;
+	u16	tmp_htinfo_len = 0;
+	char rates_str[64];
+	char *p;
+
+	while (length >= sizeof(*info_element)) {
+		if (sizeof(*info_element) + info_element->len > length) {
+			netdev_dbg(ieee->dev,
+				   "Info elem: parse failed: info_element->len + 2 > left : info_element->len+2=%zd left=%d, id=%d.\n",
+				   info_element->len + sizeof(*info_element),
+				   length, info_element->id);
+			/* We stop processing but don't return an error here
+			 * because some misbehaviour APs break this rule. ie.
+			 * Orinoco AP1000.
+			 */
+			break;
+		}
+
+		switch (info_element->id) {
+		case MFIE_TYPE_SSID:
+			if (rtllib_is_empty_essid(info_element->data,
+						     info_element->len)) {
+				network->flags |= NETWORK_EMPTY_ESSID;
+				break;
+			}
+
+			network->ssid_len = min(info_element->len,
+						(u8)IW_ESSID_MAX_SIZE);
+			memcpy(network->ssid, info_element->data,
+			       network->ssid_len);
+			if (network->ssid_len < IW_ESSID_MAX_SIZE)
+				memset(network->ssid + network->ssid_len, 0,
+				       IW_ESSID_MAX_SIZE - network->ssid_len);
+
+			netdev_dbg(ieee->dev, "MFIE_TYPE_SSID: '%s' len=%d.\n",
+				   network->ssid, network->ssid_len);
+			break;
+
+		case MFIE_TYPE_RATES:
+			p = rates_str;
+			network->rates_len = min(info_element->len,
+						 MAX_RATES_LENGTH);
+			for (i = 0; i < network->rates_len; i++) {
+				network->rates[i] = info_element->data[i];
+				p += scnprintf(p, sizeof(rates_str) -
+					      (p - rates_str), "%02X ",
+					      network->rates[i]);
+				if (rtllib_is_ofdm_rate
+				    (info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    RTLLIB_BASIC_RATE_MASK)
+						network->flags &=
+						    ~NETWORK_HAS_CCK;
+				}
+
+				if (rtllib_is_cck_rate
+				    (info_element->data[i])) {
+					network->flags |= NETWORK_HAS_CCK;
+				}
+			}
+
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RATES: '%s' (%d)\n",
+				   rates_str, network->rates_len);
+			break;
+
+		case MFIE_TYPE_RATES_EX:
+			p = rates_str;
+			network->rates_ex_len = min(info_element->len,
+						    MAX_RATES_EX_LENGTH);
+			for (i = 0; i < network->rates_ex_len; i++) {
+				network->rates_ex[i] = info_element->data[i];
+				p += scnprintf(p, sizeof(rates_str) -
+					      (p - rates_str), "%02X ",
+					      network->rates_ex[i]);
+				if (rtllib_is_ofdm_rate
+				    (info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    RTLLIB_BASIC_RATE_MASK)
+						network->flags &=
+						    ~NETWORK_HAS_CCK;
+				}
+			}
+
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RATES_EX: '%s' (%d)\n",
+				   rates_str, network->rates_ex_len);
+			break;
+
+		case MFIE_TYPE_DS_SET:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_DS_SET: %d\n",
+				   info_element->data[0]);
+			network->channel = info_element->data[0];
+			break;
+
+		case MFIE_TYPE_FH_SET:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_FH_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_CF_SET:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_CF_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_TIM:
+			if (info_element->len < 4)
+				break;
+
+			network->tim.tim_count = info_element->data[0];
+			network->tim.tim_period = info_element->data[1];
+
+			network->dtim_period = info_element->data[1];
+			if (ieee->state != RTLLIB_LINKED)
+				break;
+			network->last_dtim_sta_time = jiffies;
+
+			network->dtim_data = RTLLIB_DTIM_VALID;
+
+
+			if (info_element->data[2] & 1)
+				network->dtim_data |= RTLLIB_DTIM_MBCAST;
+
+			offset = (info_element->data[2] >> 1)*2;
+
+
+			if (ieee->assoc_id < 8*offset ||
+			    ieee->assoc_id > 8*(offset + info_element->len - 3))
+				break;
+
+			offset = (ieee->assoc_id / 8) - offset;
+			if (info_element->data[3 + offset] &
+			   (1 << (ieee->assoc_id % 8)))
+				network->dtim_data |= RTLLIB_DTIM_UCAST;
+
+			network->listen_interval = network->dtim_period;
+			break;
+
+		case MFIE_TYPE_ERP:
+			network->erp_value = info_element->data[0];
+			network->flags |= NETWORK_HAS_ERP_VALUE;
+			netdev_dbg(ieee->dev, "MFIE_TYPE_ERP_SET: %d\n",
+				   network->erp_value);
+			break;
+		case MFIE_TYPE_IBSS_SET:
+			network->atim_window = info_element->data[0];
+			netdev_dbg(ieee->dev, "MFIE_TYPE_IBSS_SET: %d\n",
+				   network->atim_window);
+			break;
+
+		case MFIE_TYPE_CHALLENGE:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_CHALLENGE: ignored\n");
+			break;
+
+		case MFIE_TYPE_GENERIC:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_GENERIC: %d bytes\n",
+				   info_element->len);
+
+			rtllib_parse_mife_generic(ieee, info_element, network,
+						  &tmp_htcap_len,
+						  &tmp_htinfo_len);
+			break;
+
+		case MFIE_TYPE_RSN:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RSN: %d bytes\n",
+				   info_element->len);
+			network->rsn_ie_len = min(info_element->len + 2,
+						  MAX_WPA_IE_LEN);
+			memcpy(network->rsn_ie, info_element,
+			       network->rsn_ie_len);
+			break;
+
+		case MFIE_TYPE_HT_CAP:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_HT_CAP: %d bytes\n",
+				   info_element->len);
+
+			rtllib_parse_mfie_ht_cap(info_element, network,
+						 &tmp_htcap_len);
+			break;
+
+
+		case MFIE_TYPE_HT_INFO:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_HT_INFO: %d bytes\n",
+				   info_element->len);
+			tmp_htinfo_len = min_t(u8, info_element->len,
+					       MAX_IE_LEN);
+			if (tmp_htinfo_len) {
+				network->bssht.bd_ht_spec_ver = HT_SPEC_VER_IEEE;
+				network->bssht.bd_ht_info_len = tmp_htinfo_len >
+					sizeof(network->bssht.bd_ht_info_buf) ?
+					sizeof(network->bssht.bd_ht_info_buf) :
+					tmp_htinfo_len;
+				memcpy(network->bssht.bd_ht_info_buf,
+				       info_element->data,
+				       network->bssht.bd_ht_info_len);
+			}
+			break;
+
+		case MFIE_TYPE_AIRONET:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_AIRONET: %d bytes\n",
+				   info_element->len);
+			if (info_element->len > IE_CISCO_FLAG_POSITION) {
+				network->bWithAironetIE = true;
+
+				if ((info_element->data[IE_CISCO_FLAG_POSITION]
+				     & SUPPORT_CKIP_MIC) ||
+				     (info_element->data[IE_CISCO_FLAG_POSITION]
+				     & SUPPORT_CKIP_PK))
+					network->bCkipSupported = true;
+				else
+					network->bCkipSupported = false;
+			} else {
+				network->bWithAironetIE = false;
+				network->bCkipSupported = false;
+			}
+			break;
+		case MFIE_TYPE_QOS_PARAMETER:
+			netdev_err(ieee->dev,
+				   "QoS Error need to parse QOS_PARAMETER IE\n");
+			break;
+
+		case MFIE_TYPE_COUNTRY:
+			netdev_dbg(ieee->dev, "MFIE_TYPE_COUNTRY: %d bytes\n",
+				   info_element->len);
+			rtllib_extract_country_ie(ieee, info_element, network,
+						  network->bssid);
+			break;
+/* TODO */
+		default:
+			netdev_dbg(ieee->dev,
+				   "Unsupported info element: %s (%d)\n",
+				   get_info_element_string(info_element->id),
+				   info_element->id);
+			break;
+		}
+
+		length -= sizeof(*info_element) + info_element->len;
+		info_element =
+		    (struct rtllib_info_element *)&info_element->data[info_element->len];
+	}
+
+	if (!network->atheros_cap_exist && !network->broadcom_cap_exist &&
+	    !network->cisco_cap_exist && !network->ralink_cap_exist &&
+	    !network->bssht.bd_rt2rt_aggregation)
+		network->unknown_cap_exist = true;
+	else
+		network->unknown_cap_exist = false;
+	return 0;
+}
+
+static long rtllib_translate_todbm(u8 signal_strength_index)
+{
+	long	signal_power;
+
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+
+	return signal_power;
+}
+
+static inline int rtllib_network_init(
+	struct rtllib_device *ieee,
+	struct rtllib_probe_response *beacon,
+	struct rtllib_network *network,
+	struct rtllib_rx_stats *stats)
+{
+	memset(&network->qos_data, 0, sizeof(struct rtllib_qos_data));
+
+	/* Pull out fixed field data */
+	ether_addr_copy(network->bssid, beacon->header.addr3);
+	network->capability = le16_to_cpu(beacon->capability);
+	network->last_scanned = jiffies;
+	network->time_stamp[0] = beacon->time_stamp[0];
+	network->time_stamp[1] = beacon->time_stamp[1];
+	network->beacon_interval = le16_to_cpu(beacon->beacon_interval);
+	/* Where to pull this? beacon->listen_interval;*/
+	network->listen_interval = 0x0A;
+	network->rates_len = network->rates_ex_len = 0;
+	network->ssid_len = 0;
+	network->hidden_ssid_len = 0;
+	memset(network->hidden_ssid, 0, sizeof(network->hidden_ssid));
+	network->flags = 0;
+	network->atim_window = 0;
+	network->erp_value = (network->capability & WLAN_CAPABILITY_IBSS) ?
+	    0x3 : 0x0;
+	network->berp_info_valid = false;
+	network->broadcom_cap_exist = false;
+	network->ralink_cap_exist = false;
+	network->atheros_cap_exist = false;
+	network->cisco_cap_exist = false;
+	network->unknown_cap_exist = false;
+	network->realtek_cap_exit = false;
+	network->marvell_cap_exist = false;
+	network->airgo_cap_exist = false;
+	network->Turbo_Enable = 0;
+	network->SignalStrength = stats->SignalStrength;
+	network->RSSI = stats->SignalStrength;
+	network->CountryIeLen = 0;
+	memset(network->CountryIeBuf, 0, MAX_IE_LEN);
+	HTInitializeBssDesc(&network->bssht);
+	if (stats->freq == RTLLIB_52GHZ_BAND) {
+		/* for A band (No DS info) */
+		network->channel = stats->received_channel;
+	} else
+		network->flags |= NETWORK_HAS_CCK;
+
+	network->wpa_ie_len = 0;
+	network->rsn_ie_len = 0;
+	network->wzc_ie_len = 0;
+
+	if (rtllib_parse_info_param(ieee,
+			beacon->info_element,
+			(stats->len - sizeof(*beacon)),
+			network,
+			stats))
+		return 1;
+
+	network->mode = 0;
+	if (stats->freq == RTLLIB_52GHZ_BAND)
+		network->mode = IEEE_A;
+	else {
+		if (network->flags & NETWORK_HAS_OFDM)
+			network->mode |= IEEE_G;
+		if (network->flags & NETWORK_HAS_CCK)
+			network->mode |= IEEE_B;
+	}
+
+	if (network->mode == 0) {
+		netdev_dbg(ieee->dev, "Filtered out '%s (%pM)' network.\n",
+			   escape_essid(network->ssid, network->ssid_len),
+			   network->bssid);
+		return 1;
+	}
+
+	if (network->bssht.bd_support_ht) {
+		if (network->mode == IEEE_A)
+			network->mode = IEEE_N_5G;
+		else if (network->mode & (IEEE_G | IEEE_B))
+			network->mode = IEEE_N_24G;
+	}
+	if (rtllib_is_empty_essid(network->ssid, network->ssid_len))
+		network->flags |= NETWORK_EMPTY_ESSID;
+	stats->signal = 30 + (stats->SignalStrength * 70) / 100;
+	stats->noise = rtllib_translate_todbm((u8)(100-stats->signal)) - 25;
+
+	memcpy(&network->stats, stats, sizeof(network->stats));
+
+	return 0;
+}
+
+static inline int is_same_network(struct rtllib_network *src,
+				  struct rtllib_network *dst, u8 ssidbroad)
+{
+	/* A network is only a duplicate if the channel, BSSID, ESSID
+	 * and the capability field (in particular IBSS and BSS) all match.
+	 * We treat all <hidden> with the same BSSID and channel
+	 * as one network
+	 */
+	return (((src->ssid_len == dst->ssid_len) || (!ssidbroad)) &&
+		(src->channel == dst->channel) &&
+		!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
+		(!memcmp(src->ssid, dst->ssid, src->ssid_len) ||
+		(!ssidbroad)) &&
+		((src->capability & WLAN_CAPABILITY_IBSS) ==
+		(dst->capability & WLAN_CAPABILITY_IBSS)) &&
+		((src->capability & WLAN_CAPABILITY_ESS) ==
+		(dst->capability & WLAN_CAPABILITY_ESS)));
+}
+
+
+static inline void update_network(struct rtllib_device *ieee,
+				  struct rtllib_network *dst,
+				  struct rtllib_network *src)
+{
+	int qos_active;
+	u8 old_param;
+
+	memcpy(&dst->stats, &src->stats, sizeof(struct rtllib_rx_stats));
+	dst->capability = src->capability;
+	memcpy(dst->rates, src->rates, src->rates_len);
+	dst->rates_len = src->rates_len;
+	memcpy(dst->rates_ex, src->rates_ex, src->rates_ex_len);
+	dst->rates_ex_len = src->rates_ex_len;
+	if (src->ssid_len > 0) {
+		if (dst->ssid_len == 0) {
+			memset(dst->hidden_ssid, 0, sizeof(dst->hidden_ssid));
+			dst->hidden_ssid_len = src->ssid_len;
+			memcpy(dst->hidden_ssid, src->ssid, src->ssid_len);
+		} else {
+			memset(dst->ssid, 0, dst->ssid_len);
+			dst->ssid_len = src->ssid_len;
+			memcpy(dst->ssid, src->ssid, src->ssid_len);
+		}
+	}
+	dst->mode = src->mode;
+	dst->flags = src->flags;
+	dst->time_stamp[0] = src->time_stamp[0];
+	dst->time_stamp[1] = src->time_stamp[1];
+	if (src->flags & NETWORK_HAS_ERP_VALUE) {
+		dst->erp_value = src->erp_value;
+		dst->berp_info_valid = src->berp_info_valid = true;
+	}
+	dst->beacon_interval = src->beacon_interval;
+	dst->listen_interval = src->listen_interval;
+	dst->atim_window = src->atim_window;
+	dst->dtim_period = src->dtim_period;
+	dst->dtim_data = src->dtim_data;
+	dst->last_dtim_sta_time = src->last_dtim_sta_time;
+	memcpy(&dst->tim, &src->tim, sizeof(struct rtllib_tim_parameters));
+
+	dst->bssht.bd_support_ht = src->bssht.bd_support_ht;
+	dst->bssht.bd_rt2rt_aggregation = src->bssht.bd_rt2rt_aggregation;
+	dst->bssht.bd_ht_cap_len = src->bssht.bd_ht_cap_len;
+	memcpy(dst->bssht.bd_ht_cap_buf, src->bssht.bd_ht_cap_buf,
+	       src->bssht.bd_ht_cap_len);
+	dst->bssht.bd_ht_info_len = src->bssht.bd_ht_info_len;
+	memcpy(dst->bssht.bd_ht_info_buf, src->bssht.bd_ht_info_buf,
+	       src->bssht.bd_ht_info_len);
+	dst->bssht.bd_ht_spec_ver = src->bssht.bd_ht_spec_ver;
+	dst->bssht.bd_rt2rt_long_slot_time = src->bssht.bd_rt2rt_long_slot_time;
+	dst->broadcom_cap_exist = src->broadcom_cap_exist;
+	dst->ralink_cap_exist = src->ralink_cap_exist;
+	dst->atheros_cap_exist = src->atheros_cap_exist;
+	dst->realtek_cap_exit = src->realtek_cap_exit;
+	dst->marvell_cap_exist = src->marvell_cap_exist;
+	dst->cisco_cap_exist = src->cisco_cap_exist;
+	dst->airgo_cap_exist = src->airgo_cap_exist;
+	dst->unknown_cap_exist = src->unknown_cap_exist;
+	memcpy(dst->wpa_ie, src->wpa_ie, src->wpa_ie_len);
+	dst->wpa_ie_len = src->wpa_ie_len;
+	memcpy(dst->rsn_ie, src->rsn_ie, src->rsn_ie_len);
+	dst->rsn_ie_len = src->rsn_ie_len;
+	memcpy(dst->wzc_ie, src->wzc_ie, src->wzc_ie_len);
+	dst->wzc_ie_len = src->wzc_ie_len;
+
+	dst->last_scanned = jiffies;
+	/* qos related parameters */
+	qos_active = dst->qos_data.active;
+	old_param = dst->qos_data.param_count;
+	dst->qos_data.supported = src->qos_data.supported;
+	if (dst->flags & NETWORK_HAS_QOS_PARAMETERS)
+		memcpy(&dst->qos_data, &src->qos_data,
+		       sizeof(struct rtllib_qos_data));
+	if (dst->qos_data.supported == 1) {
+		if (dst->ssid_len)
+			netdev_dbg(ieee->dev,
+				   "QoS the network %s is QoS supported\n",
+				   dst->ssid);
+		else
+			netdev_dbg(ieee->dev,
+				   "QoS the network is QoS supported\n");
+	}
+	dst->qos_data.active = qos_active;
+	dst->qos_data.old_param_count = old_param;
+
+	dst->wmm_info = src->wmm_info;
+	if (src->wmm_param[0].ac_aci_acm_aifsn ||
+	   src->wmm_param[1].ac_aci_acm_aifsn ||
+	   src->wmm_param[2].ac_aci_acm_aifsn ||
+	   src->wmm_param[3].ac_aci_acm_aifsn)
+		memcpy(dst->wmm_param, src->wmm_param, WME_AC_PRAM_LEN);
+
+	dst->SignalStrength = src->SignalStrength;
+	dst->RSSI = src->RSSI;
+	dst->Turbo_Enable = src->Turbo_Enable;
+
+	dst->CountryIeLen = src->CountryIeLen;
+	memcpy(dst->CountryIeBuf, src->CountryIeBuf, src->CountryIeLen);
+
+	dst->bWithAironetIE = src->bWithAironetIE;
+	dst->bCkipSupported = src->bCkipSupported;
+	memcpy(dst->CcxRmState, src->CcxRmState, 2);
+	dst->bCcxRmEnable = src->bCcxRmEnable;
+	dst->MBssidMask = src->MBssidMask;
+	dst->bMBssidValid = src->bMBssidValid;
+	memcpy(dst->MBssid, src->MBssid, 6);
+	dst->bWithCcxVerNum = src->bWithCcxVerNum;
+	dst->BssCcxVerNumber = src->BssCcxVerNumber;
+}
+
+static inline int is_beacon(u16 fc)
+{
+	return (WLAN_FC_GET_STYPE(fc) == RTLLIB_STYPE_BEACON);
+}
+
+static int IsPassiveChannel(struct rtllib_device *rtllib, u8 channel)
+{
+	if (channel > MAX_CHANNEL_NUMBER) {
+		netdev_info(rtllib->dev, "%s(): Invalid Channel\n", __func__);
+		return 0;
+	}
+
+	if (rtllib->active_channel_map[channel] == 2)
+		return 1;
+
+	return 0;
+}
+
+int rtllib_legal_channel(struct rtllib_device *rtllib, u8 channel)
+{
+	if (channel > MAX_CHANNEL_NUMBER) {
+		netdev_info(rtllib->dev, "%s(): Invalid Channel\n", __func__);
+		return 0;
+	}
+	if (rtllib->active_channel_map[channel] > 0)
+		return 1;
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_legal_channel);
+
+static inline void rtllib_process_probe_response(
+	struct rtllib_device *ieee,
+	struct rtllib_probe_response *beacon,
+	struct rtllib_rx_stats *stats)
+{
+	struct rtllib_network *target;
+	struct rtllib_network *oldest = NULL;
+	struct rtllib_info_element *info_element = &beacon->info_element[0];
+	unsigned long flags;
+	short renew;
+	struct rtllib_network *network = kzalloc(sizeof(struct rtllib_network),
+						 GFP_ATOMIC);
+	u16 frame_ctl = le16_to_cpu(beacon->header.frame_ctl);
+
+	if (!network)
+		return;
+
+	netdev_dbg(ieee->dev,
+		   "'%s' ( %pM ): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
+		   escape_essid(info_element->data, info_element->len),
+		   beacon->header.addr3,
+		   (le16_to_cpu(beacon->capability) & (1<<0xf)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xe)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xd)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xc)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xb)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xa)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x9)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x8)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x7)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x6)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x5)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x4)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x3)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x2)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x1)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x0)) ? '1' : '0');
+
+	if (rtllib_network_init(ieee, beacon, network, stats)) {
+		netdev_dbg(ieee->dev, "Dropped '%s' ( %pM) via %s.\n",
+			   escape_essid(info_element->data, info_element->len),
+			   beacon->header.addr3,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
+		goto free_network;
+	}
+
+
+	if (!rtllib_legal_channel(ieee, network->channel))
+		goto free_network;
+
+	if (WLAN_FC_GET_STYPE(frame_ctl) == RTLLIB_STYPE_PROBE_RESP) {
+		if (IsPassiveChannel(ieee, network->channel)) {
+			netdev_info(ieee->dev,
+				    "GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n",
+				    network->channel);
+			goto free_network;
+		}
+	}
+
+	/* The network parsed correctly -- so now we scan our known networks
+	 * to see if we can find it in our list.
+	 *
+	 * NOTE:  This search is definitely not optimized.  Once its doing
+	 *	the "right thing" we'll optimize it for efficiency if
+	 *	necessary
+	 */
+
+	/* Search for this entry in the list and update it if it is
+	 * already there.
+	 */
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	if (is_same_network(&ieee->current_network, network,
+	   (network->ssid_len ? 1 : 0))) {
+		update_network(ieee, &ieee->current_network, network);
+		if ((ieee->current_network.mode == IEEE_N_24G ||
+		     ieee->current_network.mode == IEEE_G) &&
+		    ieee->current_network.berp_info_valid) {
+			if (ieee->current_network.erp_value & ERP_UseProtection)
+				ieee->current_network.buseprotection = true;
+			else
+				ieee->current_network.buseprotection = false;
+		}
+		if (is_beacon(frame_ctl)) {
+			if (ieee->state >= RTLLIB_LINKED)
+				ieee->LinkDetectInfo.NumRecvBcnInPeriod++;
+		}
+	}
+	list_for_each_entry(target, &ieee->network_list, list) {
+		if (is_same_network(target, network,
+		   (target->ssid_len ? 1 : 0)))
+			break;
+		if ((oldest == NULL) ||
+		    (target->last_scanned < oldest->last_scanned))
+			oldest = target;
+	}
+
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information
+	 */
+	if (&target->list == &ieee->network_list) {
+		if (list_empty(&ieee->network_free_list)) {
+			/* If there are no more slots, expire the oldest */
+			list_del(&oldest->list);
+			target = oldest;
+			netdev_dbg(ieee->dev,
+				   "Expired '%s' ( %pM) from network list.\n",
+				   escape_essid(target->ssid, target->ssid_len),
+				   target->bssid);
+		} else {
+			/* Otherwise just pull from the free list */
+			target = list_entry(ieee->network_free_list.next,
+					    struct rtllib_network, list);
+			list_del(ieee->network_free_list.next);
+		}
+
+		netdev_dbg(ieee->dev, "Adding '%s' ( %pM) via %s.\n",
+			   escape_essid(network->ssid, network->ssid_len),
+			   network->bssid,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
+
+		memcpy(target, network, sizeof(*target));
+		list_add_tail(&target->list, &ieee->network_list);
+		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE)
+			rtllib_softmac_new_net(ieee, network);
+	} else {
+		netdev_dbg(ieee->dev, "Updating '%s' ( %pM) via %s.\n",
+			   escape_essid(target->ssid, target->ssid_len),
+			   target->bssid,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
+
+		/* we have an entry and we are going to update it. But this
+		 *  entry may be already expired. In this case we do the same
+		 * as we found a new net and call the new_net handler
+		 */
+		renew = !time_after(target->last_scanned + ieee->scan_age,
+				    jiffies);
+		if ((!target->ssid_len) &&
+		    (((network->ssid_len > 0) && (target->hidden_ssid_len == 0))
+		    || ((ieee->current_network.ssid_len == network->ssid_len) &&
+		    (strncmp(ieee->current_network.ssid, network->ssid,
+		    network->ssid_len) == 0) &&
+		    (ieee->state == RTLLIB_NOLINK))))
+			renew = 1;
+		update_network(ieee, target, network);
+		if (renew && (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE))
+			rtllib_softmac_new_net(ieee, network);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	if (is_beacon(frame_ctl) &&
+	    is_same_network(&ieee->current_network, network,
+	    (network->ssid_len ? 1 : 0)) &&
+	    (ieee->state == RTLLIB_LINKED)) {
+		if (ieee->handle_beacon != NULL)
+			ieee->handle_beacon(ieee->dev, beacon,
+					    &ieee->current_network);
+	}
+free_network:
+	kfree(network);
+}
+
+static void rtllib_rx_mgt(struct rtllib_device *ieee,
+			  struct sk_buff *skb,
+			  struct rtllib_rx_stats *stats)
+{
+	struct rtllib_hdr_4addr *header = (struct rtllib_hdr_4addr *)skb->data;
+
+	if ((WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)) !=
+	    RTLLIB_STYPE_PROBE_RESP) &&
+	    (WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)) !=
+	    RTLLIB_STYPE_BEACON))
+		ieee->last_rx_ps_time = jiffies;
+
+	switch (WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl))) {
+
+	case RTLLIB_STYPE_BEACON:
+		netdev_dbg(ieee->dev, "received BEACON (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
+		rtllib_process_probe_response(
+				ieee, (struct rtllib_probe_response *)header,
+				stats);
+
+		if (ieee->sta_sleep || (ieee->ps != RTLLIB_PS_DISABLED &&
+		    ieee->iw_mode == IW_MODE_INFRA &&
+		    ieee->state == RTLLIB_LINKED))
+			schedule_work(&ieee->ps_task);
+
+		break;
+
+	case RTLLIB_STYPE_PROBE_RESP:
+		netdev_dbg(ieee->dev, "received PROBE RESPONSE (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
+		rtllib_process_probe_response(ieee,
+			      (struct rtllib_probe_response *)header, stats);
+		break;
+	case RTLLIB_STYPE_PROBE_REQ:
+		netdev_dbg(ieee->dev, "received PROBE REQUEST (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
+		if ((ieee->softmac_features & IEEE_SOFTMAC_PROBERS) &&
+		    ((ieee->iw_mode == IW_MODE_ADHOC ||
+		    ieee->iw_mode == IW_MODE_MASTER) &&
+		    ieee->state == RTLLIB_LINKED))
+			rtllib_rx_probe_rq(ieee, skb);
+		break;
+	}
+}
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
new file mode 100644
index 000000000..1a3ca3e57
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -0,0 +1,3256 @@
+// SPDX-License-Identifier: GPL-2.0
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Few lines might be stolen from other part of the rtllib
+ * stack. Copyright who own it's copyright
+ *
+ * WPA code stolen from the ipw2200 driver.
+ * Copyright who own it's copyright.
+ */
+#include "rtllib.h"
+
+#include <linux/random.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/etherdevice.h>
+#include <linux/ieee80211.h>
+#include "dot11d.h"
+
+static void rtllib_sta_wakeup(struct rtllib_device *ieee, short nl);
+
+
+static short rtllib_is_54g(struct rtllib_network *net)
+{
+	return (net->rates_ex_len > 0) || (net->rates_len > 4);
+}
+
+/* returns the total length needed for placing the RATE MFIE
+ * tag and the EXTENDED RATE MFIE tag if needed.
+ * It encludes two bytes per tag for the tag itself and its len
+ */
+static unsigned int rtllib_MFIE_rate_len(struct rtllib_device *ieee)
+{
+	unsigned int rate_len = 0;
+
+	if (ieee->modulation & RTLLIB_CCK_MODULATION)
+		rate_len = RTLLIB_CCK_RATE_LEN + 2;
+
+	if (ieee->modulation & RTLLIB_OFDM_MODULATION)
+
+		rate_len += RTLLIB_OFDM_RATE_LEN + 2;
+
+	return rate_len;
+}
+
+/* place the MFIE rate, tag to the memory (double) pointed.
+ * Then it updates the pointer so that
+ * it points after the new MFIE tag added.
+ */
+static void rtllib_MFIE_Brate(struct rtllib_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	if (ieee->modulation & RTLLIB_CCK_MODULATION) {
+		*tag++ = MFIE_TYPE_RATES;
+		*tag++ = 4;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_1MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_2MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_5MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_11MB;
+	}
+
+	/* We may add an option for custom rates that specific HW
+	 * might support
+	 */
+	*tag_p = tag;
+}
+
+static void rtllib_MFIE_Grate(struct rtllib_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	if (ieee->modulation & RTLLIB_OFDM_MODULATION) {
+		*tag++ = MFIE_TYPE_RATES_EX;
+		*tag++ = 8;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_6MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_9MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_12MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_18MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_24MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_36MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_48MB;
+		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_54MB;
+	}
+	/* We may add an option for custom rates that specific HW might
+	 * support
+	 */
+	*tag_p = tag;
+}
+
+static void rtllib_WMM_Info(struct rtllib_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	*tag++ = MFIE_TYPE_GENERIC;
+	*tag++ = 7;
+	*tag++ = 0x00;
+	*tag++ = 0x50;
+	*tag++ = 0xf2;
+	*tag++ = 0x02;
+	*tag++ = 0x00;
+	*tag++ = 0x01;
+	*tag++ = MAX_SP_Len;
+	*tag_p = tag;
+}
+
+static void rtllib_TURBO_Info(struct rtllib_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	*tag++ = MFIE_TYPE_GENERIC;
+	*tag++ = 7;
+	*tag++ = 0x00;
+	*tag++ = 0xe0;
+	*tag++ = 0x4c;
+	*tag++ = 0x01;
+	*tag++ = 0x02;
+	*tag++ = 0x11;
+	*tag++ = 0x00;
+
+	*tag_p = tag;
+	netdev_alert(ieee->dev, "This is enable turbo mode IE process\n");
+}
+
+static void enqueue_mgmt(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	int nh;
+
+	nh = (ieee->mgmt_queue_head + 1) % MGMT_QUEUE_NUM;
+
+/* if the queue is full but we have newer frames then
+ * just overwrites the oldest.
+ *
+ * if (nh == ieee->mgmt_queue_tail)
+ *		return -1;
+ */
+	ieee->mgmt_queue_head = nh;
+	ieee->mgmt_queue_ring[nh] = skb;
+
+}
+
+static void init_mgmt_queue(struct rtllib_device *ieee)
+{
+	ieee->mgmt_queue_tail = ieee->mgmt_queue_head = 0;
+}
+
+
+u8
+MgntQuery_TxRateExcludeCCKRates(struct rtllib_device *ieee)
+{
+	u16	i;
+	u8	QueryRate = 0;
+	u8	BasicRate;
+
+
+	for (i = 0; i < ieee->current_network.rates_len; i++) {
+		BasicRate = ieee->current_network.rates[i]&0x7F;
+		if (!rtllib_is_cck_rate(BasicRate)) {
+			if (QueryRate == 0) {
+				QueryRate = BasicRate;
+			} else {
+				if (BasicRate < QueryRate)
+					QueryRate = BasicRate;
+			}
+		}
+	}
+
+	if (QueryRate == 0) {
+		QueryRate = 12;
+		netdev_info(ieee->dev, "No BasicRate found!!\n");
+	}
+	return QueryRate;
+}
+
+static u8 MgntQuery_MgntFrameTxRate(struct rtllib_device *ieee)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	u8 rate;
+
+	if (pHTInfo->iot_action & HT_IOT_ACT_MGNT_USE_CCK_6M)
+		rate = 0x0c;
+	else
+		rate = ieee->basic_rate & 0x7f;
+
+	if (rate == 0) {
+		if (ieee->mode == IEEE_A ||
+		   ieee->mode == IEEE_N_5G ||
+		   (ieee->mode == IEEE_N_24G && !pHTInfo->bCurSuppCCK))
+			rate = 0x0c;
+		else
+			rate = 0x02;
+	}
+
+	return rate;
+}
+
+inline void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee)
+{
+	unsigned long flags;
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct rtllib_hdr_3addr  *header =
+		(struct rtllib_hdr_3addr  *)skb->data;
+
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + 8);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* called with 2nd param 0, no mgmt lock required */
+	rtllib_sta_wakeup(ieee, 0);
+
+	if (le16_to_cpu(header->frame_ctl) == RTLLIB_STYPE_BEACON)
+		tcb_desc->queue_index = BEACON_QUEUE;
+	else
+		tcb_desc->queue_index = MGNT_QUEUE;
+
+	if (ieee->disable_mgnt_queue)
+		tcb_desc->queue_index = HIGH_QUEUE;
+
+	tcb_desc->data_rate = MgntQuery_MgntFrameTxRate(ieee);
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+	if (single) {
+		if (ieee->queue_stop) {
+			enqueue_mgmt(ieee, skb);
+		} else {
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			/* avoid watchdog triggers */
+			ieee->softmac_data_hard_start_xmit(skb, ieee->dev,
+							   ieee->basic_rate);
+		}
+
+		spin_unlock_irqrestore(&ieee->lock, flags);
+	} else {
+		spin_unlock_irqrestore(&ieee->lock, flags);
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags);
+
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* check whether the managed packet queued greater than 5 */
+		if (!ieee->check_nic_enough_desc(ieee->dev,
+						 tcb_desc->queue_index) ||
+		    skb_queue_len(&ieee->skb_waitQ[tcb_desc->queue_index]) ||
+		    ieee->queue_stop) {
+			/* insert the skb packet to the management queue
+			 *
+			 * as for the completion function, it does not need
+			 * to check it any more.
+			 */
+			netdev_info(ieee->dev,
+			       "%s():insert to waitqueue, queue_index:%d!\n",
+			       __func__, tcb_desc->queue_index);
+			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index],
+				       skb);
+		} else {
+			ieee->softmac_hard_start_xmit(skb, ieee->dev);
+		}
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags);
+	}
+}
+
+static inline void
+softmac_ps_mgmt_xmit(struct sk_buff *skb,
+		     struct rtllib_device *ieee)
+{
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct rtllib_hdr_3addr  *header =
+		(struct rtllib_hdr_3addr  *)skb->data;
+	u16 fc, type, stype;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + 8);
+
+	fc = le16_to_cpu(header->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+
+
+	if (stype != RTLLIB_STYPE_PSPOLL)
+		tcb_desc->queue_index = MGNT_QUEUE;
+	else
+		tcb_desc->queue_index = HIGH_QUEUE;
+
+	if (ieee->disable_mgnt_queue)
+		tcb_desc->queue_index = HIGH_QUEUE;
+
+
+	tcb_desc->data_rate = MgntQuery_MgntFrameTxRate(ieee);
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+	if (single) {
+		if (type != RTLLIB_FTYPE_CTL) {
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+		}
+		/* avoid watchdog triggers */
+		ieee->softmac_data_hard_start_xmit(skb, ieee->dev,
+						   ieee->basic_rate);
+
+	} else {
+		if (type != RTLLIB_FTYPE_CTL) {
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+		}
+		ieee->softmac_hard_start_xmit(skb, ieee->dev);
+
+	}
+}
+
+static inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
+{
+	unsigned int len, rate_len;
+	u8 *tag;
+	struct sk_buff *skb;
+	struct rtllib_probe_request *req;
+
+	len = ieee->current_network.ssid_len;
+
+	rate_len = rtllib_MFIE_rate_len(ieee);
+
+	skb = dev_alloc_skb(sizeof(struct rtllib_probe_request) +
+			    2 + len + rate_len + ieee->tx_headroom);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	req = skb_put(skb, sizeof(struct rtllib_probe_request));
+	req->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_PROBE_REQ);
+	req->header.duration_id = 0;
+
+	eth_broadcast_addr(req->header.addr1);
+	ether_addr_copy(req->header.addr2, ieee->dev->dev_addr);
+	eth_broadcast_addr(req->header.addr3);
+
+	tag = skb_put(skb, len + 2 + rate_len);
+
+	*tag++ = MFIE_TYPE_SSID;
+	*tag++ = len;
+	memcpy(tag, ieee->current_network.ssid, len);
+	tag += len;
+
+	rtllib_MFIE_Brate(ieee, &tag);
+	rtllib_MFIE_Grate(ieee, &tag);
+
+	return skb;
+}
+
+static struct sk_buff *rtllib_get_beacon_(struct rtllib_device *ieee);
+
+static void rtllib_send_beacon(struct rtllib_device *ieee)
+{
+	struct sk_buff *skb;
+
+	if (!ieee->ieee_up)
+		return;
+	skb = rtllib_get_beacon_(ieee);
+
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+	}
+
+	if (ieee->beacon_txing && ieee->ieee_up)
+		mod_timer(&ieee->beacon_timer, jiffies +
+			  (msecs_to_jiffies(ieee->current_network.beacon_interval - 5)));
+}
+
+
+static void rtllib_send_beacon_cb(struct timer_list *t)
+{
+	struct rtllib_device *ieee =
+		from_timer(ieee, t, beacon_timer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+	rtllib_send_beacon(ieee);
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+}
+
+/* Enables network monitor mode, all rx packets will be received. */
+void rtllib_EnableNetMonitorMode(struct net_device *dev,
+		bool bInitState)
+{
+	struct rtllib_device *ieee = netdev_priv_rsl(dev);
+
+	netdev_info(dev, "========>Enter Monitor Mode\n");
+
+	ieee->AllowAllDestAddrHandler(dev, true, !bInitState);
+}
+
+
+/* Disables network monitor mode. Only packets destinated to
+ * us will be received.
+ */
+void rtllib_DisableNetMonitorMode(struct net_device *dev,
+		bool bInitState)
+{
+	struct rtllib_device *ieee = netdev_priv_rsl(dev);
+
+	netdev_info(dev, "========>Exit Monitor Mode\n");
+
+	ieee->AllowAllDestAddrHandler(dev, false, !bInitState);
+}
+
+
+/* Enables the specialized promiscuous mode required by Intel.
+ * In this mode, Intel intends to hear traffics from/to other STAs in the
+ * same BSS. Therefore we don't have to disable checking BSSID and we only need
+ * to allow all dest. BUT: if we enable checking BSSID then we can't recv
+ * packets from other STA.
+ */
+void rtllib_EnableIntelPromiscuousMode(struct net_device *dev,
+		bool bInitState)
+{
+	bool bFilterOutNonAssociatedBSSID = false;
+
+	struct rtllib_device *ieee = netdev_priv_rsl(dev);
+
+	netdev_info(dev, "========>Enter Intel Promiscuous Mode\n");
+
+	ieee->AllowAllDestAddrHandler(dev, true, !bInitState);
+	ieee->SetHwRegHandler(dev, HW_VAR_CECHK_BSSID,
+			     (u8 *)&bFilterOutNonAssociatedBSSID);
+
+	ieee->bNetPromiscuousMode = true;
+}
+EXPORT_SYMBOL(rtllib_EnableIntelPromiscuousMode);
+
+
+/* Disables the specialized promiscuous mode required by Intel.
+ * See MgntEnableIntelPromiscuousMode for detail.
+ */
+void rtllib_DisableIntelPromiscuousMode(struct net_device *dev,
+		bool bInitState)
+{
+	bool bFilterOutNonAssociatedBSSID = true;
+
+	struct rtllib_device *ieee = netdev_priv_rsl(dev);
+
+	netdev_info(dev, "========>Exit Intel Promiscuous Mode\n");
+
+	ieee->AllowAllDestAddrHandler(dev, false, !bInitState);
+	ieee->SetHwRegHandler(dev, HW_VAR_CECHK_BSSID,
+			     (u8 *)&bFilterOutNonAssociatedBSSID);
+
+	ieee->bNetPromiscuousMode = false;
+}
+EXPORT_SYMBOL(rtllib_DisableIntelPromiscuousMode);
+
+static void rtllib_send_probe(struct rtllib_device *ieee, u8 is_mesh)
+{
+	struct sk_buff *skb;
+
+	skb = rtllib_probe_req(ieee);
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_probe_rq++;
+	}
+}
+
+
+static void rtllib_send_probe_requests(struct rtllib_device *ieee, u8 is_mesh)
+{
+	if (ieee->active_scan && (ieee->softmac_features &
+	    IEEE_SOFTMAC_PROBERQ)) {
+		rtllib_send_probe(ieee, 0);
+		rtllib_send_probe(ieee, 0);
+	}
+}
+
+static void rtllib_update_active_chan_map(struct rtllib_device *ieee)
+{
+	memcpy(ieee->active_channel_map, GET_DOT11D_INFO(ieee)->channel_map,
+	       MAX_CHANNEL_NUMBER+1);
+}
+
+/* this performs syncro scan blocking the caller until all channels
+ * in the allowed channel map has been checked.
+ */
+static void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
+{
+	union iwreq_data wrqu;
+	short ch = 0;
+
+	rtllib_update_active_chan_map(ieee);
+
+	ieee->be_scan_inprogress = true;
+
+	mutex_lock(&ieee->scan_mutex);
+
+	while (1) {
+		do {
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				goto out; /* scan completed */
+		} while (!ieee->active_channel_map[ch]);
+
+		/* this function can be called in two situations
+		 * 1- We have switched to ad-hoc mode and we are
+		 *    performing a complete syncro scan before conclude
+		 *    there are no interesting cell and to create a
+		 *    new one. In this case the link state is
+		 *    RTLLIB_NOLINK until we found an interesting cell.
+		 *    If so the ieee8021_new_net, called by the RX path
+		 *    will set the state to RTLLIB_LINKED, so we stop
+		 *    scanning
+		 * 2- We are linked and the root uses run iwlist scan.
+		 *    So we switch to RTLLIB_LINKED_SCANNING to remember
+		 *    that we are still logically linked (not interested in
+		 *    new network events, despite for updating the net list,
+		 *    but we are temporarly 'unlinked' as the driver shall
+		 *    not filter RX frames and the channel is changing.
+		 * So the only situation in which are interested is to check
+		 * if the state become LINKED because of the #1 situation
+		 */
+
+		if (ieee->state == RTLLIB_LINKED)
+			goto out;
+		if (ieee->sync_scan_hurryup) {
+			netdev_info(ieee->dev,
+				    "============>sync_scan_hurryup out\n");
+			goto out;
+		}
+
+		ieee->set_chan(ieee->dev, ch);
+		if (ieee->active_channel_map[ch] == 1)
+			rtllib_send_probe_requests(ieee, 0);
+
+		/* this prevent excessive time wait when we
+		 * need to wait for a syncro scan to end..
+		 */
+		msleep_interruptible_rsl(RTLLIB_SOFTMAC_SCAN_TIME);
+	}
+out:
+	ieee->actscanning = false;
+	ieee->sync_scan_hurryup = 0;
+
+	if (ieee->state >= RTLLIB_LINKED) {
+		if (IS_DOT11D_ENABLE(ieee))
+			dot11d_scan_complete(ieee);
+	}
+	mutex_unlock(&ieee->scan_mutex);
+
+	ieee->be_scan_inprogress = false;
+
+	memset(&wrqu, 0, sizeof(wrqu));
+	wireless_send_event(ieee->dev, SIOCGIWSCAN, &wrqu, NULL);
+}
+
+static void rtllib_softmac_scan_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, softmac_scan_wq);
+	u8 last_channel = ieee->current_network.channel;
+
+	rtllib_update_active_chan_map(ieee);
+
+	if (!ieee->ieee_up)
+		return;
+	if (rtllib_act_scanning(ieee, true))
+		return;
+
+	mutex_lock(&ieee->scan_mutex);
+
+	if (ieee->rf_power_state == rf_off) {
+		netdev_info(ieee->dev,
+			    "======>%s():rf state is rf_off, return\n",
+			    __func__);
+		goto out1;
+	}
+
+	do {
+		ieee->current_network.channel =
+			(ieee->current_network.channel + 1) %
+			MAX_CHANNEL_NUMBER;
+		if (ieee->scan_watch_dog++ > MAX_CHANNEL_NUMBER) {
+			if (!ieee->active_channel_map[ieee->current_network.channel])
+				ieee->current_network.channel = 6;
+			goto out; /* no good chans */
+		}
+	} while (!ieee->active_channel_map[ieee->current_network.channel]);
+
+	if (ieee->scanning_continue == 0)
+		goto out;
+
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+	if (ieee->active_channel_map[ieee->current_network.channel] == 1)
+		rtllib_send_probe_requests(ieee, 0);
+
+	schedule_delayed_work(&ieee->softmac_scan_wq,
+			      msecs_to_jiffies(RTLLIB_SOFTMAC_SCAN_TIME));
+
+	mutex_unlock(&ieee->scan_mutex);
+	return;
+
+out:
+	if (IS_DOT11D_ENABLE(ieee))
+		dot11d_scan_complete(ieee);
+	ieee->current_network.channel = last_channel;
+
+out1:
+	ieee->actscanning = false;
+	ieee->scan_watch_dog = 0;
+	ieee->scanning_continue = 0;
+	mutex_unlock(&ieee->scan_mutex);
+}
+
+
+
+static void rtllib_beacons_start(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+
+	ieee->beacon_txing = 1;
+	rtllib_send_beacon(ieee);
+
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+}
+
+static void rtllib_beacons_stop(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+
+	ieee->beacon_txing = 0;
+
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+	del_timer_sync(&ieee->beacon_timer);
+
+}
+
+
+void rtllib_stop_send_beacons(struct rtllib_device *ieee)
+{
+	if (ieee->stop_send_beacons)
+		ieee->stop_send_beacons(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		rtllib_beacons_stop(ieee);
+}
+EXPORT_SYMBOL(rtllib_stop_send_beacons);
+
+
+void rtllib_start_send_beacons(struct rtllib_device *ieee)
+{
+	if (ieee->start_send_beacons)
+		ieee->start_send_beacons(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		rtllib_beacons_start(ieee);
+}
+EXPORT_SYMBOL(rtllib_start_send_beacons);
+
+
+static void rtllib_softmac_stop_scan(struct rtllib_device *ieee)
+{
+	mutex_lock(&ieee->scan_mutex);
+	ieee->scan_watch_dog = 0;
+	if (ieee->scanning_continue == 1) {
+		ieee->scanning_continue = 0;
+		ieee->actscanning = false;
+
+		cancel_delayed_work_sync(&ieee->softmac_scan_wq);
+	}
+
+	mutex_unlock(&ieee->scan_mutex);
+}
+
+void rtllib_stop_scan(struct rtllib_device *ieee)
+{
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		rtllib_softmac_stop_scan(ieee);
+	} else {
+		if (ieee->rtllib_stop_hw_scan)
+			ieee->rtllib_stop_hw_scan(ieee->dev);
+	}
+}
+EXPORT_SYMBOL(rtllib_stop_scan);
+
+void rtllib_stop_scan_syncro(struct rtllib_device *ieee)
+{
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		ieee->sync_scan_hurryup = 1;
+	} else {
+		if (ieee->rtllib_stop_hw_scan)
+			ieee->rtllib_stop_hw_scan(ieee->dev);
+	}
+}
+EXPORT_SYMBOL(rtllib_stop_scan_syncro);
+
+bool rtllib_act_scanning(struct rtllib_device *ieee, bool sync_scan)
+{
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		if (sync_scan)
+			return ieee->be_scan_inprogress;
+		else
+			return ieee->actscanning || ieee->be_scan_inprogress;
+	} else {
+		return test_bit(STATUS_SCANNING, &ieee->status);
+	}
+}
+EXPORT_SYMBOL(rtllib_act_scanning);
+
+/* called with ieee->lock held */
+static void rtllib_start_scan(struct rtllib_device *ieee)
+{
+	if (ieee->rtllib_ips_leave_wq != NULL)
+		ieee->rtllib_ips_leave_wq(ieee->dev);
+
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (IS_COUNTRY_IE_VALID(ieee))
+			RESET_CIE_WATCHDOG(ieee);
+	}
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		if (ieee->scanning_continue == 0) {
+			ieee->actscanning = true;
+			ieee->scanning_continue = 1;
+			schedule_delayed_work(&ieee->softmac_scan_wq, 0);
+		}
+	} else {
+		if (ieee->rtllib_start_hw_scan)
+			ieee->rtllib_start_hw_scan(ieee->dev);
+	}
+}
+
+/* called with wx_mutex held */
+void rtllib_start_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
+{
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (IS_COUNTRY_IE_VALID(ieee))
+			RESET_CIE_WATCHDOG(ieee);
+	}
+	ieee->sync_scan_hurryup = 0;
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		rtllib_softmac_scan_syncro(ieee, is_mesh);
+	} else {
+		if (ieee->rtllib_start_hw_scan)
+			ieee->rtllib_start_hw_scan(ieee->dev);
+	}
+}
+EXPORT_SYMBOL(rtllib_start_scan_syncro);
+
+static inline struct sk_buff *
+rtllib_authentication_req(struct rtllib_network *beacon,
+			  struct rtllib_device *ieee,
+			  int challengelen, u8 *daddr)
+{
+	struct sk_buff *skb;
+	struct rtllib_authentication *auth;
+	int  len;
+
+	len = sizeof(struct rtllib_authentication) + challengelen +
+		     ieee->tx_headroom + 4;
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	auth = skb_put(skb, sizeof(struct rtllib_authentication));
+
+	auth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_AUTH);
+	if (challengelen)
+		auth->header.frame_ctl |= cpu_to_le16(RTLLIB_FCTL_WEP);
+
+	auth->header.duration_id = cpu_to_le16(0x013a);
+	ether_addr_copy(auth->header.addr1, beacon->bssid);
+	ether_addr_copy(auth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr3, beacon->bssid);
+	if (ieee->auth_mode == 0)
+		auth->algorithm = WLAN_AUTH_OPEN;
+	else if (ieee->auth_mode == 1)
+		auth->algorithm = cpu_to_le16(WLAN_AUTH_SHARED_KEY);
+	else if (ieee->auth_mode == 2)
+		auth->algorithm = WLAN_AUTH_OPEN;
+	auth->transaction = cpu_to_le16(ieee->associate_seq);
+	ieee->associate_seq++;
+
+	auth->status = cpu_to_le16(WLAN_STATUS_SUCCESS);
+
+	return skb;
+}
+
+static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee,
+					 const u8 *dest)
+{
+	u8 *tag;
+	int beacon_size;
+	struct rtllib_probe_response *beacon_buf;
+	struct sk_buff *skb = NULL;
+	int encrypt;
+	int atim_len, erp_len;
+	struct lib80211_crypt_data *crypt;
+
+	char *ssid = ieee->current_network.ssid;
+	int ssid_len = ieee->current_network.ssid_len;
+	int rate_len = ieee->current_network.rates_len+2;
+	int rate_ex_len = ieee->current_network.rates_ex_len;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	u8 erpinfo_content = 0;
+
+	u8 *tmp_ht_cap_buf = NULL;
+	u8 tmp_ht_cap_len = 0;
+	u8 *tmp_ht_info_buf = NULL;
+	u8 tmp_ht_info_len = 0;
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	u8 *tmp_generic_ie_buf = NULL;
+	u8 tmp_generic_ie_len = 0;
+
+	if (rate_ex_len > 0)
+		rate_ex_len += 2;
+
+	if (ieee->current_network.capability & WLAN_CAPABILITY_IBSS)
+		atim_len = 4;
+	else
+		atim_len = 0;
+
+	if ((ieee->current_network.mode == IEEE_G) ||
+	   (ieee->current_network.mode == IEEE_N_24G &&
+	   ieee->pHTInfo->bCurSuppCCK)) {
+		erp_len = 3;
+		erpinfo_content = 0;
+		if (ieee->current_network.buseprotection)
+			erpinfo_content |= ERP_UseProtection;
+	} else
+		erp_len = 0;
+
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
+		((strcmp(crypt->ops->name, "R-WEP") == 0 || wpa_ie_len));
+	if (ieee->pHTInfo->bCurrentHTSupport) {
+		tmp_ht_cap_buf = (u8 *)&(ieee->pHTInfo->SelfHTCap);
+		tmp_ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
+		tmp_ht_info_buf = (u8 *)&(ieee->pHTInfo->SelfHTInfo);
+		tmp_ht_info_len = sizeof(ieee->pHTInfo->SelfHTInfo);
+		HTConstructCapabilityElement(ieee, tmp_ht_cap_buf,
+					     &tmp_ht_cap_len, encrypt, false);
+		HTConstructInfoElement(ieee, tmp_ht_info_buf, &tmp_ht_info_len,
+				       encrypt);
+
+		if (pHTInfo->reg_rt2rt_aggregation) {
+			tmp_generic_ie_buf = ieee->pHTInfo->sz_rt2rt_agg_buf;
+			tmp_generic_ie_len =
+				 sizeof(ieee->pHTInfo->sz_rt2rt_agg_buf);
+			HTConstructRT2RTAggElement(ieee, tmp_generic_ie_buf,
+						   &tmp_generic_ie_len);
+		}
+	}
+
+	beacon_size = sizeof(struct rtllib_probe_response)+2+
+		ssid_len + 3 + rate_len + rate_ex_len + atim_len + erp_len
+		+ wpa_ie_len + ieee->tx_headroom;
+	skb = dev_alloc_skb(beacon_size);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	beacon_buf = skb_put(skb, (beacon_size - ieee->tx_headroom));
+	ether_addr_copy(beacon_buf->header.addr1, dest);
+	ether_addr_copy(beacon_buf->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(beacon_buf->header.addr3, ieee->current_network.bssid);
+
+	beacon_buf->header.duration_id = 0;
+	beacon_buf->beacon_interval =
+		cpu_to_le16(ieee->current_network.beacon_interval);
+	beacon_buf->capability =
+		cpu_to_le16(ieee->current_network.capability &
+		WLAN_CAPABILITY_IBSS);
+	beacon_buf->capability |=
+		cpu_to_le16(ieee->current_network.capability &
+		WLAN_CAPABILITY_SHORT_PREAMBLE);
+
+	if (ieee->short_slot && (ieee->current_network.capability &
+	    WLAN_CAPABILITY_SHORT_SLOT_TIME))
+		beacon_buf->capability |=
+			cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME);
+
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+	if (encrypt)
+		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+
+	beacon_buf->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_PROBE_RESP);
+	beacon_buf->info_element[0].id = MFIE_TYPE_SSID;
+	beacon_buf->info_element[0].len = ssid_len;
+
+	tag = (u8 *)beacon_buf->info_element[0].data;
+
+	memcpy(tag, ssid, ssid_len);
+
+	tag += ssid_len;
+
+	*(tag++) = MFIE_TYPE_RATES;
+	*(tag++) = rate_len-2;
+	memcpy(tag, ieee->current_network.rates, rate_len-2);
+	tag += rate_len-2;
+
+	*(tag++) = MFIE_TYPE_DS_SET;
+	*(tag++) = 1;
+	*(tag++) = ieee->current_network.channel;
+
+	if (atim_len) {
+		u16 val16;
+		*(tag++) = MFIE_TYPE_IBSS_SET;
+		*(tag++) = 2;
+		val16 = ieee->current_network.atim_window;
+		memcpy((u8 *)tag, (u8 *)&val16, 2);
+		tag += 2;
+	}
+
+	if (erp_len) {
+		*(tag++) = MFIE_TYPE_ERP;
+		*(tag++) = 1;
+		*(tag++) = erpinfo_content;
+	}
+	if (rate_ex_len) {
+		*(tag++) = MFIE_TYPE_RATES_EX;
+		*(tag++) = rate_ex_len-2;
+		memcpy(tag, ieee->current_network.rates_ex, rate_ex_len-2);
+		tag += rate_ex_len-2;
+	}
+
+	if (wpa_ie_len) {
+		if (ieee->iw_mode == IW_MODE_ADHOC)
+			memcpy(&ieee->wpa_ie[14], &ieee->wpa_ie[8], 4);
+		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+		tag += ieee->wpa_ie_len;
+	}
+	return skb;
+}
+
+static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
+{
+	struct sk_buff *skb;
+	u8 *tag;
+
+	struct lib80211_crypt_data *crypt;
+	struct rtllib_assoc_response_frame *assoc;
+	short encrypt;
+
+	unsigned int rate_len = rtllib_MFIE_rate_len(ieee);
+	int len = sizeof(struct rtllib_assoc_response_frame) + rate_len +
+		  ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	assoc = skb_put(skb, sizeof(struct rtllib_assoc_response_frame));
+
+	assoc->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_RESP);
+	ether_addr_copy(assoc->header.addr1, dest);
+	ether_addr_copy(assoc->header.addr3, ieee->dev->dev_addr);
+	ether_addr_copy(assoc->header.addr2, ieee->dev->dev_addr);
+	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
+		WLAN_CAPABILITY_ESS : WLAN_CAPABILITY_IBSS);
+
+
+	if (ieee->short_slot)
+		assoc->capability |=
+				 cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME);
+
+	if (ieee->host_encrypt)
+		crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+	else
+		crypt = NULL;
+
+	encrypt = (crypt && crypt->ops);
+
+	if (encrypt)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	assoc->status = 0;
+	assoc->aid = cpu_to_le16(ieee->assoc_id);
+	if (ieee->assoc_id == 0x2007)
+		ieee->assoc_id = 0;
+	else
+		ieee->assoc_id++;
+
+	tag = skb_put(skb, rate_len);
+	rtllib_MFIE_Brate(ieee, &tag);
+	rtllib_MFIE_Grate(ieee, &tag);
+
+	return skb;
+}
+
+static struct sk_buff *rtllib_auth_resp(struct rtllib_device *ieee, int status,
+				 u8 *dest)
+{
+	struct sk_buff *skb = NULL;
+	struct rtllib_authentication *auth;
+	int len = ieee->tx_headroom + sizeof(struct rtllib_authentication) + 1;
+
+	skb = dev_alloc_skb(len);
+	if (!skb)
+		return NULL;
+
+	skb->len = sizeof(struct rtllib_authentication);
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	auth = skb_put(skb, sizeof(struct rtllib_authentication));
+
+	auth->status = cpu_to_le16(status);
+	auth->transaction = cpu_to_le16(2);
+	auth->algorithm = cpu_to_le16(WLAN_AUTH_OPEN);
+
+	ether_addr_copy(auth->header.addr3, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr1, dest);
+	auth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_AUTH);
+	return skb;
+
+
+}
+
+static struct sk_buff *rtllib_null_func(struct rtllib_device *ieee, short pwr)
+{
+	struct sk_buff *skb;
+	struct rtllib_hdr_3addr *hdr;
+
+	skb = dev_alloc_skb(sizeof(struct rtllib_hdr_3addr)+ieee->tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	hdr = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
+
+	ether_addr_copy(hdr->addr1, ieee->current_network.bssid);
+	ether_addr_copy(hdr->addr2, ieee->dev->dev_addr);
+	ether_addr_copy(hdr->addr3, ieee->current_network.bssid);
+
+	hdr->frame_ctl = cpu_to_le16(RTLLIB_FTYPE_DATA |
+		RTLLIB_STYPE_NULLFUNC | RTLLIB_FCTL_TODS |
+		(pwr ? RTLLIB_FCTL_PM : 0));
+
+	return skb;
+
+
+}
+
+static struct sk_buff *rtllib_pspoll_func(struct rtllib_device *ieee)
+{
+	struct sk_buff *skb;
+	struct rtllib_pspoll_hdr *hdr;
+
+	skb = dev_alloc_skb(sizeof(struct rtllib_pspoll_hdr)+ieee->tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	hdr = skb_put(skb, sizeof(struct rtllib_pspoll_hdr));
+
+	ether_addr_copy(hdr->bssid, ieee->current_network.bssid);
+	ether_addr_copy(hdr->ta, ieee->dev->dev_addr);
+
+	hdr->aid = cpu_to_le16(ieee->assoc_id | 0xc000);
+	hdr->frame_ctl = cpu_to_le16(RTLLIB_FTYPE_CTL | RTLLIB_STYPE_PSPOLL |
+			 RTLLIB_FCTL_PM);
+
+	return skb;
+
+}
+
+static void rtllib_resp_to_assoc_rq(struct rtllib_device *ieee, u8 *dest)
+{
+	struct sk_buff *buf = rtllib_assoc_resp(ieee, dest);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+
+static void rtllib_resp_to_auth(struct rtllib_device *ieee, int s, u8 *dest)
+{
+	struct sk_buff *buf = rtllib_auth_resp(ieee, s, dest);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+
+static void rtllib_resp_to_probe(struct rtllib_device *ieee, u8 *dest)
+{
+	struct sk_buff *buf = rtllib_probe_resp(ieee, dest);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+
+static inline int SecIsInPMKIDList(struct rtllib_device *ieee, u8 *bssid)
+{
+	int i = 0;
+
+	do {
+		if ((ieee->PMKIDList[i].bUsed) &&
+		   (memcmp(ieee->PMKIDList[i].Bssid, bssid, ETH_ALEN) == 0))
+			break;
+		i++;
+	} while (i < NUM_PMKID_CACHE);
+
+	if (i == NUM_PMKID_CACHE)
+		i = -1;
+	return i;
+}
+
+static inline struct sk_buff *
+rtllib_association_req(struct rtllib_network *beacon,
+		       struct rtllib_device *ieee)
+{
+	struct sk_buff *skb;
+	struct rtllib_assoc_request_frame *hdr;
+	u8 *tag, *ies;
+	int i;
+	u8 *ht_cap_buf = NULL;
+	u8 ht_cap_len = 0;
+	u8 *realtek_ie_buf = NULL;
+	u8 realtek_ie_len = 0;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	int wps_ie_len = ieee->wps_ie_len;
+	unsigned int ckip_ie_len = 0;
+	unsigned int ccxrm_ie_len = 0;
+	unsigned int cxvernum_ie_len = 0;
+	struct lib80211_crypt_data *crypt;
+	int encrypt;
+	int	PMKCacheIdx;
+
+	unsigned int rate_len = (beacon->rates_len ?
+				(beacon->rates_len + 2) : 0) +
+				(beacon->rates_ex_len ? (beacon->rates_ex_len) +
+				2 : 0);
+
+	unsigned int wmm_info_len = beacon->qos_data.supported ? 9 : 0;
+	unsigned int turbo_info_len = beacon->Turbo_Enable ? 9 : 0;
+
+	int len = 0;
+
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+	if (crypt != NULL)
+		encrypt = ieee->host_encrypt && crypt && crypt->ops &&
+			  ((strcmp(crypt->ops->name, "R-WEP") == 0 ||
+			  wpa_ie_len));
+	else
+		encrypt = 0;
+
+	if ((ieee->rtllib_ap_sec_type &&
+	    (ieee->rtllib_ap_sec_type(ieee) & SEC_ALG_TKIP)) ||
+	    ieee->bForcedBgMode) {
+		ieee->pHTInfo->bEnableHT = 0;
+		ieee->mode = WIRELESS_MODE_G;
+	}
+
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		ht_cap_buf = (u8 *)&(ieee->pHTInfo->SelfHTCap);
+		ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
+		HTConstructCapabilityElement(ieee, ht_cap_buf, &ht_cap_len,
+					     encrypt, true);
+		if (ieee->pHTInfo->current_rt2rt_aggregation) {
+			realtek_ie_buf = ieee->pHTInfo->sz_rt2rt_agg_buf;
+			realtek_ie_len =
+				 sizeof(ieee->pHTInfo->sz_rt2rt_agg_buf);
+			HTConstructRT2RTAggElement(ieee, realtek_ie_buf,
+						   &realtek_ie_len);
+		}
+	}
+
+	if (beacon->bCkipSupported)
+		ckip_ie_len = 30+2;
+	if (beacon->bCcxRmEnable)
+		ccxrm_ie_len = 6+2;
+	if (beacon->BssCcxVerNumber >= 2)
+		cxvernum_ie_len = 5+2;
+
+	PMKCacheIdx = SecIsInPMKIDList(ieee, ieee->current_network.bssid);
+	if (PMKCacheIdx >= 0) {
+		wpa_ie_len += 18;
+		netdev_info(ieee->dev, "[PMK cache]: WPA2 IE length: %x\n",
+			    wpa_ie_len);
+	}
+	len = sizeof(struct rtllib_assoc_request_frame) + 2
+		+ beacon->ssid_len
+		+ rate_len
+		+ wpa_ie_len
+		+ wps_ie_len
+		+ wmm_info_len
+		+ turbo_info_len
+		+ ht_cap_len
+		+ realtek_ie_len
+		+ ckip_ie_len
+		+ ccxrm_ie_len
+		+ cxvernum_ie_len
+		+ ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	hdr = skb_put(skb, sizeof(struct rtllib_assoc_request_frame) + 2);
+
+
+	hdr->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_REQ);
+	hdr->header.duration_id = cpu_to_le16(37);
+	ether_addr_copy(hdr->header.addr1, beacon->bssid);
+	ether_addr_copy(hdr->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(hdr->header.addr3, beacon->bssid);
+
+	ether_addr_copy(ieee->ap_mac_addr, beacon->bssid);
+
+	hdr->capability = cpu_to_le16(WLAN_CAPABILITY_ESS);
+	if (beacon->capability & WLAN_CAPABILITY_PRIVACY)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	if (beacon->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
+
+	if (ieee->short_slot &&
+	   (beacon->capability&WLAN_CAPABILITY_SHORT_SLOT_TIME))
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME);
+
+
+	hdr->listen_interval = cpu_to_le16(beacon->listen_interval);
+
+	hdr->info_element[0].id = MFIE_TYPE_SSID;
+
+	hdr->info_element[0].len = beacon->ssid_len;
+	skb_put_data(skb, beacon->ssid, beacon->ssid_len);
+
+	tag = skb_put(skb, rate_len);
+
+	if (beacon->rates_len) {
+		*tag++ = MFIE_TYPE_RATES;
+		*tag++ = beacon->rates_len;
+		for (i = 0; i < beacon->rates_len; i++)
+			*tag++ = beacon->rates[i];
+	}
+
+	if (beacon->rates_ex_len) {
+		*tag++ = MFIE_TYPE_RATES_EX;
+		*tag++ = beacon->rates_ex_len;
+		for (i = 0; i < beacon->rates_ex_len; i++)
+			*tag++ = beacon->rates_ex[i];
+	}
+
+	if (beacon->bCkipSupported) {
+		static const u8 AironetIeOui[] = {0x00, 0x01, 0x66};
+		u8	CcxAironetBuf[30];
+		struct octet_string osCcxAironetIE;
+
+		memset(CcxAironetBuf, 0, 30);
+		osCcxAironetIE.Octet = CcxAironetBuf;
+		osCcxAironetIE.Length = sizeof(CcxAironetBuf);
+		memcpy(osCcxAironetIE.Octet, AironetIeOui,
+		       sizeof(AironetIeOui));
+
+		osCcxAironetIE.Octet[IE_CISCO_FLAG_POSITION] |=
+					 (SUPPORT_CKIP_PK|SUPPORT_CKIP_MIC);
+		tag = skb_put(skb, ckip_ie_len);
+		*tag++ = MFIE_TYPE_AIRONET;
+		*tag++ = osCcxAironetIE.Length;
+		memcpy(tag, osCcxAironetIE.Octet, osCcxAironetIE.Length);
+		tag += osCcxAironetIE.Length;
+	}
+
+	if (beacon->bCcxRmEnable) {
+		static const u8 CcxRmCapBuf[] = {0x00, 0x40, 0x96, 0x01, 0x01,
+			0x00};
+		struct octet_string osCcxRmCap;
+
+		osCcxRmCap.Octet = (u8 *)CcxRmCapBuf;
+		osCcxRmCap.Length = sizeof(CcxRmCapBuf);
+		tag = skb_put(skb, ccxrm_ie_len);
+		*tag++ = MFIE_TYPE_GENERIC;
+		*tag++ = osCcxRmCap.Length;
+		memcpy(tag, osCcxRmCap.Octet, osCcxRmCap.Length);
+		tag += osCcxRmCap.Length;
+	}
+
+	if (beacon->BssCcxVerNumber >= 2) {
+		u8 CcxVerNumBuf[] = {0x00, 0x40, 0x96, 0x03, 0x00};
+		struct octet_string osCcxVerNum;
+
+		CcxVerNumBuf[4] = beacon->BssCcxVerNumber;
+		osCcxVerNum.Octet = CcxVerNumBuf;
+		osCcxVerNum.Length = sizeof(CcxVerNumBuf);
+		tag = skb_put(skb, cxvernum_ie_len);
+		*tag++ = MFIE_TYPE_GENERIC;
+		*tag++ = osCcxVerNum.Length;
+		memcpy(tag, osCcxVerNum.Octet, osCcxVerNum.Length);
+		tag += osCcxVerNum.Length;
+	}
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		if (ieee->pHTInfo->ePeerHTSpecVer != HT_SPEC_VER_EWC) {
+			tag = skb_put(skb, ht_cap_len);
+			*tag++ = MFIE_TYPE_HT_CAP;
+			*tag++ = ht_cap_len - 2;
+			memcpy(tag, ht_cap_buf, ht_cap_len - 2);
+			tag += ht_cap_len - 2;
+		}
+	}
+
+	if (wpa_ie_len) {
+		skb_put_data(skb, ieee->wpa_ie, ieee->wpa_ie_len);
+
+		if (PMKCacheIdx >= 0) {
+			tag = skb_put(skb, 18);
+			*tag = 1;
+			*(tag + 1) = 0;
+			memcpy((tag + 2), &ieee->PMKIDList[PMKCacheIdx].PMKID,
+			       16);
+		}
+	}
+	if (wmm_info_len) {
+		tag = skb_put(skb, wmm_info_len);
+		rtllib_WMM_Info(ieee, &tag);
+	}
+
+	if (wps_ie_len && ieee->wps_ie)
+		skb_put_data(skb, ieee->wps_ie, wps_ie_len);
+
+	if (turbo_info_len) {
+		tag = skb_put(skb, turbo_info_len);
+		rtllib_TURBO_Info(ieee, &tag);
+	}
+
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		if (ieee->pHTInfo->ePeerHTSpecVer == HT_SPEC_VER_EWC) {
+			tag = skb_put(skb, ht_cap_len);
+			*tag++ = MFIE_TYPE_GENERIC;
+			*tag++ = ht_cap_len - 2;
+			memcpy(tag, ht_cap_buf, ht_cap_len - 2);
+			tag += ht_cap_len - 2;
+		}
+
+		if (ieee->pHTInfo->current_rt2rt_aggregation) {
+			tag = skb_put(skb, realtek_ie_len);
+			*tag++ = MFIE_TYPE_GENERIC;
+			*tag++ = realtek_ie_len - 2;
+			memcpy(tag, realtek_ie_buf, realtek_ie_len - 2);
+		}
+	}
+
+	kfree(ieee->assocreq_ies);
+	ieee->assocreq_ies = NULL;
+	ies = &(hdr->info_element[0].id);
+	ieee->assocreq_ies_len = (skb->data + skb->len) - ies;
+	ieee->assocreq_ies = kmemdup(ies, ieee->assocreq_ies_len, GFP_ATOMIC);
+	if (!ieee->assocreq_ies)
+		ieee->assocreq_ies_len = 0;
+
+	return skb;
+}
+
+static void rtllib_associate_abort(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	ieee->associate_seq++;
+
+	/* don't scan, and avoid to have the RX path possibily
+	 * try again to associate. Even do not react to AUTH or
+	 * ASSOC response. Just wait for the retry wq to be scheduled.
+	 * Here we will check if there are good nets to associate
+	 * with, so we retry or just get back to NO_LINK and scanning
+	 */
+	if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING) {
+		netdev_dbg(ieee->dev, "Authentication failed\n");
+		ieee->softmac_stats.no_auth_rs++;
+	} else {
+		netdev_dbg(ieee->dev, "Association failed\n");
+		ieee->softmac_stats.no_ass_rs++;
+	}
+
+	ieee->state = RTLLIB_ASSOCIATING_RETRY;
+
+	schedule_delayed_work(&ieee->associate_retry_wq,
+			      RTLLIB_SOFTMAC_ASSOC_RETRY_TIME);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+static void rtllib_associate_abort_cb(struct timer_list *t)
+{
+	struct rtllib_device *dev = from_timer(dev, t, associate_timer);
+
+	rtllib_associate_abort(dev);
+}
+
+static void rtllib_associate_step1(struct rtllib_device *ieee, u8 *daddr)
+{
+	struct rtllib_network *beacon = &ieee->current_network;
+	struct sk_buff *skb;
+
+	netdev_dbg(ieee->dev, "Stopping scan\n");
+
+	ieee->softmac_stats.tx_auth_rq++;
+
+	skb = rtllib_authentication_req(beacon, ieee, 0, daddr);
+
+	if (!skb)
+		rtllib_associate_abort(ieee);
+	else {
+		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATING;
+		netdev_dbg(ieee->dev, "Sending authentication request\n");
+		softmac_mgmt_xmit(skb, ieee);
+		if (!timer_pending(&ieee->associate_timer)) {
+			ieee->associate_timer.expires = jiffies + (HZ / 2);
+			add_timer(&ieee->associate_timer);
+		}
+	}
+}
+
+static void rtllib_auth_challenge(struct rtllib_device *ieee, u8 *challenge,
+				  int chlen)
+{
+	u8 *c;
+	struct sk_buff *skb;
+	struct rtllib_network *beacon = &ieee->current_network;
+
+	ieee->associate_seq++;
+	ieee->softmac_stats.tx_auth_rq++;
+
+	skb = rtllib_authentication_req(beacon, ieee, chlen + 2, beacon->bssid);
+
+	if (!skb)
+		rtllib_associate_abort(ieee);
+	else {
+		c = skb_put(skb, chlen+2);
+		*(c++) = MFIE_TYPE_CHALLENGE;
+		*(c++) = chlen;
+		memcpy(c, challenge, chlen);
+
+		netdev_dbg(ieee->dev,
+			   "Sending authentication challenge response\n");
+
+		rtllib_encrypt_fragment(ieee, skb,
+					sizeof(struct rtllib_hdr_3addr));
+
+		softmac_mgmt_xmit(skb, ieee);
+		mod_timer(&ieee->associate_timer, jiffies + (HZ/2));
+	}
+	kfree(challenge);
+}
+
+static void rtllib_associate_step2(struct rtllib_device *ieee)
+{
+	struct sk_buff *skb;
+	struct rtllib_network *beacon = &ieee->current_network;
+
+	del_timer_sync(&ieee->associate_timer);
+
+	netdev_dbg(ieee->dev, "Sending association request\n");
+
+	ieee->softmac_stats.tx_ass_rq++;
+	skb = rtllib_association_req(beacon, ieee);
+	if (!skb)
+		rtllib_associate_abort(ieee);
+	else {
+		softmac_mgmt_xmit(skb, ieee);
+		mod_timer(&ieee->associate_timer, jiffies + (HZ/2));
+	}
+}
+
+static void rtllib_associate_complete_wq(void *data)
+{
+	struct rtllib_device *ieee = (struct rtllib_device *)
+				     container_of_work_rsl(data,
+				     struct rtllib_device,
+				     associate_complete_wq);
+	struct rt_pwr_save_ctrl *pPSC = &(ieee->PowerSaveControl);
+
+	netdev_info(ieee->dev, "Associated successfully with %pM\n",
+		    ieee->current_network.bssid);
+	if (!ieee->is_silent_reset) {
+		netdev_info(ieee->dev, "normal associate\n");
+		notify_wx_assoc_event(ieee);
+	}
+
+	netif_carrier_on(ieee->dev);
+	ieee->is_roaming = false;
+	if (rtllib_is_54g(&ieee->current_network) &&
+	   (ieee->modulation & RTLLIB_OFDM_MODULATION)) {
+		ieee->rate = 108;
+		netdev_info(ieee->dev, "Using G rates:%d\n", ieee->rate);
+	} else {
+		ieee->rate = 22;
+		ieee->SetWirelessMode(ieee->dev, IEEE_B);
+		netdev_info(ieee->dev, "Using B rates:%d\n", ieee->rate);
+	}
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		netdev_info(ieee->dev, "Successfully associated, ht enabled\n");
+		HTOnAssocRsp(ieee);
+	} else {
+		netdev_info(ieee->dev,
+			    "Successfully associated, ht not enabled(%d, %d)\n",
+			    ieee->pHTInfo->bCurrentHTSupport,
+			    ieee->pHTInfo->bEnableHT);
+		memset(ieee->dot11HTOperationalRateSet, 0, 16);
+	}
+	ieee->LinkDetectInfo.SlotNum = 2 * (1 +
+				       ieee->current_network.beacon_interval /
+				       500);
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod == 0 ||
+	    ieee->LinkDetectInfo.NumRecvDataInPeriod == 0) {
+		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
+		ieee->LinkDetectInfo.NumRecvDataInPeriod = 1;
+	}
+	pPSC->LpsIdleCount = 0;
+	ieee->link_change(ieee->dev);
+
+	if (ieee->is_silent_reset) {
+		netdev_info(ieee->dev, "silent reset associate\n");
+		ieee->is_silent_reset = false;
+	}
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+}
+
+static void rtllib_sta_send_associnfo(struct rtllib_device *ieee)
+{
+}
+
+static void rtllib_associate_complete(struct rtllib_device *ieee)
+{
+	del_timer_sync(&ieee->associate_timer);
+
+	ieee->state = RTLLIB_LINKED;
+	rtllib_sta_send_associnfo(ieee);
+
+	schedule_work(&ieee->associate_complete_wq);
+}
+
+static void rtllib_associate_procedure_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device,
+				     associate_procedure_wq);
+	rtllib_stop_scan_syncro(ieee);
+	if (ieee->rtllib_ips_leave != NULL)
+		ieee->rtllib_ips_leave(ieee->dev);
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	rtllib_stop_scan(ieee);
+	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+	if (ieee->rf_power_state == rf_off) {
+		if (ieee->rtllib_ips_leave_wq != NULL)
+			ieee->rtllib_ips_leave_wq(ieee->dev);
+		mutex_unlock(&ieee->wx_mutex);
+		return;
+	}
+	ieee->associate_seq = 1;
+
+	rtllib_associate_step1(ieee, ieee->current_network.bssid);
+
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
+				   struct rtllib_network *net)
+{
+	u8 tmp_ssid[IW_ESSID_MAX_SIZE + 1];
+	int tmp_ssid_len = 0;
+
+	short apset, ssidset, ssidbroad, apmatch, ssidmatch;
+
+	/* we are interested in new only if we are not associated
+	 * and we are not associating / authenticating
+	 */
+	if (ieee->state != RTLLIB_NOLINK)
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_INFRA) && !(net->capability &
+	    WLAN_CAPABILITY_ESS))
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_ADHOC) && !(net->capability &
+	     WLAN_CAPABILITY_IBSS))
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_ADHOC) &&
+	    (net->channel > ieee->ibss_maxjoin_chal))
+		return;
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
+		/* if the user specified the AP MAC, we need also the essid
+		 * This could be obtained by beacons or, if the network does not
+		 * broadcast it, it can be put manually.
+		 */
+		apset = ieee->wap_set;
+		ssidset = ieee->ssid_set;
+		ssidbroad =  !(net->ssid_len == 0 || net->ssid[0] == '\0');
+		apmatch = (memcmp(ieee->current_network.bssid, net->bssid,
+				  ETH_ALEN) == 0);
+		if (!ssidbroad) {
+			ssidmatch = (ieee->current_network.ssid_len ==
+				    net->hidden_ssid_len) &&
+				    (!strncmp(ieee->current_network.ssid,
+				    net->hidden_ssid, net->hidden_ssid_len));
+			if (net->hidden_ssid_len > 0) {
+				strncpy(net->ssid, net->hidden_ssid,
+					net->hidden_ssid_len);
+				net->ssid_len = net->hidden_ssid_len;
+				ssidbroad = 1;
+			}
+		} else
+			ssidmatch =
+			   (ieee->current_network.ssid_len == net->ssid_len) &&
+			   (!strncmp(ieee->current_network.ssid, net->ssid,
+			   net->ssid_len));
+
+		/* if the user set the AP check if match.
+		 * if the network does not broadcast essid we check the
+		 *	 user supplied ANY essid
+		 * if the network does broadcast and the user does not set
+		 *	 essid it is OK
+		 * if the network does broadcast and the user did set essid
+		 * check if essid match
+		 * if the ap is not set, check that the user set the bssid
+		 * and the network does broadcast and that those two bssid match
+		 */
+		if ((apset && apmatch &&
+		   ((ssidset && ssidbroad && ssidmatch) ||
+		   (ssidbroad && !ssidset) || (!ssidbroad && ssidset))) ||
+		   (!apset && ssidset && ssidbroad && ssidmatch) ||
+		   (ieee->is_roaming && ssidset && ssidbroad && ssidmatch)) {
+			/* Save the essid so that if it is hidden, it is
+			 * replaced with the essid provided by the user.
+			 */
+			if (!ssidbroad) {
+				memcpy(tmp_ssid, ieee->current_network.ssid,
+				       ieee->current_network.ssid_len);
+				tmp_ssid_len = ieee->current_network.ssid_len;
+			}
+			memcpy(&ieee->current_network, net,
+				sizeof(ieee->current_network));
+			if (!ssidbroad) {
+				memcpy(ieee->current_network.ssid, tmp_ssid,
+				       tmp_ssid_len);
+				ieee->current_network.ssid_len = tmp_ssid_len;
+			}
+			netdev_info(ieee->dev,
+				    "Linking with %s,channel:%d, qos:%d, myHT:%d, networkHT:%d, mode:%x cur_net.flags:0x%x\n",
+				    ieee->current_network.ssid,
+				    ieee->current_network.channel,
+				    ieee->current_network.qos_data.supported,
+				    ieee->pHTInfo->bEnableHT,
+				    ieee->current_network.bssht.bd_support_ht,
+				    ieee->current_network.mode,
+				    ieee->current_network.flags);
+
+			if ((rtllib_act_scanning(ieee, false)) &&
+			   !(ieee->softmac_features & IEEE_SOFTMAC_SCAN))
+				rtllib_stop_scan_syncro(ieee);
+
+			HTResetIOTSetting(ieee->pHTInfo);
+			ieee->wmm_acm = 0;
+			if (ieee->iw_mode == IW_MODE_INFRA) {
+				/* Join the network for the first time */
+				ieee->AsocRetryCount = 0;
+				if ((ieee->current_network.qos_data.supported == 1) &&
+				    ieee->current_network.bssht.bd_support_ht)
+					HTResetSelfAndSavePeerSetting(ieee,
+						 &(ieee->current_network));
+				else
+					ieee->pHTInfo->bCurrentHTSupport =
+								 false;
+
+				ieee->state = RTLLIB_ASSOCIATING;
+				if (ieee->LedControlHandler != NULL)
+					ieee->LedControlHandler(ieee->dev,
+							 LED_CTL_START_TO_LINK);
+				schedule_delayed_work(
+					   &ieee->associate_procedure_wq, 0);
+			} else {
+				if (rtllib_is_54g(&ieee->current_network) &&
+				    (ieee->modulation &
+				     RTLLIB_OFDM_MODULATION)) {
+					ieee->rate = 108;
+					ieee->SetWirelessMode(ieee->dev,
+							      IEEE_G);
+					netdev_info(ieee->dev,
+						    "Using G rates\n");
+				} else {
+					ieee->rate = 22;
+					ieee->SetWirelessMode(ieee->dev,
+							      IEEE_B);
+					netdev_info(ieee->dev,
+						    "Using B rates\n");
+				}
+				memset(ieee->dot11HTOperationalRateSet, 0, 16);
+				ieee->state = RTLLIB_LINKED;
+			}
+		}
+	}
+}
+
+static void rtllib_softmac_check_all_nets(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+	struct rtllib_network *target;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	list_for_each_entry(target, &ieee->network_list, list) {
+
+		/* if the state become different that NOLINK means
+		 * we had found what we are searching for
+		 */
+
+		if (ieee->state != RTLLIB_NOLINK)
+			break;
+
+		if (ieee->scan_age == 0 || time_after(target->last_scanned +
+		    ieee->scan_age, jiffies))
+			rtllib_softmac_new_net(ieee, target);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+static inline int auth_parse(struct net_device *dev, struct sk_buff *skb,
+			     u8 **challenge, int *chlen)
+{
+	struct rtllib_authentication *a;
+	u8 *t;
+
+	if (skb->len <  (sizeof(struct rtllib_authentication) -
+	    sizeof(struct rtllib_info_element))) {
+		netdev_dbg(dev, "invalid len in auth resp: %d\n", skb->len);
+		return -EINVAL;
+	}
+	*challenge = NULL;
+	a = (struct rtllib_authentication *)skb->data;
+	if (skb->len > (sizeof(struct rtllib_authentication) + 3)) {
+		t = skb->data + sizeof(struct rtllib_authentication);
+
+		if (*(t++) == MFIE_TYPE_CHALLENGE) {
+			*chlen = *(t++);
+			*challenge = kmemdup(t, *chlen, GFP_ATOMIC);
+			if (!*challenge)
+				return -ENOMEM;
+		}
+	}
+
+	if (a->status) {
+		netdev_dbg(dev, "auth_parse() failed\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int auth_rq_parse(struct net_device *dev, struct sk_buff *skb, u8 *dest)
+{
+	struct rtllib_authentication *a;
+
+	if (skb->len <  (sizeof(struct rtllib_authentication) -
+	    sizeof(struct rtllib_info_element))) {
+		netdev_dbg(dev, "invalid len in auth request: %d\n", skb->len);
+		return -1;
+	}
+	a = (struct rtllib_authentication *)skb->data;
+
+	ether_addr_copy(dest, a->header.addr2);
+
+	if (le16_to_cpu(a->algorithm) != WLAN_AUTH_OPEN)
+		return  WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
+
+	return WLAN_STATUS_SUCCESS;
+}
+
+static short probe_rq_parse(struct rtllib_device *ieee, struct sk_buff *skb,
+			    u8 *src)
+{
+	u8 *tag;
+	u8 *skbend;
+	u8 *ssid = NULL;
+	u8 ssidlen = 0;
+	struct rtllib_hdr_3addr   *header =
+		(struct rtllib_hdr_3addr   *)skb->data;
+	bool bssid_match;
+
+	if (skb->len < sizeof(struct rtllib_hdr_3addr))
+		return -1; /* corrupted */
+
+	bssid_match =
+	  (!ether_addr_equal(header->addr3, ieee->current_network.bssid)) &&
+	  (!is_broadcast_ether_addr(header->addr3));
+	if (bssid_match)
+		return -1;
+
+	ether_addr_copy(src, header->addr2);
+
+	skbend = (u8 *)skb->data + skb->len;
+
+	tag = skb->data + sizeof(struct rtllib_hdr_3addr);
+
+	while (tag + 1 < skbend) {
+		if (*tag == 0) {
+			ssid = tag + 2;
+			ssidlen = *(tag + 1);
+			break;
+		}
+		tag++; /* point to the len field */
+		tag = tag + *(tag); /* point to the last data byte of the tag */
+		tag++; /* point to the next tag */
+	}
+
+	if (ssidlen == 0)
+		return 1;
+
+	if (!ssid)
+		return 1; /* ssid not found in tagged param */
+
+	return !strncmp(ssid, ieee->current_network.ssid, ssidlen);
+}
+
+static int assoc_rq_parse(struct net_device *dev, struct sk_buff *skb, u8 *dest)
+{
+	struct rtllib_assoc_request_frame *a;
+
+	if (skb->len < (sizeof(struct rtllib_assoc_request_frame) -
+		sizeof(struct rtllib_info_element))) {
+		netdev_dbg(dev, "invalid len in auth request:%d\n", skb->len);
+		return -1;
+	}
+
+	a = (struct rtllib_assoc_request_frame *)skb->data;
+
+	ether_addr_copy(dest, a->header.addr2);
+
+	return 0;
+}
+
+static inline u16 assoc_parse(struct rtllib_device *ieee, struct sk_buff *skb,
+			      int *aid)
+{
+	struct rtllib_assoc_response_frame *response_head;
+	u16 status_code;
+
+	if (skb->len <  sizeof(struct rtllib_assoc_response_frame)) {
+		netdev_dbg(ieee->dev, "Invalid len in auth resp: %d\n",
+			   skb->len);
+		return 0xcafe;
+	}
+
+	response_head = (struct rtllib_assoc_response_frame *)skb->data;
+	*aid = le16_to_cpu(response_head->aid) & 0x3fff;
+
+	status_code = le16_to_cpu(response_head->status);
+	if ((status_code == WLAN_STATUS_ASSOC_DENIED_RATES ||
+	   status_code == WLAN_STATUS_CAPS_UNSUPPORTED) &&
+	   ((ieee->mode == IEEE_G) &&
+	   (ieee->current_network.mode == IEEE_N_24G) &&
+	   (ieee->AsocRetryCount++ < (RT_ASOC_RETRY_LIMIT-1)))) {
+		ieee->pHTInfo->iot_action |= HT_IOT_ACT_PURE_N_MODE;
+	} else {
+		ieee->AsocRetryCount = 0;
+	}
+
+	return le16_to_cpu(response_head->status);
+}
+
+void rtllib_rx_probe_rq(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+
+	ieee->softmac_stats.rx_probe_rq++;
+	if (probe_rq_parse(ieee, skb, dest) > 0) {
+		ieee->softmac_stats.tx_probe_rs++;
+		rtllib_resp_to_probe(ieee, dest);
+	}
+}
+
+static inline void rtllib_rx_auth_rq(struct rtllib_device *ieee,
+				     struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+	int status;
+
+	ieee->softmac_stats.rx_auth_rq++;
+
+	status = auth_rq_parse(ieee->dev, skb, dest);
+	if (status != -1)
+		rtllib_resp_to_auth(ieee, status, dest);
+}
+
+static inline void rtllib_rx_assoc_rq(struct rtllib_device *ieee,
+				      struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+
+
+	ieee->softmac_stats.rx_ass_rq++;
+	if (assoc_rq_parse(ieee->dev, skb, dest) != -1)
+		rtllib_resp_to_assoc_rq(ieee, dest);
+
+	netdev_info(ieee->dev, "New client associated: %pM\n", dest);
+}
+
+void rtllib_sta_ps_send_null_frame(struct rtllib_device *ieee, short pwr)
+{
+
+	struct sk_buff *buf = rtllib_null_func(ieee, pwr);
+
+	if (buf)
+		softmac_ps_mgmt_xmit(buf, ieee);
+}
+EXPORT_SYMBOL(rtllib_sta_ps_send_null_frame);
+
+void rtllib_sta_ps_send_pspoll_frame(struct rtllib_device *ieee)
+{
+	struct sk_buff *buf = rtllib_pspoll_func(ieee);
+
+	if (buf)
+		softmac_ps_mgmt_xmit(buf, ieee);
+}
+
+static short rtllib_sta_ps_sleep(struct rtllib_device *ieee, u64 *time)
+{
+	int timeout;
+	u8 dtim;
+	struct rt_pwr_save_ctrl *pPSC = &(ieee->PowerSaveControl);
+
+	if (ieee->LPSDelayCnt) {
+		ieee->LPSDelayCnt--;
+		return 0;
+	}
+
+	dtim = ieee->current_network.dtim_data;
+	if (!(dtim & RTLLIB_DTIM_VALID))
+		return 0;
+	timeout = ieee->current_network.beacon_interval;
+	ieee->current_network.dtim_data = RTLLIB_DTIM_INVALID;
+	/* there's no need to nofity AP that I find you buffered
+	 * with broadcast packet
+	 */
+	if (dtim & (RTLLIB_DTIM_UCAST & ieee->ps))
+		return 2;
+
+	if (!time_after(jiffies,
+			dev_trans_start(ieee->dev) + msecs_to_jiffies(timeout)))
+		return 0;
+	if (!time_after(jiffies,
+			ieee->last_rx_ps_time + msecs_to_jiffies(timeout)))
+		return 0;
+	if ((ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) &&
+	    (ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
+		return 0;
+
+	if (time) {
+		if (ieee->bAwakePktSent) {
+			pPSC->LPSAwakeIntvl = 1;
+		} else {
+			u8 MaxPeriod = 1;
+
+			if (pPSC->LPSAwakeIntvl == 0)
+				pPSC->LPSAwakeIntvl = 1;
+			if (pPSC->RegMaxLPSAwakeIntvl == 0)
+				MaxPeriod = 1;
+			else if (pPSC->RegMaxLPSAwakeIntvl == 0xFF)
+				MaxPeriod = ieee->current_network.dtim_period;
+			else
+				MaxPeriod = pPSC->RegMaxLPSAwakeIntvl;
+			pPSC->LPSAwakeIntvl = (pPSC->LPSAwakeIntvl >=
+					       MaxPeriod) ? MaxPeriod :
+					       (pPSC->LPSAwakeIntvl + 1);
+		}
+		{
+			u8 LPSAwakeIntvl_tmp = 0;
+			u8 period = ieee->current_network.dtim_period;
+			u8 count = ieee->current_network.tim.tim_count;
+
+			if (count == 0) {
+				if (pPSC->LPSAwakeIntvl > period)
+					LPSAwakeIntvl_tmp = period +
+						 (pPSC->LPSAwakeIntvl -
+						 period) -
+						 ((pPSC->LPSAwakeIntvl-period) %
+						 period);
+				else
+					LPSAwakeIntvl_tmp = pPSC->LPSAwakeIntvl;
+
+			} else {
+				if (pPSC->LPSAwakeIntvl >
+				    ieee->current_network.tim.tim_count)
+					LPSAwakeIntvl_tmp = count +
+					(pPSC->LPSAwakeIntvl - count) -
+					((pPSC->LPSAwakeIntvl-count)%period);
+				else
+					LPSAwakeIntvl_tmp = pPSC->LPSAwakeIntvl;
+			}
+
+		*time = ieee->current_network.last_dtim_sta_time
+			+ msecs_to_jiffies(ieee->current_network.beacon_interval *
+			LPSAwakeIntvl_tmp);
+	}
+	}
+
+	return 1;
+
+
+}
+
+static inline void rtllib_sta_ps(struct work_struct *work)
+{
+	struct rtllib_device *ieee;
+	u64 time;
+	short sleep;
+	unsigned long flags, flags2;
+
+	ieee = container_of(work, struct rtllib_device, ps_task);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if ((ieee->ps == RTLLIB_PS_DISABLED ||
+	     ieee->iw_mode != IW_MODE_INFRA ||
+	     ieee->state != RTLLIB_LINKED)) {
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+		rtllib_sta_wakeup(ieee, 1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+	sleep = rtllib_sta_ps_sleep(ieee, &time);
+	/* 2 wake, 1 sleep, 0 do nothing */
+	if (sleep == 0)
+		goto out;
+	if (sleep == 1) {
+		if (ieee->sta_sleep == LPS_IS_SLEEP) {
+			ieee->enter_sleep_state(ieee->dev, time);
+		} else if (ieee->sta_sleep == LPS_IS_WAKE) {
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+			if (ieee->ps_is_queue_empty(ieee->dev)) {
+				ieee->sta_sleep = LPS_WAIT_NULL_DATA_SEND;
+				ieee->ack_tx_to_ieee = 1;
+				rtllib_sta_ps_send_null_frame(ieee, 1);
+				ieee->ps_time = time;
+			}
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+
+		}
+
+		ieee->bAwakePktSent = false;
+
+	} else if (sleep == 2) {
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+		rtllib_sta_wakeup(ieee, 1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+static void rtllib_sta_wakeup(struct rtllib_device *ieee, short nl)
+{
+	if (ieee->sta_sleep == LPS_IS_WAKE) {
+		if (nl) {
+			if (ieee->pHTInfo->iot_action &
+			    HT_IOT_ACT_NULL_DATA_POWER_SAVING) {
+				ieee->ack_tx_to_ieee = 1;
+				rtllib_sta_ps_send_null_frame(ieee, 0);
+			} else {
+				ieee->ack_tx_to_ieee = 1;
+				rtllib_sta_ps_send_pspoll_frame(ieee);
+			}
+		}
+		return;
+
+	}
+
+	if (ieee->sta_sleep == LPS_IS_SLEEP)
+		ieee->sta_wake_up(ieee->dev);
+	if (nl) {
+		if (ieee->pHTInfo->iot_action &
+		    HT_IOT_ACT_NULL_DATA_POWER_SAVING) {
+			ieee->ack_tx_to_ieee = 1;
+			rtllib_sta_ps_send_null_frame(ieee, 0);
+		} else {
+			ieee->ack_tx_to_ieee = 1;
+			ieee->polling = true;
+			rtllib_sta_ps_send_pspoll_frame(ieee);
+		}
+
+	} else {
+		ieee->sta_sleep = LPS_IS_WAKE;
+		ieee->polling = false;
+	}
+}
+
+void rtllib_ps_tx_ack(struct rtllib_device *ieee, short success)
+{
+	unsigned long flags, flags2;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->sta_sleep == LPS_WAIT_NULL_DATA_SEND) {
+		/* Null frame with PS bit set */
+		if (success) {
+			ieee->sta_sleep = LPS_IS_SLEEP;
+			ieee->enter_sleep_state(ieee->dev, ieee->ps_time);
+		}
+		/* if the card report not success we can't be sure the AP
+		 * has not RXed so we can't assume the AP believe us awake
+		 */
+	} else {/* 21112005 - tx again null without PS bit if lost */
+
+		if ((ieee->sta_sleep == LPS_IS_WAKE) && !success) {
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+			if (ieee->pHTInfo->iot_action &
+			    HT_IOT_ACT_NULL_DATA_POWER_SAVING)
+				rtllib_sta_ps_send_null_frame(ieee, 0);
+			else
+				rtllib_sta_ps_send_pspoll_frame(ieee);
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+EXPORT_SYMBOL(rtllib_ps_tx_ack);
+
+static void rtllib_process_action(struct rtllib_device *ieee,
+				  struct sk_buff *skb)
+{
+	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *)skb->data;
+	u8 *act = rtllib_get_payload((struct rtllib_hdr *)header);
+	u8 category = 0;
+
+	if (act == NULL) {
+		netdev_warn(ieee->dev,
+			    "Error getting payload of action frame\n");
+		return;
+	}
+
+	category = *act;
+	act++;
+	switch (category) {
+	case ACT_CAT_BA:
+		switch (*act) {
+		case ACT_ADDBAREQ:
+			rtllib_rx_ADDBAReq(ieee, skb);
+			break;
+		case ACT_ADDBARSP:
+			rtllib_rx_ADDBARsp(ieee, skb);
+			break;
+		case ACT_DELBA:
+			rtllib_rx_DELBA(ieee, skb);
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static inline int
+rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
+		     struct rtllib_rx_stats *rx_stats)
+{
+	u16 errcode;
+	int aid;
+	u8 *ies;
+	struct rtllib_assoc_response_frame *assoc_resp;
+	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *)skb->data;
+	u16 frame_ctl = le16_to_cpu(header->frame_ctl);
+
+	netdev_dbg(ieee->dev, "received [RE]ASSOCIATION RESPONSE (%d)\n",
+		   WLAN_FC_GET_STYPE(frame_ctl));
+
+	if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+	     ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATED &&
+	     (ieee->iw_mode == IW_MODE_INFRA)) {
+		errcode = assoc_parse(ieee, skb, &aid);
+		if (!errcode) {
+			struct rtllib_network *network =
+				 kzalloc(sizeof(struct rtllib_network),
+				 GFP_ATOMIC);
+
+			if (!network)
+				return 1;
+			ieee->state = RTLLIB_LINKED;
+			ieee->assoc_id = aid;
+			ieee->softmac_stats.rx_ass_ok++;
+			/* station support qos */
+			/* Let the register setting default with Legacy station */
+			assoc_resp = (struct rtllib_assoc_response_frame *)skb->data;
+			if (ieee->current_network.qos_data.supported == 1) {
+				if (rtllib_parse_info_param(ieee, assoc_resp->info_element,
+							rx_stats->len - sizeof(*assoc_resp),
+							network, rx_stats)) {
+					kfree(network);
+					return 1;
+				}
+				memcpy(ieee->pHTInfo->PeerHTCapBuf,
+				       network->bssht.bd_ht_cap_buf,
+				       network->bssht.bd_ht_cap_len);
+				memcpy(ieee->pHTInfo->PeerHTInfoBuf,
+				       network->bssht.bd_ht_info_buf,
+				       network->bssht.bd_ht_info_len);
+				if (ieee->handle_assoc_response != NULL)
+					ieee->handle_assoc_response(ieee->dev,
+						 (struct rtllib_assoc_response_frame *)header,
+						 network);
+			}
+			kfree(network);
+
+			kfree(ieee->assocresp_ies);
+			ieee->assocresp_ies = NULL;
+			ies = &(assoc_resp->info_element[0].id);
+			ieee->assocresp_ies_len = (skb->data + skb->len) - ies;
+			ieee->assocresp_ies = kmemdup(ies,
+						      ieee->assocresp_ies_len,
+						      GFP_ATOMIC);
+			if (!ieee->assocresp_ies)
+				ieee->assocresp_ies_len = 0;
+
+			rtllib_associate_complete(ieee);
+		} else {
+			/* aid could not been allocated */
+			ieee->softmac_stats.rx_ass_err++;
+			netdev_info(ieee->dev,
+				    "Association response status code 0x%x\n",
+				    errcode);
+			if (ieee->AsocRetryCount < RT_ASOC_RETRY_LIMIT)
+				schedule_delayed_work(
+					 &ieee->associate_procedure_wq, 0);
+			else
+				rtllib_associate_abort(ieee);
+		}
+	}
+	return 0;
+}
+
+static void rtllib_rx_auth_resp(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	int errcode;
+	u8 *challenge;
+	int chlen = 0;
+	bool bSupportNmode = true, bHalfSupportNmode = false;
+
+	errcode = auth_parse(ieee->dev, skb, &challenge, &chlen);
+
+	if (errcode) {
+		ieee->softmac_stats.rx_auth_rs_err++;
+		netdev_info(ieee->dev,
+			    "Authentication response status code %d", errcode);
+		rtllib_associate_abort(ieee);
+		return;
+	}
+
+	if (ieee->open_wep || !challenge) {
+		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATED;
+		ieee->softmac_stats.rx_auth_rs_ok++;
+		if (!(ieee->pHTInfo->iot_action & HT_IOT_ACT_PURE_N_MODE)) {
+			if (!ieee->GetNmodeSupportBySecCfg(ieee->dev)) {
+				if (IsHTHalfNmodeAPs(ieee)) {
+					bSupportNmode = true;
+					bHalfSupportNmode = true;
+				} else {
+					bSupportNmode = false;
+					bHalfSupportNmode = false;
+				}
+			}
+		}
+		/* Dummy wirless mode setting to avoid encryption issue */
+		if (bSupportNmode) {
+			ieee->SetWirelessMode(ieee->dev,
+					      ieee->current_network.mode);
+		} else {
+			/*TODO*/
+			ieee->SetWirelessMode(ieee->dev, IEEE_G);
+		}
+
+		if ((ieee->current_network.mode == IEEE_N_24G) &&
+		    bHalfSupportNmode) {
+			netdev_info(ieee->dev, "======>enter half N mode\n");
+			ieee->bHalfWirelessN24GMode = true;
+		} else {
+			ieee->bHalfWirelessN24GMode = false;
+		}
+		rtllib_associate_step2(ieee);
+	} else {
+		rtllib_auth_challenge(ieee, challenge,  chlen);
+	}
+}
+
+static inline int
+rtllib_rx_auth(struct rtllib_device *ieee, struct sk_buff *skb,
+	       struct rtllib_rx_stats *rx_stats)
+{
+
+	if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
+		if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING &&
+		    (ieee->iw_mode == IW_MODE_INFRA)) {
+			netdev_dbg(ieee->dev,
+				   "Received authentication response");
+			rtllib_rx_auth_resp(ieee, skb);
+		} else if (ieee->iw_mode == IW_MODE_MASTER) {
+			rtllib_rx_auth_rq(ieee, skb);
+		}
+	}
+	return 0;
+}
+
+static inline int
+rtllib_rx_deauth(struct rtllib_device *ieee, struct sk_buff *skb)
+{
+	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *)skb->data;
+	u16 frame_ctl;
+
+	if (memcmp(header->addr3, ieee->current_network.bssid, ETH_ALEN) != 0)
+		return 0;
+
+	/* FIXME for now repeat all the association procedure
+	 * both for disassociation and deauthentication
+	 */
+	if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+	    ieee->state == RTLLIB_LINKED &&
+	    (ieee->iw_mode == IW_MODE_INFRA)) {
+		frame_ctl = le16_to_cpu(header->frame_ctl);
+		netdev_info(ieee->dev,
+			    "==========>received disassoc/deauth(%x) frame, reason code:%x\n",
+			    WLAN_FC_GET_STYPE(frame_ctl),
+			    ((struct rtllib_disassoc *)skb->data)->reason);
+		ieee->state = RTLLIB_ASSOCIATING;
+		ieee->softmac_stats.reassoc++;
+		ieee->is_roaming = true;
+		ieee->LinkDetectInfo.bBusyTraffic = false;
+		rtllib_disassociate(ieee);
+		RemovePeerTS(ieee, header->addr2);
+		if (ieee->LedControlHandler != NULL)
+			ieee->LedControlHandler(ieee->dev,
+						LED_CTL_START_TO_LINK);
+
+		if (!(ieee->rtllib_ap_sec_type(ieee) &
+		    (SEC_ALG_CCMP|SEC_ALG_TKIP)))
+			schedule_delayed_work(
+				       &ieee->associate_procedure_wq, 5);
+	}
+	return 0;
+}
+
+inline int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
+				   struct sk_buff *skb,
+				   struct rtllib_rx_stats *rx_stats, u16 type,
+				   u16 stype)
+{
+	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *)skb->data;
+	u16 frame_ctl;
+
+	if (!ieee->proto_started)
+		return 0;
+
+	frame_ctl = le16_to_cpu(header->frame_ctl);
+	switch (WLAN_FC_GET_STYPE(frame_ctl)) {
+	case RTLLIB_STYPE_ASSOC_RESP:
+	case RTLLIB_STYPE_REASSOC_RESP:
+		if (rtllib_rx_assoc_resp(ieee, skb, rx_stats) == 1)
+			return 1;
+		break;
+	case RTLLIB_STYPE_ASSOC_REQ:
+	case RTLLIB_STYPE_REASSOC_REQ:
+		if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+		     ieee->iw_mode == IW_MODE_MASTER)
+			rtllib_rx_assoc_rq(ieee, skb);
+		break;
+	case RTLLIB_STYPE_AUTH:
+		rtllib_rx_auth(ieee, skb, rx_stats);
+		break;
+	case RTLLIB_STYPE_DISASSOC:
+	case RTLLIB_STYPE_DEAUTH:
+		rtllib_rx_deauth(ieee, skb);
+		break;
+	case RTLLIB_STYPE_MANAGE_ACT:
+		rtllib_process_action(ieee, skb);
+		break;
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+/* following are for a simpler TX queue management.
+ * Instead of using netif_[stop/wake]_queue the driver
+ * will use these two functions (plus a reset one), that
+ * will internally use the kernel netif_* and takes
+ * care of the ieee802.11 fragmentation.
+ * So the driver receives a fragment per time and might
+ * call the stop function when it wants to not
+ * have enough room to TX an entire packet.
+ * This might be useful if each fragment needs it's own
+ * descriptor, thus just keep a total free memory > than
+ * the max fragmentation threshold is not enough.. If the
+ * ieee802.11 stack passed a TXB struct then you need
+ * to keep N free descriptors where
+ * N = MAX_PACKET_SIZE / MIN_FRAG_TRESHOLD
+ * In this way you need just one and the 802.11 stack
+ * will take care of buffering fragments and pass them to
+ * the driver later, when it wakes the queue.
+ */
+void rtllib_softmac_xmit(struct rtllib_txb *txb, struct rtllib_device *ieee)
+{
+
+	unsigned int queue_index = txb->queue_index;
+	unsigned long flags;
+	int  i;
+	struct cb_desc *tcb_desc = NULL;
+	unsigned long queue_len = 0;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* called with 2nd parm 0, no tx mgmt lock required */
+	rtllib_sta_wakeup(ieee, 0);
+
+	/* update the tx status */
+	tcb_desc = (struct cb_desc *)(txb->fragments[0]->cb +
+		   MAX_DEV_ADDR_SIZE);
+	if (tcb_desc->bMulticast)
+		ieee->stats.multicast++;
+
+	/* if xmit available, just xmit it immediately, else just insert it to
+	 * the wait queue
+	 */
+	for (i = 0; i < txb->nr_frags; i++) {
+		queue_len = skb_queue_len(&ieee->skb_waitQ[queue_index]);
+		if ((queue_len  != 0) ||
+		    (!ieee->check_nic_enough_desc(ieee->dev, queue_index)) ||
+		    (ieee->queue_stop)) {
+			/* insert the skb packet to the wait queue
+			 * as for the completion function, it does not need
+			 * to check it any more.
+			 */
+			if (queue_len < 200)
+				skb_queue_tail(&ieee->skb_waitQ[queue_index],
+					       txb->fragments[i]);
+			else
+				kfree_skb(txb->fragments[i]);
+		} else {
+			ieee->softmac_data_hard_start_xmit(
+					txb->fragments[i],
+					ieee->dev, ieee->rate);
+		}
+	}
+
+	rtllib_txb_free(txb);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+void rtllib_reset_queue(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	init_mgmt_queue(ieee);
+	if (ieee->tx_pending.txb) {
+		rtllib_txb_free(ieee->tx_pending.txb);
+		ieee->tx_pending.txb = NULL;
+	}
+	ieee->queue_stop = 0;
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+EXPORT_SYMBOL(rtllib_reset_queue);
+
+void rtllib_stop_all_queues(struct rtllib_device *ieee)
+{
+	unsigned int i;
+
+	for (i = 0; i < ieee->dev->num_tx_queues; i++)
+		txq_trans_cond_update(netdev_get_tx_queue(ieee->dev, i));
+
+	netif_tx_stop_all_queues(ieee->dev);
+}
+
+void rtllib_wake_all_queues(struct rtllib_device *ieee)
+{
+	netif_tx_wake_all_queues(ieee->dev);
+}
+
+/* called in user context only */
+static void rtllib_start_master_bss(struct rtllib_device *ieee)
+{
+	ieee->assoc_id = 1;
+
+	if (ieee->current_network.ssid_len == 0) {
+		strncpy(ieee->current_network.ssid,
+			RTLLIB_DEFAULT_TX_ESSID,
+			IW_ESSID_MAX_SIZE);
+
+		ieee->current_network.ssid_len =
+				 strlen(RTLLIB_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	ether_addr_copy(ieee->current_network.bssid, ieee->dev->dev_addr);
+
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	ieee->state = RTLLIB_LINKED;
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	netif_carrier_on(ieee->dev);
+}
+
+static void rtllib_start_monitor_mode(struct rtllib_device *ieee)
+{
+	/* reset hardware status */
+	if (ieee->raw_tx) {
+		if (ieee->data_hard_resume)
+			ieee->data_hard_resume(ieee->dev);
+
+		netif_carrier_on(ieee->dev);
+	}
+}
+
+static void rtllib_start_ibss_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, start_ibss_wq);
+	/* iwconfig mode ad-hoc will schedule this and return
+	 * on the other hand this will block further iwconfig SET
+	 * operations because of the wx_mutex hold.
+	 * Anyway some most set operations set a flag to speed-up
+	 * (abort) this wq (when syncro scanning) before sleeping
+	 * on the mutex
+	 */
+	if (!ieee->proto_started) {
+		netdev_info(ieee->dev, "==========oh driver down return\n");
+		return;
+	}
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->current_network.ssid_len == 0) {
+		strscpy(ieee->current_network.ssid, RTLLIB_DEFAULT_TX_ESSID,
+			sizeof(ieee->current_network.ssid));
+		ieee->current_network.ssid_len = strlen(RTLLIB_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	ieee->state = RTLLIB_NOLINK;
+	ieee->mode = IEEE_G;
+	/* check if we have this cell in our network list */
+	rtllib_softmac_check_all_nets(ieee);
+
+
+	/* if not then the state is not linked. Maybe the user switched to
+	 * ad-hoc mode just after being in monitor mode, or just after
+	 * being very few time in managed mode (so the card have had no
+	 * time to scan all the chans..) or we have just run up the iface
+	 * after setting ad-hoc mode. So we have to give another try..
+	 * Here, in ibss mode, should be safe to do this without extra care
+	 * (in bss mode we had to make sure no-one tried to associate when
+	 * we had just checked the ieee->state and we was going to start the
+	 * scan) because in ibss mode the rtllib_new_net function, when
+	 * finds a good net, just set the ieee->state to RTLLIB_LINKED,
+	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
+	 * scan, that will stop at the first round because it sees the state
+	 * associated.
+	 */
+	if (ieee->state == RTLLIB_NOLINK)
+		rtllib_start_scan_syncro(ieee, 0);
+
+	/* the network definitively is not here.. create a new cell */
+	if (ieee->state == RTLLIB_NOLINK) {
+		netdev_info(ieee->dev, "creating new IBSS cell\n");
+		ieee->current_network.channel = ieee->bss_start_channel;
+		if (!ieee->wap_set)
+			eth_random_addr(ieee->current_network.bssid);
+
+		if (ieee->modulation & RTLLIB_CCK_MODULATION) {
+
+			ieee->current_network.rates_len = 4;
+
+			ieee->current_network.rates[0] =
+				 RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_1MB;
+			ieee->current_network.rates[1] =
+				 RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_2MB;
+			ieee->current_network.rates[2] =
+				 RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_5MB;
+			ieee->current_network.rates[3] =
+				 RTLLIB_BASIC_RATE_MASK | RTLLIB_CCK_RATE_11MB;
+
+		} else
+			ieee->current_network.rates_len = 0;
+
+		if (ieee->modulation & RTLLIB_OFDM_MODULATION) {
+			ieee->current_network.rates_ex_len = 8;
+
+			ieee->current_network.rates_ex[0] =
+						 RTLLIB_OFDM_RATE_6MB;
+			ieee->current_network.rates_ex[1] =
+						 RTLLIB_OFDM_RATE_9MB;
+			ieee->current_network.rates_ex[2] =
+						 RTLLIB_OFDM_RATE_12MB;
+			ieee->current_network.rates_ex[3] =
+						 RTLLIB_OFDM_RATE_18MB;
+			ieee->current_network.rates_ex[4] =
+						 RTLLIB_OFDM_RATE_24MB;
+			ieee->current_network.rates_ex[5] =
+						 RTLLIB_OFDM_RATE_36MB;
+			ieee->current_network.rates_ex[6] =
+						 RTLLIB_OFDM_RATE_48MB;
+			ieee->current_network.rates_ex[7] =
+						 RTLLIB_OFDM_RATE_54MB;
+
+			ieee->rate = 108;
+		} else {
+			ieee->current_network.rates_ex_len = 0;
+			ieee->rate = 22;
+		}
+
+		ieee->current_network.qos_data.supported = 0;
+		ieee->SetWirelessMode(ieee->dev, IEEE_G);
+		ieee->current_network.mode = ieee->mode;
+		ieee->current_network.atim_window = 0;
+		ieee->current_network.capability = WLAN_CAPABILITY_IBSS;
+	}
+
+	netdev_info(ieee->dev, "%s(): ieee->mode = %d\n", __func__, ieee->mode);
+	if ((ieee->mode == IEEE_N_24G) || (ieee->mode == IEEE_N_5G))
+		HTUseDefaultSetting(ieee);
+	else
+		ieee->pHTInfo->bCurrentHTSupport = false;
+
+	ieee->SetHwRegHandler(ieee->dev, HW_VAR_MEDIA_STATUS,
+			      (u8 *)(&ieee->state));
+
+	ieee->state = RTLLIB_LINKED;
+	ieee->link_change(ieee->dev);
+
+	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+	if (ieee->LedControlHandler != NULL)
+		ieee->LedControlHandler(ieee->dev, LED_CTL_LINK);
+
+	rtllib_start_send_beacons(ieee);
+
+	notify_wx_assoc_event(ieee);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	netif_carrier_on(ieee->dev);
+
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+inline void rtllib_start_ibss(struct rtllib_device *ieee)
+{
+	schedule_delayed_work(&ieee->start_ibss_wq, msecs_to_jiffies(150));
+}
+
+/* this is called only in user context, with wx_mutex held */
+static void rtllib_start_bss(struct rtllib_device *ieee)
+{
+	unsigned long flags;
+
+	if (IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee)) {
+		if (!ieee->global_domain)
+			return;
+	}
+	/* check if we have already found the net we
+	 * are interested in (if any).
+	 * if not (we are disassociated and we are not
+	 * in associating / authenticating phase) start the background scanning.
+	 */
+	rtllib_softmac_check_all_nets(ieee);
+
+	/* ensure no-one start an associating process (thus setting
+	 * the ieee->state to rtllib_ASSOCIATING) while we
+	 * have just checked it and we are going to enable scan.
+	 * The rtllib_new_net function is always called with
+	 * lock held (from both rtllib_softmac_check_all_nets and
+	 * the rx path), so we cannot be in the middle of such function
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state == RTLLIB_NOLINK)
+		rtllib_start_scan(ieee);
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+static void rtllib_link_change_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, link_change_wq);
+	ieee->link_change(ieee->dev);
+}
+/* called only in userspace context */
+void rtllib_disassociate(struct rtllib_device *ieee)
+{
+	netif_carrier_off(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)
+		rtllib_reset_queue(ieee);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+	if (IS_DOT11D_ENABLE(ieee))
+		dot11d_reset(ieee);
+	ieee->state = RTLLIB_NOLINK;
+	ieee->is_set_key = false;
+	ieee->wap_set = 0;
+
+	schedule_delayed_work(&ieee->link_change_wq, 0);
+
+	notify_wx_assoc_event(ieee);
+}
+
+static void rtllib_associate_retry_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_dwork_rsl(data,
+				     struct rtllib_device, associate_retry_wq);
+	unsigned long flags;
+
+	mutex_lock(&ieee->wx_mutex);
+	if (!ieee->proto_started)
+		goto exit;
+
+	if (ieee->state != RTLLIB_ASSOCIATING_RETRY)
+		goto exit;
+
+	/* until we do not set the state to RTLLIB_NOLINK
+	 * there are no possibility to have someone else trying
+	 * to start an association procedure (we get here with
+	 * ieee->state = RTLLIB_ASSOCIATING).
+	 * When we set the state to RTLLIB_NOLINK it is possible
+	 * that the RX path run an attempt to associate, but
+	 * both rtllib_softmac_check_all_nets and the
+	 * RX path works with ieee->lock held so there are no
+	 * problems. If we are still disassociated then start a scan.
+	 * the lock here is necessary to ensure no one try to start
+	 * an association procedure when we have just checked the
+	 * state and we are going to start the scan.
+	 */
+	ieee->beinretry = true;
+	ieee->state = RTLLIB_NOLINK;
+
+	rtllib_softmac_check_all_nets(ieee);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state == RTLLIB_NOLINK)
+		rtllib_start_scan(ieee);
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	ieee->beinretry = false;
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+static struct sk_buff *rtllib_get_beacon_(struct rtllib_device *ieee)
+{
+	static const u8 broadcast_addr[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+	struct sk_buff *skb;
+	struct rtllib_probe_response *b;
+
+	skb = rtllib_probe_resp(ieee, broadcast_addr);
+
+	if (!skb)
+		return NULL;
+
+	b = (struct rtllib_probe_response *)skb->data;
+	b->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_BEACON);
+
+	return skb;
+
+}
+
+struct sk_buff *rtllib_get_beacon(struct rtllib_device *ieee)
+{
+	struct sk_buff *skb;
+	struct rtllib_probe_response *b;
+
+	skb = rtllib_get_beacon_(ieee);
+	if (!skb)
+		return NULL;
+
+	b = (struct rtllib_probe_response *)skb->data;
+	b->header.seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+
+	return skb;
+}
+EXPORT_SYMBOL(rtllib_get_beacon);
+
+void rtllib_softmac_stop_protocol(struct rtllib_device *ieee, u8 mesh_flag,
+				  u8 shutdown)
+{
+	rtllib_stop_scan_syncro(ieee);
+	mutex_lock(&ieee->wx_mutex);
+	rtllib_stop_protocol(ieee, shutdown);
+	mutex_unlock(&ieee->wx_mutex);
+}
+EXPORT_SYMBOL(rtllib_softmac_stop_protocol);
+
+
+void rtllib_stop_protocol(struct rtllib_device *ieee, u8 shutdown)
+{
+	if (!ieee->proto_started)
+		return;
+
+	if (shutdown) {
+		ieee->proto_started = 0;
+		ieee->proto_stoppping = 1;
+		if (ieee->rtllib_ips_leave != NULL)
+			ieee->rtllib_ips_leave(ieee->dev);
+	}
+
+	rtllib_stop_send_beacons(ieee);
+	del_timer_sync(&ieee->associate_timer);
+	cancel_delayed_work_sync(&ieee->associate_retry_wq);
+	cancel_delayed_work_sync(&ieee->start_ibss_wq);
+	cancel_delayed_work_sync(&ieee->link_change_wq);
+	rtllib_stop_scan(ieee);
+
+	if (ieee->state <= RTLLIB_ASSOCIATING_AUTHENTICATED)
+		ieee->state = RTLLIB_NOLINK;
+
+	if (ieee->state == RTLLIB_LINKED) {
+		if (ieee->iw_mode == IW_MODE_INFRA)
+			SendDisassociation(ieee, 1, WLAN_REASON_DEAUTH_LEAVING);
+		rtllib_disassociate(ieee);
+	}
+
+	if (shutdown) {
+		RemoveAllTS(ieee);
+		ieee->proto_stoppping = 0;
+	}
+	kfree(ieee->assocreq_ies);
+	ieee->assocreq_ies = NULL;
+	ieee->assocreq_ies_len = 0;
+	kfree(ieee->assocresp_ies);
+	ieee->assocresp_ies = NULL;
+	ieee->assocresp_ies_len = 0;
+}
+
+void rtllib_softmac_start_protocol(struct rtllib_device *ieee, u8 mesh_flag)
+{
+	mutex_lock(&ieee->wx_mutex);
+	rtllib_start_protocol(ieee);
+	mutex_unlock(&ieee->wx_mutex);
+}
+EXPORT_SYMBOL(rtllib_softmac_start_protocol);
+
+void rtllib_start_protocol(struct rtllib_device *ieee)
+{
+	short ch = 0;
+	int i = 0;
+
+	rtllib_update_active_chan_map(ieee);
+
+	if (ieee->proto_started)
+		return;
+
+	ieee->proto_started = 1;
+
+	if (ieee->current_network.channel == 0) {
+		do {
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				return; /* no channel found */
+		} while (!ieee->active_channel_map[ch]);
+		ieee->current_network.channel = ch;
+	}
+
+	if (ieee->current_network.beacon_interval == 0)
+		ieee->current_network.beacon_interval = 100;
+
+	for (i = 0; i < 17; i++) {
+		ieee->last_rxseq_num[i] = -1;
+		ieee->last_rxfrag_num[i] = -1;
+		ieee->last_packet_time[i] = 0;
+	}
+
+	if (ieee->UpdateBeaconInterruptHandler)
+		ieee->UpdateBeaconInterruptHandler(ieee->dev, false);
+
+	ieee->wmm_acm = 0;
+	/* if the user set the MAC of the ad-hoc cell and then
+	 * switch to managed mode, shall we  make sure that association
+	 * attempts does not fail just because the user provide the essid
+	 * and the nic is still checking for the AP MAC ??
+	 */
+	if (ieee->iw_mode == IW_MODE_INFRA) {
+		rtllib_start_bss(ieee);
+	} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+		if (ieee->UpdateBeaconInterruptHandler)
+			ieee->UpdateBeaconInterruptHandler(ieee->dev, true);
+
+		rtllib_start_ibss(ieee);
+
+	} else if (ieee->iw_mode == IW_MODE_MASTER) {
+		rtllib_start_master_bss(ieee);
+	} else if (ieee->iw_mode == IW_MODE_MONITOR) {
+		rtllib_start_monitor_mode(ieee);
+	}
+}
+
+int rtllib_softmac_init(struct rtllib_device *ieee)
+{
+	int i;
+
+	memset(&ieee->current_network, 0, sizeof(struct rtllib_network));
+
+	ieee->state = RTLLIB_NOLINK;
+	for (i = 0; i < 5; i++)
+		ieee->seq_ctrl[i] = 0;
+	ieee->dot11d_info = kzalloc(sizeof(struct rt_dot11d_info), GFP_ATOMIC);
+	if (!ieee->dot11d_info)
+		return -ENOMEM;
+
+	ieee->LinkDetectInfo.SlotIndex = 0;
+	ieee->LinkDetectInfo.SlotNum = 2;
+	ieee->LinkDetectInfo.NumRecvBcnInPeriod = 0;
+	ieee->LinkDetectInfo.NumRecvDataInPeriod = 0;
+	ieee->LinkDetectInfo.NumTxOkInPeriod = 0;
+	ieee->LinkDetectInfo.NumRxOkInPeriod = 0;
+	ieee->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	ieee->bIsAggregateFrame = false;
+	ieee->assoc_id = 0;
+	ieee->queue_stop = 0;
+	ieee->scanning_continue = 0;
+	ieee->softmac_features = 0;
+	ieee->wap_set = 0;
+	ieee->ssid_set = 0;
+	ieee->proto_started = 0;
+	ieee->proto_stoppping = 0;
+	ieee->basic_rate = RTLLIB_DEFAULT_BASIC_RATE;
+	ieee->rate = 22;
+	ieee->ps = RTLLIB_PS_DISABLED;
+	ieee->sta_sleep = LPS_IS_WAKE;
+
+	ieee->Regdot11HTOperationalRateSet[0] = 0xff;
+	ieee->Regdot11HTOperationalRateSet[1] = 0xff;
+	ieee->Regdot11HTOperationalRateSet[4] = 0x01;
+
+	ieee->Regdot11TxHTOperationalRateSet[0] = 0xff;
+	ieee->Regdot11TxHTOperationalRateSet[1] = 0xff;
+	ieee->Regdot11TxHTOperationalRateSet[4] = 0x01;
+
+	ieee->FirstIe_InScan = false;
+	ieee->actscanning = false;
+	ieee->beinretry = false;
+	ieee->is_set_key = false;
+	init_mgmt_queue(ieee);
+
+	ieee->tx_pending.txb = NULL;
+
+	timer_setup(&ieee->associate_timer, rtllib_associate_abort_cb, 0);
+
+	timer_setup(&ieee->beacon_timer, rtllib_send_beacon_cb, 0);
+
+	INIT_DELAYED_WORK_RSL(&ieee->link_change_wq,
+			      (void *)rtllib_link_change_wq, ieee);
+	INIT_DELAYED_WORK_RSL(&ieee->start_ibss_wq,
+			      (void *)rtllib_start_ibss_wq, ieee);
+	INIT_WORK_RSL(&ieee->associate_complete_wq,
+		      (void *)rtllib_associate_complete_wq, ieee);
+	INIT_DELAYED_WORK_RSL(&ieee->associate_procedure_wq,
+			      (void *)rtllib_associate_procedure_wq, ieee);
+	INIT_DELAYED_WORK_RSL(&ieee->softmac_scan_wq,
+			      (void *)rtllib_softmac_scan_wq, ieee);
+	INIT_DELAYED_WORK_RSL(&ieee->associate_retry_wq,
+			      (void *)rtllib_associate_retry_wq, ieee);
+	INIT_WORK_RSL(&ieee->wx_sync_scan_wq, (void *)rtllib_wx_sync_scan_wq,
+		      ieee);
+
+	mutex_init(&ieee->wx_mutex);
+	mutex_init(&ieee->scan_mutex);
+	mutex_init(&ieee->ips_mutex);
+
+	spin_lock_init(&ieee->mgmt_tx_lock);
+	spin_lock_init(&ieee->beacon_lock);
+
+	INIT_WORK(&ieee->ps_task, rtllib_sta_ps);
+
+	return 0;
+}
+
+void rtllib_softmac_free(struct rtllib_device *ieee)
+{
+	mutex_lock(&ieee->wx_mutex);
+	kfree(ieee->dot11d_info);
+	ieee->dot11d_info = NULL;
+	del_timer_sync(&ieee->associate_timer);
+
+	cancel_delayed_work_sync(&ieee->associate_retry_wq);
+	cancel_delayed_work_sync(&ieee->associate_procedure_wq);
+	cancel_delayed_work_sync(&ieee->softmac_scan_wq);
+	cancel_delayed_work_sync(&ieee->start_ibss_wq);
+	cancel_delayed_work_sync(&ieee->hw_wakeup_wq);
+	cancel_delayed_work_sync(&ieee->hw_sleep_wq);
+	cancel_delayed_work_sync(&ieee->link_change_wq);
+	cancel_work_sync(&ieee->associate_complete_wq);
+	cancel_work_sync(&ieee->ips_leave_wq);
+	cancel_work_sync(&ieee->wx_sync_scan_wq);
+	cancel_work_sync(&ieee->ps_task);
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+static inline struct sk_buff *
+rtllib_disauth_skb(struct rtllib_network *beacon,
+		   struct rtllib_device *ieee, u16 asRsn)
+{
+	struct sk_buff *skb;
+	struct rtllib_disauth *disauth;
+	int len = sizeof(struct rtllib_disauth) + ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	disauth = skb_put(skb, sizeof(struct rtllib_disauth));
+	disauth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DEAUTH);
+	disauth->header.duration_id = 0;
+
+	ether_addr_copy(disauth->header.addr1, beacon->bssid);
+	ether_addr_copy(disauth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(disauth->header.addr3, beacon->bssid);
+
+	disauth->reason = cpu_to_le16(asRsn);
+	return skb;
+}
+
+static inline struct sk_buff *
+rtllib_disassociate_skb(struct rtllib_network *beacon,
+			struct rtllib_device *ieee, u16 asRsn)
+{
+	struct sk_buff *skb;
+	struct rtllib_disassoc *disass;
+	int len = sizeof(struct rtllib_disassoc) + ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	disass = skb_put(skb, sizeof(struct rtllib_disassoc));
+	disass->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DISASSOC);
+	disass->header.duration_id = 0;
+
+	ether_addr_copy(disass->header.addr1, beacon->bssid);
+	ether_addr_copy(disass->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(disass->header.addr3, beacon->bssid);
+
+	disass->reason = cpu_to_le16(asRsn);
+	return skb;
+}
+
+void SendDisassociation(struct rtllib_device *ieee, bool deauth, u16 asRsn)
+{
+	struct rtllib_network *beacon = &ieee->current_network;
+	struct sk_buff *skb;
+
+	if (deauth)
+		skb = rtllib_disauth_skb(beacon, ieee, asRsn);
+	else
+		skb = rtllib_disassociate_skb(beacon, ieee, asRsn);
+
+	if (skb)
+		softmac_mgmt_xmit(skb, ieee);
+}
+
+u8 rtllib_ap_sec_type(struct rtllib_device *ieee)
+{
+	static u8 ccmp_ie[4] = {0x00, 0x50, 0xf2, 0x04};
+	static u8 ccmp_rsn_ie[4] = {0x00, 0x0f, 0xac, 0x04};
+	int wpa_ie_len = ieee->wpa_ie_len;
+	struct lib80211_crypt_data *crypt;
+	int encrypt;
+
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+	encrypt = (ieee->current_network.capability & WLAN_CAPABILITY_PRIVACY)
+		  || (ieee->host_encrypt && crypt && crypt->ops &&
+		  (strcmp(crypt->ops->name, "R-WEP") == 0));
+
+	/* simply judge  */
+	if (encrypt && (wpa_ie_len == 0)) {
+		return SEC_ALG_WEP;
+	} else if ((wpa_ie_len != 0)) {
+		if (((ieee->wpa_ie[0] == 0xdd) &&
+		    (!memcmp(&(ieee->wpa_ie[14]), ccmp_ie, 4))) ||
+		    ((ieee->wpa_ie[0] == 0x30) &&
+		    (!memcmp(&ieee->wpa_ie[10], ccmp_rsn_ie, 4))))
+			return SEC_ALG_CCMP;
+		else
+			return SEC_ALG_TKIP;
+	} else {
+		return SEC_ALG_NONE;
+	}
+}
+
+static void rtllib_MgntDisconnectIBSS(struct rtllib_device *rtllib)
+{
+	u8	OpMode;
+	u8	i;
+	bool	bFilterOutNonAssociatedBSSID = false;
+
+	rtllib->state = RTLLIB_NOLINK;
+
+	for (i = 0; i < 6; i++)
+		rtllib->current_network.bssid[i] = 0x55;
+
+	rtllib->OpMode = RT_OP_MODE_NO_LINK;
+	rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_BSSID,
+				rtllib->current_network.bssid);
+	OpMode = RT_OP_MODE_NO_LINK;
+	rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_MEDIA_STATUS, &OpMode);
+	rtllib_stop_send_beacons(rtllib);
+
+	bFilterOutNonAssociatedBSSID = false;
+	rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_CECHK_BSSID,
+				(u8 *)(&bFilterOutNonAssociatedBSSID));
+	notify_wx_assoc_event(rtllib);
+
+}
+
+static void rtllib_MlmeDisassociateRequest(struct rtllib_device *rtllib,
+					   u8 *asSta, u8 asRsn)
+{
+	u8 i;
+	u8	OpMode;
+
+	RemovePeerTS(rtllib, asSta);
+
+	if (memcmp(rtllib->current_network.bssid, asSta, 6) == 0) {
+		rtllib->state = RTLLIB_NOLINK;
+
+		for (i = 0; i < 6; i++)
+			rtllib->current_network.bssid[i] = 0x22;
+		OpMode = RT_OP_MODE_NO_LINK;
+		rtllib->OpMode = RT_OP_MODE_NO_LINK;
+		rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_MEDIA_STATUS,
+					(u8 *)(&OpMode));
+		rtllib_disassociate(rtllib);
+
+		rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_BSSID,
+					rtllib->current_network.bssid);
+
+	}
+
+}
+
+static void
+rtllib_MgntDisconnectAP(
+	struct rtllib_device *rtllib,
+	u8 asRsn
+)
+{
+	bool bFilterOutNonAssociatedBSSID = false;
+
+	bFilterOutNonAssociatedBSSID = false;
+	rtllib->SetHwRegHandler(rtllib->dev, HW_VAR_CECHK_BSSID,
+				(u8 *)(&bFilterOutNonAssociatedBSSID));
+	rtllib_MlmeDisassociateRequest(rtllib, rtllib->current_network.bssid,
+				       asRsn);
+
+	rtllib->state = RTLLIB_NOLINK;
+}
+
+bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn)
+{
+	if (rtllib->ps != RTLLIB_PS_DISABLED)
+		rtllib->sta_wake_up(rtllib->dev);
+
+	if (rtllib->state == RTLLIB_LINKED) {
+		if (rtllib->iw_mode == IW_MODE_ADHOC)
+			rtllib_MgntDisconnectIBSS(rtllib);
+		if (rtllib->iw_mode == IW_MODE_INFRA)
+			rtllib_MgntDisconnectAP(rtllib, asRsn);
+
+	}
+
+	return true;
+}
+EXPORT_SYMBOL(rtllib_MgntDisconnect);
+
+void notify_wx_assoc_event(struct rtllib_device *ieee)
+{
+	union iwreq_data wrqu;
+
+	if (ieee->cannot_notify)
+		return;
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	if (ieee->state == RTLLIB_LINKED)
+		memcpy(wrqu.ap_addr.sa_data, ieee->current_network.bssid,
+		       ETH_ALEN);
+	else {
+
+		netdev_info(ieee->dev, "%s(): Tell user space disconnected\n",
+			    __func__);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
+	}
+	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
+}
+EXPORT_SYMBOL(notify_wx_assoc_event);
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
new file mode 100644
index 000000000..1e5ad3b47
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -0,0 +1,635 @@
+// SPDX-License-Identifier: GPL-2.0
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Some pieces of code might be stolen from ipw2100 driver
+ * copyright of who own it's copyright ;-)
+ *
+ * PS wx handler mostly stolen from hostap, copyright who
+ * own it's copyright ;-)
+ */
+#include <linux/etherdevice.h>
+
+#include "rtllib.h"
+#include "dot11d.h"
+/* FIXME: add A freqs */
+
+const long rtllib_wlan_frequencies[] = {
+	2412, 2417, 2422, 2427,
+	2432, 2437, 2442, 2447,
+	2452, 2457, 2462, 2467,
+	2472, 2484
+};
+EXPORT_SYMBOL(rtllib_wlan_frequencies);
+
+
+int rtllib_wx_set_freq(struct rtllib_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct iw_freq *fwrq = &wrqu->freq;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->iw_mode == IW_MODE_INFRA) {
+		ret = 0;
+		goto out;
+	}
+
+	/* if setting by freq convert to channel */
+	if (fwrq->e == 1) {
+		if ((fwrq->m >= (int)2.412e8 &&
+		     fwrq->m <= (int)2.487e8)) {
+			int f = fwrq->m / 100000;
+			int c = 0;
+
+			while ((c < 14) && (f != rtllib_wlan_frequencies[c]))
+				c++;
+
+			/* hack to fall through */
+			fwrq->e = 0;
+			fwrq->m = c + 1;
+		}
+	}
+
+	if (fwrq->e > 0 || fwrq->m > 14 || fwrq->m < 1) {
+		ret = -EOPNOTSUPP;
+		goto out;
+
+	} else { /* Set the channel */
+
+		if (ieee->active_channel_map[fwrq->m] != 1) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ieee->current_network.channel = fwrq->m;
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+		if (ieee->iw_mode == IW_MODE_ADHOC ||
+		    ieee->iw_mode == IW_MODE_MASTER)
+			if (ieee->state == RTLLIB_LINKED) {
+				rtllib_stop_send_beacons(ieee);
+				rtllib_start_send_beacons(ieee);
+			}
+	}
+
+	ret = 0;
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(rtllib_wx_set_freq);
+
+
+int rtllib_wx_get_freq(struct rtllib_device *ieee,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct iw_freq *fwrq = &wrqu->freq;
+
+	if (ieee->current_network.channel == 0)
+		return -1;
+	fwrq->m = rtllib_wlan_frequencies[ieee->current_network.channel-1] *
+		  100000;
+	fwrq->e = 1;
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_freq);
+
+int rtllib_wx_get_wap(struct rtllib_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	unsigned long flags;
+
+	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state != RTLLIB_LINKED &&
+		ieee->state != RTLLIB_LINKED_SCANNING &&
+		ieee->wap_set == 0)
+
+		eth_zero_addr(wrqu->ap_addr.sa_data);
+	else
+		memcpy(wrqu->ap_addr.sa_data,
+		       ieee->current_network.bssid, ETH_ALEN);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_wap);
+
+
+int rtllib_wx_set_wap(struct rtllib_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+
+	int ret = 0;
+	unsigned long flags;
+
+	short ifup = ieee->proto_started;
+	struct sockaddr *temp = (struct sockaddr *)awrq;
+
+	rtllib_stop_scan_syncro(ieee);
+
+	mutex_lock(&ieee->wx_mutex);
+	/* use ifconfig hw ether */
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		ret = -1;
+		goto out;
+	}
+
+	if (temp->sa_family != ARPHRD_ETHER) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (is_zero_ether_addr(temp->sa_data)) {
+		spin_lock_irqsave(&ieee->lock, flags);
+		ether_addr_copy(ieee->current_network.bssid, temp->sa_data);
+		ieee->wap_set = 0;
+		spin_unlock_irqrestore(&ieee->lock, flags);
+		ret = -1;
+		goto out;
+	}
+
+
+	if (ifup)
+		rtllib_stop_protocol(ieee, true);
+
+	/* just to avoid to give inconsistent infos in the
+	 * get wx method. not really needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	ieee->cannot_notify = false;
+	ether_addr_copy(ieee->current_network.bssid, temp->sa_data);
+	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (ifup)
+		rtllib_start_protocol(ieee);
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(rtllib_wx_set_wap);
+
+int rtllib_wx_get_essid(struct rtllib_device *ieee, struct iw_request_info *a,
+			 union iwreq_data *wrqu, char *b)
+{
+	int len, ret = 0;
+	unsigned long flags;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->current_network.ssid[0] == '\0' ||
+		ieee->current_network.ssid_len == 0) {
+		ret = -1;
+		goto out;
+	}
+
+	if (ieee->state != RTLLIB_LINKED &&
+		ieee->state != RTLLIB_LINKED_SCANNING &&
+		ieee->ssid_set == 0) {
+		ret = -1;
+		goto out;
+	}
+	len = ieee->current_network.ssid_len;
+	wrqu->essid.length = len;
+	strncpy(b, ieee->current_network.ssid, len);
+	wrqu->essid.flags = 1;
+
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return ret;
+
+}
+EXPORT_SYMBOL(rtllib_wx_get_essid);
+
+int rtllib_wx_set_rate(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+
+	u32 target_rate = wrqu->bitrate.value;
+
+	ieee->rate = target_rate/100000;
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_rate);
+
+int rtllib_wx_get_rate(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	u32 tmp_rate;
+
+	tmp_rate = TxCountToDataRate(ieee,
+				     ieee->softmac_stats.CurrentShowTxate);
+	wrqu->bitrate.value = tmp_rate * 500000;
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_rate);
+
+
+int rtllib_wx_set_rts(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	if (wrqu->rts.disabled || !wrqu->rts.fixed)
+		ieee->rts = DEFAULT_RTS_THRESHOLD;
+	else {
+		if (wrqu->rts.value < MIN_RTS_THRESHOLD ||
+				wrqu->rts.value > MAX_RTS_THRESHOLD)
+			return -EINVAL;
+		ieee->rts = wrqu->rts.value;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_rts);
+
+int rtllib_wx_get_rts(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->rts.value = ieee->rts;
+	wrqu->rts.fixed = 0;	/* no auto select */
+	wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD);
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_rts);
+
+int rtllib_wx_set_mode(struct rtllib_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int set_mode_status = 0;
+
+	rtllib_stop_scan_syncro(ieee);
+	mutex_lock(&ieee->wx_mutex);
+	switch (wrqu->mode) {
+	case IW_MODE_MONITOR:
+	case IW_MODE_ADHOC:
+	case IW_MODE_INFRA:
+		break;
+	case IW_MODE_AUTO:
+		wrqu->mode = IW_MODE_INFRA;
+		break;
+	default:
+		set_mode_status = -EINVAL;
+		goto out;
+	}
+
+	if (wrqu->mode == ieee->iw_mode)
+		goto out;
+
+	if (wrqu->mode == IW_MODE_MONITOR) {
+		ieee->dev->type = ARPHRD_IEEE80211;
+		rtllib_EnableNetMonitorMode(ieee->dev, false);
+	} else {
+		ieee->dev->type = ARPHRD_ETHER;
+		if (ieee->iw_mode == IW_MODE_MONITOR)
+			rtllib_DisableNetMonitorMode(ieee->dev, false);
+	}
+
+	if (!ieee->proto_started) {
+		ieee->iw_mode = wrqu->mode;
+	} else {
+		rtllib_stop_protocol(ieee, true);
+		ieee->iw_mode = wrqu->mode;
+		rtllib_start_protocol(ieee);
+	}
+
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return set_mode_status;
+}
+EXPORT_SYMBOL(rtllib_wx_set_mode);
+
+void rtllib_wx_sync_scan_wq(void *data)
+{
+	struct rtllib_device *ieee = container_of_work_rsl(data,
+				     struct rtllib_device, wx_sync_scan_wq);
+	short chan;
+	enum ht_extchnl_offset chan_offset = 0;
+	enum ht_channel_width bandwidth = 0;
+	int b40M = 0;
+
+	if (!(ieee->softmac_features & IEEE_SOFTMAC_SCAN)) {
+		rtllib_start_scan_syncro(ieee, 0);
+		goto out;
+	}
+
+	chan = ieee->current_network.channel;
+
+	if (ieee->LeisurePSLeave)
+		ieee->LeisurePSLeave(ieee->dev);
+	/* notify AP to be in PS mode */
+	rtllib_sta_ps_send_null_frame(ieee, 1);
+	rtllib_sta_ps_send_null_frame(ieee, 1);
+
+	rtllib_stop_all_queues(ieee);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+	rtllib_stop_send_beacons(ieee);
+	ieee->state = RTLLIB_LINKED_SCANNING;
+	ieee->link_change(ieee->dev);
+	/* wait for ps packet to be kicked out successfully */
+	msleep(50);
+
+	if (ieee->ScanOperationBackupHandler)
+		ieee->ScanOperationBackupHandler(ieee->dev, SCAN_OPT_BACKUP);
+
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT &&
+	    ieee->pHTInfo->bCurBW40MHz) {
+		b40M = 1;
+		chan_offset = ieee->pHTInfo->CurSTAExtChnlOffset;
+		bandwidth = (enum ht_channel_width)ieee->pHTInfo->bCurBW40MHz;
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20,
+				       HT_EXTCHNL_OFFSET_NO_EXT);
+	}
+
+	rtllib_start_scan_syncro(ieee, 0);
+
+	if (b40M) {
+		if (chan_offset == HT_EXTCHNL_OFFSET_UPPER)
+			ieee->set_chan(ieee->dev, chan + 2);
+		else if (chan_offset == HT_EXTCHNL_OFFSET_LOWER)
+			ieee->set_chan(ieee->dev, chan - 2);
+		else
+			ieee->set_chan(ieee->dev, chan);
+		ieee->SetBWModeHandler(ieee->dev, bandwidth, chan_offset);
+	} else {
+		ieee->set_chan(ieee->dev, chan);
+	}
+
+	if (ieee->ScanOperationBackupHandler)
+		ieee->ScanOperationBackupHandler(ieee->dev, SCAN_OPT_RESTORE);
+
+	ieee->state = RTLLIB_LINKED;
+	ieee->link_change(ieee->dev);
+
+	/* Notify AP that I wake up again */
+	rtllib_sta_ps_send_null_frame(ieee, 0);
+
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod == 0 ||
+	    ieee->LinkDetectInfo.NumRecvDataInPeriod == 0) {
+		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
+		ieee->LinkDetectInfo.NumRecvDataInPeriod = 1;
+	}
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	if (ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
+		rtllib_start_send_beacons(ieee);
+
+	rtllib_wake_all_queues(ieee);
+
+out:
+	mutex_unlock(&ieee->wx_mutex);
+
+}
+
+int rtllib_wx_set_scan(struct rtllib_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret = 0;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)) {
+		ret = -1;
+		goto out;
+	}
+
+	if (ieee->state == RTLLIB_LINKED) {
+		schedule_work(&ieee->wx_sync_scan_wq);
+		/* intentionally forget to up sem */
+		return 0;
+	}
+
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(rtllib_wx_set_scan);
+
+int rtllib_wx_set_essid(struct rtllib_device *ieee,
+			struct iw_request_info *a,
+			union iwreq_data *wrqu, char *extra)
+{
+
+	int ret = 0, len;
+	short proto_started;
+	unsigned long flags;
+
+	rtllib_stop_scan_syncro(ieee);
+	mutex_lock(&ieee->wx_mutex);
+
+	proto_started = ieee->proto_started;
+
+	len = min_t(__u16, wrqu->essid.length, IW_ESSID_MAX_SIZE);
+
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		ret = -1;
+		goto out;
+	}
+
+	if (proto_started)
+		rtllib_stop_protocol(ieee, true);
+
+
+	/* this is just to be sure that the GET wx callback
+	 * has consistent infos. not needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (wrqu->essid.flags && wrqu->essid.length) {
+		strncpy(ieee->current_network.ssid, extra, len);
+		ieee->current_network.ssid_len = len;
+		ieee->cannot_notify = false;
+		ieee->ssid_set = 1;
+	} else {
+		ieee->ssid_set = 0;
+		ieee->current_network.ssid[0] = '\0';
+		ieee->current_network.ssid_len = 0;
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (proto_started)
+		rtllib_start_protocol(ieee);
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(rtllib_wx_set_essid);
+
+int rtllib_wx_get_mode(struct rtllib_device *ieee, struct iw_request_info *a,
+		       union iwreq_data *wrqu, char *b)
+{
+	wrqu->mode = ieee->iw_mode;
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_mode);
+
+int rtllib_wx_set_rawtx(struct rtllib_device *ieee,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+
+	int *parms = (int *)extra;
+	int enable = (parms[0] > 0);
+	short prev = ieee->raw_tx;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (enable)
+		ieee->raw_tx = 1;
+	else
+		ieee->raw_tx = 0;
+
+	netdev_info(ieee->dev, "raw TX is %s\n",
+		    ieee->raw_tx ? "enabled" : "disabled");
+
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		if (prev == 0 && ieee->raw_tx) {
+			if (ieee->data_hard_resume)
+				ieee->data_hard_resume(ieee->dev);
+
+			netif_carrier_on(ieee->dev);
+		}
+
+		if (prev && ieee->raw_tx == 1)
+			netif_carrier_off(ieee->dev);
+	}
+
+	mutex_unlock(&ieee->wx_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_rawtx);
+
+int rtllib_wx_get_name(struct rtllib_device *ieee, struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra)
+{
+	const char *b = ieee->modulation & RTLLIB_CCK_MODULATION ? "b" : "";
+	const char *g = ieee->modulation & RTLLIB_OFDM_MODULATION ? "g" : "";
+	const char *n = ieee->mode & (IEEE_N_24G | IEEE_N_5G) ? "n" : "";
+
+	scnprintf(wrqu->name, sizeof(wrqu->name), "802.11%s%s%s", b, g, n);
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_name);
+
+
+/* this is mostly stolen from hostap */
+int rtllib_wx_set_power(struct rtllib_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	if ((!ieee->sta_wake_up) ||
+	    (!ieee->enter_sleep_state) ||
+	    (!ieee->ps_is_queue_empty)) {
+		netdev_warn(ieee->dev,
+			    "%s(): PS mode is tried to be use but driver missed a callback\n",
+			    __func__);
+		return -1;
+	}
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (wrqu->power.disabled) {
+		ieee->ps = RTLLIB_PS_DISABLED;
+		goto exit;
+	}
+	if (wrqu->power.flags & IW_POWER_TIMEOUT) {
+		ieee->ps_timeout = wrqu->power.value / 1000;
+	}
+
+	if (wrqu->power.flags & IW_POWER_PERIOD)
+		ieee->ps_period = wrqu->power.value / 1000;
+
+	switch (wrqu->power.flags & IW_POWER_MODE) {
+	case IW_POWER_UNICAST_R:
+		ieee->ps = RTLLIB_PS_UNICAST;
+		break;
+	case IW_POWER_MULTICAST_R:
+		ieee->ps = RTLLIB_PS_MBCAST;
+		break;
+	case IW_POWER_ALL_R:
+		ieee->ps = RTLLIB_PS_UNICAST | RTLLIB_PS_MBCAST;
+		break;
+
+	case IW_POWER_ON:
+		break;
+
+	default:
+		ret = -EINVAL;
+		goto exit;
+
+	}
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+
+}
+EXPORT_SYMBOL(rtllib_wx_set_power);
+
+/* this is stolen from hostap */
+int rtllib_wx_get_power(struct rtllib_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->ps == RTLLIB_PS_DISABLED) {
+		wrqu->power.disabled = 1;
+		goto exit;
+	}
+
+	wrqu->power.disabled = 0;
+
+	if ((wrqu->power.flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
+		wrqu->power.flags = IW_POWER_TIMEOUT;
+		wrqu->power.value = ieee->ps_timeout * 1000;
+	} else {
+		wrqu->power.flags = IW_POWER_PERIOD;
+		wrqu->power.value = ieee->ps_period * 1000;
+	}
+
+	if ((ieee->ps & (RTLLIB_PS_MBCAST | RTLLIB_PS_UNICAST)) ==
+	    (RTLLIB_PS_MBCAST | RTLLIB_PS_UNICAST))
+		wrqu->power.flags |= IW_POWER_ALL_R;
+	else if (ieee->ps & RTLLIB_PS_MBCAST)
+		wrqu->power.flags |= IW_POWER_MULTICAST_R;
+	else
+		wrqu->power.flags |= IW_POWER_UNICAST_R;
+
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+	return 0;
+
+}
+EXPORT_SYMBOL(rtllib_wx_get_power);
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
new file mode 100644
index 000000000..e30702058
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -0,0 +1,970 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2003 - 2004 Intel Corporation. All rights reserved.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * Few modifications for Realtek's Wi-Fi drivers by
+ * Andrea Merello <andrea.merello@gmail.com>
+ *
+ * A special thanks goes to Realtek for their support !
+ */
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <linux/if_vlan.h>
+
+#include "rtllib.h"
+
+/* 802.11 Data Frame
+ *
+ *
+ * 802.11 frame_control for data frames - 2 bytes
+ *      ,--------------------------------------------------------------------.
+ * bits | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |  9 |  a |  b  |  c  |  d  | e  |
+ *      |---|---|---|---|---|---|---|---|---|----|----|-----|-----|-----|----|
+ * val  | 0 | 0 | 0 | 1 | x | 0 | 0 | 0 | 1 |  0 |  x |  x  |  x  |  x  | x  |
+ *      |---|---|---|---|---|---|---|---|---|----|----|-----|-----|-----|----|
+ * desc |  ver  | type  |  ^-subtype-^  |to |from|more|retry| pwr |more |wep |
+ *      |       |       | x=0 data      |DS | DS |frag|     | mgm |data |    |
+ *      |       |       | x=1 data+ack  |   |    |    |     |     |     |    |
+ *      '--------------------------------------------------------------------'
+ *                                           /\
+ *                                           |
+ * 802.11 Data Frame                         |
+ *          ,--------- 'ctrl' expands to >---'
+ *          |
+ *       ,--'---,-------------------------------------------------------------.
+ * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *       |------|------|---------|---------|---------|------|---------|------|
+ * Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
+ *       |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *       `--------------------------------------------------|         |------'
+ * Total: 28 non-data bytes                                 `----.----'
+ *                                                               |
+ *        .- 'Frame data' expands to <---------------------------'
+ *        |
+ *        V
+ *       ,---------------------------------------------------.
+ * Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
+ *       |------|------|---------|----------|------|---------|
+ * Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
+ *       | DSAP | SSAP |         |          |      | Packet  |
+ *       | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
+ *       `-----------------------------------------|         |
+ * Total: 8 non-data bytes                         `----.----'
+ *                                                      |
+ *        .- 'IP Packet' expands, if WEP enabled, to <--'
+ *        |
+ *        V
+ *       ,-----------------------.
+ * Bytes |  4  |   0-2296  |  4  |
+ *       |-----|-----------|-----|
+ * Desc. | IV  | Encrypted | ICV |
+ *       |     | IP Packet |     |
+ *       `-----------------------'
+ * Total: 8 non-data bytes
+ *
+ *
+ * 802.3 Ethernet Data Frame
+ *
+ *       ,-----------------------------------------.
+ * Bytes |   6   |   6   |  2   |  Variable |   4  |
+ *       |-------|-------|------|-----------|------|
+ * Desc. | Dest. | Source| Type | IP Packet |  fcs |
+ *       |  MAC  |  MAC  |      |	   |      |
+ *       `-----------------------------------------'
+ * Total: 18 non-data bytes
+ *
+ * In the event that fragmentation is required, the incoming payload is split
+ * into N parts of size ieee->fts.  The first fragment contains the SNAP header
+ * and the remaining packets are just data.
+ *
+ * If encryption is enabled, each fragment payload size is reduced by enough
+ * space to add the prefix and postfix (IV and ICV totalling 8 bytes in
+ * the case of WEP) So if you have 1500 bytes of payload with ieee->fts set to
+ * 500 without encryption it will take 3 frames.  With WEP it will take 4 frames
+ * as the payload of each frame is reduced to 492 bytes.
+ *
+ * SKB visualization
+ *
+ * ,- skb->data
+ * |
+ * |    ETHERNET HEADER        ,-<-- PAYLOAD
+ * |                           |     14 bytes from skb->data
+ * |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
+ * |                       | | |
+ * |,-Dest.--. ,--Src.---. | | |
+ * |  6 bytes| | 6 bytes | | | |
+ * v         | |         | | | |
+ * 0         | v       1 | v | v           2
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
+ *     ^     | ^         | ^ |
+ *     |     | |         | | |
+ *     |     | |         | `T' <---- 2 bytes for Type
+ *     |     | |         |
+ *     |     | '---SNAP--' <-------- 6 bytes for SNAP
+ *     |     |
+ *     `-IV--' <-------------------- 4 bytes for IV (WEP)
+ *
+ *      SNAP HEADER
+ *
+ */
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static int rtllib_put_snap(u8 *data, u16 h_proto)
+{
+	struct rtllib_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct rtllib_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+int rtllib_encrypt_fragment(struct rtllib_device *ieee, struct sk_buff *frag,
+			    int hdr_len)
+{
+	struct lib80211_crypt_data *crypt = NULL;
+	int res;
+
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+
+	if (!(crypt && crypt->ops)) {
+		netdev_info(ieee->dev, "=========>%s(), crypt is null\n",
+			    __func__);
+		return -1;
+	}
+	/* To encrypt, frame format is:
+	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes)
+	 */
+
+	/* Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
+	 * call both MSDU and MPDU encryption functions from here.
+	 */
+	atomic_inc(&crypt->refcnt);
+	res = 0;
+	if (crypt->ops->encrypt_msdu)
+		res = crypt->ops->encrypt_msdu(frag, hdr_len, crypt->priv);
+	if (res == 0 && crypt->ops->encrypt_mpdu)
+		res = crypt->ops->encrypt_mpdu(frag, hdr_len, crypt->priv);
+
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		netdev_info(ieee->dev, "%s: Encryption failed: len=%d.\n",
+			    ieee->dev->name, frag->len);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+void rtllib_txb_free(struct rtllib_txb *txb)
+{
+	if (unlikely(!txb))
+		return;
+	kfree(txb);
+}
+
+static struct rtllib_txb *rtllib_alloc_txb(int nr_frags, int txb_size,
+					   gfp_t gfp_mask)
+{
+	struct rtllib_txb *txb;
+	int i;
+
+	txb = kzalloc(struct_size(txb, fragments, nr_frags), gfp_mask);
+	if (!txb)
+		return NULL;
+
+	txb->nr_frags = nr_frags;
+	txb->frag_size = cpu_to_le16(txb_size);
+
+	for (i = 0; i < nr_frags; i++) {
+		txb->fragments[i] = dev_alloc_skb(txb_size);
+		if (unlikely(!txb->fragments[i]))
+			goto err_free;
+		memset(txb->fragments[i]->cb, 0, sizeof(txb->fragments[i]->cb));
+	}
+
+	return txb;
+
+err_free:
+	while (--i >= 0)
+		dev_kfree_skb_any(txb->fragments[i]);
+	kfree(txb);
+
+	return NULL;
+}
+
+static int rtllib_classify(struct sk_buff *skb, u8 bIsAmsdu)
+{
+	struct ethhdr *eth;
+	struct iphdr *ip;
+
+	eth = (struct ethhdr *)skb->data;
+	if (eth->h_proto != htons(ETH_P_IP))
+		return 0;
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("%s: ", __func__, DUMP_PREFIX_NONE, skb->data,
+			     skb->len);
+#endif
+	ip = ip_hdr(skb);
+	switch (ip->tos & 0xfc) {
+	case 0x20:
+		return 2;
+	case 0x40:
+		return 1;
+	case 0x60:
+		return 3;
+	case 0x80:
+		return 4;
+	case 0xa0:
+		return 5;
+	case 0xc0:
+		return 6;
+	case 0xe0:
+		return 7;
+	default:
+		return 0;
+	}
+}
+
+static void rtllib_tx_query_agg_cap(struct rtllib_device *ieee,
+				    struct sk_buff *skb,
+				    struct cb_desc *tcb_desc)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	struct tx_ts_record *pTxTs = NULL;
+	struct rtllib_hdr_1addr *hdr = (struct rtllib_hdr_1addr *)skb->data;
+
+	if (rtllib_act_scanning(ieee, false))
+		return;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+	if (!IsQoSDataFrame(skb->data))
+		return;
+	if (is_multicast_ether_addr(hdr->addr1))
+		return;
+
+	if (tcb_desc->bdhcp || ieee->CntAfterLink < 2)
+		return;
+
+	if (pHTInfo->iot_action & HT_IOT_ACT_TX_NO_AGGREGATION)
+		return;
+
+	if (!ieee->GetNmodeSupportBySecCfg(ieee->dev))
+		return;
+	if (pHTInfo->bCurrentAMPDUEnable) {
+		if (!GetTs(ieee, (struct ts_common_info **)(&pTxTs), hdr->addr1,
+		    skb->priority, TX_DIR, true)) {
+			netdev_info(ieee->dev, "%s: can't get TS\n", __func__);
+			return;
+		}
+		if (!pTxTs->TxAdmittedBARecord.b_valid) {
+			if (ieee->wpa_ie_len && (ieee->pairwise_key_type ==
+			    KEY_TYPE_NA)) {
+				;
+			} else if (tcb_desc->bdhcp == 1) {
+				;
+			} else if (!pTxTs->bDisable_AddBa) {
+				TsStartAddBaProcess(ieee, pTxTs);
+			}
+			goto FORCED_AGG_SETTING;
+		} else if (!pTxTs->bUsingBa) {
+			if (SN_LESS(pTxTs->TxAdmittedBARecord.ba_start_seq_ctrl.field.seq_num,
+			   (pTxTs->TxCurSeq+1)%4096))
+				pTxTs->bUsingBa = true;
+			else
+				goto FORCED_AGG_SETTING;
+		}
+		if (ieee->iw_mode == IW_MODE_INFRA) {
+			tcb_desc->bAMPDUEnable = true;
+			tcb_desc->ampdu_factor = pHTInfo->CurrentAMPDUFactor;
+			tcb_desc->ampdu_density = pHTInfo->current_mpdu_density;
+		}
+	}
+FORCED_AGG_SETTING:
+	switch (pHTInfo->ForcedAMPDUMode) {
+	case HT_AGG_AUTO:
+		break;
+
+	case HT_AGG_FORCE_ENABLE:
+		tcb_desc->bAMPDUEnable = true;
+		tcb_desc->ampdu_density = pHTInfo->forced_mpdu_density;
+		tcb_desc->ampdu_factor = pHTInfo->forced_ampdu_factor;
+		break;
+
+	case HT_AGG_FORCE_DISABLE:
+		tcb_desc->bAMPDUEnable = false;
+		tcb_desc->ampdu_density = 0;
+		tcb_desc->ampdu_factor = 0;
+		break;
+	}
+}
+
+static void rtllib_query_ShortPreambleMode(struct rtllib_device *ieee,
+					   struct cb_desc *tcb_desc)
+{
+	tcb_desc->bUseShortPreamble = false;
+	if (tcb_desc->data_rate == 2)
+		return;
+	else if (ieee->current_network.capability &
+		 WLAN_CAPABILITY_SHORT_PREAMBLE)
+		tcb_desc->bUseShortPreamble = true;
+}
+
+static void rtllib_query_HTCapShortGI(struct rtllib_device *ieee,
+				      struct cb_desc *tcb_desc)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	tcb_desc->bUseShortGI		= false;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+
+	if (pHTInfo->forced_short_gi) {
+		tcb_desc->bUseShortGI = true;
+		return;
+	}
+
+	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI40MHz)
+		tcb_desc->bUseShortGI = true;
+	else if (!pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI20MHz)
+		tcb_desc->bUseShortGI = true;
+}
+
+static void rtllib_query_BandwidthMode(struct rtllib_device *ieee,
+				       struct cb_desc *tcb_desc)
+{
+	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
+	tcb_desc->bPacketBW = false;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+
+	if (tcb_desc->bMulticast || tcb_desc->bBroadcast)
+		return;
+
+	if ((tcb_desc->data_rate & 0x80) == 0)
+		return;
+	if (pHTInfo->bCurBW40MHz && pHTInfo->cur_tx_bw40mhz &&
+	    !ieee->bandwidth_auto_switch.bforced_tx20Mhz)
+		tcb_desc->bPacketBW = true;
+}
+
+static void rtllib_query_protectionmode(struct rtllib_device *ieee,
+					struct cb_desc *tcb_desc,
+					struct sk_buff *skb)
+{
+	struct rt_hi_throughput *pHTInfo;
+
+	tcb_desc->bRTSSTBC			= false;
+	tcb_desc->bRTSUseShortGI		= false;
+	tcb_desc->bCTSEnable			= false;
+	tcb_desc->RTSSC				= 0;
+	tcb_desc->bRTSBW			= false;
+
+	if (tcb_desc->bBroadcast || tcb_desc->bMulticast)
+		return;
+
+	if (is_broadcast_ether_addr(skb->data+16))
+		return;
+
+	if (ieee->mode < IEEE_N_24G) {
+		if (skb->len > ieee->rts) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+		} else if (ieee->current_network.buseprotection) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->bCTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+		}
+		return;
+	}
+
+	pHTInfo = ieee->pHTInfo;
+
+	while (true) {
+		if (pHTInfo->iot_action & HT_IOT_ACT_FORCED_CTS2SELF) {
+			tcb_desc->bCTSEnable	= true;
+			tcb_desc->rts_rate  =	MGN_24M;
+			tcb_desc->bRTSEnable = true;
+			break;
+		} else if (pHTInfo->iot_action & (HT_IOT_ACT_FORCED_RTS |
+			   HT_IOT_ACT_PURE_N_MODE)) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->rts_rate  =	MGN_24M;
+			break;
+		}
+		if (ieee->current_network.buseprotection) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->bCTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+			break;
+		}
+		if (pHTInfo->bCurrentHTSupport  && pHTInfo->bEnableHT) {
+			u8 HTOpMode = pHTInfo->current_op_mode;
+
+			if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 ||
+			     HTOpMode == 3)) ||
+			     (!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
+				tcb_desc->rts_rate = MGN_24M;
+				tcb_desc->bRTSEnable = true;
+				break;
+			}
+		}
+		if (skb->len > ieee->rts) {
+			tcb_desc->rts_rate = MGN_24M;
+			tcb_desc->bRTSEnable = true;
+			break;
+		}
+		if (tcb_desc->bAMPDUEnable) {
+			tcb_desc->rts_rate = MGN_24M;
+			tcb_desc->bRTSEnable = false;
+			break;
+		}
+		goto NO_PROTECTION;
+	}
+	if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		tcb_desc->bUseShortPreamble = true;
+	if (ieee->iw_mode == IW_MODE_MASTER)
+		goto NO_PROTECTION;
+	return;
+NO_PROTECTION:
+	tcb_desc->bRTSEnable	= false;
+	tcb_desc->bCTSEnable	= false;
+	tcb_desc->rts_rate	= 0;
+	tcb_desc->RTSSC		= 0;
+	tcb_desc->bRTSBW	= false;
+}
+
+
+static void rtllib_txrate_selectmode(struct rtllib_device *ieee,
+				     struct cb_desc *tcb_desc)
+{
+	if (ieee->bTxDisableRateFallBack)
+		tcb_desc->bTxDisableRateFallBack = true;
+
+	if (ieee->bTxUseDriverAssingedRate)
+		tcb_desc->bTxUseDriverAssingedRate = true;
+	if (!tcb_desc->bTxDisableRateFallBack ||
+	    !tcb_desc->bTxUseDriverAssingedRate) {
+		if (ieee->iw_mode == IW_MODE_INFRA ||
+		    ieee->iw_mode == IW_MODE_ADHOC)
+			tcb_desc->RATRIndex = 0;
+	}
+}
+
+static u16 rtllib_query_seqnum(struct rtllib_device *ieee, struct sk_buff *skb,
+			       u8 *dst)
+{
+	u16 seqnum = 0;
+
+	if (is_multicast_ether_addr(dst))
+		return 0;
+	if (IsQoSDataFrame(skb->data)) {
+		struct tx_ts_record *pTS = NULL;
+
+		if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
+		    skb->priority, TX_DIR, true))
+			return 0;
+		seqnum = pTS->TxCurSeq;
+		pTS->TxCurSeq = (pTS->TxCurSeq+1)%4096;
+		return seqnum;
+	}
+	return 0;
+}
+
+static int wme_downgrade_ac(struct sk_buff *skb)
+{
+	switch (skb->priority) {
+	case 6:
+	case 7:
+		skb->priority = 5; /* VO -> VI */
+		return 0;
+	case 4:
+	case 5:
+		skb->priority = 3; /* VI -> BE */
+		return 0;
+	case 0:
+	case 3:
+		skb->priority = 1; /* BE -> BK */
+		return 0;
+	default:
+		return -1;
+	}
+}
+
+static u8 rtllib_current_rate(struct rtllib_device *ieee)
+{
+	if (ieee->mode & IEEE_MODE_MASK)
+		return ieee->rate;
+
+	if (ieee->HTCurrentOperaRate)
+		return ieee->HTCurrentOperaRate;
+	else
+		return ieee->rate & 0x7F;
+}
+
+static int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
+{
+	struct rtllib_device *ieee = (struct rtllib_device *)
+				     netdev_priv_rsl(dev);
+	struct rtllib_txb *txb = NULL;
+	struct rtllib_hdr_3addrqos *frag_hdr;
+	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
+	unsigned long flags;
+	struct net_device_stats *stats = &ieee->stats;
+	int ether_type = 0, encrypt;
+	int bytes, fc, qos_ctl = 0, hdr_len;
+	struct sk_buff *skb_frag;
+	struct rtllib_hdr_3addrqos header = { /* Ensure zero initialized */
+		.duration_id = 0,
+		.seq_ctl = 0,
+		.qos_ctl = 0
+	};
+	int qos_activated = ieee->current_network.qos_data.active;
+	u8 dest[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	struct lib80211_crypt_data *crypt = NULL;
+	struct cb_desc *tcb_desc;
+	u8 bIsMulticast = false;
+	u8 IsAmsdu = false;
+	bool	bdhcp = false;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* If there is no driver handler to take the TXB, don't bother
+	 * creating it...
+	 */
+	if ((!ieee->hard_start_xmit && !(ieee->softmac_features &
+	   IEEE_SOFTMAC_TX_QUEUE)) ||
+	   ((!ieee->softmac_data_hard_start_xmit &&
+	   (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
+		netdev_warn(ieee->dev, "No xmit handler.\n");
+		goto success;
+	}
+
+
+	if (likely(ieee->raw_tx == 0)) {
+		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
+			goto success;
+		}
+		/* Save source and destination addresses */
+		ether_addr_copy(dest, skb->data);
+		ether_addr_copy(src, skb->data + ETH_ALEN);
+
+		memset(skb->cb, 0, sizeof(skb->cb));
+		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+		if (ieee->iw_mode == IW_MODE_MONITOR) {
+			txb = rtllib_alloc_txb(1, skb->len, GFP_ATOMIC);
+			if (unlikely(!txb)) {
+				netdev_warn(ieee->dev,
+					    "Could not allocate TXB\n");
+				goto failed;
+			}
+
+			txb->encrypted = 0;
+			txb->payload_size = cpu_to_le16(skb->len);
+			skb_put_data(txb->fragments[0], skb->data, skb->len);
+
+			goto success;
+		}
+
+		if (skb->len > 282) {
+			if (ether_type == ETH_P_IP) {
+				const struct iphdr *ip = (struct iphdr *)
+					((u8 *)skb->data+14);
+				if (ip->protocol == IPPROTO_UDP) {
+					struct udphdr *udp;
+
+					udp = (struct udphdr *)((u8 *)ip +
+					      (ip->ihl << 2));
+					if (((((u8 *)udp)[1] == 68) &&
+					   (((u8 *)udp)[3] == 67)) ||
+					   ((((u8 *)udp)[1] == 67) &&
+					   (((u8 *)udp)[3] == 68))) {
+						bdhcp = true;
+						ieee->LPSDelayCnt = 200;
+					}
+				}
+			} else if (ether_type == ETH_P_ARP) {
+				netdev_info(ieee->dev,
+					    "=================>DHCP Protocol start tx ARP pkt!!\n");
+				bdhcp = true;
+				ieee->LPSDelayCnt =
+					 ieee->current_network.tim.tim_count;
+			}
+		}
+
+		skb->priority = rtllib_classify(skb, IsAmsdu);
+		crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
+		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
+			ieee->host_encrypt && crypt && crypt->ops;
+		if (!encrypt && ieee->ieee802_1x &&
+		    ieee->drop_unencrypted && ether_type != ETH_P_PAE) {
+			stats->tx_dropped++;
+			goto success;
+		}
+		if (crypt && !encrypt && ether_type == ETH_P_PAE) {
+			struct eapol *eap = (struct eapol *)(skb->data +
+				sizeof(struct ethhdr) - SNAP_SIZE -
+				sizeof(u16));
+			netdev_dbg(ieee->dev,
+				   "TX: IEEE 802.11 EAPOL frame: %s\n",
+				   eap_get_type(eap->type));
+		}
+
+		/* Advance the SKB to the start of the payload */
+		skb_pull(skb, sizeof(struct ethhdr));
+
+		/* Determine total amount of storage required for TXB packets */
+		bytes = skb->len + SNAP_SIZE + sizeof(u16);
+
+		if (encrypt)
+			fc = RTLLIB_FTYPE_DATA | RTLLIB_FCTL_WEP;
+		else
+			fc = RTLLIB_FTYPE_DATA;
+
+		if (qos_activated)
+			fc |= RTLLIB_STYPE_QOS_DATA;
+		else
+			fc |= RTLLIB_STYPE_DATA;
+
+		if (ieee->iw_mode == IW_MODE_INFRA) {
+			fc |= RTLLIB_FCTL_TODS;
+			/* To DS: Addr1 = BSSID, Addr2 = SA,
+			 * Addr3 = DA
+			 */
+			ether_addr_copy(header.addr1,
+					ieee->current_network.bssid);
+			ether_addr_copy(header.addr2, src);
+			if (IsAmsdu)
+				ether_addr_copy(header.addr3,
+						ieee->current_network.bssid);
+			else
+				ether_addr_copy(header.addr3, dest);
+		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+			/* not From/To DS: Addr1 = DA, Addr2 = SA,
+			 * Addr3 = BSSID
+			 */
+			ether_addr_copy(header.addr1, dest);
+			ether_addr_copy(header.addr2, src);
+			ether_addr_copy(header.addr3,
+					ieee->current_network.bssid);
+		}
+
+		bIsMulticast = is_multicast_ether_addr(header.addr1);
+
+		header.frame_ctl = cpu_to_le16(fc);
+
+		/* Determine fragmentation size based on destination (multicast
+		 * and broadcast are not fragmented)
+		 */
+		if (bIsMulticast) {
+			frag_size = MAX_FRAG_THRESHOLD;
+			qos_ctl |= QOS_CTL_NOTCONTAIN_ACK;
+		} else {
+			frag_size = ieee->fts;
+			qos_ctl = 0;
+		}
+
+		if (qos_activated) {
+			hdr_len = RTLLIB_3ADDR_LEN + 2;
+
+			/* in case we are a client verify acm is not set for this ac */
+			while (unlikely(ieee->wmm_acm & (0x01 << skb->priority))) {
+				netdev_info(ieee->dev, "skb->priority = %x\n",
+						skb->priority);
+				if (wme_downgrade_ac(skb))
+					break;
+				netdev_info(ieee->dev, "converted skb->priority = %x\n",
+					   skb->priority);
+			}
+
+			qos_ctl |= skb->priority;
+			header.qos_ctl = cpu_to_le16(qos_ctl & RTLLIB_QOS_TID);
+
+		} else {
+			hdr_len = RTLLIB_3ADDR_LEN;
+		}
+		/* Determine amount of payload per fragment.  Regardless of if
+		 * this stack is providing the full 802.11 header, one will
+		 * eventually be affixed to this fragment -- so we must account
+		 * for it when determining the amount of payload space.
+		 */
+		bytes_per_frag = frag_size - hdr_len;
+		if (ieee->config &
+		   (CFG_RTLLIB_COMPUTE_FCS | CFG_RTLLIB_RESERVE_FCS))
+			bytes_per_frag -= RTLLIB_FCS_LEN;
+
+		/* Each fragment may need to have room for encrypting
+		 * pre/postfix
+		 */
+		if (encrypt) {
+			bytes_per_frag -= crypt->ops->extra_mpdu_prefix_len +
+				crypt->ops->extra_mpdu_postfix_len +
+				crypt->ops->extra_msdu_prefix_len +
+				crypt->ops->extra_msdu_postfix_len;
+		}
+		/* Number of fragments is the total bytes_per_frag /
+		 * payload_per_fragment
+		 */
+		nr_frags = bytes / bytes_per_frag;
+		bytes_last_frag = bytes % bytes_per_frag;
+		if (bytes_last_frag)
+			nr_frags++;
+		else
+			bytes_last_frag = bytes_per_frag;
+
+		/* When we allocate the TXB we allocate enough space for the
+		 * reserve and full fragment bytes (bytes_per_frag doesn't
+		 * include prefix, postfix, header, FCS, etc.)
+		 */
+		txb = rtllib_alloc_txb(nr_frags, frag_size +
+				       ieee->tx_headroom, GFP_ATOMIC);
+		if (unlikely(!txb)) {
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
+			goto failed;
+		}
+		txb->encrypted = encrypt;
+		txb->payload_size = cpu_to_le16(bytes);
+
+		if (qos_activated)
+			txb->queue_index = UP2AC(skb->priority);
+		else
+			txb->queue_index = WME_AC_BE;
+
+		for (i = 0; i < nr_frags; i++) {
+			skb_frag = txb->fragments[i];
+			tcb_desc = (struct cb_desc *)(skb_frag->cb +
+				    MAX_DEV_ADDR_SIZE);
+			if (qos_activated) {
+				skb_frag->priority = skb->priority;
+				tcb_desc->queue_index =  UP2AC(skb->priority);
+			} else {
+				skb_frag->priority = WME_AC_BE;
+				tcb_desc->queue_index = WME_AC_BE;
+			}
+			skb_reserve(skb_frag, ieee->tx_headroom);
+
+			if (encrypt) {
+				if (ieee->hwsec_active)
+					tcb_desc->bHwSec = 1;
+				else
+					tcb_desc->bHwSec = 0;
+				skb_reserve(skb_frag,
+					    crypt->ops->extra_mpdu_prefix_len +
+					    crypt->ops->extra_msdu_prefix_len);
+			} else {
+				tcb_desc->bHwSec = 0;
+			}
+			frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
+
+			/* If this is not the last fragment, then add the
+			 * MOREFRAGS bit to the frame control
+			 */
+			if (i != nr_frags - 1) {
+				frag_hdr->frame_ctl = cpu_to_le16(
+					fc | RTLLIB_FCTL_MOREFRAGS);
+				bytes = bytes_per_frag;
+
+			} else {
+				/* The last fragment has the remaining length */
+				bytes = bytes_last_frag;
+			}
+			if ((qos_activated) && (!bIsMulticast)) {
+				frag_hdr->seq_ctl =
+					 cpu_to_le16(rtllib_query_seqnum(ieee, skb_frag,
+							     header.addr1));
+				frag_hdr->seq_ctl =
+					 cpu_to_le16(le16_to_cpu(frag_hdr->seq_ctl)<<4 | i);
+			} else {
+				frag_hdr->seq_ctl =
+					 cpu_to_le16(ieee->seq_ctrl[0]<<4 | i);
+			}
+			/* Put a SNAP header on the first fragment */
+			if (i == 0) {
+				rtllib_put_snap(
+					skb_put(skb_frag, SNAP_SIZE +
+					sizeof(u16)), ether_type);
+				bytes -= SNAP_SIZE + sizeof(u16);
+			}
+
+			skb_put_data(skb_frag, skb->data, bytes);
+
+			/* Advance the SKB... */
+			skb_pull(skb, bytes);
+
+			/* Encryption routine will move the header forward in
+			 * order to insert the IV between the header and the
+			 * payload
+			 */
+			if (encrypt)
+				rtllib_encrypt_fragment(ieee, skb_frag,
+							hdr_len);
+			if (ieee->config &
+			   (CFG_RTLLIB_COMPUTE_FCS | CFG_RTLLIB_RESERVE_FCS))
+				skb_put(skb_frag, 4);
+		}
+
+		if ((qos_activated) && (!bIsMulticast)) {
+			if (ieee->seq_ctrl[UP2AC(skb->priority) + 1] == 0xFFF)
+				ieee->seq_ctrl[UP2AC(skb->priority) + 1] = 0;
+			else
+				ieee->seq_ctrl[UP2AC(skb->priority) + 1]++;
+		} else {
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+		}
+	} else {
+		if (unlikely(skb->len < sizeof(struct rtllib_hdr_3addr))) {
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
+			goto success;
+		}
+
+		txb = rtllib_alloc_txb(1, skb->len, GFP_ATOMIC);
+		if (!txb) {
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
+			goto failed;
+		}
+
+		txb->encrypted = 0;
+		txb->payload_size = cpu_to_le16(skb->len);
+		skb_put_data(txb->fragments[0], skb->data, skb->len);
+	}
+
+ success:
+	if (txb) {
+		tcb_desc = (struct cb_desc *)
+				(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->bTxEnableFwCalcDur = 1;
+		tcb_desc->priority = skb->priority;
+
+		if (ether_type == ETH_P_PAE) {
+			if (ieee->pHTInfo->iot_action &
+			    HT_IOT_ACT_WA_IOT_Broadcom) {
+				tcb_desc->data_rate =
+					 MgntQuery_TxRateExcludeCCKRates(ieee);
+				tcb_desc->bTxDisableRateFallBack = false;
+			} else {
+				tcb_desc->data_rate = ieee->basic_rate;
+				tcb_desc->bTxDisableRateFallBack = 1;
+			}
+
+
+			tcb_desc->RATRIndex = 7;
+			tcb_desc->bTxUseDriverAssingedRate = 1;
+		} else {
+			if (is_multicast_ether_addr(header.addr1))
+				tcb_desc->bMulticast = 1;
+			if (is_broadcast_ether_addr(header.addr1))
+				tcb_desc->bBroadcast = 1;
+			rtllib_txrate_selectmode(ieee, tcb_desc);
+			if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
+				tcb_desc->data_rate = ieee->basic_rate;
+			else
+				tcb_desc->data_rate = rtllib_current_rate(ieee);
+
+			if (bdhcp) {
+				if (ieee->pHTInfo->iot_action &
+				    HT_IOT_ACT_WA_IOT_Broadcom) {
+					tcb_desc->data_rate =
+					   MgntQuery_TxRateExcludeCCKRates(ieee);
+					tcb_desc->bTxDisableRateFallBack = false;
+				} else {
+					tcb_desc->data_rate = MGN_1M;
+					tcb_desc->bTxDisableRateFallBack = 1;
+				}
+
+
+				tcb_desc->RATRIndex = 7;
+				tcb_desc->bTxUseDriverAssingedRate = 1;
+				tcb_desc->bdhcp = 1;
+			}
+
+			rtllib_query_ShortPreambleMode(ieee, tcb_desc);
+			rtllib_tx_query_agg_cap(ieee, txb->fragments[0],
+						tcb_desc);
+			rtllib_query_HTCapShortGI(ieee, tcb_desc);
+			rtllib_query_BandwidthMode(ieee, tcb_desc);
+			rtllib_query_protectionmode(ieee, tcb_desc,
+						    txb->fragments[0]);
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	dev_kfree_skb_any(skb);
+	if (txb) {
+		if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE) {
+			dev->stats.tx_packets++;
+			dev->stats.tx_bytes += le16_to_cpu(txb->payload_size);
+			rtllib_softmac_xmit(txb, ieee);
+		} else {
+			if ((*ieee->hard_start_xmit)(txb, dev) == 0) {
+				stats->tx_packets++;
+				stats->tx_bytes += le16_to_cpu(txb->payload_size);
+				return 0;
+			}
+			rtllib_txb_free(txb);
+		}
+	}
+
+	return 0;
+
+ failed:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	netif_stop_queue(dev);
+	stats->tx_errors++;
+	return 1;
+
+}
+
+netdev_tx_t rtllib_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	memset(skb->cb, 0, sizeof(skb->cb));
+	return rtllib_xmit_inter(skb, dev) ? NETDEV_TX_BUSY : NETDEV_TX_OK;
+}
+EXPORT_SYMBOL(rtllib_xmit);
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
new file mode 100644
index 000000000..da2c41c9b
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -0,0 +1,787 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright(c) 2004 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are based on the WEP enablement code provided by the
+ * Host AP project hostap-drivers v0.1.3
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+#include <linux/wireless.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include "rtllib.h"
+
+static const char * const rtllib_modes[] = {
+	"a", "b", "g", "?", "N-24G", "N-5G"
+};
+
+#define MAX_CUSTOM_LEN 64
+static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
+					   char *start, char *stop,
+					   struct rtllib_network *network,
+					   struct iw_request_info *info)
+{
+	char custom[MAX_CUSTOM_LEN];
+	char proto_name[IFNAMSIZ];
+	char *pname = proto_name;
+	char *p;
+	struct iw_event iwe;
+	int i, j;
+	u16 max_rate, rate;
+	static u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+
+	/* First entry *MUST* be the AP MAC address */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+	ether_addr_copy(iwe.u.ap_addr.sa_data, network->bssid);
+	start = iwe_stream_add_event_rsl(info, start, stop,
+					 &iwe, IW_EV_ADDR_LEN);
+	/* Remaining entries will be displayed in the order we provide them */
+
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	if (network->ssid_len > 0) {
+		iwe.u.data.length = min_t(u8, network->ssid_len, 32);
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe,
+						 network->ssid);
+	} else if (network->hidden_ssid_len == 0) {
+		iwe.u.data.length = sizeof("<hidden>");
+		start = iwe_stream_add_point_rsl(info, start, stop,
+						 &iwe, "<hidden>");
+	} else {
+		iwe.u.data.length = min_t(u8, network->hidden_ssid_len, 32);
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe,
+						 network->hidden_ssid);
+	}
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	for (i = 0; i < ARRAY_SIZE(rtllib_modes); i++) {
+		if (network->mode & BIT(i)) {
+			strcpy(pname, rtllib_modes[i]);
+			pname += strlen(rtllib_modes[i]);
+		}
+	}
+	*pname = '\0';
+	snprintf(iwe.u.name, IFNAMSIZ, "IEEE802.11%s", proto_name);
+	start = iwe_stream_add_event_rsl(info, start, stop,
+					 &iwe, IW_EV_CHAR_LEN);
+	/* Add mode */
+	iwe.cmd = SIOCGIWMODE;
+	if (network->capability &
+	    (WLAN_CAPABILITY_ESS | WLAN_CAPABILITY_IBSS)) {
+		if (network->capability & WLAN_CAPABILITY_ESS)
+			iwe.u.mode = IW_MODE_MASTER;
+		else
+			iwe.u.mode = IW_MODE_ADHOC;
+		start = iwe_stream_add_event_rsl(info, start, stop,
+						 &iwe, IW_EV_UINT_LEN);
+	}
+
+	/* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+	iwe.u.freq.m = network->channel;
+	iwe.u.freq.e = 0;
+	iwe.u.freq.i = 0;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe,
+					 IW_EV_FREQ_LEN);
+
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	if (network->capability & WLAN_CAPABILITY_PRIVACY)
+		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+	else
+		iwe.u.data.flags = IW_ENCODE_DISABLED;
+	iwe.u.data.length = 0;
+	start = iwe_stream_add_point_rsl(info, start, stop,
+					 &iwe, network->ssid);
+	/* Add basic and extended rates */
+	max_rate = 0;
+	p = custom;
+	p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom), " Rates (Mb/s): ");
+	for (i = 0, j = 0; i < network->rates_len;) {
+		if (j < network->rates_ex_len &&
+		    ((network->rates_ex[j] & 0x7F) <
+		     (network->rates[i] & 0x7F)))
+			rate = network->rates_ex[j++] & 0x7F;
+		else
+			rate = network->rates[i++] & 0x7F;
+		if (rate > max_rate)
+			max_rate = rate;
+		p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+	}
+	for (; j < network->rates_ex_len; j++) {
+		rate = network->rates_ex[j] & 0x7F;
+		p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+		if (rate > max_rate)
+			max_rate = rate;
+	}
+
+	if (network->mode >= IEEE_N_24G) {
+		struct ht_capab_ele *ht_cap = NULL;
+		bool is40M = false, isShortGI = false;
+		u8 max_mcs = 0;
+
+		if (!memcmp(network->bssht.bd_ht_cap_buf, EWC11NHTCap, 4))
+			ht_cap = (struct ht_capab_ele *)
+				 &network->bssht.bd_ht_cap_buf[4];
+		else
+			ht_cap = (struct ht_capab_ele *)
+				 &network->bssht.bd_ht_cap_buf[0];
+		is40M = (ht_cap->ChlWidth) ? 1 : 0;
+		isShortGI = (ht_cap->ChlWidth) ?
+				((ht_cap->ShortGI40Mhz) ? 1 : 0) :
+				((ht_cap->ShortGI20Mhz) ? 1 : 0);
+
+		max_mcs = HTGetHighestMCSRate(ieee, ht_cap->MCS,
+					      MCS_FILTER_ALL);
+		rate = MCS_DATA_RATE[is40M][isShortGI][max_mcs & 0x7f];
+		if (rate > max_rate)
+			max_rate = rate;
+	}
+	iwe.cmd = SIOCGIWRATE;
+	iwe.u.bitrate.disabled = 0;
+	iwe.u.bitrate.fixed = 0;
+	iwe.u.bitrate.value = max_rate * 500000;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_PARAM_LEN);
+	iwe.cmd = IWEVCUSTOM;
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point_rsl(info, start, stop,
+						 &iwe, custom);
+	/* Add quality statistics */
+	/* TODO: Fix these values... */
+	iwe.cmd = IWEVQUAL;
+	iwe.u.qual.qual = network->stats.signal;
+	iwe.u.qual.level = network->stats.rssi;
+	iwe.u.qual.noise = network->stats.noise;
+	iwe.u.qual.updated = network->stats.mask & RTLLIB_STATMASK_WEMASK;
+	if (!(network->stats.mask & RTLLIB_STATMASK_RSSI))
+		iwe.u.qual.updated |= IW_QUAL_LEVEL_INVALID;
+	if (!(network->stats.mask & RTLLIB_STATMASK_NOISE))
+		iwe.u.qual.updated |= IW_QUAL_NOISE_INVALID;
+	if (!(network->stats.mask & RTLLIB_STATMASK_SIGNAL))
+		iwe.u.qual.updated |= IW_QUAL_QUAL_INVALID;
+	iwe.u.qual.updated = 7;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe, custom);
+
+	memset(&iwe, 0, sizeof(iwe));
+	if (network->wpa_ie_len) {
+		char buf[MAX_WPA_IE_LEN];
+
+		memcpy(buf, network->wpa_ie, network->wpa_ie_len);
+		iwe.cmd = IWEVGENIE;
+		iwe.u.data.length = network->wpa_ie_len;
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe, buf);
+	}
+	memset(&iwe, 0, sizeof(iwe));
+	if (network->rsn_ie_len) {
+		char buf[MAX_WPA_IE_LEN];
+
+		memcpy(buf, network->rsn_ie, network->rsn_ie_len);
+		iwe.cmd = IWEVGENIE;
+		iwe.u.data.length = network->rsn_ie_len;
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe, buf);
+	}
+
+	/* add info for WZC */
+	memset(&iwe, 0, sizeof(iwe));
+	if (network->wzc_ie_len) {
+		char buf[MAX_WZC_IE_LEN];
+
+		memcpy(buf, network->wzc_ie, network->wzc_ie_len);
+		iwe.cmd = IWEVGENIE;
+		iwe.u.data.length = network->wzc_ie_len;
+		start = iwe_stream_add_point_rsl(info, start, stop, &iwe, buf);
+	}
+
+	/* Add EXTRA: Age to display seconds since last beacon/probe response
+	 * for given network.
+	 */
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+	p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+		      " Last beacon: %lums ago",
+		      (jiffies - network->last_scanned) / (HZ / 100));
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point_rsl(info, start, stop,
+						 &iwe, custom);
+
+	return start;
+}
+
+int rtllib_wx_get_scan(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra)
+{
+	struct rtllib_network *network;
+	unsigned long flags;
+
+	char *ev = extra;
+	char *stop = ev + wrqu->data.length;
+	int i = 0;
+	int err = 0;
+
+	netdev_dbg(ieee->dev, "Getting scan\n");
+	mutex_lock(&ieee->wx_mutex);
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	list_for_each_entry(network, &ieee->network_list, list) {
+		i++;
+		if ((stop - ev) < 200) {
+			err = -E2BIG;
+			break;
+		}
+		if (ieee->scan_age == 0 ||
+		    time_after(network->last_scanned + ieee->scan_age, jiffies))
+			ev = rtl819x_translate_scan(ieee, ev, stop, network,
+						    info);
+		else
+			netdev_dbg(ieee->dev,
+				   "Network '%s ( %pM)' hidden due to age (%lums).\n",
+				   escape_essid(network->ssid,
+						network->ssid_len),
+				   network->bssid,
+				   (jiffies - network->last_scanned) /
+				   (HZ / 100));
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	mutex_unlock(&ieee->wx_mutex);
+	wrqu->data.length = ev -  extra;
+	wrqu->data.flags = 0;
+
+	netdev_dbg(ieee->dev, "%s(): %d networks returned.\n", __func__, i);
+
+	return err;
+}
+EXPORT_SYMBOL(rtllib_wx_get_scan);
+
+int rtllib_wx_set_encode(struct rtllib_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &wrqu->encoding;
+	struct net_device *dev = ieee->dev;
+	struct rtllib_security sec = {
+		.flags = 0
+	};
+	int i, key, key_provided, len;
+	struct lib80211_crypt_data **crypt;
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > NUM_WEP_KEYS)
+			return -EINVAL;
+		key--;
+		key_provided = 1;
+	} else {
+		key_provided = 0;
+		key = ieee->crypt_info.tx_keyidx;
+	}
+
+	netdev_dbg(ieee->dev, "Key: %d [%s]\n", key, key_provided ?
+			   "provided" : "default");
+	crypt = &ieee->crypt_info.crypt[key];
+	if (erq->flags & IW_ENCODE_DISABLED) {
+		if (key_provided && *crypt) {
+			netdev_dbg(ieee->dev,
+				   "Disabling encryption on key %d.\n", key);
+			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
+		} else {
+			netdev_dbg(ieee->dev, "Disabling encryption.\n");
+		}
+
+		/* Check all the keys to see if any are still configured,
+		 * and if no key index was provided, de-init them all
+		 */
+		for (i = 0; i < NUM_WEP_KEYS; i++) {
+			if (ieee->crypt_info.crypt[i]) {
+				if (key_provided)
+					break;
+				lib80211_crypt_delayed_deinit(&ieee->crypt_info,
+							      &ieee->crypt_info.crypt[i]);
+			}
+		}
+
+		if (i == NUM_WEP_KEYS) {
+			sec.enabled = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_ENABLED | SEC_LEVEL;
+		}
+
+		goto done;
+	}
+
+	sec.enabled = 1;
+	sec.flags |= SEC_ENABLED;
+
+	if (*crypt && (*crypt)->ops &&
+	    strcmp((*crypt)->ops->name, "R-WEP") != 0) {
+		/* changing to use WEP; deinit previously used algorithm
+		 * on this key
+		 */
+		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
+	}
+
+	if (!*crypt) {
+		struct lib80211_crypt_data *new_crypt;
+
+		/* take WEP into use */
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
+		if (!new_crypt)
+			return -ENOMEM;
+		new_crypt->ops = lib80211_get_crypto_ops("R-WEP");
+		if (!new_crypt->ops) {
+			request_module("rtllib_crypt_wep");
+			new_crypt->ops = lib80211_get_crypto_ops("R-WEP");
+		}
+
+		if (new_crypt->ops)
+			new_crypt->priv = new_crypt->ops->init(key);
+
+		if (!new_crypt->ops || !new_crypt->priv) {
+			kfree(new_crypt);
+			new_crypt = NULL;
+
+			netdev_warn(dev,
+				    "%s: could not initialize WEP: load module rtllib_crypt_wep\n",
+				    dev->name);
+			return -EOPNOTSUPP;
+		}
+		*crypt = new_crypt;
+	}
+
+	/* If a new key was provided, set it up */
+	if (erq->length > 0) {
+		len = erq->length <= 5 ? 5 : 13;
+		memcpy(sec.keys[key], keybuf, erq->length);
+		if (len > erq->length)
+			memset(sec.keys[key] + erq->length, 0,
+			       len - erq->length);
+		netdev_dbg(ieee->dev, "Setting key %d to '%s' (%d:%d bytes)\n",
+			   key, escape_essid(sec.keys[key], len), erq->length,
+			   len);
+		sec.key_sizes[key] = len;
+		(*crypt)->ops->set_key(sec.keys[key], len, NULL,
+				       (*crypt)->priv);
+		sec.flags |= (1 << key);
+		/* This ensures a key will be activated if no key is
+		 * explicitly set
+		 */
+		if (key == sec.active_key)
+			sec.flags |= SEC_ACTIVE_KEY;
+		ieee->crypt_info.tx_keyidx = key;
+
+	} else {
+		len = (*crypt)->ops->get_key(sec.keys[key], WEP_KEY_LEN,
+					     NULL, (*crypt)->priv);
+		if (len == 0) {
+			/* Set a default key of all 0 */
+			netdev_info(ieee->dev, "Setting key %d to all zero.\n", key);
+
+			memset(sec.keys[key], 0, 13);
+			(*crypt)->ops->set_key(sec.keys[key], 13, NULL,
+					       (*crypt)->priv);
+			sec.key_sizes[key] = 13;
+			sec.flags |= (1 << key);
+		}
+
+		/* No key data - just set the default TX key index */
+		if (key_provided) {
+			netdev_dbg(ieee->dev,
+				   "Setting key %d as default Tx key.\n", key);
+			ieee->crypt_info.tx_keyidx = key;
+			sec.active_key = key;
+			sec.flags |= SEC_ACTIVE_KEY;
+		}
+	}
+ done:
+	ieee->open_wep = !(erq->flags & IW_ENCODE_RESTRICTED);
+	ieee->auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN :
+			  WLAN_AUTH_SHARED_KEY;
+	sec.auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+	sec.flags |= SEC_AUTH_MODE;
+	netdev_dbg(ieee->dev, "Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
+			   "OPEN" : "SHARED KEY");
+
+	/* For now we just support WEP, so only set that security level...
+	 * TODO: When WPA is added this is one place that needs to change
+	 */
+	sec.flags |= SEC_LEVEL;
+	sec.level = SEC_LEVEL_1; /* 40 and 104 bit WEP */
+
+	if (ieee->set_security)
+		ieee->set_security(dev, &sec);
+
+	/* Do not reset port if card is in Managed mode since resetting will
+	 * generate new IEEE 802.11 authentication which may end up in looping
+	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
+	 * configuration (for example... Prism2), implement the reset_port in
+	 * the callbacks structures used to initialize the 802.11 stack.
+	 */
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port && ieee->reset_port(dev)) {
+		netdev_dbg(dev, "%s: reset_port failed\n", dev->name);
+		return -EINVAL;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_encode);
+
+int rtllib_wx_get_encode(struct rtllib_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &wrqu->encoding;
+	int len, key;
+	struct lib80211_crypt_data *crypt;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > NUM_WEP_KEYS)
+			return -EINVAL;
+		key--;
+	} else {
+		key = ieee->crypt_info.tx_keyidx;
+	}
+	crypt = ieee->crypt_info.crypt[key];
+
+	erq->flags = key + 1;
+
+	if (!crypt || !crypt->ops) {
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		return 0;
+	}
+	len = crypt->ops->get_key(keybuf, SCM_KEY_LEN, NULL, crypt->priv);
+
+	erq->length = max(len, 0);
+
+	erq->flags |= IW_ENCODE_ENABLED;
+
+	if (ieee->open_wep)
+		erq->flags |= IW_ENCODE_OPEN;
+	else
+		erq->flags |= IW_ENCODE_RESTRICTED;
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_get_encode);
+
+int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct net_device *dev = ieee->dev;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int i, idx;
+	int group_key = 0;
+	const char *alg, *module;
+	struct lib80211_crypto_ops *ops;
+	struct lib80211_crypt_data **crypt;
+
+	struct rtllib_security sec = {
+		.flags = 0,
+	};
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx) {
+		if (idx < 1 || idx > NUM_WEP_KEYS)
+			return -EINVAL;
+		idx--;
+	} else {
+		idx = ieee->crypt_info.tx_keyidx;
+	}
+	if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
+		crypt = &ieee->crypt_info.crypt[idx];
+		group_key = 1;
+	} else {
+		/* some Cisco APs use idx>0 for unicast in dynamic WEP */
+		if (idx != 0 && ext->alg != IW_ENCODE_ALG_WEP)
+			return -EINVAL;
+		if (ieee->iw_mode == IW_MODE_INFRA)
+			crypt = &ieee->crypt_info.crypt[idx];
+		else
+			return -EINVAL;
+	}
+
+	sec.flags |= SEC_ENABLED;
+	if ((encoding->flags & IW_ENCODE_DISABLED) ||
+	    ext->alg == IW_ENCODE_ALG_NONE) {
+		if (*crypt)
+			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
+
+		for (i = 0; i < NUM_WEP_KEYS; i++) {
+			if (ieee->crypt_info.crypt[i])
+				break;
+		}
+		if (i == NUM_WEP_KEYS) {
+			sec.enabled = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_LEVEL;
+		}
+		goto done;
+	}
+
+	sec.enabled = 1;
+	switch (ext->alg) {
+	case IW_ENCODE_ALG_WEP:
+		alg = "R-WEP";
+		module = "rtllib_crypt_wep";
+		break;
+	case IW_ENCODE_ALG_TKIP:
+		alg = "R-TKIP";
+		module = "rtllib_crypt_tkip";
+		break;
+	case IW_ENCODE_ALG_CCMP:
+		alg = "R-CCMP";
+		module = "rtllib_crypt_ccmp";
+		break;
+	default:
+		netdev_dbg(ieee->dev, "Unknown crypto alg %d\n", ext->alg);
+		ret = -EINVAL;
+		goto done;
+	}
+	netdev_dbg(dev, "alg name:%s\n", alg);
+
+	ops = lib80211_get_crypto_ops(alg);
+	if (!ops) {
+		char tempbuf[100];
+
+		memset(tempbuf, 0x00, 100);
+		sprintf(tempbuf, "%s", module);
+		request_module("%s", tempbuf);
+		ops = lib80211_get_crypto_ops(alg);
+	}
+	if (!ops) {
+		netdev_info(dev, "========>unknown crypto alg %d\n", ext->alg);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if (!*crypt || (*crypt)->ops != ops) {
+		struct lib80211_crypt_data *new_crypt;
+
+		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
+
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
+		if (!new_crypt) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		new_crypt->ops = ops;
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv = new_crypt->ops->init(idx);
+
+		if (!new_crypt->priv) {
+			kfree(new_crypt);
+			ret = -EINVAL;
+			goto done;
+		}
+		*crypt = new_crypt;
+	}
+
+	if (ext->key_len > 0 && (*crypt)->ops->set_key &&
+	    (*crypt)->ops->set_key(ext->key, ext->key_len, ext->rx_seq,
+				   (*crypt)->priv) < 0) {
+		netdev_info(dev, "key setting failed\n");
+		ret = -EINVAL;
+		goto done;
+	}
+	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+		ieee->crypt_info.tx_keyidx = idx;
+		sec.active_key = idx;
+		sec.flags |= SEC_ACTIVE_KEY;
+	}
+	if (ext->alg != IW_ENCODE_ALG_NONE) {
+		sec.key_sizes[idx] = ext->key_len;
+		sec.flags |= (1 << idx);
+		if (ext->alg == IW_ENCODE_ALG_WEP) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_1;
+		} else if (ext->alg == IW_ENCODE_ALG_TKIP) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_2;
+		} else if (ext->alg == IW_ENCODE_ALG_CCMP) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_3;
+		}
+		/* Don't set sec level for group keys. */
+		if (group_key)
+			sec.flags &= ~SEC_LEVEL;
+	}
+done:
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port && ieee->reset_port(dev)) {
+		netdev_dbg(ieee->dev, "Port reset failed\n");
+		return -EINVAL;
+	}
+	return ret;
+}
+EXPORT_SYMBOL(rtllib_wx_set_encode_ext);
+
+int rtllib_wx_set_mlme(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       union iwreq_data *wrqu, char *extra)
+{
+	u8 i = 0;
+	bool deauth = false;
+	struct iw_mlme *mlme = (struct iw_mlme *)extra;
+
+	if (ieee->state != RTLLIB_LINKED)
+		return -ENOLINK;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	switch (mlme->cmd) {
+	case IW_MLME_DEAUTH:
+		deauth = true;
+		fallthrough;
+	case IW_MLME_DISASSOC:
+		if (deauth)
+			netdev_info(ieee->dev, "disauth packet !\n");
+		else
+			netdev_info(ieee->dev, "dis associate packet!\n");
+
+		ieee->cannot_notify = true;
+
+		SendDisassociation(ieee, deauth, mlme->reason_code);
+		rtllib_disassociate(ieee);
+
+		ieee->wap_set = 0;
+		for (i = 0; i < 6; i++)
+			ieee->current_network.bssid[i] = 0x55;
+
+		ieee->ssid_set = 0;
+		ieee->current_network.ssid[0] = '\0';
+		ieee->current_network.ssid_len = 0;
+		break;
+	default:
+		mutex_unlock(&ieee->wx_mutex);
+		return -EOPNOTSUPP;
+	}
+
+	mutex_unlock(&ieee->wx_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_mlme);
+
+int rtllib_wx_set_auth(struct rtllib_device *ieee,
+		       struct iw_request_info *info,
+		       struct iw_param *data, char *extra)
+{
+	switch (data->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_WPA_VERSION:
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+	case IW_AUTH_CIPHER_GROUP:
+	case IW_AUTH_KEY_MGMT:
+		/* Host AP driver does not use these parameters and allows
+		 * wpa_supplicant to control them internally.
+		 */
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		ieee->tkip_countermeasures = data->value;
+		break;
+	case IW_AUTH_DROP_UNENCRYPTED:
+		ieee->drop_unencrypted = data->value;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		if (data->value & IW_AUTH_ALG_SHARED_KEY) {
+			ieee->open_wep = 0;
+			ieee->auth_mode = 1;
+		} else if (data->value & IW_AUTH_ALG_OPEN_SYSTEM) {
+			ieee->open_wep = 1;
+			ieee->auth_mode = 0;
+		} else if (data->value & IW_AUTH_ALG_LEAP) {
+			ieee->open_wep = 1;
+			ieee->auth_mode = 2;
+		} else {
+			return -EINVAL;
+		}
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		ieee->wpa_enabled = (data->value) ? 1 : 0;
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		ieee->ieee802_1x = data->value;
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		ieee->privacy_invoked = data->value;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_auth);
+
+int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
+{
+	u8 *buf;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (len > MAX_WPA_IE_LEN || (len && !ie))
+		return -EINVAL;
+
+	if (len) {
+		eid = ie[0];
+		if ((eid == MFIE_TYPE_GENERIC) && (!memcmp(&ie[2], wps_oui, 4))) {
+			ieee->wps_ie_len = min_t(size_t, len, MAX_WZC_IE_LEN);
+			buf = kmemdup(ie, ieee->wps_ie_len, GFP_KERNEL);
+			if (!buf)
+				return -ENOMEM;
+			ieee->wps_ie = buf;
+			return 0;
+		}
+	}
+	ieee->wps_ie_len = 0;
+	kfree(ieee->wps_ie);
+	ieee->wps_ie = NULL;
+	if (len) {
+		if (len != ie[1] + 2)
+			return -EINVAL;
+		buf = kmemdup(ie, len, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = len;
+	} else {
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(rtllib_wx_set_gen_ie);
diff --git a/drivers/staging/rtl8192u/Kconfig b/drivers/staging/rtl8192u/Kconfig
new file mode 100644
index 000000000..f3b112a05
--- /dev/null
+++ b/drivers/staging/rtl8192u/Kconfig
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+config RTL8192U
+	tristate "RealTek RTL8192U Wireless LAN NIC driver"
+	depends on PCI && WLAN && USB
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select CRC32
+	select CRYPTO
+	select CRYPTO_AES
+	select CRYPTO_CCM
+	select CRYPTO_LIB_ARC4
diff --git a/drivers/staging/rtl8192u/Makefile b/drivers/staging/rtl8192u/Makefile
new file mode 100644
index 000000000..d32dfd89a
--- /dev/null
+++ b/drivers/staging/rtl8192u/Makefile
@@ -0,0 +1,27 @@
+# SPDX-License-Identifier: GPL-2.0
+NIC_SELECT = RTL8192U
+
+ccflags-y += -DCONFIG_FORCE_HARD_FLOAT=y
+ccflags-y += -DJACKSON_NEW_8187 -DJACKSON_NEW_RX
+ccflags-y += -DTHOMAS_BEACON -DTHOMAS_TASKLET -DTHOMAS_SKB -DTHOMAS_TURBO
+
+r8192u_usb-y := r8192U_core.o r8180_93cx6.o r8192U_wx.o		\
+		  r8190_rtl8256.o r819xU_phy.o r819xU_firmware.o	\
+		  r819xU_cmdpkt.o r8192U_dm.o r819xU_firmware_img.o	\
+		  r8192U_debugfs.o					\
+		  ieee80211/ieee80211_crypt.o				\
+		  ieee80211/ieee80211_crypt_tkip.o			\
+		  ieee80211/ieee80211_crypt_ccmp.o			\
+		  ieee80211/ieee80211_crypt_wep.o			\
+		  ieee80211/ieee80211_rx.o				\
+		  ieee80211/ieee80211_softmac.o				\
+		  ieee80211/ieee80211_tx.o				\
+		  ieee80211/ieee80211_wx.o				\
+		  ieee80211/ieee80211_module.o				\
+		  ieee80211/ieee80211_softmac_wx.o			\
+		  ieee80211/rtl819x_HTProc.o				\
+		  ieee80211/rtl819x_TSProc.o				\
+		  ieee80211/rtl819x_BAProc.o				\
+		  ieee80211/dot11d.o
+
+obj-$(CONFIG_RTL8192U) += r8192u_usb.o
diff --git a/drivers/staging/rtl8192u/authors b/drivers/staging/rtl8192u/authors
new file mode 100644
index 000000000..0fab11228
--- /dev/null
+++ b/drivers/staging/rtl8192u/authors
@@ -0,0 +1 @@
+Andrea Merello <andrea.merello@gmail.com>
diff --git a/drivers/staging/rtl8192u/changes b/drivers/staging/rtl8192u/changes
new file mode 100644
index 000000000..0485d6eec
--- /dev/null
+++ b/drivers/staging/rtl8192u/changes
@@ -0,0 +1,4 @@
+v 0.1
+
+First version.
+This is based on the rtl8180-sa2400 pre-0.22-CVS code..
diff --git a/drivers/staging/rtl8192u/ieee80211/dot11d.c b/drivers/staging/rtl8192u/ieee80211/dot11d.c
new file mode 100644
index 000000000..ddaf66fa0
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/dot11d.c
@@ -0,0 +1,174 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Implement 802.11d. */
+
+#include "dot11d.h"
+
+void rtl8192u_dot11d_init(struct ieee80211_device *ieee)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(ieee);
+
+	dot11d_info->dot11d_enabled = false;
+
+	dot11d_info->state = DOT11D_STATE_NONE;
+	dot11d_info->country_ie_len = 0;
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_pwr_dbm_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	RESET_CIE_WATCHDOG(ieee);
+}
+EXPORT_SYMBOL(rtl8192u_dot11d_init);
+
+/* Reset to the state as we are just entering a regulatory domain. */
+void dot11d_reset(struct ieee80211_device *ieee)
+{
+	u32 i;
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(ieee);
+	/* Clear old channel map */
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_pwr_dbm_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	/* Set new channel map */
+	for (i = 1; i <= 11; i++)
+		(dot11d_info->channel_map)[i] = 1;
+
+	for (i = 12; i <= 14; i++)
+		(dot11d_info->channel_map)[i] = 2;
+
+	dot11d_info->state = DOT11D_STATE_NONE;
+	dot11d_info->country_ie_len = 0;
+	RESET_CIE_WATCHDOG(ieee);
+}
+EXPORT_SYMBOL(dot11d_reset);
+
+/*
+ * Update country IE from Beacon or Probe Resopnse and configure PHY for
+ * operation in the regulatory domain.
+ *
+ * TODO: Configure Tx power.
+ * Assumption:
+ * 1. IS_DOT11D_ENABLE() is TRUE.
+ * 2. Input IE is an valid one.
+ */
+void dot11d_update_country_ie(struct ieee80211_device *dev, u8 *pTaddr,
+			    u16 CoutryIeLen, u8 *pCoutryIe)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+	u8 i, j, NumTriples, MaxChnlNum;
+	struct chnl_txpower_triple *pTriple;
+
+	memset(dot11d_info->channel_map, 0, MAX_CHANNEL_NUMBER + 1);
+	memset(dot11d_info->max_tx_pwr_dbm_list, 0xFF, MAX_CHANNEL_NUMBER + 1);
+	MaxChnlNum = 0;
+	NumTriples = (CoutryIeLen - 3) / 3; /* skip 3-byte country string. */
+	pTriple = (struct chnl_txpower_triple *)(pCoutryIe + 3);
+	for (i = 0; i < NumTriples; i++) {
+		if (MaxChnlNum >= pTriple->first_channel) {
+			/* It is not in a monotonically increasing order, so
+			 * stop processing.
+			 */
+			netdev_err(dev->dev, "%s: Invalid country IE, skip it 1\n", __func__);
+			return;
+		}
+		if (MAX_CHANNEL_NUMBER < (pTriple->first_channel + pTriple->num_channels)) {
+			/* It is not a valid set of channel id, so stop
+			 * processing.
+			 */
+			netdev_err(dev->dev, "%s: Invalid country IE, skip it 2\n", __func__);
+			return;
+		}
+
+		for (j = 0; j < pTriple->num_channels; j++) {
+			dot11d_info->channel_map[pTriple->first_channel + j] = 1;
+			dot11d_info->max_tx_pwr_dbm_list[pTriple->first_channel + j] = pTriple->max_tx_pwr_dbm;
+			MaxChnlNum = pTriple->first_channel + j;
+		}
+
+		pTriple = (struct chnl_txpower_triple *)((u8 *)pTriple + 3);
+	}
+	netdev_info(dev->dev, "Channel List:");
+	for (i = 1; i <= MAX_CHANNEL_NUMBER; i++)
+		if (dot11d_info->channel_map[i] > 0)
+			netdev_info(dev->dev, " %d", i);
+	netdev_info(dev->dev, "\n");
+
+	UPDATE_CIE_SRC(dev, pTaddr);
+
+	dot11d_info->country_ie_len = CoutryIeLen;
+	memcpy(dot11d_info->country_ie_buf, pCoutryIe, CoutryIeLen);
+	dot11d_info->state = DOT11D_STATE_LEARNED;
+}
+EXPORT_SYMBOL(dot11d_update_country_ie);
+
+u8 dot11d_get_max_tx_pwr_in_dbm(struct ieee80211_device *dev, u8 Channel)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+	u8 MaxTxPwrInDbm = 255;
+
+	if (Channel > MAX_CHANNEL_NUMBER) {
+		netdev_err(dev->dev, "%s: Invalid Channel\n", __func__);
+		return MaxTxPwrInDbm;
+	}
+	if (dot11d_info->channel_map[Channel])
+		MaxTxPwrInDbm = dot11d_info->max_tx_pwr_dbm_list[Channel];
+
+	return MaxTxPwrInDbm;
+}
+EXPORT_SYMBOL(dot11d_get_max_tx_pwr_in_dbm);
+
+void dot11d_scan_complete(struct ieee80211_device *dev)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+
+	switch (dot11d_info->state) {
+	case DOT11D_STATE_LEARNED:
+		dot11d_info->state = DOT11D_STATE_DONE;
+		break;
+
+	case DOT11D_STATE_DONE:
+		if (GET_CIE_WATCHDOG(dev) == 0) {
+			/* Reset country IE if previous one is gone. */
+			dot11d_reset(dev);
+		}
+		break;
+	case DOT11D_STATE_NONE:
+		break;
+	}
+}
+EXPORT_SYMBOL(dot11d_scan_complete);
+
+int is_legal_channel(struct ieee80211_device *dev, u8 channel)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+
+	if (channel > MAX_CHANNEL_NUMBER) {
+		netdev_err(dev->dev, "%s: Invalid Channel\n", __func__);
+		return 0;
+	}
+	if (dot11d_info->channel_map[channel] > 0)
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL(is_legal_channel);
+
+int to_legal_channel(struct ieee80211_device *dev, u8 channel)
+{
+	struct rt_dot11d_info *dot11d_info = GET_DOT11D_INFO(dev);
+	u8 default_chn = 0;
+	u32 i = 0;
+
+	for (i = 1; i <= MAX_CHANNEL_NUMBER; i++) {
+		if (dot11d_info->channel_map[i] > 0) {
+			default_chn = i;
+			break;
+		}
+	}
+
+	if (channel > MAX_CHANNEL_NUMBER) {
+		netdev_err(dev->dev, "%s: Invalid Channel\n", __func__);
+		return default_chn;
+	}
+
+	if (dot11d_info->channel_map[channel] > 0)
+		return channel;
+
+	return default_chn;
+}
+EXPORT_SYMBOL(to_legal_channel);
diff --git a/drivers/staging/rtl8192u/ieee80211/dot11d.h b/drivers/staging/rtl8192u/ieee80211/dot11d.h
new file mode 100644
index 000000000..8b485fa18
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/dot11d.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __INC_DOT11D_H
+#define __INC_DOT11D_H
+
+#include "ieee80211.h"
+
+struct chnl_txpower_triple {
+	u8  first_channel;
+	u8  num_channels;
+	u8  max_tx_pwr_dbm;
+};
+
+enum dot11d_state {
+	DOT11D_STATE_NONE = 0,
+	DOT11D_STATE_LEARNED,
+	DOT11D_STATE_DONE,
+};
+
+struct rt_dot11d_info {
+	u16 country_ie_len; /* > 0 if country_ie_buf[] contains valid country information element. */
+
+	/*  country_ie_src_addr u16 aligned for comparison and copy */
+	u8  country_ie_src_addr[ETH_ALEN]; /* Source AP of the country IE. */
+	u8  country_ie_buf[MAX_IE_LEN];
+	u8  country_ie_watchdog;
+
+	u8  channel_map[MAX_CHANNEL_NUMBER + 1];  /* !Value 0: Invalid, 1: Valid (active scan), 2: Valid (passive scan) */
+	u8  max_tx_pwr_dbm_list[MAX_CHANNEL_NUMBER + 1];
+
+	enum dot11d_state state;
+	u8  dot11d_enabled; /* dot11MultiDomainCapabilityEnabled */
+};
+
+#define GET_DOT11D_INFO(ieee_dev) ((struct rt_dot11d_info *)((ieee_dev)->dot11d_info))
+
+#define IS_DOT11D_ENABLE(ieee_dev) (GET_DOT11D_INFO(ieee_dev)->dot11d_enabled)
+#define IS_COUNTRY_IE_VALID(ieee_dev) (GET_DOT11D_INFO(ieee_dev)->country_ie_len > 0)
+
+#define IS_EQUAL_CIE_SRC(ieee_dev, addr) ether_addr_equal(GET_DOT11D_INFO(ieee_dev)->country_ie_src_addr, addr)
+#define UPDATE_CIE_SRC(ieee_dev, addr) ether_addr_copy(GET_DOT11D_INFO(ieee_dev)->country_ie_src_addr, addr)
+
+#define GET_CIE_WATCHDOG(ieee_dev) (GET_DOT11D_INFO(ieee_dev)->country_ie_watchdog)
+#define RESET_CIE_WATCHDOG(ieee_dev) (GET_CIE_WATCHDOG(ieee_dev) = 0)
+#define UPDATE_CIE_WATCHDOG(ieee_dev) (++GET_CIE_WATCHDOG(ieee_dev))
+
+void rtl8192u_dot11d_init(struct ieee80211_device *dev);
+void dot11d_reset(struct ieee80211_device *dev);
+void dot11d_update_country_ie(struct ieee80211_device *dev,
+			      u8 *addr,
+			      u16 coutry_ie_len,
+			      u8 *coutry_ie);
+u8 dot11d_get_max_tx_pwr_in_dbm(struct ieee80211_device *dev, u8 channel);
+void dot11d_scan_complete(struct ieee80211_device *dev);
+int is_legal_channel(struct ieee80211_device *dev, u8 channel);
+int to_legal_channel(struct ieee80211_device *dev, u8 channel);
+
+#endif /* #ifndef __INC_DOT11D_H */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
new file mode 100644
index 000000000..9cd4b1896
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -0,0 +1,2449 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Merged with mainline ieee80211.h in Aug 2004.  Original ieee802_11
+ * remains copyright by the original authors
+ *
+ * Portions of the merged code are based on Host AP (software wireless
+ * LAN access point) driver for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * Modified for Realtek's wi-fi cards by Andrea Merello
+ * <andrea.merello@gmail.com>
+ */
+#ifndef IEEE80211_H
+#define IEEE80211_H
+#include <linux/if_ether.h> /* ETH_ALEN */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/semaphore.h>
+#include <linux/interrupt.h>
+
+#include <linux/delay.h>
+#include <linux/wireless.h>
+#include <linux/ieee80211.h>
+
+#include "rtl819x_HT.h"
+#include "rtl819x_BA.h"
+#include "rtl819x_TS.h"
+
+
+#ifndef IW_MODE_MONITOR
+#define IW_MODE_MONITOR 6
+#endif
+
+#ifndef IWEVCUSTOM
+#define IWEVCUSTOM 0x8c02
+#endif
+
+#define KEY_TYPE_NA		0x0
+#define KEY_TYPE_WEP40		0x1
+#define KEY_TYPE_TKIP		0x2
+#define KEY_TYPE_CCMP		0x4
+#define KEY_TYPE_WEP104		0x5
+
+/* added for rtl819x tx procedure */
+#define MAX_QUEUE_SIZE		0x10
+
+//
+// 8190 queue mapping
+//
+#define BK_QUEUE                               0
+#define BE_QUEUE                               1
+#define VI_QUEUE                               2
+#define VO_QUEUE                               3
+#define HCCA_QUEUE                             4
+#define TXCMD_QUEUE                            5
+#define MGNT_QUEUE                             6
+#define HIGH_QUEUE                             7
+#define BEACON_QUEUE                           8
+
+#define LOW_QUEUE                              BE_QUEUE
+#define NORMAL_QUEUE                           MGNT_QUEUE
+
+//added by amy for ps
+#define SWRF_TIMEOUT				50
+
+//added by amy for LEAP related
+#define IE_CISCO_FLAG_POSITION		0x08	// Flag byte: byte 8, numbered from 0.
+#define SUPPORT_CKIP_MIC			0x08	// bit3
+#define SUPPORT_CKIP_PK			0x10	// bit4
+/* defined for skb cb field */
+/* At most 28 byte */
+struct cb_desc {
+	/* Tx Desc Related flags (8-9) */
+	u8 bLastIniPkt:1;
+	u8 bCmdOrInit:1;
+	u8 bFirstSeg:1;
+	u8 bLastSeg:1;
+	u8 bEncrypt:1;
+	u8 bTxDisableRateFallBack:1;
+	u8 bTxUseDriverAssingedRate:1;
+	u8 bHwSec:1; //indicate whether use Hw security. WB
+
+	u8 reserved1;
+
+	/* Tx Firmware Relaged flags (10-11)*/
+	u8 bCTSEnable:1;
+	u8 bRTSEnable:1;
+	u8 bUseShortGI:1;
+	u8 bUseShortPreamble:1;
+	u8 bTxEnableFwCalcDur:1;
+	u8 bAMPDUEnable:1;
+	u8 bRTSSTBC:1;
+	u8 RTSSC:1;
+
+	u8 bRTSBW:1;
+	u8 bPacketBW:1;
+	u8 bRTSUseShortPreamble:1;
+	u8 bRTSUseShortGI:1;
+	u8 bMulticast:1;
+	u8 bBroadcast:1;
+	//u8 reserved2:2;
+	u8 drv_agg_enable:1;
+	u8 reserved2:1;
+
+	/* Tx Desc related element(12-19) */
+	u8 rata_index;
+	u8 queue_index;
+	//u8 reserved3;
+	//u8 reserved4;
+	u16 txbuf_size;
+	//u8 reserved5;
+	u8 RATRIndex;
+	u8 reserved6;
+	u8 reserved7;
+	u8 reserved8;
+
+	/* Tx firmware related element(20-27) */
+	u8 data_rate;
+	u8 rts_rate;
+	u8 ampdu_factor;
+	u8 ampdu_density;
+	//u8 reserved9;
+	//u8 reserved10;
+	//u8 reserved11;
+	u8 DrvAggrNum;
+	u16 pkt_size;
+	u8 reserved12;
+};
+
+/*--------------------------Define -------------------------------------------*/
+#define MGN_1M                  0x02
+#define MGN_2M                  0x04
+#define MGN_5_5M                0x0b
+#define MGN_11M                 0x16
+
+#define MGN_6M                  0x0c
+#define MGN_9M                  0x12
+#define MGN_12M                 0x18
+#define MGN_18M                 0x24
+#define MGN_24M                 0x30
+#define MGN_36M                 0x48
+#define MGN_48M                 0x60
+#define MGN_54M                 0x6c
+
+#define MGN_MCS0                0x80
+#define MGN_MCS1                0x81
+#define MGN_MCS2                0x82
+#define MGN_MCS3                0x83
+#define MGN_MCS4                0x84
+#define MGN_MCS5                0x85
+#define MGN_MCS6                0x86
+#define MGN_MCS7                0x87
+#define MGN_MCS8                0x88
+#define MGN_MCS9                0x89
+#define MGN_MCS10               0x8a
+#define MGN_MCS11               0x8b
+#define MGN_MCS12               0x8c
+#define MGN_MCS13               0x8d
+#define MGN_MCS14               0x8e
+#define MGN_MCS15               0x8f
+
+#define aSifsTime ((priv->ieee80211->current_network.mode == IEEE_A ||     \
+		    priv->ieee80211->current_network.mode == IEEE_N_24G || \
+		    priv->ieee80211->current_network.mode == IEEE_N_5G) ?  \
+		   16 : 10)
+
+#define MGMT_QUEUE_NUM 5
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define	IEEE_CMD_SET_WPA_IE			2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME				4
+
+#define IEEE_PARAM_WPA_ENABLED			1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED		3
+#define IEEE_PARAM_PRIVACY_INVOKED		4
+#define IEEE_PARAM_AUTH_ALGS			5
+#define IEEE_PARAM_IEEE_802_1X			6
+//It should consistent with the driver_XXX.c
+//   David, 2006.9.26
+#define IEEE_PARAM_WPAX_SELECT			7
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_PROTO_WPA				1
+#define IEEE_PROTO_RSN				2
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_WPAX_USEGROUP			0
+#define IEEE_WPAX_WEP40				1
+#define IEEE_WPAX_TKIP				2
+#define IEEE_WPAX_WRAP				3
+#define IEEE_WPAX_CCMP				4
+#define IEEE_WPAX_WEP104			5
+
+#define IEEE_KEY_MGMT_IEEE8021X			1
+#define IEEE_KEY_MGMT_PSK			2
+
+#define IEEE_MLME_STA_DEAUTH			1
+#define IEEE_MLME_STA_DISASSOC			2
+
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED	4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED	6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#define MAX_IE_LEN  0xff
+
+// added for kernel conflict
+#define ieee80211_crypt_deinit_entries	ieee80211_crypt_deinit_entries_rsl
+#define ieee80211_crypt_deinit_handler	ieee80211_crypt_deinit_handler_rsl
+#define ieee80211_crypt_delayed_deinit	ieee80211_crypt_delayed_deinit_rsl
+#define ieee80211_register_crypto_ops	ieee80211_register_crypto_ops_rsl
+#define ieee80211_unregister_crypto_ops ieee80211_unregister_crypto_ops_rsl
+#define ieee80211_get_crypto_ops	ieee80211_get_crypto_ops_rsl
+
+#define ieee80211_ccmp_null		ieee80211_ccmp_null_rsl
+
+#define ieee80211_tkip_null		ieee80211_tkip_null_rsl
+
+#define free_ieee80211			free_ieee80211_rsl
+#define alloc_ieee80211			alloc_ieee80211_rsl
+
+#define ieee80211_rx			ieee80211_rx_rsl
+#define ieee80211_rx_mgt		ieee80211_rx_mgt_rsl
+
+#define ieee80211_get_beacon		ieee80211_get_beacon_rsl
+#define ieee80211_wake_queue		ieee80211_wake_queue_rsl
+#define ieee80211_stop_queue		ieee80211_stop_queue_rsl
+#define ieee80211_reset_queue		ieee80211_reset_queue_rsl
+#define ieee80211_softmac_stop_protocol	ieee80211_softmac_stop_protocol_rsl
+#define ieee80211_softmac_start_protocol ieee80211_softmac_start_protocol_rsl
+#define ieee80211_is_shortslot		ieee80211_is_shortslot_rsl
+#define ieee80211_is_54g		ieee80211_is_54g_rsl
+#define ieee80211_wpa_supplicant_ioctl	ieee80211_wpa_supplicant_ioctl_rsl
+#define ieee80211_ps_tx_ack		ieee80211_ps_tx_ack_rsl
+#define ieee80211_softmac_xmit		ieee80211_softmac_xmit_rsl
+#define ieee80211_stop_send_beacons	ieee80211_stop_send_beacons_rsl
+#define notify_wx_assoc_event		notify_wx_assoc_event_rsl
+#define SendDisassociation		SendDisassociation_rsl
+#define ieee80211_disassociate		ieee80211_disassociate_rsl
+#define ieee80211_start_send_beacons	ieee80211_start_send_beacons_rsl
+#define ieee80211_stop_scan		ieee80211_stop_scan_rsl
+#define ieee80211_send_probe_requests	ieee80211_send_probe_requests_rsl
+#define ieee80211_softmac_scan_syncro	ieee80211_softmac_scan_syncro_rsl
+#define ieee80211_start_scan_syncro	ieee80211_start_scan_syncro_rsl
+
+#define ieee80211_wx_get_essid		ieee80211_wx_get_essid_rsl
+#define ieee80211_wx_set_essid		ieee80211_wx_set_essid_rsl
+#define ieee80211_wx_set_rate		ieee80211_wx_set_rate_rsl
+#define ieee80211_wx_get_rate		ieee80211_wx_get_rate_rsl
+#define ieee80211_wx_set_wap		ieee80211_wx_set_wap_rsl
+#define ieee80211_wx_get_wap		ieee80211_wx_get_wap_rsl
+#define ieee80211_wx_set_mode		ieee80211_wx_set_mode_rsl
+#define ieee80211_wx_get_mode		ieee80211_wx_get_mode_rsl
+#define ieee80211_wx_set_scan		ieee80211_wx_set_scan_rsl
+#define ieee80211_wx_get_freq		ieee80211_wx_get_freq_rsl
+#define ieee80211_wx_set_freq		ieee80211_wx_set_freq_rsl
+#define ieee80211_wx_set_rawtx		ieee80211_wx_set_rawtx_rsl
+#define ieee80211_wx_get_name		ieee80211_wx_get_name_rsl
+#define ieee80211_wx_set_power		ieee80211_wx_set_power_rsl
+#define ieee80211_wx_get_power		ieee80211_wx_get_power_rsl
+#define ieee80211_wlan_frequencies	ieee80211_wlan_frequencies_rsl
+#define ieee80211_wx_set_rts		ieee80211_wx_set_rts_rsl
+#define ieee80211_wx_get_rts		ieee80211_wx_get_rts_rsl
+
+#define ieee80211_txb_free		ieee80211_txb_free_rsl
+
+#define ieee80211_wx_set_gen_ie		ieee80211_wx_set_gen_ie_rsl
+#define ieee80211_wx_get_scan		ieee80211_wx_get_scan_rsl
+#define ieee80211_wx_set_encode		ieee80211_wx_set_encode_rsl
+#define ieee80211_wx_get_encode		ieee80211_wx_get_encode_rsl
+#define ieee80211_wx_set_mlme		ieee80211_wx_set_mlme_rsl
+#define ieee80211_wx_set_auth		ieee80211_wx_set_auth_rsl
+#define ieee80211_wx_set_encode_ext	ieee80211_wx_set_encode_ext_rsl
+#define ieee80211_wx_get_encode_ext	ieee80211_wx_get_encode_ext_rsl
+
+
+struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[];
+		} wpa_ie;
+		struct{
+			int command;
+			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[];
+		} crypt;
+	} u;
+};
+
+
+// linux under 2.6.9 release may not support it, so modify it for common use
+#define IEEE80211_DATA_LEN		2304
+/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
+ *   6.2.1.1.2.
+ *
+ *   The figure in section 7.1.2 suggests a body size of up to 2312
+ *   bytes is allowed, which is a bit confusing, I suspect this
+ *   represents the 2304 bytes of real data, plus a possible 8 bytes of
+ *   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro)
+ */
+#define IEEE80211_1ADDR_LEN 10
+#define IEEE80211_2ADDR_LEN 16
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+#define IEEE80211_HLEN                  (IEEE80211_4ADDR_LEN)
+#define IEEE80211_FRAME_LEN             (IEEE80211_DATA_LEN + IEEE80211_HLEN)
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+#define MIN_FRAG_THRESHOLD     256U
+#define MAX_FRAG_THRESHOLD     2346U
+
+
+/* Frame control field constants */
+#define IEEE80211_FCTL_VERS		0x0003
+#define IEEE80211_FCTL_FTYPE		0x000c
+#define IEEE80211_FCTL_STYPE		0x00f0
+#define IEEE80211_FCTL_FRAMETYPE	0x00fc
+#define IEEE80211_FCTL_TODS		0x0100
+#define IEEE80211_FCTL_FROMDS		0x0200
+#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
+#define IEEE80211_FCTL_MOREFRAGS	0x0400
+#define IEEE80211_FCTL_RETRY		0x0800
+#define IEEE80211_FCTL_PM		0x1000
+#define IEEE80211_FCTL_MOREDATA		0x2000
+#define IEEE80211_FCTL_WEP		0x4000
+#define IEEE80211_FCTL_ORDER		0x8000
+
+#define IEEE80211_FTYPE_MGMT		0x0000
+#define IEEE80211_FTYPE_CTL		0x0004
+#define IEEE80211_FTYPE_DATA		0x0008
+
+/* management */
+#define IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define IEEE80211_STYPE_ASSOC_RESP	0x0010
+#define IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define IEEE80211_STYPE_PROBE_REQ	0x0040
+#define IEEE80211_STYPE_PROBE_RESP	0x0050
+#define IEEE80211_STYPE_BEACON		0x0080
+#define IEEE80211_STYPE_ATIM		0x0090
+#define IEEE80211_STYPE_DISASSOC	0x00A0
+#define IEEE80211_STYPE_AUTH		0x00B0
+#define IEEE80211_STYPE_DEAUTH		0x00C0
+#define IEEE80211_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define IEEE80211_STYPE_PSPOLL		0x00A0
+#define IEEE80211_STYPE_RTS		0x00B0
+#define IEEE80211_STYPE_CTS		0x00C0
+#define IEEE80211_STYPE_ACK		0x00D0
+#define IEEE80211_STYPE_CFEND		0x00E0
+#define IEEE80211_STYPE_CFENDACK	0x00F0
+#define IEEE80211_STYPE_BLOCKACK	0x0094
+
+/* data */
+#define IEEE80211_STYPE_DATA		0x0000
+#define IEEE80211_STYPE_DATA_CFACK	0x0010
+#define IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define IEEE80211_STYPE_NULLFUNC	0x0040
+#define IEEE80211_STYPE_CFACK		0x0050
+#define IEEE80211_STYPE_CFPOLL		0x0060
+#define IEEE80211_STYPE_CFACKPOLL	0x0070
+#define IEEE80211_STYPE_QOS_DATA	0x0080 //added for WMM 2006/8/2
+#define IEEE80211_STYPE_QOS_NULL	0x00C0
+
+#define IEEE80211_SCTL_FRAG		0x000F
+#define IEEE80211_SCTL_SEQ		0xFFF0
+
+/* QOS control */
+#define IEEE80211_QCTL_TID              0x000F
+
+#define	FC_QOS_BIT					BIT(7)
+#define IsDataFrame(pdu)			(((pdu[0] & 0x0C) == 0x08) ? true : false)
+#define	IsLegacyDataFrame(pdu)	(IsDataFrame(pdu) && (!(pdu[0] & FC_QOS_BIT)))
+//added by wb. Is this right?
+#define IsQoSDataFrame(pframe)  ((*(u16 *)pframe & (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA)) == (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA))
+#define Frame_Order(pframe)     (*(u16 *)pframe & IEEE80211_FCTL_ORDER)
+#define SN_LESS(a, b)		(((a - b) & 0x800) != 0)
+#define SN_EQUAL(a, b)	(a == b)
+#define MAX_DEV_ADDR_SIZE 8
+typedef enum _ACT_CATEGORY {
+	ACT_CAT_QOS = 1,
+	ACT_CAT_DLS = 2,
+	ACT_CAT_BA  = 3,
+	ACT_CAT_HT  = 7,
+	ACT_CAT_WMM = 17,
+} ACT_CATEGORY, *PACT_CATEGORY;
+
+typedef enum _TS_ACTION {
+	ACT_ADDTSREQ = 0,
+	ACT_ADDTSRSP = 1,
+	ACT_DELTS    = 2,
+	ACT_SCHEDULE = 3,
+} TS_ACTION, *PTS_ACTION;
+
+typedef enum _BA_ACTION {
+	ACT_ADDBAREQ = 0,
+	ACT_ADDBARSP = 1,
+	ACT_DELBA    = 2,
+} BA_ACTION, *PBA_ACTION;
+
+typedef enum _InitialGainOpType {
+	IG_Backup = 0,
+	IG_Restore,
+	IG_Max
+} InitialGainOpType;
+
+/* debug macros */
+#define CONFIG_IEEE80211_DEBUG
+#ifdef CONFIG_IEEE80211_DEBUG
+extern u32 ieee80211_debug_level;
+#define IEEE80211_DEBUG(level, fmt, args...) \
+do { if (ieee80211_debug_level & (level)) \
+  printk(KERN_DEBUG "ieee80211: " fmt, ## args); } while (0)
+//wb added to debug out data buf
+//if you want print DATA buffer related BA, please set ieee80211_debug_level to DATA|BA
+#define IEEE80211_DEBUG_DATA(level, data, datalen)                             \
+	do { if ((ieee80211_debug_level & (level)) == (level))                 \
+		{                                                              \
+			int i;                                                 \
+			u8 *pdata = (u8 *)data;                                \
+			printk(KERN_DEBUG "ieee80211: %s()\n", __func__);      \
+			for (i = 0; i < (int)(datalen); i++) {                 \
+				printk("%2x ", pdata[i]);                      \
+				if ((i + 1) % 16 == 0)                         \
+					printk("\n");                          \
+			}                                                      \
+			printk("\n");                                          \
+		}                                                              \
+	} while (0)
+#else
+#define IEEE80211_DEBUG(level, fmt, args...)
+#define IEEE80211_DEBUG_DATA(level, data, datalen)
+#endif	/* CONFIG_IEEE80211_DEBUG */
+
+/* debug macros not dependent on CONFIG_IEEE80211_DEBUG */
+
+/*
+ * To use the debug system;
+ *
+ * If you are defining a new debug classification, simply add it to the #define
+ * list here in the form of:
+ *
+ * #define IEEE80211_DL_xxxx VALUE
+ *
+ * shifting value to the left one bit from the previous entry.  xxxx should be
+ * the name of the classification (for example, WEP)
+ *
+ * You then need to either add a IEEE80211_xxxx_DEBUG() macro definition for your
+ * classification, or use IEEE80211_DEBUG(IEEE80211_DL_xxxx, ...) whenever you want
+ * to send output to that classification.
+ *
+ * To add your debug level to the list of levels seen when you perform
+ *
+ * % cat /proc/net/ipw/debug_level
+ *
+ * you simply need to add your entry to the ipw_debug_levels array.
+ *
+ * If you do not see debug_level in /proc/net/ipw then you do not have
+ * CONFIG_IEEE80211_DEBUG defined in your kernel configuration
+ *
+ */
+
+#define IEEE80211_DL_INFO          (1<<0)
+#define IEEE80211_DL_WX            (1<<1)
+#define IEEE80211_DL_SCAN          (1<<2)
+#define IEEE80211_DL_STATE         (1<<3)
+#define IEEE80211_DL_MGMT          (1<<4)
+#define IEEE80211_DL_FRAG          (1<<5)
+#define IEEE80211_DL_EAP           (1<<6)
+#define IEEE80211_DL_DROP          (1<<7)
+
+#define IEEE80211_DL_TX            (1<<8)
+#define IEEE80211_DL_RX            (1<<9)
+
+#define IEEE80211_DL_HT		   (1<<10)  //HT
+#define IEEE80211_DL_BA		   (1<<11)  //ba
+#define IEEE80211_DL_TS		   (1<<12)  //TS
+#define IEEE80211_DL_QOS           (1<<13)
+#define IEEE80211_DL_REORDER	   (1<<14)
+#define IEEE80211_DL_IOT	   (1<<15)
+#define IEEE80211_DL_IPS	   (1<<16)
+#define IEEE80211_DL_TRACE	   (1<<29)  //trace function, need to user net_ratelimit() together in order not to print too much to the screen
+#define IEEE80211_DL_DATA	   (1<<30)   //use this flag to control whether print data buf out.
+#define IEEE80211_DL_ERR	   (1<<31)   //always open
+#define IEEE80211_ERROR(f, a...) printk(KERN_ERR "ieee80211: " f, ## a)
+#define IEEE80211_WARNING(f, a...) printk(KERN_WARNING "ieee80211: " f, ## a)
+#define IEEE80211_DEBUG_INFO(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_INFO, f, ## a)
+
+#define IEEE80211_DEBUG_WX(f, a...)     IEEE80211_DEBUG(IEEE80211_DL_WX, f, ## a)
+#define IEEE80211_DEBUG_SCAN(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_SCAN, f, ## a)
+#define IEEE80211_DEBUG_STATE(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_STATE, f, ## a)
+#define IEEE80211_DEBUG_MGMT(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_MGMT, f, ## a)
+#define IEEE80211_DEBUG_FRAG(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_FRAG, f, ## a)
+#define IEEE80211_DEBUG_EAP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_EAP, f, ## a)
+#define IEEE80211_DEBUG_DROP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_DROP, f, ## a)
+#define IEEE80211_DEBUG_TX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_TX, f, ## a)
+#define IEEE80211_DEBUG_RX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_RX, f, ## a)
+#define IEEE80211_DEBUG_QOS(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_QOS, f, ## a)
+
+#include <linux/if_arp.h> /* ARPHRD_ETHER */
+
+#ifndef WIRELESS_SPY
+#define WIRELESS_SPY		// enable iwspy support
+#endif
+#include <net/iw_handler.h>	// new driver API
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
+#endif /* ETH_P_PAE */
+
+#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+
+	u8    dsap;   /* always 0xAA */
+	u8    ssap;   /* always 0xAA */
+	u8    ctrl;   /* always 0x03 */
+	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+
+} __packed;
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define WLAN_FC_GET_VERS(fc) ((fc) & IEEE80211_FCTL_VERS)
+#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+
+#define WLAN_FC_GET_FRAMETYPE(fc) ((fc) & IEEE80211_FCTL_FRAMETYPE)
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  (((seq) & IEEE80211_SCTL_SEQ) >> 4)
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SPECTRUM_MGMT (1<<8)
+#define WLAN_CAPABILITY_QOS (1<<9)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+#define WLAN_CAPABILITY_DSSS_OFDM (1<<13)
+
+/* 802.11g ERP information element */
+#define WLAN_ERP_NON_ERP_PRESENT (1<<0)
+#define WLAN_ERP_USE_PROTECTION (1<<1)
+#define WLAN_ERP_BARKER_PREAMBLE (1<<2)
+
+#define IEEE80211_STATMASK_SIGNAL (1<<0)
+#define IEEE80211_STATMASK_RSSI (1<<1)
+#define IEEE80211_STATMASK_NOISE (1<<2)
+#define IEEE80211_STATMASK_RATE (1<<3)
+#define IEEE80211_STATMASK_WEMASK 0x7
+
+#define IEEE80211_CCK_MODULATION    (1<<0)
+#define IEEE80211_OFDM_MODULATION   (1<<1)
+
+#define IEEE80211_24GHZ_BAND     (1<<0)
+#define IEEE80211_52GHZ_BAND     (1<<1)
+
+#define IEEE80211_CCK_RATE_LEN			4
+#define IEEE80211_CCK_RATE_1MB			0x02
+#define IEEE80211_CCK_RATE_2MB			0x04
+#define IEEE80211_CCK_RATE_5MB			0x0B
+#define IEEE80211_CCK_RATE_11MB			0x16
+#define IEEE80211_OFDM_RATE_LEN			8
+#define IEEE80211_OFDM_RATE_6MB			0x0C
+#define IEEE80211_OFDM_RATE_9MB			0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
+#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
+#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
+#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
+#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
+#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
+#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
+#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
+#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
+#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
+#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
+#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
+
+#define IEEE80211_CCK_RATES_MASK		0x0000000F
+#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
+	IEEE80211_CCK_RATE_2MB_MASK)
+#define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
+	IEEE80211_CCK_RATE_5MB_MASK | \
+	IEEE80211_CCK_RATE_11MB_MASK)
+
+#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
+#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
+	IEEE80211_OFDM_RATE_12MB_MASK | \
+	IEEE80211_OFDM_RATE_24MB_MASK)
+#define IEEE80211_OFDM_DEFAULT_RATES_MASK	(IEEE80211_OFDM_BASIC_RATES_MASK | \
+	IEEE80211_OFDM_RATE_9MB_MASK  | \
+	IEEE80211_OFDM_RATE_18MB_MASK | \
+	IEEE80211_OFDM_RATE_36MB_MASK | \
+	IEEE80211_OFDM_RATE_48MB_MASK | \
+	IEEE80211_OFDM_RATE_54MB_MASK)
+#define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
+				IEEE80211_CCK_DEFAULT_RATES_MASK)
+
+#define IEEE80211_NUM_OFDM_RATES	    8
+#define IEEE80211_NUM_CCK_RATES		    4
+#define IEEE80211_OFDM_SHIFT_MASK_A         4
+
+
+/* this is stolen and modified from the madwifi driver*/
+#define IEEE80211_FC0_TYPE_MASK		0x0c
+#define IEEE80211_FC0_TYPE_DATA		0x08
+#define IEEE80211_FC0_SUBTYPE_MASK	0xB0
+#define IEEE80211_FC0_SUBTYPE_QOS	0x80
+
+#define IEEE80211_QOS_HAS_SEQ(fc) \
+	(((fc) & (IEEE80211_FC0_TYPE_MASK | IEEE80211_FC0_SUBTYPE_MASK)) == \
+	 (IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_QOS))
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+struct ieee_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num[17];
+	u16 frag_num[17];
+	unsigned long packet_time[17];
+	struct list_head list;
+};
+
+/* NOTE: This data is for statistical purposes; not all hardware provides this
+ *       information for frames received.  Not setting these will not cause
+ *       any adverse affects.
+ */
+struct ieee80211_rx_stats {
+	u32 mac_time[2];
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u16 rate; /* in 100 kbps */
+	u8 received_channel;
+	u8 control;
+	u8 mask;
+	u8 freq;
+	u16 len;
+	u64 tsf;
+	u32 beacon_time;
+	u8 nic_type;
+	u16       Length;
+	//      u8        DataRate;      // In 0.5 Mbps
+	u8        SignalQuality; // in 0-100 index.
+	s32       RecvSignalPower; // Real power in dBm for this packet, no beautification and aggregation.
+	s8        RxPower; // in dBm Translate from PWdB
+	u8        SignalStrength; // in 0-100 index.
+	u16       bHwError:1;
+	u16       bCRC:1;
+	u16       bICV:1;
+	u16       bShortPreamble:1;
+	u16       Antenna:1;      //for rtl8185
+	u16       Decrypted:1;    //for rtl8185, rtl8187
+	u16       Wakeup:1;       //for rtl8185
+	u16       Reserved0:1;    //for rtl8185
+	u8        AGC;
+	u32       TimeStampLow;
+	u32       TimeStampHigh;
+	bool      bShift;
+	bool      bIsQosData;             // Added by Annie, 2005-12-22.
+	u8        UserPriority;
+
+	//1!!!!!!!!!!!!!!!!!!!!!!!!!!!
+	//1Attention Please!!!<11n or 8190 specific code should be put below this line>
+	//1!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+	u8        RxDrvInfoSize;
+	u8        RxBufShift;
+	bool      bIsAMPDU;
+	bool      bFirstMPDU;
+	bool      bContainHTC;
+	bool      RxIs40MHzPacket;
+	u32       RxPWDBAll;
+	u8        RxMIMOSignalStrength[4];        // in 0~100 index
+	s8        RxMIMOSignalQuality[2];
+	bool      bPacketMatchBSSID;
+	bool      bIsCCK;
+	bool      bPacketToSelf;
+	//added by amy
+	u8        *virtual_address;
+	u16          packetlength;              // Total packet length: Must equal to sum of all FragLength
+	u16          fraglength;                        // FragLength should equal to PacketLength in non-fragment case
+	u16          fragoffset;                        // Data offset for this fragment
+	u16          ntotalfrag;
+	bool		  bisrxaggrsubframe;
+	bool		  bPacketBeacon;	//cosa add for rssi
+	bool		  bToSelfBA;		//cosa add for rssi
+	s8		  cck_adc_pwdb[4];	//cosa add for rx path selection
+	u16		  Seq_Num;
+
+};
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly.
+ */
+#define IEEE80211_FRAG_CACHE_LEN 4
+
+struct ieee80211_frag_entry {
+	unsigned long first_frag_time;
+	unsigned int seq;
+	unsigned int last_frag;
+	struct sk_buff *skb;
+	u8 src_addr[ETH_ALEN];
+	u8 dst_addr[ETH_ALEN];
+};
+
+struct ieee80211_stats {
+	unsigned int tx_unicast_frames;
+	unsigned int tx_multicast_frames;
+	unsigned int tx_fragments;
+	unsigned int tx_unicast_octets;
+	unsigned int tx_multicast_octets;
+	unsigned int tx_deferred_transmissions;
+	unsigned int tx_single_retry_frames;
+	unsigned int tx_multiple_retry_frames;
+	unsigned int tx_retry_limit_exceeded;
+	unsigned int tx_discards;
+	unsigned int rx_unicast_frames;
+	unsigned int rx_multicast_frames;
+	unsigned int rx_fragments;
+	unsigned int rx_unicast_octets;
+	unsigned int rx_multicast_octets;
+	unsigned int rx_fcs_errors;
+	unsigned int rx_discards_no_buffer;
+	unsigned int tx_discards_wrong_sa;
+	unsigned int rx_discards_undecryptable;
+	unsigned int rx_message_in_msg_fragments;
+	unsigned int rx_message_in_bad_msg_fragments;
+};
+
+struct ieee80211_device;
+
+#include "ieee80211_crypt.h"
+
+#define SEC_KEY_1         (1<<0)
+#define SEC_KEY_2         (1<<1)
+#define SEC_KEY_3         (1<<2)
+#define SEC_KEY_4         (1<<3)
+#define SEC_ACTIVE_KEY    (1<<4)
+#define SEC_AUTH_MODE     (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL         (1<<7)
+#define SEC_ENABLED       (1<<8)
+#define SEC_ENCRYPT       (1<<9)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define SEC_ALG_NONE            0
+#define SEC_ALG_WEP             1
+#define SEC_ALG_TKIP            2
+#define SEC_ALG_CCMP            3
+
+#define WEP_KEYS		4
+#define WEP_KEY_LEN		13
+#define SCM_KEY_LEN             32
+#define SCM_TEMPORAL_KEY_LENGTH 16
+
+struct ieee80211_security {
+	u16 active_key:2,
+	    enabled:1,
+	    auth_algo:4,
+	    unicast_uses_group:1,
+	    encrypt:1;
+	u8 auth_mode;
+	u8 key_sizes[WEP_KEYS];
+	u8 keys[WEP_KEYS][SCM_KEY_LEN];
+	u8 level;
+	u16 flags;
+} __packed;
+
+
+/*
+ *  802.11 data frame from AP
+ *       ,-------------------------------------------------------------------.
+ * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *       |------|------|---------|---------|---------|------|---------|------|
+ * Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+ *       |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *       `-------------------------------------------------------------------'
+ *  Total: 28-2340 bytes
+ */
+
+/* Management Frame Information Element Types */
+enum ieee80211_mfie {
+	MFIE_TYPE_SSID = 0,
+	MFIE_TYPE_RATES = 1,
+	MFIE_TYPE_FH_SET = 2,
+	MFIE_TYPE_DS_SET = 3,
+	MFIE_TYPE_CF_SET = 4,
+	MFIE_TYPE_TIM = 5,
+	MFIE_TYPE_IBSS_SET = 6,
+	MFIE_TYPE_COUNTRY = 7,
+	MFIE_TYPE_HOP_PARAMS = 8,
+	MFIE_TYPE_HOP_TABLE = 9,
+	MFIE_TYPE_REQUEST = 10,
+	MFIE_TYPE_CHALLENGE = 16,
+	MFIE_TYPE_POWER_CONSTRAINT = 32,
+	MFIE_TYPE_POWER_CAPABILITY = 33,
+	MFIE_TYPE_TPC_REQUEST = 34,
+	MFIE_TYPE_TPC_REPORT = 35,
+	MFIE_TYPE_SUPP_CHANNELS = 36,
+	MFIE_TYPE_CSA = 37,
+	MFIE_TYPE_MEASURE_REQUEST = 38,
+	MFIE_TYPE_MEASURE_REPORT = 39,
+	MFIE_TYPE_QUIET = 40,
+	MFIE_TYPE_IBSS_DFS = 41,
+	MFIE_TYPE_ERP = 42,
+	MFIE_TYPE_RSN = 48,
+	MFIE_TYPE_RATES_EX = 50,
+	MFIE_TYPE_HT_CAP = 45,
+	 MFIE_TYPE_HT_INFO = 61,
+	 MFIE_TYPE_AIRONET = 133,
+	MFIE_TYPE_GENERIC = 221,
+	MFIE_TYPE_QOS_PARAMETER = 222,
+};
+
+/* Minimal header; can be used for passing 802.11 frames with sufficient
+ * information to determine what type of underlying data type is actually
+ * stored in the data.
+ */
+struct rtl_80211_hdr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 payload[];
+} __packed;
+
+struct rtl_80211_hdr_1addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtl_80211_hdr_2addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtl_80211_hdr_3addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 payload[];
+} __packed;
+
+struct rtl_80211_hdr_4addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	u8 payload[];
+} __packed;
+
+struct rtl_80211_hdr_3addrqos {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 payload[0];
+	__le16 qos_ctl;
+} __packed;
+
+struct rtl_80211_hdr_4addrqos {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	u8 payload[0];
+	__le16 qos_ctl;
+} __packed;
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[];
+} __packed;
+
+struct ieee80211_authentication {
+	struct rtl_80211_hdr_3addr header;
+	__le16 algorithm;
+	__le16 transaction;
+	__le16 status;
+	/*challenge*/
+	struct ieee80211_info_element info_element[];
+} __packed;
+
+struct ieee80211_disassoc {
+	struct rtl_80211_hdr_3addr header;
+	__le16 reason;
+} __packed;
+
+struct ieee80211_probe_request {
+	struct rtl_80211_hdr_3addr header;
+	/* SSID, supported rates */
+	struct ieee80211_info_element info_element[];
+} __packed;
+
+struct ieee80211_probe_response {
+	struct rtl_80211_hdr_3addr header;
+	__le32 time_stamp[2];
+	__le16 beacon_interval;
+	__le16 capability;
+	/* SSID, supported rates, FH params, DS params,
+	 * CF params, IBSS params, TIM (if beacon), RSN
+	 */
+	struct ieee80211_info_element info_element[];
+} __packed;
+
+/* Alias beacon for probe_response */
+#define ieee80211_beacon ieee80211_probe_response
+
+struct ieee80211_assoc_request_frame {
+	struct rtl_80211_hdr_3addr header;
+	__le16 capability;
+	__le16 listen_interval;
+	/* SSID, supported rates, RSN */
+	struct ieee80211_info_element info_element[];
+} __packed;
+
+struct ieee80211_reassoc_request_frame {
+	struct rtl_80211_hdr_3addr header;
+	__le16 capability;
+	__le16 listen_interval;
+	u8 current_ap[ETH_ALEN];
+	/* SSID, supported rates, RSN */
+	struct ieee80211_info_element info_element[];
+} __packed;
+
+struct ieee80211_assoc_response_frame {
+	struct rtl_80211_hdr_3addr header;
+	__le16 capability;
+	__le16 status;
+	__le16 aid;
+	struct ieee80211_info_element info_element[]; /* supported rates */
+} __packed;
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u8 queue_index;
+	u8 rts_included;
+	u16 reserved;
+	__le16 frag_size;
+	__le16 payload_size;
+	struct sk_buff *fragments[];
+};
+
+#define MAX_TX_AGG_COUNT		  16
+struct ieee80211_drv_agg_txb {
+	u8 nr_drv_agg_frames;
+	struct sk_buff *tx_agg_frames[MAX_TX_AGG_COUNT];
+} __packed;
+
+#define MAX_SUBFRAME_COUNT		  64
+struct ieee80211_rxb {
+	u8 nr_subframes;
+	struct sk_buff *subframes[MAX_SUBFRAME_COUNT];
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+} __packed;
+
+typedef union _frameqos {
+	u16 shortdata;
+	u8  chardata[2];
+	struct {
+		u16 tid:4;
+		u16 eosp:1;
+		u16 ack_policy:2;
+		u16 reserved:1;
+		u16 txop:8;
+	} field;
+} frameqos, *pframeqos;
+
+/* SWEEP TABLE ENTRIES NUMBER*/
+#define MAX_SWEEP_TAB_ENTRIES		  42
+#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element...
+ */
+#define MAX_RATES_LENGTH                  ((u8)12)
+#define MAX_RATES_EX_LENGTH               ((u8)16)
+#define MAX_NETWORK_COUNT                  128
+
+#define MAX_CHANNEL_NUMBER                 161
+#define IEEE80211_SOFTMAC_SCAN_TIME	   100
+//(HZ / 2)
+#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define CRC_LENGTH                 4U
+
+#define MAX_WPA_IE_LEN 64
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+/* QoS structure */
+#define NETWORK_HAS_QOS_PARAMETERS      (1<<3)
+#define NETWORK_HAS_QOS_INFORMATION     (1<<4)
+#define NETWORK_HAS_QOS_MASK            (NETWORK_HAS_QOS_PARAMETERS | \
+					 NETWORK_HAS_QOS_INFORMATION)
+/* 802.11h */
+#define NETWORK_HAS_POWER_CONSTRAINT    (1<<5)
+#define NETWORK_HAS_CSA                 (1<<6)
+#define NETWORK_HAS_QUIET               (1<<7)
+#define NETWORK_HAS_IBSS_DFS            (1<<8)
+#define NETWORK_HAS_TPC_REPORT          (1<<9)
+
+#define NETWORK_HAS_ERP_VALUE           (1<<10)
+
+#define QOS_QUEUE_NUM                   4
+#define QOS_OUI_LEN                     3
+#define QOS_OUI_TYPE                    2
+#define QOS_ELEMENT_ID                  221
+#define QOS_OUI_INFO_SUB_TYPE           0
+#define QOS_OUI_PARAM_SUB_TYPE          1
+#define QOS_VERSION_1                   1
+#define QOS_AIFSN_MIN_VALUE             2
+struct ieee80211_qos_information_element {
+	u8 elementID;
+	u8 length;
+	u8 qui[QOS_OUI_LEN];
+	u8 qui_type;
+	u8 qui_subtype;
+	u8 version;
+	u8 ac_info;
+} __packed;
+
+struct ieee80211_qos_ac_parameter {
+	u8 aci_aifsn;
+	u8 ecw_min_max;
+	__le16 tx_op_limit;
+} __packed;
+
+struct ieee80211_qos_parameter_info {
+	struct ieee80211_qos_information_element info_element;
+	u8 reserved;
+	struct ieee80211_qos_ac_parameter ac_params_record[QOS_QUEUE_NUM];
+} __packed;
+
+struct ieee80211_qos_parameters {
+	__le16 cw_min[QOS_QUEUE_NUM];
+	__le16 cw_max[QOS_QUEUE_NUM];
+	u8 aifs[QOS_QUEUE_NUM];
+	u8 flag[QOS_QUEUE_NUM];
+	__le16 tx_op_limit[QOS_QUEUE_NUM];
+} __packed;
+
+struct ieee80211_qos_data {
+	struct ieee80211_qos_parameters parameters;
+	int active;
+	int supported;
+	u8 param_count;
+	u8 old_param_count;
+};
+
+struct ieee80211_tim_parameters {
+	u8 tim_count;
+	u8 tim_period;
+} __packed;
+
+//#else
+struct ieee80211_wmm_ts_info {
+	u8 ac_dir_tid;
+	u8 ac_up_psb;
+	u8 reserved;
+} __packed;
+
+struct ieee80211_wmm_tspec_elem {
+	struct ieee80211_wmm_ts_info ts_info;
+	u16 norm_msdu_size;
+	u16 max_msdu_size;
+	u32 min_serv_inter;
+	u32 max_serv_inter;
+	u32 inact_inter;
+	u32 suspen_inter;
+	u32 serv_start_time;
+	u32 min_data_rate;
+	u32 mean_data_rate;
+	u32 peak_data_rate;
+	u32 max_burst_size;
+	u32 delay_bound;
+	u32 min_phy_rate;
+	u16 surp_band_allow;
+	u16 medium_time;
+} __packed;
+enum eap_type {
+	EAP_PACKET = 0,
+	EAPOL_START,
+	EAPOL_LOGOFF,
+	EAPOL_KEY,
+	EAPOL_ENCAP_ASF_ALERT
+};
+
+static const char *eap_types[] = {
+	[EAP_PACKET]		= "EAP-Packet",
+	[EAPOL_START]		= "EAPOL-Start",
+	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
+	[EAPOL_KEY]		= "EAPOL-Key",
+	[EAPOL_ENCAP_ASF_ALERT]	= "EAPOL-Encap-ASF-Alert"
+};
+
+static inline const char *eap_get_type(int type)
+{
+	return ((u32)type >= ARRAY_SIZE(eap_types)) ? "Unknown" : eap_types[type];
+}
+//added by amy for reorder
+static inline u8 Frame_QoSTID(u8 *buf)
+{
+	struct rtl_80211_hdr_3addr *hdr;
+	u16 fc;
+	hdr = (struct rtl_80211_hdr_3addr *)buf;
+	fc = le16_to_cpu(hdr->frame_ctl);
+	return (u8)((frameqos *)(buf + (((fc & IEEE80211_FCTL_TODS) && (fc & IEEE80211_FCTL_FROMDS)) ? 30 : 24)))->field.tid;
+}
+
+//added by amy for reorder
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __packed;
+
+struct ieee80211_softmac_stats {
+	unsigned int rx_ass_ok;
+	unsigned int rx_ass_err;
+	unsigned int rx_probe_rq;
+	unsigned int tx_probe_rs;
+	unsigned int tx_beacons;
+	unsigned int rx_auth_rq;
+	unsigned int rx_auth_rs_ok;
+	unsigned int rx_auth_rs_err;
+	unsigned int tx_auth_rq;
+	unsigned int no_auth_rs;
+	unsigned int no_ass_rs;
+	unsigned int tx_ass_rq;
+	unsigned int rx_ass_rq;
+	unsigned int tx_probe_rq;
+	unsigned int reassoc;
+	unsigned int swtxstop;
+	unsigned int swtxawake;
+	unsigned char CurrentShowTxate;
+	unsigned char last_packet_rate;
+	unsigned int txretrycount;
+};
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __packed;
+
+/*
+ * These are the data types that can make up management packets
+ *
+	u16 auth_algorithm;
+	u16 auth_sequence;
+	u16 beacon_interval;
+	u16 capability;
+	u8 current_ap[ETH_ALEN];
+	u16 listen_interval;
+	struct {
+		u16 association_id:14, reserved:2;
+	} __packed;
+	u32 time_stamp[2];
+	u16 reason;
+	u16 status;
+*/
+
+#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
+#define IEEE80211_DEFAULT_BASIC_RATE 2 //1Mbps
+
+enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
+#define MAX_SP_Len  (WMM_all_frame << 4)
+#define IEEE80211_QOS_TID 0x0f
+#define QOS_CTL_NOTCONTAIN_ACK (0x01 << 5)
+
+#define IEEE80211_DTIM_MBCAST 4
+#define IEEE80211_DTIM_UCAST 2
+#define IEEE80211_DTIM_VALID 1
+#define IEEE80211_DTIM_INVALID 0
+
+#define IEEE80211_PS_DISABLED 0
+#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
+#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
+
+//added by David for QoS 2006/6/30
+//#define WMM_Hang_8187
+#ifdef WMM_Hang_8187
+#undef WMM_Hang_8187
+#endif
+
+#define WME_AC_BK   0x00
+#define WME_AC_BE   0x01
+#define WME_AC_VI   0x02
+#define WME_AC_VO   0x03
+#define WME_ACI_MASK 0x03
+#define WME_AIFSN_MASK 0x03
+#define WME_AC_PRAM_LEN 16
+
+#define MAX_RECEIVE_BUFFER_SIZE 9100
+
+//UP Mapping to AC, using in MgntQuery_SequenceNumber() and maybe for DSCP
+//#define UP2AC(up)	((up<3) ? ((up==0)?1:0) : (up>>1))
+#define UP2AC(up) (		   \
+	((up) < 1) ? WME_AC_BE : \
+	((up) < 3) ? WME_AC_BK : \
+	((up) < 4) ? WME_AC_BE : \
+	((up) < 6) ? WME_AC_VI : \
+	WME_AC_VO)
+//AC Mapping to UP, using in Tx part for selecting the corresponding TX queue
+#define AC2UP(_ac)	(       \
+	((_ac) == WME_AC_VO) ? 6 : \
+	((_ac) == WME_AC_VI) ? 5 : \
+	((_ac) == WME_AC_BK) ? 1 : \
+	0)
+
+#define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
+#define ETHERNET_HEADER_SIZE    14      /* length of two Ethernet address plus ether type*/
+
+struct	ether_header {
+	u8 ether_dhost[ETHER_ADDR_LEN];
+	u8 ether_shost[ETHER_ADDR_LEN];
+	u16 ether_type;
+} __packed;
+
+#ifndef ETHERTYPE_PAE
+#define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
+#endif
+#ifndef ETHERTYPE_IP
+#define	ETHERTYPE_IP	0x0800		/* IP protocol */
+#endif
+
+typedef enum _erp_t {
+	ERP_NonERPpresent	= 0x01,
+	ERP_UseProtection	= 0x02,
+	ERP_BarkerPreambleMode = 0x04,
+} erp_t;
+
+
+struct ieee80211_network {
+	/* These entries are used to identify a unique network */
+	u8 bssid[ETH_ALEN];   /* u16 aligned! */
+	u8 channel;
+
+	// CCXv4 S59, MBSSID.
+	bool	bMBssidValid;
+	u8	MBssid[ETH_ALEN];    /* u16 aligned! */
+	u8	MBssidMask;
+	/* Ensure null-terminated for any debug msgs */
+	u8 ssid[IW_ESSID_MAX_SIZE + 1];
+	u8 ssid_len;
+	struct ieee80211_qos_data qos_data;
+
+	//added by amy for LEAP
+	bool	bWithAironetIE;
+	bool	bCkipSupported;
+	bool	bCcxRmEnable;
+	u16	CcxRmState[2];
+	// CCX 2 S38, WLAN Device Version Number element. Annie, 2006-08-20.
+	bool	bWithCcxVerNum;
+	u8	BssCcxVerNumber;
+	/* These are network statistics */
+	struct ieee80211_rx_stats stats;
+	u16 capability;
+	u8  rates[MAX_RATES_LENGTH];
+	u8  rates_len;
+	u8  rates_ex[MAX_RATES_EX_LENGTH];
+	u8  rates_ex_len;
+	unsigned long last_scanned;
+	u8  mode;
+	u32 flags;
+	u32 last_associate;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 listen_interval;
+	u16 atim_window;
+	u8  erp_value;
+	u8  wpa_ie[MAX_WPA_IE_LEN];
+	size_t wpa_ie_len;
+	u8  rsn_ie[MAX_WPA_IE_LEN];
+	size_t rsn_ie_len;
+
+	struct ieee80211_tim_parameters tim;
+	u8  dtim_period;
+	u8  dtim_data;
+	u32 last_dtim_sta_time[2];
+
+	//appeded for QoS
+	u8 wmm_info;
+	struct ieee80211_wmm_ac_param wmm_param[4];
+	u8 QoS_Enable;
+#ifdef THOMAS_TURBO
+	u8 Turbo_Enable;//enable turbo mode, added by thomas
+#endif
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+	// HT Related, by amy, 2008.04.29
+	BSS_HT	bssht;
+	// Add to handle broadcom AP management frame CCK rate.
+	bool broadcom_cap_exist;
+	bool ralink_cap_exist;
+	bool atheros_cap_exist;
+	bool cisco_cap_exist;
+	bool unknown_cap_exist;
+//	u8	berp_info;
+	bool	berp_info_valid;
+	bool buseprotection;
+	//put at the end of the structure.
+	struct list_head list;
+};
+
+enum ieee80211_state {
+
+	/* the card is not linked at all */
+	IEEE80211_NOLINK = 0,
+
+	/* IEEE80211_ASSOCIATING* are for BSS client mode
+	 * the driver shall not perform RX filtering unless
+	 * the state is LINKED.
+	 * The driver shall just check for the state LINKED and
+	 * defaults to NOLINK for ALL the other states (including
+	 * LINKED_SCANNING)
+	 */
+
+	/* the association procedure will start (wq scheduling)*/
+	IEEE80211_ASSOCIATING,
+	IEEE80211_ASSOCIATING_RETRY,
+
+	/* the association procedure is sending AUTH request*/
+	IEEE80211_ASSOCIATING_AUTHENTICATING,
+
+	/* the association procedure has successfully authentcated
+	 * and is sending association request
+	 */
+	IEEE80211_ASSOCIATING_AUTHENTICATED,
+
+	/* the link is ok. the card associated to a BSS or linked
+	 * to a ibss cell or acting as an AP and creating the bss
+	 */
+	IEEE80211_LINKED,
+
+	/* same as LINKED, but the driver shall apply RX filter
+	 * rules as we are in NO_LINK mode. As the card is still
+	 * logically linked, but it is doing a syncro site survey
+	 * then it will be back to LINKED state.
+	 */
+	IEEE80211_LINKED_SCANNING,
+
+};
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+
+#define CFG_IEEE80211_RESERVE_FCS (1<<0)
+#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
+#define CFG_IEEE80211_RTS (1<<2)
+
+#define IEEE80211_24GHZ_MIN_CHANNEL 1
+#define IEEE80211_24GHZ_MAX_CHANNEL 14
+#define IEEE80211_24GHZ_CHANNELS (IEEE80211_24GHZ_MAX_CHANNEL - \
+				  IEEE80211_24GHZ_MIN_CHANNEL + 1)
+
+#define IEEE80211_52GHZ_MIN_CHANNEL 34
+#define IEEE80211_52GHZ_MAX_CHANNEL 165
+#define IEEE80211_52GHZ_CHANNELS (IEEE80211_52GHZ_MAX_CHANNEL - \
+				  IEEE80211_52GHZ_MIN_CHANNEL + 1)
+
+
+
+struct tx_pending {
+	int frag;
+	struct ieee80211_txb *txb;
+};
+
+struct bandwidth_autoswitch {
+	long threshold_20Mhzto40Mhz;
+	long	threshold_40Mhzto20Mhz;
+	bool bforced_tx20Mhz;
+	bool bautoswitch_enable;
+};
+
+
+//added by amy for order
+
+#define REORDER_WIN_SIZE	128
+#define REORDER_ENTRY_NUM	128
+struct rx_reorder_entry {
+	struct list_head	List;
+	u16			SeqNum;
+	struct ieee80211_rxb *prxb;
+};
+//added by amy for order
+typedef enum _Fsync_State {
+	Default_Fsync,
+	HW_Fsync,
+	SW_Fsync
+} Fsync_State;
+
+// Power save mode configured.
+typedef	enum _RT_PS_MODE {
+	eActive,	// Active/Continuous access.
+	eMaxPs,		// Max power save mode.
+	eFastPs		// Fast power save mode.
+} RT_PS_MODE;
+
+typedef enum _IPS_CALLBACK_FUNCION {
+	IPS_CALLBACK_NONE = 0,
+	IPS_CALLBACK_MGNT_LINK_REQUEST = 1,
+	IPS_CALLBACK_JOIN_REQUEST = 2,
+} IPS_CALLBACK_FUNCION;
+
+typedef enum _RT_JOIN_ACTION {
+	RT_JOIN_INFRA   = 1,
+	RT_JOIN_IBSS  = 2,
+	RT_START_IBSS = 3,
+	RT_NO_ACTION  = 4,
+} RT_JOIN_ACTION;
+
+struct ibss_parms {
+	u16   atimWin;
+};
+#define MAX_NUM_RATES	264 // Max num of support rates element: 8,  Max num of ext. support rate: 255. 061122, by rcnjko.
+
+// RF state.
+typedef	enum _RT_RF_POWER_STATE {
+	eRfOn,
+	eRfSleep,
+	eRfOff
+} RT_RF_POWER_STATE;
+
+struct rt_power_save_control {
+
+	//
+	// Inactive Power Save(IPS) : Disable RF when disconnected
+	//
+	bool				bInactivePs;
+	bool				bIPSModeBackup;
+	bool				bSwRfProcessing;
+	RT_RF_POWER_STATE	eInactivePowerState;
+	struct work_struct	InactivePsWorkItem;
+	struct timer_list	InactivePsTimer;
+
+	// Return point for join action
+	IPS_CALLBACK_FUNCION	ReturnPoint;
+
+	// Recored Parameters for rescheduled JoinRequest
+	bool				bTmpBssDesc;
+	RT_JOIN_ACTION		tmpJoinAction;
+	struct ieee80211_network tmpBssDesc;
+
+	// Recored Parameters for rescheduled MgntLinkRequest
+	bool				bTmpScanOnly;
+	bool				bTmpActiveScan;
+	bool				bTmpFilterHiddenAP;
+	bool				bTmpUpdateParms;
+	u8					tmpSsidBuf[33];
+	struct octet_string			tmpSsid2Scan;
+	bool				bTmpSsid2Scan;
+	u8					tmpNetworkType;
+	u8					tmpChannelNumber;
+	u16					tmpBcnPeriod;
+	u8					tmpDtimPeriod;
+	u16					tmpmCap;
+	struct octet_string			tmpSuppRateSet;
+	u8					tmpSuppRateBuf[MAX_NUM_RATES];
+	bool				bTmpSuppRate;
+	struct ibss_parms			tmpIbpm;
+	bool				bTmpIbpm;
+
+	//
+	// Leisre Poswer Save : Disable RF if connected but traffic is not busy
+	//
+	bool				bLeisurePs;
+
+};
+
+typedef u32 RT_RF_CHANGE_SOURCE;
+#define RF_CHANGE_BY_SW		BIT(31)
+#define RF_CHANGE_BY_HW		BIT(30)
+#define RF_CHANGE_BY_PS		BIT(29)
+#define RF_CHANGE_BY_IPS	BIT(28)
+#define RF_CHANGE_BY_INIT	0	// Do not change the RFOff reason. Defined by Bruce, 2008-01-17.
+
+typedef enum {
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC,
+	COUNTRY_CODE_MIC,
+	COUNTRY_CODE_GLOBAL_DOMAIN
+} country_code_type_t;
+
+#define RT_MAX_LD_SLOT_NUM	10
+struct rt_link_detect {
+
+	u32				NumRecvBcnInPeriod;
+	u32				NumRecvDataInPeriod;
+
+	u32				RxBcnNum[RT_MAX_LD_SLOT_NUM];	// number of Rx beacon / CheckForHang_period  to determine link status
+	u32				RxDataNum[RT_MAX_LD_SLOT_NUM];	// number of Rx data / CheckForHang_period  to determine link status
+	u16				SlotNum;	// number of CheckForHang period to determine link status
+	u16				SlotIndex;
+
+	u32				NumTxOkInPeriod;
+	u32				NumRxOkInPeriod;
+	bool				bBusyTraffic;
+};
+
+
+struct ieee80211_device {
+	struct net_device *dev;
+	struct ieee80211_security sec;
+
+	//hw security related
+//	u8 hwsec_support; //support?
+	u8 hwsec_active;  //hw security active.
+	bool is_silent_reset;
+	bool ieee_up;
+	//added by amy
+	bool bSupportRemoteWakeUp;
+	RT_PS_MODE	dot11PowerSaveMode; // Power save mode configured.
+	bool actscanning;
+	bool beinretry;
+	RT_RF_POWER_STATE		eRFPowerState;
+	RT_RF_CHANGE_SOURCE	RfOffReason;
+	bool is_set_key;
+	//11n spec related I wonder if These info structure need to be moved out of ieee80211_device
+
+	//11n HT below
+	PRT_HIGH_THROUGHPUT	pHTInfo;
+	//struct timer_list		SwBwTimer;
+//	spinlock_t chnlop_spinlock;
+	spinlock_t bw_spinlock;
+
+	spinlock_t reorder_spinlock;
+	// for HT operation rate set.  we use this one for HT data rate to separate different descriptors
+	//the way fill this is the same as in the IE
+	u8	Regdot11HTOperationalRateSet[16];		//use RATR format
+	u8	dot11HTOperationalRateSet[16];		//use RATR format
+	u8	RegHTSuppRateSet[16];
+	u8				HTCurrentOperaRate;
+	u8				HTHighestOperaRate;
+	//wb added for rate operation mode to firmware
+	u8	bTxDisableRateFallBack;
+	u8	bTxUseDriverAssingedRate;
+	atomic_t	atm_chnlop;
+	atomic_t	atm_swbw;
+//	u8	HTHighestOperaRate;
+//	u8	HTCurrentOperaRate;
+
+	// 802.11e and WMM Traffic Stream Info (TX)
+	struct list_head		Tx_TS_Admit_List;
+	struct list_head		Tx_TS_Pending_List;
+	struct list_head		Tx_TS_Unused_List;
+	struct tx_ts_record		TxTsRecord[TOTAL_TS_NUM];
+	// 802.11e and WMM Traffic Stream Info (RX)
+	struct list_head		Rx_TS_Admit_List;
+	struct list_head		Rx_TS_Pending_List;
+	struct list_head		Rx_TS_Unused_List;
+	struct rx_ts_record		RxTsRecord[TOTAL_TS_NUM];
+	struct rx_reorder_entry	RxReorderEntry[128];
+	struct list_head		RxReorder_Unused_List;
+	// Qos related. Added by Annie, 2005-11-01.
+//	PSTA_QOS			pStaQos;
+	u8				ForcedPriority;		// Force per-packet priority 1~7. (default: 0, not to force it.)
+
+
+	/* Bookkeeping structures */
+	struct net_device_stats stats;
+	struct ieee80211_stats ieee_stats;
+	struct ieee80211_softmac_stats softmac_stats;
+
+	/* Probe / Beacon management */
+	struct list_head network_free_list;
+	struct list_head network_list;
+	struct ieee80211_network *networks;
+	int scans;
+	int scan_age;
+
+	int iw_mode; /* operating mode (IW_MODE_*) */
+	struct iw_spy_data spy_data;
+
+	spinlock_t lock;
+	spinlock_t wpax_suitlist_lock;
+
+	int tx_headroom; /* Set to size of any additional room needed at front
+			  * of allocated Tx SKBs
+			  */
+	u32 config;
+
+	/* WEP and other encryption related settings at the device level */
+	int open_wep; /* Set to 1 to allow unencrypted frames */
+	int auth_mode;
+	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
+				 * WEP key changes
+				 */
+
+	/* If the host performs {en,de}cryption, then set to 1 */
+	int host_encrypt;
+	int host_encrypt_msdu;
+	int host_decrypt;
+	/* host performs multicast decryption */
+	int host_mc_decrypt;
+
+	/* host should strip IV and ICV from protected frames */
+	/* meaningful only when hardware decryption is being used */
+	int host_strip_iv_icv;
+
+	int host_open_frag;
+	int host_build_iv;
+	int ieee802_1x; /* is IEEE 802.1X used */
+
+	/* WPA data */
+	bool bHalfWirelessN24GMode;
+	int wpa_enabled;
+	int drop_unencrypted;
+	int tkip_countermeasures;
+	int privacy_invoked;
+	size_t wpa_ie_len;
+	u8 *wpa_ie;
+	u8 ap_mac_addr[6];
+	u16 pairwise_key_type;
+	u16 group_key_type;
+	struct list_head crypt_deinit_list;
+	struct ieee80211_crypt_data *crypt[WEP_KEYS];
+	int tx_keyidx; /* default TX key index (crypt[tx_keyidx]) */
+	struct timer_list crypt_deinit_timer;
+	int crypt_quiesced;
+
+	int bcrx_sta_key; /* use individual keys to override default keys even
+			   * with RX of broad/multicast frames
+			   */
+
+	/* Fragmentation structures */
+	// each streaming contain a entry
+	struct ieee80211_frag_entry frag_cache[17][IEEE80211_FRAG_CACHE_LEN];
+	unsigned int frag_next_idx[17];
+	u16 fts; /* Fragmentation Threshold */
+#define DEFAULT_RTS_THRESHOLD 2346U
+#define MIN_RTS_THRESHOLD 1
+#define MAX_RTS_THRESHOLD 2346U
+	u16 rts; /* RTS threshold */
+
+	/* Association info */
+	u8 bssid[ETH_ALEN];
+
+	/* This stores infos for the current network.
+	 * Either the network we are associated in INFRASTRUCTURE
+	 * or the network that we are creating in MASTER mode.
+	 * ad-hoc is a mixture ;-).
+	 * Note that in infrastructure mode, even when not associated,
+	 * fields bssid and essid may be valid (if wpa_set and essid_set
+	 * are true) as thy carry the value set by the user via iwconfig
+	 */
+	struct ieee80211_network current_network;
+
+	enum ieee80211_state state;
+
+	int short_slot;
+	int reg_mode;
+	int mode;       /* A, B, G */
+	int modulation; /* CCK, OFDM */
+	int freq_band;  /* 2.4Ghz, 5.2Ghz, Mixed */
+	int abg_true;   /* ABG flag              */
+
+	/* used for forcing the ibss workqueue to terminate
+	 * without wait for the syncro scan to terminate
+	 */
+	short sync_scan_hurryup;
+
+	int perfect_rssi;
+	int worst_rssi;
+
+	u16 prev_seq_ctl;       /* used to drop duplicate frames */
+
+	/* map of allowed channels. 0 is dummy */
+	// FIXME: remember to default to a basic channel plan depending of the PHY type
+	void *dot11d_info;
+	bool bGlobalDomain;
+	int rate;       /* current rate */
+	int basic_rate;
+	//FIXME: pleace callback, see if redundant with softmac_features
+	short active_scan;
+
+	/* this contains flags for selectively enable softmac support */
+	u16 softmac_features;
+
+	/* if the sequence control field is not filled by HW */
+	u16 seq_ctrl[5];
+
+	/* association procedure transaction sequence number */
+	u16 associate_seq;
+
+	/* AID for RTXed association responses */
+	u16 assoc_id;
+
+	/* power save mode related*/
+	short ps;
+	short sta_sleep;
+	int ps_timeout;
+	int ps_period;
+	struct work_struct ps_task;
+	u32 ps_th;
+	u32 ps_tl;
+
+	short raw_tx;
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	short queue_stop;
+	short scanning;
+	short proto_started;
+
+	struct mutex wx_mutex;
+	struct mutex scan_mutex;
+
+	spinlock_t mgmt_tx_lock;
+	spinlock_t beacon_lock;
+
+	short beacon_txing;
+
+	short wap_set;
+	short ssid_set;
+
+	u8  wpax_type_set;    //{added by David, 2006.9.28}
+	u32 wpax_type_notify; //{added by David, 2006.9.26}
+
+	/* QoS related flag */
+	s8  init_wmmparam_flag;
+	/* set on initialization */
+	u8  qos_support;
+
+	/* for discarding duplicated packets in IBSS */
+	struct list_head ibss_mac_hash[IEEE_IBSS_MAC_HASH_SIZE];
+
+	/* for discarding duplicated packets in BSS */
+	u16 last_rxseq_num[17]; /* rx seq previous per-tid */
+	u16 last_rxfrag_num[17];/* tx frag previous per-tid */
+	unsigned long last_packet_time[17];
+
+	/* for PS mode */
+	unsigned long last_rx_ps_time;
+
+	/* used if IEEE_SOFTMAC_SINGLE_QUEUE is set */
+	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
+	int mgmt_queue_head;
+	int mgmt_queue_tail;
+//{ added for rtl819x
+#define IEEE80211_QUEUE_LIMIT 128
+	u8 AsocRetryCount;
+	unsigned int hw_header;
+	struct sk_buff_head skb_waitQ[MAX_QUEUE_SIZE];
+	struct sk_buff_head  skb_aggQ[MAX_QUEUE_SIZE];
+	struct sk_buff_head  skb_drv_aggQ[MAX_QUEUE_SIZE];
+	u32	sta_edca_param[4];
+	bool aggregation;
+	// Enable/Disable Rx immediate BA capability.
+	bool enable_rx_imm_BA;
+	bool bibsscoordinator;
+
+	//+by amy for DM ,080515
+	//Dynamic Tx power for near/far range enable/Disable  , by amy , 2008-05-15
+	bool	bdynamic_txpower_enable;
+
+	bool bCTSToSelfEnable;
+	u8	CTSToSelfTH;
+
+	u32	fsync_time_interval;
+	u32	fsync_rate_bitmap;
+	u8	fsync_rssi_threshold;
+	bool	bfsync_enable;
+
+	u8	fsync_multiple_timeinterval;		// FsyncMultipleTimeInterval * FsyncTimeInterval
+	u32	fsync_firstdiff_ratethreshold;		// low threshold
+	u32	fsync_seconddiff_ratethreshold;	 // decrease threshold
+	Fsync_State			fsync_state;
+	bool		bis_any_nonbepkts;
+	//20Mhz 40Mhz AutoSwitch Threshold
+	struct bandwidth_autoswitch bandwidth_auto_switch;
+	//for txpower tracking
+	bool FwRWRF;
+
+	//added by amy for AP roaming
+	struct rt_link_detect LinkDetectInfo;
+	//added by amy for ps
+	struct rt_power_save_control PowerSaveControl;
+//}
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	struct  tx_pending tx_pending;
+
+	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
+	struct timer_list associate_timer;
+
+	/* used if IEEE_SOFTMAC_BEACONS is set */
+	struct timer_list beacon_timer;
+	struct work_struct associate_complete_wq;
+	struct work_struct associate_procedure_wq;
+	struct delayed_work softmac_scan_wq;
+	struct delayed_work associate_retry_wq;
+	 struct delayed_work start_ibss_wq;
+	struct work_struct wx_sync_scan_wq;
+	struct workqueue_struct *wq;
+	// Qos related. Added by Annie, 2005-11-01.
+	//STA_QOS  StaQos;
+
+	//u32 STA_EDCA_PARAM[4];
+	//CHANNEL_ACCESS_SETTING ChannelAccessSetting;
+
+	struct ieee80211_rxb *stats_IndicateArray[REORDER_WIN_SIZE];
+
+	/* Callback functions */
+	void (*set_security)(struct net_device *dev,
+			     struct ieee80211_security *sec);
+
+	/* Used to TX data frame by using txb structs.
+	 * this is not used if in the softmac_features
+	 * is set the flag IEEE_SOFTMAC_TX_QUEUE
+	 */
+	int (*hard_start_xmit)(struct ieee80211_txb *txb,
+			       struct net_device *dev);
+
+	int (*reset_port)(struct net_device *dev);
+	int (*is_queue_full)(struct net_device *dev, int pri);
+
+	int (*handle_management)(struct net_device *dev,
+				  struct ieee80211_network *network, u16 type);
+	int (*is_qos_active)(struct net_device *dev, struct sk_buff *skb);
+
+	/* Softmac-generated frames (management) are TXed via this
+	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
+	 * not set. As some cards may have different HW queues that
+	 * one might want to use for data and management frames
+	 * the option to have two callbacks might be useful.
+	 * This function can't sleep.
+	 */
+	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev);
+
+	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
+	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
+	 * frames. I the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * then also management frames are sent via this callback.
+	 * This function can't sleep.
+	 */
+	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev, int rate);
+
+	/* stops the HW queue for DATA frames. Useful to avoid
+	 * waste time to TX data frame when we are reassociating
+	 * This function can sleep.
+	 */
+	void (*data_hard_stop)(struct net_device *dev);
+
+	/* OK this is complementar to data_poll_hard_stop */
+	void (*data_hard_resume)(struct net_device *dev);
+
+	/* ask to the driver to retune the radio .
+	 * This function can sleep. the driver should ensure
+	 * the radio has been switched before return.
+	 */
+	void (*set_chan)(struct net_device *dev, short ch);
+
+	/* These are not used if the ieee stack takes care of
+	 * scanning (IEEE_SOFTMAC_SCAN feature set).
+	 * In this case only the set_chan is used.
+	 *
+	 * The syncro version is similar to the start_scan but
+	 * does not return until all channels has been scanned.
+	 * this is called in user context and should sleep,
+	 * it is called in a work_queue when switching to ad-hoc mode
+	 * or in behalf of iwlist scan when the card is associated
+	 * and root user ask for a scan.
+	 * the function stop_scan should stop both the syncro and
+	 * background scanning and can sleep.
+	 * The function start_scan should initiate the background
+	 * scanning and can't sleep.
+	 */
+	void (*scan_syncro)(struct net_device *dev);
+	void (*start_scan)(struct net_device *dev);
+	void (*stop_scan)(struct net_device *dev);
+
+	/* indicate the driver that the link state is changed
+	 * for example it may indicate the card is associated now.
+	 * Driver might be interested in this to apply RX filter
+	 * rules or simply light the LINK led
+	 */
+	void (*link_change)(struct net_device *dev);
+
+	/* these two function indicates to the HW when to start
+	 * and stop to send beacons. This is used when the
+	 * IEEE_SOFTMAC_BEACONS is not set. For now the
+	 * stop_send_bacons is NOT guaranteed to be called only
+	 * after start_send_beacons.
+	 */
+	void (*start_send_beacons)(struct net_device *dev, u16 tx_rate);
+	void (*stop_send_beacons)(struct net_device *dev);
+
+	/* power save mode related */
+	void (*sta_wake_up)(struct net_device *dev);
+	void (*ps_request_tx_ack)(struct net_device *dev);
+	void (*enter_sleep_state)(struct net_device *dev, u32 th, u32 tl);
+	short (*ps_is_queue_empty)(struct net_device *dev);
+	int (*handle_beacon)(struct net_device *dev, struct ieee80211_beacon *beacon, struct ieee80211_network *network);
+	int (*handle_assoc_response)(struct net_device *dev, struct ieee80211_assoc_response_frame *resp, struct ieee80211_network *network);
+
+
+	/* check whether Tx hw resource available */
+	short (*check_nic_enough_desc)(struct net_device *dev, int queue_index);
+	//added by wb for HT related
+//	void (*SwChnlByTimerHandler)(struct net_device *dev, int channel);
+	void (*SetBWModeHandler)(struct net_device *dev, enum ht_channel_width Bandwidth, enum ht_extension_chan_offset Offset);
+//	void (*UpdateHalRATRTableHandler)(struct net_device* dev, u8* pMcsRate);
+	bool (*GetNmodeSupportBySecCfg)(struct net_device *dev);
+	void (*SetWirelessMode)(struct net_device *dev, u8 wireless_mode);
+	bool (*GetHalfNmodeSupportByAPsHandler)(struct net_device *dev);
+	void (*InitialGainHandler)(struct net_device *dev, u8 Operation);
+
+	/* This must be the last item so that it points to the data
+	 * allocated beyond this structure by alloc_ieee80211
+	 */
+	u8 priv[];
+};
+
+#define IEEE_A            (1<<0)
+#define IEEE_B            (1<<1)
+#define IEEE_G            (1<<2)
+#define IEEE_N_24G        (1<<4)
+#define IEEE_N_5G         (1<<5)
+#define IEEE_MODE_MASK    (IEEE_A | IEEE_B | IEEE_G)
+
+/* Generate a 802.11 header */
+
+/* Uses the channel change callback directly
+ * instead of [start/stop] scan callbacks
+ */
+#define IEEE_SOFTMAC_SCAN (1<<2)
+
+/* Perform authentication and association handshake */
+#define IEEE_SOFTMAC_ASSOCIATE (1<<3)
+
+/* Generate probe requests */
+#define IEEE_SOFTMAC_PROBERQ (1<<4)
+
+/* Generate respones to probe requests */
+#define IEEE_SOFTMAC_PROBERS (1<<5)
+
+/* The ieee802.11 stack will manages the netif queue
+ * wake/stop for the driver, taking care of 802.11
+ * fragmentation. See softmac.c for details.
+ */
+#define IEEE_SOFTMAC_TX_QUEUE (1<<7)
+
+/* Uses only the softmac_data_hard_start_xmit
+ * even for TX management frames.
+ */
+#define IEEE_SOFTMAC_SINGLE_QUEUE (1<<8)
+
+/* Generate beacons.  The stack will enqueue beacons
+ * to the card
+ */
+#define IEEE_SOFTMAC_BEACONS (1<<6)
+
+static inline void *ieee80211_priv(struct net_device *dev)
+{
+	return ((struct ieee80211_device *)netdev_priv(dev))->priv;
+}
+
+static inline int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+static inline int ieee80211_is_valid_mode(struct ieee80211_device *ieee, int mode)
+{
+	/*
+	 * It is possible for both access points and our device to support
+	 * combinations of modes, so as long as there is one valid combination
+	 * of ap/device supported modes, then return success
+	 *
+	 */
+	if ((mode & IEEE_A) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_52GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_G) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_B) &&
+	    (ieee->modulation & IEEE80211_CCK_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	return 0;
+}
+
+static inline int ieee80211_get_hdrlen(u16 fc)
+{
+	int hdrlen = IEEE80211_3ADDR_LEN;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case IEEE80211_FTYPE_DATA:
+		if ((fc & IEEE80211_FCTL_FROMDS) && (fc & IEEE80211_FCTL_TODS))
+			hdrlen = IEEE80211_4ADDR_LEN; /* Addr4 */
+		if (IEEE80211_QOS_HAS_SEQ(fc))
+			hdrlen += 2; /* QOS ctrl*/
+		break;
+	case IEEE80211_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case IEEE80211_STYPE_CTS:
+		case IEEE80211_STYPE_ACK:
+			hdrlen = IEEE80211_1ADDR_LEN;
+			break;
+		default:
+			hdrlen = IEEE80211_2ADDR_LEN;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+static inline u8 *ieee80211_get_payload(struct rtl_80211_hdr *hdr)
+{
+	switch (ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl))) {
+	case IEEE80211_1ADDR_LEN:
+		return ((struct rtl_80211_hdr_1addr *)hdr)->payload;
+	case IEEE80211_2ADDR_LEN:
+		return ((struct rtl_80211_hdr_2addr *)hdr)->payload;
+	case IEEE80211_3ADDR_LEN:
+		return ((struct rtl_80211_hdr_3addr *)hdr)->payload;
+	case IEEE80211_4ADDR_LEN:
+		return ((struct rtl_80211_hdr_4addr *)hdr)->payload;
+	}
+	return NULL;
+}
+
+static inline int ieee80211_is_ofdm_rate(u8 rate)
+{
+	switch (rate & ~IEEE80211_BASIC_RATE_MASK) {
+	case IEEE80211_OFDM_RATE_6MB:
+	case IEEE80211_OFDM_RATE_9MB:
+	case IEEE80211_OFDM_RATE_12MB:
+	case IEEE80211_OFDM_RATE_18MB:
+	case IEEE80211_OFDM_RATE_24MB:
+	case IEEE80211_OFDM_RATE_36MB:
+	case IEEE80211_OFDM_RATE_48MB:
+	case IEEE80211_OFDM_RATE_54MB:
+		return 1;
+	}
+	return 0;
+}
+
+static inline int ieee80211_is_cck_rate(u8 rate)
+{
+	switch (rate & ~IEEE80211_BASIC_RATE_MASK) {
+	case IEEE80211_CCK_RATE_1MB:
+	case IEEE80211_CCK_RATE_2MB:
+	case IEEE80211_CCK_RATE_5MB:
+	case IEEE80211_CCK_RATE_11MB:
+		return 1;
+	}
+	return 0;
+}
+
+
+/* ieee80211.c */
+void free_ieee80211(struct net_device *dev);
+struct net_device *alloc_ieee80211(int sizeof_priv);
+
+int ieee80211_set_encryption(struct ieee80211_device *ieee);
+
+/* ieee80211_tx.c */
+
+int ieee80211_encrypt_fragment(struct ieee80211_device *ieee,
+			       struct sk_buff *frag, int hdr_len);
+
+netdev_tx_t ieee80211_xmit(struct sk_buff *skb, struct net_device *dev);
+void ieee80211_txb_free(struct ieee80211_txb *txb);
+
+
+/* ieee80211_rx.c */
+int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
+		 struct ieee80211_rx_stats *rx_stats);
+void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+		      struct rtl_80211_hdr_4addr *header,
+		      struct ieee80211_rx_stats *stats);
+
+/* ieee80211_wx.c */
+int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *key);
+int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key);
+int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key);
+int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra);
+int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra);
+int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  struct iw_param *data, char *extra);
+int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra);
+int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
+
+/* ieee80211_softmac.c */
+short ieee80211_is_54g(const struct ieee80211_network *net);
+short ieee80211_is_shortslot(const struct ieee80211_network *net);
+int ieee80211_rx_frame_softmac(struct ieee80211_device *ieee,
+			       struct sk_buff *skb,
+			       struct ieee80211_rx_stats *rx_stats,
+			       u16 type, u16 stype);
+void ieee80211_softmac_new_net(struct ieee80211_device *ieee,
+			       struct ieee80211_network *net);
+
+void SendDisassociation(struct ieee80211_device *ieee, u8 *asSta, u8 asRsn);
+void ieee80211_softmac_xmit(struct ieee80211_txb *txb,
+			    struct ieee80211_device *ieee);
+
+void ieee80211_stop_send_beacons(struct ieee80211_device *ieee);
+void notify_wx_assoc_event(struct ieee80211_device *ieee);
+void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee);
+void ieee80211_start_bss(struct ieee80211_device *ieee);
+void ieee80211_start_master_bss(struct ieee80211_device *ieee);
+void ieee80211_start_ibss(struct ieee80211_device *ieee);
+void ieee80211_softmac_init(struct ieee80211_device *ieee);
+void ieee80211_softmac_free(struct ieee80211_device *ieee);
+void ieee80211_associate_abort(struct ieee80211_device *ieee);
+void ieee80211_disassociate(struct ieee80211_device *ieee);
+void ieee80211_stop_scan(struct ieee80211_device *ieee);
+void ieee80211_start_scan_syncro(struct ieee80211_device *ieee);
+void ieee80211_check_all_nets(struct ieee80211_device *ieee);
+void ieee80211_start_protocol(struct ieee80211_device *ieee);
+void ieee80211_stop_protocol(struct ieee80211_device *ieee);
+void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee);
+void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee);
+void ieee80211_reset_queue(struct ieee80211_device *ieee);
+void ieee80211_wake_queue(struct ieee80211_device *ieee);
+void ieee80211_stop_queue(struct ieee80211_device *ieee);
+struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee);
+void ieee80211_start_send_beacons(struct ieee80211_device *ieee);
+int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee,
+				   struct iw_point *p);
+void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success);
+
+void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee);
+
+/* ieee80211_crypt_ccmp&tkip&wep.c */
+void ieee80211_tkip_null(void);
+
+int ieee80211_crypto_init(void);
+void ieee80211_crypto_deinit(void);
+int ieee80211_crypto_tkip_init(void);
+void ieee80211_crypto_tkip_exit(void);
+int ieee80211_crypto_ccmp_init(void);
+void ieee80211_crypto_ccmp_exit(void);
+int ieee80211_crypto_wep_init(void);
+void ieee80211_crypto_wep_exit(void);
+
+/* ieee80211_softmac_wx.c */
+
+int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *ext);
+
+int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra);
+
+int ieee80211_wx_get_essid(struct ieee80211_device *ieee,
+			   struct iw_request_info *a,
+			   union iwreq_data *wrqu, char *b);
+
+int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_set_mode(struct ieee80211_device *ieee,
+			  struct iw_request_info *a,
+			  union iwreq_data *wrqu, char *b);
+
+int ieee80211_wx_set_scan(struct ieee80211_device *ieee,
+			  struct iw_request_info *a,
+			  union iwreq_data *wrqu, char *b);
+
+int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
+			   struct iw_request_info *a,
+			   union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_get_mode(struct ieee80211_device *ieee,
+			  struct iw_request_info *a,
+			  union iwreq_data *wrqu, char *b);
+
+int ieee80211_wx_set_freq(struct ieee80211_device *ieee,
+			  struct iw_request_info *a,
+			  union iwreq_data *wrqu, char *b);
+
+int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
+			  struct iw_request_info *a,
+			  union iwreq_data *wrqu, char *b);
+
+/* ieee80211_module.c */
+#ifdef CONFIG_IEEE80211_DEBUG
+int ieee80211_debug_init(void);
+void ieee80211_debug_exit(void);
+#else
+static inline int ieee80211_debug_init(void) { return 0; }
+static inline void ieee80211_debug_exit(void) { }
+#endif
+
+//extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+void ieee80211_wx_sync_scan_wq(struct work_struct *work);
+
+
+int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
+			   struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_get_name(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_set_power(struct ieee80211_device *ieee,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_get_power(struct ieee80211_device *ieee,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *wrqu, char *extra);
+//HT
+#define MAX_RECEIVE_BUFFER_SIZE 9100  //
+void HTDebugHTCapability(u8 *CapIE, u8 *TitleString);
+void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString);
+
+void HTSetConnectBwMode(struct ieee80211_device *ieee,
+			enum ht_channel_width Bandwidth, enum ht_extension_chan_offset Offset);
+void HTUpdateDefaultSetting(struct ieee80211_device *ieee);
+void HTConstructCapabilityElement(struct ieee80211_device *ieee, u8 *posHTCap,
+				  u8 *len, u8 isEncrypt);
+void HTConstructInfoElement(struct ieee80211_device *ieee, u8 *posHTInfo,
+			    u8 *len, u8 isEncrypt);
+void HTConstructRT2RTAggElement(struct ieee80211_device *ieee, u8 *posRT2RTAgg,
+				u8 *len);
+void HTOnAssocRsp(struct ieee80211_device *ieee);
+void HTInitializeHTInfo(struct ieee80211_device *ieee);
+void HTInitializeBssDesc(PBSS_HT pBssHT);
+void HTResetSelfAndSavePeerSetting(struct ieee80211_device *ieee,
+				   struct ieee80211_network *pNetwork);
+void HTUpdateSelfAndPeerSetting(struct ieee80211_device *ieee,
+				struct ieee80211_network *pNetwork);
+u8 HTGetHighestMCSRate(struct ieee80211_device *ieee,
+		       u8 *pMCSRateSet, u8 *pMCSFilter);
+extern u8 MCS_FILTER_ALL[];
+extern u16 MCS_DATA_RATE[2][2][77];
+u8 HTCCheck(struct ieee80211_device *ieee, u8 *pFrame);
+void HTResetIOTSetting(PRT_HIGH_THROUGHPUT pHTInfo);
+bool IsHTHalfNmodeAPs(struct ieee80211_device *ieee);
+u16 TxCountToDataRate(struct ieee80211_device *ieee, u8 nDataRate);
+//function in BAPROC.c
+int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb);
+int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb);
+int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb);
+void TsInitAddBA(struct ieee80211_device *ieee, struct tx_ts_record *pTS,
+		 u8 Policy, u8 bOverwritePending);
+void TsInitDelBA(struct ieee80211_device *ieee,
+		 struct ts_common_info *pTsCommonInfo, enum tr_select TxRxSelect);
+void BaSetupTimeOut(struct timer_list *t);
+void TxBaInactTimeout(struct timer_list *t);
+void RxBaInactTimeout(struct timer_list *t);
+void ResetBaEntry(struct ba_record *pBA);
+//function in TS.c
+bool GetTs(
+	struct ieee80211_device		*ieee,
+	struct ts_common_info           **ppTS,
+	u8                              *Addr,
+	u8                              TID,
+	enum tr_select                  TxRxSelect,  //Rx:1, Tx:0
+	bool                            bAddNewTs
+	);
+void TSInitialize(struct ieee80211_device *ieee);
+void TsStartAddBaProcess(struct ieee80211_device *ieee, struct tx_ts_record   *pTxTS);
+void RemovePeerTS(struct ieee80211_device *ieee, u8 *Addr);
+void RemoveAllTS(struct ieee80211_device *ieee);
+void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee);
+
+extern const long ieee80211_wlan_frequencies[];
+
+static inline void ieee80211_increment_scans(struct ieee80211_device *ieee)
+{
+	ieee->scans++;
+}
+
+static inline int ieee80211_get_scans(struct ieee80211_device *ieee)
+{
+	return ieee->scans;
+}
+
+static inline const char *escape_essid(const char *essid, u8 essid_len)
+{
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+
+	if (ieee80211_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	snprintf(escaped, sizeof(escaped), "%*pE", essid_len, essid);
+	return escaped;
+}
+
+/* For the function is more related to hardware setting, it's better to use the
+ * ieee handler to refer to it.
+ */
+short check_nic_enough_desc(struct net_device *dev, int queue_index);
+int ieee80211_data_xmit(struct sk_buff *skb, struct net_device *dev);
+int ieee80211_parse_info_param(struct ieee80211_device *ieee,
+			       struct ieee80211_info_element *info_element,
+			       u16 length,
+			       struct ieee80211_network *network,
+			       struct ieee80211_rx_stats *stats);
+
+void ieee80211_indicate_packets(struct ieee80211_device *ieee,
+				struct ieee80211_rxb **prxbIndicateArray,
+				u8 index);
+#define RT_ASOC_RETRY_LIMIT	5
+#endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c
new file mode 100644
index 000000000..840db6250
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c
@@ -0,0 +1,235 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypto routines
+ *
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ * Portions Copyright (C) 2004, Intel Corporation <jketreno@linux.intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+
+#include "ieee80211.h"
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("HostAP crypto");
+MODULE_LICENSE("GPL");
+
+struct ieee80211_crypto_alg {
+	struct list_head list;
+	struct ieee80211_crypto_ops *ops;
+};
+
+
+struct ieee80211_crypto {
+	struct list_head algs;
+	spinlock_t lock;
+};
+
+static struct ieee80211_crypto *hcrypt;
+
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *ieee,
+					   int force)
+{
+	struct list_head *ptr, *n;
+	struct ieee80211_crypt_data *entry;
+
+	for (ptr = ieee->crypt_deinit_list.next, n = ptr->next;
+	     ptr != &ieee->crypt_deinit_list; ptr = n, n = ptr->next) {
+		entry = list_entry(ptr, struct ieee80211_crypt_data, list);
+
+		if (atomic_read(&entry->refcnt) != 0 && !force)
+			continue;
+
+		list_del(ptr);
+
+		if (entry->ops)
+			entry->ops->deinit(entry->priv);
+		kfree(entry);
+	}
+}
+
+void ieee80211_crypt_deinit_handler(struct timer_list *t)
+{
+	struct ieee80211_device *ieee = from_timer(ieee, t, crypt_deinit_timer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	ieee80211_crypt_deinit_entries(ieee, 0);
+	if (!list_empty(&ieee->crypt_deinit_list)) {
+		netdev_dbg(ieee->dev, "%s: entries remaining in delayed crypt deletion list\n",
+				ieee->dev->name);
+		ieee->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&ieee->crypt_deinit_timer);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
+				    struct ieee80211_crypt_data **crypt)
+{
+	struct ieee80211_crypt_data *tmp;
+	unsigned long flags;
+
+	if (!(*crypt))
+		return;
+
+	tmp = *crypt;
+	*crypt = NULL;
+
+	/* must not run ops->deinit() while there may be pending encrypt or
+	 * decrypt operations. Use a list of delayed deinits to avoid needing
+	 * locking.
+	 */
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	list_add(&tmp->list, &ieee->crypt_deinit_list);
+	if (!timer_pending(&ieee->crypt_deinit_timer)) {
+		ieee->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&ieee->crypt_deinit_timer);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops)
+{
+	unsigned long flags;
+	struct ieee80211_crypto_alg *alg;
+
+	if (!hcrypt)
+		return -1;
+
+	alg = kzalloc(sizeof(*alg), GFP_KERNEL);
+	if (!alg)
+		return -ENOMEM;
+
+	alg->ops = ops;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	list_add(&alg->list, &hcrypt->algs);
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	pr_debug("ieee80211_crypt: registered algorithm '%s'\n",
+	       ops->name);
+
+	return 0;
+}
+
+int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops)
+{
+	unsigned long flags;
+	struct list_head *ptr;
+	struct ieee80211_crypto_alg *del_alg = NULL;
+
+	if (!hcrypt)
+		return -1;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *)ptr;
+		if (alg->ops == ops) {
+			list_del(&alg->list);
+			del_alg = alg;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	if (del_alg) {
+		pr_debug("ieee80211_crypt: unregistered algorithm '%s'\n",
+				ops->name);
+		kfree(del_alg);
+	}
+
+	return del_alg ? 0 : -1;
+}
+
+
+struct ieee80211_crypto_ops *ieee80211_get_crypto_ops(const char *name)
+{
+	unsigned long flags;
+	struct list_head *ptr;
+	struct ieee80211_crypto_alg *found_alg = NULL;
+
+	if (!hcrypt)
+		return NULL;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *)ptr;
+		if (strcmp(alg->ops->name, name) == 0) {
+			found_alg = alg;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	if (found_alg)
+		return found_alg->ops;
+	return NULL;
+}
+
+
+static void *ieee80211_crypt_null_init(int keyidx) { return (void *)1; }
+static void ieee80211_crypt_null_deinit(void *priv) {}
+
+static struct ieee80211_crypto_ops ieee80211_crypt_null = {
+	.name			= "NULL",
+	.init			= ieee80211_crypt_null_init,
+	.deinit			= ieee80211_crypt_null_deinit,
+	.encrypt_mpdu		= NULL,
+	.decrypt_mpdu		= NULL,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= NULL,
+	.get_key		= NULL,
+	.extra_prefix_len	= 0,
+	.extra_postfix_len	= 0,
+	.owner			= THIS_MODULE,
+};
+
+int __init ieee80211_crypto_init(void)
+{
+	int ret = -ENOMEM;
+
+	hcrypt = kzalloc(sizeof(*hcrypt), GFP_KERNEL);
+	if (!hcrypt)
+		goto out;
+
+	INIT_LIST_HEAD(&hcrypt->algs);
+	spin_lock_init(&hcrypt->lock);
+
+	ret = ieee80211_register_crypto_ops(&ieee80211_crypt_null);
+	if (ret < 0) {
+		kfree(hcrypt);
+		hcrypt = NULL;
+	}
+out:
+	return ret;
+}
+
+void ieee80211_crypto_deinit(void)
+{
+	struct list_head *ptr, *n;
+
+	if (!hcrypt)
+		return;
+
+	for (ptr = hcrypt->algs.next, n = ptr->next; ptr != &hcrypt->algs;
+	     ptr = n, n = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *)ptr;
+		list_del(ptr);
+		pr_debug("ieee80211_crypt: unregistered algorithm '%s' (deinit)\n",
+				alg->ops->name);
+		kfree(alg);
+	}
+
+	kfree(hcrypt);
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h
new file mode 100644
index 000000000..d3bd5598b
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Original code based on Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ *
+ * Copyright (c) 2004, Intel Corporation
+ */
+
+/*
+ * This file defines the interface to the ieee80211 crypto module.
+ */
+#ifndef IEEE80211_CRYPT_H
+#define IEEE80211_CRYPT_H
+
+#include <linux/skbuff.h>
+
+struct ieee80211_crypto_ops {
+	const char *name;
+
+	/* init new crypto context (e.g., allocate private data space,
+	 * select IV, etc.); returns NULL on failure or pointer to allocated
+	 * private data on success
+	 */
+	void * (*init)(int keyidx);
+
+	/* deinitialize crypto context and free allocated private data */
+	void (*deinit)(void *priv);
+
+	/* encrypt/decrypt return < 0 on error or >= 0 on success. The return
+	 * value from decrypt_mpdu is passed as the keyidx value for
+	 * decrypt_msdu. skb must have enough head and tail room for the
+	 * encryption; if not, error will be returned; these functions are
+	 * called for all MPDUs (i.e., fragments).
+	 */
+	int (*encrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+
+	/* These functions are called for full MSDUs, i.e. full frames.
+	 * These can be NULL if full MSDU operations are not needed.
+	 */
+	int (*encrypt_msdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_msdu)(struct sk_buff *skb, int keyidx, int hdr_len,
+			    void *priv);
+
+	int (*set_key)(void *key, int len, u8 *seq, void *priv);
+	int (*get_key)(void *key, int len, u8 *seq, void *priv);
+
+	/* procfs handler for printing out key information and possible
+	 * statistics
+	 */
+	char * (*print_stats)(char *p, void *priv);
+
+	/* maximum number of bytes added by encryption; encrypt buf is
+	 * allocated with extra_prefix_len bytes, copy of in_buf, and
+	 * extra_postfix_len; encrypt need not use all this space, but
+	 * the result must start at the beginning of the buffer and correct
+	 * length must be returned
+	 */
+	int extra_prefix_len, extra_postfix_len;
+
+	struct module *owner;
+};
+
+struct ieee80211_crypt_data {
+	struct list_head list; /* delayed deletion list */
+	struct ieee80211_crypto_ops *ops;
+	void *priv;
+	atomic_t refcnt;
+};
+
+int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops);
+int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops);
+struct ieee80211_crypto_ops *ieee80211_get_crypto_ops(const char *name);
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *ieee, int force);
+void ieee80211_crypt_deinit_handler(struct timer_list *t);
+void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
+				    struct ieee80211_crypt_data **crypt);
+
+#endif
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
new file mode 100644
index 000000000..f17d07dad
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -0,0 +1,421 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypt: host-based CCMP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/string.h>
+#include <linux/wireless.h>
+
+#include "ieee80211.h"
+
+#include <linux/crypto.h>
+#include <crypto/aead.h>
+    #include <linux/scatterlist.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: CCMP");
+MODULE_LICENSE("GPL");
+
+#define AES_BLOCK_LEN 16
+#define CCMP_HDR_LEN 8
+#define CCMP_MIC_LEN 8
+#define CCMP_TK_LEN 16
+#define CCMP_PN_LEN 6
+
+struct ieee80211_ccmp_data {
+	u8 key[CCMP_TK_LEN];
+	int key_set;
+
+	u8 tx_pn[CCMP_PN_LEN];
+	u8 rx_pn[CCMP_PN_LEN];
+
+	u32 dot11RSNAStatsCCMPFormatErrors;
+	u32 dot11RSNAStatsCCMPReplays;
+	u32 dot11RSNAStatsCCMPDecryptErrors;
+
+	int key_idx;
+
+	struct crypto_aead *tfm;
+
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 tx_aad[2 * AES_BLOCK_LEN];
+	u8 rx_aad[2 * AES_BLOCK_LEN];
+};
+
+static void *ieee80211_ccmp_init(int key_idx)
+{
+	struct ieee80211_ccmp_data *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		goto fail;
+	priv->key_idx = key_idx;
+
+	priv->tfm = crypto_alloc_aead("ccm(aes)", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tfm)) {
+		pr_debug("ieee80211_crypt_ccmp: could not allocate crypto API aes\n");
+		priv->tfm = NULL;
+		goto fail;
+	}
+
+	return priv;
+
+fail:
+	if (priv) {
+		if (priv->tfm)
+			crypto_free_aead(priv->tfm);
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+static void ieee80211_ccmp_deinit(void *priv)
+{
+	struct ieee80211_ccmp_data *_priv = priv;
+
+	if (_priv && _priv->tfm)
+		crypto_free_aead(_priv->tfm);
+	kfree(priv);
+}
+
+static int ccmp_init_iv_and_aad(struct rtl_80211_hdr_4addr *hdr,
+			     u8 *pn, u8 *iv, u8 *aad)
+{
+	u8 *pos, qc = 0;
+	size_t aad_len;
+	u16 fc;
+	int a4_included, qc_included;
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	a4_included = ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		       (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS));
+	/* qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
+	 *	       (WLAN_FC_GET_STYPE(fc) & 0x08));
+	 */
+	/* fixed by David :2006.9.6 */
+	qc_included = (WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
+		       (WLAN_FC_GET_STYPE(fc) & 0x80);
+	aad_len = 22;
+	if (a4_included)
+		aad_len += 6;
+	if (qc_included) {
+		pos = (u8 *)&hdr->addr4;
+		if (a4_included)
+			pos += 6;
+		qc = *pos & 0x0f;
+		aad_len += 2;
+	}
+
+	/* In CCM, the initial vectors (IV) used for CTR mode encryption and CBC
+	 * mode authentication are not allowed to collide, yet both are derived
+	 * from the same vector. We only set L := 1 here to indicate that the
+	 * data size can be represented in (L+1) bytes. The CCM layer will take
+	 * care of storing the data length in the top (L+1) bytes and setting
+	 * and clearing the other bits as is required to derive the two IVs.
+	 */
+	iv[0] = 0x1;
+
+	/* Nonce: QC | A2 | PN */
+	iv[1] = qc;
+	memcpy(iv + 2, hdr->addr2, ETH_ALEN);
+	memcpy(iv + 8, pn, CCMP_PN_LEN);
+
+	/* AAD:
+	 * FC with bits 4..6 and 11..13 masked to zero; 14 is always one
+	 * A1 | A2 | A3
+	 * SC with bits 4..15 (seq#) masked to zero
+	 * A4 (if present)
+	 * QC (if present)
+	 */
+	pos = (u8 *)hdr;
+	aad[0] = pos[0] & 0x8f;
+	aad[1] = pos[1] & 0xc7;
+	memcpy(&aad[2], &hdr->addr1, ETH_ALEN);
+	memcpy(&aad[8], &hdr->addr2, ETH_ALEN);
+	memcpy(&aad[14], &hdr->addr3, ETH_ALEN);
+	pos = (u8 *)&hdr->seq_ctl;
+	aad[20] = pos[0] & 0x0f;
+	aad[21] = 0; /* all bits masked */
+	memset(aad + 22, 0, 8);
+	if (a4_included)
+		memcpy(aad + 22, hdr->addr4, ETH_ALEN);
+	if (qc_included) {
+		aad[a4_included ? 28 : 22] = qc;
+		/* rest of QC masked */
+	}
+
+	return aad_len;
+}
+
+static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_ccmp_data *key = priv;
+	int i;
+	u8 *pos;
+	struct rtl_80211_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+
+	if (skb_headroom(skb) < CCMP_HDR_LEN ||
+	    skb_tailroom(skb) < CCMP_MIC_LEN ||
+	    skb->len < hdr_len)
+		return -1;
+
+	pos = skb_push(skb, CCMP_HDR_LEN);
+	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
+	pos += hdr_len;
+	/* mic = skb_put(skb, CCMP_MIC_LEN); */
+
+	i = CCMP_PN_LEN - 1;
+	while (i >= 0) {
+		key->tx_pn[i]++;
+		if (key->tx_pn[i] != 0)
+			break;
+		i--;
+	}
+
+	*pos++ = key->tx_pn[5];
+	*pos++ = key->tx_pn[4];
+	*pos++ = 0;
+	*pos++ = (key->key_idx << 6) | BIT(5) /* Ext IV included */;
+	*pos++ = key->tx_pn[3];
+	*pos++ = key->tx_pn[2];
+	*pos++ = key->tx_pn[1];
+	*pos++ = key->tx_pn[0];
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	if (!tcb_desc->bHwSec) {
+		struct aead_request *req;
+		struct scatterlist sg[2];
+		u8 *aad = key->tx_aad;
+		u8 iv[AES_BLOCK_LEN];
+		int aad_len, ret;
+		size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN;
+
+		req = aead_request_alloc(key->tfm, GFP_ATOMIC);
+		if (!req)
+			return -ENOMEM;
+
+		aad_len = ccmp_init_iv_and_aad(hdr, key->tx_pn, iv, aad);
+
+		skb_put(skb, CCMP_MIC_LEN);
+
+		sg_init_table(sg, 2);
+		sg_set_buf(&sg[0], aad, aad_len);
+		sg_set_buf(&sg[1], skb->data + hdr_len + CCMP_HDR_LEN,
+			   data_len + CCMP_MIC_LEN);
+
+		aead_request_set_callback(req, 0, NULL, NULL);
+		aead_request_set_ad(req, aad_len);
+		aead_request_set_crypt(req, sg, sg, data_len, iv);
+
+		ret = crypto_aead_encrypt(req);
+		aead_request_free(req);
+
+		return ret;
+	}
+	return 0;
+}
+
+static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_ccmp_data *key = priv;
+	u8 keyidx, *pos;
+	struct rtl_80211_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 pn[6];
+
+	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -1;
+	}
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & BIT(5))) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev, "CCMP: received packet without ExtIV flag from %pM\n",
+				   hdr->addr2);
+		}
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -2;
+	}
+	keyidx >>= 6;
+	if (key->key_idx != keyidx) {
+		netdev_dbg(skb->dev, "CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+			   key->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!key->key_set) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev, "CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
+				   hdr->addr2, keyidx);
+		}
+		return -3;
+	}
+
+	pn[0] = pos[7];
+	pn[1] = pos[6];
+	pn[2] = pos[5];
+	pn[3] = pos[4];
+	pn[4] = pos[1];
+	pn[5] = pos[0];
+	pos += 8;
+
+	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev, "CCMP: replay detected: STA=%pM previous PN %pm received PN %pm\n",
+				   hdr->addr2, key->rx_pn, pn);
+		}
+		key->dot11RSNAStatsCCMPReplays++;
+		return -4;
+	}
+	if (!tcb_desc->bHwSec) {
+		struct aead_request *req;
+		struct scatterlist sg[2];
+		u8 *aad = key->rx_aad;
+		u8 iv[AES_BLOCK_LEN];
+		int aad_len, ret;
+		size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN;
+
+		req = aead_request_alloc(key->tfm, GFP_ATOMIC);
+		if (!req)
+			return -ENOMEM;
+
+		aad_len = ccmp_init_iv_and_aad(hdr, pn, iv, aad);
+
+		sg_init_table(sg, 2);
+		sg_set_buf(&sg[0], aad, aad_len);
+		sg_set_buf(&sg[1], pos, data_len);
+
+		aead_request_set_callback(req, 0, NULL, NULL);
+		aead_request_set_ad(req, aad_len);
+		aead_request_set_crypt(req, sg, sg, data_len, iv);
+
+		ret = crypto_aead_decrypt(req);
+		aead_request_free(req);
+
+		if (ret) {
+			if (net_ratelimit()) {
+				netdev_dbg(skb->dev, "CCMP: decrypt failed: STA=%pM\n",
+					   hdr->addr2);
+			}
+			key->dot11RSNAStatsCCMPDecryptErrors++;
+			return -5;
+		}
+
+		memcpy(key->rx_pn, pn, CCMP_PN_LEN);
+	}
+	/* Remove hdr and MIC */
+	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
+	skb_pull(skb, CCMP_HDR_LEN);
+	skb_trim(skb, skb->len - CCMP_MIC_LEN);
+
+	return keyidx;
+}
+
+static int ieee80211_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_ccmp_data *data = priv;
+	int keyidx;
+	struct crypto_aead *tfm = data->tfm;
+
+	keyidx = data->key_idx;
+	memset(data, 0, sizeof(*data));
+	data->key_idx = keyidx;
+	if (len == CCMP_TK_LEN) {
+		memcpy(data->key, key, CCMP_TK_LEN);
+		data->key_set = 1;
+		if (seq) {
+			data->rx_pn[0] = seq[5];
+			data->rx_pn[1] = seq[4];
+			data->rx_pn[2] = seq[3];
+			data->rx_pn[3] = seq[2];
+			data->rx_pn[4] = seq[1];
+			data->rx_pn[5] = seq[0];
+		}
+		if (crypto_aead_setauthsize(tfm, CCMP_MIC_LEN) ||
+		    crypto_aead_setkey(tfm, data->key, CCMP_TK_LEN))
+			return -1;
+	} else if (len == 0) {
+		data->key_set = 0;
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int ieee80211_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_ccmp_data *data = priv;
+
+	if (len < CCMP_TK_LEN)
+		return 0;
+
+	if (!data->key_set)
+		return 0;
+	memcpy(key, data->key, CCMP_TK_LEN);
+
+	if (seq) {
+		seq[0] = data->tx_pn[5];
+		seq[1] = data->tx_pn[4];
+		seq[2] = data->tx_pn[3];
+		seq[3] = data->tx_pn[2];
+		seq[4] = data->tx_pn[1];
+		seq[5] = data->tx_pn[0];
+	}
+
+	return CCMP_TK_LEN;
+}
+
+static char *ieee80211_ccmp_print_stats(char *p, void *priv)
+{
+	struct ieee80211_ccmp_data *ccmp = priv;
+
+	p += sprintf(p, "key[%d] alg=CCMP key_set=%d tx_pn=%pm rx_pn=%pm format_errors=%d replays=%d decrypt_errors=%d\n",
+		     ccmp->key_idx, ccmp->key_set,
+		     ccmp->tx_pn, ccmp->rx_pn,
+		     ccmp->dot11RSNAStatsCCMPFormatErrors,
+		     ccmp->dot11RSNAStatsCCMPReplays,
+		     ccmp->dot11RSNAStatsCCMPDecryptErrors);
+
+	return p;
+}
+
+static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
+	.name			= "CCMP",
+	.init			= ieee80211_ccmp_init,
+	.deinit			= ieee80211_ccmp_deinit,
+	.encrypt_mpdu		= ieee80211_ccmp_encrypt,
+	.decrypt_mpdu		= ieee80211_ccmp_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= ieee80211_ccmp_set_key,
+	.get_key		= ieee80211_ccmp_get_key,
+	.print_stats		= ieee80211_ccmp_print_stats,
+	.extra_prefix_len	= CCMP_HDR_LEN,
+	.extra_postfix_len	= CCMP_MIC_LEN,
+	.owner			= THIS_MODULE,
+};
+
+int __init ieee80211_crypto_ccmp_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
+}
+
+void ieee80211_crypto_ccmp_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
new file mode 100644
index 000000000..7b120b8cb
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -0,0 +1,724 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypt: host-based TKIP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <linux/fips.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/string.h>
+
+#include "ieee80211.h"
+
+#include <crypto/arc4.h>
+#include <crypto/hash.h>
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: TKIP");
+MODULE_LICENSE("GPL");
+
+struct ieee80211_tkip_data {
+#define TKIP_KEY_LEN 32
+	u8 key[TKIP_KEY_LEN];
+	int key_set;
+
+	u32 tx_iv32;
+	u16 tx_iv16;
+	u16 tx_ttak[5];
+	int tx_phase1_done;
+
+	u32 rx_iv32;
+	u16 rx_iv16;
+	u16 rx_ttak[5];
+	int rx_phase1_done;
+	u32 rx_iv32_new;
+	u16 rx_iv16_new;
+
+	u32 dot11RSNAStatsTKIPReplays;
+	u32 dot11RSNAStatsTKIPICVErrors;
+	u32 dot11RSNAStatsTKIPLocalMICFailures;
+
+	int key_idx;
+
+	struct arc4_ctx rx_ctx_arc4;
+	struct arc4_ctx tx_ctx_arc4;
+	struct crypto_shash *rx_tfm_michael;
+	struct crypto_shash *tx_tfm_michael;
+
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 rx_hdr[16], tx_hdr[16];
+};
+
+static void *ieee80211_tkip_init(int key_idx)
+{
+	struct ieee80211_tkip_data *priv;
+
+	if (fips_enabled)
+		return NULL;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		goto fail;
+	priv->key_idx = key_idx;
+
+	priv->tx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
+	if (IS_ERR(priv->tx_tfm_michael)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+				"crypto API michael_mic\n");
+		priv->tx_tfm_michael = NULL;
+		goto fail;
+	}
+
+	priv->rx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
+	if (IS_ERR(priv->rx_tfm_michael)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+				"crypto API michael_mic\n");
+		priv->rx_tfm_michael = NULL;
+		goto fail;
+	}
+
+	return priv;
+
+fail:
+	if (priv) {
+		crypto_free_shash(priv->tx_tfm_michael);
+		crypto_free_shash(priv->rx_tfm_michael);
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void ieee80211_tkip_deinit(void *priv)
+{
+	struct ieee80211_tkip_data *_priv = priv;
+
+	if (_priv) {
+		crypto_free_shash(_priv->tx_tfm_michael);
+		crypto_free_shash(_priv->rx_tfm_michael);
+	}
+	kfree_sensitive(priv);
+}
+
+
+static inline u16 RotR1(u16 val)
+{
+	return (val >> 1) | (val << 15);
+}
+
+
+static inline u8 Lo8(u16 val)
+{
+	return val & 0xff;
+}
+
+
+static inline u8 Hi8(u16 val)
+{
+	return val >> 8;
+}
+
+
+static inline u16 Lo16(u32 val)
+{
+	return val & 0xffff;
+}
+
+
+static inline u16 Hi16(u32 val)
+{
+	return val >> 16;
+}
+
+
+static inline u16 Mk16(u8 hi, u8 lo)
+{
+	return lo | (((u16)hi) << 8);
+}
+
+static const u16 Sbox[256] = {
+	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+};
+
+
+static inline u16 _S_(u16 v)
+{
+	u16 t = Sbox[Hi8(v)];
+	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
+}
+
+
+#define PHASE1_LOOP_COUNT 8
+
+
+static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
+{
+	int i, j;
+
+	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
+	TTAK[0] = Lo16(IV32);
+	TTAK[1] = Hi16(IV32);
+	TTAK[2] = Mk16(TA[1], TA[0]);
+	TTAK[3] = Mk16(TA[3], TA[2]);
+	TTAK[4] = Mk16(TA[5], TA[4]);
+
+	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
+		j = 2 * (i & 1);
+		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
+		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
+		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
+		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
+		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
+	}
+}
+
+
+static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
+			       u16 IV16)
+{
+	/*
+	 * Make temporary area overlap WEP seed so that the final copy can be
+	 * avoided on little endian hosts.
+	 */
+	u16 *PPK = (u16 *)&WEPSeed[4];
+
+	/* Step 1 - make copy of TTAK and bring in TSC */
+	PPK[0] = TTAK[0];
+	PPK[1] = TTAK[1];
+	PPK[2] = TTAK[2];
+	PPK[3] = TTAK[3];
+	PPK[4] = TTAK[4];
+	PPK[5] = TTAK[4] + IV16;
+
+	/* Step 2 - 96-bit bijective mixing using S-box */
+	PPK[0] += _S_(PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[0])));
+	PPK[1] += _S_(PPK[0] ^ le16_to_cpu(*(__le16 *)(&TK[2])));
+	PPK[2] += _S_(PPK[1] ^ le16_to_cpu(*(__le16 *)(&TK[4])));
+	PPK[3] += _S_(PPK[2] ^ le16_to_cpu(*(__le16 *)(&TK[6])));
+	PPK[4] += _S_(PPK[3] ^ le16_to_cpu(*(__le16 *)(&TK[8])));
+	PPK[5] += _S_(PPK[4] ^ le16_to_cpu(*(__le16 *)(&TK[10])));
+
+	PPK[0] += RotR1(PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[12])));
+	PPK[1] += RotR1(PPK[0] ^ le16_to_cpu(*(__le16 *)(&TK[14])));
+	PPK[2] += RotR1(PPK[1]);
+	PPK[3] += RotR1(PPK[2]);
+	PPK[4] += RotR1(PPK[3]);
+	PPK[5] += RotR1(PPK[4]);
+
+	/*
+	 * Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
+	 * WEPSeed[0..2] is transmitted as WEP IV
+	 */
+	WEPSeed[0] = Hi8(IV16);
+	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
+	WEPSeed[2] = Lo8(IV16);
+	WEPSeed[3] = Lo8((PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[0]))) >> 1);
+
+#ifdef __BIG_ENDIAN
+	{
+		int i;
+
+		for (i = 0; i < 6; i++)
+			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
+	}
+#endif
+}
+
+
+static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	int len;
+	u8 *pos;
+	struct rtl_80211_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 rc4key[16],  *icv;
+	u32 crc;
+
+	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+
+	if (!tcb_desc->bHwSec) {
+		if (!tkey->tx_phase1_done) {
+			tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
+					   tkey->tx_iv32);
+			tkey->tx_phase1_done = 1;
+		}
+		tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
+	} else
+		tkey->tx_phase1_done = 1;
+
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 8);
+	memmove(pos, pos + 8, hdr_len);
+	pos += hdr_len;
+
+	if (tcb_desc->bHwSec) {
+		*pos++ = Hi8(tkey->tx_iv16);
+		*pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
+		*pos++ = Lo8(tkey->tx_iv16);
+	} else {
+		*pos++ = rc4key[0];
+		*pos++ = rc4key[1];
+		*pos++ = rc4key[2];
+	}
+
+	*pos++ = (tkey->key_idx << 6) | BIT(5) /* Ext IV included */;
+	*pos++ = tkey->tx_iv32 & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
+
+	if (!tcb_desc->bHwSec) {
+		icv = skb_put(skb, 4);
+		crc = ~crc32_le(~0, pos, len);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		arc4_setkey(&tkey->tx_ctx_arc4, rc4key, 16);
+		arc4_crypt(&tkey->tx_ctx_arc4, pos, pos, len + 4);
+	}
+
+	tkey->tx_iv16++;
+	if (tkey->tx_iv16 == 0) {
+		tkey->tx_phase1_done = 0;
+		tkey->tx_iv32++;
+	}
+
+	return 0;
+}
+
+static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	u8 keyidx, *pos;
+	u32 iv32;
+	u16 iv16;
+	struct rtl_80211_hdr_4addr *hdr;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 rc4key[16];
+	u8 icv[4];
+	u32 crc;
+	int plen;
+
+	if (skb->len < hdr_len + 8 + 4)
+		return -1;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & BIT(5))) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev, "TKIP: received packet without ExtIV"
+			       " flag from %pM\n", hdr->addr2);
+		}
+		return -2;
+	}
+	keyidx >>= 6;
+	if (tkey->key_idx != keyidx) {
+		netdev_dbg(skb->dev, "TKIP: RX tkey->key_idx=%d frame "
+		       "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!tkey->key_set) {
+		if (net_ratelimit()) {
+			netdev_dbg(skb->dev, "TKIP: received packet from %pM"
+			       " with keyid=%d that does not have a configured"
+			       " key\n", hdr->addr2, keyidx);
+		}
+		return -3;
+	}
+	iv16 = (pos[0] << 8) | pos[2];
+	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
+	pos += 8;
+
+	if (!tcb_desc->bHwSec) {
+		if (iv32 < tkey->rx_iv32 ||
+		(iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
+			if (net_ratelimit()) {
+				netdev_dbg(skb->dev, "TKIP: replay detected: STA=%pM"
+				" previous TSC %08x%04x received TSC "
+				"%08x%04x\n", hdr->addr2,
+				tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
+			}
+			tkey->dot11RSNAStatsTKIPReplays++;
+			return -4;
+		}
+
+		if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
+			tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
+			tkey->rx_phase1_done = 1;
+		}
+		tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
+
+		plen = skb->len - hdr_len - 12;
+
+		arc4_setkey(&tkey->rx_ctx_arc4, rc4key, 16);
+		arc4_crypt(&tkey->rx_ctx_arc4, pos, pos, plen + 4);
+
+		crc = ~crc32_le(~0, pos, plen);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		if (memcmp(icv, pos + plen, 4) != 0) {
+			if (iv32 != tkey->rx_iv32) {
+				/*
+				 * Previously cached Phase1 result was already
+				 * lost, so it needs to be recalculated for the
+				 * next packet.
+				 */
+				tkey->rx_phase1_done = 0;
+			}
+			if (net_ratelimit()) {
+				netdev_dbg(skb->dev, "TKIP: ICV error detected: STA="
+				"%pM\n", hdr->addr2);
+			}
+			tkey->dot11RSNAStatsTKIPICVErrors++;
+			return -5;
+		}
+
+	}
+
+	/*
+	 * Update real counters only after Michael MIC verification has
+	 * completed.
+	 */
+	tkey->rx_iv32_new = iv32;
+	tkey->rx_iv16_new = iv16;
+
+	/* Remove IV and ICV */
+	memmove(skb->data + 8, skb->data, hdr_len);
+	skb_pull(skb, 8);
+	skb_trim(skb, skb->len - 4);
+
+	return keyidx;
+}
+
+static int michael_mic(struct crypto_shash *tfm_michael, u8 *key, u8 *hdr,
+		       u8 *data, size_t data_len, u8 *mic)
+{
+	SHASH_DESC_ON_STACK(desc, tfm_michael);
+	int err;
+
+	desc->tfm = tfm_michael;
+
+	if (crypto_shash_setkey(tfm_michael, key, 8))
+		return -1;
+
+	err = crypto_shash_init(desc);
+	if (err)
+		goto out;
+	err = crypto_shash_update(desc, hdr, 16);
+	if (err)
+		goto out;
+	err = crypto_shash_update(desc, data, data_len);
+	if (err)
+		goto out;
+	err = crypto_shash_final(desc, mic);
+
+out:
+	shash_desc_zero(desc);
+	return err;
+}
+
+static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
+{
+	struct rtl_80211_hdr_4addr *hdr11;
+
+	hdr11 = (struct rtl_80211_hdr_4addr *)skb->data;
+	switch (le16_to_cpu(hdr11->frame_ctl) &
+		(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
+	case IEEE80211_FCTL_TODS:
+		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		break;
+	case IEEE80211_FCTL_FROMDS:
+		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
+		break;
+	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
+		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
+		break;
+	default:
+		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		break;
+	}
+
+	hdr[12] = 0; /* priority */
+
+	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
+}
+
+
+static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	u8 *pos;
+	struct rtl_80211_hdr_4addr *hdr;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+
+	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
+		netdev_dbg(skb->dev, "Invalid packet for Michael MIC add "
+		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
+		       skb_tailroom(skb), hdr_len, skb->len);
+		return -1;
+	}
+
+	michael_mic_hdr(skb, tkey->tx_hdr);
+
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	if (IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
+		tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	// }
+	pos = skb_put(skb, 8);
+
+	if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
+				skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+		return -1;
+
+	return 0;
+}
+
+static void ieee80211_michael_mic_failure(struct net_device *dev,
+				       struct rtl_80211_hdr_4addr *hdr,
+				       int keyidx)
+{
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure ev;
+
+	/* TODO: needed parameters: count, keyid, key type, TSC */
+	memset(&ev, 0, sizeof(ev));
+	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
+	if (hdr->addr1[0] & 0x01)
+		ev.flags |= IW_MICFAILURE_GROUP;
+	else
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
+}
+
+static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
+					int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	u8 mic[8];
+	struct rtl_80211_hdr_4addr *hdr;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+
+	if (!tkey->key_set)
+		return -1;
+
+	michael_mic_hdr(skb, tkey->rx_hdr);
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	if (IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
+		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	// }
+
+	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+		return -1;
+	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
+		struct rtl_80211_hdr_4addr *hdr;
+		hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+
+		netdev_dbg(skb->dev, "Michael MIC verification failed for "
+		       "MSDU from %pM keyidx=%d\n",
+		       hdr->addr2, keyidx);
+		if (skb->dev)
+			ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
+		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
+		return -1;
+	}
+
+	/*
+	 * Update TSC counters for RX now that the packet verification has
+	 * completed.
+	 */
+	tkey->rx_iv32 = tkey->rx_iv32_new;
+	tkey->rx_iv16 = tkey->rx_iv16_new;
+
+	skb_trim(skb, skb->len - 8);
+
+	return 0;
+}
+
+
+static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	int keyidx;
+	struct crypto_shash *tfm = tkey->tx_tfm_michael;
+	struct crypto_shash *tfm3 = tkey->rx_tfm_michael;
+
+	keyidx = tkey->key_idx;
+	memset(tkey, 0, sizeof(*tkey));
+	tkey->key_idx = keyidx;
+	tkey->tx_tfm_michael = tfm;
+	tkey->rx_tfm_michael = tfm3;
+
+	if (len == TKIP_KEY_LEN) {
+		memcpy(tkey->key, key, TKIP_KEY_LEN);
+		tkey->key_set = 1;
+		tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
+		if (seq) {
+			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
+				(seq[3] << 8) | seq[2];
+			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
+		}
+	} else if (len == 0)
+		tkey->key_set = 0;
+	else
+		return -1;
+
+	return 0;
+}
+
+
+static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+
+	if (len < TKIP_KEY_LEN)
+		return 0;
+
+	if (!tkey->key_set)
+		return 0;
+	memcpy(key, tkey->key, TKIP_KEY_LEN);
+
+	if (seq) {
+		/* Return the sequence number of the last transmitted frame. */
+		u16 iv16 = tkey->tx_iv16;
+		u32 iv32 = tkey->tx_iv32;
+
+		if (iv16 == 0)
+			iv32--;
+		iv16--;
+		seq[0] = tkey->tx_iv16;
+		seq[1] = tkey->tx_iv16 >> 8;
+		seq[2] = tkey->tx_iv32;
+		seq[3] = tkey->tx_iv32 >> 8;
+		seq[4] = tkey->tx_iv32 >> 16;
+		seq[5] = tkey->tx_iv32 >> 24;
+	}
+
+	return TKIP_KEY_LEN;
+}
+
+
+static char *ieee80211_tkip_print_stats(char *p, void *priv)
+{
+	struct ieee80211_tkip_data *tkip = priv;
+
+	p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
+		     "tx_pn=%02x%02x%02x%02x%02x%02x "
+		     "rx_pn=%02x%02x%02x%02x%02x%02x "
+		     "replays=%d icv_errors=%d local_mic_failures=%d\n",
+		     tkip->key_idx, tkip->key_set,
+		     (tkip->tx_iv32 >> 24) & 0xff,
+		     (tkip->tx_iv32 >> 16) & 0xff,
+		     (tkip->tx_iv32 >> 8) & 0xff,
+		     tkip->tx_iv32 & 0xff,
+		     (tkip->tx_iv16 >> 8) & 0xff,
+		     tkip->tx_iv16 & 0xff,
+		     (tkip->rx_iv32 >> 24) & 0xff,
+		     (tkip->rx_iv32 >> 16) & 0xff,
+		     (tkip->rx_iv32 >> 8) & 0xff,
+		     tkip->rx_iv32 & 0xff,
+		     (tkip->rx_iv16 >> 8) & 0xff,
+		     tkip->rx_iv16 & 0xff,
+		     tkip->dot11RSNAStatsTKIPReplays,
+		     tkip->dot11RSNAStatsTKIPICVErrors,
+		     tkip->dot11RSNAStatsTKIPLocalMICFailures);
+	return p;
+}
+
+
+static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
+	.name			= "TKIP",
+	.init			= ieee80211_tkip_init,
+	.deinit			= ieee80211_tkip_deinit,
+	.encrypt_mpdu		= ieee80211_tkip_encrypt,
+	.decrypt_mpdu		= ieee80211_tkip_decrypt,
+	.encrypt_msdu		= ieee80211_michael_mic_add,
+	.decrypt_msdu		= ieee80211_michael_mic_verify,
+	.set_key		= ieee80211_tkip_set_key,
+	.get_key		= ieee80211_tkip_get_key,
+	.print_stats		= ieee80211_tkip_print_stats,
+	.extra_prefix_len	= 4 + 4, /* IV + ExtIV */
+	.extra_postfix_len	= 8 + 4, /* MIC + ICV */
+	.owner			= THIS_MODULE,
+};
+
+int __init ieee80211_crypto_tkip_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
+}
+
+void ieee80211_crypto_tkip_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
+}
+
+void ieee80211_tkip_null(void)
+{
+//    printk("============>%s()\n", __func__);
+	return;
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
new file mode 100644
index 000000000..a2cdf3bfd
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Host AP crypt: host-based WEP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2002-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ */
+
+#include <linux/fips.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/string.h>
+
+#include "ieee80211.h"
+
+#include <crypto/arc4.h>
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: WEP");
+MODULE_LICENSE("GPL");
+
+struct prism2_wep_data {
+	u32 iv;
+#define WEP_KEY_LEN 13
+	u8 key[WEP_KEY_LEN + 1];
+	u8 key_len;
+	u8 key_idx;
+	struct arc4_ctx rx_ctx_arc4;
+	struct arc4_ctx tx_ctx_arc4;
+};
+
+
+static void *prism2_wep_init(int keyidx)
+{
+	struct prism2_wep_data *priv;
+
+	if (fips_enabled)
+		return NULL;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return NULL;
+	priv->key_idx = keyidx;
+
+	/* start WEP IV from a random value */
+	get_random_bytes(&priv->iv, 4);
+
+	return priv;
+}
+
+
+static void prism2_wep_deinit(void *priv)
+{
+	kfree_sensitive(priv);
+}
+
+/* Perform WEP encryption on given skb that has at least 4 bytes of headroom
+ * for IV and 4 bytes of tailroom for ICV. Both IV and ICV will be transmitted,
+ * so the payload length increases with 8 bytes.
+ *
+ * WEP frame payload: IV + TX key idx, RC4(data), ICV = RC4(CRC32(data))
+ */
+static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+	u32 klen, len;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 *pos;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u32 crc;
+	u8 *icv;
+
+	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 4);
+	memmove(pos, pos + 4, hdr_len);
+	pos += hdr_len;
+
+	klen = 3 + wep->key_len;
+
+	wep->iv++;
+
+	/* Fluhrer, Mantin, and Shamir have reported weaknesses in the key
+	 * scheduling algorithm of RC4. At least IVs (KeyByte + 3, 0xff, N)
+	 * can be used to speedup attacks, so avoid using them.
+	 */
+	if ((wep->iv & 0xff00) == 0xff00) {
+		u8 B = (wep->iv >> 16) & 0xff;
+
+		if (B >= 3 && B < klen)
+			wep->iv += 0x0100;
+	}
+
+	/* Prepend 24-bit IV to RC4 key and TX frame */
+	*pos++ = key[0] = (wep->iv >> 16) & 0xff;
+	*pos++ = key[1] = (wep->iv >> 8) & 0xff;
+	*pos++ = key[2] = wep->iv & 0xff;
+	*pos++ = wep->key_idx << 6;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+	if (!tcb_desc->bHwSec) {
+		/* Append little-endian CRC32 and encrypt it to produce ICV */
+		crc = ~crc32_le(~0, pos, len);
+		icv = skb_put(skb, 4);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+
+		arc4_setkey(&wep->tx_ctx_arc4, key, klen);
+		arc4_crypt(&wep->tx_ctx_arc4, pos, pos, len + 4);
+	}
+
+	return 0;
+}
+
+
+/* Perform WEP decryption on given buffer. Buffer includes whole WEP part of
+ * the frame: IV (4 bytes), encrypted payload (including SNAP header),
+ * ICV (4 bytes). len includes both IV and ICV.
+ *
+ * Returns 0 if frame was decrypted successfully and ICV was correct and -1 on
+ * failure. If frame is OK, IV and ICV will be removed.
+ */
+static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+	u32  klen, plen;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 keyidx, *pos;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u32 crc;
+	u8 icv[4];
+
+	if (skb->len < hdr_len + 8)
+		return -1;
+
+	pos = skb->data + hdr_len;
+	key[0] = *pos++;
+	key[1] = *pos++;
+	key[2] = *pos++;
+	keyidx = *pos++ >> 6;
+	if (keyidx != wep->key_idx)
+		return -1;
+
+	klen = 3 + wep->key_len;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+	/* Apply RC4 to data and compute CRC32 over decrypted data */
+	plen = skb->len - hdr_len - 8;
+
+	if (!tcb_desc->bHwSec) {
+		arc4_setkey(&wep->rx_ctx_arc4, key, klen);
+		arc4_crypt(&wep->rx_ctx_arc4, pos, pos, plen + 4);
+
+		crc = ~crc32_le(~0, pos, plen);
+		icv[0] = crc;
+		icv[1] = crc >> 8;
+		icv[2] = crc >> 16;
+		icv[3] = crc >> 24;
+		if (memcmp(icv, pos + plen, 4) != 0) {
+			/* ICV mismatch - drop frame */
+			return -2;
+		}
+	}
+	/* Remove IV and ICV */
+	memmove(skb->data + 4, skb->data, hdr_len);
+	skb_pull(skb, 4);
+	skb_trim(skb, skb->len - 4);
+
+	return 0;
+}
+
+
+static int prism2_wep_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < 0 || len > WEP_KEY_LEN)
+		return -1;
+
+	memcpy(wep->key, key, len);
+	wep->key_len = len;
+
+	return 0;
+}
+
+
+static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < wep->key_len)
+		return 0;
+
+	memcpy(key, wep->key, wep->key_len);
+
+	return wep->key_len;
+}
+
+
+static char *prism2_wep_print_stats(char *p, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	p += sprintf(p, "key[%d] alg=WEP len=%d\n",
+		     wep->key_idx, wep->key_len);
+	return p;
+}
+
+
+static struct ieee80211_crypto_ops ieee80211_crypt_wep = {
+	.name			= "WEP",
+	.init			= prism2_wep_init,
+	.deinit			= prism2_wep_deinit,
+	.encrypt_mpdu		= prism2_wep_encrypt,
+	.decrypt_mpdu		= prism2_wep_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= prism2_wep_set_key,
+	.get_key		= prism2_wep_get_key,
+	.print_stats		= prism2_wep_print_stats,
+	.extra_prefix_len	= 4, /* IV */
+	.extra_postfix_len	= 4, /* ICV */
+	.owner			= THIS_MODULE,
+};
+
+int __init ieee80211_crypto_wep_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_wep);
+}
+
+void ieee80211_crypto_wep_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_wep);
+}
+
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
new file mode 100644
index 000000000..b94fe9b44
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -0,0 +1,290 @@
+// SPDX-License-Identifier: GPL-2.0
+/*******************************************************************************
+ *
+ *  Copyright(c) 2004 Intel Corporation. All rights reserved.
+ *
+ *  Portions of this file are based on the WEP enablement code provided by the
+ *  Host AP project hostap-drivers v0.1.3
+ *  Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ *  <jkmaline@cc.hut.fi>
+ *  Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ *  Contact Information:
+ *  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ *  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ ******************************************************************************/
+
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <net/arp.h>
+
+#include "ieee80211.h"
+
+MODULE_DESCRIPTION("802.11 data/management/control stack");
+MODULE_AUTHOR("Copyright (C) 2004 Intel Corporation <jketreno@linux.intel.com>");
+MODULE_LICENSE("GPL");
+
+#define DRV_NAME "ieee80211"
+
+static inline int ieee80211_networks_allocate(struct ieee80211_device *ieee)
+{
+	if (ieee->networks)
+		return 0;
+
+	ieee->networks = kcalloc(MAX_NETWORK_COUNT,
+				 sizeof(struct ieee80211_network),
+				 GFP_KERNEL);
+	if (!ieee->networks) {
+		netdev_warn(ieee->dev, "Out of memory allocating beacons\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static inline void ieee80211_networks_free(struct ieee80211_device *ieee)
+{
+	if (!ieee->networks)
+		return;
+	kfree(ieee->networks);
+	ieee->networks = NULL;
+}
+
+static inline void ieee80211_networks_initialize(struct ieee80211_device *ieee)
+{
+	int i;
+
+	INIT_LIST_HEAD(&ieee->network_free_list);
+	INIT_LIST_HEAD(&ieee->network_list);
+	for (i = 0; i < MAX_NETWORK_COUNT; i++)
+		list_add_tail(&ieee->networks[i].list, &ieee->network_free_list);
+}
+
+struct net_device *alloc_ieee80211(int sizeof_priv)
+{
+	struct ieee80211_device *ieee;
+	struct net_device *dev;
+	int i, err;
+
+	IEEE80211_DEBUG_INFO("Initializing...\n");
+
+	dev = alloc_etherdev(sizeof(struct ieee80211_device) + sizeof_priv);
+	if (!dev) {
+		IEEE80211_ERROR("Unable to network device.\n");
+		goto failed;
+	}
+
+	ieee = netdev_priv(dev);
+	ieee->dev = dev;
+
+	err = ieee80211_networks_allocate(ieee);
+	if (err) {
+		IEEE80211_ERROR("Unable to allocate beacon storage: %d\n",
+				err);
+		goto failed;
+	}
+	ieee80211_networks_initialize(ieee);
+
+	/* Default fragmentation threshold is maximum payload size */
+	ieee->fts = DEFAULT_FTS;
+	ieee->scan_age = DEFAULT_MAX_SCAN_AGE;
+	ieee->open_wep = 1;
+
+	/* Default to enabling full open WEP with host based encrypt/decrypt */
+	ieee->host_encrypt = 1;
+	ieee->host_decrypt = 1;
+	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
+
+	INIT_LIST_HEAD(&ieee->crypt_deinit_list);
+	timer_setup(&ieee->crypt_deinit_timer, ieee80211_crypt_deinit_handler,
+		    0);
+
+	spin_lock_init(&ieee->lock);
+	spin_lock_init(&ieee->wpax_suitlist_lock);
+	spin_lock_init(&ieee->bw_spinlock);
+	spin_lock_init(&ieee->reorder_spinlock);
+	/* added by WB */
+	atomic_set(&ieee->atm_chnlop, 0);
+	atomic_set(&ieee->atm_swbw, 0);
+
+	ieee->wpax_type_set = 0;
+	ieee->wpa_enabled = 0;
+	ieee->tkip_countermeasures = 0;
+	ieee->drop_unencrypted = 0;
+	ieee->privacy_invoked = 0;
+	ieee->ieee802_1x = 1;
+	ieee->raw_tx = 0;
+	//ieee->hwsec_support = 1; //defalt support hw security. //use module_param instead.
+	ieee->hwsec_active = 0; /* disable hwsec, switch it on when necessary. */
+
+	ieee80211_softmac_init(ieee);
+
+	ieee->pHTInfo = kzalloc(sizeof(RT_HIGH_THROUGHPUT), GFP_KERNEL);
+	if (!ieee->pHTInfo) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc memory for HTInfo\n");
+
+		/* By this point in code ieee80211_networks_allocate() has been
+		 * successfully called so the memory allocated should be freed
+		 */
+		ieee80211_networks_free(ieee);
+		goto failed;
+	}
+	HTUpdateDefaultSetting(ieee);
+	HTInitializeHTInfo(ieee); /* may move to other place. */
+	TSInitialize(ieee);
+
+	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&ieee->ibss_mac_hash[i]);
+
+	for (i = 0; i < 17; i++) {
+		ieee->last_rxseq_num[i] = -1;
+		ieee->last_rxfrag_num[i] = -1;
+		ieee->last_packet_time[i] = 0;
+	}
+
+/* These function were added to load crypte module autoly */
+	ieee80211_tkip_null();
+
+	return dev;
+
+ failed:
+	if (dev)
+		free_netdev(dev);
+
+	return NULL;
+}
+
+void free_ieee80211(struct net_device *dev)
+{
+	struct ieee80211_device *ieee = netdev_priv(dev);
+	int i;
+	/* struct list_head *p, *q; */
+//	del_timer_sync(&ieee->SwBwTimer);
+	kfree(ieee->pHTInfo);
+	ieee->pHTInfo = NULL;
+	RemoveAllTS(ieee);
+	ieee80211_softmac_free(ieee);
+	del_timer_sync(&ieee->crypt_deinit_timer);
+	ieee80211_crypt_deinit_entries(ieee, 1);
+
+	for (i = 0; i < WEP_KEYS; i++) {
+		struct ieee80211_crypt_data *crypt = ieee->crypt[i];
+
+		if (crypt) {
+			if (crypt->ops)
+				crypt->ops->deinit(crypt->priv);
+			kfree(crypt);
+			ieee->crypt[i] = NULL;
+		}
+	}
+
+	ieee80211_networks_free(ieee);
+	free_netdev(dev);
+}
+
+#ifdef CONFIG_IEEE80211_DEBUG
+
+u32 ieee80211_debug_level;
+static int debug = //	    IEEE80211_DL_INFO	|
+	//		    IEEE80211_DL_WX	|
+	//		    IEEE80211_DL_SCAN	|
+	//		    IEEE80211_DL_STATE	|
+	//		    IEEE80211_DL_MGMT	|
+	//		    IEEE80211_DL_FRAG	|
+	//		    IEEE80211_DL_EAP	|
+	//		    IEEE80211_DL_DROP	|
+	//		    IEEE80211_DL_TX	|
+	//		    IEEE80211_DL_RX	|
+			    //IEEE80211_DL_QOS    |
+	//		    IEEE80211_DL_HT	|
+	//		    IEEE80211_DL_TS	|
+//			    IEEE80211_DL_BA	|
+	//		    IEEE80211_DL_REORDER|
+//			    IEEE80211_DL_TRACE  |
+			    //IEEE80211_DL_DATA	|
+			    IEEE80211_DL_ERR	  /* awayls open this flags to show error out */
+			    ;
+static struct proc_dir_entry *ieee80211_proc;
+
+static int show_debug_level(struct seq_file *m, void *v)
+{
+	seq_printf(m, "0x%08X\n", ieee80211_debug_level);
+
+	return 0;
+}
+
+static ssize_t write_debug_level(struct file *file, const char __user *buffer,
+				 size_t count, loff_t *ppos)
+{
+	unsigned long val;
+	int err = kstrtoul_from_user(buffer, count, 0, &val);
+
+	if (err)
+		return err;
+	ieee80211_debug_level = val;
+	return count;
+}
+
+static int open_debug_level(struct inode *inode, struct file *file)
+{
+	return single_open(file, show_debug_level, NULL);
+}
+
+static const struct proc_ops debug_level_proc_ops = {
+	.proc_open	= open_debug_level,
+	.proc_read	= seq_read,
+	.proc_lseek	= seq_lseek,
+	.proc_write	= write_debug_level,
+	.proc_release	= single_release,
+};
+
+int __init ieee80211_debug_init(void)
+{
+	struct proc_dir_entry *e;
+
+	ieee80211_debug_level = debug;
+
+	ieee80211_proc = proc_mkdir(DRV_NAME, init_net.proc_net);
+	if (!ieee80211_proc) {
+		IEEE80211_ERROR("Unable to create " DRV_NAME
+				" proc directory\n");
+		return -EIO;
+	}
+	e = proc_create("debug_level", 0644, ieee80211_proc, &debug_level_proc_ops);
+	if (!e) {
+		remove_proc_entry(DRV_NAME, init_net.proc_net);
+		ieee80211_proc = NULL;
+		return -EIO;
+	}
+	return 0;
+}
+
+void ieee80211_debug_exit(void)
+{
+	if (ieee80211_proc) {
+		remove_proc_entry("debug_level", ieee80211_proc);
+		remove_proc_entry(DRV_NAME, init_net.proc_net);
+		ieee80211_proc = NULL;
+	}
+}
+
+module_param(debug, int, 0444);
+MODULE_PARM_DESC(debug, "debug output mask");
+#endif
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
new file mode 100644
index 000000000..3686b3c59
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -0,0 +1,2466 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Original code based Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3 - hostap.o module, common routines
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ * Copyright (c) 2004, Intel Corporation
+ ******************************************************************************
+
+  Few modifications for Realtek's Wi-Fi drivers by
+  Andrea Merello <andrea.merello@gmail.com>
+
+  A special thanks goes to Realtek for their support !
+
+******************************************************************************/
+
+
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <linux/ctype.h>
+
+#include "ieee80211.h"
+#include "dot11d.h"
+static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
+					struct sk_buff *skb,
+					struct ieee80211_rx_stats *rx_stats)
+{
+	struct rtl_80211_hdr_4addr *hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+
+	skb->dev = ieee->dev;
+	skb_reset_mac_header(skb);
+
+	skb_pull(skb, ieee80211_get_hdrlen(fc));
+	skb->pkt_type = PACKET_OTHERHOST;
+	skb->protocol = htons(ETH_P_80211_RAW);
+	memset(skb->cb, 0, sizeof(skb->cb));
+	netif_rx(skb);
+}
+
+
+/* Called only as a tasklet (software IRQ) */
+static struct ieee80211_frag_entry *
+ieee80211_frag_cache_find(struct ieee80211_device *ieee, unsigned int seq,
+			  unsigned int frag, u8 tid, u8 *src, u8 *dst)
+{
+	struct ieee80211_frag_entry *entry;
+	int i;
+
+	for (i = 0; i < IEEE80211_FRAG_CACHE_LEN; i++) {
+		entry = &ieee->frag_cache[tid][i];
+		if (entry->skb &&
+		    time_after(jiffies, entry->first_frag_time + 2 * HZ)) {
+			IEEE80211_DEBUG_FRAG(
+				"expiring fragment cache entry "
+				"seq=%u last_frag=%u\n",
+				entry->seq, entry->last_frag);
+			dev_kfree_skb_any(entry->skb);
+			entry->skb = NULL;
+		}
+
+		if (entry->skb && entry->seq == seq &&
+		    (entry->last_frag + 1 == frag || frag == -1) &&
+		    memcmp(entry->src_addr, src, ETH_ALEN) == 0 &&
+		    memcmp(entry->dst_addr, dst, ETH_ALEN) == 0)
+			return entry;
+	}
+
+	return NULL;
+}
+
+/* Called only as a tasklet (software IRQ) */
+static struct sk_buff *
+ieee80211_frag_cache_get(struct ieee80211_device *ieee,
+			 struct rtl_80211_hdr_4addr *hdr)
+{
+	struct sk_buff *skb = NULL;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int frag = WLAN_GET_SEQ_FRAG(sc);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct ieee80211_frag_entry *entry;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)hdr;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+		hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)hdr;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else {
+		tid = 0;
+	}
+
+	if (frag == 0) {
+		/* Reserve enough space to fit maximum frame length */
+		skb = dev_alloc_skb(ieee->dev->mtu +
+				    sizeof(struct rtl_80211_hdr_4addr) +
+				    8 /* LLC */ +
+				    2 /* alignment */ +
+				    8 /* WEP */ +
+				    ETH_ALEN /* WDS */ +
+				    (IEEE80211_QOS_HAS_SEQ(fc) ? 2 : 0) /* QOS Control */);
+		if (!skb)
+			return NULL;
+
+		entry = &ieee->frag_cache[tid][ieee->frag_next_idx[tid]];
+		ieee->frag_next_idx[tid]++;
+		if (ieee->frag_next_idx[tid] >= IEEE80211_FRAG_CACHE_LEN)
+			ieee->frag_next_idx[tid] = 0;
+
+		if (entry->skb)
+			dev_kfree_skb_any(entry->skb);
+
+		entry->first_frag_time = jiffies;
+		entry->seq = seq;
+		entry->last_frag = frag;
+		entry->skb = skb;
+		memcpy(entry->src_addr, hdr->addr2, ETH_ALEN);
+		memcpy(entry->dst_addr, hdr->addr1, ETH_ALEN);
+	} else {
+		/* received a fragment of a frame for which the head fragment
+		 * should have already been received */
+		entry = ieee80211_frag_cache_find(ieee, seq, frag, tid, hdr->addr2,
+						  hdr->addr1);
+		if (entry) {
+			entry->last_frag = frag;
+			skb = entry->skb;
+		}
+	}
+
+	return skb;
+}
+
+
+/* Called only as a tasklet (software IRQ) */
+static int ieee80211_frag_cache_invalidate(struct ieee80211_device *ieee,
+					   struct rtl_80211_hdr_4addr *hdr)
+{
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct ieee80211_frag_entry *entry;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)hdr;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+		hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)hdr;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else {
+		tid = 0;
+	}
+
+	entry = ieee80211_frag_cache_find(ieee, seq, -1, tid, hdr->addr2,
+					  hdr->addr1);
+
+	if (!entry) {
+		IEEE80211_DEBUG_FRAG(
+			"could not invalidate fragment cache "
+			"entry (seq=%u)\n", seq);
+		return -1;
+	}
+
+	entry->skb = NULL;
+	return 0;
+}
+
+
+
+/* ieee80211_rx_frame_mgtmt
+ *
+ * Responsible for handling management control frames
+ *
+ * Called by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats, u16 type,
+			u16 stype)
+{
+	/* On the struct stats definition there is written that
+	 * this is not mandatory.... but seems that the probe
+	 * response parser uses it
+	 */
+	struct rtl_80211_hdr_3addr *hdr = (struct rtl_80211_hdr_3addr *)skb->data;
+
+	rx_stats->len = skb->len;
+	ieee80211_rx_mgt(ieee, (struct rtl_80211_hdr_4addr *)skb->data, rx_stats);
+	/* if ((ieee->state == IEEE80211_LINKED) && (memcmp(hdr->addr3, ieee->current_network.bssid, ETH_ALEN))) */
+	if ((memcmp(hdr->addr1, ieee->dev->dev_addr, ETH_ALEN))) {
+		/* use ADDR1 to perform address matching for Management frames */
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	ieee80211_rx_frame_softmac(ieee, skb, rx_stats, type, stype);
+
+	dev_kfree_skb_any(skb);
+
+	return 0;
+
+	#ifdef NOT_YET
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		netdev_dbg(ieee->dev, "Master mode not yet supported.\n");
+		return 0;
+/*
+  hostap_update_sta_ps(ieee, (struct hostap_ieee80211_hdr_4addr *)
+  skb->data);*/
+	}
+
+	if (ieee->hostapd && type == IEEE80211_TYPE_MGMT) {
+		if (stype == WLAN_FC_STYPE_BEACON &&
+		    ieee->iw_mode == IW_MODE_MASTER) {
+			struct sk_buff *skb2;
+			/* Process beacon frames also in kernel driver to
+			 * update STA(AP) table statistics */
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+			if (skb2)
+				hostap_rx(skb2->dev, skb2, rx_stats);
+		}
+
+		/* send management frames to the user space daemon for
+		 * processing */
+		ieee->apdevstats.rx_packets++;
+		ieee->apdevstats.rx_bytes += skb->len;
+		prism2_rx_80211(ieee->apdev, skb, rx_stats, PRISM2_RX_MGMT);
+		return 0;
+	}
+
+	    if (ieee->iw_mode == IW_MODE_MASTER) {
+		if (type != WLAN_FC_TYPE_MGMT && type != WLAN_FC_TYPE_CTRL) {
+			netdev_dbg(skb->dev, "unknown management frame "
+			       "(type=0x%02x, stype=0x%02x) dropped\n",
+			       type, stype);
+			return -1;
+		}
+
+		hostap_rx(skb->dev, skb, rx_stats);
+		return 0;
+	}
+
+	netdev_dbg(skb->dev, "hostap_rx_frame_mgmt: management frame "
+	       "received in non-Host AP mode\n");
+	return -1;
+	#endif
+}
+
+
+
+/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
+/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
+static unsigned char rfc1042_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static unsigned char bridge_tunnel_header[] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+/* No encapsulation header if EtherType < 0x600 (=length) */
+
+/* Called by ieee80211_rx_frame_decrypt */
+static int ieee80211_is_eapol_frame(struct ieee80211_device *ieee,
+				    struct sk_buff *skb, size_t hdrlen)
+{
+	struct net_device *dev = ieee->dev;
+	u16 fc, ethertype;
+	struct rtl_80211_hdr_4addr *hdr;
+	u8 *pos;
+
+	if (skb->len < 24)
+		return 0;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	fc = le16_to_cpu(hdr->frame_ctl);
+
+	/* check that the frame is unicast frame to us */
+	if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+	    IEEE80211_FCTL_TODS &&
+	    memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0 &&
+	    memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN) == 0) {
+		/* ToDS frame with own addr BSSID and DA */
+	} else if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		   IEEE80211_FCTL_FROMDS &&
+		   memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0) {
+		/* FromDS frame with own addr as DA */
+	} else
+		return 0;
+
+	if (skb->len < 24 + 8)
+		return 0;
+
+	/* check for port access entity Ethernet type */
+//	pos = skb->data + 24;
+	pos = skb->data + hdrlen;
+	ethertype = (pos[6] << 8) | pos[7];
+	if (ethertype == ETH_P_PAE)
+		return 1;
+
+	return 0;
+}
+
+/* Called only as a tasklet (software IRQ), by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_decrypt(struct ieee80211_device *ieee, struct sk_buff *skb,
+			   struct ieee80211_crypt_data *crypt)
+{
+	struct rtl_80211_hdr_4addr *hdr;
+	int res, hdrlen;
+
+	if (!crypt || !crypt->ops->decrypt_mpdu)
+		return 0;
+	if (ieee->hwsec_active) {
+		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->bHwSec = 1;
+	}
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+	if (ieee->tkip_countermeasures &&
+	    strcmp(crypt->ops->name, "TKIP") == 0) {
+		if (net_ratelimit()) {
+			netdev_dbg(ieee->dev, "TKIP countermeasures: dropped "
+			       "received packet from %pM\n",
+			       hdr->addr2);
+		}
+		return -1;
+	}
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		IEEE80211_DEBUG_DROP(
+			"decryption failed (SA=%pM"
+			") res=%d\n", hdr->addr2, res);
+		if (res == -2)
+			IEEE80211_DEBUG_DROP("Decryption failed ICV "
+					     "mismatch (key %d)\n",
+					     skb->data[hdrlen + 3] >> 6);
+		ieee->ieee_stats.rx_discards_undecryptable++;
+		return -1;
+	}
+
+	return res;
+}
+
+
+/* Called only as a tasklet (software IRQ), by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device *ieee, struct sk_buff *skb,
+			     int keyidx, struct ieee80211_crypt_data *crypt)
+{
+	struct rtl_80211_hdr_4addr *hdr;
+	int res, hdrlen;
+
+	if (!crypt || !crypt->ops->decrypt_msdu)
+		return 0;
+	if (ieee->hwsec_active) {
+		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->bHwSec = 1;
+	}
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		netdev_dbg(ieee->dev, "MSDU decryption/MIC verification failed"
+		       " (SA=%pM keyidx=%d)\n",
+		       hdr->addr2, keyidx);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+/* this function is stolen from ipw2200 driver*/
+#define IEEE_PACKET_RETRY_TIME (5 * HZ)
+static int is_duplicate_packet(struct ieee80211_device *ieee,
+				      struct rtl_80211_hdr_4addr *header)
+{
+	u16 fc = le16_to_cpu(header->frame_ctl);
+	u16 sc = le16_to_cpu(header->seq_ctl);
+	u16 seq = WLAN_GET_SEQ_SEQ(sc);
+	u16 frag = WLAN_GET_SEQ_FRAG(sc);
+	u16 *last_seq, *last_frag;
+	unsigned long *last_time;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
+	u8 tid;
+
+
+	//TO2DS and QoS
+	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
+		hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)header;
+		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) { //QoS
+		hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)header;
+		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
+		tid = UP2AC(tid);
+		tid++;
+	} else { // no QoS
+		tid = 0;
+	}
+
+	switch (ieee->iw_mode) {
+	case IW_MODE_ADHOC:
+	{
+		struct list_head *p;
+		struct ieee_ibss_seq *entry = NULL;
+		u8 *mac = header->addr2;
+		int index = mac[5] % IEEE_IBSS_MAC_HASH_SIZE;
+
+		list_for_each(p, &ieee->ibss_mac_hash[index]) {
+			entry = list_entry(p, struct ieee_ibss_seq, list);
+			if (!memcmp(entry->mac, mac, ETH_ALEN))
+				break;
+		}
+	//	if (memcmp(entry->mac, mac, ETH_ALEN)){
+		if (p == &ieee->ibss_mac_hash[index]) {
+			entry = kmalloc(sizeof(struct ieee_ibss_seq), GFP_ATOMIC);
+			if (!entry)
+				return 0;
+			memcpy(entry->mac, mac, ETH_ALEN);
+			entry->seq_num[tid] = seq;
+			entry->frag_num[tid] = frag;
+			entry->packet_time[tid] = jiffies;
+			list_add(&entry->list, &ieee->ibss_mac_hash[index]);
+			return 0;
+		}
+		last_seq = &entry->seq_num[tid];
+		last_frag = &entry->frag_num[tid];
+		last_time = &entry->packet_time[tid];
+		break;
+	}
+
+	case IW_MODE_INFRA:
+		last_seq = &ieee->last_rxseq_num[tid];
+		last_frag = &ieee->last_rxfrag_num[tid];
+		last_time = &ieee->last_packet_time[tid];
+
+		break;
+	default:
+		return 0;
+	}
+
+//	if(tid != 0) {
+//		printk(KERN_WARNING ":)))))))))))%x %x %x, fc(%x)\n", tid, *last_seq, seq, header->frame_ctl);
+//	}
+	if ((*last_seq == seq) &&
+	    time_after(*last_time + IEEE_PACKET_RETRY_TIME, jiffies)) {
+		if (*last_frag == frag)
+			goto drop;
+		if (*last_frag + 1 != frag)
+			/* out-of-order fragment */
+			goto drop;
+	} else
+		*last_seq = seq;
+
+	*last_frag = frag;
+	*last_time = jiffies;
+	return 0;
+
+drop:
+//	BUG_ON(!(fc & IEEE80211_FCTL_RETRY));
+
+	return 1;
+}
+
+static bool AddReorderEntry(struct rx_ts_record *pTS, struct rx_reorder_entry *pReorderEntry)
+{
+	struct list_head *pList = &pTS->rx_pending_pkt_list;
+	while (pList->next != &pTS->rx_pending_pkt_list) {
+		if (SN_LESS(pReorderEntry->SeqNum, list_entry(pList->next, struct rx_reorder_entry, List)->SeqNum))
+			pList = pList->next;
+		else if (SN_EQUAL(pReorderEntry->SeqNum, list_entry(pList->next, struct rx_reorder_entry, List)->SeqNum))
+			return false;
+		else
+			break;
+	}
+	pReorderEntry->List.next = pList->next;
+	pReorderEntry->List.next->prev = &pReorderEntry->List;
+	pReorderEntry->List.prev = pList;
+	pList->next = &pReorderEntry->List;
+
+	return true;
+}
+
+static void indicate_packets(struct ieee80211_device *ieee,
+			     struct ieee80211_rxb *rxb)
+{
+	struct net_device_stats *stats = &ieee->stats;
+	struct net_device *dev = ieee->dev;
+	u16 ethertype;
+	u8 i;
+
+	for (i = 0; i < rxb->nr_subframes; i++) {
+		struct sk_buff *sub_skb = rxb->subframes[i];
+
+		if (!sub_skb)
+			continue;
+
+		/* convert hdr + possible LLC headers into Ethernet header */
+		ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
+		if (sub_skb->len >= 8 &&
+		    ((!memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) &&
+			ethertype != ETH_P_AARP &&
+			ethertype != ETH_P_IPX) ||
+		     !memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE))) {
+			/* remove RFC1042 or Bridge-Tunnel encapsulation and
+			 * replace EtherType */
+			skb_pull(sub_skb, SNAP_SIZE);
+		} else {
+			/* Leave Ethernet header part of hdr and full payload */
+			put_unaligned_be16(sub_skb->len, skb_push(sub_skb, 2));
+		}
+		memcpy(skb_push(sub_skb, ETH_ALEN), rxb->src, ETH_ALEN);
+		memcpy(skb_push(sub_skb, ETH_ALEN), rxb->dst, ETH_ALEN);
+
+		stats->rx_packets++;
+		stats->rx_bytes += sub_skb->len;
+		if (is_multicast_ether_addr(rxb->dst))
+			stats->multicast++;
+
+		/* Indicate the packets to upper layer */
+		sub_skb->protocol = eth_type_trans(sub_skb, dev);
+		memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
+		sub_skb->dev = dev;
+		/* 802.11 crc not sufficient */
+		sub_skb->ip_summed = CHECKSUM_NONE;
+		ieee->last_rx_ps_time = jiffies;
+		netif_rx(sub_skb);
+	}
+}
+
+void ieee80211_indicate_packets(struct ieee80211_device *ieee,
+				struct ieee80211_rxb **prxbIndicateArray,
+				u8 index)
+{
+	u8 i;
+
+	for (i = 0; i < index; i++) {
+		struct ieee80211_rxb *prxb = prxbIndicateArray[i];
+
+		indicate_packets(ieee, prxb);
+		kfree(prxb);
+		prxb = NULL;
+	}
+}
+
+static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
+				    struct ieee80211_rxb *prxb,
+				    struct rx_ts_record *pTS, u16 SeqNum)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	struct rx_reorder_entry *pReorderEntry = NULL;
+	struct ieee80211_rxb **prxbIndicateArray;
+	u8			WinSize = pHTInfo->RxReorderWinSize;
+	u16			WinEnd = (pTS->rx_indicate_seq + WinSize - 1) % 4096;
+	u8			index = 0;
+	bool			bMatchWinStart = false, bPktInBuf = false;
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): Seq is %d,pTS->rx_indicate_seq is %d, WinSize is %d\n", __func__, SeqNum, pTS->rx_indicate_seq, WinSize);
+
+	prxbIndicateArray = kmalloc_array(REORDER_WIN_SIZE,
+					  sizeof(struct ieee80211_rxb *),
+					  GFP_ATOMIC);
+	if (!prxbIndicateArray)
+		return;
+
+	/* Rx Reorder initialize condition.*/
+	if (pTS->rx_indicate_seq == 0xffff)
+		pTS->rx_indicate_seq = SeqNum;
+
+	/* Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(SeqNum, pTS->rx_indicate_seq)) {
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Packet Drop! IndicateSeq: %d, NewSeq: %d\n",
+				 pTS->rx_indicate_seq, SeqNum);
+		pHTInfo->RxReorderDropCounter++;
+		{
+			int i;
+			for (i = 0; i < prxb->nr_subframes; i++)
+				dev_kfree_skb(prxb->subframes[i]);
+
+			kfree(prxb);
+			prxb = NULL;
+		}
+
+		kfree(prxbIndicateArray);
+		return;
+	}
+
+	/*
+	 * Sliding window manipulation. Conditions includes:
+	 * 1. Incoming SeqNum is equal to WinStart =>Window shift 1
+	 * 2. Incoming SeqNum is larger than the WinEnd => Window shift N
+	 */
+	if (SN_EQUAL(SeqNum, pTS->rx_indicate_seq)) {
+		pTS->rx_indicate_seq = (pTS->rx_indicate_seq + 1) % 4096;
+		bMatchWinStart = true;
+	} else if (SN_LESS(WinEnd, SeqNum)) {
+		if (SeqNum >= (WinSize - 1))
+			pTS->rx_indicate_seq = SeqNum + 1 - WinSize;
+		else
+			pTS->rx_indicate_seq = 4095 - (WinSize - (SeqNum + 1)) + 1;
+
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->rx_indicate_seq, SeqNum);
+	}
+
+	/*
+	 * Indication process.
+	 * After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets
+	 * with the SeqNum smaller than latest WinStart and buffer other packets.
+	 */
+	/* For Rx Reorder condition:
+	 * 1. All packets with SeqNum smaller than WinStart => Indicate
+	 * 2. All packets with SeqNum larger than or equal to WinStart => Buffer it.
+	 */
+	if (bMatchWinStart) {
+		/* Current packet is going to be indicated.*/
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Packets indication!! IndicateSeq: %d, NewSeq: %d\n",\
+				pTS->rx_indicate_seq, SeqNum);
+		prxbIndicateArray[0] = prxb;
+//		printk("========================>%s(): SeqNum is %d\n",__func__,SeqNum);
+		index = 1;
+	} else {
+		/* Current packet is going to be inserted into pending list.*/
+		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__func__);
+		if (!list_empty(&ieee->RxReorder_Unused_List)) {
+			pReorderEntry = list_entry(ieee->RxReorder_Unused_List.next, struct rx_reorder_entry, List);
+			list_del_init(&pReorderEntry->List);
+
+			/* Make a reorder entry and insert into a the packet list.*/
+			pReorderEntry->SeqNum = SeqNum;
+			pReorderEntry->prxb = prxb;
+	//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pREorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
+
+			if (!AddReorderEntry(pTS, pReorderEntry)) {
+				IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n",
+					__func__, pTS->rx_indicate_seq, SeqNum);
+				list_add_tail(&pReorderEntry->List, &ieee->RxReorder_Unused_List);
+				{
+					int i;
+					for (i = 0; i < prxb->nr_subframes; i++)
+						dev_kfree_skb(prxb->subframes[i]);
+
+					kfree(prxb);
+					prxb = NULL;
+				}
+			} else {
+				IEEE80211_DEBUG(IEEE80211_DL_REORDER,
+					 "Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->rx_indicate_seq, SeqNum);
+			}
+		} else {
+			/*
+			 * Packets are dropped if there is not enough reorder entries.
+			 * This part shall be modified!! We can just indicate all the
+			 * packets in buffer and get reorder entries.
+			 */
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): There is no reorder entry!! Packet is dropped!!\n");
+			{
+				int i;
+				for (i = 0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+
+				kfree(prxb);
+				prxb = NULL;
+			}
+		}
+	}
+
+	/* Check if there is any packet need indicate.*/
+	while (!list_empty(&pTS->rx_pending_pkt_list)) {
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): start RREORDER indicate\n", __func__);
+		pReorderEntry = list_entry(pTS->rx_pending_pkt_list.prev, struct rx_reorder_entry, List);
+		if (SN_LESS(pReorderEntry->SeqNum, pTS->rx_indicate_seq) ||
+		    SN_EQUAL(pReorderEntry->SeqNum, pTS->rx_indicate_seq)) {
+			/* This protect buffer from overflow. */
+			if (index >= REORDER_WIN_SIZE) {
+				IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): Buffer overflow!! \n");
+				bPktInBuf = true;
+				break;
+			}
+
+			list_del_init(&pReorderEntry->List);
+
+			if (SN_EQUAL(pReorderEntry->SeqNum, pTS->rx_indicate_seq))
+				pTS->rx_indicate_seq = (pTS->rx_indicate_seq + 1) % 4096;
+
+			IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Packets indication!! IndicateSeq: %d, NewSeq: %d\n", pTS->rx_indicate_seq, SeqNum);
+			prxbIndicateArray[index] = pReorderEntry->prxb;
+		//	printk("========================>%s(): pReorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
+			index++;
+
+			list_add_tail(&pReorderEntry->List, &ieee->RxReorder_Unused_List);
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+	}
+
+	/* Handling pending timer. Set this timer to prevent from long time Rx buffering.*/
+	if (index > 0) {
+		// Cancel previous pending timer.
+	//	del_timer_sync(&pTS->rx_pkt_pending_timer);
+		pTS->rx_timeout_indicate_seq = 0xffff;
+
+		// Indicate packets
+		if (index > REORDER_WIN_SIZE) {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): Rx Reorder buffer full!! \n");
+			kfree(prxbIndicateArray);
+			return;
+		}
+		ieee80211_indicate_packets(ieee, prxbIndicateArray, index);
+	}
+
+	if (bPktInBuf && pTS->rx_timeout_indicate_seq == 0xffff) {
+		// Set new pending timer.
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): SET rx timeout timer\n", __func__);
+		pTS->rx_timeout_indicate_seq = pTS->rx_indicate_seq;
+		if (timer_pending(&pTS->rx_pkt_pending_timer))
+			del_timer_sync(&pTS->rx_pkt_pending_timer);
+		pTS->rx_pkt_pending_timer.expires = jiffies +
+				msecs_to_jiffies(pHTInfo->RxReorderPendingTime);
+		add_timer(&pTS->rx_pkt_pending_timer);
+	}
+
+	kfree(prxbIndicateArray);
+}
+
+static u8 parse_subframe(struct ieee80211_device *ieee,
+			 struct sk_buff *skb,
+			 struct ieee80211_rx_stats *rx_stats,
+			 struct ieee80211_rxb *rxb, u8 *src, u8 *dst)
+{
+	struct rtl_80211_hdr_3addr  *hdr = (struct rtl_80211_hdr_3addr *)skb->data;
+	u16		fc = le16_to_cpu(hdr->frame_ctl);
+
+	u16		LLCOffset = sizeof(struct rtl_80211_hdr_3addr);
+	u16		ChkLength;
+	bool		bIsAggregateFrame = false;
+	u16		nSubframe_Length;
+	u8		nPadding_Length = 0;
+	u16		SeqNum = 0;
+
+	struct sk_buff *sub_skb;
+	/* just for debug purpose */
+	SeqNum = WLAN_GET_SEQ_SEQ(le16_to_cpu(hdr->seq_ctl));
+
+	if ((IEEE80211_QOS_HAS_SEQ(fc)) && \
+			(((frameqos *)(skb->data + IEEE80211_3ADDR_LEN))->field.reserved)) {
+		bIsAggregateFrame = true;
+	}
+
+	if (IEEE80211_QOS_HAS_SEQ(fc))
+		LLCOffset += 2;
+
+	if (rx_stats->bContainHTC)
+		LLCOffset += HTCLNG;
+
+	// Null packet, don't indicate it to upper layer
+	ChkLength = LLCOffset;/* + (Frame_WEP(frame)!=0 ?Adapter->MgntInfo.SecurityInfo.EncryptionHeadOverhead:0);*/
+
+	if (skb->len <= ChkLength)
+		return 0;
+
+	skb_pull(skb, LLCOffset);
+
+	if (!bIsAggregateFrame) {
+		rxb->nr_subframes = 1;
+#ifdef JOHN_NOCPY
+		rxb->subframes[0] = skb;
+#else
+		rxb->subframes[0] = skb_copy(skb, GFP_ATOMIC);
+#endif
+
+		memcpy(rxb->src, src, ETH_ALEN);
+		memcpy(rxb->dst, dst, ETH_ALEN);
+		//IEEE80211_DEBUG_DATA(IEEE80211_DL_RX,skb->data,skb->len);
+		return 1;
+	} else {
+		rxb->nr_subframes = 0;
+		memcpy(rxb->src, src, ETH_ALEN);
+		memcpy(rxb->dst, dst, ETH_ALEN);
+		while (skb->len > ETHERNET_HEADER_SIZE) {
+			/* Offset 12 denote 2 mac address */
+			nSubframe_Length = *((u16 *)(skb->data + 12));
+			//==m==>change the length order
+			nSubframe_Length = (nSubframe_Length >> 8) + (nSubframe_Length << 8);
+
+			if (skb->len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+				netdev_dbg(ieee->dev, "A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",
+					   rxb->nr_subframes);
+				netdev_dbg(ieee->dev, "A-MSDU parse error!! Subframe Length: %d\n", nSubframe_Length);
+				netdev_dbg(ieee->dev, "nRemain_Length is %d and nSubframe_Length is : %d\n", skb->len, nSubframe_Length);
+				netdev_dbg(ieee->dev, "The Packet SeqNum is %d\n", SeqNum);
+				return 0;
+			}
+
+			/* move the data point to data content */
+			skb_pull(skb, ETHERNET_HEADER_SIZE);
+
+#ifdef JOHN_NOCPY
+			sub_skb = skb_clone(skb, GFP_ATOMIC);
+			sub_skb->len = nSubframe_Length;
+			sub_skb->tail = sub_skb->data + nSubframe_Length;
+#else
+			/* Allocate new skb for releasing to upper layer */
+			sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+			if (!sub_skb)
+				return 0;
+			skb_reserve(sub_skb, 12);
+			skb_put_data(sub_skb, skb->data, nSubframe_Length);
+#endif
+			rxb->subframes[rxb->nr_subframes++] = sub_skb;
+			if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
+				IEEE80211_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+				break;
+			}
+			skb_pull(skb, nSubframe_Length);
+
+			if (skb->len != 0) {
+				nPadding_Length = 4 - ((nSubframe_Length + ETHERNET_HEADER_SIZE) % 4);
+				if (nPadding_Length == 4)
+					nPadding_Length = 0;
+
+				if (skb->len < nPadding_Length)
+					return 0;
+
+				skb_pull(skb, nPadding_Length);
+			}
+		}
+#ifdef JOHN_NOCPY
+		dev_kfree_skb(skb);
+#endif
+		//{just for debug added by david
+		//printk("AMSDU::rxb->nr_subframes = %d\n",rxb->nr_subframes);
+		//}
+		return rxb->nr_subframes;
+	}
+}
+
+/* All received frames are sent to this function. @skb contains the frame in
+ * IEEE 802.11 format, i.e., in the format it was sent over air.
+ * This function is called only as a tasklet (software IRQ). */
+int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
+		 struct ieee80211_rx_stats *rx_stats)
+{
+	struct net_device *dev = ieee->dev;
+	struct rtl_80211_hdr_4addr *hdr;
+	//struct rtl_80211_hdr_3addrqos *hdr;
+
+	size_t hdrlen;
+	u16 fc, type, stype, sc;
+	struct net_device_stats *stats;
+	unsigned int frag;
+	//added by amy for reorder
+	u8	TID = 0;
+	u16	SeqNum = 0;
+	struct rx_ts_record *pTS = NULL;
+	//bool bIsAggregateFrame = false;
+	//added by amy for reorder
+#ifdef NOT_YET
+	struct net_device *wds = NULL;
+	struct net_device *wds = NULL;
+	int from_assoc_ap = 0;
+	void *sta = NULL;
+#endif
+//	u16 qos_ctl = 0;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+	struct ieee80211_crypt_data *crypt = NULL;
+	int keyidx = 0;
+
+	int i;
+	struct ieee80211_rxb *rxb = NULL;
+	// cheat the hdr type
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	stats = &ieee->stats;
+
+	if (skb->len < 10) {
+		netdev_info(dev, "SKB length < 10\n");
+		goto rx_dropped;
+	}
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+	sc = le16_to_cpu(hdr->seq_ctl);
+
+	frag = WLAN_GET_SEQ_FRAG(sc);
+	hdrlen = ieee80211_get_hdrlen(fc);
+
+	if (HTCCheck(ieee, skb->data)) {
+		if (net_ratelimit())
+			netdev_warn(dev, "find HTCControl\n");
+		hdrlen += 4;
+		rx_stats->bContainHTC = true;
+	}
+
+	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
+#ifdef NOT_YET
+	/* Put this code here so that we avoid duplicating it in all
+	 * Rx paths. - Jean II */
+#ifdef IW_WIRELESS_SPY		/* defined in iw_handler.h */
+	/* If spy monitoring on */
+	if (iface->spy_data.spy_number > 0) {
+		struct iw_quality wstats;
+		wstats.level = rx_stats->rssi;
+		wstats.noise = rx_stats->noise;
+		wstats.updated = 6;	/* No qual value */
+		/* Update spy records */
+		wireless_spy_update(dev, hdr->addr2, &wstats);
+	}
+#endif /* IW_WIRELESS_SPY */
+	hostap_update_rx_stats(local->ap, hdr, rx_stats);
+#endif
+
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		unsigned int len = skb->len;
+
+		ieee80211_monitor_rx(ieee, skb, rx_stats);
+		stats->rx_packets++;
+		stats->rx_bytes += len;
+		return 1;
+	}
+
+	if (ieee->host_decrypt) {
+		int idx = 0;
+		if (skb->len >= hdrlen + 3)
+			idx = skb->data[hdrlen + 3] >> 6;
+		crypt = ieee->crypt[idx];
+#ifdef NOT_YET
+		sta = NULL;
+
+		/* Use station specific key to override default keys if the
+		 * receiver address is a unicast address ("individual RA"). If
+		 * bcrx_sta_key parameter is set, station specific key is used
+		 * even with broad/multicast targets (this is against IEEE
+		 * 802.11, but makes it easier to use different keys with
+		 * stations that do not support WEP key mapping). */
+
+		if (!(hdr->addr1[0] & 0x01) || local->bcrx_sta_key)
+			(void)hostap_handle_sta_crypto(local, hdr, &crypt,
+							&sta);
+#endif
+
+		/* allow NULL decrypt to indicate an station specific override
+		 * for default encryption */
+		if (crypt && (!crypt->ops || !crypt->ops->decrypt_mpdu))
+			crypt = NULL;
+
+		if (!crypt && (fc & IEEE80211_FCTL_WEP)) {
+			/* This seems to be triggered by some (multicast?)
+			 * frames from other than current BSS, so just drop the
+			 * frames silently instead of filling system log with
+			 * these reports. */
+			IEEE80211_DEBUG_DROP("Decryption failed (not set)"
+					     " (SA=%pM)\n",
+					     hdr->addr2);
+			ieee->ieee_stats.rx_discards_undecryptable++;
+			goto rx_dropped;
+		}
+	}
+
+	if (skb->len < IEEE80211_DATA_HDR3_LEN)
+		goto rx_dropped;
+
+	// if QoS enabled, should check the sequence for each of the AC
+	if ((!ieee->pHTInfo->bCurRxReorderEnable) || !ieee->current_network.qos_data.active || !IsDataFrame(skb->data) || IsLegacyDataFrame(skb->data)) {
+		if (is_duplicate_packet(ieee, hdr))
+			goto rx_dropped;
+
+	} else {
+		struct rx_ts_record *pRxTS = NULL;
+			//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): QOS ENABLE AND RECEIVE QOS DATA , we will get Ts, tid:%d\n",__func__, tid);
+		if (GetTs(
+				ieee,
+				(struct ts_common_info **)&pRxTS,
+				hdr->addr2,
+				Frame_QoSTID((u8 *)(skb->data)),
+				RX_DIR,
+				true)) {
+
+		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->rx_last_frag_num is %d,frag is %d,pRxTS->rx_last_seq_num is %d,seq is %d\n",__func__,pRxTS->rx_last_frag_num,frag,pRxTS->rx_last_seq_num,WLAN_GET_SEQ_SEQ(sc));
+			if ((fc & (1 << 11)) &&
+			    (frag == pRxTS->rx_last_frag_num) &&
+			    (WLAN_GET_SEQ_SEQ(sc) == pRxTS->rx_last_seq_num)) {
+				goto rx_dropped;
+			} else {
+				pRxTS->rx_last_frag_num = frag;
+				pRxTS->rx_last_seq_num = WLAN_GET_SEQ_SEQ(sc);
+			}
+		} else {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s(): No TS!! Skip the check!!\n", __func__);
+			goto rx_dropped;
+		}
+	}
+	if (type == IEEE80211_FTYPE_MGMT) {
+
+
+	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
+		if (ieee80211_rx_frame_mgmt(ieee, skb, rx_stats, type, stype))
+			goto rx_dropped;
+		else
+			goto rx_exit;
+	}
+
+	/* Data frame - extract src/dst addresses */
+	switch (fc & (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
+	case IEEE80211_FCTL_FROMDS:
+		memcpy(dst, hdr->addr1, ETH_ALEN);
+		memcpy(src, hdr->addr3, ETH_ALEN);
+		memcpy(bssid, hdr->addr2, ETH_ALEN);
+		break;
+	case IEEE80211_FCTL_TODS:
+		memcpy(dst, hdr->addr3, ETH_ALEN);
+		memcpy(src, hdr->addr2, ETH_ALEN);
+		memcpy(bssid, hdr->addr1, ETH_ALEN);
+		break;
+	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
+		if (skb->len < IEEE80211_DATA_HDR4_LEN)
+			goto rx_dropped;
+		memcpy(dst, hdr->addr3, ETH_ALEN);
+		memcpy(src, hdr->addr4, ETH_ALEN);
+		memcpy(bssid, ieee->current_network.bssid, ETH_ALEN);
+		break;
+	default:
+		memcpy(dst, hdr->addr1, ETH_ALEN);
+		memcpy(src, hdr->addr2, ETH_ALEN);
+		memcpy(bssid, hdr->addr3, ETH_ALEN);
+		break;
+	}
+
+#ifdef NOT_YET
+	if (hostap_rx_frame_wds(ieee, hdr, fc, &wds))
+		goto rx_dropped;
+	if (wds) {
+		skb->dev = dev = wds;
+		stats = hostap_get_stats(dev);
+	}
+
+	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
+	    (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) == IEEE80211_FCTL_FROMDS &&
+	    ieee->stadev &&
+	    memcmp(hdr->addr2, ieee->assoc_ap_addr, ETH_ALEN) == 0) {
+		/* Frame from BSSID of the AP for which we are a client */
+		skb->dev = dev = ieee->stadev;
+		stats = hostap_get_stats(dev);
+		from_assoc_ap = 1;
+	}
+
+	if ((ieee->iw_mode == IW_MODE_MASTER ||
+	     ieee->iw_mode == IW_MODE_REPEAT) &&
+	    !from_assoc_ap) {
+		switch (hostap_handle_sta_rx(ieee, dev, skb, rx_stats,
+					     wds)) {
+		case AP_RX_CONTINUE_NOT_AUTHORIZED:
+		case AP_RX_CONTINUE:
+			break;
+		case AP_RX_DROP:
+			goto rx_dropped;
+		case AP_RX_EXIT:
+			goto rx_exit;
+		}
+	}
+#endif
+	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
+	/* Nullfunc frames may have PS-bit set, so they must be passed to
+	 * hostap_handle_sta_rx() before being dropped here. */
+	if (stype != IEEE80211_STYPE_DATA &&
+	    stype != IEEE80211_STYPE_DATA_CFACK &&
+	    stype != IEEE80211_STYPE_DATA_CFPOLL &&
+	    stype != IEEE80211_STYPE_DATA_CFACKPOLL &&
+	    stype != IEEE80211_STYPE_QOS_DATA//add by David,2006.8.4
+	    ) {
+		if (stype != IEEE80211_STYPE_NULLFUNC)
+			IEEE80211_DEBUG_DROP(
+				"RX: dropped data frame "
+				"with no data (type=0x%02x, "
+				"subtype=0x%02x, len=%d)\n",
+				type, stype, skb->len);
+		goto rx_dropped;
+	}
+	if (memcmp(bssid, ieee->current_network.bssid, ETH_ALEN))
+		goto rx_dropped;
+
+	/* skb: hdr + (possibly fragmented, possibly encrypted) payload */
+
+	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	    (keyidx = ieee80211_rx_frame_decrypt(ieee, skb, crypt)) < 0) {
+		netdev_dbg(ieee->dev, "decrypt frame error\n");
+		goto rx_dropped;
+	}
+
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+
+	/* skb: hdr + (possibly fragmented) plaintext payload */
+	// PR: FIXME: hostap has additional conditions in the "if" below:
+	// ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	if ((frag != 0 || (fc & IEEE80211_FCTL_MOREFRAGS))) {
+		int flen;
+		struct sk_buff *frag_skb = ieee80211_frag_cache_get(ieee, hdr);
+		IEEE80211_DEBUG_FRAG("Rx Fragment received (%u)\n", frag);
+
+		if (!frag_skb) {
+			IEEE80211_DEBUG(IEEE80211_DL_RX | IEEE80211_DL_FRAG,
+					"Rx cannot get skb from fragment "
+					"cache (morefrag=%d seq=%u frag=%u)\n",
+					(fc & IEEE80211_FCTL_MOREFRAGS) != 0,
+					WLAN_GET_SEQ_SEQ(sc), frag);
+			goto rx_dropped;
+		}
+		flen = skb->len;
+		if (frag != 0)
+			flen -= hdrlen;
+
+		if (frag_skb->tail + flen > frag_skb->end) {
+			netdev_warn(dev, "host decrypted and "
+			       "reassembled frame did not fit skb\n");
+			ieee80211_frag_cache_invalidate(ieee, hdr);
+			goto rx_dropped;
+		}
+
+		if (frag == 0) {
+			/* copy first fragment (including full headers) into
+			 * beginning of the fragment cache skb */
+			skb_put_data(frag_skb, skb->data, flen);
+		} else {
+			/* append frame payload to the end of the fragment
+			 * cache skb */
+			skb_put_data(frag_skb, skb->data + hdrlen, flen);
+		}
+		dev_kfree_skb_any(skb);
+		skb = NULL;
+
+		if (fc & IEEE80211_FCTL_MOREFRAGS) {
+			/* more fragments expected - leave the skb in fragment
+			 * cache for now; it will be delivered to upper layers
+			 * after all fragments have been received */
+			goto rx_exit;
+		}
+
+		/* this was the last fragment and the frame will be
+		 * delivered, so remove skb from fragment cache */
+		skb = frag_skb;
+		hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+		ieee80211_frag_cache_invalidate(ieee, hdr);
+	}
+
+	/* skb: hdr + (possible reassembled) full MSDU payload; possibly still
+	 * encrypted/authenticated */
+	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	    ieee80211_rx_frame_decrypt_msdu(ieee, skb, keyidx, crypt)) {
+		netdev_dbg(ieee->dev, "==>decrypt msdu error\n");
+		goto rx_dropped;
+	}
+
+	//added by amy for AP roaming
+	ieee->LinkDetectInfo.NumRecvDataInPeriod++;
+	ieee->LinkDetectInfo.NumRxOkInPeriod++;
+
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep) {
+		if (/*ieee->ieee802_1x &&*/
+		    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+
+#ifdef CONFIG_IEEE80211_DEBUG
+			/* pass unencrypted EAPOL frames even if encryption is
+			 * configured */
+			struct eapol *eap = (struct eapol *)(skb->data +
+				24);
+			IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
+						eap_get_type(eap->type));
+#endif
+		} else {
+			IEEE80211_DEBUG_DROP(
+				"encryption configured, but RX "
+				"frame not encrypted (SA=%pM)\n",
+				hdr->addr2);
+			goto rx_dropped;
+		}
+	}
+
+#ifdef CONFIG_IEEE80211_DEBUG
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) &&
+	    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+		struct eapol *eap = (struct eapol *)(skb->data +
+			24);
+		IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
+					eap_get_type(eap->type));
+	}
+#endif
+
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep &&
+	    !ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+		IEEE80211_DEBUG_DROP(
+			"dropped unencrypted RX data "
+			"frame from %pM"
+			" (drop_unencrypted=1)\n",
+			hdr->addr2);
+		goto rx_dropped;
+	}
+/*
+	if(ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+		printk(KERN_WARNING "RX: IEEE802.1X EPAOL frame!\n");
+	}
+*/
+//added by amy for reorder
+	if (ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
+		&& !is_multicast_ether_addr(hdr->addr1)) {
+		TID = Frame_QoSTID(skb->data);
+		SeqNum = WLAN_GET_SEQ_SEQ(sc);
+		GetTs(ieee, (struct ts_common_info **)&pTS, hdr->addr2, TID, RX_DIR, true);
+		if (TID != 0 && TID != 3)
+			ieee->bis_any_nonbepkts = true;
+	}
+//added by amy for reorder
+	/* skb: hdr + (possible reassembled) full plaintext payload */
+	//ethertype = (payload[6] << 8) | payload[7];
+	rxb = kmalloc(sizeof(struct ieee80211_rxb), GFP_ATOMIC);
+	if (!rxb)
+		goto rx_dropped;
+	/* to parse amsdu packets */
+	/* qos data packets & reserved bit is 1 */
+	if (parse_subframe(ieee, skb, rx_stats, rxb, src, dst) == 0) {
+		/* only to free rxb, and not submit the packets to upper layer */
+		for (i = 0; i < rxb->nr_subframes; i++)
+			dev_kfree_skb(rxb->subframes[i]);
+
+		kfree(rxb);
+		rxb = NULL;
+		goto rx_dropped;
+	}
+
+//added by amy for reorder
+	if (!ieee->pHTInfo->bCurRxReorderEnable || !pTS) {
+		indicate_packets(ieee, rxb);
+		kfree(rxb);
+		rxb = NULL;
+
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): REORDER ENABLE AND PTS not NULL, and we will enter RxReorderIndicatePacket()\n", __func__);
+		RxReorderIndicatePacket(ieee, rxb, pTS, SeqNum);
+	}
+#ifndef JOHN_NOCPY
+	dev_kfree_skb(skb);
+#endif
+
+ rx_exit:
+#ifdef NOT_YET
+	if (sta)
+		hostap_handle_sta_release(sta);
+#endif
+	return 1;
+
+ rx_dropped:
+	kfree(rxb);
+	rxb = NULL;
+	stats->rx_dropped++;
+
+	/* Returning 0 indicates to caller that we have not handled the SKB--
+	 * so it is still allocated and can be used again by underlying
+	 * hardware as a DMA target */
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_rx);
+
+#define MGMT_FRAME_FIXED_PART_LENGTH            0x24
+
+static u8 qos_oui[QOS_OUI_LEN] = { 0x00, 0x50, 0xF2 };
+
+/*
+* Make the structure we read from the beacon packet to have
+* the right values
+*/
+static int ieee80211_verify_qos_info(struct ieee80211_qos_information_element
+				     *info_element, int sub_type)
+{
+	if (info_element->elementID != QOS_ELEMENT_ID)
+		return -1;
+	if (info_element->qui_subtype != sub_type)
+		return -1;
+	if (memcmp(info_element->qui, qos_oui, QOS_OUI_LEN))
+		return -1;
+	if (info_element->qui_type != QOS_OUI_TYPE)
+		return -1;
+	if (info_element->version != QOS_VERSION_1)
+		return -1;
+
+	return 0;
+}
+
+
+/*
+ * Parse a QoS parameter element
+ */
+static int ieee80211_read_qos_param_element(
+		struct ieee80211_qos_parameter_info *element_param,
+		struct ieee80211_info_element *info_element)
+{
+	size_t size = sizeof(*element_param);
+
+	if (!element_param || !info_element || info_element->len != size - 2)
+		return -1;
+
+	memcpy(element_param, info_element, size);
+	return ieee80211_verify_qos_info(&element_param->info_element,
+					 QOS_OUI_PARAM_SUB_TYPE);
+}
+
+/*
+ * Parse a QoS information element
+ */
+static int ieee80211_read_qos_info_element(
+		struct ieee80211_qos_information_element *element_info,
+		struct ieee80211_info_element *info_element)
+{
+	size_t size = sizeof(*element_info);
+
+	if (!element_info || !info_element || info_element->len != size - 2)
+		return -1;
+
+	memcpy(element_info, info_element, size);
+	return ieee80211_verify_qos_info(element_info, QOS_OUI_INFO_SUB_TYPE);
+}
+
+
+/*
+ * Write QoS parameters from the ac parameters.
+ */
+static int ieee80211_qos_convert_ac_to_parameters(
+		struct ieee80211_qos_parameter_info *param_elm,
+		struct ieee80211_qos_parameters *qos_param)
+{
+	int i;
+	struct ieee80211_qos_ac_parameter *ac_params;
+	u8 aci;
+	//u8 cw_min;
+	//u8 cw_max;
+
+	for (i = 0; i < QOS_QUEUE_NUM; i++) {
+		ac_params = &(param_elm->ac_params_record[i]);
+
+		aci = (ac_params->aci_aifsn & 0x60) >> 5;
+
+		if (aci >= QOS_QUEUE_NUM)
+			continue;
+		qos_param->aifs[aci] = (ac_params->aci_aifsn) & 0x0f;
+
+		/* WMM spec P.11: The minimum value for AIFSN shall be 2 */
+		qos_param->aifs[aci] = (qos_param->aifs[aci] < 2) ? 2 : qos_param->aifs[aci];
+
+		qos_param->cw_min[aci] =
+		    cpu_to_le16(ac_params->ecw_min_max & 0x0F);
+
+		qos_param->cw_max[aci] =
+		    cpu_to_le16((ac_params->ecw_min_max & 0xF0) >> 4);
+
+		qos_param->flag[aci] =
+		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
+		qos_param->tx_op_limit[aci] = ac_params->tx_op_limit;
+	}
+	return 0;
+}
+
+/*
+ * we have a generic data element which it may contain QoS information or
+ * parameters element. check the information element length to decide
+ * which type to read
+ */
+static int ieee80211_parse_qos_info_param_IE(struct ieee80211_info_element
+					     *info_element,
+					     struct ieee80211_network *network)
+{
+	int rc = 0;
+	struct ieee80211_qos_parameters *qos_param = NULL;
+	struct ieee80211_qos_information_element qos_info_element;
+
+	rc = ieee80211_read_qos_info_element(&qos_info_element, info_element);
+
+	if (rc == 0) {
+		network->qos_data.param_count = qos_info_element.ac_info & 0x0F;
+		network->flags |= NETWORK_HAS_QOS_INFORMATION;
+	} else {
+		struct ieee80211_qos_parameter_info param_element;
+
+		rc = ieee80211_read_qos_param_element(&param_element,
+						      info_element);
+		if (rc == 0) {
+			qos_param = &(network->qos_data.parameters);
+			ieee80211_qos_convert_ac_to_parameters(&param_element,
+							       qos_param);
+			network->flags |= NETWORK_HAS_QOS_PARAMETERS;
+			network->qos_data.param_count =
+			    param_element.info_element.ac_info & 0x0F;
+		}
+	}
+
+	if (rc == 0) {
+		IEEE80211_DEBUG_QOS("QoS is supported\n");
+		network->qos_data.supported = 1;
+	}
+	return rc;
+}
+
+#ifdef CONFIG_IEEE80211_DEBUG
+#define MFIE_STRING(x) case MFIE_TYPE_ ##x: return #x
+
+static const char *get_info_element_string(u16 id)
+{
+	switch (id) {
+		MFIE_STRING(SSID);
+		MFIE_STRING(RATES);
+		MFIE_STRING(FH_SET);
+		MFIE_STRING(DS_SET);
+		MFIE_STRING(CF_SET);
+		MFIE_STRING(TIM);
+		MFIE_STRING(IBSS_SET);
+		MFIE_STRING(COUNTRY);
+		MFIE_STRING(HOP_PARAMS);
+		MFIE_STRING(HOP_TABLE);
+		MFIE_STRING(REQUEST);
+		MFIE_STRING(CHALLENGE);
+		MFIE_STRING(POWER_CONSTRAINT);
+		MFIE_STRING(POWER_CAPABILITY);
+		MFIE_STRING(TPC_REQUEST);
+		MFIE_STRING(TPC_REPORT);
+		MFIE_STRING(SUPP_CHANNELS);
+		MFIE_STRING(CSA);
+		MFIE_STRING(MEASURE_REQUEST);
+		MFIE_STRING(MEASURE_REPORT);
+		MFIE_STRING(QUIET);
+		MFIE_STRING(IBSS_DFS);
+	       // MFIE_STRING(ERP_INFO);
+		MFIE_STRING(RSN);
+		MFIE_STRING(RATES_EX);
+		MFIE_STRING(GENERIC);
+		MFIE_STRING(QOS_PARAMETER);
+	default:
+		return "UNKNOWN";
+	}
+}
+#endif
+
+static inline void ieee80211_extract_country_ie(
+	struct ieee80211_device *ieee,
+	struct ieee80211_info_element *info_element,
+	struct ieee80211_network *network,
+	u8 *addr2
+)
+{
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (info_element->len != 0) {
+			memcpy(network->CountryIeBuf, info_element->data, info_element->len);
+			network->CountryIeLen = info_element->len;
+
+			if (!IS_COUNTRY_IE_VALID(ieee)) {
+				dot11d_update_country_ie(ieee, addr2, info_element->len, info_element->data);
+			}
+		}
+
+		//
+		// 070305, rcnjko: I update country IE watch dog here because
+		// some AP (e.g. Cisco 1242) don't include country IE in their
+		// probe response frame.
+		//
+		if (IS_EQUAL_CIE_SRC(ieee, addr2))
+			UPDATE_CIE_WATCHDOG(ieee);
+	}
+}
+
+int ieee80211_parse_info_param(struct ieee80211_device *ieee,
+		struct ieee80211_info_element *info_element,
+		u16 length,
+		struct ieee80211_network *network,
+		struct ieee80211_rx_stats *stats)
+{
+	u8 i;
+	short offset;
+	u16	tmp_htcap_len = 0;
+	u16	tmp_htinfo_len = 0;
+	u16 ht_realtek_agg_len = 0;
+	u8  ht_realtek_agg_buf[MAX_IE_LEN];
+//	u16 broadcom_len = 0;
+#ifdef CONFIG_IEEE80211_DEBUG
+	char rates_str[64];
+	char *p;
+#endif
+
+	while (length >= sizeof(*info_element)) {
+		if (sizeof(*info_element) + info_element->len > length) {
+			IEEE80211_DEBUG_MGMT("Info elem: parse failed: "
+					     "info_element->len + 2 > left : "
+					     "info_element->len+2=%zd left=%d, id=%d.\n",
+					     info_element->len +
+					     sizeof(*info_element),
+					     length, info_element->id);
+			/* We stop processing but don't return an error here
+			 * because some misbehaviour APs break this rule. ie.
+			 * Orinoco AP1000. */
+			break;
+		}
+
+		switch (info_element->id) {
+		case MFIE_TYPE_SSID:
+			if (ieee80211_is_empty_essid(info_element->data,
+						     info_element->len)) {
+				network->flags |= NETWORK_EMPTY_ESSID;
+				break;
+			}
+
+			network->ssid_len = min(info_element->len,
+						(u8)IW_ESSID_MAX_SIZE);
+			memcpy(network->ssid, info_element->data, network->ssid_len);
+			if (network->ssid_len < IW_ESSID_MAX_SIZE)
+				memset(network->ssid + network->ssid_len, 0,
+				       IW_ESSID_MAX_SIZE - network->ssid_len);
+
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_SSID: '%s' len=%d.\n",
+					     network->ssid, network->ssid_len);
+			break;
+
+		case MFIE_TYPE_RATES:
+#ifdef CONFIG_IEEE80211_DEBUG
+			p = rates_str;
+#endif
+			network->rates_len = min(info_element->len,
+						 MAX_RATES_LENGTH);
+			for (i = 0; i < network->rates_len; i++) {
+				network->rates[i] = info_element->data[i];
+#ifdef CONFIG_IEEE80211_DEBUG
+				p += scnprintf(p, sizeof(rates_str) -
+					      (p - rates_str), "%02X ",
+					      network->rates[i]);
+#endif
+				if (ieee80211_is_ofdm_rate
+				    (info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    IEEE80211_BASIC_RATE_MASK)
+						network->flags &=
+						    ~NETWORK_HAS_CCK;
+				}
+			}
+
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_RATES: '%s' (%d)\n",
+					     rates_str, network->rates_len);
+			break;
+
+		case MFIE_TYPE_RATES_EX:
+#ifdef CONFIG_IEEE80211_DEBUG
+			p = rates_str;
+#endif
+			network->rates_ex_len = min(info_element->len,
+						    MAX_RATES_EX_LENGTH);
+			for (i = 0; i < network->rates_ex_len; i++) {
+				network->rates_ex[i] = info_element->data[i];
+#ifdef CONFIG_IEEE80211_DEBUG
+				p += scnprintf(p, sizeof(rates_str) -
+					      (p - rates_str), "%02X ",
+					      network->rates_ex[i]);
+#endif
+				if (ieee80211_is_ofdm_rate
+				    (info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    IEEE80211_BASIC_RATE_MASK)
+						network->flags &=
+						    ~NETWORK_HAS_CCK;
+				}
+			}
+
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_RATES_EX: '%s' (%d)\n",
+					     rates_str, network->rates_ex_len);
+			break;
+
+		case MFIE_TYPE_DS_SET:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_DS_SET: %d\n",
+					     info_element->data[0]);
+			network->channel = info_element->data[0];
+			break;
+
+		case MFIE_TYPE_FH_SET:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_FH_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_CF_SET:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_CF_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_TIM:
+			if (info_element->len < 4)
+				break;
+
+			network->tim.tim_count = info_element->data[0];
+			network->tim.tim_period = info_element->data[1];
+
+			network->dtim_period = info_element->data[1];
+			if (ieee->state != IEEE80211_LINKED)
+				break;
+
+			network->last_dtim_sta_time[0] = stats->mac_time[0];
+			network->last_dtim_sta_time[1] = stats->mac_time[1];
+
+			network->dtim_data = IEEE80211_DTIM_VALID;
+
+			if (info_element->data[0] != 0)
+				break;
+
+			if (info_element->data[2] & 1)
+				network->dtim_data |= IEEE80211_DTIM_MBCAST;
+
+			offset = (info_element->data[2] >> 1) * 2;
+
+			if (ieee->assoc_id < 8 * offset ||
+				ieee->assoc_id > 8 * (offset + info_element->len - 3))
+
+				break;
+
+			offset = (ieee->assoc_id / 8) - offset;// + ((aid % 8)? 0 : 1) ;
+
+			if (info_element->data[3 + offset] & (1 << (ieee->assoc_id % 8)))
+				network->dtim_data |= IEEE80211_DTIM_UCAST;
+
+			//IEEE80211_DEBUG_MGMT("MFIE_TYPE_TIM: partially ignored\n");
+			break;
+
+		case MFIE_TYPE_ERP:
+			network->erp_value = info_element->data[0];
+			network->flags |= NETWORK_HAS_ERP_VALUE;
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_ERP_SET: %d\n",
+					     network->erp_value);
+			break;
+		case MFIE_TYPE_IBSS_SET:
+			network->atim_window = info_element->data[0];
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_IBSS_SET: %d\n",
+					     network->atim_window);
+			break;
+
+		case MFIE_TYPE_CHALLENGE:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_CHALLENGE: ignored\n");
+			break;
+
+		case MFIE_TYPE_GENERIC:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_GENERIC: %d bytes\n",
+					     info_element->len);
+			if (!ieee80211_parse_qos_info_param_IE(info_element,
+							       network))
+				break;
+
+			if (info_element->len >= 4 &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0x50 &&
+			    info_element->data[2] == 0xf2 &&
+			    info_element->data[3] == 0x01) {
+				network->wpa_ie_len = min(info_element->len + 2,
+							  MAX_WPA_IE_LEN);
+				memcpy(network->wpa_ie, info_element,
+				       network->wpa_ie_len);
+				break;
+			}
+
+#ifdef THOMAS_TURBO
+			if (info_element->len == 7 &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0xe0 &&
+			    info_element->data[2] == 0x4c &&
+			    info_element->data[3] == 0x01 &&
+			    info_element->data[4] == 0x02) {
+				network->Turbo_Enable = 1;
+			}
+#endif
+
+			//for HTcap and HTinfo parameters
+			if (tmp_htcap_len == 0) {
+				if (info_element->len >= 4 &&
+				   info_element->data[0] == 0x00 &&
+				   info_element->data[1] == 0x90 &&
+				   info_element->data[2] == 0x4c &&
+				   info_element->data[3] == 0x033){
+
+					tmp_htcap_len = min(info_element->len, (u8)MAX_IE_LEN);
+					if (tmp_htcap_len != 0) {
+						network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
+						network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ? \
+							sizeof(network->bssht.bdHTCapBuf) : tmp_htcap_len;
+						memcpy(network->bssht.bdHTCapBuf, info_element->data, network->bssht.bdHTCapLen);
+					}
+				}
+				if (tmp_htcap_len != 0)
+					network->bssht.bdSupportHT = true;
+				else
+					network->bssht.bdSupportHT = false;
+			}
+
+
+			if (tmp_htinfo_len == 0) {
+				if (info_element->len >= 4 &&
+					info_element->data[0] == 0x00 &&
+					info_element->data[1] == 0x90 &&
+					info_element->data[2] == 0x4c &&
+					info_element->data[3] == 0x034){
+
+					tmp_htinfo_len = min(info_element->len, (u8)MAX_IE_LEN);
+					if (tmp_htinfo_len != 0) {
+						network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
+						if (tmp_htinfo_len) {
+							network->bssht.bdHTInfoLen = tmp_htinfo_len > sizeof(network->bssht.bdHTInfoBuf) ? \
+								sizeof(network->bssht.bdHTInfoBuf) : tmp_htinfo_len;
+							memcpy(network->bssht.bdHTInfoBuf, info_element->data, network->bssht.bdHTInfoLen);
+						}
+
+					}
+
+				}
+			}
+
+			if (ieee->aggregation) {
+				if (network->bssht.bdSupportHT) {
+					if (info_element->len >= 4 &&
+						info_element->data[0] == 0x00 &&
+						info_element->data[1] == 0xe0 &&
+						info_element->data[2] == 0x4c &&
+						info_element->data[3] == 0x02){
+
+						ht_realtek_agg_len = min(info_element->len, (u8)MAX_IE_LEN);
+						memcpy(ht_realtek_agg_buf, info_element->data, info_element->len);
+
+					}
+					if (ht_realtek_agg_len >= 5) {
+						network->bssht.bdRT2RTAggregation = true;
+
+						if ((ht_realtek_agg_buf[4] == 1) && (ht_realtek_agg_buf[5] & 0x02))
+							network->bssht.bdRT2RTLongSlotTime = true;
+					}
+				}
+
+			}
+
+			//if(tmp_htcap_len !=0  ||  tmp_htinfo_len != 0)
+			{
+				if ((info_element->len >= 3 &&
+					 info_element->data[0] == 0x00 &&
+					 info_element->data[1] == 0x05 &&
+					 info_element->data[2] == 0xb5) ||
+					 (info_element->len >= 3 &&
+					 info_element->data[0] == 0x00 &&
+					 info_element->data[1] == 0x0a &&
+					 info_element->data[2] == 0xf7) ||
+					 (info_element->len >= 3 &&
+					 info_element->data[0] == 0x00 &&
+					 info_element->data[1] == 0x10 &&
+					 info_element->data[2] == 0x18)){
+
+					network->broadcom_cap_exist = true;
+
+				}
+			}
+			if (info_element->len >= 3 &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x0c &&
+				info_element->data[2] == 0x43) {
+				network->ralink_cap_exist = true;
+			} else
+				network->ralink_cap_exist = false;
+			//added by amy for atheros AP
+			if ((info_element->len >= 3 &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x03 &&
+				info_element->data[2] == 0x7f) ||
+				(info_element->len >= 3 &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x13 &&
+				info_element->data[2] == 0x74)) {
+				netdev_dbg(ieee->dev, "========> athros AP is exist\n");
+				network->atheros_cap_exist = true;
+			} else
+				network->atheros_cap_exist = false;
+
+			if (info_element->len >= 3 &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x40 &&
+				info_element->data[2] == 0x96) {
+				network->cisco_cap_exist = true;
+			} else
+				network->cisco_cap_exist = false;
+			//added by amy for LEAP of cisco
+			if (info_element->len > 4 &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x40 &&
+				info_element->data[2] == 0x96 &&
+				info_element->data[3] == 0x01) {
+				if (info_element->len == 6) {
+					memcpy(network->CcxRmState, &info_element[4], 2);
+					if (network->CcxRmState[0] != 0)
+						network->bCcxRmEnable = true;
+					else
+						network->bCcxRmEnable = false;
+					//
+					// CCXv4 Table 59-1 MBSSID Masks.
+					//
+					network->MBssidMask = network->CcxRmState[1] & 0x07;
+					if (network->MBssidMask != 0) {
+						network->bMBssidValid = true;
+						network->MBssidMask = 0xff << (network->MBssidMask);
+						ether_addr_copy(network->MBssid, network->bssid);
+						network->MBssid[5] &= network->MBssidMask;
+					} else {
+						network->bMBssidValid = false;
+					}
+				} else {
+					network->bCcxRmEnable = false;
+				}
+			}
+			if (info_element->len > 4  &&
+				info_element->data[0] == 0x00 &&
+				info_element->data[1] == 0x40 &&
+				info_element->data[2] == 0x96 &&
+				info_element->data[3] == 0x03) {
+				if (info_element->len == 5) {
+					network->bWithCcxVerNum = true;
+					network->BssCcxVerNumber = info_element->data[4];
+				} else {
+					network->bWithCcxVerNum = false;
+					network->BssCcxVerNumber = 0;
+				}
+			}
+			break;
+
+		case MFIE_TYPE_RSN:
+			IEEE80211_DEBUG_MGMT("MFIE_TYPE_RSN: %d bytes\n",
+					     info_element->len);
+			network->rsn_ie_len = min(info_element->len + 2,
+						  MAX_WPA_IE_LEN);
+			memcpy(network->rsn_ie, info_element,
+			       network->rsn_ie_len);
+			break;
+
+			//HT related element.
+		case MFIE_TYPE_HT_CAP:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_HT_CAP: %d bytes\n",
+					     info_element->len);
+			tmp_htcap_len = min(info_element->len, (u8)MAX_IE_LEN);
+			if (tmp_htcap_len != 0) {
+				network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
+				network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ? \
+					sizeof(network->bssht.bdHTCapBuf) : tmp_htcap_len;
+				memcpy(network->bssht.bdHTCapBuf, info_element->data, network->bssht.bdHTCapLen);
+
+				//If peer is HT, but not WMM, call QosSetLegacyWMMParamWithHT()
+				// windows driver will update WMM parameters each beacon received once connected
+				// Linux driver is a bit different.
+				network->bssht.bdSupportHT = true;
+			} else
+				network->bssht.bdSupportHT = false;
+			break;
+
+
+		case MFIE_TYPE_HT_INFO:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_HT_INFO: %d bytes\n",
+					     info_element->len);
+			tmp_htinfo_len = min(info_element->len, (u8)MAX_IE_LEN);
+			if (tmp_htinfo_len) {
+				network->bssht.bdHTSpecVer = HT_SPEC_VER_IEEE;
+				network->bssht.bdHTInfoLen = tmp_htinfo_len > sizeof(network->bssht.bdHTInfoBuf) ? \
+					sizeof(network->bssht.bdHTInfoBuf) : tmp_htinfo_len;
+				memcpy(network->bssht.bdHTInfoBuf, info_element->data, network->bssht.bdHTInfoLen);
+			}
+			break;
+
+		case MFIE_TYPE_AIRONET:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_AIRONET: %d bytes\n",
+					     info_element->len);
+			if (info_element->len > IE_CISCO_FLAG_POSITION) {
+				network->bWithAironetIE = true;
+
+				// CCX 1 spec v1.13, A01.1 CKIP Negotiation (page23):
+				// "A Cisco access point advertises support for CKIP in beacon and probe response packets,
+				//  by adding an Aironet element and setting one or both of the CKIP negotiation bits."
+				if ((info_element->data[IE_CISCO_FLAG_POSITION] & SUPPORT_CKIP_MIC)	||
+					(info_element->data[IE_CISCO_FLAG_POSITION] & SUPPORT_CKIP_PK)) {
+					network->bCkipSupported = true;
+				} else {
+					network->bCkipSupported = false;
+				}
+			} else {
+				network->bWithAironetIE = false;
+				network->bCkipSupported = false;
+			}
+			break;
+		case MFIE_TYPE_QOS_PARAMETER:
+			netdev_err(ieee->dev,
+				   "QoS Error need to parse QOS_PARAMETER IE\n");
+			break;
+
+		case MFIE_TYPE_COUNTRY:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_COUNTRY: %d bytes\n",
+					     info_element->len);
+			ieee80211_extract_country_ie(ieee, info_element, network, network->bssid);//addr2 is same as addr3 when from an AP
+			break;
+/* TODO */
+		default:
+			IEEE80211_DEBUG_MGMT
+			    ("Unsupported info element: %s (%d)\n",
+			     get_info_element_string(info_element->id),
+			     info_element->id);
+			break;
+		}
+
+		length -= sizeof(*info_element) + info_element->len;
+		info_element =
+		    (struct ieee80211_info_element *)&info_element->
+		    data[info_element->len];
+	}
+
+	if (!network->atheros_cap_exist && !network->broadcom_cap_exist &&
+		!network->cisco_cap_exist && !network->ralink_cap_exist && !network->bssht.bdRT2RTAggregation) {
+		network->unknown_cap_exist = true;
+	} else {
+		network->unknown_cap_exist = false;
+	}
+	return 0;
+}
+
+static inline u8 ieee80211_SignalStrengthTranslate(
+	u8  CurrSS
+	)
+{
+	u8 RetSS;
+
+	// Step 1. Scale mapping.
+	if (CurrSS >= 71 && CurrSS <= 100) {
+		RetSS = 90 + ((CurrSS - 70) / 3);
+	} else if (CurrSS >= 41 && CurrSS <= 70) {
+		RetSS = 78 + ((CurrSS - 40) / 3);
+	} else if (CurrSS >= 31 && CurrSS <= 40) {
+		RetSS = 66 + (CurrSS - 30);
+	} else if (CurrSS >= 21 && CurrSS <= 30) {
+		RetSS = 54 + (CurrSS - 20);
+	} else if (CurrSS >= 5 && CurrSS <= 20) {
+		RetSS = 42 + (((CurrSS - 5) * 2) / 3);
+	} else if (CurrSS == 4) {
+		RetSS = 36;
+	} else if (CurrSS == 3) {
+		RetSS = 27;
+	} else if (CurrSS == 2) {
+		RetSS = 18;
+	} else if (CurrSS == 1) {
+		RetSS = 9;
+	} else {
+		RetSS = CurrSS;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("##### After Mapping:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	// Step 2. Smoothing.
+
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("$$$$$ After Smoothing:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	return RetSS;
+}
+
+/* 0-100 index */
+static long ieee80211_translate_todbm(u8 signal_strength_index)
+{
+	long	signal_power; // in dBm.
+
+	// Translate to dBm (x=0.5y-95).
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+
+	return signal_power;
+}
+
+static inline int ieee80211_network_init(
+	struct ieee80211_device *ieee,
+	struct ieee80211_probe_response *beacon,
+	struct ieee80211_network *network,
+	struct ieee80211_rx_stats *stats)
+{
+#ifdef CONFIG_IEEE80211_DEBUG
+	//char rates_str[64];
+	//char *p;
+#endif
+
+	network->qos_data.active = 0;
+	network->qos_data.supported = 0;
+	network->qos_data.param_count = 0;
+	network->qos_data.old_param_count = 0;
+
+	/* Pull out fixed field data */
+	memcpy(network->bssid, beacon->header.addr3, ETH_ALEN);
+	network->capability = le16_to_cpu(beacon->capability);
+	network->last_scanned = jiffies;
+	network->time_stamp[0] = le32_to_cpu(beacon->time_stamp[0]);
+	network->time_stamp[1] = le32_to_cpu(beacon->time_stamp[1]);
+	network->beacon_interval = le16_to_cpu(beacon->beacon_interval);
+	/* Where to pull this? beacon->listen_interval;*/
+	network->listen_interval = 0x0A;
+	network->rates_len = network->rates_ex_len = 0;
+	network->last_associate = 0;
+	network->ssid_len = 0;
+	network->flags = 0;
+	network->atim_window = 0;
+	network->erp_value = (network->capability & WLAN_CAPABILITY_IBSS) ?
+	    0x3 : 0x0;
+	network->berp_info_valid = false;
+	network->broadcom_cap_exist = false;
+	network->ralink_cap_exist = false;
+	network->atheros_cap_exist = false;
+	network->cisco_cap_exist = false;
+	network->unknown_cap_exist = false;
+#ifdef THOMAS_TURBO
+	network->Turbo_Enable = 0;
+#endif
+	network->CountryIeLen = 0;
+	memset(network->CountryIeBuf, 0, MAX_IE_LEN);
+//Initialize HT parameters
+	//ieee80211_ht_initialize(&network->bssht);
+	HTInitializeBssDesc(&network->bssht);
+	if (stats->freq == IEEE80211_52GHZ_BAND) {
+		/* for A band (No DS info) */
+		network->channel = stats->received_channel;
+	} else
+		network->flags |= NETWORK_HAS_CCK;
+
+	network->wpa_ie_len = 0;
+	network->rsn_ie_len = 0;
+
+	if (ieee80211_parse_info_param
+	    (ieee, beacon->info_element, stats->len - sizeof(*beacon), network, stats))
+		return 1;
+
+	network->mode = 0;
+	if (stats->freq == IEEE80211_52GHZ_BAND)
+		network->mode = IEEE_A;
+	else {
+		if (network->flags & NETWORK_HAS_OFDM)
+			network->mode |= IEEE_G;
+		if (network->flags & NETWORK_HAS_CCK)
+			network->mode |= IEEE_B;
+	}
+
+	if (network->mode == 0) {
+		IEEE80211_DEBUG_SCAN("Filtered out '%s (%pM)' "
+				     "network.\n",
+				     escape_essid(network->ssid,
+						  network->ssid_len),
+				     network->bssid);
+		return 1;
+	}
+
+	if (network->bssht.bdSupportHT) {
+		if (network->mode == IEEE_A)
+			network->mode = IEEE_N_5G;
+		else if (network->mode & (IEEE_G | IEEE_B))
+			network->mode = IEEE_N_24G;
+	}
+	if (ieee80211_is_empty_essid(network->ssid, network->ssid_len))
+		network->flags |= NETWORK_EMPTY_ESSID;
+
+	stats->signal = 30 + (stats->SignalStrength * 70) / 100;
+	//stats->signal = ieee80211_SignalStrengthTranslate(stats->signal);
+	stats->noise = ieee80211_translate_todbm((u8)(100 - stats->signal)) - 25;
+
+	memcpy(&network->stats, stats, sizeof(network->stats));
+
+	return 0;
+}
+
+static inline int is_same_network(struct ieee80211_network *src,
+				  struct ieee80211_network *dst, struct ieee80211_device *ieee)
+{
+	/* A network is only a duplicate if the channel, BSSID, ESSID
+	 * and the capability field (in particular IBSS and BSS) all match.
+	 * We treat all <hidden> with the same BSSID and channel
+	 * as one network */
+	return //((src->ssid_len == dst->ssid_len) &&
+		(((src->ssid_len == dst->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) &&
+		(src->channel == dst->channel) &&
+		!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
+		//!memcmp(src->ssid, dst->ssid, src->ssid_len) &&
+		(!memcmp(src->ssid, dst->ssid, src->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) &&
+		((src->capability & WLAN_CAPABILITY_IBSS) ==
+		(dst->capability & WLAN_CAPABILITY_IBSS)) &&
+		((src->capability & WLAN_CAPABILITY_BSS) ==
+		(dst->capability & WLAN_CAPABILITY_BSS)));
+}
+
+static inline void update_network(struct ieee80211_network *dst,
+				  struct ieee80211_network *src)
+{
+	int qos_active;
+	u8 old_param;
+
+	memcpy(&dst->stats, &src->stats, sizeof(struct ieee80211_rx_stats));
+	dst->capability = src->capability;
+	memcpy(dst->rates, src->rates, src->rates_len);
+	dst->rates_len = src->rates_len;
+	memcpy(dst->rates_ex, src->rates_ex, src->rates_ex_len);
+	dst->rates_ex_len = src->rates_ex_len;
+	if (src->ssid_len > 0) {
+		memset(dst->ssid, 0, dst->ssid_len);
+		dst->ssid_len = src->ssid_len;
+		memcpy(dst->ssid, src->ssid, src->ssid_len);
+	}
+	dst->mode = src->mode;
+	dst->flags = src->flags;
+	dst->time_stamp[0] = src->time_stamp[0];
+	dst->time_stamp[1] = src->time_stamp[1];
+	if (src->flags & NETWORK_HAS_ERP_VALUE) {
+		dst->erp_value = src->erp_value;
+		dst->berp_info_valid = src->berp_info_valid = true;
+	}
+	dst->beacon_interval = src->beacon_interval;
+	dst->listen_interval = src->listen_interval;
+	dst->atim_window = src->atim_window;
+	dst->dtim_period = src->dtim_period;
+	dst->dtim_data = src->dtim_data;
+	dst->last_dtim_sta_time[0] = src->last_dtim_sta_time[0];
+	dst->last_dtim_sta_time[1] = src->last_dtim_sta_time[1];
+	memcpy(&dst->tim, &src->tim, sizeof(struct ieee80211_tim_parameters));
+
+	dst->bssht.bdSupportHT = src->bssht.bdSupportHT;
+	dst->bssht.bdRT2RTAggregation = src->bssht.bdRT2RTAggregation;
+	dst->bssht.bdHTCapLen = src->bssht.bdHTCapLen;
+	memcpy(dst->bssht.bdHTCapBuf, src->bssht.bdHTCapBuf, src->bssht.bdHTCapLen);
+	dst->bssht.bdHTInfoLen = src->bssht.bdHTInfoLen;
+	memcpy(dst->bssht.bdHTInfoBuf, src->bssht.bdHTInfoBuf, src->bssht.bdHTInfoLen);
+	dst->bssht.bdHTSpecVer = src->bssht.bdHTSpecVer;
+	dst->bssht.bdRT2RTLongSlotTime = src->bssht.bdRT2RTLongSlotTime;
+	dst->broadcom_cap_exist = src->broadcom_cap_exist;
+	dst->ralink_cap_exist = src->ralink_cap_exist;
+	dst->atheros_cap_exist = src->atheros_cap_exist;
+	dst->cisco_cap_exist = src->cisco_cap_exist;
+	dst->unknown_cap_exist = src->unknown_cap_exist;
+	memcpy(dst->wpa_ie, src->wpa_ie, src->wpa_ie_len);
+	dst->wpa_ie_len = src->wpa_ie_len;
+	memcpy(dst->rsn_ie, src->rsn_ie, src->rsn_ie_len);
+	dst->rsn_ie_len = src->rsn_ie_len;
+
+	dst->last_scanned = jiffies;
+	/* qos related parameters */
+	//qos_active = src->qos_data.active;
+	qos_active = dst->qos_data.active;
+	//old_param = dst->qos_data.old_param_count;
+	old_param = dst->qos_data.param_count;
+	if (dst->flags & NETWORK_HAS_QOS_MASK)
+		memcpy(&dst->qos_data, &src->qos_data,
+			sizeof(struct ieee80211_qos_data));
+	else {
+		dst->qos_data.supported = src->qos_data.supported;
+		dst->qos_data.param_count = src->qos_data.param_count;
+	}
+
+	if (dst->qos_data.supported == 1) {
+		dst->QoS_Enable = 1;
+		if (dst->ssid_len)
+			IEEE80211_DEBUG_QOS
+				("QoS the network %s is QoS supported\n",
+				dst->ssid);
+		else
+			IEEE80211_DEBUG_QOS
+				("QoS the network is QoS supported\n");
+	}
+	dst->qos_data.active = qos_active;
+	dst->qos_data.old_param_count = old_param;
+
+	/* dst->last_associate is not overwritten */
+	dst->wmm_info = src->wmm_info; //sure to exist in beacon or probe response frame.
+	if (src->wmm_param[0].aci_aifsn || \
+	   src->wmm_param[1].aci_aifsn || \
+	   src->wmm_param[2].aci_aifsn || \
+	   src->wmm_param[3].aci_aifsn) {
+		memcpy(dst->wmm_param, src->wmm_param, WME_AC_PRAM_LEN);
+	}
+	//dst->QoS_Enable = src->QoS_Enable;
+#ifdef THOMAS_TURBO
+	dst->Turbo_Enable = src->Turbo_Enable;
+#endif
+
+	dst->CountryIeLen = src->CountryIeLen;
+	memcpy(dst->CountryIeBuf, src->CountryIeBuf, src->CountryIeLen);
+
+	//added by amy for LEAP
+	dst->bWithAironetIE = src->bWithAironetIE;
+	dst->bCkipSupported = src->bCkipSupported;
+	memcpy(dst->CcxRmState, src->CcxRmState, 2);
+	dst->bCcxRmEnable = src->bCcxRmEnable;
+	dst->MBssidMask = src->MBssidMask;
+	dst->bMBssidValid = src->bMBssidValid;
+	memcpy(dst->MBssid, src->MBssid, 6);
+	dst->bWithCcxVerNum = src->bWithCcxVerNum;
+	dst->BssCcxVerNumber = src->BssCcxVerNumber;
+
+}
+
+static inline int is_beacon(__le16 fc)
+{
+	return (WLAN_FC_GET_STYPE(le16_to_cpu(fc)) == IEEE80211_STYPE_BEACON);
+}
+
+static inline void ieee80211_process_probe_response(
+	struct ieee80211_device *ieee,
+	struct ieee80211_probe_response *beacon,
+	struct ieee80211_rx_stats *stats)
+{
+	struct ieee80211_network *network;
+	struct ieee80211_network *target;
+	struct ieee80211_network *oldest = NULL;
+#ifdef CONFIG_IEEE80211_DEBUG
+	struct ieee80211_info_element *info_element = &beacon->info_element[0];
+#endif
+	int fc = WLAN_FC_GET_STYPE(le16_to_cpu(beacon->header.frame_ctl));
+	unsigned long flags;
+	short renew;
+	u16 capability;
+	//u8 wmm_info;
+
+	network = kzalloc(sizeof(*network), GFP_ATOMIC);
+	if (!network)
+		goto out;
+
+	capability = le16_to_cpu(beacon->capability);
+	IEEE80211_DEBUG_SCAN(
+		"'%s' (%pM): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
+		escape_essid(info_element->data, info_element->len),
+		beacon->header.addr3,
+		(capability & BIT(0xf)) ? '1' : '0',
+		(capability & BIT(0xe)) ? '1' : '0',
+		(capability & BIT(0xd)) ? '1' : '0',
+		(capability & BIT(0xc)) ? '1' : '0',
+		(capability & BIT(0xb)) ? '1' : '0',
+		(capability & BIT(0xa)) ? '1' : '0',
+		(capability & BIT(0x9)) ? '1' : '0',
+		(capability & BIT(0x8)) ? '1' : '0',
+		(capability & BIT(0x7)) ? '1' : '0',
+		(capability & BIT(0x6)) ? '1' : '0',
+		(capability & BIT(0x5)) ? '1' : '0',
+		(capability & BIT(0x4)) ? '1' : '0',
+		(capability & BIT(0x3)) ? '1' : '0',
+		(capability & BIT(0x2)) ? '1' : '0',
+		(capability & BIT(0x1)) ? '1' : '0',
+		(capability & BIT(0x0)) ? '1' : '0');
+
+	if (ieee80211_network_init(ieee, beacon, network, stats)) {
+		IEEE80211_DEBUG_SCAN("Dropped '%s' (%pM) via %s.\n",
+				     escape_essid(info_element->data,
+						  info_element->len),
+				     beacon->header.addr3,
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+		goto out;
+	}
+
+	// For Asus EeePc request,
+	// (1) if wireless adapter receive get any 802.11d country code in AP beacon,
+	//	   wireless adapter should follow the country code.
+	// (2)  If there is no any country code in beacon,
+	//       then wireless adapter should do active scan from ch1~11 and
+	//       passive scan from ch12~14
+
+	if (!is_legal_channel(ieee, network->channel))
+		goto out;
+	if (ieee->bGlobalDomain) {
+		if (fc == IEEE80211_STYPE_PROBE_RESP) {
+			if (IS_COUNTRY_IE_VALID(ieee)) {
+				// Case 1: Country code
+				if (!is_legal_channel(ieee, network->channel)) {
+					netdev_warn(ieee->dev, "GetScanInfo(): For Country code, filter probe response at channel(%d).\n", network->channel);
+					goto out;
+				}
+			} else {
+				// Case 2: No any country code.
+				// Filter over channel ch12~14
+				if (network->channel > 11) {
+					netdev_warn(ieee->dev, "GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n", network->channel);
+					goto out;
+				}
+			}
+		} else {
+			if (IS_COUNTRY_IE_VALID(ieee)) {
+				// Case 1: Country code
+				if (!is_legal_channel(ieee, network->channel)) {
+					netdev_warn(ieee->dev, "GetScanInfo(): For Country code, filter beacon at channel(%d).\n", network->channel);
+					goto out;
+				}
+			} else {
+				// Case 2: No any country code.
+				// Filter over channel ch12~14
+				if (network->channel > 14) {
+					netdev_warn(ieee->dev, "GetScanInfo(): For Global Domain, filter beacon at channel(%d).\n", network->channel);
+					goto out;
+				}
+			}
+		}
+	}
+
+	/* The network parsed correctly -- so now we scan our known networks
+	 * to see if we can find it in our list.
+	 *
+	 * NOTE:  This search is definitely not optimized.  Once its doing
+	 *        the "right thing" we'll optimize it for efficiency if
+	 *        necessary */
+
+	/* Search for this entry in the list and update it if it is
+	 * already there. */
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (is_same_network(&ieee->current_network, network, ieee)) {
+		update_network(&ieee->current_network, network);
+		if ((ieee->current_network.mode == IEEE_N_24G || ieee->current_network.mode == IEEE_G)
+		    && ieee->current_network.berp_info_valid){
+			if (ieee->current_network.erp_value & ERP_UseProtection)
+				ieee->current_network.buseprotection = true;
+			else
+				ieee->current_network.buseprotection = false;
+		}
+		if (is_beacon(beacon->header.frame_ctl)) {
+			if (ieee->state == IEEE80211_LINKED)
+				ieee->LinkDetectInfo.NumRecvBcnInPeriod++;
+		} else //hidden AP
+			network->flags = (~NETWORK_EMPTY_ESSID & network->flags) | (NETWORK_EMPTY_ESSID & ieee->current_network.flags);
+	}
+
+	list_for_each_entry(target, &ieee->network_list, list) {
+		if (is_same_network(target, network, ieee))
+			break;
+		if (!oldest ||
+		    (target->last_scanned < oldest->last_scanned))
+			oldest = target;
+	}
+
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information */
+	if (&target->list == &ieee->network_list) {
+		if (list_empty(&ieee->network_free_list)) {
+			/* If there are no more slots, expire the oldest */
+			list_del(&oldest->list);
+			target = oldest;
+			IEEE80211_DEBUG_SCAN("Expired '%s' (%pM) from "
+					     "network list.\n",
+					     escape_essid(target->ssid,
+							  target->ssid_len),
+					     target->bssid);
+		} else {
+			/* Otherwise just pull from the free list */
+			target = list_entry(ieee->network_free_list.next,
+					    struct ieee80211_network, list);
+			list_del(ieee->network_free_list.next);
+		}
+
+
+#ifdef CONFIG_IEEE80211_DEBUG
+		IEEE80211_DEBUG_SCAN("Adding '%s' (%pM) via %s.\n",
+				     escape_essid(network->ssid,
+						  network->ssid_len),
+				     network->bssid,
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+#endif
+		memcpy(target, network, sizeof(*target));
+		list_add_tail(&target->list, &ieee->network_list);
+		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE)
+			ieee80211_softmac_new_net(ieee, network);
+	} else {
+		IEEE80211_DEBUG_SCAN("Updating '%s' (%pM) via %s.\n",
+				     escape_essid(target->ssid,
+						  target->ssid_len),
+				     target->bssid,
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+
+		/* we have an entry and we are going to update it. But this entry may
+		 * be already expired. In this case we do the same as we found a new
+		 * net and call the new_net handler
+		 */
+		renew = !time_after(target->last_scanned + ieee->scan_age, jiffies);
+		//YJ,add,080819,for hidden ap
+		if (is_beacon(beacon->header.frame_ctl) == 0)
+			network->flags = (~NETWORK_EMPTY_ESSID & network->flags) | (NETWORK_EMPTY_ESSID & target->flags);
+		//if(strncmp(network->ssid, "linksys-c",9) == 0)
+		//	printk("====>2 network->ssid=%s FLAG=%d target.ssid=%s FLAG=%d\n", network->ssid, network->flags, target->ssid, target->flags);
+		if (((network->flags & NETWORK_EMPTY_ESSID) == NETWORK_EMPTY_ESSID) \
+		    && (((network->ssid_len > 0) && (strncmp(target->ssid, network->ssid, network->ssid_len)))\
+ || ((ieee->current_network.ssid_len == network->ssid_len) && (strncmp(ieee->current_network.ssid, network->ssid, network->ssid_len) == 0) && (ieee->state == IEEE80211_NOLINK))))
+			renew = 1;
+		//YJ,add,080819,for hidden ap,end
+
+		update_network(target, network);
+		if (renew && (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE))
+			ieee80211_softmac_new_net(ieee, network);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	if (is_beacon(beacon->header.frame_ctl) && is_same_network(&ieee->current_network, network, ieee) && \
+		(ieee->state == IEEE80211_LINKED)) {
+		if (ieee->handle_beacon)
+			ieee->handle_beacon(ieee->dev, beacon, &ieee->current_network);
+	}
+
+out:
+	kfree(network);
+}
+
+void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+		      struct rtl_80211_hdr_4addr *header,
+		      struct ieee80211_rx_stats *stats)
+{
+	switch (WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl))) {
+
+	case IEEE80211_STYPE_BEACON:
+		IEEE80211_DEBUG_MGMT("received BEACON (%d)\n",
+			WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
+		IEEE80211_DEBUG_SCAN("Beacon\n");
+		ieee80211_process_probe_response(
+			ieee, (struct ieee80211_probe_response *)header, stats);
+		break;
+
+	case IEEE80211_STYPE_PROBE_RESP:
+		IEEE80211_DEBUG_MGMT("received PROBE RESPONSE (%d)\n",
+			WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
+		IEEE80211_DEBUG_SCAN("Probe response\n");
+		ieee80211_process_probe_response(
+			ieee, (struct ieee80211_probe_response *)header, stats);
+		break;
+
+	}
+}
+EXPORT_SYMBOL(ieee80211_rx_mgt);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
new file mode 100644
index 000000000..92001cb36
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -0,0 +1,3056 @@
+// SPDX-License-Identifier: GPL-2.0
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Few lines might be stolen from other part of the ieee80211
+ * stack. Copyright who own it's copyright
+ *
+ * WPA code stolen from the ipw2200 driver.
+ * Copyright who own it's copyright.
+ */
+#include "ieee80211.h"
+
+#include <linux/random.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/etherdevice.h>
+
+#include "dot11d.h"
+
+short ieee80211_is_54g(const struct ieee80211_network *net)
+{
+	return (net->rates_ex_len > 0) || (net->rates_len > 4);
+}
+EXPORT_SYMBOL(ieee80211_is_54g);
+
+short ieee80211_is_shortslot(const struct ieee80211_network *net)
+{
+	return net->capability & WLAN_CAPABILITY_SHORT_SLOT;
+}
+EXPORT_SYMBOL(ieee80211_is_shortslot);
+
+/* returns the total length needed for placing the RATE MFIE
+ * tag and the EXTENDED RATE MFIE tag if needed.
+ * It includes two bytes per tag for the tag itself and its len
+ */
+static unsigned int ieee80211_MFIE_rate_len(struct ieee80211_device *ieee)
+{
+	unsigned int rate_len = 0;
+
+	if (ieee->modulation & IEEE80211_CCK_MODULATION)
+		rate_len = IEEE80211_CCK_RATE_LEN + 2;
+
+	if (ieee->modulation & IEEE80211_OFDM_MODULATION)
+		rate_len += IEEE80211_OFDM_RATE_LEN + 2;
+
+	return rate_len;
+}
+
+/* place the MFIE rate, tag to the memory (double) pointer.
+ * Then it updates the pointer so that
+ * it points after the new MFIE tag added.
+ */
+static void ieee80211_MFIE_Brate(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
+		*tag++ = MFIE_TYPE_RATES;
+		*tag++ = 4;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
+	}
+
+	/* We may add an option for custom rates that specific HW might support */
+	*tag_p = tag;
+}
+
+static void ieee80211_MFIE_Grate(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
+		*tag++ = MFIE_TYPE_RATES_EX;
+		*tag++ = 8;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
+	}
+
+	/* We may add an option for custom rates that specific HW might support */
+	*tag_p = tag;
+}
+
+static void ieee80211_WMM_Info(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	*tag++ = MFIE_TYPE_GENERIC; /* 0 */
+	*tag++ = 7;
+	*tag++ = 0x00;
+	*tag++ = 0x50;
+	*tag++ = 0xf2;
+	*tag++ = 0x02;	/* 5 */
+	*tag++ = 0x00;
+	*tag++ = 0x01;
+#ifdef SUPPORT_USPD
+	if (ieee->current_network.wmm_info & 0x80)
+		*tag++ = 0x0f | MAX_SP_Len;
+	else
+		*tag++ = MAX_SP_Len;
+#else
+	*tag++ = MAX_SP_Len;
+#endif
+	*tag_p = tag;
+}
+
+#ifdef THOMAS_TURBO
+static void ieee80211_TURBO_Info(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	*tag++ = MFIE_TYPE_GENERIC; /* 0 */
+	*tag++ = 7;
+	*tag++ = 0x00;
+	*tag++ = 0xe0;
+	*tag++ = 0x4c;
+	*tag++ = 0x01;	/* 5 */
+	*tag++ = 0x02;
+	*tag++ = 0x11;
+	*tag++ = 0x00;
+
+	*tag_p = tag;
+	netdev_alert(ieee->dev, "This is enable turbo mode IE process\n");
+}
+#endif
+
+static void enqueue_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	int nh;
+
+	nh = (ieee->mgmt_queue_head + 1) % MGMT_QUEUE_NUM;
+
+/*
+ * if the queue is full but we have newer frames then
+ * just overwrites the oldest.
+ *
+ * if (nh == ieee->mgmt_queue_tail)
+ *		return -1;
+ */
+	ieee->mgmt_queue_head = nh;
+	ieee->mgmt_queue_ring[nh] = skb;
+
+	//return 0;
+}
+
+static struct sk_buff *dequeue_mgmt(struct ieee80211_device *ieee)
+{
+	struct sk_buff *ret;
+
+	if (ieee->mgmt_queue_tail == ieee->mgmt_queue_head)
+		return NULL;
+
+	ret = ieee->mgmt_queue_ring[ieee->mgmt_queue_tail];
+
+	ieee->mgmt_queue_tail =
+		(ieee->mgmt_queue_tail + 1) % MGMT_QUEUE_NUM;
+
+	return ret;
+}
+
+static void init_mgmt_queue(struct ieee80211_device *ieee)
+{
+	ieee->mgmt_queue_tail = ieee->mgmt_queue_head = 0;
+}
+
+static u8 MgntQuery_MgntFrameTxRate(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT      pHTInfo = ieee->pHTInfo;
+	u8 rate;
+
+	/* 2008/01/25 MH For broadcom, MGNT frame set as OFDM 6M. */
+	if (pHTInfo->IOTAction & HT_IOT_ACT_MGNT_USE_CCK_6M)
+		rate = 0x0c;
+	else
+		rate = ieee->basic_rate & 0x7f;
+
+	if (rate == 0) {
+		/* 2005.01.26, by rcnjko. */
+		if (ieee->mode == IEEE_A ||
+		    ieee->mode == IEEE_N_5G ||
+		    (ieee->mode == IEEE_N_24G && !pHTInfo->bCurSuppCCK))
+			rate = 0x0c;
+		else
+			rate = 0x02;
+	}
+
+	/*
+	// Data rate of ProbeReq is already decided. Annie, 2005-03-31
+	if( pMgntInfo->bScanInProgress || (pMgntInfo->bDualModeScanStep!=0) ) {
+	if(pMgntInfo->dot11CurrentWirelessMode==WIRELESS_MODE_A)
+	rate = 0x0c;
+	else
+	rate = 0x02;
+	}
+	 */
+	return rate;
+}
+
+void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl);
+
+inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct rtl_80211_hdr_3addr  *header =
+		(struct rtl_80211_hdr_3addr  *)skb->data;
+
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* called with 2nd param 0, no mgmt lock required */
+	ieee80211_sta_wakeup(ieee, 0);
+
+	tcb_desc->queue_index = MGNT_QUEUE;
+	tcb_desc->data_rate = MgntQuery_MgntFrameTxRate(ieee);
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+
+	if (single) {
+		if (ieee->queue_stop) {
+			enqueue_mgmt(ieee, skb);
+		} else {
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			/* avoid watchdog triggers */
+			netif_trans_update(ieee->dev);
+			ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
+			//dev_kfree_skb_any(skb);//edit by thomas
+		}
+
+		spin_unlock_irqrestore(&ieee->lock, flags);
+	} else {
+		spin_unlock_irqrestore(&ieee->lock, flags);
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags);
+
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* check whether the managed packet queued greater than 5 */
+		if (!ieee->check_nic_enough_desc(ieee->dev, tcb_desc->queue_index) || \
+		    (skb_queue_len(&ieee->skb_waitQ[tcb_desc->queue_index]) != 0) || \
+		    (ieee->queue_stop)) {
+			/* insert the skb packet to the management queue */
+			/* as for the completion function, it does not need
+			 * to check it any more.
+			 * */
+			printk("%s():insert to waitqueue!\n", __func__);
+			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index], skb);
+		} else {
+			ieee->softmac_hard_start_xmit(skb, ieee->dev);
+			//dev_kfree_skb_any(skb);//edit by thomas
+		}
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags);
+	}
+}
+
+static inline void
+softmac_ps_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee)
+{
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct rtl_80211_hdr_3addr  *header =
+		(struct rtl_80211_hdr_3addr  *)skb->data;
+
+	if (single) {
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* avoid watchdog triggers */
+		netif_trans_update(ieee->dev);
+		ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
+	} else {
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		ieee->softmac_hard_start_xmit(skb, ieee->dev);
+	}
+	//dev_kfree_skb_any(skb);//edit by thomas
+}
+
+static inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
+{
+	unsigned int len, rate_len;
+	u8 *tag;
+	struct sk_buff *skb;
+	struct ieee80211_probe_request *req;
+
+	len = ieee->current_network.ssid_len;
+
+	rate_len = ieee80211_MFIE_rate_len(ieee);
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len + ieee->tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	req = skb_put(skb, sizeof(struct ieee80211_probe_request));
+	req->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	req->header.duration_id = 0; /* FIXME: is this OK? */
+
+	eth_broadcast_addr(req->header.addr1);
+	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	eth_broadcast_addr(req->header.addr3);
+
+	tag = skb_put(skb, len + 2 + rate_len);
+
+	*tag++ = MFIE_TYPE_SSID;
+	*tag++ = len;
+	memcpy(tag, ieee->current_network.ssid, len);
+	tag += len;
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+	return skb;
+}
+
+struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee);
+
+static void ieee80211_send_beacon(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+	if (!ieee->ieee_up)
+		return;
+	//unsigned long flags;
+	skb = ieee80211_get_beacon_(ieee);
+
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+//	ieee->beacon_timer.expires = jiffies +
+//		(MSECS( ieee->current_network.beacon_interval -5));
+
+	//spin_lock_irqsave(&ieee->beacon_lock,flags);
+	if (ieee->beacon_txing && ieee->ieee_up) {
+//		if(!timer_pending(&ieee->beacon_timer))
+//			add_timer(&ieee->beacon_timer);
+		mod_timer(&ieee->beacon_timer,
+			  jiffies + msecs_to_jiffies(ieee->current_network.beacon_interval - 5));
+	}
+	//spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+static void ieee80211_send_beacon_cb(struct timer_list *t)
+{
+	struct ieee80211_device *ieee =
+		from_timer(ieee, t, beacon_timer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+	ieee80211_send_beacon(ieee);
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+}
+
+static void ieee80211_send_probe(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+	skb = ieee80211_probe_req(ieee);
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_probe_rq++;
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+static void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
+{
+	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)) {
+		ieee80211_send_probe(ieee);
+		ieee80211_send_probe(ieee);
+	}
+}
+
+/* this performs syncro scan blocking the caller until all channels
+ * in the allowed channel map has been checked.
+ */
+void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
+{
+	short ch = 0;
+	u8 channel_map[MAX_CHANNEL_NUMBER + 1];
+
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER + 1);
+	mutex_lock(&ieee->scan_mutex);
+
+	while (1) {
+		do {
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				goto out; /* scan completed */
+		} while (!channel_map[ch]);
+
+		/* this function can be called in two situations
+		 * 1- We have switched to ad-hoc mode and we are
+		 *    performing a complete syncro scan before conclude
+		 *    there are no interesting cell and to create a
+		 *    new one. In this case the link state is
+		 *    IEEE80211_NOLINK until we found an interesting cell.
+		 *    If so the ieee8021_new_net, called by the RX path
+		 *    will set the state to IEEE80211_LINKED, so we stop
+		 *    scanning
+		 * 2- We are linked and the root uses run iwlist scan.
+		 *    So we switch to IEEE80211_LINKED_SCANNING to remember
+		 *    that we are still logically linked (not interested in
+		 *    new network events, despite for updating the net list,
+		 *    but we are temporarily 'unlinked' as the driver shall
+		 *    not filter RX frames and the channel is changing.
+		 * So the only situation in witch are interested is to check
+		 * if the state become LINKED because of the #1 situation
+		 */
+
+		if (ieee->state == IEEE80211_LINKED)
+			goto out;
+		ieee->set_chan(ieee->dev, ch);
+		if (channel_map[ch] == 1)
+			ieee80211_send_probe_requests(ieee);
+
+		/* this prevent excessive time wait when we
+		 * need to wait for a syncro scan to end..
+		 */
+		if (ieee->state >= IEEE80211_LINKED && ieee->sync_scan_hurryup)
+			goto out;
+
+		msleep_interruptible(IEEE80211_SOFTMAC_SCAN_TIME);
+	}
+out:
+	if (ieee->state < IEEE80211_LINKED) {
+		ieee->actscanning = false;
+		mutex_unlock(&ieee->scan_mutex);
+	} else {
+		ieee->sync_scan_hurryup = 0;
+		if (IS_DOT11D_ENABLE(ieee))
+			dot11d_scan_complete(ieee);
+		mutex_unlock(&ieee->scan_mutex);
+	}
+}
+EXPORT_SYMBOL(ieee80211_softmac_scan_syncro);
+
+static void ieee80211_softmac_scan_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, softmac_scan_wq);
+	static short watchdog;
+	u8 channel_map[MAX_CHANNEL_NUMBER + 1];
+
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER + 1);
+	if (!ieee->ieee_up)
+		return;
+	mutex_lock(&ieee->scan_mutex);
+	do {
+		ieee->current_network.channel =
+			(ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
+		if (watchdog++ > MAX_CHANNEL_NUMBER) {
+		//if current channel is not in channel map, set to default channel.
+			if (!channel_map[ieee->current_network.channel]) {
+				ieee->current_network.channel = 6;
+				goto out; /* no good chans */
+			}
+		}
+	} while (!channel_map[ieee->current_network.channel]);
+	if (ieee->scanning == 0)
+		goto out;
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	if (channel_map[ieee->current_network.channel] == 1)
+		ieee80211_send_probe_requests(ieee);
+
+	schedule_delayed_work(&ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
+
+	mutex_unlock(&ieee->scan_mutex);
+	return;
+out:
+	if (IS_DOT11D_ENABLE(ieee))
+		dot11d_scan_complete(ieee);
+	ieee->actscanning = false;
+	watchdog = 0;
+	ieee->scanning = 0;
+	mutex_unlock(&ieee->scan_mutex);
+}
+
+static void ieee80211_beacons_start(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+
+	ieee->beacon_txing = 1;
+	ieee80211_send_beacon(ieee);
+
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+}
+
+static void ieee80211_beacons_stop(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+
+	ieee->beacon_txing = 0;
+
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+	del_timer_sync(&ieee->beacon_timer);
+}
+
+void ieee80211_stop_send_beacons(struct ieee80211_device *ieee)
+{
+	if (ieee->stop_send_beacons)
+		ieee->stop_send_beacons(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		ieee80211_beacons_stop(ieee);
+}
+EXPORT_SYMBOL(ieee80211_stop_send_beacons);
+
+void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
+{
+	if (ieee->start_send_beacons)
+		ieee->start_send_beacons(ieee->dev, ieee->basic_rate);
+	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		ieee80211_beacons_start(ieee);
+}
+EXPORT_SYMBOL(ieee80211_start_send_beacons);
+
+static void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
+{
+//	unsigned long flags;
+
+	//ieee->sync_scan_hurryup = 1;
+
+	mutex_lock(&ieee->scan_mutex);
+//	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->scanning == 1) {
+		ieee->scanning = 0;
+
+		cancel_delayed_work(&ieee->softmac_scan_wq);
+	}
+
+//	spin_unlock_irqrestore(&ieee->lock, flags);
+	mutex_unlock(&ieee->scan_mutex);
+}
+
+void ieee80211_stop_scan(struct ieee80211_device *ieee)
+{
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
+		ieee80211_softmac_stop_scan(ieee);
+	else
+		ieee->stop_scan(ieee->dev);
+}
+EXPORT_SYMBOL(ieee80211_stop_scan);
+
+/* called with ieee->lock held */
+static void ieee80211_start_scan(struct ieee80211_device *ieee)
+{
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (IS_COUNTRY_IE_VALID(ieee))
+			RESET_CIE_WATCHDOG(ieee);
+	}
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
+		if (ieee->scanning == 0) {
+			ieee->scanning = 1;
+			schedule_delayed_work(&ieee->softmac_scan_wq, 0);
+		}
+	} else {
+		ieee->start_scan(ieee->dev);
+	}
+}
+
+/* called with wx_mutex held */
+void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
+{
+	if (IS_DOT11D_ENABLE(ieee)) {
+		if (IS_COUNTRY_IE_VALID(ieee))
+			RESET_CIE_WATCHDOG(ieee);
+	}
+	ieee->sync_scan_hurryup = 0;
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
+		ieee80211_softmac_scan_syncro(ieee);
+	else
+		ieee->scan_syncro(ieee->dev);
+}
+EXPORT_SYMBOL(ieee80211_start_scan_syncro);
+
+static inline struct sk_buff *
+ieee80211_authentication_req(struct ieee80211_network *beacon,
+			     struct ieee80211_device *ieee, int challengelen)
+{
+	struct sk_buff *skb;
+	struct ieee80211_authentication *auth;
+	int len = sizeof(struct ieee80211_authentication) + challengelen + ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+	auth = skb_put(skb, sizeof(struct ieee80211_authentication));
+
+	if (challengelen)
+		auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH
+						     | IEEE80211_FCTL_WEP);
+	else
+		auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH);
+
+	auth->header.duration_id = cpu_to_le16(0x013a);
+
+	memcpy(auth->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(auth->header.addr3, beacon->bssid, ETH_ALEN);
+
+	//auth->algorithm = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+	if (ieee->auth_mode == 0)
+		auth->algorithm = WLAN_AUTH_OPEN;
+	else if (ieee->auth_mode == 1)
+		auth->algorithm = cpu_to_le16(WLAN_AUTH_SHARED_KEY);
+	else if (ieee->auth_mode == 2)
+		auth->algorithm = WLAN_AUTH_OPEN; /* 0x80; */
+	printk("=================>%s():auth->algorithm is %d\n", __func__, auth->algorithm);
+	auth->transaction = cpu_to_le16(ieee->associate_seq);
+	ieee->associate_seq++;
+
+	auth->status = cpu_to_le16(WLAN_STATUS_SUCCESS);
+
+	return skb;
+}
+
+static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *dest)
+{
+	u8 *tag;
+	int beacon_size;
+	struct ieee80211_probe_response *beacon_buf;
+	struct sk_buff *skb = NULL;
+	int encrypt;
+	int atim_len, erp_len;
+	struct ieee80211_crypt_data *crypt;
+
+	char *ssid = ieee->current_network.ssid;
+	int ssid_len = ieee->current_network.ssid_len;
+	int rate_len = ieee->current_network.rates_len + 2;
+	int rate_ex_len = ieee->current_network.rates_ex_len;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	u8 erpinfo_content = 0;
+
+	u8 *tmp_ht_cap_buf;
+	u8 tmp_ht_cap_len = 0;
+	u8 *tmp_ht_info_buf;
+	u8 tmp_ht_info_len = 0;
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	u8 *tmp_generic_ie_buf = NULL;
+	u8 tmp_generic_ie_len = 0;
+
+	if (rate_ex_len > 0)
+		rate_ex_len += 2;
+
+	if (ieee->current_network.capability & WLAN_CAPABILITY_IBSS)
+		atim_len = 4;
+	else
+		atim_len = 0;
+
+	if (ieee80211_is_54g(&ieee->current_network))
+		erp_len = 3;
+	else
+		erp_len = 0;
+
+	crypt = ieee->crypt[ieee->tx_keyidx];
+
+	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
+		((0 == strcmp(crypt->ops->name, "WEP") || wpa_ie_len));
+	/* HT ralated element */
+	tmp_ht_cap_buf = (u8 *)&ieee->pHTInfo->SelfHTCap;
+	tmp_ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
+	tmp_ht_info_buf = (u8 *)&ieee->pHTInfo->SelfHTInfo;
+	tmp_ht_info_len = sizeof(ieee->pHTInfo->SelfHTInfo);
+	HTConstructCapabilityElement(ieee, tmp_ht_cap_buf, &tmp_ht_cap_len, encrypt);
+	HTConstructInfoElement(ieee, tmp_ht_info_buf, &tmp_ht_info_len, encrypt);
+
+	if (pHTInfo->bRegRT2RTAggregation) {
+		tmp_generic_ie_buf = ieee->pHTInfo->szRT2RTAggBuffer;
+		tmp_generic_ie_len = sizeof(ieee->pHTInfo->szRT2RTAggBuffer);
+		HTConstructRT2RTAggElement(ieee, tmp_generic_ie_buf, &tmp_generic_ie_len);
+	}
+//	printk("===============>tmp_ht_cap_len is %d,tmp_ht_info_len is %d, tmp_generic_ie_len is %d\n",tmp_ht_cap_len,tmp_ht_info_len,tmp_generic_ie_len);
+	beacon_size = sizeof(struct ieee80211_probe_response) + 2
+		+ ssid_len
+		+ 3 //channel
+		+ rate_len
+		+ rate_ex_len
+		+ atim_len
+		+ erp_len
+		+ wpa_ie_len
+	//	+ tmp_ht_cap_len
+	//	+ tmp_ht_info_len
+	//	+ tmp_generic_ie_len
+//		+ wmm_len+2
+		+ ieee->tx_headroom;
+	skb = dev_alloc_skb(beacon_size);
+	if (!skb)
+		return NULL;
+	skb_reserve(skb, ieee->tx_headroom);
+	beacon_buf = skb_put(skb, (beacon_size - ieee->tx_headroom));
+	memcpy(beacon_buf->header.addr1, dest, ETH_ALEN);
+	memcpy(beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	beacon_buf->header.duration_id = 0; /* FIXME */
+	beacon_buf->beacon_interval =
+		cpu_to_le16(ieee->current_network.beacon_interval);
+	beacon_buf->capability =
+		cpu_to_le16(ieee->current_network.capability & WLAN_CAPABILITY_IBSS);
+	beacon_buf->capability |=
+		cpu_to_le16(ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE); /* add short preamble here */
+
+	if (ieee->short_slot && (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_SLOT))
+		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+
+	if (encrypt)
+		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	beacon_buf->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_RESP);
+	beacon_buf->info_element[0].id = MFIE_TYPE_SSID;
+	beacon_buf->info_element[0].len = ssid_len;
+
+	tag = (u8 *)beacon_buf->info_element[0].data;
+
+	memcpy(tag, ssid, ssid_len);
+
+	tag += ssid_len;
+
+	*(tag++) = MFIE_TYPE_RATES;
+	*(tag++) = rate_len - 2;
+	memcpy(tag, ieee->current_network.rates, rate_len - 2);
+	tag += rate_len - 2;
+
+	*(tag++) = MFIE_TYPE_DS_SET;
+	*(tag++) = 1;
+	*(tag++) = ieee->current_network.channel;
+
+	if (atim_len) {
+		*(tag++) = MFIE_TYPE_IBSS_SET;
+		*(tag++) = 2;
+
+		put_unaligned_le16(ieee->current_network.atim_window,
+				   tag);
+		tag += 2;
+	}
+
+	if (erp_len) {
+		*(tag++) = MFIE_TYPE_ERP;
+		*(tag++) = 1;
+		*(tag++) = erpinfo_content;
+	}
+	if (rate_ex_len) {
+		*(tag++) = MFIE_TYPE_RATES_EX;
+		*(tag++) = rate_ex_len - 2;
+		memcpy(tag, ieee->current_network.rates_ex, rate_ex_len - 2);
+		tag += rate_ex_len - 2;
+	}
+
+	if (wpa_ie_len) {
+		if (ieee->iw_mode == IW_MODE_ADHOC) {
+			//as Windows will set pairwise key same as the group key which is not allowed in Linux, so set this for IOT issue. WB 2008.07.07
+			memcpy(&ieee->wpa_ie[14], &ieee->wpa_ie[8], 4);
+		}
+		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+		tag += wpa_ie_len;
+	}
+
+	//skb->dev = ieee->dev;
+	return skb;
+}
+
+static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
+					    u8 *dest)
+{
+	struct sk_buff *skb;
+	u8 *tag;
+
+	struct ieee80211_crypt_data *crypt;
+	struct ieee80211_assoc_response_frame *assoc;
+	short encrypt;
+
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	int len = sizeof(struct ieee80211_assoc_response_frame) + rate_len + ieee->tx_headroom;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	assoc = skb_put(skb, sizeof(struct ieee80211_assoc_response_frame));
+
+	assoc->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP);
+	memcpy(assoc->header.addr1, dest, ETH_ALEN);
+	memcpy(assoc->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(assoc->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
+		WLAN_CAPABILITY_BSS : WLAN_CAPABILITY_IBSS);
+
+	if (ieee->short_slot)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+
+	if (ieee->host_encrypt)
+		crypt = ieee->crypt[ieee->tx_keyidx];
+	else
+		crypt = NULL;
+
+	encrypt = crypt && crypt->ops;
+
+	if (encrypt)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	assoc->status = 0;
+	assoc->aid = cpu_to_le16(ieee->assoc_id);
+	if (ieee->assoc_id == 0x2007)
+		ieee->assoc_id = 0;
+	else
+		ieee->assoc_id++;
+
+	tag = skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+
+	return skb;
+}
+
+static struct sk_buff *ieee80211_auth_resp(struct ieee80211_device *ieee,
+					   int status, u8 *dest)
+{
+	struct sk_buff *skb;
+	struct ieee80211_authentication *auth;
+	int len = ieee->tx_headroom + sizeof(struct ieee80211_authentication) + 1;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb->len = sizeof(struct ieee80211_authentication);
+
+	auth = (struct ieee80211_authentication *)skb->data;
+
+	auth->status = cpu_to_le16(status);
+	auth->transaction = cpu_to_le16(2);
+	auth->algorithm = cpu_to_le16(WLAN_AUTH_OPEN);
+
+	memcpy(auth->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(auth->header.addr1, dest, ETH_ALEN);
+	auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH);
+	return skb;
+}
+
+static struct sk_buff *ieee80211_null_func(struct ieee80211_device *ieee,
+					   short pwr)
+{
+	struct sk_buff *skb;
+	struct rtl_80211_hdr_3addr *hdr;
+
+	skb = dev_alloc_skb(sizeof(struct rtl_80211_hdr_3addr));
+
+	if (!skb)
+		return NULL;
+
+	hdr = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
+
+	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
+	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(hdr->addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	hdr->frame_ctl = cpu_to_le16(IEEE80211_FTYPE_DATA |
+				     IEEE80211_STYPE_NULLFUNC | IEEE80211_FCTL_TODS |
+				     (pwr ? IEEE80211_FCTL_PM : 0));
+
+	return skb;
+}
+
+static void ieee80211_resp_to_assoc_rq(struct ieee80211_device *ieee, u8 *dest)
+{
+	struct sk_buff *buf = ieee80211_assoc_resp(ieee, dest);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+static void ieee80211_resp_to_auth(struct ieee80211_device *ieee, int s,
+				   u8 *dest)
+{
+	struct sk_buff *buf = ieee80211_auth_resp(ieee, s, dest);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+static void ieee80211_resp_to_probe(struct ieee80211_device *ieee, u8 *dest)
+{
+	struct sk_buff *buf = ieee80211_probe_resp(ieee, dest);
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+static inline struct sk_buff *
+ieee80211_association_req(struct ieee80211_network *beacon,
+			  struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	//unsigned long flags;
+
+	struct ieee80211_assoc_request_frame *hdr;
+	u8 *tag;//,*rsn_ie;
+	//short info_addr = 0;
+	//int i;
+	//u16 suite_count = 0;
+	//u8 suit_select = 0;
+	//unsigned int wpa_len = beacon->wpa_ie_len;
+	//for HT
+	u8 *ht_cap_buf = NULL;
+	u8 ht_cap_len = 0;
+	u8 *realtek_ie_buf = NULL;
+	u8 realtek_ie_len = 0;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	unsigned int ckip_ie_len = 0;
+	unsigned int ccxrm_ie_len = 0;
+	unsigned int cxvernum_ie_len = 0;
+	struct ieee80211_crypt_data *crypt;
+	int encrypt;
+
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	unsigned int wmm_info_len = beacon->qos_data.supported ? 9 : 0;
+#ifdef THOMAS_TURBO
+	unsigned int turbo_info_len = beacon->Turbo_Enable ? 9 : 0;
+#endif
+
+	int len = 0;
+
+	crypt = ieee->crypt[ieee->tx_keyidx];
+	encrypt = ieee->host_encrypt && crypt && crypt->ops && ((0 == strcmp(crypt->ops->name, "WEP") || wpa_ie_len));
+
+	/* Include High Throuput capability && Realtek proprietary */
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		ht_cap_buf = (u8 *)&ieee->pHTInfo->SelfHTCap;
+		ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
+		HTConstructCapabilityElement(ieee, ht_cap_buf, &ht_cap_len, encrypt);
+		if (ieee->pHTInfo->bCurrentRT2RTAggregation) {
+			realtek_ie_buf = ieee->pHTInfo->szRT2RTAggBuffer;
+			realtek_ie_len = sizeof(ieee->pHTInfo->szRT2RTAggBuffer);
+			HTConstructRT2RTAggElement(ieee, realtek_ie_buf, &realtek_ie_len);
+		}
+	}
+	if (ieee->qos_support)
+		wmm_info_len = beacon->qos_data.supported ? 9 : 0;
+
+	if (beacon->bCkipSupported)
+		ckip_ie_len = 30 + 2;
+
+	if (beacon->bCcxRmEnable)
+		ccxrm_ie_len = 6 + 2;
+
+	if (beacon->BssCcxVerNumber >= 2)
+		cxvernum_ie_len = 5 + 2;
+
+#ifdef THOMAS_TURBO
+	len = sizeof(struct ieee80211_assoc_request_frame) + 2
+		+ beacon->ssid_len	/* essid tagged val */
+		+ rate_len	/* rates tagged val */
+		+ wpa_ie_len
+		+ wmm_info_len
+		+ turbo_info_len
+		+ ht_cap_len
+		+ realtek_ie_len
+		+ ckip_ie_len
+		+ ccxrm_ie_len
+		+ cxvernum_ie_len
+		+ ieee->tx_headroom;
+#else
+	len = sizeof(struct ieee80211_assoc_request_frame) + 2
+		+ beacon->ssid_len	/* essid tagged val */
+		+ rate_len	/* rates tagged val */
+		+ wpa_ie_len
+		+ wmm_info_len
+		+ ht_cap_len
+		+ realtek_ie_len
+		+ ckip_ie_len
+		+ ccxrm_ie_len
+		+ cxvernum_ie_len
+		+ ieee->tx_headroom;
+#endif
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, ieee->tx_headroom);
+
+	hdr = skb_put(skb, sizeof(struct ieee80211_assoc_request_frame) + 2);
+
+	hdr->header.frame_ctl = IEEE80211_STYPE_ASSOC_REQ;
+	hdr->header.duration_id = cpu_to_le16(37);
+	memcpy(hdr->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(hdr->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(hdr->header.addr3, beacon->bssid, ETH_ALEN);
+
+	memcpy(ieee->ap_mac_addr, beacon->bssid, ETH_ALEN);//for HW security, John
+
+	hdr->capability = cpu_to_le16(WLAN_CAPABILITY_BSS);
+	if (beacon->capability & WLAN_CAPABILITY_PRIVACY)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	if (beacon->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE); //add short_preamble here
+
+	if (ieee->short_slot)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+	if (wmm_info_len) //QOS
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_QOS);
+
+	hdr->listen_interval = cpu_to_le16(0xa);
+
+	hdr->info_element[0].id = MFIE_TYPE_SSID;
+
+	hdr->info_element[0].len = beacon->ssid_len;
+	skb_put_data(skb, beacon->ssid, beacon->ssid_len);
+
+	tag = skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+	// For CCX 1 S13, CKIP. Added by Annie, 2006-08-14.
+	if (beacon->bCkipSupported) {
+		static u8	AironetIeOui[] = {0x00, 0x01, 0x66}; // "4500-client"
+		u8	CcxAironetBuf[30];
+		struct octet_string	osCcxAironetIE;
+
+		memset(CcxAironetBuf, 0, 30);
+		osCcxAironetIE.octet = CcxAironetBuf;
+		osCcxAironetIE.length = sizeof(CcxAironetBuf);
+		//
+		// Ref. CCX test plan v3.61, 3.2.3.1 step 13.
+		// We want to make the device type as "4500-client". 060926, by CCW.
+		//
+		memcpy(osCcxAironetIE.octet, AironetIeOui, sizeof(AironetIeOui));
+
+		// CCX1 spec V1.13, A01.1 CKIP Negotiation (page23):
+		// "The CKIP negotiation is started with the associate request from the client to the access point,
+		//  containing an Aironet element with both the MIC and KP bits set."
+		osCcxAironetIE.octet[IE_CISCO_FLAG_POSITION] |= (SUPPORT_CKIP_PK | SUPPORT_CKIP_MIC);
+		tag = skb_put(skb, ckip_ie_len);
+		*tag++ = MFIE_TYPE_AIRONET;
+		*tag++ = osCcxAironetIE.length;
+		memcpy(tag, osCcxAironetIE.octet, osCcxAironetIE.length);
+		tag += osCcxAironetIE.length;
+	}
+
+	if (beacon->bCcxRmEnable) {
+		static u8 CcxRmCapBuf[] = {0x00, 0x40, 0x96, 0x01, 0x01, 0x00};
+		struct octet_string osCcxRmCap;
+
+		osCcxRmCap.octet = CcxRmCapBuf;
+		osCcxRmCap.length = sizeof(CcxRmCapBuf);
+		tag = skb_put(skb, ccxrm_ie_len);
+		*tag++ = MFIE_TYPE_GENERIC;
+		*tag++ = osCcxRmCap.length;
+		memcpy(tag, osCcxRmCap.octet, osCcxRmCap.length);
+		tag += osCcxRmCap.length;
+	}
+
+	if (beacon->BssCcxVerNumber >= 2) {
+		u8			CcxVerNumBuf[] = {0x00, 0x40, 0x96, 0x03, 0x00};
+		struct octet_string	osCcxVerNum;
+		CcxVerNumBuf[4] = beacon->BssCcxVerNumber;
+		osCcxVerNum.octet = CcxVerNumBuf;
+		osCcxVerNum.length = sizeof(CcxVerNumBuf);
+		tag = skb_put(skb, cxvernum_ie_len);
+		*tag++ = MFIE_TYPE_GENERIC;
+		*tag++ = osCcxVerNum.length;
+		memcpy(tag, osCcxVerNum.octet, osCcxVerNum.length);
+		tag += osCcxVerNum.length;
+	}
+	//HT cap element
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		if (ieee->pHTInfo->ePeerHTSpecVer != HT_SPEC_VER_EWC) {
+			tag = skb_put(skb, ht_cap_len);
+			*tag++ = MFIE_TYPE_HT_CAP;
+			*tag++ = ht_cap_len - 2;
+			memcpy(tag, ht_cap_buf, ht_cap_len - 2);
+			tag += ht_cap_len - 2;
+		}
+	}
+
+	//choose what wpa_supplicant gives to associate.
+	if (wpa_ie_len)
+		skb_put_data(skb, ieee->wpa_ie, wpa_ie_len);
+
+	if (wmm_info_len) {
+		tag = skb_put(skb, wmm_info_len);
+		ieee80211_WMM_Info(ieee, &tag);
+	}
+#ifdef THOMAS_TURBO
+	if (turbo_info_len) {
+		tag = skb_put(skb, turbo_info_len);
+		ieee80211_TURBO_Info(ieee, &tag);
+	}
+#endif
+
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		if (ieee->pHTInfo->ePeerHTSpecVer == HT_SPEC_VER_EWC) {
+			tag = skb_put(skb, ht_cap_len);
+			*tag++ = MFIE_TYPE_GENERIC;
+			*tag++ = ht_cap_len - 2;
+			memcpy(tag, ht_cap_buf, ht_cap_len - 2);
+			tag += ht_cap_len - 2;
+		}
+
+		if (ieee->pHTInfo->bCurrentRT2RTAggregation) {
+			tag = skb_put(skb, realtek_ie_len);
+			*tag++ = MFIE_TYPE_GENERIC;
+			*tag++ = realtek_ie_len - 2;
+			memcpy(tag, realtek_ie_buf, realtek_ie_len - 2);
+		}
+	}
+//	printk("<=====%s(), %p, %p\n", __func__, ieee->dev, ieee->dev->dev_addr);
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
+	return skb;
+}
+
+void ieee80211_associate_abort(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	ieee->associate_seq++;
+
+	/* don't scan, and avoid having the RX path possibly
+	 * try again to associate. Even do not react to AUTH or
+	 * ASSOC response. Just wait for the retry wq to be scheduled.
+	 * Here we will check if there are good nets to associate
+	 * with, so we retry or just get back to NO_LINK and scanning
+	 */
+	if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING) {
+		IEEE80211_DEBUG_MGMT("Authentication failed\n");
+		ieee->softmac_stats.no_auth_rs++;
+	} else {
+		IEEE80211_DEBUG_MGMT("Association failed\n");
+		ieee->softmac_stats.no_ass_rs++;
+	}
+
+	ieee->state = IEEE80211_ASSOCIATING_RETRY;
+
+	schedule_delayed_work(&ieee->associate_retry_wq, \
+			      IEEE80211_SOFTMAC_ASSOC_RETRY_TIME);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+static void ieee80211_associate_abort_cb(struct timer_list *t)
+{
+	struct ieee80211_device *dev = from_timer(dev, t, associate_timer);
+
+	ieee80211_associate_abort(dev);
+}
+
+static void ieee80211_associate_step1(struct ieee80211_device *ieee)
+{
+	struct ieee80211_network *beacon = &ieee->current_network;
+	struct sk_buff *skb;
+
+	IEEE80211_DEBUG_MGMT("Stopping scan\n");
+
+	ieee->softmac_stats.tx_auth_rq++;
+	skb = ieee80211_authentication_req(beacon, ieee, 0);
+
+	if (!skb) {
+		ieee80211_associate_abort(ieee);
+	} else {
+		ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATING;
+		IEEE80211_DEBUG_MGMT("Sending authentication request\n");
+		softmac_mgmt_xmit(skb, ieee);
+		//BUGON when you try to add_timer twice, using mod_timer may be better, john0709
+		if (!timer_pending(&ieee->associate_timer)) {
+			ieee->associate_timer.expires = jiffies + (HZ / 2);
+			add_timer(&ieee->associate_timer);
+		}
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+static void ieee80211_auth_challenge(struct ieee80211_device *ieee,
+				     u8 *challenge,
+				     int chlen)
+{
+	u8 *c;
+	struct sk_buff *skb;
+	struct ieee80211_network *beacon = &ieee->current_network;
+//	int hlen = sizeof(struct ieee80211_authentication);
+
+	ieee->associate_seq++;
+	ieee->softmac_stats.tx_auth_rq++;
+
+	skb = ieee80211_authentication_req(beacon, ieee, chlen + 2);
+	if (!skb) {
+		ieee80211_associate_abort(ieee);
+	} else {
+		c = skb_put(skb, chlen + 2);
+		*(c++) = MFIE_TYPE_CHALLENGE;
+		*(c++) = chlen;
+		memcpy(c, challenge, chlen);
+
+		IEEE80211_DEBUG_MGMT("Sending authentication challenge response\n");
+
+		ieee80211_encrypt_fragment(ieee, skb, sizeof(struct rtl_80211_hdr_3addr));
+
+		softmac_mgmt_xmit(skb, ieee);
+		mod_timer(&ieee->associate_timer, jiffies + (HZ / 2));
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+	kfree(challenge);
+}
+
+static void ieee80211_associate_step2(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	struct ieee80211_network *beacon = &ieee->current_network;
+
+	del_timer_sync(&ieee->associate_timer);
+
+	IEEE80211_DEBUG_MGMT("Sending association request\n");
+
+	ieee->softmac_stats.tx_ass_rq++;
+	skb = ieee80211_association_req(beacon, ieee);
+	if (!skb) {
+		ieee80211_associate_abort(ieee);
+	} else {
+		softmac_mgmt_xmit(skb, ieee);
+		mod_timer(&ieee->associate_timer, jiffies + (HZ / 2));
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+static void ieee80211_associate_complete_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_complete_wq);
+
+	netdev_info(ieee->dev, "Associated successfully\n");
+	if (ieee80211_is_54g(&ieee->current_network) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION)) {
+		ieee->rate = 108;
+		netdev_info(ieee->dev, "Using G rates:%d\n", ieee->rate);
+	} else {
+		ieee->rate = 22;
+		netdev_info(ieee->dev, "Using B rates:%d\n", ieee->rate);
+	}
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		printk("Successfully associated, ht enabled\n");
+		HTOnAssocRsp(ieee);
+	} else {
+		printk("Successfully associated, ht not enabled(%d, %d)\n", ieee->pHTInfo->bCurrentHTSupport, ieee->pHTInfo->bEnableHT);
+		memset(ieee->dot11HTOperationalRateSet, 0, 16);
+		//HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+	}
+	ieee->LinkDetectInfo.SlotNum = 2 * (1 + ieee->current_network.beacon_interval / 500);
+	// To prevent the immediately calling watch_dog after association.
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod == 0 || ieee->LinkDetectInfo.NumRecvDataInPeriod == 0) {
+		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
+		ieee->LinkDetectInfo.NumRecvDataInPeriod = 1;
+	}
+	ieee->link_change(ieee->dev);
+	if (!ieee->is_silent_reset) {
+		printk("============>normal associate\n");
+		notify_wx_assoc_event(ieee);
+	} else {
+		printk("==================>silent reset associate\n");
+		ieee->is_silent_reset = false;
+	}
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+	netif_carrier_on(ieee->dev);
+}
+
+static void ieee80211_associate_complete(struct ieee80211_device *ieee)
+{
+//	int i;
+//	struct net_device* dev = ieee->dev;
+	del_timer_sync(&ieee->associate_timer);
+
+	ieee->state = IEEE80211_LINKED;
+	//ieee->UpdateHalRATRTableHandler(dev, ieee->dot11HTOperationalRateSet);
+	schedule_work(&ieee->associate_complete_wq);
+}
+
+static void ieee80211_associate_procedure_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_procedure_wq);
+	ieee->sync_scan_hurryup = 1;
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_scan(ieee);
+	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
+	//ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+
+	ieee->associate_seq = 1;
+	ieee80211_associate_step1(ieee);
+
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee80211_network *net)
+{
+	u8 tmp_ssid[IW_ESSID_MAX_SIZE + 1];
+	int tmp_ssid_len = 0;
+
+	short apset, ssidset, ssidbroad, apmatch, ssidmatch;
+
+	/* we are interested in new only if we are not associated
+	 * and we are not associating / authenticating
+	 */
+	if (ieee->state != IEEE80211_NOLINK)
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_INFRA) && !(net->capability & WLAN_CAPABILITY_BSS))
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_ADHOC) && !(net->capability & WLAN_CAPABILITY_IBSS))
+		return;
+
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
+		/* if the user specified the AP MAC, we need also the essid
+		 * This could be obtained by beacons or, if the network does not
+		 * broadcast it, it can be put manually.
+		 */
+		apset = ieee->wap_set;//(memcmp(ieee->current_network.bssid, zero,ETH_ALEN)!=0 );
+		ssidset = ieee->ssid_set;//ieee->current_network.ssid[0] != '\0';
+		ssidbroad =  !(net->ssid_len == 0 || net->ssid[0] == '\0');
+		apmatch = (memcmp(ieee->current_network.bssid, net->bssid, ETH_ALEN) == 0);
+		ssidmatch = (ieee->current_network.ssid_len == net->ssid_len) &&
+			(!strncmp(ieee->current_network.ssid, net->ssid, net->ssid_len));
+
+		/* if the user set the AP check if match.
+		 * if the network does not broadcast essid we check the user supplyed ANY essid
+		 * if the network does broadcast and the user does not set essid it is OK
+		 * if the network does broadcast and the user did set essid check if essid match
+		 */
+		if ((apset && apmatch &&
+		     ((ssidset && ssidbroad && ssidmatch) || (ssidbroad && !ssidset) || (!ssidbroad && ssidset))) ||
+		    /* if the ap is not set, check that the user set the bssid
+		     * and the network does broadcast and that those two bssid matches
+		     */
+		    (!apset && ssidset && ssidbroad && ssidmatch)) {
+			/* if the essid is hidden replace it with the
+			 * essid provided by the user.
+			 */
+			if (!ssidbroad) {
+				strncpy(tmp_ssid, ieee->current_network.ssid, IW_ESSID_MAX_SIZE);
+				tmp_ssid_len = ieee->current_network.ssid_len;
+			}
+			memcpy(&ieee->current_network, net, sizeof(struct ieee80211_network));
+
+			strncpy(ieee->current_network.ssid, tmp_ssid, IW_ESSID_MAX_SIZE);
+			ieee->current_network.ssid_len = tmp_ssid_len;
+			netdev_info(ieee->dev,
+				    "Linking with %s,channel:%d, qos:%d, myHT:%d, networkHT:%d\n",
+				    ieee->current_network.ssid,
+				    ieee->current_network.channel,
+				    ieee->current_network.qos_data.supported,
+				    ieee->pHTInfo->bEnableHT,
+				    ieee->current_network.bssht.bdSupportHT);
+
+			//ieee->pHTInfo->IOTAction = 0;
+			HTResetIOTSetting(ieee->pHTInfo);
+			if (ieee->iw_mode == IW_MODE_INFRA) {
+				/* Join the network for the first time */
+				ieee->AsocRetryCount = 0;
+				//for HT by amy 080514
+				if ((ieee->current_network.qos_data.supported == 1) &&
+				    // (ieee->pHTInfo->bEnableHT && ieee->current_network.bssht.bdSupportHT))
+				    ieee->current_network.bssht.bdSupportHT) {
+/*WB, 2008.09.09:bCurrentHTSupport and bEnableHT two flags are going to put together to check whether we are in HT now, so needn't to check bEnableHT flags here. That's is to say we will set to HT support whenever joined AP has the ability to support HT. And whether we are in HT or not, please check bCurrentHTSupport&&bEnableHT now please.*/
+					//	ieee->pHTInfo->bCurrentHTSupport = true;
+					HTResetSelfAndSavePeerSetting(ieee, &ieee->current_network);
+				} else {
+					ieee->pHTInfo->bCurrentHTSupport = false;
+				}
+
+				ieee->state = IEEE80211_ASSOCIATING;
+				schedule_work(&ieee->associate_procedure_wq);
+			} else {
+				if (ieee80211_is_54g(&ieee->current_network) &&
+				    (ieee->modulation & IEEE80211_OFDM_MODULATION)) {
+					ieee->rate = 108;
+					ieee->SetWirelessMode(ieee->dev, IEEE_G);
+					netdev_info(ieee->dev,
+						    "Using G rates\n");
+				} else {
+					ieee->rate = 22;
+					ieee->SetWirelessMode(ieee->dev, IEEE_B);
+					netdev_info(ieee->dev,
+						    "Using B rates\n");
+				}
+				memset(ieee->dot11HTOperationalRateSet, 0, 16);
+				//HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+				ieee->state = IEEE80211_LINKED;
+			}
+		}
+	}
+}
+
+void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	struct ieee80211_network *target;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	list_for_each_entry(target, &ieee->network_list, list) {
+		/* if the state become different that NOLINK means
+		 * we had found what we are searching for
+		 */
+
+		if (ieee->state != IEEE80211_NOLINK)
+			break;
+
+		if (ieee->scan_age == 0 || time_after(target->last_scanned + ieee->scan_age, jiffies))
+			ieee80211_softmac_new_net(ieee, target);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+static inline int auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
+{
+	struct ieee80211_authentication *a;
+	u8 *t;
+	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
+		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
+		return -EINVAL;
+	}
+	*challenge = NULL;
+	a = (struct ieee80211_authentication *)skb->data;
+	if (skb->len > (sizeof(struct ieee80211_authentication) + 3)) {
+		t = skb->data + sizeof(struct ieee80211_authentication);
+
+		if (*(t++) == MFIE_TYPE_CHALLENGE) {
+			*chlen = *(t++);
+			*challenge = kmemdup(t, *chlen, GFP_ATOMIC);
+			if (!*challenge)
+				return -ENOMEM;
+		}
+	}
+
+	if (a->status) {
+		IEEE80211_DEBUG_MGMT("auth_parse() failed\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int auth_rq_parse(struct sk_buff *skb, u8 *dest)
+{
+	struct ieee80211_authentication *a;
+
+	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
+		IEEE80211_DEBUG_MGMT("invalid len in auth request: %d\n", skb->len);
+		return -1;
+	}
+	a = (struct ieee80211_authentication *)skb->data;
+
+	memcpy(dest, a->header.addr2, ETH_ALEN);
+
+	if (le16_to_cpu(a->algorithm) != WLAN_AUTH_OPEN)
+		return  WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
+
+	return WLAN_STATUS_SUCCESS;
+}
+
+static short probe_rq_parse(struct ieee80211_device *ieee, struct sk_buff *skb, u8 *src)
+{
+	u8 *tag;
+	u8 *skbend;
+	u8 *ssid = NULL;
+	u8 ssidlen = 0;
+
+	struct rtl_80211_hdr_3addr   *header =
+		(struct rtl_80211_hdr_3addr   *)skb->data;
+
+	if (skb->len < sizeof(struct rtl_80211_hdr_3addr))
+		return -1; /* corrupted */
+
+	memcpy(src, header->addr2, ETH_ALEN);
+
+	skbend = (u8 *)skb->data + skb->len;
+
+	tag = skb->data + sizeof(struct rtl_80211_hdr_3addr);
+
+	while (tag + 1 < skbend) {
+		if (*tag == 0) {
+			ssid = tag + 2;
+			ssidlen = *(tag + 1);
+			break;
+		}
+		tag++; /* point to the len field */
+		tag = tag + *(tag); /* point to the last data byte of the tag */
+		tag++; /* point to the next tag */
+	}
+
+	//IEEE80211DMESG("Card MAC address is "MACSTR, MAC2STR(src));
+	if (ssidlen == 0)
+		return 1;
+
+	if (!ssid)
+		return 1; /* ssid not found in tagged param */
+
+	return (!strncmp(ssid, ieee->current_network.ssid, ssidlen));
+}
+
+static int assoc_rq_parse(struct sk_buff *skb, u8 *dest)
+{
+	struct ieee80211_assoc_request_frame *a;
+
+	if (skb->len < (sizeof(struct ieee80211_assoc_request_frame) -
+		sizeof(struct ieee80211_info_element))) {
+		IEEE80211_DEBUG_MGMT("invalid len in auth request:%d \n", skb->len);
+		return -1;
+	}
+
+	a = (struct ieee80211_assoc_request_frame *)skb->data;
+
+	memcpy(dest, a->header.addr2, ETH_ALEN);
+
+	return 0;
+}
+
+static inline u16 assoc_parse(struct ieee80211_device *ieee, struct sk_buff *skb, int *aid)
+{
+	struct ieee80211_assoc_response_frame *response_head;
+	u16 status_code;
+
+	if (skb->len < sizeof(struct ieee80211_assoc_response_frame)) {
+		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
+		return 0xcafe;
+	}
+
+	response_head = (struct ieee80211_assoc_response_frame *)skb->data;
+	*aid = le16_to_cpu(response_head->aid) & 0x3fff;
+
+	status_code = le16_to_cpu(response_head->status);
+	if ((status_code == WLAN_STATUS_ASSOC_DENIED_RATES ||
+	     status_code == WLAN_STATUS_CAPS_UNSUPPORTED) &&
+	    ((ieee->mode == IEEE_G) &&
+	     (ieee->current_network.mode == IEEE_N_24G) &&
+	     (ieee->AsocRetryCount++ < (RT_ASOC_RETRY_LIMIT - 1)))) {
+		ieee->pHTInfo->IOTAction |= HT_IOT_ACT_PURE_N_MODE;
+	} else {
+		ieee->AsocRetryCount = 0;
+	}
+
+	return le16_to_cpu(response_head->status);
+}
+
+static inline void
+ieee80211_rx_probe_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_probe_rq++;
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+	if (probe_rq_parse(ieee, skb, dest)) {
+		//IEEE80211DMESG("Was for me!");
+		ieee->softmac_stats.tx_probe_rs++;
+		ieee80211_resp_to_probe(ieee, dest);
+	}
+}
+
+static inline void
+ieee80211_rx_auth_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+	int status;
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_auth_rq++;
+
+	status = auth_rq_parse(skb, dest);
+	if (status != -1)
+		ieee80211_resp_to_auth(ieee, status, dest);
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+}
+
+static inline void
+ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+	//unsigned long flags;
+
+	ieee->softmac_stats.rx_ass_rq++;
+	if (assoc_rq_parse(skb, dest) != -1)
+		ieee80211_resp_to_assoc_rq(ieee, dest);
+
+	netdev_info(ieee->dev, "New client associated: %pM\n", dest);
+	//FIXME
+}
+
+static void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
+					     short pwr)
+{
+	struct sk_buff *buf = ieee80211_null_func(ieee, pwr);
+
+	if (buf)
+		softmac_ps_mgmt_xmit(buf, ieee);
+}
+/* EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame); */
+
+static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
+				    u32 *time_l)
+{
+	int timeout;
+	u8 dtim;
+	/*if(ieee->ps == IEEE80211_PS_DISABLED ||
+		ieee->iw_mode != IW_MODE_INFRA ||
+		ieee->state != IEEE80211_LINKED)
+
+		return 0;
+	*/
+	dtim = ieee->current_network.dtim_data;
+	if (!(dtim & IEEE80211_DTIM_VALID))
+		return 0;
+	timeout = ieee->current_network.beacon_interval; //should we use ps_timeout value or beacon_interval
+	ieee->current_network.dtim_data = IEEE80211_DTIM_INVALID;
+
+	if (dtim & ((IEEE80211_DTIM_UCAST | IEEE80211_DTIM_MBCAST) & ieee->ps))
+		return 2;
+
+	if (!time_after(jiffies,
+			dev_trans_start(ieee->dev) + msecs_to_jiffies(timeout)))
+		return 0;
+
+	if (!time_after(jiffies,
+			ieee->last_rx_ps_time + msecs_to_jiffies(timeout)))
+		return 0;
+
+	if ((ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) &&
+	    (ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
+		return 0;
+
+	if (time_l) {
+		*time_l = ieee->current_network.last_dtim_sta_time[0]
+			+ (ieee->current_network.beacon_interval
+			   * ieee->current_network.dtim_period) * 1000;
+	}
+
+	if (time_h) {
+		*time_h = ieee->current_network.last_dtim_sta_time[1];
+		if (time_l && *time_l < ieee->current_network.last_dtim_sta_time[0])
+			*time_h += 1;
+	}
+
+	return 1;
+}
+
+static inline void ieee80211_sta_ps(struct work_struct *work)
+{
+	struct ieee80211_device *ieee;
+	u32 th, tl;
+	short sleep;
+	unsigned long flags, flags2;
+
+	ieee = container_of(work, struct ieee80211_device, ps_task);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if ((ieee->ps == IEEE80211_PS_DISABLED ||
+	     ieee->iw_mode != IW_MODE_INFRA ||
+	     ieee->state != IEEE80211_LINKED)) {
+		//	#warning CHECK_LOCK_HERE
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+		ieee80211_sta_wakeup(ieee, 1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+
+	sleep = ieee80211_sta_ps_sleep(ieee, &th, &tl);
+	/* 2 wake, 1 sleep, 0 do nothing */
+	if (sleep == 0)
+		goto out;
+
+	if (sleep == 1) {
+		if (ieee->sta_sleep == 1) {
+			ieee->enter_sleep_state(ieee->dev, th, tl);
+		} else if (ieee->sta_sleep == 0) {
+		//	printk("send null 1\n");
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+			if (ieee->ps_is_queue_empty(ieee->dev)) {
+				ieee->sta_sleep = 2;
+
+				ieee->ps_request_tx_ack(ieee->dev);
+
+				ieee80211_sta_ps_send_null_frame(ieee, 1);
+
+				ieee->ps_th = th;
+				ieee->ps_tl = tl;
+			}
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+		}
+	} else if (sleep == 2) {
+//#warning CHECK_LOCK_HERE
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+		ieee80211_sta_wakeup(ieee, 1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
+{
+	if (ieee->sta_sleep == 0) {
+		if (nl) {
+			printk("Warning: driver is probably failing to report TX ps error\n");
+			ieee->ps_request_tx_ack(ieee->dev);
+			ieee80211_sta_ps_send_null_frame(ieee, 0);
+		}
+		return;
+	}
+
+	if (ieee->sta_sleep == 1)
+		ieee->sta_wake_up(ieee->dev);
+
+	ieee->sta_sleep = 0;
+
+	if (nl) {
+		ieee->ps_request_tx_ack(ieee->dev);
+		ieee80211_sta_ps_send_null_frame(ieee, 0);
+	}
+}
+
+void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
+{
+	unsigned long flags, flags2;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->sta_sleep == 2) {
+		/* Null frame with PS bit set */
+		if (success) {
+			ieee->sta_sleep = 1;
+			ieee->enter_sleep_state(ieee->dev, ieee->ps_th, ieee->ps_tl);
+		}
+		/* if the card report not success we can't be sure the AP
+		 * has not RXed so we can't assume the AP believe us awake
+		 */
+	} else {
+		/* 21112005 - tx again null without PS bit if lost */
+		if ((ieee->sta_sleep == 0) && !success) {
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+			ieee80211_sta_ps_send_null_frame(ieee, 0);
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+EXPORT_SYMBOL(ieee80211_ps_tx_ack);
+
+static void ieee80211_process_action(struct ieee80211_device *ieee,
+				     struct sk_buff *skb)
+{
+	struct rtl_80211_hdr *header = (struct rtl_80211_hdr *)skb->data;
+	u8 *act = ieee80211_get_payload(header);
+	u8 tmp = 0;
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
+	if (!act) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "error to get payload of action frame\n");
+		return;
+	}
+	tmp = *act;
+	act++;
+	switch (tmp) {
+	case ACT_CAT_BA:
+		if (*act == ACT_ADDBAREQ)
+			ieee80211_rx_ADDBAReq(ieee, skb);
+		else if (*act == ACT_ADDBARSP)
+			ieee80211_rx_ADDBARsp(ieee, skb);
+		else if (*act == ACT_DELBA)
+			ieee80211_rx_DELBA(ieee, skb);
+		break;
+	default:
+		break;
+	}
+	return;
+}
+
+static void ieee80211_check_auth_response(struct ieee80211_device *ieee,
+					  struct sk_buff *skb)
+{
+	/* default support N mode, disable halfNmode */
+	bool bSupportNmode = true, bHalfSupportNmode = false;
+	int errcode;
+	u8 *challenge;
+	int chlen = 0;
+	u32 iotAction;
+
+	errcode = auth_parse(skb, &challenge, &chlen);
+	if (!errcode) {
+		if (ieee->open_wep || !challenge) {
+			ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
+			ieee->softmac_stats.rx_auth_rs_ok++;
+			iotAction = ieee->pHTInfo->IOTAction;
+			if (!(iotAction & HT_IOT_ACT_PURE_N_MODE)) {
+				if (!ieee->GetNmodeSupportBySecCfg(ieee->dev)) {
+					/* WEP or TKIP encryption */
+					if (IsHTHalfNmodeAPs(ieee)) {
+						bSupportNmode = true;
+						bHalfSupportNmode = true;
+					} else {
+						bSupportNmode = false;
+						bHalfSupportNmode = false;
+					}
+					netdev_dbg(ieee->dev, "SEC(%d, %d)\n",
+							bSupportNmode,
+							bHalfSupportNmode);
+				}
+			}
+			/* Dummy wirless mode setting- avoid encryption issue */
+			if (bSupportNmode) {
+				/* N mode setting */
+				ieee->SetWirelessMode(ieee->dev,
+						ieee->current_network.mode);
+			} else {
+				/* b/g mode setting - TODO */
+				ieee->SetWirelessMode(ieee->dev, IEEE_G);
+			}
+
+			if (ieee->current_network.mode == IEEE_N_24G &&
+					bHalfSupportNmode) {
+				netdev_dbg(ieee->dev, "enter half N mode\n");
+				ieee->bHalfWirelessN24GMode = true;
+			} else {
+				ieee->bHalfWirelessN24GMode = false;
+			}
+			ieee80211_associate_step2(ieee);
+		} else {
+			ieee80211_auth_challenge(ieee, challenge, chlen);
+		}
+	} else {
+		ieee->softmac_stats.rx_auth_rs_err++;
+		IEEE80211_DEBUG_MGMT("Auth response status code %d\n", errcode);
+		ieee80211_associate_abort(ieee);
+	}
+}
+
+inline int
+ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
+			   struct ieee80211_rx_stats *rx_stats, u16 type,
+			   u16 stype)
+{
+	struct rtl_80211_hdr_3addr *header = (struct rtl_80211_hdr_3addr *)skb->data;
+	u16 errcode;
+	int aid;
+	struct ieee80211_assoc_response_frame *assoc_resp;
+//	struct ieee80211_info_element *info_element;
+
+	if (!ieee->proto_started)
+		return 0;
+
+	if (ieee->sta_sleep || (ieee->ps != IEEE80211_PS_DISABLED &&
+				ieee->iw_mode == IW_MODE_INFRA &&
+				ieee->state == IEEE80211_LINKED))
+		schedule_work(&ieee->ps_task);
+
+	if (WLAN_FC_GET_STYPE(header->frame_ctl) != IEEE80211_STYPE_PROBE_RESP &&
+	    WLAN_FC_GET_STYPE(header->frame_ctl) != IEEE80211_STYPE_BEACON)
+		ieee->last_rx_ps_time = jiffies;
+
+	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+	case IEEE80211_STYPE_ASSOC_RESP:
+	case IEEE80211_STYPE_REASSOC_RESP:
+		IEEE80211_DEBUG_MGMT("received [RE]ASSOCIATION RESPONSE (%d)\n",
+				WLAN_FC_GET_STYPE(header->frame_ctl));
+		if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+		    ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATED &&
+		    ieee->iw_mode == IW_MODE_INFRA) {
+			struct ieee80211_network *network;
+
+			network = kzalloc(sizeof(*network), GFP_KERNEL);
+			if (!network)
+				return -ENOMEM;
+
+			errcode = assoc_parse(ieee, skb, &aid);
+			if (!errcode) {
+				ieee->state = IEEE80211_LINKED;
+				ieee->assoc_id = aid;
+				ieee->softmac_stats.rx_ass_ok++;
+				/* station support qos */
+				/* Let the register setting defaultly with Legacy station */
+				if (ieee->qos_support) {
+					assoc_resp = (struct ieee80211_assoc_response_frame *)skb->data;
+					if (ieee80211_parse_info_param(ieee, assoc_resp->info_element,\
+								       rx_stats->len - sizeof(*assoc_resp), \
+								       network, rx_stats)) {
+						return 1;
+					} else {
+						//filling the PeerHTCap. //maybe not necessary as we can get its info from current_network.
+						memcpy(ieee->pHTInfo->PeerHTCapBuf, network->bssht.bdHTCapBuf, network->bssht.bdHTCapLen);
+						memcpy(ieee->pHTInfo->PeerHTInfoBuf, network->bssht.bdHTInfoBuf, network->bssht.bdHTInfoLen);
+					}
+					if (ieee->handle_assoc_response)
+						ieee->handle_assoc_response(ieee->dev, (struct ieee80211_assoc_response_frame *)header, network);
+				}
+				ieee80211_associate_complete(ieee);
+			} else {
+				/* aid could not been allocated */
+				ieee->softmac_stats.rx_ass_err++;
+				printk("Association response status code 0x%x\n",
+				       errcode);
+				IEEE80211_DEBUG_MGMT("Association response status code 0x%x\n",
+						     errcode);
+				if (ieee->AsocRetryCount < RT_ASOC_RETRY_LIMIT)
+					schedule_work(&ieee->associate_procedure_wq);
+				else
+					ieee80211_associate_abort(ieee);
+			}
+			kfree(network);
+		}
+		break;
+
+	case IEEE80211_STYPE_ASSOC_REQ:
+	case IEEE80211_STYPE_REASSOC_REQ:
+		if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+		    ieee->iw_mode == IW_MODE_MASTER)
+			ieee80211_rx_assoc_rq(ieee, skb);
+		break;
+
+	case IEEE80211_STYPE_AUTH:
+		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
+			if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING
+			    && ieee->iw_mode == IW_MODE_INFRA) {
+				IEEE80211_DEBUG_MGMT("Received auth response");
+				ieee80211_check_auth_response(ieee, skb);
+			} else if (ieee->iw_mode == IW_MODE_MASTER) {
+				ieee80211_rx_auth_rq(ieee, skb);
+			}
+		}
+		break;
+
+	case IEEE80211_STYPE_PROBE_REQ:
+		if ((ieee->softmac_features & IEEE_SOFTMAC_PROBERS) &&
+		    ((ieee->iw_mode == IW_MODE_ADHOC ||
+		      ieee->iw_mode == IW_MODE_MASTER) &&
+		     ieee->state == IEEE80211_LINKED)) {
+			ieee80211_rx_probe_rq(ieee, skb);
+		}
+		break;
+
+	case IEEE80211_STYPE_DISASSOC:
+	case IEEE80211_STYPE_DEAUTH:
+		/* FIXME for now repeat all the association procedure
+		* both for disassociation and deauthentication
+		*/
+		if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+		    ieee->state == IEEE80211_LINKED &&
+		    ieee->iw_mode == IW_MODE_INFRA) {
+			ieee->state = IEEE80211_ASSOCIATING;
+			ieee->softmac_stats.reassoc++;
+
+			notify_wx_assoc_event(ieee);
+			//HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+			RemovePeerTS(ieee, header->addr2);
+			schedule_work(&ieee->associate_procedure_wq);
+		}
+		break;
+	case IEEE80211_STYPE_MANAGE_ACT:
+		ieee80211_process_action(ieee, skb);
+		break;
+	default:
+		return -1;
+	}
+
+	//dev_kfree_skb_any(skb);
+	return 0;
+}
+
+/* The following are for a simpler TX queue management.
+ * Instead of using netif_[stop/wake]_queue, the driver
+ * will use these two functions (plus a reset one) that
+ * will internally call the kernel netif_* and take care
+ * of the ieee802.11 fragmentation.
+ * So, the driver receives a fragment at a time and might
+ * call the stop function when it wants, without taking
+ * care to have enough room to TX an entire packet.
+ * This might be useful if each fragment needs its own
+ * descriptor. Thus, just keeping a total free memory > than
+ * the max fragmentation threshold is not enough. If the
+ * ieee802.11 stack passed a TXB struct, then you would need
+ * to keep N free descriptors where
+ * N = MAX_PACKET_SIZE / MIN_FRAG_THRESHOLD.
+ * In this way you need just one and the 802.11 stack
+ * will take care of buffering fragments and pass them to
+ * the driver later, when it wakes the queue.
+ */
+void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *ieee)
+{
+	unsigned int queue_index = txb->queue_index;
+	unsigned long flags;
+	int  i;
+	struct cb_desc *tcb_desc = NULL;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* called with 2nd parm 0, no tx mgmt lock required */
+	ieee80211_sta_wakeup(ieee, 0);
+
+	/* update the tx status */
+	ieee->stats.tx_bytes += le16_to_cpu(txb->payload_size);
+	ieee->stats.tx_packets++;
+	tcb_desc = (struct cb_desc *)(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
+	if (tcb_desc->bMulticast)
+		ieee->stats.multicast++;
+
+	/* if xmit available, just xmit it immediately, else just insert it to the wait queue */
+	for (i = 0; i < txb->nr_frags; i++) {
+#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
+		if ((skb_queue_len(&ieee->skb_drv_aggQ[queue_index]) != 0) ||
+#else
+		if ((skb_queue_len(&ieee->skb_waitQ[queue_index]) != 0) ||
+#endif
+		    (!ieee->check_nic_enough_desc(ieee->dev, queue_index)) ||
+		    (ieee->queue_stop)) {
+			/* insert the skb packet to the wait queue */
+			/* as for the completion function, it does not need
+			 * to check it any more.
+			 * */
+			//printk("error:no descriptor left@queue_index %d\n", queue_index);
+			//ieee80211_stop_queue(ieee);
+#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
+			skb_queue_tail(&ieee->skb_drv_aggQ[queue_index], txb->fragments[i]);
+#else
+			skb_queue_tail(&ieee->skb_waitQ[queue_index], txb->fragments[i]);
+#endif
+		} else {
+			ieee->softmac_data_hard_start_xmit(txb->fragments[i],
+							   ieee->dev, ieee->rate);
+			//ieee->stats.tx_packets++;
+			//ieee->stats.tx_bytes += txb->fragments[i]->len;
+			//ieee->dev->trans_start = jiffies;
+		}
+	}
+	ieee80211_txb_free(txb);
+
+//exit:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+EXPORT_SYMBOL(ieee80211_softmac_xmit);
+
+/* called with ieee->lock acquired */
+static void ieee80211_resume_tx(struct ieee80211_device *ieee)
+{
+	int i;
+	for (i = ieee->tx_pending.frag; i < ieee->tx_pending.txb->nr_frags; i++) {
+		if (ieee->queue_stop) {
+			ieee->tx_pending.frag = i;
+			return;
+		} else {
+			ieee->softmac_data_hard_start_xmit(ieee->tx_pending.txb->fragments[i],
+							   ieee->dev, ieee->rate);
+			//(i+1)<ieee->tx_pending.txb->nr_frags);
+			ieee->stats.tx_packets++;
+			netif_trans_update(ieee->dev);
+		}
+	}
+
+	ieee80211_txb_free(ieee->tx_pending.txb);
+	ieee->tx_pending.txb = NULL;
+}
+
+void ieee80211_reset_queue(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	init_mgmt_queue(ieee);
+	if (ieee->tx_pending.txb) {
+		ieee80211_txb_free(ieee->tx_pending.txb);
+		ieee->tx_pending.txb = NULL;
+	}
+	ieee->queue_stop = 0;
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+EXPORT_SYMBOL(ieee80211_reset_queue);
+
+void ieee80211_wake_queue(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	struct sk_buff *skb;
+	struct rtl_80211_hdr_3addr  *header;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	if (!ieee->queue_stop)
+		goto exit;
+
+	ieee->queue_stop = 0;
+
+	if (ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) {
+		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))) {
+			header = (struct rtl_80211_hdr_3addr  *)skb->data;
+
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
+			//dev_kfree_skb_any(skb);//edit by thomas
+		}
+	}
+	if (!ieee->queue_stop && ieee->tx_pending.txb)
+		ieee80211_resume_tx(ieee);
+
+	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)) {
+		ieee->softmac_stats.swtxawake++;
+		netif_wake_queue(ieee->dev);
+	}
+exit:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+EXPORT_SYMBOL(ieee80211_wake_queue);
+
+void ieee80211_stop_queue(struct ieee80211_device *ieee)
+{
+	//unsigned long flags;
+	//spin_lock_irqsave(&ieee->lock,flags);
+
+	if (!netif_queue_stopped(ieee->dev)) {
+		netif_stop_queue(ieee->dev);
+		ieee->softmac_stats.swtxstop++;
+	}
+	ieee->queue_stop = 1;
+	//spin_unlock_irqrestore(&ieee->lock,flags);
+}
+EXPORT_SYMBOL(ieee80211_stop_queue);
+
+/* called in user context only */
+void ieee80211_start_master_bss(struct ieee80211_device *ieee)
+{
+	ieee->assoc_id = 1;
+
+	if (ieee->current_network.ssid_len == 0) {
+		strncpy(ieee->current_network.ssid,
+			IEEE80211_DEFAULT_TX_ESSID,
+			IW_ESSID_MAX_SIZE);
+
+		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	memcpy(ieee->current_network.bssid, ieee->dev->dev_addr, ETH_ALEN);
+
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	ieee->state = IEEE80211_LINKED;
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	netif_carrier_on(ieee->dev);
+}
+
+static void ieee80211_start_monitor_mode(struct ieee80211_device *ieee)
+{
+	if (ieee->raw_tx) {
+		if (ieee->data_hard_resume)
+			ieee->data_hard_resume(ieee->dev);
+
+		netif_carrier_on(ieee->dev);
+	}
+}
+static void ieee80211_start_ibss_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, start_ibss_wq);
+	/* iwconfig mode ad-hoc will schedule this and return
+	 * on the other hand this will block further iwconfig SET
+	 * operations because of the wx_mutex hold.
+	 * Anyway some most set operations set a flag to speed-up
+	 * (abort) this wq (when syncro scanning) before sleeping
+	 * on the semaphore
+	 */
+	if (!ieee->proto_started) {
+		printk("==========oh driver down return\n");
+		return;
+	}
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->current_network.ssid_len == 0) {
+		strscpy(ieee->current_network.ssid, IEEE80211_DEFAULT_TX_ESSID,
+			sizeof(ieee->current_network.ssid));
+		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	/* check if we have this cell in our network list */
+	ieee80211_softmac_check_all_nets(ieee);
+
+//	if((IS_DOT11D_ENABLE(ieee)) && (ieee->state == IEEE80211_NOLINK))
+	if (ieee->state == IEEE80211_NOLINK)
+		ieee->current_network.channel = 6;
+	/* if not then the state is not linked. Maybe the user switched to
+	 * ad-hoc mode just after being in monitor mode, or just after
+	 * being very few time in managed mode (so the card have had no
+	 * time to scan all the chans..) or we have just run up the iface
+	 * after setting ad-hoc mode. So we have to give another try..
+	 * Here, in ibss mode, should be safe to do this without extra care
+	 * (in bss mode we had to make sure no-one tried to associate when
+	 * we had just checked the ieee->state and we was going to start the
+	 * scan) because in ibss mode the ieee80211_new_net function, when
+	 * finds a good net, just set the ieee->state to IEEE80211_LINKED,
+	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
+	 * scan, that will stop at the first round because it sees the state
+	 * associated.
+	 */
+	if (ieee->state == IEEE80211_NOLINK)
+		ieee80211_start_scan_syncro(ieee);
+
+	/* the network definitively is not here.. create a new cell */
+	if (ieee->state == IEEE80211_NOLINK) {
+		printk("creating new IBSS cell\n");
+		if (!ieee->wap_set)
+			eth_random_addr(ieee->current_network.bssid);
+
+		if (ieee->modulation & IEEE80211_CCK_MODULATION) {
+			ieee->current_network.rates_len = 4;
+
+			ieee->current_network.rates[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
+			ieee->current_network.rates[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
+			ieee->current_network.rates[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
+			ieee->current_network.rates[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
+		} else {
+			ieee->current_network.rates_len = 0;
+		}
+		if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
+			ieee->current_network.rates_ex_len = 8;
+
+			ieee->current_network.rates_ex[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
+			ieee->current_network.rates_ex[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
+			ieee->current_network.rates_ex[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
+			ieee->current_network.rates_ex[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
+			ieee->current_network.rates_ex[4] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
+			ieee->current_network.rates_ex[5] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
+			ieee->current_network.rates_ex[6] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
+			ieee->current_network.rates_ex[7] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
+
+			ieee->rate = 108;
+		} else {
+			ieee->current_network.rates_ex_len = 0;
+			ieee->rate = 22;
+		}
+
+		// By default, WMM function will be disabled in IBSS mode
+		ieee->current_network.QoS_Enable = 0;
+		ieee->SetWirelessMode(ieee->dev, IEEE_G);
+		ieee->current_network.atim_window = 0;
+		ieee->current_network.capability = WLAN_CAPABILITY_IBSS;
+		if (ieee->short_slot)
+			ieee->current_network.capability |= WLAN_CAPABILITY_SHORT_SLOT;
+	}
+
+	ieee->state = IEEE80211_LINKED;
+
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	ieee->link_change(ieee->dev);
+
+	notify_wx_assoc_event(ieee);
+
+	ieee80211_start_send_beacons(ieee);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+	netif_carrier_on(ieee->dev);
+
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+inline void ieee80211_start_ibss(struct ieee80211_device *ieee)
+{
+	schedule_delayed_work(&ieee->start_ibss_wq, 150);
+}
+
+/* this is called only in user context, with wx_mutex held */
+void ieee80211_start_bss(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	//
+	// Ref: 802.11d 11.1.3.3
+	// STA shall not start a BSS unless properly formed Beacon frame including a Country IE.
+	//
+	if (IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee)) {
+		if (!ieee->bGlobalDomain)
+			return;
+	}
+	/* check if we have already found the net we
+	 * are interested in (if any).
+	 * if not (we are disassociated and we are not
+	 * in associating / authenticating phase) start the background scanning.
+	 */
+	ieee80211_softmac_check_all_nets(ieee);
+
+	/* ensure no-one start an associating process (thus setting
+	 * the ieee->state to ieee80211_ASSOCIATING) while we
+	 * have just checked it and we are going to enable scan.
+	 * The ieee80211_new_net function is always called with
+	 * lock held (from both ieee80211_softmac_check_all_nets and
+	 * the rx path), so we cannot be in the middle of such function
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state == IEEE80211_NOLINK) {
+		ieee->actscanning = true;
+		ieee80211_start_scan(ieee);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+/* called only in userspace context */
+void ieee80211_disassociate(struct ieee80211_device *ieee)
+{
+	netif_carrier_off(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)
+		ieee80211_reset_queue(ieee);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+	if (IS_DOT11D_ENABLE(ieee))
+		dot11d_reset(ieee);
+	ieee->state = IEEE80211_NOLINK;
+	ieee->is_set_key = false;
+	ieee->link_change(ieee->dev);
+	//HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+	notify_wx_assoc_event(ieee);
+}
+EXPORT_SYMBOL(ieee80211_disassociate);
+
+static void ieee80211_associate_retry_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, associate_retry_wq);
+	unsigned long flags;
+
+	mutex_lock(&ieee->wx_mutex);
+	if (!ieee->proto_started)
+		goto exit;
+
+	if (ieee->state != IEEE80211_ASSOCIATING_RETRY)
+		goto exit;
+
+	/* until we do not set the state to IEEE80211_NOLINK
+	* there are no possibility to have someone else trying
+	* to start an association procedure (we get here with
+	* ieee->state = IEEE80211_ASSOCIATING).
+	* When we set the state to IEEE80211_NOLINK it is possible
+	* that the RX path run an attempt to associate, but
+	* both ieee80211_softmac_check_all_nets and the
+	* RX path works with ieee->lock held so there are no
+	* problems. If we are still disassociated then start a scan.
+	* the lock here is necessary to ensure no one try to start
+	* an association procedure when we have just checked the
+	* state and we are going to start the scan.
+	*/
+	ieee->state = IEEE80211_NOLINK;
+
+	ieee80211_softmac_check_all_nets(ieee);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state == IEEE80211_NOLINK)
+		ieee80211_start_scan(ieee);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee)
+{
+	u8 broadcast_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	struct sk_buff *skb;
+	struct ieee80211_probe_response *b;
+
+	skb = ieee80211_probe_resp(ieee, broadcast_addr);
+
+	if (!skb)
+		return NULL;
+
+	b = (struct ieee80211_probe_response *)skb->data;
+	b->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_BEACON);
+
+	return skb;
+}
+
+struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	struct ieee80211_probe_response *b;
+
+	skb = ieee80211_get_beacon_(ieee);
+	if (!skb)
+		return NULL;
+
+	b = (struct ieee80211_probe_response *)skb->data;
+	b->header.seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+
+	return skb;
+}
+EXPORT_SYMBOL(ieee80211_get_beacon);
+
+void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
+{
+	ieee->sync_scan_hurryup = 1;
+	mutex_lock(&ieee->wx_mutex);
+	ieee80211_stop_protocol(ieee);
+	mutex_unlock(&ieee->wx_mutex);
+}
+EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
+
+void ieee80211_stop_protocol(struct ieee80211_device *ieee)
+{
+	if (!ieee->proto_started)
+		return;
+
+	ieee->proto_started = 0;
+
+	ieee80211_stop_send_beacons(ieee);
+	del_timer_sync(&ieee->associate_timer);
+	cancel_delayed_work(&ieee->associate_retry_wq);
+	cancel_delayed_work(&ieee->start_ibss_wq);
+	ieee80211_stop_scan(ieee);
+
+	ieee80211_disassociate(ieee);
+	RemoveAllTS(ieee); //added as we disconnect from the previous BSS, Remove all TS
+}
+
+void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee)
+{
+	ieee->sync_scan_hurryup = 0;
+	mutex_lock(&ieee->wx_mutex);
+	ieee80211_start_protocol(ieee);
+	mutex_unlock(&ieee->wx_mutex);
+}
+EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
+
+void ieee80211_start_protocol(struct ieee80211_device *ieee)
+{
+	short ch = 0;
+	int i = 0;
+
+	if (ieee->proto_started)
+		return;
+
+	ieee->proto_started = 1;
+
+	if (ieee->current_network.channel == 0) {
+		do {
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				return; /* no channel found */
+		} while (!GET_DOT11D_INFO(ieee)->channel_map[ch]);
+		ieee->current_network.channel = ch;
+	}
+
+	if (ieee->current_network.beacon_interval == 0)
+		ieee->current_network.beacon_interval = 100;
+//	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
+//	ieee->set_chan(ieee->dev,ieee->current_network.channel);
+
+	for (i = 0; i < 17; i++) {
+		ieee->last_rxseq_num[i] = -1;
+		ieee->last_rxfrag_num[i] = -1;
+		ieee->last_packet_time[i] = 0;
+	}
+
+	ieee->init_wmmparam_flag = 0;//reinitialize AC_xx_PARAM registers.
+
+	/* if the user set the MAC of the ad-hoc cell and then
+	 * switch to managed mode, shall we  make sure that association
+	 * attempts does not fail just because the user provide the essid
+	 * and the nic is still checking for the AP MAC ??
+	 */
+	if (ieee->iw_mode == IW_MODE_INFRA)
+		ieee80211_start_bss(ieee);
+
+	else if (ieee->iw_mode == IW_MODE_ADHOC)
+		ieee80211_start_ibss(ieee);
+
+	else if (ieee->iw_mode == IW_MODE_MASTER)
+		ieee80211_start_master_bss(ieee);
+
+	else if (ieee->iw_mode == IW_MODE_MONITOR)
+		ieee80211_start_monitor_mode(ieee);
+}
+
+#define DRV_NAME  "Ieee80211"
+void ieee80211_softmac_init(struct ieee80211_device *ieee)
+{
+	int i;
+	memset(&ieee->current_network, 0, sizeof(struct ieee80211_network));
+
+	ieee->state = IEEE80211_NOLINK;
+	ieee->sync_scan_hurryup = 0;
+	for (i = 0; i < 5; i++)
+		ieee->seq_ctrl[i] = 0;
+
+	ieee->dot11d_info = kzalloc(sizeof(struct rt_dot11d_info), GFP_KERNEL);
+	if (!ieee->dot11d_info)
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc memory for DOT11D\n");
+	//added for  AP roaming
+	ieee->LinkDetectInfo.SlotNum = 2;
+	ieee->LinkDetectInfo.NumRecvBcnInPeriod = 0;
+	ieee->LinkDetectInfo.NumRecvDataInPeriod = 0;
+
+	ieee->assoc_id = 0;
+	ieee->queue_stop = 0;
+	ieee->scanning = 0;
+	ieee->softmac_features = 0; //so IEEE2100-like driver are happy
+	ieee->wap_set = 0;
+	ieee->ssid_set = 0;
+	ieee->proto_started = 0;
+	ieee->basic_rate = IEEE80211_DEFAULT_BASIC_RATE;
+	ieee->rate = 22;
+	ieee->ps = IEEE80211_PS_DISABLED;
+	ieee->sta_sleep = 0;
+	ieee->Regdot11HTOperationalRateSet[0] = 0xff;//support MCS 0~7
+	ieee->Regdot11HTOperationalRateSet[1] = 0xff;//support MCS 8~15
+	ieee->Regdot11HTOperationalRateSet[4] = 0x01;
+	//added by amy
+	ieee->actscanning = false;
+	ieee->beinretry = false;
+	ieee->is_set_key = false;
+	init_mgmt_queue(ieee);
+
+	ieee->sta_edca_param[0] = 0x0000A403;
+	ieee->sta_edca_param[1] = 0x0000A427;
+	ieee->sta_edca_param[2] = 0x005E4342;
+	ieee->sta_edca_param[3] = 0x002F3262;
+	ieee->aggregation = true;
+	ieee->enable_rx_imm_BA = true;
+	ieee->tx_pending.txb = NULL;
+
+	timer_setup(&ieee->associate_timer, ieee80211_associate_abort_cb, 0);
+
+	timer_setup(&ieee->beacon_timer, ieee80211_send_beacon_cb, 0);
+
+	INIT_DELAYED_WORK(&ieee->start_ibss_wq, ieee80211_start_ibss_wq);
+	INIT_WORK(&ieee->associate_complete_wq, ieee80211_associate_complete_wq);
+	INIT_WORK(&ieee->associate_procedure_wq, ieee80211_associate_procedure_wq);
+	INIT_DELAYED_WORK(&ieee->softmac_scan_wq, ieee80211_softmac_scan_wq);
+	INIT_DELAYED_WORK(&ieee->associate_retry_wq, ieee80211_associate_retry_wq);
+	INIT_WORK(&ieee->wx_sync_scan_wq, ieee80211_wx_sync_scan_wq);
+
+	mutex_init(&ieee->wx_mutex);
+	mutex_init(&ieee->scan_mutex);
+
+	spin_lock_init(&ieee->mgmt_tx_lock);
+	spin_lock_init(&ieee->beacon_lock);
+
+	INIT_WORK(&ieee->ps_task, ieee80211_sta_ps);
+}
+
+void ieee80211_softmac_free(struct ieee80211_device *ieee)
+{
+	mutex_lock(&ieee->wx_mutex);
+	kfree(ieee->dot11d_info);
+	ieee->dot11d_info = NULL;
+	del_timer_sync(&ieee->associate_timer);
+
+	cancel_delayed_work(&ieee->associate_retry_wq);
+	cancel_work_sync(&ieee->ps_task);
+	mutex_unlock(&ieee->wx_mutex);
+}
+
+/********************************************************
+ * Start of WPA code.                                   *
+ * this is stolen from the ipw2200 driver               *
+ ********************************************************/
+static int ieee80211_wpa_enable(struct ieee80211_device *ieee, int value)
+{
+	/* This is called when wpa_supplicant loads and closes the driver
+	 * interface. */
+	printk("%s WPA\n", value ? "enabling" : "disabling");
+	ieee->wpa_enabled = value;
+	return 0;
+}
+
+static void ieee80211_wpa_assoc_frame(struct ieee80211_device *ieee,
+				      char *wpa_ie, int wpa_ie_len)
+{
+	/* make sure WPA is enabled */
+	ieee80211_wpa_enable(ieee, 1);
+
+	ieee80211_disassociate(ieee);
+}
+
+static int ieee80211_wpa_mlme(struct ieee80211_device *ieee, int command, int reason)
+{
+	int ret = 0;
+
+	switch (command) {
+	case IEEE_MLME_STA_DEAUTH:
+		// silently ignore
+		break;
+
+	case IEEE_MLME_STA_DISASSOC:
+		ieee80211_disassociate(ieee);
+		break;
+
+	default:
+		printk("Unknown MLME request: %d\n", command);
+		ret = -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+static int ieee80211_wpa_set_wpa_ie(struct ieee80211_device *ieee,
+			      struct ieee_param *param, int plen)
+{
+	u8 *buf;
+
+	if (param->u.wpa_ie.len > MAX_WPA_IE_LEN)
+		return -EINVAL;
+
+	if (param->u.wpa_ie.len) {
+		buf = kmemdup(param->u.wpa_ie.data, param->u.wpa_ie.len,
+			      GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = param->u.wpa_ie.len;
+	} else {
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+
+	ieee80211_wpa_assoc_frame(ieee, ieee->wpa_ie, ieee->wpa_ie_len);
+	return 0;
+}
+
+#define AUTH_ALG_OPEN_SYSTEM			0x1
+#define AUTH_ALG_SHARED_KEY			0x2
+
+static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
+{
+	struct ieee80211_security sec = {
+		.flags = SEC_AUTH_MODE,
+	};
+
+	if (value & AUTH_ALG_SHARED_KEY) {
+		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
+		ieee->open_wep = 0;
+		ieee->auth_mode = 1;
+	} else if (value & AUTH_ALG_OPEN_SYSTEM) {
+		sec.auth_mode = WLAN_AUTH_OPEN;
+		ieee->open_wep = 1;
+		ieee->auth_mode = 0;
+	} else if (value & IW_AUTH_ALG_LEAP) {
+		sec.auth_mode = WLAN_AUTH_LEAP;
+		ieee->open_wep = 1;
+		ieee->auth_mode = 2;
+	}
+
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+	//else
+	//	ret = -EOPNOTSUPP;
+
+	return 0;
+}
+
+static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name, u32 value)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	switch (name) {
+	case IEEE_PARAM_WPA_ENABLED:
+		ret = ieee80211_wpa_enable(ieee, value);
+		break;
+
+	case IEEE_PARAM_TKIP_COUNTERMEASURES:
+		ieee->tkip_countermeasures = value;
+		break;
+
+	case IEEE_PARAM_DROP_UNENCRYPTED: {
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+		struct ieee80211_security sec = {
+			.flags = SEC_ENABLED,
+			.enabled = value,
+		};
+		ieee->drop_unencrypted = value;
+		/* We only change SEC_LEVEL for open mode. Others
+		 * are set by ipw_wpa_set_encryption.
+		 */
+		if (!value) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_0;
+		} else {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_1;
+		}
+		if (ieee->set_security)
+			ieee->set_security(ieee->dev, &sec);
+		break;
+	}
+
+	case IEEE_PARAM_PRIVACY_INVOKED:
+		ieee->privacy_invoked = value;
+		break;
+
+	case IEEE_PARAM_AUTH_ALGS:
+		ret = ieee80211_wpa_set_auth_algs(ieee, value);
+		break;
+
+	case IEEE_PARAM_IEEE_802_1X:
+		ieee->ieee802_1x = value;
+		break;
+	case IEEE_PARAM_WPAX_SELECT:
+		// added for WPA2 mixed mode
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock, flags);
+		ieee->wpax_type_set = 1;
+		ieee->wpax_type_notify = value;
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock, flags);
+		break;
+
+	default:
+		printk("Unknown WPA param: %d\n", name);
+		ret = -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+/* implementation borrowed from hostap driver */
+static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
+				  struct ieee_param *param, int param_len)
+{
+	int ret = 0;
+	const char *module = NULL;
+
+	struct ieee80211_crypto_ops *ops = NULL;
+	struct ieee80211_crypt_data **crypt;
+
+	struct ieee80211_security sec = {
+		.flags = 0,
+	};
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len !=
+	    (int)((char *)param->u.crypt.key - (char *)param) +
+	    param->u.crypt.key_len) {
+		printk("Len mismatch %d, %d\n", param_len,
+			       param->u.crypt.key_len);
+		return -EINVAL;
+	}
+	if (is_broadcast_ether_addr(param->sta_addr)) {
+		if (param->u.crypt.idx >= WEP_KEYS)
+			return -EINVAL;
+		crypt = &ieee->crypt[param->u.crypt.idx];
+	} else {
+		return -EINVAL;
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0) {
+		if (crypt) {
+			sec.enabled = 0;
+			// FIXME FIXME
+			//sec.encrypt = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_ENABLED | SEC_LEVEL;
+			ieee80211_crypt_delayed_deinit(ieee, crypt);
+		}
+		goto done;
+	}
+	sec.enabled = 1;
+// FIXME FIXME
+//	sec.encrypt = 1;
+	sec.flags |= SEC_ENABLED;
+
+	/* IPW HW cannot build TKIP MIC, host decryption still needed. */
+	if (!(ieee->host_encrypt || ieee->host_decrypt) &&
+	    strcmp(param->u.crypt.alg, "TKIP"))
+		goto skip_host_crypt;
+
+	//set WEP40 first, it will be modified according to WEP104 or WEP40 at other place
+	if (!strcmp(param->u.crypt.alg, "WEP"))
+		module = "ieee80211_crypt_wep";
+	else if (!strcmp(param->u.crypt.alg, "TKIP"))
+		module = "ieee80211_crypt_tkip";
+	else if (!strcmp(param->u.crypt.alg, "CCMP"))
+		module = "ieee80211_crypt_ccmp";
+	if (module)
+		ops = try_then_request_module(ieee80211_get_crypto_ops(param->u.crypt.alg),
+					      module);
+	if (!ops) {
+		printk("unknown crypto alg '%s'\n", param->u.crypt.alg);
+		param->u.crypt.err = IEEE_CRYPT_ERR_UNKNOWN_ALG;
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if (!*crypt || (*crypt)->ops != ops) {
+		struct ieee80211_crypt_data *new_crypt;
+
+		ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
+		if (!new_crypt) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		new_crypt->ops = ops;
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv =
+				new_crypt->ops->init(param->u.crypt.idx);
+
+		if (!new_crypt->priv) {
+			kfree(new_crypt);
+			param->u.crypt.err = IEEE_CRYPT_ERR_CRYPT_INIT_FAILED;
+			ret = -EINVAL;
+			goto done;
+		}
+
+		*crypt = new_crypt;
+	}
+
+	if (param->u.crypt.key_len > 0 && (*crypt)->ops->set_key &&
+	    (*crypt)->ops->set_key(param->u.crypt.key,
+				   param->u.crypt.key_len, param->u.crypt.seq,
+				   (*crypt)->priv) < 0) {
+		printk("key setting failed\n");
+		param->u.crypt.err = IEEE_CRYPT_ERR_KEY_SET_FAILED;
+		ret = -EINVAL;
+		goto done;
+	}
+
+ skip_host_crypt:
+	if (param->u.crypt.set_tx) {
+		ieee->tx_keyidx = param->u.crypt.idx;
+		sec.active_key = param->u.crypt.idx;
+		sec.flags |= SEC_ACTIVE_KEY;
+	} else {
+		sec.flags &= ~SEC_ACTIVE_KEY;
+	}
+	memcpy(sec.keys[param->u.crypt.idx],
+	       param->u.crypt.key,
+	       param->u.crypt.key_len);
+	sec.key_sizes[param->u.crypt.idx] = param->u.crypt.key_len;
+	sec.flags |= (1 << param->u.crypt.idx);
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+		sec.flags |= SEC_LEVEL;
+		sec.level = SEC_LEVEL_1;
+	} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+		sec.flags |= SEC_LEVEL;
+		sec.level = SEC_LEVEL_2;
+	} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+		sec.flags |= SEC_LEVEL;
+		sec.level = SEC_LEVEL_3;
+	}
+ done:
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+
+	/* Do not reset port if card is in Managed mode since resetting will
+	 * generate new IEEE 802.11 authentication which may end up in looping
+	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
+	 * configuration (for example... Prism2), implement the reset_port in
+	 * the callbacks structures used to initialize the 802.11 stack. */
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port &&
+	    ieee->reset_port(ieee->dev)) {
+		printk("reset_port failed\n");
+		param->u.crypt.err = IEEE_CRYPT_ERR_CARD_CONF_FAILED;
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static inline struct sk_buff *ieee80211_disassociate_skb(struct ieee80211_network *beacon,
+							 struct ieee80211_device *ieee,
+							 u8	asRsn)
+{
+	struct sk_buff *skb;
+	struct ieee80211_disassoc *disass;
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_disassoc));
+	if (!skb)
+		return NULL;
+
+	disass = skb_put(skb, sizeof(struct ieee80211_disassoc));
+	disass->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_DISASSOC);
+	disass->header.duration_id = 0;
+
+	memcpy(disass->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(disass->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(disass->header.addr3, beacon->bssid, ETH_ALEN);
+
+	disass->reason = cpu_to_le16(asRsn);
+	return skb;
+}
+
+void
+SendDisassociation(struct ieee80211_device *ieee,
+		   u8			   *asSta,
+		   u8			    asRsn
+)
+{
+	struct ieee80211_network *beacon = &ieee->current_network;
+	struct sk_buff *skb;
+
+	skb = ieee80211_disassociate_skb(beacon, ieee, asRsn);
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+EXPORT_SYMBOL(SendDisassociation);
+
+int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p)
+{
+	struct ieee_param *param;
+	int ret = 0;
+
+	mutex_lock(&ieee->wx_mutex);
+	//IEEE_DEBUG_INFO("wpa_supplicant: len=%d\n", p->length);
+
+	if (p->length < sizeof(struct ieee_param) || !p->pointer) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param)) {
+		ret = PTR_ERR(param);
+		goto out;
+	}
+
+	switch (param->cmd) {
+	case IEEE_CMD_SET_WPA_PARAM:
+		ret = ieee80211_wpa_set_param(ieee, param->u.wpa_param.name,
+					param->u.wpa_param.value);
+		break;
+
+	case IEEE_CMD_SET_WPA_IE:
+		ret = ieee80211_wpa_set_wpa_ie(ieee, param, p->length);
+		break;
+
+	case IEEE_CMD_SET_ENCRYPTION:
+		ret = ieee80211_wpa_set_encryption(ieee, param, p->length);
+		break;
+
+	case IEEE_CMD_MLME:
+		ret = ieee80211_wpa_mlme(ieee, param->u.mlme.command,
+				   param->u.mlme.reason_code);
+		break;
+
+	default:
+		printk("Unknown WPA supplicant request: %d\n", param->cmd);
+		ret = -EOPNOTSUPP;
+		break;
+	}
+
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+
+	kfree(param);
+out:
+	mutex_unlock(&ieee->wx_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
+
+void notify_wx_assoc_event(struct ieee80211_device *ieee)
+{
+	union iwreq_data wrqu;
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	if (ieee->state == IEEE80211_LINKED)
+		memcpy(wrqu.ap_addr.sa_data, ieee->current_network.bssid, ETH_ALEN);
+	else
+		eth_zero_addr(wrqu.ap_addr.sa_data);
+	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
+}
+EXPORT_SYMBOL(notify_wx_assoc_event);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
new file mode 100644
index 000000000..afa92ddfa
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -0,0 +1,598 @@
+// SPDX-License-Identifier: GPL-2.0
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Some pieces of code might be stolen from ipw2100 driver
+ * copyright of who own it's copyright ;-)
+ *
+ * PS wx handler mostly stolen from hostap, copyright who
+ * own it's copyright ;-)
+ */
+
+
+#include <linux/etherdevice.h>
+
+#include "ieee80211.h"
+#include "dot11d.h"
+/* FIXME: add A freqs */
+
+const long ieee80211_wlan_frequencies[] = {
+	2412, 2417, 2422, 2427,
+	2432, 2437, 2442, 2447,
+	2452, 2457, 2462, 2467,
+	2472, 2484
+};
+EXPORT_SYMBOL(ieee80211_wlan_frequencies);
+
+int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct iw_freq *fwrq = &wrqu->freq;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->iw_mode == IW_MODE_INFRA) {
+		ret = -EOPNOTSUPP;
+		goto out;
+	}
+
+	/* if setting by freq convert to channel */
+	if (fwrq->e == 1) {
+		if ((fwrq->m >= (int)2.412e8 &&
+		     fwrq->m <= (int)2.487e8)) {
+			int f = fwrq->m / 100000;
+			int c = 0;
+
+			while ((c < 14) && (f != ieee80211_wlan_frequencies[c]))
+				c++;
+
+			/* hack to fall through */
+			fwrq->e = 0;
+			fwrq->m = c + 1;
+		}
+	}
+
+	if (fwrq->e > 0 || fwrq->m > 14 || fwrq->m < 1) {
+		ret = -EOPNOTSUPP;
+		goto out;
+
+	} else { /* Set the channel */
+
+		if (!(GET_DOT11D_INFO(ieee)->channel_map)[fwrq->m]) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ieee->current_network.channel = fwrq->m;
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+		if (ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
+			if (ieee->state == IEEE80211_LINKED) {
+				ieee80211_stop_send_beacons(ieee);
+				ieee80211_start_send_beacons(ieee);
+			}
+	}
+
+	ret = 0;
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_freq);
+
+int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct iw_freq *fwrq = &wrqu->freq;
+
+	if (ieee->current_network.channel == 0)
+		return -1;
+	/* NM 0.7.0 will not accept channel any more. */
+	fwrq->m = ieee80211_wlan_frequencies[ieee->current_network.channel - 1] * 100000;
+	fwrq->e = 1;
+	/* fwrq->m = ieee->current_network.channel; */
+	/* fwrq->e = 0; */
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_freq);
+
+int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	unsigned long flags;
+
+	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state != IEEE80211_LINKED &&
+		ieee->state != IEEE80211_LINKED_SCANNING &&
+		ieee->wap_set == 0)
+
+		eth_zero_addr(wrqu->ap_addr.sa_data);
+	else
+		memcpy(wrqu->ap_addr.sa_data,
+		       ieee->current_network.bssid, ETH_ALEN);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_wap);
+
+int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+
+	int ret = 0;
+	unsigned long flags;
+
+	short ifup = ieee->proto_started; /* dev->flags & IFF_UP; */
+	struct sockaddr *temp = (struct sockaddr *)awrq;
+
+	ieee->sync_scan_hurryup = 1;
+
+	mutex_lock(&ieee->wx_mutex);
+	/* use ifconfig hw ether */
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		ret = -1;
+		goto out;
+	}
+
+	if (temp->sa_family != ARPHRD_ETHER) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (ifup)
+		ieee80211_stop_protocol(ieee);
+
+	/* just to avoid to give inconsistent infos in the
+	 * get wx method. not really needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
+	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (ifup)
+		ieee80211_start_protocol(ieee);
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_wap);
+
+int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a, union iwreq_data *wrqu, char *b)
+{
+	int len, ret = 0;
+	unsigned long flags;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->current_network.ssid[0] == '\0' ||
+		ieee->current_network.ssid_len == 0) {
+		ret = -1;
+		goto out;
+	}
+
+	if (ieee->state != IEEE80211_LINKED &&
+		ieee->state != IEEE80211_LINKED_SCANNING &&
+		ieee->ssid_set == 0) {
+		ret = -1;
+		goto out;
+	}
+	len = ieee->current_network.ssid_len;
+	wrqu->essid.length = len;
+	strncpy(b, ieee->current_network.ssid, len);
+	wrqu->essid.flags = 1;
+
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return ret;
+
+}
+EXPORT_SYMBOL(ieee80211_wx_get_essid);
+
+int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+
+	u32 target_rate = wrqu->bitrate.value;
+
+	ieee->rate = target_rate / 100000;
+	/* FIXME: we might want to limit rate also in management protocols. */
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_rate);
+
+int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	u32 tmp_rate;
+
+	tmp_rate = TxCountToDataRate(ieee, ieee->softmac_stats.CurrentShowTxate);
+
+	wrqu->bitrate.value = tmp_rate * 500000;
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_rate);
+
+int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	if (wrqu->rts.disabled || !wrqu->rts.fixed) {
+		ieee->rts = DEFAULT_RTS_THRESHOLD;
+	} else {
+		if (wrqu->rts.value < MIN_RTS_THRESHOLD ||
+				wrqu->rts.value > MAX_RTS_THRESHOLD)
+			return -EINVAL;
+		ieee->rts = wrqu->rts.value;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_rts);
+
+int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->rts.value = ieee->rts;
+	wrqu->rts.fixed = 0;	/* no auto select */
+	wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD);
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_rts);
+
+int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+
+	ieee->sync_scan_hurryup = 1;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (wrqu->mode == ieee->iw_mode)
+		goto out;
+
+	if (wrqu->mode == IW_MODE_MONITOR)
+		ieee->dev->type = ARPHRD_IEEE80211;
+	else
+		ieee->dev->type = ARPHRD_ETHER;
+
+	if (!ieee->proto_started) {
+		ieee->iw_mode = wrqu->mode;
+	} else {
+		ieee80211_stop_protocol(ieee);
+		ieee->iw_mode = wrqu->mode;
+		ieee80211_start_protocol(ieee);
+	}
+
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_mode);
+
+void ieee80211_wx_sync_scan_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, wx_sync_scan_wq);
+	short chan;
+	enum ht_extension_chan_offset chan_offset = 0;
+	enum ht_channel_width bandwidth = 0;
+	int b40M = 0;
+
+	chan = ieee->current_network.channel;
+	netif_carrier_off(ieee->dev);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_send_beacons(ieee);
+
+	ieee->state = IEEE80211_LINKED_SCANNING;
+	ieee->link_change(ieee->dev);
+	ieee->InitialGainHandler(ieee->dev, IG_Backup);
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT && ieee->pHTInfo->bCurBW40MHz) {
+		b40M = 1;
+		chan_offset = ieee->pHTInfo->CurSTAExtChnlOffset;
+		bandwidth = (enum ht_channel_width)ieee->pHTInfo->bCurBW40MHz;
+		printk("Scan in 40M, force to 20M first:%d, %d\n", chan_offset, bandwidth);
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+		}
+	ieee80211_start_scan_syncro(ieee);
+	if (b40M) {
+		printk("Scan in 20M, back to 40M\n");
+		if (chan_offset == HT_EXTCHNL_OFFSET_UPPER)
+			ieee->set_chan(ieee->dev, chan + 2);
+		else if (chan_offset == HT_EXTCHNL_OFFSET_LOWER)
+			ieee->set_chan(ieee->dev, chan - 2);
+		else
+			ieee->set_chan(ieee->dev, chan);
+		ieee->SetBWModeHandler(ieee->dev, bandwidth, chan_offset);
+	} else {
+		ieee->set_chan(ieee->dev, chan);
+	}
+
+	ieee->InitialGainHandler(ieee->dev, IG_Restore);
+	ieee->state = IEEE80211_LINKED;
+	ieee->link_change(ieee->dev);
+	/* To prevent the immediately calling watch_dog after scan. */
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod == 0 || ieee->LinkDetectInfo.NumRecvDataInPeriod == 0) {
+		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
+		ieee->LinkDetectInfo.NumRecvDataInPeriod = 1;
+	}
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	if (ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
+		ieee80211_start_send_beacons(ieee);
+
+	netif_carrier_on(ieee->dev);
+	mutex_unlock(&ieee->wx_mutex);
+
+}
+
+int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret = 0;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)) {
+		ret = -1;
+		goto out;
+	}
+
+	if (ieee->state == IEEE80211_LINKED) {
+		queue_work(ieee->wq, &ieee->wx_sync_scan_wq);
+		/* intentionally forget to up sem */
+		return 0;
+	}
+
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_scan);
+
+int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra)
+{
+
+	int ret = 0, len;
+	short proto_started;
+	unsigned long flags;
+
+	ieee->sync_scan_hurryup = 1;
+	mutex_lock(&ieee->wx_mutex);
+
+	proto_started = ieee->proto_started;
+
+	if (wrqu->essid.length > IW_ESSID_MAX_SIZE) {
+		ret = -E2BIG;
+		goto out;
+	}
+
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		ret = -1;
+		goto out;
+	}
+
+	if (proto_started)
+		ieee80211_stop_protocol(ieee);
+
+
+	/* this is just to be sure that the GET wx callback
+	 * has consisten infos. not needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (wrqu->essid.flags && wrqu->essid.length) {
+		/* first flush current network.ssid */
+		len = ((wrqu->essid.length - 1) < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length - 1) : IW_ESSID_MAX_SIZE;
+		strncpy(ieee->current_network.ssid, extra, len + 1);
+		ieee->current_network.ssid_len = len + 1;
+		ieee->ssid_set = 1;
+	} else {
+		ieee->ssid_set = 0;
+		ieee->current_network.ssid[0] = '\0';
+		ieee->current_network.ssid_len = 0;
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (proto_started)
+		ieee80211_start_protocol(ieee);
+out:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_essid);
+
+int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+
+	wrqu->mode = ieee->iw_mode;
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_mode);
+
+int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+
+	int *parms = (int *)extra;
+	int enable = (parms[0] > 0);
+	short prev = ieee->raw_tx;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (enable)
+		ieee->raw_tx = 1;
+	else
+		ieee->raw_tx = 0;
+
+	netdev_info(ieee->dev, "raw TX is %s\n",
+		    ieee->raw_tx ? "enabled" : "disabled");
+
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		if (prev == 0 && ieee->raw_tx) {
+			if (ieee->data_hard_resume)
+				ieee->data_hard_resume(ieee->dev);
+
+			netif_carrier_on(ieee->dev);
+		}
+
+		if (prev && ieee->raw_tx == 1)
+			netif_carrier_off(ieee->dev);
+	}
+
+	mutex_unlock(&ieee->wx_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
+
+int ieee80211_wx_get_name(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	strscpy(wrqu->name, "802.11", IFNAMSIZ);
+	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
+		strlcat(wrqu->name, "b", IFNAMSIZ);
+		if (ieee->modulation & IEEE80211_OFDM_MODULATION)
+			strlcat(wrqu->name, "/g", IFNAMSIZ);
+	} else if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
+		strlcat(wrqu->name, "g", IFNAMSIZ);
+	}
+
+	if (ieee->mode & (IEEE_N_24G | IEEE_N_5G))
+		strlcat(wrqu->name, "/n", IFNAMSIZ);
+
+	if ((ieee->state == IEEE80211_LINKED) ||
+	    (ieee->state == IEEE80211_LINKED_SCANNING))
+		strlcat(wrqu->name, " linked", IFNAMSIZ);
+	else if (ieee->state != IEEE80211_NOLINK)
+		strlcat(wrqu->name, " link..", IFNAMSIZ);
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_name);
+
+/* this is mostly stolen from hostap */
+int ieee80211_wx_set_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	mutex_lock(&ieee->wx_mutex);
+
+	if (wrqu->power.disabled) {
+		ieee->ps = IEEE80211_PS_DISABLED;
+		goto exit;
+	}
+	if (wrqu->power.flags & IW_POWER_TIMEOUT) {
+		/* ieee->ps_period = wrqu->power.value / 1000; */
+		ieee->ps_timeout = wrqu->power.value / 1000;
+	}
+
+	if (wrqu->power.flags & IW_POWER_PERIOD) {
+
+		/* ieee->ps_timeout = wrqu->power.value / 1000; */
+		ieee->ps_period = wrqu->power.value / 1000;
+		/* wrq->value / 1024; */
+
+	}
+	switch (wrqu->power.flags & IW_POWER_MODE) {
+	case IW_POWER_UNICAST_R:
+		ieee->ps = IEEE80211_PS_UNICAST;
+		break;
+	case IW_POWER_MULTICAST_R:
+		ieee->ps = IEEE80211_PS_MBCAST;
+		break;
+	case IW_POWER_ALL_R:
+		ieee->ps = IEEE80211_PS_UNICAST | IEEE80211_PS_MBCAST;
+		break;
+
+	case IW_POWER_ON:
+		/* ieee->ps = IEEE80211_PS_DISABLED; */
+		break;
+
+	default:
+		ret = -EINVAL;
+		goto exit;
+
+	}
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+	return ret;
+
+}
+EXPORT_SYMBOL(ieee80211_wx_set_power);
+
+/* this is stolen from hostap */
+int ieee80211_wx_get_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	mutex_lock(&ieee->wx_mutex);
+
+	if (ieee->ps == IEEE80211_PS_DISABLED) {
+		wrqu->power.disabled = 1;
+		goto exit;
+	}
+
+	wrqu->power.disabled = 0;
+
+	if ((wrqu->power.flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
+		wrqu->power.flags = IW_POWER_TIMEOUT;
+		wrqu->power.value = ieee->ps_timeout * 1000;
+	} else {
+		/* ret = -EOPNOTSUPP; */
+		/* goto exit; */
+		wrqu->power.flags = IW_POWER_PERIOD;
+		wrqu->power.value = ieee->ps_period * 1000;
+		/* ieee->current_network.dtim_period * ieee->current_network.beacon_interval * 1024; */
+	}
+
+	if ((ieee->ps & (IEEE80211_PS_MBCAST | IEEE80211_PS_UNICAST)) == (IEEE80211_PS_MBCAST | IEEE80211_PS_UNICAST))
+		wrqu->power.flags |= IW_POWER_ALL_R;
+	else if (ieee->ps & IEEE80211_PS_MBCAST)
+		wrqu->power.flags |= IW_POWER_MULTICAST_R;
+	else
+		wrqu->power.flags |= IW_POWER_UNICAST_R;
+
+exit:
+	mutex_unlock(&ieee->wx_mutex);
+	return 0;
+
+}
+EXPORT_SYMBOL(ieee80211_wx_get_power);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
new file mode 100644
index 000000000..e4b645480
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -0,0 +1,839 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ *  Copyright(c) 2003 - 2004 Intel Corporation. All rights reserved.
+ *
+ *  Contact Information:
+ *  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ *  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *
+ *  Few modifications for Realtek's Wi-Fi drivers by
+ *  Andrea Merello <andrea.merello@gmail.com>
+ *
+ *  A special thanks goes to Realtek for their support !
+ *
+ ******************************************************************************/
+
+#include <linux/compiler.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <linux/uaccess.h>
+#include <linux/if_vlan.h>
+
+#include "ieee80211.h"
+
+
+/*
+ *
+ *
+ * 802.11 Data Frame
+ *
+ *
+ * 802.11 frame_contorl for data frames - 2 bytes
+ *      ,-----------------------------------------------------------------------------------------.
+ * bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
+ *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+ * val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
+ *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+ * desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
+ *      |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
+ *      '-----------------------------------------------------------------------------------------'
+ *                                                    /\
+ *                                                    |
+ * 802.11 Data Frame                                  |
+ *           ,--------- 'ctrl' expands to >-----------'
+ *           |
+ *        ,--'---,-------------------------------------------------------------.
+ *  Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *        |------|------|---------|---------|---------|------|---------|------|
+ *  Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
+ *        |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *        `--------------------------------------------------|         |------'
+ *  Total: 28 non-data bytes                                 `----.----'
+ *                                                                |
+ *         .- 'Frame data' expands to <---------------------------'
+ *         |
+ *         V
+ *        ,---------------------------------------------------.
+ *  Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
+ *        |------|------|---------|----------|------|---------|
+ *  Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
+ *        | DSAP | SSAP |         |          |      | Packet  |
+ *        | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
+ *        `-----------------------------------------|         |
+ *  Total: 8 non-data bytes                         `----.----'
+ *                                                       |
+ *         .- 'IP Packet' expands, if WEP enabled, to <--'
+ *         |
+ *         V
+ *        ,-----------------------.
+ *  Bytes |  4  |   0-2296  |  4  |
+ *        |-----|-----------|-----|
+ *  Desc. | IV  | Encrypted | ICV |
+ *        |     | IP Packet |     |
+ *        `-----------------------'
+ *  Total: 8 non-data bytes
+ *
+ *
+ *  802.3 Ethernet Data Frame
+ *
+ *        ,-----------------------------------------.
+ *  Bytes |   6   |   6   |  2   |  Variable |   4  |
+ *        |-------|-------|------|-----------|------|
+ *  Desc. | Dest. | Source| Type | IP Packet |  fcs |
+ *        |  MAC  |  MAC  |      |           |      |
+ *        `-----------------------------------------'
+ *  Total: 18 non-data bytes
+ *
+ *  In the event that fragmentation is required, the incoming payload is split into
+ *  N parts of size ieee->fts.  The first fragment contains the SNAP header and the
+ *  remaining packets are just data.
+ *
+ *  If encryption is enabled, each fragment payload size is reduced by enough space
+ *  to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
+ *  So if you have 1500 bytes of payload with ieee->fts set to 500 without
+ *  encryption it will take 3 frames.  With WEP it will take 4 frames as the
+ *  payload of each frame is reduced to 492 bytes.
+ *
+ * SKB visualization
+ *
+ *  ,- skb->data
+ * |
+ * |    ETHERNET HEADER        ,-<-- PAYLOAD
+ * |                           |     14 bytes from skb->data
+ * |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
+ * |                       | | |
+ * |,-Dest.--. ,--Src.---. | | |
+ * |  6 bytes| | 6 bytes | | | |
+ * v         | |         | | | |
+ * 0         | v       1 | v | v           2
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
+ *     ^     | ^         | ^ |
+ *     |     | |         | | |
+ *     |     | |         | `T' <---- 2 bytes for Type
+ *     |     | |         |
+ *     |     | '---SNAP--' <-------- 6 bytes for SNAP
+ *     |     |
+ *     `-IV--' <-------------------- 4 bytes for IV (WEP)
+ *
+ *      SNAP HEADER
+ *
+ */
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static inline int ieee80211_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+int ieee80211_encrypt_fragment(
+	struct ieee80211_device *ieee,
+	struct sk_buff *frag,
+	int hdr_len)
+{
+	struct ieee80211_crypt_data *crypt = ieee->crypt[ieee->tx_keyidx];
+	int res;
+
+	if (!(crypt && crypt->ops)) {
+		printk("=========>%s(), crypt is null\n", __func__);
+		return -1;
+	}
+
+	if (ieee->tkip_countermeasures &&
+	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
+		if (net_ratelimit()) {
+			struct rtl_80211_hdr_3addrqos *header;
+
+			header = (struct rtl_80211_hdr_3addrqos *)frag->data;
+			netdev_dbg(ieee->dev, "TKIP countermeasures: dropped "
+			       "TX packet to %pM\n", header->addr1);
+		}
+		return -1;
+	}
+
+	/* To encrypt, frame format is:
+	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes)
+	 */
+
+	// PR: FIXME: Copied from hostap. Check fragmentation/MSDU/MPDU encryption.
+	/* Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
+	 * call both MSDU and MPDU encryption functions from here.
+	 */
+	atomic_inc(&crypt->refcnt);
+	res = 0;
+	if (crypt->ops->encrypt_msdu)
+		res = crypt->ops->encrypt_msdu(frag, hdr_len, crypt->priv);
+	if (res == 0 && crypt->ops->encrypt_mpdu)
+		res = crypt->ops->encrypt_mpdu(frag, hdr_len, crypt->priv);
+
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		netdev_info(ieee->dev, "Encryption failed: len=%d.\n",
+			    frag->len);
+		ieee->ieee_stats.tx_discards++;
+		return -1;
+	}
+
+	return 0;
+}
+
+
+void ieee80211_txb_free(struct ieee80211_txb *txb)
+{
+	//int i;
+	if (unlikely(!txb))
+		return;
+	kfree(txb);
+}
+EXPORT_SYMBOL(ieee80211_txb_free);
+
+static struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
+						 gfp_t gfp_mask)
+{
+	struct ieee80211_txb *txb;
+	int i;
+	txb = kmalloc(
+		sizeof(struct ieee80211_txb) + (sizeof(u8 *) * nr_frags),
+		gfp_mask);
+	if (!txb)
+		return NULL;
+
+	memset(txb, 0, sizeof(struct ieee80211_txb));
+	txb->nr_frags = nr_frags;
+	txb->frag_size = __cpu_to_le16(txb_size);
+
+	for (i = 0; i < nr_frags; i++) {
+		txb->fragments[i] = dev_alloc_skb(txb_size);
+		if (unlikely(!txb->fragments[i])) {
+			i--;
+			break;
+		}
+		memset(txb->fragments[i]->cb, 0, sizeof(txb->fragments[i]->cb));
+	}
+	if (unlikely(i != nr_frags)) {
+		while (i >= 0)
+			dev_kfree_skb_any(txb->fragments[i--]);
+		kfree(txb);
+		return NULL;
+	}
+	return txb;
+}
+
+// Classify the to-be send data packet
+// Need to acquire the sent queue index.
+static int
+ieee80211_classify(struct sk_buff *skb, struct ieee80211_network *network)
+{
+	struct ethhdr *eth;
+	struct iphdr *ip;
+	eth = (struct ethhdr *)skb->data;
+	if (eth->h_proto != htons(ETH_P_IP))
+		return 0;
+
+	ip = ip_hdr(skb);
+	switch (ip->tos & 0xfc) {
+	case 0x20:
+		return 2;
+	case 0x40:
+		return 1;
+	case 0x60:
+		return 3;
+	case 0x80:
+		return 4;
+	case 0xa0:
+		return 5;
+	case 0xc0:
+		return 6;
+	case 0xe0:
+		return 7;
+	default:
+		return 0;
+	}
+}
+
+static void ieee80211_tx_query_agg_cap(struct ieee80211_device *ieee,
+				       struct sk_buff *skb, struct cb_desc *tcb_desc)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	struct tx_ts_record        *pTxTs = NULL;
+	struct rtl_80211_hdr_1addr *hdr = (struct rtl_80211_hdr_1addr *)skb->data;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+	if (!IsQoSDataFrame(skb->data))
+		return;
+
+	if (is_multicast_ether_addr(hdr->addr1))
+		return;
+	//check packet and mode later
+	if (!ieee->GetNmodeSupportBySecCfg(ieee->dev))
+		return;
+
+	if (pHTInfo->bCurrentAMPDUEnable) {
+		if (!GetTs(ieee, (struct ts_common_info **)(&pTxTs), hdr->addr1, skb->priority, TX_DIR, true)) {
+			printk("===>can't get TS\n");
+			return;
+		}
+		if (!pTxTs->tx_admitted_ba_record.valid) {
+			TsStartAddBaProcess(ieee, pTxTs);
+			goto FORCED_AGG_SETTING;
+		} else if (!pTxTs->using_ba) {
+			if (SN_LESS(pTxTs->tx_admitted_ba_record.start_seq_ctrl.field.seq_num, (pTxTs->tx_cur_seq + 1) % 4096))
+				pTxTs->using_ba = true;
+			else
+				goto FORCED_AGG_SETTING;
+		}
+
+		if (ieee->iw_mode == IW_MODE_INFRA) {
+			tcb_desc->bAMPDUEnable = true;
+			tcb_desc->ampdu_factor = pHTInfo->CurrentAMPDUFactor;
+			tcb_desc->ampdu_density = pHTInfo->CurrentMPDUDensity;
+		}
+	}
+FORCED_AGG_SETTING:
+	switch (pHTInfo->ForcedAMPDUMode) {
+	case HT_AGG_AUTO:
+		break;
+
+	case HT_AGG_FORCE_ENABLE:
+		tcb_desc->bAMPDUEnable = true;
+		tcb_desc->ampdu_density = pHTInfo->ForcedMPDUDensity;
+		tcb_desc->ampdu_factor = pHTInfo->ForcedAMPDUFactor;
+		break;
+
+	case HT_AGG_FORCE_DISABLE:
+		tcb_desc->bAMPDUEnable = false;
+		tcb_desc->ampdu_density = 0;
+		tcb_desc->ampdu_factor = 0;
+		break;
+
+	}
+		return;
+}
+
+static void ieee80211_qurey_ShortPreambleMode(struct ieee80211_device *ieee,
+					      struct cb_desc *tcb_desc)
+{
+	tcb_desc->bUseShortPreamble = false;
+	if (tcb_desc->data_rate == 2) {//// 1M can only use Long Preamble. 11B spec
+		return;
+	} else if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE) {
+		tcb_desc->bUseShortPreamble = true;
+	}
+	return;
+}
+static void
+ieee80211_query_HTCapShortGI(struct ieee80211_device *ieee, struct cb_desc *tcb_desc)
+{
+	PRT_HIGH_THROUGHPUT		pHTInfo = ieee->pHTInfo;
+
+	tcb_desc->bUseShortGI		= false;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+
+	if (pHTInfo->bForcedShortGI) {
+		tcb_desc->bUseShortGI = true;
+		return;
+	}
+
+	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI40MHz)
+		tcb_desc->bUseShortGI = true;
+	else if (!pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI20MHz)
+		tcb_desc->bUseShortGI = true;
+}
+
+static void ieee80211_query_BandwidthMode(struct ieee80211_device *ieee,
+					  struct cb_desc *tcb_desc)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+
+	tcb_desc->bPacketBW = false;
+
+	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
+		return;
+
+	if (tcb_desc->bMulticast || tcb_desc->bBroadcast)
+		return;
+
+	if ((tcb_desc->data_rate & 0x80) == 0) // If using legacy rate, it shall use 20MHz channel.
+		return;
+	//BandWidthAutoSwitch is for auto switch to 20 or 40 in long distance
+	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurTxBW40MHz && !ieee->bandwidth_auto_switch.bforced_tx20Mhz)
+		tcb_desc->bPacketBW = true;
+	return;
+}
+
+static void ieee80211_query_protectionmode(struct ieee80211_device *ieee,
+					   struct cb_desc *tcb_desc,
+					   struct sk_buff *skb)
+{
+	// Common Settings
+	tcb_desc->bRTSSTBC			= false;
+	tcb_desc->bRTSUseShortGI		= false; // Since protection frames are always sent by legacy rate, ShortGI will never be used.
+	tcb_desc->bCTSEnable			= false; // Most of protection using RTS/CTS
+	tcb_desc->RTSSC				= 0;		// 20MHz: Don't care;  40MHz: Duplicate.
+	tcb_desc->bRTSBW			= false; // RTS frame bandwidth is always 20MHz
+
+	if (tcb_desc->bBroadcast || tcb_desc->bMulticast) //only unicast frame will use rts/cts
+		return;
+
+	if (is_broadcast_ether_addr(skb->data + 16))  //check addr3 as infrastructure add3 is DA.
+		return;
+
+	if (ieee->mode < IEEE_N_24G) /* b, g mode */ {
+			// (1) RTS_Threshold is compared to the MPDU, not MSDU.
+			// (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame.
+			//		Other fragments are protected by previous fragment.
+			//		So we only need to check the length of first fragment.
+		if (skb->len > ieee->rts) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+		} else if (ieee->current_network.buseprotection) {
+			// Use CTS-to-SELF in protection mode.
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->bCTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+		}
+		//otherwise return;
+		return;
+	} else { // 11n High throughput case.
+		PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
+		while (true) {
+			//check ERP protection
+			if (ieee->current_network.buseprotection) {// CTS-to-SELF
+				tcb_desc->bRTSEnable = true;
+				tcb_desc->bCTSEnable = true;
+				tcb_desc->rts_rate = MGN_24M;
+				break;
+			}
+			//check HT op mode
+			if (pHTInfo->bCurrentHTSupport && pHTInfo->bEnableHT) {
+				u8 HTOpMode = pHTInfo->CurrentOpMode;
+				if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 || HTOpMode == 3)) ||
+							(!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
+					tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
+					tcb_desc->bRTSEnable = true;
+					break;
+				}
+			}
+			//check rts
+			if (skb->len > ieee->rts) {
+				tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
+				tcb_desc->bRTSEnable = true;
+				break;
+			}
+			//to do list: check MIMO power save condition.
+			//check AMPDU aggregation for TXOP
+			if (tcb_desc->bAMPDUEnable) {
+				tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
+				// According to 8190 design, firmware sends CF-End only if RTS/CTS is enabled. However, it degrads
+				// throughput around 10M, so we disable of this mechanism. 2007.08.03 by Emily
+				tcb_desc->bRTSEnable = false;
+				break;
+			}
+			//check IOT action
+			if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) {
+				tcb_desc->bCTSEnable	= true;
+				tcb_desc->rts_rate  =	MGN_24M;
+				tcb_desc->bRTSEnable = true;
+				break;
+			}
+			// Totally no protection case!!
+			goto NO_PROTECTION;
+		}
+		}
+	// For test , CTS replace with RTS
+	if (0) {
+		tcb_desc->bCTSEnable	= true;
+		tcb_desc->rts_rate = MGN_24M;
+		tcb_desc->bRTSEnable	= true;
+	}
+	if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		tcb_desc->bUseShortPreamble = true;
+	if (ieee->mode == IW_MODE_MASTER)
+		goto NO_PROTECTION;
+	return;
+NO_PROTECTION:
+	tcb_desc->bRTSEnable	= false;
+	tcb_desc->bCTSEnable	= false;
+	tcb_desc->rts_rate		= 0;
+	tcb_desc->RTSSC		= 0;
+	tcb_desc->bRTSBW		= false;
+}
+
+
+static void ieee80211_txrate_selectmode(struct ieee80211_device *ieee,
+					struct cb_desc *tcb_desc)
+{
+	if (ieee->bTxDisableRateFallBack)
+		tcb_desc->bTxDisableRateFallBack = true;
+
+	if (ieee->bTxUseDriverAssingedRate)
+		tcb_desc->bTxUseDriverAssingedRate = true;
+	if (!tcb_desc->bTxDisableRateFallBack || !tcb_desc->bTxUseDriverAssingedRate) {
+		if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC)
+			tcb_desc->RATRIndex = 0;
+	}
+}
+
+static void ieee80211_query_seqnum(struct ieee80211_device *ieee,
+				   struct sk_buff *skb, u8 *dst)
+{
+	if (is_multicast_ether_addr(dst))
+		return;
+	if (IsQoSDataFrame(skb->data)) /* we deal qos data only */ {
+		struct tx_ts_record *pTS = NULL;
+		if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst, skb->priority, TX_DIR, true)) {
+			return;
+		}
+		pTS->tx_cur_seq = (pTS->tx_cur_seq + 1) % 4096;
+	}
+}
+
+netdev_tx_t ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ieee80211_device *ieee = netdev_priv(dev);
+	struct ieee80211_txb *txb = NULL;
+	struct rtl_80211_hdr_3addrqos *frag_hdr;
+	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
+	unsigned long flags;
+	struct net_device_stats *stats = &ieee->stats;
+	int ether_type = 0, encrypt;
+	int bytes, fc, qos_ctl = 0, hdr_len;
+	struct sk_buff *skb_frag;
+	struct rtl_80211_hdr_3addrqos header = { /* Ensure zero initialized */
+		.duration_id = 0,
+		.seq_ctl = 0,
+		.qos_ctl = 0
+	};
+	u8 dest[ETH_ALEN], src[ETH_ALEN];
+	int qos_actived = ieee->current_network.qos_data.active;
+
+	struct ieee80211_crypt_data *crypt;
+
+	struct cb_desc *tcb_desc;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* If there is no driver handler to take the TXB, dont' bother
+	 * creating it...
+	 */
+	if ((!ieee->hard_start_xmit && !(ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)) ||
+	   ((!ieee->softmac_data_hard_start_xmit && (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
+		netdev_warn(ieee->dev, "No xmit handler.\n");
+		goto success;
+	}
+
+
+	if (likely(ieee->raw_tx == 0)) {
+		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
+			goto success;
+		}
+
+		memset(skb->cb, 0, sizeof(skb->cb));
+		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+		crypt = ieee->crypt[ieee->tx_keyidx];
+
+		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
+			ieee->host_encrypt && crypt && crypt->ops;
+
+		if (!encrypt && ieee->ieee802_1x &&
+		ieee->drop_unencrypted && ether_type != ETH_P_PAE) {
+			stats->tx_dropped++;
+			goto success;
+		}
+	#ifdef CONFIG_IEEE80211_DEBUG
+		if (crypt && !encrypt && ether_type == ETH_P_PAE) {
+			struct eapol *eap = (struct eapol *)(skb->data +
+				sizeof(struct ethhdr) - SNAP_SIZE - sizeof(u16));
+			IEEE80211_DEBUG_EAP("TX: IEEE 802.11 EAPOL frame: %s\n",
+				eap_get_type(eap->type));
+		}
+	#endif
+
+		/* Save source and destination addresses */
+		memcpy(&dest, skb->data, ETH_ALEN);
+		memcpy(&src, skb->data + ETH_ALEN, ETH_ALEN);
+
+		/* Advance the SKB to the start of the payload */
+		skb_pull(skb, sizeof(struct ethhdr));
+
+		/* Determine total amount of storage required for TXB packets */
+		bytes = skb->len + SNAP_SIZE + sizeof(u16);
+
+		if (encrypt)
+			fc = IEEE80211_FTYPE_DATA | IEEE80211_FCTL_WEP;
+		else
+
+			fc = IEEE80211_FTYPE_DATA;
+
+		//if(ieee->current_network.QoS_Enable)
+		if (qos_actived)
+			fc |= IEEE80211_STYPE_QOS_DATA;
+		else
+			fc |= IEEE80211_STYPE_DATA;
+
+		if (ieee->iw_mode == IW_MODE_INFRA) {
+			fc |= IEEE80211_FCTL_TODS;
+			/* To DS: Addr1 = BSSID, Addr2 = SA,
+			 * Addr3 = DA
+			 */
+			memcpy(&header.addr1, ieee->current_network.bssid, ETH_ALEN);
+			memcpy(&header.addr2, &src, ETH_ALEN);
+			memcpy(&header.addr3, &dest, ETH_ALEN);
+		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+			/* not From/To DS: Addr1 = DA, Addr2 = SA,
+			 * Addr3 = BSSID
+			 */
+			memcpy(&header.addr1, dest, ETH_ALEN);
+			memcpy(&header.addr2, src, ETH_ALEN);
+			memcpy(&header.addr3, ieee->current_network.bssid, ETH_ALEN);
+		}
+
+		header.frame_ctl = cpu_to_le16(fc);
+
+		/* Determine fragmentation size based on destination (multicast
+		 * and broadcast are not fragmented)
+		 */
+		if (is_multicast_ether_addr(header.addr1)) {
+			frag_size = MAX_FRAG_THRESHOLD;
+			qos_ctl |= QOS_CTL_NOTCONTAIN_ACK;
+		} else {
+			frag_size = ieee->fts;//default:392
+			qos_ctl = 0;
+		}
+
+		//if (ieee->current_network.QoS_Enable)
+		if (qos_actived) {
+			hdr_len = IEEE80211_3ADDR_LEN + 2;
+
+			skb->priority = ieee80211_classify(skb, &ieee->current_network);
+			qos_ctl |= skb->priority; //set in the ieee80211_classify
+			header.qos_ctl = cpu_to_le16(qos_ctl & IEEE80211_QOS_TID);
+		} else {
+			hdr_len = IEEE80211_3ADDR_LEN;
+		}
+		/* Determine amount of payload per fragment.  Regardless of if
+		 * this stack is providing the full 802.11 header, one will
+		 * eventually be affixed to this fragment -- so we must account for
+		 * it when determining the amount of payload space.
+		 */
+		bytes_per_frag = frag_size - hdr_len;
+		if (ieee->config &
+		(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+			bytes_per_frag -= IEEE80211_FCS_LEN;
+
+		/* Each fragment may need to have room for encryption pre/postfix */
+		if (encrypt)
+			bytes_per_frag -= crypt->ops->extra_prefix_len +
+				crypt->ops->extra_postfix_len;
+
+		/* Number of fragments is the total bytes_per_frag /
+		 * payload_per_fragment
+		 */
+		nr_frags = bytes / bytes_per_frag;
+		bytes_last_frag = bytes % bytes_per_frag;
+		if (bytes_last_frag)
+			nr_frags++;
+		else
+			bytes_last_frag = bytes_per_frag;
+
+		/* When we allocate the TXB we allocate enough space for the reserve
+		 * and full fragment bytes (bytes_per_frag doesn't include prefix,
+		 * postfix, header, FCS, etc.)
+		 */
+		txb = ieee80211_alloc_txb(nr_frags, frag_size + ieee->tx_headroom, GFP_ATOMIC);
+		if (unlikely(!txb)) {
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
+			goto failed;
+		}
+		txb->encrypted = encrypt;
+		txb->payload_size = __cpu_to_le16(bytes);
+
+		//if (ieee->current_network.QoS_Enable)
+		if (qos_actived)
+			txb->queue_index = UP2AC(skb->priority);
+		else
+			txb->queue_index = WME_AC_BK;
+
+
+
+		for (i = 0; i < nr_frags; i++) {
+			skb_frag = txb->fragments[i];
+			tcb_desc = (struct cb_desc *)(skb_frag->cb + MAX_DEV_ADDR_SIZE);
+			if (qos_actived) {
+				skb_frag->priority = skb->priority;//UP2AC(skb->priority);
+				tcb_desc->queue_index =  UP2AC(skb->priority);
+			} else {
+				skb_frag->priority = WME_AC_BK;
+				tcb_desc->queue_index = WME_AC_BK;
+			}
+			skb_reserve(skb_frag, ieee->tx_headroom);
+
+			if (encrypt) {
+				if (ieee->hwsec_active)
+					tcb_desc->bHwSec = 1;
+				else
+					tcb_desc->bHwSec = 0;
+				skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
+			} else {
+				tcb_desc->bHwSec = 0;
+			}
+			frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
+
+			/* If this is not the last fragment, then add the MOREFRAGS
+			 * bit to the frame control
+			 */
+			if (i != nr_frags - 1) {
+				frag_hdr->frame_ctl = cpu_to_le16(
+					fc | IEEE80211_FCTL_MOREFRAGS);
+				bytes = bytes_per_frag;
+
+			} else {
+				/* The last fragment takes the remaining length */
+				bytes = bytes_last_frag;
+			}
+			//if(ieee->current_network.QoS_Enable)
+			if (qos_actived) {
+				// add 1 only indicate to corresponding seq number control 2006/7/12
+				frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[UP2AC(skb->priority) + 1] << 4 | i);
+			} else {
+				frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4 | i);
+			}
+
+			/* Put a SNAP header on the first fragment */
+			if (i == 0) {
+				ieee80211_put_snap(
+					skb_put(skb_frag, SNAP_SIZE + sizeof(u16)),
+					ether_type);
+				bytes -= SNAP_SIZE + sizeof(u16);
+			}
+
+			skb_put_data(skb_frag, skb->data, bytes);
+
+			/* Advance the SKB... */
+			skb_pull(skb, bytes);
+
+			/* Encryption routine will move the header forward in order
+			 * to insert the IV between the header and the payload
+			 */
+			if (encrypt)
+				ieee80211_encrypt_fragment(ieee, skb_frag, hdr_len);
+			if (ieee->config &
+			(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+				skb_put(skb_frag, 4);
+		}
+
+		if (qos_actived) {
+			if (ieee->seq_ctrl[UP2AC(skb->priority) + 1] == 0xFFF)
+				ieee->seq_ctrl[UP2AC(skb->priority) + 1] = 0;
+			else
+				ieee->seq_ctrl[UP2AC(skb->priority) + 1]++;
+		} else {
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+		}
+	} else {
+		if (unlikely(skb->len < sizeof(struct rtl_80211_hdr_3addr))) {
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
+			goto success;
+		}
+
+		txb = ieee80211_alloc_txb(1, skb->len, GFP_ATOMIC);
+		if (!txb) {
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
+			goto failed;
+		}
+
+		txb->encrypted = 0;
+		txb->payload_size = __cpu_to_le16(skb->len);
+		skb_put_data(txb->fragments[0], skb->data, skb->len);
+	}
+
+ success:
+//WB add to fill data tcb_desc here. only first fragment is considered, need to change, and you may remove to other place.
+	if (txb) {
+		tcb_desc = (struct cb_desc *)(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->bTxEnableFwCalcDur = 1;
+		if (is_multicast_ether_addr(header.addr1))
+			tcb_desc->bMulticast = 1;
+		if (is_broadcast_ether_addr(header.addr1))
+			tcb_desc->bBroadcast = 1;
+		ieee80211_txrate_selectmode(ieee, tcb_desc);
+		if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
+			tcb_desc->data_rate = ieee->basic_rate;
+		else
+			tcb_desc->data_rate = CURRENT_RATE(ieee->mode, ieee->rate, ieee->HTCurrentOperaRate);
+		ieee80211_qurey_ShortPreambleMode(ieee, tcb_desc);
+		ieee80211_tx_query_agg_cap(ieee, txb->fragments[0], tcb_desc);
+		ieee80211_query_HTCapShortGI(ieee, tcb_desc);
+		ieee80211_query_BandwidthMode(ieee, tcb_desc);
+		ieee80211_query_protectionmode(ieee, tcb_desc, txb->fragments[0]);
+		ieee80211_query_seqnum(ieee, txb->fragments[0], header.addr1);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	dev_kfree_skb_any(skb);
+	if (txb) {
+		if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE) {
+			ieee80211_softmac_xmit(txb, ieee);
+		} else {
+			if ((*ieee->hard_start_xmit)(txb, dev) == 0) {
+				stats->tx_packets++;
+				stats->tx_bytes += __le16_to_cpu(txb->payload_size);
+				return NETDEV_TX_OK;
+			}
+			ieee80211_txb_free(txb);
+		}
+	}
+
+	return NETDEV_TX_OK;
+
+ failed:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	netif_stop_queue(dev);
+	stats->tx_errors++;
+	return 1;
+
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
new file mode 100644
index 000000000..d6829cf6f
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -0,0 +1,810 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+
+  Copyright(c) 2004 Intel Corporation. All rights reserved.
+
+  Portions of this file are based on the WEP enablement code provided by the
+  Host AP project hostap-drivers v0.1.3
+  Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+  <jkmaline@cc.hut.fi>
+  Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+
+  Contact Information:
+  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+******************************************************************************/
+#include <linux/wireless.h>
+#include <linux/kmod.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+
+#include "ieee80211.h"
+struct modes_unit {
+	char *mode_string;
+	int mode_size;
+};
+static struct modes_unit ieee80211_modes[] = {
+	{"a", 1},
+	{"b", 1},
+	{"g", 1},
+	{"?", 1},
+	{"N-24G", 5},
+	{"N-5G", 4},
+};
+
+#define iwe_stream_add_event_rsl iwe_stream_add_event
+
+#define MAX_CUSTOM_LEN 64
+static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
+					   char *start, char *stop,
+					   struct ieee80211_network *network,
+					   struct iw_request_info *info)
+{
+	char custom[MAX_CUSTOM_LEN];
+	char proto_name[IFNAMSIZ];
+	char *pname = proto_name;
+	char *p;
+	struct iw_event iwe;
+	int i, j;
+	u16 max_rate, rate;
+	static u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+
+	/* First entry *MUST* be the AP MAC address */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+	memcpy(iwe.u.ap_addr.sa_data, network->bssid, ETH_ALEN);
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_ADDR_LEN);
+	/* Remaining entries will be displayed in the order we provide them */
+
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+//	if (network->flags & NETWORK_EMPTY_ESSID) {
+	if (network->ssid_len == 0) {
+		iwe.u.data.length = sizeof("<hidden>");
+		start = iwe_stream_add_point(info, start, stop, &iwe, "<hidden>");
+	} else {
+		iwe.u.data.length = min(network->ssid_len, (u8)32);
+		start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+	}
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	for (i = 0; i < ARRAY_SIZE(ieee80211_modes); i++) {
+		if (network->mode & BIT(i)) {
+			sprintf(pname, ieee80211_modes[i].mode_string, ieee80211_modes[i].mode_size);
+			pname += ieee80211_modes[i].mode_size;
+		}
+	}
+	*pname = '\0';
+	snprintf(iwe.u.name, IFNAMSIZ, "IEEE802.11%s", proto_name);
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_CHAR_LEN);
+	/* Add mode */
+	iwe.cmd = SIOCGIWMODE;
+	if (network->capability &
+	    (WLAN_CAPABILITY_BSS | WLAN_CAPABILITY_IBSS)) {
+		if (network->capability & WLAN_CAPABILITY_BSS)
+			iwe.u.mode = IW_MODE_MASTER;
+		else
+			iwe.u.mode = IW_MODE_ADHOC;
+		start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_UINT_LEN);
+	}
+
+	/* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+/*	iwe.u.freq.m = ieee80211_frequency(network->channel, network->mode);
+	iwe.u.freq.e = 3; */
+	iwe.u.freq.m = network->channel;
+	iwe.u.freq.e = 0;
+	iwe.u.freq.i = 0;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_FREQ_LEN);
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	if (network->capability & WLAN_CAPABILITY_PRIVACY)
+		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+	else
+		iwe.u.data.flags = IW_ENCODE_DISABLED;
+	iwe.u.data.length = 0;
+	start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+	/* Add basic and extended rates */
+	max_rate = 0;
+	p = custom;
+	p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom), " Rates (Mb/s): ");
+	for (i = 0, j = 0; i < network->rates_len; ) {
+		if (j < network->rates_ex_len &&
+		    ((network->rates_ex[j] & 0x7F) <
+		     (network->rates[i] & 0x7F)))
+			rate = network->rates_ex[j++] & 0x7F;
+		else
+			rate = network->rates[i++] & 0x7F;
+		if (rate > max_rate)
+			max_rate = rate;
+		p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+	}
+	for (; j < network->rates_ex_len; j++) {
+		rate = network->rates_ex[j] & 0x7F;
+		p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+		if (rate > max_rate)
+			max_rate = rate;
+	}
+
+	if (network->mode >= IEEE_N_24G) /* add N rate here */ {
+		struct ht_capability_ele *ht_cap = NULL;
+		bool is40M = false, isShortGI = false;
+		u8 max_mcs = 0;
+		if (!memcmp(network->bssht.bdHTCapBuf, EWC11NHTCap, 4))
+			ht_cap = (struct ht_capability_ele *)&network->bssht.bdHTCapBuf[4];
+		else
+			ht_cap = (struct ht_capability_ele *)&network->bssht.bdHTCapBuf[0];
+		is40M = (ht_cap->ChlWidth) ? 1 : 0;
+		isShortGI = (ht_cap->ChlWidth) ?
+					((ht_cap->ShortGI40Mhz) ? 1 : 0) :
+					((ht_cap->ShortGI20Mhz) ? 1 : 0);
+
+		max_mcs = HTGetHighestMCSRate(ieee, ht_cap->MCS, MCS_FILTER_ALL);
+		rate = MCS_DATA_RATE[is40M][isShortGI][max_mcs & 0x7f];
+		if (rate > max_rate)
+			max_rate = rate;
+	}
+	iwe.cmd = SIOCGIWRATE;
+	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+	iwe.u.bitrate.value = max_rate * 500000;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe,
+				     IW_EV_PARAM_LEN);
+	iwe.cmd = IWEVCUSTOM;
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+	/* Add quality statistics */
+	/* TODO: Fix these values... */
+	iwe.cmd = IWEVQUAL;
+	iwe.u.qual.qual = network->stats.signal;
+	iwe.u.qual.level = network->stats.rssi;
+	iwe.u.qual.noise = network->stats.noise;
+	iwe.u.qual.updated = network->stats.mask & IEEE80211_STATMASK_WEMASK;
+	if (!(network->stats.mask & IEEE80211_STATMASK_RSSI))
+		iwe.u.qual.updated |= IW_QUAL_LEVEL_INVALID;
+	if (!(network->stats.mask & IEEE80211_STATMASK_NOISE))
+		iwe.u.qual.updated |= IW_QUAL_NOISE_INVALID;
+	if (!(network->stats.mask & IEEE80211_STATMASK_SIGNAL))
+		iwe.u.qual.updated |= IW_QUAL_QUAL_INVALID;
+	iwe.u.qual.updated = 7;
+	start = iwe_stream_add_event_rsl(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+
+	if (ieee->wpa_enabled && network->wpa_ie_len) {
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+	//	printk("WPA IE\n");
+		u8 *p = buf;
+		p += sprintf(p, "wpa_ie=");
+		for (i = 0; i < network->wpa_ie_len; i++)
+			p += sprintf(p, "%02x", network->wpa_ie[i]);
+
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = IWEVCUSTOM;
+		iwe.u.data.length = strlen(buf);
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+	}
+
+	if (ieee->wpa_enabled && network->rsn_ie_len) {
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+
+		u8 *p = buf;
+		p += sprintf(p, "rsn_ie=");
+		for (i = 0; i < network->rsn_ie_len; i++)
+			p += sprintf(p, "%02x", network->rsn_ie[i]);
+
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = IWEVCUSTOM;
+		iwe.u.data.length = strlen(buf);
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+	}
+
+
+	/* Add EXTRA: Age to display seconds since last beacon/probe response
+	 * for given network. */
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+	p += scnprintf(p, MAX_CUSTOM_LEN - (p - custom),
+		      " Last beacon: %lums ago", (jiffies - network->last_scanned) / (HZ / 100));
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+
+	return start;
+}
+
+int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	struct ieee80211_network *network;
+	unsigned long flags;
+
+	char *ev = extra;
+//	char *stop = ev + IW_SCAN_MAX_DATA;
+	char *stop = ev + wrqu->data.length;//IW_SCAN_MAX_DATA;
+	//char *stop = ev + IW_SCAN_MAX_DATA;
+	int i = 0;
+	int err = 0;
+	IEEE80211_DEBUG_WX("Getting scan\n");
+	mutex_lock(&ieee->wx_mutex);
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	list_for_each_entry(network, &ieee->network_list, list) {
+		i++;
+		if ((stop - ev) < 200) {
+			err = -E2BIG;
+			break;
+		}
+		if (ieee->scan_age == 0 ||
+		    time_after(network->last_scanned + ieee->scan_age, jiffies))
+			ev = rtl819x_translate_scan(ieee, ev, stop, network, info);
+		else
+			IEEE80211_DEBUG_SCAN(
+				"Not showing network '%s ("
+				"%pM)' due to age (%lums).\n",
+				escape_essid(network->ssid,
+					     network->ssid_len),
+				network->bssid,
+				(jiffies - network->last_scanned) / (HZ / 100));
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	mutex_unlock(&ieee->wx_mutex);
+	wrqu->data.length = ev -  extra;
+	wrqu->data.flags = 0;
+
+	IEEE80211_DEBUG_WX("exit: %d networks returned.\n", i);
+
+	return err;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_scan);
+
+int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &(wrqu->encoding);
+	struct net_device *dev = ieee->dev;
+	struct ieee80211_security sec = {
+		.flags = 0
+	};
+	int i, key, key_provided, len;
+	struct ieee80211_crypt_data **crypt;
+
+	IEEE80211_DEBUG_WX("SET_ENCODE\n");
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+		key_provided = 1;
+	} else {
+		key_provided = 0;
+		key = ieee->tx_keyidx;
+	}
+
+	IEEE80211_DEBUG_WX("Key: %d [%s]\n", key, key_provided ?
+			   "provided" : "default");
+	crypt = &ieee->crypt[key];
+
+	if (erq->flags & IW_ENCODE_DISABLED) {
+		if (key_provided && *crypt) {
+			IEEE80211_DEBUG_WX("Disabling encryption on key %d.\n",
+					   key);
+			ieee80211_crypt_delayed_deinit(ieee, crypt);
+		} else
+			IEEE80211_DEBUG_WX("Disabling encryption.\n");
+
+		/* Check all the keys to see if any are still configured,
+		 * and if no key index was provided, de-init them all */
+		for (i = 0; i < WEP_KEYS; i++) {
+			if (ieee->crypt[i]) {
+				if (key_provided)
+					break;
+				ieee80211_crypt_delayed_deinit(
+					ieee, &ieee->crypt[i]);
+			}
+		}
+
+		if (i == WEP_KEYS) {
+			sec.enabled = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_ENABLED | SEC_LEVEL;
+		}
+
+		goto done;
+	}
+
+
+
+	sec.enabled = 1;
+	sec.flags |= SEC_ENABLED;
+
+	if (*crypt && (*crypt)->ops &&
+	    strcmp((*crypt)->ops->name, "WEP") != 0) {
+		/* changing to use WEP; deinit previously used algorithm
+		 * on this key */
+		ieee80211_crypt_delayed_deinit(ieee, crypt);
+	}
+
+	if (!*crypt) {
+		struct ieee80211_crypt_data *new_crypt;
+
+		/* take WEP into use */
+		new_crypt = kzalloc(sizeof(struct ieee80211_crypt_data),
+				    GFP_KERNEL);
+		if (!new_crypt)
+			return -ENOMEM;
+		new_crypt->ops = try_then_request_module(ieee80211_get_crypto_ops("WEP"),
+							 "ieee80211_crypt_wep");
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv = new_crypt->ops->init(key);
+
+		if (!new_crypt->ops || !new_crypt->priv) {
+			kfree(new_crypt);
+			new_crypt = NULL;
+
+			netdev_warn(dev, "could not initialize WEP: "
+				    "load module ieee80211_crypt_wep\n");
+			return -EOPNOTSUPP;
+		}
+		*crypt = new_crypt;
+	}
+
+	/* If a new key was provided, set it up */
+	if (erq->length > 0) {
+		len = erq->length <= 5 ? 5 : 13;
+		memcpy(sec.keys[key], keybuf, erq->length);
+		if (len > erq->length)
+			memset(sec.keys[key] + erq->length, 0,
+			       len - erq->length);
+		IEEE80211_DEBUG_WX("Setting key %d to '%s' (%d:%d bytes)\n",
+				   key, escape_essid(sec.keys[key], len),
+				   erq->length, len);
+		sec.key_sizes[key] = len;
+		(*crypt)->ops->set_key(sec.keys[key], len, NULL,
+				       (*crypt)->priv);
+		sec.flags |= BIT(key);
+		/* This ensures a key will be activated if no key is
+		 * explicitly set
+		 */
+		if (key == sec.active_key)
+			sec.flags |= SEC_ACTIVE_KEY;
+		ieee->tx_keyidx = key;
+
+	} else {
+		len = (*crypt)->ops->get_key(sec.keys[key], WEP_KEY_LEN,
+					     NULL, (*crypt)->priv);
+		if (len == 0) {
+			/* Set a default key of all 0 */
+			printk("Setting key %d to all zero.\n",
+					   key);
+
+			IEEE80211_DEBUG_WX("Setting key %d to all zero.\n",
+					   key);
+			memset(sec.keys[key], 0, 13);
+			(*crypt)->ops->set_key(sec.keys[key], 13, NULL,
+					       (*crypt)->priv);
+			sec.key_sizes[key] = 13;
+			sec.flags |= BIT(key);
+		}
+
+		/* No key data - just set the default TX key index */
+		if (key_provided) {
+			IEEE80211_DEBUG_WX(
+				"Setting key %d to default Tx key.\n", key);
+			ieee->tx_keyidx = key;
+			sec.active_key = key;
+			sec.flags |= SEC_ACTIVE_KEY;
+		}
+	}
+
+ done:
+	ieee->open_wep = !(erq->flags & IW_ENCODE_RESTRICTED);
+	ieee->auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+	sec.auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+	sec.flags |= SEC_AUTH_MODE;
+	IEEE80211_DEBUG_WX("Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
+			   "OPEN" : "SHARED KEY");
+
+	/* For now we just support WEP, so only set that security level...
+	 * TODO: When WPA is added this is one place that needs to change */
+	sec.flags |= SEC_LEVEL;
+	sec.level = SEC_LEVEL_1; /* 40 and 104 bit WEP */
+
+	if (ieee->set_security)
+		ieee->set_security(dev, &sec);
+
+	/* Do not reset port if card is in Managed mode since resetting will
+	 * generate new IEEE 802.11 authentication which may end up in looping
+	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
+	 * configuration (for example... Prism2), implement the reset_port in
+	 * the callbacks structures used to initialize the 802.11 stack. */
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port && ieee->reset_port(dev)) {
+		netdev_dbg(ieee->dev, "reset_port failed\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_encode);
+
+int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &(wrqu->encoding);
+	int len, key;
+	struct ieee80211_crypt_data *crypt;
+
+	IEEE80211_DEBUG_WX("GET_ENCODE\n");
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+	} else
+		key = ieee->tx_keyidx;
+
+	crypt = ieee->crypt[key];
+	erq->flags = key + 1;
+
+	if (!crypt || !crypt->ops) {
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		return 0;
+	}
+	len = crypt->ops->get_key(keybuf, SCM_KEY_LEN, NULL, crypt->priv);
+	if (len < 0)
+		len = 0;
+	erq->length = len;
+
+	erq->flags |= IW_ENCODE_ENABLED;
+
+	if (ieee->open_wep)
+		erq->flags |= IW_ENCODE_OPEN;
+	else
+		erq->flags |= IW_ENCODE_RESTRICTED;
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_encode);
+
+int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct net_device *dev = ieee->dev;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int i, idx;
+	int group_key = 0;
+	const char *alg, *module;
+	struct ieee80211_crypto_ops *ops;
+	struct ieee80211_crypt_data **crypt;
+
+	struct ieee80211_security sec = {
+		.flags = 0,
+	};
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx) {
+		if (idx < 1 || idx > WEP_KEYS)
+			return -EINVAL;
+		idx--;
+	} else
+		idx = ieee->tx_keyidx;
+
+	if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
+
+		crypt = &ieee->crypt[idx];
+
+		group_key = 1;
+	} else {
+		/* some Cisco APs use idx>0 for unicast in dynamic WEP */
+		if (idx != 0 && ext->alg != IW_ENCODE_ALG_WEP)
+			return -EINVAL;
+		if (ieee->iw_mode == IW_MODE_INFRA)
+
+			crypt = &ieee->crypt[idx];
+
+		else
+			return -EINVAL;
+	}
+
+	sec.flags |= SEC_ENABLED;// | SEC_ENCRYPT;
+	if ((encoding->flags & IW_ENCODE_DISABLED) ||
+	    ext->alg == IW_ENCODE_ALG_NONE) {
+		if (*crypt)
+			ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+		for (i = 0; i < WEP_KEYS; i++)
+
+			if (ieee->crypt[i])
+
+				break;
+
+		if (i == WEP_KEYS) {
+			sec.enabled = 0;
+		      //  sec.encrypt = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_LEVEL;
+		}
+		goto done;
+	}
+
+	sec.enabled = 1;
+    //    sec.encrypt = 1;
+	switch (ext->alg) {
+	case IW_ENCODE_ALG_WEP:
+		alg = "WEP";
+		module = "ieee80211_crypt_wep";
+		break;
+	case IW_ENCODE_ALG_TKIP:
+		alg = "TKIP";
+		module = "ieee80211_crypt_tkip";
+		break;
+	case IW_ENCODE_ALG_CCMP:
+		alg = "CCMP";
+		module = "ieee80211_crypt_ccmp";
+		break;
+	default:
+		IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+				   dev->name, ext->alg);
+		ret = -EINVAL;
+		goto done;
+	}
+	printk("alg name:%s\n", alg);
+
+	ops = try_then_request_module(ieee80211_get_crypto_ops(alg), module);
+	if (!ops) {
+		IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+				   dev->name, ext->alg);
+		printk("========>unknown crypto alg %d\n", ext->alg);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if (!*crypt || (*crypt)->ops != ops) {
+		struct ieee80211_crypt_data *new_crypt;
+
+		ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
+		if (!new_crypt) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		new_crypt->ops = ops;
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv = new_crypt->ops->init(idx);
+		if (!new_crypt->priv) {
+			kfree(new_crypt);
+			ret = -EINVAL;
+			goto done;
+		}
+		*crypt = new_crypt;
+	}
+
+	if (ext->key_len > 0 && (*crypt)->ops->set_key &&
+	    (*crypt)->ops->set_key(ext->key, ext->key_len, ext->rx_seq,
+				   (*crypt)->priv) < 0) {
+		IEEE80211_DEBUG_WX("%s: key setting failed\n", dev->name);
+		printk("key setting failed\n");
+		ret = -EINVAL;
+		goto done;
+	}
+ //skip_host_crypt:
+	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+		ieee->tx_keyidx = idx;
+		sec.active_key = idx;
+		sec.flags |= SEC_ACTIVE_KEY;
+	}
+
+	if (ext->alg != IW_ENCODE_ALG_NONE) {
+		//memcpy(sec.keys[idx], ext->key, ext->key_len);
+		sec.key_sizes[idx] = ext->key_len;
+		sec.flags |= BIT(idx);
+		if (ext->alg == IW_ENCODE_ALG_WEP) {
+		      //  sec.encode_alg[idx] = SEC_ALG_WEP;
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_1;
+		} else if (ext->alg == IW_ENCODE_ALG_TKIP) {
+		      //  sec.encode_alg[idx] = SEC_ALG_TKIP;
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_2;
+		} else if (ext->alg == IW_ENCODE_ALG_CCMP) {
+		       // sec.encode_alg[idx] = SEC_ALG_CCMP;
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_3;
+		}
+		/* Don't set sec level for group keys. */
+		if (group_key)
+			sec.flags &= ~SEC_LEVEL;
+	}
+done:
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port && ieee->reset_port(dev)) {
+		IEEE80211_DEBUG_WX("%s: reset_port failed\n", dev->name);
+		return -EINVAL;
+	}
+	return ret;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
+
+int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	struct ieee80211_crypt_data *crypt;
+	int idx, max_key_len;
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx) {
+		if (idx < 1 || idx > WEP_KEYS)
+			return -EINVAL;
+		idx--;
+	} else
+		idx = ieee->tx_keyidx;
+
+	if (!(ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) &&
+	    ext->alg != IW_ENCODE_ALG_WEP)
+		if (idx != 0 || ieee->iw_mode != IW_MODE_INFRA)
+			return -EINVAL;
+
+	crypt = ieee->crypt[idx];
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	if (!crypt || !crypt->ops) {
+		ext->alg = IW_ENCODE_ALG_NONE;
+		ext->key_len = 0;
+		encoding->flags |= IW_ENCODE_DISABLED;
+	} else {
+		if (strcmp(crypt->ops->name, "WEP") == 0)
+			ext->alg = IW_ENCODE_ALG_WEP;
+		else if (strcmp(crypt->ops->name, "TKIP") == 0)
+			ext->alg = IW_ENCODE_ALG_TKIP;
+		else if (strcmp(crypt->ops->name, "CCMP") == 0)
+			ext->alg = IW_ENCODE_ALG_CCMP;
+		else
+			return -EINVAL;
+		ext->key_len = crypt->ops->get_key(ext->key, SCM_KEY_LEN, NULL, crypt->priv);
+		encoding->flags |= IW_ENCODE_ENABLED;
+		if (ext->key_len &&
+		    (ext->alg == IW_ENCODE_ALG_TKIP ||
+		     ext->alg == IW_ENCODE_ALG_CCMP))
+			ext->ext_flags |= IW_ENCODE_EXT_TX_SEQ_VALID;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_get_encode_ext);
+
+int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct iw_mlme *mlme = (struct iw_mlme *)extra;
+	switch (mlme->cmd) {
+	case IW_MLME_DEAUTH:
+	case IW_MLME_DISASSOC:
+		ieee80211_disassociate(ieee);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_mlme);
+
+int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       struct iw_param *data, char *extra)
+{
+	switch (data->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_WPA_VERSION:
+	     /*need to support wpa2 here*/
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+	case IW_AUTH_CIPHER_GROUP:
+	case IW_AUTH_KEY_MGMT:
+		/*
+ *                  * Host AP driver does not use these parameters and allows
+ *                                   * wpa_supplicant to control them internally.
+ *                                                    */
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		ieee->tkip_countermeasures = data->value;
+		break;
+	case IW_AUTH_DROP_UNENCRYPTED:
+		ieee->drop_unencrypted = data->value;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		//printk("======>%s():data->value is %d\n",__func__,data->value);
+	//	ieee->open_wep = (data->value&IW_AUTH_ALG_OPEN_SYSTEM)?1:0;
+		if (data->value & IW_AUTH_ALG_SHARED_KEY) {
+			ieee->open_wep = 0;
+			ieee->auth_mode = 1;
+		} else if (data->value & IW_AUTH_ALG_OPEN_SYSTEM) {
+			ieee->open_wep = 1;
+			ieee->auth_mode = 0;
+		} else if (data->value & IW_AUTH_ALG_LEAP) {
+			ieee->open_wep = 1;
+			ieee->auth_mode = 2;
+		} else
+			return -EINVAL;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		ieee->wpa_enabled = (data->value) ? 1 : 0;
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		ieee->ieee802_1x = data->value;
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		ieee->privacy_invoked = data->value;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_auth);
+
+int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
+{
+	u8 *buf;
+
+	if (len > MAX_WPA_IE_LEN || (len && !ie)) {
+		//printk("return error out, len:%d\n", len);
+		return -EINVAL;
+	}
+
+
+	if (len) {
+		if (len != ie[1] + 2) {
+			printk("len:%zu, ie:%d\n", len, ie[1]);
+			return -EINVAL;
+		}
+		buf = kmemdup(ie, len, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = len;
+	} else {
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ieee80211_wx_set_gen_ie);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h
new file mode 100644
index 000000000..1a727856b
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _BATYPE_H_
+#define _BATYPE_H_
+
+#define	BA_SETUP_TIMEOUT	       200
+
+#define	BA_POLICY_DELAYED		 0
+#define	BA_POLICY_IMMEDIATE	         1
+
+#define	ADDBA_STATUS_SUCCESS		 0
+#define	ADDBA_STATUS_REFUSED		37
+#define	ADDBA_STATUS_INVALID_PARAM	38
+
+#define	DELBA_REASON_END_BA		37
+#define	DELBA_REASON_UNKNOWN_BA	        38
+#define	DELBA_REASON_TIMEOUT		39
+
+union sequence_control {
+	u16 short_data;
+	struct {
+		u16	frag_num:4;
+		u16	seq_num:12;
+	} field;
+};
+
+union ba_param_set {
+	u16 short_data;
+	struct {
+		u16	amsdu_support:1;
+		u16	ba_policy:1;
+		u16	tid:4;
+		u16	buffer_size:10;
+	} field;
+};
+
+union delba_param_set {
+	u16 short_data;
+	struct {
+		u16	reserved:11;
+		u16	initiator:1;
+		u16	tid:4;
+	} field;
+};
+
+struct ba_record {
+	struct timer_list	timer;
+	u8			valid;
+	u8			dialog_token;
+	union ba_param_set	param_set;
+	u16			timeout_value;
+	union sequence_control	start_seq_ctrl;
+};
+
+#endif //end _BATYPE_H_
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
new file mode 100644
index 000000000..6823e405e
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -0,0 +1,700 @@
+// SPDX-License-Identifier: GPL-2.0
+/********************************************************************************************************************************
+ * This file is created to process BA Action Frame. According to 802.11 spec, there are 3 BA action types at all. And as BA is
+ * related to TS, this part need some structure defined in QOS side code. Also TX RX is going to be resturctured, so how to send
+ * ADDBAREQ ADDBARSP and DELBA packet is still on consideration. Temporarily use MANAGE QUEUE instead of Normal Queue.
+ * WB 2008-05-27
+ * *****************************************************************************************************************************/
+#include <asm/byteorder.h>
+#include <asm/unaligned.h>
+#include "ieee80211.h"
+#include "rtl819x_BA.h"
+
+/********************************************************************************************************************
+ *function:  Activate BA entry. And if Time is nozero, start timer.
+ *   input:  struct ba_record          *pBA  //BA entry to be enabled
+ *	     u16			Time //indicate time delay.
+ *  output:  none
+ ********************************************************************************************************************/
+static void ActivateBAEntry(struct ieee80211_device *ieee, struct ba_record *pBA, u16 Time)
+{
+	pBA->valid = true;
+	if (Time != 0)
+		mod_timer(&pBA->timer, jiffies + msecs_to_jiffies(Time));
+}
+
+/********************************************************************************************************************
+ *function:  deactivate BA entry, including its timer.
+ *   input:  struct ba_record       *pBA  //BA entry to be disabled
+ *  output:  none
+ ********************************************************************************************************************/
+static void DeActivateBAEntry(struct ieee80211_device *ieee, struct ba_record *pBA)
+{
+	pBA->valid = false;
+	del_timer_sync(&pBA->timer);
+}
+/********************************************************************************************************************
+ *function: deactivete BA entry in Tx Ts, and send DELBA.
+ *   input:
+ *	     struct tx_ts_record *pTxTs //Tx Ts which is to deactivate BA entry.
+ *  output:  none
+ *  notice:  As struct tx_ts_record * structure will be defined in QOS, so wait to be merged. //FIXME
+ ********************************************************************************************************************/
+static u8 TxTsDeleteBA(struct ieee80211_device *ieee, struct tx_ts_record *pTxTs)
+{
+	struct ba_record *pAdmittedBa = &pTxTs->tx_admitted_ba_record;  //These two BA entries must exist in TS structure
+	struct ba_record *pPendingBa = &pTxTs->tx_pending_ba_record;
+	u8			bSendDELBA = false;
+
+	// Delete pending BA
+	if (pPendingBa->valid) {
+		DeActivateBAEntry(ieee, pPendingBa);
+		bSendDELBA = true;
+	}
+
+	// Delete admitted BA
+	if (pAdmittedBa->valid) {
+		DeActivateBAEntry(ieee, pAdmittedBa);
+		bSendDELBA = true;
+	}
+
+	return bSendDELBA;
+}
+
+/********************************************************************************************************************
+ *function: deactivete BA entry in Tx Ts, and send DELBA.
+ *   input:
+ *	     struct rx_ts_record  *pRxTs //Rx Ts which is to deactivate BA entry.
+ *  output:  none
+ *  notice:  As struct rx_ts_record * structure will be defined in QOS, so wait to be merged. //FIXME, same with above
+ ********************************************************************************************************************/
+static u8 RxTsDeleteBA(struct ieee80211_device *ieee, struct rx_ts_record *pRxTs)
+{
+	struct ba_record       *pBa = &pRxTs->rx_admitted_ba_record;
+	u8			bSendDELBA = false;
+
+	if (pBa->valid) {
+		DeActivateBAEntry(ieee, pBa);
+		bSendDELBA = true;
+	}
+
+	return bSendDELBA;
+}
+
+/********************************************************************************************************************
+ *function: reset BA entry
+ *   input:
+ *	     struct ba_record *pBA //entry to be reset
+ *  output:  none
+ ********************************************************************************************************************/
+void ResetBaEntry(struct ba_record *pBA)
+{
+	pBA->valid			= false;
+	pBA->param_set.short_data	= 0;
+	pBA->timeout_value		= 0;
+	pBA->dialog_token		= 0;
+	pBA->start_seq_ctrl.short_data	= 0;
+}
+//These functions need porting here or not?
+/*******************************************************************************************************************************
+ *function:  construct ADDBAREQ and ADDBARSP frame here together.
+ *   input:  u8*		Dst	//ADDBA frame's destination
+ *	     struct ba_record  *pBA	//BA_RECORD entry which stores the necessary information for BA.
+ *	     u16		StatusCode  //status code in RSP and I will use it to indicate whether it's RSP or REQ(will I?)
+ *	     u8			type	//indicate whether it's RSP(ACT_ADDBARSP) ow REQ(ACT_ADDBAREQ)
+ *  output:  none
+ *  return:  sk_buff*		skb     //return constructed skb to xmit
+ *******************************************************************************************************************************/
+static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, struct ba_record *pBA, u16 StatusCode, u8 type)
+{
+	struct sk_buff *skb = NULL;
+	struct rtl_80211_hdr_3addr *BAReq = NULL;
+	u8 *tag = NULL;
+	u16 len = ieee->tx_headroom + 9;
+	//category(1) + action field(1) + Dialog Token(1) + BA Parameter Set(2) +  BA Timeout Value(2) +  BA Start SeqCtrl(2)(or StatusCode(2))
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __func__, type, Dst, ieee->dev);
+	if (pBA == NULL) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "pBA is NULL\n");
+		return NULL;
+	}
+	skb = dev_alloc_skb(len + sizeof(struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
+	if (!skb)
+		return NULL;
+
+	memset(skb->data, 0, sizeof(struct rtl_80211_hdr_3addr));	//I wonder whether it's necessary. Apparently kernel will not do it when alloc a skb.
+	skb_reserve(skb, ieee->tx_headroom);
+
+	BAReq = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
+
+	memcpy(BAReq->addr1, Dst, ETH_ALEN);
+	memcpy(BAReq->addr2, ieee->dev->dev_addr, ETH_ALEN);
+
+	memcpy(BAReq->addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	BAReq->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame
+
+	//tag += sizeof( struct rtl_80211_hdr_3addr); //move to action field
+	tag = skb_put(skb, 9);
+	*tag++ = ACT_CAT_BA;
+	*tag++ = type;
+	// Dialog Token
+	*tag++ = pBA->dialog_token;
+
+	if (type == ACT_ADDBARSP) {
+		// Status Code
+		netdev_info(ieee->dev, "=====>to send ADDBARSP\n");
+
+		put_unaligned_le16(StatusCode, tag);
+		tag += 2;
+	}
+	// BA Parameter Set
+
+	put_unaligned_le16(pBA->param_set.short_data, tag);
+	tag += 2;
+	// BA Timeout Value
+
+	put_unaligned_le16(pBA->timeout_value, tag);
+	tag += 2;
+
+	if (type == ACT_ADDBAREQ) {
+	// BA Start SeqCtrl
+		memcpy(tag, (u8 *)&(pBA->start_seq_ctrl), 2);
+		tag += 2;
+	}
+
+	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_BA, skb->data, skb->len);
+	return skb;
+	//return NULL;
+}
+
+
+/********************************************************************************************************************
+ *function:  construct DELBA frame
+ *   input:  u8*		dst	//DELBA frame's destination
+ *	     struct ba_record  *pBA	//BA_RECORD entry which stores the necessary information for BA
+ *	     enum tr_select	TxRxSelect  //TX RX direction
+ *	     u16		ReasonCode  //status code.
+ *  output:  none
+ *  return:  sk_buff*		skb     //return constructed skb to xmit
+ ********************************************************************************************************************/
+static struct sk_buff *ieee80211_DELBA(
+	struct ieee80211_device  *ieee,
+	u8		         *dst,
+	struct ba_record         *pBA,
+	enum tr_select		 TxRxSelect,
+	u16			 ReasonCode
+	)
+{
+	union delba_param_set	DelbaParamSet;
+	struct sk_buff *skb = NULL;
+	struct rtl_80211_hdr_3addr *Delba = NULL;
+	u8 *tag = NULL;
+	//len = head len + DELBA Parameter Set(2) + Reason Code(2)
+	u16 len = 6 + ieee->tx_headroom;
+
+	if (net_ratelimit())
+		IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA,
+				"========>%s(), ReasonCode(%d) sentd to:%pM\n",
+				__func__, ReasonCode, dst);
+
+	memset(&DelbaParamSet, 0, 2);
+
+	DelbaParamSet.field.initiator	= (TxRxSelect == TX_DIR) ? 1 : 0;
+	DelbaParamSet.field.tid	= pBA->param_set.field.tid;
+
+	skb = dev_alloc_skb(len + sizeof(struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
+	if (!skb)
+		return NULL;
+//	memset(skb->data, 0, len+sizeof( struct rtl_80211_hdr_3addr));
+	skb_reserve(skb, ieee->tx_headroom);
+
+	Delba = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
+
+	memcpy(Delba->addr1, dst, ETH_ALEN);
+	memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(Delba->addr3, ieee->current_network.bssid, ETH_ALEN);
+	Delba->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame
+
+	tag = skb_put(skb, 6);
+
+	*tag++ = ACT_CAT_BA;
+	*tag++ = ACT_DELBA;
+
+	// DELBA Parameter Set
+
+	put_unaligned_le16(DelbaParamSet.short_data, tag);
+	tag += 2;
+	// Reason Code
+
+	put_unaligned_le16(ReasonCode, tag);
+	tag += 2;
+
+	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_BA, skb->data, skb->len);
+	if (net_ratelimit())
+		IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA,
+				"<=====%s()\n", __func__);
+	return skb;
+}
+
+/********************************************************************************************************************
+ *function: send ADDBAReq frame out
+ *   input:  u8*		dst	//ADDBAReq frame's destination
+ *	     struct ba_record  *pBA	//BA_RECORD entry which stores the necessary information for BA
+ *  output:  none
+ *  notice: If any possible, please hide pBA in ieee. And temporarily use Manage Queue as softmac_mgmt_xmit() usually does
+ ********************************************************************************************************************/
+static void ieee80211_send_ADDBAReq(struct ieee80211_device *ieee,
+				    u8 *dst, struct ba_record *pBA)
+{
+	struct sk_buff *skb;
+	skb = ieee80211_ADDBA(ieee, dst, pBA, 0, ACT_ADDBAREQ); //construct ACT_ADDBAREQ frames so set statuscode zero.
+
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		//add statistic needed here.
+		//and skb will be freed in softmac_mgmt_xmit(), so omit all dev_kfree_skb_any() outside softmac_mgmt_xmit()
+		//WB
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
+	}
+}
+
+/********************************************************************************************************************
+ *function: send ADDBARSP frame out
+ *   input:  u8*		dst	//DELBA frame's destination
+ *	     struct ba_record  *pBA	//BA_RECORD entry which stores the necessary information for BA
+ *	     u16		StatusCode //RSP StatusCode
+ *  output:  none
+ *  notice: If any possible, please hide pBA in ieee. And temporarily use Manage Queue as softmac_mgmt_xmit() usually does
+ ********************************************************************************************************************/
+static void ieee80211_send_ADDBARsp(struct ieee80211_device *ieee, u8 *dst,
+				    struct ba_record *pBA, u16 StatusCode)
+{
+	struct sk_buff *skb;
+	skb = ieee80211_ADDBA(ieee, dst, pBA, StatusCode, ACT_ADDBARSP); //construct ACT_ADDBARSP frames
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		//same above
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
+	}
+
+	return;
+
+}
+/********************************************************************************************************************
+ *function: send ADDBARSP frame out
+ *   input:  u8*		dst	//DELBA frame's destination
+ *	     struct ba_record  *pBA	//BA_RECORD entry which stores the necessary information for BA
+ *	     enum tr_select     TxRxSelect //TX or RX
+ *	     u16		ReasonCode //DEL ReasonCode
+ *  output:  none
+ *  notice: If any possible, please hide pBA in ieee. And temporarily use Manage Queue as softmac_mgmt_xmit() usually does
+ ********************************************************************************************************************/
+
+static void ieee80211_send_DELBA(struct ieee80211_device *ieee, u8 *dst,
+				 struct ba_record *pBA, enum tr_select TxRxSelect,
+				 u16 ReasonCode)
+{
+	struct sk_buff *skb;
+	skb = ieee80211_DELBA(ieee, dst, pBA, TxRxSelect, ReasonCode); //construct ACT_ADDBARSP frames
+	if (skb) {
+		softmac_mgmt_xmit(skb, ieee);
+		//same above
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
+	}
+}
+
+/********************************************************************************************************************
+ *function: RX ADDBAReq
+ *   input:  struct sk_buff *   skb	//incoming ADDBAReq skb.
+ *  return:  0(pass), other(fail)
+ *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
+ ********************************************************************************************************************/
+int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	struct rtl_80211_hdr_3addr *req = NULL;
+	u16 rc = 0;
+	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
+	struct ba_record *pBA = NULL;
+	union ba_param_set     *pBaParamSet = NULL;
+	u16 *pBaTimeoutVal = NULL;
+	union sequence_control *pBaStartSeqCtrl = NULL;
+	struct rx_ts_record  *pTS = NULL;
+
+	if (skb->len < sizeof(struct rtl_80211_hdr_3addr) + 9) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in BAREQ(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct rtl_80211_hdr_3addr) + 9));
+		return -1;
+	}
+
+	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_BA, skb->data, skb->len);
+
+	req = (struct rtl_80211_hdr_3addr *)skb->data;
+	tag = (u8 *)req;
+	dst = &req->addr2[0];
+	tag += sizeof(struct rtl_80211_hdr_3addr);
+	pDialogToken = tag + 2;  //category+action
+	pBaParamSet = (union ba_param_set *)(tag + 3);   //+DialogToken
+	pBaTimeoutVal = (u16 *)(tag + 5);
+	pBaStartSeqCtrl = (union sequence_control *)(req + 7);
+
+	netdev_info(ieee->dev, "====================>rx ADDBAREQ from :%pM\n", dst);
+//some other capability is not ready now.
+	if ((ieee->current_network.qos_data.active == 0) ||
+		(!ieee->pHTInfo->bCurrentHTSupport)) //||
+	//	(!ieee->pStaQos->bEnableRxImmBA)	)
+	{
+		rc = ADDBA_STATUS_REFUSED;
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "Failed to reply on ADDBA_REQ as some capability is not ready(%d, %d)\n", ieee->current_network.qos_data.active, ieee->pHTInfo->bCurrentHTSupport);
+		goto OnADDBAReq_Fail;
+	}
+	// Search for related traffic stream.
+	// If there is no matched TS, reject the ADDBA request.
+	if (!GetTs(
+			ieee,
+			(struct ts_common_info **)(&pTS),
+			dst,
+			(u8)(pBaParamSet->field.tid),
+			RX_DIR,
+			true)) {
+		rc = ADDBA_STATUS_REFUSED;
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
+		goto OnADDBAReq_Fail;
+	}
+	pBA = &pTS->rx_admitted_ba_record;
+	// To Determine the ADDBA Req content
+	// We can do much more check here, including buffer_size, AMSDU_Support, Policy, StartSeqCtrl...
+	// I want to check StartSeqCtrl to make sure when we start aggregation!!!
+	//
+	if (pBaParamSet->field.ba_policy == BA_POLICY_DELAYED) {
+		rc = ADDBA_STATUS_INVALID_PARAM;
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __func__);
+		goto OnADDBAReq_Fail;
+	}
+		// Admit the ADDBA Request
+	//
+	DeActivateBAEntry(ieee, pBA);
+	pBA->dialog_token = *pDialogToken;
+	pBA->param_set = *pBaParamSet;
+	pBA->timeout_value = *pBaTimeoutVal;
+	pBA->start_seq_ctrl = *pBaStartSeqCtrl;
+	//for half N mode we only aggregate 1 frame
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		pBA->param_set.field.buffer_size = 1;
+	else
+		pBA->param_set.field.buffer_size = 32;
+	ActivateBAEntry(ieee, pBA, pBA->timeout_value);
+	ieee80211_send_ADDBARsp(ieee, dst, pBA, ADDBA_STATUS_SUCCESS);
+
+	// End of procedure.
+	return 0;
+
+OnADDBAReq_Fail:
+	{
+		struct ba_record	BA;
+		BA.param_set = *pBaParamSet;
+		BA.timeout_value = *pBaTimeoutVal;
+		BA.dialog_token = *pDialogToken;
+		BA.param_set.field.ba_policy = BA_POLICY_IMMEDIATE;
+		ieee80211_send_ADDBARsp(ieee, dst, &BA, rc);
+		return 0; //we send RSP out.
+	}
+
+}
+
+/********************************************************************************************************************
+ *function: RX ADDBARSP
+ *   input:  struct sk_buff *   skb	//incoming ADDBAReq skb.
+ *  return:  0(pass), other(fail)
+ *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
+ ********************************************************************************************************************/
+int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	struct rtl_80211_hdr_3addr *rsp = NULL;
+	struct ba_record        *pPendingBA, *pAdmittedBA;
+	struct tx_ts_record     *pTS = NULL;
+	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
+	u16 *pStatusCode = NULL, *pBaTimeoutVal = NULL;
+	union ba_param_set       *pBaParamSet = NULL;
+	u16			ReasonCode;
+
+	if (skb->len < sizeof(struct rtl_80211_hdr_3addr) + 9) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in BARSP(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct rtl_80211_hdr_3addr) + 9));
+		return -1;
+	}
+	rsp = (struct rtl_80211_hdr_3addr *)skb->data;
+	tag = (u8 *)rsp;
+	dst = &rsp->addr2[0];
+	tag += sizeof(struct rtl_80211_hdr_3addr);
+	pDialogToken = tag + 2;
+	pStatusCode = (u16 *)(tag + 3);
+	pBaParamSet = (union ba_param_set *)(tag + 5);
+	pBaTimeoutVal = (u16 *)(tag + 7);
+
+	// Check the capability
+	// Since we can always receive A-MPDU, we just check if it is under HT mode.
+	if (ieee->current_network.qos_data.active == 0  ||
+	    !ieee->pHTInfo->bCurrentHTSupport ||
+	    !ieee->pHTInfo->bCurrentAMPDUEnable) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "reject to ADDBA_RSP as some capability is not ready(%d, %d, %d)\n", ieee->current_network.qos_data.active, ieee->pHTInfo->bCurrentHTSupport, ieee->pHTInfo->bCurrentAMPDUEnable);
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	}
+
+
+	//
+	// Search for related TS.
+	// If there is no TS found, we wil reject ADDBA Rsp by sending DELBA frame.
+	//
+	if (!GetTs(
+			ieee,
+			(struct ts_common_info **)(&pTS),
+			dst,
+			(u8)(pBaParamSet->field.tid),
+			TX_DIR,
+			false)) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	}
+
+	pTS->add_ba_req_in_progress = false;
+	pPendingBA = &pTS->tx_pending_ba_record;
+	pAdmittedBA = &pTS->tx_admitted_ba_record;
+
+
+	//
+	// Check if related BA is waiting for setup.
+	// If not, reject by sending DELBA frame.
+	//
+	if (pAdmittedBA->valid) {
+		// Since BA is already setup, we ignore all other ADDBA Response.
+		IEEE80211_DEBUG(IEEE80211_DL_BA, "OnADDBARsp(): Recv ADDBA Rsp. Drop because already admit it! \n");
+		return -1;
+	} else if ((!pPendingBA->valid) || (*pDialogToken != pPendingBA->dialog_token)) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,  "OnADDBARsp(): Recv ADDBA Rsp. BA invalid, DELBA! \n");
+		ReasonCode = DELBA_REASON_UNKNOWN_BA;
+		goto OnADDBARsp_Reject;
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_BA, "OnADDBARsp(): Recv ADDBA Rsp. BA is admitted! Status code:%X\n", *pStatusCode);
+		DeActivateBAEntry(ieee, pPendingBA);
+	}
+
+
+	if (*pStatusCode == ADDBA_STATUS_SUCCESS) {
+		//
+		// Determine ADDBA Rsp content here.
+		// We can compare the value of BA parameter set that Peer returned and Self sent.
+		// If it is OK, then admitted. Or we can send DELBA to cancel BA mechanism.
+		//
+		if (pBaParamSet->field.ba_policy == BA_POLICY_DELAYED) {
+			// Since this is a kind of ADDBA failed, we delay next ADDBA process.
+			pTS->add_ba_req_delayed = true;
+			DeActivateBAEntry(ieee, pAdmittedBA);
+			ReasonCode = DELBA_REASON_END_BA;
+			goto OnADDBARsp_Reject;
+		}
+
+
+		//
+		// Admitted condition
+		//
+		pAdmittedBA->dialog_token = *pDialogToken;
+		pAdmittedBA->timeout_value = *pBaTimeoutVal;
+		pAdmittedBA->start_seq_ctrl = pPendingBA->start_seq_ctrl;
+		pAdmittedBA->param_set = *pBaParamSet;
+		DeActivateBAEntry(ieee, pAdmittedBA);
+		ActivateBAEntry(ieee, pAdmittedBA, *pBaTimeoutVal);
+	} else {
+		// Delay next ADDBA process.
+		pTS->add_ba_req_delayed = true;
+	}
+
+	// End of procedure
+	return 0;
+
+OnADDBARsp_Reject:
+	{
+		struct ba_record	BA;
+		BA.param_set = *pBaParamSet;
+		ieee80211_send_DELBA(ieee, dst, &BA, TX_DIR, ReasonCode);
+		return 0;
+	}
+
+}
+
+/********************************************************************************************************************
+ *function: RX DELBA
+ *   input:  struct sk_buff *   skb	//incoming ADDBAReq skb.
+ *  return:  0(pass), other(fail)
+ *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
+ ********************************************************************************************************************/
+int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	struct rtl_80211_hdr_3addr *delba = NULL;
+	union delba_param_set   *pDelBaParamSet = NULL;
+	u8			*dst = NULL;
+
+	if (skb->len < sizeof(struct rtl_80211_hdr_3addr) + 6) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in DELBA(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct rtl_80211_hdr_3addr) + 6));
+		return -1;
+	}
+
+	if (ieee->current_network.qos_data.active == 0 ||
+	    !ieee->pHTInfo->bCurrentHTSupport) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "received DELBA while QOS or HT is not supported(%d, %d)\n", ieee->current_network.qos_data.active, ieee->pHTInfo->bCurrentHTSupport);
+		return -1;
+	}
+
+	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_BA, skb->data, skb->len);
+	delba = (struct rtl_80211_hdr_3addr *)skb->data;
+	dst = &delba->addr2[0];
+	pDelBaParamSet = (union delba_param_set *)&delba->payload[2];
+
+	if (pDelBaParamSet->field.initiator == 1) {
+		struct rx_ts_record *pRxTs;
+
+		if (!GetTs(
+				ieee,
+				(struct ts_common_info **)&pRxTs,
+				dst,
+				(u8)pDelBaParamSet->field.tid,
+				RX_DIR,
+				false)) {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for RXTS in %s()\n", __func__);
+			return -1;
+		}
+
+		RxTsDeleteBA(ieee, pRxTs);
+	} else {
+		struct tx_ts_record *pTxTs;
+
+		if (!GetTs(
+			ieee,
+			(struct ts_common_info **)&pTxTs,
+			dst,
+			(u8)pDelBaParamSet->field.tid,
+			TX_DIR,
+			false)) {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for TXTS in %s()\n", __func__);
+			return -1;
+		}
+
+		pTxTs->using_ba = false;
+		pTxTs->add_ba_req_in_progress = false;
+		pTxTs->add_ba_req_delayed = false;
+		del_timer_sync(&pTxTs->ts_add_ba_timer);
+		//PlatformCancelTimer(Adapter, &pTxTs->ts_add_ba_timer);
+		TxTsDeleteBA(ieee, pTxTs);
+	}
+	return 0;
+}
+
+//
+// ADDBA initiate. This can only be called by TX side.
+//
+void
+TsInitAddBA(
+	struct ieee80211_device *ieee,
+	struct tx_ts_record     *pTS,
+	u8		Policy,
+	u8		bOverwritePending
+	)
+{
+	struct ba_record *pBA = &pTS->tx_pending_ba_record;
+
+	if (pBA->valid && !bOverwritePending)
+		return;
+
+	// Set parameters to "Pending" variable set
+	DeActivateBAEntry(ieee, pBA);
+
+	pBA->dialog_token++;						// DialogToken: Only keep the latest dialog token
+	pBA->param_set.field.amsdu_support = 0;	// Do not support A-MSDU with A-MPDU now!!
+	pBA->param_set.field.ba_policy = Policy;	// Policy: Delayed or Immediate
+	pBA->param_set.field.tid = pTS->ts_common_info.t_spec.ts_info.uc_tsid;	// TID
+	// buffer_size: This need to be set according to A-MPDU vector
+	pBA->param_set.field.buffer_size = 32;		// buffer_size: This need to be set according to A-MPDU vector
+	pBA->timeout_value = 0;					// Timeout value: Set 0 to disable Timer
+	pBA->start_seq_ctrl.field.seq_num = (pTS->tx_cur_seq + 3) % 4096;	// Block Ack will start after 3 packets later.
+
+	ActivateBAEntry(ieee, pBA, BA_SETUP_TIMEOUT);
+
+	ieee80211_send_ADDBAReq(ieee, pTS->ts_common_info.addr, pBA);
+}
+
+void
+TsInitDelBA(struct ieee80211_device *ieee, struct ts_common_info *pTsCommonInfo, enum tr_select TxRxSelect)
+{
+	if (TxRxSelect == TX_DIR) {
+		struct tx_ts_record *pTxTs = (struct tx_ts_record *)pTsCommonInfo;
+
+		if (TxTsDeleteBA(ieee, pTxTs))
+			ieee80211_send_DELBA(
+				ieee,
+				pTsCommonInfo->addr,
+				(pTxTs->tx_admitted_ba_record.valid) ? (&pTxTs->tx_admitted_ba_record) : (&pTxTs->tx_pending_ba_record),
+				TxRxSelect,
+				DELBA_REASON_END_BA);
+	} else if (TxRxSelect == RX_DIR) {
+		struct rx_ts_record *pRxTs = (struct rx_ts_record *)pTsCommonInfo;
+		if (RxTsDeleteBA(ieee, pRxTs))
+			ieee80211_send_DELBA(
+				ieee,
+				pTsCommonInfo->addr,
+				&pRxTs->rx_admitted_ba_record,
+				TxRxSelect,
+				DELBA_REASON_END_BA);
+	}
+}
+/********************************************************************************************************************
+ *function:  BA setup timer
+ *   input:  unsigned long	 data		//acturally we send struct tx_ts_record or struct rx_ts_record to these timer
+ *  return:  NULL
+ *  notice:
+ ********************************************************************************************************************/
+void BaSetupTimeOut(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t, tx_pending_ba_record.timer);
+
+	pTxTs->add_ba_req_in_progress = false;
+	pTxTs->add_ba_req_delayed = true;
+	pTxTs->tx_pending_ba_record.valid = false;
+}
+
+void TxBaInactTimeout(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t, tx_admitted_ba_record.timer);
+	struct ieee80211_device *ieee = container_of(pTxTs, struct ieee80211_device, TxTsRecord[pTxTs->num]);
+	TxTsDeleteBA(ieee, pTxTs);
+	ieee80211_send_DELBA(
+		ieee,
+		pTxTs->ts_common_info.addr,
+		&pTxTs->tx_admitted_ba_record,
+		TX_DIR,
+		DELBA_REASON_TIMEOUT);
+}
+
+void RxBaInactTimeout(struct timer_list *t)
+{
+	struct rx_ts_record *pRxTs = from_timer(pRxTs, t, rx_admitted_ba_record.timer);
+	struct ieee80211_device *ieee = container_of(pRxTs, struct ieee80211_device, RxTsRecord[pRxTs->num]);
+
+	RxTsDeleteBA(ieee, pRxTs);
+	ieee80211_send_DELBA(
+		ieee,
+		pRxTs->ts_common_info.addr,
+		&pRxTs->rx_admitted_ba_record,
+		RX_DIR,
+		DELBA_REASON_TIMEOUT);
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
new file mode 100644
index 000000000..79346a00a
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
@@ -0,0 +1,302 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _RTL819XU_HTTYPE_H_
+#define _RTL819XU_HTTYPE_H_
+
+/*
+ * The HT Capability element is present in beacons, association request,
+ * reassociation request and probe response frames
+ */
+
+/*
+ * MIMO Power Save Settings
+ */
+#define MIMO_PS_STATIC				0
+
+/*
+ * There should be 128 bits to cover all of the MCS rates. However, since
+ * 8190 does not support too much rates, one integer is quite enough.
+ */
+#define HTCLNG	4
+
+/*
+ * Represent Channel Width in HT Capabilities
+ */
+enum ht_channel_width {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_20_40 = 1,
+};
+
+/*
+ * Represent Extension Channel Offset in HT Capabilities
+ * This is available only in 40Mhz mode.
+ */
+enum ht_extension_chan_offset {
+	HT_EXTCHNL_OFFSET_NO_EXT = 0,
+	HT_EXTCHNL_OFFSET_UPPER = 1,
+	HT_EXTCHNL_OFFSET_NO_DEF = 2,
+	HT_EXTCHNL_OFFSET_LOWER = 3,
+};
+
+struct ht_capability_ele {
+	//HT capability info
+	u8	AdvCoding:1;
+	u8	ChlWidth:1;
+	u8	MimoPwrSave:2;
+	u8	GreenField:1;
+	u8	ShortGI20Mhz:1;
+	u8	ShortGI40Mhz:1;
+	u8	TxSTBC:1;
+	u8	RxSTBC:2;
+	u8	DelayBA:1;
+	u8	MaxAMSDUSize:1;
+	u8	DssCCk:1;
+	u8	PSMP:1;
+	u8	Rsvd1:1;
+	u8	LSigTxopProtect:1;
+
+	//MAC HT parameters info
+	u8	MaxRxAMPDUFactor:2;
+	u8	MPDUDensity:3;
+	u8	Rsvd2:3;
+
+	//Supported MCS set
+	u8	MCS[16];
+
+	//Extended HT Capability Info
+	u16	ExtHTCapInfo;
+
+	//TXBF Capabilities
+	u8	TxBFCap[4];
+
+	//Antenna Selection Capabilities
+	u8	ASCap;
+
+} __packed;
+
+/*
+ * The HT Information element is present in beacons
+ * Only AP is required to include this element
+ */
+typedef struct _HT_INFORMATION_ELE {
+	u8	ControlChl;
+
+	u8	ExtChlOffset:2;
+	u8	RecommemdedTxWidth:1;
+	u8	RIFS:1;
+	u8	PSMPAccessOnly:1;
+	u8	SrvIntGranularity:3;
+
+	u8	OptMode:2;
+	u8	NonGFDevPresent:1;
+	u8	Revd1:5;
+	u8	Revd2:8;
+
+	u8	Rsvd3:6;
+	u8	DualBeacon:1;
+	u8	DualCTSProtect:1;
+
+	u8	SecondaryBeacon:1;
+	u8	LSigTxopProtectFull:1;
+	u8	PcoActive:1;
+	u8	PcoPhase:1;
+	u8	Rsvd4:4;
+
+	u8	BasicMSC[16];
+} __attribute__ ((packed)) HT_INFORMATION_ELE, *PHT_INFORMATION_ELE;
+
+typedef enum _HT_SPEC_VER {
+	HT_SPEC_VER_IEEE = 0,
+	HT_SPEC_VER_EWC = 1,
+} HT_SPEC_VER, *PHT_SPEC_VER;
+
+typedef enum _HT_AGGRE_MODE_E {
+	HT_AGG_AUTO = 0,
+	HT_AGG_FORCE_ENABLE = 1,
+	HT_AGG_FORCE_DISABLE = 2,
+} HT_AGGRE_MODE_E, *PHT_AGGRE_MODE_E;
+
+/*
+ *  The Data structure is used to keep HT related variables when card is
+ *  configured as non-AP STA mode.  **Note**  Current_xxx should be set
+ *  to default value in HTInitializeHTInfo()
+ */
+typedef struct _RT_HIGH_THROUGHPUT {
+	u8				bEnableHT;
+	u8				bCurrentHTSupport;
+
+	u8				bRegBW40MHz;				// Tx 40MHz channel capability
+	u8				bCurBW40MHz;				// Tx 40MHz channel capability
+
+	u8				bRegShortGI40MHz;			// Tx Short GI for 40Mhz
+	u8				bCurShortGI40MHz;			// Tx Short GI for 40MHz
+
+	u8				bRegShortGI20MHz;			// Tx Short GI for 20MHz
+	u8				bCurShortGI20MHz;			// Tx Short GI for 20MHz
+
+	u8				bRegSuppCCK;				// Tx CCK rate capability
+	u8				bCurSuppCCK;				// Tx CCK rate capability
+
+	// 802.11n spec version for "peer"
+	HT_SPEC_VER			ePeerHTSpecVer;
+
+	// HT related information for "Self"
+	struct ht_capability_ele	SelfHTCap;		// This is HT cap element sent to peer STA, which also indicate HT Rx capabilities.
+	HT_INFORMATION_ELE	SelfHTInfo;		// This is HT info element sent to peer STA, which also indicate HT Rx capabilities.
+
+	// HT related information for "Peer"
+	u8				PeerHTCapBuf[32];
+	u8				PeerHTInfoBuf[32];
+
+	// A-MSDU related
+	u8				bAMSDU_Support;			// This indicates Tx A-MSDU capability
+	u16				nAMSDU_MaxSize;			// This indicates Tx A-MSDU capability
+	u8				bCurrent_AMSDU_Support;	// This indicates Tx A-MSDU capability
+	u16				nCurrent_AMSDU_MaxSize;	// This indicates Tx A-MSDU capability
+
+	// AMPDU  related <2006.08.10 Emily>
+	u8				bAMPDUEnable;				// This indicate Tx A-MPDU capability
+	u8				bCurrentAMPDUEnable;		// This indicate Tx A-MPDU capability
+	u8				AMPDU_Factor;				// This indicate Tx A-MPDU capability
+	u8				CurrentAMPDUFactor;		// This indicate Tx A-MPDU capability
+	u8				MPDU_Density;				// This indicate Tx A-MPDU capability
+	u8				CurrentMPDUDensity;			// This indicate Tx A-MPDU capability
+
+	// Forced A-MPDU enable
+	HT_AGGRE_MODE_E	ForcedAMPDUMode;
+	u8				ForcedAMPDUFactor;
+	u8				ForcedMPDUDensity;
+
+	// Forced A-MSDU enable
+	HT_AGGRE_MODE_E	ForcedAMSDUMode;
+	u16				ForcedAMSDUMaxSize;
+
+	u8				bForcedShortGI;
+
+	u8				CurrentOpMode;
+
+	// MIMO PS related
+	u8				SelfMimoPs;
+	u8				PeerMimoPs;
+
+	// 40MHz Channel Offset settings.
+	enum ht_extension_chan_offset	CurSTAExtChnlOffset;
+	u8				bCurTxBW40MHz;	// If we use 40 MHz to Tx
+	u8				PeerBandwidth;
+
+	// For Bandwidth Switching
+	u8				bSwBwInProgress;
+	u8				SwBwStep;
+	//struct timer_list		SwBwTimer;  //moved to ieee80211_device. as timer_list need include some header file here.
+
+	// For Realtek proprietary A-MPDU factor for aggregation
+	u8				bRegRT2RTAggregation;
+	u8				bCurrentRT2RTAggregation;
+	u8				bCurrentRT2RTLongSlotTime;
+	u8				szRT2RTAggBuffer[10];
+
+	// Rx Reorder control
+	u8				bRegRxReorderEnable;
+	u8				bCurRxReorderEnable;
+	u8				RxReorderWinSize;
+	u8				RxReorderPendingTime;
+	u16				RxReorderDropCounter;
+
+#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
+	u8				UsbTxAggrNum;
+#endif
+#ifdef USB_RX_AGGREGATION_SUPPORT
+	u8				UsbRxFwAggrEn;
+	u8				UsbRxFwAggrPageNum;
+	u8				UsbRxFwAggrPacketNum;
+	u8				UsbRxFwAggrTimeout;
+#endif
+
+	// Add for Broadcom(Linksys) IOT. Joseph
+	u8				bIsPeerBcm;
+
+	// For IOT issue.
+	u8				IOTPeer;
+	u32				IOTAction;
+} __attribute__ ((packed)) RT_HIGH_THROUGHPUT, *PRT_HIGH_THROUGHPUT;
+
+/*
+ * The Data structure is used to keep HT related variable for "each AP"
+ * when card is configured as "STA mode"
+ */
+typedef struct _BSS_HT {
+	u8				bdSupportHT;
+
+	// HT related elements
+	u8					bdHTCapBuf[32];
+	u16					bdHTCapLen;
+	u8					bdHTInfoBuf[32];
+	u16					bdHTInfoLen;
+
+	HT_SPEC_VER				bdHTSpecVer;
+	//struct ht_capability_ele              bdHTCapEle;
+	//HT_INFORMATION_ELE		bdHTInfoEle;
+
+	u8					bdRT2RTAggregation;
+	u8					bdRT2RTLongSlotTime;
+} __attribute__ ((packed)) BSS_HT, *PBSS_HT;
+
+extern u8 MCS_FILTER_ALL[16];
+extern u8 MCS_FILTER_1SS[16];
+
+/*
+ * 2007/07/11 MH Modify the macro. Becaus STA may link with a N-AP. If we set
+ * STA in A/B/G mode and AP is still in N mode. The macro will be wrong. We have
+ * to add a macro to judge wireless mode.
+ */
+#define PICK_RATE(_nLegacyRate, _nMcsRate)	\
+		(_nMcsRate == 0) ? (_nLegacyRate & 0x7f) : (_nMcsRate)
+/* 2007/07/12 MH We only define legacy and HT wireless mode now. */
+#define	LEGACY_WIRELESS_MODE	IEEE_MODE_MASK
+
+#define CURRENT_RATE(WirelessMode, LegacyRate, HTRate)           \
+		((WirelessMode & (LEGACY_WIRELESS_MODE)) != 0) ? \
+			(LegacyRate) :                           \
+			(PICK_RATE(LegacyRate, HTRate))
+
+// MCS Bw 40 {1~7, 12~15,32}
+#define	RATE_ADPT_1SS_MASK		0xFF
+#define	RATE_ADPT_2SS_MASK		0xF0 //Skip MCS8~11 because mcs7 > mcs6, 9, 10, 11. 2007.01.16 by Emily
+#define	RATE_ADPT_MCS32_MASK		0x01
+
+#define		IS_11N_MCS_RATE(rate)		(rate & 0x80)
+
+typedef enum _HT_AGGRE_SIZE {
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+} HT_AGGRE_SIZE_E, *PHT_AGGRE_SIZE_E;
+
+/* Indicate different AP vendor for IOT issue */
+typedef enum _HT_IOT_PEER {
+	HT_IOT_PEER_UNKNOWN = 0,
+	HT_IOT_PEER_REALTEK = 1,
+	HT_IOT_PEER_BROADCOM = 2,
+	HT_IOT_PEER_RALINK = 3,
+	HT_IOT_PEER_ATHEROS = 4,
+	HT_IOT_PEER_CISCO = 5,
+	HT_IOT_PEER_MAX = 6
+} HT_IOT_PEER_E, *PHTIOT_PEER_E;
+
+/*
+ * IOT Action for different AP
+ */
+typedef enum _HT_IOT_ACTION {
+	HT_IOT_ACT_TX_USE_AMSDU_4K = 0x00000001,
+	HT_IOT_ACT_TX_USE_AMSDU_8K = 0x00000002,
+	HT_IOT_ACT_DISABLE_MCS14 = 0x00000004,
+	HT_IOT_ACT_DISABLE_MCS15 = 0x00000008,
+	HT_IOT_ACT_DISABLE_ALL_2SS = 0x00000010,
+	HT_IOT_ACT_DISABLE_EDCA_TURBO = 0x00000020,
+	HT_IOT_ACT_MGNT_USE_CCK_6M = 0x00000040,
+	HT_IOT_ACT_CDD_FSYNC = 0x00000080,
+	HT_IOT_ACT_PURE_N_MODE = 0x00000100,
+	HT_IOT_ACT_FORCED_CTS2SELF = 0x00000200,
+} HT_IOT_ACTION_E, *PHT_IOT_ACTION_E;
+
+#endif //_RTL819XU_HTTYPE_H_
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
new file mode 100644
index 000000000..a93f09033
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -0,0 +1,1295 @@
+// SPDX-License-Identifier: GPL-2.0
+
+/*
+ * As this function is mainly ported from Windows driver, so leave the name
+ * little changed. If any confusion caused, tell me. Created by WB. 2008.05.08
+ */
+#include "ieee80211.h"
+
+u8 MCS_FILTER_ALL[16] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+u8 MCS_FILTER_1SS[16] = {0xff, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+u16 MCS_DATA_RATE[2][2][77] = {
+	{	{13, 26, 39, 52, 78, 104, 117, 130, 26, 52, 78, 104, 156, 208, 234, 260,
+		 39, 78, 117, 234, 312, 351, 390, 52, 104, 156, 208, 312, 416, 468, 520,
+		 0, 78, 104, 130, 117, 156, 195, 104, 130, 130, 156, 182, 182, 208, 156, 195,
+		 195, 234, 273, 273, 312, 130, 156, 181, 156, 181, 208, 234, 208, 234, 260, 260,
+		 286, 195, 234, 273, 234, 273, 312, 351, 312, 351, 390, 390, 429},			// Long GI, 20MHz
+		{14, 29, 43, 58, 87, 116, 130, 144, 29, 58, 87, 116, 173, 231, 260, 289,
+		 43, 87, 130, 173, 260, 347, 390, 433, 58, 116, 173, 231, 347, 462, 520, 578,
+		 0, 87, 116, 144, 130, 173, 217, 116, 144, 144, 173, 202, 202, 231, 173, 217,
+		 217, 260, 303, 303, 347, 144, 173, 202, 173, 202, 231, 260, 231, 260, 289, 289,
+		 318, 217, 260, 303, 260, 303, 347, 390, 347, 390, 433, 433, 477}	},		// Short GI, 20MHz
+	{	{27, 54, 81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540,
+		 81, 162, 243, 324, 486, 648, 729, 810, 108, 216, 324, 432, 648, 864, 972, 1080,
+		 12, 162, 216, 270, 243, 324, 405, 216, 270, 270, 324, 378, 378, 432, 324, 405,
+		 405, 486, 567, 567, 648, 270, 324, 378, 324, 378, 432, 486, 432, 486, 540, 540,
+		 594, 405, 486, 567, 486, 567, 648, 729, 648, 729, 810, 810, 891},	// Long GI, 40MHz
+		{30, 60, 90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600,
+		 90, 180, 270, 360, 540, 720, 810, 900, 120, 240, 360, 480, 720, 960, 1080, 1200,
+		 13, 180, 240, 300, 270, 360, 450, 240, 300, 300, 360, 420, 420, 480, 360, 450,
+		 450, 540, 630, 630, 720, 300, 360, 420, 360, 420, 480, 540, 480, 540, 600, 600,
+		 660, 450, 540, 630, 540, 630, 720, 810, 720, 810, 900, 900, 990}	}	// Short GI, 40MHz
+};
+
+static u8 UNKNOWN_BORADCOM[3] = {0x00, 0x14, 0xbf};
+static u8 LINKSYSWRT330_LINKSYSWRT300_BROADCOM[3] = {0x00, 0x1a, 0x70};
+static u8 LINKSYSWRT350_LINKSYSWRT150_BROADCOM[3] = {0x00, 0x1d, 0x7e};
+static u8 NETGEAR834Bv2_BROADCOM[3] = {0x00, 0x1b, 0x2f};
+static u8 BELKINF5D8233V1_RALINK[3] = {0x00, 0x17, 0x3f};	//cosa 03202008
+static u8 BELKINF5D82334V3_RALINK[3] = {0x00, 0x1c, 0xdf};
+static u8 PCI_RALINK[3] = {0x00, 0x90, 0xcc};
+static u8 EDIMAX_RALINK[3] = {0x00, 0x0e, 0x2e};
+static u8 AIRLINK_RALINK[3] = {0x00, 0x18, 0x02};
+//static u8 DLINK_ATHEROS[3] = {0x00, 0x1c, 0xf0};
+static u8 CISCO_BROADCOM[3] = {0x00, 0x17, 0x94};
+/*
+ * 2008/04/01 MH For Cisco G mode RX TP We need to change FW duration. Should we
+ * put the code in other place??
+ * static u8 WIFI_CISCO_G_AP[3] = {0x00, 0x40, 0x96};
+ */
+/*
+ *function:  This function update default settings in pHTInfo structure
+ *   input:  PRT_HIGH_THROUGHPUT	pHTInfo
+ *  output:  none
+ *  return:  none
+ *  notice:  These value need be modified if any changes.
+ */
+void HTUpdateDefaultSetting(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	//const typeof( ((struct ieee80211_device *)0)->pHTInfo ) *__mptr = &pHTInfo;
+
+	//printk("pHTinfo:%p, &pHTinfo:%p, mptr:%p,  offsetof:%x\n", pHTInfo, &pHTInfo, __mptr, offsetof(struct ieee80211_device, pHTInfo));
+	//printk("===>ieee:%p,\n", ieee);
+	// ShortGI support
+	pHTInfo->bRegShortGI20MHz = 1;
+	pHTInfo->bRegShortGI40MHz = 1;
+
+	// 40MHz channel support
+	pHTInfo->bRegBW40MHz = 1;
+
+	// CCK rate support in 40MHz channel
+	if (pHTInfo->bRegBW40MHz)
+		pHTInfo->bRegSuppCCK = 1;
+	else
+		pHTInfo->bRegSuppCCK = true;
+
+	// AMSDU related
+	pHTInfo->nAMSDU_MaxSize = 7935UL;
+	pHTInfo->bAMSDU_Support = 0;
+
+	// AMPDU related
+	pHTInfo->bAMPDUEnable = 1;
+	pHTInfo->AMPDU_Factor = 2; //// 0: 2n13(8K), 1:2n14(16K), 2:2n15(32K), 3:2n16(64k)
+	pHTInfo->MPDU_Density = 0;// 0: No restriction, 1: 1/8usec, 2: 1/4usec, 3: 1/2usec, 4: 1usec, 5: 2usec, 6: 4usec, 7:8usec
+
+	// MIMO Power Save
+	pHTInfo->SelfMimoPs = 3;// 0: Static Mimo Ps, 1: Dynamic Mimo Ps, 3: No Limitation, 2: Reserved(Set to 3 automatically.)
+	if (pHTInfo->SelfMimoPs == 2)
+		pHTInfo->SelfMimoPs = 3;
+	// 8190 only. Assign rate operation mode to firmware
+	ieee->bTxDisableRateFallBack = 0;
+	ieee->bTxUseDriverAssingedRate = 0;
+
+	/*
+	 * 8190 only, Realtek proprietary aggregation mode
+	 * Set MPDUDensity=2,   1: Set MPDUDensity=2(32k)  for Realtek AP and set MPDUDensity=0(8k) for others
+	 */
+	pHTInfo->bRegRT2RTAggregation = 1;//0: Set MPDUDensity=2,   1: Set MPDUDensity=2(32k)  for Realtek AP and set MPDUDensity=0(8k) for others
+
+	// For Rx Reorder Control
+	pHTInfo->bRegRxReorderEnable = 1;
+	pHTInfo->RxReorderWinSize = 64;
+	pHTInfo->RxReorderPendingTime = 30;
+
+#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
+	pHTInfo->UsbTxAggrNum = 4;
+#endif
+#ifdef USB_RX_AGGREGATION_SUPPORT
+	pHTInfo->UsbRxFwAggrEn = 1;
+	pHTInfo->UsbRxFwAggrPageNum = 24;
+	pHTInfo->UsbRxFwAggrPacketNum = 8;
+	pHTInfo->UsbRxFwAggrTimeout = 16; ////usb rx FW aggregation timeout threshold.It's in units of 64us
+#endif
+}
+
+/*
+ *function:  This function print out each field on HT capability
+ *           IE mainly from (Beacon/ProbeRsp/AssocReq)
+ *   input:  u8*	CapIE       //Capability IE to be printed out
+ *	     u8*	TitleString //mainly print out caller function
+ *  output:  none
+ *  return:  none
+ *  notice:  Driver should not print out this message by default.
+ */
+void HTDebugHTCapability(u8 *CapIE, u8 *TitleString)
+{
+	static u8	          EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};	// For 11n EWC definition, 2007.07.17, by Emily
+	struct ht_capability_ele *pCapELE;
+
+	if (!memcmp(CapIE, EWC11NHTCap, sizeof(EWC11NHTCap))) {
+		//EWC IE
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
+		pCapELE = (struct ht_capability_ele *)(&CapIE[4]);
+	} else {
+		pCapELE = (struct ht_capability_ele *)(&CapIE[0]);
+	}
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "<Log HT Capability>. Called by %s\n", TitleString);
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tSupported Channel Width = %s\n", (pCapELE->ChlWidth) ? "20MHz" : "20/40MHz");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tSupport Short GI for 20M = %s\n", (pCapELE->ShortGI20Mhz) ? "YES" : "NO");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tSupport Short GI for 40M = %s\n", (pCapELE->ShortGI40Mhz) ? "YES" : "NO");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tSupport TX STBC = %s\n", (pCapELE->TxSTBC) ? "YES" : "NO");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tMax AMSDU Size = %s\n", (pCapELE->MaxAMSDUSize) ? "3839" : "7935");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tSupport CCK in 20/40 mode = %s\n", (pCapELE->DssCCk) ? "YES" : "NO");
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tMax AMPDU Factor = %d\n", pCapELE->MaxRxAMPDUFactor);
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tMPDU Density = %d\n", pCapELE->MPDUDensity);
+	IEEE80211_DEBUG(IEEE80211_DL_HT,  "\tMCS Rate Set = [%x][%x][%x][%x][%x]\n", pCapELE->MCS[0],\
+				pCapELE->MCS[1], pCapELE->MCS[2], pCapELE->MCS[3], pCapELE->MCS[4]);
+}
+
+/*
+ *function:  This function print out each field on HT Information
+ *           IE mainly from (Beacon/ProbeRsp)
+ *   input:  u8*	InfoIE       //Capability IE to be printed out
+ *	     u8*	TitleString //mainly print out caller function
+ *  output:  none
+ *  return:  none
+ *  notice:  Driver should not print out this message by default.
+ */
+void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString)
+{
+	static u8	EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};	// For 11n EWC definition, 2007.07.17, by Emily
+	PHT_INFORMATION_ELE		pHTInfoEle;
+
+	if (!memcmp(InfoIE, EWC11NHTInfo, sizeof(EWC11NHTInfo))) {
+		// Not EWC IE
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
+		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[4]);
+	} else {
+		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[0]);
+	}
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "<Log HT Information Element>. Called by %s\n", TitleString);
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "\tPrimary channel = %d\n", pHTInfoEle->ControlChl);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "\tSecondary channel =");
+	switch (pHTInfoEle->ExtChlOffset) {
+	case 0:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "Not Present\n");
+		break;
+	case 1:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "Upper channel\n");
+		break;
+	case 2:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "Reserved. Eooro!!!\n");
+		break;
+	case 3:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "Lower Channel\n");
+		break;
+	}
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "\tRecommended channel width = %s\n", (pHTInfoEle->RecommemdedTxWidth) ? "20Mhz" : "40Mhz");
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "\tOperation mode for protection = ");
+	switch (pHTInfoEle->OptMode) {
+	case 0:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "No Protection\n");
+		break;
+	case 1:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "HT non-member protection mode\n");
+		break;
+	case 2:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "Suggest to open protection\n");
+		break;
+	case 3:
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "HT mixed mode\n");
+		break;
+	}
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "\tBasic MCS Rate Set = [%x][%x][%x][%x][%x]\n", pHTInfoEle->BasicMSC[0],\
+				pHTInfoEle->BasicMSC[1], pHTInfoEle->BasicMSC[2], pHTInfoEle->BasicMSC[3], pHTInfoEle->BasicMSC[4]);
+}
+
+static u16 HTMcsToDataRate(struct ieee80211_device *ieee, u8 nMcsRate)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+
+	u8	is40MHz = (pHTInfo->bCurBW40MHz) ? 1 : 0;
+	u8	isShortGI = (pHTInfo->bCurBW40MHz) ?
+						((pHTInfo->bCurShortGI40MHz) ? 1 : 0) :
+						((pHTInfo->bCurShortGI20MHz) ? 1 : 0);
+	return MCS_DATA_RATE[is40MHz][isShortGI][(nMcsRate & 0x7f)];
+}
+
+/*
+ *function:  This function returns current datarate.
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8				nDataRate
+ *  output:  none
+ *  return:  tx rate
+ *  notice:  quite unsure about how to use this function //wb
+ */
+u16  TxCountToDataRate(struct ieee80211_device *ieee, u8 nDataRate)
+{
+	//PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	u16		CCKOFDMRate[12] = {0x02, 0x04, 0x0b, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c};
+	u8	is40MHz = 0;
+	u8	isShortGI = 0;
+
+	if (nDataRate < 12) {
+		return CCKOFDMRate[nDataRate];
+	} else {
+		if (nDataRate >= 0x10 && nDataRate <= 0x1f) { //if(nDataRate > 11 && nDataRate < 28 )
+			is40MHz = 0;
+			isShortGI = 0;
+
+		      // nDataRate = nDataRate - 12;
+		} else if (nDataRate >= 0x20  && nDataRate <= 0x2f) { //(27, 44)
+			is40MHz = 1;
+			isShortGI = 0;
+
+			//nDataRate = nDataRate - 28;
+		} else if (nDataRate >= 0x30  && nDataRate <= 0x3f) { //(43, 60)
+			is40MHz = 0;
+			isShortGI = 1;
+
+			//nDataRate = nDataRate - 44;
+		} else if (nDataRate >= 0x40  && nDataRate <= 0x4f) { //(59, 76)
+			is40MHz = 1;
+			isShortGI = 1;
+
+			//nDataRate = nDataRate - 60;
+		}
+		return MCS_DATA_RATE[is40MHz][isShortGI][nDataRate & 0xf];
+	}
+}
+
+bool IsHTHalfNmodeAPs(struct ieee80211_device *ieee)
+{
+	bool			retValue = false;
+	struct ieee80211_network *net = &ieee->current_network;
+
+	if ((memcmp(net->bssid, BELKINF5D8233V1_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, BELKINF5D82334V3_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, PCI_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, EDIMAX_RALINK, 3) == 0) ||
+	    (memcmp(net->bssid, AIRLINK_RALINK, 3) == 0) ||
+	    (net->ralink_cap_exist))
+		retValue = true;
+	else if ((memcmp(net->bssid, UNKNOWN_BORADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, LINKSYSWRT330_LINKSYSWRT300_BROADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, LINKSYSWRT350_LINKSYSWRT150_BROADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, NETGEAR834Bv2_BROADCOM, 3) == 0) ||
+		 (net->broadcom_cap_exist))
+		retValue = true;
+	else if (net->bssht.bdRT2RTAggregation)
+		retValue = true;
+	else
+		retValue = false;
+
+	return retValue;
+}
+
+/*
+ *function:  This function returns peer IOT.
+ *   input:  struct ieee80211_device*	ieee
+ *  output:  none
+ *  return:
+ *  notice:
+ */
+static void HTIOTPeerDetermine(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	struct ieee80211_network *net = &ieee->current_network;
+
+	if (net->bssht.bdRT2RTAggregation)
+		pHTInfo->IOTPeer = HT_IOT_PEER_REALTEK;
+	else if (net->broadcom_cap_exist)
+		pHTInfo->IOTPeer = HT_IOT_PEER_BROADCOM;
+	else if ((memcmp(net->bssid, UNKNOWN_BORADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, LINKSYSWRT330_LINKSYSWRT300_BROADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, LINKSYSWRT350_LINKSYSWRT150_BROADCOM, 3) == 0) ||
+		 (memcmp(net->bssid, NETGEAR834Bv2_BROADCOM, 3) == 0))
+		pHTInfo->IOTPeer = HT_IOT_PEER_BROADCOM;
+	else if ((memcmp(net->bssid, BELKINF5D8233V1_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, BELKINF5D82334V3_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, PCI_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, EDIMAX_RALINK, 3) == 0) ||
+		 (memcmp(net->bssid, AIRLINK_RALINK, 3) == 0) ||
+		 net->ralink_cap_exist)
+		pHTInfo->IOTPeer = HT_IOT_PEER_RALINK;
+	else if (net->atheros_cap_exist)
+		pHTInfo->IOTPeer = HT_IOT_PEER_ATHEROS;
+	else if (memcmp(net->bssid, CISCO_BROADCOM, 3) == 0)
+		pHTInfo->IOTPeer = HT_IOT_PEER_CISCO;
+	else
+		pHTInfo->IOTPeer = HT_IOT_PEER_UNKNOWN;
+
+	IEEE80211_DEBUG(IEEE80211_DL_IOT, "Joseph debug!! IOTPEER: %x\n", pHTInfo->IOTPeer);
+}
+
+/*
+ *function:  Check whether driver should declare received rate up to MCS13
+ *           only since some chipset is not good at receiving MCS14~15 frame
+ *           from some AP.
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8 *			PeerMacAddr
+ *  output:  none
+ *  return:  return 1 if driver should declare MCS13 only(otherwise return 0)
+ */
+static u8 HTIOTActIsDisableMCS14(struct ieee80211_device *ieee, u8 *PeerMacAddr)
+{
+	return 0;
+}
+
+/*
+ * Function:	HTIOTActIsDisableMCS15
+ *
+ * Overview:	Check whether driver should declare capability of receiving
+ *              MCS15
+ *
+ * Input:
+ *			PADAPTER		Adapter,
+ *
+ * Output:		None
+ * Return:	true if driver should disable MCS15
+ * 2008.04.15	Emily
+ */
+static bool HTIOTActIsDisableMCS15(struct ieee80211_device *ieee)
+{
+	bool retValue = false;
+
+#ifdef TODO
+	// Apply for 819u only
+#if (HAL_CODE_BASE == RTL8192)
+
+#if (DEV_BUS_TYPE == USB_INTERFACE)
+	// Alway disable MCS15 by Jerry Chang's request.by Emily, 2008.04.15
+	retValue = true;
+#elif (DEV_BUS_TYPE == PCI_INTERFACE)
+	// Enable MCS15 if the peer is Cisco AP. by Emily, 2008.05.12
+//	if(pBssDesc->bCiscoCapExist)
+//		retValue = false;
+//	else
+		retValue = false;
+#endif
+#endif
+#endif
+	// Jerry Chang suggest that 8190 1x2 does not need to disable MCS15
+
+	return retValue;
+}
+
+/*
+ * Function:	HTIOTActIsDisableMCSTwoSpatialStream
+ *
+ * Overview:	Check whether driver should declare capability of receiving
+ *              All 2 ss packets
+ *
+ * Input:
+ *			PADAPTER		Adapter,
+ *
+ * Output:		None
+ * Return:	true if driver should disable all two spatial stream packet
+ * 2008.04.21	Emily
+ */
+static bool HTIOTActIsDisableMCSTwoSpatialStream(struct ieee80211_device *ieee,
+						 u8 *PeerMacAddr)
+{
+#ifdef TODO
+	// Apply for 819u only
+#endif
+	return false;
+}
+
+/*
+ *function:  Check whether driver should disable EDCA turbo mode
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*			PeerMacAddr
+ *  output:  none
+ *  return:  return 1 if driver should disable EDCA turbo mode
+ *           (otherwise return 0)
+ */
+static u8 HTIOTActIsDisableEDCATurbo(struct ieee80211_device *ieee,
+				     u8 *PeerMacAddr)
+{	/* default enable EDCA Turbo mode. */
+	return false;
+}
+
+/*
+ *function:  Check whether we need to use OFDM to sned MGNT frame for
+ *           broadcom AP
+ *   input:  struct ieee80211_network *network   //current network we live
+ *  output:  none
+ *  return:  return 1 if true
+ */
+static u8 HTIOTActIsMgntUseCCK6M(struct ieee80211_network *network)
+{
+	u8	retValue = 0;
+
+	// 2008/01/25 MH Judeg if we need to use OFDM to sned MGNT frame for broadcom AP.
+	// 2008/01/28 MH We must prevent that we select null bssid to link.
+
+	if (network->broadcom_cap_exist)
+		retValue = 1;
+
+	return retValue;
+}
+
+static u8 HTIOTActIsCCDFsync(u8 *PeerMacAddr)
+{
+	u8	retValue = 0;
+
+	if ((memcmp(PeerMacAddr, UNKNOWN_BORADCOM, 3) == 0) ||
+	    (memcmp(PeerMacAddr, LINKSYSWRT330_LINKSYSWRT300_BROADCOM, 3) == 0) ||
+	    (memcmp(PeerMacAddr, LINKSYSWRT350_LINKSYSWRT150_BROADCOM, 3) == 0))
+		retValue = 1;
+
+	return retValue;
+}
+
+void HTResetIOTSetting(PRT_HIGH_THROUGHPUT pHTInfo)
+{
+	pHTInfo->IOTAction = 0;
+	pHTInfo->IOTPeer = HT_IOT_PEER_UNKNOWN;
+}
+
+/*
+ *function:  Construct Capablility Element in Beacon... if HTEnable is turned on
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*		     posHTCap //pointer to store Capability Ele
+ *	     u8*		     len //store length of CE
+ *	     u8			     IsEncrypt //whether encrypt, needed further
+ *  output:  none
+ *  return:  none
+ *  notice:  posHTCap can't be null and should be initialized before.
+ */
+void HTConstructCapabilityElement(struct ieee80211_device *ieee, u8 *posHTCap, u8 *len, u8 IsEncrypt)
+{
+	PRT_HIGH_THROUGHPUT	pHT = ieee->pHTInfo;
+	struct ht_capability_ele   *pCapELE = NULL;
+	//u8 bIsDeclareMCS13;
+
+	if (!posHTCap || !pHT) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"posHTCap or pHTInfo can't be null in %s\n",
+				__func__);
+		return;
+	}
+	memset(posHTCap, 0, *len);
+	if (pHT->ePeerHTSpecVer == HT_SPEC_VER_EWC) {
+		static const u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+
+		memcpy(posHTCap, EWC11NHTCap, sizeof(EWC11NHTCap));
+		pCapELE = (struct ht_capability_ele *)&posHTCap[4];
+	} else {
+		pCapELE = (struct ht_capability_ele *)posHTCap;
+	}
+
+	//HT capability info
+	pCapELE->AdvCoding		= 0; // This feature is not supported now!!
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		pCapELE->ChlWidth = 0;
+	else
+		pCapELE->ChlWidth = (pHT->bRegBW40MHz ? 1 : 0);
+
+//	pCapELE->ChlWidth		= (pHT->bRegBW40MHz?1:0);
+	pCapELE->MimoPwrSave		= pHT->SelfMimoPs;
+	pCapELE->GreenField		= 0; // This feature is not supported now!!
+	pCapELE->ShortGI20Mhz		= 1; // We can receive Short GI!!
+	pCapELE->ShortGI40Mhz		= 1; // We can receive Short GI!!
+	//DbgPrint("TX HT cap/info ele BW=%d SG20=%d SG40=%d\n\r",
+	//pCapELE->ChlWidth, pCapELE->ShortGI20Mhz, pCapELE->ShortGI40Mhz);
+	pCapELE->TxSTBC			= 1;
+	pCapELE->RxSTBC			= 0;
+	pCapELE->DelayBA		= 0;	// Do not support now!!
+	pCapELE->MaxAMSDUSize	        = (MAX_RECEIVE_BUFFER_SIZE >= 7935) ? 1 : 0;
+	pCapELE->DssCCk			= ((pHT->bRegBW40MHz) ? (pHT->bRegSuppCCK ? 1 : 0) : 0);
+	pCapELE->PSMP			= 0; // Do not support now!!
+	pCapELE->LSigTxopProtect	= 0; // Do not support now!!
+
+	/*
+	 * MAC HT parameters info
+	 * TODO: Nedd to take care of this part
+	 */
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n", pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
+
+	if (IsEncrypt) {
+		pCapELE->MPDUDensity	= 7; // 8us
+		pCapELE->MaxRxAMPDUFactor = 2; // 2 is for 32 K and 3 is 64K
+	} else {
+		pCapELE->MaxRxAMPDUFactor = 3; // 2 is for 32 K and 3 is 64K
+		pCapELE->MPDUDensity	= 0; // no density
+	}
+
+	//Supported MCS set
+	memcpy(pCapELE->MCS, ieee->Regdot11HTOperationalRateSet, 16);
+	if (pHT->IOTAction & HT_IOT_ACT_DISABLE_MCS15)
+		pCapELE->MCS[1] &= 0x7f;
+
+	if (pHT->IOTAction & HT_IOT_ACT_DISABLE_MCS14)
+		pCapELE->MCS[1] &= 0xbf;
+
+	if (pHT->IOTAction & HT_IOT_ACT_DISABLE_ALL_2SS)
+		pCapELE->MCS[1] &= 0x00;
+
+	/*
+	 * 2008.06.12
+	 * For RTL819X, if pairwisekey = wep/tkip, ap is ralink, we support only MCS0~7.
+	 */
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev)) {
+		int i;
+
+		for (i = 1; i < 16; i++)
+			pCapELE->MCS[i] = 0;
+	}
+
+	//Extended HT Capability Info
+	memset(&pCapELE->ExtHTCapInfo, 0, 2);
+
+	//TXBF Capabilities
+	memset(pCapELE->TxBFCap, 0, 4);
+
+	//Antenna Selection Capabilities
+	pCapELE->ASCap = 0;
+//add 2 to give space for element ID and len when construct frames
+	if (pHT->ePeerHTSpecVer == HT_SPEC_VER_EWC)
+		*len = 30 + 2;
+	else
+		*len = 26 + 2;
+
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_HT, posHTCap, *len -2);
+
+	/*
+	 * Print each field in detail. Driver should not print out this message
+	 * by default
+	 */
+//	HTDebugHTCapability(posHTCap, (u8*)"HTConstructCapability()");
+}
+
+/*
+ *function:  Construct Information Element in Beacon... if HTEnable is turned on
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*		     posHTCap //pointer to store Information Ele
+ *	     u8*		     len   //store len of
+ *	     u8			     IsEncrypt //whether encrypt, needed further
+ *  output:  none
+ *  return:  none
+ *  notice:  posHTCap can't be null and be initialized before.
+ *           Only AP and IBSS sta should do this
+ */
+void HTConstructInfoElement(struct ieee80211_device *ieee, u8 *posHTInfo, u8 *len, u8 IsEncrypt)
+{
+	PRT_HIGH_THROUGHPUT	pHT = ieee->pHTInfo;
+	PHT_INFORMATION_ELE		pHTInfoEle = (PHT_INFORMATION_ELE)posHTInfo;
+
+	if (!posHTInfo || !pHTInfoEle) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"posHTInfo or pHTInfoEle can't be null in %s\n",
+				__func__);
+		return;
+	}
+
+	memset(posHTInfo, 0, *len);
+	if ((ieee->iw_mode == IW_MODE_ADHOC) || (ieee->iw_mode == IW_MODE_MASTER)) { //ap mode is not currently supported
+		pHTInfoEle->ControlChl			= ieee->current_network.channel;
+		pHTInfoEle->ExtChlOffset		= ((!pHT->bRegBW40MHz) ? HT_EXTCHNL_OFFSET_NO_EXT :
+											(ieee->current_network.channel <= 6) ?
+												HT_EXTCHNL_OFFSET_UPPER : HT_EXTCHNL_OFFSET_LOWER);
+		pHTInfoEle->RecommemdedTxWidth	= pHT->bRegBW40MHz;
+		pHTInfoEle->RIFS					= 0;
+		pHTInfoEle->PSMPAccessOnly		= 0;
+		pHTInfoEle->SrvIntGranularity		= 0;
+		pHTInfoEle->OptMode				= pHT->CurrentOpMode;
+		pHTInfoEle->NonGFDevPresent		= 0;
+		pHTInfoEle->DualBeacon			= 0;
+		pHTInfoEle->SecondaryBeacon		= 0;
+		pHTInfoEle->LSigTxopProtectFull		= 0;
+		pHTInfoEle->PcoActive				= 0;
+		pHTInfoEle->PcoPhase				= 0;
+
+		memset(pHTInfoEle->BasicMSC, 0, 16);
+
+		*len = 22 + 2; //same above
+	} else {
+		//STA should not generate High Throughput Information Element
+		*len = 0;
+	}
+	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_HT, posHTInfo, *len - 2);
+	//HTDebugHTInfo(posHTInfo, "HTConstructInforElement");
+}
+
+/*
+ * According to experiment, Realtek AP to STA (based on rtl8190) may achieve
+ * best performance if both STA and AP set limitation of aggregation size to
+ * 32K, that is, set AMPDU density to 2 (Ref: IEEE 11n specification).
+ * However, if Realtek STA associates to other AP, STA should set limitation of
+ * aggregation size to 8K, otherwise, performance of traffic stream from STA to
+ * AP will be much less than the traffic stream from AP to STA if both of the
+ * stream runs concurrently at the same time.
+ *
+ *  Frame Format
+ *  Element ID		Length		OUI		Type1		Reserved
+ *  1 byte		1 byte		3 bytes		1 byte		1 byte
+ *
+ *  OUI		= 0x00, 0xe0, 0x4c,
+ *  Type	= 0x02
+ *  Reserved	= 0x00
+ *
+ *  2007.8.21 by Emily
+ */
+/*
+ *function:  Construct  Information Element in Beacon... in RT2RT condition
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*		  posRT2RTAgg //pointer to store Information Ele
+ *	     u8*		  len   //store len
+ *  output:  none
+ *  return:  none
+ *  notice:
+ */
+void HTConstructRT2RTAggElement(struct ieee80211_device *ieee, u8 *posRT2RTAgg, u8 *len)
+{
+	if (!posRT2RTAgg) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"posRT2RTAgg can't be null in %s\n",
+				__func__);
+		return;
+	}
+	memset(posRT2RTAgg, 0, *len);
+	*posRT2RTAgg++ = 0x00;
+	*posRT2RTAgg++ = 0xe0;
+	*posRT2RTAgg++ = 0x4c;
+	*posRT2RTAgg++ = 0x02;
+	*posRT2RTAgg++ = 0x01;
+	*posRT2RTAgg = 0x10;//*posRT2RTAgg = 0x02;
+
+	if (ieee->bSupportRemoteWakeUp)
+		*posRT2RTAgg |= 0x08;//RT_HT_CAP_USE_WOW;
+
+	*len = 6 + 2;
+	return;
+#ifdef TODO
+#if (HAL_CODE_BASE == RTL8192 && DEV_BUS_TYPE == USB_INTERFACE)
+	/*
+	//Emily. If it is required to Ask Realtek AP to send AMPDU during AES mode, enable this
+	   section of code.
+	if(IS_UNDER_11N_AES_MODE(Adapter))
+	{
+		posRT2RTAgg->octet[5] |= RT_HT_CAP_USE_AMPDU;
+	}else
+	{
+		posRT2RTAgg->octet[5] &= 0xfb;
+	}
+	*/
+#else
+	// Do Nothing
+#endif
+
+	posRT2RTAgg->Length = 6;
+#endif
+}
+
+/*
+ *function:  Pick the right Rate Adaptive table to use
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*		      pOperateMCS //A pointer to MCS rate bitmap
+ *  return:  always we return true
+ *  notice:
+ */
+static u8 HT_PickMCSRate(struct ieee80211_device *ieee, u8 *pOperateMCS)
+{
+	if (!pOperateMCS) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"pOperateMCS can't be null in %s\n",
+				__func__);
+		return false;
+	}
+
+	switch (ieee->mode) {
+	case IEEE_A:
+	case IEEE_B:
+	case IEEE_G:
+		//legacy rate routine handled at selectedrate
+
+		//no MCS rate
+		memset(pOperateMCS, 0, 16);
+		break;
+
+	case IEEE_N_24G:	//assume CCK rate ok
+	case IEEE_N_5G:
+		// Legacy part we only use 6, 5.5,2,1 for N_24G and 6 for N_5G.
+		// Legacy part shall be handled at SelectRateSet().
+
+		//HT part
+		// TODO: may be different if we have different number of antenna
+		pOperateMCS[0] &= RATE_ADPT_1SS_MASK;	//support MCS 0~7
+		pOperateMCS[1] &= RATE_ADPT_2SS_MASK;
+		pOperateMCS[3] &= RATE_ADPT_MCS32_MASK;
+		break;
+
+	//should never reach here
+	default:
+		break;
+	}
+
+	return true;
+}
+
+/*
+ *	Description:
+ *		This function will get the highest speed rate in input MCS set.
+ *
+ *	/param	Adapter			Pionter to Adapter entity
+ *			pMCSRateSet		Pointer to MCS rate bitmap
+ *			pMCSFilter		Pointer to MCS rate filter
+ *
+ *	/return	Highest MCS rate included in pMCSRateSet and filtered by pMCSFilter.
+ *
+ */
+/*
+ *function:  This function will get the highest speed rate in input MCS set.
+ *   input:  struct ieee80211_device*	ieee
+ *	     u8*			pMCSRateSet //Pointer to MCS rate bitmap
+ *	     u8*			pMCSFilter //Pointer to MCS rate filter
+ *  return:  Highest MCS rate included in pMCSRateSet and filtered by pMCSFilter
+ *  notice:
+ */
+u8 HTGetHighestMCSRate(struct ieee80211_device *ieee, u8 *pMCSRateSet, u8 *pMCSFilter)
+{
+	u8		i, j;
+	u8		bitMap;
+	u8		mcsRate = 0;
+	u8		availableMcsRate[16];
+
+	if (!pMCSRateSet || !pMCSFilter) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"pMCSRateSet or pMCSFilter can't be null in %s\n",
+				__func__);
+		return false;
+	}
+	for (i = 0; i < 16; i++)
+		availableMcsRate[i] = pMCSRateSet[i] & pMCSFilter[i];
+
+	for (i = 0; i < 16; i++) {
+		if (availableMcsRate[i] != 0)
+			break;
+	}
+	if (i == 16)
+		return false;
+
+	for (i = 0; i < 16; i++) {
+		if (availableMcsRate[i] != 0) {
+			bitMap = availableMcsRate[i];
+			for (j = 0; j < 8; j++) {
+				if ((bitMap % 2) != 0) {
+					if (HTMcsToDataRate(ieee, (8 * i + j)) > HTMcsToDataRate(ieee, mcsRate))
+						mcsRate = (8 * i + j);
+				}
+				bitMap >>= 1;
+			}
+		}
+	}
+	return (mcsRate | 0x80);
+}
+
+/*
+ * 1.Filter our operation rate set with AP's rate set
+ * 2.shall reference channel bandwidth, STBC, Antenna number
+ * 3.generate rate adative table for firmware
+ * David 20060906
+ *
+ * \pHTSupportedCap: the connected STA's supported rate Capability element
+ */
+static u8 HTFilterMCSRate(struct ieee80211_device *ieee, u8 *pSupportMCS,
+			  u8 *pOperateMCS)
+{
+	u8 i = 0;
+
+	// filter out operational rate set not supported by AP, the length of it is 16
+	for (i = 0; i <= 15; i++)
+		pOperateMCS[i] = ieee->Regdot11HTOperationalRateSet[i] & pSupportMCS[i];
+
+	// TODO: adjust our operational rate set  according to our channel bandwidth, STBC and Antenna number
+	/*
+	 * TODO: fill suggested rate adaptive rate index and give firmware info
+	 * using Tx command packet we also shall suggested the first start rate
+	 * set according to our signal strength
+	 */
+	HT_PickMCSRate(ieee, pOperateMCS);
+
+	// For RTL819X, if pairwisekey = wep/tkip, we support only MCS0~7.
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+		pOperateMCS[1] = 0;
+
+	/*
+	 * For RTL819X, we support only MCS0~15.
+	 * And also, we do not know how to use MCS32 now.
+	 */
+	for (i = 2; i <= 15; i++)
+		pOperateMCS[i] = 0;
+
+	return true;
+}
+
+void HTOnAssocRsp(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
+	struct ht_capability_ele       *pPeerHTCap = NULL;
+	PHT_INFORMATION_ELE		pPeerHTInfo = NULL;
+	u16	nMaxAMSDUSize = 0;
+	u8	*pMcsFilter = NULL;
+
+	static u8				EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};		// For 11n EWC definition, 2007.07.17, by Emily
+	static u8				EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};	// For 11n EWC definition, 2007.07.17, by Emily
+
+	if (!pHTInfo->bCurrentHTSupport) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				"<=== %s: HT_DISABLE\n",
+				__func__);
+		return;
+	}
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "===> HTOnAssocRsp_wq(): HT_ENABLE\n");
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, pHTInfo->PeerHTCapBuf, sizeof(struct ht_capability_ele));
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, pHTInfo->PeerHTInfoBuf, sizeof(HT_INFORMATION_ELE));
+
+//	HTDebugHTCapability(pHTInfo->PeerHTCapBuf,"HTOnAssocRsp_wq");
+//	HTDebugHTInfo(pHTInfo->PeerHTInfoBuf,"HTOnAssocRsp_wq");
+	//
+	if (!memcmp(pHTInfo->PeerHTCapBuf, EWC11NHTCap, sizeof(EWC11NHTCap)))
+		pPeerHTCap = (struct ht_capability_ele *)(&pHTInfo->PeerHTCapBuf[4]);
+	else
+		pPeerHTCap = (struct ht_capability_ele *)(pHTInfo->PeerHTCapBuf);
+
+	if (!memcmp(pHTInfo->PeerHTInfoBuf, EWC11NHTInfo, sizeof(EWC11NHTInfo)))
+		pPeerHTInfo = (PHT_INFORMATION_ELE)(&pHTInfo->PeerHTInfoBuf[4]);
+	else
+		pPeerHTInfo = (PHT_INFORMATION_ELE)(pHTInfo->PeerHTInfoBuf);
+
+	////////////////////////////////////////////////////////
+	// Configurations:
+	////////////////////////////////////////////////////////
+	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA | IEEE80211_DL_HT, pPeerHTCap, sizeof(struct ht_capability_ele));
+//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_HT, pPeerHTInfo, sizeof(HT_INFORMATION_ELE));
+	// Config Supported Channel Width setting
+	//
+	HTSetConnectBwMode(ieee, (enum ht_channel_width)(pPeerHTCap->ChlWidth), (enum ht_extension_chan_offset)(pPeerHTInfo->ExtChlOffset));
+
+	pHTInfo->bCurTxBW40MHz = (pPeerHTInfo->RecommemdedTxWidth == 1);
+
+	/*
+	 * Update short GI/ long GI setting
+	 *
+	 * TODO:
+	 */
+	pHTInfo->bCurShortGI20MHz = pHTInfo->bRegShortGI20MHz &&
+				    (pPeerHTCap->ShortGI20Mhz == 1);
+	pHTInfo->bCurShortGI40MHz = pHTInfo->bRegShortGI40MHz &&
+				   (pPeerHTCap->ShortGI40Mhz == 1);
+
+	/*
+	 * Config TX STBC setting
+	 *
+	 * TODO:
+	 */
+
+	/*
+	 * Config DSSS/CCK  mode in 40MHz mode
+	 *
+	 * TODO:
+	 */
+	pHTInfo->bCurSuppCCK = pHTInfo->bRegSuppCCK &&
+			       (pPeerHTCap->DssCCk == 1);
+
+	/*
+	 * Config and configure A-MSDU setting
+	 */
+	pHTInfo->bCurrent_AMSDU_Support = pHTInfo->bAMSDU_Support;
+
+	nMaxAMSDUSize = (pPeerHTCap->MaxAMSDUSize == 0) ? 3839 : 7935;
+
+	if (pHTInfo->nAMSDU_MaxSize > nMaxAMSDUSize)
+		pHTInfo->nCurrent_AMSDU_MaxSize = nMaxAMSDUSize;
+	else
+		pHTInfo->nCurrent_AMSDU_MaxSize = pHTInfo->nAMSDU_MaxSize;
+	/*
+	 * Config A-MPDU setting
+	 */
+	pHTInfo->bCurrentAMPDUEnable = pHTInfo->bAMPDUEnable;
+
+	/*
+	 * <1> Decide AMPDU Factor
+	 * By Emily
+	 */
+	if (!pHTInfo->bRegRT2RTAggregation) {
+		// Decide AMPDU Factor according to protocol handshake
+		if (pHTInfo->AMPDU_Factor > pPeerHTCap->MaxRxAMPDUFactor)
+			pHTInfo->CurrentAMPDUFactor = pPeerHTCap->MaxRxAMPDUFactor;
+		else
+			pHTInfo->CurrentAMPDUFactor = pHTInfo->AMPDU_Factor;
+	} else {
+		/*
+		 * Set MPDU density to 2 to Realtek AP, and set it to 0 for others
+		 * Replace MPDU factor declared in original association response frame format. 2007.08.20 by Emily
+		 */
+		if (ieee->current_network.bssht.bdRT2RTAggregation) {
+			if (ieee->pairwise_key_type != KEY_TYPE_NA)
+				// Realtek may set 32k in security mode and 64k for others
+				pHTInfo->CurrentAMPDUFactor = pPeerHTCap->MaxRxAMPDUFactor;
+			else
+				pHTInfo->CurrentAMPDUFactor = HT_AGG_SIZE_64K;
+		} else {
+			pHTInfo->CurrentAMPDUFactor = min_t(u32, pPeerHTCap->MaxRxAMPDUFactor,
+							    HT_AGG_SIZE_32K);
+		}
+	}
+
+	/*
+	 * <2> Set AMPDU Minimum MPDU Start Spacing
+	 * 802.11n 3.0 section 9.7d.3
+	 */
+	pHTInfo->CurrentMPDUDensity = max_t(u32, pHTInfo->MPDU_Density,
+					    pPeerHTCap->MPDUDensity);
+
+	if (ieee->pairwise_key_type != KEY_TYPE_NA)
+		pHTInfo->CurrentMPDUDensity	= 7; // 8us
+	// Force TX AMSDU
+
+	// Lanhsin: mark for tmp to avoid deauth by ap from  s3
+	//if(memcmp(pMgntInfo->Bssid, NETGEAR834Bv2_BROADCOM, 3)==0)
+	if (0) {
+		pHTInfo->bCurrentAMPDUEnable = false;
+		pHTInfo->ForcedAMSDUMode = HT_AGG_FORCE_ENABLE;
+		pHTInfo->ForcedAMSDUMaxSize = 7935;
+
+		pHTInfo->IOTAction |=  HT_IOT_ACT_TX_USE_AMSDU_8K;
+	}
+
+	// Rx Reorder Setting
+	pHTInfo->bCurRxReorderEnable = pHTInfo->bRegRxReorderEnable;
+
+	/*
+	 * Filter out unsupported HT rate for this AP
+	 * Update RATR table
+	 * This is only for 8190 ,8192 or later product which using firmware to
+	 * handle rate adaptive mechanism.
+	 */
+
+	/*
+	 * Handle Ralink AP bad MCS rate set condition. Joseph.
+	 * This fix the bug of Ralink AP. This may be removed in the future.
+	 */
+	if (pPeerHTCap->MCS[0] == 0)
+		pPeerHTCap->MCS[0] = 0xff;
+
+	HTFilterMCSRate(ieee, pPeerHTCap->MCS, ieee->dot11HTOperationalRateSet);
+
+	/*
+	 * Config MIMO Power Save setting
+	 */
+	pHTInfo->PeerMimoPs = pPeerHTCap->MimoPwrSave;
+	if (pHTInfo->PeerMimoPs == MIMO_PS_STATIC)
+		pMcsFilter = MCS_FILTER_1SS;
+	else
+		pMcsFilter = MCS_FILTER_ALL;
+	//WB add for MCS8 bug
+//	pMcsFilter = MCS_FILTER_1SS;
+	ieee->HTHighestOperaRate = HTGetHighestMCSRate(ieee, ieee->dot11HTOperationalRateSet, pMcsFilter);
+	ieee->HTCurrentOperaRate = ieee->HTHighestOperaRate;
+
+	/*
+	 * Config current operation mode.
+	 */
+	pHTInfo->CurrentOpMode = pPeerHTInfo->OptMode;
+}
+
+/*
+ *function:  initialize HT info(struct PRT_HIGH_THROUGHPUT)
+ *   input:  struct ieee80211_device*	ieee
+ *  output:  none
+ *  return:  none
+ *  notice: This function is called when
+ *                                  *  (1) MPInitialization Phase
+ *                                  *  (2) Receiving of Deauthentication from AP
+ */
+// TODO: Should this funciton be called when receiving of Disassociation?
+void HTInitializeHTInfo(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
+
+	/*
+	 * These parameters will be reset when receiving deauthentication packet
+	 */
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "===========>%s()\n", __func__);
+	pHTInfo->bCurrentHTSupport = false;
+
+	// 40MHz channel support
+	pHTInfo->bCurBW40MHz = false;
+	pHTInfo->bCurTxBW40MHz = false;
+
+	// Short GI support
+	pHTInfo->bCurShortGI20MHz = false;
+	pHTInfo->bCurShortGI40MHz = false;
+	pHTInfo->bForcedShortGI = false;
+
+	/*
+	 * CCK rate support
+	 * This flag is set to true to support CCK rate by default.
+	 * It will be affected by "pHTInfo->bRegSuppCCK" and AP capabilities
+	 * only when associate to 11N BSS.
+	 */
+	pHTInfo->bCurSuppCCK = true;
+
+	// AMSDU related
+	pHTInfo->bCurrent_AMSDU_Support = false;
+	pHTInfo->nCurrent_AMSDU_MaxSize = pHTInfo->nAMSDU_MaxSize;
+
+	// AMPUD related
+	pHTInfo->CurrentMPDUDensity = pHTInfo->MPDU_Density;
+	pHTInfo->CurrentAMPDUFactor = pHTInfo->AMPDU_Factor;
+
+	// Initialize all of the parameters related to 11n
+	memset(&pHTInfo->SelfHTCap, 0, sizeof(pHTInfo->SelfHTCap));
+	memset(&pHTInfo->SelfHTInfo, 0, sizeof(pHTInfo->SelfHTInfo));
+	memset(&pHTInfo->PeerHTCapBuf, 0, sizeof(pHTInfo->PeerHTCapBuf));
+	memset(&pHTInfo->PeerHTInfoBuf, 0, sizeof(pHTInfo->PeerHTInfoBuf));
+
+	pHTInfo->bSwBwInProgress = false;
+
+	// Set default IEEE spec for Draft N
+	pHTInfo->ePeerHTSpecVer = HT_SPEC_VER_IEEE;
+
+	// Realtek proprietary aggregation mode
+	pHTInfo->bCurrentRT2RTAggregation = false;
+	pHTInfo->bCurrentRT2RTLongSlotTime = false;
+	pHTInfo->IOTPeer = 0;
+	pHTInfo->IOTAction = 0;
+
+	//MCS rate initialized here
+	{
+		u8 *RegHTSuppRateSets = &ieee->RegHTSuppRateSet[0];
+
+		RegHTSuppRateSets[0] = 0xFF;	//support MCS 0~7
+		RegHTSuppRateSets[1] = 0xFF;	//support MCS 8~15
+		RegHTSuppRateSets[4] = 0x01;	//support MCS 32
+	}
+}
+
+/*
+ *function:  initialize Bss HT structure(struct PBSS_HT)
+ *   input:  PBSS_HT pBssHT //to be initialized
+ *  output:  none
+ *  return:  none
+ *  notice: This function is called when initialize network structure
+ */
+void HTInitializeBssDesc(PBSS_HT pBssHT)
+{
+	pBssHT->bdSupportHT = false;
+	memset(pBssHT->bdHTCapBuf, 0, sizeof(pBssHT->bdHTCapBuf));
+	pBssHT->bdHTCapLen = 0;
+	memset(pBssHT->bdHTInfoBuf, 0, sizeof(pBssHT->bdHTInfoBuf));
+	pBssHT->bdHTInfoLen = 0;
+
+	pBssHT->bdHTSpecVer = HT_SPEC_VER_IEEE;
+
+	pBssHT->bdRT2RTAggregation = false;
+	pBssHT->bdRT2RTLongSlotTime = false;
+}
+
+/*
+ *function:  initialize Bss HT structure(struct PBSS_HT)
+ *   input:  struct ieee80211_device	*ieee
+ *	     struct ieee80211_network	*pNetwork //usually current network
+ *                                                  we are live in
+ *  output:  none
+ *  return:  none
+ *  notice: This function should ONLY be called before association
+ */
+void HTResetSelfAndSavePeerSetting(struct ieee80211_device *ieee,	struct ieee80211_network *pNetwork)
+{
+	PRT_HIGH_THROUGHPUT		pHTInfo = ieee->pHTInfo;
+//	u16						nMaxAMSDUSize;
+//	struct ht_capability_ele       *pPeerHTCap = (struct ht_capability_ele *)pNetwork->bssht.bdHTCapBuf;
+//	PHT_INFORMATION_ELE		pPeerHTInfo = (PHT_INFORMATION_ELE)pNetwork->bssht.bdHTInfoBuf;
+//	u8*	pMcsFilter;
+	u8	bIOTAction = 0;
+
+	//
+	//  Save Peer Setting before Association
+	//
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "==============>%s()\n", __func__);
+	/*unmark bEnableHT flag here is the same reason why unmarked in function ieee80211_softmac_new_net. WB 2008.09.10*/
+//	if( pHTInfo->bEnableHT &&  pNetwork->bssht.bdSupportHT)
+	if (pNetwork->bssht.bdSupportHT) {
+		pHTInfo->bCurrentHTSupport = true;
+		pHTInfo->ePeerHTSpecVer = pNetwork->bssht.bdHTSpecVer;
+
+		// Save HTCap and HTInfo information Element
+		if (pNetwork->bssht.bdHTCapLen > 0 &&	pNetwork->bssht.bdHTCapLen <= sizeof(pHTInfo->PeerHTCapBuf))
+			memcpy(pHTInfo->PeerHTCapBuf, pNetwork->bssht.bdHTCapBuf, pNetwork->bssht.bdHTCapLen);
+
+		if (pNetwork->bssht.bdHTInfoLen > 0 && pNetwork->bssht.bdHTInfoLen <= sizeof(pHTInfo->PeerHTInfoBuf))
+			memcpy(pHTInfo->PeerHTInfoBuf, pNetwork->bssht.bdHTInfoBuf, pNetwork->bssht.bdHTInfoLen);
+
+		// Check whether RT to RT aggregation mode is enabled
+		if (pHTInfo->bRegRT2RTAggregation) {
+			pHTInfo->bCurrentRT2RTAggregation = pNetwork->bssht.bdRT2RTAggregation;
+			pHTInfo->bCurrentRT2RTLongSlotTime = pNetwork->bssht.bdRT2RTLongSlotTime;
+		} else {
+			pHTInfo->bCurrentRT2RTAggregation = false;
+			pHTInfo->bCurrentRT2RTLongSlotTime = false;
+		}
+
+		// Determine the IOT Peer Vendor.
+		HTIOTPeerDetermine(ieee);
+
+		/*
+		 * Decide IOT Action
+		 * Must be called after the parameter of pHTInfo->bCurrentRT2RTAggregation is decided
+		 */
+		pHTInfo->IOTAction = 0;
+		bIOTAction = HTIOTActIsDisableMCS14(ieee, pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_DISABLE_MCS14;
+
+		bIOTAction = HTIOTActIsDisableMCS15(ieee);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_DISABLE_MCS15;
+
+		bIOTAction = HTIOTActIsDisableMCSTwoSpatialStream(ieee, pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_DISABLE_ALL_2SS;
+
+		bIOTAction = HTIOTActIsDisableEDCATurbo(ieee, pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_DISABLE_EDCA_TURBO;
+
+		bIOTAction = HTIOTActIsMgntUseCCK6M(pNetwork);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_MGNT_USE_CCK_6M;
+
+		bIOTAction = HTIOTActIsCCDFsync(pNetwork->bssid);
+		if (bIOTAction)
+			pHTInfo->IOTAction |= HT_IOT_ACT_CDD_FSYNC;
+	} else {
+		pHTInfo->bCurrentHTSupport = false;
+		pHTInfo->bCurrentRT2RTAggregation = false;
+		pHTInfo->bCurrentRT2RTLongSlotTime = false;
+
+		pHTInfo->IOTAction = 0;
+	}
+}
+
+void HTUpdateSelfAndPeerSetting(struct ieee80211_device *ieee,	struct ieee80211_network *pNetwork)
+{
+	PRT_HIGH_THROUGHPUT	        pHTInfo = ieee->pHTInfo;
+//	struct ht_capability_ele       *pPeerHTCap = (struct ht_capability_ele *)pNetwork->bssht.bdHTCapBuf;
+	PHT_INFORMATION_ELE		pPeerHTInfo = (PHT_INFORMATION_ELE)pNetwork->bssht.bdHTInfoBuf;
+
+	if (pHTInfo->bCurrentHTSupport) {
+		/*
+		 * Config current operation mode.
+		 */
+		if (pNetwork->bssht.bdHTInfoLen != 0)
+			pHTInfo->CurrentOpMode = pPeerHTInfo->OptMode;
+
+		/*
+		 * <TODO: Config according to OBSS non-HT STA present!!>
+		 */
+	}
+}
+EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
+
+/*
+ *function:  check whether HT control field exists
+ *   input:  struct ieee80211_device	*ieee
+ *	     u8*			pFrame //coming skb->data
+ *  output:  none
+ *  return:  return true if HT control field exists(false otherwise)
+ *  notice:
+ */
+u8 HTCCheck(struct ieee80211_device *ieee, u8 *pFrame)
+{
+	if (ieee->pHTInfo->bCurrentHTSupport) {
+		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
+			IEEE80211_DEBUG(IEEE80211_DL_HT, "HT CONTROL FILED EXIST!!\n");
+			return true;
+		}
+	}
+	return false;
+}
+
+static void HTSetConnectBwModeCallback(struct ieee80211_device *ieee)
+{
+	PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
+
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "======>%s()\n", __func__);
+
+	if (pHTInfo->bCurBW40MHz) {
+		if (pHTInfo->CurSTAExtChnlOffset == HT_EXTCHNL_OFFSET_UPPER)
+			ieee->set_chan(ieee->dev, ieee->current_network.channel + 2);
+		else if (pHTInfo->CurSTAExtChnlOffset == HT_EXTCHNL_OFFSET_LOWER)
+			ieee->set_chan(ieee->dev, ieee->current_network.channel - 2);
+		else
+			ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20_40, pHTInfo->CurSTAExtChnlOffset);
+	} else {
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+		ieee->SetBWModeHandler(ieee->dev, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
+	}
+
+	pHTInfo->bSwBwInProgress = false;
+}
+
+/*
+ * This function set bandwidth mode in protocol layer.
+ */
+void HTSetConnectBwMode(struct ieee80211_device *ieee, enum ht_channel_width Bandwidth, enum ht_extension_chan_offset Offset)
+{
+	PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
+//	u32 flags = 0;
+
+	if (!pHTInfo->bRegBW40MHz)
+		return;
+
+	// To reduce dummy operation
+//	if((pHTInfo->bCurBW40MHz==false && Bandwidth==HT_CHANNEL_WIDTH_20) ||
+//	   (pHTInfo->bCurBW40MHz==true && Bandwidth==HT_CHANNEL_WIDTH_20_40 && Offset==pHTInfo->CurSTAExtChnlOffset))
+//		return;
+
+//	spin_lock_irqsave(&(ieee->bw_spinlock), flags);
+	if (pHTInfo->bSwBwInProgress) {
+//		spin_unlock_irqrestore(&(ieee->bw_spinlock), flags);
+		return;
+	}
+	//if in half N mode, set to 20M bandwidth please 09.08.2008 WB.
+	if (Bandwidth == HT_CHANNEL_WIDTH_20_40 && (!ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))) {
+			// Handle Illegal extension channel offset!!
+		if (ieee->current_network.channel < 2 && Offset == HT_EXTCHNL_OFFSET_LOWER)
+			Offset = HT_EXTCHNL_OFFSET_NO_EXT;
+		if (Offset == HT_EXTCHNL_OFFSET_UPPER || Offset == HT_EXTCHNL_OFFSET_LOWER) {
+			pHTInfo->bCurBW40MHz = true;
+			pHTInfo->CurSTAExtChnlOffset = Offset;
+		} else {
+			pHTInfo->bCurBW40MHz = false;
+			pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
+		}
+	} else {
+		pHTInfo->bCurBW40MHz = false;
+		pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
+	}
+
+	pHTInfo->bSwBwInProgress = true;
+
+	/*
+	 * TODO: 2007.7.13 by Emily Wait 2000ms  in order to guarantee that
+	 * switching bandwidth is executed after scan is finished. It is a
+	 * temporal solution because software should ganrantee the last
+	 * operation of switching bandwidth is executed properlly.
+	 */
+	HTSetConnectBwModeCallback(ieee);
+
+//	spin_unlock_irqrestore(&(ieee->bw_spinlock), flags);
+}
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h
new file mode 100644
index 000000000..3052f53d2
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h
@@ -0,0 +1,82 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __INC_QOS_TYPE_H
+#define __INC_QOS_TYPE_H
+
+/*
+ * ACI/AIFSN Field.
+ * Ref: WMM spec 2.2.2: WME Parameter Element, p.12.
+ * Note: 1 Byte Length
+ */
+struct aci_aifsn {
+	u8	aifsn:4;
+	u8	acm:1;
+	u8	aci:2;
+	u8:1;
+};
+
+/*
+ * Direction Field Values.
+ * Ref: WMM spec 2.2.11: WME TSPEC Element, p.18.
+ */
+enum direction_value {
+	DIR_UP			= 0,		// 0x00	// UpLink
+	DIR_DOWN		= 1,		// 0x01	// DownLink
+	DIR_DIRECT		= 2,		// 0x10	// DirectLink
+	DIR_BI_DIR		= 3,		// 0x11	// Bi-Direction
+};
+
+/*
+ * TS Info field in WMM TSPEC Element.
+ * Ref:
+ *	1. WMM spec 2.2.11: WME TSPEC Element, p.18.
+ *	2. 8185 QoS code: QOS_TSINFO [def. in QoS_mp.h]
+ * Note: sizeof 3 Bytes
+ */
+struct qos_tsinfo {
+	u16		uc_traffic_type:1;	        //WMM is reserved
+	u16		uc_tsid:4;
+	u16		uc_direction:2;
+	u16		uc_access_policy:2;	        //WMM: bit8=0, bit7=1
+	u16		uc_aggregation:1;	        //WMM is reserved
+	u16		uc_psb:1;		        //WMMSA is APSD
+	u16		uc_up:3;
+	u16		uc_ts_info_ack_policy:2;	//WMM is reserved
+	u8		uc_schedule:1;		        //WMM is reserved
+	u8:7;
+};
+
+/*
+ * WMM TSPEC Body.
+ * Ref: WMM spec 2.2.11: WME TSPEC Element, p.16.
+ * Note: sizeof 55 bytes
+ */
+struct tspec_body {
+	struct qos_tsinfo	ts_info;	//u8	TSInfo[3];
+	u16	nominal_msd_usize;
+	u16	max_msd_usize;
+	u32	min_service_itv;
+	u32	max_service_itv;
+	u32	inactivity_itv;
+	u32	suspen_itv;
+	u32	service_start_time;
+	u32	min_data_rate;
+	u32	mean_data_rate;
+	u32	peak_data_rate;
+	u32	max_burst_size;
+	u32	delay_bound;
+	u32	min_phy_rate;
+	u16	surplus_bandwidth_allowance;
+	u16	medium_time;
+};
+
+/*
+ *      802.11 Management frame Status Code field
+ */
+struct octet_string {
+	u8		*octet;
+	u16             length;
+};
+
+#define is_ac_valid(ac)			(((ac) <= 7) ? true : false)
+
+#endif // #ifndef __INC_QOS_TYPE_H
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h
new file mode 100644
index 000000000..7ed140009
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _TSTYPE_H_
+#define _TSTYPE_H_
+#include "rtl819x_Qos.h"
+
+#define TS_ADDBA_DELAY		60
+
+#define TOTAL_TS_NUM		16
+#define TCLAS_NUM		4
+
+/*  This define the Tx/Rx directions */
+enum tr_select {
+	TX_DIR = 0,
+	RX_DIR = 1,
+};
+
+union qos_tclas {
+	struct type_general {
+		u8		priority;
+		u8		classifier_type;
+		u8		mask;
+	} type_general;
+
+	struct type0_eth {
+		u8		priority;
+		u8		classifier_type;
+		u8		mask;
+		u8		src_addr[6];
+		u8		dst_addr[6];
+		u16		type;
+	} type0_eth;
+
+	struct type1_ipv4 {
+		u8		priority;
+		u8		classifier_type;
+		u8		mask;
+		u8		version;
+		u8		src_ip[4];
+		u8		dst_ip[4];
+		u16		src_port;
+		u16		dst_port;
+		u8		dscp;
+		u8		protocol;
+		u8		reserved;
+	} type1_ipv4;
+
+	struct type1_ipv6 {
+		u8		priority;
+		u8		classifier_type;
+		u8		mask;
+		u8		version;
+		u8		src_ip[16];
+		u8		dst_ip[16];
+		u16		src_port;
+		u16		dst_port;
+		u8		flow_label[3];
+	} type1_ipv6;
+
+	struct type2_8021q {
+		u8		priority;
+		u8		classifier_type;
+		u8		mask;
+		u16		tag_type;
+	} type2_8021q;
+};
+
+struct ts_common_info {
+	struct list_head		list;
+	struct timer_list		setup_timer;
+	struct timer_list		inact_timer;
+	u8				addr[6];
+	struct tspec_body		t_spec;
+	union qos_tclas			t_class[TCLAS_NUM];
+	u8				t_clas_proc;
+	u8				t_clas_num;
+};
+
+struct tx_ts_record {
+	struct ts_common_info		ts_common_info;
+	u16				tx_cur_seq;
+	struct ba_record		tx_pending_ba_record;
+	struct ba_record		tx_admitted_ba_record;
+	u8				add_ba_req_in_progress;
+	u8				add_ba_req_delayed;
+	u8				using_ba;
+	struct timer_list		ts_add_ba_timer;
+	u8				num;
+};
+
+struct rx_ts_record {
+	struct ts_common_info		ts_common_info;
+	u16				rx_indicate_seq;
+	u16				rx_timeout_indicate_seq;
+	struct list_head		rx_pending_pkt_list;
+	struct timer_list		rx_pkt_pending_timer;
+	struct ba_record		rx_admitted_ba_record;
+	u16				rx_last_seq_num;
+	u8				rx_last_frag_num;
+	u8				num;
+};
+
+#endif
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
new file mode 100644
index 000000000..3aabb401b
--- /dev/null
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -0,0 +1,534 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "ieee80211.h"
+#include <linux/etherdevice.h>
+#include <linux/slab.h>
+#include "rtl819x_TS.h"
+
+static void TsSetupTimeOut(struct timer_list *unused)
+{
+	// Not implement yet
+	// This is used for WMMSA and ACM , that would send ADDTSReq frame.
+}
+
+static void TsInactTimeout(struct timer_list *unused)
+{
+	// Not implement yet
+	// This is used for WMMSA and ACM.
+	// This function would be call when TS is no Tx/Rx for some period of time.
+}
+
+/********************************************************************************************************************
+ *function:  I still not understand this function, so wait for further implementation
+ *   input:  unsigned long	 data		//acturally we send struct tx_ts_record or struct rx_ts_record to these timer
+ *  return:  NULL
+ *  notice:
+ ********************************************************************************************************************/
+static void RxPktPendingTimeout(struct timer_list *t)
+{
+	struct rx_ts_record     *pRxTs = from_timer(pRxTs, t, rx_pkt_pending_timer);
+	struct ieee80211_device *ieee = container_of(pRxTs, struct ieee80211_device, RxTsRecord[pRxTs->num]);
+
+	struct rx_reorder_entry	*pReorderEntry = NULL;
+
+	//u32 flags = 0;
+	unsigned long flags = 0;
+	u8 index = 0;
+	bool bPktInBuf = false;
+
+	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER, "==================>%s()\n", __func__);
+	if (pRxTs->rx_timeout_indicate_seq != 0xffff) {
+		// Indicate the pending packets sequentially according to SeqNum until meet the gap.
+		while (!list_empty(&pRxTs->rx_pending_pkt_list)) {
+			pReorderEntry = list_entry(pRxTs->rx_pending_pkt_list.prev, struct rx_reorder_entry, List);
+			if (index == 0)
+				pRxTs->rx_indicate_seq = pReorderEntry->SeqNum;
+
+			if (SN_LESS(pReorderEntry->SeqNum, pRxTs->rx_indicate_seq) ||
+				SN_EQUAL(pReorderEntry->SeqNum, pRxTs->rx_indicate_seq)) {
+				list_del_init(&pReorderEntry->List);
+
+				if (SN_EQUAL(pReorderEntry->SeqNum, pRxTs->rx_indicate_seq))
+					pRxTs->rx_indicate_seq = (pRxTs->rx_indicate_seq + 1) % 4096;
+
+				IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s: IndicateSeq: %d\n", __func__, pReorderEntry->SeqNum);
+				ieee->stats_IndicateArray[index] = pReorderEntry->prxb;
+				index++;
+
+				list_add_tail(&pReorderEntry->List, &ieee->RxReorder_Unused_List);
+			} else {
+				bPktInBuf = true;
+				break;
+			}
+		}
+	}
+
+	if (index > 0) {
+		// Set rx_timeout_indicate_seq to 0xffff to indicate no pending packets in buffer now.
+		pRxTs->rx_timeout_indicate_seq = 0xffff;
+
+		// Indicate packets
+		if (index > REORDER_WIN_SIZE) {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): Rx Reorder buffer full!! \n");
+			spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+			return;
+		}
+		ieee80211_indicate_packets(ieee, ieee->stats_IndicateArray, index);
+	}
+
+	if (bPktInBuf && (pRxTs->rx_timeout_indicate_seq == 0xffff)) {
+		pRxTs->rx_timeout_indicate_seq = pRxTs->rx_indicate_seq;
+		mod_timer(&pRxTs->rx_pkt_pending_timer,
+			  jiffies + msecs_to_jiffies(ieee->pHTInfo->RxReorderPendingTime));
+	}
+	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+}
+
+/********************************************************************************************************************
+ *function:  Add BA timer function
+ *   input:  unsigned long	 data		//acturally we send struct tx_ts_record or struct rx_ts_record to these timer
+ *  return:  NULL
+ *  notice:
+ ********************************************************************************************************************/
+static void TsAddBaProcess(struct timer_list *t)
+{
+	struct tx_ts_record *pTxTs = from_timer(pTxTs, t, ts_add_ba_timer);
+	u8 num = pTxTs->num;
+	struct ieee80211_device *ieee = container_of(pTxTs, struct ieee80211_device, TxTsRecord[num]);
+
+	TsInitAddBA(ieee, pTxTs, BA_POLICY_IMMEDIATE, false);
+	IEEE80211_DEBUG(IEEE80211_DL_BA, "%s: ADDBA Req is started!! \n", __func__);
+}
+
+
+static void ResetTsCommonInfo(struct ts_common_info *pTsCommonInfo)
+{
+	eth_zero_addr(pTsCommonInfo->addr);
+	memset(&pTsCommonInfo->t_spec, 0, sizeof(struct tspec_body));
+	memset(&pTsCommonInfo->t_class, 0, sizeof(union qos_tclas) * TCLAS_NUM);
+	pTsCommonInfo->t_clas_proc = 0;
+	pTsCommonInfo->t_clas_num = 0;
+}
+
+static void ResetTxTsEntry(struct tx_ts_record *pTS)
+{
+	ResetTsCommonInfo(&pTS->ts_common_info);
+	pTS->tx_cur_seq = 0;
+	pTS->add_ba_req_in_progress = false;
+	pTS->add_ba_req_delayed = false;
+	pTS->using_ba = false;
+	ResetBaEntry(&pTS->tx_admitted_ba_record); //For BA Originator
+	ResetBaEntry(&pTS->tx_pending_ba_record);
+}
+
+static void ResetRxTsEntry(struct rx_ts_record *pTS)
+{
+	ResetTsCommonInfo(&pTS->ts_common_info);
+	pTS->rx_indicate_seq = 0xffff; // This indicate the rx_indicate_seq is not used now!!
+	pTS->rx_timeout_indicate_seq = 0xffff; // This indicate the rx_timeout_indicate_seq is not used now!!
+	ResetBaEntry(&pTS->rx_admitted_ba_record);	  // For BA Recipient
+}
+
+void TSInitialize(struct ieee80211_device *ieee)
+{
+	struct tx_ts_record     *pTxTS  = ieee->TxTsRecord;
+	struct rx_ts_record     *pRxTS  = ieee->RxTsRecord;
+	struct rx_reorder_entry	*pRxReorderEntry = ieee->RxReorderEntry;
+	u8				count = 0;
+	IEEE80211_DEBUG(IEEE80211_DL_TS, "==========>%s()\n", __func__);
+	// Initialize Tx TS related info.
+	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
+	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
+	INIT_LIST_HEAD(&ieee->Tx_TS_Unused_List);
+
+	for (count = 0; count < TOTAL_TS_NUM; count++) {
+		//
+		pTxTS->num = count;
+		// The timers for the operation of Traffic Stream and Block Ack.
+		// DLS related timer will be add here in the future!!
+		timer_setup(&pTxTS->ts_common_info.setup_timer, TsSetupTimeOut,
+			    0);
+		timer_setup(&pTxTS->ts_common_info.inact_timer, TsInactTimeout,
+			    0);
+		timer_setup(&pTxTS->ts_add_ba_timer, TsAddBaProcess, 0);
+		timer_setup(&pTxTS->tx_pending_ba_record.timer, BaSetupTimeOut,
+			    0);
+		timer_setup(&pTxTS->tx_admitted_ba_record.timer,
+			    TxBaInactTimeout, 0);
+		ResetTxTsEntry(pTxTS);
+		list_add_tail(&pTxTS->ts_common_info.list, &ieee->Tx_TS_Unused_List);
+		pTxTS++;
+	}
+
+	// Initialize Rx TS related info.
+	INIT_LIST_HEAD(&ieee->Rx_TS_Admit_List);
+	INIT_LIST_HEAD(&ieee->Rx_TS_Pending_List);
+	INIT_LIST_HEAD(&ieee->Rx_TS_Unused_List);
+	for (count = 0; count < TOTAL_TS_NUM; count++) {
+		pRxTS->num = count;
+		INIT_LIST_HEAD(&pRxTS->rx_pending_pkt_list);
+		timer_setup(&pRxTS->ts_common_info.setup_timer, TsSetupTimeOut,
+			    0);
+		timer_setup(&pRxTS->ts_common_info.inact_timer, TsInactTimeout,
+			    0);
+		timer_setup(&pRxTS->rx_admitted_ba_record.timer,
+			    RxBaInactTimeout, 0);
+		timer_setup(&pRxTS->rx_pkt_pending_timer, RxPktPendingTimeout, 0);
+		ResetRxTsEntry(pRxTS);
+		list_add_tail(&pRxTS->ts_common_info.list, &ieee->Rx_TS_Unused_List);
+		pRxTS++;
+	}
+	// Initialize unused Rx Reorder List.
+	INIT_LIST_HEAD(&ieee->RxReorder_Unused_List);
+	for (count = 0; count < REORDER_ENTRY_NUM; count++) {
+		list_add_tail(&pRxReorderEntry->List, &ieee->RxReorder_Unused_List);
+		if (count == (REORDER_ENTRY_NUM - 1))
+			break;
+		pRxReorderEntry = &ieee->RxReorderEntry[count + 1];
+	}
+}
+
+static void AdmitTS(struct ieee80211_device *ieee,
+		    struct ts_common_info *pTsCommonInfo, u32 InactTime)
+{
+	del_timer_sync(&pTsCommonInfo->setup_timer);
+	del_timer_sync(&pTsCommonInfo->inact_timer);
+
+	if (InactTime != 0)
+		mod_timer(&pTsCommonInfo->inact_timer,
+			  jiffies + msecs_to_jiffies(InactTime));
+}
+
+
+static struct ts_common_info *SearchAdmitTRStream(struct ieee80211_device *ieee,
+						  u8 *Addr, u8 TID,
+						  enum tr_select TxRxSelect)
+{
+	//DIRECTION_VALUE	dir;
+	u8	dir;
+	bool				search_dir[4] = {0};
+	struct list_head		*psearch_list; //FIXME
+	struct ts_common_info	*pRet = NULL;
+	if (ieee->iw_mode == IW_MODE_MASTER) { //ap mode
+		if (TxRxSelect == TX_DIR) {
+			search_dir[DIR_DOWN] = true;
+			search_dir[DIR_BI_DIR] = true;
+		} else {
+			search_dir[DIR_UP]	= true;
+			search_dir[DIR_BI_DIR] = true;
+		}
+	} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+		if (TxRxSelect == TX_DIR)
+			search_dir[DIR_UP]	= true;
+		else
+			search_dir[DIR_DOWN] = true;
+	} else {
+		if (TxRxSelect == TX_DIR) {
+			search_dir[DIR_UP]	= true;
+			search_dir[DIR_BI_DIR] = true;
+			search_dir[DIR_DIRECT] = true;
+		} else {
+			search_dir[DIR_DOWN] = true;
+			search_dir[DIR_BI_DIR] = true;
+			search_dir[DIR_DIRECT] = true;
+		}
+	}
+
+	if (TxRxSelect == TX_DIR)
+		psearch_list = &ieee->Tx_TS_Admit_List;
+	else
+		psearch_list = &ieee->Rx_TS_Admit_List;
+
+	//for(dir = DIR_UP; dir <= DIR_BI_DIR; dir++)
+	for (dir = 0; dir <= DIR_BI_DIR; dir++) {
+		if (!search_dir[dir])
+			continue;
+		list_for_each_entry(pRet, psearch_list, list) {
+	//		IEEE80211_DEBUG(IEEE80211_DL_TS, "ADD:%pM, TID:%d, dir:%d\n", pRet->Addr, pRet->TSpec.ts_info.ucTSID, pRet->TSpec.ts_info.ucDirection);
+			if (memcmp(pRet->addr, Addr, 6) == 0)
+				if (pRet->t_spec.ts_info.uc_tsid == TID)
+					if (pRet->t_spec.ts_info.uc_direction == dir) {
+	//					printk("Bingo! got it\n");
+						break;
+					}
+		}
+		if (&pRet->list  != psearch_list)
+			break;
+	}
+
+	if (&pRet->list  != psearch_list)
+		return pRet;
+	else
+		return NULL;
+}
+
+static void MakeTSEntry(struct ts_common_info *pTsCommonInfo, u8 *Addr,
+			struct tspec_body *pTSPEC, union qos_tclas *pTCLAS, u8 TCLAS_Num,
+			u8 TCLAS_Proc)
+{
+	u8	count;
+
+	if (pTsCommonInfo == NULL)
+		return;
+
+	memcpy(pTsCommonInfo->addr, Addr, 6);
+
+	if (pTSPEC != NULL)
+		memcpy((u8 *)(&(pTsCommonInfo->t_spec)), (u8 *)pTSPEC, sizeof(struct tspec_body));
+
+	for (count = 0; count < TCLAS_Num; count++)
+		memcpy((u8 *)(&(pTsCommonInfo->t_class[count])), (u8 *)pTCLAS, sizeof(union qos_tclas));
+
+	pTsCommonInfo->t_clas_proc = TCLAS_Proc;
+	pTsCommonInfo->t_clas_num = TCLAS_Num;
+}
+
+
+bool GetTs(
+	struct ieee80211_device		*ieee,
+	struct ts_common_info		**ppTS,
+	u8				*Addr,
+	u8				TID,
+	enum tr_select			TxRxSelect,  //Rx:1, Tx:0
+	bool				bAddNewTs
+	)
+{
+	u8	UP = 0;
+	//
+	// We do not build any TS for Broadcast or Multicast stream.
+	// So reject these kinds of search here.
+	//
+	if (is_multicast_ether_addr(Addr)) {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "get TS for Broadcast or Multicast\n");
+		return false;
+	}
+
+	if (ieee->current_network.qos_data.supported == 0) {
+		UP = 0;
+	} else {
+		// In WMM case: we use 4 TID only
+		if (!is_ac_valid(TID)) {
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __func__, TID);
+			return false;
+		}
+
+		switch (TID) {
+		case 0:
+		case 3:
+			UP = 0;
+			break;
+
+		case 1:
+		case 2:
+			UP = 2;
+			break;
+
+		case 4:
+		case 5:
+			UP = 5;
+			break;
+
+		case 6:
+		case 7:
+			UP = 7;
+			break;
+		}
+	}
+
+	*ppTS = SearchAdmitTRStream(
+			ieee,
+			Addr,
+			UP,
+			TxRxSelect);
+	if (*ppTS != NULL) {
+		return true;
+	} else {
+		if (!bAddNewTs) {
+			IEEE80211_DEBUG(IEEE80211_DL_TS, "add new TS failed(tid:%d)\n", UP);
+			return false;
+		} else {
+			//
+			// Create a new Traffic stream for current Tx/Rx
+			// This is for EDCA and WMM to add a new TS.
+			// For HCCA or WMMSA, TS cannot be addmit without negotiation.
+			//
+			struct tspec_body	TSpec;
+			struct qos_tsinfo	*pTSInfo = &TSpec.ts_info;
+			struct list_head	*pUnusedList =
+								(TxRxSelect == TX_DIR) ?
+								(&ieee->Tx_TS_Unused_List) :
+								(&ieee->Rx_TS_Unused_List);
+
+			struct list_head	*pAddmitList =
+								(TxRxSelect == TX_DIR) ?
+								(&ieee->Tx_TS_Admit_List) :
+								(&ieee->Rx_TS_Admit_List);
+
+			enum direction_value	Dir =		(ieee->iw_mode == IW_MODE_MASTER) ?
+								((TxRxSelect == TX_DIR) ? DIR_DOWN : DIR_UP) :
+								((TxRxSelect == TX_DIR) ? DIR_UP : DIR_DOWN);
+			IEEE80211_DEBUG(IEEE80211_DL_TS, "to add Ts\n");
+			if (!list_empty(pUnusedList)) {
+				(*ppTS) = list_entry(pUnusedList->next, struct ts_common_info, list);
+				list_del_init(&(*ppTS)->list);
+				if (TxRxSelect == TX_DIR) {
+					struct tx_ts_record *tmp = container_of(*ppTS, struct tx_ts_record, ts_common_info);
+					ResetTxTsEntry(tmp);
+				} else {
+					struct rx_ts_record *tmp = container_of(*ppTS, struct rx_ts_record, ts_common_info);
+					ResetRxTsEntry(tmp);
+				}
+
+				IEEE80211_DEBUG(IEEE80211_DL_TS, "to init current TS, UP:%d, Dir:%d, addr:%pM\n", UP, Dir, Addr);
+				// Prepare TS Info related field
+				pTSInfo->uc_traffic_type = 0;		// Traffic type: WMM is reserved in this field
+				pTSInfo->uc_tsid = UP;			// TSID
+				pTSInfo->uc_direction = Dir;		// Direction: if there is DirectLink, this need additional consideration.
+				pTSInfo->uc_access_policy = 1;		// Access policy
+				pTSInfo->uc_aggregation = 0;		// Aggregation
+				pTSInfo->uc_psb = 0;			// Aggregation
+				pTSInfo->uc_up = UP;			// User priority
+				pTSInfo->uc_ts_info_ack_policy = 0;	// Ack policy
+				pTSInfo->uc_schedule = 0;		// Schedule
+
+				MakeTSEntry(*ppTS, Addr, &TSpec, NULL, 0, 0);
+				AdmitTS(ieee, *ppTS, 0);
+				list_add_tail(&((*ppTS)->list), pAddmitList);
+				// if there is DirectLink, we need to do additional operation here!!
+
+				return true;
+			} else {
+				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __func__);
+				return false;
+			}
+		}
+	}
+}
+
+static void RemoveTsEntry(struct ieee80211_device *ieee, struct ts_common_info *pTs,
+			  enum tr_select TxRxSelect)
+{
+	//u32 flags = 0;
+	unsigned long flags = 0;
+	del_timer_sync(&pTs->setup_timer);
+	del_timer_sync(&pTs->inact_timer);
+	TsInitDelBA(ieee, pTs, TxRxSelect);
+
+	if (TxRxSelect == RX_DIR) {
+		struct rx_reorder_entry	*pRxReorderEntry;
+		struct rx_ts_record     *pRxTS = (struct rx_ts_record *)pTs;
+		if (timer_pending(&pRxTS->rx_pkt_pending_timer))
+			del_timer_sync(&pRxTS->rx_pkt_pending_timer);
+
+		while (!list_empty(&pRxTS->rx_pending_pkt_list)) {
+			spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
+			//pRxReorderEntry = list_entry(&pRxTS->rx_pending_pkt_list.prev,RX_REORDER_ENTRY,List);
+			pRxReorderEntry = list_entry(pRxTS->rx_pending_pkt_list.prev, struct rx_reorder_entry, List);
+			list_del_init(&pRxReorderEntry->List);
+			{
+				int i = 0;
+				struct ieee80211_rxb *prxb = pRxReorderEntry->prxb;
+				if (unlikely(!prxb)) {
+					spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+					return;
+				}
+				for (i =  0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+
+				kfree(prxb);
+				prxb = NULL;
+			}
+			list_add_tail(&pRxReorderEntry->List, &ieee->RxReorder_Unused_List);
+			spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
+		}
+
+	} else {
+		struct tx_ts_record *pTxTS = (struct tx_ts_record *)pTs;
+		del_timer_sync(&pTxTS->ts_add_ba_timer);
+	}
+}
+
+void RemovePeerTS(struct ieee80211_device *ieee, u8 *Addr)
+{
+	struct ts_common_info	*pTS, *pTmpTS;
+
+	printk("===========>%s,%pM\n", __func__, Addr);
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, list) {
+		if (memcmp(pTS->addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, TX_DIR);
+			list_del_init(&pTS->list);
+			list_add_tail(&pTS->list, &ieee->Tx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, list) {
+		if (memcmp(pTS->addr, Addr, 6) == 0) {
+			printk("====>remove Tx_TS_admin_list\n");
+			RemoveTsEntry(ieee, pTS, TX_DIR);
+			list_del_init(&pTS->list);
+			list_add_tail(&pTS->list, &ieee->Tx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, list) {
+		if (memcmp(pTS->addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, RX_DIR);
+			list_del_init(&pTS->list);
+			list_add_tail(&pTS->list, &ieee->Rx_TS_Unused_List);
+		}
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, list) {
+		if (memcmp(pTS->addr, Addr, 6) == 0) {
+			RemoveTsEntry(ieee, pTS, RX_DIR);
+			list_del_init(&pTS->list);
+			list_add_tail(&pTS->list, &ieee->Rx_TS_Unused_List);
+		}
+	}
+}
+
+void RemoveAllTS(struct ieee80211_device *ieee)
+{
+	struct ts_common_info *pTS, *pTmpTS;
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, list) {
+		RemoveTsEntry(ieee, pTS, TX_DIR);
+		list_del_init(&pTS->list);
+		list_add_tail(&pTS->list, &ieee->Tx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, list) {
+		RemoveTsEntry(ieee, pTS, TX_DIR);
+		list_del_init(&pTS->list);
+		list_add_tail(&pTS->list, &ieee->Tx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, list) {
+		RemoveTsEntry(ieee, pTS, RX_DIR);
+		list_del_init(&pTS->list);
+		list_add_tail(&pTS->list, &ieee->Rx_TS_Unused_List);
+	}
+
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, list) {
+		RemoveTsEntry(ieee, pTS, RX_DIR);
+		list_del_init(&pTS->list);
+		list_add_tail(&pTS->list, &ieee->Rx_TS_Unused_List);
+	}
+}
+
+void TsStartAddBaProcess(struct ieee80211_device *ieee, struct tx_ts_record *pTxTS)
+{
+	if (!pTxTS->add_ba_req_in_progress) {
+		pTxTS->add_ba_req_in_progress = true;
+		if (pTxTS->add_ba_req_delayed)	{
+			IEEE80211_DEBUG(IEEE80211_DL_BA, "%s: Delayed Start ADDBA after 60 sec!!\n", __func__);
+			mod_timer(&pTxTS->ts_add_ba_timer,
+				  jiffies + msecs_to_jiffies(TS_ADDBA_DELAY));
+		} else {
+			IEEE80211_DEBUG(IEEE80211_DL_BA, "%s: Immediately Start ADDBA now!!\n", __func__);
+			mod_timer(&pTxTS->ts_add_ba_timer, jiffies + 10); //set 10 ticks
+		}
+	} else {
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __func__);
+	}
+}
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.c b/drivers/staging/rtl8192u/r8180_93cx6.c
new file mode 100644
index 000000000..2527cea60
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8180_93cx6.c
@@ -0,0 +1,170 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *  This files contains card eeprom (93c46 or 93c56) programming routines,
+ *  memory is addressed by 16 bits words.
+ *
+ *  This is part of rtl8180 OpenSource driver.
+ *  Copyright (C) Andrea Merello 2004  <andrea.merello@gmail.com>
+ *
+ *  Parts of this driver are based on the GPL part of the
+ *  official realtek driver.
+ *
+ *  Parts of this driver are based on the rtl8180 driver skeleton
+ *  from Patric Schenke & Andres Salomon.
+ *
+ *  Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+ *
+ *  We want to thank the Authors of those projects and the Ndiswrapper
+ *  project Authors.
+ */
+
+#include "r8180_93cx6.h"
+
+static void eprom_cs(struct net_device *dev, short bit)
+{
+	u8 cmdreg;
+	int err;
+
+	err = read_nic_byte_E(dev, EPROM_CMD, &cmdreg);
+	if (err)
+		return;
+	if (bit)
+		/* enable EPROM */
+		write_nic_byte_E(dev, EPROM_CMD, cmdreg | EPROM_CS_BIT);
+	else
+		/* disable EPROM */
+		write_nic_byte_E(dev, EPROM_CMD, cmdreg & ~EPROM_CS_BIT);
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+static void eprom_ck_cycle(struct net_device *dev)
+{
+	u8 cmdreg;
+	int err;
+
+	err = read_nic_byte_E(dev, EPROM_CMD, &cmdreg);
+	if (err)
+		return;
+	write_nic_byte_E(dev, EPROM_CMD, cmdreg | EPROM_CK_BIT);
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+
+	read_nic_byte_E(dev, EPROM_CMD, &cmdreg);
+	write_nic_byte_E(dev, EPROM_CMD, cmdreg & ~EPROM_CK_BIT);
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+static void eprom_w(struct net_device *dev, short bit)
+{
+	u8 cmdreg;
+	int err;
+
+	err = read_nic_byte_E(dev, EPROM_CMD, &cmdreg);
+	if (err)
+		return;
+	if (bit)
+		write_nic_byte_E(dev, EPROM_CMD, cmdreg | EPROM_W_BIT);
+	else
+		write_nic_byte_E(dev, EPROM_CMD, cmdreg & ~EPROM_W_BIT);
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+static short eprom_r(struct net_device *dev)
+{
+	u8 bit;
+	int err;
+
+	err = read_nic_byte_E(dev, EPROM_CMD, &bit);
+	if (err)
+		return err;
+
+	udelay(EPROM_DELAY);
+
+	if (bit & EPROM_R_BIT)
+		return 1;
+
+	return 0;
+}
+
+static void eprom_send_bits_string(struct net_device *dev, short b[], int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		eprom_w(dev, b[i]);
+		eprom_ck_cycle(dev);
+	}
+}
+
+int eprom_read(struct net_device *dev, u32 addr)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	short read_cmd[] = {1, 1, 0};
+	short addr_str[8];
+	int i;
+	int addr_len;
+	u32 ret;
+	int err;
+
+	ret = 0;
+	/* enable EPROM programming */
+	write_nic_byte_E(dev, EPROM_CMD,
+		       (EPROM_CMD_PROGRAM << EPROM_CMD_OPERATING_MODE_SHIFT));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+
+	if (priv->epromtype == EPROM_93c56) {
+		addr_str[7] = addr & 1;
+		addr_str[6] = addr & BIT(1);
+		addr_str[5] = addr & BIT(2);
+		addr_str[4] = addr & BIT(3);
+		addr_str[3] = addr & BIT(4);
+		addr_str[2] = addr & BIT(5);
+		addr_str[1] = addr & BIT(6);
+		addr_str[0] = addr & BIT(7);
+		addr_len = 8;
+	} else {
+		addr_str[5] = addr & 1;
+		addr_str[4] = addr & BIT(1);
+		addr_str[3] = addr & BIT(2);
+		addr_str[2] = addr & BIT(3);
+		addr_str[1] = addr & BIT(4);
+		addr_str[0] = addr & BIT(5);
+		addr_len = 6;
+	}
+	eprom_cs(dev, 1);
+	eprom_ck_cycle(dev);
+	eprom_send_bits_string(dev, read_cmd, 3);
+	eprom_send_bits_string(dev, addr_str, addr_len);
+
+	/*
+	 * keep chip pin D to low state while reading.
+	 * I'm unsure if it is necessary, but anyway shouldn't hurt
+	 */
+	eprom_w(dev, 0);
+
+	for (i = 0; i < 16; i++) {
+		/* eeprom needs a clk cycle between writing opcode&adr
+		 * and reading data. (eeprom outs a dummy 0)
+		 */
+		eprom_ck_cycle(dev);
+		err = eprom_r(dev);
+		if (err < 0)
+			return err;
+
+		ret |= err << (15 - i);
+	}
+
+	eprom_cs(dev, 0);
+	eprom_ck_cycle(dev);
+
+	/* disable EPROM programming */
+	write_nic_byte_E(dev, EPROM_CMD,
+		       (EPROM_CMD_NORMAL << EPROM_CMD_OPERATING_MODE_SHIFT));
+	return ret;
+}
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.h b/drivers/staging/rtl8192u/r8180_93cx6.h
new file mode 100644
index 000000000..0cdd00a4f
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8180_93cx6.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *	This is part of rtl8187 OpenSource driver
+ *	Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
+ *	Released under the terms of GPL (General Public Licence)
+ *
+ *	Parts of this driver are based on the GPL part of the
+ *	official realtek driver
+ *	Parts of this driver are based on the rtl8180 driver skeleton
+ *	from Patric Schenke & Andres Salomon
+ *	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+ *
+ *	We want to thank the Authors of such projects and the Ndiswrapper
+ *	project Authors.
+ */
+
+/*This files contains card eeprom (93c46 or 93c56) programming routines*/
+/*memory is addressed by WORDS*/
+
+#include "r8192U.h"
+#include "r8192U_hw.h"
+
+#define EPROM_DELAY 10
+
+int eprom_read(struct net_device *dev, u32 addr); /* reads a 16 bits word */
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.c b/drivers/staging/rtl8192u/r8190_rtl8256.c
new file mode 100644
index 000000000..54747fda5
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.c
@@ -0,0 +1,294 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This is part of the rtl8192 driver
+ *
+ * This files contains programming code for the rtl8256
+ * radio frontend.
+ *
+ * *Many* thanks to Realtek Corp. for their great support!
+ */
+
+#include "r8192U.h"
+#include "r8192U_hw.h"
+#include "r819xU_phyreg.h"
+#include "r819xU_phy.h"
+#include "r8190_rtl8256.h"
+
+/*
+ * Forward declaration of local functions
+ */
+static void phy_rf8256_config_para_file(struct net_device *dev);
+
+/*--------------------------------------------------------------------------
+ * Overview:	set RF band width (20M or 40M)
+ * Input:       struct net_device*	dev
+ *		WIRELESS_BANDWIDTH_E	Bandwidth	//20M or 40M
+ * Output:      NONE
+ * Return:      NONE
+ * Note:	8226 support both 20M  and 40 MHz
+ *--------------------------------------------------------------------------
+ */
+void phy_set_rf8256_bandwidth(struct net_device *dev, enum ht_channel_width Bandwidth)
+{
+	u8	eRFPath;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* for(eRFPath = RF90_PATH_A; eRFPath <pHalData->NumTotalRFPath;
+	 *  eRFPath++)
+	 */
+	for (eRFPath = 0; eRFPath < RF90_PATH_MAX; eRFPath++) {
+		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
+			continue;
+
+		switch (Bandwidth) {
+		case HT_CHANNEL_WIDTH_20:
+				if (priv->card_8192_version == VERSION_819XU_A ||
+				    priv->card_8192_version == VERSION_819XU_B) {
+					/* 8256 D-cut, E-cut, xiong: consider it later! */
+					rtl8192_phy_SetRFReg(dev,
+						(enum rf90_radio_path_e)eRFPath,
+						0x0b, bMask12Bits, 0x100); /* phy para:1ba */
+					rtl8192_phy_SetRFReg(dev,
+						(enum rf90_radio_path_e)eRFPath,
+						0x2c, bMask12Bits, 0x3d7);
+					rtl8192_phy_SetRFReg(dev,
+						(enum rf90_radio_path_e)eRFPath,
+						0x0e, bMask12Bits, 0x021);
+					rtl8192_phy_SetRFReg(dev,
+						(enum rf90_radio_path_e)eRFPath,
+						0x14, bMask12Bits, 0x5ab);
+				} else {
+					RT_TRACE(COMP_ERR, "%s(): unknown hardware version\n", __func__);
+					}
+				break;
+		case HT_CHANNEL_WIDTH_20_40:
+				if (priv->card_8192_version == VERSION_819XU_A || priv->card_8192_version == VERSION_819XU_B) { /* 8256 D-cut, E-cut, xiong: consider it later! */
+					rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x0b, bMask12Bits, 0x300); /* phy para:3ba */
+					rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x2c, bMask12Bits, 0x3df);
+					rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x0e, bMask12Bits, 0x0a1);
+
+					if (priv->chan == 3 || priv->chan == 9)
+						/* I need to set priv->chan whenever current channel changes */
+						rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x14, bMask12Bits, 0x59b);
+					else
+						rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x14, bMask12Bits, 0x5ab);
+				} else {
+					RT_TRACE(COMP_ERR, "%s(): unknown hardware version\n", __func__);
+					}
+				break;
+		default:
+				RT_TRACE(COMP_ERR, "%s(): unknown Bandwidth: %#X\n", __func__, Bandwidth);
+				break;
+		}
+	}
+}
+
+/*--------------------------------------------------------------------------
+ * Overview:    Interface to config 8256
+ * Input:       struct net_device*	dev
+ * Output:      NONE
+ * Return:      NONE
+ *--------------------------------------------------------------------------
+ */
+void phy_rf8256_config(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/* Initialize general global value
+	 *
+	 * TODO: Extend RF_PATH_C and RF_PATH_D in the future
+	 */
+	priv->NumTotalRFPath = RTL819X_TOTAL_RF_PATH;
+	/* Config BB and RF */
+	phy_rf8256_config_para_file(dev);
+}
+
+/*--------------------------------------------------------------------------
+ * Overview:    Interface to config 8256
+ * Input:       struct net_device*	dev
+ * Output:      NONE
+ * Return:      NONE
+ *--------------------------------------------------------------------------
+ */
+static void phy_rf8256_config_para_file(struct net_device *dev)
+{
+	u32	u4RegValue = 0;
+	u8	eRFPath;
+	BB_REGISTER_DEFINITION_T	*pPhyReg;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32	RegOffSetToBeCheck = 0x3;
+	u32	RegValueToBeCheck = 0x7f1;
+	u32	RF3_Final_Value = 0;
+	u8	ConstRetryTimes = 5, RetryTimes = 5;
+	u8 ret = 0;
+	/* Initialize RF */
+	for (eRFPath = (enum rf90_radio_path_e)RF90_PATH_A; eRFPath < priv->NumTotalRFPath; eRFPath++) {
+		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
+			continue;
+
+		pPhyReg = &priv->PHYRegDef[eRFPath];
+
+		/* Joseph test for shorten RF config
+		 * pHalData->RfReg0Value[eRFPath] =  rtl8192_phy_QueryRFReg(dev, (enum rf90_radio_path_e)eRFPath, rGlobalCtrl, bMaskDWord);
+		 * ----Store original RFENV control type
+		 */
+		switch (eRFPath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4RegValue = rtl8192_QueryBBReg(dev, pPhyReg->rfintfs, bRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4RegValue = rtl8192_QueryBBReg(dev, pPhyReg->rfintfs, bRFSI_RFENV << 16);
+			break;
+		}
+
+		/*----Set RF_ENV enable----*/
+		rtl8192_setBBreg(dev, pPhyReg->rfintfe, bRFSI_RFENV << 16, 0x1);
+
+		/*----Set RF_ENV output high----*/
+		rtl8192_setBBreg(dev, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
+
+		/* Set bit number of Address and Data for RF register */
+		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/* Set 0 to 4 bits for Z-serial and set 1 to 6 bits for 8258 */
+		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/* Set 0 to 12 bits for Z-serial and 8258, and set 1 to 14 bits for ??? */
+
+		rtl8192_phy_SetRFReg(dev, (enum rf90_radio_path_e)eRFPath, 0x0, bMask12Bits, 0xbf);
+
+		/* Check RF block (for FPGA platform only)----
+		 * TODO: this function should be removed on ASIC , Emily 2007.2.2
+		 */
+		if (rtl8192_phy_checkBBAndRF(dev, HW90_BLOCK_RF, (enum rf90_radio_path_e)eRFPath)) {
+			RT_TRACE(COMP_ERR, "phy_rf8256_config():Check Radio[%d] Fail!!\n", eRFPath);
+			goto phy_RF8256_Config_ParaFile_Fail;
+		}
+
+		RetryTimes = ConstRetryTimes;
+		RF3_Final_Value = 0;
+		/*----Initialize RF fom connfiguration file----*/
+		switch (eRFPath) {
+		case RF90_PATH_A:
+			while (RF3_Final_Value != RegValueToBeCheck && RetryTimes != 0) {
+				ret = rtl8192_phy_ConfigRFWithHeaderFile(dev, (enum rf90_radio_path_e)eRFPath);
+				RF3_Final_Value = rtl8192_phy_QueryRFReg(dev, (enum rf90_radio_path_e)eRFPath, RegOffSetToBeCheck, bMask12Bits);
+				RT_TRACE(COMP_RF, "RF %d %d register final value: %x\n", eRFPath, RegOffSetToBeCheck, RF3_Final_Value);
+				RetryTimes--;
+			}
+			break;
+		case RF90_PATH_B:
+			while (RF3_Final_Value != RegValueToBeCheck && RetryTimes != 0) {
+				ret = rtl8192_phy_ConfigRFWithHeaderFile(dev, (enum rf90_radio_path_e)eRFPath);
+				RF3_Final_Value = rtl8192_phy_QueryRFReg(dev, (enum rf90_radio_path_e)eRFPath, RegOffSetToBeCheck, bMask12Bits);
+				RT_TRACE(COMP_RF, "RF %d %d register final value: %x\n", eRFPath, RegOffSetToBeCheck, RF3_Final_Value);
+				RetryTimes--;
+			}
+			break;
+		case RF90_PATH_C:
+			while (RF3_Final_Value != RegValueToBeCheck && RetryTimes != 0) {
+				ret = rtl8192_phy_ConfigRFWithHeaderFile(dev, (enum rf90_radio_path_e)eRFPath);
+				RF3_Final_Value = rtl8192_phy_QueryRFReg(dev, (enum rf90_radio_path_e)eRFPath, RegOffSetToBeCheck, bMask12Bits);
+				RT_TRACE(COMP_RF, "RF %d %d register final value: %x\n", eRFPath, RegOffSetToBeCheck, RF3_Final_Value);
+				RetryTimes--;
+			}
+			break;
+		case RF90_PATH_D:
+			while (RF3_Final_Value != RegValueToBeCheck && RetryTimes != 0) {
+				ret = rtl8192_phy_ConfigRFWithHeaderFile(dev, (enum rf90_radio_path_e)eRFPath);
+				RF3_Final_Value = rtl8192_phy_QueryRFReg(dev, (enum rf90_radio_path_e)eRFPath, RegOffSetToBeCheck, bMask12Bits);
+				RT_TRACE(COMP_RF, "RF %d %d register final value: %x\n", eRFPath, RegOffSetToBeCheck, RF3_Final_Value);
+				RetryTimes--;
+			}
+			break;
+		}
+
+		/*----Restore RFENV control type----*/
+		switch (eRFPath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			rtl8192_setBBreg(dev, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			rtl8192_setBBreg(dev, pPhyReg->rfintfs, bRFSI_RFENV << 16, u4RegValue);
+			break;
+		}
+
+		if (ret) {
+			RT_TRACE(COMP_ERR, "%s():Radio[%d] Fail!!", __func__, eRFPath);
+			goto phy_RF8256_Config_ParaFile_Fail;
+		}
+	}
+
+	RT_TRACE(COMP_PHY, "PHY Initialization Success\n");
+	return;
+
+phy_RF8256_Config_ParaFile_Fail:
+	RT_TRACE(COMP_ERR, "PHY Initialization failed\n");
+}
+
+void phy_set_rf8256_cck_tx_power(struct net_device *dev, u8 powerlevel)
+{
+	u32	TxAGC = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	TxAGC = powerlevel;
+
+	if (priv->bDynamicTxLowPower) {
+		if (priv->CustomerID == RT_CID_819x_Netcore)
+			TxAGC = 0x22;
+		else
+			TxAGC += priv->CckPwEnl;
+	}
+
+	if (TxAGC > 0x24)
+		TxAGC = 0x24;
+	rtl8192_setBBreg(dev, rTxAGC_CCK_Mcs32, bTxAGCRateCCK, TxAGC);
+}
+
+void phy_set_rf8256_ofdm_tx_power(struct net_device *dev, u8 powerlevel)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/* Joseph TxPower for 8192 testing */
+	u32 writeVal, powerBase0, powerBase1, writeVal_tmp;
+	u8 index = 0;
+	u16 RegOffset[6] = {0xe00, 0xe04, 0xe10, 0xe14, 0xe18, 0xe1c};
+	u8 byte0, byte1, byte2, byte3;
+
+	powerBase0 = powerlevel + priv->TxPowerDiff;	/* OFDM rates */
+	powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) | (powerBase0 << 8) | powerBase0;
+	powerBase1 = powerlevel;							/* MCS rates */
+	powerBase1 = (powerBase1 << 24) | (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
+
+	for (index = 0; index < 6; index++) {
+		writeVal = priv->MCSTxPowerLevelOriginalOffset[index] + ((index < 2) ? powerBase0 : powerBase1);
+		byte0 = (u8)(writeVal & 0x7f);
+		byte1 = (u8)((writeVal & 0x7f00) >> 8);
+		byte2 = (u8)((writeVal & 0x7f0000) >> 16);
+		byte3 = (u8)((writeVal & 0x7f000000) >> 24);
+
+		if (byte0 > 0x24)
+			/* Max power index = 0x24 */
+			byte0 = 0x24;
+		if (byte1 > 0x24)
+			byte1 = 0x24;
+		if (byte2 > 0x24)
+			byte2 = 0x24;
+		if (byte3 > 0x24)
+			byte3 = 0x24;
+
+		/* for tx power track */
+		if (index == 3) {
+			writeVal_tmp = (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;
+			priv->Pwr_Track = writeVal_tmp;
+		}
+
+		if (priv->bDynamicTxHighPower) {
+			/*Add by Jacken 2008/03/06
+			 *Emily, 20080613. Set low tx power for both MCS and legacy OFDM
+			 */
+			writeVal = 0x03030303;
+		} else {
+			writeVal = (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;
+		}
+		rtl8192_setBBreg(dev, RegOffset[index], 0x7f7f7f7f, writeVal);
+	}
+}
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.h b/drivers/staging/rtl8192u/r8190_rtl8256.h
new file mode 100644
index 000000000..9ea67f86f
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *	 This is part of the rtl8180-sa2400 driver
+ *	 released under the GPL (See file COPYING for details).
+ *	 Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
+ *
+ *
+ *	 This files contains programming code for the rtl8256
+ *	 radio frontend.
+ *
+ *	 *Many* thanks to Realtek Corp. for their great support!
+ */
+
+#ifndef RTL8225H
+#define RTL8225H
+
+#define RTL819X_TOTAL_RF_PATH 2 /* for 8192U */
+void phy_set_rf8256_bandwidth(struct net_device *dev,
+			      enum ht_channel_width bandwidth);
+void phy_rf8256_config(struct net_device *dev);
+void phy_set_rf8256_cck_tx_power(struct net_device *dev, u8 powerlevel);
+void phy_set_rf8256_ofdm_tx_power(struct net_device *dev, u8 powerlevel);
+
+#endif
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
new file mode 100644
index 000000000..ff0ada00b
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -0,0 +1,1129 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This is part of rtl8187 OpenSource driver.
+ * Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
+ * Released under the terms of GPL (General Public Licence)
+ *
+ * Parts of this driver are based on the GPL part of the
+ * official realtek driver
+ *
+ * Parts of this driver are based on the rtl8192 driver skeleton
+ * from Patric Schenke & Andres Salomon
+ *
+ * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+ *
+ * We want to thank the Authors of those projects and the Ndiswrapper
+ * project Authors.
+ */
+
+#ifndef R8192U_H
+#define R8192U_H
+
+#include <linux/compiler.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/usb.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+#include <linux/rtnetlink.h>
+#include <linux/wireless.h>
+#include <linux/timer.h>
+#include <linux/proc_fs.h>
+#include <linux/if_arp.h>
+#include <linux/random.h>
+#include <linux/io.h>
+#include "ieee80211/ieee80211.h"
+
+#define RTL8192U
+#define RTL819XU_MODULE_NAME "rtl819xU"
+/* HW security */
+#define MAX_KEY_LEN     61
+#define KEY_BUF_SIZE    5
+
+#define	RX_SMOOTH_FACTOR		20
+#define DMESG(x, a...)  no_printk(x, ##a)
+#define DMESGW(x, a...) no_printk(x, ##a)
+#define DMESGE(x, a...) no_printk(x, ##a)
+extern u32 rt_global_debug_component;
+#define RT_TRACE(component, x, args...) \
+	do {							\
+		if (rt_global_debug_component & (component))	\
+			pr_debug("RTL8192U: " x "\n", ##args);	\
+	} while (0)
+
+#define COMP_TRACE              BIT(0)  /* Function call tracing. */
+#define COMP_DBG                BIT(1)
+#define COMP_INIT               BIT(2)  /* Driver initialization/halt/reset. */
+
+#define COMP_RECV               BIT(3)  /* Receive data path. */
+#define COMP_SEND               BIT(4)  /* Send data path. */
+#define COMP_IO                 BIT(5)
+/* 802.11 Power Save mode or System/Device Power state. */
+#define COMP_POWER              BIT(6)
+/* 802.11 link related: join/start BSS, leave BSS. */
+#define COMP_EPROM              BIT(7)
+#define COMP_SWBW               BIT(8)  /* Bandwidth switch. */
+#define COMP_POWER_TRACKING     BIT(9)  /* 8190 TX Power Tracking */
+#define COMP_TURBO              BIT(10) /* Turbo Mode */
+#define COMP_QOS                BIT(11)
+#define COMP_RATE               BIT(12) /* Rate Adaptive mechanism */
+#define COMP_RM                 BIT(13) /* Radio Measurement */
+#define COMP_DIG                BIT(14)
+#define COMP_PHY                BIT(15)
+#define COMP_CH                 BIT(16) /* Channel setting debug */
+#define COMP_TXAGC              BIT(17) /* Tx power */
+#define COMP_HIPWR              BIT(18) /* High Power Mechanism */
+#define COMP_HALDM              BIT(19) /* HW Dynamic Mechanism */
+#define COMP_SEC                BIT(20) /* Event handling */
+#define COMP_LED                BIT(21)
+#define COMP_RF                 BIT(22)
+#define COMP_RXDESC             BIT(23) /* Rx desc information for SD3 debug */
+
+/* 11n or 8190 specific code */
+
+#define COMP_FIRMWARE           BIT(24) /* Firmware downloading */
+#define COMP_HT                 BIT(25) /* 802.11n HT related information */
+#define COMP_AMSDU              BIT(26) /* A-MSDU Debugging */
+#define COMP_SCAN               BIT(27)
+#define COMP_DOWN               BIT(29) /* rm driver module */
+#define COMP_RESET              BIT(30) /* Silent reset */
+#define COMP_ERR                BIT(31) /* Error out, always on */
+
+#define RTL819x_DEBUG
+#ifdef RTL819x_DEBUG
+#define RTL8192U_ASSERT(expr) \
+	do {								\
+		if (!(expr)) {						\
+			pr_debug("Assertion failed! %s, %s, %s, line = %d\n", \
+				 #expr, __FILE__, __func__, __LINE__);	\
+		}							\
+	} while (0)
+/*
+ * Debug out data buf.
+ * If you want to print DATA buffer related BA,
+ * please set ieee80211_debug_level to DATA|BA
+ */
+#define RT_DEBUG_DATA(level, data, datalen) \
+	do {								\
+		if ((rt_global_debug_component & (level)) == (level)) {	\
+			int i;						\
+			u8 *pdata = (u8 *)data;				\
+			pr_debug("RTL8192U: %s()\n", __func__);		\
+			for (i = 0; i < (int)(datalen); i++) {		\
+				printk("%2x ", pdata[i]);               \
+				if ((i+1)%16 == 0)			\
+					printk("\n");			\
+			}						\
+			printk("\n");					\
+		}							\
+	} while (0)
+#else
+#define RTL8192U_ASSERT(expr) do {} while (0)
+#define RT_DEBUG_DATA(level, data, datalen) do {} while (0)
+#endif /* RTL8169_DEBUG */
+
+/* Queue Select Value in TxDesc */
+#define QSLT_BK                                 0x1
+#define QSLT_BE                                 0x0
+#define QSLT_VI                                 0x4
+#define QSLT_VO                                 0x6
+#define QSLT_BEACON                             0x10
+#define QSLT_HIGH                               0x11
+#define QSLT_MGNT                               0x12
+#define QSLT_CMD                                0x13
+
+#define DESC90_RATE1M                           0x00
+#define DESC90_RATE2M                           0x01
+#define DESC90_RATE5_5M                         0x02
+#define DESC90_RATE11M                          0x03
+#define DESC90_RATE6M                           0x04
+#define DESC90_RATE9M                           0x05
+#define DESC90_RATE12M                          0x06
+#define DESC90_RATE18M                          0x07
+#define DESC90_RATE24M                          0x08
+#define DESC90_RATE36M                          0x09
+#define DESC90_RATE48M                          0x0a
+#define DESC90_RATE54M                          0x0b
+#define DESC90_RATEMCS0                         0x00
+#define DESC90_RATEMCS1                         0x01
+#define DESC90_RATEMCS2                         0x02
+#define DESC90_RATEMCS3                         0x03
+#define DESC90_RATEMCS4                         0x04
+#define DESC90_RATEMCS5                         0x05
+#define DESC90_RATEMCS6                         0x06
+#define DESC90_RATEMCS7                         0x07
+#define DESC90_RATEMCS8                         0x08
+#define DESC90_RATEMCS9                         0x09
+#define DESC90_RATEMCS10                        0x0a
+#define DESC90_RATEMCS11                        0x0b
+#define DESC90_RATEMCS12                        0x0c
+#define DESC90_RATEMCS13                        0x0d
+#define DESC90_RATEMCS14                        0x0e
+#define DESC90_RATEMCS15                        0x0f
+#define DESC90_RATEMCS32                        0x20
+
+#define RTL819X_DEFAULT_RF_TYPE RF_1T2R
+
+#define IEEE80211_WATCH_DOG_TIME    2000
+#define		PHY_Beacon_RSSI_SLID_WIN_MAX		10
+/* For Tx Power Tracking */
+#define		OFDM_Table_Length	19
+#define	CCK_Table_length	12
+
+/* For rtl819x */
+struct tx_desc_819x_usb {
+	/* DWORD 0 */
+	u16	PktSize;
+	u8	Offset;
+	u8	Reserved0:3;
+	u8	CmdInit:1;
+	u8	LastSeg:1;
+	u8	FirstSeg:1;
+	u8	LINIP:1;
+	u8	OWN:1;
+
+	/* DWORD 1 */
+	u8	TxFWInfoSize;
+	u8	RATid:3;
+	u8	DISFB:1;
+	u8	USERATE:1;
+	u8	MOREFRAG:1;
+	u8	NoEnc:1;
+	u8	PIFS:1;
+	u8	QueueSelect:5;
+	u8	NoACM:1;
+	u8	Reserved1:2;
+	u8	SecCAMID:5;
+	u8	SecDescAssign:1;
+	u8	SecType:2;
+
+	/* DWORD 2 */
+	u16	TxBufferSize;
+	u8	ResvForPaddingLen:7;
+	u8	Reserved3:1;
+	u8	Reserved4;
+
+	/* DWORD 3, 4, 5 */
+	u32	Reserved5;
+	u32	Reserved6;
+	u32	Reserved7;
+};
+
+struct tx_desc_cmd_819x_usb {
+	/* DWORD 0 */
+	u16	Reserved0;
+	u8	Reserved1;
+	u8	Reserved2:3;
+	u8	CmdInit:1;
+	u8	LastSeg:1;
+	u8	FirstSeg:1;
+	u8	LINIP:1;
+	u8	OWN:1;
+
+	/* DOWRD 1 */
+	u8	TxFWInfoSize;
+	u8	Reserved3;
+	u8	QueueSelect;
+	u8	Reserved4;
+
+	/* DOWRD 2 */
+	u16	TxBufferSize;
+	u16	Reserved5;
+
+	/* DWORD 3, 4, 5 */
+	u32	Reserved6;
+	u32	Reserved7;
+	u32	Reserved8;
+};
+
+struct tx_fwinfo_819x_usb {
+	/* DOWRD 0 */
+	u8	TxRate:7;
+	u8	CtsEnable:1;
+	u8	RtsRate:7;
+	u8	RtsEnable:1;
+	u8	TxHT:1;
+	u8	Short:1;        /* Error out, always on */
+	u8	TxBandwidth:1;	/* Used for HT MCS rate only */
+	u8	TxSubCarrier:2; /* Used for legacy OFDM rate only */
+	u8	STBC:2;
+	u8	AllowAggregation:1;
+	/* Interpret RtsRate field as high throughput data rate */
+	u8	RtsHT:1;
+	u8	RtsShort:1;     /* Short PLCP for CCK or short GI for 11n MCS */
+	u8	RtsBandwidth:1;	/* Used for HT MCS rate only */
+	u8	RtsSubcarrier:2;/* Used for legacy OFDM rate only */
+	u8	RtsSTBC:2;
+	/* Enable firmware to recalculate and assign packet duration */
+	u8	EnableCPUDur:1;
+
+	/* DWORD 1 */
+	u32	RxMF:2;
+	u32	RxAMD:3;
+	/* 1 indicate Tx info gathered by firmware and returned by Rx Cmd */
+	u32	TxPerPktInfoFeedback:1;
+	u32	Reserved1:2;
+	u32	TxAGCOffSet:4;
+	u32	TxAGCSign:1;
+	u32	Tx_INFO_RSVD:6;
+	u32	PacketID:13;
+};
+
+struct rtl8192_rx_info {
+	struct urb *urb;
+	struct net_device *dev;
+	u8 out_pipe;
+};
+
+struct rx_desc_819x_usb {
+	/* DOWRD 0 */
+	u16                 Length:14;
+	u16                 CRC32:1;
+	u16                 ICV:1;
+	u8                  RxDrvInfoSize;
+	u8                  Shift:2;
+	u8                  PHYStatus:1;
+	u8                  SWDec:1;
+	u8                  Reserved1:4;
+
+	/* DWORD 1 */
+	u32                 Reserved2;
+};
+
+struct rx_drvinfo_819x_usb {
+	/* DWORD 0 */
+	u16                 Reserved1:12;
+	u16                 PartAggr:1;
+	u16                 FirstAGGR:1;
+	u16                 Reserved2:2;
+
+	u8                  RxRate:7;
+	u8                  RxHT:1;
+
+	u8                  BW:1;
+	u8                  SPLCP:1;
+	u8                  Reserved3:2;
+	u8                  PAM:1;
+	u8                  Mcast:1;
+	u8                  Bcast:1;
+	u8                  Reserved4:1;
+
+	/* DWORD 1 */
+	u32                  TSFL;
+
+};
+
+/* Support till 64 bit bus width OS */
+#define MAX_DEV_ADDR_SIZE		8
+/* For RTL8190 */
+#define MAX_FIRMWARE_INFORMATION_SIZE   32
+#define MAX_802_11_HEADER_LENGTH        (40 + MAX_FIRMWARE_INFORMATION_SIZE)
+#define ENCRYPTION_MAX_OVERHEAD		128
+#define	USB_HWDESC_HEADER_LEN		sizeof(struct tx_desc_819x_usb)
+#define TX_PACKET_SHIFT_BYTES		(USB_HWDESC_HEADER_LEN + sizeof(struct tx_fwinfo_819x_usb))
+#define MAX_FRAGMENT_COUNT		8
+#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
+#define MAX_TRANSMIT_BUFFER_SIZE			32000
+#else
+#define MAX_TRANSMIT_BUFFER_SIZE			8000
+#endif
+/* Octets for crc32 (FCS, ICV) */
+#define scrclng					4
+
+enum rf_op_type {
+	RF_OP_By_SW_3wire = 0,
+	RF_OP_By_FW,
+	RF_OP_MAX
+};
+
+/* 8190 Loopback Mode definition */
+typedef enum _rtl819xUsb_loopback {
+	RTL819xU_NO_LOOPBACK = 0,
+	RTL819xU_MAC_LOOPBACK = 1,
+	RTL819xU_DMA_LOOPBACK = 2,
+	RTL819xU_CCK_LOOPBACK = 3,
+} rtl819xUsb_loopback_e;
+
+/* due to rtl8192 firmware */
+typedef enum _desc_packet_type_e {
+	DESC_PACKET_TYPE_INIT = 0,
+	DESC_PACKET_TYPE_NORMAL = 1,
+} desc_packet_type_e;
+
+typedef enum _firmware_status {
+	FW_STATUS_0_INIT = 0,
+	FW_STATUS_1_MOVE_BOOT_CODE = 1,
+	FW_STATUS_2_MOVE_MAIN_CODE = 2,
+	FW_STATUS_3_TURNON_CPU = 3,
+	FW_STATUS_4_MOVE_DATA_CODE = 4,
+	FW_STATUS_5_READY = 5,
+} firmware_status_e;
+
+typedef struct _fw_seg_container {
+	u16	seg_size;
+	u8	*seg_ptr;
+} fw_seg_container, *pfw_seg_container;
+typedef struct _rt_firmware {
+	firmware_status_e firmware_status;
+	u16               cmdpacket_frag_threshold;
+#define RTL8190_MAX_FIRMWARE_CODE_SIZE  64000
+	u8                firmware_buf[RTL8190_MAX_FIRMWARE_CODE_SIZE];
+	u16               firmware_buf_size;
+} rt_firmware, *prt_firmware;
+
+/* Add this to 9100 bytes to receive A-MSDU from RT-AP */
+#define MAX_RECEIVE_BUFFER_SIZE	9100
+
+typedef struct _rt_firmware_info_819xUsb {
+	u8		sz_info[16];
+} rt_firmware_info_819xUsb, *prt_firmware_info_819xUsb;
+
+/* Firmware Queue Layout */
+#define NUM_OF_FIRMWARE_QUEUE		10
+#define NUM_OF_PAGES_IN_FW		0x100
+
+#ifdef USE_ONE_PIPE
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE	0x000
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK	0x000
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI	0x0ff
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO	0x000
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA	0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD	0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT	0x00
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH	0
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN	0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB	0x00
+#else
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE	0x020
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK	0x020
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI	0x040
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO	0x040
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA	0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD	0x4
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT	0x20
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH	0
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN	0x4
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB	0x18
+
+#endif
+
+#define APPLIED_RESERVED_QUEUE_IN_FW	0x80000000
+#define RSVD_FW_QUEUE_PAGE_BK_SHIFT	0x00
+#define RSVD_FW_QUEUE_PAGE_BE_SHIFT	0x08
+#define RSVD_FW_QUEUE_PAGE_VI_SHIFT	0x10
+#define RSVD_FW_QUEUE_PAGE_VO_SHIFT	0x18
+#define RSVD_FW_QUEUE_PAGE_MGNT_SHIFT	0x10
+#define RSVD_FW_QUEUE_PAGE_CMD_SHIFT	0x08
+#define RSVD_FW_QUEUE_PAGE_BCN_SHIFT	0x00
+#define RSVD_FW_QUEUE_PAGE_PUB_SHIFT	0x08
+
+/*
+ * =================================================================
+ * =================================================================
+ */
+
+#define EPROM_93c46 0
+#define EPROM_93c56 1
+
+#define DEFAULT_FRAG_THRESHOLD 2342U
+#define MIN_FRAG_THRESHOLD     256U
+#define DEFAULT_BEACONINTERVAL 0x64U
+#define DEFAULT_BEACON_ESSID "Rtl819xU"
+
+#define DEFAULT_SSID ""
+#define DEFAULT_RETRY_RTS 7
+#define DEFAULT_RETRY_DATA 7
+#define PRISM_HDR_SIZE 64
+
+#define		PHY_RSSI_SLID_WIN_MAX				100
+
+typedef enum _WIRELESS_MODE {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_A = 0x01,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+	WIRELESS_MODE_N_24G = 0x10,
+	WIRELESS_MODE_N_5G = 0x20
+} WIRELESS_MODE;
+
+#define RTL_IOCTL_WPA_SUPPLICANT		(SIOCIWFIRSTPRIV + 30)
+
+typedef struct buffer {
+	struct buffer *next;
+	u32 *buf;
+
+} buffer;
+
+typedef struct rtl_reg_debug {
+	unsigned int  cmd;
+	struct {
+		unsigned char type;
+		unsigned char addr;
+		unsigned char page;
+		unsigned char length;
+	} head;
+	unsigned char buf[0xff];
+} rtl_reg_debug;
+
+typedef struct _rt_9x_tx_rate_history {
+	u32             cck[4];
+	u32             ofdm[8];
+	u32             ht_mcs[4][16];
+} rt_tx_rahis_t, *prt_tx_rahis_t;
+typedef struct _RT_SMOOTH_DATA_4RF {
+	s8    elements[4][100]; /* array to store values */
+	u32     index;            /* index to current array to store */
+	u32     TotalNum;         /* num of valid elements */
+	u32     TotalVal[4];      /* sum of valid elements */
+} RT_SMOOTH_DATA_4RF, *PRT_SMOOTH_DATA_4RF;
+
+/* This maybe changed for D-cut larger aggregation size */
+#define MAX_8192U_RX_SIZE			8192
+/* Stats seems messed up, clean it ASAP */
+typedef struct Stats {
+	unsigned long txrdu;
+	unsigned long rxok;
+	unsigned long rxframgment;
+	unsigned long rxurberr;
+	unsigned long rxstaterr;
+	/* 0: Total, 1: OK, 2: CRC, 3: ICV */
+	unsigned long received_rate_histogram[4][32];
+	/* 0: Long preamble/GI, 1: Short preamble/GI */
+	unsigned long received_preamble_GI[2][32];
+	/* level: (<4K), (4K~8K), (8K~16K), (16K~32K), (32K~64K) */
+	unsigned long rx_AMPDUsize_histogram[5];
+	/* level: (<5), (5~10), (10~20), (20~40), (>40) */
+	unsigned long rx_AMPDUnum_histogram[5];
+	unsigned long numpacket_matchbssid;
+	unsigned long numpacket_toself;
+	unsigned long num_process_phyinfo;
+	unsigned long numqry_phystatus;
+	unsigned long numqry_phystatusCCK;
+	unsigned long numqry_phystatusHT;
+	/* 0: 20M, 1: funn40M, 2: upper20M, 3: lower20M, 4: duplicate */
+	unsigned long received_bwtype[5];
+	unsigned long txnperr;
+	unsigned long txnpdrop;
+	unsigned long txresumed;
+	unsigned long txnpokint;
+	unsigned long txoverflow;
+	unsigned long txlpokint;
+	unsigned long txlpdrop;
+	unsigned long txlperr;
+	unsigned long txbeokint;
+	unsigned long txbedrop;
+	unsigned long txbeerr;
+	unsigned long txbkokint;
+	unsigned long txbkdrop;
+	unsigned long txbkerr;
+	unsigned long txviokint;
+	unsigned long txvidrop;
+	unsigned long txvierr;
+	unsigned long txvookint;
+	unsigned long txvodrop;
+	unsigned long txvoerr;
+	unsigned long txbeaconokint;
+	unsigned long txbeacondrop;
+	unsigned long txbeaconerr;
+	unsigned long txmanageokint;
+	unsigned long txmanagedrop;
+	unsigned long txmanageerr;
+	unsigned long txdatapkt;
+	unsigned long txfeedback;
+	unsigned long txfeedbackok;
+
+	unsigned long txoktotal;
+	unsigned long txokbytestotal;
+	unsigned long txokinperiod;
+	unsigned long txmulticast;
+	unsigned long txbytesmulticast;
+	unsigned long txbroadcast;
+	unsigned long txbytesbroadcast;
+	unsigned long txunicast;
+	unsigned long txbytesunicast;
+
+	unsigned long rxoktotal;
+	unsigned long rxbytesunicast;
+	unsigned long txfeedbackfail;
+	unsigned long txerrtotal;
+	unsigned long txerrbytestotal;
+	unsigned long txerrmulticast;
+	unsigned long txerrbroadcast;
+	unsigned long txerrunicast;
+	unsigned long txretrycount;
+	unsigned long txfeedbackretry;
+	u8	      last_packet_rate;
+	unsigned long slide_signal_strength[100];
+	unsigned long slide_evm[100];
+	/* For recording sliding window's RSSI value */
+	unsigned long slide_rssi_total;
+	/* For recording sliding window's EVM value */
+	unsigned long slide_evm_total;
+	/* Transformed in dbm. Beautified signal strength for UI, not correct */
+	long signal_strength;
+	long signal_quality;
+	long last_signal_strength_inpercent;
+	/* Correct smoothed ss in dbm, only used in driver
+	 * to report real power now
+	 */
+	long recv_signal_power;
+	u8 rx_rssi_percentage[4];
+	u8 rx_evm_percentage[2];
+	long rxSNRdB[4];
+	rt_tx_rahis_t txrate;
+	/* For beacon RSSI */
+	u32 Slide_Beacon_pwdb[100];
+	u32 Slide_Beacon_Total;
+	RT_SMOOTH_DATA_4RF              cck_adc_pwdb;
+
+	u32	CurrentShowTxate;
+} Stats;
+
+/* Bandwidth Offset */
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
+#define HAL_PRIME_CHNL_OFFSET_LOWER			1
+#define HAL_PRIME_CHNL_OFFSET_UPPER			2
+
+typedef struct	ChnlAccessSetting {
+	u16 SIFS_Timer;
+	u16 DIFS_Timer;
+	u16 SlotTimeTimer;
+	u16 EIFS_Timer;
+	u16 CWminIndex;
+	u16 CWmaxIndex;
+} *PCHANNEL_ACCESS_SETTING, CHANNEL_ACCESS_SETTING;
+
+typedef struct _BB_REGISTER_DEFINITION {
+	/* set software control:        0x870~0x877 [8 bytes]  */
+	u32 rfintfs;
+	/* readback data:               0x8e0~0x8e7 [8 bytes]  */
+	u32 rfintfi;
+	/* output data:                 0x860~0x86f [16 bytes] */
+	u32 rfintfo;
+	/* output enable:               0x860~0x86f [16 bytes] */
+	u32 rfintfe;
+	/* LSSI data:                   0x840~0x84f [16 bytes] */
+	u32 rf3wireOffset;
+	/* BB Band Select:              0x878~0x87f [8 bytes]  */
+	u32 rfLSSI_Select;
+	/* Tx gain stage:               0x80c~0x80f [4 bytes]  */
+	u32 rfTxGainStage;
+	/* wire parameter control1:     0x820~0x823, 0x828~0x82b,
+	 *                              0x830~0x833, 0x838~0x83b [16 bytes]
+	 */
+	u32 rfHSSIPara1;
+	/* wire parameter control2:     0x824~0x827, 0x82c~0x82f,
+	 *                              0x834~0x837, 0x83c~0x83f [16 bytes]
+	 */
+	u32 rfHSSIPara2;
+	/* Tx Rx antenna control:       0x858~0x85f [16 bytes] */
+	u32 rfSwitchControl;
+	/* AGC parameter control1:	0xc50~0xc53, 0xc58~0xc5b,
+	 *                              0xc60~0xc63, 0xc68~0xc6b [16 bytes]
+	 */
+	u32 rfAGCControl1;
+	/* AGC parameter control2:      0xc54~0xc57, 0xc5c~0xc5f,
+	 *                              0xc64~0xc67, 0xc6c~0xc6f [16 bytes]
+	 */
+	u32 rfAGCControl2;
+	/* OFDM Rx IQ imbalance matrix:	0xc14~0xc17, 0xc1c~0xc1f,
+	 *                              0xc24~0xc27, 0xc2c~0xc2f [16 bytes]
+	 */
+	u32 rfRxIQImbalance;
+	/* Rx IQ DC offset and Rx digital filter, Rx DC notch filter:
+	 *                              0xc10~0xc13, 0xc18~0xc1b,
+	 *                              0xc20~0xc23, 0xc28~0xc2b [16 bytes]
+	 */
+	u32 rfRxAFE;
+	/* OFDM Tx IQ imbalance matrix:	0xc80~0xc83, 0xc88~0xc8b,
+	 *                              0xc90~0xc93, 0xc98~0xc9b [16 bytes]
+	 */
+	u32 rfTxIQImbalance;
+	/* Tx IQ DC Offset and Tx DFIR type:
+	 *                              0xc84~0xc87, 0xc8c~0xc8f,
+	 *                              0xc94~0xc97, 0xc9c~0xc9f [16 bytes]
+	 */
+	u32 rfTxAFE;
+	/* LSSI RF readback data:       0x8a0~0x8af [16 bytes] */
+	u32 rfLSSIReadBack;
+} BB_REGISTER_DEFINITION_T, *PBB_REGISTER_DEFINITION_T;
+
+typedef enum _RT_RF_TYPE_819xU {
+	RF_TYPE_MIN = 0,
+	RF_8225,
+	RF_8256,
+	RF_8258,
+	RF_PSEUDO_11N = 4,
+} RT_RF_TYPE_819xU, *PRT_RF_TYPE_819xU;
+
+/* 2007/10/08 MH Define RATR state. */
+enum dynamic_ratr_state {
+	DM_RATR_STA_HIGH = 0,
+	DM_RATR_STA_MIDDLE = 1,
+	DM_RATR_STA_LOW = 2,
+	DM_RATR_STA_MAX
+};
+
+typedef struct _rate_adaptive {
+	u8				rate_adaptive_disabled;
+	enum dynamic_ratr_state		ratr_state;
+	u16				reserve;
+
+	u32				high_rssi_thresh_for_ra;
+	u32				high2low_rssi_thresh_for_ra;
+	u8				low2high_rssi_thresh_for_ra40M;
+	u32				low_rssi_thresh_for_ra40M;
+	u8				low2high_rssi_thresh_for_ra20M;
+	u32				low_rssi_thresh_for_ra20M;
+	u32				upper_rssi_threshold_ratr;
+	u32				middle_rssi_threshold_ratr;
+	u32				low_rssi_threshold_ratr;
+	u32				low_rssi_threshold_ratr_40M;
+	u32				low_rssi_threshold_ratr_20M;
+	u8				ping_rssi_enable;
+	u32				ping_rssi_ratr;
+	u32				ping_rssi_thresh_for_ra;
+	u32				last_ratr;
+
+} rate_adaptive, *prate_adaptive;
+
+#define TxBBGainTableLength 37
+#define	CCKTxBBGainTableLength 23
+
+typedef struct _txbbgain_struct {
+	long	txbb_iq_amplifygain;
+	u32	txbbgain_value;
+} txbbgain_struct, *ptxbbgain_struct;
+
+typedef struct _ccktxbbgain_struct {
+	/* The value is from a22 to a29, one byte one time is much safer */
+	u8	ccktxbb_valuearray[8];
+} ccktxbbgain_struct, *pccktxbbgain_struct;
+
+typedef struct _init_gain {
+	u8				xaagccore1;
+	u8				xbagccore1;
+	u8				xcagccore1;
+	u8				xdagccore1;
+	u8				cca;
+
+} init_gain, *pinit_gain;
+
+typedef struct _phy_ofdm_rx_status_report_819xusb {
+	u8	trsw_gain_X[4];
+	u8	pwdb_all;
+	u8	cfosho_X[4];
+	u8	cfotail_X[4];
+	u8	rxevm_X[2];
+	u8	rxsnr_X[4];
+	u8	pdsnr_X[2];
+	u8	csi_current_X[2];
+	u8	csi_target_X[2];
+	u8	sigevm;
+	u8	max_ex_pwr;
+	u8	sgi_en;
+	u8  rxsc_sgien_exflg;
+} phy_sts_ofdm_819xusb_t;
+
+typedef struct _phy_cck_rx_status_report_819xusb {
+	/* For CCK rate descriptor. This is an unsigned 8:1 variable.
+	 * LSB bit presend 0.5. And MSB 7 bts presend a signed value.
+	 * Range from -64~+63.5.
+	 */
+	u8	adc_pwdb_X[4];
+	u8	sq_rpt;
+	u8	cck_agc_rpt;
+} phy_sts_cck_819xusb_t;
+
+struct phy_ofdm_rx_status_rxsc_sgien_exintfflag {
+	u8			reserved:4;
+	u8			rxsc:2;
+	u8			sgi_en:1;
+	u8			ex_intf_flag:1;
+};
+
+typedef enum _RT_CUSTOMER_ID {
+	RT_CID_DEFAULT = 0,
+	RT_CID_8187_ALPHA0 = 1,
+	RT_CID_8187_SERCOMM_PS = 2,
+	RT_CID_8187_HW_LED = 3,
+	RT_CID_8187_NETGEAR = 4,
+	RT_CID_WHQL = 5,
+	RT_CID_819x_CAMEO  = 6,
+	RT_CID_819x_RUNTOP = 7,
+	RT_CID_819x_Senao = 8,
+	RT_CID_TOSHIBA = 9,
+	RT_CID_819x_Netcore = 10,
+	RT_CID_Nettronix = 11,
+	RT_CID_DLINK = 12,
+	RT_CID_PRONET = 13,
+} RT_CUSTOMER_ID, *PRT_CUSTOMER_ID;
+
+/*
+ * ==========================================================================
+ * LED customization.
+ * ==========================================================================
+ */
+
+typedef	enum _LED_STRATEGY_8190 {
+	SW_LED_MODE0, /* SW control 1 LED via GPIO0. It is default option. */
+	SW_LED_MODE1, /* SW control for PCI Express */
+	SW_LED_MODE2, /* SW control for Cameo. */
+	SW_LED_MODE3, /* SW control for RunTop. */
+	SW_LED_MODE4, /* SW control for Netcore. */
+	/* HW control 2 LEDs, LED0 and LED1 (4 different control modes) */
+	HW_LED,
+} LED_STRATEGY_8190, *PLED_STRATEGY_8190;
+
+typedef enum _RESET_TYPE {
+	RESET_TYPE_NORESET = 0x00,
+	RESET_TYPE_NORMAL = 0x01,
+	RESET_TYPE_SILENT = 0x02
+} RESET_TYPE;
+
+/* The simple tx command OP code. */
+typedef enum _tag_TxCmd_Config_Index {
+	TXCMD_TXRA_HISTORY_CTRL				= 0xFF900000,
+	TXCMD_RESET_TX_PKT_BUFF				= 0xFF900001,
+	TXCMD_RESET_RX_PKT_BUFF				= 0xFF900002,
+	TXCMD_SET_TX_DURATION				= 0xFF900003,
+	TXCMD_SET_RX_RSSI						= 0xFF900004,
+	TXCMD_SET_TX_PWR_TRACKING			= 0xFF900005,
+	TXCMD_XXXX_CTRL,
+} DCMD_TXCMD_OP;
+
+enum version_819xu {
+	VERSION_819XU_A, // A-cut
+	VERSION_819XU_B, // B-cut
+	VERSION_819XU_C,// C-cut
+};
+
+//added for different RF type
+enum rt_rf_type {
+	RF_1T2R = 0,
+	RF_2T4R,
+};
+
+typedef struct r8192_priv {
+	struct usb_device *udev;
+	/* For maintain info from eeprom */
+	short epromtype;
+	u16 eeprom_vid;
+	u16 eeprom_pid;
+	u8  eeprom_CustomerID;
+	u8  eeprom_ChannelPlan;
+	RT_CUSTOMER_ID CustomerID;
+	LED_STRATEGY_8190	LedStrategy;
+	u8  txqueue_to_outpipemap[9];
+	int irq;
+	struct ieee80211_device *ieee80211;
+
+	/* O: rtl8192, 1: rtl8185 V B/C, 2: rtl8185 V D */
+	short card_8192;
+	/* If TCR reports card V B/C, this discriminates */
+	enum version_819xu card_8192_version;
+	short enable_gpio0;
+	enum card_type {
+		PCI, MINIPCI, CARDBUS, USB
+	} card_type;
+	short hw_plcp_len;
+	short plcp_preamble_mode;
+
+	spinlock_t irq_lock;
+	spinlock_t tx_lock;
+	struct mutex mutex;
+
+	u16 irq_mask;
+	short chan;
+	short sens;
+	short max_sens;
+
+	short up;
+	/* If 1, allow bad crc frame, reception in monitor mode */
+	short crcmon;
+
+	struct mutex wx_mutex;
+
+	enum rt_rf_type   rf_type;	    /* 0: 1T2R, 1: 2T4R */
+	RT_RF_TYPE_819xU rf_chip;
+
+	short (*rf_set_sens)(struct net_device *dev, short sens);
+	u8 (*rf_set_chan)(struct net_device *dev, u8 ch);
+	void (*rf_close)(struct net_device *dev);
+	void (*rf_init)(struct net_device *dev);
+	short promisc;
+	/* Stats */
+	struct Stats stats;
+	struct iw_statistics wstats;
+
+	/* RX stuff */
+	struct urb **rx_urb;
+	struct urb **rx_cmd_urb;
+#ifdef THOMAS_BEACON
+	u32 *oldaddr;
+#endif
+#ifdef THOMAS_TASKLET
+	atomic_t irt_counter; /* count for irq_rx_tasklet */
+#endif
+#ifdef JACKSON_NEW_RX
+	struct sk_buff **pp_rxskb;
+	int     rx_inx;
+#endif
+
+	struct sk_buff_head rx_queue;
+	struct sk_buff_head skb_queue;
+	struct work_struct qos_activate;
+	short  tx_urb_index;
+	atomic_t tx_pending[0x10]; /* UART_PRIORITY + 1 */
+
+	struct tasklet_struct irq_rx_tasklet;
+	struct urb *rxurb_task;
+
+	/* Tx Related variables */
+	u16	ShortRetryLimit;
+	u16	LongRetryLimit;
+	u32	TransmitConfig;
+	u8	RegCWinMin;	/* For turbo mode CW adaptive */
+
+	u32     LastRxDescTSFHigh;
+	u32     LastRxDescTSFLow;
+
+	/* Rx Related variables */
+	u16	EarlyRxThreshold;
+	u32	ReceiveConfig;
+	u8	AcmControl;
+
+	u8	RFProgType;
+
+	u8 retry_data;
+	u8 retry_rts;
+	u16 rts;
+
+	struct	ChnlAccessSetting  ChannelAccessSetting;
+	struct work_struct reset_wq;
+
+/**********************************************************/
+	/* For rtl819xUsb */
+	u16     basic_rate;
+	u8      short_preamble;
+	u8      slot_time;
+	bool	bDcut;
+	bool bCurrentRxAggrEnable;
+	enum rf_op_type Rf_Mode;	/* For Firmware RF -R/W switch */
+	prt_firmware		pFirmware;
+	rtl819xUsb_loopback_e	LoopbackMode;
+	u16 EEPROMTxPowerDiff;
+	u8 EEPROMThermalMeter;
+	u8 EEPROMPwDiff;
+	u8 EEPROMCrystalCap;
+	u8 EEPROM_Def_Ver;
+	u8 EEPROMTxPowerLevelCCK;		/* CCK channel 1~14 */
+	u8 EEPROMTxPowerLevelCCK_V1[3];
+	u8 EEPROMTxPowerLevelOFDM24G[3];	/* OFDM 2.4G channel 1~14 */
+	u8 EEPROMTxPowerLevelOFDM5G[24];	/* OFDM 5G */
+
+	/* PHY related */
+	BB_REGISTER_DEFINITION_T PHYRegDef[4];	/* Radio A/B/C/D */
+	/* Read/write are allow for following hardware information variables */
+	u32	MCSTxPowerLevelOriginalOffset[6];
+	u32	CCKTxPowerLevelOriginalOffset;
+	u8	TxPowerLevelCCK[14];		/* CCK channel 1~14 */
+	u8	TxPowerLevelOFDM24G[14];	/* OFDM 2.4G channel 1~14 */
+	u8	TxPowerLevelOFDM5G[14];		/* OFDM 5G */
+	u32	Pwr_Track;
+	u8	TxPowerDiff;
+	u8	AntennaTxPwDiff[2]; /* Antenna gain offset, 0: B, 1: C, 2: D */
+	u8	CrystalCap;
+	u8	ThermalMeter[2];    /* index 0: RFIC0, index 1: RFIC1 */
+
+	u8	CckPwEnl;
+	/* Use to calculate PWBD */
+	u8	bCckHighPower;
+	long	undecorated_smoothed_pwdb;
+
+	/* For set channel */
+	u8	SwChnlInProgress;
+	u8	SwChnlStage;
+	u8	SwChnlStep;
+	u8	SetBWModeInProgress;
+	enum ht_channel_width 	CurrentChannelBW;
+	u8      ChannelPlan;
+	/* 8190 40MHz mode */
+	/* Control channel sub-carrier */
+	u8	nCur40MhzPrimeSC;
+	/* Test for shorten RF configuration time.
+	 * We save RF reg0 in this variable to reduce RF reading.
+	 */
+	u32					RfReg0Value[4];
+	u8					NumTotalRFPath;
+	bool				brfpath_rxenable[4];
+	/* RF set related */
+	bool				SetRFPowerStateInProgress;
+	struct timer_list watch_dog_timer;
+
+	/* For dynamic mechanism */
+	/* Tx Power Control for Near/Far Range */
+	bool	bdynamic_txpower;
+	bool	bDynamicTxHighPower;
+	bool	bDynamicTxLowPower;
+	bool	bLastDTPFlag_High;
+	bool	bLastDTPFlag_Low;
+
+	bool	bstore_last_dtpflag;
+	/* Define to discriminate on High power State or
+	 * on sitesurvey to change Tx gain index
+	 */
+	bool	bstart_txctrl_bydtp;
+	rate_adaptive rate_adaptive;
+	/* TX power tracking
+	 * OPEN/CLOSE TX POWER TRACKING
+	 */
+	txbbgain_struct txbbgain_table[TxBBGainTableLength];
+	u8		txpower_count; /* For 6 sec do tracking again */
+	bool		btxpower_trackingInit;
+	u8		OFDM_index;
+	u8		CCK_index;
+	/* CCK TX Power Tracking */
+	ccktxbbgain_struct	cck_txbbgain_table[CCKTxBBGainTableLength];
+	ccktxbbgain_struct	cck_txbbgain_ch14_table[CCKTxBBGainTableLength];
+	u8 rfa_txpowertrackingindex;
+	u8 rfa_txpowertrackingindex_real;
+	u8 rfa_txpowertracking_default;
+	u8 rfc_txpowertrackingindex;
+	u8 rfc_txpowertrackingindex_real;
+
+	s8 cck_present_attenuation;
+	u8 cck_present_attenuation_20Mdefault;
+	u8 cck_present_attenuation_40Mdefault;
+	s8 cck_present_attenuation_difference;
+	bool btxpower_tracking;
+	bool bcck_in_ch14;
+	bool btxpowerdata_readfromEEPORM;
+	u16	TSSI_13dBm;
+	init_gain initgain_backup;
+	u8 DefaultInitialGain[4];
+	/* For EDCA Turbo mode */
+	bool		bis_any_nonbepkts;
+	bool		bcurrent_turbo_EDCA;
+	bool		bis_cur_rdlstate;
+	struct delayed_work fsync_work;
+	bool bfsync_processing;	/* 500ms Fsync timer is active or not */
+	u32	rate_record;
+	u32	rateCountDiffRecord;
+	u32	ContinueDiffCount;
+	bool bswitch_fsync;
+
+	u8	framesync;
+	u32	framesyncC34;
+	u8	framesyncMonitor;
+	u16	nrxAMPDU_size;
+	u8	nrxAMPDU_aggr_num;
+
+	/* For gpio */
+	bool bHwRadioOff;
+
+	u32 reset_count;
+	bool bpbc_pressed;
+	u32 txpower_checkcnt;
+	u32 txpower_tracking_callback_cnt;
+	u8 thermal_read_val[40];
+	u8 thermal_readback_index;
+	u32 ccktxpower_adjustcnt_not_ch14;
+	u32 ccktxpower_adjustcnt_ch14;
+	u8 tx_fwinfo_force_subcarriermode;
+	u8 tx_fwinfo_force_subcarrierval;
+	/* For silent reset */
+	RESET_TYPE	ResetProgress;
+	bool		bForcedSilentReset;
+	bool		bDisableNormalResetCheck;
+	u16		TxCounter;
+	u16		RxCounter;
+	int		IrpPendingCount;
+	bool		bResetInProgress;
+	bool		force_reset;
+	u8		InitialGainOperateType;
+
+	u16		SifsTime;
+
+	/* Define work item */
+
+	struct delayed_work update_beacon_wq;
+	struct delayed_work watch_dog_wq;
+	struct delayed_work txpower_tracking_wq;
+	struct delayed_work rfpath_check_wq;
+	struct delayed_work gpio_change_rf_wq;
+	struct delayed_work initialgain_operate_wq;
+	struct workqueue_struct *priv_wq;
+
+	/* debugfs */
+	struct dentry *debugfs_dir;
+} r8192_priv;
+
+/* For rtl8187B */
+typedef enum{
+	BULK_PRIORITY = 0x01,
+	LOW_PRIORITY,
+	NORM_PRIORITY,
+	VO_PRIORITY,
+	VI_PRIORITY,
+	BE_PRIORITY,
+	BK_PRIORITY,
+	RSVD2,
+	RSVD3,
+	BEACON_PRIORITY,
+	HIGH_PRIORITY,
+	MANAGE_PRIORITY,
+	RSVD4,
+	RSVD5,
+	UART_PRIORITY
+} priority_t;
+
+typedef enum {
+	NIC_8192U = 1,
+	NIC_8190P = 2,
+	NIC_8192E = 3,
+} nic_t;
+
+bool init_firmware(struct net_device *dev);
+short rtl819xU_tx_cmd(struct net_device *dev, struct sk_buff *skb);
+short rtl8192_tx(struct net_device *dev, struct sk_buff *skb);
+
+int read_nic_byte(struct net_device *dev, int x, u8 *data);
+int read_nic_byte_E(struct net_device *dev, int x, u8 *data);
+int read_nic_dword(struct net_device *dev, int x, u32 *data);
+int read_nic_word(struct net_device *dev, int x, u16 *data);
+int write_nic_byte(struct net_device *dev, int x, u8 y);
+int write_nic_byte_E(struct net_device *dev, int x, u8 y);
+int write_nic_word(struct net_device *dev, int x, u16 y);
+int write_nic_dword(struct net_device *dev, int x, u32 y);
+void force_pci_posting(struct net_device *dev);
+
+void rtl8192_rtx_disable(struct net_device *dev);
+void rtl8192_rx_enable(struct net_device *dev);
+
+void rtl8192_update_msr(struct net_device *dev);
+int rtl8192_down(struct net_device *dev);
+int rtl8192_up(struct net_device *dev);
+void rtl8192_commit(struct net_device *dev);
+void rtl8192_set_chan(struct net_device *dev, short ch);
+void rtl8192_set_rxconf(struct net_device *dev);
+void rtl819xusb_beacon_tx(struct net_device *dev, u16 tx_rate);
+
+void EnableHWSecurityConfig8192(struct net_device *dev);
+void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
+	    const u8 *MacAddr, u8 DefaultKey, u32 *KeyContent);
+
+void rtl8192_debugfs_init_one(struct net_device *dev);
+void rtl8192_debugfs_exit_one(struct net_device *dev);
+void rtl8192_debugfs_rename_one(struct net_device *dev);
+void rtl8192_debugfs_init(void);
+void rtl8192_debugfs_exit(void);
+
+#endif
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
new file mode 100644
index 000000000..0a60ef201
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -0,0 +1,4800 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192U
+ *
+ * Based on the r8187 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andrea.merello@gmail.com>, et al.
+ *
+ * Contact Information:
+ * Jerry chuang <wlanfae@realtek.com>
+ */
+
+#ifndef CONFIG_FORCE_HARD_FLOAT
+double __floatsidf(int i)
+{
+	return i;
+}
+
+unsigned int __fixunsdfsi(double d)
+{
+	return d;
+}
+
+double __adddf3(double a, double b)
+{
+	return a + b;
+}
+
+double __addsf3(float a, float b)
+{
+	return a + b;
+}
+
+double __subdf3(double a, double b)
+{
+	return a - b;
+}
+
+double __extendsfdf2(float a)
+{
+	return a;
+}
+#endif
+
+#define CONFIG_RTL8192_IO_MAP
+
+#include <linux/uaccess.h>
+#include "r8192U_hw.h"
+#include "r8192U.h"
+#include "r8190_rtl8256.h" /* RTL8225 Radio frontend */
+#include "r8180_93cx6.h"   /* Card EEPROM */
+#include "r8192U_wx.h"
+#include "r819xU_phy.h"
+#include "r819xU_phyreg.h"
+#include "r819xU_cmdpkt.h"
+#include "r8192U_dm.h"
+#include <linux/usb.h>
+#include <linux/slab.h>
+#include <linux/seq_file.h>
+/* FIXME: check if 2.6.7 is ok */
+
+#include "ieee80211/dot11d.h"
+/* set here to open your trace code. */
+u32 rt_global_debug_component = COMP_DOWN	|
+				COMP_SEC	|
+				COMP_ERR; /* always open err flags on */
+
+#define TOTAL_CAM_ENTRY 32
+#define CAM_CONTENT_COUNT 8
+
+static const struct usb_device_id rtl8192_usb_id_tbl[] = {
+	/* Realtek */
+	{USB_DEVICE(0x0bda, 0x8709)},
+	/* Corega */
+	{USB_DEVICE(0x07aa, 0x0043)},
+	/* Belkin */
+	{USB_DEVICE(0x050d, 0x805E)},
+	/* Sitecom */
+	{USB_DEVICE(0x0df6, 0x0031)},
+	/* EnGenius */
+	{USB_DEVICE(0x1740, 0x9201)},
+	/* Dlink */
+	{USB_DEVICE(0x2001, 0x3301)},
+	/* Zinwell */
+	{USB_DEVICE(0x5a57, 0x0290)},
+	/* LG */
+	{USB_DEVICE(0x043e, 0x7a01)},
+	{}
+};
+
+MODULE_LICENSE("GPL");
+MODULE_VERSION("V 1.1");
+MODULE_DEVICE_TABLE(usb, rtl8192_usb_id_tbl);
+MODULE_DESCRIPTION("Linux driver for Realtek RTL8192 USB WiFi cards");
+
+static char *ifname = "wlan%d";
+static int hwwep = 1;  /* default use hw. set 0 to use software security */
+
+module_param(ifname, charp, 0644);
+module_param(hwwep, int, 0644);
+
+MODULE_PARM_DESC(ifname, " Net interface name, wlan%d=default");
+MODULE_PARM_DESC(hwwep, " Try to use hardware security support. ");
+
+static int rtl8192_usb_probe(struct usb_interface *intf,
+			     const struct usb_device_id *id);
+static void rtl8192_usb_disconnect(struct usb_interface *intf);
+
+static struct usb_driver rtl8192_usb_driver = {
+	.name		= RTL819XU_MODULE_NAME,		  /* Driver name   */
+	.id_table	= rtl8192_usb_id_tbl,		  /* PCI_ID table  */
+	.probe		= rtl8192_usb_probe,		  /* probe fn      */
+	.disconnect	= rtl8192_usb_disconnect,	  /* remove fn     */
+	.suspend	= NULL,				  /* PM suspend fn */
+	.resume		= NULL,				  /* PM resume fn  */
+};
+
+struct CHANNEL_LIST {
+	u8	Channel[32];
+	u8	Len;
+};
+
+static struct CHANNEL_LIST ChannelPlan[] = {
+	/* FCC */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 24},
+	/* IC */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},
+	/* ETSI */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},
+	/* Spain. Change to ETSI. */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* France. Change to ETSI. */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* MKK */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},
+	/* MKK1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},
+	/* Israel. */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* For 11a , TELEC */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},
+	/* MIC */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},
+	/* For Global Domain. 1-11:active scan, 12-14 passive scan. */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14}
+};
+
+static void rtl819x_set_channel_map(u8 channel_plan, struct r8192_priv *priv)
+{
+	int i, max_chan = -1, min_chan = -1;
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	switch (channel_plan) {
+	case COUNTRY_CODE_FCC:
+	case COUNTRY_CODE_IC:
+	case COUNTRY_CODE_ETSI:
+	case COUNTRY_CODE_SPAIN:
+	case COUNTRY_CODE_FRANCE:
+	case COUNTRY_CODE_MKK:
+	case COUNTRY_CODE_MKK1:
+	case COUNTRY_CODE_ISRAEL:
+	case COUNTRY_CODE_TELEC:
+	case COUNTRY_CODE_MIC:
+		rtl8192u_dot11d_init(ieee);
+		ieee->bGlobalDomain = false;
+		/* actually 8225 & 8256 rf chips only support B,G,24N mode */
+		if ((priv->rf_chip == RF_8225) || (priv->rf_chip == RF_8256)) {
+			min_chan = 1;
+			max_chan = 14;
+		} else {
+			RT_TRACE(COMP_ERR,
+				 "unknown rf chip, can't set channel map in function:%s()\n",
+				 __func__);
+		}
+		if (ChannelPlan[channel_plan].Len != 0) {
+			/* Clear old channel map */
+			memset(GET_DOT11D_INFO(ieee)->channel_map, 0,
+			       sizeof(GET_DOT11D_INFO(ieee)->channel_map));
+			/* Set new channel map */
+			for (i = 0; i < ChannelPlan[channel_plan].Len; i++) {
+				if (ChannelPlan[channel_plan].Channel[i] < min_chan || ChannelPlan[channel_plan].Channel[i] > max_chan)
+					break;
+				GET_DOT11D_INFO(ieee)->channel_map[ChannelPlan[channel_plan].Channel[i]] = 1;
+			}
+		}
+		break;
+
+	case COUNTRY_CODE_GLOBAL_DOMAIN:
+		/* this flag enabled to follow 11d country IE setting,
+		 * otherwise, it shall follow global domain settings.
+		 */
+		GET_DOT11D_INFO(ieee)->dot11d_enabled = 0;
+		dot11d_reset(ieee);
+		ieee->bGlobalDomain = true;
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void CamResetAllEntry(struct net_device *dev)
+{
+	u32 ulcommand = 0;
+	/* In static WEP, OID_ADD_KEY or OID_ADD_WEP are set before STA
+	 * associate to AP. However, ResetKey is called on
+	 * OID_802_11_INFRASTRUCTURE_MODE and MlmeAssociateRequest. In this
+	 * condition, Cam can not be reset because upper layer will not set
+	 * this static key again.
+	 */
+	ulcommand |= BIT(31) | BIT(30);
+	write_nic_dword(dev, RWCAM, ulcommand);
+}
+
+int write_nic_byte_E(struct net_device *dev, int indx, u8 data)
+{
+	int status;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u8 *usbdata = kzalloc(sizeof(data), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+	*usbdata = data;
+
+	status = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 RTL8187_REQ_SET_REGS, RTL8187_REQT_WRITE,
+				 indx | 0xfe00, 0, usbdata, 1, 500);
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s TimeOut! status: %d\n", __func__, status);
+		return status;
+	}
+	return 0;
+}
+
+int read_nic_byte_E(struct net_device *dev, int indx, u8 *data)
+{
+	int status;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u8 *usbdata = kzalloc(sizeof(u8), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+				 RTL8187_REQ_GET_REGS, RTL8187_REQT_READ,
+				 indx | 0xfe00, 0, usbdata, 1, 500);
+	*data = *usbdata;
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s failure status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+/* as 92U has extend page from 4 to 16, so modify functions below. */
+int write_nic_byte(struct net_device *dev, int indx, u8 data)
+{
+	int status;
+
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u8 *usbdata = kzalloc(sizeof(data), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+	*usbdata = data;
+
+	status = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 RTL8187_REQ_SET_REGS, RTL8187_REQT_WRITE,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 1, 500);
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s TimeOut! status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+int write_nic_word(struct net_device *dev, int indx, u16 data)
+{
+	int status;
+
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u16 *usbdata = kzalloc(sizeof(data), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+	*usbdata = data;
+
+	status = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 RTL8187_REQ_SET_REGS, RTL8187_REQT_WRITE,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 2, 500);
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s TimeOut! status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+int write_nic_dword(struct net_device *dev, int indx, u32 data)
+{
+	int status;
+
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u32 *usbdata = kzalloc(sizeof(data), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+	*usbdata = data;
+
+	status = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 RTL8187_REQ_SET_REGS, RTL8187_REQT_WRITE,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 4, 500);
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s TimeOut! status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+int read_nic_byte(struct net_device *dev, int indx, u8 *data)
+{
+	int status;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u8 *usbdata = kzalloc(sizeof(u8), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+				 RTL8187_REQ_GET_REGS, RTL8187_REQT_READ,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 1, 500);
+	*data = *usbdata;
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s failure status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+int read_nic_word(struct net_device *dev, int indx, u16 *data)
+{
+	int status;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u16 *usbdata = kzalloc(sizeof(u16), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+				 RTL8187_REQ_GET_REGS, RTL8187_REQT_READ,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 2, 500);
+	*data = *usbdata;
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s failure status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+static int read_nic_word_E(struct net_device *dev, int indx, u16 *data)
+{
+	int status;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u16 *usbdata = kzalloc(sizeof(u16), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+				 RTL8187_REQ_GET_REGS, RTL8187_REQT_READ,
+				 indx | 0xfe00, 0, usbdata, 2, 500);
+	*data = *usbdata;
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s failure status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+int read_nic_dword(struct net_device *dev, int indx, u32 *data)
+{
+	int status;
+
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct usb_device *udev = priv->udev;
+	u32 *usbdata = kzalloc(sizeof(u32), GFP_KERNEL);
+
+	if (!usbdata)
+		return -ENOMEM;
+
+	status = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+				 RTL8187_REQ_GET_REGS, RTL8187_REQT_READ,
+				 (indx & 0xff) | 0xff00, (indx >> 8) & 0x0f,
+				 usbdata, 4, 500);
+	*data = *usbdata;
+	kfree(usbdata);
+
+	if (status < 0) {
+		netdev_err(dev, "%s failure status: %d\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+/* u8 read_phy_cck(struct net_device *dev, u8 adr); */
+/* u8 read_phy_ofdm(struct net_device *dev, u8 adr); */
+/* this might still called in what was the PHY rtl8185/rtl8192 common code
+ * plans are to possibility turn it again in one common code...
+ */
+inline void force_pci_posting(struct net_device *dev)
+{
+}
+
+static struct net_device_stats *rtl8192_stats(struct net_device *dev);
+static void rtl8192_restart(struct work_struct *work);
+static void watch_dog_timer_callback(struct timer_list *t);
+
+/****************************************************************************
+ *  -----------------------------MISC STUFF-------------------------
+ *****************************************************************************/
+
+short check_nic_enough_desc(struct net_device *dev, int queue_index)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int used = atomic_read(&priv->tx_pending[queue_index]);
+
+	return (used < MAX_TX_URB);
+}
+
+static void tx_timeout(struct net_device *dev, unsigned int txqueue)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	schedule_work(&priv->reset_wq);
+}
+
+void rtl8192_update_msr(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 msr;
+
+	read_nic_byte(dev, MSR, &msr);
+	msr &= ~MSR_LINK_MASK;
+
+	/* do not change in link_state != WLAN_LINK_ASSOCIATED.
+	 * msr must be updated if the state is ASSOCIATING.
+	 * this is intentional and make sense for ad-hoc and
+	 * master (see the create BSS/IBSS func)
+	 */
+	if (priv->ieee80211->state == IEEE80211_LINKED) {
+		if (priv->ieee80211->iw_mode == IW_MODE_INFRA)
+			msr |= (MSR_LINK_MANAGED << MSR_LINK_SHIFT);
+		else if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			msr |= (MSR_LINK_ADHOC << MSR_LINK_SHIFT);
+		else if (priv->ieee80211->iw_mode == IW_MODE_MASTER)
+			msr |= (MSR_LINK_MASTER << MSR_LINK_SHIFT);
+
+	} else {
+		msr |= (MSR_LINK_NONE << MSR_LINK_SHIFT);
+	}
+
+	write_nic_byte(dev, MSR, msr);
+}
+
+void rtl8192_set_chan(struct net_device *dev, short ch)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+
+	RT_TRACE(COMP_CH, "=====>%s()====ch:%d\n", __func__, ch);
+	priv->chan = ch;
+
+	/* this hack should avoid frame TX during channel setting*/
+
+	/* need to implement rf set channel here */
+
+	if (priv->rf_set_chan)
+		priv->rf_set_chan(dev, priv->chan);
+	mdelay(10);
+}
+
+static void rtl8192_rx_isr(struct urb *urb);
+
+static u32 get_rxpacket_shiftbytes_819xusb(struct ieee80211_rx_stats *pstats)
+{
+	return (sizeof(struct rx_desc_819x_usb) + pstats->RxDrvInfoSize
+		+ pstats->RxBufShift);
+}
+
+void rtl8192_rx_enable(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct urb *entry;
+	struct sk_buff *skb;
+	struct rtl8192_rx_info *info;
+
+	/* nomal packet rx procedure */
+	while (skb_queue_len(&priv->rx_queue) < MAX_RX_URB) {
+		skb = __dev_alloc_skb(RX_URB_SIZE, GFP_KERNEL);
+		if (!skb)
+			break;
+		entry = usb_alloc_urb(0, GFP_KERNEL);
+		if (!entry) {
+			kfree_skb(skb);
+			break;
+		}
+		usb_fill_bulk_urb(entry, priv->udev,
+				  usb_rcvbulkpipe(priv->udev, 3),
+				  skb_tail_pointer(skb),
+				  RX_URB_SIZE, rtl8192_rx_isr, skb);
+		info = (struct rtl8192_rx_info *)skb->cb;
+		info->urb = entry;
+		info->dev = dev;
+		info->out_pipe = 3; /* denote rx normal packet queue */
+		skb_queue_tail(&priv->rx_queue, skb);
+		usb_submit_urb(entry, GFP_KERNEL);
+	}
+
+	/* command packet rx procedure */
+	while (skb_queue_len(&priv->rx_queue) < MAX_RX_URB + 3) {
+		skb = __dev_alloc_skb(RX_URB_SIZE, GFP_KERNEL);
+		if (!skb)
+			break;
+		entry = usb_alloc_urb(0, GFP_KERNEL);
+		if (!entry) {
+			kfree_skb(skb);
+			break;
+		}
+		usb_fill_bulk_urb(entry, priv->udev,
+				  usb_rcvbulkpipe(priv->udev, 9),
+				  skb_tail_pointer(skb),
+				  RX_URB_SIZE, rtl8192_rx_isr, skb);
+		info = (struct rtl8192_rx_info *)skb->cb;
+		info->urb = entry;
+		info->dev = dev;
+		info->out_pipe = 9; /* denote rx cmd packet queue */
+		skb_queue_tail(&priv->rx_queue, skb);
+		usb_submit_urb(entry, GFP_KERNEL);
+	}
+}
+
+void rtl8192_set_rxconf(struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	u32 rxconf;
+
+	read_nic_dword(dev, RCR, &rxconf);
+	rxconf = rxconf & ~MAC_FILTER_MASK;
+	rxconf = rxconf | RCR_AMF;
+	rxconf = rxconf | RCR_ADF;
+	rxconf = rxconf | RCR_AB;
+	rxconf = rxconf | RCR_AM;
+
+	if (dev->flags & IFF_PROMISC)
+		DMESG("NIC in promisc mode");
+
+	if (priv->ieee80211->iw_mode == IW_MODE_MONITOR ||
+	    dev->flags & IFF_PROMISC) {
+		rxconf = rxconf | RCR_AAP;
+	} else {
+		rxconf = rxconf | RCR_APM;
+		rxconf = rxconf | RCR_CBSSID;
+	}
+
+	if (priv->ieee80211->iw_mode == IW_MODE_MONITOR) {
+		rxconf = rxconf | RCR_AICV;
+		rxconf = rxconf | RCR_APWRMGT;
+	}
+
+	if (priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		rxconf = rxconf | RCR_ACRC32;
+
+	rxconf = rxconf & ~RX_FIFO_THRESHOLD_MASK;
+	rxconf = rxconf | (RX_FIFO_THRESHOLD_NONE << RX_FIFO_THRESHOLD_SHIFT);
+	rxconf = rxconf & ~MAX_RX_DMA_MASK;
+	rxconf = rxconf | ((u32)7 << RCR_MXDMA_OFFSET);
+
+	rxconf = rxconf | RCR_ONLYERLPKT;
+
+	write_nic_dword(dev, RCR, rxconf);
+}
+
+void rtl8192_rtx_disable(struct net_device *dev)
+{
+	u8 cmd;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct sk_buff *skb;
+	struct rtl8192_rx_info *info;
+
+	read_nic_byte(dev, CMDR, &cmd);
+	write_nic_byte(dev, CMDR, cmd & ~(CR_TE | CR_RE));
+	force_pci_posting(dev);
+	mdelay(10);
+
+	while ((skb = __skb_dequeue(&priv->rx_queue))) {
+		info = (struct rtl8192_rx_info *)skb->cb;
+		if (!info->urb)
+			continue;
+
+		usb_kill_urb(info->urb);
+		kfree_skb(skb);
+	}
+
+	if (skb_queue_len(&priv->skb_queue))
+		netdev_warn(dev, "skb_queue not empty\n");
+
+	skb_queue_purge(&priv->skb_queue);
+}
+
+/* The prototype of rx_isr has changed since one version of Linux Kernel */
+static void rtl8192_rx_isr(struct urb *urb)
+{
+	struct sk_buff *skb = (struct sk_buff *)urb->context;
+	struct rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
+	struct net_device *dev = info->dev;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int out_pipe = info->out_pipe;
+	int err;
+
+	if (!priv->up)
+		return;
+
+	if (unlikely(urb->status)) {
+		info->urb = NULL;
+		priv->stats.rxstaterr++;
+		priv->ieee80211->stats.rx_errors++;
+		usb_free_urb(urb);
+		return;
+	}
+	skb_unlink(skb, &priv->rx_queue);
+	skb_put(skb, urb->actual_length);
+
+	skb_queue_tail(&priv->skb_queue, skb);
+	tasklet_schedule(&priv->irq_rx_tasklet);
+
+	skb = dev_alloc_skb(RX_URB_SIZE);
+	if (unlikely(!skb)) {
+		usb_free_urb(urb);
+		netdev_err(dev, "%s(): can't alloc skb\n", __func__);
+		/* TODO check rx queue length and refill *somewhere* */
+		return;
+	}
+
+	usb_fill_bulk_urb(urb, priv->udev,
+			  usb_rcvbulkpipe(priv->udev, out_pipe),
+			  skb_tail_pointer(skb),
+			  RX_URB_SIZE, rtl8192_rx_isr, skb);
+
+	info = (struct rtl8192_rx_info *)skb->cb;
+	info->urb = urb;
+	info->dev = dev;
+	info->out_pipe = out_pipe;
+
+	urb->transfer_buffer = skb_tail_pointer(skb);
+	urb->context = skb;
+	skb_queue_tail(&priv->rx_queue, skb);
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (err && err != -EPERM)
+		netdev_err(dev,
+			   "can not submit rxurb, err is %x, URB status is %x\n",
+			   err, urb->status);
+}
+
+static u32 rtl819xusb_rx_command_packet(struct net_device *dev,
+					struct ieee80211_rx_stats *pstats)
+{
+	u32	status;
+
+	status = cmpk_message_handle_rx(dev, pstats);
+	if (status)
+		DMESG("rxcommandpackethandle819xusb: It is a command packet\n");
+
+	return status;
+}
+
+static void rtl8192_data_hard_stop(struct net_device *dev)
+{
+	/* FIXME !! */
+}
+
+static void rtl8192_data_hard_resume(struct net_device *dev)
+{
+	/* FIXME !! */
+}
+
+/* this function TX data frames when the ieee80211 stack requires this.
+ * It checks also if we need to stop the ieee tx queue, eventually do it
+ */
+static void rtl8192_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
+				   int rate)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	unsigned long flags;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 queue_index = tcb_desc->queue_index;
+
+	/* shall not be referred by command packet */
+	RTL8192U_ASSERT(queue_index != TXCMD_QUEUE);
+
+	spin_lock_irqsave(&priv->tx_lock, flags);
+
+	*(struct net_device **)(skb->cb) = dev;
+	tcb_desc->bTxEnableFwCalcDur = 1;
+	skb_push(skb, priv->ieee80211->tx_headroom);
+	rtl8192_tx(dev, skb);
+
+	spin_unlock_irqrestore(&priv->tx_lock, flags);
+}
+
+/* This is a rough attempt to TX a frame
+ * This is called by the ieee 80211 stack to TX management frames.
+ * If the ring is full packet are dropped (for data frame the queue
+ * is stopped before this can happen).
+ */
+static int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	int ret;
+	unsigned long flags;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 queue_index = tcb_desc->queue_index;
+
+	spin_lock_irqsave(&priv->tx_lock, flags);
+
+	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+	if (queue_index == TXCMD_QUEUE) {
+		skb_push(skb, USB_HWDESC_HEADER_LEN);
+		rtl819xU_tx_cmd(dev, skb);
+		ret = 1;
+	} else {
+		skb_push(skb, priv->ieee80211->tx_headroom);
+		ret = rtl8192_tx(dev, skb);
+	}
+
+	spin_unlock_irqrestore(&priv->tx_lock, flags);
+
+	return ret;
+}
+
+static void rtl8192_tx_isr(struct urb *tx_urb)
+{
+	struct sk_buff *skb = (struct sk_buff *)tx_urb->context;
+	struct net_device *dev;
+	struct r8192_priv *priv = NULL;
+	struct cb_desc *tcb_desc;
+	u8  queue_index;
+
+	if (!skb)
+		return;
+
+	dev = *(struct net_device **)(skb->cb);
+	tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	queue_index = tcb_desc->queue_index;
+
+	priv = ieee80211_priv(dev);
+
+	if (tcb_desc->queue_index != TXCMD_QUEUE) {
+		if (tx_urb->status == 0) {
+			netif_trans_update(dev);
+			priv->stats.txoktotal++;
+			priv->ieee80211->LinkDetectInfo.NumTxOkInPeriod++;
+			priv->stats.txbytesunicast +=
+				(skb->len - priv->ieee80211->tx_headroom);
+		} else {
+			priv->ieee80211->stats.tx_errors++;
+			/* TODO */
+		}
+	}
+
+	/* free skb and tx_urb */
+	dev_kfree_skb_any(skb);
+	usb_free_urb(tx_urb);
+	atomic_dec(&priv->tx_pending[queue_index]);
+
+	/*
+	 * Handle HW Beacon:
+	 * We had transfer our beacon frame to host controller at this moment.
+	 *
+	 *
+	 * Caution:
+	 * Handling the wait queue of command packets.
+	 * For Tx command packets, we must not do TCB fragment because it is
+	 * not handled right now. We must cut the packets to match the size of
+	 * TX_CMD_PKT before we send it.
+	 */
+
+	/* Handle MPDU in wait queue. */
+	if (queue_index != BEACON_QUEUE) {
+		/* Don't send data frame during scanning.*/
+		if ((skb_queue_len(&priv->ieee80211->skb_waitQ[queue_index]) != 0) &&
+		    (!(priv->ieee80211->queue_stop))) {
+			skb = skb_dequeue(&(priv->ieee80211->skb_waitQ[queue_index]));
+			if (skb)
+				priv->ieee80211->softmac_hard_start_xmit(skb,
+									 dev);
+
+			return; /* avoid further processing AMSDU */
+		}
+	}
+}
+
+static void rtl8192_config_rate(struct net_device *dev, u16 *rate_config)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_network *net;
+	u8 i = 0, basic_rate = 0;
+
+	net = &priv->ieee80211->current_network;
+
+	for (i = 0; i < net->rates_len; i++) {
+		basic_rate = net->rates[i] & 0x7f;
+		switch (basic_rate) {
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
+		}
+	}
+	for (i = 0; i < net->rates_ex_len; i++) {
+		basic_rate = net->rates_ex[i] & 0x7f;
+		switch (basic_rate) {
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
+		}
+	}
+}
+
+#define SHORT_SLOT_TIME 9
+#define NON_SHORT_SLOT_TIME 20
+
+static void rtl8192_update_cap(struct net_device *dev, u16 cap)
+{
+	u32 tmp = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_network *net = &priv->ieee80211->current_network;
+
+	priv->short_preamble = cap & WLAN_CAPABILITY_SHORT_PREAMBLE;
+	tmp = priv->basic_rate;
+	if (priv->short_preamble)
+		tmp |= BRSR_AckShortPmb;
+	write_nic_dword(dev, RRSR, tmp);
+
+	if (net->mode & (IEEE_G | IEEE_N_24G)) {
+		u8 slot_time = 0;
+
+		if ((cap & WLAN_CAPABILITY_SHORT_SLOT) &&
+		    (!priv->ieee80211->pHTInfo->bCurrentRT2RTLongSlotTime))
+			/* short slot time */
+			slot_time = SHORT_SLOT_TIME;
+		else	/* long slot time */
+			slot_time = NON_SHORT_SLOT_TIME;
+		priv->slot_time = slot_time;
+		write_nic_byte(dev, SLOT_TIME, slot_time);
+	}
+}
+
+static void rtl8192_net_update(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_network *net;
+	u16 BcnTimeCfg = 0, BcnCW = 6, BcnIFS = 0xf;
+	u16 rate_config = 0;
+
+	net = &priv->ieee80211->current_network;
+
+	rtl8192_config_rate(dev, &rate_config);
+	priv->basic_rate = rate_config & 0x15f;
+
+	write_nic_dword(dev, BSSIDR, ((u32 *)net->bssid)[0]);
+	write_nic_word(dev, BSSIDR + 4, ((u16 *)net->bssid)[2]);
+
+	rtl8192_update_msr(dev);
+	if (priv->ieee80211->iw_mode == IW_MODE_ADHOC) {
+		write_nic_word(dev, ATIMWND, 2);
+		write_nic_word(dev, BCN_DMATIME, 1023);
+		write_nic_word(dev, BCN_INTERVAL, net->beacon_interval);
+		write_nic_word(dev, BCN_DRV_EARLY_INT, 1);
+		write_nic_byte(dev, BCN_ERR_THRESH, 100);
+		BcnTimeCfg |= (BcnCW << BCN_TCFG_CW_SHIFT);
+		/* TODO: BcnIFS may required to be changed on ASIC */
+		BcnTimeCfg |= BcnIFS << BCN_TCFG_IFS;
+
+		write_nic_word(dev, BCN_TCFG, BcnTimeCfg);
+	}
+}
+
+/* temporary hw beacon is not used any more.
+ * open it when necessary
+ */
+void rtl819xusb_beacon_tx(struct net_device *dev, u16  tx_rate)
+{
+}
+
+short rtl819xU_tx_cmd(struct net_device *dev, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int			status;
+	struct urb		*tx_urb;
+	unsigned int		idx_pipe;
+	struct tx_desc_cmd_819x_usb *pdesc = (struct tx_desc_cmd_819x_usb *)skb->data;
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	u8 queue_index = tcb_desc->queue_index;
+
+	atomic_inc(&priv->tx_pending[queue_index]);
+	tx_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!tx_urb) {
+		dev_kfree_skb(skb);
+		return -ENOMEM;
+	}
+
+	memset(pdesc, 0, USB_HWDESC_HEADER_LEN);
+	/* Tx descriptor ought to be set according to the skb->cb */
+	pdesc->FirstSeg = 1;
+	pdesc->LastSeg = 1;
+	pdesc->CmdInit = tcb_desc->bCmdOrInit;
+	pdesc->TxBufferSize = tcb_desc->txbuf_size;
+	pdesc->OWN = 1;
+	pdesc->LINIP = tcb_desc->bLastIniPkt;
+
+	/*---------------------------------------------------------------------
+	 * Fill up USB_OUT_CONTEXT.
+	 *---------------------------------------------------------------------
+	 */
+	idx_pipe = 0x04;
+	usb_fill_bulk_urb(tx_urb, priv->udev,
+			  usb_sndbulkpipe(priv->udev, idx_pipe),
+			  skb->data, skb->len, rtl8192_tx_isr, skb);
+
+	status = usb_submit_urb(tx_urb, GFP_ATOMIC);
+
+	if (!status)
+		return 0;
+
+	DMESGE("Error TX CMD URB, error %d", status);
+	dev_kfree_skb(skb);
+	usb_free_urb(tx_urb);
+	return -1;
+}
+
+/*
+ * Mapping Software/Hardware descriptor queue id to "Queue Select Field"
+ * in TxFwInfo data structure
+ * 2006.10.30 by Emily
+ *
+ * \param QUEUEID       Software Queue
+ */
+static u8 MapHwQueueToFirmwareQueue(u8 QueueID)
+{
+	u8 QueueSelect = 0x0;       /* default set to */
+
+	switch (QueueID) {
+	case BE_QUEUE:
+		QueueSelect = QSLT_BE;
+		break;
+
+	case BK_QUEUE:
+		QueueSelect = QSLT_BK;
+		break;
+
+	case VO_QUEUE:
+		QueueSelect = QSLT_VO;
+		break;
+
+	case VI_QUEUE:
+		QueueSelect = QSLT_VI;
+		break;
+	case MGNT_QUEUE:
+		QueueSelect = QSLT_MGNT;
+		break;
+
+	case BEACON_QUEUE:
+		QueueSelect = QSLT_BEACON;
+		break;
+
+		/* TODO: mark other queue selection until we verify it is OK */
+		/* TODO: Remove Assertions */
+	case TXCMD_QUEUE:
+		QueueSelect = QSLT_CMD;
+		break;
+	case HIGH_QUEUE:
+		QueueSelect = QSLT_HIGH;
+		break;
+
+	default:
+		RT_TRACE(COMP_ERR,
+			 "TransmitTCB(): Impossible Queue Selection: %d\n",
+			 QueueID);
+		break;
+	}
+	return QueueSelect;
+}
+
+static u8 MRateToHwRate8190Pci(u8 rate)
+{
+	u8  ret = DESC90_RATE1M;
+
+	switch (rate) {
+	case MGN_1M:
+		ret = DESC90_RATE1M;
+		break;
+	case MGN_2M:
+		ret = DESC90_RATE2M;
+		break;
+	case MGN_5_5M:
+		ret = DESC90_RATE5_5M;
+		break;
+	case MGN_11M:
+		ret = DESC90_RATE11M;
+		break;
+	case MGN_6M:
+		ret = DESC90_RATE6M;
+		break;
+	case MGN_9M:
+		ret = DESC90_RATE9M;
+		break;
+	case MGN_12M:
+		ret = DESC90_RATE12M;
+		break;
+	case MGN_18M:
+		ret = DESC90_RATE18M;
+		break;
+	case MGN_24M:
+		ret = DESC90_RATE24M;
+		break;
+	case MGN_36M:
+		ret = DESC90_RATE36M;
+		break;
+	case MGN_48M:
+		ret = DESC90_RATE48M;
+		break;
+	case MGN_54M:
+		ret = DESC90_RATE54M;
+		break;
+
+	/* HT rate since here */
+	case MGN_MCS0:
+		ret = DESC90_RATEMCS0;
+		break;
+	case MGN_MCS1:
+		ret = DESC90_RATEMCS1;
+		break;
+	case MGN_MCS2:
+		ret = DESC90_RATEMCS2;
+		break;
+	case MGN_MCS3:
+		ret = DESC90_RATEMCS3;
+		break;
+	case MGN_MCS4:
+		ret = DESC90_RATEMCS4;
+		break;
+	case MGN_MCS5:
+		ret = DESC90_RATEMCS5;
+		break;
+	case MGN_MCS6:
+		ret = DESC90_RATEMCS6;
+		break;
+	case MGN_MCS7:
+		ret = DESC90_RATEMCS7;
+		break;
+	case MGN_MCS8:
+		ret = DESC90_RATEMCS8;
+		break;
+	case MGN_MCS9:
+		ret = DESC90_RATEMCS9;
+		break;
+	case MGN_MCS10:
+		ret = DESC90_RATEMCS10;
+		break;
+	case MGN_MCS11:
+		ret = DESC90_RATEMCS11;
+		break;
+	case MGN_MCS12:
+		ret = DESC90_RATEMCS12;
+		break;
+	case MGN_MCS13:
+		ret = DESC90_RATEMCS13;
+		break;
+	case MGN_MCS14:
+		ret = DESC90_RATEMCS14;
+		break;
+	case MGN_MCS15:
+		ret = DESC90_RATEMCS15;
+		break;
+	case (0x80 | 0x20):
+		ret = DESC90_RATEMCS32;
+		break;
+
+	default:
+		break;
+	}
+	return ret;
+}
+
+static u8 QueryIsShort(u8 TxHT, u8 TxRate, struct cb_desc *tcb_desc)
+{
+	u8   tmp_Short;
+
+	tmp_Short = (TxHT == 1) ?
+			((tcb_desc->bUseShortGI) ? 1 : 0) :
+			((tcb_desc->bUseShortPreamble) ? 1 : 0);
+
+	if (TxHT == 1 && TxRate != DESC90_RATEMCS15)
+		tmp_Short = 0;
+
+	return tmp_Short;
+}
+
+static void tx_zero_isr(struct urb *tx_urb)
+{
+}
+
+/*
+ * The tx procedure is just as following,
+ * skb->cb will contain all the following information,
+ * priority, morefrag, rate, &dev.
+ */
+short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	struct tx_desc_819x_usb *tx_desc = (struct tx_desc_819x_usb *)skb->data;
+	struct tx_fwinfo_819x_usb *tx_fwinfo =
+		(struct tx_fwinfo_819x_usb *)(skb->data + USB_HWDESC_HEADER_LEN);
+	struct usb_device *udev = priv->udev;
+	int pend;
+	int status, rt = -1;
+	struct urb *tx_urb = NULL, *tx_urb_zero = NULL;
+	unsigned int idx_pipe;
+
+	pend = atomic_read(&priv->tx_pending[tcb_desc->queue_index]);
+	/* we are locked here so the two atomic_read and inc are executed
+	 * without interleaves
+	 * !!! For debug purpose
+	 */
+	if (pend > MAX_TX_URB) {
+		netdev_dbg(dev, "To discard skb packet!\n");
+		dev_kfree_skb_any(skb);
+		return -1;
+	}
+
+	tx_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!tx_urb) {
+		dev_kfree_skb_any(skb);
+		return -ENOMEM;
+	}
+
+	/* Fill Tx firmware info */
+	memset(tx_fwinfo, 0, sizeof(struct tx_fwinfo_819x_usb));
+	/* DWORD 0 */
+	tx_fwinfo->TxHT = (tcb_desc->data_rate & 0x80) ? 1 : 0;
+	tx_fwinfo->TxRate = MRateToHwRate8190Pci(tcb_desc->data_rate);
+	tx_fwinfo->EnableCPUDur = tcb_desc->bTxEnableFwCalcDur;
+	tx_fwinfo->Short = QueryIsShort(tx_fwinfo->TxHT, tx_fwinfo->TxRate,
+					tcb_desc);
+	if (tcb_desc->bAMPDUEnable) { /* AMPDU enabled */
+		tx_fwinfo->AllowAggregation = 1;
+		/* DWORD 1 */
+		tx_fwinfo->RxMF = tcb_desc->ampdu_factor;
+		tx_fwinfo->RxAMD = tcb_desc->ampdu_density & 0x07;
+	} else {
+		tx_fwinfo->AllowAggregation = 0;
+		/* DWORD 1 */
+		tx_fwinfo->RxMF = 0;
+		tx_fwinfo->RxAMD = 0;
+	}
+
+	/* Protection mode related */
+	tx_fwinfo->RtsEnable = (tcb_desc->bRTSEnable) ? 1 : 0;
+	tx_fwinfo->CtsEnable = (tcb_desc->bCTSEnable) ? 1 : 0;
+	tx_fwinfo->RtsSTBC = (tcb_desc->bRTSSTBC) ? 1 : 0;
+	tx_fwinfo->RtsHT = (tcb_desc->rts_rate & 0x80) ? 1 : 0;
+	tx_fwinfo->RtsRate =  MRateToHwRate8190Pci((u8)tcb_desc->rts_rate);
+	tx_fwinfo->RtsSubcarrier = (tx_fwinfo->RtsHT == 0) ? (tcb_desc->RTSSC) : 0;
+	tx_fwinfo->RtsBandwidth = (tx_fwinfo->RtsHT == 1) ? ((tcb_desc->bRTSBW) ? 1 : 0) : 0;
+	tx_fwinfo->RtsShort = (tx_fwinfo->RtsHT == 0) ? (tcb_desc->bRTSUseShortPreamble ? 1 : 0) :
+			      (tcb_desc->bRTSUseShortGI ? 1 : 0);
+
+	/* Set Bandwidth and sub-channel settings. */
+	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20_40) {
+		if (tcb_desc->bPacketBW) {
+			tx_fwinfo->TxBandwidth = 1;
+			/* use duplicated mode */
+			tx_fwinfo->TxSubCarrier = 0;
+		} else {
+			tx_fwinfo->TxBandwidth = 0;
+			tx_fwinfo->TxSubCarrier = priv->nCur40MhzPrimeSC;
+		}
+	} else {
+		tx_fwinfo->TxBandwidth = 0;
+		tx_fwinfo->TxSubCarrier = 0;
+	}
+
+	/* Fill Tx descriptor */
+	memset(tx_desc, 0, sizeof(struct tx_desc_819x_usb));
+	/* DWORD 0 */
+	tx_desc->LINIP = 0;
+	tx_desc->CmdInit = 1;
+	tx_desc->Offset =  sizeof(struct tx_fwinfo_819x_usb) + 8;
+	tx_desc->PktSize = (skb->len - TX_PACKET_SHIFT_BYTES) & 0xffff;
+
+	/*DWORD 1*/
+	tx_desc->SecCAMID = 0;
+	tx_desc->RATid = tcb_desc->RATRIndex;
+	tx_desc->NoEnc = 1;
+	tx_desc->SecType = 0x0;
+	if (tcb_desc->bHwSec) {
+		switch (priv->ieee80211->pairwise_key_type) {
+		case KEY_TYPE_WEP40:
+		case KEY_TYPE_WEP104:
+			tx_desc->SecType = 0x1;
+			tx_desc->NoEnc = 0;
+			break;
+		case KEY_TYPE_TKIP:
+			tx_desc->SecType = 0x2;
+			tx_desc->NoEnc = 0;
+			break;
+		case KEY_TYPE_CCMP:
+			tx_desc->SecType = 0x3;
+			tx_desc->NoEnc = 0;
+			break;
+		case KEY_TYPE_NA:
+			tx_desc->SecType = 0x0;
+			tx_desc->NoEnc = 1;
+			break;
+		}
+	}
+
+	tx_desc->QueueSelect = MapHwQueueToFirmwareQueue(tcb_desc->queue_index);
+	tx_desc->TxFWInfoSize =  sizeof(struct tx_fwinfo_819x_usb);
+
+	tx_desc->DISFB = tcb_desc->bTxDisableRateFallBack;
+	tx_desc->USERATE = tcb_desc->bTxUseDriverAssingedRate;
+
+	/* Fill fields that are required to be initialized in
+	 * all of the descriptors
+	 */
+	/* DWORD 0 */
+	tx_desc->FirstSeg = 1;
+	tx_desc->LastSeg = 1;
+	tx_desc->OWN = 1;
+
+	/* DWORD 2 */
+	tx_desc->TxBufferSize = (u32)(skb->len - USB_HWDESC_HEADER_LEN);
+	idx_pipe = 0x5;
+
+	/* To submit bulk urb */
+	usb_fill_bulk_urb(tx_urb, udev,
+			  usb_sndbulkpipe(udev, idx_pipe), skb->data,
+			  skb->len, rtl8192_tx_isr, skb);
+
+	status = usb_submit_urb(tx_urb, GFP_ATOMIC);
+	if (!status) {
+		/* We need to send 0 byte packet whenever
+		 * 512N bytes/64N(HIGN SPEED/NORMAL SPEED) bytes packet has
+		 * been transmitted. Otherwise, it will be halt to wait for
+		 * another packet.
+		 */
+		bool bSend0Byte = false;
+		u8 zero = 0;
+
+		if (udev->speed == USB_SPEED_HIGH) {
+			if (skb->len > 0 && skb->len % 512 == 0)
+				bSend0Byte = true;
+		} else {
+			if (skb->len > 0 && skb->len % 64 == 0)
+				bSend0Byte = true;
+		}
+		if (bSend0Byte) {
+			tx_urb_zero = usb_alloc_urb(0, GFP_ATOMIC);
+			if (!tx_urb_zero) {
+				rt = -ENOMEM;
+				goto error;
+			}
+			usb_fill_bulk_urb(tx_urb_zero, udev,
+					  usb_sndbulkpipe(udev, idx_pipe),
+					  &zero, 0, tx_zero_isr, dev);
+			status = usb_submit_urb(tx_urb_zero, GFP_ATOMIC);
+			if (status) {
+				RT_TRACE(COMP_ERR,
+					 "Error TX URB for zero byte %d, error %d",
+					 atomic_read(&priv->tx_pending[tcb_desc->queue_index]),
+					 status);
+				goto error;
+			}
+		}
+		netif_trans_update(dev);
+		atomic_inc(&priv->tx_pending[tcb_desc->queue_index]);
+		return 0;
+	}
+
+	RT_TRACE(COMP_ERR, "Error TX URB %d, error %d",
+		 atomic_read(&priv->tx_pending[tcb_desc->queue_index]),
+		 status);
+
+error:
+	dev_kfree_skb_any(skb);
+	usb_free_urb(tx_urb);
+	usb_free_urb(tx_urb_zero);
+	return rt;
+}
+
+static short rtl8192_usb_initendpoints(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->rx_urb = kmalloc_array(MAX_RX_URB + 1, sizeof(struct urb *),
+				     GFP_KERNEL);
+	if (!priv->rx_urb)
+		return -ENOMEM;
+
+#ifndef JACKSON_NEW_RX
+	for (i = 0; i < (MAX_RX_URB + 1); i++) {
+		priv->rx_urb[i] = usb_alloc_urb(0, GFP_KERNEL);
+		if (!priv->rx_urb[i])
+			return -ENOMEM;
+
+		priv->rx_urb[i]->transfer_buffer =
+			kmalloc(RX_URB_SIZE, GFP_KERNEL);
+		if (!priv->rx_urb[i]->transfer_buffer)
+			return -ENOMEM;
+
+		priv->rx_urb[i]->transfer_buffer_length = RX_URB_SIZE;
+	}
+#endif
+
+#ifdef THOMAS_BEACON
+	{
+		long align = 0;
+		void *oldaddr, *newaddr;
+
+		priv->rx_urb[16] = usb_alloc_urb(0, GFP_KERNEL);
+		if (!priv->rx_urb[16])
+			return -ENOMEM;
+		priv->oldaddr = kmalloc(16, GFP_KERNEL);
+		if (!priv->oldaddr)
+			return -ENOMEM;
+		oldaddr = priv->oldaddr;
+		align = ((long)oldaddr) & 3;
+		if (align) {
+			newaddr = oldaddr + 4 - align;
+			priv->rx_urb[16]->transfer_buffer_length = 16 - 4 + align;
+		} else {
+			newaddr = oldaddr;
+			priv->rx_urb[16]->transfer_buffer_length = 16;
+		}
+		priv->rx_urb[16]->transfer_buffer = newaddr;
+	}
+#endif
+
+	memset(priv->rx_urb, 0, sizeof(struct urb *) * MAX_RX_URB);
+	priv->pp_rxskb = kcalloc(MAX_RX_URB, sizeof(struct sk_buff *),
+				 GFP_KERNEL);
+	if (!priv->pp_rxskb) {
+		kfree(priv->rx_urb);
+
+		priv->pp_rxskb = NULL;
+		priv->rx_urb = NULL;
+
+		DMESGE("Endpoint Alloc Failure");
+		return -ENOMEM;
+	}
+
+	netdev_dbg(dev, "End of initendpoints\n");
+	return 0;
+}
+
+#ifdef THOMAS_BEACON
+static void rtl8192_usb_deleteendpoints(struct net_device *dev)
+{
+	int i;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->rx_urb) {
+		for (i = 0; i < (MAX_RX_URB + 1); i++) {
+			usb_kill_urb(priv->rx_urb[i]);
+			usb_free_urb(priv->rx_urb[i]);
+		}
+		kfree(priv->rx_urb);
+		priv->rx_urb = NULL;
+	}
+	kfree(priv->oldaddr);
+	priv->oldaddr = NULL;
+
+	kfree(priv->pp_rxskb);
+	priv->pp_rxskb = NULL;
+}
+#else
+void rtl8192_usb_deleteendpoints(struct net_device *dev)
+{
+	int i;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+#ifndef JACKSON_NEW_RX
+
+	if (priv->rx_urb) {
+		for (i = 0; i < (MAX_RX_URB + 1); i++) {
+			usb_kill_urb(priv->rx_urb[i]);
+			kfree(priv->rx_urb[i]->transfer_buffer);
+			usb_free_urb(priv->rx_urb[i]);
+		}
+		kfree(priv->rx_urb);
+		priv->rx_urb = NULL;
+	}
+#else
+	kfree(priv->rx_urb);
+	priv->rx_urb = NULL;
+	kfree(priv->oldaddr);
+	priv->oldaddr = NULL;
+
+	kfree(priv->pp_rxskb);
+	priv->pp_rxskb = 0;
+
+#endif
+}
+#endif
+
+static void rtl8192_update_ratr_table(struct net_device *dev);
+static void rtl8192_link_change(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	if (ieee->state == IEEE80211_LINKED) {
+		rtl8192_net_update(dev);
+		rtl8192_update_ratr_table(dev);
+		/* Add this as in pure N mode, wep encryption will use software
+		 * way, but there is no chance to set this as wep will not set
+		 * group key in wext.
+		 */
+		if (ieee->pairwise_key_type == KEY_TYPE_WEP40 ||
+		    ieee->pairwise_key_type == KEY_TYPE_WEP104)
+			EnableHWSecurityConfig8192(dev);
+	}
+	/*update timing params*/
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
+		u32 reg = 0;
+
+		read_nic_dword(dev, RCR, &reg);
+		if (priv->ieee80211->state == IEEE80211_LINKED)
+			priv->ReceiveConfig = reg |= RCR_CBSSID;
+		else
+			priv->ReceiveConfig = reg &= ~RCR_CBSSID;
+		write_nic_dword(dev, RCR, reg);
+	}
+}
+
+static const struct ieee80211_qos_parameters def_qos_parameters = {
+	{cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3)},
+	{cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7)},
+	{2, 2, 2, 2},/* aifs */
+	{0, 0, 0, 0},/* flags */
+	{0, 0, 0, 0} /* tx_op_limit */
+};
+
+static void rtl8192_update_beacon(struct work_struct *work)
+{
+	struct r8192_priv *priv = container_of(work, struct r8192_priv,
+					       update_beacon_wq.work);
+	struct net_device *dev = priv->ieee80211->dev;
+	struct ieee80211_device *ieee = priv->ieee80211;
+	struct ieee80211_network *net = &ieee->current_network;
+
+	if (ieee->pHTInfo->bCurrentHTSupport)
+		HTUpdateSelfAndPeerSetting(ieee, net);
+	ieee->pHTInfo->bCurrentRT2RTLongSlotTime =
+		net->bssht.bdRT2RTLongSlotTime;
+	rtl8192_update_cap(dev, net->capability);
+}
+
+/*
+ * background support to run QoS activate functionality
+ */
+static int WDCAPARA_ADD[] = {EDCAPARA_BE, EDCAPARA_BK,
+			     EDCAPARA_VI, EDCAPARA_VO};
+static void rtl8192_qos_activate(struct work_struct *work)
+{
+	struct r8192_priv *priv = container_of(work, struct r8192_priv,
+					       qos_activate);
+	struct net_device *dev = priv->ieee80211->dev;
+	struct ieee80211_qos_parameters *qos_parameters =
+		&priv->ieee80211->current_network.qos_data.parameters;
+	u8 mode = priv->ieee80211->current_network.mode;
+	u32  u1bAIFS;
+	u32 u4bAcParam;
+	u32 op_limit;
+	u32 cw_max;
+	u32 cw_min;
+	int i;
+
+	mutex_lock(&priv->mutex);
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		goto success;
+	RT_TRACE(COMP_QOS,
+		 "qos active process with associate response received\n");
+	/* It better set slot time at first
+	 *
+	 * For we just support b/g mode at present, let the slot time at
+	 * 9/20 selection
+	 *
+	 * update the ac parameter to related registers
+	 */
+	for (i = 0; i <  QOS_QUEUE_NUM; i++) {
+		/* Mode G/A: slotTimeTimer = 9; Mode B: 20 */
+		u1bAIFS = qos_parameters->aifs[i] * ((mode & (IEEE_G | IEEE_N_24G)) ? 9 : 20) + aSifsTime;
+		u1bAIFS <<= AC_PARAM_AIFS_OFFSET;
+		op_limit = (u32)le16_to_cpu(qos_parameters->tx_op_limit[i]);
+		op_limit <<= AC_PARAM_TXOP_LIMIT_OFFSET;
+		cw_max = (u32)le16_to_cpu(qos_parameters->cw_max[i]);
+		cw_max <<= AC_PARAM_ECW_MAX_OFFSET;
+		cw_min = (u32)le16_to_cpu(qos_parameters->cw_min[i]);
+		cw_min <<= AC_PARAM_ECW_MIN_OFFSET;
+		u4bAcParam = op_limit | cw_max | cw_min | u1bAIFS;
+		write_nic_dword(dev, WDCAPARA_ADD[i], u4bAcParam);
+	}
+
+success:
+	mutex_unlock(&priv->mutex);
+}
+
+static int rtl8192_qos_handle_probe_response(struct r8192_priv *priv,
+					     int active_network,
+					     struct ieee80211_network *network)
+{
+	int ret = 0;
+	u32 size = sizeof(struct ieee80211_qos_parameters);
+
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		return ret;
+
+	if (priv->ieee80211->iw_mode != IW_MODE_INFRA)
+		return ret;
+
+	if (network->flags & NETWORK_HAS_QOS_MASK) {
+		if (active_network &&
+		    (network->flags & NETWORK_HAS_QOS_PARAMETERS))
+			network->qos_data.active = network->qos_data.supported;
+
+		if ((network->qos_data.active == 1) && (active_network == 1) &&
+		    (network->flags & NETWORK_HAS_QOS_PARAMETERS) &&
+		    (network->qos_data.old_param_count !=
+		     network->qos_data.param_count)) {
+			network->qos_data.old_param_count =
+				network->qos_data.param_count;
+			schedule_work(&priv->qos_activate);
+			RT_TRACE(COMP_QOS,
+				 "QoS parameters change call qos_activate\n");
+		}
+	} else {
+		memcpy(&priv->ieee80211->current_network.qos_data.parameters,
+		       &def_qos_parameters, size);
+
+		if ((network->qos_data.active == 1) && (active_network == 1)) {
+			schedule_work(&priv->qos_activate);
+			RT_TRACE(COMP_QOS,
+				 "QoS was disabled call qos_activate\n");
+		}
+		network->qos_data.active = 0;
+		network->qos_data.supported = 0;
+	}
+
+	return 0;
+}
+
+/* handle and manage frame from beacon and probe response */
+static int rtl8192_handle_beacon(struct net_device *dev,
+				 struct ieee80211_beacon *beacon,
+				 struct ieee80211_network *network)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	rtl8192_qos_handle_probe_response(priv, 1, network);
+	schedule_delayed_work(&priv->update_beacon_wq, 0);
+	return 0;
+}
+
+/*
+ * handling the beaconing responses. if we get different QoS setting
+ * off the network from the associated setting, adjust the QoS
+ * setting
+ */
+static int rtl8192_qos_association_resp(struct r8192_priv *priv,
+					struct ieee80211_network *network)
+{
+	unsigned long flags;
+	u32 size = sizeof(struct ieee80211_qos_parameters);
+	int set_qos_param = 0;
+
+	if (!priv || !network)
+		return 0;
+
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		return 0;
+
+	if (priv->ieee80211->iw_mode != IW_MODE_INFRA)
+		return 0;
+
+	spin_lock_irqsave(&priv->ieee80211->lock, flags);
+	if (network->flags & NETWORK_HAS_QOS_PARAMETERS) {
+		memcpy(&priv->ieee80211->current_network.qos_data.parameters,
+		       &network->qos_data.parameters,
+		       sizeof(struct ieee80211_qos_parameters));
+		priv->ieee80211->current_network.qos_data.active = 1;
+		set_qos_param = 1;
+		/* update qos parameter for current network */
+		priv->ieee80211->current_network.qos_data.old_param_count =
+			priv->ieee80211->current_network.qos_data.param_count;
+		priv->ieee80211->current_network.qos_data.param_count =
+			network->qos_data.param_count;
+	} else {
+		memcpy(&priv->ieee80211->current_network.qos_data.parameters,
+		       &def_qos_parameters, size);
+		priv->ieee80211->current_network.qos_data.active = 0;
+		priv->ieee80211->current_network.qos_data.supported = 0;
+		set_qos_param = 1;
+	}
+
+	spin_unlock_irqrestore(&priv->ieee80211->lock, flags);
+
+	RT_TRACE(COMP_QOS, "%s: network->flags = %d,%d\n", __func__,
+		 network->flags,
+		 priv->ieee80211->current_network.qos_data.active);
+	if (set_qos_param == 1)
+		schedule_work(&priv->qos_activate);
+
+	return 0;
+}
+
+static int rtl8192_handle_assoc_response(struct net_device *dev,
+					 struct ieee80211_assoc_response_frame *resp,
+					 struct ieee80211_network *network)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	rtl8192_qos_association_resp(priv, network);
+	return 0;
+}
+
+static void rtl8192_update_ratr_table(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	u8 *pMcsRate = ieee->dot11HTOperationalRateSet;
+	u32 ratr_value = 0;
+	u8 rate_index = 0;
+
+	rtl8192_config_rate(dev, (u16 *)(&ratr_value));
+	ratr_value |= (*(u16 *)(pMcsRate)) << 12;
+	switch (ieee->mode) {
+	case IEEE_A:
+		ratr_value &= 0x00000FF0;
+		break;
+	case IEEE_B:
+		ratr_value &= 0x0000000F;
+		break;
+	case IEEE_G:
+		ratr_value &= 0x00000FF7;
+		break;
+	case IEEE_N_24G:
+	case IEEE_N_5G:
+		if (ieee->pHTInfo->PeerMimoPs == MIMO_PS_STATIC) {
+			ratr_value &= 0x0007F007;
+		} else {
+			if (priv->rf_type == RF_1T2R)
+				ratr_value &= 0x000FF007;
+			else
+				ratr_value &= 0x0F81F007;
+		}
+		break;
+	default:
+		break;
+	}
+	ratr_value &= 0x0FFFFFFF;
+	if (ieee->pHTInfo->bCurTxBW40MHz && ieee->pHTInfo->bCurShortGI40MHz)
+		ratr_value |= 0x80000000;
+	else if (!ieee->pHTInfo->bCurTxBW40MHz &&
+		 ieee->pHTInfo->bCurShortGI20MHz)
+		ratr_value |= 0x80000000;
+	write_nic_dword(dev, RATR0 + rate_index * 4, ratr_value);
+	write_nic_byte(dev, UFWP, 1);
+}
+
+static u8 ccmp_ie[4] = {0x00, 0x50, 0xf2, 0x04};
+static u8 ccmp_rsn_ie[4] = {0x00, 0x0f, 0xac, 0x04};
+static bool GetNmodeSupportBySecCfg8192(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	struct ieee80211_network *network = &ieee->current_network;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	struct ieee80211_crypt_data *crypt;
+	int encrypt;
+
+	crypt = ieee->crypt[ieee->tx_keyidx];
+	/* we use connecting AP's capability instead of only security config
+	 * on our driver to distinguish whether it should use N mode or G mode
+	 */
+	encrypt = (network->capability & WLAN_CAPABILITY_PRIVACY) ||
+		  (ieee->host_encrypt && crypt && crypt->ops &&
+		   (strcmp(crypt->ops->name, "WEP") == 0));
+
+	/* simply judge  */
+	if (encrypt && (wpa_ie_len == 0)) {
+		/* wep encryption, no N mode setting */
+		return false;
+	} else if ((wpa_ie_len != 0)) {
+		/* parse pairwise key type */
+		if (((ieee->wpa_ie[0] == 0xdd) && (!memcmp(&(ieee->wpa_ie[14]), ccmp_ie, 4))) || ((ieee->wpa_ie[0] == 0x30) && (!memcmp(&ieee->wpa_ie[10], ccmp_rsn_ie, 4))))
+			return true;
+		else
+			return false;
+	} else {
+		return true;
+	}
+
+	return true;
+}
+
+static bool GetHalfNmodeSupportByAPs819xUsb(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return priv->ieee80211->bHalfWirelessN24GMode;
+}
+
+static void rtl8192_refresh_supportrate(struct r8192_priv *priv)
+{
+	struct ieee80211_device *ieee = priv->ieee80211;
+	/* We do not consider set support rate for ABG mode, only
+	 * HT MCS rate is set here.
+	 */
+	if (ieee->mode == WIRELESS_MODE_N_24G ||
+	    ieee->mode == WIRELESS_MODE_N_5G)
+		memcpy(ieee->Regdot11HTOperationalRateSet,
+		       ieee->RegHTSuppRateSet, 16);
+	else
+		memset(ieee->Regdot11HTOperationalRateSet, 0, 16);
+}
+
+static u8 rtl8192_getSupportedWireleeMode(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 ret = 0;
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+	case RF_8256:
+	case RF_PSEUDO_11N:
+		ret = WIRELESS_MODE_N_24G | WIRELESS_MODE_G | WIRELESS_MODE_B;
+		break;
+	case RF_8258:
+		ret = WIRELESS_MODE_A | WIRELESS_MODE_N_5G;
+		break;
+	default:
+		ret = WIRELESS_MODE_B;
+		break;
+	}
+	return ret;
+}
+
+static void rtl8192_SetWirelessMode(struct net_device *dev, u8 wireless_mode)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 bSupportMode = rtl8192_getSupportedWireleeMode(dev);
+
+	if (wireless_mode == WIRELESS_MODE_AUTO ||
+	    (wireless_mode & bSupportMode) == 0) {
+		if (bSupportMode & WIRELESS_MODE_N_24G) {
+			wireless_mode = WIRELESS_MODE_N_24G;
+		} else if (bSupportMode & WIRELESS_MODE_N_5G) {
+			wireless_mode = WIRELESS_MODE_N_5G;
+		} else if ((bSupportMode & WIRELESS_MODE_A)) {
+			wireless_mode = WIRELESS_MODE_A;
+		} else if ((bSupportMode & WIRELESS_MODE_G)) {
+			wireless_mode = WIRELESS_MODE_G;
+		} else if ((bSupportMode & WIRELESS_MODE_B)) {
+			wireless_mode = WIRELESS_MODE_B;
+		} else {
+			RT_TRACE(COMP_ERR,
+				 "%s(), No valid wireless mode supported, SupportedWirelessMode(%x)!!!\n",
+				 __func__, bSupportMode);
+			wireless_mode = WIRELESS_MODE_B;
+		}
+	}
+	priv->ieee80211->mode = wireless_mode;
+
+	if (wireless_mode == WIRELESS_MODE_N_24G ||
+	    wireless_mode == WIRELESS_MODE_N_5G)
+		priv->ieee80211->pHTInfo->bEnableHT = 1;
+	else
+		priv->ieee80211->pHTInfo->bEnableHT = 0;
+	RT_TRACE(COMP_INIT, "Current Wireless Mode is %x\n", wireless_mode);
+	rtl8192_refresh_supportrate(priv);
+}
+
+/* init priv variables here. only non_zero value should be initialized here. */
+static int rtl8192_init_priv_variable(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 i;
+
+	priv->card_8192 = NIC_8192U;
+	priv->chan = 1; /* set to channel 1 */
+	priv->ieee80211->mode = WIRELESS_MODE_AUTO; /* SET AUTO */
+	priv->ieee80211->iw_mode = IW_MODE_INFRA;
+	priv->ieee80211->ieee_up = 0;
+	priv->retry_rts = DEFAULT_RETRY_RTS;
+	priv->retry_data = DEFAULT_RETRY_DATA;
+	priv->ieee80211->rts = DEFAULT_RTS_THRESHOLD;
+	priv->ieee80211->rate = 110; /* 11 mbps */
+	priv->ieee80211->short_slot = 1;
+	priv->promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
+	priv->CckPwEnl = 6;
+	/* for silent reset */
+	priv->IrpPendingCount = 1;
+	priv->ResetProgress = RESET_TYPE_NORESET;
+	priv->bForcedSilentReset = false;
+	priv->bDisableNormalResetCheck = false;
+	priv->force_reset = false;
+
+	/* we don't use FW read/write RF until stable firmware is available. */
+	priv->ieee80211->FwRWRF = 0;
+	priv->ieee80211->current_network.beacon_interval =
+		DEFAULT_BEACONINTERVAL;
+	priv->ieee80211->softmac_features  = IEEE_SOFTMAC_SCAN |
+		IEEE_SOFTMAC_ASSOCIATE | IEEE_SOFTMAC_PROBERQ |
+		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE |
+		IEEE_SOFTMAC_BEACONS;
+
+	priv->ieee80211->active_scan = 1;
+	priv->ieee80211->modulation =
+		IEEE80211_CCK_MODULATION | IEEE80211_OFDM_MODULATION;
+	priv->ieee80211->host_encrypt = 1;
+	priv->ieee80211->host_decrypt = 1;
+	priv->ieee80211->start_send_beacons = NULL;
+	priv->ieee80211->stop_send_beacons = NULL;
+	priv->ieee80211->softmac_hard_start_xmit = rtl8192_hard_start_xmit;
+	priv->ieee80211->set_chan = rtl8192_set_chan;
+	priv->ieee80211->link_change = rtl8192_link_change;
+	priv->ieee80211->softmac_data_hard_start_xmit = rtl8192_hard_data_xmit;
+	priv->ieee80211->data_hard_stop = rtl8192_data_hard_stop;
+	priv->ieee80211->data_hard_resume = rtl8192_data_hard_resume;
+	priv->ieee80211->init_wmmparam_flag = 0;
+	priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
+	priv->ieee80211->check_nic_enough_desc = check_nic_enough_desc;
+	priv->ieee80211->tx_headroom = TX_PACKET_SHIFT_BYTES;
+	priv->ieee80211->qos_support = 1;
+
+	priv->ieee80211->SetBWModeHandler = rtl8192_SetBWMode;
+	priv->ieee80211->handle_assoc_response = rtl8192_handle_assoc_response;
+	priv->ieee80211->handle_beacon = rtl8192_handle_beacon;
+
+	priv->ieee80211->GetNmodeSupportBySecCfg = GetNmodeSupportBySecCfg8192;
+	priv->ieee80211->GetHalfNmodeSupportByAPsHandler =
+		GetHalfNmodeSupportByAPs819xUsb;
+	priv->ieee80211->SetWirelessMode = rtl8192_SetWirelessMode;
+
+	priv->ieee80211->InitialGainHandler = InitialGain819xUsb;
+	priv->card_type = USB;
+	priv->ShortRetryLimit = 0x30;
+	priv->LongRetryLimit = 0x30;
+	priv->EarlyRxThreshold = 7;
+	priv->enable_gpio0 = 0;
+	priv->TransmitConfig =
+		/* Max DMA Burst Size per Tx DMA Burst, 7: reserved. */
+		(TCR_MXDMA_2048 << TCR_MXDMA_OFFSET)	  |
+		/* Short retry limit */
+		(priv->ShortRetryLimit << TCR_SRL_OFFSET) |
+		/* Long retry limit */
+		(priv->LongRetryLimit << TCR_LRL_OFFSET)  |
+		/* FALSE: HW provides PLCP length and LENGEXT
+		 * TRUE: SW provides them
+		 */
+		(false ? TCR_SAT : 0);
+	priv->ReceiveConfig	=
+		/* accept management/data */
+		RCR_AMF | RCR_ADF |
+		/* accept control frame for SW AP needs PS-poll */
+		RCR_ACF |
+		/* accept BC/MC/UC */
+		RCR_AB | RCR_AM | RCR_APM |
+		/* Max DMA Burst Size per Rx DMA Burst, 7: unlimited. */
+		((u32)7 << RCR_MXDMA_OFFSET) |
+		/* Rx FIFO Threshold, 7: No Rx threshold. */
+		(priv->EarlyRxThreshold << RX_FIFO_THRESHOLD_SHIFT) |
+		(priv->EarlyRxThreshold == 7 ? RCR_ONLYERLPKT : 0);
+
+	priv->AcmControl = 0;
+	priv->pFirmware = kzalloc(sizeof(rt_firmware), GFP_KERNEL);
+	if (!priv->pFirmware)
+		return -ENOMEM;
+
+	/* rx related queue */
+	skb_queue_head_init(&priv->rx_queue);
+	skb_queue_head_init(&priv->skb_queue);
+
+	/* Tx related queue */
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_head_init(&priv->ieee80211->skb_waitQ[i]);
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_head_init(&priv->ieee80211->skb_aggQ[i]);
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_head_init(&priv->ieee80211->skb_drv_aggQ[i]);
+	priv->rf_set_chan = rtl8192_phy_SwChnl;
+
+	return 0;
+}
+
+/* init lock here */
+static void rtl8192_init_priv_lock(struct r8192_priv *priv)
+{
+	spin_lock_init(&priv->tx_lock);
+	spin_lock_init(&priv->irq_lock);
+	mutex_init(&priv->wx_mutex);
+	mutex_init(&priv->mutex);
+}
+
+static void rtl819x_watchdog_wqcallback(struct work_struct *work);
+
+static void rtl8192_irq_rx_tasklet(struct tasklet_struct *t);
+/* init tasklet and wait_queue here. only 2.6 above kernel is considered */
+#define DRV_NAME "wlan0"
+static void rtl8192_init_priv_task(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	INIT_WORK(&priv->reset_wq, rtl8192_restart);
+
+	INIT_DELAYED_WORK(&priv->watch_dog_wq,
+			  rtl819x_watchdog_wqcallback);
+	INIT_DELAYED_WORK(&priv->txpower_tracking_wq,
+			  dm_txpower_trackingcallback);
+	INIT_DELAYED_WORK(&priv->rfpath_check_wq,
+			  dm_rf_pathcheck_workitemcallback);
+	INIT_DELAYED_WORK(&priv->update_beacon_wq,
+			  rtl8192_update_beacon);
+	INIT_DELAYED_WORK(&priv->initialgain_operate_wq,
+			  InitialGainOperateWorkItemCallBack);
+	INIT_WORK(&priv->qos_activate, rtl8192_qos_activate);
+
+	tasklet_setup(&priv->irq_rx_tasklet, rtl8192_irq_rx_tasklet);
+}
+
+static void rtl8192_get_eeprom_size(struct net_device *dev)
+{
+	u16 curCR = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	RT_TRACE(COMP_EPROM, "===========>%s()\n", __func__);
+	read_nic_word_E(dev, EPROM_CMD, &curCR);
+	RT_TRACE(COMP_EPROM,
+		 "read from Reg EPROM_CMD(%x):%x\n", EPROM_CMD, curCR);
+	/* whether need I consider BIT(5?) */
+	priv->epromtype =
+		(curCR & Cmd9346CR_9356SEL) ? EPROM_93c56 : EPROM_93c46;
+	RT_TRACE(COMP_EPROM,
+		 "<===========%s(), epromtype:%d\n", __func__, priv->epromtype);
+}
+
+/* used to swap endian. as ntohl & htonl are not necessary
+ * to swap endian, so use this instead.
+ */
+static inline u16 endian_swap(u16 *data)
+{
+	u16 tmp = *data;
+	*data = (tmp >> 8) | (tmp << 8);
+	return *data;
+}
+
+static int rtl8192_read_eeprom_info(struct net_device *dev)
+{
+	u16 wEPROM_ID = 0;
+	u8 bMac_Tmp_Addr[6] = {0x00, 0xe0, 0x4c, 0x00, 0x00, 0x02};
+	u8 bLoad_From_EEPOM = false;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u16 tmpValue = 0;
+	int i;
+	int ret;
+
+	RT_TRACE(COMP_EPROM, "===========>%s()\n", __func__);
+	ret = eprom_read(dev, 0); /* first read EEPROM ID out; */
+	if (ret < 0)
+		return ret;
+	wEPROM_ID = (u16)ret;
+	RT_TRACE(COMP_EPROM, "EEPROM ID is 0x%x\n", wEPROM_ID);
+
+	if (wEPROM_ID != RTL8190_EEPROM_ID)
+		RT_TRACE(COMP_ERR,
+			 "EEPROM ID is invalid(is 0x%x(should be 0x%x)\n",
+			 wEPROM_ID, RTL8190_EEPROM_ID);
+	else
+		bLoad_From_EEPOM = true;
+
+	if (bLoad_From_EEPOM) {
+		tmpValue = eprom_read(dev, EEPROM_VID >> 1);
+		ret = eprom_read(dev, EEPROM_VID >> 1);
+		if (ret < 0)
+			return ret;
+		tmpValue = (u16)ret;
+		priv->eeprom_vid = endian_swap(&tmpValue);
+		ret = eprom_read(dev, EEPROM_PID >> 1);
+		if (ret < 0)
+			return ret;
+		priv->eeprom_pid = (u16)ret;
+		ret = eprom_read(dev, EEPROM_CHANNEL_PLAN >> 1);
+		if (ret < 0)
+			return ret;
+		tmpValue = (u16)ret;
+		priv->eeprom_ChannelPlan = (tmpValue & 0xff00) >> 8;
+		priv->btxpowerdata_readfromEEPORM = true;
+		ret = eprom_read(dev, (EEPROM_CUSTOMER_ID >> 1)) >> 8;
+		if (ret < 0)
+			return ret;
+		priv->eeprom_CustomerID = (u16)ret;
+	} else {
+		priv->eeprom_vid = 0;
+		priv->eeprom_pid = 0;
+		priv->card_8192_version = VERSION_819XU_B;
+		priv->eeprom_ChannelPlan = 0;
+		priv->eeprom_CustomerID = 0;
+	}
+	RT_TRACE(COMP_EPROM,
+		 "vid:0x%4x, pid:0x%4x, CustomID:0x%2x, ChanPlan:0x%x\n",
+		 priv->eeprom_vid, priv->eeprom_pid, priv->eeprom_CustomerID,
+		 priv->eeprom_ChannelPlan);
+	/* set channelplan from eeprom */
+	priv->ChannelPlan = priv->eeprom_ChannelPlan;
+	if (bLoad_From_EEPOM) {
+		u8 addr[ETH_ALEN];
+
+		for (i = 0; i < 6; i += 2) {
+			ret = eprom_read(dev, (u16)((EEPROM_NODE_ADDRESS_BYTE_0 + i) >> 1));
+			if (ret < 0)
+				return ret;
+			*(u16 *)(&addr[i]) = (u16)ret;
+		}
+		eth_hw_addr_set(dev, addr);
+	} else {
+		eth_hw_addr_set(dev, bMac_Tmp_Addr);
+		/* should I set IDR0 here? */
+	}
+	RT_TRACE(COMP_EPROM, "MAC addr:%pM\n", dev->dev_addr);
+	priv->rf_type = RTL819X_DEFAULT_RF_TYPE; /* default 1T2R */
+	priv->rf_chip = RF_8256;
+
+	if (priv->card_8192_version == VERSION_819XU_A) {
+		/* read Tx power gain offset of legacy OFDM to HT rate */
+		if (bLoad_From_EEPOM) {
+			ret = eprom_read(dev, (EEPROM_TX_POWER_DIFF >> 1));
+			if (ret < 0)
+				return ret;
+			priv->EEPROMTxPowerDiff = ((u16)ret & 0xff00) >> 8;
+		} else
+			priv->EEPROMTxPowerDiff = EEPROM_DEFAULT_TX_POWER;
+		RT_TRACE(COMP_EPROM, "TxPowerDiff:%d\n", priv->EEPROMTxPowerDiff);
+		/* read ThermalMeter from EEPROM */
+		if (bLoad_From_EEPOM) {
+			ret = eprom_read(dev, (EEPROM_THERMAL_METER >> 1));
+			if (ret < 0)
+				return ret;
+			priv->EEPROMThermalMeter = (u8)((u16)ret & 0x00ff);
+		} else
+			priv->EEPROMThermalMeter = EEPROM_DEFAULT_THERNAL_METER;
+		RT_TRACE(COMP_EPROM, "ThermalMeter:%d\n", priv->EEPROMThermalMeter);
+		/* for tx power track */
+		priv->TSSI_13dBm = priv->EEPROMThermalMeter * 100;
+		/* read antenna tx power offset of B/C/D to A from EEPROM */
+		if (bLoad_From_EEPOM) {
+			ret = eprom_read(dev, (EEPROM_PW_DIFF >> 1));
+			if (ret < 0)
+				return ret;
+			priv->EEPROMPwDiff = ((u16)ret & 0x0f00) >> 8;
+		} else
+			priv->EEPROMPwDiff = EEPROM_DEFAULT_PW_DIFF;
+		RT_TRACE(COMP_EPROM, "TxPwDiff:%d\n", priv->EEPROMPwDiff);
+		/* Read CrystalCap from EEPROM */
+		if (bLoad_From_EEPOM) {
+			ret = eprom_read(dev, (EEPROM_CRYSTAL_CAP >> 1));
+			if (ret < 0)
+				return ret;
+			priv->EEPROMCrystalCap = (u16)ret & 0x0f;
+		} else
+			priv->EEPROMCrystalCap = EEPROM_DEFAULT_CRYSTAL_CAP;
+		RT_TRACE(COMP_EPROM, "CrystalCap = %d\n", priv->EEPROMCrystalCap);
+		/* get per-channel Tx power level */
+		if (bLoad_From_EEPOM) {
+			ret = eprom_read(dev, (EEPROM_TX_PW_INDEX_VER >> 1));
+			if (ret < 0)
+				return ret;
+			priv->EEPROM_Def_Ver = ((u16)ret & 0xff00) >> 8;
+		} else
+			priv->EEPROM_Def_Ver = 1;
+		RT_TRACE(COMP_EPROM, "EEPROM_DEF_VER:%d\n", priv->EEPROM_Def_Ver);
+		if (priv->EEPROM_Def_Ver == 0) { /* old eeprom definition */
+			if (bLoad_From_EEPOM) {
+				ret = eprom_read(dev, (EEPROM_TX_PW_INDEX_CCK >> 1));
+				if (ret < 0)
+					return ret;
+				priv->EEPROMTxPowerLevelCCK = ((u16)ret & 0xff00) >> 8;
+			} else
+				priv->EEPROMTxPowerLevelCCK = 0x10;
+			RT_TRACE(COMP_EPROM, "CCK Tx Power Levl: 0x%02x\n", priv->EEPROMTxPowerLevelCCK);
+			for (i = 0; i < 3; i++) {
+				if (bLoad_From_EEPOM) {
+					ret = eprom_read(dev, (EEPROM_TX_PW_INDEX_OFDM_24G + i) >> 1);
+					if (ret < 0)
+						return ret;
+					if (((EEPROM_TX_PW_INDEX_OFDM_24G + i) % 2) == 0)
+						tmpValue = (u16)ret & 0x00ff;
+					else
+						tmpValue = ((u16)ret & 0xff00) >> 8;
+				} else {
+					tmpValue = 0x10;
+				}
+				priv->EEPROMTxPowerLevelOFDM24G[i] = (u8)tmpValue;
+				RT_TRACE(COMP_EPROM, "OFDM 2.4G Tx Power Level, Index %d = 0x%02x\n", i, priv->EEPROMTxPowerLevelCCK);
+			}
+		} else if (priv->EEPROM_Def_Ver == 1) {
+			if (bLoad_From_EEPOM) {
+				ret = eprom_read(dev, EEPROM_TX_PW_INDEX_CCK_V1 >> 1);
+				if (ret < 0)
+					return ret;
+				tmpValue = ((u16)ret & 0xff00) >> 8;
+			} else {
+				tmpValue = 0x10;
+			}
+			priv->EEPROMTxPowerLevelCCK_V1[0] = (u8)tmpValue;
+
+			if (bLoad_From_EEPOM) {
+				ret = eprom_read(dev, (EEPROM_TX_PW_INDEX_CCK_V1 + 2) >> 1);
+				if (ret < 0)
+					return ret;
+				tmpValue = (u16)ret;
+			} else
+				tmpValue = 0x1010;
+			*((u16 *)(&priv->EEPROMTxPowerLevelCCK_V1[1])) = tmpValue;
+			if (bLoad_From_EEPOM)
+				tmpValue = eprom_read(dev,
+					EEPROM_TX_PW_INDEX_OFDM_24G_V1 >> 1);
+			else
+				tmpValue = 0x1010;
+			*((u16 *)(&priv->EEPROMTxPowerLevelOFDM24G[0])) = tmpValue;
+			if (bLoad_From_EEPOM)
+				tmpValue = eprom_read(dev, (EEPROM_TX_PW_INDEX_OFDM_24G_V1 + 2) >> 1);
+			else
+				tmpValue = 0x10;
+			priv->EEPROMTxPowerLevelOFDM24G[2] = (u8)tmpValue;
+		} /* endif EEPROM_Def_Ver == 1 */
+
+		/* update HAL variables */
+		for (i = 0; i < 14; i++) {
+			if (i <= 3)
+				priv->TxPowerLevelOFDM24G[i] = priv->EEPROMTxPowerLevelOFDM24G[0];
+			else if (i >= 4 && i <= 9)
+				priv->TxPowerLevelOFDM24G[i] = priv->EEPROMTxPowerLevelOFDM24G[1];
+			else
+				priv->TxPowerLevelOFDM24G[i] = priv->EEPROMTxPowerLevelOFDM24G[2];
+		}
+
+		for (i = 0; i < 14; i++) {
+			if (priv->EEPROM_Def_Ver == 0) {
+				if (i <= 3)
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelOFDM24G[0] + (priv->EEPROMTxPowerLevelCCK - priv->EEPROMTxPowerLevelOFDM24G[1]);
+				else if (i >= 4 && i <= 9)
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelCCK;
+				else
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelOFDM24G[2] + (priv->EEPROMTxPowerLevelCCK - priv->EEPROMTxPowerLevelOFDM24G[1]);
+			} else if (priv->EEPROM_Def_Ver == 1) {
+				if (i <= 3)
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelCCK_V1[0];
+				else if (i >= 4 && i <= 9)
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelCCK_V1[1];
+				else
+					priv->TxPowerLevelCCK[i] = priv->EEPROMTxPowerLevelCCK_V1[2];
+			}
+		}
+		priv->TxPowerDiff = priv->EEPROMPwDiff;
+		/* Antenna B gain offset to antenna A, bit0~3 */
+		priv->AntennaTxPwDiff[0] = (priv->EEPROMTxPowerDiff & 0xf);
+		/* Antenna C gain offset to antenna A, bit4~7 */
+		priv->AntennaTxPwDiff[1] =
+			(priv->EEPROMTxPowerDiff & 0xf0) >> 4;
+		/* CrystalCap, bit12~15 */
+		priv->CrystalCap = priv->EEPROMCrystalCap;
+		/* ThermalMeter, bit0~3 for RFIC1, bit4~7 for RFIC2
+		 * 92U does not enable TX power tracking.
+		 */
+		priv->ThermalMeter[0] = priv->EEPROMThermalMeter;
+	} /* end if VersionID == VERSION_819XU_A */
+
+	/* for dlink led */
+	switch (priv->eeprom_CustomerID) {
+	case EEPROM_CID_RUNTOP:
+		priv->CustomerID = RT_CID_819x_RUNTOP;
+		break;
+
+	case EEPROM_CID_DLINK:
+		priv->CustomerID = RT_CID_DLINK;
+		break;
+
+	default:
+		priv->CustomerID = RT_CID_DEFAULT;
+		break;
+	}
+
+	switch (priv->CustomerID) {
+	case RT_CID_819x_RUNTOP:
+		priv->LedStrategy = SW_LED_MODE2;
+		break;
+
+	case RT_CID_DLINK:
+		priv->LedStrategy = SW_LED_MODE4;
+		break;
+
+	default:
+		priv->LedStrategy = SW_LED_MODE0;
+		break;
+	}
+
+	if (priv->rf_type == RF_1T2R)
+		RT_TRACE(COMP_EPROM, "\n1T2R config\n");
+	else
+		RT_TRACE(COMP_EPROM, "\n2T4R config\n");
+
+	/* We can only know RF type in the function. So we have to init
+	 * DIG RATR table again.
+	 */
+	init_rate_adaptive(dev);
+
+	RT_TRACE(COMP_EPROM, "<===========%s()\n", __func__);
+
+	return 0;
+}
+
+static short rtl8192_get_channel_map(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->ChannelPlan > COUNTRY_CODE_GLOBAL_DOMAIN) {
+		netdev_err(dev,
+			   "rtl8180_init: Error channel plan! Set to default.\n");
+		priv->ChannelPlan = 0;
+	}
+	RT_TRACE(COMP_INIT, "Channel plan is %d\n", priv->ChannelPlan);
+
+	rtl819x_set_channel_map(priv->ChannelPlan, priv);
+	return 0;
+}
+
+static short rtl8192_init(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int err;
+
+	memset(&(priv->stats), 0, sizeof(struct Stats));
+	memset(priv->txqueue_to_outpipemap, 0, 9);
+#ifdef PIPE12
+	{
+		int i = 0;
+		static const u8 queuetopipe[] = {3, 2, 1, 0, 4, 8, 7, 6, 5};
+
+		memcpy(priv->txqueue_to_outpipemap, queuetopipe, 9);
+	}
+#else
+	{
+		static const u8 queuetopipe[] = {3, 2, 1, 0, 4, 4, 0, 4, 4};
+
+		memcpy(priv->txqueue_to_outpipemap, queuetopipe, 9);
+	}
+#endif
+	err = rtl8192_init_priv_variable(dev);
+	if (err)
+		return err;
+
+	rtl8192_init_priv_lock(priv);
+	rtl8192_init_priv_task(dev);
+	rtl8192_get_eeprom_size(dev);
+	err = rtl8192_read_eeprom_info(dev);
+	if (err) {
+		DMESG("Reading EEPROM info failed");
+		return err;
+	}
+	rtl8192_get_channel_map(dev);
+	init_hal_dm(dev);
+	timer_setup(&priv->watch_dog_timer, watch_dog_timer_callback, 0);
+	if (rtl8192_usb_initendpoints(dev) != 0) {
+		DMESG("Endopoints initialization failed");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *function:  This function actually only set RRSR, RATR and BW_OPMODE registers
+ *	     not to do all the hw config as its name says
+ *   input:  net_device dev
+ *  output:  none
+ *  return:  none
+ *  notice:  This part need to modified according to the rate set we filtered
+ * ****************************************************************************/
+static void rtl8192_hwconfig(struct net_device *dev)
+{
+	u32 regRATR = 0, regRRSR = 0;
+	u8 regBwOpMode = 0, regTmp = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 ratr_value = 0;
+
+	/* Set RRSR, RATR, and BW_OPMODE registers */
+	switch (priv->ieee80211->mode) {
+	case WIRELESS_MODE_B:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK;
+		regRRSR = RATE_ALL_CCK;
+		break;
+	case WIRELESS_MODE_A:
+		regBwOpMode = BW_OPMODE_5G | BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_OFDM_AG;
+		regRRSR = RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_G:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_AUTO:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+			  RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_N_24G:
+		/* It support CCK rate by default. CCK rate will be filtered
+		 * out only when associated AP does not support it.
+		 */
+		regBwOpMode = BW_OPMODE_20MHZ;
+		regRATR = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+			  RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+		break;
+	case WIRELESS_MODE_N_5G:
+		regBwOpMode = BW_OPMODE_5G;
+		regRATR = RATE_ALL_OFDM_AG | RATE_ALL_OFDM_1SS |
+			  RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_OFDM_AG;
+		break;
+	}
+
+	write_nic_byte(dev, BW_OPMODE, regBwOpMode);
+	ratr_value = regRATR;
+	if (priv->rf_type == RF_1T2R)
+		ratr_value &= ~(RATE_ALL_OFDM_2SS);
+	write_nic_dword(dev, RATR0, ratr_value);
+	write_nic_byte(dev, UFWP, 1);
+	read_nic_byte(dev, 0x313, &regTmp);
+	regRRSR = ((regTmp) << 24) | (regRRSR & 0x00ffffff);
+	write_nic_dword(dev, RRSR, regRRSR);
+
+	/* Set Retry Limit here */
+	write_nic_word(dev, RETRY_LIMIT,
+		       priv->ShortRetryLimit << RETRY_LIMIT_SHORT_SHIFT |
+		       priv->LongRetryLimit << RETRY_LIMIT_LONG_SHIFT);
+	/* Set Contention Window here */
+
+	/* Set Tx AGC */
+
+	/* Set Tx Antenna including Feedback control */
+
+	/* Set Auto Rate fallback control */
+}
+
+/* InitializeAdapter and PhyCfg */
+static bool rtl8192_adapter_start(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 dwRegRead = 0;
+	bool init_status = true;
+	u8 SECR_value = 0x0;
+	u8 tmp;
+
+	RT_TRACE(COMP_INIT, "====>%s()\n", __func__);
+	priv->Rf_Mode = RF_OP_By_SW_3wire;
+	/* for ASIC power on sequence */
+	write_nic_byte_E(dev, 0x5f, 0x80);
+	mdelay(50);
+	write_nic_byte_E(dev, 0x5f, 0xf0);
+	write_nic_byte_E(dev, 0x5d, 0x00);
+	write_nic_byte_E(dev, 0x5e, 0x80);
+	write_nic_byte(dev, 0x17, 0x37);
+	mdelay(10);
+	priv->pFirmware->firmware_status = FW_STATUS_0_INIT;
+	/* config CPUReset Register */
+	/* Firmware Reset or not? */
+	read_nic_dword(dev, CPU_GEN, &dwRegRead);
+	dwRegRead |= CPU_GEN_SYSTEM_RESET; /* do nothing here? */
+
+	write_nic_dword(dev, CPU_GEN, dwRegRead);
+	/* config BB. */
+	rtl8192_BBConfig(dev);
+
+	/* Loopback mode or not */
+	priv->LoopbackMode = RTL819xU_NO_LOOPBACK;
+
+	read_nic_dword(dev, CPU_GEN, &dwRegRead);
+	if (priv->LoopbackMode == RTL819xU_NO_LOOPBACK)
+		dwRegRead = (dwRegRead & CPU_GEN_NO_LOOPBACK_MSK) |
+			    CPU_GEN_NO_LOOPBACK_SET;
+	else if (priv->LoopbackMode == RTL819xU_MAC_LOOPBACK)
+		dwRegRead |= CPU_CCK_LOOPBACK;
+	else
+		RT_TRACE(COMP_ERR,
+			 "Serious error in %s(): wrong loopback mode setting(%d)\n",
+			 __func__,  priv->LoopbackMode);
+
+	write_nic_dword(dev, CPU_GEN, dwRegRead);
+
+	/* after reset cpu, we need wait for a seconds to write in register. */
+	udelay(500);
+
+	/* add for new bitfile:usb suspend reset pin set to 1. Do we need? */
+	read_nic_byte_E(dev, 0x5f, &tmp);
+	write_nic_byte_E(dev, 0x5f, tmp | 0x20);
+
+	/* Set Hardware */
+	rtl8192_hwconfig(dev);
+
+	/* turn on Tx/Rx */
+	write_nic_byte(dev, CMDR, CR_RE | CR_TE);
+
+	/* set IDR0 here */
+	write_nic_dword(dev, MAC0, ((u32 *)dev->dev_addr)[0]);
+	write_nic_word(dev, MAC4, ((u16 *)(dev->dev_addr + 4))[0]);
+
+	/* set RCR */
+	write_nic_dword(dev, RCR, priv->ReceiveConfig);
+
+	/* Initialize Number of Reserved Pages in Firmware Queue */
+	write_nic_dword(dev, RQPN1,
+		NUM_OF_PAGE_IN_FW_QUEUE_BK << RSVD_FW_QUEUE_PAGE_BK_SHIFT |
+		NUM_OF_PAGE_IN_FW_QUEUE_BE << RSVD_FW_QUEUE_PAGE_BE_SHIFT |
+		NUM_OF_PAGE_IN_FW_QUEUE_VI << RSVD_FW_QUEUE_PAGE_VI_SHIFT |
+		NUM_OF_PAGE_IN_FW_QUEUE_VO << RSVD_FW_QUEUE_PAGE_VO_SHIFT);
+	write_nic_dword(dev, RQPN2,
+		NUM_OF_PAGE_IN_FW_QUEUE_MGNT << RSVD_FW_QUEUE_PAGE_MGNT_SHIFT |
+		NUM_OF_PAGE_IN_FW_QUEUE_CMD << RSVD_FW_QUEUE_PAGE_CMD_SHIFT);
+	write_nic_dword(dev, RQPN3,
+		APPLIED_RESERVED_QUEUE_IN_FW |
+		NUM_OF_PAGE_IN_FW_QUEUE_BCN << RSVD_FW_QUEUE_PAGE_BCN_SHIFT);
+	write_nic_dword(dev, RATR0 + 4 * 7, (RATE_ALL_OFDM_AG | RATE_ALL_CCK));
+
+	/* Set AckTimeout */
+	/* TODO: (it value is only for FPGA version). need to be changed!! */
+	write_nic_byte(dev, ACK_TIMEOUT, 0x30);
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET)
+		rtl8192_SetWirelessMode(dev, priv->ieee80211->mode);
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		CamResetAllEntry(dev);
+		SECR_value |= SCR_TxEncEnable;
+		SECR_value |= SCR_RxDecEnable;
+		SECR_value |= SCR_NoSKMC;
+		write_nic_byte(dev, SECR, SECR_value);
+	}
+
+	/* Beacon related */
+	write_nic_word(dev, ATIMWND, 2);
+	write_nic_word(dev, BCN_INTERVAL, 100);
+
+#define DEFAULT_EDCA 0x005e4332
+	{
+		int i;
+
+		for (i = 0; i < QOS_QUEUE_NUM; i++)
+			write_nic_dword(dev, WDCAPARA_ADD[i], DEFAULT_EDCA);
+	}
+
+	rtl8192_phy_configmac(dev);
+
+	if (priv->card_8192_version == VERSION_819XU_A) {
+		rtl8192_phy_getTxPower(dev);
+		rtl8192_phy_setTxPower(dev, priv->chan);
+	}
+
+	/* Firmware download */
+	init_status = init_firmware(dev);
+	if (!init_status) {
+		RT_TRACE(COMP_ERR, "ERR!!! %s(): Firmware download is failed\n",
+			 __func__);
+		return init_status;
+	}
+	RT_TRACE(COMP_INIT, "%s():after firmware download\n", __func__);
+
+	/* config RF. */
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		rtl8192_phy_RFConfig(dev);
+		RT_TRACE(COMP_INIT, "%s():after phy RF config\n", __func__);
+	}
+
+	if (priv->ieee80211->FwRWRF)
+		/* We can force firmware to do RF-R/W */
+		priv->Rf_Mode = RF_OP_By_FW;
+	else
+		priv->Rf_Mode = RF_OP_By_SW_3wire;
+
+	rtl8192_phy_updateInitGain(dev);
+	/*--set CCK and OFDM Block "ON"--*/
+	rtl8192_setBBreg(dev, rFPGA0_RFMOD, bCCKEn, 0x1);
+	rtl8192_setBBreg(dev, rFPGA0_RFMOD, bOFDMEn, 0x1);
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+		/* if D or C cut */
+		u8 tmpvalue;
+
+		read_nic_byte(dev, 0x301, &tmpvalue);
+		if (tmpvalue == 0x03) {
+			priv->bDcut = true;
+			RT_TRACE(COMP_POWER_TRACKING, "D-cut\n");
+		} else {
+			priv->bDcut = false;
+			RT_TRACE(COMP_POWER_TRACKING, "C-cut\n");
+		}
+		dm_initialize_txpower_tracking(dev);
+
+		if (priv->bDcut) {
+			u32 i, TempCCk;
+			u32 tmpRegA = rtl8192_QueryBBReg(dev,
+							 rOFDM0_XATxIQImbalance,
+							 bMaskDWord);
+
+			for (i = 0; i < TxBBGainTableLength; i++) {
+				if (tmpRegA == priv->txbbgain_table[i].txbbgain_value) {
+					priv->rfa_txpowertrackingindex = (u8)i;
+					priv->rfa_txpowertrackingindex_real =
+						(u8)i;
+					priv->rfa_txpowertracking_default =
+						priv->rfa_txpowertrackingindex;
+					break;
+				}
+			}
+
+			TempCCk = rtl8192_QueryBBReg(dev,
+						     rCCK0_TxFilter1,
+						     bMaskByte2);
+
+			for (i = 0; i < CCKTxBBGainTableLength; i++) {
+				if (TempCCk == priv->cck_txbbgain_table[i].ccktxbb_valuearray[0]) {
+					priv->cck_present_attenuation_20Mdefault = (u8)i;
+					break;
+				}
+			}
+			priv->cck_present_attenuation_40Mdefault = 0;
+			priv->cck_present_attenuation_difference = 0;
+			priv->cck_present_attenuation =
+				priv->cck_present_attenuation_20Mdefault;
+		}
+	}
+	write_nic_byte(dev, 0x87, 0x0);
+
+	return init_status;
+}
+
+/* this configures registers for beacon tx and enables it via
+ * rtl8192_beacon_tx_enable(). rtl8192_beacon_tx_disable() might
+ * be used to stop beacon transmission
+ */
+/***************************************************************************
+ *   -------------------------------NET STUFF---------------------------
+ ***************************************************************************/
+
+static struct net_device_stats *rtl8192_stats(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return &priv->ieee80211->stats;
+}
+
+static bool HalTxCheckStuck819xUsb(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u16		RegTxCounter;
+	bool		bStuck = false;
+
+	read_nic_word(dev, 0x128, &RegTxCounter);
+	RT_TRACE(COMP_RESET,
+		 "%s():RegTxCounter is %d,TxCounter is %d\n", __func__,
+		 RegTxCounter, priv->TxCounter);
+	if (priv->TxCounter == RegTxCounter)
+		bStuck = true;
+
+	priv->TxCounter = RegTxCounter;
+
+	return bStuck;
+}
+
+/*
+ *	<Assumption: RT_TX_SPINLOCK is acquired.>
+ *	First added: 2006.11.19 by emily
+ */
+static RESET_TYPE TxCheckStuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8			QueueID;
+	bool			bCheckFwTxCnt = false;
+
+	/* Decide such threshold according to current power save mode */
+
+	for (QueueID = 0; QueueID <= BEACON_QUEUE; QueueID++) {
+		if (QueueID == TXCMD_QUEUE)
+			continue;
+		if ((skb_queue_len(&priv->ieee80211->skb_waitQ[QueueID]) == 0)  && (skb_queue_len(&priv->ieee80211->skb_aggQ[QueueID]) == 0))
+			continue;
+
+		bCheckFwTxCnt = true;
+	}
+	if (bCheckFwTxCnt) {
+		if (HalTxCheckStuck819xUsb(dev)) {
+			RT_TRACE(COMP_RESET,
+				 "%s: Fw indicates no Tx condition!\n",
+				 __func__);
+			return RESET_TYPE_SILENT;
+		}
+	}
+	return RESET_TYPE_NORESET;
+}
+
+static bool HalRxCheckStuck819xUsb(struct net_device *dev)
+{
+	u16	RegRxCounter;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	bool bStuck = false;
+	static u8	rx_chk_cnt;
+
+	read_nic_word(dev, 0x130, &RegRxCounter);
+	RT_TRACE(COMP_RESET,
+		 "%s(): RegRxCounter is %d,RxCounter is %d\n", __func__,
+		 RegRxCounter, priv->RxCounter);
+	/* If rssi is small, we should check rx for long time because of bad rx.
+	 * or maybe it will continuous silent reset every 2 seconds.
+	 */
+	rx_chk_cnt++;
+	if (priv->undecorated_smoothed_pwdb >= (RATE_ADAPTIVE_TH_HIGH + 5)) {
+		rx_chk_cnt = 0;	/* high rssi, check rx stuck right now. */
+	} else if (priv->undecorated_smoothed_pwdb < (RATE_ADAPTIVE_TH_HIGH + 5) &&
+		   ((priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20 && priv->undecorated_smoothed_pwdb >= RATE_ADAPTIVE_TH_LOW_40M) ||
+		    (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 && priv->undecorated_smoothed_pwdb >= RATE_ADAPTIVE_TH_LOW_20M))) {
+		if (rx_chk_cnt < 2)
+			return bStuck;
+
+		rx_chk_cnt = 0;
+	} else if (((priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20 && priv->undecorated_smoothed_pwdb < RATE_ADAPTIVE_TH_LOW_40M) ||
+		    (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 && priv->undecorated_smoothed_pwdb < RATE_ADAPTIVE_TH_LOW_20M)) &&
+		     priv->undecorated_smoothed_pwdb >= VERY_LOW_RSSI) {
+		if (rx_chk_cnt < 4)
+			return bStuck;
+
+		rx_chk_cnt = 0;
+	} else {
+		if (rx_chk_cnt < 8)
+			return bStuck;
+
+		rx_chk_cnt = 0;
+	}
+
+	if (priv->RxCounter == RegRxCounter)
+		bStuck = true;
+
+	priv->RxCounter = RegRxCounter;
+
+	return bStuck;
+}
+
+static RESET_TYPE RxCheckStuck(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	bool        bRxCheck = false;
+
+	if (priv->IrpPendingCount > 1)
+		bRxCheck = true;
+
+	if (bRxCheck) {
+		if (HalRxCheckStuck819xUsb(dev)) {
+			RT_TRACE(COMP_RESET, "RxStuck Condition\n");
+			return RESET_TYPE_SILENT;
+		}
+	}
+	return RESET_TYPE_NORESET;
+}
+
+/*
+ * This function is called by Checkforhang to check whether we should
+ * ask OS to reset driver
+ *
+ * Note:NIC with USB interface sholud not call this function because we
+ * cannot scan descriptor to judge whether there is tx stuck.
+ * Note: This function may be required to be rewrite for Vista OS.
+ * <<<Assumption: Tx spinlock has been acquired >>>
+ *
+ * 8185 and 8185b does not implement this function.
+ */
+static RESET_TYPE rtl819x_ifcheck_resetornot(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	RESET_TYPE	TxResetType = RESET_TYPE_NORESET;
+	RESET_TYPE	RxResetType = RESET_TYPE_NORESET;
+	RT_RF_POWER_STATE	rfState;
+
+	rfState = priv->ieee80211->eRFPowerState;
+
+	TxResetType = TxCheckStuck(dev);
+	if (rfState != eRfOff ||
+	    (priv->ieee80211->iw_mode != IW_MODE_ADHOC)) {
+		/* If driver is in the status of firmware download failure,
+		 * driver skips RF initialization and RF is in turned off
+		 * state. Driver should check whether Rx stuck and do silent
+		 * reset. And if driver is in firmware download failure status,
+		 * driver should initialize RF in the following silent reset
+		 * procedure
+		 *
+		 * Driver should not check RX stuck in IBSS mode because it is
+		 * required to set Check BSSID in order to send beacon,
+		 * however, if check BSSID is set, STA cannot hear any packet
+		 * at all.
+		 */
+		RxResetType = RxCheckStuck(dev);
+	}
+	if (TxResetType == RESET_TYPE_NORMAL ||
+	    RxResetType == RESET_TYPE_NORMAL) {
+		return RESET_TYPE_NORMAL;
+	} else if (TxResetType == RESET_TYPE_SILENT ||
+		   RxResetType == RESET_TYPE_SILENT) {
+		RT_TRACE(COMP_RESET, "%s():silent reset\n", __func__);
+		return RESET_TYPE_SILENT;
+	} else {
+		return RESET_TYPE_NORESET;
+	}
+}
+
+static void rtl8192_cancel_deferred_work(struct r8192_priv *priv);
+static int _rtl8192_up(struct net_device *dev);
+static int rtl8192_close(struct net_device *dev);
+
+static void CamRestoreAllEntry(struct net_device *dev)
+{
+	u8 EntryId = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8	*MacAddr = priv->ieee80211->current_network.bssid;
+
+	static u8	CAM_CONST_ADDR[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03} };
+	static u8	CAM_CONST_BROAD[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	RT_TRACE(COMP_SEC, "%s:\n", __func__);
+
+	if ((priv->ieee80211->pairwise_key_type == KEY_TYPE_WEP40) ||
+	    (priv->ieee80211->pairwise_key_type == KEY_TYPE_WEP104)) {
+		for (EntryId = 0; EntryId < 4; EntryId++) {
+			MacAddr = CAM_CONST_ADDR[EntryId];
+			setKey(dev, EntryId, EntryId,
+			       priv->ieee80211->pairwise_key_type,
+			       MacAddr, 0, NULL);
+		}
+
+	} else if (priv->ieee80211->pairwise_key_type == KEY_TYPE_TKIP) {
+		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			setKey(dev, 4, 0, priv->ieee80211->pairwise_key_type,
+			       (const u8 *)dev->dev_addr, 0, NULL);
+		else
+			setKey(dev, 4, 0, priv->ieee80211->pairwise_key_type,
+			       MacAddr, 0, NULL);
+	} else if (priv->ieee80211->pairwise_key_type == KEY_TYPE_CCMP) {
+		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			setKey(dev, 4, 0, priv->ieee80211->pairwise_key_type,
+			       (const u8 *)dev->dev_addr, 0, NULL);
+		else
+			setKey(dev, 4, 0, priv->ieee80211->pairwise_key_type,
+			       MacAddr, 0, NULL);
+	}
+
+	if (priv->ieee80211->group_key_type == KEY_TYPE_TKIP) {
+		MacAddr = CAM_CONST_BROAD;
+		for (EntryId = 1; EntryId < 4; EntryId++) {
+			setKey(dev, EntryId, EntryId,
+			       priv->ieee80211->group_key_type,
+			       MacAddr, 0, NULL);
+		}
+		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			setKey(dev, 0, 0, priv->ieee80211->group_key_type,
+			       CAM_CONST_ADDR[0], 0, NULL);
+	} else if (priv->ieee80211->group_key_type == KEY_TYPE_CCMP) {
+		MacAddr = CAM_CONST_BROAD;
+		for (EntryId = 1; EntryId < 4; EntryId++) {
+			setKey(dev, EntryId, EntryId,
+			       priv->ieee80211->group_key_type,
+			       MacAddr, 0, NULL);
+		}
+
+		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			setKey(dev, 0, 0, priv->ieee80211->group_key_type,
+			       CAM_CONST_ADDR[0], 0, NULL);
+	}
+}
+
+/* This function is used to fix Tx/Rx stop bug temporarily.
+ * This function will do "system reset" to NIC when Tx or Rx is stuck.
+ * The method checking Tx/Rx stuck of this function is supported by FW,
+ * which reports Tx and Rx counter to register 0x128 and 0x130.
+ */
+static void rtl819x_ifsilentreset(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8	reset_times = 0;
+	int reset_status = 0;
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	/* If we need to check CCK stop, please uncomment this line. */
+	/* bStuck = Adapter->HalFunc.CheckHWStopHandler(Adapter); */
+
+	if (priv->ResetProgress == RESET_TYPE_NORESET) {
+RESET_START:
+
+		RT_TRACE(COMP_RESET, "=========>Reset progress!!\n");
+
+		/* Set the variable for reset. */
+		priv->ResetProgress = RESET_TYPE_SILENT;
+		mutex_lock(&priv->wx_mutex);
+		if (priv->up == 0) {
+			RT_TRACE(COMP_ERR,
+				 "%s():the driver is not up! return\n",
+				 __func__);
+			mutex_unlock(&priv->wx_mutex);
+			return;
+		}
+		priv->up = 0;
+		RT_TRACE(COMP_RESET,
+			 "%s():======>start to down the driver\n",
+			 __func__);
+
+		rtl8192_rtx_disable(dev);
+		rtl8192_cancel_deferred_work(priv);
+		deinit_hal_dm(dev);
+		del_timer_sync(&priv->watch_dog_timer);
+
+		ieee->sync_scan_hurryup = 1;
+		if (ieee->state == IEEE80211_LINKED) {
+			mutex_lock(&ieee->wx_mutex);
+			netdev_dbg(dev, "ieee->state is IEEE80211_LINKED\n");
+			ieee80211_stop_send_beacons(priv->ieee80211);
+			del_timer_sync(&ieee->associate_timer);
+			cancel_delayed_work(&ieee->associate_retry_wq);
+			ieee80211_stop_scan(ieee);
+			netif_carrier_off(dev);
+			mutex_unlock(&ieee->wx_mutex);
+		} else {
+			netdev_dbg(dev, "ieee->state is NOT LINKED\n");
+			ieee80211_softmac_stop_protocol(priv->ieee80211);
+		}
+		mutex_unlock(&priv->wx_mutex);
+		RT_TRACE(COMP_RESET,
+			 "%s():<==========down process is finished\n",
+			 __func__);
+		RT_TRACE(COMP_RESET,
+			 "%s():===========>start up the driver\n",
+			 __func__);
+		reset_status = _rtl8192_up(dev);
+
+		RT_TRACE(COMP_RESET,
+			 "%s():<===========up process is finished\n",
+			 __func__);
+		if (reset_status == -EAGAIN) {
+			if (reset_times < 3) {
+				reset_times++;
+				goto RESET_START;
+			} else {
+				RT_TRACE(COMP_ERR,
+					 " ERR!!! %s():  Reset Failed!!\n",
+					 __func__);
+			}
+		}
+		ieee->is_silent_reset = 1;
+		EnableHWSecurityConfig8192(dev);
+		if (ieee->state == IEEE80211_LINKED &&
+		    ieee->iw_mode == IW_MODE_INFRA) {
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel);
+
+			queue_work(ieee->wq, &ieee->associate_complete_wq);
+
+		} else if (ieee->state == IEEE80211_LINKED &&
+			   ieee->iw_mode == IW_MODE_ADHOC) {
+			ieee->set_chan(ieee->dev,
+				       ieee->current_network.channel);
+			ieee->link_change(ieee->dev);
+
+			ieee80211_start_send_beacons(ieee);
+
+			if (ieee->data_hard_resume)
+				ieee->data_hard_resume(ieee->dev);
+			netif_carrier_on(ieee->dev);
+		}
+
+		CamRestoreAllEntry(dev);
+
+		priv->ResetProgress = RESET_TYPE_NORESET;
+		priv->reset_count++;
+
+		priv->bForcedSilentReset = false;
+		priv->bResetInProgress = false;
+
+		/* For test --> force write UFWP. */
+		write_nic_byte(dev, UFWP, 1);
+		RT_TRACE(COMP_RESET,
+			 "Reset finished!! ====>[%d]\n",
+			 priv->reset_count);
+	}
+}
+
+static void rtl819x_update_rxcounts(struct r8192_priv *priv, u32 *TotalRxBcnNum,
+			     u32 *TotalRxDataNum)
+{
+	u16			SlotIndex;
+	u16			i;
+
+	*TotalRxBcnNum = 0;
+	*TotalRxDataNum = 0;
+
+	SlotIndex = (priv->ieee80211->LinkDetectInfo.SlotIndex++) %
+		    (priv->ieee80211->LinkDetectInfo.SlotNum);
+	priv->ieee80211->LinkDetectInfo.RxBcnNum[SlotIndex] =
+		priv->ieee80211->LinkDetectInfo.NumRecvBcnInPeriod;
+	priv->ieee80211->LinkDetectInfo.RxDataNum[SlotIndex] =
+		priv->ieee80211->LinkDetectInfo.NumRecvDataInPeriod;
+	for (i = 0; i < priv->ieee80211->LinkDetectInfo.SlotNum; i++) {
+		*TotalRxBcnNum += priv->ieee80211->LinkDetectInfo.RxBcnNum[i];
+		*TotalRxDataNum += priv->ieee80211->LinkDetectInfo.RxDataNum[i];
+	}
+}
+
+static void rtl819x_watchdog_wqcallback(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct r8192_priv *priv = container_of(dwork,
+					       struct r8192_priv, watch_dog_wq);
+	struct net_device *dev = priv->ieee80211->dev;
+	struct ieee80211_device *ieee = priv->ieee80211;
+	RESET_TYPE	ResetType = RESET_TYPE_NORESET;
+	static u8	check_reset_cnt;
+	bool bBusyTraffic = false;
+	u32	TotalRxBcnNum = 0;
+	u32	TotalRxDataNum = 0;
+
+	if (!priv->up)
+		return;
+	hal_dm_watchdog(dev);
+
+	/* to get busy traffic condition */
+	if (ieee->state == IEEE80211_LINKED) {
+		if (ieee->LinkDetectInfo.NumRxOkInPeriod > 666 ||
+		    ieee->LinkDetectInfo.NumTxOkInPeriod > 666) {
+			bBusyTraffic = true;
+		}
+		ieee->LinkDetectInfo.NumRxOkInPeriod = 0;
+		ieee->LinkDetectInfo.NumTxOkInPeriod = 0;
+		ieee->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+	}
+	/* for AP roaming */
+	if (priv->ieee80211->state == IEEE80211_LINKED &&
+	    priv->ieee80211->iw_mode == IW_MODE_INFRA) {
+		rtl819x_update_rxcounts(priv, &TotalRxBcnNum, &TotalRxDataNum);
+		if ((TotalRxBcnNum + TotalRxDataNum) == 0) {
+#ifdef TODO
+			if (rfState == eRfOff)
+				RT_TRACE(COMP_ERR, "========>%s()\n", __func__);
+#endif
+			netdev_dbg(dev,
+				   "===>%s(): AP is power off, connect another one\n",
+				   __func__);
+			priv->ieee80211->state = IEEE80211_ASSOCIATING;
+			notify_wx_assoc_event(priv->ieee80211);
+			RemovePeerTS(priv->ieee80211,
+				     priv->ieee80211->current_network.bssid);
+			priv->ieee80211->link_change(dev);
+			queue_work(priv->ieee80211->wq,
+				   &priv->ieee80211->associate_procedure_wq);
+		}
+	}
+	priv->ieee80211->LinkDetectInfo.NumRecvBcnInPeriod = 0;
+	priv->ieee80211->LinkDetectInfo.NumRecvDataInPeriod = 0;
+	/* check if reset the driver */
+	if (check_reset_cnt++ >= 3) {
+		ResetType = rtl819x_ifcheck_resetornot(dev);
+		check_reset_cnt = 3;
+	}
+	/* This is control by OID set in Pomelo */
+	if ((priv->force_reset) || (priv->ResetProgress == RESET_TYPE_NORESET &&
+	    (priv->bForcedSilentReset ||
+	    (!priv->bDisableNormalResetCheck && ResetType == RESET_TYPE_SILENT)))) {
+		RT_TRACE(COMP_RESET,
+			 "%s():priv->force_reset is %d,priv->ResetProgress is %d, priv->bForcedSilentReset is %d,priv->bDisableNormalResetCheck is %d,ResetType is %d\n",
+			 __func__, priv->force_reset, priv->ResetProgress,
+			 priv->bForcedSilentReset,
+			 priv->bDisableNormalResetCheck, ResetType);
+		rtl819x_ifsilentreset(dev);
+	}
+	priv->force_reset = false;
+	priv->bForcedSilentReset = false;
+	priv->bResetInProgress = false;
+	RT_TRACE(COMP_TRACE, " <==RtUsbCheckForHangWorkItemCallback()\n");
+}
+
+static void watch_dog_timer_callback(struct timer_list *t)
+{
+	struct r8192_priv *priv = from_timer(priv, t, watch_dog_timer);
+
+	schedule_delayed_work(&priv->watch_dog_wq, 0);
+	mod_timer(&priv->watch_dog_timer,
+		  jiffies + msecs_to_jiffies(IEEE80211_WATCH_DOG_TIME));
+}
+
+static int _rtl8192_up(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int init_status = 0;
+
+	priv->up = 1;
+	priv->ieee80211->ieee_up = 1;
+	RT_TRACE(COMP_INIT, "Bringing up iface");
+	init_status = rtl8192_adapter_start(dev);
+	if (!init_status) {
+		RT_TRACE(COMP_ERR, "ERR!!! %s(): initialization failed!\n",
+			 __func__);
+		priv->up = priv->ieee80211->ieee_up = 0;
+		return -EAGAIN;
+	}
+	RT_TRACE(COMP_INIT, "start adapter finished\n");
+	rtl8192_rx_enable(dev);
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		ieee80211_softmac_start_protocol(priv->ieee80211);
+	ieee80211_reset_queue(priv->ieee80211);
+	watch_dog_timer_callback(&priv->watch_dog_timer);
+	if (!netif_queue_stopped(dev))
+		netif_start_queue(dev);
+	else
+		netif_wake_queue(dev);
+
+	return 0;
+}
+
+static int rtl8192_open(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = rtl8192_up(dev);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+int rtl8192_up(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 1)
+		return -1;
+
+	return _rtl8192_up(dev);
+}
+
+static int rtl8192_close(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = rtl8192_down(dev);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+int rtl8192_down(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int i;
+
+	if (priv->up == 0)
+		return -1;
+
+	priv->up = 0;
+	priv->ieee80211->ieee_up = 0;
+	RT_TRACE(COMP_DOWN, "==========>%s()\n", __func__);
+	/* FIXME */
+	if (!netif_queue_stopped(dev))
+		netif_stop_queue(dev);
+
+	rtl8192_rtx_disable(dev);
+
+	/* Tx related queue release */
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_purge(&priv->ieee80211->skb_waitQ[i]);
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_purge(&priv->ieee80211->skb_aggQ[i]);
+
+	for (i = 0; i < MAX_QUEUE_SIZE; i++)
+		skb_queue_purge(&priv->ieee80211->skb_drv_aggQ[i]);
+
+	/* as cancel_delayed_work will del work->timer, so if work is not
+	 * defined as struct delayed_work, it will corrupt
+	 */
+	rtl8192_cancel_deferred_work(priv);
+	deinit_hal_dm(dev);
+	del_timer_sync(&priv->watch_dog_timer);
+
+	ieee80211_softmac_stop_protocol(priv->ieee80211);
+	memset(&priv->ieee80211->current_network, 0,
+	       offsetof(struct ieee80211_network, list));
+	RT_TRACE(COMP_DOWN, "<==========%s()\n", __func__);
+
+	return 0;
+}
+
+void rtl8192_commit(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 0)
+		return;
+	priv->up = 0;
+
+	rtl8192_cancel_deferred_work(priv);
+	del_timer_sync(&priv->watch_dog_timer);
+
+	ieee80211_softmac_stop_protocol(priv->ieee80211);
+
+	rtl8192_rtx_disable(dev);
+	_rtl8192_up(dev);
+}
+
+static void rtl8192_restart(struct work_struct *work)
+{
+	struct r8192_priv *priv = container_of(work, struct r8192_priv,
+					       reset_wq);
+	struct net_device *dev = priv->ieee80211->dev;
+
+	mutex_lock(&priv->wx_mutex);
+
+	rtl8192_commit(dev);
+
+	mutex_unlock(&priv->wx_mutex);
+}
+
+static void r8192_set_multicast(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	short promisc;
+
+	/* FIXME FIXME */
+
+	promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
+
+	if (promisc != priv->promisc)
+
+		priv->promisc = promisc;
+}
+
+static int r8192_set_mac_adr(struct net_device *dev, void *mac)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct sockaddr *addr = mac;
+
+	mutex_lock(&priv->wx_mutex);
+
+	eth_hw_addr_set(dev, addr->sa_data);
+
+	schedule_work(&priv->reset_wq);
+	mutex_unlock(&priv->wx_mutex);
+
+	return 0;
+}
+
+/* based on ipw2200 driver */
+static int rtl8192_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct iwreq *wrq = (struct iwreq *)rq;
+	int ret = -1;
+	struct ieee80211_device *ieee = priv->ieee80211;
+	u32 key[4];
+	u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct iw_point *p = &wrq->u.data;
+	struct ieee_param *ipw = NULL;
+
+	mutex_lock(&priv->wx_mutex);
+
+	if (p->length < sizeof(struct ieee_param) || !p->pointer) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ipw = memdup_user(p->pointer, p->length);
+	if (IS_ERR(ipw)) {
+		ret = PTR_ERR(ipw);
+		goto out;
+	}
+
+	switch (cmd) {
+	case RTL_IOCTL_WPA_SUPPLICANT:
+		/* parse here for HW security */
+		if (ipw->cmd == IEEE_CMD_SET_ENCRYPTION) {
+			if (ipw->u.crypt.set_tx) {
+				if (strcmp(ipw->u.crypt.alg, "CCMP") == 0) {
+					ieee->pairwise_key_type = KEY_TYPE_CCMP;
+				} else if (strcmp(ipw->u.crypt.alg, "TKIP") == 0) {
+					ieee->pairwise_key_type = KEY_TYPE_TKIP;
+				} else if (strcmp(ipw->u.crypt.alg, "WEP") == 0) {
+					if (ipw->u.crypt.key_len == 13)
+						ieee->pairwise_key_type = KEY_TYPE_WEP104;
+					else if (ipw->u.crypt.key_len == 5)
+						ieee->pairwise_key_type = KEY_TYPE_WEP40;
+				} else {
+					ieee->pairwise_key_type = KEY_TYPE_NA;
+				}
+
+				if (ieee->pairwise_key_type) {
+					memcpy((u8 *)key, ipw->u.crypt.key, 16);
+					EnableHWSecurityConfig8192(dev);
+					/* We fill both index entry and 4th
+					 * entry for pairwise key as in IPW
+					 * interface, adhoc will only get here,
+					 * so we need index entry for its
+					 * default key serching!
+					 */
+					setKey(dev, 4, ipw->u.crypt.idx,
+					       ieee->pairwise_key_type,
+					       (u8 *)ieee->ap_mac_addr,
+					       0, key);
+					if (ieee->auth_mode != 2)
+						setKey(dev, ipw->u.crypt.idx,
+						       ipw->u.crypt.idx,
+						       ieee->pairwise_key_type,
+						       (u8 *)ieee->ap_mac_addr,
+						       0, key);
+				}
+			} else {
+				memcpy((u8 *)key, ipw->u.crypt.key, 16);
+				if (strcmp(ipw->u.crypt.alg, "CCMP") == 0) {
+					ieee->group_key_type = KEY_TYPE_CCMP;
+				} else if (strcmp(ipw->u.crypt.alg, "TKIP") == 0) {
+					ieee->group_key_type = KEY_TYPE_TKIP;
+				} else if (strcmp(ipw->u.crypt.alg, "WEP") == 0) {
+					if (ipw->u.crypt.key_len == 13)
+						ieee->group_key_type = KEY_TYPE_WEP104;
+					else if (ipw->u.crypt.key_len == 5)
+						ieee->group_key_type = KEY_TYPE_WEP40;
+				} else {
+					ieee->group_key_type = KEY_TYPE_NA;
+				}
+
+				if (ieee->group_key_type) {
+					setKey(dev, ipw->u.crypt.idx,
+					       /* KeyIndex */
+					       ipw->u.crypt.idx,
+					       /* KeyType */
+					       ieee->group_key_type,
+					       /* MacAddr */
+					       broadcast_addr,
+					       /* DefaultKey */
+					       0,
+					       /* KeyContent */
+					       key);
+				}
+			}
+		}
+		ret = ieee80211_wpa_supplicant_ioctl(priv->ieee80211,
+						     &wrq->u.data);
+		break;
+
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
+	kfree(ipw);
+	ipw = NULL;
+out:
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static u8 HwRateToMRate90(bool bIsHT, u8 rate)
+{
+	u8  ret_rate = 0xff;
+
+	if (!bIsHT) {
+		switch (rate) {
+		case DESC90_RATE1M:
+			ret_rate = MGN_1M;
+			break;
+		case DESC90_RATE2M:
+			ret_rate = MGN_2M;
+			break;
+		case DESC90_RATE5_5M:
+			ret_rate = MGN_5_5M;
+			break;
+		case DESC90_RATE11M:
+			ret_rate = MGN_11M;
+			break;
+		case DESC90_RATE6M:
+			ret_rate = MGN_6M;
+			break;
+		case DESC90_RATE9M:
+			ret_rate = MGN_9M;
+			break;
+		case DESC90_RATE12M:
+			ret_rate = MGN_12M;
+			break;
+		case DESC90_RATE18M:
+			ret_rate = MGN_18M;
+			break;
+		case DESC90_RATE24M:
+			ret_rate = MGN_24M;
+			break;
+		case DESC90_RATE36M:
+			ret_rate = MGN_36M;
+			break;
+		case DESC90_RATE48M:
+			ret_rate = MGN_48M;
+			break;
+		case DESC90_RATE54M:
+			ret_rate = MGN_54M;
+			break;
+
+		default:
+			ret_rate = 0xff;
+			RT_TRACE(COMP_RECV,
+				 "%s: Non supported Rate [%x], bIsHT = %d!!!\n",
+				 __func__, rate, bIsHT);
+			break;
+		}
+
+	} else {
+		switch (rate) {
+		case DESC90_RATEMCS0:
+			ret_rate = MGN_MCS0;
+			break;
+		case DESC90_RATEMCS1:
+			ret_rate = MGN_MCS1;
+			break;
+		case DESC90_RATEMCS2:
+			ret_rate = MGN_MCS2;
+			break;
+		case DESC90_RATEMCS3:
+			ret_rate = MGN_MCS3;
+			break;
+		case DESC90_RATEMCS4:
+			ret_rate = MGN_MCS4;
+			break;
+		case DESC90_RATEMCS5:
+			ret_rate = MGN_MCS5;
+			break;
+		case DESC90_RATEMCS6:
+			ret_rate = MGN_MCS6;
+			break;
+		case DESC90_RATEMCS7:
+			ret_rate = MGN_MCS7;
+			break;
+		case DESC90_RATEMCS8:
+			ret_rate = MGN_MCS8;
+			break;
+		case DESC90_RATEMCS9:
+			ret_rate = MGN_MCS9;
+			break;
+		case DESC90_RATEMCS10:
+			ret_rate = MGN_MCS10;
+			break;
+		case DESC90_RATEMCS11:
+			ret_rate = MGN_MCS11;
+			break;
+		case DESC90_RATEMCS12:
+			ret_rate = MGN_MCS12;
+			break;
+		case DESC90_RATEMCS13:
+			ret_rate = MGN_MCS13;
+			break;
+		case DESC90_RATEMCS14:
+			ret_rate = MGN_MCS14;
+			break;
+		case DESC90_RATEMCS15:
+			ret_rate = MGN_MCS15;
+			break;
+		case DESC90_RATEMCS32:
+			ret_rate = 0x80 | 0x20;
+			break;
+
+		default:
+			ret_rate = 0xff;
+			RT_TRACE(COMP_RECV,
+				 "%s: Non supported Rate [%x], bIsHT = %d!!!\n",
+				 __func__, rate, bIsHT);
+			break;
+		}
+	}
+
+	return ret_rate;
+}
+
+/*
+ * Function:     UpdateRxPktTimeStamp
+ * Overview:     Record the TSF time stamp when receiving a packet
+ *
+ * Input:
+ *       PADAPTER        Adapter
+ *       PRT_RFD         pRfd,
+ *
+ * Output:
+ *       PRT_RFD         pRfd
+ *                               (pRfd->Status.TimeStampHigh is updated)
+ *                               (pRfd->Status.TimeStampLow is updated)
+ * Return:
+ *               None
+ */
+static void UpdateRxPktTimeStamp8190(struct net_device *dev,
+				     struct ieee80211_rx_stats *stats)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+
+	if (stats->bIsAMPDU && !stats->bFirstMPDU) {
+		stats->mac_time[0] = priv->LastRxDescTSFLow;
+		stats->mac_time[1] = priv->LastRxDescTSFHigh;
+	} else {
+		priv->LastRxDescTSFLow = stats->mac_time[0];
+		priv->LastRxDescTSFHigh = stats->mac_time[1];
+	}
+}
+
+/* 0-100 index. */
+static long rtl819x_translate_todbm(u8 signal_strength_index)
+{
+	long	signal_power; /* in dBm. */
+
+	/* Translate to dBm (x=0.5y-95). */
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+
+	return signal_power;
+}
+
+/* We can not declare RSSI/EVM total value of sliding window to
+ * be a local static. Otherwise, it may increase when we return from S3/S4. The
+ * value will be kept in memory or disk. Declare the value in the adaptor
+ * and it will be reinitialized when returned from S3/S4.
+ */
+static void rtl8192_process_phyinfo(struct r8192_priv *priv, u8 *buffer,
+				    struct ieee80211_rx_stats *pprevious_stats,
+				    struct ieee80211_rx_stats *pcurrent_stats)
+{
+	bool bcheck = false;
+	u8	rfpath;
+	u32	nspatial_stream, tmp_val;
+	static u32 slide_rssi_index, slide_rssi_statistics;
+	static u32 slide_evm_index, slide_evm_statistics;
+	static u32 last_rssi, last_evm;
+
+	static u32 slide_beacon_adc_pwdb_index;
+	static u32 slide_beacon_adc_pwdb_statistics;
+	static u32 last_beacon_adc_pwdb;
+
+	struct rtl_80211_hdr_3addr *hdr;
+	u16 sc;
+	unsigned int seq;
+
+	hdr = (struct rtl_80211_hdr_3addr *)buffer;
+	sc = le16_to_cpu(hdr->seq_ctl);
+	seq = WLAN_GET_SEQ_SEQ(sc);
+	/* to record the sequence number */
+	pcurrent_stats->Seq_Num = seq;
+
+	/* Check whether we should take the previous packet into accounting */
+	if (!pprevious_stats->bIsAMPDU) {
+		/* if previous packet is not aggregated packet */
+		bcheck = true;
+	}
+
+	if (slide_rssi_statistics++ >= PHY_RSSI_SLID_WIN_MAX) {
+		slide_rssi_statistics = PHY_RSSI_SLID_WIN_MAX;
+		last_rssi = priv->stats.slide_signal_strength[slide_rssi_index];
+		priv->stats.slide_rssi_total -= last_rssi;
+	}
+	priv->stats.slide_rssi_total += pprevious_stats->SignalStrength;
+
+	priv->stats.slide_signal_strength[slide_rssi_index++] =
+		pprevious_stats->SignalStrength;
+	if (slide_rssi_index >= PHY_RSSI_SLID_WIN_MAX)
+		slide_rssi_index = 0;
+
+	/* <1> Showed on UI for user, in dbm */
+	tmp_val = priv->stats.slide_rssi_total / slide_rssi_statistics;
+	priv->stats.signal_strength = rtl819x_translate_todbm((u8)tmp_val);
+	pcurrent_stats->rssi = priv->stats.signal_strength;
+
+	/* If the previous packet does not match the criteria, neglect it */
+	if (!pprevious_stats->bPacketMatchBSSID) {
+		if (!pprevious_stats->bToSelfBA)
+			return;
+	}
+
+	if (!bcheck)
+		return;
+
+	/* only rtl8190 supported
+	 * rtl8190_process_cck_rxpathsel(priv,pprevious_stats);
+	 */
+
+	/* Check RSSI */
+	priv->stats.num_process_phyinfo++;
+
+	/* record the general signal strength to the sliding window. */
+
+	/* <2> Showed on UI for engineering
+	 * hardware does not provide rssi information for each rf path in CCK
+	 */
+	if (!pprevious_stats->bIsCCK &&
+	    (pprevious_stats->bPacketToSelf || pprevious_stats->bToSelfBA)) {
+		for (rfpath = RF90_PATH_A; rfpath < priv->NumTotalRFPath; rfpath++) {
+			if (!rtl8192_phy_CheckIsLegalRFPath(priv->ieee80211->dev,
+							    rfpath))
+				continue;
+			if (priv->stats.rx_rssi_percentage[rfpath] == 0)
+				priv->stats.rx_rssi_percentage[rfpath] =
+					pprevious_stats->RxMIMOSignalStrength[rfpath];
+			if (pprevious_stats->RxMIMOSignalStrength[rfpath]  > priv->stats.rx_rssi_percentage[rfpath]) {
+				priv->stats.rx_rssi_percentage[rfpath] =
+					((priv->stats.rx_rssi_percentage[rfpath] * (RX_SMOOTH_FACTOR - 1)) +
+					 (pprevious_stats->RxMIMOSignalStrength[rfpath])) / (RX_SMOOTH_FACTOR);
+				priv->stats.rx_rssi_percentage[rfpath] = priv->stats.rx_rssi_percentage[rfpath]  + 1;
+			} else {
+				priv->stats.rx_rssi_percentage[rfpath] =
+					((priv->stats.rx_rssi_percentage[rfpath] * (RX_SMOOTH_FACTOR - 1)) +
+					 (pprevious_stats->RxMIMOSignalStrength[rfpath])) / (RX_SMOOTH_FACTOR);
+			}
+			RT_TRACE(COMP_DBG,
+				 "priv->stats.rx_rssi_percentage[rfPath]  = %d\n",
+				 priv->stats.rx_rssi_percentage[rfpath]);
+		}
+	}
+
+	/* Check PWDB. */
+	RT_TRACE(COMP_RXDESC, "Smooth %s PWDB = %d\n",
+		 pprevious_stats->bIsCCK ? "CCK" : "OFDM",
+		 pprevious_stats->RxPWDBAll);
+
+	if (pprevious_stats->bPacketBeacon) {
+		/* record the beacon pwdb to the sliding window. */
+		if (slide_beacon_adc_pwdb_statistics++ >= PHY_Beacon_RSSI_SLID_WIN_MAX) {
+			slide_beacon_adc_pwdb_statistics = PHY_Beacon_RSSI_SLID_WIN_MAX;
+			last_beacon_adc_pwdb = priv->stats.Slide_Beacon_pwdb[slide_beacon_adc_pwdb_index];
+			priv->stats.Slide_Beacon_Total -= last_beacon_adc_pwdb;
+		}
+		priv->stats.Slide_Beacon_Total += pprevious_stats->RxPWDBAll;
+		priv->stats.Slide_Beacon_pwdb[slide_beacon_adc_pwdb_index] = pprevious_stats->RxPWDBAll;
+		slide_beacon_adc_pwdb_index++;
+		if (slide_beacon_adc_pwdb_index >= PHY_Beacon_RSSI_SLID_WIN_MAX)
+			slide_beacon_adc_pwdb_index = 0;
+		pprevious_stats->RxPWDBAll = priv->stats.Slide_Beacon_Total / slide_beacon_adc_pwdb_statistics;
+		if (pprevious_stats->RxPWDBAll >= 3)
+			pprevious_stats->RxPWDBAll -= 3;
+	}
+
+	RT_TRACE(COMP_RXDESC, "Smooth %s PWDB = %d\n",
+		 pprevious_stats->bIsCCK ? "CCK" : "OFDM",
+		 pprevious_stats->RxPWDBAll);
+
+	if (pprevious_stats->bPacketToSelf ||
+	    pprevious_stats->bPacketBeacon ||
+	    pprevious_stats->bToSelfBA) {
+		if (priv->undecorated_smoothed_pwdb < 0)
+			/* initialize */
+			priv->undecorated_smoothed_pwdb =
+				pprevious_stats->RxPWDBAll;
+		if (pprevious_stats->RxPWDBAll > (u32)priv->undecorated_smoothed_pwdb) {
+			priv->undecorated_smoothed_pwdb =
+				(((priv->undecorated_smoothed_pwdb) * (RX_SMOOTH_FACTOR - 1)) +
+				 (pprevious_stats->RxPWDBAll)) / (RX_SMOOTH_FACTOR);
+			priv->undecorated_smoothed_pwdb = priv->undecorated_smoothed_pwdb + 1;
+		} else {
+			priv->undecorated_smoothed_pwdb =
+				(((priv->undecorated_smoothed_pwdb) * (RX_SMOOTH_FACTOR - 1)) +
+				 (pprevious_stats->RxPWDBAll)) / (RX_SMOOTH_FACTOR);
+		}
+	}
+
+	/* Check EVM */
+	/* record the general EVM to the sliding window. */
+	if (pprevious_stats->SignalQuality) {
+		if (pprevious_stats->bPacketToSelf ||
+		    pprevious_stats->bPacketBeacon ||
+		    pprevious_stats->bToSelfBA) {
+			if (slide_evm_statistics++ >= PHY_RSSI_SLID_WIN_MAX) {
+				slide_evm_statistics = PHY_RSSI_SLID_WIN_MAX;
+				last_evm = priv->stats.slide_evm[slide_evm_index];
+				priv->stats.slide_evm_total -= last_evm;
+			}
+
+			priv->stats.slide_evm_total +=
+				pprevious_stats->SignalQuality;
+
+			priv->stats.slide_evm[slide_evm_index++] =
+				pprevious_stats->SignalQuality;
+			if (slide_evm_index >= PHY_RSSI_SLID_WIN_MAX)
+				slide_evm_index = 0;
+
+			/* <1> Showed on UI for user, in percentage. */
+			tmp_val = priv->stats.slide_evm_total /
+				  slide_evm_statistics;
+			priv->stats.signal_quality = tmp_val;
+			/* Showed on UI for user in Windows Vista,
+			 * for Link quality.
+			 */
+			priv->stats.last_signal_strength_inpercent = tmp_val;
+		}
+
+		/* <2> Showed on UI for engineering */
+		if (pprevious_stats->bPacketToSelf ||
+		    pprevious_stats->bPacketBeacon ||
+		    pprevious_stats->bToSelfBA) {
+			for (nspatial_stream = 0; nspatial_stream < 2; nspatial_stream++) { /* 2 spatial stream */
+				if (pprevious_stats->RxMIMOSignalQuality[nspatial_stream] != -1) {
+					if (priv->stats.rx_evm_percentage[nspatial_stream] == 0) /* initialize */
+						priv->stats.rx_evm_percentage[nspatial_stream] = pprevious_stats->RxMIMOSignalQuality[nspatial_stream];
+					priv->stats.rx_evm_percentage[nspatial_stream] =
+						((priv->stats.rx_evm_percentage[nspatial_stream] * (RX_SMOOTH_FACTOR - 1)) +
+						 (pprevious_stats->RxMIMOSignalQuality[nspatial_stream] * 1)) / (RX_SMOOTH_FACTOR);
+				}
+			}
+		}
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	rtl819x_query_rxpwrpercentage()
+ *
+ * Overview:
+ *
+ * Input:		char		antpower
+ *
+ * Output:		NONE
+ *
+ * Return:		0-100 percentage
+ *---------------------------------------------------------------------------
+ */
+static u8 rtl819x_query_rxpwrpercentage(s8 antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return	0;
+	else if (antpower >= 0)
+		return	100;
+	else
+		return	100 + antpower;
+
+}	/* QueryRxPwrPercentage */
+
+static u8 rtl819x_evm_dbtopercentage(s8 value)
+{
+	s8 ret_val = clamp(-value, 0, 33) * 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+/* We want good-looking for signal strength/quality */
+static long rtl819x_signal_scale_mapping(long currsig)
+{
+	long retsig;
+
+	/* Step 1. Scale mapping. */
+	if (currsig >= 61 && currsig <= 100)
+		retsig = 90 + ((currsig - 60) / 4);
+	else if (currsig >= 41 && currsig <= 60)
+		retsig = 78 + ((currsig - 40) / 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 5 && currsig <= 20)
+		retsig = 42 + (((currsig - 5) * 2) / 3);
+	else if (currsig == 4)
+		retsig = 36;
+	else if (currsig == 3)
+		retsig = 27;
+	else if (currsig == 2)
+		retsig = 18;
+	else if (currsig == 1)
+		retsig = 9;
+	else
+		retsig = currsig;
+
+	return retsig;
+}
+
+static inline bool rx_hal_is_cck_rate(struct rx_drvinfo_819x_usb *pdrvinfo)
+{
+	if (pdrvinfo->RxHT)
+		return false;
+
+	switch (pdrvinfo->RxRate) {
+	case DESC90_RATE1M:
+	case DESC90_RATE2M:
+	case DESC90_RATE5_5M:
+	case DESC90_RATE11M:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void rtl8192_query_rxphystatus(struct r8192_priv *priv,
+				      struct ieee80211_rx_stats *pstats,
+				      struct rx_drvinfo_819x_usb  *pdrvinfo,
+				      struct ieee80211_rx_stats *precord_stats,
+				      bool bpacket_match_bssid,
+				      bool bpacket_toself,
+				      bool bPacketBeacon,
+				      bool bToSelfBA)
+{
+	phy_sts_ofdm_819xusb_t *pofdm_buf;
+	phy_sts_cck_819xusb_t	*pcck_buf;
+	struct phy_ofdm_rx_status_rxsc_sgien_exintfflag *prxsc;
+	u8	*prxpkt;
+	u8	i, max_spatial_stream, tmp_rxsnr, tmp_rxevm, rxsc_sgien_exflg;
+	s8	rx_pwr[4], rx_pwr_all = 0;
+	s8	rx_snrX, rx_evmX;
+	u8	evm, pwdb_all;
+	u32	RSSI, total_rssi = 0;
+	u8	is_cck_rate = 0;
+	u8	rf_rx_num = 0;
+	u8	sq;
+
+	priv->stats.numqry_phystatus++;
+
+	is_cck_rate = rx_hal_is_cck_rate(pdrvinfo);
+
+	/* Record it for next packet processing */
+	memset(precord_stats, 0, sizeof(struct ieee80211_rx_stats));
+	pstats->bPacketMatchBSSID =
+		precord_stats->bPacketMatchBSSID = bpacket_match_bssid;
+	pstats->bPacketToSelf = precord_stats->bPacketToSelf = bpacket_toself;
+	pstats->bIsCCK = precord_stats->bIsCCK = is_cck_rate;
+	pstats->bPacketBeacon = precord_stats->bPacketBeacon = bPacketBeacon;
+	pstats->bToSelfBA = precord_stats->bToSelfBA = bToSelfBA;
+
+	prxpkt = (u8 *)pdrvinfo;
+
+	/* Move pointer to the 16th bytes. Phy status start address. */
+	prxpkt += sizeof(struct rx_drvinfo_819x_usb);
+
+	/* Initial the cck and ofdm buffer pointer */
+	pcck_buf = (phy_sts_cck_819xusb_t *)prxpkt;
+	pofdm_buf = (phy_sts_ofdm_819xusb_t *)prxpkt;
+
+	pstats->RxMIMOSignalQuality[0] = -1;
+	pstats->RxMIMOSignalQuality[1] = -1;
+	precord_stats->RxMIMOSignalQuality[0] = -1;
+	precord_stats->RxMIMOSignalQuality[1] = -1;
+
+	if (is_cck_rate) {
+		/* (1)Hardware does not provide RSSI for CCK */
+
+		/* (2)PWDB, Average PWDB calculated by hardware
+		 * (for rate adaptive)
+		 */
+		u8 report;
+
+		priv->stats.numqry_phystatusCCK++;
+
+		if (!priv->bCckHighPower) {
+			report = pcck_buf->cck_agc_rpt & 0xc0;
+			report >>= 6;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -35 - (pcck_buf->cck_agc_rpt & 0x3e);
+				break;
+			case 0x2:
+				rx_pwr_all = -23 - (pcck_buf->cck_agc_rpt & 0x3e);
+				break;
+			case 0x1:
+				rx_pwr_all = -11 - (pcck_buf->cck_agc_rpt & 0x3e);
+				break;
+			case 0x0:
+				rx_pwr_all = 6 - (pcck_buf->cck_agc_rpt & 0x3e);
+				break;
+			}
+		} else {
+			report = pcck_buf->cck_agc_rpt & 0x60;
+			report >>= 5;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -35 - ((pcck_buf->cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x2:
+				rx_pwr_all = -23 - ((pcck_buf->cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x1:
+				rx_pwr_all = -11 - ((pcck_buf->cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x0:
+				rx_pwr_all = 6 - ((pcck_buf->cck_agc_rpt & 0x1f) << 1);
+				break;
+			}
+		}
+
+		pwdb_all = rtl819x_query_rxpwrpercentage(rx_pwr_all);
+		pstats->RxPWDBAll = precord_stats->RxPWDBAll = pwdb_all;
+		pstats->RecvSignalPower = pwdb_all;
+
+		/* (3) Get Signal Quality (EVM) */
+
+		if (pstats->RxPWDBAll > 40) {
+			sq = 100;
+		} else {
+			sq = pcck_buf->sq_rpt;
+
+			if (pcck_buf->sq_rpt > 64)
+				sq = 0;
+			else if (pcck_buf->sq_rpt < 20)
+				sq = 100;
+			else
+				sq = ((64 - sq) * 100) / 44;
+		}
+		pstats->SignalQuality = precord_stats->SignalQuality = sq;
+		pstats->RxMIMOSignalQuality[0] =
+			precord_stats->RxMIMOSignalQuality[0] = sq;
+		pstats->RxMIMOSignalQuality[1] =
+			precord_stats->RxMIMOSignalQuality[1] = -1;
+
+	} else {
+		priv->stats.numqry_phystatusHT++;
+
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < priv->NumTotalRFPath; i++) {
+			/* We will judge RF RX path now. */
+			if (priv->brfpath_rxenable[i])
+				rf_rx_num++;
+			else
+				continue;
+
+			if (!rtl8192_phy_CheckIsLegalRFPath(priv->ieee80211->dev, i))
+				continue;
+
+			rx_pwr[i] =
+				((pofdm_buf->trsw_gain_X[i] & 0x3F) * 2) - 106;
+
+			/* Get Rx snr value in DB */
+			tmp_rxsnr =	pofdm_buf->rxsnr_X[i];
+			rx_snrX = (s8)(tmp_rxsnr);
+			rx_snrX /= 2;
+			priv->stats.rxSNRdB[i] = (long)rx_snrX;
+
+			/* Translate DBM to percentage. */
+			RSSI = rtl819x_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += RSSI;
+
+			/* Record Signal Strength for next packet */
+			pstats->RxMIMOSignalStrength[i] = (u8)RSSI;
+			precord_stats->RxMIMOSignalStrength[i] = (u8)RSSI;
+		}
+
+		/* (2)PWDB, Average PWDB calculated by hardware
+		 * (for rate adaptive)
+		 */
+		rx_pwr_all = (((pofdm_buf->pwdb_all) >> 1) & 0x7f) - 106;
+		pwdb_all = rtl819x_query_rxpwrpercentage(rx_pwr_all);
+
+		pstats->RxPWDBAll = precord_stats->RxPWDBAll = pwdb_all;
+		pstats->RxPower = precord_stats->RxPower =  rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if (pdrvinfo->RxHT && pdrvinfo->RxRate >= DESC90_RATEMCS8 &&
+		    pdrvinfo->RxRate <= DESC90_RATEMCS15)
+			/* both spatial stream make sense */
+			max_spatial_stream = 2;
+		else
+			/* only spatial stream 1 makes sense */
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			tmp_rxevm =	pofdm_buf->rxevm_X[i];
+			rx_evmX = (s8)(tmp_rxevm);
+
+			/* Do not use shift operation like "rx_evmX >>= 1"
+			 * because the compiler of free build environment will
+			 * set the most significant bit to "zero" when doing
+			 * shifting operation which may change a negative value
+			 * to positive one, then the dbm value (which is
+			 * supposed to be negative) is not correct anymore.
+			 */
+			rx_evmX /= 2;	/* dbm */
+
+			evm = rtl819x_evm_dbtopercentage(rx_evmX);
+			if (i == 0)
+				/* Fill value in RFD, Get the first spatial
+				 * stream only
+				 */
+				pstats->SignalQuality =
+					precord_stats->SignalQuality =
+					evm & 0xff;
+			pstats->RxMIMOSignalQuality[i] =
+				precord_stats->RxMIMOSignalQuality[i] =
+				evm & 0xff;
+		}
+
+		/* record rx statistics for debug */
+		rxsc_sgien_exflg = pofdm_buf->rxsc_sgien_exflg;
+		prxsc =	(struct phy_ofdm_rx_status_rxsc_sgien_exintfflag *)
+			&rxsc_sgien_exflg;
+		if (pdrvinfo->BW)	/* 40M channel */
+			priv->stats.received_bwtype[1 + prxsc->rxsc]++;
+		else			/* 20M channel */
+			priv->stats.received_bwtype[0]++;
+	}
+
+	/* UI BSS List signal strength(in percentage), make it good looking,
+	 * from 0~100. It is assigned to the BSS List in
+	 * GetValueFromBeaconOrProbeRsp().
+	 */
+	if (is_cck_rate) {
+		pstats->SignalStrength =
+			precord_stats->SignalStrength =
+			(u8)(rtl819x_signal_scale_mapping((long)pwdb_all));
+	} else {
+		/* We can judge RX path number now. */
+		if (rf_rx_num != 0) {
+			pstats->SignalStrength =
+				precord_stats->SignalStrength =
+				(u8)(rtl819x_signal_scale_mapping((long)(total_rssi /= rf_rx_num)));
+		}
+	}
+}	/* QueryRxPhyStatus8190Pci */
+
+static void rtl8192_record_rxdesc_forlateruse(struct ieee80211_rx_stats *psrc_stats,
+					      struct ieee80211_rx_stats *ptarget_stats)
+{
+	ptarget_stats->bIsAMPDU = psrc_stats->bIsAMPDU;
+	ptarget_stats->bFirstMPDU = psrc_stats->bFirstMPDU;
+	ptarget_stats->Seq_Num = psrc_stats->Seq_Num;
+}
+
+static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
+					  struct ieee80211_rx_stats *pstats,
+					  struct rx_drvinfo_819x_usb  *pdrvinfo)
+{
+	/* TODO: We must only check packet for current MAC address.
+	 * Not finish
+	 */
+	struct rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
+	struct net_device *dev = info->dev;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	bool bpacket_match_bssid, bpacket_toself;
+	bool bPacketBeacon = false, bToSelfBA = false;
+	static struct ieee80211_rx_stats  previous_stats;
+	struct rtl_80211_hdr_3addr *hdr;
+	u16 fc, type;
+
+	/* Get Signal Quality for only RX data queue (but not command queue) */
+
+	u8 *tmp_buf;
+	u8  *praddr;
+
+	/* Get MAC frame start address. */
+	tmp_buf = (u8 *)skb->data;
+
+	hdr = (struct rtl_80211_hdr_3addr *)tmp_buf;
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+
+	/* Check if the received packet is acceptable. */
+	bpacket_match_bssid = (type != IEEE80211_FTYPE_CTL) &&
+			       (ether_addr_equal(priv->ieee80211->current_network.bssid,  (fc & IEEE80211_FCTL_TODS) ? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 : hdr->addr3))
+			       && (!pstats->bHwError) && (!pstats->bCRC) && (!pstats->bICV);
+	bpacket_toself =  bpacket_match_bssid &&
+			  (ether_addr_equal(praddr, priv->ieee80211->dev->dev_addr));
+
+	if (WLAN_FC_GET_FRAMETYPE(fc) == IEEE80211_STYPE_BEACON)
+		bPacketBeacon = true;
+	if (WLAN_FC_GET_FRAMETYPE(fc) == IEEE80211_STYPE_BLOCKACK) {
+		if ((ether_addr_equal(praddr, dev->dev_addr)))
+			bToSelfBA = true;
+	}
+
+	if (bpacket_match_bssid)
+		priv->stats.numpacket_matchbssid++;
+	if (bpacket_toself)
+		priv->stats.numpacket_toself++;
+	/* Process PHY information for previous packet (RSSI/PWDB/EVM)
+	 * Because phy information is contained in the last packet of AMPDU
+	 * only, so driver should process phy information of previous packet
+	 */
+	rtl8192_process_phyinfo(priv, tmp_buf, &previous_stats, pstats);
+	rtl8192_query_rxphystatus(priv, pstats, pdrvinfo, &previous_stats,
+				  bpacket_match_bssid, bpacket_toself,
+				  bPacketBeacon, bToSelfBA);
+	rtl8192_record_rxdesc_forlateruse(pstats, &previous_stats);
+}
+
+/*
+ * Function:	UpdateReceivedRateHistogramStatistics
+ * Overview:	Record the received data rate
+ *
+ * Input:
+ *	struct net_device *dev
+ *	struct ieee80211_rx_stats *stats
+ *
+ * Output:
+ *
+ *			(priv->stats.ReceivedRateHistogram[] is updated)
+ * Return:
+ *		None
+ */
+static void
+UpdateReceivedRateHistogramStatistics8190(struct net_device *dev,
+					  struct ieee80211_rx_stats *stats)
+{
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	/* 0: Total, 1:OK, 2:CRC, 3:ICV */
+	u32 rcvType = 1;
+	u32 rateIndex;
+	/* 1: short preamble/GI, 0: long preamble/GI */
+	u32 preamble_guardinterval;
+
+	if (stats->bCRC)
+		rcvType = 2;
+	else if (stats->bICV)
+		rcvType = 3;
+
+	if (stats->bShortPreamble)
+		preamble_guardinterval = 1; /* short */
+	else
+		preamble_guardinterval = 0; /* long */
+
+	switch (stats->rate) {
+	/* CCK rate */
+	case MGN_1M:
+		rateIndex = 0;
+		break;
+	case MGN_2M:
+		rateIndex = 1;
+		break;
+	case MGN_5_5M:
+		rateIndex = 2;
+		break;
+	case MGN_11M:
+		rateIndex = 3;
+		break;
+	/* Legacy OFDM rate */
+	case MGN_6M:
+		rateIndex = 4;
+		break;
+	case MGN_9M:
+		rateIndex = 5;
+		break;
+	case MGN_12M:
+		rateIndex = 6;
+		break;
+	case MGN_18M:
+		rateIndex = 7;
+		break;
+	case MGN_24M:
+		rateIndex = 8;
+		break;
+	case MGN_36M:
+		rateIndex = 9;
+		break;
+	case MGN_48M:
+		rateIndex = 10;
+		break;
+	case MGN_54M:
+		rateIndex = 11;
+		break;
+	/* 11n High throughput rate */
+	case MGN_MCS0:
+		rateIndex = 12;
+		break;
+	case MGN_MCS1:
+		rateIndex = 13;
+		break;
+	case MGN_MCS2:
+		rateIndex = 14;
+		break;
+	case MGN_MCS3:
+		rateIndex = 15;
+		break;
+	case MGN_MCS4:
+		rateIndex = 16;
+		break;
+	case MGN_MCS5:
+		rateIndex = 17;
+		break;
+	case MGN_MCS6:
+		rateIndex = 18;
+		break;
+	case MGN_MCS7:
+		rateIndex = 19;
+		break;
+	case MGN_MCS8:
+		rateIndex = 20;
+		break;
+	case MGN_MCS9:
+		rateIndex = 21;
+		break;
+	case MGN_MCS10:
+		rateIndex = 22;
+		break;
+	case MGN_MCS11:
+		rateIndex = 23;
+		break;
+	case MGN_MCS12:
+		rateIndex = 24;
+		break;
+	case MGN_MCS13:
+		rateIndex = 25;
+		break;
+	case MGN_MCS14:
+		rateIndex = 26;
+		break;
+	case MGN_MCS15:
+		rateIndex = 27;
+		break;
+	default:
+		rateIndex = 28;
+		break;
+	}
+	priv->stats.received_preamble_GI[preamble_guardinterval][rateIndex]++;
+	priv->stats.received_rate_histogram[0][rateIndex]++; /* total */
+	priv->stats.received_rate_histogram[rcvType][rateIndex]++;
+}
+
+static void query_rxdesc_status(struct sk_buff *skb,
+				struct ieee80211_rx_stats *stats,
+				bool bIsRxAggrSubframe)
+{
+	struct rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
+	struct net_device *dev = info->dev;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct rx_drvinfo_819x_usb  *driver_info = NULL;
+
+	/* Get Rx Descriptor Information */
+	struct rx_desc_819x_usb *desc = (struct rx_desc_819x_usb *)skb->data;
+
+	stats->Length = desc->Length;
+	stats->RxDrvInfoSize = desc->RxDrvInfoSize;
+	stats->RxBufShift = 0;
+	stats->bICV = desc->ICV;
+	stats->bCRC = desc->CRC32;
+	stats->bHwError = stats->bCRC | stats->bICV;
+	/* RTL8190 set this bit to indicate that Hw does not decrypt packet */
+	stats->Decrypted = !desc->SWDec;
+
+	if ((priv->ieee80211->pHTInfo->bCurrentHTSupport) &&
+	    (priv->ieee80211->pairwise_key_type == KEY_TYPE_CCMP))
+		stats->bHwError = false;
+	else
+		stats->bHwError = stats->bCRC | stats->bICV;
+
+	if (stats->Length < 24 || stats->Length > MAX_8192U_RX_SIZE)
+		stats->bHwError |= 1;
+	/* Get Driver Info */
+	/* TODO: Need to verify it on FGPA platform
+	 * Driver info are written to the RxBuffer following rx desc
+	 */
+	if (stats->RxDrvInfoSize != 0) {
+		driver_info = (struct rx_drvinfo_819x_usb *)(skb->data
+				+ sizeof(struct rx_desc_819x_usb)
+				+ stats->RxBufShift
+			      );
+		/* unit: 0.5M */
+		/* TODO */
+		if (!stats->bHwError) {
+			u8	ret_rate;
+
+			ret_rate = HwRateToMRate90(driver_info->RxHT,
+						   driver_info->RxRate);
+			if (ret_rate == 0xff) {
+				/* Abnormal Case: Receive CRC OK packet with Rx
+				 * descriptor indicating non supported rate.
+				 * Special Error Handling here
+				 */
+
+				stats->bHwError = 1;
+				/* Set 1M rate by default */
+				stats->rate = MGN_1M;
+			} else {
+				stats->rate = ret_rate;
+			}
+		} else {
+			stats->rate = 0x02;
+		}
+
+		stats->bShortPreamble = driver_info->SPLCP;
+
+		UpdateReceivedRateHistogramStatistics8190(dev, stats);
+
+		stats->bIsAMPDU = (driver_info->PartAggr == 1);
+		stats->bFirstMPDU = (driver_info->PartAggr == 1) &&
+				    (driver_info->FirstAGGR == 1);
+		stats->TimeStampLow = driver_info->TSFL;
+
+		UpdateRxPktTimeStamp8190(dev, stats);
+
+		/* Rx A-MPDU */
+		if (driver_info->FirstAGGR == 1 || driver_info->PartAggr == 1)
+			RT_TRACE(COMP_RXDESC,
+				 "driver_info->FirstAGGR = %d, driver_info->PartAggr = %d\n",
+				 driver_info->FirstAGGR, driver_info->PartAggr);
+	}
+
+	skb_pull(skb, sizeof(struct rx_desc_819x_usb));
+	/* Get Total offset of MPDU Frame Body */
+	if ((stats->RxBufShift + stats->RxDrvInfoSize) > 0) {
+		stats->bShift = 1;
+		skb_pull(skb, stats->RxBufShift + stats->RxDrvInfoSize);
+	}
+
+	if (driver_info) {
+		stats->RxIs40MHzPacket = driver_info->BW;
+		TranslateRxSignalStuff819xUsb(skb, stats, driver_info);
+	}
+}
+
+static void rtl8192_rx_nomal(struct sk_buff *skb)
+{
+	struct rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
+	struct net_device *dev = info->dev;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct ieee80211_rx_stats stats = {
+		.signal = 0,
+		.noise = 0x100 - 98,
+		.rate = 0,
+		.freq = IEEE80211_24GHZ_BAND,
+	};
+	u32 rx_pkt_len = 0;
+	struct rtl_80211_hdr_1addr *ieee80211_hdr = NULL;
+	bool unicast_packet = false;
+
+	/* 20 is for ps-poll */
+	if ((skb->len >= (20 + sizeof(struct rx_desc_819x_usb))) && (skb->len < RX_URB_SIZE)) {
+		/* first packet should not contain Rx aggregation header */
+		query_rxdesc_status(skb, &stats, false);
+		/* TODO */
+		/* hardware related info */
+		/* Process the MPDU received */
+		skb_trim(skb, skb->len - 4/*sCrcLng*/);
+
+		rx_pkt_len = skb->len;
+		ieee80211_hdr = (struct rtl_80211_hdr_1addr *)skb->data;
+		unicast_packet = false;
+		if (is_broadcast_ether_addr(ieee80211_hdr->addr1)) {
+			/* TODO */
+		} else if (is_multicast_ether_addr(ieee80211_hdr->addr1)) {
+			/* TODO */
+		} else {
+			/* unicast packet */
+			unicast_packet = true;
+		}
+
+		if (!ieee80211_rx(priv->ieee80211, skb, &stats)) {
+			dev_kfree_skb_any(skb);
+		} else {
+			priv->stats.rxoktotal++;
+			if (unicast_packet)
+				priv->stats.rxbytesunicast += rx_pkt_len;
+		}
+	} else {
+		priv->stats.rxurberr++;
+		netdev_dbg(dev, "actual_length: %d\n", skb->len);
+		dev_kfree_skb_any(skb);
+	}
+}
+
+static void rtl819xusb_process_received_packet(struct net_device *dev,
+					       struct ieee80211_rx_stats *pstats)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Get shifted bytes of Starting address of 802.11 header. */
+	pstats->virtual_address += get_rxpacket_shiftbytes_819xusb(pstats);
+#ifdef TODO	/* about HCT */
+	if (!Adapter->bInHctTest)
+		CountRxErrStatistics(Adapter, pRfd);
+#endif
+#ifdef ENABLE_PS  /* for adding ps function in future */
+	RT_RF_POWER_STATE rtState;
+	/* When RF is off, we should not count the packet for hw/sw synchronize
+	 * reason, ie. there may be a duration while sw switch is changed and
+	 * hw switch is being changed.
+	 */
+	Adapter->HalFunc.GetHwRegHandler(Adapter, HW_VAR_RF_STATE,
+					 (u8 *)(&rtState));
+	if (rtState == eRfOff)
+		return;
+#endif
+	priv->stats.rxframgment++;
+
+#ifdef TODO
+	RmMonitorSignalStrength(Adapter, pRfd);
+#endif
+	/* We have to release RFD and return if rx pkt is cmd pkt. */
+	if (rtl819xusb_rx_command_packet(dev, pstats))
+		return;
+
+#ifdef SW_CRC_CHECK
+	SwCrcCheck();
+#endif
+}
+
+static void query_rx_cmdpkt_desc_status(struct sk_buff *skb,
+					struct ieee80211_rx_stats *stats)
+{
+	struct rx_desc_819x_usb *desc = (struct rx_desc_819x_usb *)skb->data;
+
+	/* Get Rx Descriptor Information */
+	stats->virtual_address = (u8 *)skb->data;
+	stats->Length = desc->Length;
+	stats->RxDrvInfoSize = 0;
+	stats->RxBufShift = 0;
+	stats->packetlength = stats->Length - scrclng;
+	stats->fraglength = stats->packetlength;
+	stats->fragoffset = 0;
+	stats->ntotalfrag = 1;
+}
+
+static void rtl8192_rx_cmd(struct sk_buff *skb)
+{
+	struct rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
+	struct net_device *dev = info->dev;
+	/* TODO */
+	struct ieee80211_rx_stats stats = {
+		.signal = 0,
+		.noise = 0x100 - 98,
+		.rate = 0,
+		.freq = IEEE80211_24GHZ_BAND,
+	};
+
+	if ((skb->len >= (20 + sizeof(struct rx_desc_819x_usb))) && (skb->len < RX_URB_SIZE)) {
+		query_rx_cmdpkt_desc_status(skb, &stats);
+		/* prfd->queue_id = 1; */
+
+		/* Process the command packet received. */
+
+		rtl819xusb_process_received_packet(dev, &stats);
+
+		dev_kfree_skb_any(skb);
+	}
+}
+
+static void rtl8192_irq_rx_tasklet(struct tasklet_struct *t)
+{
+	struct r8192_priv *priv = from_tasklet(priv, t, irq_rx_tasklet);
+	struct sk_buff *skb;
+	struct rtl8192_rx_info *info;
+
+	while (NULL != (skb = skb_dequeue(&priv->skb_queue))) {
+		info = (struct rtl8192_rx_info *)skb->cb;
+		switch (info->out_pipe) {
+		/* Nomal packet pipe */
+		case 3:
+			priv->IrpPendingCount--;
+			rtl8192_rx_nomal(skb);
+			break;
+
+		/* Command packet pipe */
+		case 9:
+			RT_TRACE(COMP_RECV, "command in-pipe index(%d)\n",
+				 info->out_pipe);
+
+			rtl8192_rx_cmd(skb);
+			break;
+
+		default: /* should never get here! */
+			RT_TRACE(COMP_ERR, "Unknown in-pipe index(%d)\n",
+				 info->out_pipe);
+			dev_kfree_skb(skb);
+			break;
+		}
+	}
+}
+
+static const struct net_device_ops rtl8192_netdev_ops = {
+	.ndo_open               = rtl8192_open,
+	.ndo_stop               = rtl8192_close,
+	.ndo_get_stats          = rtl8192_stats,
+	.ndo_tx_timeout         = tx_timeout,
+	.ndo_do_ioctl           = rtl8192_ioctl,
+	.ndo_set_rx_mode	= r8192_set_multicast,
+	.ndo_set_mac_address    = r8192_set_mac_adr,
+	.ndo_validate_addr      = eth_validate_addr,
+	.ndo_start_xmit         = ieee80211_xmit,
+};
+
+/****************************************************************************
+ *    ---------------------------- USB_STUFF---------------------------
+ *****************************************************************************/
+
+static int rtl8192_usb_probe(struct usb_interface *intf,
+			     const struct usb_device_id *id)
+{
+	struct net_device *dev = NULL;
+	struct r8192_priv *priv = NULL;
+	struct usb_device *udev = interface_to_usbdev(intf);
+	int ret;
+
+	RT_TRACE(COMP_INIT, "Oops: i'm coming\n");
+
+	dev = alloc_ieee80211(sizeof(struct r8192_priv));
+	if (!dev)
+		return -ENOMEM;
+
+	usb_set_intfdata(intf, dev);
+	SET_NETDEV_DEV(dev, &intf->dev);
+	priv = ieee80211_priv(dev);
+	priv->ieee80211 = netdev_priv(dev);
+	priv->udev = udev;
+
+	dev->netdev_ops = &rtl8192_netdev_ops;
+
+	dev->wireless_handlers = &r8192_wx_handlers_def;
+
+	dev->type = ARPHRD_ETHER;
+
+	dev->watchdog_timeo = HZ * 3;
+
+	if (dev_alloc_name(dev, ifname) < 0) {
+		RT_TRACE(COMP_INIT,
+			 "Oops: devname already taken! Trying wlan%%d...\n");
+		ifname = "wlan%d";
+		dev_alloc_name(dev, ifname);
+	}
+
+	RT_TRACE(COMP_INIT, "Driver probe completed1\n");
+	if (rtl8192_init(dev) != 0) {
+		RT_TRACE(COMP_ERR, "Initialization failed");
+		ret = -ENODEV;
+		goto fail;
+	}
+	netif_carrier_off(dev);
+	netif_stop_queue(dev);
+
+	ret = register_netdev(dev);
+	if (ret)
+		goto fail2;
+
+	RT_TRACE(COMP_INIT, "dev name=======> %s\n", dev->name);
+	rtl8192_debugfs_init_one(dev);
+
+	RT_TRACE(COMP_INIT, "Driver probe completed\n");
+	return 0;
+
+fail2:
+	rtl8192_down(dev);
+fail:
+	kfree(priv->pFirmware);
+	priv->pFirmware = NULL;
+	rtl8192_usb_deleteendpoints(dev);
+	msleep(10);
+	free_ieee80211(dev);
+
+	RT_TRACE(COMP_ERR, "wlan driver load failed\n");
+	return ret;
+}
+
+/* detach all the work and timer structure declared or inititialize
+ * in r8192U_init function.
+ */
+static void rtl8192_cancel_deferred_work(struct r8192_priv *priv)
+{
+	cancel_work_sync(&priv->reset_wq);
+	cancel_delayed_work(&priv->watch_dog_wq);
+	cancel_delayed_work(&priv->update_beacon_wq);
+	cancel_work_sync(&priv->qos_activate);
+}
+
+static void rtl8192_usb_disconnect(struct usb_interface *intf)
+{
+	struct net_device *dev = usb_get_intfdata(intf);
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+
+	RT_TRACE(COMP_DOWN, "=============>wlan driver to be removed\n");
+	rtl8192_debugfs_exit_one(dev);
+
+	unregister_netdev(dev);
+
+	rtl8192_down(dev);
+	kfree(priv->pFirmware);
+	priv->pFirmware = NULL;
+	rtl8192_usb_deleteendpoints(dev);
+	usleep_range(10000, 11000);
+	free_ieee80211(dev);
+
+	RT_TRACE(COMP_DOWN, "wlan driver removed\n");
+}
+
+static int rtl8192_usb_netdev_event(struct notifier_block *nb, unsigned long event,
+				    void *data)
+{
+	struct net_device *netdev = netdev_notifier_info_to_dev(data);
+
+	if (netdev->netdev_ops != &rtl8192_netdev_ops)
+		goto out;
+
+	switch (event) {
+	case NETDEV_CHANGENAME:
+		rtl8192_debugfs_rename_one(netdev);
+		break;
+	default:
+		break;
+	}
+
+out:
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block rtl8192_usb_netdev_notifier = {
+	.notifier_call = rtl8192_usb_netdev_event,
+};
+
+static int __init rtl8192_usb_module_init(void)
+{
+	int ret;
+
+	pr_info("\nLinux kernel driver for RTL8192 based WLAN cards\n");
+	pr_info("Copyright (c) 2007-2008, Realsil Wlan\n");
+	RT_TRACE(COMP_INIT, "Initializing module");
+	RT_TRACE(COMP_INIT, "Wireless extensions version %d", WIRELESS_EXT);
+
+	ret = register_netdevice_notifier(&rtl8192_usb_netdev_notifier);
+	if (ret) {
+		pr_err("register_netdevice_notifier failed %d\n", ret);
+		return ret;
+	}
+
+	rtl8192_debugfs_init();
+	ret = ieee80211_debug_init();
+	if (ret) {
+		pr_err("ieee80211_debug_init() failed %d\n", ret);
+		goto debugfs_exit;
+	}
+
+	ret = ieee80211_crypto_init();
+	if (ret) {
+		pr_err("ieee80211_crypto_init() failed %d\n", ret);
+		goto debug_exit;
+	}
+
+	ret = ieee80211_crypto_tkip_init();
+	if (ret) {
+		pr_err("ieee80211_crypto_tkip_init() failed %d\n", ret);
+		goto crypto_exit;
+	}
+
+	ret = ieee80211_crypto_ccmp_init();
+	if (ret) {
+		pr_err("ieee80211_crypto_ccmp_init() failed %d\n", ret);
+		goto crypto_tkip_exit;
+	}
+
+	ret = ieee80211_crypto_wep_init();
+	if (ret) {
+		pr_err("ieee80211_crypto_wep_init() failed %d\n", ret);
+		goto crypto_ccmp_exit;
+	}
+
+	ret = usb_register(&rtl8192_usb_driver);
+	if (ret)
+		goto crypto_wep_exit;
+	return ret;
+
+crypto_wep_exit:
+	ieee80211_crypto_wep_exit();
+crypto_ccmp_exit:
+	ieee80211_crypto_ccmp_exit();
+crypto_tkip_exit:
+	ieee80211_crypto_tkip_exit();
+crypto_exit:
+	ieee80211_crypto_deinit();
+debug_exit:
+	ieee80211_debug_exit();
+debugfs_exit:
+	rtl8192_debugfs_exit();
+	unregister_netdevice_notifier(&rtl8192_usb_netdev_notifier);
+	return ret;
+}
+
+static void __exit rtl8192_usb_module_exit(void)
+{
+	usb_deregister(&rtl8192_usb_driver);
+	ieee80211_crypto_wep_exit();
+	ieee80211_crypto_ccmp_exit();
+	ieee80211_crypto_tkip_exit();
+	ieee80211_crypto_deinit();
+	ieee80211_debug_exit();
+	rtl8192_debugfs_exit();
+	unregister_netdevice_notifier(&rtl8192_usb_netdev_notifier);
+	RT_TRACE(COMP_DOWN, "Exiting");
+}
+
+void EnableHWSecurityConfig8192(struct net_device *dev)
+{
+	u8 SECR_value = 0x0;
+	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	SECR_value = SCR_TxEncEnable | SCR_RxDecEnable;
+	if (((ieee->pairwise_key_type == KEY_TYPE_WEP40) || (ieee->pairwise_key_type == KEY_TYPE_WEP104)) && (priv->ieee80211->auth_mode != 2)) {
+		SECR_value |= SCR_RxUseDK;
+		SECR_value |= SCR_TxUseDK;
+	} else if ((ieee->iw_mode == IW_MODE_ADHOC) && (ieee->pairwise_key_type & (KEY_TYPE_CCMP | KEY_TYPE_TKIP))) {
+		SECR_value |= SCR_RxUseDK;
+		SECR_value |= SCR_TxUseDK;
+	}
+	/* add HWSec active enable here.
+	 * default using hwsec. when peer AP is in N mode only and
+	 * pairwise_key_type is none_aes(which HT_IOT_ACT_PURE_N_MODE indicates
+	 * it), use software security. when peer AP is in b,g,n mode mixed and
+	 * pairwise_key_type is none_aes, use g mode hw security.
+	 */
+
+	ieee->hwsec_active = 1;
+
+	/* add hwsec_support flag to totol control hw_sec on/off */
+	if ((ieee->pHTInfo->IOTAction & HT_IOT_ACT_PURE_N_MODE) || !hwwep) {
+		ieee->hwsec_active = 0;
+		SECR_value &= ~SCR_RxDecEnable;
+	}
+	RT_TRACE(COMP_SEC, "%s:, hwsec:%d, pairwise_key:%d, SECR_value:%x\n",
+		 __func__, ieee->hwsec_active, ieee->pairwise_key_type,
+		 SECR_value);
+	write_nic_byte(dev, SECR,  SECR_value);
+}
+
+void setKey(struct net_device *dev, u8 entryno, u8 keyindex, u16 keytype,
+	    const u8 *macaddr, u8 defaultkey, u32 *keycontent)
+{
+	u32 target_command = 0;
+	u32 target_content = 0;
+	u16 us_config = 0;
+	u8 i;
+
+	if (entryno >= TOTAL_CAM_ENTRY)
+		RT_TRACE(COMP_ERR, "cam entry exceeds in %s\n", __func__);
+
+	RT_TRACE(COMP_SEC,
+		 "====>to %s, dev:%p, EntryNo:%d, KeyIndex:%d, KeyType:%d, MacAddr%pM\n",
+		 __func__, dev, entryno, keyindex, keytype, macaddr);
+
+	if (defaultkey)
+		us_config |= BIT(15) | (keytype << 2);
+	else
+		us_config |= BIT(15) | (keytype << 2) | keyindex;
+
+	for (i = 0; i < CAM_CONTENT_COUNT; i++) {
+		target_command  = i + CAM_CONTENT_COUNT * entryno;
+		target_command |= BIT(31) | BIT(16);
+
+		if (i == 0) { /* MAC|Config */
+			target_content = (u32)(*(macaddr + 0)) << 16 |
+					(u32)(*(macaddr + 1)) << 24 |
+					(u32)us_config;
+
+			write_nic_dword(dev, WCAMI, target_content);
+			write_nic_dword(dev, RWCAM, target_command);
+		} else if (i == 1) { /* MAC */
+			target_content = (u32)(*(macaddr + 2))	 |
+					(u32)(*(macaddr + 3)) <<  8 |
+					(u32)(*(macaddr + 4)) << 16 |
+					(u32)(*(macaddr + 5)) << 24;
+			write_nic_dword(dev, WCAMI, target_content);
+			write_nic_dword(dev, RWCAM, target_command);
+		} else {
+			/* Key Material */
+			if (keycontent) {
+				write_nic_dword(dev, WCAMI,
+						*(keycontent + i - 2));
+				write_nic_dword(dev, RWCAM, target_command);
+			}
+		}
+	}
+}
+
+/***************************************************************************
+ *    ------------------- module init / exit stubs ----------------
+ ****************************************************************************/
+module_init(rtl8192_usb_module_init);
+module_exit(rtl8192_usb_module_exit);
diff --git a/drivers/staging/rtl8192u/r8192U_debugfs.c b/drivers/staging/rtl8192u/r8192U_debugfs.c
new file mode 100644
index 000000000..fe8ef7250
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_debugfs.c
@@ -0,0 +1,188 @@
+// SPDX-License-Identifier: GPL-2.0
+/****************************************************************************
+ *   -----------------------------DEGUGFS STUFF-------------------------
+ ****************************************************************************/
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include "r8192U.h"
+
+#define KBUILD_MODNAME "r8192u_usb"
+
+static int rtl8192_usb_stats_ap_show(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	struct ieee80211_network *target;
+
+	list_for_each_entry(target, &ieee->network_list, list) {
+		const char *wpa = "non_WPA";
+
+		if (target->wpa_ie_len > 0 || target->rsn_ie_len > 0)
+			wpa = "WPA";
+
+		seq_printf(m, "%s %s\n", target->ssid, wpa);
+	}
+
+	return 0;
+}
+
+static int rtl8192_usb_registers_show(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i, n, max = 0xff;
+	u8 byte_rd;
+
+	seq_puts(m, "\n####################page 0##################\n ");
+
+	for (n = 0; n <= max;) {
+		seq_printf(m, "\nD:  %2x > ", n);
+
+		for (i = 0; i < 16 && n <= max; i++, n++) {
+			read_nic_byte(dev, 0x000 | n, &byte_rd);
+			seq_printf(m, "%2x ", byte_rd);
+		}
+	}
+
+	seq_puts(m, "\n####################page 1##################\n ");
+	for (n = 0; n <= max;) {
+		seq_printf(m, "\nD:  %2x > ", n);
+
+		for (i = 0; i < 16 && n <= max; i++, n++) {
+			read_nic_byte(dev, 0x100 | n, &byte_rd);
+			seq_printf(m, "%2x ", byte_rd);
+		}
+	}
+
+	seq_puts(m, "\n####################page 3##################\n ");
+	for (n = 0; n <= max;) {
+		seq_printf(m, "\nD:  %2x > ", n);
+
+		for (i = 0; i < 16 && n <= max; i++, n++) {
+			read_nic_byte(dev, 0x300 | n, &byte_rd);
+			seq_printf(m, "%2x ", byte_rd);
+		}
+	}
+
+	seq_putc(m, '\n');
+	return 0;
+}
+
+static int rtl8192_usb_stats_tx_show(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	seq_printf(m,
+		   "TX VI priority ok int: %lu\n"
+		   "TX VI priority error int: %lu\n"
+		   "TX VO priority ok int: %lu\n"
+		   "TX VO priority error int: %lu\n"
+		   "TX BE priority ok int: %lu\n"
+		   "TX BE priority error int: %lu\n"
+		   "TX BK priority ok int: %lu\n"
+		   "TX BK priority error int: %lu\n"
+		   "TX MANAGE priority ok int: %lu\n"
+		   "TX MANAGE priority error int: %lu\n"
+		   "TX BEACON priority ok int: %lu\n"
+		   "TX BEACON priority error int: %lu\n"
+		   "TX queue resume: %lu\n"
+		   "TX queue stopped?: %d\n"
+		   "TX fifo overflow: %lu\n"
+		   "TX VI queue: %d\n"
+		   "TX VO queue: %d\n"
+		   "TX BE queue: %d\n"
+		   "TX BK queue: %d\n"
+		   "TX VI dropped: %lu\n"
+		   "TX VO dropped: %lu\n"
+		   "TX BE dropped: %lu\n"
+		   "TX BK dropped: %lu\n"
+		   "TX total data packets %lu\n",
+		   priv->stats.txviokint,
+		   priv->stats.txvierr,
+		   priv->stats.txvookint,
+		   priv->stats.txvoerr,
+		   priv->stats.txbeokint,
+		   priv->stats.txbeerr,
+		   priv->stats.txbkokint,
+		   priv->stats.txbkerr,
+		   priv->stats.txmanageokint,
+		   priv->stats.txmanageerr,
+		   priv->stats.txbeaconokint,
+		   priv->stats.txbeaconerr,
+		   priv->stats.txresumed,
+		   netif_queue_stopped(dev),
+		   priv->stats.txoverflow,
+		   atomic_read(&(priv->tx_pending[VI_PRIORITY])),
+		   atomic_read(&(priv->tx_pending[VO_PRIORITY])),
+		   atomic_read(&(priv->tx_pending[BE_PRIORITY])),
+		   atomic_read(&(priv->tx_pending[BK_PRIORITY])),
+		   priv->stats.txvidrop,
+		   priv->stats.txvodrop,
+		   priv->stats.txbedrop,
+		   priv->stats.txbkdrop,
+		   priv->stats.txdatapkt
+		);
+
+	return 0;
+}
+
+static int rtl8192_usb_stats_rx_show(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	seq_printf(m,
+		   "RX packets: %lu\n"
+		   "RX urb status error: %lu\n"
+		   "RX invalid urb error: %lu\n",
+		   priv->stats.rxoktotal,
+		   priv->stats.rxstaterr,
+		   priv->stats.rxurberr);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(rtl8192_usb_stats_rx);
+DEFINE_SHOW_ATTRIBUTE(rtl8192_usb_stats_tx);
+DEFINE_SHOW_ATTRIBUTE(rtl8192_usb_stats_ap);
+DEFINE_SHOW_ATTRIBUTE(rtl8192_usb_registers);
+
+void rtl8192_debugfs_init_one(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct dentry *parent_dir = debugfs_lookup(KBUILD_MODNAME, NULL);
+	struct dentry *dir = debugfs_create_dir(dev->name, parent_dir);
+
+	debugfs_create_file("stats-rx", 0444, dir, dev, &rtl8192_usb_stats_rx_fops);
+	debugfs_create_file("stats-tx", 0444, dir, dev, &rtl8192_usb_stats_tx_fops);
+	debugfs_create_file("stats-ap", 0444, dir, dev, &rtl8192_usb_stats_ap_fops);
+	debugfs_create_file("registers", 0444, dir, dev, &rtl8192_usb_registers_fops);
+
+	priv->debugfs_dir = dir;
+}
+
+void rtl8192_debugfs_exit_one(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	debugfs_remove_recursive(priv->debugfs_dir);
+}
+
+void rtl8192_debugfs_rename_one(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct dentry *parent_dir = debugfs_lookup(KBUILD_MODNAME, NULL);
+
+	debugfs_rename(parent_dir, priv->debugfs_dir, parent_dir, dev->name);
+}
+
+void rtl8192_debugfs_init(void)
+{
+	debugfs_create_dir(KBUILD_MODNAME, NULL);
+}
+
+void rtl8192_debugfs_exit(void)
+{
+	debugfs_remove_recursive(debugfs_lookup(KBUILD_MODNAME, NULL));
+}
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
new file mode 100644
index 000000000..00fc8fd34
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -0,0 +1,3019 @@
+// SPDX-License-Identifier: GPL-2.0
+/*++
+Copyright-c Realtek Semiconductor Corp. All rights reserved.
+
+Module Name:
+	r8192U_dm.c
+
+Abstract:
+	HW dynamic mechanism.
+
+Major Change History:
+	When		Who				What
+	----------	--------------- -------------------------------
+	2008-05-14	amy                     create version 0 porting from windows code.
+
+--*/
+#include "r8192U.h"
+#include "r8192U_dm.h"
+#include "r8192U_hw.h"
+#include "r819xU_phy.h"
+#include "r819xU_phyreg.h"
+#include "r8190_rtl8256.h"
+#include "r819xU_cmdpkt.h"
+/*---------------------------Define Local Constant---------------------------*/
+/* Indicate different AP vendor for IOT issue. */
+static u32 edca_setting_DL[HT_IOT_PEER_MAX] = {
+	0x5e4322, 0x5e4322, 0x5e4322, 0x604322, 0x00a44f, 0x5ea44f
+};
+
+static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
+	0x5e4322, 0x00a44f, 0x5e4322, 0x604322, 0x5ea44f, 0x5ea44f
+};
+
+#define RTK_UL_EDCA 0xa44f
+#define RTK_DL_EDCA 0x5e4322
+/*---------------------------Define Local Constant---------------------------*/
+
+
+/*------------------------Define global variable-----------------------------*/
+/* Debug variable ? */
+struct dig dm_digtable;
+/* Store current software write register content for MAC PHY. */
+u8		dm_shadow[16][256] = { {0} };
+/* For Dynamic Rx Path Selection by Signal Strength */
+static struct dynamic_rx_path_sel DM_RxPathSelTable;
+
+extern	void dm_check_fsync(struct net_device *dev);
+
+/* DM --> Rate Adaptive */
+static	void	dm_check_rate_adaptive(struct net_device *dev);
+
+/* DM --> Bandwidth switch */
+static	void	dm_init_bandwidth_autoswitch(struct net_device *dev);
+static	void	dm_bandwidth_autoswitch(struct net_device *dev);
+
+/* DM --> TX power control */
+/*static	void	dm_initialize_txpower_tracking(struct net_device *dev);*/
+
+static	void	dm_check_txpower_tracking(struct net_device *dev);
+
+/*static	void	dm_txpower_reset_recovery(struct net_device *dev);*/
+
+/* DM --> Dynamic Init Gain by RSSI */
+static	void	dm_dig_init(struct net_device *dev);
+static	void	dm_ctrl_initgain_byrssi(struct net_device *dev);
+static	void	dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev);
+static	void	dm_ctrl_initgain_byrssi_by_driverrssi(struct net_device *dev);
+static	void	dm_ctrl_initgain_byrssi_by_fwfalse_alarm(struct net_device *dev);
+static	void	dm_initial_gain(struct net_device *dev);
+static	void	dm_pd_th(struct net_device *dev);
+static	void	dm_cs_ratio(struct net_device *dev);
+
+static	void dm_init_ctstoself(struct net_device *dev);
+/* DM --> EDCA turbo mode control */
+static	void	dm_check_edca_turbo(struct net_device *dev);
+
+/*static	void	dm_gpio_change_rf(struct net_device *dev);*/
+/* DM --> Check PBC */
+static	void dm_check_pbc_gpio(struct net_device *dev);
+
+/* DM --> Check current RX RF path state */
+static	void	dm_check_rx_path_selection(struct net_device *dev);
+static	void dm_init_rxpath_selection(struct net_device *dev);
+static	void dm_rxpath_sel_byrssi(struct net_device *dev);
+
+/* DM --> Fsync for broadcom ap */
+static void dm_init_fsync(struct net_device *dev);
+static void dm_deInit_fsync(struct net_device *dev);
+
+/* Added by vivi, 20080522 */
+static	void	dm_check_txrateandretrycount(struct net_device *dev);
+
+/*---------------------Define local function prototype-----------------------*/
+
+/*---------------------Define of Tx Power Control For Near/Far Range --------*/   /*Add by Jacken 2008/02/18 */
+static	void	dm_init_dynamic_txpower(struct net_device *dev);
+static	void	dm_dynamic_txpower(struct net_device *dev);
+
+/* DM --> For rate adaptive and DIG, we must send RSSI to firmware */
+static	void dm_send_rssi_tofw(struct net_device *dev);
+static	void	dm_ctstoself(struct net_device *dev);
+/*---------------------------Define function prototype------------------------*/
+/* ================================================================================
+ *	HW Dynamic mechanism interface.
+ * ================================================================================
+ *
+ *
+ *	Description:
+ *		Prepare SW resource for HW dynamic mechanism.
+ *
+ *	Assumption:
+ *		This function is only invoked at driver initialization once.
+ */
+void init_hal_dm(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Undecorated Smoothed Signal Strength, it can utilized to dynamic mechanism. */
+	priv->undecorated_smoothed_pwdb = -1;
+
+	/* Initial TX Power Control for near/far range , add by amy 2008/05/15, porting from windows code. */
+	dm_init_dynamic_txpower(dev);
+	init_rate_adaptive(dev);
+	/*dm_initialize_txpower_tracking(dev);*/
+	dm_dig_init(dev);
+	dm_init_edca_turbo(dev);
+	dm_init_bandwidth_autoswitch(dev);
+	dm_init_fsync(dev);
+	dm_init_rxpath_selection(dev);
+	dm_init_ctstoself(dev);
+
+}	/* InitHalDm */
+
+void deinit_hal_dm(struct net_device *dev)
+{
+	dm_deInit_fsync(dev);
+}
+
+#ifdef USB_RX_AGGREGATION_SUPPORT
+void dm_CheckRxAggregation(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	PRT_HIGH_THROUGHPUT	pHTInfo = priv->ieee80211->pHTInfo;
+	static unsigned long	lastTxOkCnt;
+	static unsigned long	lastRxOkCnt;
+	unsigned long		curTxOkCnt = 0;
+	unsigned long		curRxOkCnt = 0;
+
+/*
+	if (pHalData->bForcedUsbRxAggr) {
+		if (pHalData->ForcedUsbRxAggrInfo == 0) {
+			if (pHalData->bCurrentRxAggrEnable) {
+				Adapter->HalFunc.HalUsbRxAggrHandler(Adapter, FALSE);
+			}
+		} else {
+			if (!pHalData->bCurrentRxAggrEnable || (pHalData->ForcedUsbRxAggrInfo != pHalData->LastUsbRxAggrInfoSetting)) {
+				Adapter->HalFunc.HalUsbRxAggrHandler(Adapter, TRUE);
+			}
+		}
+		return;
+	}
+
+*/
+	curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
+	curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
+
+	if ((curTxOkCnt + curRxOkCnt) < 15000000)
+		return;
+
+	if (curTxOkCnt > 4*curRxOkCnt) {
+		if (priv->bCurrentRxAggrEnable) {
+			write_nic_dword(dev, 0x1a8, 0);
+			priv->bCurrentRxAggrEnable = false;
+		}
+	} else {
+		if (!priv->bCurrentRxAggrEnable && !pHTInfo->bCurrentRT2RTAggregation) {
+			u32 ulValue;
+
+			ulValue = (pHTInfo->UsbRxFwAggrEn<<24) | (pHTInfo->UsbRxFwAggrPageNum<<16) |
+				(pHTInfo->UsbRxFwAggrPacketNum<<8) | (pHTInfo->UsbRxFwAggrTimeout);
+			/* If usb rx firmware aggregation is enabled,
+			 * when anyone of three threshold conditions above is reached,
+			 * firmware will send aggregated packet to driver.
+			 */
+			write_nic_dword(dev, 0x1a8, ulValue);
+			priv->bCurrentRxAggrEnable = true;
+		}
+	}
+
+	lastTxOkCnt = priv->stats.txbytesunicast;
+	lastRxOkCnt = priv->stats.rxbytesunicast;
+}	/* dm_CheckEdcaTurbo */
+#endif
+
+void hal_dm_watchdog(struct net_device *dev)
+{
+	/*struct r8192_priv *priv = ieee80211_priv(dev);*/
+
+	/*static u8	previous_bssid[6] ={0};*/
+
+	/*Add by amy 2008/05/15 ,porting from windows code.*/
+	dm_check_rate_adaptive(dev);
+	dm_dynamic_txpower(dev);
+	dm_check_txrateandretrycount(dev);
+	dm_check_txpower_tracking(dev);
+	dm_ctrl_initgain_byrssi(dev);
+	dm_check_edca_turbo(dev);
+	dm_bandwidth_autoswitch(dev);
+	dm_check_rx_path_selection(dev);
+	dm_check_fsync(dev);
+
+	/* Add by amy 2008-05-15 porting from windows code. */
+	dm_check_pbc_gpio(dev);
+	dm_send_rssi_tofw(dev);
+	dm_ctstoself(dev);
+#ifdef USB_RX_AGGREGATION_SUPPORT
+	dm_CheckRxAggregation(dev);
+#endif
+}	/* HalDmWatchDog */
+
+/* Decide Rate Adaptive Set according to distance (signal strength)
+ *	01/11/2008	MHC		Modify input arguments and RATR table level.
+ *	01/16/2008	MHC		RF_Type is assigned in ReadAdapterInfo(). We must call
+ *						the function after making sure RF_Type.
+ */
+void init_rate_adaptive(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	prate_adaptive	pra = (prate_adaptive)&priv->rate_adaptive;
+
+	pra->ratr_state = DM_RATR_STA_MAX;
+	pra->high2low_rssi_thresh_for_ra = RATE_ADAPTIVE_TH_HIGH;
+	pra->low2high_rssi_thresh_for_ra20M = RATE_ADAPTIVE_TH_LOW_20M + 5;
+	pra->low2high_rssi_thresh_for_ra40M = RATE_ADAPTIVE_TH_LOW_40M + 5;
+
+	pra->high_rssi_thresh_for_ra = RATE_ADAPTIVE_TH_HIGH + 5;
+	pra->low_rssi_thresh_for_ra20M = RATE_ADAPTIVE_TH_LOW_20M;
+	pra->low_rssi_thresh_for_ra40M = RATE_ADAPTIVE_TH_LOW_40M;
+
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		pra->ping_rssi_enable = 1;
+	else
+		pra->ping_rssi_enable = 0;
+	pra->ping_rssi_thresh_for_ra = 15;
+
+	if (priv->rf_type == RF_2T4R) {
+		/* 07/10/08 MH Modify for RA smooth scheme.
+		 * 2008/01/11 MH Modify 2T RATR table for different RSSI. 080515 porting by amy from windows code.
+		 */
+		pra->upper_rssi_threshold_ratr		=	0x8f0f0000;
+		pra->middle_rssi_threshold_ratr		=	0x8f0ff000;
+		pra->low_rssi_threshold_ratr		=	0x8f0ff001;
+		pra->low_rssi_threshold_ratr_40M	=	0x8f0ff005;
+		pra->low_rssi_threshold_ratr_20M	=	0x8f0ff001;
+		pra->ping_rssi_ratr	=	0x0000000d;/* cosa add for test */
+	} else if (priv->rf_type == RF_1T2R) {
+		pra->upper_rssi_threshold_ratr		=	0x000f0000;
+		pra->middle_rssi_threshold_ratr		=	0x000ff000;
+		pra->low_rssi_threshold_ratr		=	0x000ff001;
+		pra->low_rssi_threshold_ratr_40M	=	0x000ff005;
+		pra->low_rssi_threshold_ratr_20M	=	0x000ff001;
+		pra->ping_rssi_ratr	=	0x0000000d;/* cosa add for test */
+	}
+
+}	/* InitRateAdaptive */
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_check_rate_adaptive()
+ *
+ * Overview:
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/26/08	amy	Create version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------*/
+static void dm_check_rate_adaptive(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	PRT_HIGH_THROUGHPUT	pHTInfo = priv->ieee80211->pHTInfo;
+	prate_adaptive			pra = (prate_adaptive)&priv->rate_adaptive;
+	u32						currentRATR, targetRATR = 0;
+	u32						LowRSSIThreshForRA = 0, HighRSSIThreshForRA = 0;
+	bool						bshort_gi_enabled = false;
+	static u8					ping_rssi_state;
+
+	if (!priv->up) {
+		RT_TRACE(COMP_RATE, "<---- dm_check_rate_adaptive(): driver is going to unload\n");
+		return;
+	}
+
+	if (pra->rate_adaptive_disabled) /* this variable is set by ioctl. */
+		return;
+
+	/* TODO: Only 11n mode is implemented currently, */
+	if (!(priv->ieee80211->mode == WIRELESS_MODE_N_24G ||
+	      priv->ieee80211->mode == WIRELESS_MODE_N_5G))
+		return;
+
+	if (priv->ieee80211->state == IEEE80211_LINKED) {
+		/*RT_TRACE(COMP_RATE, "dm_CheckRateAdaptive(): \t");*/
+
+		/* Check whether Short GI is enabled */
+		bshort_gi_enabled = (pHTInfo->bCurTxBW40MHz && pHTInfo->bCurShortGI40MHz) ||
+			(!pHTInfo->bCurTxBW40MHz && pHTInfo->bCurShortGI20MHz);
+
+		pra->upper_rssi_threshold_ratr =
+				(pra->upper_rssi_threshold_ratr & (~BIT(31))) |
+				((bshort_gi_enabled) ? BIT(31) : 0);
+
+		pra->middle_rssi_threshold_ratr =
+				(pra->middle_rssi_threshold_ratr & (~BIT(31))) |
+				((bshort_gi_enabled) ? BIT(31) : 0);
+
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+			pra->low_rssi_threshold_ratr =
+			      (pra->low_rssi_threshold_ratr_40M & (~BIT(31))) |
+			      ((bshort_gi_enabled) ? BIT(31) : 0);
+		} else {
+			pra->low_rssi_threshold_ratr =
+			(pra->low_rssi_threshold_ratr_20M & (~BIT(31))) |
+			((bshort_gi_enabled) ? BIT(31) : 0);
+		}
+		/* cosa add for test */
+		pra->ping_rssi_ratr =
+				(pra->ping_rssi_ratr & (~BIT(31))) |
+				((bshort_gi_enabled) ? BIT(31) : 0);
+
+		/* 2007/10/08 MH We support RA smooth scheme now. When it is the first
+		 * time to link with AP. We will not change upper/lower threshold. If
+		 * STA stay in high or low level, we must change two different threshold
+		 * to prevent jumping frequently.
+		 */
+		if (pra->ratr_state == DM_RATR_STA_HIGH) {
+			HighRSSIThreshForRA	= pra->high2low_rssi_thresh_for_ra;
+			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low_rssi_thresh_for_ra40M):(pra->low_rssi_thresh_for_ra20M);
+		} else if (pra->ratr_state == DM_RATR_STA_LOW) {
+			HighRSSIThreshForRA	= pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low2high_rssi_thresh_for_ra40M):(pra->low2high_rssi_thresh_for_ra20M);
+		} else {
+			HighRSSIThreshForRA	= pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+					(pra->low_rssi_thresh_for_ra40M):(pra->low_rssi_thresh_for_ra20M);
+		}
+
+		/*DbgPrint("[DM] THresh H/L=%d/%d\n\r", RATR.HighRSSIThreshForRA, RATR.LowRSSIThreshForRA);*/
+		if (priv->undecorated_smoothed_pwdb >= (long)HighRSSIThreshForRA) {
+			/*DbgPrint("[DM] RSSI=%d STA=HIGH\n\r", pHalData->UndecoratedSmoothedPWDB);*/
+			pra->ratr_state = DM_RATR_STA_HIGH;
+			targetRATR = pra->upper_rssi_threshold_ratr;
+		} else if (priv->undecorated_smoothed_pwdb >= (long)LowRSSIThreshForRA) {
+			/*DbgPrint("[DM] RSSI=%d STA=Middle\n\r", pHalData->UndecoratedSmoothedPWDB);*/
+			pra->ratr_state = DM_RATR_STA_MIDDLE;
+			targetRATR = pra->middle_rssi_threshold_ratr;
+		} else {
+			/*DbgPrint("[DM] RSSI=%d STA=LOW\n\r", pHalData->UndecoratedSmoothedPWDB);*/
+			pra->ratr_state = DM_RATR_STA_LOW;
+			targetRATR = pra->low_rssi_threshold_ratr;
+		}
+
+		/* cosa add for test */
+		if (pra->ping_rssi_enable) {
+			/*pHalData->UndecoratedSmoothedPWDB = 19;*/
+			if (priv->undecorated_smoothed_pwdb < (long)(pra->ping_rssi_thresh_for_ra+5)) {
+				if ((priv->undecorated_smoothed_pwdb < (long)pra->ping_rssi_thresh_for_ra) ||
+					ping_rssi_state) {
+					/*DbgPrint("TestRSSI = %d, set RATR to 0x%x\n", pHalData->UndecoratedSmoothedPWDB, pRA->TestRSSIRATR);*/
+					pra->ratr_state = DM_RATR_STA_LOW;
+					targetRATR = pra->ping_rssi_ratr;
+					ping_rssi_state = 1;
+				}
+				/*else
+					DbgPrint("TestRSSI is between the range.\n");*/
+			} else {
+				/*DbgPrint("TestRSSI Recover to 0x%x\n", targetRATR);*/
+				ping_rssi_state = 0;
+			}
+		}
+
+		/* 2008.04.01
+		 * For RTL819X, if pairwisekey = wep/tkip, we support only MCS0~7.
+		 */
+		if (priv->ieee80211->GetHalfNmodeSupportByAPsHandler(dev))
+			targetRATR &= 0xf00fffff;
+
+		/* Check whether updating of RATR0 is required */
+		read_nic_dword(dev, RATR0, &currentRATR);
+		if (targetRATR !=  currentRATR) {
+			u32 ratr_value;
+
+			ratr_value = targetRATR;
+			RT_TRACE(COMP_RATE, "currentRATR = %x, targetRATR = %x\n", currentRATR, targetRATR);
+			if (priv->rf_type == RF_1T2R)
+				ratr_value &= ~(RATE_ALL_OFDM_2SS);
+			write_nic_dword(dev, RATR0, ratr_value);
+			write_nic_byte(dev, UFWP, 1);
+
+			pra->last_ratr = targetRATR;
+		}
+
+	} else {
+		pra->ratr_state = DM_RATR_STA_MAX;
+	}
+
+}	/* dm_CheckRateAdaptive */
+
+static void dm_init_bandwidth_autoswitch(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->ieee80211->bandwidth_auto_switch.threshold_20Mhzto40Mhz = BW_AUTO_SWITCH_LOW_HIGH;
+	priv->ieee80211->bandwidth_auto_switch.threshold_40Mhzto20Mhz = BW_AUTO_SWITCH_HIGH_LOW;
+	priv->ieee80211->bandwidth_auto_switch.bforced_tx20Mhz = false;
+	priv->ieee80211->bandwidth_auto_switch.bautoswitch_enable = false;
+
+}	/* dm_init_bandwidth_autoswitch */
+
+static void dm_bandwidth_autoswitch(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 || !priv->ieee80211->bandwidth_auto_switch.bautoswitch_enable)
+		return;
+	if (!priv->ieee80211->bandwidth_auto_switch.bforced_tx20Mhz) { /* If send packets in 40 Mhz in 20/40 */
+		if (priv->undecorated_smoothed_pwdb <= priv->ieee80211->bandwidth_auto_switch.threshold_40Mhzto20Mhz)
+			priv->ieee80211->bandwidth_auto_switch.bforced_tx20Mhz = true;
+	} else { /* in force send packets in 20 Mhz in 20/40 */
+		if (priv->undecorated_smoothed_pwdb >= priv->ieee80211->bandwidth_auto_switch.threshold_20Mhzto40Mhz)
+			priv->ieee80211->bandwidth_auto_switch.bforced_tx20Mhz = false;
+	}
+}	/* dm_BandwidthAutoSwitch */
+
+/* OFDM default at 0db, index=6. */
+static u32 OFDMSwingTable[OFDM_Table_Length] = {
+	0x7f8001fe,	/* 0, +6db */
+	0x71c001c7,	/* 1, +5db */
+	0x65400195,	/* 2, +4db */
+	0x5a400169,	/* 3, +3db */
+	0x50800142,	/* 4, +2db */
+	0x47c0011f,	/* 5, +1db */
+	0x40000100,	/* 6, +0db ===> default, upper for higher temperature, lower for low temperature */
+	0x390000e4,	/* 7, -1db */
+	0x32c000cb,	/* 8, -2db */
+	0x2d4000b5,	/* 9, -3db */
+	0x288000a2,	/* 10, -4db */
+	0x24000090,	/* 11, -5db */
+	0x20000080,	/* 12, -6db */
+	0x1c800072,	/* 13, -7db */
+	0x19800066,	/* 14, -8db */
+	0x26c0005b,	/* 15, -9db */
+	0x24400051,	/* 16, -10db */
+	0x12000048,	/* 17, -11db */
+	0x10000040	/* 18, -12db */
+};
+
+static u8	CCKSwingTable_Ch1_Ch13[CCK_Table_length][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},	/* 0, +0db ===> CCK40M default */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},	/* 1, -1db */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},	/* 2, -2db */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},	/* 3, -3db */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},	/* 4, -4db */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},	/* 5, -5db */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},	/* 6, -6db ===> CCK20M default */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},	/* 7, -7db */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},	/* 8, -8db */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},	/* 9, -9db */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	/* 10, -10db */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}	/* 11, -11db */
+};
+
+static u8	CCKSwingTable_Ch14[CCK_Table_length][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},	/* 0, +0db  ===> CCK40M default */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},	/* 1, -1db */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},	/* 2, -2db */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},	/* 3, -3db */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},	/* 4, -4db */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},	/* 5, -5db */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},	/* 6, -6db  ===> CCK20M default */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},	/* 7, -7db */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},	/* 8, -8db */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},	/* 9, -9db */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	/* 10, -10db */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}	/* 11, -11db */
+};
+
+static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	bool						viviflag = false;
+	struct tx_config_cmd			        tx_cmd;
+	u8						powerlevelOFDM24G;
+	int						i = 0, j = 0, k = 0;
+	u8						RF_Type, tmp_report[5] = {0, 0, 0, 0, 0};
+	u32						Value;
+	u8						Pwr_Flag;
+	u16						Avg_TSSI_Meas, TSSI_13dBm, Avg_TSSI_Meas_from_driver = 0;
+	/*RT_STATUS				rtStatus = RT_STATUS_SUCCESS;*/
+	bool rtStatus = true;
+	u32						delta = 0;
+
+	write_nic_byte(dev, 0x1ba, 0);
+
+	priv->ieee80211->bdynamic_txpower_enable = false;
+
+	powerlevelOFDM24G = (u8)(priv->Pwr_Track>>24);
+	RF_Type = priv->rf_type;
+	Value = (RF_Type<<8) | powerlevelOFDM24G;
+
+	RT_TRACE(COMP_POWER_TRACKING, "powerlevelOFDM24G = %x\n", powerlevelOFDM24G);
+
+	for (j = 0; j <= 30; j++) { /* fill tx_cmd */
+		tx_cmd.cmd_op = TXCMD_SET_TX_PWR_TRACKING;
+		tx_cmd.cmd_length = sizeof(tx_cmd.cmd_op);
+		tx_cmd.cmd_value = Value;
+		rtStatus = SendTxCommandPacket(dev, &tx_cmd, sizeof(struct tx_config_cmd));
+		if (rtStatus == RT_STATUS_FAILURE)
+			RT_TRACE(COMP_POWER_TRACKING, "Set configuration with tx cmd queue fail!\n");
+		usleep_range(1000, 2000);
+		/*DbgPrint("hi, vivi, strange\n");*/
+		for (i = 0; i <= 30; i++) {
+			read_nic_byte(dev, 0x1ba, &Pwr_Flag);
+
+			if (Pwr_Flag == 0) {
+				usleep_range(1000, 2000);
+				continue;
+			}
+			read_nic_word(dev, 0x13c, &Avg_TSSI_Meas);
+			if (Avg_TSSI_Meas == 0) {
+				write_nic_byte(dev, 0x1ba, 0);
+				break;
+			}
+
+			for (k = 0; k < 5; k++) {
+				if (k != 4)
+					read_nic_byte(dev, 0x134+k, &tmp_report[k]);
+				else
+					read_nic_byte(dev, 0x13e, &tmp_report[k]);
+				RT_TRACE(COMP_POWER_TRACKING, "TSSI_report_value = %d\n", tmp_report[k]);
+			}
+
+			/* check if the report value is right */
+			for (k = 0; k < 5; k++) {
+				if (tmp_report[k] <= 20) {
+					viviflag = true;
+					break;
+				}
+			}
+			if (viviflag) {
+				write_nic_byte(dev, 0x1ba, 0);
+				viviflag = false;
+				RT_TRACE(COMP_POWER_TRACKING, "we filtered the data\n");
+				for (k = 0; k < 5; k++)
+					tmp_report[k] = 0;
+				break;
+			}
+
+			for (k = 0; k < 5; k++)
+				Avg_TSSI_Meas_from_driver += tmp_report[k];
+
+			Avg_TSSI_Meas_from_driver = Avg_TSSI_Meas_from_driver*100/5;
+			RT_TRACE(COMP_POWER_TRACKING, "Avg_TSSI_Meas_from_driver = %d\n", Avg_TSSI_Meas_from_driver);
+			TSSI_13dBm = priv->TSSI_13dBm;
+			RT_TRACE(COMP_POWER_TRACKING, "TSSI_13dBm = %d\n", TSSI_13dBm);
+
+			/*if (abs(Avg_TSSI_Meas_from_driver - TSSI_13dBm) <= E_FOR_TX_POWER_TRACK)*/
+			/* For MacOS-compatible */
+			if (Avg_TSSI_Meas_from_driver > TSSI_13dBm)
+				delta = Avg_TSSI_Meas_from_driver - TSSI_13dBm;
+			else
+				delta = TSSI_13dBm - Avg_TSSI_Meas_from_driver;
+
+			if (delta <= E_FOR_TX_POWER_TRACK) {
+				priv->ieee80211->bdynamic_txpower_enable = true;
+				write_nic_byte(dev, 0x1ba, 0);
+				RT_TRACE(COMP_POWER_TRACKING, "tx power track is done\n");
+				RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpowertrackingindex = %d\n", priv->rfa_txpowertrackingindex);
+				RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpowertrackingindex_real = %d\n", priv->rfa_txpowertrackingindex_real);
+				RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attenuation_difference = %d\n", priv->cck_present_attenuation_difference);
+				RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attenuation = %d\n", priv->cck_present_attenuation);
+				return;
+			}
+			if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK) {
+				if (priv->rfa_txpowertrackingindex > 0) {
+					priv->rfa_txpowertrackingindex--;
+					if (priv->rfa_txpowertrackingindex_real > 4) {
+						priv->rfa_txpowertrackingindex_real--;
+						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance, bMaskDWord, priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
+					}
+				}
+			} else {
+				if (priv->rfa_txpowertrackingindex < 36) {
+					priv->rfa_txpowertrackingindex++;
+					priv->rfa_txpowertrackingindex_real++;
+					rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance, bMaskDWord, priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
+				}
+			}
+			priv->cck_present_attenuation_difference
+				= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
+
+			if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				priv->cck_present_attenuation
+					= priv->cck_present_attenuation_20Mdefault + priv->cck_present_attenuation_difference;
+			else
+				priv->cck_present_attenuation
+					= priv->cck_present_attenuation_40Mdefault + priv->cck_present_attenuation_difference;
+
+			if (priv->cck_present_attenuation > -1 && priv->cck_present_attenuation < 23) {
+				if (priv->ieee80211->current_network.channel == 14 && !priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = true;
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else if (priv->ieee80211->current_network.channel != 14 && priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = false;
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+			}
+			RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpowertrackingindex = %d\n", priv->rfa_txpowertrackingindex);
+			RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpowertrackingindex_real = %d\n", priv->rfa_txpowertrackingindex_real);
+			RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attenuation_difference = %d\n", priv->cck_present_attenuation_difference);
+			RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attenuation = %d\n", priv->cck_present_attenuation);
+
+			if (priv->cck_present_attenuation_difference <= -12 || priv->cck_present_attenuation_difference >= 24) {
+				priv->ieee80211->bdynamic_txpower_enable = true;
+				write_nic_byte(dev, 0x1ba, 0);
+				RT_TRACE(COMP_POWER_TRACKING, "tx power track--->limited\n");
+				return;
+			}
+
+			write_nic_byte(dev, 0x1ba, 0);
+			Avg_TSSI_Meas_from_driver = 0;
+			for (k = 0; k < 5; k++)
+				tmp_report[k] = 0;
+			break;
+		}
+	}
+	priv->ieee80211->bdynamic_txpower_enable = true;
+	write_nic_byte(dev, 0x1ba, 0);
+}
+
+static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
+{
+#define ThermalMeterVal	9
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 tmpRegA, TempCCk;
+	u8 tmpOFDMindex, tmpCCKindex, tmpCCK20Mindex, tmpCCK40Mindex, tmpval;
+	int i = 0, CCKSwingNeedUpdate = 0;
+
+	if (!priv->btxpower_trackingInit) {
+		/* Query OFDM default setting */
+		tmpRegA = rtl8192_QueryBBReg(dev, rOFDM0_XATxIQImbalance, bMaskDWord);
+		for (i = 0; i < OFDM_Table_Length; i++) { /* find the index */
+			if (tmpRegA == OFDMSwingTable[i]) {
+				priv->OFDM_index = (u8)i;
+				RT_TRACE(COMP_POWER_TRACKING, "Initial reg0x%x = 0x%x, OFDM_index=0x%x\n",
+					rOFDM0_XATxIQImbalance, tmpRegA, priv->OFDM_index);
+			}
+		}
+
+		/* Query CCK default setting From 0xa22 */
+		TempCCk = rtl8192_QueryBBReg(dev, rCCK0_TxFilter1, bMaskByte2);
+		for (i = 0; i < CCK_Table_length; i++) {
+			if (TempCCk == (u32)CCKSwingTable_Ch1_Ch13[i][0]) {
+				priv->CCK_index = (u8) i;
+				RT_TRACE(COMP_POWER_TRACKING, "Initial reg0x%x = 0x%x, CCK_index=0x%x\n",
+					rCCK0_TxFilter1, TempCCk, priv->CCK_index);
+				break;
+			}
+		}
+		priv->btxpower_trackingInit = true;
+		/*pHalData->TXPowercount = 0;*/
+		return;
+	}
+
+	/* ==========================
+	 * this is only for test, should be masked
+	 * ==========================
+	 */
+
+	/* read and filter out unreasonable value */
+	tmpRegA = rtl8192_phy_QueryRFReg(dev, RF90_PATH_A, 0x12, 0x078);	/* 0x12: RF Reg[10:7] */
+	RT_TRACE(COMP_POWER_TRACKING, "Readback ThermalMeterA = %d\n", tmpRegA);
+	if (tmpRegA < 3 || tmpRegA > 13)
+		return;
+	if (tmpRegA >= 12)	/* if over 12, TP will be bad when high temperature */
+		tmpRegA = 12;
+	RT_TRACE(COMP_POWER_TRACKING, "Valid ThermalMeterA = %d\n", tmpRegA);
+	priv->ThermalMeter[0] = ThermalMeterVal;	/* We use fixed value by Bryant's suggestion */
+	priv->ThermalMeter[1] = ThermalMeterVal;	/* We use fixed value by Bryant's suggestion */
+
+	/* Get current RF-A temperature index */
+	if (priv->ThermalMeter[0] >= (u8)tmpRegA) {	/* lower temperature */
+		tmpOFDMindex = tmpCCK20Mindex = 6+(priv->ThermalMeter[0]-(u8)tmpRegA);
+		tmpCCK40Mindex = tmpCCK20Mindex - 6;
+		if (tmpOFDMindex >= OFDM_Table_Length)
+			tmpOFDMindex = OFDM_Table_Length-1;
+		if (tmpCCK20Mindex >= CCK_Table_length)
+			tmpCCK20Mindex = CCK_Table_length-1;
+		if (tmpCCK40Mindex >= CCK_Table_length)
+			tmpCCK40Mindex = CCK_Table_length-1;
+	} else {
+		tmpval = (u8)tmpRegA - priv->ThermalMeter[0];
+
+		if (tmpval >= 6) {
+			/* higher temperature */
+			tmpOFDMindex = 0;
+			tmpCCK20Mindex = 0;
+		} else {
+			/* max to +6dB */
+			tmpOFDMindex = 6 - tmpval;
+			tmpCCK20Mindex = 6 - tmpval;
+		}
+		tmpCCK40Mindex = 0;
+	}
+	/*DbgPrint("%ddb, tmpOFDMindex = %d, tmpCCK20Mindex = %d, tmpCCK40Mindex = %d",
+		((u1Byte)tmpRegA - pHalData->ThermalMeter[0]),
+		tmpOFDMindex, tmpCCK20Mindex, tmpCCK40Mindex);*/
+	if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)	/* 40M */
+		tmpCCKindex = tmpCCK40Mindex;
+	else
+		tmpCCKindex = tmpCCK20Mindex;
+
+	if (priv->ieee80211->current_network.channel == 14 && !priv->bcck_in_ch14) {
+		priv->bcck_in_ch14 = true;
+		CCKSwingNeedUpdate = 1;
+	} else if (priv->ieee80211->current_network.channel != 14 && priv->bcck_in_ch14) {
+		priv->bcck_in_ch14 = false;
+		CCKSwingNeedUpdate = 1;
+	}
+
+	if (priv->CCK_index != tmpCCKindex) {
+		priv->CCK_index = tmpCCKindex;
+		CCKSwingNeedUpdate = 1;
+	}
+
+	if (CCKSwingNeedUpdate) {
+		/*DbgPrint("Update CCK Swing, CCK_index = %d\n", pHalData->CCK_index);*/
+		dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+	}
+	if (priv->OFDM_index != tmpOFDMindex) {
+		priv->OFDM_index = tmpOFDMindex;
+		rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance, bMaskDWord, OFDMSwingTable[priv->OFDM_index]);
+		RT_TRACE(COMP_POWER_TRACKING, "Update OFDMSwing[%d] = 0x%x\n",
+			priv->OFDM_index, OFDMSwingTable[priv->OFDM_index]);
+	}
+	priv->txpower_count = 0;
+}
+
+void dm_txpower_trackingcallback(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct r8192_priv *priv = container_of(dwork, struct r8192_priv, txpower_tracking_wq);
+	struct net_device *dev = priv->ieee80211->dev;
+
+	if (priv->bDcut)
+		dm_TXPowerTrackingCallback_TSSI(dev);
+	else
+		dm_TXPowerTrackingCallback_ThermalMeter(dev);
+}
+
+static void dm_InitializeTXPowerTracking_TSSI(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Initial the Tx BB index and mapping value */
+	priv->txbbgain_table[0].txbb_iq_amplifygain =			12;
+	priv->txbbgain_table[0].txbbgain_value = 0x7f8001fe;
+	priv->txbbgain_table[1].txbb_iq_amplifygain =			11;
+	priv->txbbgain_table[1].txbbgain_value = 0x788001e2;
+	priv->txbbgain_table[2].txbb_iq_amplifygain =			10;
+	priv->txbbgain_table[2].txbbgain_value = 0x71c001c7;
+	priv->txbbgain_table[3].txbb_iq_amplifygain =			9;
+	priv->txbbgain_table[3].txbbgain_value = 0x6b8001ae;
+	priv->txbbgain_table[4].txbb_iq_amplifygain =		       8;
+	priv->txbbgain_table[4].txbbgain_value = 0x65400195;
+	priv->txbbgain_table[5].txbb_iq_amplifygain =		       7;
+	priv->txbbgain_table[5].txbbgain_value = 0x5fc0017f;
+	priv->txbbgain_table[6].txbb_iq_amplifygain =		       6;
+	priv->txbbgain_table[6].txbbgain_value = 0x5a400169;
+	priv->txbbgain_table[7].txbb_iq_amplifygain =		       5;
+	priv->txbbgain_table[7].txbbgain_value = 0x55400155;
+	priv->txbbgain_table[8].txbb_iq_amplifygain =		       4;
+	priv->txbbgain_table[8].txbbgain_value = 0x50800142;
+	priv->txbbgain_table[9].txbb_iq_amplifygain =		       3;
+	priv->txbbgain_table[9].txbbgain_value = 0x4c000130;
+	priv->txbbgain_table[10].txbb_iq_amplifygain =		       2;
+	priv->txbbgain_table[10].txbbgain_value = 0x47c0011f;
+	priv->txbbgain_table[11].txbb_iq_amplifygain =		       1;
+	priv->txbbgain_table[11].txbbgain_value = 0x43c0010f;
+	priv->txbbgain_table[12].txbb_iq_amplifygain =		       0;
+	priv->txbbgain_table[12].txbbgain_value = 0x40000100;
+	priv->txbbgain_table[13].txbb_iq_amplifygain =		       -1;
+	priv->txbbgain_table[13].txbbgain_value = 0x3c8000f2;
+	priv->txbbgain_table[14].txbb_iq_amplifygain =		     -2;
+	priv->txbbgain_table[14].txbbgain_value = 0x390000e4;
+	priv->txbbgain_table[15].txbb_iq_amplifygain =		     -3;
+	priv->txbbgain_table[15].txbbgain_value = 0x35c000d7;
+	priv->txbbgain_table[16].txbb_iq_amplifygain =		     -4;
+	priv->txbbgain_table[16].txbbgain_value = 0x32c000cb;
+	priv->txbbgain_table[17].txbb_iq_amplifygain =		     -5;
+	priv->txbbgain_table[17].txbbgain_value = 0x300000c0;
+	priv->txbbgain_table[18].txbb_iq_amplifygain =			    -6;
+	priv->txbbgain_table[18].txbbgain_value = 0x2d4000b5;
+	priv->txbbgain_table[19].txbb_iq_amplifygain =		     -7;
+	priv->txbbgain_table[19].txbbgain_value = 0x2ac000ab;
+	priv->txbbgain_table[20].txbb_iq_amplifygain =		     -8;
+	priv->txbbgain_table[20].txbbgain_value = 0x288000a2;
+	priv->txbbgain_table[21].txbb_iq_amplifygain =		     -9;
+	priv->txbbgain_table[21].txbbgain_value = 0x26000098;
+	priv->txbbgain_table[22].txbb_iq_amplifygain =		     -10;
+	priv->txbbgain_table[22].txbbgain_value = 0x24000090;
+	priv->txbbgain_table[23].txbb_iq_amplifygain =		     -11;
+	priv->txbbgain_table[23].txbbgain_value = 0x22000088;
+	priv->txbbgain_table[24].txbb_iq_amplifygain =		     -12;
+	priv->txbbgain_table[24].txbbgain_value = 0x20000080;
+	priv->txbbgain_table[25].txbb_iq_amplifygain =		     -13;
+	priv->txbbgain_table[25].txbbgain_value = 0x1a00006c;
+	priv->txbbgain_table[26].txbb_iq_amplifygain =		     -14;
+	priv->txbbgain_table[26].txbbgain_value = 0x1c800072;
+	priv->txbbgain_table[27].txbb_iq_amplifygain =		     -15;
+	priv->txbbgain_table[27].txbbgain_value = 0x18000060;
+	priv->txbbgain_table[28].txbb_iq_amplifygain =		     -16;
+	priv->txbbgain_table[28].txbbgain_value = 0x19800066;
+	priv->txbbgain_table[29].txbb_iq_amplifygain =		     -17;
+	priv->txbbgain_table[29].txbbgain_value = 0x15800056;
+	priv->txbbgain_table[30].txbb_iq_amplifygain =		     -18;
+	priv->txbbgain_table[30].txbbgain_value = 0x26c0005b;
+	priv->txbbgain_table[31].txbb_iq_amplifygain =		     -19;
+	priv->txbbgain_table[31].txbbgain_value = 0x14400051;
+	priv->txbbgain_table[32].txbb_iq_amplifygain =		     -20;
+	priv->txbbgain_table[32].txbbgain_value = 0x24400051;
+	priv->txbbgain_table[33].txbb_iq_amplifygain =		     -21;
+	priv->txbbgain_table[33].txbbgain_value = 0x1300004c;
+	priv->txbbgain_table[34].txbb_iq_amplifygain =		     -22;
+	priv->txbbgain_table[34].txbbgain_value = 0x12000048;
+	priv->txbbgain_table[35].txbb_iq_amplifygain =		     -23;
+	priv->txbbgain_table[35].txbbgain_value = 0x11000044;
+	priv->txbbgain_table[36].txbb_iq_amplifygain =		     -24;
+	priv->txbbgain_table[36].txbbgain_value = 0x10000040;
+
+	/* ccktxbb_valuearray[0] is 0xA22 [1] is 0xA24 ...[7] is 0xA29
+	 * This Table is for CH1~CH13
+	 */
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[0] = 0x36;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[1] = 0x35;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[2] = 0x2e;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[3] = 0x25;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[4] = 0x1c;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[5] = 0x12;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[6] = 0x09;
+	priv->cck_txbbgain_table[0].ccktxbb_valuearray[7] = 0x04;
+
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[0] = 0x33;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[1] = 0x32;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[2] = 0x2b;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[3] = 0x23;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[4] = 0x1a;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[5] = 0x11;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[6] = 0x08;
+	priv->cck_txbbgain_table[1].ccktxbb_valuearray[7] = 0x04;
+
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[0] = 0x30;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[1] = 0x2f;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[2] = 0x29;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[3] = 0x21;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[4] = 0x19;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[5] = 0x10;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[6] = 0x08;
+	priv->cck_txbbgain_table[2].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[0] = 0x2d;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[1] = 0x2d;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[2] = 0x27;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[3] = 0x1f;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[4] = 0x18;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[5] = 0x0f;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[6] = 0x08;
+	priv->cck_txbbgain_table[3].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[0] = 0x2b;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[1] = 0x2a;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[2] = 0x25;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[3] = 0x1e;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[4] = 0x16;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[5] = 0x0e;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[6] = 0x07;
+	priv->cck_txbbgain_table[4].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[0] = 0x28;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[1] = 0x28;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[2] = 0x22;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[3] = 0x1c;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[4] = 0x15;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[5] = 0x0d;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[6] = 0x07;
+	priv->cck_txbbgain_table[5].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[0] = 0x26;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[1] = 0x25;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[2] = 0x21;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[3] = 0x1b;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[4] = 0x14;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[5] = 0x0d;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[6] = 0x06;
+	priv->cck_txbbgain_table[6].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[0] = 0x24;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[1] = 0x23;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[2] = 0x1f;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[3] = 0x19;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[4] = 0x13;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[5] = 0x0c;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[6] = 0x06;
+	priv->cck_txbbgain_table[7].ccktxbb_valuearray[7] = 0x03;
+
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[0] = 0x22;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[1] = 0x21;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[2] = 0x1d;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[3] = 0x18;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[4] = 0x11;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[5] = 0x0b;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[6] = 0x06;
+	priv->cck_txbbgain_table[8].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[0] = 0x20;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[1] = 0x20;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[2] = 0x1b;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[3] = 0x16;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[4] = 0x11;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[5] = 0x08;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[6] = 0x05;
+	priv->cck_txbbgain_table[9].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[0] = 0x1f;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[1] = 0x1e;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[2] = 0x1a;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[3] = 0x15;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[4] = 0x10;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[5] = 0x0a;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[6] = 0x05;
+	priv->cck_txbbgain_table[10].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[0] = 0x1d;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[1] = 0x1c;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[2] = 0x18;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[3] = 0x14;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[4] = 0x0f;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[5] = 0x0a;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[6] = 0x05;
+	priv->cck_txbbgain_table[11].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[0] = 0x1b;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[1] = 0x1a;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[2] = 0x17;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[3] = 0x13;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[4] = 0x0e;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[5] = 0x09;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[6] = 0x04;
+	priv->cck_txbbgain_table[12].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[0] = 0x1a;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[1] = 0x19;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[2] = 0x16;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[3] = 0x12;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[4] = 0x0d;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[5] = 0x09;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[6] = 0x04;
+	priv->cck_txbbgain_table[13].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[0] = 0x18;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[1] = 0x17;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[2] = 0x15;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[3] = 0x11;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[4] = 0x0c;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[5] = 0x08;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[6] = 0x04;
+	priv->cck_txbbgain_table[14].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[0] = 0x17;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[1] = 0x16;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[2] = 0x13;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[3] = 0x10;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[4] = 0x0c;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[5] = 0x08;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[6] = 0x04;
+	priv->cck_txbbgain_table[15].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[0] = 0x16;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[1] = 0x15;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[2] = 0x12;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[3] = 0x0f;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[4] = 0x0b;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[5] = 0x07;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[6] = 0x04;
+	priv->cck_txbbgain_table[16].ccktxbb_valuearray[7] = 0x01;
+
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[0] = 0x14;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[1] = 0x14;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[2] = 0x11;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[3] = 0x0e;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[4] = 0x0b;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[5] = 0x07;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[17].ccktxbb_valuearray[7] = 0x02;
+
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[0] = 0x13;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[1] = 0x13;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[2] = 0x10;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[3] = 0x0d;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[4] = 0x0a;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[5] = 0x06;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[18].ccktxbb_valuearray[7] = 0x01;
+
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[0] = 0x12;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[1] = 0x12;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[2] = 0x0f;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[3] = 0x0c;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[4] = 0x09;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[5] = 0x06;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[19].ccktxbb_valuearray[7] = 0x01;
+
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[0] = 0x11;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[1] = 0x11;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[2] = 0x0f;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[3] = 0x0c;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[4] = 0x09;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[5] = 0x06;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[20].ccktxbb_valuearray[7] = 0x01;
+
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[0] = 0x10;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[1] = 0x10;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[2] = 0x0e;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[3] = 0x0b;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[4] = 0x08;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[5] = 0x05;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[21].ccktxbb_valuearray[7] = 0x01;
+
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[0] = 0x0f;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[1] = 0x0f;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[2] = 0x0d;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[3] = 0x0b;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[4] = 0x08;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[5] = 0x05;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[6] = 0x03;
+	priv->cck_txbbgain_table[22].ccktxbb_valuearray[7] = 0x01;
+
+	/* ccktxbb_valuearray[0] is 0xA22 [1] is 0xA24 ...[7] is 0xA29
+	 * This Table is for CH14
+	 */
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[0] = 0x36;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[1] = 0x35;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[2] = 0x2e;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[3] = 0x1b;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[0] = 0x33;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[1] = 0x32;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[2] = 0x2b;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[3] = 0x19;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[0] = 0x30;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[1] = 0x2f;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[2] = 0x29;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[3] = 0x18;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[0] = 0x2d;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[1] = 0x2d;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[2] = 0x27;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[3] = 0x17;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[0] = 0x2b;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[1] = 0x2a;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[2] = 0x25;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[3] = 0x15;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[0] = 0x28;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[1] = 0x28;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[2] = 0x22;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[3] = 0x14;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[0] = 0x26;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[1] = 0x25;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[2] = 0x21;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[3] = 0x13;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[0] = 0x24;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[1] = 0x23;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[2] = 0x1f;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[3] = 0x12;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[0] = 0x22;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[1] = 0x21;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[2] = 0x1d;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[3] = 0x11;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[0] = 0x20;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[1] = 0x20;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[2] = 0x1b;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[3] = 0x10;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[0] = 0x1f;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[1] = 0x1e;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[2] = 0x1a;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[3] = 0x0f;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[0] = 0x1d;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[1] = 0x1c;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[2] = 0x18;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[3] = 0x0e;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[0] = 0x1b;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[1] = 0x1a;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[2] = 0x17;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[3] = 0x0e;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[0] = 0x1a;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[1] = 0x19;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[2] = 0x16;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[3] = 0x0d;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[0] = 0x18;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[1] = 0x17;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[2] = 0x15;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[3] = 0x0c;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[0] = 0x17;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[1] = 0x16;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[2] = 0x13;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[3] = 0x0b;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[0] = 0x16;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[1] = 0x15;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[2] = 0x12;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[3] = 0x0b;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[0] = 0x14;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[1] = 0x14;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[2] = 0x11;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[3] = 0x0a;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[0] = 0x13;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[1] = 0x13;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[2] = 0x10;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[3] = 0x0a;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[0] = 0x12;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[1] = 0x12;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[2] = 0x0f;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[3] = 0x09;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[0] = 0x11;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[1] = 0x11;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[2] = 0x0f;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[3] = 0x09;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[0] = 0x10;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[1] = 0x10;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[2] = 0x0e;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[3] = 0x08;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[7] = 0x00;
+
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[0] = 0x0f;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[1] = 0x0f;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[2] = 0x0d;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[3] = 0x08;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[4] = 0x00;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[5] = 0x00;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[6] = 0x00;
+	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[7] = 0x00;
+
+	priv->btxpower_tracking = true;
+	priv->txpower_count       = 0;
+	priv->btxpower_trackingInit = false;
+}
+
+static void dm_InitializeTXPowerTracking_ThermalMeter(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Tx Power tracking by Thermal Meter requires Firmware R/W 3-wire. This mechanism
+	 * can be enabled only when Firmware R/W 3-wire is enabled. Otherwise, frequent r/w
+	 * 3-wire by driver causes RF to go into a wrong state.
+	 */
+	if (priv->ieee80211->FwRWRF)
+		priv->btxpower_tracking = true;
+	else
+		priv->btxpower_tracking = false;
+	priv->txpower_count       = 0;
+	priv->btxpower_trackingInit = false;
+}
+
+void dm_initialize_txpower_tracking(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->bDcut)
+		dm_InitializeTXPowerTracking_TSSI(dev);
+	else
+		dm_InitializeTXPowerTracking_ThermalMeter(dev);
+} /* dm_InitializeTXPowerTracking */
+
+static void dm_CheckTXPowerTracking_TSSI(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u32 tx_power_track_counter;
+
+	if (!priv->btxpower_tracking)
+		return;
+	if ((tx_power_track_counter % 30 == 0) && (tx_power_track_counter != 0))
+		queue_delayed_work(priv->priv_wq, &priv->txpower_tracking_wq, 0);
+	tx_power_track_counter++;
+}
+
+static void dm_CheckTXPowerTracking_ThermalMeter(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u8	TM_Trigger;
+	/*DbgPrint("dm_CheckTXPowerTracking()\n");*/
+	if (!priv->btxpower_tracking)
+		return;
+	if (priv->txpower_count  <= 2) {
+		priv->txpower_count++;
+		return;
+	}
+
+	if (!TM_Trigger) {
+		/* Attention!! You have to write all 12bits of data to RF, or it may cause RF to crash
+		 * actually write reg0x02 bit1=0, then bit1=1.
+		 * DbgPrint("Trigger ThermalMeter, write RF reg0x2 = 0x4d to 0x4f\n");
+		 */
+		rtl8192_phy_SetRFReg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4d);
+		rtl8192_phy_SetRFReg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4f);
+		rtl8192_phy_SetRFReg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4d);
+		rtl8192_phy_SetRFReg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4f);
+		TM_Trigger = 1;
+		return;
+	}
+	/*DbgPrint("Schedule TxPowerTrackingWorkItem\n");*/
+	queue_delayed_work(priv->priv_wq, &priv->txpower_tracking_wq, 0);
+	TM_Trigger = 0;
+}
+
+static void dm_check_txpower_tracking(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/*static u32 tx_power_track_counter = 0;*/
+
+#ifdef RTL8190P
+	dm_CheckTXPowerTracking_TSSI(dev);
+#else
+	if (priv->bDcut)
+		dm_CheckTXPowerTracking_TSSI(dev);
+	else
+		dm_CheckTXPowerTracking_ThermalMeter(dev);
+#endif
+
+}	/* dm_CheckTXPowerTracking */
+
+static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
+{
+	u32 TempVal;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Write 0xa22 0xa23 */
+	TempVal = 0;
+	if (!bInCH14) {
+		/* Write 0xa22 0xa23 */
+		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[0] +
+					(priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[1]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		/* Write 0xa24 ~ 0xa27 */
+		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[2] +
+					(priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[3]<<8) +
+					(priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[4]<<16)+
+					(priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[5]<<24);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		/* Write 0xa28  0xa29 */
+		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[6] +
+					(priv->cck_txbbgain_table[priv->cck_present_attenuation].ccktxbb_valuearray[7]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	} else {
+		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[0] +
+					(priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[1]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		/* Write 0xa24 ~ 0xa27 */
+		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[2] +
+					(priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[3]<<8) +
+					(priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[4]<<16)+
+					(priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[5]<<24);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		/* Write 0xa28  0xa29 */
+		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[6] +
+					(priv->cck_txbbgain_ch14_table[priv->cck_present_attenuation].ccktxbb_valuearray[7]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+	}
+}
+
+static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev, bool  bInCH14)
+{
+	u32 TempVal;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	TempVal = 0;
+	if (!bInCH14) {
+		/* Write 0xa22 0xa23 */
+		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][0] +
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][1]<<8);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
+			rCCK0_TxFilter1, TempVal);
+		/* Write 0xa24 ~ 0xa27 */
+		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][3]<<8) +
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][4]<<16)+
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][5]<<24);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
+			rCCK0_TxFilter2, TempVal);
+		/* Write 0xa28  0xa29 */
+		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
+			rCCK0_DebugPort, TempVal);
+	} else {
+		/*priv->CCKTxPowerAdjustCntNotCh14++;	cosa add for debug.*/
+		/* Write 0xa22 0xa23 */
+		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][0] +
+					(CCKSwingTable_Ch14[priv->CCK_index][1]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
+			 rCCK0_TxFilter1, TempVal);
+		/* Write 0xa24 ~ 0xa27 */
+		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][2] +
+					(CCKSwingTable_Ch14[priv->CCK_index][3]<<8) +
+					(CCKSwingTable_Ch14[priv->CCK_index][4]<<16)+
+					(CCKSwingTable_Ch14[priv->CCK_index][5]<<24);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
+			 rCCK0_TxFilter2, TempVal);
+		/* Write 0xa28  0xa29 */
+		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][6] +
+					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8);
+
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
+			 rCCK0_DebugPort, TempVal);
+	}
+}
+
+void dm_cck_txpower_adjust(struct net_device *dev, bool binch14)
+{	/*  dm_CCKTxPowerAdjust */
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->bDcut)
+		dm_CCKTxPowerAdjust_TSSI(dev, binch14);
+	else
+		dm_CCKTxPowerAdjust_ThermalMeter(dev, binch14);
+}
+
+#ifndef RTL8192U
+static void dm_txpower_reset_recovery(
+	struct net_device *dev
+)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	RT_TRACE(COMP_POWER_TRACKING, "Start Reset Recovery ==>\n");
+	rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance, bMaskDWord, priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbbgain_value);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in 0xc80 is %08x\n", priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbbgain_value);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in RFA_txPowerTrackingIndex is %x\n", priv->rfa_txpowertrackingindex);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery : RF A I/Q Amplify Gain is %ld\n", priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbb_iq_amplifygain);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: CCK Attenuation is %d dB\n", priv->cck_present_attenuation);
+	dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+
+	rtl8192_setBBreg(dev, rOFDM0_XCTxIQImbalance, bMaskDWord, priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in 0xc90 is %08x\n", priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in RFC_txPowerTrackingIndex is %x\n", priv->rfc_txpowertrackingindex);
+	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery : RF C I/Q Amplify Gain is %ld\n", priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbb_iq_amplifygain);
+
+}	/* dm_TXPowerResetRecovery */
+
+void dm_restore_dynamic_mechanism_state(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32	reg_ratr = priv->rate_adaptive.last_ratr;
+
+	if (!priv->up) {
+		RT_TRACE(COMP_RATE, "<---- dm_restore_dynamic_mechanism_state(): driver is going to unload\n");
+		return;
+	}
+
+	/* Restore previous state for rate adaptive */
+	if (priv->rate_adaptive.rate_adaptive_disabled)
+		return;
+	/* TODO: Only 11n mode is implemented currently, */
+	if (!(priv->ieee80211->mode == WIRELESS_MODE_N_24G ||
+	      priv->ieee80211->mode == WIRELESS_MODE_N_5G))
+		return;
+
+	{
+			/* 2007/11/15 MH Copy from 8190PCI. */
+			u32 ratr_value;
+
+			ratr_value = reg_ratr;
+			if (priv->rf_type == RF_1T2R) {	/* 1T2R, Spatial Stream 2 should be disabled */
+				ratr_value &= ~(RATE_ALL_OFDM_2SS);
+				/*DbgPrint("HW_VAR_TATR_0 from 0x%x ==> 0x%x\n", ((pu4Byte)(val))[0], ratr_value);*/
+			}
+			/*DbgPrint("set HW_VAR_TATR_0 = 0x%x\n", ratr_value);*/
+			/*cosa PlatformEFIOWrite4Byte(Adapter, RATR0, ((pu4Byte)(val))[0]);*/
+			write_nic_dword(dev, RATR0, ratr_value);
+			write_nic_byte(dev, UFWP, 1);
+	}
+	/* Restore TX Power Tracking Index */
+	if (priv->btxpower_trackingInit && priv->btxpower_tracking)
+		dm_txpower_reset_recovery(dev);
+
+	/* Restore BB Initial Gain */
+	dm_bb_initialgain_restore(dev);
+
+}	/* DM_RestoreDynamicMechanismState */
+
+static void dm_bb_initialgain_restore(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 bit_mask = 0x7f; /* Bit0~ Bit6 */
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		return;
+
+	/* Disable Initial Gain */
+	/*PHY_SetBBReg(Adapter, UFWP, bMaskLWord, 0x800);*/
+	rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);	/* Only clear byte 1 and rewrite. */
+	rtl8192_setBBreg(dev, rOFDM0_XAAGCCore1, bit_mask, (u32)priv->initgain_backup.xaagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XBAGCCore1, bit_mask, (u32)priv->initgain_backup.xbagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XCAGCCore1, bit_mask, (u32)priv->initgain_backup.xcagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XDAGCCore1, bit_mask, (u32)priv->initgain_backup.xdagccore1);
+	bit_mask  = bMaskByte2;
+	rtl8192_setBBreg(dev, rCCK0_CCA, bit_mask, (u32)priv->initgain_backup.cca);
+
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc50 is %x\n", priv->initgain_backup.xaagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc58 is %x\n", priv->initgain_backup.xbagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc60 is %x\n", priv->initgain_backup.xcagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc68 is %x\n", priv->initgain_backup.xdagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xa0a is %x\n", priv->initgain_backup.cca);
+	/* Enable Initial Gain */
+	/*PHY_SetBBReg(Adapter, UFWP, bMaskLWord, 0x100);*/
+	rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);	/* Only clear byte 1 and rewrite. */
+
+}	/* dm_BBInitialGainRestore */
+
+static void dm_bb_initialgain_backup(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 bit_mask = bMaskByte0; /* Bit0~ Bit6 */
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		return;
+
+	/*PHY_SetBBReg(Adapter, UFWP, bMaskLWord, 0x800);*/
+	rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);	/* Only clear byte 1 and rewrite. */
+	priv->initgain_backup.xaagccore1 = (u8)rtl8192_QueryBBReg(dev, rOFDM0_XAAGCCore1, bit_mask);
+	priv->initgain_backup.xbagccore1 = (u8)rtl8192_QueryBBReg(dev, rOFDM0_XBAGCCore1, bit_mask);
+	priv->initgain_backup.xcagccore1 = (u8)rtl8192_QueryBBReg(dev, rOFDM0_XCAGCCore1, bit_mask);
+	priv->initgain_backup.xdagccore1 = (u8)rtl8192_QueryBBReg(dev, rOFDM0_XDAGCCore1, bit_mask);
+	bit_mask  = bMaskByte2;
+	priv->initgain_backup.cca = (u8)rtl8192_QueryBBReg(dev, rCCK0_CCA, bit_mask);
+
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc50 is %x\n", priv->initgain_backup.xaagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc58 is %x\n", priv->initgain_backup.xbagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc60 is %x\n", priv->initgain_backup.xcagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc68 is %x\n", priv->initgain_backup.xdagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xa0a is %x\n", priv->initgain_backup.cca);
+
+}   /* dm_BBInitialGainBakcup */
+
+#endif
+/*-----------------------------------------------------------------------------
+ * Function:	dm_dig_init()
+ *
+ * Overview:	Set DIG scheme init value.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/15/2008	amy		Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------*/
+static void dm_dig_init(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/* 2007/10/05 MH Disable DIG scheme now. Not tested. */
+	dm_digtable.dig_enable_flag	= true;
+	dm_digtable.dig_algorithm = DIG_ALGO_BY_RSSI;
+	dm_digtable.dig_algorithm_switch = 0;
+
+	/* 2007/10/04 MH Define init gain threshold. */
+	dm_digtable.dig_state		= DM_STA_DIG_MAX;
+	dm_digtable.dig_highpwr_state	= DM_STA_DIG_MAX;
+
+	dm_digtable.rssi_low_thresh	= DM_DIG_THRESH_LOW;
+	dm_digtable.rssi_high_thresh	= DM_DIG_THRESH_HIGH;
+
+	dm_digtable.rssi_high_power_lowthresh = DM_DIG_HIGH_PWR_THRESH_LOW;
+	dm_digtable.rssi_high_power_highthresh = DM_DIG_HIGH_PWR_THRESH_HIGH;
+
+	dm_digtable.rssi_val = 50;	/* for new dig debug rssi value */
+	dm_digtable.backoff_val = DM_DIG_BACKOFF;
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		dm_digtable.rx_gain_range_min = DM_DIG_MIN_NETCORE;
+	else
+		dm_digtable.rx_gain_range_min = DM_DIG_MIN;
+
+}	/* dm_dig_init */
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_ctrl_initgain_byrssi()
+ *
+ * Overview:	Driver must monitor RSSI and notify firmware to change initial
+ *				gain according to different threshold. BB team provide the
+ *				suggested solution.
+ *
+ * Input:			struct net_device *dev
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/27/2008	amy		Create Version 0 porting from windows code.
+ *---------------------------------------------------------------------------*/
+static void dm_ctrl_initgain_byrssi(struct net_device *dev)
+{
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
+		dm_ctrl_initgain_byrssi_by_fwfalse_alarm(dev);
+	else if (dm_digtable.dig_algorithm == DIG_ALGO_BY_RSSI)
+		dm_ctrl_initgain_byrssi_by_driverrssi(dev);
+	/* ; */
+	else
+		return;
+}
+
+static void dm_ctrl_initgain_byrssi_by_driverrssi(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 i;
+	static u8	fw_dig;
+
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	/*DbgPrint("Dig by Sw Rssi\n");*/
+	if (dm_digtable.dig_algorithm_switch)	/* if switched algorithm, we have to disable FW Dig. */
+		fw_dig = 0;
+
+	if (fw_dig <= 3) { /* execute several times to make sure the FW Dig is disabled */
+		/* FW DIG Off */
+		for (i = 0; i < 3; i++)
+			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);	/* Only clear byte 1 and rewrite. */
+		fw_dig++;
+		dm_digtable.dig_state = DM_STA_DIG_OFF;	/* fw dig off. */
+	}
+
+	if (priv->ieee80211->state == IEEE80211_LINKED)
+		dm_digtable.cur_connect_state = DIG_CONNECT;
+	else
+		dm_digtable.cur_connect_state = DIG_DISCONNECT;
+
+	/*DbgPrint("DM_DigTable.PreConnectState = %d, DM_DigTable.CurConnectState = %d\n",
+		DM_DigTable.PreConnectState, DM_DigTable.CurConnectState);*/
+
+	dm_digtable.rssi_val = priv->undecorated_smoothed_pwdb;
+	/*DbgPrint("DM_DigTable.Rssi_val = %d\n", DM_DigTable.Rssi_val);*/
+	dm_initial_gain(dev);
+	dm_pd_th(dev);
+	dm_cs_ratio(dev);
+	if (dm_digtable.dig_algorithm_switch)
+		dm_digtable.dig_algorithm_switch = 0;
+	dm_digtable.pre_connect_state = dm_digtable.cur_connect_state;
+
+}	/* dm_CtrlInitGainByRssi */
+
+static void dm_ctrl_initgain_byrssi_by_fwfalse_alarm(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u32 reset_cnt;
+	u8 i;
+
+	if (!dm_digtable.dig_enable_flag)
+		return;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		dm_digtable.dig_state = DM_STA_DIG_MAX;
+		/* Fw DIG On. */
+		for (i = 0; i < 3; i++)
+			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);	/* Only clear byte 1 and rewrite.*/
+		dm_digtable.dig_algorithm_switch = 0;
+	}
+
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		return;
+
+	/* For smooth, we can not change DIG state. */
+	if ((priv->undecorated_smoothed_pwdb > dm_digtable.rssi_low_thresh) &&
+	    (priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_thresh))
+		return;
+
+	/*DbgPrint("Dig by Fw False Alarm\n");*/
+	/*if (DM_DigTable.Dig_State == DM_STA_DIG_OFF)*/
+	/*DbgPrint("DIG Check\n\r RSSI=%d LOW=%d HIGH=%d STATE=%d",
+	pHalData->UndecoratedSmoothedPWDB, DM_DigTable.RssiLowThresh,
+	DM_DigTable.RssiHighThresh, DM_DigTable.Dig_State);*/
+	/* 1. When RSSI decrease, We have to judge if it is smaller than a threshold
+	 * and then execute the step below.
+	 */
+	if (priv->undecorated_smoothed_pwdb <= dm_digtable.rssi_low_thresh) {
+		/* 2008/02/05 MH When we execute silent reset, the DIG PHY parameters
+		 * will be reset to init value. We must prevent the condition.
+		 */
+		if (dm_digtable.dig_state == DM_STA_DIG_OFF &&
+		    (priv->reset_count == reset_cnt)) {
+			return;
+		}
+		reset_cnt = priv->reset_count;
+
+		/* If DIG is off, DIG high power state must reset. */
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_MAX;
+		dm_digtable.dig_state = DM_STA_DIG_OFF;
+
+		/*  1.1 DIG Off. */
+		rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);	/*  Only clear byte 1 and rewrite. */
+
+		/*  1.2 Set initial gain. */
+		write_nic_byte(dev, rOFDM0_XAAGCCore1, 0x17);
+		write_nic_byte(dev, rOFDM0_XBAGCCore1, 0x17);
+		write_nic_byte(dev, rOFDM0_XCAGCCore1, 0x17);
+		write_nic_byte(dev, rOFDM0_XDAGCCore1, 0x17);
+
+		/*  1.3 Lower PD_TH for OFDM. */
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+			/* 2008/01/11 MH 40MHZ 90/92 register are not the same.
+			 * 2008/02/05 MH SD3-Jerry 92U/92E PD_TH are the same.
+			 */
+			write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x00);
+			/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+				write_nic_byte(pAdapter, rOFDM0_RxDetector1, 0x40);
+			else if (pAdapter->HardwareType == HARDWARE_TYPE_RTL8192E)
+			else
+				PlatformEFIOWrite1Byte(pAdapter, rOFDM0_RxDetector1, 0x40);
+			*/
+		} else
+			write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
+
+		/* 1.4 Lower CS ratio for CCK. */
+		write_nic_byte(dev, 0xa0a, 0x08);
+
+		/* 1.5 Higher EDCCA. */
+		/*PlatformEFIOWrite4Byte(pAdapter, rOFDM0_ECCAThreshold, 0x325);*/
+		return;
+	}
+
+	/* 2. When RSSI increase, We have to judge if it is larger than a threshold
+	 * and then execute the step below.
+	 */
+	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
+		u8 reset_flag = 0;
+
+		if (dm_digtable.dig_state == DM_STA_DIG_ON &&
+		    (priv->reset_count == reset_cnt)) {
+			dm_ctrl_initgain_byrssi_highpwr(dev);
+			return;
+		}
+		if (priv->reset_count != reset_cnt)
+			reset_flag = 1;
+
+		reset_cnt = priv->reset_count;
+
+		dm_digtable.dig_state = DM_STA_DIG_ON;
+		/*DbgPrint("DIG ON\n\r");*/
+
+		/* 2.1 Set initial gain.
+		 * 2008/02/26 MH SD3-Jerry suggest to prevent dirty environment.
+		 */
+		if (reset_flag == 1) {
+			write_nic_byte(dev, rOFDM0_XAAGCCore1, 0x2c);
+			write_nic_byte(dev, rOFDM0_XBAGCCore1, 0x2c);
+			write_nic_byte(dev, rOFDM0_XCAGCCore1, 0x2c);
+			write_nic_byte(dev, rOFDM0_XDAGCCore1, 0x2c);
+		} else {
+			write_nic_byte(dev, rOFDM0_XAAGCCore1, 0x20);
+			write_nic_byte(dev, rOFDM0_XBAGCCore1, 0x20);
+			write_nic_byte(dev, rOFDM0_XCAGCCore1, 0x20);
+			write_nic_byte(dev, rOFDM0_XDAGCCore1, 0x20);
+		}
+
+		/* 2.2 Higher PD_TH for OFDM. */
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+			/* 2008/01/11 MH 40MHZ 90/92 register are not the same.
+			 * 2008/02/05 MH SD3-Jerry 92U/92E PD_TH are the same.
+			 */
+			write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x20);
+			/*
+			else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+				write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
+			else if (pAdapter->HardwareType == HARDWARE_TYPE_RTL8192E)
+			else
+				PlatformEFIOWrite1Byte(pAdapter, rOFDM0_RxDetector1, 0x42);
+			*/
+		} else
+			write_nic_byte(dev, rOFDM0_RxDetector1, 0x44);
+
+		/* 2.3 Higher CS ratio for CCK. */
+		write_nic_byte(dev, 0xa0a, 0xcd);
+
+		/* 2.4 Lower EDCCA.
+		 * 2008/01/11 MH 90/92 series are the same.
+		 */
+		/*PlatformEFIOWrite4Byte(pAdapter, rOFDM0_ECCAThreshold, 0x346);*/
+
+		/* 2.5 DIG On. */
+		rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);	/*  Only clear byte 1 and rewrite. */
+	}
+
+	dm_ctrl_initgain_byrssi_highpwr(dev);
+
+}	/* dm_CtrlInitGainByRssi */
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_ctrl_initgain_byrssi_highpwr()
+ *
+ * Overview:
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/28/2008	amy		Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------*/
+static void dm_ctrl_initgain_byrssi_highpwr(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u32 reset_cnt_highpwr;
+
+	/*  For smooth, we can not change high power DIG state in the range. */
+	if ((priv->undecorated_smoothed_pwdb > dm_digtable.rssi_high_power_lowthresh) &&
+		(priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_power_highthresh))
+		return;
+
+	/* 3. When RSSI >75% or <70%, it is a high power issue. We have to judge if
+	 *    it is larger than a threshold and then execute the step below.
+	 *
+	 * 2008/02/05 MH SD3-Jerry Modify PD_TH for high power issue.
+	 */
+	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_power_highthresh) {
+		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_ON &&
+		    (priv->reset_count == reset_cnt_highpwr))
+			return;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_ON;
+
+		/* 3.1 Higher PD_TH for OFDM for high power state. */
+		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+			write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x10);
+
+			/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+				write_nic_byte(dev, rOFDM0_RxDetector1, 0x41);
+			*/
+
+		} else
+			write_nic_byte(dev, rOFDM0_RxDetector1, 0x43);
+	} else {
+		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_OFF &&
+		    (priv->reset_count == reset_cnt_highpwr))
+			return;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_OFF;
+
+		if (priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_power_lowthresh &&
+			 priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
+			/*  3.2 Recover PD_TH for OFDM for normal power region. */
+			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+				write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x20);
+				/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+					write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
+				*/
+
+			} else
+				write_nic_byte(dev, rOFDM0_RxDetector1, 0x44);
+		}
+	}
+
+	reset_cnt_highpwr = priv->reset_count;
+
+}	/* dm_CtrlInitGainByRssiHighPwr */
+
+static void dm_initial_gain(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8					initial_gain = 0;
+	static u8				initialized, force_write;
+	static u32			reset_cnt;
+	u8				tmp;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (dm_digtable.pre_connect_state == dm_digtable.cur_connect_state) {
+		if (dm_digtable.cur_connect_state == DIG_CONNECT) {
+			if ((dm_digtable.rssi_val + 10 - dm_digtable.backoff_val) > DM_DIG_MAX)
+				dm_digtable.cur_ig_value = DM_DIG_MAX;
+			else if ((dm_digtable.rssi_val+10-dm_digtable.backoff_val) < dm_digtable.rx_gain_range_min)
+				dm_digtable.cur_ig_value = dm_digtable.rx_gain_range_min;
+			else
+				dm_digtable.cur_ig_value = dm_digtable.rssi_val+10-dm_digtable.backoff_val;
+		} else {	/* current state is disconnected */
+			if (dm_digtable.cur_ig_value == 0)
+				dm_digtable.cur_ig_value = priv->DefaultInitialGain[0];
+			else
+				dm_digtable.cur_ig_value = dm_digtable.pre_ig_value;
+		}
+	} else { /*  disconnected -> connected or connected -> disconnected */
+		dm_digtable.cur_ig_value = priv->DefaultInitialGain[0];
+		dm_digtable.pre_ig_value = 0;
+	}
+	/*DbgPrint("DM_DigTable.CurIGValue = 0x%x, DM_DigTable.PreIGValue = 0x%x\n", DM_DigTable.CurIGValue, DM_DigTable.PreIGValue);*/
+
+	/* if silent reset happened, we should rewrite the values back */
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+	read_nic_byte(dev, rOFDM0_XAAGCCore1, &tmp);
+	if (dm_digtable.pre_ig_value != tmp)
+		force_write = 1;
+
+	{
+		if ((dm_digtable.pre_ig_value != dm_digtable.cur_ig_value)
+			|| !initialized || force_write) {
+			initial_gain = (u8)dm_digtable.cur_ig_value;
+			/*DbgPrint("Write initial gain = 0x%x\n", initial_gain);*/
+			/*  Set initial gain. */
+			write_nic_byte(dev, rOFDM0_XAAGCCore1, initial_gain);
+			write_nic_byte(dev, rOFDM0_XBAGCCore1, initial_gain);
+			write_nic_byte(dev, rOFDM0_XCAGCCore1, initial_gain);
+			write_nic_byte(dev, rOFDM0_XDAGCCore1, initial_gain);
+			dm_digtable.pre_ig_value = dm_digtable.cur_ig_value;
+			initialized = 1;
+			force_write = 0;
+		}
+	}
+}
+
+static void dm_pd_th(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u8				initialized, force_write;
+	static u32			reset_cnt;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (dm_digtable.pre_connect_state == dm_digtable.cur_connect_state) {
+		if (dm_digtable.cur_connect_state == DIG_CONNECT) {
+			if (dm_digtable.rssi_val >= dm_digtable.rssi_high_power_highthresh)
+				dm_digtable.curpd_thstate = DIG_PD_AT_HIGH_POWER;
+			else if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
+				dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
+			else if ((dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh) &&
+					(dm_digtable.rssi_val < dm_digtable.rssi_high_power_lowthresh))
+				dm_digtable.curpd_thstate = DIG_PD_AT_NORMAL_POWER;
+			else
+				dm_digtable.curpd_thstate = dm_digtable.prepd_thstate;
+		} else {
+			dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
+		}
+	} else { /* disconnected -> connected or connected -> disconnected */
+		dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
+	}
+
+	/*  if silent reset happened, we should rewrite the values back */
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+	{
+		if ((dm_digtable.prepd_thstate != dm_digtable.curpd_thstate) ||
+		    (initialized <= 3) || force_write) {
+			/*DbgPrint("Write PD_TH state = %d\n", DM_DigTable.CurPD_THState);*/
+			if (dm_digtable.curpd_thstate == DIG_PD_AT_LOW_POWER) {
+				/*  Lower PD_TH for OFDM. */
+				if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+					/* 2008/01/11 MH 40MHZ 90/92 register are not the same.
+					 * 2008/02/05 MH SD3-Jerry 92U/92E PD_TH are the same.
+					 */
+					write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x00);
+					/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+						write_nic_byte(dev, rOFDM0_RxDetector1, 0x40);
+					*/
+				} else
+					write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
+			} else if (dm_digtable.curpd_thstate == DIG_PD_AT_NORMAL_POWER) {
+				/* Higher PD_TH for OFDM. */
+				if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+					/* 2008/01/11 MH 40MHZ 90/92 register are not the same.
+					 * 2008/02/05 MH SD3-Jerry 92U/92E PD_TH are the same.
+					 */
+					write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x20);
+					/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+						write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
+					*/
+				} else
+					write_nic_byte(dev, rOFDM0_RxDetector1, 0x44);
+			} else if (dm_digtable.curpd_thstate == DIG_PD_AT_HIGH_POWER) {
+				/* Higher PD_TH for OFDM for high power state. */
+				if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
+					write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x10);
+					/*else if (priv->card_8192 == HARDWARE_TYPE_RTL8190P)
+						write_nic_byte(dev, rOFDM0_RxDetector1, 0x41);
+					*/
+				} else
+					write_nic_byte(dev, rOFDM0_RxDetector1, 0x43);
+			}
+			dm_digtable.prepd_thstate = dm_digtable.curpd_thstate;
+			if (initialized <= 3)
+				initialized++;
+			force_write = 0;
+		}
+	}
+}
+
+static	void dm_cs_ratio(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	static u8				initialized, force_write;
+	static u32			reset_cnt;
+
+	if (dm_digtable.dig_algorithm_switch) {
+		initialized = 0;
+		reset_cnt = 0;
+	}
+
+	if (dm_digtable.pre_connect_state == dm_digtable.cur_connect_state) {
+		if (dm_digtable.cur_connect_state == DIG_CONNECT) {
+			if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
+				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+			else if (dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh)
+				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_HIGHER;
+			else
+				dm_digtable.curcs_ratio_state = dm_digtable.precs_ratio_state;
+		} else {
+			dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+		}
+	} else	/* disconnected -> connected or connected -> disconnected */
+		dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
+
+	/* if silent reset happened, we should rewrite the values back */
+	if (priv->reset_count != reset_cnt) {
+		force_write = 1;
+		reset_cnt = priv->reset_count;
+	}
+
+	{
+		if ((dm_digtable.precs_ratio_state != dm_digtable.curcs_ratio_state) ||
+		    !initialized || force_write) {
+			/*DbgPrint("Write CS_ratio state = %d\n", DM_DigTable.CurCS_ratioState);*/
+			if (dm_digtable.curcs_ratio_state == DIG_CS_RATIO_LOWER) {
+				/*  Lower CS ratio for CCK. */
+				write_nic_byte(dev, 0xa0a, 0x08);
+			} else if (dm_digtable.curcs_ratio_state == DIG_CS_RATIO_HIGHER) {
+				/*  Higher CS ratio for CCK. */
+				write_nic_byte(dev, 0xa0a, 0xcd);
+			}
+			dm_digtable.precs_ratio_state = dm_digtable.curcs_ratio_state;
+			initialized = 1;
+			force_write = 0;
+		}
+	}
+}
+
+void dm_init_edca_turbo(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->bcurrent_turbo_EDCA = false;
+	priv->ieee80211->bis_any_nonbepkts = false;
+	priv->bis_cur_rdlstate = false;
+}	/* dm_init_edca_turbo */
+
+static void dm_check_edca_turbo(
+	struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	PRT_HIGH_THROUGHPUT	pHTInfo = priv->ieee80211->pHTInfo;
+	/*PSTA_QOS			pStaQos = pMgntInfo->pStaQos;*/
+
+	/* Keep past Tx/Rx packet count for RT-to-RT EDCA turbo. */
+	static unsigned long			lastTxOkCnt;
+	static unsigned long			lastRxOkCnt;
+	unsigned long				curTxOkCnt = 0;
+	unsigned long				curRxOkCnt = 0;
+
+	/* Do not be Turbo if it's under WiFi config and Qos Enabled, because the EDCA parameters
+	 * should follow the settings from QAP. By Bruce, 2007-12-07.
+	 */
+	if (priv->ieee80211->state != IEEE80211_LINKED)
+		goto dm_CheckEdcaTurbo_EXIT;
+	/* We do not turn on EDCA turbo mode for some AP that has IOT issue */
+	if (priv->ieee80211->pHTInfo->IOTAction & HT_IOT_ACT_DISABLE_EDCA_TURBO)
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	/*printk("========>%s():bis_any_nonbepkts is %d\n", __func__, priv->bis_any_nonbepkts);*/
+	/* Check the status for current condition. */
+	if (!priv->ieee80211->bis_any_nonbepkts) {
+		curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
+		curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
+		/* For RT-AP, we needs to turn it on when Rx>Tx */
+		if (curRxOkCnt > 4*curTxOkCnt) {
+			/*printk("%s():curRxOkCnt > 4*curTxOkCnt\n");*/
+			if (!priv->bis_cur_rdlstate || !priv->bcurrent_turbo_EDCA) {
+				write_nic_dword(dev, EDCAPARA_BE, edca_setting_DL[pHTInfo->IOTPeer]);
+				priv->bis_cur_rdlstate = true;
+			}
+		} else {
+			/*printk("%s():curRxOkCnt < 4*curTxOkCnt\n");*/
+			if (priv->bis_cur_rdlstate || !priv->bcurrent_turbo_EDCA) {
+				write_nic_dword(dev, EDCAPARA_BE, edca_setting_UL[pHTInfo->IOTPeer]);
+				priv->bis_cur_rdlstate = false;
+			}
+		}
+
+		priv->bcurrent_turbo_EDCA = true;
+	} else {
+		/* Turn Off EDCA turbo here.
+		 * Restore original EDCA according to the declaration of AP.
+		 */
+		if (priv->bcurrent_turbo_EDCA) {
+			u8	u1bAIFS;
+			u32	u4bAcParam, op_limit, cw_max, cw_min;
+
+			struct ieee80211_qos_parameters *qos_parameters = &priv->ieee80211->current_network.qos_data.parameters;
+			u8 mode = priv->ieee80211->mode;
+
+			/*  For Each time updating EDCA parameter, reset EDCA turbo mode status. */
+			dm_init_edca_turbo(dev);
+
+			u1bAIFS = qos_parameters->aifs[0] * ((mode & (IEEE_G | IEEE_N_24G)) ? 9 : 20) + aSifsTime;
+
+			op_limit = (u32)le16_to_cpu(qos_parameters->tx_op_limit[0]);
+			cw_max   = (u32)le16_to_cpu(qos_parameters->cw_max[0]);
+			cw_min   = (u32)le16_to_cpu(qos_parameters->cw_min[0]);
+
+			op_limit <<= AC_PARAM_TXOP_LIMIT_OFFSET;
+			cw_max   <<= AC_PARAM_ECW_MAX_OFFSET;
+			cw_min   <<= AC_PARAM_ECW_MIN_OFFSET;
+			u1bAIFS  <<= AC_PARAM_AIFS_OFFSET;
+
+			u4bAcParam = op_limit | cw_max | cw_min | u1bAIFS;
+			cpu_to_le32s(&u4bAcParam);
+
+			write_nic_dword(dev, EDCAPARA_BE, u4bAcParam);
+
+			/* Check ACM bit.
+			 * If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.
+			 */
+			{
+				/*  TODO:  Modified this part and try to set acm control in only 1 IO processing!! */
+
+				struct aci_aifsn *pAciAifsn = (struct aci_aifsn *)&(qos_parameters->aifs[0]);
+				u8		AcmCtrl;
+
+				read_nic_byte(dev, AcmHwCtrl, &AcmCtrl);
+
+				if (pAciAifsn->acm) { /*  acm bit is 1. */
+					AcmCtrl |= AcmHw_BeqEn;
+				} else {	/* ACM bit is 0. */
+					AcmCtrl &= (~AcmHw_BeqEn);
+				}
+
+				RT_TRACE(COMP_QOS, "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n", AcmCtrl);
+				write_nic_byte(dev, AcmHwCtrl, AcmCtrl);
+			}
+			priv->bcurrent_turbo_EDCA = false;
+		}
+	}
+
+dm_CheckEdcaTurbo_EXIT:
+	/* Set variables for next time. */
+	priv->ieee80211->bis_any_nonbepkts = false;
+	lastTxOkCnt = priv->stats.txbytesunicast;
+	lastRxOkCnt = priv->stats.rxbytesunicast;
+}	/* dm_CheckEdcaTurbo */
+
+static void dm_init_ctstoself(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->ieee80211->bCTSToSelfEnable = true;
+	priv->ieee80211->CTSToSelfTH = CTS_TO_SELF_TH_VAL;
+}
+
+static void dm_ctstoself(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	PRT_HIGH_THROUGHPUT	pHTInfo = priv->ieee80211->pHTInfo;
+	static unsigned long				lastTxOkCnt;
+	static unsigned long				lastRxOkCnt;
+	unsigned long						curTxOkCnt = 0;
+	unsigned long						curRxOkCnt = 0;
+
+	if (!priv->ieee80211->bCTSToSelfEnable) {
+		pHTInfo->IOTAction &= ~HT_IOT_ACT_FORCED_CTS2SELF;
+		return;
+	}
+	/* 1. Uplink
+	 * 2. Linksys350/Linksys300N
+	 * 3. <50 disable, >55 enable
+	 */
+
+	if (pHTInfo->IOTPeer == HT_IOT_PEER_BROADCOM) {
+		curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
+		curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
+		if (curRxOkCnt > 4*curTxOkCnt) { /* downlink, disable CTS to self */
+			pHTInfo->IOTAction &= ~HT_IOT_ACT_FORCED_CTS2SELF;
+			/*DbgPrint("dm_CTSToSelf() ==> CTS to self disabled -- downlink\n");*/
+		} else { /* uplink */
+			pHTInfo->IOTAction |= HT_IOT_ACT_FORCED_CTS2SELF;
+		}
+
+		lastTxOkCnt = priv->stats.txbytesunicast;
+		lastRxOkCnt = priv->stats.rxbytesunicast;
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_check_pbc_gpio()
+ *
+ * Overview:	Check if PBC button is pressed.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/28/2008	amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------*/
+static	void	dm_check_pbc_gpio(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 tmp1byte;
+
+	read_nic_byte(dev, GPI, &tmp1byte);
+	if (tmp1byte == 0xff)
+		return;
+
+	if (tmp1byte & BIT(6) || tmp1byte & BIT(0)) {
+		/* Here we only set bPbcPressed to TRUE
+		 * After trigger PBC, the variable will be set to FALSE
+		 */
+		RT_TRACE(COMP_IO, "CheckPbcGPIO - PBC is pressed\n");
+		priv->bpbc_pressed = true;
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	DM_RFPathCheckWorkItemCallBack()
+ *
+ * Overview:	Check if Current RF RX path is enabled
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	01/30/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void dm_rf_pathcheck_workitemcallback(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct r8192_priv *priv = container_of(dwork, struct r8192_priv, rfpath_check_wq);
+	struct net_device *dev = priv->ieee80211->dev;
+	/*bool bactually_set = false;*/
+	u8 rfpath = 0, i;
+
+	/* 2008/01/30 MH After discussing with SD3 Jerry, 0xc04/0xd04 register will
+	 * always be the same. We only read 0xc04 now.
+	 */
+	read_nic_byte(dev, 0xc04, &rfpath);
+
+	/* Check Bit 0-3, it means if RF A-D is enabled. */
+	for (i = 0; i < RF90_PATH_MAX; i++) {
+		if (rfpath & (0x01<<i))
+			priv->brfpath_rxenable[i] = true;
+		else
+			priv->brfpath_rxenable[i] = false;
+	}
+
+	dm_rxpath_sel_byrssi(dev);
+}	/* DM_RFPathCheckWorkItemCallBack */
+
+static void dm_init_rxpath_selection(struct net_device *dev)
+{
+	u8 i;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->CustomerID == RT_CID_819x_Netcore)
+		DM_RxPathSelTable.cck_method = CCK_RX_VERSION_2;
+	else
+		DM_RxPathSelTable.cck_method = CCK_RX_VERSION_1;
+	DM_RxPathSelTable.disabled_rf = 0;
+	for (i = 0; i < 4; i++) {
+		DM_RxPathSelTable.rf_rssi[i] = 50;
+		DM_RxPathSelTable.cck_pwdb_sta[i] = -64;
+		DM_RxPathSelTable.rf_enable_rssi_th[i] = 100;
+	}
+}
+
+static void dm_rxpath_sel_byrssi(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8				i, max_rssi_index = 0, min_rssi_index = 0, sec_rssi_index = 0, rf_num = 0;
+	u8				tmp_max_rssi = 0, tmp_min_rssi = 0, tmp_sec_rssi = 0;
+	u8				cck_default_Rx = 0x2;  /* RF-C */
+	u8				cck_optional_Rx = 0x3; /* RF-D */
+	long				tmp_cck_max_pwdb = 0, tmp_cck_min_pwdb = 0, tmp_cck_sec_pwdb = 0;
+	u8				cck_rx_ver2_max_index = 0, cck_rx_ver2_min_index = 0, cck_rx_ver2_sec_index = 0;
+	u8				cur_rf_rssi;
+	long				cur_cck_pwdb;
+	static u8			disabled_rf_cnt, cck_Rx_Path_initialized;
+	u8				update_cck_rx_path;
+
+	if (priv->rf_type != RF_2T4R)
+		return;
+
+	if (!cck_Rx_Path_initialized) {
+		read_nic_byte(dev, 0xa07, &DM_RxPathSelTable.cck_rx_path);
+		DM_RxPathSelTable.cck_rx_path &= 0xf;
+		cck_Rx_Path_initialized = 1;
+	}
+
+	read_nic_byte(dev, 0xc04, &DM_RxPathSelTable.disabled_rf);
+	DM_RxPathSelTable.disabled_rf = ~DM_RxPathSelTable.disabled_rf & 0xf;
+
+	if (priv->ieee80211->mode == WIRELESS_MODE_B) {
+		DM_RxPathSelTable.cck_method = CCK_RX_VERSION_2;	/* pure B mode, fixed cck version2 */
+		/*DbgPrint("Pure B mode, use cck rx version2\n");*/
+	}
+
+	/* decide max/sec/min rssi index */
+	for (i = 0; i < RF90_PATH_MAX; i++) {
+		DM_RxPathSelTable.rf_rssi[i] = priv->stats.rx_rssi_percentage[i];
+
+		if (priv->brfpath_rxenable[i]) {
+			rf_num++;
+			cur_rf_rssi = DM_RxPathSelTable.rf_rssi[i];
+
+			if (rf_num == 1) { /* find first enabled rf path and the rssi values */
+				/* initialize, set all rssi index to the same one */
+				max_rssi_index = min_rssi_index = sec_rssi_index = i;
+				tmp_max_rssi = tmp_min_rssi = tmp_sec_rssi = cur_rf_rssi;
+			} else if (rf_num == 2) { /* we pick up the max index first, and let sec and min to be the same one */
+				if (cur_rf_rssi >= tmp_max_rssi) {
+					tmp_max_rssi = cur_rf_rssi;
+					max_rssi_index = i;
+				} else {
+					tmp_sec_rssi = tmp_min_rssi = cur_rf_rssi;
+					sec_rssi_index = min_rssi_index = i;
+				}
+			} else {
+				if (cur_rf_rssi > tmp_max_rssi) {
+					tmp_sec_rssi = tmp_max_rssi;
+					sec_rssi_index = max_rssi_index;
+					tmp_max_rssi = cur_rf_rssi;
+					max_rssi_index = i;
+				} else if (cur_rf_rssi == tmp_max_rssi) {	/* let sec and min point to the different index */
+					tmp_sec_rssi = cur_rf_rssi;
+					sec_rssi_index = i;
+				} else if ((cur_rf_rssi < tmp_max_rssi) && (cur_rf_rssi > tmp_sec_rssi)) {
+					tmp_sec_rssi = cur_rf_rssi;
+					sec_rssi_index = i;
+				} else if (cur_rf_rssi == tmp_sec_rssi) {
+					if (tmp_sec_rssi == tmp_min_rssi) {
+						/* let sec and min point to the different index */
+						tmp_sec_rssi = cur_rf_rssi;
+						sec_rssi_index = i;
+					} else {
+						/* This case we don't need to set any index */
+					}
+				} else if ((cur_rf_rssi < tmp_sec_rssi) && (cur_rf_rssi > tmp_min_rssi)) {
+					/* This case we don't need to set any index */
+				} else if (cur_rf_rssi == tmp_min_rssi) {
+					if (tmp_sec_rssi == tmp_min_rssi) {
+						/* let sec and min point to the different index */
+						tmp_min_rssi = cur_rf_rssi;
+						min_rssi_index = i;
+					} else {
+						/* This case we don't need to set any index */
+					}
+				} else if (cur_rf_rssi < tmp_min_rssi) {
+					tmp_min_rssi = cur_rf_rssi;
+					min_rssi_index = i;
+				}
+			}
+		}
+	}
+
+	rf_num = 0;
+	/* decide max/sec/min cck pwdb index */
+	if (DM_RxPathSelTable.cck_method == CCK_RX_VERSION_2) {
+		for (i = 0; i < RF90_PATH_MAX; i++) {
+			if (priv->brfpath_rxenable[i]) {
+				rf_num++;
+				cur_cck_pwdb =  DM_RxPathSelTable.cck_pwdb_sta[i];
+
+				if (rf_num == 1) {	/* find first enabled rf path and the rssi values */
+					/* initialize, set all rssi index to the same one */
+					cck_rx_ver2_max_index = cck_rx_ver2_min_index = cck_rx_ver2_sec_index = i;
+					tmp_cck_max_pwdb = tmp_cck_min_pwdb = tmp_cck_sec_pwdb = cur_cck_pwdb;
+				} else if (rf_num == 2) {	/* we pick up the max index first, and let sec and min to be the same one */
+					if (cur_cck_pwdb >= tmp_cck_max_pwdb) {
+						tmp_cck_max_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_max_index = i;
+					} else {
+						tmp_cck_sec_pwdb = tmp_cck_min_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = cck_rx_ver2_min_index = i;
+					}
+				} else {
+					if (cur_cck_pwdb > tmp_cck_max_pwdb) {
+						tmp_cck_sec_pwdb = tmp_cck_max_pwdb;
+						cck_rx_ver2_sec_index = cck_rx_ver2_max_index;
+						tmp_cck_max_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_max_index = i;
+					} else if (cur_cck_pwdb == tmp_cck_max_pwdb) {
+						/* let sec and min point to the different index */
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+					} else if ((cur_cck_pwdb < tmp_cck_max_pwdb) && (cur_cck_pwdb > tmp_cck_sec_pwdb)) {
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+					} else if (cur_cck_pwdb == tmp_cck_sec_pwdb && tmp_cck_sec_pwdb == tmp_cck_min_pwdb) {
+						/* let sec and min point to the different index */
+						tmp_cck_sec_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_sec_index = i;
+						/* otherwise we don't need to set any index */
+					} else if ((cur_cck_pwdb < tmp_cck_sec_pwdb) && (cur_cck_pwdb > tmp_cck_min_pwdb)) {
+						/*  This case we don't need to set any index */
+					} else if (cur_cck_pwdb == tmp_cck_min_pwdb && tmp_cck_sec_pwdb == tmp_cck_min_pwdb) {
+						/*  let sec and min point to the different index */
+						tmp_cck_min_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_min_index = i;
+						/* otherwise we don't need to set any index */
+					} else if (cur_cck_pwdb < tmp_cck_min_pwdb) {
+						tmp_cck_min_pwdb = cur_cck_pwdb;
+						cck_rx_ver2_min_index = i;
+					}
+				}
+			}
+		}
+	}
+
+	/* Set CCK Rx path
+	 * reg0xA07[3:2]=cck default rx path, reg0xa07[1:0]=cck optional rx path.
+	 */
+	update_cck_rx_path = 0;
+	if (DM_RxPathSelTable.cck_method == CCK_RX_VERSION_2) {
+		cck_default_Rx = cck_rx_ver2_max_index;
+		cck_optional_Rx = cck_rx_ver2_sec_index;
+		if (tmp_cck_max_pwdb != -64)
+			update_cck_rx_path = 1;
+	}
+
+	if (tmp_min_rssi < RX_PATH_SELECTION_SS_TH_LOW && disabled_rf_cnt < 2) {
+		if ((tmp_max_rssi - tmp_min_rssi) >= RX_PATH_SELECTION_DIFF_TH) {
+			/* record the enabled rssi threshold */
+			DM_RxPathSelTable.rf_enable_rssi_th[min_rssi_index] = tmp_max_rssi+5;
+			/* disable the BB Rx path, OFDM */
+			rtl8192_setBBreg(dev, rOFDM0_TRxPathEnable, 0x1<<min_rssi_index, 0x0);	/* 0xc04[3:0] */
+			rtl8192_setBBreg(dev, rOFDM1_TRxPathEnable, 0x1<<min_rssi_index, 0x0);	/* 0xd04[3:0] */
+			disabled_rf_cnt++;
+		}
+		if (DM_RxPathSelTable.cck_method == CCK_RX_VERSION_1) {
+			cck_default_Rx = max_rssi_index;
+			cck_optional_Rx = sec_rssi_index;
+			if (tmp_max_rssi)
+				update_cck_rx_path = 1;
+		}
+	}
+
+	if (update_cck_rx_path) {
+		DM_RxPathSelTable.cck_rx_path = (cck_default_Rx<<2)|(cck_optional_Rx);
+		rtl8192_setBBreg(dev, rCCK0_AFESetting, 0x0f000000, DM_RxPathSelTable.cck_rx_path);
+	}
+
+	if (DM_RxPathSelTable.disabled_rf) {
+		for (i = 0; i < 4; i++) {
+			if ((DM_RxPathSelTable.disabled_rf >> i) & 0x1) {	/* disabled rf */
+				if (tmp_max_rssi >= DM_RxPathSelTable.rf_enable_rssi_th[i]) {
+					/* enable the BB Rx path */
+					/*DbgPrint("RF-%d is enabled.\n", 0x1<<i);*/
+					rtl8192_setBBreg(dev, rOFDM0_TRxPathEnable, 0x1<<i, 0x1);	/* 0xc04[3:0] */
+					rtl8192_setBBreg(dev, rOFDM1_TRxPathEnable, 0x1<<i, 0x1);	/* 0xd04[3:0] */
+					DM_RxPathSelTable.rf_enable_rssi_th[i] = 100;
+					disabled_rf_cnt--;
+				}
+			}
+		}
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_check_rx_path_selection()
+ *
+ * Overview:	Call a workitem to check current RXRF path and Rx Path selection by RSSI.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/28/2008	amy		Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------*/
+static void dm_check_rx_path_selection(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	queue_delayed_work(priv->priv_wq, &priv->rfpath_check_wq, 0);
+}	/* dm_CheckRxRFPath */
+
+static void dm_init_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->ieee80211->fsync_time_interval = 500;
+	priv->ieee80211->fsync_rate_bitmap = 0x0f000800;
+	priv->ieee80211->fsync_rssi_threshold = 30;
+	priv->ieee80211->bfsync_enable = false;
+	priv->ieee80211->fsync_multiple_timeinterval = 3;
+	priv->ieee80211->fsync_firstdiff_ratethreshold = 100;
+	priv->ieee80211->fsync_seconddiff_ratethreshold = 200;
+	priv->ieee80211->fsync_state = Default_Fsync;
+	priv->framesyncMonitor = 1;	/* current default 0xc38 monitor on */
+	INIT_DELAYED_WORK(&priv->fsync_work, dm_fsync_work_callback);
+}
+
+static void dm_deInit_fsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	cancel_delayed_work_sync(&priv->fsync_work);
+}
+
+void dm_fsync_work_callback(struct work_struct *work)
+{
+	struct r8192_priv *priv =
+	    container_of(work, struct r8192_priv, fsync_work.work);
+	struct net_device *dev = priv->ieee80211->dev;
+	u32 rate_index, rate_count = 0, rate_count_diff = 0;
+	bool		bSwitchFromCountDiff = false;
+	bool		bDoubleTimeInterval = false;
+
+	if (priv->ieee80211->state == IEEE80211_LINKED &&
+	    priv->ieee80211->bfsync_enable &&
+		(priv->ieee80211->pHTInfo->IOTAction & HT_IOT_ACT_CDD_FSYNC)) {
+		/* Count rate 54, MCS [7], [12, 13, 14, 15] */
+		u32 rate_bitmap;
+
+		for (rate_index = 0; rate_index <= 27; rate_index++) {
+			rate_bitmap  = 1 << rate_index;
+			if (priv->ieee80211->fsync_rate_bitmap &  rate_bitmap)
+				rate_count += priv->stats.received_rate_histogram[1][rate_index];
+		}
+
+		if (rate_count < priv->rate_record)
+			rate_count_diff = 0xffffffff - rate_count + priv->rate_record;
+		else
+			rate_count_diff = rate_count - priv->rate_record;
+		if (rate_count_diff < priv->rateCountDiffRecord) {
+			u32 DiffNum = priv->rateCountDiffRecord - rate_count_diff;
+			/* Continue count */
+			if (DiffNum >= priv->ieee80211->fsync_seconddiff_ratethreshold)
+				priv->ContinueDiffCount++;
+			else
+				priv->ContinueDiffCount = 0;
+
+			/* Continue count over */
+			if (priv->ContinueDiffCount >= 2) {
+				bSwitchFromCountDiff = true;
+				priv->ContinueDiffCount = 0;
+			}
+		} else {
+			/* Stop the continued count */
+			priv->ContinueDiffCount = 0;
+		}
+
+		/* If Count diff <= FsyncRateCountThreshold */
+		if (rate_count_diff <= priv->ieee80211->fsync_firstdiff_ratethreshold) {
+			bSwitchFromCountDiff = true;
+			priv->ContinueDiffCount = 0;
+		}
+		priv->rate_record = rate_count;
+		priv->rateCountDiffRecord = rate_count_diff;
+		RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rateCountdiff %d bSwitchFsync %d\n", priv->rate_record, rate_count, rate_count_diff, priv->bswitch_fsync);
+		/* if we never receive those mcs rate and rssi > 30 % then switch fsyn */
+		if (priv->undecorated_smoothed_pwdb > priv->ieee80211->fsync_rssi_threshold && bSwitchFromCountDiff) {
+			bDoubleTimeInterval = true;
+			priv->bswitch_fsync = !priv->bswitch_fsync;
+			if (priv->bswitch_fsync) {
+				write_nic_byte(dev, 0xC36, 0x1c);
+				write_nic_byte(dev, 0xC3e, 0x90);
+			} else {
+				write_nic_byte(dev, 0xC36, 0x5c);
+				write_nic_byte(dev, 0xC3e, 0x96);
+			}
+		} else if (priv->undecorated_smoothed_pwdb <= priv->ieee80211->fsync_rssi_threshold) {
+			if (priv->bswitch_fsync) {
+				priv->bswitch_fsync  = false;
+				write_nic_byte(dev, 0xC36, 0x5c);
+				write_nic_byte(dev, 0xC3e, 0x96);
+			}
+		}
+		if (bDoubleTimeInterval) {
+			cancel_delayed_work_sync(&priv->fsync_work);
+			schedule_delayed_work(&priv->fsync_work,
+					      msecs_to_jiffies(priv
+					      ->ieee80211->fsync_time_interval *
+					      priv->ieee80211->fsync_multiple_timeinterval));
+		} else {
+			cancel_delayed_work_sync(&priv->fsync_work);
+			schedule_delayed_work(&priv->fsync_work,
+					      msecs_to_jiffies(priv
+					      ->ieee80211->fsync_time_interval));
+		}
+	} else {
+		/* Let Register return to default value; */
+		if (priv->bswitch_fsync) {
+			priv->bswitch_fsync  = false;
+			write_nic_byte(dev, 0xC36, 0x5c);
+			write_nic_byte(dev, 0xC3e, 0x96);
+		}
+		priv->ContinueDiffCount = 0;
+		write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
+	}
+	RT_TRACE(COMP_HALDM, "ContinueDiffCount %d\n", priv->ContinueDiffCount);
+	RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rateCountdiff %d bSwitchFsync %d\n", priv->rate_record, rate_count, rate_count_diff, priv->bswitch_fsync);
+}
+
+static void dm_StartHWFsync(struct net_device *dev)
+{
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
+	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cf);
+	write_nic_byte(dev, 0xc3b, 0x41);
+}
+
+static void dm_EndSWFsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
+	cancel_delayed_work_sync(&priv->fsync_work);
+
+	/* Let Register return to default value; */
+	if (priv->bswitch_fsync) {
+		priv->bswitch_fsync  = false;
+
+		write_nic_byte(dev, 0xC36, 0x5c);
+
+		write_nic_byte(dev, 0xC3e, 0x96);
+	}
+
+	priv->ContinueDiffCount = 0;
+	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
+}
+
+static void dm_StartSWFsync(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32			rateIndex;
+	u32			rateBitmap;
+
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
+	/* Initial rate record to zero, start to record. */
+	priv->rate_record = 0;
+	/* Initialize continue diff count to zero, start to record. */
+	priv->ContinueDiffCount = 0;
+	priv->rateCountDiffRecord = 0;
+	priv->bswitch_fsync  = false;
+
+	if (priv->ieee80211->mode == WIRELESS_MODE_N_24G) {
+		priv->ieee80211->fsync_firstdiff_ratethreshold = 600;
+		priv->ieee80211->fsync_seconddiff_ratethreshold = 0xffff;
+	} else {
+		priv->ieee80211->fsync_firstdiff_ratethreshold = 200;
+		priv->ieee80211->fsync_seconddiff_ratethreshold = 200;
+	}
+	for (rateIndex = 0; rateIndex <= 27; rateIndex++) {
+		rateBitmap = 1 << rateIndex;
+		if (priv->ieee80211->fsync_rate_bitmap &  rateBitmap)
+			priv->rate_record += priv->stats.received_rate_histogram[1][rateIndex];
+	}
+	cancel_delayed_work_sync(&priv->fsync_work);
+	schedule_delayed_work(&priv->fsync_work,
+			      msecs_to_jiffies(priv->ieee80211->fsync_time_interval));
+
+	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cd);
+}
+
+static void dm_EndHWFsync(struct net_device *dev)
+{
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
+	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
+	write_nic_byte(dev, 0xc3b, 0x49);
+}
+
+void dm_check_fsync(struct net_device *dev)
+{
+#define	RegC38_Default				0
+#define	RegC38_NonFsync_Other_AP		1
+#define	RegC38_Fsync_AP_BCM			2
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/*u32			framesyncC34;*/
+	static u8		reg_c38_State = RegC38_Default;
+	static u32	reset_cnt;
+
+	RT_TRACE(COMP_HALDM, "RSSI %d TimeInterval %d MultipleTimeInterval %d\n", priv->ieee80211->fsync_rssi_threshold, priv->ieee80211->fsync_time_interval, priv->ieee80211->fsync_multiple_timeinterval);
+	RT_TRACE(COMP_HALDM, "RateBitmap 0x%x FirstDiffRateThreshold %d SecondDiffRateThreshold %d\n", priv->ieee80211->fsync_rate_bitmap, priv->ieee80211->fsync_firstdiff_ratethreshold, priv->ieee80211->fsync_seconddiff_ratethreshold);
+
+	if (priv->ieee80211->state == IEEE80211_LINKED &&
+		(priv->ieee80211->pHTInfo->IOTAction & HT_IOT_ACT_CDD_FSYNC)) {
+		if (priv->ieee80211->bfsync_enable == 0) {
+			switch (priv->ieee80211->fsync_state) {
+			case Default_Fsync:
+				dm_StartHWFsync(dev);
+				priv->ieee80211->fsync_state = HW_Fsync;
+				break;
+			case SW_Fsync:
+				dm_EndSWFsync(dev);
+				dm_StartHWFsync(dev);
+				priv->ieee80211->fsync_state = HW_Fsync;
+				break;
+			case HW_Fsync:
+			default:
+				break;
+			}
+		} else {
+			switch (priv->ieee80211->fsync_state) {
+			case Default_Fsync:
+				dm_StartSWFsync(dev);
+				priv->ieee80211->fsync_state = SW_Fsync;
+				break;
+			case HW_Fsync:
+				dm_EndHWFsync(dev);
+				dm_StartSWFsync(dev);
+				priv->ieee80211->fsync_state = SW_Fsync;
+				break;
+			case SW_Fsync:
+			default:
+				break;
+			}
+		}
+		if (priv->framesyncMonitor) {
+			if (reg_c38_State != RegC38_Fsync_AP_BCM) {
+				/* For broadcom AP we write different default value */
+				write_nic_byte(dev, rOFDM0_RxDetector3, 0x95);
+
+				reg_c38_State = RegC38_Fsync_AP_BCM;
+			}
+		}
+	} else {
+		switch (priv->ieee80211->fsync_state) {
+		case HW_Fsync:
+			dm_EndHWFsync(dev);
+			priv->ieee80211->fsync_state = Default_Fsync;
+			break;
+		case SW_Fsync:
+			dm_EndSWFsync(dev);
+			priv->ieee80211->fsync_state = Default_Fsync;
+			break;
+		case Default_Fsync:
+		default:
+			break;
+		}
+
+		if (priv->framesyncMonitor) {
+			if (priv->ieee80211->state == IEEE80211_LINKED) {
+				if (priv->undecorated_smoothed_pwdb <= REG_C38_TH) {
+					if (reg_c38_State != RegC38_NonFsync_Other_AP) {
+						write_nic_byte(dev, rOFDM0_RxDetector3, 0x90);
+
+						reg_c38_State = RegC38_NonFsync_Other_AP;
+					}
+				} else if (priv->undecorated_smoothed_pwdb >= (REG_C38_TH + 5)) {
+					if (reg_c38_State) {
+						write_nic_byte(dev, rOFDM0_RxDetector3, priv->framesync);
+						reg_c38_State = RegC38_Default;
+						/*DbgPrint("Fsync is idle, rssi>=40, write 0xc38 = 0x%x\n", pHalData->framesync);*/
+					}
+				}
+			} else {
+				if (reg_c38_State) {
+					write_nic_byte(dev, rOFDM0_RxDetector3, priv->framesync);
+					reg_c38_State = RegC38_Default;
+					/*DbgPrint("Fsync is idle, not connected, write 0xc38 = 0x%x\n", pHalData->framesync);*/
+				}
+			}
+		}
+	}
+	if (priv->framesyncMonitor) {
+		if (priv->reset_count != reset_cnt) { /* After silent reset, the reg_c38_State will be returned to default value */
+			write_nic_byte(dev, rOFDM0_RxDetector3, priv->framesync);
+			reg_c38_State = RegC38_Default;
+			reset_cnt = priv->reset_count;
+			/*DbgPrint("reg_c38_State = 0 for silent reset.\n");*/
+		}
+	} else {
+		if (reg_c38_State) {
+			write_nic_byte(dev, rOFDM0_RxDetector3, priv->framesync);
+			reg_c38_State = RegC38_Default;
+			/*DbgPrint("framesync no monitor, write 0xc38 = 0x%x\n", pHalData->framesync);*/
+		}
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	dm_shadow_init()
+ *
+ * Overview:	Store all NIC MAC/BB register content.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/29/2008	amy		Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+void dm_shadow_init(struct net_device *dev)
+{
+	u8	page;
+	u16	offset;
+
+	for (page = 0; page < 5; page++)
+		for (offset = 0; offset < 256; offset++) {
+			read_nic_byte(dev, offset + page * 256, &dm_shadow[page][offset]);
+			/*DbgPrint("P-%d/O-%02x=%02x\r\n", page, offset, DM_Shadow[page][offset]);*/
+		}
+
+	for (page = 8; page < 11; page++)
+		for (offset = 0; offset < 256; offset++)
+			read_nic_byte(dev, offset + page * 256, &dm_shadow[page][offset]);
+
+	for (page = 12; page < 15; page++)
+		for (offset = 0; offset < 256; offset++)
+			read_nic_byte(dev, offset + page * 256, &dm_shadow[page][offset]);
+
+}   /* dm_shadow_init */
+
+/*---------------------------Define function prototype------------------------*/
+/*-----------------------------------------------------------------------------
+ * Function:	DM_DynamicTxPower()
+ *
+ * Overview:	Detect Signal strength to control TX Registry
+			Tx Power Control For Near/Far Range
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	03/06/2008	Jacken	Create Version 0.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void dm_init_dynamic_txpower(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* Initial TX Power Control for near/far range , add by amy 2008/05/15, porting from windows code. */
+	priv->ieee80211->bdynamic_txpower_enable = true;    /* Default to enable Tx Power Control */
+	priv->bLastDTPFlag_High = false;
+	priv->bLastDTPFlag_Low = false;
+	priv->bDynamicTxHighPower = false;
+	priv->bDynamicTxLowPower = false;
+}
+
+static void dm_dynamic_txpower(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	unsigned int txhipower_threshold = 0;
+	unsigned int txlowpower_threshold = 0;
+
+	if (!priv->ieee80211->bdynamic_txpower_enable) {
+		priv->bDynamicTxHighPower = false;
+		priv->bDynamicTxLowPower = false;
+		return;
+	}
+	/*printk("priv->ieee80211->current_network.unknown_cap_exist is %d , priv->ieee80211->current_network.broadcom_cap_exist is %d\n", priv->ieee80211->current_network.unknown_cap_exist, priv->ieee80211->current_network.broadcom_cap_exist);*/
+	if ((priv->ieee80211->current_network.atheros_cap_exist) && (priv->ieee80211->mode == IEEE_G)) {
+		txhipower_threshold = TX_POWER_ATHEROAP_THRESH_HIGH;
+		txlowpower_threshold = TX_POWER_ATHEROAP_THRESH_LOW;
+	} else {
+		txhipower_threshold = TX_POWER_NEAR_FIELD_THRESH_HIGH;
+		txlowpower_threshold = TX_POWER_NEAR_FIELD_THRESH_LOW;
+	}
+
+	/*printk("=======>%s(): txhipower_threshold is %d, txlowpower_threshold is %d\n", __func__, txhipower_threshold, txlowpower_threshold);*/
+	RT_TRACE(COMP_TXAGC, "priv->undecorated_smoothed_pwdb = %ld\n", priv->undecorated_smoothed_pwdb);
+
+	if (priv->ieee80211->state == IEEE80211_LINKED) {
+		if (priv->undecorated_smoothed_pwdb >= txhipower_threshold) {
+			priv->bDynamicTxHighPower = true;
+			priv->bDynamicTxLowPower = false;
+		} else {
+			/* high power state check */
+			if (priv->undecorated_smoothed_pwdb < txlowpower_threshold && priv->bDynamicTxHighPower)
+				priv->bDynamicTxHighPower = false;
+
+			/* low power state check */
+			if (priv->undecorated_smoothed_pwdb < 35)
+				priv->bDynamicTxLowPower = true;
+			else if (priv->undecorated_smoothed_pwdb >= 40)
+				priv->bDynamicTxLowPower = false;
+		}
+	} else {
+		/*pHalData->bTXPowerCtrlforNearFarRange = !pHalData->bTXPowerCtrlforNearFarRange;*/
+		priv->bDynamicTxHighPower = false;
+		priv->bDynamicTxLowPower = false;
+	}
+
+	if ((priv->bDynamicTxHighPower != priv->bLastDTPFlag_High) ||
+		(priv->bDynamicTxLowPower != priv->bLastDTPFlag_Low)) {
+		RT_TRACE(COMP_TXAGC, "SetTxPowerLevel8190()  channel = %d\n", priv->ieee80211->current_network.channel);
+
+#if  defined(RTL8190P) || defined(RTL8192E)
+		SetTxPowerLevel8190(Adapter, pHalData->CurrentChannel);
+#endif
+
+		rtl8192_phy_setTxPower(dev, priv->ieee80211->current_network.channel);
+		/*pHalData->bStartTxCtrlByTPCNFR = FALSE;    Clear th flag of Set TX Power from Sitesurvey*/
+	}
+	priv->bLastDTPFlag_High = priv->bDynamicTxHighPower;
+	priv->bLastDTPFlag_Low = priv->bDynamicTxLowPower;
+
+}	/* dm_dynamic_txpower */
+
+/* added by vivi, for read tx rate and retrycount */
+static void dm_check_txrateandretrycount(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	/* for 11n tx rate */
+	/*priv->stats.CurrentShowTxate = read_nic_byte(dev, CURRENT_TX_RATE_REG);*/
+	read_nic_byte(dev, CURRENT_TX_RATE_REG, &ieee->softmac_stats.CurrentShowTxate);
+	/*printk("=============>tx_rate_reg:%x\n", ieee->softmac_stats.CurrentShowTxate);*/
+	/* for initial tx rate */
+	/*priv->stats.last_packet_rate = read_nic_byte(dev, INITIAL_TX_RATE_REG);*/
+	read_nic_byte(dev, INITIAL_TX_RATE_REG, &ieee->softmac_stats.last_packet_rate);
+	/* for tx retry count */
+	/*priv->stats.txretrycount = read_nic_dword(dev, TX_RETRY_COUNT_REG);*/
+	read_nic_dword(dev, TX_RETRY_COUNT_REG, &ieee->softmac_stats.txretrycount);
+}
+
+static void dm_send_rssi_tofw(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* If we test chariot, we should stop the TX command ?
+	 * Because 92E will always silent reset when we send tx command. We use register
+	 * 0x1e0(byte) to notify driver.
+	 */
+	write_nic_byte(dev, DRIVER_RSSI, (u8)priv->undecorated_smoothed_pwdb);
+}
+
+/*---------------------------Define function prototype------------------------*/
diff --git a/drivers/staging/rtl8192u/r8192U_dm.h b/drivers/staging/rtl8192u/r8192U_dm.h
new file mode 100644
index 000000000..2159018b4
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_dm.h
@@ -0,0 +1,177 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*****************************************************************************
+ *	Copyright(c) 2007,  RealTEK Technology Inc. All Right Reserved.
+ *
+ * Module:		Hal819xUsbDM.h	(RTL8192  Header H File)
+ *
+ *
+ * Note:		For dynamic control definition constant structure.
+ *
+ *
+ * Export:
+ *
+ * Abbrev:
+ *
+ * History:
+ *	Data		Who		Remark
+ *	10/04/2007  MHC		Create initial version.
+ *
+ *****************************************************************************/
+ /* Check to see if the file has been included already.  */
+#ifndef	__R8192UDM_H__
+#define __R8192UDM_H__
+
+/*--------------------------Define Parameters-------------------------------*/
+#define         DM_DIG_THRESH_HIGH                      40
+#define         DM_DIG_THRESH_LOW                       35
+
+#define         DM_DIG_HIGH_PWR_THRESH_HIGH             75
+#define         DM_DIG_HIGH_PWR_THRESH_LOW              70
+
+#define         BW_AUTO_SWITCH_HIGH_LOW                 25
+#define         BW_AUTO_SWITCH_LOW_HIGH                 30
+
+#define         DM_DIG_BACKOFF                          12
+#define         DM_DIG_MAX                            0x36
+#define         DM_DIG_MIN                            0x1c
+#define         DM_DIG_MIN_NETCORE                    0x12
+
+#define         RX_PATH_SELECTION_SS_TH_LOW             30
+#define         RX_PATH_SELECTION_DIFF_TH               18
+
+#define         RATE_ADAPTIVE_TH_HIGH                   50
+#define         RATE_ADAPTIVE_TH_LOW_20M                30
+#define         RATE_ADAPTIVE_TH_LOW_40M                10
+#define         VERY_LOW_RSSI                           15
+#define         CTS_TO_SELF_TH_VAL                      30
+
+/* defined by vivi, for tx power track */
+#define         E_FOR_TX_POWER_TRACK                   300
+/* Dynamic Tx Power Control Threshold */
+#define         TX_POWER_NEAR_FIELD_THRESH_HIGH         68
+#define         TX_POWER_NEAR_FIELD_THRESH_LOW          62
+/* added by amy for atheros AP */
+#define         TX_POWER_ATHEROAP_THRESH_HIGH           78
+#define         TX_POWER_ATHEROAP_THRESH_LOW            72
+
+/* defined by vivi, for showing on UI */
+#define         CURRENT_TX_RATE_REG                  0x1b8
+#define         INITIAL_TX_RATE_REG                  0x1b9
+#define         TX_RETRY_COUNT_REG                   0x1ac
+#define         REG_C38_TH                              20
+/*--------------------------Define Parameters-------------------------------*/
+
+/*------------------------------Define structure----------------------------*/
+
+enum dig_algorithm {
+	DIG_ALGO_BY_FALSE_ALARM = 0,
+	DIG_ALGO_BY_RSSI	= 1,
+};
+
+enum dynamic_init_gain_state {
+	DM_STA_DIG_OFF = 0,
+	DM_STA_DIG_ON,
+	DM_STA_DIG_MAX
+};
+
+enum dig_connect {
+	DIG_DISCONNECT = 0,
+	DIG_CONNECT    = 1,
+};
+
+enum dig_pkt_detection_threshold {
+	DIG_PD_AT_LOW_POWER    = 0,
+	DIG_PD_AT_NORMAL_POWER = 1,
+	DIG_PD_AT_HIGH_POWER   = 2,
+};
+
+enum dig_cck_cs_ratio_state {
+	DIG_CS_RATIO_LOWER  = 0,
+	DIG_CS_RATIO_HIGHER = 1,
+};
+
+/* 2007/10/04 MH Define upper and lower threshold of DIG enable or disable. */
+struct dig {
+	u8                                 dig_enable_flag;
+	enum dig_algorithm                 dig_algorithm;
+	u8                                 dig_algorithm_switch;
+
+	long                               rssi_low_thresh;
+	long                               rssi_high_thresh;
+
+	long                               rssi_high_power_lowthresh;
+	long                               rssi_high_power_highthresh;
+
+	enum dynamic_init_gain_state       dig_state;
+	enum dynamic_init_gain_state       dig_highpwr_state;
+	enum dig_connect                   cur_connect_state;
+	enum dig_connect                   pre_connect_state;
+
+	enum dig_pkt_detection_threshold   curpd_thstate;
+	enum dig_pkt_detection_threshold   prepd_thstate;
+	enum dig_cck_cs_ratio_state        curcs_ratio_state;
+	enum dig_cck_cs_ratio_state        precs_ratio_state;
+
+	u32                                pre_ig_value;
+	u32                                cur_ig_value;
+
+	u8                                 backoff_val;
+	u8                                 rx_gain_range_min;
+
+	long                               rssi_val;
+};
+
+enum cck_rx_path_method {
+	CCK_RX_VERSION_1 = 0,
+	CCK_RX_VERSION_2 = 1,
+};
+
+struct dynamic_rx_path_sel {
+	enum cck_rx_path_method            cck_method;
+	u8                                 cck_rx_path;
+
+	u8                                 disabled_rf;
+
+	u8                                 rf_rssi[4];
+	u8                                 rf_enable_rssi_th[4];
+	long                               cck_pwdb_sta[4];
+};
+
+struct tx_config_cmd {
+	u32     cmd_op;        /* Command packet type. */
+	u32     cmd_length;    /* Command packet length. */
+	u32     cmd_value;
+};
+
+/*------------------------------Define structure----------------------------*/
+
+/*------------------------Export global variable----------------------------*/
+extern struct dig dm_digtable;
+extern u8 dm_shadow[16][256];
+/*------------------------Export global variable----------------------------*/
+
+/*------------------------Export Marco Definition---------------------------*/
+
+/*------------------------Export Marco Definition---------------------------*/
+
+/*--------------------------Exported Function prototype---------------------*/
+void init_hal_dm(struct net_device *dev);
+void deinit_hal_dm(struct net_device *dev);
+void hal_dm_watchdog(struct net_device *dev);
+void init_rate_adaptive(struct net_device *dev);
+void dm_txpower_trackingcallback(struct work_struct *work);
+void dm_restore_dynamic_mechanism_state(struct net_device *dev);
+void dm_force_tx_fw_info(struct net_device *dev,
+			 u32 force_type, u32 force_value);
+void dm_init_edca_turbo(struct net_device *dev);
+void dm_rf_operation_test_callback(unsigned long data);
+void dm_rf_pathcheck_workitemcallback(struct work_struct *work);
+void dm_fsync_work_callback(struct work_struct *work);
+void dm_cck_txpower_adjust(struct net_device *dev, bool  binch14);
+void dm_shadow_init(struct net_device *dev);
+void dm_initialize_txpower_tracking(struct net_device *dev);
+/*--------------------------Exported Function prototype---------------------*/
+
+#endif	/*__R8192UDM_H__ */
+
+/* End of r8192U_dm.h */
diff --git a/drivers/staging/rtl8192u/r8192U_hw.h b/drivers/staging/rtl8192u/r8192U_hw.h
new file mode 100644
index 000000000..217e77766
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_hw.h
@@ -0,0 +1,246 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *	This is part of rtl8187 OpenSource driver.
+ *	Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
+ *
+ *	Parts of this driver are based on the GPL part of the
+ *	official Realtek driver.
+ *	Parts of this driver are based on the rtl8180 driver skeleton
+ *	from Patric Schenke & Andres Salomon.
+ *	Parts of this driver are based on the Intel Pro Wireless
+ *	2100 GPL driver.
+ *
+ *	We want to thank the Authors of those projects
+ *	and the Ndiswrapper project Authors.
+ */
+
+/* Mariusz Matuszek added full registers definition with Realtek's name */
+
+/* this file contains register definitions for the rtl8187 MAC controller */
+#ifndef R8192_HW
+#define R8192_HW
+
+#define	RTL8187_REQT_READ	0xc0
+#define	RTL8187_REQT_WRITE	0x40
+#define	RTL8187_REQ_GET_REGS	0x05
+#define	RTL8187_REQ_SET_REGS	0x05
+
+#define MAX_TX_URB 5
+#define MAX_RX_URB 16
+
+#define R8180_MAX_RETRY 255
+
+#define RX_URB_SIZE 9100
+
+#define RTL8190_EEPROM_ID	0x8129
+#define EEPROM_VID		0x02
+#define EEPROM_PID		0x04
+#define EEPROM_NODE_ADDRESS_BYTE_0	0x0C
+
+#define EEPROM_TX_POWER_DIFF	0x1F
+#define EEPROM_THERMAL_METER	0x20
+#define EEPROM_PW_DIFF		0x21	//0x21
+#define EEPROM_CRYSTAL_CAP	0x22	//0x22
+
+#define EEPROM_TX_PW_INDEX_CCK	0x23	//0x23
+#define EEPROM_TX_PW_INDEX_OFDM_24G	0x24	//0x24~0x26
+#define EEPROM_TX_PW_INDEX_CCK_V1	0x29	//0x29~0x2B
+#define EEPROM_TX_PW_INDEX_OFDM_24G_V1	0x2C	//0x2C~0x2E
+#define EEPROM_TX_PW_INDEX_VER		0x27	//0x27
+
+#define EEPROM_DEFAULT_THERNAL_METER		0x7
+#define EEPROM_DEFAULT_PW_DIFF			0x4
+#define EEPROM_DEFAULT_CRYSTAL_CAP		0x5
+#define EEPROM_DEFAULT_TX_POWER		0x1010
+#define EEPROM_CUSTOMER_ID			0x7B	//0x7B:CustomerID
+#define EEPROM_CHANNEL_PLAN			0x16	//0x7C
+
+#define EEPROM_CID_RUNTOP				0x2
+#define EEPROM_CID_DLINK				0x8
+
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
+
+//#endif
+enum _RTL8192Usb_HW {
+	MAC0			= 0x000,
+	MAC4			= 0x004,
+
+#define	BB_GLOBAL_RESET_BIT	0x1
+	BB_GLOBAL_RESET		= 0x020, // BasebandGlobal Reset Register
+	BSSIDR			= 0x02E, // BSSID Register
+	CMDR			= 0x037, // Command register
+#define CR_RE			0x08
+#define CR_TE			0x04
+	SIFS			= 0x03E, // SIFS register
+
+#define TCR_MXDMA_2048		7
+#define TCR_LRL_OFFSET		0
+#define TCR_SRL_OFFSET		8
+#define TCR_MXDMA_OFFSET	21
+#define TCR_SAT			BIT(24)	// Enable Rate depedent ack timeout timer
+	RCR			= 0x044, // Receive Configuration Register
+#define MAC_FILTER_MASK (BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(5) | \
+			 BIT(12) | BIT(18) | BIT(19) | BIT(20) | BIT(21) | \
+			 BIT(22) | BIT(23))
+#define RX_FIFO_THRESHOLD_MASK (BIT(13) | BIT(14) | BIT(15))
+#define RX_FIFO_THRESHOLD_SHIFT 13
+#define RX_FIFO_THRESHOLD_NONE 7
+#define MAX_RX_DMA_MASK	(BIT(8) | BIT(9) | BIT(10))
+#define RCR_MXDMA_OFFSET	8
+#define RCR_FIFO_OFFSET		13
+#define RCR_ONLYERLPKT		BIT(31)			// Early Receiving based on Packet Size.
+#define RCR_CBSSID		BIT(23)			// Accept BSSID match packet
+#define RCR_APWRMGT		BIT(22)			// Accept power management packet
+#define RCR_AMF			BIT(20)			// Accept management type frame
+#define RCR_ACF			BIT(19)			// Accept control type frame
+#define RCR_ADF			BIT(18)			// Accept data type frame
+#define RCR_AICV		BIT(12)			// Accept ICV error packet
+#define	RCR_ACRC32		BIT(5)			// Accept CRC32 error packet
+#define	RCR_AB			BIT(3)			// Accept broadcast packet
+#define	RCR_AM			BIT(2)			// Accept multicast packet
+#define	RCR_APM			BIT(1)			// Accept physical match packet
+#define	RCR_AAP			BIT(0)			// Accept all unicast packet
+	SLOT_TIME		= 0x049, // Slot Time Register
+	ACK_TIMEOUT		= 0x04c, // Ack Timeout Register
+	EDCAPARA_BE		= 0x050, // EDCA Parameter of AC BE
+	EDCAPARA_BK		= 0x054, // EDCA Parameter of AC BK
+	EDCAPARA_VO		= 0x058, // EDCA Parameter of AC VO
+	EDCAPARA_VI		= 0x05C, // EDCA Parameter of AC VI
+	BCN_TCFG		= 0x062, // Beacon Time Configuration
+#define BCN_TCFG_CW_SHIFT		8
+#define BCN_TCFG_IFS			0
+	BCN_INTERVAL		= 0x070, // Beacon Interval (TU)
+	ATIMWND			= 0x072, // ATIM Window Size (TU)
+	BCN_DRV_EARLY_INT	= 0x074, // Driver Early Interrupt Time (TU). Time to send interrupt to notify to change beacon content before TBTT
+	BCN_DMATIME		= 0x076, // Beacon DMA and ATIM interrupt time (US). Indicates the time before TBTT to perform beacon queue DMA
+	BCN_ERR_THRESH		= 0x078, // Beacon Error Threshold
+	RWCAM			= 0x0A0, //IN 8190 Data Sheet is called CAMcmd
+	WCAMI			= 0x0A4, // Software write CAM input content
+	SECR			= 0x0B0, //Security Configuration Register
+#define	SCR_TxUseDK		BIT(0)			//Force Tx Use Default Key
+#define SCR_RxUseDK		BIT(1)			//Force Rx Use Default Key
+#define SCR_TxEncEnable		BIT(2)			//Enable Tx Encryption
+#define SCR_RxDecEnable		BIT(3)			//Enable Rx Decryption
+#define SCR_SKByA2		BIT(4)			//Search kEY BY A2
+#define SCR_NoSKMC		BIT(5)			//No Key Search for Multicast
+
+//----------------------------------------------------------------------------
+//       8190 CPU General Register		(offset 0x100, 4 byte)
+//----------------------------------------------------------------------------
+#define	CPU_CCK_LOOPBACK	0x00030000
+#define	CPU_GEN_SYSTEM_RESET	0x00000001
+#define	CPU_GEN_FIRMWARE_RESET	0x00000008
+#define	CPU_GEN_BOOT_RDY	0x00000010
+#define	CPU_GEN_FIRM_RDY	0x00000020
+#define	CPU_GEN_PUT_CODE_OK	0x00000080
+#define	CPU_GEN_BB_RST		0x00000100
+#define	CPU_GEN_PWR_STB_CPU	0x00000004
+#define CPU_GEN_NO_LOOPBACK_MSK	0xFFF8FFFF // Set bit18,17,16 to 0. Set bit19
+#define CPU_GEN_NO_LOOPBACK_SET	0x00080000 // Set BIT19 to 1
+	CPU_GEN			= 0x100, // CPU Reset Register
+
+	AcmHwCtrl		= 0x171, // ACM Hardware Control Register
+//----------------------------------------------------------------------------
+////
+////       8190 AcmHwCtrl bits                                    (offset 0x171, 1 byte)
+////----------------------------------------------------------------------------
+//
+#define AcmHw_BeqEn             BIT(1)
+
+	RQPN1			= 0x180, // Reserved Queue Page Number , Vo Vi, Be, Bk
+	RQPN2			= 0x184, // Reserved Queue Page Number, HCCA, Cmd, Mgnt, High
+	RQPN3			= 0x188, // Reserved Queue Page Number, Bcn, Public,
+	QPNR			= 0x1D0, //0x1F0, // Queue Packet Number report per TID
+
+#define	BW_OPMODE_5G			BIT(1)
+#define	BW_OPMODE_20MHZ			BIT(2)
+	BW_OPMODE		= 0x300, // Bandwidth operation mode
+	MSR			= 0x303, // Media Status register
+#define MSR_LINK_MASK      (BIT(0) | BIT(1))
+#define MSR_LINK_MANAGED   2
+#define MSR_LINK_NONE      0
+#define MSR_LINK_SHIFT     0
+#define MSR_LINK_ADHOC     1
+#define MSR_LINK_MASTER    3
+	RETRY_LIMIT		= 0x304, // Retry Limit [15:8]-short, [7:0]-long
+#define RETRY_LIMIT_SHORT_SHIFT 8
+#define RETRY_LIMIT_LONG_SHIFT 0
+	RRSR			= 0x310, // Response Rate Set
+#define RRSR_1M						BIT(0)
+#define RRSR_2M						BIT(1)
+#define RRSR_5_5M					BIT(2)
+#define RRSR_11M					BIT(3)
+#define RRSR_6M						BIT(4)
+#define RRSR_9M						BIT(5)
+#define RRSR_12M					BIT(6)
+#define RRSR_18M					BIT(7)
+#define RRSR_24M					BIT(8)
+#define RRSR_36M					BIT(9)
+#define RRSR_48M					BIT(10)
+#define RRSR_54M					BIT(11)
+#define BRSR_AckShortPmb			BIT(23)		// CCK ACK: use Short Preamble or not.
+	UFWP			= 0x318,
+	RATR0			= 0x320, // Rate Adaptive Table register1
+	DRIVER_RSSI		= 0x32c,					// Driver tell Firmware current RSSI
+//----------------------------------------------------------------------------
+//       8190 Rate Adaptive Table Register	(offset 0x320, 4 byte)
+//----------------------------------------------------------------------------
+//CCK
+#define	RATR_1M			0x00000001
+#define	RATR_2M			0x00000002
+#define	RATR_55M		0x00000004
+#define	RATR_11M		0x00000008
+//OFDM
+#define	RATR_6M			0x00000010
+#define	RATR_9M			0x00000020
+#define	RATR_12M		0x00000040
+#define	RATR_18M		0x00000080
+#define	RATR_24M		0x00000100
+#define	RATR_36M		0x00000200
+#define	RATR_48M		0x00000400
+#define	RATR_54M		0x00000800
+//MCS 1 Spatial Stream
+#define	RATR_MCS0		0x00001000
+#define	RATR_MCS1		0x00002000
+#define	RATR_MCS2		0x00004000
+#define	RATR_MCS3		0x00008000
+#define	RATR_MCS4		0x00010000
+#define	RATR_MCS5		0x00020000
+#define	RATR_MCS6		0x00040000
+#define	RATR_MCS7		0x00080000
+//MCS 2 Spatial Stream
+#define	RATR_MCS8		0x00100000
+#define	RATR_MCS9		0x00200000
+#define	RATR_MCS10		0x00400000
+#define	RATR_MCS11		0x00800000
+#define	RATR_MCS12		0x01000000
+#define	RATR_MCS13		0x02000000
+#define	RATR_MCS14		0x04000000
+#define	RATR_MCS15		0x08000000
+// ALL CCK Rate
+#define RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
+				 RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 | RATR_MCS3 |\
+				 RATR_MCS4 | RATR_MCS5 | RATR_MCS6 | RATR_MCS7)
+#define RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 | RATR_MCS11 |\
+				 RATR_MCS12 | RATR_MCS13 | RATR_MCS14 | RATR_MCS15)
+	EPROM_CMD		= 0xfe58,
+#define Cmd9346CR_9356SEL	BIT(4)
+#define EPROM_CMD_OPERATING_MODE_SHIFT 6
+#define EPROM_CMD_NORMAL 0
+#define EPROM_CMD_PROGRAM 2
+#define EPROM_CS_BIT BIT(3)
+#define EPROM_CK_BIT BIT(2)
+#define EPROM_W_BIT  BIT(1)
+#define EPROM_R_BIT  BIT(0)
+};
+
+//----------------------------------------------------------------------------
+//       818xB AnaParm & AnaParm2 Register
+//----------------------------------------------------------------------------
+#define GPI 0x108
+#endif
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
new file mode 100644
index 000000000..203ea3bfc
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -0,0 +1,943 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * This file contains wireless extension handlers.
+ *
+ * This is part of rtl8180 OpenSource driver.
+ * Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
+ *
+ * Parts of this driver are based on the GPL part
+ * of the official realtek driver.
+ *
+ * Parts of this driver are based on the rtl8180 driver skeleton
+ * from Patric Schenke & Andres Salomon.
+ *
+ * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+ *
+ * We want to thank the Authors of those projects and the Ndiswrapper
+ * project Authors.
+ *
+ *****************************************************************************/
+
+#include <linux/string.h>
+#include "r8192U.h"
+#include "r8192U_hw.h"
+
+#include "ieee80211/dot11d.h"
+#include "r8192U_wx.h"
+
+#define RATE_COUNT 12
+static const u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
+	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
+
+#ifndef ENETDOWN
+#define ENETDOWN 1
+#endif
+
+static int r8192_wx_get_freq(struct net_device *dev,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_freq(priv->ieee80211, a, wrqu, b);
+}
+
+static int r8192_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_mode(priv->ieee80211, a, wrqu, b);
+}
+
+static int r8192_wx_get_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_rate(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8192_wx_set_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_rate(priv->ieee80211, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_set_rts(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_rts(priv->ieee80211, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_get_rts(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_rts(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8192_wx_set_power(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_get_power(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_power(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8192_wx_force_reset(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	netdev_dbg(dev, "%s(): force reset ! extra is %d\n", __func__, *extra);
+	priv->force_reset = *extra;
+	mutex_unlock(&priv->wx_mutex);
+	return 0;
+}
+
+static int r8192_wx_set_rawtx(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_rawtx(priv->ieee80211, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_set_crcmon(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int *parms = (int *)extra;
+	int enable = (parms[0] > 0);
+
+	mutex_lock(&priv->wx_mutex);
+
+	if (enable)
+		priv->crcmon = 1;
+	else
+		priv->crcmon = 0;
+
+	DMESG("bad CRC in monitor mode are %s",
+	      priv->crcmon ? "accepted" : "rejected");
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return 0;
+}
+
+static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_mode(priv->ieee80211, a, wrqu, b);
+
+	rtl8192_set_rxconf(dev);
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+struct  iw_range_with_scan_capa {
+	/* Informative stuff (to choose between different interface) */
+	__u32           throughput;     /* To give an idea... */
+	/* In theory this value should be the maximum benchmarked
+	 * TCP/IP throughput, because with most of these devices the
+	 * bit rate is meaningless (overhead an co) to estimate how
+	 * fast the connection will go and pick the fastest one.
+	 * I suggest people to play with Netperf or any benchmark...
+	 */
+
+	/* NWID (or domain id) */
+	__u32           min_nwid;       /* Minimal NWID we are able to set */
+	__u32           max_nwid;       /* Maximal NWID we are able to set */
+
+	/* Old Frequency (backward compat - moved lower ) */
+	__u16           old_num_channels;
+	__u8            old_num_frequency;
+
+	/* Scan capabilities */
+	__u8            scan_capa;
+};
+
+static int rtl8180_wx_get_range(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct iw_range *range = (struct iw_range *)extra;
+	struct iw_range_with_scan_capa *tmp = (struct iw_range_with_scan_capa *)range;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u16 val;
+	int i;
+
+	wrqu->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+
+	/* Let's try to keep this struct in the same order as in
+	 * linux/include/wireless.h
+	 */
+
+	/* TODO: See what values we can set, and remove the ones we can't
+	 * set, or fill them with some default data.
+	 */
+
+	/* ~5 Mb/s real (802.11b) */
+	range->throughput = 5 * 1000 * 1000;
+
+	/* TODO: Not used in 802.11b? */
+	/* range->min_nwid; */	/* Minimal NWID we are able to set */
+	/* TODO: Not used in 802.11b? */
+	/* range->max_nwid; */	/* Maximal NWID we are able to set */
+
+	/* Old Frequency (backward compat - moved lower ) */
+	/* range->old_num_channels; */
+	/* range->old_num_frequency; */
+	/* range->old_freq[6]; */ /* Filler to keep "version" at the same offset */
+	if (priv->rf_set_sens)
+		range->sensitivity = priv->max_sens;	/* signal level threshold range */
+
+	range->max_qual.qual = 100;
+	/* TODO: Find real max RSSI and stick here */
+	range->max_qual.level = 0;
+	range->max_qual.noise = 0x100 - 98;
+	range->max_qual.updated = 7; /* Updated all three */
+
+	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
+	/* TODO: Find real 'good' to 'bad' threshold value for RSSI */
+	range->avg_qual.level = 0x100 - 78;
+	range->avg_qual.noise = 0;
+	range->avg_qual.updated = 7; /* Updated all three */
+
+	range->num_bitrates = RATE_COUNT;
+
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
+		range->bitrate[i] = rtl8180_rates[i];
+
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+
+	range->min_pmp = 0;
+	range->max_pmp = 5000000;
+	range->min_pmt = 0;
+	range->max_pmt = 65535 * 1000;
+	range->pmp_flags = IW_POWER_PERIOD;
+	range->pmt_flags = IW_POWER_TIMEOUT;
+	range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 16;
+
+	/* range->retry_capa; */	/* What retry options are supported */
+	/* range->retry_flags; */	/* How to decode max/min retry limit */
+	/* range->r_time_flags; */	/* How to decode max/min retry life */
+	/* range->min_retry; */		/* Minimal number of retries */
+	/* range->max_retry; */		/* Maximal number of retries */
+	/* range->min_r_time; */	/* Minimal retry lifetime */
+	/* range->max_r_time; */	/* Maximal retry lifetime */
+
+	for (i = 0, val = 0; i < 14; i++) {
+		/* Include only legal frequencies for some countries */
+		if ((GET_DOT11D_INFO(priv->ieee80211)->channel_map)[i + 1]) {
+			range->freq[val].i = i + 1;
+			range->freq[val].m = ieee80211_wlan_frequencies[i] * 100000;
+			range->freq[val].e = 1;
+			val++;
+		} else {
+			/* FIXME: do we need to set anything for channels */
+			/* we don't use ? */
+		}
+
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+	range->num_channels = val;
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+			  IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+	tmp->scan_capa = 0x01;
+	return 0;
+}
+
+static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	int ret = 0;
+
+	if (!priv->up)
+		return -ENETDOWN;
+
+	if (priv->ieee80211->LinkDetectInfo.bBusyTraffic)
+		return -EAGAIN;
+	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+		struct iw_scan_req *req = (struct iw_scan_req *)b;
+
+		if (req->essid_len) {
+			int len = min_t(int, req->essid_len, IW_ESSID_MAX_SIZE);
+
+			ieee->current_network.ssid_len = len;
+			memcpy(ieee->current_network.ssid, req->essid, len);
+		}
+	}
+
+	mutex_lock(&priv->wx_mutex);
+	if (priv->ieee80211->state != IEEE80211_LINKED) {
+		priv->ieee80211->scanning = 0;
+		ieee80211_softmac_scan_syncro(priv->ieee80211);
+		ret = 0;
+	} else {
+		ret = ieee80211_wx_set_scan(priv->ieee80211, a, wrqu, b);
+	}
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int r8192_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (!priv->up)
+		return -ENETDOWN;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_get_scan(priv->ieee80211, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_set_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *b)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_essid(priv->ieee80211, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_get_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_get_essid(priv->ieee80211, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_set_freq(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_freq(priv->ieee80211, a, wrqu, b);
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int r8192_wx_get_name(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_name(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8192_wx_set_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (wrqu->frag.disabled) {
+		priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
+	} else {
+		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
+		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
+			return -EINVAL;
+
+		priv->ieee80211->fts = wrqu->frag.value & ~0x1;
+	}
+
+	return 0;
+}
+
+static int r8192_wx_get_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	wrqu->frag.value = priv->ieee80211->fts;
+	wrqu->frag.fixed = 0;	/* no auto select */
+	wrqu->frag.disabled = (wrqu->frag.value == DEFAULT_FRAG_THRESHOLD);
+
+	return 0;
+}
+
+static int r8192_wx_set_wap(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *awrq,
+			    char *extra)
+{
+	int ret;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	/* struct sockaddr *temp = (struct sockaddr *)awrq; */
+	mutex_lock(&priv->wx_mutex);
+
+	ret = ieee80211_wx_set_wap(priv->ieee80211, info, awrq, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	return ret;
+}
+
+static int r8192_wx_get_wap(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_wap(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8192_wx_get_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_encode(priv->ieee80211, info, wrqu, key);
+}
+
+static int r8192_wx_set_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	int ret;
+	u32 hwkey[4] = {0, 0, 0, 0};
+	u8 mask = 0xff;
+	u32 key_idx = 0;
+	u8 zero_addr[4][6] = {	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x03} };
+	int i;
+
+	if (!priv->up)
+		return -ENETDOWN;
+
+	mutex_lock(&priv->wx_mutex);
+
+	RT_TRACE(COMP_SEC, "Setting SW wep key");
+	ret = ieee80211_wx_set_encode(priv->ieee80211, info, wrqu, key);
+
+	mutex_unlock(&priv->wx_mutex);
+
+	/* sometimes, the length is zero while we do not type key value */
+	if (wrqu->encoding.length != 0) {
+		for (i = 0; i < 4; i++) {
+			hwkey[i] |=  key[4 * i + 0] & mask;
+			if (i == 1 && (4 * i + 1) == wrqu->encoding.length)
+				mask = 0x00;
+			if (i == 3 && (4 * i + 1) == wrqu->encoding.length)
+				mask = 0x00;
+			hwkey[i] |= (key[4 * i + 1] & mask) << 8;
+			hwkey[i] |= (key[4 * i + 2] & mask) << 16;
+			hwkey[i] |= (key[4 * i + 3] & mask) << 24;
+		}
+
+		#define CONF_WEP40  0x4
+		#define CONF_WEP104 0x14
+
+		switch (wrqu->encoding.flags & IW_ENCODE_INDEX) {
+		case 0:
+			key_idx = ieee->tx_keyidx;
+			break;
+		case 1:
+			key_idx = 0;
+			break;
+		case 2:
+			key_idx = 1;
+			break;
+		case 3:
+			key_idx = 2;
+			break;
+		case 4:
+			key_idx	= 3;
+			break;
+		default:
+			break;
+		}
+
+		if (wrqu->encoding.length == 0x5) {
+			ieee->pairwise_key_type = KEY_TYPE_WEP40;
+			EnableHWSecurityConfig8192(dev);
+
+			setKey(dev,
+			       key_idx,                /* EntryNo */
+			       key_idx,                /* KeyIndex */
+			       KEY_TYPE_WEP40,         /* KeyType */
+			       zero_addr[key_idx],
+			       0,                      /* DefaultKey */
+			       hwkey);                 /* KeyContent */
+		} else if (wrqu->encoding.length == 0xd) {
+			ieee->pairwise_key_type = KEY_TYPE_WEP104;
+			EnableHWSecurityConfig8192(dev);
+
+			setKey(dev,
+			       key_idx,                /* EntryNo */
+			       key_idx,                /* KeyIndex */
+			       KEY_TYPE_WEP104,        /* KeyType */
+			       zero_addr[key_idx],
+			       0,                      /* DefaultKey */
+			       hwkey);                 /* KeyContent */
+		} else {
+			netdev_warn(dev, "wrong type in WEP, not WEP40 and WEP104\n");
+		}
+	}
+
+	return ret;
+}
+
+static int r8192_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa,
+				  union iwreq_data *wrqu, char *p)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int *parms = (int *)p;
+	int mode = parms[0];
+
+	priv->ieee80211->active_scan = mode;
+
+	return 1;
+}
+
+static int r8192_wx_set_retry(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	int err = 0;
+
+	mutex_lock(&priv->wx_mutex);
+
+	if (wrqu->retry.flags & IW_RETRY_LIFETIME ||
+	    wrqu->retry.disabled){
+		err = -EINVAL;
+		goto exit;
+	}
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)) {
+		err = -EINVAL;
+		goto exit;
+	}
+
+	if (wrqu->retry.value > R8180_MAX_RETRY) {
+		err = -EINVAL;
+		goto exit;
+	}
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		priv->retry_rts = wrqu->retry.value;
+		DMESG("Setting retry for RTS/CTS data to %d", wrqu->retry.value);
+
+	} else {
+		priv->retry_data = wrqu->retry.value;
+		DMESG("Setting retry for non RTS/CTS data to %d", wrqu->retry.value);
+	}
+
+	/* FIXME !
+	 * We might try to write directly the TX config register
+	 * or to restart just the (R)TX process.
+	 * I'm unsure if whole reset is really needed
+	 */
+
+	rtl8192_commit(dev);
+exit:
+	mutex_unlock(&priv->wx_mutex);
+
+	return err;
+}
+
+static int r8192_wx_get_retry(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	wrqu->retry.disabled = 0; /* can't be disabled */
+
+	if ((wrqu->retry.flags & IW_RETRY_TYPE) ==
+	    IW_RETRY_LIFETIME)
+		return -EINVAL;
+
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+		wrqu->retry.value = priv->retry_rts;
+	} else {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
+		wrqu->retry.value = priv->retry_data;
+	}
+
+	return 0;
+}
+
+static int r8192_wx_get_sens(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (!priv->rf_set_sens)
+		return -1; /* we have not this support for this radio */
+	wrqu->sens.value = priv->sens;
+	return 0;
+}
+
+static int r8192_wx_set_sens(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	short err = 0;
+
+	mutex_lock(&priv->wx_mutex);
+	if (!priv->rf_set_sens) {
+		err = -1; /* we have not this support for this radio */
+		goto exit;
+	}
+	if (priv->rf_set_sens(dev, wrqu->sens.value) == 0)
+		priv->sens = wrqu->sens.value;
+	else
+		err = -EINVAL;
+
+exit:
+	mutex_unlock(&priv->wx_mutex);
+
+	return err;
+}
+
+/* hw security need to reorganized. */
+static int r8192_wx_set_enc_ext(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	mutex_lock(&priv->wx_mutex);
+	ret = ieee80211_wx_set_encode_ext(priv->ieee80211, info, wrqu, extra);
+
+	{
+		u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		u8 zero[6] = {0};
+		u32 key[4] = {0};
+		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+		struct iw_point *encoding = &wrqu->encoding;
+		u8 idx = 0, alg = 0, group = 0;
+
+		if ((encoding->flags & IW_ENCODE_DISABLED) || ext->alg == IW_ENCODE_ALG_NONE)
+			/* none is not allowed to use hwsec WB 2008.07.01 */
+			goto end_hw_sec;
+
+		/* as IW_ENCODE_ALG_CCMP is defined to be 3 and KEY_TYPE_CCMP is defined to 4; */
+		alg =  (ext->alg == IW_ENCODE_ALG_CCMP) ? KEY_TYPE_CCMP : ext->alg;
+		idx = encoding->flags & IW_ENCODE_INDEX;
+		if (idx)
+			idx--;
+		group = ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY;
+
+		if ((!group) || (ieee->iw_mode == IW_MODE_ADHOC) || (alg ==  KEY_TYPE_WEP40)) {
+			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40))
+				alg = KEY_TYPE_WEP104;
+			ieee->pairwise_key_type = alg;
+			EnableHWSecurityConfig8192(dev);
+		}
+		memcpy((u8 *)key, ext->key, 16); /* we only get 16 bytes key.why? WB 2008.7.1 */
+
+		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode != 2)) {
+			setKey(dev,
+			       idx,	/* EntryNao */
+			       idx,	/* KeyIndex */
+			       alg,	/* KeyType */
+			       zero,	/* MacAddr */
+			       0,	/* DefaultKey */
+			       key);	/* KeyContent */
+		} else if (group) {
+			ieee->group_key_type = alg;
+			setKey(dev,
+			       idx,	/* EntryNo */
+			       idx,	/* KeyIndex */
+			       alg,	/* KeyType */
+			       broadcast_addr,	/* MacAddr */
+			       0,		/* DefaultKey */
+			       key);		/* KeyContent */
+		} else {	/* pairwise key */
+			setKey(dev,
+			       4,	/* EntryNo */
+			       idx,	/* KeyIndex */
+			       alg,	/* KeyType */
+			       (u8 *)ieee->ap_mac_addr,/* MacAddr */
+			       0,			/* DefaultKey */
+			       key);			/* KeyContent */
+		}
+	}
+
+end_hw_sec:
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int r8192_wx_set_auth(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *data, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+	ret = ieee80211_wx_set_auth(priv->ieee80211, info, &data->param, extra);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int r8192_wx_set_mlme(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+	ret = ieee80211_wx_set_mlme(priv->ieee80211, info, wrqu, extra);
+
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int r8192_wx_set_gen_ie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *data, char *extra)
+{
+	int ret = 0;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	mutex_lock(&priv->wx_mutex);
+	ret = ieee80211_wx_set_gen_ie(priv->ieee80211, extra, data->data.length);
+	mutex_unlock(&priv->wx_mutex);
+	return ret;
+}
+
+static int dummy(struct net_device *dev, struct iw_request_info *a,
+		 union iwreq_data *wrqu, char *b)
+{
+	return -1;
+}
+
+static iw_handler r8192_wx_handlers[] = {
+	NULL,                     /* SIOCSIWCOMMIT */
+	r8192_wx_get_name,	  /* SIOCGIWNAME */
+	dummy,                    /* SIOCSIWNWID */
+	dummy,                    /* SIOCGIWNWID */
+	r8192_wx_set_freq,        /* SIOCSIWFREQ */
+	r8192_wx_get_freq,        /* SIOCGIWFREQ */
+	r8192_wx_set_mode,        /* SIOCSIWMODE */
+	r8192_wx_get_mode,        /* SIOCGIWMODE */
+	r8192_wx_set_sens,        /* SIOCSIWSENS */
+	r8192_wx_get_sens,        /* SIOCGIWSENS */
+	NULL,                     /* SIOCSIWRANGE */
+	rtl8180_wx_get_range,	  /* SIOCGIWRANGE */
+	NULL,                     /* SIOCSIWPRIV */
+	NULL,                     /* SIOCGIWPRIV */
+	NULL,                     /* SIOCSIWSTATS */
+	NULL,                     /* SIOCGIWSTATS */
+	dummy,                    /* SIOCSIWSPY */
+	dummy,                    /* SIOCGIWSPY */
+	NULL,                     /* SIOCGIWTHRSPY */
+	NULL,                     /* SIOCWIWTHRSPY */
+	r8192_wx_set_wap,	  /* SIOCSIWAP */
+	r8192_wx_get_wap,         /* SIOCGIWAP */
+	r8192_wx_set_mlme,                     /* MLME-- */
+	dummy,                     /* SIOCGIWAPLIST -- deprecated */
+	r8192_wx_set_scan,        /* SIOCSIWSCAN */
+	r8192_wx_get_scan,        /* SIOCGIWSCAN */
+	r8192_wx_set_essid,       /* SIOCSIWESSID */
+	r8192_wx_get_essid,       /* SIOCGIWESSID */
+	dummy,                    /* SIOCSIWNICKN */
+	dummy,                    /* SIOCGIWNICKN */
+	NULL,                     /* -- hole -- */
+	NULL,                     /* -- hole -- */
+	r8192_wx_set_rate,        /* SIOCSIWRATE */
+	r8192_wx_get_rate,        /* SIOCGIWRATE */
+	r8192_wx_set_rts,                    /* SIOCSIWRTS */
+	r8192_wx_get_rts,                    /* SIOCGIWRTS */
+	r8192_wx_set_frag,        /* SIOCSIWFRAG */
+	r8192_wx_get_frag,        /* SIOCGIWFRAG */
+	dummy,                    /* SIOCSIWTXPOW */
+	dummy,                    /* SIOCGIWTXPOW */
+	r8192_wx_set_retry,       /* SIOCSIWRETRY */
+	r8192_wx_get_retry,       /* SIOCGIWRETRY */
+	r8192_wx_set_enc,         /* SIOCSIWENCODE */
+	r8192_wx_get_enc,         /* SIOCGIWENCODE */
+	r8192_wx_set_power,                    /* SIOCSIWPOWER */
+	r8192_wx_get_power,                    /* SIOCGIWPOWER */
+	NULL,			/*---hole---*/
+	NULL,			/*---hole---*/
+	r8192_wx_set_gen_ie, /* NULL, */		/* SIOCSIWGENIE */
+	NULL,			/* SIOCSIWGENIE */
+
+	r8192_wx_set_auth,/* NULL, */			/* SIOCSIWAUTH */
+	NULL,/* r8192_wx_get_auth, */ /* NULL, */	/* SIOCSIWAUTH */
+	r8192_wx_set_enc_ext,			/* SIOCSIWENCODEEXT */
+	NULL,/* r8192_wx_get_enc_ext, *//* NULL, */			/* SIOCSIWENCODEEXT */
+	NULL,			/* SIOCSIWPMKSA */
+	NULL,			 /*---hole---*/
+
+};
+
+static const struct iw_priv_args r8192_private_args[] = {
+	{
+		SIOCIWFIRSTPRIV + 0x0,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "badcrc"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x1,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "activescan"
+
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x2,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "rawtx"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x3,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "forcereset"
+	}
+};
+
+static iw_handler r8192_private_handler[] = {
+	r8192_wx_set_crcmon,
+	r8192_wx_set_scan_type,
+	r8192_wx_set_rawtx,
+	r8192_wx_force_reset,
+};
+
+struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	struct iw_statistics *wstats = &priv->wstats;
+	int tmp_level = 0;
+	int tmp_qual = 0;
+	int tmp_noise = 0;
+
+	if (ieee->state < IEEE80211_LINKED) {
+		wstats->qual.qual = 0;
+		wstats->qual.level = 0;
+		wstats->qual.noise = 0;
+		wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+		return wstats;
+	}
+
+	tmp_level = (&ieee->current_network)->stats.rssi;
+	tmp_qual = (&ieee->current_network)->stats.signal;
+	tmp_noise = (&ieee->current_network)->stats.noise;
+
+	wstats->qual.level = tmp_level;
+	wstats->qual.qual = tmp_qual;
+	wstats->qual.noise = tmp_noise;
+	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+	return wstats;
+}
+
+const struct iw_handler_def  r8192_wx_handlers_def = {
+	.standard = r8192_wx_handlers,
+	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
+	.private = r8192_private_handler,
+	.num_private = ARRAY_SIZE(r8192_private_handler),
+	.num_private_args = sizeof(r8192_private_args) / sizeof(struct iw_priv_args),
+	.get_wireless_stats = r8192_get_wireless_stats,
+	.private_args = (struct iw_priv_args *)r8192_private_args,
+};
diff --git a/drivers/staging/rtl8192u/r8192U_wx.h b/drivers/staging/rtl8192u/r8192U_wx.h
new file mode 100644
index 000000000..27423cd64
--- /dev/null
+++ b/drivers/staging/rtl8192u/r8192U_wx.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This is part of rtl8180 OpenSource driver - v 0.3
+ * Copyright (C) Andrea Merello 2004  <andrea.merello@gmail.com>
+ *
+ * Parts of this driver are based on the GPL part of the official realtek driver
+ * Parts of this driver are based on the rtl8180 driver skeleton from Patric
+ * Schenke & Andres Salomon
+ * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+ *
+ * We want to thank the Authors of such projects and the Ndiswrapper project
+ * Authors.
+ */
+
+/* this file (will) contains wireless extension handlers */
+
+#ifndef R8180_WX_H
+#define R8180_WX_H
+
+extern const struct iw_handler_def r8192_wx_handlers_def;
+/* Enable  the rtl819x_core.c to share this function, david 2008.9.22 */
+struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev);
+
+#endif
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
new file mode 100644
index 000000000..30a320422
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -0,0 +1,508 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ *  (c) Copyright 2008, RealTEK Technologies Inc. All Rights Reserved.
+ *
+ *  Module:	r819xusb_cmdpkt.c
+ *		(RTL8190 TX/RX command packet handler Source C File)
+ *
+ *  Note:	The module is responsible for handling TX and RX command packet.
+ *		1. TX : Send set and query configuration command packet.
+ *		2. RX : Receive tx feedback, beacon state, query configuration
+ *			command packet.
+ *
+ *  Function:
+ *
+ *  Export:
+ *
+ *  Abbrev:
+ *
+ *  History:
+ *
+ *	Date		Who		Remark
+ *	05/06/2008	amy		Create initial version porting from
+ *					windows driver.
+ *
+ ******************************************************************************/
+#include "r8192U.h"
+#include "r819xU_cmdpkt.h"
+
+rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
+{
+	struct r8192_priv   *priv = ieee80211_priv(dev);
+	struct sk_buff	    *skb;
+	struct cb_desc	    *tcb_desc;
+
+	/* Get TCB and local buffer from common pool.
+	 * (It is shared by CmdQ, MgntQ, and USB coalesce DataQ)
+	 */
+	skb  = dev_alloc_skb(USB_HWDESC_HEADER_LEN + DataLen + 4);
+	if (!skb)
+		return RT_STATUS_FAILURE;
+	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+	tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+	tcb_desc->queue_index = TXCMD_QUEUE;
+	tcb_desc->bCmdOrInit = DESC_PACKET_TYPE_NORMAL;
+	tcb_desc->bLastIniPkt = 0;
+	skb_reserve(skb, USB_HWDESC_HEADER_LEN);
+	skb_put_data(skb, pData, DataLen);
+	tcb_desc->txbuf_size = (u16)DataLen;
+
+	if (!priv->ieee80211->check_nic_enough_desc(dev, tcb_desc->queue_index) ||
+	    (!skb_queue_empty(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index])) ||
+	    (priv->ieee80211->queue_stop)) {
+		RT_TRACE(COMP_FIRMWARE, "=== NULL packet ======> tx full!\n");
+		skb_queue_tail(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index], skb);
+	} else {
+		priv->ieee80211->softmac_hard_start_xmit(skb, dev);
+	}
+
+	return RT_STATUS_SUCCESS;
+}
+
+static void cmpk_count_txstatistic(struct net_device *dev, struct cmd_pkt_tx_feedback *pstx_fb)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+#ifdef ENABLE_PS
+	RT_RF_POWER_STATE	rtState;
+
+	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
+					  (pu1Byte)(&rtState));
+
+	/* When RF is off, we should not count the packet for hw/sw synchronize
+	 * reason, ie. there may be a duration while sw switch is changed and
+	 * hw switch is being changed.
+	 */
+	if (rtState == eRfOff)
+		return;
+#endif
+
+#ifdef TODO
+	if (pAdapter->bInHctTest)
+		return;
+#endif
+	/* We can not know the packet length and transmit type:
+	 * broadcast or uni or multicast. So the relative statistics
+	 * must be collected in tx feedback info.
+	 */
+	if (pstx_fb->tok) {
+		priv->stats.txfeedbackok++;
+		priv->stats.txoktotal++;
+		priv->stats.txokbytestotal += pstx_fb->pkt_length;
+		priv->stats.txokinperiod++;
+
+		/* We can not make sure broadcast/multicast or unicast mode. */
+		if (pstx_fb->pkt_type == PACKET_MULTICAST) {
+			priv->stats.txmulticast++;
+			priv->stats.txbytesmulticast += pstx_fb->pkt_length;
+		} else if (pstx_fb->pkt_type == PACKET_BROADCAST) {
+			priv->stats.txbroadcast++;
+			priv->stats.txbytesbroadcast += pstx_fb->pkt_length;
+		} else {
+			priv->stats.txunicast++;
+			priv->stats.txbytesunicast += pstx_fb->pkt_length;
+		}
+	} else {
+		priv->stats.txfeedbackfail++;
+		priv->stats.txerrtotal++;
+		priv->stats.txerrbytestotal += pstx_fb->pkt_length;
+
+		/* We can not make sure broadcast/multicast or unicast mode. */
+		if (pstx_fb->pkt_type == PACKET_MULTICAST)
+			priv->stats.txerrmulticast++;
+		else if (pstx_fb->pkt_type == PACKET_BROADCAST)
+			priv->stats.txerrbroadcast++;
+		else
+			priv->stats.txerrunicast++;
+	}
+
+	priv->stats.txretrycount += pstx_fb->retry_cnt;
+	priv->stats.txfeedbackretry += pstx_fb->retry_cnt;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:    cmpk_handle_tx_feedback()
+ *
+ * Overview:	The function is responsible for extract the message inside TX
+ *		feedbck message from firmware. It will contain dedicated info in
+ *		ws-06-0063-rtl8190-command-packet-specification.
+ *		Please refer to chapter "TX Feedback Element".
+ *              We have to read 20 bytes in the command packet.
+ *
+ * Input:       struct net_device	*dev
+ *              u8			*pmsg	- Msg Ptr of the command packet.
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ *  When		Who	Remark
+ *  05/08/2008		amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void cmpk_handle_tx_feedback(struct net_device *dev, u8 *pmsg)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct cmd_pkt_tx_feedback rx_tx_fb;
+
+	priv->stats.txfeedback++;
+
+	/* 1. Extract TX feedback info from RFD to temp structure buffer. */
+	/* It seems that FW use big endian(MIPS) and DRV use little endian in
+	 * windows OS. So we have to read the content byte by byte or transfer
+	 * endian type before copy the message copy.
+	 */
+	/* Use pointer to transfer structure memory. */
+	memcpy((u8 *)&rx_tx_fb, pmsg, sizeof(struct cmd_pkt_tx_feedback));
+	/* 2. Use tx feedback info to count TX statistics. */
+	cmpk_count_txstatistic(dev, &rx_tx_fb);
+	/* Comment previous method for TX statistic function. */
+	/* Collect info TX feedback packet to fill TCB. */
+	/* We can not know the packet length and transmit type: broadcast or uni
+	 * or multicast.
+	 */
+}
+
+static void cmdpkt_beacontimerinterrupt_819xusb(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u16 tx_rate;
+
+	/* 87B have to S/W beacon for DTM encryption_cmn. */
+	if (priv->ieee80211->current_network.mode == IEEE_A ||
+	    priv->ieee80211->current_network.mode == IEEE_N_5G ||
+	    (priv->ieee80211->current_network.mode == IEEE_N_24G &&
+	     (!priv->ieee80211->pHTInfo->bCurSuppCCK))) {
+		tx_rate = 60;
+		DMESG("send beacon frame  tx rate is 6Mbpm\n");
+	} else {
+		tx_rate = 10;
+		DMESG("send beacon frame  tx rate is 1Mbpm\n");
+	}
+
+	rtl819xusb_beacon_tx(dev, tx_rate); /* HW Beacon */
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:    cmpk_handle_interrupt_status()
+ *
+ * Overview:    The function is responsible for extract the message from
+ *		firmware. It will contain dedicated info in
+ *		ws-07-0063-v06-rtl819x-command-packet-specification-070315.doc.
+ *		Please refer to chapter "Interrupt Status Element".
+ *
+ * Input:       struct net_device *dev
+ *              u8 *pmsg		- Message Pointer of the command packet.
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ *  When		Who	Remark
+ *  05/12/2008		amy	Add this for rtl8192 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void cmpk_handle_interrupt_status(struct net_device *dev, u8 *pmsg)
+{
+	struct cmd_pkt_interrupt_status	 rx_intr_status;	/* */
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	DMESG("---> cmpk_Handle_Interrupt_Status()\n");
+
+	/* 1. Extract TX feedback info from RFD to temp structure buffer. */
+	/* It seems that FW use big endian(MIPS) and DRV use little endian in
+	 * windows OS. So we have to read the content byte by byte or transfer
+	 * endian type before copy the message copy.
+	 */
+	rx_intr_status.length = pmsg[1];
+	if (rx_intr_status.length != (sizeof(struct cmd_pkt_interrupt_status) - 2)) {
+		DMESG("cmpk_Handle_Interrupt_Status: wrong length!\n");
+		return;
+	}
+
+	/* Statistics of beacon for ad-hoc mode. */
+	if (priv->ieee80211->iw_mode == IW_MODE_ADHOC) {
+		/* 2 maybe need endian transform? */
+		rx_intr_status.interrupt_status = *((u32 *)(pmsg + 4));
+
+		DMESG("interrupt status = 0x%x\n",
+		      rx_intr_status.interrupt_status);
+
+		if (rx_intr_status.interrupt_status & ISR_TX_BCN_OK) {
+			priv->ieee80211->bibsscoordinator = true;
+			priv->stats.txbeaconokint++;
+		} else if (rx_intr_status.interrupt_status & ISR_TX_BCN_ERR) {
+			priv->ieee80211->bibsscoordinator = false;
+			priv->stats.txbeaconerr++;
+		}
+
+		if (rx_intr_status.interrupt_status & ISR_BCN_TIMER_INTR)
+			cmdpkt_beacontimerinterrupt_819xusb(dev);
+	}
+
+	/* Other information in interrupt status we need? */
+
+	DMESG("<---- cmpk_handle_interrupt_status()\n");
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	cmpk_count_tx_status()
+ *
+ * Overview:	Count aggregated tx status from firmwar of one type rx command
+ *		packet element id = RX_TX_STATUS.
+ *
+ * Input:	NONE
+ *
+ * Output:	NONE
+ *
+ * Return:	NONE
+ *
+ * Revised History:
+ *	When		Who	Remark
+ *	05/12/2008	amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void cmpk_count_tx_status(struct net_device *dev,
+				 cmpk_tx_status_t *pstx_status)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+#ifdef ENABLE_PS
+
+	RT_RF_POWER_STATE	rtstate;
+
+	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
+					  (pu1Byte)(&rtState));
+
+	/* When RF is off, we should not count the packet for hw/sw synchronize
+	 * reason, ie. there may be a duration while sw switch is changed and
+	 * hw switch is being changed.
+	 */
+	if (rtState == eRfOff)
+		return;
+#endif
+
+	priv->stats.txfeedbackok	+= pstx_status->txok;
+	priv->stats.txoktotal		+= pstx_status->txok;
+
+	priv->stats.txfeedbackfail	+= pstx_status->txfail;
+	priv->stats.txerrtotal		+= pstx_status->txfail;
+
+	priv->stats.txretrycount	+= pstx_status->txretry;
+	priv->stats.txfeedbackretry	+= pstx_status->txretry;
+
+	priv->stats.txmulticast		+= pstx_status->txmcok;
+	priv->stats.txbroadcast		+= pstx_status->txbcok;
+	priv->stats.txunicast		+= pstx_status->txucok;
+
+	priv->stats.txerrmulticast	+= pstx_status->txmcfail;
+	priv->stats.txerrbroadcast	+= pstx_status->txbcfail;
+	priv->stats.txerrunicast	+= pstx_status->txucfail;
+
+	priv->stats.txbytesmulticast	+= pstx_status->txmclength;
+	priv->stats.txbytesbroadcast	+= pstx_status->txbclength;
+	priv->stats.txbytesunicast	+= pstx_status->txuclength;
+
+	priv->stats.last_packet_rate	= pstx_status->rate;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	cmpk_handle_tx_status()
+ *
+ * Overview:	Firmware add a new tx feedback status to reduce rx command
+ *		packet buffer operation load.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who	Remark
+ *	05/12/2008	amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void cmpk_handle_tx_status(struct net_device *dev, u8 *pmsg)
+{
+	cmpk_tx_status_t	rx_tx_sts;
+
+	memcpy((void *)&rx_tx_sts, (void *)pmsg, sizeof(cmpk_tx_status_t));
+	/* 2. Use tx feedback info to count TX statistics. */
+	cmpk_count_tx_status(dev, &rx_tx_sts);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	cmpk_handle_tx_rate_history()
+ *
+ * Overview:	Firmware add a new tx rate history
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who	Remark
+ *	05/12/2008	amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+static void cmpk_handle_tx_rate_history(struct net_device *dev, u8 *pmsg)
+{
+	cmpk_tx_rahis_t	*ptxrate;
+	u8		i, j;
+	u16		length = sizeof(cmpk_tx_rahis_t);
+	u32		*ptemp;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+#ifdef ENABLE_PS
+	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
+					  (pu1Byte)(&rtState));
+
+	/* When RF is off, we should not count the packet for hw/sw synchronize
+	 * reason, ie. there may be a duration while sw switch is changed and
+	 * hw switch is being changed.
+	 */
+	if (rtState == eRfOff)
+		return;
+#endif
+
+	ptemp = (u32 *)pmsg;
+
+	/* Do endian transfer to word alignment(16 bits) for windows system.
+	 * You must do different endian transfer for linux and MAC OS
+	 */
+	for (i = 0; i < (length/4); i++) {
+		u16	 temp1, temp2;
+
+		temp1 = ptemp[i] & 0x0000FFFF;
+		temp2 = ptemp[i] >> 16;
+		ptemp[i] = (temp1 << 16) | temp2;
+	}
+
+	ptxrate = (cmpk_tx_rahis_t *)pmsg;
+
+	if (!ptxrate)
+		return;
+
+	for (i = 0; i < 16; i++) {
+		/* Collect CCK rate packet num */
+		if (i < 4)
+			priv->stats.txrate.cck[i] += ptxrate->cck[i];
+
+		/* Collect OFDM rate packet num */
+		if (i < 8)
+			priv->stats.txrate.ofdm[i] += ptxrate->ofdm[i];
+
+		for (j = 0; j < 4; j++)
+			priv->stats.txrate.ht_mcs[j][i] += ptxrate->ht_mcs[j][i];
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:    cmpk_message_handle_rx()
+ *
+ * Overview:    In the function, we will capture different RX command packet
+ *		info. Every RX command packet element has different message
+ *		length and meaning in content. We only support three type of RX
+ *		command packet now. Please refer to document
+ *		ws-06-0063-rtl8190-command-packet-specification.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ *  When		Who	Remark
+ *  05/06/2008		amy	Create Version 0 porting from windows code.
+ *
+ *---------------------------------------------------------------------------
+ */
+u32 cmpk_message_handle_rx(struct net_device *dev,
+			   struct ieee80211_rx_stats *pstats)
+{
+	int			total_length;
+	u8			cmd_length, exe_cnt = 0;
+	u8			element_id;
+	u8			*pcmd_buff;
+
+	/* 0. Check inpt arguments. It is a command queue message or
+	 * pointer is null.
+	 */
+	if (!pstats)
+		return 0;	/* This is not a command packet. */
+
+	/* 1. Read received command packet message length from RFD. */
+	total_length = pstats->Length;
+
+	/* 2. Read virtual address from RFD. */
+	pcmd_buff = pstats->virtual_address;
+
+	/* 3. Read command packet element id and length. */
+	element_id = pcmd_buff[0];
+
+	/* 4. Check every received command packet content according to different
+	 *    element type. Because FW may aggregate RX command packet to
+	 *    minimize transmit time between DRV and FW.
+	 */
+	/* Add a counter to prevent the lock in the loop from being held too
+	 * long
+	 */
+	while (total_length > 0 && exe_cnt++ < 100) {
+		/* We support aggregation of different cmd in the same packet */
+		element_id = pcmd_buff[0];
+
+		switch (element_id) {
+		case RX_TX_FEEDBACK:
+			cmpk_handle_tx_feedback(dev, pcmd_buff);
+			cmd_length = CMPK_RX_TX_FB_SIZE;
+			break;
+
+		case RX_INTERRUPT_STATUS:
+			cmpk_handle_interrupt_status(dev, pcmd_buff);
+			cmd_length = sizeof(struct cmd_pkt_interrupt_status);
+			break;
+
+		case BOTH_QUERY_CONFIG:
+			cmd_length = CMPK_BOTH_QUERY_CONFIG_SIZE;
+			break;
+
+		case RX_TX_STATUS:
+			cmpk_handle_tx_status(dev, pcmd_buff);
+			cmd_length = CMPK_RX_TX_STS_SIZE;
+			break;
+
+		case RX_TX_PER_PKT_FEEDBACK:
+			/* You must at lease add a switch case element here,
+			 * Otherwise, we will jump to default case.
+			 */
+			cmd_length = CMPK_RX_TX_FB_SIZE;
+			break;
+
+		case RX_TX_RATE_HISTORY:
+			cmpk_handle_tx_rate_history(dev, pcmd_buff);
+			cmd_length = CMPK_TX_RAHIS_SIZE;
+			break;
+
+		default:
+
+			RT_TRACE(COMP_ERR, "---->%s():unknown CMD Element\n",
+				 __func__);
+			return 1;	/* This is a command packet. */
+		}
+
+		total_length -= cmd_length;
+		pcmd_buff    += cmd_length;
+	}
+	return	1;	/* This is a command packet. */
+}
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.h b/drivers/staging/rtl8192u/r819xU_cmdpkt.h
new file mode 100644
index 000000000..be45cd609
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.h
@@ -0,0 +1,190 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef R819XUSB_CMDPKT_H
+#define R819XUSB_CMDPKT_H
+/* Different command packet have dedicated message length and definition. */
+#define		CMPK_RX_TX_FB_SIZE		sizeof(struct cmd_pkt_tx_feedback)	/* 20 */
+#define		CMPK_BOTH_QUERY_CONFIG_SIZE	sizeof(struct cmd_pkt_set_configuration)	/* 16 */
+#define		CMPK_RX_TX_STS_SIZE		sizeof(cmpk_tx_status_t)
+#define		CMPK_TX_RAHIS_SIZE		sizeof(cmpk_tx_rahis_t)
+
+/* 2008/05/08 amy For USB constant. */
+#define ISR_TX_BCN_OK		BIT(27)		/* Transmit Beacon OK */
+#define ISR_TX_BCN_ERR		BIT(26)		/* Transmit Beacon Error */
+#define ISR_BCN_TIMER_INTR	BIT(13)		/* Beacon Timer Interrupt */
+
+/* Define element ID of command packet. */
+
+/*------------------------------Define structure----------------------------*/
+/* Define different command packet structure. */
+/* 1. RX side: TX feedback packet. */
+struct cmd_pkt_tx_feedback {
+	/* DWORD 0 */
+	u8	element_id;			/* Command packet type. */
+	u8	length;				/* Command packet length. */
+	/* Change tx feedback info field. */
+	/*------TX Feedback Info Field */
+	u8	TID:4;
+	u8	fail_reason:3;
+	u8	tok:1;				/* Transmit ok. */
+	u8	reserve1:4;
+	u8	pkt_type:2;
+	u8	bandwidth:1;
+	u8	qos_pkt:1;
+
+	/* DWORD 1 */
+	u8	reserve2;
+	/*------TX Feedback Info Field */
+	u8	retry_cnt;
+	u16	pkt_id;
+
+	/* DWORD 3 */
+	u16	seq_num;
+	u8	s_rate;				/* Start rate. */
+	u8	f_rate;				/* Final rate. */
+
+	/* DWORD 4 */
+	u8	s_rts_rate;
+	u8	f_rts_rate;
+	u16	pkt_length;
+
+	/* DWORD 5 */
+	u16	reserve3;
+	u16	duration;
+};
+
+/* 2. RX side: Interrupt status packet. It includes Beacon State,
+ * Beacon Timer Interrupt and other useful information in MAC ISR Reg.
+ */
+struct cmd_pkt_interrupt_status {
+	u8	element_id;			/* Command packet type. */
+	u8	length;				/* Command packet length. */
+	u16	reserve;
+	u32	interrupt_status;		/* Interrupt Status. */
+};
+
+/* 3. TX side: Set configuration packet. */
+struct cmd_pkt_set_configuration {
+	u8	element_id;			/* Command packet type. */
+	u8	length;				/* Command packet length. */
+	u16	reserve1;
+	/* Configuration info. */
+	u8	cfg_reserve1:3;
+	u8	cfg_size:2;
+	u8	cfg_type:2;
+	u8	cfg_action:1;
+	u8	cfg_reserve2;
+	u8	cfg_page:4;
+	u8	cfg_reserve3:4;
+	u8	cfg_offset;
+	u32	value;
+	u32	mask;
+};
+
+/* 4. Both side : TX/RX query configuration packet. The query structure is the
+ *    same as set configuration.
+ */
+#define		cmpk_query_cfg	cmd_pkt_set_configuration
+
+/* 5. Multi packet feedback status. */
+typedef struct tag_tx_stats_feedback {
+	/* For endian transfer --> Driver will not the same as
+	 *  firmware structure.
+	 */
+	/* DW 0 */
+	u16	reserve1;
+	u8	length;				/* Command packet length */
+	u8	element_id;			/* Command packet type */
+
+	/* DW 1 */
+	u16	txfail;				/* Tx fail count */
+	u16	txok;				/* Tx ok count */
+
+	/* DW 2 */
+	u16	txmcok;				/* Tx multicast */
+	u16	txretry;			/* Tx retry count */
+
+	/* DW 3 */
+	u16	txucok;				/* Tx unicast */
+	u16	txbcok;				/* Tx broadcast */
+
+	/* DW 4 */
+	u16	txbcfail;
+	u16	txmcfail;
+
+	/* DW 5 */
+	u16	reserve2;
+	u16	txucfail;
+
+	/* DW 6-8 */
+	u32	txmclength;
+	u32	txbclength;
+	u32	txuclength;
+
+	/* DW 9 */
+	u16	reserve3_23;
+	u8	reserve3_1;
+	u8	rate;
+} __packed cmpk_tx_status_t;
+
+/* 6. Debug feedback message. */
+/* Define RX debug message  */
+typedef struct tag_rx_debug_message_feedback {
+	/* For endian transfer --> for driver */
+	/* DW 0 */
+	u16	reserve1;
+	u8	length;				/* Command packet length */
+	u8	element_id;			/* Command packet type */
+
+	/* DW 1-?? */
+	/* Variable debug message. */
+
+} cmpk_rx_dbginfo_t;
+
+/* Define transmit rate history. For big endian format. */
+typedef struct tag_tx_rate_history {
+	/* For endian transfer --> for driver */
+	/* DW 0 */
+	u8	element_id;			/* Command packet type */
+	u8	length;				/* Command packet length */
+	u16	reserved1;
+
+	/* DW 1-2	CCK rate counter */
+	u16	cck[4];
+
+	/* DW 3-6 */
+	u16	ofdm[8];
+
+	/* DW 7-14	BW=0 SG=0
+	 * DW 15-22	BW=1 SG=0
+	 * DW 23-30	BW=0 SG=1
+	 * DW 31-38	BW=1 SG=1
+	 */
+	u16	ht_mcs[4][16];
+
+} __packed cmpk_tx_rahis_t;
+
+typedef enum tag_command_packet_directories {
+	RX_TX_FEEDBACK			= 0,
+	RX_INTERRUPT_STATUS		= 1,
+	TX_SET_CONFIG			= 2,
+	BOTH_QUERY_CONFIG		= 3,
+	RX_TX_STATUS			= 4,
+	RX_DBGINFO_FEEDBACK		= 5,
+	RX_TX_PER_PKT_FEEDBACK		= 6,
+	RX_TX_RATE_HISTORY		= 7,
+	RX_CMD_ELE_MAX
+} cmpk_element_e;
+
+typedef enum _rt_status {
+	RT_STATUS_SUCCESS,
+	RT_STATUS_FAILURE,
+	RT_STATUS_PENDING,
+	RT_STATUS_RESOURCE
+} rt_status, *prt_status;
+
+u32 cmpk_message_handle_rx(struct net_device *dev,
+			   struct ieee80211_rx_stats *pstats);
+rt_status SendTxCommandPacket(struct net_device *dev,
+			      void *pData, u32 DataLen);
+
+#endif
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
new file mode 100644
index 000000000..4f8629e47
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -0,0 +1,340 @@
+// SPDX-License-Identifier: GPL-2.0
+/**************************************************************************************************
+ * Procedure:    Init boot code/firmware code/data session
+ *
+ * Description: This routine will initialize firmware. If any error occurs during the initialization
+ *		process, the routine shall terminate immediately and return fail.
+ *		NIC driver should call NdisOpenFile only from MiniportInitialize.
+ *
+ * Arguments:   The pointer of the adapter
+
+ * Returns:
+ *        NDIS_STATUS_FAILURE - the following initialization process should be terminated
+ *        NDIS_STATUS_SUCCESS - if firmware initialization process success
+ **************************************************************************************************/
+
+#include "r8192U.h"
+#include "r8192U_hw.h"
+#include "r819xU_firmware_img.h"
+#include "r819xU_firmware.h"
+#include <linux/firmware.h>
+
+static void firmware_init_param(struct net_device *dev)
+{
+	struct r8192_priv	*priv = ieee80211_priv(dev);
+	rt_firmware		*pfirmware = priv->pFirmware;
+
+	pfirmware->cmdpacket_frag_threshold = GET_COMMAND_PACKET_FRAG_THRESHOLD(MAX_TRANSMIT_BUFFER_SIZE);
+}
+
+/*
+ * segment the img and use the ptr and length to remember info on each segment
+ *
+ */
+static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
+			     u32 buffer_len)
+{
+	struct r8192_priv   *priv = ieee80211_priv(dev);
+	bool		    rt_status = true;
+	u16		    frag_threshold;
+	u16		    frag_length, frag_offset = 0;
+	int		    i;
+
+	rt_firmware	    *pfirmware = priv->pFirmware;
+	struct sk_buff	    *skb;
+	unsigned char	    *seg_ptr;
+	struct cb_desc		    *tcb_desc;
+	u8                  bLastIniPkt;
+	u8		    index;
+
+	firmware_init_param(dev);
+	/* Fragmentation might be required */
+	frag_threshold = pfirmware->cmdpacket_frag_threshold;
+	do {
+		if ((buffer_len - frag_offset) > frag_threshold) {
+			frag_length = frag_threshold;
+			bLastIniPkt = 0;
+		} else {
+			frag_length = buffer_len - frag_offset;
+			bLastIniPkt = 1;
+		}
+
+		/* Allocate skb buffer to contain firmware info and tx descriptor info
+		 * add 4 to avoid packet appending overflow.
+		 */
+		skb  = dev_alloc_skb(USB_HWDESC_HEADER_LEN + frag_length + 4);
+		if (!skb)
+			return false;
+		memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
+		tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		tcb_desc->queue_index = TXCMD_QUEUE;
+		tcb_desc->bCmdOrInit = DESC_PACKET_TYPE_INIT;
+		tcb_desc->bLastIniPkt = bLastIniPkt;
+
+		skb_reserve(skb, USB_HWDESC_HEADER_LEN);
+		seg_ptr = skb->data;
+		/*
+		 * Transform from little endian to big endian
+		 * and pending  zero
+		 */
+		for (i = 0; i < frag_length; i += 4) {
+			*seg_ptr++ = ((i+0) < frag_length)?code_virtual_address[i+3] : 0;
+			*seg_ptr++ = ((i+1) < frag_length)?code_virtual_address[i+2] : 0;
+			*seg_ptr++ = ((i+2) < frag_length)?code_virtual_address[i+1] : 0;
+			*seg_ptr++ = ((i+3) < frag_length)?code_virtual_address[i+0] : 0;
+		}
+		tcb_desc->txbuf_size = (u16)i;
+		skb_put(skb, i);
+
+		index = tcb_desc->queue_index;
+		if (!priv->ieee80211->check_nic_enough_desc(dev, index) ||
+		       (!skb_queue_empty(&priv->ieee80211->skb_waitQ[index])) ||
+		       (priv->ieee80211->queue_stop)) {
+			RT_TRACE(COMP_FIRMWARE, "=====================================================> tx full!\n");
+			skb_queue_tail(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index], skb);
+		} else {
+			priv->ieee80211->softmac_hard_start_xmit(skb, dev);
+		}
+
+		code_virtual_address += frag_length;
+		frag_offset += frag_length;
+
+	} while (frag_offset < buffer_len);
+
+	return rt_status;
+}
+
+/*
+ * Procedure:	Check whether main code is download OK. If OK, turn on CPU
+ *
+ * Description:	CPU register locates in different page against general register.
+ *	    Switch to CPU register in the begin and switch back before return
+ *
+ *
+ * Arguments:   The pointer of the adapter
+ *
+ * Returns:
+ *        NDIS_STATUS_FAILURE - the following initialization process should
+ *				be terminated
+ *        NDIS_STATUS_SUCCESS - if firmware initialization process success
+ */
+static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
+{
+	bool		rt_status = true;
+	int		check_putcodeOK_time = 200000, check_bootOk_time = 200000;
+	u32		CPU_status = 0;
+
+	/* Check whether put code OK */
+	do {
+		read_nic_dword(dev, CPU_GEN, &CPU_status);
+
+		if (CPU_status&CPU_GEN_PUT_CODE_OK)
+			break;
+
+	} while (check_putcodeOK_time--);
+
+	if (!(CPU_status&CPU_GEN_PUT_CODE_OK)) {
+		RT_TRACE(COMP_ERR, "Download Firmware: Put code fail!\n");
+		goto CPUCheckMainCodeOKAndTurnOnCPU_Fail;
+	} else {
+		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Put code ok!\n");
+	}
+
+	/* Turn On CPU */
+	read_nic_dword(dev, CPU_GEN, &CPU_status);
+	write_nic_byte(dev, CPU_GEN,
+		       (u8)((CPU_status | CPU_GEN_PWR_STB_CPU) & 0xff));
+	mdelay(1000);
+
+	/* Check whether CPU boot OK */
+	do {
+		read_nic_dword(dev, CPU_GEN, &CPU_status);
+
+		if (CPU_status&CPU_GEN_BOOT_RDY)
+			break;
+	} while (check_bootOk_time--);
+
+	if (!(CPU_status&CPU_GEN_BOOT_RDY))
+		goto CPUCheckMainCodeOKAndTurnOnCPU_Fail;
+	else
+		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Boot ready!\n");
+
+	return rt_status;
+
+CPUCheckMainCodeOKAndTurnOnCPU_Fail:
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
+	rt_status = false;
+	return rt_status;
+}
+
+static bool CPUcheck_firmware_ready(struct net_device *dev)
+{
+	bool		rt_status = true;
+	int		check_time = 200000;
+	u32		CPU_status = 0;
+
+	/* Check Firmware Ready */
+	do {
+		read_nic_dword(dev, CPU_GEN, &CPU_status);
+
+		if (CPU_status&CPU_GEN_FIRM_RDY)
+			break;
+
+	} while (check_time--);
+
+	if (!(CPU_status&CPU_GEN_FIRM_RDY))
+		goto CPUCheckFirmwareReady_Fail;
+	else
+		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Firmware ready!\n");
+
+	return rt_status;
+
+CPUCheckFirmwareReady_Fail:
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
+	rt_status = false;
+	return rt_status;
+}
+
+bool init_firmware(struct net_device *dev)
+{
+	struct r8192_priv	*priv = ieee80211_priv(dev);
+	bool			rt_status = true;
+
+	u32			file_length = 0;
+	u8			*mapped_file = NULL;
+	u32			init_step = 0;
+	enum opt_rst_type_e	   rst_opt = OPT_SYSTEM_RESET;
+	enum firmware_init_step_e  starting_state = FW_INIT_STEP0_BOOT;
+
+	rt_firmware		*pfirmware = priv->pFirmware;
+	const struct firmware	*fw_entry;
+	const char *fw_name[3] = { "RTL8192U/boot.img",
+			   "RTL8192U/main.img",
+			   "RTL8192U/data.img"};
+	int rc;
+
+	RT_TRACE(COMP_FIRMWARE, " PlatformInitFirmware()==>\n");
+
+	if (pfirmware->firmware_status == FW_STATUS_0_INIT) {
+		/* it is called by reset */
+		rst_opt = OPT_SYSTEM_RESET;
+		starting_state = FW_INIT_STEP0_BOOT;
+		/* TODO: system reset */
+
+	} else if (pfirmware->firmware_status == FW_STATUS_5_READY) {
+		/* it is called by Initialize */
+		rst_opt = OPT_FIRMWARE_RESET;
+		starting_state = FW_INIT_STEP2_DATA;
+	} else {
+		RT_TRACE(COMP_FIRMWARE, "PlatformInitFirmware: undefined firmware state\n");
+	}
+
+	/*
+	 * Download boot, main, and data image for System reset.
+	 * Download data image for firmware reset
+	 */
+	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA; init_step++) {
+		/*
+		 * Open image file, and map file to continuous memory if open file success.
+		 * or read image file from array. Default load from IMG file
+		 */
+		if (rst_opt == OPT_SYSTEM_RESET) {
+			rc = request_firmware(&fw_entry, fw_name[init_step], &priv->udev->dev);
+			if (rc < 0) {
+				RT_TRACE(COMP_ERR, "request firmware fail!\n");
+				goto download_firmware_fail;
+			}
+
+			if (fw_entry->size > sizeof(pfirmware->firmware_buf)) {
+				RT_TRACE(COMP_ERR, "img file size exceed the container buffer fail!\n");
+				goto download_firmware_fail;
+			}
+
+			if (init_step != FW_INIT_STEP1_MAIN) {
+				memcpy(pfirmware->firmware_buf, fw_entry->data, fw_entry->size);
+				mapped_file = pfirmware->firmware_buf;
+				file_length = fw_entry->size;
+			} else {
+				memset(pfirmware->firmware_buf, 0, 128);
+				memcpy(&pfirmware->firmware_buf[128], fw_entry->data, fw_entry->size);
+				mapped_file = pfirmware->firmware_buf;
+				file_length = fw_entry->size + 128;
+			}
+			pfirmware->firmware_buf_size = file_length;
+		} else if (rst_opt == OPT_FIRMWARE_RESET) {
+			/* we only need to download data.img here */
+			mapped_file = pfirmware->firmware_buf;
+			file_length = pfirmware->firmware_buf_size;
+		}
+
+		/* Download image file */
+		/* The firmware download process is just as following,
+		 * 1. that is each packet will be segmented and inserted to the wait queue.
+		 * 2. each packet segment will be put in the skb_buff packet.
+		 * 3. each skb_buff packet data content will already include the firmware info
+		 *   and Tx descriptor info
+		 */
+		rt_status = fw_download_code(dev, mapped_file, file_length);
+		if (rst_opt == OPT_SYSTEM_RESET)
+			release_firmware(fw_entry);
+
+		if (!rt_status)
+			goto download_firmware_fail;
+
+		switch (init_step) {
+		case FW_INIT_STEP0_BOOT:
+			/* Download boot
+			 * initialize command descriptor.
+			 * will set polling bit when firmware code is also configured
+			 */
+			pfirmware->firmware_status = FW_STATUS_1_MOVE_BOOT_CODE;
+			/* mdelay(1000); */
+			/*
+			 * To initialize IMEM, CPU move code  from 0x80000080,
+			 * hence, we send 0x80 byte packet
+			 */
+			break;
+
+		case FW_INIT_STEP1_MAIN:
+			/* Download firmware code. Wait until Boot Ready and Turn on CPU */
+			pfirmware->firmware_status = FW_STATUS_2_MOVE_MAIN_CODE;
+
+			/* Check Put Code OK and Turn On CPU */
+			rt_status = CPUcheck_maincodeok_turnonCPU(dev);
+			if (!rt_status) {
+				RT_TRACE(COMP_ERR, "CPUcheck_maincodeok_turnonCPU fail!\n");
+				goto download_firmware_fail;
+			}
+
+			pfirmware->firmware_status = FW_STATUS_3_TURNON_CPU;
+			break;
+
+		case FW_INIT_STEP2_DATA:
+			/* download initial data code */
+			pfirmware->firmware_status = FW_STATUS_4_MOVE_DATA_CODE;
+			mdelay(1);
+
+			rt_status = CPUcheck_firmware_ready(dev);
+			if (!rt_status) {
+				RT_TRACE(COMP_ERR, "CPUcheck_firmware_ready fail(%d)!\n", rt_status);
+				goto download_firmware_fail;
+			}
+
+			/* wait until data code is initialized ready.*/
+			pfirmware->firmware_status = FW_STATUS_5_READY;
+			break;
+		}
+	}
+
+	RT_TRACE(COMP_FIRMWARE, "Firmware Download Success\n");
+	return rt_status;
+
+download_firmware_fail:
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
+	rt_status = false;
+	return rt_status;
+}
+
+MODULE_FIRMWARE("RTL8192U/boot.img");
+MODULE_FIRMWARE("RTL8192U/main.img");
+MODULE_FIRMWARE("RTL8192U/data.img");
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.h b/drivers/staging/rtl8192u/r819xU_firmware.h
new file mode 100644
index 000000000..b84344c1e
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_firmware.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __INC_FIRMWARE_H
+#define __INC_FIRMWARE_H
+
+#define GET_COMMAND_PACKET_FRAG_THRESHOLD(v) \
+	(4 * ((v) / 4) - 8 - USB_HWDESC_HEADER_LEN)
+
+enum firmware_init_step_e {
+	FW_INIT_STEP0_BOOT = 0,
+	FW_INIT_STEP1_MAIN = 1,
+	FW_INIT_STEP2_DATA = 2,
+};
+
+enum opt_rst_type_e {
+	OPT_SYSTEM_RESET = 0,
+	OPT_FIRMWARE_RESET = 1,
+};
+
+#endif
diff --git a/drivers/staging/rtl8192u/r819xU_firmware_img.c b/drivers/staging/rtl8192u/r819xU_firmware_img.c
new file mode 100644
index 000000000..0af062036
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_firmware_img.c
@@ -0,0 +1,549 @@
+// SPDX-License-Identifier: GPL-2.0
+/*Created on  2008/ 7/16,  5:31*/
+#include <linux/types.h>
+#include "r819xU_firmware_img.h"
+
+u32 Rtl8192UsbPHY_REGArray[] = {
+	0x0, };
+
+u32 Rtl8192UsbPHY_REG_1T2RArray[] = {
+	0x800, 0x00000000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000001c,
+	0x810, 0x801010aa,
+	0x814, 0x008514d0,
+	0x818, 0x00000040,
+	0x81c, 0x00000000,
+	0x820, 0x00000004,
+	0x824, 0x00690000,
+	0x828, 0x00000004,
+	0x82c, 0x00e90000,
+	0x830, 0x00000004,
+	0x834, 0x00690000,
+	0x838, 0x00000004,
+	0x83c, 0x00e90000,
+	0x840, 0x00000000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x65a965a9,
+	0x85c, 0x65a965a9,
+	0x860, 0x001f0010,
+	0x864, 0x007f0010,
+	0x868, 0x001f0010,
+	0x86c, 0x007f0010,
+	0x870, 0x0f100f70,
+	0x874, 0x0f100f70,
+	0x878, 0x00000000,
+	0x87c, 0x00000000,
+	0x880, 0x6870e36c,
+	0x884, 0xe3573600,
+	0x888, 0x4260c340,
+	0x88c, 0x0000ff00,
+	0x890, 0x00000000,
+	0x894, 0xfffffffe,
+	0x898, 0x4c42382f,
+	0x89c, 0x00656056,
+	0x8b0, 0x00000000,
+	0x8e0, 0x00000000,
+	0x8e4, 0x00000000,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x31121311,
+	0xa00, 0x00d0c7d8,
+	0xa04, 0x811f0008,
+	0xa08, 0x80cd8300,
+	0xa0c, 0x2e62740f,
+	0xa10, 0x95009b78,
+	0xa14, 0x11145008,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140fa0,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00000000,
+	0xc00, 0x00000040,
+	0xc04, 0x00005433,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08000000,
+	0xc1c, 0x40000100,
+	0xc20, 0x08000000,
+	0xc24, 0x40000100,
+	0xc28, 0x08000000,
+	0xc2c, 0x40000100,
+	0xc30, 0x6de9ac44,
+	0xc34, 0x465c52cd,
+	0xc38, 0x497f5994,
+	0xc3c, 0x0a969764,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020000,
+	0xc4c, 0x00000300,
+	0xc50, 0x69543420,
+	0xc54, 0x433c0094,
+	0xc58, 0x69543420,
+	0xc5c, 0x433c0094,
+	0xc60, 0x69543420,
+	0xc64, 0x433c0094,
+	0xc68, 0x69543420,
+	0xc6c, 0x433c0094,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x0186175b,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x40000100,
+	0xc84, 0x20000000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x40000100,
+	0xc94, 0x00000000,
+	0xc98, 0x40000100,
+	0xc9c, 0x00000000,
+	0xca0, 0x00492492,
+	0xca4, 0x00000000,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x00492492,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xd00, 0x00000750,
+	0xd04, 0x00000403,
+	0xd08, 0x0000907f,
+	0xd0c, 0x00000001,
+	0xd10, 0xa0633333,
+	0xd14, 0x33333c63,
+	0xd18, 0x6a8f5b6b,
+	0xd1c, 0x00000000,
+	0xd20, 0x00000000,
+	0xd24, 0x00000000,
+	0xd28, 0x00000000,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x00000000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x024dbd02,
+	0xd58, 0x00000000,
+	0xd5c, 0x04032064,
+	0xe00, 0x161a1a1a,
+	0xe04, 0x12121416,
+	0xe08, 0x00001800,
+	0xe0c, 0x00000000,
+	0xe10, 0x161a1a1a,
+	0xe14, 0x12121416,
+	0xe18, 0x161a1a1a,
+	0xe1c, 0x12121416,
+};
+
+u32 Rtl8192UsbRadioA_Array[] = {
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x00000ee0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x009, 0x000007f0,
+	0x00a, 0x000009d0,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x00f, 0x00000990,
+	0x012, 0x00000806,
+	0x014, 0x000005ab,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000f80,
+	0x01b, 0x00000f5e,
+	0x01c, 0x00000008,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x01f, 0x00000000,
+	0x020, 0x000001a5,
+	0x01f, 0x00000001,
+	0x020, 0x00000165,
+	0x01f, 0x00000002,
+	0x020, 0x000000c6,
+	0x01f, 0x00000003,
+	0x020, 0x00000086,
+	0x01f, 0x00000004,
+	0x020, 0x00000046,
+	0x01f, 0x00000005,
+	0x020, 0x000001e6,
+	0x01f, 0x00000006,
+	0x020, 0x000001a6,
+	0x01f, 0x00000007,
+	0x020, 0x00000166,
+	0x01f, 0x00000008,
+	0x020, 0x000000c7,
+	0x01f, 0x00000009,
+	0x020, 0x00000087,
+	0x01f, 0x0000000a,
+	0x020, 0x000000f7,
+	0x01f, 0x0000000b,
+	0x020, 0x000000d7,
+	0x01f, 0x0000000c,
+	0x020, 0x000000b7,
+	0x01f, 0x0000000d,
+	0x020, 0x00000097,
+	0x01f, 0x0000000e,
+	0x020, 0x00000077,
+	0x01f, 0x0000000f,
+	0x020, 0x00000057,
+	0x01f, 0x00000010,
+	0x020, 0x00000037,
+	0x01f, 0x00000011,
+	0x020, 0x000000fb,
+	0x01f, 0x00000012,
+	0x020, 0x000000db,
+	0x01f, 0x00000013,
+	0x020, 0x000000bb,
+	0x01f, 0x00000014,
+	0x020, 0x000000ff,
+	0x01f, 0x00000015,
+	0x020, 0x000000e3,
+	0x01f, 0x00000016,
+	0x020, 0x000000c3,
+	0x01f, 0x00000017,
+	0x020, 0x000000a3,
+	0x01f, 0x00000018,
+	0x020, 0x00000083,
+	0x01f, 0x00000019,
+	0x020, 0x00000063,
+	0x01f, 0x0000001a,
+	0x020, 0x00000043,
+	0x01f, 0x0000001b,
+	0x020, 0x00000023,
+	0x01f, 0x0000001c,
+	0x020, 0x00000003,
+	0x01f, 0x0000001d,
+	0x020, 0x000001e3,
+	0x01f, 0x0000001e,
+	0x020, 0x000001c3,
+	0x01f, 0x0000001f,
+	0x020, 0x000001a3,
+	0x01f, 0x00000020,
+	0x020, 0x00000183,
+	0x01f, 0x00000021,
+	0x020, 0x00000163,
+	0x01f, 0x00000022,
+	0x020, 0x00000143,
+	0x01f, 0x00000023,
+	0x020, 0x00000123,
+	0x01f, 0x00000024,
+	0x020, 0x00000103,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x00b, 0x000001ba,
+	0x02c, 0x000003d7,
+	0x02d, 0x00000ff0,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x007, 0x00000080,
+	0x002, 0x0000088d,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x000, 0x000000bf,
+	0x00d, 0x0000001f,
+	0x00d, 0x00000c9f,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
+};
+
+u32 Rtl8192UsbRadioB_Array[] = {
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x000006e0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x000, 0x000000b7,
+	0x00a, 0x00000850,
+	0x000, 0x000000bf,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000e00,
+	0x01b, 0x00000f5e,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x00b, 0x000001ba,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x00d, 0x00000ccc,
+	0x000, 0x000000bf,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
+};
+
+u32 Rtl8192UsbRadioC_Array[] = {
+	0x0, };
+
+u32 Rtl8192UsbRadioD_Array[] = {
+	0x0, };
+
+u32 Rtl8192UsbMACPHY_Array[] = {
+	0x03c, 0xffff0000, 0x00000f0f,
+	0x340, 0xffffffff, 0x161a1a1a,
+	0x344, 0xffffffff, 0x12121416,
+	0x348, 0x0000ffff, 0x00001818,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000100,
+};
+
+u32 Rtl8192UsbMACPHY_Array_PG[] = {
+	0x03c, 0xffff0000, 0x00000f0f,
+	0xe00, 0xffffffff, 0x06090909,
+	0xe04, 0xffffffff, 0x00030306,
+	0xe08, 0x0000ff00, 0x00000000,
+	0xe10, 0xffffffff, 0x0a0c0d0f,
+	0xe14, 0xffffffff, 0x06070809,
+	0xe18, 0xffffffff, 0x0a0c0d0f,
+	0xe1c, 0xffffffff, 0x06070809,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000800,
+};
+
+u32 Rtl8192UsbAGCTAB_Array[] = {
+	0xc78, 0x7d000001,
+	0xc78, 0x7d010001,
+	0xc78, 0x7d020001,
+	0xc78, 0x7d030001,
+	0xc78, 0x7d040001,
+	0xc78, 0x7d050001,
+	0xc78, 0x7c060001,
+	0xc78, 0x7b070001,
+	0xc78, 0x7a080001,
+	0xc78, 0x79090001,
+	0xc78, 0x780a0001,
+	0xc78, 0x770b0001,
+	0xc78, 0x760c0001,
+	0xc78, 0x750d0001,
+	0xc78, 0x740e0001,
+	0xc78, 0x730f0001,
+	0xc78, 0x72100001,
+	0xc78, 0x71110001,
+	0xc78, 0x70120001,
+	0xc78, 0x6f130001,
+	0xc78, 0x6e140001,
+	0xc78, 0x6d150001,
+	0xc78, 0x6c160001,
+	0xc78, 0x6b170001,
+	0xc78, 0x6a180001,
+	0xc78, 0x69190001,
+	0xc78, 0x681a0001,
+	0xc78, 0x671b0001,
+	0xc78, 0x661c0001,
+	0xc78, 0x651d0001,
+	0xc78, 0x641e0001,
+	0xc78, 0x491f0001,
+	0xc78, 0x48200001,
+	0xc78, 0x47210001,
+	0xc78, 0x46220001,
+	0xc78, 0x45230001,
+	0xc78, 0x44240001,
+	0xc78, 0x43250001,
+	0xc78, 0x28260001,
+	0xc78, 0x27270001,
+	0xc78, 0x26280001,
+	0xc78, 0x25290001,
+	0xc78, 0x242a0001,
+	0xc78, 0x232b0001,
+	0xc78, 0x222c0001,
+	0xc78, 0x212d0001,
+	0xc78, 0x202e0001,
+	0xc78, 0x0a2f0001,
+	0xc78, 0x08300001,
+	0xc78, 0x06310001,
+	0xc78, 0x05320001,
+	0xc78, 0x04330001,
+	0xc78, 0x03340001,
+	0xc78, 0x02350001,
+	0xc78, 0x01360001,
+	0xc78, 0x00370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7d400001,
+	0xc78, 0x7d410001,
+	0xc78, 0x7d420001,
+	0xc78, 0x7d430001,
+	0xc78, 0x7d440001,
+	0xc78, 0x7d450001,
+	0xc78, 0x7c460001,
+	0xc78, 0x7b470001,
+	0xc78, 0x7a480001,
+	0xc78, 0x79490001,
+	0xc78, 0x784a0001,
+	0xc78, 0x774b0001,
+	0xc78, 0x764c0001,
+	0xc78, 0x754d0001,
+	0xc78, 0x744e0001,
+	0xc78, 0x734f0001,
+	0xc78, 0x72500001,
+	0xc78, 0x71510001,
+	0xc78, 0x70520001,
+	0xc78, 0x6f530001,
+	0xc78, 0x6e540001,
+	0xc78, 0x6d550001,
+	0xc78, 0x6c560001,
+	0xc78, 0x6b570001,
+	0xc78, 0x6a580001,
+	0xc78, 0x69590001,
+	0xc78, 0x685a0001,
+	0xc78, 0x675b0001,
+	0xc78, 0x665c0001,
+	0xc78, 0x655d0001,
+	0xc78, 0x645e0001,
+	0xc78, 0x495f0001,
+	0xc78, 0x48600001,
+	0xc78, 0x47610001,
+	0xc78, 0x46620001,
+	0xc78, 0x45630001,
+	0xc78, 0x44640001,
+	0xc78, 0x43650001,
+	0xc78, 0x28660001,
+	0xc78, 0x27670001,
+	0xc78, 0x26680001,
+	0xc78, 0x25690001,
+	0xc78, 0x246a0001,
+	0xc78, 0x236b0001,
+	0xc78, 0x226c0001,
+	0xc78, 0x216d0001,
+	0xc78, 0x206e0001,
+	0xc78, 0x0a6f0001,
+	0xc78, 0x08700001,
+	0xc78, 0x06710001,
+	0xc78, 0x05720001,
+	0xc78, 0x04730001,
+	0xc78, 0x03740001,
+	0xc78, 0x02750001,
+	0xc78, 0x01760001,
+	0xc78, 0x00770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x2e00001e,
+	0xc78, 0x2e01001e,
+	0xc78, 0x2e02001e,
+	0xc78, 0x2e03001e,
+	0xc78, 0x2e04001e,
+	0xc78, 0x2e05001e,
+	0xc78, 0x3006001e,
+	0xc78, 0x3407001e,
+	0xc78, 0x3908001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3f0a001e,
+	0xc78, 0x420b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x450d001e,
+	0xc78, 0x460e001e,
+	0xc78, 0x460f001e,
+	0xc78, 0x4710001e,
+	0xc78, 0x4811001e,
+	0xc78, 0x4912001e,
+	0xc78, 0x4a13001e,
+	0xc78, 0x4b14001e,
+	0xc78, 0x4b15001e,
+	0xc78, 0x4c16001e,
+	0xc78, 0x4d17001e,
+	0xc78, 0x4e18001e,
+	0xc78, 0x4f19001e,
+	0xc78, 0x4f1a001e,
+	0xc78, 0x501b001e,
+	0xc78, 0x511c001e,
+	0xc78, 0x521d001e,
+	0xc78, 0x521e001e,
+	0xc78, 0x531f001e,
+	0xc78, 0x5320001e,
+	0xc78, 0x5421001e,
+	0xc78, 0x5522001e,
+	0xc78, 0x5523001e,
+	0xc78, 0x5624001e,
+	0xc78, 0x5725001e,
+	0xc78, 0x5726001e,
+	0xc78, 0x5827001e,
+	0xc78, 0x5828001e,
+	0xc78, 0x5929001e,
+	0xc78, 0x592a001e,
+	0xc78, 0x5a2b001e,
+	0xc78, 0x5b2c001e,
+	0xc78, 0x5c2d001e,
+	0xc78, 0x5c2e001e,
+	0xc78, 0x5d2f001e,
+	0xc78, 0x5e30001e,
+	0xc78, 0x5f31001e,
+	0xc78, 0x6032001e,
+	0xc78, 0x6033001e,
+	0xc78, 0x6134001e,
+	0xc78, 0x6235001e,
+	0xc78, 0x6336001e,
+	0xc78, 0x6437001e,
+	0xc78, 0x6438001e,
+	0xc78, 0x6539001e,
+	0xc78, 0x663a001e,
+	0xc78, 0x673b001e,
+	0xc78, 0x673c001e,
+	0xc78, 0x683d001e,
+	0xc78, 0x693e001e,
+	0xc78, 0x6a3f001e,
+};
diff --git a/drivers/staging/rtl8192u/r819xU_firmware_img.h b/drivers/staging/rtl8192u/r819xU_firmware_img.h
new file mode 100644
index 000000000..61585a724
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_firmware_img.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef IMG_H
+#define IMG_H
+
+#define MACPHY_Array_PGLength 30
+#define PHY_REG_1T2RArrayLength 296
+#define AGCTAB_ArrayLength 384
+#define MACPHY_ArrayLength 18
+
+#define RadioA_ArrayLength 246
+#define RadioB_ArrayLength 78
+#define RadioC_ArrayLength 1
+#define RadioD_ArrayLength 1
+#define PHY_REGArrayLength 1
+
+extern u32 Rtl8192UsbPHY_REGArray[];
+extern u32 Rtl8192UsbPHY_REG_1T2RArray[];
+extern u32 Rtl8192UsbRadioA_Array[];
+extern u32 Rtl8192UsbRadioB_Array[];
+extern u32 Rtl8192UsbRadioC_Array[];
+extern u32 Rtl8192UsbRadioD_Array[];
+extern u32 Rtl8192UsbMACPHY_Array[];
+extern u32 Rtl8192UsbMACPHY_Array_PG[];
+extern u32 Rtl8192UsbAGCTAB_Array[];
+
+#endif
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
new file mode 100644
index 000000000..97f4d8950
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -0,0 +1,1733 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "r8192U.h"
+#include "r8192U_hw.h"
+#include "r819xU_phy.h"
+#include "r819xU_phyreg.h"
+#include "r8190_rtl8256.h"
+#include "r8192U_dm.h"
+#include "r819xU_firmware_img.h"
+
+#include "ieee80211/dot11d.h"
+#include <linux/bitops.h>
+
+static u32 RF_CHANNEL_TABLE_ZEBRA[] = {
+	0,
+	0x085c, /* 2412 1  */
+	0x08dc, /* 2417 2  */
+	0x095c, /* 2422 3  */
+	0x09dc, /* 2427 4  */
+	0x0a5c, /* 2432 5  */
+	0x0adc, /* 2437 6  */
+	0x0b5c, /* 2442 7  */
+	0x0bdc, /* 2447 8  */
+	0x0c5c, /* 2452 9  */
+	0x0cdc, /* 2457 10 */
+	0x0d5c, /* 2462 11 */
+	0x0ddc, /* 2467 12 */
+	0x0e5c, /* 2472 13 */
+	0x0f72, /* 2484    */
+};
+
+#define rtl819XMACPHY_Array Rtl8192UsbMACPHY_Array
+
+/******************************************************************************
+ * function:  This function checks different RF type to execute legal judgement.
+ *            If RF Path is illegal, we will return false.
+ * input:     net_device	 *dev
+ *            u32		 e_rfpath
+ * output:    none
+ * return:    0(illegal, false), 1(legal, true)
+ *****************************************************************************/
+u8 rtl8192_phy_CheckIsLegalRFPath(struct net_device *dev, u32 e_rfpath)
+{
+	u8 ret = 1;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->rf_type == RF_2T4R) {
+		ret = 0;
+	} else if (priv->rf_type == RF_1T2R) {
+		if (e_rfpath == RF90_PATH_A || e_rfpath == RF90_PATH_B)
+			ret = 1;
+		else if (e_rfpath == RF90_PATH_C || e_rfpath == RF90_PATH_D)
+			ret = 0;
+	}
+	return ret;
+}
+
+/******************************************************************************
+ * function:  This function sets specific bits to BB register
+ * input:     net_device *dev
+ *            u32        reg_addr   //target addr to be modified
+ *            u32        bitmask    //taget bit pos to be modified
+ *            u32        data       //value to be write
+ * output:    none
+ * return:    none
+ * notice:
+ ******************************************************************************/
+void rtl8192_setBBreg(struct net_device *dev, u32 reg_addr, u32 bitmask,
+		      u32 data)
+{
+	u32 reg, bitshift;
+
+	if (bitmask != bMaskDWord) {
+		read_nic_dword(dev, reg_addr, &reg);
+		bitshift = ffs(bitmask) - 1;
+		reg &= ~bitmask;
+		reg |= data << bitshift;
+		write_nic_dword(dev, reg_addr, reg);
+	} else {
+		write_nic_dword(dev, reg_addr, data);
+	}
+}
+
+/******************************************************************************
+ * function:  This function reads specific bits from BB register
+ * input:     net_device	*dev
+ *            u32		reg_addr   //target addr to be readback
+ *            u32		bitmask    //taget bit pos to be readback
+ * output:    none
+ * return:    u32		data       //the readback register value
+ * notice:
+ ******************************************************************************/
+u32 rtl8192_QueryBBReg(struct net_device *dev, u32 reg_addr, u32 bitmask)
+{
+	u32 reg, bitshift;
+
+	read_nic_dword(dev, reg_addr, &reg);
+	bitshift = ffs(bitmask) - 1;
+
+	return (reg & bitmask) >> bitshift;
+}
+
+static u32 phy_FwRFSerialRead(struct net_device *dev,
+			      enum rf90_radio_path_e e_rfpath,
+			      u32 offset);
+
+static void phy_FwRFSerialWrite(struct net_device *dev,
+				enum rf90_radio_path_e e_rfpath,
+				u32  offset,
+				u32  data);
+
+/******************************************************************************
+ * function:  This function reads register from RF chip
+ * input:     net_device        *dev
+ *            rf90_radio_path_e e_rfpath    //radio path of A/B/C/D
+ *            u32               offset     //target address to be read
+ * output:    none
+ * return:    u32               readback value
+ * notice:    There are three types of serial operations:
+ *            (1) Software serial write.
+ *            (2)Hardware LSSI-Low Speed Serial Interface.
+ *            (3)Hardware HSSI-High speed serial write.
+ *            Driver here need to implement (1) and (2)
+ *            ---need more spec for this information.
+ ******************************************************************************/
+static u32 rtl8192_phy_RFSerialRead(struct net_device *dev,
+				    enum rf90_radio_path_e e_rfpath, u32 offset)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 ret = 0;
+	u32 new_offset = 0;
+	BB_REGISTER_DEFINITION_T *pPhyReg = &priv->PHYRegDef[e_rfpath];
+
+	rtl8192_setBBreg(dev, pPhyReg->rfLSSIReadBack, bLSSIReadBackData, 0);
+	/* Make sure RF register offset is correct */
+	offset &= 0x3f;
+
+	/* Switch page for 8256 RF IC */
+	if (priv->rf_chip == RF_8256) {
+		if (offset >= 31) {
+			priv->RfReg0Value[e_rfpath] |= 0x140;
+			/* Switch to Reg_Mode2 for Reg 31-45 */
+			rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset,
+					 bMaskDWord,
+					 priv->RfReg0Value[e_rfpath]<<16);
+			/* Modify offset */
+			new_offset = offset - 30;
+		} else if (offset >= 16) {
+			priv->RfReg0Value[e_rfpath] |= 0x100;
+			priv->RfReg0Value[e_rfpath] &= (~0x40);
+			/* Switch to Reg_Mode1 for Reg16-30 */
+			rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset,
+					 bMaskDWord,
+					 priv->RfReg0Value[e_rfpath]<<16);
+
+			new_offset = offset - 15;
+		} else {
+			new_offset = offset;
+		}
+	} else {
+		RT_TRACE((COMP_PHY|COMP_ERR),
+			 "check RF type here, need to be 8256\n");
+		new_offset = offset;
+	}
+	/* Put desired read addr to LSSI control Register */
+	rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, bLSSIReadAddress,
+			 new_offset);
+	/* Issue a posedge trigger */
+	rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2,  bLSSIReadEdge, 0x0);
+	rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2,  bLSSIReadEdge, 0x1);
+
+	/* TODO: we should not delay such a long time. Ask for help from SD3 */
+	usleep_range(1000, 1000);
+
+	ret = rtl8192_QueryBBReg(dev, pPhyReg->rfLSSIReadBack,
+				 bLSSIReadBackData);
+
+	/* Switch back to Reg_Mode0 */
+	if (priv->rf_chip == RF_8256) {
+		priv->RfReg0Value[e_rfpath] &= 0xebf;
+
+		rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset, bMaskDWord,
+				 priv->RfReg0Value[e_rfpath] << 16);
+	}
+
+	return ret;
+}
+
+/******************************************************************************
+ * function:  This function writes data to RF register
+ * input:     net_device        *dev
+ *            rf90_radio_path_e e_rfpath  //radio path of A/B/C/D
+ *            u32               offset   //target address to be written
+ *            u32               data	 //the new register data to be written
+ * output:    none
+ * return:    none
+ * notice:    For RF8256 only.
+ * ===========================================================================
+ * Reg Mode	RegCTL[1]	RegCTL[0]		Note
+ *		(Reg00[12])	(Reg00[10])
+ * ===========================================================================
+ * Reg_Mode0	0		x			Reg 0 ~ 15(0x0 ~ 0xf)
+ * ---------------------------------------------------------------------------
+ * Reg_Mode1	1		0			Reg 16 ~ 30(0x1 ~ 0xf)
+ * ---------------------------------------------------------------------------
+ * Reg_Mode2	1		1			Reg 31 ~ 45(0x1 ~ 0xf)
+ * ---------------------------------------------------------------------------
+ *****************************************************************************/
+static void rtl8192_phy_RFSerialWrite(struct net_device *dev,
+				      enum rf90_radio_path_e e_rfpath,
+				      u32 offset,
+				      u32 data)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 DataAndAddr = 0, new_offset = 0;
+	BB_REGISTER_DEFINITION_T	*pPhyReg = &priv->PHYRegDef[e_rfpath];
+
+	offset &= 0x3f;
+	if (priv->rf_chip == RF_8256) {
+		if (offset >= 31) {
+			priv->RfReg0Value[e_rfpath] |= 0x140;
+			rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset,
+					 bMaskDWord,
+					 priv->RfReg0Value[e_rfpath] << 16);
+			new_offset = offset - 30;
+		} else if (offset >= 16) {
+			priv->RfReg0Value[e_rfpath] |= 0x100;
+			priv->RfReg0Value[e_rfpath] &= (~0x40);
+			rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset,
+					 bMaskDWord,
+					 priv->RfReg0Value[e_rfpath]<<16);
+			new_offset = offset - 15;
+		} else {
+			new_offset = offset;
+		}
+	} else {
+		RT_TRACE((COMP_PHY|COMP_ERR),
+			 "check RF type here, need to be 8256\n");
+		new_offset = offset;
+	}
+
+	/* Put write addr in [5:0] and write data in [31:16] */
+	DataAndAddr = (data<<16) | (new_offset&0x3f);
+
+	/* Write operation */
+	rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
+
+	if (offset == 0x0)
+		priv->RfReg0Value[e_rfpath] = data;
+
+	/* Switch back to Reg_Mode0 */
+	if (priv->rf_chip == RF_8256) {
+		if (offset != 0) {
+			priv->RfReg0Value[e_rfpath] &= 0xebf;
+			rtl8192_setBBreg(dev, pPhyReg->rf3wireOffset,
+					 bMaskDWord,
+					 priv->RfReg0Value[e_rfpath] << 16);
+		}
+	}
+}
+
+/******************************************************************************
+ * function:  This function set specific bits to RF register
+ * input:     net_device        dev
+ *            rf90_radio_path_e e_rfpath  //radio path of A/B/C/D
+ *            u32               reg_addr //target addr to be modified
+ *            u32               bitmask  //taget bit pos to be modified
+ *            u32               data     //value to be written
+ * output:    none
+ * return:    none
+ * notice:
+ *****************************************************************************/
+void rtl8192_phy_SetRFReg(struct net_device *dev,
+			  enum rf90_radio_path_e e_rfpath,
+			  u32 reg_addr, u32 bitmask, u32 data)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32 reg, bitshift;
+
+	if (!rtl8192_phy_CheckIsLegalRFPath(dev, e_rfpath))
+		return;
+
+	if (priv->Rf_Mode == RF_OP_By_FW) {
+		if (bitmask != bMask12Bits) {
+			/* RF data is 12 bits only */
+			reg = phy_FwRFSerialRead(dev, e_rfpath, reg_addr);
+			bitshift =  ffs(bitmask) - 1;
+			reg &= ~bitmask;
+			reg |= data << bitshift;
+
+			phy_FwRFSerialWrite(dev, e_rfpath, reg_addr, reg);
+		} else {
+			phy_FwRFSerialWrite(dev, e_rfpath, reg_addr, data);
+		}
+
+		udelay(200);
+
+	} else {
+		if (bitmask != bMask12Bits) {
+			/* RF data is 12 bits only */
+			reg = rtl8192_phy_RFSerialRead(dev, e_rfpath, reg_addr);
+			bitshift =  ffs(bitmask) - 1;
+			reg &= ~bitmask;
+			reg |= data << bitshift;
+
+			rtl8192_phy_RFSerialWrite(dev, e_rfpath, reg_addr, reg);
+		} else {
+			rtl8192_phy_RFSerialWrite(dev, e_rfpath, reg_addr, data);
+		}
+	}
+}
+
+/******************************************************************************
+ * function:  This function reads specific bits from RF register
+ * input:     net_device        *dev
+ *            u32               reg_addr //target addr to be readback
+ *            u32               bitmask  //taget bit pos to be readback
+ * output:    none
+ * return:    u32               data     //the readback register value
+ * notice:
+ *****************************************************************************/
+u32 rtl8192_phy_QueryRFReg(struct net_device *dev,
+			   enum rf90_radio_path_e e_rfpath,
+			   u32 reg_addr, u32 bitmask)
+{
+	u32 reg, bitshift;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (!rtl8192_phy_CheckIsLegalRFPath(dev, e_rfpath))
+		return 0;
+	if (priv->Rf_Mode == RF_OP_By_FW) {
+		reg = phy_FwRFSerialRead(dev, e_rfpath, reg_addr);
+		udelay(200);
+	} else {
+		reg = rtl8192_phy_RFSerialRead(dev, e_rfpath, reg_addr);
+	}
+	bitshift =  ffs(bitmask) - 1;
+	reg = (reg & bitmask) >> bitshift;
+	return reg;
+}
+
+/******************************************************************************
+ * function:  We support firmware to execute RF-R/W.
+ * input:     net_device        *dev
+ *            rf90_radio_path_e e_rfpath
+ *            u32               offset
+ * output:    none
+ * return:    u32
+ * notice:
+ ****************************************************************************/
+static u32 phy_FwRFSerialRead(struct net_device *dev,
+			      enum rf90_radio_path_e e_rfpath,
+			      u32 offset)
+{
+	u32		reg = 0;
+	u32		data = 0;
+	u8		time = 0;
+	u32		tmp;
+
+	/* Firmware RF Write control.
+	 * We can not execute the scheme in the initial step.
+	 * Otherwise, RF-R/W will waste much time.
+	 * This is only for site survey.
+	 */
+	/* 1. Read operation need not insert data. bit 0-11 */
+	/* 2. Write RF register address. bit 12-19 */
+	data |= ((offset&0xFF)<<12);
+	/* 3. Write RF path.  bit 20-21 */
+	data |= ((e_rfpath&0x3)<<20);
+	/* 4. Set RF read indicator. bit 22=0 */
+	/* 5. Trigger Fw to operate the command. bit 31 */
+	data |= 0x80000000;
+	/* 6. We can not execute read operation if bit 31 is 1. */
+	read_nic_dword(dev, QPNR, &tmp);
+	while (tmp & 0x80000000) {
+		/* If FW can not finish RF-R/W for more than ?? times.
+		 * We must reset FW.
+		 */
+		if (time++ < 100) {
+			udelay(10);
+			read_nic_dword(dev, QPNR, &tmp);
+		} else {
+			break;
+		}
+	}
+	/* 7. Execute read operation. */
+	write_nic_dword(dev, QPNR, data);
+	/* 8. Check if firmware send back RF content. */
+	read_nic_dword(dev, QPNR, &tmp);
+	while (tmp & 0x80000000) {
+		/* If FW can not finish RF-R/W for more than ?? times.
+		 * We must reset FW.
+		 */
+		if (time++ < 100) {
+			udelay(10);
+			read_nic_dword(dev, QPNR, &tmp);
+		} else {
+			return 0;
+		}
+	}
+	read_nic_dword(dev, RF_DATA, &reg);
+
+	return reg;
+}
+
+/******************************************************************************
+ * function:  We support firmware to execute RF-R/W.
+ * input:     net_device        *dev
+ *            rf90_radio_path_e e_rfpath
+ *            u32               offset
+ *            u32               data
+ * output:    none
+ * return:    none
+ * notice:
+ ****************************************************************************/
+static void phy_FwRFSerialWrite(struct net_device *dev,
+				enum rf90_radio_path_e e_rfpath,
+				u32 offset, u32 data)
+{
+	u8	time = 0;
+	u32	tmp;
+
+	/* Firmware RF Write control.
+	 * We can not execute the scheme in the initial step.
+	 * Otherwise, RF-R/W will waste much time.
+	 * This is only for site survey.
+	 */
+
+	/* 1. Set driver write bit and 12 bit data. bit 0-11 */
+	/* 2. Write RF register address. bit 12-19 */
+	data |= ((offset&0xFF)<<12);
+	/* 3. Write RF path.  bit 20-21 */
+	data |= ((e_rfpath&0x3)<<20);
+	/* 4. Set RF write indicator. bit 22=1 */
+	data |= 0x400000;
+	/* 5. Trigger Fw to operate the command. bit 31=1 */
+	data |= 0x80000000;
+
+	/* 6. Write operation. We can not write if bit 31 is 1. */
+	read_nic_dword(dev, QPNR, &tmp);
+	while (tmp & 0x80000000) {
+		/* If FW can not finish RF-R/W for more than ?? times.
+		 * We must reset FW.
+		 */
+		if (time++ < 100) {
+			udelay(10);
+			read_nic_dword(dev, QPNR, &tmp);
+		} else {
+			break;
+		}
+	}
+	/* 7. No matter check bit. We always force the write.
+	 * Because FW will not accept the command.
+	 */
+	write_nic_dword(dev, QPNR, data);
+	/* According to test, we must delay 20us to wait firmware
+	 * to finish RF write operation.
+	 */
+	/* We support delay in firmware side now. */
+}
+
+/******************************************************************************
+ * function:  This function reads BB parameters from header file we generate,
+ *            and do register read/write
+ * input:     net_device	*dev
+ * output:    none
+ * return:    none
+ * notice:    BB parameters may change all the time, so please make
+ *            sure it has been synced with the newest.
+ *****************************************************************************/
+void rtl8192_phy_configmac(struct net_device *dev)
+{
+	u32 dwArrayLen = 0, i;
+	u32 *pdwArray = NULL;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->btxpowerdata_readfromEEPORM) {
+		RT_TRACE(COMP_PHY, "Rtl819XMACPHY_Array_PG\n");
+		dwArrayLen = MACPHY_Array_PGLength;
+		pdwArray = Rtl8192UsbMACPHY_Array_PG;
+
+	} else {
+		RT_TRACE(COMP_PHY, "Rtl819XMACPHY_Array\n");
+		dwArrayLen = MACPHY_ArrayLength;
+		pdwArray = rtl819XMACPHY_Array;
+	}
+	for (i = 0; i < dwArrayLen; i = i+3) {
+		if (pdwArray[i] == 0x318)
+			pdwArray[i+2] = 0x00000800;
+
+		RT_TRACE(COMP_DBG,
+			 "Rtl8190MACPHY_Array[0]=%x Rtl8190MACPHY_Array[1]=%x Rtl8190MACPHY_Array[2]=%x\n",
+			 pdwArray[i], pdwArray[i+1], pdwArray[i+2]);
+		rtl8192_setBBreg(dev, pdwArray[i], pdwArray[i+1],
+				 pdwArray[i+2]);
+	}
+}
+
+/******************************************************************************
+ * function:  This function does dirty work
+ * input:     net_device	*dev
+ *            u8                ConfigType
+ * output:    none
+ * return:    none
+ * notice:    BB parameters may change all the time, so please make
+ *            sure it has been synced with the newest.
+ *****************************************************************************/
+static void rtl8192_phyConfigBB(struct net_device *dev,
+				enum baseband_config_type ConfigType)
+{
+	u32 i;
+
+	if (ConfigType == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < PHY_REG_1T2RArrayLength; i += 2) {
+			rtl8192_setBBreg(dev, Rtl8192UsbPHY_REG_1T2RArray[i],
+					 bMaskDWord,
+					 Rtl8192UsbPHY_REG_1T2RArray[i+1]);
+			RT_TRACE(COMP_DBG,
+				 "i: %x, Rtl819xUsbPHY_REGArray[0]=%x Rtl819xUsbPHY_REGArray[1]=%x\n",
+				 i, Rtl8192UsbPHY_REG_1T2RArray[i],
+				 Rtl8192UsbPHY_REG_1T2RArray[i+1]);
+		}
+	} else if (ConfigType == BASEBAND_CONFIG_AGC_TAB) {
+		for (i = 0; i < AGCTAB_ArrayLength; i += 2) {
+			rtl8192_setBBreg(dev, Rtl8192UsbAGCTAB_Array[i],
+					 bMaskDWord, Rtl8192UsbAGCTAB_Array[i+1]);
+			RT_TRACE(COMP_DBG,
+				 "i: %x, Rtl8192UsbAGCTAB_Array[0]=%x Rtl8192UsbAGCTAB_Array[1]=%x\n",
+				 i, Rtl8192UsbAGCTAB_Array[i],
+				 Rtl8192UsbAGCTAB_Array[i+1]);
+		}
+	}
+}
+
+/******************************************************************************
+ * function:  This function initializes Register definition offset for
+ *            Radio Path A/B/C/D
+ * input:     net_device	*dev
+ * output:    none
+ * return:    none
+ * notice:    Initialization value here is constant and it should never
+ *            be changed
+ *****************************************************************************/
+static void rtl8192_InitBBRFRegDef(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	/* RF Interface Software Control */
+	/* 16 LSBs if read 32-bit from 0x870 */
+	priv->PHYRegDef[RF90_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	/* 16 MSBs if read 32-bit from 0x870 (16-bit for 0x872) */
+	priv->PHYRegDef[RF90_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	/* 16 LSBs if read 32-bit from 0x874 */
+	priv->PHYRegDef[RF90_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+	/* 16 MSBs if read 32-bit from 0x874 (16-bit for 0x876) */
+	priv->PHYRegDef[RF90_PATH_D].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+
+	/* RF Interface Readback Value */
+	/* 16 LSBs if read 32-bit from 0x8E0 */
+	priv->PHYRegDef[RF90_PATH_A].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	/* 16 MSBs if read 32-bit from 0x8E0 (16-bit for 0x8E2) */
+	priv->PHYRegDef[RF90_PATH_B].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	/* 16 LSBs if read 32-bit from 0x8E4 */
+	priv->PHYRegDef[RF90_PATH_C].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+	/* 16 MSBs if read 32-bit from 0x8E4 (16-bit for 0x8E6) */
+	priv->PHYRegDef[RF90_PATH_D].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+
+	/* RF Interface Output (and Enable) */
+	/* 16 LSBs if read 32-bit from 0x860 */
+	priv->PHYRegDef[RF90_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE;
+	/* 16 LSBs if read 32-bit from 0x864 */
+	priv->PHYRegDef[RF90_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE;
+	/* 16 LSBs if read 32-bit from 0x868 */
+	priv->PHYRegDef[RF90_PATH_C].rfintfo = rFPGA0_XC_RFInterfaceOE;
+	/* 16 LSBs if read 32-bit from 0x86C */
+	priv->PHYRegDef[RF90_PATH_D].rfintfo = rFPGA0_XD_RFInterfaceOE;
+
+	/* RF Interface (Output and) Enable */
+	/* 16 MSBs if read 32-bit from 0x860 (16-bit for 0x862) */
+	priv->PHYRegDef[RF90_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE;
+	/* 16 MSBs if read 32-bit from 0x864 (16-bit for 0x866) */
+	priv->PHYRegDef[RF90_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE;
+	/* 16 MSBs if read 32-bit from 0x86A (16-bit for 0x86A) */
+	priv->PHYRegDef[RF90_PATH_C].rfintfe = rFPGA0_XC_RFInterfaceOE;
+	/* 16 MSBs if read 32-bit from 0x86C (16-bit for 0x86E) */
+	priv->PHYRegDef[RF90_PATH_D].rfintfe = rFPGA0_XD_RFInterfaceOE;
+
+	/* Addr of LSSI. Write RF register by driver */
+	priv->PHYRegDef[RF90_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_C].rf3wireOffset = rFPGA0_XC_LSSIParameter;
+	priv->PHYRegDef[RF90_PATH_D].rf3wireOffset = rFPGA0_XD_LSSIParameter;
+
+	/* RF parameter */
+	/* BB Band Select */
+	priv->PHYRegDef[RF90_PATH_A].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	priv->PHYRegDef[RF90_PATH_B].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	priv->PHYRegDef[RF90_PATH_C].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+	priv->PHYRegDef[RF90_PATH_D].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+
+	/* Tx AGC Gain Stage (same for all path. Should we remove this?) */
+	priv->PHYRegDef[RF90_PATH_A].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_B].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_C].rfTxGainStage = rFPGA0_TxGainStage;
+	priv->PHYRegDef[RF90_PATH_D].rfTxGainStage = rFPGA0_TxGainStage;
+
+	/* Tranceiver A~D HSSI Parameter-1 */
+	/* wire control parameter1 */
+	priv->PHYRegDef[RF90_PATH_A].rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_B].rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_C].rfHSSIPara1 = rFPGA0_XC_HSSIParameter1;
+	priv->PHYRegDef[RF90_PATH_D].rfHSSIPara1 = rFPGA0_XD_HSSIParameter1;
+
+	/* Tranceiver A~D HSSI Parameter-2 */
+	/* wire control parameter2 */
+	priv->PHYRegDef[RF90_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_C].rfHSSIPara2 = rFPGA0_XC_HSSIParameter2;
+	priv->PHYRegDef[RF90_PATH_D].rfHSSIPara2 = rFPGA0_XD_HSSIParameter2;
+
+	/* RF Switch Control */
+	/* TR/Ant switch control */
+	priv->PHYRegDef[RF90_PATH_A].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_B].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_C].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+	priv->PHYRegDef[RF90_PATH_D].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+
+	/* AGC control 1 */
+	priv->PHYRegDef[RF90_PATH_A].rfAGCControl1 = rOFDM0_XAAGCCore1;
+	priv->PHYRegDef[RF90_PATH_B].rfAGCControl1 = rOFDM0_XBAGCCore1;
+	priv->PHYRegDef[RF90_PATH_C].rfAGCControl1 = rOFDM0_XCAGCCore1;
+	priv->PHYRegDef[RF90_PATH_D].rfAGCControl1 = rOFDM0_XDAGCCore1;
+
+	/* AGC control 2 */
+	priv->PHYRegDef[RF90_PATH_A].rfAGCControl2 = rOFDM0_XAAGCCore2;
+	priv->PHYRegDef[RF90_PATH_B].rfAGCControl2 = rOFDM0_XBAGCCore2;
+	priv->PHYRegDef[RF90_PATH_C].rfAGCControl2 = rOFDM0_XCAGCCore2;
+	priv->PHYRegDef[RF90_PATH_D].rfAGCControl2 = rOFDM0_XDAGCCore2;
+
+	/* RX AFE control 1 */
+	priv->PHYRegDef[RF90_PATH_A].rfRxIQImbalance = rOFDM0_XARxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_B].rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_C].rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_D].rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
+
+	/* RX AFE control 1 */
+	priv->PHYRegDef[RF90_PATH_A].rfRxAFE = rOFDM0_XARxAFE;
+	priv->PHYRegDef[RF90_PATH_B].rfRxAFE = rOFDM0_XBRxAFE;
+	priv->PHYRegDef[RF90_PATH_C].rfRxAFE = rOFDM0_XCRxAFE;
+	priv->PHYRegDef[RF90_PATH_D].rfRxAFE = rOFDM0_XDRxAFE;
+
+	/* Tx AFE control 1 */
+	priv->PHYRegDef[RF90_PATH_A].rfTxIQImbalance = rOFDM0_XATxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_B].rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_C].rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
+	priv->PHYRegDef[RF90_PATH_D].rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
+
+	/* Tx AFE control 2 */
+	priv->PHYRegDef[RF90_PATH_A].rfTxAFE = rOFDM0_XATxAFE;
+	priv->PHYRegDef[RF90_PATH_B].rfTxAFE = rOFDM0_XBTxAFE;
+	priv->PHYRegDef[RF90_PATH_C].rfTxAFE = rOFDM0_XCTxAFE;
+	priv->PHYRegDef[RF90_PATH_D].rfTxAFE = rOFDM0_XDTxAFE;
+
+	/* Tranceiver LSSI Readback */
+	priv->PHYRegDef[RF90_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_C].rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
+	priv->PHYRegDef[RF90_PATH_D].rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
+}
+
+/******************************************************************************
+ * function:  This function is to write register and then readback to make
+ *            sure whether BB and RF is OK
+ * input:     net_device        *dev
+ *            hw90_block_e      CheckBlock
+ *            rf90_radio_path_e e_rfpath  //only used when checkblock is
+ *                                       //HW90_BLOCK_RF
+ * output:    none
+ * return:    return whether BB and RF is ok (0:OK, 1:Fail)
+ * notice:    This function may be removed in the ASIC
+ ******************************************************************************/
+u8 rtl8192_phy_checkBBAndRF(struct net_device *dev, enum hw90_block_e CheckBlock,
+			    enum rf90_radio_path_e e_rfpath)
+{
+	u8 ret = 0;
+	u32 i, CheckTimes = 4, reg = 0;
+	u32 WriteAddr[4];
+	u32 WriteData[] = {0xfffff027, 0xaa55a02f, 0x00000027, 0x55aa502f};
+
+	/* Initialize register address offset to be checked */
+	WriteAddr[HW90_BLOCK_MAC] = 0x100;
+	WriteAddr[HW90_BLOCK_PHY0] = 0x900;
+	WriteAddr[HW90_BLOCK_PHY1] = 0x800;
+	WriteAddr[HW90_BLOCK_RF] = 0x3;
+	RT_TRACE(COMP_PHY, "%s(), CheckBlock: %d\n", __func__, CheckBlock);
+	for (i = 0; i < CheckTimes; i++) {
+		/* Write data to register and readback */
+		switch (CheckBlock) {
+		case HW90_BLOCK_MAC:
+			RT_TRACE(COMP_ERR,
+				 "PHY_CheckBBRFOK(): Never Write 0x100 here!\n");
+			break;
+
+		case HW90_BLOCK_PHY0:
+		case HW90_BLOCK_PHY1:
+			write_nic_dword(dev, WriteAddr[CheckBlock],
+					WriteData[i]);
+			read_nic_dword(dev, WriteAddr[CheckBlock], &reg);
+			break;
+
+		case HW90_BLOCK_RF:
+			WriteData[i] &= 0xfff;
+			rtl8192_phy_SetRFReg(dev, e_rfpath,
+					     WriteAddr[HW90_BLOCK_RF],
+					     bMask12Bits, WriteData[i]);
+			/* TODO: we should not delay for such a long time.
+			 * Ask SD3
+			 */
+			usleep_range(1000, 1000);
+			reg = rtl8192_phy_QueryRFReg(dev, e_rfpath,
+						     WriteAddr[HW90_BLOCK_RF],
+						     bMask12Bits);
+			usleep_range(1000, 1000);
+			break;
+
+		default:
+			ret = 1;
+			break;
+		}
+
+		/* Check whether readback data is correct */
+		if (reg != WriteData[i]) {
+			RT_TRACE((COMP_PHY|COMP_ERR),
+				 "error reg: %x, WriteData: %x\n",
+				 reg, WriteData[i]);
+			ret = 1;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/******************************************************************************
+ * function:  This function initializes BB&RF
+ * input:     net_device	*dev
+ * output:    none
+ * return:    none
+ * notice:    Initialization value may change all the time, so please make
+ *            sure it has been synced with the newest.
+ ******************************************************************************/
+static void rtl8192_BB_Config_ParaFile(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 reg_u8 = 0, eCheckItem = 0, status = 0;
+	u32 reg_u32 = 0;
+
+	/**************************************
+	 * <1> Initialize BaseBand
+	 *************************************/
+
+	/* --set BB Global Reset-- */
+	read_nic_byte(dev, BB_GLOBAL_RESET, &reg_u8);
+	write_nic_byte(dev, BB_GLOBAL_RESET, (reg_u8|BB_GLOBAL_RESET_BIT));
+	mdelay(50);
+	/* ---set BB reset Active--- */
+	read_nic_dword(dev, CPU_GEN, &reg_u32);
+	write_nic_dword(dev, CPU_GEN, (reg_u32&(~CPU_GEN_BB_RST)));
+
+	/* ----Ckeck FPGAPHY0 and PHY1 board is OK---- */
+	/* TODO: this function should be removed on ASIC */
+	for (eCheckItem = (enum hw90_block_e)HW90_BLOCK_PHY0;
+	     eCheckItem <= HW90_BLOCK_PHY1; eCheckItem++) {
+		/* don't care RF path */
+		status = rtl8192_phy_checkBBAndRF(dev, (enum hw90_block_e)eCheckItem,
+						  (enum rf90_radio_path_e)0);
+		if (status != 0) {
+			RT_TRACE((COMP_ERR | COMP_PHY),
+				 "phy_rf8256_config(): Check PHY%d Fail!!\n",
+				 eCheckItem-1);
+			return;
+		}
+	}
+	/* ---- Set CCK and OFDM Block "OFF"---- */
+	rtl8192_setBBreg(dev, rFPGA0_RFMOD, bCCKEn|bOFDMEn, 0x0);
+	/* ----BB Register Initilazation---- */
+	/* ==m==>Set PHY REG From Header<==m== */
+	rtl8192_phyConfigBB(dev, BASEBAND_CONFIG_PHY_REG);
+
+	/* ----Set BB reset de-Active---- */
+	read_nic_dword(dev, CPU_GEN, &reg_u32);
+	write_nic_dword(dev, CPU_GEN, (reg_u32|CPU_GEN_BB_RST));
+
+	/* ----BB AGC table Initialization---- */
+	/* ==m==>Set PHY REG From Header<==m== */
+	rtl8192_phyConfigBB(dev, BASEBAND_CONFIG_AGC_TAB);
+
+	/* ----Enable XSTAL ---- */
+	write_nic_byte_E(dev, 0x5e, 0x00);
+	if (priv->card_8192_version == VERSION_819XU_A) {
+		/* Antenna gain offset from B/C/D to A */
+		reg_u32 = priv->AntennaTxPwDiff[1]<<4 |
+			   priv->AntennaTxPwDiff[0];
+		rtl8192_setBBreg(dev, rFPGA0_TxGainStage, (bXBTxAGC|bXCTxAGC),
+				 reg_u32);
+
+		/* XSTALLCap */
+		reg_u32 = priv->CrystalCap & 0xf;
+		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, bXtalCap,
+				 reg_u32);
+	}
+
+	/* Check if the CCK HighPower is turned ON.
+	 * This is used to calculate PWDB.
+	 */
+	priv->bCckHighPower = (u8)rtl8192_QueryBBReg(dev,
+						     rFPGA0_XA_HSSIParameter2,
+						     0x200);
+}
+
+/******************************************************************************
+ * function:  This function initializes BB&RF
+ * input:     net_device	*dev
+ * output:    none
+ * return:    none
+ * notice:    Initialization value may change all the time, so please make
+ *            sure it has been synced with the newest.
+ *****************************************************************************/
+void rtl8192_BBConfig(struct net_device *dev)
+{
+	rtl8192_InitBBRFRegDef(dev);
+	/* config BB&RF. As hardCode based initialization has not been well
+	 * implemented, so use file first.
+	 * FIXME: should implement it for hardcode?
+	 */
+	rtl8192_BB_Config_ParaFile(dev);
+}
+
+/******************************************************************************
+ * function:  This function obtains the initialization value of Tx power Level
+ *            offset
+ * input:     net_device	*dev
+ * output:    none
+ * return:    none
+ *****************************************************************************/
+void rtl8192_phy_getTxPower(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 tmp;
+
+	read_nic_dword(dev, rTxAGC_Rate18_06,
+		       &priv->MCSTxPowerLevelOriginalOffset[0]);
+	read_nic_dword(dev, rTxAGC_Rate54_24,
+		       &priv->MCSTxPowerLevelOriginalOffset[1]);
+	read_nic_dword(dev, rTxAGC_Mcs03_Mcs00,
+		       &priv->MCSTxPowerLevelOriginalOffset[2]);
+	read_nic_dword(dev, rTxAGC_Mcs07_Mcs04,
+		       &priv->MCSTxPowerLevelOriginalOffset[3]);
+	read_nic_dword(dev, rTxAGC_Mcs11_Mcs08,
+		       &priv->MCSTxPowerLevelOriginalOffset[4]);
+	read_nic_dword(dev, rTxAGC_Mcs15_Mcs12,
+		       &priv->MCSTxPowerLevelOriginalOffset[5]);
+
+	/* Read rx initial gain */
+	read_nic_byte(dev, rOFDM0_XAAGCCore1, &priv->DefaultInitialGain[0]);
+	read_nic_byte(dev, rOFDM0_XBAGCCore1, &priv->DefaultInitialGain[1]);
+	read_nic_byte(dev, rOFDM0_XCAGCCore1, &priv->DefaultInitialGain[2]);
+	read_nic_byte(dev, rOFDM0_XDAGCCore1, &priv->DefaultInitialGain[3]);
+	RT_TRACE(COMP_INIT,
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x)\n",
+		 priv->DefaultInitialGain[0], priv->DefaultInitialGain[1],
+		 priv->DefaultInitialGain[2], priv->DefaultInitialGain[3]);
+
+	/* Read framesync */
+	read_nic_byte(dev, rOFDM0_RxDetector3, &priv->framesync);
+	read_nic_byte(dev, rOFDM0_RxDetector2, &tmp);
+	priv->framesyncC34 = tmp;
+	RT_TRACE(COMP_INIT, "Default framesync (0x%x) = 0x%x\n",
+		rOFDM0_RxDetector3, priv->framesync);
+
+	/* Read SIFS (save the value read fome MACPHY_REG.txt) */
+	read_nic_word(dev, SIFS, &priv->SifsTime);
+}
+
+/******************************************************************************
+ * function:  This function sets the initialization value of Tx power Level
+ *            offset
+ * input:     net_device        *dev
+ *            u8                channel
+ * output:    none
+ * return:    none
+ ******************************************************************************/
+void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8	powerlevel = priv->TxPowerLevelCCK[channel-1];
+	u8	powerlevelOFDM24G = priv->TxPowerLevelOFDM24G[channel-1];
+
+	switch (priv->rf_chip) {
+	case RF_8256:
+		/* need further implement */
+		phy_set_rf8256_cck_tx_power(dev, powerlevel);
+		phy_set_rf8256_ofdm_tx_power(dev, powerlevelOFDM24G);
+		break;
+	default:
+		RT_TRACE((COMP_PHY|COMP_ERR),
+			 "error RF chipID(8225 or 8258) in function %s()\n",
+			 __func__);
+		break;
+	}
+}
+
+/******************************************************************************
+ * function:  This function checks Rf chip to do RF config
+ * input:     net_device	*dev
+ * output:    none
+ * return:    only 8256 is supported
+ ******************************************************************************/
+void rtl8192_phy_RFConfig(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	switch (priv->rf_chip) {
+	case RF_8256:
+		phy_rf8256_config(dev);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, "error chip id\n");
+		break;
+	}
+}
+
+/******************************************************************************
+ * function:  This function updates Initial gain
+ * input:     net_device	*dev
+ * output:    none
+ * return:    As Windows has not implemented this, wait for complement
+ ******************************************************************************/
+void rtl8192_phy_updateInitGain(struct net_device *dev)
+{
+}
+
+/******************************************************************************
+ * function:  This function read RF parameters from general head file,
+ *            and do RF 3-wire
+ * input:     net_device	*dev
+ *            rf90_radio_path_e e_rfpath
+ * output:    none
+ * return:    return code show if RF configuration is successful(0:pass, 1:fail)
+ * notice:    Delay may be required for RF configuration
+ *****************************************************************************/
+u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
+				      enum rf90_radio_path_e	e_rfpath)
+{
+	int i;
+
+	switch (e_rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < RadioA_ArrayLength; i = i+2) {
+			if (Rtl8192UsbRadioA_Array[i] == 0xfe) {
+				mdelay(100);
+				continue;
+			}
+			rtl8192_phy_SetRFReg(dev, e_rfpath,
+					     Rtl8192UsbRadioA_Array[i],
+					     bMask12Bits,
+					     Rtl8192UsbRadioA_Array[i+1]);
+			mdelay(1);
+		}
+		break;
+	case RF90_PATH_B:
+		for (i = 0; i < RadioB_ArrayLength; i = i+2) {
+			if (Rtl8192UsbRadioB_Array[i] == 0xfe) {
+				mdelay(100);
+				continue;
+			}
+			rtl8192_phy_SetRFReg(dev, e_rfpath,
+					     Rtl8192UsbRadioB_Array[i],
+					     bMask12Bits,
+					     Rtl8192UsbRadioB_Array[i+1]);
+			mdelay(1);
+		}
+		break;
+	case RF90_PATH_C:
+		for (i = 0; i < RadioC_ArrayLength; i = i+2) {
+			if (Rtl8192UsbRadioC_Array[i] == 0xfe) {
+				mdelay(100);
+				continue;
+			}
+			rtl8192_phy_SetRFReg(dev, e_rfpath,
+					     Rtl8192UsbRadioC_Array[i],
+					     bMask12Bits,
+					     Rtl8192UsbRadioC_Array[i+1]);
+			mdelay(1);
+		}
+		break;
+	case RF90_PATH_D:
+		for (i = 0; i < RadioD_ArrayLength; i = i+2) {
+			if (Rtl8192UsbRadioD_Array[i] == 0xfe) {
+				mdelay(100);
+				continue;
+			}
+			rtl8192_phy_SetRFReg(dev, e_rfpath,
+					     Rtl8192UsbRadioD_Array[i],
+					     bMask12Bits,
+					     Rtl8192UsbRadioD_Array[i+1]);
+			mdelay(1);
+		}
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ * function:  This function sets Tx Power of the channel
+ * input:     net_device        *dev
+ *            u8                channel
+ * output:    none
+ * return:    none
+ * notice:
+ ******************************************************************************/
+static void rtl8192_SetTxPowerLevel(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8	powerlevel = priv->TxPowerLevelCCK[channel-1];
+	u8	powerlevelOFDM24G = priv->TxPowerLevelOFDM24G[channel-1];
+
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+
+	case RF_8256:
+		phy_set_rf8256_cck_tx_power(dev, powerlevel);
+		phy_set_rf8256_ofdm_tx_power(dev, powerlevelOFDM24G);
+		break;
+
+	case RF_8258:
+		break;
+	default:
+		RT_TRACE(COMP_ERR, "unknown rf chip ID in %s()\n", __func__);
+		break;
+	}
+}
+
+/******************************************************************************
+ * function:  This function sets RF state on or off
+ * input:     net_device         *dev
+ *            RT_RF_POWER_STATE  eRFPowerState  //Power State to set
+ * output:    none
+ * return:    none
+ * notice:
+ *****************************************************************************/
+bool rtl8192_SetRFPowerState(struct net_device *dev,
+			     RT_RF_POWER_STATE eRFPowerState)
+{
+	bool				bResult = true;
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (eRFPowerState == priv->ieee80211->eRFPowerState)
+		return false;
+
+	if (priv->SetRFPowerStateInProgress)
+		return false;
+
+	priv->SetRFPowerStateInProgress = true;
+
+	switch (priv->rf_chip) {
+	case RF_8256:
+		switch (eRFPowerState) {
+		case eRfOn:
+			/* RF-A, RF-B */
+			/* enable RF-Chip A/B - 0x860[4] */
+			rtl8192_setBBreg(dev, rFPGA0_XA_RFInterfaceOE, BIT(4),
+					 0x1);
+			/* analog to digital on - 0x88c[9:8] */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter4, 0x300,
+					 0x3);
+			/* digital to analog on - 0x880[4:3] */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x18,
+					 0x3);
+			/* rx antenna on - 0xc04[1:0] */
+			rtl8192_setBBreg(dev, rOFDM0_TRxPathEnable, 0x3, 0x3);
+			/* rx antenna on - 0xd04[1:0] */
+			rtl8192_setBBreg(dev, rOFDM1_TRxPathEnable, 0x3, 0x3);
+			/* analog to digital part2 on - 0x880[6:5] */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x60,
+					 0x3);
+
+			break;
+
+		case eRfSleep:
+
+			break;
+
+		case eRfOff:
+			/* RF-A, RF-B */
+			/* disable RF-Chip A/B - 0x860[4] */
+			rtl8192_setBBreg(dev, rFPGA0_XA_RFInterfaceOE, BIT(4),
+					 0x0);
+			/* analog to digital off, for power save */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter4, 0xf00,
+					 0x0); /* 0x88c[11:8] */
+			/* digital to analog off, for power save - 0x880[4:3] */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x18,
+					 0x0);
+			/* rx antenna off - 0xc04[3:0] */
+			rtl8192_setBBreg(dev, rOFDM0_TRxPathEnable, 0xf, 0x0);
+			/* rx antenna off - 0xd04[3:0] */
+			rtl8192_setBBreg(dev, rOFDM1_TRxPathEnable, 0xf, 0x0);
+			/* analog to digital part2 off, for power save */
+			rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x60,
+					 0x0); /* 0x880[6:5] */
+
+			break;
+
+		default:
+			bResult = false;
+			RT_TRACE(COMP_ERR, "%s(): unknown state to set: 0x%X\n",
+				 __func__, eRFPowerState);
+			break;
+		}
+		break;
+	default:
+		RT_TRACE(COMP_ERR, "Not support rf_chip(%x)\n", priv->rf_chip);
+		break;
+	}
+	priv->SetRFPowerStateInProgress = false;
+
+	return bResult;
+}
+
+/******************************************************************************
+ * function:  This function sets command table variable (struct sw_chnl_cmd).
+ * input:     sw_chnl_cmd      *CmdTable    //table to be set
+ *            u32            CmdTableIdx  //variable index in table to be set
+ *            u32            CmdTableSz   //table size
+ *            switch_chan_cmd_id    CmdID        //command ID to set
+ *            u32            Para1
+ *            u32            Para2
+ *            u32            msDelay
+ * output:
+ * return:    true if finished, false otherwise
+ * notice:
+ ******************************************************************************/
+static u8 rtl8192_phy_SetSwChnlCmdArray(struct sw_chnl_cmd *CmdTable, u32 CmdTableIdx,
+					u32 CmdTableSz, enum switch_chan_cmd_id CmdID,
+					u32 Para1, u32 Para2, u32 msDelay)
+{
+	struct sw_chnl_cmd *pCmd;
+
+	if (!CmdTable) {
+		RT_TRACE(COMP_ERR, "%s(): CmdTable cannot be NULL\n", __func__);
+		return false;
+	}
+	if (CmdTableIdx >= CmdTableSz) {
+		RT_TRACE(COMP_ERR, "%s(): Access invalid index, please check size of the table, CmdTableIdx:%d, CmdTableSz:%d\n",
+			 __func__, CmdTableIdx, CmdTableSz);
+		return false;
+	}
+
+	pCmd = CmdTable + CmdTableIdx;
+	pCmd->cmd_id = CmdID;
+	pCmd->para_1 = Para1;
+	pCmd->para_2 = Para2;
+	pCmd->ms_delay = msDelay;
+
+	return true;
+}
+
+/******************************************************************************
+ * function:  This function sets channel step by step
+ * input:     net_device        *dev
+ *            u8                channel
+ *            u8                *stage   //3 stages
+ *            u8                *step
+ *            u32               *delay   //whether need to delay
+ * output:    store new stage, step and delay for next step
+ *            (combine with function above)
+ * return:    true if finished, false otherwise
+ * notice:    Wait for simpler function to replace it
+ *****************************************************************************/
+static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
+				       u8 *stage, u8 *step, u32 *delay)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	struct sw_chnl_cmd *pre_cmd;
+	u32 pre_cmd_cnt = 0;
+	struct sw_chnl_cmd *post_cmd;
+	u32 post_cmd_cnt = 0;
+	struct sw_chnl_cmd *rf_cmd;
+	u32 rf_cmd_cnt = 0;
+	struct sw_chnl_cmd *current_cmd = NULL;
+	u8 e_rfpath;
+	bool ret;
+
+	pre_cmd = kcalloc(MAX_PRECMD_CNT, sizeof(*pre_cmd), GFP_KERNEL);
+	if (!pre_cmd)
+		return false;
+
+	post_cmd = kcalloc(MAX_POSTCMD_CNT, sizeof(*post_cmd), GFP_KERNEL);
+	if (!post_cmd) {
+		kfree(pre_cmd);
+		return false;
+	}
+
+	rf_cmd = kcalloc(MAX_RFDEPENDCMD_CNT, sizeof(*rf_cmd), GFP_KERNEL);
+	if (!rf_cmd) {
+		kfree(pre_cmd);
+		kfree(post_cmd);
+		return false;
+	}
+
+	RT_TRACE(COMP_CH, "%s() stage: %d, step: %d, channel: %d\n",
+		 __func__, *stage, *step, channel);
+	if (!is_legal_channel(priv->ieee80211, channel)) {
+		RT_TRACE(COMP_ERR, "set to illegal channel: %d\n", channel);
+		/* return true to tell upper caller function this channel
+		 * setting is finished! Or it will in while loop.
+		 */
+		ret = true;
+		goto out;
+	}
+	/* FIXME: need to check whether channel is legal or not here */
+
+	/* <1> Fill up pre common command. */
+	rtl8192_phy_SetSwChnlCmdArray(pre_cmd, pre_cmd_cnt++,
+				      MAX_PRECMD_CNT, CMD_ID_SET_TX_PWR_LEVEL,
+				      0, 0, 0);
+	rtl8192_phy_SetSwChnlCmdArray(pre_cmd, pre_cmd_cnt++,
+				      MAX_PRECMD_CNT, CMD_ID_END, 0, 0, 0);
+
+	/* <2> Fill up post common command. */
+	rtl8192_phy_SetSwChnlCmdArray(post_cmd, post_cmd_cnt++,
+				      MAX_POSTCMD_CNT, CMD_ID_END, 0, 0, 0);
+
+	/* <3> Fill up RF dependent command. */
+	switch (priv->rf_chip) {
+	case RF_8225:
+		if (!(channel >= 1 && channel <= 14)) {
+			RT_TRACE(COMP_ERR,
+				 "illegal channel for Zebra 8225: %d\n",
+				 channel);
+			ret = true;
+			goto out;
+		}
+		rtl8192_phy_SetSwChnlCmdArray(rf_cmd, rf_cmd_cnt++,
+					      MAX_RFDEPENDCMD_CNT,
+					      CMD_ID_RF_WRITE_REG,
+					      rZebra1_Channel,
+					      RF_CHANNEL_TABLE_ZEBRA[channel],
+					      10);
+		rtl8192_phy_SetSwChnlCmdArray(rf_cmd, rf_cmd_cnt++,
+					      MAX_RFDEPENDCMD_CNT,
+					      CMD_ID_END, 0, 0, 0);
+		break;
+
+	case RF_8256:
+		/* TEST!! This is not the table for 8256!! */
+		if (!(channel >= 1 && channel <= 14)) {
+			RT_TRACE(COMP_ERR,
+				 "illegal channel for Zebra 8256: %d\n",
+				 channel);
+			ret = true;
+			goto out;
+		}
+		rtl8192_phy_SetSwChnlCmdArray(rf_cmd, rf_cmd_cnt++,
+					      MAX_RFDEPENDCMD_CNT,
+					      CMD_ID_RF_WRITE_REG,
+					      rZebra1_Channel, channel, 10);
+		rtl8192_phy_SetSwChnlCmdArray(rf_cmd, rf_cmd_cnt++,
+					      MAX_RFDEPENDCMD_CNT,
+					      CMD_ID_END, 0, 0, 0);
+		break;
+
+	case RF_8258:
+		break;
+
+	default:
+		RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n", priv->rf_chip);
+		ret = true;
+		goto out;
+	}
+
+	do {
+		switch (*stage) {
+		case 0:
+			current_cmd = &pre_cmd[*step];
+			break;
+		case 1:
+			current_cmd = &rf_cmd[*step];
+			break;
+		case 2:
+			current_cmd = &post_cmd[*step];
+			break;
+		}
+
+		if (current_cmd->cmd_id == CMD_ID_END) {
+			if ((*stage) == 2) {
+				*delay = current_cmd->ms_delay;
+				ret = true;
+				goto out;
+			}
+			(*stage)++;
+			(*step) = 0;
+			continue;
+		}
+
+		switch (current_cmd->cmd_id) {
+		case CMD_ID_SET_TX_PWR_LEVEL:
+			if (priv->card_8192_version == VERSION_819XU_A)
+				/* consider it later! */
+				rtl8192_SetTxPowerLevel(dev, channel);
+			break;
+		case CMD_ID_WRITE_PORT_ULONG:
+			write_nic_dword(dev, current_cmd->para_1,
+					current_cmd->para_2);
+			break;
+		case CMD_ID_WRITE_PORT_USHORT:
+			write_nic_word(dev, current_cmd->para_1,
+				       (u16)current_cmd->para_2);
+			break;
+		case CMD_ID_WRITE_PORT_UCHAR:
+			write_nic_byte(dev, current_cmd->para_1,
+				       (u8)current_cmd->para_2);
+			break;
+		case CMD_ID_RF_WRITE_REG:
+			for (e_rfpath = 0; e_rfpath < RF90_PATH_MAX; e_rfpath++) {
+				rtl8192_phy_SetRFReg(dev,
+						     (enum rf90_radio_path_e)e_rfpath,
+						     current_cmd->para_1,
+						     bZebra1_ChannelNum,
+						     current_cmd->para_2);
+			}
+			break;
+		default:
+			break;
+		}
+
+		break;
+	} while (true);
+
+	*delay = current_cmd->ms_delay;
+	(*step)++;
+	ret = false;
+
+out:
+	kfree(pre_cmd);
+	kfree(post_cmd);
+	kfree(rf_cmd);
+
+	return ret;
+}
+
+/******************************************************************************
+ * function:  This function does actually set channel work
+ * input:     net_device        *dev
+ *            u8                channel
+ * output:    none
+ * return:    none
+ * notice:    We should not call this function directly
+ *****************************************************************************/
+static void rtl8192_phy_FinishSwChnlNow(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u32	delay = 0;
+
+	while (!rtl8192_phy_SwChnlStepByStep(dev, channel, &priv->SwChnlStage,
+					     &priv->SwChnlStep, &delay)) {
+		if (!priv->up)
+			break;
+	}
+}
+
+/******************************************************************************
+ * function:  Callback routine of the work item for switch channel.
+ * input:     net_device	*dev
+ *
+ * output:    none
+ * return:    none
+ *****************************************************************************/
+void rtl8192_SwChnl_WorkItem(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	RT_TRACE(COMP_CH, "==> SwChnlCallback819xUsbWorkItem(), chan:%d\n",
+		 priv->chan);
+
+	rtl8192_phy_FinishSwChnlNow(dev, priv->chan);
+
+	RT_TRACE(COMP_CH, "<== SwChnlCallback819xUsbWorkItem()\n");
+}
+
+/******************************************************************************
+ * function:  This function scheduled actual work item to set channel
+ * input:     net_device        *dev
+ *            u8                channel   //channel to set
+ * output:    none
+ * return:    return code show if workitem is scheduled (1:pass, 0:fail)
+ * notice:    Delay may be required for RF configuration
+ ******************************************************************************/
+u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	RT_TRACE(COMP_CH, "%s(), SwChnlInProgress: %d\n", __func__,
+		 priv->SwChnlInProgress);
+	if (!priv->up)
+		return false;
+	if (priv->SwChnlInProgress)
+		return false;
+
+	/* -------------------------------------------- */
+	switch (priv->ieee80211->mode) {
+	case WIRELESS_MODE_A:
+	case WIRELESS_MODE_N_5G:
+		if (channel <= 14) {
+			RT_TRACE(COMP_ERR, "WIRELESS_MODE_A but channel<=14\n");
+			return false;
+		}
+		break;
+	case WIRELESS_MODE_B:
+		if (channel > 14) {
+			RT_TRACE(COMP_ERR, "WIRELESS_MODE_B but channel>14\n");
+			return false;
+		}
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		if (channel > 14) {
+			RT_TRACE(COMP_ERR, "WIRELESS_MODE_G but channel>14\n");
+			return false;
+		}
+		break;
+	}
+	/* -------------------------------------------- */
+
+	priv->SwChnlInProgress = true;
+	if (channel == 0)
+		channel = 1;
+
+	priv->chan = channel;
+
+	priv->SwChnlStage = 0;
+	priv->SwChnlStep = 0;
+	if (priv->up)
+		rtl8192_SwChnl_WorkItem(dev);
+
+	priv->SwChnlInProgress = false;
+	return true;
+}
+
+/******************************************************************************
+ * function:  Callback routine of the work item for set bandwidth mode.
+ * input:     net_device	 *dev
+ * output:    none
+ * return:    none
+ * notice:    I doubt whether SetBWModeInProgress flag is necessary as we can
+ *            test whether current work in the queue or not.//do I?
+ *****************************************************************************/
+void rtl8192_SetBWModeWorkItem(struct net_device *dev)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+	u8 regBwOpMode;
+
+	RT_TRACE(COMP_SWBW, "%s()  Switch to %s bandwidth\n", __func__,
+		 priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20?"20MHz":"40MHz");
+
+	if (priv->rf_chip == RF_PSEUDO_11N) {
+		priv->SetBWModeInProgress = false;
+		return;
+	}
+
+	/* <1> Set MAC register */
+	read_nic_byte(dev, BW_OPMODE, &regBwOpMode);
+
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		regBwOpMode |= BW_OPMODE_20MHZ;
+		/* We have not verify whether this register works */
+		write_nic_byte(dev, BW_OPMODE, regBwOpMode);
+		break;
+
+	case HT_CHANNEL_WIDTH_20_40:
+		regBwOpMode &= ~BW_OPMODE_20MHZ;
+		/* We have not verify whether this register works */
+		write_nic_byte(dev, BW_OPMODE, regBwOpMode);
+		break;
+
+	default:
+		RT_TRACE(COMP_ERR,
+			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
+			 priv->CurrentChannelBW);
+		break;
+	}
+
+	/* <2> Set PHY related register */
+	switch (priv->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl8192_setBBreg(dev, rFPGA0_RFMOD, bRFMOD, 0x0);
+		rtl8192_setBBreg(dev, rFPGA1_RFMOD, bRFMOD, 0x0);
+		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1,
+				 0x00100000, 1);
+
+		/* Correct the tx power for CCK rate in 20M. */
+		priv->cck_present_attenuation =
+			priv->cck_present_attenuation_20Mdefault +
+			priv->cck_present_attenuation_difference;
+
+		if (priv->cck_present_attenuation > 22)
+			priv->cck_present_attenuation = 22;
+		if (priv->cck_present_attenuation < 0)
+			priv->cck_present_attenuation = 0;
+		RT_TRACE(COMP_INIT,
+			 "20M, pHalData->CCKPresentAttentuation = %d\n",
+			 priv->cck_present_attenuation);
+
+		if (priv->chan == 14 && !priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = true;
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else if (priv->chan != 14 && priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = false;
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else {
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		}
+
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl8192_setBBreg(dev, rFPGA0_RFMOD, bRFMOD, 0x1);
+		rtl8192_setBBreg(dev, rFPGA1_RFMOD, bRFMOD, 0x1);
+		rtl8192_setBBreg(dev, rCCK0_System, bCCKSideBand,
+				 priv->nCur40MhzPrimeSC >> 1);
+		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x00100000, 0);
+		rtl8192_setBBreg(dev, rOFDM1_LSTF, 0xC00,
+				 priv->nCur40MhzPrimeSC);
+		priv->cck_present_attenuation =
+			priv->cck_present_attenuation_40Mdefault +
+			priv->cck_present_attenuation_difference;
+
+		if (priv->cck_present_attenuation > 22)
+			priv->cck_present_attenuation = 22;
+		if (priv->cck_present_attenuation < 0)
+			priv->cck_present_attenuation = 0;
+
+		RT_TRACE(COMP_INIT,
+			 "40M, pHalData->CCKPresentAttentuation = %d\n",
+			 priv->cck_present_attenuation);
+		if (priv->chan == 14 && !priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = true;
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else if (priv->chan != 14 && priv->bcck_in_ch14) {
+			priv->bcck_in_ch14 = false;
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		} else {
+			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+		}
+
+		break;
+	default:
+		RT_TRACE(COMP_ERR,
+			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
+			 priv->CurrentChannelBW);
+		break;
+	}
+	/* Skip over setting of J-mode in BB register here.
+	 * Default value is "None J mode".
+	 */
+
+	/* <3> Set RF related register */
+	switch (priv->rf_chip) {
+	case RF_8225:
+		break;
+
+	case RF_8256:
+		phy_set_rf8256_bandwidth(dev, priv->CurrentChannelBW);
+		break;
+
+	case RF_8258:
+		break;
+
+	case RF_PSEUDO_11N:
+		break;
+
+	default:
+		RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n", priv->rf_chip);
+		break;
+	}
+	priv->SetBWModeInProgress = false;
+
+	RT_TRACE(COMP_SWBW, "<==SetBWMode819xUsb(), %d\n",
+		 atomic_read(&priv->ieee80211->atm_swbw));
+}
+
+/******************************************************************************
+ * function:  This function schedules bandwidth switch work.
+ * input:     struct net_deviceq   *dev
+ *            HT_CHANNEL_WIDTH     bandwidth  //20M or 40M
+ *            HT_EXTCHNL_OFFSET    offset     //Upper, Lower, or Don't care
+ * output:    none
+ * return:    none
+ * notice:    I doubt whether SetBWModeInProgress flag is necessary as we can
+ *	      test whether current work in the queue or not.//do I?
+ *****************************************************************************/
+void rtl8192_SetBWMode(struct net_device *dev,
+		       enum ht_channel_width bandwidth,
+		       enum ht_extension_chan_offset offset)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	if (priv->SetBWModeInProgress)
+		return;
+	priv->SetBWModeInProgress = true;
+
+	priv->CurrentChannelBW = bandwidth;
+
+	if (offset == HT_EXTCHNL_OFFSET_LOWER)
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_UPPER;
+	else if (offset == HT_EXTCHNL_OFFSET_UPPER)
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_LOWER;
+	else
+		priv->nCur40MhzPrimeSC = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	rtl8192_SetBWModeWorkItem(dev);
+}
+
+void InitialGain819xUsb(struct net_device *dev,	u8 Operation)
+{
+	struct r8192_priv *priv = ieee80211_priv(dev);
+
+	priv->InitialGainOperateType = Operation;
+
+	if (priv->up)
+		queue_delayed_work(priv->priv_wq, &priv->initialgain_operate_wq, 0);
+}
+
+void InitialGainOperateWorkItemCallBack(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct r8192_priv *priv = container_of(dwork, struct r8192_priv,
+					       initialgain_operate_wq);
+	struct net_device *dev = priv->ieee80211->dev;
+#define SCAN_RX_INITIAL_GAIN	0x17
+#define POWER_DETECTION_TH	0x08
+	u32	bitmask;
+	u8	initial_gain;
+	u8	Operation;
+
+	Operation = priv->InitialGainOperateType;
+
+	switch (Operation) {
+	case IG_Backup:
+		RT_TRACE(COMP_SCAN, "IG_Backup, backup the initial gain.\n");
+		initial_gain = SCAN_RX_INITIAL_GAIN;
+		bitmask = bMaskByte0;
+		if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
+			/* FW DIG OFF */
+			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);
+		priv->initgain_backup.xaagccore1 =
+			(u8)rtl8192_QueryBBReg(dev, rOFDM0_XAAGCCore1, bitmask);
+		priv->initgain_backup.xbagccore1 =
+			(u8)rtl8192_QueryBBReg(dev, rOFDM0_XBAGCCore1, bitmask);
+		priv->initgain_backup.xcagccore1 =
+			(u8)rtl8192_QueryBBReg(dev, rOFDM0_XCAGCCore1, bitmask);
+		priv->initgain_backup.xdagccore1 =
+			(u8)rtl8192_QueryBBReg(dev, rOFDM0_XDAGCCore1, bitmask);
+		bitmask = bMaskByte2;
+		priv->initgain_backup.cca =
+			(u8)rtl8192_QueryBBReg(dev, rCCK0_CCA, bitmask);
+
+		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc50 is %x\n",
+			 priv->initgain_backup.xaagccore1);
+		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc58 is %x\n",
+			 priv->initgain_backup.xbagccore1);
+		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc60 is %x\n",
+			 priv->initgain_backup.xcagccore1);
+		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc68 is %x\n",
+			 priv->initgain_backup.xdagccore1);
+		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xa0a is %x\n",
+			 priv->initgain_backup.cca);
+
+		RT_TRACE(COMP_SCAN, "Write scan initial gain = 0x%x\n",
+			 initial_gain);
+		write_nic_byte(dev, rOFDM0_XAAGCCore1, initial_gain);
+		write_nic_byte(dev, rOFDM0_XBAGCCore1, initial_gain);
+		write_nic_byte(dev, rOFDM0_XCAGCCore1, initial_gain);
+		write_nic_byte(dev, rOFDM0_XDAGCCore1, initial_gain);
+		RT_TRACE(COMP_SCAN, "Write scan 0xa0a = 0x%x\n",
+			 POWER_DETECTION_TH);
+		write_nic_byte(dev, 0xa0a, POWER_DETECTION_TH);
+		break;
+	case IG_Restore:
+		RT_TRACE(COMP_SCAN, "IG_Restore, restore the initial gain.\n");
+		bitmask = 0x7f; /* Bit0 ~ Bit6 */
+		if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
+			/* FW DIG OFF */
+			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);
+
+		rtl8192_setBBreg(dev, rOFDM0_XAAGCCore1, bitmask,
+				 (u32)priv->initgain_backup.xaagccore1);
+		rtl8192_setBBreg(dev, rOFDM0_XBAGCCore1, bitmask,
+				 (u32)priv->initgain_backup.xbagccore1);
+		rtl8192_setBBreg(dev, rOFDM0_XCAGCCore1, bitmask,
+				 (u32)priv->initgain_backup.xcagccore1);
+		rtl8192_setBBreg(dev, rOFDM0_XDAGCCore1, bitmask,
+				 (u32)priv->initgain_backup.xdagccore1);
+		bitmask  = bMaskByte2;
+		rtl8192_setBBreg(dev, rCCK0_CCA, bitmask,
+				 (u32)priv->initgain_backup.cca);
+
+		RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc50 is %x\n",
+			 priv->initgain_backup.xaagccore1);
+		RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc58 is %x\n",
+			 priv->initgain_backup.xbagccore1);
+		RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc60 is %x\n",
+			 priv->initgain_backup.xcagccore1);
+		RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc68 is %x\n",
+			 priv->initgain_backup.xdagccore1);
+		RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xa0a is %x\n",
+			 priv->initgain_backup.cca);
+
+		rtl8192_phy_setTxPower(dev, priv->ieee80211->current_network.channel);
+
+		if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
+			/* FW DIG ON */
+			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);
+		break;
+	default:
+		RT_TRACE(COMP_SCAN, "Unknown IG Operation.\n");
+		break;
+	}
+}
diff --git a/drivers/staging/rtl8192u/r819xU_phy.h b/drivers/staging/rtl8192u/r819xU_phy.h
new file mode 100644
index 000000000..8c2933264
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_phy.h
@@ -0,0 +1,83 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _R819XU_PHY_H
+#define _R819XU_PHY_H
+
+/* Channel switch: The size of command tables for switch channel */
+#define MAX_PRECMD_CNT 16
+#define MAX_RFDEPENDCMD_CNT 16
+#define MAX_POSTCMD_CNT 16
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,			//Radio Path A
+	BASEBAND_CONFIG_AGC_TAB = 1,			//Radio Path B
+};
+
+enum switch_chan_cmd_id {
+	CMD_ID_END,
+	CMD_ID_SET_TX_PWR_LEVEL,
+	CMD_ID_WRITE_PORT_ULONG,
+	CMD_ID_WRITE_PORT_USHORT,
+	CMD_ID_WRITE_PORT_UCHAR,
+	CMD_ID_RF_WRITE_REG,
+};
+
+/* -----------------------Define structure---------------------- */
+/* 1. Switch channel related */
+struct sw_chnl_cmd {
+	enum switch_chan_cmd_id	cmd_id;
+	u32		        para_1;
+	u32		        para_2;
+	u32		        ms_delay;
+} __packed;
+
+enum hw90_block_e {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4, /* Never use this */
+};
+
+enum rf90_radio_path_e {
+	RF90_PATH_A = 0,			/* Radio Path A */
+	RF90_PATH_B = 1,			/* Radio Path B */
+	RF90_PATH_C = 2,			/* Radio Path C */
+	RF90_PATH_D = 3,			/* Radio Path D */
+	RF90_PATH_MAX				/* Max RF number 92 support */
+};
+
+u8 rtl8192_phy_CheckIsLegalRFPath(struct net_device *dev, u32 e_rfpath);
+void rtl8192_setBBreg(struct net_device *dev, u32 reg_addr,
+		      u32 bitmask, u32 data);
+u32 rtl8192_QueryBBReg(struct net_device *dev, u32 reg_addr, u32 bitmask);
+void rtl8192_phy_SetRFReg(struct net_device *dev,
+			  enum rf90_radio_path_e e_rfpath,
+			  u32 reg_addr, u32 bitmask, u32 data);
+u32 rtl8192_phy_QueryRFReg(struct net_device *dev,
+			   enum rf90_radio_path_e e_rfpath,
+			   u32 reg_addr, u32 bitmask);
+void rtl8192_phy_configmac(struct net_device *dev);
+u8 rtl8192_phy_checkBBAndRF(struct net_device *dev,
+			    enum hw90_block_e CheckBlock,
+			    enum rf90_radio_path_e e_rfpath);
+void rtl8192_BBConfig(struct net_device *dev);
+void rtl8192_phy_getTxPower(struct net_device *dev);
+void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel);
+void rtl8192_phy_RFConfig(struct net_device *dev);
+void rtl8192_phy_updateInitGain(struct net_device *dev);
+u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
+				      enum rf90_radio_path_e e_rfpath);
+
+u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel);
+void rtl8192_SetBWMode(struct net_device *dev,
+		       enum ht_channel_width bandwidth,
+		       enum ht_extension_chan_offset offset);
+void rtl8192_SwChnl_WorkItem(struct net_device *dev);
+void rtl8192_SetBWModeWorkItem(struct net_device *dev);
+bool rtl8192_SetRFPowerState(struct net_device *dev,
+			     RT_RF_POWER_STATE eRFPowerState);
+void InitialGain819xUsb(struct net_device *dev, u8 Operation);
+
+void InitialGainOperateWorkItemCallBack(struct work_struct *work);
+
+#endif
diff --git a/drivers/staging/rtl8192u/r819xU_phyreg.h b/drivers/staging/rtl8192u/r819xU_phyreg.h
new file mode 100644
index 000000000..c9669821b
--- /dev/null
+++ b/drivers/staging/rtl8192u/r819xU_phyreg.h
@@ -0,0 +1,143 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _R819XU_PHYREG_H
+#define _R819XU_PHYREG_H
+
+#define   RF_DATA				0x1d4					/* FW will write RF data in the register.*/
+
+/* page8 */
+#define rFPGA0_RFMOD				0x800  /* RF mode & CCK TxSC */
+#define rFPGA0_TxGainStage			0x80c
+#define rFPGA0_XA_HSSIParameter1	0x820
+#define rFPGA0_XA_HSSIParameter2	0x824
+#define rFPGA0_XB_HSSIParameter1	0x828
+#define rFPGA0_XB_HSSIParameter2	0x82c
+#define rFPGA0_XC_HSSIParameter1	0x830
+#define rFPGA0_XC_HSSIParameter2	0x834
+#define rFPGA0_XD_HSSIParameter1	0x838
+#define rFPGA0_XD_HSSIParameter2	0x83c
+#define rFPGA0_XA_LSSIParameter		0x840
+#define rFPGA0_XB_LSSIParameter		0x844
+#define rFPGA0_XC_LSSIParameter		0x848
+#define rFPGA0_XD_LSSIParameter		0x84c
+#define rFPGA0_XAB_SwitchControl	0x858
+#define rFPGA0_XCD_SwitchControl	0x85c
+#define rFPGA0_XA_RFInterfaceOE		0x860
+#define rFPGA0_XB_RFInterfaceOE		0x864
+#define rFPGA0_XC_RFInterfaceOE		0x868
+#define rFPGA0_XD_RFInterfaceOE		0x86c
+#define rFPGA0_XAB_RFInterfaceSW	0x870
+#define rFPGA0_XCD_RFInterfaceSW	0x874
+#define rFPGA0_XAB_RFParameter		0x878
+#define rFPGA0_XCD_RFParameter		0x87c
+#define rFPGA0_AnalogParameter1		0x880
+#define rFPGA0_AnalogParameter4		0x88c
+#define rFPGA0_XA_LSSIReadBack		0x8a0
+#define rFPGA0_XB_LSSIReadBack		0x8a4
+#define rFPGA0_XC_LSSIReadBack		0x8a8
+#define rFPGA0_XD_LSSIReadBack		0x8ac
+#define rFPGA0_XAB_RFInterfaceRB	0x8e0
+#define rFPGA0_XCD_RFInterfaceRB	0x8e4
+
+/* page 9 */
+#define rFPGA1_RFMOD				0x900  /* RF mode & OFDM TxSC */
+
+/* page a */
+#define rCCK0_System				0xa00
+#define rCCK0_AFESetting			0xa04
+#define rCCK0_CCA					0xa08
+#define rCCK0_TxFilter1				0xa20
+#define rCCK0_TxFilter2				0xa24
+#define rCCK0_DebugPort				0xa28  /* debug port and Tx filter3 */
+
+/* page c */
+#define rOFDM0_TRxPathEnable		0xc04
+#define rOFDM0_XARxAFE				0xc10  /* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
+#define rOFDM0_XBRxAFE				0xc18
+#define rOFDM0_XBRxIQImbalance		0xc1c
+#define rOFDM0_XCRxAFE				0xc20
+#define rOFDM0_XCRxIQImbalance		0xc24
+#define rOFDM0_XDRxAFE				0xc28
+#define rOFDM0_XDRxIQImbalance		0xc2c
+#define rOFDM0_RxDetector1			0xc30  /* PD,BW & SBD */
+#define rOFDM0_RxDetector2			0xc34  /* SBD & Fame Sync.*/
+#define rOFDM0_RxDetector3			0xc38  /* Frame Sync.*/
+#define rOFDM0_ECCAThreshold		0xc4c /* energy CCA */
+#define rOFDM0_XAAGCCore1		0xc50
+#define rOFDM0_XAAGCCore2		0xc54
+#define rOFDM0_XBAGCCore1		0xc58
+#define rOFDM0_XBAGCCore2		0xc5c
+#define rOFDM0_XCAGCCore1		0xc60
+#define rOFDM0_XCAGCCore2		0xc64
+#define rOFDM0_XDAGCCore1		0xc68
+#define rOFDM0_XDAGCCore2		0xc6c
+#define rOFDM0_XATxIQImbalance		0xc80
+#define rOFDM0_XATxAFE				0xc84
+#define rOFDM0_XBTxIQImbalance		0xc88
+#define rOFDM0_XBTxAFE				0xc8c
+#define rOFDM0_XCTxIQImbalance		0xc90
+#define rOFDM0_XCTxAFE				0xc94
+#define rOFDM0_XDTxIQImbalance		0xc98
+#define rOFDM0_XDTxAFE				0xc9c
+
+/* page d */
+#define rOFDM1_LSTF				0xd00
+#define rOFDM1_TRxPathEnable		0xd04
+
+/* page e */
+#define rTxAGC_Rate18_06			0xe00
+#define rTxAGC_Rate54_24			0xe04
+#define rTxAGC_CCK_Mcs32			0xe08
+#define rTxAGC_Mcs03_Mcs00			0xe10
+#define rTxAGC_Mcs07_Mcs04			0xe14
+#define rTxAGC_Mcs11_Mcs08			0xe18
+#define rTxAGC_Mcs15_Mcs12			0xe1c
+
+/* RF
+ * Zebra1
+ */
+#define rZebra1_Channel				0x7
+
+/* Zebra4 */
+#define rGlobalCtrl				0
+
+/* Bit Mask
+ * page-8
+ */
+#define bRFMOD						0x1
+#define bCCKEn						0x1000000
+#define bOFDMEn						0x2000000
+#define bXBTxAGC					0xf00
+#define bXCTxAGC					0xf000
+#define b3WireDataLength			0x800
+#define b3WireAddressLength			0x400
+#define bRFSI_RFENV				0x10
+#define bLSSIReadAddress			0x3f000000   /* LSSI "Read" Address */
+#define bLSSIReadEdge				0x80000000   /* LSSI "Read" edge signal */
+#define bLSSIReadBackData			0xfff
+#define bXtalCap					0x0f000000
+
+/* page-a */
+#define bCCKSideBand				0x10
+
+/* page e */
+#define bTxAGCRateCCK			0x7f00
+
+/* RF
+ * Zebra1
+ */
+#define bZebra1_ChannelNum        0xf80
+
+/* RTL8258 */
+/* for PutRegsetting & GetRegSetting BitMask */
+#define bMaskByte0                0xff
+#define bMaskByte1                0xff00
+#define bMaskByte2                0xff0000
+#define bMaskHWord                0xffff0000
+#define bMaskLWord                0x0000ffff
+#define bMaskDWord                0xffffffff
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+#define bMask12Bits               0xfff
+
+#endif	/* __INC_HAL8190PCIPHYREG_H */
diff --git a/drivers/staging/rtl8712/Kconfig b/drivers/staging/rtl8712/Kconfig
new file mode 100644
index 000000000..8de264252
--- /dev/null
+++ b/drivers/staging/rtl8712/Kconfig
@@ -0,0 +1,21 @@
+# SPDX-License-Identifier: GPL-2.0
+config R8712U
+	tristate "RealTek RTL8712U (RTL8192SU) Wireless LAN NIC driver"
+	depends on WLAN && USB && CFG80211
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select FW_LOADER
+	help
+	    This option adds the Realtek RTL8712 USB device such as the
+	    D-Link DWA-130.
+
+	    If built as a module, it will be called r8712u.
+
+config R8712_TX_AGGR
+	bool "Realtek RTL8712U Transmit Aggregation code"
+	depends on R8712U && BROKEN
+	help
+	    This option provides transmit aggregation for the Realtek
+	    RTL8712 USB device.
+
+
diff --git a/drivers/staging/rtl8712/Makefile b/drivers/staging/rtl8712/Makefile
new file mode 100644
index 000000000..3ae216b66
--- /dev/null
+++ b/drivers/staging/rtl8712/Makefile
@@ -0,0 +1,35 @@
+# SPDX-License-Identifier: GPL-2.0
+r8712u-y :=				\
+		rtl871x_cmd.o		\
+		rtl8712_cmd.o		\
+		rtl871x_security.o	\
+		rtl871x_eeprom.o	\
+		rtl8712_efuse.o		\
+		hal_init.o		\
+		usb_halinit.o		\
+		usb_ops.o		\
+		usb_ops_linux.o		\
+		rtl871x_io.o		\
+		rtl8712_io.o		\
+		rtl871x_ioctl_linux.o	\
+		rtl871x_ioctl_rtl.o	\
+		rtl871x_ioctl_set.o	\
+		rtl8712_led.o		\
+		rtl871x_mlme.o		\
+		ieee80211.o		\
+		rtl871x_mp_ioctl.o	\
+		rtl871x_mp.o		\
+		mlme_linux.o		\
+		recv_linux.o		\
+		xmit_linux.o		\
+		usb_intf.o		\
+		os_intfs.o		\
+		rtl871x_pwrctrl.o	\
+		rtl8712_recv.o		\
+		rtl871x_recv.o		\
+		rtl871x_sta_mgt.o	\
+		rtl871x_xmit.o		\
+		rtl8712_xmit.o
+
+obj-$(CONFIG_R8712U)	:= r8712u.o
+
diff --git a/drivers/staging/rtl8712/TODO b/drivers/staging/rtl8712/TODO
new file mode 100644
index 000000000..847c8c41f
--- /dev/null
+++ b/drivers/staging/rtl8712/TODO
@@ -0,0 +1,13 @@
+TODO:
+- merge Realtek's bugfixes and new features into the driver
+- switch to use LIB80211
+- switch to use MAC80211
+- checkpatch.pl fixes - only a few remain
+
+A replacement for this driver with MAC80211 support is available
+at https://github.com/chunkeey/rtl8192su
+
+Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org>,
+Larry Finger <Larry.Finger@lwfinger.net>,
+Florian Schilhabel <florian.c.schilhabel@googlemail.com> and
+Linux Driver Project Developer List <driverdev-devel@linuxdriverproject.org>.
diff --git a/drivers/staging/rtl8712/basic_types.h b/drivers/staging/rtl8712/basic_types.h
new file mode 100644
index 000000000..aecded87d
--- /dev/null
+++ b/drivers/staging/rtl8712/basic_types.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __BASIC_TYPES_H__
+#define __BASIC_TYPES_H__
+
+#include <linux/types.h>
+
+#define sint signed int
+
+/* Should we extend this to be host_addr_t and target_addr_t for case:
+ *	host : x86_64
+ *	target : mips64
+ */
+#define addr_t unsigned long
+
+#endif /*__BASIC_TYPES_H__*/
+
diff --git a/drivers/staging/rtl8712/drv_types.h b/drivers/staging/rtl8712/drv_types.h
new file mode 100644
index 000000000..76ac79864
--- /dev/null
+++ b/drivers/staging/rtl8712/drv_types.h
@@ -0,0 +1,175 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+/* ---------------------------------------------------------------------
+ *
+ *	For type defines and data structure defines
+ *
+ * ---------------------------------------------------------------------
+ */
+#ifndef __DRV_TYPES_H__
+#define __DRV_TYPES_H__
+
+struct _adapter;
+
+#include "osdep_service.h"
+#include "wlan_bssdef.h"
+#include "rtl8712_spec.h"
+#include "rtl8712_hal.h"
+#include <linux/mutex.h>
+#include <linux/completion.h>
+
+enum _NIC_VERSION {
+	RTL8711_NIC,
+	RTL8712_NIC,
+	RTL8713_NIC,
+	RTL8716_NIC
+};
+
+struct	qos_priv	{
+	/* bit mask option: u-apsd, s-apsd, ts, block ack... */
+	unsigned int qos_option;
+};
+
+#include "rtl871x_ht.h"
+#include "rtl871x_cmd.h"
+#include "rtl871x_xmit.h"
+#include "rtl871x_recv.h"
+#include "rtl871x_security.h"
+#include "rtl871x_pwrctrl.h"
+#include "rtl871x_io.h"
+#include "rtl871x_eeprom.h"
+#include "sta_info.h"
+#include "rtl871x_mlme.h"
+#include "rtl871x_mp.h"
+#include "rtl871x_debug.h"
+#include "rtl871x_rf.h"
+#include "rtl871x_event.h"
+#include "rtl871x_led.h"
+
+#define SPEC_DEV_ID_DISABLE_HT BIT(1)
+
+struct specific_device_id {
+	u32		flags;
+	u16		idVendor;
+	u16		idProduct;
+
+};
+
+struct registry_priv {
+	u8	chip_version;
+	u8	rfintfs;
+	u8	lbkmode;
+	u8	hci;
+	u8	network_mode;	/*infra, ad-hoc, auto*/
+	struct ndis_802_11_ssid	ssid;
+	u8	channel;/* ad-hoc support requirement */
+	u8	wireless_mode;/* A, B, G, auto */
+	u8	vrtl_carrier_sense; /*Enable, Disable, Auto*/
+	u8	vcs_type;/*RTS/CTS, CTS-to-self*/
+	u16	rts_thresh;
+	u16  frag_thresh;
+	u8	preamble;/*long, short, auto*/
+	u8  scan_mode;/*active, passive*/
+	u8  adhoc_tx_pwr;
+	u8  soft_ap;
+	u8  smart_ps;
+	u8 power_mgnt;
+	u8 radio_enable;
+	u8 long_retry_lmt;
+	u8 short_retry_lmt;
+	u16 busy_thresh;
+	u8 ack_policy;
+	u8 mp_mode;
+	u8 software_encrypt;
+	u8 software_decrypt;
+	/* UAPSD */
+	u8 wmm_enable;
+	u8 uapsd_enable;
+	u8 uapsd_max_sp;
+	u8 uapsd_acbk_en;
+	u8 uapsd_acbe_en;
+	u8 uapsd_acvi_en;
+	u8 uapsd_acvo_en;
+
+	struct wlan_bssid_ex dev_network;
+
+	u8 ht_enable;
+	u8 cbw40_enable;
+	u8 ampdu_enable;/*for tx*/
+	u8 rf_config;
+	u8 low_power;
+	u8 wifi_test;
+};
+
+struct dvobj_priv {
+	struct _adapter *padapter;
+	u32 nr_endpoint;
+	u8   ishighspeed;
+	uint (*inirp_init)(struct _adapter *adapter);
+	uint (*inirp_deinit)(struct _adapter *adapter);
+	struct usb_device *pusbdev;
+};
+
+/**
+ * struct _adapter - the main adapter structure for this device.
+ *
+ * bup: True indicates that the interface is up.
+ */
+struct _adapter {
+	struct	dvobj_priv dvobjpriv;
+	struct	mlme_priv mlmepriv;
+	struct	cmd_priv	cmdpriv;
+	struct	evt_priv	evtpriv;
+	struct	io_queue	*pio_queue;
+	struct	xmit_priv	xmitpriv;
+	struct	recv_priv	recvpriv;
+	struct	sta_priv	stapriv;
+	struct	security_priv	securitypriv;
+	struct	registry_priv	registrypriv;
+	struct	wlan_acl_pool	acl_list;
+	struct	pwrctrl_priv	pwrctrlpriv;
+	struct	eeprom_priv eeprompriv;
+	struct	hal_priv	halpriv;
+	struct	led_priv	ledpriv;
+	struct mp_priv  mppriv;
+	bool	driver_stopped;
+	bool	surprise_removed;
+	bool	suspended;
+	u8	eeprom_address_size;
+	u8	hw_init_completed;
+	struct task_struct *cmd_thread;
+	uint (*dvobj_init)(struct _adapter *adapter);
+	void (*dvobj_deinit)(struct _adapter *adapter);
+	struct net_device *pnetdev;
+	int bup;
+	struct net_device_stats stats;
+	struct iw_statistics iwstats;
+	int pid; /*process id from UI*/
+	struct work_struct wk_filter_rx_ff0;
+	const struct firmware *fw;
+	struct usb_interface *pusb_intf;
+	struct mutex mutex_start;
+	struct completion rtl8712_fw_ready;
+	struct completion rx_filter_ready;
+};
+
+static inline u8 *myid(struct eeprom_priv *peepriv)
+{
+	return peepriv->mac_addr;
+}
+
+u8 r8712_usb_hal_bus_init(struct _adapter *adapter);
+
+#endif /*__DRV_TYPES_H__*/
+
diff --git a/drivers/staging/rtl8712/ethernet.h b/drivers/staging/rtl8712/ethernet.h
new file mode 100644
index 000000000..4b9b8a97a
--- /dev/null
+++ b/drivers/staging/rtl8712/ethernet.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __INC_ETHERNET_H
+#define __INC_ETHERNET_H
+
+#define ETHERNET_HEADER_SIZE		14	/*!< Ethernet Header Length*/
+#define LLC_HEADER_SIZE			6	/*!< LLC Header Length*/
+
+#endif /* #ifndef __INC_ETHERNET_H */
+
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
new file mode 100644
index 000000000..1148075f0
--- /dev/null
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -0,0 +1,401 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _HAL_INIT_C_
+
+#include <linux/usb.h>
+#include <linux/device.h>
+#include <linux/usb/ch9.h>
+#include <linux/firmware.h>
+#include <linux/module.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "usb_osintf.h"
+
+#define FWBUFF_ALIGN_SZ 512
+#define MAX_DUMP_FWSZ (48 * 1024)
+
+static void rtl871x_load_fw_fail(struct _adapter *adapter)
+{
+	struct usb_device *udev = adapter->dvobjpriv.pusbdev;
+	struct device *dev = &udev->dev;
+	struct device *parent = dev->parent;
+
+	complete(&adapter->rtl8712_fw_ready);
+
+	dev_err(&udev->dev, "r8712u: Firmware request failed\n");
+
+	if (parent)
+		device_lock(parent);
+
+	device_release_driver(dev);
+
+	if (parent)
+		device_unlock(parent);
+}
+
+static void rtl871x_load_fw_cb(const struct firmware *firmware, void *context)
+{
+	struct _adapter *adapter = context;
+
+	if (!firmware) {
+		rtl871x_load_fw_fail(adapter);
+		return;
+	}
+	adapter->fw = firmware;
+	/* firmware available - start netdev */
+	register_netdev(adapter->pnetdev);
+	complete(&adapter->rtl8712_fw_ready);
+}
+
+static const char firmware_file[] = "rtlwifi/rtl8712u.bin";
+
+int rtl871x_load_fw(struct _adapter *padapter)
+{
+	struct device *dev = &padapter->dvobjpriv.pusbdev->dev;
+	int rc;
+
+	init_completion(&padapter->rtl8712_fw_ready);
+	dev_info(dev, "r8712u: Loading firmware from \"%s\"\n", firmware_file);
+	rc = request_firmware_nowait(THIS_MODULE, 1, firmware_file, dev,
+				     GFP_KERNEL, padapter, rtl871x_load_fw_cb);
+	if (rc)
+		dev_err(dev, "r8712u: Firmware request error %d\n", rc);
+	return rc;
+}
+MODULE_FIRMWARE("rtlwifi/rtl8712u.bin");
+
+static u32 rtl871x_open_fw(struct _adapter *adapter, const u8 **mappedfw)
+{
+	if (adapter->fw->size > 200000) {
+		dev_err(&adapter->pnetdev->dev, "r8712u: Bad fw->size of %zu\n",
+			adapter->fw->size);
+		return 0;
+	}
+	*mappedfw = adapter->fw->data;
+	return adapter->fw->size;
+}
+
+static void fill_fwpriv(struct _adapter *adapter, struct fw_priv *fwpriv)
+{
+	struct dvobj_priv *dvobj = &adapter->dvobjpriv;
+	struct registry_priv *regpriv = &adapter->registrypriv;
+
+	memset(fwpriv, 0, sizeof(struct fw_priv));
+	/* todo: check if needs endian conversion */
+	fwpriv->hci_sel =  RTL8712_HCI_TYPE_72USB;
+	fwpriv->usb_ep_num = (u8)dvobj->nr_endpoint;
+	fwpriv->bw_40MHz_en = regpriv->cbw40_enable;
+	switch (regpriv->rf_config) {
+	case RTL8712_RF_1T1R:
+		fwpriv->rf_config = RTL8712_RFC_1T1R;
+		break;
+	case RTL8712_RF_2T2R:
+		fwpriv->rf_config = RTL8712_RFC_2T2R;
+		break;
+	case RTL8712_RF_1T2R:
+	default:
+		fwpriv->rf_config = RTL8712_RFC_1T2R;
+	}
+	fwpriv->mp_mode = (regpriv->mp_mode == 1);
+	/* 0:off 1:on 2:auto */
+	fwpriv->vcs_type = regpriv->vrtl_carrier_sense;
+	fwpriv->vcs_mode = regpriv->vcs_type; /* 1:RTS/CTS 2:CTS to self */
+	/* default enable turbo_mode */
+	fwpriv->turbo_mode = (regpriv->wifi_test != 1);
+	fwpriv->low_power_mode = regpriv->low_power;
+}
+
+static void update_fwhdr(struct fw_hdr	*pfwhdr, const u8 *pmappedfw)
+{
+	pfwhdr->signature = le16_to_cpu(*(__le16 *)pmappedfw);
+	pfwhdr->version = le16_to_cpu(*(__le16 *)(pmappedfw + 2));
+	/* define the size of boot loader */
+	pfwhdr->dmem_size = le32_to_cpu(*(__le32 *)(pmappedfw + 4));
+	/* define the size of FW in IMEM */
+	pfwhdr->img_IMEM_size = le32_to_cpu(*(__le32 *)(pmappedfw + 8));
+	/* define the size of FW in SRAM */
+	pfwhdr->img_SRAM_size = le32_to_cpu(*(__le32 *)(pmappedfw + 12));
+	/* define the size of DMEM variable */
+	pfwhdr->fw_priv_sz = le32_to_cpu(*(__le32 *)(pmappedfw + 16));
+}
+
+static u8 chk_fwhdr(struct fw_hdr *pfwhdr, u32 ulfilelength)
+{
+	u32	fwhdrsz, fw_sz;
+
+	/* check signature */
+	if ((pfwhdr->signature != 0x8712) && (pfwhdr->signature != 0x8192))
+		return _FAIL;
+	/* check fw_priv_sze & sizeof(struct fw_priv) */
+	if (pfwhdr->fw_priv_sz != sizeof(struct fw_priv))
+		return _FAIL;
+	/* check fw_sz & image_fw_sz */
+	fwhdrsz = offsetof(struct fw_hdr, fwpriv) + pfwhdr->fw_priv_sz;
+	fw_sz =  fwhdrsz + pfwhdr->img_IMEM_size + pfwhdr->img_SRAM_size +
+		 pfwhdr->dmem_size;
+	if (fw_sz != ulfilelength)
+		return _FAIL;
+	return _SUCCESS;
+}
+
+static u8 rtl8712_dl_fw(struct _adapter *adapter)
+{
+	sint i;
+	u8 tmp8, tmp8_a;
+	u16 tmp16;
+	u32 maxlen = 0; /* for compare usage */
+	uint dump_imem_sz, imem_sz, dump_emem_sz, emem_sz; /* max = 49152; */
+	struct fw_hdr fwhdr;
+	u32 ulfilelength;	/* FW file size */
+	const u8 *mappedfw = NULL;
+	u8 *tmpchar = NULL, *payload, *ptr;
+	struct tx_desc *txdesc;
+	u32 txdscp_sz = sizeof(struct tx_desc);
+	u8 ret = _FAIL;
+
+	ulfilelength = rtl871x_open_fw(adapter, &mappedfw);
+	if (mappedfw && (ulfilelength > 0)) {
+		update_fwhdr(&fwhdr, mappedfw);
+		if (chk_fwhdr(&fwhdr, ulfilelength) == _FAIL)
+			return ret;
+		fill_fwpriv(adapter, &fwhdr.fwpriv);
+		/* firmware check ok */
+		maxlen = (fwhdr.img_IMEM_size > fwhdr.img_SRAM_size) ?
+			  fwhdr.img_IMEM_size : fwhdr.img_SRAM_size;
+		maxlen += txdscp_sz;
+		tmpchar = kmalloc(maxlen + FWBUFF_ALIGN_SZ, GFP_KERNEL);
+		if (!tmpchar)
+			return ret;
+
+		txdesc = (struct tx_desc *)(tmpchar + FWBUFF_ALIGN_SZ -
+			    ((addr_t)(tmpchar) & (FWBUFF_ALIGN_SZ - 1)));
+		payload = (u8 *)(txdesc) + txdscp_sz;
+		ptr = (u8 *)mappedfw + offsetof(struct fw_hdr, fwpriv) +
+		      fwhdr.fw_priv_sz;
+		/* Download FirmWare */
+		/* 1. determine IMEM code size and Load IMEM Code Section */
+		imem_sz = fwhdr.img_IMEM_size;
+		do {
+			memset(txdesc, 0, TXDESC_SIZE);
+			if (imem_sz >  MAX_DUMP_FWSZ/*49152*/) {
+				dump_imem_sz = MAX_DUMP_FWSZ;
+			} else {
+				dump_imem_sz = imem_sz;
+				txdesc->txdw0 |= cpu_to_le32(BIT(28));
+			}
+			txdesc->txdw0 |= cpu_to_le32(dump_imem_sz &
+						       0x0000ffff);
+			memcpy(payload, ptr, dump_imem_sz);
+			r8712_write_mem(adapter, RTL8712_DMA_VOQ,
+					dump_imem_sz + TXDESC_SIZE,
+					(u8 *)txdesc);
+			ptr += dump_imem_sz;
+			imem_sz -= dump_imem_sz;
+		} while (imem_sz > 0);
+		i = 10;
+		tmp16 = r8712_read16(adapter, TCR);
+		while (((tmp16 & _IMEM_CODE_DONE) == 0) && (i > 0)) {
+			usleep_range(10, 1000);
+			tmp16 = r8712_read16(adapter, TCR);
+			i--;
+		}
+		if (i == 0 || (tmp16 & _IMEM_CHK_RPT) == 0)
+			goto exit_fail;
+
+		/* 2.Download EMEM code size and Load EMEM Code Section */
+		emem_sz = fwhdr.img_SRAM_size;
+		do {
+			memset(txdesc, 0, TXDESC_SIZE);
+			if (emem_sz >  MAX_DUMP_FWSZ) { /* max=48k */
+				dump_emem_sz = MAX_DUMP_FWSZ;
+			} else {
+				dump_emem_sz = emem_sz;
+				txdesc->txdw0 |= cpu_to_le32(BIT(28));
+			}
+			txdesc->txdw0 |= cpu_to_le32(dump_emem_sz &
+						       0x0000ffff);
+			memcpy(payload, ptr, dump_emem_sz);
+			r8712_write_mem(adapter, RTL8712_DMA_VOQ,
+					dump_emem_sz + TXDESC_SIZE,
+					(u8 *)txdesc);
+			ptr += dump_emem_sz;
+			emem_sz -= dump_emem_sz;
+		} while (emem_sz > 0);
+		i = 5;
+		tmp16 = r8712_read16(adapter, TCR);
+		while (((tmp16 & _EMEM_CODE_DONE) == 0) && (i > 0)) {
+			usleep_range(10, 1000);
+			tmp16 = r8712_read16(adapter, TCR);
+			i--;
+		}
+		if (i == 0 || (tmp16 & _EMEM_CHK_RPT) == 0)
+			goto exit_fail;
+
+		/* 3.Enable CPU */
+		tmp8 = r8712_read8(adapter, SYS_CLKR);
+		r8712_write8(adapter, SYS_CLKR, tmp8 | BIT(2));
+		tmp8_a = r8712_read8(adapter, SYS_CLKR);
+		if (tmp8_a != (tmp8 | BIT(2)))
+			goto exit_fail;
+
+		tmp8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, tmp8 | BIT(2));
+		tmp8_a = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		if (tmp8_a != (tmp8 | BIT(2)))
+			goto exit_fail;
+
+		r8712_read32(adapter, TCR);
+
+		/* 4.polling IMEM Ready */
+		i = 100;
+		tmp16 = r8712_read16(adapter, TCR);
+		while (((tmp16 & _IMEM_RDY) == 0) && (i > 0)) {
+			msleep(20);
+			tmp16 = r8712_read16(adapter, TCR);
+			i--;
+		}
+		if (i == 0) {
+			r8712_write16(adapter, 0x10250348, 0xc000);
+			r8712_write16(adapter, 0x10250348, 0xc001);
+			r8712_write16(adapter, 0x10250348, 0x2000);
+			r8712_write16(adapter, 0x10250348, 0x2001);
+			r8712_write16(adapter, 0x10250348, 0x2002);
+			r8712_write16(adapter, 0x10250348, 0x2003);
+			goto exit_fail;
+		}
+		/* 5.Download DMEM code size and Load EMEM Code Section */
+		memset(txdesc, 0, TXDESC_SIZE);
+		txdesc->txdw0 |= cpu_to_le32(fwhdr.fw_priv_sz & 0x0000ffff);
+		txdesc->txdw0 |= cpu_to_le32(BIT(28));
+		memcpy(payload, &fwhdr.fwpriv, fwhdr.fw_priv_sz);
+		r8712_write_mem(adapter, RTL8712_DMA_VOQ,
+				fwhdr.fw_priv_sz + TXDESC_SIZE, (u8 *)txdesc);
+
+		/* polling dmem code done */
+		i = 100;
+		tmp16 = r8712_read16(adapter, TCR);
+		while (((tmp16 & _DMEM_CODE_DONE) == 0) && (i > 0)) {
+			msleep(20);
+			tmp16 = r8712_read16(adapter, TCR);
+			i--;
+		}
+		if (i == 0)
+			goto exit_fail;
+
+		tmp8 = r8712_read8(adapter, 0x1025000A);
+		if (tmp8 & BIT(4)) /* When boot from EEPROM,
+				    * & FW need more time to read EEPROM
+				    */
+			i = 60;
+		else			/* boot from EFUSE */
+			i = 30;
+		tmp16 = r8712_read16(adapter, TCR);
+		while (((tmp16 & _FWRDY) == 0) && (i > 0)) {
+			msleep(100);
+			tmp16 = r8712_read16(adapter, TCR);
+			i--;
+		}
+		if (i == 0)
+			goto exit_fail;
+	} else {
+		goto exit_fail;
+	}
+	ret = _SUCCESS;
+
+exit_fail:
+	kfree(tmpchar);
+	return ret;
+}
+
+uint rtl8712_hal_init(struct _adapter *padapter)
+{
+	u32 val32;
+	int i;
+
+	/* r8712 firmware download */
+	if (rtl8712_dl_fw(padapter) != _SUCCESS)
+		return _FAIL;
+
+	netdev_info(padapter->pnetdev, "1 RCR=0x%x\n",
+		    r8712_read32(padapter, RCR));
+	val32 = r8712_read32(padapter, RCR);
+	r8712_write32(padapter, RCR, (val32 | BIT(26))); /* Enable RX TCP
+							  * Checksum offload
+							  */
+	netdev_info(padapter->pnetdev, "2 RCR=0x%x\n",
+		    r8712_read32(padapter, RCR));
+	val32 = r8712_read32(padapter, RCR);
+	r8712_write32(padapter, RCR, (val32 | BIT(25))); /* Append PHY status */
+	val32 = r8712_read32(padapter, 0x10250040);
+	r8712_write32(padapter,  0x10250040, (val32 & 0x00FFFFFF));
+	/* for usb rx aggregation */
+	r8712_write8(padapter, 0x102500B5, r8712_read8(padapter, 0x102500B5) |
+	       BIT(0)); /* page = 128bytes */
+	r8712_write8(padapter, 0x102500BD, r8712_read8(padapter, 0x102500BD) |
+	       BIT(7)); /* enable usb rx aggregation */
+	r8712_write8(padapter, 0x102500D9, 1); /* TH=1 => means that invalidate
+						*  usb rx aggregation
+						*/
+	r8712_write8(padapter, 0x1025FE5B, 0x04); /* 1.7ms/4 */
+	/* Fix the RX FIFO issue(USB error) */
+	r8712_write8(padapter, 0x1025fe5C, r8712_read8(padapter, 0x1025fe5C)
+		     | BIT(7));
+	for (i = 0; i < ETH_ALEN; i++)
+		padapter->eeprompriv.mac_addr[i] = r8712_read8(padapter,
+							       MACID + i);
+	return _SUCCESS;
+}
+
+uint rtl8712_hal_deinit(struct _adapter *padapter)
+{
+	r8712_write8(padapter, RF_CTRL, 0x00);
+	/* Turn off BB */
+	msleep(20);
+	/* Turn off MAC	*/
+	r8712_write8(padapter, SYS_CLKR + 1, 0x38); /* Switch Control Path */
+	r8712_write8(padapter, SYS_FUNC_EN + 1, 0x70);
+	r8712_write8(padapter, PMC_FSM, 0x06);  /* Enable Loader Data Keep */
+	r8712_write8(padapter, SYS_ISO_CTRL, 0xF9); /* Isolation signals from
+						     * CORE, PLL
+						     */
+	r8712_write8(padapter, SYS_ISO_CTRL + 1, 0xe8); /* Enable EFUSE 1.2V */
+	r8712_write8(padapter, AFE_PLL_CTRL, 0x00); /* Disable AFE PLL. */
+	r8712_write8(padapter, LDOA15_CTRL, 0x54);  /* Disable A15V */
+	r8712_write8(padapter, SYS_FUNC_EN + 1, 0x50); /* Disable E-Fuse 1.2V */
+	r8712_write8(padapter, LDOV12D_CTRL, 0x24); /* Disable LDO12(for CE) */
+	r8712_write8(padapter, AFE_MISC, 0x30); /* Disable AFE BG&MB */
+	/* Option for Disable 1.6V LDO.	*/
+	r8712_write8(padapter, SPS0_CTRL, 0x56); /* Disable 1.6V LDO */
+	r8712_write8(padapter, SPS0_CTRL + 1, 0x43);  /* Set SW PFM */
+	return _SUCCESS;
+}
+
+uint rtl871x_hal_init(struct _adapter *padapter)
+{
+	padapter->hw_init_completed = false;
+	if (!padapter->halpriv.hal_bus_init)
+		return _FAIL;
+	if (padapter->halpriv.hal_bus_init(padapter) != _SUCCESS)
+		return _FAIL;
+	if (rtl8712_hal_init(padapter) == _SUCCESS) {
+		padapter->hw_init_completed = true;
+	} else {
+		padapter->hw_init_completed = false;
+		return _FAIL;
+	}
+	return _SUCCESS;
+}
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
new file mode 100644
index 000000000..7d8f1a29d
--- /dev/null
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -0,0 +1,415 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * ieee80211.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _IEEE80211_C
+
+#include "drv_types.h"
+#include "ieee80211.h"
+#include "wifi.h"
+#include "osdep_service.h"
+#include "wlan_bssdef.h"
+
+static const u8 WPA_OUI_TYPE[] = {0x00, 0x50, 0xf2, 1};
+static const u8 WPA_CIPHER_SUITE_NONE[] = {0x00, 0x50, 0xf2, 0};
+static const u8 WPA_CIPHER_SUITE_WEP40[] = {0x00, 0x50, 0xf2, 1};
+static const u8 WPA_CIPHER_SUITE_TKIP[] = {0x00, 0x50, 0xf2, 2};
+static const u8 WPA_CIPHER_SUITE_CCMP[] = {0x00, 0x50, 0xf2, 4};
+static const u8 WPA_CIPHER_SUITE_WEP104[] = {0x00, 0x50, 0xf2, 5};
+
+static const u8 RSN_CIPHER_SUITE_NONE[] = {0x00, 0x0f, 0xac, 0};
+static const u8 RSN_CIPHER_SUITE_WEP40[] = {0x00, 0x0f, 0xac, 1};
+static const u8 RSN_CIPHER_SUITE_TKIP[] = {0x00, 0x0f, 0xac, 2};
+static const u8 RSN_CIPHER_SUITE_CCMP[] = {0x00, 0x0f, 0xac, 4};
+static const u8 RSN_CIPHER_SUITE_WEP104[] = {0x00, 0x0f, 0xac, 5};
+
+/*-----------------------------------------------------------
+ * for adhoc-master to generate ie and provide supported-rate to fw
+ *-----------------------------------------------------------
+ */
+
+static u8 WIFI_CCKRATES[] =  {
+	(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+};
+
+static u8 WIFI_OFDMRATES[] = {
+	(IEEE80211_OFDM_RATE_6MB),
+	(IEEE80211_OFDM_RATE_9MB),
+	(IEEE80211_OFDM_RATE_12MB),
+	(IEEE80211_OFDM_RATE_18MB),
+	(IEEE80211_OFDM_RATE_24MB),
+	(IEEE80211_OFDM_RATE_36MB),
+	(IEEE80211_OFDM_RATE_48MB),
+	(IEEE80211_OFDM_RATE_54MB)
+};
+
+uint r8712_is_cckrates_included(u8 *rate)
+{
+	u32 i = 0;
+
+	while (rate[i] != 0) {
+		if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
+		    (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
+			return true;
+		i++;
+	}
+	return false;
+}
+
+uint r8712_is_cckratesonly_included(u8 *rate)
+{
+	u32 i = 0;
+
+	while (rate[i] != 0) {
+		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+		    (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+			return false;
+		i++;
+	}
+	return true;
+}
+
+/* r8712_set_ie will update frame length */
+u8 *r8712_set_ie(u8 *pbuf, sint index, uint len, u8 *source, uint *frlen)
+{
+	*pbuf = (u8)index;
+	*(pbuf + 1) = (u8)len;
+	if (len > 0)
+		memcpy((void *)(pbuf + 2), (void *)source, len);
+	*frlen = *frlen + (len + 2);
+	return pbuf + len + 2;
+}
+
+/* ---------------------------------------------------------------------------
+ * index: the information element id index, limit is the limit for search
+ * ---------------------------------------------------------------------------
+ */
+u8 *r8712_get_ie(u8 *pbuf, sint index, uint *len, sint limit)
+{
+	sint tmp, i;
+	u8 *p;
+
+	if (limit < 1)
+		return NULL;
+	p = pbuf;
+	i = 0;
+	*len = 0;
+	while (1) {
+		if (*p == index) {
+			*len = *(p + 1);
+			return p;
+		}
+		tmp = *(p + 1);
+		p += (tmp + 2);
+		i += (tmp + 2);
+		if (i >= limit)
+			break;
+	}
+	return NULL;
+}
+
+static void set_supported_rate(u8 *rates, uint mode)
+{
+	memset(rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+	switch (mode) {
+	case WIRELESS_11B:
+		memcpy(rates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11A:
+		memcpy(rates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+	case WIRELESS_11BG:
+		memcpy(rates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		memcpy(rates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES,
+		       IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+	}
+}
+
+static uint r8712_get_rateset_len(u8 *rateset)
+{
+	uint i = 0;
+
+	while (1) {
+		if ((rateset[i]) == 0)
+			break;
+		if (i > 12)
+			break;
+		i++;
+	}
+	return i;
+}
+
+int r8712_generate_ie(struct registry_priv *registrypriv)
+{
+	int rate_len;
+	uint sz = 0;
+	struct wlan_bssid_ex *dev_network = &registrypriv->dev_network;
+	u8 *ie = dev_network->IEs;
+	u16 beacon_period = (u16)dev_network->Configuration.BeaconPeriod;
+
+	/*timestamp will be inserted by hardware*/
+	sz += 8;
+	ie += sz;
+	/*beacon interval : 2bytes*/
+	*(__le16 *)ie = cpu_to_le16(beacon_period);
+	sz += 2;
+	ie += 2;
+	/*capability info*/
+	*(u16 *)ie = 0;
+	*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_IBSS);
+	if (registrypriv->preamble == PREAMBLE_SHORT)
+		*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
+	if (dev_network->Privacy)
+		*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+	sz += 2;
+	ie += 2;
+	/*SSID*/
+	ie = r8712_set_ie(ie, WLAN_EID_SSID, dev_network->Ssid.SsidLength,
+			  dev_network->Ssid.Ssid, &sz);
+	/*supported rates*/
+	set_supported_rate(dev_network->rates, registrypriv->wireless_mode);
+	rate_len = r8712_get_rateset_len(dev_network->rates);
+	if (rate_len > 8) {
+		ie = r8712_set_ie(ie, WLAN_EID_SUPP_RATES, 8,
+				  dev_network->rates, &sz);
+		ie = r8712_set_ie(ie, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8),
+				  (dev_network->rates + 8), &sz);
+	} else {
+		ie = r8712_set_ie(ie, WLAN_EID_SUPP_RATES,
+				  rate_len, dev_network->rates, &sz);
+	}
+	/*DS parameter set*/
+	ie = r8712_set_ie(ie, WLAN_EID_DS_PARAMS, 1,
+			  (u8 *)&dev_network->Configuration.DSConfig, &sz);
+	/*IBSS Parameter Set*/
+	ie = r8712_set_ie(ie, WLAN_EID_IBSS_PARAMS, 2,
+			  (u8 *)&dev_network->Configuration.ATIMWindow, &sz);
+	return sz;
+}
+
+unsigned char *r8712_get_wpa_ie(unsigned char *ie, uint *wpa_ie_len, int limit)
+{
+	u32 len;
+	u16 val16;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *buf = ie;
+
+	while (1) {
+		buf = r8712_get_ie(buf, _WPA_IE_ID_, &len, limit);
+		if (buf) {
+			/*check if oui matches...*/
+			if (memcmp((buf + 2), wpa_oui_type,
+				   sizeof(wpa_oui_type)))
+				goto check_next_ie;
+			/*check version...*/
+			memcpy((u8 *)&val16, (buf + 6), sizeof(val16));
+			le16_to_cpus(&val16);
+			if (val16 != 0x0001)
+				goto check_next_ie;
+			*wpa_ie_len = *(buf + 1);
+			return buf;
+		}
+		*wpa_ie_len = 0;
+		return NULL;
+check_next_ie:
+		limit = limit - (buf - ie) - 2 - len;
+		if (limit <= 0)
+			break;
+		buf += (2 + len);
+	}
+	*wpa_ie_len = 0;
+	return NULL;
+}
+
+unsigned char *r8712_get_wpa2_ie(unsigned char *pie, uint *rsn_ie_len,
+				 int limit)
+{
+	return r8712_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
+}
+
+static int r8712_get_wpa_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, (void *)WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, (void *)WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, (void *)WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, (void *)WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, (void *)WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+	return 0;
+}
+
+static int r8712_get_wpa2_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, (void *)RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, (void *)RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, (void *)RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, (void *)RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, (void *)RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+	return 0;
+}
+
+int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
+		       int *pairwise_cipher)
+{
+	int i;
+	int left, count;
+	u8 *pos;
+
+	if (wpa_ie_len <= 0) {
+		/* No WPA IE - fail silently */
+		return -EINVAL;
+	}
+	if ((*wpa_ie != _WPA_IE_ID_) ||
+	    (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) ||
+	    (memcmp(wpa_ie + 2, (void *)WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
+		return -EINVAL;
+	pos = wpa_ie;
+	pos += 8;
+	left = wpa_ie_len - 8;
+	/*group_cipher*/
+	if (left >= WPA_SELECTOR_LEN) {
+		*group_cipher = r8712_get_wpa_cipher_suite(pos);
+		pos += WPA_SELECTOR_LEN;
+		left -= WPA_SELECTOR_LEN;
+	} else if (left > 0) {
+		return -EINVAL;
+	}
+	/*pairwise_cipher*/
+	if (left >= 2) {
+		count = le16_to_cpu(*(__le16 *)pos);
+		pos += 2;
+		left -= 2;
+		if (count == 0 || left < count * WPA_SELECTOR_LEN)
+			return -EINVAL;
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= r8712_get_wpa_cipher_suite(pos);
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+	} else if (left == 1) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
+			int *pairwise_cipher)
+{
+	int i;
+	int left, count;
+	u8 *pos;
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return -EINVAL;
+	}
+	if ((*rsn_ie != _WPA2_IE_ID_) ||
+	    (*(rsn_ie + 1) != (u8)(rsn_ie_len - 2)))
+		return -EINVAL;
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;
+	/*group_cipher*/
+	if (left >= RSN_SELECTOR_LEN) {
+		*group_cipher = r8712_get_wpa2_cipher_suite(pos);
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+	} else if (left > 0) {
+		return -EINVAL;
+	}
+	/*pairwise_cipher*/
+	if (left >= 2) {
+		count = le16_to_cpu(*(__le16 *)pos);
+		pos += 2;
+		left -= 2;
+		if (count == 0 || left < count * RSN_SELECTOR_LEN)
+			return -EINVAL;
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= r8712_get_wpa2_cipher_suite(pos);
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+	} else if (left == 1) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int r8712_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
+		     u8 *wpa_ie, u16 *wpa_len)
+{
+	u8 authmode;
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint cnt;
+
+	/*Search required WPA or WPA2 IE and copy to sec_ie[ ]*/
+	cnt = _TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_;
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+		if ((authmode == _WPA_IE_ID_) &&
+		    (!memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4))) {
+			memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+			*wpa_len = in_ie[cnt + 1] + 2;
+			cnt += in_ie[cnt + 1] + 2;  /*get next */
+		} else {
+			if (authmode == _WPA2_IE_ID_) {
+				memcpy(rsn_ie, &in_ie[cnt],
+				       in_ie[cnt + 1] + 2);
+				*rsn_len = in_ie[cnt + 1] + 2;
+				cnt += in_ie[cnt + 1] + 2;  /*get next*/
+			} else {
+				cnt += in_ie[cnt + 1] + 2;   /*get next*/
+			}
+		}
+	}
+	return *rsn_len + *wpa_len;
+}
+
+int r8712_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	int match;
+	uint cnt;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	cnt = 12;
+	match = false;
+	while (cnt < in_len) {
+		eid = in_ie[cnt];
+		if ((eid == _WPA_IE_ID_) &&
+		    (!memcmp(&in_ie[cnt + 2], wps_oui, 4))) {
+			memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+			*wps_ielen = in_ie[cnt + 1] + 2;
+			cnt += in_ie[cnt + 1] + 2;
+			match = true;
+			break;
+		}
+		cnt += in_ie[cnt + 1] + 2; /* goto next */
+	}
+	return match;
+}
diff --git a/drivers/staging/rtl8712/ieee80211.h b/drivers/staging/rtl8712/ieee80211.h
new file mode 100644
index 000000000..65ceaca9b
--- /dev/null
+++ b/drivers/staging/rtl8712/ieee80211.h
@@ -0,0 +1,165 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __IEEE80211_H
+#define __IEEE80211_H
+
+#include <linux/ieee80211.h>
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define IEEE_CMD_SET_WPA_IE			2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME				4
+
+#define IEEE_PARAM_WPA_ENABLED			1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED		3
+#define IEEE_PARAM_PRIVACY_INVOKED		4
+#define IEEE_PARAM_AUTH_ALGS			5
+#define IEEE_PARAM_IEEE_802_1X			6
+#define IEEE_PARAM_WPAX_SELECT			7
+
+#define AUTH_ALG_OPEN_SYSTEM			0x1
+#define AUTH_ALG_SHARED_KEY			0x2
+#define AUTH_ALG_LEAP				0x00000004
+
+#define IEEE_MLME_STA_DEAUTH			1
+#define IEEE_MLME_STA_DISASSOC			2
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED	4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED	6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#define WPA_CIPHER_NONE				BIT(0)
+#define WPA_CIPHER_WEP40			BIT(1)
+#define WPA_CIPHER_WEP104			BIT(2)
+#define WPA_CIPHER_TKIP				BIT(3)
+#define WPA_CIPHER_CCMP				BIT(4)
+
+#define WPA_SELECTOR_LEN			4
+#define RSN_HEADER_LEN				4
+
+#define RSN_SELECTOR_LEN 4
+
+enum NETWORK_TYPE {
+	WIRELESS_INVALID	= 0,
+	WIRELESS_11B		= 1,
+	WIRELESS_11G		= 2,
+	WIRELESS_11BG		= (WIRELESS_11B | WIRELESS_11G),
+	WIRELESS_11A		= 4,
+	WIRELESS_11N		= 8,
+	WIRELESS_11GN		= (WIRELESS_11G | WIRELESS_11N),
+	WIRELESS_11BGN		= (WIRELESS_11B | WIRELESS_11G | WIRELESS_11N),
+};
+
+struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[];
+		} wpa_ie;
+		struct {
+			int command;
+			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[];
+		} crypt;
+	} u;
+};
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* QoS,QOS */
+#define NORMAL_ACK			0
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+	u8    dsap;   /* always 0xAA */
+	u8    ssap;   /* always 0xAA */
+	u8    ctrl;   /* always 0x03 */
+	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+} __packed;
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define IEEE80211_CCK_RATE_LEN			4
+#define IEEE80211_NUM_OFDM_RATESLEN	8
+
+#define IEEE80211_CCK_RATE_1MB		        0x02
+#define IEEE80211_CCK_RATE_2MB		        0x04
+#define IEEE80211_CCK_RATE_5MB		        0x0B
+#define IEEE80211_CCK_RATE_11MB		        0x16
+#define IEEE80211_OFDM_RATE_6MB		        0x0C
+#define IEEE80211_OFDM_RATE_9MB		        0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define WEP_KEYS 4
+
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element...
+ */
+#define MAX_RATES_LENGTH                  ((u8)12)
+#define MAX_WPA_IE_LEN 128
+
+struct registry_priv;
+
+u8 *r8712_set_ie(u8 *pbuf, sint index, uint len, u8 *source, uint *frlen);
+u8 *r8712_get_ie(u8 *pbuf, sint index, uint *len, sint limit);
+unsigned char *r8712_get_wpa_ie(unsigned char *pie, uint *rsn_ie_len,
+				int limit);
+unsigned char *r8712_get_wpa2_ie(unsigned char *pie, uint *rsn_ie_len,
+				 int limit);
+int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
+		       int *pairwise_cipher);
+int r8712_parse_wpa2_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
+			int *pairwise_cipher);
+int r8712_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
+		     u8 *wpa_ie, u16 *wpa_len);
+int r8712_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen);
+int r8712_generate_ie(struct registry_priv *pregistrypriv);
+uint r8712_is_cckrates_included(u8 *rate);
+uint r8712_is_cckratesonly_included(u8 *rate);
+
+#endif /* IEEE80211_H */
+
diff --git a/drivers/staging/rtl8712/mlme_linux.c b/drivers/staging/rtl8712/mlme_linux.c
new file mode 100644
index 000000000..b9f5104f3
--- /dev/null
+++ b/drivers/staging/rtl8712/mlme_linux.c
@@ -0,0 +1,160 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * mlme_linux.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _MLME_OSDEP_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "mlme_osdep.h"
+
+static void sitesurvey_ctrl_handler(struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t,
+			   mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer);
+
+	_r8712_sitesurvey_ctrl_handler(adapter);
+	mod_timer(&adapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
+		  jiffies + msecs_to_jiffies(3000));
+}
+
+static void join_timeout_handler (struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t, mlmepriv.assoc_timer);
+
+	_r8712_join_timeout_handler(adapter);
+}
+
+static void _scan_timeout_handler (struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t, mlmepriv.scan_to_timer);
+
+	r8712_scan_timeout_handler(adapter);
+}
+
+static void dhcp_timeout_handler (struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t, mlmepriv.dhcp_timer);
+
+	_r8712_dhcp_timeout_handler(adapter);
+}
+
+static void wdg_timeout_handler (struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t, mlmepriv.wdg_timer);
+
+	r8712_wdg_wk_cmd(adapter);
+
+	mod_timer(&adapter->mlmepriv.wdg_timer,
+		  jiffies + msecs_to_jiffies(2000));
+}
+
+void r8712_init_mlme_timer(struct _adapter *adapter)
+{
+	struct	mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	timer_setup(&mlmepriv->assoc_timer, join_timeout_handler, 0);
+	timer_setup(&mlmepriv->sitesurveyctrl.sitesurvey_ctrl_timer,
+		    sitesurvey_ctrl_handler, 0);
+	timer_setup(&mlmepriv->scan_to_timer, _scan_timeout_handler, 0);
+	timer_setup(&mlmepriv->dhcp_timer, dhcp_timeout_handler, 0);
+	timer_setup(&mlmepriv->wdg_timer, wdg_timeout_handler, 0);
+}
+
+void r8712_os_indicate_connect(struct _adapter *adapter)
+{
+	r8712_indicate_wx_assoc_event(adapter);
+	netif_carrier_on(adapter->pnetdev);
+}
+
+static struct RT_PMKID_LIST   backupPMKIDList[NUM_PMKID_CACHE];
+void r8712_os_indicate_disconnect(struct _adapter *adapter)
+{
+	u8 backupPMKIDIndex = 0;
+	u8 backupTKIPCountermeasure = 0x00;
+
+	r8712_indicate_wx_disassoc_event(adapter);
+	netif_carrier_off(adapter->pnetdev);
+	if (adapter->securitypriv.AuthAlgrthm == 2) { /*/802.1x*/
+		/* We have to backup the PMK information for WiFi PMK Caching
+		 * test item. Backup the btkip_countermeasure information.
+		 * When the countermeasure is trigger, the driver have to
+		 * disconnect with AP for 60 seconds.
+		 */
+
+		memcpy(&backupPMKIDList[0],
+		       &adapter->securitypriv.PMKIDList[0],
+		       sizeof(struct RT_PMKID_LIST) * NUM_PMKID_CACHE);
+		backupPMKIDIndex = adapter->securitypriv.PMKIDIndex;
+		backupTKIPCountermeasure =
+			adapter->securitypriv.btkip_countermeasure;
+		memset((unsigned char *)&adapter->securitypriv, 0,
+		       sizeof(struct security_priv));
+		timer_setup(&adapter->securitypriv.tkip_timer,
+			    r8712_use_tkipkey_handler, 0);
+		/* Restore the PMK information to securitypriv structure
+		 * for the following connection.
+		 */
+		memcpy(&adapter->securitypriv.PMKIDList[0],
+		       &backupPMKIDList[0],
+		       sizeof(struct RT_PMKID_LIST) * NUM_PMKID_CACHE);
+		adapter->securitypriv.PMKIDIndex = backupPMKIDIndex;
+		adapter->securitypriv.btkip_countermeasure =
+					 backupTKIPCountermeasure;
+	} else { /*reset values in securitypriv*/
+		struct security_priv *sec_priv = &adapter->securitypriv;
+
+		sec_priv->AuthAlgrthm = 0; /*open system*/
+		sec_priv->PrivacyAlgrthm = _NO_PRIVACY_;
+		sec_priv->PrivacyKeyIndex = 0;
+		sec_priv->XGrpPrivacy = _NO_PRIVACY_;
+		sec_priv->XGrpKeyid = 1;
+		sec_priv->ndisauthtype = Ndis802_11AuthModeOpen;
+		sec_priv->ndisencryptstatus = Ndis802_11WEPDisabled;
+		sec_priv->wps_phase = false;
+	}
+}
+
+void r8712_report_sec_ie(struct _adapter *adapter, u8 authmode, u8 *sec_ie)
+{
+	uint len;
+	u8 *buff, *p, i;
+	union iwreq_data wrqu;
+
+	buff = NULL;
+	if (authmode == _WPA_IE_ID_) {
+		buff = kzalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
+		if (!buff)
+			return;
+		p = buff;
+		p += sprintf(p, "ASSOCINFO(ReqIEs=");
+		len = sec_ie[1] + 2;
+		len =  (len < IW_CUSTOM_MAX) ? len : IW_CUSTOM_MAX;
+		for (i = 0; i < len; i++)
+			p += sprintf(p, "%02x", sec_ie[i]);
+		p += sprintf(p, ")");
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.length = p - buff;
+		wrqu.data.length = (wrqu.data.length < IW_CUSTOM_MAX) ?
+				   wrqu.data.length : IW_CUSTOM_MAX;
+		wireless_send_event(adapter->pnetdev, IWEVCUSTOM, &wrqu, buff);
+		kfree(buff);
+	}
+}
diff --git a/drivers/staging/rtl8712/mlme_osdep.h b/drivers/staging/rtl8712/mlme_osdep.h
new file mode 100644
index 000000000..a02c78258
--- /dev/null
+++ b/drivers/staging/rtl8712/mlme_osdep.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef	__MLME_OSDEP_H_
+#define __MLME_OSDEP_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+void r8712_init_mlme_timer(struct _adapter *padapter);
+void r8712_os_indicate_disconnect(struct _adapter *adapter);
+void r8712_os_indicate_connect(struct _adapter *adapter);
+void r8712_report_sec_ie(struct _adapter *adapter, u8 authmode, u8 *sec_ie);
+int r8712_recv_indicatepkts_in_order(struct _adapter *adapter,
+				     struct recv_reorder_ctrl *precvreorder_ctrl,
+				     int bforced);
+void r8712_indicate_wx_assoc_event(struct _adapter *padapter);
+void r8712_indicate_wx_disassoc_event(struct _adapter *padapter);
+
+#endif	/*_MLME_OSDEP_H_*/
+
diff --git a/drivers/staging/rtl8712/mp_custom_oid.h b/drivers/staging/rtl8712/mp_custom_oid.h
new file mode 100644
index 000000000..a9fac87fc
--- /dev/null
+++ b/drivers/staging/rtl8712/mp_custom_oid.h
@@ -0,0 +1,287 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef	__CUSTOM_OID_H
+#define __CUSTOM_OID_H
+
+/* 0xFF818000 - 0xFF81802F		RTL8180 Mass Production Kit
+ * 0xFF818500 - 0xFF81850F		RTL8185 Setup Utility
+ * 0xFF818580 - 0xFF81858F		RTL8185 Phy Status Utility
+ *
+ * by Owen for Production Kit
+ * For Production Kit with Agilent Equipments
+ * in order to make our custom oids hopefully somewhat unique
+ * we will use 0xFF (indicating implementation specific OID)
+ *	 81(first byte of non zero Realtek unique identifier)
+ *	 80 (second byte of non zero Realtek unique identifier)
+ *	 XX (the custom OID number - providing 255 possible custom oids)
+ */
+#define OID_RT_PRO_RESET_DUT				0xFF818000
+#define OID_RT_PRO_SET_DATA_RATE			0xFF818001
+#define OID_RT_PRO_START_TEST				0xFF818002
+#define OID_RT_PRO_STOP_TEST				0xFF818003
+#define OID_RT_PRO_SET_PREAMBLE				0xFF818004
+#define OID_RT_PRO_SET_SCRAMBLER			0xFF818005
+#define OID_RT_PRO_SET_FILTER_BB			0xFF818006
+#define OID_RT_PRO_SET_MANUAL_DIVERSITY_BB		0xFF818007
+#define OID_RT_PRO_SET_CHANNEL_DIRECT_CALL		0xFF818008
+#define OID_RT_PRO_SET_SLEEP_MODE_DIRECT_CALL		0xFF818009
+#define OID_RT_PRO_SET_WAKE_MODE_DIRECT_CALL		0xFF81800A
+
+#define OID_RT_PRO_SET_TX_ANTENNA_BB			0xFF81800D
+#define OID_RT_PRO_SET_ANTENNA_BB			0xFF81800E
+#define OID_RT_PRO_SET_CR_SCRAMBLER			0xFF81800F
+#define OID_RT_PRO_SET_CR_NEW_FILTER			0xFF818010
+#define OID_RT_PRO_SET_TX_POWER_CONTROL			0xFF818011
+#define OID_RT_PRO_SET_CR_TX_CONFIG			0xFF818012
+#define OID_RT_PRO_GET_TX_POWER_CONTROL			0xFF818013
+#define OID_RT_PRO_GET_CR_SIGNAL_QUALITY		0xFF818014
+#define OID_RT_PRO_SET_CR_SETPOINT			0xFF818015
+#define OID_RT_PRO_SET_INTEGRATOR			0xFF818016
+#define OID_RT_PRO_SET_SIGNAL_QUALITY			0xFF818017
+#define OID_RT_PRO_GET_INTEGRATOR			0xFF818018
+#define OID_RT_PRO_GET_SIGNAL_QUALITY			0xFF818019
+#define OID_RT_PRO_QUERY_EEPROM_TYPE			0xFF81801A
+#define OID_RT_PRO_WRITE_MAC_ADDRESS			0xFF81801B
+#define OID_RT_PRO_READ_MAC_ADDRESS			0xFF81801C
+#define OID_RT_PRO_WRITE_CIS_DATA			0xFF81801D
+#define OID_RT_PRO_READ_CIS_DATA			0xFF81801E
+#define OID_RT_PRO_WRITE_POWER_CONTROL			0xFF81801F
+#define OID_RT_PRO_READ_POWER_CONTROL			0xFF818020
+#define OID_RT_PRO_WRITE_EEPROM				0xFF818021
+#define OID_RT_PRO_READ_EEPROM				0xFF818022
+#define OID_RT_PRO_RESET_TX_PACKET_SENT			0xFF818023
+#define OID_RT_PRO_QUERY_TX_PACKET_SENT			0xFF818024
+#define OID_RT_PRO_RESET_RX_PACKET_RECEIVED		0xFF818025
+#define OID_RT_PRO_QUERY_RX_PACKET_RECEIVED		0xFF818026
+#define OID_RT_PRO_QUERY_RX_PACKET_CRC32_ERROR		0xFF818027
+#define OID_RT_PRO_QUERY_CURRENT_ADDRESS		0xFF818028
+#define OID_RT_PRO_QUERY_PERMANENT_ADDRESS		0xFF818029
+#define OID_RT_PRO_SET_PHILIPS_RF_PARAMETERS		0xFF81802A
+#define OID_RT_PRO_RECEIVE_PACKET			0xFF81802C
+#define OID_RT_PRO_WRITE_EEPROM_BYTE			0xFF81802D
+#define OID_RT_PRO_READ_EEPROM_BYTE			0xFF81802E
+#define OID_RT_PRO_SET_MODULATION			0xFF81802F
+#define OID_RT_DRIVER_OPTION				0xFF818080
+#define OID_RT_RF_OFF					0xFF818081
+#define OID_RT_AUTH_STATUS				0xFF818082
+#define OID_RT_PRO_SET_CONTINUOUS_TX			0xFF81800B
+#define OID_RT_PRO_SET_SINGLE_CARRIER_TX		0xFF81800C
+#define OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX		0xFF81802B
+#define OID_RT_PRO_SET_SINGLE_TONE_TX			0xFF818043
+#define OID_RT_UTILITY_FALSE_ALARM_COUNTERS		0xFF818580
+#define OID_RT_UTILITY_SELECT_DEBUG_MODE		0xFF818581
+#define OID_RT_UTILITY_SELECT_SUBCARRIER_NUMBER		0xFF818582
+#define OID_RT_UTILITY_GET_RSSI_STATUS			0xFF818583
+#define OID_RT_UTILITY_GET_FRAME_DETECTION_STATUS	0xFF818584
+#define OID_RT_UTILITY_GET_AGC_AND_FREQUENCY_OFFSET_ESTIMATION_STATUS \
+							0xFF818585
+#define OID_RT_UTILITY_GET_CHANNEL_ESTIMATION_STATUS	0xFF818586
+#define OID_RT_WIRELESS_MODE				0xFF818500
+#define OID_RT_SUPPORTED_RATES				0xFF818501
+#define OID_RT_DESIRED_RATES				0xFF818502
+#define OID_RT_WIRELESS_MODE_STARTING_ADHOC		0xFF818503
+#define OID_RT_GET_CONNECT_STATE			0xFF030001
+#define OID_RT_RESCAN					0xFF030002
+#define OID_RT_SET_KEY_LENGTH				0xFF030003
+#define OID_RT_SET_DEFAULT_KEY_ID			0xFF030004
+#define OID_RT_SET_CHANNEL				0xFF010182
+#define OID_RT_SET_SNIFFER_MODE				0xFF010183
+#define OID_RT_GET_SIGNAL_QUALITY			0xFF010184
+#define OID_RT_GET_SMALL_PACKET_CRC			0xFF010185
+#define OID_RT_GET_MIDDLE_PACKET_CRC			0xFF010186
+#define OID_RT_GET_LARGE_PACKET_CRC			0xFF010187
+#define OID_RT_GET_TX_RETRY				0xFF010188
+#define OID_RT_GET_RX_RETRY				0xFF010189
+#define OID_RT_PRO_SET_FW_DIG_STATE			0xFF01018A
+#define OID_RT_PRO_SET_FW_RA_STATE			0xFF01018B
+#define OID_RT_GET_RX_TOTAL_PACKET			0xFF010190
+#define OID_RT_GET_TX_BEACON_OK				0xFF010191
+#define OID_RT_GET_TX_BEACON_ERR			0xFF010192
+#define OID_RT_GET_RX_ICV_ERR				0xFF010193
+#define OID_RT_SET_ENCRYPTION_ALGORITHM			0xFF010194
+#define OID_RT_SET_NO_AUTO_RESCAN			0xFF010195
+#define OID_RT_GET_PREAMBLE_MODE			0xFF010196
+#define OID_RT_GET_DRIVER_UP_DELTA_TIME			0xFF010197
+#define OID_RT_GET_AP_IP				0xFF010198
+#define OID_RT_GET_CHANNELPLAN				0xFF010199
+#define OID_RT_SET_PREAMBLE_MODE			0xFF01019A
+#define OID_RT_SET_BCN_INTVL				0xFF01019B
+#define OID_RT_GET_RF_VENDER				0xFF01019C
+#define OID_RT_DEDICATE_PROBE				0xFF01019D
+#define OID_RT_PRO_RX_FILTER_PATTERN			0xFF01019E
+#define OID_RT_GET_DCST_CURRENT_THRESHOLD		0xFF01019F
+#define OID_RT_GET_CCA_ERR				0xFF0101A0
+#define OID_RT_GET_CCA_UPGRADE_THRESHOLD		0xFF0101A1
+#define OID_RT_GET_CCA_FALLBACK_THRESHOLD		0xFF0101A2
+#define OID_RT_GET_CCA_UPGRADE_EVALUATE_TIMES		0xFF0101A3
+#define OID_RT_GET_CCA_FALLBACK_EVALUATE_TIMES		0xFF0101A4
+#define OID_RT_SET_RATE_ADAPTIVE			0xFF0101A5
+#define OID_RT_GET_DCST_EVALUATE_PERIOD			0xFF0101A5
+#define OID_RT_GET_DCST_TIME_UNIT_INDEX			0xFF0101A6
+#define OID_RT_GET_TOTAL_TX_BYTES			0xFF0101A7
+#define OID_RT_GET_TOTAL_RX_BYTES			0xFF0101A8
+#define OID_RT_CURRENT_TX_POWER_LEVEL			0xFF0101A9
+#define OID_RT_GET_ENC_KEY_MISMATCH_COUNT		0xFF0101AA
+#define OID_RT_GET_ENC_KEY_MATCH_COUNT			0xFF0101AB
+#define OID_RT_GET_CHANNEL				0xFF0101AC
+#define OID_RT_SET_CHANNELPLAN				0xFF0101AD
+#define OID_RT_GET_HARDWARE_RADIO_OFF			0xFF0101AE
+#define OID_RT_CHANNELPLAN_BY_COUNTRY			0xFF0101AF
+#define OID_RT_SCAN_AVAILABLE_BSSID			0xFF0101B0
+#define OID_RT_GET_HARDWARE_VERSION			0xFF0101B1
+#define OID_RT_GET_IS_ROAMING				0xFF0101B2
+#define OID_RT_GET_IS_PRIVACY				0xFF0101B3
+#define OID_RT_GET_KEY_MISMATCH				0xFF0101B4
+#define OID_RT_SET_RSSI_ROAM_TRAFFIC_TH			0xFF0101B5
+#define OID_RT_SET_RSSI_ROAM_SIGNAL_TH			0xFF0101B6
+#define OID_RT_RESET_LOG				0xFF0101B7
+#define OID_RT_GET_LOG					0xFF0101B8
+#define OID_RT_SET_INDICATE_HIDDEN_AP			0xFF0101B9
+#define OID_RT_GET_HEADER_FAIL				0xFF0101BA
+#define OID_RT_SUPPORTED_WIRELESS_MODE			0xFF0101BB
+#define OID_RT_GET_CHANNEL_LIST				0xFF0101BC
+#define OID_RT_GET_SCAN_IN_PROGRESS			0xFF0101BD
+#define OID_RT_GET_TX_INFO				0xFF0101BE
+#define OID_RT_RF_READ_WRITE_OFFSET			0xFF0101BF
+#define OID_RT_RF_READ_WRITE				0xFF0101C0
+#define OID_RT_FORCED_DATA_RATE				0xFF0101C1
+#define OID_RT_WIRELESS_MODE_FOR_SCAN_LIST		0xFF0101C2
+#define OID_RT_GET_BSS_WIRELESS_MODE			0xFF0101C3
+#define OID_RT_SCAN_WITH_MAGIC_PACKET			0xFF0101C4
+#define OID_RT_PRO_RX_FILTER				0xFF0111C0
+#define OID_CE_USB_WRITE_REGISTRY			0xFF0111C1
+#define OID_CE_USB_READ_REGISTRY			0xFF0111C2
+#define OID_RT_PRO_SET_INITIAL_GAIN			0xFF0111C3
+#define OID_RT_PRO_SET_BB_RF_STANDBY_MODE		0xFF0111C4
+#define OID_RT_PRO_SET_BB_RF_SHUTDOWN_MODE		0xFF0111C5
+#define OID_RT_PRO_SET_TX_CHARGE_PUMP			0xFF0111C6
+#define OID_RT_PRO_SET_RX_CHARGE_PUMP			0xFF0111C7
+#define OID_RT_PRO_RF_WRITE_REGISTRY			0xFF0111C8
+#define OID_RT_PRO_RF_READ_REGISTRY			0xFF0111C9
+#define OID_RT_PRO_QUERY_RF_TYPE			0xFF0111CA
+#define OID_RT_AP_GET_ASSOCIATED_STATION_LIST		0xFF010300
+#define OID_RT_AP_GET_CURRENT_TIME_STAMP		0xFF010301
+#define OID_RT_AP_SWITCH_INTO_AP_MODE			0xFF010302
+#define OID_RT_AP_SET_DTIM_PERIOD			0xFF010303
+#define OID_RT_AP_SUPPORTED				0xFF010304
+#define OID_RT_AP_SET_PASSPHRASE			0xFF010305
+#define OID_RT_PRO8187_WI_POLL				0xFF818780
+#define OID_RT_PRO_WRITE_BB_REG				0xFF818781
+#define OID_RT_PRO_READ_BB_REG				0xFF818782
+#define OID_RT_PRO_WRITE_RF_REG				0xFF818783
+#define OID_RT_PRO_READ_RF_REG				0xFF818784
+#define OID_RT_MH_VENDER_ID				0xFFEDC100
+#define OID_RT_PRO8711_JOIN_BSS				0xFF871100
+#define OID_RT_PRO_READ_REGISTER			0xFF871101
+#define OID_RT_PRO_WRITE_REGISTER			0xFF871102
+#define OID_RT_PRO_BURST_READ_REGISTER			0xFF871103
+#define OID_RT_PRO_BURST_WRITE_REGISTER			0xFF871104
+#define OID_RT_PRO_WRITE_TXCMD				0xFF871105
+#define OID_RT_PRO_READ16_EEPROM			0xFF871106
+#define OID_RT_PRO_WRITE16_EEPROM			0xFF871107
+#define OID_RT_PRO_H2C_SET_COMMAND			0xFF871108
+#define OID_RT_PRO_H2C_QUERY_RESULT			0xFF871109
+#define OID_RT_PRO8711_WI_POLL				0xFF87110A
+#define OID_RT_PRO8711_PKT_LOSS				0xFF87110B
+#define OID_RT_RD_ATTRIB_MEM				0xFF87110C
+#define OID_RT_WR_ATTRIB_MEM				0xFF87110D
+/*Method 2 for H2C/C2H*/
+#define OID_RT_PRO_H2C_CMD_MODE				0xFF871110
+#define OID_RT_PRO_H2C_CMD_RSP_MODE			0xFF871111
+#define OID_RT_PRO_H2C_CMD_EVENT_MODE			0xFF871112
+#define OID_RT_PRO_WAIT_C2H_EVENT			0xFF871113
+#define OID_RT_PRO_RW_ACCESS_PROTOCOL_TEST		0xFF871114
+#define OID_RT_PRO_SCSI_ACCESS_TEST			0xFF871115
+#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_OUT		0xFF871116
+#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_IN			0xFF871117
+#define OID_RT_RRO_RX_PKT_VIA_IOCTRL			0xFF871118
+#define OID_RT_RRO_RX_PKTARRAY_VIA_IOCTRL		0xFF871119
+#define OID_RT_RPO_SET_PWRMGT_TEST			0xFF87111A
+#define OID_RT_PRO_QRY_PWRMGT_TEST			0XFF87111B
+#define OID_RT_RPO_ASYNC_RWIO_TEST			0xFF87111C
+#define OID_RT_RPO_ASYNC_RWIO_POLL			0xFF87111D
+#define OID_RT_PRO_SET_RF_INTFS				0xFF87111E
+#define OID_RT_POLL_RX_STATUS				0xFF87111F
+#define OID_RT_PRO_CFG_DEBUG_MESSAGE			0xFF871120
+#define OID_RT_PRO_SET_DATA_RATE_EX			0xFF871121
+#define OID_RT_PRO_SET_BASIC_RATE			0xFF871122
+#define OID_RT_PRO_READ_TSSI				0xFF871123
+#define OID_RT_PRO_SET_POWER_TRACKING			0xFF871124
+#define OID_RT_PRO_QRY_PWRSTATE				0xFF871150
+#define OID_RT_PRO_SET_PWRSTATE				0xFF871151
+/*Method 2 , using workitem */
+#define OID_RT_SET_READ_REG				0xFF871181
+#define OID_RT_SET_WRITE_REG				0xFF871182
+#define OID_RT_SET_BURST_READ_REG			0xFF871183
+#define OID_RT_SET_BURST_WRITE_REG			0xFF871184
+#define OID_RT_SET_WRITE_TXCMD				0xFF871185
+#define OID_RT_SET_READ16_EEPROM			0xFF871186
+#define OID_RT_SET_WRITE16_EEPROM			0xFF871187
+#define OID_RT_QRY_POLL_WKITEM				0xFF871188
+
+/*For SDIO INTERFACE only*/
+#define OID_RT_PRO_SYNCPAGERW_SRAM			0xFF8711A0
+#define OID_RT_PRO_871X_DRV_EXT				0xFF8711A1
+
+/*For USB INTERFACE only*/
+#define OID_RT_PRO_USB_VENDOR_REQ			0xFF8711B0
+#define OID_RT_PRO_SCSI_AUTO_TEST			0xFF8711B1
+#define OID_RT_PRO_USB_MAC_AC_FIFO_WRITE		0xFF8711B2
+#define OID_RT_PRO_USB_MAC_RX_FIFO_READ			0xFF8711B3
+#define OID_RT_PRO_USB_MAC_RX_FIFO_POLLING		0xFF8711B4
+
+#define OID_RT_PRO_H2C_SET_RATE_TABLE			0xFF8711FB
+#define OID_RT_PRO_H2C_GET_RATE_TABLE			0xFF8711FC
+#define OID_RT_PRO_H2C_C2H_LBK_TEST			0xFF8711FE
+
+#define OID_RT_PRO_ENCRYPTION_CTRL			0xFF871200
+#define OID_RT_PRO_ADD_STA_INFO				0xFF871201
+#define OID_RT_PRO_DELE_STA_INFO			0xFF871202
+#define OID_RT_PRO_QUERY_DR_VARIABLE			0xFF871203
+
+#define OID_RT_PRO_RX_PACKET_TYPE			0xFF871204
+
+#define OID_RT_PRO_READ_EFUSE				0xFF871205
+#define OID_RT_PRO_WRITE_EFUSE				0xFF871206
+#define OID_RT_PRO_RW_EFUSE_PGPKT			0xFF871207
+#define OID_RT_GET_EFUSE_CURRENT_SIZE			0xFF871208
+
+#define OID_RT_SET_BANDWIDTH				0xFF871209
+#define OID_RT_SET_CRYSTAL_CAP				0xFF87120A
+
+#define OID_RT_SET_RX_PACKET_TYPE			0xFF87120B
+
+#define OID_RT_GET_EFUSE_MAX_SIZE			0xFF87120C
+
+#define OID_RT_PRO_SET_TX_AGC_OFFSET			0xFF87120D
+
+#define OID_RT_PRO_SET_PKT_TEST_MODE			0xFF87120E
+
+#define OID_RT_PRO_FOR_EVM_TEST_SETTING			0xFF87120F
+
+#define OID_RT_PRO_GET_THERMAL_METER			0xFF871210
+
+#define OID_RT_RESET_PHY_RX_PACKET_COUNT		0xFF871211
+#define OID_RT_GET_PHY_RX_PACKET_RECEIVED		0xFF871212
+#define OID_RT_GET_PHY_RX_PACKET_CRC32_ERROR		0xFF871213
+
+#define OID_RT_SET_POWER_DOWN				0xFF871214
+
+#define OID_RT_GET_POWER_MODE				0xFF871215
+
+#define OID_RT_PRO_EFUSE				0xFF871216
+#define OID_RT_PRO_EFUSE_MAP				0xFF871217
+
+#endif /*#ifndef __CUSTOM_OID_H */
+
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
new file mode 100644
index 000000000..3ea70d042
--- /dev/null
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -0,0 +1,465 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * os_intfs.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _OS_INTFS_C_
+
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/firmware.h>
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "xmit_osdep.h"
+#include "recv_osdep.h"
+#include "rtl871x_ioctl.h"
+#include "usb_osintf.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("rtl871x wireless lan driver");
+MODULE_AUTHOR("Larry Finger");
+
+static char ifname[IFNAMSIZ] = "wlan%d";
+
+/* module param defaults */
+static int chip_version = RTL8712_2ndCUT;
+static int rfintfs = HWPI;
+static int lbkmode = RTL8712_AIR_TRX;
+static int hci = RTL8712_USB;
+static int ampdu_enable = 1;/*for enable tx_ampdu*/
+
+/* The video_mode variable is for video mode.*/
+/* It may be specify when inserting module with video_mode=1 parameter.*/
+static int video_mode = 1;   /* enable video mode*/
+
+/*Ndis802_11Infrastructure; infra, ad-hoc, auto*/
+static int network_mode = Ndis802_11IBSS;
+static int channel = 1;/*ad-hoc support requirement*/
+static int wireless_mode = WIRELESS_11BG;
+static int vrtl_carrier_sense = AUTO_VCS;
+static int vcs_type = RTS_CTS;
+static int frag_thresh = 2346;
+static int preamble = PREAMBLE_LONG;/*long, short, auto*/
+static int scan_mode = 1;/*active, passive*/
+static int adhoc_tx_pwr = 1;
+static int soft_ap;
+static int smart_ps = 1;
+static int power_mgnt = PS_MODE_ACTIVE;
+static int radio_enable = 1;
+static int long_retry_lmt = 7;
+static int short_retry_lmt = 7;
+static int busy_thresh = 40;
+static int ack_policy = NORMAL_ACK;
+static int mp_mode;
+static int software_encrypt;
+static int software_decrypt;
+
+static int wmm_enable;/* default is set to disable the wmm.*/
+static int uapsd_enable;
+static int uapsd_max_sp = NO_LIMIT;
+static int uapsd_acbk_en;
+static int uapsd_acbe_en;
+static int uapsd_acvi_en;
+static int uapsd_acvo_en;
+
+static int ht_enable = 1;
+static int cbw40_enable = 1;
+static int rf_config = RTL8712_RF_1T2R;  /* 1T2R*/
+static int low_power;
+/* mac address to use instead of the one stored in Efuse */
+char *r8712_initmac;
+static char *initmac;
+/* if wifi_test = 1, driver will disable the turbo mode and pass it to
+ * firmware private.
+ */
+static int wifi_test;
+
+module_param_string(ifname, ifname, sizeof(ifname), 0644);
+module_param(wifi_test, int, 0644);
+module_param(initmac, charp, 0644);
+module_param(video_mode, int, 0644);
+module_param(chip_version, int, 0644);
+module_param(rfintfs, int, 0644);
+module_param(lbkmode, int, 0644);
+module_param(hci, int, 0644);
+module_param(network_mode, int, 0644);
+module_param(channel, int, 0644);
+module_param(mp_mode, int, 0644);
+module_param(wmm_enable, int, 0644);
+module_param(vrtl_carrier_sense, int, 0644);
+module_param(vcs_type, int, 0644);
+module_param(busy_thresh, int, 0644);
+module_param(ht_enable, int, 0644);
+module_param(cbw40_enable, int, 0644);
+module_param(ampdu_enable, int, 0644);
+module_param(rf_config, int, 0644);
+module_param(power_mgnt, int, 0644);
+module_param(low_power, int, 0644);
+
+MODULE_PARM_DESC(ifname, " Net interface name, wlan%d=default");
+MODULE_PARM_DESC(initmac, "MAC-Address, default: use FUSE");
+
+static int netdev_open(struct net_device *pnetdev);
+static int netdev_close(struct net_device *pnetdev);
+
+static void loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
+{
+	struct registry_priv  *registry_par = &padapter->registrypriv;
+
+	registry_par->chip_version = (u8)chip_version;
+	registry_par->rfintfs = (u8)rfintfs;
+	registry_par->lbkmode = (u8)lbkmode;
+	registry_par->hci = (u8)hci;
+	registry_par->network_mode  = (u8)network_mode;
+	memcpy(registry_par->ssid.Ssid, "ANY", 3);
+	registry_par->ssid.SsidLength = 3;
+	registry_par->channel = (u8)channel;
+	registry_par->wireless_mode = (u8)wireless_mode;
+	registry_par->vrtl_carrier_sense = (u8)vrtl_carrier_sense;
+	registry_par->vcs_type = (u8)vcs_type;
+	registry_par->frag_thresh = (u16)frag_thresh;
+	registry_par->preamble = (u8)preamble;
+	registry_par->scan_mode = (u8)scan_mode;
+	registry_par->adhoc_tx_pwr = (u8)adhoc_tx_pwr;
+	registry_par->soft_ap = (u8)soft_ap;
+	registry_par->smart_ps = (u8)smart_ps;
+	registry_par->power_mgnt = (u8)power_mgnt;
+	registry_par->radio_enable = (u8)radio_enable;
+	registry_par->long_retry_lmt = (u8)long_retry_lmt;
+	registry_par->short_retry_lmt = (u8)short_retry_lmt;
+	registry_par->busy_thresh = (u16)busy_thresh;
+	registry_par->ack_policy = (u8)ack_policy;
+	registry_par->mp_mode = (u8)mp_mode;
+	registry_par->software_encrypt = (u8)software_encrypt;
+	registry_par->software_decrypt = (u8)software_decrypt;
+	/*UAPSD*/
+	registry_par->wmm_enable = (u8)wmm_enable;
+	registry_par->uapsd_enable = (u8)uapsd_enable;
+	registry_par->uapsd_max_sp = (u8)uapsd_max_sp;
+	registry_par->uapsd_acbk_en = (u8)uapsd_acbk_en;
+	registry_par->uapsd_acbe_en = (u8)uapsd_acbe_en;
+	registry_par->uapsd_acvi_en = (u8)uapsd_acvi_en;
+	registry_par->uapsd_acvo_en = (u8)uapsd_acvo_en;
+	registry_par->ht_enable = (u8)ht_enable;
+	registry_par->cbw40_enable = (u8)cbw40_enable;
+	registry_par->ampdu_enable = (u8)ampdu_enable;
+	registry_par->rf_config = (u8)rf_config;
+	registry_par->low_power = (u8)low_power;
+	registry_par->wifi_test = (u8)wifi_test;
+	r8712_initmac = initmac;
+}
+
+static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
+{
+	struct _adapter *padapter = netdev_priv(pnetdev);
+	struct sockaddr *addr = p;
+
+	if (!padapter->bup)
+		eth_hw_addr_set(pnetdev, addr->sa_data);
+	return 0;
+}
+
+static struct net_device_stats *r871x_net_get_stats(struct net_device *pnetdev)
+{
+	struct _adapter *padapter = netdev_priv(pnetdev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	padapter->stats.tx_packets = pxmitpriv->tx_pkts;
+	padapter->stats.rx_packets = precvpriv->rx_pkts;
+	padapter->stats.tx_dropped = pxmitpriv->tx_drop;
+	padapter->stats.rx_dropped = precvpriv->rx_drop;
+	padapter->stats.tx_bytes = pxmitpriv->tx_bytes;
+	padapter->stats.rx_bytes = precvpriv->rx_bytes;
+	return &padapter->stats;
+}
+
+static const struct net_device_ops rtl8712_netdev_ops = {
+	.ndo_open = netdev_open,
+	.ndo_stop = netdev_close,
+	.ndo_start_xmit = r8712_xmit_entry,
+	.ndo_set_mac_address = r871x_net_set_mac_address,
+	.ndo_get_stats = r871x_net_get_stats,
+	.ndo_do_ioctl = r871x_ioctl,
+};
+
+struct net_device *r8712_init_netdev(void)
+{
+	struct _adapter *padapter;
+	struct net_device *pnetdev;
+
+	pnetdev = alloc_etherdev(sizeof(struct _adapter));
+	if (!pnetdev)
+		return NULL;
+	if (dev_alloc_name(pnetdev, ifname) < 0) {
+		strscpy(ifname, "wlan%d", sizeof(ifname));
+		dev_alloc_name(pnetdev, ifname);
+	}
+	padapter = netdev_priv(pnetdev);
+	padapter->pnetdev = pnetdev;
+	pr_info("r8712u: register rtl8712_netdev_ops to netdev_ops\n");
+	pnetdev->netdev_ops = &rtl8712_netdev_ops;
+	pnetdev->watchdog_timeo = HZ; /* 1 second timeout */
+	pnetdev->wireless_handlers = (struct iw_handler_def *)
+				     &r871x_handlers_def;
+	loadparam(padapter, pnetdev);
+	netif_carrier_off(pnetdev);
+	padapter->pid = 0;  /* Initial the PID value used for HW PBC.*/
+	return pnetdev;
+}
+
+static u32 start_drv_threads(struct _adapter *padapter)
+{
+	padapter->cmd_thread = kthread_run(r8712_cmd_thread, padapter, "%s",
+					  padapter->pnetdev->name);
+	if (IS_ERR(padapter->cmd_thread))
+		return _FAIL;
+	return _SUCCESS;
+}
+
+void r8712_stop_drv_threads(struct _adapter *padapter)
+{
+	struct completion *completion =
+		&padapter->cmdpriv.terminate_cmdthread_comp;
+
+	/*Below is to terminate r8712_cmd_thread & event_thread...*/
+	complete(&padapter->cmdpriv.cmd_queue_comp);
+	if (padapter->cmd_thread)
+		wait_for_completion_interruptible(completion);
+	padapter->cmdpriv.cmd_seq = 1;
+}
+
+static void start_drv_timers(struct _adapter *padapter)
+{
+	mod_timer(&padapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
+		  jiffies + msecs_to_jiffies(5000));
+	mod_timer(&padapter->mlmepriv.wdg_timer,
+		  jiffies + msecs_to_jiffies(2000));
+}
+
+void r8712_stop_drv_timers(struct _adapter *padapter)
+{
+	del_timer_sync(&padapter->mlmepriv.assoc_timer);
+	del_timer_sync(&padapter->securitypriv.tkip_timer);
+	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
+	del_timer_sync(&padapter->mlmepriv.dhcp_timer);
+	del_timer_sync(&padapter->mlmepriv.wdg_timer);
+	del_timer_sync(&padapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer);
+}
+
+static void init_default_value(struct _adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	/*xmit_priv*/
+	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
+	pxmitpriv->vcs = pregistrypriv->vcs_type;
+	pxmitpriv->vcs_type = pregistrypriv->vcs_type;
+	pxmitpriv->rts_thresh = pregistrypriv->rts_thresh;
+	pxmitpriv->frag_len = pregistrypriv->frag_thresh;
+	/* mlme_priv */
+	/* Maybe someday we should rename this variable to "active_mode"(Jeff)*/
+	pmlmepriv->passive_mode = 1; /* 1: active, 0: passive. */
+	/*ht_priv*/
+	{
+		int i;
+		struct ht_priv	 *phtpriv = &pmlmepriv->htpriv;
+
+		phtpriv->ampdu_enable = false;/*set to disabled*/
+		for (i = 0; i < 16; i++)
+			phtpriv->baddbareq_issued[i] = false;
+	}
+	/*security_priv*/
+	psecuritypriv->sw_encrypt = pregistrypriv->software_encrypt;
+	psecuritypriv->sw_decrypt = pregistrypriv->software_decrypt;
+	psecuritypriv->binstallGrpkey = _FAIL;
+	/*pwrctrl_priv*/
+	/*registry_priv*/
+	r8712_init_registrypriv_dev_network(padapter);
+	r8712_update_registrypriv_dev_network(padapter);
+	/*misc.*/
+}
+
+int r8712_init_drv_sw(struct _adapter *padapter)
+{
+	int ret;
+
+	ret = r8712_init_cmd_priv(&padapter->cmdpriv);
+	if (ret)
+		return ret;
+	padapter->cmdpriv.padapter = padapter;
+	ret = r8712_init_evt_priv(&padapter->evtpriv);
+	if (ret)
+		return ret;
+	ret = r8712_init_mlme_priv(padapter);
+	if (ret)
+		return ret;
+	_r8712_init_xmit_priv(&padapter->xmitpriv, padapter);
+	_r8712_init_recv_priv(&padapter->recvpriv, padapter);
+	memset((unsigned char *)&padapter->securitypriv, 0,
+	       sizeof(struct security_priv));
+	timer_setup(&padapter->securitypriv.tkip_timer,
+		    r8712_use_tkipkey_handler, 0);
+	ret = _r8712_init_sta_priv(&padapter->stapriv);
+	if (ret)
+		return ret;
+	padapter->stapriv.padapter = padapter;
+	r8712_init_bcmc_stainfo(padapter);
+	r8712_init_pwrctrl_priv(padapter);
+	mp871xinit(padapter);
+	init_default_value(padapter);
+	r8712_InitSwLeds(padapter);
+	mutex_init(&padapter->mutex_start);
+	return ret;
+}
+
+void r8712_free_drv_sw(struct _adapter *padapter)
+{
+	r8712_free_cmd_priv(&padapter->cmdpriv);
+	r8712_free_evt_priv(&padapter->evtpriv);
+	r8712_DeInitSwLeds(padapter);
+	r8712_free_mlme_priv(&padapter->mlmepriv);
+	_free_xmit_priv(&padapter->xmitpriv);
+	_r8712_free_sta_priv(&padapter->stapriv);
+	_r8712_free_recv_priv(&padapter->recvpriv);
+	mp871xdeinit(padapter);
+}
+
+static void enable_video_mode(struct _adapter *padapter, int cbw40_value)
+{
+	/*   bit 8:
+	 *   1 -> enable video mode to 96B AP
+	 *   0 -> disable video mode to 96B AP
+	 *   bit 9:
+	 *   1 -> enable 40MHz mode
+	 *   0 -> disable 40MHz mode
+	 *   bit 10:
+	 *   1 -> enable STBC
+	 *   0 -> disable STBC
+	 */
+	u32  intcmd = 0xf4000500;   /* enable bit8, bit10*/
+
+	if (cbw40_value) {
+		/* if the driver supports the 40M bandwidth,
+		 * we can enable the bit 9.
+		 */
+		intcmd |= 0x200;
+	}
+	r8712_fw_cmd(padapter, intcmd);
+}
+
+/*
+ *
+ * This function intends to handle the activation of an interface
+ * i.e. when it is brought Up/Active from a Down state.
+ *
+ */
+static int netdev_open(struct net_device *pnetdev)
+{
+	struct _adapter *padapter = netdev_priv(pnetdev);
+
+	mutex_lock(&padapter->mutex_start);
+	if (!padapter->bup) {
+		padapter->driver_stopped = false;
+		padapter->surprise_removed = false;
+		padapter->bup = true;
+		if (rtl871x_hal_init(padapter) != _SUCCESS)
+			goto netdev_open_error;
+		if (!r8712_initmac) {
+			/* Use the mac address stored in the Efuse */
+			eth_hw_addr_set(pnetdev,
+					padapter->eeprompriv.mac_addr);
+		} else {
+			/* We have to inform f/w to use user-supplied MAC
+			 * address.
+			 */
+			msleep(200);
+			r8712_setMacAddr_cmd(padapter,
+					     (const u8 *)pnetdev->dev_addr);
+			/*
+			 * The "myid" function will get the wifi mac address
+			 * from eeprompriv structure instead of netdev
+			 * structure. So, we have to overwrite the mac_addr
+			 * stored in the eeprompriv structure. In this case,
+			 * the real mac address won't be used anymore. So that,
+			 * the eeprompriv.mac_addr should store the mac which
+			 * users specify.
+			 */
+			memcpy(padapter->eeprompriv.mac_addr,
+			       pnetdev->dev_addr, ETH_ALEN);
+		}
+		if (start_drv_threads(padapter) != _SUCCESS)
+			goto netdev_open_error;
+		if (!padapter->dvobjpriv.inirp_init)
+			goto netdev_open_error;
+		else
+			padapter->dvobjpriv.inirp_init(padapter);
+		r8712_set_ps_mode(padapter, padapter->registrypriv.power_mgnt,
+				  padapter->registrypriv.smart_ps);
+	}
+	if (!netif_queue_stopped(pnetdev))
+		netif_start_queue(pnetdev);
+	else
+		netif_wake_queue(pnetdev);
+
+	if (video_mode)
+		enable_video_mode(padapter, cbw40_enable);
+	/* start driver mlme relation timer */
+	start_drv_timers(padapter);
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_NO_LINK);
+	mutex_unlock(&padapter->mutex_start);
+	return 0;
+netdev_open_error:
+	padapter->bup = false;
+	netif_carrier_off(pnetdev);
+	netif_stop_queue(pnetdev);
+	mutex_unlock(&padapter->mutex_start);
+	return -1;
+}
+
+/*
+ *
+ * This function intends to handle the shutdown of an interface
+ * i.e. when it is brought Down from an Up/Active state.
+ *
+ */
+static int netdev_close(struct net_device *pnetdev)
+{
+	struct _adapter *padapter = netdev_priv(pnetdev);
+
+	/* Close LED*/
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_POWER_OFF);
+	msleep(200);
+
+	/*s1.*/
+	if (pnetdev) {
+		if (!netif_queue_stopped(pnetdev))
+			netif_stop_queue(pnetdev);
+	}
+	/*s2.*/
+	/*s2-1.  issue disassoc_cmd to fw*/
+	r8712_disassoc_cmd(padapter);
+	/*s2-2.  indicate disconnect to os*/
+	r8712_ind_disconnect(padapter);
+	/*s2-3.*/
+	r8712_free_assoc_resources(padapter);
+	/*s2-4.*/
+	r8712_free_network_queue(padapter);
+	return 0;
+}
+
+#include "mlme_osdep.h"
diff --git a/drivers/staging/rtl8712/osdep_intf.h b/drivers/staging/rtl8712/osdep_intf.h
new file mode 100644
index 000000000..9e75116c9
--- /dev/null
+++ b/drivers/staging/rtl8712/osdep_intf.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __OSDEP_INTF_H_
+#define __OSDEP_INTF_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define RND4(x)	(((x >> 2) + ((x & 3) != 0)) << 2)
+
+struct intf_priv {
+	u8 *intf_dev;
+	/* when in USB, IO is through interrupt in/out endpoints */
+	struct usb_device *udev;
+	struct urb *piorw_urb;
+	struct completion io_retevt_comp;
+};
+
+int r871x_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+
+#endif	/*_OSDEP_INTF_H_*/
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
new file mode 100644
index 000000000..0d9bb42cb
--- /dev/null
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __OSDEP_SERVICE_H_
+#define __OSDEP_SERVICE_H_
+
+#define _SUCCESS	1
+#define _FAIL		0
+
+#include <linux/spinlock.h>
+
+#include <linux/interrupt.h>
+#include <linux/semaphore.h>
+#include <linux/sched/signal.h>
+#include <linux/sem.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <net/iw_handler.h>
+#include <linux/proc_fs.h>      /* Necessary because we use the proc fs */
+
+#include "basic_types.h"
+
+struct	__queue	{
+	struct	list_head	queue;
+	spinlock_t lock;
+};
+
+#define _pkt struct sk_buff
+#define _buffer unsigned char
+
+#define _init_queue(pqueue)				\
+	do {						\
+		INIT_LIST_HEAD(&((pqueue)->queue));	\
+		spin_lock_init(&((pqueue)->lock));	\
+	} while (0)
+
+static inline u32 end_of_queue_search(struct list_head *head,
+				      struct list_head *plist)
+{
+	return (head == plist);
+}
+
+static inline void flush_signals_thread(void)
+{
+	if (signal_pending(current))
+		flush_signals(current);
+}
+
+#endif
+
diff --git a/drivers/staging/rtl8712/recv_linux.c b/drivers/staging/rtl8712/recv_linux.c
new file mode 100644
index 000000000..215fca4ab
--- /dev/null
+++ b/drivers/staging/rtl8712/recv_linux.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * recv_linux.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RECV_OSDEP_C_
+
+#include <linux/usb.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wifi.h"
+#include "recv_osdep.h"
+#include "osdep_intf.h"
+#include "ethernet.h"
+#include <linux/if_arp.h>
+#include "usb_ops.h"
+
+/*init os related resource in struct recv_priv*/
+/*alloc os related resource in union recv_frame*/
+void r8712_os_recv_resource_alloc(struct _adapter *padapter,
+				  union recv_frame *precvframe)
+{
+	precvframe->u.hdr.pkt_newalloc = NULL;
+	precvframe->u.hdr.pkt = NULL;
+}
+
+/*alloc os related resource in struct recv_buf*/
+int r8712_os_recvbuf_resource_alloc(struct _adapter *padapter,
+				    struct recv_buf *precvbuf)
+{
+	int res = 0;
+
+	precvbuf->irp_pending = false;
+	precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!precvbuf->purb)
+		res = -ENOMEM;
+	precvbuf->pskb = NULL;
+	precvbuf->pallocated_buf = NULL;
+	precvbuf->pbuf = NULL;
+	precvbuf->pdata = NULL;
+	precvbuf->phead = NULL;
+	precvbuf->ptail = NULL;
+	precvbuf->pend = NULL;
+	precvbuf->transfer_len = 0;
+	precvbuf->len = 0;
+	return res;
+}
+
+/*free os related resource in struct recv_buf*/
+void r8712_os_recvbuf_resource_free(struct _adapter *padapter,
+				    struct recv_buf *precvbuf)
+{
+	if (precvbuf->pskb)
+		dev_kfree_skb_any(precvbuf->pskb);
+	if (precvbuf->purb) {
+		usb_kill_urb(precvbuf->purb);
+		usb_free_urb(precvbuf->purb);
+	}
+}
+
+void r8712_handle_tkip_mic_err(struct _adapter *adapter, u8 bgroup)
+{
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure ev;
+	struct mlme_priv *mlmepriv  = &adapter->mlmepriv;
+
+	memset(&ev, 0x00, sizeof(ev));
+	if (bgroup)
+		ev.flags |= IW_MICFAILURE_GROUP;
+	else
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	ether_addr_copy(ev.src_addr.sa_data, &mlmepriv->assoc_bssid[0]);
+	memset(&wrqu, 0x00, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+	wireless_send_event(adapter->pnetdev, IWEVMICHAELMICFAILURE, &wrqu,
+			    (char *)&ev);
+}
+
+void r8712_recv_indicatepkt(struct _adapter *adapter,
+			    union recv_frame *recvframe)
+{
+	struct recv_priv *recvpriv;
+	struct  __queue	*free_recv_queue;
+	_pkt *skb;
+	struct rx_pkt_attrib *attrib = &recvframe->u.hdr.attrib;
+
+	recvpriv = &adapter->recvpriv;
+	free_recv_queue = &recvpriv->free_recv_queue;
+	skb = recvframe->u.hdr.pkt;
+	if (!skb)
+		goto _recv_indicatepkt_drop;
+	skb->data = recvframe->u.hdr.rx_data;
+	skb->len = recvframe->u.hdr.len;
+	skb_set_tail_pointer(skb, skb->len);
+	if ((attrib->tcpchk_valid == 1) && (attrib->tcp_chkrpt == 1))
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	else
+		skb->ip_summed = CHECKSUM_NONE;
+	skb->dev = adapter->pnetdev;
+	skb->protocol = eth_type_trans(skb, adapter->pnetdev);
+	netif_rx(skb);
+	recvframe->u.hdr.pkt = NULL; /* pointers to NULL before
+				      * r8712_free_recvframe()
+				      */
+	r8712_free_recvframe(recvframe, free_recv_queue);
+	return;
+_recv_indicatepkt_drop:
+	 /*enqueue back to free_recv_queue*/
+	if (recvframe)
+		r8712_free_recvframe(recvframe, free_recv_queue);
+	recvpriv->rx_drop++;
+}
+
+static void _r8712_reordering_ctrl_timeout_handler (struct timer_list *t)
+{
+	struct recv_reorder_ctrl *reorder_ctrl =
+			 from_timer(reorder_ctrl, t, reordering_ctrl_timer);
+
+	r8712_reordering_ctrl_timeout_handler(reorder_ctrl);
+}
+
+void r8712_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
+{
+	timer_setup(&preorder_ctrl->reordering_ctrl_timer,
+		    _r8712_reordering_ctrl_timeout_handler, 0);
+}
diff --git a/drivers/staging/rtl8712/recv_osdep.h b/drivers/staging/rtl8712/recv_osdep.h
new file mode 100644
index 000000000..d8c1fa74f
--- /dev/null
+++ b/drivers/staging/rtl8712/recv_osdep.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RECV_OSDEP_H_
+#define __RECV_OSDEP_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include <linux/skbuff.h>
+
+void _r8712_init_recv_priv(struct recv_priv *precvpriv,
+			   struct _adapter *padapter);
+void _r8712_free_recv_priv(struct recv_priv *precvpriv);
+void r8712_recv_entry(union recv_frame *precv_frame);
+void r8712_recv_indicatepkt(struct _adapter *adapter,
+			    union recv_frame *precv_frame);
+void r8712_handle_tkip_mic_err(struct _adapter *padapter, u8 bgroup);
+void r8712_init_recv_priv(struct recv_priv *precvpriv,
+			  struct _adapter *padapter);
+void r8712_free_recv_priv(struct recv_priv *precvpriv);
+void r8712_os_recv_resource_alloc(struct _adapter *padapter,
+				  union recv_frame *precvframe);
+int r8712_os_recvbuf_resource_alloc(struct _adapter *padapter,
+				    struct recv_buf *precvbuf);
+void r8712_os_recvbuf_resource_free(struct _adapter *padapter,
+				    struct recv_buf *precvbuf);
+void r8712_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl);
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl8712_bitdef.h b/drivers/staging/rtl8712/rtl8712_bitdef.h
new file mode 100644
index 000000000..a4a687dcc
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_bitdef.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __RTL8712_BITDEF_H__
+#define __RTL8712_BITDEF_H__
+
+#include "rtl8712_cmdctrl_bitdef.h"
+#include "rtl8712_syscfg_bitdef.h"
+#include "rtl8712_macsetting_bitdef.h"
+#include "rtl8712_timectrl_bitdef.h"
+#include "rtl8712_fifoctrl_bitdef.h"
+#include "rtl8712_ratectrl_bitdef.h"
+#include "rtl8712_edcasetting_bitdef.h"
+#include "rtl8712_wmac_bitdef.h"
+#include "rtl8712_security_bitdef.h"
+#include "rtl8712_powersave_bitdef.h"
+#include "rtl8712_gp_bitdef.h"
+#include "rtl8712_interrupt_bitdef.h"
+#include "rtl8712_debugctrl_bitdef.h"
+
+#endif /* __RTL8712_BITDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
new file mode 100644
index 000000000..bb7db96ed
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -0,0 +1,409 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl8712_cmd.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL8712_CMD_C_
+
+#include <linux/compiler.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/sched/signal.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+#include <linux/circ_buf.h>
+#include <linux/uaccess.h>
+#include <asm/byteorder.h>
+#include <linux/atomic.h>
+#include <linux/semaphore.h>
+#include <linux/rtnetlink.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "mlme_osdep.h"
+#include "rtl871x_ioctl_set.h"
+
+static void check_hw_pbc(struct _adapter *padapter)
+{
+	u8	tmp1byte;
+
+	r8712_write8(padapter, MAC_PINMUX_CTRL, (GPIOMUX_EN | GPIOSEL_GPIO));
+	tmp1byte = r8712_read8(padapter, GPIO_IO_SEL);
+	tmp1byte &= ~(HAL_8192S_HW_GPIO_WPS_BIT);
+	r8712_write8(padapter, GPIO_IO_SEL, tmp1byte);
+	tmp1byte = r8712_read8(padapter, GPIO_CTRL);
+	if (tmp1byte == 0xff)
+		return;
+	if (tmp1byte & HAL_8192S_HW_GPIO_WPS_BIT) {
+		/* Here we only set bPbcPressed to true
+		 * After trigger PBC, the variable will be set to false
+		 */
+		netdev_dbg(padapter->pnetdev, "CheckPbcGPIO - PBC is pressed !!!!\n");
+		/* 0 is the default value and it means the application monitors
+		 * the HW PBC doesn't provide its pid to driver.
+		 */
+		if (padapter->pid == 0)
+			return;
+		kill_pid(find_vpid(padapter->pid), SIGUSR1, 1);
+	}
+}
+
+/* query rx phy status from fw.
+ * Adhoc mode: beacon.
+ * Infrastructure mode: beacon , data.
+ */
+static void query_fw_rx_phy_status(struct _adapter *padapter)
+{
+	u32 val32 = 0;
+	int pollingcnts = 50;
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		r8712_write32(padapter, IOCMD_CTRL_REG, 0xf4000001);
+		msleep(100);
+		/* Wait FW complete IO Cmd */
+		while ((r8712_read32(padapter, IOCMD_CTRL_REG)) &&
+		       (pollingcnts > 0)) {
+			pollingcnts--;
+			msleep(20);
+		}
+		if (pollingcnts != 0)
+			val32 = r8712_read32(padapter, IOCMD_DATA_REG);
+		else /* time out */
+			val32 = 0;
+		val32 >>= 4;
+		padapter->recvpriv.fw_rssi =
+			 (u8)r8712_signal_scale_mapping(val32);
+	}
+}
+
+/* check mlme, hw, phy, or dynamic algorithm status. */
+static void StatusWatchdogCallback(struct _adapter *padapter)
+{
+	check_hw_pbc(padapter);
+	query_fw_rx_phy_status(padapter);
+}
+
+static void r871x_internal_cmd_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	struct drvint_cmd_parm *pdrvcmd;
+
+	if (!pbuf)
+		return;
+	pdrvcmd = (struct drvint_cmd_parm *)pbuf;
+	switch (pdrvcmd->i_cid) {
+	case WDG_WK_CID:
+		StatusWatchdogCallback(padapter);
+		break;
+	default:
+		break;
+	}
+	kfree(pdrvcmd->pbuf);
+}
+
+static u8 read_bbreg_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
+
+	r8712_free_cmd_obj(pcmd);
+	return H2C_SUCCESS;
+}
+
+static u8 write_bbreg_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
+	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
+
+	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
+	if (!pcmd_callback)
+		r8712_free_cmd_obj(pcmd);
+	else
+		pcmd_callback(padapter, pcmd);
+	return H2C_SUCCESS;
+}
+
+static u8 read_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	u32 val;
+	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
+	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
+
+	if (pcmd->rsp && pcmd->rspsz > 0)
+		memcpy(pcmd->rsp, (u8 *)&val, pcmd->rspsz);
+	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
+	if (!pcmd_callback)
+		r8712_free_cmd_obj(pcmd);
+	else
+		pcmd_callback(padapter, pcmd);
+	return H2C_SUCCESS;
+}
+
+static u8 write_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
+	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
+
+	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
+	if (!pcmd_callback)
+		r8712_free_cmd_obj(pcmd);
+	else
+		pcmd_callback(padapter, pcmd);
+	return H2C_SUCCESS;
+}
+
+static u8 sys_suspend_hdl(struct _adapter *padapter, u8 *pbuf)
+{
+	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
+
+	r8712_free_cmd_obj(pcmd);
+	return H2C_SUCCESS;
+}
+
+static struct cmd_obj *cmd_hdl_filter(struct _adapter *padapter,
+				      struct cmd_obj *pcmd)
+{
+	struct cmd_obj *pcmd_r;
+
+	if (!pcmd)
+		return pcmd;
+	pcmd_r = NULL;
+
+	switch (pcmd->cmdcode) {
+	case GEN_CMD_CODE(_Read_BBREG):
+		read_bbreg_hdl(padapter, (u8 *)pcmd);
+		break;
+	case GEN_CMD_CODE(_Write_BBREG):
+		write_bbreg_hdl(padapter, (u8 *)pcmd);
+		break;
+	case GEN_CMD_CODE(_Read_RFREG):
+		read_rfreg_hdl(padapter, (u8 *)pcmd);
+		break;
+	case GEN_CMD_CODE(_Write_RFREG):
+		write_rfreg_hdl(padapter, (u8 *)pcmd);
+		break;
+	case GEN_CMD_CODE(_SetUsbSuspend):
+		sys_suspend_hdl(padapter, (u8 *)pcmd);
+		break;
+	case GEN_CMD_CODE(_JoinBss):
+		r8712_joinbss_reset(padapter);
+		/* Before set JoinBss_CMD to FW, driver must ensure FW is in
+		 * PS_MODE_ACTIVE. Directly write rpwm to radio on and assign
+		 * new pwr_mode to Driver, instead of use workitem to change
+		 * state.
+		 */
+		if (padapter->pwrctrlpriv.pwr_mode > PS_MODE_ACTIVE) {
+			padapter->pwrctrlpriv.pwr_mode = PS_MODE_ACTIVE;
+			mutex_lock(&padapter->pwrctrlpriv.mutex_lock);
+			r8712_set_rpwm(padapter, PS_STATE_S4);
+			mutex_unlock(&padapter->pwrctrlpriv.mutex_lock);
+		}
+		pcmd_r = pcmd;
+		break;
+	case _DRV_INT_CMD_:
+		r871x_internal_cmd_hdl(padapter, pcmd->parmbuf);
+		r8712_free_cmd_obj(pcmd);
+		pcmd_r = NULL;
+		break;
+	default:
+		pcmd_r = pcmd;
+		break;
+	}
+	return pcmd_r; /* if returning pcmd_r == NULL, pcmd must be free. */
+}
+
+u8 r8712_fw_cmd(struct _adapter *pAdapter, u32 cmd)
+{
+	int pollingcnts = 50;
+
+	r8712_write32(pAdapter, IOCMD_CTRL_REG, cmd);
+	msleep(100);
+	while ((r8712_read32(pAdapter, IOCMD_CTRL_REG != 0)) &&
+	       (pollingcnts > 0)) {
+		pollingcnts--;
+		msleep(20);
+	}
+	if (pollingcnts == 0)
+		return false;
+	return true;
+}
+
+void r8712_fw_cmd_data(struct _adapter *pAdapter, u32 *value, u8 flag)
+{
+	if (flag == 0)	/* set */
+		r8712_write32(pAdapter, IOCMD_DATA_REG, *value);
+	else		/* query */
+		*value = r8712_read32(pAdapter, IOCMD_DATA_REG);
+}
+
+int r8712_cmd_thread(void *context)
+{
+	struct cmd_obj *pcmd;
+	unsigned int cmdsz, wr_sz;
+	__le32 *pcmdbuf;
+	struct tx_desc *pdesc;
+	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
+	struct _adapter *padapter = context;
+	struct	cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct completion *cmd_queue_comp =
+		&pcmdpriv->cmd_queue_comp;
+	struct mutex *pwctrl_lock = &padapter->pwrctrlpriv.mutex_lock;
+
+	allow_signal(SIGTERM);
+	while (1) {
+		if (wait_for_completion_interruptible(cmd_queue_comp))
+			break;
+		if (padapter->driver_stopped || padapter->surprise_removed)
+			break;
+		if (r8712_register_cmd_alive(padapter))
+			continue;
+_next:
+		pcmd = r8712_dequeue_cmd(&pcmdpriv->cmd_queue);
+		if (!(pcmd)) {
+			r8712_unregister_cmd_alive(padapter);
+			continue;
+		}
+		pcmdbuf = (__le32 *)pcmdpriv->cmd_buf;
+		pdesc = (struct tx_desc *)pcmdbuf;
+		memset(pdesc, 0, TXDESC_SIZE);
+		pcmd = cmd_hdl_filter(padapter, pcmd);
+		if (pcmd) { /* if pcmd != NULL, cmd will be handled by f/w */
+			struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+			u8 blnPending = 0;
+			u16 cmdcode = pcmd->cmdcode;
+
+			pcmdpriv->cmd_issued_cnt++;
+			cmdsz = round_up(pcmd->cmdsz, 8);
+			wr_sz = TXDESC_SIZE + 8 + cmdsz;
+			pdesc->txdw0 |= cpu_to_le32((wr_sz - TXDESC_SIZE) &
+						     0x0000ffff);
+			if (pdvobj->ishighspeed) {
+				if ((wr_sz % 512) == 0)
+					blnPending = 1;
+			} else {
+				if ((wr_sz % 64) == 0)
+					blnPending = 1;
+			}
+			if (blnPending) { /* 32 bytes for TX Desc - 8 offset */
+				pdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE +
+						OFFSET_SZ + 8) << OFFSET_SHT) &
+						0x00ff0000);
+			} else {
+				pdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE +
+							      OFFSET_SZ) <<
+							      OFFSET_SHT) &
+							      0x00ff0000);
+			}
+			pdesc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+			pdesc->txdw1 |= cpu_to_le32((0x13 << QSEL_SHT) &
+						    0x00001f00);
+			pcmdbuf += (TXDESC_SIZE >> 2);
+			*pcmdbuf = cpu_to_le32((cmdsz & 0x0000ffff) |
+					       (pcmd->cmdcode << 16) |
+					       (pcmdpriv->cmd_seq << 24));
+			pcmdbuf += 2; /* 8 bytes alignment */
+			memcpy((u8 *)pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+			if (blnPending)
+				wr_sz += 8;   /* Append 8 bytes */
+			r8712_write_mem(padapter, RTL8712_DMA_H2CCMD, wr_sz,
+					(u8 *)pdesc);
+			pcmdpriv->cmd_seq++;
+			if (cmdcode == GEN_CMD_CODE(_CreateBss)) {
+				pcmd->res = H2C_SUCCESS;
+				pcmd_callback = cmd_callback[cmdcode].callback;
+				if (pcmd_callback)
+					pcmd_callback(padapter, pcmd);
+				continue;
+			}
+			if (cmdcode == GEN_CMD_CODE(_SetPwrMode)) {
+				if (padapter->pwrctrlpriv.bSleep) {
+					mutex_lock(pwctrl_lock);
+					r8712_set_rpwm(padapter, PS_STATE_S2);
+					mutex_unlock(pwctrl_lock);
+				}
+			}
+			r8712_free_cmd_obj(pcmd);
+			if (list_empty(&pcmdpriv->cmd_queue.queue)) {
+				r8712_unregister_cmd_alive(padapter);
+				continue;
+			} else {
+				goto _next;
+			}
+		} else {
+			goto _next;
+		}
+		flush_signals_thread();
+	}
+	/* free all cmd_obj resources */
+	do {
+		pcmd = r8712_dequeue_cmd(&pcmdpriv->cmd_queue);
+		if (!pcmd)
+			break;
+		r8712_free_cmd_obj(pcmd);
+	} while (1);
+	complete(&pcmdpriv->terminate_cmdthread_comp);
+	return 0;
+}
+
+void r8712_event_handle(struct _adapter *padapter, __le32 *peventbuf)
+{
+	u8 evt_code, evt_seq;
+	u16 evt_sz;
+	void (*event_callback)(struct _adapter *dev, u8 *pbuf);
+	struct	evt_priv *pevt_priv = &padapter->evtpriv;
+
+	if (!peventbuf)
+		goto _abort_event_;
+	evt_sz = (u16)(le32_to_cpu(*peventbuf) & 0xffff);
+	evt_seq = (u8)((le32_to_cpu(*peventbuf) >> 24) & 0x7f);
+	evt_code = (u8)((le32_to_cpu(*peventbuf) >> 16) & 0xff);
+	/* checking event sequence... */
+	if ((evt_seq & 0x7f) != pevt_priv->event_seq) {
+		pevt_priv->event_seq = ((evt_seq + 1) & 0x7f);
+		goto _abort_event_;
+	}
+	/* checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT) {
+		pevt_priv->event_seq = ((evt_seq + 1) & 0x7f);
+		goto _abort_event_;
+	} else if ((evt_code == GEN_EVT_CODE(_Survey)) &&
+		   (evt_sz > sizeof(struct wlan_bssid_ex))) {
+		pevt_priv->event_seq = ((evt_seq + 1) & 0x7f);
+		goto _abort_event_;
+	}
+	/* checking if event size match the event parm size */
+	if ((wlanevents[evt_code].parmsize) &&
+	    (wlanevents[evt_code].parmsize != evt_sz)) {
+		pevt_priv->event_seq = ((evt_seq + 1) & 0x7f);
+		goto _abort_event_;
+	} else if ((evt_sz == 0) && (evt_code != GEN_EVT_CODE(_WPS_PBC))) {
+		pevt_priv->event_seq = ((evt_seq + 1) & 0x7f);
+		goto _abort_event_;
+	}
+	pevt_priv->event_seq++;	/* update evt_seq */
+	if (pevt_priv->event_seq > 127)
+		pevt_priv->event_seq = 0;
+	/* move to event content, 8 bytes alignment */
+	peventbuf = peventbuf + 2;
+	event_callback = wlanevents[evt_code].event_callback;
+	if (event_callback)
+		event_callback(padapter, (u8 *)peventbuf);
+	pevt_priv->evt_done_cnt++;
+_abort_event_:
+	return;
+}
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.h b/drivers/staging/rtl8712/rtl8712_cmd.h
new file mode 100644
index 000000000..a34d0dd02
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_cmd.h
@@ -0,0 +1,231 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_CMD_H_
+#define __RTL8712_CMD_H_
+
+#define CMD_HDR_SZ      8
+
+u8 r8712_fw_cmd(struct _adapter *pAdapter, u32 cmd);
+void r8712_fw_cmd_data(struct _adapter *pAdapter, u32 *value, u8 flag);
+
+struct cmd_hdr {
+	u32 cmd_dw0;
+	u32 cmd_dw1;
+};
+
+enum rtl8712_h2c_cmd {
+	GEN_CMD_CODE(_Read_MACREG),	/*0*/
+	GEN_CMD_CODE(_Write_MACREG),
+	GEN_CMD_CODE(_Read_BBREG),
+	GEN_CMD_CODE(_Write_BBREG),
+	GEN_CMD_CODE(_Read_RFREG),
+	GEN_CMD_CODE(_Write_RFREG), /*5*/
+	GEN_CMD_CODE(_Read_EEPROM),
+	GEN_CMD_CODE(_Write_EEPROM),
+	GEN_CMD_CODE(_Read_EFUSE),
+	GEN_CMD_CODE(_Write_EFUSE),
+
+	GEN_CMD_CODE(_Read_CAM),	/*10*/
+	GEN_CMD_CODE(_Write_CAM),
+	GEN_CMD_CODE(_setBCNITV),
+	GEN_CMD_CODE(_setMBIDCFG),
+	GEN_CMD_CODE(_JoinBss),   /*14*/
+	GEN_CMD_CODE(_DisConnect), /*15*/
+	GEN_CMD_CODE(_CreateBss),
+	GEN_CMD_CODE(_SetOpMode),
+	GEN_CMD_CODE(_SiteSurvey),  /*18*/
+	GEN_CMD_CODE(_SetAuth),
+
+	GEN_CMD_CODE(_SetKey),	/*20*/
+	GEN_CMD_CODE(_SetStaKey),
+	GEN_CMD_CODE(_SetAssocSta),
+	GEN_CMD_CODE(_DelAssocSta),
+	GEN_CMD_CODE(_SetStaPwrState),
+	GEN_CMD_CODE(_SetBasicRate), /*25*/
+	GEN_CMD_CODE(_GetBasicRate),
+	GEN_CMD_CODE(_SetDataRate),
+	GEN_CMD_CODE(_GetDataRate),
+	GEN_CMD_CODE(_SetPhyInfo),
+
+	GEN_CMD_CODE(_GetPhyInfo),	/*30*/
+	GEN_CMD_CODE(_SetPhy),
+	GEN_CMD_CODE(_GetPhy),
+	GEN_CMD_CODE(_readRssi),
+	GEN_CMD_CODE(_readGain),
+	GEN_CMD_CODE(_SetAtim), /*35*/
+	GEN_CMD_CODE(_SetPwrMode),
+	GEN_CMD_CODE(_JoinbssRpt),
+	GEN_CMD_CODE(_SetRaTable),
+	GEN_CMD_CODE(_GetRaTable),
+
+	GEN_CMD_CODE(_GetCCXReport), /*40*/
+	GEN_CMD_CODE(_GetDTMReport),
+	GEN_CMD_CODE(_GetTXRateStatistics),
+	GEN_CMD_CODE(_SetUsbSuspend),
+	GEN_CMD_CODE(_SetH2cLbk),
+	GEN_CMD_CODE(_AddBAReq), /*45*/
+
+	GEN_CMD_CODE(_SetChannel), /*46*/
+/* MP_OFFLOAD Start (47~54)*/
+	GEN_CMD_CODE(_SetTxPower),
+	GEN_CMD_CODE(_SwitchAntenna),
+	GEN_CMD_CODE(_SetCrystalCap),
+	GEN_CMD_CODE(_SetSingleCarrierTx), /*50*/
+	GEN_CMD_CODE(_SetSingleToneTx),
+	GEN_CMD_CODE(_SetCarrierSuppressionTx),
+	GEN_CMD_CODE(_SetContinuousTx),
+	GEN_CMD_CODE(_SwitchBandwidth), /*54*/
+/* MP_OFFLOAD End*/
+	GEN_CMD_CODE(_TX_Beacon), /*55*/
+	GEN_CMD_CODE(_SetPowerTracking),
+	GEN_CMD_CODE(_AMSDU_TO_AMPDU), /*57*/
+	GEN_CMD_CODE(_SetMacAddress), /*58*/
+
+	GEN_CMD_CODE(_DisconnectCtrl), /*59*/
+	GEN_CMD_CODE(_SetChannelPlan), /*60*/
+	GEN_CMD_CODE(_DisconnectCtrlEx), /*61*/
+
+	/* To do, modify these h2c cmd, add or delete */
+	GEN_CMD_CODE(_GetH2cLbk),
+
+	/* WPS extra IE */
+	GEN_CMD_CODE(_SetProbeReqExtraIE),
+	GEN_CMD_CODE(_SetAssocReqExtraIE),
+	GEN_CMD_CODE(_SetProbeRspExtraIE),
+	GEN_CMD_CODE(_SetAssocRspExtraIE),
+
+	/* the following is driver will do */
+	GEN_CMD_CODE(_GetCurDataRate),
+
+	GEN_CMD_CODE(_GetTxRetrycnt),  /* to record times that Tx retry to
+					* transmit packet after association
+					*/
+	GEN_CMD_CODE(_GetRxRetrycnt),  /* to record total number of the
+					* received frame with ReTry bit set in
+					* the WLAN header
+					*/
+
+	GEN_CMD_CODE(_GetBCNOKcnt),
+	GEN_CMD_CODE(_GetBCNERRcnt),
+	GEN_CMD_CODE(_GetCurTxPwrLevel),
+
+	GEN_CMD_CODE(_SetDIG),
+	GEN_CMD_CODE(_SetRA),
+	GEN_CMD_CODE(_SetPT),
+	GEN_CMD_CODE(_ReadTSSI),
+
+	MAX_H2CCMD
+};
+
+#define _GetBBReg_CMD_		_Read_BBREG_CMD_
+#define _SetBBReg_CMD_		_Write_BBREG_CMD_
+#define _GetRFReg_CMD_		_Read_RFREG_CMD_
+#define _SetRFReg_CMD_		_Write_RFREG_CMD_
+#define _DRV_INT_CMD_		(MAX_H2CCMD + 1)
+#define _SetRFIntFs_CMD_	(MAX_H2CCMD + 2)
+
+#ifdef _RTL8712_CMD_C_
+static struct _cmd_callback	cmd_callback[] = {
+	{GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/
+	{GEN_CMD_CODE(_Write_MACREG), NULL},
+	{GEN_CMD_CODE(_Read_BBREG), NULL},
+	{GEN_CMD_CODE(_Write_BBREG), NULL},
+	{GEN_CMD_CODE(_Read_RFREG), &r8712_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/
+	{GEN_CMD_CODE(_Read_EEPROM), NULL},
+	{GEN_CMD_CODE(_Write_EEPROM), NULL},
+	{GEN_CMD_CODE(_Read_EFUSE), NULL},
+	{GEN_CMD_CODE(_Write_EFUSE), NULL},
+
+	{GEN_CMD_CODE(_Read_CAM),	NULL},	/*10*/
+	{GEN_CMD_CODE(_Write_CAM),	 NULL},
+	{GEN_CMD_CODE(_setBCNITV), NULL},
+	{GEN_CMD_CODE(_setMBIDCFG), NULL},
+	{GEN_CMD_CODE(_JoinBss), &r8712_joinbss_cmd_callback},  /*14*/
+	{GEN_CMD_CODE(_DisConnect), &r8712_disassoc_cmd_callback}, /*15*/
+	{GEN_CMD_CODE(_CreateBss), &r8712_createbss_cmd_callback},
+	{GEN_CMD_CODE(_SetOpMode), NULL},
+	{GEN_CMD_CODE(_SiteSurvey), &r8712_survey_cmd_callback}, /*18*/
+	{GEN_CMD_CODE(_SetAuth), NULL},
+
+	{GEN_CMD_CODE(_SetKey), NULL},	/*20*/
+	{GEN_CMD_CODE(_SetStaKey), &r8712_setstaKey_cmdrsp_callback},
+	{GEN_CMD_CODE(_SetAssocSta), &r8712_setassocsta_cmdrsp_callback},
+	{GEN_CMD_CODE(_DelAssocSta), NULL},
+	{GEN_CMD_CODE(_SetStaPwrState), NULL},
+	{GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/
+	{GEN_CMD_CODE(_GetBasicRate), NULL},
+	{GEN_CMD_CODE(_SetDataRate), NULL},
+	{GEN_CMD_CODE(_GetDataRate), NULL},
+	{GEN_CMD_CODE(_SetPhyInfo), NULL},
+
+	{GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/
+	{GEN_CMD_CODE(_SetPhy), NULL},
+	{GEN_CMD_CODE(_GetPhy), NULL},
+	{GEN_CMD_CODE(_readRssi), NULL},
+	{GEN_CMD_CODE(_readGain), NULL},
+	{GEN_CMD_CODE(_SetAtim), NULL}, /*35*/
+	{GEN_CMD_CODE(_SetPwrMode), NULL},
+	{GEN_CMD_CODE(_JoinbssRpt), NULL},
+	{GEN_CMD_CODE(_SetRaTable), NULL},
+	{GEN_CMD_CODE(_GetRaTable), NULL},
+
+	{GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/
+	{GEN_CMD_CODE(_GetDTMReport),	NULL},
+	{GEN_CMD_CODE(_GetTXRateStatistics), NULL},
+	{GEN_CMD_CODE(_SetUsbSuspend), NULL},
+	{GEN_CMD_CODE(_SetH2cLbk), NULL},
+	{GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/
+
+	{GEN_CMD_CODE(_SetChannel), NULL},		/*46*/
+/* MP_OFFLOAD Start (47~54)*/
+	{GEN_CMD_CODE(_SetTxPower), NULL},
+	{GEN_CMD_CODE(_SwitchAntenna), NULL},
+	{GEN_CMD_CODE(_SetCrystalCap), NULL},
+	{GEN_CMD_CODE(_SetSingleCarrierTx), NULL},	/*50*/
+	{GEN_CMD_CODE(_SetSingleToneTx), NULL},
+	{GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL},
+	{GEN_CMD_CODE(_SetContinuousTx), NULL},
+	{GEN_CMD_CODE(_SwitchBandwidth), NULL},		/*54*/
+/* MP_OFFLOAD End*/
+	{GEN_CMD_CODE(_TX_Beacon), NULL}, /*55*/
+	{GEN_CMD_CODE(_SetPowerTracking), NULL},
+	{GEN_CMD_CODE(_AMSDU_TO_AMPDU), NULL}, /*57*/
+	{GEN_CMD_CODE(_SetMacAddress), NULL}, /*58*/
+
+	{GEN_CMD_CODE(_DisconnectCtrl), NULL}, /*59*/
+	{GEN_CMD_CODE(_SetChannelPlan), NULL}, /*60*/
+	{GEN_CMD_CODE(_DisconnectCtrlEx), NULL}, /*61*/
+
+	/* To do, modify these h2c cmd, add or delete */
+	{GEN_CMD_CODE(_GetH2cLbk), NULL},
+
+	{_SetProbeReqExtraIE_CMD_, NULL},
+	{_SetAssocReqExtraIE_CMD_, NULL},
+	{_SetProbeRspExtraIE_CMD_, NULL},
+	{_SetAssocRspExtraIE_CMD_, NULL},
+	{_GetCurDataRate_CMD_, NULL},
+	{_GetTxRetrycnt_CMD_, NULL},
+	{_GetRxRetrycnt_CMD_, NULL},
+	{_GetBCNOKcnt_CMD_, NULL},
+	{_GetBCNERRcnt_CMD_, NULL},
+	{_GetCurTxPwrLevel_CMD_, NULL},
+	{_SetDIG_CMD_, NULL},
+	{_SetRA_CMD_, NULL},
+	{_SetPT_CMD_, NULL},
+	{GEN_CMD_CODE(_ReadTSSI), &r8712_readtssi_cmdrsp_callback}
+};
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl8712_cmdctrl_bitdef.h b/drivers/staging/rtl8712/rtl8712_cmdctrl_bitdef.h
new file mode 100644
index 000000000..68bdec07f
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_cmdctrl_bitdef.h
@@ -0,0 +1,95 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_CMDCTRL_BITDEF_H__
+#define __RTL8712_CMDCTRL_BITDEF_H__
+
+/*
+ * 2. Command Control Registers	 (Offset: 0x0040 - 0x004F)
+ */
+/*--------------------------------------------------------------------------*/
+/*       8192S (CMD) command register bits	(Offset 0x40, 16 bits)*/
+/*--------------------------------------------------------------------------*/
+#define		_APSDOFF_STATUS		BIT(15)
+#define		_APSDOFF		BIT(14)
+#define		_BBRSTn			BIT(13)  /*Enable OFDM/CCK*/
+#define		_BB_GLB_RSTn		BIT(12)   /*Enable BB*/
+#define		_SCHEDULE_EN		BIT(10)  /*Enable MAC scheduler*/
+#define		_MACRXEN		BIT(9)
+#define		_MACTXEN		BIT(8)
+#define		_DDMA_EN		BIT(7)  /*FW off load function enable*/
+#define		_FW2HW_EN		BIT(6)  /*MAC every module reset */
+#define		_RXDMA_EN		BIT(5)
+#define		_TXDMA_EN		BIT(4)
+#define		_HCI_RXDMA_EN		BIT(3)
+#define		_HCI_TXDMA_EN		BIT(2)
+
+/*TXPAUSE*/
+#define	_STOPHCCA			BIT(6)
+#define	_STOPHIGH			BIT(5)
+#define	_STOPMGT			BIT(4)
+#define	_STOPVO				BIT(3)
+#define	_STOPVI				BIT(2)
+#define	_STOPBE				BIT(1)
+#define	_STOPBK				BIT(0)
+
+/*TCR*/
+#define	_DISCW				BIT(20)
+#define	_ICV				BIT(19)
+#define	_CFEND_FMT			BIT(17)
+#define	_CRC				BIT(16)
+#define	_FWRDY				BIT(7)
+#define _BASECHG			BIT(6)
+#define	_IMEM_RDY			BIT(5)
+#define _DMEM_CODE_DONE			BIT(4)
+#define _EMEM_CHK_RPT			BIT(3)
+#define _EMEM_CODE_DONE			BIT(2)
+#define _IMEM_CHK_RPT			BIT(1)
+#define _IMEM_CODE_DONE			BIT(0)
+
+#define	_TXDMA_INIT_VALUE	(_IMEM_CHK_RPT | _EMEM_CHK_RPT)
+
+/*RCR*/
+#define	_ENMBID				BIT(27)
+#define	_APP_PHYST_RXFF			BIT(25)
+#define	_APP_PHYST_STAFF		BIT(24)
+#define	_CBSSID				BIT(23)
+#define	_APWRMGT			BIT(22)
+#define	_ADD3				BIT(21)
+#define	_AMF				BIT(20)
+#define	_ACF				BIT(19)
+#define	_ADF				BIT(18)
+#define	_APP_MIC			BIT(17)
+#define	_APP_ICV			BIT(16)
+#define	_RXFTH_MSK			0x0000E000
+#define	_RXFTH_SHT			13
+#define	_AICV				BIT(12)
+#define	_RXPKTLMT_MSK			0x00000FC0
+#define	_RXPKTLMT_SHT			6
+#define	_ACRC32				BIT(5)
+#define	_AB				BIT(3)
+#define	_AM				BIT(2)
+#define	_APM				BIT(1)
+#define	_AAP				BIT(0)
+
+/*MSR*/
+#define	_NETTYPE_MSK			0x03
+#define	_NETTYPE_SHT			0
+
+/*BT*/
+#define _BTMODE_MSK			0x06
+#define _BTMODE_SHT			1
+#define _ENBT				BIT(0)
+
+/*MBIDCTRL*/
+#define	_ENMBID_MODE			BIT(15)
+#define	_BCNNO_MSK			0x7000
+#define	_BCNNO_SHT			12
+#define	_BCNSPACE_MSK			0x0FFF
+#define	_BCNSPACE_SHT			0
+
+#endif /* __RTL8712_CMDCTRL_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_cmdctrl_regdef.h b/drivers/staging/rtl8712/rtl8712_cmdctrl_regdef.h
new file mode 100644
index 000000000..fc67771c8
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_cmdctrl_regdef.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_CMDCTRL_REGDEF_H__
+#define __RTL8712_CMDCTRL_REGDEF_H__
+
+#define CR			(RTL8712_CMDCTRL_ + 0x0000)
+#define TXPAUSE			(RTL8712_CMDCTRL_ + 0x0002)
+#define TCR			(RTL8712_CMDCTRL_ + 0x0004)
+#define RCR			(RTL8712_CMDCTRL_ + 0x0008)
+#define MSR			(RTL8712_CMDCTRL_ + 0x000C)
+#define SYSF_CFG		(RTL8712_CMDCTRL_ + 0x000D)
+#define MBIDCTRL		(RTL8712_CMDCTRL_ + 0x000E)
+
+#endif /* __RTL8712_CMDCTRL_REGDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_debugctrl_bitdef.h b/drivers/staging/rtl8712/rtl8712_debugctrl_bitdef.h
new file mode 100644
index 000000000..bb3863467
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_debugctrl_bitdef.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_DEBUGCTRL_BITDEF_H__
+#define __RTL8712_DEBUGCTRL_BITDEF_H__
+
+/*BIST*/
+#define	_BIST_RST			BIT(0)
+
+/*LMS*/
+#define	_LMS_MSK			0x03
+
+/*WDG_CTRL*/
+#define	_OVSEL_MSK			0x0600
+#define	_OVSEL_SHT			9
+#define	_WDGCLR				BIT(8)
+#define	_WDGEN_MSK			0x00FF
+#define	_WDGEN_SHT			0
+
+/*INTM*/
+#define	_TXTIMER_MSK		0xF000
+#define	_TXTIMER_SHT		12
+#define	_TXNUM_MSK			0x0F00
+#define	_TXNUM_SHT			8
+#define	_RXTIMER_MSK		0x00F0
+#define	_RXTIMER_SHT		4
+#define	_RXNUM_MSK			0x000F
+#define	_RXNUM_SHT			0
+
+/*FDLOCKTURN0*/
+/*FDLOCKTURN1*/
+#define	_TURN1				BIT(0)
+
+/*FDLOCKFLAG0*/
+/*FDLOCKFLAG1*/
+#define	_LOCKFLAG1_MSK		0x03
+
+#endif /* __RTL8712_DEBUGCTRL_BITDEF_H__ */
diff --git a/drivers/staging/rtl8712/rtl8712_debugctrl_regdef.h b/drivers/staging/rtl8712/rtl8712_debugctrl_regdef.h
new file mode 100644
index 000000000..319220e9d
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_debugctrl_regdef.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_DEBUGCTRL_REGDEF_H__
+#define __RTL8712_DEBUGCTRL_REGDEF_H__
+
+#define BIST			(RTL8712_DEBUGCTRL_ + 0x00)
+#define DBS			(RTL8712_DEBUGCTRL_ + 0x04)
+#define LMS			(RTL8712_DEBUGCTRL_ + 0x05)
+#define CPUINST			(RTL8712_DEBUGCTRL_ + 0x08)
+#define CPUCAUSE		(RTL8712_DEBUGCTRL_ + 0x0C)
+#define LBUS_ERR_ADDR		(RTL8712_DEBUGCTRL_ + 0x10)
+#define LBUS_ERR_CMD		(RTL8712_DEBUGCTRL_ + 0x14)
+#define LBUS_ERR_DATA_L		(RTL8712_DEBUGCTRL_ + 0x18)
+#define LBUS_ERR_DATA_H		(RTL8712_DEBUGCTRL_ + 0x1C)
+#define LBUS_EXCEPTION_ADDR	(RTL8712_DEBUGCTRL_ + 0x20)
+#define WDG_CTRL		(RTL8712_DEBUGCTRL_ + 0x24)
+#define INTMTU			(RTL8712_DEBUGCTRL_ + 0x28)
+#define INTM			(RTL8712_DEBUGCTRL_ + 0x2A)
+#define FDLOCKTURN0		(RTL8712_DEBUGCTRL_ + 0x2C)
+#define FDLOCKTURN1		(RTL8712_DEBUGCTRL_ + 0x2D)
+#define FDLOCKFLAG0		(RTL8712_DEBUGCTRL_ + 0x2E)
+#define FDLOCKFLAG1		(RTL8712_DEBUGCTRL_ + 0x2F)
+#define TRXPKTBUF_DBG_DATA	(RTL8712_DEBUGCTRL_ + 0x30)
+#define TRXPKTBUF_DBG_CTRL	(RTL8712_DEBUGCTRL_ + 0x38)
+#define DPLL_MON		(RTL8712_DEBUGCTRL_ + 0x3A)
+
+#endif /* __RTL8712_DEBUGCTRL_REGDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_edcasetting_bitdef.h b/drivers/staging/rtl8712/rtl8712_edcasetting_bitdef.h
new file mode 100644
index 000000000..9048d6a65
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_edcasetting_bitdef.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_EDCASETTING_BITDEF_H__
+#define __RTL8712_EDCASETTING_BITDEF_H__
+
+/*EDCAPARAM*/
+#define	_TXOPLIMIT_MSK		0xFFFF0000
+#define	_TXOPLIMIT_SHT		16
+#define	_ECWIN_MSK		0x0000FF00
+#define	_ECWIN_SHT		8
+#define	_AIFS_MSK		0x000000FF
+#define	_AIFS_SHT		0
+
+/*BCNTCFG*/
+#define	_BCNECW_MSK		0xFF00
+#define	_BCNECW_SHT		8
+#define	_BCNIFS_MSK		0x00FF
+#define	_BCNIFS_SHT		0
+
+/*CWRR*/
+#define	_CWRR_MSK		0x03FF
+
+/*ACMAVG*/
+#define	_AVG_TIME_UP		BIT(3)
+#define	_AVGPERIOD_MSK		0x03
+
+/*ACMHWCTRL*/
+#define	_VOQ_ACM_STATUS		BIT(6)
+#define	_VIQ_ACM_STATUS		BIT(5)
+#define	_BEQ_ACM_STATUS		BIT(4)
+#define	_VOQ_ACM_EN		BIT(3)
+#define	_VIQ_ACM_EN		BIT(2)
+#define	_BEQ_ACM_EN		BIT(1)
+#define	_ACMHWEN		BIT(0)
+
+/*VO_ADMTIME*/
+#define	_VO_ACM_RUT		BIT(18)
+#define	_VO_ADMTIME_MSK		0x0003FFF
+
+/*VI_ADMTIME*/
+#define	_VI_ACM_RUT		BIT(18)
+#define	_VI_ADMTIME_MSK		0x0003FFF
+
+/*BE_ADMTIME*/
+#define	_BE_ACM_RUT		BIT(18)
+#define	_BE_ADMTIME_MSK		0x0003FFF
+
+/*Retry limit reg*/
+#define	_SRL_MSK		0xFF00
+#define	_SRL_SHT		8
+#define	_LRL_MSK		0x00FF
+#define	_LRL_SHT		0
+
+#endif /* __RTL8712_EDCASETTING_BITDEF_H__*/
diff --git a/drivers/staging/rtl8712/rtl8712_edcasetting_regdef.h b/drivers/staging/rtl8712/rtl8712_edcasetting_regdef.h
new file mode 100644
index 000000000..02ec9f3bb
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_edcasetting_regdef.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_EDCASETTING_REGDEF_H__
+#define __RTL8712_EDCASETTING_REGDEF_H__
+
+#define EDCA_VO_PARAM		(RTL8712_EDCASETTING_ + 0x00)
+#define EDCA_VI_PARAM		(RTL8712_EDCASETTING_ + 0x04)
+#define EDCA_BE_PARAM		(RTL8712_EDCASETTING_ + 0x08)
+#define EDCA_BK_PARAM		(RTL8712_EDCASETTING_ + 0x0C)
+#define BCNTCFG			(RTL8712_EDCASETTING_ + 0x10)
+#define CWRR			(RTL8712_EDCASETTING_ + 0x12)
+#define ACMAVG			(RTL8712_EDCASETTING_ + 0x16)
+#define ACMHWCTRL		(RTL8712_EDCASETTING_ + 0x17)
+#define VO_ADMTIME		(RTL8712_EDCASETTING_ + 0x18)
+#define VI_ADMTIME		(RTL8712_EDCASETTING_ + 0x1C)
+#define BE_ADMTIME		(RTL8712_EDCASETTING_ + 0x20)
+#define RL			(RTL8712_EDCASETTING_ + 0x24)
+
+#endif /* __RTL8712_EDCASETTING_REGDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_efuse.c b/drivers/staging/rtl8712/rtl8712_efuse.c
new file mode 100644
index 000000000..c9400e40a
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_efuse.c
@@ -0,0 +1,564 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * rtl8712_efuse.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL8712_EFUSE_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "rtl8712_efuse.h"
+
+/* reserve 3 bytes for HW stop read */
+static int efuse_available_max_size = EFUSE_MAX_SIZE - 3 /*0x1FD*/;
+
+static void efuse_reg_ctrl(struct _adapter *adapter, u8 bPowerOn)
+{
+	u8 tmpu8 = 0;
+
+	if (bPowerOn) {
+		/* -----------------e-fuse pwr & clk reg ctrl ---------------
+		 * Enable LDOE25 Macro Block
+		 */
+		tmpu8 = r8712_read8(adapter, EFUSE_TEST + 3);
+		tmpu8 |= 0x80;
+		r8712_write8(adapter, EFUSE_TEST + 3, tmpu8);
+		msleep(20); /* for some platform , need some delay time */
+		/* Change Efuse Clock for write action to 40MHZ */
+		r8712_write8(adapter, EFUSE_CLK_CTRL, 0x03);
+		msleep(20); /* for some platform , need some delay time */
+	} else {
+		/* -----------------e-fuse pwr & clk reg ctrl -----------------
+		 * Disable LDOE25 Macro Block
+		 */
+		tmpu8 = r8712_read8(adapter, EFUSE_TEST + 3);
+		tmpu8 &= 0x7F;
+		r8712_write8(adapter, EFUSE_TEST + 3, tmpu8);
+		/* Change Efuse Clock for write action to 500K */
+		r8712_write8(adapter, EFUSE_CLK_CTRL, 0x02);
+	}
+}
+
+/*
+ * Before write E-Fuse, this function must be called.
+ */
+u8 r8712_efuse_reg_init(struct _adapter *adapter)
+{
+	return true;
+}
+
+void r8712_efuse_reg_uninit(struct _adapter *adapter)
+{
+	efuse_reg_ctrl(adapter, false);
+}
+
+static u8 efuse_one_byte_read(struct _adapter *adapter, u16 addr, u8 *data)
+{
+	u8 tmpidx = 0, bResult;
+
+	/* -----------------e-fuse reg ctrl --------------------------------- */
+	r8712_write8(adapter, EFUSE_CTRL + 1, (u8)(addr & 0xFF)); /* address */
+	r8712_write8(adapter, EFUSE_CTRL + 2, ((u8)((addr >> 8) & 0x03)) |
+	       (r8712_read8(adapter, EFUSE_CTRL + 2) & 0xFC));
+	r8712_write8(adapter, EFUSE_CTRL + 3, 0x72); /* read cmd */
+	/* wait for complete */
+	while (!(0x80 & r8712_read8(adapter, EFUSE_CTRL + 3)) &&
+	       (tmpidx < 100))
+		tmpidx++;
+	if (tmpidx < 100) {
+		*data = r8712_read8(adapter, EFUSE_CTRL);
+		bResult = true;
+	} else {
+		*data = 0xff;
+		bResult = false;
+	}
+	return bResult;
+}
+
+static u8 efuse_one_byte_write(struct _adapter *adapter, u16 addr, u8 data)
+{
+	u8 tmpidx = 0, bResult;
+
+	/* -----------------e-fuse reg ctrl -------------------------------- */
+	r8712_write8(adapter, EFUSE_CTRL + 1, (u8)(addr & 0xFF)); /* address */
+	r8712_write8(adapter, EFUSE_CTRL + 2, ((u8)((addr >> 8) & 0x03)) |
+	       (r8712_read8(adapter, EFUSE_CTRL + 2) & 0xFC));
+	r8712_write8(adapter, EFUSE_CTRL, data); /* data */
+	r8712_write8(adapter, EFUSE_CTRL + 3, 0xF2); /* write cmd */
+	/* wait for complete */
+	while ((0x80 &  r8712_read8(adapter, EFUSE_CTRL + 3)) &&
+	       (tmpidx < 100))
+		tmpidx++;
+	if (tmpidx < 100)
+		bResult = true;
+	else
+		bResult = false;
+	return bResult;
+}
+
+static u8 efuse_one_byte_rw(struct _adapter *adapter, u8 bRead, u16 addr,
+			    u8 *data)
+{
+	u8 tmpidx = 0, tmpv8 = 0, bResult;
+
+	/* -----------------e-fuse reg ctrl --------------------------------- */
+	r8712_write8(adapter, EFUSE_CTRL + 1, (u8)(addr & 0xFF)); /* address */
+	tmpv8 = ((u8)((addr >> 8) & 0x03)) |
+		 (r8712_read8(adapter, EFUSE_CTRL + 2) & 0xFC);
+	r8712_write8(adapter, EFUSE_CTRL + 2, tmpv8);
+	if (bRead) {
+		r8712_write8(adapter, EFUSE_CTRL + 3,  0x72); /* read cmd */
+		while (!(0x80 & r8712_read8(adapter, EFUSE_CTRL + 3)) &&
+		       (tmpidx < 100))
+			tmpidx++;
+		if (tmpidx < 100) {
+			*data = r8712_read8(adapter, EFUSE_CTRL);
+			bResult = true;
+		} else {
+			*data = 0;
+			bResult = false;
+		}
+	} else {
+		r8712_write8(adapter, EFUSE_CTRL, *data); /* data */
+		r8712_write8(adapter, EFUSE_CTRL + 3, 0xF2); /* write cmd */
+		while ((0x80 & r8712_read8(adapter, EFUSE_CTRL + 3)) &&
+		       (tmpidx < 100))
+			tmpidx++;
+		if (tmpidx < 100)
+			bResult = true;
+		else
+			bResult = false;
+	}
+	return bResult;
+}
+
+static u8 efuse_is_empty(struct _adapter *adapter, u8 *empty)
+{
+	u8 value, ret = true;
+
+	/* read one byte to check if E-Fuse is empty */
+	if (efuse_one_byte_rw(adapter, true, 0, &value)) {
+		if (value == 0xFF)
+			*empty = true;
+		else
+			*empty = false;
+	} else {
+		ret = false;
+	}
+	return ret;
+}
+
+void r8712_efuse_change_max_size(struct _adapter *adapter)
+{
+	u16 pre_pg_data_saddr = 0x1FB;
+	u16 i;
+	u16 pre_pg_data_size = 5;
+	u8 pre_pg_data[5];
+
+	for (i = 0; i < pre_pg_data_size; i++)
+		efuse_one_byte_read(adapter, pre_pg_data_saddr + i,
+				    &pre_pg_data[i]);
+	if ((pre_pg_data[0] == 0x03) && (pre_pg_data[1] == 0x00) &&
+	    (pre_pg_data[2] == 0x00) && (pre_pg_data[3] == 0x00) &&
+	    (pre_pg_data[4] == 0x0C))
+		efuse_available_max_size -= pre_pg_data_size;
+}
+
+int r8712_efuse_get_max_size(struct _adapter *adapter)
+{
+	return	efuse_available_max_size;
+}
+
+static u8 calculate_word_cnts(const u8 word_en)
+{
+	u8 word_cnts = 0;
+	u8 word_idx;
+
+	for (word_idx = 0; word_idx < PGPKG_MAX_WORDS; word_idx++)
+		if (!(word_en & BIT(word_idx)))
+			word_cnts++; /* 0 : write enable */
+	return word_cnts;
+}
+
+static void pgpacket_copy_data(const u8 word_en, const u8 *sourdata,
+			       u8 *targetdata)
+{
+	u8 tmpindex = 0;
+	u8 word_idx, byte_idx;
+
+	for (word_idx = 0; word_idx < PGPKG_MAX_WORDS; word_idx++) {
+		if (!(word_en & BIT(word_idx))) {
+			byte_idx = word_idx * 2;
+			targetdata[byte_idx] = sourdata[tmpindex++];
+			targetdata[byte_idx + 1] = sourdata[tmpindex++];
+		}
+	}
+}
+
+u16 r8712_efuse_get_current_size(struct _adapter *adapter)
+{
+	int bContinual = true;
+	u16 efuse_addr = 0;
+	u8 hworden = 0;
+	u8 efuse_data, word_cnts = 0;
+
+	while (bContinual && efuse_one_byte_read(adapter, efuse_addr,
+	       &efuse_data) && (efuse_addr < efuse_available_max_size)) {
+		if (efuse_data != 0xFF) {
+			hworden =  efuse_data & 0x0F;
+			word_cnts = calculate_word_cnts(hworden);
+			/* read next header */
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+		} else {
+			bContinual = false;
+		}
+	}
+	return efuse_addr;
+}
+
+u8 r8712_efuse_pg_packet_read(struct _adapter *adapter, u8 offset, u8 *data)
+{
+	u8 hoffset = 0, hworden = 0, word_cnts = 0;
+	u16 efuse_addr = 0;
+	u8 efuse_data;
+	u8 tmpidx = 0;
+	u8 tmpdata[PGPKT_DATA_SIZE];
+	u8 ret = true;
+
+	if (!data)
+		return false;
+	if (offset > 0x0f)
+		return false;
+	memset(data, 0xFF, sizeof(u8) * PGPKT_DATA_SIZE);
+	while (efuse_addr < efuse_available_max_size) {
+		if (efuse_one_byte_read(adapter, efuse_addr, &efuse_data)) {
+			if (efuse_data == 0xFF)
+				break;
+			hoffset = (efuse_data >> 4) & 0x0F;
+			hworden =  efuse_data & 0x0F;
+			word_cnts = calculate_word_cnts(hworden);
+			if (hoffset == offset) {
+				memset(tmpdata, 0xFF, PGPKT_DATA_SIZE);
+				for (tmpidx = 0; tmpidx < word_cnts * 2;
+				     tmpidx++) {
+					if (efuse_one_byte_read(adapter,
+					    efuse_addr + 1 + tmpidx,
+					    &efuse_data)) {
+						tmpdata[tmpidx] = efuse_data;
+					} else {
+						ret = false;
+					}
+				}
+				pgpacket_copy_data(hworden, tmpdata, data);
+			}
+			efuse_addr += 1 + (word_cnts * 2);
+		} else {
+			ret = false;
+			break;
+		}
+	}
+	return ret;
+}
+
+static u8 fix_header(struct _adapter *adapter, u8 header, u16 header_addr)
+{
+	struct PGPKT_STRUCT pkt;
+	u8 offset, word_en, value;
+	u16 addr;
+	int i;
+	u8 ret = true;
+
+	pkt.offset = GET_EFUSE_OFFSET(header);
+	pkt.word_en = GET_EFUSE_WORD_EN(header);
+	addr = header_addr + 1 + calculate_word_cnts(pkt.word_en) * 2;
+	if (addr > efuse_available_max_size)
+		return false;
+	/* retrieve original data */
+	addr = 0;
+	while (addr < header_addr) {
+		if (!efuse_one_byte_read(adapter, addr++, &value)) {
+			ret = false;
+			break;
+		}
+		offset = GET_EFUSE_OFFSET(value);
+		word_en = GET_EFUSE_WORD_EN(value);
+		if (pkt.offset != offset) {
+			addr += calculate_word_cnts(word_en) * 2;
+			continue;
+		}
+		for (i = 0; i < PGPKG_MAX_WORDS; i++) {
+			if (!(BIT(i) & word_en))
+				continue;
+			if (BIT(i) & pkt.word_en) {
+				if (efuse_one_byte_read(adapter,
+							addr,
+							&value))
+					pkt.data[i * 2] = value;
+				else
+					return false;
+				if (efuse_one_byte_read(adapter,
+							addr + 1,
+							&value))
+					pkt.data[i * 2 + 1] = value;
+				else
+					return false;
+			}
+			addr += 2;
+		}
+	}
+	if (addr != header_addr)
+		return false;
+	addr++;
+	/* fill original data */
+	for (i = 0; i < PGPKG_MAX_WORDS; i++) {
+		if (BIT(i) & pkt.word_en) {
+			efuse_one_byte_write(adapter, addr, pkt.data[i * 2]);
+			efuse_one_byte_write(adapter, addr + 1,
+					     pkt.data[i * 2 + 1]);
+			/* additional check */
+			if (!efuse_one_byte_read(adapter, addr, &value)) {
+				ret = false;
+			} else if (pkt.data[i * 2] != value) {
+				ret = false;
+				if (value == 0xFF) /* write again */
+					efuse_one_byte_write(adapter, addr,
+							     pkt.data[i * 2]);
+			}
+			if (!efuse_one_byte_read(adapter, addr + 1, &value)) {
+				ret = false;
+			} else if (pkt.data[i * 2 + 1] != value) {
+				ret = false;
+				if (value == 0xFF) /* write again */
+					efuse_one_byte_write(adapter, addr + 1,
+							     pkt.data[i * 2 +
+								      1]);
+			}
+		}
+		addr += 2;
+	}
+	return ret;
+}
+
+u8 r8712_efuse_pg_packet_write(struct _adapter *adapter, const u8 offset,
+			       const u8 word_en, const u8 *data)
+{
+	u8 pg_header = 0;
+	u16 efuse_addr = 0, curr_size = 0;
+	u8 efuse_data, target_word_cnts = 0;
+	int repeat_times;
+	int sub_repeat;
+	u8 bResult = true;
+
+	/* check if E-Fuse Clock Enable and E-Fuse Clock is 40M */
+	efuse_data = r8712_read8(adapter, EFUSE_CLK_CTRL);
+	if (efuse_data != 0x03)
+		return false;
+	pg_header = MAKE_EFUSE_HEADER(offset, word_en);
+	target_word_cnts = calculate_word_cnts(word_en);
+	repeat_times = 0;
+	efuse_addr = 0;
+	while (efuse_addr < efuse_available_max_size) {
+		curr_size = r8712_efuse_get_current_size(adapter);
+		if ((curr_size + 1 + target_word_cnts * 2) >
+		     efuse_available_max_size)
+			return false; /*target_word_cnts + pg header(1 byte)*/
+		efuse_addr = curr_size; /* current size is also the last addr*/
+		efuse_one_byte_write(adapter, efuse_addr, pg_header); /*hdr*/
+		sub_repeat = 0;
+		/* check if what we read is what we write */
+		while (!efuse_one_byte_read(adapter, efuse_addr,
+					    &efuse_data)) {
+			if (++sub_repeat > _REPEAT_THRESHOLD_) {
+				bResult = false; /* continue to blind write */
+				break; /* continue to blind write */
+			}
+		}
+		if ((sub_repeat > _REPEAT_THRESHOLD_) ||
+		    (pg_header == efuse_data)) {
+			/* write header ok OR can't check header(creep) */
+			u8 i;
+
+			/* go to next address */
+			efuse_addr++;
+			for (i = 0; i < target_word_cnts * 2; i++) {
+				efuse_one_byte_write(adapter,
+						     efuse_addr + i,
+						     *(data + i));
+				if (!efuse_one_byte_read(adapter,
+							 efuse_addr + i,
+							 &efuse_data))
+					bResult = false;
+				else if (*(data + i) != efuse_data) /* fail */
+					bResult = false;
+			}
+			break;
+		}
+		/* write header fail */
+		bResult = false;
+		if (efuse_data == 0xFF)
+			return bResult; /* nothing damaged. */
+		/* call rescue procedure */
+		if (!fix_header(adapter, efuse_data, efuse_addr))
+			return false; /* rescue fail */
+
+		if (++repeat_times > _REPEAT_THRESHOLD_) /* fail */
+			break;
+		/* otherwise, take another risk... */
+	}
+	return bResult;
+}
+
+u8 r8712_efuse_access(struct _adapter *adapter, u8 bRead, u16 start_addr,
+		      u16 cnts, u8 *data)
+{
+	int i;
+	u8 res = true;
+
+	if (start_addr > EFUSE_MAX_SIZE)
+		return false;
+	if (!bRead && ((start_addr + cnts) >
+	   efuse_available_max_size))
+		return false;
+	if (!bRead && !r8712_efuse_reg_init(adapter))
+		return false;
+	/* -----------------e-fuse one byte read / write ---------------------*/
+	for (i = 0; i < cnts; i++) {
+		if ((start_addr + i) > EFUSE_MAX_SIZE) {
+			res = false;
+			break;
+		}
+		res = efuse_one_byte_rw(adapter, bRead, start_addr + i,
+					data + i);
+		if (!bRead && !res)
+			break;
+	}
+	if (!bRead)
+		r8712_efuse_reg_uninit(adapter);
+	return res;
+}
+
+u8 r8712_efuse_map_read(struct _adapter *adapter, u16 addr, u16 cnts, u8 *data)
+{
+	u8 offset, ret = true;
+	u8 pktdata[PGPKT_DATA_SIZE];
+	int i, idx;
+
+	if ((addr + cnts) > EFUSE_MAP_MAX_SIZE)
+		return false;
+	if (efuse_is_empty(adapter, &offset) && offset) {
+		for (i = 0; i < cnts; i++)
+			data[i] = 0xFF;
+		return ret;
+	}
+	offset = (addr >> 3) & 0xF;
+	ret = r8712_efuse_pg_packet_read(adapter, offset, pktdata);
+	i = addr & 0x7;	/* pktdata index */
+	idx = 0;	/* data index */
+
+	do {
+		for (; i < PGPKT_DATA_SIZE; i++) {
+			data[idx++] = pktdata[i];
+			if (idx == cnts)
+				return ret;
+		}
+		offset++;
+		if (!r8712_efuse_pg_packet_read(adapter, offset, pktdata))
+			ret = false;
+		i = 0;
+	} while (1);
+	return ret;
+}
+
+u8 r8712_efuse_map_write(struct _adapter *adapter, u16 addr, u16 cnts,
+			 u8 *data)
+{
+	u8 offset, word_en, empty;
+	u8 pktdata[PGPKT_DATA_SIZE], newdata[PGPKT_DATA_SIZE];
+	int i, j, idx;
+
+	if ((addr + cnts) > EFUSE_MAP_MAX_SIZE)
+		return false;
+	/* check if E-Fuse Clock Enable and E-Fuse Clock is 40M */
+	empty = r8712_read8(adapter, EFUSE_CLK_CTRL);
+	if (empty != 0x03)
+		return false;
+	if (efuse_is_empty(adapter, &empty)) {
+		if (empty)
+			memset(pktdata, 0xFF, PGPKT_DATA_SIZE);
+	} else {
+		return false;
+	}
+	offset = (addr >> 3) & 0xF;
+	if (!empty)
+		if (!r8712_efuse_pg_packet_read(adapter, offset, pktdata))
+			return false;
+	word_en = 0xF;
+	memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	i = addr & 0x7;	/* pktdata index */
+	j = 0;		/* newdata index */
+	idx = 0;	/* data index */
+
+	if (i & 0x1) {
+		/*  odd start */
+		if (data[idx] != pktdata[i]) {
+			word_en &= ~BIT(i >> 1);
+			newdata[j++] = pktdata[i - 1];
+			newdata[j++] = data[idx];
+		}
+		i++;
+		idx++;
+	}
+	do {
+		for (; i < PGPKT_DATA_SIZE; i += 2) {
+			if ((cnts - idx) == 1) {
+				if (data[idx] != pktdata[i]) {
+					word_en &= ~BIT(i >> 1);
+					newdata[j++] = data[idx];
+					newdata[j++] = pktdata[1 + 1];
+				}
+				idx++;
+				break;
+			}
+
+			if ((data[idx] != pktdata[i]) || (data[idx + 1] !=
+			     pktdata[i + 1])) {
+				word_en &= ~BIT(i >> 1);
+				newdata[j++] = data[idx];
+				newdata[j++] = data[idx + 1];
+			}
+			idx += 2;
+
+			if (idx == cnts)
+				break;
+		}
+
+		if (word_en != 0xF)
+			if (!r8712_efuse_pg_packet_write(adapter, offset,
+							 word_en, newdata))
+				return false;
+		if (idx == cnts)
+			break;
+		offset++;
+		if (!empty)
+			if (!r8712_efuse_pg_packet_read(adapter, offset,
+							pktdata))
+				return false;
+		i = 0;
+		j = 0;
+		word_en = 0xF;
+		memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	} while (1);
+
+	return true;
+}
diff --git a/drivers/staging/rtl8712/rtl8712_efuse.h b/drivers/staging/rtl8712/rtl8712_efuse.h
new file mode 100644
index 000000000..2e1ea9d7a
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_efuse.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __RTL8712_EFUSE_H__
+#define __RTL8712_EFUSE_H__
+
+#include "osdep_service.h"
+
+#define _REPEAT_THRESHOLD_	3
+
+#define EFUSE_MAX_SIZE		512
+#define EFUSE_MAP_MAX_SIZE	128
+
+#define PGPKG_MAX_WORDS	4
+#define PGPKT_DATA_SIZE	8 /* PGPKG_MAX_WORDS*2; BYTES sizeof(u8)*8*/
+#define MAX_PGPKT_SIZE	9 /* 1 + PGPKT_DATA_SIZE; header + 2 * 4 words (BYTES)*/
+
+#define GET_EFUSE_OFFSET(header)	((header & 0xF0) >> 4)
+#define GET_EFUSE_WORD_EN(header)	(header & 0x0F)
+#define MAKE_EFUSE_HEADER(offset, word_en)	((((offset) & 0x0F) << 4) | \
+						((word_en) & 0x0F))
+/*--------------------------------------------------------------------------*/
+struct PGPKT_STRUCT {
+	u8 offset;
+	u8 word_en;
+	u8 data[PGPKT_DATA_SIZE];
+};
+/*--------------------------------------------------------------------------*/
+u8 r8712_efuse_reg_init(struct _adapter *padapter);
+void r8712_efuse_reg_uninit(struct _adapter *padapter);
+u16 r8712_efuse_get_current_size(struct _adapter *padapter);
+int r8712_efuse_get_max_size(struct _adapter *padapter);
+void r8712_efuse_change_max_size(struct _adapter *padapter);
+u8 r8712_efuse_pg_packet_read(struct _adapter *padapter,
+			      u8 offset, u8 *data);
+u8 r8712_efuse_pg_packet_write(struct _adapter *padapter,
+			       const u8 offset, const u8 word_en,
+			       const u8 *data);
+u8 r8712_efuse_access(struct _adapter *padapter, u8 bRead,
+		      u16 start_addr, u16 cnts, u8 *data);
+u8 r8712_efuse_map_read(struct _adapter *padapter, u16 addr,
+			u16 cnts, u8 *data);
+u8 r8712_efuse_map_write(struct _adapter *padapter, u16 addr,
+			 u16 cnts, u8 *data);
+#endif
diff --git a/drivers/staging/rtl8712/rtl8712_event.h b/drivers/staging/rtl8712/rtl8712_event.h
new file mode 100644
index 000000000..0d3e5fead
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_event.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL8712_EVENT_H_
+#define _RTL8712_EVENT_H_
+
+void r8712_event_handle(struct _adapter *padapter, __le32 *peventbuf);
+void r8712_got_addbareq_event_callback(struct _adapter *adapter, u8 *pbuf);
+
+enum rtl8712_c2h_event {
+	GEN_EVT_CODE(_Read_MACREG) = 0,		/*0*/
+	GEN_EVT_CODE(_Read_BBREG),
+	GEN_EVT_CODE(_Read_RFREG),
+	GEN_EVT_CODE(_Read_EEPROM),
+	GEN_EVT_CODE(_Read_EFUSE),
+	GEN_EVT_CODE(_Read_CAM),		/*5*/
+	GEN_EVT_CODE(_Get_BasicRate),
+	GEN_EVT_CODE(_Get_DataRate),
+	GEN_EVT_CODE(_Survey),			/*8*/
+	GEN_EVT_CODE(_SurveyDone),		/*9*/
+
+	GEN_EVT_CODE(_JoinBss),			/*10*/
+	GEN_EVT_CODE(_AddSTA),
+	GEN_EVT_CODE(_DelSTA),
+	GEN_EVT_CODE(_AtimDone),
+	GEN_EVT_CODE(_TX_Report),
+	GEN_EVT_CODE(_CCX_Report),		/*15*/
+	GEN_EVT_CODE(_DTM_Report),
+	GEN_EVT_CODE(_TX_Rate_Statistics),
+	GEN_EVT_CODE(_C2HLBK),
+	GEN_EVT_CODE(_FWDBG),
+	GEN_EVT_CODE(_C2HFEEDBACK),		/*20*/
+	GEN_EVT_CODE(_ADDBA),
+	GEN_EVT_CODE(_C2HBCN),
+	GEN_EVT_CODE(_ReportPwrState),		/*filen: only for PCIE, USB*/
+	GEN_EVT_CODE(_WPS_PBC),			/*24*/
+	GEN_EVT_CODE(_ADDBAReq_Report),		/*25*/
+	MAX_C2HEVT
+};
+
+#ifdef _RTL8712_CMD_C_
+
+static struct fwevent wlanevents[] = {
+	{0, NULL},	/*0*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, &r8712_survey_event_callback},		/*8*/
+	{sizeof(struct surveydone_event),
+		&r8712_surveydone_event_callback},	/*9*/
+
+	{0, &r8712_joinbss_event_callback},		/*10*/
+	{sizeof(struct stassoc_event), &r8712_stassoc_event_callback},
+	{sizeof(struct stadel_event), &r8712_stadel_event_callback},
+	{0, &r8712_atimdone_event_callback},
+	{0, NULL},
+	{0, NULL},	/*15*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},	/*fwdbg_event_callback},*/
+	{0, NULL},	/*20*/
+	{0, NULL},
+	{0, NULL},
+	{0, &r8712_cpwm_event_callback},
+	{0, &r8712_wpspbc_event_callback},
+	{0, &r8712_got_addbareq_event_callback},
+};
+
+#endif/*_RTL8712_CMD_C_*/
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl8712_fifoctrl_bitdef.h b/drivers/staging/rtl8712/rtl8712_fifoctrl_bitdef.h
new file mode 100644
index 000000000..f09645fa1
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_fifoctrl_bitdef.h
@@ -0,0 +1,131 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_FIFOCTRL_BITDEF_H__
+#define __RTL8712_FIFOCTRL_BITDEF_H__
+
+/*PBP*/
+#define	_PSTX_MSK			0xF0
+#define	_PSTX_SHT			4
+#define	_PSRX_MSK			0x0F
+#define	_PSRX_SHT			0
+
+/*TXFF_STATUS*/
+#define	_TXSTATUS_OVF		BIT(15)
+
+/*RXFF_STATUS*/
+#define	_STATUSFF1_OVF		BIT(7)
+#define	_STATUSFF1_EMPTY	BIT(6)
+#define	_STATUSFF0_OVF		BIT(5)
+#define	_STATUSFF0_EMPTY	BIT(4)
+#define	_RXFF1_OVF			BIT(3)
+#define	_RXFF1_EMPTY		BIT(2)
+#define	_RXFF0_OVF			BIT(1)
+#define	_RXFF0_EMPTY		BIT(0)
+
+/*TXFF_EMPTY_TH*/
+#define	_BKQ_EMPTY_TH_MSK	0x0F0000
+#define	_BKQ_EMPTY_TH_SHT	16
+#define	_BEQ_EMPTY_TH_MSK	0x00F000
+#define	_BEQ_EMPTY_TH_SHT	12
+#define	_VIQ_EMPTY_TH_MSK	0x000F00
+#define	_VIQ_EMPTY_TH_SHT	8
+#define	_VOQ_EMPTY_TH_MSK	0x0000F0
+#define	_VOQ_EMPTY_TH_SHT	4
+#define	_BMCQ_EMPTY_TH_MSK	0x00000F
+#define	_BMCQ_EMPTY_TH_SHT	0
+
+/*SDIO_RX_BLKSZ*/
+#define	_SDIO_RX_BLKSZ_MSK	0x07
+
+/*RXDMA_CTRL*/
+#define	_C2HFF_POLL		BIT(4)
+#define	_RXPKT_POLL		BIT(0)
+
+/*RXPKT_NUM*/
+#define	_RXCMD_NUM_MSK		0xFF00
+#define	_RXCMD_NUM_SHT		8
+#define	_RXFF0_NUM_MSK		0x00FF
+#define	_RXFF0_NUM_SHT		0
+
+/*FIFOPAGE2*/
+#define	_PUB_AVAL_PG_MSK	0xFFFF0000
+#define	_PUB_AVAL_PG_SHT	16
+#define	_BCN_AVAL_PG_MSK	0x0000FFFF
+#define	_BCN_AVAL_PG_SHT	0
+
+/*RX0PKTNUM*/
+#define	_RXFF0_DEC_POLL				BIT(15)
+#define	_RXFF0_PKT_DEC_NUM_MSK		0x3F00
+#define	_RXFF0_PKT_DEC_NUM_SHT		8
+#define	_RXFF0_PKTNUM_RPT_MSK		0x00FF
+#define	_RXFF0_PKTNUM_RPT_SHT		0
+
+/*RX1PKTNUM*/
+#define	_RXFF1_DEC_POLL				BIT(15)
+#define	_RXFF1_PKT_DEC_NUM_MSK		0x3F00
+#define	_RXFF1_PKT_DEC_NUM_SHT		8
+#define	_RXFF1_PKTNUM_RPT_MSK		0x00FF
+#define	_RXFF1_PKTNUM_RPT_SHT		0
+
+/*RXFLTMAP0*/
+#define	_MGTFLT13EN		BIT(13)
+#define	_MGTFLT12EN		BIT(12)
+#define	_MGTFLT11EN		BIT(11)
+#define	_MGTFLT10EN		BIT(10)
+#define	_MGTFLT9EN		BIT(9)
+#define	_MGTFLT8EN		BIT(8)
+#define	_MGTFLT5EN		BIT(5)
+#define	_MGTFLT4EN		BIT(4)
+#define	_MGTFLT3EN		BIT(3)
+#define	_MGTFLT2EN		BIT(2)
+#define	_MGTFLT1EN		BIT(1)
+#define	_MGTFLT0EN		BIT(0)
+
+/*RXFLTMAP1*/
+#define	_CTRLFLT15EN	BIT(15)
+#define	_CTRLFLT14EN	BIT(14)
+#define	_CTRLFLT13EN	BIT(13)
+#define	_CTRLFLT12EN	BIT(12)
+#define	_CTRLFLT11EN	BIT(11)
+#define	_CTRLFLT10EN	BIT(10)
+#define	_CTRLFLT9EN		BIT(9)
+#define	_CTRLFLT8EN		BIT(8)
+#define	_CTRLFLT7EN		BIT(7)
+#define	_CTRLFLT6EN		BIT(6)
+
+/*RXFLTMAP2*/
+#define	_DATAFLT15EN	BIT(15)
+#define	_DATAFLT14EN	BIT(14)
+#define	_DATAFLT13EN	BIT(13)
+#define	_DATAFLT12EN	BIT(12)
+#define	_DATAFLT11EN	BIT(11)
+#define	_DATAFLT10EN	BIT(10)
+#define	_DATAFLT9EN		BIT(9)
+#define	_DATAFLT8EN		BIT(8)
+#define	_DATAFLT7EN		BIT(7)
+#define	_DATAFLT6EN		BIT(6)
+#define	_DATAFLT5EN		BIT(5)
+#define	_DATAFLT4EN		BIT(4)
+#define	_DATAFLT3EN		BIT(3)
+#define	_DATAFLT2EN		BIT(2)
+#define	_DATAFLT1EN		BIT(1)
+#define	_DATAFLT0EN		BIT(0)
+
+/*RXFLTMAP3*/
+#define	_MESHAFLT1EN		BIT(1)
+#define	_MESHAFLT0EN		BIT(0)
+
+/*TXPKT_NUM_CTRL*/
+#define	_TXPKTNUM_DEC		BIT(8)
+#define	_TXPKTNUM_MSK		0x00FF
+#define	_TXPKTNUM_SHT		0
+
+/*TXFF_PG_NUM*/
+#define	_TXFF_PG_NUM_MSK	0x0FFF
+
+#endif	/*	__RTL8712_FIFOCTRL_BITDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_fifoctrl_regdef.h b/drivers/staging/rtl8712/rtl8712_fifoctrl_regdef.h
new file mode 100644
index 000000000..189fdeb16
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_fifoctrl_regdef.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_FIFOCTRL_REGDEF_H__
+#define __RTL8712_FIFOCTRL_REGDEF_H__
+
+#define RQPN			(RTL8712_FIFOCTRL_ + 0x00)
+#define RXFF_BNDY		(RTL8712_FIFOCTRL_ + 0x0C)
+#define RXRPT_BNDY		(RTL8712_FIFOCTRL_ + 0x10)
+#define TXPKTBUF_PGBNDY		(RTL8712_FIFOCTRL_ + 0x14)
+#define PBP			(RTL8712_FIFOCTRL_ + 0x15)
+#define RX_DRVINFO_SZ		(RTL8712_FIFOCTRL_ + 0x16)
+#define TXFF_STATUS		(RTL8712_FIFOCTRL_ + 0x17)
+#define RXFF_STATUS		(RTL8712_FIFOCTRL_ + 0x18)
+#define TXFF_EMPTY_TH		(RTL8712_FIFOCTRL_ + 0x19)
+#define SDIO_RX_BLKSZ		(RTL8712_FIFOCTRL_ + 0x1C)
+#define RXDMA_RXCTRL		(RTL8712_FIFOCTRL_ + 0x1D)
+#define RXPKT_NUM		(RTL8712_FIFOCTRL_ + 0x1E)
+#define RXPKT_NUM_C2H		(RTL8712_FIFOCTRL_ + 0x1F)
+#define C2HCMD_UDT_SIZE		(RTL8712_FIFOCTRL_ + 0x20)
+#define C2HCMD_UDT_ADDR		(RTL8712_FIFOCTRL_ + 0x22)
+#define FIFOPAGE2		(RTL8712_FIFOCTRL_ + 0x24)
+#define FIFOPAGE1		(RTL8712_FIFOCTRL_ + 0x28)
+#define FW_RSVD_PG_CTRL		(RTL8712_FIFOCTRL_ + 0x30)
+#define TXRPTFF_RDPTR		(RTL8712_FIFOCTRL_ + 0x40)
+#define TXRPTFF_WTPTR		(RTL8712_FIFOCTRL_ + 0x44)
+#define C2HFF_RDPTR		(RTL8712_FIFOCTRL_ + 0x48)
+#define C2HFF_WTPTR		(RTL8712_FIFOCTRL_ + 0x4C)
+#define RXFF0_RDPTR		(RTL8712_FIFOCTRL_ + 0x50)
+#define RXFF0_WTPTR		(RTL8712_FIFOCTRL_ + 0x54)
+#define RXFF1_RDPTR		(RTL8712_FIFOCTRL_ + 0x58)
+#define RXFF1_WTPTR		(RTL8712_FIFOCTRL_ + 0x5C)
+#define RXRPT0FF_RDPTR		(RTL8712_FIFOCTRL_ + 0x60)
+#define RXRPT0FF_WTPTR		(RTL8712_FIFOCTRL_ + 0x64)
+#define RXRPT1FF_RDPTR		(RTL8712_FIFOCTRL_ + 0x68)
+#define RXRPT1FF_WTPTR		(RTL8712_FIFOCTRL_ + 0x6C)
+#define RX0PKTNUM		(RTL8712_FIFOCTRL_ + 0x72)
+#define RX1PKTNUM		(RTL8712_FIFOCTRL_ + 0x74)
+#define RXFLTMAP0		(RTL8712_FIFOCTRL_ + 0x76)
+#define RXFLTMAP1		(RTL8712_FIFOCTRL_ + 0x78)
+#define RXFLTMAP2		(RTL8712_FIFOCTRL_ + 0x7A)
+#define RXFLTMAP3		(RTL8712_FIFOCTRL_ + 0x7c)
+#define TBDA			(RTL8712_FIFOCTRL_ + 0x84)
+#define THPDA			(RTL8712_FIFOCTRL_ + 0x88)
+#define TCDA			(RTL8712_FIFOCTRL_ + 0x8C)
+#define TMDA			(RTL8712_FIFOCTRL_ + 0x90)
+#define HDA			(RTL8712_FIFOCTRL_ + 0x94)
+#define TVODA			(RTL8712_FIFOCTRL_ + 0x98)
+#define TVIDA			(RTL8712_FIFOCTRL_ + 0x9C)
+#define TBEDA			(RTL8712_FIFOCTRL_ + 0xA0)
+#define TBKDA			(RTL8712_FIFOCTRL_ + 0xA4)
+#define RCDA			(RTL8712_FIFOCTRL_ + 0xA8)
+#define RDSA			(RTL8712_FIFOCTRL_ + 0xAC)
+#define TXPKT_NUM_CTRL		(RTL8712_FIFOCTRL_ + 0xB0)
+#define TXQ_PGADD		(RTL8712_FIFOCTRL_ + 0xB3)
+#define TXFF_PG_NUM		(RTL8712_FIFOCTRL_ + 0xB4)
+
+#endif	/* __RTL8712_FIFOCTRL_REGDEF_H__ */
diff --git a/drivers/staging/rtl8712/rtl8712_gp_bitdef.h b/drivers/staging/rtl8712/rtl8712_gp_bitdef.h
new file mode 100644
index 000000000..ee651fb3f
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_gp_bitdef.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_GP_BITDEF_H__
+#define __RTL8712_GP_BITDEF_H__
+
+/*GPIO_CTRL*/
+#define	_GPIO_MOD_MSK			0xFF000000
+#define	_GPIO_MOD_SHT			24
+#define	_GPIO_IO_SEL_MSK		0x00FF0000
+#define	_GPIO_IO_SEL_SHT		16
+#define	_GPIO_OUT_MSK			0x0000FF00
+#define	_GPIO_OUT_SHT			8
+#define	_GPIO_IN_MSK			0x000000FF
+#define	_GPIO_IN_SHT			0
+
+/*SYS_PINMUX_CFG*/
+#define	_GPIOSEL_MSK			0x0003
+#define	_GPIOSEL_SHT			0
+
+/*LED_CFG*/
+#define _LED1SV				BIT(7)
+#define _LED1CM_MSK			0x0070
+#define _LED1CM_SHT			4
+#define _LED0SV				BIT(3)
+#define _LED0CM_MSK			0x0007
+#define _LED0CM_SHT			0
+
+/*PHY_REG*/
+#define _HST_RDRDY_SHT			0
+#define _HST_RDRDY_MSK			0xFF
+#define _HST_RDRDY			BIT(_HST_RDRDY_SHT)
+#define _CPU_WTBUSY_SHT			1
+#define _CPU_WTBUSY_MSK			0xFF
+#define _CPU_WTBUSY			BIT(_CPU_WTBUSY_SHT)
+
+/* 11. General Purpose Registers   (Offset: 0x02E0 - 0x02FF)*/
+
+/*       8192S GPIO Config Setting (offset 0x2F1, 1 byte)*/
+
+/*----------------------------------------------------------------------------*/
+
+#define		GPIOMUX_EN	BIT(3)	/* When this bit is set to "1",
+					 * GPIO PINs will switch to MAC
+					 * GPIO Function
+					 */
+#define		GPIOSEL_GPIO	0	/* UART or JTAG or pure GPIO*/
+#define		GPIOSEL_PHYDBG	1	/* PHYDBG*/
+#define		GPIOSEL_BT	2	/* BT_coex*/
+#define		GPIOSEL_WLANDBG	3	/* WLANDBG*/
+#define		GPIOSEL_GPIO_MASK	(~(BIT(0) | BIT(1)))
+/* HW Radio OFF switch (GPIO BIT) */
+#define		HAL_8192S_HW_GPIO_OFF_BIT	BIT(3)
+#define		HAL_8192S_HW_GPIO_OFF_MASK	0xF7
+#define		HAL_8192S_HW_GPIO_WPS_BIT	BIT(4)
+
+#endif	/*__RTL8712_GP_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_gp_regdef.h b/drivers/staging/rtl8712/rtl8712_gp_regdef.h
new file mode 100644
index 000000000..892a7fb13
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_gp_regdef.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_GP_REGDEF_H__
+#define __RTL8712_GP_REGDEF_H__
+
+#define PSTIMER			(RTL8712_GP_ + 0x00)
+#define TIMER1			(RTL8712_GP_ + 0x04)
+#define TIMER2			(RTL8712_GP_ + 0x08)
+#define GPIO_CTRL		(RTL8712_GP_ + 0x0C)
+#define GPIO_IO_SEL		(RTL8712_GP_ + 0x0E)
+#define GPIO_INTCTRL		(RTL8712_GP_ + 0x10)
+#define MAC_PINMUX_CTRL		(RTL8712_GP_ + 0x11)
+#define LEDCFG			(RTL8712_GP_ + 0x12)
+#define PHY_REG_RPT		(RTL8712_GP_ + 0x13)
+#define PHY_REG_DATA		(RTL8712_GP_ + 0x14)
+
+#endif	/*__RTL8712_GP_REGDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_hal.h b/drivers/staging/rtl8712/rtl8712_hal.h
new file mode 100644
index 000000000..66cc4645e
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_hal.h
@@ -0,0 +1,142 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_HAL_H__
+#define __RTL8712_HAL_H__
+
+enum _HW_VERSION {
+	RTL8712_FPGA,
+	RTL8712_1stCUT,	/*A Cut (RTL8712_ASIC)*/
+	RTL8712_2ndCUT,	/*B Cut*/
+	RTL8712_3rdCUT,	/*C Cut*/
+};
+
+enum _LOOPBACK_TYPE {
+	RTL8712_AIR_TRX = 0,
+	RTL8712_MAC_LBK,
+	RTL8712_BB_LBK,
+	RTL8712_MAC_FW_LBK = 4,
+	RTL8712_BB_FW_LBK = 8,
+};
+
+enum RTL871X_HCI_TYPE {
+	RTL8712_SDIO,
+	RTL8712_USB,
+};
+
+enum RTL8712_RF_CONFIG {
+	RTL8712_RF_1T1R,
+	RTL8712_RF_1T2R,
+	RTL8712_RF_2T2R
+};
+
+enum _RTL8712_HCI_TYPE_ {
+	RTL8712_HCI_TYPE_PCIE = 0x01,
+	RTL8712_HCI_TYPE_AP_PCIE = 0x81,
+	RTL8712_HCI_TYPE_USB = 0x02,
+	RTL8712_HCI_TYPE_92USB = 0x02,
+	RTL8712_HCI_TYPE_AP_USB = 0x82,
+	RTL8712_HCI_TYPE_72USB = 0x12,
+	RTL8712_HCI_TYPE_SDIO = 0x04,
+	RTL8712_HCI_TYPE_72SDIO = 0x14
+};
+
+struct fw_priv {   /*8-bytes alignment required*/
+	/*--- long word 0 ----*/
+	unsigned char signature_0;  /*0x12: CE product, 0x92: IT product*/
+	unsigned char signature_1;  /*0x87: CE product, 0x81: IT product*/
+	unsigned char hci_sel; /*0x81: PCI-AP, 01:PCIe, 02: 92S-U, 0x82: USB-AP,
+				* 0x12: 72S-U, 03:SDIO
+				*/
+	unsigned char chip_version; /*the same value as register value*/
+	unsigned char customer_ID_0; /*customer  ID low byte*/
+	unsigned char customer_ID_1; /*customer  ID high byte*/
+	unsigned char rf_config;  /*0x11:  1T1R, 0x12: 1T2R, 0x92: 1T2R turbo,
+				   * 0x22: 2T2R
+				   */
+	unsigned char usb_ep_num;  /* 4: 4EP, 6: 6EP, 11: 11EP*/
+	/*--- long word 1 ----*/
+	unsigned char regulatory_class_0; /*regulatory class bit map 0*/
+	unsigned char regulatory_class_1; /*regulatory class bit map 1*/
+	unsigned char regulatory_class_2; /*regulatory class bit map 2*/
+	unsigned char regulatory_class_3; /*regulatory class bit map 3*/
+	unsigned char rfintfs;    /* 0:SWSI, 1:HWSI, 2:HWPI*/
+	unsigned char def_nettype;
+	unsigned char turbo_mode;
+	unsigned char low_power_mode;/* 0: normal mode, 1: low power mode*/
+	/*--- long word 2 ----*/
+	unsigned char lbk_mode; /*0x00: normal, 0x03: MACLBK, 0x01: PHYLBK*/
+	unsigned char mp_mode; /* 1: for MP use, 0: for normal driver */
+	unsigned char vcs_type; /* 0:off 1:on 2:auto */
+	unsigned char vcs_mode; /* 1:RTS/CTS 2:CTS to self */
+	unsigned char rsvd022;
+	unsigned char rsvd023;
+	unsigned char rsvd024;
+	unsigned char rsvd025;
+	/*--- long word 3 ----*/
+	unsigned char qos_en;    /*1: QoS enable*/
+	unsigned char bw_40MHz_en;   /*1: 40MHz BW enable*/
+	unsigned char AMSDU2AMPDU_en;   /*1: 4181 convert AMSDU to AMPDU,
+					 * 0: disable
+					 */
+	unsigned char AMPDU_en;   /*1: 11n AMPDU enable*/
+	unsigned char rate_control_offload; /*1: FW offloads,0: driver handles*/
+	unsigned char aggregation_offload;  /*1: FW offloads,0: driver handles*/
+	unsigned char rsvd030;
+	unsigned char rsvd031;
+	/*--- long word 4 ----*/
+	unsigned char beacon_offload;   /* 1. FW offloads, 0: driver handles*/
+	unsigned char MLME_offload;   /* 2. FW offloads, 0: driver handles*/
+	unsigned char hwpc_offload;   /* 3. FW offloads, 0: driver handles*/
+	unsigned char tcp_checksum_offload; /*4. FW offloads,0: driver handles*/
+	unsigned char tcp_offload;    /* 5. FW offloads, 0: driver handles*/
+	unsigned char ps_control_offload; /* 6. FW offloads, 0: driver handles*/
+	unsigned char WWLAN_offload;   /* 7. FW offloads, 0: driver handles*/
+	unsigned char rsvd040;
+	/*--- long word 5 ----*/
+	unsigned char tcp_tx_frame_len_L;  /*tcp tx packet length low byte*/
+	unsigned char tcp_tx_frame_len_H;  /*tcp tx packet length high byte*/
+	unsigned char tcp_rx_frame_len_L;  /*tcp rx packet length low byte*/
+	unsigned char tcp_rx_frame_len_H;  /*tcp rx packet length high byte*/
+	unsigned char rsvd050;
+	unsigned char rsvd051;
+	unsigned char rsvd052;
+	unsigned char rsvd053;
+};
+
+struct fw_hdr {/*8-byte alignment required*/
+	unsigned short	signature;
+	unsigned short	version;	/* 0x8000 ~ 0x8FFF for FPGA version,
+					 * 0x0000 ~ 0x7FFF for ASIC version,
+					 */
+	unsigned int		dmem_size;    /*define the size of boot loader*/
+	unsigned int		img_IMEM_size; /*define the size of FW in IMEM*/
+	unsigned int		img_SRAM_size; /*define the size of FW in SRAM*/
+	unsigned int		fw_priv_sz; /*define the size of DMEM variable*/
+	unsigned short	efuse_addr;
+	unsigned short	h2ccnd_resp_addr;
+	unsigned int		SVNRevision;
+	unsigned int		release_time; /*Mon:Day:Hr:Min*/
+	struct fw_priv	fwpriv;
+};
+
+struct hal_priv {
+	/*Endpoint handles*/
+	struct  net_device *pipehdls_r8712[10];
+	u8 (*hal_bus_init)(struct _adapter *adapter);
+};
+
+uint	 rtl8712_hal_init(struct _adapter *padapter);
+int rtl871x_load_fw(struct _adapter *padapter);
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl8712_interrupt_bitdef.h b/drivers/staging/rtl8712/rtl8712_interrupt_bitdef.h
new file mode 100644
index 000000000..e9732a1bc
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_interrupt_bitdef.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_INTERRUPT_BITDEF_H__
+#define __RTL8712_INTERRUPT_BITDEF_H__
+
+/*HIMR*/
+/*HISR*/
+#define	_CPUERR					BIT(29)
+#define	_ATIMEND				BIT(28)
+#define	_TXBCNOK				BIT(27)
+#define	_TXBCNERR				BIT(26)
+#define	_BCNDMAINT4				BIT(25)
+#define	_BCNDMAINT3				BIT(24)
+#define	_BCNDMAINT2				BIT(23)
+#define	_BCNDMAINT1				BIT(22)
+#define	_BCNDOK4				BIT(21)
+#define	_BCNDOK3				BIT(20)
+#define	_BCNDOK2				BIT(19)
+#define	_BCNDOK1				BIT(18)
+#define	_TIMEOUT2				BIT(17)
+#define	_TIMEOUT1				BIT(16)
+#define	_TXFOVW					BIT(15)
+#define	_PSTIMEOUT				BIT(14)
+#define	_BCNDMAINT0				BIT(13)
+#define	_FOVW					BIT(12)
+#define	_RDU					BIT(11)
+#define	_RXCMDOK				BIT(10)
+#define	_BCNDOK0				BIT(9)
+#define	_HIGHDOK				BIT(8)
+#define	_COMDOK					BIT(7)
+#define	_MGTDOK					BIT(6)
+#define	_HCCADOK				BIT(5)
+#define	_BKDOK					BIT(4)
+#define	_BEDOK					BIT(3)
+#define	_VIDOK					BIT(2)
+#define	_VODOK					BIT(1)
+#define	_RXOK					BIT(0)
+
+#endif	/*__RTL8712_INTERRUPT_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_io.c b/drivers/staging/rtl8712/rtl8712_io.c
new file mode 100644
index 000000000..384cbdb05
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_io.c
@@ -0,0 +1,99 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl8712_io.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL8712_IO_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "rtl871x_io.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+
+u8 r8712_read8(struct _adapter *adapter, u32 addr)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	return hdl->io_ops._read8(hdl, addr);
+}
+
+u16 r8712_read16(struct _adapter *adapter, u32 addr)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	return hdl->io_ops._read16(hdl, addr);
+}
+
+u32 r8712_read32(struct _adapter *adapter, u32 addr)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	return hdl->io_ops._read32(hdl, addr);
+}
+
+void r8712_write8(struct _adapter *adapter, u32 addr, u8 val)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	hdl->io_ops._write8(hdl, addr, val);
+}
+
+void r8712_write16(struct _adapter *adapter, u32 addr, u16 val)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	hdl->io_ops._write16(hdl, addr, val);
+}
+
+void r8712_write32(struct _adapter *adapter, u32 addr, u32 val)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	hdl->io_ops._write32(hdl, addr, val);
+}
+
+void r8712_read_mem(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	if (adapter->driver_stopped || adapter->surprise_removed)
+		return;
+
+	hdl->io_ops._read_mem(hdl, addr, cnt, pmem);
+}
+
+void r8712_write_mem(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	hdl->io_ops._write_mem(hdl, addr, cnt, pmem);
+}
+
+void r8712_read_port(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	if (adapter->driver_stopped || adapter->surprise_removed)
+		return;
+
+	hdl->io_ops._read_port(hdl, addr, cnt, pmem);
+}
+
+void r8712_write_port(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	struct intf_hdl *hdl = &adapter->pio_queue->intf;
+
+	hdl->io_ops._write_port(hdl, addr, cnt, pmem);
+}
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
new file mode 100644
index 000000000..d5fc9026b
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -0,0 +1,1830 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl8712_led.c
+ *
+ * Copyright(c) 2007 - 2010  Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#include "drv_types.h"
+
+/*===========================================================================
+ *	Constant.
+ *===========================================================================
+
+ *
+ * Default LED behavior.
+ */
+#define LED_BLINK_NORMAL_INTERVAL	100
+#define LED_BLINK_SLOWLY_INTERVAL	200
+#define LED_BLINK_LONG_INTERVAL	400
+
+#define LED_BLINK_NO_LINK_INTERVAL_ALPHA	1000
+#define LED_BLINK_LINK_INTERVAL_ALPHA		500
+#define LED_BLINK_SCAN_INTERVAL_ALPHA		180
+#define LED_BLINK_FASTER_INTERVAL_ALPHA		50
+#define LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA	5000
+
+/*===========================================================================
+ * LED object.
+ *===========================================================================
+ */
+enum _LED_STATE_871x {
+	LED_UNKNOWN = 0,
+	LED_STATE_ON = 1,
+	LED_STATE_OFF = 2,
+	LED_BLINK_NORMAL = 3,
+	LED_BLINK_SLOWLY = 4,
+	LED_POWER_ON_BLINK = 5,
+	LED_SCAN_BLINK = 6, /* LED is blinking during scanning period,
+			     * the # of times to blink is depend on time
+			     * for scanning.
+			     */
+	LED_NO_LINK_BLINK = 7, /* LED is blinking during no link state. */
+	LED_BLINK_StartToBlink = 8,/* Customized for Sercomm Printer
+				    * Server case
+				    */
+	LED_BLINK_WPS = 9,	/* LED is blinkg during WPS communication */
+	LED_TXRX_BLINK = 10,
+	LED_BLINK_WPS_STOP = 11,	/*for ALPHA */
+	LED_BLINK_WPS_STOP_OVERLAP = 12,	/*for BELKIN */
+};
+
+/*===========================================================================
+ *	Prototype of protected function.
+ *===========================================================================
+ */
+static void BlinkTimerCallback(struct timer_list *t);
+
+static void BlinkWorkItemCallback(struct work_struct *work);
+/*===========================================================================
+ * LED_819xUsb routines.
+ *===========================================================================
+ *
+ *
+ *
+ *	Description:
+ *		Initialize an LED_871x object.
+ */
+static void InitLed871x(struct _adapter *padapter, struct LED_871x *pLed,
+			enum LED_PIN_871x	LedPin)
+{
+	pLed->padapter = padapter;
+	pLed->LedPin = LedPin;
+	pLed->CurrLedState = LED_STATE_OFF;
+	pLed->bLedOn = false;
+	pLed->bLedBlinkInProgress = false;
+	pLed->BlinkTimes = 0;
+	pLed->BlinkingLedState = LED_UNKNOWN;
+	timer_setup(&pLed->BlinkTimer, BlinkTimerCallback, 0);
+	INIT_WORK(&pLed->BlinkWorkItem, BlinkWorkItemCallback);
+}
+
+/*
+ *	Description:
+ *		DeInitialize an LED_871x object.
+ */
+static void DeInitLed871x(struct LED_871x *pLed)
+{
+	del_timer_sync(&pLed->BlinkTimer);
+	/* We should reset bLedBlinkInProgress if we cancel
+	 * the LedControlTimer,
+	 */
+	pLed->bLedBlinkInProgress = false;
+}
+
+/*
+ *	Description:
+ *		Turn on LED according to LedPin specified.
+ */
+static void SwLedOn(struct _adapter *padapter, struct LED_871x *pLed)
+{
+	u8	LedCfg;
+
+	if (padapter->surprise_removed || padapter->driver_stopped)
+		return;
+	LedCfg = r8712_read8(padapter, LEDCFG);
+	switch (pLed->LedPin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		/* SW control led0 on.*/
+		r8712_write8(padapter, LEDCFG, LedCfg & 0xf0);
+		break;
+	case LED_PIN_LED1:
+		/* SW control led1 on.*/
+		r8712_write8(padapter, LEDCFG, LedCfg & 0x0f);
+		break;
+	default:
+		break;
+	}
+	pLed->bLedOn = true;
+}
+
+/*
+ *	Description:
+ *		Turn off LED according to LedPin specified.
+ */
+static void SwLedOff(struct _adapter *padapter, struct LED_871x *pLed)
+{
+	u8	LedCfg;
+
+	if (padapter->surprise_removed || padapter->driver_stopped)
+		return;
+	LedCfg = r8712_read8(padapter, LEDCFG);
+	switch (pLed->LedPin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		LedCfg &= 0xf0; /* Set to software control.*/
+		r8712_write8(padapter, LEDCFG, (LedCfg | BIT(3)));
+		break;
+	case LED_PIN_LED1:
+		LedCfg &= 0x0f; /* Set to software control.*/
+		r8712_write8(padapter, LEDCFG, (LedCfg | BIT(7)));
+		break;
+	default:
+		break;
+	}
+	pLed->bLedOn = false;
+}
+
+/*===========================================================================
+ * Interface to manipulate LED objects.
+ *===========================================================================
+ *
+ *	Description:
+ *		Initialize all LED_871x objects.
+ */
+void r8712_InitSwLeds(struct _adapter *padapter)
+{
+	struct led_priv	*pledpriv = &padapter->ledpriv;
+
+	pledpriv->LedControlHandler = LedControl871x;
+	InitLed871x(padapter, &pledpriv->SwLed0, LED_PIN_LED0);
+	InitLed871x(padapter, &pledpriv->SwLed1, LED_PIN_LED1);
+}
+
+/*	Description:
+ *		DeInitialize all LED_819xUsb objects.
+ */
+void r8712_DeInitSwLeds(struct _adapter *padapter)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+
+	DeInitLed871x(&ledpriv->SwLed0);
+	DeInitLed871x(&ledpriv->SwLed1);
+}
+
+/*	Description:
+ *		Implementation of LED blinking behavior.
+ *		It toggle off LED and schedule corresponding timer if necessary.
+ */
+static void SwLedBlink(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	/* Determine if we shall change LED state again. */
+	pLed->BlinkTimes--;
+	switch (pLed->CurrLedState) {
+	case LED_BLINK_NORMAL:
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		break;
+	case LED_BLINK_StartToBlink:
+		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+		    (pmlmepriv->fw_state & WIFI_STATION_STATE))
+			bStopBlinking = true;
+		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+		    ((pmlmepriv->fw_state & WIFI_ADHOC_STATE) ||
+		    (pmlmepriv->fw_state & WIFI_ADHOC_MASTER_STATE)))
+			bStopBlinking = true;
+		else if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		break;
+	case LED_BLINK_WPS:
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		break;
+	default:
+		bStopBlinking = true;
+		break;
+	}
+	if (bStopBlinking) {
+		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+		    !pLed->bLedOn)
+			SwLedOn(padapter, pLed);
+		else if (check_fwstate(pmlmepriv, _FW_LINKED) &&  pLed->bLedOn)
+			SwLedOff(padapter, pLed);
+		pLed->BlinkTimes = 0;
+		pLed->bLedBlinkInProgress = false;
+	} else {
+		/* Assign LED state to toggle. */
+		if (pLed->BlinkingLedState == LED_STATE_ON)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+
+		/* Schedule a timer to toggle LED state. */
+		switch (pLed->CurrLedState) {
+		case LED_BLINK_NORMAL:
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+			break;
+		case LED_BLINK_SLOWLY:
+		case LED_BLINK_StartToBlink:
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+			break;
+		case LED_BLINK_WPS:
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LONG_INTERVAL));
+			break;
+		default:
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+			break;
+		}
+	}
+}
+
+static void SwLedBlink1(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	struct led_priv *ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct eeprom_priv *peeprompriv = &padapter->eeprompriv;
+	struct LED_871x *pLed1 = &ledpriv->SwLed1;
+	u8 bStopBlinking = false;
+
+	if (peeprompriv->CustomerID == RT_CID_819x_CAMEO)
+		pLed = &ledpriv->SwLed1;
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	if (peeprompriv->CustomerID == RT_CID_DEFAULT) {
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			if (!pLed1->bSWLedCtrl) {
+				SwLedOn(padapter, pLed1);
+				pLed1->bSWLedCtrl = true;
+			} else if (!pLed1->bLedOn) {
+				SwLedOn(padapter, pLed1);
+			}
+		} else {
+			if (!pLed1->bSWLedCtrl) {
+				SwLedOff(padapter, pLed1);
+				pLed1->bSWLedCtrl = true;
+			} else if (pLed1->bLedOn) {
+				SwLedOff(padapter, pLed1);
+			}
+		}
+	}
+	switch (pLed->CurrLedState) {
+	case LED_BLINK_SLOWLY:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		break;
+	case LED_BLINK_NORMAL:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+		break;
+	case LED_SCAN_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_NORMAL;
+				if (pLed->bLedOn)
+					pLed->BlinkingLedState = LED_STATE_OFF;
+				else
+					pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedNoLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_SLOWLY;
+				if (pLed->bLedOn)
+					pLed->BlinkingLedState = LED_STATE_OFF;
+				else
+					pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+			}
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_NORMAL;
+				if (pLed->bLedOn)
+					pLed->BlinkingLedState = LED_STATE_OFF;
+				else
+					pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->bLedNoLinkBlinkInProgress = true;
+				pLed->CurrLedState = LED_BLINK_SLOWLY;
+				if (pLed->bLedOn)
+					pLed->BlinkingLedState = LED_STATE_OFF;
+				else
+					pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+			}
+			pLed->BlinkTimes = 0;
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_BLINK_WPS:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		break;
+	case LED_BLINK_WPS_STOP:	/* WPS success */
+		if (pLed->BlinkingLedState == LED_STATE_ON) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
+			bStopBlinking = false;
+		} else {
+			bStopBlinking = true;
+		}
+		if (bStopBlinking) {
+			pLed->bLedLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_NORMAL;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+		}
+		pLed->bLedWPSBlinkInProgress = false;
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedBlink2(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	switch (pLed->CurrLedState) {
+	case LED_SCAN_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_ON;
+				pLed->BlinkingLedState = LED_STATE_ON;
+				SwLedOn(padapter, pLed);
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_OFF;
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				SwLedOff(padapter, pLed);
+			}
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_ON;
+				pLed->BlinkingLedState = LED_STATE_ON;
+				SwLedOn(padapter, pLed);
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_OFF;
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				SwLedOff(padapter, pLed);
+			}
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedBlink3(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		if (pLed->CurrLedState != LED_BLINK_WPS_STOP)
+			SwLedOff(padapter, pLed);
+	switch (pLed->CurrLedState) {
+	case LED_SCAN_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_ON;
+				pLed->BlinkingLedState = LED_STATE_ON;
+				if (!pLed->bLedOn)
+					SwLedOn(padapter, pLed);
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_OFF;
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				if (pLed->bLedOn)
+					SwLedOff(padapter, pLed);
+			}
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_ON;
+				pLed->BlinkingLedState = LED_STATE_ON;
+				if (!pLed->bLedOn)
+					SwLedOn(padapter, pLed);
+			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				pLed->CurrLedState = LED_STATE_OFF;
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				if (pLed->bLedOn)
+					SwLedOff(padapter, pLed);
+			}
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_BLINK_WPS:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		break;
+	case LED_BLINK_WPS_STOP:	/*WPS success*/
+		if (pLed->BlinkingLedState == LED_STATE_ON) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
+			bStopBlinking = false;
+		} else {
+			bStopBlinking = true;
+		}
+		if (bStopBlinking) {
+			pLed->CurrLedState = LED_STATE_ON;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			SwLedOn(padapter, pLed);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedBlink4(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+	struct LED_871x *pLed1 = &ledpriv->SwLed1;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	if (!pLed1->bLedWPSBlinkInProgress &&
+	    pLed1->BlinkingLedState == LED_UNKNOWN) {
+		pLed1->BlinkingLedState = LED_STATE_OFF;
+		pLed1->CurrLedState = LED_STATE_OFF;
+		SwLedOff(padapter, pLed1);
+	}
+	switch (pLed->CurrLedState) {
+	case LED_BLINK_SLOWLY:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		break;
+	case LED_BLINK_StartToBlink:
+		if (pLed->bLedOn) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+		} else {
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		}
+		break;
+	case LED_SCAN_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			pLed->bLedNoLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_SLOWLY;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			pLed->bLedNoLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_SLOWLY;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_BLINK_WPS:
+		if (pLed->bLedOn) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+		} else {
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		}
+		break;
+	case LED_BLINK_WPS_STOP:	/*WPS authentication fail*/
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		break;
+	case LED_BLINK_WPS_STOP_OVERLAP:	/*WPS session overlap */
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0) {
+			if (pLed->bLedOn)
+				pLed->BlinkTimes = 1;
+			else
+				bStopBlinking = true;
+		}
+		if (bStopBlinking) {
+			pLed->BlinkTimes = 10;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedBlink5(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	switch (pLed->CurrLedState) {
+	case LED_SCAN_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			pLed->CurrLedState = LED_STATE_ON;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			if (!pLed->bLedOn)
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+			pLed->bLedScanBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			pLed->CurrLedState = LED_STATE_ON;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			if (!pLed->bLedOn)
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedBlink6(struct LED_871x *pLed)
+{
+	struct _adapter *padapter = pLed->padapter;
+	u8 bStopBlinking = false;
+
+	/* Change LED according to BlinkingLedState specified. */
+	if (pLed->BlinkingLedState == LED_STATE_ON)
+		SwLedOn(padapter, pLed);
+	else
+		SwLedOff(padapter, pLed);
+	switch (pLed->CurrLedState) {
+	case LED_TXRX_BLINK:
+		pLed->BlinkTimes--;
+		if (pLed->BlinkTimes == 0)
+			bStopBlinking = true;
+		if (bStopBlinking) {
+			pLed->CurrLedState = LED_STATE_ON;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			if (!pLed->bLedOn)
+				SwLedOn(padapter, pLed);
+			pLed->bLedBlinkInProgress = false;
+		} else {
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_BLINK_WPS:
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*	Description:
+ *		Callback function of LED BlinkTimer,
+ *		it just schedules to corresponding BlinkWorkItem.
+ */
+static void BlinkTimerCallback(struct timer_list *t)
+{
+	struct LED_871x  *pLed = from_timer(pLed, t, BlinkTimer);
+
+	/* This fixed the crash problem on Fedora 12 when trying to do the
+	 * insmod;ifconfig up;rmmod commands.
+	 */
+	if (pLed->padapter->surprise_removed || pLed->padapter->driver_stopped)
+		return;
+	schedule_work(&pLed->BlinkWorkItem);
+}
+
+/*	Description:
+ *		Callback function of LED BlinkWorkItem.
+ *		We dispatch actual LED blink action according to LedStrategy.
+ */
+static void BlinkWorkItemCallback(struct work_struct *work)
+{
+	struct LED_871x *pLed = container_of(work, struct LED_871x,
+				BlinkWorkItem);
+	struct led_priv	*ledpriv = &pLed->padapter->ledpriv;
+
+	switch (ledpriv->LedStrategy) {
+	case SW_LED_MODE0:
+		SwLedBlink(pLed);
+		break;
+	case SW_LED_MODE1:
+		SwLedBlink1(pLed);
+		break;
+	case SW_LED_MODE2:
+		SwLedBlink2(pLed);
+		break;
+	case SW_LED_MODE3:
+		SwLedBlink3(pLed);
+		break;
+	case SW_LED_MODE4:
+		SwLedBlink4(pLed);
+		break;
+	case SW_LED_MODE5:
+		SwLedBlink5(pLed);
+		break;
+	case SW_LED_MODE6:
+		SwLedBlink6(pLed);
+		break;
+	default:
+		SwLedBlink(pLed);
+		break;
+	}
+}
+
+/*============================================================================
+ * Default LED behavior.
+ *============================================================================
+ *
+ *	Description:
+ *		Implement each led action for SW_LED_MODE0.
+ *		This is default strategy.
+ */
+
+static void SwLedControlMode1(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &padapter->ledpriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sitesurvey_ctrl *psitesurveyctrl = &pmlmepriv->sitesurveyctrl;
+
+	if (padapter->eeprompriv.CustomerID == RT_CID_819x_CAMEO)
+		pLed = &ledpriv->SwLed1;
+	switch (LedAction) {
+	case LED_CTL_START_TO_LINK:
+	case LED_CTL_NO_LINK:
+		if (!pLed->bLedNoLinkBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedNoLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_SLOWLY;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_LINK:
+		if (!pLed->bLedLinkBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_NORMAL;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_SITE_SURVEY:
+		if (psitesurveyctrl->traffic_busy &&
+		    check_fwstate(pmlmepriv, _FW_LINKED))
+			; /* dummy branch */
+		else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedScanBlinkInProgress = true;
+			pLed->CurrLedState = LED_SCAN_BLINK;
+			pLed->BlinkTimes = 24;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedLinkBlinkInProgress = false;
+			}
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+
+	case LED_CTL_START_WPS: /*wait until xinpin finish */
+	case LED_CTL_START_WPS_BOTTON:
+		if (!pLed->bLedWPSBlinkInProgress) {
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedScanBlinkInProgress = false;
+			}
+			pLed->bLedWPSBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_WPS;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_STOP_WPS:
+		if (pLed->bLedNoLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedNoLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress)
+			del_timer(&pLed->BlinkTimer);
+		else
+			pLed->bLedWPSBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_WPS_STOP;
+		if (pLed->bLedOn) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
+		} else {
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
+		}
+		break;
+	case LED_CTL_STOP_WPS_FAIL:
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->bLedNoLinkBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedNoLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedNoLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedControlMode2(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	 *ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+
+	switch (LedAction) {
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->sitesurveyctrl.traffic_busy)
+			; /* dummy branch */
+		else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedScanBlinkInProgress = true;
+			pLed->CurrLedState = LED_SCAN_BLINK;
+			pLed->BlinkTimes = 24;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress &&
+		    check_fwstate(pmlmepriv, _FW_LINKED)) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+
+	case LED_CTL_LINK:
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+
+	case LED_CTL_START_WPS: /*wait until xinpin finish*/
+	case LED_CTL_START_WPS_BOTTON:
+		if (!pLed->bLedWPSBlinkInProgress) {
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedScanBlinkInProgress = false;
+			}
+			pLed->bLedWPSBlinkInProgress = true;
+			pLed->CurrLedState = LED_STATE_ON;
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
+		}
+		break;
+
+	case LED_CTL_STOP_WPS:
+		pLed->bLedWPSBlinkInProgress = false;
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+
+	case LED_CTL_STOP_WPS_FAIL:
+		pLed->bLedWPSBlinkInProgress = false;
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+
+	case LED_CTL_START_TO_LINK:
+	case LED_CTL_NO_LINK:
+		if (!IS_LED_BLINKING(pLed)) {
+			pLed->CurrLedState = LED_STATE_OFF;
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
+		}
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedControlMode3(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+
+	switch (LedAction) {
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->sitesurveyctrl.traffic_busy)
+			; /* dummy branch */
+		else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedScanBlinkInProgress = true;
+			pLed->CurrLedState = LED_SCAN_BLINK;
+			pLed->BlinkTimes = 24;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress &&
+		    check_fwstate(pmlmepriv, _FW_LINKED)) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_LINK:
+		if (IS_LED_WPS_BLINKING(pLed))
+			return;
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	case LED_CTL_START_WPS: /* wait until xinpin finish */
+	case LED_CTL_START_WPS_BOTTON:
+		if (!pLed->bLedWPSBlinkInProgress) {
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedScanBlinkInProgress = false;
+			}
+			pLed->bLedWPSBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_WPS;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_STOP_WPS:
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		} else {
+			pLed->bLedWPSBlinkInProgress = true;
+		}
+		pLed->CurrLedState = LED_BLINK_WPS_STOP;
+		if (pLed->bLedOn) {
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
+		} else {
+			pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
+		}
+		break;
+	case LED_CTL_STOP_WPS_FAIL:
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	case LED_CTL_START_TO_LINK:
+	case LED_CTL_NO_LINK:
+		if (!IS_LED_BLINKING(pLed)) {
+			pLed->CurrLedState = LED_STATE_OFF;
+			pLed->BlinkingLedState = LED_STATE_OFF;
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
+		}
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedControlMode4(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+	struct LED_871x *pLed1 = &ledpriv->SwLed1;
+
+	switch (LedAction) {
+	case LED_CTL_START_TO_LINK:
+		if (pLed1->bLedWPSBlinkInProgress) {
+			pLed1->bLedWPSBlinkInProgress = false;
+			del_timer(&pLed1->BlinkTimer);
+			pLed1->BlinkingLedState = LED_STATE_OFF;
+			pLed1->CurrLedState = LED_STATE_OFF;
+			if (pLed1->bLedOn)
+				mod_timer(&pLed->BlinkTimer,
+					  jiffies + msecs_to_jiffies(0));
+		}
+		if (!pLed->bLedStartToLinkBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			pLed->bLedStartToLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_StartToBlink;
+			if (pLed->bLedOn) {
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+			} else {
+				pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+			}
+		}
+		break;
+	case LED_CTL_LINK:
+	case LED_CTL_NO_LINK:
+		/*LED1 settings*/
+		if (LedAction == LED_CTL_LINK) {
+			if (pLed1->bLedWPSBlinkInProgress) {
+				pLed1->bLedWPSBlinkInProgress = false;
+				del_timer(&pLed1->BlinkTimer);
+				pLed1->BlinkingLedState = LED_STATE_OFF;
+				pLed1->CurrLedState = LED_STATE_OFF;
+				if (pLed1->bLedOn)
+					mod_timer(&pLed->BlinkTimer,
+						  jiffies + msecs_to_jiffies(0));
+			}
+		}
+		if (!pLed->bLedNoLinkBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedNoLinkBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_SLOWLY;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->sitesurveyctrl.traffic_busy &&
+		    check_fwstate(pmlmepriv, _FW_LINKED))
+			;
+		else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedScanBlinkInProgress = true;
+			pLed->CurrLedState = LED_SCAN_BLINK;
+			pLed->BlinkTimes = 24;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK ||
+			    IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_START_WPS: /*wait until xinpin finish*/
+	case LED_CTL_START_WPS_BOTTON:
+		if (pLed1->bLedWPSBlinkInProgress) {
+			pLed1->bLedWPSBlinkInProgress = false;
+			del_timer(&pLed1->BlinkTimer);
+			pLed1->BlinkingLedState = LED_STATE_OFF;
+			pLed1->CurrLedState = LED_STATE_OFF;
+			if (pLed1->bLedOn)
+				mod_timer(&pLed->BlinkTimer,
+					  jiffies + msecs_to_jiffies(0));
+		}
+		if (!pLed->bLedWPSBlinkInProgress) {
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedScanBlinkInProgress = false;
+			}
+			pLed->bLedWPSBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_WPS;
+			if (pLed->bLedOn) {
+				pLed->BlinkingLedState = LED_STATE_OFF;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
+			} else {
+				pLed->BlinkingLedState = LED_STATE_ON;
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+			}
+		}
+		break;
+	case LED_CTL_STOP_WPS:	/*WPS connect success*/
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->bLedNoLinkBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		break;
+	case LED_CTL_STOP_WPS_FAIL:	/*WPS authentication fail*/
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->bLedNoLinkBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		/*LED1 settings*/
+		if (pLed1->bLedWPSBlinkInProgress)
+			del_timer(&pLed1->BlinkTimer);
+		else
+			pLed1->bLedWPSBlinkInProgress = true;
+		pLed1->CurrLedState = LED_BLINK_WPS_STOP;
+		if (pLed1->bLedOn)
+			pLed1->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed1->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		break;
+	case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/*WPS session overlap*/
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->bLedNoLinkBlinkInProgress = true;
+		pLed->CurrLedState = LED_BLINK_SLOWLY;
+		if (pLed->bLedOn)
+			pLed->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
+		/*LED1 settings*/
+		if (pLed1->bLedWPSBlinkInProgress)
+			del_timer(&pLed1->BlinkTimer);
+		else
+			pLed1->bLedWPSBlinkInProgress = true;
+		pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
+		pLed1->BlinkTimes = 10;
+		if (pLed1->bLedOn)
+			pLed1->BlinkingLedState = LED_STATE_OFF;
+		else
+			pLed1->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedNoLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedNoLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedLinkBlinkInProgress = false;
+		}
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		if (pLed->bLedScanBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedScanBlinkInProgress = false;
+		}
+		if (pLed->bLedStartToLinkBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedStartToLinkBlinkInProgress = false;
+		}
+		if (pLed1->bLedWPSBlinkInProgress) {
+			del_timer(&pLed1->BlinkTimer);
+			pLed1->bLedWPSBlinkInProgress = false;
+		}
+		pLed1->BlinkingLedState = LED_UNKNOWN;
+		SwLedOff(padapter, pLed);
+		SwLedOff(padapter, pLed1);
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedControlMode5(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+
+	if (padapter->eeprompriv.CustomerID == RT_CID_819x_CAMEO)
+		pLed = &ledpriv->SwLed1;
+
+	switch (LedAction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_NO_LINK:
+	case LED_CTL_LINK:	/* solid blue */
+		if (pLed->CurrLedState == LED_SCAN_BLINK)
+			return;
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		pLed->bLedBlinkInProgress = false;
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->sitesurveyctrl.traffic_busy &&
+		    check_fwstate(pmlmepriv, _FW_LINKED))
+			; /* dummy branch */
+		else if (!pLed->bLedScanBlinkInProgress) {
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedScanBlinkInProgress = true;
+			pLed->CurrLedState = LED_SCAN_BLINK;
+			pLed->BlinkTimes = 24;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress) {
+			if (pLed->CurrLedState == LED_SCAN_BLINK)
+				return;
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		SwLedOff(padapter, pLed);
+		break;
+	default:
+		break;
+	}
+}
+
+static void SwLedControlMode6(struct _adapter *padapter,
+			      enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &ledpriv->SwLed0;
+
+	switch (LedAction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_NO_LINK:
+	case LED_CTL_LINK:	/*solid blue*/
+	case LED_CTL_SITE_SURVEY:
+		if (IS_LED_WPS_BLINKING(pLed))
+			return;
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		pLed->bLedBlinkInProgress = false;
+		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(0));
+		break;
+	case LED_CTL_TX:
+	case LED_CTL_RX:
+		if (!pLed->bLedBlinkInProgress &&
+		    check_fwstate(pmlmepriv, _FW_LINKED)) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+			pLed->bLedBlinkInProgress = true;
+			pLed->CurrLedState = LED_TXRX_BLINK;
+			pLed->BlinkTimes = 2;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_START_WPS: /*wait until xinpin finish*/
+	case LED_CTL_START_WPS_BOTTON:
+		if (!pLed->bLedWPSBlinkInProgress) {
+			if (pLed->bLedBlinkInProgress) {
+				del_timer(&pLed->BlinkTimer);
+				pLed->bLedBlinkInProgress = false;
+			}
+			pLed->bLedWPSBlinkInProgress = true;
+			pLed->CurrLedState = LED_BLINK_WPS;
+			if (pLed->bLedOn)
+				pLed->BlinkingLedState = LED_STATE_OFF;
+			else
+				pLed->BlinkingLedState = LED_STATE_ON;
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
+		}
+		break;
+	case LED_CTL_STOP_WPS_FAIL:
+	case LED_CTL_STOP_WPS:
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		pLed->CurrLedState = LED_STATE_ON;
+		pLed->BlinkingLedState = LED_STATE_ON;
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
+		break;
+	case LED_CTL_POWER_OFF:
+		pLed->CurrLedState = LED_STATE_OFF;
+		pLed->BlinkingLedState = LED_STATE_OFF;
+		if (pLed->bLedBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedBlinkInProgress = false;
+		}
+		if (pLed->bLedWPSBlinkInProgress) {
+			del_timer(&pLed->BlinkTimer);
+			pLed->bLedWPSBlinkInProgress = false;
+		}
+		SwLedOff(padapter, pLed);
+		break;
+	default:
+		break;
+	}
+}
+
+/*	Description:
+ *		Dispatch LED action according to pHalData->LedStrategy.
+ */
+void LedControl871x(struct _adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv	*ledpriv = &padapter->ledpriv;
+
+	if (!ledpriv->bRegUseLed)
+		return;
+	switch (ledpriv->LedStrategy) {
+	case SW_LED_MODE0:
+		break;
+	case SW_LED_MODE1:
+		SwLedControlMode1(padapter, LedAction);
+		break;
+	case SW_LED_MODE2:
+		SwLedControlMode2(padapter, LedAction);
+		break;
+	case SW_LED_MODE3:
+		SwLedControlMode3(padapter, LedAction);
+		break;
+	case SW_LED_MODE4:
+		SwLedControlMode4(padapter, LedAction);
+		break;
+	case SW_LED_MODE5:
+		SwLedControlMode5(padapter, LedAction);
+		break;
+	case SW_LED_MODE6:
+		SwLedControlMode6(padapter, LedAction);
+		break;
+	default:
+		break;
+	}
+}
+
+void r8712_flush_led_works(struct _adapter *padapter)
+{
+	struct led_priv *pledpriv = &padapter->ledpriv;
+
+	flush_work(&pledpriv->SwLed0.BlinkWorkItem);
+	flush_work(&pledpriv->SwLed1.BlinkWorkItem);
+}
diff --git a/drivers/staging/rtl8712/rtl8712_macsetting_bitdef.h b/drivers/staging/rtl8712/rtl8712_macsetting_bitdef.h
new file mode 100644
index 000000000..46d758d3f
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_macsetting_bitdef.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_MACSETTING_BITDEF_H__
+#define __RTL8712_MACSETTING_BITDEF_H__
+
+/*MACID*/
+/*BSSID*/
+
+/*HWVID*/
+#define	_HWVID_MSK				0x0F
+
+/*MAR*/
+/*MBIDCANCONTENT*/
+
+/*MBIDCANCFG*/
+#define	_POOLING				BIT(31)
+#define	_WRITE_EN				BIT(16)
+#define	_CAM_ADDR_MSK			0x001F
+#define	_CAM_ADDR_SHT			0
+
+/*BUILDTIME*/
+#define _BUILDTIME_MSK			0x3FFFFFFF
+
+/*BUILDUSER*/
+
+#endif /* __RTL8712_MACSETTING_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_macsetting_regdef.h b/drivers/staging/rtl8712/rtl8712_macsetting_regdef.h
new file mode 100644
index 000000000..64740d99c
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_macsetting_regdef.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_MACSETTING_REGDEF_H__
+#define __RTL8712_MACSETTING_REGDEF_H__
+
+#define MACID				(RTL8712_MACIDSETTING_ + 0x0000)
+#define BSSIDR				(RTL8712_MACIDSETTING_ + 0x0008)
+#define HWVID				(RTL8712_MACIDSETTING_ + 0x000E)
+#define MAR				(RTL8712_MACIDSETTING_ + 0x0010)
+#define MBIDCANCONTENT			(RTL8712_MACIDSETTING_ + 0x0018)
+#define MBIDCANCFG			(RTL8712_MACIDSETTING_ + 0x0020)
+#define BUILDTIME			(RTL8712_MACIDSETTING_ + 0x0024)
+#define BUILDUSER			(RTL8712_MACIDSETTING_ + 0x0028)
+
+#endif /*__RTL8712_MACSETTING_REGDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_powersave_bitdef.h b/drivers/staging/rtl8712/rtl8712_powersave_bitdef.h
new file mode 100644
index 000000000..53e0d6b44
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_powersave_bitdef.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_POWERSAVE_BITDEF_H__
+#define __RTL8712_POWERSAVE_BITDEF_H__
+
+/*WOWCTRL*/
+#define	_UWF			BIT(3)
+#define	_MAGIC			BIT(2)
+#define	_WOW_EN			BIT(1)
+#define	_PMEN			BIT(0)
+
+/*PSSTATUS*/
+#define	_PSSTATUS_SEL_MSK		0x0F
+
+/*PSSWITCH*/
+#define	_PSSWITCH_ACT			BIT(7)
+#define	_PSSWITCH_SEL_MSK		0x0F
+#define	_PSSWITCH_SEL_SHT		0
+
+/*LPNAV_CTRL*/
+#define	_LPNAV_EN			BIT(31)
+#define	_LPNAV_EARLY_MSK		0x7FFF0000
+#define	_LPNAV_EARLY_SHT		16
+#define	_LPNAV_TH_MSK			0x0000FFFF
+#define	_LPNAV_TH_SHT			0
+
+/*RPWM*/
+/*CPWM*/
+#define	_TOGGLING			BIT(7)
+#define	_WWLAN				BIT(3)
+#define	_RPS_ST				BIT(2)
+#define	_WLAN_TRX			BIT(1)
+#define	_SYS_CLK			BIT(0)
+
+#endif /* __RTL8712_POWERSAVE_BITDEF_H__*/
diff --git a/drivers/staging/rtl8712/rtl8712_powersave_regdef.h b/drivers/staging/rtl8712/rtl8712_powersave_regdef.h
new file mode 100644
index 000000000..1bcfde4b1
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_powersave_regdef.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_POWERSAVE_REGDEF_H__
+#define __RTL8712_POWERSAVE_REGDEF_H__
+
+#define WOWCTRL				(RTL8712_POWERSAVE_ + 0x00)
+#define PSSTATUS			(RTL8712_POWERSAVE_ + 0x01)
+#define PSSWITCH			(RTL8712_POWERSAVE_ + 0x02)
+#define MIMOPS_WAITPERIOD		(RTL8712_POWERSAVE_ + 0x03)
+#define LPNAV_CTRL			(RTL8712_POWERSAVE_ + 0x04)
+#define WFM0				(RTL8712_POWERSAVE_ + 0x10)
+#define WFM1				(RTL8712_POWERSAVE_ + 0x20)
+#define WFM2				(RTL8712_POWERSAVE_ + 0x30)
+#define WFM3				(RTL8712_POWERSAVE_ + 0x40)
+#define WFM4				(RTL8712_POWERSAVE_ + 0x50)
+#define WFM5				(RTL8712_POWERSAVE_ + 0x60)
+#define WFCRC				(RTL8712_POWERSAVE_ + 0x70)
+#define RPWM				(RTL8712_POWERSAVE_ + 0x7C)
+#define CPWM				(RTL8712_POWERSAVE_ + 0x7D)
+
+#endif /* __RTL8712_POWERSAVE_REGDEF_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_ratectrl_bitdef.h b/drivers/staging/rtl8712/rtl8712_ratectrl_bitdef.h
new file mode 100644
index 000000000..1de51c48f
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_ratectrl_bitdef.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_RATECTRL_BITDEF_H__
+#define __RTL8712_RATECTRL_BITDEF_H__
+
+/*INIRTSMCS_SEL*/
+#define	_INIRTSMCS_SEL_MSK		0x3F
+
+/* RRSR*/
+#define	_RRSR_SHORT			BIT(23)
+#define	_RRSR_RSC_MSK		0x600000
+#define	_RRSR_RSC_SHT		21
+#define	_RRSR_BITMAP_MSK	0x0FFFFF
+#define	_RRSR_BITMAP_SHT	0
+
+/* AGGLEN_LMT_H*/
+#define	_AGGLMT_MCS32_MSK			0xF0
+#define	_AGGLMT_MCS32_SHT			4
+#define	_AGGLMT_MCS15_SGI_MSK		0x0F
+#define	_AGGLMT_MCS15_SGI_SHT		0
+
+/* DARFRC*/
+/* RARFRC*/
+/* MCS_TXAGC*/
+/* CCK_TXAGC*/
+#define	_CCK_MSK			0xFF00
+#define	_CCK_SHT			8
+#define	_BARKER_MSK			0x00FF
+#define	_BARKER_SHT			0
+
+#endif	/*	__RTL8712_RATECTRL_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_ratectrl_regdef.h b/drivers/staging/rtl8712/rtl8712_ratectrl_regdef.h
new file mode 100644
index 000000000..9ed5653f3
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_ratectrl_regdef.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_RATECTRL_REGDEF_H__
+#define __RTL8712_RATECTRL_REGDEF_H__
+
+#define INIMCS_SEL			(RTL8712_RATECTRL_ + 0x00)
+#define INIRTSMCS_SEL		(RTL8712_RATECTRL_ + 0x20)
+#define RRSR				(RTL8712_RATECTRL_ + 0x21)
+#define ARFR0				(RTL8712_RATECTRL_ + 0x24)
+#define ARFR1				(RTL8712_RATECTRL_ + 0x28)
+#define ARFR2				(RTL8712_RATECTRL_ + 0x2C)
+#define ARFR3				(RTL8712_RATECTRL_ + 0x30)
+#define ARFR4				(RTL8712_RATECTRL_ + 0x34)
+#define ARFR5				(RTL8712_RATECTRL_ + 0x38)
+#define ARFR6				(RTL8712_RATECTRL_ + 0x3C)
+#define ARFR7				(RTL8712_RATECTRL_ + 0x40)
+#define AGGLEN_LMT_H		(RTL8712_RATECTRL_ + 0x47)
+#define AGGLEN_LMT_L		(RTL8712_RATECTRL_ + 0x48)
+#define DARFRC				(RTL8712_RATECTRL_ + 0x50)
+#define RARFRC				(RTL8712_RATECTRL_ + 0x58)
+#define MCS_TXAGC0			(RTL8712_RATECTRL_ + 0x60)
+#define MCS_TXAGC1			(RTL8712_RATECTRL_ + 0x61)
+#define MCS_TXAGC2			(RTL8712_RATECTRL_ + 0x62)
+#define MCS_TXAGC3			(RTL8712_RATECTRL_ + 0x63)
+#define MCS_TXAGC4			(RTL8712_RATECTRL_ + 0x64)
+#define MCS_TXAGC5			(RTL8712_RATECTRL_ + 0x65)
+#define MCS_TXAGC6			(RTL8712_RATECTRL_ + 0x66)
+#define MCS_TXAGC7			(RTL8712_RATECTRL_ + 0x67)
+#define CCK_TXAGC			(RTL8712_RATECTRL_ + 0x68)
+
+#endif	/*__RTL8712_RATECTRL_REGDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
new file mode 100644
index 000000000..7f1fdd058
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -0,0 +1,1079 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl8712_recv.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL8712_RECV_C_
+
+#include <linux/if_ether.h>
+#include <linux/ip.h>
+#include <net/cfg80211.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "mlme_osdep.h"
+#include "ethernet.h"
+#include "usb_ops.h"
+#include "wifi.h"
+
+static void recv_tasklet(struct tasklet_struct *t);
+
+void r8712_init_recv_priv(struct recv_priv *precvpriv,
+			  struct _adapter *padapter)
+{
+	int i;
+	struct recv_buf *precvbuf;
+	addr_t tmpaddr = 0;
+	int alignment = 0;
+	struct sk_buff *pskb = NULL;
+
+	/*init recv_buf*/
+	_init_queue(&precvpriv->free_recv_buf_queue);
+	precvpriv->pallocated_recv_buf =
+		kzalloc(NR_RECVBUFF * sizeof(struct recv_buf) + 4, GFP_ATOMIC);
+	if (!precvpriv->pallocated_recv_buf)
+		return;
+	precvpriv->precv_buf = precvpriv->pallocated_recv_buf + 4 -
+			      ((addr_t)(precvpriv->pallocated_recv_buf) & 3);
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		INIT_LIST_HEAD(&precvbuf->list);
+		spin_lock_init(&precvbuf->recvbuf_lock);
+		if (r8712_os_recvbuf_resource_alloc(padapter, precvbuf))
+			break;
+		precvbuf->ref_cnt = 0;
+		precvbuf->adapter = padapter;
+		list_add_tail(&precvbuf->list,
+			      &precvpriv->free_recv_buf_queue.queue);
+		precvbuf++;
+	}
+	precvpriv->free_recv_buf_queue_cnt = NR_RECVBUFF;
+	tasklet_setup(&precvpriv->recv_tasklet, recv_tasklet);
+	skb_queue_head_init(&precvpriv->rx_skb_queue);
+
+	skb_queue_head_init(&precvpriv->free_recv_skb_queue);
+	for (i = 0; i < NR_PREALLOC_RECV_SKB; i++) {
+		pskb = netdev_alloc_skb(padapter->pnetdev, MAX_RECVBUF_SZ +
+		       RECVBUFF_ALIGN_SZ);
+		if (pskb) {
+			tmpaddr = (addr_t)pskb->data;
+			alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+			skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignment));
+			skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+		}
+		pskb = NULL;
+	}
+}
+
+void r8712_free_recv_priv(struct recv_priv *precvpriv)
+{
+	int i;
+	struct recv_buf *precvbuf;
+	struct _adapter *padapter = precvpriv->adapter;
+
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		r8712_os_recvbuf_resource_free(padapter, precvbuf);
+		precvbuf++;
+	}
+	kfree(precvpriv->pallocated_recv_buf);
+	skb_queue_purge(&precvpriv->rx_skb_queue);
+	if (skb_queue_len(&precvpriv->rx_skb_queue))
+		netdev_warn(padapter->pnetdev, "r8712u: rx_skb_queue not empty\n");
+	skb_queue_purge(&precvpriv->free_recv_skb_queue);
+	if (skb_queue_len(&precvpriv->free_recv_skb_queue))
+		netdev_warn(padapter->pnetdev, "r8712u: free_recv_skb_queue not empty %d\n",
+			    skb_queue_len(&precvpriv->free_recv_skb_queue));
+}
+
+void r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf)
+{
+	precvbuf->transfer_len = 0;
+	precvbuf->len = 0;
+	precvbuf->ref_cnt = 0;
+	if (precvbuf->pbuf) {
+		precvbuf->pdata = precvbuf->pbuf;
+		precvbuf->phead = precvbuf->pbuf;
+		precvbuf->ptail = precvbuf->pbuf;
+		precvbuf->pend = precvbuf->pdata + MAX_RECVBUF_SZ;
+	}
+}
+
+void r8712_free_recvframe(union recv_frame *precvframe,
+			  struct  __queue *pfree_recv_queue)
+{
+	unsigned long irqL;
+	struct _adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	if (precvframe->u.hdr.pkt) {
+		dev_kfree_skb_any(precvframe->u.hdr.pkt);/*free skb by driver*/
+		precvframe->u.hdr.pkt = NULL;
+	}
+	spin_lock_irqsave(&pfree_recv_queue->lock, irqL);
+	list_del_init(&precvframe->u.hdr.list);
+	list_add_tail(&precvframe->u.hdr.list, &pfree_recv_queue->queue);
+	if (padapter) {
+		if (pfree_recv_queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+	}
+	spin_unlock_irqrestore(&pfree_recv_queue->lock, irqL);
+}
+
+static void update_recvframe_attrib_from_recvstat(struct rx_pkt_attrib *pattrib,
+						  struct recv_stat *prxstat)
+{
+	u16 drvinfo_sz;
+
+	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
+	drvinfo_sz <<= 3;
+	/*TODO:
+	 * Offset 0
+	 */
+	pattrib->bdecrypted = (le32_to_cpu(prxstat->rxdw0) & BIT(27)) == 0;
+	pattrib->crc_err = (le32_to_cpu(prxstat->rxdw0) & BIT(14)) != 0;
+	/*Offset 4*/
+	/*Offset 8*/
+	/*Offset 12*/
+	if (le32_to_cpu(prxstat->rxdw3) & BIT(13)) {
+		pattrib->tcpchk_valid = 1; /* valid */
+		if (le32_to_cpu(prxstat->rxdw3) & BIT(11))
+			pattrib->tcp_chkrpt = 1; /* correct */
+		else
+			pattrib->tcp_chkrpt = 0; /* incorrect */
+		if (le32_to_cpu(prxstat->rxdw3) & BIT(12))
+			pattrib->ip_chkrpt = 1; /* correct */
+		else
+			pattrib->ip_chkrpt = 0; /* incorrect */
+	} else {
+		pattrib->tcpchk_valid = 0; /* invalid */
+	}
+	pattrib->mcs_rate = (u8)((le32_to_cpu(prxstat->rxdw3)) & 0x3f);
+	pattrib->htc = (u8)((le32_to_cpu(prxstat->rxdw3) >> 14) & 0x1);
+	/*Offset 16*/
+	/*Offset 20*/
+	/*phy_info*/
+}
+
+/*perform defrag*/
+static union recv_frame *recvframe_defrag(struct _adapter *adapter,
+					  struct  __queue *defrag_q)
+{
+	struct list_head *plist, *phead;
+	u8 wlanhdr_offset;
+	u8	curfragnum;
+	struct recv_frame_hdr *pfhdr, *pnfhdr;
+	union recv_frame *prframe, *pnextrframe;
+	struct  __queue	*pfree_recv_queue;
+
+	pfree_recv_queue = &adapter->recvpriv.free_recv_queue;
+	phead = &defrag_q->queue;
+	plist = phead->next;
+	prframe = container_of(plist, union recv_frame, u.list);
+	list_del_init(&prframe->u.list);
+	pfhdr = &prframe->u.hdr;
+	curfragnum = 0;
+	if (curfragnum != pfhdr->attrib.frag_num) {
+		/*the first fragment number must be 0
+		 *free the whole queue
+		 */
+		r8712_free_recvframe(prframe, pfree_recv_queue);
+		r8712_free_recvframe_queue(defrag_q, pfree_recv_queue);
+		return NULL;
+	}
+	curfragnum++;
+	plist = &defrag_q->queue;
+	plist = plist->next;
+	while (!end_of_queue_search(phead, plist)) {
+		pnextrframe = container_of(plist, union recv_frame, u.list);
+		pnfhdr = &pnextrframe->u.hdr;
+		/*check the fragment sequence  (2nd ~n fragment frame) */
+		if (curfragnum != pnfhdr->attrib.frag_num) {
+			/* the fragment number must increase  (after decache)
+			 * release the defrag_q & prframe
+			 */
+			r8712_free_recvframe(prframe, pfree_recv_queue);
+			r8712_free_recvframe_queue(defrag_q, pfree_recv_queue);
+			return NULL;
+		}
+		curfragnum++;
+		/* copy the 2nd~n fragment frame's payload to the first fragment
+		 * get the 2nd~last fragment frame's payload
+		 */
+		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+		recvframe_pull(pnextrframe, wlanhdr_offset);
+		/* append  to first fragment frame's tail (if privacy frame,
+		 * pull the ICV)
+		 */
+		recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+		memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+		recvframe_put(prframe, pnfhdr->len);
+		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		plist = plist->next;
+	}
+	/* free the defrag_q queue and return the prframe */
+	r8712_free_recvframe_queue(defrag_q, pfree_recv_queue);
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+union recv_frame *r8712_recvframe_chk_defrag(struct _adapter *padapter,
+					     union recv_frame *precv_frame)
+{
+	u8	ismfrag;
+	u8	fragnum;
+	u8   *psta_addr;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct	sta_priv *pstapriv;
+	struct list_head *phead;
+	union recv_frame *prtnframe = NULL;
+	struct  __queue *pfree_recv_queue, *pdefrag_q;
+
+	pstapriv = &padapter->stapriv;
+	pfhdr = &precv_frame->u.hdr;
+	pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	/* need to define struct of wlan header frame ctrl */
+	ismfrag = pfhdr->attrib.mfrag;
+	fragnum = pfhdr->attrib.frag_num;
+	psta_addr = pfhdr->attrib.ta;
+	psta = r8712_get_stainfo(pstapriv, psta_addr);
+	if (!psta)
+		pdefrag_q = NULL;
+	else
+		pdefrag_q = &psta->sta_recvpriv.defrag_q;
+
+	if ((ismfrag == 0) && (fragnum == 0))
+		prtnframe = precv_frame;/*isn't a fragment frame*/
+	if (ismfrag == 1) {
+		/* 0~(n-1) fragment frame
+		 * enqueue to defraf_g
+		 */
+		if (pdefrag_q) {
+			if (fragnum == 0) {
+				/*the first fragment*/
+				if (!list_empty(&pdefrag_q->queue)) {
+					/*free current defrag_q */
+					r8712_free_recvframe_queue(pdefrag_q,
+							     pfree_recv_queue);
+				}
+			}
+			/* Then enqueue the 0~(n-1) fragment to the defrag_q */
+			phead = &pdefrag_q->queue;
+			list_add_tail(&pfhdr->list, phead);
+			prtnframe = NULL;
+		} else {
+			/* can't find this ta's defrag_queue, so free this
+			 * recv_frame
+			 */
+			r8712_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame
+		 * enqueue the last fragment
+		 */
+		if (pdefrag_q) {
+			phead = &pdefrag_q->queue;
+			list_add_tail(&pfhdr->list, phead);
+			/*call recvframe_defrag to defrag*/
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe = precv_frame;
+		} else {
+			/* can't find this ta's defrag_queue, so free this
+			 *  recv_frame
+			 */
+			r8712_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	if (prtnframe && (prtnframe->u.hdr.attrib.privacy)) {
+		/* after defrag we must check tkip mic code */
+		if (r8712_recvframe_chkmic(padapter, prtnframe) == _FAIL) {
+			r8712_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	return prtnframe;
+}
+
+static void amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
+{
+	int	a_len, padding_len;
+	u16	eth_type, nSubframe_Length;
+	u8	nr_subframes, i;
+	unsigned char *pdata;
+	struct rx_pkt_attrib *pattrib;
+	_pkt *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct  __queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+
+	nr_subframes = 0;
+	pattrib = &prframe->u.hdr.attrib;
+	recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen);
+	if (prframe->u.hdr.attrib.iv_len > 0)
+		recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len);
+	a_len = prframe->u.hdr.len;
+	pdata = prframe->u.hdr.rx_data;
+	while (a_len > ETH_HLEN) {
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = *((u16 *)(pdata + 12));
+		/*==m==>change the length order*/
+		nSubframe_Length = (nSubframe_Length >> 8) +
+				   (nSubframe_Length << 8);
+		if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			netdev_warn(padapter->pnetdev, "r8712u: nRemain_Length is %d and nSubframe_Length is: %d\n",
+				    a_len, nSubframe_Length);
+			goto exit;
+		}
+		/* move the data point to data content */
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+		/* Allocate new skb for releasing to upper layer */
+		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if (!sub_skb)
+			break;
+		skb_reserve(sub_skb, 12);
+		skb_put_data(sub_skb, pdata, nSubframe_Length);
+		subframes[nr_subframes++] = sub_skb;
+		if (nr_subframes >= MAX_SUBFRAME_COUNT) {
+			netdev_warn(padapter->pnetdev, "r8712u: ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
+		}
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if (a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & 3);
+			if (padding_len == 4)
+				padding_len = 0;
+			if (a_len < padding_len)
+				goto exit;
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+	for (i = 0; i < nr_subframes; i++) {
+		sub_skb = subframes[i];
+		/* convert hdr + possible LLC headers into Ethernet header */
+		eth_type = (sub_skb->data[6] << 8) | sub_skb->data[7];
+		if (sub_skb->len >= 8 &&
+		    ((!memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) &&
+		      eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+		     !memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE))) {
+			/* remove RFC1042 or Bridge-Tunnel encapsulation and
+			 * replace EtherType
+			 */
+			skb_pull(sub_skb, SNAP_SIZE);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src,
+			       ETH_ALEN);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst,
+			       ETH_ALEN);
+		} else {
+			__be16 len;
+			/* Leave Ethernet header part of hdr and full payload */
+			len = htons(sub_skb->len);
+			memcpy(skb_push(sub_skb, 2), &len, 2);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src,
+			       ETH_ALEN);
+			memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst,
+			       ETH_ALEN);
+		}
+		/* Indicate the packets to upper layer */
+		if (sub_skb) {
+			sub_skb->protocol =
+				 eth_type_trans(sub_skb, padapter->pnetdev);
+			sub_skb->dev = padapter->pnetdev;
+			if ((pattrib->tcpchk_valid == 1) &&
+			    (pattrib->tcp_chkrpt == 1)) {
+				sub_skb->ip_summed = CHECKSUM_UNNECESSARY;
+			} else {
+				sub_skb->ip_summed = CHECKSUM_NONE;
+			}
+			netif_rx(sub_skb);
+		}
+	}
+exit:
+	prframe->u.hdr.len = 0;
+	r8712_free_recvframe(prframe, pfree_recv_queue);
+}
+
+void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
+{
+	__le32 voffset;
+	u8 *poffset;
+	u16 cmd_len, drvinfo_sz;
+	struct recv_stat *prxstat;
+
+	poffset = prxcmdbuf;
+	voffset = *(__le32 *)poffset;
+	prxstat = prxcmdbuf;
+	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
+	drvinfo_sz <<= 3;
+	poffset += RXDESC_SIZE + drvinfo_sz;
+	do {
+		voffset  = *(__le32 *)poffset;
+		cmd_len = (u16)(le32_to_cpu(voffset) & 0xffff);
+		r8712_event_handle(padapter, (__le32 *)poffset);
+		poffset += (cmd_len + 8);/*8 bytes alignment*/
+	} while (le32_to_cpu(voffset) & BIT(31));
+}
+
+static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl,
+			      u16 seq_num)
+{
+	u8 wsize = preorder_ctrl->wsize_b;
+	u16 wend = (preorder_ctrl->indicate_seq + wsize - 1) % 4096;
+
+	/* Rx Reorder initialize condition.*/
+	if (preorder_ctrl->indicate_seq == 0xffff)
+		preorder_ctrl->indicate_seq = seq_num;
+	/* Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(seq_num, preorder_ctrl->indicate_seq))
+		return false;
+	/*
+	 * Sliding window manipulation. Conditions includes:
+	 * 1. Incoming SeqNum is equal to WinStart =>Window shift 1
+	 * 2. Incoming SeqNum is larger than the WinEnd => Window shift N
+	 */
+	if (SN_EQUAL(seq_num, preorder_ctrl->indicate_seq))
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq +
+					      1) % 4096;
+	else if (SN_LESS(wend, seq_num)) {
+		if (seq_num >= (wsize - 1))
+			preorder_ctrl->indicate_seq = seq_num + 1 - wsize;
+		else
+			preorder_ctrl->indicate_seq = 4095 - (wsize -
+						      (seq_num + 1)) + 1;
+	}
+	return true;
+}
+
+static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl,
+				     union recv_frame *prframe)
+{
+	struct list_head *phead, *plist;
+	union recv_frame *pnextrframe;
+	struct rx_pkt_attrib *pnextattrib;
+	struct  __queue *ppending_recvframe_queue =
+					&preorder_ctrl->pending_recvframe_queue;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+
+	phead = &ppending_recvframe_queue->queue;
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist)) {
+		pnextrframe = container_of(plist, union recv_frame, u.list);
+		pnextattrib = &pnextrframe->u.hdr.attrib;
+
+		if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+			return false;
+
+		if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+			plist = plist->next;
+		else
+			break;
+	}
+	list_del_init(&prframe->u.hdr.list);
+	list_add_tail(&prframe->u.hdr.list, plist);
+	return true;
+}
+
+int r8712_recv_indicatepkts_in_order(struct _adapter *padapter,
+				     struct recv_reorder_ctrl *preorder_ctrl,
+				     int bforced)
+{
+	struct list_head *phead, *plist;
+	union recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	int bPktInBuf = false;
+	struct  __queue *ppending_recvframe_queue =
+			 &preorder_ctrl->pending_recvframe_queue;
+
+	phead = &ppending_recvframe_queue->queue;
+	plist = phead->next;
+	/* Handling some condition for forced indicate case.*/
+	if (bforced) {
+		if (list_empty(phead))
+			return true;
+
+		prframe = container_of(plist, union recv_frame, u.list);
+		pattrib = &prframe->u.hdr.attrib;
+		preorder_ctrl->indicate_seq = pattrib->seq_num;
+	}
+	/* Prepare indication list and indication.
+	 * Check if there is any packet need indicate.
+	 */
+	while (!list_empty(phead)) {
+		prframe = container_of(plist, union recv_frame, u.list);
+		pattrib = &prframe->u.hdr.attrib;
+		if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+			plist = plist->next;
+			list_del_init(&prframe->u.hdr.list);
+			if (SN_EQUAL(preorder_ctrl->indicate_seq,
+				     pattrib->seq_num))
+				preorder_ctrl->indicate_seq =
+				  (preorder_ctrl->indicate_seq + 1) % 4096;
+			/*indicate this recv_frame*/
+			if (!pattrib->amsdu) {
+				if (!padapter->driver_stopped &&
+				    !padapter->surprise_removed) {
+					/* indicate this recv_frame */
+					r8712_recv_indicatepkt(padapter,
+							       prframe);
+				}
+			} else if (pattrib->amsdu == 1) {
+				amsdu_to_msdu(padapter, prframe);
+			}
+			/* Update local variables. */
+			bPktInBuf = false;
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+	}
+	return bPktInBuf;
+}
+
+static int recv_indicatepkt_reorder(struct _adapter *padapter,
+				    union recv_frame *prframe)
+{
+	unsigned long irql;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl;
+	struct  __queue *ppending_recvframe_queue =
+			 &preorder_ctrl->pending_recvframe_queue;
+
+	if (!pattrib->amsdu) {
+		/* s1. */
+		r8712_wlanhdr_to_ethhdr(prframe);
+		if (pattrib->qos != 1) {
+			if (!padapter->driver_stopped &&
+			    !padapter->surprise_removed) {
+				r8712_recv_indicatepkt(padapter, prframe);
+				return 0;
+			} else {
+				return -EINVAL;
+			}
+		}
+	}
+	spin_lock_irqsave(&ppending_recvframe_queue->lock, irql);
+	/*s2. check if winstart_b(indicate_seq) needs to be updated*/
+	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num))
+		goto _err_exit;
+	/*s3. Insert all packet into Reorder Queue to maintain its ordering.*/
+	if (!enqueue_reorder_recvframe(preorder_ctrl, prframe))
+		goto _err_exit;
+	/*s4.
+	 * Indication process.
+	 * After Packet dropping and Sliding Window shifting as above, we can
+	 * now just indicate the packets with the SeqNum smaller than latest
+	 * WinStart and buffer other packets.
+	 *
+	 * For Rx Reorder condition:
+	 * 1. All packets with SeqNum smaller than WinStart => Indicate
+	 * 2. All packets with SeqNum larger than or equal to
+	 * WinStart => Buffer it.
+	 */
+	if (r8712_recv_indicatepkts_in_order(padapter, preorder_ctrl, false)) {
+		mod_timer(&preorder_ctrl->reordering_ctrl_timer,
+			  jiffies + msecs_to_jiffies(REORDER_WAIT_TIME));
+		spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
+	} else {
+		spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
+		del_timer(&preorder_ctrl->reordering_ctrl_timer);
+	}
+	return 0;
+_err_exit:
+	spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
+	return -ENOMEM;
+}
+
+void r8712_reordering_ctrl_timeout_handler(void *pcontext)
+{
+	unsigned long irql;
+	struct recv_reorder_ctrl *preorder_ctrl = pcontext;
+	struct _adapter *padapter = preorder_ctrl->padapter;
+	struct  __queue *ppending_recvframe_queue =
+				 &preorder_ctrl->pending_recvframe_queue;
+
+	if (padapter->driver_stopped || padapter->surprise_removed)
+		return;
+	spin_lock_irqsave(&ppending_recvframe_queue->lock, irql);
+	r8712_recv_indicatepkts_in_order(padapter, preorder_ctrl, true);
+	spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
+}
+
+static int r8712_process_recv_indicatepkts(struct _adapter *padapter,
+					   union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
+
+	if (phtpriv->ht_option == 1) { /*B/G/N Mode*/
+		if (recv_indicatepkt_reorder(padapter, prframe)) {
+			/* including perform A-MPDU Rx Ordering Buffer Control*/
+			if (!padapter->driver_stopped &&
+			    !padapter->surprise_removed)
+				return _FAIL;
+		}
+	} else { /*B/G mode*/
+		retval = r8712_wlanhdr_to_ethhdr(prframe);
+		if (retval)
+			return _FAIL;
+		if (!padapter->driver_stopped && !padapter->surprise_removed) {
+			/* indicate this recv_frame */
+			r8712_recv_indicatepkt(padapter, prframe);
+		} else {
+			return _FAIL;
+		}
+	}
+	return retval;
+}
+
+static u8 query_rx_pwr_percentage(s8 antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return	0;
+	else if (antpower >= 0)
+		return	100;
+	else
+		return 100 + antpower;
+}
+
+static u8 evm_db2percentage(s8 value)
+{
+	/*
+	 * -33dB~0dB to 0%~99%
+	 */
+	s8 ret_val = clamp(-value, 0, 33) * 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+s32 r8712_signal_scale_mapping(s32 cur_sig)
+{
+	s32 ret_sig;
+
+	if (cur_sig >= 51 && cur_sig <= 100)
+		ret_sig = 100;
+	else if (cur_sig >= 41 && cur_sig <= 50)
+		ret_sig = 80 + ((cur_sig - 40) * 2);
+	else if (cur_sig >= 31 && cur_sig <= 40)
+		ret_sig = 66 + (cur_sig - 30);
+	else if (cur_sig >= 21 && cur_sig <= 30)
+		ret_sig = 54 + (cur_sig - 20);
+	else if (cur_sig >= 10 && cur_sig <= 20)
+		ret_sig = 42 + (((cur_sig - 10) * 2) / 3);
+	else if (cur_sig >= 5 && cur_sig <= 9)
+		ret_sig = 22 + (((cur_sig - 5) * 3) / 2);
+	else if (cur_sig >= 1 && cur_sig <= 4)
+		ret_sig = 6 + (((cur_sig - 1) * 3) / 2);
+	else
+		ret_sig = cur_sig;
+	return ret_sig;
+}
+
+static s32  translate2dbm(struct _adapter *padapter, u8 signal_strength_idx)
+{
+	s32 signal_power; /* in dBm.*/
+	/* Translate to dBm (x=0.5y-95).*/
+	signal_power = (s32)((signal_strength_idx + 1) >> 1);
+	signal_power -= 95;
+	return signal_power;
+}
+
+static void query_rx_phy_status(struct _adapter *padapter,
+				union recv_frame *prframe)
+{
+	u8 i, max_spatial_stream, evm;
+	struct recv_stat *prxstat = (struct recv_stat *)prframe->u.hdr.rx_head;
+	struct phy_stat *pphy_stat = (struct phy_stat *)(prxstat + 1);
+	u8 *pphy_head = (u8 *)(prxstat + 1);
+	s8 rx_pwr[4], rx_pwr_all;
+	u8 pwdb_all;
+	u32 rssi, total_rssi = 0;
+	u8 bcck_rate = 0, rf_rx_num = 0, cck_highpwr = 0;
+	struct phy_cck_rx_status *pcck_buf;
+	u8 sq;
+
+	/* Record it for next packet processing*/
+	bcck_rate = (prframe->u.hdr.attrib.mcs_rate <= 3 ? 1 : 0);
+	if (bcck_rate) {
+		u8 report;
+
+		/* CCK Driver info Structure is not the same as OFDM packet.*/
+		pcck_buf = (struct phy_cck_rx_status *)pphy_stat;
+		/* (1)Hardware does not provide RSSI for CCK
+		 * (2)PWDB, Average PWDB calculated by hardware
+		 * (for rate adaptive)
+		 */
+		if (!cck_highpwr) {
+			report = pcck_buf->cck_agc_rpt & 0xc0;
+			report >>= 6;
+			switch (report) {
+			/* Modify the RF RNA gain value to -40, -20,
+			 * -2, 14 by Jenyu's suggestion
+			 * Note: different RF with the different
+			 * RNA gain.
+			 */
+			case 0x3:
+				rx_pwr_all = -40 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x2:
+				rx_pwr_all = -20 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x1:
+				rx_pwr_all = -2 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			case 0x0:
+				rx_pwr_all = 14 - (pcck_buf->cck_agc_rpt &
+					     0x3e);
+				break;
+			}
+		} else {
+			report = ((u8)(le32_to_cpu(pphy_stat->phydw1) >> 8)) &
+				 0x60;
+			report >>= 5;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -40 - ((pcck_buf->cck_agc_rpt &
+					     0x1f) << 1);
+				break;
+			case 0x2:
+				rx_pwr_all = -20 - ((pcck_buf->cck_agc_rpt &
+					     0x1f) << 1);
+				break;
+			case 0x1:
+				rx_pwr_all = -2 - ((pcck_buf->cck_agc_rpt &
+					     0x1f) << 1);
+				break;
+			case 0x0:
+				rx_pwr_all = 14 - ((pcck_buf->cck_agc_rpt &
+					     0x1f) << 1);
+				break;
+			}
+		}
+		pwdb_all = query_rx_pwr_percentage(rx_pwr_all);
+		/* CCK gain is smaller than OFDM/MCS gain,*/
+		/* so we add gain diff by experiences, the val is 6 */
+		pwdb_all += 6;
+		if (pwdb_all > 100)
+			pwdb_all = 100;
+		/* modify the offset to make the same gain index with OFDM.*/
+		if (pwdb_all > 34 && pwdb_all <= 42)
+			pwdb_all -= 2;
+		else if (pwdb_all > 26 && pwdb_all <= 34)
+			pwdb_all -= 6;
+		else if (pwdb_all > 14 && pwdb_all <= 26)
+			pwdb_all -= 8;
+		else if (pwdb_all > 4 && pwdb_all <= 14)
+			pwdb_all -= 4;
+		/*
+		 * (3) Get Signal Quality (EVM)
+		 */
+		if (pwdb_all > 40) {
+			sq = 100;
+		} else {
+			sq = pcck_buf->sq_rpt;
+			if (pcck_buf->sq_rpt > 64)
+				sq = 0;
+			else if (pcck_buf->sq_rpt < 20)
+				sq = 100;
+			else
+				sq = ((64 - sq) * 100) / 44;
+		}
+		prframe->u.hdr.attrib.signal_qual = sq;
+		prframe->u.hdr.attrib.rx_mimo_signal_qual[0] = sq;
+		prframe->u.hdr.attrib.rx_mimo_signal_qual[1] = -1;
+	} else {
+		/* (1)Get RSSI for HT rate */
+		for (i = 0; i < ((padapter->registrypriv.rf_config) &
+			    0x0f); i++) {
+			rf_rx_num++;
+			rx_pwr[i] = ((pphy_head[PHY_STAT_GAIN_TRSW_SHT + i]
+				    & 0x3F) * 2) - 110;
+			/* Translate DBM to percentage. */
+			rssi = query_rx_pwr_percentage(rx_pwr[i]);
+			total_rssi += rssi;
+		}
+		/* (2)PWDB, Average PWDB calculated by hardware (for
+		 * rate adaptive)
+		 */
+		rx_pwr_all = (((pphy_head[PHY_STAT_PWDB_ALL_SHT]) >> 1) & 0x7f)
+			     - 106;
+		pwdb_all = query_rx_pwr_percentage(rx_pwr_all);
+
+		{
+			/* (3)EVM of HT rate */
+			if (prframe->u.hdr.attrib.htc &&
+			    prframe->u.hdr.attrib.mcs_rate >= 20 &&
+			    prframe->u.hdr.attrib.mcs_rate <= 27) {
+				/* both spatial stream make sense */
+				max_spatial_stream = 2;
+			} else {
+				/* only spatial stream 1 makes sense */
+				max_spatial_stream = 1;
+			}
+			for (i = 0; i < max_spatial_stream; i++) {
+				evm = evm_db2percentage((pphy_head
+				      [PHY_STAT_RXEVM_SHT + i]));/*dbm*/
+				prframe->u.hdr.attrib.signal_qual =
+					 (u8)(evm & 0xff);
+				prframe->u.hdr.attrib.rx_mimo_signal_qual[i] =
+					 (u8)(evm & 0xff);
+			}
+		}
+	}
+	/* UI BSS List signal strength(in percentage), make it good looking,
+	 * from 0~100. It is assigned to the BSS List in
+	 * GetValueFromBeaconOrProbeRsp().
+	 */
+	if (bcck_rate) {
+		prframe->u.hdr.attrib.signal_strength =
+			 (u8)r8712_signal_scale_mapping(pwdb_all);
+	} else {
+		if (rf_rx_num != 0)
+			prframe->u.hdr.attrib.signal_strength =
+				 (u8)(r8712_signal_scale_mapping(total_rssi /=
+				 rf_rx_num));
+	}
+}
+
+static void process_link_qual(struct _adapter *padapter,
+			      union recv_frame *prframe)
+{
+	u32	last_evm = 0, tmpVal;
+	struct rx_pkt_attrib *pattrib;
+	struct smooth_rssi_data *sqd = &padapter->recvpriv.signal_qual_data;
+
+	if (!prframe || !padapter)
+		return;
+	pattrib = &prframe->u.hdr.attrib;
+	if (pattrib->signal_qual != 0) {
+		/*
+		 * 1. Record the general EVM to the sliding window.
+		 */
+		if (sqd->total_num++ >= PHY_LINKQUALITY_SLID_WIN_MAX) {
+			sqd->total_num = PHY_LINKQUALITY_SLID_WIN_MAX;
+			last_evm = sqd->elements[sqd->index];
+			sqd->total_val -= last_evm;
+		}
+		sqd->total_val += pattrib->signal_qual;
+		sqd->elements[sqd->index++] = pattrib->signal_qual;
+		if (sqd->index >= PHY_LINKQUALITY_SLID_WIN_MAX)
+			sqd->index = 0;
+
+		/* <1> Showed on UI for user, in percentage. */
+		tmpVal = sqd->total_val / sqd->total_num;
+		padapter->recvpriv.signal = (u8)tmpVal;
+	}
+}
+
+static void process_rssi(struct _adapter *padapter, union recv_frame *prframe)
+{
+	u32 last_rssi, tmp_val;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct smooth_rssi_data *ssd = &padapter->recvpriv.signal_strength_data;
+
+	if (ssd->total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
+		ssd->total_num = PHY_RSSI_SLID_WIN_MAX;
+		last_rssi = ssd->elements[ssd->index];
+		ssd->total_val -= last_rssi;
+	}
+	ssd->total_val += pattrib->signal_strength;
+	ssd->elements[ssd->index++] = pattrib->signal_strength;
+	if (ssd->index >= PHY_RSSI_SLID_WIN_MAX)
+		ssd->index = 0;
+	tmp_val = ssd->total_val / ssd->total_num;
+	padapter->recvpriv.rssi = (s8)translate2dbm(padapter, (u8)tmp_val);
+}
+
+static void process_phy_info(struct _adapter *padapter,
+			     union recv_frame *prframe)
+{
+	query_rx_phy_status(padapter, prframe);
+	process_rssi(padapter, prframe);
+	process_link_qual(padapter,  prframe);
+}
+
+int recv_func(struct _adapter *padapter, void *pcontext)
+{
+	struct rx_pkt_attrib *pattrib;
+	union recv_frame *prframe, *orig_prframe;
+	int retval = _SUCCESS;
+	struct  __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	prframe = pcontext;
+	orig_prframe = prframe;
+	pattrib = &prframe->u.hdr.attrib;
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		if (pattrib->crc_err == 1)
+			padapter->mppriv.rx_crcerrpktcount++;
+		else
+			padapter->mppriv.rx_pktcount++;
+		if (!check_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE)) {
+			/* free this recv_frame */
+			r8712_free_recvframe(orig_prframe, pfree_recv_queue);
+			goto _exit_recv_func;
+		}
+	}
+	/* check the frame crtl field and decache */
+	retval = r8712_validate_recv_frame(padapter, prframe);
+	if (retval != _SUCCESS) {
+		/* free this recv_frame */
+		r8712_free_recvframe(orig_prframe, pfree_recv_queue);
+		goto _exit_recv_func;
+	}
+	process_phy_info(padapter, prframe);
+	prframe = r8712_decryptor(padapter, prframe);
+	if (!prframe) {
+		retval = _FAIL;
+		goto _exit_recv_func;
+	}
+	prframe = r8712_recvframe_chk_defrag(padapter, prframe);
+	if (!prframe)
+		goto _exit_recv_func;
+	prframe = r8712_portctrl(padapter, prframe);
+	if (!prframe) {
+		retval = _FAIL;
+		goto _exit_recv_func;
+	}
+	retval = r8712_process_recv_indicatepkts(padapter, prframe);
+	if (retval != _SUCCESS) {
+		r8712_free_recvframe(orig_prframe, pfree_recv_queue);
+		goto _exit_recv_func;
+	}
+_exit_recv_func:
+	return retval;
+}
+
+static void recvbuf2recvframe(struct _adapter *padapter, struct sk_buff *pskb)
+{
+	u8 *pbuf, shift_sz = 0;
+	u8	frag, mf;
+	uint	pkt_len;
+	u32 transfer_len;
+	struct recv_stat *prxstat;
+	u16	pkt_cnt, drvinfo_sz, pkt_offset, tmp_len, alloc_sz;
+	struct  __queue *pfree_recv_queue;
+	_pkt  *pkt_copy = NULL;
+	union recv_frame *precvframe = NULL;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	pfree_recv_queue = &precvpriv->free_recv_queue;
+	pbuf = pskb->data;
+	prxstat = (struct recv_stat *)pbuf;
+	pkt_cnt = (le32_to_cpu(prxstat->rxdw2) >> 16) & 0xff;
+	pkt_len =  le32_to_cpu(prxstat->rxdw0) & 0x00003fff;
+	transfer_len = pskb->len;
+	/* Test throughput with Netgear 3700 (No security) with Chariot 3T3R
+	 * pairs. The packet count will be a big number so that the containing
+	 * packet will effect the Rx reordering.
+	 */
+	if (transfer_len < pkt_len) {
+		/* In this case, it means the MAX_RECVBUF_SZ is too small to
+		 * get the data from 8712u.
+		 */
+		return;
+	}
+	do {
+		prxstat = (struct recv_stat *)pbuf;
+		pkt_len =  le32_to_cpu(prxstat->rxdw0) & 0x00003fff;
+		/* more fragment bit */
+		mf = (le32_to_cpu(prxstat->rxdw1) >> 27) & 0x1;
+		/* ragmentation number */
+		frag = (le32_to_cpu(prxstat->rxdw2) >> 12) & 0xf;
+		/* uint 2^3 = 8 bytes */
+		drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
+		drvinfo_sz <<= 3;
+		if (pkt_len <= 0)
+			return;
+		/* Qos data, wireless lan header length is 26 */
+		if ((le32_to_cpu(prxstat->rxdw0) >> 23) & 0x01)
+			shift_sz = 2;
+		precvframe = r8712_alloc_recvframe(pfree_recv_queue);
+		if (!precvframe)
+			return;
+		INIT_LIST_HEAD(&precvframe->u.hdr.list);
+		precvframe->u.hdr.precvbuf = NULL; /*can't access the precvbuf*/
+		precvframe->u.hdr.len = 0;
+		tmp_len = pkt_len + drvinfo_sz + RXDESC_SIZE;
+		pkt_offset = (u16)round_up(tmp_len, 128);
+		/* for first fragment packet, driver need allocate 1536 +
+		 * drvinfo_sz + RXDESC_SIZE to defrag packet.
+		 */
+		if ((mf == 1) && (frag == 0))
+			/*1658+6=1664, 1664 is 128 alignment.*/
+			alloc_sz = max_t(u16, tmp_len, 1658);
+		else
+			alloc_sz = tmp_len;
+		/* 2 is for IP header 4 bytes alignment in QoS packet case.
+		 * 4 is for skb->data 4 bytes alignment.
+		 */
+		alloc_sz += 6;
+		pkt_copy = netdev_alloc_skb(padapter->pnetdev, alloc_sz);
+		if (!pkt_copy)
+			return;
+
+		precvframe->u.hdr.pkt = pkt_copy;
+		skb_reserve(pkt_copy, 4 - ((addr_t)(pkt_copy->data) % 4));
+		skb_reserve(pkt_copy, shift_sz);
+		memcpy(pkt_copy->data, pbuf, tmp_len);
+		precvframe->u.hdr.rx_head = pkt_copy->data;
+		precvframe->u.hdr.rx_data = pkt_copy->data;
+		precvframe->u.hdr.rx_tail = pkt_copy->data;
+		precvframe->u.hdr.rx_end = pkt_copy->data + alloc_sz;
+
+		recvframe_put(precvframe, tmp_len);
+		recvframe_pull(precvframe, drvinfo_sz + RXDESC_SIZE);
+		/* because the endian issue, driver avoid reference to the
+		 * rxstat after calling update_recvframe_attrib_from_recvstat();
+		 */
+		update_recvframe_attrib_from_recvstat(&precvframe->u.hdr.attrib,
+						      prxstat);
+		r8712_recv_entry(precvframe);
+		transfer_len -= pkt_offset;
+		pbuf += pkt_offset;
+		pkt_cnt--;
+		precvframe = NULL;
+		pkt_copy = NULL;
+	} while ((transfer_len > 0) && pkt_cnt > 0);
+}
+
+static void recv_tasklet(struct tasklet_struct *t)
+{
+	struct sk_buff *pskb;
+	struct _adapter *padapter = from_tasklet(padapter, t,
+						 recvpriv.recv_tasklet);
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
+		recvbuf2recvframe(padapter, pskb);
+		skb_reset_tail_pointer(pskb);
+		pskb->len = 0;
+		if (!skb_cloned(pskb))
+			skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+		else
+			consume_skb(pskb);
+	}
+}
diff --git a/drivers/staging/rtl8712/rtl8712_recv.h b/drivers/staging/rtl8712/rtl8712_recv.h
new file mode 100644
index 000000000..f4d20b0ef
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_recv.h
@@ -0,0 +1,145 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL8712_RECV_H_
+#define _RTL8712_RECV_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+/* Realtek's v2.6.6 reduced this to 4. However, under heavy network and CPU
+ * loads, even 8 receive buffers might not be enough; cutting it to 4 seemed
+ * unwise.
+ */
+#define NR_RECVBUFF (8)
+
+#define NR_PREALLOC_RECV_SKB (8)
+#define RXDESC_SIZE	24
+#define RXDESC_OFFSET RXDESC_SIZE
+#define RECV_BLK_SZ 512
+#define RECV_BLK_CNT 16
+#define RECV_BLK_TH RECV_BLK_CNT
+#define MAX_RECVBUF_SZ 9100
+#define RECVBUFF_ALIGN_SZ 512
+#define RSVD_ROOM_SZ (0)
+/*These definition is used for Rx packet reordering.*/
+#define SN_LESS(a, b)		(((a-b) & 0x800) != 0)
+#define SN_EQUAL(a, b)	(a == b)
+#define REORDER_WAIT_TIME	30 /* (ms)*/
+
+struct recv_stat {
+	__le32 rxdw0;
+	__le32 rxdw1;
+	__le32 rxdw2;
+	__le32 rxdw3;
+	__le32 rxdw4;
+	__le32 rxdw5;
+};
+
+struct phy_cck_rx_status {
+	/* For CCK rate descriptor. This is a unsigned 8:1 variable.
+	 * LSB bit present 0.5. And MSB 7 bts present a signed value.
+	 * Range from -64~+63.5.
+	 */
+	u8	adc_pwdb_X[4];
+	u8	sq_rpt;
+	u8	cck_agc_rpt;
+};
+
+struct phy_stat {
+	__le32 phydw0;
+	__le32 phydw1;
+	__le32 phydw2;
+	__le32 phydw3;
+	__le32 phydw4;
+	__le32 phydw5;
+	__le32 phydw6;
+	__le32 phydw7;
+};
+
+#define PHY_STAT_GAIN_TRSW_SHT 0
+#define PHY_STAT_PWDB_ALL_SHT 4
+#define PHY_STAT_CFOSHO_SHT 5
+#define PHY_STAT_CCK_AGC_RPT_SHT 5
+#define PHY_STAT_CFOTAIL_SHT 9
+#define PHY_STAT_RXEVM_SHT 13
+#define PHY_STAT_RXSNR_SHT 15
+#define PHY_STAT_PDSNR_SHT 19
+#define PHY_STAT_CSI_CURRENT_SHT 21
+#define PHY_STAT_CSI_TARGET_SHT 23
+#define PHY_STAT_SIGEVM_SHT 25
+#define PHY_STAT_MAX_EX_PWR_SHT 26
+
+union recvstat {
+	struct recv_stat recv_stat;
+	unsigned int value[RXDESC_SIZE>>2];
+};
+
+struct recv_buf {
+	struct list_head list;
+	spinlock_t recvbuf_lock;
+	u32	ref_cnt;
+	struct _adapter  *adapter;
+	struct urb *purb;
+	_pkt *pskb;
+	u8  irp_pending;
+	u32  transfer_len;
+	uint  len;
+	u8 *phead;
+	u8 *pdata;
+	u8 *ptail;
+	u8 *pend;
+	u8 *pbuf;
+	u8 *pallocated_buf;
+};
+
+/*
+ *	head  ----->
+ *		data  ----->
+ *			payload
+ *		tail  ----->
+ *	end   ----->
+ *	len = (unsigned int )(tail - data);
+ */
+struct recv_frame_hdr {
+	struct list_head list;
+	_pkt	*pkt;
+	_pkt *pkt_newalloc;
+	struct _adapter  *adapter;
+	u8 fragcnt;
+	struct rx_pkt_attrib attrib;
+	uint  len;
+	u8 *rx_head;
+	u8 *rx_data;
+	u8 *rx_tail;
+	u8 *rx_end;
+	void *precvbuf;
+	struct sta_info *psta;
+	/*for A-MPDU Rx reordering buffer control*/
+	struct recv_reorder_ctrl *preorder_ctrl;
+};
+
+union recv_frame {
+	union {
+		struct list_head list;
+		struct recv_frame_hdr hdr;
+	} u;
+};
+
+void r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf);
+void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf);
+s32 r8712_signal_scale_mapping(s32 cur_sig);
+void r8712_reordering_ctrl_timeout_handler(void *pcontext);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl8712_regdef.h b/drivers/staging/rtl8712/rtl8712_regdef.h
new file mode 100644
index 000000000..28aec9aa5
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_regdef.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_REGDEF_H__
+#define __RTL8712_REGDEF_H__
+
+#include "rtl8712_syscfg_regdef.h"
+#include "rtl8712_cmdctrl_regdef.h"
+#include "rtl8712_macsetting_regdef.h"
+#include "rtl8712_timectrl_regdef.h"
+#include "rtl8712_fifoctrl_regdef.h"
+#include "rtl8712_ratectrl_regdef.h"
+#include "rtl8712_edcasetting_regdef.h"
+#include "rtl8712_wmac_regdef.h"
+#include "rtl8712_powersave_regdef.h"
+#include "rtl8712_gp_regdef.h"
+#include "rtl8712_debugctrl_regdef.h"
+
+#define HIMR	(RTL8712_INTERRUPT_ + 0x08)
+
+#endif /* __RTL8712_REGDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_security_bitdef.h b/drivers/staging/rtl8712/rtl8712_security_bitdef.h
new file mode 100644
index 000000000..44275ef45
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_security_bitdef.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_SECURITY_BITDEF_H__
+#define __RTL8712_SECURITY_BITDEF_H__
+
+/*CAMCMD*/
+#define	_SECCAM_POLLING				BIT(31)
+#define	_SECCAM_CLR					BIT(30)
+#define	_SECCAM_WE					BIT(16)
+#define	_SECCAM_ADR_MSK				0x000000FF
+#define	_SECCAM_ADR_SHT				0
+
+/*CAMDBG*/
+#define	_SECCAM_INFO				BIT(31)
+#define	_SEC_KEYFOUND				BIT(30)
+#define	_SEC_CONFIG_MSK				0x3F000000
+#define	_SEC_CONFIG_SHT				24
+#define	_SEC_KEYCONTENT_MSK			0x00FFFFFF
+#define	_SEC_KEYCONTENT_SHT			0
+
+/*SECCFG*/
+#define	_NOSKMC						BIT(5)
+#define	_SKBYA2						BIT(4)
+#define	_RXDEC						BIT(3)
+#define	_TXENC						BIT(2)
+#define	_RXUSEDK					BIT(1)
+#define	_TXUSEDK					BIT(0)
+
+#endif	/*__RTL8712_SECURITY_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_spec.h b/drivers/staging/rtl8712/rtl8712_spec.h
new file mode 100644
index 000000000..613a410e5
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_spec.h
@@ -0,0 +1,121 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_SPEC_H__
+#define __RTL8712_SPEC_H__
+
+#define RTL8712_IOBASE_TXPKT		0x10200000	/*IOBASE_TXPKT*/
+#define RTL8712_IOBASE_RXPKT		0x10210000	/*IOBASE_RXPKT*/
+#define RTL8712_IOBASE_RXCMD		0x10220000	/*IOBASE_RXCMD*/
+#define RTL8712_IOBASE_TXSTATUS		0x10230000	/*IOBASE_TXSTATUS*/
+#define RTL8712_IOBASE_RXSTATUS		0x10240000	/*IOBASE_RXSTATUS*/
+#define RTL8712_IOBASE_IOREG		0x10250000	/*IOBASE_IOREG ADDR*/
+#define RTL8712_IOBASE_SCHEDULER	0x10260000	/*IOBASE_SCHEDULE*/
+
+#define RTL8712_IOBASE_TRXDMA		0x10270000	/*IOBASE_TRXDMA*/
+#define RTL8712_IOBASE_TXLLT		0x10280000	/*IOBASE_TXLLT*/
+#define RTL8712_IOBASE_WMAC		0x10290000	/*IOBASE_WMAC*/
+#define RTL8712_IOBASE_FW2HW		0x102A0000	/*IOBASE_FW2HW*/
+#define RTL8712_IOBASE_ACCESS_PHYREG	0x102B0000	/*IOBASE_ACCESS_PHYREG*/
+
+#define RTL8712_IOBASE_FF	0x10300000 /*IOBASE_FIFO 0x1031000~0x103AFFFF*/
+
+/*IOREG Offset for 8712*/
+#define RTL8712_SYSCFG_		RTL8712_IOBASE_IOREG
+#define RTL8712_CMDCTRL_	(RTL8712_IOBASE_IOREG + 0x40)
+#define RTL8712_MACIDSETTING_	(RTL8712_IOBASE_IOREG + 0x50)
+#define RTL8712_TIMECTRL_	(RTL8712_IOBASE_IOREG + 0x80)
+#define RTL8712_FIFOCTRL_	(RTL8712_IOBASE_IOREG + 0xA0)
+#define RTL8712_RATECTRL_	(RTL8712_IOBASE_IOREG + 0x160)
+#define RTL8712_EDCASETTING_	(RTL8712_IOBASE_IOREG + 0x1D0)
+#define RTL8712_WMAC_		(RTL8712_IOBASE_IOREG + 0x200)
+#define RTL8712_SECURITY_	(RTL8712_IOBASE_IOREG + 0x240)
+#define RTL8712_POWERSAVE_	(RTL8712_IOBASE_IOREG + 0x260)
+#define RTL8712_GP_		(RTL8712_IOBASE_IOREG + 0x2E0)
+#define RTL8712_INTERRUPT_	(RTL8712_IOBASE_IOREG + 0x300)
+#define RTL8712_DEBUGCTRL_	(RTL8712_IOBASE_IOREG + 0x310)
+#define RTL8712_OFFLOAD_	(RTL8712_IOBASE_IOREG + 0x2D0)
+
+/*FIFO for 8712*/
+#define RTL8712_DMA_BCNQ	(RTL8712_IOBASE_FF + 0x10000)
+#define RTL8712_DMA_MGTQ	(RTL8712_IOBASE_FF + 0x20000)
+#define RTL8712_DMA_BMCQ	(RTL8712_IOBASE_FF + 0x30000)
+#define RTL8712_DMA_VOQ		(RTL8712_IOBASE_FF + 0x40000)
+#define RTL8712_DMA_VIQ		(RTL8712_IOBASE_FF + 0x50000)
+#define RTL8712_DMA_BEQ		(RTL8712_IOBASE_FF + 0x60000)
+#define RTL8712_DMA_BKQ		(RTL8712_IOBASE_FF + 0x70000)
+#define RTL8712_DMA_RX0FF	(RTL8712_IOBASE_FF + 0x80000)
+#define RTL8712_DMA_H2CCMD	(RTL8712_IOBASE_FF + 0x90000)
+#define RTL8712_DMA_C2HCMD	(RTL8712_IOBASE_FF + 0xA0000)
+
+/*------------------------------*/
+
+/*BIT 16 15*/
+#define	DID_SDIO_LOCAL			0	/* 0 0*/
+#define	DID_WLAN_IOREG			1	/* 0 1*/
+#define	DID_WLAN_FIFO			3	/* 1 1*/
+#define   DID_UNDEFINE				(-1)
+
+#define CMD_ADDR_MAPPING_SHIFT		2	/*SDIO CMD ADDR MAPPING,
+						 *shift 2 bit for match
+						 * offset[14:2]
+						 */
+
+/*Offset for SDIO LOCAL*/
+#define	OFFSET_SDIO_LOCAL				0x0FFF
+
+/*Offset for WLAN IOREG*/
+#define OFFSET_WLAN_IOREG				0x0FFF
+
+/*Offset for WLAN FIFO*/
+#define	OFFSET_TX_BCNQ				0x0300
+#define	OFFSET_TX_HIQ					0x0310
+#define	OFFSET_TX_CMDQ				0x0320
+#define	OFFSET_TX_MGTQ				0x0330
+#define	OFFSET_TX_HCCAQ				0x0340
+#define	OFFSET_TX_VOQ					0x0350
+#define	OFFSET_TX_VIQ					0x0360
+#define	OFFSET_TX_BEQ					0x0370
+#define	OFFSET_TX_BKQ					0x0380
+#define	OFFSET_RX_RX0FFQ				0x0390
+#define	OFFSET_RX_C2HFFQ				0x03A0
+
+#define	BK_QID_01	1
+#define	BK_QID_02	2
+#define	BE_QID_01	0
+#define	BE_QID_02	3
+#define	VI_QID_01	4
+#define	VI_QID_02	5
+#define	VO_QID_01	6
+#define	VO_QID_02	7
+#define	HCCA_QID_01	8
+#define	HCCA_QID_02	9
+#define	HCCA_QID_03	10
+#define	HCCA_QID_04	11
+#define	HCCA_QID_05	12
+#define	HCCA_QID_06	13
+#define	HCCA_QID_07	14
+#define	HCCA_QID_08	15
+#define	HI_QID		17
+#define	CMD_QID	19
+#define	MGT_QID	18
+#define	BCN_QID	16
+
+#include "rtl8712_regdef.h"
+
+#include "rtl8712_bitdef.h"
+
+#include "basic_types.h"
+
+#endif /* __RTL8712_SPEC_H__ */
+
diff --git a/drivers/staging/rtl8712/rtl8712_syscfg_bitdef.h b/drivers/staging/rtl8712/rtl8712_syscfg_bitdef.h
new file mode 100644
index 000000000..d92df3fbd
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_syscfg_bitdef.h
@@ -0,0 +1,163 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_SYSCFG_BITDEF_H__
+#define __RTL8712_SYSCFG_BITDEF_H__
+
+/*SYS_PWR_CTRL*/
+/*SRCTRL0*/
+/*SRCTRL1*/
+/*SYS_CLKR*/
+
+/*SYS_IOS_CTRL*/
+#define iso_LDR2RP_SHT		8 /* EE Loader to Retention Path*/
+#define iso_LDR2RP		BIT(iso_LDR2RP_SHT) /* 1:isolation, 0:attach*/
+
+/*SYS_CTRL*/
+#define FEN_DIO_SDIO_SHT	0
+#define FEN_DIO_SDIO		BIT(FEN_DIO_SDIO_SHT)
+#define FEN_SDIO_SHT		1
+#define FEN_SDIO		BIT(FEN_SDIO_SHT)
+#define FEN_USBA_SHT		2
+#define FEN_USBA		BIT(FEN_USBA_SHT)
+#define FEN_UPLL_SHT		3
+#define FEN_UPLL		BIT(FEN_UPLL_SHT)
+#define FEN_USBD_SHT		4
+#define FEN_USBD		BIT(FEN_USBD_SHT)
+#define FEN_DIO_PCIE_SHT	5
+#define FEN_DIO_PCIE		BIT(FEN_DIO_PCIE_SHT)
+#define FEN_PCIEA_SHT		6
+#define FEN_PCIEA		BIT(FEN_PCIEA_SHT)
+#define FEN_PPLL_SHT		7
+#define FEN_PPLL		BIT(FEN_PPLL_SHT)
+#define FEN_PCIED_SHT		8
+#define FEN_PCIED		BIT(FEN_PCIED_SHT)
+#define FEN_CPUEN_SHT		10
+#define FEN_CPUEN		BIT(FEN_CPUEN_SHT)
+#define FEN_DCORE_SHT		11
+#define FEN_DCORE		BIT(FEN_DCORE_SHT)
+#define FEN_ELDR_SHT		12
+#define FEN_ELDR		BIT(FEN_ELDR_SHT)
+#define PWC_DV2LDR_SHT		13
+#define PWC_DV2LDR		BIT(PWC_DV2LDR_SHT) /* Loader Power Enable*/
+
+/*=== SYS_CLKR ===*/
+#define SYS_CLKSEL_SHT		0
+#define SYS_CLKSEL		BIT(SYS_CLKSEL_SHT) /* System Clock 80MHz*/
+#define PS_CLKSEL_SHT		1
+#define PS_CLKSEL		BIT(PS_CLKSEL_SHT) /*System power save
+						    * clock select.
+						    */
+#define CPU_CLKSEL_SHT		2
+#define CPU_CLKSEL		BIT(CPU_CLKSEL_SHT) /* System Clock select,
+						     * 1: AFE source,
+						     * 0: System clock(L-Bus)
+						     */
+#define INT32K_EN_SHT		3
+#define INT32K_EN		BIT(INT32K_EN_SHT)
+#define MACSLP_SHT		4
+#define MACSLP			BIT(MACSLP_SHT)
+#define MAC_CLK_EN_SHT		11
+#define MAC_CLK_EN		BIT(MAC_CLK_EN_SHT) /* MAC Clock Enable.*/
+#define SYS_CLK_EN_SHT		12
+#define SYS_CLK_EN		BIT(SYS_CLK_EN_SHT)
+#define RING_CLK_EN_SHT		13
+#define RING_CLK_EN		BIT(RING_CLK_EN_SHT)
+#define SWHW_SEL_SHT		14
+#define SWHW_SEL		BIT(SWHW_SEL_SHT) /* Load done,
+						   * control path switch.
+						   */
+#define FWHW_SEL_SHT		15
+#define FWHW_SEL		BIT(FWHW_SEL_SHT) /* Sleep exit,
+						   * control path switch.
+						   */
+
+/*9346CR*/
+#define	_VPDIDX_MSK		0xFF00
+#define	_VPDIDX_SHT		8
+#define	_EEM_MSK		0x00C0
+#define	_EEM_SHT		6
+#define	_EEM0			BIT(6)
+#define	_EEM1			BIT(7)
+#define	_EEPROM_EN		BIT(5)
+#define	_9356SEL		BIT(4)
+#define	_EECS			BIT(3)
+#define	_EESK			BIT(2)
+#define	_EEDI			BIT(1)
+#define	_EEDO			BIT(0)
+
+/*AFE_MISC*/
+#define	AFE_MISC_USB_MBEN_SHT	7
+#define	AFE_MISC_USB_MBEN	BIT(AFE_MISC_USB_MBEN_SHT)
+#define	AFE_MISC_USB_BGEN_SHT	6
+#define	AFE_MISC_USB_BGEN	BIT(AFE_MISC_USB_BGEN_SHT)
+#define	AFE_MISC_LD12_VDAJ_SHT	4
+#define	AFE_MISC_LD12_VDAJ_MSK	0X0030
+#define	AFE_MISC_LD12_VDAJ	BIT(AFE_MISC_LD12_VDAJ_SHT)
+#define	AFE_MISC_I32_EN_SHT	3
+#define	AFE_MISC_I32_EN		BIT(AFE_MISC_I32_EN_SHT)
+#define	AFE_MISC_E32_EN_SHT	2
+#define	AFE_MISC_E32_EN		BIT(AFE_MISC_E32_EN_SHT)
+#define	AFE_MISC_MBEN_SHT	1
+#define	AFE_MISC_MBEN		BIT(AFE_MISC_MBEN_SHT)/* Enable AFE Macro
+						       * Block's Mbias.
+						       */
+#define	AFE_MISC_BGEN_SHT	0
+#define	AFE_MISC_BGEN		BIT(AFE_MISC_BGEN_SHT)/* Enable AFE Macro
+						       * Block's Bandgap.
+						       */
+
+/*--------------------------------------------------------------------------*/
+/*       SPS1_CTRL bits				(Offset 0x18-1E, 56bits)*/
+/*--------------------------------------------------------------------------*/
+#define	SPS1_SWEN		BIT(1)	/* Enable vsps18 SW Macro Block.*/
+#define	SPS1_LDEN		BIT(0)	/* Enable VSPS12 LDO Macro block.*/
+
+/*----------------------------------------------------------------------------*/
+/*       LDOA15_CTRL bits		(Offset 0x20, 8bits)*/
+/*----------------------------------------------------------------------------*/
+#define	LDA15_EN		BIT(0)	/* Enable LDOA15 Macro Block*/
+
+/*----------------------------------------------------------------------------*/
+/*       8192S LDOV12D_CTRL bit		(Offset 0x21, 8bits)*/
+/*----------------------------------------------------------------------------*/
+#define	LDV12_EN		BIT(0)	/* Enable LDOVD12 Macro Block*/
+#define	LDV12_SDBY		BIT(1)	/* LDOVD12 standby mode*/
+
+/*CLK_PS_CTRL*/
+#define	_CLK_GATE_EN		BIT(0)
+
+/* EFUSE_CTRL*/
+#define EF_FLAG			BIT(31)		/* Access Flag, Write:1;
+						 *	        Read:0
+						 */
+#define EF_PGPD			0x70000000	/* E-fuse Program time*/
+#define EF_RDT			0x0F000000	/* E-fuse read time: in the
+						 * unit of cycle time
+						 */
+#define EF_PDN_EN		BIT(19)		/* EFuse Power down enable*/
+#define ALD_EN			BIT(18)		/* Autoload Enable*/
+#define EF_ADDR			0x0003FF00	/* Access Address*/
+#define EF_DATA			0x000000FF	/* Access Data*/
+
+/* EFUSE_TEST*/
+#define LDOE25_EN		BIT(31)		/* Enable LDOE25 Macro Block*/
+
+/* EFUSE_CLK_CTRL*/
+#define EFUSE_CLK_EN		BIT(1)		/* E-Fuse Clock Enable*/
+#define EFUSE_CLK_SEL		BIT(0)		/* E-Fuse Clock Select,
+						 * 0:500K, 1:40M
+						 */
+
+#endif	/*__RTL8712_SYSCFG_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_syscfg_regdef.h b/drivers/staging/rtl8712/rtl8712_syscfg_regdef.h
new file mode 100644
index 000000000..da5efcded
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_syscfg_regdef.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_SYSCFG_REGDEF_H__
+#define __RTL8712_SYSCFG_REGDEF_H__
+
+#define SYS_ISO_CTRL		(RTL8712_SYSCFG_ + 0x0000)
+#define SYS_FUNC_EN		(RTL8712_SYSCFG_ + 0x0002)
+#define PMC_FSM			(RTL8712_SYSCFG_ + 0x0004)
+#define SYS_CLKR		(RTL8712_SYSCFG_ + 0x0008)
+#define EE_9346CR		(RTL8712_SYSCFG_ + 0x000A)
+#define EE_VPD			(RTL8712_SYSCFG_ + 0x000C)
+#define AFE_MISC		(RTL8712_SYSCFG_ + 0x0010)
+#define SPS0_CTRL		(RTL8712_SYSCFG_ + 0x0011)
+#define SPS1_CTRL		(RTL8712_SYSCFG_ + 0x0018)
+#define RF_CTRL			(RTL8712_SYSCFG_ + 0x001F)
+#define LDOA15_CTRL		(RTL8712_SYSCFG_ + 0x0020)
+#define LDOV12D_CTRL		(RTL8712_SYSCFG_ + 0x0021)
+#define LDOHCI12_CTRL		(RTL8712_SYSCFG_ + 0x0022)
+#define LDO_USB_CTRL		(RTL8712_SYSCFG_ + 0x0023)
+#define LPLDO_CTRL		(RTL8712_SYSCFG_ + 0x0024)
+#define AFE_XTAL_CTRL		(RTL8712_SYSCFG_ + 0x0026)
+#define AFE_PLL_CTRL		(RTL8712_SYSCFG_ + 0x0028)
+#define EFUSE_CTRL		(RTL8712_SYSCFG_ + 0x0030)
+#define EFUSE_TEST		(RTL8712_SYSCFG_ + 0x0034)
+#define PWR_DATA		(RTL8712_SYSCFG_ + 0x0038)
+#define DPS_TIMER		(RTL8712_SYSCFG_ + 0x003C)
+#define RCLK_MON		(RTL8712_SYSCFG_ + 0x003E)
+#define EFUSE_CLK_CTRL		(RTL8712_SYSCFG_ + 0x02F8)
+
+#endif /*__RTL8712_SYSCFG_REGDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_timectrl_bitdef.h b/drivers/staging/rtl8712/rtl8712_timectrl_bitdef.h
new file mode 100644
index 000000000..d7bc9dd5c
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_timectrl_bitdef.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_TIMECTRL_BITDEF_H__
+#define __RTL8712_TIMECTRL_BITDEF_H__
+
+/*TSFTR*/
+/*SLOT*/
+/*USTIME*/
+
+/*TUBASE*/
+#define	_TUBASE_MSK			0x07FF
+
+/*SIFS_CCK*/
+#define	_SIFS_CCK_TRX_MSK		0xFF00
+#define	_SIFS_CCK_TRX_SHT		0x8
+#define	_SIFS_CCK_CTX_MSK		0x00FF
+#define	_SIFS_CCK_CTX_SHT		0
+
+/*SIFS_OFDM*/
+#define	_SIFS_OFDM_TRX_MSK		0xFF00
+#define	_SIFS_OFDM_TRX_SHT		0x8
+#define	_SIFS_OFDM_CTX_MSK		0x00FF
+#define	_SIFS_OFDM_CTX_SHT		0
+
+/*PIFS*/
+/*ACKTO*/
+/*EIFS*/
+/*BCNITV*/
+/*ATIMWND*/
+
+/*DRVERLYINT*/
+#define	_ENSWBCN				BIT(15)
+#define	_DRVERLY_TU_MSK			0x0FF0
+#define	_DRVERLY_TU_SHT			4
+#define	_DRVERLY_US_MSK			0x000F
+#define	_DRVERLY_US_SHT			0
+
+/*BCNDMATIM*/
+#define	_BCNDMATIM_MSK			0x03FF
+
+/*BCNERRTH*/
+/*MLT*/
+
+#endif /* __RTL8712_TIMECTRL_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_timectrl_regdef.h b/drivers/staging/rtl8712/rtl8712_timectrl_regdef.h
new file mode 100644
index 000000000..b51603f1b
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_timectrl_regdef.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_TIMECTRL_REGDEF_H__
+#define __RTL8712_TIMECTRL_REGDEF_H__
+
+#define TSFTR			(RTL8712_TIMECTRL_ + 0x00)
+#define USTIME			(RTL8712_TIMECTRL_ + 0x08)
+#define SLOT			(RTL8712_TIMECTRL_ + 0x09)
+#define TUBASE			(RTL8712_TIMECTRL_ + 0x0A)
+#define SIFS_CCK		(RTL8712_TIMECTRL_ + 0x0C)
+#define SIFS_OFDM		(RTL8712_TIMECTRL_ + 0x0E)
+#define PIFS			(RTL8712_TIMECTRL_ + 0x10)
+#define ACKTO			(RTL8712_TIMECTRL_ + 0x11)
+#define EIFS			(RTL8712_TIMECTRL_ + 0x12)
+#define BCNITV			(RTL8712_TIMECTRL_ + 0x14)
+#define ATIMWND			(RTL8712_TIMECTRL_ + 0x16)
+#define DRVERLYINT		(RTL8712_TIMECTRL_ + 0x18)
+#define BCNDMATIM		(RTL8712_TIMECTRL_ + 0x1A)
+#define BCNERRTH		(RTL8712_TIMECTRL_ + 0x1C)
+#define MLT			(RTL8712_TIMECTRL_ + 0x1D)
+
+#endif /* __RTL8712_TIMECTRL_REGDEF_H__ */
diff --git a/drivers/staging/rtl8712/rtl8712_wmac_bitdef.h b/drivers/staging/rtl8712/rtl8712_wmac_bitdef.h
new file mode 100644
index 000000000..ea164e482
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_wmac_bitdef.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_WMAC_BITDEF_H__
+#define __RTL8712_WMAC_BITDEF_H__
+
+/*NAVCTRL*/
+#define	_NAV_UPPER_EN			BIT(18)
+#define	_NAV_MTO_EN				BIT(17)
+#define	_NAV_UPPER				BIT(16)
+#define	_NAV_MTO_MSK			0xFF00
+#define	_NAV_MTO_SHT			8
+#define	_RTSRST_MSK				0x00FF
+#define	_RTSRST_SHT				0
+
+/*BWOPMODE*/
+#define	_20MHZBW				BIT(2)
+
+/*BACAMCMD*/
+#define	_BACAM_POLL				BIT(31)
+#define	_BACAM_RST				BIT(17)
+#define	_BACAM_RW				BIT(16)
+#define	_BACAM_ADDR_MSK			0x0000007F
+#define	_BACAM_ADDR_SHT			0
+
+/*LBDLY*/
+#define	_LBDLY_MSK				0x1F
+
+/*FWDLY*/
+#define	_FWDLY_MSK				0x0F
+
+/*RXERR_RPT*/
+#define	_RXERR_RPT_SEL_MSK		0xF0000000
+#define	_RXERR_RPT_SEL_SHT		28
+#define	_RPT_CNT_MSK			0x000FFFFF
+#define	_RPT_CNT_SHT			0
+
+#endif	/*__RTL8712_WMAC_BITDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_wmac_regdef.h b/drivers/staging/rtl8712/rtl8712_wmac_regdef.h
new file mode 100644
index 000000000..dfe3e9fbe
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_wmac_regdef.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_WMAC_REGDEF_H__
+#define __RTL8712_WMAC_REGDEF_H__
+
+#define NAVCTRL				(RTL8712_WMAC_ + 0x00)
+#define BWOPMODE			(RTL8712_WMAC_ + 0x03)
+#define BACAMCMD			(RTL8712_WMAC_ + 0x04)
+#define BACAMCONTENT			(RTL8712_WMAC_ + 0x08)
+#define LBDLY				(RTL8712_WMAC_ + 0x10)
+#define FWDLY				(RTL8712_WMAC_ + 0x11)
+#define HWPC_RX_CTRL			(RTL8712_WMAC_ + 0x18)
+#define MQ				(RTL8712_WMAC_ + 0x20)
+#define MA				(RTL8712_WMAC_ + 0x22)
+#define MS				(RTL8712_WMAC_ + 0x24)
+#define CLM_RESULT			(RTL8712_WMAC_ + 0x27)
+#define NHM_RPI_CNT			(RTL8712_WMAC_ + 0x28)
+#define RXERR_RPT			(RTL8712_WMAC_ + 0x30)
+#define NAV_PROT_LEN			(RTL8712_WMAC_ + 0x34)
+#define CFEND_TH			(RTL8712_WMAC_ + 0x36)
+#define AMPDU_MIN_SPACE			(RTL8712_WMAC_ + 0x37)
+#define	TXOP_STALL_CTRL			(RTL8712_WMAC_ + 0x38)
+
+#endif /*__RTL8712_WMAC_REGDEF_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.c b/drivers/staging/rtl8712/rtl8712_xmit.c
new file mode 100644
index 000000000..84a22eba7
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_xmit.c
@@ -0,0 +1,745 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl8712_xmit.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL8712_XMIT_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wifi.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+
+static void dump_xframe(struct _adapter *padapter,
+			struct xmit_frame *pxmitframe);
+static void update_txdesc(struct xmit_frame *pxmitframe, uint *pmem, int sz);
+
+sint _r8712_init_hw_txqueue(struct hw_txqueue *phw_txqueue, u8 ac_tag)
+{
+	phw_txqueue->ac_tag = ac_tag;
+	switch (ac_tag) {
+	case BE_QUEUE_INX:
+		phw_txqueue->ff_hwaddr = RTL8712_DMA_BEQ;
+		break;
+	case BK_QUEUE_INX:
+		phw_txqueue->ff_hwaddr = RTL8712_DMA_BKQ;
+		break;
+	case VI_QUEUE_INX:
+		phw_txqueue->ff_hwaddr = RTL8712_DMA_VIQ;
+		break;
+	case VO_QUEUE_INX:
+		phw_txqueue->ff_hwaddr = RTL8712_DMA_VOQ;
+		break;
+	case BMC_QUEUE_INX:
+		phw_txqueue->ff_hwaddr = RTL8712_DMA_BEQ;
+		break;
+	}
+	return _SUCCESS;
+}
+
+int r8712_txframes_sta_ac_pending(struct _adapter *padapter,
+				  struct pkt_attrib *pattrib)
+{
+	struct sta_info *psta;
+	struct tx_servq *ptxservq;
+	int priority = pattrib->priority;
+
+	psta = pattrib->psta;
+	switch (priority) {
+	case 1:
+	case 2:
+		ptxservq = &psta->sta_xmitpriv.bk_q;
+		break;
+	case 4:
+	case 5:
+		ptxservq = &psta->sta_xmitpriv.vi_q;
+		break;
+	case 6:
+	case 7:
+		ptxservq = &psta->sta_xmitpriv.vo_q;
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &psta->sta_xmitpriv.be_q;
+	break;
+	}
+	return ptxservq->qcnt;
+}
+
+static u32 get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr = 0;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+
+	if (pxmitframe->frame_tag == TXAGG_FRAMETAG) {
+		addr = RTL8712_DMA_H2CCMD;
+	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
+		addr = RTL8712_DMA_MGTQ;
+	} else if (pdvobj->nr_endpoint == 6) {
+		switch (pattrib->priority) {
+		case 0:
+		case 3:
+			addr = RTL8712_DMA_BEQ;
+			break;
+		case 1:
+		case 2:
+			addr = RTL8712_DMA_BKQ;
+			break;
+		case 4:
+		case 5:
+			addr = RTL8712_DMA_VIQ;
+			break;
+		case 6:
+		case 7:
+			addr = RTL8712_DMA_VOQ;
+			break;
+		case 0x10:
+		case 0x11:
+		case 0x12:
+		case 0x13:
+			addr = RTL8712_DMA_H2CCMD;
+			break;
+		default:
+			addr = RTL8712_DMA_BEQ;
+			break;
+		}
+	} else if (pdvobj->nr_endpoint == 4) {
+		switch (pattrib->qsel) {
+		case 0:
+		case 3:
+		case 1:
+		case 2:
+			addr = RTL8712_DMA_BEQ;/*RTL8712_EP_LO;*/
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			addr = RTL8712_DMA_VOQ;/*RTL8712_EP_HI;*/
+			break;
+		case 0x10:
+		case 0x11:
+		case 0x12:
+		case 0x13:
+			addr = RTL8712_DMA_H2CCMD;
+			break;
+		default:
+			addr = RTL8712_DMA_BEQ;/*RTL8712_EP_LO;*/
+			break;
+		}
+	}
+	return addr;
+}
+
+static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv,
+					 struct hw_xmit *phwxmit,
+					 struct tx_servq *ptxservq,
+					 struct  __queue *pframe_queue)
+{
+	struct list_head *xmitframe_plist, *xmitframe_phead;
+	struct	xmit_frame *pxmitframe = NULL;
+
+	xmitframe_phead = &pframe_queue->queue;
+	xmitframe_plist = xmitframe_phead->next;
+	if (!end_of_queue_search(xmitframe_phead, xmitframe_plist)) {
+		pxmitframe = container_of(xmitframe_plist,
+					  struct xmit_frame, list);
+		list_del_init(&pxmitframe->list);
+		ptxservq->qcnt--;
+		phwxmit->txcmdcnt++;
+	}
+	return pxmitframe;
+}
+
+static struct xmit_frame *dequeue_xframe_ex(struct xmit_priv *pxmitpriv,
+				     struct hw_xmit *phwxmit_i, sint entry)
+{
+	unsigned long irqL0;
+	struct list_head *sta_plist, *sta_phead;
+	struct hw_xmit *phwxmit;
+	struct tx_servq *ptxservq = NULL;
+	struct  __queue *pframe_queue = NULL;
+	struct	xmit_frame *pxmitframe = NULL;
+	int i, inx[4];
+	int j, acirp_cnt[4];
+
+	/*entry indx: 0->vo, 1->vi, 2->be, 3->bk.*/
+	inx[0] = 0; acirp_cnt[0] = pxmitpriv->voq_cnt;
+	inx[1] = 1; acirp_cnt[1] = pxmitpriv->viq_cnt;
+	inx[2] = 2; acirp_cnt[2] = pxmitpriv->beq_cnt;
+	inx[3] = 3; acirp_cnt[3] = pxmitpriv->bkq_cnt;
+	for (i = 0; i < 4; i++) {
+		for (j = i + 1; j < 4; j++) {
+			if (acirp_cnt[j] < acirp_cnt[i]) {
+				swap(acirp_cnt[i], acirp_cnt[j]);
+				swap(inx[i], inx[j]);
+			}
+		}
+	}
+	spin_lock_irqsave(&pxmitpriv->lock, irqL0);
+	for (i = 0; i < entry; i++) {
+		phwxmit = phwxmit_i + inx[i];
+		sta_phead = &phwxmit->sta_queue->queue;
+		sta_plist = sta_phead->next;
+		while (!end_of_queue_search(sta_phead, sta_plist)) {
+			ptxservq = container_of(sta_plist, struct tx_servq,
+						tx_pending);
+			pframe_queue = &ptxservq->sta_pending;
+			pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit,
+				     ptxservq, pframe_queue);
+			if (pxmitframe) {
+				phwxmit->accnt--;
+				goto exit_dequeue_xframe_ex;
+			}
+			sta_plist = sta_plist->next;
+			/*Remove sta node when there are no pending packets.*/
+			if (list_empty(&pframe_queue->queue)) {
+				/* must be done after sta_plist->next
+				 * and before break
+				 */
+				list_del_init(&ptxservq->tx_pending);
+			}
+		}
+	}
+exit_dequeue_xframe_ex:
+	spin_unlock_irqrestore(&pxmitpriv->lock, irqL0);
+	return pxmitframe;
+}
+
+void r8712_do_queue_select(struct _adapter *padapter,
+			   struct pkt_attrib *pattrib)
+{
+	unsigned int qsel = 0;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+
+	if (pdvobj->nr_endpoint == 6) {
+		qsel = (unsigned int)pattrib->priority;
+	} else if (pdvobj->nr_endpoint == 4) {
+		qsel = (unsigned int)pattrib->priority;
+		if (qsel == 0 || qsel == 3)
+			qsel = 3;
+		else if (qsel == 1 || qsel == 2)
+			qsel = 1;
+		else if (qsel == 4 || qsel == 5)
+			qsel = 5;
+		else if (qsel == 6 || qsel == 7)
+			qsel = 7;
+		else
+			qsel = 3;
+	}
+	pattrib->qsel = qsel;
+}
+
+#ifdef CONFIG_R8712_TX_AGGR
+void r8712_construct_txaggr_cmd_desc(struct xmit_buf *pxmitbuf)
+{
+	struct tx_desc *ptx_desc = (struct tx_desc *)pxmitbuf->pbuf;
+
+	/* Fill up TxCmd Descriptor according as USB FW Tx Aaggregation info.*/
+	/* dw0 */
+	ptx_desc->txdw0 = cpu_to_le32(CMD_HDR_SZ & 0xffff);
+	ptx_desc->txdw0 |=
+		cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) &
+			    0x00ff0000);
+	ptx_desc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+
+	/* dw1 */
+	ptx_desc->txdw1 |= cpu_to_le32((0x13 << QSEL_SHT) & 0x00001f00);
+}
+
+void r8712_construct_txaggr_cmd_hdr(struct xmit_buf *pxmitbuf)
+{
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)
+		pxmitbuf->priv_data;
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_hdr *pcmd_hdr = (struct cmd_hdr  *)
+		(pxmitbuf->pbuf + TXDESC_SIZE);
+
+	/* Fill up Cmd Header for USB FW Tx Aggregation.*/
+	/* dw0 */
+	pcmd_hdr->cmd_dw0 = cpu_to_le32((GEN_CMD_CODE(_AMSDU_TO_AMPDU) << 16) |
+					(pcmdpriv->cmd_seq << 24));
+	pcmdpriv->cmd_seq++;
+}
+
+void r8712_append_mpdu_unit(struct xmit_buf *pxmitbuf,
+			    struct xmit_frame *pxmitframe)
+{
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct tx_desc *ptx_desc = (struct tx_desc *)pxmitbuf->pbuf;
+	int last_txcmdsz = 0;
+	int padding_sz = 0;
+
+	/* 802.3->802.11 converter */
+	r8712_xmitframe_coalesce(padapter, pxmitframe->pkt, pxmitframe);
+	/* free skb struct */
+	r8712_xmit_complete(padapter, pxmitframe);
+	if (pxmitframe->attrib.ether_type != 0x0806) {
+		if ((pxmitframe->attrib.ether_type != 0x888e) &&
+			(pxmitframe->attrib.dhcp_pkt != 1)) {
+			r8712_issue_addbareq_cmd(padapter,
+					pxmitframe->attrib.priority);
+		}
+	}
+	pxmitframe->last[0] = 1;
+	update_txdesc(pxmitframe, (uint *)(pxmitframe->buf_addr),
+		pxmitframe->attrib.last_txcmdsz);
+	/*padding zero */
+	last_txcmdsz = pxmitframe->attrib.last_txcmdsz;
+	padding_sz = (8 - (last_txcmdsz % 8));
+	if ((last_txcmdsz % 8) != 0) {
+		int i;
+
+		for (i = 0; i < padding_sz; i++)
+			*(pxmitframe->buf_addr + TXDESC_SIZE + last_txcmdsz +
+			  i) = 0;
+	}
+	/* Add the new mpdu's length */
+	ptx_desc->txdw0 = cpu_to_le32((ptx_desc->txdw0 & 0xffff0000) |
+		((ptx_desc->txdw0 & 0x0000ffff) +
+			((TXDESC_SIZE + last_txcmdsz + padding_sz) &
+			 0x0000ffff)));
+}
+
+void r8712_xmitframe_aggr_1st(struct xmit_buf *pxmitbuf,
+			      struct xmit_frame *pxmitframe)
+{
+	/* linux complete context doesn't need to protect */
+	pxmitframe->pxmitbuf = pxmitbuf;
+	pxmitbuf->priv_data = pxmitframe;
+	pxmitframe->pxmit_urb[0] = pxmitbuf->pxmit_urb[0];
+	/* buffer addr assoc */
+	pxmitframe->buf_addr = pxmitbuf->pbuf + TXDESC_SIZE + CMD_HDR_SZ;
+	/*RTL8712_DMA_H2CCMD */
+	r8712_construct_txaggr_cmd_desc(pxmitbuf);
+	r8712_construct_txaggr_cmd_hdr(pxmitbuf);
+	r8712_append_mpdu_unit(pxmitbuf, pxmitframe);
+	pxmitbuf->aggr_nr = 1;
+}
+
+u16 r8712_xmitframe_aggr_next(struct xmit_buf *pxmitbuf,
+			struct xmit_frame *pxmitframe)
+{
+	pxmitframe->pxmitbuf = pxmitbuf;
+	pxmitbuf->priv_data = pxmitframe;
+	pxmitframe->pxmit_urb[0] = pxmitbuf->pxmit_urb[0];
+	/* buffer addr assoc */
+	pxmitframe->buf_addr = pxmitbuf->pbuf + TXDESC_SIZE +
+		(((struct tx_desc *)pxmitbuf->pbuf)->txdw0 & 0x0000ffff);
+	r8712_append_mpdu_unit(pxmitbuf, pxmitframe);
+	r8712_free_xmitframe_ex(&pxmitframe->padapter->xmitpriv,
+				pxmitframe);
+	pxmitbuf->aggr_nr++;
+
+	return TXDESC_SIZE +
+		(((struct tx_desc *)pxmitbuf->pbuf)->txdw0 & 0x0000ffff);
+}
+
+void r8712_dump_aggr_xframe(struct xmit_buf *pxmitbuf,
+			    struct xmit_frame *pxmitframe)
+{
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+	struct tx_desc *ptxdesc = pxmitbuf->pbuf;
+	struct cmd_hdr *pcmd_hdr = (struct cmd_hdr *)
+		(pxmitbuf->pbuf + TXDESC_SIZE);
+	u16 total_length = (u16)(ptxdesc->txdw0 & 0xffff);
+
+	/* use 1st xmitframe as media */
+	xmitframe_xmitbuf_attach(pxmitframe, pxmitbuf);
+	pcmd_hdr->cmd_dw0 = cpu_to_le32(((total_length - CMD_HDR_SZ) &
+					 0x0000ffff) | (pcmd_hdr->cmd_dw0 &
+							0xffff0000));
+
+	/* urb length in cmd_dw1 */
+	pcmd_hdr->cmd_dw1 = cpu_to_le32((pxmitbuf->aggr_nr & 0xff) |
+					((total_length + TXDESC_SIZE) << 16));
+	pxmitframe->last[0] = 1;
+	pxmitframe->bpending[0] = false;
+	pxmitframe->mem_addr = pxmitbuf->pbuf;
+
+	if ((pdvobj->ishighspeed && ((total_length + TXDESC_SIZE) % 0x200) ==
+	     0) || ((!pdvobj->ishighspeed && ((total_length + TXDESC_SIZE) %
+					      0x40) == 0))) {
+		ptxdesc->txdw0 |= cpu_to_le32
+			(((TXDESC_SIZE + OFFSET_SZ + 8) << OFFSET_SHT) &
+			 0x00ff0000);
+		/*32 bytes for TX Desc + 8 bytes pending*/
+	} else {
+		ptxdesc->txdw0 |= cpu_to_le32
+			(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) &
+			 0x00ff0000);
+		/*default = 32 bytes for TX Desc*/
+	}
+	r8712_write_port(pxmitframe->padapter, RTL8712_DMA_H2CCMD,
+			total_length + TXDESC_SIZE, (u8 *)pxmitframe);
+}
+
+#endif
+
+static void update_txdesc(struct xmit_frame *pxmitframe, uint *pmem, int sz)
+{
+	uint qsel;
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct tx_desc *ptxdesc = (struct tx_desc *)pmem;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+#ifdef CONFIG_R8712_TX_AGGR
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+#endif
+	u8 blnSetTxDescOffset;
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	struct tx_desc txdesc_mp;
+
+	memcpy(&txdesc_mp, ptxdesc, sizeof(struct tx_desc));
+	memset(ptxdesc, 0, sizeof(struct tx_desc));
+	/* offset 0 */
+	ptxdesc->txdw0 |= cpu_to_le32(sz & 0x0000ffff);
+	if (pdvobj->ishighspeed) {
+		if (((sz + TXDESC_SIZE) % 512) == 0)
+			blnSetTxDescOffset = 1;
+		else
+			blnSetTxDescOffset = 0;
+	} else {
+		if (((sz + TXDESC_SIZE) % 64) == 0)
+			blnSetTxDescOffset = 1;
+		else
+			blnSetTxDescOffset = 0;
+	}
+	if (blnSetTxDescOffset) {
+		/* 32 bytes for TX Desc + 8 bytes pending */
+		ptxdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ + 8) <<
+			      OFFSET_SHT) & 0x00ff0000);
+	} else {
+		/* default = 32 bytes for TX Desc */
+		ptxdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) <<
+				  OFFSET_SHT) & 0x00ff0000);
+	}
+	ptxdesc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+	if (pxmitframe->frame_tag == DATA_FRAMETAG) {
+		/* offset 4 */
+		ptxdesc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x1f);
+
+#ifdef CONFIG_R8712_TX_AGGR
+		/* dirty workaround, need to check if it is aggr cmd. */
+		if ((u8 *)pmem != (u8 *)pxmitframe->pxmitbuf->pbuf) {
+			ptxdesc->txdw0 |= cpu_to_le32
+				((0x3 << TYPE_SHT) & TYPE_MSK);
+			qsel = (uint)(pattrib->qsel & 0x0000001f);
+			if (qsel == 2)
+				qsel = 0;
+			ptxdesc->txdw1 |= cpu_to_le32
+				((qsel << QSEL_SHT) & 0x00001f00);
+			ptxdesc->txdw2 = cpu_to_le32
+				((qsel << RTS_RC_SHT) & 0x001f0000);
+			ptxdesc->txdw6 |= cpu_to_le32
+				((0x5 << RSVD6_SHT) & RSVD6_MSK);
+		} else {
+			ptxdesc->txdw0 |= cpu_to_le32
+				((0x3 << TYPE_SHT) & TYPE_MSK);
+			ptxdesc->txdw1 |= cpu_to_le32
+				((0x13 << QSEL_SHT) & 0x00001f00);
+			qsel = (uint)(pattrib->qsel & 0x0000001f);
+			if (qsel == 2)
+				qsel = 0;
+			ptxdesc->txdw2 = cpu_to_le32
+				((qsel << RTS_RC_SHT) & 0x0001f000);
+			ptxdesc->txdw7 |= cpu_to_le32
+				(pcmdpriv->cmd_seq << 24);
+			pcmdpriv->cmd_seq++;
+		}
+		pattrib->qsel = 0x13;
+#else
+		qsel = (uint)(pattrib->qsel & 0x0000001f);
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+#endif
+		if (!pqospriv->qos_option)
+			ptxdesc->txdw1 |= cpu_to_le32(BIT(16));/*Non-QoS*/
+		if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
+			switch (pattrib->encrypt) {	/*SEC_TYPE*/
+			case _WEP40_:
+			case _WEP104_:
+				ptxdesc->txdw1 |= cpu_to_le32((0x01 << 22) &
+						  0x00c00000);
+				/*KEY_ID when WEP is used;*/
+				ptxdesc->txdw1 |=
+					cpu_to_le32((psecuritypriv->PrivacyKeyIndex << 17) &
+						    0x00060000);
+				break;
+			case _TKIP_:
+			case _TKIP_WTMIC_:
+				ptxdesc->txdw1 |= cpu_to_le32((0x02 << 22) &
+						  0x00c00000);
+				break;
+			case _AES_:
+				ptxdesc->txdw1 |= cpu_to_le32((0x03 << 22) &
+						  0x00c00000);
+				break;
+			case _NO_PRIVACY_:
+			default:
+				break;
+			}
+		}
+		/*offset 8*/
+		if (bmcst)
+			ptxdesc->txdw2 |= cpu_to_le32(BMC);
+
+		/*offset 12*/
+		/* f/w will increase the seqnum by itself, driver pass the
+		 * correct priority to fw.
+		 * fw will check the correct priority for increasing the
+		 * seqnum per tid. about usb using 4-endpoint, qsel points out
+		 * the correct mapping between AC&Endpoint,
+		 * the purpose is that correct mapping lets the MAC release
+		 * the AC Queue list correctly.
+		 */
+		ptxdesc->txdw3 = cpu_to_le32((pattrib->priority << SEQ_SHT) &
+				 0x0fff0000);
+		if ((pattrib->ether_type != 0x888e) &&
+		    (pattrib->ether_type != 0x0806) &&
+		    (pattrib->dhcp_pkt != 1)) {
+			/*Not EAP & ARP type data packet*/
+			if (phtpriv->ht_option == 1) { /*B/G/N Mode*/
+				if (!phtpriv->ampdu_enable)
+					ptxdesc->txdw2 |= cpu_to_le32(BK);
+			}
+		} else {
+			/* EAP data packet and ARP packet.
+			 * Use the 1M data rate to send the EAP/ARP packet.
+			 * This will maybe make the handshake smooth.
+			 */
+			/*driver uses data rate*/
+			ptxdesc->txdw4 = cpu_to_le32(0x80000000);
+			ptxdesc->txdw5 = cpu_to_le32(0x001f8000);/*1M*/
+		}
+		if (pattrib->pctrl == 1) { /* mp tx packets */
+			struct tx_desc *ptxdesc_mp;
+
+			ptxdesc_mp = &txdesc_mp;
+			/* offset 8 */
+			ptxdesc->txdw2 = ptxdesc_mp->txdw2;
+			if (bmcst)
+				ptxdesc->txdw2 |= cpu_to_le32(BMC);
+			ptxdesc->txdw2 |= cpu_to_le32(BK);
+			/* offset 16 */
+			ptxdesc->txdw4 = ptxdesc_mp->txdw4;
+			/* offset 20 */
+			ptxdesc->txdw5 = ptxdesc_mp->txdw5;
+			pattrib->pctrl = 0;/* reset to zero; */
+		}
+	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
+		/* offset 4 */
+		/* CAM_ID(MAC_ID), default=5; */
+		ptxdesc->txdw1 |= cpu_to_le32((0x05) & 0x1f);
+		qsel = (uint)(pattrib->qsel & 0x0000001f);
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+		ptxdesc->txdw1 |= cpu_to_le32(BIT(16));/* Non-QoS */
+		/* offset 8 */
+		if (bmcst)
+			ptxdesc->txdw2 |= cpu_to_le32(BMC);
+		/* offset 12 */
+		/* f/w will increase the seqnum by itself, driver pass the
+		 * correct priority to fw.
+		 * fw will check the correct priority for increasing the seqnum
+		 * per tid. about usb using 4-endpoint, qsel points out the
+		 * correct mapping between AC&Endpoint,
+		 * the purpose is that correct mapping let the MAC releases
+		 * the AC Queue list correctly.
+		 */
+		ptxdesc->txdw3 = cpu_to_le32((pattrib->priority << SEQ_SHT) &
+					      0x0fff0000);
+		/* offset 16 */
+		ptxdesc->txdw4 = cpu_to_le32(0x80002040);/*gtest*/
+		/* offset 20 */
+		ptxdesc->txdw5 = cpu_to_le32(0x001f8000);/* gtest 1M */
+	} else if (pxmitframe->frame_tag == TXAGG_FRAMETAG) {
+		/* offset 4 */
+		qsel = 0x13;
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+	} else {
+		/* offset 4 */
+		qsel = (uint)(pattrib->priority & 0x0000001f);
+		ptxdesc->txdw1 |= cpu_to_le32((qsel << QSEL_SHT) & 0x00001f00);
+		/*offset 8*/
+		/*offset 12*/
+		ptxdesc->txdw3 = cpu_to_le32((pattrib->seqnum << SEQ_SHT) &
+					      0x0fff0000);
+		/*offset 16*/
+		ptxdesc->txdw4 = cpu_to_le32(0x80002040);/*gtest*/
+		/*offset 20*/
+		ptxdesc->txdw5 = cpu_to_le32(0x001f9600);/*gtest*/
+	}
+}
+
+int r8712_xmitframe_complete(struct _adapter *padapter,
+			     struct xmit_priv *pxmitpriv,
+			     struct xmit_buf *pxmitbuf)
+{
+	struct hw_xmit *phwxmits;
+	sint hwentry;
+	struct xmit_frame *pxmitframe = NULL;
+#ifdef CONFIG_R8712_TX_AGGR
+	struct xmit_frame *p2ndxmitframe = NULL;
+#else
+	int res = _SUCCESS, xcnt = 0;
+#endif
+
+	phwxmits = pxmitpriv->hwxmits;
+	hwentry = pxmitpriv->hwxmit_entry;
+	if (!pxmitbuf) {
+		pxmitbuf = r8712_alloc_xmitbuf(pxmitpriv);
+		if (!pxmitbuf)
+			return false;
+#ifdef CONFIG_R8712_TX_AGGR
+		pxmitbuf->aggr_nr = 0;
+#endif
+	}
+	/* 1st frame dequeued */
+	pxmitframe = dequeue_xframe_ex(pxmitpriv, phwxmits, hwentry);
+	/* need to remember the 1st frame */
+	if (pxmitframe) {
+
+#ifdef CONFIG_R8712_TX_AGGR
+		/* 1. dequeue 2nd frame
+		 * 2. aggr if 2nd xframe is dequeued, else dump directly
+		 */
+		if (AGGR_NR_HIGH_BOUND > 1)
+			p2ndxmitframe = dequeue_xframe_ex(pxmitpriv, phwxmits,
+							hwentry);
+		if (pxmitframe->frame_tag != DATA_FRAMETAG) {
+			r8712_free_xmitbuf(pxmitpriv, pxmitbuf);
+			return false;
+		}
+		if (p2ndxmitframe)
+			if (p2ndxmitframe->frame_tag != DATA_FRAMETAG) {
+				r8712_free_xmitbuf(pxmitpriv, pxmitbuf);
+				return false;
+			}
+		r8712_xmitframe_aggr_1st(pxmitbuf, pxmitframe);
+		if (p2ndxmitframe) {
+			u16 total_length;
+
+			total_length = r8712_xmitframe_aggr_next(
+				pxmitbuf, p2ndxmitframe);
+			do {
+				p2ndxmitframe = dequeue_xframe_ex(
+					pxmitpriv, phwxmits, hwentry);
+				if (p2ndxmitframe)
+					total_length =
+						r8712_xmitframe_aggr_next(
+							pxmitbuf,
+							p2ndxmitframe);
+				else
+					break;
+			} while (total_length <= 0x1800 &&
+				pxmitbuf->aggr_nr <= AGGR_NR_HIGH_BOUND);
+		}
+		if (pxmitbuf->aggr_nr > 0)
+			r8712_dump_aggr_xframe(pxmitbuf, pxmitframe);
+
+#else
+
+		xmitframe_xmitbuf_attach(pxmitframe, pxmitbuf);
+		if (pxmitframe->frame_tag == DATA_FRAMETAG) {
+			if (pxmitframe->attrib.priority <= 15)
+				res = r8712_xmitframe_coalesce(padapter,
+					pxmitframe->pkt, pxmitframe);
+			/* always return ndis_packet after
+			 * r8712_xmitframe_coalesce
+			 */
+			r8712_xmit_complete(padapter, pxmitframe);
+		}
+		if (res == _SUCCESS)
+			dump_xframe(padapter, pxmitframe);
+		else
+			r8712_free_xmitframe_ex(pxmitpriv, pxmitframe);
+		xcnt++;
+#endif
+
+	} else { /* pxmitframe == NULL && p2ndxmitframe == NULL */
+		r8712_free_xmitbuf(pxmitpriv, pxmitbuf);
+		return false;
+	}
+	return true;
+}
+
+static void dump_xframe(struct _adapter *padapter,
+			struct xmit_frame *pxmitframe)
+{
+	int t, sz, w_sz;
+	u8 *mem_addr;
+	u32 ff_hwaddr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if (pxmitframe->attrib.ether_type != 0x0806) {
+		if (pxmitframe->attrib.ether_type != 0x888e)
+			r8712_issue_addbareq_cmd(padapter, pattrib->priority);
+	}
+	mem_addr = pxmitframe->buf_addr;
+	for (t = 0; t < pattrib->nr_frags; t++) {
+		if (t != (pattrib->nr_frags - 1)) {
+			sz = pxmitpriv->frag_len;
+			sz = sz - 4 - (psecuritypriv->sw_encrypt ? 0 :
+			     pattrib->icv_len);
+			pxmitframe->last[t] = 0;
+		} else {
+			sz = pattrib->last_txcmdsz;
+			pxmitframe->last[t] = 1;
+		}
+		update_txdesc(pxmitframe, (uint *)mem_addr, sz);
+		w_sz = sz + TXDESC_SIZE;
+		pxmitframe->mem_addr = mem_addr;
+		pxmitframe->bpending[t] = false;
+		ff_hwaddr = get_ff_hwaddr(pxmitframe);
+#ifdef CONFIG_R8712_TX_AGGR
+		r8712_write_port(padapter, RTL8712_DMA_H2CCMD, w_sz,
+				(unsigned char *)pxmitframe);
+#else
+		r8712_write_port(padapter, ff_hwaddr, w_sz,
+			   (unsigned char *)pxmitframe);
+#endif
+		mem_addr += w_sz;
+		mem_addr = (u8 *)RND4(((addr_t)(mem_addr)));
+	}
+}
+
+void r8712_xmit_direct(struct _adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	int res;
+
+	res = r8712_xmitframe_coalesce(padapter, pxmitframe->pkt, pxmitframe);
+	pxmitframe->pkt = NULL;
+	if (res == _SUCCESS)
+		dump_xframe(padapter, pxmitframe);
+}
+
+int r8712_xmit_enqueue(struct _adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (r8712_xmit_classifier(padapter, pxmitframe)) {
+		pxmitframe->pkt = NULL;
+		return _FAIL;
+	}
+	return _SUCCESS;
+}
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.h b/drivers/staging/rtl8712/rtl8712_xmit.h
new file mode 100644
index 000000000..5cd651a0d
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl8712_xmit.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL8712_XMIT_H_
+#define _RTL8712_XMIT_H_
+
+#define HWXMIT_ENTRY	4
+
+#define VO_QUEUE_INX	0
+#define VI_QUEUE_INX	1
+#define BE_QUEUE_INX	2
+#define BK_QUEUE_INX	3
+#define TS_QUEUE_INX	4
+#define MGT_QUEUE_INX	5
+#define BMC_QUEUE_INX	6
+#define BCN_QUEUE_INX	7
+
+#define HW_QUEUE_ENTRY	8
+
+#define TXDESC_SIZE 32
+#define TXDESC_OFFSET TXDESC_SIZE
+
+#define NR_AMSDU_XMITFRAME 8
+#define NR_TXAGG_XMITFRAME 8
+
+#define MAX_AMSDU_XMITBUF_SZ 8704
+#define MAX_TXAGG_XMITBUF_SZ 16384 /*16k*/
+
+#define tx_cmd tx_desc
+
+/*
+ *defined for TX DESC Operation
+ */
+
+#define MAX_TID (15)
+
+/*OFFSET 0*/
+#define OFFSET_SZ (0)
+#define OFFSET_SHT (16)
+#define OWN	BIT(31)
+#define FSG	BIT(27)
+#define LSG	BIT(26)
+#define TYPE_SHT (24)
+#define TYPE_MSK (0x03000000)
+
+/*OFFSET 4*/
+#define PKT_OFFSET_SZ (0)
+#define QSEL_SHT (8)
+#define HWPC BIT(31)
+
+/*OFFSET 8*/
+#define BMC BIT(7)
+#define BK BIT(30)
+#define AGG_EN BIT(29)
+#define RTS_RC_SHT (16)
+
+/*OFFSET 12*/
+#define SEQ_SHT (16)
+
+/*OFFSET 16*/
+#define TXBW BIT(18)
+
+/*OFFSET 20*/
+#define DISFB BIT(15)
+#define RSVD6_MSK (0x00E00000)
+#define RSVD6_SHT (21)
+
+struct tx_desc {
+	/*DWORD 0*/
+	__le32 txdw0;
+	__le32 txdw1;
+	__le32 txdw2;
+	__le32 txdw3;
+	__le32 txdw4;
+	__le32 txdw5;
+	__le32 txdw6;
+	__le32 txdw7;
+};
+
+union txdesc {
+	struct tx_desc txdesc;
+	unsigned int value[TXDESC_SIZE >> 2];
+};
+
+int r8712_xmitframe_complete(struct _adapter *padapter,
+			     struct xmit_priv *pxmitpriv,
+			     struct xmit_buf *pxmitbuf);
+void r8712_do_queue_select(struct _adapter *padapter,
+			   struct pkt_attrib *pattrib);
+
+#ifdef CONFIG_R8712_TX_AGGR
+void r8712_xmitframe_aggr_1st(struct xmit_buf *pxmitbuf,
+			      struct xmit_frame *pxmitframe);
+void r8712_dump_aggr_xframe(struct xmit_buf *pxmitbuf,
+			    struct xmit_frame *pxmitframe);
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
new file mode 100644
index 000000000..4be96df5a
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -0,0 +1,796 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_cmd.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_CMD_C_
+
+#include <linux/compiler.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+#include <linux/circ_buf.h>
+#include <linux/uaccess.h>
+#include <asm/byteorder.h>
+#include <linux/atomic.h>
+#include <linux/semaphore.h>
+#include <linux/rtnetlink.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "mlme_osdep.h"
+
+/*
+ * Caller and the r8712_cmd_thread can protect cmd_q by spin_lock.
+ * No irqsave is necessary.
+ */
+
+int r8712_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	init_completion(&pcmdpriv->cmd_queue_comp);
+	init_completion(&pcmdpriv->terminate_cmdthread_comp);
+
+	_init_queue(&(pcmdpriv->cmd_queue));
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	pcmdpriv->cmd_seq = 1;
+	pcmdpriv->cmd_allocated_buf = kmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
+					      GFP_ATOMIC);
+	if (!pcmdpriv->cmd_allocated_buf)
+		return -ENOMEM;
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ -
+			    ((addr_t)(pcmdpriv->cmd_allocated_buf) &
+			    (CMDBUFF_ALIGN_SZ - 1));
+	pcmdpriv->rsp_allocated_buf = kmalloc(MAX_RSPSZ + 4, GFP_ATOMIC);
+	if (!pcmdpriv->rsp_allocated_buf) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+		pcmdpriv->cmd_allocated_buf = NULL;
+		return -ENOMEM;
+	}
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 -
+			    ((addr_t)(pcmdpriv->rsp_allocated_buf) & 3);
+	pcmdpriv->cmd_issued_cnt = 0;
+	pcmdpriv->cmd_done_cnt = 0;
+	pcmdpriv->rsp_cnt = 0;
+	return 0;
+}
+
+int r8712_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	pevtpriv->event_seq = 0;
+	pevtpriv->evt_allocated_buf = kmalloc(MAX_EVTSZ + 4, GFP_ATOMIC);
+
+	if (!pevtpriv->evt_allocated_buf)
+		return -ENOMEM;
+	pevtpriv->evt_buf = pevtpriv->evt_allocated_buf  +  4 -
+			    ((addr_t)(pevtpriv->evt_allocated_buf) & 3);
+	pevtpriv->evt_done_cnt = 0;
+	return 0;
+}
+
+void r8712_free_evt_priv(struct evt_priv *pevtpriv)
+{
+	kfree(pevtpriv->evt_allocated_buf);
+}
+
+void r8712_free_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	if (pcmdpriv) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+		kfree(pcmdpriv->rsp_allocated_buf);
+	}
+}
+
+/*
+ * Calling Context:
+ *
+ * r8712_enqueue_cmd can only be called between kernel thread,
+ * since only spin_lock is used.
+ *
+ * ISR/Call-Back functions can't call this sub-function.
+ *
+ */
+
+void r8712_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *obj)
+{
+	struct __queue *queue;
+	unsigned long irqL;
+
+	if (pcmdpriv->padapter->eeprompriv.bautoload_fail_flag)
+		return;
+	if (!obj)
+		return;
+	queue = &pcmdpriv->cmd_queue;
+	spin_lock_irqsave(&queue->lock, irqL);
+	list_add_tail(&obj->list, &queue->queue);
+	spin_unlock_irqrestore(&queue->lock, irqL);
+	complete(&pcmdpriv->cmd_queue_comp);
+}
+
+struct cmd_obj *r8712_dequeue_cmd(struct  __queue *queue)
+{
+	unsigned long irqL;
+	struct cmd_obj *obj;
+
+	spin_lock_irqsave(&queue->lock, irqL);
+	obj = list_first_entry_or_null(&queue->queue,
+				       struct cmd_obj, list);
+	if (obj)
+		list_del_init(&obj->list);
+	spin_unlock_irqrestore(&queue->lock, irqL);
+	return obj;
+}
+
+void r8712_enqueue_cmd_ex(struct cmd_priv *pcmdpriv, struct cmd_obj *obj)
+{
+	unsigned long irqL;
+	struct  __queue *queue;
+
+	if (!obj)
+		return;
+	if (pcmdpriv->padapter->eeprompriv.bautoload_fail_flag)
+		return;
+	queue = &pcmdpriv->cmd_queue;
+	spin_lock_irqsave(&queue->lock, irqL);
+	list_add_tail(&obj->list, &queue->queue);
+	spin_unlock_irqrestore(&queue->lock, irqL);
+	complete(&pcmdpriv->cmd_queue_comp);
+}
+
+void r8712_free_cmd_obj(struct cmd_obj *pcmd)
+{
+	if ((pcmd->cmdcode != _JoinBss_CMD_) &&
+	    (pcmd->cmdcode != _CreateBss_CMD_))
+		kfree(pcmd->parmbuf);
+	if (pcmd->rsp) {
+		if (pcmd->rspsz != 0)
+			kfree(pcmd->rsp);
+	}
+	kfree(pcmd);
+}
+
+u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
+			struct ndis_802_11_ssid *pssid)
+	__must_hold(&padapter->mlmepriv.lock)
+{
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return _FAIL;
+	psurveyPara = kmalloc(sizeof(*psurveyPara), GFP_ATOMIC);
+	if (!psurveyPara) {
+		kfree(ph2c);
+		return _FAIL;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
+				   GEN_CMD_CODE(_SiteSurvey));
+	psurveyPara->bsslimit = cpu_to_le32(48);
+	psurveyPara->passive_mode = cpu_to_le32(pmlmepriv->passive_mode);
+	psurveyPara->ss_ssidlen = 0;
+	memset(psurveyPara->ss_ssid, 0, IW_ESSID_MAX_SIZE + 1);
+	if (pssid && pssid->SsidLength) {
+		int len = min_t(int, pssid->SsidLength, IW_ESSID_MAX_SIZE);
+
+		memcpy(psurveyPara->ss_ssid, pssid->Ssid, len);
+		psurveyPara->ss_ssidlen = cpu_to_le32(len);
+	}
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+	mod_timer(&pmlmepriv->scan_to_timer,
+		  jiffies + msecs_to_jiffies(SCANNING_TIMEOUT));
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_SITE_SURVEY);
+	complete(&padapter->rx_filter_ready);
+	return _SUCCESS;
+}
+
+int r8712_setdatarate_cmd(struct _adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj		*ph2c;
+	struct setdatarate_parm	*pbsetdataratepara;
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return -ENOMEM;
+	pbsetdataratepara = kmalloc(sizeof(*pbsetdataratepara), GFP_ATOMIC);
+	if (!pbsetdataratepara) {
+		kfree(ph2c);
+		return -ENOMEM;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara,
+				   GEN_CMD_CODE(_SetDataRate));
+	pbsetdataratepara->mac_id = 5;
+	memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+	return 0;
+}
+
+void r8712_set_chplan_cmd(struct _adapter *padapter, int chplan)
+{
+	struct cmd_obj *ph2c;
+	struct SetChannelPlan_param *psetchplanpara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	psetchplanpara = kmalloc(sizeof(*psetchplanpara), GFP_ATOMIC);
+	if (!psetchplanpara) {
+		kfree(ph2c);
+		return;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetchplanpara,
+				GEN_CMD_CODE(_SetChannelPlan));
+	psetchplanpara->ChannelPlan = chplan;
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+}
+
+int r8712_setrfreg_cmd(struct _adapter  *padapter, u8 offset, u32 val)
+{
+	struct cmd_obj *ph2c;
+	struct writeRF_parm *pwriterfparm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return -ENOMEM;
+	pwriterfparm = kmalloc(sizeof(*pwriterfparm), GFP_ATOMIC);
+	if (!pwriterfparm) {
+		kfree(ph2c);
+		return -ENOMEM;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
+	pwriterfparm->offset = offset;
+	pwriterfparm->value = val;
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+	return 0;
+}
+
+int r8712_getrfreg_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
+{
+	struct cmd_obj *ph2c;
+	struct readRF_parm *prdrfparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return -ENOMEM;
+	prdrfparm = kmalloc(sizeof(*prdrfparm), GFP_ATOMIC);
+	if (!prdrfparm) {
+		kfree(ph2c);
+		return -ENOMEM;
+	}
+	INIT_LIST_HEAD(&ph2c->list);
+	ph2c->cmdcode = GEN_CMD_CODE(_GetRFReg);
+	ph2c->parmbuf = (unsigned char *)prdrfparm;
+	ph2c->cmdsz =  sizeof(struct readRF_parm);
+	ph2c->rsp = pval;
+	ph2c->rspsz = sizeof(struct readRF_rsp);
+	prdrfparm->offset = offset;
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+	return 0;
+}
+
+void r8712_getbbrfreg_cmdrsp_callback(struct _adapter *padapter,
+				      struct cmd_obj *pcmd)
+{
+	kfree(pcmd->parmbuf);
+	kfree(pcmd);
+	padapter->mppriv.workparam.bcompleted = true;
+}
+
+void r8712_readtssi_cmdrsp_callback(struct _adapter *padapter,
+				struct cmd_obj *pcmd)
+{
+	kfree(pcmd->parmbuf);
+	kfree(pcmd);
+
+	padapter->mppriv.workparam.bcompleted = true;
+}
+
+int r8712_createbss_cmd(struct _adapter *padapter)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct wlan_bssid_ex *pdev_network =
+				 &padapter->registrypriv.dev_network;
+
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
+	pcmd = kmalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd)
+		return -ENOMEM;
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _CreateBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)pdev_network;
+	pcmd->cmdsz = r8712_get_wlan_bssid_ex_sz(pdev_network);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	/* notes: translate IELength & Length after assign to cmdsz; */
+	pdev_network->Length = pcmd->cmdsz;
+	pdev_network->IELength = pdev_network->IELength;
+	pdev_network->Ssid.SsidLength =	pdev_network->Ssid.SsidLength;
+	r8712_enqueue_cmd(pcmdpriv, pcmd);
+	return 0;
+}
+
+int r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
+{
+	struct wlan_bssid_ex *psecnetwork;
+	struct cmd_obj		*pcmd;
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE ndis_network_mode =
+		pnetwork->network.InfrastructureMode;
+
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
+	pcmd = kmalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd)
+		return -ENOMEM;
+
+	/* for hidden ap to set fw_state here */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE) !=
+	    true) {
+		switch (ndis_network_mode) {
+		case Ndis802_11IBSS:
+			pmlmepriv->fw_state |= WIFI_ADHOC_STATE;
+			break;
+		case Ndis802_11Infrastructure:
+			pmlmepriv->fw_state |= WIFI_STATION_STATE;
+			break;
+		case Ndis802_11APMode:
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+	}
+	psecnetwork = &psecuritypriv->sec_bss;
+	memcpy(psecnetwork, &pnetwork->network, sizeof(*psecnetwork));
+	psecuritypriv->authenticator_ie[0] = (unsigned char)
+					     psecnetwork->IELength;
+	if ((psecnetwork->IELength - 12) < (256 - 1))
+		memcpy(&psecuritypriv->authenticator_ie[1],
+			&psecnetwork->IEs[12], psecnetwork->IELength - 12);
+	else
+		memcpy(&psecuritypriv->authenticator_ie[1],
+			&psecnetwork->IEs[12], (256 - 1));
+	psecnetwork->IELength = 0;
+	/*
+	 * If the driver wants to use the bssid to create the connection.
+	 * If not, we copy the connecting AP's MAC address to it so that
+	 * the driver just has the bssid information for PMKIDList searching.
+	 */
+	if (!pmlmepriv->assoc_by_bssid)
+		ether_addr_copy(&pmlmepriv->assoc_bssid[0],
+				&pnetwork->network.MacAddress[0]);
+	psecnetwork->IELength = r8712_restruct_sec_ie(padapter,
+						&pnetwork->network.IEs[0],
+						&psecnetwork->IEs[0],
+						pnetwork->network.IELength);
+	pqospriv->qos_option = 0;
+	if (pregistrypriv->wmm_enable) {
+		u32 tmp_len;
+
+		tmp_len = r8712_restruct_wmm_ie(padapter,
+					  &pnetwork->network.IEs[0],
+					  &psecnetwork->IEs[0],
+					  pnetwork->network.IELength,
+					  psecnetwork->IELength);
+		if (psecnetwork->IELength != tmp_len) {
+			psecnetwork->IELength = tmp_len;
+			pqospriv->qos_option = 1; /* WMM IE in beacon */
+		} else {
+			pqospriv->qos_option = 0; /* no WMM IE in beacon */
+		}
+	}
+	if (pregistrypriv->ht_enable) {
+		/*
+		 * For WEP mode, we will use the bg mode to do the connection
+		 * to avoid some IOT issues, especially for Realtek 8192u
+		 * SoftAP.
+		 */
+		if ((padapter->securitypriv.PrivacyAlgrthm != _WEP40_) &&
+		    (padapter->securitypriv.PrivacyAlgrthm != _WEP104_)) {
+			/* restructure_ht_ie */
+			r8712_restructure_ht_ie(padapter,
+						&pnetwork->network.IEs[0],
+						&psecnetwork->IEs[0],
+						pnetwork->network.IELength,
+						&psecnetwork->IELength);
+		}
+	}
+	psecuritypriv->supplicant_ie[0] = (u8)psecnetwork->IELength;
+	if (psecnetwork->IELength < 255)
+		memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0],
+			psecnetwork->IELength);
+	else
+		memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0],
+			255);
+	/* get cmdsz before endian conversion */
+	pcmd->cmdsz = r8712_get_wlan_bssid_ex_sz(psecnetwork);
+#ifdef __BIG_ENDIAN
+	/* wlan_network endian conversion */
+	psecnetwork->Length = cpu_to_le32(psecnetwork->Length);
+	psecnetwork->Ssid.SsidLength = cpu_to_le32(
+				       psecnetwork->Ssid.SsidLength);
+	psecnetwork->Privacy = cpu_to_le32(psecnetwork->Privacy);
+	psecnetwork->Rssi = cpu_to_le32(psecnetwork->Rssi);
+	psecnetwork->NetworkTypeInUse = cpu_to_le32(
+					psecnetwork->NetworkTypeInUse);
+	psecnetwork->Configuration.ATIMWindow = cpu_to_le32(
+				psecnetwork->Configuration.ATIMWindow);
+	psecnetwork->Configuration.BeaconPeriod = cpu_to_le32(
+				 psecnetwork->Configuration.BeaconPeriod);
+	psecnetwork->Configuration.DSConfig = cpu_to_le32(
+				psecnetwork->Configuration.DSConfig);
+	psecnetwork->Configuration.FHConfig.DwellTime = cpu_to_le32(
+				psecnetwork->Configuration.FHConfig.DwellTime);
+	psecnetwork->Configuration.FHConfig.HopPattern = cpu_to_le32(
+				psecnetwork->Configuration.FHConfig.HopPattern);
+	psecnetwork->Configuration.FHConfig.HopSet = cpu_to_le32(
+				psecnetwork->Configuration.FHConfig.HopSet);
+	psecnetwork->Configuration.FHConfig.Length = cpu_to_le32(
+				psecnetwork->Configuration.FHConfig.Length);
+	psecnetwork->Configuration.Length = cpu_to_le32(
+				psecnetwork->Configuration.Length);
+	psecnetwork->InfrastructureMode = cpu_to_le32(
+				psecnetwork->InfrastructureMode);
+	psecnetwork->IELength = cpu_to_le32(psecnetwork->IELength);
+#endif
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _JoinBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)psecnetwork;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	r8712_enqueue_cmd(pcmdpriv, pcmd);
+	return 0;
+}
+
+void r8712_disassoc_cmd(struct _adapter *padapter) /* for sta_mode */
+{
+	struct cmd_obj *pdisconnect_cmd;
+	struct disconnect_parm *pdisconnect;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pdisconnect_cmd = kmalloc(sizeof(*pdisconnect_cmd), GFP_ATOMIC);
+	if (!pdisconnect_cmd)
+		return;
+	pdisconnect = kmalloc(sizeof(*pdisconnect), GFP_ATOMIC);
+	if (!pdisconnect) {
+		kfree(pdisconnect_cmd);
+		return;
+	}
+	init_h2fwcmd_w_parm_no_rsp(pdisconnect_cmd, pdisconnect,
+				   _DisConnect_CMD_);
+	r8712_enqueue_cmd(pcmdpriv, pdisconnect_cmd);
+}
+
+void r8712_setopmode_cmd(struct _adapter *padapter,
+		 enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	struct cmd_obj *ph2c;
+	struct setopmode_parm *psetop;
+
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	psetop = kmalloc(sizeof(*psetop), GFP_ATOMIC);
+	if (!psetop) {
+		kfree(ph2c);
+		return;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+	psetop->mode = (u8)networktype;
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+}
+
+void r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct sta_info *sta = (struct sta_info *)psta;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	psetstakey_para = kmalloc(sizeof(*psetstakey_para), GFP_ATOMIC);
+	if (!psetstakey_para) {
+		kfree(ph2c);
+		return;
+	}
+	psetstakey_rsp = kmalloc(sizeof(*psetstakey_rsp), GFP_ATOMIC);
+	if (!psetstakey_rsp) {
+		kfree(ph2c);
+		kfree(psetstakey_para);
+		return;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+	ph2c->rsp = (u8 *)psetstakey_rsp;
+	ph2c->rspsz = sizeof(struct set_stakey_rsp);
+	ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		psetstakey_para->algorithm = (unsigned char)
+					    psecuritypriv->PrivacyAlgrthm;
+	else
+		GET_ENCRY_ALGO(psecuritypriv, sta,
+			       psetstakey_para->algorithm, false);
+	if (unicast_key)
+		memcpy(&psetstakey_para->key, &sta->x_UncstKey, 16);
+	else
+		memcpy(&psetstakey_para->key,
+			&psecuritypriv->XGrpKey[
+			psecuritypriv->XGrpKeyid - 1]. skey, 16);
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+}
+
+void r8712_setMacAddr_cmd(struct _adapter *padapter, const u8 *mac_addr)
+{
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct SetMacAddr_param	*psetMacAddr_para;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	psetMacAddr_para = kmalloc(sizeof(*psetMacAddr_para), GFP_ATOMIC);
+	if (!psetMacAddr_para) {
+		kfree(ph2c);
+		return;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetMacAddr_para,
+				   _SetMacAddress_CMD_);
+	ether_addr_copy(psetMacAddr_para->MacAddr, mac_addr);
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+}
+
+void r8712_addbareq_cmd(struct _adapter *padapter, u8 tid)
+{
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj		*ph2c;
+	struct addBaReq_parm	*paddbareq_parm;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	paddbareq_parm = kmalloc(sizeof(*paddbareq_parm), GFP_ATOMIC);
+	if (!paddbareq_parm) {
+		kfree(ph2c);
+		return;
+	}
+	paddbareq_parm->tid = tid;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm,
+				   GEN_CMD_CODE(_AddBAReq));
+	r8712_enqueue_cmd_ex(pcmdpriv, ph2c);
+}
+
+void r8712_wdg_wk_cmd(struct _adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvint_cmd_parm  *pdrvintcmd_param;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	pdrvintcmd_param = kmalloc(sizeof(*pdrvintcmd_param), GFP_ATOMIC);
+	if (!pdrvintcmd_param) {
+		kfree(ph2c);
+		return;
+	}
+	pdrvintcmd_param->i_cid = WDG_WK_CID;
+	pdrvintcmd_param->sz = 0;
+	pdrvintcmd_param->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvintcmd_param, _DRV_INT_CMD_);
+	r8712_enqueue_cmd_ex(pcmdpriv, ph2c);
+}
+
+void r8712_survey_cmd_callback(struct _adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS)
+		clr_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_disassoc_cmd_callback(struct _adapter *padapter,
+				 struct cmd_obj *pcmd)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		spin_lock_irqsave(&pmlmepriv->lock, irqL);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+		return;
+	}
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_joinbss_cmd_callback(struct _adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS)
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_createbss_cmd_callback(struct _adapter *padapter,
+				  struct cmd_obj *pcmd)
+{
+	unsigned long irqL;
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+
+	if (pcmd->res != H2C_SUCCESS)
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
+	del_timer(&pmlmepriv->assoc_timer);
+#ifdef __BIG_ENDIAN
+	/* endian_convert */
+	pnetwork->Length = le32_to_cpu(pnetwork->Length);
+	pnetwork->Ssid.SsidLength = le32_to_cpu(pnetwork->Ssid.SsidLength);
+	pnetwork->Privacy = le32_to_cpu(pnetwork->Privacy);
+	pnetwork->Rssi = le32_to_cpu(pnetwork->Rssi);
+	pnetwork->NetworkTypeInUse = le32_to_cpu(pnetwork->NetworkTypeInUse);
+	pnetwork->Configuration.ATIMWindow =
+		le32_to_cpu(pnetwork->Configuration.ATIMWindow);
+	pnetwork->Configuration.DSConfig =
+		le32_to_cpu(pnetwork->Configuration.DSConfig);
+	pnetwork->Configuration.FHConfig.DwellTime =
+		le32_to_cpu(pnetwork->Configuration.FHConfig.DwellTime);
+	pnetwork->Configuration.FHConfig.HopPattern =
+		le32_to_cpu(pnetwork->Configuration.FHConfig.HopPattern);
+	pnetwork->Configuration.FHConfig.HopSet =
+		le32_to_cpu(pnetwork->Configuration.FHConfig.HopSet);
+	pnetwork->Configuration.FHConfig.Length =
+		le32_to_cpu(pnetwork->Configuration.FHConfig.Length);
+	pnetwork->Configuration.Length =
+		le32_to_cpu(pnetwork->Configuration.Length);
+	pnetwork->InfrastructureMode =
+		le32_to_cpu(pnetwork->InfrastructureMode);
+	pnetwork->IELength = le32_to_cpu(pnetwork->IELength);
+#endif
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if ((pmlmepriv->fw_state) & WIFI_AP_STATE) {
+		psta = r8712_get_stainfo(&padapter->stapriv,
+					 pnetwork->MacAddress);
+		if (!psta) {
+			psta = r8712_alloc_stainfo(&padapter->stapriv,
+						   pnetwork->MacAddress);
+			if (!psta)
+				goto createbss_cmd_fail;
+		}
+		r8712_indicate_connect(padapter);
+	} else {
+		pwlan = _r8712_alloc_network(pmlmepriv);
+		if (!pwlan) {
+			pwlan = r8712_get_oldest_wlan_network(
+				&pmlmepriv->scanned_queue);
+			if (!pwlan)
+				goto createbss_cmd_fail;
+			pwlan->last_scanned = jiffies;
+		} else {
+			list_add_tail(&(pwlan->list),
+					 &pmlmepriv->scanned_queue.queue);
+		}
+		pnetwork->Length = r8712_get_wlan_bssid_ex_sz(pnetwork);
+		memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
+		pwlan->fixed = true;
+		memcpy(&tgt_network->network, pnetwork,
+			(r8712_get_wlan_bssid_ex_sz(pnetwork)));
+		if (pmlmepriv->fw_state & _FW_UNDER_LINKING)
+			pmlmepriv->fw_state ^= _FW_UNDER_LINKING;
+		/*
+		 * we will set _FW_LINKED when there is one more sat to
+		 * join us (stassoc_event_callback)
+		 */
+	}
+createbss_cmd_fail:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_setstaKey_cmdrsp_callback(struct _adapter *padapter,
+				     struct cmd_obj *pcmd)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)
+						(pcmd->rsp);
+	struct sta_info *psta = r8712_get_stainfo(pstapriv,
+						  psetstakey_rsp->addr);
+
+	if (!psta)
+		goto exit;
+	psta->aid = psta->mac_id = psetstakey_rsp->keyid; /*CAM_ID(CAM_ENTRY)*/
+exit:
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_setassocsta_cmdrsp_callback(struct _adapter *padapter,
+				       struct cmd_obj *pcmd)
+{
+	unsigned long	irqL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct set_assocsta_parm *passocsta_parm =
+				(struct set_assocsta_parm *)(pcmd->parmbuf);
+	struct set_assocsta_rsp *passocsta_rsp =
+				(struct set_assocsta_rsp *) (pcmd->rsp);
+	struct sta_info *psta = r8712_get_stainfo(pstapriv,
+						  passocsta_parm->addr);
+
+	if (!psta)
+		return;
+	psta->aid = psta->mac_id = passocsta_rsp->cam_id;
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE)) &&
+	    (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)))
+		pmlmepriv->fw_state ^= _FW_UNDER_LINKING;
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	r8712_free_cmd_obj(pcmd);
+}
+
+void r8712_disconnectCtrlEx_cmd(struct _adapter *adapter, u32 enableDrvCtrl,
+			u32 tryPktCnt, u32 tryPktInterval, u32 firstStageTO)
+{
+	struct cmd_obj *ph2c;
+	struct DisconnectCtrlEx_param *param;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+
+	ph2c = kmalloc(sizeof(*ph2c), GFP_ATOMIC);
+	if (!ph2c)
+		return;
+	param = kzalloc(sizeof(*param), GFP_ATOMIC);
+	if (!param) {
+		kfree(ph2c);
+		return;
+	}
+
+	param->EnableDrvCtrl = (unsigned char)enableDrvCtrl;
+	param->TryPktCnt = (unsigned char)tryPktCnt;
+	param->TryPktInterval = (unsigned char)tryPktInterval;
+	param->FirstStageTO = (unsigned int)firstStageTO;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, param,
+				GEN_CMD_CODE(_DisconnectCtrlEx));
+	r8712_enqueue_cmd(pcmdpriv, ph2c);
+}
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.h b/drivers/staging/rtl8712/rtl871x_cmd.h
new file mode 100644
index 000000000..8453d8de8
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_cmd.h
@@ -0,0 +1,761 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_CMD_H_
+#define __RTL871X_CMD_H_
+
+#include "wlan_bssdef.h"
+#include "rtl871x_rf.h"
+#define C2H_MEM_SZ (16*1024)
+
+#include "osdep_service.h"
+#include "ieee80211.h"
+
+#define FREE_CMDOBJ_SZ	128
+#define MAX_CMDSZ	512
+#define MAX_RSPSZ	512
+#define MAX_EVTSZ	1024
+#define CMDBUFF_ALIGN_SZ 512
+
+struct cmd_obj {
+	u16	cmdcode;
+	u8	res;
+	u8	*parmbuf;
+	u32	cmdsz;
+	u8	*rsp;
+	u32	rspsz;
+	struct list_head list;
+};
+
+struct cmd_priv {
+	struct completion cmd_queue_comp;
+	struct completion terminate_cmdthread_comp;
+	struct  __queue	cmd_queue;
+	u8 cmd_seq;
+	u8 *cmd_buf;	/*shall be non-paged, and 4 bytes aligned*/
+	u8 *cmd_allocated_buf;
+	u8 *rsp_buf;	/*shall be non-paged, and 4 bytes aligned*/
+	u8 *rsp_allocated_buf;
+	u32 cmd_issued_cnt;
+	u32 cmd_done_cnt;
+	u32 rsp_cnt;
+	struct _adapter *padapter;
+};
+
+struct evt_obj {
+	u16 evtcode;
+	u8 res;
+	u8 *parmbuf;
+	u32 evtsz;
+	struct list_head list;
+};
+
+struct	evt_priv {
+	struct  __queue	evt_queue;
+	u8	event_seq;
+	u8	*evt_buf;	/*shall be non-paged, and 4 bytes aligned*/
+	u8	*evt_allocated_buf;
+	u32	evt_done_cnt;
+};
+
+#define init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code) \
+do {\
+	INIT_LIST_HEAD(&pcmd->list);\
+	pcmd->cmdcode = code;\
+	pcmd->parmbuf = (u8 *)(pparm);\
+	pcmd->cmdsz = sizeof(*pparm);\
+	pcmd->rsp = NULL;\
+	pcmd->rspsz = 0;\
+} while (0)
+
+void r8712_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+void r8712_enqueue_cmd_ex(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+struct cmd_obj *r8712_dequeue_cmd(struct  __queue *queue);
+void r8712_free_cmd_obj(struct cmd_obj *pcmd);
+int r8712_cmd_thread(void *context);
+int r8712_init_cmd_priv(struct cmd_priv *pcmdpriv);
+void r8712_free_cmd_priv(struct cmd_priv *pcmdpriv);
+int r8712_init_evt_priv(struct evt_priv *pevtpriv);
+void r8712_free_evt_priv(struct evt_priv *pevtpriv);
+
+enum rtl871x_drvint_cid {
+	NONE_WK_CID,
+	WDG_WK_CID,
+	MAX_WK_CID
+};
+
+enum RFINTFS {
+	SWSI,
+	HWSI,
+	HWPI,
+};
+
+/*
+ * Caller Mode: Infra, Ad-HoC(C)
+ * Notes: To enter USB suspend mode
+ * Command Mode
+ */
+struct usb_suspend_parm {
+	u32 action; /* 1: sleep, 0:resume */
+};
+
+/*
+ * Caller Mode: Infra, Ad-HoC(C)
+ * Notes: To disconnect the current associated BSS
+ * Command Mode
+ */
+struct disconnect_parm {
+	u32 rsvd;
+};
+
+/*
+ * Caller Mode: AP, Ad-HoC, Infra
+ * Notes: To set the NIC mode of RTL8711
+ * Command Mode
+ * The definition of mode:
+ *
+ * #define IW_MODE_AUTO	0	// Let the driver decides which AP to join
+ * #define IW_MODE_ADHOC	1	// Single cell network (Ad-Hoc Clients)
+ * #define IW_MODE_INFRA	2	// Multi cell network, roaming, ..
+ * #define IW_MODE_MASTER	3	// Synchronisation master or AP
+ * #define IW_MODE_REPEAT	4	// Wireless Repeater (forwarder)
+ * #define IW_MODE_SECOND	5	// Secondary master/repeater (backup)
+ * #define IW_MODE_MONITOR	6	// Passive monitor (listen only)
+ */
+struct	setopmode_parm {
+	u8	mode;
+	u8	rsvd[3];
+};
+
+/*
+ * Caller Mode: AP, Ad-HoC, Infra
+ * Notes: To ask RTL8711 performing site-survey
+ * Command-Event Mode
+ */
+struct sitesurvey_parm {
+	__le32	passive_mode;	/*active: 1, passive: 0 */
+	__le32	bsslimit;	/* 1 ~ 48 */
+	__le32	ss_ssidlen;
+	u8	ss_ssid[IW_ESSID_MAX_SIZE + 1];
+};
+
+/*
+ * Caller Mode: Any
+ * Notes: To set the auth type of RTL8711. open/shared/802.1x
+ * Command Mode
+ */
+struct setauth_parm {
+	u8 mode;  /*0: legacy open, 1: legacy shared 2: 802.1x*/
+	u8 _1x;   /*0: PSK, 1: TLS*/
+	u8 rsvd[2];
+};
+
+/*
+ * Caller Mode: Infra
+ * a. algorithm: wep40, wep104, tkip & aes
+ * b. keytype: grp key/unicast key
+ * c. key contents
+ *
+ * when shared key ==> keyid is the camid
+ * when 802.1x ==> keyid [0:1] ==> grp key
+ * when 802.1x ==> keyid > 2 ==> unicast key
+ */
+struct setkey_parm {
+	u8	algorithm;	/* encryption algorithm, could be none, wep40,
+				 * TKIP, CCMP, wep104
+				 */
+	u8	keyid;
+	u8	grpkey;		/* 1: this is the grpkey for 802.1x.
+				 * 0: this is the unicast key for 802.1x
+				 */
+	u8	key[16];	/* this could be 40 or 104 */
+};
+
+/*
+ * When in AP or Ad-Hoc mode, this is used to
+ * allocate an sw/hw entry for a newly associated sta.
+ * Command
+ * when shared key ==> algorithm/keyid
+ */
+struct set_stakey_parm {
+	u8	addr[ETH_ALEN];
+	u8	algorithm;
+	u8	key[16];
+};
+
+struct set_stakey_rsp {
+	u8	addr[ETH_ALEN];
+	u8	keyid;
+	u8	rsvd;
+};
+
+struct SetMacAddr_param {
+	u8	MacAddr[ETH_ALEN];
+};
+
+/*
+ *	Caller Ad-Hoc/AP
+ *
+ *	Command -Rsp(AID == CAMID) mode
+ *
+ *	This is to force fw to add an sta_data entry per driver's request.
+ *
+ *	FW will write an cam entry associated with it.
+ *
+ */
+struct set_assocsta_parm {
+	u8	addr[ETH_ALEN];
+};
+
+struct set_assocsta_rsp {
+	u8	cam_id;
+	u8	rsvd[3];
+};
+
+/*
+ *	Caller Ad-Hoc/AP
+ *
+ *	Command mode
+ *
+ *	This is to force fw to del an sta_data entry per driver's request
+ *
+ *	FW will invalidate the cam entry associated with it.
+ *
+ */
+struct del_assocsta_parm {
+	u8	addr[ETH_ALEN];
+};
+
+/*
+ *	Caller Mode: AP/Ad-HoC(M)
+ *
+ *	Notes: To notify fw that given staid has changed its power state
+ *
+ *	Command Mode
+ *
+ */
+struct setstapwrstate_parm {
+	u8	staid;
+	u8	status;
+	u8	hwaddr[6];
+};
+
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To setup the basic rate of RTL8711
+ *
+ *	Command Mode
+ *
+ */
+struct	setbasicrate_parm {
+	u8	basicrates[NumRates];
+};
+
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To read the current basic rate
+ *
+ *	Command-Rsp Mode
+ *
+ */
+struct getbasicrate_parm {
+	u32 rsvd;
+};
+
+struct getbasicrate_rsp {
+	u8 basicrates[NumRates];
+};
+
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To setup the data rate of RTL8711
+ *
+ *	Command Mode
+ *
+ */
+struct setdatarate_parm {
+	u8	mac_id;
+	u8	datarates[NumRates];
+};
+
+enum _RT_CHANNEL_DOMAIN {
+	RT_CHANNEL_DOMAIN_FCC = 0,
+	RT_CHANNEL_DOMAIN_IC = 1,
+	RT_CHANNEL_DOMAIN_ETSI = 2,
+	RT_CHANNEL_DOMAIN_SPAIN = 3,
+	RT_CHANNEL_DOMAIN_FRANCE = 4,
+	RT_CHANNEL_DOMAIN_MKK = 5,
+	RT_CHANNEL_DOMAIN_MKK1 = 6,
+	RT_CHANNEL_DOMAIN_ISRAEL = 7,
+	RT_CHANNEL_DOMAIN_TELEC = 8,
+
+	/* Be compatible with old channel plan. No good! */
+	RT_CHANNEL_DOMAIN_MIC = 9,
+	RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN = 10,
+	RT_CHANNEL_DOMAIN_WORLD_WIDE_13 = 11,
+	RT_CHANNEL_DOMAIN_TELEC_NETGEAR = 12,
+
+	RT_CHANNEL_DOMAIN_NCC = 13,
+	RT_CHANNEL_DOMAIN_5G = 14,
+	RT_CHANNEL_DOMAIN_5G_40M = 15,
+ /*===== Add new channel plan above this line===============*/
+	RT_CHANNEL_DOMAIN_MAX,
+};
+
+struct SetChannelPlan_param {
+	enum _RT_CHANNEL_DOMAIN ChannelPlan;
+};
+
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To read the current data rate
+ *
+ *	Command-Rsp Mode
+ *
+ */
+struct getdatarate_parm {
+	u32 rsvd;
+
+};
+
+struct getdatarate_rsp {
+	u8 datarates[NumRates];
+};
+
+/*
+ *	Caller Mode: Any
+ *	AP: AP can use the info for the contents of beacon frame
+ *	Infra: STA can use the info when sitesurveying
+ *	Ad-HoC(M): Like AP
+ *	Ad-HoC(C): Like STA
+ *
+ *
+ *	Notes: To set the phy capability of the NIC
+ *
+ *	Command Mode
+ *
+ */
+
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To set the channel/modem/band
+ *	This command will be used when channel/modem/band is changed.
+ *
+ *	Command Mode
+ *
+ */
+/*
+ *	Caller Mode: Any
+ *
+ *	Notes: To get the current setting of channel/modem/band
+ *
+ *	Command-Rsp Mode
+ *
+ */
+struct	getphy_rsp {
+	u8	rfchannel;
+	u8	modem;
+};
+
+struct readBB_parm {
+	u8	offset;
+};
+
+struct readBB_rsp {
+	u8	value;
+};
+
+struct readTSSI_parm {
+	u8	offset;
+};
+
+struct readTSSI_rsp {
+	u8	value;
+};
+
+struct writeBB_parm {
+	u8	offset;
+	u8	value;
+};
+
+struct writePTM_parm {
+	u8	type;
+};
+
+struct readRF_parm {
+	u8	offset;
+};
+
+struct readRF_rsp {
+	u32	value;
+};
+
+struct writeRF_parm {
+	u32	offset;
+	u32	value;
+};
+
+struct setrfintfs_parm {
+	u8	rfintfs;
+};
+
+struct getrfintfs_parm {
+	u8	rfintfs;
+};
+
+/*
+ *	Notes: This command is used for H2C/C2H loopback testing
+ *
+ *	mac[0] == 0
+ *	==> CMD mode, return H2C_SUCCESS.
+ *	The following condition must be true under CMD mode
+ *		mac[1] == mac[4], mac[2] == mac[3], mac[0]=mac[5]= 0;
+ *		s0 == 0x1234, s1 == 0xabcd, w0 == 0x78563412, w1 == 0x5aa5def7;
+ *		s2 == (b1 << 8 | b0);
+ *
+ *	mac[0] == 1
+ *	==> CMD_RSP mode, return H2C_SUCCESS_RSP
+ *
+ *	The rsp layout shall be:
+ *	rsp:			parm:
+ *		mac[0]  =   mac[5];
+ *		mac[1]  =   mac[4];
+ *		mac[2]  =   mac[3];
+ *		mac[3]  =   mac[2];
+ *		mac[4]  =   mac[1];
+ *		mac[5]  =   mac[0];
+ *		s0		=   s1;
+ *		s1		=   swap16(s0);
+ *		w0		=	swap32(w1);
+ *		b0		=	b1
+ *		s2		=	s0 + s1
+ *		b1		=	b0
+ *		w1		=	w0
+ *
+ *	mac[0] ==	2
+ *	==> CMD_EVENT mode, return	H2C_SUCCESS
+ *	The event layout shall be:
+ *	event:	     parm:
+ *	mac[0]  =   mac[5];
+ *	mac[1]  =   mac[4];
+ *	mac[2]  =   event's sequence number, starting from 1 to parm's marc[3]
+ *	mac[3]  =   mac[2];
+ *	mac[4]  =   mac[1];
+ *	mac[5]  =   mac[0];
+ *	s0		=   swap16(s0) - event.mac[2];
+ *	s1		=   s1 + event.mac[2];
+ *	w0		=	swap32(w0);
+ *	b0		=	b1
+ *	s2		=	s0 + event.mac[2]
+ *	b1		=	b0
+ *	w1		=	swap32(w1) - event.mac[2];
+ *
+ *	parm->mac[3] is the total event counts that host requested.
+ *
+ *
+ *	event will be the same with the cmd's param.
+ *
+ */
+
+/* CMD param Formart for DRV INTERNAL CMD HDL*/
+struct drvint_cmd_parm {
+	int i_cid; /*internal cmd id*/
+	int sz; /* buf sz*/
+	unsigned char *pbuf;
+};
+
+/*------------------- Below are used for RF/BB tuning ---------------------*/
+
+struct	setantenna_parm {
+	u8	tx_antset;
+	u8	rx_antset;
+	u8	tx_antenna;
+	u8	rx_antenna;
+};
+
+struct	enrateadaptive_parm {
+	u32	en;
+};
+
+struct settxagctbl_parm {
+	u32	txagc[MAX_RATES_LENGTH];
+};
+
+struct gettxagctbl_parm {
+	u32 rsvd;
+};
+
+struct gettxagctbl_rsp {
+	u32	txagc[MAX_RATES_LENGTH];
+};
+
+struct setagcctrl_parm {
+	u32	agcctrl;	/* 0: pure hw, 1: fw */
+};
+
+struct setssup_parm	{
+	u32	ss_ForceUp[MAX_RATES_LENGTH];
+};
+
+struct getssup_parm	{
+	u32 rsvd;
+};
+
+struct getssup_rsp	{
+	u8	ss_ForceUp[MAX_RATES_LENGTH];
+};
+
+struct setssdlevel_parm	{
+	u8	ss_DLevel[MAX_RATES_LENGTH];
+};
+
+struct getssdlevel_parm	{
+	u32 rsvd;
+};
+
+struct getssdlevel_rsp	{
+	u8	ss_DLevel[MAX_RATES_LENGTH];
+};
+
+struct setssulevel_parm	{
+	u8	ss_ULevel[MAX_RATES_LENGTH];
+};
+
+struct getssulevel_parm	{
+	u32 rsvd;
+};
+
+struct getssulevel_rsp	{
+	u8	ss_ULevel[MAX_RATES_LENGTH];
+};
+
+struct	setcountjudge_parm {
+	u8	count_judge[MAX_RATES_LENGTH];
+};
+
+struct	getcountjudge_parm {
+	u32 rsvd;
+};
+
+struct	getcountjudge_rsp {
+	u8	count_judge[MAX_RATES_LENGTH];
+};
+
+struct setpwrmode_parm  {
+	u8	mode;
+	u8	flag_low_traffic_en;
+	u8	flag_lpnav_en;
+	u8	flag_rf_low_snr_en;
+	u8	flag_dps_en; /* 1: dps, 0: 32k */
+	u8	bcn_rx_en;
+	u8	bcn_pass_cnt;	  /* fw report one beacon information to
+				   * driver  when it receives bcn_pass_cnt
+				   * beacons.
+				   */
+	u8	bcn_to;		  /* beacon TO (ms). ¡§=0¡¨ no limit.*/
+	u16	bcn_itv;
+	u8	app_itv; /* only for VOIP mode. */
+	u8	awake_bcn_itv;
+	u8	smart_ps;
+	u8	bcn_pass_time;	/* unit: 100ms */
+};
+
+struct setatim_parm {
+	u8 op;   /*0: add, 1:del*/
+	u8 txid; /* id of dest station.*/
+};
+
+struct setratable_parm {
+	u8 ss_ForceUp[NumRates];
+	u8 ss_ULevel[NumRates];
+	u8 ss_DLevel[NumRates];
+	u8 count_judge[NumRates];
+};
+
+struct getratable_parm {
+	uint rsvd;
+};
+
+struct getratable_rsp {
+	u8 ss_ForceUp[NumRates];
+	u8 ss_ULevel[NumRates];
+	u8 ss_DLevel[NumRates];
+	u8 count_judge[NumRates];
+};
+
+/*to get TX,RX retry count*/
+struct gettxretrycnt_parm {
+	unsigned int rsvd;
+};
+
+struct gettxretrycnt_rsp {
+	unsigned long tx_retrycnt;
+};
+
+struct getrxretrycnt_parm {
+	unsigned int rsvd;
+};
+
+struct getrxretrycnt_rsp {
+	unsigned long rx_retrycnt;
+};
+
+/*to get BCNOK,BCNERR count*/
+struct getbcnokcnt_parm {
+	unsigned int rsvd;
+};
+
+struct getbcnokcnt_rsp {
+	unsigned long bcnokcnt;
+};
+
+struct getbcnerrcnt_parm {
+	unsigned int rsvd;
+};
+
+struct getbcnerrcnt_rsp {
+	unsigned long bcnerrcnt;
+};
+
+/* to get current TX power level*/
+struct getcurtxpwrlevel_parm {
+	unsigned int rsvd;
+};
+
+struct getcurtxpwrlevel_rsp {
+	unsigned short tx_power;
+};
+
+/*dynamic on/off DIG*/
+struct setdig_parm {
+	unsigned char dig_on;	/* 1:on , 0:off */
+};
+
+/*dynamic on/off RA*/
+struct setra_parm {
+	unsigned char ra_on;	/* 1:on , 0:off */
+};
+
+struct setprobereqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setassocreqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setproberspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct setassocrspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[];
+};
+
+struct addBaReq_parm {
+	unsigned int tid;
+};
+
+/*H2C Handler index: 46 */
+struct SetChannel_parm {
+	u32 curr_ch;
+};
+
+/*H2C Handler index: 61 */
+struct DisconnectCtrlEx_param {
+	/* MAXTIME = (2 * FirstStageTO) + (TryPktCnt * TryPktInterval) */
+	unsigned char EnableDrvCtrl;
+	unsigned char TryPktCnt;
+	unsigned char TryPktInterval; /* Unit: ms */
+	unsigned char rsvd;
+	unsigned int  FirstStageTO; /* Unit: ms */
+};
+
+#define GEN_CMD_CODE(cmd)	cmd ## _CMD_
+
+/*
+ * Result:
+ * 0x00: success
+ * 0x01: success, and check Response.
+ * 0x02: cmd ignored due to duplicated sequence number
+ * 0x03: cmd dropped due to invalid cmd code
+ * 0x04: reserved.
+ */
+
+#define H2C_RSP_OFFSET			512
+#define H2C_SUCCESS			0x00
+#define H2C_SUCCESS_RSP			0x01
+#define H2C_DUPLICATED			0x02
+#define H2C_DROPPED			0x03
+#define H2C_PARAMETERS_ERROR		0x04
+#define H2C_REJECTED			0x05
+#define H2C_CMD_OVERFLOW		0x06
+#define H2C_RESERVED			0x07
+
+void r8712_setMacAddr_cmd(struct _adapter *padapter, const u8 *mac_addr);
+u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
+			struct ndis_802_11_ssid *pssid);
+int r8712_createbss_cmd(struct _adapter *padapter);
+void r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key);
+int r8712_joinbss_cmd(struct _adapter *padapter,
+		      struct wlan_network *pnetwork);
+void r8712_disassoc_cmd(struct _adapter *padapter);
+void r8712_setopmode_cmd(struct _adapter *padapter,
+		 enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype);
+int r8712_setdatarate_cmd(struct _adapter *padapter, u8 *rateset);
+void r8712_set_chplan_cmd(struct _adapter  *padapter, int chplan);
+int r8712_getrfreg_cmd(struct _adapter *padapter, u8 offset, u8 *pval);
+int r8712_setrfreg_cmd(struct _adapter  *padapter, u8 offset, u32 val);
+void r8712_addbareq_cmd(struct _adapter *padapter, u8 tid);
+void r8712_wdg_wk_cmd(struct _adapter *padapter);
+void r8712_survey_cmd_callback(struct _adapter  *padapter,
+			       struct cmd_obj *pcmd);
+void r8712_disassoc_cmd_callback(struct _adapter  *padapter,
+				 struct cmd_obj *pcmd);
+void r8712_joinbss_cmd_callback(struct _adapter  *padapter,
+				struct cmd_obj *pcmd);
+void r8712_createbss_cmd_callback(struct _adapter *padapter,
+				  struct cmd_obj *pcmd);
+void r8712_getbbrfreg_cmdrsp_callback(struct _adapter *padapter,
+				      struct cmd_obj *pcmd);
+void r8712_readtssi_cmdrsp_callback(struct _adapter *padapter,
+				struct cmd_obj *pcmd);
+void r8712_setstaKey_cmdrsp_callback(struct _adapter  *padapter,
+				     struct cmd_obj *pcmd);
+void r8712_setassocsta_cmdrsp_callback(struct _adapter  *padapter,
+				       struct cmd_obj *pcmd);
+void r8712_disconnectCtrlEx_cmd(struct _adapter *adapter, u32 enableDrvCtrl,
+			u32 tryPktCnt, u32 tryPktInterval, u32 firstStageTO);
+
+struct _cmd_callback {
+	u32	cmd_code;
+	void (*callback)(struct _adapter  *padapter, struct cmd_obj *cmd);
+};
+
+#include "rtl8712_cmd.h"
+
+#endif /* _CMD_H_ */
+
diff --git a/drivers/staging/rtl8712/rtl871x_debug.h b/drivers/staging/rtl8712/rtl871x_debug.h
new file mode 100644
index 000000000..69c631af2
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_debug.h
@@ -0,0 +1,130 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_DEBUG_H__
+#define __RTL871X_DEBUG_H__
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define _drv_emerg_			1
+#define _drv_alert_			2
+#define _drv_crit_			3
+#define _drv_err_			4
+#define	_drv_warning_			5
+#define _drv_notice_			6
+#define _drv_info_			7
+#define _drv_dump_			8
+#define	_drv_debug_			9
+
+#define _module_rtl871x_xmit_c_		BIT(0)
+#define _module_xmit_osdep_c_		BIT(1)
+#define _module_rtl871x_recv_c_		BIT(2)
+#define _module_recv_osdep_c_		BIT(3)
+#define _module_rtl871x_mlme_c_		BIT(4)
+#define	_module_mlme_osdep_c_		BIT(5)
+#define _module_rtl871x_sta_mgt_c_	BIT(6)
+#define _module_rtl871x_cmd_c_		BIT(7)
+#define	_module_cmd_osdep_c_		BIT(8)
+#define _module_rtl871x_io_c_		BIT(9)
+#define	_module_io_osdep_c_		BIT(10)
+#define _module_os_intfs_c_		BIT(11)
+#define _module_rtl871x_security_c_	BIT(12)
+#define _module_rtl871x_eeprom_c_	BIT(13)
+#define _module_hal_init_c_		BIT(14)
+#define _module_hci_hal_init_c_		BIT(15)
+#define _module_rtl871x_ioctl_c_	BIT(16)
+#define _module_rtl871x_ioctl_set_c_	BIT(17)
+#define _module_rtl871x_pwrctrl_c_	BIT(19)
+#define _module_hci_intfs_c_		BIT(20)
+#define _module_hci_ops_c_		BIT(21)
+#define _module_osdep_service_c_	BIT(22)
+#define _module_rtl871x_mp_ioctl_c_	BIT(23)
+#define _module_hci_ops_os_c_		BIT(24)
+#define _module_rtl871x_ioctl_os_c	BIT(25)
+#define _module_rtl8712_cmd_c_		BIT(26)
+#define _module_rtl871x_mp_c_		BIT(27)
+#define _module_rtl8712_xmit_c_		BIT(28)
+#define _module_rtl8712_efuse_c_	BIT(29)
+#define _module_rtl8712_recv_c_		BIT(30)
+#define _module_rtl8712_led_c_		BIT(31)
+
+#undef _MODULE_DEFINE_
+
+#if defined _RTL871X_XMIT_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_xmit_c_
+#elif defined _XMIT_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_xmit_osdep_c_
+#elif defined _RTL871X_RECV_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_recv_c_
+#elif defined _RECV_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_recv_osdep_c_
+#elif defined _RTL871X_MLME_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_mlme_c_
+#elif defined _MLME_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_mlme_osdep_c_
+#elif defined _RTL871X_STA_MGT_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_sta_mgt_c_
+#elif defined _RTL871X_CMD_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_cmd_c_
+#elif defined _CMD_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_cmd_osdep_c_
+#elif defined _RTL871X_IO_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_io_c_
+#elif defined _IO_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_io_osdep_c_
+#elif defined _OS_INTFS_C_
+	#define	_MODULE_DEFINE_	_module_os_intfs_c_
+#elif defined _RTL871X_SECURITY_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_security_c_
+#elif defined _RTL871X_EEPROM_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_eeprom_c_
+#elif defined _HAL_INIT_C_
+	#define	_MODULE_DEFINE_	_module_hal_init_c_
+#elif defined _HCI_HAL_INIT_C_
+	#define	_MODULE_DEFINE_	_module_hci_hal_init_c_
+#elif defined _RTL871X_IOCTL_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_ioctl_c_
+#elif defined _RTL871X_IOCTL_SET_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_ioctl_set_c_
+#elif defined _RTL871X_PWRCTRL_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_pwrctrl_c_
+#elif defined _HCI_INTF_C_
+	#define	_MODULE_DEFINE_	_module_hci_intfs_c_
+#elif defined _HCI_OPS_C_
+	#define	_MODULE_DEFINE_	_module_hci_ops_c_
+#elif defined _OSDEP_HCI_INTF_C_
+	#define	_MODULE_DEFINE_	_module_hci_intfs_c_
+#elif defined _OSDEP_SERVICE_C_
+	#define	_MODULE_DEFINE_	_module_osdep_service_c_
+#elif defined _RTL871X_MP_IOCTL_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_mp_ioctl_c_
+#elif defined _HCI_OPS_OS_C_
+	#define	_MODULE_DEFINE_	_module_hci_ops_os_c_
+#elif defined _RTL871X_IOCTL_LINUX_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_ioctl_os_c
+#elif defined _RTL871X_MP_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_mp_c_
+#elif defined _RTL8712_CMD_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_cmd_c_
+#elif defined _RTL8712_XMIT_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_xmit_c_
+#elif defined _RTL8712_EFUSE_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_efuse_c_
+#elif defined _RTL8712_RECV_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_recv_c_
+#else
+	#undef	_MODULE_DEFINE_
+#endif
+
+#endif	/*__RTL871X_DEBUG_H__*/
diff --git a/drivers/staging/rtl8712/rtl871x_eeprom.c b/drivers/staging/rtl8712/rtl871x_eeprom.c
new file mode 100644
index 000000000..221bf92e1
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_eeprom.c
@@ -0,0 +1,220 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_eeprom.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_EEPROM_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+static void up_clk(struct _adapter *padapter, u16 *x)
+{
+	*x = *x | _EESK;
+	r8712_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+}
+
+static void down_clk(struct _adapter *padapter, u16 *x)
+{
+	*x = *x & ~_EESK;
+	r8712_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+}
+
+static void shift_out_bits(struct _adapter *padapter, u16 data, u16 count)
+{
+	u16 x, mask;
+
+	if (padapter->surprise_removed)
+		goto out;
+	mask = 0x01 << (count - 1);
+	x = r8712_read8(padapter, EE_9346CR);
+	x &= ~(_EEDO | _EEDI);
+	do {
+		x &= ~_EEDI;
+		if (data & mask)
+			x |= _EEDI;
+		if (padapter->surprise_removed)
+			goto out;
+		r8712_write8(padapter, EE_9346CR, (u8)x);
+		udelay(CLOCK_RATE);
+		up_clk(padapter, &x);
+		down_clk(padapter, &x);
+		mask >>= 1;
+	} while (mask);
+	if (padapter->surprise_removed)
+		goto out;
+	x &= ~_EEDI;
+	r8712_write8(padapter, EE_9346CR, (u8)x);
+out:;
+}
+
+static u16 shift_in_bits(struct _adapter *padapter)
+{
+	u16 x, d = 0, i;
+
+	if (padapter->surprise_removed)
+		goto out;
+	x = r8712_read8(padapter, EE_9346CR);
+	x &= ~(_EEDO | _EEDI);
+	d = 0;
+	for (i = 0; i < 16; i++) {
+		d <<= 1;
+		up_clk(padapter, &x);
+		if (padapter->surprise_removed)
+			goto out;
+		x = r8712_read8(padapter, EE_9346CR);
+		x &= ~(_EEDI);
+		if (x & _EEDO)
+			d |= 1;
+		down_clk(padapter, &x);
+	}
+out:
+	return d;
+}
+
+static void standby(struct _adapter *padapter)
+{
+	u8   x;
+
+	x = r8712_read8(padapter, EE_9346CR);
+	x &= ~(_EECS | _EESK);
+	r8712_write8(padapter, EE_9346CR, x);
+	udelay(CLOCK_RATE);
+	x |= _EECS;
+	r8712_write8(padapter, EE_9346CR, x);
+	udelay(CLOCK_RATE);
+}
+
+static u16 wait_eeprom_cmd_done(struct _adapter *padapter)
+{
+	u8	x;
+	u16	i;
+
+	standby(padapter);
+	for (i = 0; i < 200; i++) {
+		x = r8712_read8(padapter, EE_9346CR);
+		if (x & _EEDO)
+			return true;
+		udelay(CLOCK_RATE);
+	}
+	return false;
+}
+
+static void eeprom_clean(struct _adapter *padapter)
+{
+	u16 x;
+
+	if (padapter->surprise_removed)
+		return;
+	x = r8712_read8(padapter, EE_9346CR);
+	if (padapter->surprise_removed)
+		return;
+	x &= ~(_EECS | _EEDI);
+	r8712_write8(padapter, EE_9346CR, (u8)x);
+	if (padapter->surprise_removed)
+		return;
+	up_clk(padapter, &x);
+	if (padapter->surprise_removed)
+		return;
+	down_clk(padapter, &x);
+}
+
+void r8712_eeprom_write16(struct _adapter *padapter, u16 reg, u16 data)
+{
+	u8 x;
+	u8 tmp8_ori, tmp8_new, tmp8_clk_ori, tmp8_clk_new;
+
+	tmp8_ori = r8712_read8(padapter, 0x102502f1);
+	tmp8_new = tmp8_ori & 0xf7;
+	if (tmp8_ori != tmp8_new)
+		r8712_write8(padapter, 0x102502f1, tmp8_new);
+	tmp8_clk_ori = r8712_read8(padapter, 0x10250003);
+	tmp8_clk_new = tmp8_clk_ori | 0x20;
+	if (tmp8_clk_new != tmp8_clk_ori)
+		r8712_write8(padapter, 0x10250003, tmp8_clk_new);
+	x = r8712_read8(padapter, EE_9346CR);
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	r8712_write8(padapter, EE_9346CR, x);
+	shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5);
+	if (padapter->eeprom_address_size == 8)	/*CF+ and SDIO*/
+		shift_out_bits(padapter, 0, 6);
+	else	/* USB */
+		shift_out_bits(padapter, 0, 4);
+	standby(padapter);
+	/* Erase this particular word.  Write the erase opcode and register
+	 * number in that order. The opcode is 3bits in length; reg is 6
+	 * bits long.
+	 */
+	standby(padapter);
+	/* write the new word to the EEPROM
+	 * send the write opcode the EEPORM
+	 */
+	shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3);
+	/* select which word in the EEPROM that we are writing to. */
+	shift_out_bits(padapter, reg, padapter->eeprom_address_size);
+	/* write the data to the selected EEPROM word. */
+	shift_out_bits(padapter, data, 16);
+	if (wait_eeprom_cmd_done(padapter)) {
+		standby(padapter);
+		shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5);
+		shift_out_bits(padapter, reg, 4);
+		eeprom_clean(padapter);
+	}
+	if (tmp8_clk_new != tmp8_clk_ori)
+		r8712_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if (tmp8_new != tmp8_ori)
+		r8712_write8(padapter, 0x102502f1, tmp8_ori);
+}
+
+u16 r8712_eeprom_read16(struct _adapter *padapter, u16 reg) /*ReadEEprom*/
+{
+	u16 x;
+	u16 data = 0;
+	u8 tmp8_ori, tmp8_new, tmp8_clk_ori, tmp8_clk_new;
+
+	tmp8_ori = r8712_read8(padapter, 0x102502f1);
+	tmp8_new = tmp8_ori & 0xf7;
+	if (tmp8_ori != tmp8_new)
+		r8712_write8(padapter, 0x102502f1, tmp8_new);
+	tmp8_clk_ori = r8712_read8(padapter, 0x10250003);
+	tmp8_clk_new = tmp8_clk_ori | 0x20;
+	if (tmp8_clk_new != tmp8_clk_ori)
+		r8712_write8(padapter, 0x10250003, tmp8_clk_new);
+	if (padapter->surprise_removed)
+		goto out;
+	/* select EEPROM, reset bits, set _EECS */
+	x = r8712_read8(padapter, EE_9346CR);
+	if (padapter->surprise_removed)
+		goto out;
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	r8712_write8(padapter, EE_9346CR, (unsigned char)x);
+	/* write the read opcode and register number in that order
+	 * The opcode is 3bits in length, reg is 6 bits long
+	 */
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->eeprom_address_size);
+	/* Now read the data (16 bits) in from the selected EEPROM word */
+	data = shift_in_bits(padapter);
+	eeprom_clean(padapter);
+out:
+	if (tmp8_clk_new != tmp8_clk_ori)
+		r8712_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if (tmp8_new != tmp8_ori)
+		r8712_write8(padapter, 0x102502f1, tmp8_ori);
+	return data;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_eeprom.h b/drivers/staging/rtl8712/rtl871x_eeprom.h
new file mode 100644
index 000000000..7bdeb2aaa
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_eeprom.h
@@ -0,0 +1,88 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_EEPROM_H__
+#define __RTL871X_EEPROM_H__
+
+#include "osdep_service.h"
+
+#define	RTL8712_EEPROM_ID		0x8712
+#define	EEPROM_MAX_SIZE			256
+#define	CLOCK_RATE			50	/*100us*/
+
+/*- EEPROM opcodes*/
+#define EEPROM_READ_OPCODE		06
+#define EEPROM_WRITE_OPCODE		05
+#define EEPROM_ERASE_OPCODE		07
+#define EEPROM_EWEN_OPCODE		19      /* Erase/write enable*/
+#define EEPROM_EWDS_OPCODE		16      /* Erase/write disable*/
+
+#define	EEPROM_CID_DEFAULT		0x0
+#define	EEPROM_CID_ALPHA		0x1
+#define	EEPROM_CID_Senao		0x3
+#define	EEPROM_CID_NetCore		0x5
+#define	EEPROM_CID_CAMEO		0X8
+#define	EEPROM_CID_SITECOM		0x9
+#define	EEPROM_CID_COREGA		0xB
+#define	EEPROM_CID_EDIMAX_BELKIN	0xC
+#define	EEPROM_CID_SERCOMM_BELKIN	0xE
+#define	EEPROM_CID_CAMEO1		0xF
+#define	EEPROM_CID_WNC_COREGA		0x12
+#define	EEPROM_CID_CLEVO		0x13
+#define	EEPROM_CID_WHQL			0xFE
+
+enum RT_CUSTOMER_ID {
+	RT_CID_DEFAULT = 0,
+	RT_CID_8187_ALPHA0 = 1,
+	RT_CID_8187_SERCOMM_PS = 2,
+	RT_CID_8187_HW_LED = 3,
+	RT_CID_8187_NETGEAR = 4,
+	RT_CID_WHQL = 5,
+	RT_CID_819x_CAMEO  = 6,
+	RT_CID_819x_RUNTOP = 7,
+	RT_CID_819x_Senao = 8,
+	RT_CID_TOSHIBA = 9,
+	RT_CID_819x_Netcore = 10,
+	RT_CID_Nettronix = 11,
+	RT_CID_DLINK = 12,
+	RT_CID_PRONET = 13,
+	RT_CID_COREGA = 14,
+	RT_CID_819x_ALPHA = 15,
+	RT_CID_819x_Sitecom = 16,
+	RT_CID_CCX = 17,
+	RT_CID_819x_Lenovo = 18,
+	RT_CID_819x_QMI = 19,
+	RT_CID_819x_Edimax_Belkin = 20,
+	RT_CID_819x_Sercomm_Belkin = 21,
+	RT_CID_819x_CAMEO1 = 22,
+	RT_CID_819x_MSI = 23,
+	RT_CID_819x_Acer = 24,
+	RT_CID_819x_AzWave_ASUS = 25,
+	RT_CID_819x_AzWave = 26,
+	RT_CID_819x_WNC_COREGA = 27,
+	RT_CID_819x_CLEVO = 28,
+};
+
+struct eeprom_priv {
+	u8 bautoload_fail_flag;
+	u8 bempty;
+	u8 sys_config;
+	u8 mac_addr[6];
+	u8 config0;
+	u16 channel_plan;
+	u8 country_string[3];
+	u8 tx_power_b[15];
+	u8 tx_power_g[15];
+	u8 tx_power_a[201];
+	u8 efuse_eeprom_data[EEPROM_MAX_SIZE];
+	enum RT_CUSTOMER_ID CustomerID;
+};
+
+void r8712_eeprom_write16(struct _adapter *padapter, u16 reg, u16 data);
+u16 r8712_eeprom_read16(struct _adapter *padapter, u16 reg);
+
+#endif  /*__RTL871X_EEPROM_H__*/
+
diff --git a/drivers/staging/rtl8712/rtl871x_event.h b/drivers/staging/rtl8712/rtl871x_event.h
new file mode 100644
index 000000000..759a2d27d
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_event.h
@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871x_EVENT_H_
+#define _RTL871x_EVENT_H_
+
+#include "osdep_service.h"
+
+#include "wlan_bssdef.h"
+#include <linux/semaphore.h>
+#include <linux/sem.h>
+
+/*
+ * Used to report a bss has been scanned
+ */
+struct survey_event	{
+	struct wlan_bssid_ex bss;
+};
+
+/*
+ * Used to report that the requested site survey has been done.
+ * bss_cnt indicates the number of bss that has been reported.
+ */
+struct surveydone_event {
+	unsigned int	bss_cnt;
+
+};
+
+/*
+ * Used to report the link result of joinning the given bss
+ * join_res:
+ *  -1: authentication fail
+ *  -2: association fail
+ *  > 0: TID
+ */
+struct joinbss_event {
+	struct	wlan_network	network;
+};
+
+/*
+ * Used to report a given STA has joinned the created BSS.
+ * It is used in AP/Ad-HoC(M) mode.
+ */
+struct stassoc_event {
+	unsigned char macaddr[6];
+	unsigned char rsvd[2];
+	__le32    cam_id;
+};
+
+struct stadel_event {
+	unsigned char macaddr[6];
+	unsigned char rsvd[2];
+};
+
+struct addba_event {
+	unsigned int tid;
+};
+
+#define GEN_EVT_CODE(event)	event ## _EVT_
+
+struct fwevent {
+	u32	parmsize;
+	void (*event_callback)(struct _adapter *dev, u8 *pbuf);
+};
+
+#define C2HEVENT_SZ			32
+struct event_node {
+	unsigned char *node;
+	unsigned char evt_code;
+	unsigned short evt_sz;
+	/*volatile*/ int *caller_ff_tail;
+	int	caller_ff_sz;
+};
+
+struct c2hevent_queue {
+	/*volatile*/ int	head;
+	/*volatile*/ int	tail;
+	struct	event_node	nodes[C2HEVENT_SZ];
+	unsigned char	seq;
+};
+
+#define NETWORK_QUEUE_SZ	4
+
+struct network_queue {
+	/*volatile*/ int	head;
+	/*volatile*/ int	tail;
+	struct wlan_bssid_ex networks[NETWORK_QUEUE_SZ];
+};
+
+struct ADDBA_Req_Report_parm {
+	unsigned char MacAddress[ETH_ALEN];
+	unsigned short StartSeqNum;
+	unsigned char tid;
+};
+
+#include "rtl8712_event.h"
+
+#endif /* _WLANEVENT_H_ */
+
diff --git a/drivers/staging/rtl8712/rtl871x_ht.h b/drivers/staging/rtl8712/rtl871x_ht.h
new file mode 100644
index 000000000..ebd786657
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ht.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_HT_H_
+#define _RTL871X_HT_H_
+
+#include "osdep_service.h"
+#include "wifi.h"
+
+struct ht_priv {
+	unsigned int	ht_option;
+	unsigned int	ampdu_enable;/*for enable Tx A-MPDU*/
+	unsigned char	baddbareq_issued[16];
+	unsigned int	tx_amsdu_enable;/*for enable Tx A-MSDU */
+	unsigned int	tx_amdsu_maxlen; /* 1: 8k, 0:4k ; default:8k, for tx */
+	unsigned int	rx_ampdu_maxlen; /* for rx reordering ctrl win_sz,
+					  * updated when join_callback.
+					  */
+	struct ieee80211_ht_cap ht_cap;
+};
+
+#endif	/*_RTL871X_HT_H_ */
+
diff --git a/drivers/staging/rtl8712/rtl871x_io.c b/drivers/staging/rtl8712/rtl871x_io.c
new file mode 100644
index 000000000..6789a4c98
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_io.c
@@ -0,0 +1,147 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_io.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+/*
+ *
+ * The purpose of rtl871x_io.c
+ *
+ * a. provides the API
+ * b. provides the protocol engine
+ * c. provides the software interface between caller and the hardware interface
+ *
+ * For r8712u, both sync/async operations are provided.
+ *
+ * Only sync read/write_mem operations are provided.
+ *
+ */
+
+#define _RTL871X_IO_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "rtl871x_io.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+
+static uint _init_intf_hdl(struct _adapter *padapter,
+			   struct intf_hdl *pintf_hdl)
+{
+	struct	intf_priv	*pintf_priv;
+	void (*set_intf_option)(u32 *poption) = NULL;
+	void (*set_intf_funs)(struct intf_hdl *pintf_hdl);
+	void (*set_intf_ops)(struct _io_ops	*pops);
+	uint (*init_intf_priv)(struct intf_priv *pintfpriv);
+
+	set_intf_option = &(r8712_usb_set_intf_option);
+	set_intf_funs = &(r8712_usb_set_intf_funs);
+	set_intf_ops = &r8712_usb_set_intf_ops;
+	init_intf_priv = &r8712_usb_init_intf_priv;
+	pintf_priv = pintf_hdl->pintfpriv = kmalloc(sizeof(struct intf_priv),
+						    GFP_ATOMIC);
+	if (!pintf_priv)
+		goto _init_intf_hdl_fail;
+	pintf_hdl->adapter = (u8 *)padapter;
+	set_intf_option(&pintf_hdl->intf_option);
+	set_intf_funs(pintf_hdl);
+	set_intf_ops(&pintf_hdl->io_ops);
+	pintf_priv->intf_dev = (u8 *)&padapter->dvobjpriv;
+	if (init_intf_priv(pintf_priv) == _FAIL)
+		goto _init_intf_hdl_fail;
+	return _SUCCESS;
+_init_intf_hdl_fail:
+	kfree(pintf_priv);
+	return _FAIL;
+}
+
+static void _unload_intf_hdl(struct intf_priv *pintfpriv)
+{
+	void (*unload_intf_priv)(struct intf_priv *pintfpriv);
+
+	unload_intf_priv = &r8712_usb_unload_intf_priv;
+	unload_intf_priv(pintfpriv);
+	kfree(pintfpriv);
+}
+
+static uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl)
+{
+	struct _adapter *adapter = (struct _adapter *)dev;
+
+	pintfhdl->intf_option = 0;
+	pintfhdl->adapter = dev;
+	pintfhdl->intf_dev = (u8 *)&adapter->dvobjpriv;
+	if (!_init_intf_hdl(adapter, pintfhdl))
+		goto register_intf_hdl_fail;
+	return _SUCCESS;
+register_intf_hdl_fail:
+	return false;
+}
+
+static  void unregister_intf_hdl(struct intf_hdl *pintfhdl)
+{
+	_unload_intf_hdl(pintfhdl->pintfpriv);
+	memset((u8 *)pintfhdl, 0, sizeof(struct intf_hdl));
+}
+
+uint r8712_alloc_io_queue(struct _adapter *adapter)
+{
+	u32 i;
+	struct io_queue *pio_queue;
+	struct io_req *pio_req;
+
+	pio_queue = kmalloc(sizeof(*pio_queue), GFP_ATOMIC);
+	if (!pio_queue)
+		goto alloc_io_queue_fail;
+	INIT_LIST_HEAD(&pio_queue->free_ioreqs);
+	INIT_LIST_HEAD(&pio_queue->processing);
+	INIT_LIST_HEAD(&pio_queue->pending);
+	spin_lock_init(&pio_queue->lock);
+	pio_queue->pallocated_free_ioreqs_buf = kzalloc(NUM_IOREQ *
+						(sizeof(struct io_req)) + 4,
+						GFP_ATOMIC);
+	if ((pio_queue->pallocated_free_ioreqs_buf) == NULL)
+		goto alloc_io_queue_fail;
+	pio_queue->free_ioreqs_buf = pio_queue->pallocated_free_ioreqs_buf + 4
+			- ((addr_t)(pio_queue->pallocated_free_ioreqs_buf)
+			& 3);
+	pio_req = (struct io_req *)(pio_queue->free_ioreqs_buf);
+	for (i = 0; i < NUM_IOREQ; i++) {
+		INIT_LIST_HEAD(&pio_req->list);
+		list_add_tail(&pio_req->list, &pio_queue->free_ioreqs);
+		pio_req++;
+	}
+	if ((register_intf_hdl((u8 *)adapter, &pio_queue->intf)) == _FAIL)
+		goto alloc_io_queue_fail;
+	adapter->pio_queue = pio_queue;
+	return _SUCCESS;
+alloc_io_queue_fail:
+	if (pio_queue) {
+		kfree(pio_queue->pallocated_free_ioreqs_buf);
+		kfree(pio_queue);
+	}
+	adapter->pio_queue = NULL;
+	return _FAIL;
+}
+
+void r8712_free_io_queue(struct _adapter *adapter)
+{
+	struct io_queue *pio_queue = adapter->pio_queue;
+
+	if (pio_queue) {
+		kfree(pio_queue->pallocated_free_ioreqs_buf);
+		adapter->pio_queue = NULL;
+		unregister_intf_hdl(&pio_queue->intf);
+		kfree(pio_queue);
+	}
+}
diff --git a/drivers/staging/rtl8712/rtl871x_io.h b/drivers/staging/rtl8712/rtl871x_io.h
new file mode 100644
index 000000000..c5b12f74e
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_io.h
@@ -0,0 +1,236 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_IO_H_
+#define _RTL871X_IO_H_
+
+#include "osdep_service.h"
+#include "osdep_intf.h"
+
+#define NUM_IOREQ		8
+
+#define MAX_PROT_SZ	(64-16)
+
+#define _IOREADY			0
+#define _IO_WAIT_COMPLETE   1
+#define _IO_WAIT_RSP        2
+
+/* IO COMMAND TYPE */
+#define _IOSZ_MASK_		(0x7F)
+#define _IO_WRITE_		BIT(7)
+#define _IO_FIXED_		BIT(8)
+#define _IO_BURST_		BIT(9)
+#define _IO_BYTE_		BIT(10)
+#define _IO_HW_			BIT(11)
+#define _IO_WORD_		BIT(12)
+#define _IO_SYNC_		BIT(13)
+#define _IO_CMDMASK_	(0x1F80)
+
+/*
+ *	For prompt mode accessing, caller shall free io_req
+ *	Otherwise, io_handler will free io_req
+ */
+/* IO STATUS TYPE */
+#define _IO_ERR_		BIT(2)
+#define _IO_SUCCESS_	BIT(1)
+#define _IO_DONE_		BIT(0)
+#define IO_RD32			(_IO_SYNC_ | _IO_WORD_)
+#define IO_RD16			(_IO_SYNC_ | _IO_HW_)
+#define IO_RD8			(_IO_SYNC_ | _IO_BYTE_)
+#define IO_RD32_ASYNC	(_IO_WORD_)
+#define IO_RD16_ASYNC	(_IO_HW_)
+#define IO_RD8_ASYNC	(_IO_BYTE_)
+#define IO_WR32			(_IO_WRITE_ | _IO_SYNC_ | _IO_WORD_)
+#define IO_WR16			(_IO_WRITE_ | _IO_SYNC_ | _IO_HW_)
+#define IO_WR8			(_IO_WRITE_ | _IO_SYNC_ | _IO_BYTE_)
+#define IO_WR32_ASYNC	(_IO_WRITE_ | _IO_WORD_)
+#define IO_WR16_ASYNC	(_IO_WRITE_ | _IO_HW_)
+#define IO_WR8_ASYNC	(_IO_WRITE_ | _IO_BYTE_)
+/*
+ *	Only Sync. burst accessing is provided.
+ */
+#define IO_WR_BURST(x)		(IO_WRITE_ | _IO_SYNC_ | _IO_BURST_ | \
+				((x) & _IOSZ_MASK_))
+#define IO_RD_BURST(x)		(_IO_SYNC_ | _IO_BURST_ | ((x) & _IOSZ_MASK_))
+/*below is for the intf_option bit defition...*/
+#define _INTF_ASYNC_	BIT(0)	/*support async io*/
+struct intf_priv;
+struct	intf_hdl;
+struct io_queue;
+struct	_io_ops {
+	uint (*_sdbus_read_bytes_to_membuf)(struct intf_priv *pintfpriv,
+					    u32 addr, u32 cnt, u8 *pbuf);
+	uint (*_sdbus_read_blocks_to_membuf)(struct intf_priv *pintfpriv,
+					     u32 addr, u32 cnt, u8 *pbuf);
+	u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+	u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+	u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+	uint (*_sdbus_write_blocks_from_membuf)(struct intf_priv *pintfpriv,
+						u32 addr, u32 cnt, u8 *pbuf,
+						u8 async);
+	uint (*_sdbus_write_bytes_from_membuf)(struct intf_priv *pintfpriv,
+					       u32 addr, u32 cnt, u8 *pbuf);
+	u8 (*_cmd52r)(struct intf_priv *pintfpriv, u32 addr);
+	void (*_cmd52w)(struct intf_priv *pintfpriv, u32 addr, u8 val8);
+	u8 (*_cmdfunc152r)(struct intf_priv *pintfpriv, u32 addr);
+	void (*_cmdfunc152w)(struct intf_priv *pintfpriv, u32 addr, u8 val8);
+	void (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	void (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	void (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
+			  u8 *pmem);
+	void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
+			   u8 *pmem);
+	void (*_sync_irp_protocol_rw)(struct io_queue *pio_q);
+	u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
+			  u8 *pmem);
+	u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
+			   u8 *pmem);
+};
+
+struct io_req {
+	struct list_head list;
+	u32	addr;
+	/*volatile*/ u32	val;
+	u32	command;
+	u32	status;
+	u8	*pbuf;
+	void (*_async_io_callback)(struct _adapter *padapter,
+				   struct io_req *pio_req, u8 *cnxt);
+	u8 *cnxt;
+};
+
+struct	intf_hdl {
+	u32	intf_option;
+	u8	*adapter;
+	u8	*intf_dev;
+	struct intf_priv	*pintfpriv;
+	void (*intf_hdl_init)(u8 *priv);
+	void (*intf_hdl_unload)(u8 *priv);
+	void (*intf_hdl_open)(u8 *priv);
+	void (*intf_hdl_close)(u8 *priv);
+	struct	_io_ops	io_ops;
+};
+
+struct reg_protocol_rd {
+#ifdef __LITTLE_ENDIAN
+	/* DW1 */
+	u32		NumOfTrans:4;
+	u32		Reserved1:4;
+	u32		Reserved2:24;
+	/* DW2 */
+	u32		ByteCount:7;
+	u32		WriteEnable:1;		/*0:read, 1:write*/
+	u32		FixOrContinuous:1;	/*0:continuous, 1: Fix*/
+	u32		BurstMode:1;
+	u32		Byte1Access:1;
+	u32		Byte2Access:1;
+	u32		Byte4Access:1;
+	u32		Reserved3:3;
+	u32		Reserved4:16;
+	/*DW3*/
+	u32		BusAddress;
+	/*DW4*/
+#else
+/*DW1*/
+	u32 Reserved1:4;
+	u32 NumOfTrans:4;
+	u32 Reserved2:24;
+	/*DW2*/
+	u32 WriteEnable:1;
+	u32 ByteCount:7;
+	u32 Reserved3:3;
+	u32 Byte4Access:1;
+	u32 Byte2Access:1;
+	u32 Byte1Access:1;
+	u32 BurstMode:1;
+	u32 FixOrContinuous:1;
+	u32 Reserved4:16;
+	/*DW3*/
+	u32 BusAddress;
+	/*DW4*/
+#endif
+};
+
+struct reg_protocol_wt {
+#ifdef __LITTLE_ENDIAN
+	/*DW1*/
+	u32 NumOfTrans:4;
+	u32 Reserved1:4;
+	u32 Reserved2:24;
+	/*DW2*/
+	u32 ByteCount:7;
+	u32 WriteEnable:1;	/*0:read, 1:write*/
+	u32 FixOrContinuous:1;	/*0:continuous, 1: Fix*/
+	u32 BurstMode:1;
+	u32 Byte1Access:1;
+	u32 Byte2Access:1;
+	u32 Byte4Access:1;
+	u32 Reserved3:3;
+	u32 Reserved4:16;
+	/*DW3*/
+	u32 BusAddress;
+	/*DW4*/
+	u32 Value;
+#else
+	/*DW1*/
+	u32 Reserved1:4;
+	u32 NumOfTrans:4;
+	u32 Reserved2:24;
+	/*DW2*/
+	u32 WriteEnable:1;
+	u32 ByteCount:7;
+	u32 Reserved3:3;
+	u32 Byte4Access:1;
+	u32 Byte2Access:1;
+	u32 Byte1Access:1;
+	u32 BurstMode:1;
+	u32 FixOrContinuous:1;
+	u32 Reserved4:16;
+	/*DW3*/
+	u32 BusAddress;
+	/*DW4*/
+	u32 Value;
+#endif
+};
+
+/*
+ * Below is the data structure used by _io_handler
+ */
+
+struct io_queue {
+	spinlock_t lock;
+	struct list_head free_ioreqs;
+	/*The io_req list that will be served in the single protocol r/w.*/
+	struct list_head pending;
+	struct list_head processing;
+	u8 *free_ioreqs_buf; /* 4-byte aligned */
+	u8 *pallocated_free_ioreqs_buf;
+	struct	intf_hdl intf;
+};
+
+u8 r8712_read8(struct _adapter *adapter, u32 addr);
+u16 r8712_read16(struct _adapter *adapter, u32 addr);
+u32 r8712_read32(struct _adapter *adapter, u32 addr);
+void r8712_read_mem(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void r8712_read_port(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void r8712_write8(struct _adapter *adapter, u32 addr, u8 val);
+void r8712_write16(struct _adapter *adapter, u32 addr, u16 val);
+void r8712_write32(struct _adapter *adapter, u32 addr, u32 val);
+void r8712_write_mem(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+void r8712_write_port(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+/*ioreq */
+uint r8712_alloc_io_queue(struct _adapter *adapter);
+void r8712_free_io_queue(struct _adapter *adapter);
+
+#endif	/*_RTL871X_IO_H_*/
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl.h b/drivers/staging/rtl8712/rtl871x_ioctl.h
new file mode 100644
index 000000000..d6332a8c7
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl.h
@@ -0,0 +1,94 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __IOCTL_H
+#define __IOCTL_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#ifndef OID_802_11_CAPABILITY
+	#define OID_802_11_CAPABILITY                   0x0d010122
+#endif
+
+#ifndef OID_802_11_PMKID
+	#define OID_802_11_PMKID                        0x0d010123
+#endif
+
+/* For DDK-defined OIDs*/
+#define OID_NDIS_SEG1	0x00010100
+#define OID_NDIS_SEG2	0x00010200
+#define OID_NDIS_SEG3	0x00020100
+#define OID_NDIS_SEG4	0x01010100
+#define OID_NDIS_SEG5	0x01020100
+#define OID_NDIS_SEG6	0x01020200
+#define OID_NDIS_SEG7	0xFD010100
+#define OID_NDIS_SEG8	0x0D010100
+#define OID_NDIS_SEG9	0x0D010200
+#define OID_NDIS_SEG10	0x0D020200
+#define SZ_OID_NDIS_SEG1	23
+#define SZ_OID_NDIS_SEG2	3
+#define SZ_OID_NDIS_SEG3	6
+#define SZ_OID_NDIS_SEG4	6
+#define SZ_OID_NDIS_SEG5	4
+#define SZ_OID_NDIS_SEG6	8
+#define SZ_OID_NDIS_SEG7	7
+#define SZ_OID_NDIS_SEG8	36
+#define SZ_OID_NDIS_SEG9	24
+#define SZ_OID_NDIS_SEG10	19
+
+/* For Realtek-defined OIDs*/
+#define OID_MP_SEG1	0xFF871100
+#define OID_MP_SEG2	0xFF818000
+#define OID_MP_SEG3	0xFF818700
+#define OID_MP_SEG4	0xFF011100
+
+enum oid_type {
+	QUERY_OID,
+	SET_OID
+};
+
+struct oid_funs_node {
+	unsigned int oid_start; /*the starting number for OID*/
+	unsigned int oid_end; /*the ending number for OID*/
+	struct oid_obj_priv *node_array;
+	unsigned int array_sz; /*the size of node_array*/
+	int query_counter; /*count the number of query hits for this segment*/
+	int set_counter; /*count the number of set hits for this segment*/
+};
+
+struct oid_par_priv {
+	void	*adapter_context;
+	uint oid;
+	void *information_buf;
+	unsigned long information_buf_len;
+	unsigned long *bytes_rw;
+	unsigned long *bytes_needed;
+	enum oid_type	type_of_oid;
+	unsigned int dbg;
+};
+
+struct oid_obj_priv {
+	unsigned char	dbg; /* 0: without OID debug message
+			      * 1: with OID debug message
+			      */
+	uint (*oidfuns)(struct oid_par_priv *poid_par_priv);
+};
+
+uint oid_null_function(struct oid_par_priv *poid_par_priv);
+
+extern struct iw_handler_def  r871x_handlers_def;
+
+uint drv_query_info(struct net_device *MiniportAdapterContext,
+		    uint Oid,
+		    void *InformationBuffer,
+		    u32 InformationBufferLength,
+		    u32 *BytesWritten,
+		    u32 *BytesNeeded);
+
+uint drv_set_info(struct net_device *MiniportAdapterContext,
+		  uint Oid,
+		  void *InformationBuffer,
+		  u32 InformationBufferLength,
+		  u32 *BytesRead,
+		  u32 *BytesNeeded);
+
+#endif
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
new file mode 100644
index 000000000..36f6904d2
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -0,0 +1,2330 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_ioctl_linux.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_IOCTL_LINUX_C_
+#define _RTL871X_MP_IOCTL_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wlan_bssdef.h"
+#include "rtl871x_debug.h"
+#include "wifi.h"
+#include "rtl871x_mlme.h"
+#include "rtl871x_ioctl.h"
+#include "rtl871x_ioctl_set.h"
+#include "rtl871x_mp_ioctl.h"
+#include "mlme_osdep.h"
+#include <linux/wireless.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <net/iw_handler.h>
+#include <linux/if_arp.h>
+#include <linux/etherdevice.h>
+
+#define RTL_IOCTL_WPA_SUPPLICANT	(SIOCIWFIRSTPRIV + 0x1E)
+
+#define SCAN_ITEM_SIZE 768
+#define MAX_CUSTOM_LEN 64
+#define RATE_COUNT 4
+
+static const u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
+		       6000000, 9000000, 12000000, 18000000,
+		       24000000, 36000000, 48000000, 54000000};
+
+static const long ieee80211_wlan_frequencies[] = {
+	2412, 2417, 2422, 2427,
+	2432, 2437, 2442, 2447,
+	2452, 2457, 2462, 2467,
+	2472, 2484
+};
+
+void r8712_indicate_wx_assoc_event(struct _adapter *padapter)
+{
+	union iwreq_data wrqu;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	memcpy(wrqu.ap_addr.sa_data, pmlmepriv->cur_network.network.MacAddress,
+		ETH_ALEN);
+	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
+}
+
+void r8712_indicate_wx_disassoc_event(struct _adapter *padapter)
+{
+	union iwreq_data wrqu;
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	eth_zero_addr(wrqu.ap_addr.sa_data);
+	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
+}
+
+static inline void handle_pairwise_key(struct sta_info *psta,
+				       struct ieee_param *param,
+				       struct _adapter *padapter)
+{
+	/* pairwise key */
+	memcpy(psta->x_UncstKey.skey, param->u.crypt.key,
+	       (param->u.crypt. key_len > 16 ? 16 : param->u.crypt.key_len));
+	if (strcmp(param->u.crypt.alg, "TKIP") == 0) { /* set mic key */
+		memcpy(psta->tkiptxmickey. skey,
+		       &param->u.crypt.key[16], 8);
+		memcpy(psta->tkiprxmickey. skey,
+		       &param->u.crypt.key[24], 8);
+		padapter->securitypriv. busetkipkey = false;
+		mod_timer(&padapter->securitypriv.tkip_timer,
+			  jiffies + msecs_to_jiffies(50));
+	}
+	r8712_setstakey_cmd(padapter, (unsigned char *)psta, true);
+}
+
+static inline void handle_group_key(struct ieee_param *param,
+				    struct _adapter *padapter)
+{
+	union Keytype *gk = padapter->securitypriv.XGrpKey;
+	union Keytype *gtk = padapter->securitypriv.XGrptxmickey;
+	union Keytype *grk = padapter->securitypriv.XGrprxmickey;
+
+	if (param->u.crypt.idx > 0 &&
+	    param->u.crypt.idx < 3) {
+		/* group key idx is 1 or 2 */
+		memcpy(gk[param->u.crypt.idx - 1].skey,
+		       param->u.crypt.key,
+		       (param->u.crypt.key_len > 16 ? 16 :
+			param->u.crypt.key_len));
+		memcpy(gtk[param->u.crypt.idx - 1].skey,
+		       &param->u.crypt.key[16], 8);
+		memcpy(grk[param->u.crypt.idx - 1].skey,
+		       &param->u.crypt.key[24], 8);
+		padapter->securitypriv.binstallGrpkey = true;
+		r8712_set_key(padapter, &padapter->securitypriv,
+			param->u.crypt.idx);
+		if (padapter->registrypriv.power_mgnt > PS_MODE_ACTIVE) {
+			if (padapter->registrypriv.power_mgnt !=
+			    padapter->pwrctrlpriv.pwr_mode)
+				mod_timer(&padapter->mlmepriv.dhcp_timer,
+					  jiffies + msecs_to_jiffies(60000));
+		}
+	}
+}
+
+static noinline_for_stack char *translate_scan_wpa(struct iw_request_info *info,
+						   struct wlan_network *pnetwork,
+						   struct iw_event *iwe,
+						   char *start, char *stop)
+{
+	/* parsing WPA/WPA2 IE */
+	u8 buf[MAX_WPA_IE_LEN];
+	u8 wpa_ie[255], rsn_ie[255];
+	u16 wpa_len = 0, rsn_len = 0;
+	int n, i;
+
+	r8712_get_sec_ie(pnetwork->network.IEs,
+			 pnetwork->network.IELength, rsn_ie, &rsn_len,
+			 wpa_ie, &wpa_len);
+	if (wpa_len > 0) {
+		memset(buf, 0, MAX_WPA_IE_LEN);
+		n = sprintf(buf, "wpa_ie=");
+		for (i = 0; i < wpa_len; i++) {
+			n += scnprintf(buf + n, MAX_WPA_IE_LEN - n,
+						"%02x", wpa_ie[i]);
+			if (n == MAX_WPA_IE_LEN - 1)
+				break;
+		}
+		memset(iwe, 0, sizeof(*iwe));
+		iwe->cmd = IWEVCUSTOM;
+		iwe->u.data.length = (u16)strlen(buf);
+		start = iwe_stream_add_point(info, start, stop,
+			iwe, buf);
+		memset(iwe, 0, sizeof(*iwe));
+		iwe->cmd = IWEVGENIE;
+		iwe->u.data.length = (u16)wpa_len;
+		start = iwe_stream_add_point(info, start, stop,
+			iwe, wpa_ie);
+	}
+	if (rsn_len > 0) {
+		memset(buf, 0, MAX_WPA_IE_LEN);
+		n = sprintf(buf, "rsn_ie=");
+		for (i = 0; i < rsn_len; i++) {
+			n += scnprintf(buf + n, MAX_WPA_IE_LEN - n,
+						"%02x", rsn_ie[i]);
+			if (n == MAX_WPA_IE_LEN - 1)
+				break;
+		}
+		memset(iwe, 0, sizeof(*iwe));
+		iwe->cmd = IWEVCUSTOM;
+		iwe->u.data.length = strlen(buf);
+		start = iwe_stream_add_point(info, start, stop,
+			iwe, buf);
+		memset(iwe, 0, sizeof(*iwe));
+		iwe->cmd = IWEVGENIE;
+		iwe->u.data.length = rsn_len;
+		start = iwe_stream_add_point(info, start, stop, iwe,
+			rsn_ie);
+	}
+
+	return start;
+}
+
+static noinline_for_stack char *translate_scan_wps(struct iw_request_info *info,
+						   struct wlan_network *pnetwork,
+						   struct iw_event *iwe,
+						   char *start, char *stop)
+{
+	/* parsing WPS IE */
+	u8 wps_ie[512];
+	uint wps_ielen;
+
+	if (r8712_get_wps_ie(pnetwork->network.IEs,
+	    pnetwork->network.IELength,
+	    wps_ie, &wps_ielen)) {
+		if (wps_ielen > 2) {
+			iwe->cmd = IWEVGENIE;
+			iwe->u.data.length = (u16)wps_ielen;
+			start = iwe_stream_add_point(info, start, stop,
+				iwe, wps_ie);
+		}
+	}
+
+	return start;
+}
+
+static char *translate_scan(struct _adapter *padapter,
+			    struct iw_request_info *info,
+			    struct wlan_network *pnetwork,
+			    char *start, char *stop)
+{
+	struct iw_event iwe;
+	char *current_val;
+	s8 *p;
+	u32 i = 0, ht_ielen = 0;
+	u16	cap, ht_cap = false;
+	u8 rssi;
+
+	if ((pnetwork->network.Configuration.DSConfig < 1) ||
+	    (pnetwork->network.Configuration.DSConfig > 14)) {
+		if (pnetwork->network.Configuration.DSConfig < 1)
+			pnetwork->network.Configuration.DSConfig = 1;
+		else
+			pnetwork->network.Configuration.DSConfig = 14;
+	}
+	/* AP MAC address */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+	ether_addr_copy(iwe.u.ap_addr.sa_data, pnetwork->network.MacAddress);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	iwe.u.data.length = min_t(u32, pnetwork->network.Ssid.SsidLength, 32);
+	start = iwe_stream_add_point(info, start, stop, &iwe,
+				     pnetwork->network.Ssid.Ssid);
+	/* parsing HT_CAP_IE */
+	p = r8712_get_ie(&pnetwork->network.IEs[12], WLAN_EID_HT_CAPABILITY,
+			 &ht_ielen, pnetwork->network.IELength - 12);
+	if (p && ht_ielen > 0)
+		ht_cap = true;
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	if (r8712_is_cckratesonly_included(pnetwork->network.rates)) {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11b");
+	} else if (r8712_is_cckrates_included(pnetwork->network.rates)) {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bgn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bg");
+	} else {
+		if (ht_cap)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11gn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11g");
+	}
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_CHAR_LEN);
+	/* Add mode */
+	iwe.cmd = SIOCGIWMODE;
+	memcpy((u8 *)&cap, r8712_get_capability_from_ie(pnetwork->network.IEs),
+		2);
+	le16_to_cpus(&cap);
+	if (cap & (WLAN_CAPABILITY_IBSS | WLAN_CAPABILITY_ESS)) {
+		if (cap & WLAN_CAPABILITY_ESS)
+			iwe.u.mode = (u32)IW_MODE_MASTER;
+		else
+			iwe.u.mode = (u32)IW_MODE_ADHOC;
+		start = iwe_stream_add_event(info, start, stop, &iwe,
+			IW_EV_UINT_LEN);
+	}
+	/* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+	{
+		/*  check legal index */
+		u8 dsconfig = pnetwork->network.Configuration.DSConfig;
+
+		if (dsconfig >= 1 && dsconfig <= sizeof(
+		    ieee80211_wlan_frequencies) / sizeof(long))
+			iwe.u.freq.m =
+				(s32)(ieee80211_wlan_frequencies
+				      [dsconfig - 1] * 100000);
+		else
+			iwe.u.freq.m = 0;
+	}
+	iwe.u.freq.e = (s16)1;
+	iwe.u.freq.i = (u8)pnetwork->network.Configuration.DSConfig;
+	start = iwe_stream_add_event(info, start, stop, &iwe,
+		IW_EV_FREQ_LEN);
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	if (cap & WLAN_CAPABILITY_PRIVACY)
+		iwe.u.data.flags = (u16)(IW_ENCODE_ENABLED |
+				    IW_ENCODE_NOKEY);
+	else
+		iwe.u.data.flags = (u16)(IW_ENCODE_DISABLED);
+	iwe.u.data.length = (u16)0;
+	start = iwe_stream_add_point(info, start, stop, &iwe,
+		pnetwork->network.Ssid.Ssid);
+	/*Add basic and extended rates */
+	current_val = start + iwe_stream_lcp_len(info);
+	iwe.cmd = SIOCGIWRATE;
+	iwe.u.bitrate.fixed = 0;
+	iwe.u.bitrate.disabled = 0;
+	iwe.u.bitrate.value = 0;
+	i = 0;
+	while (pnetwork->network.rates[i] != 0) {
+		/* Bit rate given in 500 kb/s units */
+		iwe.u.bitrate.value = (pnetwork->network.rates[i++] &
+				      0x7F) * 500000;
+		current_val = iwe_stream_add_value(info, start, current_val,
+			      stop, &iwe, IW_EV_PARAM_LEN);
+	}
+	/* Check if we added any event */
+	if ((current_val - start) > iwe_stream_lcp_len(info))
+		start = current_val;
+
+	start = translate_scan_wpa(info, pnetwork, &iwe, start, stop);
+
+	start = translate_scan_wps(info, pnetwork, &iwe, start, stop);
+
+	/* Add quality statistics */
+	iwe.cmd = IWEVQUAL;
+	rssi = r8712_signal_scale_mapping(pnetwork->network.Rssi);
+	/* we only update signal_level (signal strength) that is rssi. */
+	iwe.u.qual.updated = (u8)(IW_QUAL_QUAL_INVALID | IW_QUAL_LEVEL_UPDATED |
+				  IW_QUAL_NOISE_INVALID);
+	iwe.u.qual.level = rssi;  /* signal strength */
+	iwe.u.qual.qual = 0; /* signal quality */
+	iwe.u.qual.noise = 0; /* noise level */
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+	/* how to translate rssi to ?% */
+	return start;
+}
+
+static int wpa_set_auth_algs(struct net_device *dev, u32 value)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	int ret = 0;
+
+	if ((value & AUTH_ALG_SHARED_KEY) && (value & AUTH_ALG_OPEN_SYSTEM)) {
+		padapter->securitypriv.ndisencryptstatus =
+						 Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.ndisauthtype =
+						 Ndis802_11AuthModeAutoSwitch;
+		padapter->securitypriv.AuthAlgrthm = 3;
+	} else if (value & AUTH_ALG_SHARED_KEY) {
+		padapter->securitypriv.ndisencryptstatus =
+						 Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeShared;
+		padapter->securitypriv.AuthAlgrthm = 1;
+	} else if (value & AUTH_ALG_OPEN_SYSTEM) {
+		if (padapter->securitypriv.ndisauthtype <
+						 Ndis802_11AuthModeWPAPSK) {
+			padapter->securitypriv.ndisauthtype =
+						 Ndis802_11AuthModeOpen;
+			padapter->securitypriv.AuthAlgrthm = 0;
+		}
+	} else {
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
+			      u32 param_len)
+{
+	int ret = 0;
+	u32 wep_key_idx, wep_key_len = 0;
+	struct NDIS_802_11_WEP	 *pwep = NULL;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+	if (param_len != (u32)((u8 *) param->u.crypt.key - (u8 *)param) +
+			 param->u.crypt.key_len)
+		return -EINVAL;
+	if (!is_broadcast_ether_addr(param->sta_addr))
+		return -EINVAL;
+
+	if (param->u.crypt.idx >= WEP_KEYS) {
+		/* for large key indices, set the default (0) */
+		param->u.crypt.idx = 0;
+	}
+	if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+		netdev_info(dev, "r8712u: %s: crypt.alg = WEP\n", __func__);
+		padapter->securitypriv.ndisencryptstatus =
+			     Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.XGrpPrivacy = _WEP40_;
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+		if (wep_key_idx >= WEP_KEYS)
+			wep_key_idx = 0;
+		if (wep_key_len <= 0)
+			return -EINVAL;
+
+		wep_key_len = wep_key_len <= 5 ? 5 : 13;
+		pwep = kzalloc(sizeof(*pwep), GFP_ATOMIC);
+		if (!pwep)
+			return -ENOMEM;
+		pwep->KeyLength = wep_key_len;
+		pwep->Length = wep_key_len +
+			offsetof(struct NDIS_802_11_WEP, KeyMaterial);
+		if (wep_key_len == 13) {
+			padapter->securitypriv.PrivacyAlgrthm = _WEP104_;
+			padapter->securitypriv.XGrpPrivacy = _WEP104_;
+		}
+		pwep->KeyIndex = wep_key_idx;
+		pwep->KeyIndex |= 0x80000000;
+		memcpy(pwep->KeyMaterial, param->u.crypt.key, pwep->KeyLength);
+		if (param->u.crypt.set_tx) {
+			if (r8712_set_802_11_add_wep(padapter, pwep))
+				ret = -EOPNOTSUPP;
+		} else {
+			/* don't update "psecuritypriv->PrivacyAlgrthm" and
+			 * "psecuritypriv->PrivacyKeyIndex=keyid", but can
+			 * r8712_set_key to fw/cam
+			 */
+			if (wep_key_idx >= WEP_KEYS) {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			}
+			memcpy(&psecuritypriv->DefKey[wep_key_idx].skey[0],
+			       pwep->KeyMaterial,
+			       pwep->KeyLength);
+			psecuritypriv->DefKeylen[wep_key_idx] =
+				pwep->KeyLength;
+			r8712_set_key(padapter, psecuritypriv, wep_key_idx);
+		}
+		goto exit;
+	}
+	if (padapter->securitypriv.AuthAlgrthm == 2) { /* 802_1x */
+		struct sta_info *psta, *pbcmc_sta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		struct security_priv *spriv = &padapter->securitypriv;
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE |
+		    WIFI_MP_STATE)) { /* sta mode */
+			psta = r8712_get_stainfo(pstapriv,
+						 get_bssid(pmlmepriv));
+			if (psta) {
+				psta->ieee8021x_blocked = false;
+				if (spriv->ndisencryptstatus ==
+				    Ndis802_11Encryption2Enabled ||
+				    spriv->ndisencryptstatus ==
+				    Ndis802_11Encryption3Enabled)
+					psta->XPrivacy = spriv->PrivacyAlgrthm;
+				if (param->u.crypt.set_tx == 1)
+					handle_pairwise_key(psta, param,
+							    padapter);
+				else /* group key */
+					handle_group_key(param, padapter);
+			}
+			pbcmc_sta = r8712_get_bcmc_stainfo(padapter);
+			if (pbcmc_sta) {
+				pbcmc_sta->ieee8021x_blocked = false;
+				if (spriv->ndisencryptstatus ==
+				    Ndis802_11Encryption2Enabled ||
+				    spriv->ndisencryptstatus ==
+				    Ndis802_11Encryption3Enabled)
+					pbcmc_sta->XPrivacy =
+						spriv->PrivacyAlgrthm;
+			}
+		}
+	}
+exit:
+	kfree(pwep);
+	return ret;
+}
+
+static int r871x_set_wpa_ie(struct _adapter *padapter, char *pie,
+			    unsigned short ielen)
+{
+	u8 *buf = NULL;
+	int group_cipher = 0, pairwise_cipher = 0;
+	int ret = 0;
+
+	if (ielen > MAX_WPA_IE_LEN || !pie)
+		return -EINVAL;
+	if (ielen) {
+		buf = kmemdup(pie, ielen, GFP_ATOMIC);
+		if (!buf)
+			return -ENOMEM;
+		if (ielen < RSN_HEADER_LEN) {
+			ret  = -EINVAL;
+			goto exit;
+		}
+		if (r8712_parse_wpa_ie(buf, ielen, &group_cipher,
+		    &pairwise_cipher) == 0) {
+			padapter->securitypriv.AuthAlgrthm = 2;
+			padapter->securitypriv.ndisauthtype =
+				  Ndis802_11AuthModeWPAPSK;
+		}
+		if (r8712_parse_wpa2_ie(buf, ielen, &group_cipher,
+		    &pairwise_cipher) == 0) {
+			padapter->securitypriv.AuthAlgrthm = 2;
+			padapter->securitypriv.ndisauthtype =
+				  Ndis802_11AuthModeWPA2PSK;
+		}
+		switch (group_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.XGrpPrivacy =
+				 _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.XGrpPrivacy = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.XGrpPrivacy = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.XGrpPrivacy = _AES_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.XGrpPrivacy = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+			break;
+		}
+		switch (pairwise_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.PrivacyAlgrthm =
+				 _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.PrivacyAlgrthm = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.PrivacyAlgrthm = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.PrivacyAlgrthm = _AES_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.PrivacyAlgrthm = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+			break;
+		}
+		padapter->securitypriv.wps_phase = false;
+		{/* set wps_ie */
+			u16 cnt = 0;
+			u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+			while (cnt < ielen) {
+				eid = buf[cnt];
+
+				if ((eid == WLAN_EID_VENDOR_SPECIFIC) &&
+				    (!memcmp(&buf[cnt + 2], wps_oui, 4))) {
+					netdev_info(padapter->pnetdev, "r8712u: SET WPS_IE\n");
+					padapter->securitypriv.wps_ie_len =
+					    ((buf[cnt + 1] + 2) <
+					    (MAX_WPA_IE_LEN << 2)) ?
+					    (buf[cnt + 1] + 2) :
+					    (MAX_WPA_IE_LEN << 2);
+					memcpy(padapter->securitypriv.wps_ie,
+					    &buf[cnt],
+					    padapter->securitypriv.wps_ie_len);
+					padapter->securitypriv.wps_phase =
+								 true;
+					netdev_info(padapter->pnetdev, "r8712u: SET WPS_IE, wps_phase==true\n");
+					cnt += buf[cnt + 1] + 2;
+					break;
+				}
+
+				cnt += buf[cnt + 1] + 2;
+			}
+		}
+	}
+exit:
+	kfree(buf);
+	return ret;
+}
+
+static int r8711_wx_get_name(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	u32 ht_ielen = 0;
+	char *p;
+	u8 ht_cap = false;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+	u8 *prates;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE) ==
+	    true) {
+		/* parsing HT_CAP_IE */
+		p = r8712_get_ie(&pcur_bss->IEs[12], WLAN_EID_HT_CAPABILITY,
+				 &ht_ielen, pcur_bss->IELength - 12);
+		if (p && ht_ielen > 0)
+			ht_cap = true;
+		prates = pcur_bss->rates;
+		if (r8712_is_cckratesonly_included(prates)) {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11bn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11b");
+		} else if (r8712_is_cckrates_included(prates)) {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11bgn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11bg");
+		} else {
+			if (ht_cap)
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11gn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ,
+					 "IEEE 802.11g");
+		}
+	} else {
+		snprintf(wrqu->name, IFNAMSIZ, "unassociated");
+	}
+	return 0;
+}
+
+static const long frequency_list[] = {
+	2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462,
+	2467, 2472, 2484, 4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
+	5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210,
+	5220, 5230, 5240, 5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560,
+	5580, 5600, 5620, 5640, 5660, 5680, 5700, 5745, 5765, 5785, 5805,
+	5825
+};
+
+static int r8711_wx_set_freq(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_freq *fwrq = &wrqu->freq;
+	int rc = 0;
+
+/* If setting by frequency, convert to a channel */
+	if ((fwrq->e == 1) &&
+	  (fwrq->m >= 241200000) &&
+	  (fwrq->m <= 248700000)) {
+		int f = fwrq->m / 100000;
+		int c = 0;
+
+		while ((c < 14) && (f != frequency_list[c]))
+			c++;
+		fwrq->e = 0;
+		fwrq->m = c + 1;
+	}
+	/* Setting by channel number */
+	if ((fwrq->m > 14) || (fwrq->e > 0)) {
+		rc = -EOPNOTSUPP;
+	} else {
+		int channel = fwrq->m;
+
+		if ((channel < 1) || (channel > 14)) {
+			rc = -EINVAL;
+		} else {
+			/* Yes ! We can set it !!! */
+			padapter->registrypriv.channel = channel;
+		}
+	}
+	return rc;
+}
+
+static int r8711_wx_get_freq(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED))
+		return -ENOLINK;
+
+	wrqu->freq.m = ieee80211_wlan_frequencies[
+		       pcur_bss->Configuration.DSConfig - 1] * 100000;
+	wrqu->freq.e = 1;
+	wrqu->freq.i = pcur_bss->Configuration.DSConfig;
+
+	return 0;
+}
+
+static int r8711_wx_set_mode(struct net_device *dev,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE networkType;
+
+	switch (wrqu->mode) {
+	case IW_MODE_AUTO:
+		networkType = Ndis802_11AutoUnknown;
+		break;
+	case IW_MODE_ADHOC:
+		networkType = Ndis802_11IBSS;
+		break;
+	case IW_MODE_MASTER:
+		networkType = Ndis802_11APMode;
+		break;
+	case IW_MODE_INFRA:
+		networkType = Ndis802_11Infrastructure;
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (Ndis802_11APMode == networkType)
+		r8712_setopmode_cmd(padapter, networkType);
+	else
+		r8712_setopmode_cmd(padapter, Ndis802_11AutoUnknown);
+
+	r8712_set_802_11_infrastructure_mode(padapter, networkType);
+	return 0;
+}
+
+static int r8711_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		wrqu->mode = IW_MODE_INFRA;
+	else if (check_fwstate(pmlmepriv,
+		 WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE))
+		wrqu->mode = IW_MODE_ADHOC;
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		wrqu->mode = IW_MODE_MASTER;
+	else
+		wrqu->mode = IW_MODE_AUTO;
+	return 0;
+}
+
+static int r871x_wx_set_pmkid(struct net_device *dev,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct iw_pmksa *pPMK = (struct iw_pmksa *) extra;
+	struct RT_PMKID_LIST *pl = psecuritypriv->PMKIDList;
+	u8 strZeroMacAddress[ETH_ALEN] = {0x00};
+	u8 strIssueBssid[ETH_ALEN] = {0x00};
+	u8 j, blInserted = false;
+	int intReturn = false;
+
+/*
+ *	There are the BSSID information in the bssid.sa_data array.
+ *	If cmd is IW_PMKSA_FLUSH, it means the wpa_supplicant wants to clear
+ *	all the PMKID information. If cmd is IW_PMKSA_ADD, it means the
+ *	wpa_supplicant wants to add a PMKID/BSSID to driver.
+ *	If cmd is IW_PMKSA_REMOVE, it means the wpa_supplicant wants to
+ *	remove a PMKID/BSSID from driver.
+ */
+	if (!pPMK)
+		return -EINVAL;
+	memcpy(strIssueBssid, pPMK->bssid.sa_data, ETH_ALEN);
+	switch (pPMK->cmd) {
+	case IW_PMKSA_ADD:
+		if (!memcmp(strIssueBssid, strZeroMacAddress, ETH_ALEN))
+			return intReturn;
+		intReturn = true;
+		blInserted = false;
+		/* overwrite PMKID */
+		for (j = 0; j < NUM_PMKID_CACHE; j++) {
+			if (!memcmp(pl[j].Bssid, strIssueBssid, ETH_ALEN)) {
+				/* BSSID is matched, the same AP => rewrite
+				 * with new PMKID.
+				 */
+				netdev_info(dev, "r8712u: %s: BSSID exists in the PMKList.\n",
+					    __func__);
+				memcpy(pl[j].PMKID, pPMK->pmkid, IW_PMKID_LEN);
+				pl[j].bUsed = true;
+				psecuritypriv->PMKIDIndex = j + 1;
+				blInserted = true;
+				break;
+			}
+		}
+		if (!blInserted) {
+			/* Find a new entry */
+			netdev_info(dev, "r8712u: %s: Use the new entry index = %d for this PMKID.\n",
+				    __func__, psecuritypriv->PMKIDIndex);
+			memcpy(pl[psecuritypriv->PMKIDIndex].Bssid,
+			       strIssueBssid, ETH_ALEN);
+			memcpy(pl[psecuritypriv->PMKIDIndex].PMKID,
+			       pPMK->pmkid, IW_PMKID_LEN);
+			pl[psecuritypriv->PMKIDIndex].bUsed = true;
+			psecuritypriv->PMKIDIndex++;
+			if (psecuritypriv->PMKIDIndex == NUM_PMKID_CACHE)
+				psecuritypriv->PMKIDIndex = 0;
+		}
+		break;
+	case IW_PMKSA_REMOVE:
+		intReturn = true;
+		for (j = 0; j < NUM_PMKID_CACHE; j++) {
+			if (!memcmp(pl[j].Bssid, strIssueBssid, ETH_ALEN)) {
+				/* BSSID is matched, the same AP => Remove
+				 * this PMKID information and reset it.
+				 */
+				eth_zero_addr(pl[j].Bssid);
+				pl[j].bUsed = false;
+				break;
+			}
+		}
+		break;
+	case IW_PMKSA_FLUSH:
+		memset(psecuritypriv->PMKIDList, 0,
+			sizeof(struct RT_PMKID_LIST) * NUM_PMKID_CACHE);
+		psecuritypriv->PMKIDIndex = 0;
+		intReturn = true;
+		break;
+	default:
+		netdev_info(dev, "r8712u: %s: unknown Command\n", __func__);
+		intReturn = false;
+		break;
+	}
+	return intReturn;
+}
+
+static int r8711_wx_get_sens(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	wrqu->sens.value = 0;
+	wrqu->sens.fixed = 0;	/* no auto select */
+	wrqu->sens.disabled = 1;
+	return 0;
+}
+
+static int r8711_wx_get_range(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct iw_range *range = (struct iw_range *)extra;
+	u16 val;
+	int i;
+
+	wrqu->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+	/* Let's try to keep this struct in the same order as in
+	 * linux/include/wireless.h
+	 */
+
+	/* TODO: See what values we can set, and remove the ones we can't
+	 * set, or fill them with some default data.
+	 */
+	/* ~5 Mb/s real (802.11b) */
+	range->throughput = 5 * 1000 * 1000;
+	/* TODO: 8711 sensitivity ? */
+	/* signal level threshold range */
+	/* percent values between 0 and 100. */
+	range->max_qual.qual = 100;
+	range->max_qual.level = 100;
+	range->max_qual.noise = 100;
+	range->max_qual.updated = 7; /* Updated all three */
+	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
+	/* TODO: Find real 'good' to 'bad' threshold value for RSSI */
+	range->avg_qual.level = 0x100 - 78;
+	range->avg_qual.noise = 0;
+	range->avg_qual.updated = 7; /* Updated all three */
+	range->num_bitrates = RATE_COUNT;
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
+		range->bitrate[i] = rtl8180_rates[i];
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+	range->pm_capa = 0;
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 16;
+	range->num_channels = 14;
+	for (i = 0, val = 0; i < 14; i++) {
+		/* Include only legal frequencies for some countries */
+		range->freq[val].i = i + 1;
+		range->freq[val].m = ieee80211_wlan_frequencies[i] * 100000;
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+	range->enc_capa = IW_ENC_CAPA_WPA |
+			  IW_ENC_CAPA_WPA2 |
+			  IW_ENC_CAPA_CIPHER_TKIP |
+			  IW_ENC_CAPA_CIPHER_CCMP;
+	return 0;
+}
+
+static int r8711_wx_get_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+static int r871x_wx_set_priv(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *awrq,
+				char *extra)
+{
+	int ret = 0, len = 0;
+	char *ext;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *dwrq = (struct iw_point *)awrq;
+
+	len = dwrq->length;
+	ext = strndup_user(dwrq->pointer, len);
+	if (IS_ERR(ext))
+		return PTR_ERR(ext);
+
+	if (!strcasecmp(ext, "RSSI")) {
+		/*Return received signal strength indicator in -db for */
+		/* current AP */
+		/*<ssid> Rssi xx */
+		struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+		struct wlan_network *pcur_network = &pmlmepriv->cur_network;
+		/*static u8 xxxx; */
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			sprintf(ext, "%s rssi %d",
+				pcur_network->network.Ssid.Ssid,
+				/*(xxxx=xxxx+10) */
+				((padapter->recvpriv.fw_rssi) >> 1) - 95
+				/*pcur_network->network.Rssi */
+				);
+		} else {
+			sprintf(ext, "OK");
+		}
+	} else if (!strcasecmp(ext, "LINKSPEED")) {
+		/*Return link speed in MBPS */
+		/*LinkSpeed xx */
+		union iwreq_data wrqd;
+		int ret_inner;
+		int mbps;
+
+		ret_inner = r8711_wx_get_rate(dev, info, &wrqd, extra);
+		if (ret_inner != 0)
+			mbps = 0;
+		else
+			mbps = wrqd.bitrate.value / 1000000;
+		sprintf(ext, "LINKSPEED %d", mbps);
+	} else if (!strcasecmp(ext, "MACADDR")) {
+		/*Return mac address of the station */
+		/* Macaddr = xx:xx:xx:xx:xx:xx */
+		sprintf(ext, "MACADDR = %pM", dev->dev_addr);
+	} else if (!strcasecmp(ext, "SCAN-ACTIVE")) {
+		/*Set scan type to active */
+		/*OK if successful */
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+		pmlmepriv->passive_mode = 1;
+		sprintf(ext, "OK");
+	} else if (!strcasecmp(ext, "SCAN-PASSIVE")) {
+		/*Set scan type to passive */
+		/*OK if successful */
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+		pmlmepriv->passive_mode = 0;
+		sprintf(ext, "OK");
+	} else if (!strncmp(ext, "DCE-E", 5)) {
+		/*Set scan type to passive */
+		/*OK if successful */
+		r8712_disconnectCtrlEx_cmd(padapter
+			, 1 /*u32 enableDrvCtrl */
+			, 5 /*u32 tryPktCnt */
+			, 100 /*u32 tryPktInterval */
+			, 5000 /*u32 firstStageTO */
+		);
+		sprintf(ext, "OK");
+	} else if (!strncmp(ext, "DCE-D", 5)) {
+		/*Set scan type to passive */
+		/*OK if successfu */
+		r8712_disconnectCtrlEx_cmd(padapter
+			, 0 /*u32 enableDrvCtrl */
+			, 5 /*u32 tryPktCnt */
+			, 100 /*u32 tryPktInterval */
+			, 5000 /*u32 firstStageTO */
+		);
+		sprintf(ext, "OK");
+	} else {
+		netdev_info(dev, "r8712u: %s: unknown Command %s.\n",
+			    __func__, ext);
+		goto FREE_EXT;
+	}
+	if (copy_to_user(dwrq->pointer, ext,
+				min(dwrq->length, (__u16)(strlen(ext) + 1))))
+		ret = -EFAULT;
+
+FREE_EXT:
+	kfree(ext);
+	return ret;
+}
+
+/* set bssid flow
+ * s1. set_802_11_infrastructure_mode()
+ * s2. set_802_11_authentication_mode()
+ * s3. set_802_11_encryption_mode()
+ * s4. set_802_11_bssid()
+ *
+ * This function intends to handle the Set AP command, which specifies the
+ * MAC# of a preferred Access Point.
+ * Currently, the request comes via Wireless Extensions' SIOCSIWAP ioctl.
+ *
+ * For this operation to succeed, there is no need for the interface to be up.
+ *
+ */
+static int r8711_wx_set_wap(struct net_device *dev,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+	int ret = -EINPROGRESS;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+	struct sockaddr *temp = (struct sockaddr *)awrq;
+	unsigned long irqL;
+	struct list_head *phead;
+	u8 *dst_bssid;
+	struct wlan_network *pnetwork = NULL;
+	enum NDIS_802_11_AUTHENTICATION_MODE	authmode;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		return -EBUSY;
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		return ret;
+	if (temp->sa_family != ARPHRD_ETHER)
+		return -EINVAL;
+	authmode = padapter->securitypriv.ndisauthtype;
+	spin_lock_irqsave(&queue->lock, irqL);
+	phead = &queue->queue;
+	pmlmepriv->pscanned = phead->next;
+	while (1) {
+		if (end_of_queue_search(phead, pmlmepriv->pscanned))
+			break;
+		pnetwork = container_of(pmlmepriv->pscanned,
+					struct wlan_network, list);
+		pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+		dst_bssid = pnetwork->network.MacAddress;
+		if (!memcmp(dst_bssid, temp->sa_data, ETH_ALEN)) {
+			r8712_set_802_11_infrastructure_mode(padapter,
+			    pnetwork->network.InfrastructureMode);
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&queue->lock, irqL);
+	if (!ret) {
+		if (!r8712_set_802_11_authentication_mode(padapter, authmode)) {
+			ret = -ENOMEM;
+		} else {
+			if (!r8712_set_802_11_bssid(padapter, temp->sa_data))
+				ret = -1;
+		}
+	}
+	return ret;
+}
+
+static int r8711_wx_get_wap(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+
+	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE |
+				     WIFI_AP_STATE))
+		ether_addr_copy(wrqu->ap_addr.sa_data, pcur_bss->MacAddress);
+	else
+		eth_zero_addr(wrqu->ap_addr.sa_data);
+	return 0;
+}
+
+static int r871x_wx_set_mlme(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_mlme *mlme = (struct iw_mlme *) extra;
+
+	if (!mlme)
+		return -1;
+	switch (mlme->cmd) {
+	case IW_MLME_DEAUTH:
+		if (!r8712_set_802_11_disassociate(padapter))
+			ret = -1;
+		break;
+	case IW_MLME_DISASSOC:
+		if (!r8712_set_802_11_disassociate(padapter))
+			ret = -1;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return ret;
+}
+
+/*
+ *
+ * This function intends to handle the Set Scan command.
+ * Currently, the request comes via Wireless Extensions' SIOCSIWSCAN ioctl.
+ *
+ * For this operation to succeed, the interface is brought Up beforehand.
+ *
+ */
+static int r8711_wx_set_scan(struct net_device *dev,
+			struct iw_request_info *a,
+			union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 status = true;
+
+	if (padapter->driver_stopped) {
+		netdev_info(dev, "In %s: driver_stopped=%d\n",
+			    __func__, padapter->driver_stopped);
+		return -1;
+	}
+	if (!padapter->bup)
+		return -ENETDOWN;
+	if (!padapter->hw_init_completed)
+		return -1;
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING)) ||
+	    (pmlmepriv->sitesurveyctrl.traffic_busy))
+		return 0;
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		struct iw_scan_req *req = (struct iw_scan_req *)extra;
+
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			struct ndis_802_11_ssid ssid;
+			unsigned long irqL;
+			u32 len = min_t(u8, req->essid_len, IW_ESSID_MAX_SIZE);
+
+			memset((unsigned char *)&ssid, 0,
+				 sizeof(struct ndis_802_11_ssid));
+			memcpy(ssid.Ssid, req->essid, len);
+			ssid.SsidLength = len;
+			spin_lock_irqsave(&pmlmepriv->lock, irqL);
+			if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY |
+			     _FW_UNDER_LINKING)) ||
+			    (pmlmepriv->sitesurveyctrl.traffic_busy)) {
+				if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+					status = false;
+			} else {
+				status = r8712_sitesurvey_cmd(padapter, &ssid);
+			}
+			spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+		}
+	} else {
+		status = r8712_set_802_11_bssid_list_scan(padapter);
+	}
+	if (!status)
+		return -1;
+	return 0;
+}
+
+static int r8711_wx_get_scan(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+	struct wlan_network *pnetwork = NULL;
+	unsigned long irqL;
+	struct list_head *plist, *phead;
+	char *ev = extra;
+	char *stop = ev + wrqu->data.length;
+	u32 ret = 0, cnt = 0;
+
+	if (padapter->driver_stopped)
+		return -EINVAL;
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY |
+			     _FW_UNDER_LINKING)) {
+		msleep(30);
+		cnt++;
+		if (cnt > 100)
+			break;
+	}
+	spin_lock_irqsave(&queue->lock, irqL);
+	phead = &queue->queue;
+	plist = phead->next;
+	while (1) {
+		if (end_of_queue_search(phead, plist))
+			break;
+		if ((stop - ev) < SCAN_ITEM_SIZE) {
+			ret = -E2BIG;
+			break;
+		}
+		pnetwork = container_of(plist, struct wlan_network, list);
+		ev = translate_scan(padapter, a, pnetwork, ev, stop);
+		plist = plist->next;
+	}
+	spin_unlock_irqrestore(&queue->lock, irqL);
+	wrqu->data.length = ev - extra;
+	wrqu->data.flags = 0;
+	return ret;
+}
+
+/* set ssid flow
+ * s1. set_802_11_infrastructure_mode()
+ * s2. set_802_11_authenticaion_mode()
+ * s3. set_802_11_encryption_mode()
+ * s4. set_802_11_ssid()
+ *
+ * This function intends to handle the Set ESSID command.
+ * Currently, the request comes via the Wireless Extensions' SIOCSIWESSID ioctl.
+ *
+ * For this operation to succeed, there is no need for the interface to be Up.
+ *
+ */
+static int r8711_wx_set_essid(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+	struct wlan_network *pnetwork = NULL;
+	enum NDIS_802_11_AUTHENTICATION_MODE	authmode;
+	struct ndis_802_11_ssid ndis_ssid;
+	u8 *dst_ssid, *src_ssid;
+	struct list_head *phead;
+	u32 len;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		return -EBUSY;
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		return 0;
+	if (wrqu->essid.length > IW_ESSID_MAX_SIZE)
+		return -E2BIG;
+	authmode = padapter->securitypriv.ndisauthtype;
+	if (wrqu->essid.flags && wrqu->essid.length) {
+		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ?
+		       wrqu->essid.length : IW_ESSID_MAX_SIZE;
+		memset(&ndis_ssid, 0, sizeof(struct ndis_802_11_ssid));
+		ndis_ssid.SsidLength = len;
+		memcpy(ndis_ssid.Ssid, extra, len);
+		src_ssid = ndis_ssid.Ssid;
+		phead = &queue->queue;
+		pmlmepriv->pscanned = phead->next;
+		while (1) {
+			if (end_of_queue_search(phead, pmlmepriv->pscanned))
+				break;
+			pnetwork = container_of(pmlmepriv->pscanned,
+						struct wlan_network, list);
+			pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+			dst_ssid = pnetwork->network.Ssid.Ssid;
+			if ((!memcmp(dst_ssid, src_ssid, ndis_ssid.SsidLength))
+			    && (pnetwork->network.Ssid.SsidLength ==
+			     ndis_ssid.SsidLength)) {
+				if (check_fwstate(pmlmepriv,
+							WIFI_ADHOC_STATE)) {
+					if (pnetwork->network.
+						InfrastructureMode
+						!=
+						padapter->mlmepriv.
+						cur_network.network.
+						InfrastructureMode)
+						continue;
+				}
+
+				r8712_set_802_11_infrastructure_mode(
+				     padapter,
+				     pnetwork->network.InfrastructureMode);
+				break;
+			}
+		}
+		r8712_set_802_11_authentication_mode(padapter, authmode);
+		r8712_set_802_11_ssid(padapter, &ndis_ssid);
+	}
+	return -EINPROGRESS;
+}
+
+static int r8711_wx_get_essid(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+	u32 len, ret = 0;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		len = pcur_bss->Ssid.SsidLength;
+		wrqu->essid.length = len;
+		memcpy(extra, pcur_bss->Ssid.Ssid, len);
+		wrqu->essid.flags = 1;
+	} else {
+		ret = -ENOLINK;
+	}
+	return ret;
+}
+
+static int r8711_wx_set_rate(struct net_device *dev,
+				struct iw_request_info *a,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	u32 target_rate = wrqu->bitrate.value;
+	u32 fixed = wrqu->bitrate.fixed;
+	u32 ratevalue = 0;
+	u8 datarates[NumRates];
+	u8 mpdatarate[NumRates] = {11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 0xff};
+	int i;
+
+	if (target_rate == -1) {
+		ratevalue = 11;
+		goto set_rate;
+	}
+	target_rate = target_rate / 100000;
+	switch (target_rate) {
+	case 10:
+		ratevalue = 0;
+		break;
+	case 20:
+		ratevalue = 1;
+		break;
+	case 55:
+		ratevalue = 2;
+		break;
+	case 60:
+		ratevalue = 3;
+		break;
+	case 90:
+		ratevalue = 4;
+		break;
+	case 110:
+		ratevalue = 5;
+		break;
+	case 120:
+		ratevalue = 6;
+		break;
+	case 180:
+		ratevalue = 7;
+		break;
+	case 240:
+		ratevalue = 8;
+		break;
+	case 360:
+		ratevalue = 9;
+		break;
+	case 480:
+		ratevalue = 10;
+		break;
+	case 540:
+		ratevalue = 11;
+		break;
+	default:
+		ratevalue = 11;
+		break;
+	}
+set_rate:
+	for (i = 0; i < NumRates; i++) {
+		if (ratevalue == mpdatarate[i]) {
+			datarates[i] = mpdatarate[i];
+			if (fixed == 0)
+				break;
+		} else {
+			datarates[i] = 0xff;
+		}
+	}
+	return r8712_setdatarate_cmd(padapter, datarates);
+}
+
+static int r8711_wx_get_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+	struct ieee80211_ht_cap *pht_capie;
+	unsigned char rf_type = padapter->registrypriv.rf_config;
+	int i;
+	u8 *p;
+	u16 rate, max_rate = 0, ht_cap = false;
+	u32 ht_ielen = 0;
+	u8 bw_40MHz = 0, short_GI = 0;
+	u16 mcs_rate = 0;
+
+	i = 0;
+	if (!check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE))
+		return -ENOLINK;
+	p = r8712_get_ie(&pcur_bss->IEs[12], WLAN_EID_HT_CAPABILITY, &ht_ielen,
+			 pcur_bss->IELength - 12);
+	if (p && ht_ielen > 0) {
+		ht_cap = true;
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		memcpy(&mcs_rate, &pht_capie->mcs, 2);
+		bw_40MHz = (le16_to_cpu(pht_capie->cap_info) &
+			    IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+		short_GI = (le16_to_cpu(pht_capie->cap_info) &
+			    (IEEE80211_HT_CAP_SGI_20 |
+			    IEEE80211_HT_CAP_SGI_40)) ? 1 : 0;
+	}
+	while ((pcur_bss->rates[i] != 0) &&
+	       (pcur_bss->rates[i] != 0xFF)) {
+		rate = pcur_bss->rates[i] & 0x7F;
+		if (rate > max_rate)
+			max_rate = rate;
+		wrqu->bitrate.fixed = 0;	/* no auto select */
+		wrqu->bitrate.value = rate * 500000;
+		i++;
+	}
+	if (ht_cap) {
+		if (mcs_rate & 0x8000 /* MCS15 */
+		    &&
+		    rf_type == RTL8712_RF_2T2R)
+			max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) :
+			((short_GI) ? 144 : 130);
+		else /* default MCS7 */
+			max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) :
+			((short_GI) ? 72 : 65);
+		max_rate *= 2; /* Mbps/2 */
+	}
+	wrqu->bitrate.value = max_rate * 500000;
+	return 0;
+}
+
+static int r8711_wx_get_rts(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	wrqu->rts.value = padapter->registrypriv.rts_thresh;
+	wrqu->rts.fixed = 0;	/* no auto select */
+	return 0;
+}
+
+static int r8711_wx_set_frag(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	if (wrqu->frag.disabled) {
+		padapter->xmitpriv.frag_len = MAX_FRAG_THRESHOLD;
+	} else {
+		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
+		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
+			return -EINVAL;
+		padapter->xmitpriv.frag_len = wrqu->frag.value & ~0x1;
+	}
+	return 0;
+}
+
+static int r8711_wx_get_frag(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	wrqu->frag.value = padapter->xmitpriv.frag_len;
+	wrqu->frag.fixed = 0;	/* no auto select */
+	return 0;
+}
+
+static int r8711_wx_get_retry(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	wrqu->retry.value = 7;
+	wrqu->retry.fixed = 0;	/* no auto select */
+	wrqu->retry.disabled = 1;
+	return 0;
+}
+
+static int r8711_wx_set_enc(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *keybuf)
+{
+	u32 key;
+	u32 keyindex_provided;
+	struct NDIS_802_11_WEP	 wep;
+	enum NDIS_802_11_AUTHENTICATION_MODE authmode;
+	struct iw_point *erq = &wrqu->encoding;
+	struct _adapter *padapter = netdev_priv(dev);
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	memset(&wep, 0, sizeof(struct NDIS_802_11_WEP));
+	if (erq->flags & IW_ENCODE_DISABLED) {
+		netdev_info(dev, "r8712u: %s: EncryptionDisabled\n", __func__);
+		padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11EncryptionDisabled;
+		padapter->securitypriv.PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.XGrpPrivacy = _NO_PRIVACY_;
+		padapter->securitypriv.AuthAlgrthm = 0; /* open system */
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+		return 0;
+	}
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+		keyindex_provided = 1;
+	} else {
+		keyindex_provided = 0;
+		key = padapter->securitypriv.PrivacyKeyIndex;
+	}
+	/* set authentication mode */
+	if (erq->flags & IW_ENCODE_OPEN) {
+		netdev_info(dev, "r8712u: %s: IW_ENCODE_OPEN\n", __func__);
+		padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.AuthAlgrthm = 0; /* open system */
+		padapter->securitypriv.PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.XGrpPrivacy = _NO_PRIVACY_;
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+	} else if (erq->flags & IW_ENCODE_RESTRICTED) {
+		netdev_info(dev,
+				"r8712u: %s: IW_ENCODE_RESTRICTED\n", __func__);
+		padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.AuthAlgrthm = 1; /* shared system */
+		padapter->securitypriv.PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.XGrpPrivacy = _WEP40_;
+		authmode = Ndis802_11AuthModeShared;
+		padapter->securitypriv.ndisauthtype = authmode;
+	} else {
+		padapter->securitypriv.ndisencryptstatus =
+				 Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.AuthAlgrthm = 0; /* open system */
+		padapter->securitypriv.PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.XGrpPrivacy = _NO_PRIVACY_;
+		authmode = Ndis802_11AuthModeOpen;
+		padapter->securitypriv.ndisauthtype = authmode;
+	}
+	wep.KeyIndex = key;
+	if (erq->length > 0) {
+		wep.KeyLength = erq->length <= 5 ? 5 : 13;
+		wep.Length = wep.KeyLength +
+			     offsetof(struct NDIS_802_11_WEP, KeyMaterial);
+	} else {
+		wep.KeyLength = 0;
+		if (keyindex_provided == 1) { /* set key_id only, no given
+					       * KeyMaterial(erq->length==0).
+					       */
+			padapter->securitypriv.PrivacyKeyIndex = key;
+			switch (padapter->securitypriv.DefKeylen[key]) {
+			case 5:
+				padapter->securitypriv.PrivacyAlgrthm =
+						 _WEP40_;
+				break;
+			case 13:
+				padapter->securitypriv.PrivacyAlgrthm =
+						 _WEP104_;
+				break;
+			default:
+				padapter->securitypriv.PrivacyAlgrthm =
+						 _NO_PRIVACY_;
+				break;
+			}
+			return 0;
+		}
+	}
+	wep.KeyIndex |= 0x80000000;	/* transmit key */
+	memcpy(wep.KeyMaterial, keybuf, wep.KeyLength);
+	if (r8712_set_802_11_add_wep(padapter, &wep))
+		return -EOPNOTSUPP;
+	return 0;
+}
+
+static int r8711_wx_get_enc(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *keybuf)
+{
+	uint key;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *erq = &wrqu->encoding;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	union Keytype *dk = padapter->securitypriv.DefKey;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+		if (!check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			erq->length = 0;
+			erq->flags |= IW_ENCODE_DISABLED;
+			return 0;
+		}
+	}
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+	} else {
+		key = padapter->securitypriv.PrivacyKeyIndex;
+	}
+	erq->flags = key + 1;
+	switch (padapter->securitypriv.ndisencryptstatus) {
+	case Ndis802_11EncryptionNotSupported:
+	case Ndis802_11EncryptionDisabled:
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		break;
+	case Ndis802_11Encryption1Enabled:
+		erq->length = padapter->securitypriv.DefKeylen[key];
+		if (erq->length) {
+			memcpy(keybuf, dk[key].skey,
+			       padapter->securitypriv.DefKeylen[key]);
+			erq->flags |= IW_ENCODE_ENABLED;
+			if (padapter->securitypriv.ndisauthtype ==
+			    Ndis802_11AuthModeOpen)
+				erq->flags |= IW_ENCODE_OPEN;
+			else if (padapter->securitypriv.ndisauthtype ==
+				 Ndis802_11AuthModeShared)
+				erq->flags |= IW_ENCODE_RESTRICTED;
+		} else {
+			erq->length = 0;
+			erq->flags |= IW_ENCODE_DISABLED;
+		}
+		break;
+	case Ndis802_11Encryption2Enabled:
+	case Ndis802_11Encryption3Enabled:
+		erq->length = 16;
+		erq->flags |= (IW_ENCODE_ENABLED | IW_ENCODE_OPEN |
+			       IW_ENCODE_NOKEY);
+		break;
+	default:
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		break;
+	}
+	return 0;
+}
+
+static int r8711_wx_get_power(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	wrqu->power.value = 0;
+	wrqu->power.fixed = 0;	/* no auto select */
+	wrqu->power.disabled = 1;
+	return 0;
+}
+
+static int r871x_wx_set_gen_ie(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	return r871x_set_wpa_ie(padapter, extra, wrqu->data.length);
+}
+
+static int r871x_wx_set_auth(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_param *param = (struct iw_param *)&wrqu->param;
+	int paramid;
+	int paramval;
+	int ret = 0;
+
+	paramid = param->flags & IW_AUTH_INDEX;
+	paramval = param->value;
+	switch (paramid) {
+	case IW_AUTH_WPA_VERSION:
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+		break;
+	case IW_AUTH_CIPHER_GROUP:
+		break;
+	case IW_AUTH_KEY_MGMT:
+		/*
+		 *  ??? does not use these parameters
+		 */
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		if (paramval) {
+			/* wpa_supplicant is enabling tkip countermeasure. */
+			padapter->securitypriv.btkip_countermeasure = true;
+		} else {
+			/* wpa_supplicant is disabling tkip countermeasure. */
+			padapter->securitypriv.btkip_countermeasure = false;
+		}
+		break;
+	case IW_AUTH_DROP_UNENCRYPTED:
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+		if (padapter->securitypriv.ndisencryptstatus ==
+		    Ndis802_11Encryption1Enabled) {
+			/* it means init value, or using wep,
+			 * ndisencryptstatus =
+			 *	Ndis802_11Encryption1Enabled,
+			 * then it needn't reset it;
+			 */
+			break;
+		}
+
+		if (paramval) {
+			padapter->securitypriv.ndisencryptstatus =
+				   Ndis802_11EncryptionDisabled;
+			padapter->securitypriv.PrivacyAlgrthm =
+				  _NO_PRIVACY_;
+			padapter->securitypriv.XGrpPrivacy =
+				  _NO_PRIVACY_;
+			padapter->securitypriv.AuthAlgrthm = 0;
+			padapter->securitypriv.ndisauthtype =
+				  Ndis802_11AuthModeOpen;
+		}
+		break;
+	case IW_AUTH_80211_AUTH_ALG:
+		ret = wpa_set_auth_algs(dev, (u32)paramval);
+		break;
+	case IW_AUTH_WPA_ENABLED:
+		break;
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+static int r871x_wx_set_enc_ext(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct iw_point *pencoding = &wrqu->encoding;
+	struct iw_encode_ext *pext = (struct iw_encode_ext *)extra;
+	struct ieee_param *param = NULL;
+	char *alg_name;
+	u32 param_len;
+	int ret = 0;
+
+	switch (pext->alg) {
+	case IW_ENCODE_ALG_NONE:
+		alg_name = "none";
+		break;
+	case IW_ENCODE_ALG_WEP:
+		alg_name = "WEP";
+		break;
+	case IW_ENCODE_ALG_TKIP:
+		alg_name = "TKIP";
+		break;
+	case IW_ENCODE_ALG_CCMP:
+		alg_name = "CCMP";
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	param_len = sizeof(struct ieee_param) + pext->key_len;
+	param = kzalloc(param_len, GFP_ATOMIC);
+	if (!param)
+		return -ENOMEM;
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	eth_broadcast_addr(param->sta_addr);
+	strscpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
+	if (pext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+		param->u.crypt.set_tx = 0;
+	if (pext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		param->u.crypt.set_tx = 1;
+	param->u.crypt.idx = (pencoding->flags & 0x00FF) - 1;
+	if (pext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID)
+		memcpy(param->u.crypt.seq, pext->rx_seq, 8);
+	if (pext->key_len) {
+		param->u.crypt.key_len = pext->key_len;
+		memcpy(param + 1, pext + 1, pext->key_len);
+	}
+	ret = wpa_set_encryption(dev, param, param_len);
+	kfree(param);
+	return ret;
+}
+
+static int r871x_wx_get_nick(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	if (extra) {
+		wrqu->data.length = 8;
+		wrqu->data.flags = 1;
+		memcpy(extra, "rtl_wifi", 8);
+	}
+	return 0;
+}
+
+static int r8711_wx_read32(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *keybuf)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	u32 addr;
+	u32 data32;
+
+	get_user(addr, (u32 __user *)wrqu->data.pointer);
+	data32 = r8712_read32(padapter, addr);
+	put_user(data32, (u32 __user *)wrqu->data.pointer);
+	wrqu->data.length = (data32 & 0xffff0000) >> 16;
+	wrqu->data.flags = data32 & 0xffff;
+	get_user(addr, (u32 __user *)wrqu->data.pointer);
+	return 0;
+}
+
+static int r8711_wx_write32(struct net_device *dev,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *keybuf)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	u32 addr;
+	u32 data32;
+
+	get_user(addr, (u32 __user *)wrqu->data.pointer);
+	data32 = ((u32)wrqu->data.length << 16) | (u32)wrqu->data.flags;
+	r8712_write32(padapter, addr, data32);
+	return 0;
+}
+
+static int dummy(struct net_device *dev,
+		struct iw_request_info *a,
+		union iwreq_data *wrqu, char *b)
+{
+	return -EINVAL;
+}
+
+static int r8711_drvext_hdl(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	return 0;
+}
+
+static int r871x_mp_ioctl_hdl(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *p = &wrqu->data;
+	struct oid_par_priv oid_par;
+	struct mp_ioctl_handler *phandler;
+	struct mp_ioctl_param *poidparam;
+	unsigned long BytesRead, BytesWritten, BytesNeeded;
+	u8 *pparmbuf, bset;
+	u16 len;
+	uint status;
+	int ret = 0;
+
+	if ((!p->length) || (!p->pointer))
+		return -EINVAL;
+
+	bset = (u8)(p->flags & 0xFFFF);
+	len = p->length;
+	pparmbuf = memdup_user(p->pointer, len);
+	if (IS_ERR(pparmbuf))
+		return PTR_ERR(pparmbuf);
+
+	poidparam = (struct mp_ioctl_param *)pparmbuf;
+	if (poidparam->subcode >= MAX_MP_IOCTL_SUBCODE) {
+		ret = -EINVAL;
+		goto _r871x_mp_ioctl_hdl_exit;
+	}
+	phandler = mp_ioctl_hdl + poidparam->subcode;
+	if ((phandler->paramsize != 0) &&
+	    (poidparam->len < phandler->paramsize)) {
+		ret = -EINVAL;
+		goto _r871x_mp_ioctl_hdl_exit;
+	}
+	if (phandler->oid == 0 && phandler->handler) {
+		status = phandler->handler(&oid_par);
+	} else if (phandler->handler) {
+		oid_par.adapter_context = padapter;
+		oid_par.oid = phandler->oid;
+		oid_par.information_buf = poidparam->data;
+		oid_par.information_buf_len = poidparam->len;
+		oid_par.dbg = 0;
+		BytesWritten = 0;
+		BytesNeeded = 0;
+		if (bset) {
+			oid_par.bytes_rw = &BytesRead;
+			oid_par.bytes_needed = &BytesNeeded;
+			oid_par.type_of_oid = SET_OID;
+		} else {
+			oid_par.bytes_rw = &BytesWritten;
+			oid_par.bytes_needed = &BytesNeeded;
+			oid_par.type_of_oid = QUERY_OID;
+		}
+		status = phandler->handler(&oid_par);
+		/* todo:check status, BytesNeeded, etc. */
+	} else {
+		netdev_info(dev, "r8712u: %s: err!, subcode=%d, oid=%d, handler=%p\n",
+			    __func__, poidparam->subcode, phandler->oid,
+			    phandler->handler);
+		ret = -EFAULT;
+		goto _r871x_mp_ioctl_hdl_exit;
+	}
+	if (bset == 0x00) { /* query info */
+		if (copy_to_user(p->pointer, pparmbuf, len))
+			ret = -EFAULT;
+	}
+	if (status) {
+		ret = -EFAULT;
+		goto _r871x_mp_ioctl_hdl_exit;
+	}
+_r871x_mp_ioctl_hdl_exit:
+	kfree(pparmbuf);
+	return ret;
+}
+
+static int r871x_get_ap_info(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+	struct iw_point *pdata = &wrqu->data;
+	struct wlan_network *pnetwork = NULL;
+	u32 cnt = 0, wpa_ielen;
+	unsigned long irqL;
+	struct list_head *plist, *phead;
+	unsigned char *pbuf;
+	u8 bssid[ETH_ALEN];
+	char data[33];
+
+	if (padapter->driver_stopped || !pdata)
+		return -EINVAL;
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY |
+			     _FW_UNDER_LINKING)) {
+		msleep(30);
+		cnt++;
+		if (cnt > 100)
+			break;
+	}
+	pdata->flags = 0;
+	if (pdata->length < 32)
+		return -EINVAL;
+	if (copy_from_user(data, pdata->pointer, 32))
+		return -EINVAL;
+	data[32] = 0;
+
+	spin_lock_irqsave(&pmlmepriv->scanned_queue.lock, irqL);
+	phead = &queue->queue;
+	plist = phead->next;
+	while (1) {
+		if (end_of_queue_search(phead, plist))
+			break;
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (!mac_pton(data, bssid)) {
+			netdev_info(dev, "r8712u: Invalid BSSID '%s'.\n",
+				    (u8 *)data);
+			spin_unlock_irqrestore(&pmlmepriv->scanned_queue.lock,
+					       irqL);
+			return -EINVAL;
+		}
+		netdev_info(dev, "r8712u: BSSID:%pM\n", bssid);
+		if (ether_addr_equal(bssid, pnetwork->network.MacAddress)) {
+			/* BSSID match, then check if supporting wpa/wpa2 */
+			pbuf = r8712_get_wpa_ie(&pnetwork->network.IEs[12],
+			       &wpa_ielen, pnetwork->network.IELength - 12);
+			if (pbuf && (wpa_ielen > 0)) {
+				pdata->flags = 1;
+				break;
+			}
+			pbuf = r8712_get_wpa2_ie(&pnetwork->network.IEs[12],
+			       &wpa_ielen, pnetwork->network.IELength - 12);
+			if (pbuf && (wpa_ielen > 0)) {
+				pdata->flags = 2;
+				break;
+			}
+		}
+		plist = plist->next;
+	}
+	spin_unlock_irqrestore(&pmlmepriv->scanned_queue.lock, irqL);
+	if (pdata->length >= 34) {
+		if (copy_to_user((u8 __user *)pdata->pointer + 32,
+		    (u8 *)&pdata->flags, 1))
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static int r871x_set_pid(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *pdata = &wrqu->data;
+
+	if (padapter->driver_stopped || !pdata)
+		return -EINVAL;
+	if (copy_from_user(&padapter->pid, pdata->pointer, sizeof(int)))
+		return -EINVAL;
+	return 0;
+}
+
+static int r871x_set_chplan(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *pdata = &wrqu->data;
+	int ch_plan = -1;
+
+	if (padapter->driver_stopped || !pdata) {
+		ret = -EINVAL;
+		goto exit;
+	}
+	ch_plan = (int)*extra;
+	r8712_set_chplan_cmd(padapter, ch_plan);
+
+exit:
+
+	return ret;
+}
+
+static int r871x_wps_start(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu, char *extra)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_point *pdata = &wrqu->data;
+	u32   u32wps_start = 0;
+
+	if (padapter->driver_stopped || !pdata)
+		return -EINVAL;
+	if (copy_from_user((void *)&u32wps_start, pdata->pointer, 4))
+		return -EFAULT;
+	if (u32wps_start == 0)
+		u32wps_start = *extra;
+	if (u32wps_start == 1) /* WPS Start */
+		padapter->ledpriv.LedControlHandler(padapter,
+			   LED_CTL_START_WPS);
+	else if (u32wps_start == 2) /* WPS Stop because of wps success */
+		padapter->ledpriv.LedControlHandler(padapter,
+			   LED_CTL_STOP_WPS);
+	else if (u32wps_start == 3) /* WPS Stop because of wps fail */
+		padapter->ledpriv.LedControlHandler(padapter,
+			   LED_CTL_STOP_WPS_FAIL);
+	return 0;
+}
+
+static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	switch (name) {
+	case IEEE_PARAM_WPA_ENABLED:
+		padapter->securitypriv.AuthAlgrthm = 2; /* 802.1x */
+		switch ((value) & 0xff) {
+		case 1: /* WPA */
+			padapter->securitypriv.ndisauthtype =
+				Ndis802_11AuthModeWPAPSK; /* WPA_PSK */
+			padapter->securitypriv.ndisencryptstatus =
+				Ndis802_11Encryption2Enabled;
+			break;
+		case 2: /* WPA2 */
+			padapter->securitypriv.ndisauthtype =
+				Ndis802_11AuthModeWPA2PSK; /* WPA2_PSK */
+			padapter->securitypriv.ndisencryptstatus =
+				Ndis802_11Encryption3Enabled;
+			break;
+		}
+		break;
+	case IEEE_PARAM_TKIP_COUNTERMEASURES:
+		break;
+	case IEEE_PARAM_DROP_UNENCRYPTED:
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+		break;
+	case IEEE_PARAM_PRIVACY_INVOKED:
+		break;
+	case IEEE_PARAM_AUTH_ALGS:
+		return wpa_set_auth_algs(dev, value);
+	case IEEE_PARAM_IEEE_802_1X:
+		break;
+	case IEEE_PARAM_WPAX_SELECT:
+		/* added for WPA2 mixed mode */
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static int wpa_mlme(struct net_device *dev, u32 command, u32 reason)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+
+	switch (command) {
+	case IEEE_MLME_STA_DEAUTH:
+		if (!r8712_set_802_11_disassociate(padapter))
+			return -1;
+		break;
+	case IEEE_MLME_STA_DISASSOC:
+		if (!r8712_set_802_11_disassociate(padapter))
+			return -1;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
+{
+	struct ieee_param *param;
+	int ret = 0;
+	struct _adapter *padapter = netdev_priv(dev);
+
+	if (p->length < sizeof(struct ieee_param) || !p->pointer)
+		return -EINVAL;
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param))
+		return PTR_ERR(param);
+	switch (param->cmd) {
+	case IEEE_CMD_SET_WPA_PARAM:
+		ret = wpa_set_param(dev, param->u.wpa_param.name,
+		      param->u.wpa_param.value);
+		break;
+	case IEEE_CMD_SET_WPA_IE:
+		ret =  r871x_set_wpa_ie(padapter, (char *)param->u.wpa_ie.data,
+		       (u16)param->u.wpa_ie.len);
+		break;
+	case IEEE_CMD_SET_ENCRYPTION:
+		ret = wpa_set_encryption(dev, param, p->length);
+		break;
+	case IEEE_CMD_MLME:
+		ret = wpa_mlme(dev, param->u.mlme.command,
+		      param->u.mlme.reason_code);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+	kfree(param);
+	return ret;
+}
+
+/* based on "driver_ipw" and for hostapd */
+int r871x_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	struct iwreq *wrq = (struct iwreq *)rq;
+
+	switch (cmd) {
+	case RTL_IOCTL_WPA_SUPPLICANT:
+		return wpa_supplicant_ioctl(dev, &wrq->u.data);
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static iw_handler r8711_handlers[] = {
+	NULL,				/* SIOCSIWCOMMIT */
+	r8711_wx_get_name,		/* SIOCGIWNAME */
+	dummy,				/* SIOCSIWNWID */
+	dummy,				/* SIOCGIWNWID */
+	r8711_wx_set_freq,		/* SIOCSIWFREQ */
+	r8711_wx_get_freq,		/* SIOCGIWFREQ */
+	r8711_wx_set_mode,		/* SIOCSIWMODE */
+	r8711_wx_get_mode,		/* SIOCGIWMODE */
+	dummy,				/* SIOCSIWSENS */
+	r8711_wx_get_sens,		/* SIOCGIWSENS */
+	NULL,				/* SIOCSIWRANGE */
+	r8711_wx_get_range,		/* SIOCGIWRANGE */
+	r871x_wx_set_priv,		/* SIOCSIWPRIV */
+	NULL,				/* SIOCGIWPRIV */
+	NULL,				/* SIOCSIWSTATS */
+	NULL,				/* SIOCGIWSTATS */
+	dummy,				/* SIOCSIWSPY */
+	dummy,				/* SIOCGIWSPY */
+	NULL,				/* SIOCGIWTHRSPY */
+	NULL,				/* SIOCWIWTHRSPY */
+	r8711_wx_set_wap,		/* SIOCSIWAP */
+	r8711_wx_get_wap,		/* SIOCGIWAP */
+	r871x_wx_set_mlme,		/* request MLME operation;
+					 *  uses struct iw_mlme
+					 */
+	dummy,				/* SIOCGIWAPLIST -- deprecated */
+	r8711_wx_set_scan,		/* SIOCSIWSCAN */
+	r8711_wx_get_scan,		/* SIOCGIWSCAN */
+	r8711_wx_set_essid,		/* SIOCSIWESSID */
+	r8711_wx_get_essid,		/* SIOCGIWESSID */
+	dummy,				/* SIOCSIWNICKN */
+	r871x_wx_get_nick,		/* SIOCGIWNICKN */
+	NULL,				/* -- hole -- */
+	NULL,				/* -- hole -- */
+	r8711_wx_set_rate,		/* SIOCSIWRATE */
+	r8711_wx_get_rate,		/* SIOCGIWRATE */
+	dummy,				/* SIOCSIWRTS */
+	r8711_wx_get_rts,		/* SIOCGIWRTS */
+	r8711_wx_set_frag,		/* SIOCSIWFRAG */
+	r8711_wx_get_frag,		/* SIOCGIWFRAG */
+	dummy,				/* SIOCSIWTXPOW */
+	dummy,				/* SIOCGIWTXPOW */
+	dummy,				/* SIOCSIWRETRY */
+	r8711_wx_get_retry,		/* SIOCGIWRETRY */
+	r8711_wx_set_enc,		/* SIOCSIWENCODE */
+	r8711_wx_get_enc,		/* SIOCGIWENCODE */
+	dummy,				/* SIOCSIWPOWER */
+	r8711_wx_get_power,		/* SIOCGIWPOWER */
+	NULL,				/*---hole---*/
+	NULL,				/*---hole---*/
+	r871x_wx_set_gen_ie,		/* SIOCSIWGENIE */
+	NULL,				/* SIOCGIWGENIE */
+	r871x_wx_set_auth,		/* SIOCSIWAUTH */
+	NULL,				/* SIOCGIWAUTH */
+	r871x_wx_set_enc_ext,		/* SIOCSIWENCODEEXT */
+	NULL,				/* SIOCGIWENCODEEXT */
+	r871x_wx_set_pmkid,		/* SIOCSIWPMKSA */
+	NULL,				/*---hole---*/
+};
+
+static const struct iw_priv_args r8711_private_args[] = {
+	{
+		SIOCIWFIRSTPRIV + 0x0,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "read32"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x1,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "write32"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x2, 0, 0, "driver_ext"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x3, 0, 0, "mp_ioctl"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x4,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "apinfo"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x5,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setpid"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x6,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wps_start"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x7,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "chplan"
+	}
+};
+
+static iw_handler r8711_private_handler[] = {
+	r8711_wx_read32,
+	r8711_wx_write32,
+	r8711_drvext_hdl,
+	r871x_mp_ioctl_hdl,
+	r871x_get_ap_info, /*for MM DTV platform*/
+	r871x_set_pid,
+	r871x_wps_start,
+	r871x_set_chplan
+};
+
+static struct iw_statistics *r871x_get_wireless_stats(struct net_device *dev)
+{
+	struct _adapter *padapter = netdev_priv(dev);
+	struct iw_statistics *piwstats = &padapter->iwstats;
+	int tmp_level = 0;
+	int tmp_qual = 0;
+	int tmp_noise = 0;
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) != true) {
+		piwstats->qual.qual = 0;
+		piwstats->qual.level = 0;
+		piwstats->qual.noise = 0;
+	} else {
+		/* show percentage, we need transfer dbm to original value. */
+		tmp_level = padapter->recvpriv.fw_rssi;
+		tmp_qual = padapter->recvpriv.signal;
+		tmp_noise = padapter->recvpriv.noise;
+		piwstats->qual.level = tmp_level;
+		piwstats->qual.qual = tmp_qual;
+		piwstats->qual.noise = tmp_noise;
+	}
+	piwstats->qual.updated = IW_QUAL_ALL_UPDATED;
+	return &padapter->iwstats;
+}
+
+struct iw_handler_def r871x_handlers_def = {
+	.standard = r8711_handlers,
+	.num_standard = ARRAY_SIZE(r8711_handlers),
+	.private = r8711_private_handler,
+	.private_args = (struct iw_priv_args *)r8711_private_args,
+	.num_private = ARRAY_SIZE(r8711_private_handler),
+	.num_private_args = sizeof(r8711_private_args) /
+			    sizeof(struct iw_priv_args),
+	.get_wireless_stats = r871x_get_wireless_stats
+};
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c
new file mode 100644
index 000000000..2b5393352
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c
@@ -0,0 +1,519 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_ioctl_rtl.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define  _RTL871X_IOCTL_RTL_C_
+
+#include <linux/rndis.h>
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wlan_bssdef.h"
+#include "wifi.h"
+#include "rtl871x_ioctl.h"
+#include "rtl871x_ioctl_set.h"
+#include "rtl871x_ioctl_rtl.h"
+#include "mp_custom_oid.h"
+#include "rtl871x_mp.h"
+#include "rtl871x_mp_ioctl.h"
+
+uint oid_rt_get_signal_quality_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_small_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >=  sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+				padapter->recvpriv.rx_smallpacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_middle_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >=  sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+				padapter->recvpriv.rx_middlepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_large_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >=  sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+				 padapter->recvpriv.rx_largepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_tx_retry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_rx_retry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_rx_total_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >=  sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+					 padapter->recvpriv.rx_pkts +
+					 padapter->recvpriv.rx_drop;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_tx_beacon_ok_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_tx_beacon_err_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_rx_icv_err_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		*(uint *)poid_par_priv->information_buf =
+					 padapter->recvpriv.rx_icv_err;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_encryption_algorithm_hdl(struct oid_par_priv
+						*poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_preamble_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	u32 preamblemode = 0;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		if (padapter->registrypriv.preamble == PREAMBLE_LONG)
+			preamblemode = 0;
+		else if (padapter->registrypriv.preamble == PREAMBLE_AUTO)
+			preamblemode = 1;
+		else if (padapter->registrypriv.preamble == PREAMBLE_SHORT)
+			preamblemode = 2;
+		*(u32 *)poid_par_priv->information_buf = preamblemode;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_ap_ip_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_channelplan_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	struct eeprom_priv *peeprompriv = &padapter->eeprompriv;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	*(u16 *)poid_par_priv->information_buf = peeprompriv->channel_plan;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_channelplan_hdl(struct oid_par_priv
+				       *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	struct eeprom_priv *peeprompriv = &padapter->eeprompriv;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	peeprompriv->channel_plan = *(u16 *)poid_par_priv->information_buf;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_preamble_mode_hdl(struct oid_par_priv
+					 *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	u32 preamblemode = 0;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		preamblemode = *(u32 *)poid_par_priv->information_buf;
+		if (preamblemode == 0)
+			padapter->registrypriv.preamble = PREAMBLE_LONG;
+		else if (preamblemode == 1)
+			padapter->registrypriv.preamble = PREAMBLE_AUTO;
+		else if (preamblemode == 2)
+			padapter->registrypriv.preamble = PREAMBLE_SHORT;
+		*(u32 *)poid_par_priv->information_buf = preamblemode;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_bcn_intvl_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_dedicate_probe_hdl(struct oid_par_priv
+				      *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_total_tx_bytes_hdl(struct oid_par_priv
+					  *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+						 padapter->xmitpriv.tx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_total_rx_bytes_hdl(struct oid_par_priv
+					  *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+					   padapter->recvpriv.rx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_current_tx_power_level_hdl(struct oid_par_priv
+					      *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_enc_key_mismatch_count_hdl(struct oid_par_priv
+						  *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_enc_key_match_count_hdl(struct oid_par_priv
+					       *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_channel_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct NDIS_802_11_CONFIGURATION *pnic_Config;
+	u32   channelnum;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+		pnic_Config = &pmlmepriv->cur_network.network.Configuration;
+	else
+		pnic_Config = &padapter->registrypriv.dev_network.Configuration;
+	channelnum = pnic_Config->DSConfig;
+	*(u32 *)poid_par_priv->information_buf = channelnum;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_hardware_radio_off_hdl(struct oid_par_priv
+			 *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_key_mismatch_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_supported_wireless_mode_hdl(struct oid_par_priv
+					       *poid_par_priv)
+{
+	u32 ulInfo = 0;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		ulInfo |= 0x0100; /* WIRELESS_MODE_B */
+		ulInfo |= 0x0200; /* WIRELESS_MODE_G */
+		ulInfo |= 0x0400; /* WIRELESS_MODE_A */
+		*(u32 *) poid_par_priv->information_buf = ulInfo;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_channel_list_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_scan_in_progress_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_forced_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_wireless_mode_for_scan_list_hdl(struct oid_par_priv
+						   *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_bss_wireless_mode_hdl(struct oid_par_priv
+					     *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_scan_with_magic_packet_hdl(struct oid_par_priv
+					      *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_ap_get_associated_station_list_hdl(struct oid_par_priv
+						      *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_ap_switch_into_ap_mode_hdl(struct oid_par_priv*
+					      poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_ap_supported_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_ap_set_passphrase_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_rf_write_registry_hdl(struct oid_par_priv*
+					     poid_par_priv)
+{
+	uint status = RNDIS_STATUS_SUCCESS;
+	struct _adapter *Adapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != SET_OID) /* QUERY_OID */
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len ==
+	   (sizeof(unsigned long) * 3)) {
+		if (r8712_setrfreg_cmd(Adapter,
+			*(unsigned char *)poid_par_priv->information_buf,
+			(unsigned long)(*((unsigned long *)
+					poid_par_priv->information_buf + 2))))
+			status = RNDIS_STATUS_NOT_ACCEPTED;
+	} else {
+		status = RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return status;
+}
+
+uint oid_rt_pro_rf_read_registry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	uint status = RNDIS_STATUS_SUCCESS;
+	struct _adapter *Adapter = poid_par_priv->adapter_context;
+
+	if (poid_par_priv->type_of_oid != SET_OID) /* QUERY_OID */
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len == (sizeof(unsigned long) *
+						   3)) {
+		if (Adapter->mppriv.act_in_progress) {
+			status = RNDIS_STATUS_NOT_ACCEPTED;
+		} else {
+			/* init workparam */
+			Adapter->mppriv.act_in_progress = true;
+			Adapter->mppriv.workparam.bcompleted = false;
+			Adapter->mppriv.workparam.act_type = MPT_READ_RF;
+			Adapter->mppriv.workparam.io_offset = *(unsigned long *)
+						poid_par_priv->information_buf;
+			Adapter->mppriv.workparam.io_value = 0xcccccccc;
+
+		/* RegOffsetValue	- The offset of RF register to read.
+		 * RegDataWidth	- The data width of RF register to read.
+		 * RegDataValue	- The value to read.
+		 * RegOffsetValue = *((unsigned long *)InformationBuffer);
+		 * RegDataWidth = *((unsigned long *)InformationBuffer+1);
+		 * RegDataValue =  *((unsigned long *)InformationBuffer+2);
+		 */
+			if (r8712_getrfreg_cmd(Adapter,
+			    *(unsigned char *)poid_par_priv->information_buf,
+			    (unsigned char *)&Adapter->mppriv.workparam.io_value
+			    ))
+				status = RNDIS_STATUS_NOT_ACCEPTED;
+		}
+	} else {
+		status = RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return status;
+}
+
+enum _CONNECT_STATE_ {
+	CHECKINGSTATUS,
+	ASSOCIATED,
+	ADHOCMODE,
+	NOTASSOCIATED
+};
+
+uint oid_rt_get_connect_state_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *padapter = poid_par_priv->adapter_context;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u32 ulInfo;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	/* nStatus==0	CheckingStatus
+	 * nStatus==1	Associated
+	 * nStatus==2	AdHocMode
+	 * nStatus==3	NotAssociated
+	 */
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		ulInfo = CHECKINGSTATUS;
+	else if (check_fwstate(pmlmepriv, _FW_LINKED))
+		ulInfo = ASSOCIATED;
+	else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
+		ulInfo = ADHOCMODE;
+	else
+		ulInfo = NOTASSOCIATED;
+	*(u32 *)poid_par_priv->information_buf = ulInfo;
+	*poid_par_priv->bytes_rw =  poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_default_key_id_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_rtl.h b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.h
new file mode 100644
index 000000000..7c0b880ac
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.h
@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_IOCTL_RTL_H
+#define _RTL871X_IOCTL_RTL_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+/*************** oid_rtl_seg_01_01 **************/
+uint oid_rt_get_signal_quality_hdl(
+			struct oid_par_priv *poid_par_priv);/*84*/
+uint oid_rt_get_small_packet_crc_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_middle_packet_crc_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_large_packet_crc_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_tx_retry_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_rx_retry_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_rx_total_packet_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_tx_beacon_ok_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_tx_beacon_err_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_rx_icv_err_hdl(
+			struct oid_par_priv *poid_par_priv);/*93*/
+uint oid_rt_set_encryption_algorithm_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_preamble_mode_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_ap_ip_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_channelplan_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_channelplan_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_preamble_mode_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_bcn_intvl_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_dedicate_probe_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_total_tx_bytes_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_total_rx_bytes_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_current_tx_power_level_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_enc_key_mismatch_count_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_enc_key_match_count_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_channel_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_hardware_radio_off_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_key_mismatch_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_supported_wireless_mode_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_channel_list_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_scan_in_progress_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_forced_data_rate_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_wireless_mode_for_scan_list_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_bss_wireless_mode_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_scan_with_magic_packet_hdl(
+			struct oid_par_priv *poid_par_priv);
+
+/**************  oid_rtl_seg_01_03 section start **************/
+uint oid_rt_ap_get_associated_station_list_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_ap_switch_into_ap_mode_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_ap_supported_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_ap_set_passphrase_hdl(
+			struct oid_par_priv *poid_par_priv);
+/* oid_rtl_seg_01_11 */
+uint oid_rt_pro_rf_write_registry_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_rf_read_registry_hdl(
+			struct oid_par_priv *poid_par_priv);
+/***************  oid_rtl_seg_03_00 section start **************/
+uint oid_rt_get_connect_state_hdl(
+			struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_default_key_id_hdl(
+			struct oid_par_priv *poid_par_priv);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.c b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
new file mode 100644
index 000000000..34c9a52b4
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
@@ -0,0 +1,354 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_ioctl_set.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_IOCTL_SET_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "rtl871x_ioctl_set.h"
+#include "usb_osintf.h"
+#include "usb_ops.h"
+
+static u8 validate_ssid(struct ndis_802_11_ssid *ssid)
+{
+	u8 i;
+
+	if (ssid->SsidLength > 32)
+		return false;
+	for (i = 0; i < ssid->SsidLength; i++) {
+		/* wifi, printable ascii code must be supported */
+		if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e)))
+			return false;
+	}
+	return true;
+}
+
+static u8 do_join(struct _adapter *padapter)
+{
+	struct list_head *plist, *phead;
+	u8 *pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct  __queue	*queue	= &(pmlmepriv->scanned_queue);
+	int ret;
+
+	phead = &queue->queue;
+	plist = phead->next;
+	pmlmepriv->cur_network.join_res = -2;
+	pmlmepriv->fw_state |= _FW_UNDER_LINKING;
+	pmlmepriv->pscanned = plist;
+	pmlmepriv->to_join = true;
+
+	/* adhoc mode will start with an empty queue, but skip checking */
+	if (!check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) &&
+	    list_empty(&queue->queue)) {
+		if (pmlmepriv->fw_state & _FW_UNDER_LINKING)
+			pmlmepriv->fw_state ^= _FW_UNDER_LINKING;
+		/* when set_ssid/set_bssid for do_join(), but scanning queue
+		 * is empty we try to issue sitesurvey firstly
+		 */
+		if (!pmlmepriv->sitesurveyctrl.traffic_busy)
+			r8712_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid);
+		return true;
+	}
+
+	ret = r8712_select_and_join_from_scan(pmlmepriv);
+	if (!ret) {
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
+	} else {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			/* submit r8712_createbss_cmd to change to an
+			 * ADHOC_MASTER pmlmepriv->lock has been
+			 * acquired by caller...
+			 */
+			struct wlan_bssid_ex *pdev_network =
+				&padapter->registrypriv.dev_network;
+			pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+			pibss = padapter->registrypriv.dev_network.MacAddress;
+			memcpy(&pdev_network->Ssid,
+			       &pmlmepriv->assoc_ssid,
+			       sizeof(struct ndis_802_11_ssid));
+			r8712_update_registrypriv_dev_network(padapter);
+			r8712_generate_random_ibss(pibss);
+			if (r8712_createbss_cmd(padapter))
+				return false;
+			pmlmepriv->to_join = false;
+		} else {
+			/* can't associate ; reset under-linking */
+			if (pmlmepriv->fw_state & _FW_UNDER_LINKING)
+				pmlmepriv->fw_state ^=
+					_FW_UNDER_LINKING;
+			/* when set_ssid/set_bssid for do_join(), but
+			 * there are no desired bss in scanning queue
+			 * we try to issue sitesurvey first
+			 */
+			if (!pmlmepriv->sitesurveyctrl.traffic_busy)
+				r8712_sitesurvey_cmd(padapter,
+						     &pmlmepriv->assoc_ssid);
+		}
+	}
+	return true;
+}
+
+u8 r8712_set_802_11_bssid(struct _adapter *padapter, u8 *bssid)
+{
+	unsigned long irqL;
+	u8 status = true;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (is_zero_ether_addr(bssid) || is_broadcast_ether_addr(bssid)) {
+		status = false;
+		return status;
+	}
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY |
+	    _FW_UNDER_LINKING)) {
+		status = check_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+		goto _Abort_Set_BSSID;
+	}
+	if (check_fwstate(pmlmepriv,
+	    _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid,
+		    ETH_ALEN)) {
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+				/* driver is in
+				 * WIFI_ADHOC_MASTER_STATE
+				 */
+				goto _Abort_Set_BSSID;
+		} else {
+			r8712_disassoc_cmd(padapter);
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				r8712_ind_disconnect(padapter);
+			r8712_free_assoc_resources(padapter);
+			if ((check_fwstate(pmlmepriv,
+			     WIFI_ADHOC_MASTER_STATE))) {
+				_clr_fwstate_(pmlmepriv,
+					      WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+	memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid = true;
+	status = do_join(padapter);
+	goto done;
+_Abort_Set_BSSID:
+done:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	return status;
+}
+
+void r8712_set_802_11_ssid(struct _adapter *padapter,
+			   struct ndis_802_11_ssid *ssid)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+	if (!padapter->hw_init_completed)
+		return;
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING)) {
+		check_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+		goto _Abort_Set_SSID;
+	}
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid,
+		    ssid->SsidLength))) {
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				if (!r8712_is_same_ibss(padapter,
+				     pnetwork)) {
+					/* if in WIFI_ADHOC_MASTER_STATE or
+					 *  WIFI_ADHOC_STATE, create bss or
+					 * rejoin again
+					 */
+					r8712_disassoc_cmd(padapter);
+					if (check_fwstate(pmlmepriv,
+					    _FW_LINKED))
+						r8712_ind_disconnect(padapter);
+					r8712_free_assoc_resources(padapter);
+					if (check_fwstate(pmlmepriv,
+					     WIFI_ADHOC_MASTER_STATE)) {
+						_clr_fwstate_(pmlmepriv,
+						    WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv,
+							    WIFI_ADHOC_STATE);
+					}
+				} else {
+					/* driver is in
+					 * WIFI_ADHOC_MASTER_STATE
+					 */
+					goto _Abort_Set_SSID;
+				}
+			}
+		} else {
+			r8712_disassoc_cmd(padapter);
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				r8712_ind_disconnect(padapter);
+			r8712_free_assoc_resources(padapter);
+			if (check_fwstate(pmlmepriv,
+			    WIFI_ADHOC_MASTER_STATE)) {
+				_clr_fwstate_(pmlmepriv,
+					      WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+	if (padapter->securitypriv.btkip_countermeasure)
+		goto _Abort_Set_SSID;
+	if (!validate_ssid(ssid))
+		goto _Abort_Set_SSID;
+	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
+	pmlmepriv->assoc_by_bssid = false;
+	do_join(padapter);
+	goto done;
+_Abort_Set_SSID:
+done:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+void r8712_set_802_11_infrastructure_mode(struct _adapter *padapter,
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	unsigned long irqL;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network	*cur_network = &pmlmepriv->cur_network;
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE *pold_state =
+				&(cur_network->network.InfrastructureMode);
+
+	if (*pold_state != networktype) {
+		spin_lock_irqsave(&pmlmepriv->lock, irqL);
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    (*pold_state == Ndis802_11IBSS))
+			r8712_disassoc_cmd(padapter);
+		if (check_fwstate(pmlmepriv,
+		    _FW_LINKED | WIFI_ADHOC_MASTER_STATE))
+			r8712_free_assoc_resources(padapter);
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    (*pold_state == Ndis802_11Infrastructure) ||
+		    (*pold_state == Ndis802_11IBSS)) {
+			/* will clr Linked_state before this function,
+			 * we must have checked whether issue dis-assoc_cmd or
+			 * not
+			 */
+			r8712_ind_disconnect(padapter);
+		}
+		*pold_state = networktype;
+		/* clear WIFI_STATION_STATE; WIFI_AP_STATE; WIFI_ADHOC_STATE;
+		 * WIFI_ADHOC_MASTER_STATE
+		 */
+		_clr_fwstate_(pmlmepriv, WIFI_STATION_STATE | WIFI_AP_STATE |
+			      WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE);
+		switch (networktype) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+		case Ndis802_11APMode:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+			break;
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+		spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	}
+}
+
+u8 r8712_set_802_11_disassociate(struct _adapter *padapter)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		r8712_disassoc_cmd(padapter);
+		r8712_ind_disconnect(padapter);
+		r8712_free_assoc_resources(padapter);
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	return true;
+}
+
+u8 r8712_set_802_11_bssid_list_scan(struct _adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = NULL;
+	unsigned long irqL;
+	u8 ret = true;
+
+	if (!padapter)
+		return false;
+	pmlmepriv = &padapter->mlmepriv;
+	if (!padapter->hw_init_completed)
+		return false;
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING) ||
+	    pmlmepriv->sitesurveyctrl.traffic_busy) {
+		/* Scan or linking is in progress, do nothing. */
+		ret = (u8)check_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+	} else {
+		r8712_free_network_queue(padapter);
+		ret = r8712_sitesurvey_cmd(padapter, NULL);
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	return ret;
+}
+
+u8 r8712_set_802_11_authentication_mode(struct _adapter *padapter,
+				enum NDIS_802_11_AUTHENTICATION_MODE authmode)
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8 ret;
+
+	psecuritypriv->ndisauthtype = authmode;
+	if (psecuritypriv->ndisauthtype > 3)
+		psecuritypriv->AuthAlgrthm = 2; /* 802.1x */
+	if (r8712_set_auth(padapter, psecuritypriv))
+		ret = false;
+	else
+		ret = true;
+	return ret;
+}
+
+int r8712_set_802_11_add_wep(struct _adapter *padapter,
+			     struct NDIS_802_11_WEP *wep)
+{
+	sint	keyid;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	keyid = wep->KeyIndex & 0x3fffffff;
+	if (keyid >= WEP_KEYS)
+		return -EINVAL;
+	switch (wep->KeyLength) {
+	case 5:
+		psecuritypriv->PrivacyAlgrthm = _WEP40_;
+		break;
+	case 13:
+		psecuritypriv->PrivacyAlgrthm = _WEP104_;
+		break;
+	default:
+		psecuritypriv->PrivacyAlgrthm = _NO_PRIVACY_;
+		break;
+	}
+	memcpy(psecuritypriv->DefKey[keyid].skey, &wep->KeyMaterial,
+		wep->KeyLength);
+	psecuritypriv->DefKeylen[keyid] = wep->KeyLength;
+	psecuritypriv->PrivacyKeyIndex = keyid;
+	return r8712_set_key(padapter, psecuritypriv, keyid);
+}
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.h b/drivers/staging/rtl8712/rtl871x_ioctl_set.h
new file mode 100644
index 000000000..e2de820f6
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __IOCTL_SET_H
+#define __IOCTL_SET_H
+
+#include "drv_types.h"
+
+typedef u8 NDIS_802_11_PMKID_VALUE[16];
+
+struct BSSIDInfo {
+	unsigned char BSSID[6];
+	NDIS_802_11_PMKID_VALUE PMKID;
+};
+
+u8 r8712_set_802_11_authentication_mode(struct _adapter *pdapter,
+			enum NDIS_802_11_AUTHENTICATION_MODE authmode);
+
+u8 r8712_set_802_11_bssid(struct _adapter *padapter, u8 *bssid);
+
+int r8712_set_802_11_add_wep(struct _adapter *padapter,
+			     struct NDIS_802_11_WEP *wep);
+
+u8 r8712_set_802_11_disassociate(struct _adapter *padapter);
+
+u8 r8712_set_802_11_bssid_list_scan(struct _adapter *padapter);
+
+void r8712_set_802_11_infrastructure_mode(struct _adapter *padapter,
+			enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype);
+
+void r8712_set_802_11_ssid(struct _adapter *padapter,
+			   struct ndis_802_11_ssid *ssid);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_led.h b/drivers/staging/rtl8712/rtl871x_led.h
new file mode 100644
index 000000000..2f0768132
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_led.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL8712_LED_H
+#define __RTL8712_LED_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+/*===========================================================================
+ * LED customization.
+ *===========================================================================
+ */
+enum LED_CTL_MODE {
+	LED_CTL_POWER_ON = 1,
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7,
+	LED_CTL_START_TO_LINK = 8,
+	LED_CTL_START_WPS = 9,
+	LED_CTL_STOP_WPS = 10,
+	LED_CTL_START_WPS_BOTTON = 11,
+	LED_CTL_STOP_WPS_FAIL = 12,
+	LED_CTL_STOP_WPS_FAIL_OVERLAP = 13,
+};
+
+#define IS_LED_WPS_BLINKING(_LED_871x)	\
+	(((struct LED_871x *)_LED_871x)->CurrLedState == LED_BLINK_WPS \
+	|| ((struct LED_871x *)_LED_871x)->CurrLedState == LED_BLINK_WPS_STOP \
+	|| ((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress)
+
+#define IS_LED_BLINKING(_LED_871x)	\
+		(((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress \
+		|| ((struct LED_871x *)_LED_871x)->bLedScanBlinkInProgress)
+
+enum LED_PIN_871x {
+	LED_PIN_GPIO0,
+	LED_PIN_LED0,
+	LED_PIN_LED1
+};
+
+/*===========================================================================
+ * LED customization.
+ *===========================================================================
+ */
+enum LED_STRATEGY_871x {
+	SW_LED_MODE0, /* SW control 1 LED via GPIO0. It is default option. */
+	SW_LED_MODE1, /* 2 LEDs, through LED0 and LED1. For ALPHA. */
+	SW_LED_MODE2, /* SW control 1 LED via GPIO0,
+		       * custom for AzWave 8187 minicard.
+		       */
+	SW_LED_MODE3, /* SW control 1 LED via GPIO0,
+		       * customized for Sercomm Printer Server case.
+		       */
+	SW_LED_MODE4, /*for Edimax / Belkin*/
+	SW_LED_MODE5, /*for Sercomm / Belkin*/
+	SW_LED_MODE6, /*for WNC / Corega*/
+	HW_LED, /* HW control 2 LEDs, LED0 and LED1 (there are 4 different
+		 * control modes, see MAC.CONFIG1 for details.)
+		 */
+};
+
+struct LED_871x {
+	struct _adapter		*padapter;
+	enum LED_PIN_871x	LedPin;	/* Implementation for this SW led. */
+	u32			CurrLedState; /* Current LED state. */
+	u8			bLedOn; /* true if LED is ON */
+	u8			bSWLedCtrl;
+	u8			bLedBlinkInProgress; /*true if blinking */
+	u8			bLedNoLinkBlinkInProgress;
+	u8			bLedLinkBlinkInProgress;
+	u8			bLedStartToLinkBlinkInProgress;
+	u8			bLedScanBlinkInProgress;
+	u8			bLedWPSBlinkInProgress;
+	u32			BlinkTimes; /* No. times to toggle for blink.*/
+	u32			BlinkingLedState; /* Next state for blinking,
+						   * either LED_ON or OFF.
+						   */
+
+	struct timer_list	BlinkTimer; /* Timer object for led blinking.*/
+	struct work_struct	BlinkWorkItem; /* Workitem used by BlinkTimer */
+};
+
+struct led_priv {
+	/* add for led control */
+	struct LED_871x		SwLed0;
+	struct LED_871x		SwLed1;
+	enum LED_STRATEGY_871x	LedStrategy;
+	u8			bRegUseLed;
+	void (*LedControlHandler)(struct _adapter *padapter,
+				  enum LED_CTL_MODE LedAction);
+	/* add for led control */
+};
+
+/*===========================================================================
+ * Interface to manipulate LED objects.
+ *===========================================================================
+ */
+void r8712_InitSwLeds(struct _adapter *padapter);
+void r8712_DeInitSwLeds(struct _adapter *padapter);
+void LedControl871x(struct _adapter *padapter, enum LED_CTL_MODE LedAction);
+void r8712_flush_led_works(struct _adapter *padapter);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
new file mode 100644
index 000000000..63e12b157
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -0,0 +1,1709 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_mlme.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_MLME_C_
+
+#include <linux/etherdevice.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "xmit_osdep.h"
+#include "mlme_osdep.h"
+#include "sta_info.h"
+#include "wifi.h"
+#include "wlan_bssdef.h"
+
+static void update_ht_cap(struct _adapter *padapter, u8 *pie, uint ie_len);
+
+int r8712_init_mlme_priv(struct _adapter *padapter)
+{
+	sint	i;
+	u8	*pbuf;
+	struct wlan_network	*pnetwork;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv));
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+	pmlmepriv->pscanned = NULL;
+	pmlmepriv->fw_state = 0;
+	pmlmepriv->cur_network.network.InfrastructureMode =
+				 Ndis802_11AutoUnknown;
+	/* Maybe someday we should rename this variable to "active_mode"(Jeff)*/
+	pmlmepriv->passive_mode = 1; /* 1: active, 0: passive. */
+	spin_lock_init(&(pmlmepriv->lock));
+	spin_lock_init(&(pmlmepriv->lock2));
+	_init_queue(&(pmlmepriv->free_bss_pool));
+	_init_queue(&(pmlmepriv->scanned_queue));
+	set_scanned_network_val(pmlmepriv, 0);
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+	pbuf = kmalloc_array(MAX_BSS_CNT, sizeof(struct wlan_network),
+			     GFP_ATOMIC);
+	if (!pbuf)
+		return -ENOMEM;
+	pmlmepriv->free_bss_buf = pbuf;
+	pnetwork = (struct wlan_network *)pbuf;
+	for (i = 0; i < MAX_BSS_CNT; i++) {
+		INIT_LIST_HEAD(&(pnetwork->list));
+		list_add_tail(&(pnetwork->list),
+				 &(pmlmepriv->free_bss_pool.queue));
+		pnetwork++;
+	}
+	pmlmepriv->sitesurveyctrl.last_rx_pkts = 0;
+	pmlmepriv->sitesurveyctrl.last_tx_pkts = 0;
+	pmlmepriv->sitesurveyctrl.traffic_busy = false;
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	r8712_init_mlme_timer(padapter);
+	return 0;
+}
+
+struct wlan_network *_r8712_alloc_network(struct mlme_priv *pmlmepriv)
+{
+	unsigned long irqL;
+	struct wlan_network *pnetwork;
+	struct  __queue *free_queue = &pmlmepriv->free_bss_pool;
+
+	spin_lock_irqsave(&free_queue->lock, irqL);
+	pnetwork = list_first_entry_or_null(&free_queue->queue,
+					    struct wlan_network, list);
+	if (pnetwork) {
+		list_del_init(&pnetwork->list);
+		pnetwork->last_scanned = jiffies;
+		pmlmepriv->num_of_scanned++;
+	}
+	spin_unlock_irqrestore(&free_queue->lock, irqL);
+	return pnetwork;
+}
+
+static void _free_network(struct mlme_priv *pmlmepriv,
+			  struct wlan_network *pnetwork)
+{
+	u32 curr_time, delta_time;
+	unsigned long irqL;
+	struct  __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+	if (!pnetwork)
+		return;
+	if (pnetwork->fixed)
+		return;
+	curr_time = jiffies;
+	delta_time = (curr_time - (u32)pnetwork->last_scanned) / HZ;
+	if (delta_time < SCANQUEUE_LIFETIME)
+		return;
+	spin_lock_irqsave(&free_queue->lock, irqL);
+	list_del_init(&pnetwork->list);
+	list_add_tail(&pnetwork->list, &free_queue->queue);
+	pmlmepriv->num_of_scanned--;
+	spin_unlock_irqrestore(&free_queue->lock, irqL);
+}
+
+static void free_network_nolock(struct mlme_priv *pmlmepriv,
+			  struct wlan_network *pnetwork)
+{
+	struct  __queue *free_queue = &pmlmepriv->free_bss_pool;
+
+	if (!pnetwork)
+		return;
+	if (pnetwork->fixed)
+		return;
+	list_del_init(&pnetwork->list);
+	list_add_tail(&pnetwork->list, &free_queue->queue);
+	pmlmepriv->num_of_scanned--;
+}
+
+/* return the wlan_network with the matching addr
+ * Shall be called under atomic context...
+ * to avoid possible racing condition...
+ */
+static struct wlan_network *r8712_find_network(struct  __queue *scanned_queue,
+					       u8 *addr)
+{
+	unsigned long irqL;
+	struct list_head *phead, *plist;
+	struct wlan_network *pnetwork = NULL;
+
+	if (is_zero_ether_addr(addr))
+		return NULL;
+	spin_lock_irqsave(&scanned_queue->lock, irqL);
+	phead = &scanned_queue->queue;
+	list_for_each(plist, phead) {
+		pnetwork = list_entry(plist, struct wlan_network, list);
+		if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
+			break;
+	}
+	if (plist == phead)
+		pnetwork = NULL;
+	spin_unlock_irqrestore(&scanned_queue->lock, irqL);
+	return pnetwork;
+}
+
+void r8712_free_network_queue(struct _adapter *padapter)
+{
+	unsigned long irqL;
+	struct list_head *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct  __queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+	spin_lock_irqsave(&scanned_queue->lock, irqL);
+	phead = &scanned_queue->queue;
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist)) {
+		pnetwork = container_of(plist, struct wlan_network, list);
+		plist = plist->next;
+		_free_network(pmlmepriv, pnetwork);
+	}
+	spin_unlock_irqrestore(&scanned_queue->lock, irqL);
+}
+
+sint r8712_if_up(struct _adapter *padapter)
+{
+	sint res;
+
+	if (padapter->driver_stopped || padapter->surprise_removed ||
+	    !check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		res = false;
+	} else {
+		res = true;
+	}
+	return res;
+}
+
+void r8712_generate_random_ibss(u8 *pibss)
+{
+	u32 curtime = jiffies;
+
+	pibss[0] = 0x02; /*in ad-hoc mode bit1 must set to 1 */
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+	pibss[3] = (u8)(curtime & 0xff);
+	pibss[4] = (u8)((curtime >> 8) & 0xff);
+	pibss[5] = (u8)((curtime >> 16) & 0xff);
+}
+
+uint r8712_get_wlan_bssid_ex_sz(struct wlan_bssid_ex *bss)
+{
+	return sizeof(*bss) + bss->IELength - MAX_IE_SZ;
+}
+
+u8 *r8712_get_capability_from_ie(u8 *ie)
+{
+	return ie + 8 + 2;
+}
+
+void r8712_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	kfree(pmlmepriv->free_bss_buf);
+}
+
+static struct	wlan_network *alloc_network(struct mlme_priv *pmlmepriv)
+{
+	return _r8712_alloc_network(pmlmepriv);
+}
+
+int r8712_is_same_ibss(struct _adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret = true;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ((psecuritypriv->PrivacyAlgrthm != _NO_PRIVACY_) &&
+		    (pnetwork->network.Privacy == cpu_to_le32(0)))
+		ret = false;
+	else if ((psecuritypriv->PrivacyAlgrthm == _NO_PRIVACY_) &&
+		 (pnetwork->network.Privacy == cpu_to_le32(1)))
+		ret = false;
+	else
+		ret = true;
+	return ret;
+
+}
+
+static int is_same_network(struct wlan_bssid_ex *src,
+			   struct wlan_bssid_ex *dst)
+{
+	u16 s_cap, d_cap;
+
+	memcpy((u8 *)&s_cap, r8712_get_capability_from_ie(src->IEs), 2);
+	memcpy((u8 *)&d_cap, r8712_get_capability_from_ie(dst->IEs), 2);
+	return (src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+			(src->Configuration.DSConfig ==
+			dst->Configuration.DSConfig) &&
+			((!memcmp(src->MacAddress, dst->MacAddress,
+			ETH_ALEN))) &&
+			((!memcmp(src->Ssid.Ssid,
+			  dst->Ssid.Ssid,
+			  src->Ssid.SsidLength))) &&
+			((s_cap & WLAN_CAPABILITY_IBSS) ==
+			(d_cap & WLAN_CAPABILITY_IBSS)) &&
+			((s_cap & WLAN_CAPABILITY_ESS) ==
+			(d_cap & WLAN_CAPABILITY_ESS));
+
+}
+
+struct	wlan_network *r8712_get_oldest_wlan_network(
+				struct  __queue *scanned_queue)
+{
+	struct list_head *plist, *phead;
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+
+	phead = &scanned_queue->queue;
+	plist = phead->next;
+	while (1) {
+		if (end_of_queue_search(phead, plist))
+			break;
+		pwlan = container_of(plist, struct wlan_network, list);
+		if (!pwlan->fixed) {
+			if (!oldest ||
+			    time_after((unsigned long)oldest->last_scanned,
+				       (unsigned long)pwlan->last_scanned))
+				oldest = pwlan;
+		}
+		plist = plist->next;
+	}
+	return oldest;
+}
+
+static void update_network(struct wlan_bssid_ex *dst,
+			   struct wlan_bssid_ex *src,
+			   struct _adapter *padapter)
+{
+	u32 last_evm = 0, tmpVal;
+	struct smooth_rssi_data *sqd = &padapter->recvpriv.signal_qual_data;
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) &&
+	    is_same_network(&(padapter->mlmepriv.cur_network.network), src)) {
+		if (padapter->recvpriv.signal_qual_data.total_num++ >=
+		    PHY_LINKQUALITY_SLID_WIN_MAX) {
+			padapter->recvpriv.signal_qual_data.total_num =
+				   PHY_LINKQUALITY_SLID_WIN_MAX;
+			last_evm = sqd->elements[sqd->index];
+			padapter->recvpriv.signal_qual_data.total_val -=
+				 last_evm;
+		}
+		padapter->recvpriv.signal_qual_data.total_val += src->Rssi;
+
+		sqd->elements[sqd->index++] = src->Rssi;
+		if (padapter->recvpriv.signal_qual_data.index >=
+		    PHY_LINKQUALITY_SLID_WIN_MAX)
+			padapter->recvpriv.signal_qual_data.index = 0;
+		/* <1> Showed on UI for user, in percentage. */
+		tmpVal = padapter->recvpriv.signal_qual_data.total_val /
+			 padapter->recvpriv.signal_qual_data.total_num;
+		padapter->recvpriv.signal = (u8)tmpVal;
+
+		src->Rssi = padapter->recvpriv.signal;
+	} else {
+		src->Rssi = (src->Rssi + dst->Rssi) / 2;
+	}
+	memcpy((u8 *)dst, (u8 *)src, r8712_get_wlan_bssid_ex_sz(src));
+}
+
+static void update_current_network(struct _adapter *adapter,
+				   struct wlan_bssid_ex *pnetwork)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	if (is_same_network(&(pmlmepriv->cur_network.network), pnetwork)) {
+		update_network(&(pmlmepriv->cur_network.network),
+			       pnetwork, adapter);
+		r8712_update_protection(adapter,
+			       (pmlmepriv->cur_network.network.IEs) +
+			       sizeof(struct NDIS_802_11_FIXED_IEs),
+			       pmlmepriv->cur_network.network.IELength);
+	}
+}
+
+/* Caller must hold pmlmepriv->lock first */
+static void update_scanned_network(struct _adapter *adapter,
+			    struct wlan_bssid_ex *target)
+{
+	struct list_head *plist, *phead;
+
+	u32 bssid_ex_sz;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+	struct wlan_network *pnetwork = NULL;
+	struct wlan_network *oldest = NULL;
+
+	phead = &queue->queue;
+	plist = phead->next;
+
+	while (1) {
+		if (end_of_queue_search(phead, plist))
+			break;
+
+		pnetwork = container_of(plist, struct wlan_network, list);
+		if (is_same_network(&pnetwork->network, target))
+			break;
+		if ((oldest == ((struct wlan_network *)0)) ||
+		    time_after((unsigned long)oldest->last_scanned,
+				(unsigned long)pnetwork->last_scanned))
+			oldest = pnetwork;
+
+		plist = plist->next;
+	}
+
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information
+	 */
+	if (end_of_queue_search(phead, plist)) {
+		if (list_empty(&pmlmepriv->free_bss_pool.queue)) {
+			/* If there are no more slots, expire the oldest */
+			pnetwork = oldest;
+			target->Rssi = (pnetwork->network.Rssi +
+					target->Rssi) / 2;
+			memcpy(&pnetwork->network, target,
+				r8712_get_wlan_bssid_ex_sz(target));
+			pnetwork->last_scanned = jiffies;
+		} else {
+			/* Otherwise just pull from the free list */
+			/* update scan_time */
+			pnetwork = alloc_network(pmlmepriv);
+			if (!pnetwork)
+				return;
+			bssid_ex_sz = r8712_get_wlan_bssid_ex_sz(target);
+			target->Length = bssid_ex_sz;
+			memcpy(&pnetwork->network, target, bssid_ex_sz);
+			list_add_tail(&pnetwork->list, &queue->queue);
+		}
+	} else {
+		/* we have an entry and we are going to update it. But
+		 * this entry may be already expired. In this case we
+		 * do the same as we found a new net and call the new_net
+		 * handler
+		 */
+		update_network(&pnetwork->network, target, adapter);
+		pnetwork->last_scanned = jiffies;
+	}
+}
+
+static void rtl8711_add_network(struct _adapter *adapter,
+			 struct wlan_bssid_ex *pnetwork)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &(((struct _adapter *)adapter)->mlmepriv);
+	struct  __queue *queue = &pmlmepriv->scanned_queue;
+
+	spin_lock_irqsave(&queue->lock, irqL);
+	update_current_network(adapter, pnetwork);
+	update_scanned_network(adapter, pnetwork);
+	spin_unlock_irqrestore(&queue->lock, irqL);
+}
+
+/*select the desired network based on the capability of the (i)bss.
+ * check items:		(1) security
+ *			(2) network_type
+ *			(3) WMM
+ *			(4) HT
+ *			(5) others
+ */
+static int is_desired_network(struct _adapter *adapter,
+				struct wlan_network *pnetwork)
+{
+	u8 wps_ie[512];
+	uint wps_ielen;
+	int bselected = true;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if (psecuritypriv->wps_phase) {
+		if (r8712_get_wps_ie(pnetwork->network.IEs,
+		    pnetwork->network.IELength, wps_ie,
+		    &wps_ielen))
+			return true;
+		return false;
+	}
+	if ((psecuritypriv->PrivacyAlgrthm != _NO_PRIVACY_) &&
+		    (pnetwork->network.Privacy == 0))
+		bselected = false;
+	if (check_fwstate(&adapter->mlmepriv, WIFI_ADHOC_STATE)) {
+		if (pnetwork->network.InfrastructureMode !=
+			adapter->mlmepriv.cur_network.network.InfrastructureMode)
+			bselected = false;
+	}
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void r8712_atimdone_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+}
+
+void r8712_survey_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	unsigned long flags;
+	u32 len;
+	struct wlan_bssid_ex *pnetwork;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	pnetwork = (struct wlan_bssid_ex *)pbuf;
+#ifdef __BIG_ENDIAN
+	/* endian_convert */
+	pnetwork->Length = le32_to_cpu(pnetwork->Length);
+	pnetwork->Ssid.SsidLength = le32_to_cpu(pnetwork->Ssid.SsidLength);
+	pnetwork->Privacy = le32_to_cpu(pnetwork->Privacy);
+	pnetwork->Rssi = le32_to_cpu(pnetwork->Rssi);
+	pnetwork->NetworkTypeInUse = le32_to_cpu(pnetwork->NetworkTypeInUse);
+	pnetwork->Configuration.ATIMWindow =
+		 le32_to_cpu(pnetwork->Configuration.ATIMWindow);
+	pnetwork->Configuration.BeaconPeriod =
+		 le32_to_cpu(pnetwork->Configuration.BeaconPeriod);
+	pnetwork->Configuration.DSConfig =
+		 le32_to_cpu(pnetwork->Configuration.DSConfig);
+	pnetwork->Configuration.FHConfig.DwellTime =
+		 le32_to_cpu(pnetwork->Configuration.FHConfig.DwellTime);
+	pnetwork->Configuration.FHConfig.HopPattern =
+		 le32_to_cpu(pnetwork->Configuration.FHConfig.HopPattern);
+	pnetwork->Configuration.FHConfig.HopSet =
+		 le32_to_cpu(pnetwork->Configuration.FHConfig.HopSet);
+	pnetwork->Configuration.FHConfig.Length =
+		 le32_to_cpu(pnetwork->Configuration.FHConfig.Length);
+	pnetwork->Configuration.Length =
+		 le32_to_cpu(pnetwork->Configuration.Length);
+	pnetwork->InfrastructureMode =
+		 le32_to_cpu(pnetwork->InfrastructureMode);
+	pnetwork->IELength = le32_to_cpu(pnetwork->IELength);
+#endif
+	len = r8712_get_wlan_bssid_ex_sz(pnetwork);
+	if (len > sizeof(struct wlan_bssid_ex))
+		return;
+	spin_lock_irqsave(&pmlmepriv->lock2, flags);
+	/* update IBSS_network 's timestamp */
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		if (!memcmp(&(pmlmepriv->cur_network.network.MacAddress),
+		    pnetwork->MacAddress, ETH_ALEN)) {
+			struct wlan_network *ibss_wlan = NULL;
+
+			memcpy(pmlmepriv->cur_network.network.IEs,
+				pnetwork->IEs, 8);
+			ibss_wlan = r8712_find_network(
+						&pmlmepriv->scanned_queue,
+						pnetwork->MacAddress);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.IEs,
+					pnetwork->IEs, 8);
+				goto exit;
+			}
+		}
+	}
+	/* lock pmlmepriv->lock when you accessing network_q */
+	if (!check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		if (pnetwork->Ssid.Ssid[0] != 0) {
+			rtl8711_add_network(adapter, pnetwork);
+		} else {
+			pnetwork->Ssid.SsidLength = 8;
+			memcpy(pnetwork->Ssid.Ssid, "<hidden>", 8);
+			rtl8711_add_network(adapter, pnetwork);
+		}
+	}
+exit:
+	spin_unlock_irqrestore(&pmlmepriv->lock2, flags);
+}
+
+void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		del_timer(&pmlmepriv->scan_to_timer);
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+
+	if (pmlmepriv->to_join) {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+				if (!r8712_select_and_join_from_scan(pmlmepriv)) {
+					mod_timer(&pmlmepriv->assoc_timer, jiffies +
+						  msecs_to_jiffies(MAX_JOIN_TIMEOUT));
+				} else {
+					struct wlan_bssid_ex *pdev_network =
+					  &(adapter->registrypriv.dev_network);
+					u8 *pibss =
+						 adapter->registrypriv.dev_network.MacAddress;
+					pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;
+					memcpy(&pdev_network->Ssid,
+						&pmlmepriv->assoc_ssid,
+						sizeof(struct
+							 ndis_802_11_ssid));
+					r8712_update_registrypriv_dev_network
+						(adapter);
+					r8712_generate_random_ibss(pibss);
+					pmlmepriv->fw_state =
+						 WIFI_ADHOC_MASTER_STATE;
+					pmlmepriv->to_join = false;
+				}
+			}
+		} else {
+			pmlmepriv->to_join = false;
+			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+			if (!r8712_select_and_join_from_scan(pmlmepriv))
+				mod_timer(&pmlmepriv->assoc_timer, jiffies +
+					  msecs_to_jiffies(MAX_JOIN_TIMEOUT));
+			else
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		}
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+/*
+ *r8712_free_assoc_resources: the caller has to lock pmlmepriv->lock
+ */
+void r8712_free_assoc_resources(struct _adapter *adapter)
+{
+	unsigned long irqL;
+	struct wlan_network *pwlan = NULL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	pwlan = r8712_find_network(&pmlmepriv->scanned_queue,
+				   tgt_network->network.MacAddress);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		psta = r8712_get_stainfo(&adapter->stapriv,
+					 tgt_network->network.MacAddress);
+
+		spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
+		r8712_free_stainfo(adapter,  psta);
+		spin_unlock_irqrestore(&pstapriv->sta_hash_lock, irqL);
+	}
+
+	if (check_fwstate(pmlmepriv,
+	    WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
+		r8712_free_all_stainfo(adapter);
+	if (pwlan)
+		pwlan->fixed = false;
+
+	if (((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) &&
+	     (adapter->stapriv.asoc_sta_count == 1)))
+		free_network_nolock(pmlmepriv, pwlan);
+}
+
+/*
+ * r8712_indicate_connect: the caller has to lock pmlmepriv->lock
+ */
+void r8712_indicate_connect(struct _adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	pmlmepriv->to_join = false;
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_LINK);
+	r8712_os_indicate_connect(padapter);
+	if (padapter->registrypriv.power_mgnt > PS_MODE_ACTIVE)
+		mod_timer(&pmlmepriv->dhcp_timer,
+			  jiffies + msecs_to_jiffies(60000));
+}
+
+/*
+ * r8712_ind_disconnect: the caller has to lock pmlmepriv->lock
+ */
+void r8712_ind_disconnect(struct _adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+		padapter->ledpriv.LedControlHandler(padapter, LED_CTL_NO_LINK);
+		r8712_os_indicate_disconnect(padapter);
+	}
+	if (padapter->pwrctrlpriv.pwr_mode !=
+	    padapter->registrypriv.power_mgnt) {
+		del_timer(&pmlmepriv->dhcp_timer);
+		r8712_set_ps_mode(padapter, padapter->registrypriv.power_mgnt,
+				  padapter->registrypriv.smart_ps);
+	}
+}
+
+/*Notes:
+ *pnetwork : returns from r8712_joinbss_event_callback
+ *ptarget_wlan: found from scanned_queue
+ *if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if
+ *  "ptarget_sta" & "ptarget_wlan" exist.
+ *if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check
+ * if "ptarget_wlan" exist.
+ *if join_res > 0, update "cur_network->network" from
+ * "pnetwork->network" if (ptarget_wlan !=NULL).
+ */
+void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL = 0, irqL2;
+	struct sta_info	*ptarget_sta = NULL, *pcur_sta = NULL;
+	struct sta_priv	*pstapriv = &adapter->stapriv;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network	*cur_network = &pmlmepriv->cur_network;
+	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
+	unsigned int		the_same_macaddr = false;
+	struct wlan_network *pnetwork;
+
+	if (sizeof(struct list_head) == 4 * sizeof(u32)) {
+		pnetwork = kmalloc(sizeof(struct wlan_network), GFP_ATOMIC);
+		if (!pnetwork)
+			return;
+		memcpy((u8 *)pnetwork + 16, (u8 *)pbuf + 8,
+		       sizeof(struct wlan_network) - 16);
+	} else {
+		pnetwork = (struct wlan_network *)pbuf;
+	}
+
+#ifdef __BIG_ENDIAN
+	/* endian_convert */
+	pnetwork->join_res = le32_to_cpu(pnetwork->join_res);
+	pnetwork->network_type = le32_to_cpu(pnetwork->network_type);
+	pnetwork->network.Length = le32_to_cpu(pnetwork->network.Length);
+	pnetwork->network.Ssid.SsidLength =
+		 le32_to_cpu(pnetwork->network.Ssid.SsidLength);
+	pnetwork->network.Privacy = le32_to_cpu(pnetwork->network.Privacy);
+	pnetwork->network.Rssi = le32_to_cpu(pnetwork->network.Rssi);
+	pnetwork->network.NetworkTypeInUse =
+		 le32_to_cpu(pnetwork->network.NetworkTypeInUse);
+	pnetwork->network.Configuration.ATIMWindow =
+		 le32_to_cpu(pnetwork->network.Configuration.ATIMWindow);
+	pnetwork->network.Configuration.BeaconPeriod =
+		 le32_to_cpu(pnetwork->network.Configuration.BeaconPeriod);
+	pnetwork->network.Configuration.DSConfig =
+		 le32_to_cpu(pnetwork->network.Configuration.DSConfig);
+	pnetwork->network.Configuration.FHConfig.DwellTime =
+		 le32_to_cpu(pnetwork->network.Configuration.FHConfig.DwellTime);
+	pnetwork->network.Configuration.FHConfig.HopPattern =
+		 le32_to_cpu(pnetwork->network.Configuration.FHConfig.HopPattern);
+	pnetwork->network.Configuration.FHConfig.HopSet =
+		 le32_to_cpu(pnetwork->network.Configuration.FHConfig.HopSet);
+	pnetwork->network.Configuration.FHConfig.Length =
+		 le32_to_cpu(pnetwork->network.Configuration.FHConfig.Length);
+	pnetwork->network.Configuration.Length =
+		 le32_to_cpu(pnetwork->network.Configuration.Length);
+	pnetwork->network.InfrastructureMode =
+		 le32_to_cpu(pnetwork->network.InfrastructureMode);
+	pnetwork->network.IELength = le32_to_cpu(pnetwork->network.IELength);
+#endif
+
+	the_same_macaddr = !memcmp(pnetwork->network.MacAddress,
+				   cur_network->network.MacAddress, ETH_ALEN);
+	pnetwork->network.Length =
+		 r8712_get_wlan_bssid_ex_sz(&pnetwork->network);
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex))
+		goto ignore_joinbss_callback;
+	if (pnetwork->join_res > 0) {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/*s1. find ptarget_wlan*/
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr) {
+					ptarget_wlan =
+					    r8712_find_network(&pmlmepriv->scanned_queue,
+					    cur_network->network.MacAddress);
+				} else {
+					pcur_wlan =
+					     r8712_find_network(&pmlmepriv->scanned_queue,
+					     cur_network->network.MacAddress);
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
+
+					pcur_sta = r8712_get_stainfo(pstapriv,
+					     cur_network->network.MacAddress);
+					spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL2);
+					r8712_free_stainfo(adapter, pcur_sta);
+					spin_unlock_irqrestore(&(pstapriv->sta_hash_lock), irqL2);
+
+					ptarget_wlan =
+						 r8712_find_network(&pmlmepriv->scanned_queue,
+						 pnetwork->network.MacAddress);
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
+				}
+			} else {
+				ptarget_wlan = r8712_find_network(&pmlmepriv->scanned_queue,
+						pnetwork->network.MacAddress);
+				if (ptarget_wlan)
+					ptarget_wlan->fixed = true;
+			}
+
+			if (!ptarget_wlan) {
+				if (check_fwstate(pmlmepriv,
+					_FW_UNDER_LINKING))
+					pmlmepriv->fw_state ^=
+						 _FW_UNDER_LINKING;
+				goto ignore_joinbss_callback;
+			}
+
+			/*s2. find ptarget_sta & update ptarget_sta*/
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				if (the_same_macaddr) {
+					ptarget_sta =
+						 r8712_get_stainfo(pstapriv,
+						 pnetwork->network.MacAddress);
+					if (!ptarget_sta)
+						ptarget_sta =
+						 r8712_alloc_stainfo(pstapriv,
+						 pnetwork->network.MacAddress);
+				} else {
+					ptarget_sta =
+						 r8712_alloc_stainfo(pstapriv,
+						 pnetwork->network.MacAddress);
+				}
+				if (ptarget_sta) /*update ptarget_sta*/ {
+					ptarget_sta->aid = pnetwork->join_res;
+					ptarget_sta->qos_option = 1;
+					ptarget_sta->mac_id = 5;
+					if (adapter->securitypriv.AuthAlgrthm == 2) {
+						adapter->securitypriv.binstallGrpkey = false;
+						adapter->securitypriv.busetkipkey = false;
+						adapter->securitypriv.bgrpkey_handshake = false;
+						ptarget_sta->ieee8021x_blocked = true;
+						ptarget_sta->XPrivacy = adapter->
+						securitypriv.PrivacyAlgrthm;
+						memset((u8 *)&ptarget_sta->x_UncstKey,
+							 0,
+							 sizeof(union Keytype));
+						memset((u8 *)&ptarget_sta->tkiprxmickey,
+							 0,
+							 sizeof(union Keytype));
+						memset((u8 *)&ptarget_sta->tkiptxmickey,
+							 0,
+							 sizeof(union Keytype));
+						memset((u8 *)&ptarget_sta->txpn, 0,
+							 sizeof(union pn48));
+						memset((u8 *)&ptarget_sta->rxpn, 0,
+							 sizeof(union pn48));
+					}
+				} else {
+					if (check_fwstate(pmlmepriv,
+					    _FW_UNDER_LINKING))
+						pmlmepriv->fw_state ^=
+							 _FW_UNDER_LINKING;
+					goto ignore_joinbss_callback;
+				}
+			}
+
+			/*s3. update cur_network & indicate connect*/
+			memcpy(&cur_network->network, &pnetwork->network,
+				pnetwork->network.Length);
+			cur_network->aid = pnetwork->join_res;
+			/*update fw_state will clr _FW_UNDER_LINKING*/
+			switch (pnetwork->network.InfrastructureMode) {
+			case Ndis802_11Infrastructure:
+				pmlmepriv->fw_state = WIFI_STATION_STATE;
+				break;
+			case Ndis802_11IBSS:
+				pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+				break;
+			default:
+				pmlmepriv->fw_state = WIFI_NULL_STATE;
+				break;
+			}
+			r8712_update_protection(adapter,
+					  (cur_network->network.IEs) +
+					  sizeof(struct NDIS_802_11_FIXED_IEs),
+					  (cur_network->network.IELength));
+			/*TODO: update HT_Capability*/
+			update_ht_cap(adapter, cur_network->network.IEs,
+				      cur_network->network.IELength);
+			/*indicate connect*/
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+				r8712_indicate_connect(adapter);
+			del_timer(&pmlmepriv->assoc_timer);
+		} else {
+			goto ignore_joinbss_callback;
+		}
+	} else {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			mod_timer(&pmlmepriv->assoc_timer,
+				  jiffies + msecs_to_jiffies(1));
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		}
+	}
+ignore_joinbss_callback:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	if (sizeof(struct list_head) == 4 * sizeof(u32))
+		kfree(pnetwork);
+}
+
+void r8712_stassoc_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL;
+	struct sta_info *psta;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct stassoc_event *pstassoc	= (struct stassoc_event *)pbuf;
+
+	/* to do: */
+	if (!r8712_access_ctrl(&adapter->acl_list, pstassoc->macaddr))
+		return;
+	psta = r8712_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta) {
+		/*the sta have been in sta_info_queue => do nothing
+		 *(between drv has received this event before and
+		 * fw have not yet to set key to CAM_ENTRY)
+		 */
+		return;
+	}
+
+	psta = r8712_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (!psta)
+		return;
+	/* to do : init sta_info variable */
+	psta->qos_option = 0;
+	psta->mac_id = le32_to_cpu(pstassoc->cam_id);
+	/* psta->aid = (uint)pstassoc->cam_id; */
+
+	if (adapter->securitypriv.AuthAlgrthm == 2)
+		psta->XPrivacy = adapter->securitypriv.PrivacyAlgrthm;
+	psta->ieee8021x_blocked = false;
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
+			/* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			r8712_indicate_connect(adapter);
+		}
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+void r8712_stadel_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL, irqL2;
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	struct wlan_bssid_ex *pdev_network = NULL;
+	u8 *pibss = NULL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct stadel_event *pstadel = (struct stadel_event *)pbuf;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL2);
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		r8712_ind_disconnect(adapter);
+		r8712_free_assoc_resources(adapter);
+	}
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE |
+	    WIFI_ADHOC_STATE)) {
+		psta = r8712_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+		spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
+		r8712_free_stainfo(adapter, psta);
+		spin_unlock_irqrestore(&pstapriv->sta_hash_lock, irqL);
+		if (adapter->stapriv.asoc_sta_count == 1) {
+			/*a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			pwlan = r8712_find_network(&pmlmepriv->scanned_queue,
+				tgt_network->network.MacAddress);
+			if (pwlan) {
+				pwlan->fixed = false;
+				free_network_nolock(pmlmepriv, pwlan);
+			}
+			/*re-create ibss*/
+			pdev_network = &(adapter->registrypriv.dev_network);
+			pibss = adapter->registrypriv.dev_network.MacAddress;
+			memcpy(pdev_network, &tgt_network->network,
+				r8712_get_wlan_bssid_ex_sz(&tgt_network->network));
+			memcpy(&pdev_network->Ssid,
+				&pmlmepriv->assoc_ssid,
+				sizeof(struct ndis_802_11_ssid));
+			r8712_update_registrypriv_dev_network(adapter);
+			r8712_generate_random_ibss(pibss);
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL2);
+}
+
+void r8712_cpwm_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	struct reportpwrstate_parm *preportpwrstate =
+			 (struct reportpwrstate_parm *)pbuf;
+
+	preportpwrstate->state |= (u8)(adapter->pwrctrlpriv.cpwm_tog + 0x80);
+	r8712_cpwm_int_hdl(adapter, preportpwrstate);
+}
+
+/*	When the Netgear 3500 AP is with WPA2PSK-AES mode, it will send
+ *	 the ADDBA req frame with start seq control = 0 to wifi client after
+ *	 the WPA handshake and the seqence number of following data packet
+ *	will be 0. In this case, the Rx reorder sequence is not longer than 0
+ *	 and the WiFi client will drop the data with seq number 0.
+ *	So, the 8712 firmware has to inform driver with receiving the
+ *	 ADDBA-Req frame so that the driver can reset the
+ *	sequence value of Rx reorder control.
+ */
+void r8712_got_addbareq_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	struct	ADDBA_Req_Report_parm *pAddbareq_pram =
+			 (struct ADDBA_Req_Report_parm *)pbuf;
+	struct	sta_info *psta;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	recv_reorder_ctrl *precvreorder_ctrl = NULL;
+
+	psta = r8712_get_stainfo(pstapriv, pAddbareq_pram->MacAddress);
+	if (psta) {
+		precvreorder_ctrl =
+			 &psta->recvreorder_ctrl[pAddbareq_pram->tid];
+		/* set the indicate_seq to 0xffff so that the rx reorder
+		 * can store any following data packet.
+		 */
+		precvreorder_ctrl->indicate_seq = 0xffff;
+	}
+}
+
+void r8712_wpspbc_event_callback(struct _adapter *adapter, u8 *pbuf)
+{
+	if (!adapter->securitypriv.wps_hw_pbc_pressed)
+		adapter->securitypriv.wps_hw_pbc_pressed = true;
+}
+
+void _r8712_sitesurvey_ctrl_handler(struct _adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct sitesurvey_ctrl	*psitesurveyctrl = &pmlmepriv->sitesurveyctrl;
+	struct registry_priv	*pregistrypriv = &adapter->registrypriv;
+	u64 current_tx_pkts;
+	uint current_rx_pkts;
+
+	current_tx_pkts = (adapter->xmitpriv.tx_pkts) -
+			  (psitesurveyctrl->last_tx_pkts);
+	current_rx_pkts = (adapter->recvpriv.rx_pkts) -
+			  (psitesurveyctrl->last_rx_pkts);
+	psitesurveyctrl->last_tx_pkts = adapter->xmitpriv.tx_pkts;
+	psitesurveyctrl->last_rx_pkts = adapter->recvpriv.rx_pkts;
+	if ((current_tx_pkts > pregistrypriv->busy_thresh) ||
+	    (current_rx_pkts > pregistrypriv->busy_thresh))
+		psitesurveyctrl->traffic_busy = true;
+	else
+		psitesurveyctrl->traffic_busy = false;
+}
+
+void _r8712_join_timeout_handler(struct _adapter *adapter)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	if (adapter->driver_stopped || adapter->surprise_removed)
+		return;
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	pmlmepriv->to_join = false;
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		r8712_os_indicate_disconnect(adapter);
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+	}
+	if (adapter->pwrctrlpriv.pwr_mode != adapter->registrypriv.power_mgnt) {
+		r8712_set_ps_mode(adapter, adapter->registrypriv.power_mgnt,
+				  adapter->registrypriv.smart_ps);
+	}
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+void r8712_scan_timeout_handler (struct _adapter *adapter)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	pmlmepriv->to_join = false;	/* scan fail, so clear to_join flag */
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+void _r8712_dhcp_timeout_handler (struct _adapter *adapter)
+{
+	if (adapter->driver_stopped || adapter->surprise_removed)
+		return;
+	if (adapter->pwrctrlpriv.pwr_mode != adapter->registrypriv.power_mgnt)
+		r8712_set_ps_mode(adapter, adapter->registrypriv.power_mgnt,
+			    adapter->registrypriv.smart_ps);
+}
+
+int r8712_select_and_join_from_scan(struct mlme_priv *pmlmepriv)
+{
+	struct list_head *phead;
+	unsigned char *dst_ssid, *src_ssid;
+	struct _adapter *adapter;
+	struct  __queue *queue = NULL;
+	struct wlan_network *pnetwork = NULL;
+	struct wlan_network *pnetwork_max_rssi = NULL;
+
+	adapter = (struct _adapter *)pmlmepriv->nic_hdl;
+	queue = &pmlmepriv->scanned_queue;
+	phead = &queue->queue;
+	pmlmepriv->pscanned = phead->next;
+	while (1) {
+		if (end_of_queue_search(phead, pmlmepriv->pscanned)) {
+			if (pmlmepriv->assoc_by_rssi && pnetwork_max_rssi) {
+				pnetwork = pnetwork_max_rssi;
+				goto ask_for_joinbss;
+			}
+			return -EINVAL;
+		}
+		pnetwork = container_of(pmlmepriv->pscanned,
+					struct wlan_network, list);
+		pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+		if (pmlmepriv->assoc_by_bssid) {
+			dst_ssid = pnetwork->network.MacAddress;
+			src_ssid = pmlmepriv->assoc_bssid;
+			if (!memcmp(dst_ssid, src_ssid, ETH_ALEN)) {
+				if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+					if (is_same_network(&pmlmepriv->cur_network.network,
+					    &pnetwork->network)) {
+						_clr_fwstate_(pmlmepriv,
+							_FW_UNDER_LINKING);
+						/*r8712_indicate_connect again*/
+						r8712_indicate_connect(adapter);
+						return 2;
+					}
+					r8712_disassoc_cmd(adapter);
+					r8712_ind_disconnect(adapter);
+					r8712_free_assoc_resources(adapter);
+				}
+				goto ask_for_joinbss;
+			}
+		} else if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+			goto ask_for_joinbss;
+		}
+		dst_ssid = pnetwork->network.Ssid.Ssid;
+		src_ssid = pmlmepriv->assoc_ssid.Ssid;
+		if ((pnetwork->network.Ssid.SsidLength ==
+		    pmlmepriv->assoc_ssid.SsidLength) &&
+		    (!memcmp(dst_ssid, src_ssid,
+		     pmlmepriv->assoc_ssid.SsidLength))) {
+			if (pmlmepriv->assoc_by_rssi) {
+				/* if the ssid is the same, select the bss
+				 * which has the max rssi
+				 */
+				if (pnetwork_max_rssi) {
+					if (pnetwork->network.Rssi >
+					    pnetwork_max_rssi->network.Rssi)
+						pnetwork_max_rssi = pnetwork;
+				} else {
+					pnetwork_max_rssi = pnetwork;
+				}
+			} else if (is_desired_network(adapter, pnetwork)) {
+				if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+					r8712_disassoc_cmd(adapter);
+					r8712_free_assoc_resources(adapter);
+				}
+				goto ask_for_joinbss;
+			}
+		}
+	}
+
+ask_for_joinbss:
+	return r8712_joinbss_cmd(adapter, pnetwork);
+}
+
+int r8712_set_auth(struct _adapter *adapter,
+		   struct security_priv *psecuritypriv)
+{
+	struct cmd_priv	*pcmdpriv = &adapter->cmdpriv;
+	struct cmd_obj *pcmd;
+	struct setauth_parm *psetauthparm;
+
+	pcmd = kmalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd)
+		return -ENOMEM;
+
+	psetauthparm = kzalloc(sizeof(*psetauthparm), GFP_ATOMIC);
+	if (!psetauthparm) {
+		kfree(pcmd);
+		return -ENOMEM;
+	}
+	psetauthparm->mode = (u8)psecuritypriv->AuthAlgrthm;
+	pcmd->cmdcode = _SetAuth_CMD_;
+	pcmd->parmbuf = (unsigned char *)psetauthparm;
+	pcmd->cmdsz = sizeof(struct setauth_parm);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	INIT_LIST_HEAD(&pcmd->list);
+	r8712_enqueue_cmd(pcmdpriv, pcmd);
+	return 0;
+}
+
+int r8712_set_key(struct _adapter *adapter,
+		  struct security_priv *psecuritypriv,
+		  sint keyid)
+{
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct cmd_obj *pcmd;
+	struct setkey_parm *psetkeyparm;
+	u8 keylen;
+	int ret;
+
+	pcmd = kmalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd)
+		return -ENOMEM;
+	psetkeyparm = kzalloc(sizeof(*psetkeyparm), GFP_ATOMIC);
+	if (!psetkeyparm) {
+		ret = -ENOMEM;
+		goto err_free_cmd;
+	}
+	if (psecuritypriv->AuthAlgrthm == 2) { /* 802.1X */
+		psetkeyparm->algorithm =
+			 (u8)psecuritypriv->XGrpPrivacy;
+	} else { /* WEP */
+		psetkeyparm->algorithm =
+			 (u8)psecuritypriv->PrivacyAlgrthm;
+	}
+	psetkeyparm->keyid = (u8)keyid;
+
+	switch (psetkeyparm->algorithm) {
+	case _WEP40_:
+		keylen = 5;
+		memcpy(psetkeyparm->key,
+			psecuritypriv->DefKey[keyid].skey, keylen);
+		break;
+	case _WEP104_:
+		keylen = 13;
+		memcpy(psetkeyparm->key,
+			psecuritypriv->DefKey[keyid].skey, keylen);
+		break;
+	case _TKIP_:
+		if (keyid < 1 || keyid > 2) {
+			ret = -EINVAL;
+			goto err_free_parm;
+		}
+		keylen = 16;
+		memcpy(psetkeyparm->key,
+			&psecuritypriv->XGrpKey[keyid - 1], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	case _AES_:
+		if (keyid < 1 || keyid > 2) {
+			ret = -EINVAL;
+			goto err_free_parm;
+		}
+		keylen = 16;
+		memcpy(psetkeyparm->key,
+			&psecuritypriv->XGrpKey[keyid - 1], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	default:
+		ret = -EINVAL;
+		goto err_free_parm;
+	}
+	pcmd->cmdcode = _SetKey_CMD_;
+	pcmd->parmbuf = (u8 *)psetkeyparm;
+	pcmd->cmdsz =  (sizeof(struct setkey_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	INIT_LIST_HEAD(&pcmd->list);
+	r8712_enqueue_cmd(pcmdpriv, pcmd);
+	return 0;
+
+err_free_parm:
+	kfree(psetkeyparm);
+err_free_cmd:
+	kfree(pcmd);
+	return ret;
+}
+
+/* adjust IEs for r8712_joinbss_cmd in WMM */
+int r8712_restruct_wmm_ie(struct _adapter *adapter, u8 *in_ie, u8 *out_ie,
+		    uint in_len, uint initial_out_len)
+{
+	unsigned int ielength = 0;
+	unsigned int i, j;
+
+	i = 12; /* after the fixed IE */
+	while (i < in_len) {
+		ielength = initial_out_len;
+		if (in_ie[i] == 0xDD && in_ie[i + 2] == 0x00 &&
+		    in_ie[i + 3] == 0x50 && in_ie[i + 4] == 0xF2 &&
+		    in_ie[i + 5] == 0x02 && i + 5 < in_len) {
+			/*WMM element ID and OUI*/
+			for (j = i; j < i + 9; j++) {
+				out_ie[ielength] = in_ie[j];
+				ielength++;
+			}
+			out_ie[initial_out_len + 1] = 0x07;
+			out_ie[initial_out_len + 6] = 0x00;
+			out_ie[initial_out_len + 8] = 0x00;
+			break;
+		}
+		i += (in_ie[i + 1] + 2); /* to the next IE element */
+	}
+	return ielength;
+}
+
+/*
+ * Ported from 8185: IsInPreAuthKeyList().
+ *
+ * Search by BSSID,
+ * Return Value:
+ *	-1		:if there is no pre-auth key in the  table
+ *	>=0		:if there is pre-auth key, and   return the entry id
+ */
+static int SecIsInPMKIDList(struct _adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *p = &Adapter->securitypriv;
+	int i;
+
+	for (i = 0; i < NUM_PMKID_CACHE; i++)
+		if (p->PMKIDList[i].bUsed && !memcmp(p->PMKIDList[i].Bssid, bssid, ETH_ALEN))
+			return i;
+	return -1;
+}
+
+sint r8712_restruct_sec_ie(struct _adapter *adapter, u8 *in_ie,
+		     u8 *out_ie, uint in_len)
+{
+	u8 authmode = 0, match;
+	u8 sec_ie[IW_CUSTOM_MAX], uncst_oui[4], bkup_ie[255];
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint ielength, cnt, remove_cnt;
+	int iEntry;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	uint ndisauthmode = psecuritypriv->ndisauthtype;
+	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
+
+	if ((ndisauthmode == Ndis802_11AuthModeWPA) ||
+	    (ndisauthmode == Ndis802_11AuthModeWPAPSK)) {
+		authmode = _WPA_IE_ID_;
+		uncst_oui[0] = 0x0;
+		uncst_oui[1] = 0x50;
+		uncst_oui[2] = 0xf2;
+	}
+	if ((ndisauthmode == Ndis802_11AuthModeWPA2) ||
+	    (ndisauthmode == Ndis802_11AuthModeWPA2PSK)) {
+		authmode = _WPA2_IE_ID_;
+		uncst_oui[0] = 0x0;
+		uncst_oui[1] = 0x0f;
+		uncst_oui[2] = 0xac;
+	}
+	switch (ndissecuritytype) {
+	case Ndis802_11Encryption1Enabled:
+	case Ndis802_11Encryption1KeyAbsent:
+		uncst_oui[3] = 0x1;
+		break;
+	case Ndis802_11Encryption2Enabled:
+	case Ndis802_11Encryption2KeyAbsent:
+		uncst_oui[3] = 0x2;
+		break;
+	case Ndis802_11Encryption3Enabled:
+	case Ndis802_11Encryption3KeyAbsent:
+		uncst_oui[3] = 0x4;
+		break;
+	default:
+		break;
+	}
+	/*Search required WPA or WPA2 IE and copy to sec_ie[] */
+	cnt = 12;
+	match = false;
+	while (cnt < in_len) {
+		if (in_ie[cnt] == authmode) {
+			if ((authmode == _WPA_IE_ID_) &&
+			    (!memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4))) {
+				memcpy(&sec_ie[0], &in_ie[cnt],
+					in_ie[cnt + 1] + 2);
+				match = true;
+				break;
+			}
+			if (authmode == _WPA2_IE_ID_) {
+				memcpy(&sec_ie[0], &in_ie[cnt],
+					in_ie[cnt + 1] + 2);
+				match = true;
+				break;
+			}
+			if (((authmode == _WPA_IE_ID_) &&
+			     (!memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4))) ||
+			     (authmode == _WPA2_IE_ID_))
+				memcpy(&bkup_ie[0], &in_ie[cnt],
+					in_ie[cnt + 1] + 2);
+		}
+		cnt += in_ie[cnt + 1] + 2; /*get next*/
+	}
+	/*restruct WPA IE or WPA2 IE in sec_ie[] */
+	if (match) {
+		if (sec_ie[0] == _WPA_IE_ID_) {
+			/* parsing SSN IE to select required encryption
+			 * algorithm, and set the bc/mc encryption algorithm
+			 */
+			while (true) {
+				/*check wpa_oui tag*/
+				if (memcmp(&sec_ie[2], &wpa_oui[0], 4)) {
+					match = false;
+					break;
+				}
+				if ((sec_ie[6] != 0x01) || (sec_ie[7] != 0x0)) {
+					/*IE Ver error*/
+					match = false;
+					break;
+				}
+				if (!memcmp(&sec_ie[8], &wpa_oui[0], 3)) {
+					/* get bc/mc encryption type (group
+					 * key type)
+					 */
+					switch (sec_ie[11]) {
+					case 0x0: /*none*/
+						psecuritypriv->XGrpPrivacy =
+								_NO_PRIVACY_;
+						break;
+					case 0x1: /*WEP_40*/
+						psecuritypriv->XGrpPrivacy =
+								_WEP40_;
+						break;
+					case 0x2: /*TKIP*/
+						psecuritypriv->XGrpPrivacy =
+								_TKIP_;
+						break;
+					case 0x3: /*AESCCMP*/
+					case 0x4:
+						psecuritypriv->XGrpPrivacy =
+								_AES_;
+						break;
+					case 0x5: /*WEP_104*/
+						psecuritypriv->XGrpPrivacy =
+								_WEP104_;
+						break;
+					}
+				} else {
+					match = false;
+					break;
+				}
+				if (sec_ie[12] == 0x01) {
+					/*check the unicast encryption type*/
+					if (memcmp(&sec_ie[14],
+					    &uncst_oui[0], 4)) {
+						match = false;
+						break;
+
+					} /*else the uncst_oui is match*/
+				} else { /*mixed mode, unicast_enc_type > 1*/
+					/*select the uncst_oui and remove
+					 * the other uncst_oui
+					 */
+					cnt = sec_ie[12];
+					remove_cnt = (cnt - 1) * 4;
+					sec_ie[12] = 0x01;
+					memcpy(&sec_ie[14], &uncst_oui[0], 4);
+					/*remove the other unicast suit*/
+					memcpy(&sec_ie[18],
+						&sec_ie[18 + remove_cnt],
+						sec_ie[1] - 18 + 2 -
+						remove_cnt);
+					sec_ie[1] = sec_ie[1] - remove_cnt;
+				}
+				break;
+			}
+		}
+		if (authmode == _WPA2_IE_ID_) {
+			/* parsing RSN IE to select required encryption
+			 * algorithm, and set the bc/mc encryption algorithm
+			 */
+			while (true) {
+				if ((sec_ie[2] != 0x01) || (sec_ie[3] != 0x0)) {
+					/*IE Ver error*/
+					match = false;
+					break;
+				}
+				if (!memcmp(&sec_ie[4], &uncst_oui[0], 3)) {
+					/*get bc/mc encryption type*/
+					switch (sec_ie[7]) {
+					case 0x1: /*WEP_40*/
+						psecuritypriv->XGrpPrivacy =
+								_WEP40_;
+						break;
+					case 0x2: /*TKIP*/
+						psecuritypriv->XGrpPrivacy =
+								_TKIP_;
+						break;
+					case 0x4: /*AESWRAP*/
+						psecuritypriv->XGrpPrivacy =
+								_AES_;
+						break;
+					case 0x5: /*WEP_104*/
+						psecuritypriv->XGrpPrivacy =
+								_WEP104_;
+						break;
+					default: /*one*/
+						psecuritypriv->XGrpPrivacy =
+								_NO_PRIVACY_;
+						break;
+					}
+				} else {
+					match = false;
+					break;
+				}
+				if (sec_ie[8] == 0x01) {
+					/*check the unicast encryption type*/
+					if (memcmp(&sec_ie[10],
+						     &uncst_oui[0], 4)) {
+						match = false;
+						break;
+					} /*else the uncst_oui is match*/
+				} else { /*mixed mode, unicast_enc_type > 1*/
+					/*select the uncst_oui and remove the
+					 * other uncst_oui
+					 */
+					cnt = sec_ie[8];
+					remove_cnt = (cnt - 1) * 4;
+					sec_ie[8] = 0x01;
+					memcpy(&sec_ie[10], &uncst_oui[0], 4);
+					/*remove the other unicast suit*/
+					memcpy(&sec_ie[14],
+						&sec_ie[14 + remove_cnt],
+						(sec_ie[1] - 14 + 2 -
+						remove_cnt));
+					sec_ie[1] = sec_ie[1] - remove_cnt;
+				}
+				break;
+			}
+		}
+	}
+	if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) {
+		/*copy fixed ie*/
+		memcpy(out_ie, in_ie, 12);
+		ielength = 12;
+		/*copy RSN or SSN*/
+		if (match) {
+			memcpy(&out_ie[ielength], &sec_ie[0], sec_ie[1] + 2);
+			ielength += sec_ie[1] + 2;
+			if (authmode == _WPA2_IE_ID_) {
+				/*the Pre-Authentication bit should be zero*/
+				out_ie[ielength - 1] = 0;
+				out_ie[ielength - 2] = 0;
+			}
+			r8712_report_sec_ie(adapter, authmode, sec_ie);
+		}
+	} else {
+		/*copy fixed ie only*/
+		memcpy(out_ie, in_ie, 12);
+		ielength = 12;
+		if (psecuritypriv->wps_phase) {
+			memcpy(out_ie + ielength, psecuritypriv->wps_ie,
+			       psecuritypriv->wps_ie_len);
+			ielength += psecuritypriv->wps_ie_len;
+		}
+	}
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if (iEntry < 0)
+		return ielength;
+	if (authmode == _WPA2_IE_ID_) {
+		out_ie[ielength] = 1;
+		ielength++;
+		out_ie[ielength] = 0;	/*PMKID count = 0x0100*/
+		ielength++;
+		memcpy(&out_ie[ielength],
+			&psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+		ielength += 16;
+		out_ie[13] += 18;/*PMKID length = 2+16*/
+	}
+	return ielength;
+}
+
+void r8712_init_registrypriv_dev_network(struct _adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
+	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+
+	memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid,
+		sizeof(struct ndis_802_11_ssid));
+	pdev_network->Configuration.Length =
+			 sizeof(struct NDIS_802_11_CONFIGURATION);
+	pdev_network->Configuration.BeaconPeriod = 100;
+	pdev_network->Configuration.FHConfig.Length = 0;
+	pdev_network->Configuration.FHConfig.HopPattern = 0;
+	pdev_network->Configuration.FHConfig.HopSet = 0;
+	pdev_network->Configuration.FHConfig.DwellTime = 0;
+}
+
+void r8712_update_registrypriv_dev_network(struct _adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv	*pregistrypriv = &adapter->registrypriv;
+	struct wlan_bssid_ex	*pdev_network = &pregistrypriv->dev_network;
+	struct security_priv	*psecuritypriv = &adapter->securitypriv;
+	struct wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+
+	pdev_network->Privacy = cpu_to_le32(psecuritypriv->PrivacyAlgrthm
+					    > 0 ? 1 : 0); /* adhoc no 802.1x */
+	pdev_network->Rssi = 0;
+	switch (pregistrypriv->wireless_mode) {
+	case WIRELESS_11B:
+		pdev_network->NetworkTypeInUse = Ndis802_11DS;
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+		pdev_network->NetworkTypeInUse = Ndis802_11OFDM24;
+		break;
+	case WIRELESS_11A:
+		pdev_network->NetworkTypeInUse = Ndis802_11OFDM5;
+		break;
+	default:
+		/* TODO */
+		break;
+	}
+	pdev_network->Configuration.DSConfig = pregistrypriv->channel;
+	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+		pdev_network->Configuration.ATIMWindow = 3;
+	pdev_network->InfrastructureMode = cur_network->network.InfrastructureMode;
+	/* 1. Supported rates
+	 * 2. IE
+	 */
+	sz = r8712_generate_ie(pregistrypriv);
+	pdev_network->IELength = sz;
+	pdev_network->Length = r8712_get_wlan_bssid_ex_sz(pdev_network);
+}
+
+/*the function is at passive_level*/
+void r8712_joinbss_reset(struct _adapter *padapter)
+{
+	int i;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+	/* todo: if you want to do something io/reg/hw setting before join_bss,
+	 * please add code here
+	 */
+	phtpriv->ampdu_enable = false;/*reset to disabled*/
+	for (i = 0; i < 16; i++)
+		phtpriv->baddbareq_issued[i] = false;/*reset it*/
+	if (phtpriv->ht_option) {
+		/* validate  usb rx aggregation */
+		r8712_write8(padapter, 0x102500D9, 48);/*TH = 48 pages, 6k*/
+	} else {
+		/* invalidate  usb rx aggregation */
+		/* TH=1 => means that invalidate usb rx aggregation */
+		r8712_write8(padapter, 0x102500D9, 1);
+	}
+}
+
+/*the function is >= passive_level*/
+unsigned int r8712_restructure_ht_ie(struct _adapter *padapter, u8 *in_ie,
+				     u8 *out_ie, uint in_len, uint *pout_len)
+{
+	u32 ielen, out_len;
+	unsigned char *p;
+	struct ieee80211_ht_cap ht_capie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+	phtpriv->ht_option = 0;
+	p = r8712_get_ie(in_ie + 12, WLAN_EID_HT_CAPABILITY, &ielen, in_len - 12);
+	if (p && (ielen > 0)) {
+		if (pqospriv->qos_option == 0) {
+			out_len = *pout_len;
+			r8712_set_ie(out_ie + out_len, WLAN_EID_VENDOR_SPECIFIC,
+				     _WMM_IE_Length_, WMM_IE, pout_len);
+			pqospriv->qos_option = 1;
+		}
+		out_len = *pout_len;
+		memset(&ht_capie, 0, sizeof(struct ieee80211_ht_cap));
+		ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+				    IEEE80211_HT_CAP_SGI_20 |
+				    IEEE80211_HT_CAP_SGI_40 |
+				    IEEE80211_HT_CAP_TX_STBC |
+				    IEEE80211_HT_CAP_MAX_AMSDU |
+				    IEEE80211_HT_CAP_DSSSCCK40);
+		ht_capie.ampdu_params_info = (IEEE80211_HT_AMPDU_PARM_FACTOR &
+				0x03) | (IEEE80211_HT_AMPDU_PARM_DENSITY & 0x00);
+		r8712_set_ie(out_ie + out_len, WLAN_EID_HT_CAPABILITY,
+			     sizeof(struct ieee80211_ht_cap),
+			     (unsigned char *)&ht_capie, pout_len);
+		phtpriv->ht_option = 1;
+	}
+	return phtpriv->ht_option;
+}
+
+/* the function is > passive_level (in critical_section) */
+static void update_ht_cap(struct _adapter *padapter, u8 *pie, uint ie_len)
+{
+	u8 *p, max_ampdu_sz;
+	int i;
+	uint len;
+	struct sta_info *bmc_sta, *psta;
+	struct ieee80211_ht_cap *pht_capie;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct wlan_network *pcur_network = &(pmlmepriv->cur_network);
+
+	if (!phtpriv->ht_option)
+		return;
+	/* maybe needs check if ap supports rx ampdu. */
+	if (!phtpriv->ampdu_enable &&
+	    (pregistrypriv->ampdu_enable == 1))
+		phtpriv->ampdu_enable = true;
+	/*check Max Rx A-MPDU Size*/
+	len = 0;
+	p = r8712_get_ie(pie + sizeof(struct NDIS_802_11_FIXED_IEs),
+				WLAN_EID_HT_CAPABILITY,
+				&len, ie_len -
+				sizeof(struct NDIS_802_11_FIXED_IEs));
+	if (p && len > 0) {
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info &
+				IEEE80211_HT_AMPDU_PARM_FACTOR);
+		/* max_ampdu_sz (kbytes); */
+		max_ampdu_sz = 1 << (max_ampdu_sz + 3);
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+	}
+	/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info
+	 * if A-MPDU Rx is enabled, resetting rx_ordering_ctrl
+	 * wstart_b(indicate_seq) to default value=0xffff
+	 * todo: check if AP can send A-MPDU packets
+	 */
+	bmc_sta = r8712_get_bcmc_stainfo(padapter);
+	if (bmc_sta) {
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+		}
+	}
+	psta = r8712_get_stainfo(&padapter->stapriv,
+				 pcur_network->network.MacAddress);
+	if (psta) {
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+		}
+	}
+	len = 0;
+	p = r8712_get_ie(pie + sizeof(struct NDIS_802_11_FIXED_IEs),
+		   WLAN_EID_HT_OPERATION, &len,
+		   ie_len - sizeof(struct NDIS_802_11_FIXED_IEs));
+}
+
+void r8712_issue_addbareq_cmd(struct _adapter *padapter, int priority)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv	 *phtpriv = &pmlmepriv->htpriv;
+
+	if ((phtpriv->ht_option == 1) && (phtpriv->ampdu_enable)) {
+		if (!phtpriv->baddbareq_issued[priority]) {
+			r8712_addbareq_cmd(padapter, (u8)priority);
+			phtpriv->baddbareq_issued[priority] = true;
+		}
+	}
+}
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.h b/drivers/staging/rtl8712/rtl871x_mlme.h
new file mode 100644
index 000000000..d7d25f240
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mlme.h
@@ -0,0 +1,205 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_MLME_H_
+#define __RTL871X_MLME_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wlan_bssdef.h"
+
+#define	MAX_BSS_CNT	64
+#define   MAX_JOIN_TIMEOUT	6000
+
+#define		SCANNING_TIMEOUT	4500
+
+#define	SCANQUEUE_LIFETIME 20 /* unit:sec */
+
+#define		WIFI_NULL_STATE	0x00000000
+#define	WIFI_ASOC_STATE		0x00000001	/* Under Linked state...*/
+#define		WIFI_REASOC_STATE 0x00000002
+#define	WIFI_SLEEP_STATE	0x00000004
+#define	WIFI_STATION_STATE	0x00000008
+#define	WIFI_AP_STATE		0x00000010
+#define	WIFI_ADHOC_STATE	0x00000020
+#define   WIFI_ADHOC_MASTER_STATE 0x00000040
+#define   WIFI_UNDER_LINKING	0x00000080
+#define WIFI_SITE_MONITOR	0x00000800	/* to indicate the station
+						 * is under site surveying
+						 */
+#define	WIFI_MP_STATE		0x00010000
+#define	WIFI_MP_CTX_BACKGROUND	0x00020000	/* in cont. tx background*/
+#define	WIFI_MP_CTX_ST		0x00040000	/* in cont. tx with
+						 * single-tone
+						 */
+#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000 /* pending in cont, tx
+						    * background due
+						    * to out of skb
+						    */
+#define	WIFI_MP_CTX_CCK_HW	0x00100000	/* in continuous tx*/
+#define	WIFI_MP_CTX_CCK_CS	0x00200000	/* in cont, tx with carrier
+						 * suppression
+						 */
+#define   WIFI_MP_LPBK_STATE	0x00400000
+
+#define _FW_UNDER_LINKING	WIFI_UNDER_LINKING
+#define _FW_LINKED		WIFI_ASOC_STATE
+#define _FW_UNDER_SURVEY	WIFI_SITE_MONITOR
+
+/*
+ * there are several "locks" in mlme_priv,
+ * since mlme_priv is a shared resource between many threads,
+ * like ISR/Call-Back functions, the OID handlers, and even timer functions.
+ * Each _queue has its own locks, already.
+ * Other items are protected by mlme_priv.lock.
+ * To avoid possible dead lock, any thread trying to modify mlme_priv
+ * SHALL not lock up more than one lock at a time!
+ */
+
+#define traffic_threshold	10
+#define	traffic_scan_period	500
+
+struct sitesurvey_ctrl {
+	u64	last_tx_pkts;
+	uint	last_rx_pkts;
+	sint	traffic_busy;
+	struct timer_list sitesurvey_ctrl_timer;
+};
+
+struct mlme_priv {
+	spinlock_t lock;
+	spinlock_t lock2;
+	sint	fw_state;	/*shall we protect this variable? */
+	u8 to_join; /*flag*/
+	u8 *nic_hdl;
+	struct list_head *pscanned;
+	struct  __queue free_bss_pool;
+	struct  __queue scanned_queue;
+	u8 *free_bss_buf;
+	unsigned long num_of_scanned;
+	u8 passive_mode; /*add for Android's SCAN-ACTIVE/SCAN-PASSIVE */
+	struct ndis_802_11_ssid	assoc_ssid;
+	u8 assoc_bssid[6];
+	struct wlan_network cur_network;
+	struct sitesurvey_ctrl sitesurveyctrl;
+	struct timer_list assoc_timer;
+	uint assoc_by_bssid;
+	uint assoc_by_rssi;
+	struct timer_list scan_to_timer; /* driver handles scan_timeout.*/
+	struct timer_list dhcp_timer; /* set dhcp to if driver in ps mode.*/
+	struct qos_priv qospriv;
+	struct ht_priv	htpriv;
+	struct timer_list wdg_timer; /*watchdog periodic timer*/
+};
+
+static inline u8 *get_bssid(struct mlme_priv *pmlmepriv)
+{
+	return pmlmepriv->cur_network.network.MacAddress;
+}
+
+static inline u8 check_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	if (pmlmepriv->fw_state & state)
+		return true;
+	return false;
+}
+
+static inline sint get_fwstate(struct mlme_priv *pmlmepriv)
+{
+	return pmlmepriv->fw_state;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ *
+ * ### NOTE:#### (!!!!)
+ * TAKE CARE BEFORE CALLING THIS FUNC, LOCK pmlmepriv->lock
+ */
+static inline void set_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	pmlmepriv->fw_state |= state;
+}
+
+static inline void _clr_fwstate_(struct mlme_priv *pmlmepriv, sint state)
+{
+	pmlmepriv->fw_state &= ~state;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ */
+static inline void clr_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	unsigned long irqL;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, state))
+		pmlmepriv->fw_state ^= state;
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+static inline void set_scanned_network_val(struct mlme_priv *pmlmepriv,
+					     sint val)
+{
+	unsigned long irqL;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	pmlmepriv->num_of_scanned = val;
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+}
+
+void r8712_survey_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_stassoc_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_stadel_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_atimdone_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_cpwm_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_wpspbc_event_callback(struct _adapter *adapter, u8 *pbuf);
+void r8712_free_network_queue(struct _adapter *adapter);
+int r8712_init_mlme_priv(struct _adapter *adapter);
+void r8712_free_mlme_priv(struct mlme_priv *pmlmepriv);
+int r8712_select_and_join_from_scan(struct mlme_priv *pmlmepriv);
+int r8712_set_key(struct _adapter *adapter,
+		  struct security_priv *psecuritypriv, sint keyid);
+int r8712_set_auth(struct _adapter *adapter,
+		   struct security_priv *psecuritypriv);
+uint r8712_get_wlan_bssid_ex_sz(struct wlan_bssid_ex *bss);
+void r8712_generate_random_ibss(u8 *pibss);
+u8 *r8712_get_capability_from_ie(u8 *ie);
+struct wlan_network *r8712_get_oldest_wlan_network(
+				struct  __queue *scanned_queue);
+void r8712_free_assoc_resources(struct _adapter *adapter);
+void r8712_ind_disconnect(struct _adapter *adapter);
+void r8712_indicate_connect(struct _adapter *adapter);
+int r8712_restruct_sec_ie(struct _adapter *adapter, u8 *in_ie,
+			  u8 *out_ie, uint in_len);
+int r8712_restruct_wmm_ie(struct _adapter *adapter, u8 *in_ie,
+			  u8 *out_ie, uint in_len, uint initial_out_len);
+void r8712_init_registrypriv_dev_network(struct _adapter *adapter);
+void r8712_update_registrypriv_dev_network(struct _adapter *adapter);
+void _r8712_sitesurvey_ctrl_handler(struct _adapter *adapter);
+void _r8712_join_timeout_handler(struct _adapter *adapter);
+void r8712_scan_timeout_handler(struct _adapter *adapter);
+void _r8712_dhcp_timeout_handler(struct _adapter *adapter);
+struct wlan_network *_r8712_alloc_network(struct mlme_priv *pmlmepriv);
+sint r8712_if_up(struct _adapter *padapter);
+void r8712_joinbss_reset(struct _adapter *padapter);
+unsigned int r8712_restructure_ht_ie(struct _adapter *padapter, u8 *in_ie,
+				     u8 *out_ie, uint in_len, uint *pout_len);
+void r8712_issue_addbareq_cmd(struct _adapter *padapter, int priority);
+int r8712_is_same_ibss(struct _adapter *adapter, struct wlan_network *pnetwork);
+
+#endif /*__RTL871X_MLME_H_*/
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
new file mode 100644
index 000000000..099c512c8
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -0,0 +1,724 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#define _RTL871X_MP_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "rtl871x_mp_phy_regdef.h"
+#include "rtl8712_cmd.h"
+
+static void _init_mp_priv_(struct mp_priv *pmp_priv)
+{
+	pmp_priv->mode = _LOOPBOOK_MODE_;
+	pmp_priv->curr_ch = 1;
+	pmp_priv->curr_modem = MIXED_PHY;
+	pmp_priv->curr_rateidx = 0;
+	pmp_priv->curr_txpoweridx = 0x14;
+	pmp_priv->antenna_tx = ANTENNA_A;
+	pmp_priv->antenna_rx = ANTENNA_AB;
+	pmp_priv->check_mp_pkt = 0;
+	pmp_priv->tx_pktcount = 0;
+	pmp_priv->rx_pktcount = 0;
+	pmp_priv->rx_crcerrpktcount = 0;
+}
+
+static int init_mp_priv(struct mp_priv *pmp_priv)
+{
+	int i;
+	struct mp_xmit_frame *pmp_xmitframe;
+
+	_init_mp_priv_(pmp_priv);
+	_init_queue(&pmp_priv->free_mp_xmitqueue);
+	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	pmp_priv->pallocated_mp_xmitframe_buf = kmalloc(NR_MP_XMITFRAME *
+				sizeof(struct mp_xmit_frame) + 4,
+				GFP_ATOMIC);
+	if (!pmp_priv->pallocated_mp_xmitframe_buf)
+		return -ENOMEM;
+
+	pmp_priv->pmp_xmtframe_buf = pmp_priv->pallocated_mp_xmitframe_buf +
+			 4 -
+			 ((addr_t)(pmp_priv->pallocated_mp_xmitframe_buf) & 3);
+	pmp_xmitframe = (struct mp_xmit_frame *)pmp_priv->pmp_xmtframe_buf;
+	for (i = 0; i < NR_MP_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&(pmp_xmitframe->list));
+		list_add_tail(&(pmp_xmitframe->list),
+				 &(pmp_priv->free_mp_xmitqueue.queue));
+		pmp_xmitframe->pkt = NULL;
+		pmp_xmitframe->frame_tag = MP_FRAMETAG;
+		pmp_xmitframe->padapter = pmp_priv->papdater;
+		pmp_xmitframe++;
+	}
+	pmp_priv->free_mp_xmitframe_cnt = NR_MP_XMITFRAME;
+	return 0;
+}
+
+static int free_mp_priv(struct mp_priv *pmp_priv)
+{
+	kfree(pmp_priv->pallocated_mp_xmitframe_buf);
+	return 0;
+}
+
+void mp871xinit(struct _adapter *padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+
+	pmppriv->papdater = padapter;
+	init_mp_priv(pmppriv);
+}
+
+void mp871xdeinit(struct _adapter *padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+
+	free_mp_priv(pmppriv);
+}
+
+/*
+ * Special for bb and rf reg read/write
+ */
+static u32 fw_iocmd_read(struct _adapter *pAdapter, struct IOCMD_STRUCT iocmd)
+{
+	u32 cmd32 = 0, val32 = 0;
+	u8 iocmd_class	= iocmd.cmdclass;
+	u16 iocmd_value	= iocmd.value;
+	u8 iocmd_idx	= iocmd.index;
+
+	cmd32 = (iocmd_class << 24) | (iocmd_value << 8) | iocmd_idx;
+	if (r8712_fw_cmd(pAdapter, cmd32))
+		r8712_fw_cmd_data(pAdapter, &val32, 1);
+	else
+		val32 = 0;
+	return val32;
+}
+
+static u8 fw_iocmd_write(struct _adapter *pAdapter,
+			 struct IOCMD_STRUCT iocmd, u32 value)
+{
+	u32 cmd32 = 0;
+	u8 iocmd_class	= iocmd.cmdclass;
+	u32 iocmd_value	= iocmd.value;
+	u8 iocmd_idx	= iocmd.index;
+
+	r8712_fw_cmd_data(pAdapter, &value, 0);
+	msleep(100);
+	cmd32 = (iocmd_class << 24) | (iocmd_value << 8) | iocmd_idx;
+	return r8712_fw_cmd(pAdapter, cmd32);
+}
+
+/* offset : 0X800~0XFFF */
+u32 r8712_bb_reg_read(struct _adapter *pAdapter, u16 offset)
+{
+	u8 shift = offset & 0x0003;	/* 4 byte access */
+	u16 bb_addr = offset & 0x0FFC;	/* 4 byte access */
+	u32 bb_val = 0;
+	struct IOCMD_STRUCT iocmd;
+
+	iocmd.cmdclass	= IOCMD_CLASS_BB_RF;
+	iocmd.value	= bb_addr;
+	iocmd.index	= IOCMD_BB_READ_IDX;
+	bb_val = fw_iocmd_read(pAdapter, iocmd);
+	if (shift != 0) {
+		u32 bb_val2 = 0;
+
+		bb_val >>= (shift * 8);
+		iocmd.value += 4;
+		bb_val2 = fw_iocmd_read(pAdapter, iocmd);
+		bb_val2 <<= ((4 - shift) * 8);
+		bb_val |= bb_val2;
+	}
+	return bb_val;
+}
+
+/* offset : 0X800~0XFFF */
+u8 r8712_bb_reg_write(struct _adapter *pAdapter, u16 offset, u32 value)
+{
+	u8 shift = offset & 0x0003;	/* 4 byte access */
+	u16 bb_addr = offset & 0x0FFC;	/* 4 byte access */
+	struct IOCMD_STRUCT iocmd;
+
+	iocmd.cmdclass	= IOCMD_CLASS_BB_RF;
+	iocmd.value	= bb_addr;
+	iocmd.index	= IOCMD_BB_WRITE_IDX;
+	if (shift != 0) {
+		u32 oldValue = 0;
+		u32 newValue = value;
+
+		oldValue = r8712_bb_reg_read(pAdapter, iocmd.value);
+		oldValue &= (0xFFFFFFFF >> ((4 - shift) * 8));
+		value = oldValue | (newValue << (shift * 8));
+		if (!fw_iocmd_write(pAdapter, iocmd, value))
+			return false;
+		iocmd.value += 4;
+		oldValue = r8712_bb_reg_read(pAdapter, iocmd.value);
+		oldValue &= (0xFFFFFFFF << (shift * 8));
+		value = oldValue | (newValue >> ((4 - shift) * 8));
+	}
+	return fw_iocmd_write(pAdapter, iocmd, value);
+}
+
+/* offset : 0x00 ~ 0xFF */
+u32 r8712_rf_reg_read(struct _adapter *pAdapter, u8 path, u8 offset)
+{
+	u16 rf_addr = (path << 8) | offset;
+	struct IOCMD_STRUCT iocmd;
+
+	iocmd.cmdclass	= IOCMD_CLASS_BB_RF;
+	iocmd.value	= rf_addr;
+	iocmd.index	= IOCMD_RF_READ_IDX;
+	return fw_iocmd_read(pAdapter, iocmd);
+}
+
+u8 r8712_rf_reg_write(struct _adapter *pAdapter, u8 path, u8 offset, u32 value)
+{
+	u16 rf_addr = (path << 8) | offset;
+	struct IOCMD_STRUCT iocmd;
+
+	iocmd.cmdclass	= IOCMD_CLASS_BB_RF;
+	iocmd.value	= rf_addr;
+	iocmd.index	= IOCMD_RF_WRIT_IDX;
+	return fw_iocmd_write(pAdapter, iocmd, value);
+}
+
+static u32 bitshift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++)
+		if (((bitmask >> i) &  0x1) == 1)
+			break;
+	return i;
+}
+
+static u32 get_bb_reg(struct _adapter *pAdapter, u16 offset, u32 bitmask)
+{
+	u32 org_value, bit_shift;
+
+	org_value = r8712_bb_reg_read(pAdapter, offset);
+	bit_shift = bitshift(bitmask);
+	return (org_value & bitmask) >> bit_shift;
+}
+
+static u8 set_bb_reg(struct _adapter *pAdapter,
+		     u16 offset,
+		     u32 bitmask,
+		     u32 value)
+{
+	u32 org_value, bit_shift, new_value;
+
+	if (bitmask != bMaskDWord) {
+		org_value = r8712_bb_reg_read(pAdapter, offset);
+		bit_shift = bitshift(bitmask);
+		new_value = (org_value & (~bitmask)) | (value << bit_shift);
+	} else {
+		new_value = value;
+	}
+	return r8712_bb_reg_write(pAdapter, offset, new_value);
+}
+
+static u32 get_rf_reg(struct _adapter *pAdapter, u8 path, u8 offset,
+		      u32 bitmask)
+{
+	u32 org_value, bit_shift;
+
+	org_value = r8712_rf_reg_read(pAdapter, path, offset);
+	bit_shift = bitshift(bitmask);
+	return (org_value & bitmask) >> bit_shift;
+}
+
+static u8 set_rf_reg(struct _adapter *pAdapter, u8 path, u8 offset, u32 bitmask,
+	      u32 value)
+{
+	u32 org_value, bit_shift, new_value;
+
+	if (bitmask != bMaskDWord) {
+		org_value = r8712_rf_reg_read(pAdapter, path, offset);
+		bit_shift = bitshift(bitmask);
+		new_value = (org_value & (~bitmask)) | (value << bit_shift);
+	} else {
+		new_value = value;
+	}
+	return r8712_rf_reg_write(pAdapter, path, offset, new_value);
+}
+
+/*
+ * SetChannel
+ * Description
+ *	Use H2C command to change channel,
+ *	not only modify rf register, but also other setting need to be done.
+ */
+void r8712_SetChannel(struct _adapter *pAdapter)
+{
+	struct cmd_priv *pcmdpriv = &pAdapter->cmdpriv;
+	struct cmd_obj *pcmd = NULL;
+	struct SetChannel_parm *pparm = NULL;
+	u16 code = GEN_CMD_CODE(_SetChannel);
+
+	pcmd = kmalloc(sizeof(*pcmd), GFP_ATOMIC);
+	if (!pcmd)
+		return;
+	pparm = kmalloc(sizeof(*pparm), GFP_ATOMIC);
+	if (!pparm) {
+		kfree(pcmd);
+		return;
+	}
+	pparm->curr_ch = pAdapter->mppriv.curr_ch;
+	init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code);
+	r8712_enqueue_cmd(pcmdpriv, pcmd);
+}
+
+static void SetCCKTxPower(struct _adapter *pAdapter, u8 TxPower)
+{
+	u16 TxAGC = 0;
+
+	TxAGC = TxPower;
+	set_bb_reg(pAdapter, rTxAGC_CCK_Mcs32, bTxAGCRateCCK, TxAGC);
+}
+
+static void SetOFDMTxPower(struct _adapter *pAdapter, u8 TxPower)
+{
+	u32 TxAGC = 0;
+
+	TxAGC |= ((TxPower << 24) | (TxPower << 16) | (TxPower << 8) |
+		  TxPower);
+	set_bb_reg(pAdapter, rTxAGC_Rate18_06, bTxAGCRate18_06, TxAGC);
+	set_bb_reg(pAdapter, rTxAGC_Rate54_24, bTxAGCRate54_24, TxAGC);
+	set_bb_reg(pAdapter, rTxAGC_Mcs03_Mcs00, bTxAGCRateMCS3_MCS0, TxAGC);
+	set_bb_reg(pAdapter, rTxAGC_Mcs07_Mcs04, bTxAGCRateMCS7_MCS4, TxAGC);
+	set_bb_reg(pAdapter, rTxAGC_Mcs11_Mcs08, bTxAGCRateMCS11_MCS8, TxAGC);
+	set_bb_reg(pAdapter, rTxAGC_Mcs15_Mcs12, bTxAGCRateMCS15_MCS12, TxAGC);
+}
+
+void r8712_SetTxPower(struct _adapter *pAdapter)
+{
+	u8 TxPower = pAdapter->mppriv.curr_txpoweridx;
+
+	SetCCKTxPower(pAdapter, TxPower);
+	SetOFDMTxPower(pAdapter, TxPower);
+}
+
+void r8712_SetTxAGCOffset(struct _adapter *pAdapter, u32 ulTxAGCOffset)
+{
+	u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D, tmpAGC;
+
+	TxAGCOffset_B = ulTxAGCOffset & 0x000000ff;
+	TxAGCOffset_C = (ulTxAGCOffset & 0x0000ff00) >> 8;
+	TxAGCOffset_D = (ulTxAGCOffset & 0x00ff0000) >> 16;
+	tmpAGC = TxAGCOffset_D << 8 | TxAGCOffset_C << 4 | TxAGCOffset_B;
+	set_bb_reg(pAdapter, rFPGA0_TxGainStage,
+			(bXBTxAGC | bXCTxAGC | bXDTxAGC), tmpAGC);
+}
+
+void r8712_SetDataRate(struct _adapter *pAdapter)
+{
+	u8 path = RF_PATH_A;
+	u8 offset = RF_SYN_G2;
+	u32 value;
+
+	value = (pAdapter->mppriv.curr_rateidx < 4) ? 0x4440 : 0xF200;
+	r8712_rf_reg_write(pAdapter, path, offset, value);
+}
+
+void r8712_SwitchBandwidth(struct _adapter *pAdapter)
+{
+	/* 3 1.Set MAC register : BWOPMODE  bit2:1 20MhzBW */
+	u8 regBwOpMode = 0;
+	u8 Bandwidth = pAdapter->mppriv.curr_bandwidth;
+
+	regBwOpMode = r8712_read8(pAdapter, 0x10250203);
+	if (Bandwidth == HT_CHANNEL_WIDTH_20)
+		regBwOpMode |= BIT(2);
+	else
+		regBwOpMode &= ~(BIT(2));
+	r8712_write8(pAdapter, 0x10250203, regBwOpMode);
+	/* 3 2.Set PHY related register */
+	switch (Bandwidth) {
+	/* 20 MHz channel*/
+	case HT_CHANNEL_WIDTH_20:
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bRFMOD, 0x0);
+		set_bb_reg(pAdapter, rFPGA1_RFMOD, bRFMOD, 0x0);
+		/* Use PHY_REG.txt default value. Do not need to change.
+		 * Correct the tx power for CCK rate in 40M.
+		 * It is set in Tx descriptor for 8192x series
+		 */
+		set_bb_reg(pAdapter, rFPGA0_AnalogParameter2, bMaskDWord, 0x58);
+		break;
+	/* 40 MHz channel*/
+	case HT_CHANNEL_WIDTH_40:
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bRFMOD, 0x1);
+		set_bb_reg(pAdapter, rFPGA1_RFMOD, bRFMOD, 0x1);
+		/* Use PHY_REG.txt default value. Do not need to change.
+		 * Correct the tx power for CCK rate in 40M.
+		 * Set Control channel to upper or lower. These settings are
+		 * required only for 40MHz
+		 */
+		set_bb_reg(pAdapter, rCCK0_System, bCCKSideBand,
+			   (HAL_PRIME_CHNL_OFFSET_DONT_CARE >> 1));
+		set_bb_reg(pAdapter, rOFDM1_LSTF, 0xC00,
+			   HAL_PRIME_CHNL_OFFSET_DONT_CARE);
+		set_bb_reg(pAdapter, rFPGA0_AnalogParameter2, bMaskDWord, 0x18);
+		break;
+	default:
+		break;
+	}
+
+	/* 3 3.Set RF related register */
+	switch (Bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		set_rf_reg(pAdapter, RF_PATH_A, RF_CHNLBW,
+			   BIT(10) | BIT(11), 0x01);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		set_rf_reg(pAdapter, RF_PATH_A, RF_CHNLBW,
+			   BIT(10) | BIT(11), 0x00);
+		break;
+	default:
+		break;
+	}
+}
+
+/*------------------------------Define structure----------------------------*/
+struct R_ANTENNA_SELECT_OFDM {
+	u32	r_tx_antenna:4;
+	u32	r_ant_l:4;
+	u32	r_ant_non_ht:4;
+	u32	r_ant_ht1:4;
+	u32	r_ant_ht2:4;
+	u32	r_ant_ht_s1:4;
+	u32	r_ant_non_ht_s1:4;
+	u32	OFDM_TXSC:2;
+	u32	Reserved:2;
+};
+
+struct R_ANTENNA_SELECT_CCK {
+	u8	r_cckrx_enable_2:2;
+	u8	r_cckrx_enable:2;
+	u8	r_ccktx_enable:4;
+};
+
+void r8712_SwitchAntenna(struct _adapter *pAdapter)
+{
+	u32	ofdm_tx_en_val = 0, ofdm_tx_ant_sel_val = 0;
+	u8	ofdm_rx_ant_sel_val = 0;
+	u8	cck_ant_select_val = 0;
+	u32	cck_ant_sel_val = 0;
+	struct R_ANTENNA_SELECT_CCK *p_cck_txrx;
+
+	p_cck_txrx = (struct R_ANTENNA_SELECT_CCK *)&cck_ant_select_val;
+
+	switch (pAdapter->mppriv.antenna_tx) {
+	case ANTENNA_A:
+		/* From SD3 Willis suggestion !!! Set RF A=TX and B as standby*/
+		set_bb_reg(pAdapter, rFPGA0_XA_HSSIParameter2, 0xe, 2);
+		set_bb_reg(pAdapter, rFPGA0_XB_HSSIParameter2, 0xe, 1);
+		ofdm_tx_en_val = 0x3;
+		ofdm_tx_ant_sel_val = 0x11111111;/* Power save */
+		p_cck_txrx->r_ccktx_enable = 0x8;
+		break;
+	case ANTENNA_B:
+		set_bb_reg(pAdapter, rFPGA0_XA_HSSIParameter2, 0xe, 1);
+		set_bb_reg(pAdapter, rFPGA0_XB_HSSIParameter2, 0xe, 2);
+		ofdm_tx_en_val = 0x3;
+		ofdm_tx_ant_sel_val = 0x22222222;/* Power save */
+		p_cck_txrx->r_ccktx_enable = 0x4;
+		break;
+	case ANTENNA_AB:	/* For 8192S */
+		set_bb_reg(pAdapter, rFPGA0_XA_HSSIParameter2, 0xe, 2);
+		set_bb_reg(pAdapter, rFPGA0_XB_HSSIParameter2, 0xe, 2);
+		ofdm_tx_en_val = 0x3;
+		ofdm_tx_ant_sel_val = 0x3321333; /* Disable Power save */
+		p_cck_txrx->r_ccktx_enable = 0xC;
+		break;
+	default:
+		break;
+	}
+	/*OFDM Tx*/
+	set_bb_reg(pAdapter, rFPGA1_TxInfo, 0xffffffff, ofdm_tx_ant_sel_val);
+	/*OFDM Tx*/
+	set_bb_reg(pAdapter, rFPGA0_TxInfo, 0x0000000f, ofdm_tx_en_val);
+	switch (pAdapter->mppriv.antenna_rx) {
+	case ANTENNA_A:
+		ofdm_rx_ant_sel_val = 0x1;	/* A */
+		p_cck_txrx->r_cckrx_enable = 0x0; /* default: A */
+		p_cck_txrx->r_cckrx_enable_2 = 0x0; /* option: A */
+		break;
+	case ANTENNA_B:
+		ofdm_rx_ant_sel_val = 0x2;	/* B */
+		p_cck_txrx->r_cckrx_enable = 0x1; /* default: B */
+		p_cck_txrx->r_cckrx_enable_2 = 0x1; /* option: B */
+		break;
+	case ANTENNA_AB:
+		ofdm_rx_ant_sel_val = 0x3; /* AB */
+		p_cck_txrx->r_cckrx_enable = 0x0; /* default:A */
+		p_cck_txrx->r_cckrx_enable_2 = 0x1; /* option:B */
+		break;
+	default:
+		break;
+	}
+	/*OFDM Rx*/
+	set_bb_reg(pAdapter, rOFDM0_TRxPathEnable, 0x0000000f,
+		   ofdm_rx_ant_sel_val);
+	/*OFDM Rx*/
+	set_bb_reg(pAdapter, rOFDM1_TRxPathEnable, 0x0000000f,
+		   ofdm_rx_ant_sel_val);
+
+	cck_ant_sel_val = cck_ant_select_val;
+	/*CCK TxRx*/
+	set_bb_reg(pAdapter, rCCK0_AFESetting, bMaskByte3, cck_ant_sel_val);
+}
+
+static void TriggerRFThermalMeter(struct _adapter *pAdapter)
+{
+	/* 0x24: RF Reg[6:5] */
+	set_rf_reg(pAdapter, RF_PATH_A, RF_T_METER, bRFRegOffsetMask, 0x60);
+}
+
+static u32 ReadRFThermalMeter(struct _adapter *pAdapter)
+{
+	/* 0x24: RF Reg[4:0] */
+	return get_rf_reg(pAdapter, RF_PATH_A, RF_T_METER, 0x1F);
+}
+
+void r8712_GetThermalMeter(struct _adapter *pAdapter, u32 *value)
+{
+	TriggerRFThermalMeter(pAdapter);
+	msleep(1000);
+	*value = ReadRFThermalMeter(pAdapter);
+}
+
+void r8712_SetSingleCarrierTx(struct _adapter *pAdapter, u8 bStart)
+{
+	if (bStart) { /* Start Single Carrier. */
+		/* 1. if OFDM block on? */
+		if (!get_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn))
+			/*set OFDM block on*/
+			set_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn, bEnable);
+		/* 2. set CCK test mode off, set to CCK normal mode */
+		set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, bDisable);
+		/* 3. turn on scramble setting */
+		set_bb_reg(pAdapter, rCCK0_System, bCCKScramble, bEnable);
+		/* 4. Turn On Single Carrier Tx and off the other test modes. */
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier, bEnable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone, bDisable);
+	} else { /* Stop Single Carrier.*/
+		/* Turn off all test modes.*/
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier,
+			   bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone, bDisable);
+		msleep(20);
+		/*BB Reset*/
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x0);
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x1);
+	}
+}
+
+void r8712_SetSingleToneTx(struct _adapter *pAdapter, u8 bStart)
+{
+	u8 rfPath;
+
+	switch (pAdapter->mppriv.antenna_tx) {
+	case ANTENNA_B:
+		rfPath = RF_PATH_B;
+		break;
+	case ANTENNA_A:
+	default:
+		rfPath = RF_PATH_A;
+		break;
+	}
+	if (bStart) { /* Start Single Tone.*/
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn, bDisable);
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn, bDisable);
+		set_rf_reg(pAdapter, rfPath, RF_TX_G2, bRFRegOffsetMask,
+			   0xd4000);
+		msleep(100);
+		/* PAD all on.*/
+		set_rf_reg(pAdapter, rfPath, RF_AC, bRFRegOffsetMask, 0x2001f);
+		msleep(100);
+	} else { /* Stop Single Tone.*/
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn, bEnable);
+		set_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn, bEnable);
+		set_rf_reg(pAdapter, rfPath, RF_TX_G2, bRFRegOffsetMask,
+			   0x54000);
+		msleep(100);
+		/* PAD all on.*/
+		set_rf_reg(pAdapter, rfPath, RF_AC, bRFRegOffsetMask, 0x30000);
+		msleep(100);
+	}
+}
+
+void r8712_SetCarrierSuppressionTx(struct _adapter *pAdapter, u8 bStart)
+{
+	if (bStart) { /* Start Carrier Suppression.*/
+		if (pAdapter->mppriv.curr_rateidx <= MPT_RATE_11M) {
+			/* 1. if CCK block on? */
+			if (!get_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn)) {
+				/*set CCK block on*/
+				set_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn,
+					   bEnable);
+			}
+			/* Turn Off All Test Mode */
+			set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx,
+				   bDisable);
+			set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier,
+				   bDisable);
+			set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone,
+				   bDisable);
+			/*transmit mode*/
+			set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, 0x2);
+			/*turn off scramble setting*/
+			set_bb_reg(pAdapter, rCCK0_System, bCCKScramble,
+				   bDisable);
+			/*Set CCK Tx Test Rate*/
+			/*Set FTxRate to 1Mbps*/
+			set_bb_reg(pAdapter, rCCK0_System, bCCKTxRate, 0x0);
+		}
+	} else { /* Stop Carrier Suppression. */
+		if (pAdapter->mppriv.curr_rateidx <= MPT_RATE_11M) {
+			/*normal mode*/
+			set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, 0x0);
+			/*turn on scramble setting*/
+			set_bb_reg(pAdapter, rCCK0_System, bCCKScramble,
+				   bEnable);
+			/*BB Reset*/
+			set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x0);
+			set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x1);
+		}
+	}
+}
+
+static void SetCCKContinuousTx(struct _adapter *pAdapter, u8 bStart)
+{
+	u32 cckrate;
+
+	if (bStart) {
+		/* 1. if CCK block on? */
+		if (!get_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn)) {
+			/*set CCK block on*/
+			set_bb_reg(pAdapter, rFPGA0_RFMOD, bCCKEn, bEnable);
+		}
+		/* Turn Off All Test Mode */
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone, bDisable);
+		/*Set CCK Tx Test Rate*/
+		cckrate  = pAdapter->mppriv.curr_rateidx;
+		set_bb_reg(pAdapter, rCCK0_System, bCCKTxRate, cckrate);
+		/*transmit mode*/
+		set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, 0x2);
+		/*turn on scramble setting*/
+		set_bb_reg(pAdapter, rCCK0_System, bCCKScramble, bEnable);
+	} else {
+		/*normal mode*/
+		set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, 0x0);
+		/*turn on scramble setting*/
+		set_bb_reg(pAdapter, rCCK0_System, bCCKScramble, bEnable);
+		/*BB Reset*/
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x0);
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x1);
+	}
+} /* mpt_StartCckContTx */
+
+static void SetOFDMContinuousTx(struct _adapter *pAdapter, u8 bStart)
+{
+	if (bStart) {
+		/* 1. if OFDM block on? */
+		if (!get_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn)) {
+			/*set OFDM block on*/
+			set_bb_reg(pAdapter, rFPGA0_RFMOD, bOFDMEn, bEnable);
+		}
+		/* 2. set CCK test mode off, set to CCK normal mode*/
+		set_bb_reg(pAdapter, rCCK0_System, bCCKBBMode, bDisable);
+		/* 3. turn on scramble setting */
+		set_bb_reg(pAdapter, rCCK0_System, bCCKScramble, bEnable);
+		/* 4. Turn On Continue Tx and turn off the other test modes.*/
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx, bEnable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone, bDisable);
+	} else {
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMContinueTx, bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleCarrier,
+			   bDisable);
+		set_bb_reg(pAdapter, rOFDM1_LSTF, bOFDMSingleTone, bDisable);
+		msleep(20);
+		/*BB Reset*/
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x0);
+		set_bb_reg(pAdapter, rPMAC_Reset, bBBResetB, 0x1);
+	}
+} /* mpt_StartOfdmContTx */
+
+void r8712_SetContinuousTx(struct _adapter *pAdapter, u8 bStart)
+{
+	/* ADC turn off [bit24-21] adc port0 ~ port1 */
+	if (bStart) {
+		r8712_bb_reg_write(pAdapter, rRx_Wait_CCCA,
+				   r8712_bb_reg_read(pAdapter,
+				   rRx_Wait_CCCA) & 0xFE1FFFFF);
+		msleep(100);
+	}
+	if (pAdapter->mppriv.curr_rateidx <= MPT_RATE_11M)
+		SetCCKContinuousTx(pAdapter, bStart);
+	else if ((pAdapter->mppriv.curr_rateidx >= MPT_RATE_6M) &&
+		 (pAdapter->mppriv.curr_rateidx <= MPT_RATE_MCS15))
+		SetOFDMContinuousTx(pAdapter, bStart);
+	/* ADC turn on [bit24-21] adc port0 ~ port1 */
+	if (!bStart)
+		r8712_bb_reg_write(pAdapter, rRx_Wait_CCCA,
+				   r8712_bb_reg_read(pAdapter,
+				   rRx_Wait_CCCA) | 0x01E00000);
+}
+
+void r8712_ResetPhyRxPktCount(struct _adapter *pAdapter)
+{
+	u32 i, phyrx_set = 0;
+
+	for (i = OFDM_PPDU_BIT; i <= HT_MPDU_FAIL_BIT; i++) {
+		phyrx_set = 0;
+		phyrx_set |= (i << 28);		/*select*/
+		phyrx_set |= 0x08000000;	/* set counter to zero*/
+		r8712_write32(pAdapter, RXERR_RPT, phyrx_set);
+	}
+}
+
+static u32 GetPhyRxPktCounts(struct _adapter *pAdapter, u32 selbit)
+{
+	/*selection*/
+	u32 phyrx_set = 0;
+	u32 SelectBit;
+
+	SelectBit = selbit << 28;
+	phyrx_set |= (SelectBit & 0xF0000000);
+	r8712_write32(pAdapter, RXERR_RPT, phyrx_set);
+	/*Read packet count*/
+	return r8712_read32(pAdapter, RXERR_RPT) & RPTMaxCount;
+}
+
+u32 r8712_GetPhyRxPktReceived(struct _adapter *pAdapter)
+{
+	u32 OFDM_cnt = GetPhyRxPktCounts(pAdapter, OFDM_MPDU_OK_BIT);
+	u32 CCK_cnt  = GetPhyRxPktCounts(pAdapter, CCK_MPDU_OK_BIT);
+	u32 HT_cnt   = GetPhyRxPktCounts(pAdapter, HT_MPDU_OK_BIT);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+u32 r8712_GetPhyRxPktCRC32Error(struct _adapter *pAdapter)
+{
+	u32 OFDM_cnt = GetPhyRxPktCounts(pAdapter, OFDM_MPDU_FAIL_BIT);
+	u32 CCK_cnt  = GetPhyRxPktCounts(pAdapter, CCK_MPDU_FAIL_BIT);
+	u32 HT_cnt   = GetPhyRxPktCounts(pAdapter, HT_MPDU_FAIL_BIT);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_mp.h b/drivers/staging/rtl8712/rtl871x_mp.h
new file mode 100644
index 000000000..0a60b1e6c
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mp.h
@@ -0,0 +1,275 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_MP_H_
+#define __RTL871X_MP_H_
+
+#define MPT_NOOP			0
+#define MPT_READ_MAC_1BYTE		1
+#define MPT_READ_MAC_2BYTE		2
+#define MPT_READ_MAC_4BYTE		3
+#define MPT_WRITE_MAC_1BYTE		4
+#define MPT_WRITE_MAC_2BYTE		5
+#define MPT_WRITE_MAC_4BYTE		6
+#define MPT_READ_BB_CCK			7
+#define MPT_WRITE_BB_CCK		8
+#define MPT_READ_BB_OFDM		9
+#define MPT_WRITE_BB_OFDM		10
+#define MPT_READ_RF			11
+#define MPT_WRITE_RF			12
+#define MPT_READ_EEPROM_1BYTE		13
+#define MPT_WRITE_EEPROM_1BYTE		14
+#define MPT_READ_EEPROM_2BYTE		15
+#define MPT_WRITE_EEPROM_2BYTE		16
+#define MPT_SET_CSTHRESHOLD		21
+#define MPT_SET_INITGAIN		22
+#define MPT_SWITCH_BAND			23
+#define MPT_SWITCH_CHANNEL		24
+#define MPT_SET_DATARATE		25
+#define MPT_SWITCH_ANTENNA		26
+#define MPT_SET_TX_POWER		27
+#define MPT_SET_CONT_TX			28
+#define MPT_SET_SINGLE_CARRIER		29
+#define MPT_SET_CARRIER_SUPPRESSION	30
+#define MPT_GET_RATE_TABLE		31
+#define MPT_READ_TSSI			32
+#define MPT_GET_THERMAL_METER		33
+#define MAX_MP_XMITBUF_SZ	2048
+#define NR_MP_XMITFRAME		8
+
+struct mp_xmit_frame {
+	struct list_head list;
+	struct pkt_attrib attrib;
+	_pkt *pkt;
+	int frame_tag;
+	struct _adapter *padapter;
+	u8 *mem_addr;
+	u16 sz[8];
+	struct urb *pxmit_urb[8];
+	u8 bpending[8];
+	u8 last[8];
+};
+
+struct mp_wiparam {
+	u32 bcompleted;
+	u32 act_type;
+	u32 io_offset;
+	u32 io_value;
+};
+
+struct mp_priv {
+	struct _adapter *papdater;
+	/*OID cmd handler*/
+	struct mp_wiparam workparam;
+	u8 act_in_progress;
+	/*Tx Section*/
+	u8 TID;
+	u32 tx_pktcount;
+	/*Rx Section*/
+	u32 rx_pktcount;
+	u32 rx_crcerrpktcount;
+	u32 rx_pktloss;
+	struct recv_stat rxstat;
+	/*RF/BB relative*/
+	u32 curr_ch;
+	u32 curr_rateidx;
+	u8 curr_bandwidth;
+	u8 curr_modem;
+	u8 curr_txpoweridx;
+	u32 curr_crystalcap;
+	u16 antenna_tx;
+	u16 antenna_rx;
+	u8 curr_rfpath;
+	u8 check_mp_pkt;
+	uint ForcedDataRate;
+	struct wlan_network mp_network;
+	unsigned char network_macaddr[6];
+	/*Testing Flag*/
+	u32 mode;/*0 for normal type packet,
+		  * 1 for loopback packet (16bytes TXCMD)
+		  */
+	sint prev_fw_state;
+	u8 *pallocated_mp_xmitframe_buf;
+	u8 *pmp_xmtframe_buf;
+	struct  __queue free_mp_xmitqueue;
+	u32 free_mp_xmitframe_cnt;
+};
+
+struct IOCMD_STRUCT {
+	u8	cmdclass;
+	u16	value;
+	u8	index;
+};
+
+struct rf_reg_param {
+	u32 path;
+	u32 offset;
+	u32 value;
+};
+
+struct bb_reg_param {
+	u32 offset;
+	u32 value;
+};
+
+/* ======================================================================= */
+
+#define LOWER	true
+#define RAISE	false
+#define IOCMD_CTRL_REG			0x10250370
+#define IOCMD_DATA_REG			0x10250374
+#define IOCMD_GET_THERMAL_METER		0xFD000028
+#define IOCMD_CLASS_BB_RF		0xF0
+#define IOCMD_BB_READ_IDX		0x00
+#define IOCMD_BB_WRITE_IDX		0x01
+#define IOCMD_RF_READ_IDX		0x02
+#define IOCMD_RF_WRIT_IDX		0x03
+#define BB_REG_BASE_ADDR		0x800
+#define RF_PATH_A	0
+#define RF_PATH_B	1
+#define RF_PATH_C	2
+#define RF_PATH_D	3
+#define MAX_RF_PATH_NUMS	2
+#define _2MAC_MODE_	0
+#define _LOOPBOOK_MODE_	1
+
+/* MP set force data rate base on the definition. */
+enum {
+	/* CCK rate. */
+	MPT_RATE_1M,	/* 0 */
+	MPT_RATE_2M,
+	MPT_RATE_55M,
+	MPT_RATE_11M,	/* 3 */
+
+	/* OFDM rate. */
+	MPT_RATE_6M,	/* 4 */
+	MPT_RATE_9M,
+	MPT_RATE_12M,
+	MPT_RATE_18M,
+	MPT_RATE_24M,
+	MPT_RATE_36M,
+	MPT_RATE_48M,
+	MPT_RATE_54M,	/* 11 */
+
+	/* HT rate. */
+	MPT_RATE_MCS0,	/* 12 */
+	MPT_RATE_MCS1,
+	MPT_RATE_MCS2,
+	MPT_RATE_MCS3,
+	MPT_RATE_MCS4,
+	MPT_RATE_MCS5,
+	MPT_RATE_MCS6,
+	MPT_RATE_MCS7,	/* 19 */
+	MPT_RATE_MCS8,
+	MPT_RATE_MCS9,
+	MPT_RATE_MCS10,
+	MPT_RATE_MCS11,
+	MPT_RATE_MCS12,
+	MPT_RATE_MCS13,
+	MPT_RATE_MCS14,
+	MPT_RATE_MCS15,	/* 27 */
+	MPT_RATE_LAST
+};
+
+/* Represent Channel Width in HT Capabilities */
+enum HT_CHANNEL_WIDTH {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_40 = 1,
+};
+
+#define MAX_TX_PWR_INDEX_N_MODE 64	/* 0x3F */
+
+enum POWER_MODE {
+	POWER_LOW = 0,
+	POWER_NORMAL
+};
+
+#define RX_PKT_BROADCAST	1
+#define RX_PKT_DEST_ADDR	2
+#define RX_PKT_PHY_MATCH	3
+
+#define RPTMaxCount 0x000FFFFF
+
+/* parameter 1 : BitMask
+ *	bit 0  : OFDM PPDU
+ *	bit 1  : OFDM False Alarm
+ *	bit 2  : OFDM MPDU OK
+ *	bit 3  : OFDM MPDU Fail
+ *	bit 4  : CCK PPDU
+ *	bit 5  : CCK False Alarm
+ *	bit 6  : CCK MPDU ok
+ *	bit 7  : CCK MPDU fail
+ *	bit 8  : HT PPDU counter
+ *	bit 9  : HT false alarm
+ *	bit 10 : HT MPDU total
+ *	bit 11 : HT MPDU OK
+ *	bit 12 : HT MPDU fail
+ *	bit 15 : RX full drop
+ */
+enum RXPHY_BITMASK {
+	OFDM_PPDU_BIT = 0,
+	OFDM_MPDU_OK_BIT,
+	OFDM_MPDU_FAIL_BIT,
+	CCK_PPDU_BIT,
+	CCK_MPDU_OK_BIT,
+	CCK_MPDU_FAIL_BIT,
+	HT_PPDU_BIT,
+	HT_MPDU_BIT,
+	HT_MPDU_OK_BIT,
+	HT_MPDU_FAIL_BIT,
+};
+
+enum ENCRY_CTRL_STATE {
+	HW_CONTROL,		/*hw encryption& decryption*/
+	SW_CONTROL,		/*sw encryption& decryption*/
+	HW_ENCRY_SW_DECRY,	/*hw encryption & sw decryption*/
+	SW_ENCRY_HW_DECRY	/*sw encryption & hw decryption*/
+};
+
+/* Bandwidth Offset */
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE	0
+#define HAL_PRIME_CHNL_OFFSET_LOWER	1
+#define HAL_PRIME_CHNL_OFFSET_UPPER	2
+/*=======================================================================*/
+void mp871xinit(struct _adapter *padapter);
+void mp871xdeinit(struct _adapter *padapter);
+u32 r8712_bb_reg_read(struct _adapter *Adapter, u16 offset);
+u8 r8712_bb_reg_write(struct _adapter *Adapter, u16 offset, u32 value);
+u32 r8712_rf_reg_read(struct _adapter *Adapter, u8 path, u8 offset);
+u8 r8712_rf_reg_write(struct _adapter *Adapter, u8 path,
+		      u8 offset, u32 value);
+u32 r8712_get_bb_reg(struct _adapter *Adapter, u16 offset, u32 bitmask);
+u8 r8712_set_bb_reg(struct _adapter *Adapter, u16 offset,
+		    u32 bitmask, u32 value);
+u32 r8712_get_rf_reg(struct _adapter *Adapter, u8 path, u8 offset,
+		     u32 bitmask);
+u8 r8712_set_rf_reg(struct _adapter *Adapter, u8 path, u8 offset,
+		    u32 bitmask, u32 value);
+
+void r8712_SetChannel(struct _adapter *pAdapter);
+void r8712_SetTxPower(struct _adapter *pAdapte);
+void r8712_SetTxAGCOffset(struct _adapter *pAdapter, u32 ulTxAGCOffset);
+void r8712_SetDataRate(struct _adapter *pAdapter);
+void r8712_SwitchBandwidth(struct _adapter *pAdapter);
+void r8712_SwitchAntenna(struct _adapter *pAdapter);
+void r8712_GetThermalMeter(struct _adapter *pAdapter, u32 *value);
+void r8712_SetContinuousTx(struct _adapter *pAdapter, u8 bStart);
+void r8712_SetSingleCarrierTx(struct _adapter *pAdapter, u8 bStart);
+void r8712_SetSingleToneTx(struct _adapter *pAdapter, u8 bStart);
+void r8712_SetCarrierSuppressionTx(struct _adapter *pAdapter, u8 bStart);
+void r8712_ResetPhyRxPktCount(struct _adapter *pAdapter);
+u32 r8712_GetPhyRxPktReceived(struct _adapter *pAdapter);
+u32 r8712_GetPhyRxPktCRC32Error(struct _adapter *pAdapter);
+
+#endif /*__RTL871X_MP_H_*/
+
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
new file mode 100644
index 000000000..26fa09b45
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
@@ -0,0 +1,883 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_mp_ioctl.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#include <linux/rndis.h>
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "mlme_osdep.h"
+#include "rtl871x_mp.h"
+#include "rtl871x_mp_ioctl.h"
+
+uint oid_null_function(struct oid_par_priv *poid_par_priv)
+{
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	uint status = RNDIS_STATUS_SUCCESS;
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid == SET_OID) {
+		if (poid_par_priv->information_buf_len >= sizeof(u8))
+			Adapter->registrypriv.wireless_mode =
+					*(u8 *)poid_par_priv->information_buf;
+		else
+			status = RNDIS_STATUS_INVALID_LENGTH;
+	} else if (poid_par_priv->type_of_oid == QUERY_OID) {
+		if (poid_par_priv->information_buf_len >= sizeof(u8)) {
+			*(u8 *)poid_par_priv->information_buf =
+					 Adapter->registrypriv.wireless_mode;
+			*poid_par_priv->bytes_rw =
+					poid_par_priv->information_buf_len;
+		} else {
+			status = RNDIS_STATUS_INVALID_LENGTH;
+		}
+	} else {
+		status = RNDIS_STATUS_NOT_ACCEPTED;
+	}
+	return status;
+}
+
+uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+	offset = (u16)(pbbreg->offset) & 0xFFF; /*0ffset :0x800~0xfff*/
+	if (offset < BB_REG_BASE_ADDR)
+		offset |= BB_REG_BASE_ADDR;
+	value = pbbreg->value;
+	r8712_bb_reg_write(Adapter, offset, value);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+	offset = (u16)(pbbreg->offset) & 0xFFF; /*0ffset :0x800~0xfff*/
+	if (offset < BB_REG_BASE_ADDR)
+		offset |= BB_REG_BASE_ADDR;
+	value = r8712_bb_reg_read(Adapter, offset);
+	pbbreg->value = value;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+	path = (u8)pbbreg->path;
+	if (path > RF_PATH_B)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	offset = (u8)pbbreg->offset;
+	value = pbbreg->value;
+	r8712_rf_reg_write(Adapter, path, offset, value);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+	path = (u8)pbbreg->path;
+	if (path > RF_PATH_B) /* 1T2R  path_a /path_b */
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	offset = (u8)pbbreg->offset;
+	value = r8712_rf_reg_read(Adapter, path, offset);
+	pbbreg->value = value;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+/*This function initializes the DUT to the MP test mode*/
+static int mp_start_test(struct _adapter *padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct wlan_bssid_ex *bssid;
+	struct sta_info *psta;
+	unsigned long length;
+	unsigned long irqL;
+	int res = 0;
+
+	bssid = kzalloc(sizeof(*bssid), GFP_KERNEL);
+	if (!bssid)
+		return -ENOMEM;
+
+	/* 3 1. initialize a new struct wlan_bssid_ex */
+	memcpy(bssid->MacAddress, pmppriv->network_macaddr, ETH_ALEN);
+	bssid->Ssid.SsidLength = 16;
+	memcpy(bssid->Ssid.Ssid, (unsigned char *)"mp_pseudo_adhoc",
+		bssid->Ssid.SsidLength);
+	bssid->InfrastructureMode = Ndis802_11IBSS;
+	bssid->NetworkTypeInUse = Ndis802_11DS;
+	bssid->IELength = 0;
+	length = r8712_get_wlan_bssid_ex_sz(bssid);
+	if (length % 4) {
+		/*round up to multiple of 4 bytes.*/
+		bssid->Length = ((length >> 2) + 1) << 2;
+	} else {
+		bssid->Length = length;
+	}
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+		goto end_of_mp_start_test;
+	/*init mp_start_test status*/
+	pmppriv->prev_fw_state = get_fwstate(pmlmepriv);
+	pmlmepriv->fw_state = WIFI_MP_STATE;
+	if (pmppriv->mode == _LOOPBOOK_MODE_)
+		set_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE); /*append txdesc*/
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+	/* 3 2. create a new psta for mp driver */
+	/* clear psta in the cur_network, if any */
+	psta = r8712_get_stainfo(&padapter->stapriv,
+				 tgt_network->network.MacAddress);
+	if (psta)
+		r8712_free_stainfo(padapter, psta);
+	psta = r8712_alloc_stainfo(&padapter->stapriv, bssid->MacAddress);
+	if (!psta) {
+		res = -ENOMEM;
+		goto end_of_mp_start_test;
+	}
+	/* 3 3. join pseudo AdHoc */
+	tgt_network->join_res = 1;
+	tgt_network->aid = psta->aid = 1;
+	memcpy(&tgt_network->network, bssid, length);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	r8712_os_indicate_connect(padapter);
+	/* Set to LINKED STATE for MP TRX Testing */
+	set_fwstate(pmlmepriv, _FW_LINKED);
+end_of_mp_start_test:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	kfree(bssid);
+	return res;
+}
+
+/*This function change the DUT from the MP test mode into normal mode */
+static int mp_stop_test(struct _adapter *padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct sta_info *psta;
+	unsigned long irqL;
+
+	spin_lock_irqsave(&pmlmepriv->lock, irqL);
+	if (!check_fwstate(pmlmepriv, WIFI_MP_STATE))
+		goto end_of_mp_stop_test;
+	/* 3 1. disconnect pseudo AdHoc */
+	r8712_os_indicate_disconnect(padapter);
+	/* 3 2. clear psta used in mp test mode. */
+	psta = r8712_get_stainfo(&padapter->stapriv,
+				 tgt_network->network.MacAddress);
+	if (psta)
+		r8712_free_stainfo(padapter, psta);
+	/* 3 3. return to normal state (default:station mode) */
+	pmlmepriv->fw_state = pmppriv->prev_fw_state; /* WIFI_STATION_STATE;*/
+	/*flush the cur_network*/
+	memset(tgt_network, 0, sizeof(struct wlan_network));
+end_of_mp_stop_test:
+	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
+	return _SUCCESS;
+}
+
+uint oid_rt_pro_set_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32 ratevalue;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	ratevalue = *((u32 *)poid_par_priv->information_buf);
+	if (ratevalue >= MPT_RATE_LAST)
+		return RNDIS_STATUS_INVALID_DATA;
+	Adapter->mppriv.curr_rateidx = ratevalue;
+	r8712_SetDataRate(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	uint status = RNDIS_STATUS_SUCCESS;
+	u32 mode;
+	u8 val8;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return  RNDIS_STATUS_NOT_ACCEPTED;
+	mode = *((u32 *)poid_par_priv->information_buf);
+	Adapter->mppriv.mode = mode;/* 1 for loopback*/
+	if (mp_start_test(Adapter))
+		status = RNDIS_STATUS_NOT_ACCEPTED;
+	r8712_write8(Adapter, MSR, 1); /* Link in ad hoc network, 0x1025004C */
+	r8712_write8(Adapter, RCR, 0); /* RCR : disable all pkt, 0x10250048 */
+	/* RCR disable Check BSSID, 0x1025004a */
+	r8712_write8(Adapter, RCR + 2, 0x57);
+	/* disable RX filter map , mgt frames will put in RX FIFO 0 */
+	r8712_write16(Adapter, RXFLTMAP0, 0x0);
+	val8 = r8712_read8(Adapter, EE_9346CR);
+	if (!(val8 & _9356SEL)) { /*boot from EFUSE*/
+		r8712_efuse_reg_init(Adapter);
+		r8712_efuse_change_max_size(Adapter);
+		r8712_efuse_reg_uninit(Adapter);
+	}
+	return status;
+}
+
+uint oid_rt_pro_stop_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (mp_stop_test(Adapter) == _FAIL)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		Channel;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	Channel = *((u32 *)poid_par_priv->information_buf);
+	if (Channel > 14)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	Adapter->mppriv.curr_ch = Channel;
+	r8712_SetChannel(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32 antenna;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	antenna = *((u32 *)poid_par_priv->information_buf);
+	Adapter->mppriv.antenna_tx = (u16)((antenna & 0xFFFF0000) >> 16);
+	Adapter->mppriv.antenna_rx = (u16)(antenna & 0x0000FFFF);
+	r8712_SwitchAntenna(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_tx_power_control_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32 tx_pwr_idx;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	tx_pwr_idx = *((u32 *)poid_par_priv->information_buf);
+	if (tx_pwr_idx > MAX_TX_PWR_INDEX_N_MODE)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	Adapter->mppriv.curr_txpoweridx = (u8)tx_pwr_idx;
+	r8712_SetTxPower(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_query_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len == sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+					Adapter->mppriv.tx_pktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_query_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len == sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+					Adapter->mppriv.rx_pktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_query_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len == sizeof(u32)) {
+		*(u32 *)poid_par_priv->information_buf =
+					Adapter->mppriv.rx_crcerrpktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_reset_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	Adapter->mppriv.tx_pktcount = 0;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_reset_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len == sizeof(u32)) {
+		Adapter->mppriv.rx_pktcount = 0;
+		Adapter->mppriv.rx_crcerrpktcount = 0;
+	} else {
+		return RNDIS_STATUS_INVALID_LENGTH;
+	}
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_reset_phy_rx_packet_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	r8712_ResetPhyRxPktCount(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_phy_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	*(u32 *)poid_par_priv->information_buf =
+					 r8712_GetPhyRxPktReceived(Adapter);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_phy_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	*(u32 *)poid_par_priv->information_buf =
+					 r8712_GetPhyRxPktCRC32Error(Adapter);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_modulation_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	Adapter->mppriv.curr_modem = *((u8 *)poid_par_priv->information_buf);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_continuous_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		bStartTest;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	bStartTest = *((u32 *)poid_par_priv->information_buf);
+	r8712_SetContinuousTx(Adapter, (u8)bStartTest);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_single_carrier_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		bStartTest;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	bStartTest = *((u32 *)poid_par_priv->information_buf);
+	r8712_SetSingleCarrierTx(Adapter, (u8)bStartTest);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_carrier_suppression_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		bStartTest;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	bStartTest = *((u32 *)poid_par_priv->information_buf);
+	r8712_SetCarrierSuppressionTx(Adapter, (u8)bStartTest);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_set_single_tone_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		bStartTest;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	bStartTest = *((u32 *)poid_par_priv->information_buf);
+	r8712_SetSingleToneTx(Adapter, (u8)bStartTest);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	uint status = RNDIS_STATUS_SUCCESS;
+	struct mp_rw_reg *RegRWStruct;
+	u16		offset;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	RegRWStruct = (struct mp_rw_reg *)poid_par_priv->information_buf;
+	if ((RegRWStruct->offset >= 0x10250800) &&
+	    (RegRWStruct->offset <= 0x10250FFF)) {
+		/*baseband register*/
+		/*0ffset :0x800~0xfff*/
+		offset = (u16)(RegRWStruct->offset) & 0xFFF;
+		RegRWStruct->value = r8712_bb_reg_read(Adapter, offset);
+	} else {
+		switch (RegRWStruct->width) {
+		case 1:
+			RegRWStruct->value = r8712_read8(Adapter,
+						   RegRWStruct->offset);
+			break;
+		case 2:
+			RegRWStruct->value = r8712_read16(Adapter,
+						    RegRWStruct->offset);
+			break;
+		case 4:
+			RegRWStruct->value = r8712_read32(Adapter,
+						    RegRWStruct->offset);
+			break;
+		default:
+			status = RNDIS_STATUS_NOT_ACCEPTED;
+			break;
+		}
+	}
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return status;
+}
+
+uint oid_rt_pro_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	uint status = RNDIS_STATUS_SUCCESS;
+	struct mp_rw_reg *RegRWStruct;
+	u16		offset;
+	u32		value;
+	u32 oldValue = 0;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	RegRWStruct = (struct mp_rw_reg *)poid_par_priv->information_buf;
+	if ((RegRWStruct->offset >= 0x10250800) &&
+	    (RegRWStruct->offset <= 0x10250FFF)) {
+		/*baseband register*/
+		offset = (u16)(RegRWStruct->offset) & 0xFFF;
+		value = RegRWStruct->value;
+		switch (RegRWStruct->width) {
+		case 1:
+			oldValue = r8712_bb_reg_read(Adapter, offset);
+			oldValue &= 0xFFFFFF00;
+			value &= 0x000000FF;
+			value |= oldValue;
+			break;
+		case 2:
+			oldValue = r8712_bb_reg_read(Adapter, offset);
+			oldValue &= 0xFFFF0000;
+			value &= 0x0000FFFF;
+			value |= oldValue;
+			break;
+		}
+		r8712_bb_reg_write(Adapter, offset, value);
+	} else {
+		switch (RegRWStruct->width) {
+		case 1:
+			r8712_write8(Adapter, RegRWStruct->offset,
+			       (unsigned char)RegRWStruct->value);
+			break;
+		case 2:
+			r8712_write16(Adapter, RegRWStruct->offset,
+				(unsigned short)RegRWStruct->value);
+			break;
+		case 4:
+			r8712_write32(Adapter, RegRWStruct->offset,
+				(unsigned int)RegRWStruct->value);
+			break;
+		default:
+			status = RNDIS_STATUS_NOT_ACCEPTED;
+			break;
+		}
+	}
+	return status;
+}
+
+uint oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	if (Adapter->mppriv.act_in_progress)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	/*init workparam*/
+	Adapter->mppriv.act_in_progress = true;
+	Adapter->mppriv.workparam.bcompleted = false;
+	Adapter->mppriv.workparam.act_type = MPT_GET_THERMAL_METER;
+	Adapter->mppriv.workparam.io_offset = 0;
+	Adapter->mppriv.workparam.io_value = 0xFFFFFFFF;
+	r8712_GetThermalMeter(Adapter, &Adapter->mppriv.workparam.io_value);
+	Adapter->mppriv.workparam.bcompleted = true;
+	Adapter->mppriv.act_in_progress = false;
+	*(u32 *)poid_par_priv->information_buf =
+				 Adapter->mppriv.workparam.io_value;
+	*poid_par_priv->bytes_rw = sizeof(u32);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_read_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	uint status = RNDIS_STATUS_SUCCESS;
+
+	struct EFUSE_ACCESS_STRUCT *pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len <
+	    sizeof(struct EFUSE_ACCESS_STRUCT))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	pefuse = (struct EFUSE_ACCESS_STRUCT *)poid_par_priv->information_buf;
+	addr = pefuse->start_addr;
+	cnts = pefuse->cnts;
+	data = pefuse->data;
+	memset(data, 0xFF, cnts);
+	if ((addr > 511) || (cnts < 1) || (cnts > 512) || (addr + cnts) >
+	     EFUSE_MAX_SIZE)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (!r8712_efuse_access(Adapter, true, addr, cnts, data))
+		status = RNDIS_STATUS_FAILURE;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return status;
+}
+
+/*------------------------------------------------------------------------*/
+uint oid_rt_pro_write_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	uint status = RNDIS_STATUS_SUCCESS;
+
+	struct EFUSE_ACCESS_STRUCT *pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+
+	pefuse = (struct EFUSE_ACCESS_STRUCT *)poid_par_priv->information_buf;
+	addr = pefuse->start_addr;
+	cnts = pefuse->cnts;
+	data = pefuse->data;
+
+	if ((addr > 511) || (cnts < 1) || (cnts > 512) ||
+	    (addr + cnts) > r8712_efuse_get_max_size(Adapter))
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (!r8712_efuse_access(Adapter, false, addr, cnts, data))
+		status = RNDIS_STATUS_FAILURE;
+	return status;
+}
+
+/*----------------------------------------------------------------------*/
+
+uint oid_rt_get_efuse_current_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(int))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	r8712_efuse_reg_init(Adapter);
+	*(int *)poid_par_priv->information_buf =
+				 r8712_efuse_get_current_size(Adapter);
+	r8712_efuse_reg_uninit(Adapter);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	*(int *)poid_par_priv->information_buf =
+					 r8712_efuse_get_max_size(Adapter);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	uint status = RNDIS_STATUS_SUCCESS;
+
+	if (poid_par_priv->type_of_oid == QUERY_OID)
+		status = oid_rt_pro_read_efuse_hdl(poid_par_priv);
+	else
+		status = oid_rt_pro_write_efuse_hdl(poid_par_priv);
+	return status;
+}
+
+uint oid_rt_pro_efuse_map_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	uint status = RNDIS_STATUS_SUCCESS;
+	u8		*data;
+
+	*poid_par_priv->bytes_rw = 0;
+	if (poid_par_priv->information_buf_len < EFUSE_MAP_MAX_SIZE)
+		return RNDIS_STATUS_INVALID_LENGTH;
+	data = (u8 *)poid_par_priv->information_buf;
+	if (poid_par_priv->type_of_oid == QUERY_OID) {
+		if (r8712_efuse_map_read(Adapter, 0, EFUSE_MAP_MAX_SIZE, data))
+			*poid_par_priv->bytes_rw = EFUSE_MAP_MAX_SIZE;
+		else
+			status = RNDIS_STATUS_FAILURE;
+	} else {
+		/* SET_OID */
+		if (r8712_efuse_reg_init(Adapter)) {
+			if (r8712_efuse_map_write(Adapter, 0,
+			    EFUSE_MAP_MAX_SIZE, data))
+				*poid_par_priv->bytes_rw = EFUSE_MAP_MAX_SIZE;
+			else
+				status = RNDIS_STATUS_FAILURE;
+			r8712_efuse_reg_uninit(Adapter);
+		} else {
+			status = RNDIS_STATUS_FAILURE;
+		}
+	}
+	return status;
+}
+
+uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u32		bandwidth;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	bandwidth = *((u32 *)poid_par_priv->information_buf);/*4*/
+	if (bandwidth != HT_CHANNEL_WIDTH_20)
+		bandwidth = HT_CHANNEL_WIDTH_40;
+	Adapter->mppriv.curr_bandwidth = (u8)bandwidth;
+	r8712_SwitchBandwidth(Adapter);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+uint oid_rt_set_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+	u8		rx_pkt_type;
+	u32		rcr_val32;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	rx_pkt_type = *((u8 *)poid_par_priv->information_buf);/*4*/
+	rcr_val32 = r8712_read32(Adapter, RCR);/*RCR = 0x10250048*/
+	rcr_val32 &= ~(RCR_CBSSID | RCR_AB | RCR_AM | RCR_APM | RCR_AAP);
+	switch (rx_pkt_type) {
+	case RX_PKT_BROADCAST:
+		rcr_val32 |= (RCR_AB | RCR_AM | RCR_APM | RCR_AAP | RCR_ACRC32);
+		break;
+	case RX_PKT_DEST_ADDR:
+		rcr_val32 |= (RCR_AB | RCR_AM | RCR_APM | RCR_AAP | RCR_ACRC32);
+		break;
+	case RX_PKT_PHY_MATCH:
+		rcr_val32 |= (RCR_APM | RCR_ACRC32);
+		break;
+	default:
+		rcr_val32 &= ~(RCR_AAP |
+			       RCR_APM |
+			       RCR_AM |
+			       RCR_AB |
+			       RCR_ACRC32);
+		break;
+	}
+	if (rx_pkt_type == RX_PKT_DEST_ADDR)
+		Adapter->mppriv.check_mp_pkt = 1;
+	else
+		Adapter->mppriv.check_mp_pkt = 0;
+	r8712_write32(Adapter, RCR, rcr_val32);
+	return RNDIS_STATUS_SUCCESS;
+}
+
+/*--------------------------------------------------------------------------*/
+/*Linux*/
+unsigned int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return _SUCCESS;
+}
+
+/*-------------------------------------------------------------------------*/
+uint oid_rt_set_power_down_hdl(struct oid_par_priv *poid_par_priv)
+{
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	/*CALL  the power_down function*/
+	return RNDIS_STATUS_SUCCESS;
+}
+
+/*-------------------------------------------------------------------------- */
+uint oid_rt_get_power_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct _adapter *Adapter = (struct _adapter *)
+				   (poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return RNDIS_STATUS_NOT_ACCEPTED;
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return RNDIS_STATUS_INVALID_LENGTH;
+	*(int *)poid_par_priv->information_buf =
+		 Adapter->registrypriv.low_power ? POWER_LOW : POWER_NORMAL;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return RNDIS_STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.h b/drivers/staging/rtl8712/rtl871x_mp_ioctl.h
new file mode 100644
index 000000000..aa4d5ce47
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.h
@@ -0,0 +1,328 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_MP_IOCTL_H
+#define _RTL871X_MP_IOCTL_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "mp_custom_oid.h"
+#include "rtl871x_ioctl.h"
+#include "rtl871x_ioctl_rtl.h"
+#include "rtl8712_efuse.h"
+
+#define TESTFWCMDNUMBER			1000000
+#define TEST_H2CINT_WAIT_TIME		500
+#define TEST_C2HINT_WAIT_TIME		500
+#define HCI_TEST_SYSCFG_HWMASK		1
+#define _BUSCLK_40M			(4 << 2)
+
+struct CFG_DBG_MSG_STRUCT {
+	u32 DebugLevel;
+	u32 DebugComponent_H32;
+	u32 DebugComponent_L32;
+};
+
+struct mp_rw_reg {
+	uint offset;
+	uint width;
+	u32 value;
+};
+
+/* for OID_RT_PRO_READ16_EEPROM & OID_RT_PRO_WRITE16_EEPROM */
+struct eeprom_rw_param {
+	uint offset;
+	u16 value;
+};
+
+struct EFUSE_ACCESS_STRUCT {
+	u16	start_addr;
+	u16	cnts;
+	u8	data[];
+};
+
+struct burst_rw_reg {
+	uint offset;
+	uint len;
+	u8 Data[256];
+};
+
+struct usb_vendor_req {
+	u8	bRequest;
+	u16	wValue;
+	u16	wIndex;
+	u16	wLength;
+	u8	u8Dir;/*0:OUT, 1:IN */
+	u8	u8InData;
+};
+
+struct DR_VARIABLE_STRUCT {
+	u8 offset;
+	u32 variable;
+};
+
+/* oid_rtl_seg_87_11_00 */
+uint oid_rt_pro_read_register_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_write_register_hdl(struct oid_par_priv *poid_par_priv);
+/* oid_rtl_seg_81_80_00 */
+uint oid_rt_pro_set_data_rate_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_stop_test_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_channel_direct_call_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_antenna_bb_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_tx_power_control_hdl(
+				struct oid_par_priv *poid_par_priv);
+/* oid_rtl_seg_81_80_20 */
+uint oid_rt_pro_query_tx_packet_sent_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_query_rx_packet_received_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_query_rx_packet_crc32_error_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_reset_tx_packet_sent_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_reset_rx_packet_received_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_modulation_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_continuous_tx_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_single_carrier_tx_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_carrier_suppression_tx_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_set_single_tone_tx_hdl(
+				struct oid_par_priv *poid_par_priv);
+/* oid_rtl_seg_81_87 */
+uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv);
+/* oid_rtl_seg_81_85 */
+uint oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_read_efuse_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_write_efuse_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_efuse_current_size_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_pro_efuse_map_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_thermal_meter_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_reset_phy_rx_packet_count_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_phy_rx_packet_received_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_phy_rx_packet_crc32_error_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_set_power_down_hdl(
+				struct oid_par_priv *poid_par_priv);
+uint oid_rt_get_power_mode_hdl(
+				struct oid_par_priv *poid_par_priv);
+#ifdef _RTL871X_MP_IOCTL_C_ /* CAUTION!!! */
+/* This ifdef _MUST_ be left in!! */
+
+#else /* _RTL871X_MP_IOCTL_C_ */
+extern struct oid_obj_priv oid_rtl_seg_81_87[5];
+extern struct oid_obj_priv oid_rtl_seg_87_11_00[32];
+extern struct oid_obj_priv oid_rtl_seg_87_11_20[5];
+extern struct oid_obj_priv oid_rtl_seg_87_11_50[2];
+extern struct oid_obj_priv oid_rtl_seg_87_11_80[1];
+extern struct oid_obj_priv oid_rtl_seg_87_11_B0[1];
+extern struct oid_obj_priv oid_rtl_seg_87_11_F0[16];
+extern struct oid_obj_priv oid_rtl_seg_87_12_00[32];
+
+#endif /* _RTL871X_MP_IOCTL_C_ */
+
+enum MP_MODE {
+	MP_START_MODE,
+	MP_STOP_MODE,
+	MP_ERR_MODE
+};
+
+struct rwreg_param {
+	unsigned int offset;
+	unsigned int width;
+	unsigned int value;
+};
+
+struct bbreg_param {
+	unsigned int offset;
+	unsigned int phymask;
+	unsigned int value;
+};
+
+struct txpower_param {
+	unsigned int pwr_index;
+};
+
+struct datarate_param {
+	unsigned int rate_index;
+};
+
+struct rfintfs_parm {
+	unsigned int rfintfs;
+};
+
+struct mp_xmit_packet {
+	unsigned int len;
+};
+
+struct psmode_param {
+	unsigned int ps_mode;
+	unsigned int smart_ps;
+};
+
+struct mp_ioctl_handler {
+	unsigned int paramsize;
+	unsigned int (*handler)(struct oid_par_priv *poid_par_priv);
+	unsigned int oid;
+};
+
+struct mp_ioctl_param {
+	unsigned int subcode;
+	unsigned int len;
+	unsigned char data[];
+};
+
+#define GEN_MP_IOCTL_SUBCODE(code) _MP_IOCTL_ ## code ## _CMD_
+
+enum RTL871X_MP_IOCTL_SUBCODE {
+	GEN_MP_IOCTL_SUBCODE(MP_START),			/*0*/
+	GEN_MP_IOCTL_SUBCODE(MP_STOP),			/*1*/
+	GEN_MP_IOCTL_SUBCODE(READ_REG),			/*2*/
+	GEN_MP_IOCTL_SUBCODE(WRITE_REG),
+	GEN_MP_IOCTL_SUBCODE(SET_CHANNEL),		/*4*/
+	GEN_MP_IOCTL_SUBCODE(SET_TXPOWER),		/*5*/
+	GEN_MP_IOCTL_SUBCODE(SET_DATARATE),		/*6*/
+	GEN_MP_IOCTL_SUBCODE(READ_BB_REG),		/*7*/
+	GEN_MP_IOCTL_SUBCODE(WRITE_BB_REG),
+	GEN_MP_IOCTL_SUBCODE(READ_RF_REG),		/*9*/
+	GEN_MP_IOCTL_SUBCODE(WRITE_RF_REG),
+	GEN_MP_IOCTL_SUBCODE(SET_RF_INTFS),
+	GEN_MP_IOCTL_SUBCODE(IOCTL_XMIT_PACKET),	/*12*/
+	GEN_MP_IOCTL_SUBCODE(PS_STATE),			/*13*/
+	GEN_MP_IOCTL_SUBCODE(READ16_EEPROM),		/*14*/
+	GEN_MP_IOCTL_SUBCODE(WRITE16_EEPROM),		/*15*/
+	GEN_MP_IOCTL_SUBCODE(SET_PTM),			/*16*/
+	GEN_MP_IOCTL_SUBCODE(READ_TSSI),		/*17*/
+	GEN_MP_IOCTL_SUBCODE(CNTU_TX),			/*18*/
+	GEN_MP_IOCTL_SUBCODE(SET_BANDWIDTH),		/*19*/
+	GEN_MP_IOCTL_SUBCODE(SET_RX_PKT_TYPE),		/*20*/
+	GEN_MP_IOCTL_SUBCODE(RESET_PHY_RX_PKT_CNT),	/*21*/
+	GEN_MP_IOCTL_SUBCODE(GET_PHY_RX_PKT_RECV),	/*22*/
+	GEN_MP_IOCTL_SUBCODE(GET_PHY_RX_PKT_ERROR),	/*23*/
+	GEN_MP_IOCTL_SUBCODE(SET_POWER_DOWN),		/*24*/
+	GEN_MP_IOCTL_SUBCODE(GET_THERMAL_METER),	/*25*/
+	GEN_MP_IOCTL_SUBCODE(GET_POWER_MODE),		/*26*/
+	GEN_MP_IOCTL_SUBCODE(EFUSE),			/*27*/
+	GEN_MP_IOCTL_SUBCODE(EFUSE_MAP),		/*28*/
+	GEN_MP_IOCTL_SUBCODE(GET_EFUSE_MAX_SIZE),	/*29*/
+	GEN_MP_IOCTL_SUBCODE(GET_EFUSE_CURRENT_SIZE),	/*30*/
+	GEN_MP_IOCTL_SUBCODE(SC_TX),			/*31*/
+	GEN_MP_IOCTL_SUBCODE(CS_TX),			/*32*/
+	GEN_MP_IOCTL_SUBCODE(ST_TX),			/*33*/
+	GEN_MP_IOCTL_SUBCODE(SET_ANTENNA),		/*34*/
+	MAX_MP_IOCTL_SUBCODE,
+};
+
+unsigned int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv);
+
+#ifdef _RTL871X_MP_IOCTL_C_ /* CAUTION!!! */
+/* This ifdef _MUST_ be left in!! */
+
+static struct mp_ioctl_handler mp_ioctl_hdl[] = {
+	{sizeof(u32), oid_rt_pro_start_test_hdl,
+			     OID_RT_PRO_START_TEST},/*0*/
+	{sizeof(u32), oid_rt_pro_stop_test_hdl,
+			     OID_RT_PRO_STOP_TEST},/*1*/
+	{sizeof(struct rwreg_param),
+			     oid_rt_pro_read_register_hdl,
+			     OID_RT_PRO_READ_REGISTER},/*2*/
+	{sizeof(struct rwreg_param),
+			     oid_rt_pro_write_register_hdl,
+			     OID_RT_PRO_WRITE_REGISTER},
+	{sizeof(u32),
+			     oid_rt_pro_set_channel_direct_call_hdl,
+			     OID_RT_PRO_SET_CHANNEL_DIRECT_CALL},
+	{sizeof(struct txpower_param),
+			     oid_rt_pro_set_tx_power_control_hdl,
+			     OID_RT_PRO_SET_TX_POWER_CONTROL},
+	{sizeof(u32),
+			     oid_rt_pro_set_data_rate_hdl,
+			     OID_RT_PRO_SET_DATA_RATE},
+	{sizeof(struct bb_reg_param),
+			     oid_rt_pro_read_bb_reg_hdl,
+			     OID_RT_PRO_READ_BB_REG},/*7*/
+	{sizeof(struct bb_reg_param),
+			     oid_rt_pro_write_bb_reg_hdl,
+			     OID_RT_PRO_WRITE_BB_REG},
+	{sizeof(struct rwreg_param),
+			     oid_rt_pro_read_rf_reg_hdl,
+			     OID_RT_PRO_RF_READ_REGISTRY},/*9*/
+	{sizeof(struct rwreg_param),
+			     oid_rt_pro_write_rf_reg_hdl,
+			     OID_RT_PRO_RF_WRITE_REGISTRY},
+	{sizeof(struct rfintfs_parm), NULL, 0},
+	{0, mp_ioctl_xmit_packet_hdl, 0},/*12*/
+	{sizeof(struct psmode_param), NULL, 0},/*13*/
+	{sizeof(struct eeprom_rw_param), NULL, 0},/*14*/
+	{sizeof(struct eeprom_rw_param), NULL, 0},/*15*/
+	{sizeof(unsigned char), NULL, 0},/*16*/
+	{sizeof(u32), NULL, 0},/*17*/
+	{sizeof(u32), oid_rt_pro_set_continuous_tx_hdl,
+			     OID_RT_PRO_SET_CONTINUOUS_TX},/*18*/
+	{sizeof(u32), oid_rt_set_bandwidth_hdl,
+			     OID_RT_SET_BANDWIDTH},/*19*/
+	{sizeof(u32), oid_rt_set_rx_packet_type_hdl,
+			     OID_RT_SET_RX_PACKET_TYPE},/*20*/
+	{0, oid_rt_reset_phy_rx_packet_count_hdl,
+			     OID_RT_RESET_PHY_RX_PACKET_COUNT},/*21*/
+	{sizeof(u32), oid_rt_get_phy_rx_packet_received_hdl,
+			     OID_RT_GET_PHY_RX_PACKET_RECEIVED},/*22*/
+	{sizeof(u32), oid_rt_get_phy_rx_packet_crc32_error_hdl,
+			     OID_RT_GET_PHY_RX_PACKET_CRC32_ERROR},/*23*/
+	{sizeof(unsigned char), oid_rt_set_power_down_hdl,
+			     OID_RT_SET_POWER_DOWN},/*24*/
+	{sizeof(u32), oid_rt_get_thermal_meter_hdl,
+			     OID_RT_PRO_GET_THERMAL_METER},/*25*/
+	{sizeof(u32), oid_rt_get_power_mode_hdl,
+			     OID_RT_GET_POWER_MODE},/*26*/
+	{sizeof(struct EFUSE_ACCESS_STRUCT),
+			     oid_rt_pro_efuse_hdl, OID_RT_PRO_EFUSE},/*27*/
+	{EFUSE_MAP_MAX_SIZE, oid_rt_pro_efuse_map_hdl,
+			     OID_RT_PRO_EFUSE_MAP},/*28*/
+	{sizeof(u32), oid_rt_get_efuse_max_size_hdl,
+			     OID_RT_GET_EFUSE_MAX_SIZE},/*29*/
+	{sizeof(u32), oid_rt_get_efuse_current_size_hdl,
+			     OID_RT_GET_EFUSE_CURRENT_SIZE},/*30*/
+	{sizeof(u32), oid_rt_pro_set_single_carrier_tx_hdl,
+			     OID_RT_PRO_SET_SINGLE_CARRIER_TX},/*31*/
+	{sizeof(u32), oid_rt_pro_set_carrier_suppression_tx_hdl,
+			     OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX},/*32*/
+	{sizeof(u32), oid_rt_pro_set_single_tone_tx_hdl,
+			     OID_RT_PRO_SET_SINGLE_TONE_TX},/*33*/
+	{sizeof(u32), oid_rt_pro_set_antenna_bb_hdl,
+			     OID_RT_PRO_SET_ANTENNA_BB},/*34*/
+};
+
+#else /* _RTL871X_MP_IOCTL_C_ */
+extern struct mp_ioctl_handler mp_ioctl_hdl[];
+#endif /* _RTL871X_MP_IOCTL_C_ */
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
new file mode 100644
index 000000000..a08c5d2f5
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
@@ -0,0 +1,1034 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*****************************************************************************
+ *	Copyright(c) 2008,  RealTEK Technology Inc. All Right Reserved.
+ *
+ * Module:	__INC_HAL8192SPHYREG_H
+ *
+ *
+ * Note:	1. Define PMAC/BB register map
+ *			2. Define RF register map
+ *			3. PMAC/BB register bit mask.
+ *			4. RF reg bit mask.
+ *			5. Other BB/RF relative definition.
+ *
+ *
+ * Export:	Constants, macro, functions(API), global variables(None).
+ *
+ * Abbrev:
+ *
+ * History:
+ *	Data			Who		Remark
+ *	08/07/2007	MHC		1. Porting from 9x series PHYCFG.h.
+ *						2. Reorganize code architecture.
+ *	09/25/2008	MH		1. Add RL6052 register definition
+ *
+ *****************************************************************************/
+#ifndef __RTL871X_MP_PHY_REGDEF_H
+#define __RTL871X_MP_PHY_REGDEF_H
+
+/*--------------------------Define Parameters-------------------------------*/
+
+/*============================================================
+ *       8192S Register offset definition
+ *============================================================
+ *
+ *
+ * BB-PHY register PMAC 0x100 PHY 0x800 - 0xEFF
+ * 1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF
+ * 2. 0x800/0x900/0xA00/0xC00/0xD00/0xE00
+ * 3. RF register 0x00-2E
+ * 4. Bit Mask for BB/RF register
+ * 5. Other definition for BB/RF R/W
+ *
+ * 1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF
+ * 1. Page1(0x100)
+ */
+#define	rPMAC_Reset			0x100
+#define	rPMAC_TxStart			0x104
+#define	rPMAC_TxLegacySIG		0x108
+#define	rPMAC_TxHTSIG1			0x10c
+#define	rPMAC_TxHTSIG2			0x110
+#define	rPMAC_PHYDebug			0x114
+#define	rPMAC_TxPacketNum		0x118
+#define	rPMAC_TxIdle			0x11c
+#define	rPMAC_TxMACHeader0		0x120
+#define	rPMAC_TxMACHeader1		0x124
+#define	rPMAC_TxMACHeader2		0x128
+#define	rPMAC_TxMACHeader3		0x12c
+#define	rPMAC_TxMACHeader4		0x130
+#define	rPMAC_TxMACHeader5		0x134
+#define	rPMAC_TxDataType		0x138
+#define	rPMAC_TxRandomSeed		0x13c
+#define	rPMAC_CCKPLCPPreamble		0x140
+#define	rPMAC_CCKPLCPHeader		0x144
+#define	rPMAC_CCKCRC16			0x148
+#define	rPMAC_OFDMRxCRC32OK		0x170
+#define	rPMAC_OFDMRxCRC32Er		0x174
+#define	rPMAC_OFDMRxParityEr		0x178
+#define	rPMAC_OFDMRxCRC8Er		0x17c
+#define	rPMAC_CCKCRxRC16Er		0x180
+#define	rPMAC_CCKCRxRC32Er		0x184
+#define	rPMAC_CCKCRxRC32OK		0x188
+#define	rPMAC_TxStatus			0x18c
+
+/*
+ * 2. Page2(0x200)
+ *
+ * The following two definition are only used for USB interface.
+ *#define RF_BB_CMD_ADDR	0x02c0	// RF/BB read/write command address.
+ *#define RF_BB_CMD_DATA	0x02c4	// RF/BB read/write command data.
+ *
+ *
+ * 3. Page8(0x800)
+ */
+#define	rFPGA0_RFMOD			0x800	/*RF mode & CCK TxSC RF
+						 * BW Setting??
+						 */
+#define	rFPGA0_TxInfo			0x804	/* Status report?? */
+#define	rFPGA0_PSDFunction		0x808
+#define	rFPGA0_TxGainStage		0x80c	/* Set TX PWR init gain? */
+#define	rFPGA0_RFTiming1		0x810	/* Useless now */
+#define	rFPGA0_RFTiming2		0x814
+#define	rFPGA0_XA_HSSIParameter1	0x820	/* RF 3 wire register */
+#define	rFPGA0_XA_HSSIParameter2	0x824
+#define	rFPGA0_XB_HSSIParameter1	0x828
+#define	rFPGA0_XB_HSSIParameter2	0x82c
+#define	rFPGA0_XC_HSSIParameter1	0x830
+#define	rFPGA0_XC_HSSIParameter2	0x834
+#define	rFPGA0_XD_HSSIParameter1	0x838
+#define	rFPGA0_XD_HSSIParameter2	0x83c
+#define	rFPGA0_XA_LSSIParameter		0x840
+#define	rFPGA0_XB_LSSIParameter		0x844
+#define	rFPGA0_XC_LSSIParameter		0x848
+#define	rFPGA0_XD_LSSIParameter		0x84c
+
+#define	rFPGA0_RFWakeUpParameter	0x850	/* Useless now */
+#define	rFPGA0_RFSleepUpParameter	0x854
+
+#define	rFPGA0_XAB_SwitchControl	0x858	/* RF Channel switch */
+#define	rFPGA0_XCD_SwitchControl	0x85c
+
+#define	rFPGA0_XA_RFInterfaceOE		0x860	/* RF Channel switch */
+#define	rFPGA0_XB_RFInterfaceOE		0x864
+#define	rFPGA0_XC_RFInterfaceOE		0x868
+#define	rFPGA0_XD_RFInterfaceOE		0x86c
+#define	rFPGA0_XAB_RFInterfaceSW	0x870	/* RF Interface Software Ctrl */
+#define	rFPGA0_XCD_RFInterfaceSW	0x874
+
+#define	rFPGA0_XAB_RFParameter		0x878	/* RF Parameter */
+#define	rFPGA0_XCD_RFParameter		0x87c
+
+#define	rFPGA0_AnalogParameter1		0x880	/* Crystal cap setting
+						 * RF-R/W protection
+						 * for parameter4??
+						 */
+#define	rFPGA0_AnalogParameter2		0x884
+#define	rFPGA0_AnalogParameter3		0x888	/* Useless now */
+#define	rFPGA0_AnalogParameter4		0x88c
+
+#define	rFPGA0_XA_LSSIReadBack		0x8a0	/* Tranceiver LSSI Readback */
+#define	rFPGA0_XB_LSSIReadBack		0x8a4
+#define	rFPGA0_XC_LSSIReadBack		0x8a8
+#define	rFPGA0_XD_LSSIReadBack		0x8ac
+
+#define	rFPGA0_PSDReport		0x8b4	/* Useless now */
+#define	rFPGA0_XAB_RFInterfaceRB	0x8e0	/* Useless now */
+#define	rFPGA0_XCD_RFInterfaceRB	0x8e4	/* Useless now */
+
+/*
+ * 4. Page9(0x900)
+ */
+#define	rFPGA1_RFMOD			0x900	/* RF mode & OFDM TxSC */
+
+#define	rFPGA1_TxBlock			0x904	/* Useless now */
+#define	rFPGA1_DebugSelect		0x908	/* Useless now */
+#define	rFPGA1_TxInfo			0x90c	/* Useless now */
+
+/*
+ * 5. PageA(0xA00)
+ *
+ * Set Control channel to upper or lower.
+ * These settings are required only for 40MHz
+ */
+#define	rCCK0_System			0xa00
+
+#define	rCCK0_AFESetting		0xa04	/* Disable init gain now */
+#define	rCCK0_CCA			0xa08	/* Disable init gain now */
+
+#define	rCCK0_RxAGC1			0xa0c
+/* AGC default value, saturation level
+ * Antenna Diversity, RX AGC, LNA Threshold, RX LNA Threshold useless now.
+ * Not the same as 90 series
+ */
+#define	rCCK0_RxAGC2			0xa10	/* AGC & DAGC */
+
+#define	rCCK0_RxHP			0xa14
+
+#define	rCCK0_DSPParameter1		0xa18	/* Timing recovery & Channel
+						 * estimation threshold
+						 */
+#define	rCCK0_DSPParameter2		0xa1c	/* SQ threshold */
+
+#define	rCCK0_TxFilter1			0xa20
+#define	rCCK0_TxFilter2			0xa24
+#define	rCCK0_DebugPort			0xa28	/* debug port and Tx filter3 */
+#define	rCCK0_FalseAlarmReport		0xa2c	/* 0xa2d useless now 0xa30-a4f
+						 * channel report
+						 */
+#define	rCCK0_TRSSIReport		0xa50
+#define	rCCK0_RxReport			0xa54   /* 0xa57 */
+#define	rCCK0_FACounterLower		0xa5c   /* 0xa5b */
+#define	rCCK0_FACounterUpper		0xa58   /* 0xa5c */
+
+/*
+ * 6. PageC(0xC00)
+ */
+#define	rOFDM0_LSTF			0xc00
+#define	rOFDM0_TRxPathEnable		0xc04
+#define	rOFDM0_TRMuxPar			0xc08
+#define	rOFDM0_TRSWIsolation		0xc0c
+
+/*RxIQ DC offset, Rx digital filter, DC notch filter */
+#define	rOFDM0_XARxAFE			0xc10
+#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
+#define	rOFDM0_XBRxAFE			0xc18
+#define	rOFDM0_XBRxIQImbalance		0xc1c
+#define	rOFDM0_XCRxAFE			0xc20
+#define	rOFDM0_XCRxIQImbalance		0xc24
+#define	rOFDM0_XDRxAFE			0xc28
+#define	rOFDM0_XDRxIQImbalance		0xc2c
+
+#define	rOFDM0_RxDetector1		0xc30  /* PD,BW & SBD DM tune
+						* init gain
+						*/
+#define	rOFDM0_RxDetector2		0xc34  /* SBD & Fame Sync. */
+#define	rOFDM0_RxDetector3		0xc38  /* Frame Sync. */
+#define	rOFDM0_RxDetector4		0xc3c  /* PD, SBD, Frame Sync &
+						* Short-GI
+						*/
+
+#define	rOFDM0_RxDSP			0xc40  /* Rx Sync Path */
+#define	rOFDM0_CFOandDAGC		0xc44  /* CFO & DAGC */
+#define	rOFDM0_CCADropThreshold		0xc48 /* CCA Drop threshold */
+#define	rOFDM0_ECCAThreshold		0xc4c /* energy CCA */
+
+#define	rOFDM0_XAAGCCore1		0xc50	/* DIG */
+#define	rOFDM0_XAAGCCore2		0xc54
+#define	rOFDM0_XBAGCCore1		0xc58
+#define	rOFDM0_XBAGCCore2		0xc5c
+#define	rOFDM0_XCAGCCore1		0xc60
+#define	rOFDM0_XCAGCCore2		0xc64
+#define	rOFDM0_XDAGCCore1		0xc68
+#define	rOFDM0_XDAGCCore2		0xc6c
+#define	rOFDM0_AGCParameter1		0xc70
+#define	rOFDM0_AGCParameter2		0xc74
+#define	rOFDM0_AGCRSSITable		0xc78
+#define	rOFDM0_HTSTFAGC			0xc7c
+
+#define	rOFDM0_XATxIQImbalance		0xc80	/* TX PWR TRACK and DIG */
+#define	rOFDM0_XATxAFE			0xc84
+#define	rOFDM0_XBTxIQImbalance		0xc88
+#define	rOFDM0_XBTxAFE			0xc8c
+#define	rOFDM0_XCTxIQImbalance		0xc90
+#define	rOFDM0_XCTxAFE			0xc94
+#define	rOFDM0_XDTxIQImbalance		0xc98
+#define	rOFDM0_XDTxAFE			0xc9c
+
+#define	rOFDM0_RxHPParameter		0xce0
+#define	rOFDM0_TxPseudoNoiseWgt		0xce4
+#define	rOFDM0_FrameSync		0xcf0
+#define	rOFDM0_DFSReport		0xcf4
+#define	rOFDM0_TxCoeff1			0xca4
+#define	rOFDM0_TxCoeff2			0xca8
+#define	rOFDM0_TxCoeff3			0xcac
+#define	rOFDM0_TxCoeff4			0xcb0
+#define	rOFDM0_TxCoeff5			0xcb4
+#define	rOFDM0_TxCoeff6			0xcb8
+
+/*
+ * 7. PageD(0xD00)
+ */
+#define	rOFDM1_LSTF			0xd00
+#define	rOFDM1_TRxPathEnable		0xd04
+
+#define	rOFDM1_CFO			0xd08	/* No setting now */
+#define	rOFDM1_CSI1			0xd10
+#define	rOFDM1_SBD			0xd14
+#define	rOFDM1_CSI2			0xd18
+#define	rOFDM1_CFOTracking		0xd2c
+#define	rOFDM1_TRxMesaure1		0xd34
+#define	rOFDM1_IntfDet			0xd3c
+#define	rOFDM1_PseudoNoiseStateAB	0xd50
+#define	rOFDM1_PseudoNoiseStateCD	0xd54
+#define	rOFDM1_RxPseudoNoiseWgt		0xd58
+
+#define	rOFDM_PHYCounter1		0xda0  /* cca, parity fail */
+#define	rOFDM_PHYCounter2		0xda4  /* rate illegal, crc8 fail */
+#define	rOFDM_PHYCounter3		0xda8  /* MCS not support */
+#define	rOFDM_ShortCFOAB		0xdac  /* No setting now */
+#define	rOFDM_ShortCFOCD		0xdb0
+#define	rOFDM_LongCFOAB			0xdb4
+#define	rOFDM_LongCFOCD			0xdb8
+#define	rOFDM_TailCFOAB			0xdbc
+#define	rOFDM_TailCFOCD			0xdc0
+#define	rOFDM_PWMeasure1		0xdc4
+#define	rOFDM_PWMeasure2		0xdc8
+#define	rOFDM_BWReport			0xdcc
+#define	rOFDM_AGCReport			0xdd0
+#define	rOFDM_RxSNR			0xdd4
+#define	rOFDM_RxEVMCSI			0xdd8
+#define	rOFDM_SIGReport			0xddc
+
+/*
+ * 8. PageE(0xE00)
+ */
+#define	rTxAGC_Rate18_06		0xe00
+#define	rTxAGC_Rate54_24		0xe04
+#define	rTxAGC_CCK_Mcs32		0xe08
+#define	rTxAGC_Mcs03_Mcs00		0xe10
+#define	rTxAGC_Mcs07_Mcs04		0xe14
+#define	rTxAGC_Mcs11_Mcs08		0xe18
+#define	rTxAGC_Mcs15_Mcs12		0xe1c
+
+/* Analog- control in RX_WAIT_CCA : REG: EE0
+ * [Analog- Power & Control Register]
+ */
+#define		rRx_Wait_CCCA		0xe70
+#define	rAnapar_Ctrl_BB			0xee0
+
+/*
+ * 7. RF Register 0x00-0x2E (RF 8256)
+ *    RF-0222D 0x00-3F
+ *
+ * Zebra1
+ */
+#define	rZebra1_HSSIEnable		0x0	/* Useless now */
+#define	rZebra1_TRxEnable1		0x1
+#define	rZebra1_TRxEnable2		0x2
+#define	rZebra1_AGC			0x4
+#define	rZebra1_ChargePump		0x5
+#define	rZebra1_Channel			0x7	/* RF channel switch */
+#define	rZebra1_TxGain			0x8	/* Useless now */
+#define	rZebra1_TxLPF			0x9
+#define	rZebra1_RxLPF			0xb
+#define	rZebra1_RxHPFCorner		0xc
+
+/* Zebra4 */
+#define	rGlobalCtrl			0	/* Useless now */
+#define	rRTL8256_TxLPF			19
+#define	rRTL8256_RxLPF			11
+
+/* RTL8258 */
+#define	rRTL8258_TxLPF			0x11	/* Useless now */
+#define	rRTL8258_RxLPF			0x13
+#define	rRTL8258_RSSILPF		0xa
+
+/* RL6052 Register definition */
+#define	RF_AC				0x00
+#define	RF_IQADJ_G1			0x01
+#define	RF_IQADJ_G2			0x02
+#define	RF_POW_TRSW			0x05
+
+#define	RF_GAIN_RX			0x06
+#define	RF_GAIN_TX			0x07
+
+#define	RF_TXM_IDAC			0x08
+#define	RF_BS_IQGEN			0x0F
+
+#define	RF_MODE1			0x10
+#define	RF_MODE2			0x11
+
+#define	RF_RX_AGC_HP			0x12
+#define	RF_TX_AGC			0x13
+#define	RF_BIAS				0x14
+#define	RF_IPA				0x15
+#define	RF_POW_ABILITY			0x17
+#define	RF_MODE_AG			0x18
+#define	rRfChannel			0x18	/* RF channel and BW switch */
+#define	RF_CHNLBW			0x18	/* RF channel and BW switch */
+#define	RF_TOP				0x19
+#define	RF_RX_G1			0x1A
+#define	RF_RX_G2			0x1B
+#define	RF_RX_BB2			0x1C
+#define	RF_RX_BB1			0x1D
+
+#define	RF_RCK1				0x1E
+#define	RF_RCK2				0x1F
+
+#define	RF_TX_G1			0x20
+#define	RF_TX_G2			0x21
+#define	RF_TX_G3			0x22
+
+#define	RF_TX_BB1			0x23
+#define	RF_T_METER			0x24
+
+#define	RF_SYN_G1			0x25	/* RF TX Power control */
+#define	RF_SYN_G2			0x26	/* RF TX Power control */
+#define	RF_SYN_G3			0x27	/* RF TX Power control */
+#define	RF_SYN_G4			0x28	/* RF TX Power control */
+#define	RF_SYN_G5			0x29	/* RF TX Power control */
+#define	RF_SYN_G6			0x2A	/* RF TX Power control */
+#define	RF_SYN_G7			0x2B	/* RF TX Power control */
+#define	RF_SYN_G8			0x2C	/* RF TX Power control */
+
+#define	RF_RCK_OS			0x30	/* RF TX PA control */
+
+#define	RF_TXPA_G1			0x31	/* RF TX PA control */
+#define	RF_TXPA_G2			0x32	/* RF TX PA control */
+#define	RF_TXPA_G3			0x33	/* RF TX PA control */
+
+/*
+ * Bit Mask
+ *
+ * 1. Page1(0x100)
+ */
+#define	bBBResetB			0x100	/* Useless now? */
+#define	bGlobalResetB			0x200
+#define	bOFDMTxStart			0x4
+#define	bCCKTxStart			0x8
+#define	bCRC32Debug			0x100
+#define	bPMACLoopback			0x10
+#define	bTxLSIG				0xffffff
+#define	bOFDMTxRate			0xf
+#define	bOFDMTxReserved			0x10
+#define	bOFDMTxLength			0x1ffe0
+#define	bOFDMTxParity			0x20000
+#define	bTxHTSIG1			0xffffff
+#define	bTxHTMCSRate			0x7f
+#define	bTxHTBW				0x80
+#define	bTxHTLength			0xffff00
+#define	bTxHTSIG2			0xffffff
+#define	bTxHTSmoothing			0x1
+#define	bTxHTSounding			0x2
+#define	bTxHTReserved			0x4
+#define	bTxHTAggreation			0x8
+#define	bTxHTSTBC			0x30
+#define	bTxHTAdvanceCoding		0x40
+#define	bTxHTShortGI			0x80
+#define	bTxHTNumberHT_LTF		0x300
+#define	bTxHTCRC8			0x3fc00
+#define	bCounterReset			0x10000
+#define	bNumOfOFDMTx			0xffff
+#define	bNumOfCCKTx			0xffff0000
+#define	bTxIdleInterval			0xffff
+#define	bOFDMService			0xffff0000
+#define	bTxMACHeader			0xffffffff
+#define	bTxDataInit			0xff
+#define	bTxHTMode			0x100
+#define	bTxDataType			0x30000
+#define	bTxRandomSeed			0xffffffff
+#define	bCCKTxPreamble			0x1
+#define	bCCKTxSFD			0xffff0000
+#define	bCCKTxSIG			0xff
+#define	bCCKTxService			0xff00
+#define	bCCKLengthExt			0x8000
+#define	bCCKTxLength			0xffff0000
+#define	bCCKTxCRC16			0xffff
+#define	bCCKTxStatus			0x1
+#define	bOFDMTxStatus			0x2
+#define IS_BB_REG_OFFSET_92S(_Offset)	((_Offset >= 0x800) && \
+					(_Offset <= 0xfff))
+
+/* 2. Page8(0x800) */
+#define	bRFMOD			0x1	/* Reg 0x800 rFPGA0_RFMOD */
+#define	bJapanMode		0x2
+#define	bCCKTxSC		0x30
+#define	bCCKEn			0x1000000
+#define	bOFDMEn			0x2000000
+
+#define	bOFDMRxADCPhase         0x10000	/* Useless now */
+#define	bOFDMTxDACPhase         0x40000
+#define	bXATxAGC                0x3f
+#define	bXBTxAGC                0xf00	/* Reg 80c rFPGA0_TxGainStage */
+#define	bXCTxAGC                0xf000
+#define	bXDTxAGC                0xf0000
+
+#define	bPAStart		0xf0000000	/* Useless now */
+#define	bTRStart		0x00f00000
+#define	bRFStart		0x0000f000
+#define	bBBStart		0x000000f0
+#define	bBBCCKStart		0x0000000f
+#define	bPAEnd			0xf          /* Reg0x814 */
+#define	bTREnd			0x0f000000
+#define	bRFEnd			0x000f0000
+#define	bCCAMask		0x000000f0   /* T2R */
+#define	bR2RCCAMask		0x00000f00
+#define	bHSSI_R2TDelay		0xf8000000
+#define	bHSSI_T2RDelay		0xf80000
+#define	bContTxHSSI		0x400     /* change gain at continue Tx */
+#define	bIGFromCCK		0x200
+#define	bAGCAddress		0x3f
+#define	bRxHPTx			0x7000
+#define	bRxHPT2R		0x38000
+#define	bRxHPCCKIni		0xc0000
+#define	bAGCTxCode		0xc00000
+#define	bAGCRxCode		0x300000
+#define	b3WireDataLength	0x800	/* Reg 0x820~84f rFPGA0_XA_HSSIParm1 */
+#define	b3WireAddressLength	0x400
+#define	b3WireRFPowerDown	0x1	/* Useless now */
+#define	b5GPAPEPolarity		0x40000000
+#define	b2GPAPEPolarity		0x80000000
+#define	bRFSW_TxDefaultAnt	0x3
+#define	bRFSW_TxOptionAnt	0x30
+#define	bRFSW_RxDefaultAnt	0x300
+#define	bRFSW_RxOptionAnt	0x3000
+#define	bRFSI_3WireData		0x1
+#define	bRFSI_3WireClock	0x2
+#define	bRFSI_3WireLoad		0x4
+#define	bRFSI_3WireRW		0x8
+#define	bRFSI_3Wire		0xf
+#define	bRFSI_RFENV		0x10	/* Reg 0x870 rFPGA0_XAB_RFInterfaceSW */
+#define	bRFSI_TRSW		0x20	/* Useless now */
+#define	bRFSI_TRSWB		0x40
+#define	bRFSI_ANTSW		0x100
+#define	bRFSI_ANTSWB		0x200
+#define	bRFSI_PAPE		0x400
+#define	bRFSI_PAPE5G		0x800
+#define	bBandSelect		0x1
+#define	bHTSIG2_GI		0x80
+#define	bHTSIG2_Smoothing	0x01
+#define	bHTSIG2_Sounding	0x02
+#define	bHTSIG2_Aggreaton	0x08
+#define	bHTSIG2_STBC		0x30
+#define	bHTSIG2_AdvCoding	0x40
+#define	bHTSIG2_NumOfHTLTF	0x300
+#define	bHTSIG2_CRC8		0x3fc
+#define	bHTSIG1_MCS		0x7f
+#define	bHTSIG1_BandWidth	0x80
+#define	bHTSIG1_HTLength	0xffff
+#define	bLSIG_Rate		0xf
+#define	bLSIG_Reserved		0x10
+#define	bLSIG_Length		0x1fffe
+#define	bLSIG_Parity		0x20
+#define	bCCKRxPhase		0x4
+#define	bLSSIReadAddress	0x7f800000   /* T65 RF */
+#define	bLSSIReadEdge		0x80000000   /* LSSI "Read" edge signal */
+#define	bLSSIReadBackData	0xfffff		/* T65 RF */
+#define	bLSSIReadOKFlag		0x1000	/* Useless now */
+#define	bCCKSampleRate		0x8       /*0: 44MHz, 1:88MHz*/
+#define	bRegulator0Standby	0x1
+#define	bRegulatorPLLStandby	0x2
+#define	bRegulator1Standby	0x4
+#define	bPLLPowerUp		0x8
+#define	bDPLLPowerUp		0x10
+#define	bDA10PowerUp		0x20
+#define	bAD7PowerUp		0x200
+#define	bDA6PowerUp		0x2000
+#define	bXtalPowerUp		0x4000
+#define	b40MDClkPowerUP		0x8000
+#define	bDA6DebugMode		0x20000
+#define	bDA6Swing		0x380000
+
+/* Reg 0x880 rFPGA0_AnalogParameter1 20/40 CCK support switch 40/80 BB MHZ */
+#define	bADClkPhase		0x4000000
+
+#define	b80MClkDelay		0x18000000	/* Useless */
+#define	bAFEWatchDogEnable	0x20000000
+
+/* Reg 0x884 rFPGA0_AnalogParameter2 Crystal cap */
+#define	bXtalCap01		0xc0000000
+#define	bXtalCap23		0x3
+#define	bXtalCap92x		0x0f000000
+#define bXtalCap		0x0f000000
+#define	bIntDifClkEnable	0x400	/* Useless */
+#define	bExtSigClkEnable	0x800
+#define	bBandgapMbiasPowerUp	0x10000
+#define	bAD11SHGain		0xc0000
+#define	bAD11InputRange		0x700000
+#define	bAD11OPCurrent		0x3800000
+#define	bIPathLoopback		0x4000000
+#define	bQPathLoopback		0x8000000
+#define	bAFELoopback		0x10000000
+#define	bDA10Swing		0x7e0
+#define	bDA10Reverse		0x800
+#define	bDAClkSource		0x1000
+#define	bAD7InputRange		0x6000
+#define	bAD7Gain		0x38000
+#define	bAD7OutputCMMode	0x40000
+#define	bAD7InputCMMode		0x380000
+#define	bAD7Current		0xc00000
+#define	bRegulatorAdjust	0x7000000
+#define	bAD11PowerUpAtTx	0x1
+#define	bDA10PSAtTx		0x10
+#define	bAD11PowerUpAtRx	0x100
+#define	bDA10PSAtRx		0x1000
+#define	bCCKRxAGCFormat		0x200
+#define	bPSDFFTSamplepPoint	0xc000
+#define	bPSDAverageNum		0x3000
+#define	bIQPathControl		0xc00
+#define	bPSDFreq		0x3ff
+#define	bPSDAntennaPath		0x30
+#define	bPSDIQSwitch		0x40
+#define	bPSDRxTrigger		0x400000
+#define	bPSDTxTrigger		0x80000000
+#define	bPSDSineToneScale	0x7f000000
+#define	bPSDReport		0xffff
+
+/* 3. Page9(0x900) */
+#define	bOFDMTxSC		0x30000000	/* Useless */
+#define	bCCKTxOn		0x1
+#define	bOFDMTxOn		0x2
+#define	bDebugPage		0xfff  /* reset debug page and HWord, LWord */
+#define	bDebugItem		0xff   /* reset debug page and LWord */
+#define	bAntL			0x10
+#define	bAntNonHT		0x100
+#define	bAntHT1			0x1000
+#define	bAntHT2			0x10000
+#define	bAntHT1S1		0x100000
+#define	bAntNonHTS1		0x1000000
+
+/* 4. PageA(0xA00) */
+#define	bCCKBBMode		0x3	/* Useless */
+#define	bCCKTxPowerSaving	0x80
+#define	bCCKRxPowerSaving	0x40
+
+#define	bCCKSideBand		0x10	/* Reg 0xa00 rCCK0_System 20/40 switch*/
+#define	bCCKScramble		0x8	/* Useless */
+#define	bCCKAntDiversity	0x8000
+#define	bCCKCarrierRecovery	0x4000
+#define	bCCKTxRate		0x3000
+#define	bCCKDCCancel		0x0800
+#define	bCCKISICancel		0x0400
+#define	bCCKMatchFilter		0x0200
+#define	bCCKEqualizer		0x0100
+#define	bCCKPreambleDetect	0x800000
+#define	bCCKFastFalseCCA	0x400000
+#define	bCCKChEstStart		0x300000
+#define	bCCKCCACount		0x080000
+#define	bCCKcs_lim		0x070000
+#define	bCCKBistMode		0x80000000
+#define	bCCKCCAMask		0x40000000
+#define	bCCKTxDACPhase		0x4
+#define	bCCKRxADCPhase		0x20000000   /* r_rx_clk */
+#define	bCCKr_cp_mode0		0x0100
+#define	bCCKTxDCOffset		0xf0
+#define	bCCKRxDCOffset		0xf
+#define	bCCKCCAMode		0xc000
+#define	bCCKFalseCS_lim		0x3f00
+#define	bCCKCS_ratio		0xc00000
+#define	bCCKCorgBit_sel		0x300000
+#define	bCCKPD_lim		0x0f0000
+#define	bCCKNewCCA		0x80000000
+#define	bCCKRxHPofIG		0x8000
+#define	bCCKRxIG		0x7f00
+#define	bCCKLNAPolarity		0x800000
+#define	bCCKRx1stGain		0x7f0000
+#define	bCCKRFExtend		0x20000000 /* CCK Rx initial gain polarity */
+#define	bCCKRxAGCSatLevel	0x1f000000
+#define	bCCKRxAGCSatCount       0xe0
+#define	bCCKRxRFSettle          0x1f       /* AGCsamp_dly */
+#define	bCCKFixedRxAGC          0x8000
+#define	bCCKAntennaPolarity     0x2000
+#define	bCCKTxFilterType        0x0c00
+#define	bCCKRxAGCReportType	0x0300
+#define	bCCKRxDAGCEn            0x80000000
+#define	bCCKRxDAGCPeriod        0x20000000
+#define	bCCKRxDAGCSatLevel	0x1f000000
+#define	bCCKTimingRecovery      0x800000
+#define	bCCKTxC0                0x3f0000
+#define	bCCKTxC1                0x3f000000
+#define	bCCKTxC2                0x3f
+#define	bCCKTxC3                0x3f00
+#define	bCCKTxC4                0x3f0000
+#define	bCCKTxC5		0x3f000000
+#define	bCCKTxC6		0x3f
+#define	bCCKTxC7		0x3f00
+#define	bCCKDebugPort		0xff0000
+#define	bCCKDACDebug		0x0f000000
+#define	bCCKFalseAlarmEnable	0x8000
+#define	bCCKFalseAlarmRead	0x4000
+#define	bCCKTRSSI		0x7f
+#define	bCCKRxAGCReport		0xfe
+#define	bCCKRxReport_AntSel	0x80000000
+#define	bCCKRxReport_MFOff	0x40000000
+#define	bCCKRxRxReport_SQLoss	0x20000000
+#define	bCCKRxReport_Pktloss	0x10000000
+#define	bCCKRxReport_Lockedbit	0x08000000
+#define	bCCKRxReport_RateError	0x04000000
+#define	bCCKRxReport_RxRate	0x03000000
+#define	bCCKRxFACounterLower	0xff
+#define	bCCKRxFACounterUpper	0xff000000
+#define	bCCKRxHPAGCStart	0xe000
+#define	bCCKRxHPAGCFinal	0x1c00
+#define	bCCKRxFalseAlarmEnable	0x8000
+#define	bCCKFACounterFreeze	0x4000
+#define	bCCKTxPathSel		0x10000000
+#define	bCCKDefaultRxPath	0xc000000
+#define	bCCKOptionRxPath	0x3000000
+
+/* 5. PageC(0xC00) */
+#define	bNumOfSTF		0x3	/* Useless */
+#define	bShift_L                0xc0
+#define	bGI_TH			0xc
+#define	bRxPathA		0x1
+#define	bRxPathB		0x2
+#define	bRxPathC		0x4
+#define	bRxPathD		0x8
+#define	bTxPathA		0x1
+#define	bTxPathB		0x2
+#define	bTxPathC		0x4
+#define	bTxPathD		0x8
+#define	bTRSSIFreq		0x200
+#define	bADCBackoff		0x3000
+#define	bDFIRBackoff		0xc000
+#define	bTRSSILatchPhase	0x10000
+#define	bRxIDCOffset		0xff
+#define	bRxQDCOffset		0xff00
+#define	bRxDFIRMode		0x1800000
+#define	bRxDCNFType		0xe000000
+#define	bRXIQImb_A		0x3ff
+#define	bRXIQImb_B		0xfc00
+#define	bRXIQImb_C		0x3f0000
+#define	bRXIQImb_D		0xffc00000
+#define	bDC_dc_Notch		0x60000
+#define	bRxNBINotch		0x1f000000
+#define	bPD_TH			0xf
+#define	bPD_TH_Opt2		0xc000
+#define	bPWED_TH		0x700
+#define	bIfMF_Win_L		0x800
+#define	bPD_Option		0x1000
+#define	bMF_Win_L		0xe000
+#define	bBW_Search_L		0x30000
+#define	bwin_enh_L		0xc0000
+#define	bBW_TH			0x700000
+#define	bED_TH2			0x3800000
+#define	bBW_option		0x4000000
+#define	bRatio_TH		0x18000000
+#define	bWindow_L		0xe0000000
+#define	bSBD_Option		0x1
+#define	bFrame_TH		0x1c
+#define	bFS_Option		0x60
+#define	bDC_Slope_check		0x80
+#define	bFGuard_Counter_DC_L	0xe00
+#define	bFrame_Weight_Short	0x7000
+#define	bSub_Tune		0xe00000
+#define	bFrame_DC_Length	0xe000000
+#define	bSBD_start_offset	0x30000000
+#define	bFrame_TH_2		0x7
+#define	bFrame_GI2_TH		0x38
+#define	bGI2_Sync_en		0x40
+#define	bSarch_Short_Early	0x300
+#define	bSarch_Short_Late	0xc00
+#define	bSarch_GI2_Late		0x70000
+#define	bCFOAntSum		0x1
+#define	bCFOAcc			0x2
+#define	bCFOStartOffset		0xc
+#define	bCFOLookBack		0x70
+#define	bCFOSumWeight		0x80
+#define	bDAGCEnable		0x10000
+#define	bTXIQImb_A		0x3ff
+#define	bTXIQImb_B		0xfc00
+#define	bTXIQImb_C		0x3f0000
+#define	bTXIQImb_D		0xffc00000
+#define	bTxIDCOffset		0xff
+#define	bTxQDCOffset		0xff00
+#define	bTxDFIRMode		0x10000
+#define	bTxPesudoNoiseOn	0x4000000
+#define	bTxPesudoNoise_A	0xff
+#define	bTxPesudoNoise_B	0xff00
+#define	bTxPesudoNoise_C	0xff0000
+#define	bTxPesudoNoise_D	0xff000000
+#define	bCCADropOption		0x20000
+#define	bCCADropThres		0xfff00000
+#define	bEDCCA_H		0xf
+#define	bEDCCA_L		0xf0
+#define	bLambda_ED              0x300
+#define	bRxInitialGain          0x7f
+#define	bRxAntDivEn             0x80
+#define	bRxAGCAddressForLNA     0x7f00
+#define	bRxHighPowerFlow        0x8000
+#define	bRxAGCFreezeThres       0xc0000
+#define	bRxFreezeStep_AGC1      0x300000
+#define	bRxFreezeStep_AGC2      0xc00000
+#define	bRxFreezeStep_AGC3      0x3000000
+#define	bRxFreezeStep_AGC0      0xc000000
+#define	bRxRssi_Cmp_En          0x10000000
+#define	bRxQuickAGCEn           0x20000000
+#define	bRxAGCFreezeThresMode   0x40000000
+#define	bRxOverFlowCheckType    0x80000000
+#define	bRxAGCShift             0x7f
+#define	bTRSW_Tri_Only          0x80
+#define	bPowerThres             0x300
+#define	bRxAGCEn                0x1
+#define	bRxAGCTogetherEn        0x2
+#define	bRxAGCMin               0x4
+#define	bRxHP_Ini               0x7
+#define	bRxHP_TRLNA             0x70
+#define	bRxHP_RSSI              0x700
+#define	bRxHP_BBP1              0x7000
+#define	bRxHP_BBP2              0x70000
+#define	bRxHP_BBP3              0x700000
+#define	bRSSI_H                 0x7f0000     /* the threshold for high power */
+#define	bRSSI_Gen               0x7f000000   /* the threshold for ant divers */
+#define	bRxSettle_TRSW          0x7
+#define	bRxSettle_LNA           0x38
+#define	bRxSettle_RSSI          0x1c0
+#define	bRxSettle_BBP           0xe00
+#define	bRxSettle_RxHP          0x7000
+#define	bRxSettle_AntSW_RSSI    0x38000
+#define	bRxSettle_AntSW         0xc0000
+#define	bRxProcessTime_DAGC     0x300000
+#define	bRxSettle_HSSI          0x400000
+#define	bRxProcessTime_BBPPW    0x800000
+#define	bRxAntennaPowerShift    0x3000000
+#define	bRSSITableSelect        0xc000000
+#define	bRxHP_Final             0x7000000
+#define	bRxHTSettle_BBP         0x7
+#define	bRxHTSettle_HSSI        0x8
+#define	bRxHTSettle_RxHP        0x70
+#define	bRxHTSettle_BBPPW       0x80
+#define	bRxHTSettle_Idle        0x300
+#define	bRxHTSettle_Reserved    0x1c00
+#define	bRxHTRxHPEn             0x8000
+#define	bRxHTAGCFreezeThres     0x30000
+#define	bRxHTAGCTogetherEn      0x40000
+#define	bRxHTAGCMin             0x80000
+#define	bRxHTAGCEn              0x100000
+#define	bRxHTDAGCEn             0x200000
+#define	bRxHTRxHP_BBP           0x1c00000
+#define	bRxHTRxHP_Final         0xe0000000
+#define	bRxPWRatioTH            0x3
+#define	bRxPWRatioEn            0x4
+#define	bRxMFHold               0x3800
+#define	bRxPD_Delay_TH1         0x38
+#define	bRxPD_Delay_TH2         0x1c0
+#define	bRxPD_DC_COUNT_MAX      0x600
+#define	bRxPD_Delay_TH          0x8000
+#define	bRxProcess_Delay        0xf0000
+#define	bRxSearchrange_GI2_Early 0x700000
+#define	bRxFrame_Guard_Counter_L 0x3800000
+#define	bRxSGI_Guard_L          0xc000000
+#define	bRxSGI_Search_L         0x30000000
+#define	bRxSGI_TH               0xc0000000
+#define	bDFSCnt0                0xff
+#define	bDFSCnt1                0xff00
+#define	bDFSFlag                0xf0000
+#define	bMFWeightSum            0x300000
+#define	bMinIdxTH               0x7f000000
+#define	bDAFormat               0x40000
+#define	bTxChEmuEnable          0x01000000
+#define	bTRSWIsolation_A        0x7f
+#define	bTRSWIsolation_B        0x7f00
+#define	bTRSWIsolation_C        0x7f0000
+#define	bTRSWIsolation_D        0x7f000000
+#define	bExtLNAGain             0x7c00
+
+/* 6. PageE(0xE00) */
+#define	bSTBCEn                 0x4	/* Useless */
+#define	bAntennaMapping         0x10
+#define	bNss                    0x20
+#define	bCFOAntSumD             0x200
+#define	bPHYCounterReset        0x8000000
+#define	bCFOReportGet           0x4000000
+#define	bOFDMContinueTx         0x10000000
+#define	bOFDMSingleCarrier      0x20000000
+#define	bOFDMSingleTone         0x40000000
+#define	bHTDetect               0x100
+#define	bCFOEn                  0x10000
+#define	bCFOValue               0xfff00000
+#define	bSigTone_Re             0x3f
+#define	bSigTone_Im             0x7f00
+#define	bCounter_CCA            0xffff
+#define	bCounter_ParityFail     0xffff0000
+#define	bCounter_RateIllegal    0xffff
+#define	bCounter_CRC8Fail       0xffff0000
+#define	bCounter_MCSNoSupport   0xffff
+#define	bCounter_FastSync       0xffff
+#define	bShortCFO               0xfff
+#define	bShortCFOTLength        12   /* total */
+#define	bShortCFOFLength        11   /* fraction */
+#define	bLongCFO                0x7ff
+#define	bLongCFOTLength         11
+#define	bLongCFOFLength         11
+#define	bTailCFO                0x1fff
+#define	bTailCFOTLength         13
+#define	bTailCFOFLength         12
+#define	bmax_en_pwdB            0xffff
+#define	bCC_power_dB            0xffff0000
+#define	bnoise_pwdB             0xffff
+#define	bPowerMeasTLength       10
+#define	bPowerMeasFLength       3
+#define	bRx_HT_BW               0x1
+#define	bRxSC                   0x6
+#define	bRx_HT                  0x8
+#define	bNB_intf_det_on         0x1
+#define	bIntf_win_len_cfg       0x30
+#define	bNB_Intf_TH_cfg         0x1c0
+#define	bRFGain                 0x3f
+#define	bTableSel               0x40
+#define	bTRSW                   0x80
+#define	bRxSNR_A                0xff
+#define	bRxSNR_B                0xff00
+#define	bRxSNR_C                0xff0000
+#define	bRxSNR_D                0xff000000
+#define	bSNREVMTLength          8
+#define	bSNREVMFLength          1
+#define	bCSI1st                 0xff
+#define	bCSI2nd                 0xff00
+#define	bRxEVM1st               0xff0000
+#define	bRxEVM2nd               0xff000000
+#define	bSIGEVM                 0xff
+#define	bPWDB                   0xff00
+#define	bSGIEN                  0x10000
+
+#define	bSFactorQAM1            0xf	/* Useless */
+#define	bSFactorQAM2            0xf0
+#define	bSFactorQAM3            0xf00
+#define	bSFactorQAM4            0xf000
+#define	bSFactorQAM5            0xf0000
+#define	bSFactorQAM6            0xf0000
+#define	bSFactorQAM7            0xf00000
+#define	bSFactorQAM8            0xf000000
+#define	bSFactorQAM9            0xf0000000
+#define	bCSIScheme              0x100000
+
+#define	bNoiseLvlTopSet         0x3	/* Useless */
+#define	bChSmooth               0x4
+#define	bChSmoothCfg1           0x38
+#define	bChSmoothCfg2           0x1c0
+#define	bChSmoothCfg3           0xe00
+#define	bChSmoothCfg4           0x7000
+#define	bMRCMode                0x800000
+#define	bTHEVMCfg               0x7000000
+
+#define	bLoopFitType            0x1	/* Useless */
+#define	bUpdCFO                 0x40
+#define	bUpdCFOOffData          0x80
+#define	bAdvUpdCFO              0x100
+#define	bAdvTimeCtrl            0x800
+#define	bUpdClko                0x1000
+#define	bFC                     0x6000
+#define	bTrackingMode           0x8000
+#define	bPhCmpEnable            0x10000
+#define	bUpdClkoLTF             0x20000
+#define	bComChCFO               0x40000
+#define	bCSIEstiMode            0x80000
+#define	bAdvUpdEqz              0x100000
+#define	bUChCfg                 0x7000000
+#define	bUpdEqz			0x8000000
+
+#define	bTxAGCRate18_06		0x7f7f7f7f	/* Useless */
+#define	bTxAGCRate54_24		0x7f7f7f7f
+#define	bTxAGCRateMCS32		0x7f
+#define	bTxAGCRateCCK		0x7f00
+#define	bTxAGCRateMCS3_MCS0	0x7f7f7f7f
+#define	bTxAGCRateMCS7_MCS4	0x7f7f7f7f
+#define	bTxAGCRateMCS11_MCS8	0x7f7f7f7f
+#define	bTxAGCRateMCS15_MCS12	0x7f7f7f7f
+
+/* Rx Pseduo noise */
+#define	bRxPesudoNoiseOn         0x20000000	/* Useless */
+#define	bRxPesudoNoise_A         0xff
+#define	bRxPesudoNoise_B         0xff00
+#define	bRxPesudoNoise_C         0xff0000
+#define	bRxPesudoNoise_D         0xff000000
+#define	bPesudoNoiseState_A      0xffff
+#define	bPesudoNoiseState_B      0xffff0000
+#define	bPesudoNoiseState_C      0xffff
+#define	bPesudoNoiseState_D      0xffff0000
+
+/* 7. RF Register
+ * Zebra1
+ */
+#define	bZebra1_HSSIEnable        0x8		/* Useless */
+#define	bZebra1_TRxControl        0xc00
+#define	bZebra1_TRxGainSetting    0x07f
+#define	bZebra1_RxCorner          0xc00
+#define	bZebra1_TxChargePump      0x38
+#define	bZebra1_RxChargePump      0x7
+#define	bZebra1_ChannelNum        0xf80
+#define	bZebra1_TxLPFBW           0x400
+#define	bZebra1_RxLPFBW           0x600
+
+/*Zebra4 */
+#define	bRTL8256RegModeCtrl1      0x100	/* Useless */
+#define	bRTL8256RegModeCtrl0      0x40
+#define	bRTL8256_TxLPFBW          0x18
+#define	bRTL8256_RxLPFBW          0x600
+
+/* RTL8258 */
+#define	bRTL8258_TxLPFBW          0xc	/* Useless */
+#define	bRTL8258_RxLPFBW          0xc00
+#define	bRTL8258_RSSILPFBW        0xc0
+
+/*
+ * Other Definition
+ */
+
+/* byte endable for sb_write */
+#define	bByte0                    0x1	/* Useless */
+#define	bByte1                    0x2
+#define	bByte2                    0x4
+#define	bByte3                    0x8
+#define	bWord0                    0x3
+#define	bWord1                    0xc
+#define	bDWord                    0xf
+
+/* for PutRegsetting & GetRegSetting BitMask */
+#define	bMaskByte0                0xff	/* Reg 0xc50 rOFDM0_XAAGCCore~0xC6f */
+#define	bMaskByte1                0xff00
+#define	bMaskByte2                0xff0000
+#define	bMaskByte3                0xff000000
+#define	bMaskHWord                0xffff0000
+#define	bMaskLWord                0x0000ffff
+#define	bMaskDWord                0xffffffff
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+#define	bRFRegOffsetMask	0xfffff
+#define	bEnable                   0x1	/* Useless */
+#define	bDisable                  0x0
+
+#define	LeftAntenna               0x0	/* Useless */
+#define	RightAntenna              0x1
+
+#define	tCheckTxStatus            500   /* 500ms Useless */
+#define	tUpdateRxCounter          100   /* 100ms */
+
+#define	rateCCK     0	/* Useless */
+#define	rateOFDM    1
+#define	rateHT      2
+
+/* define Register-End */
+#define	bPMAC_End       0x1ff	/* Useless */
+#define	bFPGAPHY0_End   0x8ff
+#define	bFPGAPHY1_End   0x9ff
+#define	bCCKPHY0_End    0xaff
+#define	bOFDMPHY0_End   0xcff
+#define	bOFDMPHY1_End   0xdff
+
+#define	bPMACControl	0x0	/* Useless */
+#define	bWMACControl	0x1
+#define	bWNICControl	0x2
+
+#define	ANTENNA_A	0x1	/* Useless */
+#define	ANTENNA_B	0x2
+#define	ANTENNA_AB	0x3	/* ANTENNA_A |ANTENNA_B */
+
+#define	ANTENNA_C	0x4
+#define	ANTENNA_D	0x8
+
+/* accept all physical address */
+#define RCR_AAP		BIT(0)
+#define RCR_APM		BIT(1)		/* accept physical match */
+#define RCR_AM		BIT(2)		/* accept multicast */
+#define RCR_AB		BIT(3)		/* accept broadcast */
+#define RCR_ACRC32	BIT(5)		/* accept error packet */
+#define RCR_9356SEL	BIT(6)
+#define RCR_AICV	BIT(12)		/* Accept ICV error packet */
+#define RCR_RXFTH0	(BIT(13)|BIT(14)|BIT(15))	/* Rx FIFO threshold */
+#define RCR_ADF		BIT(18)		/* Accept Data(frame type) frame */
+#define RCR_ACF		BIT(19)		/* Accept control frame */
+#define RCR_AMF		BIT(20)		/* Accept management frame */
+#define RCR_ADD3	BIT(21)
+#define RCR_APWRMGT	BIT(22)		/* Accept power management packet */
+#define RCR_CBSSID	BIT(23)		/* Accept BSSID match packet */
+#define RCR_ENMARP	BIT(28)		/* enable mac auto reset phy */
+#define RCR_EnCS1	BIT(29)		/* enable carrier sense method 1 */
+#define RCR_EnCS2	BIT(30)		/* enable carrier sense method 2 */
+/* Rx Early mode is performed for packet size greater than 1536 */
+#define RCR_OnlyErlPkt	BIT(31)
+
+/*--------------------------Define Parameters-------------------------------*/
+
+#endif	/*__INC_HAL8192SPHYREG_H */
+
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.c b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
new file mode 100644
index 000000000..cd6d9ff0b
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
@@ -0,0 +1,234 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_pwrctrl.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_PWRCTRL_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+
+#define RTL8712_SDIO_LOCAL_BASE 0X10100000
+#define SDIO_HCPWM (RTL8712_SDIO_LOCAL_BASE + 0x0081)
+
+void r8712_set_rpwm(struct _adapter *padapter, u8 val8)
+{
+	u8	rpwm;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	if (pwrpriv->rpwm == val8) {
+		if (pwrpriv->rpwm_retry == 0)
+			return;
+	}
+	if (padapter->driver_stopped || padapter->surprise_removed)
+		return;
+	rpwm = val8 | pwrpriv->tog;
+	switch (val8) {
+	case PS_STATE_S1:
+		pwrpriv->cpwm = val8;
+		break;
+	case PS_STATE_S2:/* only for USB normal powersave mode use,
+			  * temp mark some code.
+			  */
+	case PS_STATE_S3:
+	case PS_STATE_S4:
+		pwrpriv->cpwm = val8;
+		break;
+	default:
+		break;
+	}
+	pwrpriv->rpwm_retry = 0;
+	pwrpriv->rpwm = val8;
+	r8712_write8(padapter, 0x1025FE58, rpwm);
+	pwrpriv->tog += 0x80;
+}
+
+void r8712_set_ps_mode(struct _adapter *padapter, uint ps_mode, uint smart_ps)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	if (ps_mode > PM_Card_Disable)
+		return;
+	/* if driver is in active state, we dont need set smart_ps.*/
+	if (ps_mode == PS_MODE_ACTIVE)
+		smart_ps = 0;
+	if ((pwrpriv->pwr_mode != ps_mode) || (pwrpriv->smart_ps != smart_ps)) {
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			pwrpriv->bSleep = true;
+		else
+			pwrpriv->bSleep = false;
+		pwrpriv->pwr_mode = ps_mode;
+		pwrpriv->smart_ps = smart_ps;
+		schedule_work(&pwrpriv->SetPSModeWorkItem);
+	}
+}
+
+/*
+ * Caller:ISR handler...
+ *
+ * This will be called when CPWM interrupt is up.
+ *
+ * using to update cpwn of drv; and drv will make a decision to up or
+ * down pwr level
+ */
+void r8712_cpwm_int_hdl(struct _adapter *padapter,
+			struct reportpwrstate_parm *preportpwrstate)
+{
+	struct pwrctrl_priv *pwrpriv = &(padapter->pwrctrlpriv);
+	struct cmd_priv	*pcmdpriv = &(padapter->cmdpriv);
+
+	if (pwrpriv->cpwm_tog == ((preportpwrstate->state) & 0x80))
+		return;
+	del_timer(&padapter->pwrctrlpriv.rpwm_check_timer);
+	mutex_lock(&pwrpriv->mutex_lock);
+	pwrpriv->cpwm = (preportpwrstate->state) & 0xf;
+	if (pwrpriv->cpwm >= PS_STATE_S2) {
+		if (pwrpriv->alives & CMD_ALIVE)
+			complete(&(pcmdpriv->cmd_queue_comp));
+	}
+	pwrpriv->cpwm_tog = (preportpwrstate->state) & 0x80;
+	mutex_unlock(&pwrpriv->mutex_lock);
+}
+
+static inline void register_task_alive(struct pwrctrl_priv *pwrctrl, uint tag)
+{
+		pwrctrl->alives |= tag;
+}
+
+static inline void unregister_task_alive(struct pwrctrl_priv *pwrctrl, uint tag)
+{
+	if (pwrctrl->alives & tag)
+		pwrctrl->alives ^= tag;
+}
+
+static void _rpwm_check_handler (struct _adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	if (padapter->driver_stopped || padapter->surprise_removed)
+		return;
+	if (pwrpriv->cpwm != pwrpriv->rpwm)
+		schedule_work(&pwrpriv->rpwm_workitem);
+}
+
+static void SetPSModeWorkItemCallback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work,
+				       struct pwrctrl_priv, SetPSModeWorkItem);
+	struct _adapter *padapter = container_of(pwrpriv,
+				    struct _adapter, pwrctrlpriv);
+	if (!pwrpriv->bSleep) {
+		mutex_lock(&pwrpriv->mutex_lock);
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			r8712_set_rpwm(padapter, PS_STATE_S4);
+		mutex_unlock(&pwrpriv->mutex_lock);
+	}
+}
+
+static void rpwm_workitem_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work,
+				       struct pwrctrl_priv, rpwm_workitem);
+	struct _adapter *padapter = container_of(pwrpriv,
+				    struct _adapter, pwrctrlpriv);
+	if (pwrpriv->cpwm != pwrpriv->rpwm) {
+		mutex_lock(&pwrpriv->mutex_lock);
+		r8712_read8(padapter, SDIO_HCPWM);
+		pwrpriv->rpwm_retry = 1;
+		r8712_set_rpwm(padapter, pwrpriv->rpwm);
+		mutex_unlock(&pwrpriv->mutex_lock);
+	}
+}
+
+static void rpwm_check_handler (struct timer_list *t)
+{
+	struct _adapter *adapter =
+		from_timer(adapter, t, pwrctrlpriv.rpwm_check_timer);
+
+	_rpwm_check_handler(adapter);
+}
+
+void r8712_init_pwrctrl_priv(struct _adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv));
+	mutex_init(&pwrctrlpriv->mutex_lock);
+	pwrctrlpriv->cpwm = PS_STATE_S4;
+	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+	pwrctrlpriv->smart_ps = 0;
+	pwrctrlpriv->tog = 0x80;
+/* clear RPWM to ensure driver and fw back to initial state. */
+	r8712_write8(padapter, 0x1025FE58, 0);
+	INIT_WORK(&pwrctrlpriv->SetPSModeWorkItem, SetPSModeWorkItemCallback);
+	INIT_WORK(&pwrctrlpriv->rpwm_workitem, rpwm_workitem_callback);
+	timer_setup(&pwrctrlpriv->rpwm_check_timer, rpwm_check_handler, 0);
+}
+
+/*
+ * Caller: r8712_cmd_thread
+ * Check if the fw_pwrstate is okay for issuing cmd.
+ * If not (cpwm should be is less than P2 state), then the sub-routine
+ * will raise the cpwm to be greater than or equal to P2.
+ * Calling Context: Passive
+ * Return Value:
+ * 0:	    r8712_cmd_thread can issue cmds to firmware afterwards.
+ * -EINVAL: r8712_cmd_thread can not do anything.
+ */
+int r8712_register_cmd_alive(struct _adapter *padapter)
+{
+	int res = 0;
+	struct pwrctrl_priv *pwrctrl = &padapter->pwrctrlpriv;
+
+	mutex_lock(&pwrctrl->mutex_lock);
+	register_task_alive(pwrctrl, CMD_ALIVE);
+	if (pwrctrl->cpwm < PS_STATE_S2) {
+		r8712_set_rpwm(padapter, PS_STATE_S3);
+		res = -EINVAL;
+	}
+	mutex_unlock(&pwrctrl->mutex_lock);
+	return res;
+}
+
+/*
+ * Caller: ISR
+ * If ISR's txdone,
+ * No more pkts for TX,
+ * Then driver shall call this fun. to power down firmware again.
+ */
+void r8712_unregister_cmd_alive(struct _adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl = &padapter->pwrctrlpriv;
+
+	mutex_lock(&pwrctrl->mutex_lock);
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+	if ((pwrctrl->cpwm > PS_STATE_S2) &&
+	   (pwrctrl->pwr_mode > PS_MODE_ACTIVE)) {
+		if ((pwrctrl->alives == 0) &&
+		    (check_fwstate(&padapter->mlmepriv,
+		     _FW_UNDER_LINKING) != true)) {
+			r8712_set_rpwm(padapter, PS_STATE_S0);
+		}
+	}
+	mutex_unlock(&pwrctrl->mutex_lock);
+}
+
+void r8712_flush_rwctrl_works(struct _adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl = &padapter->pwrctrlpriv;
+
+	flush_work(&pwrctrl->SetPSModeWorkItem);
+	flush_work(&pwrctrl->rpwm_workitem);
+}
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.h b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
new file mode 100644
index 000000000..b35b9c792
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
@@ -0,0 +1,113 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_PWRCTRL_H_
+#define __RTL871X_PWRCTRL_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define CMD_ALIVE	BIT(2)
+
+enum Power_Mgnt {
+	PS_MODE_ACTIVE	= 0,
+	PS_MODE_MIN,
+	PS_MODE_MAX,
+	PS_MODE_DTIM,
+	PS_MODE_VOIP,
+	PS_MODE_UAPSD_WMM,
+	PS_MODE_UAPSD,
+	PS_MODE_IBSS,
+	PS_MODE_WWLAN,
+	PM_Radio_Off,
+	PM_Card_Disable,
+	PS_MODE_NUM
+};
+
+/*
+ * BIT[2:0] = HW state
+ * BIT[3] = Protocol PS state, 0: register active state,
+ *				1: register sleep state
+ * BIT[4] = sub-state
+ */
+
+#define		PS_DPS				BIT(0)
+#define		PS_LCLK				(PS_DPS)
+#define	PS_RF_OFF			BIT(1)
+#define	PS_ALL_ON			BIT(2)
+#define	PS_ST_ACTIVE		BIT(3)
+#define	PS_LP				BIT(4)	/* low performance */
+
+#define	PS_STATE_MASK		(0x0F)
+#define	PS_STATE_HW_MASK	(0x07)
+#define		PS_SEQ_MASK		(0xc0)
+
+#define	PS_STATE(x)			(PS_STATE_MASK & (x))
+#define	PS_STATE_HW(x)	(PS_STATE_HW_MASK & (x))
+#define	PS_SEQ(x)			(PS_SEQ_MASK & (x))
+
+#define	PS_STATE_S0		(PS_DPS)
+#define		PS_STATE_S1		(PS_LCLK)
+#define	PS_STATE_S2		(PS_RF_OFF)
+#define		PS_STATE_S3		(PS_ALL_ON)
+#define	PS_STATE_S4		((PS_ST_ACTIVE) | (PS_ALL_ON))
+
+#define		PS_IS_RF_ON(x)		((x) & (PS_ALL_ON))
+#define		PS_IS_ACTIVE(x)		((x) & (PS_ST_ACTIVE))
+#define		CLR_PS_STATE(x)	((x) = ((x) & (0xF0)))
+
+struct reportpwrstate_parm {
+	unsigned char mode;
+	unsigned char state; /* the CPWM value */
+	unsigned short rsvd;
+};
+
+struct	pwrctrl_priv {
+	struct mutex mutex_lock;
+	/*volatile*/ u8 rpwm; /* requested power state for fw */
+	/* fw current power state. updated when 1. read from HCPWM or
+	 * 2. driver lowers power level
+	 */
+	/*volatile*/ u8 cpwm;
+	/*volatile*/ u8 tog; /* toggling */
+	/*volatile*/ u8 cpwm_tog; /* toggling */
+	/*volatile*/ u8 tgt_rpwm; /* wanted power state */
+	uint pwr_mode;
+	uint smart_ps;
+	uint alives;
+	uint ImrContent;	/* used to store original imr. */
+	uint bSleep; /* sleep -> active is different from active -> sleep. */
+
+	struct work_struct SetPSModeWorkItem;
+	struct work_struct rpwm_workitem;
+	struct timer_list rpwm_check_timer;
+	u8	rpwm_retry;
+	uint	bSetPSModeWorkItemInProgress;
+
+	spinlock_t pnp_pwr_mgnt_lock;
+	s32	pnp_current_pwr_state;
+	u8	pnp_bstop_trx;
+	u8	pnp_wwirp_pending;
+};
+
+void r8712_init_pwrctrl_priv(struct _adapter *adapter);
+int r8712_register_cmd_alive(struct _adapter *padapter);
+void r8712_unregister_cmd_alive(struct _adapter *padapter);
+void r8712_cpwm_int_hdl(struct _adapter *padapter,
+			struct reportpwrstate_parm *preportpwrstate);
+void r8712_set_ps_mode(struct _adapter *padapter, uint ps_mode,
+			uint smart_ps);
+void r8712_set_rpwm(struct _adapter *padapter, u8 val8);
+void r8712_flush_rwctrl_works(struct _adapter *padapter);
+
+#endif  /* __RTL871X_PWRCTRL_H_ */
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
new file mode 100644
index 000000000..de9a568ea
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -0,0 +1,669 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_recv.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_RECV_C_
+
+#include <linux/ip.h>
+#include <linux/slab.h>
+#include <linux/if_ether.h>
+#include <linux/kmemleak.h>
+#include <linux/etherdevice.h>
+#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "mlme_osdep.h"
+#include "ethernet.h"
+#include "usb_ops.h"
+#include "wifi.h"
+
+static const u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+
+/* Datagram Delivery Protocol */
+static const u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+void _r8712_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+	memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv));
+	spin_lock_init(&psta_recvpriv->lock);
+	_init_queue(&psta_recvpriv->defrag_q);
+}
+
+void _r8712_init_recv_priv(struct recv_priv *precvpriv,
+			   struct _adapter *padapter)
+{
+	sint i;
+	union recv_frame *precvframe;
+
+	memset((unsigned char *)precvpriv, 0, sizeof(struct  recv_priv));
+	spin_lock_init(&precvpriv->lock);
+	_init_queue(&precvpriv->free_recv_queue);
+	_init_queue(&precvpriv->recv_pending_queue);
+	precvpriv->adapter = padapter;
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+	precvpriv->pallocated_frame_buf = kzalloc(NR_RECVFRAME *
+				sizeof(union recv_frame) + RXFRAME_ALIGN_SZ,
+				GFP_ATOMIC);
+	if (!precvpriv->pallocated_frame_buf)
+		return;
+	kmemleak_not_leak(precvpriv->pallocated_frame_buf);
+	precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf +
+				    RXFRAME_ALIGN_SZ -
+				    ((addr_t)(precvpriv->pallocated_frame_buf) &
+				    (RXFRAME_ALIGN_SZ - 1));
+	precvframe = (union recv_frame *)precvpriv->precv_frame_buf;
+	for (i = 0; i < NR_RECVFRAME; i++) {
+		INIT_LIST_HEAD(&(precvframe->u.list));
+		list_add_tail(&(precvframe->u.list),
+			      &(precvpriv->free_recv_queue.queue));
+		r8712_os_recv_resource_alloc(padapter, precvframe);
+		precvframe->u.hdr.adapter = padapter;
+		precvframe++;
+	}
+	precvpriv->rx_pending_cnt = 1;
+	r8712_init_recv_priv(precvpriv, padapter);
+}
+
+void _r8712_free_recv_priv(struct recv_priv *precvpriv)
+{
+	kfree(precvpriv->pallocated_frame_buf);
+	r8712_free_recv_priv(precvpriv);
+}
+
+union recv_frame *r8712_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+	unsigned long irqL;
+	union recv_frame  *precvframe;
+	struct _adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	spin_lock_irqsave(&pfree_recv_queue->lock, irqL);
+	precvframe = list_first_entry_or_null(&pfree_recv_queue->queue,
+					      union recv_frame, u.hdr.list);
+	if (precvframe) {
+		list_del_init(&precvframe->u.hdr.list);
+		padapter = precvframe->u.hdr.adapter;
+		if (padapter) {
+			precvpriv = &padapter->recvpriv;
+			if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt--;
+		}
+	}
+	spin_unlock_irqrestore(&pfree_recv_queue->lock, irqL);
+	return precvframe;
+}
+
+/*
+ * caller : defrag; recvframe_chk_defrag in recv_thread  (passive)
+ * pframequeue: defrag_queue : will be accessed in recv_thread  (passive)
+ * using spin_lock to protect
+ */
+void r8712_free_recvframe_queue(struct  __queue *pframequeue,
+				struct  __queue *pfree_recv_queue)
+{
+	union	recv_frame *precvframe;
+	struct list_head *plist, *phead;
+
+	spin_lock(&pframequeue->lock);
+	phead = &pframequeue->queue;
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist)) {
+		precvframe = container_of(plist, union recv_frame, u.list);
+		plist = plist->next;
+		r8712_free_recvframe(precvframe, pfree_recv_queue);
+	}
+	spin_unlock(&pframequeue->lock);
+}
+
+sint r8712_recvframe_chkmic(struct _adapter *adapter,
+			    union recv_frame *precvframe)
+{
+	sint i, res = _SUCCESS;
+	u32	datalen;
+	u8 miccode[8];
+	u8 bmic_err = false;
+	u8 *pframe, *payload, *pframemic;
+	u8   *mickey, idx, *iv;
+	struct	sta_info *stainfo;
+	struct	rx_pkt_attrib *prxattrib = &precvframe->u.hdr.attrib;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+
+	stainfo = r8712_get_stainfo(&adapter->stapriv, &prxattrib->ta[0]);
+	if (prxattrib->encrypt == _TKIP_) {
+		/* calculate mic code */
+		if (stainfo) {
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				iv = precvframe->u.hdr.rx_data +
+				     prxattrib->hdrlen;
+				idx = iv[3];
+				mickey = &psecuritypriv->XGrprxmickey[(((idx >>
+					 6) & 0x3)) - 1].skey[0];
+				if (!psecuritypriv->binstallGrpkey)
+					return _FAIL;
+			} else {
+				mickey = &stainfo->tkiprxmickey.skey[0];
+			}
+			/*icv_len included the mic code*/
+			datalen = precvframe->u.hdr.len - prxattrib->hdrlen -
+				  prxattrib->iv_len - prxattrib->icv_len - 8;
+			pframe = precvframe->u.hdr.rx_data;
+			payload = pframe + prxattrib->hdrlen +
+				  prxattrib->iv_len;
+			seccalctkipmic(mickey, pframe, payload, datalen,
+				       &miccode[0],
+				       (unsigned char)prxattrib->priority);
+			pframemic = payload + datalen;
+			bmic_err = false;
+			for (i = 0; i < 8; i++) {
+				if (miccode[i] != *(pframemic + i))
+					bmic_err = true;
+			}
+			if (bmic_err) {
+				if (prxattrib->bdecrypted)
+					r8712_handle_tkip_mic_err(adapter,
+								  (u8)is_multicast_ether_addr(prxattrib->ra));
+				res = _FAIL;
+			} else {
+				/* mic checked ok */
+				if (!psecuritypriv->bcheck_grpkey &&
+				    is_multicast_ether_addr(prxattrib->ra))
+					psecuritypriv->bcheck_grpkey = true;
+			}
+			recvframe_pull_tail(precvframe, 8);
+		}
+	}
+	return res;
+}
+
+/* decrypt and set the ivlen,icvlen of the recv_frame */
+union recv_frame *r8712_decryptor(struct _adapter *padapter,
+				  union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	union recv_frame *return_packet = precv_frame;
+
+	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) ||
+					 psecuritypriv->sw_decrypt)) {
+		psecuritypriv->hw_decrypted = false;
+		switch (prxattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			r8712_wep_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _TKIP_:
+			r8712_tkip_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _AES_:
+			r8712_aes_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		default:
+				break;
+		}
+	} else if (prxattrib->bdecrypted == 1) {
+		psecuritypriv->hw_decrypted = true;
+	}
+	return return_packet;
+}
+
+/*###set the security information in the recv_frame */
+union recv_frame *r8712_portctrl(struct _adapter *adapter,
+				 union recv_frame *precv_frame)
+{
+	u8 *psta_addr, *ptr;
+	uint auth_alg;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct	sta_priv *pstapriv;
+	union recv_frame *prtnframe;
+	u16 ether_type;
+
+	pstapriv = &adapter->stapriv;
+	ptr = precv_frame->u.hdr.rx_data;
+	pfhdr = &precv_frame->u.hdr;
+	psta_addr = pfhdr->attrib.ta;
+	psta = r8712_get_stainfo(pstapriv, psta_addr);
+	auth_alg = adapter->securitypriv.AuthAlgrthm;
+	if (auth_alg == 2) {
+		/* get ether_type */
+		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
+		ether_type = get_unaligned_be16(ptr);
+
+		if (psta && psta->ieee8021x_blocked) {
+			/* blocked
+			 * only accept EAPOL frame
+			 */
+			if (ether_type == 0x888e) {
+				prtnframe = precv_frame;
+			} else {
+				/*free this frame*/
+				r8712_free_recvframe(precv_frame,
+						     &adapter->recvpriv.free_recv_queue);
+				prtnframe = NULL;
+			}
+		} else {
+			/* allowed
+			 * check decryption status, and decrypt the
+			 * frame if needed
+			 */
+			prtnframe = precv_frame;
+			/* check is the EAPOL frame or not (Rekey) */
+			if (ether_type == 0x888e) {
+				/* check Rekey */
+				prtnframe = precv_frame;
+			}
+		}
+	} else {
+		prtnframe = precv_frame;
+	}
+	return prtnframe;
+}
+
+static sint recv_decache(union recv_frame *precv_frame, u8 bretry,
+			 struct stainfo_rxcache *prxcache)
+{
+	sint tid = precv_frame->u.hdr.attrib.priority;
+	u16 seq_ctrl = ((precv_frame->u.hdr.attrib.seq_num & 0xffff) << 4) |
+			(precv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (tid > 15)
+		return _FAIL;
+	if (seq_ctrl == prxcache->tid_rxseq[tid])
+		return _FAIL;
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+	return _SUCCESS;
+}
+
+static sint sta2sta_data_frame(struct _adapter *adapter,
+			       union recv_frame *precv_frame,
+			       struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	sint ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	u8 *sta_addr = NULL;
+	bool bmcast = is_multicast_ether_addr(pattrib->dst);
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		/* filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN))
+			return _FAIL;
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast))
+			return _FAIL;
+		if (is_zero_ether_addr(pattrib->bssid) ||
+		    is_zero_ether_addr(mybssid) ||
+		    (memcmp(pattrib->bssid, mybssid, ETH_ALEN)))
+			return _FAIL;
+		sta_addr = pattrib->src;
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		/* For Station mode, sa and bssid should always be BSSID,
+		 * and DA is my mac-address
+		 */
+		if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN))
+			return _FAIL;
+		sta_addr = pattrib->bssid;
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		if (bmcast) {
+			/* For AP mode, if DA == MCAST, then BSSID should
+			 * be also MCAST
+			 */
+			if (!is_multicast_ether_addr(pattrib->bssid))
+				return _FAIL;
+		} else { /* not mc-frame */
+			/* For AP mode, if DA is non-MCAST, then it must be
+			 * BSSID, and bssid == BSSID
+			 */
+			if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN))
+				return _FAIL;
+			sta_addr = pattrib->src;
+		}
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+		sta_addr = mybssid;
+	} else {
+		ret  = _FAIL;
+	}
+	if (bmcast)
+		*psta = r8712_get_bcmc_stainfo(adapter);
+	else
+		*psta = r8712_get_stainfo(pstapriv, sta_addr); /* get ap_info */
+	if (!*psta) {
+		if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+			adapter->mppriv.rx_pktloss++;
+		return _FAIL;
+	}
+	return ret;
+}
+
+static sint ap2sta_data_frame(struct _adapter *adapter,
+			      union recv_frame *precv_frame,
+			      struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	bool bmcast = is_multicast_ether_addr(pattrib->dst);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_LINKED)) {
+		/* if NULL-frame, drop packet */
+		if ((GetFrameSubType(ptr)) == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC))
+			return _FAIL;
+		/* drop QoS-SubType Data, including QoS NULL,
+		 * excluding QoS-Data
+		 */
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) ==
+		     WIFI_QOS_DATA_TYPE) {
+			if (GetFrameSubType(ptr) & (BIT(4) | BIT(5) | BIT(6)))
+				return _FAIL;
+		}
+
+		/* filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN))
+			return _FAIL;
+
+		/* da should be for me */
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast))
+			return _FAIL;
+		/* check BSSID */
+		if (is_zero_ether_addr(pattrib->bssid) ||
+		    is_zero_ether_addr(mybssid) ||
+		     (memcmp(pattrib->bssid, mybssid, ETH_ALEN)))
+			return _FAIL;
+		if (bmcast)
+			*psta = r8712_get_bcmc_stainfo(adapter);
+		else
+			*psta = r8712_get_stainfo(pstapriv, pattrib->bssid);
+		if (!*psta)
+			return _FAIL;
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+		   check_fwstate(pmlmepriv, _FW_LINKED)) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+		memcpy(pattrib->bssid,  mybssid, ETH_ALEN);
+		*psta = r8712_get_stainfo(pstapriv, pattrib->bssid);
+		if (!*psta)
+			return _FAIL;
+	} else {
+		return _FAIL;
+	}
+	return _SUCCESS;
+}
+
+static sint sta2ap_data_frame(struct _adapter *adapter,
+			      union recv_frame *precv_frame,
+			      struct sta_info **psta)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	unsigned char *mybssid  = get_bssid(pmlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* For AP mode, if DA is non-MCAST, then it must be BSSID,
+		 * and bssid == BSSID
+		 * For AP mode, RA=BSSID, TX=STA(SRC_ADDR), A3=DST_ADDR
+		 */
+		if (memcmp(pattrib->bssid, mybssid, ETH_ALEN))
+			return _FAIL;
+		*psta = r8712_get_stainfo(pstapriv, pattrib->src);
+		if (!*psta)
+			return _FAIL;
+	}
+	return _SUCCESS;
+}
+
+static sint validate_recv_ctrl_frame(struct _adapter *adapter,
+				     union recv_frame *precv_frame)
+{
+	return _FAIL;
+}
+
+static sint validate_recv_mgnt_frame(struct _adapter *adapter,
+				     union recv_frame *precv_frame)
+{
+	return _FAIL;
+}
+
+static sint validate_recv_data_frame(struct _adapter *adapter,
+				     union recv_frame *precv_frame)
+{
+	int res;
+	u8 bretry;
+	u8 *psa, *pda, *pbssid;
+	struct sta_info *psta = NULL;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	bretry = GetRetry(ptr);
+	pda = ieee80211_get_DA((struct ieee80211_hdr *)ptr);
+	psa = ieee80211_get_SA((struct ieee80211_hdr *)ptr);
+	pbssid = get_hdr_bssid(ptr);
+	if (!pbssid)
+		return _FAIL;
+	memcpy(pattrib->dst, pda, ETH_ALEN);
+	memcpy(pattrib->src, psa, ETH_ALEN);
+	memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+	switch (pattrib->to_fr_ds) {
+	case 0:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		res = sta2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+	case 1:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, pbssid, ETH_ALEN);
+		res = ap2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+	case 2:
+		memcpy(pattrib->ra, pbssid, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		res = sta2ap_data_frame(adapter, precv_frame, &psta);
+		break;
+	case 3:
+		memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+		return _FAIL;
+	default:
+		return _FAIL;
+	}
+	if (res == _FAIL)
+		return _FAIL;
+	if (!psta)
+		return _FAIL;
+	precv_frame->u.hdr.psta = psta;
+	pattrib->amsdu = 0;
+	/* parsing QC field */
+	if (pattrib->qos == 1) {
+		pattrib->priority = GetPriority((ptr + 24));
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26;
+	} else {
+		pattrib->priority = 0;
+		pattrib->hdrlen = (pattrib->to_fr_ds == 3) ? 30 : 24;
+	}
+
+	if (pattrib->order)/*HT-CTRL 11n*/
+		pattrib->hdrlen += 4;
+	precv_frame->u.hdr.preorder_ctrl =
+			 &psta->recvreorder_ctrl[pattrib->priority];
+
+	/* decache, drop duplicate recv packets */
+	if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) ==
+	    _FAIL)
+		return _FAIL;
+
+	if (pattrib->privacy) {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt,
+			       is_multicast_ether_addr(pattrib->ra));
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len,
+			       pattrib->encrypt);
+	} else {
+		pattrib->encrypt = 0;
+		pattrib->iv_len = pattrib->icv_len = 0;
+	}
+	return _SUCCESS;
+}
+
+sint r8712_validate_recv_frame(struct _adapter *adapter,
+			       union recv_frame *precv_frame)
+{
+	/*shall check frame subtype, to / from ds, da, bssid */
+	/*then call check if rx seq/frag. duplicated.*/
+
+	u8 type;
+	u8 subtype;
+	sint retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8  ver = (unsigned char)(*ptr) & 0x3;
+
+	/*add version chk*/
+	if (ver != 0)
+		return _FAIL;
+	type =  GetFrameType(ptr);
+	subtype = GetFrameSubType(ptr); /*bit(7)~bit(2)*/
+	pattrib->to_fr_ds = get_tofr_ds(ptr);
+	pattrib->frag_num = GetFragNum(ptr);
+	pattrib->seq_num = GetSequence(ptr);
+	pattrib->pw_save = GetPwrMgt(ptr);
+	pattrib->mfrag = GetMFrag(ptr);
+	pattrib->mdata = GetMData(ptr);
+	pattrib->privacy =  GetPrivacy(ptr);
+	pattrib->order = GetOrder(ptr);
+	switch (type) {
+	case IEEE80211_FTYPE_MGMT:
+		retval = validate_recv_mgnt_frame(adapter, precv_frame);
+		break;
+	case IEEE80211_FTYPE_CTL:
+		retval = validate_recv_ctrl_frame(adapter, precv_frame);
+		break;
+	case IEEE80211_FTYPE_DATA:
+		pattrib->qos = (subtype & BIT(7)) ? 1 : 0;
+		retval = validate_recv_data_frame(adapter, precv_frame);
+		break;
+	default:
+		return _FAIL;
+	}
+	return retval;
+}
+
+int r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	/*remove the wlanhdr and add the eth_hdr*/
+	sint	rmv_len;
+	u16	len;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	struct ieee80211_snap_hdr *psnap;
+	struct _adapter	*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	u8 *ptr = precvframe->u.hdr.rx_data; /*point to frame_ctrl field*/
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+
+	if (pattrib->encrypt)
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+	psnap = (struct ieee80211_snap_hdr *)(ptr + pattrib->hdrlen +
+		 pattrib->iv_len);
+	psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	if ((!memcmp(psnap, (void *)rfc1042_header, SNAP_SIZE) &&
+	     (memcmp(psnap_type, (void *)SNAP_ETH_TYPE_IPX, 2)) &&
+	    (memcmp(psnap_type, (void *)SNAP_ETH_TYPE_APPLETALK_AARP, 2))) ||
+	     !memcmp(psnap, (void *)bridge_tunnel_header, SNAP_SIZE)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and
+		 * replace EtherType
+		 */
+		bsnaphdr = true;
+	} else {
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = false;
+	}
+	rmv_len = pattrib->hdrlen + pattrib->iv_len +
+		  (bsnaphdr ? SNAP_SIZE : 0);
+	len = precvframe->u.hdr.len - rmv_len;
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		ptr += rmv_len;
+		*ptr = 0x87;
+		*(ptr + 1) = 0x12;
+		/* append rx status for mp test packets */
+		ptr = recvframe_pull(precvframe, (rmv_len -
+		      sizeof(struct ethhdr) + 2) - 24);
+		if (!ptr)
+			return -ENOMEM;
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else {
+		ptr = recvframe_pull(precvframe, (rmv_len -
+		      sizeof(struct ethhdr) + (bsnaphdr ? 2 : 0)));
+		if (!ptr)
+			return -ENOMEM;
+	}
+
+	memcpy(ptr, pattrib->dst, ETH_ALEN);
+	memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN);
+	if (!bsnaphdr) {
+		__be16 be_tmp = htons(len);
+
+		memcpy(ptr + 12, &be_tmp, 2);
+	}
+	return 0;
+}
+
+void r8712_recv_entry(union recv_frame *precvframe)
+{
+	struct _adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	s32 ret = _SUCCESS;
+
+	padapter = precvframe->u.hdr.adapter;
+	precvpriv = &(padapter->recvpriv);
+
+	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_RX);
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL)
+		goto _recv_entry_drop;
+	precvpriv->rx_pkts++;
+	precvpriv->rx_bytes += (uint)(precvframe->u.hdr.rx_tail -
+				precvframe->u.hdr.rx_data);
+	return;
+_recv_entry_drop:
+	precvpriv->rx_drop++;
+	padapter->mppriv.rx_pktloss = precvpriv->rx_drop;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_recv.h b/drivers/staging/rtl8712/rtl871x_recv.h
new file mode 100644
index 000000000..0760bccbf
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_recv.h
@@ -0,0 +1,208 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _RTL871X_RECV_H_
+#define _RTL871X_RECV_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define NR_RECVFRAME 256
+
+#define RXFRAME_ALIGN	8
+#define RXFRAME_ALIGN_SZ	(1 << RXFRAME_ALIGN)
+
+#define MAX_SUBFRAME_COUNT	64
+
+/* for Rx reordering buffer control */
+struct recv_reorder_ctrl {
+	struct _adapter	*padapter;
+	u16 indicate_seq; /* =wstart_b, init_value=0xffff */
+	u16 wend_b;
+	u8 wsize_b;
+	struct  __queue pending_recvframe_queue;
+	struct timer_list reordering_ctrl_timer;
+};
+
+struct	stainfo_rxcache	{
+	u16	tid_rxseq[16];
+};
+
+#define		PHY_RSSI_SLID_WIN_MAX			100
+#define		PHY_LINKQUALITY_SLID_WIN_MAX		20
+
+struct smooth_rssi_data {
+	u32	elements[100];	/* array to store values */
+	u32	index;		/* index to current array to store */
+	u32	total_num;	/* num of valid elements */
+	u32	total_val;	/* sum of valid elements */
+};
+
+struct rx_pkt_attrib {
+	u8	amsdu;
+	u8	order;
+	u8	qos;
+	u8	to_fr_ds;
+	u8	frag_num;
+	u16	seq_num;
+	u8   pw_save;
+	u8    mfrag;
+	u8    mdata;
+	u8	privacy; /* in frame_ctrl field */
+	u8	bdecrypted;
+	int	hdrlen;	 /* the WLAN Header Len */
+	int	encrypt; /* 0 no encrypt. != 0 encrypt algorithm */
+	int	iv_len;
+	int	icv_len;
+	int	priority;
+	int	ack_policy;
+	u8	crc_err;
+	u8	dst[ETH_ALEN];
+	u8	src[ETH_ALEN];
+	u8	ta[ETH_ALEN];
+	u8	ra[ETH_ALEN];
+	u8	bssid[ETH_ALEN];
+	u8	tcpchk_valid; /* 0: invalid, 1: valid */
+	u8	ip_chkrpt; /* 0: incorrect, 1: correct */
+	u8	tcp_chkrpt; /* 0: incorrect, 1: correct */
+	u8	signal_qual;
+	s8	rx_mimo_signal_qual[2];
+	u8	mcs_rate;
+	u8	htc;
+	u8	signal_strength;
+};
+
+/*
+ * accesser of recv_priv: recv_entry(dispatch / passive level);
+ * recv_thread(passive) ; returnpkt(dispatch)
+ * ; halt(passive) ;
+ *
+ * using enter_critical section to protect
+ */
+struct recv_priv {
+	spinlock_t lock;
+	struct  __queue	free_recv_queue;
+	struct  __queue	recv_pending_queue;
+	u8 *pallocated_frame_buf;
+	u8 *precv_frame_buf;
+	uint free_recvframe_cnt;
+	struct _adapter	*adapter;
+	uint	rx_bytes;
+	uint	rx_pkts;
+	uint	rx_drop;
+	uint  rx_icv_err;
+	uint  rx_largepacket_crcerr;
+	uint  rx_smallpacket_crcerr;
+	uint  rx_middlepacket_crcerr;
+	u8  rx_pending_cnt;
+	uint	ff_hwaddr;
+	struct tasklet_struct recv_tasklet;
+	struct sk_buff_head free_recv_skb_queue;
+	struct sk_buff_head rx_skb_queue;
+	u8 *pallocated_recv_buf;
+	u8 *precv_buf;    /* 4 alignment */
+	struct  __queue	free_recv_buf_queue;
+	u32	free_recv_buf_queue_cnt;
+	/* For the phy information */
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u8 fw_rssi;
+	struct smooth_rssi_data signal_qual_data;
+	struct smooth_rssi_data signal_strength_data;
+};
+
+struct sta_recv_priv {
+	spinlock_t lock;
+	sint	option;
+	struct  __queue defrag_q; /* keeping the fragment frame until defrag */
+	struct	stainfo_rxcache rxcache;
+	uint	sta_rx_bytes;
+	uint	sta_rx_pkts;
+	uint	sta_rx_fail;
+};
+
+#include "rtl8712_recv.h"
+
+/* get a free recv_frame from pfree_recv_queue */
+union recv_frame *r8712_alloc_recvframe(struct  __queue *pfree_recv_queue);
+void r8712_free_recvframe(union recv_frame *precvframe,
+			  struct  __queue *pfree_recv_queue);
+void r8712_free_recvframe_queue(struct  __queue *pframequeue,
+				 struct  __queue *pfree_recv_queue);
+int r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe);
+int recv_func(struct _adapter *padapter, void *pcontext);
+
+static inline u8 *get_rxmem(union recv_frame *precvframe)
+{
+	/* always return rx_head... */
+	if (!precvframe)
+		return NULL;
+	return precvframe->u.hdr.rx_head;
+}
+
+static inline u8 *recvframe_pull(union recv_frame *precvframe, sint sz)
+{
+	/* used for extract sz bytes from rx_data, update rx_data and return
+	 * the updated rx_data to the caller
+	 */
+	if (!precvframe)
+		return NULL;
+	precvframe->u.hdr.rx_data += sz;
+	if (precvframe->u.hdr.rx_data > precvframe->u.hdr.rx_tail) {
+		precvframe->u.hdr.rx_data -= sz;
+		return NULL;
+	}
+	precvframe->u.hdr.len -= sz;
+	return precvframe->u.hdr.rx_data;
+}
+
+static inline u8 *recvframe_put(union recv_frame *precvframe, sint sz)
+{
+	/* used for append sz bytes from ptr to rx_tail, update rx_tail and
+	 * return the updated rx_tail to the caller
+	 * after putting, rx_tail must be still larger than rx_end.
+	 */
+	if (!precvframe)
+		return NULL;
+	precvframe->u.hdr.rx_tail += sz;
+	if (precvframe->u.hdr.rx_tail > precvframe->u.hdr.rx_end) {
+		precvframe->u.hdr.rx_tail -= sz;
+		return NULL;
+	}
+	precvframe->u.hdr.len += sz;
+	return precvframe->u.hdr.rx_tail;
+}
+
+static inline u8 *recvframe_pull_tail(union recv_frame *precvframe, sint sz)
+{
+	/* rmv data from rx_tail (by yitsen)
+	 * used for extract sz bytes from rx_end, update rx_end and return the
+	 * updated rx_end to the caller
+	 * after pulling, rx_end must be still larger than rx_data.
+	 */
+	if (!precvframe)
+		return NULL;
+	precvframe->u.hdr.rx_tail -= sz;
+	if (precvframe->u.hdr.rx_tail < precvframe->u.hdr.rx_data) {
+		precvframe->u.hdr.rx_tail += sz;
+		return NULL;
+	}
+	precvframe->u.hdr.len -= sz;
+	return precvframe->u.hdr.rx_tail;
+}
+
+struct sta_info;
+
+void	_r8712_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv);
+sint r8712_recvframe_chkmic(struct _adapter *adapter,
+			    union recv_frame *precvframe);
+union recv_frame *r8712_decryptor(struct _adapter *adapter,
+				  union recv_frame *precv_frame);
+union recv_frame *r8712_recvframe_chk_defrag(struct _adapter *adapter,
+					     union recv_frame *precv_frame);
+int r8712_validate_recv_frame(struct _adapter *adapter,
+			      union recv_frame *precv_frame);
+union recv_frame *r8712_portctrl(struct _adapter *adapter,
+				 union recv_frame *precv_frame);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_rf.h b/drivers/staging/rtl8712/rtl871x_rf.h
new file mode 100644
index 000000000..7d98921a4
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_rf.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef	__RTL871X_RF_H_
+#define __RTL871X_RF_H_
+
+#include "rtl871x_cmd.h"
+#include "rtl871x_mp_phy_regdef.h"
+
+#define OFDM_PHY		1
+#define MIXED_PHY		2
+#define CCK_PHY		3
+#define NumRates	(13)
+#define RTL8711_RF_MAX_SENS 6
+#define RTL8711_RF_DEF_SENS 4
+#define NUM_CHANNELS	15
+
+struct	regulatory_class {
+	u32	starting_freq;		/*MHz, */
+	u8	channel_set[NUM_CHANNELS];
+	u8	channel_cck_power[NUM_CHANNELS]; /*dbm*/
+	u8	channel_ofdm_power[NUM_CHANNELS];/*dbm*/
+	u8	txpower_limit;		/*dbm*/
+	u8	channel_spacing;	/*MHz*/
+	u8	modem;
+};
+
+enum	_REG_PREAMBLE_MODE {
+	PREAMBLE_LONG	= 1,
+	PREAMBLE_AUTO	= 2,
+	PREAMBLE_SHORT	= 3,
+};
+
+enum {
+	RTL8712_RFC_1T = 0x10,
+	RTL8712_RFC_2T = 0x20,
+	RTL8712_RFC_1R = 0x01,
+	RTL8712_RFC_2R = 0x02,
+	RTL8712_RFC_1T1R = 0x11,
+	RTL8712_RFC_1T2R = 0x12,
+	RTL8712_RFC_TURBO = 0x92,
+	RTL8712_RFC_2T2R = 0x22
+};
+
+#endif /*__RTL871X_RF_H_*/
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
new file mode 100644
index 000000000..e46a5dbc7
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -0,0 +1,1386 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_security.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define  _RTL871X_SECURITY_C_
+
+#include <linux/compiler.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/circ_buf.h>
+#include <linux/uaccess.h>
+#include <asm/byteorder.h>
+#include <linux/atomic.h>
+#include <linux/crc32poly.h>
+#include <linux/semaphore.h>
+#include <linux/ieee80211.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+
+/* =====WEP related===== */
+
+struct arc4context {
+	u32 x;
+	u32 y;
+	u8 state[256];
+};
+
+static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
+{
+	u32	t, u;
+	u32	keyindex;
+	u32	stateindex;
+	u8 *state;
+	u32	counter;
+
+	state = parc4ctx->state;
+	parc4ctx->x = 0;
+	parc4ctx->y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (u8)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++) {
+		t = state[counter];
+		stateindex = (stateindex + key[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = (u8)t;
+		state[counter] = (u8)u;
+		if (++keyindex >= key_len)
+			keyindex = 0;
+	}
+}
+
+static u32 arcfour_byte(struct arc4context *parc4ctx)
+{
+	u32 x;
+	u32 y;
+	u32 sx, sy;
+	u8 *state;
+
+	state = parc4ctx->state;
+	x = (parc4ctx->x + 1) & 0xff;
+	sx = state[x];
+	y = (sx + parc4ctx->y) & 0xff;
+	sy = state[y];
+	parc4ctx->x = x;
+	parc4ctx->y = y;
+	state[y] = (u8)sx;
+	state[x] = (u8)sy;
+	return state[(sx + sy) & 0xff];
+}
+
+static void arcfour_encrypt(struct arc4context	*parc4ctx,
+		     u8 *dest, u8 *src, u32 len)
+{
+	u32 i;
+
+	for (i = 0; i < len; i++)
+		dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
+}
+
+static sint bcrc32initialized;
+static u32 crc32_table[256];
+
+static u8 crc32_reverseBit(u8 data)
+{
+	return ((u8)(data << 7) & 0x80) | ((data << 5) & 0x40) | ((data << 3)
+		 & 0x20) | ((data << 1) & 0x10) | ((data >> 1) & 0x08) |
+		 ((data >> 3) & 0x04) | ((data >> 5) & 0x02) | ((data >> 7) &
+		 0x01);
+}
+
+static void crc32_init(void)
+{
+	sint i, j;
+	u32 c;
+	u8 *p = (u8 *)&c, *p1;
+	u8 k;
+
+	if (bcrc32initialized == 1)
+		return;
+
+	for (i = 0; i < 256; ++i) {
+		k = crc32_reverseBit((u8)i);
+		for (c = ((u32)k) << 24, j = 8; j > 0; --j)
+			c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY_BE : (c << 1);
+		p1 = (u8 *)&crc32_table[i];
+		p1[0] = crc32_reverseBit(p[3]);
+		p1[1] = crc32_reverseBit(p[2]);
+		p1[2] = crc32_reverseBit(p[1]);
+		p1[3] = crc32_reverseBit(p[0]);
+	}
+	bcrc32initialized = 1;
+}
+
+static u32 getcrc32(u8 *buf, u32 len)
+{
+	u8 *p;
+	u32  crc;
+
+	if (!bcrc32initialized)
+		crc32_init();
+	crc = 0xffffffff; /* preload shift register, per CRC-32 spec */
+	for (p = buf; len > 0; ++p, --len)
+		crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8);
+	return ~crc;    /* transmit complement, per CRC-32 spec */
+}
+
+/*
+ * Need to consider the fragment situation
+ */
+void r8712_wep_encrypt(struct _adapter *padapter, u8 *pxmitframe)
+{	/* exclude ICV */
+	unsigned char	crc[4];
+	struct arc4context  mycontext;
+	u32 curfragnum, length, keylength, pki;
+	u8 *pframe, *payload, *iv;    /*,*wepkey*/
+	u8 wepkey[16];
+	struct	pkt_attrib  *pattrib = &((struct xmit_frame *)
+				       pxmitframe)->attrib;
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
+	/*start to encrypt each fragment*/
+	if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
+		pki = psecuritypriv->PrivacyKeyIndex;
+		keylength = psecuritypriv->DefKeylen[pki];
+		for (curfragnum = 0; curfragnum < pattrib->nr_frags;
+		     curfragnum++) {
+			iv = pframe + pattrib->hdrlen;
+			memcpy(&wepkey[0], iv, 3);
+			memcpy(&wepkey[3], &psecuritypriv->DefKey[
+				psecuritypriv->PrivacyKeyIndex].skey[0],
+				keylength);
+			payload = pframe + pattrib->iv_len + pattrib->hdrlen;
+			if ((curfragnum + 1) == pattrib->nr_frags) {
+				length = pattrib->last_txcmdsz -
+					pattrib->hdrlen -
+					pattrib->iv_len -
+					pattrib->icv_len;
+				*((__le32 *)crc) = cpu_to_le32(getcrc32(
+						payload, length));
+				arcfour_init(&mycontext, wepkey, 3 + keylength);
+				arcfour_encrypt(&mycontext, payload, payload,
+						length);
+				arcfour_encrypt(&mycontext, payload + length,
+						crc, 4);
+			} else {
+				length = pxmitpriv->frag_len -
+					 pattrib->hdrlen - pattrib->iv_len -
+					 pattrib->icv_len;
+				*((__le32 *)crc) = cpu_to_le32(getcrc32(
+						payload, length));
+				arcfour_init(&mycontext, wepkey, 3 + keylength);
+				arcfour_encrypt(&mycontext, payload, payload,
+						length);
+				arcfour_encrypt(&mycontext, payload + length,
+						crc, 4);
+				pframe += pxmitpriv->frag_len;
+				pframe = (u8 *)RND4((addr_t)(pframe));
+			}
+		}
+	}
+}
+
+void r8712_wep_decrypt(struct _adapter  *padapter, u8 *precvframe)
+{
+	/* exclude ICV */
+	u8 crc[4];
+	struct arc4context  mycontext;
+	u32 length, keylength;
+	u8 *pframe, *payload, *iv, wepkey[16];
+	u8  keyindex;
+	struct rx_pkt_attrib  *prxattrib = &(((union recv_frame *)
+					  precvframe)->u.hdr.attrib);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->
+		  u.hdr.rx_data;
+	/* start to decrypt recvframe */
+	if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt ==
+	     _WEP104_)) {
+		iv = pframe + prxattrib->hdrlen;
+		keyindex = (iv[3] & 0x3);
+		keylength = psecuritypriv->DefKeylen[keyindex];
+		memcpy(&wepkey[0], iv, 3);
+		memcpy(&wepkey[3], &psecuritypriv->DefKey[
+			psecuritypriv->PrivacyKeyIndex].skey[0],
+			keylength);
+		length = ((union recv_frame *)precvframe)->
+			   u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
+		payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+		/* decrypt payload include icv */
+		arcfour_init(&mycontext, wepkey, 3 + keylength);
+		arcfour_encrypt(&mycontext, payload, payload,  length);
+		/* calculate icv and compare the icv */
+		*((__le32 *)crc) = cpu_to_le32(getcrc32(payload, length - 4));
+	}
+}
+
+/* 3 =====TKIP related===== */
+
+static u32 secmicgetuint32(u8 *p)
+/* Convert from Byte[] to Us4Byte32 in a portable way */
+{
+	s32 i;
+	u32 res = 0;
+
+	for (i = 0; i < 4; i++)
+		res |= ((u32)(*p++)) << (8 * i);
+	return res;
+}
+
+static void secmicputuint32(u8 *p, u32 val)
+/* Convert from Us4Byte32 to Byte[] in a portable way */
+{
+	long i;
+
+	for (i = 0; i < 4; i++) {
+		*p++ = (u8)(val & 0xff);
+		val >>= 8;
+	}
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+/* Reset the state to the empty message. */
+	pmicdata->L = pmicdata->K0;
+	pmicdata->R = pmicdata->K1;
+	pmicdata->nBytesInM = 0;
+	pmicdata->M = 0;
+}
+
+void r8712_secmicsetkey(struct mic_data *pmicdata, u8 *key)
+{
+	/* Set the key */
+	pmicdata->K0 = secmicgetuint32(key);
+	pmicdata->K1 = secmicgetuint32(key + 4);
+	/* and reset the message */
+	secmicclear(pmicdata);
+}
+
+static void secmicappendbyte(struct mic_data *pmicdata, u8 b)
+{
+	/* Append the byte to our word-sized buffer */
+	pmicdata->M |= ((u32)b) << (8 * pmicdata->nBytesInM);
+	pmicdata->nBytesInM++;
+	/* Process the word if it is full. */
+	if (pmicdata->nBytesInM >= 4) {
+		pmicdata->L ^= pmicdata->M;
+		pmicdata->R ^= ROL32(pmicdata->L, 17);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) |
+			       ((pmicdata->L & 0x00ff00ff) << 8);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROL32(pmicdata->L, 3);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROR32(pmicdata->L, 2);
+		pmicdata->L += pmicdata->R;
+		/* Clear the buffer */
+		pmicdata->M = 0;
+		pmicdata->nBytesInM = 0;
+	}
+}
+
+void r8712_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
+{
+	/* This is simple */
+	while (nbytes > 0) {
+		secmicappendbyte(pmicdata, *src++);
+		nbytes--;
+	}
+}
+
+void r8712_secgetmic(struct mic_data *pmicdata, u8 *dst)
+{
+	/* Append the minimum padding */
+	secmicappendbyte(pmicdata, 0x5a);
+	secmicappendbyte(pmicdata, 0);
+	secmicappendbyte(pmicdata, 0);
+	secmicappendbyte(pmicdata, 0);
+	secmicappendbyte(pmicdata, 0);
+	/* and then zeroes until the length is a multiple of 4 */
+	while (pmicdata->nBytesInM != 0)
+		secmicappendbyte(pmicdata, 0);
+	/* The appendByte function has already computed the result. */
+	secmicputuint32(dst, pmicdata->L);
+	secmicputuint32(dst + 4, pmicdata->R);
+	/* Reset to the empty message. */
+	secmicclear(pmicdata);
+}
+
+void seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code,
+		    u8 pri)
+{
+
+	struct mic_data	micdata;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+
+	r8712_secmicsetkey(&micdata, key);
+	priority[0] = pri;
+	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+	if (header[1] & 1) {   /* ToDS==1 */
+		r8712_secmicappend(&micdata, &header[16], 6);  /* DA */
+		if (header[1] & 2)  /* From Ds==1 */
+			r8712_secmicappend(&micdata, &header[24], 6);
+		else
+			r8712_secmicappend(&micdata, &header[10], 6);
+	} else {	/* ToDS==0 */
+		r8712_secmicappend(&micdata, &header[4], 6);   /* DA */
+		if (header[1] & 2)  /* From Ds==1 */
+			r8712_secmicappend(&micdata, &header[16], 6);
+		else
+			r8712_secmicappend(&micdata, &header[10], 6);
+	}
+	r8712_secmicappend(&micdata, &priority[0], 4);
+	r8712_secmicappend(&micdata, data, data_len);
+	r8712_secgetmic(&micdata, mic_code);
+}
+
+/* macros for extraction/creation of unsigned char/unsigned short values  */
+#define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define   Lo8(v16)   ((u8)((v16) & 0x00FF))
+#define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
+#define  Lo16(v32)   ((u16)((v32) & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
+#define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
+#define  TK16(N)  Mk16(tk[2 * (N) + 1], tk[2 * (N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16)  (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
+#define TA_SIZE           6    /*  48-bit transmitter address       */
+#define TK_SIZE          16    /* 128-bit temporal key              */
+#define P1K_SIZE         10    /*  80-bit Phase1 key                */
+#define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256] = {/* Sbox for hash (can be in ROM) */
+	{
+	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+	},
+	{  /* second half is unsigned char-reversed version of first! */
+	0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+	0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+	0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+	0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+	0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+	0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+	0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+	0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+	0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+	0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+	0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+	0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+	0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+	0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+	0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+	0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+	0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+	0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+	0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+	0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+	0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+	0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+	0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+	0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+	0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+	0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+	0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+	0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+	0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+	0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+	0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+	0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+	}
+};
+
+/*
+ **********************************************************************
+ * Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+ *
+ * Inputs:
+ *     tk[]      = temporal key                         [128 bits]
+ *     ta[]      = transmitter's MAC address            [ 48 bits]
+ *     iv32      = upper 32 bits of IV                  [ 32 bits]
+ * Output:
+ *     p1k[]     = Phase 1 key                          [ 80 bits]
+ *
+ * Note:
+ *     This function only needs to be called every 2**16 packets,
+ *     although in theory it could be called every packet.
+ *
+ **********************************************************************
+ */
+static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
+{
+	sint  i;
+
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
+	p1k[0] = Lo16(iv32);
+	p1k[1] = Hi16(iv32);
+	p1k[2] = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
+	p1k[3] = Mk16(ta[3], ta[2]);
+	p1k[4] = Mk16(ta[5], ta[4]);
+	/* Now compute an unbalanced Feistel cipher with 80-bit block */
+	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+	for (i = 0; i < PHASE1_LOOP_CNT; i++) {  /* Each add is mod 2**16 */
+		p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0));
+		p1k[4] +=  (unsigned short)i;	/* avoid "slide attacks" */
+	}
+}
+
+/*
+ **********************************************************************
+ * Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+ *
+ * Inputs:
+ *     tk[]      = Temporal key                         [128 bits]
+ *     p1k[]     = Phase 1 output key                   [ 80 bits]
+ *     iv16      = low 16 bits of IV counter            [ 16 bits]
+ * Output:
+ *     rc4key[]  = the key used to encrypt the packet   [128 bits]
+ *
+ * Note:
+ *     The value {TA,IV32,IV16} for Phase1/Phase2 must be unique
+ *     across all packets using the same key TK value. Then, for a
+ *     given value of TK[], this TKIP48 construction guarantees that
+ *     the final RC4KEY value is unique across all packets.
+ *
+ * Suggested implementation optimization: if PPK[] is "overlaid"
+ *     appropriately on RC4KEY[], there is no need for the final
+ *     for loop below that copies the PPK[] result into RC4KEY[].
+ *
+ **********************************************************************
+ */
+static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
+{
+	sint  i;
+	u16 PPK[6];			/* temporary key for mixing    */
+
+	/* Note: all adds in the PPK[] equations below are mod 2**16 */
+	for (i = 0; i < 5; i++)
+		PPK[i] = p1k[i]; /* first, copy P1K to PPK */
+	PPK[5]  =  p1k[4] + iv16; /* next,  add in IV16 */
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5] */
+	PPK[0] += _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round" */
+	PPK[1] += _S_(PPK[0] ^ TK16(1));
+	PPK[2] += _S_(PPK[1] ^ TK16(2));
+	PPK[3] += _S_(PPK[2] ^ TK16(3));
+	PPK[4] += _S_(PPK[3] ^ TK16(4));
+	PPK[5] += _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
+	PPK[2] +=  RotR1(PPK[1]);
+	PPK[3] +=  RotR1(PPK[2]);
+	PPK[4] +=  RotR1(PPK[3]);
+	PPK[5] +=  RotR1(PPK[4]);
+	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
+	/* value PPK[0..5] is guaranteed to be unique, as a function   */
+	/* of the 96-bit "input" value   {TA,IV32,IV16}. That is, P1K  */
+	/* is now a keyed permutation of {TA,IV32,IV16}. */
+	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
+	rc4key[0] = Hi8(iv16); /* RC4KEY[0..2] is the WEP IV  */
+	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
+	rc4key[2] = Lo8(iv16);
+	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian) */
+	for (i = 0; i < 6; i++) {
+		rc4key[4 + 2 * i] = Lo8(PPK[i]);
+		rc4key[5 + 2 * i] = Hi8(PPK[i]);
+	}
+}
+
+/*The hlen isn't include the IV*/
+u32 r8712_tkip_encrypt(struct _adapter *padapter, u8 *pxmitframe)
+{	/*  exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8 rc4key[16];
+	u8 ttkey[16];
+	u8 crc[4];
+	struct arc4context mycontext;
+	u32 curfragnum, length;
+
+	u8 *pframe, *payload, *iv, *prwskey;
+	union pn48 txpn;
+	struct sta_info *stainfo;
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u32 res = _SUCCESS;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return _FAIL;
+
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _TKIP_) {
+		if (pattrib->psta)
+			stainfo = pattrib->psta;
+		else
+			stainfo = r8712_get_stainfo(&padapter->stapriv,
+				  &pattrib->ra[0]);
+		if (stainfo) {
+			prwskey = &stainfo->x_UncstKey.skey[0];
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
+			     curfragnum++) {
+				iv = pframe + pattrib->hdrlen;
+				payload = pframe + pattrib->iv_len +
+					  pattrib->hdrlen;
+				GET_TKIP_PN(iv, txpn);
+				pnl = (u16)(txpn.val);
+				pnh = (u32)(txpn.val >> 16);
+				phase1((u16 *)&ttkey[0], prwskey,
+				       &pattrib->ta[0], pnh);
+				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0],
+				       pnl);
+				if ((curfragnum + 1) == pattrib->nr_frags) {
+					/* 4 the last fragment */
+					length = pattrib->last_txcmdsz -
+					     pattrib->hdrlen -
+					     pattrib->iv_len -
+					     pattrib->icv_len;
+					*((__le32 *)crc) = cpu_to_le32(
+						getcrc32(payload, length));
+					arcfour_init(&mycontext, rc4key, 16);
+					arcfour_encrypt(&mycontext, payload,
+							payload, length);
+					arcfour_encrypt(&mycontext, payload +
+							length, crc, 4);
+				} else {
+					length = pxmitpriv->frag_len -
+						 pattrib->hdrlen -
+						 pattrib->iv_len -
+						 pattrib->icv_len;
+					*((__le32 *)crc) = cpu_to_le32(getcrc32(
+							payload, length));
+					arcfour_init(&mycontext, rc4key, 16);
+					arcfour_encrypt(&mycontext, payload,
+							 payload, length);
+					arcfour_encrypt(&mycontext,
+							payload + length, crc,
+							4);
+					pframe += pxmitpriv->frag_len;
+					pframe = (u8 *)RND4((addr_t)(pframe));
+				}
+			}
+		} else {
+			res = _FAIL;
+		}
+	}
+	return res;
+}
+
+/* The hlen doesn't include the IV */
+void r8712_tkip_decrypt(struct _adapter *padapter, u8 *precvframe)
+{	/* exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8 rc4key[16];
+	u8 ttkey[16];
+	u8 crc[4];
+	struct arc4context mycontext;
+	u32 length;
+	u8 *pframe, *payload, *iv, *prwskey, idx = 0;
+	union pn48 txpn;
+	struct	sta_info *stainfo;
+	struct	rx_pkt_attrib *prxattrib = &((union recv_frame *)
+					   precvframe)->u.hdr.attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+
+	pframe = (unsigned char *)((union recv_frame *)
+				   precvframe)->u.hdr.rx_data;
+	/* 4 start to decrypt recvframe */
+	if (prxattrib->encrypt == _TKIP_) {
+		stainfo = r8712_get_stainfo(&padapter->stapriv,
+					    &prxattrib->ta[0]);
+		if (stainfo) {
+			iv = pframe + prxattrib->hdrlen;
+			payload = pframe + prxattrib->iv_len +
+				  prxattrib->hdrlen;
+			length = ((union recv_frame *)precvframe)->
+				 u.hdr.len - prxattrib->hdrlen -
+				 prxattrib->iv_len;
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				idx = iv[3];
+				prwskey = &psecuritypriv->XGrpKey[
+					 ((idx >> 6) & 0x3) - 1].skey[0];
+				if (!psecuritypriv->binstallGrpkey)
+					return;
+			} else {
+				prwskey = &stainfo->x_UncstKey.skey[0];
+			}
+			GET_TKIP_PN(iv, txpn);
+			pnl = (u16)(txpn.val);
+			pnh = (u32)(txpn.val >> 16);
+			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0],
+				pnh);
+			phase2(&rc4key[0], prwskey, (unsigned short *)
+			       &ttkey[0], pnl);
+			/* 4 decrypt payload include icv */
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+			*((__le32 *)crc) = cpu_to_le32(getcrc32(payload,
+					length - 4));
+		}
+	}
+}
+
+/* 3 =====AES related===== */
+
+#define MAX_MSG_SIZE	2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+static const u8 sbox_table[256] = {
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+static void xor_128(u8 *a, u8 *b, u8 *out)
+{
+	sint i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = a[i] ^ b[i];
+}
+
+static void xor_32(u8 *a, u8 *b, u8 *out)
+{
+	sint i;
+
+	for (i = 0; i < 4; i++)
+		out[i] = a[i] ^ b[i];
+}
+
+static u8 sbox(u8 a)
+{
+	return sbox_table[(sint)a];
+}
+
+static void next_key(u8 *key, sint round)
+{
+	u8 rcon;
+	u8 sbox_key[4];
+	static const u8 rcon_table[12] = {
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = sbox(key[13]);
+	sbox_key[1] = sbox(key[14]);
+	sbox_key[2] = sbox(key[15]);
+	sbox_key[3] = sbox(key[12]);
+	rcon = rcon_table[round];
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+static void byte_sub(u8 *in, u8 *out)
+{
+	sint i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = sbox(in[i]);
+}
+
+static void shift_row(u8 *in, u8 *out)
+{
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+}
+
+static void mix_column(u8 *in, u8 *out)
+{
+	sint i;
+	u8 add1b[4];
+	u8 add1bf7[4];
+	u8 rotl[4];
+	u8 swap_halves[4];
+	u8 andf7[4];
+	u8 rotr[4];
+	u8 temp[4];
+	u8 tempb[4];
+
+	for (i = 0; i < 4; i++) {
+		if ((in[i] & 0x80) == 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+	swap_halves[0] = in[2];    /* Swap halves */
+	swap_halves[1] = in[3];
+	swap_halves[2] = in[0];
+	swap_halves[3] = in[1];
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+	for (i = 3; i > 0; i--) {   /* logical shift left 1 bit */
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i - 1] & 0x80) == 0x80)
+			andf7[i] = (andf7[i] | 0x01);
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+	xor_32(add1b, andf7, add1bf7);
+	xor_32(in, add1bf7, rotr);
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halves, rotl, tempb);
+	xor_32(temp, tempb, out);
+}
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+	sint round;
+	sint i;
+	u8 intermediatea[16];
+	u8 intermediateb[16];
+	u8 round_key[16];
+
+	for (i = 0; i < 16; i++)
+		round_key[i] = key[i];
+	for (round = 0; round < 11; round++) {
+		if (round == 0) {
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		} else if (round == 10) {
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		} else {   /* 1 - 9 */
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+}
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+static void construct_mic_iv(u8 *mic_iv, sint qc_exists, sint a4_exists,
+			     u8 *mpdu, uint payload_length, u8 *pn_vector)
+{
+	sint i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];
+	for (i = 8; i < 14; i++)
+		mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
+	mic_iv[14] = (unsigned char)(payload_length / 256);
+	mic_iv[15] = (unsigned char)(payload_length % 256);
+}
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+static void construct_mic_header1(u8 *mic_header1, sint header_length, u8 *mpdu)
+{
+	mic_header1[0] = (u8)((header_length - 2) / 256);
+	mic_header1[1] = (u8)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	/* Mute retry, more data and pwr mgt bits */
+	mic_header1[3] = mpdu[1] & 0xc7;
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, sint a4_exists,
+			   sint qc_exists)
+{
+	sint i;
+
+	for (i = 0; i < 16; i++)
+		mic_header2[i] = 0x00;
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+	mic_header2[6] = 0x00;
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+	if (!qc_exists && a4_exists)
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
+	if (qc_exists && !a4_exists) {
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+	if (qc_exists && a4_exists) {
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+static void construct_ctr_preload(u8 *ctr_preload,
+				  sint a4_exists, sint qc_exists,
+				  u8 *mpdu, u8 *pn_vector, sint c)
+{
+	sint i;
+
+	for (i = 0; i < 16; i++)
+		ctr_preload[i] = 0x00;
+	i = 0;
+	ctr_preload[0] = 0x01;    /* flag */
+	if (qc_exists && a4_exists)
+		ctr_preload[1] = mpdu[30] & 0x0f;
+	if (qc_exists && !a4_exists)
+		ctr_preload[1] = mpdu[24] & 0x0f;
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] = pn_vector[13 - i];
+	ctr_preload[14] = (unsigned char)(c / 256); /* Ctr */
+	ctr_preload[15] = (unsigned char)(c % 256);
+}
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+	sint i;
+
+	for (i = 0; i < 16; i++)
+		out[i] = ina[i] ^ inb[i];
+}
+
+static void aes_cipher(u8 *key, uint hdrlen,
+		       u8 *pframe, uint plen)
+{
+	uint qc_exists, a4_exists, i, j, payload_remainder;
+	uint num_blocks, payload_index;
+
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+	u16 frtype  = GetFrameType(pframe);
+	u16 frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype >>= 4;
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if ((frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFACK)) ||
+	    (frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFPOLL)) ||
+	    (frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFACKPOLL))) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+	} else if ((frsubtype == 0x08) ||
+		   (frsubtype == 0x09) ||
+		   (frsubtype == 0x0a) ||
+		   (frsubtype == 0x0b)) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
+	construct_mic_header1(mic_header1, hdrlen, pframe);
+	construct_mic_header2(mic_header2, pframe, a4_exists, qc_exists);
+	payload_remainder = plen % 16;
+	num_blocks = plen / 16;
+	/* Find start of payload */
+	payload_index = hdrlen + 8;
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index++];
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+	for (j = 0; j < 8; j++)
+		mic[j] = aes_out[j];
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		pframe[payload_index + j] = mic[j];
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      pframe, pn_vector, i + 1);
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+	if (payload_remainder > 0) {  /* If short final block, then pad it,*/
+				      /* encrypt and copy unpadded part back */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      pframe, pn_vector, num_blocks + 1);
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+			      pframe, pn_vector, 0);
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = pframe[j + hdrlen + 8 + plen];
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		pframe[payload_index++] = chain_buffer[j];
+}
+
+u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
+{	/* exclude ICV */
+	/* Intermediate Buffers */
+	sint	curfragnum, length;
+	u8	*pframe, *prwskey;
+	struct	sta_info *stainfo;
+	struct	pkt_attrib  *pattrib = &((struct xmit_frame *)
+				       pxmitframe)->attrib;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u32 res = _SUCCESS;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return _FAIL;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _AES_) {
+		if (pattrib->psta)
+			stainfo = pattrib->psta;
+		else
+			stainfo = r8712_get_stainfo(&padapter->stapriv,
+				  &pattrib->ra[0]);
+		if (stainfo) {
+			prwskey = &stainfo->x_UncstKey.skey[0];
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
+			     curfragnum++) {
+				if ((curfragnum + 1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz -
+						 pattrib->hdrlen -
+						 pattrib->iv_len -
+						 pattrib->icv_len;
+					aes_cipher(prwskey, pattrib->hdrlen,
+						   pframe, length);
+				} else {
+					length = pxmitpriv->frag_len -
+						 pattrib->hdrlen -
+						 pattrib->iv_len -
+						 pattrib->icv_len;
+					aes_cipher(prwskey, pattrib->hdrlen,
+						   pframe, length);
+					pframe += pxmitpriv->frag_len;
+					pframe = (u8 *)RND4((addr_t)(pframe));
+				}
+			}
+		} else {
+			res = _FAIL;
+		}
+	}
+	return res;
+}
+
+static void aes_decipher(u8 *key, uint hdrlen,
+			 u8 *pframe, uint plen)
+{
+	static u8 message[MAX_MSG_SIZE];
+	uint qc_exists, a4_exists, i, j, payload_remainder;
+	uint num_blocks, payload_index;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+	uint frtype  = GetFrameType(pframe);
+	uint frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype >>= 4;
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+	/* start to decrypt the payload */
+	/*(plen including llc, payload and mic) */
+	num_blocks = (plen - 8) / 16;
+	payload_remainder = (plen - 8) % 16;
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+	if ((frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFACK)) ||
+	    (frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFPOLL)) ||
+	    (frtype == (IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA_CFACKPOLL))) {
+		qc_exists = 1;
+		if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+	} else if ((frsubtype == 0x08) ||
+		   (frsubtype == 0x09) ||
+		   (frsubtype == 0x0a) ||
+		   (frsubtype == 0x0b)) {
+		if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+	/* now, decrypt pframe with hdrlen offset and plen long */
+	payload_index = hdrlen + 8; /* 8 is for extiv */
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      pframe, pn_vector, i + 1);
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+	if (payload_remainder > 0) {  /* If short final block, pad it,*/
+		/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      pframe, pn_vector, num_blocks + 1);
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+	/* start to calculate the mic */
+	memcpy((void *)message, pframe, (hdrlen + plen + 8));
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen - 8,
+			 pn_vector);
+	construct_mic_header1(mic_header1, hdrlen, message);
+	construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
+	payload_remainder = (plen - 8) % 16;
+	num_blocks = (plen - 8) / 16;
+	/* Find start of payload */
+	payload_index = hdrlen + 8;
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index++];
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+	for (j = 0; j < 8; j++)
+		mic[j] = aes_out[j];
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		message[payload_index + j] = mic[j];
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      message, pn_vector, i + 1);
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+	if (payload_remainder > 0) { /* If short final block, pad it,*/
+				     /* encrypt and copy unpadded part back */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
+				      message, pn_vector, num_blocks + 1);
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message,
+			      pn_vector, 0);
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = message[j + hdrlen + plen];
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		message[payload_index++] = chain_buffer[j];
+	/* compare the mic */
+}
+
+void r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
+{	/* exclude ICV */
+	/* Intermediate Buffers */
+	sint		length;
+	u8	*pframe, *prwskey, *iv, idx;
+	struct	sta_info *stainfo;
+	struct	rx_pkt_attrib *prxattrib = &((union recv_frame *)
+					   precvframe)->u.hdr.attrib;
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->
+		 u.hdr.rx_data;
+	/* 4 start to encrypt each fragment */
+	if (prxattrib->encrypt == _AES_) {
+		stainfo = r8712_get_stainfo(&padapter->stapriv,
+					    &prxattrib->ta[0]);
+		if (stainfo) {
+			if (is_multicast_ether_addr(prxattrib->ra)) {
+				iv = pframe + prxattrib->hdrlen;
+				idx = iv[3];
+				prwskey = &psecuritypriv->XGrpKey[
+					  ((idx >> 6) & 0x3) - 1].skey[0];
+				if (!psecuritypriv->binstallGrpkey)
+					return;
+
+			} else {
+				prwskey = &stainfo->x_UncstKey.skey[0];
+			}
+			length = ((union recv_frame *)precvframe)->
+				 u.hdr.len - prxattrib->hdrlen -
+				 prxattrib->iv_len;
+			aes_decipher(prwskey, prxattrib->hdrlen, pframe,
+				     length);
+		}
+	}
+}
+
+void r8712_use_tkipkey_handler(struct timer_list *t)
+{
+	struct _adapter *padapter =
+		from_timer(padapter, t, securitypriv.tkip_timer);
+
+	padapter->securitypriv.busetkipkey = true;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_security.h b/drivers/staging/rtl8712/rtl871x_security.h
new file mode 100644
index 000000000..8461b7f05
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_security.h
@@ -0,0 +1,218 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __RTL871X_SECURITY_H_
+#define __RTL871X_SECURITY_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#define _NO_PRIVACY_	0x0
+#define _WEP40_		0x1
+#define _TKIP_		0x2
+#define _TKIP_WTMIC_	0x3
+#define _AES_		0x4
+#define _WEP104_	0x5
+
+#define _WPA_IE_ID_	0xdd
+#define _WPA2_IE_ID_	0x30
+
+#ifndef Ndis802_11AuthModeWPA2
+#define Ndis802_11AuthModeWPA2 (Ndis802_11AuthModeWPANone + 1)
+#endif
+
+#ifndef Ndis802_11AuthModeWPA2PSK
+#define Ndis802_11AuthModeWPA2PSK (Ndis802_11AuthModeWPANone + 2)
+#endif
+
+union pn48 {
+	u64 val;
+#if defined(__BIG_ENDIAN)
+	struct {
+		u8 TSC7;
+		u8 TSC6;
+		u8 TSC5;
+		u8 TSC4;
+		u8 TSC3;
+		u8 TSC2;
+		u8 TSC1;
+		u8 TSC0;
+	} _byte_;
+#else
+	struct {
+		u8 TSC0;
+		u8 TSC1;
+		u8 TSC2;
+		u8 TSC3;
+		u8 TSC4;
+		u8 TSC5;
+		u8 TSC6;
+		u8 TSC7;
+	} _byte_;
+#endif
+};
+
+union Keytype {
+	u8 skey[16];
+	u32 lkey[4];
+};
+
+struct RT_PMKID_LIST {
+	u8 bUsed;
+	u8 Bssid[6];
+	u8 PMKID[16];
+	u8 SsidBuf[33];
+	u8 *ssid_octet;
+	u16 ssid_length;
+};
+
+struct security_priv {
+	u32 AuthAlgrthm;		/* 802.11 auth, could be open, shared,
+					 * 8021x and authswitch
+					 */
+	u32 PrivacyAlgrthm;		/* This specify the privacy for shared
+					 * auth. algorithm.
+					 */
+	u32 PrivacyKeyIndex;		/* this is only valid for legendary
+					 * wep, 0~3 for key id.
+					 */
+	union Keytype DefKey[4];	/* this is only valid for def. key */
+	u32 DefKeylen[4];
+	u32 XGrpPrivacy;		/* This specify the privacy algthm.
+					 * used for Grp key
+					 */
+	u32 XGrpKeyid;			/* key id used for Grp Key */
+	union Keytype	XGrpKey[2];	/* 802.1x Group Key, for
+					 * inx0 and inx1
+					 */
+	union Keytype	XGrptxmickey[2];
+	union Keytype	XGrprxmickey[2];
+	union pn48 Grptxpn;		/* PN48 used for Grp Key xmit. */
+	union pn48 Grprxpn;		/* PN48 used for Grp Key recv. */
+	u8 wps_hw_pbc_pressed;/*for hw pbc pressed*/
+	u8 wps_phase;/*for wps*/
+	u8 wps_ie[MAX_WPA_IE_LEN << 2];
+	int wps_ie_len;
+	u8	binstallGrpkey;
+	u8	busetkipkey;
+	struct timer_list tkip_timer;
+	u8	bcheck_grpkey;
+	u8	bgrpkey_handshake;
+	s32	sw_encrypt;	/* from registry_priv */
+	s32	sw_decrypt;	/* from registry_priv */
+	s32	hw_decrypted;	/* if the rx packets is hw_decrypted==false,
+				 * it means the hw has not been ready.
+				 */
+	u32 ndisauthtype;	/* keeps the auth_type & enc_status from upper
+				 * layer ioctl(wpa_supplicant or wzc)
+				 */
+	u32 ndisencryptstatus;
+	struct wlan_bssid_ex sec_bss;  /* for joinbss (h2c buffer) usage */
+	struct NDIS_802_11_WEP ndiswep;
+	u8 assoc_info[600];
+	u8 szofcapability[256]; /* for wpa2 usage */
+	u8 oidassociation[512]; /* for wpa/wpa2 usage */
+	u8 authenticator_ie[256];  /* store ap security information element */
+	u8 supplicant_ie[256];  /* store sta security information element */
+	/* for tkip countermeasure */
+	u32 last_mic_err_time;
+	u8	btkip_countermeasure;
+	u8	btkip_wait_report;
+	u32 btkip_countermeasure_time;
+	/*-------------------------------------------------------------------
+	 * For WPA2 Pre-Authentication.
+	 *------------------------------------------------------------------
+	 **/
+	struct RT_PMKID_LIST		PMKIDList[NUM_PMKID_CACHE];
+	u8				PMKIDIndex;
+};
+
+#define GET_ENCRY_ALGO(psecuritypriv, psta, encry_algo, bmcst) \
+do { \
+	switch (psecuritypriv->AuthAlgrthm) { \
+	case 0: \
+	case 1: \
+	case 3: \
+		encry_algo = (u8)psecuritypriv->PrivacyAlgrthm; \
+		break; \
+	case 2: \
+		if (bmcst) \
+			encry_algo = (u8)psecuritypriv->XGrpPrivacy; \
+		else \
+			encry_algo = (u8)psta->XPrivacy; \
+		break; \
+	} \
+} while (0)
+#define SET_ICE_IV_LEN(iv_len, icv_len, encrypt)\
+do {\
+	switch (encrypt) { \
+	case _WEP40_: \
+	case _WEP104_: \
+		iv_len = 4; \
+		icv_len = 4; \
+		break; \
+	case _TKIP_: \
+		iv_len = 8; \
+		icv_len = 4; \
+		break; \
+	case _AES_: \
+		iv_len = 8; \
+		icv_len = 8; \
+		break; \
+	default: \
+		iv_len = 0; \
+		icv_len = 0; \
+		break; \
+	} \
+} while (0)
+#define GET_TKIP_PN(iv, txpn) \
+do {\
+	txpn._byte_.TSC0 = iv[2];\
+	txpn._byte_.TSC1 = iv[0];\
+	txpn._byte_.TSC2 = iv[4];\
+	txpn._byte_.TSC3 = iv[5];\
+	txpn._byte_.TSC4 = iv[6];\
+	txpn._byte_.TSC5 = iv[7];\
+} while (0)
+
+#define ROL32(A, n) (((A) << (n)) | (((A) >> (32 - (n)))  & ((1UL << (n)) - 1)))
+#define ROR32(A, n) ROL32((A), 32 - (n))
+
+struct mic_data {
+	u32  K0, K1;         /* Key */
+	u32  L, R;           /* Current state */
+	u32  M;              /* Message accumulator (single word) */
+	u32  nBytesInM;      /* # bytes in M */
+};
+
+void seccalctkipmic(
+	u8  *key,
+	u8  *header,
+	u8  *data,
+	u32  data_len,
+	u8  *Miccode,
+	u8   priority);
+
+void r8712_secmicsetkey(struct mic_data *pmicdata, u8 *key);
+void r8712_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nBytes);
+void r8712_secgetmic(struct mic_data *pmicdata, u8 *dst);
+u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe);
+u32 r8712_tkip_encrypt(struct _adapter *padapter, u8 *pxmitframe);
+void r8712_wep_encrypt(struct _adapter *padapter, u8  *pxmitframe);
+void r8712_aes_decrypt(struct _adapter *padapter, u8  *precvframe);
+void r8712_tkip_decrypt(struct _adapter *padapter, u8  *precvframe);
+void r8712_wep_decrypt(struct _adapter *padapter, u8  *precvframe);
+void r8712_use_tkipkey_handler(struct timer_list *t);
+
+#endif	/*__RTL871X_SECURITY_H_ */
+
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
new file mode 100644
index 000000000..2c806a010
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -0,0 +1,263 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_sta_mgt.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_STA_MGT_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "xmit_osdep.h"
+#include "sta_info.h"
+
+static void _init_stainfo(struct sta_info *psta)
+{
+	memset((u8 *)psta, 0, sizeof(struct sta_info));
+	spin_lock_init(&psta->lock);
+	INIT_LIST_HEAD(&psta->list);
+	INIT_LIST_HEAD(&psta->hash_list);
+	_r8712_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_r8712_init_sta_recv_priv(&psta->sta_recvpriv);
+	INIT_LIST_HEAD(&psta->asoc_list);
+	INIT_LIST_HEAD(&psta->auth_list);
+}
+
+int _r8712_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+	pstapriv->pallocated_stainfo_buf = kmalloc(sizeof(struct sta_info) *
+						   NUM_STA + 4, GFP_ATOMIC);
+	if (!pstapriv->pallocated_stainfo_buf)
+		return -ENOMEM;
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+		((addr_t)(pstapriv->pallocated_stainfo_buf) & 3);
+	_init_queue(&pstapriv->free_sta_queue);
+	spin_lock_init(&pstapriv->sta_hash_lock);
+	pstapriv->asoc_sta_count = 0;
+	_init_queue(&pstapriv->sleep_q);
+	_init_queue(&pstapriv->wakeup_q);
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+	for (i = 0; i < NUM_STA; i++) {
+		_init_stainfo(psta);
+		INIT_LIST_HEAD(&(pstapriv->sta_hash[i]));
+		list_add_tail(&psta->list, &pstapriv->free_sta_queue.queue);
+		psta++;
+	}
+	INIT_LIST_HEAD(&pstapriv->asoc_list);
+	INIT_LIST_HEAD(&pstapriv->auth_list);
+	return 0;
+}
+
+/* this function is used to free the memory of lock || sema for all stainfos */
+static void mfree_all_stainfo(struct sta_priv *pstapriv)
+{
+	unsigned long irqL;
+	struct list_head *plist, *phead;
+
+	spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
+	phead = &pstapriv->free_sta_queue.queue;
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist))
+		plist = plist->next;
+
+	spin_unlock_irqrestore(&pstapriv->sta_hash_lock, irqL);
+}
+
+void _r8712_free_sta_priv(struct sta_priv *pstapriv)
+{
+	if (pstapriv) {
+		/* be done before free sta_hash_lock */
+		mfree_all_stainfo(pstapriv);
+		kfree(pstapriv->pallocated_stainfo_buf);
+	}
+}
+
+struct sta_info *r8712_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	s32	index;
+	struct list_head *phash_list;
+	struct sta_info	*psta;
+	struct  __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+	unsigned long flags;
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+	spin_lock_irqsave(&pfree_sta_queue->lock, flags);
+	psta = list_first_entry_or_null(&pfree_sta_queue->queue,
+					struct sta_info, list);
+	if (psta) {
+		list_del_init(&psta->list);
+		_init_stainfo(psta);
+		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+		index = wifi_mac_hash(hwaddr);
+		if (index >= NUM_STA) {
+			psta = NULL;
+			goto exit;
+		}
+		phash_list = &pstapriv->sta_hash[index];
+		list_add_tail(&psta->hash_list, phash_list);
+		pstapriv->asoc_sta_count++;
+
+/* For the SMC router, the sequence number of first packet of WPS handshake
+ * will be 0. In this case, this packet will be dropped by recv_decache function
+ * if we use the 0x00 as the default value for tid_rxseq variable. So, we
+ * initialize the tid_rxseq variable as the 0xffff.
+ */
+		for (i = 0; i < 16; i++)
+			memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i],
+				&wRxSeqInitialValue, 2);
+		/* for A-MPDU Rx reordering buffer control */
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->padapter = pstapriv->padapter;
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+			preorder_ctrl->wsize_b = 64;
+			_init_queue(&preorder_ctrl->pending_recvframe_queue);
+			r8712_init_recv_timer(preorder_ctrl);
+		}
+	}
+exit:
+	spin_unlock_irqrestore(&pfree_sta_queue->lock, flags);
+	return psta;
+}
+
+/* using pstapriv->sta_hash_lock to protect */
+void r8712_free_stainfo(struct _adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	unsigned long irqL0;
+	struct  __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv *pstaxmitpriv;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		return;
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+	pstaxmitpriv = &psta->sta_xmitpriv;
+	spin_lock_irqsave(&(pxmitpriv->vo_pending.lock), irqL0);
+	r8712_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+	spin_unlock_irqrestore(&(pxmitpriv->vo_pending.lock), irqL0);
+	spin_lock_irqsave(&(pxmitpriv->vi_pending.lock), irqL0);
+	r8712_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+	spin_unlock_irqrestore(&(pxmitpriv->vi_pending.lock), irqL0);
+	spin_lock_irqsave(&(pxmitpriv->bk_pending.lock), irqL0);
+	r8712_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+	spin_unlock_irqrestore(&(pxmitpriv->bk_pending.lock), irqL0);
+	spin_lock_irqsave(&(pxmitpriv->be_pending.lock), irqL0);
+	r8712_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+	spin_unlock_irqrestore(&(pxmitpriv->be_pending.lock), irqL0);
+	list_del_init(&psta->hash_list);
+	pstapriv->asoc_sta_count--;
+	/* re-init sta_info; 20061114 */
+	_r8712_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_r8712_init_sta_recv_priv(&psta->sta_recvpriv);
+	/* for A-MPDU Rx reordering buffer control,
+	 * cancel reordering_ctrl_timer
+	 */
+	for (i = 0; i < 16; i++) {
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+		del_timer(&preorder_ctrl->reordering_ctrl_timer);
+	}
+	spin_lock(&(pfree_sta_queue->lock));
+	/* insert into free_sta_queue; 20061114 */
+	list_add_tail(&psta->list, &pfree_sta_queue->queue);
+	spin_unlock(&(pfree_sta_queue->lock));
+}
+
+/* free all stainfo which in sta_hash[all] */
+void r8712_free_all_stainfo(struct _adapter *padapter)
+{
+	unsigned long irqL;
+	struct list_head *plist, *phead;
+	s32 index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_stainfo = r8712_get_bcmc_stainfo(padapter);
+
+	if (pstapriv->asoc_sta_count == 1)
+		return;
+	spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &(pstapriv->sta_hash[index]);
+		plist = phead->next;
+		while (!end_of_queue_search(phead, plist)) {
+			psta = container_of(plist,
+					    struct sta_info, hash_list);
+			plist = plist->next;
+			if (pbcmc_stainfo != psta)
+				r8712_free_stainfo(padapter, psta);
+		}
+	}
+	spin_unlock_irqrestore(&pstapriv->sta_hash_lock, irqL);
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *r8712_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	unsigned long	 irqL;
+	struct list_head *plist, *phead;
+	struct sta_info *psta = NULL;
+	u32	index;
+
+	if (!hwaddr)
+		return NULL;
+	index = wifi_mac_hash(hwaddr);
+	spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
+	phead = &(pstapriv->sta_hash[index]);
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist)) {
+		psta = container_of(plist, struct sta_info, hash_list);
+		if ((!memcmp(psta->hwaddr, hwaddr, ETH_ALEN))) {
+			/* if found the matched address */
+			break;
+		}
+		psta = NULL;
+		plist = plist->next;
+	}
+	spin_unlock_irqrestore(&pstapriv->sta_hash_lock, irqL);
+	return psta;
+}
+
+void r8712_init_bcmc_stainfo(struct _adapter *padapter)
+{
+	unsigned char bcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+
+	r8712_alloc_stainfo(pstapriv, bcast_addr);
+}
+
+struct sta_info *r8712_get_bcmc_stainfo(struct _adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	return r8712_get_stainfo(pstapriv, bc_addr);
+}
+
+u8 r8712_access_ctrl(struct wlan_acl_pool *pacl_list, u8 *mac_addr)
+{
+	return true;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_wlan_sme.h b/drivers/staging/rtl8712/rtl871x_wlan_sme.h
new file mode 100644
index 000000000..97ea14514
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_wlan_sme.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_WLAN_SME_H_
+#define _RTL871X_WLAN_SME_H_
+
+#define MSR_APMODE		0x0C
+#define MSR_STAMODE	0x08
+#define MSR_ADHOCMODE	0x04
+#define MSR_NOLINKMODE	0x00
+#define		_1M_RATE_	0
+#define		_2M_RATE_	1
+#define		_5M_RATE_	2
+#define		_11M_RATE_	3
+#define		_6M_RATE_	4
+#define		_9M_RATE_	5
+#define		_12M_RATE_	6
+#define		_18M_RATE_	7
+#define		_24M_RATE_	8
+#define		_36M_RATE_	9
+#define		_48M_RATE_	10
+#define		_54M_RATE_	11
+
+#endif
+
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
new file mode 100644
index 000000000..6353dbe55
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -0,0 +1,1059 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * rtl871x_xmit.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _RTL871X_XMIT_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+
+#include <linux/usb.h>
+#include <linux/ieee80211.h>
+
+static const u8 P802_1H_OUI[P80211_OUI_LEN] = {0x00, 0x00, 0xf8};
+static const u8 RFC1042_OUI[P80211_OUI_LEN] = {0x00, 0x00, 0x00};
+static void init_hwxmits(struct hw_xmit *phwxmit, sint entry);
+static void alloc_hwxmits(struct _adapter *padapter);
+static void free_hwxmits(struct _adapter *padapter);
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+	INIT_LIST_HEAD(&ptxservq->tx_pending);
+	_init_queue(&ptxservq->sta_pending);
+	ptxservq->qcnt = 0;
+}
+
+void _r8712_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{
+	memset((unsigned char *)psta_xmitpriv, 0,
+		 sizeof(struct sta_xmit_priv));
+	spin_lock_init(&psta_xmitpriv->lock);
+	_init_txservq(&psta_xmitpriv->be_q);
+	_init_txservq(&psta_xmitpriv->bk_q);
+	_init_txservq(&psta_xmitpriv->vi_q);
+	_init_txservq(&psta_xmitpriv->vo_q);
+	INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
+	INIT_LIST_HEAD(&psta_xmitpriv->apsd);
+}
+
+int _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
+			  struct _adapter *padapter)
+{
+	sint i;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxframe;
+	int j;
+
+	memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv));
+	spin_lock_init(&pxmitpriv->lock);
+	/*
+	 *Please insert all the queue initialization using _init_queue below
+	 */
+	pxmitpriv->adapter = padapter;
+	_init_queue(&pxmitpriv->be_pending);
+	_init_queue(&pxmitpriv->bk_pending);
+	_init_queue(&pxmitpriv->vi_pending);
+	_init_queue(&pxmitpriv->vo_pending);
+	_init_queue(&pxmitpriv->bm_pending);
+	_init_queue(&pxmitpriv->legacy_dz_queue);
+	_init_queue(&pxmitpriv->apsd_queue);
+	_init_queue(&pxmitpriv->free_xmit_queue);
+	/*
+	 * Please allocate memory with sz = (struct xmit_frame) * NR_XMITFRAME,
+	 * and initialize free_xmit_frame below.
+	 * Please also apply  free_txobj to link_up all the xmit_frames...
+	 */
+	pxmitpriv->pallocated_frame_buf =
+		kmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4,
+			GFP_ATOMIC);
+	if (!pxmitpriv->pallocated_frame_buf) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		return -ENOMEM;
+	}
+	pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 -
+			((addr_t) (pxmitpriv->pallocated_frame_buf) & 3);
+	pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&(pxframe->list));
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = DATA_FRAMETAG;
+		pxframe->pkt = NULL;
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+		list_add_tail(&(pxframe->list),
+				 &(pxmitpriv->free_xmit_queue.queue));
+		pxframe++;
+	}
+	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+	/*
+	 * init xmit hw_txqueue
+	 */
+	_r8712_init_hw_txqueue(&pxmitpriv->be_txqueue, BE_QUEUE_INX);
+	_r8712_init_hw_txqueue(&pxmitpriv->bk_txqueue, BK_QUEUE_INX);
+	_r8712_init_hw_txqueue(&pxmitpriv->vi_txqueue, VI_QUEUE_INX);
+	_r8712_init_hw_txqueue(&pxmitpriv->vo_txqueue, VO_QUEUE_INX);
+	_r8712_init_hw_txqueue(&pxmitpriv->bmc_txqueue, BMC_QUEUE_INX);
+	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+	pxmitpriv->txirp_cnt = 1;
+	/*per AC pending irp*/
+	pxmitpriv->beq_cnt = 0;
+	pxmitpriv->bkq_cnt = 0;
+	pxmitpriv->viq_cnt = 0;
+	pxmitpriv->voq_cnt = 0;
+	/*init xmit_buf*/
+	_init_queue(&pxmitpriv->free_xmitbuf_queue);
+	_init_queue(&pxmitpriv->pending_xmitbuf_queue);
+	pxmitpriv->pallocated_xmitbuf =
+		kmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4, GFP_ATOMIC);
+	if (!pxmitpriv->pallocated_xmitbuf)
+		goto clean_up_frame_buf;
+	pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 -
+			      ((addr_t)(pxmitpriv->pallocated_xmitbuf) & 3);
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+		pxmitbuf->pallocated_buf =
+			kmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ, GFP_ATOMIC);
+		if (!pxmitbuf->pallocated_buf) {
+			j = 0;
+			goto clean_up_alloc_buf;
+		}
+		pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -
+				 ((addr_t) (pxmitbuf->pallocated_buf) &
+				 (XMITBUF_ALIGN_SZ - 1));
+		if (r8712_xmit_resource_alloc(padapter, pxmitbuf)) {
+			j = 1;
+			goto clean_up_alloc_buf;
+		}
+		list_add_tail(&pxmitbuf->list,
+				 &(pxmitpriv->free_xmitbuf_queue.queue));
+		pxmitbuf++;
+	}
+	pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+	INIT_WORK(&padapter->wk_filter_rx_ff0, r8712_SetFilter);
+	alloc_hwxmits(padapter);
+	init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+	tasklet_setup(&pxmitpriv->xmit_tasklet, r8712_xmit_bh);
+	return 0;
+
+clean_up_alloc_buf:
+	if (j) {
+		/* failure happened in r8712_xmit_resource_alloc()
+		 * delete extra pxmitbuf->pallocated_buf
+		 */
+		kfree(pxmitbuf->pallocated_buf);
+	}
+	for (j = 0; j < i; j++) {
+		int k;
+
+		pxmitbuf--;			/* reset pointer */
+		kfree(pxmitbuf->pallocated_buf);
+		for (k = 0; k < 8; k++)		/* delete xmit urb's */
+			usb_free_urb(pxmitbuf->pxmit_urb[k]);
+	}
+	kfree(pxmitpriv->pallocated_xmitbuf);
+	pxmitpriv->pallocated_xmitbuf = NULL;
+clean_up_frame_buf:
+	kfree(pxmitpriv->pallocated_frame_buf);
+	pxmitpriv->pallocated_frame_buf = NULL;
+	return -ENOMEM;
+}
+
+void _free_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+	int i;
+	struct _adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)
+					pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	if (!pxmitpriv->pxmit_frame_buf)
+		return;
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		r8712_xmit_complete(padapter, pxmitframe);
+		pxmitframe++;
+	}
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		r8712_xmit_resource_free(padapter, pxmitbuf);
+		kfree(pxmitbuf->pallocated_buf);
+		pxmitbuf++;
+	}
+	kfree(pxmitpriv->pallocated_frame_buf);
+	kfree(pxmitpriv->pallocated_xmitbuf);
+	free_hwxmits(padapter);
+}
+
+int r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
+			struct pkt_attrib *pattrib)
+{
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	struct tx_cmd txdesc;
+
+	bool bmcast;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+
+	_r8712_open_pktfile(pkt, &pktfile);
+
+	_r8712_pktfile_read(&pktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+	/*
+	 * If driver xmit ARP packet, driver can set ps mode to initial
+	 * setting. It stands for getting DHCP or fix IP.
+	 */
+	if (pattrib->ether_type == 0x0806) {
+		if (padapter->pwrctrlpriv.pwr_mode !=
+		    padapter->registrypriv.power_mgnt) {
+			del_timer_sync(&pmlmepriv->dhcp_timer);
+			r8712_set_ps_mode(padapter,
+					  padapter->registrypriv.power_mgnt,
+					  padapter->registrypriv.smart_ps);
+		}
+	}
+
+	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
+	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+	pattrib->pctrl = 0;
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		/*firstly, filter packet not belongs to mp*/
+		if (pattrib->ether_type != 0x8712)
+			return -EINVAL;
+		/* for mp storing the txcmd per packet,
+		 * according to the info of txcmd to update pattrib
+		 */
+		/*get MP_TXDESC_SIZE bytes txcmd per packet*/
+		_r8712_pktfile_read(&pktfile, (u8 *)&txdesc, TXDESC_SIZE);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+		pattrib->pctrl = 1;
+	}
+	/* r8712_xmitframe_coalesce() overwrite this!*/
+	pattrib->pktlen = pktfile.pkt_len;
+	if (pattrib->ether_type == ETH_P_IP) {
+		/* The following is for DHCP and ARP packet, we use cck1M to
+		 * tx these packets and let LPS awake some time
+		 * to prevent DHCP protocol fail
+		 */
+		u8 tmp[24];
+
+		_r8712_pktfile_read(&pktfile, &tmp[0], 24);
+		pattrib->dhcp_pkt = 0;
+		if (pktfile.pkt_len > 282) {/*MINIMUM_DHCP_PACKET_SIZE)*/
+			if (pattrib->ether_type == ETH_P_IP) {/* IP header*/
+				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+					((tmp[21] == 67) && (tmp[23] == 68))) {
+					/* 68 : UDP BOOTP client
+					 * 67 : UDP BOOTP server
+					 * Use low rate to send DHCP packet.
+					 */
+					pattrib->dhcp_pkt = 1;
+				}
+			}
+		}
+	}
+	bmcast = is_multicast_ether_addr(pattrib->ra);
+	/* get sta_info*/
+	if (bmcast) {
+		psta = r8712_get_bcmc_stainfo(padapter);
+		pattrib->mac_id = 4;
+	} else {
+		if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+			psta = r8712_get_stainfo(pstapriv,
+						 get_bssid(pmlmepriv));
+			pattrib->mac_id = 5;
+		} else {
+			psta = r8712_get_stainfo(pstapriv, pattrib->ra);
+			if (!psta)  /* drop the pkt */
+				return -ENOMEM;
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+				pattrib->mac_id = 5;
+			else
+				pattrib->mac_id = psta->mac_id;
+		}
+	}
+
+	if (psta) {
+		pattrib->psta = psta;
+	} else {
+		/* if we cannot get psta => drrp the pkt */
+		return -ENOMEM;
+	}
+
+	pattrib->ack_policy = 0;
+	/* get ether_hdr_len */
+	pattrib->pkt_hdrlen = ETH_HLEN;
+
+	if (pqospriv->qos_option) {
+		r8712_set_qos(&pktfile, pattrib);
+	} else {
+		pattrib->hdrlen = WLAN_HDR_A3_LEN;
+		pattrib->subtype = IEEE80211_FTYPE_DATA;
+		pattrib->priority = 0;
+	}
+	if (psta->ieee8021x_blocked) {
+		pattrib->encrypt = 0;
+		if ((pattrib->ether_type != 0x888e) &&
+		    !check_fwstate(pmlmepriv, WIFI_MP_STATE))
+			return -EINVAL;
+	} else {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+	}
+	switch (pattrib->encrypt) {
+	case _WEP40_:
+	case _WEP104_:
+		pattrib->iv_len = 4;
+		pattrib->icv_len = 4;
+		break;
+	case _TKIP_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 4;
+		if (padapter->securitypriv.busetkipkey == _FAIL)
+			return -EINVAL;
+		break;
+	case _AES_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 8;
+		break;
+	default:
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+		break;
+	}
+
+	if (pattrib->encrypt &&
+	    (padapter->securitypriv.sw_encrypt ||
+	    !psecuritypriv->hw_decrypted))
+		pattrib->bswenc = true;
+	else
+		pattrib->bswenc = false;
+	/* if in MP_STATE, update pkt_attrib from mp_txcmd, and overwrite
+	 * some settings above.
+	 */
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+		pattrib->priority =
+		    (le32_to_cpu(txdesc.txdw1) >> QSEL_SHT) & 0x1f;
+	return 0;
+}
+
+static int xmitframe_addmic(struct _adapter *padapter,
+			    struct xmit_frame *pxmitframe)
+{
+	u32	curfragnum, length;
+	u8	*pframe, *payload, mic[8];
+	struct	mic_data micdata;
+	struct	sta_info *stainfo;
+	struct	qos_priv *pqospriv = &(padapter->mlmepriv.qospriv);
+	struct	pkt_attrib  *pattrib = &pxmitframe->attrib;
+	struct	security_priv *psecpriv = &padapter->securitypriv;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u8 priority[4] = {};
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+	if (pattrib->psta)
+		stainfo = pattrib->psta;
+	else
+		stainfo = r8712_get_stainfo(&padapter->stapriv,
+					    &pattrib->ra[0]);
+	if (pattrib->encrypt == _TKIP_) {
+		/*encode mic code*/
+		if (stainfo) {
+			u8 null_key[16] = {};
+
+			pframe = pxmitframe->buf_addr + TXDESC_OFFSET;
+			if (bmcst) {
+				if (!memcmp(psecpriv->XGrptxmickey
+				   [psecpriv->XGrpKeyid].skey,
+				   null_key, 16))
+					return -ENOMEM;
+				/*start to calculate the mic code*/
+				r8712_secmicsetkey(&micdata,
+					psecpriv->XGrptxmickey
+					[psecpriv->XGrpKeyid].skey);
+			} else {
+				if (!memcmp(&stainfo->tkiptxmickey.skey[0],
+					    null_key, 16))
+					return -ENOMEM;
+				/* start to calculate the mic code */
+				r8712_secmicsetkey(&micdata,
+					     &stainfo->tkiptxmickey.skey[0]);
+			}
+			if (pframe[1] & 1) {   /* ToDS==1 */
+				r8712_secmicappend(&micdata,
+						   &pframe[16], 6); /*DA*/
+				if (pframe[1] & 2)  /* From Ds==1 */
+					r8712_secmicappend(&micdata,
+							   &pframe[24], 6);
+				else
+					r8712_secmicappend(&micdata,
+							   &pframe[10], 6);
+			} else {	/* ToDS==0 */
+				r8712_secmicappend(&micdata,
+						   &pframe[4], 6); /* DA */
+				if (pframe[1] & 2)  /* From Ds==1 */
+					r8712_secmicappend(&micdata,
+							   &pframe[16], 6);
+				else
+					r8712_secmicappend(&micdata,
+							   &pframe[10], 6);
+			}
+			if (pqospriv->qos_option == 1)
+				priority[0] = (u8)pxmitframe->attrib.priority;
+			r8712_secmicappend(&micdata, &priority[0], 4);
+			payload = pframe;
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
+			     curfragnum++) {
+				payload = (u8 *)RND4((addr_t)(payload));
+				payload += pattrib->hdrlen + pattrib->iv_len;
+				if ((curfragnum + 1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz -
+						  pattrib->hdrlen -
+						  pattrib->iv_len -
+						  ((psecpriv->sw_encrypt)
+						  ? pattrib->icv_len : 0);
+					r8712_secmicappend(&micdata, payload,
+							   length);
+					payload = payload + length;
+				} else {
+					length = pxmitpriv->frag_len -
+					    pattrib->hdrlen - pattrib->iv_len -
+					    ((psecpriv->sw_encrypt) ?
+					    pattrib->icv_len : 0);
+					r8712_secmicappend(&micdata, payload,
+							   length);
+					payload = payload + length +
+						  pattrib->icv_len;
+				}
+			}
+			r8712_secgetmic(&micdata, &(mic[0]));
+			/* add mic code  and add the mic code length in
+			 * last_txcmdsz
+			 */
+			memcpy(payload, &(mic[0]), 8);
+			pattrib->last_txcmdsz += 8;
+			payload = payload - pattrib->last_txcmdsz + 8;
+		}
+	}
+	return 0;
+}
+
+static sint xmitframe_swencrypt(struct _adapter *padapter,
+				struct xmit_frame *pxmitframe)
+{
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+
+	if (pattrib->bswenc) {
+		switch (pattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			r8712_wep_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _TKIP_:
+			r8712_tkip_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _AES_:
+			r8712_aes_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		default:
+				break;
+		}
+	}
+	return _SUCCESS;
+}
+
+static int make_wlanhdr(struct _adapter *padapter, u8 *hdr,
+			struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	__le16 *fctrl = &pwlanhdr->frame_control;
+	u8 *bssid;
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+	SetFrameSubType(fctrl, pattrib->subtype);
+	if (!(pattrib->subtype & IEEE80211_FTYPE_DATA))
+		return 0;
+
+	bssid = get_bssid(pmlmepriv);
+
+	if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
+		/* to_ds = 1, fr_ds = 0; */
+		SetToDs(fctrl);
+		ether_addr_copy(pwlanhdr->addr1, bssid);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, pattrib->dst);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* to_ds = 0, fr_ds = 1; */
+		SetFrDs(fctrl);
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, bssid);
+		ether_addr_copy(pwlanhdr->addr3, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+		   check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, bssid);
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, bssid);
+	} else {
+		return -EINVAL;
+	}
+
+	if (pattrib->encrypt)
+		SetPrivacy(fctrl);
+	if (pqospriv->qos_option) {
+		qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+		if (pattrib->priority)
+			SetPriority(qc, pattrib->priority);
+		SetAckpolicy(qc, pattrib->ack_policy);
+	}
+	/* TODO: fill HT Control Field */
+	/* Update Seq Num will be handled by f/w */
+	{
+		struct sta_info *psta;
+		bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+		if (pattrib->psta)
+			psta = pattrib->psta;
+		else if (bmcst)
+			psta = r8712_get_bcmc_stainfo(padapter);
+		else
+			psta = r8712_get_stainfo(&padapter->stapriv,
+						 pattrib->ra);
+
+		if (psta) {
+			u16 *txtid = psta->sta_xmitpriv.txseq_tid;
+
+			txtid[pattrib->priority]++;
+			txtid[pattrib->priority] &= 0xFFF;
+			pattrib->seqnum = txtid[pattrib->priority];
+			SetSeqNum(hdr, pattrib->seqnum);
+		}
+	}
+
+	return 0;
+}
+
+static sint r8712_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	const u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+	return SNAP_SIZE + sizeof(u16);
+}
+
+/*
+ * This sub-routine will perform all the following:
+ * 1. remove 802.3 header.
+ * 2. create wlan_header, based on the info in pxmitframe
+ * 3. append sta's iv/ext-iv
+ * 4. append LLC
+ * 5. move frag chunk from pframe to pxmitframe->mem
+ * 6. apply sw-encrypt, if necessary.
+ */
+sint r8712_xmitframe_coalesce(struct _adapter *padapter, _pkt *pkt,
+			struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+
+	sint	frg_len, mpdu_len, llc_sz;
+	u32	mem_sz;
+	u8	frg_inx;
+	addr_t addr;
+	u8 *pframe, *mem_start, *ptxdesc;
+	struct sta_info		*psta;
+	struct security_priv	*psecpriv = &padapter->securitypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+	if (!pattrib->psta)
+		return _FAIL;
+	psta = pattrib->psta;
+	if (!pxmitframe->buf_addr)
+		return _FAIL;
+	pbuf_start = pxmitframe->buf_addr;
+	ptxdesc = pbuf_start;
+	mem_start = pbuf_start + TXDESC_OFFSET;
+	if (make_wlanhdr(padapter, mem_start, pattrib))
+		return _FAIL;
+	_r8712_open_pktfile(pkt, &pktfile);
+	_r8712_pktfile_read(&pktfile, NULL, (uint) pattrib->pkt_hdrlen);
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		/* truncate TXDESC_SIZE bytes txcmd if at mp mode for 871x */
+		if (pattrib->ether_type == 0x8712) {
+			/* take care -  update_txdesc overwrite this */
+			_r8712_pktfile_read(&pktfile, ptxdesc, TXDESC_SIZE);
+		}
+	}
+	pattrib->pktlen = pktfile.pkt_len;
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;
+	while (1) {
+		llc_sz = 0;
+		mpdu_len = frg_len;
+		pframe = mem_start;
+		SetMFrag(mem_start);
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+		/* adding icv, if necessary...*/
+		if (pattrib->iv_len) {
+			if (psta) {
+				switch (pattrib->encrypt) {
+				case _WEP40_:
+				case _WEP104_:
+					WEP_IV(pattrib->iv, psta->txpn,
+					       (u8)psecpriv->PrivacyKeyIndex);
+					break;
+				case _TKIP_:
+					if (bmcst)
+						TKIP_IV(pattrib->iv,
+						    psta->txpn,
+						    (u8)psecpriv->XGrpKeyid);
+					else
+						TKIP_IV(pattrib->iv, psta->txpn,
+							0);
+					break;
+				case _AES_:
+					if (bmcst)
+						AES_IV(pattrib->iv, psta->txpn,
+						    (u8)psecpriv->XGrpKeyid);
+					else
+						AES_IV(pattrib->iv, psta->txpn,
+						       0);
+					break;
+				}
+			}
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+			pframe += pattrib->iv_len;
+			mpdu_len -= pattrib->iv_len;
+		}
+		if (frg_inx == 0) {
+			llc_sz = r8712_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc))
+			mpdu_len -= pattrib->icv_len;
+		if (bmcst)
+			mem_sz = _r8712_pktfile_read(&pktfile, pframe,
+				 pattrib->pktlen);
+		else
+			mem_sz = _r8712_pktfile_read(&pktfile, pframe,
+				 mpdu_len);
+		pframe += mem_sz;
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			memcpy(pframe, pattrib->icv, pattrib->icv_len);
+			pframe += pattrib->icv_len;
+		}
+		frg_inx++;
+		if (bmcst || r8712_endofpktfile(&pktfile)) {
+			pattrib->nr_frags = frg_inx;
+			pattrib->last_txcmdsz = pattrib->hdrlen +
+						pattrib->iv_len +
+						((pattrib->nr_frags == 1) ?
+						llc_sz : 0) +
+						((pattrib->bswenc) ?
+						pattrib->icv_len : 0) + mem_sz;
+			ClearMFrag(mem_start);
+			break;
+		}
+		addr = (addr_t)(pframe);
+		mem_start = (unsigned char *)RND4(addr) + TXDESC_OFFSET;
+		memcpy(mem_start, pbuf_start + TXDESC_OFFSET, pattrib->hdrlen);
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe))
+		return _FAIL;
+	xmitframe_swencrypt(padapter, pxmitframe);
+	return _SUCCESS;
+}
+
+void r8712_update_protection(struct _adapter *padapter, u8 *ie, uint ie_len)
+{
+	uint	protection;
+	u8	*perp;
+	uint	erp_len;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	switch (pxmitpriv->vcs_setting) {
+	case DISABLE_VCS:
+		pxmitpriv->vcs = NONE_VCS;
+		break;
+	case ENABLE_VCS:
+		break;
+	case AUTO_VCS:
+	default:
+		perp = r8712_get_ie(ie, WLAN_EID_ERP_INFO, &erp_len, ie_len);
+		if (!perp) {
+			pxmitpriv->vcs = NONE_VCS;
+		} else {
+			protection = (*(perp + 2)) & BIT(1);
+			if (protection) {
+				if (pregistrypriv->vcs_type == RTS_CTS)
+					pxmitpriv->vcs = RTS_CTS;
+				else
+					pxmitpriv->vcs = CTS_TO_SELF;
+			} else {
+				pxmitpriv->vcs = NONE_VCS;
+			}
+		}
+		break;
+	}
+}
+
+struct xmit_buf *r8712_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_buf *pxmitbuf;
+	struct  __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
+	pxmitbuf = list_first_entry_or_null(&pfree_xmitbuf_queue->queue,
+					    struct xmit_buf, list);
+	if (pxmitbuf) {
+		list_del_init(&pxmitbuf->list);
+		pxmitpriv->free_xmitbuf_cnt--;
+	}
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+	return pxmitbuf;
+}
+
+void r8712_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	unsigned long irqL;
+	struct  __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	if (!pxmitbuf)
+		return;
+	spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
+	list_del_init(&pxmitbuf->list);
+	list_add_tail(&(pxmitbuf->list), &pfree_xmitbuf_queue->queue);
+	pxmitpriv->free_xmitbuf_cnt++;
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+}
+
+/*
+ * Calling context:
+ * 1. OS_TXENTRY
+ * 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+ *
+ * If we turn on USE_RXTHREAD, then, no need for critical section.
+ * Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+ *
+ * Must be very very cautious...
+ *
+ */
+struct xmit_frame *r8712_alloc_xmitframe(struct xmit_priv *pxmitpriv)
+{
+	/*
+	 * Please remember to use all the osdep_service api,
+	 * and lock/unlock or _enter/_exit critical to protect
+	 * pfree_xmit_queue
+	 */
+	unsigned long irqL;
+	struct xmit_frame *pxframe;
+	struct  __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+	spin_lock_irqsave(&pfree_xmit_queue->lock, irqL);
+	pxframe = list_first_entry_or_null(&pfree_xmit_queue->queue,
+					   struct xmit_frame, list);
+	if (pxframe) {
+		list_del_init(&pxframe->list);
+		pxmitpriv->free_xmitframe_cnt--;
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+		pxframe->attrib.psta = NULL;
+		pxframe->pkt = NULL;
+	}
+	spin_unlock_irqrestore(&pfree_xmit_queue->lock, irqL);
+	return pxframe;
+}
+
+void r8712_free_xmitframe(struct xmit_priv *pxmitpriv,
+			  struct xmit_frame *pxmitframe)
+{
+	unsigned long irqL;
+	struct  __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+	struct _adapter *padapter = pxmitpriv->adapter;
+
+	if (!pxmitframe)
+		return;
+	spin_lock_irqsave(&pfree_xmit_queue->lock, irqL);
+	list_del_init(&pxmitframe->list);
+	if (pxmitframe->pkt)
+		pxmitframe->pkt = NULL;
+	list_add_tail(&pxmitframe->list, &pfree_xmit_queue->queue);
+	pxmitpriv->free_xmitframe_cnt++;
+	spin_unlock_irqrestore(&pfree_xmit_queue->lock, irqL);
+	if (netif_queue_stopped(padapter->pnetdev))
+		netif_wake_queue(padapter->pnetdev);
+}
+
+void r8712_free_xmitframe_ex(struct xmit_priv *pxmitpriv,
+		      struct xmit_frame *pxmitframe)
+{
+	if (!pxmitframe)
+		return;
+	if (pxmitframe->frame_tag == DATA_FRAMETAG)
+		r8712_free_xmitframe(pxmitpriv, pxmitframe);
+}
+
+void r8712_free_xmitframe_queue(struct xmit_priv *pxmitpriv,
+				struct  __queue *pframequeue)
+{
+	unsigned long irqL;
+	struct list_head *plist, *phead;
+	struct	xmit_frame	*pxmitframe;
+
+	spin_lock_irqsave(&(pframequeue->lock), irqL);
+	phead = &pframequeue->queue;
+	plist = phead->next;
+	while (!end_of_queue_search(phead, plist)) {
+		pxmitframe = container_of(plist, struct xmit_frame, list);
+		plist = plist->next;
+		r8712_free_xmitframe(pxmitpriv, pxmitframe);
+	}
+	spin_unlock_irqrestore(&(pframequeue->lock), irqL);
+}
+
+static inline struct tx_servq *get_sta_pending(struct _adapter *padapter,
+					       struct  __queue **ppstapending,
+					       struct sta_info *psta, sint up)
+{
+	struct tx_servq *ptxservq;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+
+	switch (up) {
+	case 1:
+	case 2:
+		ptxservq = &(psta->sta_xmitpriv.bk_q);
+		*ppstapending = &padapter->xmitpriv.bk_pending;
+		(phwxmits + 3)->accnt++;
+		break;
+	case 4:
+	case 5:
+		ptxservq = &(psta->sta_xmitpriv.vi_q);
+		*ppstapending = &padapter->xmitpriv.vi_pending;
+		(phwxmits + 1)->accnt++;
+		break;
+	case 6:
+	case 7:
+		ptxservq = &(psta->sta_xmitpriv.vo_q);
+		*ppstapending = &padapter->xmitpriv.vo_pending;
+		(phwxmits + 0)->accnt++;
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &(psta->sta_xmitpriv.be_q);
+		*ppstapending = &padapter->xmitpriv.be_pending;
+		(phwxmits + 2)->accnt++;
+		break;
+	}
+	return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue, and indicate it
+ * to xx_pending list.....
+ */
+int r8712_xmit_classifier(struct _adapter *padapter,
+			  struct xmit_frame *pxmitframe)
+{
+	unsigned long irqL0;
+	struct  __queue *pstapending;
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+	if (pattrib->psta) {
+		psta = pattrib->psta;
+	} else {
+		if (bmcst) {
+			psta = r8712_get_bcmc_stainfo(padapter);
+		} else {
+			if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+				psta = r8712_get_stainfo(pstapriv,
+				       get_bssid(pmlmepriv));
+			else
+				psta = r8712_get_stainfo(pstapriv, pattrib->ra);
+		}
+	}
+	if (!psta)
+		return -EINVAL;
+	ptxservq = get_sta_pending(padapter, &pstapending,
+		   psta, pattrib->priority);
+	spin_lock_irqsave(&pstapending->lock, irqL0);
+	if (list_empty(&ptxservq->tx_pending))
+		list_add_tail(&ptxservq->tx_pending, &pstapending->queue);
+	list_add_tail(&pxmitframe->list, &ptxservq->sta_pending.queue);
+	ptxservq->qcnt++;
+	spin_unlock_irqrestore(&pstapending->lock, irqL0);
+	return 0;
+}
+
+static void alloc_hwxmits(struct _adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+	pxmitpriv->hwxmits = kmalloc_array(pxmitpriv->hwxmit_entry,
+				sizeof(struct hw_xmit), GFP_ATOMIC);
+	if (!pxmitpriv->hwxmits)
+		return;
+	hwxmits = pxmitpriv->hwxmits;
+	if (pxmitpriv->hwxmit_entry == 5) {
+		pxmitpriv->bmc_txqueue.head = 0;
+		hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue;
+		hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
+		pxmitpriv->vo_txqueue.head = 0;
+		hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue;
+		hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
+		pxmitpriv->vi_txqueue.head = 0;
+		hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue;
+		hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
+		pxmitpriv->bk_txqueue.head = 0;
+		hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+		pxmitpriv->be_txqueue.head = 0;
+		hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue;
+		hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
+	} else if (pxmitpriv->hwxmit_entry == 4) {
+		pxmitpriv->vo_txqueue.head = 0;
+		hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue;
+		hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+		pxmitpriv->vi_txqueue.head = 0;
+		hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue;
+		hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+		pxmitpriv->be_txqueue.head = 0;
+		hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue;
+		hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+		pxmitpriv->bk_txqueue.head = 0;
+		hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+	}
+}
+
+static void free_hwxmits(struct _adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	kfree(pxmitpriv->hwxmits);
+}
+
+static void init_hwxmits(struct hw_xmit *phwxmit, sint entry)
+{
+	sint i;
+
+	for (i = 0; i < entry; i++, phwxmit++) {
+		spin_lock_init(&phwxmit->xmit_lock);
+		INIT_LIST_HEAD(&phwxmit->pending);
+		phwxmit->txcmdcnt = 0;
+		phwxmit->accnt = 0;
+	}
+}
+
+void xmitframe_xmitbuf_attach(struct xmit_frame *pxmitframe,
+			struct xmit_buf *pxmitbuf)
+{
+	/* pxmitbuf attach to pxmitframe */
+	pxmitframe->pxmitbuf = pxmitbuf;
+	/* urb and irp connection */
+	pxmitframe->pxmit_urb[0] = pxmitbuf->pxmit_urb[0];
+	/* buffer addr assoc */
+	pxmitframe->buf_addr = pxmitbuf->pbuf;
+	/* pxmitframe attach to pxmitbuf */
+	pxmitbuf->priv_data = pxmitframe;
+}
+
+/*
+ * tx_action == 0 == no frames to transmit
+ * tx_action > 0 ==> we have frames to transmit
+ * tx_action < 0 ==> we have frames to transmit, but TXFF is not even enough
+ *						 to transmit 1 frame.
+ */
+
+int r8712_pre_xmit(struct _adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	unsigned long irqL;
+	int ret;
+	struct xmit_buf *pxmitbuf = NULL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	r8712_do_queue_select(padapter, pattrib);
+	spin_lock_irqsave(&pxmitpriv->lock, irqL);
+	if (r8712_txframes_sta_ac_pending(padapter, pattrib) > 0) {
+		ret = false;
+		r8712_xmit_enqueue(padapter, pxmitframe);
+		spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
+		return ret;
+	}
+	pxmitbuf = r8712_alloc_xmitbuf(pxmitpriv);
+	if (!pxmitbuf) { /*enqueue packet*/
+		ret = false;
+		r8712_xmit_enqueue(padapter, pxmitframe);
+		spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
+	} else { /*dump packet directly*/
+		spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
+		ret = true;
+		xmitframe_xmitbuf_attach(pxmitframe, pxmitbuf);
+		r8712_xmit_direct(padapter, pxmitframe);
+	}
+	return ret;
+}
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.h b/drivers/staging/rtl8712/rtl871x_xmit.h
new file mode 100644
index 000000000..cdcbc87a3
--- /dev/null
+++ b/drivers/staging/rtl8712/rtl871x_xmit.h
@@ -0,0 +1,288 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_XMIT_H_
+#define _RTL871X_XMIT_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "xmit_osdep.h"
+
+#ifdef CONFIG_R8712_TX_AGGR
+#define MAX_XMITBUF_SZ  (16384)
+#else
+#define MAX_XMITBUF_SZ  (2048)
+#endif
+
+#define NR_XMITBUFF     (4)
+
+#ifdef CONFIG_R8712_TX_AGGR
+#define AGGR_NR_HIGH_BOUND      (4) /*(8) */
+#define AGGR_NR_LOW_BOUND       (2)
+#endif
+
+#define XMITBUF_ALIGN_SZ 512
+#define TX_GUARD_BAND		5
+#define MAX_NUMBLKS		(1)
+
+/* Fixed the Big Endian bug when using the software driver encryption.*/
+#define WEP_IV(pattrib_iv, txpn, keyidx)\
+do { \
+	pattrib_iv[0] = txpn._byte_.TSC0;\
+	pattrib_iv[1] = txpn._byte_.TSC1;\
+	pattrib_iv[2] = txpn._byte_.TSC2;\
+	pattrib_iv[3] = ((keyidx & 0x3) << 6);\
+	txpn.val = (txpn.val == 0xffffff) ? 0 : (txpn.val + 1);\
+} while (0)
+
+/* Fixed the Big Endian bug when doing the Tx.
+ * The Linksys WRH54G will check this.
+ */
+#define TKIP_IV(pattrib_iv, txpn, keyidx)\
+do { \
+	pattrib_iv[0] = txpn._byte_.TSC1;\
+	pattrib_iv[1] = (txpn._byte_.TSC1 | 0x20) & 0x7f;\
+	pattrib_iv[2] = txpn._byte_.TSC0;\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3) << 6);\
+	pattrib_iv[4] = txpn._byte_.TSC2;\
+	pattrib_iv[5] = txpn._byte_.TSC3;\
+	pattrib_iv[6] = txpn._byte_.TSC4;\
+	pattrib_iv[7] = txpn._byte_.TSC5;\
+	txpn.val = txpn.val == 0xffffffffffffULL ? 0 : \
+	(txpn.val + 1);\
+} while (0)
+
+#define AES_IV(pattrib_iv, txpn, keyidx)\
+do { \
+	pattrib_iv[0] = txpn._byte_.TSC0;\
+	pattrib_iv[1] = txpn._byte_.TSC1;\
+	pattrib_iv[2] = 0;\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3) << 6);\
+	pattrib_iv[4] = txpn._byte_.TSC2;\
+	pattrib_iv[5] = txpn._byte_.TSC3;\
+	pattrib_iv[6] = txpn._byte_.TSC4;\
+	pattrib_iv[7] = txpn._byte_.TSC5;\
+	txpn.val = txpn.val == 0xffffffffffffULL ? 0 : \
+	(txpn.val + 1);\
+} while (0)
+
+struct hw_xmit {
+	spinlock_t xmit_lock;
+	struct list_head pending;
+	struct  __queue *sta_queue;
+	struct hw_txqueue *phwtxqueue;
+	sint	txcmdcnt;
+	int	accnt;
+};
+
+struct pkt_attrib {
+	u8	type;
+	u8	subtype;
+	u8	bswenc;
+	u8	dhcp_pkt;
+
+	u16	seqnum;
+	u16	ether_type;
+	u16	pktlen;		/* the original 802.3 pkt raw_data len
+				 * (not include ether_hdr data)
+				 */
+	u16	last_txcmdsz;
+
+	u8	pkt_hdrlen;	/*the original 802.3 pkt header len*/
+	u8	hdrlen;		/*the WLAN Header Len*/
+	u8	nr_frags;
+	u8	ack_policy;
+	u8	mac_id;
+	u8	vcs_mode;	/*virtual carrier sense method*/
+	u8	pctrl;/*per packet txdesc control enable*/
+	u8	qsel;
+
+	u8	priority;
+	u8	encrypt;	/* when 0 indicate no encrypt. when non-zero,
+				 * indicate the encrypt algorithm
+				 */
+	u8	iv_len;
+	u8	icv_len;
+	unsigned char iv[8];
+	unsigned char icv[8];
+	u8	dst[ETH_ALEN] __aligned(2);	/* for ether_addr_copy */
+	u8	src[ETH_ALEN];
+	u8	ta[ETH_ALEN];
+	u8	ra[ETH_ALEN];
+	struct sta_info *psta;
+};
+
+#define WLANHDR_OFFSET	64
+#define DATA_FRAMETAG		0x01
+#define L2_FRAMETAG		0x02
+#define MGNT_FRAMETAG		0x03
+#define AMSDU_FRAMETAG	0x04
+#define EII_FRAMETAG		0x05
+#define IEEE8023_FRAMETAG  0x06
+#define MP_FRAMETAG		0x07
+#define TXAGG_FRAMETAG	0x08
+
+struct xmit_buf {
+	struct list_head list;
+
+	u8 *pallocated_buf;
+	u8 *pbuf;
+	void *priv_data;
+	struct urb *pxmit_urb[8];
+	u32 aggr_nr;
+};
+
+struct xmit_frame {
+	struct list_head list;
+	struct pkt_attrib attrib;
+	_pkt *pkt;
+	int frame_tag;
+	struct _adapter *padapter;
+	u8 *buf_addr;
+	struct xmit_buf *pxmitbuf;
+	u8 *mem_addr;
+	u16 sz[8];
+	struct urb *pxmit_urb[8];
+	u8 bpending[8];
+	u8 last[8];
+};
+
+struct tx_servq {
+	struct list_head tx_pending;
+	struct  __queue	sta_pending;
+	int qcnt;
+};
+
+struct sta_xmit_priv {
+	spinlock_t lock;
+	sint	option;
+	sint	apsd_setting;	/* When bit mask is on, the associated edca
+				 * queue supports APSD.
+				 */
+	struct tx_servq	be_q;	/* priority == 0,3 */
+	struct tx_servq	bk_q;	/* priority == 1,2*/
+	struct tx_servq	vi_q;	/*priority == 4,5*/
+	struct tx_servq	vo_q;	/*priority == 6,7*/
+	struct list_head  legacy_dz;
+	struct list_head apsd;
+	u16 txseq_tid[16];
+	uint	sta_tx_bytes;
+	u64	sta_tx_pkts;
+	uint	sta_tx_fail;
+};
+
+struct	hw_txqueue {
+	sint	head;
+	sint	tail;
+	sint	free_sz;	/* in units of 64 bytes */
+	sint	free_cmdsz;
+	sint	txsz[8];
+	uint	ff_hwaddr;
+	uint	cmd_hwaddr;
+	sint	ac_tag;
+};
+
+struct	xmit_priv {
+	spinlock_t lock;
+	struct  __queue	be_pending;
+	struct  __queue	bk_pending;
+	struct  __queue	vi_pending;
+	struct  __queue	vo_pending;
+	struct  __queue	bm_pending;
+	struct  __queue	legacy_dz_queue;
+	struct  __queue	apsd_queue;
+	u8 *pallocated_frame_buf;
+	u8 *pxmit_frame_buf;
+	uint free_xmitframe_cnt;
+	uint mapping_addr;
+	uint pkt_sz;
+	struct  __queue	free_xmit_queue;
+	struct	hw_txqueue	be_txqueue;
+	struct	hw_txqueue	bk_txqueue;
+	struct	hw_txqueue	vi_txqueue;
+	struct	hw_txqueue	vo_txqueue;
+	struct	hw_txqueue	bmc_txqueue;
+	uint	frag_len;
+	struct _adapter	*adapter;
+	u8   vcs_setting;
+	u8	vcs;
+	u8	vcs_type;
+	u16  rts_thresh;
+	uint	tx_bytes;
+	u64	tx_pkts;
+	uint	tx_drop;
+	struct hw_xmit *hwxmits;
+	u8	hwxmit_entry;
+	u8	txirp_cnt;
+	struct tasklet_struct xmit_tasklet;
+	struct work_struct xmit_pipe4_reset_wi;
+	struct work_struct xmit_pipe6_reset_wi;
+	struct work_struct xmit_piped_reset_wi;
+	/*per AC pending irp*/
+	int beq_cnt;
+	int bkq_cnt;
+	int viq_cnt;
+	int voq_cnt;
+	struct  __queue	free_amsdu_xmit_queue;
+	u8 *pallocated_amsdu_frame_buf;
+	u8 *pxmit_amsdu_frame_buf;
+	uint free_amsdu_xmitframe_cnt;
+	struct  __queue free_txagg_xmit_queue;
+	u8 *pallocated_txagg_frame_buf;
+	u8 *pxmit_txagg_frame_buf;
+	uint free_txagg_xmitframe_cnt;
+	int cmdseq;
+	struct  __queue free_xmitbuf_queue;
+	struct  __queue pending_xmitbuf_queue;
+	u8 *pallocated_xmitbuf;
+	u8 *pxmitbuf;
+	uint free_xmitbuf_cnt;
+};
+
+void r8712_free_xmitbuf(struct xmit_priv *pxmitpriv,
+			struct xmit_buf *pxmitbuf);
+struct xmit_buf *r8712_alloc_xmitbuf(struct xmit_priv *pxmitpriv);
+void r8712_update_protection(struct _adapter *padapter, u8 *ie, uint ie_len);
+struct xmit_frame *r8712_alloc_xmitframe(struct xmit_priv *pxmitpriv);
+void r8712_free_xmitframe(struct xmit_priv *pxmitpriv,
+			  struct xmit_frame *pxmitframe);
+void r8712_free_xmitframe_queue(struct xmit_priv *pxmitpriv,
+				struct  __queue *pframequeue);
+int r8712_xmit_classifier(struct _adapter *padapter,
+			  struct xmit_frame *pxmitframe);
+sint r8712_xmitframe_coalesce(struct _adapter *padapter, _pkt *pkt,
+			      struct xmit_frame *pxmitframe);
+sint _r8712_init_hw_txqueue(struct hw_txqueue *phw_txqueue, u8 ac_tag);
+void _r8712_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv);
+int r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
+			struct pkt_attrib *pattrib);
+int r8712_txframes_sta_ac_pending(struct _adapter *padapter,
+				  struct pkt_attrib *pattrib);
+int _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
+			  struct _adapter *padapter);
+void _free_xmit_priv(struct xmit_priv *pxmitpriv);
+void r8712_free_xmitframe_ex(struct xmit_priv *pxmitpriv,
+			     struct xmit_frame *pxmitframe);
+int r8712_pre_xmit(struct _adapter *padapter, struct xmit_frame *pxmitframe);
+int r8712_xmit_enqueue(struct _adapter *padapter,
+		       struct xmit_frame *pxmitframe);
+void r8712_xmit_direct(struct _adapter *padapter, struct xmit_frame *pxmitframe);
+void r8712_xmit_bh(struct tasklet_struct *t);
+
+void xmitframe_xmitbuf_attach(struct xmit_frame *pxmitframe,
+			      struct xmit_buf *pxmitbuf);
+
+#include "rtl8712_xmit.h"
+
+#endif	/*_RTL871X_XMIT_H_*/
+
diff --git a/drivers/staging/rtl8712/sta_info.h b/drivers/staging/rtl8712/sta_info.h
new file mode 100644
index 000000000..6286c6224
--- /dev/null
+++ b/drivers/staging/rtl8712/sta_info.h
@@ -0,0 +1,132 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __STA_INFO_H_
+#define __STA_INFO_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "wifi.h"
+
+#define NUM_STA 32
+#define NUM_ACL 64
+
+/* if mode ==0, then the sta is allowed once the addr is hit.
+ * if mode ==1, then the sta is rejected once the addr is non-hit.
+ */
+struct wlan_acl_node {
+	struct list_head list;
+	u8       addr[ETH_ALEN];
+	u8       mode;
+};
+
+struct wlan_acl_pool {
+	struct wlan_acl_node aclnode[NUM_ACL];
+};
+
+struct	stainfo_stats {
+	uint	rx_pkts;
+	uint	rx_bytes;
+	u64	tx_pkts;
+	uint	tx_bytes;
+};
+
+struct sta_info {
+	spinlock_t lock;
+	struct list_head list; /*free_sta_queue*/
+	struct list_head hash_list; /*sta_hash*/
+	struct sta_xmit_priv sta_xmitpriv;
+	struct sta_recv_priv sta_recvpriv;
+	uint state;
+	uint aid;
+	uint	mac_id;
+	uint	qos_option;
+	u8	hwaddr[ETH_ALEN];
+	uint	ieee8021x_blocked;	/*0: allowed, 1:blocked */
+	uint	XPrivacy; /*aes, tkip...*/
+	union Keytype	tkiptxmickey;
+	union Keytype	tkiprxmickey;
+	union Keytype	x_UncstKey;
+	union pn48		txpn;	/* PN48 used for Unicast xmit.*/
+	union pn48		rxpn;	/* PN48 used for Unicast recv.*/
+	u8	bssrateset[16];
+	uint	bssratelen;
+	s32  rssi;
+	s32	signal_quality;
+	struct stainfo_stats sta_stats;
+	/*for A-MPDU Rx reordering buffer control */
+	struct recv_reorder_ctrl recvreorder_ctrl[16];
+	struct ht_priv	htpriv;
+	/* Notes:
+	 * STA_Mode:
+	 * curr_network(mlme_priv/security_priv/qos/ht)
+	 *   + sta_info: (STA & AP) CAP/INFO
+	 * scan_q: AP CAP/INFO
+	 * AP_Mode:
+	 * curr_network(mlme_priv/security_priv/qos/ht) : AP CAP/INFO
+	 * sta_info: (AP & STA) CAP/INFO
+	 */
+	struct list_head asoc_list;
+	struct list_head auth_list;
+	unsigned int expire_to;
+	unsigned int auth_seq;
+	unsigned int authalg;
+	unsigned char chg_txt[128];
+	unsigned int tx_ra_bitmap;
+};
+
+struct	sta_priv {
+	u8 *pallocated_stainfo_buf;
+	u8 *pstainfo_buf;
+	struct  __queue	free_sta_queue;
+	spinlock_t sta_hash_lock;
+	struct list_head sta_hash[NUM_STA];
+	int asoc_sta_count;
+	struct  __queue sleep_q;
+	struct  __queue wakeup_q;
+	struct _adapter *padapter;
+	struct list_head asoc_list;
+	struct list_head auth_list;
+	unsigned int auth_to;  /* sec, time to expire in authenticating. */
+	unsigned int assoc_to; /* sec, time to expire before associating. */
+	unsigned int expire_to; /* sec , time to expire after associated. */
+};
+
+static inline u32 wifi_mac_hash(u8 *mac)
+{
+	u32 x;
+
+	x = mac[0];
+	x = (x << 2) ^ mac[1];
+	x = (x << 2) ^ mac[2];
+	x = (x << 2) ^ mac[3];
+	x = (x << 2) ^ mac[4];
+	x = (x << 2) ^ mac[5];
+	x ^= x >> 8;
+	x  = x & (NUM_STA - 1);
+	return x;
+}
+
+int _r8712_init_sta_priv(struct sta_priv *pstapriv);
+void _r8712_free_sta_priv(struct sta_priv *pstapriv);
+struct sta_info *r8712_alloc_stainfo(struct sta_priv *pstapriv,
+				     u8 *hwaddr);
+void r8712_free_stainfo(struct _adapter *padapter, struct sta_info *psta);
+void r8712_free_all_stainfo(struct _adapter *padapter);
+struct sta_info *r8712_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr);
+void r8712_init_bcmc_stainfo(struct _adapter *padapter);
+struct sta_info *r8712_get_bcmc_stainfo(struct _adapter *padapter);
+u8 r8712_access_ctrl(struct wlan_acl_pool *pacl_list, u8 *mac_addr);
+
+#endif /* _STA_INFO_H_ */
+
diff --git a/drivers/staging/rtl8712/usb_halinit.c b/drivers/staging/rtl8712/usb_halinit.c
new file mode 100644
index 000000000..313c56974
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_halinit.c
@@ -0,0 +1,307 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * usb_halinit.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _HCI_HAL_INIT_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "usb_ops.h"
+#include "usb_osintf.h"
+
+u8 r8712_usb_hal_bus_init(struct _adapter *adapter)
+{
+	u8 val8 = 0;
+	u8 ret = _SUCCESS;
+	int PollingCnt = 20;
+	struct registry_priv *registrypriv = &adapter->registrypriv;
+
+	if (registrypriv->chip_version == RTL8712_FPGA) {
+		val8 = 0x01;
+		/* switch to 80M clock */
+		r8712_write8(adapter, SYS_CLKR, val8);
+		val8 = r8712_read8(adapter, SPS1_CTRL);
+		val8 = val8 | 0x01;
+		/* enable VSPS12 LDO Macro block */
+		r8712_write8(adapter, SPS1_CTRL, val8);
+		val8 = r8712_read8(adapter, AFE_MISC);
+		val8 = val8 | 0x01;
+		/* Enable AFE Macro Block's Bandgap */
+		r8712_write8(adapter, AFE_MISC, val8);
+		val8 = r8712_read8(adapter, LDOA15_CTRL);
+		val8 = val8 | 0x01;
+		/* enable LDOA15 block */
+		r8712_write8(adapter, LDOA15_CTRL, val8);
+		val8 = r8712_read8(adapter, SPS1_CTRL);
+		val8 = val8 | 0x02;
+		/* Enable VSPS12_SW Macro Block */
+		r8712_write8(adapter, SPS1_CTRL, val8);
+		val8 = r8712_read8(adapter, AFE_MISC);
+		val8 = val8 | 0x02;
+		/* Enable AFE Macro Block's Mbias */
+		r8712_write8(adapter, AFE_MISC, val8);
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		val8 = val8 | 0x08;
+		/* isolate PCIe Analog 1.2V to PCIe 3.3V and PCIE Digital */
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, val8);
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		val8 = val8 & 0xEF;
+		/* attach AFE PLL to MACTOP/BB/PCIe Digital */
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, val8);
+		val8 = r8712_read8(adapter, AFE_XTAL_CTRL + 1);
+		val8 = val8 & 0xFB;
+		/* enable AFE clock */
+		r8712_write8(adapter, AFE_XTAL_CTRL + 1, val8);
+		val8 = r8712_read8(adapter, AFE_PLL_CTRL);
+		val8 = val8 | 0x01;
+		/* Enable AFE PLL Macro Block */
+		r8712_write8(adapter, AFE_PLL_CTRL, val8);
+		val8 = 0xEE;
+		/* release isolation AFE PLL & MD */
+		r8712_write8(adapter, SYS_ISO_CTRL, val8);
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		val8 = val8 | 0x08;
+		/* enable MAC clock */
+		r8712_write8(adapter, SYS_CLKR + 1, val8);
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		val8 = val8 | 0x08;
+		/* enable Core digital and enable IOREG R/W */
+		r8712_write8(adapter, SYS_FUNC_EN + 1, val8);
+		val8 = val8 | 0x80;
+		/* enable REG_EN */
+		r8712_write8(adapter, SYS_FUNC_EN + 1, val8);
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		val8 = (val8 | 0x80) & 0xBF;
+		/* switch the control path */
+		r8712_write8(adapter, SYS_CLKR + 1, val8);
+		val8 = 0xFC;
+		r8712_write8(adapter, CR, val8);
+		val8 = 0x37;
+		r8712_write8(adapter, CR + 1, val8);
+		/* reduce EndPoint & init it */
+		r8712_write8(adapter, 0x102500ab, r8712_read8(adapter,
+			     0x102500ab) | BIT(6) | BIT(7));
+		/* consideration of power consumption - init */
+		r8712_write8(adapter, 0x10250008, r8712_read8(adapter,
+			     0x10250008) & 0xfffffffb);
+	} else if (registrypriv->chip_version == RTL8712_1stCUT) {
+		/* Initialization for power on sequence, */
+		r8712_write8(adapter, SPS0_CTRL + 1, 0x53);
+		r8712_write8(adapter, SPS0_CTRL, 0x57);
+		/* Enable AFE Macro Block's Bandgap and Enable AFE Macro
+		 * Block's Mbias
+		 */
+		val8 = r8712_read8(adapter, AFE_MISC);
+		r8712_write8(adapter, AFE_MISC, (val8 | AFE_MISC_BGEN |
+			     AFE_MISC_MBEN));
+		/* Enable LDOA15 block */
+		val8 = r8712_read8(adapter, LDOA15_CTRL);
+		r8712_write8(adapter, LDOA15_CTRL, (val8 | LDA15_EN));
+		val8 = r8712_read8(adapter, SPS1_CTRL);
+		r8712_write8(adapter, SPS1_CTRL, (val8 | SPS1_LDEN));
+		msleep(20);
+		/* Enable Switch Regulator Block */
+		val8 = r8712_read8(adapter, SPS1_CTRL);
+		r8712_write8(adapter, SPS1_CTRL, (val8 | SPS1_SWEN));
+		r8712_write32(adapter, SPS1_CTRL, 0x00a7b267);
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, (val8 | 0x08));
+		/* Engineer Packet CP test Enable */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x20));
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, (val8 & 0x6F));
+		/* Enable AFE clock */
+		val8 = r8712_read8(adapter, AFE_XTAL_CTRL + 1);
+		r8712_write8(adapter, AFE_XTAL_CTRL + 1, (val8 & 0xfb));
+		/* Enable AFE PLL Macro Block */
+		val8 = r8712_read8(adapter, AFE_PLL_CTRL);
+		r8712_write8(adapter, AFE_PLL_CTRL, (val8 | 0x11));
+		/* Attach AFE PLL to MACTOP/BB/PCIe Digital */
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL);
+		r8712_write8(adapter, SYS_ISO_CTRL, (val8 & 0xEE));
+		/* Switch to 40M clock */
+		val8 = r8712_read8(adapter, SYS_CLKR);
+		r8712_write8(adapter, SYS_CLKR, val8 & (~SYS_CLKSEL));
+		/* SSC Disable */
+		val8 = r8712_read8(adapter, SYS_CLKR);
+		/* Enable MAC clock */
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		r8712_write8(adapter, SYS_CLKR + 1, (val8 | 0x18));
+		/* Revised POS, */
+		r8712_write8(adapter, PMC_FSM, 0x02);
+		/* Enable Core digital and enable IOREG R/W */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x08));
+		/* Enable REG_EN */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x80));
+		/* Switch the control path to FW */
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		r8712_write8(adapter, SYS_CLKR + 1, (val8 | 0x80) & 0xBF);
+		r8712_write8(adapter, CR, 0xFC);
+		r8712_write8(adapter, CR + 1, 0x37);
+		/* Fix the RX FIFO issue(usb error), */
+		val8 = r8712_read8(adapter, 0x1025FE5c);
+		r8712_write8(adapter, 0x1025FE5c, (val8 | BIT(7)));
+		val8 = r8712_read8(adapter, 0x102500ab);
+		r8712_write8(adapter, 0x102500ab, (val8 | BIT(6) | BIT(7)));
+		/* For power save, used this in the bit file after 970621 */
+		val8 = r8712_read8(adapter, SYS_CLKR);
+		r8712_write8(adapter, SYS_CLKR, val8 & (~CPU_CLKSEL));
+	} else if (registrypriv->chip_version == RTL8712_2ndCUT ||
+		   registrypriv->chip_version == RTL8712_3rdCUT) {
+		/* Initialization for power on sequence,
+		 * E-Fuse leakage prevention sequence
+		 */
+		r8712_write8(adapter, 0x37, 0xb0);
+		msleep(20);
+		r8712_write8(adapter, 0x37, 0x30);
+		/* Set control path switch to HW control and reset Digital Core,
+		 * CPU Core and MAC I/O to solve FW download fail when system
+		 * from resume sate.
+		 */
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		if (val8 & 0x80) {
+			val8 &= 0x3f;
+			r8712_write8(adapter, SYS_CLKR + 1, val8);
+		}
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		val8 &= 0x73;
+		r8712_write8(adapter, SYS_FUNC_EN + 1, val8);
+		msleep(20);
+		/* Revised POS, */
+		/* Enable AFE Macro Block's Bandgap and Enable AFE Macro
+		 * Block's Mbias
+		 */
+		r8712_write8(adapter, SPS0_CTRL + 1, 0x53);
+		r8712_write8(adapter, SPS0_CTRL, 0x57);
+		val8 = r8712_read8(adapter, AFE_MISC);
+		/*Bandgap*/
+		r8712_write8(adapter, AFE_MISC, (val8 | AFE_MISC_BGEN));
+		r8712_write8(adapter, AFE_MISC, (val8 | AFE_MISC_BGEN |
+			     AFE_MISC_MBEN | AFE_MISC_I32_EN));
+		/* Enable PLL Power (LDOA15V) */
+		val8 = r8712_read8(adapter, LDOA15_CTRL);
+		r8712_write8(adapter, LDOA15_CTRL, (val8 | LDA15_EN));
+		/* Enable LDOV12D block */
+		val8 = r8712_read8(adapter, LDOV12D_CTRL);
+		r8712_write8(adapter, LDOV12D_CTRL, (val8 | LDV12_EN));
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, (val8 | 0x08));
+		/* Engineer Packet CP test Enable */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x20));
+		/* Support 64k IMEM */
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
+		r8712_write8(adapter, SYS_ISO_CTRL + 1, (val8 & 0x68));
+		/* Enable AFE clock */
+		val8 = r8712_read8(adapter, AFE_XTAL_CTRL + 1);
+		r8712_write8(adapter, AFE_XTAL_CTRL + 1, (val8 & 0xfb));
+		/* Enable AFE PLL Macro Block */
+		val8 = r8712_read8(adapter, AFE_PLL_CTRL);
+		r8712_write8(adapter, AFE_PLL_CTRL, (val8 | 0x11));
+		/* Some sample will download fw failure. The clock will be
+		 * stable with 500 us delay after reset the PLL
+		 * TODO: When usleep is added to kernel, change next 3
+		 * udelay(500) to usleep(500)
+		 */
+		udelay(500);
+		r8712_write8(adapter, AFE_PLL_CTRL, (val8 | 0x51));
+		udelay(500);
+		r8712_write8(adapter, AFE_PLL_CTRL, (val8 | 0x11));
+		udelay(500);
+		/* Attach AFE PLL to MACTOP/BB/PCIe Digital */
+		val8 = r8712_read8(adapter, SYS_ISO_CTRL);
+		r8712_write8(adapter, SYS_ISO_CTRL, (val8 & 0xEE));
+		/* Switch to 40M clock */
+		r8712_write8(adapter, SYS_CLKR, 0x00);
+		/* CPU Clock and 80M Clock SSC Disable to overcome FW download
+		 * fail timing issue.
+		 */
+		val8 = r8712_read8(adapter, SYS_CLKR);
+		r8712_write8(adapter, SYS_CLKR, (val8 | 0xa0));
+		/* Enable MAC clock */
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		r8712_write8(adapter, SYS_CLKR + 1, (val8 | 0x18));
+		/* Revised POS, */
+		r8712_write8(adapter, PMC_FSM, 0x02);
+		/* Enable Core digital and enable IOREG R/W */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x08));
+		/* Enable REG_EN */
+		val8 = r8712_read8(adapter, SYS_FUNC_EN + 1);
+		r8712_write8(adapter, SYS_FUNC_EN + 1, (val8 | 0x80));
+		/* Switch the control path to FW */
+		val8 = r8712_read8(adapter, SYS_CLKR + 1);
+		r8712_write8(adapter, SYS_CLKR + 1, (val8 | 0x80) & 0xBF);
+		r8712_write8(adapter, CR, 0xFC);
+		r8712_write8(adapter, CR + 1, 0x37);
+		/* Fix the RX FIFO issue(usb error), 970410 */
+		val8 = r8712_read8(adapter, 0x1025FE5c);
+		r8712_write8(adapter, 0x1025FE5c, (val8 | BIT(7)));
+		/* For power save, used this in the bit file after 970621 */
+		val8 = r8712_read8(adapter, SYS_CLKR);
+		r8712_write8(adapter, SYS_CLKR, val8 & (~CPU_CLKSEL));
+		/* Revised for 8051 ROM code wrong operation. */
+		r8712_write8(adapter, 0x1025fe1c, 0x80);
+		/* To make sure that TxDMA can ready to download FW.
+		 * We should reset TxDMA if IMEM RPT was not ready.
+		 */
+		do {
+			val8 = r8712_read8(adapter, TCR);
+			if ((val8 & _TXDMA_INIT_VALUE) == _TXDMA_INIT_VALUE)
+				break;
+			udelay(5); /* PlatformStallExecution(5); */
+		} while (PollingCnt--);	/* Delay 1ms */
+
+		if (PollingCnt <= 0) {
+			val8 = r8712_read8(adapter, CR);
+			r8712_write8(adapter, CR, val8 & (~_TXDMA_EN));
+			udelay(2); /* PlatformStallExecution(2); */
+			/* Reset TxDMA */
+			r8712_write8(adapter, CR, val8 | _TXDMA_EN);
+		}
+	} else {
+		ret = _FAIL;
+	}
+	return ret;
+}
+
+unsigned int r8712_usb_inirp_init(struct _adapter *adapter)
+{
+	u8 i;
+	struct recv_buf *recvbuf;
+	struct intf_hdl *intfhdl = &adapter->pio_queue->intf;
+	struct recv_priv *recvpriv = &(adapter->recvpriv);
+
+	recvpriv->ff_hwaddr = RTL8712_DMA_RX0FF; /* mapping rx fifo address */
+	/* issue Rx irp to receive data */
+	recvbuf = (struct recv_buf *)recvpriv->precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		if (r8712_usb_read_port(intfhdl, recvpriv->ff_hwaddr, 0,
+					(unsigned char *)recvbuf) == false)
+			return _FAIL;
+		recvbuf++;
+		recvpriv->free_recv_buf_queue_cnt--;
+	}
+	return _SUCCESS;
+}
+
+unsigned int r8712_usb_inirp_deinit(struct _adapter *adapter)
+{
+	r8712_usb_read_port_cancel(adapter);
+	return _SUCCESS;
+}
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
new file mode 100644
index 000000000..df05213f9
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -0,0 +1,638 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * usb_intf.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _HCI_INTF_C_
+
+#include <linux/usb.h>
+#include <linux/module.h>
+#include <linux/firmware.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "recv_osdep.h"
+#include "xmit_osdep.h"
+#include "rtl8712_efuse.h"
+#include "usb_ops.h"
+#include "usb_osintf.h"
+
+static struct usb_interface *pintf;
+
+static int r871xu_drv_init(struct usb_interface *pusb_intf,
+			   const struct usb_device_id *pdid);
+
+static void r871xu_dev_remove(struct usb_interface *pusb_intf);
+
+static const struct usb_device_id rtl871x_usb_id_tbl[] = {
+/* RTL8188SU */
+	/* Realtek */
+	{USB_DEVICE(0x0BDA, 0x8171)},
+	{USB_DEVICE(0x0bda, 0x8173)},
+	{USB_DEVICE(0x0bda, 0x8712)},
+	{USB_DEVICE(0x0bda, 0x8713)},
+	{USB_DEVICE(0x0bda, 0xC512)},
+	/* Abocom */
+	{USB_DEVICE(0x07B8, 0x8188)},
+	/* ASUS */
+	{USB_DEVICE(0x0B05, 0x1786)},
+	{USB_DEVICE(0x0B05, 0x1791)}, /* 11n mode disable */
+	/* Belkin */
+	{USB_DEVICE(0x050D, 0x945A)},
+	/* ISY IWL - Belkin clone */
+	{USB_DEVICE(0x050D, 0x11F1)},
+	/* Corega */
+	{USB_DEVICE(0x07AA, 0x0047)},
+	/* D-Link */
+	{USB_DEVICE(0x2001, 0x3306)},
+	{USB_DEVICE(0x07D1, 0x3306)}, /* 11n mode disable */
+	/* Edimax */
+	{USB_DEVICE(0x7392, 0x7611)},
+	/* EnGenius */
+	{USB_DEVICE(0x1740, 0x9603)},
+	/* Hawking */
+	{USB_DEVICE(0x0E66, 0x0016)},
+	/* Hercules */
+	{USB_DEVICE(0x06F8, 0xE034)},
+	{USB_DEVICE(0x06F8, 0xE032)},
+	/* Logitec */
+	{USB_DEVICE(0x0789, 0x0167)},
+	/* PCI */
+	{USB_DEVICE(0x2019, 0xAB28)},
+	{USB_DEVICE(0x2019, 0xED16)},
+	/* Sitecom */
+	{USB_DEVICE(0x0DF6, 0x0057)},
+	{USB_DEVICE(0x0DF6, 0x0045)},
+	{USB_DEVICE(0x0DF6, 0x0059)}, /* 11n mode disable */
+	{USB_DEVICE(0x0DF6, 0x004B)},
+	{USB_DEVICE(0x0DF6, 0x005B)},
+	{USB_DEVICE(0x0DF6, 0x005D)},
+	{USB_DEVICE(0x0DF6, 0x0063)},
+	/* Sweex */
+	{USB_DEVICE(0x177F, 0x0154)},
+	/* Thinkware */
+	{USB_DEVICE(0x0BDA, 0x5077)},
+	/* Toshiba */
+	{USB_DEVICE(0x1690, 0x0752)},
+	/* - */
+	{USB_DEVICE(0x20F4, 0x646B)},
+	{USB_DEVICE(0x083A, 0xC512)},
+	{USB_DEVICE(0x25D4, 0x4CA1)},
+	{USB_DEVICE(0x25D4, 0x4CAB)},
+
+/* RTL8191SU */
+	/* Realtek */
+	{USB_DEVICE(0x0BDA, 0x8172)},
+	{USB_DEVICE(0x0BDA, 0x8192)},
+	/* Amigo */
+	{USB_DEVICE(0x0EB0, 0x9061)},
+	/* ASUS/EKB */
+	{USB_DEVICE(0x13D3, 0x3323)},
+	{USB_DEVICE(0x13D3, 0x3311)}, /* 11n mode disable */
+	{USB_DEVICE(0x13D3, 0x3342)},
+	/* ASUS/EKBLenovo */
+	{USB_DEVICE(0x13D3, 0x3333)},
+	{USB_DEVICE(0x13D3, 0x3334)},
+	{USB_DEVICE(0x13D3, 0x3335)}, /* 11n mode disable */
+	{USB_DEVICE(0x13D3, 0x3336)}, /* 11n mode disable */
+	/* ASUS/Media BOX */
+	{USB_DEVICE(0x13D3, 0x3309)},
+	/* Belkin */
+	{USB_DEVICE(0x050D, 0x815F)},
+	/* D-Link */
+	{USB_DEVICE(0x07D1, 0x3302)},
+	{USB_DEVICE(0x07D1, 0x3300)},
+	{USB_DEVICE(0x07D1, 0x3303)},
+	/* Edimax */
+	{USB_DEVICE(0x7392, 0x7612)},
+	/* EnGenius */
+	{USB_DEVICE(0x1740, 0x9605)},
+	/* Guillemot */
+	{USB_DEVICE(0x06F8, 0xE031)},
+	/* Hawking */
+	{USB_DEVICE(0x0E66, 0x0015)},
+	/* Mediao */
+	{USB_DEVICE(0x13D3, 0x3306)},
+	/* PCI */
+	{USB_DEVICE(0x2019, 0xED18)},
+	{USB_DEVICE(0x2019, 0x4901)},
+	/* Sitecom */
+	{USB_DEVICE(0x0DF6, 0x0058)},
+	{USB_DEVICE(0x0DF6, 0x0049)},
+	{USB_DEVICE(0x0DF6, 0x004C)},
+	{USB_DEVICE(0x0DF6, 0x006C)},
+	{USB_DEVICE(0x0DF6, 0x0064)},
+	/* Skyworth */
+	{USB_DEVICE(0x14b2, 0x3300)},
+	{USB_DEVICE(0x14b2, 0x3301)},
+	{USB_DEVICE(0x14B2, 0x3302)},
+	/* - */
+	{USB_DEVICE(0x04F2, 0xAFF2)},
+	{USB_DEVICE(0x04F2, 0xAFF5)},
+	{USB_DEVICE(0x04F2, 0xAFF6)},
+	{USB_DEVICE(0x13D3, 0x3339)},
+	{USB_DEVICE(0x13D3, 0x3340)}, /* 11n mode disable */
+	{USB_DEVICE(0x13D3, 0x3341)}, /* 11n mode disable */
+	{USB_DEVICE(0x13D3, 0x3310)},
+	{USB_DEVICE(0x13D3, 0x3325)},
+
+/* RTL8192SU */
+	/* Realtek */
+	{USB_DEVICE(0x0BDA, 0x8174)},
+	/* Belkin */
+	{USB_DEVICE(0x050D, 0x845A)},
+	/* Corega */
+	{USB_DEVICE(0x07AA, 0x0051)},
+	/* Edimax */
+	{USB_DEVICE(0x7392, 0x7622)},
+	/* NEC */
+	{USB_DEVICE(0x0409, 0x02B6)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, rtl871x_usb_id_tbl);
+
+static struct specific_device_id specific_device_id_tbl[] = {
+	{.idVendor = 0x0b05, .idProduct = 0x1791,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x0df6, .idProduct = 0x0059,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13d3, .idProduct = 0x3306,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13D3, .idProduct = 0x3311,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13d3, .idProduct = 0x3335,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13d3, .idProduct = 0x3336,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13d3, .idProduct = 0x3340,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{.idVendor = 0x13d3, .idProduct = 0x3341,
+		 .flags = SPEC_DEV_ID_DISABLE_HT},
+	{}
+};
+
+struct drv_priv {
+	struct usb_driver r871xu_drv;
+	int drv_registered;
+};
+
+#ifdef CONFIG_PM
+static int r871x_suspend(struct usb_interface *pusb_intf, pm_message_t state)
+{
+	struct net_device *pnetdev = usb_get_intfdata(pusb_intf);
+	struct _adapter *padapter = netdev_priv(pnetdev);
+
+	netdev_info(pnetdev, "Suspending...\n");
+	padapter->suspended = true;
+	rtl871x_intf_stop(padapter);
+	if (pnetdev->netdev_ops->ndo_stop)
+		pnetdev->netdev_ops->ndo_stop(pnetdev);
+	mdelay(10);
+	netif_device_detach(pnetdev);
+	return 0;
+}
+
+static void rtl871x_intf_resume(struct _adapter *padapter)
+{
+	if (padapter->dvobjpriv.inirp_init)
+		padapter->dvobjpriv.inirp_init(padapter);
+}
+
+static int r871x_resume(struct usb_interface *pusb_intf)
+{
+	struct net_device *pnetdev = usb_get_intfdata(pusb_intf);
+	struct _adapter *padapter = netdev_priv(pnetdev);
+
+	netdev_info(pnetdev,  "Resuming...\n");
+	netif_device_attach(pnetdev);
+	if (pnetdev->netdev_ops->ndo_open)
+		pnetdev->netdev_ops->ndo_open(pnetdev);
+	padapter->suspended = false;
+	rtl871x_intf_resume(padapter);
+	return 0;
+}
+#endif
+
+static struct drv_priv drvpriv = {
+	.r871xu_drv.name = "r8712u",
+	.r871xu_drv.id_table = rtl871x_usb_id_tbl,
+	.r871xu_drv.probe = r871xu_drv_init,
+	.r871xu_drv.disconnect = r871xu_dev_remove,
+#ifdef CONFIG_PM
+	.r871xu_drv.suspend = r871x_suspend,
+	.r871xu_drv.resume = r871x_resume,
+#endif
+};
+
+static uint r8712_usb_dvobj_init(struct _adapter *padapter)
+{
+	uint	status = _SUCCESS;
+	struct	usb_host_interface		*phost_iface;
+	struct	usb_interface_descriptor	*piface_desc;
+	struct dvobj_priv *pdvobjpriv = &padapter->dvobjpriv;
+	struct usb_device *pusbd = pdvobjpriv->pusbdev;
+
+	pdvobjpriv->padapter = padapter;
+	padapter->eeprom_address_size = 6;
+	phost_iface = pintf->cur_altsetting;
+	piface_desc = &phost_iface->desc;
+	pdvobjpriv->nr_endpoint = piface_desc->bNumEndpoints;
+	if (pusbd->speed == USB_SPEED_HIGH) {
+		pdvobjpriv->ishighspeed = true;
+		dev_info(&pusbd->dev, "r8712u: USB_SPEED_HIGH with %d endpoints\n",
+			 pdvobjpriv->nr_endpoint);
+	} else {
+		pdvobjpriv->ishighspeed = false;
+		dev_info(&pusbd->dev, "r8712u: USB_SPEED_LOW with %d endpoints\n",
+			 pdvobjpriv->nr_endpoint);
+	}
+	if ((r8712_alloc_io_queue(padapter)) == _FAIL)
+		status = _FAIL;
+	return status;
+}
+
+static void r8712_usb_dvobj_deinit(struct _adapter *padapter)
+{
+	r8712_free_io_queue(padapter);
+}
+
+void rtl871x_intf_stop(struct _adapter *padapter)
+{
+	/*disable_hw_interrupt*/
+	if (!padapter->surprise_removed) {
+		/*device still exists, so driver can do i/o operation
+		 * TODO:
+		 */
+	}
+
+	/* cancel in irp */
+	if (padapter->dvobjpriv.inirp_deinit)
+		padapter->dvobjpriv.inirp_deinit(padapter);
+	/* cancel out irp */
+	r8712_usb_write_port_cancel(padapter);
+	/* TODO:cancel other irps */
+}
+
+void r871x_dev_unload(struct _adapter *padapter)
+{
+	if (padapter->bup) {
+		/*s1.*/
+		padapter->driver_stopped = true;
+
+		/*s3.*/
+		rtl871x_intf_stop(padapter);
+
+		/*s4.*/
+		r8712_stop_drv_threads(padapter);
+
+		/*s5.*/
+		if (!padapter->surprise_removed) {
+			padapter->hw_init_completed = false;
+			rtl8712_hal_deinit(padapter);
+		}
+
+		padapter->bup = false;
+	}
+}
+
+static void disable_ht_for_spec_devid(const struct usb_device_id *pdid,
+				      struct _adapter *padapter)
+{
+	u16 vid, pid;
+	u32 flags;
+	int i;
+	int num = ARRAY_SIZE(specific_device_id_tbl);
+
+	for (i = 0; i < num; i++) {
+		vid = specific_device_id_tbl[i].idVendor;
+		pid = specific_device_id_tbl[i].idProduct;
+		flags = specific_device_id_tbl[i].flags;
+
+		if ((pdid->idVendor == vid) && (pdid->idProduct == pid) &&
+		    (flags & SPEC_DEV_ID_DISABLE_HT)) {
+			padapter->registrypriv.ht_enable = 0;
+			padapter->registrypriv.cbw40_enable = 0;
+			padapter->registrypriv.ampdu_enable = 0;
+		}
+	}
+}
+
+static const struct device_type wlan_type = {
+	.name = "wlan",
+};
+
+/*
+ * drv_init() - a device potentially for us
+ *
+ * notes: drv_init() is called when the bus driver has located a card for us
+ * to support. We accept the new device by returning 0.
+ */
+static int r871xu_drv_init(struct usb_interface *pusb_intf,
+			   const struct usb_device_id *pdid)
+{
+	uint status;
+	struct _adapter *padapter = NULL;
+	struct dvobj_priv *pdvobjpriv;
+	struct net_device *pnetdev;
+	struct usb_device *udev;
+
+	/* In this probe function, O.S. will provide the usb interface pointer
+	 * to driver. We have to increase the reference count of the usb device
+	 * structure by using the usb_get_dev function.
+	 */
+	udev = interface_to_usbdev(pusb_intf);
+	usb_get_dev(udev);
+	pintf = pusb_intf;
+	/* step 1. */
+	pnetdev = r8712_init_netdev();
+	if (!pnetdev)
+		goto put_dev;
+	padapter = netdev_priv(pnetdev);
+	disable_ht_for_spec_devid(pdid, padapter);
+	pdvobjpriv = &padapter->dvobjpriv;
+	pdvobjpriv->padapter = padapter;
+	padapter->dvobjpriv.pusbdev = udev;
+	padapter->pusb_intf = pusb_intf;
+	usb_set_intfdata(pusb_intf, pnetdev);
+	SET_NETDEV_DEV(pnetdev, &pusb_intf->dev);
+	pnetdev->dev.type = &wlan_type;
+	/* step 2. */
+	padapter->dvobj_init = r8712_usb_dvobj_init;
+	padapter->dvobj_deinit = r8712_usb_dvobj_deinit;
+	padapter->halpriv.hal_bus_init = r8712_usb_hal_bus_init;
+	padapter->dvobjpriv.inirp_init = r8712_usb_inirp_init;
+	padapter->dvobjpriv.inirp_deinit = r8712_usb_inirp_deinit;
+	/* step 3.
+	 * initialize the dvobj_priv
+	 */
+
+	status = padapter->dvobj_init(padapter);
+	if (status != _SUCCESS)
+		goto free_netdev;
+
+	/* step 4. */
+	status = r8712_init_drv_sw(padapter);
+	if (status)
+		goto dvobj_deinit;
+	/* step 5. read efuse/eeprom data and get mac_addr */
+	{
+		int i, offset;
+		u8 mac[6];
+		u8 tmpU1b, AutoloadFail, eeprom_CustomerID;
+		u8 *pdata = padapter->eeprompriv.efuse_eeprom_data;
+
+		tmpU1b = r8712_read8(padapter, EE_9346CR);/*CR9346*/
+
+		/* To check system boot selection.*/
+		dev_info(&udev->dev, "r8712u: Boot from %s: Autoload %s\n",
+			 (tmpU1b & _9356SEL) ? "EEPROM" : "EFUSE",
+			 (tmpU1b & _EEPROM_EN) ? "OK" : "Failed");
+
+		/* To check autoload success or not.*/
+		if (tmpU1b & _EEPROM_EN) {
+			AutoloadFail = true;
+			/* The following operations prevent Efuse leakage by
+			 * turning on 2.5V.
+			 */
+			tmpU1b = r8712_read8(padapter, EFUSE_TEST + 3);
+			r8712_write8(padapter, EFUSE_TEST + 3, tmpU1b | 0x80);
+			msleep(20);
+			r8712_write8(padapter, EFUSE_TEST + 3,
+				     (tmpU1b & (~BIT(7))));
+
+			/* Retrieve Chip version.
+			 * Recognize IC version by Reg0x4 BIT15.
+			 */
+			tmpU1b = (u8)((r8712_read32(padapter, PMC_FSM) >> 15) &
+						    0x1F);
+			if (tmpU1b == 0x3)
+				padapter->registrypriv.chip_version =
+				     RTL8712_3rdCUT;
+			else
+				padapter->registrypriv.chip_version =
+				     (tmpU1b >> 1) + 1;
+			switch (padapter->registrypriv.chip_version) {
+			case RTL8712_1stCUT:
+			case RTL8712_2ndCUT:
+			case RTL8712_3rdCUT:
+				break;
+			default:
+				padapter->registrypriv.chip_version =
+				     RTL8712_2ndCUT;
+				break;
+			}
+
+			for (i = 0, offset = 0; i < 128; i += 8, offset++)
+				r8712_efuse_pg_packet_read(padapter, offset,
+						     &pdata[i]);
+
+			if (!r8712_initmac || !mac_pton(r8712_initmac, mac)) {
+				/* Use the mac address stored in the Efuse
+				 * offset = 0x12 for usb in efuse
+				 */
+				ether_addr_copy(mac, &pdata[0x12]);
+			}
+			eeprom_CustomerID = pdata[0x52];
+			switch (eeprom_CustomerID) {
+			case EEPROM_CID_ALPHA:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_ALPHA;
+				break;
+			case EEPROM_CID_CAMEO:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_CAMEO;
+				break;
+			case EEPROM_CID_SITECOM:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_Sitecom;
+				break;
+			case EEPROM_CID_COREGA:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_COREGA;
+				break;
+			case EEPROM_CID_Senao:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_Senao;
+				break;
+			case EEPROM_CID_EDIMAX_BELKIN:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_Edimax_Belkin;
+				break;
+			case EEPROM_CID_SERCOMM_BELKIN:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_Sercomm_Belkin;
+				break;
+			case EEPROM_CID_WNC_COREGA:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_WNC_COREGA;
+				break;
+			case EEPROM_CID_WHQL:
+				break;
+			case EEPROM_CID_NetCore:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_Netcore;
+				break;
+			case EEPROM_CID_CAMEO1:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_CAMEO1;
+				break;
+			case EEPROM_CID_CLEVO:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_819x_CLEVO;
+				break;
+			default:
+				padapter->eeprompriv.CustomerID =
+						 RT_CID_DEFAULT;
+				break;
+			}
+			dev_info(&udev->dev, "r8712u: CustomerID = 0x%.4x\n",
+				 padapter->eeprompriv.CustomerID);
+			/* Led mode */
+			switch (padapter->eeprompriv.CustomerID) {
+			case RT_CID_DEFAULT:
+			case RT_CID_819x_ALPHA:
+			case RT_CID_819x_CAMEO:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE1;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			case RT_CID_819x_Sitecom:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE2;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			case RT_CID_COREGA:
+			case RT_CID_819x_Senao:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE3;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			case RT_CID_819x_Edimax_Belkin:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE4;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			case RT_CID_819x_Sercomm_Belkin:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE5;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			case RT_CID_819x_WNC_COREGA:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE6;
+				padapter->ledpriv.bRegUseLed = true;
+				break;
+			default:
+				padapter->ledpriv.LedStrategy = SW_LED_MODE0;
+				padapter->ledpriv.bRegUseLed = false;
+				break;
+			}
+		} else {
+			AutoloadFail = false;
+		}
+		if ((!AutoloadFail) ||
+		    ((mac[0] == 0xff) && (mac[1] == 0xff) &&
+		     (mac[2] == 0xff) && (mac[3] == 0xff) &&
+		     (mac[4] == 0xff) && (mac[5] == 0xff)) ||
+		    ((mac[0] == 0x00) && (mac[1] == 0x00) &&
+		     (mac[2] == 0x00) && (mac[3] == 0x00) &&
+		     (mac[4] == 0x00) && (mac[5] == 0x00))) {
+			mac[0] = 0x00;
+			mac[1] = 0xe0;
+			mac[2] = 0x4c;
+			mac[3] = 0x87;
+			mac[4] = 0x00;
+			mac[5] = 0x00;
+		}
+		if (r8712_initmac) {
+			/* Make sure the user did not select a multicast
+			 * address by setting bit 1 of first octet.
+			 */
+			mac[0] &= 0xFE;
+			dev_info(&udev->dev,
+				"r8712u: MAC Address from user = %pM\n", mac);
+		} else {
+			dev_info(&udev->dev,
+				"r8712u: MAC Address from efuse = %pM\n", mac);
+		}
+		eth_hw_addr_set(pnetdev, mac);
+	}
+	/* step 6. Load the firmware asynchronously */
+	if (rtl871x_load_fw(padapter))
+		goto deinit_drv_sw;
+	init_completion(&padapter->rx_filter_ready);
+	return 0;
+
+deinit_drv_sw:
+	r8712_free_drv_sw(padapter);
+dvobj_deinit:
+	padapter->dvobj_deinit(padapter);
+free_netdev:
+	free_netdev(pnetdev);
+put_dev:
+	usb_put_dev(udev);
+	usb_set_intfdata(pusb_intf, NULL);
+	return -ENODEV;
+}
+
+/* rmmod module & unplug(SurpriseRemoved) will call r871xu_dev_remove()
+ * => how to recognize both
+ */
+static void r871xu_dev_remove(struct usb_interface *pusb_intf)
+{
+	struct net_device *pnetdev = usb_get_intfdata(pusb_intf);
+	struct usb_device *udev = interface_to_usbdev(pusb_intf);
+	struct _adapter *padapter = netdev_priv(pnetdev);
+
+	/* never exit with a firmware callback pending */
+	wait_for_completion(&padapter->rtl8712_fw_ready);
+	if (pnetdev->reg_state != NETREG_UNINITIALIZED)
+		unregister_netdev(pnetdev); /* will call netdev_close() */
+	usb_set_intfdata(pusb_intf, NULL);
+	release_firmware(padapter->fw);
+	if (drvpriv.drv_registered)
+		padapter->surprise_removed = true;
+	r8712_flush_rwctrl_works(padapter);
+	r8712_flush_led_works(padapter);
+	udelay(1);
+	/* Stop driver mlme relation timer */
+	r8712_stop_drv_timers(padapter);
+	r871x_dev_unload(padapter);
+	if (padapter->dvobj_deinit)
+		padapter->dvobj_deinit(padapter);
+	r8712_free_drv_sw(padapter);
+	free_netdev(pnetdev);
+
+	/* decrease the reference count of the usb device structure
+	 * when disconnect
+	 */
+	usb_put_dev(udev);
+
+	/* If we didn't unplug usb dongle and remove/insert module, driver
+	 * fails on sitesurvey for the first time when device is up.
+	 * Reset usb port for sitesurvey fail issue.
+	 */
+	if (udev->state != USB_STATE_NOTATTACHED)
+		usb_reset_device(udev);
+}
+
+static int __init r8712u_drv_entry(void)
+{
+	drvpriv.drv_registered = true;
+	return usb_register(&drvpriv.r871xu_drv);
+}
+
+static void __exit r8712u_drv_halt(void)
+{
+	drvpriv.drv_registered = false;
+	usb_deregister(&drvpriv.r871xu_drv);
+}
+
+module_init(r8712u_drv_entry);
+module_exit(r8712u_drv_halt);
diff --git a/drivers/staging/rtl8712/usb_ops.c b/drivers/staging/rtl8712/usb_ops.c
new file mode 100644
index 000000000..af9966d03
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_ops.c
@@ -0,0 +1,195 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * usb_ops.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _HCI_OPS_C_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+#include "recv_osdep.h"
+
+static u8 usb_read8(struct intf_hdl *intfhdl, u32 addr)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	int status;
+	__le32 data = 0;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x01; /* read_in */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 1;
+	status = r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index,
+					 &data, len, requesttype);
+	if (status < 0)
+		return 0;
+	return (u8)(le32_to_cpu(data) & 0x0ff);
+}
+
+static u16 usb_read16(struct intf_hdl *intfhdl, u32 addr)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	int status;
+	__le32 data = 0;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x01; /* read_in */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 2;
+	status = r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index,
+					 &data, len, requesttype);
+	if (status < 0)
+		return 0;
+	return (u16)(le32_to_cpu(data) & 0xffff);
+}
+
+static u32 usb_read32(struct intf_hdl *intfhdl, u32 addr)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	int status;
+	__le32 data = 0;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x01; /* read_in */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 4;
+	status = r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index,
+					 &data, len, requesttype);
+	if (status < 0)
+		return 0;
+	return le32_to_cpu(data);
+}
+
+static void usb_write8(struct intf_hdl *intfhdl, u32 addr, u8 val)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	__le32 data;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x00; /* write_out */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 1;
+	data = cpu_to_le32((u32)val & 0x000000ff);
+	r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index, &data, len,
+				requesttype);
+}
+
+static void usb_write16(struct intf_hdl *intfhdl, u32 addr, u16 val)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	__le32 data;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x00; /* write_out */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 2;
+	data = cpu_to_le32((u32)val & 0x0000ffff);
+	r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index, &data, len,
+				requesttype);
+}
+
+static void usb_write32(struct intf_hdl *intfhdl, u32 addr, u32 val)
+{
+	u8 request;
+	u8 requesttype;
+	u16 wvalue;
+	u16 index;
+	u16 len;
+	__le32 data;
+	struct intf_priv *intfpriv = intfhdl->pintfpriv;
+
+	request = 0x05;
+	requesttype = 0x00; /* write_out */
+	index = 0;
+	wvalue = (u16)(addr & 0x0000ffff);
+	len = 4;
+	data = cpu_to_le32(val);
+	r8712_usbctrl_vendorreq(intfpriv, request, wvalue, index, &data, len,
+				requesttype);
+}
+
+void r8712_usb_set_intf_option(u32 *option)
+{
+	*option = ((*option) | _INTF_ASYNC_);
+}
+
+static void usb_intf_hdl_init(u8 *priv)
+{
+}
+
+static void usb_intf_hdl_unload(u8 *priv)
+{
+}
+
+static void usb_intf_hdl_open(u8 *priv)
+{
+}
+
+static void usb_intf_hdl_close(u8 *priv)
+{
+}
+
+void r8712_usb_set_intf_funs(struct intf_hdl *intfhdl)
+{
+	intfhdl->intf_hdl_init = usb_intf_hdl_init;
+	intfhdl->intf_hdl_unload = usb_intf_hdl_unload;
+	intfhdl->intf_hdl_open = usb_intf_hdl_open;
+	intfhdl->intf_hdl_close = usb_intf_hdl_close;
+}
+
+void r8712_usb_set_intf_ops(struct _io_ops *ops)
+{
+	memset((u8 *)ops, 0, sizeof(struct _io_ops));
+	ops->_read8 = usb_read8;
+	ops->_read16 = usb_read16;
+	ops->_read32 = usb_read32;
+	ops->_read_port = r8712_usb_read_port;
+	ops->_write8 = usb_write8;
+	ops->_write16 = usb_write16;
+	ops->_write32 = usb_write32;
+	ops->_write_mem = r8712_usb_write_mem;
+	ops->_write_port = r8712_usb_write_port;
+}
diff --git a/drivers/staging/rtl8712/usb_ops.h b/drivers/staging/rtl8712/usb_ops.h
new file mode 100644
index 000000000..7a6b619b7
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_ops.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __USB_OPS_H_
+#define __USB_OPS_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+
+void r8712_usb_write_mem(struct intf_hdl *pintfhdl, u32 addr,
+			 u32 cnt, u8 *wmem);
+u32 r8712_usb_write_port(struct intf_hdl *pintfhdl, u32 addr,
+			 u32 cnt, u8 *wmem);
+u32 r8712_usb_read_port(struct intf_hdl *pintfhdl, u32 addr,
+			u32 cnt, u8 *rmem);
+void r8712_usb_set_intf_option(u32 *poption);
+void r8712_usb_set_intf_funs(struct intf_hdl *pintf_hdl);
+uint r8712_usb_init_intf_priv(struct intf_priv *pintfpriv);
+void r8712_usb_unload_intf_priv(struct intf_priv *pintfpriv);
+void r8712_usb_set_intf_ops(struct _io_ops *pops);
+void r8712_usb_read_port_cancel(struct _adapter *padapter);
+void r8712_usb_write_port_cancel(struct _adapter *padapter);
+int r8712_usbctrl_vendorreq(struct intf_priv *pintfpriv, u8 request, u16 value,
+			    u16 index, void *pdata, u16 len, u8 requesttype);
+
+#endif
+
diff --git a/drivers/staging/rtl8712/usb_ops_linux.c b/drivers/staging/rtl8712/usb_ops_linux.c
new file mode 100644
index 000000000..b2181e1e2
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_ops_linux.c
@@ -0,0 +1,515 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * usb_ops_linux.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _HCI_OPS_OS_C_
+
+#include <linux/usb.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+#include "osdep_intf.h"
+#include "usb_ops.h"
+
+#define	RTL871X_VENQT_READ	0xc0
+#define	RTL871X_VENQT_WRITE	0x40
+
+struct zero_bulkout_context {
+	void *pbuf;
+	void *purb;
+	void *pirp;
+	void *padapter;
+};
+
+uint r8712_usb_init_intf_priv(struct intf_priv *pintfpriv)
+{
+	pintfpriv->piorw_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!pintfpriv->piorw_urb)
+		return _FAIL;
+	init_completion(&pintfpriv->io_retevt_comp);
+	return _SUCCESS;
+}
+
+void r8712_usb_unload_intf_priv(struct intf_priv *pintfpriv)
+{
+	if (pintfpriv->piorw_urb) {
+		usb_kill_urb(pintfpriv->piorw_urb);
+		usb_free_urb(pintfpriv->piorw_urb);
+	}
+}
+
+static unsigned int ffaddr2pipehdl(struct dvobj_priv *pdvobj, u32 addr)
+{
+	unsigned int pipe = 0;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+
+	if (pdvobj->nr_endpoint == 11) {
+		switch (addr) {
+		case RTL8712_DMA_BKQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x07);
+			break;
+		case RTL8712_DMA_BEQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x06);
+			break;
+		case RTL8712_DMA_VIQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x05);
+			break;
+		case RTL8712_DMA_VOQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x04);
+			break;
+		case RTL8712_DMA_BCNQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x0a);
+			break;
+		case RTL8712_DMA_BMCQ:	/* HI Queue */
+			pipe = usb_sndbulkpipe(pusbd, 0x0b);
+			break;
+		case RTL8712_DMA_MGTQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x0c);
+			break;
+		case RTL8712_DMA_RX0FF:
+			pipe = usb_rcvbulkpipe(pusbd, 0x03); /* in */
+			break;
+		case RTL8712_DMA_C2HCMD:
+			pipe = usb_rcvbulkpipe(pusbd, 0x09); /* in */
+			break;
+		case RTL8712_DMA_H2CCMD:
+			pipe = usb_sndbulkpipe(pusbd, 0x0d);
+			break;
+		}
+	} else if (pdvobj->nr_endpoint == 6) {
+		switch (addr) {
+		case RTL8712_DMA_BKQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x07);
+			break;
+		case RTL8712_DMA_BEQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x06);
+			break;
+		case RTL8712_DMA_VIQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x05);
+			break;
+		case RTL8712_DMA_VOQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x04);
+			break;
+		case RTL8712_DMA_RX0FF:
+		case RTL8712_DMA_C2HCMD:
+			pipe = usb_rcvbulkpipe(pusbd, 0x03); /* in */
+			break;
+		case RTL8712_DMA_H2CCMD:
+		case RTL8712_DMA_BCNQ:
+		case RTL8712_DMA_BMCQ:
+		case RTL8712_DMA_MGTQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x0d);
+			break;
+		}
+	} else if (pdvobj->nr_endpoint == 4) {
+		switch (addr) {
+		case RTL8712_DMA_BEQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x06);
+			break;
+		case RTL8712_DMA_VOQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x04);
+			break;
+		case RTL8712_DMA_RX0FF:
+		case RTL8712_DMA_C2HCMD:
+			pipe = usb_rcvbulkpipe(pusbd, 0x03); /* in */
+			break;
+		case RTL8712_DMA_H2CCMD:
+		case RTL8712_DMA_BCNQ:
+		case RTL8712_DMA_BMCQ:
+		case RTL8712_DMA_MGTQ:
+			pipe = usb_sndbulkpipe(pusbd, 0x0d);
+			break;
+		}
+	} else {
+		pipe = 0;
+	}
+	return pipe;
+}
+
+static void usb_write_mem_complete(struct urb *purb)
+{
+	struct io_queue *pio_q = (struct io_queue *)purb->context;
+	struct intf_hdl *pintf = &(pio_q->intf);
+	struct intf_priv *pintfpriv = pintf->pintfpriv;
+	struct _adapter *padapter = (struct _adapter *)pintf->adapter;
+
+	if (purb->status != 0) {
+		if (purb->status == (-ESHUTDOWN))
+			padapter->driver_stopped = true;
+		else
+			padapter->surprise_removed = true;
+	}
+	complete(&pintfpriv->io_retevt_comp);
+}
+
+void r8712_usb_write_mem(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
+{
+	unsigned int pipe;
+	struct _adapter *padapter = (struct _adapter *)pintfhdl->adapter;
+	struct intf_priv *pintfpriv = pintfhdl->pintfpriv;
+	struct io_queue *pio_queue = padapter->pio_queue;
+	struct dvobj_priv *pdvobj = (struct dvobj_priv *)pintfpriv->intf_dev;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+	struct urb *piorw_urb = pintfpriv->piorw_urb;
+
+	if ((padapter->driver_stopped) || (padapter->surprise_removed) ||
+	    (padapter->pwrctrlpriv.pnp_bstop_trx))
+		return;
+	/* translate DMA FIFO addr to pipehandle */
+	pipe = ffaddr2pipehdl(pdvobj, addr);
+	if (pipe == 0)
+		return;
+	usb_fill_bulk_urb(piorw_urb, pusbd, pipe,
+			  wmem, cnt, usb_write_mem_complete,
+			  pio_queue);
+	usb_submit_urb(piorw_urb, GFP_ATOMIC);
+	wait_for_completion_interruptible(&pintfpriv->io_retevt_comp);
+}
+
+static void r8712_usb_read_port_complete(struct urb *purb)
+{
+	uint isevt;
+	__le32 *pbuf;
+	struct recv_buf	*precvbuf = (struct recv_buf *)purb->context;
+	struct _adapter *padapter = (struct _adapter *)precvbuf->adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	if (padapter->surprise_removed || padapter->driver_stopped)
+		return;
+	if (purb->status == 0) { /* SUCCESS */
+		if ((purb->actual_length > (MAX_RECVBUF_SZ)) ||
+		    (purb->actual_length < RXDESC_SIZE)) {
+			r8712_read_port(padapter, precvpriv->ff_hwaddr, 0,
+				  (unsigned char *)precvbuf);
+		} else {
+			_pkt *pskb = precvbuf->pskb;
+
+			precvbuf->transfer_len = purb->actual_length;
+			pbuf = (__le32 *)precvbuf->pbuf;
+			isevt = le32_to_cpu(*(pbuf + 1)) & 0x1ff;
+			if ((isevt & 0x1ff) == 0x1ff) {
+				r8712_rxcmd_event_hdl(padapter, pbuf);
+				skb_queue_tail(&precvpriv->rx_skb_queue, pskb);
+				r8712_read_port(padapter, precvpriv->ff_hwaddr,
+						0, (unsigned char *)precvbuf);
+			} else {
+				skb_put(pskb, purb->actual_length);
+				skb_queue_tail(&precvpriv->rx_skb_queue, pskb);
+				tasklet_hi_schedule(&precvpriv->recv_tasklet);
+				r8712_read_port(padapter, precvpriv->ff_hwaddr,
+						0, (unsigned char *)precvbuf);
+			}
+		}
+	} else {
+		switch (purb->status) {
+		case -EINVAL:
+		case -EPIPE:
+		case -ENODEV:
+		case -ESHUTDOWN:
+			padapter->driver_stopped = true;
+			break;
+		case -ENOENT:
+			if (!padapter->suspended) {
+				padapter->driver_stopped = true;
+				break;
+			}
+			fallthrough;
+		case -EPROTO:
+			r8712_read_port(padapter, precvpriv->ff_hwaddr, 0,
+				  (unsigned char *)precvbuf);
+			break;
+		case -EINPROGRESS:
+			netdev_err(padapter->pnetdev, "ERROR: URB IS IN PROGRESS!\n");
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+u32 r8712_usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
+{
+	unsigned int pipe;
+	int err;
+	u32 tmpaddr = 0;
+	int alignment = 0;
+	u32 ret = _SUCCESS;
+	struct urb *purb = NULL;
+	struct recv_buf	*precvbuf = (struct recv_buf *)rmem;
+	struct intf_priv *pintfpriv = pintfhdl->pintfpriv;
+	struct dvobj_priv *pdvobj = (struct dvobj_priv *)pintfpriv->intf_dev;
+	struct _adapter *adapter = pdvobj->padapter;
+	struct recv_priv *precvpriv = &adapter->recvpriv;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+
+	if (adapter->driver_stopped || adapter->surprise_removed ||
+	    adapter->pwrctrlpriv.pnp_bstop_trx || !precvbuf)
+		return _FAIL;
+	r8712_init_recvbuf(adapter, precvbuf);
+	/* Try to use skb from the free queue */
+	precvbuf->pskb = skb_dequeue(&precvpriv->free_recv_skb_queue);
+
+	if (!precvbuf->pskb) {
+		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev,
+				 MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
+		if (!precvbuf->pskb)
+			return _FAIL;
+		tmpaddr = (addr_t)precvbuf->pskb->data;
+		alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+		skb_reserve(precvbuf->pskb,
+			    (RECVBUFF_ALIGN_SZ - alignment));
+		precvbuf->phead = precvbuf->pskb->head;
+		precvbuf->pdata = precvbuf->pskb->data;
+		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
+		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
+		precvbuf->pbuf = precvbuf->pskb->data;
+	} else { /* skb is reused */
+		precvbuf->phead = precvbuf->pskb->head;
+		precvbuf->pdata = precvbuf->pskb->data;
+		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
+		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
+		precvbuf->pbuf = precvbuf->pskb->data;
+	}
+	purb = precvbuf->purb;
+	/* translate DMA FIFO addr to pipehandle */
+	pipe = ffaddr2pipehdl(pdvobj, addr);
+	usb_fill_bulk_urb(purb, pusbd, pipe,
+			  precvbuf->pbuf, MAX_RECVBUF_SZ,
+			  r8712_usb_read_port_complete,
+			  precvbuf);
+	err = usb_submit_urb(purb, GFP_ATOMIC);
+	if ((err) && (err != (-EPERM)))
+		ret = _FAIL;
+	return ret;
+}
+
+void r8712_usb_read_port_cancel(struct _adapter *padapter)
+{
+	int i;
+	struct recv_buf *precvbuf;
+
+	precvbuf = (struct recv_buf *)padapter->recvpriv.precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		if (precvbuf->purb)
+			usb_kill_urb(precvbuf->purb);
+		precvbuf++;
+	}
+}
+
+void r8712_xmit_bh(struct tasklet_struct *t)
+{
+	int ret = false;
+	struct _adapter *padapter = from_tasklet(padapter, t,
+						 xmitpriv.xmit_tasklet);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->driver_stopped ||
+	    padapter->surprise_removed) {
+		netdev_err(padapter->pnetdev, "xmit_bh => driver_stopped or surprise_removed\n");
+		return;
+	}
+	ret = r8712_xmitframe_complete(padapter, pxmitpriv, NULL);
+	if (!ret)
+		return;
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+}
+
+static void usb_write_port_complete(struct urb *purb)
+{
+	int i;
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)purb->context;
+	struct xmit_buf *pxmitbuf = pxmitframe->pxmitbuf;
+	struct _adapter *padapter = pxmitframe->padapter;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	switch (pattrib->priority) {
+	case 1:
+	case 2:
+		pxmitpriv->bkq_cnt--;
+		break;
+	case 4:
+	case 5:
+		pxmitpriv->viq_cnt--;
+		break;
+	case 6:
+	case 7:
+		pxmitpriv->voq_cnt--;
+		break;
+	case 0:
+	case 3:
+	default:
+		pxmitpriv->beq_cnt--;
+		break;
+	}
+	pxmitpriv->txirp_cnt--;
+	for (i = 0; i < 8; i++) {
+		if (purb == pxmitframe->pxmit_urb[i]) {
+			pxmitframe->bpending[i] = false;
+			break;
+		}
+	}
+	if (padapter->surprise_removed)
+		return;
+	switch (purb->status) {
+	case 0:
+		break;
+	default:
+		netdev_warn(padapter->pnetdev,
+				"r8712u: pipe error: (%d)\n", purb->status);
+		break;
+	}
+	/* not to consider tx fragment */
+	r8712_free_xmitframe_ex(pxmitpriv, pxmitframe);
+	r8712_free_xmitbuf(pxmitpriv, pxmitbuf);
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+}
+
+u32 r8712_usb_write_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
+{
+	unsigned long irqL;
+	int i, status;
+	unsigned int pipe;
+	u32 ret, bwritezero;
+	struct urb *purb = NULL;
+	struct _adapter *padapter = (struct _adapter *)pintfhdl->adapter;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = (struct xmit_frame *)wmem;
+	struct usb_device *pusbd = pdvobj->pusbdev;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	if ((padapter->driver_stopped) || (padapter->surprise_removed) ||
+	    (padapter->pwrctrlpriv.pnp_bstop_trx))
+		return _FAIL;
+	for (i = 0; i < 8; i++) {
+		if (!pxmitframe->bpending[i]) {
+			spin_lock_irqsave(&pxmitpriv->lock, irqL);
+			pxmitpriv->txirp_cnt++;
+			pxmitframe->bpending[i]  = true;
+			switch (pattrib->priority) {
+			case 1:
+			case 2:
+				pxmitpriv->bkq_cnt++;
+				break;
+			case 4:
+			case 5:
+				pxmitpriv->viq_cnt++;
+				break;
+			case 6:
+			case 7:
+				pxmitpriv->voq_cnt++;
+				break;
+			case 0:
+			case 3:
+			default:
+				pxmitpriv->beq_cnt++;
+				break;
+			}
+			spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
+			pxmitframe->sz[i] = (u16)cnt;
+			purb = pxmitframe->pxmit_urb[i];
+			break;
+		}
+	}
+	bwritezero = false;
+	if (pdvobj->ishighspeed) {
+		if (cnt > 0 && cnt % 512 == 0)
+			bwritezero = true;
+	} else {
+		if (cnt > 0 && cnt % 64 == 0)
+			bwritezero = true;
+	}
+	/* translate DMA FIFO addr to pipehandle */
+	pipe = ffaddr2pipehdl(pdvobj, addr);
+	if (pxmitpriv->free_xmitbuf_cnt % NR_XMITBUFF == 0)
+		purb->transfer_flags  &=  (~URB_NO_INTERRUPT);
+	else
+		purb->transfer_flags  |=  URB_NO_INTERRUPT;
+	if (bwritezero)
+		cnt += 8;
+	usb_fill_bulk_urb(purb, pusbd, pipe,
+			  pxmitframe->mem_addr,
+			  cnt, usb_write_port_complete,
+			  pxmitframe); /* context is xmit_frame */
+	status = usb_submit_urb(purb, GFP_ATOMIC);
+	if (!status)
+		ret = _SUCCESS;
+	else
+		ret = _FAIL;
+	return ret;
+}
+
+void r8712_usb_write_port_cancel(struct _adapter *padapter)
+{
+	int i, j;
+	struct xmit_buf	*pxmitbuf = (struct xmit_buf *)
+				     padapter->xmitpriv.pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		for (j = 0; j < 8; j++) {
+			if (pxmitbuf->pxmit_urb[j])
+				usb_kill_urb(pxmitbuf->pxmit_urb[j]);
+		}
+		pxmitbuf++;
+	}
+}
+
+int r8712_usbctrl_vendorreq(struct intf_priv *pintfpriv, u8 request, u16 value,
+		      u16 index, void *pdata, u16 len, u8 requesttype)
+{
+	unsigned int pipe;
+	int status;
+	u8 reqtype;
+	struct dvobj_priv *pdvobjpriv = (struct dvobj_priv *)
+					 pintfpriv->intf_dev;
+	struct usb_device *udev = pdvobjpriv->pusbdev;
+	/* For mstar platform, mstar suggests the address for USB IO
+	 * should be 16 bytes alignment. Trying to fix it here.
+	 */
+	u8 *palloc_buf, *pIo_buf;
+
+	palloc_buf = kmalloc((u32)len + 16, GFP_ATOMIC);
+	if (!palloc_buf)
+		return -ENOMEM;
+	pIo_buf = palloc_buf + 16 - ((addr_t)(palloc_buf) & 0x0f);
+	if (requesttype == 0x01) {
+		pipe = usb_rcvctrlpipe(udev, 0); /* read_in */
+		reqtype =  RTL871X_VENQT_READ;
+	} else {
+		pipe = usb_sndctrlpipe(udev, 0); /* write_out */
+		reqtype =  RTL871X_VENQT_WRITE;
+		memcpy(pIo_buf, pdata, len);
+	}
+	status = usb_control_msg(udev, pipe, request, reqtype, value, index,
+				 pIo_buf, len, 500);
+	if (status < 0)
+		goto free;
+	if (status != len) {
+		status = -EREMOTEIO;
+		goto free;
+	}
+	/* Success this control transfer. */
+	if (requesttype == 0x01) {
+		/* For Control read transfer, we have to copy the read
+		 * data from pIo_buf to pdata.
+		 */
+		memcpy(pdata, pIo_buf, status);
+	}
+
+free:
+	kfree(palloc_buf);
+	return status;
+}
diff --git a/drivers/staging/rtl8712/usb_osintf.h b/drivers/staging/rtl8712/usb_osintf.h
new file mode 100644
index 000000000..2e512b4a5
--- /dev/null
+++ b/drivers/staging/rtl8712/usb_osintf.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __USB_OSINTF_H
+#define __USB_OSINTF_H
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+extern char *r8712_initmac;
+
+unsigned int r8712_usb_inirp_init(struct _adapter *padapter);
+unsigned int r8712_usb_inirp_deinit(struct _adapter *padapter);
+uint rtl871x_hal_init(struct _adapter *padapter);
+uint rtl8712_hal_deinit(struct _adapter *padapter);
+
+void rtl871x_intf_stop(struct _adapter *padapter);
+void r871x_dev_unload(struct _adapter *padapter);
+void r8712_stop_drv_threads(struct _adapter *padapter);
+void r8712_stop_drv_timers(struct _adapter *padapter);
+int r8712_init_drv_sw(struct _adapter *padapter);
+void r8712_free_drv_sw(struct _adapter *padapter);
+struct net_device *r8712_init_netdev(void);
+
+#endif
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
new file mode 100644
index 000000000..498e6dec7
--- /dev/null
+++ b/drivers/staging/rtl8712/wifi.h
@@ -0,0 +1,196 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef _WIFI_H_
+#define _WIFI_H_
+
+#include <linux/compiler.h>
+#include <linux/ieee80211.h>
+
+#define WLAN_HDR_A3_LEN		24
+#define WLAN_HDR_A3_QOS_LEN	26
+
+enum WIFI_FRAME_TYPE {
+	WIFI_QOS_DATA_TYPE	= (BIT(7) | BIT(3)),	/*!< QoS Data */
+};
+
+#define SetToDs(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_TODS); \
+})
+
+#define GetToDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(IEEE80211_FCTL_TODS)) != 0)
+
+#define ClearToDs(pbuf)	({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_TODS)); \
+})
+
+#define SetFrDs(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_FROMDS); \
+})
+
+#define GetFrDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(IEEE80211_FCTL_FROMDS)) != 0)
+
+#define ClearFrDs(pbuf)	({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_FROMDS)); \
+})
+
+static inline unsigned char get_tofr_ds(unsigned char *pframe)
+{
+	return ((GetToDs(pframe) << 1) | GetFrDs(pframe));
+}
+
+#define SetMFrag(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS); \
+})
+
+#define GetMFrag(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) != 0)
+
+#define ClearMFrag(pbuf) ({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)); \
+})
+
+#define SetRetry(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_RETRY); \
+})
+
+#define GetRetry(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(IEEE80211_FCTL_RETRY)) != 0)
+
+#define ClearRetry(pbuf) ({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_RETRY)); \
+})
+
+#define SetPwrMgt(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PM); \
+})
+
+#define GetPwrMgt(pbuf)	(((*(__le16 *)(pbuf)) & \
+			cpu_to_le16(IEEE80211_FCTL_PM)) != 0)
+
+#define ClearPwrMgt(pbuf) ({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_PM)); \
+})
+
+#define SetMData(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREDATA); \
+})
+
+#define GetMData(pbuf)	(((*(__le16 *)(pbuf)) & \
+			cpu_to_le16(IEEE80211_FCTL_MOREDATA)) != 0)
+
+#define ClearMData(pbuf) ({ \
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_MOREDATA)); \
+})
+
+#define SetPrivacy(pbuf) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PROTECTED); \
+})
+
+#define GetPrivacy(pbuf)	(((*(__le16 *)(pbuf)) & \
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED)) != 0)
+
+#define GetOrder(pbuf)	(((*(__le16 *)(pbuf)) & \
+			cpu_to_le16(IEEE80211_FCTL_ORDER)) != 0)
+
+#define GetFrameType(pbuf)	(le16_to_cpu(*(__le16 *)(pbuf)) & \
+				(BIT(3) | BIT(2)))
+
+#define SetFrameType(pbuf, type)	\
+	do {	\
+		*(__le16 *)(pbuf) &= cpu_to_le16(~(BIT(3) | \
+		BIT(2))); \
+		*(__le16 *)(pbuf) |= cpu_to_le16(type); \
+	} while (0)
+
+#define GetFrameSubType(pbuf)	(le16_to_cpu(*(__le16 *)(pbuf)) & \
+				(BIT(7) | BIT(6) | BIT(5) | BIT(4) | BIT(3) | \
+				BIT(2)))
+
+#define SetFrameSubType(pbuf, type) \
+	do {    \
+		*(__le16 *)(pbuf) &= cpu_to_le16(~(BIT(7) | BIT(6) | \
+		BIT(5) | BIT(4) | BIT(3) | BIT(2))); \
+		*(__le16 *)(pbuf) |= cpu_to_le16(type); \
+	} while (0)
+
+#define GetSequence(pbuf)	(le16_to_cpu(*(__le16 *)\
+				((addr_t)(pbuf) + 22)) >> 4)
+
+#define GetFragNum(pbuf)	(le16_to_cpu(*(__le16 *)((addr_t)\
+				(pbuf) + 22)) & 0x0f)
+
+#define SetSeqNum(pbuf, num) ({ \
+	*(__le16 *)((addr_t)(pbuf) + 22) = \
+	cpu_to_le16((le16_to_cpu(*(__le16 *)((addr_t)(pbuf) + 22)) & \
+	0x000f) | (0xfff0 & (num << 4))); \
+})
+
+#define SetPriority(pbuf, tid) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16(tid & 0xf); \
+})
+
+#define GetPriority(pbuf)	((le16_to_cpu(*(__le16 *)(pbuf))) & 0xf)
+
+#define SetAckpolicy(pbuf, ack) ({ \
+	*(__le16 *)(pbuf) |= cpu_to_le16((ack & 3) << 5); \
+})
+
+#define GetAckpolicy(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 5) & 0x3)
+
+#define GetAMsdu(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 7) & 0x1)
+
+#define GetAddr1Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 4))
+
+#define GetAddr2Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 10))
+
+#define GetAddr3Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 16))
+
+#define GetAddr4Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 24))
+
+static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
+{
+	unsigned char	*sa;
+	unsigned int	to_fr_ds = (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/* ToDs=0, FromDs=0 */
+		sa = GetAddr3Ptr(pframe);
+		break;
+	case 0x01:	/* ToDs=0, FromDs=1 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	case 0x02:	/* ToDs=1, FromDs=0 */
+		sa = GetAddr1Ptr(pframe);
+		break;
+	default:	/* ToDs=1, FromDs=1 */
+		sa = NULL;
+		break;
+	}
+	return sa;
+}
+
+/* ---------------------------------------------------------------------------
+ *			Below is the fixed elements...
+ * ---------------------------------------------------------------------------
+ */
+#define _BEACON_ITERVAL_		2
+#define _CAPABILITY_			2
+#define _TIMESTAMP_				8
+
+/*-----------------------------------------------------------------------------
+ *			Below is the definition for WMM
+ *------------------------------------------------------------------------------
+ */
+#define _WMM_IE_Length_				7  /* for WMM STA */
+
+#endif /* _WIFI_H_ */
+
diff --git a/drivers/staging/rtl8712/wlan_bssdef.h b/drivers/staging/rtl8712/wlan_bssdef.h
new file mode 100644
index 000000000..ec3749813
--- /dev/null
+++ b/drivers/staging/rtl8712/wlan_bssdef.h
@@ -0,0 +1,223 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __WLAN_BSSDEF_H__
+#define __WLAN_BSSDEF_H__
+
+#define MAX_IE_SZ	768
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+
+struct ndis_802_11_ssid {
+	u32 SsidLength;
+	u8  Ssid[32];
+};
+
+enum NDIS_802_11_NETWORK_TYPE {
+	Ndis802_11FH,
+	Ndis802_11DS,
+	Ndis802_11OFDM5,
+	Ndis802_11OFDM24,
+	Ndis802_11NetworkTypeMax /* not a real type, defined as an upper bound*/
+};
+
+struct NDIS_802_11_CONFIGURATION_FH {
+	u32 Length;             /* Length of structure */
+	u32 HopPattern;         /* As defined by 802.11, MSB set */
+	u32 HopSet;             /* to one if non-802.11 */
+	u32 DwellTime;          /* units are Kusec */
+};
+
+/*
+ * FW will only save the channel number in DSConfig.
+ * ODI Handler will convert the channel number to freq. number.
+ */
+struct NDIS_802_11_CONFIGURATION {
+	u32 Length;             /* Length of structure */
+	u32 BeaconPeriod;       /* units are Kusec */
+	u32 ATIMWindow;         /* units are Kusec */
+	u32 DSConfig;           /* Frequency, units are kHz */
+	struct NDIS_802_11_CONFIGURATION_FH FHConfig;
+};
+
+enum NDIS_802_11_NETWORK_INFRASTRUCTURE {
+	Ndis802_11IBSS,
+	Ndis802_11Infrastructure,
+	Ndis802_11AutoUnknown,
+	Ndis802_11InfrastructureMax, /*Not a real value,defined as upper bound*/
+	Ndis802_11APMode
+};
+
+struct NDIS_802_11_FIXED_IEs {
+	u8  Timestamp[8];
+	u16 BeaconInterval;
+	u16 Capabilities;
+};
+
+struct wlan_bssid_ex {
+	u32 Length;
+	unsigned char  MacAddress[6];
+	u8  Reserved[2];
+	struct ndis_802_11_ssid  Ssid;
+	__le32 Privacy;
+	s32 Rssi;
+	enum NDIS_802_11_NETWORK_TYPE  NetworkTypeInUse;
+	struct NDIS_802_11_CONFIGURATION  Configuration;
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+	u8 rates[NDIS_802_11_LENGTH_RATES_EX];
+	/* number of content bytes in EIs, which varies */
+	u32 IELength;
+	/*(timestamp, beacon interval, and capability information) */
+	u8 IEs[MAX_IE_SZ];
+};
+
+enum NDIS_802_11_AUTHENTICATION_MODE {
+	Ndis802_11AuthModeOpen,
+	Ndis802_11AuthModeShared,
+	Ndis802_11AuthModeAutoSwitch,
+	Ndis802_11AuthModeWPA,
+	Ndis802_11AuthModeWPAPSK,
+	Ndis802_11AuthModeWPANone,
+	Ndis802_11AuthModeMax      /* Not a real mode, defined as upper bound */
+};
+
+enum {
+	Ndis802_11WEPEnabled,
+	Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+	Ndis802_11WEPDisabled,
+	Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+	Ndis802_11WEPKeyAbsent,
+	Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+	Ndis802_11WEPNotSupported,
+	Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+	Ndis802_11Encryption2Enabled,
+	Ndis802_11Encryption2KeyAbsent,
+	Ndis802_11Encryption3Enabled,
+	Ndis802_11Encryption3KeyAbsent
+};
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+struct NDIS_802_11_AI_REQFI {
+	u16 Capabilities;
+	u16 ListenInterval;
+	unsigned char CurrentAPAddress[6];
+};
+
+struct NDIS_802_11_AI_RESFI {
+	u16 Capabilities;
+	u16 StatusCode;
+	u16 AssociationId;
+};
+
+struct NDIS_802_11_ASSOCIATION_INFORMATION {
+	u32 Length;
+	u16 AvailableRequestFixedIEs;
+	struct NDIS_802_11_AI_REQFI RequestFixedIEs;
+	u32 RequestIELength;
+	u32 OffsetRequestIEs;
+	u16 AvailableResponseFixedIEs;
+	struct NDIS_802_11_AI_RESFI ResponseFixedIEs;
+	u32 ResponseIELength;
+	u32 OffsetResponseIEs;
+};
+
+/* Key mapping keys require a BSSID*/
+struct NDIS_802_11_KEY {
+	u32 Length;			/* Length of this structure */
+	u32 KeyIndex;
+	u32 KeyLength;			/* length of key in bytes */
+	unsigned char BSSID[6];
+	unsigned long long KeyRSC;
+	u8  KeyMaterial[32];		/* variable length */
+};
+
+struct NDIS_802_11_REMOVE_KEY {
+	u32 Length;			/* Length of this structure */
+	u32 KeyIndex;
+	unsigned char BSSID[6];
+};
+
+struct NDIS_802_11_WEP {
+	u32 Length;		  /* Length of this structure */
+	u32 KeyIndex;		  /* 0 is the per-client key,
+				   * 1-N are the global keys
+				   */
+	u32 KeyLength;		  /* length of key in bytes */
+	u8  KeyMaterial[16];      /* variable length depending on above field */
+};
+
+/* mask for authentication/integrity fields */
+#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
+#define NDIS_802_11_AUTH_REQUEST_REAUTH			0x01
+#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE		0x02
+#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR		0x06
+#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR		0x0E
+
+/* MIC check time, 60 seconds. */
+#define MIC_CHECK_TIME	60000000
+
+#ifndef Ndis802_11APMode
+#define Ndis802_11APMode (Ndis802_11InfrastructureMax + 1)
+#endif
+
+struct	wlan_network {
+	struct list_head list;
+	int	network_type;	/*refer to ieee80211.h for WIRELESS_11A/B/G */
+	int	fixed;		/* set to fixed when not to be removed asi
+				 * site-surveying
+				 */
+	unsigned int	last_scanned; /*timestamp for the network */
+	int	aid;		/*will only be valid when a BSS is joined. */
+	int	join_res;
+	struct wlan_bssid_ex network; /*must be the last item */
+};
+
+enum VRTL_CARRIER_SENSE {
+	DISABLE_VCS,
+	ENABLE_VCS,
+	AUTO_VCS
+};
+
+enum VCS_TYPE {
+	NONE_VCS,
+	RTS_CTS,
+	CTS_TO_SELF
+};
+
+#define PWR_CAM 0
+#define PWR_MINPS 1
+#define PWR_MAXPS 2
+#define PWR_UAPSD 3
+#define PWR_VOIP 4
+
+enum UAPSD_MAX_SP {
+	NO_LIMIT,
+	TWO_MSDU,
+	FOUR_MSDU,
+	SIX_MSDU
+};
+
+#define NUM_PRE_AUTH_KEY 16
+#define NUM_PMKID_CACHE NUM_PRE_AUTH_KEY
+
+#endif /* #ifndef WLAN_BSSDEF_H_ */
+
diff --git a/drivers/staging/rtl8712/xmit_linux.c b/drivers/staging/rtl8712/xmit_linux.c
new file mode 100644
index 000000000..ceb6b590b
--- /dev/null
+++ b/drivers/staging/rtl8712/xmit_linux.c
@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ * xmit_linux.c
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ * Linux device driver for RTL8192SU
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+
+#define _XMIT_OSDEP_C_
+
+#include <linux/usb.h>
+#include <linux/ip.h>
+#include <linux/if_ether.h>
+#include <linux/kmemleak.h>
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+#include "wifi.h"
+#include "mlme_osdep.h"
+#include "xmit_osdep.h"
+#include "osdep_intf.h"
+
+static uint remainder_len(struct pkt_file *pfile)
+{
+	return (uint)(pfile->buf_len - ((addr_t)(pfile->cur_addr) -
+	       (addr_t)(pfile->buf_start)));
+}
+
+void _r8712_open_pktfile(_pkt *pktptr, struct pkt_file *pfile)
+{
+	pfile->pkt = pktptr;
+	pfile->cur_addr = pfile->buf_start = pktptr->data;
+	pfile->pkt_len = pfile->buf_len = pktptr->len;
+	pfile->cur_buffer = pfile->buf_start;
+}
+
+uint _r8712_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen)
+{
+	uint len;
+
+	len = remainder_len(pfile);
+	len = (rlen > len) ? len : rlen;
+	if (rmem)
+		skb_copy_bits(pfile->pkt, pfile->buf_len - pfile->pkt_len,
+			      rmem, len);
+	pfile->cur_addr += len;
+	pfile->pkt_len -= len;
+	return len;
+}
+
+sint r8712_endofpktfile(struct pkt_file *pfile)
+{
+	return (pfile->pkt_len == 0);
+}
+
+void r8712_set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	u16 user_priority = 0;
+
+	_r8712_open_pktfile(ppktfile->pkt, ppktfile);
+	_r8712_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	/* get user_priority from IP hdr*/
+	if (pattrib->ether_type == 0x0800) {
+		_r8712_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+		/*user_priority = (ntohs(ip_hdr.tos) >> 5) & 0x3 ;*/
+		user_priority = ip_hdr.tos >> 5;
+	} else {
+		/* "When priority processing of data frames is supported,
+		 * a STA's SME should send EAPOL-Key frames at the highest
+		 * priority."
+		 */
+
+		if (pattrib->ether_type == 0x888e)
+			user_priority = 7;
+	}
+	pattrib->priority = user_priority;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+void r8712_SetFilter(struct work_struct *work)
+{
+	struct _adapter *adapter = container_of(work, struct _adapter,
+						wk_filter_rx_ff0);
+	u8  oldvalue = 0x00, newvalue = 0x00;
+
+	oldvalue = r8712_read8(adapter, 0x117);
+	newvalue = oldvalue & 0xfe;
+	r8712_write8(adapter, 0x117, newvalue);
+
+	wait_for_completion(&adapter->rx_filter_ready);
+	r8712_write8(adapter, 0x117, oldvalue);
+}
+
+int r8712_xmit_resource_alloc(struct _adapter *padapter,
+			      struct xmit_buf *pxmitbuf)
+{
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		pxmitbuf->pxmit_urb[i] = usb_alloc_urb(0, GFP_KERNEL);
+		if (!pxmitbuf->pxmit_urb[i]) {
+			int k;
+
+			for (k = i - 1; k >= 0; k--) {
+				/* handle allocation errors part way through loop */
+				usb_free_urb(pxmitbuf->pxmit_urb[k]);
+			}
+			netdev_err(padapter->pnetdev, "pxmitbuf->pxmit_urb[i] == NULL\n");
+			return -ENOMEM;
+		}
+		kmemleak_not_leak(pxmitbuf->pxmit_urb[i]);
+	}
+	return 0;
+}
+
+void r8712_xmit_resource_free(struct _adapter *padapter,
+			      struct xmit_buf *pxmitbuf)
+{
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		if (pxmitbuf->pxmit_urb[i]) {
+			usb_kill_urb(pxmitbuf->pxmit_urb[i]);
+			usb_free_urb(pxmitbuf->pxmit_urb[i]);
+		}
+	}
+}
+
+void r8712_xmit_complete(struct _adapter *padapter, struct xmit_frame *pxframe)
+{
+	if (pxframe->pkt)
+		dev_kfree_skb_any(pxframe->pkt);
+	pxframe->pkt = NULL;
+}
+
+netdev_tx_t r8712_xmit_entry(_pkt *pkt, struct  net_device *netdev)
+{
+	struct xmit_frame *xmitframe = NULL;
+	struct _adapter *adapter = netdev_priv(netdev);
+	struct xmit_priv *xmitpriv = &(adapter->xmitpriv);
+
+	if (!r8712_if_up(adapter))
+		goto _xmit_entry_drop;
+
+	xmitframe = r8712_alloc_xmitframe(xmitpriv);
+	if (!xmitframe)
+		goto _xmit_entry_drop;
+
+	if (r8712_update_attrib(adapter, pkt, &xmitframe->attrib))
+		goto _xmit_entry_drop;
+
+	adapter->ledpriv.LedControlHandler(adapter, LED_CTL_TX);
+	xmitframe->pkt = pkt;
+	if (r8712_pre_xmit(adapter, xmitframe)) {
+		/*dump xmitframe directly or drop xframe*/
+		dev_kfree_skb_any(pkt);
+		xmitframe->pkt = NULL;
+	}
+	xmitpriv->tx_pkts++;
+	xmitpriv->tx_bytes += xmitframe->attrib.last_txcmdsz;
+	return NETDEV_TX_OK;
+_xmit_entry_drop:
+	if (xmitframe)
+		r8712_free_xmitframe(xmitpriv, xmitframe);
+	xmitpriv->tx_drop++;
+	dev_kfree_skb_any(pkt);
+	return NETDEV_TX_OK;
+}
diff --git a/drivers/staging/rtl8712/xmit_osdep.h b/drivers/staging/rtl8712/xmit_osdep.h
new file mode 100644
index 000000000..1ad42658c
--- /dev/null
+++ b/drivers/staging/rtl8712/xmit_osdep.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Modifications for inclusion into the Linux staging tree are
+ * Copyright(c) 2010 Larry Finger. All rights reserved.
+ *
+ * Contact information:
+ * WLAN FAE <wlanfae@realtek.com>
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ ******************************************************************************/
+#ifndef __XMIT_OSDEP_H_
+#define __XMIT_OSDEP_H_
+
+#include "osdep_service.h"
+#include "drv_types.h"
+
+struct pkt_file {
+	_pkt	*pkt;
+	u32	pkt_len;	 /*the remainder length of the open_file*/
+	_buffer	*cur_buffer;
+	u8	*buf_start;
+	u8	*cur_addr;
+	u32	buf_len;
+};
+
+#define NR_XMITFRAME	256
+
+struct xmit_priv;
+struct pkt_attrib;
+struct sta_xmit_priv;
+struct xmit_frame;
+struct xmit_buf;
+
+netdev_tx_t r8712_xmit_entry(_pkt *pkt, struct  net_device *pnetdev);
+void r8712_SetFilter(struct work_struct *work);
+int r8712_xmit_resource_alloc(struct _adapter *padapter,
+			      struct xmit_buf *pxmitbuf);
+void r8712_xmit_resource_free(struct _adapter *padapter,
+			      struct xmit_buf *pxmitbuf);
+
+void r8712_set_qos(struct pkt_file *ppktfile,
+		   struct pkt_attrib *pattrib);
+void _r8712_open_pktfile(_pkt *pktptr, struct pkt_file *pfile);
+uint _r8712_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen);
+sint r8712_endofpktfile(struct pkt_file *pfile);
+void r8712_xmit_complete(struct _adapter *padapter,
+			 struct xmit_frame *pxframe);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/Kconfig b/drivers/staging/rtl8723bs/Kconfig
new file mode 100644
index 000000000..f23e29b67
--- /dev/null
+++ b/drivers/staging/rtl8723bs/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+config RTL8723BS
+	tristate "Realtek RTL8723BS SDIO Wireless LAN NIC driver"
+	depends on WLAN && MMC && CFG80211
+	depends on m
+	select CFG80211_WEXT
+	select CRYPTO
+	select CRYPTO_LIB_ARC4
+	help
+	This option enables support for RTL8723BS SDIO drivers, such as
+	the wifi found on the 1st gen Intel Compute Stick, the CHIP
+	and many other Intel Atom and ARM based devices.
+	If built as a module, it will be called r8723bs.
diff --git a/drivers/staging/rtl8723bs/Makefile b/drivers/staging/rtl8723bs/Makefile
new file mode 100644
index 000000000..590bde020
--- /dev/null
+++ b/drivers/staging/rtl8723bs/Makefile
@@ -0,0 +1,65 @@
+# SPDX-License-Identifier: GPL-2.0
+r8723bs-y = \
+		core/rtw_ap.o \
+		core/rtw_btcoex.o \
+	 	core/rtw_cmd.o \
+		core/rtw_debug.o \
+		core/rtw_efuse.o \
+		core/rtw_io.o \
+		core/rtw_ioctl_set.o \
+		core/rtw_ieee80211.o \
+		core/rtw_mlme.o \
+		core/rtw_mlme_ext.o \
+		core/rtw_pwrctrl.o \
+		core/rtw_recv.o \
+		core/rtw_rf.o \
+		core/rtw_security.o \
+		core/rtw_sta_mgt.o \
+		core/rtw_wlan_util.o \
+		core/rtw_xmit.o	\
+		hal/hal_intf.o \
+		hal/hal_com.o \
+		hal/hal_com_phycfg.o \
+		hal/hal_btcoex.o \
+		hal/hal_sdio.o \
+		hal/hal_pwr_seq.o \
+		hal/HalPhyRf.o \
+		hal/HalPwrSeqCmd.o \
+		hal/odm.o \
+		hal/odm_CfoTracking.o \
+		hal/odm_DIG.o \
+		hal/odm_DynamicBBPowerSaving.o \
+		hal/odm_DynamicTxPower.o \
+		hal/odm_EdcaTurboCheck.o \
+		hal/odm_HWConfig.o \
+		hal/odm_RegConfig8723B.o \
+		hal/rtl8723b_cmd.o \
+		hal/rtl8723b_dm.o \
+		hal/rtl8723b_hal_init.o \
+		hal/rtl8723b_phycfg.o \
+		hal/rtl8723b_rf6052.o \
+		hal/rtl8723b_rxdesc.o \
+		hal/rtl8723bs_recv.o \
+		hal/rtl8723bs_xmit.o \
+		hal/sdio_halinit.o \
+		hal/sdio_ops.o \
+		hal/HalBtc8723b1Ant.o \
+		hal/HalBtc8723b2Ant.o \
+		hal/HalHWImg8723B_BB.o \
+		hal/HalHWImg8723B_MAC.o \
+		hal/HalHWImg8723B_RF.o \
+		hal/HalPhyRf_8723B.o \
+		os_dep/ioctl_cfg80211.o \
+		os_dep/ioctl_linux.o \
+		os_dep/mlme_linux.o \
+		os_dep/osdep_service.o \
+		os_dep/os_intfs.o \
+		os_dep/recv_linux.o \
+		os_dep/sdio_intf.o \
+		os_dep/sdio_ops_linux.o \
+		os_dep/wifi_regd.o \
+		os_dep/xmit_linux.o
+
+obj-$(CONFIG_RTL8723BS) := r8723bs.o
+
+ccflags-y += -I$(srctree)/$(src)/include -I$(srctree)/$(src)/hal
diff --git a/drivers/staging/rtl8723bs/TODO b/drivers/staging/rtl8723bs/TODO
new file mode 100644
index 000000000..3d8f5a634
--- /dev/null
+++ b/drivers/staging/rtl8723bs/TODO
@@ -0,0 +1,12 @@
+TODO:
+- find and remove any code for other chips that is left over
+- convert any remaining unusual variable types
+- find codes that can use %pM and %Nph formatting
+- checkpatch.pl fixes - most of the remaining ones are lines too long. Many
+  of them will require refactoring
+- merge Realtek's bugfixes and new features into the driver
+- switch to use LIB80211
+- switch to use MAC80211
+
+Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org>,
+Hans de Goede <hdegoede@redhat.com> and Larry Finger <Larry.Finger@lwfinger.net>.
diff --git a/drivers/staging/rtl8723bs/core/rtw_ap.c b/drivers/staging/rtl8723bs/core/rtw_ap.c
new file mode 100644
index 000000000..d30d6e6bc
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ap.c
@@ -0,0 +1,2175 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <asm/unaligned.h>
+
+void init_mlme_ap_info(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	spin_lock_init(&pmlmepriv->bcn_update_lock);
+
+	/* for ACL */
+	INIT_LIST_HEAD(&pacl_list->acl_node_q.queue);
+	spin_lock_init(&pacl_list->acl_node_q.lock);
+
+	/* pmlmeext->bstart_bss = false; */
+
+	start_ap_mode(padapter);
+}
+
+void free_mlme_ap_info(struct adapter *padapter)
+{
+	struct sta_info *psta = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* stop_ap_mode(padapter); */
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = false;
+
+	rtw_sta_flush(padapter);
+
+	pmlmeinfo->state = _HW_STATE_NOLINK_;
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	/* free bc/mc sta_info */
+	psta = rtw_get_bcmc_stainfo(padapter);
+	rtw_free_stainfo(padapter, psta);
+}
+
+static void update_BCNTIM(struct adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
+	unsigned char *pie = pnetwork_mlmeext->ies;
+
+	/* update TIM IE */
+	u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
+	__le16 tim_bitmap_le;
+	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
+
+	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
+
+	p = rtw_get_ie(pie + _FIXED_IE_LENGTH_,
+		       WLAN_EID_TIM,
+		       &tim_ielen,
+		       pnetwork_mlmeext->ie_length - _FIXED_IE_LENGTH_
+	);
+	if (p && tim_ielen > 0) {
+		tim_ielen += 2;
+
+		premainder_ie = p + tim_ielen;
+
+		tim_ie_offset = (signed int)(p - pie);
+
+		remainder_ielen = pnetwork_mlmeext->ie_length - tim_ie_offset - tim_ielen;
+
+		/* append TIM IE from dst_ie offset */
+		dst_ie = p;
+	} else {
+		tim_ielen = 0;
+
+		/* calculate head_len */
+		offset = _FIXED_IE_LENGTH_;
+
+		/* get ssid_ie len */
+		p = rtw_get_ie(pie + _BEACON_IE_OFFSET_,
+			       WLAN_EID_SSID,
+			       &tmp_len,
+			       (pnetwork_mlmeext->ie_length - _BEACON_IE_OFFSET_)
+		);
+		if (p)
+			offset += tmp_len + 2;
+
+		/*  get supported rates len */
+		p = rtw_get_ie(pie + _BEACON_IE_OFFSET_,
+			       WLAN_EID_SUPP_RATES, &tmp_len,
+			       (pnetwork_mlmeext->ie_length - _BEACON_IE_OFFSET_)
+		);
+		if (p)
+			offset += tmp_len + 2;
+
+		/* DS Parameter Set IE, len =3 */
+		offset += 3;
+
+		premainder_ie = pie + offset;
+
+		remainder_ielen = pnetwork_mlmeext->ie_length - offset - tim_ielen;
+
+		/* append TIM IE from offset */
+		dst_ie = pie + offset;
+	}
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie && premainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	*dst_ie++ = WLAN_EID_TIM;
+
+	if ((pstapriv->tim_bitmap & 0xff00) && (pstapriv->tim_bitmap & 0x00fe))
+		tim_ielen = 5;
+	else
+		tim_ielen = 4;
+
+	*dst_ie++ = tim_ielen;
+
+	*dst_ie++ = 0;/* DTIM count */
+	*dst_ie++ = 1;/* DTIM period */
+
+	if (pstapriv->tim_bitmap & BIT(0))/* for bc/mc frames */
+		*dst_ie++ = BIT(0);/* bitmap ctrl */
+	else
+		*dst_ie++ = 0;
+
+	if (tim_ielen == 4) {
+		__le16 pvb;
+
+		if (pstapriv->tim_bitmap & 0xff00)
+			pvb = cpu_to_le16(pstapriv->tim_bitmap >> 8);
+		else
+			pvb = tim_bitmap_le;
+
+		*dst_ie++ = le16_to_cpu(pvb);
+
+	} else if (tim_ielen == 5) {
+		memcpy(dst_ie, &tim_bitmap_le, 2);
+		dst_ie += 2;
+	}
+
+	/* copy remainder IE */
+	if (pbackup_remainder_ie) {
+		memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+		kfree(pbackup_remainder_ie);
+	}
+
+	offset =  (uint)(dst_ie - pie);
+	pnetwork_mlmeext->ie_length = offset + remainder_ielen;
+}
+
+static u8 chk_sta_is_alive(struct sta_info *psta)
+{
+	sta_update_last_rx_pkts(psta);
+
+	return true;
+}
+
+void expire_timeout_chk(struct adapter *padapter)
+{
+	struct list_head *phead, *plist, *tmp;
+	u8 updated = false;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	int i;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+
+	phead = &pstapriv->auth_list;
+	/* check auth_queue */
+	list_for_each_safe(plist, tmp, phead) {
+		psta = list_entry(plist, struct sta_info, auth_list);
+
+		if (psta->expire_to > 0) {
+			psta->expire_to--;
+			if (psta->expire_to == 0) {
+				list_del_init(&psta->auth_list);
+				pstapriv->auth_list_cnt--;
+
+				spin_unlock_bh(&pstapriv->auth_list_lock);
+
+				rtw_free_stainfo(padapter, psta);
+
+				spin_lock_bh(&pstapriv->auth_list_lock);
+			}
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+	psta = NULL;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	/* check asoc_queue */
+	list_for_each_safe(plist, tmp, phead) {
+		psta = list_entry(plist, struct sta_info, asoc_list);
+		if (chk_sta_is_alive(psta) || !psta->expire_to) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->keep_alive_trycnt = 0;
+			psta->under_exist_checking = 0;
+		} else {
+			if (psta->expire_to > 0)
+				psta->expire_to--;
+		}
+
+		if (psta->expire_to == 0) {
+			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+			if (padapter->registrypriv.wifi_spec == 1) {
+				psta->expire_to = pstapriv->expire_to;
+				continue;
+			}
+
+			if (psta->state & WIFI_SLEEP_STATE) {
+				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+					/* to check if alive by another methods */
+					/* if station is at ps mode. */
+					psta->expire_to = pstapriv->expire_to;
+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+					/* to update bcn with tim_bitmap for this station */
+					pstapriv->tim_bitmap |= BIT(psta->aid);
+					update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+
+					if (!pmlmeext->active_keep_alive_check)
+						continue;
+				}
+			}
+			if (pmlmeext->active_keep_alive_check) {
+				int stainfo_offset;
+
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					chk_alive_list[chk_alive_num++] = stainfo_offset;
+
+				continue;
+			}
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			updated = ap_free_sta(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING);
+		} else {
+			/* TODO: Aging mechanism to digest frames in sleep_q to */
+			/* avoid running out of xmitframe */
+			if (psta->sleepq_len > (NR_XMITFRAME / pstapriv->asoc_list_cnt)
+				&& padapter->xmitpriv.free_xmitframe_cnt < ((
+					NR_XMITFRAME / pstapriv->asoc_list_cnt
+				) / 2)
+			)
+				wakeup_sta_to_xmit(padapter, psta);
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	if (chk_alive_num) {
+		u8 backup_oper_channel = 0;
+		struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+		/* switch to correct channel of current network  before issue keep-alive frames */
+		if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+			backup_oper_channel = rtw_get_oper_ch(padapter);
+			SelectChannel(padapter, pmlmeext->cur_channel);
+		}
+
+		/* issue null data to check sta alive*/
+		for (i = 0; i < chk_alive_num; i++) {
+			int ret = _FAIL;
+
+			psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+			if (!(psta->state & _FW_LINKED))
+				continue;
+
+			if (psta->state & WIFI_SLEEP_STATE)
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
+			else
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+
+			psta->keep_alive_trycnt++;
+			if (ret == _SUCCESS) {
+				psta->expire_to = pstapriv->expire_to;
+				psta->keep_alive_trycnt = 0;
+				continue;
+			} else if (psta->keep_alive_trycnt <= 3) {
+				psta->expire_to = 1;
+				continue;
+			}
+
+			psta->keep_alive_trycnt = 0;
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (list_empty(&psta->asoc_list) == false) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false,
+						      WLAN_REASON_DEAUTH_LEAVING);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+		}
+
+		if (backup_oper_channel > 0) /* back to the original operation channel */
+			SelectChannel(padapter, backup_oper_channel);
+	}
+
+	associated_clients_update(padapter, updated);
+}
+
+void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
+{
+	unsigned char sta_band = 0, shortGIrate = false;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_bssid_ex
+		*pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+
+	if (!psta)
+		return;
+
+	if (!(psta->state & _FW_LINKED))
+		return;
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+	tx_ra_bitmap = psta->ra_mask;
+
+	shortGIrate = query_ra_short_GI(psta);
+
+	if (pcur_network->configuration.ds_config > 14) {
+		sta_band |= WIRELESS_INVALID;
+	} else {
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_24N;
+
+		if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11G;
+
+		if (tx_ra_bitmap & 0x0f)
+			sta_band |= WIRELESS_11B;
+	}
+
+	psta->wireless_mode = sta_band;
+	psta->raid = networktype_to_raid_ex(padapter, psta);
+
+	if (psta->aid < NUM_STA) {
+		u8 arg[4] = {0};
+
+		arg[0] = psta->mac_id;
+		arg[1] = psta->raid;
+		arg[2] = shortGIrate;
+		arg[3] = psta->init_rate;
+
+		rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level);
+	}
+}
+
+void update_bmc_sta(struct adapter *padapter)
+{
+	unsigned char network_type;
+	int supportRateNum = 0;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex
+		*pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);
+
+	if (psta) {
+		psta->aid = 0;/* default set to 0 */
+		/* psta->mac_id = psta->aid+4; */
+		psta->mac_id = psta->aid + 1;/* mac_id = 1 for bc/mc stainfo */
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		psta->qos_option = 0;
+		psta->htpriv.ht_option = false;
+
+		psta->ieee8021x_blocked = 0;
+
+		memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+		/* psta->dot118021XPrivacy = _NO_PRIVACY_;//!!! remove it, because it has been set before this. */
+
+		/* prepare for add_RATid */
+		supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->supported_rates);
+		network_type = rtw_check_network_type((u8 *)&pcur_network->supported_rates,
+						      supportRateNum,
+						      pcur_network->configuration.ds_config
+		);
+		if (is_supported_tx_cck(network_type)) {
+			network_type = WIRELESS_11B;
+		} else if (network_type == WIRELESS_INVALID) { /*  error handling */
+
+			if (pcur_network->configuration.ds_config > 14)
+				network_type = WIRELESS_INVALID;
+			else
+				network_type = WIRELESS_11B;
+		}
+		update_sta_basic_rate(psta, network_type);
+		psta->wireless_mode = network_type;
+
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+		tx_ra_bitmap = psta->ra_mask;
+
+		psta->raid = networktype_to_raid_ex(padapter, psta);
+
+		/* ap mode */
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+		/* if (pHalData->fw_ractrl == true) */
+		{
+			u8 arg[4] = {0};
+
+			arg[0] = psta->mac_id;
+			arg[1] = psta->raid;
+			arg[2] = 0;
+			arg[3] = psta->init_rate;
+
+			rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, 0);
+		}
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+
+		spin_lock_bh(&psta->lock);
+		psta->state = _FW_LINKED;
+		spin_unlock_bh(&psta->lock);
+
+	}
+}
+
+/* notes: */
+/* AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */
+/* MAC_ID = AID+1 for sta in ap/adhoc mode */
+/* MAC_ID = 1 for bc/mc for sta/ap/adhoc */
+/* MAC_ID = 0 for bssid for sta/ap/adhoc */
+/* CAM_ID = 0~3 for default key, cmd_id =macid + 3, macid =aid+1; */
+
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+	struct ht_priv *phtpriv_sta = &psta->htpriv;
+	u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0;
+	/* set intf_tag to if1 */
+	/* psta->intf_tag = 0; */
+
+	/* psta->mac_id = psta->aid+4; */
+	/* psta->mac_id = psta->aid+1;//alloc macid when call rtw_alloc_stainfo(), */
+	/* release macid when call rtw_free_stainfo() */
+
+	/* ap mode */
+	rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->ieee8021x_blocked = true;
+	else
+		psta->ieee8021x_blocked = false;
+
+	/* update sta's cap */
+
+	/* ERP */
+	VCS_update(padapter, psta);
+
+	/* HT related cap */
+	if (phtpriv_sta->ht_option) {
+		/* check if sta supports rx ampdu */
+		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
+
+		phtpriv_sta->rx_ampdu_min_spacing = (
+			phtpriv_sta->ht_cap.ampdu_params_info & IEEE80211_HT_CAP_AMPDU_DENSITY
+		) >> 2;
+
+		/*  bwmode */
+		if ((
+			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
+		) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))
+			psta->bw_mode = CHANNEL_WIDTH_40;
+		else
+			psta->bw_mode = CHANNEL_WIDTH_20;
+
+		if (pmlmeext->cur_bwmode < psta->bw_mode)
+			psta->bw_mode = pmlmeext->cur_bwmode;
+
+		phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+
+		/* check if sta support s Short GI 20M */
+		if ((
+			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
+		) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+			phtpriv_sta->sgi_20m = true;
+
+		/* check if sta support s Short GI 40M */
+		if ((
+			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
+		) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) {
+			if (psta->bw_mode == CHANNEL_WIDTH_40) /* according to psta->bw_mode */
+				phtpriv_sta->sgi_40m = true;
+			else
+				phtpriv_sta->sgi_40m = false;
+		}
+
+		psta->qos_option = true;
+
+		/*  B0 Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv_ap->ldpc_cap, LDPC_HT_ENABLE_TX) &&
+		    GET_HT_CAPABILITY_ELE_LDPC_CAP((u8 *)(&phtpriv_sta->ht_cap)))
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+
+		/*  B7 B8 B9 Config STBC setting */
+		if (TEST_FLAG(phtpriv_ap->stbc_cap, STBC_HT_ENABLE_TX) &&
+		    GET_HT_CAPABILITY_ELE_RX_STBC((u8 *)(&phtpriv_sta->ht_cap)))
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+	} else {
+		phtpriv_sta->ampdu_enable = false;
+
+		phtpriv_sta->sgi_20m = false;
+		phtpriv_sta->sgi_40m = false;
+		psta->bw_mode = CHANNEL_WIDTH_20;
+		phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	phtpriv_sta->ldpc_cap = cur_ldpc_cap;
+	phtpriv_sta->stbc_cap = cur_stbc_cap;
+	phtpriv_sta->beamform_cap = cur_beamform_cap;
+
+	/* Rx AMPDU */
+	send_delba(padapter, 0, psta->hwaddr);/*  recipient */
+
+	/* TX AMPDU */
+	send_delba(padapter, 1, psta->hwaddr);/* originator */
+	phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */
+	phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */
+
+	update_ldpc_stbc_cap(psta);
+
+	/* todo: init other variables */
+
+	memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+	/* add ratid */
+	/* add_RATid(padapter, psta);//move to ap_sta_info_defer_update() */
+
+	spin_lock_bh(&psta->lock);
+	psta->state |= _FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+}
+
+static void update_ap_info(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_bssid_ex
+		*pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+
+	psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+	psta->bssratelen = rtw_get_rateset_len(pnetwork->supported_rates);
+	memcpy(psta->bssrateset, pnetwork->supported_rates, psta->bssratelen);
+
+	/* HT related cap */
+	if (phtpriv_ap->ht_option) {
+		/* check if sta supports rx ampdu */
+		/* phtpriv_ap->ampdu_enable = phtpriv_ap->ampdu_enable; */
+
+		/* check if sta support s Short GI 20M */
+		if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+			phtpriv_ap->sgi_20m = true;
+
+		/* check if sta support s Short GI 40M */
+		if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40))
+			phtpriv_ap->sgi_40m = true;
+
+		psta->qos_option = true;
+	} else {
+		phtpriv_ap->ampdu_enable = false;
+
+		phtpriv_ap->sgi_20m = false;
+		phtpriv_ap->sgi_40m = false;
+	}
+
+	psta->bw_mode = pmlmeext->cur_bwmode;
+	phtpriv_ap->ch_offset = pmlmeext->cur_ch_offset;
+
+	phtpriv_ap->agg_enable_bitmap = 0x0;/* reset */
+	phtpriv_ap->candidate_tid_bitmap = 0x0;/* reset */
+
+	memcpy(&psta->htpriv, &pmlmepriv->htpriv, sizeof(struct ht_priv));
+}
+
+static void update_hw_ht_param(struct adapter *padapter)
+{
+	unsigned char max_AMPDU_len;
+	unsigned char min_MPDU_spacing;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* handle A-MPDU parameter field
+	 *
+	 *	AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+	 *	AMPDU_para [4:2]:Min MPDU Start Spacing
+	 */
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+
+	/*  */
+	/*  Config SM Power Save setting */
+	/*  */
+	pmlmeinfo->SM_PS = (le16_to_cpu(
+		pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info
+	) & 0x0C) >> 2;
+
+	/*  */
+	/*  Config current HT Protection mode. */
+	/*  */
+	/* pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; */
+}
+
+void start_bss_network(struct adapter *padapter)
+{
+	u8 *p;
+	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
+	u16 bcn_interval;
+	u32 acparm;
+	int	ie_len;
+	struct registry_priv  *pregpriv = &padapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct wlan_bssid_ex
+		*pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network);
+	struct HT_info_element *pht_info = NULL;
+	u8 cbw40_enable = 0;
+
+	bcn_interval = (u16)pnetwork->configuration.beacon_period;
+	cur_channel = pnetwork->configuration.ds_config;
+	cur_bwmode = CHANNEL_WIDTH_20;
+	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	/* check if there is wps ie, */
+	/* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
+	/* and at first time the security ie (RSN/WPA IE) will not include in beacon. */
+	if (!rtw_get_wps_ie(pnetwork->ies + _FIXED_IE_LENGTH_,
+			    pnetwork->ie_length - _FIXED_IE_LENGTH_, NULL, NULL))
+		pmlmeext->bstart_bss = true;
+
+	/* todo: update wmm, ht cap */
+	/* pmlmeinfo->WMM_enable; */
+	/* pmlmeinfo->HT_enable; */
+	if (pmlmepriv->qospriv.qos_option)
+		pmlmeinfo->WMM_enable = true;
+	if (pmlmepriv->htpriv.ht_option) {
+		pmlmeinfo->WMM_enable = true;
+		pmlmeinfo->HT_enable = true;
+		/* pmlmeinfo->HT_info_enable = true; */
+		/* pmlmeinfo->HT_caps_enable = true; */
+
+		update_hw_ht_param(padapter);
+	}
+
+	if (!pmlmepriv->cur_network.join_res) { /* setting only at  first time */
+
+		/* WEP Key will be set before this function, do not clear CAM. */
+		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) &&
+		    (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
+			flush_all_cam_entry(padapter);	/* clear CAM */
+	}
+
+	/* set MSR to AP_Mode */
+	Set_MSR(padapter, _HW_STATE_AP_);
+
+	/* Set BSSID REG */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->mac_address);
+
+	/* Set EDCA param reg */
+	acparm = 0x002F3217; /*  VO */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+	acparm = 0x005E4317; /*  VI */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+	/* acparm = 0x00105320; // BE */
+	acparm = 0x005ea42b;
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+	acparm = 0x0000A444; /*  BK */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+
+	/* Set Security */
+	val8 = (
+		psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X
+	) ? 0xcc : 0xcf;
+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+	/* Beacon Control related register */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+	if (!pmlmepriv->cur_network.join_res) { /* setting only at  first time */
+		/* u32 initialgain; */
+
+		/* initialgain = 0x1e; */
+
+		/* disable dynamic functions, such as high power, DIG */
+		/* Save_DM_Func_Flag(padapter); */
+		/* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */
+
+		/* turn on all dynamic functions */
+		Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+	}
+
+	/* set channel, bwmode */
+	p = rtw_get_ie((pnetwork->ies + sizeof(struct ndis_802_11_fix_ie)),
+		       WLAN_EID_HT_OPERATION,
+		       &ie_len,
+		       (pnetwork->ie_length - sizeof(struct ndis_802_11_fix_ie))
+	);
+	if (p && ie_len) {
+		pht_info = (struct HT_info_element *)(p + 2);
+
+		if (cur_channel > 14) {
+			if ((pregpriv->bw_mode & 0xf0) > 0)
+				cbw40_enable = 1;
+		} else {
+			if ((pregpriv->bw_mode & 0x0f) > 0)
+				cbw40_enable = 1;
+		}
+
+		if ((cbw40_enable) &&	 (pht_info->infos[0] & BIT(2))) {
+			/* switch to the 40M Hz mode */
+			/* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */
+			cur_bwmode = CHANNEL_WIDTH_40;
+			switch (pht_info->infos[0] & 0x3) {
+			case 1:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+
+			case 3:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+
+			default:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+			}
+		}
+	}
+
+	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+	pmlmeext->cur_channel = cur_channel;
+	pmlmeext->cur_bwmode = cur_bwmode;
+	pmlmeext->cur_ch_offset = cur_ch_offset;
+	pmlmeext->cur_wireless_mode = pmlmepriv->cur_network.network_type;
+
+	/* let pnetwork_mlmeext == pnetwork_mlme. */
+	memcpy(pnetwork_mlmeext, pnetwork, pnetwork->length);
+
+	/* update cur_wireless_mode */
+	update_wireless_mode(padapter);
+
+	/* update RRSR after set channel and bandwidth */
+	UpdateBrateTbl(padapter, pnetwork->supported_rates);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->supported_rates);
+
+	/* update capability after cur_wireless_mode updated */
+	update_capinfo(
+		padapter,
+		rtw_get_capability((struct wlan_bssid_ex *)pnetwork)
+	);
+
+	if (pmlmeext->bstart_bss) {
+		update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+
+		/* issue beacon frame */
+		send_beacon(padapter);
+	}
+
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+
+	/* pmlmeext->bstart_bss = true; */
+}
+
+int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
+{
+	int ret = _SUCCESS;
+	u8 *p;
+	u8 *pHT_caps_ie = NULL;
+	u8 *pHT_info_ie = NULL;
+	struct sta_info *psta = NULL;
+	u16 cap, ht_cap = false;
+	uint ie_len = 0;
+	int group_cipher, pairwise_cipher;
+	u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
+	int supportRateNum = 0;
+	u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_bssid_ex
+		*pbss_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	u8 *ie = pbss_network->ies;
+
+	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		return _FAIL;
+
+	if (len < 0 || len > MAX_IE_SZ)
+		return _FAIL;
+
+	pbss_network->ie_length = len;
+
+	memset(ie, 0, MAX_IE_SZ);
+
+	memcpy(ie, pbuf, pbss_network->ie_length);
+
+	if (pbss_network->infrastructure_mode != Ndis802_11APMode)
+		return _FAIL;
+
+	pbss_network->rssi = 0;
+
+	memcpy(pbss_network->mac_address, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	/* beacon interval */
+	p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8;	8: TimeStamp, 2: Beacon Interval 2:Capability */
+	/* pbss_network->configuration.beacon_period = le16_to_cpu(*(unsigned short*)p); */
+	pbss_network->configuration.beacon_period = get_unaligned_le16(p);
+
+	/* capability */
+	/* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */
+	/* cap = le16_to_cpu(cap); */
+	cap = get_unaligned_le16(ie);
+
+	/* SSID */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_SSID,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0) {
+		memset(&pbss_network->ssid, 0, sizeof(struct ndis_802_11_ssid));
+		memcpy(pbss_network->ssid.ssid, (p + 2), ie_len);
+		pbss_network->ssid.ssid_length = ie_len;
+	}
+
+	/* channel */
+	channel = 0;
+	pbss_network->configuration.length = 0;
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_DS_PARAMS, &ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		channel = *(p + 2);
+
+	pbss_network->configuration.ds_config = channel;
+
+	memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
+	/*  get supported rates */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_SUPP_RATES,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p) {
+		memcpy(supportRate, p + 2, ie_len);
+		supportRateNum = ie_len;
+	}
+
+	/* get ext_supported rates */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_EXT_SUPP_RATES,
+		&ie_len,
+		pbss_network->ie_length - _BEACON_IE_OFFSET_
+	);
+	if (p) {
+		memcpy(supportRate + supportRateNum, p + 2, ie_len);
+		supportRateNum += ie_len;
+	}
+
+	network_type = rtw_check_network_type(supportRate, supportRateNum, channel);
+
+	rtw_set_supported_rate(pbss_network->supported_rates, network_type);
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_ERP_INFO,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		ERP_IE_handler(padapter, (struct ndis_80211_var_ie *)p);
+
+	/* update privacy/security */
+	if (cap & BIT(4))
+		pbss_network->privacy = 1;
+	else
+		pbss_network->privacy = 0;
+
+	psecuritypriv->wpa_psk = 0;
+
+	/* wpa2 */
+	group_cipher = 0; pairwise_cipher = 0;
+	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_RSN,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0) {
+		if (rtw_parse_wpa2_ie(
+			p,
+			ie_len + 2,
+			&group_cipher,
+			&pairwise_cipher,
+			NULL
+		) == _SUCCESS) {
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+			psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
+			psecuritypriv->wpa_psk |= BIT(1);
+
+			psecuritypriv->wpa2_group_cipher = group_cipher;
+			psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher;
+		}
+	}
+
+	/* wpa */
+	ie_len = 0;
+	group_cipher = 0; pairwise_cipher = 0;
+	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
+	for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+		p = rtw_get_ie(
+			p,
+			WLAN_EID_VENDOR_SPECIFIC,
+			&ie_len,
+			(pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2))
+		);
+		if ((p) && (!memcmp(p + 2, OUI1, 4))) {
+			if (rtw_parse_wpa_ie(
+				p,
+				ie_len + 2,
+				&group_cipher,
+				&pairwise_cipher,
+				NULL
+			) == _SUCCESS) {
+				psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+				psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
+
+				psecuritypriv->wpa_psk |= BIT(0);
+
+				psecuritypriv->wpa_group_cipher = group_cipher;
+				psecuritypriv->wpa_pairwise_cipher = pairwise_cipher;
+			}
+
+			break;
+		}
+
+		if (!p || ie_len == 0)
+			break;
+	}
+
+	/* wmm */
+	ie_len = 0;
+	pmlmepriv->qospriv.qos_option = 0;
+	if (pregistrypriv->wmm_enable) {
+		for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+			p = rtw_get_ie(
+				p,
+				WLAN_EID_VENDOR_SPECIFIC,
+				&ie_len,
+				(pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2))
+			);
+			if ((p) && !memcmp(p + 2, WMM_PARA_IE, 6)) {
+				pmlmepriv->qospriv.qos_option = 1;
+
+				*(p + 8) |= BIT(7);/* QoS Info, support U-APSD */
+
+				/* disable all ACM bits since the WMM admission */
+				/* control is not supported */
+				*(p + 10) &= ~BIT(4); /* BE */
+				*(p + 14) &= ~BIT(4); /* BK */
+				*(p + 18) &= ~BIT(4); /* VI */
+				*(p + 22) &= ~BIT(4); /* VO */
+
+				break;
+			}
+
+			if (!p || ie_len == 0)
+				break;
+		}
+	}
+
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_HT_CAPABILITY,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0) {
+		u8 max_rx_ampdu_factor = 0;
+		struct ieee80211_ht_cap *pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+
+		pHT_caps_ie = p;
+
+		ht_cap = true;
+		network_type |= WIRELESS_11_24N;
+
+		rtw_ht_use_default_setting(padapter);
+
+		if (pmlmepriv->htpriv.sgi_20m == false)
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_20));
+
+		if (pmlmepriv->htpriv.sgi_40m == false)
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_40));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.ldpc_cap, LDPC_HT_ENABLE_RX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_LDPC_CODING));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_TX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_TX_STBC));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_RX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_RX_STBC_3R));
+
+		pht_cap->ampdu_params_info &= ~(
+			IEEE80211_HT_CAP_AMPDU_FACTOR | IEEE80211_HT_CAP_AMPDU_DENSITY
+		);
+
+		if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
+		    (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP)) {
+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY &
+						       (0x07 << 2));
+		} else {
+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY &
+						       0x00);
+		}
+
+		rtw_hal_get_def_var(
+			padapter,
+			HW_VAR_MAX_RX_AMPDU_FACTOR,
+			&max_rx_ampdu_factor
+		);
+		pht_cap->ampdu_params_info |= (
+			IEEE80211_HT_CAP_AMPDU_FACTOR & max_rx_ampdu_factor
+		); /* set  Max Rx AMPDU size  to 64K */
+
+		pht_cap->mcs.rx_mask[0] = 0xff;
+		pht_cap->mcs.rx_mask[1] = 0x0;
+
+		memcpy(&pmlmepriv->htpriv.ht_cap, p + 2, ie_len);
+	}
+
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_HT_OPERATION,
+		&ie_len,
+		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		pHT_info_ie = p;
+
+	switch (network_type) {
+	case WIRELESS_11B:
+		pbss_network->network_type_in_use = Ndis802_11DS;
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pbss_network->network_type_in_use = Ndis802_11OFDM24;
+		break;
+	default:
+		pbss_network->network_type_in_use = Ndis802_11OFDM24;
+		break;
+	}
+
+	pmlmepriv->cur_network.network_type = network_type;
+
+	pmlmepriv->htpriv.ht_option = false;
+
+	if ((psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) ||
+	    (psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
+		/* todo: */
+		/* ht_cap = false; */
+	}
+
+	/* ht_cap */
+	if (pregistrypriv->ht_enable && ht_cap) {
+		pmlmepriv->htpriv.ht_option = true;
+		pmlmepriv->qospriv.qos_option = 1;
+
+		if (pregistrypriv->ampdu_enable == 1)
+			pmlmepriv->htpriv.ampdu_enable = true;
+
+		HT_caps_handler(padapter, (struct ndis_80211_var_ie *)pHT_caps_ie);
+
+		HT_info_handler(padapter, (struct ndis_80211_var_ie *)pHT_info_ie);
+	}
+
+	pbss_network->length = get_wlan_bssid_ex_sz(
+		(struct wlan_bssid_ex  *)pbss_network
+	);
+
+	/* issue beacon to start bss network */
+	/* start_bss_network(padapter, (u8 *)pbss_network); */
+	rtw_startbss_cmd(padapter, RTW_CMDF_WAIT_ACK);
+
+	/* alloc sta_info for ap itself */
+	psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->mac_address);
+	if (!psta) {
+		psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->mac_address);
+		if (!psta)
+			return _FAIL;
+	}
+
+	/*  update AP's sta info */
+	update_ap_info(padapter, psta);
+
+	psta->state |= WIFI_AP_STATE;		/* Aries, add, fix bug of flush_cam_entry at STOP AP mode , 0724 */
+	rtw_indicate_connect(padapter);
+
+	pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */
+
+	/* update bc/mc sta_info */
+	/* update_bmc_sta(padapter); */
+
+	return ret;
+}
+
+void rtw_set_macaddr_acl(struct adapter *padapter, int mode)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	pacl_list->mode = mode;
+}
+
+int rtw_acl_add_sta(struct adapter *padapter, u8 *addr)
+{
+	struct list_head	*plist, *phead;
+	u8 added = false;
+	int i, ret = 0;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	if ((NUM_ACL - 1) < pacl_list->num)
+		return (-1);
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	list_for_each(plist, phead) {
+		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
+
+		if (!memcmp(paclnode->addr, addr, ETH_ALEN)) {
+			if (paclnode->valid == true) {
+				added = true;
+				break;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	if (added)
+		return ret;
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	for (i = 0; i < NUM_ACL; i++) {
+		paclnode = &pacl_list->aclnode[i];
+
+		if (!paclnode->valid) {
+			INIT_LIST_HEAD(&paclnode->list);
+
+			memcpy(paclnode->addr, addr, ETH_ALEN);
+
+			paclnode->valid = true;
+
+			list_add_tail(&paclnode->list, get_list_head(pacl_node_q));
+
+			pacl_list->num++;
+
+			break;
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	return ret;
+}
+
+void rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
+{
+	struct list_head *plist, *phead, *tmp;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+	u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };	/* Baddr is used for clearing acl_list */
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	list_for_each_safe(plist, tmp, phead) {
+		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
+
+		if (
+			!memcmp(paclnode->addr, addr, ETH_ALEN) ||
+			!memcmp(baddr, addr, ETH_ALEN)
+		) {
+			if (paclnode->valid) {
+				paclnode->valid = false;
+
+				list_del_init(&paclnode->list);
+
+				pacl_list->num--;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+}
+
+u8 rtw_ap_set_pairwise_key(struct adapter *padapter, struct sta_info *psta)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm	*psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetstakey_para = rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if (!psetstakey_para) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+
+	psetstakey_para->algorithm = (u8)psta->dot118021XPrivacy;
+
+	memcpy(psetstakey_para->addr, psta->hwaddr, ETH_ALEN);
+
+	memcpy(psetstakey_para->key, &psta->dot118021x_UncstKey, 16);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+static int rtw_ap_set_key(
+	struct adapter *padapter,
+	u8 *key,
+	u8 alg,
+	int keyid,
+	u8 set_tx
+)
+{
+	u8 keylen;
+	struct cmd_obj *pcmd;
+	struct setkey_parm *psetkeyparm;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	int res = _SUCCESS;
+
+	pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetkeyparm = rtw_zmalloc(sizeof(struct setkey_parm));
+	if (!psetkeyparm) {
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetkeyparm->keyid = (u8)keyid;
+	if (is_wep_enc(alg))
+		padapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	psetkeyparm->algorithm = alg;
+
+	psetkeyparm->set_tx = set_tx;
+
+	switch (alg) {
+	case _WEP40_:
+		keylen = 5;
+		break;
+	case _WEP104_:
+		keylen = 13;
+		break;
+	case _TKIP_:
+	case _TKIP_WTMIC_:
+	case _AES_:
+	default:
+		keylen = 16;
+	}
+
+	memcpy(&(psetkeyparm->key[0]), key, keylen);
+
+	pcmd->cmdcode = _SetKey_CMD_;
+	pcmd->parmbuf = (u8 *)psetkeyparm;
+	pcmd->cmdsz =  (sizeof(struct setkey_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	INIT_LIST_HEAD(&pcmd->list);
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+	return res;
+}
+
+int rtw_ap_set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid)
+{
+	return rtw_ap_set_key(padapter, key, alg, keyid, 1);
+}
+
+int rtw_ap_set_wep_key(
+	struct adapter *padapter,
+	u8 *key,
+	u8 keylen,
+	int keyid,
+	u8 set_tx
+)
+{
+	u8 alg;
+
+	switch (keylen) {
+	case 5:
+		alg = _WEP40_;
+		break;
+	case 13:
+		alg = _WEP104_;
+		break;
+	default:
+		alg = _NO_PRIVACY_;
+	}
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, set_tx);
+}
+
+static void update_bcn_fixed_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_erpinfo_ie(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	unsigned char *p, *ie = pnetwork->ies;
+	u32 len = 0;
+
+	if (!pmlmeinfo->ERP_enable)
+		return;
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		WLAN_EID_ERP_INFO,
+		&len,
+		(pnetwork->ie_length - _BEACON_IE_OFFSET_)
+	);
+	if (p && len > 0) {
+		struct ndis_80211_var_ie *pIE = (struct ndis_80211_var_ie *)p;
+
+		if (pmlmepriv->num_sta_non_erp == 1)
+			pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION;
+		else
+			pIE->data[0] &= ~(
+				RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION
+			);
+
+		if (pmlmepriv->num_sta_no_short_preamble > 0)
+			pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;
+		else
+			pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE);
+
+		ERP_IE_handler(padapter, pIE);
+	}
+}
+
+static void update_bcn_htcap_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_htinfo_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_rsn_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_wpa_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_wmm_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_wps_ie(struct adapter *padapter)
+{
+	u8 *pwps_ie = NULL;
+	u8 *pwps_ie_src;
+	u8 *premainder_ie;
+	u8 *pbackup_remainder_ie = NULL;
+
+	uint wps_ielen = 0, wps_offset, remainder_ielen;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	unsigned char *ie = pnetwork->ies;
+	u32 ielen = pnetwork->ie_length;
+
+	pwps_ie = rtw_get_wps_ie(
+		ie + _FIXED_IE_LENGTH_,
+		ielen - _FIXED_IE_LENGTH_,
+		NULL,
+		&wps_ielen
+	);
+
+	if (!pwps_ie || wps_ielen == 0)
+		return;
+
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (!pwps_ie_src)
+		return;
+
+	wps_offset = (uint)(pwps_ie - ie);
+
+	premainder_ie = pwps_ie + wps_ielen;
+
+	remainder_ielen = ielen - wps_offset - wps_ielen;
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
+	if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
+		memcpy(pwps_ie, pwps_ie_src, wps_ielen + 2);
+		pwps_ie += (wps_ielen+2);
+
+		if (pbackup_remainder_ie)
+			memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);
+
+		/* update ie_length */
+		pnetwork->ie_length = wps_offset + (wps_ielen + 2) + remainder_ielen;
+	}
+
+	kfree(pbackup_remainder_ie);
+}
+
+static void update_bcn_p2p_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
+{
+	if (!memcmp(RTW_WPA_OUI, oui, 4))
+		update_bcn_wpa_ie(padapter);
+
+	else if (!memcmp(WMM_OUI, oui, 4))
+		update_bcn_wmm_ie(padapter);
+
+	else if (!memcmp(WPS_OUI, oui, 4))
+		update_bcn_wps_ie(padapter);
+
+	else if (!memcmp(P2P_OUI, oui, 4))
+		update_bcn_p2p_ie(padapter);
+}
+
+void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
+{
+	struct mlme_priv *pmlmepriv;
+	struct mlme_ext_priv *pmlmeext;
+	/* struct mlme_ext_info *pmlmeinfo; */
+
+	if (!padapter)
+		return;
+
+	pmlmepriv = &(padapter->mlmepriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	/* pmlmeinfo = &(pmlmeext->mlmext_info); */
+
+	if (!pmlmeext->bstart_bss)
+		return;
+
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	switch (ie_id) {
+	case 0xFF:
+
+		update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+
+		break;
+
+	case WLAN_EID_TIM:
+
+		update_BCNTIM(padapter);
+
+		break;
+
+	case WLAN_EID_ERP_INFO:
+
+		update_bcn_erpinfo_ie(padapter);
+
+		break;
+
+	case WLAN_EID_HT_CAPABILITY:
+
+		update_bcn_htcap_ie(padapter);
+
+		break;
+
+	case WLAN_EID_RSN:
+
+		update_bcn_rsn_ie(padapter);
+
+		break;
+
+	case WLAN_EID_HT_OPERATION:
+
+		update_bcn_htinfo_ie(padapter);
+
+		break;
+
+	case WLAN_EID_VENDOR_SPECIFIC:
+
+		update_bcn_vendor_spec_ie(padapter, oui);
+
+		break;
+
+	default:
+		break;
+	}
+
+	pmlmepriv->update_bcn = true;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if (tx) {
+		/* send_beacon(padapter);//send_beacon must execute on TSR level */
+		set_tx_beacon_cmd(padapter);
+	}
+}
+
+/*
+ * op_mode
+ * Set to 0 (HT pure) under the following conditions
+ *	  - all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+ *	  - all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+ * Set to 1 (HT non-member protection) if there may be non-HT STAs
+ *	  in both the primary and the secondary channel
+ * Set to 2 if only HT STAs are associated in BSS,
+ *	  however and at least one 20 MHz HT STA is associated
+ * Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+ *	  (currently non-GF HT station is considered as non-HT STA also)
+ */
+static int rtw_ht_operation_update(struct adapter *padapter)
+{
+	u16 cur_op_mode, new_op_mode;
+	int op_mode_changes = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+
+	if (pmlmepriv->htpriv.ht_option)
+		return 0;
+
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT)
+	    && pmlmepriv->num_sta_ht_no_gf) {
+		pmlmepriv->ht_op_mode |=
+			IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT) &&
+		   pmlmepriv->num_sta_ht_no_gf == 0) {
+		pmlmepriv->ht_op_mode &=
+			~IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
+		op_mode_changes++;
+	}
+
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
+	    (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode |= IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
+		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode &=
+			~IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
+		op_mode_changes++;
+	}
+
+	/* Note: currently we switch to the MIXED op mode if HT non-greenfield
+	 * station is associated. Probably it's a theoretical case, since
+	 * it looks like all known HT STAs support greenfield.
+	 */
+	new_op_mode = 0;
+	if (pmlmepriv->num_sta_no_ht ||
+	    (pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED;
+	else if (
+		(le16_to_cpu(phtpriv_ap->ht_cap.cap_info) & IEEE80211_HT_CAP_SUP_WIDTH)
+		&& pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_20MHZ;
+	else if (pmlmepriv->olbc_ht)
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER;
+	else
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONE;
+
+	cur_op_mode = pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_PROTECTION;
+	if (cur_op_mode != new_op_mode) {
+		pmlmepriv->ht_op_mode &= ~IEEE80211_HT_OP_MODE_PROTECTION;
+		pmlmepriv->ht_op_mode |= new_op_mode;
+		op_mode_changes++;
+	}
+
+	return op_mode_changes;
+}
+
+void associated_clients_update(struct adapter *padapter, u8 updated)
+{
+	/* update associated stations cap. */
+	if (updated) {
+		struct list_head	*phead, *plist;
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+
+		phead = &pstapriv->asoc_list;
+		/* check asoc_queue */
+		list_for_each(plist, phead) {
+			psta = list_entry(plist, struct sta_info, asoc_list);
+
+			VCS_update(padapter, psta);
+		}
+
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+	}
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) {
+		if (!psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 1;
+
+			pmlmepriv->num_sta_no_short_preamble++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	} else {
+		if (psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 0;
+
+			pmlmepriv->num_sta_no_short_preamble--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	}
+
+	if (psta->flags & WLAN_STA_NONERP) {
+		if (!psta->nonerp_set) {
+			psta->nonerp_set = 1;
+
+			pmlmepriv->num_sta_non_erp++;
+
+			if (pmlmepriv->num_sta_non_erp == 1) {
+				beacon_updated = true;
+				update_beacon(padapter, WLAN_EID_ERP_INFO, NULL, true);
+			}
+		}
+	} else {
+		if (psta->nonerp_set) {
+			psta->nonerp_set = 0;
+
+			pmlmepriv->num_sta_non_erp--;
+
+			if (pmlmepriv->num_sta_non_erp == 0) {
+				beacon_updated = true;
+				update_beacon(padapter, WLAN_EID_ERP_INFO, NULL, true);
+			}
+		}
+	}
+
+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)) {
+		if (!psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 1;
+
+			pmlmepriv->num_sta_no_short_slot_time++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	} else {
+		if (psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 0;
+
+			pmlmepriv->num_sta_no_short_slot_time--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	}
+
+	if (psta->flags & WLAN_STA_HT) {
+		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
+
+		if (psta->no_ht_set) {
+			psta->no_ht_set = 0;
+			pmlmepriv->num_sta_no_ht--;
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {
+			if (!psta->no_ht_gf_set) {
+				psta->no_ht_gf_set = 1;
+				pmlmepriv->num_sta_ht_no_gf++;
+			}
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH) == 0) {
+			if (!psta->ht_20mhz_set) {
+				psta->ht_20mhz_set = 1;
+				pmlmepriv->num_sta_ht_20mhz++;
+			}
+		}
+
+	} else {
+		if (!psta->no_ht_set) {
+			psta->no_ht_set = 1;
+			pmlmepriv->num_sta_no_ht++;
+		}
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon(padapter, WLAN_EID_HT_OPERATION, NULL, true);
+	}
+
+	/* update associated stations cap. */
+	associated_clients_update(padapter,  beacon_updated);
+}
+
+u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	if (!psta)
+		return beacon_updated;
+
+	if (psta->no_short_preamble_set) {
+		psta->no_short_preamble_set = 0;
+		pmlmepriv->num_sta_no_short_preamble--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
+		    && pmlmepriv->num_sta_no_short_preamble == 0){
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->nonerp_set) {
+		psta->nonerp_set = 0;
+		pmlmepriv->num_sta_non_erp--;
+		if (pmlmepriv->num_sta_non_erp == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, WLAN_EID_ERP_INFO, NULL, true);
+		}
+	}
+
+	if (psta->no_short_slot_time_set) {
+		psta->no_short_slot_time_set = 0;
+		pmlmepriv->num_sta_no_short_slot_time--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
+		    && pmlmepriv->num_sta_no_short_slot_time == 0){
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->no_ht_gf_set) {
+		psta->no_ht_gf_set = 0;
+		pmlmepriv->num_sta_ht_no_gf--;
+	}
+
+	if (psta->no_ht_set) {
+		psta->no_ht_set = 0;
+		pmlmepriv->num_sta_no_ht--;
+	}
+
+	if (psta->ht_20mhz_set) {
+		psta->ht_20mhz_set = 0;
+		pmlmepriv->num_sta_ht_20mhz--;
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon(padapter, WLAN_EID_HT_OPERATION, NULL, true);
+	}
+
+	return beacon_updated;
+}
+
+u8 ap_free_sta(
+	struct adapter *padapter,
+	struct sta_info *psta,
+	bool active,
+	u16 reason
+)
+{
+	u8 beacon_updated = false;
+
+	if (!psta)
+		return beacon_updated;
+
+	if (active) {
+		/* tear down Rx AMPDU */
+		send_delba(padapter, 0, psta->hwaddr);/*  recipient */
+
+		/* tear down TX AMPDU */
+		send_delba(padapter, 1, psta->hwaddr);/*  // originator */
+
+		issue_deauth(padapter, psta->hwaddr, reason);
+	}
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	/* report_del_sta_event(padapter, psta->hwaddr, reason); */
+
+	/* clear cam entry / key */
+	rtw_clearstakey_cmd(padapter, psta, true);
+
+	spin_lock_bh(&psta->lock);
+	psta->state &= ~_FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+	rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);
+
+	report_del_sta_event(padapter, psta->hwaddr, reason);
+
+	beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);
+
+	rtw_free_stainfo(padapter, psta);
+
+	return beacon_updated;
+}
+
+void rtw_sta_flush(struct adapter *padapter)
+{
+	struct list_head *phead, *plist, *tmp;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	/* free sta asoc_queue */
+	list_for_each_safe(plist, tmp, phead) {
+		psta = list_entry(plist, struct sta_info, asoc_list);
+
+		list_del_init(&psta->asoc_list);
+		pstapriv->asoc_list_cnt--;
+
+		/* spin_unlock_bh(&pstapriv->asoc_list_lock); */
+		ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+		/* spin_lock_bh(&pstapriv->asoc_list_lock); */
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
+
+	associated_clients_update(padapter, true);
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void sta_info_update(struct adapter *padapter, struct sta_info *psta)
+{
+	int flags = psta->flags;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	/* update wmm cap. */
+	if (WLAN_STA_WME & flags)
+		psta->qos_option = 1;
+	else
+		psta->qos_option = 0;
+
+	if (pmlmepriv->qospriv.qos_option == 0)
+		psta->qos_option = 0;
+
+	/* update 802.11n ht cap. */
+	if (WLAN_STA_HT & flags) {
+		psta->htpriv.ht_option = true;
+		psta->qos_option = 1;
+	} else {
+		psta->htpriv.ht_option = false;
+	}
+
+	if (!pmlmepriv->htpriv.ht_option)
+		psta->htpriv.ht_option = false;
+
+	update_sta_info_apmode(padapter, psta);
+}
+
+/* called >= TSR LEVEL for USB or SDIO Interface*/
+void ap_sta_info_defer_update(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (psta->state & _FW_LINKED) {
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		/* add ratid */
+		add_RATid(padapter, psta, 0);/* DM_RATR_STA_INIT */
+	}
+}
+/* restore hw setting from sw data structures */
+void rtw_ap_restore_network(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct list_head	*phead, *plist;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	int i;
+
+	rtw_setopmode_cmd(padapter, Ndis802_11APMode, false);
+
+	set_channel_bwmode(
+		padapter,
+		pmlmeext->cur_channel,
+		pmlmeext->cur_ch_offset,
+		pmlmeext->cur_bwmode
+	);
+
+	start_bss_network(padapter);
+
+	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+		/* restore group key, WEP keys is restored in ips_leave() */
+		rtw_set_key(
+			padapter,
+			psecuritypriv,
+			psecuritypriv->dot118021XGrpKeyid,
+			0,
+			false
+		);
+	}
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	list_for_each(plist, phead) {
+		int stainfo_offset;
+
+		psta = list_entry(plist, struct sta_info, asoc_list);
+
+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+		if (stainfo_offset_valid(stainfo_offset))
+			chk_alive_list[chk_alive_num++] = stainfo_offset;
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	for (i = 0; i < chk_alive_num; i++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+
+		if (!psta)
+			continue;
+
+		if (psta->state & _FW_LINKED) {
+			rtw_sta_media_status_rpt(padapter, psta, 1);
+			Update_RA_Entry(padapter, psta);
+			/* pairwise key */
+			/* per sta pairwise key and settings */
+			if ((psecuritypriv->dot11PrivacyAlgrthm == _TKIP_) ||
+			    (psecuritypriv->dot11PrivacyAlgrthm == _AES_)) {
+				rtw_setstakey_cmd(padapter, psta, true, false);
+			}
+		}
+	}
+}
+
+void start_ap_mode(struct adapter *padapter)
+{
+	int i;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	pmlmepriv->update_bcn = false;
+
+	/* init_mlme_ap_info(padapter); */
+	pmlmeext->bstart_bss = false;
+
+	pmlmepriv->num_sta_non_erp = 0;
+
+	pmlmepriv->num_sta_no_short_slot_time = 0;
+
+	pmlmepriv->num_sta_no_short_preamble = 0;
+
+	pmlmepriv->num_sta_ht_no_gf = 0;
+	pmlmepriv->num_sta_no_ht = 0;
+	pmlmepriv->num_sta_ht_20mhz = 0;
+
+	pmlmepriv->olbc = false;
+
+	pmlmepriv->olbc_ht = false;
+
+	pmlmepriv->ht_op_mode = 0;
+
+	for (i = 0; i < NUM_STA; i++)
+		pstapriv->sta_aid[i] = NULL;
+
+	pmlmepriv->wps_beacon_ie = NULL;
+	pmlmepriv->wps_probe_resp_ie = NULL;
+	pmlmepriv->wps_assoc_resp_ie = NULL;
+
+	pmlmepriv->p2p_beacon_ie = NULL;
+	pmlmepriv->p2p_probe_resp_ie = NULL;
+
+	/* for ACL */
+	INIT_LIST_HEAD(&(pacl_list->acl_node_q.queue));
+	pacl_list->num = 0;
+	pacl_list->mode = 0;
+	for (i = 0; i < NUM_ACL; i++) {
+		INIT_LIST_HEAD(&pacl_list->aclnode[i].list);
+		pacl_list->aclnode[i].valid = false;
+	}
+}
+
+void stop_ap_mode(struct adapter *padapter)
+{
+	struct list_head *phead, *plist, *tmp;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = false;
+
+	/* reset and init security priv , this can refine with rtw_reset_securitypriv */
+	memset(
+		(unsigned char *)&padapter->securitypriv,
+		0,
+		sizeof(struct security_priv)
+	);
+	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+
+	/* for ACL */
+	spin_lock_bh(&(pacl_node_q->lock));
+	phead = get_list_head(pacl_node_q);
+	list_for_each_safe(plist, tmp, phead) {
+		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
+
+		if (paclnode->valid) {
+			paclnode->valid = false;
+
+			list_del_init(&paclnode->list);
+
+			pacl_list->num--;
+		}
+	}
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	rtw_sta_flush(padapter);
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	psta = rtw_get_bcmc_stainfo(padapter);
+	rtw_free_stainfo(padapter, psta);
+
+	rtw_init_bcmc_stainfo(padapter);
+
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+	rtw_btcoex_MediaStatusNotify(padapter, 0); /* disconnect */
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_btcoex.c b/drivers/staging/rtl8723bs/core/rtw_btcoex.c
new file mode 100644
index 000000000..62cbf84b0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_btcoex.c
@@ -0,0 +1,78 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_btcoex.h>
+#include <hal_btcoex.h>
+
+void rtw_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus)
+{
+	if ((mediaStatus == RT_MEDIA_CONNECT)
+		&& (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)) {
+		rtw_hal_set_hwreg(padapter, HW_VAR_DL_RSVD_PAGE, NULL);
+	}
+
+	hal_btcoex_MediaStatusNotify(padapter, mediaStatus);
+}
+
+void rtw_btcoex_HaltNotify(struct adapter *padapter)
+{
+	if (!padapter->bup)
+		return;
+
+	if (padapter->bSurpriseRemoved)
+		return;
+
+	hal_btcoex_HaltNotify(padapter);
+}
+
+/*  ================================================== */
+/*  Below Functions are called by BT-Coex */
+/*  ================================================== */
+void rtw_btcoex_RejectApAggregatedPacket(struct adapter *padapter, u8 enable)
+{
+	struct mlme_ext_info *pmlmeinfo;
+	struct sta_info *psta;
+
+	pmlmeinfo = &padapter->mlmeextpriv.mlmext_info;
+	psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+
+	if (enable) {
+		pmlmeinfo->accept_addba_req = false;
+		if (psta)
+			send_delba(padapter, 0, psta->hwaddr);
+	} else {
+		pmlmeinfo->accept_addba_req = true;
+	}
+}
+
+void rtw_btcoex_LPS_Enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	u8 lpsVal;
+
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	pwrpriv->bpower_saving = true;
+	lpsVal = hal_btcoex_LpsVal(padapter);
+	rtw_set_ps_mode(padapter, PS_MODE_MIN, 0, lpsVal, "BTCOEX");
+}
+
+void rtw_btcoex_LPS_Leave(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+		rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "BTCOEX");
+		LPS_RF_ON_check(padapter, 100);
+		pwrpriv->bpower_saving = false;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_cmd.c b/drivers/staging/rtl8723bs/core/rtw_cmd.c
new file mode 100644
index 000000000..d3f10a3cf
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_cmd.c
@@ -0,0 +1,1941 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <linux/jiffies.h>
+
+static struct _cmd_callback rtw_cmd_callback[] = {
+	{GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/
+	{GEN_CMD_CODE(_Write_MACREG), NULL},
+	{GEN_CMD_CODE(_Read_BBREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_BBREG), NULL},
+	{GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/
+	{GEN_CMD_CODE(_Read_EEPROM), NULL},
+	{GEN_CMD_CODE(_Write_EEPROM), NULL},
+	{GEN_CMD_CODE(_Read_EFUSE), NULL},
+	{GEN_CMD_CODE(_Write_EFUSE), NULL},
+
+	{GEN_CMD_CODE(_Read_CAM),	NULL},	/*10*/
+	{GEN_CMD_CODE(_Write_CAM),	 NULL},
+	{GEN_CMD_CODE(_setBCNITV), NULL},
+	{GEN_CMD_CODE(_setMBIDCFG), NULL},
+	{GEN_CMD_CODE(_JoinBss), &rtw_joinbss_cmd_callback},  /*14*/
+	{GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd_callback}, /*15*/
+	{GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd_callback},
+	{GEN_CMD_CODE(_SetOpMode), NULL},
+	{GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback}, /*18*/
+	{GEN_CMD_CODE(_SetAuth), NULL},
+
+	{GEN_CMD_CODE(_SetKey), NULL},	/*20*/
+	{GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback},
+	{GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback},
+	{GEN_CMD_CODE(_DelAssocSta), NULL},
+	{GEN_CMD_CODE(_SetStaPwrState), NULL},
+	{GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/
+	{GEN_CMD_CODE(_GetBasicRate), NULL},
+	{GEN_CMD_CODE(_SetDataRate), NULL},
+	{GEN_CMD_CODE(_GetDataRate), NULL},
+	{GEN_CMD_CODE(_SetPhyInfo), NULL},
+
+	{GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/
+	{GEN_CMD_CODE(_SetPhy), NULL},
+	{GEN_CMD_CODE(_GetPhy), NULL},
+	{GEN_CMD_CODE(_readRssi), NULL},
+	{GEN_CMD_CODE(_readGain), NULL},
+	{GEN_CMD_CODE(_SetAtim), NULL}, /*35*/
+	{GEN_CMD_CODE(_SetPwrMode), NULL},
+	{GEN_CMD_CODE(_JoinbssRpt), NULL},
+	{GEN_CMD_CODE(_SetRaTable), NULL},
+	{GEN_CMD_CODE(_GetRaTable), NULL},
+
+	{GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/
+	{GEN_CMD_CODE(_GetDTMReport),	NULL},
+	{GEN_CMD_CODE(_GetTXRateStatistics), NULL},
+	{GEN_CMD_CODE(_SetUsbSuspend), NULL},
+	{GEN_CMD_CODE(_SetH2cLbk), NULL},
+	{GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/
+	{GEN_CMD_CODE(_SetChannel), NULL},		/*46*/
+	{GEN_CMD_CODE(_SetTxPower), NULL},
+	{GEN_CMD_CODE(_SwitchAntenna), NULL},
+	{GEN_CMD_CODE(_SetCrystalCap), NULL},
+	{GEN_CMD_CODE(_SetSingleCarrierTx), NULL},	/*50*/
+
+	{GEN_CMD_CODE(_SetSingleToneTx), NULL}, /*51*/
+	{GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL},
+	{GEN_CMD_CODE(_SetContinuousTx), NULL},
+	{GEN_CMD_CODE(_SwitchBandwidth), NULL},		/*54*/
+	{GEN_CMD_CODE(_TX_Beacon), NULL},/*55*/
+
+	{GEN_CMD_CODE(_Set_MLME_EVT), NULL},/*56*/
+	{GEN_CMD_CODE(_Set_Drv_Extra), NULL},/*57*/
+	{GEN_CMD_CODE(_Set_H2C_MSG), NULL},/*58*/
+	{GEN_CMD_CODE(_SetChannelPlan), NULL},/*59*/
+
+	{GEN_CMD_CODE(_SetChannelSwitch), NULL},/*60*/
+	{GEN_CMD_CODE(_TDLS), NULL},/*61*/
+	{GEN_CMD_CODE(_ChkBMCSleepq), NULL}, /*62*/
+
+	{GEN_CMD_CODE(_RunInThreadCMD), NULL},/*63*/
+};
+
+static struct cmd_hdl wlancmds[] = {
+	GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*10*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct joinbss_parm), join_cmd_hdl) /*14*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct createbss_parm), createbss_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl) /*18*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl) /*20*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct del_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setstapwrstate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphyinfo_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphyinfo_parm), NULL)  /*30*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)	/*40*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl) /* 46 */
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*50*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct Tx_Beacon_param), tx_beacon_hdl) /*55*/
+
+	GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl) /*56*/
+	GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl) /*57*/
+
+	GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl) /*58*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param), set_chplan_hdl) /*59*/
+
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param), set_csa_hdl) /*60*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param), tdls_hdl) /*61*/
+	GEN_MLME_EXT_HANDLER(0, chk_bmc_sleepq_hdl) /*62*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct RunInThread_param), run_in_thread_hdl) /*63*/
+};
+
+/*
+ * Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+ * No irqsave is necessary.
+ */
+
+int rtw_init_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	init_completion(&pcmdpriv->cmd_queue_comp);
+	init_completion(&pcmdpriv->terminate_cmdthread_comp);
+
+	INIT_LIST_HEAD(&pcmdpriv->cmd_queue.queue);
+	spin_lock_init(&pcmdpriv->cmd_queue.lock);
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+	pcmdpriv->cmd_seq = 1;
+
+	pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+
+	if (!pcmdpriv->cmd_allocated_buf)
+		return -ENOMEM;
+
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ((SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
+
+	pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
+
+	if (!pcmdpriv->rsp_allocated_buf) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+		return -ENOMEM;
+	}
+
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_issued_cnt = 0;
+	pcmdpriv->cmd_done_cnt = 0;
+	pcmdpriv->rsp_cnt = 0;
+
+	mutex_init(&pcmdpriv->sctx_mutex);
+
+	return 0;
+}
+
+static void c2h_wk_callback(struct work_struct *work);
+int rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	atomic_set(&pevtpriv->event_seq, 0);
+	pevtpriv->evt_done_cnt = 0;
+
+	_init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
+	pevtpriv->c2h_wk_alive = false;
+	pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1);
+	if (!pevtpriv->c2h_queue)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void _rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	_cancel_workitem_sync(&pevtpriv->c2h_wk);
+	while (pevtpriv->c2h_wk_alive)
+		msleep(10);
+
+	while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+		void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
+
+		if (c2h && c2h != (void *)pevtpriv)
+			kfree(c2h);
+	}
+	kfree(pevtpriv->c2h_queue);
+}
+
+void _rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	if (pcmdpriv) {
+		kfree(pcmdpriv->cmd_allocated_buf);
+
+		kfree(pcmdpriv->rsp_allocated_buf);
+
+		mutex_destroy(&pcmdpriv->sctx_mutex);
+	}
+}
+
+/*
+ * Calling Context:
+ *
+ * rtw_enqueue_cmd can only be called between kernel thread,
+ * since only spin_lock is used.
+ *
+ * ISR/Call-Back functions can't call this sub-function.
+ *
+ */
+
+int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
+{
+	unsigned long irqL;
+
+	if (!obj)
+		goto exit;
+
+	/* spin_lock_bh(&queue->lock); */
+	spin_lock_irqsave(&queue->lock, irqL);
+
+	list_add_tail(&obj->list, &queue->queue);
+
+	/* spin_unlock_bh(&queue->lock); */
+	spin_unlock_irqrestore(&queue->lock, irqL);
+
+exit:
+	return _SUCCESS;
+}
+
+struct	cmd_obj	*_rtw_dequeue_cmd(struct __queue *queue)
+{
+	unsigned long irqL;
+	struct cmd_obj *obj;
+
+	/* spin_lock_bh(&(queue->lock)); */
+	spin_lock_irqsave(&queue->lock, irqL);
+	if (list_empty(&queue->queue))
+		obj = NULL;
+	else {
+		obj = container_of(get_next(&queue->queue), struct cmd_obj, list);
+		list_del_init(&obj->list);
+	}
+
+	/* spin_unlock_bh(&(queue->lock)); */
+	spin_unlock_irqrestore(&queue->lock, irqL);
+
+	return obj;
+}
+
+void rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	_rtw_free_evt_priv(pevtpriv);
+}
+
+void rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	_rtw_free_cmd_priv(pcmdpriv);
+}
+
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj);
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
+
+	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+		bAllow = true;
+
+	if ((!pcmdpriv->padapter->hw_init_completed && !bAllow) ||
+		!atomic_read(&pcmdpriv->cmdthd_running))	/* com_thread not running */
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+int rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	int res = _FAIL;
+	struct adapter *padapter = pcmdpriv->padapter;
+
+	if (!cmd_obj)
+		goto exit;
+
+	cmd_obj->padapter = padapter;
+
+	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+	if (res == _FAIL) {
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+	if (res == _SUCCESS)
+		complete(&pcmdpriv->cmd_queue_comp);
+
+exit:
+	return res;
+}
+
+struct	cmd_obj	*rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+	return _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
+}
+
+void rtw_free_cmd_obj(struct cmd_obj *pcmd)
+{
+	if ((pcmd->cmdcode != _JoinBss_CMD_) &&
+	    (pcmd->cmdcode != _CreateBss_CMD_)) {
+		/* free parmbuf in cmd_obj */
+		kfree(pcmd->parmbuf);
+	}
+
+	if (pcmd->rsp) {
+		if (pcmd->rspsz != 0) {
+			/* free rsp in cmd_obj */
+			kfree(pcmd->rsp);
+		}
+	}
+
+	/* free cmd_obj */
+	kfree(pcmd);
+}
+
+void rtw_stop_cmd_thread(struct adapter *adapter)
+{
+	if (adapter->cmdThread &&
+		atomic_read(&adapter->cmdpriv.cmdthd_running) &&
+		adapter->cmdpriv.stop_req == 0) {
+		adapter->cmdpriv.stop_req = 1;
+		complete(&adapter->cmdpriv.cmd_queue_comp);
+		wait_for_completion(&adapter->cmdpriv.terminate_cmdthread_comp);
+	}
+}
+
+int rtw_cmd_thread(void *context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf;
+	u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
+	void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
+	struct adapter *padapter = context;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct drvextra_cmd_parm *extra_parm = NULL;
+
+	thread_enter("RTW_CMD_THREAD");
+
+	pcmdbuf = pcmdpriv->cmd_buf;
+
+	pcmdpriv->stop_req = 0;
+	atomic_set(&pcmdpriv->cmdthd_running, true);
+	complete(&pcmdpriv->terminate_cmdthread_comp);
+
+	while (1) {
+		if (wait_for_completion_interruptible(&pcmdpriv->cmd_queue_comp)) {
+			netdev_dbg(padapter->pnetdev,
+				   FUNC_ADPT_FMT " wait_for_completion_interruptible(&pcmdpriv->cmd_queue_comp) return != 0, break\n",
+				   FUNC_ADPT_ARG(padapter));
+			break;
+		}
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
+			netdev_dbg(padapter->pnetdev,
+				   "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				   __func__, padapter->bDriverStopped,
+				   padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		if (pcmdpriv->stop_req) {
+			netdev_dbg(padapter->pnetdev,
+				   FUNC_ADPT_FMT " stop_req:%u, break\n",
+				   FUNC_ADPT_ARG(padapter),
+				   pcmdpriv->stop_req);
+			break;
+		}
+
+		if (list_empty(&pcmdpriv->cmd_queue.queue))
+			continue;
+
+		if (rtw_register_cmd_alive(padapter) != _SUCCESS)
+			continue;
+
+_next:
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
+			netdev_dbg(padapter->pnetdev,
+				   "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				   __func__, padapter->bDriverStopped,
+				   padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd) {
+			rtw_unregister_cmd_alive(padapter);
+			continue;
+		}
+
+		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+			pcmd->res = H2C_DROPPED;
+			goto post_process;
+		}
+
+		pcmdpriv->cmd_issued_cnt++;
+
+		pcmd->cmdsz = round_up((pcmd->cmdsz), 4);
+
+		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+
+		if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
+			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+
+			if (cmd_hdl) {
+				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+				pcmd->res = ret;
+			}
+
+			pcmdpriv->cmd_seq++;
+		} else {
+			pcmd->res = H2C_PARAMETERS_ERROR;
+		}
+
+		cmd_hdl = NULL;
+
+post_process:
+
+		if (mutex_lock_interruptible(&pcmd->padapter->cmdpriv.sctx_mutex) == 0) {
+			if (pcmd->sctx) {
+				netdev_dbg(padapter->pnetdev,
+					   FUNC_ADPT_FMT " pcmd->sctx\n",
+					   FUNC_ADPT_ARG(pcmd->padapter));
+
+				if (pcmd->res == H2C_SUCCESS)
+					rtw_sctx_done(&pcmd->sctx);
+				else
+					rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_ERROR);
+			}
+			mutex_unlock(&pcmd->padapter->cmdpriv.sctx_mutex);
+		}
+
+		/* call callback function for post-processed */
+		if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
+			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+			if (!pcmd_callback) {
+				rtw_free_cmd_obj(pcmd);
+			} else {
+				/* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
+				pcmd_callback(pcmd->padapter, pcmd);/* need consider that free cmd_obj in rtw_cmd_callback */
+			}
+		} else {
+			rtw_free_cmd_obj(pcmd);
+		}
+		flush_signals_thread();
+		goto _next;
+	}
+
+	/*  free all cmd_obj resources */
+	do {
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd) {
+			rtw_unregister_cmd_alive(padapter);
+			break;
+		}
+
+		if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+			extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf;
+			if (extra_parm->pbuf && extra_parm->size > 0)
+				kfree(extra_parm->pbuf);
+		}
+
+		rtw_free_cmd_obj(pcmd);
+	} while (1);
+
+	complete(&pcmdpriv->terminate_cmdthread_comp);
+	atomic_set(&pcmdpriv->cmdthd_running, false);
+
+	return 0;
+}
+
+/*
+ * rtw_sitesurvey_cmd(~)
+ *	### NOTE:#### (!!!!)
+ *	MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+
+u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num,
+	struct rtw_ieee80211_channel *ch, int ch_num)
+{
+	u8 res = _FAIL;
+	struct cmd_obj *ph2c;
+	struct sitesurvey_parm *psurveyPara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c)
+		return _FAIL;
+
+	psurveyPara = rtw_zmalloc(sizeof(struct sitesurvey_parm));
+	if (!psurveyPara) {
+		kfree(ph2c);
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, false);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+	/* psurveyPara->bsslimit = 48; */
+	psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+	/* prepare ssid list */
+	if (ssid) {
+		int i;
+
+		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (ssid[i].ssid_length) {
+				memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
+				psurveyPara->ssid_num++;
+			}
+		}
+	}
+
+	/* prepare channel list */
+	if (ch) {
+		int i;
+
+		for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
+			if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
+				memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
+				psurveyPara->ch_num++;
+			}
+		}
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	if (res == _SUCCESS) {
+		pmlmepriv->scan_start_time = jiffies;
+		_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+	} else {
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+	return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	/* rtw_free_cmd_obj(pcmd); */
+	kfree(pcmd->parmbuf);
+	kfree(pcmd);
+}
+
+u8 rtw_createbss_cmd(struct adapter  *padapter)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
+	u8 res = _SUCCESS;
+
+	pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _CreateBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)pdev_network;
+	pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	pdev_network->length = pcmd->cmdsz;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+int rtw_startbss_cmd(struct adapter  *padapter, int flags)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv  *pcmdpriv = &padapter->cmdpriv;
+	struct submit_ctx sctx;
+	int res = _SUCCESS;
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		start_bss_network(padapter);
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!pcmd) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		INIT_LIST_HEAD(&pcmd->list);
+		pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
+		pcmd->parmbuf = NULL;
+		pcmd->cmdsz =  0;
+		pcmd->rsp = NULL;
+		pcmd->rspsz = 0;
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			pcmd->sctx = &sctx;
+			rtw_sctx_init(&sctx, 2000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx);
+			if (mutex_lock_interruptible(&pcmdpriv->sctx_mutex) == 0) {
+				if (sctx.status == RTW_SCTX_SUBMITTED)
+					pcmd->sctx = NULL;
+				mutex_unlock(&pcmdpriv->sctx_mutex);
+			}
+		}
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
+{
+	u8 res = _SUCCESS;
+	uint	t_len = 0;
+	struct wlan_bssid_ex *psecnetwork;
+	struct cmd_obj *pcmd;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	enum ndis_802_11_network_infrastructure ndis_network_mode = pnetwork->network.infrastructure_mode;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u32 tmp_len;
+	u8 *ptmp = NULL;
+
+	pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+	/* for ies is fix buf size */
+	t_len = sizeof(struct wlan_bssid_ex);
+
+
+	/* for hidden ap to set fw_state here */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) != true) {
+		switch (ndis_network_mode) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case Ndis802_11APMode:
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+	}
+
+	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+
+	memset(psecnetwork, 0, t_len);
+
+	memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
+
+	psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->ie_length;
+
+	if ((psecnetwork->ie_length-12) < (256-1))
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->ies[12], psecnetwork->ie_length-12);
+	else
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->ies[12], (256-1));
+
+	psecnetwork->ie_length = 0;
+	/*  Added by Albert 2009/02/18 */
+	/*  If the driver wants to use the bssid to create the connection. */
+	/*  If not,  we have to copy the connecting AP's MAC address to it so that */
+	/*  the driver just has the bssid information for PMKIDList searching. */
+
+	if (!pmlmepriv->assoc_by_bssid)
+		memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.mac_address[0], ETH_ALEN);
+
+	psecnetwork->ie_length = rtw_restruct_sec_ie(padapter, &pnetwork->network.ies[0], &psecnetwork->ies[0], pnetwork->network.ie_length);
+
+
+	pqospriv->qos_option = 0;
+
+	if (pregistrypriv->wmm_enable) {
+		tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.ies[0], &psecnetwork->ies[0], pnetwork->network.ie_length, psecnetwork->ie_length);
+
+		if (psecnetwork->ie_length != tmp_len) {
+			psecnetwork->ie_length = tmp_len;
+			pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
+		} else {
+			pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
+		}
+	}
+
+	phtpriv->ht_option = false;
+	ptmp = rtw_get_ie(&pnetwork->network.ies[12], WLAN_EID_HT_CAPABILITY, &tmp_len, pnetwork->network.ie_length-12);
+	if (pregistrypriv->ht_enable && ptmp && tmp_len > 0) {
+		/* Added by Albert 2010/06/23 */
+		/* For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
+		/* Especially for Realtek 8192u SoftAP. */
+		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
+			(padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
+			(padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+			rtw_ht_use_default_setting(padapter);
+
+			rtw_build_wmm_ie_ht(padapter, &psecnetwork->ies[12], &psecnetwork->ie_length);
+
+			/* rtw_restructure_ht_ie */
+			rtw_restructure_ht_ie(padapter, &pnetwork->network.ies[12], &psecnetwork->ies[0],
+									pnetwork->network.ie_length-12, &psecnetwork->ie_length,
+									pnetwork->network.configuration.ds_config);
+		}
+	}
+
+	rtw_append_exented_cap(padapter, &psecnetwork->ies[0], &psecnetwork->ie_length);
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.ies, pnetwork->network.ie_length);
+
+	pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
+
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
+	pcmd->parmbuf = (unsigned char *)psecnetwork;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
+{
+	struct cmd_obj *cmdobj = NULL;
+	struct disconnect_parm *param = NULL;
+	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* prepare cmd parameter */
+	param = rtw_zmalloc(sizeof(*param));
+	if (!param) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = rtw_zmalloc(sizeof(*cmdobj));
+		if (!cmdobj) {
+			res = _FAIL;
+			kfree(param);
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+		res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (disconnect_hdl(padapter, (u8 *)param) != H2C_SUCCESS)
+			res = _FAIL;
+		kfree(param);
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infrastructure networktype, bool enqueue)
+{
+	struct	cmd_obj *ph2c;
+	struct	setopmode_parm *psetop;
+
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	psetop = rtw_zmalloc(sizeof(struct setopmode_parm));
+
+	if (!psetop) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetop->mode = (u8)networktype;
+
+	if (enqueue) {
+		ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			kfree(psetop);
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		setopmode_hdl(padapter, (u8 *)psetop);
+		kfree(psetop);
+	}
+exit:
+	return res;
+}
+
+u8 rtw_setstakey_cmd(struct adapter *padapter, struct sta_info *sta, u8 unicast_key, bool enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8 res = _SUCCESS;
+
+	psetstakey_para = rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if (!psetstakey_para) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		psetstakey_para->algorithm = (unsigned char)psecuritypriv->dot11PrivacyAlgrthm;
+	else
+		GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
+
+	if (unicast_key)
+		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+	else
+		memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+
+	/* jeff: set this because at least sw key is ready */
+	padapter->securitypriv.busetkipkey = true;
+
+	if (enqueue) {
+		ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			kfree(psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (!psetstakey_rsp) {
+			kfree(ph2c);
+			kfree(psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *)psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		set_stakey_hdl(padapter, (u8 *)psetstakey_para);
+		kfree(psetstakey_para);
+	}
+exit:
+	return res;
+}
+
+u8 rtw_clearstakey_cmd(struct adapter *padapter, struct sta_info *sta, u8 enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	s16 cam_id = 0;
+	u8 res = _SUCCESS;
+
+	if (!enqueue) {
+		while ((cam_id = rtw_camid_search(padapter, sta->hwaddr, -1)) >= 0) {
+			netdev_dbg(padapter->pnetdev,
+				   "clear key for addr:%pM, camid:%d\n",
+				   MAC_ARG(sta->hwaddr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else {
+		ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = rtw_zmalloc(sizeof(struct set_stakey_parm));
+		if (!psetstakey_para) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (!psetstakey_rsp) {
+			kfree(ph2c);
+			kfree(psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *)psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+		memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+		psetstakey_para->algorithm = _NO_PRIVACY_;
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+exit:
+	return res;
+}
+
+u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
+{
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct addBaReq_parm *paddbareq_parm;
+
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm = rtw_zmalloc(sizeof(struct addBaReq_parm));
+	if (!paddbareq_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm->tid = tid;
+	memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+u8 rtw_reset_securitypriv_cmd(struct adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm  *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = RESET_SECURITYPRIV;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_free_assoc_resources_cmd(struct adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm  *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = FREE_ASSOC_RESOURCES;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm  *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* only  primary padapter does this cmd */
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+static void collect_traffic_statistics(struct adapter *padapter)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	/*  Tx */
+	pdvobjpriv->traffic_stat.tx_bytes = padapter->xmitpriv.tx_bytes;
+	pdvobjpriv->traffic_stat.tx_pkts = padapter->xmitpriv.tx_pkts;
+	pdvobjpriv->traffic_stat.tx_drop = padapter->xmitpriv.tx_drop;
+
+	/*  Rx */
+	pdvobjpriv->traffic_stat.rx_bytes = padapter->recvpriv.rx_bytes;
+	pdvobjpriv->traffic_stat.rx_pkts = padapter->recvpriv.rx_pkts;
+	pdvobjpriv->traffic_stat.rx_drop = padapter->recvpriv.rx_drop;
+
+	/*  Calculate throughput in last interval */
+	pdvobjpriv->traffic_stat.cur_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes - pdvobjpriv->traffic_stat.last_tx_bytes;
+	pdvobjpriv->traffic_stat.cur_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes - pdvobjpriv->traffic_stat.last_rx_bytes;
+	pdvobjpriv->traffic_stat.last_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes;
+	pdvobjpriv->traffic_stat.last_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes;
+
+	pdvobjpriv->traffic_stat.cur_tx_tp = (u32)(pdvobjpriv->traffic_stat.cur_tx_bytes * 8/2/1024/1024);
+	pdvobjpriv->traffic_stat.cur_rx_tp = (u32)(pdvobjpriv->traffic_stat.cur_rx_bytes * 8/2/1024/1024);
+}
+
+u8 traffic_status_watchdog(struct adapter *padapter, u8 from_timer)
+{
+	u8 bEnterPS = false;
+	u16 BusyThresholdHigh = 25;
+	u16 BusyThresholdLow = 10;
+	u16 BusyThreshold = BusyThresholdHigh;
+	u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
+	u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	collect_traffic_statistics(padapter);
+
+	/*  */
+	/*  Determine if our traffic is busy now */
+	/*  */
+	if ((check_fwstate(pmlmepriv, _FW_LINKED))
+		/*&& !MgntInitAdapterInProgress(pMgntInfo)*/) {
+		/*  if we raise bBusyTraffic in last watchdog, using lower threshold. */
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			BusyThreshold = BusyThresholdLow;
+
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) {
+			bBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bRxBusyTraffic = true;
+			else
+				bTxBusyTraffic = true;
+		}
+
+		/*  Higher Tx/Rx data. */
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+			bHigherBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bHigherBusyRxTraffic = true;
+			else
+				bHigherBusyTxTraffic = true;
+		}
+
+		/*  check traffic for  powersaving. */
+		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+			(pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)) {
+			bEnterPS = false;
+
+			if (bBusyTraffic) {
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4;
+
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount++;
+
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30;
+			}
+		} else {
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount >= 2)
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount -= 2;
+			else
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0)
+				bEnterPS = true;
+		}
+
+		/*  LeisurePS only work in infra mode. */
+		if (bEnterPS) {
+			if (!from_timer)
+				LPS_Enter(padapter, "TRAFFIC_IDLE");
+		} else {
+			if (!from_timer)
+				LPS_Leave(padapter, "TRAFFIC_BUSY");
+			else
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 1);
+		}
+	} else {
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		int n_assoc_iface = 0;
+
+		if (check_fwstate(&dvobj->padapters->mlmepriv, WIFI_ASOC_STATE))
+			n_assoc_iface++;
+
+		if (!from_timer && n_assoc_iface == 0)
+			LPS_Leave(padapter, "NON_LINKED");
+	}
+
+	pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+
+	return bEnterPS;
+
+}
+
+static void dynamic_chk_wk_hdl(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		expire_timeout_chk(padapter);
+
+	/* for debug purpose */
+	_linked_info_dump(padapter);
+	/* if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY) ==false) */
+	{
+		linked_status_chk(padapter);
+		traffic_status_watchdog(padapter, 0);
+	}
+	rtw_hal_dm_watchdog(padapter);
+
+	/* check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); */
+
+	/*  */
+	/*  BT-Coexist */
+	/*  */
+	hal_btcoex_Handler(padapter);
+
+
+	/* always call rtw_ps_processor() at last one. */
+	if (is_primary_adapter(padapter))
+		rtw_ps_processor(padapter);
+}
+
+void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type);
+void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 mstatus;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+		check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		return;
+	}
+
+	switch (lps_ctrl_type) {
+	case LPS_CTRL_SCAN:
+		hal_btcoex_ScanNotify(padapter, true);
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			/*  connect */
+			LPS_Leave(padapter, "LPS_CTRL_SCAN");
+		}
+		break;
+	case LPS_CTRL_JOINBSS:
+		LPS_Leave(padapter, "LPS_CTRL_JOINBSS");
+		break;
+	case LPS_CTRL_CONNECT:
+		mstatus = 1;/* connect */
+		/*  Reset LPS Setting */
+		pwrpriv->LpsIdleCount = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+		break;
+	case LPS_CTRL_DISCONNECT:
+		mstatus = 0;/* disconnect */
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+		LPS_Leave(padapter, "LPS_CTRL_DISCONNECT");
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+		break;
+	case LPS_CTRL_SPECIAL_PACKET:
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+		hal_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP);
+		LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET");
+		break;
+	case LPS_CTRL_LEAVE:
+		LPS_Leave(padapter, "LPS_CTRL_LEAVE");
+		break;
+	case LPS_CTRL_TRAFFIC_BUSY:
+		LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY");
+		break;
+	default:
+		break;
+	}
+}
+
+u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	/* struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); */
+	u8 res = _SUCCESS;
+
+	/* if (!pwrctrlpriv->bLeisurePs) */
+	/* 	return res; */
+
+	if (enqueue) {
+		ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (!pdrvextra_cmd_parm) {
+			kfree(ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
+		pdrvextra_cmd_parm->type = lps_ctrl_type;
+		pdrvextra_cmd_parm->size = 0;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
+	}
+
+exit:
+	return res;
+}
+
+static void rtw_dm_in_lps_hdl(struct adapter *padapter)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS, NULL);
+}
+
+u8 rtw_dm_in_lps_wk_cmd(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DM_IN_LPS_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+static void rtw_lps_change_dtim_hdl(struct adapter *padapter, u8 dtim)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (dtim <= 0 || dtim > 16)
+		return;
+
+	if (hal_btcoex_IsBtControlLps(padapter))
+		return;
+
+	mutex_lock(&pwrpriv->lock);
+
+	pwrpriv->dtim = dtim;
+
+	if (pwrpriv->fw_current_in_ps_mode && (pwrpriv->pwr_mode > PS_MODE_ACTIVE)) {
+		u8 ps_mode = pwrpriv->pwr_mode;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+	}
+
+	mutex_unlock(&pwrpriv->lock);
+}
+
+static void rtw_dm_ra_mask_hdl(struct adapter *padapter, struct sta_info *psta)
+{
+	if (psta)
+		set_sta_rate(padapter, psta);
+}
+
+u8 rtw_dm_ra_mask_wk_cmd(struct adapter *padapter, u8 *psta)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DM_RA_MSK_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = psta;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+
+}
+
+u8 rtw_ps_cmd(struct adapter *padapter)
+{
+	struct cmd_obj		*ppscmd;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+	ppscmd = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ppscmd) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ppscmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
+
+exit:
+	return res;
+}
+
+u32 g_wait_hiq_empty;
+
+static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
+{
+	struct sta_info *psta_bmc;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned long start = jiffies;
+	u8 empty = false;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
+
+	while (!empty && jiffies_to_msecs(jiffies - start) < g_wait_hiq_empty) {
+		msleep(100);
+		rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
+	}
+
+	if (psta_bmc->sleepq_len == 0) {
+		if (empty == _SUCCESS) {
+			bool update_tim = false;
+
+			if (pstapriv->tim_bitmap & BIT(0))
+				update_tim = true;
+
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			if (update_tim)
+				update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+		} else {/* re check again */
+			rtw_chk_hi_queue_cmd(padapter);
+		}
+
+	}
+
+}
+
+u8 rtw_chk_hi_queue_cmd(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+struct btinfo {
+	u8 cid;
+	u8 len;
+
+	u8 bConnection:1;
+	u8 bSCOeSCO:1;
+	u8 bInQPage:1;
+	u8 bACLBusy:1;
+	u8 bSCOBusy:1;
+	u8 bHID:1;
+	u8 bA2DP:1;
+	u8 bFTP:1;
+
+	u8 retry_cnt:4;
+	u8 rsvd_34:1;
+	u8 rsvd_35:1;
+	u8 rsvd_36:1;
+	u8 rsvd_37:1;
+
+	u8 rssi;
+
+	u8 rsvd_50:1;
+	u8 rsvd_51:1;
+	u8 rsvd_52:1;
+	u8 rsvd_53:1;
+	u8 rsvd_54:1;
+	u8 rsvd_55:1;
+	u8 eSCO_SCO:1;
+	u8 Master_Slave:1;
+
+	u8 rsvd_6;
+	u8 rsvd_7;
+};
+
+static void rtw_btinfo_hdl(struct adapter *adapter, u8 *buf, u16 buf_len)
+{
+	#define BTINFO_WIFI_FETCH 0x23
+	#define BTINFO_BT_AUTO_RPT 0x27
+	struct btinfo *info = (struct btinfo *)buf;
+	u8 cmd_idx;
+	u8 len;
+
+	cmd_idx = info->cid;
+
+	if (info->len > buf_len-2) {
+		rtw_warn_on(1);
+		len = buf_len-2;
+	} else {
+		len = info->len;
+	}
+
+	/* transform BT-FW btinfo to WiFI-FW C2H format and notify */
+	if (cmd_idx == BTINFO_WIFI_FETCH)
+		buf[1] = 0;
+	else if (cmd_idx == BTINFO_BT_AUTO_RPT)
+		buf[1] = 2;
+	hal_btcoex_BtInfoNotify(adapter, len+1, &buf[1]);
+}
+
+u8 rtw_c2h_packet_wk_cmd(struct adapter *padapter, u8 *pbuf, u16 length)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = length;
+	pdrvextra_cmd_parm->pbuf = pbuf;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+/* dont call R/W in this function, beucase SDIO interrupt have claim host */
+/* or deadlock will happen and cause special-systemserver-died in android */
+u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size =  c2h_evt?16:0;
+	pdrvextra_cmd_parm->pbuf = c2h_evt;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+static void c2h_wk_callback(struct work_struct *work)
+{
+	struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv);
+	u8 *c2h_evt;
+	c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
+
+	evtpriv->c2h_wk_alive = true;
+
+	while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+		c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue);
+		if (c2h_evt) {
+			/* This C2H event is read, clear it */
+			c2h_evt_clear(adapter);
+		} else {
+			c2h_evt = rtw_malloc(16);
+			if (c2h_evt) {
+				/* This C2H event is not read, read & clear now */
+				if (c2h_evt_read_88xx(adapter, c2h_evt) != _SUCCESS) {
+					kfree(c2h_evt);
+					continue;
+				}
+			}
+		}
+
+		/* Special pointer to trigger c2h_evt_clear only */
+		if ((void *)c2h_evt == (void *)evtpriv)
+			continue;
+
+		if (!rtw_hal_c2h_valid(adapter, c2h_evt)) {
+			kfree(c2h_evt);
+			continue;
+		}
+
+		if (ccx_id_filter(c2h_evt)) {
+			/* Handle CCX report here */
+			rtw_hal_c2h_handler(adapter, c2h_evt);
+			kfree(c2h_evt);
+		} else {
+			/* Enqueue into cmd_thread for others */
+			rtw_c2h_wk_cmd(adapter, c2h_evt);
+		}
+	}
+
+	evtpriv->c2h_wk_alive = false;
+}
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct drvextra_cmd_parm *pdrvextra_cmd;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
+
+	switch (pdrvextra_cmd->ec_id) {
+	case DYNAMIC_CHK_WK_CID:/* only  primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces */
+		dynamic_chk_wk_hdl(padapter);
+		break;
+	case POWER_SAVING_CTRL_WK_CID:
+		rtw_ps_processor(padapter);
+		break;
+	case LPS_CTRL_WK_CID:
+		lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type);
+		break;
+	case DM_IN_LPS_WK_CID:
+		rtw_dm_in_lps_hdl(padapter);
+		break;
+	case LPS_CHANGE_DTIM_CID:
+		rtw_lps_change_dtim_hdl(padapter, (u8)pdrvextra_cmd->type);
+		break;
+	case CHECK_HIQ_WK_CID:
+		rtw_chk_hi_queue_hdl(padapter);
+		break;
+	/* add for CONFIG_IEEE80211W, none 11w can use it */
+	case RESET_SECURITYPRIV:
+		rtw_reset_securitypriv(padapter);
+		break;
+	case FREE_ASSOC_RESOURCES:
+		rtw_free_assoc_resources(padapter, 1);
+		break;
+	case C2H_WK_CID:
+		rtw_hal_set_hwreg_with_buf(padapter, HW_VAR_C2H_HANDLE, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+		break;
+	case DM_RA_MSK_WK_CID:
+		rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf);
+		break;
+	case BTINFO_WK_CID:
+		rtw_btinfo_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+		break;
+	default:
+		break;
+	}
+
+	if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size > 0)
+		kfree(pdrvextra_cmd->pbuf);
+
+	return H2C_SUCCESS;
+}
+
+void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+	}
+
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_disassoc_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		spin_lock_bh(&pmlmepriv->lock);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		spin_unlock_bh(&pmlmepriv->lock);
+
+		return;
+	}
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	if (!pcmd->parmbuf)
+		goto exit;
+
+	if (pcmd->res != H2C_SUCCESS)
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+	del_timer_sync(&pmlmepriv->assoc_timer);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->mac_address);
+		if (!psta) {
+			psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->mac_address);
+			if (!psta)
+				goto createbss_cmd_fail;
+		}
+
+		rtw_indicate_connect(padapter);
+	} else {
+		pwlan = rtw_alloc_network(pmlmepriv);
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+		if (!pwlan) {
+			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+			if (!pwlan) {
+				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+				goto createbss_cmd_fail;
+			}
+			pwlan->last_scanned = jiffies;
+		} else {
+			list_add_tail(&pwlan->list, &pmlmepriv->scanned_queue.queue);
+		}
+
+		pnetwork->length = get_wlan_bssid_ex_sz(pnetwork);
+		memcpy(&pwlan->network, pnetwork, pnetwork->length);
+		/* pwlan->fixed = true; */
+
+		/* list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue); */
+
+		/*  copy pdev_network information to	pmlmepriv->cur_network */
+		memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
+
+		/*  reset ds_config */
+		/* tgt_network->network.configuration.ds_config = (u32)rtw_ch2freq(pnetwork->configuration.ds_config); */
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+		/*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
+
+	}
+
+createbss_cmd_fail:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
+	struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
+
+	if (!psta)
+		goto exit;
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
+	struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
+	struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
+
+	if (!psta)
+		goto exit;
+
+	psta->aid = passocsta_rsp->cam_id;
+	psta->mac_id = passocsta_rsp->cam_id;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) && check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_debug.c b/drivers/staging/rtl8723bs/core/rtw_debug.c
new file mode 100644
index 000000000..5354fdd11
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_debug.c
@@ -0,0 +1,68 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+
+#include <rtw_version.h>
+
+static void dump_4_regs(struct adapter *adapter, int offset)
+{
+	u32 reg[4];
+	int i;
+
+	for (i = 0; i < 4; i++)
+		reg[i] = rtw_read32(adapter, offset + i);
+
+	netdev_dbg(adapter->pnetdev, "0x%03x 0x%08x 0x%08x 0x%08x 0x%08x\n",
+		   i, reg[0], reg[1], reg[2], reg[3]);
+}
+
+void mac_reg_dump(struct adapter *adapter)
+{
+	int i;
+
+	netdev_dbg(adapter->pnetdev, "======= MAC REG =======\n");
+
+	for (i = 0x0; i < 0x800; i += 4)
+		dump_4_regs(adapter, i);
+}
+
+void bb_reg_dump(struct adapter *adapter)
+{
+	int i;
+
+	netdev_dbg(adapter->pnetdev, "======= BB REG =======\n");
+
+	for (i = 0x800; i < 0x1000 ; i += 4)
+		dump_4_regs(adapter, i);
+}
+
+static void dump_4_rf_regs(struct adapter *adapter, int path, int offset)
+{
+	u8 reg[4];
+	int i;
+
+	for (i = 0; i < 4; i++)
+		reg[i] = rtw_hal_read_rfreg(adapter, path, offset + i,
+					    0xffffffff);
+
+	netdev_dbg(adapter->pnetdev, "0x%02x 0x%08x 0x%08x 0x%08x 0x%08x\n",
+		   i, reg[0], reg[1], reg[2], reg[3]);
+}
+
+void rf_reg_dump(struct adapter *adapter)
+{
+	int i, path = 0;
+
+	netdev_dbg(adapter->pnetdev, "======= RF REG =======\n");
+
+	netdev_dbg(adapter->pnetdev, "RF_Path(%x)\n", path);
+	for (i = 0; i < 0x100; i++)
+		dump_4_rf_regs(adapter, path, i);
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_efuse.c b/drivers/staging/rtl8723bs/core/rtw_efuse.c
new file mode 100644
index 000000000..06e727ce9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_efuse.c
@@ -0,0 +1,540 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/jiffies.h>
+
+
+/* Define global variables */
+u8 fakeEfuseBank;
+u32 fakeEfuseUsedBytes;
+u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0};
+u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0};
+u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0};
+
+u32 BTEfuseUsedBytes;
+u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+u32 fakeBTEfuseUsedBytes;
+u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+#define REG_EFUSE_CTRL		0x0030
+#define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
+
+static bool
+Efuse_Read1ByteFromFakeContent(u16 Offset, u8 *Value)
+{
+	if (Offset >= EFUSE_MAX_HW_SIZE)
+		return false;
+	if (fakeEfuseBank == 0)
+		*Value = fakeEfuseContent[Offset];
+	else
+		*Value = fakeBTEfuseContent[fakeEfuseBank-1][Offset];
+	return true;
+}
+
+static bool
+Efuse_Write1ByteToFakeContent(u16 Offset, u8 Value)
+{
+	if (Offset >= EFUSE_MAX_HW_SIZE)
+		return false;
+	if (fakeEfuseBank == 0)
+		fakeEfuseContent[Offset] = Value;
+	else
+		fakeBTEfuseContent[fakeEfuseBank-1][Offset] = Value;
+	return true;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_PowerSwitch
+ *
+ * Overview:	When we want to enable write operation, we should change to
+ *			pwr on state. When we stop write, we should switch to 500k mode
+ *			and disable LDO 2.5V.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/17/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+Efuse_PowerSwitch(
+struct adapter *padapter,
+u8 bWrite,
+u8 PwrState)
+{
+	padapter->HalFunc.EfusePowerSwitch(padapter, bWrite, PwrState);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_GetCurrentSize
+ *
+ * Overview:	Get current efuse size!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u16
+Efuse_GetCurrentSize(
+	struct adapter *padapter,
+	u8	efuseType,
+	bool		bPseudoTest)
+{
+	return padapter->HalFunc.EfuseGetCurrentSize(padapter, efuseType,
+						     bPseudoTest);
+}
+
+/*  11/16/2008 MH Add description. Get current efuse area enabled word!!. */
+u8
+Efuse_CalculateWordCnts(u8 word_en)
+{
+	u8 word_cnts = 0;
+	if (!(word_en & BIT(0)))
+		word_cnts++; /*  0 : write enable */
+	if (!(word_en & BIT(1)))
+		word_cnts++;
+	if (!(word_en & BIT(2)))
+		word_cnts++;
+	if (!(word_en & BIT(3)))
+		word_cnts++;
+	return word_cnts;
+}
+
+/*  */
+/* Description: */
+/*		1. Execute E-Fuse read byte operation according as map offset and */
+/*			save to E-Fuse table. */
+/*		2. Referred from SD1 Richard. */
+/*  */
+/* Assumption: */
+/*		1. Boot from E-Fuse and successfully auto-load. */
+/*		2. PASSIVE_LEVEL (USB interface) */
+/*  */
+/* Created by Roger, 2008.10.21. */
+/*  */
+/* 2008/12/12 MH	1. Reorganize code flow and reserve bytes. and add description. */
+/*					2. Add efuse utilization collect. */
+/* 2008/12/22 MH	Read Efuse must check if we write section 1 data again!!! Sec1 */
+/*					write addr must be after sec5. */
+/*  */
+
+void
+efuse_ReadEFuse(
+	struct adapter *Adapter,
+	u8 efuseType,
+	u16		_offset,
+	u16		_size_byte,
+	u8 *pbuf,
+bool	bPseudoTest
+	);
+void
+efuse_ReadEFuse(
+	struct adapter *Adapter,
+	u8 efuseType,
+	u16		_offset,
+	u16		_size_byte,
+	u8 *pbuf,
+bool	bPseudoTest
+	)
+{
+	Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest);
+}
+
+void
+EFUSE_GetEfuseDefinition(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 type,
+	void	*pOut,
+	bool		bPseudoTest
+	)
+{
+	padapter->HalFunc.EFUSEGetEfuseDefinition(padapter, efuseType, type, pOut, bPseudoTest);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_Read1Byte
+ *
+ * Overview:	Copy from WMAC fot EFUSE read 1 byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 09/23/2008	MHC		Copy from WMAC.
+ *
+ *---------------------------------------------------------------------------*/
+u8
+EFUSE_Read1Byte(
+struct adapter *Adapter,
+u16		Address)
+{
+	u8 Bytetemp = {0x00};
+	u8 temp = {0x00};
+	u32 k = 0;
+	u16 contentLen = 0;
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&contentLen, false);
+
+	if (Address < contentLen) {/* E-fuse 512Byte */
+		/* Write E-fuse Register address bit0~7 */
+		temp = Address & 0xFF;
+		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		/* Write E-fuse Register address bit8~9 */
+		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
+		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+
+		/* Write 0x30[31]= 0 */
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		temp = Bytetemp & 0x7F;
+		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+
+		/* Wait Write-ready (0x30[31]= 1) */
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		while (!(Bytetemp & 0x80)) {
+			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			k++;
+			if (k == 1000)
+				break;
+		}
+		return rtw_read8(Adapter, EFUSE_CTRL);
+	} else
+		return 0xFF;
+
+} /* EFUSE_Read1Byte */
+
+/*  11/16/2008 MH Read one byte from real Efuse. */
+u8
+efuse_OneByteRead(
+struct adapter *padapter,
+u16	addr,
+u8	*data,
+bool		bPseudoTest)
+{
+	u32 tmpidx = 0;
+	u8 bResult;
+	u8 readbyte;
+
+	if (bPseudoTest)
+		return Efuse_Read1ByteFromFakeContent(addr, data);
+
+	/*  <20130121, Kordan> For SMIC EFUSE specificatoin. */
+	/* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */
+	/* PHY_SetMacReg(padapter, 0x34, BIT11, 0); */
+	rtw_write16(padapter, 0x34, rtw_read16(padapter, 0x34) & (~BIT11));
+
+	/*  -----------------e-fuse reg ctrl --------------------------------- */
+	/* address */
+	rtw_write8(padapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtw_write8(padapter, EFUSE_CTRL+2, ((u8)((addr>>8) & 0x03)) |
+	(rtw_read8(padapter, EFUSE_CTRL+2)&0xFC));
+
+	/* rtw_write8(padapter, EFUSE_CTRL+3,  0x72); read cmd */
+	/* Write bit 32 0 */
+	readbyte = rtw_read8(padapter, EFUSE_CTRL+3);
+	rtw_write8(padapter, EFUSE_CTRL+3, (readbyte & 0x7f));
+
+	while (!(0x80 & rtw_read8(padapter, EFUSE_CTRL+3)) && (tmpidx < 1000)) {
+		mdelay(1);
+		tmpidx++;
+	}
+	if (tmpidx < 100) {
+		*data = rtw_read8(padapter, EFUSE_CTRL);
+		bResult = true;
+	} else {
+		*data = 0xff;
+		bResult = false;
+	}
+
+	return bResult;
+}
+
+/*  11/16/2008 MH Write one byte to reald Efuse. */
+u8 efuse_OneByteWrite(struct adapter *padapter, u16 addr, u8 data, bool bPseudoTest)
+{
+	u8 tmpidx = 0;
+	u8 bResult = false;
+	u32 efuseValue = 0;
+
+	if (bPseudoTest)
+		return Efuse_Write1ByteToFakeContent(addr, data);
+
+
+	/*  -----------------e-fuse reg ctrl --------------------------------- */
+	/* address */
+
+
+	efuseValue = rtw_read32(padapter, EFUSE_CTRL);
+	efuseValue |= (BIT21|BIT31);
+	efuseValue &= ~(0x3FFFF);
+	efuseValue |= ((addr<<8 | data) & 0x3FFFF);
+
+
+	/*  <20130227, Kordan> 8192E MP chip A-cut had better not set 0x34[11] until B-Cut. */
+
+	/*  <20130121, Kordan> For SMIC EFUSE specificatoin. */
+	/* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */
+	/* PHY_SetMacReg(padapter, 0x34, BIT11, 1); */
+	rtw_write16(padapter, 0x34, rtw_read16(padapter, 0x34) | (BIT11));
+	rtw_write32(padapter, EFUSE_CTRL, 0x90600000|((addr<<8 | data)));
+
+	while ((0x80 &  rtw_read8(padapter, EFUSE_CTRL+3)) && (tmpidx < 100)) {
+		mdelay(1);
+		tmpidx++;
+	}
+
+	if (tmpidx < 100)
+		bResult = true;
+	else
+		bResult = false;
+
+	/*  disable Efuse program enable */
+	PHY_SetMacReg(padapter, EFUSE_TEST, BIT(11), 0);
+
+	return bResult;
+}
+
+int
+Efuse_PgPacketRead(struct adapter *padapter,
+				u8	offset,
+				u8	*data,
+				bool		bPseudoTest)
+{
+	return padapter->HalFunc.Efuse_PgPacketRead(padapter, offset, data,
+						    bPseudoTest);
+}
+
+int
+Efuse_PgPacketWrite(struct adapter *padapter,
+				u8	offset,
+				u8	word_en,
+				u8	*data,
+				bool		bPseudoTest)
+{
+	return padapter->HalFunc.Efuse_PgPacketWrite(padapter, offset, word_en,
+						     data, bPseudoTest);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_WordEnableDataRead
+ *
+ * Overview:	Read allowed word in current efuse section data.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ * 11/21/2008	MHC		Fix Write bug when we only enable late word.
+ *
+ *---------------------------------------------------------------------------*/
+void
+efuse_WordEnableDataRead(u8 word_en,
+						u8 *sourdata,
+						u8 *targetdata)
+{
+	if (!(word_en&BIT(0))) {
+		targetdata[0] = sourdata[0];
+		targetdata[1] = sourdata[1];
+	}
+	if (!(word_en&BIT(1))) {
+		targetdata[2] = sourdata[2];
+		targetdata[3] = sourdata[3];
+	}
+	if (!(word_en&BIT(2))) {
+		targetdata[4] = sourdata[4];
+		targetdata[5] = sourdata[5];
+	}
+	if (!(word_en&BIT(3))) {
+		targetdata[6] = sourdata[6];
+		targetdata[7] = sourdata[7];
+	}
+}
+
+
+u8
+Efuse_WordEnableDataWrite(struct adapter *padapter,
+						u16		efuse_addr,
+						u8 word_en,
+						u8 *data,
+						bool		bPseudoTest)
+{
+	return padapter->HalFunc.Efuse_WordEnableDataWrite(padapter, efuse_addr,
+							   word_en, data,
+							   bPseudoTest);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_ReadAllMap
+ *
+ * Overview:	Read All Efuse content
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/11/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+Efuse_ReadAllMap(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 *Efuse,
+	bool		bPseudoTest);
+void Efuse_ReadAllMap(struct adapter *padapter, u8 efuseType, u8 *Efuse, bool bPseudoTest)
+{
+	u16 mapLen = 0;
+
+	Efuse_PowerSwitch(padapter, false, true);
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest);
+
+	efuse_ReadEFuse(padapter, efuseType, 0, mapLen, Efuse, bPseudoTest);
+
+	Efuse_PowerSwitch(padapter, false, false);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_ShadowRead1Byte
+ *		efuse_ShadowRead2Byte
+ *		efuse_ShadowRead4Byte
+ *
+ * Overview:	Read from efuse init map by one/two/four bytes !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static void efuse_ShadowRead1Byte(struct adapter *padapter, u16 Offset, u8 *Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+
+}	/*  EFUSE_ShadowRead1Byte */
+
+/* Read Two Bytes */
+static void efuse_ShadowRead2Byte(struct adapter *padapter, u16 Offset, u16 *Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+
+}	/*  EFUSE_ShadowRead2Byte */
+
+/* Read Four Bytes */
+static void efuse_ShadowRead4Byte(struct adapter *padapter, u16 Offset, u32 *Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+2]<<16;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+3]<<24;
+
+}	/*  efuse_ShadowRead4Byte */
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowMapUpdate
+ *
+ * Overview:	Transfer current EFUSE content to shadow init and modify map.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/13/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowMapUpdate(struct adapter *padapter, u8 efuseType, bool bPseudoTest)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	u16 mapLen = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest);
+
+	if (pEEPROM->bautoload_fail_flag)
+		memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen);
+	else
+		Efuse_ReadAllMap(padapter, efuseType, pEEPROM->efuse_eeprom_data, bPseudoTest);
+
+	/* PlatformMoveMemory((void *)&pHalData->EfuseMap[EFUSE_MODIFY_MAP][0], */
+	/* void *)&pHalData->EfuseMap[EFUSE_INIT_MAP][0], mapLen); */
+} /*  EFUSE_ShadowMapUpdate */
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowRead
+ *
+ * Overview:	Read from efuse init map !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowRead(struct adapter *padapter, u8 Type, u16 Offset, u32 *Value)
+{
+	if (Type == 1)
+		efuse_ShadowRead1Byte(padapter, Offset, (u8 *)Value);
+	else if (Type == 2)
+		efuse_ShadowRead2Byte(padapter, Offset, (u16 *)Value);
+	else if (Type == 4)
+		efuse_ShadowRead4Byte(padapter, Offset, (u32 *)Value);
+
+}	/* EFUSE_ShadowRead*/
diff --git a/drivers/staging/rtl8723bs/core/rtw_ieee80211.c b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
new file mode 100644
index 000000000..3d8a64f69
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
@@ -0,0 +1,1162 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/of.h>
+#include <asm/unaligned.h>
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+/*  */
+/*  for adhoc-master to generate ie and provide supported-rate to fw */
+/*  */
+
+static u8 WIFI_CCKRATES[] = {
+		(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+};
+
+static u8 WIFI_OFDMRATES[] = {
+		(IEEE80211_OFDM_RATE_6MB),
+		(IEEE80211_OFDM_RATE_9MB),
+		(IEEE80211_OFDM_RATE_12MB),
+		(IEEE80211_OFDM_RATE_18MB),
+		(IEEE80211_OFDM_RATE_24MB),
+		IEEE80211_OFDM_RATE_36MB,
+		IEEE80211_OFDM_RATE_48MB,
+		IEEE80211_OFDM_RATE_54MB
+};
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+	unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /*  last element must be zero!! */
+	int i = 0;
+
+	while (dot11_rate_table[i] != 0) {
+		if (dot11_rate_table[i] == val)
+			return BIT(i);
+		i++;
+	}
+	return 0;
+}
+
+bool rtw_is_cckrates_included(u8 *rate)
+{
+	while (*rate) {
+		u8 r = *rate & 0x7f;
+
+		if (r == 2 || r == 4 || r == 11 || r == 22)
+			return true;
+		rate++;
+	}
+
+	return false;
+}
+
+bool rtw_is_cckratesonly_included(u8 *rate)
+{
+	while (*rate) {
+		u8 r = *rate & 0x7f;
+
+		if (r != 2 && r != 4 && r != 11 && r != 22)
+			return false;
+		rate++;
+	}
+
+	return true;
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+	if (channel > 14)
+		return WIRELESS_INVALID;
+	/* could be pure B, pure G, or B/G */
+	if (rtw_is_cckratesonly_included(rate))
+		return WIRELESS_11B;
+	if (rtw_is_cckrates_included(rate))
+		return WIRELESS_11BG;
+	return WIRELESS_11G;
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+				unsigned int *frlen)
+{
+	memcpy((void *)pbuf, (void *)source, len);
+	*frlen = *frlen + len;
+	return pbuf + len;
+}
+
+/*  rtw_set_ie will update frame length */
+u8 *rtw_set_ie(u8 *pbuf,
+	       signed int index,
+	       uint len,
+	       u8 *source,
+	       uint *frlen) /* frame length */
+{
+	*pbuf = (u8)index;
+
+	*(pbuf + 1) = (u8)len;
+
+	if (len > 0)
+		memcpy((void *)(pbuf + 2), (void *)source, len);
+
+	*frlen = *frlen + (len + 2);
+
+	return pbuf + len + 2;
+}
+
+/*----------------------------------------------------------------------------
+index: the information element id index, limit is the limit for search
+-----------------------------------------------------------------------------*/
+u8 *rtw_get_ie(u8 *pbuf, signed int index, signed int *len, signed int limit)
+{
+	signed int tmp, i;
+	u8 *p;
+
+	if (limit < 1)
+		return NULL;
+
+	p = pbuf;
+	i = 0;
+	*len = 0;
+	while (1) {
+		if (*p == index) {
+			*len = *(p + 1);
+			return p;
+		}
+		tmp = *(p + 1);
+		p += (tmp + 2);
+		i += (tmp + 2);
+		if (i >= limit)
+			break;
+	}
+	return NULL;
+}
+
+/**
+ * rtw_get_ie_ex - Search specific IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
+ * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
+ *
+ * Returns: The address of the specific IE found, or NULL
+ */
+u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
+{
+	uint cnt;
+	u8 *target_ie = NULL;
+
+	if (ielen)
+		*ielen = 0;
+
+	if (!in_ie || in_len <= 0)
+		return target_ie;
+
+	cnt = 0;
+
+	while (cnt < in_len) {
+		if (eid == in_ie[cnt]
+			&& (!oui || !memcmp(&in_ie[cnt+2], oui, oui_len))) {
+			target_ie = &in_ie[cnt];
+
+			if (ie)
+				memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+			if (ielen)
+				*ielen = in_ie[cnt+1]+2;
+
+			break;
+		}
+		cnt += in_ie[cnt+1]+2; /* goto next */
+	}
+
+	return target_ie;
+}
+
+/**
+ * rtw_ies_remove_ie - Find matching IEs and remove
+ * @ies: Address of IEs to search
+ * @ies_len: Pointer of length of ies, will update to new length
+ * @offset: The offset to start search
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ *
+ * Returns: _SUCCESS: ies is updated, _FAIL: not updated
+ */
+int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
+{
+	int ret = _FAIL;
+	u8 *target_ie;
+	u32 target_ielen;
+	u8 *start;
+	uint search_len;
+
+	if (!ies || !ies_len || *ies_len <= offset)
+		goto exit;
+
+	start = ies + offset;
+	search_len = *ies_len - offset;
+
+	while (1) {
+		target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
+		if (target_ie && target_ielen) {
+			u8 *remain_ies = target_ie + target_ielen;
+			uint remain_len = search_len - (remain_ies - start);
+
+			memcpy(target_ie, remain_ies, remain_len);
+			*ies_len = *ies_len - target_ielen;
+			ret = _SUCCESS;
+
+			start = target_ie;
+			search_len = remain_len;
+		} else {
+			break;
+		}
+	}
+exit:
+	return ret;
+}
+
+void rtw_set_supported_rate(u8 *supported_rates, uint mode)
+{
+	memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	switch (mode) {
+	case WIRELESS_11B:
+		memcpy(supported_rates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		break;
+
+	case WIRELESS_11G:
+		memcpy(supported_rates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11_24N:
+	case WIRELESS_11BG_24N:
+		memcpy(supported_rates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		memcpy(supported_rates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+	}
+}
+
+uint rtw_get_rateset_len(u8 *rateset)
+{
+	uint i;
+
+	for (i = 0; i < 13; i++)
+		if (rateset[i] == 0)
+			break;
+	return i;
+}
+
+int rtw_generate_ie(struct registry_priv *pregistrypriv)
+{
+	u8 wireless_mode;
+	int	sz = 0, rateLen;
+	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+	u8 *ie = pdev_network->ies;
+
+	/* timestamp will be inserted by hardware */
+	sz += 8;
+	ie += sz;
+
+	/* beacon interval : 2bytes */
+	*(__le16 *)ie = cpu_to_le16((u16)pdev_network->configuration.beacon_period);/* BCN_INTERVAL; */
+	sz += 2;
+	ie += 2;
+
+	/* capability info */
+	*(u16 *)ie = 0;
+
+	*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_IBSS);
+
+	if (pregistrypriv->preamble == PREAMBLE_SHORT)
+		*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
+
+	if (pdev_network->privacy)
+		*(__le16 *)ie |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	sz += 2;
+	ie += 2;
+
+	/* SSID */
+	ie = rtw_set_ie(ie, WLAN_EID_SSID, pdev_network->ssid.ssid_length, pdev_network->ssid.ssid, &sz);
+
+	/* supported rates */
+	wireless_mode = pregistrypriv->wireless_mode;
+
+	rtw_set_supported_rate(pdev_network->supported_rates, wireless_mode);
+
+	rateLen = rtw_get_rateset_len(pdev_network->supported_rates);
+
+	if (rateLen > 8) {
+		ie = rtw_set_ie(ie, WLAN_EID_SUPP_RATES, 8, pdev_network->supported_rates, &sz);
+		/* ie = rtw_set_ie(ie, WLAN_EID_EXT_SUPP_RATES, (rateLen - 8), (pdev_network->supported_rates + 8), &sz); */
+	} else {
+		ie = rtw_set_ie(ie, WLAN_EID_SUPP_RATES, rateLen, pdev_network->supported_rates, &sz);
+	}
+
+	/* DS parameter set */
+	ie = rtw_set_ie(ie, WLAN_EID_DS_PARAMS, 1, (u8 *)&(pdev_network->configuration.ds_config), &sz);
+
+	/* IBSS Parameter Set */
+
+	ie = rtw_set_ie(ie, WLAN_EID_IBSS_PARAMS, 2, (u8 *)&(pdev_network->configuration.atim_window), &sz);
+
+	if (rateLen > 8)
+		ie = rtw_set_ie(ie, WLAN_EID_EXT_SUPP_RATES, (rateLen - 8), (pdev_network->supported_rates + 8), &sz);
+
+	/* HT Cap. */
+	if ((pregistrypriv->wireless_mode & WIRELESS_11_24N) &&
+	    (pregistrypriv->ht_enable == true)) {
+		/* todo: */
+	}
+
+	/* pdev_network->ie_length =  sz; update ie_length */
+
+	/* return _SUCCESS; */
+
+	return sz;
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+	int len;
+	u16 val16;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *pbuf = pie;
+	int limit_new = limit;
+	__le16 le_tmp;
+
+	while (1) {
+		pbuf = rtw_get_ie(pbuf, WLAN_EID_VENDOR_SPECIFIC, &len, limit_new);
+
+		if (pbuf) {
+			/* check if oui matches... */
+			if (memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)))
+				goto check_next_ie;
+
+			/* check version... */
+			memcpy((u8 *)&le_tmp, (pbuf + 6), sizeof(val16));
+
+			val16 = le16_to_cpu(le_tmp);
+			if (val16 != 0x0001)
+				goto check_next_ie;
+
+			*wpa_ie_len = *(pbuf + 1);
+
+			return pbuf;
+
+		} else {
+			*wpa_ie_len = 0;
+			return NULL;
+		}
+
+check_next_ie:
+
+		limit_new = limit - (pbuf - pie) - 2 - len;
+
+		if (limit_new <= 0)
+			break;
+
+		pbuf += (2 + len);
+	}
+
+	*wpa_ie_len = 0;
+
+	return NULL;
+}
+
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+{
+	return rtw_get_ie(pie, WLAN_EID_RSN, rsn_ie_len, limit);
+}
+
+int rtw_get_wpa_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_get_wpa2_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+
+	if (wpa_ie_len <= 0) {
+		/* No WPA IE - fail silently */
+		return _FAIL;
+	}
+
+	if ((*wpa_ie != WLAN_EID_VENDOR_SPECIFIC) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
+	   (memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN))) {
+		return _FAIL;
+	}
+
+	pos = wpa_ie;
+
+	pos += 8;
+	left = wpa_ie_len - 8;
+
+	/* group_cipher */
+	if (left >= WPA_SELECTOR_LEN) {
+		*group_cipher = rtw_get_wpa_cipher_suite(pos);
+
+		pos += WPA_SELECTOR_LEN;
+		left -= WPA_SELECTOR_LEN;
+
+	} else if (left > 0)
+		return _FAIL;
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		/* count = le16_to_cpu(*(u16*)pos); */
+		count = get_unaligned_le16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * WPA_SELECTOR_LEN)
+			return _FAIL;
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+
+	} else if (left == 1)
+		return _FAIL;
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4))
+				*is_8021x = 1;
+		}
+	}
+
+	return ret;
+}
+
+int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return _FAIL;
+	}
+
+	if ((*rsn_ie != WLAN_EID_RSN) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
+		return _FAIL;
+
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;
+
+	/* group_cipher */
+	if (left >= RSN_SELECTOR_LEN) {
+		*group_cipher = rtw_get_wpa2_cipher_suite(pos);
+
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+
+	} else if (left > 0)
+		return _FAIL;
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+	  /* count = le16_to_cpu(*(u16*)pos); */
+		count = get_unaligned_le16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN)
+			return _FAIL;
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+
+	} else if (left == 1)
+		return _FAIL;
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4))
+				*is_8021x = 1;
+		}
+	}
+
+	return ret;
+}
+
+/* ifdef CONFIG_WAPI_SUPPORT */
+int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
+{
+	int len = 0;
+	u8 authmode;
+	uint	cnt;
+	u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01};
+	u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02};
+
+	if (wapi_len)
+		*wapi_len = 0;
+
+	if (!in_ie || in_len <= 0)
+		return len;
+
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		/* if (authmode == WLAN_EID_BSS_AC_ACCESS_DELAY) */
+		if (authmode == WLAN_EID_BSS_AC_ACCESS_DELAY && (!memcmp(&in_ie[cnt+6], wapi_oui1, 4) ||
+					!memcmp(&in_ie[cnt+6], wapi_oui2, 4))) {
+			if (wapi_ie)
+				memcpy(wapi_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+			if (wapi_len)
+				*wapi_len = in_ie[cnt+1]+2;
+
+			cnt += in_ie[cnt+1]+2;  /* get next */
+		} else {
+			cnt += in_ie[cnt+1]+2;   /* get next */
+		}
+	}
+
+	if (wapi_len)
+		len = *wapi_len;
+
+	return len;
+}
+/* endif */
+
+void rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
+{
+	u8 authmode;
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint	cnt;
+
+	/* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
+
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		if ((authmode == WLAN_EID_VENDOR_SPECIFIC) && (!memcmp(&in_ie[cnt+2], &wpa_oui[0], 4))) {
+			if (wpa_ie)
+				memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+			*wpa_len = in_ie[cnt + 1] + 2;
+			cnt += in_ie[cnt + 1] + 2;  /* get next */
+		} else {
+			if (authmode == WLAN_EID_RSN) {
+				if (rsn_ie)
+					memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+				*rsn_len = in_ie[cnt+1]+2;
+				cnt += in_ie[cnt+1]+2;  /* get next */
+			} else {
+				cnt += in_ie[cnt+1]+2;   /* get next */
+			}
+		}
+	}
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	uint cnt;
+	u8 *wpsie_ptr = NULL;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (wps_ielen)
+		*wps_ielen = 0;
+
+	if (!in_ie || in_len <= 0)
+		return wpsie_ptr;
+
+	cnt = 0;
+
+	while (cnt < in_len) {
+		eid = in_ie[cnt];
+
+		if ((eid == WLAN_EID_VENDOR_SPECIFIC) && (!memcmp(&in_ie[cnt+2], wps_oui, 4))) {
+			wpsie_ptr = &in_ie[cnt];
+
+			if (wps_ie)
+				memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+			if (wps_ielen)
+				*wps_ielen = in_ie[cnt+1]+2;
+
+			cnt += in_ie[cnt+1]+2;
+
+			break;
+		}
+		cnt += in_ie[cnt+1]+2; /* goto next */
+	}
+
+	return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if ((wps_ie[0] != WLAN_EID_VENDOR_SPECIFIC) ||
+		(memcmp(wps_ie + 2, wps_oui, 4))) {
+		return attr_ptr;
+	}
+
+	/*  6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
+	attr_ptr = wps_ie + 6; /* goto first attr */
+
+	while (attr_ptr - wps_ie < wps_ielen) {
+		/*  4 = 2(Attribute ID) + 2(Length) */
+		u16 attr_id = get_unaligned_be16(attr_ptr);
+		u16 attr_data_len = get_unaligned_be16(attr_ptr + 2);
+		u16 attr_len = attr_data_len + 4;
+
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+
+			if (len_attr)
+				*len_attr = attr_len;
+
+			break;
+		}
+		attr_ptr += attr_len; /* goto next */
+	}
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
+ *
+ * Returns: the address of the specific WPS attribute content found, or NULL
+ */
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr+4, attr_len-4);
+
+		if (len_content)
+			*len_content = attr_len-4;
+
+		return attr_ptr+4;
+	}
+
+	return NULL;
+}
+
+static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
+					    struct rtw_ieee802_11_elems *elems,
+					    int show_errors)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4)
+		return -1;
+
+	oui = get_unaligned_be24(pos);
+	switch (oui) {
+	case OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			elems->wpa_ie = pos;
+			elems->wpa_ie_len = elen;
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5)
+				return -1;
+
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				elems->wme = pos;
+				elems->wme_len = elen;
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				elems->wme_tspec = pos;
+				elems->wme_tspec_len = elen;
+				break;
+			default:
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			return -1;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			elems->vendor_ht_cap = pos;
+			elems->vendor_ht_cap_len = elen;
+			break;
+		default:
+			return -1;
+		}
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+/**
+ * rtw_ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+enum ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				struct rtw_ieee802_11_elems *elems,
+				int show_errors)
+{
+	uint left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left)
+			return ParseFailed;
+
+		switch (id) {
+		case WLAN_EID_SSID:
+			elems->ssid = pos;
+			elems->ssid_len = elen;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			elems->supp_rates = pos;
+			elems->supp_rates_len = elen;
+			break;
+		case WLAN_EID_FH_PARAMS:
+			elems->fh_params = pos;
+			elems->fh_params_len = elen;
+			break;
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_CF_PARAMS:
+			elems->cf_params = pos;
+			elems->cf_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_CHALLENGE:
+			elems->challenge = pos;
+			elems->challenge_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_EXT_SUPP_RATES:
+			elems->ext_supp_rates = pos;
+			elems->ext_supp_rates_len = elen;
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
+							     elems,
+							     show_errors))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+			elems->rsn_ie = pos;
+			elems->rsn_ie_len = elen;
+			break;
+		case WLAN_EID_PWR_CAPABILITY:
+			elems->power_cap = pos;
+			elems->power_cap_len = elen;
+			break;
+		case WLAN_EID_SUPPORTED_CHANNELS:
+			elems->supp_channels = pos;
+			elems->supp_channels_len = elen;
+			break;
+		case WLAN_EID_MOBILITY_DOMAIN:
+			elems->mdie = pos;
+			elems->mdie_len = elen;
+			break;
+		case WLAN_EID_FAST_BSS_TRANSITION:
+			elems->ftie = pos;
+			elems->ftie_len = elen;
+			break;
+		case WLAN_EID_TIMEOUT_INTERVAL:
+			elems->timeout_int = pos;
+			elems->timeout_int_len = elen;
+			break;
+		case WLAN_EID_HT_CAPABILITY:
+			elems->ht_capabilities = pos;
+			elems->ht_capabilities_len = elen;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			elems->ht_operation = pos;
+			elems->ht_operation_len = elen;
+			break;
+		case WLAN_EID_VHT_CAPABILITY:
+			elems->vht_capabilities = pos;
+			elems->vht_capabilities_len = elen;
+			break;
+		case WLAN_EID_VHT_OPERATION:
+			elems->vht_operation = pos;
+			elems->vht_operation_len = elen;
+			break;
+		case WLAN_EID_OPMODE_NOTIF:
+			elems->vht_op_mode_notify = pos;
+			elems->vht_op_mode_notify_len = elen;
+			break;
+		default:
+			unknown++;
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return ParseFailed;
+
+	return unknown ? ParseUnknown : ParseOK;
+}
+
+void rtw_macaddr_cfg(struct device *dev, u8 *mac_addr)
+{
+	u8 mac[ETH_ALEN];
+	struct device_node *np = dev->of_node;
+	const unsigned char *addr;
+	int len;
+
+	if (!mac_addr)
+		return;
+
+	if (rtw_initmac && mac_pton(rtw_initmac, mac)) {
+		/* Users specify the mac address */
+		ether_addr_copy(mac_addr, mac);
+	} else {
+		/* Use the mac address stored in the Efuse */
+		ether_addr_copy(mac, mac_addr);
+	}
+
+	if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac)) {
+		addr = of_get_property(np, "local-mac-address", &len);
+
+		if (addr && len == ETH_ALEN) {
+			ether_addr_copy(mac_addr, addr);
+		} else {
+			eth_random_addr(mac_addr);
+		}
+	}
+}
+
+static int rtw_get_cipher_info(struct wlan_network *pnetwork)
+{
+	u32 wpa_ielen;
+	unsigned char *pbuf;
+	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
+	int ret = _FAIL;
+
+	pbuf = rtw_get_wpa_ie(&pnetwork->network.ies[12], &wpa_ielen, pnetwork->network.ie_length-12);
+
+	if (pbuf && (wpa_ielen > 0)) {
+		if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+			pnetwork->bcn_info.pairwise_cipher = pairwise_cipher;
+			pnetwork->bcn_info.group_cipher = group_cipher;
+			pnetwork->bcn_info.is_8021x = is8021x;
+			ret = _SUCCESS;
+		}
+	} else {
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.ies[12], &wpa_ielen, pnetwork->network.ie_length-12);
+
+		if (pbuf && (wpa_ielen > 0)) {
+			if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+				pnetwork->bcn_info.pairwise_cipher = pairwise_cipher;
+				pnetwork->bcn_info.group_cipher = group_cipher;
+				pnetwork->bcn_info.is_8021x = is8021x;
+				ret = _SUCCESS;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+	unsigned short cap = 0;
+	u8 bencrypt = 0;
+	/* u8 wpa_ie[255], rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct ieee80211_ht_cap *pht_cap = NULL;
+	unsigned int		len;
+	unsigned char 	*p;
+	__le16 le_cap;
+
+	memcpy((u8 *)&le_cap, rtw_get_capability_from_ie(pnetwork->network.ies), 2);
+	cap = le16_to_cpu(le_cap);
+	if (cap & WLAN_CAPABILITY_PRIVACY) {
+		bencrypt = 1;
+		pnetwork->network.privacy = 1;
+	} else {
+		pnetwork->bcn_info.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+	}
+	rtw_get_sec_ie(pnetwork->network.ies, pnetwork->network.ie_length, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0) {
+		pnetwork->bcn_info.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		pnetwork->bcn_info.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bencrypt)
+			pnetwork->bcn_info.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+	rtw_get_cipher_info(pnetwork);
+
+	/* get bwmode and ch_offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(pnetwork->network.ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_CAPABILITY, &len, pnetwork->network.ie_length - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+			pnetwork->bcn_info.ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else {
+			pnetwork->bcn_info.ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(pnetwork->network.ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_OPERATION, &len, pnetwork->network.ie_length - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			pnetwork->bcn_info.ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			pnetwork->bcn_info.ht_info_infos_0 = 0;
+	}
+}
+
+/* show MCS rate, unit: 100Kbps */
+u16 rtw_mcs_rate(u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate)
+{
+	u16 max_rate = 0;
+
+	if (MCS_rate[0] & BIT(7))
+		max_rate = (bw_40MHz) ? ((short_GI)?1500:1350):((short_GI)?722:650);
+	else if (MCS_rate[0] & BIT(6))
+		max_rate = (bw_40MHz) ? ((short_GI)?1350:1215):((short_GI)?650:585);
+	else if (MCS_rate[0] & BIT(5))
+		max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520);
+	else if (MCS_rate[0] & BIT(4))
+		max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390);
+	else if (MCS_rate[0] & BIT(3))
+		max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260);
+	else if (MCS_rate[0] & BIT(2))
+		max_rate = (bw_40MHz) ? ((short_GI)?450:405):((short_GI)?217:195);
+	else if (MCS_rate[0] & BIT(1))
+		max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
+	else if (MCS_rate[0] & BIT(0))
+		max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65);
+
+	return max_rate;
+}
+
+int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
+{
+	const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr);
+	u16 fc;
+	u8 c;
+	u8 a = ACT_PUBLIC_MAX;
+
+	fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control);
+
+	if ((fc & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE))
+		!= (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)
+	) {
+		return false;
+	}
+
+	c = frame_body[0];
+
+	switch (c) {
+	case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
+		break;
+	default:
+		a = frame_body[1];
+	}
+
+	if (category)
+		*category = c;
+	if (action)
+		*action = a;
+
+	return true;
+}
+
+static const char *_action_public_str[] = {
+	"ACT_PUB_BSSCOEXIST",
+	"ACT_PUB_DSE_ENABLE",
+	"ACT_PUB_DSE_DEENABLE",
+	"ACT_PUB_DSE_REG_LOCATION",
+	"ACT_PUB_EXT_CHL_SWITCH",
+	"ACT_PUB_DSE_MSR_REQ",
+	"ACT_PUB_DSE_MSR_RPRT",
+	"ACT_PUB_MP",
+	"ACT_PUB_DSE_PWR_CONSTRAINT",
+	"ACT_PUB_VENDOR",
+	"ACT_PUB_GAS_INITIAL_REQ",
+	"ACT_PUB_GAS_INITIAL_RSP",
+	"ACT_PUB_GAS_COMEBACK_REQ",
+	"ACT_PUB_GAS_COMEBACK_RSP",
+	"ACT_PUB_TDLS_DISCOVERY_RSP",
+	"ACT_PUB_LOCATION_TRACK",
+	"ACT_PUB_RSVD",
+};
+
+const char *action_public_str(u8 action)
+{
+	action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
+	return _action_public_str[action];
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_io.c b/drivers/staging/rtl8723bs/core/rtw_io.c
new file mode 100644
index 000000000..4d3c30ec9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_io.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*
+
+The purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+
+Compiler Flag Option:
+
+1. CONFIG_SDIO_HCI:
+    a. USE_SYNC_IRP:  Only sync operations are provided.
+    b. USE_ASYNC_IRP:Both sync/async operations are provided.
+
+jackson@realtek.com.tw
+
+*/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+u8 rtw_read8(struct adapter *adapter, u32 addr)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read8 = pintfhdl->io_ops._read8;
+
+	return _read8(pintfhdl, addr);
+}
+
+u16 rtw_read16(struct adapter *adapter, u32 addr)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read16 = pintfhdl->io_ops._read16;
+
+	return _read16(pintfhdl, addr);
+}
+
+u32 rtw_read32(struct adapter *adapter, u32 addr)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read32 = pintfhdl->io_ops._read32;
+
+	return _read32(pintfhdl, addr);
+
+}
+
+int rtw_write8(struct adapter *adapter, u32 addr, u8 val)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+
+	_write8 = pintfhdl->io_ops._write8;
+
+	ret = _write8(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int rtw_write16(struct adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+
+	_write16 = pintfhdl->io_ops._write16;
+
+	ret = _write16(pintfhdl, addr, val);
+	return RTW_STATUS_CODE(ret);
+}
+int rtw_write32(struct adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+
+	_write32 = pintfhdl->io_ops._write32;
+
+	ret = _write32(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+u32 rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+	_write_port = pintfhdl->io_ops._write_port;
+
+	return _write_port(pintfhdl, addr, cnt, pmem);
+}
+
+int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct adapter *padapter, struct _io_ops *pops))
+{
+	struct io_priv *piopriv = &padapter->iopriv;
+	struct intf_hdl *pintf = &piopriv->intf;
+
+	if (!set_intf_ops)
+		return _FAIL;
+
+	piopriv->padapter = padapter;
+	pintf->padapter = padapter;
+	pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+	set_intf_ops(padapter, &pintf->io_ops);
+
+	return _SUCCESS;
+}
+
+/*
+* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR
+* @return true:
+* @return false:
+*/
+int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj)
+{
+	int ret = false;
+	int value = atomic_inc_return(&dvobj->continual_io_error);
+	if (value > MAX_CONTINUAL_IO_ERR)
+		ret = true;
+
+	return ret;
+}
+
+/*
+* Set the continual_io_error of this @param dvobjprive to 0
+*/
+void rtw_reset_continual_io_error(struct dvobj_priv *dvobj)
+{
+	atomic_set(&dvobj->continual_io_error, 0);
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c b/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c
new file mode 100644
index 000000000..8c11daff2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c
@@ -0,0 +1,505 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+u8 rtw_validate_bssid(u8 *bssid)
+{
+	u8 ret = true;
+
+	if (is_zero_mac_addr(bssid)
+		|| is_broadcast_mac_addr(bssid)
+		|| is_multicast_mac_addr(bssid)
+	) {
+		ret = false;
+	}
+
+	return ret;
+}
+
+u8 rtw_validate_ssid(struct ndis_802_11_ssid *ssid)
+{
+	u8 ret = true;
+
+	if (ssid->ssid_length > 32) {
+		ret = false;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+u8 rtw_do_join(struct adapter *padapter)
+{
+	struct list_head	*plist, *phead;
+	u8 *pibss = NULL;
+	struct	mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	u8 ret = _SUCCESS;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	pmlmepriv->cur_network.join_res = -2;
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	pmlmepriv->pscanned = plist;
+
+	pmlmepriv->to_join = true;
+
+	if (list_empty(&queue->queue)) {
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		/* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */
+		/* we try to issue sitesurvey firstly */
+
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false
+			|| rtw_to_roam(padapter) > 0
+		) {
+			/*  submit site_survey_cmd */
+			ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+			if (ret != _SUCCESS)
+				pmlmepriv->to_join = false;
+
+		} else {
+			pmlmepriv->to_join = false;
+			ret = _FAIL;
+		}
+
+		goto exit;
+	} else {
+		int select_ret;
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+		if (select_ret == _SUCCESS) {
+			pmlmepriv->to_join = false;
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+		} else {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+				/*  submit createbss_cmd to change to a ADHOC_MASTER */
+
+				/* pmlmepriv->lock has been acquired by caller... */
+				struct wlan_bssid_ex    *pdev_network = &(padapter->registrypriv.dev_network);
+
+				pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+				pibss = padapter->registrypriv.dev_network.mac_address;
+
+				memcpy(&pdev_network->ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+				rtw_update_registrypriv_dev_network(padapter);
+
+				rtw_generate_random_ibss(pibss);
+
+				if (rtw_createbss_cmd(padapter) != _SUCCESS) {
+					ret =  false;
+					goto exit;
+				}
+
+				pmlmepriv->to_join = false;
+
+			} else {
+				/*  can't associate ; reset under-linking */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+				/* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */
+				/* we try to issue sitesurvey firstly */
+				if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false
+					|| rtw_to_roam(padapter) > 0
+				) {
+					ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+					if (ret != _SUCCESS)
+						pmlmepriv->to_join = false;
+
+				} else {
+					ret = _FAIL;
+					pmlmepriv->to_join = false;
+				}
+			}
+
+		}
+
+	}
+
+exit:
+	return ret;
+}
+
+u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid)
+{
+	u8 status = _SUCCESS;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+	netdev_dbg(padapter->pnetdev, "set ssid [%s] fw_state = 0x%08x\n",
+		   ssid->ssid, get_fwstate(pmlmepriv));
+
+	if (padapter->hw_init_completed == false) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)
+		goto release_mlme_lock;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		if ((pmlmepriv->assoc_ssid.ssid_length == ssid->ssid_length) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.ssid, ssid->ssid, ssid->ssid_length))) {
+			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) {
+				if (rtw_is_same_ibss(padapter, pnetwork) == false) {
+					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
+					rtw_disassoc_cmd(padapter, 0, true);
+
+					if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+						rtw_indicate_disconnect(padapter);
+
+					rtw_free_assoc_resources(padapter, 1);
+
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+					}
+				} else {
+					goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+				}
+			} else {
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+			}
+		} else {
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	if (rtw_validate_ssid(ssid) == false) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
+	pmlmepriv->assoc_by_bssid = false;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+
+	return status;
+}
+
+u8 rtw_set_802_11_connect(struct adapter *padapter, u8 *bssid, struct ndis_802_11_ssid *ssid)
+{
+	u8 status = _SUCCESS;
+	bool bssid_valid = true;
+	bool ssid_valid = true;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (!ssid || rtw_validate_ssid(ssid) == false)
+		ssid_valid = false;
+
+	if (!bssid || rtw_validate_bssid(bssid) == false)
+		bssid_valid = false;
+
+	if (!ssid_valid && !bssid_valid) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	if (padapter->hw_init_completed == false) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	netdev_dbg(padapter->pnetdev, FUNC_ADPT_FMT "  fw_state = 0x%08x\n",
+		   FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)
+		goto release_mlme_lock;
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	if (ssid && ssid_valid)
+		memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
+	else
+		memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+
+	if (bssid && bssid_valid) {
+		memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+		pmlmepriv->assoc_by_bssid = true;
+	} else {
+		pmlmepriv->assoc_by_bssid = false;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	return status;
+}
+
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter,
+	enum ndis_802_11_network_infrastructure networktype)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
+	enum ndis_802_11_network_infrastructure *pold_state = &(cur_network->network.infrastructure_mode);
+
+	if (*pold_state != networktype) {
+		if (*pold_state == Ndis802_11APMode) {
+			/* change to other mode from Ndis802_11APMode */
+			cur_network->join_res = -1;
+
+			stop_ap_mode(padapter);
+		}
+
+		spin_lock_bh(&pmlmepriv->lock);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) || (*pold_state == Ndis802_11IBSS))
+			rtw_disassoc_cmd(padapter, 0, true);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||
+			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true))
+			rtw_free_assoc_resources(padapter, 1);
+
+		if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+				rtw_indicate_disconnect(padapter); /* will clr Linked_state; before this function, we must have checked whether issue dis-assoc_cmd or not */
+	       }
+
+		*pold_state = networktype;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (networktype) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case Ndis802_11APMode:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+			start_ap_mode(padapter);
+			/* rtw_indicate_connect(padapter); */
+
+			break;
+
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+	return true;
+}
+
+
+u8 rtw_set_802_11_disassociate(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		/* modify for CONFIG_IEEE80211W, none 11w can use it */
+		rtw_free_assoc_resources_cmd(padapter);
+		rtw_pwr_wakeup(padapter);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	return true;
+}
+
+u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num)
+{
+	struct	mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	u8 res = true;
+
+	if (!padapter) {
+		res = false;
+		goto exit;
+	}
+	if (padapter->hw_init_completed == false) {
+		res = false;
+		goto exit;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) ||
+		(pmlmepriv->LinkDetectInfo.bBusyTraffic == true)) {
+		/*  Scan or linking is in progress, do nothing. */
+		res = true;
+
+	} else {
+		if (rtw_is_scan_deny(padapter))
+			return _SUCCESS;
+
+		spin_lock_bh(&pmlmepriv->lock);
+
+		res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, NULL, 0);
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+exit:
+
+	return res;
+}
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *padapter, enum ndis_802_11_authentication_mode authmode)
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	int res;
+	u8 ret;
+
+	psecuritypriv->ndisauthtype = authmode;
+
+	if (psecuritypriv->ndisauthtype > 3)
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+	res = rtw_set_auth(padapter, psecuritypriv);
+
+	if (res == _SUCCESS)
+		ret = true;
+	else
+		ret = false;
+
+	return ret;
+}
+
+u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct ndis_802_11_wep *wep)
+{
+
+	signed int		keyid, res;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	u8 ret = _SUCCESS;
+
+	keyid = wep->key_index & 0x3fffffff;
+
+	if (keyid >= 4) {
+		ret = false;
+		goto exit;
+	}
+
+	switch (wep->key_length) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		break;
+	}
+
+	memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->key_material), wep->key_length);
+
+	psecuritypriv->dot11DefKeylen[keyid] = wep->key_length;
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	res = rtw_set_key(padapter, psecuritypriv, keyid, 1, true);
+
+	if (res == _FAIL)
+		ret = false;
+exit:
+
+	return ret;
+}
+
+/*
+* rtw_get_cur_max_rate -
+* @adapter: pointer to struct adapter structure
+*
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(struct adapter *adapter)
+{
+	int	i = 0;
+	u16 rate = 0, max_rate = 0;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_bssid_ex	*pcur_bss = &pmlmepriv->cur_network.network;
+	struct sta_info *psta = NULL;
+	u8 short_GI = 0;
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) != true)
+		&& (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) != true))
+		return 0;
+
+	psta = rtw_get_stainfo(&adapter->stapriv, get_bssid(pmlmepriv));
+	if (!psta)
+		return 0;
+
+	short_GI = query_ra_short_GI(psta);
+
+	if (is_supported_ht(psta->wireless_mode)) {
+		max_rate = rtw_mcs_rate(psta->bw_mode == CHANNEL_WIDTH_40 ? 1 : 0,
+					short_GI,
+					psta->htpriv.ht_cap.mcs.rx_mask);
+	} else {
+		while ((pcur_bss->supported_rates[i] != 0) && (pcur_bss->supported_rates[i] != 0xFF)) {
+			rate = pcur_bss->supported_rates[i]&0x7F;
+			if (rate > max_rate)
+				max_rate = rate;
+			i++;
+		}
+
+		max_rate = max_rate*10/2;
+	}
+
+	return max_rate;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme.c b/drivers/staging/rtl8723bs/core/rtw_mlme.c
new file mode 100644
index 000000000..8159bb651
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c
@@ -0,0 +1,2628 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <linux/etherdevice.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <linux/jiffies.h>
+
+int	rtw_init_mlme_priv(struct adapter *padapter)
+{
+	int	i;
+	u8 *pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	int	res = _SUCCESS;
+
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+
+	pmlmepriv->pscanned = NULL;
+	pmlmepriv->fw_state = WIFI_STATION_STATE; /*  Must sync with rtw_wdev_alloc() */
+	/*  wdev->iftype = NL80211_IFTYPE_STATION */
+	pmlmepriv->cur_network.network.infrastructure_mode = Ndis802_11AutoUnknown;
+	pmlmepriv->scan_mode = SCAN_ACTIVE;/*  1: active, 0: passive. Maybe someday we should rename this varable to "active_mode" (Jeff) */
+
+	spin_lock_init(&pmlmepriv->lock);
+	INIT_LIST_HEAD(&pmlmepriv->free_bss_pool.queue);
+	spin_lock_init(&pmlmepriv->free_bss_pool.lock);
+	INIT_LIST_HEAD(&pmlmepriv->scanned_queue.queue);
+	spin_lock_init(&pmlmepriv->scanned_queue.lock);
+
+	set_scanned_network_val(pmlmepriv, 0);
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+
+	pbuf = vzalloc(array_size(MAX_BSS_CNT, sizeof(struct wlan_network)));
+
+	if (!pbuf) {
+		res = _FAIL;
+		goto exit;
+	}
+	pmlmepriv->free_bss_buf = pbuf;
+
+	pnetwork = (struct wlan_network *)pbuf;
+
+	for (i = 0; i < MAX_BSS_CNT; i++) {
+		INIT_LIST_HEAD(&pnetwork->list);
+
+		list_add_tail(&pnetwork->list, &pmlmepriv->free_bss_pool.queue);
+
+		pnetwork++;
+	}
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+	rtw_clear_scan_deny(padapter);
+
+	#define RTW_ROAM_SCAN_RESULT_EXP_MS 5000
+	#define RTW_ROAM_RSSI_DIFF_TH 10
+	#define RTW_ROAM_SCAN_INTERVAL_MS 10000
+
+	pmlmepriv->roam_flags = 0
+		| RTW_ROAM_ON_EXPIRED
+		| RTW_ROAM_ON_RESUME
+		;
+
+	pmlmepriv->roam_scanr_exp_ms = RTW_ROAM_SCAN_RESULT_EXP_MS;
+	pmlmepriv->roam_rssi_diff_th = RTW_ROAM_RSSI_DIFF_TH;
+	pmlmepriv->roam_scan_int_ms = RTW_ROAM_SCAN_INTERVAL_MS;
+
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+	return res;
+}
+
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	if (*ppie) {
+		kfree(*ppie);
+		*plen = 0;
+		*ppie = NULL;
+	}
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+	rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+	rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+}
+
+void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	if (pmlmepriv) {
+		rtw_free_mlme_priv_ie_data(pmlmepriv);
+		vfree(pmlmepriv->free_bss_buf);
+	}
+}
+
+/*
+struct	wlan_network *_rtw_dequeue_network(struct __queue *queue)
+{
+	_irqL irqL;
+
+	struct wlan_network *pnetwork;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->queue))
+
+		pnetwork = NULL;
+
+	else
+	{
+		pnetwork = container_of(get_next(&queue->queue), struct wlan_network, list);
+
+		list_del_init(&(pnetwork->list));
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	return pnetwork;
+}
+*/
+
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv)
+{
+	struct	wlan_network	*pnetwork;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct list_head *plist = NULL;
+
+	spin_lock_bh(&free_queue->lock);
+
+	if (list_empty(&free_queue->queue)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+	plist = get_next(&(free_queue->queue));
+
+	pnetwork = container_of(plist, struct wlan_network, list);
+
+	list_del_init(&pnetwork->list);
+
+	pnetwork->network_type = 0;
+	pnetwork->fixed = false;
+	pnetwork->last_scanned = jiffies;
+	pnetwork->aid = 0;
+	pnetwork->join_res = 0;
+
+	pmlmepriv->num_of_scanned++;
+
+exit:
+	spin_unlock_bh(&free_queue->lock);
+
+	return pnetwork;
+}
+
+void _rtw_free_network(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall)
+{
+	unsigned int delta_time;
+	u32 lifetime = SCANQUEUE_LIFETIME;
+/* 	_irqL irqL; */
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+	if (!pnetwork)
+		return;
+
+	if (pnetwork->fixed)
+		return;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+		lifetime = 1;
+
+	if (!isfreeall) {
+		delta_time = jiffies_to_msecs(jiffies - pnetwork->last_scanned);
+		if (delta_time < lifetime)/*  unit:msec */
+			return;
+	}
+
+	spin_lock_bh(&free_queue->lock);
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), &(free_queue->queue));
+
+	pmlmepriv->num_of_scanned--;
+
+	spin_unlock_bh(&free_queue->lock);
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+	if (!pnetwork)
+		return;
+
+	if (pnetwork->fixed)
+		return;
+
+	/* spin_lock_irqsave(&free_queue->lock, irqL); */
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), get_list_head(free_queue));
+
+	pmlmepriv->num_of_scanned--;
+
+	/* spin_unlock_irqrestore(&free_queue->lock, irqL); */
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be called under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+	struct list_head	*phead, *plist;
+	struct	wlan_network *pnetwork = NULL;
+	u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+	if (!memcmp(zero_addr, addr, ETH_ALEN)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+
+	/* spin_lock_bh(&scanned_queue->lock); */
+
+	phead = get_list_head(scanned_queue);
+	list_for_each(plist, phead) {
+		pnetwork = list_entry(plist, struct wlan_network, list);
+
+		if (!memcmp(addr, pnetwork->network.mac_address, ETH_ALEN))
+			break;
+	}
+
+	if (plist == phead)
+		pnetwork = NULL;
+
+	/* spin_unlock_bh(&scanned_queue->lock); */
+
+exit:
+	return pnetwork;
+}
+
+void rtw_free_network_queue(struct adapter *padapter, u8 isfreeall)
+{
+	struct list_head *phead, *plist, *tmp;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+	spin_lock_bh(&scanned_queue->lock);
+
+	phead = get_list_head(scanned_queue);
+	list_for_each_safe(plist, tmp, phead) {
+
+		pnetwork = list_entry(plist, struct wlan_network, list);
+
+		_rtw_free_network(pmlmepriv, pnetwork, isfreeall);
+
+	}
+
+	spin_unlock_bh(&scanned_queue->lock);
+}
+
+signed int rtw_if_up(struct adapter *padapter)
+{
+	signed int res;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		(check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false))
+		res = false;
+	else
+		res =  true;
+
+	return res;
+}
+
+void rtw_generate_random_ibss(u8 *pibss)
+{
+	unsigned long curtime = jiffies;
+
+	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+	pibss[3] = (u8)(curtime & 0xff) ;/* p[0]; */
+	pibss[4] = (u8)((curtime>>8) & 0xff) ;/* p[1]; */
+	pibss[5] = (u8)((curtime>>16) & 0xff) ;/* p[2]; */
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+	return ie + 8 + 2;
+}
+
+u16 rtw_get_capability(struct wlan_bssid_ex *bss)
+{
+	__le16	val;
+
+	memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->ies), 2);
+
+	return le16_to_cpu(val);
+}
+
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie)
+{
+	return ie + 8;
+}
+
+void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	_rtw_free_mlme_priv(pmlmepriv);
+}
+
+/*
+static struct	wlan_network *rtw_dequeue_network(struct __queue *queue)
+{
+	struct wlan_network *pnetwork;
+
+	pnetwork = _rtw_dequeue_network(queue);
+	return pnetwork;
+}
+*/
+
+void rtw_free_network_nolock(struct adapter *padapter, struct wlan_network *pnetwork);
+void rtw_free_network_nolock(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	_rtw_free_network_nolock(&(padapter->mlmepriv), pnetwork);
+	rtw_cfg80211_unlink_bss(padapter, pnetwork);
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be called under atomic context... to avoid possible racing condition...
+*/
+struct	wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret = true;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
+		    (pnetwork->network.privacy == 0))
+		ret = false;
+	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
+		 (pnetwork->network.privacy == 1))
+		ret = false;
+	else
+		ret = true;
+
+	return ret;
+
+}
+
+inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
+{
+	return (a->ssid.ssid_length == b->ssid.ssid_length)
+		&&  !memcmp(a->ssid.ssid, b->ssid.ssid, a->ssid.ssid_length);
+}
+
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst, u8 feature)
+{
+	u16 s_cap, d_cap;
+	__le16 tmps, tmpd;
+
+	if (rtw_bug_check(dst, src, &s_cap, &d_cap) == false)
+			return false;
+
+	memcpy((u8 *)&tmps, rtw_get_capability_from_ie(src->ies), 2);
+	memcpy((u8 *)&tmpd, rtw_get_capability_from_ie(dst->ies), 2);
+
+	s_cap = le16_to_cpu(tmps);
+	d_cap = le16_to_cpu(tmpd);
+
+	return (src->ssid.ssid_length == dst->ssid.ssid_length) &&
+		/* 	(src->configuration.ds_config == dst->configuration.ds_config) && */
+			((!memcmp(src->mac_address, dst->mac_address, ETH_ALEN))) &&
+			((!memcmp(src->ssid.ssid, dst->ssid.ssid, src->ssid.ssid_length))) &&
+			((s_cap & WLAN_CAPABILITY_IBSS) ==
+			(d_cap & WLAN_CAPABILITY_IBSS)) &&
+			((s_cap & WLAN_CAPABILITY_ESS) ==
+			(d_cap & WLAN_CAPABILITY_ESS));
+
+}
+
+struct wlan_network *_rtw_find_same_network(struct __queue *scanned_queue, struct wlan_network *network)
+{
+	struct list_head *phead, *plist;
+	struct wlan_network *found = NULL;
+
+	phead = get_list_head(scanned_queue);
+	list_for_each(plist, phead) {
+		found = list_entry(plist, struct wlan_network, list);
+
+		if (is_same_network(&network->network, &found->network, 0))
+			break;
+	}
+
+	if (plist == phead)
+		found = NULL;
+
+	return found;
+}
+
+struct	wlan_network	*rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
+{
+	struct list_head	*plist, *phead;
+
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+
+	phead = get_list_head(scanned_queue);
+
+	list_for_each(plist, phead) {
+
+		pwlan = list_entry(plist, struct wlan_network, list);
+
+		if (!pwlan->fixed) {
+			if (!oldest || time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+	}
+	return oldest;
+
+}
+
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+	struct adapter *padapter, bool update_ie)
+{
+	long rssi_ori = dst->rssi;
+
+	u8 sq_smp = src->phy_info.signal_quality;
+
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+	/* The rule below is 1/5 for sample value, 4/5 for history value */
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src, 0)) {
+		/* Take the recvpriv's value for the connected AP*/
+		ss_final = padapter->recvpriv.signal_strength;
+		sq_final = padapter->recvpriv.signal_qual;
+		/* the rssi value here is undecorated, and will be used for antenna diversity */
+		if (sq_smp != 101) /* from the right channel */
+			rssi_final = (src->rssi+dst->rssi*4)/5;
+		else
+			rssi_final = rssi_ori;
+	} else {
+		if (sq_smp != 101) { /* from the right channel */
+			ss_final = ((u32)(src->phy_info.signal_strength)+(u32)(dst->phy_info.signal_strength)*4)/5;
+			sq_final = ((u32)(src->phy_info.signal_quality)+(u32)(dst->phy_info.signal_quality)*4)/5;
+			rssi_final = (src->rssi+dst->rssi*4)/5;
+		} else {
+			/* bss info not receiving from the right channel, use the original RX signal infos */
+			ss_final = dst->phy_info.signal_strength;
+			sq_final = dst->phy_info.signal_quality;
+			rssi_final = dst->rssi;
+		}
+
+	}
+
+	if (update_ie) {
+		dst->reserved[0] = src->reserved[0];
+		dst->reserved[1] = src->reserved[1];
+		memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src));
+	}
+
+	dst->phy_info.signal_strength = ss_final;
+	dst->phy_info.signal_quality = sq_final;
+	dst->rssi = rssi_final;
+}
+
+static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	rtw_bug_check(&(pmlmepriv->cur_network.network),
+		&(pmlmepriv->cur_network.network),
+		&(pmlmepriv->cur_network.network),
+		&(pmlmepriv->cur_network.network));
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork, 0))) {
+		/* if (pmlmepriv->cur_network.network.ie_length<= pnetwork->ie_length) */
+		{
+			update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true);
+			rtw_update_protection(adapter, (pmlmepriv->cur_network.network.ies) + sizeof(struct ndis_802_11_fix_ie),
+									pmlmepriv->cur_network.network.ie_length);
+		}
+	}
+}
+
+/*
+Caller must hold pmlmepriv->lock first.
+*/
+void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
+{
+	struct list_head	*plist, *phead;
+	u32 bssid_ex_sz;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	struct wlan_network	*oldest = NULL;
+	int target_find = 0;
+	u8 feature = 0;
+
+	spin_lock_bh(&queue->lock);
+	phead = get_list_head(queue);
+	list_for_each(plist, phead) {
+		pnetwork = list_entry(plist, struct wlan_network, list);
+
+		rtw_bug_check(pnetwork, pnetwork, pnetwork, pnetwork);
+
+		if (is_same_network(&(pnetwork->network), target, feature)) {
+			target_find = 1;
+			break;
+		}
+
+		if (rtw_roam_flags(adapter)) {
+			/* TODO: don't select network in the same ess as oldest if it's new enough*/
+		}
+
+		if (!oldest || time_after(oldest->last_scanned, pnetwork->last_scanned))
+			oldest = pnetwork;
+
+	}
+
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information */
+	/* if (phead == plist) { */
+	if (!target_find) {
+		if (list_empty(&pmlmepriv->free_bss_pool.queue)) {
+			/* If there are no more slots, expire the oldest */
+			/* list_del_init(&oldest->list); */
+			pnetwork = oldest;
+			if (!pnetwork)
+				goto exit;
+
+			memcpy(&(pnetwork->network), target,  get_wlan_bssid_ex_sz(target));
+			/*  variable initialize */
+			pnetwork->fixed = false;
+			pnetwork->last_scanned = jiffies;
+
+			pnetwork->network_type = 0;
+			pnetwork->aid = 0;
+			pnetwork->join_res = 0;
+
+			/* bss info not receiving from the right channel */
+			if (pnetwork->network.phy_info.signal_quality == 101)
+				pnetwork->network.phy_info.signal_quality = 0;
+		} else {
+			/* Otherwise just pull from the free list */
+
+			pnetwork = rtw_alloc_network(pmlmepriv); /*  will update scan_time */
+
+			if (!pnetwork)
+				goto exit;
+
+			bssid_ex_sz = get_wlan_bssid_ex_sz(target);
+			target->length = bssid_ex_sz;
+			memcpy(&(pnetwork->network), target, bssid_ex_sz);
+
+			pnetwork->last_scanned = jiffies;
+
+			/* bss info not receiving from the right channel */
+			if (pnetwork->network.phy_info.signal_quality == 101)
+				pnetwork->network.phy_info.signal_quality = 0;
+
+			list_add_tail(&(pnetwork->list), &(queue->queue));
+
+		}
+	} else {
+		/* we have an entry and we are going to update it. But this entry may
+		 * be already expired. In this case we do the same as we found a new
+		 * net and call the new_net handler
+		 */
+		bool update_ie = true;
+
+		pnetwork->last_scanned = jiffies;
+
+		/* target.reserved[0]== 1, means that scanned network is a bcn frame. */
+		if (pnetwork->network.ie_length > target->ie_length && target->reserved[0] == 1)
+			update_ie = false;
+
+		/*  probe resp(3) > beacon(1) > probe req(2) */
+		if (target->reserved[0] != 2 &&
+		    target->reserved[0] >= pnetwork->network.reserved[0]) {
+			update_ie = true;
+		} else {
+			update_ie = false;
+		}
+
+		update_network(&(pnetwork->network), target, adapter, update_ie);
+	}
+
+exit:
+	spin_unlock_bh(&queue->lock);
+}
+
+void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork);
+void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+	/* struct __queue	*queue	= &(pmlmepriv->scanned_queue); */
+
+	/* spin_lock_bh(&queue->lock); */
+
+	update_current_network(adapter, pnetwork);
+
+	rtw_update_scanned_network(adapter, pnetwork);
+
+	/* spin_unlock_bh(&queue->lock); */
+}
+
+/* select the desired network based on the capability of the (i)bss. */
+/*  check items: (1) security */
+/* 			   (2) network_type */
+/* 			   (3) WMM */
+/* 			   (4) HT */
+/*                      (5) others */
+int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork);
+int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u32 desired_encmode;
+	u32 privacy;
+
+	/* u8 wps_ie[512]; */
+	uint wps_ielen;
+
+	int bselected = true;
+
+	desired_encmode = psecuritypriv->ndisencryptstatus;
+	privacy = pnetwork->network.privacy;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie(pnetwork->network.ies+_FIXED_IE_LENGTH_, pnetwork->network.ie_length-_FIXED_IE_LENGTH_, NULL, &wps_ielen))
+			return true;
+		else
+			return false;
+
+	}
+	if (adapter->registrypriv.wifi_spec == 1) { /* for  correct flow of 8021X  to do.... */
+		u8 *p = NULL;
+		uint ie_len = 0;
+
+		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+	    bselected = false;
+
+		if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) {
+			p = rtw_get_ie(pnetwork->network.ies + _BEACON_IE_OFFSET_, WLAN_EID_RSN, &ie_len, (pnetwork->network.ie_length - _BEACON_IE_OFFSET_));
+			if (p && ie_len > 0)
+				bselected = true;
+			else
+				bselected = false;
+		}
+	}
+
+	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0))
+		bselected = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+		if (pnetwork->network.infrastructure_mode != pmlmepriv->cur_network.network.infrastructure_mode)
+			bselected = false;
+	}
+
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void rtw_atimdone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+}
+
+void rtw_survey_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	u32 len;
+	struct wlan_bssid_ex *pnetwork;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	pnetwork = (struct wlan_bssid_ex *)pbuf;
+
+	len = get_wlan_bssid_ex_sz(pnetwork);
+	if (len > (sizeof(struct wlan_bssid_ex)))
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	/*  update IBSS_network 's timestamp */
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) {
+		if (!memcmp(&(pmlmepriv->cur_network.network.mac_address), pnetwork->mac_address, ETH_ALEN)) {
+			struct wlan_network *ibss_wlan = NULL;
+
+			memcpy(pmlmepriv->cur_network.network.ies, pnetwork->ies, 8);
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue,  pnetwork->mac_address);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.ies, pnetwork->ies, 8);
+				spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+				goto exit;
+			}
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		}
+	}
+
+	/*  lock pmlmepriv->lock when you accessing network_q */
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == false) {
+		if (pnetwork->ssid.ssid[0] == 0)
+			pnetwork->ssid.ssid_length = 0;
+		rtw_add_network(adapter, pnetwork);
+	}
+
+exit:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	spin_lock_bh(&pmlmepriv->lock);
+	if (pmlmepriv->wps_probe_req_ie) {
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		kfree(pmlmepriv->wps_probe_req_ie);
+		pmlmepriv->wps_probe_req_ie = NULL;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		spin_unlock_bh(&pmlmepriv->lock);
+		del_timer_sync(&pmlmepriv->scan_to_timer);
+		spin_lock_bh(&pmlmepriv->lock);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+	if (pmlmepriv->to_join) {
+		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+				if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) {
+					_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+				} else {
+					u8 ret = _SUCCESS;
+					struct wlan_bssid_ex    *pdev_network = &(adapter->registrypriv.dev_network);
+					u8 *pibss = adapter->registrypriv.dev_network.mac_address;
+
+					/* pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;because don't set assoc_timer */
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+					memcpy(&pdev_network->ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+					rtw_update_registrypriv_dev_network(adapter);
+					rtw_generate_random_ibss(pibss);
+
+					pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+					pmlmepriv->to_join = false;
+
+					ret = rtw_createbss_cmd(adapter);
+					if (ret != _SUCCESS)
+						goto unlock;
+				}
+			}
+		} else {
+			int s_ret;
+
+			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+			pmlmepriv->to_join = false;
+			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			if (s_ret == _SUCCESS) {
+			     _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			} else if (s_ret == 2) {/* there is no need to wait for join */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+				rtw_indicate_connect(adapter);
+			} else {
+				if (rtw_to_roam(adapter) != 0) {
+					if (rtw_dec_to_roam(adapter) == 0
+						|| _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)
+					) {
+						rtw_set_to_roam(adapter, 0);
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter);
+					} else {
+						pmlmepriv->to_join = true;
+					}
+				} else
+					rtw_indicate_disconnect(adapter);
+
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			}
+		}
+	} else {
+		if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+				&& check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (rtw_select_roaming_candidate(pmlmepriv) == _SUCCESS) {
+					receive_disconnect(adapter, pmlmepriv->cur_network.network.mac_address
+						, WLAN_REASON_ACTIVE_ROAM);
+				}
+			}
+		}
+	}
+
+unlock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	rtw_os_xmit_schedule(adapter);
+
+	rtw_cfg80211_surveydone_event_callback(adapter);
+
+	rtw_indicate_scan_done(adapter, false);
+}
+
+void rtw_dummy_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+}
+
+void rtw_fwdbg_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct __queue *scan_queue = &pmlmepriv->scanned_queue;
+	struct list_head	*plist, *phead, *ptemp;
+
+	spin_lock_bh(&scan_queue->lock);
+	spin_lock_bh(&free_queue->lock);
+
+	phead = get_list_head(scan_queue);
+	plist = get_next(phead);
+
+	while (plist != phead) {
+		ptemp = get_next(plist);
+		list_del_init(plist);
+		list_add_tail(plist, &free_queue->queue);
+		plist = ptemp;
+		pmlmepriv->num_of_scanned--;
+	}
+
+	spin_unlock_bh(&free_queue->lock);
+	spin_unlock_bh(&scan_queue->lock);
+}
+
+static void rtw_reset_rx_info(struct debug_priv *pdbgpriv)
+{
+	pdbgpriv->dbg_rx_ampdu_drop_count = 0;
+	pdbgpriv->dbg_rx_ampdu_forced_indicate_count = 0;
+	pdbgpriv->dbg_rx_ampdu_loss_count = 0;
+	pdbgpriv->dbg_rx_dup_mgt_frame_drop_count = 0;
+	pdbgpriv->dbg_rx_ampdu_window_shift_cnt = 0;
+}
+
+static void find_network(struct adapter *adapter)
+{
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.mac_address);
+	if (pwlan)
+		pwlan->fixed = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
+	    (adapter->stapriv.asoc_sta_count == 1))
+		rtw_free_network_nolock(adapter, pwlan);
+}
+
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.mac_address);
+		rtw_free_stainfo(adapter,  psta);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		rtw_free_all_stainfo(adapter);
+
+		psta = rtw_get_bcmc_stainfo(adapter);
+		rtw_free_stainfo(adapter, psta);
+
+		rtw_init_bcmc_stainfo(adapter);
+	}
+
+	find_network(adapter);
+
+	if (lock_scanned_queue)
+		adapter->securitypriv.key_mask = 0;
+
+	rtw_reset_rx_info(pdbgpriv);
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	pmlmepriv->to_join = false;
+
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_os_indicate_connect(padapter);
+	}
+
+	rtw_set_to_roam(padapter, 0);
+	rtw_set_scan_deny(padapter, 3000);
+
+}
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
+
+	if (rtw_to_roam(padapter) > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)
+		|| (rtw_to_roam(padapter) <= 0)
+	) {
+		rtw_os_indicate_disconnect(padapter);
+
+		/* set ips_deny_time to avoid enter IPS before LPS leave */
+		rtw_set_ips_deny(padapter, 3000);
+
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+		rtw_clear_scan_deny(padapter);
+	}
+
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+}
+
+inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
+{
+	rtw_os_indicate_scan_done(padapter, aborted);
+
+	if (is_primary_adapter(padapter) &&
+	    (!adapter_to_pwrctl(padapter)->bInSuspend) &&
+	    (!check_fwstate(&padapter->mlmepriv,
+			    WIFI_ASOC_STATE|WIFI_UNDER_LINKING))) {
+		rtw_set_ips_deny(padapter, 0);
+		_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 1);
+	}
+}
+
+void rtw_scan_abort(struct adapter *adapter)
+{
+	unsigned long start;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv);
+
+	start = jiffies;
+	pmlmeext->scan_abort = true;
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)
+		&& jiffies_to_msecs(start) <= 200) {
+
+		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+			break;
+
+		msleep(20);
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		rtw_indicate_scan_done(adapter, true);
+
+	pmlmeext->scan_abort = false;
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	int i;
+	struct sta_info *bmc_sta, *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	psta = rtw_get_stainfo(pstapriv, pnetwork->network.mac_address);
+	if (!psta)
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.mac_address);
+
+	if (psta) { /* update ptarget_sta */
+
+		psta->aid  = pnetwork->join_res;
+
+		update_sta_info(padapter, psta);
+
+		/* update station supportRate */
+		psta->bssratelen = rtw_get_rateset_len(pnetwork->network.supported_rates);
+		memcpy(psta->bssrateset, pnetwork->network.supported_rates, psta->bssratelen);
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+		psta->raid = networktype_to_raid_ex(padapter, psta);
+
+		/* sta mode */
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+		/* security related */
+		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+			padapter->securitypriv.binstallGrpkey = false;
+			padapter->securitypriv.busetkipkey = false;
+			padapter->securitypriv.bgrpkey_handshake = false;
+
+			psta->ieee8021x_blocked = true;
+			psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+
+			memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype));
+
+			memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype));
+			memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype));
+
+			memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48));
+			psta->dot11txpn.val = psta->dot11txpn.val + 1;
+			memset((u8 *)&psta->dot11wtxpn, 0, sizeof(union pn48));
+			memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+		}
+
+		/* 	Commented by Albert 2012/07/21 */
+		/* 	When doing the WPS, the wps_ie_len won't equal to 0 */
+		/* 	And the Wi-Fi driver shouldn't allow the data packet to be transmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
+			psta->ieee8021x_blocked = true;
+			padapter->securitypriv.wps_ie_len = 0;
+		}
+
+		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
+		/* if A-MPDU Rx is enabled, resetting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* todo: check if AP can send A-MPDU packets */
+		for (i = 0; i < 16 ; i++) {
+			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = false;
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+			preorder_ctrl->wsize_b = 64;/* max_ampdu_sz;ex. 32(kbytes) -> wsize_b =32 */
+		}
+
+		bmc_sta = rtw_get_bcmc_stainfo(padapter);
+		if (bmc_sta) {
+			for (i = 0; i < 16 ; i++) {
+				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+				preorder_ctrl->wend_b = 0xffff;
+				preorder_ctrl->wsize_b = 64;/* max_ampdu_sz;ex. 32(kbytes) -> wsize_b =32 */
+			}
+		}
+	}
+
+	return psta;
+
+}
+
+/* pnetwork : returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+
+	/*  why not use ptarget_wlan?? */
+	memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.length);
+	/*  some ies in pnetwork is wrong, so we should use ptarget_wlan ies */
+	cur_network->network.ie_length = ptarget_wlan->network.ie_length;
+	memcpy(&cur_network->network.ies[0], &ptarget_wlan->network.ies[0], MAX_IE_SZ);
+
+	cur_network->aid = pnetwork->join_res;
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	padapter->recvpriv.signal_strength = ptarget_wlan->network.phy_info.signal_strength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.phy_info.signal_quality;
+	/* the ptarget_wlan->network.rssi is raw data, we use ptarget_wlan->network.phy_info.signal_strength instead (has scaled) */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.phy_info.signal_strength);
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
+	switch (pnetwork->network.infrastructure_mode) {
+	case Ndis802_11Infrastructure:
+
+			if (pmlmepriv->fw_state&WIFI_UNDER_WPS)
+				pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
+			else
+				pmlmepriv->fw_state = WIFI_STATION_STATE;
+
+			break;
+	case Ndis802_11IBSS:
+			pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+			break;
+	default:
+			pmlmepriv->fw_state = WIFI_NULL_STATE;
+			break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.ies) + sizeof(struct ndis_802_11_fix_ie),
+									(cur_network->network.ie_length));
+
+	rtw_update_ht_cap(padapter, cur_network->network.ies, cur_network->network.ie_length, (u8) cur_network->network.configuration.ds_config);
+}
+
+/* Notes: the function could be > passive_level (the same context as Rx tasklet) */
+/* pnetwork : returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+/* if join_res > 0, for (fw_state ==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist. */
+/* if join_res > 0, for (fw_state ==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */
+/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan != NULL). */
+/*  */
+/* define REJOIN */
+void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
+{
+	static u8 __maybe_unused retry;
+	struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network	*cur_network = &(pmlmepriv->cur_network);
+	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
+	unsigned int		the_same_macaddr = false;
+
+	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+
+	the_same_macaddr = !memcmp(pnetwork->network.mac_address, cur_network->network.mac_address, ETH_ALEN);
+
+	pnetwork->network.length = get_wlan_bssid_ex_sz(&pnetwork->network);
+	if (pnetwork->network.length > sizeof(struct wlan_bssid_ex))
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+	if (pnetwork->join_res > 0) {
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		retry = 0;
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/* s1. find ptarget_wlan */
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr) {
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.mac_address);
+				} else {
+					pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.mac_address);
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
+
+					pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.mac_address);
+					if (pcur_sta)
+						rtw_free_stainfo(adapter,  pcur_sta);
+
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.mac_address);
+					if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed = true;
+					}
+				}
+
+			} else {
+				ptarget_wlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, pnetwork);
+				if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
+				}
+			}
+
+			/* s2. update cur_network */
+			if (ptarget_wlan) {
+				rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+			} else {
+				netdev_dbg(adapter->pnetdev,
+					   "Can't find ptarget_wlan when joinbss_event callback\n");
+				spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+				goto ignore_joinbss_callback;
+			}
+
+			/* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+				if (!ptarget_sta) {
+					spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+					goto ignore_joinbss_callback;
+				}
+			}
+
+			/* s4. indicate connect */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+				pmlmepriv->cur_network_scanned = ptarget_wlan;
+				rtw_indicate_connect(adapter);
+			}
+
+			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+			spin_unlock_bh(&pmlmepriv->lock);
+			/* s5. Cancel assoc_timer */
+			del_timer_sync(&pmlmepriv->assoc_timer);
+			spin_lock_bh(&pmlmepriv->lock);
+		} else {
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		}
+	} else if (pnetwork->join_res == -4) {
+		rtw_reset_securitypriv(adapter);
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+		/* rtw_free_assoc_resources(adapter, 1); */
+
+		if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true)
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	} else {/* if join_res < 0 (join fails), then try again */
+
+		#ifdef REJOIN
+		res = _FAIL;
+		if (retry < 2)
+			res = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+
+		if (res == _SUCCESS) {
+			/* extend time of assoc_timer */
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			retry++;
+		} else if (res == 2) {/* there is no need to wait for join */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			rtw_indicate_connect(adapter);
+		} else {
+		#endif
+
+			_set_timer(&pmlmepriv->assoc_timer, 1);
+			/* rtw_free_assoc_resources(adapter, 1); */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		#ifdef REJOIN
+			retry = 0;
+		}
+		#endif
+	}
+
+ignore_joinbss_callback:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+
+	mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+	rtw_os_xmit_schedule(adapter);
+}
+
+/* FOR STA, AP , AD-HOC mode */
+void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta, u32 mstatus)
+{
+	u16 media_status_rpt;
+
+	if (!psta)
+		return;
+
+	media_status_rpt = (u16)((psta->mac_id<<8)|mstatus); /*   MACID|OPMODE:1 connect */
+	rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status_rpt);
+}
+
+void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	struct sta_info *psta;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct stassoc_event	*pstassoc	= (struct stassoc_event *)pbuf;
+	struct wlan_network	*cur_network = &(pmlmepriv->cur_network);
+	struct wlan_network	*ptarget_wlan = NULL;
+
+	if (rtw_access_ctrl(adapter, pstassoc->macaddr) == false)
+		return;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+		if (psta) {
+			u8 *passoc_req = NULL;
+			u32 assoc_req_len = 0;
+
+			rtw_sta_media_status_rpt(adapter, psta, 1);
+
+			ap_sta_info_defer_update(adapter, psta);
+
+			/* report to upper layer */
+			spin_lock_bh(&psta->lock);
+			if (psta->passoc_req && psta->assoc_req_len > 0) {
+				passoc_req = rtw_zmalloc(psta->assoc_req_len);
+				if (passoc_req) {
+					assoc_req_len = psta->assoc_req_len;
+					memcpy(passoc_req, psta->passoc_req, assoc_req_len);
+
+					kfree(psta->passoc_req);
+					psta->passoc_req = NULL;
+					psta->assoc_req_len = 0;
+				}
+			}
+			spin_unlock_bh(&psta->lock);
+
+			if (passoc_req && assoc_req_len > 0) {
+				rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len);
+
+				kfree(passoc_req);
+			}
+		}
+		return;
+	}
+
+	/* for AD-HOC mode */
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta) {
+		/* the sta have been in sta_info_queue => do nothing */
+
+		return; /* between drv has received this event before and  fw have not yet to set key to CAM_ENTRY) */
+	}
+
+	psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (!psta)
+		return;
+
+	/* to do : init sta_info variable */
+	psta->qos_option = 0;
+	psta->mac_id = (uint)pstassoc->cam_id;
+	/* psta->aid = (uint)pstassoc->cam_id; */
+
+	/* for ad-hoc mode */
+	rtw_hal_set_odm_var(adapter, HAL_ODM_STA_INFO, psta, true);
+
+	rtw_sta_media_status_rpt(adapter, psta, 1);
+
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+
+	psta->ieee8021x_blocked = false;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.mac_address);
+			pmlmepriv->cur_network_scanned = ptarget_wlan;
+			if (ptarget_wlan)
+				ptarget_wlan->fixed = true;
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+			/*  a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			rtw_indicate_connect(adapter);
+		}
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	mlmeext_sta_add_event_callback(adapter, psta);
+}
+
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	int mac_id = (-1);
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	struct wlan_bssid_ex    *pdev_network = NULL;
+	u8 *pibss = NULL;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct	stadel_event *pstadel	= (struct stadel_event *)pbuf;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+	if (psta)
+		mac_id = psta->mac_id;
+	else
+		mac_id = pstadel->mac_id;
+
+	if (mac_id >= 0) {
+		u16 media_status;
+
+		media_status = (mac_id<<8)|0; /*   MACID|OPMODE:0 means disconnect */
+		/* for STA, AP, ADHOC mode, report disconnect stauts to FW */
+		rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
+	}
+
+	/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		return;
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		u16 reason = *((unsigned short *)(pstadel->rsvd));
+		bool roam = false;
+		struct wlan_network *roam_target = NULL;
+
+		if (adapter->registrypriv.wifi_spec == 1) {
+			roam = false;
+		} else if (reason == WLAN_REASON_EXPIRATION_CHK && rtw_chk_roam_flags(adapter, RTW_ROAM_ON_EXPIRED)) {
+			roam = true;
+		} else if (reason == WLAN_REASON_ACTIVE_ROAM && rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+			roam = true;
+			roam_target = pmlmepriv->roam_network;
+		}
+
+		if (roam) {
+			if (rtw_to_roam(adapter) > 0)
+				rtw_dec_to_roam(adapter); /* this stadel_event is caused by roaming, decrease to_roam */
+			else if (rtw_to_roam(adapter) == 0)
+				rtw_set_to_roam(adapter, adapter->registrypriv.max_roaming_times);
+		} else {
+			rtw_set_to_roam(adapter, 0);
+		}
+
+		rtw_free_uc_swdec_pending_queue(adapter);
+
+		rtw_free_assoc_resources(adapter, 1);
+		rtw_indicate_disconnect(adapter);
+
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		/*  remove the network entry in scanned_queue */
+		pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.mac_address);
+		if (pwlan) {
+			pwlan->fixed = false;
+			rtw_free_network_nolock(adapter, pwlan);
+		}
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+		_rtw_roaming(adapter, roam_target);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	      check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+
+		rtw_free_stainfo(adapter,  psta);
+
+		if (adapter->stapriv.asoc_sta_count == 1) {/* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			u8 ret = _SUCCESS;
+			/* rtw_indicate_disconnect(adapter);removed@20091105 */
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			/* free old ibss network */
+			/* pwlan = rtw_find_network(&pmlmepriv->scanned_queue, pstadel->macaddr); */
+			pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.mac_address);
+			if (pwlan) {
+				pwlan->fixed = false;
+				rtw_free_network_nolock(adapter, pwlan);
+			}
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+			/* re-create ibss */
+			pdev_network = &(adapter->registrypriv.dev_network);
+			pibss = adapter->registrypriv.dev_network.mac_address;
+
+			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+
+			memcpy(&pdev_network->ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+			rtw_update_registrypriv_dev_network(adapter);
+
+			rtw_generate_random_ibss(pibss);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+
+			ret = rtw_createbss_cmd(adapter);
+			if (ret != _SUCCESS)
+				goto unlock;
+		}
+
+	}
+
+unlock:
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+	struct reportpwrstate_parm *preportpwrstate;
+
+	preportpwrstate = (struct reportpwrstate_parm *)pbuf;
+	preportpwrstate->state |= (u8)(adapter_to_pwrctl(padapter)->cpwm_tog + 0x80);
+	cpwm_int_hdl(padapter, preportpwrstate);
+}
+
+void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+	WMMOnAssocRsp(padapter);
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t,
+						  mlmepriv.assoc_timer);
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (rtw_to_roam(adapter) > 0) { /* join timeout caused by roaming */
+		while (1) {
+			rtw_dec_to_roam(adapter);
+			if (rtw_to_roam(adapter) != 0) { /* try another */
+				int do_join_r;
+
+				do_join_r = rtw_do_join(adapter);
+				if (do_join_r != _SUCCESS) {
+					continue;
+				}
+				break;
+			} else {
+				rtw_indicate_disconnect(adapter);
+				break;
+			}
+		}
+
+	} else {
+		rtw_indicate_disconnect(adapter);
+		free_scanqueue(pmlmepriv);/*  */
+
+		/* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */
+		rtw_cfg80211_indicate_disconnect(adapter);
+
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Failure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler(struct timer_list *t)
+{
+	struct adapter *adapter = from_timer(adapter, t,
+						  mlmepriv.scan_to_timer);
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	rtw_indicate_scan_done(adapter, true);
+}
+
+void rtw_mlme_reset_auto_scan_int(struct adapter *adapter)
+{
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pmlmeinfo->VHT_enable) /* disable auto scan when connect to 11AC AP */
+		mlme->auto_scan_int_ms = 0;
+	else if (adapter->registrypriv.wifi_spec && is_client_associated_to_ap(adapter) == true)
+		mlme->auto_scan_int_ms = 60*1000;
+	else if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+		if (check_fwstate(mlme, WIFI_STATION_STATE) && check_fwstate(mlme, _FW_LINKED))
+			mlme->auto_scan_int_ms = mlme->roam_scan_int_ms;
+	} else
+		mlme->auto_scan_int_ms = 0; /* disabled */
+}
+
+static void rtw_auto_scan_handler(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	rtw_mlme_reset_auto_scan_int(padapter);
+
+	if (pmlmepriv->auto_scan_int_ms != 0
+		&& jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time) > pmlmepriv->auto_scan_int_ms) {
+
+		if (!padapter->registrypriv.wifi_spec) {
+			if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING) == true)
+				goto exit;
+
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+				goto exit;
+		}
+
+		rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+	}
+
+exit:
+	return;
+}
+
+void rtw_dynamic_check_timer_handler(struct adapter *adapter)
+{
+	if (!adapter)
+		return;
+
+	if (!adapter->hw_init_completed)
+		return;
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return;
+
+	if (adapter->net_closed)
+		return;
+
+	if ((adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+		&& !(hal_btcoex_IsBtControlLps(adapter))
+		) {
+		u8 bEnterPS;
+
+		linked_status_chk(adapter);
+
+		bEnterPS = traffic_status_watchdog(adapter, 1);
+		if (bEnterPS) {
+			/* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 1); */
+			rtw_hal_dm_watchdog_in_lps(adapter);
+		} else {
+			/* call rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1) in traffic_status_watchdog() */
+		}
+
+	} else {
+		if (is_primary_adapter(adapter))
+			rtw_dynamic_chk_wk_cmd(adapter);
+	}
+
+	/* auto site survey */
+	rtw_auto_scan_handler(adapter);
+}
+
+inline bool rtw_is_scan_deny(struct adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	return (atomic_read(&mlmepriv->set_scan_deny) != 0) ? true : false;
+}
+
+inline void rtw_clear_scan_deny(struct adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	atomic_set(&mlmepriv->set_scan_deny, 0);
+}
+
+void rtw_set_scan_deny(struct adapter *adapter, u32 ms)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	atomic_set(&mlmepriv->set_scan_deny, 1);
+	_set_timer(&mlmepriv->set_scan_deny_timer, ms);
+}
+
+/*
+* Select a new roaming candidate from the original @param candidate and @param competitor
+* @return true: candidate is updated
+* @return false: candidate is not updated
+*/
+static int rtw_check_roaming_candidate(struct mlme_priv *mlme
+	, struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = false;
+	struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv);
+
+	if (is_same_ess(&competitor->network, &mlme->cur_network.network) == false)
+		goto exit;
+
+	if (rtw_is_desired_network(adapter, competitor) == false)
+		goto exit;
+
+	/* got specific addr to roam */
+	if (!is_zero_mac_addr(mlme->roam_tgt_addr)) {
+		if (!memcmp(mlme->roam_tgt_addr, competitor->network.mac_address, ETH_ALEN))
+			goto update;
+		else
+			goto exit;
+	}
+	if (jiffies_to_msecs(jiffies - competitor->last_scanned) >= mlme->roam_scanr_exp_ms)
+		goto exit;
+
+	if (competitor->network.rssi - mlme->cur_network_scanned->network.rssi < mlme->roam_rssi_diff_th)
+		goto exit;
+
+	if (*candidate && (*candidate)->network.rssi >= competitor->network.rssi)
+		goto exit;
+
+update:
+	*candidate = competitor;
+	updated = true;
+
+exit:
+	return updated;
+}
+
+int rtw_select_roaming_candidate(struct mlme_priv *mlme)
+{
+	int ret = _FAIL;
+	struct list_head	*phead;
+	struct __queue	*queue	= &(mlme->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	if (!mlme->cur_network_scanned) {
+		rtw_warn_on(1);
+		return ret;
+	}
+
+	spin_lock_bh(&(mlme->scanned_queue.lock));
+	phead = get_list_head(queue);
+
+	list_for_each(mlme->pscanned, phead) {
+
+		pnetwork = list_entry(mlme->pscanned, struct wlan_network,
+				      list);
+
+		rtw_check_roaming_candidate(mlme, &candidate, pnetwork);
+
+	}
+
+	if (!candidate) {
+		ret = _FAIL;
+		goto exit;
+	} else {
+		mlme->roam_network = candidate;
+
+		if (!memcmp(candidate->network.mac_address, mlme->roam_tgt_addr, ETH_ALEN))
+			eth_zero_addr(mlme->roam_tgt_addr);
+	}
+
+	ret = _SUCCESS;
+exit:
+	spin_unlock_bh(&(mlme->scanned_queue.lock));
+
+	return ret;
+}
+
+/*
+* Select a new join candidate from the original @param candidate and @param competitor
+* @return true: candidate is updated
+* @return false: candidate is not updated
+*/
+static int rtw_check_join_candidate(struct mlme_priv *mlme
+	, struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = false;
+	struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv);
+
+	/* check bssid, if needed */
+	if (mlme->assoc_by_bssid) {
+		if (memcmp(competitor->network.mac_address, mlme->assoc_bssid, ETH_ALEN))
+			goto exit;
+	}
+
+	/* check ssid, if needed */
+	if (mlme->assoc_ssid.ssid[0] && mlme->assoc_ssid.ssid_length) {
+		if (competitor->network.ssid.ssid_length != mlme->assoc_ssid.ssid_length
+			|| memcmp(competitor->network.ssid.ssid, mlme->assoc_ssid.ssid, mlme->assoc_ssid.ssid_length)
+		)
+			goto exit;
+	}
+
+	if (rtw_is_desired_network(adapter, competitor)  == false)
+		goto exit;
+
+	if (rtw_to_roam(adapter) > 0) {
+		if (jiffies_to_msecs(jiffies - competitor->last_scanned) >= mlme->roam_scanr_exp_ms
+			|| is_same_ess(&competitor->network, &mlme->cur_network.network) == false
+		)
+			goto exit;
+	}
+
+	if (!*candidate || (*candidate)->network.rssi < competitor->network.rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+
+exit:
+	return updated;
+}
+
+/*
+Calling context:
+The caller of the sub-routine will be in critical section...
+The caller must hold the following spinlock
+pmlmepriv->lock
+*/
+
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
+{
+	int ret;
+	struct list_head	*phead;
+	struct adapter *adapter;
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	adapter = (struct adapter *)pmlmepriv->nic_hdl;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	if (pmlmepriv->roam_network) {
+		candidate = pmlmepriv->roam_network;
+		pmlmepriv->roam_network = NULL;
+		goto candidate_exist;
+	}
+
+	phead = get_list_head(queue);
+	list_for_each(pmlmepriv->pscanned, phead) {
+
+		pnetwork = list_entry(pmlmepriv->pscanned,
+				      struct wlan_network, list);
+
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+
+	}
+
+	if (!candidate) {
+		ret = _FAIL;
+		goto exit;
+	} else {
+		goto candidate_exist;
+	}
+
+candidate_exist:
+
+	/*  check for situation of  _FW_LINKED */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		rtw_disassoc_cmd(adapter, 0, true);
+		rtw_indicate_disconnect(adapter);
+		rtw_free_assoc_resources(adapter, 0);
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+	ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+	return ret;
+}
+
+signed int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj *pcmd;
+	struct	setauth_parm *psetauthparm;
+	struct	cmd_priv *pcmdpriv = &(adapter->cmdpriv);
+	signed int		res = _SUCCESS;
+
+	pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd) {
+		res = _FAIL;  /* try again */
+		goto exit;
+	}
+
+	psetauthparm = rtw_zmalloc(sizeof(struct setauth_parm));
+	if (!psetauthparm) {
+		kfree(pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetauthparm->mode = (unsigned char)psecuritypriv->dot11AuthAlgrthm;
+
+	pcmd->cmdcode = _SetAuth_CMD_;
+	pcmd->parmbuf = (unsigned char *)psetauthparm;
+	pcmd->cmdsz =  (sizeof(struct setauth_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	INIT_LIST_HEAD(&pcmd->list);
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+signed int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, signed int keyid, u8 set_tx, bool enqueue)
+{
+	u8 keylen;
+	struct cmd_obj		*pcmd;
+	struct setkey_parm	*psetkeyparm;
+	struct cmd_priv 	*pcmdpriv = &(adapter->cmdpriv);
+	signed int	res = _SUCCESS;
+
+	psetkeyparm = rtw_zmalloc(sizeof(struct setkey_parm));
+	if (!psetkeyparm) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy;
+	else
+		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+
+	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->set_tx = set_tx;
+	if (is_wep_enc(psetkeyparm->algorithm))
+		adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	switch (psetkeyparm->algorithm) {
+
+	case _WEP40_:
+		keylen = 5;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _WEP104_:
+		keylen = 13;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _TKIP_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	case _AES_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	default:
+		res = _FAIL;
+		kfree(psetkeyparm);
+		goto exit;
+	}
+
+	if (enqueue) {
+		pcmd = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!pcmd) {
+			kfree(psetkeyparm);
+			res = _FAIL;  /* try again */
+			goto exit;
+		}
+
+		pcmd->cmdcode = _SetKey_CMD_;
+		pcmd->parmbuf = (u8 *)psetkeyparm;
+		pcmd->cmdsz =  (sizeof(struct setkey_parm));
+		pcmd->rsp = NULL;
+		pcmd->rspsz = 0;
+
+		INIT_LIST_HEAD(&pcmd->list);
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+	} else {
+		setkey_hdl(adapter, (u8 *)psetkeyparm);
+		kfree(psetkeyparm);
+	}
+exit:
+	return res;
+}
+
+/* adjust ies for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+	unsigned	int ielength = 0;
+	unsigned int i, j;
+
+	i = 12; /* after the fixed IE */
+	while (i < in_len) {
+		ielength = initial_out_len;
+
+		if (in_ie[i] == 0xDD && in_ie[i+2] == 0x00 && in_ie[i+3] == 0x50  && in_ie[i+4] == 0xF2 && in_ie[i+5] == 0x02 && i+5 < in_len) { /* WMM element ID and OUI */
+			for (j = i; j < i + 9; j++) {
+					out_ie[ielength] = in_ie[j];
+					ielength++;
+			}
+			out_ie[initial_out_len + 1] = 0x07;
+			out_ie[initial_out_len + 6] = 0x00;
+			out_ie[initial_out_len + 8] = 0x00;
+
+			break;
+		}
+
+		i += (in_ie[i+1]+2); /*  to the next IE element */
+	}
+
+	return ielength;
+
+}
+
+/*  */
+/*  Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.) */
+/*  Added by Annie, 2006-05-07. */
+/*  */
+/*  Search by BSSID, */
+/*  Return Value: */
+/* 		-1		:if there is no pre-auth key in the  table */
+/* 		>= 0		:if there is pre-auth key, and   return the entry id */
+/*  */
+/*  */
+
+static int SecIsInPMKIDList(struct adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *p = &Adapter->securitypriv;
+	int i;
+
+	for (i = 0; i < NUM_PMKID_CACHE; i++)
+		if ((p->PMKIDList[i].bUsed) &&
+				(!memcmp(p->PMKIDList[i].Bssid, bssid, ETH_ALEN)))
+			return i;
+	return -1;
+}
+
+/*  */
+/*  Check the RSN IE length */
+/*  If the RSN IE length <= 20, the RSN IE didn't include the PMKID information */
+/*  0-11th element in the array are the fixed IE */
+/*  12th element in the array is the IE */
+/*  13th element in the array is the IE length */
+/*  */
+
+static int rtw_append_pmkid(struct adapter *Adapter, int iEntry, u8 *ie, uint ie_len)
+{
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+
+	if (ie[13] <= 20) {
+		/*  The RSN IE didn't include the PMK ID, append the PMK information */
+			ie[ie_len] = 1;
+			ie_len++;
+			ie[ie_len] = 0;	/* PMKID count = 0x0100 */
+			ie_len++;
+			memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+
+			ie_len += 16;
+			ie[13] += 18;/* PMKID length = 2+16 */
+
+	}
+	return ie_len;
+}
+
+signed int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode = 0x0;
+	uint	ielength;
+	int iEntry;
+
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	uint	ndisauthmode = psecuritypriv->ndisauthtype;
+
+	/* copy fixed ie only */
+	memcpy(out_ie, in_ie, 12);
+	ielength = 12;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA) || (ndisauthmode == Ndis802_11AuthModeWPAPSK))
+			authmode = WLAN_EID_VENDOR_SPECIFIC;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA2) || (ndisauthmode == Ndis802_11AuthModeWPA2PSK))
+			authmode = WLAN_EID_RSN;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		memcpy(out_ie+ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len);
+
+		ielength += psecuritypriv->wps_ie_len;
+	} else if ((authmode == WLAN_EID_VENDOR_SPECIFIC) || (authmode == WLAN_EID_RSN)) {
+		/* copy RSN or SSN */
+		memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1]+2);
+		/* debug for CONFIG_IEEE80211W
+		{
+			int jj;
+			printk("supplicant_ie_length =%d &&&&&&&&&&&&&&&&&&&\n", psecuritypriv->supplicant_ie[1]+2);
+			for (jj = 0; jj < psecuritypriv->supplicant_ie[1]+2; jj++)
+				printk(" %02x ", psecuritypriv->supplicant_ie[jj]);
+			printk("\n");
+		}*/
+		ielength += psecuritypriv->supplicant_ie[1]+2;
+		rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+	}
+
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if (iEntry < 0) {
+		return ielength;
+	} else {
+		if (authmode == WLAN_EID_RSN)
+			ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+	}
+	return ielength;
+}
+
+void rtw_init_registrypriv_dev_network(struct adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+
+	memcpy(pdev_network->mac_address, myhwaddr, ETH_ALEN);
+
+	memcpy(&pdev_network->ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid));
+
+	pdev_network->configuration.length = sizeof(struct ndis_802_11_conf);
+	pdev_network->configuration.beacon_period = 100;
+}
+
+void rtw_update_registrypriv_dev_network(struct adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+	/* struct	xmit_priv *pxmitpriv = &adapter->xmitpriv; */
+
+	pdev_network->privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0) ; /*  adhoc no 802.1x */
+
+	pdev_network->rssi = 0;
+
+	switch (pregistrypriv->wireless_mode) {
+	case WIRELESS_11B:
+		pdev_network->network_type_in_use = (Ndis802_11DS);
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11_24N:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pdev_network->network_type_in_use = (Ndis802_11OFDM24);
+		break;
+	default:
+		/*  TODO */
+		break;
+	}
+
+	pdev_network->configuration.ds_config = (pregistrypriv->channel);
+
+	if (cur_network->network.infrastructure_mode == Ndis802_11IBSS)
+		pdev_network->configuration.atim_window = (0);
+
+	pdev_network->infrastructure_mode = (cur_network->network.infrastructure_mode);
+
+	/*  1. Supported rates */
+	/*  2. IE */
+
+	/* rtw_set_supported_rate(pdev_network->supported_rates, pregistrypriv->wireless_mode) ;  will be called in rtw_generate_ie */
+	sz = rtw_generate_ie(pregistrypriv);
+
+	pdev_network->ie_length = sz;
+
+	pdev_network->length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex  *)pdev_network);
+
+	/* notes: translate ie_length & length after assign the length to cmdsz in createbss_cmd(); */
+	/* pdev_network->ie_length = cpu_to_le32(sz); */
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter)
+{
+}
+
+/* the function is at passive_level */
+void rtw_joinbss_reset(struct adapter *padapter)
+{
+	u8 threshold;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+
+	/* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+
+	pmlmepriv->num_FortyMHzIntolerant = 0;
+
+	pmlmepriv->num_sta_no_ht = 0;
+
+	phtpriv->ampdu_enable = false;/* reset to disabled */
+
+	/*  TH = 1 => means that invalidate usb rx aggregation */
+	/*  TH = 0 => means that validate usb rx aggregation, use init value. */
+	if (phtpriv->ht_option) {
+		if (padapter->registrypriv.wifi_spec == 1)
+			threshold = 1;
+		else
+			threshold = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	} else {
+		threshold = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	}
+}
+
+void rtw_ht_use_default_setting(struct adapter *padapter)
+{
+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	bool		bHwLDPCSupport = false, bHwSTBCSupport = false;
+	bool		bHwSupportBeamformer = false, bHwSupportBeamformee = false;
+
+	if (pregistrypriv->wifi_spec)
+		phtpriv->bss_coexist = 1;
+	else
+		phtpriv->bss_coexist = 0;
+
+	phtpriv->sgi_40m = TEST_FLAG(pregistrypriv->short_gi, BIT1) ? true : false;
+	phtpriv->sgi_20m = TEST_FLAG(pregistrypriv->short_gi, BIT0) ? true : false;
+
+	/*  LDPC support */
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport);
+	CLEAR_FLAGS(phtpriv->ldpc_cap);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT4))
+			SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT5))
+			SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX);
+	}
+
+	/*  STBC */
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport);
+	CLEAR_FLAGS(phtpriv->stbc_cap);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT5))
+			SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT4))
+			SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX);
+	}
+
+	/*  Beamforming setting */
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer);
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee);
+	CLEAR_FLAGS(phtpriv->beamform_cap);
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT4) && bHwSupportBeamformer)
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee)
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+}
+
+void rtw_build_wmm_ie_ht(struct adapter *padapter, u8 *out_ie, uint *pout_len)
+{
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	int out_len;
+
+	if (padapter->mlmepriv.qospriv.qos_option == 0) {
+		out_len = *pout_len;
+		rtw_set_ie(out_ie+out_len, WLAN_EID_VENDOR_SPECIFIC,
+			   _WMM_IE_Length_, WMM_IE, pout_len);
+
+		padapter->mlmepriv.qospriv.qos_option = 1;
+	}
+}
+
+/* the function is >= passive_level */
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel)
+{
+	u32 ielen, out_len;
+	enum ieee80211_max_ampdu_length_exp max_rx_ampdu_factor;
+	unsigned char *p;
+	struct ieee80211_ht_cap ht_capie;
+	u8 cbw40_enable = 0, stbc_rx_enable = 0, operation_bw = 0;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	phtpriv->ht_option = false;
+
+	out_len = *pout_len;
+
+	memset(&ht_capie, 0, sizeof(struct ieee80211_ht_cap));
+
+	ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_DSSSCCK40);
+
+	if (phtpriv->sgi_20m)
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_20);
+
+	/* Get HT BW */
+	if (!in_ie) {
+		/* TDLS: TODO 20/40 issue */
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+			operation_bw = padapter->mlmeextpriv.cur_bwmode;
+			if (operation_bw > CHANNEL_WIDTH_40)
+				operation_bw = CHANNEL_WIDTH_40;
+		} else
+			/* TDLS: TODO 40? */
+			operation_bw = CHANNEL_WIDTH_40;
+	} else {
+		p = rtw_get_ie(in_ie, WLAN_EID_HT_OPERATION, &ielen, in_len);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			struct HT_info_element *pht_info = (struct HT_info_element *)(p+2);
+
+			if (pht_info->infos[0] & BIT(2)) {
+				switch (pht_info->infos[0] & 0x3) {
+				case 1:
+				case 3:
+					operation_bw = CHANNEL_WIDTH_40;
+					break;
+				default:
+					operation_bw = CHANNEL_WIDTH_20;
+					break;
+				}
+			} else {
+				operation_bw = CHANNEL_WIDTH_20;
+			}
+		}
+	}
+
+	/* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
+	if (channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else {
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			cbw40_enable = 1;
+	}
+
+	if ((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) {
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH);
+		if (phtpriv->sgi_40m)
+			ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_40);
+	}
+
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX))
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_TX_STBC);
+
+	/* todo: disable SM power save mode */
+	ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS);
+
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) {
+		if ((channel <= 14 && pregistrypriv->rx_stbc == 0x1) ||	/* enable for 2.4GHz */
+			(pregistrypriv->wifi_spec == 1))
+			stbc_rx_enable = 1;
+	}
+
+	/* fill default supported_mcs_set */
+	memcpy(&ht_capie.mcs, pmlmeext->default_supported_mcs_set, 16);
+
+	/* update default supported_mcs_set */
+	if (stbc_rx_enable)
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_1R);/* RX STBC One spatial stream */
+
+	set_mcs_rate_by_mask(ht_capie.mcs.rx_mask, MCS_RATE_1R);
+
+	{
+		u32 rx_packet_offset, max_recvbuf_sz;
+
+		rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+		rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+	}
+
+	if (padapter->driver_rx_ampdu_factor != 0xFF)
+		max_rx_ampdu_factor =
+		  (enum ieee80211_max_ampdu_length_exp)padapter->driver_rx_ampdu_factor;
+	else
+		rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
+				    &max_rx_ampdu_factor);
+
+	/* rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); */
+	ht_capie.ampdu_params_info = (max_rx_ampdu_factor&0x03);
+
+	if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));
+	else
+		ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);
+
+	rtw_set_ie(out_ie+out_len, WLAN_EID_HT_CAPABILITY,
+		   sizeof(struct ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+	phtpriv->ht_option = true;
+
+	if (in_ie) {
+		p = rtw_get_ie(in_ie, WLAN_EID_HT_OPERATION, &ielen, in_len);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			out_len = *pout_len;
+			rtw_set_ie(out_ie+out_len, WLAN_EID_HT_OPERATION, ielen, p+2, pout_len);
+		}
+	}
+
+	return phtpriv->ht_option;
+
+}
+
+/* the function is > passive_level (in critical_section) */
+void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len, u8 channel)
+{
+	u8 *p, max_ampdu_sz;
+	int len;
+	/* struct sta_info *bmc_sta, *psta; */
+	struct ieee80211_ht_cap *pht_capie;
+	/* struct recv_reorder_ctrl *preorder_ctrl; */
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	/* struct wlan_network *pcur_network = &(pmlmepriv->cur_network);; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 cbw40_enable = 0;
+
+	if (!phtpriv->ht_option)
+		return;
+
+	if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable))
+		return;
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if (!(phtpriv->ampdu_enable) && pregistrypriv->ampdu_enable == 1) {
+		phtpriv->ampdu_enable = true;
+	}
+
+	/* check Max Rx A-MPDU Size */
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), WLAN_EID_HT_CAPABILITY, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		pht_capie = (struct ieee80211_ht_cap *)(p+2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+		max_ampdu_sz = 1 << (max_ampdu_sz+3); /*  max_ampdu_sz (kbytes); */
+
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+
+	}
+
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), WLAN_EID_HT_OPERATION, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		/* todo: */
+	}
+
+	if (channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else {
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			cbw40_enable = 1;
+	}
+
+	/* update cur_bwmode & cur_ch_offset */
+	if ((cbw40_enable) &&
+	    (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) &
+	      BIT(1)) && (pmlmeinfo->HT_info.infos[0] & BIT(2))) {
+		int i;
+
+		/* update the MCS set */
+		for (i = 0; i < 16; i++)
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i];
+
+		/* update the MCS rates */
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+
+		/* switch to the 40M Hz mode according to the AP */
+		/* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */
+		switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) {
+		case EXTCHNL_OFFSET_UPPER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+
+		case EXTCHNL_OFFSET_LOWER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+
+		default:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+	}
+
+	/*  */
+	/*  Config SM Power Save setting */
+	/*  */
+	pmlmeinfo->SM_PS =
+		(le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) &
+		 0x0C) >> 2;
+
+	/*  */
+	/*  Config current HT Protection mode. */
+	/*  */
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 issued;
+	int priority;
+	struct sta_info *psta;
+	struct ht_priv *phtpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+
+	/* if (bmcst || (padapter->mlmepriv.LinkDetectInfo.bTxBusyTraffic == false)) */
+	if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100))
+		return;
+
+	priority = pattrib->priority;
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta)
+		return;
+
+	if (!psta)
+		return;
+
+	if (!(psta->state & _FW_LINKED))
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if (phtpriv->ht_option && phtpriv->ampdu_enable) {
+		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+		if (issued == 0) {
+			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+			rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra);
+		}
+	}
+
+}
+
+void rtw_append_exented_cap(struct adapter *padapter, u8 *out_ie, uint *pout_len)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+	u8 cap_content[8] = {0};
+
+	if (phtpriv->bss_coexist)
+		SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1);
+
+	rtw_set_ie(out_ie + *pout_len, WLAN_EID_EXT_CAPABILITY, 8, cap_content, pout_len);
+}
+
+inline void rtw_set_to_roam(struct adapter *adapter, u8 to_roam)
+{
+	if (to_roam == 0)
+		adapter->mlmepriv.to_join = false;
+	adapter->mlmepriv.to_roam = to_roam;
+}
+
+inline u8 rtw_dec_to_roam(struct adapter *adapter)
+{
+	adapter->mlmepriv.to_roam--;
+	return adapter->mlmepriv.to_roam;
+}
+
+inline u8 rtw_to_roam(struct adapter *adapter)
+{
+	return adapter->mlmepriv.to_roam;
+}
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+	_rtw_roaming(padapter, tgt_network);
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+
+	if (rtw_to_roam(padapter) > 0) {
+		memcpy(&pmlmepriv->assoc_ssid, &cur_network->network.ssid, sizeof(struct ndis_802_11_ssid));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+		while (rtw_do_join(padapter) != _SUCCESS) {
+			rtw_dec_to_roam(padapter);
+			if (rtw_to_roam(padapter) <= 0) {
+				rtw_indicate_disconnect(padapter);
+				break;
+			}
+		}
+	}
+}
+
+signed int rtw_linked_check(struct adapter *padapter)
+{
+	if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true) ||
+			(check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true)) {
+		if (padapter->stapriv.asoc_sta_count > 2)
+			return true;
+	} else {	/* Station mode */
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true)
+			return true;
+	}
+	return false;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
new file mode 100644
index 000000000..8e74b4f47
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
@@ -0,0 +1,6023 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_wifi_regd.h>
+#include <hal_btcoex.h>
+#include <linux/kernel.h>
+#include <asm/unaligned.h>
+
+static struct mlme_handler mlme_sta_tbl[] = {
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuthClient},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+	{WIFI_ACTION_NOACK, "OnActionNoAck",	&OnAction},
+};
+
+static struct action_handler OnAction_tbl[] = {
+	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
+	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &DoReserved},
+	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &DoReserved},
+	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&DoReserved},
+	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
+	{RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
+	{RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
+	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &DoReserved},
+	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &DoReserved},
+};
+
+static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+/**************************************************
+OUI definitions for the vendor specific IE
+***************************************************/
+unsigned char RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+unsigned char P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};
+unsigned char WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A};
+
+unsigned char WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+unsigned char WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+static struct rt_channel_plan_2g	RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},			/*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},	/*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
+	{{10, 11, 12, 13}, 4},						/*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},	/*  0x05, RT_CHANNEL_DOMAIN_2G_GLOBAL , Passive scan CH 12, 13, 14 */
+	{{}, 0},								/*  0x06, RT_CHANNEL_DOMAIN_2G_NULL */
+};
+
+static struct rt_channel_plan_map	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	/*  0x00 ~ 0x1F , Old Define ===== */
+	{0x02},	/* 0x00, RT_CHANNEL_DOMAIN_FCC */
+	{0x02},	/* 0x01, RT_CHANNEL_DOMAIN_IC */
+	{0x01},	/* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+	{0x01},	/* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+	{0x01},	/* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+	{0x03},	/* 0x05, RT_CHANNEL_DOMAIN_MKK */
+	{0x03},	/* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+	{0x01},	/* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+	{0x03},	/* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+	{0x03},	/* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+	{0x00},	/* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+	{0x02},	/* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+	{0x01},	/* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+	{0x02},	/* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+	{0x02},	/* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+	{0x02},	/* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+	{0x01},	/* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+	{0x02},	/* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+	{0x01},	/* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x00},	/* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
+	{0x02},	/* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+	{0x00},	/* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+	{0x00},	/* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+	{0x03},	/* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x06},	/* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+	{0x02},	/* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+	{0x00},	/* 0x1A, */
+	{0x00},	/* 0x1B, */
+	{0x00},	/* 0x1C, */
+	{0x00},	/* 0x1D, */
+	{0x00},	/* 0x1E, */
+	{0x06},	/* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
+	/*  0x20 ~ 0x7F , New Define ===== */
+	{0x00},	/* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+	{0x01},	/* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+	{0x02},	/* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+	{0x03},	/* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+	{0x04},	/* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+	{0x02},	/* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+	{0x00},	/* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+	{0x03},	/* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+	{0x00},	/* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+	{0x00},	/* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+	{0x00},	/* 0x2A, */
+	{0x00},	/* 0x2B, */
+	{0x00},	/* 0x2C, */
+	{0x00},	/* 0x2D, */
+	{0x00},	/* 0x2E, */
+	{0x00},	/* 0x2F, */
+	{0x00},	/* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+	{0x00},	/* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+	{0x00},	/* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+	{0x00},	/* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+	{0x02},	/* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+	{0x00},	/* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+	{0x00},	/* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+	{0x03},	/* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+	{0x03},	/* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+	{0x02},	/* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+	{0x00},	/* 0x3A, */
+	{0x00},	/* 0x3B, */
+	{0x00},	/* 0x3C, */
+	{0x00},	/* 0x3D, */
+	{0x00},	/* 0x3E, */
+	{0x00},	/* 0x3F, */
+	{0x02},	/* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+	{0x05},	/* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL */
+	{0x01},	/* 0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4 */
+	{0x02},	/* 0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2 */
+	{0x02},	/* 0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3 */
+	{0x00},	/* 0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5 */
+	{0x02},	/* 0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8 */
+	{0x00},	/* 0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6 */
+	{0x00},	/* 0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7 */
+	{0x00},	/* 0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8 */
+	{0x00},	/* 0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9 */
+	{0x00},	/* 0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10 */
+	{0x00},	/* 0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11 */
+	{0x02},	/* 0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4 */
+	{0x00},	/* 0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12 */
+	{0x02},	/* 0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9 */
+	{0x00},	/* 0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13 */
+	{0x02},	/* 0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10 */
+};
+
+ /* use the combination for max channel numbers */
+static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03};
+
+/* Search the @param ch in given @param ch_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ *
+ * return the index of channel_num in channel_set, -1 if not found
+ */
+int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
+{
+	int i;
+
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (ch == ch_set[i].ChannelNum)
+			break;
+	}
+
+	if (i >= ch_set[i].ChannelNum)
+		return -1;
+	return i;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	return _SUCCESS;
+}
+
+void init_mlme_default_rate_set(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	unsigned char mixed_datarate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_, 0xff};
+	unsigned char mixed_basicrate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
+	unsigned char supported_mcs_set[16] = {0xff, 0xff, 0x00, 0x00, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set));
+}
+
+static void init_mlme_ext_priv_value(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	atomic_set(&pmlmeext->event_seq, 0);
+	pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
+	pmlmeext->sa_query_seq = 0;
+	pmlmeext->mgnt_80211w_IPN = 0;
+	pmlmeext->mgnt_80211w_IPN_rx = 0;
+	pmlmeext->cur_channel = padapter->registrypriv.channel;
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	init_mlme_default_rate_set(padapter);
+
+	pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->scan_abort = false;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	memset(pmlmeinfo->chg_txt, 0, 128);
+
+	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+	pmlmeinfo->dialogToken = 0;
+
+	pmlmeext->action_public_rxseq = 0xffff;
+	pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(struct rt_channel_info *channel_set,
+					   u8 chanset_size,
+					   u8 chan)
+{
+	int i;
+
+	for (i = 0; i < chanset_size; i++)
+		if (channel_set[i].ChannelNum == chan)
+			return 1;
+
+	return 0;
+}
+
+static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
+							  u8 chanset_size,
+							  struct p2p_channels *channel_list)
+{
+
+	static const struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+		{ IEEE80211A, 115,  36,  48,  4, BW20 },
+		{ IEEE80211A, 116,  36,  44,  8, BW40PLUS },
+		{ IEEE80211A, 117,  40,  48,  8, BW40MINUS },
+		{ IEEE80211A, 124, 149, 161,  4, BW20 },
+		{ IEEE80211A, 125, 149, 169,  4, BW20 },
+		{ IEEE80211A, 126, 149, 157,  8, BW40PLUS },
+		{ IEEE80211A, 127, 153, 161,  8, BW40MINUS },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		const struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch))
+				continue;
+
+			if ((padapter->registrypriv.ht_enable == 0) && (o->inc == 8))
+				continue;
+
+			if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
+				((o->bw == BW40MINUS) || (o->bw == BW40PLUS)))
+				continue;
+
+			if (!reg) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+
+}
+
+static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b2_4GBand = false;
+	u8 Index2G = 0;
+
+	memset(channel_set, 0, sizeof(struct rt_channel_info)*MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+		return chanset_size;
+
+	if (is_supported_24g(padapter->registrypriv.wireless_mode)) {
+		b2_4GBand = true;
+		if (ChannelPlan == RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+	}
+
+	if (b2_4GBand) {
+		for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+			if ((ChannelPlan == RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN) ||/* Channel 1~11 is active, and 12~14 is passive */
+				(ChannelPlan == RT_CHANNEL_DOMAIN_GLOBAL_NULL)) {
+				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
+			} else if (ChannelPlan == RT_CHANNEL_DOMAIN_WORLD_WIDE_13 ||
+				 Index2G == RT_CHANNEL_DOMAIN_2G_WORLD) { /*  channel 12~13, passive scan */
+				if (channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			} else
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+
+			chanset_size++;
+		}
+	}
+
+	return chanset_size;
+}
+
+void init_mlme_ext_priv(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	pmlmeext->padapter = padapter;
+
+	/* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->accept_addba_req = pregistrypriv->accept_addba_req;
+
+	init_mlme_ext_timer(padapter);
+
+	init_mlme_ap_info(padapter);
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+	pmlmeext->last_scan_time = 0;
+	pmlmeext->chan_scan_time = SURVEY_TO;
+	pmlmeext->mlmeext_init = true;
+	pmlmeext->active_keep_alive_check = true;
+
+#ifdef DBG_FIXED_CHAN
+	pmlmeext->fixed_chan = 0xFF;
+#endif
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped) {
+		del_timer_sync(&pmlmeext->survey_timer);
+		del_timer_sync(&pmlmeext->link_timer);
+		/* del_timer_sync(&pmlmeext->ADDBA_timer); */
+	}
+}
+
+static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	if (ptable->func) {
+		/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+		if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+		    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+			return;
+
+		ptable->func(padapter, precv_frame);
+	}
+}
+
+void mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE)
+		return;
+
+	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+		memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) {
+		return;
+	}
+
+	ptable = mlme_sta_tbl;
+
+	index = GetFrameSubType(pframe) >> 4;
+
+	if (index >= ARRAY_SIZE(mlme_sta_tbl))
+		return;
+
+	ptable += index;
+
+	if (psta) {
+		if (GetRetry(pframe)) {
+			if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) {
+				/* drop the duplicate management frame */
+				pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++;
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
+	}
+
+	switch (GetFrameSubType(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		fallthrough;
+	case WIFI_ASSOCREQ:
+	case WIFI_REASSOCREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_PROBEREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_BEACON:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_ACTION:
+		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	default:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	}
+}
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char *p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct wlan_bssid_ex	*cur = &pmlmeinfo->network;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 is_valid_p2p_probereq = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == false &&
+		check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == false) {
+		return _SUCCESS;
+	}
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, WLAN_EID_SSID, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+
+	/* check (wildcard) SSID */
+	if (p) {
+		if (is_valid_p2p_probereq)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && false == !memcmp((void *)(p+2), (void *)cur->ssid.ssid, cur->ssid.ssid_length))
+			|| (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
+		)
+			return _SUCCESS;
+
+_issue_probersp:
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) &&
+		     pmlmepriv->cur_network.join_res) ||
+		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int cam_idx;
+	struct sta_info *psta;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	struct wlan_bssid_ex *pbss;
+	int ret = _SUCCESS;
+	u8 *p = NULL;
+	u32 ielen = 0;
+
+	p = rtw_get_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, WLAN_EID_EXT_SUPP_RATES, &ielen, precv_frame->u.hdr.len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
+	if (p && ielen > 0) {
+		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
+			/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
+			*(p + 1) = ielen - 1;
+	}
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) {
+		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+			/* we should update current network before auth, or some IE is wrong */
+			pbss = rtw_malloc(sizeof(struct wlan_bssid_ex));
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
+					rtw_get_bcn_info(&(pmlmepriv->cur_network));
+				}
+				kfree(pbss);
+			}
+
+			/* check the vendor of the assoc AP */
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct ieee80211_hdr_3addr));
+
+			/* update TSF Value */
+			update_TSF(pmlmeext, pframe, len);
+
+			/* reset for adaptive_early_32k */
+			pmlmeext->adaptive_tsf_done = false;
+			pmlmeext->DrvBcnEarly = 0xff;
+			pmlmeext->DrvBcnTimeOut = 0xff;
+			pmlmeext->bcn_cnt = 0;
+			memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt));
+			memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio));
+
+			/* start auth */
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+					netdev_dbg(padapter->pnetdev,
+						   "ap has changed, disconnect now\n ");
+					receive_disconnect(padapter,
+							   pmlmeinfo->network.mac_address, 0);
+					return _SUCCESS;
+				}
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+
+				adaptive_early_32k(pmlmeext, pframe, len);
+			}
+		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta) {
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+			} else {
+				/* allocate a new CAM entry for IBSS station */
+				cam_idx = allocate_fw_sta_entry(padapter);
+				if (cam_idx == NUM_STA)
+					goto _END_ONBEACON_;
+
+				/* get supported rate */
+				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
+					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+					goto _END_ONBEACON_;
+				}
+
+				/* update TSF Value */
+				update_TSF(pmlmeext, pframe, len);
+
+				/* report sta add event */
+				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAuth(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	auth_mode, seq, ie_len;
+	unsigned char *sa, *p;
+	u16 algorithm;
+	int	status;
+	static struct sta_info stat;
+	struct	sta_info *pstat = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 offset = 0;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	sa = GetAddr2Ptr(pframe);
+
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+
+	if (GetPrivacy(pframe)) {
+		u8 *iv;
+		struct rx_pkt_attrib	 *prxattrib = &(precv_frame->u.hdr.attrib);
+
+		prxattrib->hdrlen = WLAN_HDR_A3_LEN;
+		prxattrib->encrypt = _WEP40_;
+
+		iv = pframe+prxattrib->hdrlen;
+		prxattrib->key_index = ((iv[3]>>6)&0x3);
+
+		prxattrib->iv_len = 4;
+		prxattrib->icv_len = 4;
+
+		rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+
+		offset = 4;
+	}
+
+	algorithm = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
+	seq	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
+
+	if (auth_mode == 2 &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	/*  rx a shared-key auth but shared not enabled */
+		(algorithm == 0 && auth_mode == 1)) {	/*  rx a open-system auth but shared-key is enabled */
+
+		status = WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
+
+		goto auth_fail;
+	}
+
+	if (rtw_access_ctrl(padapter, sa) == false) {
+		status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (!pstat) {
+
+		/*  allocate a new one */
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (!pstat) {
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+			goto auth_fail;
+		}
+
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+
+		/* pstat->flags = 0; */
+		/* pstat->capability = 0; */
+	} else {
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		if (list_empty(&pstat->asoc_list) == false) {
+			list_del_init(&pstat->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			if (pstat->expire_to > 0) {
+				/* TODO: STA re_auth within expire_to */
+			}
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		if (seq == 1) {
+			/* TODO: STA re_auth and auth timeout */
+		}
+	}
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (list_empty(&pstat->auth_list)) {
+
+		list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+		pstapriv->auth_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+
+	if ((pstat->auth_seq + 1) != seq) {
+		status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
+		goto auth_fail;
+	}
+
+	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3)) {
+		if (seq == 1) {
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_SUCCESS;
+			pstat->expire_to = pstapriv->assoc_to;
+			pstat->authalg = algorithm;
+		} else {
+			status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
+			goto auth_fail;
+		}
+	} else { /*  shared system or auto authentication */
+		if (seq == 1) {
+			/* prepare for the challenging txt... */
+			memset((void *)pstat->chg_txt, 78, 128);
+
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_STATE;
+			pstat->authalg = algorithm;
+		} else if (seq == 3) {
+
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, WLAN_EID_CHALLENGE, (int *)&ie_len,
+					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if (!p || ie_len <= 0) {
+				status = WLAN_STATUS_CHALLENGE_FAIL;
+				goto auth_fail;
+			}
+
+			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+				pstat->state &= (~WIFI_FW_AUTH_STATE);
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				/*  challenging txt is correct... */
+				pstat->expire_to =  pstapriv->assoc_to;
+			} else {
+				status = WLAN_STATUS_CHALLENGE_FAIL;
+				goto auth_fail;
+			}
+		} else {
+			status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
+			goto auth_fail;
+		}
+	}
+
+
+	/*  Now, we are going to issue_auth... */
+	pstat->auth_seq = seq + 1;
+
+	issue_auth(padapter, pstat, (unsigned short)(WLAN_STATUS_SUCCESS));
+
+	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
+		pstat->auth_seq = 0;
+
+
+	return _SUCCESS;
+
+auth_fail:
+
+	if (pstat)
+		rtw_free_stainfo(padapter, pstat);
+
+	pstat = &stat;
+	memset((char *)pstat, '\0', sizeof(stat));
+	pstat->auth_seq = 2;
+	memcpy(pstat->hwaddr, sa, 6);
+
+	issue_auth(padapter, pstat, (unsigned short)status);
+
+	return _FAIL;
+
+}
+
+unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, offset;
+	unsigned char *p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+	seq	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+			/* pmlmeinfo->reauth_count = 0; */
+		}
+
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			 /*  legendary shared system */
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, WLAN_EID_CHALLENGE, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (!p)
+				goto authclnt_fail;
+
+			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		}
+		/* open system */
+		go2asoc = 1;
+	} else if (seq == 4) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+			go2asoc = 1;
+		else
+			goto authclnt_fail;
+	} else {
+		/*  this is also illegal */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		netdev_dbg(padapter->pnetdev, "auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+
+authclnt_fail:
+
+	/* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */
+
+	return _FAIL;
+
+}
+
+unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u16 capab_info;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info *pstat;
+	unsigned char 	*p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	unsigned char 	supportRate[16];
+	int					supportRateNum;
+	unsigned short		status = WLAN_STATUS_SUCCESS;
+	unsigned short		frame_type, ie_offset = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex	*cur = &(pmlmeinfo->network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ)
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	else /*  WIFI_REASSOCREQ */
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+
+
+	if (pkt_len < sizeof(struct ieee80211_hdr_3addr) + ie_offset)
+		return _FAIL;
+
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (!pstat) {
+		status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
+		goto asoc_class2_error;
+	}
+
+	capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);
+	/* capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); */
+
+	left = pkt_len - (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
+	pos = pframe + (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
+
+	/*  check if this stat has been successfully authenticated/assocated */
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+			status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
+			goto asoc_class2_error;
+		} else {
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;
+		}
+	} else {
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+
+
+	pstat->capability = capab_info;
+
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		status = WLAN_STATUS_CHALLENGE_FAIL;
+		goto OnAssocReqFail;
+	}
+
+	/*  now we should check all the fields... */
+	/*  checking SSID */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_SSID, &ie_len,
+		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+
+	if (!p || ie_len == 0) {
+		/*  broadcast ssid, however it is not allowed in assocreq */
+		status = WLAN_STATUS_CHALLENGE_FAIL;
+		goto OnAssocReqFail;
+	} else {
+		/*  check if ssid match */
+		if (memcmp((void *)(p+2), cur->ssid.ssid, cur->ssid.ssid_length))
+			status = WLAN_STATUS_CHALLENGE_FAIL;
+
+		if (ie_len != cur->ssid.ssid_length)
+			status = WLAN_STATUS_CHALLENGE_FAIL;
+	}
+
+	if (status != WLAN_STATUS_SUCCESS)
+		goto OnAssocReqFail;
+
+	/*  check if the supported rate is ok */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_SUPP_RATES, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (!p) {
+		/*  use our own rate set as statoin used */
+		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+		/* supportRateNum = AP_BSSRATE_LEN; */
+
+		status = WLAN_STATUS_CHALLENGE_FAIL;
+		goto OnAssocReqFail;
+	} else {
+		memcpy(supportRate, p+2, ie_len);
+		supportRateNum = ie_len;
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_EXT_SUPP_RATES, &ie_len,
+				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+		if (p) {
+
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate+supportRateNum, p+2, ie_len);
+				supportRateNum += ie_len;
+			}
+		}
+	}
+
+	/* todo: mask supportRate between AP & STA -> move to update raid */
+	/* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
+
+	/* update station supportRate */
+	pstat->bssratelen = supportRateNum;
+	memcpy(pstat->bssrateset, supportRate, supportRateNum);
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	/* check RSN/WPA/WPS */
+	pstat->dot8021xalg = 0;
+	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
+			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
+
+			if (!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
+
+			if (!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
+			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
+
+			if (!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
+
+			if (!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
+
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if (status != WLAN_STATUS_SUCCESS)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	if (!wpa_ie) {
+		if (elems.wps_ie) {
+			pstat->flags |= WLAN_STA_WPS;
+			/* wpabuf_free(sta->wps_ie); */
+			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+			/* 				elems.wps_ie_len - 4); */
+		} else {
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+		}
+
+
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  that the selected registrar of AP is _FLASE */
+		if ((psecuritypriv->wpa_psk > 0)
+			&& (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+			if (pmlmepriv->wps_beacon_ie) {
+				u8 selected_registrar = 0;
+
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL);
+
+				if (!selected_registrar) {
+					status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+
+	} else {
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+
+		}
+
+		if (elems.wps_ie) {
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len = 0;
+		} else {
+			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
+		}
+
+
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
+
+	}
+
+
+	/*  check if there is WMM IE & support WWM-PS */
+	pstat->flags &= ~WLAN_STA_WME;
+	pstat->qos_option = 0;
+	pstat->qos_info = 0;
+	pstat->has_legacy_ac = true;
+	pstat->uapsd_vo = 0;
+	pstat->uapsd_vi = 0;
+	pstat->uapsd_be = 0;
+	pstat->uapsd_bk = 0;
+	if (pmlmepriv->qospriv.qos_option) {
+		p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
+		for (;;) {
+			p = rtw_get_ie(p, WLAN_EID_VENDOR_SPECIFIC, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+			if (p) {
+				if (!memcmp(p+2, WMM_IE, 6)) {
+
+					pstat->flags |= WLAN_STA_WME;
+
+					pstat->qos_option = 1;
+					pstat->qos_info = *(p+8);
+
+					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+
+					if ((pstat->qos_info&0xf) != 0xf)
+						pstat->has_legacy_ac = true;
+					else
+						pstat->has_legacy_ac = false;
+
+					if (pstat->qos_info&0xf) {
+						if (pstat->qos_info&BIT(0))
+							pstat->uapsd_vo = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vo = 0;
+
+						if (pstat->qos_info&BIT(1))
+							pstat->uapsd_vi = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vi = 0;
+
+						if (pstat->qos_info&BIT(2))
+							pstat->uapsd_bk = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_bk = 0;
+
+						if (pstat->qos_info&BIT(3))
+							pstat->uapsd_be = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_be = 0;
+
+					}
+
+					break;
+				}
+			} else {
+				break;
+			}
+			p = p + ie_len + 2;
+		}
+	}
+
+	/* save HT capabilities in the sta object */
+	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap)) {
+		pstat->flags |= WLAN_STA_HT;
+
+		pstat->flags |= WLAN_STA_WME;
+
+		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
+
+	} else
+		pstat->flags &= ~WLAN_STA_HT;
+
+
+	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT)) {
+		status = WLAN_STATUS_CHALLENGE_FAIL;
+		goto OnAssocReqFail;
+	}
+
+
+	if ((pstat->flags & WLAN_STA_HT) &&
+		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
+		/* status = WLAN_STATUS_CIPHER_SUITE_REJECTED; */
+		/* goto OnAssocReqFail; */
+	}
+	pstat->flags |= WLAN_STA_NONERP;
+	for (i = 0; i < pstat->bssratelen; i++) {
+		if ((pstat->bssrateset[i] & 0x7f) > 22) {
+			pstat->flags &= ~WLAN_STA_NONERP;
+			break;
+		}
+	}
+
+	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+	else
+		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+
+
+	if (status != WLAN_STATUS_SUCCESS)
+		goto OnAssocReqFail;
+
+	/* TODO: identify_proprietary_vendor_ie(); */
+	/*  Realtek proprietary IE */
+	/*  identify if this is Broadcom sta */
+	/*  identify if this is ralink sta */
+	/*  Customer proprietary IE */
+
+
+
+	/* get a unique AID */
+	if (pstat->aid == 0) {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+			if (!pstapriv->sta_aid[pstat->aid - 1])
+				break;
+
+		/* if (pstat->aid > NUM_STA) { */
+		if (pstat->aid > pstapriv->max_num_sta) {
+
+			pstat->aid = 0;
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+			goto OnAssocReqFail;
+
+
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+		}
+	}
+
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&pstat->auth_list)) {
+		list_del_init(&pstat->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	if (list_empty(&pstat->asoc_list)) {
+		pstat->expire_to = pstapriv->expire_to;
+		list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+		pstapriv->asoc_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	/*  now the station is qualified to join our BSS... */
+	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (status == WLAN_STATUS_SUCCESS)) {
+		/* 1 bss_cap_update & sta_info_update */
+		bss_cap_update_on_sta_join(padapter, pstat);
+		sta_info_update(padapter, pstat);
+
+		/* 2 issue assoc rsp before notify station join event. */
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+		spin_lock_bh(&pstat->lock);
+		kfree(pstat->passoc_req);
+		pstat->assoc_req_len = 0;
+		pstat->passoc_req =  rtw_zmalloc(pkt_len);
+		if (pstat->passoc_req) {
+			memcpy(pstat->passoc_req, pframe, pkt_len);
+			pstat->assoc_req_len = pkt_len;
+		}
+		spin_unlock_bh(&pstat->lock);
+
+		/* 3-(1) report sta add event */
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+	return _FAIL;
+}
+
+unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	struct ndis_80211_var_ie *pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct wlan_bssid_ex			*cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	/* status */
+	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+	if (status > 0) {
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	/* get capabilities */
+	pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	/* set slot time */
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+	/* AID */
+	res = pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
+
+	/* following are moved to join event callback function */
+	/* to handle HT, WMM, rate adaptive, update MAC reg */
+	/* for not to handle the synchronous IO in the tasklet */
+	for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
+		pIE = (struct ndis_80211_var_ie *)(pframe + i);
+
+		switch (pIE->element_id) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6))	/* WMM */
+				WMM_param_handler(padapter, pIE);
+			break;
+
+		case WLAN_EID_HT_CAPABILITY:	/* HT caps */
+			HT_caps_handler(padapter, pIE);
+			break;
+
+		case WLAN_EID_HT_OPERATION:	/* HT info */
+			HT_info_handler(padapter, pIE);
+			break;
+
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler(padapter, pIE);
+			break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->length + 2);
+	}
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+	UpdateBrateTbl(padapter, pmlmeinfo->network.supported_rates);
+
+report_assoc_result:
+	if (res > 0)
+		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+unsigned int OnDeAuth(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	int ignore_received_deauth = 0;
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* rtw_free_stainfo(padapter, psta); */
+
+		netdev_dbg(padapter->pnetdev,
+			   "ap recv deauth reason code(%d) sta:%pM\n", reason,
+			   GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (list_empty(&psta->asoc_list) == false) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+
+		return _SUCCESS;
+	}
+
+	/* 	Commented by Albert 20130604 */
+	/* 	Before sending the auth frame to start the STA/GC mode connection with AP/GO, */
+	/* 	we will send the deauth first. */
+	/* 	However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth. */
+	/* 	Added the following code to avoid this case. */
+	if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) ||
+	    (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)) {
+		if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA) {
+			ignore_received_deauth = 1;
+		} else if (reason == WLAN_REASON_PREV_AUTH_NOT_VALID) {
+			/*  TODO: 802.11r */
+			ignore_received_deauth = 1;
+		}
+	}
+
+	netdev_dbg(padapter->pnetdev,
+		   "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
+		   reason, GetAddr3Ptr(pframe),
+		   ignore_received_deauth);
+
+	if (ignore_received_deauth == 0)
+		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+}
+
+unsigned int OnDisassoc(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* rtw_free_stainfo(padapter, psta); */
+
+		netdev_dbg(padapter->pnetdev,
+			   "ap recv disassoc reason code(%d) sta:%pM\n",
+			   reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (list_empty(&psta->asoc_list) == false) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	}
+	netdev_dbg(padapter->pnetdev,
+		   "sta recv disassoc reason code(%d) sta:%pM\n",
+		   reason, GetAddr3Ptr(pframe));
+
+	receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+
+}
+
+unsigned int OnAtim(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+	if (!psta)
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case WLAN_ACTION_SPCT_MSR_REQ:
+	case WLAN_ACTION_SPCT_MSR_RPRT:
+	case WLAN_ACTION_SPCT_TPC_REQ:
+	case WLAN_ACTION_SPCT_TPC_RPRT:
+	case WLAN_ACTION_SPCT_CHL_SWITCH:
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return _FAIL;
+}
+
+unsigned int OnAction_back(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *addr;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char 	*frame_body;
+	unsigned char 	category, action;
+	unsigned short	tid, status;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	addr = GetAddr2Ptr(pframe);
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (!psta)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_BACK) {/*  representing Block Ack */
+		if (!pmlmeinfo->HT_enable)
+			return _SUCCESS;
+
+		action = frame_body[1];
+		switch (action) {
+		case WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
+
+			memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+			/* process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe)); */
+			process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
+
+			if (pmlmeinfo->accept_addba_req)
+				issue_action_BA(padapter, addr, WLAN_ACTION_ADDBA_RESP, 0);
+			else
+				issue_action_BA(padapter, addr, WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
+
+			break;
+
+		case WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+			status = get_unaligned_le16(&frame_body[3]);
+			tid = ((frame_body[5] >> 2) & 0x7);
+
+			if (status == 0) {
+				/* successful */
+				psta->htpriv.agg_enable_bitmap |= BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			} else {
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			}
+
+			if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->expire_to = pstapriv->expire_to;
+				psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+			}
+
+			break;
+
+		case WLAN_ACTION_DELBA: /* DELBA */
+			if ((frame_body[3] & BIT(3)) == 0) {
+				psta->htpriv.agg_enable_bitmap &=
+					~BIT((frame_body[3] >> 4) & 0xf);
+				psta->htpriv.candidate_tid_bitmap &=
+					~BIT((frame_body[3] >> 4) & 0xf);
+			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
+				tid = (frame_body[3] >> 4) & 0x0F;
+
+				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+			}
+			/* todo: how to notify the host while receiving DELETE BA */
+			break;
+
+		default:
+			break;
+		}
+	}
+	return _SUCCESS;
+}
+
+static s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
+{
+	struct adapter *adapter = recv_frame->u.hdr.adapter;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	u8 *frame = recv_frame->u.hdr.rx_data;
+	u16 seq_ctrl = ((recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(recv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (GetRetry(frame)) {
+		if (token >= 0) {
+			if ((seq_ctrl == mlmeext->action_public_rxseq)
+				&& (token == mlmeext->action_public_dialog_token))
+				return _FAIL;
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq)
+				return _FAIL;
+		}
+	}
+
+	mlmeext->action_public_rxseq = seq_ctrl;
+
+	if (token >= 0)
+		mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body;
+	u8 dialogToken = 0;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+
+	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+
+	if (!memcmp(frame_body + 2, P2P_OUI, 4))
+		ret = on_action_public_p2p(precv_frame);
+
+	return ret;
+}
+
+static unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 token;
+	struct adapter *adapter = precv_frame->u.hdr.adapter;
+	char msg[64];
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	scnprintf(msg, sizeof(msg), "%s(token:%u)", action_public_str(action), token);
+	rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame, action);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_ht(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_HT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case WLAN_HT_ACTION_COMPRESSED_BF:
+		break;
+	default:
+		break;
+	}
+
+exit:
+
+	return _SUCCESS;
+}
+
+unsigned int OnAction_sa_query(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	unsigned short tid;
+
+	switch (pframe[WLAN_HDR_A3_LEN+1]) {
+	case 0: /* SA Query req */
+		memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short));
+		issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
+		break;
+
+	case 1: /* SA Query rsp */
+		del_timer_sync(&pmlmeext->sa_query_timer);
+		break;
+	default:
+		break;
+	}
+	if (0) {
+		int pp;
+
+		printk("pattrib->pktlen = %d =>", pattrib->pkt_len);
+		for (pp = 0; pp < pattrib->pkt_len; pp++)
+			printk(" %02x ", pframe[pp]);
+		printk("\n");
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int OnAction(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int i;
+	unsigned char category;
+	struct action_handler *ptable;
+	unsigned char *frame_body;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+
+	for (i = 0; i < ARRAY_SIZE(OnAction_tbl); i++) {
+		ptable = &OnAction_tbl[i];
+
+		if (category == ptable->num)
+			ptable->func(padapter, precv_frame);
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int DoReserved(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
+{
+	struct xmit_frame *pmgntframe;
+	struct xmit_buf *pxmitbuf;
+
+	if (once)
+		pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
+	else
+		pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
+
+	if (!pmgntframe)
+		goto exit;
+
+	pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+	if (!pxmitbuf) {
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		pmgntframe = NULL;
+		goto exit;
+	}
+
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+
+exit:
+	return pmgntframe;
+
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, false);
+}
+
+/****************************************************************************
+
+Following are some TX functions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	pmlmeext->tx_rate = rate;
+}
+
+void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+	u8 wireless_mode;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	/* memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib)); */
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+	pattrib->mac_id = 0;
+	pattrib->qsel = 0x12;
+
+	pattrib->pktlen = 0;
+
+	if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB)
+		wireless_mode = WIRELESS_11B;
+	else
+		wireless_mode = WIRELESS_11G;
+	pattrib->raid =  rtw_get_mgntframe_raid(padapter, wireless_mode);
+	pattrib->rate = pmlmeext->tx_rate;
+
+	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->bswenc = false;
+
+	pattrib->qos_en = false;
+	pattrib->ht_en = false;
+	pattrib->bwmode = CHANNEL_WIDTH_20;
+	pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pattrib->sgi = false;
+
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+
+	pattrib->retry_ctrl = true;
+
+	pattrib->mbssid = 0;
+
+}
+
+void update_mgntframe_attrib_addr(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	u8 *pframe;
+	struct pkt_attrib	*pattrib = &pmgntframe->attrib;
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+	memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
+	memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN);
+}
+
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if (padapter->bSurpriseRemoved ||
+		padapter->bDriverStopped) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return;
+	}
+
+	rtw_hal_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	unsigned long irqL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (padapter->bSurpriseRemoved ||
+		padapter->bDriverStopped) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return ret;
+	}
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx);
+
+	spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
+	pxmitbuf->sctx = NULL;
+	spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
+
+	return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	static u8 seq_no;
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;/*   500ms */
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->bSurpriseRemoved ||
+		padapter->bDriverStopped) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return -1;
+	}
+
+	if (mutex_lock_interruptible(&pxmitpriv->ack_tx_mutex) == 0) {
+		pxmitpriv->ack_tx = true;
+		pxmitpriv->seq_no = seq_no++;
+		pmgntframe->ack_report = 1;
+		if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
+			ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
+
+		pxmitpriv->ack_tx = false;
+		mutex_unlock(&pxmitpriv->ack_tx_mutex);
+	}
+
+	return ret;
+}
+
+static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+	u8 *ssid_ie;
+	signed int ssid_len_ori;
+	int len_diff = 0;
+
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	if (ssid_ie && ssid_len_ori > 0) {
+		switch (hidden_ssid_mode) {
+		case 1:
+		{
+			u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
+			u32 remain_len = 0;
+
+			remain_len = ies_len - (next_ie-ies);
+
+			ssid_ie[1] = 0;
+			memcpy(ssid_ie+2, next_ie, remain_len);
+			len_diff -= ssid_len_ori;
+
+			break;
+		}
+		case 2:
+			memset(&ssid_ie[2], 0, ssid_len_ori);
+			break;
+		default:
+			break;
+	}
+	}
+
+	return len_diff;
+}
+
+void issue_beacon(struct adapter *padapter, int timeout_ms)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned int	rate_len;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	eth_broadcast_addr(pwlanhdr->addr1);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		{
+			int len_diff;
+
+			memcpy(pframe, cur_network->ies, cur_network->ie_length);
+			len_diff = update_hidden_ssid(pframe+_BEACON_IE_OFFSET_,
+						      cur_network->ie_length-_BEACON_IE_OFFSET_,
+						      pmlmeinfo->hidden_ssid_mode);
+			pframe += (cur_network->ie_length+len_diff);
+			pattrib->pktlen += (cur_network->ie_length+len_diff);
+		}
+
+		{
+			u8 *wps_ie;
+			uint wps_ielen;
+			u8 sr = 0;
+
+			wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
+				pattrib->pktlen-sizeof(struct ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
+			if (wps_ie && wps_ielen > 0)
+				rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+			if (sr != 0)
+				set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			else
+				_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
+		}
+
+		goto _issue_bcn;
+
+	}
+
+	/* below for ad-hoc mode */
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->ies)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  capability info: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->ies)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, WLAN_EID_SSID, cur_network->ssid.ssid_length, cur_network->ssid.ssid, &pattrib->pktlen);
+
+	/*  supported rates... */
+	rate_len = rtw_get_rateset_len(cur_network->supported_rates);
+	pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ? 8 : rate_len), cur_network->supported_rates, &pattrib->pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)&(cur_network->configuration.ds_config), &pattrib->pktlen);
+
+	/* if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */
+	{
+		u8 erpinfo = 0;
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		/* ATIMWindow = cur->configuration.ATIMWindow; */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+		/* ERP IE */
+		pframe = rtw_set_ie(pframe, WLAN_EID_ERP_INFO, 1, &erpinfo, &pattrib->pktlen);
+	}
+
+
+	/*  EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->supported_rates + 8), &pattrib->pktlen);
+
+
+	/* todo:HT for adhoc */
+
+_issue_bcn:
+
+	pmlmepriv->update_bcn = false;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512)
+		return;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (timeout_ms > 0)
+		dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+	else
+		dump_mgntframe(padapter, pmgntframe);
+
+}
+
+void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char 				*mac, *bssid;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	unsigned int	rate_len;
+
+	if (!da)
+		return;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+	bssid = cur_network->mac_address;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+
+	if (cur_network->ie_length > MAX_IE_SZ)
+		return;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		pwps_ie = rtw_get_wps_ie(cur_network->ies+_FIXED_IE_LENGTH_, cur_network->ie_length-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+		/* inerset & update wps_probe_resp_ie */
+		if (pmlmepriv->wps_probe_resp_ie && pwps_ie && wps_ielen > 0) {
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie;
+
+			wps_offset = (uint)(pwps_ie - cur_network->ies);
+
+			premainder_ie = pwps_ie + wps_ielen;
+
+			remainder_ielen = cur_network->ie_length - wps_offset - wps_ielen;
+
+			memcpy(pframe, cur_network->ies, wps_offset);
+			pframe += wps_offset;
+			pattrib->pktlen += wps_offset;
+
+			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
+			if ((wps_offset+wps_ielen+2) <= MAX_IE_SZ) {
+				memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
+				pframe += wps_ielen+2;
+				pattrib->pktlen += wps_ielen+2;
+			}
+
+			if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
+				memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;
+				pattrib->pktlen += remainder_ielen;
+			}
+		} else {
+			memcpy(pframe, cur_network->ies, cur_network->ie_length);
+			pframe += cur_network->ie_length;
+			pattrib->pktlen += cur_network->ie_length;
+		}
+
+		/* retrieve SSID IE from cur_network->ssid */
+		{
+			u8 *ssid_ie;
+			signed int ssid_ielen;
+			signed int ssid_ielen_diff;
+			u8 *buf;
+			u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct ieee80211_hdr_3addr);
+
+			buf = rtw_zmalloc(MAX_IE_SZ);
+			if (!buf)
+				return;
+
+			ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, WLAN_EID_SSID, &ssid_ielen,
+				(pframe-ies)-_FIXED_IE_LENGTH_);
+
+			ssid_ielen_diff = cur_network->ssid.ssid_length - ssid_ielen;
+
+			if (ssid_ie &&  cur_network->ssid.ssid_length) {
+				uint remainder_ielen;
+				u8 *remainder_ie;
+
+				remainder_ie = ssid_ie+2;
+				remainder_ielen = (pframe-remainder_ie);
+
+				if (remainder_ielen > MAX_IE_SZ) {
+					netdev_warn(padapter->pnetdev,
+						    FUNC_ADPT_FMT " remainder_ielen > MAX_IE_SZ\n",
+						    FUNC_ADPT_ARG(padapter));
+					remainder_ielen = MAX_IE_SZ;
+				}
+
+				memcpy(buf, remainder_ie, remainder_ielen);
+				memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
+				*(ssid_ie+1) = cur_network->ssid.ssid_length;
+				memcpy(ssid_ie+2, cur_network->ssid.ssid, cur_network->ssid.ssid_length);
+
+				pframe += ssid_ielen_diff;
+				pattrib->pktlen += ssid_ielen_diff;
+			}
+			kfree(buf);
+		}
+	} else {
+		/* timestamp will be inserted by hardware */
+		pframe += 8;
+		pattrib->pktlen += 8;
+
+		/*  beacon interval: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->ies)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/*  capability info: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->ies)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/* below for ad-hoc mode */
+
+		/*  SSID */
+		pframe = rtw_set_ie(pframe, WLAN_EID_SSID, cur_network->ssid.ssid_length, cur_network->ssid.ssid, &pattrib->pktlen);
+
+		/*  supported rates... */
+		rate_len = rtw_get_rateset_len(cur_network->supported_rates);
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ? 8 : rate_len), cur_network->supported_rates, &pattrib->pktlen);
+
+		/*  DS parameter set */
+		pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)&(cur_network->configuration.ds_config), &pattrib->pktlen);
+
+		if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+			u8 erpinfo = 0;
+			u32 ATIMWindow;
+			/*  IBSS Parameter Set... */
+			/* ATIMWindow = cur->configuration.ATIMWindow; */
+			ATIMWindow = 0;
+			pframe = rtw_set_ie(pframe, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+			/* ERP IE */
+			pframe = rtw_set_ie(pframe, WLAN_EID_ERP_INFO, 1, &erpinfo, &pattrib->pktlen);
+		}
+
+
+		/*  EXTERNDED SUPPORTED RATE */
+		if (rate_len > 8)
+			pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->supported_rates + 8), &pattrib->pktlen);
+
+
+		/* todo:HT for adhoc */
+
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+static int _issue_probereq(struct adapter *padapter,
+			   struct ndis_802_11_ssid *pssid,
+			   u8 *da, u8 ch, bool append_wps, bool wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char 		*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char 		*mac;
+	unsigned char 		bssrate[NumRates];
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	int	bssrate_len = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if (da) {
+		/* 	unicast probe request frame */
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		/* 	broadcast probe request frame */
+		eth_broadcast_addr(pwlanhdr->addr1);
+		eth_broadcast_addr(pwlanhdr->addr3);
+	}
+
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if (pssid)
+		pframe = rtw_set_ie(pframe, WLAN_EID_SSID, pssid->ssid_length, pssid->ssid, &(pattrib->pktlen));
+	else
+		pframe = rtw_set_ie(pframe, WLAN_EID_SSID, 0, NULL, &(pattrib->pktlen));
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	} else {
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, bssrate_len, bssrate, &(pattrib->pktlen));
+	}
+
+	if (ch)
+		pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, &ch, &pattrib->pktlen);
+
+	if (append_wps) {
+		/* add wps_ie for wps2.0 */
+		if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
+			memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+			pframe += pmlmepriv->wps_probe_req_ie_len;
+			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, 0, 1, false);
+}
+
+int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, u8 ch, bool append_wps,
+	int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_probereq(padapter, pssid, da, ch, append_wps,
+				      wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+/*  if psta == NULL, indicate we are station(client) now... */
+void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned int					val32;
+	unsigned short				val16;
+	int use_shared_key = 0;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	__le16 le_tmp;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_AUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+
+	if (psta) { /*  for AP mode */
+		memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (u16)psta->authalg;
+
+		if (status != WLAN_STATUS_SUCCESS)
+			val16 = 0;
+
+		if (val16)
+			use_shared_key = 1;
+
+		le_tmp = cpu_to_le16(val16);
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		val16 = (u16)psta->auth_seq;
+		le_tmp = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting status code... */
+		val16 = status;
+		le_tmp = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  added challenging text... */
+		if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
+			pframe = rtw_set_ie(pframe, WLAN_EID_CHALLENGE, 128, psta->chg_txt, &(pattrib->pktlen));
+
+	} else {
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/*  0:OPEN System, 1:Shared key */
+		if (val16)
+			use_shared_key = 1;
+		le_tmp = cpu_to_le16(val16);
+
+		/* setting IV for auth seq #3 */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			__le32 le_tmp32;
+
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			le_tmp32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
+
+			pattrib->iv_len = 4;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		le_tmp = cpu_to_le16(pmlmeinfo->auth_seq);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+
+		/*  setting status code... */
+		le_tmp = cpu_to_le16(status);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  then checking to see if sending challenging text... */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			pframe = rtw_set_ie(pframe, WLAN_EID_CHALLENGE, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
+
+			SetPrivacy(fctrl);
+
+			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+
+			pattrib->encrypt = _WEP40_;
+
+			pattrib->icv_len = 4;
+
+			pattrib->pktlen += pattrib->icv_len;
+
+		}
+
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+	struct xmit_frame	*pmgntframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	struct pkt_attrib *pattrib;
+	unsigned char *pbuf, *pframe;
+	unsigned short val;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->ies;
+	__le16 lestatus, le_tmp;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+		SetFrameSubType(pwlanhdr, pkt_type);
+	else
+		return;
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen += pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* capability */
+	val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+	pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_, (unsigned char *)&val, &(pattrib->pktlen));
+
+	lestatus = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&lestatus, &(pattrib->pktlen));
+
+	le_tmp = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+	if (pstat->bssratelen <= 8) {
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
+	} else {
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, 8, pstat->bssrateset, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
+	}
+
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+		uint ie_len = 0;
+
+		/* FILL HT CAP INFO IE */
+		/* p = hostapd_eid_ht_capabilities_info(hapd, p); */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_CAPABILITY, &ie_len, (pnetwork->ie_length - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen += (ie_len+2);
+		}
+
+		/* FILL HT ADD INFO IE */
+		/* p = hostapd_eid_ht_operation(hapd, p); */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_OPERATION, &ie_len, (pnetwork->ie_length - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen += (ie_len+2);
+		}
+
+	}
+
+	/* FILL WMM IE */
+	if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) {
+		uint ie_len = 0;
+		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+		for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) {
+			pbuf = rtw_get_ie(pbuf, WLAN_EID_VENDOR_SPECIFIC, &ie_len, (pnetwork->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+			if (pbuf && !memcmp(pbuf+2, WMM_PARA_IE, 6)) {
+				memcpy(pframe, pbuf, ie_len+2);
+				pframe += (ie_len+2);
+				pattrib->pktlen += (ie_len+2);
+
+				break;
+			}
+
+			if (!pbuf || ie_len == 0)
+				break;
+		}
+
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, 6, REALTEK_96B_IE, &(pattrib->pktlen));
+
+	/* add WPS IE ie for wps 2.0 */
+	if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) {
+		memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
+
+		pframe += pmlmepriv->wps_assoc_resp_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_assocreq(struct adapter *padapter)
+{
+	int ret = _FAIL;
+	struct xmit_frame				*pmgntframe;
+	struct pkt_attrib				*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr			*pwlanhdr;
+	__le16 *fctrl;
+	__le16 val16;
+	unsigned int					i, j, index = 0;
+	unsigned char bssrate[NumRates], sta_bssrate[NumRates];
+	struct ndis_80211_var_ie *pIE;
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0, sta_bssrate_len = 0;
+	u8 vs_ie_length = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ASSOCREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* caps */
+	memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.ies), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* listen interval */
+	/* todo: listen interval for power saving */
+	val16 = cpu_to_le16(3);
+	memcpy(pframe, (unsigned char *)&val16, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, WLAN_EID_SSID,  pmlmeinfo->network.ssid.ssid_length, pmlmeinfo->network.ssid.ssid, &(pattrib->pktlen));
+
+	/* supported rate & extended supported rate */
+
+	/*  Check if the AP's supported rates are also supported by STA. */
+	get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+
+	if (pmlmeext->cur_channel == 14) /*  for JAPAN, channel 14 can only uses B Mode(CCK) */
+		sta_bssrate_len = 4;
+
+
+	/* for (i = 0; i < sta_bssrate_len; i++) { */
+	/*  */
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.supported_rates[i] == 0)
+			break;
+	}
+
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.supported_rates[i] == 0)
+			break;
+
+
+		/*  Check if the AP's supported rates are also supported by STA. */
+		for (j = 0; j < sta_bssrate_len; j++) {
+			 /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
+			if ((pmlmeinfo->network.supported_rates[i] | IEEE80211_BASIC_RATE_MASK)
+					== (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK))
+				break;
+		}
+
+		if (j != sta_bssrate_len)
+			/*  the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.supported_rates[i];
+	}
+
+	bssrate_len = index;
+
+	if (bssrate_len == 0) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit; /* don't connect to AP if no joint supported rate */
+	}
+
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	} else
+		pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, bssrate_len, bssrate, &(pattrib->pktlen));
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.ie_length;) {
+		pIE = (struct ndis_80211_var_ie *)(pmlmeinfo->network.ies + i);
+
+		switch (pIE->element_id) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
+					(!memcmp(pIE->data, WMM_OUI, 4)) ||
+					(!memcmp(pIE->data, WPS_OUI, 4))) {
+				vs_ie_length = pIE->length;
+				if ((!padapter->registrypriv.wifi_spec) && (!memcmp(pIE->data, WPS_OUI, 4))) {
+					/* Commented by Kurt 20110629
+					 * In some older APs, WPS handshake
+					 * would be fail if we append vendor
+					 * extensions information to AP
+					 */
+
+					vs_ie_length = 14;
+				}
+
+				pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, vs_ie_length, pIE->data, &(pattrib->pktlen));
+			}
+			break;
+
+		case WLAN_EID_RSN:
+			pframe = rtw_set_ie(pframe, WLAN_EID_RSN, pIE->length, pIE->data, &(pattrib->pktlen));
+			break;
+		case WLAN_EID_HT_CAPABILITY:
+			if (padapter->mlmepriv.htpriv.ht_option) {
+				if (!(is_ap_in_tkip(padapter))) {
+					memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
+					pframe = rtw_set_ie(pframe, WLAN_EID_HT_CAPABILITY, pIE->length, (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+				}
+			}
+			break;
+
+		case WLAN_EID_EXT_CAPABILITY:
+			if (padapter->mlmepriv.htpriv.ht_option)
+				pframe = rtw_set_ie(pframe, WLAN_EID_EXT_CAPABILITY, pIE->length, pIE->data, &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->length + 2);
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, 6, REALTEK_96B_IE, &(pattrib->pktlen));
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+
+exit:
+	if (ret == _SUCCESS)
+		rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+}
+
+/* when wait_ack is true, this function should be called at process context */
+static int _issue_nulldata(struct adapter *padapter, unsigned char *da,
+			   unsigned int power_mode, bool wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+	if (!padapter)
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (power_mode)
+		SetPwrMgt(fctrl);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/*
+ * [IMPORTANT] Don't call this function in interrupt context
+ *
+ * When wait_ms > 0, this function should be called at process context
+ * da == NULL for station mode
+ */
+int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info *psta;
+
+
+	/* da == NULL, assume it's null data for sta to ap*/
+	if (!da)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	psta = rtw_get_stainfo(&padapter->stapriv, da);
+	if (psta) {
+		if (power_mode)
+			rtw_hal_macid_sleep(padapter, psta->mac_id);
+		else
+			rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	} else {
+		rtw_warn_on(1);
+	}
+
+	do {
+		ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+/*
+ * [IMPORTANT] This function run in interrupt context
+ *
+ * The null data packet would be sent without power bit,
+ * and not guarantee success.
+ */
+s32 issue_nulldata_in_interrupt(struct adapter *padapter, u8 *da)
+{
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* da == NULL, assume it's null data for sta to ap*/
+	if (!da)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	return _issue_nulldata(padapter, da, 0, false);
+}
+
+/* when wait_ack is true, this function should be called at process context */
+static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da,
+			       u16 tid, bool wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	u16 *qc;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	pattrib->hdrlen += 2;
+	pattrib->qos_en = true;
+	pattrib->eosp = 1;
+	pattrib->ack_policy = 0;
+	pattrib->mdata = 0;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+
+	SetPriority(qc, tid);
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_qos_hdr);
+	pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/* when wait_ms >0 , this function should be called at process context */
+/* da == NULL for station mode */
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* da == NULL, assume it's null data for sta to ap*/
+	if (!da)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	do {
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+static int _issue_deauth(struct adapter *padapter, unsigned char *da,
+			 unsigned short reason, bool wait_ack)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	int ret = _FAIL;
+	__le16 le_tmp;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DEAUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	le_tmp = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	return _issue_deauth(padapter, da, reason, false);
+}
+
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+	int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_deauth(padapter, da, reason, wait_ms > 0);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			mdelay(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+void issue_action_SA_Query(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid)
+{
+	u8 category = RTW_WLAN_CATEGORY_SA_QUERY;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8 			*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	__le16 le_tmp;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if (raddr)
+		memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	else
+		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
+	pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
+
+	switch (action) {
+	case 0: /* SA Query req */
+		pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen);
+		pmlmeext->sa_query_seq++;
+		/* send sa query request to AP, AP should reply sa query response in 1 second */
+		set_sa_query_timer(pmlmeext, 1000);
+		break;
+
+	case 1: /* SA Query rsp */
+		le_tmp = cpu_to_le16(tid);
+		pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&le_tmp, &pattrib->pktlen);
+		break;
+	default:
+		break;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
+{
+	u8 category = RTW_WLAN_CATEGORY_BACK;
+	u16 start_seq;
+	u16 BA_para_set;
+	u16 reason_code;
+	u16 BA_timeout_value;
+	u16 BA_starting_seqctrl = 0;
+	enum ieee80211_max_ampdu_length_exp max_rx_ampdu_factor;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8 			*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info 	*psta;
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	struct registry_priv 	*pregpriv = &padapter->registrypriv;
+	__le16 le_tmp;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	/* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+	if (category == 3) {
+		switch (action) {
+		case 0: /* ADDBA req */
+			do {
+				pmlmeinfo->dialogToken++;
+			} while (pmlmeinfo->dialogToken == 0);
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
+
+			if (hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter)) {
+				/*  A-MSDU NOT Supported */
+				BA_para_set = 0;
+				/*  immediate Block Ack */
+				BA_para_set |= BIT(1) & IEEE80211_ADDBA_PARAM_POLICY_MASK;
+				/*  TID */
+				BA_para_set |= (status << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
+				/*  max buffer size is 8 MSDU */
+				BA_para_set |= (8 << 6) & IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+			} else {
+				BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
+			}
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			BA_timeout_value = 5000;/*  5ms */
+			le_tmp = cpu_to_le16(BA_timeout_value);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			/* if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.mac_address)) != NULL) */
+			psta = rtw_get_stainfo(pstapriv, raddr);
+			if (psta) {
+				start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
+
+				psta->BA_starting_seqctrl[status & 0x07] = start_seq;
+
+				BA_starting_seqctrl = start_seq << 4;
+			}
+
+			le_tmp = cpu_to_le16(BA_starting_seqctrl);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			break;
+
+		case 1: /* ADDBA rsp */
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
+			if (padapter->driver_rx_ampdu_factor != 0xFF)
+				max_rx_ampdu_factor =
+				  (enum ieee80211_max_ampdu_length_exp)padapter->driver_rx_ampdu_factor;
+			else
+				rtw_hal_get_def_var(padapter,
+						    HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+
+			if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+			else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */
+			else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_16K)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */
+			else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_8K)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); /* 8 buffer size */
+			else
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+
+			if (hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter) &&
+			    padapter->driver_rx_ampdu_factor == 0xFF) {
+				/*  max buffer size is 8 MSDU */
+				BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+				BA_para_set |= (8 << 6) & IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+			}
+
+			if (pregpriv->ampdu_amsdu == 0)/* disabled */
+				le_tmp = cpu_to_le16(BA_para_set & ~BIT(0));
+			else if (pregpriv->ampdu_amsdu == 1)/* enabled */
+				le_tmp = cpu_to_le16(BA_para_set | BIT(0));
+			else /* auto */
+				le_tmp = cpu_to_le16(BA_para_set);
+
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
+			break;
+		case 2:/* DELBA */
+			BA_para_set = (status & 0x1F) << 3;
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			reason_code = 37;
+			le_tmp = cpu_to_le16(reason_code);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_action_BSSCoexistPacket(struct adapter *padapter)
+{
+	struct list_head		*plist, *phead;
+	unsigned char category, action;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 			*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct __queue		*queue	= &(pmlmepriv->scanned_queue);
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+
+	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+		return;
+
+	if (true == pmlmeinfo->bwmode_updated)
+		return;
+
+	category = RTW_WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+
+	/*  */
+	if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+		u8 iedata = 0;
+
+		iedata |= BIT(2);/* 20 MHz BSS Width Request */
+
+		pframe = rtw_set_ie(pframe, WLAN_EID_BSS_COEX_2040,  1, &iedata, &(pattrib->pktlen));
+
+	}
+
+
+	/*  */
+	memset(ICS, 0, sizeof(ICS));
+	if (pmlmepriv->num_sta_no_ht > 0) {
+		int i;
+
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+
+		while (1) {
+			int len;
+			u8 *p;
+			struct wlan_bssid_ex *pbss_network;
+
+			if (phead == plist)
+				break;
+
+			pnetwork = container_of(plist, struct wlan_network, list);
+
+			plist = get_next(plist);
+
+			pbss_network = (struct wlan_bssid_ex *)&pnetwork->network;
+
+			p = rtw_get_ie(pbss_network->ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_CAPABILITY, &len, pbss_network->ie_length - _FIXED_IE_LENGTH_);
+			if (!p || len == 0) {/* non-HT */
+
+				if (pbss_network->configuration.ds_config <= 0)
+					continue;
+
+				ICS[0][pbss_network->configuration.ds_config] = 1;
+
+				if (ICS[0][0] == 0)
+					ICS[0][0] = 1;
+			}
+
+		}
+
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+
+		for (i = 0; i < 8; i++) {
+			if (ICS[i][0] == 1) {
+				int j, k = 0;
+
+				InfoContent[k] = i;
+				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+				k++;
+
+				for (j = 1; j <= 14; j++) {
+					if (ICS[i][j] == 1) {
+						if (k < 16) {
+							InfoContent[k] = j; /* channel number */
+							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
+							k++;
+						}
+					}
+				}
+
+				pframe = rtw_set_ie(pframe, WLAN_EID_BSS_INTOLERANT_CHL_REPORT, k, InfoContent, &(pattrib->pktlen));
+
+			}
+
+		}
+
+
+	}
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+	/* struct recv_reorder_ctrl *preorder_ctrl; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u16 tid;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if (!psta)
+		return _SUCCESS;
+
+	if (initiator == 0) {/*  recipient */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->recvreorder_ctrl[tid].enable) {
+				issue_action_BA(padapter, addr, WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+				psta->recvreorder_ctrl[tid].enable = false;
+				psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+			}
+		}
+	} else if (initiator == 1) {/*  originator */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+				issue_action_BA(padapter, addr, WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+
+			}
+		}
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int send_beacon(struct adapter *padapter)
+{
+	u8 bxmitok = false;
+	int	issue = 0;
+	int poll = 0;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+	do {
+		issue_beacon(padapter, 100);
+		issue++;
+		do {
+			cond_resched();
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			poll++;
+		} while ((poll%10) != 0 && false == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	} while (false == bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return _FAIL;
+
+	if (!bxmitok)
+		return _FAIL;
+	else
+		return _SUCCESS;
+}
+
+/****************************************************************************
+
+Following are some utility functions for WiFi MLME
+
+*****************************************************************************/
+
+void site_survey(struct adapter *padapter)
+{
+	unsigned char 	survey_channel = 0, val8;
+	enum rt_scan_type	ScanType = SCAN_PASSIVE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 initialgain = 0;
+	u32 channel_scan_time_ms = 0;
+
+	{
+		struct rtw_ieee80211_channel *ch;
+
+		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
+			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+			survey_channel = ch->hw_value;
+			ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+		}
+	}
+
+	if (survey_channel != 0) {
+		/* PAUSE 4-AC Queue when site_survey */
+		/* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* val8 |= 0x0f; */
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		if (pmlmeext->sitesurvey_res.channel_idx == 0) {
+#ifdef DBG_FIXED_CHAN
+			if (pmlmeext->fixed_chan != 0xff)
+				set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+			else
+#endif
+				set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+		} else {
+#ifdef DBG_FIXED_CHAN
+			if (pmlmeext->fixed_chan != 0xff)
+				SelectChannel(padapter, pmlmeext->fixed_chan);
+			else
+#endif
+				SelectChannel(padapter, survey_channel);
+		}
+
+		if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
+			{
+				int i;
+
+				for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+					if (pmlmeext->sitesurvey_res.ssid[i].ssid_length) {
+						/* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
+						if (padapter->registrypriv.wifi_spec)
+							issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+						else
+							issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0);
+						issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+					}
+				}
+
+				if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+					/* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
+					if (padapter->registrypriv.wifi_spec)
+						issue_probereq(padapter, NULL, NULL);
+					else
+						issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0);
+					issue_probereq(padapter, NULL, NULL);
+				}
+			}
+		}
+
+		channel_scan_time_ms = pmlmeext->chan_scan_time;
+
+		set_survey_timer(pmlmeext, channel_scan_time_ms);
+	} else {
+
+		/* 	channel number is 0 or this channel is not valid. */
+
+		{
+			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+			/* switch back to the original channel */
+			/* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
+
+			set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+			/* flush 4-AC Queue after site_survey */
+			/* val8 = 0; */
+			/* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+
+			/* config MSR */
+			Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+			initialgain = 0xff; /* restore RX GAIN */
+			rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+			/* turn on dynamic functions */
+			Restore_DM_Func_Flag(padapter);
+			/* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+
+			if (is_client_associated_to_ap(padapter))
+				issue_nulldata(padapter, NULL, 0, 3, 500);
+
+			val8 = 0; /* survey done */
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+			report_surveydone_event(padapter);
+
+			pmlmeext->chan_scan_time = SURVEY_TO;
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+		}
+	}
+
+	return;
+
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(struct adapter *padapter, union recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+{
+	int	i;
+	u32 len;
+	u8 *p;
+	u16 val16, subtype;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u32 packet_len = precv_frame->u.hdr.len;
+	u8 ie_offset;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	__le32 le32_tmp;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	memset(bssid, 0, sizeof(struct wlan_bssid_ex));
+
+	subtype = GetFrameSubType(pframe);
+
+	if (subtype == WIFI_BEACON) {
+		bssid->reserved[0] = 1;
+		ie_offset = _BEACON_IE_OFFSET_;
+	} else {
+		/*  FIXME : more type */
+		if (subtype == WIFI_PROBERSP) {
+			ie_offset = _PROBERSP_IE_OFFSET_;
+			bssid->reserved[0] = 3;
+		} else if (subtype == WIFI_PROBEREQ) {
+			ie_offset = _PROBEREQ_IE_OFFSET_;
+			bssid->reserved[0] = 2;
+		} else {
+			bssid->reserved[0] = 0;
+			ie_offset = _FIXED_IE_LENGTH_;
+		}
+	}
+
+	bssid->length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->ie_length = len;
+	memcpy(bssid->ies, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->ie_length);
+
+	/* get the signal strength */
+	bssid->rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; /*  in dBM.raw data */
+	bssid->phy_info.signal_quality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->phy_info.signal_strength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;/* in percentage */
+
+	/*  checking SSID */
+	p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_SSID, &len, bssid->ie_length - ie_offset);
+	if (!p)
+		return _FAIL;
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID)
+			return _FAIL;
+
+		memcpy(bssid->ssid.ssid, (p + 2), *(p + 1));
+		bssid->ssid.ssid_length = *(p + 1);
+	} else
+		bssid->ssid.ssid_length = 0;
+
+	memset(bssid->supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	/* checking rate info... */
+	i = 0;
+	p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_SUPP_RATES, &len, bssid->ie_length - ie_offset);
+	if (p) {
+		if (len > NDIS_802_11_LENGTH_RATES_EX)
+			return _FAIL;
+
+		memcpy(bssid->supported_rates, (p + 2), len);
+		i = len;
+	}
+
+	p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_EXT_SUPP_RATES, &len, bssid->ie_length - ie_offset);
+	if (p) {
+		if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
+			return _FAIL;
+
+		memcpy(bssid->supported_rates + i, (p + 2), len);
+	}
+
+	bssid->network_type_in_use = Ndis802_11OFDM24;
+
+	if (bssid->ie_length < 12)
+		return _FAIL;
+
+	/*  Checking for ds_config */
+	p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_DS_PARAMS, &len, bssid->ie_length - ie_offset);
+
+	bssid->configuration.ds_config = 0;
+	bssid->configuration.length = 0;
+
+	if (p) {
+		bssid->configuration.ds_config = *(p + 2);
+	} else {
+		/*  In 5G, some ap do not have DSSET IE */
+		/*  checking HT info for channel */
+		p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_HT_OPERATION, &len, bssid->ie_length - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+
+			bssid->configuration.ds_config = HT_info->primary_channel;
+		} else { /*  use current channel */
+			bssid->configuration.ds_config = rtw_get_oper_ch(padapter);
+		}
+	}
+
+	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->ies), 2);
+	bssid->configuration.beacon_period = le32_to_cpu(le32_tmp);
+
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(0)) {
+		bssid->infrastructure_mode = Ndis802_11Infrastructure;
+		memcpy(bssid->mac_address, GetAddr2Ptr(pframe), ETH_ALEN);
+	} else {
+		bssid->infrastructure_mode = Ndis802_11IBSS;
+		memcpy(bssid->mac_address, GetAddr3Ptr(pframe), ETH_ALEN);
+	}
+
+	if (val16 & BIT(4))
+		bssid->privacy = 1;
+	else
+		bssid->privacy = 0;
+
+	bssid->configuration.atim_window = 0;
+
+	/* 20/40 BSS Coexistence check */
+	if ((pregistrypriv->wifi_spec == 1) && (false == pmlmeinfo->bwmode_updated)) {
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+		p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_HT_CAPABILITY, &len, bssid->ie_length - ie_offset);
+		if (p && len > 0) {
+			struct HT_caps_element	*pHT_caps;
+
+			pHT_caps = (struct HT_caps_element	*)(p + 2);
+
+			if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & BIT(14))
+				pmlmepriv->num_FortyMHzIntolerant++;
+		} else
+			pmlmepriv->num_sta_no_ht++;
+	}
+
+	/*  mark bss info receiving from nearby channel as signal_quality 101 */
+	if (bssid->configuration.ds_config != rtw_get_oper_ch(padapter))
+		bssid->phy_info.signal_quality = 101;
+
+	return _SUCCESS;
+}
+
+void start_create_ibss(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	u8 join_type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+
+	pmlmeext->cur_channel = (u8)pnetwork->configuration.ds_config;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* update capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&WLAN_CAPABILITY_IBSS) {/* adhoc master */
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+		/* switch channel */
+		/* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+
+		beacon_timing_control(padapter);
+
+		/* set msr to WIFI_FW_ADHOC_STATE */
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+		Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+		/* issue beacon */
+		if (send_beacon(padapter) == _FAIL) {
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else {
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.mac_address);
+			join_type = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+			rtw_indicate_connect(padapter);
+		}
+	} else {
+		return;
+	}
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+
+}
+
+void start_clnt_join(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	int beacon_timeout;
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* update capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&WLAN_CAPABILITY_ESS) {
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		/*  Because of AP's not receiving deauth before */
+		/*  AP may: 1)not response auth or 2)deauth us after link is complete */
+		/*  issue deauth before issuing auth to deal with the situation */
+
+		/* 	Commented by Albert 2012/07/21 */
+		/* 	For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
+		{
+				/* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */
+				issue_deauth_ex(padapter, pnetwork->mac_address, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
+		}
+
+		/* here wait for receiving the beacon to start auth */
+		/* and enable a timer */
+		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+		set_link_timer(pmlmeext, beacon_timeout);
+		_set_timer(&padapter->mlmepriv.assoc_timer,
+			(REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout);
+
+		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+	} else if (caps&WLAN_CAPABILITY_IBSS) { /* adhoc client */
+		Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		beacon_timing_control(padapter);
+
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+		report_join_res(padapter, 1);
+	} else {
+		return;
+	}
+
+}
+
+void start_clnt_auth(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
+	pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
+
+	pmlmeinfo->auth_seq = 1;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeext->retry = 0;
+
+
+	netdev_dbg(padapter->pnetdev, "start auth\n");
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+
+}
+
+
+void start_clnt_assoc(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+	issue_assocreq(padapter);
+
+	set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* check A3 */
+	if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_del_sta_event(padapter, MacAddr, reason);
+
+		} else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+		}
+	}
+
+	return _SUCCESS;
+}
+
+static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct rt_channel_info *chplan_new;
+	u8 channel;
+	u8 i;
+
+
+	pregistrypriv = &padapter->registrypriv;
+	pmlmeext = &padapter->mlmeextpriv;
+
+	/*  Adjust channel plan by AP Country IE */
+	if (pregistrypriv->enable80211d &&
+		(!pmlmeext->update_channel_plan_by_ap_done)) {
+		u8 *ie, *p;
+		u32 len;
+		struct rt_channel_plan chplan_ap;
+		struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
+		u8 country[4];
+		u8 fcn; /*  first channel number */
+		u8 noc; /*  number of channel */
+		u8 j, k;
+
+		ie = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_COUNTRY, &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+		if (!ie)
+			return;
+		if (len < 6)
+			return;
+
+		ie += 2;
+		p = ie;
+		ie += len;
+
+		memset(country, 0, 4);
+		memcpy(country, p, 3);
+		p += 3;
+
+		i = 0;
+		while ((ie - p) >= 3) {
+			fcn = *(p++);
+			noc = *(p++);
+			p++;
+
+			for (j = 0; j < noc; j++) {
+				if (fcn <= 14)
+					channel = fcn + j; /*  2.4 GHz */
+				else
+					channel = fcn + j*4; /*  5 GHz */
+
+				chplan_ap.Channel[i++] = channel;
+			}
+		}
+		chplan_ap.Len = i;
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+
+		memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = j = k = 0;
+		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0) ||
+					(chplan_sta[i].ChannelNum > 14))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 					chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/*  change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14)) {
+
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 				chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/*  add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else {
+			/*  keep original STA 2.4G channel plan */
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+
+			/*  skip AP 2.4G channel plan */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+				j++;
+		}
+
+		pmlmeext->update_channel_plan_by_ap_done = 1;
+	}
+
+	/*  If channel is used by AP, set channel scan type to active */
+	channel = bssid->configuration.ds_config;
+	chplan_new = pmlmeext->channel_set;
+	i = 0;
+	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
+		if (chplan_new[i].ChannelNum == channel) {
+			if (chplan_new[i].ScanType == SCAN_PASSIVE)
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+			break;
+		}
+		i++;
+	}
+}
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct survey_event	*psurvey_evt;
+	struct C2HEvent_Header *pc2h_evt_hdr;
+	struct mlme_ext_priv *pmlmeext;
+	struct cmd_priv *pcmdpriv;
+	/* u8 *pframe = precv_frame->u.hdr.rx_data; */
+	/* uint len = precv_frame->u.hdr.len; */
+
+	if (!padapter)
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct survey_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+
+	if (collect_bss_info(padapter, precv_frame, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
+		kfree(pcmd_obj);
+		kfree(pevtcmd);
+		return;
+	}
+
+	process_80211d(padapter, &psurvey_evt->bss);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+
+	return;
+
+}
+
+void report_surveydone_event(struct adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct surveydone_event *psurveydone_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct surveydone_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_join_res(struct adapter *padapter, int res)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct joinbss_event		*pjoinbss_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct joinbss_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+	pjoinbss_evt->network.join_res	= pjoinbss_evt->network.aid = res;
+
+
+	rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_wmm_edca_update(struct adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct wmm_event		*pwmm_event;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct wmm_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_WMM);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pwmm_event = (struct wmm_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	pwmm_event->wmm = 0;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stadel_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+
+
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if (psta)
+		mac_id = (int)psta->mac_id;
+	else
+		mac_id = (-1);
+
+	pdel_sta_evt->mac_id = mac_id;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct stassoc_event		*padd_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd_obj)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = rtw_zmalloc(cmdsz);
+	if (!pevtcmd) {
+		kfree(pcmd_obj);
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	padd_sta_evt->cam_id = cam_idx;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* ERP */
+	VCS_update(padapter, psta);
+
+	/* HT */
+	if (pmlmepriv->htpriv.ht_option) {
+		psta->htpriv.ht_option = true;
+
+		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+		psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para&IEEE80211_HT_CAP_AMPDU_DENSITY)>>2;
+
+		if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20))
+			psta->htpriv.sgi_20m = true;
+
+		if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40))
+			psta->htpriv.sgi_40m = true;
+
+		psta->qos_option = true;
+
+		psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap;
+		psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap;
+		psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap;
+
+		memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct ieee80211_ht_cap));
+	} else {
+		psta->htpriv.ht_option = false;
+
+		psta->htpriv.ampdu_enable = false;
+
+		psta->htpriv.sgi_20m = false;
+		psta->htpriv.sgi_40m = false;
+		psta->qos_option = false;
+
+	}
+
+	psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	psta->bw_mode = pmlmeext->cur_bwmode;
+
+	/* QoS */
+	if (pmlmepriv->qospriv.qos_option)
+		psta->qos_option = true;
+
+	update_ldpc_stbc_cap(psta);
+
+	spin_lock_bh(&psta->lock);
+	psta->state = _FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+}
+
+static void rtw_mlmeext_disconnect(struct adapter *padapter)
+{
+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+
+	/* set_opmode_cmd(padapter, infra_client_with_mlme); */
+
+	/* For safety, prevent from keeping macid sleep.
+	 * If we can sure all power mode enter/leave are paired,
+	 * this check can be removed.
+	 * Lucas@20131113
+	 */
+	/* wakeup macid after disconnect. */
+	{
+		struct sta_info *psta;
+
+		psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
+		if (psta)
+			rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	}
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+	/* set MSR to no link state -> infra. mode */
+	Set_MSR(padapter, _HW_STATE_STATION_);
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+	/* switch to the 20M Hz mode after disconnect */
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	flush_all_cam_entry(padapter);
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	/* pmlmepriv->LinkDetectInfo.TrafficBusyState = false; */
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+}
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	u8 join_type;
+	struct sta_info *psta;
+
+	if (join_res < 0) {
+		join_type = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+		rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+		return;
+	}
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+		/* update bc/mc sta_info */
+		update_bmc_sta(padapter);
+
+
+	/* turn on dynamic functions */
+	Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+
+	/*  update IOT-related issue */
+	update_IOT_info(padapter);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->supported_rates);
+
+	/* BCN interval */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+
+	/* update capability */
+	update_capinfo(padapter, pmlmeinfo->capability);
+
+	/* WMM, Update EDCA param */
+	WMMOnAssocRsp(padapter);
+
+	/* HT */
+	HTOnAssocRsp(padapter);
+
+	/* Set cur_channel&cur_bwmode&cur_ch_offset */
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->mac_address);
+	if (psta) { /* only for infra. mode */
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+		/* set per sta rate after updating HT cap. */
+		set_sta_rate(padapter, psta);
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+
+		/* wakeup macid after join bss successfully to ensure
+			the subsequent data frames can be sent out normally */
+		rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	}
+
+	join_type = 2;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+		/*  correcting TSF */
+		correct_TSF(padapter, pmlmeext);
+
+		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
+	}
+
+	if (get_iface_type(padapter) == IFACE_PORT0)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+}
+
+/* currently only adhoc mode will go here */
+void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 join_type;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { /* adhoc master or sta_count>1 */
+
+			/* nothing to do */
+		} else { /* adhoc client */
+			/* update TSF Value */
+			/* update_TSF(pmlmeext, pframe, len); */
+
+			/*  correcting TSF */
+			correct_TSF(padapter, pmlmeext);
+
+			/* start beacon */
+			if (send_beacon(padapter) == _FAIL) {
+				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
+
+				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
+
+				return;
+			}
+
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+		}
+
+		join_type = 2;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+	psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates);
+	memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen);
+
+	/* update adhoc sta_info */
+	update_sta_info(padapter, psta);
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+
+	/*  ToDo: HT for Ad-hoc */
+	psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel);
+	psta->raid = networktype_to_raid_ex(padapter, psta);
+
+	/* rate radaptive */
+	Update_RA_Entry(padapter, psta);
+}
+
+void mlmeext_sta_del_event_callback(struct adapter *padapter)
+{
+	if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
+		rtw_mlmeext_disconnect(padapter);
+}
+
+/****************************************************************************
+
+Following are the functions for the timer handlers
+
+*****************************************************************************/
+void _linked_info_dump(struct adapter *padapter)
+{
+	int i;
+	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+	int UndecoratedSmoothedPWDB;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+	if (padapter->bLinkInfoDump) {
+
+		if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+			rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
+
+		for (i = 0; i < NUM_STA; i++) {
+			if (pdvobj->macid[i]) {
+				if (i != 1) /* skip bc/mc sta */
+					/*   tx info ============ */
+					rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i);
+			}
+		}
+		rtw_hal_set_def_var(padapter, HAL_DEF_DBG_RX_INFO_DUMP, NULL);
+	}
+}
+
+static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 ret = false;
+
+	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
+		&& sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
+		&& sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
+	) {
+		ret = false;
+	} else {
+		ret = true;
+	}
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+void linked_status_chk(struct adapter *padapter)
+{
+	u32 i;
+	struct sta_info 	*psta;
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+
+
+	if (is_client_associated_to_ap(padapter)) {
+		/* linked infrastructure client mode */
+
+		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+		int rx_chk_limit;
+		int link_count_limit;
+
+		#if defined(DBG_ROAMING_TEST)
+		rx_chk_limit = 1;
+		#else
+		rx_chk_limit = 8;
+		#endif
+		link_count_limit = 7; /*  16 sec */
+
+		/*  Marked by Kurt 20130715 */
+		/*  For WiDi 3.5 and latered on, they don't ask WiDi sink to do roaming, so we could not check rx limit that strictly. */
+		/*  todo: To check why we under miracast session, rx_chk would be false */
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.mac_address);
+		if (psta) {
+			if (chk_ap_is_alive(padapter, psta) == false)
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+			{
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+						issue_probereq_ex(padapter, &pmlmeinfo->network.ssid, pmlmeinfo->network.mac_address, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.ssid, pmlmeinfo->network.mac_address, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.ssid, pmlmeinfo->network.mac_address, 0, 0, 0, 0);
+					}
+				}
+
+				if (tx_chk != _SUCCESS &&
+				    pmlmeinfo->link_count++ == link_count_limit)
+					tx_chk = issue_nulldata_in_interrupt(padapter, NULL);
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					netdev_dbg(padapter->pnetdev,
+						   FUNC_ADPT_FMT " disconnect or roaming\n",
+						   FUNC_ADPT_ARG(padapter));
+					receive_disconnect(padapter, pmlmeinfo->network.mac_address
+						, WLAN_REASON_EXPIRATION_CHK);
+					return;
+				}
+			} else {
+				pmlmeext->retry = 0;
+			}
+
+			if (tx_chk == _FAIL) {
+				pmlmeinfo->link_count %= (link_count_limit+1);
+			} else {
+				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
+				pmlmeinfo->link_count = 0;
+			}
+
+		} /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.mac_address)) != NULL) */
+	} else if (is_client_associated_to_ibss(padapter)) {
+		/* linked IBSS mode */
+		/* for each assoc list entry to check the rx pkt counter */
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+			if (pmlmeinfo->FW_sta_info[i].status == 1) {
+				psta = pmlmeinfo->FW_sta_info[i].psta;
+
+				if (psta == NULL)
+					continue;
+
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) {
+
+					if (pmlmeinfo->FW_sta_info[i].retry < 3) {
+						pmlmeinfo->FW_sta_info[i].retry++;
+					} else {
+						pmlmeinfo->FW_sta_info[i].retry = 0;
+						pmlmeinfo->FW_sta_info[i].status = 0;
+						report_del_sta_event(padapter, psta->hwaddr
+							, 65535/*  indicate disconnect caused by no rx */
+						);
+					}
+				} else {
+					pmlmeinfo->FW_sta_info[i].retry = 0;
+					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
+				}
+			}
+		}
+
+		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
+
+	}
+
+}
+
+void survey_timer_hdl(struct timer_list *t)
+{
+	struct adapter *padapter =
+		from_timer(padapter, t, mlmeextpriv.survey_timer);
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv 				*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* issue rtw_sitesurvey_cmd */
+	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
+		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
+			pmlmeext->sitesurvey_res.channel_idx++;
+
+		if (pmlmeext->scan_abort) {
+			pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+
+			pmlmeext->scan_abort = false;/* reset */
+		}
+
+		ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c)
+			return;
+
+		psurveyPara = rtw_zmalloc(sizeof(struct sitesurvey_parm));
+		if (!psurveyPara) {
+			kfree(ph2c);
+			return;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+}
+
+void link_timer_hdl(struct timer_list *t)
+{
+	struct adapter *padapter =
+		from_timer(padapter, t, mlmeextpriv.link_timer);
+	/* static unsigned int		rx_pkt = 0; */
+	/* static u64				tx_cnt = 0; */
+	/* struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv); */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct sta_priv 	*pstapriv = &padapter->stapriv; */
+
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		report_join_res(padapter, -3);
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
+		/* re-auth timer */
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
+			pmlmeinfo->state = 0;
+			report_join_res(padapter, -1);
+			return;
+		}
+
+		pmlmeinfo->auth_seq = 1;
+		issue_auth(padapter, NULL, 0);
+		set_link_timer(pmlmeext, REAUTH_TO);
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
+		/* re-assoc timer */
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+			return;
+		}
+
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+}
+
+void addba_timer_hdl(struct timer_list *t)
+{
+	struct sta_info *psta = from_timer(psta, t, addba_retry_timer);
+	struct ht_priv *phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if (phtpriv->ht_option && phtpriv->ampdu_enable) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+
+	}
+}
+
+void sa_query_timer_hdl(struct timer_list *t)
+{
+	struct adapter *padapter =
+		from_timer(padapter, t, mlmeextpriv.sa_query_timer);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	/* disconnect */
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+	if (psetop->mode == Ndis802_11APMode) {
+		pmlmeinfo->state = WIFI_FW_AP_STATE;
+		type = _HW_STATE_AP_;
+		/* start_ap_mode(padapter); */
+	} else if (psetop->mode == Ndis802_11Infrastructure) {
+		pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to	STATION_STATE */
+		type = _HW_STATE_STATION_;
+	} else if (psetop->mode == Ndis802_11IBSS) {
+		type = _HW_STATE_ADHOC_;
+	} else {
+		type = _HW_STATE_NOLINK_;
+	}
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+	/* Set_MSR(padapter, type); */
+
+	if (psetop->mode == Ndis802_11APMode) {
+		/*  Do this after port switch to */
+		/*  prevent from downloading rsvd page to wrong port */
+		rtw_btcoex_MediaStatusNotify(padapter, 1); /* connect */
+	}
+
+	return H2C_SUCCESS;
+
+}
+
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex	*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+	/* u32 initialgain; */
+
+	if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
+		start_bss_network(padapter);
+		return H2C_SUCCESS;
+	}
+
+	/* below is for ad-hoc master */
+	if (pparm->network.infrastructure_mode == Ndis802_11IBSS) {
+		rtw_joinbss_reset(padapter);
+
+		pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		pmlmeinfo->ERP_enable = 0;
+		pmlmeinfo->WMM_enable = 0;
+		pmlmeinfo->HT_enable = 0;
+		pmlmeinfo->HT_caps_enable = 0;
+		pmlmeinfo->HT_info_enable = 0;
+		pmlmeinfo->agg_enable_bitmap = 0;
+		pmlmeinfo->candidate_tid_bitmap = 0;
+
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+		/* config the initial gain under linking, need to write the BB registers */
+		/* initialgain = 0x1E; */
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+		/* cancel link timer */
+		del_timer_sync(&pmlmeext->link_timer);
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, ie_length));
+		pnetwork->ie_length = ((struct wlan_bssid_ex *)pbuf)->ie_length;
+
+		if (pnetwork->ie_length > MAX_IE_SZ)/* Check pbuf->ie_length */
+			return H2C_PARAMETERS_ERROR;
+
+		memcpy(pnetwork->ies, ((struct wlan_bssid_ex *)pbuf)->ies, pnetwork->ie_length);
+
+		start_create_ibss(padapter);
+
+	}
+
+	return H2C_SUCCESS;
+
+}
+
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 join_type;
+	struct ndis_80211_var_ie *pIE;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	u32 i;
+	u8 cbw40_enable = 0;
+	/* u32 initialgain; */
+	/* u32 acparm; */
+	u8 ch, bw, offset;
+
+	/* check already connecting to AP or not */
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
+			issue_deauth_ex(padapter, pnetwork->mac_address, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		del_timer_sync(&pmlmeext->link_timer);
+
+		/* set MSR to nolink -> infra. mode */
+		/* Set_MSR(padapter, _HW_STATE_NOLINK_); */
+		Set_MSR(padapter, _HW_STATE_STATION_);
+
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	}
+
+	rtw_joinbss_reset(padapter);
+
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pmlmeinfo->ERP_enable = 0;
+	pmlmeinfo->WMM_enable = 0;
+	pmlmeinfo->HT_enable = 0;
+	pmlmeinfo->HT_caps_enable = 0;
+	pmlmeinfo->HT_info_enable = 0;
+	pmlmeinfo->agg_enable_bitmap = 0;
+	pmlmeinfo->candidate_tid_bitmap = 0;
+	pmlmeinfo->bwmode_updated = false;
+	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
+	pmlmeinfo->VHT_enable = 0;
+
+	memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, ie_length));
+	pnetwork->ie_length = ((struct wlan_bssid_ex *)pbuf)->ie_length;
+
+	if (pnetwork->ie_length > MAX_IE_SZ)/* Check pbuf->ie_length */
+		return H2C_PARAMETERS_ERROR;
+
+	memcpy(pnetwork->ies, ((struct wlan_bssid_ex *)pbuf)->ies, pnetwork->ie_length);
+
+	pmlmeext->cur_channel = (u8)pnetwork->configuration.ds_config;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* Check AP vendor to move rtw_joinbss_cmd() */
+	/* pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->ies, pnetwork->ie_length); */
+
+	/* sizeof(struct ndis_802_11_fix_ie) */
+	for (i = _FIXED_IE_LENGTH_; i < pnetwork->ie_length;) {
+		pIE = (struct ndis_80211_var_ie *)(pnetwork->ies + i);
+
+		switch (pIE->element_id) {
+		case WLAN_EID_VENDOR_SPECIFIC:/* Get WMM IE. */
+			if (!memcmp(pIE->data, WMM_OUI, 4))
+				WMM_param_handler(padapter, pIE);
+			break;
+
+		case WLAN_EID_HT_CAPABILITY:	/* Get HT Cap IE. */
+			pmlmeinfo->HT_caps_enable = 1;
+			break;
+
+		case WLAN_EID_HT_OPERATION:	/* Get HT Info IE. */
+			pmlmeinfo->HT_info_enable = 1;
+
+			/* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
+			{
+				struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
+
+				if (pnetwork->configuration.ds_config <= 14) {
+					if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20)
+						cbw40_enable = 1;
+				}
+
+				if ((cbw40_enable) && (pht_info->infos[0] & BIT(2))) {
+					/* switch to the 40M Hz mode according to the AP */
+					pmlmeext->cur_bwmode = CHANNEL_WIDTH_40;
+					switch (pht_info->infos[0] & 0x3) {
+					case 1:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+						break;
+
+					case 3:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+						break;
+
+					default:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+						pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+						break;
+					}
+				}
+			}
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->length + 2);
+	}
+
+	/* check channel, bandwidth, offset and switch */
+	if (rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) {
+		report_join_res(padapter, (-4));
+		return H2C_SUCCESS;
+	}
+
+	/* disable dynamic functions, such as high power, DIG */
+	/* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */
+
+	/* config the initial gain under linking, need to write the BB registers */
+	/* initialgain = 0x1E; */
+	/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.mac_address);
+	join_type = 0;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+	set_channel_bwmode(padapter, ch, offset, bw);
+
+	/* cancel link timer */
+	del_timer_sync(&pmlmeext->link_timer);
+
+	start_clnt_join(padapter);
+
+	return H2C_SUCCESS;
+
+}
+
+u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	u8 val8;
+
+	if (is_client_associated_to_ap(padapter))
+			issue_deauth_ex(padapter, pnetwork->mac_address, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+
+	if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+		/* Stop BCN */
+		val8 = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
+	}
+
+	rtw_mlmeext_disconnect(padapter);
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	return	H2C_SUCCESS;
+}
+
+static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
+	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	int set_idx;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	/* clear first */
+	memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
+
+	/* acquire channels from in */
+	j = 0;
+	for (i = 0; i < in_num; i++) {
+
+		set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+		if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
+			&& set_idx >= 0
+		) {
+			if (j >= out_num) {
+				netdev_dbg(padapter->pnetdev,
+					   FUNC_ADPT_FMT " out_num:%u not enough\n",
+					   FUNC_ADPT_ARG(padapter), out_num);
+				break;
+			}
+
+			memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
+
+			if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+				out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+		if (j >= out_num)
+			break;
+	}
+
+	/* if out is empty, use channel_set as default */
+	if (j == 0) {
+		for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+
+			if (j >= out_num) {
+				netdev_dbg(padapter->pnetdev,
+					   FUNC_ADPT_FMT " out_num:%u not enough\n",
+					   FUNC_ADPT_ARG(padapter),
+					   out_num);
+				break;
+			}
+
+			out[j].hw_value = pmlmeext->channel_set[i].ChannelNum;
+
+			if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+				out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+	}
+
+	return j;
+}
+
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct sitesurvey_parm	*pparm = (struct sitesurvey_parm *)pbuf;
+	u8 bdelayscan = false;
+	u8 val8;
+	u32 initialgain;
+	u32 i;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
+		pmlmeext->sitesurvey_res.state = SCAN_START;
+		pmlmeext->sitesurvey_res.bss_cnt = 0;
+		pmlmeext->sitesurvey_res.channel_idx = 0;
+
+		for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (pparm->ssid[i].ssid_length) {
+				memcpy(pmlmeext->sitesurvey_res.ssid[i].ssid, pparm->ssid[i].ssid, IW_ESSID_MAX_SIZE);
+				pmlmeext->sitesurvey_res.ssid[i].ssid_length = pparm->ssid[i].ssid_length;
+			} else {
+				pmlmeext->sitesurvey_res.ssid[i].ssid_length = 0;
+			}
+		}
+
+		pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
+			, pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
+			, pparm->ch, pparm->ch_num
+		);
+
+		pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
+
+		/* issue null data if associating to the AP */
+		if (is_client_associated_to_ap(padapter)) {
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			issue_nulldata(padapter, NULL, 1, 3, 500);
+
+			bdelayscan = true;
+		}
+		if (bdelayscan) {
+			/* delay 50ms to protect nulldata(1). */
+			set_survey_timer(pmlmeext, 50);
+			return H2C_SUCCESS;
+		}
+	}
+
+	if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+		/* config the initial gain under scanning, need to write the BB
+		 * registers
+		 */
+		initialgain = 0x1e;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+
+		/* set MSR to no link state */
+		Set_MSR(padapter, _HW_STATE_NOLINK_);
+
+		val8 = 1; /* under site survey */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
+	}
+
+	site_survey(padapter);
+
+	return H2C_SUCCESS;
+
+}
+
+u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pparm->mode < 4)
+		pmlmeinfo->auth_algo = pparm->mode;
+
+	return	H2C_SUCCESS;
+}
+
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl = 0;
+	s16 cam_id = 0;
+	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	unsigned char null_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	u8 *addr;
+
+	/* main tx key for wep. */
+	if (pparm->set_tx)
+		pmlmeinfo->key_index = pparm->keyid;
+
+	cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid);
+
+	if (cam_id < 0) {
+	} else {
+		if (cam_id > 3) /* not default key, searched by A2 */
+			addr = get_bssid(&padapter->mlmepriv);
+		else
+			addr = null_addr;
+
+		ctrl = BIT(15) | BIT6 | ((pparm->algorithm) << 2) | pparm->keyid;
+		write_cam(padapter, cam_id, ctrl, addr, pparm->key);
+		netdev_dbg(padapter->pnetdev,
+			   "set group key camid:%d, addr:%pM, kid:%d, type:%s\n",
+			   cam_id, MAC_ARG(addr), pparm->keyid,
+			   security_type_str(pparm->algorithm));
+	}
+
+	if (cam_id >= 0 && cam_id <= 3)
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)true);
+
+	/* allow multicast packets to driver */
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
+
+	return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl = 0;
+	s16 cam_id = 0;
+	u8 ret = H2C_SUCCESS;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+
+	if (pparm->algorithm == _NO_PRIVACY_)
+		goto write_to_cam;
+
+	psta = rtw_get_stainfo(pstapriv, pparm->addr);
+	if (!psta) {
+		netdev_dbg(padapter->pnetdev, "%s sta:%pM not found\n",
+			   __func__, MAC_ARG(pparm->addr));
+		ret = H2C_REJECTED;
+		goto exit;
+	}
+
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	cam_id = rtw_camid_alloc(padapter, psta, 0);
+	if (cam_id < 0)
+		goto exit;
+
+write_to_cam:
+	if (pparm->algorithm == _NO_PRIVACY_) {
+		while ((cam_id = rtw_camid_search(padapter, pparm->addr, -1)) >= 0) {
+			netdev_dbg(padapter->pnetdev,
+				   "clear key for addr:%pM, camid:%d\n",
+				   MAC_ARG(pparm->addr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else {
+		netdev_dbg(padapter->pnetdev,
+			   "set pairwise key camid:%d, addr:%pM, kid:%d, type:%s\n",
+			   cam_id, MAC_ARG(pparm->addr), pparm->keyid,
+			   security_type_str(pparm->algorithm));
+		ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+		write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	}
+	ret = H2C_SUCCESS_RSP;
+
+exit:
+	return ret;
+}
+
+u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+
+	if (!psta)
+		return	H2C_SUCCESS;
+
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+		((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+		/* pmlmeinfo->ADDBA_retry_count = 0; */
+		/* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid); */
+		/* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */
+		issue_action_BA(padapter, pparm->addr, WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+		/* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	} else {
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+	}
+	return	H2C_SUCCESS;
+}
+
+
+u8 chk_bmc_sleepq_cmd(struct adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	u8 res = _SUCCESS;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_parm_rsp(ph2c, GEN_CMD_CODE(_ChkBMCSleepq));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 set_tx_beacon_cmd(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct Tx_Beacon_param	*ptxBeacon_parm;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 res = _SUCCESS;
+	int len_diff = 0;
+
+	ph2c = rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+	if (!ptxBeacon_parm) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+
+	len_diff = update_hidden_ssid(ptxBeacon_parm->network.ies+_BEACON_IE_OFFSET_,
+				      ptxBeacon_parm->network.ie_length-_BEACON_IE_OFFSET_,
+				      pmlmeinfo->hidden_ssid_mode);
+	ptxBeacon_parm->network.ie_length += len_diff;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+static struct fwevent wlanevents[] = {
+	{0, rtw_dummy_event_callback},	/*0*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, &rtw_survey_event_callback},		/*8*/
+	{sizeof(struct surveydone_event), &rtw_surveydone_event_callback},	/*9*/
+
+	{0, &rtw_joinbss_event_callback},		/*10*/
+	{sizeof(struct stassoc_event), &rtw_stassoc_event_callback},
+	{sizeof(struct stadel_event), &rtw_stadel_event_callback},
+	{0, &rtw_atimdone_event_callback},
+	{0, rtw_dummy_event_callback},
+	{0, NULL},	/*15*/
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, rtw_fwdbg_event_callback},
+	{0, NULL},	 /*20*/
+	{0, NULL},
+	{0, NULL},
+	{0, &rtw_cpwm_event_callback},
+	{0, NULL},
+	{0, &rtw_wmm_event_callback},
+
+};
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+	if (!pbuf)
+		goto _abort_event_;
+
+	peventbuf = (uint *)pbuf;
+	evt_sz = (u16)(*peventbuf&0xffff);
+	evt_code = (u8)((*peventbuf>>16)&0xff);
+
+	/*  checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT)
+		goto _abort_event_;
+
+	/*  checking if event size match the event parm size */
+	if ((wlanevents[evt_code].parmsize != 0) &&
+			(wlanevents[evt_code].parmsize != evt_sz))
+		goto _abort_event_;
+
+	atomic_inc(&pevt_priv->event_seq);
+
+	peventbuf += 2;
+
+	if (peventbuf) {
+		event_callback = wlanevents[evt_code].event_callback;
+		event_callback(padapter, (u8 *)peventbuf);
+
+		pevt_priv->evt_done_cnt++;
+	}
+
+
+_abort_event_:
+
+
+	return H2C_SUCCESS;
+
+}
+
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 chk_bmc_sleepq_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct sta_info *psta_bmc;
+	struct list_head *xmitframe_plist, *xmitframe_phead, *tmp;
+	struct xmit_frame *pxmitframe = NULL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct sta_priv  *pstapriv = &padapter->stapriv;
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return H2C_SUCCESS;
+
+	if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len > 0)) {
+		msleep(10);/*  10ms, ATIM(HIQ) Windows */
+
+		/* spin_lock_bh(&psta_bmc->sleep_q.lock); */
+		spin_lock_bh(&pxmitpriv->lock);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		list_for_each_safe(xmitframe_plist, tmp, xmitframe_phead) {
+			pxmitframe = list_entry(xmitframe_plist,
+						struct xmit_frame, list);
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+
+			if (xmitframe_hiq_filter(pxmitframe))
+				pxmitframe->attrib.qsel = 0x11;/* HIQ */
+
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+		}
+
+		/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
+		spin_unlock_bh(&pxmitpriv->lock);
+
+		/* check hi queue and bmc_sleepq */
+		rtw_chk_hi_queue_cmd(padapter);
+	}
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (send_beacon(padapter) == _FAIL)
+		return H2C_PARAMETERS_ERROR;
+
+	/* tx bc/mc frames after update TIM */
+	chk_bmc_sleepq_hdl(padapter, NULL);
+
+	return H2C_SUCCESS;
+}
+
+int rtw_chk_start_clnt_join(struct adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	unsigned char cur_ch = pmlmeext->cur_channel;
+	unsigned char cur_bw = pmlmeext->cur_bwmode;
+	unsigned char cur_ch_offset = pmlmeext->cur_ch_offset;
+	bool connect_allow = true;
+
+	if (!ch || !bw || !offset) {
+		rtw_warn_on(1);
+		connect_allow = false;
+	}
+
+	if (connect_allow) {
+		*ch = cur_ch;
+		*bw = cur_bw;
+		*offset = cur_ch_offset;
+	}
+
+	return connect_allow ? _SUCCESS : _FAIL;
+}
+
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	pmlmeext->cur_channel = set_ch_parm->ch;
+	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+	pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+	set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	if (padapter->rtw_wdev && padapter->rtw_wdev->wiphy) {
+		struct regulatory_request request;
+
+		request.initiator = NL80211_REGDOM_SET_BY_DRIVER;
+		rtw_reg_notifier(padapter->rtw_wdev->wiphy, &request);
+	}
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return	H2C_REJECTED;
+}
+
+/*  TDLS_ESTABLISHED	: write RCR DATA BIT */
+/*  TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure */
+/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame */
+/*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/*  TDLS_OFF_CH		: first time set channel to off channel */
+/*  TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel */
+/*  TDLS_P_OFF_CH	: periodically go to off channel */
+/*  TDLS_P_BASE_CH	: periodically go back to base channel */
+/*  TDLS_RS_RCR		: restore RCR */
+/*  TDLS_TEAR_STA	: free tdls sta */
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return H2C_REJECTED;
+}
+
+u8 run_in_thread_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct RunInThread_param *p;
+
+
+	if (pbuf == NULL)
+		return H2C_PARAMETERS_ERROR;
+	p = (struct RunInThread_param *)pbuf;
+
+	if (p->func)
+		p->func(p->context);
+
+	return H2C_SUCCESS;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
new file mode 100644
index 000000000..a392d5b4c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
@@ -0,0 +1,1174 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/jiffies.h>
+
+
+void _ips_enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	pwrpriv->bips_processing = true;
+
+	/*  syn ips_mode with request */
+	pwrpriv->ips_mode = pwrpriv->ips_mode_req;
+
+	pwrpriv->ips_enter_cnts++;
+
+	if (rf_off == pwrpriv->change_rfpwrstate) {
+		pwrpriv->bpower_saving = true;
+
+		if (pwrpriv->ips_mode == IPS_LEVEL_2)
+			pwrpriv->bkeepfwalive = true;
+
+		rtw_ips_pwr_down(padapter);
+		pwrpriv->rf_pwrstate = rf_off;
+	}
+	pwrpriv->bips_processing = false;
+
+}
+
+void ips_enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+
+	hal_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
+
+	mutex_lock(&pwrpriv->lock);
+	_ips_enter(padapter);
+	mutex_unlock(&pwrpriv->lock);
+}
+
+int _ips_leave(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	int result = _SUCCESS;
+
+	if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) {
+		pwrpriv->bips_processing = true;
+		pwrpriv->change_rfpwrstate = rf_on;
+		pwrpriv->ips_leave_cnts++;
+
+		result = rtw_ips_pwr_up(padapter);
+		if (result == _SUCCESS) {
+			pwrpriv->rf_pwrstate = rf_on;
+		}
+		pwrpriv->bips_processing = false;
+
+		pwrpriv->bkeepfwalive = false;
+		pwrpriv->bpower_saving = false;
+	}
+
+	return result;
+}
+
+int ips_leave(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	int ret;
+
+	if (!is_primary_adapter(padapter))
+		return _SUCCESS;
+
+	mutex_lock(&pwrpriv->lock);
+	ret = _ips_leave(padapter);
+	mutex_unlock(&pwrpriv->lock);
+
+	if (ret == _SUCCESS)
+		hal_btcoex_IpsNotify(padapter, IPS_NONE);
+
+	return ret;
+}
+
+static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
+{
+	struct adapter *buddy = adapter->pbuddy_adapter;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
+
+	bool ret = false;
+
+	if (adapter_to_pwrctl(adapter)->bpower_saving)
+		goto exit;
+
+	if (time_before(jiffies, adapter_to_pwrctl(adapter)->ips_deny_time))
+		goto exit;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
+		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+	)
+		goto exit;
+
+	/* consider buddy, if exist */
+	if (buddy) {
+		struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
+
+		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
+			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+		)
+			goto exit;
+	}
+
+	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
+		netdev_dbg(adapter->pnetdev,
+			   "There are some pkts to transmit\n");
+		netdev_dbg(adapter->pnetdev,
+			   "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
+			   pxmit_priv->free_xmitbuf_cnt,
+			   pxmit_priv->free_xmit_extbuf_cnt);
+		goto exit;
+	}
+
+	ret = true;
+
+exit:
+	return ret;
+}
+
+
+/*
+ * ATTENTION:
+ *rtw_ps_processor() doesn't handle LPS.
+ */
+void rtw_ps_processor(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u32 ps_deny = 0;
+
+	mutex_lock(&adapter_to_pwrctl(padapter)->lock);
+	ps_deny = rtw_ps_deny_get(padapter);
+	mutex_unlock(&adapter_to_pwrctl(padapter)->lock);
+	if (ps_deny != 0)
+		goto exit;
+
+	if (pwrpriv->bInSuspend) {/* system suspend or autosuspend */
+		pdbgpriv->dbg_ps_insuspend_cnt++;
+		return;
+	}
+
+	pwrpriv->ps_processing = true;
+
+	if (pwrpriv->ips_mode_req == IPS_NONE)
+		goto exit;
+
+	if (!rtw_pwr_unassociated_idle(padapter))
+		goto exit;
+
+	if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0)) {
+		pwrpriv->change_rfpwrstate = rf_off;
+		{
+			ips_enter(padapter);
+		}
+	}
+exit:
+	pwrpriv->ps_processing = false;
+}
+
+static void pwr_state_check_handler(struct timer_list *t)
+{
+	struct pwrctrl_priv *pwrctrlpriv =
+		from_timer(pwrctrlpriv, t, pwr_state_check_timer);
+	struct adapter *padapter = pwrctrlpriv->adapter;
+
+	rtw_ps_cmd(padapter);
+}
+
+void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets)
+{
+	static unsigned long start_time;
+	static u32 xmit_cnt;
+	u8 bLeaveLPS = false;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+
+
+	if (tx) { /* from tx */
+		xmit_cnt += tx_packets;
+
+		if (start_time == 0)
+			start_time = jiffies;
+
+		if (jiffies_to_msecs(jiffies - start_time) > 2000) { /*  2 sec == watch dog timer */
+			if (xmit_cnt > 8) {
+				if (adapter_to_pwrctl(padapter)->bLeisurePs
+				    && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
+				    && !(hal_btcoex_IsBtControlLps(padapter))) {
+					bLeaveLPS = true;
+				}
+			}
+
+			start_time = jiffies;
+			xmit_cnt = 0;
+		}
+
+	} else { /*  from rx path */
+		if (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4/*2*/) {
+			if (adapter_to_pwrctl(padapter)->bLeisurePs
+			    && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
+			    && !(hal_btcoex_IsBtControlLps(padapter)))
+				bLeaveLPS = true;
+		}
+	}
+
+	if (bLeaveLPS)
+		/* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1); */
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, tx?0:1);
+}
+
+/*
+ * Description:
+ *This function MUST be called under power lock protect
+ *
+ * Parameters
+ *padapter
+ *pslv			power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *
+ */
+void rtw_set_rpwm(struct adapter *padapter, u8 pslv)
+{
+	u8 rpwm;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	u8 cpwm_orig;
+
+	pslv = PS_STATE(pslv);
+
+	if (!pwrpriv->brpwmtimeout) {
+		if (pwrpriv->rpwm == pslv ||
+		    (pwrpriv->rpwm >= PS_STATE_S2 && pslv >= PS_STATE_S2))
+			return;
+
+	}
+
+	if ((padapter->bSurpriseRemoved) || !(padapter->hw_init_completed)) {
+		pwrpriv->cpwm = PS_STATE_S4;
+
+		return;
+	}
+
+	if (padapter->bDriverStopped) {
+		if (pslv < PS_STATE_S2)
+			return;
+	}
+
+	rpwm = pslv | pwrpriv->tog;
+	/*  only when from PS_STATE S0/S1 to S2 and higher needs ACK */
+	if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2))
+		rpwm |= PS_ACK;
+
+	pwrpriv->rpwm = pslv;
+
+	cpwm_orig = 0;
+	if (rpwm & PS_ACK)
+		rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
+
+	if (rpwm & PS_ACK)
+		_set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS);
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+
+	pwrpriv->tog += 0x80;
+
+	/*  No LPS 32K, No Ack */
+	if (rpwm & PS_ACK) {
+		unsigned long start_time;
+		u8 cpwm_now;
+		u8 poll_cnt = 0;
+
+		start_time = jiffies;
+
+		/*  polling cpwm */
+		do {
+			mdelay(1);
+			poll_cnt++;
+			rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now);
+			if ((cpwm_orig ^ cpwm_now) & 0x80) {
+				pwrpriv->cpwm = PS_STATE_S4;
+				pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE;
+				break;
+			}
+
+			if (jiffies_to_msecs(jiffies - start_time) > LPS_RPWM_WAIT_MS) {
+				_set_timer(&pwrpriv->pwr_rpwm_timer, 1);
+				break;
+			}
+		} while (1);
+	} else
+		pwrpriv->cpwm = pslv;
+}
+
+static u8 PS_RDY_CHECK(struct adapter *padapter)
+{
+	unsigned long curr_time, delta_time;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	if (pwrpriv->bInSuspend)
+		return false;
+
+	curr_time = jiffies;
+
+	delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp;
+
+	if (delta_time < LPS_DELAY_TIME)
+		return false;
+
+	if (check_fwstate(pmlmepriv, WIFI_SITE_MONITOR)
+		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
+		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+		|| rtw_is_scan_deny(padapter)
+	)
+		return false;
+
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X &&
+	    !padapter->securitypriv.binstallGrpkey)
+		return false;
+
+	if (!rtw_cfg80211_pwr_mgmt(padapter))
+		return false;
+
+	return true;
+}
+
+void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (ps_mode > PM_Card_Disable)
+		return;
+
+	if (pwrpriv->pwr_mode == ps_mode)
+		if (ps_mode == PS_MODE_ACTIVE)
+			return;
+
+
+	mutex_lock(&pwrpriv->lock);
+
+	/* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
+	if (ps_mode == PS_MODE_ACTIVE) {
+		if (!(hal_btcoex_IsBtControlLps(padapter))
+				|| (hal_btcoex_IsBtControlLps(padapter)
+					&& !(hal_btcoex_IsLpsOn(padapter)))) {
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_rpwm(padapter, PS_STATE_S4);
+
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+			pwrpriv->fw_current_in_ps_mode = false;
+
+			hal_btcoex_LpsNotify(padapter, ps_mode);
+		}
+	} else {
+		if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE)) ||
+		    ((hal_btcoex_IsBtControlLps(padapter)) && (hal_btcoex_IsLpsOn(padapter)))
+			) {
+			u8 pslv;
+
+			hal_btcoex_LpsNotify(padapter, ps_mode);
+
+			pwrpriv->fw_current_in_ps_mode = true;
+			pwrpriv->pwr_mode = ps_mode;
+			pwrpriv->smart_ps = smart_ps;
+			pwrpriv->bcn_ant_mode = bcn_ant_mode;
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+
+			pslv = PS_STATE_S2;
+			if (pwrpriv->alives == 0)
+				pslv = PS_STATE_S0;
+
+			if (!(hal_btcoex_IsBtDisabled(padapter)) &&
+			    (hal_btcoex_IsBtControlLps(padapter))) {
+				u8 val8;
+
+				val8 = hal_btcoex_LpsVal(padapter);
+				if (val8 & BIT(4))
+					pslv = PS_STATE_S2;
+			}
+
+			rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	mutex_unlock(&pwrpriv->lock);
+}
+
+/*
+ * Return:
+ *0:	Leave OK
+ *-1:	Timeout
+ *-2:	Other error
+ */
+s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
+{
+	unsigned long start_time;
+	u8 bAwake = false;
+	s32 err = 0;
+
+
+	start_time = jiffies;
+	while (1) {
+		rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
+		if (bAwake)
+			break;
+
+		if (padapter->bSurpriseRemoved) {
+			err = -2;
+			break;
+		}
+
+		if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
+			err = -1;
+			break;
+		}
+		msleep(1);
+	}
+
+	return err;
+}
+
+/*  */
+/* 	Description: */
+/* 		Enter the leisure power save mode. */
+/*  */
+void LPS_Enter(struct adapter *padapter, const char *msg)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
+	int n_assoc_iface = 0;
+	char buf[32] = {0};
+
+	if (hal_btcoex_IsBtControlLps(padapter))
+		return;
+
+	/* Skip lps enter request if number of assocated adapters is not 1 */
+	if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
+		n_assoc_iface++;
+	if (n_assoc_iface != 1)
+		return;
+
+	/* Skip lps enter request for adapter not port0 */
+	if (get_iface_type(padapter) != IFACE_PORT0)
+		return;
+
+	if (!PS_RDY_CHECK(dvobj->padapters))
+		return;
+
+	if (pwrpriv->bLeisurePs) {
+		/*  Idle for a while if we connect to AP a while ago. */
+		if (pwrpriv->LpsIdleCount >= 2) { /*   4 Sec */
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+				scnprintf(buf, sizeof(buf), "WIFI-%s", msg);
+				pwrpriv->bpower_saving = true;
+				rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, padapter->registrypriv.smart_ps, 0, buf);
+			}
+		} else
+			pwrpriv->LpsIdleCount++;
+	}
+}
+
+/*  */
+/* 	Description: */
+/* 		Leave the leisure power save mode. */
+/*  */
+void LPS_Leave(struct adapter *padapter, const char *msg)
+{
+#define LPS_LEAVE_TIMEOUT_MS 100
+
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
+	char buf[32] = {0};
+
+	if (hal_btcoex_IsBtControlLps(padapter))
+		return;
+
+	if (pwrpriv->bLeisurePs) {
+		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+			scnprintf(buf, sizeof(buf), "WIFI-%s", msg);
+			rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, buf);
+
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+				LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
+		}
+	}
+
+	pwrpriv->bpower_saving = false;
+}
+
+void LeaveAllPowerSaveModeDirect(struct adapter *Adapter)
+{
+	struct adapter *pri_padapter = GET_PRIMARY_ADAPTER(Adapter);
+	struct mlme_priv *pmlmepriv = &(Adapter->mlmepriv);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter);
+
+	if (Adapter->bSurpriseRemoved)
+		return;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) { /* connect */
+
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			return;
+
+		mutex_lock(&pwrpriv->lock);
+
+		rtw_set_rpwm(Adapter, PS_STATE_S4);
+
+		mutex_unlock(&pwrpriv->lock);
+
+		rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
+	} else {
+		if (pwrpriv->rf_pwrstate == rf_off)
+			ips_leave(pri_padapter);
+	}
+}
+
+/*  */
+/*  Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */
+/*  Move code to function by tynli. 2010.03.26. */
+/*  */
+void LeaveAllPowerSaveMode(struct adapter *Adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
+	u8 enqueue = 0;
+	int n_assoc_iface = 0;
+
+	if (!Adapter->bup)
+		return;
+
+	if (Adapter->bSurpriseRemoved)
+		return;
+
+	if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
+		n_assoc_iface++;
+
+	if (n_assoc_iface) { /* connect */
+		enqueue = 1;
+
+		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+
+		LPS_Leave_check(Adapter);
+	} else {
+		if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) {
+			ips_leave(Adapter);
+		}
+	}
+}
+
+void LPS_Leave_check(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	unsigned long	start_time;
+	u8 bReady;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	bReady = false;
+	start_time = jiffies;
+
+	cond_resched();
+
+	while (1) {
+		mutex_lock(&pwrpriv->lock);
+
+		if (padapter->bSurpriseRemoved ||
+		    !(padapter->hw_init_completed) ||
+		    (pwrpriv->pwr_mode == PS_MODE_ACTIVE))
+			bReady = true;
+
+		mutex_unlock(&pwrpriv->lock);
+
+		if (bReady)
+			break;
+
+		if (jiffies_to_msecs(jiffies - start_time) > 100)
+			break;
+
+		msleep(1);
+	}
+}
+
+/*
+ * Caller:ISR handler...
+ *
+ * This will be called when CPWM interrupt is up.
+ *
+ * using to update cpwn of drv; and drv willl make a decision to up or down pwr level
+ */
+void cpwm_int_hdl(struct adapter *padapter, struct reportpwrstate_parm *preportpwrstate)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	mutex_lock(&pwrpriv->lock);
+
+	if (pwrpriv->rpwm < PS_STATE_S2)
+		goto exit;
+
+	pwrpriv->cpwm = PS_STATE(preportpwrstate->state);
+	pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE;
+
+	if (pwrpriv->cpwm >= PS_STATE_S2) {
+		if (pwrpriv->alives & CMD_ALIVE)
+			complete(&padapter->cmdpriv.cmd_queue_comp);
+
+		if (pwrpriv->alives & XMIT_ALIVE)
+			complete(&padapter->xmitpriv.xmit_comp);
+	}
+
+exit:
+	mutex_unlock(&pwrpriv->lock);
+
+}
+
+static void cpwm_event_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	struct adapter *adapter = dvobj->if1;
+	struct reportpwrstate_parm report;
+
+	report.state = PS_STATE_S2;
+	cpwm_int_hdl(adapter, &report);
+}
+
+static void rpwmtimeout_workitem_callback(struct work_struct *work)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *dvobj;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi);
+	dvobj = pwrctl_to_dvobj(pwrpriv);
+	padapter = dvobj->if1;
+
+	mutex_lock(&pwrpriv->lock);
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+		goto exit;
+
+	mutex_unlock(&pwrpriv->lock);
+
+	if (rtw_read8(padapter, 0x100) != 0xEA) {
+		struct reportpwrstate_parm report;
+
+		report.state = PS_STATE_S2;
+		cpwm_int_hdl(padapter, &report);
+
+		return;
+	}
+
+	mutex_lock(&pwrpriv->lock);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+		goto exit;
+
+	pwrpriv->brpwmtimeout = true;
+	rtw_set_rpwm(padapter, pwrpriv->rpwm);
+	pwrpriv->brpwmtimeout = false;
+
+exit:
+	mutex_unlock(&pwrpriv->lock);
+}
+
+/*
+ * This function is a timer handler, can't do any IO in it.
+ */
+static void pwr_rpwm_timeout_handler(struct timer_list *t)
+{
+	struct pwrctrl_priv *pwrpriv = from_timer(pwrpriv, t, pwr_rpwm_timer);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+		return;
+
+	_set_workitem(&pwrpriv->rpwmtimeoutwi);
+}
+
+static inline void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives |= tag;
+}
+
+static inline void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives &= ~tag;
+}
+
+
+/*
+ * Description:
+ *Check if the fw_pwrstate is okay for I/O.
+ *If not (cpwm is less than S2), then the sub-routine
+ *will raise the cpwm to be greater than or equal to S2.
+ *
+ *Calling Context: Passive
+ *
+ *Constraint:
+ *	1. this function will request pwrctrl->lock
+ *
+ * Return Value:
+ *_SUCCESS	hardware is ready for I/O
+ *_FAIL		can't I/O right now
+ */
+s32 rtw_register_task_alive(struct adapter *padapter, u32 task)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	mutex_lock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, task);
+
+	if (pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+
+	if (res == _FAIL)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	return res;
+}
+
+/*
+ * Description:
+ *If task is done, call this func. to power down firmware again.
+ *
+ *Constraint:
+ *	1. this function will request pwrctrl->lock
+ *
+ * Return Value:
+ *none
+ */
+void rtw_unregister_task_alive(struct adapter *padapter, u32 task)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	if (!(hal_btcoex_IsBtDisabled(padapter)) && hal_btcoex_IsBtControlLps(padapter)) {
+		u8 val8;
+
+		val8 = hal_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	mutex_lock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, task);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) && pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+}
+
+/*
+ * Caller: rtw_xmit_thread
+ *
+ * Check if the fw_pwrstate is okay for xmit.
+ * If not (cpwm is less than S3), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S3.
+ *
+ * Calling Context: Passive
+ *
+ * Return Value:
+ * _SUCCESS	rtw_xmit_thread can write fifo/txcmd afterwards.
+ * _FAIL		rtw_xmit_thread can not do anything.
+ */
+s32 rtw_register_tx_alive(struct adapter *padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	mutex_lock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if (pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+
+	if (res == _FAIL)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	return res;
+}
+
+/*
+ * Caller: rtw_cmd_thread
+ *
+ * Check if the fw_pwrstate is okay for issuing cmd.
+ * If not (cpwm should be is less than S2), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S2.
+ *
+ * Calling Context: Passive
+ *
+ * Return Value:
+ *_SUCCESS	rtw_cmd_thread can issue cmds to firmware afterwards.
+ *_FAIL		rtw_cmd_thread can not do anything.
+ */
+s32 rtw_register_cmd_alive(struct adapter *padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	mutex_lock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, CMD_ALIVE);
+
+	if (pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+
+	if (res == _FAIL)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	return res;
+}
+
+/*
+ * Caller: ISR
+ *
+ * If ISR's txdone,
+ * No more pkts for TX,
+ * Then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_tx_alive(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	if (!(hal_btcoex_IsBtDisabled(padapter)) && hal_btcoex_IsBtControlLps(padapter)) {
+		u8 val8;
+
+		val8 = hal_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	mutex_lock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) && pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+}
+
+/*
+ * Caller: ISR
+ *
+ * If all commands have been done,
+ * and no more command to do,
+ * then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_cmd_alive(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	if (!(hal_btcoex_IsBtDisabled(padapter)) && hal_btcoex_IsBtControlLps(padapter)) {
+		u8 val8;
+
+		val8 = hal_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	mutex_lock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) && pwrctrl->fw_current_in_ps_mode) {
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	mutex_unlock(&pwrctrl->lock);
+}
+
+void rtw_init_pwrctrl_priv(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	mutex_init(&pwrctrlpriv->lock);
+	pwrctrlpriv->rf_pwrstate = rf_on;
+	pwrctrlpriv->ips_enter_cnts = 0;
+	pwrctrlpriv->ips_leave_cnts = 0;
+	pwrctrlpriv->bips_processing = false;
+
+	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+	pwrctrlpriv->pwr_state_check_cnts = 0;
+	pwrctrlpriv->bInternalAutoSuspend = false;
+	pwrctrlpriv->bInSuspend = false;
+	pwrctrlpriv->bkeepfwalive = false;
+
+	pwrctrlpriv->LpsIdleCount = 0;
+	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
+	pwrctrlpriv->bLeisurePs = pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
+
+	pwrctrlpriv->fw_current_in_ps_mode = false;
+
+	pwrctrlpriv->rpwm = 0;
+	pwrctrlpriv->cpwm = PS_STATE_S4;
+
+	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+	pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
+	pwrctrlpriv->bcn_ant_mode = 0;
+	pwrctrlpriv->dtim = 0;
+
+	pwrctrlpriv->tog = 0x80;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm));
+
+	_init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL);
+
+	pwrctrlpriv->brpwmtimeout = false;
+	pwrctrlpriv->adapter = padapter;
+	_init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
+	timer_setup(&pwrctrlpriv->pwr_rpwm_timer, pwr_rpwm_timeout_handler, 0);
+	timer_setup(&pwrctrlpriv->pwr_state_check_timer,
+		    pwr_state_check_handler, 0);
+
+	pwrctrlpriv->wowlan_mode = false;
+	pwrctrlpriv->wowlan_ap_mode = false;
+}
+
+
+void rtw_free_pwrctrl_priv(struct adapter *adapter)
+{
+}
+
+inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ms);
+}
+
+/*
+* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+* @adapter: pointer to struct adapter structure
+* @ips_deffer_ms: the ms will prevent from falling into IPS after wakeup
+* Return _SUCCESS or _FAIL
+*/
+
+int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
+	struct mlme_priv *pmlmepriv;
+	int ret = _SUCCESS;
+	unsigned long start = jiffies;
+	unsigned long deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
+
+	/* for LPS */
+	LeaveAllPowerSaveMode(padapter);
+
+	/* IPS still bound with primary adapter */
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (time_before(pwrpriv->ips_deny_time, deny_time))
+		pwrpriv->ips_deny_time = deny_time;
+
+
+	if (pwrpriv->ps_processing)
+		while (pwrpriv->ps_processing && jiffies_to_msecs(jiffies - start) <= 3000)
+			mdelay(10);
+
+	if (!(pwrpriv->bInternalAutoSuspend) && pwrpriv->bInSuspend)
+		while (pwrpriv->bInSuspend && jiffies_to_msecs(jiffies - start) <= 3000
+		)
+			mdelay(10);
+
+	/* System suspend is not allowed to wakeup */
+	if (!(pwrpriv->bInternalAutoSuspend) && pwrpriv->bInSuspend) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* block??? */
+	if (pwrpriv->bInternalAutoSuspend  && padapter->net_closed) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* I think this should be check in IPS, LPS, autosuspend functions... */
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+
+	if (rf_off == pwrpriv->rf_pwrstate) {
+		{
+			if (ips_leave(padapter) == _FAIL) {
+				ret = _FAIL;
+				goto exit;
+			}
+		}
+	}
+
+	/* TODO: the following checking need to be merged... */
+	if (padapter->bDriverStopped || !padapter->bup || !padapter->hw_init_completed) {
+		ret = false;
+		goto exit;
+	}
+
+exit:
+	deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, deny_time))
+		pwrpriv->ips_deny_time = deny_time;
+	return ret;
+
+}
+
+int rtw_pm_set_lps(struct adapter *padapter, u8 mode)
+{
+	int	ret = 0;
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (mode < PS_MODE_NUM) {
+		if (pwrctrlpriv->power_mgnt != mode) {
+			if (mode == PS_MODE_ACTIVE)
+				LeaveAllPowerSaveMode(padapter);
+			else
+				pwrctrlpriv->LpsIdleCount = 2;
+
+			pwrctrlpriv->power_mgnt = mode;
+			pwrctrlpriv->bLeisurePs =
+				pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
+		}
+	} else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+int rtw_pm_set_ips(struct adapter *padapter, u8 mode)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		return 0;
+	} else if (mode == IPS_NONE) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		if ((padapter->bSurpriseRemoved == 0) && (rtw_pwr_wakeup(padapter) == _FAIL))
+			return -EFAULT;
+	} else
+		return -EINVAL;
+
+	return 0;
+}
+
+/*
+ * ATTENTION:
+ *This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny(struct adapter *padapter, enum ps_deny_reason reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	mutex_lock(&pwrpriv->lock);
+	pwrpriv->ps_deny |= BIT(reason);
+	mutex_unlock(&pwrpriv->lock);
+}
+
+/*
+ * ATTENTION:
+ *This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny_cancel(struct adapter *padapter, enum ps_deny_reason reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	mutex_lock(&pwrpriv->lock);
+	pwrpriv->ps_deny &= ~BIT(reason);
+	mutex_unlock(&pwrpriv->lock);
+}
+
+/*
+ * ATTENTION:
+ *Before calling this function pwrctrl lock should be occupied already,
+ *otherwise it may return incorrect value.
+ */
+u32 rtw_ps_deny_get(struct adapter *padapter)
+{
+	return adapter_to_pwrctl(padapter)->ps_deny;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_recv.c b/drivers/staging/rtl8723bs/core/rtw_recv.c
new file mode 100644
index 000000000..2825375bf
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_recv.c
@@ -0,0 +1,2163 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+#include <rtw_recv.h>
+#include <net/cfg80211.h>
+#include <asm/unaligned.h>
+
+static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+static void rtw_signal_stat_timer_hdl(struct timer_list *t);
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+	memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv));
+
+	spin_lock_init(&psta_recvpriv->lock);
+
+	/* for (i = 0; i<MAX_RX_NUMBLKS; i++) */
+	/* _rtw_init_queue(&psta_recvpriv->blk_strms[i]); */
+
+	INIT_LIST_HEAD(&psta_recvpriv->defrag_q.queue);
+	spin_lock_init(&psta_recvpriv->defrag_q.lock);
+}
+
+signed int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
+{
+	signed int i;
+	union recv_frame *precvframe;
+	signed int	res = _SUCCESS;
+
+	spin_lock_init(&precvpriv->lock);
+
+	INIT_LIST_HEAD(&precvpriv->free_recv_queue.queue);
+	spin_lock_init(&precvpriv->free_recv_queue.lock);
+	INIT_LIST_HEAD(&precvpriv->recv_pending_queue.queue);
+	spin_lock_init(&precvpriv->recv_pending_queue.lock);
+	INIT_LIST_HEAD(&precvpriv->uc_swdec_pending_queue.queue);
+	spin_lock_init(&precvpriv->uc_swdec_pending_queue.lock);
+
+	precvpriv->adapter = padapter;
+
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+	precvpriv->pallocated_frame_buf = vzalloc(NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+
+	if (!precvpriv->pallocated_frame_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ);
+	/* precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf + RXFRAME_ALIGN_SZ - */
+	/* ((SIZE_PTR) (precvpriv->pallocated_frame_buf) &(RXFRAME_ALIGN_SZ-1)); */
+
+	precvframe = (union recv_frame *) precvpriv->precv_frame_buf;
+
+
+	for (i = 0; i < NR_RECVFRAME; i++) {
+		INIT_LIST_HEAD(&(precvframe->u.list));
+
+		list_add_tail(&(precvframe->u.list), &(precvpriv->free_recv_queue.queue));
+
+		rtw_os_recv_resource_alloc(padapter, precvframe);
+
+		precvframe->u.hdr.len = 0;
+
+		precvframe->u.hdr.adapter = padapter;
+		precvframe++;
+
+	}
+
+	res = rtw_hal_init_recv_priv(padapter);
+
+	timer_setup(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl,
+		    0);
+
+	precvpriv->signal_stat_sampling_interval = 2000; /* ms */
+
+	rtw_set_signal_stat_timer(precvpriv);
+
+exit:
+	return res;
+}
+
+void _rtw_free_recv_priv(struct recv_priv *precvpriv)
+{
+	struct adapter	*padapter = precvpriv->adapter;
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	rtw_os_recv_resource_free(precvpriv);
+
+	vfree(precvpriv->pallocated_frame_buf);
+
+	rtw_hal_free_recv_priv(padapter);
+}
+
+union recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+
+	union recv_frame  *precvframe;
+	struct list_head	*plist, *phead;
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	if (list_empty(&pfree_recv_queue->queue))
+		precvframe = NULL;
+	else {
+		phead = get_list_head(pfree_recv_queue);
+
+		plist = get_next(phead);
+
+		precvframe = (union recv_frame *)plist;
+
+		list_del_init(&precvframe->u.hdr.list);
+		padapter = precvframe->u.hdr.adapter;
+		if (padapter) {
+			precvpriv = &padapter->recvpriv;
+			if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt--;
+		}
+	}
+	return precvframe;
+}
+
+union recv_frame *rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+	union recv_frame  *precvframe;
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+	spin_unlock_bh(&pfree_recv_queue->lock);
+
+	return precvframe;
+}
+
+int rtw_free_recvframe(union recv_frame *precvframe, struct __queue *pfree_recv_queue)
+{
+	struct adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	rtw_os_free_recvframe(precvframe);
+
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	list_del_init(&(precvframe->u.hdr.list));
+
+	precvframe->u.hdr.len = 0;
+
+	list_add_tail(&(precvframe->u.hdr.list), get_list_head(pfree_recv_queue));
+
+	if (padapter) {
+		if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt++;
+	}
+	spin_unlock_bh(&pfree_recv_queue->lock);
+	return _SUCCESS;
+}
+
+
+
+
+signed int _rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue)
+{
+
+	struct adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	/* INIT_LIST_HEAD(&(precvframe->u.hdr.list)); */
+	list_del_init(&(precvframe->u.hdr.list));
+
+
+	list_add_tail(&(precvframe->u.hdr.list), get_list_head(queue));
+
+	if (padapter)
+		if (queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+
+	return _SUCCESS;
+}
+
+signed int rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue)
+{
+	signed int ret;
+
+	/* _spinlock(&pfree_recv_queue->lock); */
+	spin_lock_bh(&queue->lock);
+	ret = _rtw_enqueue_recvframe(precvframe, queue);
+	/* spin_unlock(&pfree_recv_queue->lock); */
+	spin_unlock_bh(&queue->lock);
+
+	return ret;
+}
+
+/*
+ * caller : defrag ; recvframe_chk_defrag in recv_thread  (passive)
+ * pframequeue: defrag_queue : will be accessed in recv_thread  (passive)
+ *
+ * using spinlock to protect
+ *
+ */
+
+void rtw_free_recvframe_queue(struct __queue *pframequeue,  struct __queue *pfree_recv_queue)
+{
+	union	recv_frame	*precvframe;
+	struct list_head	*plist, *phead;
+
+	spin_lock(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		precvframe = (union recv_frame *)plist;
+
+		plist = get_next(plist);
+
+		rtw_free_recvframe(precvframe, pfree_recv_queue);
+	}
+
+	spin_unlock(&pframequeue->lock);
+}
+
+u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter)
+{
+	u32 cnt = 0;
+	union recv_frame *pending_frame;
+
+	while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) {
+		rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue);
+		cnt++;
+	}
+
+	return cnt;
+}
+
+
+signed int rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&precvbuf->list);
+	list_add(&precvbuf->list, get_list_head(queue));
+
+	spin_unlock_bh(&queue->lock);
+
+	return _SUCCESS;
+}
+
+signed int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&precvbuf->list);
+
+	list_add_tail(&precvbuf->list, get_list_head(queue));
+	spin_unlock_bh(&queue->lock);
+	return _SUCCESS;
+
+}
+
+struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue)
+{
+	struct recv_buf *precvbuf;
+	struct list_head	*plist, *phead;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->queue))
+		precvbuf = NULL;
+	else {
+		phead = get_list_head(queue);
+
+		plist = get_next(phead);
+
+		precvbuf = container_of(plist, struct recv_buf, list);
+
+		list_del_init(&precvbuf->list);
+
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	return precvbuf;
+
+}
+
+static signed int recvframe_chkmic(struct adapter *adapter,  union recv_frame *precvframe)
+{
+
+	signed int	i, res = _SUCCESS;
+	u32 datalen;
+	u8 miccode[8];
+	u8 bmic_err = false, brpt_micerror = true;
+	u8 *pframe, *payload, *pframemic;
+	u8 *mickey;
+	/* u8 *iv, rxdata_key_idx = 0; */
+	struct sta_info *stainfo;
+	struct rx_pkt_attrib *prxattrib = &precvframe->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	stainfo = rtw_get_stainfo(&adapter->stapriv, &prxattrib->ta[0]);
+
+	if (prxattrib->encrypt == _TKIP_) {
+		/* calculate mic code */
+		if (stainfo) {
+			if (IS_MCAST(prxattrib->ra)) {
+				/* mickey =&psecuritypriv->dot118021XGrprxmickey.skey[0]; */
+				/* iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen; */
+				/* rxdata_key_idx =(((iv[3])>>6)&0x3) ; */
+				mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+
+				/* psecuritypriv->dot118021XGrpKeyid, pmlmeinfo->key_index, rxdata_key_idx); */
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+					goto exit;
+				}
+			} else {
+				mickey = &stainfo->dot11tkiprxmickey.skey[0];
+			}
+
+			datalen = precvframe->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len-prxattrib->icv_len-8;/* icv_len included the mic code */
+			pframe = precvframe->u.hdr.rx_data;
+			payload = pframe+prxattrib->hdrlen+prxattrib->iv_len;
+
+			rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0], (unsigned char)prxattrib->priority); /* care the length of the data */
+
+			pframemic = payload+datalen;
+
+			bmic_err = false;
+
+			for (i = 0; i < 8; i++) {
+				if (miccode[i] != *(pframemic + i))
+					bmic_err = true;
+			}
+
+
+			if (bmic_err == true) {
+				/*  double check key_index for some timing issue , */
+				/*  cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */
+				if ((IS_MCAST(prxattrib->ra) == true)  && (prxattrib->key_index != pmlmeinfo->key_index))
+					brpt_micerror = false;
+
+				if (prxattrib->bdecrypted && brpt_micerror)
+					rtw_handle_tkip_mic_err(adapter, (u8)IS_MCAST(prxattrib->ra));
+
+				res = _FAIL;
+
+			} else {
+				/* mic checked ok */
+				if (!psecuritypriv->bcheck_grpkey &&
+				    IS_MCAST(prxattrib->ra))
+					psecuritypriv->bcheck_grpkey = true;
+			}
+		}
+
+		recvframe_pull_tail(precvframe, 8);
+
+	}
+
+exit:
+	return res;
+
+}
+
+/* decrypt and set the ivlen, icvlen of the recv_frame */
+static union recv_frame *decryptor(struct adapter *padapter, union recv_frame *precv_frame)
+{
+
+	struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	union recv_frame *return_packet = precv_frame;
+	u32  res = _SUCCESS;
+
+	if (prxattrib->encrypt > 0) {
+		u8 *iv = precv_frame->u.hdr.rx_data+prxattrib->hdrlen;
+
+		prxattrib->key_index = (((iv[3])>>6)&0x3);
+
+		if (prxattrib->key_index > WEP_KEYS) {
+			switch (prxattrib->encrypt) {
+			case _WEP40_:
+			case _WEP104_:
+				prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+				break;
+			case _TKIP_:
+			case _AES_:
+			default:
+				prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+				break;
+			}
+		}
+	}
+
+	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt == true))) {
+		psecuritypriv->hw_decrypted = false;
+
+		switch (prxattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _TKIP_:
+			res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _AES_:
+			res = rtw_aes_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		default:
+				break;
+		}
+	} else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
+		   (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_)
+		) {
+		psecuritypriv->hw_decrypted = true;
+	} else {
+	}
+
+	if (res == _FAIL) {
+		rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue);
+		return_packet = NULL;
+	} else
+		prxattrib->bdecrypted = true;
+
+	return return_packet;
+}
+
+/* set the security information in the recv_frame */
+static union recv_frame *portctrl(struct adapter *adapter, union recv_frame *precv_frame)
+{
+	u8 *psta_addr = NULL;
+	u8 *ptr;
+	uint  auth_alg;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	union recv_frame *prtnframe;
+	u16 ether_type = 0;
+	u16  eapol_type = 0x888e;/* for Funia BD's WPA issue */
+	struct rx_pkt_attrib *pattrib;
+
+	pstapriv = &adapter->stapriv;
+
+	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
+
+	ptr = precv_frame->u.hdr.rx_data;
+	pfhdr = &precv_frame->u.hdr;
+	pattrib = &pfhdr->attrib;
+	psta_addr = pattrib->ta;
+
+	prtnframe = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+
+	if (auth_alg == 2) {
+		if ((psta) && (psta->ieee8021x_blocked)) {
+			__be16 be_tmp;
+
+			/* blocked */
+			/* only accept EAPOL frame */
+
+			prtnframe = precv_frame;
+
+			/* get ether_type */
+			ptr = ptr + pfhdr->attrib.hdrlen + pfhdr->attrib.iv_len + LLC_HEADER_LENGTH;
+			memcpy(&be_tmp, ptr, 2);
+			ether_type = ntohs(be_tmp);
+
+			if (ether_type == eapol_type)
+				prtnframe = precv_frame;
+			else {
+				/* free this frame */
+				rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue);
+				prtnframe = NULL;
+			}
+		} else {
+			/* allowed */
+			/* check decryption status, and decrypt the frame if needed */
+
+			prtnframe = precv_frame;
+			/* check is the EAPOL frame or not (Rekey) */
+			/* if (ether_type == eapol_type) { */
+				/* check Rekey */
+
+			/* prtnframe =precv_frame; */
+			/*  */
+			/* else { */
+			/*  */
+		}
+	} else
+		prtnframe = precv_frame;
+
+	return prtnframe;
+}
+
+static signed int recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+	signed int tid = precv_frame->u.hdr.attrib.priority;
+
+	u16 seq_ctrl = ((precv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(precv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (tid > 15)
+		return _FAIL;
+
+	if (1) { /* if (bretry) */
+		if (seq_ctrl == prxcache->tid_rxseq[tid])
+			return _FAIL;
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+	return _SUCCESS;
+
+}
+
+static void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned char pwrbit;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	pwrbit = GetPwrMgt(ptr);
+
+	if (psta) {
+		if (pwrbit) {
+			if (!(psta->state & WIFI_SLEEP_STATE)) {
+				/* psta->state |= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap |= BIT(psta->aid); */
+
+				stop_sta_xmit(padapter, psta);
+
+			}
+		} else {
+			if (psta->state & WIFI_SLEEP_STATE) {
+				/* psta->state ^= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); */
+
+				wakeup_sta_to_xmit(padapter, psta);
+			}
+		}
+
+	}
+}
+
+static void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	if (!psta)
+		return;
+
+	if (!psta->qos_option)
+		return;
+
+	if (!(psta->qos_info&0xf))
+		return;
+
+	if (psta->state&WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		switch (pattrib->priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk&BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi&BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo&BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be&BIT(1);
+			break;
+		}
+
+		if (wmmps_ac) {
+			if (psta->sleepq_ac_len > 0)
+				/* process received triggered frame */
+				xmit_delivery_enabled_frames(padapter, psta);
+			else
+				/* issue one qos null frame with More data bit = 0 and the EOSP bit set (= 1) */
+				issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0);
+		}
+	}
+}
+
+static void count_rx_stats(struct adapter *padapter, union recv_frame *prframe, struct sta_info *sta)
+{
+	int sz;
+	struct sta_info *psta = NULL;
+	struct stainfo_stats *pstats = NULL;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	sz = get_recvframe_len(prframe);
+	precvpriv->rx_bytes += sz;
+
+	padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+	if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst)))
+		padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+
+	if (sta)
+		psta = sta;
+	else
+		psta = prframe->u.hdr.psta;
+
+	if (psta) {
+		pstats = &psta->sta_stats;
+
+		pstats->rx_data_pkts++;
+		pstats->rx_bytes += sz;
+	}
+
+	traffic_check_for_leave_lps(padapter, false, 0);
+}
+
+static signed int sta2sta_data_frame(struct adapter *adapter, union recv_frame *precv_frame,
+			struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	signed int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	u8 *sta_addr = NULL;
+	signed int bmcast = IS_MCAST(pattrib->dst);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN))	&& (!bmcast)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		   !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		   (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		sta_addr = pattrib->src;
+
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+		/*  For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */
+		if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		sta_addr = pattrib->bssid;
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		if (bmcast) {
+			/*  For AP mode, if DA == MCAST, then BSSID should be also MCAST */
+			if (!IS_MCAST(pattrib->bssid)) {
+					ret = _FAIL;
+					goto exit;
+			}
+		} else { /*  not mc-frame */
+			/*  For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */
+			if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+				ret = _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->src;
+		}
+
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		sta_addr = mybssid;
+	} else
+		ret  = _FAIL;
+
+
+
+	if (bmcast)
+		*psta = rtw_get_bcmc_stainfo(adapter);
+	else
+		*psta = rtw_get_stainfo(pstapriv, sta_addr); /*  get ap_info */
+
+	if (!*psta) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+static signed int ap2sta_data_frame(struct adapter *adapter, union recv_frame *precv_frame,
+		       struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	signed int ret = _SUCCESS;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	signed int bmcast = IS_MCAST(pattrib->dst);
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) &&
+	    (check_fwstate(pmlmepriv, _FW_LINKED) == true ||
+	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)
+		) {
+
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*  da should be for me */
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+		/*  check BSSID */
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		     !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		     (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+
+			if (!bmcast)
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			*psta = rtw_get_bcmc_stainfo(adapter);
+		else
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get ap_info */
+
+		if (!*psta) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
+		     (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		/*  */
+		memcpy(pattrib->bssid,  mybssid, ETH_ALEN);
+
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get sta_info */
+		if (!*psta) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		/* Special case */
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	} else {
+		if (!memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) {
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get sta_info */
+			if (!*psta) {
+
+				/* for AP multicast issue , modify by yiwei */
+				static unsigned long send_issue_deauth_time;
+
+				if (jiffies_to_msecs(jiffies - send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) {
+					send_issue_deauth_time = jiffies;
+
+					issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+				}
+			}
+		}
+
+		ret = _FAIL;
+	}
+
+exit:
+	return ret;
+}
+
+static signed int sta2ap_data_frame(struct adapter *adapter, union recv_frame *precv_frame,
+		       struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	unsigned char *mybssid  = get_bssid(pmlmepriv);
+	signed int ret = _SUCCESS;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		/* For AP mode, RA =BSSID, TX =STA(SRC_ADDR), A3 =DST_ADDR */
+		if (memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->src);
+		if (!*psta) {
+			issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+		process_pwrbit_data(adapter, precv_frame);
+
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE)
+			process_wmmps_data(adapter, precv_frame);
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	} else {
+		u8 *myhwaddr = myid(&adapter->eeprompriv);
+
+		if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+static signed int validate_recv_ctrl_frame(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = NULL;
+	/* uint len = precv_frame->u.hdr.len; */
+
+	if (GetFrameType(pframe) != WIFI_CTRL_TYPE)
+		return _FAIL;
+
+	/* receive the frames that ra(a1) is my address */
+	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN))
+		return _FAIL;
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (!psta)
+		return _FAIL;
+
+	/* for rx pkt statistics */
+	psta->sta_stats.rx_ctrl_pkts++;
+
+	/* only handle ps-poll */
+	if (GetFrameSubType(pframe) == WIFI_PSPOLL) {
+		u16 aid;
+		u8 wmmps_ac = 0;
+
+		aid = GetAid(pframe);
+		if (psta->aid != aid)
+			return _FAIL;
+
+		switch (pattrib->priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk&BIT(0);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi&BIT(0);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo&BIT(0);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be&BIT(0);
+			break;
+		}
+
+		if (wmmps_ac)
+			return _FAIL;
+
+		if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		if ((psta->state&WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap&BIT(psta->aid))) {
+			struct list_head	*xmitframe_plist, *xmitframe_phead;
+			struct xmit_frame *pxmitframe = NULL;
+			struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+			spin_lock_bh(&pxmitpriv->lock);
+
+			xmitframe_phead = get_list_head(&psta->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			if (xmitframe_phead != xmitframe_plist) {
+				pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+				xmitframe_plist = get_next(xmitframe_plist);
+
+				list_del_init(&pxmitframe->list);
+
+				psta->sleepq_len--;
+
+				if (psta->sleepq_len > 0)
+					pxmitframe->attrib.mdata = 1;
+				else
+					pxmitframe->attrib.mdata = 0;
+
+				pxmitframe->attrib.triggered = 1;
+
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+				if (psta->sleepq_len == 0) {
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* update BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+				}
+
+				/* spin_unlock_bh(&psta->sleep_q.lock); */
+				spin_unlock_bh(&pxmitpriv->lock);
+
+			} else {
+				/* spin_unlock_bh(&psta->sleep_q.lock); */
+				spin_unlock_bh(&pxmitpriv->lock);
+
+				if (pstapriv->tim_bitmap&BIT(psta->aid)) {
+					if (psta->sleepq_len == 0) {
+						/* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+						issue_nulldata_in_interrupt(padapter, psta->hwaddr);
+					} else {
+						psta->sleepq_len = 0;
+					}
+
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* update BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+				}
+			}
+		}
+	}
+
+	return _FAIL;
+
+}
+
+/* perform defrag */
+static union recv_frame *recvframe_defrag(struct adapter *adapter,
+					  struct __queue *defrag_q)
+{
+	struct list_head	 *plist, *phead;
+	u8  wlanhdr_offset;
+	u8 curfragnum;
+	struct recv_frame_hdr *pfhdr, *pnfhdr;
+	union recv_frame *prframe, *pnextrframe;
+	struct __queue	*pfree_recv_queue;
+
+	curfragnum = 0;
+	pfree_recv_queue = &adapter->recvpriv.free_recv_queue;
+
+	phead = get_list_head(defrag_q);
+	plist = get_next(phead);
+	prframe = (union recv_frame *)plist;
+	pfhdr = &prframe->u.hdr;
+	list_del_init(&(prframe->u.list));
+
+	if (curfragnum != pfhdr->attrib.frag_num) {
+		/* the first fragment number must be 0 */
+		/* free the whole queue */
+		rtw_free_recvframe(prframe, pfree_recv_queue);
+		rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+		return NULL;
+	}
+
+	curfragnum++;
+
+	plist = get_list_head(defrag_q);
+
+	plist = get_next(plist);
+
+	while (phead != plist) {
+		pnextrframe = (union recv_frame *)plist;
+		pnfhdr = &pnextrframe->u.hdr;
+
+
+		/* check the fragment sequence  (2nd ~n fragment frame) */
+
+		if (curfragnum != pnfhdr->attrib.frag_num) {
+			/* the fragment number must be increasing  (after decache) */
+			/* release the defrag_q & prframe */
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+			rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+			return NULL;
+		}
+
+		curfragnum++;
+
+		/* copy the 2nd~n fragment frame's payload to the first fragment */
+		/* get the 2nd~last fragment frame's payload */
+
+		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+
+		recvframe_pull(pnextrframe, wlanhdr_offset);
+
+		/* append  to first fragment frame's tail (if privacy frame, pull the ICV) */
+		recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+
+		/* memcpy */
+		memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+
+		recvframe_put(prframe, pnfhdr->len);
+
+		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		plist = get_next(plist);
+
+	}
+
+	/* free the defrag_q queue and return the prframe */
+	rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+static union recv_frame *recvframe_chk_defrag(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 ismfrag;
+	u8 fragnum;
+	u8 *psta_addr;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	struct list_head *phead;
+	union recv_frame *prtnframe = NULL;
+	struct __queue *pfree_recv_queue, *pdefrag_q;
+
+	pstapriv = &padapter->stapriv;
+
+	pfhdr = &precv_frame->u.hdr;
+
+	pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/* need to define struct of wlan header frame ctrl */
+	ismfrag = pfhdr->attrib.mfrag;
+	fragnum = pfhdr->attrib.frag_num;
+
+	psta_addr = pfhdr->attrib.ta;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+	if (!psta) {
+		u8 type = GetFrameType(pfhdr->rx_data);
+
+		if (type != WIFI_DATA_TYPE) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+			pdefrag_q = &psta->sta_recvpriv.defrag_q;
+		} else
+			pdefrag_q = NULL;
+	} else
+		pdefrag_q = &psta->sta_recvpriv.defrag_q;
+
+	if ((ismfrag == 0) && (fragnum == 0))
+		prtnframe = precv_frame;/* isn't a fragment frame */
+
+	if (ismfrag == 1) {
+		/* 0~(n-1) fragment frame */
+		/* enqueue to defraf_g */
+		if (pdefrag_q) {
+			if (fragnum == 0)
+				/* the first fragment */
+				if (!list_empty(&pdefrag_q->queue))
+					/* free current defrag_q */
+					rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue);
+
+
+			/* Then enqueue the 0~(n-1) fragment into the defrag_q */
+
+			/* spin_lock(&pdefrag_q->lock); */
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+			/* spin_unlock(&pdefrag_q->lock); */
+
+			prtnframe = NULL;
+
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+
+	}
+
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame */
+		/* enqueue the last fragment */
+		if (pdefrag_q) {
+			/* spin_lock(&pdefrag_q->lock); */
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+			/* spin_unlock(&pdefrag_q->lock); */
+
+			/* call recvframe_defrag to defrag */
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe = precv_frame;
+
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+
+	}
+
+
+	if ((prtnframe) && (prtnframe->u.hdr.attrib.privacy)) {
+		/* after defrag we must check tkip mic code */
+		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
+			rtw_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	return prtnframe;
+}
+
+static signed int validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	/* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */
+
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (!precv_frame)
+		return _SUCCESS;
+
+	{
+		/* for rx pkt statistics */
+		struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(precv_frame->u.hdr.rx_data));
+
+		if (psta) {
+			psta->sta_stats.rx_mgnt_pkts++;
+			if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_BEACON)
+				psta->sta_stats.rx_beacon_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ)
+				psta->sta_stats.rx_probereq_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) {
+				if (!memcmp(padapter->eeprompriv.mac_addr, GetAddr1Ptr(precv_frame->u.hdr.rx_data), ETH_ALEN))
+					psta->sta_stats.rx_probersp_pkts++;
+				else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)) ||
+					 is_multicast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)))
+					psta->sta_stats.rx_probersp_bm_pkts++;
+				else
+					psta->sta_stats.rx_probersp_uo_pkts++;
+			}
+		}
+	}
+
+	mgt_dispatcher(padapter, precv_frame);
+
+	return _SUCCESS;
+
+}
+
+static signed int validate_recv_data_frame(struct adapter *adapter, union recv_frame *precv_frame)
+{
+	u8 bretry;
+	u8 *psa, *pda, *pbssid;
+	struct sta_info *psta = NULL;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	signed int ret = _SUCCESS;
+
+	bretry = GetRetry(ptr);
+	pda = get_da(ptr);
+	psa = get_sa(ptr);
+	pbssid = get_hdr_bssid(ptr);
+
+	if (!pbssid) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	memcpy(pattrib->dst, pda, ETH_ALEN);
+	memcpy(pattrib->src, psa, ETH_ALEN);
+
+	memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+	switch (pattrib->to_fr_ds) {
+	case 0:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		ret = sta2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 1:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, pbssid, ETH_ALEN);
+		ret = ap2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 2:
+		memcpy(pattrib->ra, pbssid, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		ret = sta2ap_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 3:
+		memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+		ret = _FAIL;
+		break;
+
+	default:
+		ret = _FAIL;
+		break;
+
+	}
+
+	if (ret == _FAIL) {
+		goto exit;
+	} else if (ret == RTW_RX_HANDLED) {
+		goto exit;
+	}
+
+
+	if (!psta) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* psta->rssi = prxcmd->rssi; */
+	/* psta->signal_quality = prxcmd->sq; */
+	precv_frame->u.hdr.psta = psta;
+
+
+	pattrib->amsdu = 0;
+	pattrib->ack_policy = 0;
+	/* parsing QC field */
+	if (pattrib->qos == 1) {
+		pattrib->priority = GetPriority((ptr + 24));
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26;
+
+		if (pattrib->priority != 0 && pattrib->priority != 3)
+			adapter->recvpriv.bIsAnyNonBEPkts = true;
+
+	} else {
+		pattrib->priority = 0;
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24;
+	}
+
+
+	if (pattrib->order)/* HT-CTRL 11n */
+		pattrib->hdrlen += 4;
+
+	precv_frame->u.hdr.preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+	/*  decache, drop duplicate recv packets */
+	if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) == _FAIL) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (pattrib->privacy) {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+	} else {
+		pattrib->encrypt = 0;
+		pattrib->iv_len = pattrib->icv_len = 0;
+	}
+
+exit:
+	return ret;
+}
+
+static signed int validate_80211w_mgmt(struct adapter *adapter, union recv_frame *precv_frame)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8 subtype;
+
+	subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */
+
+	/* only support station mode */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    adapter->securitypriv.binstallBIPkey == true) {
+		/* unicast management frame decrypt */
+		if (pattrib->privacy && !(IS_MCAST(GetAddr1Ptr(ptr))) &&
+			(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC || subtype == WIFI_ACTION)) {
+			u8 *mgmt_DATA;
+			u32 data_len = 0;
+
+			pattrib->bdecrypted = 0;
+			pattrib->encrypt = _AES_;
+			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+			/* set iv and icv length */
+			SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+			memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+			memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+			/* actual management data frame body */
+			data_len = pattrib->pkt_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+			mgmt_DATA = rtw_zmalloc(data_len);
+			if (!mgmt_DATA)
+				goto validate_80211w_fail;
+			precv_frame = decryptor(adapter, precv_frame);
+			/* save actual management data frame body */
+			memcpy(mgmt_DATA, ptr+pattrib->hdrlen+pattrib->iv_len, data_len);
+			/* overwrite the iv field */
+			memcpy(ptr+pattrib->hdrlen, mgmt_DATA, data_len);
+			/* remove the iv and icv length */
+			pattrib->pkt_len = pattrib->pkt_len - pattrib->iv_len - pattrib->icv_len;
+			kfree(mgmt_DATA);
+			if (!precv_frame)
+				goto validate_80211w_fail;
+		} else if (IS_MCAST(GetAddr1Ptr(ptr)) &&
+			(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC)) {
+			signed int BIP_ret = _SUCCESS;
+			/* verify BIP MME IE of broadcast/multicast de-auth/disassoc packet */
+			BIP_ret = rtw_BIP_verify(adapter, (u8 *)precv_frame);
+			if (BIP_ret == _FAIL) {
+				goto validate_80211w_fail;
+			} else if (BIP_ret == RTW_RX_HANDLED) {
+				/* issue sa query request */
+				issue_action_SA_Query(adapter, NULL, 0, 0);
+				goto validate_80211w_fail;
+			}
+		} else { /* 802.11w protect */
+			if (subtype == WIFI_ACTION) {
+				/* according 802.11-2012 standard, these five types are not robust types */
+				if (ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_PUBLIC          &&
+					ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_HT              &&
+					ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_UNPROTECTED_WNM &&
+					ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_SELF_PROTECTED  &&
+					ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_P2P) {
+					goto validate_80211w_fail;
+				}
+			} else if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) {
+				/* issue sa query request */
+				issue_action_SA_Query(adapter, NULL, 0, 0);
+				goto validate_80211w_fail;
+			}
+		}
+	}
+	return _SUCCESS;
+
+validate_80211w_fail:
+	return _FAIL;
+
+}
+
+static signed int validate_recv_frame(struct adapter *adapter, union recv_frame *precv_frame)
+{
+	/* shall check frame subtype, to / from ds, da, bssid */
+
+	/* then call check if rx seq/frag. duplicated. */
+
+	u8 type;
+	u8 subtype;
+	signed int retval = _SUCCESS;
+	u8 bDumpRxPkt;
+
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8  ver = (unsigned char) (*ptr)&0x3;
+
+	/* add version chk */
+	if (ver != 0) {
+		retval = _FAIL;
+		goto exit;
+	}
+
+	type =  GetFrameType(ptr);
+	subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */
+
+	pattrib->to_fr_ds = get_tofr_ds(ptr);
+
+	pattrib->frag_num = GetFragNum(ptr);
+	pattrib->seq_num = GetSequence(ptr);
+
+	pattrib->pw_save = GetPwrMgt(ptr);
+	pattrib->mfrag = GetMFrag(ptr);
+	pattrib->mdata = GetMData(ptr);
+	pattrib->privacy = GetPrivacy(ptr);
+	pattrib->order = GetOrder(ptr);
+	rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+
+	switch (type) {
+	case WIFI_MGT_TYPE: /* mgnt */
+		if (validate_80211w_mgmt(adapter, precv_frame) == _FAIL) {
+			retval = _FAIL;
+			break;
+		}
+
+		retval = validate_recv_mgnt_frame(adapter, precv_frame);
+		retval = _FAIL; /*  only data frame return _SUCCESS */
+		break;
+	case WIFI_CTRL_TYPE: /* ctrl */
+		retval = validate_recv_ctrl_frame(adapter, precv_frame);
+		retval = _FAIL; /*  only data frame return _SUCCESS */
+		break;
+	case WIFI_DATA_TYPE: /* data */
+		pattrib->qos = (subtype & BIT(7)) ? 1:0;
+		retval = validate_recv_data_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			struct recv_priv *precvpriv = &adapter->recvpriv;
+
+			precvpriv->rx_drop++;
+		} else if (retval == _SUCCESS) {
+#ifdef DBG_RX_DUMP_EAP
+			u8 bDumpRxPkt;
+			u16 eth_type;
+
+			/*  dump eapol */
+			rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+			/*  get ether_type */
+			memcpy(&eth_type, ptr + pattrib->hdrlen + pattrib->iv_len + LLC_HEADER_LENGTH, 2);
+			eth_type = ntohs((unsigned short) eth_type);
+#endif
+		}
+		break;
+	default:
+		retval = _FAIL;
+		break;
+	}
+
+exit:
+	return retval;
+}
+
+/* remove the wlanhdr and add the eth_hdr */
+static signed int wlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	signed int	rmv_len;
+	u16 eth_type, len;
+	u8 bsnaphdr;
+	u8 *psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+	__be16 be_tmp;
+	struct adapter			*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *ptr = precvframe->u.hdr.rx_data; /*  point to frame_ctrl field */
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+
+	if (pattrib->encrypt)
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+
+	psnap = (struct ieee80211_snap_hdr	*)(ptr+pattrib->hdrlen + pattrib->iv_len);
+	psnap_type = ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	/* eth_type = (psnap_type[0] << 8) | psnap_type[1]; */
+	if ((!memcmp(psnap, rfc1042_header, SNAP_SIZE) &&
+		(memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2)) &&
+		(memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2))) ||
+		/* eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || */
+		 !memcmp(psnap, bridge_tunnel_header, SNAP_SIZE)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+		bsnaphdr = true;
+	} else
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = false;
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr?SNAP_SIZE:0);
+	len = precvframe->u.hdr.len - rmv_len;
+
+	memcpy(&be_tmp, ptr+rmv_len, 2);
+	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = eth_type;
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+		ptr += rmv_len;
+		*ptr = 0x87;
+		*(ptr+1) = 0x12;
+
+		eth_type = 0x8712;
+		/*  append rx status for mp test packets */
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24);
+		if (!ptr)
+			return _FAIL;
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else {
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr) + (bsnaphdr?2:0)));
+		if (!ptr)
+			return _FAIL;
+	}
+
+	memcpy(ptr, pattrib->dst, ETH_ALEN);
+	memcpy(ptr+ETH_ALEN, pattrib->src, ETH_ALEN);
+
+	if (!bsnaphdr) {
+		be_tmp = htons(len);
+		memcpy(ptr+12, &be_tmp, 2);
+	}
+
+	return _SUCCESS;
+}
+
+static int amsdu_to_msdu(struct adapter *padapter, union recv_frame *prframe)
+{
+	int	a_len, padding_len;
+	u16 nSubframe_Length;
+	u8 nr_subframes, i;
+	u8 *pdata;
+	struct sk_buff *sub_pkt, *subframes[MAX_SUBFRAME_COUNT];
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
+
+	nr_subframes = 0;
+
+	recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen);
+
+	if (prframe->u.hdr.attrib.iv_len > 0)
+		recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len);
+
+	a_len = prframe->u.hdr.len;
+
+	pdata = prframe->u.hdr.rx_data;
+
+	while (a_len > ETH_HLEN) {
+
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = get_unaligned_be16(pdata + 12);
+
+		if (a_len < ETH_HLEN + nSubframe_Length)
+			break;
+
+		sub_pkt = rtw_os_alloc_msdu_pkt(prframe, nSubframe_Length, pdata);
+		if (!sub_pkt)
+			break;
+
+		/* move the data point to data content */
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+
+		subframes[nr_subframes++] = sub_pkt;
+
+		if (nr_subframes >= MAX_SUBFRAME_COUNT)
+			break;
+
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if (a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4-1));
+			if (padding_len == 4)
+				padding_len = 0;
+
+			if (a_len < padding_len)
+				break;
+
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+
+	for (i = 0; i < nr_subframes; i++) {
+		sub_pkt = subframes[i];
+
+		/* Indicate the packets to upper layer */
+		if (sub_pkt)
+			rtw_os_recv_indicate_pkt(padapter, sub_pkt, &prframe->u.hdr.attrib);
+	}
+
+	prframe->u.hdr.len = 0;
+	rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */
+
+	return  _SUCCESS;
+}
+
+static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
+{
+	struct adapter *padapter = preorder_ctrl->padapter;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u8 wsize = preorder_ctrl->wsize_b;
+	u16 wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF;/*  4096; */
+
+	/*  Rx Reorder initialize condition. */
+	if (preorder_ctrl->indicate_seq == 0xFFFF)
+		preorder_ctrl->indicate_seq = seq_num;
+
+	/*  Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(seq_num, preorder_ctrl->indicate_seq))
+		return false;
+
+	/*  */
+	/*  Sliding window manipulation. Conditions includes: */
+	/*  1. Incoming SeqNum is equal to WinStart =>Window shift 1 */
+	/*  2. Incoming SeqNum is larger than the WinEnd => Window shift N */
+	/*  */
+	if (SN_EQUAL(seq_num, preorder_ctrl->indicate_seq)) {
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+	} else if (SN_LESS(wend, seq_num)) {
+		/*  boundary situation, when seq_num cross 0xFFF */
+		if (seq_num >= (wsize - 1))
+			preorder_ctrl->indicate_seq = seq_num + 1 - wsize;
+		else
+			preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1;
+		pdbgpriv->dbg_rx_ampdu_window_shift_cnt++;
+	}
+
+	return true;
+}
+
+static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct list_head	*phead, *plist;
+	union recv_frame *pnextrframe;
+	struct rx_pkt_attrib *pnextattrib;
+
+	/* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+
+	phead = get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		pnextrframe = (union recv_frame *)plist;
+		pnextattrib = &pnextrframe->u.hdr.attrib;
+
+		if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+			plist = get_next(plist);
+		else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+			/* Duplicate entry is found!! Do not insert current entry. */
+			/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+			return false;
+		else
+			break;
+
+	}
+
+
+	/* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+	list_del_init(&(prframe->u.hdr.list));
+
+	list_add_tail(&(prframe->u.hdr.list), plist);
+
+	/* spin_unlock(&ppending_recvframe_queue->lock); */
+	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+
+	return true;
+
+}
+
+static void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq)
+{
+	if (current_seq < prev_seq)
+		pdbgpriv->dbg_rx_ampdu_loss_count += (4096 + current_seq - prev_seq);
+	else
+		pdbgpriv->dbg_rx_ampdu_loss_count += (current_seq - prev_seq);
+
+}
+
+static int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+	struct list_head	*phead, *plist;
+	union recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	/* u8 index = 0; */
+	int bPktInBuf = false;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	/* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+	phead =		get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	/*  Handling some condition for forced indicate case. */
+	if (bforced == true) {
+		pdbgpriv->dbg_rx_ampdu_forced_indicate_count++;
+		if (list_empty(phead)) {
+			/*  spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+			/* spin_unlock(&ppending_recvframe_queue->lock); */
+			return true;
+		}
+
+		prframe = (union recv_frame *)plist;
+		pattrib = &prframe->u.hdr.attrib;
+
+		recv_indicatepkts_pkt_loss_cnt(pdbgpriv, preorder_ctrl->indicate_seq, pattrib->seq_num);
+		preorder_ctrl->indicate_seq = pattrib->seq_num;
+
+	}
+
+	/*  Prepare indication list and indication. */
+	/*  Check if there is any packet need indicate. */
+	while (!list_empty(phead)) {
+
+		prframe = (union recv_frame *)plist;
+		pattrib = &prframe->u.hdr.attrib;
+
+		if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+			plist = get_next(plist);
+			list_del_init(&(prframe->u.hdr.list));
+
+			if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num))
+				preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+			/* Set this as a lock to make sure that only one thread is indicating packet. */
+			/* pTS->RxIndicateState = RXTS_INDICATE_PROCESSING; */
+
+			/*  Indicate packets */
+
+			/* indicate this recv_frame */
+			if (!pattrib->amsdu) {
+				if ((padapter->bDriverStopped == false) &&
+				    (padapter->bSurpriseRemoved == false))
+					rtw_recv_indicatepkt(padapter, prframe);/* indicate this recv_frame */
+
+			} else if (pattrib->amsdu == 1) {
+				if (amsdu_to_msdu(padapter, prframe) != _SUCCESS)
+					rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+
+			} else {
+				/* error condition; */
+			}
+
+
+			/* Update local variables. */
+			bPktInBuf = false;
+
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+
+	}
+
+	/* spin_unlock(&ppending_recvframe_queue->lock); */
+	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+
+	return bPktInBuf;
+}
+
+static int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (!pattrib->amsdu) {
+		/* s1. */
+		wlanhdr_to_ethhdr(prframe);
+
+		if (pattrib->qos != 1) {
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+
+			}
+
+			return _FAIL;
+
+		}
+
+		if (preorder_ctrl->enable == false) {
+			/* indicate this recv_frame */
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+
+			rtw_recv_indicatepkt(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+
+			return _SUCCESS;
+		}
+	} else if (pattrib->amsdu == 1) { /* temp filter -> means didn't support A-MSDUs in a A-MPDU */
+		if (preorder_ctrl->enable == false) {
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+
+			retval = amsdu_to_msdu(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+
+			if (retval != _SUCCESS) {
+			}
+
+			return retval;
+		}
+	}
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	/* s2. check if winstart_b(indicate_seq) needs to been updated */
+	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num)) {
+		pdbgpriv->dbg_rx_ampdu_drop_count++;
+		goto _err_exit;
+	}
+
+
+	/* s3. Insert all packet into Reorder Queue to maintain its ordering. */
+	if (!enqueue_reorder_recvframe(preorder_ctrl, prframe)) {
+		/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+		/* return _FAIL; */
+		goto _err_exit;
+	}
+
+
+	/* s4. */
+	/*  Indication process. */
+	/*  After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */
+	/*  with the SeqNum smaller than latest WinStart and buffer other packets. */
+	/*  */
+	/*  For Rx Reorder condition: */
+	/*  1. All packets with SeqNum smaller than WinStart => Indicate */
+	/*  2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */
+	/*  */
+
+	/* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false) == true) {
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+	} else {
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+	}
+
+	return _SUCCESS;
+
+_err_exit:
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+	return _FAIL;
+}
+
+
+void rtw_reordering_ctrl_timeout_handler(struct timer_list *t)
+{
+	struct recv_reorder_ctrl *preorder_ctrl =
+		from_timer(preorder_ctrl, t, reordering_ctrl_timer);
+	struct adapter *padapter = preorder_ctrl->padapter;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+		return;
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true)
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+}
+
+static int process_recv_indicatepkts(struct adapter *padapter, union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	/* struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; */
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+	if (phtpriv->ht_option == true) { /* B/G/N Mode */
+		/* prframe->u.hdr.preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */
+
+		if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) { /*  including perform A-MPDU Rx Ordering Buffer Control */
+
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	} else { /* B/G mode */
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS)
+			return retval;
+
+		if ((padapter->bDriverStopped == false) && (padapter->bSurpriseRemoved == false)) {
+			/* indicate this recv_frame */
+			rtw_recv_indicatepkt(padapter, prframe);
+		} else {
+			retval = _FAIL;
+			return retval;
+		}
+
+	}
+
+	return retval;
+
+}
+
+static int recv_func_prehandle(struct adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/* check the frame crtl field and decache */
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS) {
+		rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+static int recv_func_posthandle(struct adapter *padapter, union recv_frame *prframe)
+{
+	int ret = _SUCCESS;
+	union recv_frame *orig_prframe = prframe;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	prframe = decryptor(padapter, prframe);
+	if (!prframe) {
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if (!prframe)
+		goto _recv_data_drop;
+
+	prframe = portctrl(padapter, prframe);
+	if (!prframe) {
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+	count_rx_stats(padapter, prframe, NULL);
+
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS) {
+		rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		goto _recv_data_drop;
+	}
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+static int recv_func(struct adapter *padapter, union recv_frame *rframe)
+{
+	int ret;
+	struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *recvpriv = &padapter->recvpriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+
+	/* check if need to handle uc_swdec_pending_queue*/
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey) {
+		union recv_frame *pending_frame;
+		int cnt = 0;
+
+		while ((pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue))) {
+			cnt++;
+			recv_func_posthandle(padapter, pending_frame);
+		}
+	}
+
+	ret = recv_func_prehandle(padapter, rframe);
+
+	if (ret == _SUCCESS) {
+
+		/* check if need to enqueue into uc_swdec_pending_queue*/
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+			!IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 &&
+			(prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt == true) &&
+			psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK &&
+			!psecuritypriv->busetkipkey) {
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+
+			if (recvpriv->free_recvframe_cnt < NR_RECVFRAME/4) {
+				/* to prevent from recvframe starvation, get recvframe from uc_swdec_pending_queue to free_recvframe_cnt  */
+				rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue);
+				if (rframe)
+					goto do_posthandle;
+			}
+			goto exit;
+		}
+
+do_posthandle:
+		ret = recv_func_posthandle(padapter, rframe);
+	}
+
+exit:
+	return ret;
+}
+
+
+s32 rtw_recv_entry(union recv_frame *precvframe)
+{
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+	s32 ret = _SUCCESS;
+
+	padapter = precvframe->u.hdr.adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL)
+		goto _recv_entry_drop;
+
+	precvpriv->rx_pkts++;
+
+	return ret;
+
+_recv_entry_drop:
+
+	return ret;
+}
+
+static void rtw_signal_stat_timer_hdl(struct timer_list *t)
+{
+	struct adapter *adapter =
+		from_timer(adapter, t, recvpriv.signal_stat_timer);
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+
+	u32 tmp_s, tmp_q;
+	u8 avg_signal_strength = 0;
+	u8 avg_signal_qual = 0;
+	u32 num_signal_strength = 0;
+	u32 __maybe_unused num_signal_qual = 0;
+	u8 _alpha = 5; /*  this value is based on converging_constant = 5000 and sampling_interval = 1000 */
+
+	if (adapter->recvpriv.is_signal_dbg) {
+		/* update the user specific value, signal_strength_dbg, to signal_strength, rssi */
+		adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg;
+		adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+	} else {
+
+		if (recvpriv->signal_strength_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+			num_signal_strength = recvpriv->signal_strength_data.total_num;
+			/*  after avg_vals are acquired, we can re-stat the signal values */
+			recvpriv->signal_strength_data.update_req = 1;
+		}
+
+		if (recvpriv->signal_qual_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+			num_signal_qual = recvpriv->signal_qual_data.total_num;
+			/*  after avg_vals are acquired, we can re-stat the signal values */
+			recvpriv->signal_qual_data.update_req = 1;
+		}
+
+		if (num_signal_strength == 0) {
+			if (rtw_get_on_cur_ch_time(adapter) == 0 ||
+			    jiffies_to_msecs(jiffies - rtw_get_on_cur_ch_time(adapter)) < 2 * adapter->mlmeextpriv.mlmext_info.bcn_interval
+			) {
+				goto set_timer;
+			}
+		}
+
+		if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == true ||
+		    check_fwstate(&adapter->mlmepriv, _FW_LINKED) == false
+		) {
+			goto set_timer;
+		}
+
+		/* update value of signal_strength, rssi, signal_qual */
+		tmp_s = (avg_signal_strength+(_alpha-1)*recvpriv->signal_strength);
+		if (tmp_s % _alpha)
+			tmp_s = tmp_s/_alpha + 1;
+		else
+			tmp_s = tmp_s/_alpha;
+		if (tmp_s > 100)
+			tmp_s = 100;
+
+		tmp_q = (avg_signal_qual+(_alpha-1)*recvpriv->signal_qual);
+		if (tmp_q % _alpha)
+			tmp_q = tmp_q/_alpha + 1;
+		else
+			tmp_q = tmp_q/_alpha;
+		if (tmp_q > 100)
+			tmp_q = 100;
+
+		recvpriv->signal_strength = tmp_s;
+		recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+		recvpriv->signal_qual = tmp_q;
+	}
+
+set_timer:
+	rtw_set_signal_stat_timer(recvpriv);
+
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_rf.c b/drivers/staging/rtl8723bs/core/rtw_rf.c
new file mode 100644
index 000000000..4f120c894
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_rf.c
@@ -0,0 +1,34 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <linux/kernel.h>
+
+static const u32 ch_freq_map[] = {
+	2412,
+	2417,
+	2422,
+	2427,
+	2432,
+	2437,
+	2442,
+	2447,
+	2452,
+	2457,
+	2462,
+	2467,
+	2472,
+	2484
+};
+
+u32 rtw_ch2freq(u32 channel)
+{
+	if (channel == 0 || channel > ARRAY_SIZE(ch_freq_map))
+		return 2412;
+
+	return ch_freq_map[channel - 1];
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_security.c b/drivers/staging/rtl8723bs/core/rtw_security.c
new file mode 100644
index 000000000..ac731415f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_security.c
@@ -0,0 +1,1594 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <linux/crc32.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <crypto/aes.h>
+
+static const char * const _security_type_str[] = {
+	"N/A",
+	"WEP40",
+	"TKIP",
+	"TKIP_WM",
+	"AES",
+	"WEP104",
+	"SMS4",
+	"WEP_WPA",
+	"BIP",
+};
+
+const char *security_type_str(u8 value)
+{
+	if (value <= _BIP_)
+		return _security_type_str[value];
+	return NULL;
+}
+
+/* WEP related ===== */
+
+/*
+	Need to consider the fragment  situation
+*/
+void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{																	/*  exclude ICV */
+	union {
+		__le32 f0;
+		unsigned char f1[4];
+	} crc;
+
+	signed int	curfragnum, length;
+	u32 keylength;
+
+	u8 *pframe, *payload, *iv;    /* wepkey */
+	u8 wepkey[16];
+	u8 hw_hdr_offset = 0;
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct arc4_ctx *ctx = &psecuritypriv->xmit_arc4_ctx;
+
+	if (!((struct xmit_frame *)pxmitframe)->buf_addr)
+		return;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* start to encrypt each fragment */
+	if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
+		keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
+
+		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+			iv = pframe+pattrib->hdrlen;
+			memcpy(&wepkey[0], iv, 3);
+			memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
+			payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+
+			if ((curfragnum+1) == pattrib->nr_frags) {	/* the last fragment */
+
+				length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+
+				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+				arc4_setkey(ctx, wepkey, 3 + keylength);
+				arc4_crypt(ctx, payload, payload, length);
+				arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+			} else {
+				length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+				arc4_setkey(ctx, wepkey, 3 + keylength);
+				arc4_crypt(ctx, payload, payload, length);
+				arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+				pframe += pxmitpriv->frag_len;
+				pframe = (u8 *)round_up((SIZE_PTR)(pframe), 4);
+			}
+		}
+	}
+}
+
+void rtw_wep_decrypt(struct adapter  *padapter, u8 *precvframe)
+{
+	/*  exclude ICV */
+	u8 crc[4];
+	signed int	length;
+	u32 keylength;
+	u8 *pframe, *payload, *iv, wepkey[16];
+	u8  keyindex;
+	struct	rx_pkt_attrib	 *prxattrib = &(((union recv_frame *)precvframe)->u.hdr.attrib);
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct arc4_ctx *ctx = &psecuritypriv->recv_arc4_ctx;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+
+	/* start to decrypt recvframe */
+	if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
+		iv = pframe+prxattrib->hdrlen;
+		/* keyindex =(iv[3]&0x3); */
+		keyindex = prxattrib->key_index;
+		keylength = psecuritypriv->dot11DefKeylen[keyindex];
+		memcpy(&wepkey[0], iv, 3);
+		/* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */
+		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
+		length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+		payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+
+		/* decrypt payload include icv */
+		arc4_setkey(ctx, wepkey, 3 + keylength);
+		arc4_crypt(ctx, payload, payload,  length);
+
+		/* calculate icv and compare the icv */
+		*((u32 *)crc) = ~crc32_le(~0, payload, length - 4);
+
+	}
+}
+
+/* 3		=====TKIP related ===== */
+
+static u32 secmicgetuint32(u8 *p)
+/*  Convert from Byte[] to Us3232 in a portable way */
+{
+	s32 i;
+	u32 res = 0;
+
+	for (i = 0; i < 4; i++)
+		res |= ((u32)(*p++)) << (8 * i);
+
+	return res;
+}
+
+static void secmicputuint32(u8 *p, u32 val)
+/*  Convert from Us3232 to Byte[] in a portable way */
+{
+	long i;
+
+	for (i = 0; i < 4; i++) {
+		*p++ = (u8) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+/*  Reset the state to the empty message. */
+	pmicdata->L = pmicdata->K0;
+	pmicdata->R = pmicdata->K1;
+	pmicdata->nBytesInM = 0;
+	pmicdata->M = 0;
+}
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
+{
+	/*  Set the key */
+	pmicdata->K0 = secmicgetuint32(key);
+	pmicdata->K1 = secmicgetuint32(key + 4);
+	/*  and reset the message */
+	secmicclear(pmicdata);
+}
+
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
+{
+	/*  Append the byte to our word-sized buffer */
+	pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
+	pmicdata->nBytesInM++;
+	/*  Process the word if it is full. */
+	if (pmicdata->nBytesInM >= 4) {
+		pmicdata->L ^= pmicdata->M;
+		pmicdata->R ^= ROL32(pmicdata->L, 17);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROL32(pmicdata->L, 3);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROR32(pmicdata->L, 2);
+		pmicdata->L += pmicdata->R;
+		/*  Clear the buffer */
+		pmicdata->M = 0;
+		pmicdata->nBytesInM = 0;
+	}
+}
+
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
+{
+	/*  This is simple */
+	while (nbytes > 0) {
+		rtw_secmicappendbyte(pmicdata, *src++);
+		nbytes--;
+	}
+}
+
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst)
+{
+	/*  Append the minimum padding */
+	rtw_secmicappendbyte(pmicdata, 0x5a);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	/*  and then zeroes until the length is a multiple of 4 */
+	while (pmicdata->nBytesInM != 0)
+		rtw_secmicappendbyte(pmicdata, 0);
+	/*  The appendByte function has already computed the result. */
+	secmicputuint32(dst, pmicdata->L);
+	secmicputuint32(dst + 4, pmicdata->R);
+	/*  Reset to the empty message. */
+	secmicclear(pmicdata);
+}
+
+
+void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
+{
+
+	struct mic_data	micdata;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+
+	rtw_secmicsetkey(&micdata, key);
+	priority[0] = pri;
+
+	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+	if (header[1] & 1) {   /* ToDS == 1 */
+		rtw_secmicappend(&micdata, &header[16], 6);  /* DA */
+		if (header[1] & 2)  /* From Ds == 1 */
+			rtw_secmicappend(&micdata, &header[24], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+	} else {	/* ToDS == 0 */
+		rtw_secmicappend(&micdata, &header[4], 6);   /* DA */
+		if (header[1] & 2)  /* From Ds == 1 */
+			rtw_secmicappend(&micdata, &header[16], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+	}
+	rtw_secmicappend(&micdata, &priority[0], 4);
+
+
+	rtw_secmicappend(&micdata, data, data_len);
+
+	rtw_secgetmic(&micdata, mic_code);
+}
+
+/* macros for extraction/creation of unsigned char/unsigned short values  */
+#define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
+#define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
+#define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
+#define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
+#define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256] = {      /* Sbox for hash (can be in ROM)     */
+{
+	 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+	 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+	 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+	 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+	 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+	 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+	 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+	 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+	 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+	 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+	 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+	 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+	 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+	 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+	 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+	 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+	 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+	 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+	 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+	 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+	 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+	 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+	 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+	 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+	 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+	 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+	 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+	 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+	 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+	 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+	 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+	 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+	},
+
+
+	{  /* second half of table is unsigned char-reversed version of first! */
+	 0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+	 0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+	 0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+	 0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+	 0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+	 0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+	 0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+	 0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+	 0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+	 0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+	 0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+	 0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+	 0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+	 0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+	 0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+	 0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+	 0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+	 0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+	 0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+	 0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+	 0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+	 0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+	 0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+	 0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+	 0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+	 0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+	 0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+	 0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+	 0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+	 0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+	 0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+	 0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+	}
+};
+
+ /*
+**********************************************************************
+* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+*
+* Inputs:
+*     tk[]      = temporal key                         [128 bits]
+*     ta[]      = transmitter's MAC address            [ 48 bits]
+*     iv32      = upper 32 bits of IV                  [ 32 bits]
+* Output:
+*     p1k[]     = Phase 1 key                          [ 80 bits]
+*
+* Note:
+*     This function only needs to be called every 2**16 packets,
+*     although in theory it could be called every packet.
+*
+**********************************************************************
+*/
+static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
+{
+	signed int  i;
+
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
+	p1k[0]      = Lo16(iv32);
+	p1k[1]      = Hi16(iv32);
+	p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
+	p1k[3]      = Mk16(ta[3], ta[2]);
+	p1k[4]      = Mk16(ta[5], ta[4]);
+
+	/* Now compute an unbalanced Feistel cipher with 80-bit block */
+	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+	for (i = 0; i < PHASE1_LOOP_CNT; i++) {
+		/* Each add operation here is mod 2**16 */
+		p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
+		p1k[4] +=  (unsigned short)i;          /* avoid "slide attacks" */
+	}
+}
+
+
+/*
+**********************************************************************
+* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+*
+* Inputs:
+*     tk[]      = Temporal key                         [128 bits]
+*     p1k[]     = Phase 1 output key                   [ 80 bits]
+*     iv16      = low 16 bits of IV counter            [ 16 bits]
+* Output:
+*     rc4key[]  = the key used to encrypt the packet   [128 bits]
+*
+* Note:
+*     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
+*     across all packets using the same key TK value. Then, for a
+*     given value of TK[], this TKIP48 construction guarantees that
+*     the final RC4KEY value is unique across all packets.
+*
+* Suggested implementation optimization: if PPK[] is "overlaid"
+*     appropriately on RC4KEY[], there is no need for the final
+*     for loop below that copies the PPK[] result into RC4KEY[].
+*
+**********************************************************************
+*/
+static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
+{
+	signed int  i;
+	u16 PPK[6];                          /* temporary key for mixing    */
+
+	/* Note: all adds in the PPK[] equations below are mod 2**16         */
+	for (i = 0; i < 5; i++)
+		PPK[i] = p1k[i];      /* first, copy P1K to PPK      */
+
+	PPK[5]  =  p1k[4]+iv16;             /* next,  add in IV16          */
+
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
+	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
+	PPK[1] +=    _S_(PPK[0] ^ TK16(1));
+	PPK[2] +=    _S_(PPK[1] ^ TK16(2));
+	PPK[3] +=    _S_(PPK[2] ^ TK16(3));
+	PPK[4] +=    _S_(PPK[3] ^ TK16(4));
+	PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
+
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
+	PPK[2] +=  RotR1(PPK[1]);
+	PPK[3] +=  RotR1(PPK[2]);
+	PPK[4] +=  RotR1(PPK[3]);
+	PPK[5] +=  RotR1(PPK[4]);
+	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
+	/*       value PPK[0..5] is guaranteed to be unique, as a function   */
+	/*       of the 96-bit "input" value   {TA, IV32, IV16}. That is, P1K  */
+	/*       is now a keyed permutation of {TA, IV32, IV16}.               */
+
+	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
+	rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV  */
+	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
+	rc4key[2] = Lo8(iv16);
+	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+
+
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
+	for (i = 0; i < 6; i++) {
+		rc4key[4+2*i] = Lo8(PPK[i]);
+		rc4key[5+2*i] = Hi8(PPK[i]);
+	}
+}
+
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{																	/*  exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8 rc4key[16];
+	u8   ttkey[16];
+	union {
+		__le32 f0;
+		u8 f1[4];
+	} crc;
+	u8   hw_hdr_offset = 0;
+	signed int			curfragnum, length;
+
+	u8 *pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct arc4_ctx *ctx = &psecuritypriv->xmit_arc4_ctx;
+	u32 res = _SUCCESS;
+
+	if (!((struct xmit_frame *)pxmitframe)->buf_addr)
+		return _FAIL;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _TKIP_) {
+
+		{
+			if (IS_MCAST(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else
+				prwskey = pattrib->dot118021x_UncstKey.skey;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				iv = pframe+pattrib->hdrlen;
+				payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+
+				GET_TKIP_PN(iv, dot11txpn);
+
+				pnl = (u16)(dot11txpn.val);
+				pnh = (u32)(dot11txpn.val>>16);
+
+				phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
+
+				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
+
+				if ((curfragnum+1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+					arc4_setkey(ctx, rc4key, 16);
+					arc4_crypt(ctx, payload, payload, length);
+					arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+				} else {
+					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
+
+					arc4_setkey(ctx, rc4key, 16);
+					arc4_crypt(ctx, payload, payload, length);
+					arc4_crypt(ctx, payload + length, crc.f1, 4);
+
+					pframe += pxmitpriv->frag_len;
+					pframe = (u8 *)round_up((SIZE_PTR)(pframe), 4);
+				}
+			}
+		}
+	}
+	return res;
+}
+
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe)
+{																	/*  exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8   rc4key[16];
+	u8   ttkey[16];
+	u8 crc[4];
+	signed int			length;
+
+	u8 *pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	struct sta_info *stainfo;
+	struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct arc4_ctx *ctx = &psecuritypriv->recv_arc4_ctx;
+	u32 res = _SUCCESS;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+
+	/* 4 start to decrypt recvframe */
+	if (prxattrib->encrypt == _TKIP_) {
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+		if (stainfo) {
+			if (IS_MCAST(prxattrib->ra)) {
+				static unsigned long start;
+				static u32 no_gkey_bc_cnt;
+				static u32 no_gkey_mc_cnt;
+
+				if (!psecuritypriv->binstallGrpkey) {
+					res = _FAIL;
+
+					if (start == 0)
+						start = jiffies;
+
+					if (is_broadcast_mac_addr(prxattrib->ra))
+						no_gkey_bc_cnt++;
+					else
+						no_gkey_mc_cnt++;
+
+					if (jiffies_to_msecs(jiffies - start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							netdev_dbg(padapter->pnetdev,
+								   FUNC_ADPT_FMT " no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+								   FUNC_ADPT_ARG(padapter),
+								   no_gkey_bc_cnt,
+								   no_gkey_mc_cnt);
+						}
+						start = jiffies;
+						no_gkey_bc_cnt = 0;
+						no_gkey_mc_cnt = 0;
+					}
+					goto exit;
+				}
+
+				if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+					netdev_dbg(padapter->pnetdev,
+						   FUNC_ADPT_FMT " gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+						   FUNC_ADPT_ARG(padapter),
+						   no_gkey_bc_cnt,
+						   no_gkey_mc_cnt);
+				}
+				start = 0;
+				no_gkey_bc_cnt = 0;
+				no_gkey_mc_cnt = 0;
+
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+			} else {
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+			}
+
+			iv = pframe+prxattrib->hdrlen;
+			payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+			GET_TKIP_PN(iv, dot11txpn);
+
+			pnl = (u16)(dot11txpn.val);
+			pnh = (u32)(dot11txpn.val>>16);
+
+			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+			/* 4 decrypt payload include icv */
+
+			arc4_setkey(ctx, rc4key, 16);
+			arc4_crypt(ctx, payload, payload, length);
+
+			*((u32 *)crc) = ~crc32_le(~0, payload, length - 4);
+
+			if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] ||
+			    crc[1] != payload[length - 3] || crc[0] != payload[length - 4])
+				res = _FAIL;
+		} else {
+			res = _FAIL;
+		}
+	}
+exit:
+	return res;
+}
+
+
+/* 3			=====AES related ===== */
+
+
+
+#define MAX_MSG_SIZE	2048
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
+static void construct_mic_iv(u8 *mic_header1,
+			     signed int qc_exists,
+			     signed int a4_exists,
+			     u8 *mpdu,
+			     uint payload_length,
+			     u8 *pn_vector,
+			     uint frtype); /*  add for CONFIG_IEEE80211W, none 11w also can use */
+static void construct_mic_header1(u8 *mic_header1,
+				  signed int header_length,
+				  u8 *mpdu,
+				  uint frtype); /* for CONFIG_IEEE80211W, none 11w also can use */
+static void construct_mic_header2(u8 *mic_header2,
+				  u8 *mpdu,
+				  signed int a4_exists,
+				  signed int qc_exists);
+static void construct_ctr_preload(u8 *ctr_preload,
+				  signed int a4_exists,
+				  signed int qc_exists,
+				  u8 *mpdu,
+				  u8 *pn_vector,
+				  signed int c,
+				  uint frtype); /* for CONFIG_IEEE80211W, none 11w also can use */
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
+
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+	struct crypto_aes_ctx ctx;
+
+	aes_expandkey(&ctx, key, 16);
+	aes_encrypt(&ctx, ciphertext, data);
+	memzero_explicit(&ctx, sizeof(ctx));
+}
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/* Baron think the function is construct CCM    */
+/* nonce                                        */
+/************************************************/
+static void construct_mic_iv(u8 *mic_iv,
+			     signed int qc_exists,
+			     signed int a4_exists,
+			     u8 *mpdu,
+			     uint payload_length,
+			     u8 *pn_vector,
+			     uint frtype) /* add for CONFIG_IEEE80211W, none 11w also can use */
+{
+		signed int i;
+
+		mic_iv[0] = 0x59;
+
+		if (qc_exists && a4_exists)
+			mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+
+		if (qc_exists && !a4_exists)
+			mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+
+		if (!qc_exists)
+			mic_iv[1] = 0x00;
+
+		/* 802.11w management frame should set management bit(4) */
+		if (frtype == WIFI_MGT_TYPE)
+			mic_iv[1] |= BIT(4);
+
+		for (i = 2; i < 8; i++)
+			mic_iv[i] = mpdu[i + 8];   /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+		#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+		#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+		#endif
+		mic_iv[14] = (unsigned char) (payload_length / 256);
+		mic_iv[15] = (unsigned char) (payload_length % 256);
+}
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/* Build AAD SC, A1, A2                           */
+/************************************************/
+static void construct_mic_header1(u8 *mic_header1,
+				  signed int header_length,
+				  u8 *mpdu,
+				  uint frtype) /* for CONFIG_IEEE80211W, none 11w also can use */
+{
+		mic_header1[0] = (u8)((header_length - 2) / 256);
+		mic_header1[1] = (u8)((header_length - 2) % 256);
+
+		/* 802.11w management frame don't AND subtype bits 4, 5, 6 of frame control field */
+		if (frtype == WIFI_MGT_TYPE)
+			mic_header1[2] = mpdu[0];
+		else
+			mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+
+		mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+		mic_header1[4] = mpdu[4];       /* A1 */
+		mic_header1[5] = mpdu[5];
+		mic_header1[6] = mpdu[6];
+		mic_header1[7] = mpdu[7];
+		mic_header1[8] = mpdu[8];
+		mic_header1[9] = mpdu[9];
+		mic_header1[10] = mpdu[10];     /* A2 */
+		mic_header1[11] = mpdu[11];
+		mic_header1[12] = mpdu[12];
+		mic_header1[13] = mpdu[13];
+		mic_header1[14] = mpdu[14];
+		mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+static void construct_mic_header2(u8 *mic_header2,
+				  u8 *mpdu,
+				  signed int a4_exists,
+				  signed int qc_exists)
+{
+		signed int i;
+
+		for (i = 0; i < 16; i++)
+			mic_header2[i] = 0x00;
+
+		mic_header2[0] = mpdu[16];    /* A3 */
+		mic_header2[1] = mpdu[17];
+		mic_header2[2] = mpdu[18];
+		mic_header2[3] = mpdu[19];
+		mic_header2[4] = mpdu[20];
+		mic_header2[5] = mpdu[21];
+
+		mic_header2[6] = 0x00;
+		mic_header2[7] = 0x00; /* mpdu[23]; */
+
+		if (!qc_exists && a4_exists) {
+			for (i = 0; i < 6; i++)
+				mic_header2[8+i] = mpdu[24+i];   /* A4 */
+		}
+
+		if (qc_exists && !a4_exists) {
+			mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+			mic_header2[9] = mpdu[25] & 0x00;
+		}
+
+		if (qc_exists && a4_exists) {
+			for (i = 0; i < 6; i++)
+				mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+			mic_header2[14] = mpdu[30] & 0x0f;
+			mic_header2[15] = mpdu[31] & 0x00;
+		}
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/* Baron think the function is construct CCM    */
+/* nonce                                        */
+/************************************************/
+static void construct_ctr_preload(u8 *ctr_preload,
+				  signed int a4_exists,
+				  signed int qc_exists,
+				  u8 *mpdu,
+				  u8 *pn_vector,
+				  signed int c,
+				  uint frtype) /* for CONFIG_IEEE80211W, none 11w also can use */
+{
+	signed int i = 0;
+
+	for (i = 0; i < 16; i++)
+		ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists)
+		ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
+	if (qc_exists && !a4_exists)
+		ctr_preload[1] = mpdu[24] & 0x0f;
+
+	/* 802.11w management frame should set management bit(4) */
+	if (frtype == WIFI_MGT_TYPE)
+		ctr_preload[1] |= BIT(4);
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
+	ctr_preload[15] =  (unsigned char) (c % 256);
+}
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+		signed int i;
+
+		for (i = 0; i < 16; i++)
+			out[i] = ina[i] ^ inb[i];
+}
+
+static signed int aes_cipher(u8 *key, uint	hdrlen,
+			u8 *pframe, uint plen)
+{
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+		num_blocks, payload_index;
+
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype = frsubtype>>4;
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (((frtype|frsubtype) == WIFI_DATA_CFACK) ||
+	    ((frtype|frsubtype) == WIFI_DATA_CFPOLL) ||
+	    ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+
+	} else if ((frtype == WIFI_DATA) && /*  add for CONFIG_IEEE80211W, none 11w also can use */
+		   ((frsubtype == 0x08) ||
+		   (frsubtype == 0x09) ||
+		   (frsubtype == 0x0a) ||
+		   (frsubtype == 0x0b))) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen+1];
+	pn_vector[2] = pframe[hdrlen+4];
+	pn_vector[3] = pframe[hdrlen+5];
+	pn_vector[4] = pframe[hdrlen+6];
+	pn_vector[5] = pframe[hdrlen+7];
+
+	construct_mic_iv(mic_iv,
+			 qc_exists,
+			 a4_exists,
+			 pframe,	 /* message, */
+			 plen,
+			 pn_vector,
+			 frtype); /*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+	construct_mic_header1(mic_header1,
+			      hdrlen,
+			      pframe,	/* message */
+			      frtype); /*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+	construct_mic_header2(mic_header2,
+			      pframe,	/* message, */
+			      a4_exists,
+			      qc_exists);
+
+	payload_remainder = plen % 16;
+	num_blocks = plen / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index++];
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	for (j = 0 ; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		pframe[payload_index+j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, /* message, */
+				      pn_vector, i+1, frtype);
+		/*  add for CONFIG_IEEE80211W, none 11w also can use */
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {
+		/* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, /* message, */
+				      pn_vector, num_blocks+1, frtype);
+		/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index+j];
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, /* message, */
+			      pn_vector, 0, frtype);
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = pframe[j+hdrlen+8+plen];
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		pframe[payload_index++] = chain_buffer[j];
+
+	return _SUCCESS;
+}
+
+u32 rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{	/*  exclude ICV */
+
+	/*static*/
+	/* unsigned char message[MAX_MSG_SIZE]; */
+
+	/* Intermediate Buffers */
+	signed int curfragnum, length;
+	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	u8 hw_hdr_offset = 0;
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	u32 res = _SUCCESS;
+
+	if (!((struct xmit_frame *)pxmitframe)->buf_addr)
+		return _FAIL;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _AES_) {
+		if (IS_MCAST(pattrib->ra))
+			prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+		else
+			prwskey = pattrib->dot118021x_UncstKey.skey;
+
+		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+			if ((curfragnum+1) == pattrib->nr_frags) {	/* 4 the last fragment */
+				length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+
+				aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+			} else {
+				length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+
+				aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+				pframe += pxmitpriv->frag_len;
+				pframe = (u8 *)round_up((SIZE_PTR)(pframe), 4);
+			}
+		}
+	}
+	return res;
+}
+
+static signed int aes_decipher(u8 *key, uint	hdrlen,
+			 u8 *pframe, uint plen)
+{
+	static u8 message[MAX_MSG_SIZE];
+	uint qc_exists, a4_exists, i, j, payload_remainder,
+			num_blocks, payload_index;
+	signed int res = _SUCCESS;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+		/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+
+	uint frtype  = GetFrameType(pframe);
+	uint frsubtype  = GetFrameSubType(pframe);
+
+	frsubtype = frsubtype>>4;
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	/* start to decrypt the payload */
+
+	num_blocks = (plen-8) / 16; /* plen including LLC, payload_length and mic) */
+
+	payload_remainder = (plen-8) % 16;
+
+	pn_vector[0]  = pframe[hdrlen];
+	pn_vector[1]  = pframe[hdrlen + 1];
+	pn_vector[2]  = pframe[hdrlen + 4];
+	pn_vector[3]  = pframe[hdrlen + 5];
+	pn_vector[4]  = pframe[hdrlen + 6];
+	pn_vector[5]  = pframe[hdrlen + 7];
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (((frtype|frsubtype) == WIFI_DATA_CFACK) ||
+	    ((frtype|frsubtype) == WIFI_DATA_CFPOLL) ||
+	    ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+
+	} else if ((frtype == WIFI_DATA) && /* only for data packet . add for CONFIG_IEEE80211W, none 11w also can use */
+		   ((frsubtype == 0x08) ||
+		   (frsubtype == 0x09) ||
+		   (frsubtype == 0x0a) ||
+		   (frsubtype == 0x0b))) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+			hdrlen += 2;
+
+		qc_exists = 1;
+	} else {
+		qc_exists = 0;
+	}
+
+	/*  now, decrypt pframe with hdrlen offset and plen long */
+
+	payload_index = hdrlen + 8; /*  8 is for extiv */
+
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists,
+				      qc_exists, pframe,
+				      pn_vector, i + 1,
+				      frtype); /*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {
+		/* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector,
+				      num_blocks+1, frtype);
+		/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index+j];
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	/* start to calculate the mic */
+	if ((hdrlen + plen+8) <= MAX_MSG_SIZE)
+		memcpy((void *)message, pframe, (hdrlen + plen+8)); /* 8 is for ext iv len */
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen+1];
+	pn_vector[2] = pframe[hdrlen+4];
+	pn_vector[3] = pframe[hdrlen+5];
+	pn_vector[4] = pframe[hdrlen+6];
+	pn_vector[5] = pframe[hdrlen+7];
+
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen-8, pn_vector, frtype);
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+	construct_mic_header1(mic_header1, hdrlen, message, frtype);
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+	construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
+
+	payload_remainder = (plen-8) % 16;
+	num_blocks = (plen-8) / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index++];
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	for (j = 0; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		message[payload_index+j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i+1,
+				      frtype);
+		/*  add for CONFIG_IEEE80211W, none 11w also can use */
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {
+		/* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back   */
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector,
+				      num_blocks+1, frtype);
+		/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index+j];
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	/* Encrypt the MIC */
+	construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, 0, frtype);
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = message[j+hdrlen+8+plen-8];
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		message[payload_index++] = chain_buffer[j];
+
+	/* compare the mic */
+	for (i = 0; i < 8; i++) {
+		if (pframe[hdrlen + 8 + plen - 8 + i] != message[hdrlen + 8 + plen - 8 + i])
+			res = _FAIL;
+	}
+	return res;
+}
+
+u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
+{	/*  exclude ICV */
+
+	/*static*/
+	/* unsigned char message[MAX_MSG_SIZE]; */
+
+	/* Intermediate Buffers */
+
+	signed int length;
+	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	struct sta_info *stainfo;
+	struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u32 res = _SUCCESS;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+	/* 4 start to encrypt each fragment */
+	if (prxattrib->encrypt == _AES_) {
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+		if (stainfo) {
+			if (IS_MCAST(prxattrib->ra)) {
+				static unsigned long start;
+				static u32 no_gkey_bc_cnt;
+				static u32 no_gkey_mc_cnt;
+
+				if (!psecuritypriv->binstallGrpkey) {
+					res = _FAIL;
+
+					if (start == 0)
+						start = jiffies;
+
+					if (is_broadcast_mac_addr(prxattrib->ra))
+						no_gkey_bc_cnt++;
+					else
+						no_gkey_mc_cnt++;
+
+					if (jiffies_to_msecs(jiffies - start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							netdev_dbg(padapter->pnetdev,
+								   FUNC_ADPT_FMT " no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+								   FUNC_ADPT_ARG(padapter),
+								   no_gkey_bc_cnt,
+								   no_gkey_mc_cnt);
+						}
+						start = jiffies;
+						no_gkey_bc_cnt = 0;
+						no_gkey_mc_cnt = 0;
+					}
+
+					goto exit;
+				}
+
+				if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+					netdev_dbg(padapter->pnetdev,
+						   FUNC_ADPT_FMT " gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+						   FUNC_ADPT_ARG(padapter),
+						   no_gkey_bc_cnt,
+						   no_gkey_mc_cnt);
+				}
+				start = 0;
+				no_gkey_bc_cnt = 0;
+				no_gkey_mc_cnt = 0;
+
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
+					res = _FAIL;
+					goto exit;
+				}
+			} else {
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+			}
+
+			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+			res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
+
+		} else {
+			res = _FAIL;
+		}
+	}
+exit:
+	return res;
+}
+
+u32 rtw_BIP_verify(struct adapter *padapter, u8 *precvframe)
+{
+	struct rx_pkt_attrib *pattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	u8 *pframe;
+	u8 *BIP_AAD, *p;
+	u32 res = _FAIL;
+	uint len, ori_len;
+	struct ieee80211_hdr *pwlanhdr;
+	u8 mic[16];
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	__le16 le_tmp;
+	__le64 le_tmp64;
+
+	ori_len = pattrib->pkt_len-WLAN_HDR_A3_LEN+BIP_AAD_SIZE;
+	BIP_AAD = rtw_zmalloc(ori_len);
+
+	if (!BIP_AAD)
+		return _FAIL;
+
+	/* PKT start */
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+	/* mapping to wlan header */
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	/* save the frame body + MME */
+	memcpy(BIP_AAD+BIP_AAD_SIZE, pframe+WLAN_HDR_A3_LEN, pattrib->pkt_len-WLAN_HDR_A3_LEN);
+	/* find MME IE pointer */
+	p = rtw_get_ie(BIP_AAD+BIP_AAD_SIZE, WLAN_EID_MMIE, &len, pattrib->pkt_len-WLAN_HDR_A3_LEN);
+	/* Baron */
+	if (p) {
+		u16 keyid = 0;
+		u64 temp_ipn = 0;
+		/* save packet number */
+		memcpy(&le_tmp64, p+4, 6);
+		temp_ipn = le64_to_cpu(le_tmp64);
+		/* BIP packet number should bigger than previous BIP packet */
+		if (temp_ipn <= pmlmeext->mgnt_80211w_IPN_rx)
+			goto BIP_exit;
+
+		/* copy key index */
+		memcpy(&le_tmp, p+2, 2);
+		keyid = le16_to_cpu(le_tmp);
+		if (keyid != padapter->securitypriv.dot11wBIPKeyid)
+			goto BIP_exit;
+
+		/* clear the MIC field of MME to zero */
+		memset(p+2+len-8, 0, 8);
+
+		/* conscruct AAD, copy frame control field */
+		memcpy(BIP_AAD, &pwlanhdr->frame_control, 2);
+		ClearRetry(BIP_AAD);
+		ClearPwrMgt(BIP_AAD);
+		ClearMData(BIP_AAD);
+		/* conscruct AAD, copy address 1 to address 3 */
+		memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
+
+		if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
+			, BIP_AAD, ori_len, mic))
+			goto BIP_exit;
+
+		/* MIC field should be last 8 bytes of packet (packet without FCS) */
+		if (!memcmp(mic, pframe+pattrib->pkt_len-8, 8)) {
+			pmlmeext->mgnt_80211w_IPN_rx = temp_ipn;
+			res = _SUCCESS;
+		} else {
+		}
+
+	} else {
+		res = RTW_RX_HANDLED;
+	}
+BIP_exit:
+
+	kfree(BIP_AAD);
+	return res;
+}
+
+static void gf_mulx(u8 *pad)
+{
+	int i, carry;
+
+	carry = pad[0] & 0x80;
+	for (i = 0; i < AES_BLOCK_SIZE - 1; i++)
+		pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7);
+
+	pad[AES_BLOCK_SIZE - 1] <<= 1;
+	if (carry)
+		pad[AES_BLOCK_SIZE - 1] ^= 0x87;
+}
+
+/**
+ * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128
+ * @key: 128-bit key for the hash operation
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */
+static int omac1_aes_128_vector(u8 *key, size_t num_elem,
+				u8 *addr[], size_t *len, u8 *mac)
+{
+	struct crypto_aes_ctx ctx;
+	u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE];
+	u8 *pos, *end;
+	size_t i, e, left, total_len;
+	int ret;
+
+	ret = aes_expandkey(&ctx, key, 16);
+	if (ret)
+		return -1;
+	memset(cbc, 0, AES_BLOCK_SIZE);
+
+	total_len = 0;
+	for (e = 0; e < num_elem; e++)
+		total_len += len[e];
+	left = total_len;
+
+	e = 0;
+	pos = addr[0];
+	end = pos + len[0];
+
+	while (left >= AES_BLOCK_SIZE) {
+		for (i = 0; i < AES_BLOCK_SIZE; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		if (left > AES_BLOCK_SIZE)
+			aes_encrypt(&ctx, cbc, cbc);
+		left -= AES_BLOCK_SIZE;
+	}
+
+	memset(pad, 0, AES_BLOCK_SIZE);
+	aes_encrypt(&ctx, pad, pad);
+	gf_mulx(pad);
+
+	if (left || total_len == 0) {
+		for (i = 0; i < left; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		cbc[left] ^= 0x80;
+		gf_mulx(pad);
+	}
+
+	for (i = 0; i < AES_BLOCK_SIZE; i++)
+		pad[i] ^= cbc[i];
+	aes_encrypt(&ctx, pad, mac);
+	memzero_explicit(&ctx, sizeof(ctx));
+	return 0;
+}
+
+/**
+ * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC)
+ * @key: 128-bit key for the hash operation
+ * @data: Data buffer for which a MAC is determined
+ * @data_len: Length of data buffer in bytes
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ * modify for CONFIG_IEEE80211W */
+int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac)
+{
+	return omac1_aes_128_vector(key, 1, &data, &data_len, mac);
+}
+
+/* Restore HW wep key setting according to key_mask */
+void rtw_sec_restore_wep_key(struct adapter *adapter)
+{
+	struct security_priv *securitypriv = &(adapter->securitypriv);
+	signed int keyid;
+
+	if ((_WEP40_ == securitypriv->dot11PrivacyAlgrthm) || (_WEP104_ == securitypriv->dot11PrivacyAlgrthm)) {
+		for (keyid = 0; keyid < 4; keyid++) {
+			if (securitypriv->key_mask & BIT(keyid)) {
+				if (keyid == securitypriv->dot11PrivacyKeyIndex)
+					rtw_set_key(adapter, securitypriv, keyid, 1, false);
+				else
+					rtw_set_key(adapter, securitypriv, keyid, 0, false);
+			}
+		}
+	}
+}
+
+u8 rtw_handle_tkip_countermeasure(struct adapter *adapter, const char *caller)
+{
+	struct security_priv *securitypriv = &(adapter->securitypriv);
+	u8 status = _SUCCESS;
+
+	if (securitypriv->btkip_countermeasure) {
+		unsigned long passing_ms = jiffies_to_msecs(jiffies - securitypriv->btkip_countermeasure_time);
+
+		if (passing_ms > 60*1000) {
+			netdev_dbg(adapter->pnetdev,
+				   "%s(%s) countermeasure time:%lus > 60s\n",
+				   caller, ADPT_ARG(adapter),
+				   passing_ms / 1000);
+			securitypriv->btkip_countermeasure = false;
+			securitypriv->btkip_countermeasure_time = 0;
+		} else {
+			netdev_dbg(adapter->pnetdev,
+				   "%s(%s) countermeasure time:%lus < 60s\n",
+				   caller, ADPT_ARG(adapter),
+				   passing_ms / 1000);
+			status = _FAIL;
+		}
+	}
+
+	return status;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
new file mode 100644
index 000000000..beb11d89d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
@@ -0,0 +1,557 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+void _rtw_init_stainfo(struct sta_info *psta);
+void _rtw_init_stainfo(struct sta_info *psta)
+{
+	memset((u8 *)psta, 0, sizeof(struct sta_info));
+
+	spin_lock_init(&psta->lock);
+	INIT_LIST_HEAD(&psta->list);
+	INIT_LIST_HEAD(&psta->hash_list);
+	/* INIT_LIST_HEAD(&psta->asoc_list); */
+	/* INIT_LIST_HEAD(&psta->sleep_list); */
+	/* INIT_LIST_HEAD(&psta->wakeup_list); */
+
+	INIT_LIST_HEAD(&psta->sleep_q.queue);
+	spin_lock_init(&psta->sleep_q.lock);
+	psta->sleepq_len = 0;
+
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+	INIT_LIST_HEAD(&psta->asoc_list);
+
+	INIT_LIST_HEAD(&psta->auth_list);
+
+	psta->expire_to = 0;
+
+	psta->flags = 0;
+
+	psta->capability = 0;
+
+	psta->bpairwise_key_installed = false;
+
+	psta->nonerp_set = 0;
+	psta->no_short_slot_time_set = 0;
+	psta->no_short_preamble_set = 0;
+	psta->no_ht_gf_set = 0;
+	psta->no_ht_set = 0;
+	psta->ht_20mhz_set = 0;
+
+	psta->under_exist_checking = 0;
+
+	psta->keep_alive_trycnt = 0;
+}
+
+u32 _rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+	pstapriv->pallocated_stainfo_buf = vzalloc(sizeof(struct sta_info) * NUM_STA+4);
+
+	if (!pstapriv->pallocated_stainfo_buf)
+		return _FAIL;
+
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+		((SIZE_PTR)(pstapriv->pallocated_stainfo_buf) & 3);
+
+	INIT_LIST_HEAD(&pstapriv->free_sta_queue.queue);
+	spin_lock_init(&pstapriv->free_sta_queue.lock);
+
+	spin_lock_init(&pstapriv->sta_hash_lock);
+
+	/* _rtw_init_queue(&pstapriv->asoc_q); */
+	pstapriv->asoc_sta_count = 0;
+	INIT_LIST_HEAD(&pstapriv->sleep_q.queue);
+	spin_lock_init(&pstapriv->sleep_q.lock);
+	INIT_LIST_HEAD(&pstapriv->wakeup_q.queue);
+	spin_lock_init(&pstapriv->wakeup_q.lock);
+
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+	for (i = 0; i < NUM_STA; i++) {
+		_rtw_init_stainfo(psta);
+
+		INIT_LIST_HEAD(&(pstapriv->sta_hash[i]));
+
+		list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+		psta++;
+	}
+
+	pstapriv->sta_dz_bitmap = 0;
+	pstapriv->tim_bitmap = 0;
+
+	INIT_LIST_HEAD(&pstapriv->asoc_list);
+	INIT_LIST_HEAD(&pstapriv->auth_list);
+	spin_lock_init(&pstapriv->asoc_list_lock);
+	spin_lock_init(&pstapriv->auth_list_lock);
+	pstapriv->asoc_list_cnt = 0;
+	pstapriv->auth_list_cnt = 0;
+
+	pstapriv->auth_to = 3; /*  3*2 = 6 sec */
+	pstapriv->assoc_to = 3;
+	pstapriv->expire_to = 3; /*  3*2 = 6 sec */
+	pstapriv->max_num_sta = NUM_STA;
+	return _SUCCESS;
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+/*  this function is used to free the memory of lock || sema for all stainfos */
+void kfree_all_stainfo(struct sta_priv *pstapriv);
+void kfree_all_stainfo(struct sta_priv *pstapriv)
+{
+	struct list_head	*plist, *phead;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		plist = get_next(plist);
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+void kfree_sta_priv_lock(struct	sta_priv *pstapriv);
+void kfree_sta_priv_lock(struct	sta_priv *pstapriv)
+{
+	 kfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
+}
+
+u32 _rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct list_head	*phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int	index;
+
+	if (pstapriv) {
+		/*delete all reordering_ctrl_timer		*/
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		for (index = 0; index < NUM_STA; index++) {
+			phead = &(pstapriv->sta_hash[index]);
+			list_for_each(plist, phead) {
+				int i;
+
+				psta = list_entry(plist, struct sta_info,
+						  hash_list);
+
+				for (i = 0; i < 16 ; i++) {
+					preorder_ctrl = &psta->recvreorder_ctrl[i];
+					del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+				}
+			}
+		}
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+		/*===============================*/
+
+		kfree_sta_priv_lock(pstapriv);
+
+		vfree(pstapriv->pallocated_stainfo_buf);
+	}
+	return _SUCCESS;
+}
+
+/* struct	sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr) */
+struct	sta_info *rtw_alloc_stainfo(struct	sta_priv *pstapriv, u8 *hwaddr)
+{
+	s32	index;
+	struct list_head	*phash_list;
+	struct sta_info *psta;
+	struct __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	/* spin_lock_bh(&(pfree_sta_queue->lock)); */
+	spin_lock_bh(&(pstapriv->sta_hash_lock));
+	if (list_empty(&pfree_sta_queue->queue)) {
+		/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+		return NULL;
+	} else {
+		psta = container_of(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+
+		list_del_init(&(psta->list));
+
+		/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+
+		_rtw_init_stainfo(psta);
+
+		psta->padapter = pstapriv->padapter;
+
+		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+		index = wifi_mac_hash(hwaddr);
+
+		if (index >= NUM_STA) {
+			spin_unlock_bh(&(pstapriv->sta_hash_lock));
+			psta = NULL;
+			goto exit;
+		}
+		phash_list = &(pstapriv->sta_hash[index]);
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+
+		list_add_tail(&psta->hash_list, phash_list);
+
+		pstapriv->asoc_sta_count++;
+
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+/*  Commented by Albert 2009/08/13 */
+/*  For the SMC router, the sequence number of first packet of WPS handshake will be 0. */
+/*  In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable. */
+/*  So, we initialize the tid_rxseq variable as the 0xffff. */
+
+		for (i = 0; i < 16; i++)
+			memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2);
+
+		init_addba_retry_timer(pstapriv->padapter, psta);
+
+		/* for A-MPDU Rx reordering buffer control */
+		for (i = 0; i < 16 ; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+			preorder_ctrl->padapter = pstapriv->padapter;
+
+			preorder_ctrl->enable = false;
+
+			preorder_ctrl->indicate_seq = 0xffff;
+			preorder_ctrl->wend_b = 0xffff;
+			/* preorder_ctrl->wsize_b = (NR_RECVBUFF-2); */
+			preorder_ctrl->wsize_b = 64;/* 64; */
+
+			INIT_LIST_HEAD(&preorder_ctrl->pending_recvframe_queue.queue);
+			spin_lock_init(&preorder_ctrl->pending_recvframe_queue.lock);
+
+			rtw_init_recv_timer(preorder_ctrl);
+		}
+
+		/* init for DM */
+		psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+		psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+
+		/* init for the sequence number of received management frame */
+		psta->RxMgmtFrameSeqNum = 0xffff;
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+		/* alloc mac id for non-bc/mc station, */
+		rtw_alloc_macid(pstapriv->padapter, psta);
+	}
+
+exit:
+
+	return psta;
+}
+
+u32 rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	struct __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv *pstaxmitpriv;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct hw_xmit *phwxmit;
+
+	if (!psta)
+		goto exit;
+
+	spin_lock_bh(&psta->lock);
+	psta->state &= ~_FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* list_del_init(&psta->sleep_list); */
+
+	/* list_del_init(&psta->wakeup_list); */
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	/* vo */
+	/* spin_lock_bh(&(pxmitpriv->vo_pending.lock)); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits;
+	phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt;
+	pstaxmitpriv->vo_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->vo_pending.lock)); */
+
+	/* vi */
+	/* spin_lock_bh(&(pxmitpriv->vi_pending.lock)); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+1;
+	phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt;
+	pstaxmitpriv->vi_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->vi_pending.lock)); */
+
+	/* be */
+	/* spin_lock_bh(&(pxmitpriv->be_pending.lock)); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+2;
+	phwxmit->accnt -= pstaxmitpriv->be_q.qcnt;
+	pstaxmitpriv->be_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->be_pending.lock)); */
+
+	/* bk */
+	/* spin_lock_bh(&(pxmitpriv->bk_pending.lock)); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+3;
+	phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt;
+	pstaxmitpriv->bk_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->bk_pending.lock)); */
+
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+	list_del_init(&psta->hash_list);
+	pstapriv->asoc_sta_count--;
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	/*  re-init sta_info; 20061114 will be init in alloc_stainfo */
+	/* _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); */
+	/* _rtw_init_sta_recv_priv(&psta->sta_recvpriv); */
+
+	del_timer_sync(&psta->addba_retry_timer);
+
+	/* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+	for (i = 0; i < 16 ; i++) {
+		struct list_head	*phead, *plist;
+		union recv_frame *prframe;
+		struct __queue *ppending_recvframe_queue;
+		struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		spin_lock_bh(&ppending_recvframe_queue->lock);
+
+		phead =		get_list_head(ppending_recvframe_queue);
+		plist = get_next(phead);
+
+		while (!list_empty(phead)) {
+			prframe = (union recv_frame *)plist;
+
+			plist = get_next(plist);
+
+			list_del_init(&(prframe->u.hdr.list));
+
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+		}
+
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+	}
+
+	if (!(psta->state & WIFI_AP_STATE))
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, false);
+
+	/* release mac id for non-bc/mc station, */
+	rtw_release_macid(pstapriv->padapter, psta);
+
+/*
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	list_del_init(&psta->asoc_list);
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+*/
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&psta->auth_list)) {
+		list_del_init(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	psta->expire_to = 0;
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+
+	psta->has_legacy_ac = 0;
+
+	pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+	if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}
+
+	psta->under_exist_checking = 0;
+
+	/* spin_lock_bh(&(pfree_sta_queue->lock)); */
+	list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+	/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+
+exit:
+	return _SUCCESS;
+}
+
+/*  free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(struct adapter *padapter)
+{
+	struct list_head *plist, *phead, *tmp;
+	s32	index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
+	LIST_HEAD(stainfo_free_list);
+
+	if (pstapriv->asoc_sta_count == 1)
+		return;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &(pstapriv->sta_hash[index]);
+		list_for_each_safe(plist, tmp, phead) {
+			psta = list_entry(plist, struct sta_info, hash_list);
+
+			if (pbcmc_stainfo != psta)
+				list_move(&psta->hash_list, &stainfo_free_list);
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	list_for_each_safe(plist, tmp, &stainfo_free_list) {
+		psta = list_entry(plist, struct sta_info, hash_list);
+		rtw_free_stainfo(padapter, psta);
+	}
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	struct list_head	*plist, *phead;
+	struct sta_info *psta = NULL;
+	u32 index;
+	u8 *addr;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if (!hwaddr)
+		return NULL;
+
+	if (IS_MCAST(hwaddr))
+		addr = bc_addr;
+	else
+		addr = hwaddr;
+
+	index = wifi_mac_hash(addr);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = &(pstapriv->sta_hash[index]);
+	list_for_each(plist, phead) {
+		psta = list_entry(plist, struct sta_info, hash_list);
+
+		if ((!memcmp(psta->hwaddr, addr, ETH_ALEN)))
+		 /*  if found the matched address */
+			break;
+
+		psta = NULL;
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+	return psta;
+}
+
+u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
+{
+	struct sta_info *psta;
+	NDIS_802_11_MAC_ADDRESS	bcast_addr = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	/* struct __queue	*pstapending = &padapter->xmitpriv.bm_pending; */
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+	if (!psta)
+		return _FAIL;
+
+	/*  default broadcast & multicast use macid 1 */
+	psta->mac_id = 1;
+
+	return _SUCCESS;
+}
+
+struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	return rtw_get_stainfo(pstapriv, bc_addr);
+}
+
+u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr)
+{
+	bool res = true;
+	struct list_head	*plist, *phead;
+	struct rtw_wlan_acl_node *paclnode;
+	bool match = false;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	spin_lock_bh(&(pacl_node_q->lock));
+	phead = get_list_head(pacl_node_q);
+	list_for_each(plist, phead) {
+		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
+
+		if (!memcmp(paclnode->addr, mac_addr, ETH_ALEN))
+			if (paclnode->valid == true) {
+				match = true;
+				break;
+			}
+	}
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	if (pacl_list->mode == 1) /* accept unless in deny list */
+		res = !match;
+
+	else if (pacl_list->mode == 2)/* deny unless in accept list */
+		res = match;
+	else
+		 res = true;
+
+	return res;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
new file mode 100644
index 000000000..18ba846c0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
@@ -0,0 +1,1857 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_com_h2c.h>
+
+static unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+static unsigned char ARTHEROS_OUI2[] = {0x00, 0x13, 0x74};
+
+static unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18};
+static unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7};
+static unsigned char BROADCOM_OUI3[] = {0x00, 0x05, 0xb5};
+
+static unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96};
+static unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43};
+static unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43};
+static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
+static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
+static unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+static unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+
+/* define WAIT_FOR_BCN_TO_MIN	(3000) */
+#define WAIT_FOR_BCN_TO_MIN	(6000)
+#define WAIT_FOR_BCN_TO_MAX	(20000)
+
+#define DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS 1000
+#define DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD 3
+
+static u8 rtw_basic_rate_cck[4] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_ofdm[3] = {
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
+};
+
+u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta)
+{
+	u8 raid;
+
+	switch (psta->wireless_mode) {
+	case WIRELESS_11B:
+		raid = RATEID_IDX_B;
+		break;
+	case WIRELESS_11G:
+		raid = RATEID_IDX_G;
+		break;
+	case WIRELESS_11BG:
+		raid = RATEID_IDX_BG;
+		break;
+	case WIRELESS_11_24N:
+	case WIRELESS_11G_24N:
+		raid = RATEID_IDX_GN_N1SS;
+		break;
+	case WIRELESS_11B_24N:
+	case WIRELESS_11BG_24N:
+		if (psta->bw_mode == CHANNEL_WIDTH_20) {
+			raid = RATEID_IDX_BGN_20M_1SS_BN;
+		} else {
+			raid = RATEID_IDX_BGN_40M_1SS;
+		}
+		break;
+	default:
+		raid = RATEID_IDX_BGN_40M_2SS;
+		break;
+	}
+	return raid;
+}
+
+unsigned char ratetbl_val_2wifirate(unsigned char rate);
+unsigned char ratetbl_val_2wifirate(unsigned char rate)
+{
+	switch (rate & 0x7f) {
+	case 0:
+		return IEEE80211_CCK_RATE_1MB;
+	case 1:
+		return IEEE80211_CCK_RATE_2MB;
+	case 2:
+		return IEEE80211_CCK_RATE_5MB;
+	case 3:
+		return IEEE80211_CCK_RATE_11MB;
+	case 4:
+		return IEEE80211_OFDM_RATE_6MB;
+	case 5:
+		return IEEE80211_OFDM_RATE_9MB;
+	case 6:
+		return IEEE80211_OFDM_RATE_12MB;
+	case 7:
+		return IEEE80211_OFDM_RATE_18MB;
+	case 8:
+		return IEEE80211_OFDM_RATE_24MB;
+	case 9:
+		return IEEE80211_OFDM_RATE_36MB;
+	case 10:
+		return IEEE80211_OFDM_RATE_48MB;
+	case 11:
+		return IEEE80211_OFDM_RATE_54MB;
+	default:
+		return 0;
+	}
+}
+
+int is_basicrate(struct adapter *padapter, unsigned char rate);
+int is_basicrate(struct adapter *padapter, unsigned char rate)
+{
+	int i;
+	unsigned char val;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		val = pmlmeext->basicrate[i];
+
+		if ((val != 0xff) && (val != 0xfe))
+			if (rate == ratetbl_val_2wifirate(val))
+				return true;
+	}
+
+	return false;
+}
+
+unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset);
+unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset)
+{
+	int i;
+	unsigned char rate;
+	unsigned int	len = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		rate = pmlmeext->datarate[i];
+
+		switch (rate) {
+		case 0xff:
+			return len;
+
+		case 0xfe:
+			continue;
+
+		default:
+			rate = ratetbl_val_2wifirate(rate);
+
+			if (is_basicrate(padapter, rate) == true)
+				rate |= IEEE80211_BASIC_RATE_MASK;
+
+			rateset[len] = rate;
+			len++;
+			break;
+		}
+	}
+	return len;
+}
+
+void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+{
+	unsigned char supportedrates[NumRates];
+
+	memset(supportedrates, 0, NumRates);
+	*bssrate_len = ratetbl2rateset(padapter, supportedrates);
+	memcpy(pbssrate, supportedrates, *bssrate_len);
+}
+
+void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask)
+{
+	u8 mcs_rate_1r = (u8)(mask&0xff);
+	u8 mcs_rate_2r = (u8)((mask>>8)&0xff);
+	u8 mcs_rate_3r = (u8)((mask>>16)&0xff);
+	u8 mcs_rate_4r = (u8)((mask>>24)&0xff);
+
+	mcs_set[0] &= mcs_rate_1r;
+	mcs_set[1] &= mcs_rate_2r;
+	mcs_set[2] &= mcs_rate_3r;
+	mcs_set[3] &= mcs_rate_4r;
+}
+
+void UpdateBrateTbl(struct adapter *Adapter, u8 *mBratesOS)
+{
+	u8 i;
+	u8 rate;
+
+	/*  1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		rate = mBratesOS[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+		case IEEE80211_OFDM_RATE_6MB:
+		case IEEE80211_OFDM_RATE_12MB:
+		case IEEE80211_OFDM_RATE_24MB:
+			mBratesOS[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+}
+
+void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+{
+	u8 i;
+	u8 rate;
+
+	for (i = 0; i < bssratelen; i++) {
+		rate = bssrateset[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+			bssrateset[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+}
+
+void Save_DM_Func_Flag(struct adapter *padapter)
+{
+	u8 bSaveFlag = true;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+}
+
+void Restore_DM_Func_Flag(struct adapter *padapter)
+{
+	u8 bSaveFlag = false;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+}
+
+void Switch_DM_Func(struct adapter *padapter, u32 mode, u8 enable)
+{
+	if (enable == true)
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
+	else
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
+}
+
+void Set_MSR(struct adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+inline u8 rtw_get_oper_ch(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_channel;
+}
+
+inline void rtw_set_oper_ch(struct adapter *adapter, u8 ch)
+{
+#ifdef DBG_CH_SWITCH
+	const int len = 128;
+	char msg[128] = {0};
+	int cnt = 0;
+	int i = 0;
+#endif  /* DBG_CH_SWITCH */
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+
+	if (dvobj->oper_channel != ch) {
+		dvobj->on_oper_ch_time = jiffies;
+
+#ifdef DBG_CH_SWITCH
+		cnt += scnprintf(msg+cnt, len-cnt, "switch to ch %3u", ch);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			struct adapter *iface = dvobj->padapters[i];
+
+			cnt += scnprintf(msg+cnt, len-cnt, " [%s:", ADPT_ARG(iface));
+			if (iface->mlmeextpriv.cur_channel == ch)
+				cnt += scnprintf(msg+cnt, len-cnt, "C");
+			else
+				cnt += scnprintf(msg+cnt, len-cnt, "_");
+			if (iface->wdinfo.listen_channel == ch && !rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_NONE))
+				cnt += scnprintf(msg+cnt, len-cnt, "L");
+			else
+				cnt += scnprintf(msg+cnt, len-cnt, "_");
+			cnt += scnprintf(msg+cnt, len-cnt, "]");
+		}
+
+#endif /* DBG_CH_SWITCH */
+	}
+
+	dvobj->oper_channel = ch;
+}
+
+inline u8 rtw_get_oper_bw(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_bwmode;
+}
+
+inline void rtw_set_oper_bw(struct adapter *adapter, u8 bw)
+{
+	adapter_to_dvobj(adapter)->oper_bwmode = bw;
+}
+
+inline u8 rtw_get_oper_choffset(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_ch_offset;
+}
+
+inline void rtw_set_oper_choffset(struct adapter *adapter, u8 offset)
+{
+	adapter_to_dvobj(adapter)->oper_ch_offset = offset;
+}
+
+u8 rtw_get_center_ch(u8 channel, u8 chnl_bw, u8 chnl_offset)
+{
+	u8 center_ch = channel;
+
+	if (chnl_bw == CHANNEL_WIDTH_40) {
+		if (chnl_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+			center_ch = channel + 2;
+		else
+			center_ch = channel - 2;
+	}
+
+	return center_ch;
+}
+
+inline unsigned long rtw_get_on_cur_ch_time(struct adapter *adapter)
+{
+	if (adapter->mlmeextpriv.cur_channel == adapter_to_dvobj(adapter)->oper_channel)
+		return adapter_to_dvobj(adapter)->on_oper_ch_time;
+	else
+		return 0;
+}
+
+void SelectChannel(struct adapter *padapter, unsigned char channel)
+{
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex)))
+		return;
+
+	/* saved channel info */
+	rtw_set_oper_ch(padapter, channel);
+
+	rtw_hal_set_chan(padapter, channel);
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex));
+}
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+	u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	center_ch = rtw_get_center_ch(channel, bwmode, channel_offset);
+
+
+	/* set Channel */
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex)))
+		return;
+
+	/* saved channel/bw info */
+	rtw_set_oper_ch(padapter, channel);
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	rtw_hal_set_chnl_bw(padapter, center_ch, bwmode, channel_offset, chnl_offset80); /*  set center channel */
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex));
+}
+
+inline u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork)
+{
+	return pnetwork->mac_address;
+}
+
+u16 get_beacon_interval(struct wlan_bssid_ex *bss)
+{
+	__le16 val;
+
+	memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->ies), 2);
+
+	return le16_to_cpu(val);
+}
+
+int is_client_associated_to_ap(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+	if (!padapter)
+		return _FAIL;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_client_associated_to_ibss(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_IBSS_empty(struct adapter *padapter)
+{
+	unsigned int i;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+		if (pmlmeinfo->FW_sta_info[i].status == 1)
+			return _FAIL;
+	}
+
+	return true;
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+	if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+		return WAIT_FOR_BCN_TO_MIN;
+	else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+		return WAIT_FOR_BCN_TO_MAX;
+	else
+		return bcn_interval << 2;
+}
+
+void invalidate_cam_all(struct adapter *padapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+
+	spin_lock_bh(&cam_ctl->lock);
+	cam_ctl->bitmap = 0;
+	memset(dvobj->cam_cache, 0, sizeof(struct cam_entry_cache)*TOTAL_CAM_ENTRY);
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+static u32 _ReadCAM(struct adapter *padapter, u32 addr)
+{
+	u32 count = 0, cmd;
+
+	cmd = CAM_POLLINIG | addr;
+	rtw_write32(padapter, RWCAM, cmd);
+
+	do {
+		if (0 == (rtw_read32(padapter, REG_CAMCMD) & CAM_POLLINIG))
+			break;
+	} while (count++ < 100);
+
+	return rtw_read32(padapter, REG_CAMREAD);
+}
+
+void read_cam(struct adapter *padapter, u8 entry, u8 *get_key)
+{
+	u32 j, addr, cmd;
+
+	addr = entry << 3;
+
+	for (j = 0; j < 6; j++) {
+		cmd = _ReadCAM(padapter, addr+j);
+		if (j > 1) /* get key from cam */
+			memcpy(get_key+(j-2)*4, &cmd, 4);
+	}
+}
+
+void _write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
+{
+	unsigned int i, val, addr;
+	int j;
+	u32 cam_val[2];
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--) {
+		switch (j) {
+		case 0:
+			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
+			break;
+		case 1:
+			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+			break;
+		default:
+			i = (j - 2) << 2;
+			val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
+			break;
+		}
+
+		cam_val[0] = val;
+		cam_val[1] = addr + (unsigned int)j;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+	}
+}
+
+void _clear_cam_entry(struct adapter *padapter, u8 entry)
+{
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	_write_cam(padapter, entry, 0, null_sta, null_key);
+}
+
+inline void write_cam(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key)
+{
+	_write_cam(adapter, id, ctrl, mac, key);
+	write_cam_cache(adapter, id, ctrl, mac, key);
+}
+
+inline void clear_cam_entry(struct adapter *adapter, u8 id)
+{
+	_clear_cam_entry(adapter, id);
+	clear_cam_cache(adapter, id);
+}
+
+void write_cam_cache(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	dvobj->cam_cache[id].ctrl = ctrl;
+	memcpy(dvobj->cam_cache[id].mac, mac, ETH_ALEN);
+	memcpy(dvobj->cam_cache[id].key, key, 16);
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+void clear_cam_cache(struct adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	memset(&(dvobj->cam_cache[id]), 0, sizeof(struct cam_entry_cache));
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+static bool _rtw_camid_is_gk(struct adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	bool ret = false;
+
+	if (cam_id >= TOTAL_CAM_ENTRY)
+		goto exit;
+
+	if (!(cam_ctl->bitmap & BIT(cam_id)))
+		goto exit;
+
+	ret = (dvobj->cam_cache[cam_id].ctrl&BIT6)?true:false;
+
+exit:
+	return ret;
+}
+
+static s16 _rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	int i;
+	s16 cam_id = -1;
+
+	for (i = 0; i < TOTAL_CAM_ENTRY; i++) {
+		if (addr && memcmp(dvobj->cam_cache[i].mac, addr, ETH_ALEN))
+			continue;
+		if (kid >= 0 && kid != (dvobj->cam_cache[i].ctrl&0x03))
+			continue;
+
+		cam_id = i;
+		break;
+	}
+
+	return cam_id;
+}
+
+s16 rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	s16 cam_id = -1;
+
+	spin_lock_bh(&cam_ctl->lock);
+	cam_id = _rtw_camid_search(adapter, addr, kid);
+	spin_unlock_bh(&cam_ctl->lock);
+
+	return cam_id;
+}
+
+s16 rtw_camid_alloc(struct adapter *adapter, struct sta_info *sta, u8 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	s16 cam_id = -1;
+	struct mlme_ext_info *mlmeinfo;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	mlmeinfo = &adapter->mlmeextpriv.mlmext_info;
+
+	if ((((mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) || ((mlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+		&& !sta) {
+		/* AP/Ad-hoc mode group key: static alloction to default key by key ID */
+		if (kid > 3) {
+			netdev_dbg(adapter->pnetdev,
+				   FUNC_ADPT_FMT " group key with invalid key id:%u\n",
+				   FUNC_ADPT_ARG(adapter), kid);
+			rtw_warn_on(1);
+			goto bitmap_handle;
+		}
+
+		cam_id = kid;
+	} else {
+		int i;
+		u8 *addr = sta?sta->hwaddr:NULL;
+
+		if (!sta) {
+			if (!(mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+				/* bypass STA mode group key setting before connected(ex:WEP) because bssid is not ready */
+				goto bitmap_handle;
+			}
+
+			addr = get_bssid(&adapter->mlmepriv);
+		}
+
+		i = _rtw_camid_search(adapter, addr, kid);
+		if (i >= 0) {
+			/* Fix issue that pairwise and group key have same key id. Pairwise key first, group key can overwirte group only(ex: rekey) */
+			if (sta || _rtw_camid_is_gk(adapter, i))
+				cam_id = i;
+			else
+				netdev_dbg(adapter->pnetdev,
+					   FUNC_ADPT_FMT " group key id:%u the same key id as pairwise key\n",
+					   FUNC_ADPT_ARG(adapter), kid);
+			goto bitmap_handle;
+		}
+
+		for (i = 4; i < TOTAL_CAM_ENTRY; i++)
+			if (!(cam_ctl->bitmap & BIT(i)))
+				break;
+
+		if (i == TOTAL_CAM_ENTRY) {
+			if (sta)
+				netdev_dbg(adapter->pnetdev,
+					   FUNC_ADPT_FMT " pairwise key with %pM id:%u no room\n",
+					   FUNC_ADPT_ARG(adapter),
+					   MAC_ARG(sta->hwaddr), kid);
+			else
+				netdev_dbg(adapter->pnetdev,
+					   FUNC_ADPT_FMT " group key id:%u no room\n",
+					   FUNC_ADPT_ARG(adapter), kid);
+			rtw_warn_on(1);
+			goto bitmap_handle;
+		}
+
+		cam_id = i;
+	}
+
+bitmap_handle:
+	if (cam_id >= 0 && cam_id < 32)
+		cam_ctl->bitmap |= BIT(cam_id);
+
+	spin_unlock_bh(&cam_ctl->lock);
+
+	return cam_id;
+}
+
+void rtw_camid_free(struct adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	if (cam_id < TOTAL_CAM_ENTRY)
+		cam_ctl->bitmap &= ~(BIT(cam_id));
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+int allocate_fw_sta_entry(struct adapter *padapter)
+{
+	unsigned int mac_id;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
+		if (pmlmeinfo->FW_sta_info[mac_id].status == 0) {
+			pmlmeinfo->FW_sta_info[mac_id].status = 1;
+			pmlmeinfo->FW_sta_info[mac_id].retry = 0;
+			break;
+		}
+	}
+
+	return mac_id;
+}
+
+void flush_all_cam_entry(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	invalidate_cam_all(padapter);
+	/* clear default key related key search setting */
+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)false);
+
+	memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+}
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	/* struct registry_priv *pregpriv = &padapter->registrypriv; */
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pmlmepriv->qospriv.qos_option == 0) {
+		pmlmeinfo->WMM_enable = 0;
+		return false;
+	}
+
+	if (!memcmp(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element)))
+		return false;
+	else
+		memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element));
+
+	pmlmeinfo->WMM_enable = 1;
+	return true;
+}
+
+static void sort_wmm_ac_params(u32 *inx, u32 *edca)
+{
+	u32 i, j, change_inx = false;
+
+	/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
+	for (i = 0; i < 4; i++) {
+		for (j = i + 1; j < 4; j++) {
+			/* compare CW and AIFS */
+			if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) {
+				change_inx = true;
+			} else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+				/* compare TXOP */
+				if ((edca[j] >> 16) > (edca[i] >> 16))
+					change_inx = true;
+			}
+
+			if (change_inx) {
+				swap(edca[i], edca[j]);
+				swap(inx[i], inx[j]);
+
+				change_inx = false;
+			}
+		}
+	}
+}
+
+void WMMOnAssocRsp(struct adapter *padapter)
+{
+	u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8 acm_mask;
+	u16 TXOP;
+	u32 acParm, i;
+	u32 edca[4], inx[4];
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	acm_mask = 0;
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11_24N)
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+
+	if (pmlmeinfo->WMM_enable == 0) {
+		padapter->mlmepriv.acm_mask = 0;
+
+		AIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
+
+		if (pmlmeext->cur_wireless_mode & WIRELESS_11G) {
+			ECWMin = 4;
+			ECWMax = 10;
+		} else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
+			ECWMin = 5;
+			ECWMax = 10;
+		} else {
+			ECWMin = 4;
+			ECWMax = 10;
+		}
+
+		TXOP = 0;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+
+		ECWMin = 2;
+		ECWMax = 3;
+		TXOP = 0x2f;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+	} else {
+		edca[0] = edca[1] = edca[2] = edca[3] = 0;
+
+		for (i = 0; i < 4; i++) {
+			ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03;
+			ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
+
+			/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
+			AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+
+			ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+			ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
+			TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
+
+			acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+
+			switch (ACI) {
+			case 0x0:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(1):0);
+				edca[XMIT_BE_QUEUE] = acParm;
+				break;
+
+			case 0x1:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+				/* acm_mask |= (ACM? BIT(0):0); */
+				edca[XMIT_BK_QUEUE] = acParm;
+				break;
+
+			case 0x2:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(2):0);
+				edca[XMIT_VI_QUEUE] = acParm;
+				break;
+
+			case 0x3:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(3):0);
+				edca[XMIT_VO_QUEUE] = acParm;
+				break;
+			}
+		}
+
+		if (padapter->registrypriv.acm_method == 1)
+			rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		else
+			padapter->mlmepriv.acm_mask = acm_mask;
+
+		inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+		if (pregpriv->wifi_spec == 1)
+			sort_wmm_ac_params(inx, edca);
+
+		for (i = 0; i < 4; i++)
+			pxmitpriv->wmm_para_seq[i] = inx[i];
+	}
+}
+
+static void bwmode_update_check(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	unsigned char  new_bwmode;
+	unsigned char  new_ch_offset;
+	struct HT_info_element	 *pHT_info;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	u8 cbw40_enable = 0;
+
+	if (!pIE)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	if (pIE->length > sizeof(struct HT_info_element))
+		return;
+
+	pHT_info = (struct HT_info_element *)pIE->data;
+
+	if (pmlmeext->cur_channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else {
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			cbw40_enable = 1;
+	}
+
+	if ((pHT_info->infos[0] & BIT(2)) && cbw40_enable) {
+		new_bwmode = CHANNEL_WIDTH_40;
+
+		switch (pHT_info->infos[0] & 0x3) {
+		case 1:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+
+		case 3:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+
+		default:
+			new_bwmode = CHANNEL_WIDTH_20;
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+	} else {
+		new_bwmode = CHANNEL_WIDTH_20;
+		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	if ((new_bwmode != pmlmeext->cur_bwmode) || (new_ch_offset != pmlmeext->cur_ch_offset)) {
+		pmlmeinfo->bwmode_updated = true;
+
+		pmlmeext->cur_bwmode = new_bwmode;
+		pmlmeext->cur_ch_offset = new_ch_offset;
+
+		/* update HT info also */
+		HT_info_handler(padapter, pIE);
+	} else {
+		pmlmeinfo->bwmode_updated = false;
+	}
+
+	if (true == pmlmeinfo->bwmode_updated) {
+		struct sta_info *psta;
+		struct wlan_bssid_ex	*cur_network = &(pmlmeinfo->network);
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+
+		/* update ap's stainfo */
+		psta = rtw_get_stainfo(pstapriv, cur_network->mac_address);
+		if (psta) {
+			struct ht_priv *phtpriv_sta = &psta->htpriv;
+
+			if (phtpriv_sta->ht_option) {
+				/*  bwmode */
+				psta->bw_mode = pmlmeext->cur_bwmode;
+				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+			} else {
+				psta->bw_mode = CHANNEL_WIDTH_20;
+				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			}
+
+			rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta);
+		}
+	}
+}
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	unsigned int	i;
+	u8 max_AMPDU_len, min_MPDU_spacing;
+	u8 cur_ldpc_cap = 0, cur_stbc_cap = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+	if (!pIE)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	pmlmeinfo->HT_caps_enable = 1;
+
+	for (i = 0; i < (pIE->length); i++) {
+		if (i != 2) {
+			/* Commented by Albert 2010/07/12 */
+			/* Got the endian issue here. */
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+		} else {
+			/* modify from  fw by Thomas 2010/11/17 */
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
+				max_AMPDU_len = (pIE->data[i] & 0x3);
+			else
+				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
+				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			else
+				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+		}
+	}
+
+	/* update the MCS set */
+	for (i = 0; i < 16; i++)
+		pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i];
+
+	/* update the MCS rates */
+	set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/*  Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) &&
+		    GET_HT_CAPABILITY_ELE_TX_STBC(pIE->data))
+			SET_FLAG(cur_stbc_cap, STBC_HT_ENABLE_TX);
+
+		phtpriv->stbc_cap = cur_stbc_cap;
+	} else {
+		/*  Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) &&
+		    GET_HT_CAPABILITY_ELE_LDPC_CAP(pIE->data))
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+
+		phtpriv->ldpc_cap = cur_ldpc_cap;
+
+		/*  Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) &&
+		    GET_HT_CAPABILITY_ELE_RX_STBC(pIE->data))
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+
+		phtpriv->stbc_cap = cur_stbc_cap;
+	}
+}
+
+void HT_info_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+	if (!pIE)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	if (pIE->length > sizeof(struct HT_info_element))
+		return;
+
+	pmlmeinfo->HT_info_enable = 1;
+	memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->length);
+}
+
+void HTOnAssocRsp(struct adapter *padapter)
+{
+	unsigned char max_AMPDU_len;
+	unsigned char min_MPDU_spacing;
+	/* struct registry_priv  *pregpriv = &padapter->registrypriv; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) {
+		pmlmeinfo->HT_enable = 1;
+	} else {
+		pmlmeinfo->HT_enable = 0;
+		/* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		return;
+	}
+
+	/* handle A-MPDU parameter field */
+	/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+	*/
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+}
+
+void ERP_IE_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pIE->length > 1)
+		return;
+
+	pmlmeinfo->ERP_enable = 1;
+	memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->length);
+}
+
+void VCS_update(struct adapter *padapter, struct sta_info *psta)
+{
+	struct registry_priv  *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pregpriv->vrtl_carrier_sense) {/* 0:off 1:on 2:auto */
+	case 0: /* off */
+		psta->rtsen = 0;
+		psta->cts2self = 0;
+		break;
+
+	case 1: /* on */
+		if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+			psta->rtsen = 1;
+			psta->cts2self = 0;
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 1;
+		}
+		break;
+
+	case 2: /* auto */
+	default:
+		if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+			if (pregpriv->vcs_type == 1) {
+				psta->rtsen = 1;
+				psta->cts2self = 0;
+			} else {
+				psta->rtsen = 0;
+				psta->cts2self = 1;
+			}
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 0;
+		}
+		break;
+	}
+}
+
+void update_ldpc_stbc_cap(struct sta_info *psta)
+{
+	if (psta->htpriv.ht_option) {
+		if (TEST_FLAG(psta->htpriv.ldpc_cap, LDPC_HT_ENABLE_TX))
+			psta->ldpc = 1;
+
+		if (TEST_FLAG(psta->htpriv.stbc_cap, STBC_HT_ENABLE_TX))
+			psta->stbc = 1;
+	} else {
+		psta->ldpc = 0;
+		psta->stbc = 0;
+	}
+}
+
+int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len)
+{
+	unsigned int len;
+	unsigned char *p;
+	unsigned short	val16, subtype;
+	struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network);
+	/* u8 wpa_ie[255], rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	u8 encryp_protocol = 0;
+	struct wlan_bssid_ex *bssid;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
+	unsigned char *pbuf;
+	u32 wpa_ielen = 0;
+	u8 *pbssid = GetAddr3Ptr(pframe);
+	struct HT_info_element *pht_info = NULL;
+	struct ieee80211_ht_cap *pht_cap = NULL;
+	u32 bcn_channel;
+	unsigned short	ht_cap_info;
+	unsigned char ht_info_infos_0;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	int ssid_len;
+
+	if (is_client_associated_to_ap(Adapter) == false)
+		return true;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	if (memcmp(cur_network->network.mac_address, pbssid, 6))
+		return true;
+
+	bssid = rtw_zmalloc(sizeof(struct wlan_bssid_ex));
+	if (!bssid)
+		return true;
+
+	if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) > DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS)) {
+		pmlmepriv->timeBcnInfoChkStart = 0;
+		pmlmepriv->NumOfBcnInfoChkFail = 0;
+	}
+
+	subtype = GetFrameSubType(pframe) >> 4;
+
+	if (subtype == WIFI_BEACON)
+		bssid->reserved[0] = 1;
+
+	bssid->length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->ie_length = len;
+	memcpy(bssid->ies, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->ie_length);
+
+	/* check bw and channel offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_CAPABILITY, &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+		ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else {
+		ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_OPERATION, &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+		pht_info = (struct HT_info_element *)(p + 2);
+		ht_info_infos_0 = pht_info->infos[0];
+	} else {
+		ht_info_infos_0 = 0;
+	}
+	if (ht_cap_info != cur_network->bcn_info.ht_cap_info ||
+	    ((ht_info_infos_0&0x03) != (cur_network->bcn_info.ht_info_infos_0&0x03))) {
+		{
+			/* bcn_info_update */
+			cur_network->bcn_info.ht_cap_info = ht_cap_info;
+			cur_network->bcn_info.ht_info_infos_0 = ht_info_infos_0;
+			/* to do : need to check that whether modify related register of BB or not */
+		}
+		/* goto _mismatch; */
+	}
+
+	/* Checking for channel */
+	p = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_DS_PARAMS, &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+	if (p) {
+		bcn_channel = *(p + 2);
+	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+		rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_OPERATION,
+			   &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+		if (pht_info)
+			bcn_channel = pht_info->primary_channel;
+		else /* we don't find channel IE, so don't check it */
+			bcn_channel = Adapter->mlmeextpriv.cur_channel;
+	}
+
+	if (bcn_channel != Adapter->mlmeextpriv.cur_channel)
+		goto _mismatch;
+
+	/* checking SSID */
+	ssid_len = 0;
+	p = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_SSID, &len, bssid->ie_length - _FIXED_IE_LENGTH_);
+	if (p) {
+		ssid_len = *(p + 1);
+		if (ssid_len > NDIS_802_11_LENGTH_SSID)
+			ssid_len = 0;
+	}
+	memcpy(bssid->ssid.ssid, (p + 2), ssid_len);
+	bssid->ssid.ssid_length = ssid_len;
+
+	if (memcmp(bssid->ssid.ssid, cur_network->network.ssid.ssid, 32) ||
+			bssid->ssid.ssid_length != cur_network->network.ssid.ssid_length)
+		if (bssid->ssid.ssid[0] != '\0' &&
+		    bssid->ssid.ssid_length != 0) /* not hidden ssid */
+			goto _mismatch;
+
+	/* check encryption info */
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(4))
+		bssid->privacy = 1;
+	else
+		bssid->privacy = 0;
+
+	if (cur_network->network.privacy != bssid->privacy)
+		goto _mismatch;
+
+	rtw_get_sec_ie(bssid->ies, bssid->ie_length, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0)
+		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	else if (wpa_len > 0)
+		encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	else
+		if (bssid->privacy)
+			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+
+	if (cur_network->bcn_info.encryp_protocol != encryp_protocol)
+		goto _mismatch;
+
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		pbuf = rtw_get_wpa_ie(&bssid->ies[12], &wpa_ielen, bssid->ie_length-12);
+		if (pbuf && (wpa_ielen > 0)) {
+			rtw_parse_wpa_ie(pbuf, wpa_ielen + 2, &group_cipher,
+					 &pairwise_cipher, &is_8021x);
+		} else {
+			pbuf = rtw_get_wpa2_ie(&bssid->ies[12], &wpa_ielen, bssid->ie_length-12);
+
+			if (pbuf && (wpa_ielen > 0))
+				rtw_parse_wpa2_ie(pbuf, wpa_ielen + 2, &group_cipher,
+						  &pairwise_cipher, &is_8021x);
+		}
+
+		if (pairwise_cipher != cur_network->bcn_info.pairwise_cipher ||
+		    group_cipher != cur_network->bcn_info.group_cipher)
+			goto _mismatch;
+
+		if (is_8021x != cur_network->bcn_info.is_8021x)
+			goto _mismatch;
+	}
+
+	kfree(bssid);
+	return _SUCCESS;
+
+_mismatch:
+	kfree(bssid);
+
+	if (pmlmepriv->NumOfBcnInfoChkFail == 0)
+		pmlmepriv->timeBcnInfoChkStart = jiffies;
+
+	pmlmepriv->NumOfBcnInfoChkFail++;
+
+	if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) <= DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS)
+		&& (pmlmepriv->NumOfBcnInfoChkFail >= DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD)) {
+		pmlmepriv->timeBcnInfoChkStart = 0;
+		pmlmepriv->NumOfBcnInfoChkFail = 0;
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+	unsigned int i;
+	unsigned int len;
+	struct ndis_80211_var_ie *pIE;
+
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;) {
+		pIE = (struct ndis_80211_var_ie *)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+
+		switch (pIE->element_id) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			/* to update WMM parameter set while receiving beacon */
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6) && pIE->length == WLAN_WMM_LEN)	/* WMM */
+				if (WMM_param_handler(padapter, pIE))
+					report_wmm_edca_update(padapter);
+
+			break;
+
+		case WLAN_EID_HT_OPERATION:	/* HT info */
+			/* HT_info_handler(padapter, pIE); */
+			bwmode_update_check(padapter, pIE);
+			break;
+
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler(padapter, pIE);
+			VCS_update(padapter, psta);
+			break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->length + 2);
+	}
+}
+
+unsigned int is_ap_in_tkip(struct adapter *padapter)
+{
+	u32 i;
+	struct ndis_80211_var_ie *pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.ie_length;) {
+			pIE = (struct ndis_80211_var_ie *)(pmlmeinfo->network.ies + i);
+
+			switch (pIE->element_id) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4)))
+					return true;
+
+				break;
+
+			case WLAN_EID_RSN:
+				if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4))
+					return true;
+				break;
+
+			default:
+				break;
+			}
+
+			i += (pIE->length + 2);
+		}
+
+		return false;
+	} else {
+		return false;
+	}
+}
+
+int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps, u8 bwmode)
+{
+	unsigned char bit_offset;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (!(pmlmeinfo->HT_enable))
+		return _FAIL;
+
+	bit_offset = (bwmode & CHANNEL_WIDTH_40) ? 6 : 5;
+
+	if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & (0x1 << bit_offset))
+		return _SUCCESS;
+	else
+		return _FAIL;
+}
+
+unsigned char get_highest_rate_idx(u32 mask)
+{
+	int i;
+	unsigned char rate_idx = 0;
+
+	for (i = 31; i >= 0; i--) {
+		if (mask & BIT(i)) {
+			rate_idx = i;
+			break;
+		}
+	}
+
+	return rate_idx;
+}
+
+void Update_RA_Entry(struct adapter *padapter, struct sta_info *psta)
+{
+	rtw_hal_update_ra_mask(psta, 0);
+}
+
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta)
+{
+	/* rate adaptive */
+	Update_RA_Entry(padapter, psta);
+}
+
+static u32 get_realtek_assoc_AP_vender(struct ndis_80211_var_ie *pIE)
+{
+	u32 Vender = HT_IOT_PEER_REALTEK;
+
+	if (pIE->length >= 5) {
+		if (pIE->data[4] == 1)
+			/* if (pIE->data[5] & RT_HT_CAP_USE_LONG_PREAMBLE) */
+			/* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_LONG_PREAMBLE; */
+			if (pIE->data[5] & RT_HT_CAP_USE_92SE)
+				/* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE; */
+				Vender = HT_IOT_PEER_REALTEK_92SE;
+
+		if (pIE->data[5] & RT_HT_CAP_USE_SOFTAP)
+			Vender = HT_IOT_PEER_REALTEK_SOFTAP;
+
+		if (pIE->data[4] == 2) {
+			if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_BCUT)
+				Vender = HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP;
+
+			if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT)
+				Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP;
+		}
+	}
+
+	return Vender;
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+	unsigned int	i;
+	struct ndis_80211_var_ie *pIE;
+
+	for (i = sizeof(struct ndis_802_11_fix_ie); i < len;) {
+		pIE = (struct ndis_80211_var_ie *)(pframe + i);
+
+		switch (pIE->element_id) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (!memcmp(pIE->data, ARTHEROS_OUI2, 3)))
+				return HT_IOT_PEER_ATHEROS;
+			else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
+				 (!memcmp(pIE->data, BROADCOM_OUI2, 3)) ||
+				 (!memcmp(pIE->data, BROADCOM_OUI3, 3)))
+				return HT_IOT_PEER_BROADCOM;
+			else if (!memcmp(pIE->data, MARVELL_OUI, 3))
+				return HT_IOT_PEER_MARVELL;
+			else if (!memcmp(pIE->data, RALINK_OUI, 3))
+				return HT_IOT_PEER_RALINK;
+			else if (!memcmp(pIE->data, CISCO_OUI, 3))
+				return HT_IOT_PEER_CISCO;
+			else if (!memcmp(pIE->data, REALTEK_OUI, 3))
+				return get_realtek_assoc_AP_vender(pIE);
+			else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3))
+				return HT_IOT_PEER_AIRGO;
+			else
+				break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->length + 2);
+	}
+
+	return HT_IOT_PEER_UNKNOWN;
+}
+
+void update_IOT_info(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pmlmeinfo->assoc_AP_vendor) {
+	case HT_IOT_PEER_MARVELL:
+		pmlmeinfo->turboMode_cts2self = 1;
+		pmlmeinfo->turboMode_rtsen = 0;
+		break;
+
+	case HT_IOT_PEER_RALINK:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		/* disable high power */
+		Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+		break;
+	case HT_IOT_PEER_REALTEK:
+		/* rtw_write16(padapter, 0x4cc, 0xffff); */
+		/* rtw_write16(padapter, 0x546, 0x01c0); */
+		/* disable high power */
+		Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+		break;
+	default:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		break;
+	}
+}
+
+void update_capinfo(struct adapter *Adapter, u16 updateCap)
+{
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	bool		ShortPreamble;
+
+	/*  Check preamble mode, 2005.01.06, by rcnjko. */
+	/*  Mark to update preamble value forever, 2008.03.18 by lanhsin */
+	/* if (pMgntInfo->RegPreambleMode == PREAMBLE_AUTO) */
+	{
+		if (updateCap & cShortPreamble) {
+			/*  Short Preamble */
+			if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /*  PREAMBLE_LONG or PREAMBLE_AUTO */
+				ShortPreamble = true;
+				pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
+				rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			}
+		} else {
+			/*  Long Preamble */
+			if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) { /*  PREAMBLE_SHORT or PREAMBLE_AUTO */
+				ShortPreamble = false;
+				pmlmeinfo->preamble_mode = PREAMBLE_LONG;
+				rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			}
+		}
+	}
+
+	if (updateCap & cIBSS) {
+		/* Filen: See 802.11-2007 p.91 */
+		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+	} else {
+		/* Filen: See 802.11-2007 p.90 */
+		if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N)) {
+			pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+		} else if (pmlmeext->cur_wireless_mode & (WIRELESS_11G)) {
+			if ((updateCap & cShortSlotTime) /* && (!(pMgntInfo->pHTInfo->RT2RT_HT_Mode & RT_HT_CAP_USE_LONG_PREAMBLE)) */)
+				/*  Short Slot Time */
+				pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+			else
+				/*  Long Slot Time */
+				pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+		} else {
+			/* B Mode */
+			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+		}
+	}
+
+	rtw_hal_set_hwreg(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime);
+}
+
+void update_wireless_mode(struct adapter *padapter)
+{
+	int network_type = 0;
+	u32 SIFS_Timer;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+	unsigned char *rate = cur_network->supported_rates;
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+		pmlmeinfo->HT_enable = 1;
+
+	if (pmlmeinfo->HT_enable)
+		network_type = WIRELESS_11_24N;
+
+	if (rtw_is_cckratesonly_included(rate))
+		network_type |= WIRELESS_11B;
+	else if (rtw_is_cckrates_included(rate))
+		network_type |= WIRELESS_11BG;
+	else
+		network_type |= WIRELESS_11G;
+
+	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+
+	SIFS_Timer = 0x0a0a0808; /* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
+													/* change this value if having IOT issues. */
+
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS,  (u8 *)&SIFS_Timer);
+
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WIRELESS_MODE,  (u8 *)&(pmlmeext->cur_wireless_mode));
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void update_sta_basic_rate(struct sta_info *psta, u8 wireless_mode)
+{
+	if (is_supported_tx_cck(wireless_mode)) {
+		/*  Only B, B/G, and B/G/N AP could use CCK rate */
+		memcpy(psta->bssrateset, rtw_basic_rate_cck, 4);
+		psta->bssratelen = 4;
+	} else {
+		memcpy(psta->bssrateset, rtw_basic_rate_ofdm, 3);
+		psta->bssratelen = 3;
+	}
+}
+
+int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+{
+	unsigned int	ie_len;
+	struct ndis_80211_var_ie *pIE;
+	int	supportRateNum = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, WLAN_EID_SUPP_RATES, &ie_len, var_ie_len);
+	if (!pIE)
+		return _FAIL;
+	if (ie_len > sizeof(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates))
+		return _FAIL;
+
+	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
+	supportRateNum = ie_len;
+
+	pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, WLAN_EID_EXT_SUPP_RATES, &ie_len, var_ie_len);
+	if (pIE && (ie_len <= sizeof(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates) - supportRateNum))
+		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+
+	return _SUCCESS;
+}
+
+void process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+	struct sta_info *psta;
+	u16 tid, param;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct ADDBA_request *preq = (struct ADDBA_request *)paddba_req;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (psta) {
+		param = le16_to_cpu(preq->BA_para_set);
+		tid = (param>>2)&0x0f;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[tid];
+
+		preorder_ctrl->indicate_seq = 0xffff;
+
+		preorder_ctrl->enable = pmlmeinfo->accept_addba_req;
+	}
+}
+
+void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+	u8 *pIE;
+	__le32 *pbuf;
+
+	pIE = pframe + sizeof(struct ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1));
+
+	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
+
+	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
+}
+
+void correct_TSF(struct adapter *padapter, struct mlme_ext_priv *pmlmeext)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, NULL);
+}
+
+void adaptive_early_32k(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+	int i;
+	u8 *pIE;
+	__le32 *pbuf;
+	u64 tsf = 0;
+	u32 delay_ms;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmlmeext->bcn_cnt++;
+
+	pIE = pframe + sizeof(struct ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	tsf = le32_to_cpu(*(pbuf+1));
+	tsf = tsf << 32;
+	tsf |= le32_to_cpu(*pbuf);
+
+	/* delay = (timestamp mod 1024*100)/1000 (unit: ms) */
+	/* delay_ms = do_div(tsf, (pmlmeinfo->bcn_interval*1024))/1000; */
+	delay_ms = do_div(tsf, (pmlmeinfo->bcn_interval*1024));
+	delay_ms = delay_ms/1000;
+
+	if (delay_ms >= 8)
+		pmlmeext->bcn_delay_cnt[8]++;
+		/* pmlmeext->bcn_delay_ratio[8] = (pmlmeext->bcn_delay_cnt[8] * 100) /pmlmeext->bcn_cnt; */
+	else
+		pmlmeext->bcn_delay_cnt[delay_ms]++;
+		/* pmlmeext->bcn_delay_ratio[delay_ms] = (pmlmeext->bcn_delay_cnt[delay_ms] * 100) /pmlmeext->bcn_cnt; */
+
+/*
+
+	for (i = 0; i<9; i++)
+	{
+			pmlmeext->bcn_delay_cnt[i] , i, pmlmeext->bcn_delay_ratio[i]);
+	}
+*/
+
+	/* dump for  adaptive_early_32k */
+	if (pmlmeext->bcn_cnt > 100 && (pmlmeext->adaptive_tsf_done == true)) {
+		u8 ratio_20_delay, ratio_80_delay;
+		u8 DrvBcnEarly, DrvBcnTimeOut;
+
+		ratio_20_delay = 0;
+		ratio_80_delay = 0;
+		DrvBcnEarly = 0xff;
+		DrvBcnTimeOut = 0xff;
+
+		for (i = 0; i < 9; i++) {
+			pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i] * 100) / pmlmeext->bcn_cnt;
+
+			ratio_20_delay += pmlmeext->bcn_delay_ratio[i];
+			ratio_80_delay += pmlmeext->bcn_delay_ratio[i];
+
+			if (ratio_20_delay > 20 && DrvBcnEarly == 0xff)
+				DrvBcnEarly = i;
+
+			if (ratio_80_delay > 80 && DrvBcnTimeOut == 0xff)
+				DrvBcnTimeOut = i;
+
+			/* reset adaptive_early_32k cnt */
+			pmlmeext->bcn_delay_cnt[i] = 0;
+			pmlmeext->bcn_delay_ratio[i] = 0;
+		}
+
+		pmlmeext->DrvBcnEarly = DrvBcnEarly;
+		pmlmeext->DrvBcnTimeOut = DrvBcnTimeOut;
+
+		pmlmeext->bcn_cnt = 0;
+	}
+}
+
+void rtw_alloc_macid(struct adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN))
+		return;
+
+	if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+		psta->mac_id = NUM_STA;
+		return;
+	}
+
+	spin_lock_bh(&pdvobj->lock);
+	for (i = 0; i < NUM_STA; i++) {
+		if (pdvobj->macid[i] == false) {
+			pdvobj->macid[i]  = true;
+			break;
+		}
+	}
+	spin_unlock_bh(&pdvobj->lock);
+
+	if (i > (NUM_STA - 1))
+		psta->mac_id = NUM_STA;
+	else
+		psta->mac_id = i;
+}
+
+void rtw_release_macid(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN))
+		return;
+
+	if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN))
+		return;
+
+	spin_lock_bh(&pdvobj->lock);
+	if (psta->mac_id < NUM_STA && psta->mac_id != 1) {
+		if (pdvobj->macid[psta->mac_id] == true) {
+			pdvobj->macid[psta->mac_id] = false;
+			psta->mac_id = NUM_STA;
+		}
+	}
+	spin_unlock_bh(&pdvobj->lock);
+}
+
+/* For 8188E RA */
+u8 rtw_search_max_mac_id(struct adapter *padapter)
+{
+	u8 max_mac_id = 0;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+	int i;
+
+	spin_lock_bh(&pdvobj->lock);
+	for (i = (NUM_STA-1); i >= 0 ; i--) {
+		if (pdvobj->macid[i] == true)
+			break;
+	}
+	max_mac_id = i;
+	spin_unlock_bh(&pdvobj->lock);
+
+	return max_mac_id;
+}
+
+struct adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj)
+{
+	if (get_iface_type(dvobj->padapters[i]) != IFACE_PORT0)
+		return NULL;
+
+	return dvobj->padapters;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_xmit.c b/drivers/staging/rtl8723bs/core/rtw_xmit.c
new file mode 100644
index 000000000..a22512633
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_xmit.c
@@ -0,0 +1,2563 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+	INIT_LIST_HEAD(&ptxservq->tx_pending);
+	INIT_LIST_HEAD(&ptxservq->sta_pending.queue);
+	spin_lock_init(&ptxservq->sta_pending.lock);
+	ptxservq->qcnt = 0;
+}
+
+void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{
+	memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv));
+
+	spin_lock_init(&psta_xmitpriv->lock);
+
+	_init_txservq(&psta_xmitpriv->be_q);
+	_init_txservq(&psta_xmitpriv->bk_q);
+	_init_txservq(&psta_xmitpriv->vi_q);
+	_init_txservq(&psta_xmitpriv->vo_q);
+	INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
+	INIT_LIST_HEAD(&psta_xmitpriv->apsd);
+}
+
+s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter)
+{
+	int i;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxframe;
+	signed int	res = _SUCCESS;
+
+	spin_lock_init(&pxmitpriv->lock);
+	spin_lock_init(&pxmitpriv->lock_sctx);
+	init_completion(&pxmitpriv->xmit_comp);
+	init_completion(&pxmitpriv->terminate_xmitthread_comp);
+
+	/*
+	 * Please insert all the queue initializaiton using _rtw_init_queue below
+	 */
+
+	pxmitpriv->adapter = padapter;
+
+	INIT_LIST_HEAD(&pxmitpriv->be_pending.queue);
+	spin_lock_init(&pxmitpriv->be_pending.lock);
+	INIT_LIST_HEAD(&pxmitpriv->bk_pending.queue);
+	spin_lock_init(&pxmitpriv->bk_pending.lock);
+	INIT_LIST_HEAD(&pxmitpriv->vi_pending.queue);
+	spin_lock_init(&pxmitpriv->vi_pending.lock);
+	INIT_LIST_HEAD(&pxmitpriv->vo_pending.queue);
+	spin_lock_init(&pxmitpriv->vo_pending.lock);
+	INIT_LIST_HEAD(&pxmitpriv->bm_pending.queue);
+	spin_lock_init(&pxmitpriv->bm_pending.lock);
+
+	INIT_LIST_HEAD(&pxmitpriv->free_xmit_queue.queue);
+	spin_lock_init(&pxmitpriv->free_xmit_queue.lock);
+
+	/*
+	 * Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
+	 * and initialize free_xmit_frame below.
+	 * Please also apply  free_txobj to link_up all the xmit_frames...
+	 */
+
+	pxmitpriv->pallocated_frame_buf = vzalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+	if (!pxmitpriv->pallocated_frame_buf) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
+
+	pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&pxframe->list);
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		list_add_tail(&pxframe->list,
+			      &pxmitpriv->free_xmit_queue.queue);
+
+		pxframe++;
+	}
+
+	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+
+	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+
+	/* init xmit_buf */
+	INIT_LIST_HEAD(&pxmitpriv->free_xmitbuf_queue.queue);
+	spin_lock_init(&pxmitpriv->free_xmitbuf_queue.lock);
+	INIT_LIST_HEAD(&pxmitpriv->pending_xmitbuf_queue.queue);
+	spin_lock_init(&pxmitpriv->pending_xmitbuf_queue.lock);
+
+	pxmitpriv->pallocated_xmitbuf = vzalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (!pxmitpriv->pallocated_xmitbuf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->buf_tag = XMITBUF_DATA;
+
+		/* Tx buf allocation may fail sometimes, so sleep and retry. */
+		res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+		if (res == _FAIL) {
+			msleep(10);
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+			if (res == _FAIL)
+				goto exit;
+		}
+
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+
+		pxmitbuf->flags = XMIT_VO_QUEUE;
+
+		list_add_tail(&pxmitbuf->list,
+			      &pxmitpriv->free_xmitbuf_queue.queue);
+		#ifdef DBG_XMIT_BUF
+		pxmitbuf->no = i;
+		#endif
+
+		pxmitbuf++;
+	}
+
+	pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+
+	/* init xframe_ext queue,  the same count as extbuf  */
+	INIT_LIST_HEAD(&pxmitpriv->free_xframe_ext_queue.queue);
+	spin_lock_init(&pxmitpriv->free_xframe_ext_queue.lock);
+
+	pxmitpriv->xframe_ext_alloc_addr = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
+
+	if (!pxmitpriv->xframe_ext_alloc_addr) {
+		pxmitpriv->xframe_ext = NULL;
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
+	pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
+
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		INIT_LIST_HEAD(&pxframe->list);
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		pxframe->ext_tag = 1;
+
+		list_add_tail(&pxframe->list,
+			      &pxmitpriv->free_xframe_ext_queue.queue);
+
+		pxframe++;
+	}
+	pxmitpriv->free_xframe_ext_cnt = NR_XMIT_EXTBUFF;
+
+	/*  Init xmit extension buff */
+	INIT_LIST_HEAD(&pxmitpriv->free_xmit_extbuf_queue.queue);
+	spin_lock_init(&pxmitpriv->free_xmit_extbuf_queue.lock);
+
+	pxmitpriv->pallocated_xmit_extbuf = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (!pxmitpriv->pallocated_xmit_extbuf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->buf_tag = XMITBUF_MGNT;
+
+		res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ, true);
+		if (res == _FAIL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMIT_EXTBUF_SZ;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+
+		list_add_tail(&pxmitbuf->list,
+			      &pxmitpriv->free_xmit_extbuf_queue.queue);
+		#ifdef DBG_XMIT_BUF_EXT
+		pxmitbuf->no = i;
+		#endif
+		pxmitbuf++;
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = NR_XMIT_EXTBUFF;
+
+	for (i = 0; i < CMDBUF_MAX; i++) {
+		pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
+		if (pxmitbuf) {
+			INIT_LIST_HEAD(&pxmitbuf->list);
+
+			pxmitbuf->priv_data = NULL;
+			pxmitbuf->padapter = padapter;
+			pxmitbuf->buf_tag = XMITBUF_CMD;
+
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true);
+			if (res == _FAIL) {
+				res = _FAIL;
+				goto exit;
+			}
+
+			pxmitbuf->phead = pxmitbuf->pbuf;
+			pxmitbuf->pend = pxmitbuf->pbuf + MAX_CMDBUF_SZ;
+			pxmitbuf->len = 0;
+			pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+			pxmitbuf->alloc_sz = MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ;
+		}
+	}
+
+	res = rtw_alloc_hwxmits(padapter);
+	if (res == _FAIL)
+		goto exit;
+	rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+
+	for (i = 0; i < 4; i++)
+		pxmitpriv->wmm_para_seq[i] = i;
+
+	pxmitpriv->ack_tx = false;
+	mutex_init(&pxmitpriv->ack_tx_mutex);
+	rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
+
+	rtw_hal_init_xmit_priv(padapter);
+
+exit:
+	return res;
+}
+
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+	int i;
+	struct adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame	*pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	rtw_hal_free_xmit_priv(padapter);
+
+	if (!pxmitpriv->pxmit_frame_buf)
+		return;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		rtw_os_xmit_complete(padapter, pxmitframe);
+
+		pxmitframe++;
+	}
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+
+		pxmitbuf++;
+	}
+
+	vfree(pxmitpriv->pallocated_frame_buf);
+	vfree(pxmitpriv->pallocated_xmitbuf);
+
+	/* free xframe_ext queue,  the same count as extbuf  */
+	pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
+	if (pxmitframe) {
+		for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+			rtw_os_xmit_complete(padapter, pxmitframe);
+			pxmitframe++;
+		}
+	}
+
+	vfree(pxmitpriv->xframe_ext_alloc_addr);
+
+	/*  free xmit extension buff */
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ), true);
+
+		pxmitbuf++;
+	}
+
+	vfree(pxmitpriv->pallocated_xmit_extbuf);
+
+	for (i = 0; i < CMDBUF_MAX; i++) {
+		pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
+		if (pxmitbuf)
+			rtw_os_xmit_resource_free(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true);
+	}
+
+	rtw_free_hwxmits(padapter);
+
+	mutex_destroy(&pxmitpriv->ack_tx_mutex);
+}
+
+u8 query_ra_short_GI(struct sta_info *psta)
+{
+	u8 sgi = false, sgi_20m = false, sgi_40m = false;
+
+	sgi_20m = psta->htpriv.sgi_20m;
+	sgi_40m = psta->htpriv.sgi_40m;
+
+	switch (psta->bw_mode) {
+	case CHANNEL_WIDTH_40:
+		sgi = sgi_40m;
+		break;
+	case CHANNEL_WIDTH_20:
+	default:
+		sgi = sgi_20m;
+		break;
+	}
+
+	return sgi;
+}
+
+static void update_attrib_vcs_info(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u32 sz;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	/* struct sta_info *psta = pattrib->psta; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (pattrib->nr_frags != 1)
+		sz = padapter->xmitpriv.frag_len;
+	else /* no frag */
+		sz = pattrib->last_txcmdsz;
+
+	/*  (1) RTS_Threshold is compared to the MPDU, not MSDU. */
+	/*  (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame. */
+	/* Other fragments are protected by previous fragment. */
+	/* So we only need to check the length of first fragment. */
+	if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec) {
+		if (sz > padapter->registrypriv.rts_thresh) {
+			pattrib->vcs_mode = RTS_CTS;
+		} else {
+			if (pattrib->rtsen)
+				pattrib->vcs_mode = RTS_CTS;
+			else if (pattrib->cts2self)
+				pattrib->vcs_mode = CTS_TO_SELF;
+			else
+				pattrib->vcs_mode = NONE_VCS;
+		}
+	} else {
+		while (true) {
+			/* IOT action */
+			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en == true) &&
+				(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+				pattrib->vcs_mode = CTS_TO_SELF;
+				break;
+			}
+
+			/* check ERP protection */
+			if (pattrib->rtsen || pattrib->cts2self) {
+				if (pattrib->rtsen)
+					pattrib->vcs_mode = RTS_CTS;
+				else if (pattrib->cts2self)
+					pattrib->vcs_mode = CTS_TO_SELF;
+
+				break;
+			}
+
+			/* check HT op mode */
+			if (pattrib->ht_en) {
+				u8 HTOpMode = pmlmeinfo->HT_protection;
+
+				if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
+					(!pmlmeext->cur_bwmode && HTOpMode == 3)) {
+					pattrib->vcs_mode = RTS_CTS;
+					break;
+				}
+			}
+
+			/* check rts */
+			if (sz > padapter->registrypriv.rts_thresh) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			/* to do list: check MIMO power save condition. */
+
+			/* check AMPDU aggregation for TXOP */
+			if (pattrib->ampdu_en == true) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			pattrib->vcs_mode = NONE_VCS;
+			break;
+		}
+	}
+
+	/* for debug : force driver control vrtl_carrier_sense. */
+	if (padapter->driver_vcs_en == 1)
+		pattrib->vcs_mode = padapter->driver_vcs_type;
+}
+
+static void update_attrib_phy_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+
+	pattrib->rtsen = psta->rtsen;
+	pattrib->cts2self = psta->cts2self;
+
+	pattrib->mdata = 0;
+	pattrib->eosp = 0;
+	pattrib->triggered = 0;
+	pattrib->ampdu_spacing = 0;
+
+	/* qos_en, ht_en, init rate, , bw, ch_offset, sgi */
+	pattrib->qos_en = psta->qos_option;
+
+	pattrib->raid = psta->raid;
+
+	pattrib->bwmode = min(mlmeext->cur_bwmode, psta->bw_mode);
+
+	pattrib->sgi = query_ra_short_GI(psta);
+
+	pattrib->ldpc = psta->ldpc;
+	pattrib->stbc = psta->stbc;
+
+	pattrib->ht_en = psta->htpriv.ht_option;
+	pattrib->ch_offset = psta->htpriv.ch_offset;
+	pattrib->ampdu_en = false;
+
+	if (padapter->driver_ampdu_spacing != 0xFF) /* driver control AMPDU Density for peer sta's rx */
+		pattrib->ampdu_spacing = padapter->driver_ampdu_spacing;
+	else
+		pattrib->ampdu_spacing = psta->htpriv.rx_ampdu_min_spacing;
+
+	pattrib->retry_ctrl = false;
+}
+
+static s32 update_attrib_sec_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	signed int res = _SUCCESS;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	signed int bmcast = IS_MCAST(pattrib->ra);
+
+	memset(pattrib->dot118021x_UncstKey.skey,  0, 16);
+	memset(pattrib->dot11tkiptxmickey.skey,  0, 16);
+	pattrib->mac_id = psta->mac_id;
+
+	if (psta->ieee8021x_blocked == true) {
+		pattrib->encrypt = 0;
+
+		if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+			res = _FAIL;
+			goto exit;
+		}
+	} else {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+		switch (psecuritypriv->dot11AuthAlgrthm) {
+		case dot11AuthAlgrthm_Open:
+		case dot11AuthAlgrthm_Shared:
+		case dot11AuthAlgrthm_Auto:
+			pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+			break;
+		case dot11AuthAlgrthm_8021X:
+			if (bmcast)
+				pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
+			else
+				pattrib->key_idx = 0;
+			break;
+		default:
+			pattrib->key_idx = 0;
+			break;
+		}
+
+		/* For WPS 1.0 WEP, driver should not encrypt EAPOL Packet for WPS handshake. */
+		if (((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) && (pattrib->ether_type == 0x888e))
+			pattrib->encrypt = _NO_PRIVACY_;
+	}
+
+	switch (pattrib->encrypt) {
+	case _WEP40_:
+	case _WEP104_:
+		pattrib->iv_len = 4;
+		pattrib->icv_len = 4;
+		WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		break;
+
+	case _TKIP_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 4;
+
+		if (psecuritypriv->busetkipkey == _FAIL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		else
+			TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+
+		memcpy(pattrib->dot11tkiptxmickey.skey, psta->dot11tkiptxmickey.skey, 16);
+
+		break;
+
+	case _AES_:
+
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 8;
+
+		if (bmcast)
+			AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		else
+			AES_IV(pattrib->iv, psta->dot11txpn, 0);
+
+		break;
+
+	default:
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+		break;
+	}
+
+	if (pattrib->encrypt > 0)
+		memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+
+	if (pattrib->encrypt &&
+		((padapter->securitypriv.sw_encrypt) || (!psecuritypriv->hw_decrypted)))
+		pattrib->bswenc = true;
+	else
+		pattrib->bswenc = false;
+
+exit:
+
+	return res;
+}
+
+u8 qos_acm(u8 acm_mask, u8 priority)
+{
+	switch (priority) {
+	case 0:
+	case 3:
+		if (acm_mask & BIT(1))
+			priority = 1;
+		break;
+	case 1:
+	case 2:
+		break;
+	case 4:
+	case 5:
+		if (acm_mask & BIT(2))
+			priority = 0;
+		break;
+	case 6:
+	case 7:
+		if (acm_mask & BIT(3))
+			priority = 5;
+		break;
+	default:
+		break;
+	}
+
+	return priority;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	s32 UserPriority = 0;
+
+	_rtw_open_pktfile(ppktfile->pkt, ppktfile);
+	_rtw_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	/*  get UserPriority from IP hdr */
+	if (pattrib->ether_type == 0x0800) {
+		_rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+		UserPriority = ip_hdr.tos >> 5;
+	}
+	pattrib->priority = UserPriority;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct pkt_attrib *pattrib)
+{
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	signed int bmcast;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	signed int res = _SUCCESS;
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, (u8 *)&etherhdr, ETH_HLEN);
+
+	pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
+	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	}
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	if (pattrib->ether_type == ETH_P_IP) {
+		/*  The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */
+		/*  to prevent DHCP protocol fail */
+
+		u8 tmp[24];
+
+		_rtw_pktfile_read(&pktfile, &tmp[0], 24);
+
+		pattrib->dhcp_pkt = 0;
+		if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
+			if (pattrib->ether_type == ETH_P_IP) {/*  IP header */
+				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+					((tmp[21] == 67) && (tmp[23] == 68))) {
+					/*  68 : UDP BOOTP client */
+					/*  67 : UDP BOOTP server */
+					pattrib->dhcp_pkt = 1;
+				}
+			}
+		}
+
+		/* for parsing ICMP pakcets */
+		{
+			struct iphdr *piphdr = (struct iphdr *)tmp;
+
+			pattrib->icmp_pkt = 0;
+			if (piphdr->protocol == 0x1) /*  protocol type in ip header 0x1 is ICMP */
+				pattrib->icmp_pkt = 1;
+		}
+	} else if (pattrib->ether_type == 0x888e) {
+		netdev_dbg(padapter->pnetdev, "send eapol packet\n");
+	}
+
+	if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+		rtw_set_scan_deny(padapter, 3000);
+
+	/*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
+	if (pattrib->icmp_pkt == 1)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1);
+	else if (pattrib->dhcp_pkt == 1)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+
+	bmcast = IS_MCAST(pattrib->ra);
+
+	/*  get sta_info */
+	if (bmcast) {
+		psta = rtw_get_bcmc_stainfo(padapter);
+	} else {
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		if (!psta)	{ /*  if we cannot get psta => drop the pkt */
+			res = _FAIL;
+			goto exit;
+		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) {
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (!psta) {
+		/*  if we cannot get psta => drop the pkt */
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED))
+		return _FAIL;
+
+	/* TODO:_lock */
+	if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	update_attrib_phy_info(padapter, pattrib, psta);
+
+	pattrib->psta = psta;
+	/* TODO:_unlock */
+
+	pattrib->pctrl = 0;
+
+	pattrib->ack_policy = 0;
+	/*  get ether_hdr_len */
+	pattrib->pkt_hdrlen = ETH_HLEN;/* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */
+
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = WIFI_DATA_TYPE;
+	pattrib->priority = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) {
+		if (pattrib->qos_en)
+			set_qos(&pktfile, pattrib);
+	} else {
+		if (pqospriv->qos_option) {
+			set_qos(&pktfile, pattrib);
+
+			if (pmlmepriv->acm_mask != 0)
+				pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
+		}
+	}
+
+	/* pattrib->priority = 5; force to used VI queue, for testing */
+
+exit:
+	return res;
+}
+
+static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	signed int			curfragnum, length;
+	u8 *pframe, *payload, mic[8];
+	struct mic_data micdata;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+	u8 hw_hdr_offset = 0;
+	signed int bmcst = IS_MCAST(pattrib->ra);
+
+	hw_hdr_offset = TXDESC_OFFSET;
+
+	if (pattrib->encrypt == _TKIP_) {
+		/* encode mic code */
+		{
+			u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+			pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+			if (bmcst) {
+				if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16))
+					return _FAIL;
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
+			} else {
+				if (!memcmp(&pattrib->dot11tkiptxmickey.skey[0], null_key, 16))
+					return _FAIL;
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, &pattrib->dot11tkiptxmickey.skey[0]);
+			}
+
+			if (pframe[1]&1) {   /* ToDS == 1 */
+				rtw_secmicappend(&micdata, &pframe[16], 6);  /* DA */
+				if (pframe[1]&2)  /* From Ds == 1 */
+					rtw_secmicappend(&micdata, &pframe[24], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+			} else {	/* ToDS == 0 */
+				rtw_secmicappend(&micdata, &pframe[4], 6);   /* DA */
+				if (pframe[1]&2)  /* From Ds == 1 */
+					rtw_secmicappend(&micdata, &pframe[16], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+			}
+
+			if (pattrib->qos_en)
+				priority[0] = (u8)pxmitframe->attrib.priority;
+
+			rtw_secmicappend(&micdata, &priority[0], 4);
+
+			payload = pframe;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				payload = (u8 *)round_up((SIZE_PTR)(payload), 4);
+				payload = payload+pattrib->hdrlen+pattrib->iv_len;
+
+				if ((curfragnum+1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload+length;
+				} else {
+					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload+length+pattrib->icv_len;
+				}
+			}
+			rtw_secgetmic(&micdata, &mic[0]);
+			/* add mic code  and add the mic code length in last_txcmdsz */
+
+			memcpy(payload, &mic[0], 8);
+			pattrib->last_txcmdsz += 8;
+			}
+	}
+	return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+
+	if (pattrib->bswenc) {
+		switch (pattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _TKIP_:
+			rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _AES_:
+			rtw_aes_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		default:
+				break;
+		}
+	}
+
+	return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	u8 qos_option = false;
+	signed int res = _SUCCESS;
+	__le16 *fctrl = &pwlanhdr->frame_control;
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+
+	SetFrameSubType(fctrl, pattrib->subtype);
+
+	if (pattrib->subtype & WIFI_DATA_TYPE) {
+		if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true) {
+			/* to_ds = 1, fr_ds = 0; */
+
+			{
+				/*  1.Data transfer to AP */
+				/*  2.Arp pkt will relayed by AP */
+				SetToDs(fctrl);
+				memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+				memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+				memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			}
+
+			if (pqospriv->qos_option)
+				qos_option = true;
+		} else if (check_fwstate(pmlmepriv,  WIFI_AP_STATE) == true) {
+			/* to_ds = 0, fr_ds = 1; */
+			SetFrDs(fctrl);
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+
+			if (pattrib->qos_en)
+				qos_option = true;
+		} else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+			if (pattrib->qos_en)
+				qos_option = true;
+		} else {
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (pattrib->mdata)
+			SetMData(fctrl);
+
+		if (pattrib->encrypt)
+			SetPrivacy(fctrl);
+
+		if (qos_option) {
+			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+			if (pattrib->priority)
+				SetPriority(qc, pattrib->priority);
+
+			SetEOSP(qc, pattrib->eosp);
+
+			SetAckpolicy(qc, pattrib->ack_policy);
+		}
+
+		/* TODO: fill HT Control Field */
+
+		/* Update Seq Num will be handled by f/w */
+		{
+			struct sta_info *psta;
+
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+			if (pattrib->psta != psta)
+				return _FAIL;
+
+			if (!psta)
+				return _FAIL;
+
+			if (!(psta->state & _FW_LINKED))
+				return _FAIL;
+
+			if (psta) {
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+				pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+
+				SetSeqNum(hdr, pattrib->seqnum);
+
+				/* check if enable ampdu */
+				if (pattrib->ht_en && psta->htpriv.ampdu_enable)
+					if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+						pattrib->ampdu_en = true;
+
+				/* re-check if enable ampdu by BA_starting_seqctrl */
+				if (pattrib->ampdu_en == true) {
+					u16 tx_seq;
+
+					tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+
+					/* check BA_starting_seqctrl */
+					if (SN_LESS(pattrib->seqnum, tx_seq)) {
+						pattrib->ampdu_en = false;/* AGG BK */
+					} else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
+
+						pattrib->ampdu_en = true;/* AGG EN */
+					} else {
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
+						pattrib->ampdu_en = true;/* AGG EN */
+					}
+				}
+			}
+		}
+	} else {
+	}
+
+exit:
+	return res;
+}
+
+s32 rtw_txframes_pending(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return ((!list_empty(&pxmitpriv->be_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->bk_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->vi_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->vo_pending.queue)));
+}
+
+/*
+ * Calculate wlan 802.11 packet MAX size from pkt_attrib
+ * This function doesn't consider fragment case
+ */
+u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
+{
+	u32 len = 0;
+
+	len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
+	len += SNAP_SIZE + sizeof(u16); /*  LLC */
+	len += pattrib->pktlen;
+	if (pattrib->encrypt == _TKIP_)
+		len += 8; /*  MIC */
+	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
+
+	return len;
+}
+
+/*
+ * This sub-routine will perform all the following:
+ * 1. remove 802.3 header.
+ * 2. create wlan_header, based on the info in pxmitframe
+ * 3. append sta's iv/ext-iv
+ * 4. append LLC
+ * 5. move frag chunk from pframe to pxmitframe->mem
+ * 6. apply sw-encrypt, if necessary.
+ */
+s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+
+	SIZE_PTR addr;
+
+	u8 *pframe, *mem_start;
+	u8 hw_hdr_offset;
+
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+
+	u8 *pbuf_start;
+
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _SUCCESS;
+
+	if (!pxmitframe->buf_addr)
+		return _FAIL;
+
+	pbuf_start = pxmitframe->buf_addr;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	mem_start = pbuf_start +	hw_hdr_offset;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
+
+	while (1) {
+		llc_sz = 0;
+
+		mpdu_len = frg_len;
+
+		pframe = mem_start;
+
+		SetMFrag(mem_start);
+
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+
+		/* adding icv, if necessary... */
+		if (pattrib->iv_len) {
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+			pframe += pattrib->iv_len;
+
+			mpdu_len -= pattrib->iv_len;
+		}
+
+		if (frg_inx == 0) {
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc))
+			mpdu_len -= pattrib->icv_len;
+
+		if (bmcst) {
+			/*  don't do fragment to broadcast/multicast packets */
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+		} else {
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+		}
+
+		pframe += mem_sz;
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			memcpy(pframe, pattrib->icv, pattrib->icv_len);
+			pframe += pattrib->icv_len;
+		}
+
+		frg_inx++;
+
+		if (bmcst || (rtw_endofpktfile(&pktfile) == true)) {
+			pattrib->nr_frags = frg_inx;
+
+			pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz:0) +
+					((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+
+			ClearMFrag(mem_start);
+
+			break;
+		}
+
+		addr = (SIZE_PTR)(pframe);
+
+		mem_start = (unsigned char *)round_up(addr, 4) + hw_hdr_offset;
+		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	if (bmcst == false)
+		update_attrib_vcs_info(padapter, pxmitframe);
+	else
+		pattrib->vcs_mode = NONE_VCS;
+
+exit:
+	return res;
+}
+
+/* broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption */
+s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe)
+{
+	u8 *pframe, *mem_start = NULL, *tmp_buf = NULL;
+	u8 subtype;
+	struct sta_info *psta = NULL;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	u8 *BIP_AAD = NULL;
+	u8 *MGMT_body = NULL;
+
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ieee80211_hdr	*pwlanhdr;
+	u8 MME[_MME_IE_LENGTH_];
+	u32 ori_len;
+
+	mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	ori_len = BIP_AAD_SIZE+pattrib->pktlen;
+	tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
+	subtype = GetFrameSubType(pframe); /* bit(7)~bit(2) */
+
+	if (!BIP_AAD)
+		return _FAIL;
+
+	spin_lock_bh(&padapter->security_key_mutex);
+
+	/* only support station mode */
+	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED))
+		goto xmitframe_coalesce_success;
+
+	/* IGTK key is not install, it may not support 802.11w */
+	if (!padapter->securitypriv.binstallBIPkey)
+		goto xmitframe_coalesce_success;
+
+	/* station mode doesn't need TX BIP, just ready the code */
+	if (bmcst) {
+		int frame_body_len;
+		u8 mic[16];
+
+		memset(MME, 0, 18);
+
+		/* other types doesn't need the BIP */
+		if (GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC)
+			goto xmitframe_coalesce_fail;
+
+		MGMT_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+		pframe += pattrib->pktlen;
+
+		/* octent 0 and 1 is key index , BIP keyid is 4 or 5, LSB only need octent 0 */
+		MME[0] = padapter->securitypriv.dot11wBIPKeyid;
+		/* copy packet number */
+		memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6);
+		/* increase the packet number */
+		pmlmeext->mgnt_80211w_IPN++;
+
+		/* add MME IE with MIC all zero, MME string doesn't include element id and length */
+		pframe = rtw_set_ie(pframe, WLAN_EID_MMIE, 16,
+				    MME, &pattrib->pktlen);
+		pattrib->last_txcmdsz = pattrib->pktlen;
+		/*  total frame length - header length */
+		frame_body_len = pattrib->pktlen - sizeof(struct ieee80211_hdr_3addr);
+
+		/* conscruct AAD, copy frame control field */
+		memcpy(BIP_AAD, &pwlanhdr->frame_control, 2);
+		ClearRetry(BIP_AAD);
+		ClearPwrMgt(BIP_AAD);
+		ClearMData(BIP_AAD);
+		/* conscruct AAD, copy address 1 to address 3 */
+		memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
+		/* copy management fram body */
+		memcpy(BIP_AAD+BIP_AAD_SIZE, MGMT_body, frame_body_len);
+		/* calculate mic */
+		if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
+			, BIP_AAD, BIP_AAD_SIZE+frame_body_len, mic))
+			goto xmitframe_coalesce_fail;
+
+		/* copy right BIP mic value, total is 128bits, we use the 0~63 bits */
+		memcpy(pframe-8, mic, 8);
+	} else { /* unicast mgmt frame TX */
+		/* start to encrypt mgmt frame */
+		if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC ||
+			subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION) {
+			if (pattrib->psta)
+				psta = pattrib->psta;
+			else
+				psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+			if (!psta)
+				goto xmitframe_coalesce_fail;
+
+			if (!(psta->state & _FW_LINKED) || !pxmitframe->buf_addr)
+				goto xmitframe_coalesce_fail;
+
+			/* according 802.11-2012 standard, these five types are not robust types */
+			if (subtype == WIFI_ACTION &&
+			(pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED  ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P))
+				goto xmitframe_coalesce_fail;
+			/* before encrypt dump the management packet content */
+			if (pattrib->encrypt > 0)
+				memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+			/* bakeup original management packet */
+			memcpy(tmp_buf, pframe, pattrib->pktlen);
+			/* move to data portion */
+			pframe += pattrib->hdrlen;
+
+			/* 802.11w unicast management packet must be _AES_ */
+			pattrib->iv_len = 8;
+			/* it's MIC of AES */
+			pattrib->icv_len = 8;
+
+			switch (pattrib->encrypt) {
+			case _AES_:
+					/* set AES IV header */
+					AES_IV(pattrib->iv, psta->dot11wtxpn, 0);
+				break;
+			default:
+				goto xmitframe_coalesce_fail;
+			}
+			/* insert iv header into management frame */
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+			pframe += pattrib->iv_len;
+			/* copy mgmt data portion after CCMP header */
+			memcpy(pframe, tmp_buf+pattrib->hdrlen, pattrib->pktlen-pattrib->hdrlen);
+			/* move pframe to end of mgmt pkt */
+			pframe += pattrib->pktlen-pattrib->hdrlen;
+			/* add 8 bytes CCMP IV header to length */
+			pattrib->pktlen += pattrib->iv_len;
+			if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+				memcpy(pframe, pattrib->icv, pattrib->icv_len);
+				pframe += pattrib->icv_len;
+			}
+			/* add 8 bytes MIC */
+			pattrib->pktlen += pattrib->icv_len;
+			/* set final tx command size */
+			pattrib->last_txcmdsz = pattrib->pktlen;
+
+			/* set protected bit must be beofre SW encrypt */
+			SetPrivacy(mem_start);
+			/* software encrypt */
+			xmitframe_swencrypt(padapter, pxmitframe);
+		}
+	}
+
+xmitframe_coalesce_success:
+	spin_unlock_bh(&padapter->security_key_mutex);
+	kfree(BIP_AAD);
+	return _SUCCESS;
+
+xmitframe_coalesce_fail:
+	spin_unlock_bh(&padapter->security_key_mutex);
+	kfree(BIP_AAD);
+	return _FAIL;
+}
+
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ *Organizationally Unique Identifier(OUI), 3 octets,
+ *type, defined by that organization, 2 octets.
+ */
+s32 rtw_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len)
+{
+	uint	protection;
+	u8 *perp;
+	signed int	 erp_len;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	switch (pxmitpriv->vcs_setting) {
+	case DISABLE_VCS:
+		pxmitpriv->vcs = NONE_VCS;
+		break;
+
+	case ENABLE_VCS:
+		break;
+
+	case AUTO_VCS:
+	default:
+		perp = rtw_get_ie(ie, WLAN_EID_ERP_INFO, &erp_len, ie_len);
+		if (!perp) {
+			pxmitpriv->vcs = NONE_VCS;
+		} else {
+			protection = (*(perp + 2)) & BIT(1);
+			if (protection) {
+				if (pregistrypriv->vcs_type == RTS_CTS)
+					pxmitpriv->vcs = RTS_CTS;
+				else
+					pxmitpriv->vcs = CTS_TO_SELF;
+			} else {
+				pxmitpriv->vcs = NONE_VCS;
+			}
+		}
+
+		break;
+	}
+}
+
+void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz)
+{
+	struct sta_info *psta = NULL;
+	struct stainfo_stats *pstats = NULL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 pkt_num = 1;
+
+	if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
+		pkt_num = pxmitframe->agg_num;
+
+		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pkt_num;
+
+		pxmitpriv->tx_pkts += pkt_num;
+
+		pxmitpriv->tx_bytes += sz;
+
+		psta = pxmitframe->attrib.psta;
+		if (psta) {
+			pstats = &psta->sta_stats;
+
+			pstats->tx_pkts += pkt_num;
+
+			pstats->tx_bytes += sz;
+		}
+	}
+}
+
+static struct xmit_buf *__rtw_alloc_cmd_xmitbuf(struct xmit_priv *pxmitpriv,
+		enum cmdbuf_type buf_type)
+{
+	struct xmit_buf *pxmitbuf =  NULL;
+
+	pxmitbuf = &pxmitpriv->pcmd_xmitbuf[buf_type];
+	if (pxmitbuf) {
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+
+		if (pxmitbuf->sctx)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+	}
+
+	return pxmitbuf;
+}
+
+struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv,
+		enum cmdbuf_type buf_type)
+{
+	struct xmit_frame		*pcmdframe;
+	struct xmit_buf		*pxmitbuf;
+
+	pcmdframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (!pcmdframe)
+		return NULL;
+
+	pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type);
+	if (!pxmitbuf) {
+		rtw_free_xmitframe(pxmitpriv, pcmdframe);
+		return NULL;
+	}
+
+	pcmdframe->frame_tag = MGNT_FRAMETAG;
+
+	pcmdframe->pxmitbuf = pxmitbuf;
+
+	pcmdframe->buf_addr = pxmitbuf->pbuf;
+
+	pxmitbuf->priv_data = pcmdframe;
+
+	return pcmdframe;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	spin_lock_irqsave(&pfree_queue->lock, irqL);
+
+	if (list_empty(&pfree_queue->queue)) {
+		pxmitbuf = NULL;
+	} else {
+		phead = get_list_head(pfree_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+		list_del_init(&pxmitbuf->list);
+	}
+
+	if (pxmitbuf) {
+		pxmitpriv->free_xmit_extbuf_cnt--;
+
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 1;
+
+		if (pxmitbuf->sctx)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+	}
+
+	spin_unlock_irqrestore(&pfree_queue->lock, irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	unsigned long irqL;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	if (!pxmitbuf)
+		return _FAIL;
+
+	spin_lock_irqsave(&pfree_queue->lock, irqL);
+
+	list_del_init(&pxmitbuf->list);
+
+	list_add_tail(&pxmitbuf->list, get_list_head(pfree_queue));
+	pxmitpriv->free_xmit_extbuf_cnt++;
+
+	spin_unlock_irqrestore(&pfree_queue->lock, irqL);
+
+	return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
+
+	if (list_empty(&pfree_xmitbuf_queue->queue)) {
+		pxmitbuf = NULL;
+	} else {
+		phead = get_list_head(pfree_xmitbuf_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+		list_del_init(&pxmitbuf->list);
+	}
+
+	if (pxmitbuf) {
+		pxmitpriv->free_xmitbuf_cnt--;
+
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+
+		if (pxmitbuf->sctx)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+	}
+
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	unsigned long irqL;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	if (!pxmitbuf)
+		return _FAIL;
+
+	if (pxmitbuf->sctx)
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+
+	if (pxmitbuf->buf_tag == XMITBUF_CMD) {
+	} else if (pxmitbuf->buf_tag == XMITBUF_MGNT) {
+		rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+	} else {
+		spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
+
+		list_del_init(&pxmitbuf->list);
+
+		list_add_tail(&pxmitbuf->list,
+			      get_list_head(pfree_xmitbuf_queue));
+
+		pxmitpriv->free_xmitbuf_cnt++;
+		spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+	}
+	return _SUCCESS;
+}
+
+static void rtw_init_xmitframe(struct xmit_frame *pxframe)
+{
+	if (pxframe) { /* default value setting */
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
+
+		pxframe->frame_tag = DATA_FRAMETAG;
+
+		pxframe->pg_num = 1;
+		pxframe->agg_num = 1;
+		pxframe->ack_report = 0;
+	}
+}
+
+/*
+ * Calling context:
+ * 1. OS_TXENTRY
+ * 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+ *
+ * If we turn on USE_RXTHREAD, then, no need for critical section.
+ * Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+ *
+ * Must be very, very cautious...
+ */
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
+{
+	/*
+	 *	Please remember to use all the osdep_service api,
+	 *	and lock/unlock or _enter/_exit critical to protect
+	 *	pfree_xmit_queue
+	 */
+
+	struct xmit_frame *pxframe = NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+	spin_lock_bh(&pfree_xmit_queue->lock);
+
+	if (list_empty(&pfree_xmit_queue->queue)) {
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = get_next(phead);
+
+		pxframe = container_of(plist, struct xmit_frame, list);
+
+		list_del_init(&pxframe->list);
+		pxmitpriv->free_xmitframe_cnt--;
+	}
+
+	spin_unlock_bh(&pfree_xmit_queue->lock);
+
+	rtw_init_xmitframe(pxframe);
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame *pxframe = NULL;
+	struct list_head *plist, *phead;
+	struct __queue *queue = &pxmitpriv->free_xframe_ext_queue;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->queue)) {
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+		pxframe = container_of(plist, struct xmit_frame, list);
+
+		list_del_init(&pxframe->list);
+		pxmitpriv->free_xframe_ext_cnt--;
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	rtw_init_xmitframe(pxframe);
+
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame *pxframe = NULL;
+	u8 *alloc_addr;
+
+	alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4);
+
+	if (!alloc_addr)
+		goto exit;
+
+	pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4);
+	pxframe->alloc_addr = alloc_addr;
+
+	pxframe->padapter = pxmitpriv->adapter;
+	pxframe->frame_tag = NULL_FRAMETAG;
+
+	pxframe->pkt = NULL;
+
+	pxframe->buf_addr = NULL;
+	pxframe->pxmitbuf = NULL;
+
+	rtw_init_xmitframe(pxframe);
+
+exit:
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+	struct __queue *queue = NULL;
+	struct adapter *padapter = pxmitpriv->adapter;
+	struct sk_buff *pndis_pkt = NULL;
+
+	if (!pxmitframe)
+		goto exit;
+
+	if (pxmitframe->pkt) {
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	if (pxmitframe->alloc_addr) {
+		kfree(pxmitframe->alloc_addr);
+		goto check_pkt_complete;
+	}
+
+	if (pxmitframe->ext_tag == 0)
+		queue = &pxmitpriv->free_xmit_queue;
+	else if (pxmitframe->ext_tag == 1)
+		queue = &pxmitpriv->free_xframe_ext_queue;
+	else {
+	}
+
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&pxmitframe->list);
+	list_add_tail(&pxmitframe->list, get_list_head(queue));
+	if (pxmitframe->ext_tag == 0)
+		pxmitpriv->free_xmitframe_cnt++;
+	else if (pxmitframe->ext_tag == 1)
+		pxmitpriv->free_xframe_ext_cnt++;
+
+	spin_unlock_bh(&queue->lock);
+
+check_pkt_complete:
+
+	if (pndis_pkt)
+		rtw_os_pkt_complete(padapter, pndis_pkt);
+
+exit:
+	return _SUCCESS;
+}
+
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue)
+{
+	struct list_head *plist, *phead, *tmp;
+	struct	xmit_frame	*pxmitframe;
+
+	spin_lock_bh(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	list_for_each_safe(plist, tmp, phead) {
+		pxmitframe = list_entry(plist, struct xmit_frame, list);
+
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+	}
+	spin_unlock_bh(&pframequeue->lock);
+}
+
+s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL)
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, signed int up, u8 *ac)
+{
+	struct tx_servq *ptxservq = NULL;
+
+	switch (up) {
+	case 1:
+	case 2:
+		ptxservq = &psta->sta_xmitpriv.bk_q;
+		*(ac) = 3;
+		break;
+
+	case 4:
+	case 5:
+		ptxservq = &psta->sta_xmitpriv.vi_q;
+		*(ac) = 1;
+		break;
+
+	case 6:
+	case 7:
+		ptxservq = &psta->sta_xmitpriv.vo_q;
+		*(ac) = 0;
+		break;
+
+	case 0:
+	case 3:
+	default:
+		ptxservq = &psta->sta_xmitpriv.be_q;
+		*(ac) = 2;
+	break;
+	}
+
+	return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 ac_index;
+	struct sta_info *psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct hw_xmit	*phwxmits =  padapter->xmitpriv.hwxmits;
+	signed int res = _SUCCESS;
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta)
+		return _FAIL;
+
+	if (!psta) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED))
+		return _FAIL;
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	if (list_empty(&ptxservq->tx_pending))
+		list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+
+	list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+
+exit:
+
+	return res;
+}
+
+s32 rtw_alloc_hwxmits(struct adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+
+	pxmitpriv->hwxmits = NULL;
+
+	pxmitpriv->hwxmits = rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry);
+
+	if (!pxmitpriv->hwxmits)
+		return _FAIL;
+
+	hwxmits = pxmitpriv->hwxmits;
+
+	if (pxmitpriv->hwxmit_entry == 5) {
+		hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
+
+		hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
+
+		hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
+
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+
+		hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
+	} else if (pxmitpriv->hwxmit_entry == 4) {
+		hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+
+		hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+
+		hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+	} else {
+	}
+
+	return _SUCCESS;
+}
+
+void rtw_free_hwxmits(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	kfree(pxmitpriv->hwxmits);
+}
+
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, signed int entry)
+{
+	signed int i;
+
+	for (i = 0; i < entry; i++, phwxmit++)
+		phwxmit->accnt = 0;
+}
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	switch (pattrib->qsel) {
+	case 0:
+	case 3:
+		addr = BE_QUEUE_INX;
+		break;
+	case 1:
+	case 2:
+		addr = BK_QUEUE_INX;
+		break;
+	case 4:
+	case 5:
+		addr = VI_QUEUE_INX;
+		break;
+	case 6:
+	case 7:
+		addr = VO_QUEUE_INX;
+		break;
+	case 0x10:
+		addr = BCN_QUEUE_INX;
+		break;
+	case 0x11:/* BC/MC in PS (HIQ) */
+		addr = HIGH_QUEUE_INX;
+		break;
+	case 0x12:
+	default:
+		addr = MGT_QUEUE_INX;
+		break;
+	}
+
+	return addr;
+}
+
+static void do_queue_select(struct adapter	*padapter, struct pkt_attrib *pattrib)
+{
+	u8 qsel;
+
+	qsel = pattrib->priority;
+
+	pattrib->qsel = qsel;
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *1	enqueue
+ *0	success, hardware will handle this xmit frame(packet)
+ *<0	fail
+ */
+s32 rtw_xmit(struct adapter *padapter, struct sk_buff **ppkt)
+{
+	static unsigned long start;
+	static u32 drop_cnt;
+
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+
+	s32 res;
+
+	if (start == 0)
+		start = jiffies;
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+
+	if (jiffies_to_msecs(jiffies - start) > 2000) {
+		start = jiffies;
+		drop_cnt = 0;
+	}
+
+	if (!pxmitframe) {
+		drop_cnt++;
+		return -1;
+	}
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+	if (res == _FAIL) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+		return -1;
+	}
+	pxmitframe->pkt = *ppkt;
+
+	do_queue_select(padapter, &pxmitframe->attrib);
+
+	spin_lock_bh(&pxmitpriv->lock);
+	if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == true) {
+		spin_unlock_bh(&pxmitpriv->lock);
+		return 1;
+	}
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	/* pre_xmitframe */
+	if (rtw_hal_xmit(padapter, pxmitframe) == false)
+		return 1;
+
+	return 0;
+}
+
+#define RTW_HIQ_FILTER_ALLOW_ALL 0
+#define RTW_HIQ_FILTER_ALLOW_SPECIAL 1
+#define RTW_HIQ_FILTER_DENY_ALL 2
+
+inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe)
+{
+	bool allow = false;
+	struct adapter *adapter = xmitframe->padapter;
+	struct registry_priv *registry = &adapter->registrypriv;
+
+	if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) {
+		struct pkt_attrib *attrib = &xmitframe->attrib;
+
+		if (attrib->ether_type == 0x0806 ||
+		    attrib->ether_type == 0x888e ||
+		    attrib->dhcp_pkt
+		)
+			allow = true;
+
+	} else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL)
+		allow = true;
+	else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) {
+	} else
+		rtw_warn_on(1);
+
+	return allow;
+}
+
+signed int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	signed int ret = false;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	signed int bmcst = IS_MCAST(pattrib->ra);
+	bool update_tim = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false)
+		return ret;
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta)
+		return false;
+
+	if (!psta)
+		return false;
+
+	if (!(psta->state & _FW_LINKED))
+		return false;
+
+	if (pattrib->triggered == 1) {
+		if (bmcst && xmitframe_hiq_filter(pxmitframe))
+			pattrib->qsel = 0x11;/* HIQ */
+
+		return ret;
+	}
+
+	if (bmcst) {
+		spin_lock_bh(&psta->sleep_q.lock);
+
+		if (pstapriv->sta_dz_bitmap) { /* if anyone sta is in ps mode */
+			/* pattrib->qsel = 0x11;HIQ */
+
+			list_del_init(&pxmitframe->list);
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			if (!(pstapriv->tim_bitmap & BIT(0)))
+				update_tim = true;
+
+			pstapriv->tim_bitmap |= BIT(0);
+			pstapriv->sta_dz_bitmap |= BIT(0);
+
+			if (update_tim)
+				update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+			else
+				chk_bmc_sleepq_cmd(padapter);
+
+			ret = true;
+		}
+
+		spin_unlock_bh(&psta->sleep_q.lock);
+
+		return ret;
+	}
+
+	spin_lock_bh(&psta->sleep_q.lock);
+
+	if (psta->state&WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) {
+			list_del_init(&pxmitframe->list);
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			switch (pattrib->priority) {
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk&BIT(0);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi&BIT(0);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo&BIT(0);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be&BIT(0);
+				break;
+			}
+
+			if (wmmps_ac)
+				psta->sleepq_ac_len++;
+
+			if (((psta->has_legacy_ac) && (!wmmps_ac)) || ((!psta->has_legacy_ac) && (wmmps_ac))) {
+				if (!(pstapriv->tim_bitmap & BIT(psta->aid)))
+					update_tim = true;
+
+				pstapriv->tim_bitmap |= BIT(psta->aid);
+
+				if (update_tim)
+					/* update BCN for TIM IE */
+					update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+			}
+
+			ret = true;
+		}
+	}
+
+	spin_unlock_bh(&psta->sleep_q.lock);
+
+	return ret;
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue)
+{
+	signed int ret;
+	struct list_head *plist, *phead, *tmp;
+	u8 ac_index;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib;
+	struct xmit_frame	*pxmitframe;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+
+	phead = get_list_head(pframequeue);
+	list_for_each_safe(plist, tmp, phead) {
+		pxmitframe = list_entry(plist, struct xmit_frame, list);
+
+		pattrib = &pxmitframe->attrib;
+
+		pattrib->triggered = 0;
+
+		ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
+
+		if (true == ret) {
+			ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+			ptxservq->qcnt--;
+			phwxmits[ac_index].accnt--;
+		} else {
+		}
+	}
+}
+
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	struct sta_info *psta_bmc;
+	struct sta_xmit_priv *pstaxmitpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	psta->state |= WIFI_SLEEP_STATE;
+
+	pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&pstaxmitpriv->vo_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&pstaxmitpriv->vi_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&pstaxmitpriv->be_q.tx_pending);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&pstaxmitpriv->bk_q.tx_pending);
+
+	/* for BC/MC Frames */
+	pstaxmitpriv = &psta_bmc->sta_xmitpriv;
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&pstaxmitpriv->be_q.tx_pending);
+
+	spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 update_mask = 0, wmmps_ac = 0;
+	struct sta_info *psta_bmc;
+	struct list_head *xmitframe_plist, *xmitframe_phead, *tmp;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	list_for_each_safe(xmitframe_plist, tmp, xmitframe_phead) {
+		pxmitframe = list_entry(xmitframe_plist, struct xmit_frame,
+					list);
+
+		list_del_init(&pxmitframe->list);
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk&BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi&BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo&BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be&BIT(1);
+			break;
+		}
+
+		psta->sleepq_len--;
+		if (psta->sleepq_len > 0)
+			pxmitframe->attrib.mdata = 1;
+		else
+			pxmitframe->attrib.mdata = 0;
+
+		if (wmmps_ac) {
+			psta->sleepq_ac_len--;
+			if (psta->sleepq_ac_len > 0) {
+				pxmitframe->attrib.mdata = 1;
+				pxmitframe->attrib.eosp = 0;
+			} else {
+				pxmitframe->attrib.mdata = 0;
+				pxmitframe->attrib.eosp = 1;
+			}
+		}
+
+		pxmitframe->attrib.triggered = 1;
+
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+	}
+
+	if (psta->sleepq_len == 0) {
+		if (pstapriv->tim_bitmap & BIT(psta->aid))
+			update_mask = BIT(0);
+
+		pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+		if (psta->state&WIFI_SLEEP_STATE)
+			psta->state ^= WIFI_SLEEP_STATE;
+
+		if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	}
+
+	/* for BC/MC Frames */
+	if (!psta_bmc)
+		goto _exit;
+
+	if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) { /* no any sta in ps mode */
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		list_for_each_safe(xmitframe_plist, tmp, xmitframe_phead) {
+			pxmitframe = list_entry(xmitframe_plist,
+						struct xmit_frame, list);
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+		}
+
+		if (psta_bmc->sleepq_len == 0) {
+			if (pstapriv->tim_bitmap & BIT(0))
+				update_mask |= BIT(1);
+
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+		}
+	}
+
+_exit:
+
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	if (update_mask)
+		update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+}
+
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 wmmps_ac = 0;
+	struct list_head *xmitframe_plist, *xmitframe_phead, *tmp;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	list_for_each_safe(xmitframe_plist, tmp, xmitframe_phead) {
+		pxmitframe = list_entry(xmitframe_plist, struct xmit_frame,
+					list);
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk&BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi&BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo&BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be&BIT(1);
+			break;
+		}
+
+		if (!wmmps_ac)
+			continue;
+
+		list_del_init(&pxmitframe->list);
+
+		psta->sleepq_len--;
+		psta->sleepq_ac_len--;
+
+		if (psta->sleepq_ac_len > 0) {
+			pxmitframe->attrib.mdata = 1;
+			pxmitframe->attrib.eosp = 0;
+		} else {
+			pxmitframe->attrib.mdata = 0;
+			pxmitframe->attrib.eosp = 1;
+		}
+
+		pxmitframe->attrib.triggered = 1;
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) {
+			pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+			update_beacon(padapter, WLAN_EID_TIM, NULL, true);
+		}
+	}
+
+	spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void enqueue_pending_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pqueue;
+	struct adapter *pri_adapter = pxmitpriv->adapter;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+	list_del_init(&pxmitbuf->list);
+	list_add_tail(&pxmitbuf->list, get_list_head(pqueue));
+	spin_unlock_bh(&pqueue->lock);
+
+	complete(&pri_adapter->xmitpriv.xmit_comp);
+}
+
+void enqueue_pending_xmitbuf_to_head(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pqueue;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+	list_del_init(&pxmitbuf->list);
+	list_add(&pxmitbuf->list, get_list_head(pqueue));
+	spin_unlock_bh(&pqueue->lock);
+}
+
+struct xmit_buf *dequeue_pending_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue)) {
+		struct list_head *plist, *phead;
+
+		phead = get_list_head(pqueue);
+		plist = get_next(phead);
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+		list_del_init(&pxmitbuf->list);
+	}
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return pxmitbuf;
+}
+
+struct xmit_buf *dequeue_pending_xmitbuf_under_survey(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue)) {
+		struct list_head *plist, *phead;
+		u8 type;
+
+		phead = get_list_head(pqueue);
+		plist = phead;
+		do {
+			plist = get_next(plist);
+			if (plist == phead)
+				break;
+
+			pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+			type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET);
+
+			if ((type == WIFI_PROBEREQ) ||
+				(type == WIFI_DATA_NULL) ||
+				(type == WIFI_QOS_DATA_NULL)) {
+				list_del_init(&pxmitbuf->list);
+				break;
+			}
+			pxmitbuf = NULL;
+		} while (1);
+	}
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return pxmitbuf;
+}
+
+signed int check_pending_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	struct __queue *pqueue;
+	signed int	ret = false;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue))
+		ret = true;
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return ret;
+}
+
+int rtw_xmit_thread(void *context)
+{
+	s32 err;
+	struct adapter *padapter;
+
+	err = _SUCCESS;
+	padapter = context;
+
+	thread_enter("RTW_XMIT_THREAD");
+
+	do {
+		err = rtw_hal_xmit_thread_handler(padapter);
+		flush_signals_thread();
+	} while (err == _SUCCESS);
+
+	complete(&padapter->xmitpriv.terminate_xmitthread_comp);
+
+	return 0;
+}
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
+{
+	sctx->timeout_ms = timeout_ms;
+	sctx->submit_time = jiffies;
+	init_completion(&sctx->done);
+	sctx->status = RTW_SCTX_SUBMITTED;
+}
+
+int rtw_sctx_wait(struct submit_ctx *sctx)
+{
+	int ret = _FAIL;
+	unsigned long expire;
+	int status = 0;
+
+	expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
+	if (!wait_for_completion_timeout(&sctx->done, expire))
+		/* timeout, do something?? */
+		status = RTW_SCTX_DONE_TIMEOUT;
+	else
+		status = sctx->status;
+
+	if (status == RTW_SCTX_DONE_SUCCESS)
+		ret = _SUCCESS;
+
+	return ret;
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+	if (*sctx) {
+		(*sctx)->status = status;
+		complete(&((*sctx)->done));
+		*sctx = NULL;
+	}
+}
+
+void rtw_sctx_done(struct submit_ctx **sctx)
+{
+	rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
+}
+
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	pack_tx_ops->submit_time = jiffies;
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	return rtw_sctx_wait(pack_tx_ops);
+}
+
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	if (pxmitpriv->ack_tx)
+		rtw_sctx_done_err(&pack_tx_ops, status);
+}
diff --git a/drivers/staging/rtl8723bs/hal/Hal8723BReg.h b/drivers/staging/rtl8723bs/hal/Hal8723BReg.h
new file mode 100644
index 000000000..6bf7933cb
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/Hal8723BReg.h
@@ -0,0 +1,438 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*****************************************************************************
+ *Copyright(c) 2009,  RealTEK Technology Inc. All Right Reserved.
+ *
+ * Module:	__INC_HAL8723BREG_H
+ *
+ *
+ * Note:	1. Define Mac register address and corresponding bit mask map
+ *
+ *
+ * Export:	Constants, macro, functions(API), global variables(None).
+ *
+ * Abbrev:
+ *
+ * History:
+ *	Data		Who		Remark
+ *
+ *****************************************************************************/
+#ifndef __INC_HAL8723BREG_H
+#define __INC_HAL8723BREG_H
+
+/*  */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	0x0000h ~ 0x00FFh	System Configuration */
+/*  */
+/*  */
+#define REG_SYS_ISO_CTRL_8723B			0x0000	/*  2 Byte */
+#define REG_SYS_FUNC_EN_8723B			0x0002	/*  2 Byte */
+#define REG_APS_FSMCO_8723B			0x0004	/*  4 Byte */
+#define REG_SYS_CLKR_8723B			0x0008	/*  2 Byte */
+#define REG_9346CR_8723B			0x000A	/*  2 Byte */
+#define REG_EE_VPD_8723B			0x000C	/*  2 Byte */
+#define REG_AFE_MISC_8723B			0x0010	/*  1 Byte */
+#define REG_SPS0_CTRL_8723B			0x0011	/*  7 Byte */
+#define REG_SPS_OCP_CFG_8723B			0x0018	/*  4 Byte */
+#define REG_RSV_CTRL_8723B			0x001C	/*  3 Byte */
+#define REG_RF_CTRL_8723B			0x001F	/*  1 Byte */
+#define REG_LPLDO_CTRL_8723B			0x0023	/*  1 Byte */
+#define REG_AFE_XTAL_CTRL_8723B			0x0024	/*  4 Byte */
+#define REG_AFE_PLL_CTRL_8723B			0x0028	/*  4 Byte */
+#define REG_MAC_PLL_CTRL_EXT_8723B		0x002c	/*  4 Byte */
+#define REG_EFUSE_CTRL_8723B			0x0030
+#define REG_EFUSE_TEST_8723B			0x0034
+#define REG_PWR_DATA_8723B			0x0038
+#define REG_CAL_TIMER_8723B			0x003C
+#define REG_ACLK_MON_8723B			0x003E
+#define REG_GPIO_MUXCFG_8723B			0x0040
+#define REG_GPIO_IO_SEL_8723B			0x0042
+#define REG_MAC_PINMUX_CFG_8723B		0x0043
+#define REG_GPIO_PIN_CTRL_8723B			0x0044
+#define REG_GPIO_INTM_8723B			0x0048
+#define REG_LEDCFG0_8723B			0x004C
+#define REG_LEDCFG1_8723B			0x004D
+#define REG_LEDCFG2_8723B			0x004E
+#define REG_LEDCFG3_8723B			0x004F
+#define REG_FSIMR_8723B				0x0050
+#define REG_FSISR_8723B				0x0054
+#define REG_HSIMR_8723B				0x0058
+#define REG_HSISR_8723B				0x005c
+#define REG_GPIO_EXT_CTRL			0x0060
+#define REG_MULTI_FUNC_CTRL_8723B		0x0068
+#define REG_GPIO_STATUS_8723B			0x006C
+#define REG_SDIO_CTRL_8723B			0x0070
+#define REG_OPT_CTRL_8723B			0x0074
+#define REG_AFE_XTAL_CTRL_EXT_8723B		0x0078
+#define REG_MCUFWDL_8723B			0x0080
+#define REG_BT_PATCH_STATUS_8723B		0x0088
+#define REG_HIMR0_8723B				0x00B0
+#define REG_HISR0_8723B				0x00B4
+#define REG_HIMR1_8723B				0x00B8
+#define REG_HISR1_8723B				0x00BC
+#define REG_PMC_DBG_CTRL2_8723B			0x00CC
+#define	REG_EFUSE_BURN_GNT_8723B		0x00CF
+#define REG_HPON_FSM_8723B			0x00EC
+#define REG_SYS_CFG_8723B			0x00F0
+#define REG_SYS_CFG1_8723B			0x00FC
+#define REG_ROM_VERSION				0x00FD
+
+/*  */
+/*  */
+/*	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+#define REG_CR_8723B				0x0100
+#define REG_PBP_8723B				0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL_8723B		0x0106
+#define REG_TRXDMA_CTRL_8723B			0x010C
+#define REG_TRXFF_BNDY_8723B			0x0114
+#define REG_TRXFF_STATUS_8723B			0x0118
+#define REG_RXFF_PTR_8723B			0x011C
+#define REG_CPWM_8723B				0x012F
+#define REG_FWIMR_8723B				0x0130
+#define REG_FWISR_8723B				0x0134
+#define REG_FTIMR_8723B				0x0138
+#define REG_PKTBUF_DBG_CTRL_8723B		0x0140
+#define REG_RXPKTBUF_CTRL_8723B			0x0142
+#define REG_PKTBUF_DBG_DATA_L_8723B		0x0144
+#define REG_PKTBUF_DBG_DATA_H_8723B		0x0148
+
+#define REG_TC0_CTRL_8723B			0x0150
+#define REG_TC1_CTRL_8723B			0x0154
+#define REG_TC2_CTRL_8723B			0x0158
+#define REG_TC3_CTRL_8723B			0x015C
+#define REG_TC4_CTRL_8723B			0x0160
+#define REG_TCUNIT_BASE_8723B			0x0164
+#define REG_RSVD3_8723B				0x0168
+#define REG_C2HEVT_MSG_NORMAL_8723B		0x01A0
+#define REG_C2HEVT_CMD_SEQ_88XX			0x01A1
+#define REG_C2HEVT_CMD_CONTENT_88XX		0x01A2
+#define REG_C2HEVT_CMD_LEN_88XX			0x01AE
+#define REG_C2HEVT_CLEAR_8723B			0x01AF
+#define REG_MCUTST_1_8723B			0x01C0
+#define REG_MCUTST_WOWLAN_8723B			0x01C7
+#define REG_FMETHR_8723B			0x01C8
+#define REG_HMETFR_8723B			0x01CC
+#define REG_HMEBOX_0_8723B			0x01D0
+#define REG_HMEBOX_1_8723B			0x01D4
+#define REG_HMEBOX_2_8723B			0x01D8
+#define REG_HMEBOX_3_8723B			0x01DC
+#define REG_LLT_INIT_8723B			0x01E0
+#define REG_HMEBOX_EXT0_8723B			0x01F0
+#define REG_HMEBOX_EXT1_8723B			0x01F4
+#define REG_HMEBOX_EXT2_8723B			0x01F8
+#define REG_HMEBOX_EXT3_8723B			0x01FC
+
+/*  */
+/*  */
+/*	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+#define REG_RQPN_8723B				0x0200
+#define REG_FIFOPAGE_8723B			0x0204
+#define REG_DWBCN0_CTRL_8723B			REG_TDECTRL
+#define REG_TXDMA_OFFSET_CHK_8723B		0x020C
+#define REG_TXDMA_STATUS_8723B			0x0210
+#define REG_RQPN_NPQ_8723B			0x0214
+#define REG_DWBCN1_CTRL_8723B			0x0228
+
+/*  */
+/*  */
+/*	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*  */
+/*  */
+#define REG_RXDMA_AGG_PG_TH_8723B		0x0280
+#define REG_FW_UPD_RDPTR_8723B			0x0284 /*  FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
+#define REG_RXDMA_CONTROL_8723B			0x0286 /*  Control the RX DMA. */
+#define REG_RXPKT_NUM_8723B			0x0287 /*  The number of packets in RXPKTBUF. */
+#define REG_RXDMA_STATUS_8723B			0x0288
+#define REG_RXDMA_PRO_8723B			0x0290
+#define REG_EARLY_MODE_CONTROL_8723B		0x02BC
+#define REG_RSVD5_8723B				0x02F0
+#define REG_RSVD6_8723B				0x02F4
+
+/*  */
+/*  */
+/*	0x0300h ~ 0x03FFh	PCIe */
+/*  */
+/*  */
+#define	REG_PCIE_CTRL_REG_8723B			0x0300
+#define	REG_INT_MIG_8723B			0x0304	/*  Interrupt Migration */
+#define	REG_BCNQ_DESA_8723B			0x0308	/*  TX Beacon Descriptor Address */
+#define	REG_HQ_DESA_8723B			0x0310	/*  TX High Queue Descriptor Address */
+#define	REG_MGQ_DESA_8723B			0x0318	/*  TX Manage Queue Descriptor Address */
+#define	REG_VOQ_DESA_8723B			0x0320	/*  TX VO Queue Descriptor Address */
+#define	REG_VIQ_DESA_8723B			0x0328	/*  TX VI Queue Descriptor Address */
+#define	REG_BEQ_DESA_8723B			0x0330	/*  TX BE Queue Descriptor Address */
+#define	REG_BKQ_DESA_8723B			0x0338	/*  TX BK Queue Descriptor Address */
+#define	REG_RX_DESA_8723B			0x0340	/*  RX Queue	Descriptor Address */
+#define	REG_DBI_WDATA_8723B			0x0348	/*  DBI Write Data */
+#define	REG_DBI_RDATA_8723B			0x034C	/*  DBI Read Data */
+#define	REG_DBI_ADDR_8723B			0x0350	/*  DBI Address */
+#define	REG_DBI_FLAG_8723B			0x0352	/*  DBI Read/Write Flag */
+#define	REG_MDIO_WDATA_8723B			0x0354	/*  MDIO for Write PCIE PHY */
+#define	REG_MDIO_RDATA_8723B			0x0356	/*  MDIO for Reads PCIE PHY */
+#define	REG_MDIO_CTL_8723B			0x0358	/*  MDIO for Control */
+#define	REG_DBG_SEL_8723B			0x0360	/*  Debug Selection Register */
+#define	REG_PCIE_HRPWM_8723B			0x0361	/* PCIe RPWM */
+#define	REG_PCIE_HCPWM_8723B			0x0363	/* PCIe CPWM */
+#define	REG_PCIE_MULTIFET_CTRL_8723B		0x036A	/* PCIE Multi-Fethc Control */
+
+/*  spec version 11 */
+/*  */
+/*  */
+/*	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+#define REG_VOQ_INFORMATION_8723B		0x0400
+#define REG_VIQ_INFORMATION_8723B		0x0404
+#define REG_BEQ_INFORMATION_8723B		0x0408
+#define REG_BKQ_INFORMATION_8723B		0x040C
+#define REG_MGQ_INFORMATION_8723B		0x0410
+#define REG_HGQ_INFORMATION_8723B		0x0414
+#define REG_BCNQ_INFORMATION_8723B		0x0418
+#define REG_TXPKT_EMPTY_8723B			0x041A
+
+#define REG_FWHW_TXQ_CTRL_8723B			0x0420
+#define REG_HWSEQ_CTRL_8723B			0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY_8723B		0x0424
+#define REG_TXPKTBUF_MGQ_BDNY_8723B		0x0425
+#define REG_LIFECTRL_CTRL_8723B			0x0426
+#define REG_MULTI_BCNQ_OFFSET_8723B		0x0427
+#define REG_SPEC_SIFS_8723B			0x0428
+#define REG_RL_8723B				0x042A
+#define REG_TXBF_CTRL_8723B			0x042C
+#define REG_DARFRC_8723B			0x0430
+#define REG_RARFRC_8723B			0x0438
+#define REG_RRSR_8723B				0x0440
+#define REG_ARFR0_8723B				0x0444
+#define REG_ARFR1_8723B				0x044C
+#define REG_CCK_CHECK_8723B			0x0454
+#define REG_AMPDU_MAX_TIME_8723B		0x0456
+#define REG_TXPKTBUF_BCNQ_BDNY1_8723B		0x0457
+
+#define REG_AMPDU_MAX_LENGTH_8723B		0x0458
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD_8723B	0x045D
+#define REG_NDPA_OPT_CTRL_8723B			0x045F
+#define REG_FAST_EDCA_CTRL_8723B		0x0460
+#define REG_RD_RESP_PKT_TH_8723B		0x0463
+#define REG_DATA_SC_8723B			0x0483
+#define REG_TXRPT_START_OFFSET			0x04AC
+#define REG_POWER_STAGE1_8723B			0x04B4
+#define REG_POWER_STAGE2_8723B			0x04B8
+#define REG_AMPDU_BURST_MODE_8723B		0x04BC
+#define REG_PKT_VO_VI_LIFE_TIME_8723B		0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME_8723B		0x04C2
+#define REG_STBC_SETTING_8723B			0x04C4
+#define REG_HT_SINGLE_AMPDU_8723B		0x04C7
+#define REG_PROT_MODE_CTRL_8723B		0x04C8
+#define REG_MAX_AGGR_NUM_8723B			0x04CA
+#define REG_RTS_MAX_AGGR_NUM_8723B		0x04CB
+#define REG_BAR_MODE_CTRL_8723B			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT_8723B		0x04CF
+#define REG_MACID_PKT_DROP0_8723B		0x04D0
+#define REG_MACID_PKT_SLEEP_8723B		0x04D4
+
+/*  */
+/*  */
+/*	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+#define REG_EDCA_VO_PARAM_8723B			0x0500
+#define REG_EDCA_VI_PARAM_8723B			0x0504
+#define REG_EDCA_BE_PARAM_8723B			0x0508
+#define REG_EDCA_BK_PARAM_8723B			0x050C
+#define REG_BCNTCFG_8723B			0x0510
+#define REG_PIFS_8723B				0x0512
+#define REG_RDG_PIFS_8723B			0x0513
+#define REG_SIFS_CTX_8723B			0x0514
+#define REG_SIFS_TRX_8723B			0x0516
+#define REG_AGGR_BREAK_TIME_8723B		0x051A
+#define REG_SLOT_8723B				0x051B
+#define REG_TX_PTCL_CTRL_8723B			0x0520
+#define REG_TXPAUSE_8723B			0x0522
+#define REG_DIS_TXREQ_CLR_8723B			0x0523
+#define REG_RD_CTRL_8723B			0x0524
+/*  */
+/*  Format for offset 540h-542h: */
+/*	[3:0]:	TBTT prohibit setup in unit of 32us. The time for HW getting beacon content before TBTT. */
+/*	[7:4]:	Reserved. */
+/*	[19:8]:	TBTT prohibit hold in unit of 32us. The time for HW holding to send the beacon packet. */
+/*	[23:20]: Reserved */
+/*  Description: */
+/* 	              | */
+/*      |<--Setup--|--Hold------------>| */
+/* 	--------------|---------------------- */
+/*                 | */
+/*                TBTT */
+/*  Note: We cannot update beacon content to HW or send any AC packets during the time between Setup and Hold. */
+/*  Described by Designer Tim and Bruce, 2011-01-14. */
+/*  */
+#define REG_TBTT_PROHIBIT_8723B			0x0540
+#define REG_RD_NAV_NXT_8723B			0x0544
+#define REG_NAV_PROT_LEN_8723B			0x0546
+#define REG_BCN_CTRL_8723B			0x0550
+#define REG_BCN_CTRL_1_8723B			0x0551
+#define REG_MBID_NUM_8723B			0x0552
+#define REG_DUAL_TSF_RST_8723B			0x0553
+#define REG_BCN_INTERVAL_8723B			0x0554
+#define REG_DRVERLYINT_8723B			0x0558
+#define REG_BCNDMATIM_8723B			0x0559
+#define REG_ATIMWND_8723B			0x055A
+#define REG_USTIME_TSF_8723B			0x055C
+#define REG_BCN_MAX_ERR_8723B			0x055D
+#define REG_RXTSF_OFFSET_CCK_8723B		0x055E
+#define REG_RXTSF_OFFSET_OFDM_8723B		0x055F
+#define REG_TSFTR_8723B				0x0560
+#define REG_CTWND_8723B				0x0572
+#define REG_SECONDARY_CCA_CTRL_8723B		0x0577
+#define REG_PSTIMER_8723B			0x0580
+#define REG_TIMER0_8723B			0x0584
+#define REG_TIMER1_8723B			0x0588
+#define REG_ACMHWCTRL_8723B			0x05C0
+#define REG_SCH_TXCMD_8723B			0x05F8
+
+/*	0x0600h ~ 0x07FFh	WMAC Configuration */
+#define REG_MAC_CR_8723B			0x0600
+#define REG_TCR_8723B				0x0604
+#define REG_RCR_8723B				0x0608
+#define REG_RX_PKT_LIMIT_8723B			0x060C
+#define REG_RX_DLK_TIME_8723B			0x060D
+#define REG_RX_DRVINFO_SZ_8723B			0x060F
+
+#define REG_MACID_8723B				0x0610
+#define REG_BSSID_8723B				0x0618
+#define REG_MAR_8723B				0x0620
+#define REG_MBIDCAMCFG_8723B			0x0628
+
+#define REG_USTIME_EDCA_8723B			0x0638
+#define REG_MAC_SPEC_SIFS_8723B			0x063A
+#define REG_RESP_SIFP_CCK_8723B			0x063C
+#define REG_RESP_SIFS_OFDM_8723B		0x063E
+#define REG_ACKTO_8723B				0x0640
+#define REG_CTS2TO_8723B			0x0641
+#define REG_EIFS_8723B				0x0642
+
+#define REG_NAV_UPPER_8723B			0x0652	/*  unit of 128 */
+#define REG_TRXPTCL_CTL_8723B			0x0668
+
+/*  Security */
+#define REG_CAMCMD_8723B			0x0670
+#define REG_CAMWRITE_8723B			0x0674
+#define REG_CAMREAD_8723B			0x0678
+#define REG_CAMDBG_8723B			0x067C
+#define REG_SECCFG_8723B			0x0680
+
+/*  Power */
+#define REG_WOW_CTRL_8723B			0x0690
+#define REG_PS_RX_INFO_8723B			0x0692
+#define REG_UAPSD_TID_8723B			0x0693
+#define REG_WKFMCAM_CMD_8723B			0x0698
+#define REG_WKFMCAM_NUM_8723B			0x0698
+#define REG_WKFMCAM_RWD_8723B			0x069C
+#define REG_RXFLTMAP0_8723B			0x06A0
+#define REG_RXFLTMAP1_8723B			0x06A2
+#define REG_RXFLTMAP2_8723B			0x06A4
+#define REG_BCN_PSR_RPT_8723B			0x06A8
+#define REG_BT_COEX_TABLE_8723B			0x06C0
+#define REG_BFMER0_INFO_8723B			0x06E4
+#define REG_BFMER1_INFO_8723B			0x06EC
+#define REG_CSI_RPT_PARAM_BW20_8723B		0x06F4
+#define REG_CSI_RPT_PARAM_BW40_8723B		0x06F8
+#define REG_CSI_RPT_PARAM_BW80_8723B		0x06FC
+
+/*  Hardware Port 2 */
+#define REG_MACID1_8723B			0x0700
+#define REG_BSSID1_8723B			0x0708
+#define REG_BFMEE_SEL_8723B			0x0714
+#define REG_SND_PTCL_CTRL_8723B			0x0718
+
+/*	Redifine 8192C register definition for compatibility */
+
+/*  TODO: use these definition when using REG_xxx naming rule. */
+/*  NOTE: DO NOT Remove these definition. Use later. */
+#define	EFUSE_CTRL_8723B	REG_EFUSE_CTRL_8723B	/*  E-Fuse Control. */
+#define	EFUSE_TEST_8723B	REG_EFUSE_TEST_8723B	/*  E-Fuse Test. */
+#define	MSR_8723B		(REG_CR_8723B + 2)	/*  Media Status register */
+#define	ISR_8723B		REG_HISR0_8723B
+#define	TSFR_8723B		REG_TSFTR_8723B		/*  Timing Sync Function Timer Register. */
+
+#define PBP_8723B		REG_PBP_8723B
+
+/*  Redifine MACID register, to compatible prior ICs. */
+#define	IDR0_8723B		REG_MACID_8723B		/*  MAC ID Register, Offset 0x0050-0x0053 */
+#define	IDR4_8723B		(REG_MACID_8723B + 4)	/*  MAC ID Register, Offset 0x0054-0x0055 */
+
+/*  9. Security Control Registers	(Offset:) */
+#define	RWCAM_8723B		REG_CAMCMD_8723B	/* IN 8190 Data Sheet is called CAMcmd */
+#define	WCAMI_8723B		REG_CAMWRITE_8723B	/*  Software write CAM input content */
+#define	RCAMO_8723B		REG_CAMREAD_8723B	/*  Software read/write CAM config */
+#define	CAMDBG_8723B		REG_CAMDBG_8723B
+#define	SECR_8723B		REG_SECCFG_8723B	/* Security Configuration Register */
+
+/*        8195 IMR/ISR bits		(offset 0xB0,  8bits) */
+#define	IMR_DISABLED_8723B		0
+/*  IMR DW0(0x00B0-00B3) Bit 0-31 */
+#define	IMR_TIMER2_8723B		BIT31	/*  Timeout interrupt 2 */
+#define	IMR_TIMER1_8723B		BIT30	/*  Timeout interrupt 1 */
+#define	IMR_PSTIMEOUT_8723B		BIT29	/*  Power Save Time Out Interrupt */
+#define	IMR_GTINT4_8723B		BIT28	/*  When GTIMER4 expires, this bit is set to 1 */
+#define	IMR_GTINT3_8723B		BIT27	/*  When GTIMER3 expires, this bit is set to 1 */
+#define	IMR_TXBCN0ERR_8723B		BIT26	/*  Transmit Beacon0 Error */
+#define	IMR_TXBCN0OK_8723B		BIT25	/*  Transmit Beacon0 OK */
+#define	IMR_TSF_BIT32_TOGGLE_8723B	BIT24	/*  TSF Timer BIT32 toggle indication interrupt */
+#define	IMR_BCNDMAINT0_8723B		BIT20	/*  Beacon DMA Interrupt 0 */
+#define	IMR_BCNDERR0_8723B		BIT16	/*  Beacon Queue DMA OK0 */
+#define	IMR_HSISR_IND_ON_INT_8723B	BIT15	/*  HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define	IMR_BCNDMAINT_E_8723B		BIT14	/*  Beacon DMA Interrupt Extension for Win7 */
+#define	IMR_ATIMEND_8723B		BIT12	/*  CTWidnow End or ATIM Window End */
+#define	IMR_C2HCMD_8723B		BIT10	/*  CPU to Host Command INT Status, Write 1 clear */
+#define	IMR_CPWM2_8723B			BIT9	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_CPWM_8723B			BIT8	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_HIGHDOK_8723B		BIT7	/*  High Queue DMA OK */
+#define	IMR_MGNTDOK_8723B		BIT6	/*  Management Queue DMA OK */
+#define	IMR_BKDOK_8723B			BIT5	/*  AC_BK DMA OK */
+#define	IMR_BEDOK_8723B			BIT4	/*  AC_BE DMA OK */
+#define	IMR_VIDOK_8723B			BIT3	/*  AC_VI DMA OK */
+#define	IMR_VODOK_8723B			BIT2	/*  AC_VO DMA OK */
+#define	IMR_RDU_8723B			BIT1	/*  Rx Descriptor Unavailable */
+#define	IMR_ROK_8723B			BIT0	/*  Receive DMA OK */
+
+/*  IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7_8723B		BIT27	/*  Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6_8723B		BIT26	/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5_8723B		BIT25	/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4_8723B		BIT24	/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3_8723B		BIT23	/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2_8723B		BIT22	/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1_8723B		BIT21	/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK7_8723B		BIT20	/*  Beacon Queue DMA OK Interrupt 7 */
+#define	IMR_BCNDOK6_8723B		BIT19	/*  Beacon Queue DMA OK Interrupt 6 */
+#define	IMR_BCNDOK5_8723B		BIT18	/*  Beacon Queue DMA OK Interrupt 5 */
+#define	IMR_BCNDOK4_8723B		BIT17	/*  Beacon Queue DMA OK Interrupt 4 */
+#define	IMR_BCNDOK3_8723B		BIT16	/*  Beacon Queue DMA OK Interrupt 3 */
+#define	IMR_BCNDOK2_8723B		BIT15	/*  Beacon Queue DMA OK Interrupt 2 */
+#define	IMR_BCNDOK1_8723B		BIT14	/*  Beacon Queue DMA OK Interrupt 1 */
+#define	IMR_ATIMEND_E_8723B		BIT13	/*  ATIM Window End Extension for Win7 */
+#define	IMR_TXERR_8723B			BIT11	/*  Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_RXERR_8723B			BIT10	/*  Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_TXFOVW_8723B		BIT9	/*  Transmit FIFO Overflow */
+#define	IMR_RXFOVW_8723B		BIT8	/*  Receive FIFO Overflow */
+
+/* 2 ACMHWCTRL 0x05C0 */
+#define	ACMHW_HWEN_8723B		BIT(0)
+#define	ACMHW_VOQEN_8723B		BIT(1)
+#define	ACMHW_VIQEN_8723B		BIT(2)
+#define	ACMHW_BEQEN_8723B		BIT(3)
+#define	ACMHW_VOQSTATUS_8723B		BIT(5)
+#define	ACMHW_VIQSTATUS_8723B		BIT(6)
+#define	ACMHW_BEQSTATUS_8723B		BIT(7)
+
+/*        8195 (RCR) Receive Configuration Register	(Offset 0x608, 32 bits) */
+#define	RCR_TCPOFLD_EN			BIT25	/*  Enable TCP checksum offload */
+
+#endif /*  #ifndef __INC_HAL8723BREG_H */
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
new file mode 100644
index 000000000..b3d7f50fa
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
@@ -0,0 +1,2666 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "Mp_Precomp.h"
+
+/*  Global variables, these are static variables */
+static struct coex_dm_8723b_1ant GLCoexDm8723b1Ant;
+static struct coex_dm_8723b_1ant *pCoexDm = &GLCoexDm8723b1Ant;
+static struct coex_sta_8723b_1ant GLCoexSta8723b1Ant;
+static struct coex_sta_8723b_1ant *pCoexSta = &GLCoexSta8723b1Ant;
+
+/*  local function proto type if needed */
+/*  local function start with halbtc8723b1ant_ */
+static u8 halbtc8723b1ant_BtRssiState(
+	u8 levelNum, u8 rssiThresh, u8 rssiThresh1
+)
+{
+	s32 btRssi = 0;
+	u8 btRssiState = pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT))
+
+				btRssiState = BTC_RSSI_STATE_HIGH;
+			else
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+		} else {
+			if (btRssi < rssiThresh)
+				btRssiState = BTC_RSSI_STATE_LOW;
+			else
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1)
+			return pCoexSta->preBtRssiState;
+
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT))
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+			else
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+		} else if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (btRssi >= (rssiThresh1 + BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT))
+				btRssiState = BTC_RSSI_STATE_HIGH;
+			else if (btRssi < rssiThresh)
+				btRssiState = BTC_RSSI_STATE_LOW;
+			else
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+		} else {
+			if (btRssi < rssiThresh1)
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+			else
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static void halbtc8723b1ant_UpdateRaMask(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u32 disRateMask
+)
+{
+	pCoexDm->curRaMask = disRateMask;
+
+	if (bForceExec || (pCoexDm->preRaMask != pCoexDm->curRaMask))
+		pBtCoexist->fBtcSet(
+			pBtCoexist,
+			BTC_SET_ACT_UPDATE_RAMASK,
+			&pCoexDm->curRaMask
+		);
+	pCoexDm->preRaMask = pCoexDm->curRaMask;
+}
+
+static void halbtc8723b1ant_AutoRateFallbackRetry(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 type
+)
+{
+	bool bWifiUnderBMode = false;
+
+	pCoexDm->curArfrType = type;
+
+	if (bForceExec || (pCoexDm->preArfrType != pCoexDm->curArfrType)) {
+		switch (pCoexDm->curArfrType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite4Byte(
+				pBtCoexist, 0x430, pCoexDm->backupArfrCnt1
+			);
+			pBtCoexist->fBtcWrite4Byte(
+				pBtCoexist, 0x434, pCoexDm->backupArfrCnt2
+			);
+			break;
+		case 1:
+			pBtCoexist->fBtcGet(
+				pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+			);
+			if (bWifiUnderBMode) {
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x430, 0x0);
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x434, 0x01010101);
+			} else {
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x430, 0x0);
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x434, 0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preArfrType = pCoexDm->curArfrType;
+}
+
+static void halbtc8723b1ant_RetryLimit(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 type
+)
+{
+	pCoexDm->curRetryLimitType = type;
+
+	if (
+		bForceExec ||
+		(pCoexDm->preRetryLimitType != pCoexDm->curRetryLimitType)
+	) {
+		switch (pCoexDm->curRetryLimitType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite2Byte(
+				pBtCoexist, 0x42a, pCoexDm->backupRetryLimit
+			);
+			break;
+		case 1:	/*  retry limit =8 */
+			pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x42a, 0x0808);
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preRetryLimitType = pCoexDm->curRetryLimitType;
+}
+
+static void halbtc8723b1ant_AmpduMaxTime(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 type
+)
+{
+	pCoexDm->curAmpduTimeType = type;
+
+	if (
+		bForceExec || (pCoexDm->preAmpduTimeType != pCoexDm->curAmpduTimeType)
+	) {
+		switch (pCoexDm->curAmpduTimeType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite1Byte(
+				pBtCoexist, 0x456, pCoexDm->backupAmpduMaxTime
+			);
+			break;
+		case 1:	/*  AMPDU timw = 0x38 * 32us */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x456, 0x38);
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preAmpduTimeType = pCoexDm->curAmpduTimeType;
+}
+
+static void halbtc8723b1ant_LimitedTx(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	u8 raMaskType,
+	u8 arfrType,
+	u8 retryLimitType,
+	u8 ampduTimeType
+)
+{
+	switch (raMaskType) {
+	case 0:	/*  normal mode */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x0);
+		break;
+	case 1:	/*  disable cck 1/2 */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x00000003);
+		break;
+	case 2:	/*  disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x0001f1f7);
+		break;
+	default:
+		break;
+	}
+
+	halbtc8723b1ant_AutoRateFallbackRetry(pBtCoexist, bForceExec, arfrType);
+	halbtc8723b1ant_RetryLimit(pBtCoexist, bForceExec, retryLimitType);
+	halbtc8723b1ant_AmpduMaxTime(pBtCoexist, bForceExec, ampduTimeType);
+}
+
+static void halbtc8723b1ant_LimitedRx(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	bool bRejApAggPkt,
+	bool bBtCtrlAggBufSize,
+	u8 aggBufSize
+)
+{
+	bool bRejectRxAgg = bRejApAggPkt;
+	bool bBtCtrlRxAggSize = bBtCtrlAggBufSize;
+	u8 rxAggSize = aggBufSize;
+
+	/*  */
+	/*	Rx Aggregation related setting */
+	/*  */
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejectRxAgg
+	);
+	/*  decide BT control aggregation buf size or not */
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bBtCtrlRxAggSize
+	);
+	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxAggSize);
+	/*  real update aggregation setting */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+
+
+}
+
+static void halbtc8723b1ant_QueryBtInfo(struct btc_coexist *pBtCoexist)
+{
+	u8 H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_MonitorBtCtr(struct btc_coexist *pBtCoexist)
+{
+	u32 regHPTxRx, regLPTxRx, u4Tmp;
+	u32 regHPTx = 0, regHPRx = 0, regLPTx = 0, regLPRx = 0;
+	static u8 NumOfBtCounterChk;
+
+       /* to avoid 0x76e[3] = 1 (WLAN_Act control by PTA) during IPS */
+	/* if (! (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e) & 0x8)) */
+
+	if (pCoexSta->bUnderIps) {
+		pCoexSta->highPriorityTx = 65535;
+		pCoexSta->highPriorityRx = 65535;
+		pCoexSta->lowPriorityTx = 65535;
+		pCoexSta->lowPriorityRx = 65535;
+		return;
+	}
+
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & bMaskLWord;
+	regHPRx = (u4Tmp & bMaskHWord) >> 16;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & bMaskLWord;
+	regLPRx = (u4Tmp & bMaskHWord) >> 16;
+
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	if ((pCoexSta->lowPriorityTx >= 1050) && (!pCoexSta->bC2hBtInquiryPage))
+		pCoexSta->popEventCnt++;
+
+	/*  reset counter */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+
+	if ((regHPTx == 0) && (regHPRx == 0) && (regLPTx == 0) && (regLPRx == 0)) {
+		NumOfBtCounterChk++;
+		if (NumOfBtCounterChk >= 3) {
+			halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+			NumOfBtCounterChk = 0;
+		}
+	}
+}
+
+
+static void halbtc8723b1ant_MonitorWiFiCtr(struct btc_coexist *pBtCoexist)
+{
+	s32	wifiRssi = 0;
+	bool bWifiBusy = false, bWifiUnderBMode = false;
+	static u8 nCCKLockCounter;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+	);
+
+	if (pCoexSta->bUnderIps) {
+		pCoexSta->nCRCOK_CCK = 0;
+		pCoexSta->nCRCOK_11g = 0;
+		pCoexSta->nCRCOK_11n = 0;
+		pCoexSta->nCRCOK_11nAgg = 0;
+
+		pCoexSta->nCRCErr_CCK = 0;
+		pCoexSta->nCRCErr_11g = 0;
+		pCoexSta->nCRCErr_11n = 0;
+		pCoexSta->nCRCErr_11nAgg = 0;
+	} else {
+		pCoexSta->nCRCOK_CCK	= pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xf88);
+		pCoexSta->nCRCOK_11g	= pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf94);
+		pCoexSta->nCRCOK_11n	= pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf90);
+		pCoexSta->nCRCOK_11nAgg = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xfb8);
+
+		pCoexSta->nCRCErr_CCK	 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xf84);
+		pCoexSta->nCRCErr_11g	 = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf96);
+		pCoexSta->nCRCErr_11n	 = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf92);
+		pCoexSta->nCRCErr_11nAgg = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xfba);
+	}
+
+
+	/* reset counter */
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0xf16, 0x1, 0x1);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0xf16, 0x1, 0x0);
+
+	if (bWifiBusy && (wifiRssi >= 30) && !bWifiUnderBMode) {
+		if (
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY)
+		) {
+			if (
+				pCoexSta->nCRCOK_CCK > (
+					pCoexSta->nCRCOK_11g +
+					pCoexSta->nCRCOK_11n +
+					pCoexSta->nCRCOK_11nAgg
+				)
+			) {
+				if (nCCKLockCounter < 5)
+					nCCKLockCounter++;
+			} else {
+				if (nCCKLockCounter > 0)
+					nCCKLockCounter--;
+			}
+
+		} else {
+			if (nCCKLockCounter > 0)
+				nCCKLockCounter--;
+		}
+	} else {
+		if (nCCKLockCounter > 0)
+			nCCKLockCounter--;
+	}
+
+	if (!pCoexSta->bPreCCKLock) {
+
+		if (nCCKLockCounter >= 5)
+			pCoexSta->bCCKLock = true;
+		else
+			pCoexSta->bCCKLock = false;
+	} else {
+		if (nCCKLockCounter == 0)
+			pCoexSta->bCCKLock = false;
+		else
+			pCoexSta->bCCKLock = true;
+	}
+
+	pCoexSta->bPreCCKLock =  pCoexSta->bCCKLock;
+
+
+}
+
+static bool halbtc8723b1ant_IsWifiStatusChanged(struct btc_coexist *pBtCoexist)
+{
+	static bool	bPreWifiBusy, bPreUnder4way, bPreBtHsOn;
+	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected
+	);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way
+	);
+
+	if (bWifiConnected) {
+		if (bWifiBusy != bPreWifiBusy) {
+			bPreWifiBusy = bWifiBusy;
+			return true;
+		}
+
+		if (bUnder4way != bPreUnder4way) {
+			bPreUnder4way = bUnder4way;
+			return true;
+		}
+
+		if (bBtHsOn != bPreBtHsOn) {
+			bPreBtHsOn = bBtHsOn;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b1ant_UpdateBtLinkInfo(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
+	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
+	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
+	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
+	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
+
+	/*  work around for HS mode. */
+	if (bBtHsOn) {
+		pBtLinkInfo->bPanExist = true;
+		pBtLinkInfo->bBtLinkExist = true;
+	}
+
+	/*  check if Sco only */
+	if (
+		pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bScoOnly = true;
+	else
+		pBtLinkInfo->bScoOnly = false;
+
+	/*  check if A2dp only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bA2dpOnly = true;
+	else
+		pBtLinkInfo->bA2dpOnly = false;
+
+	/*  check if Pan only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bPanOnly = true;
+	else
+		pBtLinkInfo->bPanOnly = false;
+
+	/*  check if Hid only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bHidOnly = true;
+	else
+		pBtLinkInfo->bHidOnly = false;
+}
+
+static u8 halbtc8723b1ant_ActionAlgorithm(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+	u8 algorithm = BT_8723B_1ANT_COEX_ALGO_UNDEFINED;
+	u8 numOfDiffProfile = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	if (!pBtLinkInfo->bBtLinkExist)
+		return algorithm;
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANHS;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR;
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+			}
+		} else {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+			} else if (pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP;
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (
+				pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				else
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+			}
+		}
+	} else if (numOfDiffProfile >= 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (!bBtHsOn)
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b1ant_SetSwPenaltyTxRateAdaptive(
+	struct btc_coexist *pBtCoexist, bool bLowPenaltyRa
+)
+{
+	u8 H2C_Parameter[6] = {0};
+
+	H2C_Parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (bLowPenaltyRa) {
+		H2C_Parameter[1] |= BIT0;
+		H2C_Parameter[2] = 0x00;  /* normal rate except MCS7/6/5, OFDM54/48/36 */
+		H2C_Parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		H2C_Parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		H2C_Parameter[5] = 0xf9;	/* MCS5 or OFDM36 */
+	}
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_LowPenaltyRa(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bLowPenaltyRa
+)
+{
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa)
+			return;
+	}
+	halbtc8723b1ant_SetSwPenaltyTxRateAdaptive(
+		pBtCoexist, pCoexDm->bCurLowPenaltyRa
+	);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+static void halbtc8723b1ant_SetCoexTable(
+	struct btc_coexist *pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b1ant_CoexTable(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+		    (pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
+		    (pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+		    (pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc)
+		)
+			return;
+	}
+
+	halbtc8723b1ant_SetCoexTable(
+		pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc
+	);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+static void halbtc8723b1ant_CoexTableWithType(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 type
+)
+{
+	pCoexSta->nCoexTableType = type;
+
+	switch (type) {
+	case 0:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffffff, 0x3
+		);
+		break;
+	case 1:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0x5a5a5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 2:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 3:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0xaaaa5555, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 4:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 5:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x5a5a5a5a, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 6:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0xaaaaaaaa, 0xffffff, 0x3
+		);
+		break;
+	case 7:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffffff, 0x3
+		);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8723b1ant_SetFwIgnoreWlanAct(
+	struct btc_coexist *pBtCoexist, bool bEnable
+)
+{
+	u8 H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0; /* function enable */
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_IgnoreWlanAct(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b1ant_SetLpsRpwm(
+	struct btc_coexist *pBtCoexist, u8 lpsVal, u8 rpwmVal
+)
+{
+	u8 lps = lpsVal;
+	u8 rpwm = rpwmVal;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_LPS_VAL, &lps);
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
+}
+
+static void halbtc8723b1ant_LpsRpwm(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 lpsVal, u8 rpwmVal
+)
+{
+	pCoexDm->curLps = lpsVal;
+	pCoexDm->curRpwm = rpwmVal;
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->preLps == pCoexDm->curLps) &&
+			(pCoexDm->preRpwm == pCoexDm->curRpwm)
+		) {
+			return;
+		}
+	}
+	halbtc8723b1ant_SetLpsRpwm(pBtCoexist, lpsVal, rpwmVal);
+
+	pCoexDm->preLps = pCoexDm->curLps;
+	pCoexDm->preRpwm = pCoexDm->curRpwm;
+}
+
+static void halbtc8723b1ant_SwMechanism(
+	struct btc_coexist *pBtCoexist, bool bLowPenaltyRA
+)
+{
+	halbtc8723b1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+}
+
+static void halbtc8723b1ant_SetAntPath(
+	struct btc_coexist *pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	struct btc_board_info *pBoardInfo = &pBtCoexist->boardInfo;
+	u32 fwVer = 0, u4Tmp = 0, cntBtCalChk = 0;
+	bool bPgExtSwitch = false;
+	bool bUseExtSwitch = false;
+	bool bIsInMpMode = false;
+	u8 H2C_Parameter[2] = {0}, u1Tmp = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_EXT_SWITCH, &bPgExtSwitch);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer); /*  [31:16]=fw ver, [15:0]=fw sub ver */
+
+	if ((fwVer > 0 && fwVer < 0xc0000) || bPgExtSwitch)
+		bUseExtSwitch = true;
+
+	if (bInitHwCfg) {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780); /* WiFi TRx Mask on */
+		pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15); /* BT TRx Mask on */
+
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			H2C_Parameter[0] = 1;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else /*  set grant_bt to high */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+		/* set wlan_act control by PTA */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x39, 0x8, 0x1);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x974, 0xff);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x944, 0x3, 0x3);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x930, 0x77);
+	} else if (bWifiOff) {
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			H2C_Parameter[0] = 1;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else /*  set grant_bt to high */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+		/* set wlan_act to always low */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_IS_IN_MP_MODE, &bIsInMpMode);
+		if (!bIsInMpMode)
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x0); /* BT select s0/s1 is controlled by BT */
+		else
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+
+		/*  0x4c[24:23]= 00, Set Antenna control by BT_RFE_CTRL	BT Vendor 0xac = 0xf002 */
+		u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+		u4Tmp &= ~BIT23;
+		u4Tmp &= ~BIT24;
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+	} else {
+		/* Use H2C to set GNT_BT to LOW */
+		if (fwVer >= 0x180000) {
+			if (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765) != 0) {
+				H2C_Parameter[0] = 0;
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+			}
+		} else {
+			/*  BT calibration check */
+			while (cntBtCalChk <= 20) {
+				u1Tmp = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49d);
+				cntBtCalChk++;
+
+				if (u1Tmp & BIT0)
+					mdelay(50);
+				else
+					break;
+			}
+
+			/*  set grant_bt to PTA */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x0);
+		}
+
+		if (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e) != 0xc)
+			/* set wlan_act control by PTA */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+	}
+
+	if (bUseExtSwitch) {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp &= ~BIT23;
+			u4Tmp |= BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+				/* tell firmware "no antenna inverse" */
+				H2C_Parameter[0] = 0;
+				H2C_Parameter[1] = 1;  /* ext switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			} else {
+				/* tell firmware "antenna inverse" */
+				H2C_Parameter[0] = 1;
+				H2C_Parameter[1] = 1;  /* ext switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			}
+		}
+
+
+		/*  ext switch setting */
+		switch (antPosType) {
+		case BTC_ANT_PATH_WIFI:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			break;
+		}
+
+	} else {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 1, 0x4c[24]= 0  Antenna control by 0x64 */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp |= BIT23;
+			u4Tmp &= ~BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+
+			/* Fix Ext switch Main->S1, Aux->S0 */
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x64, 0x1, 0x0);
+
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+
+				/* tell firmware "no antenna inverse" */
+				H2C_Parameter[0] = 0;
+				H2C_Parameter[1] = 0;  /* internal switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			} else {
+
+				/* tell firmware "antenna inverse" */
+				H2C_Parameter[0] = 1;
+				H2C_Parameter[1] = 0;  /* internal switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			}
+		}
+
+
+		/*  internal switch setting */
+		switch (antPosType) {
+		case BTC_ANT_PATH_WIFI:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x200);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x80);
+			break;
+		}
+	}
+}
+
+static void halbtc8723b1ant_SetFwPstdma(
+	struct btc_coexist *pBtCoexist, u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5
+)
+{
+	u8 H2C_Parameter[5] = {0};
+	u8 realByte1 = byte1, realByte5 = byte5;
+	bool bApEnable = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+
+	if (bApEnable) {
+		if (byte1 & BIT4 && !(byte1 & BIT5)) {
+			realByte1 &= ~BIT4;
+			realByte1 |= BIT5;
+
+			realByte5 |= BIT5;
+			realByte5 &= ~BIT6;
+		}
+	}
+
+	H2C_Parameter[0] = realByte1;
+	H2C_Parameter[1] = byte2;
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = realByte5;
+
+	pCoexDm->psTdmaPara[0] = realByte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = realByte5;
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+
+static void halbtc8723b1ant_PsTdma(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiBusy = false;
+	u8 rssiAdjustVal = 0;
+	u8 psTdmaByte4Val = 0x50, psTdmaByte0Val = 0x51, psTdmaByte3Val =  0x10;
+	s8 nWiFiDurationAdjust = 0x0;
+	/* u32 fwVer = 0; */
+
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma)
+		)
+			return;
+	}
+
+	if (pCoexSta->nScanAPNum <= 5)
+		nWiFiDurationAdjust = 5;
+	else if  (pCoexSta->nScanAPNum >= 40)
+		nWiFiDurationAdjust = -15;
+	else if  (pCoexSta->nScanAPNum >= 20)
+		nWiFiDurationAdjust = -10;
+
+	if (!pCoexSta->bForceLpsOn) { /* only for A2DP-only case 1/2/9/11 */
+		psTdmaByte0Val = 0x61;  /* no null-pkt */
+		psTdmaByte3Val = 0x11; /*  no tx-pause at BT-slot */
+		psTdmaByte4Val = 0x10; /*  0x778 = d/1 toggle */
+	}
+
+
+	if (bTurnOn) {
+		if (pBtLinkInfo->bSlaveRole)
+			psTdmaByte4Val = psTdmaByte4Val | 0x1;  /* 0x778 = 0x1 at wifi slot (no blocking BT Low-Pri pkts) */
+
+
+		switch (type) {
+		default:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x1a, 0x1a, 0x0, psTdmaByte4Val
+			);
+			break;
+		case 1:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x3a + nWiFiDurationAdjust,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 2:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x2d + nWiFiDurationAdjust,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 3:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x1d, 0x1d, 0x0, 0x10
+			);
+			break;
+		case 4:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0
+			);
+			break;
+		case 5:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x15, 0x3, 0x11, 0x10
+			);
+			break;
+		case 6:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x20, 0x3, 0x11, 0x11
+			);
+			break;
+		case 7:
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0
+			);
+			break;
+		case 9:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x21,
+				0x3,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 10:
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+			break;
+		case 11:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x21,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 12:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x0a, 0x0a, 0x0, 0x50
+			);
+			break;
+		case 13:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x12, 0x12, 0x0, 0x10
+			);
+			break;
+		case 14:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x21, 0x3, 0x10, psTdmaByte4Val
+			);
+			break;
+		case 15:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x13, 0xa, 0x3, 0x8, 0x0
+			);
+			break;
+		case 16:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x15, 0x3, 0x10, 0x0
+			);
+			break;
+		case 18:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0
+			);
+			break;
+		case 20:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x3f, 0x03, 0x11, 0x10
+
+			);
+			break;
+		case 21:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x25, 0x03, 0x11, 0x11
+			);
+			break;
+		case 22:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x25, 0x03, 0x11, 0x10
+			);
+			break;
+		case 23:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x18
+			);
+			break;
+		case 24:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x15, 0x3, 0x31, 0x18
+			);
+			break;
+		case 25:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18
+			);
+			break;
+		case 26:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18
+			);
+			break;
+		case 27:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x98
+			);
+			break;
+		case 28:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x69, 0x25, 0x3, 0x31, 0x0
+			);
+			break;
+		case 29:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10
+			);
+			break;
+		case 30:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x30, 0x3, 0x10, 0x10
+			);
+			break;
+		case 31:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x58
+			);
+			break;
+		case 32:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x35, 0x3, 0x11, 0x11
+			);
+			break;
+		case 33:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xa3, 0x25, 0x3, 0x30, 0x90
+			);
+			break;
+		case 34:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x53, 0x1a, 0x1a, 0x0, 0x10
+			);
+			break;
+		case 35:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x63, 0x1a, 0x1a, 0x0, 0x10
+			);
+			break;
+		case 36:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xd3, 0x12, 0x3, 0x14, 0x50
+			);
+			break;
+		case 40: /*  SoftAP only with no sta associated, BT disable , TDMA mode for power saving */
+			/* here softap mode screen off will cost 70-80mA for phone */
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x23, 0x18, 0x00, 0x10, 0x24
+			);
+			break;
+		}
+	} else {
+
+		/*  disable PS tdma */
+		switch (type) {
+		case 8: /* PTA Control */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_PTA, false, false
+			);
+			break;
+		case 0:
+		default:  /* Software control, Antenna at BT side */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_BT, false, false
+			);
+			break;
+		case 9:   /* Software control, Antenna at WiFi side */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_WIFI, false, false
+			);
+			break;
+		}
+	}
+
+	rssiAdjustVal = 0;
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssiAdjustVal
+	);
+
+	/*  update pre state */
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+static bool halbtc8723b1ant_IsCommonAction(struct btc_coexist *pBtCoexist)
+{
+	bool bCommon = false, bWifiConnected = false, bWifiBusy = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (
+		!bWifiConnected &&
+		pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE
+	) {
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE)
+	) {
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE)
+	) {
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE)
+	) {
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(pCoexDm->btStatus != BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE)
+	) {
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else {
+		bCommon = false;
+	}
+
+	return bCommon;
+}
+
+
+static void halbtc8723b1ant_TdmaDurationAdjustForAcl(
+	struct btc_coexist *pBtCoexist, u8 wifiStatus
+)
+{
+	static s32 up, dn, m, n, WaitCount;
+	s32 result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
+	u8 retryCount = 0, btInfoExt;
+
+	if (
+		(wifiStatus == BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN) ||
+		(wifiStatus == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN) ||
+		(wifiStatus == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT)
+	) {
+		if (
+			pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 3 &&
+			pCoexDm->curPsTdma != 9
+		) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+			pCoexDm->psTdmaDuAdjType = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			WaitCount = 0;
+		}
+		return;
+	}
+
+	if (!pCoexDm->bAutoTdmaAdjust) {
+		pCoexDm->bAutoTdmaAdjust = true;
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		pCoexDm->psTdmaDuAdjType = 2;
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* acquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		btInfoExt = pCoexSta->btInfoExt;
+
+		if (pCoexSta->lowPriorityTx > 1050 || pCoexSta->lowPriorityRx > 1250)
+			retryCount++;
+
+		result = 0;
+		WaitCount++;
+
+		if (retryCount == 0) { /*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) { /*  if 連續 n 個2秒 retry count為0, 則調寬WiFi duration */
+				WaitCount = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+			}
+		} else if (retryCount <= 3) { /*  <=3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) { /*  if 連續 2 個2秒 retry count< 3, 則調窄WiFi duration */
+				if (WaitCount <= 2)
+					m++; /*  避免一直在兩個level中來回 */
+				else
+					m = 1;
+
+				if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1;
+			}
+		} else { /* retry count > 3, 只要1次 retry count > 3, 則調窄WiFi duration */
+			if (WaitCount == 1)
+				m++; /*  避免一直在兩個level中來回 */
+			else
+				m = 1;
+
+			if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+				m = 20;
+
+			n = 3 * m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0;
+			result = -1;
+		}
+
+		if (result == -1) {
+			if (
+				BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(btInfoExt) &&
+				((pCoexDm->curPsTdma == 1) || (pCoexDm->curPsTdma == 2))
+			) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 1) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			} else if (pCoexDm->curPsTdma == 2) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 9) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+				pCoexDm->psTdmaDuAdjType = 11;
+			}
+		} else if (result == 1) {
+			if (
+				BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(btInfoExt) &&
+				((pCoexDm->curPsTdma == 1) || (pCoexDm->curPsTdma == 2))
+			) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 11) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 9) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			} else if (pCoexDm->curPsTdma == 2) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+				pCoexDm->psTdmaDuAdjType = 1;
+			}
+		}
+
+		if (
+			pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 9 &&
+			pCoexDm->curPsTdma != 11
+		) /*  recover to previous adjust type */
+			halbtc8723b1ant_PsTdma(
+				pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType
+			);
+	}
+}
+
+static void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(
+	struct btc_coexist *pBtCoexist, bool bNewPsState
+)
+{
+	u8 lpsMode = 0x0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_LPS_MODE, &lpsMode);
+
+	if (lpsMode) {	/*  already under LPS state */
+		if (bNewPsState) {
+			/*  keep state under LPS, do nothing. */
+		} else /*  will leave LPS state, turn off psTdma first */
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+	} else {						/*  NO PS state */
+		if (bNewPsState) /*  will enter LPS state, turn off psTdma first */
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		else {
+			/*  keep state under NO PS state, do nothing. */
+		}
+	}
+}
+
+static void halbtc8723b1ant_PowerSaveState(
+	struct btc_coexist *pBtCoexist, u8 psType, u8 lpsVal, u8 rpwmVal
+)
+{
+	bool bLowPwrDisable = false;
+
+	switch (psType) {
+	case BTC_PS_WIFI_NATIVE:
+		/*  recover to original 32k low power setting */
+		bLowPwrDisable = false;
+		pBtCoexist->fBtcSet(
+			pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable
+		);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		pCoexSta->bForceLpsOn = false;
+		break;
+	case BTC_PS_LPS_ON:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, true);
+		halbtc8723b1ant_LpsRpwm(pBtCoexist, NORMAL_EXEC, lpsVal, rpwmVal);
+		/*  when coex force to enter LPS, do not enter 32k low power. */
+		bLowPwrDisable = true;
+		pBtCoexist->fBtcSet(
+			pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable
+		);
+		/*  power save must executed before psTdma. */
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		pCoexSta->bForceLpsOn = true;
+		break;
+	case BTC_PS_LPS_OFF:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, false);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		pCoexSta->bForceLpsOn = false;
+		break;
+	default:
+		break;
+	}
+}
+
+/*  */
+/*  */
+/*	Software Coex Mechanism start */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	Non-Software Coex Mechanism start */
+/*  */
+/*  */
+static void halbtc8723b1ant_ActionWifiMultiPort(struct btc_coexist *pBtCoexist)
+{
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_ActionHs(struct btc_coexist *pBtCoexist)
+{
+	halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_ActionBtInquiry(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false;
+	bool bApEnable = false;
+	bool bWifiBusy = false;
+	bool bBtBusy = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	if (!bWifiConnected && !pCoexSta->bWiFiIsHighPriTask) {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	} else if (
+		pBtLinkInfo->bScoExist ||
+		pBtLinkInfo->bHidExist ||
+		pBtLinkInfo->bA2dpExist
+	) {
+		/*  SCO/HID/A2DP busy */
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist || bWifiBusy) {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+	}
+}
+
+static void halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+	struct btc_coexist *pBtCoexist, u8 wifiStatus
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	/*  tdma and coex table */
+
+	if (pBtLinkInfo->bScoExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 5);
+	} else { /* HID */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 5);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+	struct btc_coexist *pBtCoexist, u8 wifiStatus
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_BtRssiState(2, 28, 0);
+
+	if ((pCoexSta->lowPriorityRx >= 1000) && (pCoexSta->lowPriorityRx != 65535))
+		pBtLinkInfo->bSlaveRole = true;
+	else
+		pBtLinkInfo->bSlaveRole = false;
+
+	if (pBtLinkInfo->bHidOnly) { /* HID */
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(pBtCoexist, wifiStatus);
+		pCoexDm->bAutoTdmaAdjust = false;
+		return;
+	} else if (pBtLinkInfo->bA2dpOnly) { /* A2DP */
+		if (wifiStatus == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+			pCoexDm->bAutoTdmaAdjust = false;
+		} else {
+			halbtc8723b1ant_TdmaDurationAdjustForAcl(pBtCoexist, wifiStatus);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+			pCoexDm->bAutoTdmaAdjust = true;
+		}
+	} else if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) { /* HID+A2DP */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+		pCoexDm->bAutoTdmaAdjust = false;
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (
+		pBtLinkInfo->bPanOnly ||
+		(pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist)
+	) { /* PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else if (
+		(pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) ||
+		(pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist)
+	) { /* A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else {
+		/* BT no-profile busy (0x9) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnected(struct btc_coexist *pBtCoexist)
+{
+	/*  power save state */
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnectedScan(
+	struct btc_coexist *pBtCoexist
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+		if (pBtLinkInfo->bA2dpExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else if (pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		}
+	} else if (
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+	) {
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+			pBtCoexist, BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN
+		);
+	} else {
+		/* Bryant Add */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(
+	struct btc_coexist *pBtCoexist
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (
+		(pBtLinkInfo->bScoExist) ||
+		(pBtLinkInfo->bHidExist) ||
+		(pBtLinkInfo->bA2dpExist)
+	) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+		if (pBtLinkInfo->bA2dpExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else if (pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		}
+	} else if (
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+	) {
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+			pBtCoexist, BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN
+		);
+	} else {
+		/* Bryant Add */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedSpecialPacket(
+	struct btc_coexist *pBtCoexist
+)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (
+		(pBtLinkInfo->bScoExist) ||
+		(pBtLinkInfo->bHidExist) ||
+		(pBtLinkInfo->bA2dpExist)
+	) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnected(struct btc_coexist *pBtCoexist)
+{
+	bool bWifiBusy = false;
+	bool bScan = false, bLink = false, bRoam = false;
+	bool bUnder4way = false, bApEnable = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+	if (bUnder4way) {
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	if (bScan || bLink || bRoam) {
+		if (bScan)
+			halbtc8723b1ant_ActionWifiConnectedScan(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	/*  power save state */
+	if (
+		!bApEnable &&
+		pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY &&
+		!pBtCoexist->btLinkInfo.bHidOnly
+	) {
+		if (pBtCoexist->btLinkInfo.bA2dpOnly) { /* A2DP */
+			if (!bWifiBusy)
+				halbtc8723b1ant_PowerSaveState(
+					pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0
+				);
+			else { /* busy */
+				if  (pCoexSta->nScanAPNum >= BT_8723B_1ANT_WIFI_NOISY_THRESH)  /* no force LPS, no PS-TDMA, use pure TDMA */
+					halbtc8723b1ant_PowerSaveState(
+						pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0
+					);
+				else
+					halbtc8723b1ant_PowerSaveState(
+						pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x4
+					);
+			}
+		} else if (
+			(!pCoexSta->bPanExist) &&
+			(!pCoexSta->bA2dpExist) &&
+			(!pCoexSta->bHidExist)
+		)
+			halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		else
+			halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+	} else
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (!bWifiBusy) {
+		if (pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE
+			);
+		} else if (
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+		) {
+			halbtc8723b1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+			if ((pCoexSta->highPriorityTx) + (pCoexSta->highPriorityRx) <= 60)
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+			else
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		}
+	} else {
+		if (pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY
+			);
+		} else if (
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+		) {
+			halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY
+			);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+			if ((pCoexSta->highPriorityTx) + (pCoexSta->highPriorityRx) <= 60)
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+			else
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		}
+	}
+}
+
+static void halbtc8723b1ant_RunSwCoexistMechanism(struct btc_coexist *pBtCoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8723b1ant_ActionAlgorithm(pBtCoexist);
+	pCoexDm->curAlgorithm = algorithm;
+
+	if (halbtc8723b1ant_IsCommonAction(pBtCoexist)) {
+
+	} else {
+		switch (pCoexDm->curAlgorithm) {
+		case BT_8723B_1ANT_COEX_ALGO_SCO:
+			/* halbtc8723b1ant_ActionSco(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID:
+			/* halbtc8723b1ant_ActionHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP:
+			/* halbtc8723b1ant_ActionA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
+			/* halbtc8723b1ant_ActionA2dpPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
+			/* halbtc8723b1ant_ActionPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANHS:
+			/* halbtc8723b1ant_ActionPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
+			/* halbtc8723b1ant_ActionPanEdrA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
+			/* halbtc8723b1ant_ActionPanEdrHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			/* halbtc8723b1ant_ActionHidA2dpPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
+			/* halbtc8723b1ant_ActionHidA2dp(pBtCoexist); */
+			break;
+		default:
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b1ant_RunCoexistMechanism(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false, bBtHsOn = false;
+	bool bIncreaseScanDevNum = false;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (pBtCoexist->bStopCoexDm)
+		return;
+
+	if (pCoexSta->bUnderIps)
+		return;
+
+	if (
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+	){
+		bIncreaseScanDevNum = true;
+	}
+
+	pBtCoexist->fBtcSet(
+		pBtCoexist,
+		BTC_SET_BL_INC_SCAN_DEV_NUM,
+		&bIncreaseScanDevNum
+	);
+	pBtCoexist->fBtcGet(
+		pBtCoexist,
+		BTC_GET_BL_WIFI_CONNECTED,
+		&bWifiConnected
+	);
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist,
+		BTC_GET_U4_WIFI_LINK_STATUS,
+		&wifiLinkStatus
+	);
+	numOfWifiLink = wifiLinkStatus >> 16;
+
+	if ((numOfWifiLink >= 2) || (wifiLinkStatus & WIFI_P2P_GO_CONNECTED)) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize);
+
+		if ((pBtLinkInfo->bA2dpExist) && (pCoexSta->bC2hBtInquiryPage))
+			halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+
+		return;
+	}
+
+	if ((pBtLinkInfo->bBtLinkExist) && (bWifiConnected)) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 1, 1, 0, 1);
+
+		if (pBtLinkInfo->bScoExist)
+			halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x5);
+		else
+			halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x8);
+
+		halbtc8723b1ant_SwMechanism(pBtCoexist, true);
+		halbtc8723b1ant_RunSwCoexistMechanism(pBtCoexist);  /* just print debug message */
+	} else {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x5);
+
+		halbtc8723b1ant_SwMechanism(pBtCoexist, false);
+		halbtc8723b1ant_RunSwCoexistMechanism(pBtCoexist); /* just print debug message */
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+
+	if (!bWifiConnected) {
+		bool bScan = false, bLink = false, bRoam = false;
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if (bScan || bLink || bRoam) {
+			if (bScan)
+				halbtc8723b1ant_ActionWifiNotConnectedScan(pBtCoexist);
+			else
+				halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(pBtCoexist);
+		} else
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+	} else /*  wifi LPS/Busy */
+		halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+}
+
+static void halbtc8723b1ant_InitCoexDm(struct btc_coexist *pBtCoexist)
+{
+	/*  force to reset coex mechanism */
+
+	/*  sw all off */
+	halbtc8723b1ant_SwMechanism(pBtCoexist, false);
+
+	/* halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8); */
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	pCoexSta->popEventCnt = 0;
+}
+
+static void halbtc8723b1ant_InitHwConfig(
+	struct btc_coexist *pBtCoexist,
+	bool bBackUp,
+	bool bWifiOnly
+)
+{
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x550, 0x8, 0x1);  /* enable TBTT nterrupt */
+
+	/*  0x790[5:0]= 0x5 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x790, 0x5);
+
+	/*  Enable counter statistics */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x778, 0x1);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x40, 0x20, 0x1);
+
+	/* Antenna config */
+	if (bWifiOnly) {
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_WIFI, true, false);
+		halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 9);
+	} else
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, true, false);
+
+	/*  PTA parameter */
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+	pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8723b1ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8723b1ant_ */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(struct btc_coexist *pBtCoexist)
+{
+	struct btc_board_info *pBoardInfo = &pBtCoexist->boardInfo;
+	u8 u1Tmp = 0x0;
+	u16 u2Tmp = 0x0;
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x67, 0x20);
+
+	/*  enable BB, REG_SYS_FUNC_EN such that we can write 0x948 correctly. */
+	u2Tmp = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x2);
+	pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x2, u2Tmp | BIT0 | BIT1);
+
+	/*  set GRAN_BT = 1 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+	/*  set WLAN_ACT = 0 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	/*  */
+	/*  S0 or S1 setting and Local register setting(By the setting fw can get ant number, S0/S1, ... info) */
+	/*  Local setting bit define */
+	/*	BIT0: "0" for no antenna inverse; "1" for antenna inverse */
+	/*	BIT1: "0" for internal switch; "1" for external switch */
+	/*	BIT2: "0" for one antenna; "1" for two antenna */
+	/*  NOTE: here default all internal switch and 1-antenna ==> BIT1 = 0 and BIT2 = 0 */
+	if (pBtCoexist->chipInterface == BTC_INTF_USB) {
+		/*  fixed at S0 for USB interface */
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		u1Tmp |= 0x1;	/*  antenna inverse */
+		pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0xfe08, u1Tmp);
+
+		pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+	} else {
+		/*  for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (pBoardInfo->singleAntPath == 0) {
+			/*  set to S1 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+		} else if (pBoardInfo->singleAntPath == 1) {
+			/*  set to S0 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			u1Tmp |= 0x1;	/*  antenna inverse */
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+		}
+
+		if (pBtCoexist->chipInterface == BTC_INTF_PCI)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x384, u1Tmp);
+		else if (pBtCoexist->chipInterface == BTC_INTF_SDIO)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x60, u1Tmp);
+	}
+}
+
+void EXhalbtc8723b1ant_InitHwConfig(struct btc_coexist *pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b1ant_InitHwConfig(pBtCoexist, true, bWifiOnly);
+}
+
+void EXhalbtc8723b1ant_InitCoexDm(struct btc_coexist *pBtCoexist)
+{
+	pBtCoexist->bStopCoexDm = false;
+
+	halbtc8723b1ant_InitCoexDm(pBtCoexist);
+
+	halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_IpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl ||	pBtCoexist->bStopCoexDm)
+		return;
+
+	if (type == BTC_IPS_ENTER) {
+		pCoexSta->bUnderIps = true;
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+	} else if (type == BTC_IPS_LEAVE) {
+		pCoexSta->bUnderIps = false;
+
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_LpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (type == BTC_LPS_ENABLE)
+		pCoexSta->bUnderLps = true;
+	else if (type == BTC_LPS_DISABLE)
+		pCoexSta->bUnderLps = false;
+}
+
+void EXhalbtc8723b1ant_ScanNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	bool bWifiConnected = false, bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (type == BTC_SCAN_START) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8);  /* Force antenna setup for no scan result issue */
+		pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+		pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+		pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+
+		pBtCoexist->fBtcGet(
+			pBtCoexist, BTC_GET_U1_AP_NUM, &pCoexSta->nScanAPNum
+		);
+	}
+
+	if (pBtCoexist->btInfo.bBtDisabled)
+		return;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus);
+	numOfWifiLink = wifiLinkStatus >> 16;
+
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(
+			pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize
+		);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (type == BTC_SCAN_START) {
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnectedScan(pBtCoexist);
+		else	/*  wifi is connected */
+			halbtc8723b1ant_ActionWifiConnectedScan(pBtCoexist);
+	} else if (type == BTC_SCAN_FINISH) {
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_ConnectNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	bool bWifiConnected = false, bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (type == BTC_ASSOCIATE_START) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+		 pCoexDm->nArpCnt = 0;
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		/* pCoexDm->nArpCnt = 0; */
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus);
+	numOfWifiLink = wifiLinkStatus >> 16;
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (type == BTC_ASSOCIATE_START) {
+		halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(pBtCoexist);
+	} else if (type == BTC_ASSOCIATE_FINISH) {
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+		if (!bWifiConnected) /*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_MediaStatusNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	u8 H2C_Parameter[3] = {0};
+	u32 wifiBw;
+	u8 wifiCentralChnl;
+	bool bWifiUnderBMode = false;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (type == BTC_MEDIA_CONNECT) {
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode);
+
+		/* Set CCK Tx/Rx high Pri except 11b mode */
+		if (bWifiUnderBMode) {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x00); /* CCK Tx */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x00); /* CCK Rx */
+		} else {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x10); /* CCK Tx */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x10); /* CCK Rx */
+		}
+
+		pCoexDm->backupArfrCnt1 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x430);
+		pCoexDm->backupArfrCnt2 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x434);
+		pCoexDm->backupRetryLimit = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x42a);
+		pCoexDm->backupAmpduMaxTime = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x456);
+	} else {
+		pCoexDm->nArpCnt = 0;
+
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x0); /* CCK Tx */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x0); /* CCK Rx */
+	}
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if ((type == BTC_MEDIA_CONNECT) && (wifiCentralChnl <= 14)) {
+		/* H2C_Parameter[0] = 0x1; */
+		H2C_Parameter[0] = 0x0;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+		if (wifiBw == BTC_WIFI_BW_HT40)
+			H2C_Parameter[2] = 0x30;
+		else
+			H2C_Parameter[2] = 0x20;
+	}
+
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b1ant_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	bool bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (
+		type == BTC_PACKET_DHCP ||
+		type == BTC_PACKET_EAPOL ||
+		type == BTC_PACKET_ARP
+	) {
+		if (type == BTC_PACKET_ARP) {
+			pCoexDm->nArpCnt++;
+
+			if (pCoexDm->nArpCnt >= 10) /*  if APR PKT > 10 after connect, do not go to ActionWifiConnectedSpecialPacket(pBtCoexist) */
+				pCoexSta->bWiFiIsHighPriTask = false;
+			else
+				pCoexSta->bWiFiIsHighPriTask = true;
+		} else {
+			pCoexSta->bWiFiIsHighPriTask = true;
+		}
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+	}
+
+	pCoexSta->specialPktPeriodCnt = 0;
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus
+	);
+	numOfWifiLink = wifiLinkStatus >> 16;
+
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(
+			pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize
+		);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (
+		type == BTC_PACKET_DHCP ||
+		type == BTC_PACKET_EAPOL ||
+		((type == BTC_PACKET_ARP) && (pCoexSta->bWiFiIsHighPriTask))
+	)
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_BtInfoNotify(
+	struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length
+)
+{
+	u8 btInfo = 0;
+	u8 i, rspSource = 0;
+	bool bWifiConnected = false;
+	bool bBtBusy = false;
+
+	pCoexSta->bC2hBtInfoReqSent = false;
+
+	rspSource = tmpBuf[0] & 0xf;
+	if (rspSource >= BT_INFO_SRC_8723B_1ANT_MAX)
+		rspSource = BT_INFO_SRC_8723B_1ANT_WIFI_FW;
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+	}
+
+	if (rspSource != BT_INFO_SRC_8723B_1ANT_WIFI_FW) {
+		pCoexSta->btRetryCnt = pCoexSta->btInfoC2h[rspSource][2] & 0xf;
+
+		if (pCoexSta->btRetryCnt >= 1)
+			pCoexSta->popEventCnt++;
+
+		if (pCoexSta->btInfoC2h[rspSource][2] & 0x20)
+			pCoexSta->bC2hBtPage = true;
+		else
+			pCoexSta->bC2hBtPage = false;
+
+		pCoexSta->btRssi = pCoexSta->btInfoC2h[rspSource][3] * 2 - 90;
+		/* pCoexSta->btInfoC2h[rspSource][3]*2+10; */
+
+		pCoexSta->btInfoExt = pCoexSta->btInfoC2h[rspSource][4];
+
+		pCoexSta->bBtTxRxMask = (pCoexSta->btInfoC2h[rspSource][2] & 0x40);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TX_RX_MASK, &pCoexSta->bBtTxRxMask);
+
+		if (!pCoexSta->bBtTxRxMask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C != 0x15 => Need to switch BT TRx Mask */
+			pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15);
+		}
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if (pCoexSta->btInfoExt & BIT1) {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+			if (bWifiConnected)
+				EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
+			else
+				EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+		}
+
+		if (pCoexSta->btInfoExt & BIT3) {
+			if (!pBtCoexist->bManualControl && !pBtCoexist->bStopCoexDm)
+				halbtc8723b1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, false);
+		} else {
+			/*  BT already NOT ignore Wlan active, do nothing here. */
+		}
+	}
+
+	/*  check BIT2 first ==> check if bt is under inquiry or page scan */
+	if (btInfo & BT_INFO_8723B_1ANT_B_INQ_PAGE)
+		pCoexSta->bC2hBtInquiryPage = true;
+	else
+		pCoexSta->bC2hBtInquiryPage = false;
+
+	/*  set link exist status */
+	if (!(btInfo & BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		pCoexSta->bBtLinkExist = false;
+		pCoexSta->bPanExist = false;
+		pCoexSta->bA2dpExist = false;
+		pCoexSta->bHidExist = false;
+		pCoexSta->bScoExist = false;
+	} else {	/*  connection exists */
+		pCoexSta->bBtLinkExist = true;
+		if (btInfo & BT_INFO_8723B_1ANT_B_FTP)
+			pCoexSta->bPanExist = true;
+		else
+			pCoexSta->bPanExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_A2DP)
+			pCoexSta->bA2dpExist = true;
+		else
+			pCoexSta->bA2dpExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_HID)
+			pCoexSta->bHidExist = true;
+		else
+			pCoexSta->bHidExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_SCO_ESCO)
+			pCoexSta->bScoExist = true;
+		else
+			pCoexSta->bScoExist = false;
+	}
+
+	halbtc8723b1ant_UpdateBtLinkInfo(pBtCoexist);
+
+	btInfo = btInfo & 0x1f;  /* mask profile bit for connect-ilde identification (for CSR case: A2DP idle --> 0x41) */
+
+	if (!(btInfo & BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+	} else if (btInfo == BT_INFO_8723B_1ANT_B_CONNECTION)	{
+		/*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
+	} else if (
+		(btInfo & BT_INFO_8723B_1ANT_B_SCO_ESCO) ||
+		(btInfo & BT_INFO_8723B_1ANT_B_SCO_BUSY)
+	) {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_SCO_BUSY;
+	} else if (btInfo & BT_INFO_8723B_1ANT_B_ACL_BUSY) {
+		if (pCoexDm->btStatus != BT_8723B_1ANT_BT_STATUS_ACL_BUSY)
+			pCoexDm->bAutoTdmaAdjust = false;
+
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
+	} else {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_MAX;
+	}
+
+	if (
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY) ||
+		(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+	)
+		bBtBusy = true;
+	else
+		bBtBusy = false;
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	halbtc8723b1ant_RunCoexistMechanism(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_HaltNotify(struct btc_coexist *pBtCoexist)
+{
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 0);
+	halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+
+	halbtc8723b1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+
+	EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+
+	pBtCoexist->bStopCoexDm = true;
+}
+
+void EXhalbtc8723b1ant_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState)
+{
+	if (pnpState == BTC_WIFI_PNP_SLEEP) {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+
+		pBtCoexist->bStopCoexDm = true;
+	} else if (pnpState == BTC_WIFI_PNP_WAKE_UP) {
+		pBtCoexist->bStopCoexDm = false;
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_Periodical(struct btc_coexist *pBtCoexist)
+{
+	static u8 disVerInfoCnt;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	}
+
+	halbtc8723b1ant_MonitorBtCtr(pBtCoexist);
+	halbtc8723b1ant_MonitorWiFiCtr(pBtCoexist);
+
+	if (
+		halbtc8723b1ant_IsWifiStatusChanged(pBtCoexist) ||
+		pCoexDm->bAutoTdmaAdjust
+	)
+		halbtc8723b1ant_RunCoexistMechanism(pBtCoexist);
+
+	pCoexSta->specialPktPeriodCnt++;
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.h b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.h
new file mode 100644
index 000000000..de471e27a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.h
@@ -0,0 +1,184 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*  The following is for 8723B 1ANT BT Co-exist definition */
+#define	BT_INFO_8723B_1ANT_B_FTP		BIT7
+#define	BT_INFO_8723B_1ANT_B_A2DP		BIT6
+#define	BT_INFO_8723B_1ANT_B_HID		BIT5
+#define	BT_INFO_8723B_1ANT_B_SCO_BUSY		BIT4
+#define	BT_INFO_8723B_1ANT_B_ACL_BUSY		BIT3
+#define	BT_INFO_8723B_1ANT_B_INQ_PAGE		BIT2
+#define	BT_INFO_8723B_1ANT_B_SCO_ESCO		BIT1
+#define	BT_INFO_8723B_1ANT_B_CONNECTION		BIT0
+
+#define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_ & BIT0)) ? true : false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT 2
+
+#define  BT_8723B_1ANT_WIFI_NOISY_THRESH 30   /* max: 255 */
+
+enum {
+	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_1ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_1ANT_MAX
+};
+
+enum {
+	BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8723B_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
+	BT_8723B_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
+	BT_8723B_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
+	BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
+	BT_8723B_1ANT_BT_STATUS_MAX
+};
+
+enum {
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE           = 0x0,
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN = 0x1,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN               = 0x2,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT        = 0x3,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE               = 0x4,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY               = 0x5,
+	BT_8723B_1ANT_WIFI_STATUS_MAX
+};
+
+enum {
+	BT_8723B_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8723B_1ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8723B_1ANT_COEX_ALGO_HID				= 0x2,
+	BT_8723B_1ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8723B_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8723B_1ANT_COEX_ALGO_MAX				= 0xb,
+};
+
+struct coex_dm_8723b_1ant {
+	/*  fw mechanism */
+	bool bCurIgnoreWlanAct;
+	bool bPreIgnoreWlanAct;
+	u8 prePsTdma;
+	u8 curPsTdma;
+	u8 psTdmaPara[5];
+	u8 psTdmaDuAdjType;
+	bool bAutoTdmaAdjust;
+	bool bPrePsTdmaOn;
+	bool bCurPsTdmaOn;
+	bool bPreBtAutoReport;
+	bool bCurBtAutoReport;
+	u8 preLps;
+	u8 curLps;
+	u8 preRpwm;
+	u8 curRpwm;
+
+	/*  sw mechanism */
+	bool bPreLowPenaltyRa;
+	bool bCurLowPenaltyRa;
+	u32 preVal0x6c0;
+	u32 curVal0x6c0;
+	u32 preVal0x6c4;
+	u32 curVal0x6c4;
+	u32 preVal0x6c8;
+	u32 curVal0x6c8;
+	u8 preVal0x6cc;
+	u8 curVal0x6cc;
+	bool bLimitedDig;
+
+	u32 backupArfrCnt1;	/*  Auto Rate Fallback Retry cnt */
+	u32 backupArfrCnt2;	/*  Auto Rate Fallback Retry cnt */
+	u16 backupRetryLimit;
+	u8 backupAmpduMaxTime;
+
+	/*  algorithm related */
+	u8 preAlgorithm;
+	u8 curAlgorithm;
+	u8 btStatus;
+	u8 wifiChnlInfo[3];
+
+	u32 preRaMask;
+	u32 curRaMask;
+	u8 preArfrType;
+	u8 curArfrType;
+	u8 preRetryLimitType;
+	u8 curRetryLimitType;
+	u8 preAmpduTimeType;
+	u8 curAmpduTimeType;
+	u32 nArpCnt;
+
+	u8 errorCondition;
+};
+
+struct coex_sta_8723b_1ant {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	bool bPanExist;
+
+	bool bUnderLps;
+	bool bUnderIps;
+	u32 specialPktPeriodCnt;
+	u32 highPriorityTx;
+	u32 highPriorityRx;
+	u32 lowPriorityTx;
+	u32 lowPriorityRx;
+	s8 btRssi;
+	bool bBtTxRxMask;
+	u8 preBtRssiState;
+	u8 preWifiRssiState[4];
+	bool bC2hBtInfoReqSent;
+	u8 btInfoC2h[BT_INFO_SRC_8723B_1ANT_MAX][10];
+	u32 btInfoC2hCnt[BT_INFO_SRC_8723B_1ANT_MAX];
+	bool bC2hBtInquiryPage;
+	bool bC2hBtPage; /* Add for win8.1 page out issue */
+	bool bWiFiIsHighPriTask; /* Add for win8.1 page out issue */
+	u8 btRetryCnt;
+	u8 btInfoExt;
+	u32 popEventCnt;
+	u8 nScanAPNum;
+
+	u32 nCRCOK_CCK;
+	u32 nCRCOK_11g;
+	u32 nCRCOK_11n;
+	u32 nCRCOK_11nAgg;
+
+	u32 nCRCErr_CCK;
+	u32 nCRCErr_11g;
+	u32 nCRCErr_11n;
+	u32 nCRCErr_11nAgg;
+
+	bool bCCKLock;
+	bool bPreCCKLock;
+	u8 nCoexTableType;
+
+	bool bForceLpsOn;
+};
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b1ant_InitHwConfig(struct btc_coexist *pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b1ant_InitCoexDm(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b1ant_IpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_LpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ScanNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ConnectNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_MediaStatusNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_BtInfoNotify(
+	struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b1ant_HaltNotify(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b1ant_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState);
+void EXhalbtc8723b1ant_Periodical(struct btc_coexist *pBtCoexist);
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
new file mode 100644
index 000000000..c1c7b5cc1
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
@@ -0,0 +1,2630 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "Mp_Precomp.h"
+
+/* defines */
+#define HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(val)			      \
+do {									      \
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, val);           \
+	pCoexDm->psTdmaDuAdjType = val;                                       \
+} while (0)
+
+/*  Global variables, these are static variables */
+static struct coex_dm_8723b_2ant GLCoexDm8723b2Ant;
+static struct coex_dm_8723b_2ant *pCoexDm = &GLCoexDm8723b2Ant;
+static struct coex_sta_8723b_2ant GLCoexSta8723b2Ant;
+static struct coex_sta_8723b_2ant *pCoexSta = &GLCoexSta8723b2Ant;
+
+/*  local function start with halbtc8723b2ant_ */
+static u8 halbtc8723b2ant_BtRssiState(
+	u8 levelNum, u8 rssiThresh, u8 rssiThresh1
+)
+{
+	s32 btRssi = 0;
+	u8 btRssiState = pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_HIGH;
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+			}
+		} else {
+			if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			return pCoexSta->preBtRssiState;
+		}
+
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+			}
+		} else if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (btRssi >= (rssiThresh1 + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_HIGH;
+			} else if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+			}
+		} else {
+			if (btRssi < rssiThresh1) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+			}
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static u8 halbtc8723b2ant_WifiRssiState(
+	struct btc_coexist *pBtCoexist,
+	u8 index,
+	u8 levelNum,
+	u8 rssiThresh,
+	u8 rssiThresh1
+)
+{
+	s32 wifiRssi = 0;
+	u8 wifiRssiState = pCoexSta->preWifiRssiState[index];
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (wifiRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+			}
+		} else {
+			if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			return pCoexSta->preWifiRssiState[index];
+		}
+
+		if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (wifiRssi >= (rssiThresh + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+			}
+		} else if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (wifiRssi >= (rssiThresh1 + BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+			} else if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+			}
+		} else {
+			if (wifiRssi < rssiThresh1) {
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+			}
+		}
+	}
+
+	pCoexSta->preWifiRssiState[index] = wifiRssiState;
+
+	return wifiRssiState;
+}
+
+static void halbtc8723b2ant_LimitedRx(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	bool bRejApAggPkt,
+	bool bBtCtrlAggBufSize,
+	u8 aggBufSize
+)
+{
+	bool bRejectRxAgg = bRejApAggPkt;
+	bool bBtCtrlRxAggSize = bBtCtrlAggBufSize;
+	u8 rxAggSize = aggBufSize;
+
+	/*  */
+	/* 	Rx Aggregation related setting */
+	/*  */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejectRxAgg);
+	/*  decide BT control aggregation buf size or not */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bBtCtrlRxAggSize);
+	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxAggSize);
+	/*  real update aggregation setting */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+}
+
+static void halbtc8723b2ant_QueryBtInfo(struct btc_coexist *pBtCoexist)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static bool halbtc8723b2ant_IsWifiStatusChanged(struct btc_coexist *pBtCoexist)
+{
+	static bool	bPreWifiBusy, bPreUnder4way, bPreBtHsOn;
+	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+
+	if (bWifiConnected) {
+		if (bWifiBusy != bPreWifiBusy) {
+			bPreWifiBusy = bWifiBusy;
+			return true;
+		}
+
+		if (bUnder4way != bPreUnder4way) {
+			bPreUnder4way = bUnder4way;
+			return true;
+		}
+
+		if (bBtHsOn != bPreBtHsOn) {
+			bPreBtHsOn = bBtHsOn;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b2ant_UpdateBtLinkInfo(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
+	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
+	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
+	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
+	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
+
+	/*  work around for HS mode. */
+	if (bBtHsOn) {
+		pBtLinkInfo->bPanExist = true;
+		pBtLinkInfo->bBtLinkExist = true;
+	}
+
+	/*  check if Sco only */
+	if (
+		pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bScoOnly = true;
+	else
+		pBtLinkInfo->bScoOnly = false;
+
+	/*  check if A2dp only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bA2dpOnly = true;
+	else
+		pBtLinkInfo->bA2dpOnly = false;
+
+	/*  check if Pan only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bPanOnly = true;
+	else
+		pBtLinkInfo->bPanOnly = false;
+
+	/*  check if Hid only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bHidOnly = true;
+	else
+		pBtLinkInfo->bHidOnly = false;
+}
+
+static u8 halbtc8723b2ant_ActionAlgorithm(struct btc_coexist *pBtCoexist)
+{
+	struct btc_bt_link_info *pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+	u8 algorithm = BT_8723B_2ANT_COEX_ALGO_UNDEFINED;
+	u8 numOfDiffProfile = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	if (!pBtLinkInfo->bBtLinkExist) {
+		return algorithm;
+	}
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANHS;
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile >= 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+				} else {
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b2ant_SetFwDacSwingLevel(
+	struct btc_coexist *pBtCoexist, u8 dacSwingLvl
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	/*  There are several type of dacswing */
+	/*  0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	H2C_Parameter[0] = dacSwingLvl;
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x64, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SetFwDecBtPwr(
+	struct btc_coexist *pBtCoexist, u8 decBtPwrLvl
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	H2C_Parameter[0] = decBtPwrLvl;
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x62, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_DecBtPwr(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 decBtPwrLvl
+)
+{
+	pCoexDm->curBtDecPwrLvl = decBtPwrLvl;
+
+	if (!bForceExec) {
+		if (pCoexDm->preBtDecPwrLvl == pCoexDm->curBtDecPwrLvl)
+			return;
+	}
+	halbtc8723b2ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->curBtDecPwrLvl);
+
+	pCoexDm->preBtDecPwrLvl = pCoexDm->curBtDecPwrLvl;
+}
+
+static void halbtc8723b2ant_FwDacSwingLvl(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 fwDacSwingLvl
+)
+{
+	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
+
+	if (!bForceExec) {
+		if (pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl)
+			return;
+	}
+
+	halbtc8723b2ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
+
+	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
+}
+
+static void halbtc8723b2ant_SetSwRfRxLpfCorner(
+	struct btc_coexist *pBtCoexist,
+	bool bRxRfShrinkOn
+)
+{
+	if (bRxRfShrinkOn) {
+		/* Shrink RF Rx LPF corner */
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner */
+		/*  After initialized, we can use pCoexDm->btRf0x1eBackup */
+		if (pBtCoexist->bInitilized) {
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+static void halbtc8723b2ant_RfShrink(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bRxRfShrinkOn
+)
+{
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink)
+			return;
+	}
+	halbtc8723b2ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+static void halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(
+	struct btc_coexist *pBtCoexist, bool bLowPenaltyRa
+)
+{
+	u8 	H2C_Parameter[6] = {0};
+
+	H2C_Parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (bLowPenaltyRa) {
+		H2C_Parameter[1] |= BIT0;
+		H2C_Parameter[2] = 0x00;  /* normal rate except MCS7/6/5, OFDM54/48/36 */
+		H2C_Parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		H2C_Parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		H2C_Parameter[5] = 0xf9;	/* MCS5 or OFDM36 */
+	}
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_LowPenaltyRa(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bLowPenaltyRa
+)
+{
+	/* return; */
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa)
+			return;
+	}
+	halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+static void halbtc8723b2ant_SetDacSwingReg(struct btc_coexist *pBtCoexist, u32 level)
+{
+	u8 val = (u8)level;
+
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x883, 0x3e, val);
+}
+
+static void halbtc8723b2ant_SetSwFullTimeDacSwing(
+	struct btc_coexist *pBtCoexist, bool bSwDacSwingOn, u32 swDacSwingLvl
+)
+{
+	if (bSwDacSwingOn)
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, swDacSwingLvl);
+	else
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, 0x18);
+}
+
+
+static void halbtc8723b2ant_DacSwing(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	bool bDacSwingOn,
+	u32 dacSwingLvl
+)
+{
+	pCoexDm->bCurDacSwingOn = bDacSwingOn;
+	pCoexDm->curDacSwingLvl = dacSwingLvl;
+
+	if (!bForceExec) {
+		if ((pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
+			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl))
+			return;
+	}
+	mdelay(30);
+	halbtc8723b2ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
+
+	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
+	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
+}
+
+static void halbtc8723b2ant_SetAgcTable(
+	struct btc_coexist *pBtCoexist, bool bAgcTableEn
+)
+{
+	u8 rssiAdjustVal = 0;
+
+	/* BB AGC Gain Table */
+	if (bAgcTableEn) {
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6e1A0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6d1B0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6c1C0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6b1D0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6a1E0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x691F0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x68200001);
+	} else {
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xaa1A0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa91B0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa81C0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa71D0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa61E0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa51F0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa4200001);
+	}
+
+
+	/* RF Gain */
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
+	if (bAgcTableEn) {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38ffe);
+	} else {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x380c3);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x28ce6);
+	}
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xed, 0xfffff, 0x1);
+	if (bAgcTableEn) {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38ffe);
+	} else {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x380c3);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x28ce6);
+	}
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xed, 0xfffff, 0x0);
+
+	/*  set rssiAdjustVal for wifi module. */
+	if (bAgcTableEn)
+		rssiAdjustVal = 8;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
+}
+
+static void halbtc8723b2ant_AgcTable(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bAgcTableEn
+)
+{
+	pCoexDm->bCurAgcTableEn = bAgcTableEn;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn)
+			return;
+	}
+	halbtc8723b2ant_SetAgcTable(pBtCoexist, bAgcTableEn);
+
+	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
+}
+
+static void halbtc8723b2ant_SetCoexTable(
+	struct btc_coexist *pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b2ant_CoexTable(
+	struct btc_coexist *pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+			(pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
+			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc)
+		)
+			return;
+	}
+	halbtc8723b2ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+static void halbtc8723b2ant_CoexTableWithType(
+	struct btc_coexist *pBtCoexist, bool bForceExec, u8 type
+)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0xffffffff, 0xffffffff, 0xffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0xfafafafa, 0xffff, 0x3);
+		break;
+	case 8:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 9:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 10:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5aff5aff, 0xffff, 0x3);
+		break;
+	case 11:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5f5a5f, 0xffff, 0x3);
+		break;
+	case 12:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5f5f5f5f, 0xffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8723b2ant_SetFwIgnoreWlanAct(
+	struct btc_coexist *pBtCoexist, bool bEnable
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0;		/*  function enable */
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_IgnoreWlanAct(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b2ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b2ant_SetFwPstdma(
+	struct btc_coexist *pBtCoexist,
+	u8 byte1,
+	u8 byte2,
+	u8 byte3,
+	u8 byte4,
+	u8 byte5
+)
+{
+	u8 	H2C_Parameter[5] = {0};
+
+	H2C_Parameter[0] = byte1;
+	H2C_Parameter[1] = byte2;
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = byte5;
+
+	pCoexDm->psTdmaPara[0] = byte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = byte5;
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SwMechanism1(
+	struct btc_coexist *pBtCoexist,
+	bool bShrinkRxLPF,
+	bool bLowPenaltyRA,
+	bool bLimitedDIG,
+	bool bBTLNAConstrain
+)
+{
+	halbtc8723b2ant_RfShrink(pBtCoexist, NORMAL_EXEC, bShrinkRxLPF);
+	halbtc8723b2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+}
+
+static void halbtc8723b2ant_SwMechanism2(
+	struct btc_coexist *pBtCoexist,
+	bool bAGCTableShift,
+	bool bADCBackOff,
+	bool bSWDACSwing,
+	u32 dacSwingLvl
+)
+{
+	halbtc8723b2ant_AgcTable(pBtCoexist, NORMAL_EXEC, bAGCTableShift);
+	halbtc8723b2ant_DacSwing(pBtCoexist, NORMAL_EXEC, bSWDACSwing, dacSwingLvl);
+}
+
+static void halbtc8723b2ant_SetAntPath(
+	struct btc_coexist *pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	struct btc_board_info *pBoardInfo = &pBtCoexist->boardInfo;
+	u32 fwVer = 0, u4Tmp = 0;
+	bool bPgExtSwitch = false;
+	bool bUseExtSwitch = false;
+	u8 	H2C_Parameter[2] = {0};
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_EXT_SWITCH, &bPgExtSwitch);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);	/*  [31:16]=fw ver, [15:0]=fw sub ver */
+
+	if ((fwVer > 0 && fwVer < 0xc0000) || bPgExtSwitch)
+		bUseExtSwitch = true;
+
+	if (bInitHwCfg) {
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x39, 0x8, 0x1);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x974, 0xff);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x944, 0x3, 0x3);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x930, 0x77);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1);
+
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to LOW */
+			H2C_Parameter[0] = 0;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x0);
+		}
+
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0); /* WiFi TRx Mask off */
+		pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x01); /* BT TRx Mask off */
+
+		if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "no antenna inverse" */
+			H2C_Parameter[0] = 0;
+		} else {
+			/* tell firmware "antenna inverse" */
+			H2C_Parameter[0] = 1;
+		}
+
+		if (bUseExtSwitch) {
+			/* ext switch type */
+			H2C_Parameter[1] = 1;
+		} else {
+			/* int switch type */
+			H2C_Parameter[1] = 0;
+		}
+		pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+	}
+
+	/*  ext switch setting */
+	if (bUseExtSwitch) {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp &= ~BIT23;
+			u4Tmp |= BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+		}
+
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+		switch (antPosType) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);	/*  ext switch main at wifi */
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);	/*  ext switch aux at wifi */
+			break;
+		}
+	} else { /*  internal switch */
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp |= BIT23;
+			u4Tmp &= ~BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+		}
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x64, 0x1, 0x0); /* fixed external switch S1->Main, S0->Aux */
+		switch (antPosType) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280); /*  fixed internal switch S0->WiFi, S1->BT */
+			break;
+		}
+	}
+}
+
+static void halbtc8723b2ant_PsTdma(
+	struct btc_coexist *pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma)
+		)
+			return;
+	}
+
+	if (bTurnOn) {
+		switch (type) {
+		case 1:
+		default:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1c, 0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x10, 0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1c, 0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x10, 0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 15:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x90);
+			break;
+		case 16:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0x60, 0x90);
+			break;
+		case 17:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x15, 0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/*  disable PS tdma */
+		switch (type) {
+		case 0:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		case 1:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x48, 0x0);
+			break;
+		default:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		}
+	}
+
+	/*  update pre state */
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+static void halbtc8723b2ant_CoexAllOff(struct btc_coexist *pBtCoexist)
+{
+	/*  fw all off */
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	/*  sw all off */
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+	/*  hw all off */
+	/* pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0); */
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8723b2ant_InitCoexDm(struct btc_coexist *pBtCoexist)
+{
+	/*  force to reset coex mechanism */
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 1);
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, FORCE_EXEC, 0);
+
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+}
+
+static void halbtc8723b2ant_ActionBtInquiry(struct btc_coexist *pBtCoexist)
+{
+	bool bWifiConnected = false;
+	bool bLowPwrDisable = true;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	if (bWifiConnected) {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+	} else {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+	}
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+	pCoexDm->bNeedRecover0x948 = true;
+	pCoexDm->backup0x948 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+
+	halbtc8723b2ant_SetAntPath(pBtCoexist, BTC_ANT_WIFI_AT_AUX, false, false);
+}
+
+static bool halbtc8723b2ant_IsCommonAction(struct btc_coexist *pBtCoexist)
+{
+	u8 btRssiState = BTC_RSSI_STATE_HIGH;
+	bool bCommon = false, bWifiConnected = false, bWifiBusy = false;
+	bool bBtHsOn = false, bLowPwrDisable = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (!bWifiConnected) {
+		bLowPwrDisable = false;
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+		halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+		halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+		halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+		bCommon = true;
+	} else {
+		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) {
+			bLowPwrDisable = false;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+			halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+			bCommon = true;
+		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) {
+			bLowPwrDisable = true;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+
+			if (bBtHsOn)
+				return false;
+
+			halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+			bCommon = true;
+		} else {
+			bLowPwrDisable = true;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+
+			if (bWifiBusy) {
+				bCommon = false;
+			} else {
+				if (bBtHsOn)
+					return false;
+
+				btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+				halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+				pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+				halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+				halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 21);
+				halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+
+				if (BTC_RSSI_HIGH(btRssiState))
+					halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+				else
+					halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+				halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+				halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+				bCommon = true;
+			}
+		}
+	}
+
+	return bCommon;
+}
+
+static void halbtc8723b2ant_TdmaDurationAdjust(
+	struct btc_coexist *pBtCoexist, bool bScoHid, bool bTxPause, u8 maxInterval
+)
+{
+	static s32 up, dn, m, n, WaitCount;
+	s32 result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
+	u8 retryCount = 0;
+
+	if (!pCoexDm->bAutoTdmaAdjust) {
+		pCoexDm->bAutoTdmaAdjust = true;
+		{
+			if (bScoHid) {
+				if (bTxPause) {
+					if (maxInterval == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(13);
+					else if (maxInterval == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+					else
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				} else {
+					if (maxInterval == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(9);
+					else if (maxInterval == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+					else
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				}
+			} else {
+				if (bTxPause) {
+					if (maxInterval == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(5);
+					else if (maxInterval == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				} else {
+					if (maxInterval == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(1);
+					else if (maxInterval == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				}
+			}
+		}
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* acquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		result = 0;
+		WaitCount++;
+
+		if (retryCount == 0) { /*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) { /*  if 連續 n 個2秒 retry count為0, 則調寬WiFi duration */
+				WaitCount = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+			}
+		} else if (retryCount <= 3) { /*  <=3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) { /*  if 連續 2 個2秒 retry count< 3, 則調窄WiFi duration */
+				if (WaitCount <= 2)
+					m++; /*  避免一直在兩個level中來回 */
+				else
+					m = 1;
+
+				if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1;
+			}
+		} else { /* retry count > 3, 只要1次 retry count > 3, 則調窄WiFi duration */
+			if (WaitCount == 1)
+				m++; /*  避免一直在兩個level中來回 */
+			else
+				m = 1;
+
+			if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+				m = 20;
+
+			n = 3 * m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0;
+			result = -1;
+		}
+
+		if (maxInterval == 1) {
+			if (bTxPause) {
+				if (pCoexDm->curPsTdma == 71)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(5);
+				else if (pCoexDm->curPsTdma == 1)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(5);
+				else if (pCoexDm->curPsTdma == 2)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+				else if (pCoexDm->curPsTdma == 3)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				else if (pCoexDm->curPsTdma == 4)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+
+				if (pCoexDm->curPsTdma == 9)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(13);
+				else if (pCoexDm->curPsTdma == 10)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+				else if (pCoexDm->curPsTdma == 11)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				else if (pCoexDm->curPsTdma == 12)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+					else if (pCoexDm->curPsTdma == 13)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(5);
+					else if (pCoexDm->curPsTdma == 16)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(13);
+				}
+			} else {
+				if (pCoexDm->curPsTdma == 5)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(71);
+				else if (pCoexDm->curPsTdma == 6)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+				else if (pCoexDm->curPsTdma == 7)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				else if (pCoexDm->curPsTdma == 8)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+
+				if (pCoexDm->curPsTdma == 13)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(9);
+				else if (pCoexDm->curPsTdma == 14)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+				else if (pCoexDm->curPsTdma == 15)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				else if (pCoexDm->curPsTdma == 16)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 71)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(1);
+					else if (pCoexDm->curPsTdma == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+					else if (pCoexDm->curPsTdma == 9)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(1);
+					else if (pCoexDm->curPsTdma == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(71);
+					else if (pCoexDm->curPsTdma == 12)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(9);
+				}
+			}
+		} else if (maxInterval == 2) {
+			if (bTxPause) {
+				if (pCoexDm->curPsTdma == 1)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+				else if (pCoexDm->curPsTdma == 2)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+				else if (pCoexDm->curPsTdma == 3)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				else if (pCoexDm->curPsTdma == 4)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+
+				if (pCoexDm->curPsTdma == 9)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+				else if (pCoexDm->curPsTdma == 10)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+				else if (pCoexDm->curPsTdma == 11)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				else if (pCoexDm->curPsTdma == 12)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+					else if (pCoexDm->curPsTdma == 13)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(6);
+					else if (pCoexDm->curPsTdma == 16)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(14);
+				}
+			} else {
+				if (pCoexDm->curPsTdma == 5)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+				else if (pCoexDm->curPsTdma == 6)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+				else if (pCoexDm->curPsTdma == 7)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				else if (pCoexDm->curPsTdma == 8)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+
+				if (pCoexDm->curPsTdma == 13)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+				else if (pCoexDm->curPsTdma == 14)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+				else if (pCoexDm->curPsTdma == 15)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				else if (pCoexDm->curPsTdma == 16)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+					else if (pCoexDm->curPsTdma == 9)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(2);
+					else if (pCoexDm->curPsTdma == 12)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(10);
+				}
+			}
+		} else if (maxInterval == 3) {
+			if (bTxPause) {
+				if (pCoexDm->curPsTdma == 1)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				else if (pCoexDm->curPsTdma == 2)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				else if (pCoexDm->curPsTdma == 3)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+				else if (pCoexDm->curPsTdma == 4)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+
+				if (pCoexDm->curPsTdma == 9)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				else if (pCoexDm->curPsTdma == 10)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				else if (pCoexDm->curPsTdma == 11)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				else if (pCoexDm->curPsTdma == 12)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(8);
+					else if (pCoexDm->curPsTdma == 13)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(16);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 7)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 6)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(7);
+					else if (pCoexDm->curPsTdma == 16)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 15)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+					else if (pCoexDm->curPsTdma == 14)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(15);
+				}
+			} else {
+				if (pCoexDm->curPsTdma == 5)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				else if (pCoexDm->curPsTdma == 6)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				else if (pCoexDm->curPsTdma == 7)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+				else if (pCoexDm->curPsTdma == 8)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+
+				if (pCoexDm->curPsTdma == 13)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				else if (pCoexDm->curPsTdma == 14)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				else if (pCoexDm->curPsTdma == 15)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				else if (pCoexDm->curPsTdma == 16)
+					HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(4);
+					else if (pCoexDm->curPsTdma == 9)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(12);
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 3)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 2)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(3);
+					else if (pCoexDm->curPsTdma == 12)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 11)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+					else if (pCoexDm->curPsTdma == 10)
+						HAL_BTC8723B2ANT_DMA_DURATION_ADJUST(11);
+				}
+			}
+		}
+	}
+
+	/*  if current PsTdma not match with the recorded one (when scan, dhcp...), */
+	/*  then we have to adjust it back to the previous record one. */
+	if (pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType) {
+		bool bScan = false, bLink = false, bRoam = false;
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if (!bScan && !bLink && !bRoam)
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
+
+	}
+}
+
+/*  SCO only or SCO+PAN(HS) */
+static void halbtc8723b2ant_ActionSco(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 4);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (BTC_WIFI_BW_LEGACY == wifiBw) /* for SCO quality at 11b/g mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	else  /* for SCO quality & wifi performance balance at 11n mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 8);
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0); /* for voice quality */
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x4);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, true, 0x4);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x4);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, true, 0x4);
+		}
+	}
+}
+
+
+static void halbtc8723b2ant_ActionHid(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (BTC_WIFI_BW_LEGACY == wifiBw) /* for HID at 11b/g mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 9);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+	else
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8723b2ant_ActionA2dp(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, wifiRssiState1, btRssiState;
+	u32 wifiBw;
+	u8 apNum = 0;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	wifiRssiState1 = halbtc8723b2ant_WifiRssiState(pBtCoexist, 1, 2, 40, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+
+	/*  define the office environment */
+	if (apNum >= 10 && BTC_RSSI_HIGH(wifiRssiState1)) {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+
+		/*  sw mechanism */
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if (BTC_WIFI_BW_HT40 == wifiBw) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x18);
+		}
+		return;
+	}
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, false, 1);
+	else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 1);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionA2dpPanHs(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 2);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionPanEdr(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 10);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+	else
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+
+/* PAN(HS) only */
+static void halbtc8723b2ant_ActionPanHs(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8723b2ant_ActionPanEdrA2dp(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 12);
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 3);
+		else
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, false, 3);
+	} else {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 3);
+	}
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionPanEdrHid(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		if (BTC_WIFI_BW_HT40 == wifiBw) {
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 3);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 11);
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
+		} else {
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		}
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 2);
+	} else {
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 11);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+	}
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/*  HID+A2DP+PAN(EDR) */
+static void halbtc8723b2ant_ActionHidA2dpPanEdr(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+		else
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 3);
+	} else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionHidA2dp(struct btc_coexist *pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+	u8 apNum = 0;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	/* btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0); */
+	btRssiState = halbtc8723b2ant_BtRssiState(3, 29, 37);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x5);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_LEGACY == wifiBw) {
+		if (BTC_RSSI_HIGH(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else if (BTC_RSSI_MEDIUM(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	} else {
+		/*  only 802.11N mode we have to dec bt power to 4 degree */
+		if (BTC_RSSI_HIGH(btRssiState)) {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+			/*  need to check ap Number of Not */
+			if (apNum < 10)
+				halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 4);
+			else
+				halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		} else if (BTC_RSSI_MEDIUM(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	}
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 2);
+	else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_RunCoexistMechanism(struct btc_coexist *pBtCoexist)
+{
+	u8 algorithm = 0;
+
+	if (pBtCoexist->bManualControl) {
+		return;
+	}
+
+	if (pCoexSta->bUnderIps) {
+		return;
+	}
+
+	algorithm = halbtc8723b2ant_ActionAlgorithm(pBtCoexist);
+	if (pCoexSta->bC2hBtInquiryPage && (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		halbtc8723b2ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else {
+		if (pCoexDm->bNeedRecover0x948) {
+			pCoexDm->bNeedRecover0x948 = false;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, pCoexDm->backup0x948);
+		}
+	}
+
+	pCoexDm->curAlgorithm = algorithm;
+
+	if (halbtc8723b2ant_IsCommonAction(pBtCoexist)) {
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else {
+		if (pCoexDm->curAlgorithm != pCoexDm->preAlgorithm) {
+			pCoexDm->bAutoTdmaAdjust = false;
+		}
+
+
+		switch (pCoexDm->curAlgorithm) {
+		case BT_8723B_2ANT_COEX_ALGO_SCO:
+			halbtc8723b2ant_ActionSco(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID:
+			halbtc8723b2ant_ActionHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP:
+			halbtc8723b2ant_ActionA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
+			halbtc8723b2ant_ActionA2dpPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
+			halbtc8723b2ant_ActionPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANHS:
+			halbtc8723b2ant_ActionPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
+			halbtc8723b2ant_ActionPanEdrA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
+			halbtc8723b2ant_ActionPanEdrHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			halbtc8723b2ant_ActionHidA2dpPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
+			halbtc8723b2ant_ActionHidA2dp(pBtCoexist);
+			break;
+		default:
+			halbtc8723b2ant_CoexAllOff(pBtCoexist);
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b2ant_WifiOffHwCfg(struct btc_coexist *pBtCoexist)
+{
+	bool bIsInMpMode = false;
+	u8 H2C_Parameter[2] = {0};
+	u32 fwVer = 0;
+
+	/*  set wlan_act to low */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780); /* WiFi goto standby while GNT_BT 0-->1 */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	if (fwVer >= 0x180000) {
+		/* Use H2C to set GNT_BT to HIGH */
+		H2C_Parameter[0] = 1;
+		pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+	} else
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_IS_IN_MP_MODE, &bIsInMpMode);
+	if (!bIsInMpMode)
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x0); /* BT select s0/s1 is controlled by BT */
+	else
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+}
+
+static void halbtc8723b2ant_InitHwConfig(struct btc_coexist *pBtCoexist, bool bBackUp)
+{
+	u8 u1Tmp = 0;
+
+	/*  backup rf 0x1e value */
+	pCoexDm->btRf0x1eBackup =
+		pBtCoexist->fBtcGetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/*  0x790[5:0]= 0x5 */
+	u1Tmp = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x790);
+	u1Tmp &= 0xc0;
+	u1Tmp |= 0x5;
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x790, u1Tmp);
+
+	/* Antenna config */
+	halbtc8723b2ant_SetAntPath(pBtCoexist, BTC_ANT_WIFI_AT_MAIN, true, false);
+
+	/*  PTA parameter */
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	/*  Enable counter statistics */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc); /* 0x76e[3] = 1, WLAN_Act control by PTA */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x778, 0x3);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x40, 0x20, 0x1);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8723b2ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8723b2ant_ */
+/*  */
+void EXhalbtc8723b2ant_PowerOnSetting(struct btc_coexist *pBtCoexist)
+{
+	struct btc_board_info *pBoardInfo = &pBtCoexist->boardInfo;
+	u8 u1Tmp = 0x4; /* Set BIT2 by default since it's 2ant case */
+	u16 u2Tmp = 0x0;
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x67, 0x20);
+
+	/*  enable BB, REG_SYS_FUNC_EN such that we can write 0x948 correctly. */
+	u2Tmp = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x2);
+	pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x2, u2Tmp | BIT0 | BIT1);
+
+	/*  set GRAN_BT = 1 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+	/*  set WLAN_ACT = 0 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	/*  */
+	/*  S0 or S1 setting and Local register setting(By the setting fw can get ant number, S0/S1, ... info) */
+	/*  Local setting bit define */
+	/* 	BIT0: "0" for no antenna inverse; "1" for antenna inverse */
+	/* 	BIT1: "0" for internal switch; "1" for external switch */
+	/* 	BIT2: "0" for one antenna; "1" for two antenna */
+	/*  NOTE: here default all internal switch and 1-antenna ==> BIT1 = 0 and BIT2 = 0 */
+	if (pBtCoexist->chipInterface == BTC_INTF_USB) {
+		/*  fixed at S0 for USB interface */
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		u1Tmp |= 0x1;	/*  antenna inverse */
+		pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0xfe08, u1Tmp);
+
+		pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+	} else {
+		/*  for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (pBoardInfo->singleAntPath == 0) {
+			/*  set to S1 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+		} else if (pBoardInfo->singleAntPath == 1) {
+			/*  set to S0 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			u1Tmp |= 0x1;	/*  antenna inverse */
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+		}
+
+		if (pBtCoexist->chipInterface == BTC_INTF_PCI)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x384, u1Tmp);
+		else if (pBtCoexist->chipInterface == BTC_INTF_SDIO)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x60, u1Tmp);
+	}
+}
+
+void EXhalbtc8723b2ant_InitHwConfig(struct btc_coexist *pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b2ant_InitHwConfig(pBtCoexist, true);
+}
+
+void EXhalbtc8723b2ant_InitCoexDm(struct btc_coexist *pBtCoexist)
+{
+	halbtc8723b2ant_InitCoexDm(pBtCoexist);
+}
+
+void EXhalbtc8723b2ant_IpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		pCoexSta->bUnderIps = true;
+		halbtc8723b2ant_WifiOffHwCfg(pBtCoexist);
+		halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+		halbtc8723b2ant_CoexAllOff(pBtCoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		pCoexSta->bUnderIps = false;
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_LpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		pCoexSta->bUnderLps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		pCoexSta->bUnderLps = false;
+	}
+}
+
+void EXhalbtc8723b2ant_ScanNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (BTC_SCAN_START == type) {
+	} else if (BTC_SCAN_FINISH == type) {
+	}
+}
+
+void EXhalbtc8723b2ant_ConnectNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type) {
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+	}
+}
+
+void EXhalbtc8723b2ant_MediaStatusNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	u8 H2C_Parameter[3] = {0};
+	u32 wifiBw;
+	u8 wifiCentralChnl;
+	u8 apNum = 0;
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if ((BTC_MEDIA_CONNECT == type) && (wifiCentralChnl <= 14)) {
+		H2C_Parameter[0] = 0x1;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			H2C_Parameter[2] = 0x30;
+		else {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+			if (apNum < 10)
+				H2C_Parameter[2] = 0x30;
+			else
+				H2C_Parameter[2] = 0x20;
+		}
+	}
+
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b2ant_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+}
+
+void EXhalbtc8723b2ant_BtInfoNotify(
+	struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length
+)
+{
+	u8 	btInfo = 0;
+	u8 	i, rspSource = 0;
+	bool bBtBusy = false, bLimitedDig = false;
+	bool bWifiConnected = false;
+
+	pCoexSta->bC2hBtInfoReqSent = false;
+
+	rspSource = tmpBuf[0] & 0xf;
+	if (rspSource >= BT_INFO_SRC_8723B_2ANT_MAX)
+		rspSource = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
+
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+
+	}
+
+	if (pBtCoexist->bManualControl) {
+		return;
+	}
+
+	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rspSource) {
+		pCoexSta->btRetryCnt = pCoexSta->btInfoC2h[rspSource][2] & 0xf; /* [3:0] */
+
+		pCoexSta->btRssi = pCoexSta->btInfoC2h[rspSource][3] * 2 + 10;
+
+		pCoexSta->btInfoExt = pCoexSta->btInfoC2h[rspSource][4];
+
+		pCoexSta->bBtTxRxMask = (pCoexSta->btInfoC2h[rspSource][2] & 0x40);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TX_RX_MASK, &pCoexSta->bBtTxRxMask);
+		if (pCoexSta->bBtTxRxMask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C != 0x01 => Need to switch BT TRx Mask */
+			pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x01);
+		}
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if ((pCoexSta->btInfoExt & BIT1)) {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+			if (bWifiConnected)
+				EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
+			else
+				EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((pCoexSta->btInfoExt & BIT3)) {
+			halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, false);
+		} else {
+			/*  BT already NOT ignore Wlan active, do nothing here. */
+		}
+	}
+
+	/*  check BIT2 first ==> check if bt is under inquiry or page scan */
+	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
+		pCoexSta->bC2hBtInquiryPage = true;
+	else
+		pCoexSta->bC2hBtInquiryPage = false;
+
+	/*  set link exist status */
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		pCoexSta->bBtLinkExist = false;
+		pCoexSta->bPanExist = false;
+		pCoexSta->bA2dpExist = false;
+		pCoexSta->bHidExist = false;
+		pCoexSta->bScoExist = false;
+	} else { /*  connection exists */
+		pCoexSta->bBtLinkExist = true;
+		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
+			pCoexSta->bPanExist = true;
+		else
+			pCoexSta->bPanExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
+			pCoexSta->bA2dpExist = true;
+		else
+			pCoexSta->bA2dpExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
+			pCoexSta->bHidExist = true;
+		else
+			pCoexSta->bHidExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
+			pCoexSta->bScoExist = true;
+		else
+			pCoexSta->bScoExist = false;
+	}
+
+	halbtc8723b2ant_UpdateBtLinkInfo(pBtCoexist);
+
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION)	{ /*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
+	} else if (
+		(btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
+		(btInfo & BT_INFO_8723B_2ANT_B_SCO_BUSY)
+	) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
+	} else if (btInfo & BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+	} else {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_MAX;
+	}
+
+	if (
+		(BT_8723B_2ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_2ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		bBtBusy = true;
+		bLimitedDig = true;
+	} else {
+		bBtBusy = false;
+		bLimitedDig = false;
+	}
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	pCoexDm->bLimitedDig = bLimitedDig;
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &bLimitedDig);
+
+	halbtc8723b2ant_RunCoexistMechanism(pBtCoexist);
+}
+
+void EXhalbtc8723b2ant_HaltNotify(struct btc_coexist *pBtCoexist)
+{
+	halbtc8723b2ant_WifiOffHwCfg(pBtCoexist);
+	pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15); /* BT goto standby while GNT_BT 1-->0 */
+	halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+
+	EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void EXhalbtc8723b2ant_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState)
+{
+	if (BTC_WIFI_PNP_SLEEP == pnpState) {
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnpState) {
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_Periodical(struct btc_coexist *pBtCoexist)
+{
+	static u8 disVerInfoCnt;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	}
+
+	if (
+		halbtc8723b2ant_IsWifiStatusChanged(pBtCoexist) ||
+		pCoexDm->bAutoTdmaAdjust
+	)
+		halbtc8723b2ant_RunCoexistMechanism(pBtCoexist);
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.h b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.h
new file mode 100644
index 000000000..1896ac546
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.h
@@ -0,0 +1,146 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*  The following is for 8723B 2Ant BT Co-exist definition */
+#define	BT_INFO_8723B_2ANT_B_FTP		BIT7
+#define	BT_INFO_8723B_2ANT_B_A2DP		BIT6
+#define	BT_INFO_8723B_2ANT_B_HID		BIT5
+#define	BT_INFO_8723B_2ANT_B_SCO_BUSY		BIT4
+#define	BT_INFO_8723B_2ANT_B_ACL_BUSY		BIT3
+#define	BT_INFO_8723B_2ANT_B_INQ_PAGE		BIT2
+#define	BT_INFO_8723B_2ANT_B_SCO_ESCO		BIT1
+#define	BT_INFO_8723B_2ANT_B_CONNECTION		BIT0
+
+#define		BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
+
+enum {
+	BT_INFO_SRC_8723B_2ANT_WIFI_FW        = 0x0,
+	BT_INFO_SRC_8723B_2ANT_BT_RSP         = 0x1,
+	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND = 0x2,
+	BT_INFO_SRC_8723B_2ANT_MAX
+};
+
+enum {
+	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE = 0x0,
+	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE     = 0x1,
+	BT_8723B_2ANT_BT_STATUS_INQ_PAGE           = 0x2,
+	BT_8723B_2ANT_BT_STATUS_ACL_BUSY           = 0x3,
+	BT_8723B_2ANT_BT_STATUS_SCO_BUSY           = 0x4,
+	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY       = 0x5,
+	BT_8723B_2ANT_BT_STATUS_MAX
+};
+
+enum {
+	BT_8723B_2ANT_COEX_ALGO_UNDEFINED       = 0x0,
+	BT_8723B_2ANT_COEX_ALGO_SCO             = 0x1,
+	BT_8723B_2ANT_COEX_ALGO_HID             = 0x2,
+	BT_8723B_2ANT_COEX_ALGO_A2DP            = 0x3,
+	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS      = 0x4,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR          = 0x5,
+	BT_8723B_2ANT_COEX_ALGO_PANHS           = 0x6,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP     = 0x7,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID      = 0x8,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP        = 0xa,
+	BT_8723B_2ANT_COEX_ALGO_MAX             = 0xb,
+};
+
+struct coex_dm_8723b_2ant {
+	/*  fw mechanism */
+	u8 preBtDecPwrLvl;
+	u8 curBtDecPwrLvl;
+	u8 preFwDacSwingLvl;
+	u8 curFwDacSwingLvl;
+	bool bCurIgnoreWlanAct;
+	bool bPreIgnoreWlanAct;
+	u8 prePsTdma;
+	u8 curPsTdma;
+	u8 psTdmaPara[5];
+	u8 psTdmaDuAdjType;
+	bool bResetTdmaAdjust;
+	bool bAutoTdmaAdjust;
+	bool bPrePsTdmaOn;
+	bool bCurPsTdmaOn;
+	bool bPreBtAutoReport;
+	bool bCurBtAutoReport;
+
+	/*  sw mechanism */
+	bool bPreRfRxLpfShrink;
+	bool bCurRfRxLpfShrink;
+	u32 btRf0x1eBackup;
+	bool bPreLowPenaltyRa;
+	bool bCurLowPenaltyRa;
+	bool bPreDacSwingOn;
+	u32  preDacSwingLvl;
+	bool bCurDacSwingOn;
+	u32  curDacSwingLvl;
+	bool bPreAdcBackOff;
+	bool bCurAdcBackOff;
+	bool bPreAgcTableEn;
+	bool bCurAgcTableEn;
+	u32 preVal0x6c0;
+	u32 curVal0x6c0;
+	u32 preVal0x6c4;
+	u32 curVal0x6c4;
+	u32 preVal0x6c8;
+	u32 curVal0x6c8;
+	u8 preVal0x6cc;
+	u8 curVal0x6cc;
+	bool bLimitedDig;
+
+	/*  algorithm related */
+	u8 preAlgorithm;
+	u8 curAlgorithm;
+	u8 btStatus;
+	u8 wifiChnlInfo[3];
+
+	bool bNeedRecover0x948;
+	u32 backup0x948;
+};
+
+struct coex_sta_8723b_2ant {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	bool bPanExist;
+
+	bool bUnderLps;
+	bool bUnderIps;
+	u32 highPriorityTx;
+	u32 highPriorityRx;
+	u32 lowPriorityTx;
+	u32 lowPriorityRx;
+	u8 btRssi;
+	bool bBtTxRxMask;
+	u8 preBtRssiState;
+	u8 preWifiRssiState[4];
+	bool bC2hBtInfoReqSent;
+	u8 btInfoC2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
+	u32 btInfoC2hCnt[BT_INFO_SRC_8723B_2ANT_MAX];
+	bool bC2hBtInquiryPage;
+	u8 btRetryCnt;
+	u8 btInfoExt;
+};
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b2ant_PowerOnSetting(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b2ant_InitHwConfig(struct btc_coexist *pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b2ant_InitCoexDm(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b2ant_IpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_LpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ScanNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ConnectNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_MediaStatusNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_BtInfoNotify(
+	struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b2ant_HaltNotify(struct btc_coexist *pBtCoexist);
+void EXhalbtc8723b2ant_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState);
+void EXhalbtc8723b2ant_Periodical(struct btc_coexist *pBtCoexist);
diff --git a/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h b/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h
new file mode 100644
index 000000000..9091f2f75
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h
@@ -0,0 +1,426 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef	__HALBTC_OUT_SRC_H__
+#define __HALBTC_OUT_SRC_H__
+
+#define NORMAL_EXEC		false
+#define FORCE_EXEC		true
+
+#define BTC_RF_OFF		0x0
+#define BTC_RF_ON		0x1
+
+#define BTC_RF_A		0x0
+#define BTC_RF_B		0x1
+#define BTC_RF_C		0x2
+#define BTC_RF_D		0x3
+
+#define BTC_SMSP		SINGLEMAC_SINGLEPHY
+#define BTC_DMDP		DUALMAC_DUALPHY
+#define BTC_DMSP		DUALMAC_SINGLEPHY
+#define BTC_MP_UNKNOWN		0xff
+
+#define BT_COEX_ANT_TYPE_PG	0
+#define BT_COEX_ANT_TYPE_ANTDIV		1
+#define BT_COEX_ANT_TYPE_DETECTED	2
+
+#define BTC_MIMO_PS_STATIC	0	/*  1ss */
+#define BTC_MIMO_PS_DYNAMIC	1	/*  2ss */
+
+#define BTC_RATE_DISABLE	0
+#define BTC_RATE_ENABLE		1
+
+/*  single Antenna definition */
+#define BTC_ANT_PATH_WIFI	0
+#define BTC_ANT_PATH_BT		1
+#define BTC_ANT_PATH_PTA	2
+/*  dual Antenna definition */
+#define BTC_ANT_WIFI_AT_MAIN	0
+#define BTC_ANT_WIFI_AT_AUX	1
+/*  coupler Antenna definition */
+#define BTC_ANT_WIFI_AT_CPL_MAIN	0
+#define BTC_ANT_WIFI_AT_CPL_AUX		1
+
+enum {
+	BTC_PS_WIFI_NATIVE	= 0,	/*  wifi original power save behavior */
+	BTC_PS_LPS_ON		= 1,
+	BTC_PS_LPS_OFF		= 2,
+	BTC_PS_MAX
+};
+
+enum {
+	BTC_BT_REG_RF		= 0,
+	BTC_BT_REG_MODEM	= 1,
+	BTC_BT_REG_BLUEWIZE	= 2,
+	BTC_BT_REG_VENDOR	= 3,
+	BTC_BT_REG_LE		= 4,
+	BTC_BT_REG_MAX
+};
+
+enum btc_chip_interface {
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_MAX
+};
+
+/*  following is for wifi link status */
+#define WIFI_STA_CONNECTED				BIT0
+#define WIFI_AP_CONNECTED				BIT1
+#define WIFI_HS_CONNECTED				BIT2
+#define WIFI_P2P_GO_CONNECTED			BIT3
+#define WIFI_P2P_GC_CONNECTED			BIT4
+
+struct btc_board_info {
+	/*  The following is some board information */
+	u8 pgAntNum;	/*  pg ant number */
+	u8 btdmAntNum;	/*  ant number for btdm */
+	u8 btdmAntPos;		/* Bryant Add to indicate Antenna Position for (pgAntNum = 2) && (btdmAntNum = 1)  (DPDT+1Ant case) */
+	u8 singleAntPath;	/*  current used for 8723b only, 1 =>s0,  0 =>s1 */
+	/* bool				bBtExist; */
+};
+
+enum {
+	BTC_RSSI_STATE_HIGH			    = 0x0,
+	BTC_RSSI_STATE_MEDIUM			= 0x1,
+	BTC_RSSI_STATE_LOW			    = 0x2,
+	BTC_RSSI_STATE_STAY_HIGH		= 0x3,
+	BTC_RSSI_STATE_STAY_MEDIUM		= 0x4,
+	BTC_RSSI_STATE_STAY_LOW			= 0x5,
+	BTC_RSSI_MAX
+};
+#define BTC_RSSI_HIGH(_rssi_)	((_rssi_ == BTC_RSSI_STATE_HIGH || _rssi_ == BTC_RSSI_STATE_STAY_HIGH) ? true : false)
+#define BTC_RSSI_MEDIUM(_rssi_)	((_rssi_ == BTC_RSSI_STATE_MEDIUM || _rssi_ == BTC_RSSI_STATE_STAY_MEDIUM) ? true : false)
+#define BTC_RSSI_LOW(_rssi_)	((_rssi_ == BTC_RSSI_STATE_LOW || _rssi_ == BTC_RSSI_STATE_STAY_LOW) ? true : false)
+
+enum {
+	BTC_WIFI_BW_LEGACY			= 0x0,
+	BTC_WIFI_BW_HT20			= 0x1,
+	BTC_WIFI_BW_HT40			= 0x2,
+	BTC_WIFI_BW_MAX
+};
+
+enum {
+	BTC_WIFI_TRAFFIC_TX			= 0x0,
+	BTC_WIFI_TRAFFIC_RX			= 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+};
+
+enum {
+	BTC_WIFI_PNP_WAKE_UP		= 0x0,
+	BTC_WIFI_PNP_SLEEP			= 0x1,
+	BTC_WIFI_PNP_MAX
+};
+
+/*  defined for BFP_BTC_GET */
+enum {
+	/*  type bool */
+	BTC_GET_BL_HS_OPERATION,
+	BTC_GET_BL_HS_CONNECTING,
+	BTC_GET_BL_WIFI_CONNECTED,
+	BTC_GET_BL_WIFI_BUSY,
+	BTC_GET_BL_WIFI_SCAN,
+	BTC_GET_BL_WIFI_LINK,
+	BTC_GET_BL_WIFI_ROAM,
+	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
+	BTC_GET_BL_WIFI_UNDER_B_MODE,
+	BTC_GET_BL_EXT_SWITCH,
+	BTC_GET_BL_WIFI_IS_IN_MP_MODE,
+
+	/*  type s32 */
+	BTC_GET_S4_WIFI_RSSI,
+	BTC_GET_S4_HS_RSSI,
+
+	/*  type u32 */
+	BTC_GET_U4_WIFI_BW,
+	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_WIFI_LINK_STATUS,
+	BTC_GET_U4_BT_PATCH_VER,
+
+	/*  type u8 */
+	BTC_GET_U1_WIFI_DOT11_CHNL,
+	BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	BTC_GET_U1_WIFI_HS_CHNL,
+	BTC_GET_U1_MAC_PHY_MODE,
+	BTC_GET_U1_AP_NUM,
+
+	/*  for 1Ant ====== */
+	BTC_GET_U1_LPS_MODE,
+
+	BTC_GET_MAX
+};
+
+/*  defined for BFP_BTC_SET */
+enum {
+	/*  type bool */
+	BTC_SET_BL_BT_DISABLE,
+	BTC_SET_BL_BT_TRAFFIC_BUSY,
+	BTC_SET_BL_BT_LIMITED_DIG,
+	BTC_SET_BL_FORCE_TO_ROAM,
+	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+	BTC_SET_BL_BT_CTRL_AGG_SIZE,
+	BTC_SET_BL_INC_SCAN_DEV_NUM,
+	BTC_SET_BL_BT_TX_RX_MASK,
+
+	/*  type u8 */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
+	BTC_SET_U1_AGG_BUF_SIZE,
+
+	/*  type trigger some action */
+	BTC_SET_ACT_GET_BT_RSSI,
+	BTC_SET_ACT_AGGREGATE_CTRL,
+	/*  for 1Ant ====== */
+	/*  type bool */
+
+	/*  type u8 */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_U1_LPS_VAL,
+	BTC_SET_U1_RPWM_VAL,
+	/*  type trigger some action */
+	BTC_SET_ACT_LEAVE_LPS,
+	BTC_SET_ACT_ENTER_LPS,
+	BTC_SET_ACT_NORMAL_LPS,
+	BTC_SET_ACT_DISABLE_LOW_POWER,
+	BTC_SET_ACT_UPDATE_RAMASK,
+	BTC_SET_ACT_SEND_MIMO_PS,
+	/*  BT Coex related */
+	BTC_SET_ACT_CTRL_BT_INFO,
+	BTC_SET_ACT_CTRL_BT_COEX,
+	BTC_SET_ACT_CTRL_8723B_ANT,
+	/*  */
+	BTC_SET_MAX
+};
+
+enum {
+	BTC_DBG_DISP_COEX_STATISTICS		= 0x0,
+	BTC_DBG_DISP_BT_LINK_INFO			= 0x1,
+	BTC_DBG_DISP_FW_PWR_MODE_CMD		= 0x2,
+	BTC_DBG_DISP_MAX
+};
+
+enum {
+	BTC_IPS_LEAVE						= 0x0,
+	BTC_IPS_ENTER						= 0x1,
+	BTC_IPS_MAX
+};
+
+enum {
+	BTC_LPS_DISABLE						= 0x0,
+	BTC_LPS_ENABLE						= 0x1,
+	BTC_LPS_MAX
+};
+
+enum {
+	BTC_SCAN_FINISH						= 0x0,
+	BTC_SCAN_START						= 0x1,
+	BTC_SCAN_MAX
+};
+
+enum {
+	BTC_ASSOCIATE_FINISH				= 0x0,
+	BTC_ASSOCIATE_START					= 0x1,
+	BTC_ASSOCIATE_MAX
+};
+
+enum {
+	BTC_MEDIA_DISCONNECT				= 0x0,
+	BTC_MEDIA_CONNECT					= 0x1,
+	BTC_MEDIA_MAX
+};
+
+enum {
+	BTC_PACKET_UNKNOWN					= 0x0,
+	BTC_PACKET_DHCP						= 0x1,
+	BTC_PACKET_ARP						= 0x2,
+	BTC_PACKET_EAPOL					= 0x3,
+	BTC_PACKET_MAX
+};
+
+/* Bryant Add */
+enum {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT  = 0x2,
+};
+
+typedef u8 (*BFP_BTC_R1)(void *pBtcContext, u32 RegAddr);
+typedef u16(*BFP_BTC_R2)(void *pBtcContext, u32 RegAddr);
+typedef u32 (*BFP_BTC_R4)(void *pBtcContext, u32 RegAddr);
+typedef void (*BFP_BTC_W1)(void *pBtcContext, u32 RegAddr, u8 Data);
+typedef void(*BFP_BTC_W1_BIT_MASK)(
+	void *pBtcContext, u32 regAddr, u8 bitMask, u8 data1b
+);
+typedef void (*BFP_BTC_W2)(void *pBtcContext, u32 RegAddr, u16 Data);
+typedef void (*BFP_BTC_W4)(void *pBtcContext, u32 RegAddr, u32 Data);
+typedef void (*BFP_BTC_LOCAL_REG_W1)(void *pBtcContext, u32 RegAddr, u8 Data);
+typedef void (*BFP_BTC_SET_BB_REG)(
+	void *pBtcContext, u32 RegAddr, u32 BitMask, u32 Data
+);
+typedef u32 (*BFP_BTC_GET_BB_REG)(void *pBtcContext, u32 RegAddr, u32 BitMask);
+typedef void (*BFP_BTC_SET_RF_REG)(
+	void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask, u32 Data
+);
+typedef u32 (*BFP_BTC_GET_RF_REG)(
+	void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask
+);
+typedef void (*BFP_BTC_FILL_H2C)(
+	void *pBtcContext, u8 elementId, u32 cmdLen, u8 *pCmdBuffer
+);
+
+typedef	u8 (*BFP_BTC_GET)(void *pBtCoexist, u8 getType, void *pOutBuf);
+
+typedef	u8 (*BFP_BTC_SET)(void *pBtCoexist, u8 setType, void *pInBuf);
+typedef void (*BFP_BTC_SET_BT_REG)(
+	void *pBtcContext, u8 regType, u32 offset, u32 value
+);
+typedef u32 (*BFP_BTC_GET_BT_REG)(void *pBtcContext, u8 regType, u32 offset);
+typedef void (*BFP_BTC_DISP_DBG_MSG)(void *pBtCoexist, u8 dispType);
+
+struct btc_bt_info {
+	bool bBtDisabled;
+	u8 rssiAdjustForAgcTableOn;
+	u8 rssiAdjustFor1AntCoexType;
+	bool bPreBtCtrlAggBufSize;
+	bool bBtCtrlAggBufSize;
+	bool bRejectAggPkt;
+	bool bIncreaseScanDevNum;
+	bool bBtTxRxMask;
+	u8 preAggBufSize;
+	u8 aggBufSize;
+	bool bBtBusy;
+	bool bLimitedDig;
+	u16 btHciVer;
+	u16 btRealFwVer;
+	u8 btFwVer;
+	u32 getBtFwVerCnt;
+
+	bool bBtDisableLowPwr;
+
+	bool bBtCtrlLps;
+	bool bBtLpsOn;
+	bool bForceToRoam;	/*  for 1Ant solution */
+	u8 lpsVal;
+	u8 rpwmVal;
+	u32 raMask;
+};
+
+struct btc_stack_info {
+	bool bProfileNotified;
+	u16 hciVersion;	/*  stack hci version */
+	u8 numOfLink;
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bAclExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	u8 numOfHid;
+	bool bPanExist;
+	bool bUnknownAclExist;
+	s8 minBtRssi;
+};
+
+struct btc_bt_link_info {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bScoOnly;
+	bool bA2dpExist;
+	bool bA2dpOnly;
+	bool bHidExist;
+	bool bHidOnly;
+	bool bPanExist;
+	bool bPanOnly;
+	bool bSlaveRole;
+};
+
+struct btc_statistics {
+	u32 cntBind;
+	u32 cntPowerOn;
+	u32 cntInitHwConfig;
+	u32 cntInitCoexDm;
+	u32 cntIpsNotify;
+	u32 cntLpsNotify;
+	u32 cntScanNotify;
+	u32 cntConnectNotify;
+	u32 cntMediaStatusNotify;
+	u32 cntSpecialPacketNotify;
+	u32 cntBtInfoNotify;
+	u32 cntRfStatusNotify;
+	u32 cntPeriodical;
+	u32 cntCoexDmSwitch;
+	u32 cntStackOperationNotify;
+	u32 cntDbgCtrl;
+};
+
+struct btc_coexist {
+	bool bBinded;		/*  make sure only one adapter can bind the data context */
+	void *Adapter;		/*  default adapter */
+	struct btc_board_info boardInfo;
+	struct btc_bt_info btInfo;		/*  some bt info referenced by non-bt module */
+	struct btc_stack_info stackInfo;
+	struct btc_bt_link_info btLinkInfo;
+	enum btc_chip_interface chipInterface;
+
+	bool bInitilized;
+	bool bStopCoexDm;
+	bool bManualControl;
+	struct btc_statistics statistics;
+	u8 pwrModeVal[10];
+
+	/*  function pointers */
+	/*  io related */
+	BFP_BTC_R1 fBtcRead1Byte;
+	BFP_BTC_W1 fBtcWrite1Byte;
+	BFP_BTC_W1_BIT_MASK fBtcWrite1ByteBitMask;
+	BFP_BTC_R2 fBtcRead2Byte;
+	BFP_BTC_W2 fBtcWrite2Byte;
+	BFP_BTC_R4 fBtcRead4Byte;
+	BFP_BTC_W4 fBtcWrite4Byte;
+	BFP_BTC_LOCAL_REG_W1 fBtcWriteLocalReg1Byte;
+	/*  read/write bb related */
+	BFP_BTC_SET_BB_REG fBtcSetBbReg;
+	BFP_BTC_GET_BB_REG fBtcGetBbReg;
+
+	/*  read/write rf related */
+	BFP_BTC_SET_RF_REG fBtcSetRfReg;
+	BFP_BTC_GET_RF_REG fBtcGetRfReg;
+
+	/*  fill h2c related */
+	BFP_BTC_FILL_H2C fBtcFillH2c;
+	/*  normal get/set related */
+	BFP_BTC_GET fBtcGet;
+	BFP_BTC_SET fBtcSet;
+
+	BFP_BTC_GET_BT_REG fBtcGetBtReg;
+	BFP_BTC_SET_BT_REG fBtcSetBtReg;
+};
+
+extern struct btc_coexist GLBtCoexist;
+
+void EXhalbtcoutsrc_PowerOnSetting(struct btc_coexist *pBtCoexist);
+void EXhalbtcoutsrc_InitHwConfig(struct btc_coexist *pBtCoexist, u8 bWifiOnly);
+void EXhalbtcoutsrc_InitCoexDm(struct btc_coexist *pBtCoexist);
+void EXhalbtcoutsrc_IpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtcoutsrc_LpsNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ScanNotify(struct btc_coexist *pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ConnectNotify(struct btc_coexist *pBtCoexist, u8 action);
+void EXhalbtcoutsrc_MediaStatusNotify(
+	struct btc_coexist *pBtCoexist, enum rt_media_status mediaStatus
+);
+void EXhalbtcoutsrc_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 pktType);
+void EXhalbtcoutsrc_BtInfoNotify(
+	struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtcoutsrc_HaltNotify(struct btc_coexist *pBtCoexist);
+void EXhalbtcoutsrc_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState);
+void EXhalbtcoutsrc_Periodical(struct btc_coexist *pBtCoexist);
+void EXhalbtcoutsrc_SetChipType(u8 chipType);
+void EXhalbtcoutsrc_SetAntNum(u8 type, u8 antNum);
+void EXhalbtcoutsrc_SetSingleAntPath(u8 singleAntPath);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c
new file mode 100644
index 000000000..dd0f74b0c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c
@@ -0,0 +1,570 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#include <linux/kernel.h>
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	struct dm_odm_t *pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+		((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+		((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+		((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+		((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+		((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32 cond1 = Condition1, cond2 = Condition2;
+	u32 driver1 =
+		pDM_Odm->CutVersion << 24 |
+		pDM_Odm->SupportPlatform << 16 |
+		pDM_Odm->PackageType << 12 |
+		pDM_Odm->SupportInterface << 8  |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA << 0 |
+		pDM_Odm->TypeGPA << 8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA << 24;
+
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) != (driver1 & 0x0000F000)))
+		return false;
+	if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) != (driver1 & 0x0F000000)))
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	/*  */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		/*  BoardType of each RF path is matched */
+		if ((cond2 & bitMask) == (driver2 & bitMask))
+			return true;
+	}
+	return false;
+}
+
+static bool CheckNegative(
+	struct dm_odm_t *pDM_Odm, const u32  Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           AGC_TAB.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_AGC_TAB[] = {
+		0xC78, 0xFD000001,
+		0xC78, 0xFC010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFA030001,
+		0xC78, 0xF9040001,
+		0xC78, 0xF8050001,
+		0xC78, 0xF7060001,
+		0xC78, 0xF6070001,
+		0xC78, 0xF5080001,
+		0xC78, 0xF4090001,
+		0xC78, 0xF30A0001,
+		0xC78, 0xF20B0001,
+		0xC78, 0xF10C0001,
+		0xC78, 0xF00D0001,
+		0xC78, 0xEF0E0001,
+		0xC78, 0xEE0F0001,
+		0xC78, 0xED100001,
+		0xC78, 0xEC110001,
+		0xC78, 0xEB120001,
+		0xC78, 0xEA130001,
+		0xC78, 0xE9140001,
+		0xC78, 0xE8150001,
+		0xC78, 0xE7160001,
+		0xC78, 0xE6170001,
+		0xC78, 0xE5180001,
+		0xC78, 0xE4190001,
+		0xC78, 0xE31A0001,
+		0xC78, 0xA51B0001,
+		0xC78, 0xA41C0001,
+		0xC78, 0xA31D0001,
+		0xC78, 0x671E0001,
+		0xC78, 0x661F0001,
+		0xC78, 0x65200001,
+		0xC78, 0x64210001,
+		0xC78, 0x63220001,
+		0xC78, 0x4A230001,
+		0xC78, 0x49240001,
+		0xC78, 0x48250001,
+		0xC78, 0x47260001,
+		0xC78, 0x46270001,
+		0xC78, 0x45280001,
+		0xC78, 0x44290001,
+		0xC78, 0x432A0001,
+		0xC78, 0x422B0001,
+		0xC78, 0x292C0001,
+		0xC78, 0x282D0001,
+		0xC78, 0x272E0001,
+		0xC78, 0x262F0001,
+		0xC78, 0x0A300001,
+		0xC78, 0x09310001,
+		0xC78, 0x08320001,
+		0xC78, 0x07330001,
+		0xC78, 0x06340001,
+		0xC78, 0x05350001,
+		0xC78, 0x04360001,
+		0xC78, 0x03370001,
+		0xC78, 0x02380001,
+		0xC78, 0x01390001,
+		0xC78, 0x013A0001,
+		0xC78, 0x013B0001,
+		0xC78, 0x013C0001,
+		0xC78, 0x013D0001,
+		0xC78, 0x013E0001,
+		0xC78, 0x013F0001,
+		0xC78, 0xFC400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFA420001,
+		0xC78, 0xF9430001,
+		0xC78, 0xF8440001,
+		0xC78, 0xF7450001,
+		0xC78, 0xF6460001,
+		0xC78, 0xF5470001,
+		0xC78, 0xF4480001,
+		0xC78, 0xF3490001,
+		0xC78, 0xF24A0001,
+		0xC78, 0xF14B0001,
+		0xC78, 0xF04C0001,
+		0xC78, 0xEF4D0001,
+		0xC78, 0xEE4E0001,
+		0xC78, 0xED4F0001,
+		0xC78, 0xEC500001,
+		0xC78, 0xEB510001,
+		0xC78, 0xEA520001,
+		0xC78, 0xE9530001,
+		0xC78, 0xE8540001,
+		0xC78, 0xE7550001,
+		0xC78, 0xE6560001,
+		0xC78, 0xE5570001,
+		0xC78, 0xE4580001,
+		0xC78, 0xE3590001,
+		0xC78, 0xA65A0001,
+		0xC78, 0xA55B0001,
+		0xC78, 0xA45C0001,
+		0xC78, 0xA35D0001,
+		0xC78, 0x675E0001,
+		0xC78, 0x665F0001,
+		0xC78, 0x65600001,
+		0xC78, 0x64610001,
+		0xC78, 0x63620001,
+		0xC78, 0x62630001,
+		0xC78, 0x61640001,
+		0xC78, 0x48650001,
+		0xC78, 0x47660001,
+		0xC78, 0x46670001,
+		0xC78, 0x45680001,
+		0xC78, 0x44690001,
+		0xC78, 0x436A0001,
+		0xC78, 0x426B0001,
+		0xC78, 0x286C0001,
+		0xC78, 0x276D0001,
+		0xC78, 0x266E0001,
+		0xC78, 0x256F0001,
+		0xC78, 0x24700001,
+		0xC78, 0x09710001,
+		0xC78, 0x08720001,
+		0xC78, 0x07730001,
+		0xC78, 0x06740001,
+		0xC78, 0x05750001,
+		0xC78, 0x04760001,
+		0xC78, 0x03770001,
+		0xC78, 0x02780001,
+		0xC78, 0x01790001,
+		0xC78, 0x017A0001,
+		0xC78, 0x017B0001,
+		0xC78, 0x017C0001,
+		0xC78, 0x017D0001,
+		0xC78, 0x017E0001,
+		0xC78, 0x017F0001,
+		0xC50, 0x69553422,
+		0xC50, 0x69553420,
+		0x824, 0x00390204,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_AGC_TAB(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = ARRAY_SIZE(Array_MP_8723B_AGC_TAB);
+	u32 *Array = Array_MP_8723B_AGC_TAB;
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigBB_AGC_8723B(pDM_Odm, v1, bMaskDWord, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (!bMatched) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else {
+				/*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigBB_AGC_8723B(pDM_Odm, v1, bMaskDWord, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           PHY_REG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_PHY_REG[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02200385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00190204,
+		0x828, 0x00000000,
+		0x82C, 0x00000000,
+		0x830, 0x00000000,
+		0x834, 0x00000000,
+		0x838, 0x00000000,
+		0x83C, 0x00000000,
+		0x840, 0x00010000,
+		0x844, 0x00000000,
+		0x848, 0x00000000,
+		0x84C, 0x00000000,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x569A11A9,
+		0x85C, 0x01000014,
+		0x860, 0x66F60110,
+		0x864, 0x061F0649,
+		0x868, 0x00000000,
+		0x86C, 0x27272700,
+		0x870, 0x07000760,
+		0x874, 0x25004000,
+		0x878, 0x00000808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCCC000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x89C, 0x00706050,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121111,
+		0x910, 0x00000002,
+		0x914, 0x00000201,
+		0xA00, 0x00D047C8,
+		0xA04, 0x80FF800C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F120F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x21806490,
+		0xB2C, 0x00000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05611,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC44,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+		0xC50, 0x69553420,
+		0xC54, 0x43BC0094,
+		0xC58, 0x00013149,
+		0xC5C, 0x00250492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x2C7F000D,
+		0xC74, 0x020610DB,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+		0xC80, 0x390000E4,
+		0xC84, 0x20F60000,
+		0xC88, 0x40000100,
+		0xC8C, 0x20200000,
+		0xC90, 0x00020E1A,
+		0xC94, 0x00000000,
+		0xC98, 0x00020E1A,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00000000,
+		0xCE8, 0x37644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00000740,
+		0xD04, 0x40020401,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC53,
+		0xD18, 0x7A8F5B6F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xE00, 0x2D2D2D2D,
+		0xE04, 0x2D2D2D2D,
+		0xE08, 0x0390272D,
+		0xE10, 0x2D2D2D2D,
+		0xE14, 0x2D2D2D2D,
+		0xE18, 0x2D2D2D2D,
+		0xE1C, 0x2D2D2D2D,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x001B2556,
+		0xE6C, 0x00C00096,
+		0xE70, 0x00C00096,
+		0xE74, 0x01000056,
+		0xE78, 0x01000014,
+		0xE7C, 0x01000056,
+		0xE80, 0x01000014,
+		0xE84, 0x00C00096,
+		0xE88, 0x01000056,
+		0xE8C, 0x00C00096,
+		0xED0, 0x00C00096,
+		0xED4, 0x00C00096,
+		0xED8, 0x00C00096,
+		0xEDC, 0x000000D6,
+		0xEE0, 0x000000D6,
+		0xEEC, 0x01C00016,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+		0x820, 0x01000100,
+		0x800, 0x83040000,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_PHY_REG(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = ARRAY_SIZE(Array_MP_8723B_PHY_REG);
+	u32 *Array = Array_MP_8723B_PHY_REG;
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigBB_PHY_8723B(pDM_Odm, v1, bMaskDWord, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (!bMatched) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigBB_PHY_8723B(pDM_Odm, v1, bMaskDWord, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_PHY_REG_PG[] = {
+	0, 0x00000e08, 0x0000ff00, 0x00003800,
+	0, 0x0000086c, 0xffffff00, 0x32343600,
+	0, 0x00000e00, 0xffffffff, 0x40424444,
+	0, 0x00000e04, 0xffffffff, 0x28323638,
+	0, 0x00000e10, 0xffffffff, 0x38404244,
+	0, 0x00000e14, 0xffffffff, 0x26303436
+};
+
+void ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u32 *Array = Array_MP_8723B_PHY_REG_PG;
+
+	pDM_Odm->PhyRegPgVersion = 1;
+	pDM_Odm->PhyRegPgValueType = PHY_REG_PG_EXACT_VALUE;
+
+	for (i = 0; i < ARRAY_SIZE(Array_MP_8723B_PHY_REG_PG); i += 4) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+		u32 v3 = Array[i+2];
+		u32 v4 = Array[i+3];
+
+		odm_ConfigBB_PHY_REG_PG_8723B(pDM_Odm, v1, v2, v3, v4);
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h
new file mode 100644
index 000000000..186007ce5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_BB_HW_IMG_8723B_H
+#define __INC_MP_BB_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           AGC_TAB.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_AGC_TAB(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_PHY_REG_PG(void);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.c b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.c
new file mode 100644
index 000000000..47e66f4ad
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.c
@@ -0,0 +1,245 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#include <linux/kernel.h>
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	struct dm_odm_t *pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+		((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+		((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+		((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+		((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+		((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32   cond1   = Condition1, cond2 = Condition2;
+	u32    driver1 =
+		pDM_Odm->CutVersion       << 24 |
+		pDM_Odm->SupportPlatform  << 16 |
+		pDM_Odm->PackageType      << 12 |
+		pDM_Odm->SupportInterface << 8  |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA <<  0 |
+		pDM_Odm->TypeGPA  <<  8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA  << 24;
+
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) != (driver1 & 0x0000F000)))
+		return false;
+	if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) != (driver1 & 0x0F000000)))
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	/*  */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		if ((cond2 & bitMask) == (driver2 & bitMask)) /*  BoardType of each RF path is matched */
+			return true;
+	}
+	return false;
+}
+
+static bool CheckNegative(
+	struct dm_odm_t *pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_MAC_REG[] = {
+		0x02F, 0x00000030,
+		0x035, 0x00000000,
+		0x039, 0x00000008,
+		0x04E, 0x000000E0,
+		0x064, 0x00000000,
+		0x067, 0x00000020,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000000,
+		0x432, 0x00000000,
+		0x433, 0x00000001,
+		0x434, 0x00000004,
+		0x435, 0x00000005,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000007,
+		0x43F, 0x00000008,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000010,
+		0x445, 0x00000000,
+		0x446, 0x00000000,
+		0x447, 0x00000000,
+		0x448, 0x00000000,
+		0x449, 0x000000F0,
+		0x44A, 0x0000000F,
+		0x44B, 0x0000003E,
+		0x44C, 0x00000010,
+		0x44D, 0x00000000,
+		0x44E, 0x00000000,
+		0x44F, 0x00000000,
+		0x450, 0x00000000,
+		0x451, 0x000000F0,
+		0x452, 0x0000000F,
+		0x453, 0x00000000,
+		0x456, 0x0000005E,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x638, 0x00000050,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x642, 0x00000040,
+		0x643, 0x00000000,
+		0x652, 0x000000C8,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+		0x765, 0x00000018,
+		0x76E, 0x00000004,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_MAC_REG(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = ARRAY_SIZE(Array_MP_8723B_MAC_REG);
+	u32 *Array = Array_MP_8723B_MAC_REG;
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigMAC_8723B(pDM_Odm, v1, (u8)v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (!bMatched) {
+				/*  Condition isn't matched. Discard the following (offset, data) pairs. */
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigMAC_8723B(pDM_Odm, v1, (u8)v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.h b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.h
new file mode 100644
index 000000000..50429c159
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_MAC_HW_IMG_8723B_H
+#define __INC_MP_MAC_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_MAC_REG(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.c b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.c
new file mode 100644
index 000000000..efc68c17b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.c
@@ -0,0 +1,564 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#include <linux/kernel.h>
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	struct dm_odm_t *pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+			((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+			((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+			((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+			((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+			((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32 cond1 = Condition1, cond2 = Condition2;
+	u32 driver1 =
+		pDM_Odm->CutVersion << 24 |
+		pDM_Odm->SupportPlatform << 16 |
+		pDM_Odm->PackageType << 12 |
+		pDM_Odm->SupportInterface << 8 |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA <<  0 |
+		pDM_Odm->TypeGPA  <<  8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA  << 24;
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (
+		((cond1 & 0x0000F000) != 0) &&
+		((cond1 & 0x0000F000) != (driver1 & 0x0000F000))
+	)
+		return false;
+
+	if (
+		((cond1 & 0x0F000000) != 0) &&
+		((cond1 & 0x0F000000) != (driver1 & 0x0F000000))
+	)
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		/*  BoardType of each RF path is matched */
+		if ((cond2 & bitMask) == (driver2 & bitMask))
+			return true;
+
+		return false;
+	}
+
+	return false;
+}
+
+static bool CheckNegative(
+	struct dm_odm_t *pDM_Odm, const u32  Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           RadioA.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_RadioA[] = {
+		0x000, 0x00010000,
+		0x0B0, 0x000DFFE0,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B1, 0x00000018,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B2, 0x00084C00,
+		0x0B5, 0x0000D2CC,
+		0x0B6, 0x000925AA,
+		0x0B7, 0x00000010,
+		0x0B8, 0x0000907F,
+		0x05C, 0x00000002,
+		0x07C, 0x00000002,
+		0x07E, 0x00000005,
+		0x08B, 0x0006FC00,
+		0x0B0, 0x000FF9F0,
+		0x01C, 0x000739D2,
+		0x01E, 0x00000000,
+		0x0DF, 0x00000780,
+		0x050, 0x00067435,
+	0x80002000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0x90003000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0x90004000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0xA0000000, 0x00000000,
+		0x051, 0x0006B04E,
+	0xB0000000, 0x00000000,
+		0x052, 0x000007D2,
+		0x053, 0x00000000,
+		0x054, 0x00050400,
+		0x055, 0x0004026E,
+		0x0DD, 0x0000004C,
+		0x070, 0x00067435,
+	0x80002000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0x90003000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0x90004000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0xA0000000, 0x00000000,
+		0x071, 0x0006B04E,
+	0xB0000000, 0x00000000,
+		0x072, 0x000007D2,
+		0x073, 0x00000000,
+		0x074, 0x00050400,
+		0x075, 0x0004026E,
+		0x0EF, 0x00000100,
+		0x034, 0x0000ADD7,
+		0x035, 0x00005C00,
+		0x034, 0x00009DD4,
+		0x035, 0x00005000,
+		0x034, 0x00008DD1,
+		0x035, 0x00004400,
+		0x034, 0x00007DCE,
+		0x035, 0x00003800,
+		0x034, 0x00006CD1,
+		0x035, 0x00004400,
+		0x034, 0x00005CCE,
+		0x035, 0x00003800,
+		0x034, 0x000048CE,
+		0x035, 0x00004400,
+		0x034, 0x000034CE,
+		0x035, 0x00003800,
+		0x034, 0x00002451,
+		0x035, 0x00004400,
+		0x034, 0x0000144E,
+		0x035, 0x00003800,
+		0x034, 0x00000051,
+		0x035, 0x00004400,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000100,
+		0x0ED, 0x00000010,
+		0x044, 0x0000ADD7,
+		0x044, 0x00009DD4,
+		0x044, 0x00008DD1,
+		0x044, 0x00007DCE,
+		0x044, 0x00006CC1,
+		0x044, 0x00005CCE,
+		0x044, 0x000044D1,
+		0x044, 0x000034CE,
+		0x044, 0x00002451,
+		0x044, 0x0000144E,
+		0x044, 0x00000051,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000000,
+		0x07F, 0x00020080,
+		0x0EF, 0x00002000,
+		0x03B, 0x000380EF,
+		0x03B, 0x000302FE,
+		0x03B, 0x00028CE6,
+		0x03B, 0x000200BC,
+		0x03B, 0x000188A5,
+		0x03B, 0x00010FBC,
+		0x03B, 0x00008F71,
+		0x03B, 0x00000900,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000001,
+		0x040, 0x000380EF,
+		0x040, 0x000302FE,
+		0x040, 0x00028CE6,
+		0x040, 0x000200BC,
+		0x040, 0x000188A5,
+		0x040, 0x00010FBC,
+		0x040, 0x00008F71,
+		0x040, 0x00000900,
+		0x0ED, 0x00000000,
+		0x082, 0x00080000,
+		0x083, 0x00008000,
+		0x084, 0x00048D80,
+		0x085, 0x00068000,
+		0x0A2, 0x00080000,
+		0x0A3, 0x00008000,
+		0x0A4, 0x00048D80,
+		0x0A5, 0x00068000,
+		0x0ED, 0x00000002,
+		0x0EF, 0x00000002,
+		0x056, 0x00000032,
+		0x076, 0x00000032,
+		0x001, 0x00000780,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_RadioA(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = ARRAY_SIZE(Array_MP_8723B_RadioA);
+	u32 *Array = Array_MP_8723B_RadioA;
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigRF_RadioA_8723B(pDM_Odm, v1, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (!bMatched) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else {
+				/*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigRF_RadioA_8723B(pDM_Odm, v1, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           TxPowerTrack_SDIO.TXT
+******************************************************************************/
+
+static u8 gDeltaSwingTableIdx_MP_2GB_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6, 6,  6,
+	7,  7,  7, 8,  8,  9,  9, 10, 10, 11, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GB_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 5, 5,  6,  6,  7,  7,  8,  8,
+	9,  9, 10, 10, 10, 11, 11, 12, 12, 13, 13, 14, 15, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GA_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  6,  6,
+	7,  7,  7,  8,  8,  9,  9, 10, 10, 11, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GA_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 5, 5,  6,  6,  7,  7,  8,  8,
+	9,  9, 10, 10, 10, 11, 11, 12, 12, 13, 13, 14, 15, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKB_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  7,  8,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKB_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  7,  7,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKA_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  7,  8,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKA_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  7,  7,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+
+void ODM_ReadAndConfig_MP_8723B_TxPowerTrack_SDIO(struct dm_odm_t *pDM_Odm)
+{
+	struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P,
+		gDeltaSwingTableIdx_MP_2GA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N,
+		gDeltaSwingTableIdx_MP_2GA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P,
+		gDeltaSwingTableIdx_MP_2GB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N,
+		gDeltaSwingTableIdx_MP_2GB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P,
+		gDeltaSwingTableIdx_MP_2GCCKA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N,
+		gDeltaSwingTableIdx_MP_2GCCKA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P,
+		gDeltaSwingTableIdx_MP_2GCCKB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N,
+		gDeltaSwingTableIdx_MP_2GCCKB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+}
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+static u8 *Array_MP_8723B_TXPWR_LMT[] = {
+	"FCC", "20M", "CCK", "1T", "01", "32",
+	"ETSI", "20M", "CCK", "1T", "01", "32",
+	"MKK", "20M", "CCK", "1T", "01", "32",
+	"FCC", "20M", "CCK", "1T", "02", "32",
+	"ETSI", "20M", "CCK", "1T", "02", "32",
+	"MKK", "20M", "CCK", "1T", "02", "32",
+	"FCC", "20M", "CCK", "1T", "03", "32",
+	"ETSI", "20M", "CCK", "1T", "03", "32",
+	"MKK", "20M", "CCK", "1T", "03", "32",
+	"FCC", "20M", "CCK", "1T", "04", "32",
+	"ETSI", "20M", "CCK", "1T", "04", "32",
+	"MKK", "20M", "CCK", "1T", "04", "32",
+	"FCC", "20M", "CCK", "1T", "05", "32",
+	"ETSI", "20M", "CCK", "1T", "05", "32",
+	"MKK", "20M", "CCK", "1T", "05", "32",
+	"FCC", "20M", "CCK", "1T", "06", "32",
+	"ETSI", "20M", "CCK", "1T", "06", "32",
+	"MKK", "20M", "CCK", "1T", "06", "32",
+	"FCC", "20M", "CCK", "1T", "07", "32",
+	"ETSI", "20M", "CCK", "1T", "07", "32",
+	"MKK", "20M", "CCK", "1T", "07", "32",
+	"FCC", "20M", "CCK", "1T", "08", "32",
+	"ETSI", "20M", "CCK", "1T", "08", "32",
+	"MKK", "20M", "CCK", "1T", "08", "32",
+	"FCC", "20M", "CCK", "1T", "09", "32",
+	"ETSI", "20M", "CCK", "1T", "09", "32",
+	"MKK", "20M", "CCK", "1T", "09", "32",
+	"FCC", "20M", "CCK", "1T", "10", "32",
+	"ETSI", "20M", "CCK", "1T", "10", "32",
+	"MKK", "20M", "CCK", "1T", "10", "32",
+	"FCC", "20M", "CCK", "1T", "11", "32",
+	"ETSI", "20M", "CCK", "1T", "11", "32",
+	"MKK", "20M", "CCK", "1T", "11", "32",
+	"FCC", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "20M", "CCK", "1T", "12", "32",
+	"MKK", "20M", "CCK", "1T", "12", "32",
+	"FCC", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "20M", "CCK", "1T", "13", "32",
+	"MKK", "20M", "CCK", "1T", "13", "32",
+	"FCC", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "20M", "CCK", "1T", "14", "63",
+	"MKK", "20M", "CCK", "1T", "14", "32",
+	"FCC", "20M", "OFDM", "1T", "01", "28",
+	"ETSI", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "20M", "OFDM", "1T", "02", "28",
+	"ETSI", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "20M", "OFDM", "1T", "03", "32",
+	"ETSI", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "20M", "OFDM", "1T", "04", "32",
+	"ETSI", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "20M", "OFDM", "1T", "05", "32",
+	"ETSI", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "20M", "OFDM", "1T", "06", "32",
+	"ETSI", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "20M", "OFDM", "1T", "07", "32",
+	"ETSI", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "20M", "OFDM", "1T", "08", "32",
+	"ETSI", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "20M", "OFDM", "1T", "09", "32",
+	"ETSI", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "20M", "OFDM", "1T", "10", "28",
+	"ETSI", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "20M", "OFDM", "1T", "11", "28",
+	"ETSI", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "20M", "HT", "1T", "01", "26",
+	"ETSI", "20M", "HT", "1T", "01", "32",
+	"MKK", "20M", "HT", "1T", "01", "32",
+	"FCC", "20M", "HT", "1T", "02", "26",
+	"ETSI", "20M", "HT", "1T", "02", "32",
+	"MKK", "20M", "HT", "1T", "02", "32",
+	"FCC", "20M", "HT", "1T", "03", "32",
+	"ETSI", "20M", "HT", "1T", "03", "32",
+	"MKK", "20M", "HT", "1T", "03", "32",
+	"FCC", "20M", "HT", "1T", "04", "32",
+	"ETSI", "20M", "HT", "1T", "04", "32",
+	"MKK", "20M", "HT", "1T", "04", "32",
+	"FCC", "20M", "HT", "1T", "05", "32",
+	"ETSI", "20M", "HT", "1T", "05", "32",
+	"MKK", "20M", "HT", "1T", "05", "32",
+	"FCC", "20M", "HT", "1T", "06", "32",
+	"ETSI", "20M", "HT", "1T", "06", "32",
+	"MKK", "20M", "HT", "1T", "06", "32",
+	"FCC", "20M", "HT", "1T", "07", "32",
+	"ETSI", "20M", "HT", "1T", "07", "32",
+	"MKK", "20M", "HT", "1T", "07", "32",
+	"FCC", "20M", "HT", "1T", "08", "32",
+	"ETSI", "20M", "HT", "1T", "08", "32",
+	"MKK", "20M", "HT", "1T", "08", "32",
+	"FCC", "20M", "HT", "1T", "09", "32",
+	"ETSI", "20M", "HT", "1T", "09", "32",
+	"MKK", "20M", "HT", "1T", "09", "32",
+	"FCC", "20M", "HT", "1T", "10", "26",
+	"ETSI", "20M", "HT", "1T", "10", "32",
+	"MKK", "20M", "HT", "1T", "10", "32",
+	"FCC", "20M", "HT", "1T", "11", "26",
+	"ETSI", "20M", "HT", "1T", "11", "32",
+	"MKK", "20M", "HT", "1T", "11", "32",
+	"FCC", "20M", "HT", "1T", "12", "63",
+	"ETSI", "20M", "HT", "1T", "12", "32",
+	"MKK", "20M", "HT", "1T", "12", "32",
+	"FCC", "20M", "HT", "1T", "13", "63",
+	"ETSI", "20M", "HT", "1T", "13", "32",
+	"MKK", "20M", "HT", "1T", "13", "32",
+	"FCC", "20M", "HT", "1T", "14", "63",
+	"ETSI", "20M", "HT", "1T", "14", "63",
+	"MKK", "20M", "HT", "1T", "14", "63",
+	"FCC", "40M", "HT", "1T", "01", "63",
+	"ETSI", "40M", "HT", "1T", "01", "63",
+	"MKK", "40M", "HT", "1T", "01", "63",
+	"FCC", "40M", "HT", "1T", "02", "63",
+	"ETSI", "40M", "HT", "1T", "02", "63",
+	"MKK", "40M", "HT", "1T", "02", "63",
+	"FCC", "40M", "HT", "1T", "03", "26",
+	"ETSI", "40M", "HT", "1T", "03", "32",
+	"MKK", "40M", "HT", "1T", "03", "32",
+	"FCC", "40M", "HT", "1T", "04", "26",
+	"ETSI", "40M", "HT", "1T", "04", "32",
+	"MKK", "40M", "HT", "1T", "04", "32",
+	"FCC", "40M", "HT", "1T", "05", "32",
+	"ETSI", "40M", "HT", "1T", "05", "32",
+	"MKK", "40M", "HT", "1T", "05", "32",
+	"FCC", "40M", "HT", "1T", "06", "32",
+	"ETSI", "40M", "HT", "1T", "06", "32",
+	"MKK", "40M", "HT", "1T", "06", "32",
+	"FCC", "40M", "HT", "1T", "07", "32",
+	"ETSI", "40M", "HT", "1T", "07", "32",
+	"MKK", "40M", "HT", "1T", "07", "32",
+	"FCC", "40M", "HT", "1T", "08", "26",
+	"ETSI", "40M", "HT", "1T", "08", "32",
+	"MKK", "40M", "HT", "1T", "08", "32",
+	"FCC", "40M", "HT", "1T", "09", "26",
+	"ETSI", "40M", "HT", "1T", "09", "32",
+	"MKK", "40M", "HT", "1T", "09", "32",
+	"FCC", "40M", "HT", "1T", "10", "26",
+	"ETSI", "40M", "HT", "1T", "10", "32",
+	"MKK", "40M", "HT", "1T", "10", "32",
+	"FCC", "40M", "HT", "1T", "11", "26",
+	"ETSI", "40M", "HT", "1T", "11", "32",
+	"MKK", "40M", "HT", "1T", "11", "32",
+	"FCC", "40M", "HT", "1T", "12", "63",
+	"ETSI", "40M", "HT", "1T", "12", "32",
+	"MKK", "40M", "HT", "1T", "12", "32",
+	"FCC", "40M", "HT", "1T", "13", "63",
+	"ETSI", "40M", "HT", "1T", "13", "32",
+	"MKK", "40M", "HT", "1T", "13", "32",
+	"FCC", "40M", "HT", "1T", "14", "63",
+	"ETSI", "40M", "HT", "1T", "14", "63",
+	"MKK", "40M", "HT", "1T", "14", "63"
+};
+
+void ODM_ReadAndConfig_MP_8723B_TXPWR_LMT(struct dm_odm_t *pDM_Odm)
+{
+	u32 i = 0;
+	u8 **Array = Array_MP_8723B_TXPWR_LMT;
+
+	for (i = 0; i < ARRAY_SIZE(Array_MP_8723B_TXPWR_LMT); i += 6) {
+		u8 *regulation = Array[i];
+		u8 *bandwidth = Array[i+1];
+		u8 *rate = Array[i+2];
+		u8 *rfPath = Array[i+3];
+		u8 *chnl = Array[i+4];
+		u8 *val = Array[i+5];
+
+		odm_ConfigBB_TXPWR_LMT_8723B(
+			pDM_Odm,
+			regulation,
+			bandwidth,
+			rate,
+			rfPath,
+			chnl,
+			val
+		);
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.h b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.h
new file mode 100644
index 000000000..acf5679d1
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_RF_HW_IMG_8723B_H
+#define __INC_MP_RF_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           RadioA.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_RadioA(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+
+/******************************************************************************
+*                           TxPowerTrack_SDIO.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_TxPowerTrack_SDIO(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_TxPowerTrack_SDIO(void);
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_TXPWR_LMT(/*  TC: Test Chip, MP: MP Chip */
+	struct dm_odm_t *pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_TXPWR_LMT(void);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf.c b/drivers/staging/rtl8723bs/hal/HalPhyRf.c
new file mode 100644
index 000000000..7bef05a9a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf.c
@@ -0,0 +1,273 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+/* include "Mp_Precomp.h" */
+#include "odm_precomp.h"
+
+void ConfigureTxpowerTrack(struct dm_odm_t *pDM_Odm, struct txpwrtrack_cfg *pConfig)
+{
+	ConfigureTxpowerTrack_8723B(pConfig);
+}
+
+/*  */
+/*  <20121113, Kordan> This function should be called when TxAGC changed. */
+/*  Otherwise the previous compensation is gone, because we record the */
+/*  delta of temperature between two TxPowerTracking watch dogs. */
+/*  */
+/*  NOTE: If Tx BB swing or Tx scaling is varified during run-time, still */
+/*        need to call this function. */
+/*  */
+void ODM_ClearTxPowerTrackingState(struct dm_odm_t *pDM_Odm)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(pDM_Odm->Adapter);
+	u8 p = 0;
+
+	pDM_Odm->BbSwingIdxCckBase = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->BbSwingIdxCck = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->RFCalibrateInfo.CCK_index = 0;
+
+	for (p = RF_PATH_A; p < MAX_RF_PATH; ++p) {
+		pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->BbSwingIdxOfdm[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.OFDM_index[p] = pDM_Odm->DefaultOfdmIndex;
+
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p] = 0;
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+
+		/*  Initial Mix mode power tracking */
+		pDM_Odm->Absolute_OFDMSwingIdx[p] = 0;
+		pDM_Odm->Remnant_OFDMSwingIdx[p] = 0;
+	}
+
+	/* Initial at Modify Tx Scaling Mode */
+	pDM_Odm->Modify_TxAGC_Flag_PathA = false;
+	/* Initial at Modify Tx Scaling Mode */
+	pDM_Odm->Modify_TxAGC_Flag_PathB = false;
+	pDM_Odm->Remnant_CCKSwingIdx = 0;
+	pDM_Odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_IQK = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = pHalData->EEPROMThermalMeter;
+}
+
+void ODM_TXPowerTrackingCallback_ThermalMeter(struct adapter *Adapter)
+{
+
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	u8 ThermalValue = 0, delta, delta_LCK, p = 0, i = 0;
+	u8 ThermalValue_AVG_count = 0;
+	u32 ThermalValue_AVG = 0;
+
+	u8 OFDM_min_index = 0;  /*  OFDM BB Swing should be less than +3.0dB, which is required by Arthur */
+	u8 Indexforchannel = 0; /*  GetRightChnlPlaceforIQK(pHalData->CurrentChannel) */
+
+	struct txpwrtrack_cfg c;
+
+
+	/* 4 1. The following TWO tables decide the final index of OFDM/CCK swing table. */
+	u8 *deltaSwingTableIdx_TUP_A;
+	u8 *deltaSwingTableIdx_TDOWN_A;
+	u8 *deltaSwingTableIdx_TUP_B;
+	u8 *deltaSwingTableIdx_TDOWN_B;
+
+	/* 4 2. Initialization (7 steps in total) */
+
+	ConfigureTxpowerTrack(pDM_Odm, &c);
+
+	(*c.GetDeltaSwingTable)(
+		pDM_Odm,
+		(u8 **)&deltaSwingTableIdx_TUP_A,
+		(u8 **)&deltaSwingTableIdx_TDOWN_A,
+		(u8 **)&deltaSwingTableIdx_TUP_B,
+		(u8 **)&deltaSwingTableIdx_TDOWN_B
+	);
+
+	/* cosa add for debug */
+	pDM_Odm->RFCalibrateInfo.TXPowerTrackingCallbackCnt++;
+	pDM_Odm->RFCalibrateInfo.bTXPowerTrackingInit = true;
+
+	ThermalValue = (u8)PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH_A, c.ThermalRegAddr, 0xfc00);	/* 0x42: RF Reg[15:10] 88E */
+	if (
+		!pDM_Odm->RFCalibrateInfo.TxPowerTrackControl ||
+		pHalData->EEPROMThermalMeter == 0 ||
+		pHalData->EEPROMThermalMeter == 0xFF
+	)
+		return;
+
+	/* 4 3. Initialize ThermalValues of RFCalibrateInfo */
+
+	/* 4 4. Calculate average thermal meter */
+
+	pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index] = ThermalValue;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index++;
+	if (pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index == c.AverageThermalNum)   /* Average times =  c.AverageThermalNum */
+		pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index = 0;
+
+	for (i = 0; i < c.AverageThermalNum; i++) {
+		if (pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[i]) {
+			ThermalValue_AVG += pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[i];
+			ThermalValue_AVG_count++;
+		}
+	}
+
+	/* Calculate Average ThermalValue after average enough times */
+	if (ThermalValue_AVG_count) {
+		ThermalValue = (u8)(ThermalValue_AVG / ThermalValue_AVG_count);
+	}
+
+	/* 4 5. Calculate delta, delta_LCK */
+	/* delta" here is used to determine whether thermal value changes or not. */
+	delta =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue - ThermalValue);
+	delta_LCK =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue_LCK) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue_LCK) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue_LCK - ThermalValue);
+
+	/* 4 6. If necessary, do LCK. */
+	/*  Delta temperature is equal to or larger than 20 centigrade. */
+	if (delta_LCK >= c.Threshold_IQK) {
+		pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
+		if (c.PHY_LCCalibrate)
+			(*c.PHY_LCCalibrate)(pDM_Odm);
+	}
+
+	/* 3 7. If necessary, move the index of swing table to adjust Tx power. */
+	if (delta > 0 && pDM_Odm->RFCalibrateInfo.TxPowerTrackControl) {
+		/* delta" here is used to record the absolute value of difference. */
+		delta =
+			ThermalValue > pHalData->EEPROMThermalMeter ?
+			(ThermalValue - pHalData->EEPROMThermalMeter) :
+			(pHalData->EEPROMThermalMeter - ThermalValue);
+
+		if (delta >= TXPWR_TRACK_TABLE_SIZE)
+			delta = TXPWR_TRACK_TABLE_SIZE - 1;
+
+		/* 4 7.1 The Final Power Index = BaseIndex + PowerIndexOffset */
+		if (ThermalValue > pHalData->EEPROMThermalMeter) {
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			if (c.RfPathCount > 1) {
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+
+				/*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+			}
+
+		} else {
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			if (c.RfPathCount > 1) {
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+
+				 /*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+			}
+		}
+
+		for (p = RF_PATH_A; p < c.RfPathCount; p++) {
+			if (
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] ==
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p]
+			) /*  If Thermal value changes but lookup table value still the same */
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+			else
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] - pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p];      /*  Power Index Diff between 2 times Power Tracking */
+
+			pDM_Odm->RFCalibrateInfo.OFDM_index[p] =
+				pDM_Odm->BbSwingIdxOfdmBase[p] +
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p];
+
+			pDM_Odm->RFCalibrateInfo.CCK_index =
+				pDM_Odm->BbSwingIdxCckBase +
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p];
+
+			pDM_Odm->BbSwingIdxCck =
+				pDM_Odm->RFCalibrateInfo.CCK_index;
+
+			pDM_Odm->BbSwingIdxOfdm[p] =
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p];
+
+			/* 4 7.1 Handle boundary conditions of index. */
+			if (pDM_Odm->RFCalibrateInfo.OFDM_index[p] > c.SwingTableSize_OFDM-1)
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p] = c.SwingTableSize_OFDM-1;
+			else if (pDM_Odm->RFCalibrateInfo.OFDM_index[p] < OFDM_min_index)
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p] = OFDM_min_index;
+		}
+		if (pDM_Odm->RFCalibrateInfo.CCK_index > c.SwingTableSize_CCK-1)
+			pDM_Odm->RFCalibrateInfo.CCK_index = c.SwingTableSize_CCK-1;
+		/* else if (pDM_Odm->RFCalibrateInfo.CCK_index < 0) */
+			/* pDM_Odm->RFCalibrateInfo.CCK_index = 0; */
+	} else {
+			for (p = RF_PATH_A; p < c.RfPathCount; p++)
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+	}
+
+	/* Print Swing base & current */
+	for (p = RF_PATH_A; p < c.RfPathCount; p++) {
+	}
+
+	if (
+		(pDM_Odm->RFCalibrateInfo.PowerIndexOffset[RF_PATH_A] != 0 ||
+		 pDM_Odm->RFCalibrateInfo.PowerIndexOffset[RF_PATH_B] != 0) &&
+		 pDM_Odm->RFCalibrateInfo.TxPowerTrackControl
+	 ) {
+		/* 4 7.2 Configure the Swing Table to adjust Tx Power. */
+
+		pDM_Odm->RFCalibrateInfo.bTxPowerChanged = true; /*  Always true after Tx Power is adjusted by power tracking. */
+		/*  */
+		/*  2012/04/23 MH According to Luke's suggestion, we can not write BB digital */
+		/*  to increase TX power. Otherwise, EVM will be bad. */
+		/*  */
+		/*  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E. */
+
+		if (ThermalValue > pHalData->EEPROMThermalMeter) {
+			for (p = RF_PATH_A; p < c.RfPathCount; p++)
+					(*c.ODM_TxPwrTrackSetPwr)(pDM_Odm, MIX_MODE, p, 0);
+		} else {
+			for (p = RF_PATH_A; p < c.RfPathCount; p++)
+				(*c.ODM_TxPwrTrackSetPwr)(pDM_Odm, MIX_MODE, p, Indexforchannel);
+		}
+
+		/*  Record last time Power Tracking result as base. */
+		pDM_Odm->BbSwingIdxCckBase = pDM_Odm->BbSwingIdxCck;
+		for (p = RF_PATH_A; p < c.RfPathCount; p++)
+			pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->BbSwingIdxOfdm[p];
+
+		/* Record last Power Tracking Thermal Value */
+		pDM_Odm->RFCalibrateInfo.ThermalValue = ThermalValue;
+	}
+
+	pDM_Odm->RFCalibrateInfo.TXPowercount = 0;
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf.h b/drivers/staging/rtl8723bs/hal/HalPhyRf.h
new file mode 100644
index 000000000..fdbdd68ed
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+ #ifndef __HAL_PHY_RF_H__
+ #define __HAL_PHY_RF_H__
+
+enum pwrtrack_method {
+	BBSWING,
+	TXAGC,
+	MIX_MODE
+};
+
+typedef void (*FuncSetPwr)(struct dm_odm_t *, enum pwrtrack_method, u8, u8);
+typedef void (*FuncIQK)(struct dm_odm_t *, u8, u8, u8);
+typedef void (*FuncLCK)(struct dm_odm_t *);
+typedef void (*FuncSwing)(struct dm_odm_t *, u8 **, u8 **, u8 **, u8 **);
+
+struct txpwrtrack_cfg {
+	u8 SwingTableSize_CCK;
+	u8 SwingTableSize_OFDM;
+	u8 Threshold_IQK;
+	u8 AverageThermalNum;
+	u8 RfPathCount;
+	u32 ThermalRegAddr;
+	FuncSetPwr ODM_TxPwrTrackSetPwr;
+	FuncIQK DoIQK;
+	FuncLCK PHY_LCCalibrate;
+	FuncSwing GetDeltaSwingTable;
+};
+
+void ConfigureTxpowerTrack(struct dm_odm_t *pDM_Odm, struct txpwrtrack_cfg *pConfig);
+
+
+void ODM_ClearTxPowerTrackingState(struct dm_odm_t *pDM_Odm);
+
+void ODM_TXPowerTrackingCallback_ThermalMeter(struct adapter *Adapter);
+
+#endif	/*  #ifndef __HAL_PHY_RF_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.c b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.c
new file mode 100644
index 000000000..a52748f7b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.c
@@ -0,0 +1,1822 @@
+// SPDX-License-Identifier: GPL-2.0
+/*****************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include "odm_precomp.h"
+
+/*  MACRO definition for pRFCalibrateInfo->TxIQC_8723B[0] */
+#define		PATH_S0							1 /*  RF_PATH_B */
+#define		IDX_0xC94						0
+#define		IDX_0xC80						1
+#define		IDX_0xC14						0
+#define		IDX_0xCA0						1
+#define		KEY							0
+#define		VAL							1
+
+/*  MACRO definition for pRFCalibrateInfo->TxIQC_8723B[1] */
+#define		PATH_S1							0 /*  RF_PATH_A */
+#define		IDX_0xC4C						2
+
+/*---------------------------Define Local Constant---------------------------*/
+
+/* In the case that we fail to read TxPowerTrack.txt, we use the table for
+ * 88E as the default table.
+ */
+static u8 DeltaSwingTableIdx_2GA_N_8188E[] = {
+	0, 0, 0, 2, 2, 3, 3, 4,  4,  4,  4,  5,  5,  6,  6,
+	7, 7, 7, 7, 8, 8, 9, 9, 10, 10, 10, 11, 11, 11, 11
+};
+static u8 DeltaSwingTableIdx_2GA_P_8188E[] = {
+	0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 4, 4,
+	4, 4, 4, 5, 5, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9
+};
+
+/* 3 ============================================================ */
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+
+
+static void setIqkMatrix_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u8 OFDM_index,
+	u8 RFPath,
+	s32 IqkResult_X,
+	s32 IqkResult_Y
+)
+{
+	s32 ele_A = 0, ele_D, ele_C = 0, value32;
+
+	if (OFDM_index >= OFDM_TABLE_SIZE)
+		OFDM_index = OFDM_TABLE_SIZE-1;
+
+	ele_D = (OFDMSwingTable_New[OFDM_index] & 0xFFC00000)>>22;
+
+	/* new element A = element D x X */
+	if (IqkResult_X != 0) {
+		if ((IqkResult_X & 0x00000200) != 0)	/* consider minus */
+			IqkResult_X = IqkResult_X | 0xFFFFFC00;
+		ele_A = ((IqkResult_X * ele_D)>>8)&0x000003FF;
+
+		/* new element C = element D x Y */
+		if ((IqkResult_Y & 0x00000200) != 0)
+			IqkResult_Y = IqkResult_Y | 0xFFFFFC00;
+		ele_C = ((IqkResult_Y * ele_D)>>8)&0x000003FF;
+
+		/* if (RFPath == RF_PATH_A) */
+		switch (RFPath) {
+		case RF_PATH_A:
+			/* write new elements A, C, D to regC80 and regC94,
+			 * element B is always 0
+			 */
+			value32 = (ele_D<<22)|((ele_C&0x3F)<<16)|ele_A;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, value32);
+
+			value32 = (ele_C&0x000003C0)>>6;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, value32);
+
+			value32 = ((IqkResult_X * ele_D)>>7)&0x01;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT24, value32);
+			break;
+		case RF_PATH_B:
+			/* write new elements A, C, D to regC88 and regC9C,
+			 * element B is always 0
+			 */
+			value32 = (ele_D<<22)|((ele_C&0x3F)<<16)|ele_A;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
+
+			value32 = (ele_C&0x000003C0)>>6;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
+
+			value32 = ((IqkResult_X * ele_D)>>7)&0x01;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT28, value32);
+
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (RFPath) {
+		case RF_PATH_A:
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, OFDMSwingTable_New[OFDM_index]);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, 0x00);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT24, 0x00);
+			break;
+
+		case RF_PATH_B:
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable_New[OFDM_index]);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT28, 0x00);
+			break;
+
+		default:
+			break;
+		}
+	}
+}
+
+
+static void setCCKFilterCoefficient(struct dm_odm_t *pDM_Odm, u8 CCKSwingIndex)
+{
+	if (!pDM_Odm->RFCalibrateInfo.bCCKinCH14) {
+		rtw_write8(pDM_Odm->Adapter, 0xa22, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][0]);
+		rtw_write8(pDM_Odm->Adapter, 0xa23, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][1]);
+		rtw_write8(pDM_Odm->Adapter, 0xa24, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][2]);
+		rtw_write8(pDM_Odm->Adapter, 0xa25, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][3]);
+		rtw_write8(pDM_Odm->Adapter, 0xa26, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][4]);
+		rtw_write8(pDM_Odm->Adapter, 0xa27, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][5]);
+		rtw_write8(pDM_Odm->Adapter, 0xa28, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][6]);
+		rtw_write8(pDM_Odm->Adapter, 0xa29, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][7]);
+	} else {
+		rtw_write8(pDM_Odm->Adapter, 0xa22, CCKSwingTable_Ch14_New[CCKSwingIndex][0]);
+		rtw_write8(pDM_Odm->Adapter, 0xa23, CCKSwingTable_Ch14_New[CCKSwingIndex][1]);
+		rtw_write8(pDM_Odm->Adapter, 0xa24, CCKSwingTable_Ch14_New[CCKSwingIndex][2]);
+		rtw_write8(pDM_Odm->Adapter, 0xa25, CCKSwingTable_Ch14_New[CCKSwingIndex][3]);
+		rtw_write8(pDM_Odm->Adapter, 0xa26, CCKSwingTable_Ch14_New[CCKSwingIndex][4]);
+		rtw_write8(pDM_Odm->Adapter, 0xa27, CCKSwingTable_Ch14_New[CCKSwingIndex][5]);
+		rtw_write8(pDM_Odm->Adapter, 0xa28, CCKSwingTable_Ch14_New[CCKSwingIndex][6]);
+		rtw_write8(pDM_Odm->Adapter, 0xa29, CCKSwingTable_Ch14_New[CCKSwingIndex][7]);
+	}
+}
+
+void DoIQK_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+)
+{
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power according to flag.
+ *			OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *When		Who	Remark
+ *04/23/2012	MHC	Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void ODM_TxPwrTrackSetPwr_8723B(
+	struct dm_odm_t *pDM_Odm,
+	enum pwrtrack_method Method,
+	u8 RFPath,
+	u8 ChannelMappedIndex
+)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u8 PwrTrackingLimit_OFDM = 34; /* 0dB */
+	u8 PwrTrackingLimit_CCK = 28; /* 2dB */
+	u8 TxRate = 0xFF;
+	u8 Final_OFDM_Swing_Index = 0;
+	u8 Final_CCK_Swing_Index = 0;
+
+	{
+		u16 rate = *(pDM_Odm->pForcedDataRate);
+
+		if (!rate) { /* auto rate */
+			if (pDM_Odm->TxRate != 0xFF)
+				TxRate = HwRateToMRate(pDM_Odm->TxRate);
+		} else /* force rate */
+			TxRate = (u8)rate;
+
+	}
+
+	if (TxRate != 0xFF) {
+		/* 2 CCK */
+		if ((TxRate >= MGN_1M) && (TxRate <= MGN_11M))
+			PwrTrackingLimit_CCK = 28;	/* 2dB */
+		/* 2 OFDM */
+		else if ((TxRate >= MGN_6M) && (TxRate <= MGN_48M))
+			PwrTrackingLimit_OFDM = 36; /* 3dB */
+		else if (TxRate == MGN_54M)
+			PwrTrackingLimit_OFDM = 34; /* 2dB */
+
+		/* 2 HT */
+		else if ((TxRate >= MGN_MCS0) && (TxRate <= MGN_MCS2)) /* QPSK/BPSK */
+			PwrTrackingLimit_OFDM = 38; /* 4dB */
+		else if ((TxRate >= MGN_MCS3) && (TxRate <= MGN_MCS4)) /* 16QAM */
+			PwrTrackingLimit_OFDM = 36; /* 3dB */
+		else if ((TxRate >= MGN_MCS5) && (TxRate <= MGN_MCS7)) /* 64QAM */
+			PwrTrackingLimit_OFDM = 34; /* 2dB */
+
+		else
+			PwrTrackingLimit_OFDM =  pDM_Odm->DefaultOfdmIndex;   /* Default OFDM index = 30 */
+	}
+
+	if (Method == TXAGC) {
+		struct adapter *Adapter = pDM_Odm->Adapter;
+
+		pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+		pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+	} else if (Method == BBSWING) {
+		Final_OFDM_Swing_Index = pDM_Odm->DefaultOfdmIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+		Final_CCK_Swing_Index = pDM_Odm->DefaultCckIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		/*  Adjust BB swing by OFDM IQ matrix */
+		if (Final_OFDM_Swing_Index >= PwrTrackingLimit_OFDM)
+			Final_OFDM_Swing_Index = PwrTrackingLimit_OFDM;
+		else if (Final_OFDM_Swing_Index <= 0)
+			Final_OFDM_Swing_Index = 0;
+
+		if (Final_CCK_Swing_Index >= CCK_TABLE_SIZE)
+			Final_CCK_Swing_Index = CCK_TABLE_SIZE-1;
+		else if (pDM_Odm->BbSwingIdxCck <= 0)
+			Final_CCK_Swing_Index = 0;
+
+		setIqkMatrix_8723B(pDM_Odm, Final_OFDM_Swing_Index, RFPath,
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+		setCCKFilterCoefficient(pDM_Odm, Final_CCK_Swing_Index);
+
+	} else if (Method == MIX_MODE) {
+		Final_OFDM_Swing_Index = pDM_Odm->DefaultOfdmIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+		Final_CCK_Swing_Index = pDM_Odm->DefaultCckIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		if (Final_OFDM_Swing_Index > PwrTrackingLimit_OFDM) { /* BBSwing higher then Limit */
+			pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = Final_OFDM_Swing_Index - PwrTrackingLimit_OFDM;
+
+			setIqkMatrix_8723B(pDM_Odm, PwrTrackingLimit_OFDM, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+		} else if (Final_OFDM_Swing_Index <= 0) {
+			pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = Final_OFDM_Swing_Index;
+
+			setIqkMatrix_8723B(pDM_Odm, 0, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+		} else {
+			setIqkMatrix_8723B(pDM_Odm, Final_OFDM_Swing_Index, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			if (pDM_Odm->Modify_TxAGC_Flag_PathA) { /* If TxAGC has changed, reset TxAGC again */
+				pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = 0;
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+				pDM_Odm->Modify_TxAGC_Flag_PathA = false;
+			}
+		}
+
+		if (Final_CCK_Swing_Index > PwrTrackingLimit_CCK) {
+			pDM_Odm->Remnant_CCKSwingIdx = Final_CCK_Swing_Index - PwrTrackingLimit_CCK;
+			setCCKFilterCoefficient(pDM_Odm, PwrTrackingLimit_CCK);
+			pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+		} else if (Final_CCK_Swing_Index <= 0) { /*  Lowest CCK Index = 0 */
+			pDM_Odm->Remnant_CCKSwingIdx = Final_CCK_Swing_Index;
+			setCCKFilterCoefficient(pDM_Odm, 0);
+			pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+		} else {
+			setCCKFilterCoefficient(pDM_Odm, Final_CCK_Swing_Index);
+
+			if (pDM_Odm->Modify_TxAGC_Flag_PathA_CCK) { /* If TxAGC has changed, reset TxAGC again */
+				pDM_Odm->Remnant_CCKSwingIdx = 0;
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+				pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = false;
+			}
+		}
+	} else
+		return; /*  This method is not supported. */
+}
+
+static void GetDeltaSwingTable_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u8 **TemperatureUP_A,
+	u8 **TemperatureDOWN_A,
+	u8 **TemperatureUP_B,
+	u8 **TemperatureDOWN_B
+)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u16 rate = *(pDM_Odm->pForcedDataRate);
+	u8 channel = pHalData->CurrentChannel;
+
+	if (1 <= channel && channel <= 14) {
+		if (IS_CCK_RATE(rate)) {
+			*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P;
+			*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N;
+			*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P;
+			*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N;
+		} else {
+			*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P;
+			*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N;
+			*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P;
+			*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N;
+		}
+	} else {
+		*TemperatureUP_A   = (u8 *)DeltaSwingTableIdx_2GA_P_8188E;
+		*TemperatureDOWN_A = (u8 *)DeltaSwingTableIdx_2GA_N_8188E;
+		*TemperatureUP_B   = (u8 *)DeltaSwingTableIdx_2GA_P_8188E;
+		*TemperatureDOWN_B = (u8 *)DeltaSwingTableIdx_2GA_N_8188E;
+	}
+}
+
+
+void ConfigureTxpowerTrack_8723B(struct txpwrtrack_cfg *pConfig)
+{
+	pConfig->SwingTableSize_CCK = CCK_TABLE_SIZE;
+	pConfig->SwingTableSize_OFDM = OFDM_TABLE_SIZE;
+	pConfig->Threshold_IQK = IQK_THRESHOLD;
+	pConfig->AverageThermalNum = AVG_THERMAL_NUM_8723B;
+	pConfig->RfPathCount = MAX_PATH_NUM_8723B;
+	pConfig->ThermalRegAddr = RF_T_METER_8723B;
+
+	pConfig->ODM_TxPwrTrackSetPwr = ODM_TxPwrTrackSetPwr_8723B;
+	pConfig->DoIQK = DoIQK_8723B;
+	pConfig->PHY_LCCalibrate = PHY_LCCalibrate_8723B;
+	pConfig->GetDeltaSwingTable = GetDeltaSwingTable_8723B;
+}
+
+/* 1 7. IQK */
+#define MAX_TOLERANCE		5
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathA_IQK_8723B(
+	struct adapter *padapter, bool configPathB, u8 RF_Path
+)
+{
+	u32 regEAC, regE94, regE9C, tmp, Path_SEL_BB /*, regEA4*/;
+	u8 result = 0x00;
+
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* 	enable path A PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87);
+	/* 	disable path B PA in TXIQK mode */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, bRFRegOffsetMask, 0x00020); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x40ec1); */
+
+	/* 1 Tx IQK */
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x8214010a); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x821303ea);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+	else					/* if Tx not OK, ignore Rx */
+		return result;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathA_RxIQK8723B(
+	struct adapter *padapter, bool configPathB, u8 RF_Path
+)
+{
+	u32 regEAC, regE94, regE9C, regEA4, u4tmp, tmp, Path_SEL_BB;
+	u8 result = 0x00;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LNA2 off, PA on for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82130ff0);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+	else				/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (regE94&0x3FF0000) | ((regE9C&0x3FF0000) >> 16);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, u4tmp);
+
+	/* modify RXIQK mode table */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LAN2 on, PA off for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* PA, PAD setting */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x55, bRFRegOffsetMask, 0x4021f);
+
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82110000);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x281604c2); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x2813001f);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a8d1);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regEA4 = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x780);
+
+	/* Allen 20131125 */
+	tmp = (regEAC & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT27) && /* if Tx is OK, check whether Rx is OK */
+		(((regEA4 & 0x03FF0000)>>16) != 0x132) &&
+		(((regEAC & 0x03FF0000)>>16) != 0x36) &&
+		(((regEA4 & 0x03FF0000)>>16) < 0x110) &&
+		(((regEA4 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x02;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathB_IQK_8723B(struct adapter *padapter)
+{
+	u32 regEAC, regE94, regE9C, tmp, Path_SEL_BB/*, regEC4, regECC, Path_SEL_BB*/;
+	u8 result = 0x00;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* 	in TXIQK mode */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x20000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87); */
+	/* 	enable path B PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30fc1);
+
+
+
+	/* 1 Tx IQK */
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82140114); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x821303ea);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathB_RxIQK8723B(struct adapter *padapter, bool configPathB)
+{
+	u32 regE94, regE9C, regEA4, regEAC, u4tmp, tmp, Path_SEL_BB;
+	u8 result = 0x00;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+	/* modify RXIQK mode table */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	/* open PA S1 & SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30fcd);
+
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+
+	/* path-B IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82130ff0);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+    /* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B TXIQK @ RXIQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42)  &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+			result |= 0x01;
+	else	/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (regE94&0x3FF0000)  | ((regE9C&0x3FF0000) >> 16);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, u4tmp);
+
+	/* modify RXIQK mode table */
+	/* 20121009, Kordan> RF Mode = 3 */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* open PA S1 & close SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30ebd);
+
+	/* PA, PAD setting */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x56, bRFRegOffsetMask, 0x51000); */
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-B IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82110000);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x281604c2); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x2813001f);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a8d1);
+
+    /* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regEA4 = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, 0xffffff00, 0x00000000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_B, 0xdf, bRFRegOffsetMask, 0x180); */
+
+
+
+	/* Allen 20131125 */
+	tmp = (regEAC & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT27) && /* if Tx is OK, check whether Rx is OK */
+		(((regEA4 & 0x03FF0000)>>16) != 0x132) &&
+		(((regEAC & 0x03FF0000)>>16) != 0x36) &&
+		(((regEA4 & 0x03FF0000)>>16) < 0x110) &&
+		(((regEA4 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x02;
+
+	return result;
+}
+
+static void _PHY_PathAFillIQKMatrix8723B(
+	struct adapter *padapter,
+	bool bIQKOK,
+	s32 result[][8],
+	u8 final_candidate,
+	bool bTxOnly
+)
+{
+	u32 Oldval_0, X, TX0_A, reg;
+	s32 Y, TX0_C;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+
+	if (final_candidate == 0xFF)
+		return;
+
+	else if (bIQKOK) {
+		Oldval_0 = (PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		X = result[final_candidate][0];
+		if ((X & 0x00000200) != 0)
+			X = X | 0xFFFFFC00;
+		TX0_A = (X * Oldval_0) >> 8;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, 0x3FF, TX0_A);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(31), ((X*Oldval_0>>7) & 0x1));
+
+		Y = result[final_candidate][1];
+		if ((Y & 0x00000200) != 0)
+			Y = Y | 0xFFFFFC00;
+
+		/* 2 Tx IQC */
+		TX0_C = (Y * Oldval_0) >> 8;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, 0xF0000000, ((TX0_C&0x3C0)>>6));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][KEY] = rOFDM0_XCTxAFE;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, 0x003F0000, (TX0_C&0x3F));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][KEY] = rOFDM0_XATxIQImbalance;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(29), ((Y*Oldval_0>>7) & 0x1));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][KEY] = rOFDM0_ECCAThreshold;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskDWord);
+
+		if (bTxOnly) {
+			/*  <20130226, Kordan> Saving RxIQC, otherwise not initialized. */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL] = 0xfffffff & PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+/* 			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord); */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = 0x40000100;
+			return;
+		}
+
+		reg = result[final_candidate][2];
+
+		/* 2 Rx IQC */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, 0x3FF, reg);
+		reg = result[final_candidate][3] & 0x3F;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, 0xFC00, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord);
+
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, 0xF0000000, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+
+	}
+}
+
+static void _PHY_PathBFillIQKMatrix8723B(
+	struct adapter *padapter,
+	bool bIQKOK,
+	s32 result[][8],
+	u8 final_candidate,
+	bool bTxOnly /* do Tx only */
+)
+{
+	u32 Oldval_1, X, TX1_A, reg;
+	s32	Y, TX1_C;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+
+	if (final_candidate == 0xFF)
+		return;
+
+	else if (bIQKOK) {
+		Oldval_1 = (PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		X = result[final_candidate][4];
+		if ((X & 0x00000200) != 0)
+			X = X | 0xFFFFFC00;
+		TX1_A = (X * Oldval_1) >> 8;
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, 0x3FF, TX1_A);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(27), ((X*Oldval_1>>7) & 0x1));
+
+		Y = result[final_candidate][5];
+		if ((Y & 0x00000200) != 0)
+			Y = Y | 0xFFFFFC00;
+
+		TX1_C = (Y * Oldval_1) >> 8;
+
+		/* 2 Tx IQC */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, 0xF0000000, ((TX1_C&0x3C0)>>6));
+/* 		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC9C][KEY] = rOFDM0_XDTxAFE; */
+/* 		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC9C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskDWord); */
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][KEY] = rOFDM0_XCTxAFE;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, 0x003F0000, (TX1_C&0x3F));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][KEY] = rOFDM0_XATxIQImbalance;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(25), ((Y*Oldval_1>>7) & 0x1));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][KEY] = rOFDM0_ECCAThreshold;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskDWord);
+
+		if (bTxOnly) {
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+/* 			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord); */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = 0x40000100;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL] = 0x0fffffff & PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+			return;
+		}
+
+		/* 2 Rx IQC */
+		reg = result[final_candidate][6];
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, 0x3FF, reg);
+		reg = result[final_candidate][7] & 0x3F;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, 0xFC00, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, bMaskDWord);
+
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+/* 		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_AGCRSSITable, 0x0000F000, reg); */
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL] = (reg << 28)|(PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord)&0x0fffffff);
+	}
+}
+
+/*  */
+/*  2011/07/26 MH Add an API for testing IQK fail case. */
+/*  */
+/*  MP Already declare in odm.c */
+
+void ODM_SetIQCbyRFpath(struct dm_odm_t *pDM_Odm, u32 RFpath)
+{
+
+	struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+
+	if (
+		(pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL] != 0x0) &&
+		(pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] != 0x0) &&
+		(pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL] != 0x0) &&
+		(pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] != 0x0)
+	) {
+		if (RFpath) { /* S1: RFpath = 0, S0:RFpath = 1 */
+			/* S0 TX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][VAL]);
+			/* S0 RX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL]);
+		} else {
+			/* S1 TX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][VAL]);
+			/* S1 RX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL]);
+		}
+	}
+}
+
+static bool ODM_CheckPowerStatus(struct adapter *Adapter)
+{
+	return true;
+}
+
+static void _PHY_SaveADDARegisters8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegisterNum
+)
+{
+	u32 i;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	if (!ODM_CheckPowerStatus(padapter))
+		return;
+
+	for (i = 0 ; i < RegisterNum ; i++) {
+		ADDABackup[i] = PHY_QueryBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord);
+	}
+}
+
+
+static void _PHY_SaveMACRegisters8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		MACBackup[i] = rtw_read8(pDM_Odm->Adapter, MACReg[i]);
+	}
+	MACBackup[i] = rtw_read32(pDM_Odm->Adapter, MACReg[i]);
+
+}
+
+
+static void _PHY_ReloadADDARegisters8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegiesterNum
+)
+{
+	u32 i;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	for (i = 0 ; i < RegiesterNum; i++) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord, ADDABackup[i]);
+	}
+}
+
+static void _PHY_ReloadMACRegisters8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i;
+
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		rtw_write8(padapter, MACReg[i], (u8)MACBackup[i]);
+	}
+	rtw_write32(padapter, MACReg[i], MACBackup[i]);
+}
+
+
+static void _PHY_PathADDAOn8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	bool is2T
+)
+{
+	u32 pathOn;
+	u32 i;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	pathOn = 0x01c00014;
+	if (!is2T) {
+		pathOn = 0x01c00014;
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[0], bMaskDWord, 0x01c00014);
+	} else {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[0], bMaskDWord, pathOn);
+	}
+
+	for (i = 1 ; i < IQK_ADDA_REG_NUM ; i++) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord, pathOn);
+	}
+
+}
+
+static void _PHY_MACSettingCalibration8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i = 0;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	rtw_write8(pDM_Odm->Adapter, MACReg[i], 0x3F);
+
+	for (i = 1 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		rtw_write8(pDM_Odm->Adapter, MACReg[i], (u8)(MACBackup[i]&(~BIT3)));
+	}
+	rtw_write8(pDM_Odm->Adapter, MACReg[i], (u8)(MACBackup[i]&(~BIT5)));
+
+}
+
+static bool phy_SimularityCompare_8723B(
+	struct adapter *padapter,
+	s32 result[][8],
+	u8  c1,
+	u8  c2
+)
+{
+	u32 i, j, diff, SimularityBitMap, bound = 0;
+	u8 final_candidate[2] = {0xFF, 0xFF}; /* for path A and path B */
+	bool bResult = true;
+	s32 tmp1 = 0, tmp2 = 0;
+
+	bound = 8;
+	SimularityBitMap = 0;
+
+	for (i = 0; i < bound; i++) {
+
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((result[c1][i] & 0x00000200) != 0)
+				tmp1 = result[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = result[c1][i];
+
+			if ((result[c2][i] & 0x00000200) != 0)
+				tmp2 = result[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = result[c2][i];
+		} else {
+			tmp1 = result[c1][i];
+			tmp2 = result[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !SimularityBitMap) {
+				if (result[c1][i]+result[c1][i+1] == 0)
+					final_candidate[(i/4)] = c2;
+				else if (result[c2][i]+result[c2][i+1] == 0)
+					final_candidate[(i/4)] = c1;
+				else
+					SimularityBitMap = SimularityBitMap|(1<<i);
+			} else
+				SimularityBitMap = SimularityBitMap|(1<<i);
+		}
+	}
+
+	if (SimularityBitMap == 0) {
+		for (i = 0; i < (bound/4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i*4; j < (i+1)*4-2; j++)
+					result[3][j] = result[final_candidate[i]][j];
+				bResult = false;
+			}
+		}
+		return bResult;
+	} else {
+
+		if (!(SimularityBitMap & 0x03)) { /* path A TX OK */
+			for (i = 0; i < 2; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0x0c)) { /* path A RX OK */
+			for (i = 2; i < 4; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0x30)) { /* path B TX OK */
+			for (i = 4; i < 6; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0xc0)) { /* path B RX OK */
+			for (i = 6; i < 8; i++)
+				result[3][i] = result[c1][i];
+		}
+		return false;
+	}
+}
+
+
+
+static void phy_IQCalibrate_8723B(
+	struct adapter *padapter,
+	s32 result[][8],
+	u8 t,
+	bool is2T,
+	u8 RF_Path
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	u32 i;
+	u8 PathAOK, PathBOK;
+	u8 tmp0xc50 = (u8)PHY_QueryBBReg(pDM_Odm->Adapter, 0xC50, bMaskByte0);
+	u8 tmp0xc58 = (u8)PHY_QueryBBReg(pDM_Odm->Adapter, 0xC58, bMaskByte0);
+	u32 ADDA_REG[IQK_ADDA_REG_NUM] = {
+		rFPGA0_XCD_SwitchControl,
+		rBlue_Tooth,
+		rRx_Wait_CCA,
+		rTx_CCK_RFON,
+		rTx_CCK_BBON,
+		rTx_OFDM_RFON,
+		rTx_OFDM_BBON,
+		rTx_To_Rx,
+		rTx_To_Tx,
+		rRx_CCK,
+		rRx_OFDM,
+		rRx_Wait_RIFS,
+		rRx_TO_Rx,
+		rStandby,
+		rSleep,
+		rPMPD_ANAEN
+	};
+	u32 IQK_MAC_REG[IQK_MAC_REG_NUM] = {
+		REG_TXPAUSE,
+		REG_BCN_CTRL,
+		REG_BCN_CTRL_1,
+		REG_GPIO_MUXCFG
+	};
+
+	/* since 92C & 92D have the different define in IQK_BB_REG */
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+		rOFDM0_TRxPathEnable,
+		rOFDM0_TRMuxPar,
+		rFPGA0_XCD_RFInterfaceSW,
+		rConfig_AntA,
+		rConfig_AntB,
+		rFPGA0_XAB_RFInterfaceSW,
+		rFPGA0_XA_RFInterfaceOE,
+		rFPGA0_XB_RFInterfaceOE,
+		rCCK0_AFESetting
+	};
+	const u32 retryCount = 2;
+
+	/*  Note: IQ calibration must be performed after loading */
+	/* 		PHY_REG.txt , and radio_a, radio_b.txt */
+
+	/* u32 bbvalue; */
+
+	if (t == 0) {
+
+		/*  Save ADDA parameters, turn Path A ADDA on */
+		_PHY_SaveADDARegisters8723B(padapter, ADDA_REG, pDM_Odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+		_PHY_SaveMACRegisters8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+		_PHY_SaveADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+	}
+
+	_PHY_PathADDAOn8723B(padapter, ADDA_REG, is2T);
+
+/* no serial mode */
+
+	/* save RF path for 8723B */
+/* 	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord); */
+/* 	Path_SEL_RF = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, 0xfffff); */
+
+	/* MAC settings */
+	_PHY_MACSettingCalibration8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+
+	/* BB setting */
+	/* PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_RFMOD, BIT24, 0x00); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rCCK0_AFESetting, 0x0f000000, 0xf);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
+
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00); */
+
+
+/* RX IQ calibration setting for 8723B D cut large current issue when leaving IPS */
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x60fbd);
+
+/* path A TX IQK */
+	for (i = 0 ; i < retryCount ; i++) {
+		PathAOK = phy_PathA_IQK_8723B(padapter, is2T, RF_Path);
+/* 		if (PathAOK == 0x03) { */
+		if (PathAOK == 0x01) {
+			/*  Path A Tx IQK Success */
+			PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+			pDM_Odm->RFCalibrateInfo.TxLOK[RF_PATH_A] = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x8, bRFRegOffsetMask);
+
+				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+			break;
+		}
+	}
+
+/* path A RXIQK */
+	for (i = 0 ; i < retryCount ; i++) {
+		PathAOK = phy_PathA_RxIQK8723B(padapter, is2T, RF_Path);
+		if (PathAOK == 0x03) {
+/* 				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+/* 				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+				result[t][2] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][3] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+			break;
+		}
+	}
+
+	if (0x00 == PathAOK) {
+	}
+
+/* path B IQK */
+	if (is2T) {
+
+		/* path B TX IQK */
+		for (i = 0 ; i < retryCount ; i++) {
+			PathBOK = phy_PathB_IQK_8723B(padapter);
+			if (PathBOK == 0x01) {
+				/*  Path B Tx IQK Success */
+				PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+				pDM_Odm->RFCalibrateInfo.TxLOK[RF_PATH_B] = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH_B, 0x8, bRFRegOffsetMask);
+
+				result[t][4] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				result[t][5] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				break;
+			}
+		}
+
+/* path B RX IQK */
+		for (i = 0 ; i < retryCount ; i++) {
+			PathBOK = phy_PathB_RxIQK8723B(padapter, is2T);
+			if (PathBOK == 0x03) {
+/* 				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+/* 				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+				result[t][6] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][7] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				break;
+			}
+		}
+
+/* Allen end */
+	}
+
+	/* Back to BB mode, load original value */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0);
+
+	if (t != 0) {
+		/*  Reload ADDA power saving parameters */
+		_PHY_ReloadADDARegisters8723B(padapter, ADDA_REG, pDM_Odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+
+		/*  Reload MAC parameters */
+		_PHY_ReloadMACRegisters8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+
+		_PHY_ReloadADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+
+		/* Reload RF path */
+/* 		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB); */
+/* 		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, 0xfffff, Path_SEL_RF); */
+
+		/* Allen initial gain 0xc50 */
+		/*  Restore RX initial gain */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0xc50, bMaskByte0, 0x50);
+		PHY_SetBBReg(pDM_Odm->Adapter, 0xc50, bMaskByte0, tmp0xc50);
+		if (is2T) {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc58, bMaskByte0, 0x50);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc58, bMaskByte0, tmp0xc58);
+		}
+
+		/* load 0xe30 IQC default value */
+		PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+		PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+
+	}
+
+}
+
+
+static void phy_LCCalibrate_8723B(struct dm_odm_t *pDM_Odm, bool is2T)
+{
+	u8 tmpReg;
+	u32 RF_Amode = 0, RF_Bmode = 0, LC_Cal;
+	struct adapter *padapter = pDM_Odm->Adapter;
+
+	/* Check continuous TX and Packet TX */
+	tmpReg = rtw_read8(pDM_Odm->Adapter, 0xd03);
+
+	if ((tmpReg&0x70) != 0)			/* Deal with contisuous TX case */
+		rtw_write8(pDM_Odm->Adapter, 0xd03, tmpReg&0x8F);	/* disable all continuous TX */
+	else							/*  Deal with Packet TX case */
+		rtw_write8(pDM_Odm->Adapter, REG_TXPAUSE, 0xFF);		/*  block all queues */
+
+	if ((tmpReg&0x70) != 0) {
+		/* 1. Read original RF mode */
+		/* Path-A */
+		RF_Amode = PHY_QueryRFReg(padapter, RF_PATH_A, RF_AC, bMask12Bits);
+
+		/* Path-B */
+		if (is2T)
+			RF_Bmode = PHY_QueryRFReg(padapter, RF_PATH_B, RF_AC, bMask12Bits);
+
+		/* 2. Set RF mode = standby mode */
+		/* Path-A */
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_AC, bMask12Bits, (RF_Amode&0x8FFFF)|0x10000);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_B, RF_AC, bMask12Bits, (RF_Bmode&0x8FFFF)|0x10000);
+	}
+
+	/* 3. Read RF reg18 */
+	LC_Cal = PHY_QueryRFReg(padapter, RF_PATH_A, RF_CHNLBW, bMask12Bits);
+
+	/* 4. Set LC calibration begin	bit15 */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFBE0); /*  LDO ON */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal|0x08000);
+
+	mdelay(100);
+
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFFE0); /*  LDO OFF */
+
+	/*  Channel 10 LC calibration issue for 8723bs with 26M xtal */
+	if (pDM_Odm->SupportInterface == ODM_ITRF_SDIO && pDM_Odm->PackageType >= 0x2) {
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal);
+	}
+
+	/* Restore original situation */
+	if ((tmpReg&0x70) != 0) { /* Deal with contisuous TX case */
+		/* Path-A */
+		rtw_write8(pDM_Odm->Adapter, 0xd03, tmpReg);
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
+	} else /*  Deal with Packet TX case */
+		rtw_write8(pDM_Odm->Adapter, REG_TXPAUSE, 0x00);
+}
+
+/* IQK version:V2.5    20140123 */
+/* IQK is controlled by Is2ant, RF path */
+void PHY_IQCalibrate_8723B(
+	struct adapter *padapter,
+	bool bReCovery,
+	bool bRestore,
+	bool Is2ant,	/* false:1ant, true:2-ant */
+	u8 RF_Path	/* 0:S1, 1:S0 */
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	s32 result[4][8];	/* last is final result */
+	u8 i, final_candidate;
+	bool bPathAOK, bPathBOK;
+	s32 RegE94, RegE9C, RegEA4, RegEB4, RegEBC, RegEC4, RegTmp = 0;
+	bool is12simular, is13simular, is23simular;
+	bool bSingleTone = false, bCarrierSuppression = false;
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+		rOFDM0_XARxIQImbalance,
+		rOFDM0_XBRxIQImbalance,
+		rOFDM0_ECCAThreshold,
+		rOFDM0_AGCRSSITable,
+		rOFDM0_XATxIQImbalance,
+		rOFDM0_XBTxIQImbalance,
+		rOFDM0_XCTxAFE,
+		rOFDM0_XDTxAFE,
+		rOFDM0_RxIQExtAnta
+	};
+/* 	u32 		Path_SEL_BB = 0; */
+	u32 		GNT_BT_default;
+
+	if (!ODM_CheckPowerStatus(padapter))
+		return;
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+
+	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
+	if (bSingleTone || bCarrierSuppression)
+		return;
+
+	if (pDM_Odm->RFCalibrateInfo.bIQKInProgress)
+		return;
+
+
+	pDM_Odm->RFCalibrateInfo.bIQKInProgress = true;
+
+	if (bRestore) {
+		u32 offset, data;
+		u8 path, bResult = SUCCESS;
+		struct odm_rf_cal_t *pRFCalibrateInfo = &pDM_Odm->RFCalibrateInfo;
+
+		path = (PHY_QueryBBReg(pDM_Odm->Adapter, rS0S1_PathSwitch, bMaskByte0) == 0x00) ? RF_PATH_A : RF_PATH_B;
+
+		/*  Restore TX IQK */
+		for (i = 0; i < 3; ++i) {
+			offset = pRFCalibrateInfo->TxIQC_8723B[path][i][0];
+			data = pRFCalibrateInfo->TxIQC_8723B[path][i][1];
+			if ((offset == 0) || (data == 0)) {
+				bResult = FAIL;
+				break;
+			}
+			PHY_SetBBReg(pDM_Odm->Adapter, offset, bMaskDWord, data);
+		}
+
+		/*  Restore RX IQK */
+		for (i = 0; i < 2; ++i) {
+			offset = pRFCalibrateInfo->RxIQC_8723B[path][i][0];
+			data = pRFCalibrateInfo->RxIQC_8723B[path][i][1];
+			if ((offset == 0) || (data == 0)) {
+				bResult = FAIL;
+				break;
+			}
+			PHY_SetBBReg(pDM_Odm->Adapter, offset, bMaskDWord, data);
+		}
+
+		if (pDM_Odm->RFCalibrateInfo.TxLOK[RF_PATH_A] == 0) {
+			bResult = FAIL;
+		} else {
+			PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[RF_PATH_A]);
+			PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_B, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[RF_PATH_B]);
+		}
+
+		if (bResult == SUCCESS)
+			return;
+	}
+
+	if (bReCovery) {
+		_PHY_ReloadADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+		return;
+	}
+
+	/* save default GNT_BT */
+	GNT_BT_default = PHY_QueryBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord);
+	/*  Save RF Path */
+/* 	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord); */
+/* 	Path_SEL_RF = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, 0xfffff); */
+
+    /* set GNT_BT = 0, pause BT traffic */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT12, 0x0); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT11, 0x1); */
+
+
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		result[3][i] = 0;
+	}
+
+	final_candidate = 0xff;
+	bPathAOK = false;
+	bPathBOK = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+
+
+	for (i = 0; i < 3; i++) {
+		phy_IQCalibrate_8723B(padapter, result, i, Is2ant, RF_Path);
+
+		if (i == 1) {
+			is12simular = phy_SimularityCompare_8723B(padapter, result, 0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = phy_SimularityCompare_8723B(padapter, result, 0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+
+				break;
+			}
+
+			is23simular = phy_SimularityCompare_8723B(padapter, result, 1, 2);
+			if (is23simular) {
+				final_candidate = 1;
+			} else {
+				for (i = 0; i < 8; i++)
+					RegTmp += result[3][i];
+
+				if (RegTmp != 0)
+					final_candidate = 3;
+				else
+					final_candidate = 0xFF;
+			}
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		RegE94 = result[i][0];
+		RegE9C = result[i][1];
+		RegEA4 = result[i][2];
+		RegEB4 = result[i][4];
+		RegEBC = result[i][5];
+		RegEC4 = result[i][6];
+	}
+
+	if (final_candidate != 0xff) {
+		pDM_Odm->RFCalibrateInfo.RegE94 = RegE94 = result[final_candidate][0];
+		pDM_Odm->RFCalibrateInfo.RegE9C = RegE9C = result[final_candidate][1];
+		RegEA4 = result[final_candidate][2];
+		pDM_Odm->RFCalibrateInfo.RegEB4 = RegEB4 = result[final_candidate][4];
+		pDM_Odm->RFCalibrateInfo.RegEBC = RegEBC = result[final_candidate][5];
+		RegEC4 = result[final_candidate][6];
+		bPathAOK = bPathBOK = true;
+	} else {
+		pDM_Odm->RFCalibrateInfo.RegE94 = pDM_Odm->RFCalibrateInfo.RegEB4 = 0x100;	/* X default value */
+		pDM_Odm->RFCalibrateInfo.RegE9C = pDM_Odm->RFCalibrateInfo.RegEBC = 0x0;		/* Y default value */
+	}
+
+	{
+		if (RegE94 != 0)
+			_PHY_PathAFillIQKMatrix8723B(padapter, bPathAOK, result, final_candidate, (RegEA4 == 0));
+	}
+	{
+		if (RegEB4 != 0)
+			_PHY_PathBFillIQKMatrix8723B(padapter, bPathBOK, result, final_candidate, (RegEC4 == 0));
+	}
+
+/* To Fix BSOD when final_candidate is 0xff */
+/* by sherry 20120321 */
+	if (final_candidate < 4) {
+		for (i = 0; i < IQK_Matrix_REG_NUM; i++)
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[0].Value[0][i] = result[final_candidate][i];
+		pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[0].bIQKDone = true;
+	}
+
+	_PHY_SaveADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+
+	/* restore GNT_BT */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, GNT_BT_default);
+	/*  Restore RF Path */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xb0, 0xfffff, Path_SEL_RF); */
+
+	/* Resotr RX mode table parameter */
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xe6177);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, 0x43, bRFRegOffsetMask, 0x300bd);
+
+	/* set GNT_BT = HW control */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT12, 0x0); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT11, 0x0); */
+
+	if (Is2ant) {
+		if (RF_Path == 0x0)	/* S1 */
+			ODM_SetIQCbyRFpath(pDM_Odm, 0);
+		else	/* S0 */
+			ODM_SetIQCbyRFpath(pDM_Odm, 1);
+	}
+
+	pDM_Odm->RFCalibrateInfo.bIQKInProgress = false;
+}
+
+
+void PHY_LCCalibrate_8723B(struct dm_odm_t *pDM_Odm)
+{
+	bool		bSingleTone = false, bCarrierSuppression = false;
+	u32 		timeout = 2000, timecount = 0;
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+
+	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
+	if (bSingleTone || bCarrierSuppression)
+		return;
+
+	while (*(pDM_Odm->pbScanInProcess) && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	pDM_Odm->RFCalibrateInfo.bLCKInProgress = true;
+
+
+	phy_LCCalibrate_8723B(pDM_Odm, false);
+
+
+	pDM_Odm->RFCalibrateInfo.bLCKInProgress = false;
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h
new file mode 100644
index 000000000..775095ad0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __HAL_PHY_RF_8723B_H__
+#define __HAL_PHY_RF_8723B_H__
+
+/*--------------------------Define Parameters-------------------------------*/
+#define	IQK_DELAY_TIME_8723B		20		/* ms */
+#define IQK_DEFERRED_TIME_8723B		4
+#define	index_mapping_NUM_8723B		15
+#define AVG_THERMAL_NUM_8723B		4
+#define	RF_T_METER_8723B					0x42	/*  */
+
+
+void ConfigureTxpowerTrack_8723B(struct txpwrtrack_cfg *pConfig);
+
+void DoIQK_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+);
+
+void ODM_TxPwrTrackSetPwr_8723B(
+	struct dm_odm_t *pDM_Odm,
+	enum pwrtrack_method Method,
+	u8 RFPath,
+	u8 ChannelMappedIndex
+);
+
+/* 1 7. IQK */
+void PHY_IQCalibrate_8723B(
+	struct adapter *Adapter,
+	bool bReCovery,
+	bool bRestore,
+	bool Is2ant,
+	u8 RF_Path
+);
+
+void ODM_SetIQCbyRFpath(struct dm_odm_t *pDM_Odm, u32 RFpath);
+
+/*  */
+/*  LC calibrate */
+/*  */
+void PHY_LCCalibrate_8723B(struct dm_odm_t *pDM_Odm);
+
+/*  */
+/*  AP calibrate */
+/*  */
+void PHY_DigitalPredistortion_8723B(struct adapter *padapter);
+
+
+void _PHY_SaveADDARegisters_8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegisterNum
+);
+
+void _PHY_PathADDAOn_8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	bool isPathAOn,
+	bool is2T
+);
+
+void _PHY_MACSettingCalibration_8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+);
+
+#endif /*  #ifndef __HAL_PHY_RF_8188E_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8723bs/hal/HalPwrSeqCmd.c
new file mode 100644
index 000000000..5f9e94a7e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPwrSeqCmd.c
@@ -0,0 +1,146 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*++
+Copyright (c) Realtek Semiconductor Corp. All rights reserved.
+
+Module Name:
+	HalPwrSeqCmd.c
+
+Abstract:
+	Implement HW Power sequence configuration CMD handling routine for Realtek devices.
+
+Major Change History:
+	When       Who               What
+	---------- ---------------   -------------------------------
+	2011-10-26 Lucas            Modify to be compatible with SD4-CE driver.
+	2011-07-07 Roger            Create.
+
+--*/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <HalPwrSeqCmd.h>
+
+
+/*  */
+/*  Description: */
+/*  This routine deal with the Power Configuration CMDs parsing for RTL8723/RTL8188E Series IC. */
+/*  */
+/*  Assumption: */
+/*  We should follow specific format which was released from HW SD. */
+/*  */
+/*  2011.07.07, added by Roger. */
+/*  */
+u8 HalPwrSeqCmdParsing(
+	struct adapter *padapter,
+	u8 CutVersion,
+	u8 FabVersion,
+	u8 InterfaceType,
+	struct wlan_pwr_cfg PwrSeqCmd[]
+)
+{
+	struct wlan_pwr_cfg PwrCfgCmd;
+	u8 bPollingBit = false;
+	u32 AryIdx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 pollingCount = 0; /*  polling autoload done. */
+	u32 maxPollingCnt = 5000;
+
+	do {
+		PwrCfgCmd = PwrSeqCmd[AryIdx];
+
+		/* 2 Only Handle the command whose FAB, CUT, and Interface are matched */
+		if (
+			(GET_PWR_CFG_FAB_MASK(PwrCfgCmd) & FabVersion) &&
+			(GET_PWR_CFG_CUT_MASK(PwrCfgCmd) & CutVersion) &&
+			(GET_PWR_CFG_INTF_MASK(PwrCfgCmd) & InterfaceType)
+		) {
+			switch (GET_PWR_CFG_CMD(PwrCfgCmd)) {
+			case PWR_CMD_READ:
+				break;
+
+			case PWR_CMD_WRITE:
+				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
+
+				/*  */
+				/*  <Roger_Notes> We should deal with interface specific address mapping for some interfaces, e.g., SDIO interface */
+				/*  2011.07.07. */
+				/*  */
+				if (GET_PWR_CFG_BASE(PwrCfgCmd) == PWR_BASEADDR_SDIO) {
+					/*  Read Back SDIO Local value */
+					value = SdioLocalCmd52Read1Byte(padapter, offset);
+
+					value &= ~(GET_PWR_CFG_MASK(PwrCfgCmd));
+					value |= (
+						GET_PWR_CFG_VALUE(PwrCfgCmd) &
+						GET_PWR_CFG_MASK(PwrCfgCmd)
+					);
+
+					/*  Write Back SDIO Local value */
+					SdioLocalCmd52Write1Byte(padapter, offset, value);
+				} else {
+					/*  Read the value from system register */
+					value = rtw_read8(padapter, offset);
+
+					value &= (~(GET_PWR_CFG_MASK(PwrCfgCmd)));
+					value |= (
+						GET_PWR_CFG_VALUE(PwrCfgCmd)
+						&GET_PWR_CFG_MASK(PwrCfgCmd)
+					);
+
+					/*  Write the value back to system register */
+					rtw_write8(padapter, offset, value);
+				}
+				break;
+
+			case PWR_CMD_POLLING:
+
+				bPollingBit = false;
+				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
+				do {
+					if (GET_PWR_CFG_BASE(PwrCfgCmd) == PWR_BASEADDR_SDIO)
+						value = SdioLocalCmd52Read1Byte(padapter, offset);
+					else
+						value = rtw_read8(padapter, offset);
+
+					value = value&GET_PWR_CFG_MASK(PwrCfgCmd);
+					if (
+						value == (GET_PWR_CFG_VALUE(PwrCfgCmd) &
+						GET_PWR_CFG_MASK(PwrCfgCmd))
+					)
+						bPollingBit = true;
+					else
+						udelay(10);
+
+					if (pollingCount++ > maxPollingCnt)
+						return false;
+
+				} while (!bPollingBit);
+
+				break;
+
+			case PWR_CMD_DELAY:
+				if (GET_PWR_CFG_VALUE(PwrCfgCmd) == PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(PwrCfgCmd));
+				else
+					udelay(GET_PWR_CFG_OFFSET(PwrCfgCmd)*1000);
+				break;
+
+			case PWR_CMD_END:
+				/*  When this command is parsed, end the process */
+				return true;
+
+			default:
+				break;
+			}
+		}
+
+		AryIdx++;/* Add Array Index */
+	} while (1);
+
+	return true;
+}
diff --git a/drivers/staging/rtl8723bs/hal/Mp_Precomp.h b/drivers/staging/rtl8723bs/hal/Mp_Precomp.h
new file mode 100644
index 000000000..f08823a8d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/Mp_Precomp.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __MP_PRECOMP_H__
+#define __MP_PRECOMP_H__
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#define BT_TMP_BUF_SIZE	100
+
+#ifdef bEnable
+#undef bEnable
+#endif
+
+#include "HalBtcOutSrc.h"
+#include "HalBtc8723b1Ant.h"
+#include "HalBtc8723b2Ant.h"
+
+#endif /*  __MP_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/hal_btcoex.c b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
new file mode 100644
index 000000000..e36f8c369
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
@@ -0,0 +1,1341 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <hal_data.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <Mp_Precomp.h>
+
+/* 		Global variables */
+
+struct btc_coexist GLBtCoexist;
+static u8 GLBtcWiFiInScanState;
+static u8 GLBtcWiFiInIQKState;
+
+/*  */
+/* 		Debug related function */
+/*  */
+static u8 halbtcoutsrc_IsBtCoexistAvailable(struct btc_coexist *pBtCoexist)
+{
+	if (!pBtCoexist->bBinded || !pBtCoexist->Adapter)
+		return false;
+
+	return true;
+}
+
+static void halbtcoutsrc_LeaveLps(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+
+	pBtCoexist->btInfo.bBtCtrlLps = true;
+	pBtCoexist->btInfo.bBtLpsOn = false;
+
+	rtw_btcoex_LPS_Leave(padapter);
+}
+
+static void halbtcoutsrc_EnterLps(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+
+	pBtCoexist->btInfo.bBtCtrlLps = true;
+	pBtCoexist->btInfo.bBtLpsOn = true;
+
+	rtw_btcoex_LPS_Enter(padapter);
+}
+
+static void halbtcoutsrc_NormalLps(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+
+	padapter = pBtCoexist->Adapter;
+
+	if (pBtCoexist->btInfo.bBtCtrlLps) {
+		pBtCoexist->btInfo.bBtLpsOn = false;
+		rtw_btcoex_LPS_Leave(padapter);
+		pBtCoexist->btInfo.bBtCtrlLps = false;
+
+		/*  recover the LPS state to the original */
+	}
+}
+
+/*
+ *  Constraint:
+ *   1. this function will request pwrctrl->lock
+ */
+static void halbtcoutsrc_LeaveLowPower(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+	s32 ready;
+	unsigned long stime;
+	unsigned long utime;
+	u32 timeout; /*  unit: ms */
+
+
+	padapter = pBtCoexist->Adapter;
+	ready = _FAIL;
+#ifdef LPS_RPWM_WAIT_MS
+	timeout = LPS_RPWM_WAIT_MS;
+#else /*  !LPS_RPWM_WAIT_MS */
+	timeout = 30;
+#endif /*  !LPS_RPWM_WAIT_MS */
+
+	stime = jiffies;
+	do {
+		ready = rtw_register_task_alive(padapter, BTCOEX_ALIVE);
+		if (_SUCCESS == ready)
+			break;
+
+		utime = jiffies_to_msecs(jiffies - stime);
+		if (utime > timeout)
+			break;
+
+		msleep(1);
+	} while (1);
+}
+
+/*
+ *  Constraint:
+ *   1. this function will request pwrctrl->lock
+ */
+static void halbtcoutsrc_NormalLowPower(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+	rtw_unregister_task_alive(padapter, BTCOEX_ALIVE);
+}
+
+static void halbtcoutsrc_DisableLowPower(struct btc_coexist *pBtCoexist, u8 bLowPwrDisable)
+{
+	pBtCoexist->btInfo.bBtDisableLowPwr = bLowPwrDisable;
+	if (bLowPwrDisable)
+		halbtcoutsrc_LeaveLowPower(pBtCoexist);		/*  leave 32k low power. */
+	else
+		halbtcoutsrc_NormalLowPower(pBtCoexist);	/*  original 32k low power behavior. */
+}
+
+static void halbtcoutsrc_AggregationCheck(struct btc_coexist *pBtCoexist)
+{
+	struct adapter *padapter;
+	bool bNeedToAct;
+
+
+	padapter = pBtCoexist->Adapter;
+	bNeedToAct = false;
+
+	if (pBtCoexist->btInfo.bRejectAggPkt) {
+		rtw_btcoex_RejectApAggregatedPacket(padapter, true);
+	} else {
+		if (pBtCoexist->btInfo.bPreBtCtrlAggBufSize !=
+			pBtCoexist->btInfo.bBtCtrlAggBufSize) {
+			bNeedToAct = true;
+			pBtCoexist->btInfo.bPreBtCtrlAggBufSize = pBtCoexist->btInfo.bBtCtrlAggBufSize;
+		}
+
+		if (pBtCoexist->btInfo.bBtCtrlAggBufSize) {
+			if (pBtCoexist->btInfo.preAggBufSize !=
+				pBtCoexist->btInfo.aggBufSize){
+				bNeedToAct = true;
+			}
+			pBtCoexist->btInfo.preAggBufSize = pBtCoexist->btInfo.aggBufSize;
+		}
+
+		if (bNeedToAct) {
+			rtw_btcoex_RejectApAggregatedPacket(padapter, true);
+			rtw_btcoex_RejectApAggregatedPacket(padapter, false);
+		}
+	}
+}
+
+static u8 halbtcoutsrc_IsWifiBusy(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+
+
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == true) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+			return true;
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			return true;
+	}
+
+	return false;
+}
+
+static u32 _halbtcoutsrc_GetWifiLinkStatus(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+	u8 bp2p;
+	u32 portConnectedStatus;
+
+
+	pmlmepriv = &padapter->mlmepriv;
+	bp2p = false;
+	portConnectedStatus = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == true) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+			if (bp2p)
+				portConnectedStatus |= WIFI_P2P_GO_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_AP_CONNECTED;
+		} else {
+			if (bp2p)
+				portConnectedStatus |= WIFI_P2P_GC_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_STA_CONNECTED;
+		}
+	}
+
+	return portConnectedStatus;
+}
+
+static u32 halbtcoutsrc_GetWifiLinkStatus(struct btc_coexist *pBtCoexist)
+{
+	/*  */
+	/*  return value: */
+	/*  [31:16]=> connected port number */
+	/*  [15:0]=> port connected bit define */
+	/*  */
+
+	struct adapter *padapter;
+	u32 retVal;
+	u32 portConnectedStatus, numOfConnectedPort;
+
+
+	padapter = pBtCoexist->Adapter;
+	portConnectedStatus = 0;
+	numOfConnectedPort = 0;
+
+	retVal = _halbtcoutsrc_GetWifiLinkStatus(padapter);
+	if (retVal) {
+		portConnectedStatus |= retVal;
+		numOfConnectedPort++;
+	}
+
+	retVal = (numOfConnectedPort << 16) | portConnectedStatus;
+
+	return retVal;
+}
+
+static u32 halbtcoutsrc_GetBtPatchVer(struct btc_coexist *pBtCoexist)
+{
+	return pBtCoexist->btInfo.btRealFwVer;
+}
+
+static s32 halbtcoutsrc_GetWifiRssi(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->dmpriv.EntryMinUndecoratedSmoothedPWDB;
+}
+
+static u8 halbtcoutsrc_GetWifiScanAPNum(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext;
+	static u8 scan_AP_num;
+
+	pmlmeext = &padapter->mlmeextpriv;
+
+	if (!GLBtcWiFiInScanState) {
+		if (pmlmeext->sitesurvey_res.bss_cnt > 0xFF)
+			scan_AP_num = 0xFF;
+		else
+			scan_AP_num = (u8)pmlmeext->sitesurvey_res.bss_cnt;
+	}
+
+	return scan_AP_num;
+}
+
+static u8 halbtcoutsrc_Get(void *pBtcContext, u8 getType, void *pOutBuf)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *mlmeext;
+	u8 *pu8;
+	s32 *pS4Tmp;
+	u32 *pU4Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return false;
+
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	mlmeext = &padapter->mlmeextpriv;
+	pu8 = pOutBuf;
+	pS4Tmp = pOutBuf;
+	pU4Tmp = pOutBuf;
+	ret = true;
+
+	switch (getType) {
+	case BTC_GET_BL_HS_OPERATION:
+		*pu8 = false;
+		ret = false;
+		break;
+
+	case BTC_GET_BL_HS_CONNECTING:
+		*pu8 = false;
+		ret = false;
+		break;
+
+	case BTC_GET_BL_WIFI_CONNECTED:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE);
+		break;
+
+	case BTC_GET_BL_WIFI_BUSY:
+		*pu8 = halbtcoutsrc_IsWifiBusy(padapter);
+		break;
+
+	case BTC_GET_BL_WIFI_SCAN:
+		/* Use the value of the new variable GLBtcWiFiInScanState to judge whether WiFi is in scan state or not, since the originally used flag
+			WIFI_SITE_MONITOR in fwstate may not be cleared in time */
+		*pu8 = GLBtcWiFiInScanState;
+		break;
+
+	case BTC_GET_BL_WIFI_LINK:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING);
+		break;
+
+	case BTC_GET_BL_WIFI_ROAM:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING);
+		break;
+
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE);
+		break;
+
+	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
+		*pu8 = padapter->securitypriv.dot11PrivacyAlgrthm != 0;
+		break;
+
+	case BTC_GET_BL_WIFI_UNDER_B_MODE:
+		if (mlmeext->cur_wireless_mode == WIRELESS_11B)
+			*pu8 = true;
+		else
+			*pu8 = false;
+		break;
+
+	case BTC_GET_BL_WIFI_IS_IN_MP_MODE:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_BL_EXT_SWITCH:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_S4_WIFI_RSSI:
+		*pS4Tmp = halbtcoutsrc_GetWifiRssi(padapter);
+		break;
+
+	case BTC_GET_S4_HS_RSSI:
+		*pS4Tmp = 0;
+		ret = false;
+		break;
+
+	case BTC_GET_U4_WIFI_BW:
+		if (is_legacy_only(mlmeext->cur_wireless_mode))
+			*pU4Tmp = BTC_WIFI_BW_LEGACY;
+		else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_20)
+			*pU4Tmp = BTC_WIFI_BW_HT20;
+		else
+			*pU4Tmp = BTC_WIFI_BW_HT40;
+		break;
+
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		{
+			struct rt_link_detect_t *plinkinfo;
+			plinkinfo = &padapter->mlmepriv.LinkDetectInfo;
+
+			if (plinkinfo->NumTxOkInPeriod > plinkinfo->NumRxOkInPeriod)
+				*pU4Tmp = BTC_WIFI_TRAFFIC_TX;
+			else
+				*pU4Tmp = BTC_WIFI_TRAFFIC_RX;
+		}
+		break;
+
+	case BTC_GET_U4_WIFI_FW_VER:
+		*pU4Tmp = pHalData->FirmwareVersion << 16;
+		*pU4Tmp |= pHalData->FirmwareSubVersion;
+		break;
+
+	case BTC_GET_U4_WIFI_LINK_STATUS:
+		*pU4Tmp = halbtcoutsrc_GetWifiLinkStatus(pBtCoexist);
+		break;
+
+	case BTC_GET_U4_BT_PATCH_VER:
+		*pU4Tmp = halbtcoutsrc_GetBtPatchVer(pBtCoexist);
+		break;
+
+	case BTC_GET_U1_WIFI_DOT11_CHNL:
+		*pu8 = padapter->mlmeextpriv.cur_channel;
+		break;
+
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*pu8 = pHalData->CurrentChannel;
+		break;
+
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*pu8 = 0;
+		ret = false;
+		break;
+
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*pu8 = BTC_SMSP;
+/* 			*pU1Tmp = BTC_DMSP; */
+/* 			*pU1Tmp = BTC_DMDP; */
+/* 			*pU1Tmp = BTC_MP_UNKNOWN; */
+		break;
+
+	case BTC_GET_U1_AP_NUM:
+		*pu8 = halbtcoutsrc_GetWifiScanAPNum(padapter);
+		break;
+
+	/* 1Ant =========== */
+	case BTC_GET_U1_LPS_MODE:
+		*pu8 = padapter->dvobj->pwrctl_priv.pwr_mode;
+		break;
+
+	default:
+		ret = false;
+		break;
+	}
+
+	return ret;
+}
+
+static u8 halbtcoutsrc_Set(void *pBtcContext, u8 setType, void *pInBuf)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+	u8 *pu8;
+	u32 *pU4Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+	pu8 = pInBuf;
+	pU4Tmp = pInBuf;
+	ret = true;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return false;
+
+	switch (setType) {
+	/*  set some u8 type variables. */
+	case BTC_SET_BL_BT_DISABLE:
+		pBtCoexist->btInfo.bBtDisabled = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_TRAFFIC_BUSY:
+		pBtCoexist->btInfo.bBtBusy = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_LIMITED_DIG:
+		pBtCoexist->btInfo.bLimitedDig = *pu8;
+		break;
+
+	case BTC_SET_BL_FORCE_TO_ROAM:
+		pBtCoexist->btInfo.bForceToRoam = *pu8;
+		break;
+
+	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
+		pBtCoexist->btInfo.bRejectAggPkt = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
+		pBtCoexist->btInfo.bBtCtrlAggBufSize = *pu8;
+		break;
+
+	case BTC_SET_BL_INC_SCAN_DEV_NUM:
+		pBtCoexist->btInfo.bIncreaseScanDevNum = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_TX_RX_MASK:
+		pBtCoexist->btInfo.bBtTxRxMask = *pu8;
+		break;
+
+	/*  set some u8 type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
+		pBtCoexist->btInfo.rssiAdjustForAgcTableOn = *pu8;
+		break;
+
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		pBtCoexist->btInfo.aggBufSize = *pu8;
+		break;
+
+	/*  the following are some action which will be triggered */
+	case BTC_SET_ACT_GET_BT_RSSI:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_AGGREGATE_CTRL:
+		halbtcoutsrc_AggregationCheck(pBtCoexist);
+		break;
+
+	/* 1Ant =========== */
+	/*  set some u8 type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
+		pBtCoexist->btInfo.rssiAdjustFor1AntCoexType = *pu8;
+		break;
+
+	case BTC_SET_U1_LPS_VAL:
+		pBtCoexist->btInfo.lpsVal = *pu8;
+		break;
+
+	case BTC_SET_U1_RPWM_VAL:
+		pBtCoexist->btInfo.rpwmVal = *pu8;
+		break;
+
+	/*  the following are some action which will be triggered */
+	case BTC_SET_ACT_LEAVE_LPS:
+		halbtcoutsrc_LeaveLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_ENTER_LPS:
+		halbtcoutsrc_EnterLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_NORMAL_LPS:
+		halbtcoutsrc_NormalLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_DISABLE_LOW_POWER:
+		halbtcoutsrc_DisableLowPower(pBtCoexist, *pu8);
+		break;
+
+	case BTC_SET_ACT_UPDATE_RAMASK:
+		pBtCoexist->btInfo.raMask = *pU4Tmp;
+
+		if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == true) {
+			struct sta_info *psta;
+			struct wlan_bssid_ex *cur_network;
+
+			cur_network = &padapter->mlmeextpriv.mlmext_info.network;
+			psta = rtw_get_stainfo(&padapter->stapriv, cur_network->mac_address);
+			rtw_hal_update_ra_mask(psta, 0);
+		}
+		break;
+
+	case BTC_SET_ACT_SEND_MIMO_PS:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_CTRL_BT_INFO:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_CTRL_BT_COEX:
+		ret = false;
+		break;
+	case BTC_SET_ACT_CTRL_8723B_ANT:
+		ret = false;
+		break;
+	/*  */
+	default:
+		ret = false;
+		break;
+	}
+
+	return ret;
+}
+
+/*  */
+/* 		IO related function */
+/*  */
+static u8 halbtcoutsrc_Read1Byte(void *pBtcContext, u32 RegAddr)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return rtw_read8(padapter, RegAddr);
+}
+
+static u16 halbtcoutsrc_Read2Byte(void *pBtcContext, u32 RegAddr)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read16(padapter, RegAddr);
+}
+
+static u32 halbtcoutsrc_Read4Byte(void *pBtcContext, u32 RegAddr)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read32(padapter, RegAddr);
+}
+
+static void halbtcoutsrc_Write1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write8(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_BitMaskWrite1Byte(void *pBtcContext, u32 regAddr, u8 bitMask, u8 data1b)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+	u8 originalValue, bitShift;
+	u8 i;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+	originalValue = 0;
+	bitShift = 0;
+
+	if (bitMask != 0xFF) {
+		originalValue = rtw_read8(padapter, regAddr);
+
+		for (i = 0; i <= 7; i++) {
+			if ((bitMask >> i) & 0x1)
+				break;
+		}
+		bitShift = i;
+
+		data1b = (originalValue & ~bitMask) | ((data1b << bitShift) & bitMask);
+	}
+
+	rtw_write8(padapter, regAddr, data1b);
+}
+
+static void halbtcoutsrc_Write2Byte(void *pBtcContext, u32 RegAddr, u16 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write16(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_Write4Byte(void *pBtcContext, u32 RegAddr, u32 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write32(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_WriteLocalReg1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	struct btc_coexist *pBtCoexist = (struct btc_coexist *)pBtcContext;
+	struct adapter *Adapter = pBtCoexist->Adapter;
+
+	if (BTC_INTF_SDIO == pBtCoexist->chipInterface)
+		rtw_write8(Adapter, SDIO_LOCAL_BASE | RegAddr, Data);
+	else
+		rtw_write8(Adapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_SetBbReg(void *pBtcContext, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetBBReg(padapter, RegAddr, BitMask, Data);
+}
+
+
+static u32 halbtcoutsrc_GetBbReg(void *pBtcContext, u32 RegAddr, u32 BitMask)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return PHY_QueryBBReg(padapter, RegAddr, BitMask);
+}
+
+static void halbtcoutsrc_SetRfReg(void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetRFReg(padapter, eRFPath, RegAddr, BitMask, Data);
+}
+
+static u32 halbtcoutsrc_GetRfReg(void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return PHY_QueryRFReg(padapter, eRFPath, RegAddr, BitMask);
+}
+
+static void halbtcoutsrc_SetBtReg(void *pBtcContext, u8 RegType, u32 RegAddr, u32 Data)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+	u8 CmdBuffer1[4] = {0};
+	u8 CmdBuffer2[4] = {0};
+	u8 *AddrToSet = (u8 *)&RegAddr;
+	u8 *ValueToSet = (u8 *)&Data;
+	u8 OperVer = 0;
+	u8 ReqNum = 0;
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	CmdBuffer1[0] |= (OperVer & 0x0f);						/* Set OperVer */
+	CmdBuffer1[0] |= ((ReqNum << 4) & 0xf0);				/* Set ReqNum */
+	CmdBuffer1[1] = 0x0d;									/* Set OpCode to BT_LO_OP_WRITE_REG_VALUE */
+	CmdBuffer1[2] = ValueToSet[0];							/* Set WriteRegValue */
+	rtw_hal_fill_h2c_cmd(padapter, 0x67, 4, &(CmdBuffer1[0]));
+
+	msleep(200);
+	ReqNum++;
+
+	CmdBuffer2[0] |= (OperVer & 0x0f);						/* Set OperVer */
+	CmdBuffer2[0] |= ((ReqNum << 4) & 0xf0);				/* Set ReqNum */
+	CmdBuffer2[1] = 0x0c;									/* Set OpCode of BT_LO_OP_WRITE_REG_ADDR */
+	CmdBuffer2[3] = AddrToSet[0];							/* Set WriteRegAddr */
+	rtw_hal_fill_h2c_cmd(padapter, 0x67, 4, &(CmdBuffer2[0]));
+}
+
+static u32 halbtcoutsrc_GetBtReg(void *pBtcContext, u8 RegType, u32 RegAddr)
+{
+	/* To be implemented. Always return 0 temporarily */
+	return 0;
+}
+
+static void halbtcoutsrc_FillH2cCmd(void *pBtcContext, u8 elementId, u32 cmdLen, u8 *pCmdBuffer)
+{
+	struct btc_coexist *pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (struct btc_coexist *)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_hal_fill_h2c_cmd(padapter, elementId, cmdLen, pCmdBuffer);
+}
+
+/*  */
+/* 		Extern functions called by other module */
+/*  */
+static u8 EXhalbtcoutsrc_BindBtCoexWithAdapter(void *padapter)
+{
+	struct btc_coexist *pBtCoexist = &GLBtCoexist;
+
+	if (pBtCoexist->bBinded)
+		return false;
+	else
+		pBtCoexist->bBinded = true;
+
+	pBtCoexist->statistics.cntBind++;
+
+	pBtCoexist->Adapter = padapter;
+
+	pBtCoexist->stackInfo.bProfileNotified = false;
+
+	pBtCoexist->btInfo.bBtCtrlAggBufSize = false;
+	pBtCoexist->btInfo.aggBufSize = 5;
+
+	pBtCoexist->btInfo.bIncreaseScanDevNum = false;
+
+	/*  set default antenna position to main  port */
+	pBtCoexist->boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+
+	return true;
+}
+
+void hal_btcoex_Initialize(void *padapter)
+{
+	struct btc_coexist *pBtCoexist;
+
+	memset(&GLBtCoexist, 0, sizeof(GLBtCoexist));
+
+	pBtCoexist = &GLBtCoexist;
+
+	/* pBtCoexist->statistics.cntBind++; */
+
+	pBtCoexist->chipInterface = BTC_INTF_SDIO;
+
+	EXhalbtcoutsrc_BindBtCoexWithAdapter(padapter);
+
+	pBtCoexist->fBtcRead1Byte = halbtcoutsrc_Read1Byte;
+	pBtCoexist->fBtcWrite1Byte = halbtcoutsrc_Write1Byte;
+	pBtCoexist->fBtcWrite1ByteBitMask = halbtcoutsrc_BitMaskWrite1Byte;
+	pBtCoexist->fBtcRead2Byte = halbtcoutsrc_Read2Byte;
+	pBtCoexist->fBtcWrite2Byte = halbtcoutsrc_Write2Byte;
+	pBtCoexist->fBtcRead4Byte = halbtcoutsrc_Read4Byte;
+	pBtCoexist->fBtcWrite4Byte = halbtcoutsrc_Write4Byte;
+	pBtCoexist->fBtcWriteLocalReg1Byte = halbtcoutsrc_WriteLocalReg1Byte;
+
+	pBtCoexist->fBtcSetBbReg = halbtcoutsrc_SetBbReg;
+	pBtCoexist->fBtcGetBbReg = halbtcoutsrc_GetBbReg;
+
+	pBtCoexist->fBtcSetRfReg = halbtcoutsrc_SetRfReg;
+	pBtCoexist->fBtcGetRfReg = halbtcoutsrc_GetRfReg;
+
+	pBtCoexist->fBtcFillH2c = halbtcoutsrc_FillH2cCmd;
+
+	pBtCoexist->fBtcGet = halbtcoutsrc_Get;
+	pBtCoexist->fBtcSet = halbtcoutsrc_Set;
+	pBtCoexist->fBtcGetBtReg = halbtcoutsrc_GetBtReg;
+	pBtCoexist->fBtcSetBtReg = halbtcoutsrc_SetBtReg;
+
+	pBtCoexist->boardInfo.singleAntPath = 0;
+
+	GLBtcWiFiInScanState = false;
+
+	GLBtcWiFiInIQKState = false;
+}
+
+void EXhalbtcoutsrc_PowerOnSetting(struct btc_coexist *pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	/* Power on setting function is only added in 8723B currently */
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_PowerOnSetting(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_PowerOnSetting(pBtCoexist);
+}
+
+void EXhalbtcoutsrc_InitHwConfig(struct btc_coexist *pBtCoexist, u8 bWifiOnly)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntInitHwConfig++;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_InitHwConfig(pBtCoexist, bWifiOnly);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_InitHwConfig(pBtCoexist, bWifiOnly);
+}
+
+void EXhalbtcoutsrc_InitCoexDm(struct btc_coexist *pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntInitCoexDm++;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_InitCoexDm(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_InitCoexDm(pBtCoexist);
+
+	pBtCoexist->bInitilized = true;
+}
+
+void EXhalbtcoutsrc_IpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	u8 ipsType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntIpsNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (IPS_NONE == type)
+		ipsType = BTC_IPS_LEAVE;
+	else
+		ipsType = BTC_IPS_ENTER;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_IpsNotify(pBtCoexist, ipsType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_IpsNotify(pBtCoexist, ipsType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_LpsNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	u8 lpsType;
+
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntLpsNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (PS_MODE_ACTIVE == type)
+		lpsType = BTC_LPS_DISABLE;
+	else
+		lpsType = BTC_LPS_ENABLE;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_LpsNotify(pBtCoexist, lpsType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_LpsNotify(pBtCoexist, lpsType);
+}
+
+void EXhalbtcoutsrc_ScanNotify(struct btc_coexist *pBtCoexist, u8 type)
+{
+	u8 scanType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntScanNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (type) {
+		scanType = BTC_SCAN_START;
+		GLBtcWiFiInScanState = true;
+	} else {
+		scanType = BTC_SCAN_FINISH;
+		GLBtcWiFiInScanState = false;
+	}
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_ScanNotify(pBtCoexist, scanType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_ScanNotify(pBtCoexist, scanType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_ConnectNotify(struct btc_coexist *pBtCoexist, u8 action)
+{
+	u8 assoType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntConnectNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (action)
+		assoType = BTC_ASSOCIATE_START;
+	else
+		assoType = BTC_ASSOCIATE_FINISH;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_ConnectNotify(pBtCoexist, assoType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_ConnectNotify(pBtCoexist, assoType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_MediaStatusNotify(struct btc_coexist *pBtCoexist, enum
+	rt_media_status mediaStatus)
+{
+	u8 mStatus;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntMediaStatusNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (RT_MEDIA_CONNECT == mediaStatus)
+		mStatus = BTC_MEDIA_CONNECT;
+	else
+		mStatus = BTC_MEDIA_DISCONNECT;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, mStatus);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, mStatus);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_SpecialPacketNotify(struct btc_coexist *pBtCoexist, u8 pktType)
+{
+	u8 packetType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntSpecialPacketNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (PACKET_DHCP == pktType) {
+		packetType = BTC_PACKET_DHCP;
+	} else if (PACKET_EAPOL == pktType) {
+		packetType = BTC_PACKET_EAPOL;
+	} else if (PACKET_ARP == pktType) {
+		packetType = BTC_PACKET_ARP;
+	} else {
+		return;
+	}
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_SpecialPacketNotify(pBtCoexist, packetType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_SpecialPacketNotify(pBtCoexist, packetType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_BtInfoNotify(struct btc_coexist *pBtCoexist, u8 *tmpBuf, u8 length)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntBtInfoNotify++;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_BtInfoNotify(pBtCoexist, tmpBuf, length);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_BtInfoNotify(pBtCoexist, tmpBuf, length);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_HaltNotify(struct btc_coexist *pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_HaltNotify(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_HaltNotify(pBtCoexist);
+
+	pBtCoexist->bBinded = false;
+}
+
+void EXhalbtcoutsrc_PnpNotify(struct btc_coexist *pBtCoexist, u8 pnpState)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	/*  */
+	/*  currently only 1ant we have to do the notification, */
+	/*  once pnp is notified to sleep state, we have to leave LPS that we can sleep normally. */
+	/*  */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_PnpNotify(pBtCoexist, pnpState);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_PnpNotify(pBtCoexist, pnpState);
+}
+
+void EXhalbtcoutsrc_Periodical(struct btc_coexist *pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntPeriodical++;
+
+	/*  Periodical should be called in cmd thread, */
+	/*  don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_Periodical(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_Periodical(pBtCoexist);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_SetAntNum(u8 type, u8 antNum)
+{
+	if (BT_COEX_ANT_TYPE_PG == type) {
+		GLBtCoexist.boardInfo.pgAntNum = antNum;
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+		/* GLBtCoexist.boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT; */
+	} else if (BT_COEX_ANT_TYPE_DETECTED == type) {
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+		/* GLBtCoexist.boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT; */
+	}
+}
+
+/*  */
+/*  Currently used by 8723b only, S0 or S1 */
+/*  */
+void EXhalbtcoutsrc_SetSingleAntPath(u8 singleAntPath)
+{
+	GLBtCoexist.boardInfo.singleAntPath = singleAntPath;
+}
+
+/*
+ * Description:
+ *Run BT-Coexist mechanism or not
+ *
+ */
+void hal_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->bt_coexist.bBtExist = bBtExist;
+}
+
+/*
+ * Dewcription:
+ *Check is co-exist mechanism enabled or not
+ *
+ * Return:
+ *true	Enable BT co-exist mechanism
+ *false	Disable BT co-exist mechanism
+ */
+bool hal_btcoex_IsBtExist(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	return pHalData->bt_coexist.bBtExist;
+}
+
+bool hal_btcoex_IsBtDisabled(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return true;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return true;
+	else
+		return false;
+}
+
+void hal_btcoex_SetChipType(struct adapter *padapter, u8 chipType)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+}
+
+void hal_btcoex_SetPgAntNum(struct adapter *padapter, u8 antNum)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->bt_coexist.btTotalAntNum = antNum;
+	EXhalbtcoutsrc_SetAntNum(BT_COEX_ANT_TYPE_PG, antNum);
+}
+
+void hal_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath)
+{
+	EXhalbtcoutsrc_SetSingleAntPath(singleAntPath);
+}
+
+void hal_btcoex_PowerOnSetting(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_PowerOnSetting(&GLBtCoexist);
+}
+
+void hal_btcoex_InitHwConfig(struct adapter *padapter, u8 bWifiOnly)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return;
+
+	EXhalbtcoutsrc_InitHwConfig(&GLBtCoexist, bWifiOnly);
+	EXhalbtcoutsrc_InitCoexDm(&GLBtCoexist);
+}
+
+void hal_btcoex_IpsNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_IpsNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_LpsNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_LpsNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_ScanNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_ScanNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_ConnectNotify(struct adapter *padapter, u8 action)
+{
+	EXhalbtcoutsrc_ConnectNotify(&GLBtCoexist, action);
+}
+
+void hal_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus)
+{
+	EXhalbtcoutsrc_MediaStatusNotify(&GLBtCoexist, mediaStatus);
+}
+
+void hal_btcoex_SpecialPacketNotify(struct adapter *padapter, u8 pktType)
+{
+	EXhalbtcoutsrc_SpecialPacketNotify(&GLBtCoexist, pktType);
+}
+
+void hal_btcoex_IQKNotify(struct adapter *padapter, u8 state)
+{
+	GLBtcWiFiInIQKState = state;
+}
+
+void hal_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf)
+{
+	if (GLBtcWiFiInIQKState)
+		return;
+
+	EXhalbtcoutsrc_BtInfoNotify(&GLBtCoexist, tmpBuf, length);
+}
+
+void hal_btcoex_SuspendNotify(struct adapter *padapter, u8 state)
+{
+	if (state == 1)
+		state = BTC_WIFI_PNP_SLEEP;
+	else
+		state = BTC_WIFI_PNP_WAKE_UP;
+
+	EXhalbtcoutsrc_PnpNotify(&GLBtCoexist, state);
+}
+
+void hal_btcoex_HaltNotify(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_HaltNotify(&GLBtCoexist);
+}
+
+void hal_btcoex_Handler(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_Periodical(&GLBtCoexist);
+}
+
+s32 hal_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter)
+{
+	return (s32)GLBtCoexist.btInfo.bBtCtrlAggBufSize;
+}
+
+bool hal_btcoex_IsBtControlLps(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtCtrlLps)
+		return true;
+
+	return false;
+}
+
+bool hal_btcoex_IsLpsOn(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtLpsOn)
+		return true;
+
+	return false;
+}
+
+u8 hal_btcoex_RpwmVal(struct adapter *padapter)
+{
+	return GLBtCoexist.btInfo.rpwmVal;
+}
+
+u8 hal_btcoex_LpsVal(struct adapter *padapter)
+{
+	return GLBtCoexist.btInfo.lpsVal;
+}
+
+u32 hal_btcoex_GetRaMask(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return 0;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return 0;
+
+	if (GLBtCoexist.boardInfo.btdmAntNum != 1)
+		return 0;
+
+	return GLBtCoexist.btInfo.raMask;
+}
+
+void hal_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen)
+{
+	memcpy(GLBtCoexist.pwrModeVal, pCmdBuf, cmdLen);
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_com.c b/drivers/staging/rtl8723bs/hal/hal_com.c
new file mode 100644
index 000000000..e42556d03
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_com.c
@@ -0,0 +1,1096 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <linux/kernel.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include "hal_com_h2c.h"
+
+#include "odm_precomp.h"
+
+u8 rtw_hal_data_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter)) {	/* if (padapter->isprimary) */
+		padapter->hal_data_sz = sizeof(struct hal_com_data);
+		padapter->HalData = vzalloc(padapter->hal_data_sz);
+		if (!padapter->HalData)
+			return _FAIL;
+	}
+	return _SUCCESS;
+}
+
+void rtw_hal_data_deinit(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter)) {	/* if (padapter->isprimary) */
+		if (padapter->HalData) {
+			vfree(padapter->HalData);
+			padapter->HalData = NULL;
+			padapter->hal_data_sz = 0;
+		}
+	}
+}
+
+
+void dump_chip_info(struct hal_version	ChipVersion)
+{
+	char buf[128];
+	size_t cnt = 0;
+
+	cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "Chip Version Info: CHIP_8723B_%s_",
+			IS_NORMAL_CHIP(ChipVersion) ? "Normal_Chip" : "Test_Chip");
+
+	if (IS_CHIP_VENDOR_TSMC(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "TSMC_");
+	else if (IS_CHIP_VENDOR_UMC(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "UMC_");
+	else if (IS_CHIP_VENDOR_SMIC(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "SMIC_");
+
+	if (IS_A_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "A_CUT_");
+	else if (IS_B_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "B_CUT_");
+	else if (IS_C_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "C_CUT_");
+	else if (IS_D_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "D_CUT_");
+	else if (IS_E_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "E_CUT_");
+	else if (IS_I_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "I_CUT_");
+	else if (IS_J_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "J_CUT_");
+	else if (IS_K_CUT(ChipVersion))
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "K_CUT_");
+	else
+		cnt += scnprintf(buf + cnt, sizeof(buf) - cnt,
+				"UNKNOWN_CUT(%d)_", ChipVersion.CUTVersion);
+
+	cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "1T1R_");
+
+	cnt += scnprintf(buf + cnt, sizeof(buf) - cnt, "RomVer(%d)\n", ChipVersion.ROMVer);
+}
+
+
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK	0x80
+
+/*
+ * Description:
+ *Use hardware(efuse), driver parameter(registry) and default channel plan
+ *to decide which one should be used.
+ *
+ * Parameters:
+ *padapter			pointer of adapter
+ *hw_channel_plan		channel plan from HW (efuse/eeprom)
+ *					BIT[7] software configure mode; 0:Enable, 1:disable
+ *					BIT[6:0] Channel Plan
+ *sw_channel_plan		channel plan from SW (registry/module param)
+ *def_channel_plan	channel plan used when HW/SW both invalid
+ *AutoLoadFail		efuse autoload fail or not
+ *
+ * Return:
+ *Final channel plan decision
+ *
+ */
+u8 hal_com_config_channel_plan(
+	struct adapter *padapter,
+	u8 hw_channel_plan,
+	u8 sw_channel_plan,
+	u8 def_channel_plan,
+	bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData;
+	u8 chnlPlan;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->bDisableSWChannelPlan = false;
+	chnlPlan = def_channel_plan;
+
+	if (0xFF == hw_channel_plan)
+		AutoLoadFail = true;
+
+	if (!AutoLoadFail) {
+		u8 hw_chnlPlan;
+
+		hw_chnlPlan = hw_channel_plan & (~EEPROM_CHANNEL_PLAN_BY_HW_MASK);
+		if (rtw_is_channel_plan_valid(hw_chnlPlan)) {
+			if (hw_channel_plan & EEPROM_CHANNEL_PLAN_BY_HW_MASK)
+				pHalData->bDisableSWChannelPlan = true;
+
+			chnlPlan = hw_chnlPlan;
+		}
+	}
+
+	if (
+		(false == pHalData->bDisableSWChannelPlan) &&
+		rtw_is_channel_plan_valid(sw_channel_plan)
+	)
+		chnlPlan = sw_channel_plan;
+
+	return chnlPlan;
+}
+
+bool HAL_IsLegalChannel(struct adapter *adapter, u32 Channel)
+{
+	bool bLegalChannel = true;
+
+	if ((Channel <= 14) && (Channel >= 1)) {
+		if (is_supported_24g(adapter->registrypriv.wireless_mode) == false)
+			bLegalChannel = false;
+	} else {
+		bLegalChannel = false;
+	}
+
+	return bLegalChannel;
+}
+
+u8 MRateToHwRate(u8 rate)
+{
+	u8 ret = DESC_RATE1M;
+
+	switch (rate) {
+	case MGN_1M:
+		ret = DESC_RATE1M;
+		break;
+	case MGN_2M:
+		ret = DESC_RATE2M;
+		break;
+	case MGN_5_5M:
+		ret = DESC_RATE5_5M;
+		break;
+	case MGN_11M:
+		ret = DESC_RATE11M;
+		break;
+	case MGN_6M:
+		ret = DESC_RATE6M;
+		break;
+	case MGN_9M:
+		ret = DESC_RATE9M;
+		break;
+	case MGN_12M:
+		ret = DESC_RATE12M;
+		break;
+	case MGN_18M:
+		ret = DESC_RATE18M;
+		break;
+	case MGN_24M:
+		ret = DESC_RATE24M;
+		break;
+	case MGN_36M:
+		ret = DESC_RATE36M;
+		break;
+	case MGN_48M:
+		ret = DESC_RATE48M;
+		break;
+	case MGN_54M:
+		ret = DESC_RATE54M;
+		break;
+	case MGN_MCS0:
+		ret = DESC_RATEMCS0;
+		break;
+	case MGN_MCS1:
+		ret = DESC_RATEMCS1;
+		break;
+	case MGN_MCS2:
+		ret = DESC_RATEMCS2;
+		break;
+	case MGN_MCS3:
+		ret = DESC_RATEMCS3;
+		break;
+	case MGN_MCS4:
+		ret = DESC_RATEMCS4;
+		break;
+	case MGN_MCS5:
+		ret = DESC_RATEMCS5;
+		break;
+	case MGN_MCS6:
+		ret = DESC_RATEMCS6;
+		break;
+	case MGN_MCS7:
+		ret = DESC_RATEMCS7;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+u8 HwRateToMRate(u8 rate)
+{
+	u8 ret_rate = MGN_1M;
+
+	switch (rate) {
+	case DESC_RATE1M:
+		ret_rate = MGN_1M;
+		break;
+	case DESC_RATE2M:
+		ret_rate = MGN_2M;
+		break;
+	case DESC_RATE5_5M:
+		ret_rate = MGN_5_5M;
+		break;
+	case DESC_RATE11M:
+		ret_rate = MGN_11M;
+		break;
+	case DESC_RATE6M:
+		ret_rate = MGN_6M;
+		break;
+	case DESC_RATE9M:
+		ret_rate = MGN_9M;
+		break;
+	case DESC_RATE12M:
+		ret_rate = MGN_12M;
+		break;
+	case DESC_RATE18M:
+		ret_rate = MGN_18M;
+		break;
+	case DESC_RATE24M:
+		ret_rate = MGN_24M;
+		break;
+	case DESC_RATE36M:
+		ret_rate = MGN_36M;
+		break;
+	case DESC_RATE48M:
+		ret_rate = MGN_48M;
+		break;
+	case DESC_RATE54M:
+		ret_rate = MGN_54M;
+		break;
+	case DESC_RATEMCS0:
+		ret_rate = MGN_MCS0;
+		break;
+	case DESC_RATEMCS1:
+		ret_rate = MGN_MCS1;
+		break;
+	case DESC_RATEMCS2:
+		ret_rate = MGN_MCS2;
+		break;
+	case DESC_RATEMCS3:
+		ret_rate = MGN_MCS3;
+		break;
+	case DESC_RATEMCS4:
+		ret_rate = MGN_MCS4;
+		break;
+	case DESC_RATEMCS5:
+		ret_rate = MGN_MCS5;
+		break;
+	case DESC_RATEMCS6:
+		ret_rate = MGN_MCS6;
+		break;
+	case DESC_RATEMCS7:
+		ret_rate = MGN_MCS7;
+		break;
+	default:
+		break;
+	}
+
+	return ret_rate;
+}
+
+void HalSetBrateCfg(struct adapter *Adapter, u8 *mBratesOS, u16 *pBrateCfg)
+{
+	u8 i, is_brate, brate;
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+
+		is_brate = mBratesOS[i] & IEEE80211_BASIC_RATE_MASK;
+		brate = mBratesOS[i] & 0x7f;
+
+		if (is_brate) {
+			switch (brate) {
+			case IEEE80211_CCK_RATE_1MB:
+				*pBrateCfg |= RATE_1M;
+				break;
+			case IEEE80211_CCK_RATE_2MB:
+				*pBrateCfg |= RATE_2M;
+				break;
+			case IEEE80211_CCK_RATE_5MB:
+				*pBrateCfg |= RATE_5_5M;
+				break;
+			case IEEE80211_CCK_RATE_11MB:
+				*pBrateCfg |= RATE_11M;
+				break;
+			case IEEE80211_OFDM_RATE_6MB:
+				*pBrateCfg |= RATE_6M;
+				break;
+			case IEEE80211_OFDM_RATE_9MB:
+				*pBrateCfg |= RATE_9M;
+				break;
+			case IEEE80211_OFDM_RATE_12MB:
+				*pBrateCfg |= RATE_12M;
+				break;
+			case IEEE80211_OFDM_RATE_18MB:
+				*pBrateCfg |= RATE_18M;
+				break;
+			case IEEE80211_OFDM_RATE_24MB:
+				*pBrateCfg |= RATE_24M;
+				break;
+			case IEEE80211_OFDM_RATE_36MB:
+				*pBrateCfg |= RATE_36M;
+				break;
+			case IEEE80211_OFDM_RATE_48MB:
+				*pBrateCfg |= RATE_48M;
+				break;
+			case IEEE80211_OFDM_RATE_54MB:
+				*pBrateCfg |= RATE_54M;
+				break;
+			}
+		}
+	}
+}
+
+static void _OneOutPipeMapping(struct adapter *padapter)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+	pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+	pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[0];/* BE */
+	pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+
+	pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+	pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+	pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+	pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+}
+
+static void _TwoOutPipeMapping(struct adapter *padapter, bool bWIFICfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	if (bWIFICfg) { /* WMM */
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   0,		1,	0,	1,	0,	0,	0,	0,		0	}; */
+		/* 0:ep_0 num, 1:ep_1 num */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[1];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[1];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	} else { /* typical setting */
+
+
+		/* BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   1,		1,	0,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:ep_0 num, 1:ep_1 num */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[1];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[1];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	}
+
+}
+
+static void _ThreeOutPipeMapping(struct adapter *padapter, bool bWIFICfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	if (bWIFICfg) { /* for WMM */
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   1,		2,	1,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:H, 1:N, 2:L */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[1];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[2];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[1];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	} else { /* typical setting */
+
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   2,		2,	1,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:H, 1:N, 2:L */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[1];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[2];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[2];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+	}
+
+}
+
+bool Hal_MappingOutPipe(struct adapter *padapter, u8 NumOutPipe)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	bool bWIFICfg = (pregistrypriv->wifi_spec) ? true : false;
+
+	bool result = true;
+
+	switch (NumOutPipe) {
+	case 2:
+		_TwoOutPipeMapping(padapter, bWIFICfg);
+		break;
+	case 3:
+	case 4:
+		_ThreeOutPipeMapping(padapter, bWIFICfg);
+		break;
+	case 1:
+		_OneOutPipeMapping(padapter);
+		break;
+	default:
+		result = false;
+		break;
+	}
+
+	return result;
+
+}
+
+void hal_init_macaddr(struct adapter *adapter)
+{
+	rtw_hal_set_hwreg(adapter, HW_VAR_MAC_ADDR, adapter->eeprompriv.mac_addr);
+}
+
+void rtw_init_hal_com_default_value(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	pHalData->AntDetection = 1;
+}
+
+/*
+* C2H event format:
+* Field	 TRIGGER		CONTENT	   CMD_SEQ	CMD_LEN		 CMD_ID
+* BITS	 [127:120]	[119:16]      [15:8]		  [7:4]		   [3:0]
+*/
+
+void c2h_evt_clear(struct adapter *adapter)
+{
+	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+}
+
+/*
+* C2H event format:
+* Field    TRIGGER    CMD_LEN    CONTENT    CMD_SEQ    CMD_ID
+* BITS    [127:120]   [119:112]    [111:16]	     [15:8]         [7:0]
+*/
+s32 c2h_evt_read_88xx(struct adapter *adapter, u8 *buf)
+{
+	s32 ret = _FAIL;
+	struct c2h_evt_hdr_88xx *c2h_evt;
+	int i;
+	u8 trigger;
+
+	if (!buf)
+		goto exit;
+
+	trigger = rtw_read8(adapter, REG_C2HEVT_CLEAR);
+
+	if (trigger == C2H_EVT_HOST_CLOSE)
+		goto exit; /* Not ready */
+	else if (trigger != C2H_EVT_FW_CLOSE)
+		goto clear_evt; /* Not a valid value */
+
+	c2h_evt = (struct c2h_evt_hdr_88xx *)buf;
+
+	memset(c2h_evt, 0, 16);
+
+	c2h_evt->id = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL);
+	c2h_evt->seq = rtw_read8(adapter, REG_C2HEVT_CMD_SEQ_88XX);
+	c2h_evt->plen = rtw_read8(adapter, REG_C2HEVT_CMD_LEN_88XX);
+
+	/* Read the content */
+	for (i = 0; i < c2h_evt->plen; i++)
+		c2h_evt->payload[i] = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 2 + i);
+
+	ret = _SUCCESS;
+
+clear_evt:
+	/*
+	* Clear event to notify FW we have read the command.
+	* If this field isn't clear, the FW won't update the next command message.
+	*/
+	c2h_evt_clear(adapter);
+exit:
+	return ret;
+}
+
+u8 rtw_get_mgntframe_raid(struct adapter *adapter, unsigned char network_type)
+{
+	return (network_type & WIRELESS_11B) ? RATEID_IDX_B : RATEID_IDX_G;
+}
+
+void rtw_hal_update_sta_rate_mask(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 i, limit;
+	u32 tx_ra_bitmap;
+
+	if (!psta)
+		return;
+
+	tx_ra_bitmap = 0;
+
+	/* b/g mode ra_bitmap */
+	for (i = 0; i < sizeof(psta->bssrateset); i++) {
+		if (psta->bssrateset[i])
+			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);
+	}
+
+	/* n mode ra_bitmap */
+	if (psta->htpriv.ht_option) {
+		limit = 8; /*   1R */
+
+		for (i = 0; i < limit; i++) {
+			if (psta->htpriv.ht_cap.mcs.rx_mask[i/8] & BIT(i%8))
+				tx_ra_bitmap |= BIT(i+12);
+		}
+	}
+
+	psta->ra_mask = tx_ra_bitmap;
+	psta->init_rate = get_highest_rate_idx(tx_ra_bitmap)&0x3f;
+}
+
+void hw_var_port_switch(struct adapter *adapter)
+{
+}
+
+void SetHwReg(struct adapter *adapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	struct dm_odm_t *odm = &(hal_data->odmpriv);
+
+	switch (variable) {
+	case HW_VAR_PORT_SWITCH:
+		hw_var_port_switch(adapter);
+		break;
+	case HW_VAR_INIT_RTS_RATE:
+		rtw_warn_on(1);
+		break;
+	case HW_VAR_SEC_CFG:
+	{
+		u16 reg_scr;
+
+		reg_scr = rtw_read16(adapter, REG_SECCFG);
+		rtw_write16(adapter, REG_SECCFG, reg_scr|SCR_CHK_KEYID|SCR_RxDecEnable|SCR_TxEncEnable);
+	}
+		break;
+	case HW_VAR_SEC_DK_CFG:
+	{
+		struct security_priv *sec = &adapter->securitypriv;
+		u8 reg_scr = rtw_read8(adapter, REG_SECCFG);
+
+		if (val) { /* Enable default key related setting */
+			reg_scr |= SCR_TXBCUSEDK;
+			if (sec->dot11AuthAlgrthm != dot11AuthAlgrthm_8021X)
+				reg_scr |= (SCR_RxUseDK|SCR_TxUseDK);
+		} else /* Disable default key related setting */
+			reg_scr &= ~(SCR_RXBCUSEDK|SCR_TXBCUSEDK|SCR_RxUseDK|SCR_TxUseDK);
+
+		rtw_write8(adapter, REG_SECCFG, reg_scr);
+	}
+		break;
+	case HW_VAR_DM_FLAG:
+		odm->SupportAbility = *((u32 *)val);
+		break;
+	case HW_VAR_DM_FUNC_OP:
+		if (*((u8 *)val) == true) {
+			/* save dm flag */
+			odm->BK_SupportAbility = odm->SupportAbility;
+		} else {
+			/* restore dm flag */
+			odm->SupportAbility = odm->BK_SupportAbility;
+		}
+		break;
+	case HW_VAR_DM_FUNC_SET:
+		if (*((u32 *)val) == DYNAMIC_ALL_FUNC_ENABLE) {
+			struct dm_priv *dm = &hal_data->dmpriv;
+			dm->DMFlag = dm->InitDMFlag;
+			odm->SupportAbility = dm->InitODMFlag;
+		} else {
+			odm->SupportAbility |= *((u32 *)val);
+		}
+		break;
+	case HW_VAR_DM_FUNC_CLR:
+		/*
+		* input is already a mask to clear function
+		* don't invert it again! George, Lucas@20130513
+		*/
+		odm->SupportAbility &= *((u32 *)val);
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE:
+		/* TODO - Is something needed here? */
+		break;
+	case HW_VAR_WIRELESS_MODE:
+		/* TODO - Is something needed here? */
+		break;
+	default:
+		netdev_dbg(adapter->pnetdev,
+			   FUNC_ADPT_FMT " variable(%d) not defined!\n",
+			   FUNC_ADPT_ARG(adapter), variable);
+		break;
+	}
+}
+
+void GetHwReg(struct adapter *adapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	struct dm_odm_t *odm = &(hal_data->odmpriv);
+
+	switch (variable) {
+	case HW_VAR_BASIC_RATE:
+		*((u16 *)val) = hal_data->BasicRateSet;
+		break;
+	case HW_VAR_DM_FLAG:
+		*((u32 *)val) = odm->SupportAbility;
+		break;
+	default:
+		netdev_dbg(adapter->pnetdev,
+			   FUNC_ADPT_FMT " variable(%d) not defined!\n",
+			   FUNC_ADPT_ARG(adapter), variable);
+		break;
+	}
+}
+
+
+
+
+u8 SetHalDefVar(
+	struct adapter *adapter, enum hal_def_variable variable, void *value
+)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	struct dm_odm_t *odm = &(hal_data->odmpriv);
+	u8 bResult = _SUCCESS;
+
+	switch (variable) {
+	case HAL_DEF_DBG_RX_INFO_DUMP:
+
+		if (odm->bLinked) {
+			#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+			rtw_dump_raw_rssi_info(adapter);
+			#endif
+		}
+		break;
+	case HW_DEF_ODM_DBG_FLAG:
+		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_COMP, *((u64 *)value));
+		break;
+	case HW_DEF_ODM_DBG_LEVEL:
+		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_LEVEL, *((u32 *)value));
+		break;
+	case HAL_DEF_DBG_DM_FUNC:
+	{
+		u8 dm_func = *((u8 *)value);
+		struct dm_priv *dm = &hal_data->dmpriv;
+
+		if (dm_func == 0) { /* disable all dynamic func */
+			odm->SupportAbility = DYNAMIC_FUNC_DISABLE;
+		} else if (dm_func == 1) {/* disable DIG */
+			odm->SupportAbility  &= (~DYNAMIC_BB_DIG);
+		} else if (dm_func == 2) {/* disable High power */
+			odm->SupportAbility  &= (~DYNAMIC_BB_DYNAMIC_TXPWR);
+		} else if (dm_func == 3) {/* disable tx power tracking */
+			odm->SupportAbility  &= (~DYNAMIC_RF_CALIBRATION);
+		} else if (dm_func == 4) {/* disable BT coexistence */
+			dm->DMFlag &= (~DYNAMIC_FUNC_BT);
+		} else if (dm_func == 5) {/* disable antenna diversity */
+			odm->SupportAbility  &= (~DYNAMIC_BB_ANT_DIV);
+		} else if (dm_func == 6) {/* turn on all dynamic func */
+			if (!(odm->SupportAbility  & DYNAMIC_BB_DIG)) {
+				struct dig_t	*pDigTable = &odm->DM_DigTable;
+				pDigTable->CurIGValue = rtw_read8(adapter, 0xc50);
+			}
+			dm->DMFlag |= DYNAMIC_FUNC_BT;
+			odm->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
+		}
+	}
+		break;
+	case HAL_DEF_DBG_DUMP_RXPKT:
+		hal_data->bDumpRxPkt = *((u8 *)value);
+		break;
+	case HAL_DEF_DBG_DUMP_TXPKT:
+		hal_data->bDumpTxPkt = *((u8 *)value);
+		break;
+	case HAL_DEF_ANT_DETECT:
+		hal_data->AntDetection = *((u8 *)value);
+		break;
+	default:
+		netdev_dbg(adapter->pnetdev,
+			   "%s: [WARNING] HAL_DEF_VARIABLE(%d) not defined!\n",
+			   __func__, variable);
+		bResult = _FAIL;
+		break;
+	}
+
+	return bResult;
+}
+
+u8 GetHalDefVar(
+	struct adapter *adapter, enum hal_def_variable variable, void *value
+)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	u8 bResult = _SUCCESS;
+
+	switch (variable) {
+	case HAL_DEF_UNDERCORATEDSMOOTHEDPWDB:
+		{
+			struct mlme_priv *pmlmepriv;
+			struct sta_priv *pstapriv;
+			struct sta_info *psta;
+
+			pmlmepriv = &adapter->mlmepriv;
+			pstapriv = &adapter->stapriv;
+			psta = rtw_get_stainfo(pstapriv, pmlmepriv->cur_network.network.mac_address);
+			if (psta)
+				*((int *)value) = psta->rssi_stat.UndecoratedSmoothedPWDB;
+		}
+		break;
+	case HAL_DEF_DBG_DM_FUNC:
+		*((u32 *)value) = hal_data->odmpriv.SupportAbility;
+		break;
+	case HAL_DEF_DBG_DUMP_RXPKT:
+		*((u8 *)value) = hal_data->bDumpRxPkt;
+		break;
+	case HAL_DEF_DBG_DUMP_TXPKT:
+		*((u8 *)value) = hal_data->bDumpTxPkt;
+		break;
+	case HAL_DEF_ANT_DETECT:
+		*((u8 *)value) = hal_data->AntDetection;
+		break;
+	case HAL_DEF_MACID_SLEEP:
+		*(u8 *)value = false;
+		break;
+	case HAL_DEF_TX_PAGE_SIZE:
+		*((u32 *)value) = PAGE_SIZE_128;
+		break;
+	default:
+		netdev_dbg(adapter->pnetdev,
+			   "%s: [WARNING] HAL_DEF_VARIABLE(%d) not defined!\n",
+			   __func__, variable);
+		bResult = _FAIL;
+		break;
+	}
+
+	return bResult;
+}
+
+void GetHalODMVar(
+	struct adapter *Adapter,
+	enum hal_odm_variable eVariable,
+	void *pValue1,
+	void *pValue2
+)
+{
+	switch (eVariable) {
+	default:
+		break;
+	}
+}
+
+void SetHalODMVar(
+	struct adapter *Adapter,
+	enum hal_odm_variable eVariable,
+	void *pValue1,
+	bool bSet
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	struct dm_odm_t *podmpriv = &pHalData->odmpriv;
+	/* _irqL irqL; */
+	switch (eVariable) {
+	case HAL_ODM_STA_INFO:
+		{
+			struct sta_info *psta = pValue1;
+			if (bSet) {
+				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, psta);
+			} else {
+				/* spin_lock_bh(&pHalData->odm_stainfo_lock); */
+				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, NULL);
+
+				/* spin_unlock_bh(&pHalData->odm_stainfo_lock); */
+		    }
+		}
+		break;
+	case HAL_ODM_P2P_STATE:
+			ODM_CmnInfoUpdate(podmpriv, ODM_CMNINFO_WIFI_DIRECT, bSet);
+		break;
+	case HAL_ODM_WIFI_DISPLAY_STATE:
+			ODM_CmnInfoUpdate(podmpriv, ODM_CMNINFO_WIFI_DISPLAY, bSet);
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+bool eqNByte(u8 *str1, u8 *str2, u32 num)
+{
+	if (num == 0)
+		return false;
+	while (num > 0) {
+		num--;
+		if (str1[num] != str2[num])
+			return false;
+	}
+	return true;
+}
+
+/*  */
+/* 	Description: */
+/* 		Translate a character to hex digit. */
+/*  */
+u32 MapCharToHexDigit(char chTmp)
+{
+	if (chTmp >= '0' && chTmp <= '9')
+		return chTmp - '0';
+	else if (chTmp >= 'a' && chTmp <= 'f')
+		return 10 + (chTmp - 'a');
+	else if (chTmp >= 'A' && chTmp <= 'F')
+		return 10 + (chTmp - 'A');
+	else
+		return 0;
+}
+
+bool GetU1ByteIntegerFromStringInDecimal(char *Str, u8 *pInt)
+{
+	u16 i = 0;
+	*pInt = 0;
+
+	while (Str[i] != '\0') {
+		if (Str[i] >= '0' && Str[i] <= '9') {
+			*pInt *= 10;
+			*pInt += (Str[i] - '0');
+		} else
+			return false;
+
+		++i;
+	}
+
+	return true;
+}
+
+/*  <20121004, Kordan> For example,
+ *  ParseQualifiedString(inString, 0, outString, '[', ']') gets "Kordan" from
+ *  a string "Hello [Kordan]".
+ *  If RightQualifier does not exist, it will hang in the while loop
+ */
+bool ParseQualifiedString(
+	char *In, u32 *Start, char *Out, char LeftQualifier, char RightQualifier
+)
+{
+	u32 i = 0, j = 0;
+	char c = In[(*Start)++];
+
+	if (c != LeftQualifier)
+		return false;
+
+	i = (*Start);
+	while ((c = In[(*Start)++]) != RightQualifier)
+		; /*  find ']' */
+	j = (*Start) - 2;
+	strncpy((char *)Out, (const char *)(In+i), j-i+1);
+
+	return true;
+}
+
+bool isAllSpaceOrTab(u8 *data, u8 size)
+{
+	u8 cnt = 0, NumOfSpaceAndTab = 0;
+
+	while (size > cnt) {
+		if (data[cnt] == ' ' || data[cnt] == '\t' || data[cnt] == '\0')
+			++NumOfSpaceAndTab;
+
+		++cnt;
+	}
+
+	return size == NumOfSpaceAndTab;
+}
+
+
+void rtw_hal_check_rxfifo_full(struct adapter *adapter)
+{
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	int save_cnt = false;
+
+	/* switch counter to RX fifo */
+	/* printk("8723b or 8192e , MAC_667 set 0xf0\n"); */
+	rtw_write8(adapter, REG_RXERR_RPT+3, rtw_read8(adapter, REG_RXERR_RPT+3)|0xf0);
+	save_cnt = true;
+	/* todo: other chips */
+
+	if (save_cnt) {
+		/* rtw_write8(adapter, REG_RXERR_RPT+3, rtw_read8(adapter, REG_RXERR_RPT+3)|0xa0); */
+		pdbgpriv->dbg_rx_fifo_last_overflow = pdbgpriv->dbg_rx_fifo_curr_overflow;
+		pdbgpriv->dbg_rx_fifo_curr_overflow = rtw_read16(adapter, REG_RXERR_RPT);
+		pdbgpriv->dbg_rx_fifo_diff_overflow = pdbgpriv->dbg_rx_fifo_curr_overflow-pdbgpriv->dbg_rx_fifo_last_overflow;
+	}
+}
+
+void linked_info_dump(struct adapter *padapter, u8 benable)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (padapter->bLinkInfoDump == benable)
+		return;
+
+	if (benable) {
+		pwrctrlpriv->org_power_mgnt = pwrctrlpriv->power_mgnt;/* keep org value */
+		rtw_pm_set_lps(padapter, PS_MODE_ACTIVE);
+
+		pwrctrlpriv->ips_org_mode = pwrctrlpriv->ips_mode;/* keep org value */
+		rtw_pm_set_ips(padapter, IPS_NONE);
+	} else {
+		rtw_pm_set_ips(padapter, pwrctrlpriv->ips_org_mode);
+
+		rtw_pm_set_lps(padapter, pwrctrlpriv->ips_org_mode);
+	}
+	padapter->bLinkInfoDump = benable;
+}
+
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+void rtw_get_raw_rssi_info(void *sel, struct adapter *padapter)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+
+	netdev_dbg(padapter->pnetdev,
+		   "RxRate = %s, PWDBALL = %d(%%), rx_pwr_all = %d(dBm)\n",
+		   HDATA_RATE(psample_pkt_rssi->data_rate),
+		   psample_pkt_rssi->pwdball, psample_pkt_rssi->pwr_all);
+
+	isCCKrate = psample_pkt_rssi->data_rate <= DESC_RATE11M;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_signal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		netdev_dbg(padapter->pnetdev,
+			   "RF_PATH_%d =>signal_strength:%d(%%), signal_quality:%d(%%)\n",
+			   rf_path,
+			   psample_pkt_rssi->mimo_signal_strength[rf_path],
+			   psample_pkt_rssi->mimo_signal_quality[rf_path]);
+
+		if (!isCCKrate) {
+			netdev_dbg(padapter->pnetdev,
+				   "\trx_ofdm_pwr:%d(dBm), rx_ofdm_snr:%d(dB)\n",
+				   psample_pkt_rssi->ofdm_pwr[rf_path],
+				   psample_pkt_rssi->ofdm_snr[rf_path]);
+		}
+	}
+}
+
+void rtw_dump_raw_rssi_info(struct adapter *padapter)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData =  GET_HAL_DATA(padapter);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+
+	isCCKrate = psample_pkt_rssi->data_rate <= DESC_RATE11M;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_signal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		if (!isCCKrate) {
+			printk(", rx_ofdm_pwr:%d(dBm), rx_ofdm_snr:%d(dB)\n",
+			psample_pkt_rssi->ofdm_pwr[rf_path], psample_pkt_rssi->ofdm_snr[rf_path]);
+		} else {
+			printk("\n");
+		}
+	}
+}
+
+void rtw_store_phy_info(struct adapter *padapter, union recv_frame *prframe)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData =  GET_HAL_DATA(padapter);
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+
+	struct odm_phy_info *pPhyInfo  = (PODM_PHY_INFO_T)(&pattrib->phy_info);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+
+	psample_pkt_rssi->data_rate = pattrib->data_rate;
+	isCCKrate = pattrib->data_rate <= DESC_RATE11M;
+
+	psample_pkt_rssi->pwdball = pPhyInfo->rx_pwd_ba11;
+	psample_pkt_rssi->pwr_all = pPhyInfo->recv_signal_power;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		psample_pkt_rssi->mimo_signal_strength[rf_path] = pPhyInfo->rx_mimo_signal_strength[rf_path];
+		psample_pkt_rssi->mimo_signal_quality[rf_path] = pPhyInfo->rx_mimo_signal_quality[rf_path];
+		if (!isCCKrate) {
+			psample_pkt_rssi->ofdm_pwr[rf_path] = pPhyInfo->RxPwr[rf_path];
+			psample_pkt_rssi->ofdm_snr[rf_path] = pPhyInfo->RxSNR[rf_path];
+		}
+	}
+}
+#endif
+
+static u32 Array_kfreemap[] = {
+	0xf8, 0xe,
+	0xf6, 0xc,
+	0xf4, 0xa,
+	0xf2, 0x8,
+	0xf0, 0x6,
+	0xf3, 0x4,
+	0xf5, 0x2,
+	0xf7, 0x0,
+	0xf9, 0x0,
+	0xfc, 0x0,
+};
+
+void rtw_bb_rf_gain_offset(struct adapter *padapter)
+{
+	u8 value = padapter->eeprompriv.EEPROMRFGainOffset;
+	u32 res, i = 0;
+	u32 *Array = Array_kfreemap;
+	u32 v1 = 0, v2 = 0, target = 0;
+
+	if (value & BIT4) {
+		if (padapter->eeprompriv.EEPROMRFGainVal != 0xff) {
+			res = rtw_hal_read_rfreg(padapter, RF_PATH_A, 0x7f, 0xffffffff);
+			res &= 0xfff87fff;
+			/* res &= 0xfff87fff; */
+			for (i = 0; i < ARRAY_SIZE(Array_kfreemap); i += 2) {
+				v1 = Array[i];
+				v2 = Array[i+1];
+				if (v1 == padapter->eeprompriv.EEPROMRFGainVal) {
+					target = v2;
+					break;
+				}
+			}
+			PHY_SetRFReg(padapter, RF_PATH_A, REG_RF_BB_GAIN_OFFSET, BIT18|BIT17|BIT16|BIT15, target);
+
+			/* res |= (padapter->eeprompriv.EEPROMRFGainVal & 0x0f)<< 15; */
+			/* rtw_hal_write_rfreg(padapter, RF_PATH_A, REG_RF_BB_GAIN_OFFSET, RF_GAIN_OFFSET_MASK, res); */
+			res = rtw_hal_read_rfreg(padapter, RF_PATH_A, 0x7f, 0xffffffff);
+		}
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c b/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
new file mode 100644
index 000000000..3e814a15e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
@@ -0,0 +1,984 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/kernel.h>
+
+u8 PHY_GetTxPowerByRateBase(struct adapter *Adapter, u8 RfPath,
+			    enum rate_section RateSection)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8	value = 0;
+
+	if (RfPath >= RF_PATH_MAX)
+		return 0;
+
+	switch (RateSection) {
+	case CCK:
+		value = pHalData->TxPwrByRateBase2_4G[RfPath][0];
+		break;
+	case OFDM:
+		value = pHalData->TxPwrByRateBase2_4G[RfPath][1];
+		break;
+	case HT_MCS0_MCS7:
+		value = pHalData->TxPwrByRateBase2_4G[RfPath][2];
+		break;
+	default:
+		break;
+	}
+
+	return value;
+}
+
+static void
+phy_SetTxPowerByRateBase(struct adapter *Adapter, u8 RfPath,
+			 enum rate_section RateSection, u8 Value)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	if (RfPath >= RF_PATH_MAX)
+		return;
+
+	switch (RateSection) {
+	case CCK:
+		pHalData->TxPwrByRateBase2_4G[RfPath][0] = Value;
+		break;
+	case OFDM:
+		pHalData->TxPwrByRateBase2_4G[RfPath][1] = Value;
+		break;
+	case HT_MCS0_MCS7:
+		pHalData->TxPwrByRateBase2_4G[RfPath][2] = Value;
+		break;
+	default:
+		break;
+	}
+}
+
+static void
+phy_StoreTxPowerByRateBase(
+struct adapter *padapter
+	)
+{
+	u8 path, base;
+
+	for (path = RF_PATH_A; path <= RF_PATH_B; ++path) {
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_11M);
+		phy_SetTxPowerByRateBase(padapter, path, CCK, base);
+
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_54M);
+		phy_SetTxPowerByRateBase(padapter, path, OFDM, base);
+
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_MCS7);
+		phy_SetTxPowerByRateBase(padapter, path, HT_MCS0_MCS7, base);
+	}
+}
+
+u8 PHY_GetRateSectionIndexOfTxPowerByRate(
+	struct adapter *padapter, u32 RegAddr, u32 BitMask
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	u8	index = 0;
+
+	if (pDM_Odm->PhyRegPgVersion == 0) {
+		switch (RegAddr) {
+		case rTxAGC_A_Rate18_06:
+			index = 0;
+			break;
+		case rTxAGC_A_Rate54_24:
+			index = 1;
+			break;
+		case rTxAGC_A_CCK1_Mcs32:
+			index = 6;
+			break;
+		case rTxAGC_B_CCK11_A_CCK2_11:
+			if (BitMask == bMaskH3Bytes)
+				index = 7;
+			else if (BitMask == 0x000000ff)
+				index = 15;
+			break;
+
+		case rTxAGC_A_Mcs03_Mcs00:
+			index = 2;
+			break;
+		case rTxAGC_A_Mcs07_Mcs04:
+			index = 3;
+			break;
+		case rTxAGC_B_Rate18_06:
+			index = 8;
+			break;
+		case rTxAGC_B_Rate54_24:
+			index = 9;
+			break;
+		case rTxAGC_B_CCK1_55_Mcs32:
+			index = 14;
+			break;
+		case rTxAGC_B_Mcs03_Mcs00:
+			index = 10;
+			break;
+		case rTxAGC_B_Mcs07_Mcs04:
+			index = 11;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return index;
+}
+
+void
+PHY_GetRateValuesOfTxPowerByRate(
+	struct adapter *padapter,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Value,
+	u8 *RateIndex,
+	s8 *PwrByRateVal,
+	u8 *RateNum
+)
+{
+	u8 i = 0;
+
+	switch (RegAddr) {
+	case rTxAGC_A_Rate18_06:
+	case rTxAGC_B_Rate18_06:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_6M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_9M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_12M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_18M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Rate54_24:
+	case rTxAGC_B_Rate54_24:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_24M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_36M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_48M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_54M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_CCK1_Mcs32:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		PwrByRateVal[0] = (s8) ((((Value >> (8 + 4)) & 0xF)) * 10 +
+										((Value >> 8) & 0xF));
+		*RateNum = 1;
+		break;
+
+	case rTxAGC_B_CCK11_A_CCK2_11:
+		if (BitMask == 0xffffff00) {
+			RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+			RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+			RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+			for (i = 1; i < 4; ++i) {
+				PwrByRateVal[i - 1] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+												((Value >> (i * 8)) & 0xF));
+			}
+			*RateNum = 3;
+		} else if (BitMask == 0x000000ff) {
+			RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+			PwrByRateVal[0] = (s8) ((((Value >> 4) & 0xF)) * 10 + (Value & 0xF));
+			*RateNum = 1;
+		}
+		break;
+
+	case rTxAGC_A_Mcs03_Mcs00:
+	case rTxAGC_B_Mcs03_Mcs00:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Mcs07_Mcs04:
+	case rTxAGC_B_Mcs07_Mcs04:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_B_CCK1_55_Mcs32:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+		for (i = 1; i < 4; ++i) {
+			PwrByRateVal[i - 1] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 3;
+		break;
+
+	case 0xC20:
+	case 0xE20:
+	case 0x1820:
+	case 0x1a20:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC24:
+	case 0xE24:
+	case 0x1824:
+	case 0x1a24:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_6M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_9M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_12M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_18M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC28:
+	case 0xE28:
+	case 0x1828:
+	case 0x1a28:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_24M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_36M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_48M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_54M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC2C:
+	case 0xE2C:
+	case 0x182C:
+	case 0x1a2C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC30:
+	case 0xE30:
+	case 0x1830:
+	case 0x1a30:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void PHY_StoreTxPowerByRateNew(struct adapter *padapter,	u32 RfPath,
+				      u32 RegAddr, u32 BitMask, u32 Data)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 i = 0, rateIndex[4] = {0}, rateNum = 0;
+	s8	PwrByRateVal[4] = {0};
+
+	PHY_GetRateValuesOfTxPowerByRate(padapter, RegAddr, BitMask, Data, rateIndex, PwrByRateVal, &rateNum);
+
+	if (RfPath >= RF_PATH_MAX)
+		return;
+
+	for (i = 0; i < rateNum; ++i) {
+		pHalData->TxPwrByRateOffset[RfPath][rateIndex[i]] = PwrByRateVal[i];
+	}
+}
+
+static void PHY_StoreTxPowerByRateOld(
+	struct adapter *padapter, u32	RegAddr, u32 BitMask, u32 Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8	index = PHY_GetRateSectionIndexOfTxPowerByRate(padapter, RegAddr, BitMask);
+
+	pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][index] = Data;
+}
+
+void PHY_InitTxPowerByRate(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 rfPath, rate;
+
+	for (rfPath = RF_PATH_A; rfPath < MAX_RF_PATH_NUM; ++rfPath)
+		for (rate = 0; rate < TX_PWR_BY_RATE_NUM_RATE; ++rate)
+			pHalData->TxPwrByRateOffset[rfPath][rate] = 0;
+}
+
+void PHY_StoreTxPowerByRate(
+	struct adapter *padapter,
+	u32	RfPath,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	if (pDM_Odm->PhyRegPgVersion > 0)
+		PHY_StoreTxPowerByRateNew(padapter, RfPath, RegAddr, BitMask, Data);
+	else if (pDM_Odm->PhyRegPgVersion == 0) {
+		PHY_StoreTxPowerByRateOld(padapter, RegAddr, BitMask, Data);
+	}
+}
+
+static void
+phy_ConvertTxPowerByRateInDbmToRelativeValues(
+struct adapter *padapter
+	)
+{
+	u8	base = 0, i = 0, value = 0, path = 0;
+	u8	cckRates[4] = {
+		MGN_1M, MGN_2M, MGN_5_5M, MGN_11M
+	};
+	u8	ofdmRates[8] = {
+		MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M
+	};
+	u8 mcs0_7Rates[8] = {
+		MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7
+	};
+	for (path = RF_PATH_A; path < RF_PATH_MAX; ++path) {
+		/*  CCK */
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_11M);
+		for (i = 0; i < ARRAY_SIZE(cckRates); ++i) {
+			value = PHY_GetTxPowerByRate(padapter, path, cckRates[i]);
+			PHY_SetTxPowerByRate(padapter, path, cckRates[i], value - base);
+		}
+
+		/*  OFDM */
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_54M);
+		for (i = 0; i < sizeof(ofdmRates); ++i) {
+			value = PHY_GetTxPowerByRate(padapter, path, ofdmRates[i]);
+			PHY_SetTxPowerByRate(padapter, path, ofdmRates[i], value - base);
+		}
+
+		/*  HT MCS0~7 */
+		base = PHY_GetTxPowerByRate(padapter, path, MGN_MCS7);
+		for (i = 0; i < sizeof(mcs0_7Rates); ++i) {
+			value = PHY_GetTxPowerByRate(padapter, path, mcs0_7Rates[i]);
+			PHY_SetTxPowerByRate(padapter, path, mcs0_7Rates[i], value - base);
+		}
+	}
+}
+
+/*
+  * This function must be called if the value in the PHY_REG_PG.txt(or header)
+  * is exact dBm values
+  */
+void PHY_TxPowerByRateConfiguration(struct adapter *padapter)
+{
+	phy_StoreTxPowerByRateBase(padapter);
+	phy_ConvertTxPowerByRateInDbmToRelativeValues(padapter);
+}
+
+void PHY_SetTxPowerIndexByRateSection(
+	struct adapter *padapter, u8 RFPath, u8 Channel, u8 RateSection
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	if (RateSection == CCK) {
+		u8 cckRates[]   = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath,
+					     pHalData->CurrentChannelBW,
+					     Channel, cckRates,
+					     ARRAY_SIZE(cckRates));
+
+	} else if (RateSection == OFDM) {
+		u8 ofdmRates[]  = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath,
+					       pHalData->CurrentChannelBW,
+					       Channel, ofdmRates,
+					       ARRAY_SIZE(ofdmRates));
+
+	} else if (RateSection == HT_MCS0_MCS7) {
+		u8 htRates1T[]  = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath,
+					       pHalData->CurrentChannelBW,
+					       Channel, htRates1T,
+					       ARRAY_SIZE(htRates1T));
+
+	}
+}
+
+u8 PHY_GetTxPowerIndexBase(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum channel_width	BandWidth,
+	u8 Channel
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 txPower = 0;
+	u8 chnlIdx = (Channel-1);
+
+	if (HAL_IsLegalChannel(padapter, Channel) == false)
+		chnlIdx = 0;
+
+	if (IS_CCK_RATE(Rate))
+		txPower = pHalData->Index24G_CCK_Base[RFPath][chnlIdx];
+	else if (MGN_6M <= Rate)
+		txPower = pHalData->Index24G_BW40_Base[RFPath][chnlIdx];
+
+	/*  OFDM-1T */
+	if ((MGN_6M <= Rate && Rate <= MGN_54M) && !IS_CCK_RATE(Rate))
+		txPower += pHalData->OFDM_24G_Diff[RFPath][TX_1S];
+
+	if (BandWidth == CHANNEL_WIDTH_20) { /*  BW20-1S, BW20-2S */
+		if (MGN_MCS0 <= Rate && Rate <= MGN_MCS7)
+			txPower += pHalData->BW20_24G_Diff[RFPath][TX_1S];
+	} else if (BandWidth == CHANNEL_WIDTH_40) { /*  BW40-1S, BW40-2S */
+		if (MGN_MCS0 <= Rate && Rate <= MGN_MCS7)
+			txPower += pHalData->BW40_24G_Diff[RFPath][TX_1S];
+	}
+
+	return txPower;
+}
+
+s8 PHY_GetTxPowerTrackingOffset(struct adapter *padapter, u8 RFPath, u8 Rate)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	s8 offset = 0;
+
+	if (pDM_Odm->RFCalibrateInfo.TxPowerTrackControl  == false)
+		return offset;
+
+	if ((Rate == MGN_1M) || (Rate == MGN_2M) || (Rate == MGN_5_5M) || (Rate == MGN_11M))
+		offset = pDM_Odm->Remnant_CCKSwingIdx;
+	else
+		offset = pDM_Odm->Remnant_OFDMSwingIdx[RFPath];
+
+	return offset;
+}
+
+u8 PHY_GetRateIndexOfTxPowerByRate(u8 Rate)
+{
+	u8 index = 0;
+	switch (Rate) {
+	case MGN_1M:
+		index = 0;
+		break;
+	case MGN_2M:
+		index = 1;
+		break;
+	case MGN_5_5M:
+		index = 2;
+		break;
+	case MGN_11M:
+		index = 3;
+		break;
+	case MGN_6M:
+		index = 4;
+		break;
+	case MGN_9M:
+		index = 5;
+		break;
+	case MGN_12M:
+		index = 6;
+		break;
+	case MGN_18M:
+		index = 7;
+		break;
+	case MGN_24M:
+		index = 8;
+		break;
+	case MGN_36M:
+		index = 9;
+		break;
+	case MGN_48M:
+		index = 10;
+		break;
+	case MGN_54M:
+		index = 11;
+		break;
+	case MGN_MCS0:
+		index = 12;
+		break;
+	case MGN_MCS1:
+		index = 13;
+		break;
+	case MGN_MCS2:
+		index = 14;
+		break;
+	case MGN_MCS3:
+		index = 15;
+		break;
+	case MGN_MCS4:
+		index = 16;
+		break;
+	case MGN_MCS5:
+		index = 17;
+		break;
+	case MGN_MCS6:
+		index = 18;
+		break;
+	case MGN_MCS7:
+		index = 19;
+		break;
+	default:
+		break;
+	}
+	return index;
+}
+
+s8 PHY_GetTxPowerByRate(struct adapter *padapter, u8 RFPath, u8 Rate)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	s8 value = 0;
+	u8 rateIndex = PHY_GetRateIndexOfTxPowerByRate(Rate);
+
+	if ((padapter->registrypriv.RegEnableTxPowerByRate == 2 && pHalData->EEPROMRegulatory == 2) ||
+		   padapter->registrypriv.RegEnableTxPowerByRate == 0)
+		return 0;
+
+	if (RFPath >= RF_PATH_MAX)
+		return value;
+
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE)
+		return value;
+
+	return pHalData->TxPwrByRateOffset[RFPath][rateIndex];
+
+}
+
+void PHY_SetTxPowerByRate(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	s8 Value
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 rateIndex = PHY_GetRateIndexOfTxPowerByRate(Rate);
+
+	if (RFPath >= RF_PATH_MAX)
+		return;
+
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE)
+		return;
+
+	pHalData->TxPwrByRateOffset[RFPath][rateIndex] = Value;
+}
+
+void PHY_SetTxPowerLevelByPath(struct adapter *Adapter, u8 channel, u8 path)
+{
+	PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, CCK);
+
+	PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, OFDM);
+	PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, HT_MCS0_MCS7);
+}
+
+void PHY_SetTxPowerIndexByRateArray(
+	struct adapter *padapter,
+	u8 RFPath,
+	enum channel_width BandWidth,
+	u8 Channel,
+	u8 *Rates,
+	u8 RateArraySize
+)
+{
+	u32 powerIndex = 0;
+	int	i = 0;
+
+	for (i = 0; i < RateArraySize; ++i) {
+		powerIndex = PHY_GetTxPowerIndex(padapter, RFPath, Rates[i], BandWidth, Channel);
+		PHY_SetTxPowerIndex(padapter, powerIndex, RFPath, Rates[i]);
+	}
+}
+
+static s8 phy_GetWorldWideLimit(s8 *LimitTable)
+{
+	s8	min = LimitTable[0];
+	u8 i = 0;
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		if (LimitTable[i] < min)
+			min = LimitTable[i];
+	}
+
+	return min;
+}
+
+static s8 phy_GetChannelIndexOfTxPowerLimit(u8 Channel)
+{
+	return Channel - 1;
+}
+
+static s16 get_bandwidth_idx(const enum channel_width bandwidth)
+{
+	switch (bandwidth) {
+	case CHANNEL_WIDTH_20:
+		return 0;
+	case CHANNEL_WIDTH_40:
+		return 1;
+	default:
+		return -1;
+	}
+}
+
+static s16 get_rate_sctn_idx(const u8 rate)
+{
+	switch (rate) {
+	case MGN_1M: case MGN_2M: case MGN_5_5M: case MGN_11M:
+		return 0;
+	case MGN_6M: case MGN_9M: case MGN_12M: case MGN_18M:
+	case MGN_24M: case MGN_36M: case MGN_48M: case MGN_54M:
+		return 1;
+	case MGN_MCS0: case MGN_MCS1: case MGN_MCS2: case MGN_MCS3:
+	case MGN_MCS4: case MGN_MCS5: case MGN_MCS6: case MGN_MCS7:
+		return 2;
+	default:
+		return -1;
+	}
+}
+
+s8 phy_get_tx_pwr_lmt(struct adapter *adapter, u32 reg_pwr_tbl_sel,
+		      enum channel_width bandwidth,
+		      u8 rf_path, u8 data_rate, u8 channel)
+{
+	s16 idx_regulation = -1;
+	s16 idx_bandwidth  = -1;
+	s16 idx_rate_sctn  = -1;
+	s16 idx_channel    = -1;
+	s8 pwr_lmt = MAX_POWER_INDEX;
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	s8 limits[10] = {0}; u8 i = 0;
+
+	if (((adapter->registrypriv.RegEnableTxPowerLimit == 2) &&
+	     (hal_data->EEPROMRegulatory != 1)) ||
+	    (adapter->registrypriv.RegEnableTxPowerLimit == 0))
+		return MAX_POWER_INDEX;
+
+	switch (adapter->registrypriv.RegPwrTblSel) {
+	case 1:
+		idx_regulation = TXPWR_LMT_ETSI;
+		break;
+	case 2:
+		idx_regulation = TXPWR_LMT_MKK;
+		break;
+	case 3:
+		idx_regulation = TXPWR_LMT_FCC;
+		break;
+	case 4:
+		idx_regulation = TXPWR_LMT_WW;
+		break;
+	default:
+		idx_regulation = hal_data->Regulation2_4G;
+		break;
+	}
+
+	idx_bandwidth = get_bandwidth_idx(bandwidth);
+	idx_rate_sctn = get_rate_sctn_idx(data_rate);
+
+	/*  workaround for wrong index combination to obtain tx power limit, */
+	/*  OFDM only exists in BW 20M */
+	/*  CCK table will only be given in BW 20M */
+	/*  HT on 80M will reference to HT on 40M */
+	if (idx_rate_sctn == 0 || idx_rate_sctn == 1)
+		idx_bandwidth = 0;
+
+	channel = phy_GetChannelIndexOfTxPowerLimit(channel);
+
+	if (idx_regulation == -1 || idx_bandwidth == -1 ||
+	    idx_rate_sctn == -1 || idx_channel == -1)
+		return MAX_POWER_INDEX;
+
+
+	for (i = 0; i < MAX_REGULATION_NUM; i++)
+		limits[i] = hal_data->TxPwrLimit_2_4G[i]
+						     [idx_bandwidth]
+						     [idx_rate_sctn]
+						     [idx_channel]
+						     [rf_path];
+
+	pwr_lmt = (idx_regulation == TXPWR_LMT_WW) ?
+		phy_GetWorldWideLimit(limits) :
+		hal_data->TxPwrLimit_2_4G[idx_regulation]
+					 [idx_bandwidth]
+					 [idx_rate_sctn]
+					 [idx_channel]
+					 [rf_path];
+
+	return pwr_lmt;
+}
+
+void PHY_ConvertTxPowerLimitToPowerIndex(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 BW40PwrBasedBm2_4G = 0x2E;
+	u8 regulation, bw, channel, rateSection;
+	s8 tempValue = 0, tempPwrLmt = 0;
+	u8 rfPath = 0;
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_2_4G_BANDWIDTH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_2G; ++channel) {
+				for (rateSection = 0; rateSection < MAX_RATE_SECTION_NUM; ++rateSection) {
+					tempPwrLmt = pHalData->TxPwrLimit_2_4G[regulation][bw][rateSection][channel][RF_PATH_A];
+
+					for (rfPath = RF_PATH_A; rfPath < MAX_RF_PATH_NUM; ++rfPath) {
+						if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+							if (rateSection == 2) /*  HT 1T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, rfPath, HT_MCS0_MCS7);
+							else if (rateSection == 1) /*  OFDM */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, rfPath, OFDM);
+							else if (rateSection == 0) /*  CCK */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, rfPath, CCK);
+						} else
+							BW40PwrBasedBm2_4G = Adapter->registrypriv.RegPowerBase * 2;
+
+						if (tempPwrLmt != MAX_POWER_INDEX) {
+							tempValue = tempPwrLmt - BW40PwrBasedBm2_4G;
+							pHalData->TxPwrLimit_2_4G[regulation][bw][rateSection][channel][rfPath] = tempValue;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+void PHY_InitTxPowerLimit(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 i, j, k, l, m;
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_2_4G_BANDWIDTH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_2G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						pHalData->TxPwrLimit_2_4G[i][j][k][m][l] = MAX_POWER_INDEX;
+	}
+}
+
+void PHY_SetTxPowerLimit(
+	struct adapter *Adapter,
+	u8 *Regulation,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 regulation = 0, bandwidth = 0, rateSection = 0, channel;
+	s8 powerLimit = 0, prevPowerLimit, channelIndex;
+
+	GetU1ByteIntegerFromStringInDecimal((s8 *)Channel, &channel);
+	GetU1ByteIntegerFromStringInDecimal((s8 *)PowerLimit, &powerLimit);
+
+	powerLimit = powerLimit > MAX_POWER_INDEX ? MAX_POWER_INDEX : powerLimit;
+
+	if (eqNByte(Regulation, (u8 *)("FCC"), 3))
+		regulation = 0;
+	else if (eqNByte(Regulation, (u8 *)("MKK"), 3))
+		regulation = 1;
+	else if (eqNByte(Regulation, (u8 *)("ETSI"), 4))
+		regulation = 2;
+	else if (eqNByte(Regulation, (u8 *)("WW13"), 4))
+		regulation = 3;
+
+	if (eqNByte(RateSection, (u8 *)("CCK"), 3) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 0;
+	else if (eqNByte(RateSection, (u8 *)("OFDM"), 4) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 1;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 2;
+	else
+		return;
+
+	if (eqNByte(Bandwidth, (u8 *)("20M"), 3))
+		bandwidth = 0;
+	else if (eqNByte(Bandwidth, (u8 *)("40M"), 3))
+		bandwidth = 1;
+
+	channelIndex = phy_GetChannelIndexOfTxPowerLimit(channel);
+
+	if (channelIndex == -1)
+		return;
+
+	prevPowerLimit = pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][RF_PATH_A];
+
+	if (powerLimit < prevPowerLimit)
+		pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][RF_PATH_A] = powerLimit;
+}
+
+void Hal_ChannelPlanToRegulation(struct adapter *Adapter, u16 ChannelPlan)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	pHalData->Regulation2_4G = TXPWR_LMT_WW;
+
+	switch (ChannelPlan) {
+	case RT_CHANNEL_DOMAIN_WORLD_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI2_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK1:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_KCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC5:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC6:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC7:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK2:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK3:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_GLOBAL_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI1_ETSI4:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI5:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC8:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI6:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI7:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI8:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI9:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI10:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI11:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI12:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC9:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI13:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC10:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_REALTEK_DEFINE: /* Realtek Reserve */
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_intf.c b/drivers/staging/rtl8723bs/hal/hal_intf.c
new file mode 100644
index 000000000..6bb0ff8d7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_intf.c
@@ -0,0 +1,439 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+void rtw_hal_chip_configure(struct adapter *padapter)
+{
+	if (padapter->HalFunc.intf_chip_configure)
+		padapter->HalFunc.intf_chip_configure(padapter);
+}
+
+void rtw_hal_read_chip_info(struct adapter *padapter)
+{
+	if (padapter->HalFunc.read_adapter_info)
+		padapter->HalFunc.read_adapter_info(padapter);
+}
+
+void rtw_hal_read_chip_version(struct adapter *padapter)
+{
+	if (padapter->HalFunc.read_chip_version)
+		padapter->HalFunc.read_chip_version(padapter);
+}
+
+void rtw_hal_def_value_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.init_default_value)
+			padapter->HalFunc.init_default_value(padapter);
+}
+
+void rtw_hal_free_data(struct adapter *padapter)
+{
+	/* free HAL Data */
+	rtw_hal_data_deinit(padapter);
+
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.free_hal_data)
+			padapter->HalFunc.free_hal_data(padapter);
+}
+
+void rtw_hal_dm_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.dm_init)
+			padapter->HalFunc.dm_init(padapter);
+}
+
+void rtw_hal_dm_deinit(struct adapter *padapter)
+{
+	/*  cancel dm  timer */
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.dm_deinit)
+			padapter->HalFunc.dm_deinit(padapter);
+}
+
+static void rtw_hal_init_opmode(struct adapter *padapter)
+{
+	enum ndis_802_11_network_infrastructure networkType = Ndis802_11InfrastructureMax;
+	struct  mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	signed int fw_state;
+
+	fw_state = get_fwstate(pmlmepriv);
+
+	if (fw_state & WIFI_ADHOC_STATE)
+		networkType = Ndis802_11IBSS;
+	else if (fw_state & WIFI_STATION_STATE)
+		networkType = Ndis802_11Infrastructure;
+	else if (fw_state & WIFI_AP_STATE)
+		networkType = Ndis802_11APMode;
+	else
+		return;
+
+	rtw_setopmode_cmd(padapter, networkType, false);
+}
+
+uint rtw_hal_init(struct adapter *padapter)
+{
+	uint status;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	status = padapter->HalFunc.hal_init(padapter);
+
+	if (status == _SUCCESS) {
+		rtw_hal_init_opmode(padapter);
+
+		dvobj->padapters->hw_init_completed = true;
+
+		if (padapter->registrypriv.notch_filter == 1)
+			rtw_hal_notch_filter(padapter, 1);
+
+		rtw_hal_reset_security_engine(padapter);
+
+		rtw_sec_restore_wep_key(dvobj->padapters);
+
+		init_hw_mlme_ext(padapter);
+
+		rtw_bb_rf_gain_offset(padapter);
+	} else {
+		dvobj->padapters->hw_init_completed = false;
+	}
+
+	return status;
+}
+
+uint rtw_hal_deinit(struct adapter *padapter)
+{
+	uint status = _SUCCESS;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	status = padapter->HalFunc.hal_deinit(padapter);
+
+	if (status == _SUCCESS) {
+		padapter = dvobj->padapters;
+		padapter->hw_init_completed = false;
+	}
+
+	return status;
+}
+
+void rtw_hal_set_hwreg(struct adapter *padapter, u8 variable, u8 *val)
+{
+	if (padapter->HalFunc.SetHwRegHandler)
+		padapter->HalFunc.SetHwRegHandler(padapter, variable, val);
+}
+
+void rtw_hal_get_hwreg(struct adapter *padapter, u8 variable, u8 *val)
+{
+	if (padapter->HalFunc.GetHwRegHandler)
+		padapter->HalFunc.GetHwRegHandler(padapter, variable, val);
+}
+
+void rtw_hal_set_hwreg_with_buf(struct adapter *padapter, u8 variable, u8 *pbuf, int len)
+{
+	if (padapter->HalFunc.SetHwRegHandlerWithBuf)
+		padapter->HalFunc.SetHwRegHandlerWithBuf(padapter, variable, pbuf, len);
+}
+
+u8 rtw_hal_set_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue)
+{
+	if (padapter->HalFunc.SetHalDefVarHandler)
+		return padapter->HalFunc.SetHalDefVarHandler(padapter, eVariable, pValue);
+	return _FAIL;
+}
+
+u8 rtw_hal_get_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue)
+{
+	if (padapter->HalFunc.GetHalDefVarHandler)
+		return padapter->HalFunc.GetHalDefVarHandler(padapter, eVariable, pValue);
+	return _FAIL;
+}
+
+void rtw_hal_set_odm_var(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet)
+{
+	if (padapter->HalFunc.SetHalODMVarHandler)
+		padapter->HalFunc.SetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
+}
+
+void rtw_hal_get_odm_var(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, void *pValue2)
+{
+	if (padapter->HalFunc.GetHalODMVarHandler)
+		padapter->HalFunc.GetHalODMVarHandler(padapter, eVariable, pValue1, pValue2);
+}
+
+void rtw_hal_enable_interrupt(struct adapter *padapter)
+{
+	if (padapter->HalFunc.enable_interrupt)
+		padapter->HalFunc.enable_interrupt(padapter);
+}
+
+void rtw_hal_disable_interrupt(struct adapter *padapter)
+{
+	if (padapter->HalFunc.disable_interrupt)
+		padapter->HalFunc.disable_interrupt(padapter);
+}
+
+u8 rtw_hal_check_ips_status(struct adapter *padapter)
+{
+	u8 val = false;
+
+	if (padapter->HalFunc.check_ips_status)
+		val = padapter->HalFunc.check_ips_status(padapter);
+
+	return val;
+}
+
+s32	rtw_hal_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (padapter->HalFunc.hal_xmitframe_enqueue)
+		return padapter->HalFunc.hal_xmitframe_enqueue(padapter, pxmitframe);
+
+	return false;
+}
+
+s32	rtw_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (padapter->HalFunc.hal_xmit)
+		return padapter->HalFunc.hal_xmit(padapter, pxmitframe);
+
+	return false;
+}
+
+/*
+ * [IMPORTANT] This function would be run in interrupt context.
+ */
+s32	rtw_hal_mgnt_xmit(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	s32 ret = _FAIL;
+
+	update_mgntframe_attrib_addr(padapter, pmgntframe);
+	/* pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; */
+	/* pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; */
+	/* memcpy(pmgntframe->attrib.ra, pwlanhdr->addr1, ETH_ALEN); */
+
+	if (padapter->securitypriv.binstallBIPkey == true) {
+		if (IS_MCAST(pmgntframe->attrib.ra)) {
+			pmgntframe->attrib.encrypt = _BIP_;
+			/* pmgntframe->attrib.bswenc = true; */
+		} else {
+			pmgntframe->attrib.encrypt = _AES_;
+			pmgntframe->attrib.bswenc = true;
+		}
+		rtw_mgmt_xmitframe_coalesce(padapter, pmgntframe->pkt, pmgntframe);
+	}
+
+	if (padapter->HalFunc.mgnt_xmit)
+		ret = padapter->HalFunc.mgnt_xmit(padapter, pmgntframe);
+	return ret;
+}
+
+s32	rtw_hal_init_xmit_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.init_xmit_priv)
+		return padapter->HalFunc.init_xmit_priv(padapter);
+	return _FAIL;
+}
+
+void rtw_hal_free_xmit_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.free_xmit_priv)
+		padapter->HalFunc.free_xmit_priv(padapter);
+}
+
+s32	rtw_hal_init_recv_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.init_recv_priv)
+		return padapter->HalFunc.init_recv_priv(padapter);
+
+	return _FAIL;
+}
+
+void rtw_hal_free_recv_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.free_recv_priv)
+		padapter->HalFunc.free_recv_priv(padapter);
+}
+
+void rtw_hal_update_ra_mask(struct sta_info *psta, u8 rssi_level)
+{
+	struct adapter *padapter;
+	struct mlme_priv *pmlmepriv;
+
+	if (!psta)
+		return;
+
+	padapter = psta->padapter;
+
+	pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		add_RATid(padapter, psta, rssi_level);
+	else {
+		if (padapter->HalFunc.UpdateRAMaskHandler)
+			padapter->HalFunc.UpdateRAMaskHandler(padapter, psta->mac_id, rssi_level);
+	}
+}
+
+void rtw_hal_add_ra_tid(struct adapter *padapter, u32 bitmap, u8 *arg, u8 rssi_level)
+{
+	if (padapter->HalFunc.Add_RateATid)
+		padapter->HalFunc.Add_RateATid(padapter, bitmap, arg, rssi_level);
+}
+
+/*Start specifical interface thread		*/
+void rtw_hal_start_thread(struct adapter *padapter)
+{
+	if (padapter->HalFunc.run_thread)
+		padapter->HalFunc.run_thread(padapter);
+}
+/*Start specifical interface thread		*/
+void rtw_hal_stop_thread(struct adapter *padapter)
+{
+	if (padapter->HalFunc.cancel_thread)
+		padapter->HalFunc.cancel_thread(padapter);
+}
+
+u32 rtw_hal_read_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask)
+{
+	u32 data = 0;
+
+	if (padapter->HalFunc.read_bbreg)
+		 data = padapter->HalFunc.read_bbreg(padapter, RegAddr, BitMask);
+	return data;
+}
+void rtw_hal_write_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	if (padapter->HalFunc.write_bbreg)
+		padapter->HalFunc.write_bbreg(padapter, RegAddr, BitMask, Data);
+}
+
+u32 rtw_hal_read_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask)
+{
+	u32 data = 0;
+
+	if (padapter->HalFunc.read_rfreg)
+		data = padapter->HalFunc.read_rfreg(padapter, eRFPath, RegAddr, BitMask);
+	return data;
+}
+void rtw_hal_write_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	if (padapter->HalFunc.write_rfreg)
+		padapter->HalFunc.write_rfreg(padapter, eRFPath, RegAddr, BitMask, Data);
+}
+
+void rtw_hal_set_chan(struct adapter *padapter, u8 channel)
+{
+	if (padapter->HalFunc.set_channel_handler)
+		padapter->HalFunc.set_channel_handler(padapter, channel);
+}
+
+void rtw_hal_set_chnl_bw(struct adapter *padapter, u8 channel,
+			 enum channel_width Bandwidth, u8 Offset40, u8 Offset80)
+{
+	if (padapter->HalFunc.set_chnl_bw_handler)
+		padapter->HalFunc.set_chnl_bw_handler(padapter, channel,
+						      Bandwidth, Offset40,
+						      Offset80);
+}
+
+void rtw_hal_dm_watchdog(struct adapter *padapter)
+{
+	if (padapter->HalFunc.hal_dm_watchdog)
+		padapter->HalFunc.hal_dm_watchdog(padapter);
+}
+
+void rtw_hal_dm_watchdog_in_lps(struct adapter *padapter)
+{
+	if (adapter_to_pwrctl(padapter)->fw_current_in_ps_mode) {
+		if (padapter->HalFunc.hal_dm_watchdog_in_lps)
+			padapter->HalFunc.hal_dm_watchdog_in_lps(padapter); /* this function caller is in interrupt context */
+	}
+}
+
+void beacon_timing_control(struct adapter *padapter)
+{
+	if (padapter->HalFunc.SetBeaconRelatedRegistersHandler)
+		padapter->HalFunc.SetBeaconRelatedRegistersHandler(padapter);
+}
+
+
+s32 rtw_hal_xmit_thread_handler(struct adapter *padapter)
+{
+	if (padapter->HalFunc.xmit_thread_handler)
+		return padapter->HalFunc.xmit_thread_handler(padapter);
+	return _FAIL;
+}
+
+void rtw_hal_notch_filter(struct adapter *adapter, bool enable)
+{
+	if (adapter->HalFunc.hal_notch_filter)
+		adapter->HalFunc.hal_notch_filter(adapter, enable);
+}
+
+void rtw_hal_reset_security_engine(struct adapter *adapter)
+{
+	if (adapter->HalFunc.hal_reset_security_engine)
+		adapter->HalFunc.hal_reset_security_engine(adapter);
+}
+
+bool rtw_hal_c2h_valid(struct adapter *adapter, u8 *buf)
+{
+	return c2h_evt_valid((struct c2h_evt_hdr_88xx *)buf);
+}
+
+s32 rtw_hal_c2h_handler(struct adapter *adapter, u8 *c2h_evt)
+{
+	s32 ret = _FAIL;
+
+	if (adapter->HalFunc.c2h_handler)
+		ret = adapter->HalFunc.c2h_handler(adapter, c2h_evt);
+	return ret;
+}
+
+c2h_id_filter rtw_hal_c2h_id_filter_ccx(struct adapter *adapter)
+{
+	return adapter->HalFunc.c2h_id_filter_ccx;
+}
+
+s32 rtw_hal_macid_sleep(struct adapter *padapter, u32 macid)
+{
+	u8 support;
+
+	support = false;
+	rtw_hal_get_def_var(padapter, HAL_DEF_MACID_SLEEP, &support);
+	if (false == support)
+		return _FAIL;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MACID_SLEEP, (u8 *)&macid);
+
+	return _SUCCESS;
+}
+
+s32 rtw_hal_macid_wakeup(struct adapter *padapter, u32 macid)
+{
+	u8 support;
+
+	support = false;
+	rtw_hal_get_def_var(padapter, HAL_DEF_MACID_SLEEP, &support);
+	if (false == support)
+		return _FAIL;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MACID_WAKEUP, (u8 *)&macid);
+
+	return _SUCCESS;
+}
+
+s32 rtw_hal_fill_h2c_cmd(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+{
+	s32 ret = _FAIL;
+
+	if (padapter->HalFunc.fill_h2c_cmd)
+		ret = padapter->HalFunc.fill_h2c_cmd(padapter, ElementID, CmdLen, pCmdBuffer);
+
+	return ret;
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_pwr_seq.c b/drivers/staging/rtl8723bs/hal/hal_pwr_seq.c
new file mode 100644
index 000000000..fba67a7c0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_pwr_seq.c
@@ -0,0 +1,130 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+/*
+*
+This file includes all kinds of Power Action event for RTL8723B
+and corresponding hardware configurtions which are released from HW SD.
+
+Major Change History:
+	When       Who               What
+	---------- ---------------   -------------------------------
+	2011-08-08 Roger            Create.
+
+*/
+
+#include "hal_pwr_seq.h"
+
+/* drivers should parse below arrays and do the corresponding actions */
+/* 3 Power on  Array */
+struct wlan_pwr_cfg rtl8723B_power_on_flow[
+	RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8723B_radio_off_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_END
+};
+
+/* 3Card Disable Array */
+struct wlan_pwr_cfg rtl8723B_card_disable_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_CARDDIS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Card Enable Array */
+struct wlan_pwr_cfg rtl8723B_card_enable_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_CARDDIS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3Suspend Array */
+struct wlan_pwr_cfg rtl8723B_suspend_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_SUS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Resume Array */
+struct wlan_pwr_cfg rtl8723B_resume_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_SUS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3HWPDN Array */
+struct wlan_pwr_cfg rtl8723B_hwpdn_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_PDN
+	RTL8723B_TRANS_END
+};
+
+/* 3 Enter LPS */
+struct wlan_pwr_cfg rtl8723B_enter_lps_flow[
+	RTL8723B_TRANS_ACT_TO_LPS_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* FW behavior */
+	RTL8723B_TRANS_ACT_TO_LPS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Leave LPS */
+struct wlan_pwr_cfg rtl8723B_leave_lps_flow[
+	RTL8723B_TRANS_LPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* FW behavior */
+	RTL8723B_TRANS_LPS_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3 Enter SW LPS */
+struct wlan_pwr_cfg rtl8723B_enter_swlps_flow[
+	RTL8723B_TRANS_ACT_TO_SWLPS_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* SW behavior */
+	RTL8723B_TRANS_ACT_TO_SWLPS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Leave SW LPS */
+struct wlan_pwr_cfg rtl8723B_leave_swlps_flow[
+	RTL8723B_TRANS_SWLPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* SW behavior */
+	RTL8723B_TRANS_SWLPS_TO_ACT
+	RTL8723B_TRANS_END
+};
diff --git a/drivers/staging/rtl8723bs/hal/hal_sdio.c b/drivers/staging/rtl8723bs/hal/hal_sdio.c
new file mode 100644
index 000000000..9de62a0f5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_sdio.c
@@ -0,0 +1,106 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+u8 rtw_hal_sdio_max_txoqt_free_space(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if (pHalData->SdioTxOQTMaxFreeSpace < 8)
+		pHalData->SdioTxOQTMaxFreeSpace = 8;
+
+	return pHalData->SdioTxOQTMaxFreeSpace;
+}
+
+u8 rtw_hal_sdio_query_tx_freepage(
+	struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if ((pHalData->SdioTxFIFOFreePage[PageIdx]+pHalData->SdioTxFIFOFreePage[PUBLIC_QUEUE_IDX]) >= (RequiredPageNum))
+		return true;
+	else
+		return false;
+}
+
+void rtw_hal_sdio_update_tx_freepage(
+	struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 DedicatedPgNum = 0;
+	u8 RequiredPublicFreePgNum = 0;
+	/* _irqL irql; */
+
+	/* spin_lock_bh(&pHalData->SdioTxFIFOFreePageLock); */
+
+	DedicatedPgNum = pHalData->SdioTxFIFOFreePage[PageIdx];
+	if (RequiredPageNum <= DedicatedPgNum) {
+		pHalData->SdioTxFIFOFreePage[PageIdx] -= RequiredPageNum;
+	} else {
+		pHalData->SdioTxFIFOFreePage[PageIdx] = 0;
+		RequiredPublicFreePgNum = RequiredPageNum - DedicatedPgNum;
+		pHalData->SdioTxFIFOFreePage[PUBLIC_QUEUE_IDX] -= RequiredPublicFreePgNum;
+	}
+
+	/* spin_unlock_bh(&pHalData->SdioTxFIFOFreePageLock); */
+}
+
+void rtw_hal_set_sdio_tx_max_length(
+	struct adapter *padapter, u8 numHQ, u8 numNQ, u8 numLQ, u8 numPubQ
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u32 page_size;
+	u32 lenHQ, lenNQ, lenLQ;
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_PAGE_SIZE, &page_size);
+
+	lenHQ = ((numHQ + numPubQ) >> 1) * page_size;
+	lenNQ = ((numNQ + numPubQ) >> 1) * page_size;
+	lenLQ = ((numLQ + numPubQ) >> 1) * page_size;
+
+	pHalData->sdio_tx_max_len[HI_QUEUE_IDX] =
+		(lenHQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenHQ;
+	pHalData->sdio_tx_max_len[MID_QUEUE_IDX] =
+		(lenNQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenNQ;
+	pHalData->sdio_tx_max_len[LOW_QUEUE_IDX] =
+		(lenLQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenLQ;
+}
+
+u32 rtw_hal_get_sdio_tx_max_length(struct adapter *padapter, u8 queue_idx)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u32 deviceId, max_len;
+
+
+	deviceId = ffaddr2deviceId(pdvobjpriv, queue_idx);
+	switch (deviceId) {
+	case WLAN_TX_HIQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[HI_QUEUE_IDX];
+		break;
+
+	case WLAN_TX_MIQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[MID_QUEUE_IDX];
+		break;
+
+	case WLAN_TX_LOQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[LOW_QUEUE_IDX];
+		break;
+
+	default:
+		max_len = pHalData->sdio_tx_max_len[MID_QUEUE_IDX];
+		break;
+	}
+
+	return max_len;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm.c b/drivers/staging/rtl8723bs/hal/odm.c
new file mode 100644
index 000000000..31f65d817
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm.c
@@ -0,0 +1,1197 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+/*  Global var */
+
+u32 OFDMSwingTable[OFDM_TABLE_SIZE] = {
+	0x7f8001fe, /*  0, +6.0dB */
+	0x788001e2, /*  1, +5.5dB */
+	0x71c001c7, /*  2, +5.0dB */
+	0x6b8001ae, /*  3, +4.5dB */
+	0x65400195, /*  4, +4.0dB */
+	0x5fc0017f, /*  5, +3.5dB */
+	0x5a400169, /*  6, +3.0dB */
+	0x55400155, /*  7, +2.5dB */
+	0x50800142, /*  8, +2.0dB */
+	0x4c000130, /*  9, +1.5dB */
+	0x47c0011f, /*  10, +1.0dB */
+	0x43c0010f, /*  11, +0.5dB */
+	0x40000100, /*  12, +0dB */
+	0x3c8000f2, /*  13, -0.5dB */
+	0x390000e4, /*  14, -1.0dB */
+	0x35c000d7, /*  15, -1.5dB */
+	0x32c000cb, /*  16, -2.0dB */
+	0x300000c0, /*  17, -2.5dB */
+	0x2d4000b5, /*  18, -3.0dB */
+	0x2ac000ab, /*  19, -3.5dB */
+	0x288000a2, /*  20, -4.0dB */
+	0x26000098, /*  21, -4.5dB */
+	0x24000090, /*  22, -5.0dB */
+	0x22000088, /*  23, -5.5dB */
+	0x20000080, /*  24, -6.0dB */
+	0x1e400079, /*  25, -6.5dB */
+	0x1c800072, /*  26, -7.0dB */
+	0x1b00006c, /*  27. -7.5dB */
+	0x19800066, /*  28, -8.0dB */
+	0x18000060, /*  29, -8.5dB */
+	0x16c0005b, /*  30, -9.0dB */
+	0x15800056, /*  31, -9.5dB */
+	0x14400051, /*  32, -10.0dB */
+	0x1300004c, /*  33, -10.5dB */
+	0x12000048, /*  34, -11.0dB */
+	0x11000044, /*  35, -11.5dB */
+	0x10000040, /*  36, -12.0dB */
+};
+
+u8 CCKSwingTable_Ch1_Ch13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}, /*  0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /*  1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /*  2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /*  3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /*  4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /*  5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /*  6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /*  7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /*  8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /*  9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /*  10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /*  11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /*  12, -6.0dB <== default */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /*  13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /*  14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /*  15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /*  16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /*  17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /*  18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*  23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*  24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*  25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*  26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*  31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}	/*  32, -16.0dB */
+};
+
+u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}, /*  0, +0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /*  1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /*  2, -1.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /*  3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /*  4, -2.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /*  5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /*  6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /*  7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /*  8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /*  9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /*  10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  12, -6.0dB  <== default */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /*  13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /*  14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}	/*  32, -16.0dB */
+};
+
+u32 OFDMSwingTable_New[OFDM_TABLE_SIZE] = {
+	0x0b40002d, /*  0,  -15.0dB */
+	0x0c000030, /*  1,  -14.5dB */
+	0x0cc00033, /*  2,  -14.0dB */
+	0x0d800036, /*  3,  -13.5dB */
+	0x0e400039, /*  4,  -13.0dB */
+	0x0f00003c, /*  5,  -12.5dB */
+	0x10000040, /*  6,  -12.0dB */
+	0x11000044, /*  7,  -11.5dB */
+	0x12000048, /*  8,  -11.0dB */
+	0x1300004c, /*  9,  -10.5dB */
+	0x14400051, /*  10, -10.0dB */
+	0x15800056, /*  11, -9.5dB */
+	0x16c0005b, /*  12, -9.0dB */
+	0x18000060, /*  13, -8.5dB */
+	0x19800066, /*  14, -8.0dB */
+	0x1b00006c, /*  15, -7.5dB */
+	0x1c800072, /*  16, -7.0dB */
+	0x1e400079, /*  17, -6.5dB */
+	0x20000080, /*  18, -6.0dB */
+	0x22000088, /*  19, -5.5dB */
+	0x24000090, /*  20, -5.0dB */
+	0x26000098, /*  21, -4.5dB */
+	0x288000a2, /*  22, -4.0dB */
+	0x2ac000ab, /*  23, -3.5dB */
+	0x2d4000b5, /*  24, -3.0dB */
+	0x300000c0, /*  25, -2.5dB */
+	0x32c000cb, /*  26, -2.0dB */
+	0x35c000d7, /*  27, -1.5dB */
+	0x390000e4, /*  28, -1.0dB */
+	0x3c8000f2, /*  29, -0.5dB */
+	0x40000100, /*  30, +0dB */
+	0x43c0010f, /*  31, +0.5dB */
+	0x47c0011f, /*  32, +1.0dB */
+	0x4c000130, /*  33, +1.5dB */
+	0x50800142, /*  34, +2.0dB */
+	0x55400155, /*  35, +2.5dB */
+	0x5a400169, /*  36, +3.0dB */
+	0x5fc0017f, /*  37, +3.5dB */
+	0x65400195, /*  38, +4.0dB */
+	0x6b8001ae, /*  39, +4.5dB */
+	0x71c001c7, /*  40, +5.0dB */
+	0x788001e2, /*  41, +5.5dB */
+	0x7f8001fe  /*  42, +6.0dB */
+};
+
+u8 CCKSwingTable_Ch1_Ch13_New[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}, /*   0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*   1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*   2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*   3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*   4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*   5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*   6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*   7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*   8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*   9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /*  14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /*  15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /*  16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /*  17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /*  18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /*  19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /*  20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /*  21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /*  22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /*  23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /*  24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /*  25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /*  26, -3.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /*  27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /*  28, -2.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /*  29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /*  30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /*  31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}	/*  32, +0dB */
+};
+
+u8 CCKSwingTable_Ch14_New[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}, /*   0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*   8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*   9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /*  18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /*  19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /*  22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /*  23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /*  24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /*  25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /*  26, -3.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /*  27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /*  28, -2.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /*  29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /*  30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /*  31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}	/*  32, +0dB */
+};
+
+u32 TxScalingTable_Jaguar[TXSCALE_TABLE_SIZE] = {
+	0x081, /*  0,  -12.0dB */
+	0x088, /*  1,  -11.5dB */
+	0x090, /*  2,  -11.0dB */
+	0x099, /*  3,  -10.5dB */
+	0x0A2, /*  4,  -10.0dB */
+	0x0AC, /*  5,  -9.5dB */
+	0x0B6, /*  6,  -9.0dB */
+	0x0C0, /*  7,  -8.5dB */
+	0x0CC, /*  8,  -8.0dB */
+	0x0D8, /*  9,  -7.5dB */
+	0x0E5, /*  10, -7.0dB */
+	0x0F2, /*  11, -6.5dB */
+	0x101, /*  12, -6.0dB */
+	0x110, /*  13, -5.5dB */
+	0x120, /*  14, -5.0dB */
+	0x131, /*  15, -4.5dB */
+	0x143, /*  16, -4.0dB */
+	0x156, /*  17, -3.5dB */
+	0x16A, /*  18, -3.0dB */
+	0x180, /*  19, -2.5dB */
+	0x197, /*  20, -2.0dB */
+	0x1AF, /*  21, -1.5dB */
+	0x1C8, /*  22, -1.0dB */
+	0x1E3, /*  23, -0.5dB */
+	0x200, /*  24, +0  dB */
+	0x21E, /*  25, +0.5dB */
+	0x23E, /*  26, +1.0dB */
+	0x261, /*  27, +1.5dB */
+	0x285, /*  28, +2.0dB */
+	0x2AB, /*  29, +2.5dB */
+	0x2D3, /*  30, +3.0dB */
+	0x2FE, /*  31, +3.5dB */
+	0x32B, /*  32, +4.0dB */
+	0x35C, /*  33, +4.5dB */
+	0x38E, /*  34, +5.0dB */
+	0x3C4, /*  35, +5.5dB */
+	0x3FE  /*  36, +6.0dB */
+};
+
+/* Remove Edca by Yu Chen */
+
+static void odm_CommonInfoSelfInit(struct dm_odm_t *pDM_Odm)
+{
+	pDM_Odm->bCckHighPower = (bool) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(CCK_RPT_FORMAT, pDM_Odm), ODM_BIT(CCK_RPT_FORMAT, pDM_Odm));
+	pDM_Odm->RFPathRxEnable = (u8) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(BB_RX_PATH, pDM_Odm), ODM_BIT(BB_RX_PATH, pDM_Odm));
+
+	pDM_Odm->TxRate = 0xFF;
+}
+
+static void odm_CommonInfoSelfUpdate(struct dm_odm_t *pDM_Odm)
+{
+	u8 EntryCnt = 0;
+	u8 i;
+	PSTA_INFO_T	pEntry;
+
+	if (*(pDM_Odm->pBandWidth) == ODM_BW40M) {
+		if (*(pDM_Odm->pSecChOffset) == 1)
+			pDM_Odm->ControlChannel = *(pDM_Odm->pChannel)-2;
+		else if (*(pDM_Odm->pSecChOffset) == 2)
+			pDM_Odm->ControlChannel = *(pDM_Odm->pChannel)+2;
+	} else
+		pDM_Odm->ControlChannel = *(pDM_Odm->pChannel);
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pEntry = pDM_Odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(pEntry))
+			EntryCnt++;
+	}
+
+	if (EntryCnt == 1)
+		pDM_Odm->bOneEntryOnly = true;
+	else
+		pDM_Odm->bOneEntryOnly = false;
+}
+
+static void odm_CmnInfoInit_Debug(struct dm_odm_t *pDM_Odm)
+{
+}
+
+static void odm_BasicDbgMessage(struct dm_odm_t *pDM_Odm)
+{
+}
+
+/* 3 ============================================================ */
+/* 3 RATR MASK */
+/* 3 ============================================================ */
+/* 3 ============================================================ */
+/* 3 Rate Adaptive */
+/* 3 ============================================================ */
+
+static void odm_RateAdaptiveMaskInit(struct dm_odm_t *pDM_Odm)
+{
+	struct odm_rate_adaptive *pOdmRA = &pDM_Odm->RateAdaptive;
+
+	pOdmRA->Type = DM_Type_ByDriver;
+	if (pOdmRA->Type == DM_Type_ByDriver)
+		pDM_Odm->bUseRAMask = true;
+	else
+		pDM_Odm->bUseRAMask = false;
+
+	pOdmRA->RATRState = DM_RATR_STA_INIT;
+	pOdmRA->LdpcThres = 35;
+	pOdmRA->bUseLdpc = false;
+	pOdmRA->HighRSSIThresh = 50;
+	pOdmRA->LowRSSIThresh = 20;
+}
+
+u32 ODM_Get_Rate_Bitmap(
+	struct dm_odm_t *pDM_Odm,
+	u32 macid,
+	u32 ra_mask,
+	u8 rssi_level
+)
+{
+	PSTA_INFO_T	pEntry;
+	u32 rate_bitmap = 0;
+	u8 WirelessMode;
+
+	pEntry = pDM_Odm->pODM_StaInfo[macid];
+	if (!IS_STA_VALID(pEntry))
+		return ra_mask;
+
+	WirelessMode = pEntry->wireless_mode;
+
+	switch (WirelessMode) {
+	case ODM_WM_B:
+		if (ra_mask & 0x0000000c)		/* 11M or 5.5M enable */
+			rate_bitmap = 0x0000000d;
+		else
+			rate_bitmap = 0x0000000f;
+		break;
+
+	case (ODM_WM_G):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x00000f00;
+		else
+			rate_bitmap = 0x00000ff0;
+		break;
+
+	case (ODM_WM_B|ODM_WM_G):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x00000f00;
+		else if (rssi_level == DM_RATR_STA_MIDDLE)
+			rate_bitmap = 0x00000ff0;
+		else
+			rate_bitmap = 0x00000ff5;
+		break;
+
+	case (ODM_WM_B|ODM_WM_G|ODM_WM_N24G):
+	case (ODM_WM_B|ODM_WM_N24G):
+	case (ODM_WM_G|ODM_WM_N24G):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x000f0000;
+		else if (rssi_level == DM_RATR_STA_MIDDLE)
+			rate_bitmap = 0x000ff000;
+		else {
+			if (*(pDM_Odm->pBandWidth) == ODM_BW40M)
+				rate_bitmap = 0x000ff015;
+			else
+				rate_bitmap = 0x000ff005;
+		}
+		break;
+
+	default:
+		rate_bitmap = 0x0fffffff;
+		break;
+	}
+
+	return ra_mask & rate_bitmap;
+
+}
+
+static void odm_RefreshRateAdaptiveMaskCE(struct dm_odm_t *pDM_Odm)
+{
+	u8 i;
+	struct adapter *padapter =  pDM_Odm->Adapter;
+
+	if (padapter->bDriverStopped) {
+		return;
+	}
+
+	if (!pDM_Odm->bUseRAMask) {
+		return;
+	}
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		PSTA_INFO_T pstat = pDM_Odm->pODM_StaInfo[i];
+
+		if (IS_STA_VALID(pstat)) {
+			if (IS_MCAST(pstat->hwaddr))  /* if (psta->mac_id == 1) */
+				continue;
+
+			if (true == ODM_RAStateCheck(pDM_Odm, pstat->rssi_stat.UndecoratedSmoothedPWDB, false, &pstat->rssi_level)) {
+				/* printk("RSSI:%d, RSSI_LEVEL:%d\n", pstat->rssi_stat.UndecoratedSmoothedPWDB, pstat->rssi_level); */
+				rtw_hal_update_ra_mask(pstat, pstat->rssi_level);
+			}
+
+		}
+	}
+}
+
+/*-----------------------------------------------------------------------------
+* Function:	odm_RefreshRateAdaptiveMask()
+*
+* Overview:	Update rate table mask according to rssi
+*
+* Input:		NONE
+*
+* Output:		NONE
+*
+* Return:		NONE
+*
+* Revised History:
+*When		Who		Remark
+*05/27/2009	hpfan	Create Version 0.
+*
+* --------------------------------------------------------------------------
+*/
+static void odm_RefreshRateAdaptiveMask(struct dm_odm_t *pDM_Odm)
+{
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RA_MASK)) {
+		return;
+	}
+	odm_RefreshRateAdaptiveMaskCE(pDM_Odm);
+}
+
+/*  Return Value: bool */
+/*  - true: RATRState is changed. */
+bool ODM_RAStateCheck(
+	struct dm_odm_t *pDM_Odm,
+	s32 RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+)
+{
+	struct odm_rate_adaptive *pRA = &pDM_Odm->RateAdaptive;
+	const u8 GoUpGap = 5;
+	u8 HighRSSIThreshForRA = pRA->HighRSSIThresh;
+	u8 LowRSSIThreshForRA = pRA->LowRSSIThresh;
+	u8 RATRState;
+
+	/*  Threshold Adjustment: */
+	/*  when RSSI state trends to go up one or two levels, make sure RSSI is high enough. */
+	/*  Here GoUpGap is added to solve the boundary's level alternation issue. */
+	switch (*pRATRState) {
+	case DM_RATR_STA_INIT:
+	case DM_RATR_STA_HIGH:
+		break;
+
+	case DM_RATR_STA_MIDDLE:
+		HighRSSIThreshForRA += GoUpGap;
+		break;
+
+	case DM_RATR_STA_LOW:
+		HighRSSIThreshForRA += GoUpGap;
+		LowRSSIThreshForRA += GoUpGap;
+		break;
+
+	default:
+		netdev_dbg(pDM_Odm->Adapter->pnetdev,
+			   "wrong rssi level setting %d !", *pRATRState);
+		break;
+	}
+
+	/*  Decide RATRState by RSSI. */
+	if (RSSI > HighRSSIThreshForRA)
+		RATRState = DM_RATR_STA_HIGH;
+	else if (RSSI > LowRSSIThreshForRA)
+		RATRState = DM_RATR_STA_MIDDLE;
+	else
+		RATRState = DM_RATR_STA_LOW;
+	/* printk("==>%s, RATRState:0x%02x , RSSI:%d\n", __func__, RATRState, RSSI); */
+
+	if (*pRATRState != RATRState || bForceUpdate) {
+		*pRATRState = RATRState;
+		return true;
+	}
+
+	return false;
+}
+
+/*  */
+
+/* 3 ============================================================ */
+/* 3 RSSI Monitor */
+/* 3 ============================================================ */
+
+static void odm_RSSIMonitorInit(struct dm_odm_t *pDM_Odm)
+{
+	struct ra_t *pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	pRA_Table->firstconnect = false;
+
+}
+
+static void FindMinimumRSSI(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	/* 1 1.Determine the minimum RSSI */
+
+	if (
+		(pDM_Odm->bLinked != true) &&
+		(pdmpriv->EntryMinUndecoratedSmoothedPWDB == 0)
+	) {
+		pdmpriv->MinUndecoratedPWDBForDM = 0;
+	} else
+		pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+}
+
+static void odm_RSSIMonitorCheckCE(struct dm_odm_t *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	int i;
+	int tmpEntryMaxPWDB = 0, tmpEntryMinPWDB = 0xff;
+	u8 sta_cnt = 0;
+	u32 PWDB_rssi[NUM_STA] = {0};/* 0~15]:MACID, [16~31]:PWDB_rssi */
+	struct ra_t *pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	if (pDM_Odm->bLinked != true)
+		return;
+
+	pRA_Table->firstconnect = pDM_Odm->bLinked;
+
+	/* if (check_fwstate(&Adapter->mlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true) */
+	{
+		struct sta_info *psta;
+
+		for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+			psta = pDM_Odm->pODM_StaInfo[i];
+			if (IS_STA_VALID(psta)) {
+				if (IS_MCAST(psta->hwaddr))  /* if (psta->mac_id == 1) */
+					continue;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB == (-1))
+					continue;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB < tmpEntryMinPWDB)
+					tmpEntryMinPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB > tmpEntryMaxPWDB)
+					tmpEntryMaxPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB != (-1))
+					PWDB_rssi[sta_cnt++] = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB<<16));
+			}
+		}
+
+		/* printk("%s ==> sta_cnt(%d)\n", __func__, sta_cnt); */
+
+		for (i = 0; i < sta_cnt; i++) {
+			if (PWDB_rssi[i] != (0)) {
+				if (pHalData->fw_ractrl == true)/*  Report every sta's RSSI to FW */
+					rtl8723b_set_rssi_cmd(Adapter, (u8 *)(&PWDB_rssi[i]));
+			}
+		}
+	}
+
+
+
+	if (tmpEntryMaxPWDB != 0)	/*  If associated entry is found */
+		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = tmpEntryMaxPWDB;
+	else
+		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = 0;
+
+	if (tmpEntryMinPWDB != 0xff) /*  If associated entry is found */
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = tmpEntryMinPWDB;
+	else
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = 0;
+
+	FindMinimumRSSI(Adapter);/* get pdmpriv->MinUndecoratedPWDBForDM */
+
+	pDM_Odm->RSSI_Min = pdmpriv->MinUndecoratedPWDBForDM;
+	/* ODM_CmnInfoUpdate(&pHalData->odmpriv , ODM_CMNINFO_RSSI_MIN, pdmpriv->MinUndecoratedPWDBForDM); */
+}
+
+static void odm_RSSIMonitorCheck(struct dm_odm_t *pDM_Odm)
+{
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		return;
+
+	odm_RSSIMonitorCheckCE(pDM_Odm);
+
+}	/*  odm_RSSIMonitorCheck */
+
+/* 3 ============================================================ */
+/* 3 SW Antenna Diversity */
+/* 3 ============================================================ */
+static void odm_SwAntDetectInit(struct dm_odm_t *pDM_Odm)
+{
+	struct swat_t *pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+
+	pDM_SWAT_Table->SWAS_NoLink_BK_Reg92c = rtw_read32(pDM_Odm->Adapter, rDPDT_control);
+	pDM_SWAT_Table->PreAntenna = MAIN_ANT;
+	pDM_SWAT_Table->CurAntenna = MAIN_ANT;
+	pDM_SWAT_Table->SWAS_NoLink_State = 0;
+}
+
+/* 3 ============================================================ */
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+
+static u8 getSwingIndex(struct dm_odm_t *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	u8 i = 0;
+	u32 bbSwing;
+	u32 swingTableSize;
+	u32 *pSwingTable;
+
+	bbSwing = PHY_QueryBBReg(Adapter, rOFDM0_XATxIQImbalance, 0xFFC00000);
+
+	pSwingTable = OFDMSwingTable_New;
+	swingTableSize = OFDM_TABLE_SIZE;
+
+	for (i = 0; i < swingTableSize; ++i) {
+		u32 tableValue = pSwingTable[i];
+
+		if (tableValue >= 0x100000)
+			tableValue >>= 22;
+		if (bbSwing == tableValue)
+			break;
+	}
+	return i;
+}
+
+void odm_TXPowerTrackingInit(struct dm_odm_t *pDM_Odm)
+{
+	u8 defaultSwingIndex = getSwingIndex(pDM_Odm);
+	u8 p = 0;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	pdmpriv->bTXPowerTracking = true;
+	pdmpriv->TXPowercount = 0;
+	pdmpriv->bTXPowerTrackingInit = false;
+
+	if (*(pDM_Odm->mp_mode) != 1)
+		pdmpriv->TxPowerTrackControl = true;
+	else
+		pdmpriv->TxPowerTrackControl = false;
+
+	/* pDM_Odm->RFCalibrateInfo.TxPowerTrackControl = true; */
+	pDM_Odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_IQK = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = pHalData->EEPROMThermalMeter;
+
+	/*  The index of "0 dB" in SwingTable. */
+	pDM_Odm->DefaultOfdmIndex = (defaultSwingIndex >= OFDM_TABLE_SIZE) ? 30 : defaultSwingIndex;
+	pDM_Odm->DefaultCckIndex = 20;
+
+	pDM_Odm->BbSwingIdxCckBase = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->RFCalibrateInfo.CCK_index = pDM_Odm->DefaultCckIndex;
+
+	for (p = RF_PATH_A; p < MAX_RF_PATH; ++p) {
+		pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.OFDM_index[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p] = 0;
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+	}
+
+}
+
+void ODM_TXPowerTrackingCheck(struct dm_odm_t *pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_TX_PWR_TRACK))
+		return;
+
+	if (!pDM_Odm->RFCalibrateInfo.TM_Trigger) { /* at least delay 1 sec */
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH_A, RF_T_METER_NEW, (BIT17 | BIT16), 0x03);
+
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 1;
+		return;
+	} else {
+		ODM_TXPowerTrackingCallback_ThermalMeter(Adapter);
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 0;
+	}
+}
+
+/*  */
+/* 3 Export Interface */
+/*  */
+
+/*  */
+/*  2011/09/21 MH Add to describe different team necessary resource allocate?? */
+/*  */
+void ODM_DMInit(struct dm_odm_t *pDM_Odm)
+{
+
+	odm_CommonInfoSelfInit(pDM_Odm);
+	odm_CmnInfoInit_Debug(pDM_Odm);
+	odm_DIGInit(pDM_Odm);
+	odm_NHMCounterStatisticsInit(pDM_Odm);
+	odm_AdaptivityInit(pDM_Odm);
+	odm_RateAdaptiveMaskInit(pDM_Odm);
+	ODM_CfoTrackingInit(pDM_Odm);
+	ODM_EdcaTurboInit(pDM_Odm);
+	odm_RSSIMonitorInit(pDM_Odm);
+	odm_TXPowerTrackingInit(pDM_Odm);
+
+	ODM_ClearTxPowerTrackingState(pDM_Odm);
+
+	odm_DynamicBBPowerSavingInit(pDM_Odm);
+	odm_DynamicTxPowerInit(pDM_Odm);
+
+	odm_SwAntDetectInit(pDM_Odm);
+}
+
+/*  */
+/*  2011/09/20 MH This is the entry pointer for all team to execute HW out source DM. */
+/*  You can not add any dummy function here, be care, you can only use DM structure */
+/*  to perform any new ODM_DM. */
+/*  */
+void ODM_DMWatchdog(struct dm_odm_t *pDM_Odm)
+{
+	odm_CommonInfoSelfUpdate(pDM_Odm);
+	odm_BasicDbgMessage(pDM_Odm);
+	odm_FalseAlarmCounterStatistics(pDM_Odm);
+	odm_NHMCounterStatistics(pDM_Odm);
+
+	odm_RSSIMonitorCheck(pDM_Odm);
+
+	/* For CE Platform(SPRD or Tablet) */
+	/* 8723A or 8189ES platform */
+	/* NeilChen--2012--08--24-- */
+	/* Fix Leave LPS issue */
+	if ((adapter_to_pwrctl(pDM_Odm->Adapter)->pwr_mode != PS_MODE_ACTIVE) /*  in LPS mode */
+		/*  */
+		/* (pDM_Odm->SupportICType & (ODM_RTL8723A))|| */
+		/* (pDM_Odm->SupportICType & (ODM_RTL8188E) &&(&&(((pDM_Odm->SupportInterface  == ODM_ITRF_SDIO))) */
+		/*  */
+	) {
+			odm_DIGbyRSSI_LPS(pDM_Odm);
+	} else
+		odm_DIG(pDM_Odm);
+
+	{
+		struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+		odm_Adaptivity(pDM_Odm, pDM_DigTable->CurIGValue);
+	}
+	odm_CCKPacketDetectionThresh(pDM_Odm);
+
+	if (*(pDM_Odm->pbPowerSaving) == true)
+		return;
+
+
+	odm_RefreshRateAdaptiveMask(pDM_Odm);
+	odm_EdcaTurboCheck(pDM_Odm);
+	ODM_CfoTracking(pDM_Odm);
+
+	ODM_TXPowerTrackingCheck(pDM_Odm);
+
+	/* odm_EdcaTurboCheck(pDM_Odm); */
+
+	/* 2010.05.30 LukeLee: For CE platform, files in IC subfolders may not be included to be compiled, */
+	/*  so compile flags must be left here to prevent from compile errors */
+	pDM_Odm->PhyDbgInfo.NumQryBeaconPkt = 0;
+}
+
+
+/*  */
+/*  Init /.. Fixed HW value. Only init time. */
+/*  */
+void ODM_CmnInfoInit(struct dm_odm_t *pDM_Odm, enum odm_cmninfo_e CmnInfo, u32 Value)
+{
+	/*  */
+	/*  This section is used for init value */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Fixed ODM value. */
+	/*  */
+	case ODM_CMNINFO_ABILITY:
+		pDM_Odm->SupportAbility = (u32)Value;
+		break;
+
+	case ODM_CMNINFO_PLATFORM:
+		pDM_Odm->SupportPlatform = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_INTERFACE:
+		pDM_Odm->SupportInterface = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_IC_TYPE:
+		pDM_Odm->SupportICType = Value;
+		break;
+
+	case ODM_CMNINFO_CUT_VER:
+		pDM_Odm->CutVersion = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_FAB_VER:
+		pDM_Odm->FabVersion = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_RFE_TYPE:
+		pDM_Odm->RFEType = (u8)Value;
+		break;
+
+	case    ODM_CMNINFO_RF_ANTENNA_TYPE:
+		pDM_Odm->AntDivType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_PACKAGE_TYPE:
+		pDM_Odm->PackageType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_LNA:
+		pDM_Odm->ExtLNA = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_PA:
+		pDM_Odm->ExtPA = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_GPA:
+		pDM_Odm->TypeGPA = (enum odm_type_gpa_e)Value;
+		break;
+	case ODM_CMNINFO_APA:
+		pDM_Odm->TypeAPA = (enum odm_type_apa_e)Value;
+		break;
+	case ODM_CMNINFO_GLNA:
+		pDM_Odm->TypeGLNA = (enum odm_type_glna_e)Value;
+		break;
+	case ODM_CMNINFO_ALNA:
+		pDM_Odm->TypeALNA = (enum odm_type_alna_e)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_TRSW:
+		pDM_Odm->ExtTRSW = (u8)Value;
+		break;
+	case ODM_CMNINFO_PATCH_ID:
+		pDM_Odm->PatchID = (u8)Value;
+		break;
+	case ODM_CMNINFO_BINHCT_TEST:
+		pDM_Odm->bInHctTest = (bool)Value;
+		break;
+	case ODM_CMNINFO_BWIFI_TEST:
+		pDM_Odm->bWIFITest = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_SMART_CONCURRENT:
+		pDM_Odm->bDualMacSmartConcurrent = (bool)Value;
+		break;
+
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+void ODM_CmnInfoHook(struct dm_odm_t *pDM_Odm, enum odm_cmninfo_e CmnInfo, void *pValue)
+{
+	/*  */
+	/*  Hook call by reference pointer. */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Dynamic call by reference pointer. */
+	/*  */
+	case ODM_CMNINFO_MAC_PHY_MODE:
+		pDM_Odm->pMacPhyMode = pValue;
+		break;
+
+	case ODM_CMNINFO_TX_UNI:
+		pDM_Odm->pNumTxBytesUnicast = pValue;
+		break;
+
+	case ODM_CMNINFO_RX_UNI:
+		pDM_Odm->pNumRxBytesUnicast = pValue;
+		break;
+
+	case ODM_CMNINFO_WM_MODE:
+		pDM_Odm->pwirelessmode = pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_CHNL_OFFSET:
+		pDM_Odm->pSecChOffset = pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_MODE:
+		pDM_Odm->pSecurity = pValue;
+		break;
+
+	case ODM_CMNINFO_BW:
+		pDM_Odm->pBandWidth = pValue;
+		break;
+
+	case ODM_CMNINFO_CHNL:
+		pDM_Odm->pChannel = pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_GET_VALUE:
+		pDM_Odm->pbGetValueFromOtherMac = pValue;
+		break;
+
+	case ODM_CMNINFO_BUDDY_ADAPTOR:
+		pDM_Odm->pBuddyAdapter = pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_IS_MASTER:
+		pDM_Odm->pbMasterOfDMSP = pValue;
+		break;
+
+	case ODM_CMNINFO_SCAN:
+		pDM_Odm->pbScanInProcess = pValue;
+		break;
+
+	case ODM_CMNINFO_POWER_SAVING:
+		pDM_Odm->pbPowerSaving = pValue;
+		break;
+
+	case ODM_CMNINFO_ONE_PATH_CCA:
+		pDM_Odm->pOnePathCCA = pValue;
+		break;
+
+	case ODM_CMNINFO_DRV_STOP:
+		pDM_Odm->pbDriverStopped =  pValue;
+		break;
+
+	case ODM_CMNINFO_PNP_IN:
+		pDM_Odm->pbDriverIsGoingToPnpSetPowerSleep =  pValue;
+		break;
+
+	case ODM_CMNINFO_INIT_ON:
+		pDM_Odm->pinit_adpt_in_progress =  pValue;
+		break;
+
+	case ODM_CMNINFO_ANT_TEST:
+		pDM_Odm->pAntennaTest =  pValue;
+		break;
+
+	case ODM_CMNINFO_NET_CLOSED:
+		pDM_Odm->pbNet_closed = pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_RATE:
+		pDM_Odm->pForcedDataRate = pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_IGI_LB:
+		pDM_Odm->pu1ForcedIgiLb = pValue;
+		break;
+
+	case ODM_CMNINFO_MP_MODE:
+		pDM_Odm->mp_mode = pValue;
+		break;
+
+	/* case ODM_CMNINFO_RTSTA_AID: */
+	/* pDM_Odm->pAidMap =  (u8 *)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_BT_COEXIST: */
+	/* pDM_Odm->BTCoexist = (bool *)pValue; */
+
+	/* case ODM_CMNINFO_STA_STATUS: */
+	/* pDM_Odm->pODM_StaInfo[] = (PSTA_INFO_T)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_PHY_STATUS: */
+	/* pDM_Odm->pPhyInfo = (ODM_PHY_INFO *)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_MAC_STATUS: */
+	/* pDM_Odm->pMacInfo = (struct odm_mac_status_info *)pValue; */
+	/* break; */
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+void ODM_CmnInfoPtrArrayHook(
+	struct dm_odm_t *pDM_Odm,
+	enum odm_cmninfo_e CmnInfo,
+	u16 Index,
+	void *pValue
+)
+{
+	/*  */
+	/*  Hook call by reference pointer. */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Dynamic call by reference pointer. */
+	/*  */
+	case ODM_CMNINFO_STA_STATUS:
+		pDM_Odm->pODM_StaInfo[Index] = (PSTA_INFO_T)pValue;
+		break;
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+/*  */
+/*  Update Band/CHannel/.. The values are dynamic but non-per-packet. */
+/*  */
+void ODM_CmnInfoUpdate(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value)
+{
+	/*  */
+	/*  This init variable may be changed in run time. */
+	/*  */
+	switch (CmnInfo) {
+	case ODM_CMNINFO_LINK_IN_PROGRESS:
+		pDM_Odm->bLinkInProcess = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_ABILITY:
+		pDM_Odm->SupportAbility = (u32)Value;
+		break;
+
+	case ODM_CMNINFO_WIFI_DIRECT:
+		pDM_Odm->bWIFI_Direct = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_WIFI_DISPLAY:
+		pDM_Odm->bWIFI_Display = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_LINK:
+		pDM_Odm->bLinked = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_STATION_STATE:
+		pDM_Odm->bsta_state = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_RSSI_MIN:
+		pDM_Odm->RSSI_Min = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_RA_THRESHOLD_HIGH:
+		pDM_Odm->RateAdaptive.HighRSSIThresh = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_RA_THRESHOLD_LOW:
+		pDM_Odm->RateAdaptive.LowRSSIThresh = (u8)Value;
+		break;
+	/*  The following is for BT HS mode and BT coexist mechanism. */
+	case ODM_CMNINFO_BT_ENABLED:
+		pDM_Odm->bBtEnabled = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_HS_CONNECT_PROCESS:
+		pDM_Odm->bBtConnectProcess = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_HS_RSSI:
+		pDM_Odm->btHsRssi = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_BT_OPERATION:
+		pDM_Odm->bBtHsOperation = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_LIMITED_DIG:
+		pDM_Odm->bBtLimitedDig = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_DISABLE_EDCA:
+		pDM_Odm->bBtDisableEdcaTurbo = (bool)Value;
+		break;
+
+/*
+	case	ODM_CMNINFO_OP_MODE:
+		pDM_Odm->OPMode = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_WM_MODE:
+		pDM_Odm->WirelessMode = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_SEC_CHNL_OFFSET:
+		pDM_Odm->SecChOffset = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_SEC_MODE:
+		pDM_Odm->Security = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_BW:
+		pDM_Odm->BandWidth = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_CHNL:
+		pDM_Odm->Channel = (u8)Value;
+		break;
+*/
+	default:
+		/* do nothing */
+		break;
+	}
+
+
+}
+
+/* 3 ============================================================ */
+/* 3 DIG */
+/* 3 ============================================================ */
+/*-----------------------------------------------------------------------------
+ * Function:	odm_DIGInit()
+ *
+ * Overview:	Set DIG scheme init value.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *When		Who		Remark
+ *
+ *---------------------------------------------------------------------------
+ */
+
+/* Remove DIG by yuchen */
+
+/* Remove DIG and FA check by Yu Chen */
+
+/* 3 ============================================================ */
+/* 3 BB Power Save */
+/* 3 ============================================================ */
+
+/* Remove BB power saving by Yuchen */
+
+/* 3 ============================================================ */
+/* 3 Dynamic Tx Power */
+/* 3 ============================================================ */
+
+/* Remove BY YuChen */
+
diff --git a/drivers/staging/rtl8723bs/hal/odm.h b/drivers/staging/rtl8723bs/hal/odm.h
new file mode 100644
index 000000000..fe9782d2d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm.h
@@ -0,0 +1,1175 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+
+#ifndef	__HALDMOUTSRC_H__
+#define __HALDMOUTSRC_H__
+
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+
+#define	TP_MODE		0
+#define	RSSI_MODE		1
+#define	TRAFFIC_LOW	0
+#define	TRAFFIC_HIGH	1
+#define	NONE			0
+
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+#define		DPK_DELTA_MAPPING_NUM	13
+#define		index_mapping_HP_NUM	15
+#define	OFDM_TABLE_SIZE		43
+#define	CCK_TABLE_SIZE			33
+#define TXSCALE_TABLE_SIZE		37
+#define TXPWR_TRACK_TABLE_SIZE	30
+#define DELTA_SWINGIDX_SIZE     30
+#define BAND_NUM				4
+
+/* 3 PSD Handler */
+/* 3 ============================================================ */
+
+#define	AFH_PSD		1	/* 0:normal PSD scan, 1: only do 20 pts PSD */
+#define	MODE_40M		0	/* 0:20M, 1:40M */
+#define	PSD_TH2		3
+#define	PSD_CHMIN		20   /*  Minimum channel number for BT AFH */
+#define	SIR_STEP_SIZE	3
+#define   Smooth_Size_1		5
+#define	Smooth_TH_1	3
+#define   Smooth_Size_2		10
+#define	Smooth_TH_2	4
+#define   Smooth_Size_3		20
+#define	Smooth_TH_3	4
+#define   Smooth_Step_Size 5
+#define	Adaptive_SIR	1
+#define	PSD_RESCAN		4
+#define	PSD_SCAN_INTERVAL	700 /* ms */
+
+/* 8723A High Power IGI Setting */
+#define		DM_DIG_HIGH_PWR_IGI_LOWER_BOUND	0x22
+#define			DM_DIG_Gmode_HIGH_PWR_IGI_LOWER_BOUND 0x28
+#define		DM_DIG_HIGH_PWR_THRESHOLD	0x3a
+#define		DM_DIG_LOW_PWR_THRESHOLD	0x14
+
+/* ANT Test */
+#define			ANTTESTALL		0x00		/* Ant A or B will be Testing */
+#define		ANTTESTA		0x01		/* Ant A will be Testing */
+#define		ANTTESTB		0x02		/* Ant B will be testing */
+
+#define	PS_MODE_ACTIVE 0x01
+
+/* for 8723A Ant Definition--2012--06--07 due to different IC may be different ANT define */
+#define		MAIN_ANT		1		/* Ant A or Ant Main */
+#define		AUX_ANT		2		/* AntB or Ant Aux */
+#define		MAX_ANT		3		/*  3 for AP using */
+
+/* Antenna Diversity Type */
+#define	SW_ANTDIV	0
+#define	HW_ANTDIV	1
+/*  structure and define */
+
+/* Remove DIG by Yuchen */
+
+/* Remoce BB power saving by Yuchn */
+
+/* Remove DIG by yuchen */
+
+struct dynamic_primary_CCA {
+	u8 PriCCA_flag;
+	u8 intf_flag;
+	u8 intf_type;
+	u8 DupRTS_flag;
+	u8 Monitor_flag;
+	u8 CH_offset;
+	u8 MF_state;
+};
+
+struct ra_t {
+	u8 firstconnect;
+};
+
+struct rxhp_t {
+	u8 RXHP_flag;
+	u8 PSD_func_trigger;
+	u8 PSD_bitmap_RXHP[80];
+	u8 Pre_IGI;
+	u8 Cur_IGI;
+	u8 Pre_pw_th;
+	u8 Cur_pw_th;
+	bool First_time_enter;
+	bool RXHP_enable;
+	u8 TP_Mode;
+	struct timer_list PSDTimer;
+};
+
+#define ASSOCIATE_ENTRY_NUM					32 /*  Max size of AsocEntry[]. */
+#define	ODM_ASSOCIATE_ENTRY_NUM				ASSOCIATE_ENTRY_NUM
+
+/*  This indicates two different the steps. */
+/*  In SWAW_STEP_PEAK, driver needs to switch antenna and listen to the signal on the air. */
+/*  In SWAW_STEP_DETERMINE, driver just compares the signal captured in SWAW_STEP_PEAK */
+/*  with original RSSI to determine if it is necessary to switch antenna. */
+#define SWAW_STEP_PEAK		0
+#define SWAW_STEP_DETERMINE	1
+
+#define	TP_MODE		0
+#define	RSSI_MODE		1
+#define	TRAFFIC_LOW	0
+#define	TRAFFIC_HIGH	1
+#define	TRAFFIC_UltraLOW	2
+
+struct swat_t { /* _SW_Antenna_Switch_ */
+	u8 Double_chk_flag;
+	u8 try_flag;
+	s32 PreRSSI;
+	u8 CurAntenna;
+	u8 PreAntenna;
+	u8 RSSI_Trying;
+	u8 TestMode;
+	u8 bTriggerAntennaSwitch;
+	u8 SelectAntennaMap;
+	u8 RSSI_target;
+	u8 reset_idx;
+	u16 Single_Ant_Counter;
+	u16 Dual_Ant_Counter;
+	u16 Aux_FailDetec_Counter;
+	u16 Retry_Counter;
+
+	/*  Before link Antenna Switch check */
+	u8 SWAS_NoLink_State;
+	u32 SWAS_NoLink_BK_Reg860;
+	u32 SWAS_NoLink_BK_Reg92c;
+	u32 SWAS_NoLink_BK_Reg948;
+	bool ANTA_ON;	/* To indicate Ant A is or not */
+	bool ANTB_ON;	/* To indicate Ant B is on or not */
+	bool Pre_Aux_FailDetec;
+	bool RSSI_AntDect_bResult;
+	u8 Ant2G;
+
+	s32 RSSI_sum_A;
+	s32 RSSI_sum_B;
+	s32 RSSI_cnt_A;
+	s32 RSSI_cnt_B;
+
+	u64 lastTxOkCnt;
+	u64 lastRxOkCnt;
+	u64 TXByteCnt_A;
+	u64 TXByteCnt_B;
+	u64 RXByteCnt_A;
+	u64 RXByteCnt_B;
+	u8 TrafficLoad;
+	u8 Train_time;
+	u8 Train_time_flag;
+	struct timer_list SwAntennaSwitchTimer;
+	struct timer_list SwAntennaSwitchTimer_8723B;
+	u32 PktCnt_SWAntDivByCtrlFrame;
+	bool bSWAntDivByCtrlFrame;
+};
+
+/* Remove Edca by YuChen */
+
+
+struct odm_rate_adaptive {
+	u8 Type;				/*  DM_Type_ByFW/DM_Type_ByDriver */
+	u8 LdpcThres;			/*  if RSSI > LdpcThres => switch from LPDC to BCC */
+	bool bUseLdpc;
+	bool bLowerRtsRate;
+	u8 HighRSSIThresh;		/*  if RSSI > HighRSSIThresh	=> RATRState is DM_RATR_STA_HIGH */
+	u8 LowRSSIThresh;		/*  if RSSI <= LowRSSIThresh	=> RATRState is DM_RATR_STA_LOW */
+	u8 RATRState;			/*  Current RSSI level, DM_RATR_STA_HIGH/DM_RATR_STA_MIDDLE/DM_RATR_STA_LOW */
+
+};
+
+#define IQK_MAC_REG_NUM		4
+#define IQK_ADDA_REG_NUM		16
+#define IQK_BB_REG_NUM_MAX	10
+#define IQK_BB_REG_NUM		9
+#define HP_THERMAL_NUM		8
+
+#define AVG_THERMAL_NUM		8
+#define IQK_Matrix_REG_NUM	8
+#define IQK_Matrix_Settings_NUM	14 /* Channels_2_4G_NUM */
+
+#define		DM_Type_ByFW			0
+#define		DM_Type_ByDriver		1
+
+/*  */
+/*  Declare for common info */
+/*  */
+#define MAX_PATH_NUM_92CS		2
+#define MAX_PATH_NUM_8188E		1
+#define MAX_PATH_NUM_8192E		2
+#define MAX_PATH_NUM_8723B		1
+#define MAX_PATH_NUM_8812A		2
+#define MAX_PATH_NUM_8821A		1
+#define MAX_PATH_NUM_8814A		4
+#define MAX_PATH_NUM_8822B		2
+
+#define IQK_THRESHOLD			8
+#define DPK_THRESHOLD			4
+
+struct odm_phy_info {
+	/*
+	 *  Be care, if you want to add any element, please insert it between
+	 *  rx_pwd_ball and signal_strength.
+	 */
+	u8 rx_pwd_ba11;
+
+	u8 signal_quality;             /* in 0-100 index. */
+	s8 rx_mimo_signal_quality[4];  /* per-path's EVM */
+	u8 rx_mimo_evm_dbm[4];         /* per-path's EVM dbm */
+
+	u8 rx_mimo_signal_strength[4]; /* in 0~100 index */
+
+	u16 cfo_short[4];              /* per-path's Cfo_short */
+	u16 cfo_tail[4];               /* per-path's Cfo_tail */
+
+	s8 rx_power;                   /* in dBm Translate from PWdB */
+
+	/*
+	 * Real power in dBm for this packet, no beautification and
+	 * aggregation. Keep this raw info to be used for the other procedures.
+	 */
+	s8 recv_signal_power;
+	u8 bt_rx_rssi_percentage;
+	u8 signal_strength;	       /* in 0-100 index. */
+
+	s8 rx_pwr[4];                  /* per-path's pwdb */
+
+	u8 rx_snr[4];                  /* per-path's SNR */
+	u8 band_width;
+	u8 bt_coex_pwr_adjust;
+};
+
+struct odm_packet_info {
+	u8 data_rate;
+	u8 station_id;
+	bool bssid_match;
+	bool to_self;
+	bool is_beacon;
+};
+
+struct odm_phy_dbg_info {
+	/* ODM Write, debug info */
+	s8 RxSNRdB[4];
+	u32 NumQryPhyStatus;
+	u32 NumQryPhyStatusCCK;
+	u32 NumQryPhyStatusOFDM;
+	u8 NumQryBeaconPkt;
+	/* Others */
+	s32 RxEVM[4];
+
+};
+
+struct odm_mac_status_info {
+	u8 test;
+};
+
+/*  */
+/*  2011/10/20 MH Define Common info enum for all team. */
+/*  */
+enum odm_cmninfo_e {
+	/*  Fixed value: */
+
+	/* HOOK BEFORE REG INIT----------- */
+	ODM_CMNINFO_PLATFORM = 0,
+	ODM_CMNINFO_ABILITY,					/*  ODM_ABILITY_E */
+	ODM_CMNINFO_INTERFACE,				/*  ODM_INTERFACE_E */
+	ODM_CMNINFO_IC_TYPE,					/*  ODM_IC_TYPE_E */
+	ODM_CMNINFO_CUT_VER,					/*  ODM_CUT_VERSION_E */
+	ODM_CMNINFO_FAB_VER,					/*  ODM_FAB_E */
+	ODM_CMNINFO_RFE_TYPE,
+	ODM_CMNINFO_PACKAGE_TYPE,
+	ODM_CMNINFO_EXT_LNA,					/*  true */
+	ODM_CMNINFO_EXT_PA,
+	ODM_CMNINFO_GPA,
+	ODM_CMNINFO_APA,
+	ODM_CMNINFO_GLNA,
+	ODM_CMNINFO_ALNA,
+	ODM_CMNINFO_EXT_TRSW,
+	ODM_CMNINFO_PATCH_ID,				/* CUSTOMER ID */
+	ODM_CMNINFO_BINHCT_TEST,
+	ODM_CMNINFO_BWIFI_TEST,
+	ODM_CMNINFO_SMART_CONCURRENT,
+	/* HOOK BEFORE REG INIT----------- */
+
+	/*  Dynamic value: */
+/*  POINTER REFERENCE----------- */
+	ODM_CMNINFO_MAC_PHY_MODE,	/*  ODM_MAC_PHY_MODE_E */
+	ODM_CMNINFO_TX_UNI,
+	ODM_CMNINFO_RX_UNI,
+	ODM_CMNINFO_WM_MODE,		/*  ODM_WIRELESS_MODE_E */
+	ODM_CMNINFO_SEC_CHNL_OFFSET,	/*  ODM_SEC_CHNL_OFFSET_E */
+	ODM_CMNINFO_SEC_MODE,		/*  ODM_SECURITY_E */
+	ODM_CMNINFO_BW,			/*  ODM_BW_E */
+	ODM_CMNINFO_CHNL,
+	ODM_CMNINFO_FORCED_RATE,
+
+	ODM_CMNINFO_DMSP_GET_VALUE,
+	ODM_CMNINFO_BUDDY_ADAPTOR,
+	ODM_CMNINFO_DMSP_IS_MASTER,
+	ODM_CMNINFO_SCAN,
+	ODM_CMNINFO_POWER_SAVING,
+	ODM_CMNINFO_ONE_PATH_CCA,	/*  ODM_CCA_PATH_E */
+	ODM_CMNINFO_DRV_STOP,
+	ODM_CMNINFO_PNP_IN,
+	ODM_CMNINFO_INIT_ON,
+	ODM_CMNINFO_ANT_TEST,
+	ODM_CMNINFO_NET_CLOSED,
+	ODM_CMNINFO_MP_MODE,
+	/* ODM_CMNINFO_RTSTA_AID,	 For win driver only? */
+	ODM_CMNINFO_FORCED_IGI_LB,
+	ODM_CMNINFO_IS1ANTENNA,
+	ODM_CMNINFO_RFDEFAULTPATH,
+/*  POINTER REFERENCE----------- */
+
+/* CALL BY VALUE------------- */
+	ODM_CMNINFO_WIFI_DIRECT,
+	ODM_CMNINFO_WIFI_DISPLAY,
+	ODM_CMNINFO_LINK_IN_PROGRESS,
+	ODM_CMNINFO_LINK,
+	ODM_CMNINFO_STATION_STATE,
+	ODM_CMNINFO_RSSI_MIN,
+	ODM_CMNINFO_DBG_COMP,			/*  u64 */
+	ODM_CMNINFO_DBG_LEVEL,			/*  u32 */
+	ODM_CMNINFO_RA_THRESHOLD_HIGH,		/*  u8 */
+	ODM_CMNINFO_RA_THRESHOLD_LOW,		/*  u8 */
+	ODM_CMNINFO_RF_ANTENNA_TYPE,		/*  u8 */
+	ODM_CMNINFO_BT_ENABLED,
+	ODM_CMNINFO_BT_HS_CONNECT_PROCESS,
+	ODM_CMNINFO_BT_HS_RSSI,
+	ODM_CMNINFO_BT_OPERATION,
+	ODM_CMNINFO_BT_LIMITED_DIG,		/* Need to Limited Dig or not */
+	ODM_CMNINFO_BT_DISABLE_EDCA,
+/* CALL BY VALUE------------- */
+
+	/*  Dynamic ptr array hook itms. */
+	ODM_CMNINFO_STA_STATUS,
+	ODM_CMNINFO_PHY_STATUS,
+	ODM_CMNINFO_MAC_STATUS,
+
+	ODM_CMNINFO_MAX,
+};
+
+/*  2011/10/20 MH Define ODM support ability.  ODM_CMNINFO_ABILITY */
+enum { /* _ODM_Support_Ability_Definition */
+	/*  */
+	/*  BB ODM section BIT 0-15 */
+	/*  */
+	ODM_BB_DIG			= BIT0,
+	ODM_BB_RA_MASK			= BIT1,
+	ODM_BB_DYNAMIC_TXPWR		= BIT2,
+	ODM_BB_FA_CNT			= BIT3,
+	ODM_BB_RSSI_MONITOR		= BIT4,
+	ODM_BB_CCK_PD			= BIT5,
+	ODM_BB_ANT_DIV			= BIT6,
+	ODM_BB_PWR_SAVE			= BIT7,
+	ODM_BB_PWR_TRAIN		= BIT8,
+	ODM_BB_RATE_ADAPTIVE		= BIT9,
+	ODM_BB_PATH_DIV			= BIT10,
+	ODM_BB_PSD			= BIT11,
+	ODM_BB_RXHP			= BIT12,
+	ODM_BB_ADAPTIVITY		= BIT13,
+	ODM_BB_CFO_TRACKING		= BIT14,
+
+	/*  MAC DM section BIT 16-23 */
+	ODM_MAC_EDCA_TURBO		= BIT16,
+	ODM_MAC_EARLY_MODE		= BIT17,
+
+	/*  RF ODM section BIT 24-31 */
+	ODM_RF_TX_PWR_TRACK		= BIT24,
+	ODM_RF_RX_GAIN_TRACK	= BIT25,
+	ODM_RF_CALIBRATION		= BIT26,
+};
+
+/* 	ODM_CMNINFO_INTERFACE */
+enum { /* tag_ODM_Support_Interface_Definition */
+	ODM_ITRF_SDIO	=	0x4,
+	ODM_ITRF_ALL	=	0x7,
+};
+
+/*  ODM_CMNINFO_IC_TYPE */
+enum { /* tag_ODM_Support_IC_Type_Definition */
+	ODM_RTL8723B	=	BIT8,
+};
+
+/* ODM_CMNINFO_CUT_VER */
+enum { /* tag_ODM_Cut_Version_Definition */
+	ODM_CUT_A		=	0,
+	ODM_CUT_B		=	1,
+	ODM_CUT_C		=	2,
+	ODM_CUT_D		=	3,
+	ODM_CUT_E		=	4,
+	ODM_CUT_F		=	5,
+
+	ODM_CUT_I		=	8,
+	ODM_CUT_J		=	9,
+	ODM_CUT_K		=	10,
+	ODM_CUT_TEST	=	15,
+};
+
+/*  ODM_CMNINFO_FAB_VER */
+enum { /* tag_ODM_Fab_Version_Definition */
+	ODM_TSMC	=	0,
+	ODM_UMC		=	1,
+};
+
+/*  */
+/*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
+/*  */
+enum { /* tag_ODM_RF_Type_Definition */
+	ODM_1T1R	=	0,
+	ODM_1T2R	=	1,
+	ODM_2T2R	=	2,
+	ODM_2T3R	=	3,
+	ODM_2T4R	=	4,
+	ODM_3T3R	=	5,
+	ODM_3T4R	=	6,
+	ODM_4T4R	=	7,
+};
+
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+/*  ODM_CMNINFO_WM_MODE */
+enum { /* tag_Wireless_Mode_Definition */
+	ODM_WM_UNKNOWN    = 0x0,
+	ODM_WM_B          = BIT0,
+	ODM_WM_G          = BIT1,
+	ODM_WM_N24G       = BIT3,
+	ODM_WM_AUTO       = BIT5,
+};
+
+/*  ODM_CMNINFO_BW */
+enum { /* tag_Bandwidth_Definition */
+	ODM_BW20M		= 0,
+	ODM_BW40M		= 1,
+};
+
+/*  For AC-series IC, external PA & LNA can be individually added on 2.4G */
+
+enum odm_type_gpa_e { /* tag_ODM_TYPE_GPA_Definition */
+	TYPE_GPA0 = 0,
+	TYPE_GPA1 = BIT(1)|BIT(0)
+};
+
+enum odm_type_apa_e { /* tag_ODM_TYPE_APA_Definition */
+	TYPE_APA0 = 0,
+	TYPE_APA1 = BIT(1)|BIT(0)
+};
+
+enum odm_type_glna_e { /* tag_ODM_TYPE_GLNA_Definition */
+	TYPE_GLNA0 = 0,
+	TYPE_GLNA1 = BIT(2)|BIT(0),
+	TYPE_GLNA2 = BIT(3)|BIT(1),
+	TYPE_GLNA3 = BIT(3)|BIT(2)|BIT(1)|BIT(0)
+};
+
+enum odm_type_alna_e { /* tag_ODM_TYPE_ALNA_Definition */
+	TYPE_ALNA0 = 0,
+	TYPE_ALNA1 = BIT(2)|BIT(0),
+	TYPE_ALNA2 = BIT(3)|BIT(1),
+	TYPE_ALNA3 = BIT(3)|BIT(2)|BIT(1)|BIT(0)
+};
+
+struct iqk_matrix_regs_setting { /* _IQK_MATRIX_REGS_SETTING */
+	bool bIQKDone;
+	s32 Value[3][IQK_Matrix_REG_NUM];
+	bool bBWIqkResultSaved[3];
+};
+
+/* Remove PATHDIV_PARA struct to odm_PathDiv.h */
+
+struct odm_rf_cal_t { /* ODM_RF_Calibration_Structure */
+	/* for tx power tracking */
+
+	u32 RegA24; /*  for TempCCK */
+	s32 RegE94;
+	s32 RegE9C;
+	s32 RegEB4;
+	s32 RegEBC;
+
+	u8 TXPowercount;
+	bool bTXPowerTrackingInit;
+	bool bTXPowerTracking;
+	u8 TxPowerTrackControl; /* for mp mode, turn off txpwrtracking as default */
+	u8 TM_Trigger;
+
+	u8 ThermalMeter[2];    /*  ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
+	u8 ThermalValue;
+	u8 ThermalValue_LCK;
+	u8 ThermalValue_IQK;
+	u8 ThermalValue_DPK;
+	u8 ThermalValue_AVG[AVG_THERMAL_NUM];
+	u8 ThermalValue_AVG_index;
+	u8 ThermalValue_RxGain;
+	u8 ThermalValue_Crystal;
+	u8 ThermalValue_DPKstore;
+	u8 ThermalValue_DPKtrack;
+	bool TxPowerTrackingInProgress;
+
+	bool bReloadtxpowerindex;
+	u8 bRfPiEnable;
+	u32 TXPowerTrackingCallbackCnt; /* cosa add for debug */
+
+	/*  Tx power Tracking ------------------------- */
+	u8 bCCKinCH14;
+	u8 CCK_index;
+	u8 OFDM_index[MAX_RF_PATH];
+	s8 PowerIndexOffset[MAX_RF_PATH];
+	s8 DeltaPowerIndex[MAX_RF_PATH];
+	s8 DeltaPowerIndexLast[MAX_RF_PATH];
+	bool bTxPowerChanged;
+
+	u8 ThermalValue_HP[HP_THERMAL_NUM];
+	u8 ThermalValue_HP_index;
+	struct iqk_matrix_regs_setting IQKMatrixRegSetting[IQK_Matrix_Settings_NUM];
+	bool bNeedIQK;
+	bool bIQKInProgress;
+	u8 Delta_IQK;
+	u8 Delta_LCK;
+	s8 BBSwingDiff2G; /*  Unit: dB */
+	u8 DeltaSwingTableIdx_2GCCKA_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKA_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKB_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKB_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GB_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GB_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_P_8188E[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_N_8188E[DELTA_SWINGIDX_SIZE];
+
+	/*  */
+
+	/* for IQK */
+	u32 RegC04;
+	u32 Reg874;
+	u32 RegC08;
+	u32 RegB68;
+	u32 RegB6C;
+	u32 Reg870;
+	u32 Reg860;
+	u32 Reg864;
+
+	bool bIQKInitialized;
+	bool bLCKInProgress;
+	bool bAntennaDetected;
+	u32 ADDA_backup[IQK_ADDA_REG_NUM];
+	u32 IQK_MAC_backup[IQK_MAC_REG_NUM];
+	u32 IQK_BB_backup_recover[9];
+	u32 IQK_BB_backup[IQK_BB_REG_NUM];
+	u32 TxIQC_8723B[2][3][2]; /*  { {S1: 0xc94, 0xc80, 0xc4c} , {S0: 0xc9c, 0xc88, 0xc4c}} */
+	u32 RxIQC_8723B[2][2][2]; /*  { {S1: 0xc14, 0xca0} ,           {S0: 0xc14, 0xca0}} */
+
+	/* for APK */
+	u32 APKoutput[2][2]; /* path A/B; output1_1a/output1_2a */
+	u8 bAPKdone;
+	u8 bAPKThermalMeterIgnore;
+
+	/*  DPK */
+	bool bDPKFail;
+	u8 bDPdone;
+	u8 bDPPathAOK;
+	u8 bDPPathBOK;
+
+	u32 TxLOK[2];
+
+};
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+struct fat_t { /* _FAST_ANTENNA_TRAINNING_ */
+	u8 Bssid[6];
+	u8 antsel_rx_keep_0;
+	u8 antsel_rx_keep_1;
+	u8 antsel_rx_keep_2;
+	u8 antsel_rx_keep_3;
+	u32 antSumRSSI[7];
+	u32 antRSSIcnt[7];
+	u32 antAveRSSI[7];
+	u8 FAT_State;
+	u32 TrainIdx;
+	u8 antsel_a[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 antsel_b[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 antsel_c[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 MainAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 AuxAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 MainAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 AuxAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 RxIdleAnt;
+	bool	bBecomeLinked;
+	u32 MinMaxRSSI;
+	u8 idx_AntDiv_counter_2G;
+	u32 CCK_counter_main;
+	u32 CCK_counter_aux;
+	u32 OFDM_counter_main;
+	u32 OFDM_counter_aux;
+
+	u32 CCK_CtrlFrame_Cnt_main;
+	u32 CCK_CtrlFrame_Cnt_aux;
+	u32 OFDM_CtrlFrame_Cnt_main;
+	u32 OFDM_CtrlFrame_Cnt_aux;
+	u32 MainAnt_CtrlFrame_Sum;
+	u32 AuxAnt_CtrlFrame_Sum;
+	u32 MainAnt_CtrlFrame_Cnt;
+	u32 AuxAnt_CtrlFrame_Cnt;
+
+};
+
+enum {
+	NO_ANTDIV			= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV	= 0x02,
+	FIXED_HW_ANTDIV		= 0x03,
+	CG_TRX_SMART_ANTDIV	= 0x04,
+	CGCS_RX_SW_ANTDIV	= 0x05,
+	S0S1_SW_ANTDIV          = 0x06 /* 8723B intrnal switch S0 S1 */
+};
+
+struct pathdiv_t { /* _ODM_PATH_DIVERSITY_ */
+	u8 RespTxPath;
+	u8 PathSel[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathA_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathB_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathA_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathB_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+};
+
+enum phy_reg_pg_type { /* _BASEBAND_CONFIG_PHY_REG_PG_VALUE_TYPE */
+	PHY_REG_PG_RELATIVE_VALUE = 0,
+	PHY_REG_PG_EXACT_VALUE = 1
+};
+
+/*  */
+/*  Antenna detection information from single tone mechanism, added by Roger, 2012.11.27. */
+/*  */
+struct ant_detected_info {
+	bool bAntDetected;
+	u32 dBForAntA;
+	u32 dBForAntB;
+	u32 dBForAntO;
+};
+
+/*  */
+/*  2011/09/22 MH Copy from SD4 defined structure. We use to support PHY DM integration. */
+/*  */
+struct dm_odm_t { /* DM_Out_Source_Dynamic_Mechanism_Structure */
+	/* struct timer_list	FastAntTrainingTimer; */
+	/*  */
+	/* 	Add for different team use temporarily */
+	/*  */
+	struct adapter *Adapter;		/*  For CE/NIC team */
+	/*  WHen you use Adapter or priv pointer, you must make sure the pointer is ready. */
+	bool odm_ready;
+
+	enum phy_reg_pg_type PhyRegPgValueType;
+	u8 PhyRegPgVersion;
+
+	u32 NumQryPhyStatusAll;	/* CCK + OFDM */
+	u32 LastNumQryPhyStatusAll;
+	u32 RxPWDBAve;
+	bool MPDIG_2G;		/* off MPDIG */
+	u8 Times_2G;
+
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+	bool bCckHighPower;
+	u8 RFPathRxEnable;		/*  ODM_CMNINFO_RFPATH_ENABLE */
+	u8 ControlChannel;
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+
+/* REMOVED COMMON INFO---------- */
+	/* u8 		PseudoMacPhyMode; */
+	/* bool			*BTCoexist; */
+	/* bool			PseudoBtCoexist; */
+	/* u8 		OPMode; */
+	/* bool			bAPMode; */
+	/* bool			bClientMode; */
+	/* bool			bAdHocMode; */
+	/* bool			bSlaveOfDMSP; */
+/* REMOVED COMMON INFO---------- */
+
+/* 1  COMMON INFORMATION */
+
+	/*  */
+	/*  Init Value */
+	/*  */
+/* HOOK BEFORE REG INIT----------- */
+	/*  ODM Platform info AP/ADSL/CE/MP = 1/2/3/4 */
+	u8 SupportPlatform;
+	/*  ODM Support Ability DIG/RATR/TX_PWR_TRACK/... = 1/2/3/... */
+	u32 SupportAbility;
+	/*  ODM PCIE/USB/SDIO = 1/2/3 */
+	u8 SupportInterface;
+	/*  ODM composite or independent. Bit oriented/ 92C+92D+ .... or any other type = 1/2/3/... */
+	u32 SupportICType;
+	/*  Cut Version TestChip/A-cut/B-cut... = 0/1/2/3/... */
+	u8 CutVersion;
+	/*  Fab Version TSMC/UMC = 0/1 */
+	u8 FabVersion;
+	/*  RF Type 4T4R/3T3R/2T2R/1T2R/1T1R/... */
+	u8 RFEType;
+	/*  Board Type Normal/HighPower/MiniCard/SLIM/Combo/... = 0/1/2/3/4/... */
+	u8 BoardType;
+	u8 PackageType;
+	u8 TypeGLNA;
+	u8 TypeGPA;
+	u8 TypeALNA;
+	u8 TypeAPA;
+	/*  with external LNA  NO/Yes = 0/1 */
+	u8 ExtLNA;
+	/*  with external PA  NO/Yes = 0/1 */
+	u8 ExtPA;
+	/*  with external TRSW  NO/Yes = 0/1 */
+	u8 ExtTRSW;
+	u8 PatchID; /* Customer ID */
+	bool bInHctTest;
+	bool bWIFITest;
+
+	bool bDualMacSmartConcurrent;
+	u32 BK_SupportAbility;
+	u8 AntDivType;
+/* HOOK BEFORE REG INIT----------- */
+
+	/*  */
+	/*  Dynamic Value */
+	/*  */
+/*  POINTER REFERENCE----------- */
+
+	u8 u8_temp;
+	bool bool_temp;
+	struct adapter *adapter_temp;
+
+	/*  MAC PHY Mode SMSP/DMSP/DMDP = 0/1/2 */
+	u8 *pMacPhyMode;
+	/* TX Unicast byte count */
+	u64 *pNumTxBytesUnicast;
+	/* RX Unicast byte count */
+	u64 *pNumRxBytesUnicast;
+	/*  Wireless mode B/G/A/N = BIT0/BIT1/BIT2/BIT3 */
+	u8 *pwirelessmode; /* ODM_WIRELESS_MODE_E */
+	/*  Secondary channel offset don't_care/below/above = 0/1/2 */
+	u8 *pSecChOffset;
+	/*  Security mode Open/WEP/AES/TKIP = 0/1/2/3 */
+	u8 *pSecurity;
+	/*  BW info 20M/40M/80M = 0/1/2 */
+	u8 *pBandWidth;
+	/*  Central channel location Ch1/Ch2/.... */
+	u8 *pChannel; /* central channel number */
+	bool DPK_Done;
+	/*  Common info for 92D DMSP */
+
+	bool *pbGetValueFromOtherMac;
+	struct adapter **pBuddyAdapter;
+	bool *pbMasterOfDMSP; /* MAC0: master, MAC1: slave */
+	/*  Common info for Status */
+	bool *pbScanInProcess;
+	bool *pbPowerSaving;
+	/*  CCA Path 2-path/path-A/path-B = 0/1/2; using ODM_CCA_PATH_E. */
+	u8 *pOnePathCCA;
+	/* pMgntInfo->AntennaTest */
+	u8 *pAntennaTest;
+	bool *pbNet_closed;
+	u8 *mp_mode;
+	/* u8 	*pAidMap; */
+	u8 *pu1ForcedIgiLb;
+/*  For 8723B IQK----------- */
+	bool *pIs1Antenna;
+	u8 *pRFDefaultPath;
+	/*  0:S1, 1:S0 */
+
+/*  POINTER REFERENCE----------- */
+	u16 *pForcedDataRate;
+/* CALL BY VALUE------------- */
+	bool bLinkInProcess;
+	bool bWIFI_Direct;
+	bool bWIFI_Display;
+	bool bLinked;
+
+	bool bsta_state;
+	u8 RSSI_Min;
+	u8 InterfaceIndex; /*  Add for 92D  dual MAC: 0--Mac0 1--Mac1 */
+	bool bOneEntryOnly;
+	/*  Common info for BTDM */
+	bool bBtEnabled;			/*  BT is disabled */
+	bool bBtConnectProcess;	/*  BT HS is under connection progress. */
+	u8 btHsRssi;				/*  BT HS mode wifi rssi value. */
+	bool bBtHsOperation;		/*  BT HS mode is under progress */
+	bool bBtDisableEdcaTurbo;	/*  Under some condition, don't enable the EDCA Turbo */
+	bool bBtLimitedDig;			/*  BT is busy. */
+/* CALL BY VALUE------------- */
+	u8 RSSI_A;
+	u8 RSSI_B;
+	u64 RSSI_TRSW;
+	u64 RSSI_TRSW_H;
+	u64 RSSI_TRSW_L;
+	u64 RSSI_TRSW_iso;
+
+	u8 RxRate;
+	bool bNoisyState;
+	u8 TxRate;
+	u8 LinkedInterval;
+	u8 preChannel;
+	u32 TxagcOffsetValueA;
+	bool IsTxagcOffsetPositiveA;
+	u32 TxagcOffsetValueB;
+	bool IsTxagcOffsetPositiveB;
+	u64	lastTxOkCnt;
+	u64	lastRxOkCnt;
+	u32 BbSwingOffsetA;
+	bool IsBbSwingOffsetPositiveA;
+	u32 BbSwingOffsetB;
+	bool IsBbSwingOffsetPositiveB;
+	s8 TH_L2H_ini;
+	s8 TH_EDCCA_HL_diff;
+	s8 IGI_Base;
+	u8 IGI_target;
+	bool ForceEDCCA;
+	u8 AdapEn_RSSI;
+	s8 Force_TH_H;
+	s8 Force_TH_L;
+	u8 IGI_LowerBound;
+	u8 antdiv_rssi;
+	u8 AntType;
+	u8 pre_AntType;
+	u8 antdiv_period;
+	u8 antdiv_select;
+	u8 NdpaPeriod;
+	bool H2C_RARpt_connect;
+
+	/*  add by Yu Cehn for adaptivtiy */
+	bool adaptivity_flag;
+	bool NHM_disable;
+	bool TxHangFlg;
+	bool Carrier_Sense_enable;
+	u8 tolerance_cnt;
+	u64 NHMCurTxOkcnt;
+	u64 NHMCurRxOkcnt;
+	u64 NHMLastTxOkcnt;
+	u64 NHMLastRxOkcnt;
+	u8 txEdcca1;
+	u8 txEdcca0;
+	s8 H2L_lb;
+	s8 L2H_lb;
+	u8 Adaptivity_IGI_upper;
+	u8 NHM_cnt_0;
+
+	/*  */
+	/* 2 Define STA info. */
+	/*  _ODM_STA_INFO */
+	/*  2012/01/12 MH For MP, we need to reduce one array pointer for default port.?? */
+	PSTA_INFO_T pODM_StaInfo[ODM_ASSOCIATE_ENTRY_NUM];
+
+	/*  */
+	/*  2012/02/14 MH Add to share 88E ra with other SW team. */
+	/*  We need to colelct all support abilit to a proper area. */
+	/*  */
+	bool RaSupport88E;
+
+	/*  Define ........... */
+
+	/*  Latest packet phy info (ODM write) */
+	struct odm_phy_dbg_info PhyDbgInfo;
+	/* PHY_INFO_88E		PhyInfo; */
+
+	/*  Latest packet phy info (ODM write) */
+	struct odm_mac_status_info *pMacInfo;
+	/* MAC_INFO_88E		MacInfo; */
+
+	/*  Different Team independt structure?? */
+
+	/*  */
+	/* TX_RTP_CMN		TX_retrpo; */
+	/* TX_RTP_88E		TX_retrpo; */
+	/* TX_RTP_8195		TX_retrpo; */
+
+	/*  */
+	/* ODM Structure */
+	/*  */
+	struct fat_t DM_FatTable;
+	struct dig_t DM_DigTable;
+	struct ps_t DM_PSTable;
+	struct dynamic_primary_CCA DM_PriCCA;
+	struct rxhp_t dM_RXHP_Table;
+	struct ra_t DM_RA_Table;
+	struct false_ALARM_STATISTICS FalseAlmCnt;
+	struct false_ALARM_STATISTICS FlaseAlmCntBuddyAdapter;
+	struct swat_t DM_SWAT_Table;
+	bool RSSI_test;
+	struct cfo_tracking DM_CfoTrack;
+
+	struct edca_t DM_EDCA_Table;
+	u32 WMMEDCA_BE;
+	struct pathdiv_t DM_PathDiv;
+	/*  Copy from SD4 structure */
+	/*  */
+	/*  ================================================== */
+	/*  */
+
+	/* common */
+	/* u8 DM_Type; */
+	/* u8    PSD_Report_RXHP[80];    Add By Gary */
+	/* u8    PSD_func_flag;                Add By Gary */
+	/* for DIG */
+	/* u8 bDMInitialGainEnable; */
+	/* u8 binitialized;  for dm_initial_gain_Multi_STA use. */
+	/* for Antenna diversity */
+	/* u8 AntDivCfg; 0:OFF , 1:ON, 2:by efuse */
+	/* PSTA_INFO_T RSSI_target; */
+
+	bool *pbDriverStopped;
+	bool *pbDriverIsGoingToPnpSetPowerSleep;
+	bool *pinit_adpt_in_progress;
+
+	/* PSD */
+	bool bUserAssignLevel;
+	struct timer_list PSDTimer;
+	u8 RSSI_BT;			/* come from BT */
+	bool bPSDinProcess;
+	bool bPSDactive;
+	bool bDMInitialGainEnable;
+
+	/* MPT DIG */
+	struct timer_list MPT_DIGTimer;
+
+	/* for rate adaptive, in fact,  88c/92c fw will handle this */
+	u8 bUseRAMask;
+
+	struct odm_rate_adaptive RateAdaptive;
+
+	struct ant_detected_info AntDetectedInfo; /*  Antenna detected information for RSSI tool */
+
+	struct odm_rf_cal_t RFCalibrateInfo;
+
+	/*  */
+	/*  TX power tracking */
+	/*  */
+	u8 BbSwingIdxOfdm[MAX_RF_PATH];
+	u8 BbSwingIdxOfdmCurrent;
+	u8 BbSwingIdxOfdmBase[MAX_RF_PATH];
+	bool BbSwingFlagOfdm;
+	u8 BbSwingIdxCck;
+	u8 BbSwingIdxCckCurrent;
+	u8 BbSwingIdxCckBase;
+	u8 DefaultOfdmIndex;
+	u8 DefaultCckIndex;
+	bool BbSwingFlagCck;
+
+	s8 Absolute_OFDMSwingIdx[MAX_RF_PATH];
+	s8 Remnant_OFDMSwingIdx[MAX_RF_PATH];
+	s8 Remnant_CCKSwingIdx;
+	s8 Modify_TxAGC_Value;       /* Remnat compensate value at TxAGC */
+	bool Modify_TxAGC_Flag_PathA;
+	bool Modify_TxAGC_Flag_PathB;
+	bool Modify_TxAGC_Flag_PathC;
+	bool Modify_TxAGC_Flag_PathD;
+	bool Modify_TxAGC_Flag_PathA_CCK;
+
+	s8 KfreeOffset[MAX_RF_PATH];
+	/*  */
+	/*  ODM system resource. */
+	/*  */
+
+	/*  ODM relative time. */
+	struct timer_list PathDivSwitchTimer;
+	/* 2011.09.27 add for Path Diversity */
+	struct timer_list CCKPathDiversityTimer;
+	struct timer_list FastAntTrainingTimer;
+
+	/*  ODM relative workitem. */
+
+	#if (BEAMFORMING_SUPPORT == 1)
+	RT_BEAMFORMING_INFO BeamformingInfo;
+	#endif
+};
+
+ enum odm_rf_content {
+	odm_radioa_txt = 0x1000,
+	odm_radiob_txt = 0x1001,
+	odm_radioc_txt = 0x1002,
+	odm_radiod_txt = 0x1003
+};
+
+enum ODM_BB_Config_Type {
+	CONFIG_BB_PHY_REG,
+	CONFIG_BB_AGC_TAB,
+	CONFIG_BB_AGC_TAB_2G,
+	CONFIG_BB_PHY_REG_PG,
+	CONFIG_BB_PHY_REG_MP,
+	CONFIG_BB_AGC_TAB_DIFF,
+};
+
+enum ODM_RF_Config_Type {
+	CONFIG_RF_RADIO,
+	CONFIG_RF_TXPWR_LMT,
+};
+
+enum ODM_FW_Config_Type {
+	CONFIG_FW_NIC,
+	CONFIG_FW_NIC_2,
+	CONFIG_FW_AP,
+	CONFIG_FW_WoWLAN,
+	CONFIG_FW_WoWLAN_2,
+	CONFIG_FW_AP_WoWLAN,
+	CONFIG_FW_BT,
+};
+
+#ifdef REMOVE_PACK
+#pragma pack()
+#endif
+
+/* include "odm_function.h" */
+
+/* 3 =========================================================== */
+/* 3 DIG */
+/* 3 =========================================================== */
+
+/* Remove DIG by Yuchen */
+
+/* 3 =========================================================== */
+/* 3 AGC RX High Power Mode */
+/* 3 =========================================================== */
+#define          LNA_Low_Gain_1                      0x64
+#define          LNA_Low_Gain_2                      0x5A
+#define          LNA_Low_Gain_3                      0x58
+
+#define          FA_RXHP_TH1                           5000
+#define          FA_RXHP_TH2                           1500
+#define          FA_RXHP_TH3                             800
+#define          FA_RXHP_TH4                             600
+#define          FA_RXHP_TH5                             500
+
+/* 3 =========================================================== */
+/* 3 EDCA */
+/* 3 =========================================================== */
+
+/* 3 =========================================================== */
+/* 3 Dynamic Tx Power */
+/* 3 =========================================================== */
+/* Dynamic Tx Power Control Threshold */
+
+/* 3 =========================================================== */
+/* 3 Rate Adaptive */
+/* 3 =========================================================== */
+#define		DM_RATR_STA_INIT			0
+#define		DM_RATR_STA_HIGH			1
+#define		DM_RATR_STA_MIDDLE			2
+#define		DM_RATR_STA_LOW				3
+
+/* 3 =========================================================== */
+/* 3 BB Power Save */
+/* 3 =========================================================== */
+
+enum { /* tag_1R_CCA_Type_Definition */
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+};
+
+enum { /* tag_RF_Type_Definition */
+	RF_Save = 0,
+	RF_Normal = 1,
+	RF_MAX = 2,
+};
+
+/*  Maximal number of antenna detection mechanism needs to perform, added by Roger, 2011.12.28. */
+#define	MAX_ANTENNA_DETECTION_CNT	10
+
+/*  */
+/*  Extern Global Variables. */
+/*  */
+extern	u32 OFDMSwingTable[OFDM_TABLE_SIZE];
+extern	u8 CCKSwingTable_Ch1_Ch13[CCK_TABLE_SIZE][8];
+extern	u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8];
+
+extern	u32 OFDMSwingTable_New[OFDM_TABLE_SIZE];
+extern	u8 CCKSwingTable_Ch1_Ch13_New[CCK_TABLE_SIZE][8];
+extern	u8 CCKSwingTable_Ch14_New[CCK_TABLE_SIZE][8];
+
+extern  u32 TxScalingTable_Jaguar[TXSCALE_TABLE_SIZE];
+
+/*  */
+/*  check Sta pointer valid or not */
+/*  */
+#define IS_STA_VALID(pSta)		(pSta)
+/*  20100514 Joseph: Add definition for antenna switching test after link. */
+/*  This indicates two different the steps. */
+/*  In SWAW_STEP_PEAK, driver needs to switch antenna and listen to the signal on the air. */
+/*  In SWAW_STEP_DETERMINE, driver just compares the signal captured in SWAW_STEP_PEAK */
+/*  with original RSSI to determine if it is necessary to switch antenna. */
+#define SWAW_STEP_PEAK		0
+#define SWAW_STEP_DETERMINE	1
+
+/* Remove BB power saving by Yuchen */
+
+#define dm_CheckTXPowerTracking ODM_TXPowerTrackingCheck
+void ODM_TXPowerTrackingCheck(struct dm_odm_t *pDM_Odm);
+
+bool ODM_RAStateCheck(
+	struct dm_odm_t *pDM_Odm,
+	s32	RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+);
+
+#define dm_SWAW_RSSI_Check	ODM_SwAntDivChkPerPktRssi
+void ODM_SwAntDivChkPerPktRssi(
+	struct dm_odm_t *pDM_Odm,
+	u8 StationID,
+	struct odm_phy_info *pPhyInfo
+);
+
+u32 ODM_Get_Rate_Bitmap(
+	struct dm_odm_t *pDM_Odm,
+	u32 macid,
+	u32 ra_mask,
+	u8 rssi_level
+);
+
+#if (BEAMFORMING_SUPPORT == 1)
+BEAMFORMING_CAP Beamforming_GetEntryBeamCapByMacId(PMGNT_INFO pMgntInfo, u8 MacId);
+#endif
+
+void odm_TXPowerTrackingInit(struct dm_odm_t *pDM_Odm);
+
+void ODM_DMInit(struct dm_odm_t *pDM_Odm);
+
+void ODM_DMWatchdog(struct dm_odm_t *pDM_Odm); /*  For common use in the future */
+
+void ODM_CmnInfoInit(struct dm_odm_t *pDM_Odm, enum odm_cmninfo_e CmnInfo, u32 Value);
+
+void ODM_CmnInfoHook(struct dm_odm_t *pDM_Odm, enum odm_cmninfo_e CmnInfo, void *pValue);
+
+void ODM_CmnInfoPtrArrayHook(
+	struct dm_odm_t *pDM_Odm,
+	enum odm_cmninfo_e CmnInfo,
+	u16 Index,
+	void *pValue
+);
+
+void ODM_CmnInfoUpdate(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value);
+
+void ODM_InitAllTimers(struct dm_odm_t *pDM_Odm);
+
+void ODM_CancelAllTimers(struct dm_odm_t *pDM_Odm);
+
+void ODM_ReleaseAllTimers(struct dm_odm_t *pDM_Odm);
+
+void ODM_AntselStatistics_88C(
+	struct dm_odm_t *pDM_Odm,
+	u8 MacId,
+	u32 PWDBAll,
+	bool isCCKrate
+);
+
+void ODM_DynamicARFBSelect(struct dm_odm_t *pDM_Odm, u8 rate, bool Collision_State);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c
new file mode 100644
index 000000000..928c58be6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c
@@ -0,0 +1,211 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+static void odm_SetCrystalCap(void *pDM_VOID, u8 CrystalCap)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	if (pCfoTrack->CrystalCap == CrystalCap)
+		return;
+
+	pCfoTrack->CrystalCap = CrystalCap;
+
+	/*  0x2C[23:18] = 0x2C[17:12] = CrystalCap */
+	CrystalCap = CrystalCap & 0x3F;
+	PHY_SetBBReg(
+		pDM_Odm->Adapter,
+		REG_MAC_PHY_CTRL,
+		0x00FFF000,
+		(CrystalCap | (CrystalCap << 6))
+	);
+}
+
+static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	return pHalData->CrystalCap & 0x3f;
+}
+
+static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	if (pCfoTrack->bATCStatus == ATCStatus)
+		return;
+
+	PHY_SetBBReg(
+		pDM_Odm->Adapter,
+		ODM_REG(BB_ATC, pDM_Odm),
+		ODM_BIT(BB_ATC, pDM_Odm),
+		ATCStatus
+	);
+	pCfoTrack->bATCStatus = ATCStatus;
+}
+
+static bool odm_GetATCStatus(void *pDM_VOID)
+{
+	bool ATCStatus;
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	ATCStatus = (bool)PHY_QueryBBReg(
+		pDM_Odm->Adapter,
+		ODM_REG(BB_ATC, pDM_Odm),
+		ODM_BIT(BB_ATC, pDM_Odm)
+	);
+	return ATCStatus;
+}
+
+void ODM_CfoTrackingReset(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	pCfoTrack->DefXCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
+	pCfoTrack->bAdjust = true;
+
+	odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
+	odm_SetATCStatus(pDM_Odm, true);
+}
+
+void ODM_CfoTrackingInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	pCfoTrack->DefXCap =
+		pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
+	pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
+	pCfoTrack->bAdjust = true;
+}
+
+void ODM_CfoTracking(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	int CFO_kHz_A, CFO_ave = 0;
+	int CFO_ave_diff;
+	int CrystalCap = (int)pCfoTrack->CrystalCap;
+	u8 Adjust_Xtal = 1;
+
+	/* 4 Support ability */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING)) {
+		return;
+	}
+
+	if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
+		/* 4 No link or more than one entry */
+		ODM_CfoTrackingReset(pDM_Odm);
+	} else {
+		/* 3 1. CFO Tracking */
+		/* 4 1.1 No new packet */
+		if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
+			return;
+		}
+		pCfoTrack->packetCount_pre = pCfoTrack->packetCount;
+
+		/* 4 1.2 Calculate CFO */
+		CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;
+
+		CFO_ave = CFO_kHz_A;
+
+		/* 4 1.3 Avoid abnormal large CFO */
+		CFO_ave_diff =
+			(pCfoTrack->CFO_ave_pre >= CFO_ave) ?
+			(pCfoTrack->CFO_ave_pre-CFO_ave) :
+			(CFO_ave-pCfoTrack->CFO_ave_pre);
+
+		if (
+			CFO_ave_diff > 20 &&
+			pCfoTrack->largeCFOHit == 0 &&
+			!pCfoTrack->bAdjust
+		) {
+			pCfoTrack->largeCFOHit = 1;
+			return;
+		} else
+			pCfoTrack->largeCFOHit = 0;
+		pCfoTrack->CFO_ave_pre = CFO_ave;
+
+		/* 4 1.4 Dynamic Xtal threshold */
+		if (pCfoTrack->bAdjust == false) {
+			if (CFO_ave > CFO_TH_XTAL_HIGH || CFO_ave < (-CFO_TH_XTAL_HIGH))
+				pCfoTrack->bAdjust = true;
+		} else {
+			if (CFO_ave < CFO_TH_XTAL_LOW && CFO_ave > (-CFO_TH_XTAL_LOW))
+				pCfoTrack->bAdjust = false;
+		}
+
+		/* 4 1.5 BT case: Disable CFO tracking */
+		if (pDM_Odm->bBtEnabled) {
+			pCfoTrack->bAdjust = false;
+			odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
+		}
+
+		/* 4 1.6 Big jump */
+		if (pCfoTrack->bAdjust) {
+			if (CFO_ave > CFO_TH_XTAL_LOW)
+				Adjust_Xtal = Adjust_Xtal+((CFO_ave-CFO_TH_XTAL_LOW)>>2);
+			else if (CFO_ave < (-CFO_TH_XTAL_LOW))
+				Adjust_Xtal = Adjust_Xtal+((CFO_TH_XTAL_LOW-CFO_ave)>>2);
+		}
+
+		/* 4 1.7 Adjust Crystal Cap. */
+		if (pCfoTrack->bAdjust) {
+			if (CFO_ave > CFO_TH_XTAL_LOW)
+				CrystalCap = CrystalCap + Adjust_Xtal;
+			else if (CFO_ave < (-CFO_TH_XTAL_LOW))
+				CrystalCap = CrystalCap - Adjust_Xtal;
+
+			if (CrystalCap > 0x3f)
+				CrystalCap = 0x3f;
+			else if (CrystalCap < 0)
+				CrystalCap = 0;
+
+			odm_SetCrystalCap(pDM_Odm, (u8)CrystalCap);
+		}
+
+		/* 3 2. Dynamic ATC switch */
+		if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
+			odm_SetATCStatus(pDM_Odm, false);
+		} else {
+			odm_SetATCStatus(pDM_Odm, true);
+		}
+	}
+}
+
+void odm_parsing_cfo(void *dm_void, void *pkt_info_void, s8 *cfotail)
+{
+	struct dm_odm_t *dm_odm = (struct dm_odm_t *)dm_void;
+	struct odm_packet_info *pkt_info = pkt_info_void;
+	struct cfo_tracking *cfo_track = &dm_odm->DM_CfoTrack;
+	u8 i;
+
+	if (!(dm_odm->SupportAbility & ODM_BB_CFO_TRACKING))
+		return;
+
+	if (pkt_info->station_id != 0) {
+		/*
+		 * 3 Update CFO report for path-A & path-B
+		 * Only paht-A and path-B have CFO tail and short CFO
+		 */
+		for (i = RF_PATH_A; i <= RF_PATH_B; i++)
+			cfo_track->CFO_tail[i] = (int)cfotail[i];
+
+		/* 3 Update packet counter */
+		if (cfo_track->packetCount == 0xffffffff)
+			cfo_track->packetCount = 0;
+		else
+			cfo_track->packetCount++;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h
new file mode 100644
index 000000000..8fa6d9ec5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMCFOTRACK_H__
+#define    __ODMCFOTRACK_H__
+
+#define		CFO_TH_XTAL_HIGH		20		/*  kHz */
+#define		CFO_TH_XTAL_LOW			10		/*  kHz */
+#define		CFO_TH_ATC			80		/*  kHz */
+
+struct cfo_tracking {
+	bool bATCStatus;
+	bool largeCFOHit;
+	bool bAdjust;
+	u8 CrystalCap;
+	u8 DefXCap;
+	int CFO_tail[2];
+	int CFO_ave_pre;
+	u32 packetCount;
+	u32 packetCount_pre;
+
+	bool bForceXtalCap;
+	bool bReset;
+};
+
+void ODM_CfoTrackingReset(void *pDM_VOID
+);
+
+void ODM_CfoTrackingInit(void *pDM_VOID);
+
+void ODM_CfoTracking(void *pDM_VOID);
+
+void odm_parsing_cfo(void *pDM_VOID, void *pPktinfo_VOID, s8 *pcfotail);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_DIG.c b/drivers/staging/rtl8723bs/hal/odm_DIG.c
new file mode 100644
index 000000000..07edf74cc
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DIG.c
@@ -0,0 +1,825 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_NHMCounterStatisticsInit(void *pDM_VOID)
+{
+	struct dm_odm_t	*pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	/* PHY parameters initialize for n series */
+	rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TIMER_11N+2, 0x2710);	/* 0x894[31:16]= 0x2710	Time duration for NHM unit: 4us, 0x2710 =40ms */
+	/* rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TIMER_11N+2, 0x4e20);	0x894[31:16]= 0x4e20	Time duration for NHM unit: 4us, 0x4e20 =80ms */
+	rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N+2, 0xffff);	/* 0x890[31:16]= 0xffff	th_9, th_10 */
+	/* rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH3_TO_TH0_11N, 0xffffff5c);	0x898 = 0xffffff5c		th_3, th_2, th_1, th_0 */
+	rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH3_TO_TH0_11N, 0xffffff52);	/* 0x898 = 0xffffff52		th_3, th_2, th_1, th_0 */
+	rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH7_TO_TH4_11N, 0xffffffff);	/* 0x89c = 0xffffffff		th_7, th_6, th_5, th_4 */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_FPGA0_IQK_11N, bMaskByte0, 0xff);		/* 0xe28[7:0]= 0xff		th_8 */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT10|BIT9|BIT8, 0x7);	/* 0x890[9:8]=3			enable CCX */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_RSTC_11N, BIT7, 0x1);		/* 0xc0c[7]= 1			max power among all RX ants */
+}
+
+void odm_NHMCounterStatistics(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	/*  Get NHM report */
+	odm_GetNHMCounterStatistics(pDM_Odm);
+
+	/*  Reset NHM counter */
+	odm_NHMCounterStatisticsReset(pDM_Odm);
+}
+
+void odm_GetNHMCounterStatistics(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	u32 value32 = 0;
+
+	value32 = PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG_NHM_CNT_11N, bMaskDWord);
+
+	pDM_Odm->NHM_cnt_0 = (u8)(value32 & bMaskByte0);
+}
+
+void odm_NHMCounterStatisticsReset(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT1, 0);
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT1, 1);
+}
+
+void odm_NHMBBInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	pDM_Odm->adaptivity_flag = 0;
+	pDM_Odm->tolerance_cnt = 3;
+	pDM_Odm->NHMLastTxOkcnt = 0;
+	pDM_Odm->NHMLastRxOkcnt = 0;
+	pDM_Odm->NHMCurTxOkcnt = 0;
+	pDM_Odm->NHMCurRxOkcnt = 0;
+}
+
+/*  */
+void odm_NHMBB(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	/* u8 test_status; */
+	/* struct false_ALARM_STATISTICS *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt; */
+
+	pDM_Odm->NHMCurTxOkcnt =
+		*(pDM_Odm->pNumTxBytesUnicast)-pDM_Odm->NHMLastTxOkcnt;
+	pDM_Odm->NHMCurRxOkcnt =
+		*(pDM_Odm->pNumRxBytesUnicast)-pDM_Odm->NHMLastRxOkcnt;
+	pDM_Odm->NHMLastTxOkcnt =
+		*(pDM_Odm->pNumTxBytesUnicast);
+	pDM_Odm->NHMLastRxOkcnt =
+		*(pDM_Odm->pNumRxBytesUnicast);
+
+
+	if ((pDM_Odm->NHMCurTxOkcnt) + 1 > (u64)(pDM_Odm->NHMCurRxOkcnt<<2) + 1) { /* Tx > 4*Rx possible for adaptivity test */
+		if (pDM_Odm->NHM_cnt_0 >= 190 || pDM_Odm->adaptivity_flag == true) {
+			/* Enable EDCCA since it is possible running Adaptivity testing */
+			/* test_status = 1; */
+			pDM_Odm->adaptivity_flag = true;
+			pDM_Odm->tolerance_cnt = 0;
+		} else {
+			if (pDM_Odm->tolerance_cnt < 3)
+				pDM_Odm->tolerance_cnt = pDM_Odm->tolerance_cnt + 1;
+			else
+				pDM_Odm->tolerance_cnt = 4;
+			/* test_status = 5; */
+			if (pDM_Odm->tolerance_cnt > 3) {
+				/* test_status = 3; */
+				pDM_Odm->adaptivity_flag = false;
+			}
+		}
+	} else { /*  TX<RX */
+		if (pDM_Odm->adaptivity_flag == true && pDM_Odm->NHM_cnt_0 <= 200) {
+			/* test_status = 2; */
+			pDM_Odm->tolerance_cnt = 0;
+		} else {
+			if (pDM_Odm->tolerance_cnt < 3)
+				pDM_Odm->tolerance_cnt = pDM_Odm->tolerance_cnt + 1;
+			else
+				pDM_Odm->tolerance_cnt = 4;
+			/* test_status = 5; */
+			if (pDM_Odm->tolerance_cnt > 3) {
+				/* test_status = 4; */
+				pDM_Odm->adaptivity_flag = false;
+			}
+		}
+	}
+}
+
+void odm_SearchPwdBLowerBound(void *pDM_VOID, u8 IGI_target)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	u32 value32 = 0;
+	u8 cnt, IGI;
+	bool bAdjust = true;
+	s8 TH_L2H_dmc, TH_H2L_dmc;
+	s8 Diff;
+
+	IGI = 0x50; /*  find H2L, L2H lower bound */
+	ODM_Write_DIG(pDM_Odm, IGI);
+
+
+	Diff = IGI_target-(s8)IGI;
+	TH_L2H_dmc = pDM_Odm->TH_L2H_ini + Diff;
+	if (TH_L2H_dmc > 10)
+		TH_L2H_dmc = 10;
+	TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+
+	mdelay(5);
+
+	while (bAdjust) {
+		for (cnt = 0; cnt < 20; cnt++) {
+			value32 = PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG_RPT_11N, bMaskDWord);
+
+			if (value32 & BIT30)
+				pDM_Odm->txEdcca1 = pDM_Odm->txEdcca1 + 1;
+			else if (value32 & BIT29)
+				pDM_Odm->txEdcca1 = pDM_Odm->txEdcca1 + 1;
+			else
+				pDM_Odm->txEdcca0 = pDM_Odm->txEdcca0 + 1;
+		}
+
+		if (pDM_Odm->txEdcca1 > 5) {
+			IGI = IGI-1;
+			TH_L2H_dmc = TH_L2H_dmc + 1;
+			if (TH_L2H_dmc > 10)
+				TH_L2H_dmc = 10;
+			TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+
+			pDM_Odm->TxHangFlg = true;
+			pDM_Odm->txEdcca1 = 0;
+			pDM_Odm->txEdcca0 = 0;
+
+			if (TH_L2H_dmc == 10) {
+				bAdjust = false;
+				pDM_Odm->TxHangFlg = false;
+				pDM_Odm->txEdcca1 = 0;
+				pDM_Odm->txEdcca0 = 0;
+				pDM_Odm->H2L_lb = TH_H2L_dmc;
+				pDM_Odm->L2H_lb = TH_L2H_dmc;
+				pDM_Odm->Adaptivity_IGI_upper = IGI;
+			}
+		} else {
+			bAdjust = false;
+			pDM_Odm->TxHangFlg = false;
+			pDM_Odm->txEdcca1 = 0;
+			pDM_Odm->txEdcca0 = 0;
+			pDM_Odm->H2L_lb = TH_H2L_dmc;
+			pDM_Odm->L2H_lb = TH_L2H_dmc;
+			pDM_Odm->Adaptivity_IGI_upper = IGI;
+		}
+	}
+}
+
+void odm_AdaptivityInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	if (pDM_Odm->Carrier_Sense_enable == false)
+		pDM_Odm->TH_L2H_ini = 0xf7; /*  -7 */
+	else
+		pDM_Odm->TH_L2H_ini = 0xa;
+
+	pDM_Odm->AdapEn_RSSI = 20;
+	pDM_Odm->TH_EDCCA_HL_diff = 7;
+
+	pDM_Odm->IGI_Base = 0x32;
+	pDM_Odm->IGI_target = 0x1c;
+	pDM_Odm->ForceEDCCA = 0;
+	pDM_Odm->NHM_disable = false;
+	pDM_Odm->TxHangFlg = true;
+	pDM_Odm->txEdcca0 = 0;
+	pDM_Odm->txEdcca1 = 0;
+	pDM_Odm->H2L_lb = 0;
+	pDM_Odm->L2H_lb = 0;
+	pDM_Odm->Adaptivity_IGI_upper = 0;
+	odm_NHMBBInit(pDM_Odm);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, REG_RD_CTRL, BIT11, 1); /*  stop counting if EDCCA is asserted */
+}
+
+
+void odm_Adaptivity(void *pDM_VOID, u8 IGI)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	s8 TH_L2H_dmc, TH_H2L_dmc;
+	s8 Diff, IGI_target;
+	bool EDCCA_State = false;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY)) {
+		return;
+	}
+
+	if (*pDM_Odm->pBandWidth == ODM_BW20M) /* CHANNEL_WIDTH_20 */
+		IGI_target = pDM_Odm->IGI_Base;
+	else if (*pDM_Odm->pBandWidth == ODM_BW40M)
+		IGI_target = pDM_Odm->IGI_Base + 2;
+	else
+		IGI_target = pDM_Odm->IGI_Base;
+	pDM_Odm->IGI_target = (u8) IGI_target;
+
+	/* Search pwdB lower bound */
+	if (pDM_Odm->TxHangFlg == true) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_DBG_RPT_11N, bMaskDWord, 0x208);
+		odm_SearchPwdBLowerBound(pDM_Odm, pDM_Odm->IGI_target);
+	}
+
+	if ((!pDM_Odm->bLinked) || (*pDM_Odm->pChannel > 149)) { /*  Band4 doesn't need adaptivity */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, 0x7f);
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, 0x7f);
+		return;
+	}
+
+	if (!pDM_Odm->ForceEDCCA) {
+		if (pDM_Odm->RSSI_Min > pDM_Odm->AdapEn_RSSI)
+			EDCCA_State = true;
+		else if (pDM_Odm->RSSI_Min < (pDM_Odm->AdapEn_RSSI - 5))
+			EDCCA_State = false;
+	} else
+		EDCCA_State = true;
+
+	if (
+		pDM_Odm->bLinked &&
+		pDM_Odm->Carrier_Sense_enable == false &&
+		pDM_Odm->NHM_disable == false &&
+		pDM_Odm->TxHangFlg == false
+	)
+		odm_NHMBB(pDM_Odm);
+
+	if (EDCCA_State) {
+		Diff = IGI_target-(s8)IGI;
+		TH_L2H_dmc = pDM_Odm->TH_L2H_ini + Diff;
+		if (TH_L2H_dmc > 10)
+			TH_L2H_dmc = 10;
+
+		TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+
+		/* replace lower bound to prevent EDCCA always equal  */
+		if (TH_H2L_dmc < pDM_Odm->H2L_lb)
+			TH_H2L_dmc = pDM_Odm->H2L_lb;
+		if (TH_L2H_dmc < pDM_Odm->L2H_lb)
+			TH_L2H_dmc = pDM_Odm->L2H_lb;
+	} else {
+		TH_L2H_dmc = 0x7f;
+		TH_H2L_dmc = 0x7f;
+	}
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+}
+
+void ODM_Write_DIG(void *pDM_VOID, u8 CurrentIGI)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	if (pDM_DigTable->bStopDIG) {
+		return;
+	}
+
+	if (pDM_DigTable->CurIGValue != CurrentIGI) {
+		/* 1 Check initial gain by upper bound */
+		if (!pDM_DigTable->bPSDInProgress) {
+			if (CurrentIGI > pDM_DigTable->rx_gain_range_max) {
+				CurrentIGI = pDM_DigTable->rx_gain_range_max;
+			}
+
+		}
+
+		/* 1 Set IGI value */
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG(IGI_B, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
+
+		pDM_DigTable->CurIGValue = CurrentIGI;
+	}
+
+}
+
+bool odm_DigAbort(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)) {
+		return	true;
+	}
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_DIG)) {
+		return	true;
+	}
+
+	/* ScanInProcess */
+	if (*(pDM_Odm->pbScanInProcess)) {
+		return	true;
+	}
+
+	/* add by Neil Chen to avoid PSD is processing */
+	if (pDM_Odm->bDMInitialGainEnable == false) {
+		return	true;
+	}
+
+	return	false;
+}
+
+void odm_DIGInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	pDM_DigTable->bStopDIG = false;
+	pDM_DigTable->bPSDInProgress = false;
+	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm));
+	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
+	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
+	pDM_DigTable->FALowThresh	= DMfalseALARM_THRESH_LOW;
+	pDM_DigTable->FAHighThresh	= DMfalseALARM_THRESH_HIGH;
+	pDM_DigTable->BackoffVal = DM_DIG_BACKOFF_DEFAULT;
+	pDM_DigTable->BackoffVal_range_max = DM_DIG_BACKOFF_MAX;
+	pDM_DigTable->BackoffVal_range_min = DM_DIG_BACKOFF_MIN;
+	pDM_DigTable->PreCCK_CCAThres = 0xFF;
+	pDM_DigTable->CurCCK_CCAThres = 0x83;
+	pDM_DigTable->ForbiddenIGI = DM_DIG_MIN_NIC;
+	pDM_DigTable->LargeFAHit = 0;
+	pDM_DigTable->Recover_cnt = 0;
+	pDM_DigTable->bMediaConnect_0 = false;
+	pDM_DigTable->bMediaConnect_1 = false;
+
+	/* To Initialize pDM_Odm->bDMInitialGainEnable == false to avoid DIG error */
+	pDM_Odm->bDMInitialGainEnable = true;
+
+	pDM_DigTable->DIG_Dynamic_MIN_0 = DM_DIG_MIN_NIC;
+	pDM_DigTable->DIG_Dynamic_MIN_1 = DM_DIG_MIN_NIC;
+
+	/* To Initi BT30 IGI */
+	pDM_DigTable->BT30_CurIGI = 0x32;
+
+	pDM_DigTable->rx_gain_range_max = DM_DIG_MAX_NIC;
+	pDM_DigTable->rx_gain_range_min = DM_DIG_MIN_NIC;
+
+}
+
+
+void odm_DIG(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	/*  Common parameters */
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct false_ALARM_STATISTICS *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	bool FirstConnect, FirstDisConnect;
+	u8 DIG_MaxOfMin, DIG_Dynamic_MIN;
+	u8 dm_dig_max, dm_dig_min;
+	u8 CurrentIGI = pDM_DigTable->CurIGValue;
+	u8 offset;
+	u32 dm_FA_thres[3];
+	u8 Adap_IGI_Upper = 0;
+	u32 TxTp = 0, RxTp = 0;
+	bool bDFSBand = false;
+	bool bPerformance = true, bFirstTpTarget = false, bFirstCoverage = false;
+
+	if (odm_DigAbort(pDM_Odm))
+		return;
+
+	if (pDM_Odm->adaptivity_flag == true)
+		Adap_IGI_Upper = pDM_Odm->Adaptivity_IGI_upper;
+
+
+	/* 1 Update status */
+	DIG_Dynamic_MIN = pDM_DigTable->DIG_Dynamic_MIN_0;
+	FirstConnect = (pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0 == false);
+	FirstDisConnect = (!pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0 == true);
+
+	/* 1 Boundary Decision */
+	/* 2 For WIN\CE */
+	dm_dig_max = 0x5A;
+	dm_dig_min = DM_DIG_MIN_NIC;
+	DIG_MaxOfMin = DM_DIG_MAX_AP;
+
+	/* 1 Adjust boundary by RSSI */
+	if (pDM_Odm->bLinked && bPerformance) {
+		/* 2 Modify DIG upper bound */
+		/* 4 Modify DIG upper bound for 92E, 8723A\B, 8821 & 8812 BT */
+		if (pDM_Odm->bBtLimitedDig == 1) {
+			offset = 10;
+		} else
+			offset = 15;
+
+		if ((pDM_Odm->RSSI_Min + offset) > dm_dig_max)
+			pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		else if ((pDM_Odm->RSSI_Min + offset) < dm_dig_min)
+			pDM_DigTable->rx_gain_range_max = dm_dig_min;
+		else
+			pDM_DigTable->rx_gain_range_max = pDM_Odm->RSSI_Min + offset;
+
+		/* 2 Modify DIG lower bound */
+		/* if (pDM_Odm->bOneEntryOnly) */
+		{
+			if (pDM_Odm->RSSI_Min < dm_dig_min)
+				DIG_Dynamic_MIN = dm_dig_min;
+			else if (pDM_Odm->RSSI_Min > DIG_MaxOfMin)
+				DIG_Dynamic_MIN = DIG_MaxOfMin;
+			else
+				DIG_Dynamic_MIN = pDM_Odm->RSSI_Min;
+		}
+	} else {
+		pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		DIG_Dynamic_MIN = dm_dig_min;
+	}
+
+	/* 1 Force Lower Bound for AntDiv */
+	if (pDM_Odm->bLinked && !pDM_Odm->bOneEntryOnly) {
+		if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
+			if (
+				pDM_Odm->AntDivType == CG_TRX_HW_ANTDIV ||
+				pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV ||
+				pDM_Odm->AntDivType == S0S1_SW_ANTDIV
+			) {
+				if (pDM_DigTable->AntDiv_RSSI_max > DIG_MaxOfMin)
+					DIG_Dynamic_MIN = DIG_MaxOfMin;
+				else
+					DIG_Dynamic_MIN = (u8) pDM_DigTable->AntDiv_RSSI_max;
+			}
+		}
+	}
+
+	/* 1 Modify DIG lower bound, deal with abnormal case */
+	/* 2 Abnormal false alarm case */
+	if (FirstDisConnect) {
+		pDM_DigTable->rx_gain_range_min = DIG_Dynamic_MIN;
+		pDM_DigTable->ForbiddenIGI = DIG_Dynamic_MIN;
+	} else
+		pDM_DigTable->rx_gain_range_min =
+			odm_ForbiddenIGICheck(pDM_Odm, DIG_Dynamic_MIN, CurrentIGI);
+
+	if (pDM_Odm->bLinked && !FirstConnect) {
+		if (
+			(pDM_Odm->PhyDbgInfo.NumQryBeaconPkt < 5) &&
+			pDM_Odm->bsta_state
+		) {
+			pDM_DigTable->rx_gain_range_min = dm_dig_min;
+		}
+	}
+
+	/* 2 Abnormal lower bound case */
+	if (pDM_DigTable->rx_gain_range_min > pDM_DigTable->rx_gain_range_max) {
+		pDM_DigTable->rx_gain_range_min = pDM_DigTable->rx_gain_range_max;
+	}
+
+
+	/* 1 False alarm threshold decision */
+	odm_FAThresholdCheck(pDM_Odm, bDFSBand, bPerformance, RxTp, TxTp, dm_FA_thres);
+
+	/* 1 Adjust initial gain by false alarm */
+	if (pDM_Odm->bLinked && bPerformance) {
+
+		if (bFirstTpTarget || FirstConnect) {
+			pDM_DigTable->LargeFAHit = 0;
+
+			if (pDM_Odm->RSSI_Min < DIG_MaxOfMin) {
+				if (CurrentIGI < pDM_Odm->RSSI_Min)
+					CurrentIGI = pDM_Odm->RSSI_Min;
+			} else {
+				if (CurrentIGI < DIG_MaxOfMin)
+					CurrentIGI = DIG_MaxOfMin;
+			}
+
+		} else {
+			if (pFalseAlmCnt->Cnt_all > dm_FA_thres[2])
+				CurrentIGI = CurrentIGI + 4;
+			else if (pFalseAlmCnt->Cnt_all > dm_FA_thres[1])
+				CurrentIGI = CurrentIGI + 2;
+			else if (pFalseAlmCnt->Cnt_all < dm_FA_thres[0])
+				CurrentIGI = CurrentIGI - 2;
+
+			if (
+				(pDM_Odm->PhyDbgInfo.NumQryBeaconPkt < 5) &&
+				(pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH1) &&
+				(pDM_Odm->bsta_state)
+			) {
+				CurrentIGI = pDM_DigTable->rx_gain_range_min;
+			}
+		}
+	} else {
+
+		if (FirstDisConnect || bFirstCoverage) {
+			CurrentIGI = dm_dig_min;
+		} else {
+			if (pFalseAlmCnt->Cnt_all > dm_FA_thres[2])
+				CurrentIGI = CurrentIGI + 4;
+			else if (pFalseAlmCnt->Cnt_all > dm_FA_thres[1])
+				CurrentIGI = CurrentIGI + 2;
+			else if (pFalseAlmCnt->Cnt_all < dm_FA_thres[0])
+				CurrentIGI = CurrentIGI - 2;
+		}
+	}
+
+	/* 1 Check initial gain by upper/lower bound */
+	if (CurrentIGI < pDM_DigTable->rx_gain_range_min)
+		CurrentIGI = pDM_DigTable->rx_gain_range_min;
+
+	if (CurrentIGI > pDM_DigTable->rx_gain_range_max)
+		CurrentIGI = pDM_DigTable->rx_gain_range_max;
+
+	/* 1 Force upper bound and lower bound for adaptivity */
+	if (
+		pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY &&
+		pDM_Odm->adaptivity_flag == true
+	) {
+		if (CurrentIGI > Adap_IGI_Upper)
+			CurrentIGI = Adap_IGI_Upper;
+
+		if (pDM_Odm->IGI_LowerBound != 0) {
+			if (CurrentIGI < pDM_Odm->IGI_LowerBound)
+				CurrentIGI = pDM_Odm->IGI_LowerBound;
+		}
+	}
+
+
+	/* 1 Update status */
+	if (pDM_Odm->bBtHsOperation) {
+		if (pDM_Odm->bLinked) {
+			if (pDM_DigTable->BT30_CurIGI > (CurrentIGI))
+				ODM_Write_DIG(pDM_Odm, CurrentIGI);
+			else
+				ODM_Write_DIG(pDM_Odm, pDM_DigTable->BT30_CurIGI);
+
+			pDM_DigTable->bMediaConnect_0 = pDM_Odm->bLinked;
+			pDM_DigTable->DIG_Dynamic_MIN_0 = DIG_Dynamic_MIN;
+		} else {
+			if (pDM_Odm->bLinkInProcess)
+				ODM_Write_DIG(pDM_Odm, 0x1c);
+			else if (pDM_Odm->bBtConnectProcess)
+				ODM_Write_DIG(pDM_Odm, 0x28);
+			else
+				ODM_Write_DIG(pDM_Odm, pDM_DigTable->BT30_CurIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+		}
+	} else { /*  BT is not using */
+		ODM_Write_DIG(pDM_Odm, CurrentIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+		pDM_DigTable->bMediaConnect_0 = pDM_Odm->bLinked;
+		pDM_DigTable->DIG_Dynamic_MIN_0 = DIG_Dynamic_MIN;
+	}
+}
+
+void odm_DIGbyRSSI_LPS(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct false_ALARM_STATISTICS *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+
+	u8 RSSI_Lower = DM_DIG_MIN_NIC;   /* 0x1E or 0x1C */
+	u8 CurrentIGI = pDM_Odm->RSSI_Min;
+
+	CurrentIGI = CurrentIGI+RSSI_OFFSET_DIG;
+
+	/*  Using FW PS mode to make IGI */
+	/* Adjust by  FA in LPS MODE */
+	if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2_LPS)
+		CurrentIGI = CurrentIGI+4;
+	else if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH1_LPS)
+		CurrentIGI = CurrentIGI+2;
+	else if (pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH0_LPS)
+		CurrentIGI = CurrentIGI-2;
+
+
+	/* Lower bound checking */
+
+	/* RSSI Lower bound check */
+	if ((pDM_Odm->RSSI_Min-10) > DM_DIG_MIN_NIC)
+		RSSI_Lower = pDM_Odm->RSSI_Min-10;
+	else
+		RSSI_Lower = DM_DIG_MIN_NIC;
+
+	/* Upper and Lower Bound checking */
+	if (CurrentIGI > DM_DIG_MAX_NIC)
+		CurrentIGI = DM_DIG_MAX_NIC;
+	else if (CurrentIGI < RSSI_Lower)
+		CurrentIGI = RSSI_Lower;
+
+	ODM_Write_DIG(pDM_Odm, CurrentIGI);
+	/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+}
+
+/* 3 ============================================================ */
+/* 3 FASLE ALARM CHECK */
+/* 3 ============================================================ */
+
+void odm_FalseAlarmCounterStatistics(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct false_ALARM_STATISTICS *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	u32 ret_value;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
+		return;
+
+	/* hold ofdm counter */
+	/* hold page C counter */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1);
+	/* hold page D counter */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Mcs_fail = (ret_value&0xffff);
+
+	FalseAlmCnt->Cnt_Ofdm_fail =
+		FalseAlmCnt->Cnt_Parity_Fail +
+		FalseAlmCnt->Cnt_Rate_Illegal +
+		FalseAlmCnt->Cnt_Crc8_fail +
+		FalseAlmCnt->Cnt_Mcs_fail +
+		FalseAlmCnt->Cnt_Fast_Fsync +
+		FalseAlmCnt->Cnt_SB_Search_fail;
+
+	{
+		/* hold cck counter */
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_FA_LSB_11N, bMaskByte0
+		);
+		FalseAlmCnt->Cnt_Cck_fail = ret_value;
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_FA_MSB_11N, bMaskByte3
+		);
+		FalseAlmCnt->Cnt_Cck_fail += (ret_value&0xff)<<8;
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord
+		);
+		FalseAlmCnt->Cnt_CCK_CCA =
+			((ret_value&0xFF)<<8) | ((ret_value&0xFF00)>>8);
+	}
+
+	FalseAlmCnt->Cnt_all = (
+		FalseAlmCnt->Cnt_Fast_Fsync +
+		FalseAlmCnt->Cnt_SB_Search_fail +
+		FalseAlmCnt->Cnt_Parity_Fail +
+		FalseAlmCnt->Cnt_Rate_Illegal +
+		FalseAlmCnt->Cnt_Crc8_fail +
+		FalseAlmCnt->Cnt_Mcs_fail +
+		FalseAlmCnt->Cnt_Cck_fail
+	);
+
+	FalseAlmCnt->Cnt_CCA_all =
+		FalseAlmCnt->Cnt_OFDM_CCA + FalseAlmCnt->Cnt_CCK_CCA;
+}
+
+
+void odm_FAThresholdCheck(
+	void *pDM_VOID,
+	bool bDFSBand,
+	bool bPerformance,
+	u32 RxTp,
+	u32 TxTp,
+	u32 *dm_FA_thres
+)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	if (pDM_Odm->bLinked && (bPerformance || bDFSBand)) {
+		/*  For NIC */
+		dm_FA_thres[0] = DM_DIG_FA_TH0;
+		dm_FA_thres[1] = DM_DIG_FA_TH1;
+		dm_FA_thres[2] = DM_DIG_FA_TH2;
+	} else {
+		dm_FA_thres[0] = 2000;
+		dm_FA_thres[1] = 4000;
+		dm_FA_thres[2] = 5000;
+	}
+}
+
+u8 odm_ForbiddenIGICheck(void *pDM_VOID, u8 DIG_Dynamic_MIN, u8 CurrentIGI)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct false_ALARM_STATISTICS *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	u8 rx_gain_range_min = pDM_DigTable->rx_gain_range_min;
+
+	if (pFalseAlmCnt->Cnt_all > 10000) {
+		if (pDM_DigTable->LargeFAHit != 3)
+			pDM_DigTable->LargeFAHit++;
+
+		/* if (pDM_DigTable->ForbiddenIGI < pDM_DigTable->CurIGValue) */
+		if (pDM_DigTable->ForbiddenIGI < CurrentIGI) {
+			pDM_DigTable->ForbiddenIGI = CurrentIGI;
+			/* pDM_DigTable->ForbiddenIGI = pDM_DigTable->CurIGValue; */
+			pDM_DigTable->LargeFAHit = 1;
+		}
+
+		if (pDM_DigTable->LargeFAHit >= 3) {
+			if ((pDM_DigTable->ForbiddenIGI + 2) > pDM_DigTable->rx_gain_range_max)
+				rx_gain_range_min = pDM_DigTable->rx_gain_range_max;
+			else
+				rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 2);
+			pDM_DigTable->Recover_cnt = 1800;
+		}
+	} else {
+		if (pDM_DigTable->Recover_cnt != 0) {
+			pDM_DigTable->Recover_cnt--;
+		} else {
+			if (pDM_DigTable->LargeFAHit < 3) {
+				if ((pDM_DigTable->ForbiddenIGI - 2) < DIG_Dynamic_MIN) { /* DM_DIG_MIN) */
+					pDM_DigTable->ForbiddenIGI = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+					rx_gain_range_min = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+				} else {
+					pDM_DigTable->ForbiddenIGI -= 2;
+					rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 2);
+				}
+			} else
+				pDM_DigTable->LargeFAHit = 0;
+		}
+	}
+
+	return rx_gain_range_min;
+
+}
+
+/* 3 ============================================================ */
+/* 3 CCK Packet Detect Threshold */
+/* 3 ============================================================ */
+
+void odm_CCKPacketDetectionThresh(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct false_ALARM_STATISTICS *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	u8 CurCCK_CCAThres;
+
+
+	if (
+		!(pDM_Odm->SupportAbility & ODM_BB_CCK_PD) ||
+		!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)
+	) {
+		return;
+	}
+
+	if (pDM_Odm->ExtLNA)
+		return;
+
+	if (pDM_Odm->bLinked) {
+		if (pDM_Odm->RSSI_Min > 25)
+			CurCCK_CCAThres = 0xcd;
+		else if ((pDM_Odm->RSSI_Min <= 25) && (pDM_Odm->RSSI_Min > 10))
+			CurCCK_CCAThres = 0x83;
+		else {
+			if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+				CurCCK_CCAThres = 0x83;
+			else
+				CurCCK_CCAThres = 0x40;
+		}
+	} else {
+		if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+			CurCCK_CCAThres = 0x83;
+		else
+			CurCCK_CCAThres = 0x40;
+	}
+
+	ODM_Write_CCK_CCA_Thres(pDM_Odm, CurCCK_CCAThres);
+}
+
+void ODM_Write_CCK_CCA_Thres(void *pDM_VOID, u8 CurCCK_CCAThres)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	/* modify by Guo.Mingzhi 2012-01-03 */
+	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)
+		rtw_write8(pDM_Odm->Adapter, ODM_REG(CCK_CCA, pDM_Odm), CurCCK_CCAThres);
+
+	pDM_DigTable->PreCCK_CCAThres = pDM_DigTable->CurCCK_CCAThres;
+	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_DIG.h b/drivers/staging/rtl8723bs/hal/odm_DIG.h
new file mode 100644
index 000000000..a5b041101
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DIG.h
@@ -0,0 +1,169 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __ODMDIG_H__
+#define __ODMDIG_H__
+
+struct dig_t { /* _Dynamic_Initial_Gain_Threshold_ */
+	bool bStopDIG;
+	bool bPSDInProgress;
+
+	u8 Dig_Enable_Flag;
+	u8 Dig_Ext_Port_Stage;
+
+	int RssiLowThresh;
+	int RssiHighThresh;
+
+	u32 FALowThresh;
+	u32 FAHighThresh;
+
+	u8 CurSTAConnectState;
+	u8 PreSTAConnectState;
+	u8 CurMultiSTAConnectState;
+
+	u8 PreIGValue;
+	u8 CurIGValue;
+	u8 BackupIGValue;		/* MP DIG */
+	u8 BT30_CurIGI;
+	u8 IGIBackup;
+
+	s8 BackoffVal;
+	s8 BackoffVal_range_max;
+	s8 BackoffVal_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 Rssi_val_min;
+
+	u8 PreCCK_CCAThres;
+	u8 CurCCK_CCAThres;
+	u8 PreCCKPDState;
+	u8 CurCCKPDState;
+	u8 CCKPDBackup;
+
+	u8 LargeFAHit;
+	u8 ForbiddenIGI;
+	u32 Recover_cnt;
+
+	u8 DIG_Dynamic_MIN_0;
+	u8 DIG_Dynamic_MIN_1;
+	bool bMediaConnect_0;
+	bool bMediaConnect_1;
+
+	u32 AntDiv_RSSI_max;
+	u32 RSSI_max;
+
+	u8 *pbP2pLinkInProgress;
+};
+
+struct  false_ALARM_STATISTICS {
+	u32 Cnt_Parity_Fail;
+	u32 Cnt_Rate_Illegal;
+	u32 Cnt_Crc8_fail;
+	u32 Cnt_Mcs_fail;
+	u32 Cnt_Ofdm_fail;
+	u32 Cnt_Ofdm_fail_pre; /* For RTL8881A */
+	u32 Cnt_Cck_fail;
+	u32 Cnt_all;
+	u32 Cnt_Fast_Fsync;
+	u32 Cnt_SB_Search_fail;
+	u32 Cnt_OFDM_CCA;
+	u32 Cnt_CCK_CCA;
+	u32 Cnt_CCA_all;
+	u32 Cnt_BW_USC; /* Gary */
+	u32 Cnt_BW_LSC; /* Gary */
+};
+
+enum ODM_Pause_DIG_TYPE {
+	ODM_PAUSE_DIG = BIT0,
+	ODM_RESUME_DIG = BIT1
+};
+
+#define		DM_DIG_THRESH_HIGH			40
+#define		DM_DIG_THRESH_LOW			35
+
+#define		DMfalseALARM_THRESH_LOW	400
+#define		DMfalseALARM_THRESH_HIGH	1000
+
+#define		DM_DIG_MAX_NIC				0x3e
+#define		DM_DIG_MIN_NIC				0x1e /* 0x22//0x1c */
+#define		DM_DIG_MAX_OF_MIN_NIC		0x3e
+
+#define		DM_DIG_MAX_AP					0x3e
+#define		DM_DIG_MIN_AP					0x1c
+#define		DM_DIG_MAX_OF_MIN			0x2A	/* 0x32 */
+#define		DM_DIG_MIN_AP_DFS				0x20
+
+#define		DM_DIG_MAX_NIC_HP			0x46
+#define		DM_DIG_MIN_NIC_HP				0x2e
+
+#define		DM_DIG_MAX_AP_HP				0x42
+#define		DM_DIG_MIN_AP_HP				0x30
+
+#define		DM_DIG_FA_TH0				0x200/* 0x20 */
+
+#define		DM_DIG_FA_TH1					0x300
+#define		DM_DIG_FA_TH2					0x400
+/* this is for 92d */
+#define		DM_DIG_FA_TH0_92D				0x100
+#define		DM_DIG_FA_TH1_92D				0x400
+#define		DM_DIG_FA_TH2_92D				0x600
+
+#define		DM_DIG_BACKOFF_MAX			12
+#define		DM_DIG_BACKOFF_MIN			-4
+#define		DM_DIG_BACKOFF_DEFAULT		10
+
+#define			DM_DIG_FA_TH0_LPS				4 /*  4 in lps */
+#define			DM_DIG_FA_TH1_LPS				15 /*  15 lps */
+#define			DM_DIG_FA_TH2_LPS				30 /*  30 lps */
+#define			RSSI_OFFSET_DIG				0x05
+
+void odm_NHMCounterStatisticsInit(void *pDM_VOID);
+
+void odm_NHMCounterStatistics(void *pDM_VOID);
+
+void odm_NHMBBInit(void *pDM_VOID);
+
+void odm_NHMBB(void *pDM_VOID);
+
+void odm_NHMCounterStatisticsReset(void *pDM_VOID);
+
+void odm_GetNHMCounterStatistics(void *pDM_VOID);
+
+void odm_SearchPwdBLowerBound(void *pDM_VOID, u8 IGI_target);
+
+void odm_AdaptivityInit(void *pDM_VOID);
+
+void odm_Adaptivity(void *pDM_VOID, u8 IGI);
+
+void ODM_Write_DIG(void *pDM_VOID, u8 CurrentIGI);
+
+void odm_DIGInit(void *pDM_VOID);
+
+void odm_DIG(void *pDM_VOID);
+
+void odm_DIGbyRSSI_LPS(void *pDM_VOID);
+
+void odm_FalseAlarmCounterStatistics(void *pDM_VOID);
+
+void odm_FAThresholdCheck(
+	void *pDM_VOID,
+	bool bDFSBand,
+	bool bPerformance,
+	u32 RxTp,
+	u32 TxTp,
+	u32 *dm_FA_thres
+);
+
+u8 odm_ForbiddenIGICheck(void *pDM_VOID, u8 DIG_Dynamic_MIN, u8 CurrentIGI);
+
+bool odm_DigAbort(void *pDM_VOID);
+
+void odm_CCKPacketDetectionThresh(void *pDM_VOID);
+
+void ODM_Write_CCK_CCA_Thres(void *pDM_VOID, u8 CurCCK_CCAThres);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.c b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.c
new file mode 100644
index 000000000..57c573652
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.c
@@ -0,0 +1,81 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_DynamicBBPowerSavingInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct ps_t *pDM_PSTable = &pDM_Odm->DM_PSTable;
+
+	pDM_PSTable->PreCCAState = CCA_MAX;
+	pDM_PSTable->CurCCAState = CCA_MAX;
+	pDM_PSTable->PreRFState = RF_MAX;
+	pDM_PSTable->CurRFState = RF_MAX;
+	pDM_PSTable->Rssi_val_min = 0;
+	pDM_PSTable->initialize = 0;
+}
+
+void ODM_RF_Saving(void *pDM_VOID, u8 bForceInNormal)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct ps_t *pDM_PSTable = &pDM_Odm->DM_PSTable;
+	u8 Rssi_Up_bound = 30;
+	u8 Rssi_Low_bound = 25;
+
+	if (pDM_Odm->PatchID == 40) { /* RT_CID_819x_FUNAI_TV */
+		Rssi_Up_bound = 50;
+		Rssi_Low_bound = 45;
+	}
+
+	if (pDM_PSTable->initialize == 0) {
+
+		pDM_PSTable->Reg874 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0x874, bMaskDWord)&0x1CC000)>>14;
+		pDM_PSTable->RegC70 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0xc70, bMaskDWord)&BIT3)>>3;
+		pDM_PSTable->Reg85C = (PHY_QueryBBReg(pDM_Odm->Adapter, 0x85c, bMaskDWord)&0xFF000000)>>24;
+		pDM_PSTable->RegA74 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0xa74, bMaskDWord)&0xF000)>>12;
+		/* Reg818 = PHY_QueryBBReg(padapter, 0x818, bMaskDWord); */
+		pDM_PSTable->initialize = 1;
+	}
+
+	if (!bForceInNormal) {
+		if (pDM_Odm->RSSI_Min != 0xFF) {
+			if (pDM_PSTable->PreRFState == RF_Normal) {
+				if (pDM_Odm->RSSI_Min >= Rssi_Up_bound)
+					pDM_PSTable->CurRFState = RF_Save;
+				else
+					pDM_PSTable->CurRFState = RF_Normal;
+			} else {
+				if (pDM_Odm->RSSI_Min <= Rssi_Low_bound)
+					pDM_PSTable->CurRFState = RF_Normal;
+				else
+					pDM_PSTable->CurRFState = RF_Save;
+			}
+		} else
+			pDM_PSTable->CurRFState = RF_MAX;
+	} else
+		pDM_PSTable->CurRFState = RF_Normal;
+
+	if (pDM_PSTable->PreRFState != pDM_PSTable->CurRFState) {
+		if (pDM_PSTable->CurRFState == RF_Save) {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]=3'b010 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc70, BIT3, 0); /* RegC70[3]= 1'b0 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]= 0x63 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0xC000, 0x2); /* Reg874[15:14]=2'b10 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xa74, 0xF000, 0x3); /* RegA75[7:4]= 0x3 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x0); /* Reg818[28]= 1'b0 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x1); /* Reg818[28]= 1'b1 */
+		} else {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0x1CC000, pDM_PSTable->Reg874);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc70, BIT3, pDM_PSTable->RegC70);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xa74, 0xF000, pDM_PSTable->RegA74);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x0);
+		}
+		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.h b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.h
new file mode 100644
index 000000000..3ebbbfd1d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMDYNAMICBBPOWERSAVING_H__
+#define    __ODMDYNAMICBBPOWERSAVING_H__
+
+struct ps_t { /* _Dynamic_Power_Saving_ */
+	u8 PreCCAState;
+	u8 CurCCAState;
+
+	u8 PreRFState;
+	u8 CurRFState;
+
+	int Rssi_val_min;
+
+	u8 initialize;
+	u32 Reg874, RegC70, Reg85C, RegA74;
+
+};
+
+#define dm_RF_Saving ODM_RF_Saving
+
+void ODM_RF_Saving(void *pDM_VOID, u8 bForceInNormal);
+
+void odm_DynamicBBPowerSavingInit(void *pDM_VOID);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.c b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.c
new file mode 100644
index 000000000..398dfa134
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.c
@@ -0,0 +1,22 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_DynamicTxPowerInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	pdmpriv->bDynamicTxPowerEnable = false;
+
+	pdmpriv->LastDTPLvl = TxHighPwrLevel_Normal;
+	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.h b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.h
new file mode 100644
index 000000000..e2d244324
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMDYNAMICTXPOWER_H__
+#define    __ODMDYNAMICTXPOWER_H__
+
+#define		TX_POWER_NEAR_FIELD_THRESH_LVL2	74
+#define		TX_POWER_NEAR_FIELD_THRESH_LVL1	67
+#define		TX_POWER_NEAR_FIELD_THRESH_AP		0x3F
+#define		TX_POWER_NEAR_FIELD_THRESH_8812	60
+
+#define		TxHighPwrLevel_Normal		0
+#define		TxHighPwrLevel_Level1		1
+#define		TxHighPwrLevel_Level2		2
+#define		TxHighPwrLevel_BT1			3
+#define		TxHighPwrLevel_BT2			4
+#define		TxHighPwrLevel_15			5
+#define		TxHighPwrLevel_35			6
+#define		TxHighPwrLevel_50			7
+#define		TxHighPwrLevel_70			8
+#define		TxHighPwrLevel_100			9
+
+void odm_DynamicTxPowerInit(void *pDM_VOID);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.c b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.c
new file mode 100644
index 000000000..578d57126
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+static u32 edca_setting_DL_GMode[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, ALTEK_92SE	BROADCOM, LINK	ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP
+ */
+	0x4322, 0xa44f, 0x5e4322, 0xa42b, 0x5e4322, 0x4322,
+	0xa42b, 0x5ea42b, 0xa44f, 0x5e4322, 0x5ea42b
+};
+
+static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, REALTEK_92SE, BROADCOM, RALINK, ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP(DownLink/Tx)
+ */
+	0x5e4322, 0xa44f, 0x5e4322, 0x5ea32b, 0x5ea422,	0x5ea322,
+	0x3ea430, 0x5ea42b, 0x5ea44f, 0x5e4322, 0x5e4322};
+
+static u32 edca_setting_DL[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, REALTEK_92SE, BROADCOM, RALINK, ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP(UpLink/Rx)
+ */
+	0xa44f, 0x5ea44f, 0x5e4322, 0x5ea42b, 0xa44f, 0xa630,
+	0x5ea630, 0x5ea42b, 0xa44f, 0xa42b, 0xa42b};
+
+void ODM_EdcaTurboInit(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+
+	pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+	pDM_Odm->DM_EDCA_Table.bIsCurRDLState = false;
+	Adapter->recvpriv.bIsAnyNonBEPkts = false;
+}	/*  ODM_InitEdcaTurbo */
+
+void odm_EdcaTurboCheck(void *pDM_VOID)
+{
+	/*  In HW integration first stage, we provide 4 different handles to
+	 *  operate at the same time. In stage2/3, we need to prove universal
+	 *  interface and merge all HW dynamic mechanism.
+	 */
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+
+	if (!(pDM_Odm->SupportAbility & ODM_MAC_EDCA_TURBO))
+		return;
+
+	odm_EdcaTurboCheckCE(pDM_Odm);
+}	/*  odm_CheckEdcaTurbo */
+
+void odm_EdcaTurboCheckCE(void *pDM_VOID)
+{
+	struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(Adapter);
+	struct recv_priv *precvpriv = &(Adapter->recvpriv);
+	struct registry_priv *pregpriv = &Adapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &(Adapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 EDCA_BE_UL = 0x5ea42b;
+	u32 EDCA_BE_DL = 0x5ea42b;
+	u32 iot_peer = 0;
+	u8 wirelessmode = 0xFF;		/* invalid value */
+	u32 trafficIndex;
+	u32 edca_param;
+	u64	cur_tx_bytes = 0;
+	u64	cur_rx_bytes = 0;
+	u8 bbtchange = false;
+	u8 biasonrx = false;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	if (!pDM_Odm->bLinked) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	if (pregpriv->wifi_spec == 1) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	if (pDM_Odm->pwirelessmode)
+		wirelessmode = *(pDM_Odm->pwirelessmode);
+
+	iot_peer = pmlmeinfo->assoc_AP_vendor;
+
+	if (iot_peer >=  HT_IOT_PEER_MAX) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	/*  Check if the status needs to be changed. */
+	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
+		cur_tx_bytes = pdvobjpriv->traffic_stat.cur_tx_bytes;
+		cur_rx_bytes = pdvobjpriv->traffic_stat.cur_rx_bytes;
+
+		/* traffic, TX or RX */
+		if (biasonrx) {
+			if (cur_tx_bytes > (cur_rx_bytes << 2)) {
+				/*  Uplink TP is present. */
+				trafficIndex = UP_LINK;
+			} else { /*  Balance TP is present. */
+				trafficIndex = DOWN_LINK;
+			}
+		} else {
+			if (cur_rx_bytes > (cur_tx_bytes << 2)) {
+				/*  Downlink TP is present. */
+				trafficIndex = DOWN_LINK;
+			} else { /*  Balance TP is present. */
+				trafficIndex = UP_LINK;
+			}
+		}
+
+		/* 92D txop can't be set to 0x3e for cisco1250 */
+		if ((iot_peer == HT_IOT_PEER_CISCO) &&
+		    (wirelessmode == ODM_WM_N24G)) {
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+			EDCA_BE_UL = edca_setting_UL[iot_peer];
+		} else if ((iot_peer == HT_IOT_PEER_CISCO) &&
+			   ((wirelessmode == ODM_WM_G) ||
+			    (wirelessmode == (ODM_WM_B | ODM_WM_G)) ||
+			    (wirelessmode == ODM_WM_B))) {
+			EDCA_BE_DL = edca_setting_DL_GMode[iot_peer];
+		} else if ((iot_peer == HT_IOT_PEER_AIRGO) &&
+			   (wirelessmode == ODM_WM_G)) {
+			EDCA_BE_DL = 0xa630;
+		} else if (iot_peer == HT_IOT_PEER_MARVELL) {
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+			EDCA_BE_UL = edca_setting_UL[iot_peer];
+		} else if (iot_peer == HT_IOT_PEER_ATHEROS) {
+			/*  Set DL EDCA for Atheros peer to 0x3ea42b. */
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+		}
+
+		if (trafficIndex == DOWN_LINK)
+			edca_param = EDCA_BE_DL;
+		else
+			edca_param = EDCA_BE_UL;
+
+		rtw_write32(Adapter, REG_EDCA_BE_PARAM, edca_param);
+
+		pDM_Odm->DM_EDCA_Table.prv_traffic_idx = trafficIndex;
+
+		pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = true;
+	} else {
+		/*  Turn Off EDCA turbo here. */
+		/*  Restore original EDCA according to the declaration of AP. */
+		 if (pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA) {
+			rtw_write32(Adapter, REG_EDCA_BE_PARAM, pHalData->AcParam_BE);
+			pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+		}
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.h b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.h
new file mode 100644
index 000000000..e9f9f0722
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __ODMEDCATURBOCHECK_H__
+#define __ODMEDCATURBOCHECK_H__
+
+struct edca_t { /* _EDCA_TURBO_ */
+	bool bCurrentTurboEDCA;
+	bool bIsCurRDLState;
+
+	u32 prv_traffic_idx; /*  edca turbo */
+};
+
+void odm_EdcaTurboCheck(void *pDM_VOID);
+void ODM_EdcaTurboInit(void *pDM_VOID);
+
+void odm_EdcaTurboCheckCE(void *pDM_VOID);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_HWConfig.c b/drivers/staging/rtl8723bs/hal/odm_HWConfig.c
new file mode 100644
index 000000000..994b8c578
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_HWConfig.c
@@ -0,0 +1,446 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+#define READ_AND_CONFIG_MP(ic, txt) (ODM_ReadAndConfig_MP_##ic##txt(pDM_Odm))
+#define READ_AND_CONFIG     READ_AND_CONFIG_MP
+
+static u8 odm_query_rx_pwr_percentage(s8 ant_power)
+{
+	if ((ant_power <= -100) || (ant_power >= 20))
+		return	0;
+	else if (ant_power >= 0)
+		return	100;
+	else
+		return 100 + ant_power;
+
+}
+
+s32 odm_signal_scale_mapping(struct dm_odm_t *dm_odm, s32 curr_sig)
+{
+	s32 ret_sig = 0;
+
+	if (dm_odm->SupportInterface  == ODM_ITRF_SDIO) {
+		if (curr_sig >= 51 && curr_sig <= 100)
+			ret_sig = 100;
+		else if (curr_sig >= 41 && curr_sig <= 50)
+			ret_sig = 80 + ((curr_sig - 40)*2);
+		else if (curr_sig >= 31 && curr_sig <= 40)
+			ret_sig = 66 + (curr_sig - 30);
+		else if (curr_sig >= 21 && curr_sig <= 30)
+			ret_sig = 54 + (curr_sig - 20);
+		else if (curr_sig >= 10 && curr_sig <= 20)
+			ret_sig = 42 + (((curr_sig - 10) * 2) / 3);
+		else if (curr_sig >= 5 && curr_sig <= 9)
+			ret_sig = 22 + (((curr_sig - 5) * 3) / 2);
+		else if (curr_sig >= 1 && curr_sig <= 4)
+			ret_sig = 6 + (((curr_sig - 1) * 3) / 2);
+		else
+			ret_sig = curr_sig;
+	}
+
+	return ret_sig;
+}
+
+static u8 odm_evm_db_to_percentage(s8 value)
+{
+	/*  */
+	/*  -33dB~0dB to 0%~99% */
+	/*  */
+	s8 ret_val;
+
+	ret_val = value;
+	ret_val /= 2;
+
+	if (ret_val >= 0)
+		ret_val = 0;
+	if (ret_val <= -33)
+		ret_val = -33;
+
+	ret_val = 0 - ret_val;
+	ret_val *= 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+static s8 odm_cck_rssi(u8 lna_idx, u8 vga_idx)
+{
+	s8 rx_pwr_all = 0x00;
+
+	switch (lna_idx) {
+	/* 46  53 73 95 201301231630 */
+	/*  46 53 77 99 201301241630 */
+
+	case 6:
+		rx_pwr_all = -34 - (2 * vga_idx);
+		break;
+	case 4:
+		rx_pwr_all = -14 - (2 * vga_idx);
+		break;
+	case 1:
+		rx_pwr_all = 6 - (2 * vga_idx);
+		break;
+	case 0:
+		rx_pwr_all = 16 - (2 * vga_idx);
+		break;
+	default:
+		/* rx_pwr_all = -53+(2*(31-VGA_idx)); */
+		break;
+	}
+	return rx_pwr_all;
+}
+
+static void odm_rx_phy_status_parsing(struct dm_odm_t *dm_odm,
+				      struct odm_phy_info *phy_info,
+				      u8 *phy_status,
+				      struct odm_packet_info *pkt_info)
+{
+	u8 i;
+	s8 rx_pwr[4], rx_pwr_all = 0;
+	u8 evm, pwdb_all = 0, pwdb_all_bt;
+	u8 rssi, total_rssi = 0;
+	bool is_cck_rate = false;
+	u8 rf_rx_num = 0;
+	u8 lna_idx, vga_idx;
+	struct phy_status_rpt_8192cd_t *phy_sta_rpt = (struct phy_status_rpt_8192cd_t *)phy_status;
+
+	is_cck_rate = pkt_info->data_rate <= DESC_RATE11M;
+	phy_info->rx_mimo_signal_quality[RF_PATH_A] = -1;
+	phy_info->rx_mimo_signal_quality[RF_PATH_B] = -1;
+
+
+	if (is_cck_rate) {
+		u8 cck_agc_rpt;
+
+		dm_odm->PhyDbgInfo.NumQryPhyStatusCCK++;
+
+		/*
+		 * (1)Hardware does not provide RSSI for CCK/
+		 * (2)PWDB, Average PWDB calculated by
+		 *    hardware (for rate adaptive)
+		 */
+
+		cck_agc_rpt = phy_sta_rpt->cck_agc_rpt_ofdm_cfosho_a;
+
+		/*
+		 * 2011.11.28 LukeLee: 88E use different LNA & VGA gain table
+		 * The RSSI formula should be modified according to the gain table
+		 */
+		lna_idx = ((cck_agc_rpt & 0xE0)>>5);
+		vga_idx = (cck_agc_rpt & 0x1F);
+		rx_pwr_all = odm_cck_rssi(lna_idx, vga_idx);
+		pwdb_all = odm_query_rx_pwr_percentage(rx_pwr_all);
+		if (pwdb_all > 100)
+			pwdb_all = 100;
+
+		phy_info->rx_pwd_ba11 = pwdb_all;
+		phy_info->bt_rx_rssi_percentage = pwdb_all;
+		phy_info->recv_signal_power = rx_pwr_all;
+
+		/*  (3) Get Signal Quality (EVM) */
+
+		/* if (pPktinfo->bPacketMatchBSSID) */
+		{
+			u8 sq, sq_rpt;
+
+			if (phy_info->rx_pwd_ba11 > 40 && !dm_odm->bInHctTest)
+				sq = 100;
+			else {
+				sq_rpt = phy_sta_rpt->cck_sig_qual_ofdm_pwdb_all;
+
+				if (sq_rpt > 64)
+					sq = 0;
+				else if (sq_rpt < 20)
+					sq = 100;
+				else
+					sq = ((64-sq_rpt) * 100) / 44;
+
+			}
+
+			phy_info->signal_quality = sq;
+			phy_info->rx_mimo_signal_quality[RF_PATH_A] = sq;
+			phy_info->rx_mimo_signal_quality[RF_PATH_B] = -1;
+		}
+	} else { /* is OFDM rate */
+		dm_odm->PhyDbgInfo.NumQryPhyStatusOFDM++;
+
+		/*
+		 * (1)Get RSSI for HT rate
+		 */
+
+		for (i = RF_PATH_A; i < RF_PATH_MAX; i++) {
+			/*  2008/01/30 MH we will judge RF RX path now. */
+			if (dm_odm->RFPathRxEnable & BIT(i))
+				rf_rx_num++;
+			/* else */
+				/* continue; */
+
+			rx_pwr[i] = ((phy_sta_rpt->path_agc[i].gain & 0x3F) * 2) - 110;
+
+			phy_info->rx_pwr[i] = rx_pwr[i];
+
+			/* Translate DBM to percentage. */
+			rssi = odm_query_rx_pwr_percentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			phy_info->rx_mimo_signal_strength[i] = (u8)rssi;
+
+			/* Get Rx snr value in DB */
+			phy_info->rx_snr[i] = dm_odm->PhyDbgInfo.RxSNRdB[i] = (s32)(phy_sta_rpt->path_rxsnr[i]/2);
+		}
+
+		/*
+		 * (2)PWDB, Average PWDB calculated by hardware (for rate adaptive)
+		 */
+		rx_pwr_all = ((phy_sta_rpt->cck_sig_qual_ofdm_pwdb_all >> 1) & 0x7f) - 110;
+
+		pwdb_all_bt = pwdb_all = odm_query_rx_pwr_percentage(rx_pwr_all);
+
+		phy_info->rx_pwd_ba11 = pwdb_all;
+		phy_info->bt_rx_rssi_percentage = pwdb_all_bt;
+		phy_info->rx_power = rx_pwr_all;
+		phy_info->recv_signal_power = rx_pwr_all;
+
+		/*
+		 * (3)EVM of HT rate
+		 *
+		 * Only spatial stream 1 makes sense
+		 *
+		 * Do not use shift operation like "rx_evmX >>= 1"
+		 * because the compiler of free build environment
+		 * fill most significant bit to "zero" when doing
+		 * shifting operation which may change a negative
+		 * value to positive one, then the dbm value (which
+		 * is supposed to be negative) is not correct
+		 * anymore.
+		 */
+		evm = odm_evm_db_to_percentage(phy_sta_rpt->stream_rxevm[0]); /* dbm */
+
+		/*  Fill value in RFD, Get the first spatial stream only */
+		phy_info->signal_quality = (u8)(evm & 0xff);
+
+		phy_info->rx_mimo_signal_quality[RF_PATH_A] = (u8)(evm & 0xff);
+
+		odm_parsing_cfo(dm_odm, pkt_info, phy_sta_rpt->path_cfotail);
+	}
+
+	/*
+	 * UI BSS List signal strength(in percentage), make it good
+	 * looking, from 0~100.
+	 * It is assigned to the BSS List in GetValueFromBeaconOrProbeRsp().
+	 */
+	if (is_cck_rate) {
+		phy_info->signal_strength = (u8)(odm_signal_scale_mapping(dm_odm, pwdb_all));
+	} else {
+		if (rf_rx_num != 0) {
+			phy_info->signal_strength = (u8)(odm_signal_scale_mapping(dm_odm, total_rssi /= rf_rx_num));
+		}
+	}
+}
+
+static void odm_Process_RSSIForDM(
+	struct dm_odm_t *pDM_Odm, struct odm_phy_info *pPhyInfo, struct odm_packet_info *pPktinfo
+)
+{
+
+	s32 UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK, UndecoratedSmoothedOFDM, RSSI_Ave;
+	u8 isCCKrate = 0;
+	u8 RSSI_max, RSSI_min, i;
+	u32 OFDM_pkt = 0;
+	u32 Weighting = 0;
+	PSTA_INFO_T pEntry;
+
+
+	if (pPktinfo->station_id == 0xFF)
+		return;
+
+	pEntry = pDM_Odm->pODM_StaInfo[pPktinfo->station_id];
+
+	if (!IS_STA_VALID(pEntry))
+		return;
+
+	if ((!pPktinfo->bssid_match))
+		return;
+
+	if (pPktinfo->is_beacon)
+		pDM_Odm->PhyDbgInfo.NumQryBeaconPkt++;
+
+	isCCKrate = ((pPktinfo->data_rate <= DESC_RATE11M)) ? true : false;
+	pDM_Odm->RxRate = pPktinfo->data_rate;
+
+	/* Statistic for antenna/path diversity------------------ */
+	if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
+
+	}
+
+	/* Smart Antenna Debug Message------------------ */
+
+	UndecoratedSmoothedCCK = pEntry->rssi_stat.UndecoratedSmoothedCCK;
+	UndecoratedSmoothedOFDM = pEntry->rssi_stat.UndecoratedSmoothedOFDM;
+	UndecoratedSmoothedPWDB = pEntry->rssi_stat.UndecoratedSmoothedPWDB;
+
+	if (pPktinfo->to_self || pPktinfo->is_beacon) {
+
+		if (!isCCKrate) { /* ofdm rate */
+			if (pPhyInfo->rx_mimo_signal_strength[RF_PATH_B] == 0) {
+				RSSI_Ave = pPhyInfo->rx_mimo_signal_strength[RF_PATH_A];
+				pDM_Odm->RSSI_A = pPhyInfo->rx_mimo_signal_strength[RF_PATH_A];
+				pDM_Odm->RSSI_B = 0;
+			} else {
+				pDM_Odm->RSSI_A =  pPhyInfo->rx_mimo_signal_strength[RF_PATH_A];
+				pDM_Odm->RSSI_B = pPhyInfo->rx_mimo_signal_strength[RF_PATH_B];
+
+				if (
+					pPhyInfo->rx_mimo_signal_strength[RF_PATH_A] >
+					pPhyInfo->rx_mimo_signal_strength[RF_PATH_B]
+				) {
+					RSSI_max = pPhyInfo->rx_mimo_signal_strength[RF_PATH_A];
+					RSSI_min = pPhyInfo->rx_mimo_signal_strength[RF_PATH_B];
+				} else {
+					RSSI_max = pPhyInfo->rx_mimo_signal_strength[RF_PATH_B];
+					RSSI_min = pPhyInfo->rx_mimo_signal_strength[RF_PATH_A];
+				}
+
+				if ((RSSI_max-RSSI_min) < 3)
+					RSSI_Ave = RSSI_max;
+				else if ((RSSI_max-RSSI_min) < 6)
+					RSSI_Ave = RSSI_max - 1;
+				else if ((RSSI_max-RSSI_min) < 10)
+					RSSI_Ave = RSSI_max - 2;
+				else
+					RSSI_Ave = RSSI_max - 3;
+			}
+
+			/* 1 Process OFDM RSSI */
+			if (UndecoratedSmoothedOFDM <= 0)	/*  initialize */
+				UndecoratedSmoothedOFDM = pPhyInfo->rx_pwd_ba11;
+			else {
+				if (pPhyInfo->rx_pwd_ba11 > (u32)UndecoratedSmoothedOFDM) {
+					UndecoratedSmoothedOFDM =
+							((UndecoratedSmoothedOFDM*(Rx_Smooth_Factor-1)) +
+							RSSI_Ave)/Rx_Smooth_Factor;
+					UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM + 1;
+				} else {
+					UndecoratedSmoothedOFDM =
+							((UndecoratedSmoothedOFDM*(Rx_Smooth_Factor-1)) +
+							RSSI_Ave)/Rx_Smooth_Factor;
+				}
+			}
+
+			pEntry->rssi_stat.PacketMap = (pEntry->rssi_stat.PacketMap<<1) | BIT0;
+
+		} else {
+			RSSI_Ave = pPhyInfo->rx_pwd_ba11;
+			pDM_Odm->RSSI_A = (u8) pPhyInfo->rx_pwd_ba11;
+			pDM_Odm->RSSI_B = 0;
+
+			/* 1 Process CCK RSSI */
+			if (UndecoratedSmoothedCCK <= 0)	/*  initialize */
+				UndecoratedSmoothedCCK = pPhyInfo->rx_pwd_ba11;
+			else {
+				if (pPhyInfo->rx_pwd_ba11 > (u32)UndecoratedSmoothedCCK) {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK*(Rx_Smooth_Factor-1)) +
+							pPhyInfo->rx_pwd_ba11)/Rx_Smooth_Factor;
+					UndecoratedSmoothedCCK = UndecoratedSmoothedCCK + 1;
+				} else {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK*(Rx_Smooth_Factor-1)) +
+							pPhyInfo->rx_pwd_ba11)/Rx_Smooth_Factor;
+				}
+			}
+			pEntry->rssi_stat.PacketMap = pEntry->rssi_stat.PacketMap<<1;
+		}
+
+		/* if (pEntry) */
+		{
+			/* 2011.07.28 LukeLee: modified to prevent unstable CCK RSSI */
+			if (pEntry->rssi_stat.ValidBit >= 64)
+				pEntry->rssi_stat.ValidBit = 64;
+			else
+				pEntry->rssi_stat.ValidBit++;
+
+			for (i = 0; i < pEntry->rssi_stat.ValidBit; i++)
+				OFDM_pkt += (u8)(pEntry->rssi_stat.PacketMap>>i)&BIT0;
+
+			if (pEntry->rssi_stat.ValidBit == 64) {
+				Weighting = ((OFDM_pkt<<4) > 64)?64:(OFDM_pkt<<4);
+				UndecoratedSmoothedPWDB = (Weighting*UndecoratedSmoothedOFDM+(64-Weighting)*UndecoratedSmoothedCCK)>>6;
+			} else {
+				if (pEntry->rssi_stat.ValidBit != 0)
+					UndecoratedSmoothedPWDB = (OFDM_pkt*UndecoratedSmoothedOFDM+(pEntry->rssi_stat.ValidBit-OFDM_pkt)*UndecoratedSmoothedCCK)/pEntry->rssi_stat.ValidBit;
+				else
+					UndecoratedSmoothedPWDB = 0;
+			}
+
+			pEntry->rssi_stat.UndecoratedSmoothedCCK = UndecoratedSmoothedCCK;
+			pEntry->rssi_stat.UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM;
+			pEntry->rssi_stat.UndecoratedSmoothedPWDB = UndecoratedSmoothedPWDB;
+		}
+
+	}
+}
+
+
+/*  */
+/*  Endianness before calling this API */
+/*  */
+void odm_phy_status_query(struct dm_odm_t *dm_odm, struct odm_phy_info *phy_info,
+			  u8 *phy_status, struct odm_packet_info *pkt_info)
+{
+
+	odm_rx_phy_status_parsing(dm_odm, phy_info, phy_status, pkt_info);
+
+	if (!dm_odm->RSSI_test)
+		odm_Process_RSSIForDM(dm_odm, phy_info, pkt_info);
+}
+
+/*  */
+/*  If you want to add a new IC, Please follow below template and generate a new one. */
+/*  */
+/*  */
+
+enum hal_status ODM_ConfigRFWithHeaderFile(
+	struct dm_odm_t *pDM_Odm,
+	enum ODM_RF_Config_Type ConfigType,
+	enum rf_path eRFPath
+)
+{
+	if (ConfigType == CONFIG_RF_RADIO)
+		READ_AND_CONFIG(8723B, _RadioA);
+	else if (ConfigType == CONFIG_RF_TXPWR_LMT)
+		READ_AND_CONFIG(8723B, _TXPWR_LMT);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+enum hal_status ODM_ConfigRFWithTxPwrTrackHeaderFile(struct dm_odm_t *pDM_Odm)
+{
+	if (pDM_Odm->SupportInterface == ODM_ITRF_SDIO)
+		READ_AND_CONFIG(8723B, _TxPowerTrack_SDIO);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+enum hal_status ODM_ConfigBBWithHeaderFile(
+	struct dm_odm_t *pDM_Odm, enum ODM_BB_Config_Type ConfigType
+)
+{
+	if (ConfigType == CONFIG_BB_PHY_REG)
+		READ_AND_CONFIG(8723B, _PHY_REG);
+	else if (ConfigType == CONFIG_BB_AGC_TAB)
+		READ_AND_CONFIG(8723B, _AGC_TAB);
+	else if (ConfigType == CONFIG_BB_PHY_REG_PG)
+		READ_AND_CONFIG(8723B, _PHY_REG_PG);
+
+	return HAL_STATUS_SUCCESS;
+}
+
diff --git a/drivers/staging/rtl8723bs/hal/odm_HWConfig.h b/drivers/staging/rtl8723bs/hal/odm_HWConfig.h
new file mode 100644
index 000000000..0c3697c38
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_HWConfig.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+
+#ifndef __HALHWOUTSRC_H__
+#define __HALHWOUTSRC_H__
+
+/*  */
+/*  structure and define */
+/*  */
+
+struct phy_rx_agc_info_t {
+	#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+		u8 gain:7, trsw:1;
+	#else
+		u8 trsw:1, gain:7;
+	#endif
+};
+
+struct phy_status_rpt_8192cd_t {
+	struct phy_rx_agc_info_t path_agc[2];
+	u8 ch_corr[2];
+	u8 cck_sig_qual_ofdm_pwdb_all;
+	u8 cck_agc_rpt_ofdm_cfosho_a;
+	u8 cck_rpt_b_ofdm_cfosho_b;
+	u8 rsvd_1;/* ch_corr_msb; */
+	u8 noise_power_db_msb;
+	s8 path_cfotail[2];
+	u8 pcts_mask[2];
+	s8 stream_rxevm[2];
+	u8 path_rxsnr[2];
+	u8 noise_power_db_lsb;
+	u8 rsvd_2[3];
+	u8 stream_csi[2];
+	u8 stream_target_csi[2];
+	s8	sig_evm;
+	u8 rsvd_3;
+
+#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+	u8 antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 idle_long:1;
+	u8 r_ant_train_en:1;
+	u8 ant_sel_b:1;
+	u8 ant_sel:1;
+#else	/*  _BIG_ENDIAN_ */
+	u8 ant_sel:1;
+	u8 ant_sel_b:1;
+	u8 r_ant_train_en:1;
+	u8 idle_long:1;
+	u8 rxsc:2;
+	u8 sgi_en:1;
+	u8 antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+#endif
+};
+
+void odm_phy_status_query(struct dm_odm_t *dm_odm, struct odm_phy_info *phy_info,
+			  u8 *phy_status, struct odm_packet_info *pkt_info);
+
+enum hal_status ODM_ConfigRFWithTxPwrTrackHeaderFile(struct dm_odm_t *pDM_Odm);
+
+enum hal_status ODM_ConfigRFWithHeaderFile(
+	struct dm_odm_t *pDM_Odm,
+	enum ODM_RF_Config_Type ConfigType,
+	enum rf_path eRFPath
+);
+
+enum hal_status ODM_ConfigBBWithHeaderFile(
+	struct dm_odm_t *pDM_Odm, enum ODM_BB_Config_Type ConfigType
+);
+
+enum hal_status ODM_ConfigFWWithHeaderFile(
+	struct dm_odm_t *pDM_Odm,
+	enum ODM_FW_Config_Type ConfigType,
+	u8 *pFirmware,
+	u32 *pSize
+);
+
+s32 odm_signal_scale_mapping(struct dm_odm_t *pDM_Odm, s32 CurrSig);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c
new file mode 100644
index 000000000..1df42069b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c
@@ -0,0 +1,179 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_ConfigRFReg_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u32 Addr,
+	u32 Data,
+	enum rf_path RF_PATH,
+	u32 RegAddr
+)
+{
+	if (Addr == 0xfe || Addr == 0xffe)
+		msleep(50);
+	else {
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH, RegAddr, bRFRegOffsetMask, Data);
+		/*  Add 1us delay between BB/RF register setting. */
+		udelay(1);
+
+		/* For disable/enable test in high temperature, the B6 value will fail to fill. Suggestion by BB Stanley, 2013.06.25. */
+		if (Addr == 0xb6) {
+			u32 getvalue = 0;
+			u8 count = 0;
+
+			getvalue = PHY_QueryRFReg(
+				pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+			);
+
+			udelay(1);
+
+			while ((getvalue>>8) != (Data>>8)) {
+				count++;
+				PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH, RegAddr, bRFRegOffsetMask, Data);
+				udelay(1);
+				getvalue = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord);
+				if (count > 5)
+					break;
+			}
+		}
+
+		if (Addr == 0xb2) {
+			u32 getvalue = 0;
+			u8 count = 0;
+
+			getvalue = PHY_QueryRFReg(
+				pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+			);
+
+			udelay(1);
+
+			while (getvalue != Data) {
+				count++;
+				PHY_SetRFReg(
+					pDM_Odm->Adapter,
+					RF_PATH,
+					RegAddr,
+					bRFRegOffsetMask,
+					Data
+				);
+				udelay(1);
+				/* Do LCK againg */
+				PHY_SetRFReg(
+					pDM_Odm->Adapter,
+					RF_PATH,
+					0x18,
+					bRFRegOffsetMask,
+					0x0fc07
+				);
+				udelay(1);
+				getvalue = PHY_QueryRFReg(
+					pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+				);
+
+				if (count > 5)
+					break;
+			}
+		}
+	}
+}
+
+
+void odm_ConfigRF_RadioA_8723B(struct dm_odm_t *pDM_Odm, u32 Addr, u32 Data)
+{
+	u32  content = 0x1000; /*  RF_Content: radioa_txt */
+	u32 maskforPhySet = (u32)(content&0xE000);
+
+	odm_ConfigRFReg_8723B(
+		pDM_Odm,
+		Addr,
+		Data,
+		RF_PATH_A,
+		Addr|maskforPhySet
+	);
+}
+
+void odm_ConfigMAC_8723B(struct dm_odm_t *pDM_Odm, u32 Addr, u8 Data)
+{
+	rtw_write8(pDM_Odm->Adapter, Addr, Data);
+}
+
+void odm_ConfigBB_AGC_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+	/*  Add 1us delay between BB/RF register setting. */
+	udelay(1);
+}
+
+void odm_ConfigBB_PHY_REG_PG_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u32 RfPath,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	if (Addr == 0xfe || Addr == 0xffe)
+		msleep(50);
+	else {
+		PHY_StoreTxPowerByRate(pDM_Odm->Adapter, RfPath, Addr, Bitmask, Data);
+	}
+}
+
+void odm_ConfigBB_PHY_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	if (Addr == 0xfe)
+		msleep(50);
+	else if (Addr == 0xfd)
+		mdelay(5);
+	else if (Addr == 0xfc)
+		mdelay(1);
+	else if (Addr == 0xfb)
+		udelay(50);
+	else if (Addr == 0xfa)
+		udelay(5);
+	else if (Addr == 0xf9)
+		udelay(1);
+	else {
+		PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+	}
+
+	/*  Add 1us delay between BB/RF register setting. */
+	udelay(1);
+}
+
+void odm_ConfigBB_TXPWR_LMT_8723B(
+	struct dm_odm_t *pDM_Odm,
+	u8 *Regulation,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+)
+{
+	PHY_SetTxPowerLimit(
+		pDM_Odm->Adapter,
+		Regulation,
+		Bandwidth,
+		RateSection,
+		RfPath,
+		Channel,
+		PowerLimit
+	);
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.h b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.h
new file mode 100644
index 000000000..fba7053ee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __INC_ODM_REGCONFIG_H_8723B
+#define __INC_ODM_REGCONFIG_H_8723B
+
+void odm_ConfigRFReg_8723B(struct dm_odm_t *pDM_Odm,
+			   u32 Addr,
+			   u32 Data,
+			   enum rf_path RF_PATH,
+			   u32 RegAddr
+);
+
+void odm_ConfigRF_RadioA_8723B(struct dm_odm_t *pDM_Odm, u32 Addr, u32 Data);
+
+void odm_ConfigMAC_8723B(struct dm_odm_t *pDM_Odm, u32 Addr, u8 Data);
+
+void odm_ConfigBB_AGC_8723B(struct dm_odm_t *pDM_Odm,
+			    u32 Addr,
+			    u32 Bitmask,
+			    u32 Data
+);
+
+void odm_ConfigBB_PHY_REG_PG_8723B(struct dm_odm_t *pDM_Odm, u32 RfPath, u32 Addr,
+				   u32 Bitmask, u32 Data);
+
+void odm_ConfigBB_PHY_8723B(struct dm_odm_t *pDM_Odm,
+			    u32 Addr,
+			    u32 Bitmask,
+			    u32 Data
+);
+
+void odm_ConfigBB_TXPWR_LMT_8723B(struct dm_odm_t *pDM_Odm,
+				  u8 *Regulation,
+				  u8 *Bandwidth,
+				  u8 *RateSection,
+				  u8 *RfPath,
+				  u8 *Channel,
+				  u8 *PowerLimit
+);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h b/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
new file mode 100644
index 000000000..3c7193889
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODM_REGDEFINE11N_H__
+#define __ODM_REGDEFINE11N_H__
+
+
+/* 2 RF REG LIST */
+#define	ODM_REG_RF_MODE_11N				0x00
+#define	ODM_REG_RF_0B_11N				0x0B
+#define	ODM_REG_CHNBW_11N				0x18
+#define	ODM_REG_T_METER_11N				0x24
+#define	ODM_REG_RF_25_11N				0x25
+#define	ODM_REG_RF_26_11N				0x26
+#define	ODM_REG_RF_27_11N				0x27
+#define	ODM_REG_RF_2B_11N				0x2B
+#define	ODM_REG_RF_2C_11N				0x2C
+#define	ODM_REG_RXRF_A3_11N				0x3C
+#define	ODM_REG_T_METER_92D_11N			0x42
+#define	ODM_REG_T_METER_88E_11N			0x42
+
+/* 2 BB REG LIST */
+/* PAGE 8 */
+#define	ODM_REG_BB_CTRL_11N				0x800
+#define	ODM_REG_RF_PIN_11N				0x804
+#define	ODM_REG_PSD_CTRL_11N				0x808
+#define	ODM_REG_TX_ANT_CTRL_11N			0x80C
+#define	ODM_REG_BB_PWR_SAV5_11N		0x818
+#define	ODM_REG_CCK_RPT_FORMAT_11N		0x824
+#define	ODM_REG_RX_DEFAULT_A_11N		0x858
+#define	ODM_REG_RX_DEFAULT_B_11N		0x85A
+#define	ODM_REG_BB_PWR_SAV3_11N		0x85C
+#define	ODM_REG_ANTSEL_CTRL_11N			0x860
+#define	ODM_REG_RX_ANT_CTRL_11N			0x864
+#define	ODM_REG_PIN_CTRL_11N				0x870
+#define	ODM_REG_BB_PWR_SAV1_11N		0x874
+#define	ODM_REG_ANTSEL_PATH_11N			0x878
+#define	ODM_REG_BB_3WIRE_11N			0x88C
+#define	ODM_REG_SC_CNT_11N				0x8C4
+#define	ODM_REG_PSD_DATA_11N				0x8B4
+#define	ODM_REG_PSD_DATA_11N				0x8B4
+#define	ODM_REG_NHM_TIMER_11N			0x894
+#define	ODM_REG_NHM_TH9_TH10_11N		0x890
+#define	ODM_REG_NHM_TH3_TO_TH0_11N		0x898
+#define	ODM_REG_NHM_TH7_TO_TH4_11N		0x89c
+#define	ODM_REG_NHM_CNT_11N				0x8d8
+/* PAGE 9 */
+#define	ODM_REG_DBG_RPT_11N				0x908
+#define	ODM_REG_ANT_MAPPING1_11N		0x914
+#define	ODM_REG_ANT_MAPPING2_11N		0x918
+/* PAGE A */
+#define	ODM_REG_CCK_ANTDIV_PARA1_11N	0xA00
+#define	ODM_REG_CCK_CCA_11N				0xA0A
+#define	ODM_REG_CCK_ANTDIV_PARA2_11N	0xA0C
+#define	ODM_REG_CCK_ANTDIV_PARA3_11N	0xA10
+#define	ODM_REG_CCK_ANTDIV_PARA4_11N	0xA14
+#define	ODM_REG_CCK_FILTER_PARA1_11N	0xA22
+#define	ODM_REG_CCK_FILTER_PARA2_11N	0xA23
+#define	ODM_REG_CCK_FILTER_PARA3_11N	0xA24
+#define	ODM_REG_CCK_FILTER_PARA4_11N	0xA25
+#define	ODM_REG_CCK_FILTER_PARA5_11N	0xA26
+#define	ODM_REG_CCK_FILTER_PARA6_11N	0xA27
+#define	ODM_REG_CCK_FILTER_PARA7_11N	0xA28
+#define	ODM_REG_CCK_FILTER_PARA8_11N	0xA29
+#define	ODM_REG_CCK_FA_RST_11N			0xA2C
+#define	ODM_REG_CCK_FA_MSB_11N			0xA58
+#define	ODM_REG_CCK_FA_LSB_11N			0xA5C
+#define	ODM_REG_CCK_CCA_CNT_11N			0xA60
+#define	ODM_REG_BB_PWR_SAV4_11N		0xA74
+/* PAGE B */
+#define	ODM_REG_LNA_SWITCH_11N			0xB2C
+#define	ODM_REG_PATH_SWITCH_11N			0xB30
+#define	ODM_REG_RSSI_CTRL_11N			0xB38
+#define	ODM_REG_CONFIG_ANTA_11N			0xB68
+#define	ODM_REG_RSSI_BT_11N				0xB9C
+/* PAGE C */
+#define	ODM_REG_OFDM_FA_HOLDC_11N		0xC00
+#define	ODM_REG_BB_RX_PATH_11N			0xC04
+#define	ODM_REG_TRMUX_11N				0xC08
+#define	ODM_REG_OFDM_FA_RSTC_11N		0xC0C
+#define	ODM_REG_RXIQI_MATRIX_11N			0xC14
+#define	ODM_REG_TXIQK_MATRIX_LSB1_11N	0xC4C
+#define	ODM_REG_IGI_A_11N					0xC50
+#define	ODM_REG_ANTDIV_PARA2_11N		0xC54
+#define	ODM_REG_IGI_B_11N					0xC58
+#define	ODM_REG_ANTDIV_PARA3_11N		0xC5C
+#define   ODM_REG_L1SBD_PD_CH_11N			0XC6C
+#define	ODM_REG_BB_PWR_SAV2_11N		0xC70
+#define	ODM_REG_RX_OFF_11N				0xC7C
+#define	ODM_REG_TXIQK_MATRIXA_11N		0xC80
+#define	ODM_REG_TXIQK_MATRIXB_11N		0xC88
+#define	ODM_REG_TXIQK_MATRIXA_LSB2_11N	0xC94
+#define	ODM_REG_TXIQK_MATRIXB_LSB2_11N	0xC9C
+#define	ODM_REG_RXIQK_MATRIX_LSB_11N	0xCA0
+#define	ODM_REG_ANTDIV_PARA1_11N		0xCA4
+#define	ODM_REG_OFDM_FA_TYPE1_11N		0xCF0
+/* PAGE D */
+#define	ODM_REG_OFDM_FA_RSTD_11N		0xD00
+#define	ODM_REG_BB_ATC_11N				0xD2C
+#define	ODM_REG_OFDM_FA_TYPE2_11N		0xDA0
+#define	ODM_REG_OFDM_FA_TYPE3_11N		0xDA4
+#define	ODM_REG_OFDM_FA_TYPE4_11N		0xDA8
+#define	ODM_REG_RPT_11N					0xDF4
+/* PAGE E */
+#define	ODM_REG_TXAGC_A_6_18_11N		0xE00
+#define	ODM_REG_TXAGC_A_24_54_11N		0xE04
+#define	ODM_REG_TXAGC_A_1_MCS32_11N	0xE08
+#define	ODM_REG_TXAGC_A_MCS0_3_11N		0xE10
+#define	ODM_REG_TXAGC_A_MCS4_7_11N		0xE14
+#define	ODM_REG_FPGA0_IQK_11N			0xE28
+#define	ODM_REG_TXIQK_TONE_A_11N		0xE30
+#define	ODM_REG_RXIQK_TONE_A_11N		0xE34
+#define	ODM_REG_TXIQK_PI_A_11N			0xE38
+#define	ODM_REG_RXIQK_PI_A_11N			0xE3C
+#define	ODM_REG_TXIQK_11N				0xE40
+#define	ODM_REG_RXIQK_11N				0xE44
+#define	ODM_REG_IQK_AGC_PTS_11N			0xE48
+#define	ODM_REG_IQK_AGC_RSP_11N			0xE4C
+#define	ODM_REG_BLUETOOTH_11N			0xE6C
+#define	ODM_REG_RX_WAIT_CCA_11N			0xE70
+#define	ODM_REG_TX_CCK_RFON_11N			0xE74
+#define	ODM_REG_TX_CCK_BBON_11N			0xE78
+#define	ODM_REG_OFDM_RFON_11N			0xE7C
+#define	ODM_REG_OFDM_BBON_11N			0xE80
+#define		ODM_REG_TX2RX_11N				0xE84
+#define	ODM_REG_TX2TX_11N				0xE88
+#define	ODM_REG_RX_CCK_11N				0xE8C
+#define	ODM_REG_RX_OFDM_11N				0xED0
+#define	ODM_REG_RX_WAIT_RIFS_11N		0xED4
+#define	ODM_REG_RX2RX_11N				0xED8
+#define	ODM_REG_STANDBY_11N				0xEDC
+#define	ODM_REG_SLEEP_11N				0xEE0
+#define	ODM_REG_PMPD_ANAEN_11N			0xEEC
+#define	ODM_REG_IGI_C_11N					0xF84
+#define	ODM_REG_IGI_D_11N					0xF88
+
+/* 2 MAC REG LIST */
+#define	ODM_REG_BB_RST_11N				0x02
+#define	ODM_REG_ANTSEL_PIN_11N			0x4C
+#define	ODM_REG_EARLY_MODE_11N			0x4D0
+#define	ODM_REG_RSSI_MONITOR_11N		0x4FE
+#define	ODM_REG_EDCA_VO_11N				0x500
+#define	ODM_REG_EDCA_VI_11N				0x504
+#define	ODM_REG_EDCA_BE_11N				0x508
+#define	ODM_REG_EDCA_BK_11N				0x50C
+#define	ODM_REG_TXPAUSE_11N				0x522
+#define	ODM_REG_RESP_TX_11N				0x6D8
+#define	ODM_REG_ANT_TRAIN_PARA1_11N		0x7b0
+#define	ODM_REG_ANT_TRAIN_PARA2_11N		0x7b4
+
+
+/* DIG Related */
+#define	ODM_BIT_IGI_11N					0x0000007F
+#define	ODM_BIT_CCK_RPT_FORMAT_11N		BIT9
+#define	ODM_BIT_BB_RX_PATH_11N			0xF
+#define	ODM_BIT_BB_ATC_11N				BIT11
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_interface.h b/drivers/staging/rtl8723bs/hal/odm_interface.h
new file mode 100644
index 000000000..d19347b02
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_interface.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+
+#ifndef	__ODM_INTERFACE_H__
+#define __ODM_INTERFACE_H__
+
+
+
+/*  =========== Macro Define */
+
+#define _reg_all(_name)			ODM_##_name
+#define _reg_ic(_name, _ic)		ODM_##_name##_ic
+#define _bit_all(_name)			BIT_##_name
+#define _bit_ic(_name, _ic)		BIT_##_name##_ic
+
+/*===================================
+
+#define ODM_REG_DIG_11N		0xC50
+#define ODM_REG_DIG_11AC	0xDDD
+
+ODM_REG(DIG, _pDM_Odm)
+=====================================*/
+
+#define _reg_11N(_name)			ODM_REG_##_name##_11N
+#define _bit_11N(_name)			ODM_BIT_##_name##_11N
+
+#define _cat(_name, _ic_type, _func) _func##_11N(_name)
+
+/*  _name: name of register or bit. */
+/*  Example: "ODM_REG(R_A_AGC_CORE1, pDM_Odm)" */
+/*         gets "ODM_R_A_AGC_CORE1" or "ODM_R_A_AGC_CORE1_8192C", depends on SupportICType. */
+#define ODM_REG(_name, _pDM_Odm)	_cat(_name, _pDM_Odm->SupportICType, _reg)
+#define ODM_BIT(_name, _pDM_Odm)	_cat(_name, _pDM_Odm->SupportICType, _bit)
+
+#endif	/*  __ODM_INTERFACE_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/odm_precomp.h b/drivers/staging/rtl8723bs/hal/odm_precomp.h
new file mode 100644
index 000000000..2987857a8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_precomp.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODM_PRECOMP_H__
+#define __ODM_PRECOMP_H__
+
+#include "odm_types.h"
+
+#define		TEST_FALG___		1
+
+/* 2 Config Flags and Structs - defined by each ODM Type */
+
+	/* include <basic_types.h> */
+	/* include <osdep_service.h> */
+	/* include <drv_types.h> */
+	/* include <rtw_byteorder.h> */
+	/* include <hal_intf.h> */
+#define BEAMFORMING_SUPPORT 0
+
+/* 2 Hardware Parameter Files */
+
+/* 2 OutSrc Header Files */
+
+#include "odm.h"
+#include "odm_HWConfig.h"
+#include "odm_RegDefine11N.h"
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+#include "HalPhyRf.h"
+#include "HalPhyRf_8723B.h"/* for IQK, LCK, Power-tracking */
+#include "rtl8723b_hal.h"
+#include "odm_interface.h"
+#include "odm_reg.h"
+#include "HalHWImg8723B_MAC.h"
+#include "HalHWImg8723B_RF.h"
+#include "HalHWImg8723B_BB.h"
+#include "Hal8723BReg.h"
+#include "odm_RegConfig8723B.h"
+
+#endif	/*  __ODM_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/odm_reg.h b/drivers/staging/rtl8723bs/hal/odm_reg.h
new file mode 100644
index 000000000..a3b1f6737
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_reg.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*  File Name: odm_reg.h */
+/*  Description: */
+/*  This file is for general register definition. */
+#ifndef	__HAL_ODM_REG_H__
+#define __HAL_ODM_REG_H__
+
+/*  Register Definition */
+
+/* MAC REG */
+#define	ODM_BB_RESET				0x002
+#define	ODM_DUMMY				0x4fe
+#define	RF_T_METER_OLD				0x24
+#define	RF_T_METER_NEW				0x42
+
+#define	ODM_EDCA_VO_PARAM			0x500
+#define	ODM_EDCA_VI_PARAM			0x504
+#define	ODM_EDCA_BE_PARAM			0x508
+#define	ODM_EDCA_BK_PARAM			0x50C
+#define	ODM_TXPAUSE				0x522
+
+/* BB REG */
+#define	ODM_FPGA_PHY0_PAGE8			0x800
+#define	ODM_PSD_SETTING				0x808
+#define	ODM_AFE_SETTING				0x818
+#define	ODM_TXAGC_B_24_54			0x834
+#define	ODM_TXAGC_B_MCS32_5			0x838
+#define	ODM_TXAGC_B_MCS0_MCS3			0x83c
+#define	ODM_TXAGC_B_MCS4_MCS7			0x848
+#define	ODM_ANALOG_REGISTER			0x85c
+#define	ODM_RF_INTERFACE_OUTPUT			0x860
+#define	ODM_TXAGC_B_11_A_2_11			0x86c
+#define	ODM_AD_DA_LSB_MASK			0x874
+#define	ODM_ENABLE_3_WIRE			0x88c
+#define	ODM_PSD_REPORT				0x8b4
+#define	ODM_R_ANT_SELECT			0x90c
+#define	ODM_CCK_ANT_SELECT			0xa07
+#define	ODM_CCK_PD_THRESH			0xa0a
+#define	ODM_CCK_RF_REG1				0xa11
+#define	ODM_CCK_MATCH_FILTER			0xa20
+#define	ODM_CCK_RAKE_MAC			0xa2e
+#define	ODM_CCK_CNT_RESET			0xa2d
+#define	ODM_CCK_TX_DIVERSITY			0xa2f
+#define	ODM_CCK_FA_CNT_MSB			0xa5b
+#define	ODM_CCK_FA_CNT_LSB			0xa5c
+#define	ODM_CCK_NEW_FUNCTION			0xa75
+#define	ODM_OFDM_PHY0_PAGE_C			0xc00
+#define	ODM_OFDM_RX_ANT				0xc04
+#define	ODM_R_A_RXIQI				0xc14
+#define	ODM_R_A_AGC_CORE1			0xc50
+#define	ODM_R_A_AGC_CORE2			0xc54
+#define	ODM_R_B_AGC_CORE1			0xc58
+#define	ODM_R_AGC_PAR				0xc70
+#define	ODM_R_HTSTF_AGC_PAR			0xc7c
+#define	ODM_TX_PWR_TRAINING_A			0xc90
+#define	ODM_TX_PWR_TRAINING_B			0xc98
+#define	ODM_OFDM_FA_CNT1			0xcf0
+#define	ODM_OFDM_PHY0_PAGE_D			0xd00
+#define	ODM_OFDM_FA_CNT2			0xda0
+#define	ODM_OFDM_FA_CNT3			0xda4
+#define	ODM_OFDM_FA_CNT4			0xda8
+#define	ODM_TXAGC_A_6_18			0xe00
+#define	ODM_TXAGC_A_24_54			0xe04
+#define	ODM_TXAGC_A_1_MCS32			0xe08
+#define	ODM_TXAGC_A_MCS0_MCS3			0xe10
+#define	ODM_TXAGC_A_MCS4_MCS7			0xe14
+
+/* RF REG */
+#define	ODM_GAIN_SETTING			0x00
+#define	ODM_CHANNEL				0x18
+
+/* Ant Detect Reg */
+#define	ODM_DPDT				0x300
+
+/* PSD Init */
+#define	ODM_PSDREG				0x808
+
+/* 92D Path Div */
+#define	PATHDIV_REG				0xB30
+#define	PATHDIV_TRI				0xBA0
+
+/*  Bitmap Definition */
+
+#define	BIT_FA_RESET				BIT0
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_types.h b/drivers/staging/rtl8723bs/hal/odm_types.h
new file mode 100644
index 000000000..8168dc14e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_types.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __ODM_TYPES_H__
+#define __ODM_TYPES_H__
+
+#include <drv_types.h>
+
+/*  Deifne HW endian support */
+#define	ODM_ENDIAN_BIG	0
+#define	ODM_ENDIAN_LITTLE	1
+
+#define GET_ODM(__padapter)	((PDM_ODM_T)(&((GET_HAL_DATA(__padapter))->odmpriv)))
+
+enum hal_status {
+	HAL_STATUS_SUCCESS,
+	HAL_STATUS_FAILURE,
+	/*RT_STATUS_PENDING,
+	RT_STATUS_RESOURCE,
+	RT_STATUS_INVALID_CONTEXT,
+	RT_STATUS_INVALID_PARAMETER,
+	RT_STATUS_NOT_SUPPORT,
+	RT_STATUS_OS_API_FAILED,*/
+};
+
+
+	#if defined(__LITTLE_ENDIAN)
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_LITTLE
+	#else
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_BIG
+	#endif
+
+	#define	STA_INFO_T			struct sta_info
+	#define	PSTA_INFO_T		struct sta_info *
+
+	#define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
+	#define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 25, 1, __Value)
+	#define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
+
+	/* define useless flag to avoid compile warning */
+	#define	USE_WORKITEM 0
+	#define   FPGA_TWO_MAC_VERIFICATION	0
+
+#define READ_NEXT_PAIR(v1, v2, i) do { if (i+2 >= ArrayLen) break; i += 2; v1 = Array[i]; v2 = Array[i+1]; } while (0)
+#define COND_ELSE  2
+#define COND_ENDIF 3
+
+#endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c b/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
new file mode 100644
index 000000000..a59ae622f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
@@ -0,0 +1,980 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+#include "hal_com_h2c.h"
+
+#define MAX_H2C_BOX_NUMS	4
+#define MESSAGE_BOX_SIZE	4
+
+#define RTL8723B_MAX_CMD_LEN	7
+#define RTL8723B_EX_MESSAGE_BOX_SIZE	4
+
+static u8 _is_fw_read_cmd_down(struct adapter *padapter, u8 msgbox_num)
+{
+	u8 read_down = false;
+	int retry_cnts = 100;
+
+	u8 valid;
+
+	do {
+		valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
+		if (0 == valid) {
+			read_down = true;
+		}
+	} while ((!read_down) && (retry_cnts--));
+
+	return read_down;
+
+}
+
+
+/*****************************************
+* H2C Msg format :
+*| 31 - 8		|7-5	| 4 - 0	|
+*| h2c_msg	|Class	|CMD_ID	|
+*| 31-0						|
+*| Ext msg					|
+*
+******************************************/
+s32 FillH2CCmd8723B(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+{
+	u8 h2c_box_num;
+	u32 msgbox_addr;
+	u32 msgbox_ex_addr = 0;
+	struct hal_com_data *pHalData;
+	u32 h2c_cmd = 0;
+	u32 h2c_cmd_ex = 0;
+	s32 ret = _FAIL;
+
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	pHalData = GET_HAL_DATA(padapter);
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex)))
+		return ret;
+
+	if (!pCmdBuffer) {
+		goto exit;
+	}
+
+	if (CmdLen > RTL8723B_MAX_CMD_LEN) {
+		goto exit;
+	}
+
+	if (padapter->bSurpriseRemoved)
+		goto exit;
+
+	/* pay attention to if  race condition happened in  H2C cmd setting. */
+	do {
+		h2c_box_num = pHalData->LastHMEBoxNum;
+
+		if (!_is_fw_read_cmd_down(padapter, h2c_box_num))
+			goto exit;
+
+		if (CmdLen <= 3)
+			memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, CmdLen);
+		else {
+			memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, 3);
+			memcpy((u8 *)(&h2c_cmd_ex), pCmdBuffer+3, CmdLen-3);
+/* 			*(u8 *)(&h2c_cmd) |= BIT(7); */
+		}
+
+		*(u8 *)(&h2c_cmd) |= ElementID;
+
+		if (CmdLen > 3) {
+			msgbox_ex_addr = REG_HMEBOX_EXT0_8723B + (h2c_box_num*RTL8723B_EX_MESSAGE_BOX_SIZE);
+			rtw_write32(padapter, msgbox_ex_addr, h2c_cmd_ex);
+		}
+		msgbox_addr = REG_HMEBOX_0 + (h2c_box_num*MESSAGE_BOX_SIZE);
+		rtw_write32(padapter, msgbox_addr, h2c_cmd);
+
+		pHalData->LastHMEBoxNum = (h2c_box_num+1) % MAX_H2C_BOX_NUMS;
+
+	} while (0);
+
+	ret = _SUCCESS;
+
+exit:
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex));
+	return ret;
+}
+
+static void ConstructBeacon(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 rate_len, pktlen;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	eth_broadcast_addr(pwlanhdr->addr1);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->ies)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	/*  capability info: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->ies)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		pktlen += cur_network->ie_length - sizeof(struct ndis_802_11_fix_ie);
+		memcpy(pframe, cur_network->ies+sizeof(struct ndis_802_11_fix_ie), pktlen);
+
+		goto _ConstructBeacon;
+	}
+
+	/* below for ad-hoc mode */
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, WLAN_EID_SSID, cur_network->ssid.ssid_length, cur_network->ssid.ssid, &pktlen);
+
+	/*  supported rates... */
+	rate_len = rtw_get_rateset_len(cur_network->supported_rates);
+	pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ? 8 : rate_len), cur_network->supported_rates, &pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)&(cur_network->configuration.ds_config), &pktlen);
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		/* ATIMWindow = cur->configuration.ATIMWindow; */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pktlen);
+	}
+
+
+	/* todo: ERP IE */
+
+
+	/*  EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->supported_rates + 8), &pktlen);
+
+
+	/* todo:HT for adhoc */
+
+_ConstructBeacon:
+
+	if ((pktlen + TXDESC_SIZE) > 512)
+		return;
+
+	*pLength = pktlen;
+
+}
+
+static void ConstructPSPoll(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	/*  Frame control. */
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	SetPwrMgt(fctrl);
+	SetFrameSubType(pframe, WIFI_PSPOLL);
+
+	/*  AID. */
+	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
+
+	/*  BSSID. */
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	/*  TA. */
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	*pLength = 16;
+}
+
+static void ConstructNullFunctionData(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *StaAddr,
+	u8 bQoS,
+	u8 AC,
+	u8 bEosp,
+	u8 bForcePowerSave
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 pktlen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	if (bForcePowerSave)
+		SetPwrMgt(fctrl);
+
+	switch (cur_network->network.infrastructure_mode) {
+	case Ndis802_11Infrastructure:
+		SetToDs(fctrl);
+		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
+		break;
+	case Ndis802_11APMode:
+		SetFrDs(fctrl);
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		break;
+	case Ndis802_11IBSS:
+	default:
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		break;
+	}
+
+	SetSeqNum(pwlanhdr, 0);
+
+	if (bQoS) {
+		struct ieee80211_qos_hdr *pwlanqoshdr;
+
+		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
+		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
+		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	} else {
+		SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+		pktlen = sizeof(struct ieee80211_hdr_3addr);
+	}
+
+	*pLength = pktlen;
+}
+
+/*
+ * To check if reserved page content is destroyed by beacon because beacon
+ * is too large.
+ */
+/* 2010.06.23. Added by tynli. */
+void CheckFwRsvdPageContent(struct adapter *Adapter)
+{
+}
+
+static void rtl8723b_set_FwRsvdPage_cmd(struct adapter *padapter, struct rsvdpage_loc *rsvdpageloc)
+{
+	u8 u1H2CRsvdPageParm[H2C_RSVDPAGE_LOC_LEN] = {0};
+
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1H2CRsvdPageParm, rsvdpageloc->LocProbeRsp);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1H2CRsvdPageParm, rsvdpageloc->LocPsPoll);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocNullData);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocQosNull);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocBTQosNull);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_RSVD_PAGE, H2C_RSVDPAGE_LOC_LEN, u1H2CRsvdPageParm);
+}
+
+static void rtl8723b_set_FwAoacRsvdPage_cmd(struct adapter *padapter, struct rsvdpage_loc *rsvdpageloc)
+{
+}
+
+void rtl8723b_set_FwMediaStatusRpt_cmd(struct adapter *padapter, u8 mstatus, u8 macid)
+{
+	u8 u1H2CMediaStatusRptParm[H2C_MEDIA_STATUS_RPT_LEN] = {0};
+	u8 macid_end = 0;
+
+	SET_8723B_H2CCMD_MSRRPT_PARM_OPMODE(u1H2CMediaStatusRptParm, mstatus);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID_IND(u1H2CMediaStatusRptParm, 0);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID(u1H2CMediaStatusRptParm, macid);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID_END(u1H2CMediaStatusRptParm, macid_end);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_MEDIA_STATUS_RPT, H2C_MEDIA_STATUS_RPT_LEN, u1H2CMediaStatusRptParm);
+}
+
+void rtl8723b_set_FwMacIdConfig_cmd(struct adapter *padapter, u8 mac_id, u8 raid, u8 bw, u8 sgi, u32 mask)
+{
+	u8 u1H2CMacIdConfigParm[H2C_MACID_CFG_LEN] = {0};
+
+	SET_8723B_H2CCMD_MACID_CFG_MACID(u1H2CMacIdConfigParm, mac_id);
+	SET_8723B_H2CCMD_MACID_CFG_RAID(u1H2CMacIdConfigParm, raid);
+	SET_8723B_H2CCMD_MACID_CFG_SGI_EN(u1H2CMacIdConfigParm, sgi ? 1 : 0);
+	SET_8723B_H2CCMD_MACID_CFG_BW(u1H2CMacIdConfigParm, bw);
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK0(u1H2CMacIdConfigParm, (u8)(mask & 0x000000ff));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK1(u1H2CMacIdConfigParm, (u8)((mask & 0x0000ff00) >> 8));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK2(u1H2CMacIdConfigParm, (u8)((mask & 0x00ff0000) >> 16));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK3(u1H2CMacIdConfigParm, (u8)((mask & 0xff000000) >> 24));
+
+	FillH2CCmd8723B(padapter, H2C_8723B_MACID_CFG, H2C_MACID_CFG_LEN, u1H2CMacIdConfigParm);
+}
+
+void rtl8723b_set_rssi_cmd(struct adapter *padapter, u8 *param)
+{
+	u8 u1H2CRssiSettingParm[H2C_RSSI_SETTING_LEN] = {0};
+	u8 mac_id = *param;
+	u8 rssi = *(param+2);
+	u8 uldl_state = 0;
+
+	SET_8723B_H2CCMD_RSSI_SETTING_MACID(u1H2CRssiSettingParm, mac_id);
+	SET_8723B_H2CCMD_RSSI_SETTING_RSSI(u1H2CRssiSettingParm, rssi);
+	SET_8723B_H2CCMD_RSSI_SETTING_ULDL_STATE(u1H2CRssiSettingParm, uldl_state);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_RSSI_SETTING, H2C_RSSI_SETTING_LEN, u1H2CRssiSettingParm);
+}
+
+void rtl8723b_set_FwPwrMode_cmd(struct adapter *padapter, u8 psmode)
+{
+	int i;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	u8 u1H2CPwrModeParm[H2C_PWRMODE_LEN] = {0};
+	u8 PowerState = 0, awake_intvl = 1, byte5 = 0, rlbm = 0;
+
+	if (pwrpriv->dtim > 0 && pwrpriv->dtim < 16)
+		awake_intvl = pwrpriv->dtim+1;/* DTIM = (awake_intvl - 1) */
+	else
+		awake_intvl = 3;/* DTIM =2 */
+
+	rlbm = 2;
+
+	if (padapter->registrypriv.wifi_spec == 1) {
+		awake_intvl = 2;
+		rlbm = 2;
+	}
+
+	if (psmode > 0) {
+		if (hal_btcoex_IsBtControlLps(padapter) == true) {
+			PowerState = hal_btcoex_RpwmVal(padapter);
+			byte5 = hal_btcoex_LpsVal(padapter);
+
+			if ((rlbm == 2) && (byte5 & BIT(4))) {
+				/*  Keep awake interval to 1 to prevent from */
+				/*  decreasing coex performance */
+				awake_intvl = 2;
+				rlbm = 2;
+			}
+		} else {
+			PowerState = 0x00;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+			byte5 = 0x40;
+		}
+	} else {
+		PowerState = 0x0C;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+		byte5 = 0x40;
+	}
+
+	SET_8723B_H2CCMD_PWRMODE_PARM_MODE(u1H2CPwrModeParm, (psmode > 0) ? 1 : 0);
+	SET_8723B_H2CCMD_PWRMODE_PARM_SMART_PS(u1H2CPwrModeParm, pwrpriv->smart_ps);
+	SET_8723B_H2CCMD_PWRMODE_PARM_RLBM(u1H2CPwrModeParm, rlbm);
+	SET_8723B_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1H2CPwrModeParm, awake_intvl);
+	SET_8723B_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1H2CPwrModeParm, padapter->registrypriv.uapsd_enable);
+	SET_8723B_H2CCMD_PWRMODE_PARM_PWR_STATE(u1H2CPwrModeParm, PowerState);
+	SET_8723B_H2CCMD_PWRMODE_PARM_BYTE5(u1H2CPwrModeParm, byte5);
+	if (psmode != PS_MODE_ACTIVE) {
+		if (!pmlmeext->adaptive_tsf_done && pmlmeext->bcn_cnt > 0) {
+			u8 ratio_20_delay, ratio_80_delay;
+
+			/* byte 6 for adaptive_early_32k */
+			/* 0:3] = DrvBcnEarly  (ms) , [4:7] = DrvBcnTimeOut  (ms) */
+			/*  20% for DrvBcnEarly, 80% for DrvBcnTimeOut */
+			ratio_20_delay = 0;
+			ratio_80_delay = 0;
+			pmlmeext->DrvBcnEarly = 0xff;
+			pmlmeext->DrvBcnTimeOut = 0xff;
+
+			for (i = 0; i < 9; i++) {
+				pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i]*100)/pmlmeext->bcn_cnt;
+
+				ratio_20_delay += pmlmeext->bcn_delay_ratio[i];
+				ratio_80_delay += pmlmeext->bcn_delay_ratio[i];
+
+				if (ratio_20_delay > 20 && pmlmeext->DrvBcnEarly == 0xff)
+					pmlmeext->DrvBcnEarly = i;
+
+				if (ratio_80_delay > 80 && pmlmeext->DrvBcnTimeOut == 0xff)
+					pmlmeext->DrvBcnTimeOut = i;
+
+				/* reset adaptive_early_32k cnt */
+				pmlmeext->bcn_delay_cnt[i] = 0;
+				pmlmeext->bcn_delay_ratio[i] = 0;
+
+			}
+
+			pmlmeext->bcn_cnt = 0;
+			pmlmeext->adaptive_tsf_done = true;
+
+		}
+
+/* offload to FW if fw version > v15.10
+		pmlmeext->DrvBcnEarly = 0;
+		pmlmeext->DrvBcnTimeOut =7;
+
+		if ((pmlmeext->DrvBcnEarly!= 0Xff) && (pmlmeext->DrvBcnTimeOut!= 0xff))
+			u1H2CPwrModeParm[H2C_PWRMODE_LEN-1] = BIT(0) | ((pmlmeext->DrvBcnEarly<<1)&0x0E) |((pmlmeext->DrvBcnTimeOut<<4)&0xf0) ;
+*/
+
+	}
+
+	hal_btcoex_RecordPwrMode(padapter, u1H2CPwrModeParm, H2C_PWRMODE_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_SET_PWR_MODE, H2C_PWRMODE_LEN, u1H2CPwrModeParm);
+}
+
+void rtl8723b_set_FwPsTuneParam_cmd(struct adapter *padapter)
+{
+	u8 u1H2CPsTuneParm[H2C_PSTUNEPARAM_LEN] = {0};
+	u8 bcn_to_limit = 10; /* 10 * 100 * awakeinterval (ms) */
+	u8 dtim_timeout = 5; /* ms wait broadcast data timer */
+	u8 ps_timeout = 20;  /* ms Keep awake when tx */
+	u8 dtim_period = 3;
+
+	SET_8723B_H2CCMD_PSTUNE_PARM_BCN_TO_LIMIT(u1H2CPsTuneParm, bcn_to_limit);
+	SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_TIMEOUT(u1H2CPsTuneParm, dtim_timeout);
+	SET_8723B_H2CCMD_PSTUNE_PARM_PS_TIMEOUT(u1H2CPsTuneParm, ps_timeout);
+	SET_8723B_H2CCMD_PSTUNE_PARM_ADOPT(u1H2CPsTuneParm, 1);
+	SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_PERIOD(u1H2CPsTuneParm, dtim_period);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_PS_TUNING_PARA, H2C_PSTUNEPARAM_LEN, u1H2CPsTuneParm);
+}
+
+void rtl8723b_set_FwPwrModeInIPS_cmd(struct adapter *padapter, u8 cmd_param)
+{
+
+	FillH2CCmd8723B(padapter, H2C_8723B_FWLPS_IN_IPS_, 1, &cmd_param);
+}
+
+/*
+ * Description: Fill the reserved packets that FW will use to RSVD page.
+ * Now we just send 4 types packet to rsvd page.
+ * (1)Beacon, (2)Ps-poll, (3)Null data, (4)ProbeRsp.
+ *
+ * Input:
+ *
+ * bDLFinished - false: At the first time we will send all the packets as
+ * a large packet to Hw, so we need to set the packet length to total length.
+ *
+ * true: At the second time, we should send the first packet (default:beacon)
+ * to Hw again and set the length in descriptor to the real beacon length.
+ */
+/* 2009.10.15 by tynli. */
+static void rtl8723b_set_FwRsvdPagePkt(
+	struct adapter *padapter, bool bDLFinished
+)
+{
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u32 BeaconLength = 0, PSPollLength = 0;
+	u32 NullDataLength = 0, QosNullLength = 0, BTQosNullLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen = TXDESC_SIZE, TxDescOffset = TXDESC_OFFSET;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u16 BufIndex, PageSize = 128;
+	u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
+
+	struct rsvdpage_loc RsvdPageLoc;
+
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B + WOWLAN_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (!pcmdframe)
+		return;
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(struct rsvdpage_loc));
+
+	/* 3 (1) beacon */
+	BufIndex = TxDescOffset;
+	ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
+	if (CurtPktPageNum == 1)
+		CurtPktPageNum += 1;
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (2) ps-poll */
+	RsvdPageLoc.LocPsPoll = TotalPageNum;
+	ConstructPSPoll(padapter, &ReservedPagePacket[BufIndex], &PSPollLength);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], PSPollLength, true, false, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + PSPollLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (3) null data */
+	RsvdPageLoc.LocNullData = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&NullDataLength,
+		get_my_bssid(&pmlmeinfo->network),
+		false, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], NullDataLength, false, false, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + NullDataLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (5) Qos null data */
+	RsvdPageLoc.LocQosNull = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&QosNullLength,
+		get_my_bssid(&pmlmeinfo->network),
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], QosNullLength, false, false, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + QosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (6) BT Qos null data */
+	RsvdPageLoc.LocBTQosNull = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&BTQosNullLength,
+		get_my_bssid(&pmlmeinfo->network),
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	TotalPacketLen = BufIndex + BTQosNullLength;
+
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		goto error;
+	} else {
+		/*  update attribute */
+		pattrib = &pcmdframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->qsel = 0x10;
+		pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+		dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
+		rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+	} else {
+		rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+	}
+	return;
+
+error:
+
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+
+void rtl8723b_download_rsvd_page(struct adapter *padapter, u8 mstatus)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	bool bcn_valid = false;
+	u8 DLBcnCount = 0;
+	u32 poll = 0;
+	u8 val8;
+
+	if (mstatus == RT_MEDIA_CONNECT) {
+		bool bRecover = false;
+		u8 v8;
+
+		/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
+		/*  Suggested by filen. Added by tynli. */
+		rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+
+		/*  set REG_CR bit 8 */
+		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 |= BIT(0); /*  ENSWBCN */
+		rtw_write8(padapter, REG_CR+1, v8);
+
+		/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
+		/*  Fix download reserved page packet fail that access collision with the protection time. */
+		/*  2010.05.11. Added by tynli. */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 &= ~EN_BCN_FUNCTION;
+		val8 |= DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		/*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
+		if (pHalData->RegFwHwTxQCtrl & BIT(6))
+			bRecover = true;
+
+		/*  To tell Hw the packet is not a real beacon frame. */
+		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl & ~BIT(6));
+		pHalData->RegFwHwTxQCtrl &= ~BIT(6);
+
+		/*  Clear beacon valid check bit. */
+		rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+		rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+		DLBcnCount = 0;
+		poll = 0;
+		do {
+			/*  download rsvd page. */
+			rtl8723b_set_FwRsvdPagePkt(padapter, 0);
+			DLBcnCount++;
+			do {
+				yield();
+				/* mdelay(10); */
+				/*  check rsvd page download OK. */
+				rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bcn_valid));
+				poll++;
+			} while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+		} while (!bcn_valid && DLBcnCount <= 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+		if (padapter->bSurpriseRemoved || padapter->bDriverStopped) {
+		} else {
+			struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+			pwrctl->fw_psmode_iface_id = padapter->iface_id;
+		}
+
+		/*  2010.05.11. Added by tynli. */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 |= EN_BCN_FUNCTION;
+		val8 &= ~DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		/*  To make sure that if there exists an adapter which would like to send beacon. */
+		/*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
+		/*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
+		/*  the beacon cannot be sent by HW. */
+		/*  2010.06.23. Added by tynli. */
+		if (bRecover) {
+			rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl | BIT(6));
+			pHalData->RegFwHwTxQCtrl |= BIT(6);
+		}
+
+		/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
+		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 &= ~BIT(0); /*  ~ENSWBCN */
+		rtw_write8(padapter, REG_CR+1, v8);
+	}
+}
+
+void rtl8723b_set_FwJoinBssRpt_cmd(struct adapter *padapter, u8 mstatus)
+{
+	if (mstatus == 1)
+		rtl8723b_download_rsvd_page(padapter, RT_MEDIA_CONNECT);
+}
+
+/* arg[0] = macid */
+/* arg[1] = raid */
+/* arg[2] = shortGIrate */
+/* arg[3] = init_rate */
+void rtl8723b_Add_RateATid(
+	struct adapter *padapter,
+	u32 bitmap,
+	u8 *arg,
+	u8 rssi_level
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info *psta;
+	u8 mac_id = arg[0];
+	u8 raid = arg[1];
+	u8 shortGI = arg[2];
+	u8 bw;
+	u32 mask = bitmap&0x0FFFFFFF;
+
+	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
+	if (!psta)
+		return;
+
+	bw = psta->bw_mode;
+
+	if (rssi_level != DM_RATR_STA_INIT)
+		mask = ODM_Get_Rate_Bitmap(&pHalData->odmpriv, mac_id, mask, rssi_level);
+
+	rtl8723b_set_FwMacIdConfig_cmd(padapter, mac_id, raid, bw, shortGI, mask);
+}
+
+static void ConstructBtNullFunctionData(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *StaAddr,
+	u8 bQoS,
+	u8 AC,
+	u8 bEosp,
+	u8 bForcePowerSave
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 pktlen;
+	u8 bssid[ETH_ALEN];
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	if (!StaAddr) {
+		memcpy(bssid, myid(&padapter->eeprompriv), ETH_ALEN);
+		StaAddr = bssid;
+	}
+
+	fctrl = &pwlanhdr->frame_control;
+	*fctrl = 0;
+	if (bForcePowerSave)
+		SetPwrMgt(fctrl);
+
+	SetFrDs(fctrl);
+	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetDuration(pwlanhdr, 0);
+	SetSeqNum(pwlanhdr, 0);
+
+	if (bQoS) {
+		struct ieee80211_qos_hdr *pwlanqoshdr;
+
+		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
+		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
+		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	} else {
+		SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+		pktlen = sizeof(struct ieee80211_hdr_3addr);
+	}
+
+	*pLength = pktlen;
+}
+
+static void SetFwRsvdPagePkt_BTCoex(struct adapter *padapter)
+{
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	u32 BeaconLength = 0;
+	u32 BTQosNullLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen, TxDescOffset;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u16 BufIndex, PageSize;
+	u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
+	struct rsvdpage_loc RsvdPageLoc;
+
+	pxmitpriv = &padapter->xmitpriv;
+	TxDescLen = TXDESC_SIZE;
+	TxDescOffset = TXDESC_OFFSET;
+	PageSize = PAGE_SIZE_TX_8723B;
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (!pcmdframe)
+		return;
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(struct rsvdpage_loc));
+
+	/* 3 (1) beacon */
+	BufIndex = TxDescOffset;
+	ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
+	if (CurtPktPageNum == 1)
+		CurtPktPageNum += 1;
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/*  Jump to lastest page */
+	if (BufIndex < (MaxRsvdPageBufSize - PageSize)) {
+		BufIndex = TxDescOffset + (MaxRsvdPageBufSize - PageSize);
+		TotalPageNum = BCNQ_PAGE_NUM_8723B - 1;
+	}
+
+	/* 3 (6) BT Qos null data */
+	RsvdPageLoc.LocBTQosNull = TotalPageNum;
+	ConstructBtNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&BTQosNullLength,
+		NULL,
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	TotalPacketLen = BufIndex + BTQosNullLength;
+	if (TotalPacketLen > MaxRsvdPageBufSize)
+		goto error;
+
+	/*  update attribute */
+	pattrib = &pcmdframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+	dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+
+	rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
+	rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+
+	return;
+
+error:
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+
+void rtl8723b_download_BTCoex_AP_mode_rsvd_page(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	u8 bRecover = false;
+	u8 bcn_valid = false;
+	u8 DLBcnCount = 0;
+	u32 poll = 0;
+	u8 val8;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
+	/*  Suggested by filen. Added by tynli. */
+	rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+
+	/*  set REG_CR bit 8 */
+	val8 = rtw_read8(padapter, REG_CR+1);
+	val8 |= BIT(0); /*  ENSWBCN */
+	rtw_write8(padapter,  REG_CR+1, val8);
+
+	/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
+	/*  Fix download reserved page packet fail that access collision with the protection time. */
+	/*  2010.05.11. Added by tynli. */
+	val8 = rtw_read8(padapter, REG_BCN_CTRL);
+	val8 &= ~EN_BCN_FUNCTION;
+	val8 |= DIS_TSF_UDT;
+	rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+	/*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
+	if (pHalData->RegFwHwTxQCtrl & BIT(6))
+		bRecover = true;
+
+	/*  To tell Hw the packet is not a real beacon frame. */
+	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
+	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+
+	/*  Clear beacon valid check bit. */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+	DLBcnCount = 0;
+	poll = 0;
+	do {
+		SetFwRsvdPagePkt_BTCoex(padapter);
+		DLBcnCount++;
+		do {
+			yield();
+/* 			mdelay(10); */
+			/*  check rsvd page download OK. */
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, &bcn_valid);
+			poll++;
+		} while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+	} while (!bcn_valid && (DLBcnCount <= 100) && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (bcn_valid) {
+		struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+		pwrctl->fw_psmode_iface_id = padapter->iface_id;
+	}
+
+	/*  2010.05.11. Added by tynli. */
+	val8 = rtw_read8(padapter, REG_BCN_CTRL);
+	val8 |= EN_BCN_FUNCTION;
+	val8 &= ~DIS_TSF_UDT;
+	rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+	/*  To make sure that if there exists an adapter which would like to send beacon. */
+	/*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
+	/*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
+	/*  the beacon cannot be sent by HW. */
+	/*  2010.06.23. Added by tynli. */
+	if (bRecover) {
+		pHalData->RegFwHwTxQCtrl |= BIT(6);
+		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	}
+
+	/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
+	val8 = rtw_read8(padapter, REG_CR+1);
+	val8 &= ~BIT(0); /*  ~ENSWBCN */
+	rtw_write8(padapter, REG_CR+1, val8);
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_dm.c b/drivers/staging/rtl8723bs/hal/rtl8723b_dm.c
new file mode 100644
index 000000000..202879198
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_dm.c
@@ -0,0 +1,272 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*  Description: */
+/*  This file is for 92CE/92CU dynamic mechanism only */
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+/*  Global var */
+
+static void dm_CheckStatistics(struct adapter *Adapter)
+{
+}
+/*  */
+/*  functions */
+/*  */
+static void Init_ODM_ComInfo_8723b(struct adapter *Adapter)
+{
+
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	u8 cut_ver, fab_ver;
+
+	/*  */
+	/*  Init Value */
+	/*  */
+	memset(pDM_Odm, 0, sizeof(*pDM_Odm));
+
+	pDM_Odm->Adapter = Adapter;
+#define ODM_CE 0x04
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PLATFORM, ODM_CE);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_INTERFACE, RTW_SDIO);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PACKAGE_TYPE, pHalData->PackageType);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_IC_TYPE, ODM_RTL8723B);
+
+	fab_ver = ODM_TSMC;
+	cut_ver = ODM_CUT_A;
+
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_FAB_VER, fab_ver);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_CUT_VER, cut_ver);
+
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PATCH_ID, pHalData->CustomerID);
+	/* 	ODM_CMNINFO_BINHCT_TEST only for MP Team */
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_BWIFI_TEST, Adapter->registrypriv.wifi_spec);
+
+	pdmpriv->InitODMFlag = ODM_RF_CALIBRATION|ODM_RF_TX_PWR_TRACK;
+
+	ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pdmpriv->InitODMFlag);
+}
+
+static void Update_ODM_ComInfo_8723b(struct adapter *Adapter)
+{
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(Adapter);
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	int i;
+	u8 zero = 0;
+
+	pdmpriv->InitODMFlag = 0
+		| ODM_BB_DIG
+		| ODM_BB_RA_MASK
+		| ODM_BB_DYNAMIC_TXPWR
+		| ODM_BB_FA_CNT
+		| ODM_BB_RSSI_MONITOR
+		| ODM_BB_CCK_PD
+		| ODM_BB_PWR_SAVE
+		| ODM_BB_CFO_TRACKING
+		| ODM_MAC_EDCA_TURBO
+		| ODM_RF_TX_PWR_TRACK
+		| ODM_RF_CALIBRATION
+		;
+
+	/*  */
+	/*  Pointer reference */
+	/*  */
+	/* ODM_CMNINFO_MAC_PHY_MODE pHalData->MacPhyMode92D */
+	/* 	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_MAC_PHY_MODE,&(pDM_Odm->u8_temp)); */
+
+	ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pdmpriv->InitODMFlag);
+
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_TX_UNI, &(dvobj->traffic_stat.tx_bytes));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_RX_UNI, &(dvobj->traffic_stat.rx_bytes));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_WM_MODE, &(pmlmeext->cur_wireless_mode));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SEC_CHNL_OFFSET, &(pHalData->nCur40MhzPrimeSC));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SEC_MODE, &(Adapter->securitypriv.dot11PrivacyAlgrthm));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_BW, &(pHalData->CurrentChannelBW));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_CHNL, &(pHalData->CurrentChannel));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_NET_CLOSED, &(Adapter->net_closed));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_MP_MODE, &zero);
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_FORCED_IGI_LB, &(pHalData->u1ForcedIgiLb));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_FORCED_RATE, &(pHalData->ForcedDataRate));
+
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SCAN, &(pmlmepriv->bScanInProcess));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_POWER_SAVING, &(pwrctrlpriv->bpower_saving));
+
+
+	for (i = 0; i < NUM_STA; i++)
+		ODM_CmnInfoPtrArrayHook(pDM_Odm, ODM_CMNINFO_STA_STATUS, i, NULL);
+}
+
+void rtl8723b_InitHalDm(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+
+	pdmpriv->DM_Type = DM_Type_ByDriver;
+	pdmpriv->DMFlag = DYNAMIC_FUNC_DISABLE;
+
+	pdmpriv->DMFlag |= DYNAMIC_FUNC_BT;
+
+	pdmpriv->InitDMFlag = pdmpriv->DMFlag;
+
+	Update_ODM_ComInfo_8723b(Adapter);
+
+	ODM_DMInit(pDM_Odm);
+}
+
+void rtl8723b_HalDmWatchDog(struct adapter *Adapter)
+{
+	bool fw_current_in_ps_mode = false;
+	bool bFwPSAwake = true;
+	u8 hw_init_completed = false;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	hw_init_completed = Adapter->hw_init_completed;
+
+	if (hw_init_completed == false)
+		goto skip_dm;
+
+	fw_current_in_ps_mode = adapter_to_pwrctl(Adapter)->fw_current_in_ps_mode;
+	rtw_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&bFwPSAwake));
+
+	if (
+		(hw_init_completed == true) &&
+		((!fw_current_in_ps_mode) && bFwPSAwake)
+	) {
+		/*  */
+		/*  Calculate Tx/Rx statistics. */
+		/*  */
+		dm_CheckStatistics(Adapter);
+		rtw_hal_check_rxfifo_full(Adapter);
+	}
+
+	/* ODM */
+	if (hw_init_completed == true) {
+		u8 bLinked = false;
+		u8 bsta_state = false;
+		bool bBtDisabled = true;
+
+		if (rtw_linked_check(Adapter)) {
+			bLinked = true;
+			if (check_fwstate(&Adapter->mlmepriv, WIFI_STATION_STATE))
+				bsta_state = true;
+		}
+
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_LINK, bLinked);
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_STATION_STATE, bsta_state);
+
+		/* ODM_CmnInfoUpdate(&pHalData->odmpriv , ODM_CMNINFO_RSSI_MIN, pdmpriv->MinUndecoratedPWDBForDM); */
+
+		bBtDisabled = hal_btcoex_IsBtDisabled(Adapter);
+
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_BT_ENABLED,
+				  !bBtDisabled);
+
+		ODM_DMWatchdog(&pHalData->odmpriv);
+	}
+
+skip_dm:
+	return;
+}
+
+void rtl8723b_hal_dm_in_lps(struct adapter *padapter)
+{
+	u32 PWDB_rssi = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	/* update IGI */
+	ODM_Write_DIG(pDM_Odm, pDM_Odm->RSSI_Min);
+
+
+	/* set rssi to fw */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (psta && (psta->rssi_stat.UndecoratedSmoothedPWDB > 0)) {
+		PWDB_rssi = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB<<16));
+
+		rtl8723b_set_rssi_cmd(padapter, (u8 *)&PWDB_rssi);
+	}
+
+}
+
+void rtl8723b_HalDmWatchDog_in_LPS(struct adapter *Adapter)
+{
+	u8 bLinked = false;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct sta_priv *pstapriv = &Adapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	if (Adapter->hw_init_completed == false)
+		goto skip_lps_dm;
+
+
+	if (rtw_linked_check(Adapter))
+		bLinked = true;
+
+	ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_LINK, bLinked);
+
+	if (bLinked == false)
+		goto skip_lps_dm;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		goto skip_lps_dm;
+
+
+	/* ODM_DMWatchdog(&pHalData->odmpriv); */
+	/* Do DIG by RSSI In LPS-32K */
+
+      /* 1 Find MIN-RSSI */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (!psta)
+		goto skip_lps_dm;
+
+	pdmpriv->EntryMinUndecoratedSmoothedPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+	if (pdmpriv->EntryMinUndecoratedSmoothedPWDB <= 0)
+		goto skip_lps_dm;
+
+	pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+
+	pDM_Odm->RSSI_Min = pdmpriv->MinUndecoratedPWDBForDM;
+
+	/* if (pDM_DigTable->CurIGValue != pDM_Odm->RSSI_Min) */
+	if (
+		(pDM_DigTable->CurIGValue > pDM_Odm->RSSI_Min + 5) ||
+		(pDM_DigTable->CurIGValue < pDM_Odm->RSSI_Min - 5)
+	)
+		rtw_dm_in_lps_wk_cmd(Adapter);
+
+
+skip_lps_dm:
+
+	return;
+
+}
+
+void rtl8723b_init_dm_priv(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	memset(pdmpriv, 0, sizeof(struct dm_priv));
+	Init_ODM_ComInfo_8723b(Adapter);
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
new file mode 100644
index 000000000..f1fc077ed
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
@@ -0,0 +1,3863 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <linux/firmware.h>
+#include <linux/slab.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+#include "hal_com_h2c.h"
+
+static void _FWDownloadEnable(struct adapter *padapter, bool enable)
+{
+	u8 tmp, count = 0;
+
+	if (enable) {
+		/*  8051 enable */
+		tmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		rtw_write8(padapter, REG_SYS_FUNC_EN+1, tmp|0x04);
+
+		tmp = rtw_read8(padapter, REG_MCUFWDL);
+		rtw_write8(padapter, REG_MCUFWDL, tmp|0x01);
+
+		do {
+			tmp = rtw_read8(padapter, REG_MCUFWDL);
+			if (tmp & 0x01)
+				break;
+			rtw_write8(padapter, REG_MCUFWDL, tmp|0x01);
+			msleep(1);
+		} while (count++ < 100);
+
+		/*  8051 reset */
+		tmp = rtw_read8(padapter, REG_MCUFWDL+2);
+		rtw_write8(padapter, REG_MCUFWDL+2, tmp&0xf7);
+	} else {
+		/*  MCU firmware download disable. */
+		tmp = rtw_read8(padapter, REG_MCUFWDL);
+		rtw_write8(padapter, REG_MCUFWDL, tmp&0xfe);
+	}
+}
+
+static int _BlockWrite(struct adapter *padapter, void *buffer, u32 buffSize)
+{
+	int ret = _SUCCESS;
+
+	u32 blockSize_p1 = 4; /*  (Default) Phase #1 : PCI muse use 4-byte write to download FW */
+	u32 blockSize_p2 = 8; /*  Phase #2 : Use 8-byte, if Phase#1 use big size to write FW. */
+	u32 blockSize_p3 = 1; /*  Phase #3 : Use 1-byte, the remnant of FW image. */
+	u32 blockCount_p1 = 0, blockCount_p2 = 0, blockCount_p3 = 0;
+	u32 remainSize_p1 = 0, remainSize_p2 = 0;
+	u8 *bufferPtr = buffer;
+	u32 i = 0, offset = 0;
+
+/* 	printk("====>%s %d\n", __func__, __LINE__); */
+
+	/* 3 Phase #1 */
+	blockCount_p1 = buffSize / blockSize_p1;
+	remainSize_p1 = buffSize % blockSize_p1;
+
+	for (i = 0; i < blockCount_p1; i++) {
+		ret = rtw_write32(padapter, (FW_8723B_START_ADDRESS + i * blockSize_p1), *((u32 *)(bufferPtr + i * blockSize_p1)));
+		if (ret == _FAIL) {
+			printk("====>%s %d i:%d\n", __func__, __LINE__, i);
+			goto exit;
+		}
+	}
+
+	/* 3 Phase #2 */
+	if (remainSize_p1) {
+		offset = blockCount_p1 * blockSize_p1;
+
+		blockCount_p2 = remainSize_p1/blockSize_p2;
+		remainSize_p2 = remainSize_p1%blockSize_p2;
+	}
+
+	/* 3 Phase #3 */
+	if (remainSize_p2) {
+		offset = (blockCount_p1 * blockSize_p1) + (blockCount_p2 * blockSize_p2);
+
+		blockCount_p3 = remainSize_p2 / blockSize_p3;
+
+		for (i = 0; i < blockCount_p3; i++) {
+			ret = rtw_write8(padapter, (FW_8723B_START_ADDRESS + offset + i), *(bufferPtr + offset + i));
+
+			if (ret == _FAIL) {
+				printk("====>%s %d i:%d\n", __func__, __LINE__, i);
+				goto exit;
+			}
+		}
+	}
+exit:
+	return ret;
+}
+
+static int _PageWrite(
+	struct adapter *padapter,
+	u32 page,
+	void *buffer,
+	u32 size
+)
+{
+	u8 value8;
+	u8 u8Page = (u8) (page & 0x07);
+
+	value8 = (rtw_read8(padapter, REG_MCUFWDL+2) & 0xF8) | u8Page;
+	rtw_write8(padapter, REG_MCUFWDL+2, value8);
+
+	return _BlockWrite(padapter, buffer, size);
+}
+
+static int _WriteFW(struct adapter *padapter, void *buffer, u32 size)
+{
+	/*  Since we need dynamic decide method of dwonload fw, so we call this function to get chip version. */
+	/*  We can remove _ReadChipVersion from ReadpadapterInfo8192C later. */
+	int ret = _SUCCESS;
+	u32 pageNums, remainSize;
+	u32 page, offset;
+	u8 *bufferPtr = buffer;
+
+	pageNums = size / MAX_DLFW_PAGE_SIZE;
+	remainSize = size % MAX_DLFW_PAGE_SIZE;
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * MAX_DLFW_PAGE_SIZE;
+		ret = _PageWrite(padapter, page, bufferPtr+offset, MAX_DLFW_PAGE_SIZE);
+
+		if (ret == _FAIL) {
+			printk("====>%s %d\n", __func__, __LINE__);
+			goto exit;
+		}
+	}
+
+	if (remainSize) {
+		offset = pageNums * MAX_DLFW_PAGE_SIZE;
+		page = pageNums;
+		ret = _PageWrite(padapter, page, bufferPtr+offset, remainSize);
+
+		if (ret == _FAIL) {
+			printk("====>%s %d\n", __func__, __LINE__);
+			goto exit;
+		}
+	}
+
+exit:
+	return ret;
+}
+
+void _8051Reset8723(struct adapter *padapter)
+{
+	u8 cpu_rst;
+	u8 io_rst;
+
+
+	/*  Reset 8051(WLMCU) IO wrapper */
+	/*  0x1c[8] = 0 */
+	/*  Suggested by Isaac@SD1 and Gimmy@SD1, coding by Lucas@20130624 */
+	io_rst = rtw_read8(padapter, REG_RSV_CTRL+1);
+	io_rst &= ~BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL+1, io_rst);
+
+	cpu_rst = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+	cpu_rst &= ~BIT(2);
+	rtw_write8(padapter, REG_SYS_FUNC_EN+1, cpu_rst);
+
+	/*  Enable 8051 IO wrapper */
+	/*  0x1c[8] = 1 */
+	io_rst = rtw_read8(padapter, REG_RSV_CTRL+1);
+	io_rst |= BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL+1, io_rst);
+
+	cpu_rst = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+	cpu_rst |= BIT(2);
+	rtw_write8(padapter, REG_SYS_FUNC_EN+1, cpu_rst);
+}
+
+u8 g_fwdl_chksum_fail;
+
+static s32 polling_fwdl_chksum(
+	struct adapter *adapter, u32 min_cnt, u32 timeout_ms
+)
+{
+	s32 ret = _FAIL;
+	u32 value32;
+	unsigned long start = jiffies;
+	u32 cnt = 0;
+
+	/* polling CheckSum report */
+	do {
+		cnt++;
+		value32 = rtw_read32(adapter, REG_MCUFWDL);
+		if (value32 & FWDL_ChkSum_rpt || adapter->bSurpriseRemoved || adapter->bDriverStopped)
+			break;
+		yield();
+	} while (jiffies_to_msecs(jiffies-start) < timeout_ms || cnt < min_cnt);
+
+	if (!(value32 & FWDL_ChkSum_rpt)) {
+		goto exit;
+	}
+
+	if (g_fwdl_chksum_fail) {
+		g_fwdl_chksum_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+
+	return ret;
+}
+
+u8 g_fwdl_wintint_rdy_fail;
+
+static s32 _FWFreeToGo(struct adapter *adapter, u32 min_cnt, u32 timeout_ms)
+{
+	s32 ret = _FAIL;
+	u32 value32;
+	unsigned long start = jiffies;
+	u32 cnt = 0;
+
+	value32 = rtw_read32(adapter, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtw_write32(adapter, REG_MCUFWDL, value32);
+
+	_8051Reset8723(adapter);
+
+	/*  polling for FW ready */
+	do {
+		cnt++;
+		value32 = rtw_read32(adapter, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY || adapter->bSurpriseRemoved || adapter->bDriverStopped)
+			break;
+		yield();
+	} while (jiffies_to_msecs(jiffies - start) < timeout_ms || cnt < min_cnt);
+
+	if (!(value32 & WINTINI_RDY)) {
+		goto exit;
+	}
+
+	if (g_fwdl_wintint_rdy_fail) {
+		g_fwdl_wintint_rdy_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+
+	return ret;
+}
+
+#define IS_FW_81xxC(padapter)	(((GET_HAL_DATA(padapter))->FirmwareSignature & 0xFFF0) == 0x88C0)
+
+void rtl8723b_FirmwareSelfReset(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 u1bTmp;
+	u8 Delay = 100;
+
+	if (
+		!(IS_FW_81xxC(padapter) && ((pHalData->FirmwareVersion < 0x21) || (pHalData->FirmwareVersion == 0x21 && pHalData->FirmwareSubVersion < 0x01)))
+	) { /*  after 88C Fw v33.1 */
+		/* 0x1cf = 0x20. Inform 8051 to reset. 2009.12.25. tynli_test */
+		rtw_write8(padapter, REG_HMETFR+3, 0x20);
+
+		u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		while (u1bTmp & BIT2) {
+			Delay--;
+			if (Delay == 0)
+				break;
+			udelay(50);
+			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		}
+
+		if (Delay == 0) {
+			/* force firmware reset */
+			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+			rtw_write8(padapter, REG_SYS_FUNC_EN+1, u1bTmp&(~BIT2));
+		}
+	}
+}
+
+/*  */
+/* 	Description: */
+/* 		Download 8192C firmware code. */
+/*  */
+/*  */
+s32 rtl8723b_FirmwareDownload(struct adapter *padapter, bool  bUsedWoWLANFw)
+{
+	s32 rtStatus = _SUCCESS;
+	u8 write_fw = 0;
+	unsigned long fwdl_start_time;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct rt_firmware *pFirmware;
+	struct rt_firmware *pBTFirmware;
+	struct rt_firmware_hdr *pFwHdr = NULL;
+	u8 *pFirmwareBuf;
+	u32 FirmwareLen;
+	const struct firmware *fw;
+	struct device *device = dvobj_to_dev(padapter->dvobj);
+	u8 *fwfilepath;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u8 tmp_ps;
+
+	pFirmware = kzalloc(sizeof(struct rt_firmware), GFP_KERNEL);
+	if (!pFirmware)
+		return _FAIL;
+	pBTFirmware = kzalloc(sizeof(struct rt_firmware), GFP_KERNEL);
+	if (!pBTFirmware) {
+		kfree(pFirmware);
+		return _FAIL;
+	}
+	tmp_ps = rtw_read8(padapter, 0xa3);
+	tmp_ps &= 0xf8;
+	tmp_ps |= 0x02;
+	/* 1. write 0xA3[:2:0] = 3b'010 */
+	rtw_write8(padapter, 0xa3, tmp_ps);
+	/* 2. read power_state = 0xA0[1:0] */
+	tmp_ps = rtw_read8(padapter, 0xa0);
+	tmp_ps &= 0x03;
+	if (tmp_ps != 0x01)
+		pdbgpriv->dbg_downloadfw_pwr_state_cnt++;
+
+	fwfilepath = "rtlwifi/rtl8723bs_nic.bin";
+
+	pr_info("rtl8723bs: acquire FW from file:%s\n", fwfilepath);
+
+	rtStatus = request_firmware(&fw, fwfilepath, device);
+	if (rtStatus) {
+		pr_err("Request firmware failed with error 0x%x\n", rtStatus);
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	if (!fw) {
+		pr_err("Firmware %s not available\n", fwfilepath);
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	if (fw->size > FW_8723B_SIZE) {
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	pFirmware->fw_buffer_sz = kmemdup(fw->data, fw->size, GFP_KERNEL);
+	if (!pFirmware->fw_buffer_sz) {
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	pFirmware->fw_length = fw->size;
+	release_firmware(fw);
+	if (pFirmware->fw_length > FW_8723B_SIZE) {
+		rtStatus = _FAIL;
+		netdev_emerg(padapter->pnetdev,
+			     "Firmware size:%u exceed %u\n",
+			     pFirmware->fw_length, FW_8723B_SIZE);
+		goto release_fw1;
+	}
+
+	pFirmwareBuf = pFirmware->fw_buffer_sz;
+	FirmwareLen = pFirmware->fw_length;
+
+	/*  To Check Fw header. Added by tynli. 2009.12.04. */
+	pFwHdr = (struct rt_firmware_hdr *)pFirmwareBuf;
+
+	pHalData->FirmwareVersion =  le16_to_cpu(pFwHdr->version);
+	pHalData->FirmwareSubVersion = le16_to_cpu(pFwHdr->subversion);
+	pHalData->FirmwareSignature = le16_to_cpu(pFwHdr->signature);
+
+	if (IS_FW_HEADER_EXIST_8723B(pFwHdr)) {
+		/*  Shift 32 bytes for FW header */
+		pFirmwareBuf = pFirmwareBuf + 32;
+		FirmwareLen = FirmwareLen - 32;
+	}
+
+	/*  Suggested by Filen. If 8051 is running in RAM code, driver should inform Fw to reset by itself, */
+	/*  or it will cause download Fw fail. 2010.02.01. by tynli. */
+	if (rtw_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) { /* 8051 RAM code */
+		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		rtl8723b_FirmwareSelfReset(padapter);
+	}
+
+	_FWDownloadEnable(padapter, true);
+	fwdl_start_time = jiffies;
+	while (
+		!padapter->bDriverStopped &&
+		!padapter->bSurpriseRemoved &&
+		(write_fw++ < 3 || jiffies_to_msecs(jiffies - fwdl_start_time) < 500)
+	) {
+		/* reset FWDL chksum */
+		rtw_write8(padapter, REG_MCUFWDL, rtw_read8(padapter, REG_MCUFWDL)|FWDL_ChkSum_rpt);
+
+		rtStatus = _WriteFW(padapter, pFirmwareBuf, FirmwareLen);
+		if (rtStatus != _SUCCESS)
+			continue;
+
+		rtStatus = polling_fwdl_chksum(padapter, 5, 50);
+		if (rtStatus == _SUCCESS)
+			break;
+	}
+	_FWDownloadEnable(padapter, false);
+	if (_SUCCESS != rtStatus)
+		goto fwdl_stat;
+
+	rtStatus = _FWFreeToGo(padapter, 10, 200);
+	if (_SUCCESS != rtStatus)
+		goto fwdl_stat;
+
+fwdl_stat:
+
+exit:
+	kfree(pFirmware->fw_buffer_sz);
+	kfree(pFirmware);
+release_fw1:
+	kfree(pBTFirmware);
+	return rtStatus;
+}
+
+void rtl8723b_InitializeFirmwareVars(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	/*  Init Fw LPS related. */
+	adapter_to_pwrctl(padapter)->fw_current_in_ps_mode = false;
+
+	/* Init H2C cmd. */
+	rtw_write8(padapter, REG_HMETFR, 0x0f);
+
+	/*  Init H2C counter. by tynli. 2009.12.09. */
+	pHalData->LastHMEBoxNum = 0;
+/* pHalData->H2CQueueHead = 0; */
+/* pHalData->H2CQueueTail = 0; */
+/* pHalData->H2CStopInsertQueue = false; */
+}
+
+static void rtl8723b_free_hal_data(struct adapter *padapter)
+{
+}
+
+/*  */
+/* 				Efuse related code */
+/*  */
+static u8 hal_EfuseSwitchToBank(
+	struct adapter *padapter, u8 bank, bool bPseudoTest
+)
+{
+	u8 bRet = false;
+	u32 value32 = 0;
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+#endif
+
+
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		pEfuseHal->fakeEfuseBank = bank;
+#else
+		fakeEfuseBank = bank;
+#endif
+		bRet = true;
+	} else {
+		value32 = rtw_read32(padapter, EFUSE_TEST);
+		bRet = true;
+		switch (bank) {
+		case 0:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
+			break;
+		case 1:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_0);
+			break;
+		case 2:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_1);
+			break;
+		case 3:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_2);
+			break;
+		default:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
+			bRet = false;
+			break;
+		}
+		rtw_write32(padapter, EFUSE_TEST, value32);
+	}
+
+	return bRet;
+}
+
+static void Hal_GetEfuseDefinition(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 type,
+	void *pOut,
+	bool bPseudoTest
+)
+{
+	switch (type) {
+	case TYPE_EFUSE_MAX_SECTION:
+		{
+			u8 *pMax_section;
+			pMax_section = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pMax_section = EFUSE_MAX_SECTION_8723B;
+			else
+				*pMax_section = EFUSE_BT_MAX_SECTION;
+		}
+		break;
+
+	case TYPE_EFUSE_REAL_CONTENT_LEN:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723B;
+			else
+				*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
+		}
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723B-EFUSE_OOB_PROTECT_BYTES);
+			else
+				*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN-EFUSE_PROTECT_BYTES_BANK);
+		}
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723B-EFUSE_OOB_PROTECT_BYTES);
+			else
+				*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN-(EFUSE_PROTECT_BYTES_BANK*3));
+		}
+		break;
+
+	case TYPE_EFUSE_MAP_LEN:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_MAX_MAP_LEN;
+			else
+				*pu2Tmp = EFUSE_BT_MAP_LEN;
+		}
+		break;
+
+	case TYPE_EFUSE_PROTECT_BYTES_BANK:
+		{
+			u8 *pu1Tmp;
+			pu1Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
+			else
+				*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
+		}
+		break;
+
+	case TYPE_EFUSE_CONTENT_LEN_BANK:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723B;
+			else
+				*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
+		}
+		break;
+
+	default:
+		{
+			u8 *pu1Tmp;
+			pu1Tmp = pOut;
+			*pu1Tmp = 0;
+		}
+		break;
+	}
+}
+
+#define VOLTAGE_V25		0x03
+
+/*  */
+/* 	The following is for compile ok */
+/* 	That should be merged with the original in the future */
+/*  */
+#define EFUSE_ACCESS_ON_8723			0x69	/*  For RTL8723 only. */
+#define REG_EFUSE_ACCESS_8723			0x00CF	/*  Efuse access protection for RTL8723 */
+
+/*  */
+static void Hal_BT_EfusePowerSwitch(
+	struct adapter *padapter, u8 bWrite, u8 PwrState
+)
+{
+	u8 tempval;
+	if (PwrState) {
+		/*  enable BT power cut */
+		/*  0x6A[14] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval |= BIT(6);
+		rtw_write8(padapter, 0x6B, tempval);
+
+		/*  Attention!! Between 0x6A[14] and 0x6A[15] setting need 100us delay */
+		/*  So don't write 0x6A[14]= 1 and 0x6A[15]= 0 together! */
+		msleep(1);
+		/*  disable BT output isolation */
+		/*  0x6A[15] = 0 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval &= ~BIT(7);
+		rtw_write8(padapter, 0x6B, tempval);
+	} else {
+		/*  enable BT output isolation */
+		/*  0x6A[15] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval |= BIT(7);
+		rtw_write8(padapter, 0x6B, tempval);
+
+		/*  Attention!! Between 0x6A[14] and 0x6A[15] setting need 100us delay */
+		/*  So don't write 0x6A[14]= 1 and 0x6A[15]= 0 together! */
+
+		/*  disable BT power cut */
+		/*  0x6A[14] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval &= ~BIT(6);
+		rtw_write8(padapter, 0x6B, tempval);
+	}
+
+}
+static void Hal_EfusePowerSwitch(
+	struct adapter *padapter, u8 bWrite, u8 PwrState
+)
+{
+	u8 tempval;
+	u16 tmpV16;
+
+
+	if (PwrState) {
+		/*  To avoid cannot access efuse regsiters after disable/enable several times during DTM test. */
+		/*  Suggested by SD1 IsaacHsu. 2013.07.08, added by tynli. */
+		tempval = rtw_read8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL);
+		if (tempval & BIT(0)) { /*  SDIO local register is suspend */
+			u8 count = 0;
+
+
+			tempval &= ~BIT(0);
+			rtw_write8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL, tempval);
+
+			/*  check 0x86[1:0]= 10'2h, wait power state to leave suspend */
+			do {
+				tempval = rtw_read8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL);
+				tempval &= 0x3;
+				if (tempval == 0x02)
+					break;
+
+				count++;
+				if (count >= 100)
+					break;
+
+				mdelay(10);
+			} while (1);
+		}
+
+		rtw_write8(padapter, REG_EFUSE_ACCESS_8723, EFUSE_ACCESS_ON_8723);
+
+		/*  Reset: 0x0000h[28], default valid */
+		tmpV16 =  rtw_read16(padapter, REG_SYS_FUNC_EN);
+		if (!(tmpV16 & FEN_ELDR)) {
+			tmpV16 |= FEN_ELDR;
+			rtw_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
+		}
+
+		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock from ANA, default valid */
+		tmpV16 = rtw_read16(padapter, REG_SYS_CLKR);
+		if ((!(tmpV16 & LOADER_CLK_EN))  || (!(tmpV16 & ANA8M))) {
+			tmpV16 |= (LOADER_CLK_EN | ANA8M);
+			rtw_write16(padapter, REG_SYS_CLKR, tmpV16);
+		}
+
+		if (bWrite) {
+			/*  Enable LDO 2.5V before read/write action */
+			tempval = rtw_read8(padapter, EFUSE_TEST+3);
+			tempval &= 0x0F;
+			tempval |= (VOLTAGE_V25 << 4);
+			rtw_write8(padapter, EFUSE_TEST+3, (tempval | 0x80));
+
+			/* rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON); */
+		}
+	} else {
+		rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
+
+		if (bWrite) {
+			/*  Disable LDO 2.5V after read/write action */
+			tempval = rtw_read8(padapter, EFUSE_TEST+3);
+			rtw_write8(padapter, EFUSE_TEST+3, (tempval & 0x7F));
+		}
+
+	}
+}
+
+static void hal_ReadEFuse_WiFi(
+	struct adapter *padapter,
+	u16 _offset,
+	u16 _size_byte,
+	u8 *pbuf,
+	bool bPseudoTest
+)
+{
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+#endif
+	u8 *efuseTbl = NULL;
+	u16 eFuse_Addr = 0;
+	u8 offset, wden;
+	u8 efuseHeader, efuseExtHdr, efuseData;
+	u16 i, total, used;
+	u8 efuse_usage = 0;
+
+	/*  */
+	/*  Do NOT excess total size of EFuse table. Added by Roger, 2008.11.10. */
+	/*  */
+	if ((_offset + _size_byte) > EFUSE_MAX_MAP_LEN)
+		return;
+
+	efuseTbl = rtw_malloc(EFUSE_MAX_MAP_LEN);
+	if (!efuseTbl)
+		return;
+
+	/*  0xff will be efuse default value instead of 0x00. */
+	memset(efuseTbl, 0xFF, EFUSE_MAX_MAP_LEN);
+
+	/*  switch bank back to bank 0 for later BT and wifi use. */
+	hal_EfuseSwitchToBank(padapter, 0, bPseudoTest);
+
+	while (AVAILABLE_EFUSE_ADDR(eFuse_Addr)) {
+		efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest);
+		if (efuseHeader == 0xFF)
+			break;
+
+		/*  Check PG header for section num. */
+		if (EXT_HEADER(efuseHeader)) { /* extended header */
+			offset = GET_HDR_OFFSET_2_0(efuseHeader);
+
+			efuse_OneByteRead(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest);
+			if (ALL_WORDS_DISABLED(efuseExtHdr))
+				continue;
+
+			offset |= ((efuseExtHdr & 0xF0) >> 1);
+			wden = (efuseExtHdr & 0x0F);
+		} else {
+			offset = ((efuseHeader >> 4) & 0x0f);
+			wden = (efuseHeader & 0x0f);
+		}
+
+		if (offset < EFUSE_MAX_SECTION_8723B) {
+			u16 addr;
+			/*  Get word enable value from PG header */
+
+			addr = offset * PGPKT_DATA_SIZE;
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				/*  Check word enable condition in the section */
+				if (!(wden & (0x01<<i))) {
+					efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+					efuseTbl[addr] = efuseData;
+
+					efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+					efuseTbl[addr+1] = efuseData;
+				}
+				addr += 2;
+			}
+		} else {
+			eFuse_Addr += Efuse_CalculateWordCnts(wden)*2;
+		}
+	}
+
+	/*  Copy from Efuse map to output pointer memory!!! */
+	for (i = 0; i < _size_byte; i++)
+		pbuf[i] = efuseTbl[_offset+i];
+
+	/*  Calculate Efuse utilization */
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total, bPseudoTest);
+	used = eFuse_Addr - 1;
+	efuse_usage = (u8)((used*100)/total);
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		pEfuseHal->fakeEfuseUsedBytes = used;
+#else
+		fakeEfuseUsedBytes = used;
+#endif
+	} else {
+		rtw_hal_set_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&used);
+		rtw_hal_set_hwreg(padapter, HW_VAR_EFUSE_USAGE, (u8 *)&efuse_usage);
+	}
+
+	kfree(efuseTbl);
+}
+
+static void hal_ReadEFuse_BT(
+	struct adapter *padapter,
+	u16 _offset,
+	u16 _size_byte,
+	u8 *pbuf,
+	bool bPseudoTest
+)
+{
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+#endif
+	u8 *efuseTbl;
+	u8 bank;
+	u16 eFuse_Addr;
+	u8 efuseHeader, efuseExtHdr, efuseData;
+	u8 offset, wden;
+	u16 i, total, used;
+	u8 efuse_usage;
+
+
+	/*  */
+	/*  Do NOT excess total size of EFuse table. Added by Roger, 2008.11.10. */
+	/*  */
+	if ((_offset + _size_byte) > EFUSE_BT_MAP_LEN)
+		return;
+
+	efuseTbl = rtw_malloc(EFUSE_BT_MAP_LEN);
+	if (!efuseTbl)
+		return;
+
+	/*  0xff will be efuse default value instead of 0x00. */
+	memset(efuseTbl, 0xFF, EFUSE_BT_MAP_LEN);
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_AVAILABLE_EFUSE_BYTES_BANK, &total, bPseudoTest);
+
+	for (bank = 1; bank < 3; bank++) { /*  8723b Max bake 0~2 */
+		if (hal_EfuseSwitchToBank(padapter, bank, bPseudoTest) == false)
+			goto exit;
+
+		eFuse_Addr = 0;
+
+		while (AVAILABLE_EFUSE_ADDR(eFuse_Addr)) {
+			efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest);
+			if (efuseHeader == 0xFF)
+				break;
+
+			/*  Check PG header for section num. */
+			if (EXT_HEADER(efuseHeader)) { /* extended header */
+				offset = GET_HDR_OFFSET_2_0(efuseHeader);
+
+				efuse_OneByteRead(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest);
+				if (ALL_WORDS_DISABLED(efuseExtHdr))
+					continue;
+
+
+				offset |= ((efuseExtHdr & 0xF0) >> 1);
+				wden = (efuseExtHdr & 0x0F);
+			} else {
+				offset = ((efuseHeader >> 4) & 0x0f);
+				wden = (efuseHeader & 0x0f);
+			}
+
+			if (offset < EFUSE_BT_MAX_SECTION) {
+				u16 addr;
+
+				addr = offset * PGPKT_DATA_SIZE;
+				for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+					/*  Check word enable condition in the section */
+					if (!(wden & (0x01<<i))) {
+						efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+						efuseTbl[addr] = efuseData;
+
+						efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+						efuseTbl[addr+1] = efuseData;
+					}
+					addr += 2;
+				}
+			} else {
+				eFuse_Addr += Efuse_CalculateWordCnts(wden)*2;
+			}
+		}
+
+		if ((eFuse_Addr - 1) < total)
+			break;
+
+	}
+
+	/*  switch bank back to bank 0 for later BT and wifi use. */
+	hal_EfuseSwitchToBank(padapter, 0, bPseudoTest);
+
+	/*  Copy from Efuse map to output pointer memory!!! */
+	for (i = 0; i < _size_byte; i++)
+		pbuf[i] = efuseTbl[_offset+i];
+
+	/*  */
+	/*  Calculate Efuse utilization. */
+	/*  */
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total, bPseudoTest);
+	used = (EFUSE_BT_REAL_BANK_CONTENT_LEN*(bank-1)) + eFuse_Addr - 1;
+	efuse_usage = (u8)((used*100)/total);
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		pEfuseHal->fakeBTEfuseUsedBytes = used;
+#else
+		fakeBTEfuseUsedBytes = used;
+#endif
+	} else {
+		rtw_hal_set_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *)&used);
+		rtw_hal_set_hwreg(padapter, HW_VAR_EFUSE_BT_USAGE, (u8 *)&efuse_usage);
+	}
+
+exit:
+	kfree(efuseTbl);
+}
+
+static void Hal_ReadEFuse(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 _offset,
+	u16 _size_byte,
+	u8 *pbuf,
+	bool bPseudoTest
+)
+{
+	if (efuseType == EFUSE_WIFI)
+		hal_ReadEFuse_WiFi(padapter, _offset, _size_byte, pbuf, bPseudoTest);
+	else
+		hal_ReadEFuse_BT(padapter, _offset, _size_byte, pbuf, bPseudoTest);
+}
+
+static u16 hal_EfuseGetCurrentSize_WiFi(
+	struct adapter *padapter, bool bPseudoTest
+)
+{
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+#endif
+	u16 efuse_addr = 0;
+	u16 start_addr = 0; /*  for debug */
+	u8 hoffset = 0, hworden = 0;
+	u8 efuse_data, word_cnts = 0;
+	u32 count = 0; /*  for debug */
+
+
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		efuse_addr = (u16)pEfuseHal->fakeEfuseUsedBytes;
+#else
+		efuse_addr = (u16)fakeEfuseUsedBytes;
+#endif
+	} else
+		rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&efuse_addr);
+
+	start_addr = efuse_addr;
+
+	/*  switch bank back to bank 0 for later BT and wifi use. */
+	hal_EfuseSwitchToBank(padapter, 0, bPseudoTest);
+
+	count = 0;
+	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
+		if (efuse_OneByteRead(padapter, efuse_addr, &efuse_data, bPseudoTest) == false)
+			goto error;
+
+		if (efuse_data == 0xFF)
+			break;
+
+		if ((start_addr != 0) && (efuse_addr == start_addr)) {
+			count++;
+
+			efuse_data = 0xFF;
+			if (count < 4) {
+				/*  try again! */
+
+				if (count > 2) {
+					/*  try again form address 0 */
+					efuse_addr = 0;
+					start_addr = 0;
+				}
+
+				continue;
+			}
+
+			goto error;
+		}
+
+		if (EXT_HEADER(efuse_data)) {
+			hoffset = GET_HDR_OFFSET_2_0(efuse_data);
+			efuse_addr++;
+			efuse_OneByteRead(padapter, efuse_addr, &efuse_data, bPseudoTest);
+			if (ALL_WORDS_DISABLED(efuse_data))
+				continue;
+
+			hoffset |= ((efuse_data & 0xF0) >> 1);
+			hworden = efuse_data & 0x0F;
+		} else {
+			hoffset = (efuse_data>>4) & 0x0F;
+			hworden = efuse_data & 0x0F;
+		}
+
+		word_cnts = Efuse_CalculateWordCnts(hworden);
+		efuse_addr += (word_cnts*2)+1;
+	}
+
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		pEfuseHal->fakeEfuseUsedBytes = efuse_addr;
+#else
+		fakeEfuseUsedBytes = efuse_addr;
+#endif
+	} else
+		rtw_hal_set_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&efuse_addr);
+
+	goto exit;
+
+error:
+	/*  report max size to prevent write efuse */
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &efuse_addr, bPseudoTest);
+
+exit:
+
+	return efuse_addr;
+}
+
+static u16 hal_EfuseGetCurrentSize_BT(struct adapter *padapter, u8 bPseudoTest)
+{
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+#endif
+	u16 btusedbytes;
+	u16 efuse_addr;
+	u8 bank, startBank;
+	u8 hoffset = 0, hworden = 0;
+	u8 efuse_data, word_cnts = 0;
+	u16 retU2 = 0;
+
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		btusedbytes = pEfuseHal->fakeBTEfuseUsedBytes;
+#else
+		btusedbytes = fakeBTEfuseUsedBytes;
+#endif
+	} else
+		rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *)&btusedbytes);
+
+	efuse_addr = (u16)((btusedbytes%EFUSE_BT_REAL_BANK_CONTENT_LEN));
+	startBank = (u8)(1+(btusedbytes/EFUSE_BT_REAL_BANK_CONTENT_LEN));
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_AVAILABLE_EFUSE_BYTES_BANK, &retU2, bPseudoTest);
+
+	for (bank = startBank; bank < 3; bank++) {
+		if (hal_EfuseSwitchToBank(padapter, bank, bPseudoTest) == false)
+			/* bank = EFUSE_MAX_BANK; */
+			break;
+
+		/*  only when bank is switched we have to reset the efuse_addr. */
+		if (bank != startBank)
+			efuse_addr = 0;
+#if 1
+
+		while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
+			if (efuse_OneByteRead(padapter, efuse_addr,
+					      &efuse_data, bPseudoTest) == false)
+				/* bank = EFUSE_MAX_BANK; */
+				break;
+
+			if (efuse_data == 0xFF)
+				break;
+
+			if (EXT_HEADER(efuse_data)) {
+				hoffset = GET_HDR_OFFSET_2_0(efuse_data);
+				efuse_addr++;
+				efuse_OneByteRead(padapter, efuse_addr, &efuse_data, bPseudoTest);
+
+				if (ALL_WORDS_DISABLED(efuse_data)) {
+					efuse_addr++;
+					continue;
+				}
+
+/* 				hoffset = ((hoffset & 0xE0) >> 5) | ((efuse_data & 0xF0) >> 1); */
+				hoffset |= ((efuse_data & 0xF0) >> 1);
+				hworden = efuse_data & 0x0F;
+			} else {
+				hoffset = (efuse_data>>4) & 0x0F;
+				hworden =  efuse_data & 0x0F;
+			}
+
+			word_cnts = Efuse_CalculateWordCnts(hworden);
+			/* read next header */
+			efuse_addr += (word_cnts*2)+1;
+		}
+#else
+	while (
+		bContinual &&
+		efuse_OneByteRead(padapter, efuse_addr, &efuse_data, bPseudoTest) &&
+		AVAILABLE_EFUSE_ADDR(efuse_addr)
+	) {
+			if (efuse_data != 0xFF) {
+				if ((efuse_data&0x1F) == 0x0F) { /* extended header */
+					hoffset = efuse_data;
+					efuse_addr++;
+					efuse_OneByteRead(padapter, efuse_addr, &efuse_data, bPseudoTest);
+					if ((efuse_data & 0x0F) == 0x0F) {
+						efuse_addr++;
+						continue;
+					} else {
+						hoffset = ((hoffset & 0xE0) >> 5) | ((efuse_data & 0xF0) >> 1);
+						hworden = efuse_data & 0x0F;
+					}
+				} else {
+					hoffset = (efuse_data>>4) & 0x0F;
+					hworden =  efuse_data & 0x0F;
+				}
+				word_cnts = Efuse_CalculateWordCnts(hworden);
+				/* read next header */
+				efuse_addr = efuse_addr + (word_cnts*2)+1;
+			} else
+				bContinual = false;
+		}
+#endif
+
+
+		/*  Check if we need to check next bank efuse */
+		if (efuse_addr < retU2)
+			break; /*  don't need to check next bank. */
+	}
+
+	retU2 = ((bank-1)*EFUSE_BT_REAL_BANK_CONTENT_LEN)+efuse_addr;
+	if (bPseudoTest) {
+		pEfuseHal->fakeBTEfuseUsedBytes = retU2;
+	} else {
+		pEfuseHal->BTEfuseUsedBytes = retU2;
+	}
+
+	return retU2;
+}
+
+static u16 Hal_EfuseGetCurrentSize(
+	struct adapter *padapter, u8 efuseType, bool bPseudoTest
+)
+{
+	u16 ret = 0;
+
+	if (efuseType == EFUSE_WIFI)
+		ret = hal_EfuseGetCurrentSize_WiFi(padapter, bPseudoTest);
+	else
+		ret = hal_EfuseGetCurrentSize_BT(padapter, bPseudoTest);
+
+	return ret;
+}
+
+static u8 Hal_EfuseWordEnableDataWrite(
+	struct adapter *padapter,
+	u16 efuse_addr,
+	u8 word_en,
+	u8 *data,
+	bool bPseudoTest
+)
+{
+	u16 tmpaddr = 0;
+	u16 start_addr = efuse_addr;
+	u8 badworden = 0x0F;
+	u8 tmpdata[PGPKT_DATA_SIZE];
+
+	memset(tmpdata, 0xFF, PGPKT_DATA_SIZE);
+
+	if (!(word_en & BIT(0))) {
+		tmpaddr = start_addr;
+		efuse_OneByteWrite(padapter, start_addr++, data[0], bPseudoTest);
+		efuse_OneByteWrite(padapter, start_addr++, data[1], bPseudoTest);
+
+		efuse_OneByteRead(padapter, tmpaddr, &tmpdata[0], bPseudoTest);
+		efuse_OneByteRead(padapter, tmpaddr+1, &tmpdata[1], bPseudoTest);
+		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1])) {
+			badworden &= (~BIT(0));
+		}
+	}
+	if (!(word_en & BIT(1))) {
+		tmpaddr = start_addr;
+		efuse_OneByteWrite(padapter, start_addr++, data[2], bPseudoTest);
+		efuse_OneByteWrite(padapter, start_addr++, data[3], bPseudoTest);
+
+		efuse_OneByteRead(padapter, tmpaddr, &tmpdata[2], bPseudoTest);
+		efuse_OneByteRead(padapter, tmpaddr+1, &tmpdata[3], bPseudoTest);
+		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3])) {
+			badworden &= (~BIT(1));
+		}
+	}
+
+	if (!(word_en & BIT(2))) {
+		tmpaddr = start_addr;
+		efuse_OneByteWrite(padapter, start_addr++, data[4], bPseudoTest);
+		efuse_OneByteWrite(padapter, start_addr++, data[5], bPseudoTest);
+
+		efuse_OneByteRead(padapter, tmpaddr, &tmpdata[4], bPseudoTest);
+		efuse_OneByteRead(padapter, tmpaddr+1, &tmpdata[5], bPseudoTest);
+		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5])) {
+			badworden &= (~BIT(2));
+		}
+	}
+
+	if (!(word_en & BIT(3))) {
+		tmpaddr = start_addr;
+		efuse_OneByteWrite(padapter, start_addr++, data[6], bPseudoTest);
+		efuse_OneByteWrite(padapter, start_addr++, data[7], bPseudoTest);
+
+		efuse_OneByteRead(padapter, tmpaddr, &tmpdata[6], bPseudoTest);
+		efuse_OneByteRead(padapter, tmpaddr+1, &tmpdata[7], bPseudoTest);
+		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7])) {
+			badworden &= (~BIT(3));
+		}
+	}
+
+	return badworden;
+}
+
+static s32 Hal_EfusePgPacketRead(
+	struct adapter *padapter,
+	u8 offset,
+	u8 *data,
+	bool bPseudoTest
+)
+{
+	u8 efuse_data, word_cnts = 0;
+	u16 efuse_addr = 0;
+	u8 hoffset = 0, hworden = 0;
+	u8 i;
+	u8 max_section = 0;
+	s32	ret;
+
+
+	if (!data)
+		return false;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAX_SECTION, &max_section, bPseudoTest);
+	if (offset > max_section)
+		return false;
+
+	memset(data, 0xFF, PGPKT_DATA_SIZE);
+	ret = true;
+
+	/*  */
+	/*  <Roger_TODO> Efuse has been pre-programmed dummy 5Bytes at the end of Efuse by CP. */
+	/*  Skip dummy parts to prevent unexpected data read from Efuse. */
+	/*  By pass right now. 2009.02.19. */
+	/*  */
+	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
+		if (efuse_OneByteRead(padapter, efuse_addr++, &efuse_data, bPseudoTest) == false) {
+			ret = false;
+			break;
+		}
+
+		if (efuse_data == 0xFF)
+			break;
+
+		if (EXT_HEADER(efuse_data)) {
+			hoffset = GET_HDR_OFFSET_2_0(efuse_data);
+			efuse_OneByteRead(padapter, efuse_addr++, &efuse_data, bPseudoTest);
+			if (ALL_WORDS_DISABLED(efuse_data))
+				continue;
+
+			hoffset |= ((efuse_data & 0xF0) >> 1);
+			hworden = efuse_data & 0x0F;
+		} else {
+			hoffset = (efuse_data>>4) & 0x0F;
+			hworden =  efuse_data & 0x0F;
+		}
+
+		if (hoffset == offset) {
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				/*  Check word enable condition in the section */
+				if (!(hworden & (0x01<<i))) {
+					efuse_OneByteRead(padapter, efuse_addr++, &efuse_data, bPseudoTest);
+					data[i*2] = efuse_data;
+
+					efuse_OneByteRead(padapter, efuse_addr++, &efuse_data, bPseudoTest);
+					data[(i*2)+1] = efuse_data;
+				}
+			}
+		} else {
+			word_cnts = Efuse_CalculateWordCnts(hworden);
+			efuse_addr += word_cnts*2;
+		}
+	}
+
+	return ret;
+}
+
+static u8 hal_EfusePgCheckAvailableAddr(
+	struct adapter *padapter, u8 efuseType, u8 bPseudoTest
+)
+{
+	u16 max_available = 0;
+	u16 current_size;
+
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &max_available, bPseudoTest);
+
+	current_size = Efuse_GetCurrentSize(padapter, efuseType, bPseudoTest);
+	if (current_size >= max_available)
+		return false;
+
+	return true;
+}
+
+static void hal_EfuseConstructPGPkt(
+	u8 offset,
+	u8 word_en,
+	u8 *pData,
+	struct pgpkt_struct *pTargetPkt
+)
+{
+	memset(pTargetPkt->data, 0xFF, PGPKT_DATA_SIZE);
+	pTargetPkt->offset = offset;
+	pTargetPkt->word_en = word_en;
+	efuse_WordEnableDataRead(word_en, pData, pTargetPkt->data);
+	pTargetPkt->word_cnts = Efuse_CalculateWordCnts(pTargetPkt->word_en);
+}
+
+static u8 hal_EfusePartialWriteCheck(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	struct pgpkt_struct *pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct efuse_hal *pEfuseHal = &pHalData->EfuseHal;
+	u8 bRet = false;
+	u16 startAddr = 0, efuse_max_available_len = 0, efuse_max = 0;
+	u8 efuse_data = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &efuse_max_available_len, bPseudoTest);
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_CONTENT_LEN_BANK, &efuse_max, bPseudoTest);
+
+	if (efuseType == EFUSE_WIFI) {
+		if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+			startAddr = (u16)pEfuseHal->fakeEfuseUsedBytes;
+#else
+			startAddr = (u16)fakeEfuseUsedBytes;
+#endif
+		} else
+			rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&startAddr);
+	} else {
+		if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+			startAddr = (u16)pEfuseHal->fakeBTEfuseUsedBytes;
+#else
+			startAddr = (u16)fakeBTEfuseUsedBytes;
+#endif
+		} else
+			rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *)&startAddr);
+	}
+	startAddr %= efuse_max;
+
+	while (1) {
+		if (startAddr >= efuse_max_available_len) {
+			bRet = false;
+			break;
+		}
+
+		if (efuse_OneByteRead(padapter, startAddr, &efuse_data, bPseudoTest) && (efuse_data != 0xFF)) {
+#if 1
+			bRet = false;
+			break;
+#else
+			if (EXT_HEADER(efuse_data)) {
+				cur_header = efuse_data;
+				startAddr++;
+				efuse_OneByteRead(padapter, startAddr, &efuse_data, bPseudoTest);
+				if (ALL_WORDS_DISABLED(efuse_data)) {
+					bRet = false;
+					break;
+				} else {
+					curPkt.offset = ((cur_header & 0xE0) >> 5) | ((efuse_data & 0xF0) >> 1);
+					curPkt.word_en = efuse_data & 0x0F;
+				}
+			} else {
+				cur_header  =  efuse_data;
+				curPkt.offset = (cur_header>>4) & 0x0F;
+				curPkt.word_en = cur_header & 0x0F;
+			}
+
+			curPkt.word_cnts = Efuse_CalculateWordCnts(curPkt.word_en);
+			/*  if same header is found but no data followed */
+			/*  write some part of data followed by the header. */
+			if (
+				(curPkt.offset == pTargetPkt->offset) &&
+				(hal_EfuseCheckIfDatafollowed(padapter, curPkt.word_cnts, startAddr+1, bPseudoTest) == false) &&
+				wordEnMatched(pTargetPkt, &curPkt, &matched_wden) == true
+			) {
+				/*  Here to write partial data */
+				badworden = Efuse_WordEnableDataWrite(padapter, startAddr+1, matched_wden, pTargetPkt->data, bPseudoTest);
+				if (badworden != 0x0F) {
+					u32 PgWriteSuccess = 0;
+					/*  if write fail on some words, write these bad words again */
+					if (efuseType == EFUSE_WIFI)
+						PgWriteSuccess = Efuse_PgPacketWrite(padapter, pTargetPkt->offset, badworden, pTargetPkt->data, bPseudoTest);
+					else
+						PgWriteSuccess = Efuse_PgPacketWrite_BT(padapter, pTargetPkt->offset, badworden, pTargetPkt->data, bPseudoTest);
+
+					if (!PgWriteSuccess) {
+						bRet = false;	/*  write fail, return */
+						break;
+					}
+				}
+				/*  partial write ok, update the target packet for later use */
+				for (i = 0; i < 4; i++) {
+					if ((matched_wden & (0x1<<i)) == 0) { /*  this word has been written */
+						pTargetPkt->word_en |= (0x1<<i);	/*  disable the word */
+					}
+				}
+				pTargetPkt->word_cnts = Efuse_CalculateWordCnts(pTargetPkt->word_en);
+			}
+			/*  read from next header */
+			startAddr = startAddr + (curPkt.word_cnts*2) + 1;
+#endif
+		} else {
+			/*  not used header, 0xff */
+			*pAddr = startAddr;
+			bRet = true;
+			break;
+		}
+	}
+
+	return bRet;
+}
+
+static u8 hal_EfusePgPacketWrite1ByteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	struct pgpkt_struct *pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u8 pg_header = 0, tmp_header = 0;
+	u16 efuse_addr = *pAddr;
+	u8 repeatcnt = 0;
+
+	pg_header = ((pTargetPkt->offset << 4) & 0xf0) | pTargetPkt->word_en;
+
+	do {
+		efuse_OneByteWrite(padapter, efuse_addr, pg_header, bPseudoTest);
+		efuse_OneByteRead(padapter, efuse_addr, &tmp_header, bPseudoTest);
+		if (tmp_header != 0xFF)
+			break;
+		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_)
+			return false;
+
+	} while (1);
+
+	if (tmp_header != pg_header)
+		return false;
+
+	*pAddr = efuse_addr;
+
+	return true;
+}
+
+static u8 hal_EfusePgPacketWrite2ByteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	struct pgpkt_struct *pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u16 efuse_addr, efuse_max_available_len = 0;
+	u8 pg_header = 0, tmp_header = 0;
+	u8 repeatcnt = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_AVAILABLE_EFUSE_BYTES_BANK, &efuse_max_available_len, bPseudoTest);
+
+	efuse_addr = *pAddr;
+	if (efuse_addr >= efuse_max_available_len)
+		return false;
+
+	pg_header = ((pTargetPkt->offset & 0x07) << 5) | 0x0F;
+
+	do {
+		efuse_OneByteWrite(padapter, efuse_addr, pg_header, bPseudoTest);
+		efuse_OneByteRead(padapter, efuse_addr, &tmp_header, bPseudoTest);
+		if (tmp_header != 0xFF)
+			break;
+		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_)
+			return false;
+
+	} while (1);
+
+	if (tmp_header != pg_header)
+		return false;
+
+	/*  to write ext_header */
+	efuse_addr++;
+	pg_header = ((pTargetPkt->offset & 0x78) << 1) | pTargetPkt->word_en;
+
+	do {
+		efuse_OneByteWrite(padapter, efuse_addr, pg_header, bPseudoTest);
+		efuse_OneByteRead(padapter, efuse_addr, &tmp_header, bPseudoTest);
+		if (tmp_header != 0xFF)
+			break;
+		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_)
+			return false;
+
+	} while (1);
+
+	if (tmp_header != pg_header) /* offset PG fail */
+		return false;
+
+	*pAddr = efuse_addr;
+
+	return true;
+}
+
+static u8 hal_EfusePgPacketWriteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	struct pgpkt_struct *pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u8 bRet = false;
+
+	if (pTargetPkt->offset >= EFUSE_MAX_SECTION_BASE)
+		bRet = hal_EfusePgPacketWrite2ByteHeader(padapter, efuseType, pAddr, pTargetPkt, bPseudoTest);
+	else
+		bRet = hal_EfusePgPacketWrite1ByteHeader(padapter, efuseType, pAddr, pTargetPkt, bPseudoTest);
+
+	return bRet;
+}
+
+static u8 hal_EfusePgPacketWriteData(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	struct pgpkt_struct *pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u16 efuse_addr;
+	u8 badworden;
+
+
+	efuse_addr = *pAddr;
+	badworden = Efuse_WordEnableDataWrite(padapter, efuse_addr+1, pTargetPkt->word_en, pTargetPkt->data, bPseudoTest);
+	if (badworden != 0x0F)
+		return false;
+
+	return true;
+}
+
+static s32 Hal_EfusePgPacketWrite(
+	struct adapter *padapter,
+	u8 offset,
+	u8 word_en,
+	u8 *pData,
+	bool bPseudoTest
+)
+{
+	struct pgpkt_struct targetPkt;
+	u16 startAddr = 0;
+	u8 efuseType = EFUSE_WIFI;
+
+	if (!hal_EfusePgCheckAvailableAddr(padapter, efuseType, bPseudoTest))
+		return false;
+
+	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
+
+	if (!hal_EfusePartialWriteCheck(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	if (!hal_EfusePgPacketWriteHeader(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	if (!hal_EfusePgPacketWriteData(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	return true;
+}
+
+static bool Hal_EfusePgPacketWrite_BT(
+	struct adapter *padapter,
+	u8 offset,
+	u8 word_en,
+	u8 *pData,
+	bool bPseudoTest
+)
+{
+	struct pgpkt_struct targetPkt;
+	u16 startAddr = 0;
+	u8 efuseType = EFUSE_BT;
+
+	if (!hal_EfusePgCheckAvailableAddr(padapter, efuseType, bPseudoTest))
+		return false;
+
+	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
+
+	if (!hal_EfusePartialWriteCheck(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	if (!hal_EfusePgPacketWriteHeader(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	if (!hal_EfusePgPacketWriteData(padapter, efuseType, &startAddr, &targetPkt, bPseudoTest))
+		return false;
+
+	return true;
+}
+
+static struct hal_version ReadChipVersion8723B(struct adapter *padapter)
+{
+	u32 value32;
+	struct hal_version ChipVersion;
+	struct hal_com_data *pHalData;
+
+/* YJ, TODO, move read chip type here */
+	pHalData = GET_HAL_DATA(padapter);
+
+	value32 = rtw_read32(padapter, REG_SYS_CFG);
+	ChipVersion.ICType = CHIP_8723B;
+	ChipVersion.ChipType = ((value32 & RTL_ID) ? TEST_CHIP : NORMAL_CHIP);
+	ChipVersion.VendorType = ((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : CHIP_VENDOR_TSMC);
+	ChipVersion.CUTVersion = (value32 & CHIP_VER_RTL_MASK)>>CHIP_VER_RTL_SHIFT; /*  IC version (CUT) */
+
+	/*  For regulator mode. by tynli. 2011.01.14 */
+	pHalData->RegulatorMode = ((value32 & SPS_SEL) ? RT_LDO_REGULATOR : RT_SWITCHING_REGULATOR);
+
+	value32 = rtw_read32(padapter, REG_GPIO_OUTSTS);
+	ChipVersion.ROMVer = ((value32 & RF_RL_ID) >> 20);	/*  ROM code version. */
+
+	/*  For multi-function consideration. Added by Roger, 2010.10.06. */
+	pHalData->MultiFunc = RT_MULTI_FUNC_NONE;
+	value32 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+	pHalData->MultiFunc |= ((value32 & WL_FUNC_EN) ? RT_MULTI_FUNC_WIFI : 0);
+	pHalData->MultiFunc |= ((value32 & BT_FUNC_EN) ? RT_MULTI_FUNC_BT : 0);
+	pHalData->MultiFunc |= ((value32 & GPS_FUNC_EN) ? RT_MULTI_FUNC_GPS : 0);
+	pHalData->PolarityCtl = ((value32 & WL_HWPDN_SL) ? RT_POLARITY_HIGH_ACT : RT_POLARITY_LOW_ACT);
+#if 1
+	dump_chip_info(ChipVersion);
+#endif
+	pHalData->VersionID = ChipVersion;
+
+	return ChipVersion;
+}
+
+static void rtl8723b_read_chip_version(struct adapter *padapter)
+{
+	ReadChipVersion8723B(padapter);
+}
+
+void rtl8723b_InitBeaconParameters(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u16 val16;
+	u8 val8;
+
+
+	val8 = DIS_TSF_UDT;
+	val16 = val8 | (val8 << 8); /*  port0 and port1 */
+
+	/*  Enable prot0 beacon function for PSTDMA */
+	val16 |= EN_BCN_FUNCTION;
+
+	rtw_write16(padapter, REG_BCN_CTRL, val16);
+
+	/*  TODO: Remove these magic number */
+	rtw_write16(padapter, REG_TBTT_PROHIBIT, 0x6404);/*  ms */
+	/*  Firmware will control REG_DRVERLYINT when power saving is enable, */
+	/*  so don't set this register on STA mode. */
+	if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == false)
+		rtw_write8(padapter, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME_8723B); /*  5ms */
+	rtw_write8(padapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME_8723B); /*  2ms */
+
+	/*  Suggested by designer timchen. Change beacon AIFS to the largest number */
+	/*  beacause test chip does not contension before sending beacon. by tynli. 2009.11.03 */
+	rtw_write16(padapter, REG_BCNTCFG, 0x660F);
+
+	pHalData->RegBcnCtrlVal = rtw_read8(padapter, REG_BCN_CTRL);
+	pHalData->RegTxPause = rtw_read8(padapter, REG_TXPAUSE);
+	pHalData->RegFwHwTxQCtrl = rtw_read8(padapter, REG_FWHW_TXQ_CTRL+2);
+	pHalData->RegReg542 = rtw_read8(padapter, REG_TBTT_PROHIBIT+2);
+	pHalData->RegCR_1 = rtw_read8(padapter, REG_CR+1);
+}
+
+void _InitBurstPktLen_8723BS(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	rtw_write8(Adapter, 0x4c7, rtw_read8(Adapter, 0x4c7)|BIT(7)); /* enable single pkt ampdu */
+	rtw_write8(Adapter, REG_RX_PKT_LIMIT_8723B, 0x18);		/* for VHT packet length 11K */
+	rtw_write8(Adapter, REG_MAX_AGGR_NUM_8723B, 0x1F);
+	rtw_write8(Adapter, REG_PIFS_8723B, 0x00);
+	rtw_write8(Adapter, REG_FWHW_TXQ_CTRL_8723B, rtw_read8(Adapter, REG_FWHW_TXQ_CTRL)&(~BIT(7)));
+	if (pHalData->AMPDUBurstMode)
+		rtw_write8(Adapter, REG_AMPDU_BURST_MODE_8723B,  0x5F);
+	rtw_write8(Adapter, REG_AMPDU_MAX_TIME_8723B, 0x70);
+
+	/*  ARFB table 9 for 11ac 5G 2SS */
+	rtw_write32(Adapter, REG_ARFR0_8723B, 0x00000010);
+	if (IS_NORMAL_CHIP(pHalData->VersionID))
+		rtw_write32(Adapter, REG_ARFR0_8723B+4, 0xfffff000);
+	else
+		rtw_write32(Adapter, REG_ARFR0_8723B+4, 0x3e0ff000);
+
+	/*  ARFB table 10 for 11ac 5G 1SS */
+	rtw_write32(Adapter, REG_ARFR1_8723B, 0x00000010);
+	rtw_write32(Adapter, REG_ARFR1_8723B+4, 0x003ff000);
+}
+
+static void ResumeTxBeacon(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->RegFwHwTxQCtrl |= BIT(6);
+	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	rtw_write8(padapter, REG_TBTT_PROHIBIT+1, 0xff);
+	pHalData->RegReg542 |= BIT(0);
+	rtw_write8(padapter, REG_TBTT_PROHIBIT+2, pHalData->RegReg542);
+}
+
+static void StopTxBeacon(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
+	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	rtw_write8(padapter, REG_TBTT_PROHIBIT+1, 0x64);
+	pHalData->RegReg542 &= ~BIT(0);
+	rtw_write8(padapter, REG_TBTT_PROHIBIT+2, pHalData->RegReg542);
+
+	CheckFwRsvdPageContent(padapter);  /*  2010.06.23. Added by tynli. */
+}
+
+static void _BeaconFunctionEnable(struct adapter *padapter, u8 Enable, u8 Linked)
+{
+	rtw_write8(padapter, REG_BCN_CTRL, DIS_TSF_UDT | EN_BCN_FUNCTION | DIS_BCNQ_SUB);
+	rtw_write8(padapter, REG_RD_CTRL+1, 0x6F);
+}
+
+static void rtl8723b_SetBeaconRelatedRegisters(struct adapter *padapter)
+{
+	u8 val8;
+	u32 value32;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u32 bcn_ctrl_reg;
+
+	/* reset TSF, enable update TSF, correcting TSF On Beacon */
+
+	/* REG_BCN_INTERVAL */
+	/* REG_BCNDMATIM */
+	/* REG_ATIMWND */
+	/* REG_TBTT_PROHIBIT */
+	/* REG_DRVERLYINT */
+	/* REG_BCN_MAX_ERR */
+	/* REG_BCNTCFG (0x510) */
+	/* REG_DUAL_TSF_RST */
+	/* REG_BCN_CTRL (0x550) */
+
+
+	bcn_ctrl_reg = REG_BCN_CTRL;
+
+	/*  */
+	/*  ATIM window */
+	/*  */
+	rtw_write16(padapter, REG_ATIMWND, 2);
+
+	/*  */
+	/*  Beacon interval (in unit of TU). */
+	/*  */
+	rtw_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
+
+	rtl8723b_InitBeaconParameters(padapter);
+
+	rtw_write8(padapter, REG_SLOT, 0x09);
+
+	/*  */
+	/*  Reset TSF Timer to zero, added by Roger. 2008.06.24 */
+	/*  */
+	value32 = rtw_read32(padapter, REG_TCR);
+	value32 &= ~TSFRST;
+	rtw_write32(padapter, REG_TCR, value32);
+
+	value32 |= TSFRST;
+	rtw_write32(padapter, REG_TCR, value32);
+
+	/*  NOTE: Fix test chip's bug (about contention windows's randomness) */
+	if (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == true) {
+		rtw_write8(padapter, REG_RXTSF_OFFSET_CCK, 0x50);
+		rtw_write8(padapter, REG_RXTSF_OFFSET_OFDM, 0x50);
+	}
+
+	_BeaconFunctionEnable(padapter, true, true);
+
+	ResumeTxBeacon(padapter);
+	val8 = rtw_read8(padapter, bcn_ctrl_reg);
+	val8 |= DIS_BCNQ_SUB;
+	rtw_write8(padapter, bcn_ctrl_reg, val8);
+}
+
+static void rtl8723b_GetHalODMVar(
+	struct adapter *Adapter,
+	enum hal_odm_variable eVariable,
+	void *pValue1,
+	void *pValue2
+)
+{
+	GetHalODMVar(Adapter, eVariable, pValue1, pValue2);
+}
+
+static void rtl8723b_SetHalODMVar(
+	struct adapter *Adapter,
+	enum hal_odm_variable eVariable,
+	void *pValue1,
+	bool bSet
+)
+{
+	SetHalODMVar(Adapter, eVariable, pValue1, bSet);
+}
+
+static void hal_notch_filter_8723b(struct adapter *adapter, bool enable)
+{
+	if (enable)
+		rtw_write8(adapter, rOFDM0_RxDSP+1, rtw_read8(adapter, rOFDM0_RxDSP+1) | BIT1);
+	else
+		rtw_write8(adapter, rOFDM0_RxDSP+1, rtw_read8(adapter, rOFDM0_RxDSP+1) & ~BIT1);
+}
+
+static void UpdateHalRAMask8723B(struct adapter *padapter, u32 mac_id, u8 rssi_level)
+{
+	u32 mask, rate_bitmap;
+	u8 shortGIrate = false;
+	struct sta_info *psta;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (mac_id >= NUM_STA) /* CAM_SIZE */
+		return;
+
+	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
+	if (!psta)
+		return;
+
+	shortGIrate = query_ra_short_GI(psta);
+
+	mask = psta->ra_mask;
+
+	rate_bitmap = 0xffffffff;
+	rate_bitmap = ODM_Get_Rate_Bitmap(&pHalData->odmpriv, mac_id, mask, rssi_level);
+
+	mask &= rate_bitmap;
+
+	rate_bitmap = hal_btcoex_GetRaMask(padapter);
+	mask &= ~rate_bitmap;
+
+	if (pHalData->fw_ractrl) {
+		rtl8723b_set_FwMacIdConfig_cmd(padapter, mac_id, psta->raid, psta->bw_mode, shortGIrate, mask);
+	}
+
+	/* set correct initial date rate for each mac_id */
+	pdmpriv->INIDATA_RATE[mac_id] = psta->init_rate;
+}
+
+
+void rtl8723b_set_hal_ops(struct hal_ops *pHalFunc)
+{
+	pHalFunc->free_hal_data = &rtl8723b_free_hal_data;
+
+	pHalFunc->dm_init = &rtl8723b_init_dm_priv;
+
+	pHalFunc->read_chip_version = &rtl8723b_read_chip_version;
+
+	pHalFunc->UpdateRAMaskHandler = &UpdateHalRAMask8723B;
+
+	pHalFunc->set_bwmode_handler = &PHY_SetBWMode8723B;
+	pHalFunc->set_channel_handler = &PHY_SwChnl8723B;
+	pHalFunc->set_chnl_bw_handler = &PHY_SetSwChnlBWMode8723B;
+
+	pHalFunc->set_tx_power_level_handler = &PHY_SetTxPowerLevel8723B;
+	pHalFunc->get_tx_power_level_handler = &PHY_GetTxPowerLevel8723B;
+
+	pHalFunc->hal_dm_watchdog = &rtl8723b_HalDmWatchDog;
+	pHalFunc->hal_dm_watchdog_in_lps = &rtl8723b_HalDmWatchDog_in_LPS;
+
+
+	pHalFunc->SetBeaconRelatedRegistersHandler = &rtl8723b_SetBeaconRelatedRegisters;
+
+	pHalFunc->Add_RateATid = &rtl8723b_Add_RateATid;
+
+	pHalFunc->run_thread = &rtl8723b_start_thread;
+	pHalFunc->cancel_thread = &rtl8723b_stop_thread;
+
+	pHalFunc->read_bbreg = &PHY_QueryBBReg_8723B;
+	pHalFunc->write_bbreg = &PHY_SetBBReg_8723B;
+	pHalFunc->read_rfreg = &PHY_QueryRFReg_8723B;
+	pHalFunc->write_rfreg = &PHY_SetRFReg_8723B;
+
+	/*  Efuse related function */
+	pHalFunc->BTEfusePowerSwitch = &Hal_BT_EfusePowerSwitch;
+	pHalFunc->EfusePowerSwitch = &Hal_EfusePowerSwitch;
+	pHalFunc->ReadEFuse = &Hal_ReadEFuse;
+	pHalFunc->EFUSEGetEfuseDefinition = &Hal_GetEfuseDefinition;
+	pHalFunc->EfuseGetCurrentSize = &Hal_EfuseGetCurrentSize;
+	pHalFunc->Efuse_PgPacketRead = &Hal_EfusePgPacketRead;
+	pHalFunc->Efuse_PgPacketWrite = &Hal_EfusePgPacketWrite;
+	pHalFunc->Efuse_WordEnableDataWrite = &Hal_EfuseWordEnableDataWrite;
+	pHalFunc->Efuse_PgPacketWrite_BT = &Hal_EfusePgPacketWrite_BT;
+
+	pHalFunc->GetHalODMVarHandler = &rtl8723b_GetHalODMVar;
+	pHalFunc->SetHalODMVarHandler = &rtl8723b_SetHalODMVar;
+
+	pHalFunc->xmit_thread_handler = &hal_xmit_handler;
+	pHalFunc->hal_notch_filter = &hal_notch_filter_8723b;
+
+	pHalFunc->c2h_handler = c2h_handler_8723b;
+	pHalFunc->c2h_id_filter_ccx = c2h_id_filter_ccx_8723b;
+
+	pHalFunc->fill_h2c_cmd = &FillH2CCmd8723B;
+}
+
+void rtl8723b_InitAntenna_Selection(struct adapter *padapter)
+{
+	u8 val;
+
+	val = rtw_read8(padapter, REG_LEDCFG2);
+	/*  Let 8051 take control antenna setting */
+	val |= BIT(7); /*  DPDT_SEL_EN, 0x4C[23] */
+	rtw_write8(padapter, REG_LEDCFG2, val);
+}
+
+void rtl8723b_init_default_value(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	struct dm_priv *pdmpriv;
+	u8 i;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pdmpriv = &pHalData->dmpriv;
+
+	padapter->registrypriv.wireless_mode = WIRELESS_11BG_24N;
+
+	/*  init default value */
+	pHalData->fw_ractrl = false;
+	pHalData->bIQKInitialized = false;
+	if (!adapter_to_pwrctl(padapter)->bkeepfwalive)
+		pHalData->LastHMEBoxNum = 0;
+
+	pHalData->bIQKInitialized = false;
+
+	/*  init dm default value */
+	pdmpriv->TM_Trigger = 0;/* for IQK */
+/* 	pdmpriv->binitialized = false; */
+/* 	pdmpriv->prv_traffic_idx = 3; */
+/* 	pdmpriv->initialize = 0; */
+
+	pdmpriv->ThermalValue_HP_index = 0;
+	for (i = 0; i < HP_THERMAL_NUM; i++)
+		pdmpriv->ThermalValue_HP[i] = 0;
+
+	/*  init Efuse variables */
+	pHalData->EfuseUsedBytes = 0;
+	pHalData->EfuseUsedPercentage = 0;
+#ifdef HAL_EFUSE_MEMORY
+	pHalData->EfuseHal.fakeEfuseBank = 0;
+	pHalData->EfuseHal.fakeEfuseUsedBytes = 0;
+	memset(pHalData->EfuseHal.fakeEfuseContent, 0xFF, EFUSE_MAX_HW_SIZE);
+	memset(pHalData->EfuseHal.fakeEfuseInitMap, 0xFF, EFUSE_MAX_MAP_LEN);
+	memset(pHalData->EfuseHal.fakeEfuseModifiedMap, 0xFF, EFUSE_MAX_MAP_LEN);
+	pHalData->EfuseHal.BTEfuseUsedBytes = 0;
+	pHalData->EfuseHal.BTEfuseUsedPercentage = 0;
+	memset(pHalData->EfuseHal.BTEfuseContent, 0xFF, EFUSE_MAX_BT_BANK*EFUSE_MAX_HW_SIZE);
+	memset(pHalData->EfuseHal.BTEfuseInitMap, 0xFF, EFUSE_BT_MAX_MAP_LEN);
+	memset(pHalData->EfuseHal.BTEfuseModifiedMap, 0xFF, EFUSE_BT_MAX_MAP_LEN);
+	pHalData->EfuseHal.fakeBTEfuseUsedBytes = 0;
+	memset(pHalData->EfuseHal.fakeBTEfuseContent, 0xFF, EFUSE_MAX_BT_BANK*EFUSE_MAX_HW_SIZE);
+	memset(pHalData->EfuseHal.fakeBTEfuseInitMap, 0xFF, EFUSE_BT_MAX_MAP_LEN);
+	memset(pHalData->EfuseHal.fakeBTEfuseModifiedMap, 0xFF, EFUSE_BT_MAX_MAP_LEN);
+#endif
+}
+
+u8 GetEEPROMSize8723B(struct adapter *padapter)
+{
+	u8 size = 0;
+	u32 cr;
+
+	cr = rtw_read16(padapter, REG_9346CR);
+	/*  6: EEPROM used is 93C46, 4: boot from E-Fuse. */
+	size = (cr & BOOT_FROM_EEPROM) ? 6 : 4;
+
+	return size;
+}
+
+/*  */
+/*  */
+/*  LLT R/W/Init function */
+/*  */
+/*  */
+s32 rtl8723b_InitLLTTable(struct adapter *padapter)
+{
+	unsigned long start, passing_time;
+	u32 val32;
+	s32 ret;
+
+
+	ret = _FAIL;
+
+	val32 = rtw_read32(padapter, REG_AUTO_LLT);
+	val32 |= BIT_AUTO_INIT_LLT;
+	rtw_write32(padapter, REG_AUTO_LLT, val32);
+
+	start = jiffies;
+
+	do {
+		val32 = rtw_read32(padapter, REG_AUTO_LLT);
+		if (!(val32 & BIT_AUTO_INIT_LLT)) {
+			ret = _SUCCESS;
+			break;
+		}
+
+		passing_time = jiffies_to_msecs(jiffies - start);
+		if (passing_time > 1000)
+			break;
+
+		msleep(1);
+	} while (1);
+
+	return ret;
+}
+
+static void hal_get_chnl_group_8723b(u8 channel, u8 *group)
+{
+	if (1  <= channel && channel <= 2)
+		*group = 0;
+	else if (3  <= channel && channel <= 5)
+		*group = 1;
+	else if (6  <= channel && channel <= 8)
+		*group = 2;
+	else if (9  <= channel && channel <= 11)
+		*group = 3;
+	else if (12 <= channel && channel <= 14)
+		*group = 4;
+}
+
+void Hal_InitPGData(struct adapter *padapter, u8 *PROMContent)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	if (!pEEPROM->bautoload_fail_flag) { /*  autoload OK. */
+		if (!pEEPROM->EepromOrEfuse) {
+			/*  Read EFUSE real map to shadow. */
+			EFUSE_ShadowMapUpdate(padapter, EFUSE_WIFI, false);
+			memcpy((void *)PROMContent, (void *)pEEPROM->efuse_eeprom_data, HWSET_MAX_SIZE_8723B);
+		}
+	} else {/* autoload fail */
+		if (!pEEPROM->EepromOrEfuse)
+			EFUSE_ShadowMapUpdate(padapter, EFUSE_WIFI, false);
+		memcpy((void *)PROMContent, (void *)pEEPROM->efuse_eeprom_data, HWSET_MAX_SIZE_8723B);
+	}
+}
+
+void Hal_EfuseParseIDCode(struct adapter *padapter, u8 *hwinfo)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+/* 	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter); */
+	u16 EEPROMId;
+
+
+	/*  Checl 0x8129 again for making sure autoload status!! */
+	EEPROMId = le16_to_cpu(*((__le16 *)hwinfo));
+	if (EEPROMId != RTL_EEPROM_ID) {
+		pEEPROM->bautoload_fail_flag = true;
+	} else
+		pEEPROM->bautoload_fail_flag = false;
+}
+
+static void Hal_ReadPowerValueFromPROM_8723B(
+	struct adapter *Adapter,
+	struct TxPowerInfo24G *pwrInfo24G,
+	u8 *PROMContent,
+	bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u32 rfPath, eeAddr = EEPROM_TX_PWR_INX_8723B, group, TxCount = 0;
+
+	memset(pwrInfo24G, 0, sizeof(struct TxPowerInfo24G));
+
+	if (0xFF == PROMContent[eeAddr+1])
+		AutoLoadFail = true;
+
+	if (AutoLoadFail) {
+		for (rfPath = 0; rfPath < MAX_RF_PATH; rfPath++) {
+			/* 2.4G default value */
+			for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+				pwrInfo24G->IndexCCK_Base[rfPath][group] = EEPROM_DEFAULT_24G_INDEX;
+				pwrInfo24G->IndexBW40_Base[rfPath][group] = EEPROM_DEFAULT_24G_INDEX;
+			}
+
+			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+				if (TxCount == 0) {
+					pwrInfo24G->BW20_Diff[rfPath][0] = EEPROM_DEFAULT_24G_HT20_DIFF;
+					pwrInfo24G->OFDM_Diff[rfPath][0] = EEPROM_DEFAULT_24G_OFDM_DIFF;
+				} else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				}
+			}
+		}
+
+		return;
+	}
+
+	pHalData->bTXPowerDataReadFromEEPORM = true;		/* YJ, move, 120316 */
+
+	for (rfPath = 0; rfPath < MAX_RF_PATH; rfPath++) {
+		/* 2 2.4G default value */
+		for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+			pwrInfo24G->IndexCCK_Base[rfPath][group] =	PROMContent[eeAddr++];
+			if (pwrInfo24G->IndexCCK_Base[rfPath][group] == 0xFF)
+				pwrInfo24G->IndexCCK_Base[rfPath][group] = EEPROM_DEFAULT_24G_INDEX;
+		}
+
+		for (group = 0; group < MAX_CHNL_GROUP_24G-1; group++) {
+			pwrInfo24G->IndexBW40_Base[rfPath][group] =	PROMContent[eeAddr++];
+			if (pwrInfo24G->IndexBW40_Base[rfPath][group] == 0xFF)
+				pwrInfo24G->IndexBW40_Base[rfPath][group] =	EEPROM_DEFAULT_24G_INDEX;
+		}
+
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (TxCount == 0) {
+				pwrInfo24G->BW40_Diff[rfPath][TxCount] = 0;
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] =	EEPROM_DEFAULT_24G_HT20_DIFF;
+				else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] =	(PROMContent[eeAddr]&0xf0)>>4;
+					if (pwrInfo24G->BW20_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW20_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = EEPROM_DEFAULT_24G_OFDM_DIFF;
+				else {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = (PROMContent[eeAddr]&0x0f);
+					if (pwrInfo24G->OFDM_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->OFDM_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				pwrInfo24G->CCK_Diff[rfPath][TxCount] = 0;
+				eeAddr++;
+			} else {
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				else {
+					pwrInfo24G->BW40_Diff[rfPath][TxCount] = (PROMContent[eeAddr]&0xf0)>>4;
+					if (pwrInfo24G->BW40_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW40_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				else {
+					pwrInfo24G->BW20_Diff[rfPath][TxCount] = (PROMContent[eeAddr]&0x0f);
+					if (pwrInfo24G->BW20_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->BW20_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				else {
+					pwrInfo24G->OFDM_Diff[rfPath][TxCount] = (PROMContent[eeAddr]&0xf0)>>4;
+					if (pwrInfo24G->OFDM_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->OFDM_Diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (PROMContent[eeAddr] == 0xFF)
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] = EEPROM_DEFAULT_DIFF;
+				else {
+					pwrInfo24G->CCK_Diff[rfPath][TxCount] = (PROMContent[eeAddr]&0x0f);
+					if (pwrInfo24G->CCK_Diff[rfPath][TxCount] & BIT3)		/* 4bit sign number to 8 bit sign number */
+						pwrInfo24G->CCK_Diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+			}
+		}
+	}
+}
+
+
+void Hal_EfuseParseTxPowerInfo_8723B(
+	struct adapter *padapter, u8 *PROMContent, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct TxPowerInfo24G	pwrInfo24G;
+	u8 	rfPath, ch, TxCount = 1;
+
+	Hal_ReadPowerValueFromPROM_8723B(padapter, &pwrInfo24G, PROMContent, AutoLoadFail);
+	for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
+		for (ch = 0 ; ch < CHANNEL_MAX_NUMBER; ch++) {
+			u8 group = 0;
+
+			hal_get_chnl_group_8723b(ch + 1, &group);
+
+			if (ch == 14-1) {
+				pHalData->Index24G_CCK_Base[rfPath][ch] = pwrInfo24G.IndexCCK_Base[rfPath][5];
+				pHalData->Index24G_BW40_Base[rfPath][ch] = pwrInfo24G.IndexBW40_Base[rfPath][group];
+			} else {
+				pHalData->Index24G_CCK_Base[rfPath][ch] = pwrInfo24G.IndexCCK_Base[rfPath][group];
+				pHalData->Index24G_BW40_Base[rfPath][ch] = pwrInfo24G.IndexBW40_Base[rfPath][group];
+			}
+		}
+
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			pHalData->CCK_24G_Diff[rfPath][TxCount] = pwrInfo24G.CCK_Diff[rfPath][TxCount];
+			pHalData->OFDM_24G_Diff[rfPath][TxCount] = pwrInfo24G.OFDM_Diff[rfPath][TxCount];
+			pHalData->BW20_24G_Diff[rfPath][TxCount] = pwrInfo24G.BW20_Diff[rfPath][TxCount];
+			pHalData->BW40_24G_Diff[rfPath][TxCount] = pwrInfo24G.BW40_Diff[rfPath][TxCount];
+		}
+	}
+
+	/*  2010/10/19 MH Add Regulator recognize for CU. */
+	if (!AutoLoadFail) {
+		pHalData->EEPROMRegulatory = (PROMContent[EEPROM_RF_BOARD_OPTION_8723B]&0x7);	/* bit0~2 */
+		if (PROMContent[EEPROM_RF_BOARD_OPTION_8723B] == 0xFF)
+			pHalData->EEPROMRegulatory = (EEPROM_DEFAULT_BOARD_OPTION&0x7);	/* bit0~2 */
+	} else
+		pHalData->EEPROMRegulatory = 0;
+}
+
+void Hal_EfuseParseBTCoexistInfo_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 tempval;
+	u32 tmpu4;
+
+	if (!AutoLoadFail) {
+		tmpu4 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+		if (tmpu4 & BT_FUNC_EN)
+			pHalData->EEPROMBluetoothCoexist = true;
+		else
+			pHalData->EEPROMBluetoothCoexist = false;
+
+		pHalData->EEPROMBluetoothType = BT_RTL8723B;
+
+		tempval = hwinfo[EEPROM_RF_BT_SETTING_8723B];
+		if (tempval != 0xFF) {
+			pHalData->EEPROMBluetoothAntNum = tempval & BIT(0);
+			/*  EFUSE_0xC3[6] == 0, S1(Main)-RF_PATH_A; */
+			/*  EFUSE_0xC3[6] == 1, S0(Aux)-RF_PATH_B */
+			pHalData->ant_path = (tempval & BIT(6))? RF_PATH_B : RF_PATH_A;
+		} else {
+			pHalData->EEPROMBluetoothAntNum = Ant_x1;
+			if (pHalData->PackageType == PACKAGE_QFN68)
+				pHalData->ant_path = RF_PATH_B;
+			else
+				pHalData->ant_path = RF_PATH_A;
+		}
+	} else {
+		pHalData->EEPROMBluetoothCoexist = false;
+		pHalData->EEPROMBluetoothType = BT_RTL8723B;
+		pHalData->EEPROMBluetoothAntNum = Ant_x1;
+		pHalData->ant_path = RF_PATH_A;
+	}
+
+	if (padapter->registrypriv.ant_num > 0) {
+		switch (padapter->registrypriv.ant_num) {
+		case 1:
+			pHalData->EEPROMBluetoothAntNum = Ant_x1;
+			break;
+		case 2:
+			pHalData->EEPROMBluetoothAntNum = Ant_x2;
+			break;
+		default:
+			break;
+		}
+	}
+
+	hal_btcoex_SetBTCoexist(padapter, pHalData->EEPROMBluetoothCoexist);
+	hal_btcoex_SetChipType(padapter, pHalData->EEPROMBluetoothType);
+	hal_btcoex_SetPgAntNum(padapter, pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1);
+	if (pHalData->EEPROMBluetoothAntNum == Ant_x1)
+		hal_btcoex_SetSingleAntPath(padapter, pHalData->ant_path);
+}
+
+void Hal_EfuseParseEEPROMVer_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if (!AutoLoadFail)
+		pHalData->EEPROMVersion = hwinfo[EEPROM_VERSION_8723B];
+	else
+		pHalData->EEPROMVersion = 1;
+}
+
+
+
+void Hal_EfuseParsePackageType_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 package;
+	u8 efuseContent;
+
+	Efuse_PowerSwitch(padapter, false, true);
+	efuse_OneByteRead(padapter, 0x1FB, &efuseContent, false);
+	Efuse_PowerSwitch(padapter, false, false);
+
+	package = efuseContent & 0x7;
+	switch (package) {
+	case 0x4:
+		pHalData->PackageType = PACKAGE_TFBGA79;
+		break;
+	case 0x5:
+		pHalData->PackageType = PACKAGE_TFBGA90;
+		break;
+	case 0x6:
+		pHalData->PackageType = PACKAGE_QFN68;
+		break;
+	case 0x7:
+		pHalData->PackageType = PACKAGE_TFBGA80;
+		break;
+
+	default:
+		pHalData->PackageType = PACKAGE_DEFAULT;
+		break;
+	}
+}
+
+
+void Hal_EfuseParseVoltage_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	/* memcpy(pEEPROM->adjuseVoltageVal, &hwinfo[EEPROM_Voltage_ADDR_8723B], 1); */
+	pEEPROM->adjuseVoltageVal = (hwinfo[EEPROM_Voltage_ADDR_8723B] & 0xf0) >> 4;
+}
+
+void Hal_EfuseParseChnlPlan_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	padapter->mlmepriv.ChannelPlan = hal_com_config_channel_plan(
+		padapter,
+		hwinfo ? hwinfo[EEPROM_ChannelPlan_8723B] : 0xFF,
+		padapter->registrypriv.channel_plan,
+		RT_CHANNEL_DOMAIN_WORLD_NULL,
+		AutoLoadFail
+	);
+
+	Hal_ChannelPlanToRegulation(padapter, padapter->mlmepriv.ChannelPlan);
+}
+
+void Hal_EfuseParseCustomerID_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if (!AutoLoadFail)
+		pHalData->EEPROMCustomerID = hwinfo[EEPROM_CustomID_8723B];
+	else
+		pHalData->EEPROMCustomerID = 0;
+}
+
+void Hal_EfuseParseAntennaDiversity_8723B(
+	struct adapter *padapter,
+	u8 *hwinfo,
+	bool AutoLoadFail
+)
+{
+}
+
+void Hal_EfuseParseXtal_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if (!AutoLoadFail) {
+		pHalData->CrystalCap = hwinfo[EEPROM_XTAL_8723B];
+		if (pHalData->CrystalCap == 0xFF)
+			pHalData->CrystalCap = EEPROM_Default_CrystalCap_8723B;	   /* what value should 8812 set? */
+	} else
+		pHalData->CrystalCap = EEPROM_Default_CrystalCap_8723B;
+}
+
+
+void Hal_EfuseParseThermalMeter_8723B(
+	struct adapter *padapter, u8 *PROMContent, u8 AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	/*  */
+	/*  ThermalMeter from EEPROM */
+	/*  */
+	if (!AutoLoadFail)
+		pHalData->EEPROMThermalMeter = PROMContent[EEPROM_THERMAL_METER_8723B];
+	else
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_8723B;
+
+	if ((pHalData->EEPROMThermalMeter == 0xff) || AutoLoadFail) {
+		pHalData->bAPKThermalMeterIgnore = true;
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_8723B;
+	}
+}
+
+
+void Hal_ReadRFGainOffset(
+	struct adapter *Adapter, u8 *PROMContent, bool AutoloadFail
+)
+{
+	/*  */
+	/*  BB_RF Gain Offset from EEPROM */
+	/*  */
+
+	if (!AutoloadFail) {
+		Adapter->eeprompriv.EEPROMRFGainOffset = PROMContent[EEPROM_RF_GAIN_OFFSET];
+		Adapter->eeprompriv.EEPROMRFGainVal = EFUSE_Read1Byte(Adapter, EEPROM_RF_GAIN_VAL);
+	} else {
+		Adapter->eeprompriv.EEPROMRFGainOffset = 0;
+		Adapter->eeprompriv.EEPROMRFGainVal = 0xFF;
+	}
+}
+
+u8 BWMapping_8723B(struct adapter *Adapter, struct pkt_attrib *pattrib)
+{
+	u8 BWSettingOfDesc = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		if (pattrib->bwmode == CHANNEL_WIDTH_40)
+			BWSettingOfDesc = 1;
+		else
+			BWSettingOfDesc = 0;
+	} else
+		BWSettingOfDesc = 0;
+
+	/* if (pTcb->bBTTxPacket) */
+	/* 	BWSettingOfDesc = 0; */
+
+	return BWSettingOfDesc;
+}
+
+u8 SCMapping_8723B(struct adapter *Adapter, struct pkt_attrib *pattrib)
+{
+	u8 SCSettingOfDesc = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		if (pattrib->bwmode == CHANNEL_WIDTH_40) {
+			SCSettingOfDesc = HT_DATA_SC_DONOT_CARE;
+		} else if (pattrib->bwmode == CHANNEL_WIDTH_20) {
+			if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) {
+				SCSettingOfDesc = HT_DATA_SC_20_UPPER_OF_40MHZ;
+			} else if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) {
+				SCSettingOfDesc = HT_DATA_SC_20_LOWER_OF_40MHZ;
+			} else {
+				SCSettingOfDesc = HT_DATA_SC_DONOT_CARE;
+			}
+		}
+	} else {
+		SCSettingOfDesc = HT_DATA_SC_DONOT_CARE;
+	}
+
+	return SCSettingOfDesc;
+}
+
+static void rtl8723b_cal_txdesc_chksum(struct tx_desc *ptxdesc)
+{
+	u16 *usPtr = (u16 *)ptxdesc;
+	u32 count;
+	u32 index;
+	u16 checksum = 0;
+
+
+	/*  Clear first */
+	ptxdesc->txdw7 &= cpu_to_le32(0xffff0000);
+
+	/*  checksume is always calculated by first 32 bytes, */
+	/*  and it doesn't depend on TX DESC length. */
+	/*  Thomas, Lucas@SD4, 20130515 */
+	count = 16;
+
+	for (index = 0; index < count; index++) {
+		checksum |= le16_to_cpu(*(__le16 *)(usPtr + index));
+	}
+
+	ptxdesc->txdw7 |= cpu_to_le32(checksum & 0x0000ffff);
+}
+
+static u8 fill_txdesc_sectype(struct pkt_attrib *pattrib)
+{
+	u8 sectype = 0;
+	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
+		switch (pattrib->encrypt) {
+		/*  SEC_TYPE */
+		case _WEP40_:
+		case _WEP104_:
+		case _TKIP_:
+		case _TKIP_WTMIC_:
+			sectype = 1;
+			break;
+
+		case _AES_:
+			sectype = 3;
+			break;
+
+		case _NO_PRIVACY_:
+		default:
+			break;
+		}
+	}
+	return sectype;
+}
+
+static void fill_txdesc_vcs_8723b(struct adapter *padapter, struct pkt_attrib *pattrib, struct txdesc_8723b *ptxdesc)
+{
+	if (pattrib->vcs_mode) {
+		switch (pattrib->vcs_mode) {
+		case RTS_CTS:
+			ptxdesc->rtsen = 1;
+			/*  ENABLE HW RTS */
+			ptxdesc->hw_rts_en = 1;
+			break;
+
+		case CTS_TO_SELF:
+			ptxdesc->cts2self = 1;
+			break;
+
+		case NONE_VCS:
+		default:
+			break;
+		}
+
+		ptxdesc->rtsrate = 8; /*  RTS Rate =24M */
+		ptxdesc->rts_ratefb_lmt = 0xF;
+
+		if (padapter->mlmeextpriv.mlmext_info.preamble_mode == PREAMBLE_SHORT)
+			ptxdesc->rts_short = 1;
+
+		/*  Set RTS BW */
+		if (pattrib->ht_en)
+			ptxdesc->rts_sc = SCMapping_8723B(padapter, pattrib);
+	}
+}
+
+static void fill_txdesc_phy_8723b(struct adapter *padapter, struct pkt_attrib *pattrib, struct txdesc_8723b *ptxdesc)
+{
+	if (pattrib->ht_en) {
+		ptxdesc->data_bw = BWMapping_8723B(padapter, pattrib);
+
+		ptxdesc->data_sc = SCMapping_8723B(padapter, pattrib);
+	}
+}
+
+static void rtl8723b_fill_default_txdesc(
+	struct xmit_frame *pxmitframe, u8 *pbuf
+)
+{
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pkt_attrib *pattrib;
+	struct txdesc_8723b *ptxdesc;
+	s32 bmcst;
+
+	memset(pbuf, 0, TXDESC_SIZE);
+
+	padapter = pxmitframe->padapter;
+	pHalData = GET_HAL_DATA(padapter);
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pattrib = &pxmitframe->attrib;
+	bmcst = IS_MCAST(pattrib->ra);
+
+	ptxdesc = (struct txdesc_8723b *)pbuf;
+
+	if (pxmitframe->frame_tag == DATA_FRAMETAG) {
+		u8 drv_userate = 0;
+
+		ptxdesc->macid = pattrib->mac_id; /*  CAM_ID(MAC_ID) */
+		ptxdesc->rate_id = pattrib->raid;
+		ptxdesc->qsel = pattrib->qsel;
+		ptxdesc->seq = pattrib->seqnum;
+
+		ptxdesc->sectype = fill_txdesc_sectype(pattrib);
+		fill_txdesc_vcs_8723b(padapter, pattrib, ptxdesc);
+
+		if (pattrib->icmp_pkt == 1 && padapter->registrypriv.wifi_spec == 1)
+			drv_userate = 1;
+
+		if (
+			(pattrib->ether_type != 0x888e) &&
+			(pattrib->ether_type != 0x0806) &&
+			(pattrib->ether_type != 0x88B4) &&
+			(pattrib->dhcp_pkt != 1) &&
+			(drv_userate != 1)
+		) {
+			/*  Non EAP & ARP & DHCP type data packet */
+
+			if (pattrib->ampdu_en) {
+				ptxdesc->agg_en = 1; /*  AGG EN */
+				ptxdesc->max_agg_num = 0x1f;
+				ptxdesc->ampdu_density = pattrib->ampdu_spacing;
+			} else
+				ptxdesc->bk = 1; /*  AGG BK */
+
+			fill_txdesc_phy_8723b(padapter, pattrib, ptxdesc);
+
+			ptxdesc->data_ratefb_lmt = 0x1F;
+
+			if (!pHalData->fw_ractrl) {
+				ptxdesc->userate = 1;
+
+				if (pHalData->dmpriv.INIDATA_RATE[pattrib->mac_id] & BIT(7))
+					ptxdesc->data_short = 1;
+
+				ptxdesc->datarate = pHalData->dmpriv.INIDATA_RATE[pattrib->mac_id] & 0x7F;
+			}
+
+			if (padapter->fix_rate != 0xFF) { /*  modify data rate by iwpriv */
+				ptxdesc->userate = 1;
+				if (padapter->fix_rate & BIT(7))
+					ptxdesc->data_short = 1;
+
+				ptxdesc->datarate = (padapter->fix_rate & 0x7F);
+				ptxdesc->disdatafb = 1;
+			}
+
+			if (pattrib->ldpc)
+				ptxdesc->data_ldpc = 1;
+			if (pattrib->stbc)
+				ptxdesc->data_stbc = 1;
+		} else {
+			/*  EAP data packet and ARP packet. */
+			/*  Use the 1M data rate to send the EAP/ARP packet. */
+			/*  This will maybe make the handshake smooth. */
+
+			ptxdesc->bk = 1; /*  AGG BK */
+			ptxdesc->userate = 1; /*  driver uses rate */
+			if (pmlmeinfo->preamble_mode == PREAMBLE_SHORT)
+				ptxdesc->data_short = 1;/*  DATA_SHORT */
+			ptxdesc->datarate = MRateToHwRate(pmlmeext->tx_rate);
+		}
+
+		ptxdesc->usb_txagg_num = pxmitframe->agg_num;
+	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
+		ptxdesc->macid = pattrib->mac_id; /*  CAM_ID(MAC_ID) */
+		ptxdesc->qsel = pattrib->qsel;
+		ptxdesc->rate_id = pattrib->raid; /*  Rate ID */
+		ptxdesc->seq = pattrib->seqnum;
+		ptxdesc->userate = 1; /*  driver uses rate, 1M */
+
+		ptxdesc->mbssid = pattrib->mbssid & 0xF;
+
+		ptxdesc->rty_lmt_en = 1; /*  retry limit enable */
+		if (pattrib->retry_ctrl) {
+			ptxdesc->data_rt_lmt = 6;
+		} else {
+			ptxdesc->data_rt_lmt = 12;
+		}
+
+		ptxdesc->datarate = MRateToHwRate(pmlmeext->tx_rate);
+
+		/*  CCX-TXRPT ack for xmit mgmt frames. */
+		if (pxmitframe->ack_report) {
+			ptxdesc->spe_rpt = 1;
+			ptxdesc->sw_define = (u8)(GET_PRIMARY_ADAPTER(padapter)->xmitpriv.seq_no);
+		}
+	} else {
+		ptxdesc->macid = pattrib->mac_id; /*  CAM_ID(MAC_ID) */
+		ptxdesc->rate_id = pattrib->raid; /*  Rate ID */
+		ptxdesc->qsel = pattrib->qsel;
+		ptxdesc->seq = pattrib->seqnum;
+		ptxdesc->userate = 1; /*  driver uses rate */
+		ptxdesc->datarate = MRateToHwRate(pmlmeext->tx_rate);
+	}
+
+	ptxdesc->pktlen = pattrib->last_txcmdsz;
+	ptxdesc->offset = TXDESC_SIZE + OFFSET_SZ;
+
+	if (bmcst)
+		ptxdesc->bmc = 1;
+
+	/* 2009.11.05. tynli_test. Suggested by SD4 Filen for FW LPS.
+	 * (1) The sequence number of each non-Qos frame / broadcast /
+	 * multicast / mgnt frame should be controlled by Hw because Fw
+	 * will also send null data which we cannot control when Fw LPS
+	 * enable.
+	 * --> default enable non-Qos data sequense number. 2010.06.23.
+	 * by tynli.
+	 * (2) Enable HW SEQ control for beacon packet, because we use
+	 * Hw beacon.
+	 * (3) Use HW Qos SEQ to control the seq num of Ext port non-Qos
+	 * packets.
+	 * 2010.06.23. Added by tynli.
+	 */
+	if (!pattrib->qos_en) /*  Hw set sequence number */
+		ptxdesc->en_hwseq = 1; /*  HWSEQ_EN */
+}
+
+/* Description:
+ *
+ * Parameters:
+ *	pxmitframe	xmitframe
+ *	pbuf		where to fill tx desc
+ */
+void rtl8723b_update_txdesc(struct xmit_frame *pxmitframe, u8 *pbuf)
+{
+	struct tx_desc *pdesc;
+
+	rtl8723b_fill_default_txdesc(pxmitframe, pbuf);
+	pdesc = (struct tx_desc *)pbuf;
+	rtl8723b_cal_txdesc_chksum(pdesc);
+}
+
+/*  */
+/*  Description: In normal chip, we should send some packet to Hw which will be used by Fw */
+/* 			in FW LPS mode. The function is to fill the Tx descriptor of this packets, then */
+/* 			Fw can tell Hw to send these packet derectly. */
+/*  Added by tynli. 2009.10.15. */
+/*  */
+/* type1:pspoll, type2:null */
+void rtl8723b_fill_fake_txdesc(
+	struct adapter *padapter,
+	u8 *pDesc,
+	u32 BufferLen,
+	u8 IsPsPoll,
+	u8 IsBTQosNull,
+	u8 bDataFrame
+)
+{
+	/*  Clear all status */
+	memset(pDesc, 0, TXDESC_SIZE);
+
+	SET_TX_DESC_FIRST_SEG_8723B(pDesc, 1); /* bFirstSeg; */
+	SET_TX_DESC_LAST_SEG_8723B(pDesc, 1); /* bLastSeg; */
+
+	SET_TX_DESC_OFFSET_8723B(pDesc, 0x28); /*  Offset = 32 */
+
+	SET_TX_DESC_PKT_SIZE_8723B(pDesc, BufferLen); /*  Buffer size + command header */
+	SET_TX_DESC_QUEUE_SEL_8723B(pDesc, QSLT_MGNT); /*  Fixed queue of Mgnt queue */
+
+	/*  Set NAVUSEHDR to prevent Ps-poll AId filed to be changed to error vlaue by Hw. */
+	if (IsPsPoll) {
+		SET_TX_DESC_NAV_USE_HDR_8723B(pDesc, 1);
+	} else {
+		SET_TX_DESC_HWSEQ_EN_8723B(pDesc, 1); /*  Hw set sequence number */
+		SET_TX_DESC_HWSEQ_SEL_8723B(pDesc, 0);
+	}
+
+	if (IsBTQosNull) {
+		SET_TX_DESC_BT_INT_8723B(pDesc, 1);
+	}
+
+	SET_TX_DESC_USE_RATE_8723B(pDesc, 1); /*  use data rate which is set by Sw */
+	SET_TX_DESC_OWN_8723B((u8 *)pDesc, 1);
+
+	SET_TX_DESC_TX_RATE_8723B(pDesc, DESC8723B_RATE1M);
+
+	/*  */
+	/*  Encrypt the data frame if under security mode excepct null data. Suggested by CCW. */
+	/*  */
+	if (bDataFrame) {
+		u32 EncAlg;
+
+		EncAlg = padapter->securitypriv.dot11PrivacyAlgrthm;
+		switch (EncAlg) {
+		case _NO_PRIVACY_:
+			SET_TX_DESC_SEC_TYPE_8723B(pDesc, 0x0);
+			break;
+		case _WEP40_:
+		case _WEP104_:
+		case _TKIP_:
+			SET_TX_DESC_SEC_TYPE_8723B(pDesc, 0x1);
+			break;
+		case _SMS4_:
+			SET_TX_DESC_SEC_TYPE_8723B(pDesc, 0x2);
+			break;
+		case _AES_:
+			SET_TX_DESC_SEC_TYPE_8723B(pDesc, 0x3);
+			break;
+		default:
+			SET_TX_DESC_SEC_TYPE_8723B(pDesc, 0x0);
+			break;
+		}
+	}
+
+	/*  USB interface drop packet if the checksum of descriptor isn't correct. */
+	/*  Using this checksum can let hardware recovery from packet bulk out error (e.g. Cancel URC, Bulk out error.). */
+	rtl8723b_cal_txdesc_chksum((struct tx_desc *)pDesc);
+}
+
+static void hw_var_set_opmode(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 val8;
+	u8 mode = *((u8 *)val);
+
+	{
+		/*  disable Port0 TSF update */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 |= DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		/*  set net_type */
+		Set_MSR(padapter, mode);
+
+		if ((mode == _HW_STATE_STATION_) || (mode == _HW_STATE_NOLINK_)) {
+			{
+				StopTxBeacon(padapter);
+			}
+
+			/*  disable atim wnd */
+			rtw_write8(padapter, REG_BCN_CTRL, DIS_TSF_UDT|EN_BCN_FUNCTION|DIS_ATIM);
+			/* rtw_write8(padapter, REG_BCN_CTRL, 0x18); */
+		} else if (mode == _HW_STATE_ADHOC_) {
+			ResumeTxBeacon(padapter);
+			rtw_write8(padapter, REG_BCN_CTRL, DIS_TSF_UDT|EN_BCN_FUNCTION|DIS_BCNQ_SUB);
+		} else if (mode == _HW_STATE_AP_) {
+
+			ResumeTxBeacon(padapter);
+
+			rtw_write8(padapter, REG_BCN_CTRL, DIS_TSF_UDT|DIS_BCNQ_SUB);
+
+			/* Set RCR */
+			rtw_write32(padapter, REG_RCR, 0x7000208e);/* CBSSID_DATA must set to 0, reject ICV_ERR packet */
+			/* enable to rx data frame */
+			rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+			/* enable to rx ps-poll */
+			rtw_write16(padapter, REG_RXFLTMAP1, 0x0400);
+
+			/* Beacon Control related register for first time */
+			rtw_write8(padapter, REG_BCNDMATIM, 0x02); /*  2ms */
+
+			/* rtw_write8(padapter, REG_BCN_MAX_ERR, 0xFF); */
+			rtw_write8(padapter, REG_ATIMWND, 0x0a); /*  10ms */
+			rtw_write16(padapter, REG_BCNTCFG, 0x00);
+			rtw_write16(padapter, REG_TBTT_PROHIBIT, 0xff04);
+			rtw_write16(padapter, REG_TSFTR_SYN_OFFSET, 0x7fff);/*  +32767 (~32ms) */
+
+			/* reset TSF */
+			rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+
+			/* enable BCN0 Function for if1 */
+			/* don't enable update TSF0 for if1 (due to TSF update when beacon/probe rsp are received) */
+			rtw_write8(padapter, REG_BCN_CTRL, (DIS_TSF_UDT|EN_BCN_FUNCTION|EN_TXBCN_RPT|DIS_BCNQ_SUB));
+
+			/* SW_BCN_SEL - Port0 */
+			/* rtw_write8(Adapter, REG_DWBCN1_CTRL_8192E+2, rtw_read8(Adapter, REG_DWBCN1_CTRL_8192E+2) & ~BIT4); */
+			rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+			/*  select BCN on port 0 */
+			rtw_write8(
+				padapter,
+				REG_CCK_CHECK_8723B,
+				(rtw_read8(padapter, REG_CCK_CHECK_8723B)&~BIT_BCN_PORT_SEL)
+			);
+
+			/*  dis BCN1 ATIM  WND if if2 is station */
+			val8 = rtw_read8(padapter, REG_BCN_CTRL_1);
+			val8 |= DIS_ATIM;
+			rtw_write8(padapter, REG_BCN_CTRL_1, val8);
+		}
+	}
+}
+
+static void hw_var_set_macaddr(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 idx = 0;
+	u32 reg_macid;
+
+	reg_macid = REG_MACID;
+
+	for (idx = 0 ; idx < 6; idx++)
+		rtw_write8(GET_PRIMARY_ADAPTER(padapter), (reg_macid+idx), val[idx]);
+}
+
+static void hw_var_set_bssid(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 idx = 0;
+	u32 reg_bssid;
+
+	reg_bssid = REG_BSSID;
+
+	for (idx = 0 ; idx < 6; idx++)
+		rtw_write8(padapter, (reg_bssid+idx), val[idx]);
+}
+
+static void hw_var_set_bcn_func(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u32 bcn_ctrl_reg;
+
+	bcn_ctrl_reg = REG_BCN_CTRL;
+
+	if (*(u8 *)val)
+		rtw_write8(padapter, bcn_ctrl_reg, (EN_BCN_FUNCTION | EN_TXBCN_RPT));
+	else {
+		u8 val8;
+		val8 = rtw_read8(padapter, bcn_ctrl_reg);
+		val8 &= ~(EN_BCN_FUNCTION | EN_TXBCN_RPT);
+
+		/*  Always enable port0 beacon function for PSTDMA */
+		if (REG_BCN_CTRL == bcn_ctrl_reg)
+			val8 |= EN_BCN_FUNCTION;
+
+		rtw_write8(padapter, bcn_ctrl_reg, val8);
+	}
+}
+
+static void hw_var_set_correct_tsf(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 val8;
+	u64 tsf;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	tsf = pmlmeext->TSFValue-do_div(pmlmeext->TSFValue, (pmlmeinfo->bcn_interval*1024))-1024; /* us */
+
+	if (
+		((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) ||
+		((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	)
+		StopTxBeacon(padapter);
+
+	{
+		/*  disable related TSF function */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 &= ~EN_BCN_FUNCTION;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		rtw_write32(padapter, REG_TSFTR, tsf);
+		rtw_write32(padapter, REG_TSFTR+4, tsf>>32);
+
+		/*  enable related TSF function */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 |= EN_BCN_FUNCTION;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+	}
+
+	if (
+		((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) ||
+		((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	)
+		ResumeTxBeacon(padapter);
+}
+
+static void hw_var_set_mlme_disconnect(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 val8;
+
+	/*  Set RCR to not to receive data frame when NO LINK state */
+	/* rtw_write32(padapter, REG_RCR, rtw_read32(padapter, REG_RCR) & ~RCR_ADF); */
+	/*  reject all data frames */
+	rtw_write16(padapter, REG_RXFLTMAP2, 0);
+
+	/*  reset TSF */
+	rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+
+	/*  disable update TSF */
+	val8 = rtw_read8(padapter, REG_BCN_CTRL);
+	val8 |= DIS_TSF_UDT;
+	rtw_write8(padapter, REG_BCN_CTRL, val8);
+}
+
+static void hw_var_set_mlme_sitesurvey(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u32 value_rcr, rcr_clear_bit, reg_bcn_ctl;
+	u16 value_rxfltmap2;
+	u8 val8;
+	struct hal_com_data *pHalData;
+	struct mlme_priv *pmlmepriv;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+
+	reg_bcn_ctl = REG_BCN_CTRL;
+
+	rcr_clear_bit = RCR_CBSSID_BCN;
+
+	/*  config RCR to receive different BSSID & not to receive data frame */
+	value_rxfltmap2 = 0;
+
+	if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true))
+		rcr_clear_bit = RCR_CBSSID_BCN;
+
+	value_rcr = rtw_read32(padapter, REG_RCR);
+
+	if (*((u8 *)val)) {
+		/*  under sitesurvey */
+		value_rcr &= ~(rcr_clear_bit);
+		rtw_write32(padapter, REG_RCR, value_rcr);
+
+		rtw_write16(padapter, REG_RXFLTMAP2, value_rxfltmap2);
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+			/*  disable update TSF */
+			val8 = rtw_read8(padapter, reg_bcn_ctl);
+			val8 |= DIS_TSF_UDT;
+			rtw_write8(padapter, reg_bcn_ctl, val8);
+		}
+
+		/*  Save original RRSR setting. */
+		pHalData->RegRRSR = rtw_read16(padapter, REG_RRSR);
+	} else {
+		/*  sitesurvey done */
+		if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)))
+			/*  enable to rx data frame */
+			rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+			/*  enable update TSF */
+			val8 = rtw_read8(padapter, reg_bcn_ctl);
+			val8 &= ~DIS_TSF_UDT;
+			rtw_write8(padapter, reg_bcn_ctl, val8);
+		}
+
+		value_rcr |= rcr_clear_bit;
+		rtw_write32(padapter, REG_RCR, value_rcr);
+
+		/*  Restore original RRSR setting. */
+		rtw_write16(padapter, REG_RRSR, pHalData->RegRRSR);
+	}
+}
+
+static void hw_var_set_mlme_join(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 val8;
+	u16 val16;
+	u32 val32;
+	u8 RetryLimit;
+	u8 type;
+	struct mlme_priv *pmlmepriv;
+	struct eeprom_priv *pEEPROM;
+
+
+	RetryLimit = 0x30;
+	type = *(u8 *)val;
+	pmlmepriv = &padapter->mlmepriv;
+	pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	if (type == 0) { /*  prepare to join */
+		/* enable to rx data frame.Accept all data frame */
+		/* rtw_write32(padapter, REG_RCR, rtw_read32(padapter, REG_RCR)|RCR_ADF); */
+		rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+
+		val32 = rtw_read32(padapter, REG_RCR);
+		if (padapter->in_cta_test)
+			val32 &= ~(RCR_CBSSID_DATA | RCR_CBSSID_BCN);/*  RCR_ADF */
+		else
+			val32 |= RCR_CBSSID_DATA|RCR_CBSSID_BCN;
+		rtw_write32(padapter, REG_RCR, val32);
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+			RetryLimit = (pEEPROM->CustomerID == RT_CID_CCX) ? 7 : 48;
+		else /*  Ad-hoc Mode */
+			RetryLimit = 0x7;
+	} else if (type == 1) /* joinbss_event call back when join res < 0 */
+		rtw_write16(padapter, REG_RXFLTMAP2, 0x00);
+	else if (type == 2) { /* sta add event call back */
+		/* enable update TSF */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 &= ~DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))
+			RetryLimit = 0x7;
+	}
+
+	val16 = (RetryLimit << RETRY_LIMIT_SHORT_SHIFT) | (RetryLimit << RETRY_LIMIT_LONG_SHIFT);
+	rtw_write16(padapter, REG_RL, val16);
+}
+
+void CCX_FwC2HTxRpt_8723b(struct adapter *padapter, u8 *pdata, u8 len)
+{
+
+#define	GET_8723B_C2H_TX_RPT_LIFE_TIME_OVER(_Header)	LE_BITS_TO_1BYTE((_Header + 0), 6, 1)
+#define	GET_8723B_C2H_TX_RPT_RETRY_OVER(_Header)	LE_BITS_TO_1BYTE((_Header + 0), 7, 1)
+
+	if (GET_8723B_C2H_TX_RPT_RETRY_OVER(pdata) | GET_8723B_C2H_TX_RPT_LIFE_TIME_OVER(pdata)) {
+		rtw_ack_tx_done(&padapter->xmitpriv, RTW_SCTX_DONE_CCX_PKT_FAIL);
+	}
+/*
+	else if (seq_no != padapter->xmitpriv.seq_no) {
+		rtw_ack_tx_done(&padapter->xmitpriv, RTW_SCTX_DONE_CCX_PKT_FAIL);
+	}
+*/
+	else
+		rtw_ack_tx_done(&padapter->xmitpriv, RTW_SCTX_DONE_SUCCESS);
+}
+
+s32 c2h_id_filter_ccx_8723b(u8 *buf)
+{
+	struct c2h_evt_hdr_88xx *c2h_evt = (struct c2h_evt_hdr_88xx *)buf;
+	s32 ret = false;
+	if (c2h_evt->id == C2H_CCX_TX_RPT)
+		ret = true;
+
+	return ret;
+}
+
+
+s32 c2h_handler_8723b(struct adapter *padapter, u8 *buf)
+{
+	struct c2h_evt_hdr_88xx *pC2hEvent = (struct c2h_evt_hdr_88xx *)buf;
+	s32 ret = _SUCCESS;
+
+	if (!pC2hEvent) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	switch (pC2hEvent->id) {
+	case C2H_AP_RPT_RSP:
+		break;
+	case C2H_DBG:
+		{
+		}
+		break;
+
+	case C2H_CCX_TX_RPT:
+/* 			CCX_FwC2HTxRpt(padapter, QueueID, pC2hEvent->payload); */
+		break;
+
+	case C2H_EXT_RA_RPT:
+/* 			C2HExtRaRptHandler(padapter, pC2hEvent->payload, C2hEvent.CmdLen); */
+		break;
+
+	case C2H_HW_INFO_EXCH:
+		break;
+
+	case C2H_8723B_BT_INFO:
+		hal_btcoex_BtInfoNotify(padapter, pC2hEvent->plen, pC2hEvent->payload);
+		break;
+
+	default:
+		break;
+	}
+
+	/*  Clear event to notify FW we have read the command. */
+	/*  Note: */
+	/* 	If this field isn't clear, the FW won't update the next command message. */
+/* 	rtw_write8(padapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE); */
+exit:
+	return ret;
+}
+
+static void process_c2h_event(struct adapter *padapter, struct c2h_evt_hdr_t *pC2hEvent, u8 *c2hBuf)
+{
+	if (!c2hBuf)
+		return;
+
+	switch (pC2hEvent->CmdID) {
+	case C2H_AP_RPT_RSP:
+		break;
+	case C2H_DBG:
+		{
+		}
+		break;
+
+	case C2H_CCX_TX_RPT:
+/* 			CCX_FwC2HTxRpt(padapter, QueueID, tmpBuf); */
+		break;
+
+	case C2H_EXT_RA_RPT:
+/* 			C2HExtRaRptHandler(padapter, tmpBuf, C2hEvent.CmdLen); */
+		break;
+
+	case C2H_HW_INFO_EXCH:
+		break;
+
+	case C2H_8723B_BT_INFO:
+		hal_btcoex_BtInfoNotify(padapter, pC2hEvent->CmdLen, c2hBuf);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void C2HPacketHandler_8723B(struct adapter *padapter, u8 *pbuffer, u16 length)
+{
+	struct c2h_evt_hdr_t	C2hEvent;
+	u8 *tmpBuf = NULL;
+	C2hEvent.CmdID = pbuffer[0];
+	C2hEvent.CmdSeq = pbuffer[1];
+	C2hEvent.CmdLen = length-2;
+	tmpBuf = pbuffer+2;
+
+	process_c2h_event(padapter, &C2hEvent, tmpBuf);
+	/* c2h_handler_8723b(padapter,&C2hEvent); */
+}
+
+void SetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 val8;
+	u32 val32;
+
+	switch (variable) {
+	case HW_VAR_MEDIA_STATUS:
+		val8 = rtw_read8(padapter, MSR) & 0x0c;
+		val8 |= *val;
+		rtw_write8(padapter, MSR, val8);
+		break;
+
+	case HW_VAR_MEDIA_STATUS1:
+		val8 = rtw_read8(padapter, MSR) & 0x03;
+		val8 |= *val << 2;
+		rtw_write8(padapter, MSR, val8);
+		break;
+
+	case HW_VAR_SET_OPMODE:
+		hw_var_set_opmode(padapter, variable, val);
+		break;
+
+	case HW_VAR_MAC_ADDR:
+		hw_var_set_macaddr(padapter, variable, val);
+		break;
+
+	case HW_VAR_BSSID:
+		hw_var_set_bssid(padapter, variable, val);
+		break;
+
+	case HW_VAR_BASIC_RATE:
+	{
+		struct mlme_ext_info *mlmext_info = &padapter->mlmeextpriv.mlmext_info;
+		u16 BrateCfg = 0;
+		u16 rrsr_2g_force_mask = (RRSR_11M|RRSR_5_5M|RRSR_1M);
+		u16 rrsr_2g_allow_mask = (RRSR_24M|RRSR_12M|RRSR_6M|RRSR_CCK_RATES);
+
+		HalSetBrateCfg(padapter, val, &BrateCfg);
+
+		/* apply force and allow mask */
+		BrateCfg |= rrsr_2g_force_mask;
+		BrateCfg &= rrsr_2g_allow_mask;
+
+		/* IOT consideration */
+		if (mlmext_info->assoc_AP_vendor == HT_IOT_PEER_CISCO) {
+			/* if peer is cisco and didn't use ofdm rate, we enable 6M ack */
+			if ((BrateCfg & (RRSR_24M|RRSR_12M|RRSR_6M)) == 0)
+				BrateCfg |= RRSR_6M;
+		}
+
+		pHalData->BasicRateSet = BrateCfg;
+
+		/*  Set RRSR rate table. */
+		rtw_write16(padapter, REG_RRSR, BrateCfg);
+		rtw_write8(padapter, REG_RRSR+2, rtw_read8(padapter, REG_RRSR+2)&0xf0);
+	}
+		break;
+
+	case HW_VAR_TXPAUSE:
+		rtw_write8(padapter, REG_TXPAUSE, *val);
+		break;
+
+	case HW_VAR_BCN_FUNC:
+		hw_var_set_bcn_func(padapter, variable, val);
+		break;
+
+	case HW_VAR_CORRECT_TSF:
+		hw_var_set_correct_tsf(padapter, variable, val);
+		break;
+
+	case HW_VAR_CHECK_BSSID:
+		{
+			u32 val32;
+			val32 = rtw_read32(padapter, REG_RCR);
+			if (*val)
+				val32 |= RCR_CBSSID_DATA|RCR_CBSSID_BCN;
+			else
+				val32 &= ~(RCR_CBSSID_DATA|RCR_CBSSID_BCN);
+			rtw_write32(padapter, REG_RCR, val32);
+		}
+		break;
+
+	case HW_VAR_MLME_DISCONNECT:
+		hw_var_set_mlme_disconnect(padapter, variable, val);
+		break;
+
+	case HW_VAR_MLME_SITESURVEY:
+		hw_var_set_mlme_sitesurvey(padapter, variable,  val);
+
+		hal_btcoex_ScanNotify(padapter, *val?true:false);
+		break;
+
+	case HW_VAR_MLME_JOIN:
+		hw_var_set_mlme_join(padapter, variable, val);
+
+		switch (*val) {
+		case 0:
+			/*  prepare to join */
+			hal_btcoex_ConnectNotify(padapter, true);
+			break;
+		case 1:
+			/*  joinbss_event callback when join res < 0 */
+			hal_btcoex_ConnectNotify(padapter, false);
+			break;
+		case 2:
+			/*  sta add event callback */
+/* 				rtw_btcoex_MediaStatusNotify(padapter, RT_MEDIA_CONNECT); */
+			break;
+		}
+		break;
+
+	case HW_VAR_ON_RCR_AM:
+		val32 = rtw_read32(padapter, REG_RCR);
+		val32 |= RCR_AM;
+		rtw_write32(padapter, REG_RCR, val32);
+		break;
+
+	case HW_VAR_OFF_RCR_AM:
+		val32 = rtw_read32(padapter, REG_RCR);
+		val32 &= ~RCR_AM;
+		rtw_write32(padapter, REG_RCR, val32);
+		break;
+
+	case HW_VAR_BEACON_INTERVAL:
+		rtw_write16(padapter, REG_BCN_INTERVAL, *((u16 *)val));
+		break;
+
+	case HW_VAR_SLOT_TIME:
+		rtw_write8(padapter, REG_SLOT, *val);
+		break;
+
+	case HW_VAR_RESP_SIFS:
+		/* SIFS_Timer = 0x0a0a0808; */
+		/* RESP_SIFS for CCK */
+		rtw_write8(padapter, REG_RESP_SIFS_CCK, val[0]); /*  SIFS_T2T_CCK (0x08) */
+		rtw_write8(padapter, REG_RESP_SIFS_CCK+1, val[1]); /* SIFS_R2T_CCK(0x08) */
+		/* RESP_SIFS for OFDM */
+		rtw_write8(padapter, REG_RESP_SIFS_OFDM, val[2]); /* SIFS_T2T_OFDM (0x0a) */
+		rtw_write8(padapter, REG_RESP_SIFS_OFDM+1, val[3]); /* SIFS_R2T_OFDM(0x0a) */
+		break;
+
+	case HW_VAR_ACK_PREAMBLE:
+		{
+			u8 regTmp;
+			u8 bShortPreamble = *val;
+
+			/*  Joseph marked out for Netgear 3500 TKIP channel 7 issue.(Temporarily) */
+			/* regTmp = (pHalData->nCur40MhzPrimeSC)<<5; */
+			regTmp = 0;
+			if (bShortPreamble)
+				regTmp |= 0x80;
+			rtw_write8(padapter, REG_RRSR+2, regTmp);
+		}
+		break;
+
+	case HW_VAR_CAM_EMPTY_ENTRY:
+		{
+			u8 ucIndex = *val;
+			u8 i;
+			u32 ulCommand = 0;
+			u32 ulContent = 0;
+			u32 ulEncAlgo = CAM_AES;
+
+			for (i = 0; i < CAM_CONTENT_COUNT; i++) {
+				/*  filled id in CAM config 2 byte */
+				if (i == 0) {
+					ulContent |= (ucIndex & 0x03) | ((u16)(ulEncAlgo)<<2);
+					/* ulContent |= CAM_VALID; */
+				} else
+					ulContent = 0;
+
+				/*  polling bit, and No Write enable, and address */
+				ulCommand = CAM_CONTENT_COUNT*ucIndex+i;
+				ulCommand = ulCommand | CAM_POLLINIG | CAM_WRITE;
+				/*  write content 0 is equall to mark invalid */
+				rtw_write32(padapter, WCAMI, ulContent);  /* mdelay(40); */
+				rtw_write32(padapter, RWCAM, ulCommand);  /* mdelay(40); */
+			}
+		}
+		break;
+
+	case HW_VAR_CAM_INVALID_ALL:
+		rtw_write32(padapter, RWCAM, BIT(31)|BIT(30));
+		break;
+
+	case HW_VAR_CAM_WRITE:
+		{
+			u32 cmd;
+			u32 *cam_val = (u32 *)val;
+
+			rtw_write32(padapter, WCAMI, cam_val[0]);
+
+			cmd = CAM_POLLINIG | CAM_WRITE | cam_val[1];
+			rtw_write32(padapter, RWCAM, cmd);
+		}
+		break;
+
+	case HW_VAR_AC_PARAM_VO:
+		rtw_write32(padapter, REG_EDCA_VO_PARAM, *((u32 *)val));
+		break;
+
+	case HW_VAR_AC_PARAM_VI:
+		rtw_write32(padapter, REG_EDCA_VI_PARAM, *((u32 *)val));
+		break;
+
+	case HW_VAR_AC_PARAM_BE:
+		pHalData->AcParam_BE = ((u32 *)(val))[0];
+		rtw_write32(padapter, REG_EDCA_BE_PARAM, *((u32 *)val));
+		break;
+
+	case HW_VAR_AC_PARAM_BK:
+		rtw_write32(padapter, REG_EDCA_BK_PARAM, *((u32 *)val));
+		break;
+
+	case HW_VAR_ACM_CTRL:
+		{
+			u8 ctrl = *((u8 *)val);
+			u8 hwctrl = 0;
+
+			if (ctrl != 0) {
+				hwctrl |= AcmHw_HwEn;
+
+				if (ctrl & BIT(1)) /*  BE */
+					hwctrl |= AcmHw_BeqEn;
+
+				if (ctrl & BIT(2)) /*  VI */
+					hwctrl |= AcmHw_ViqEn;
+
+				if (ctrl & BIT(3)) /*  VO */
+					hwctrl |= AcmHw_VoqEn;
+			}
+
+			rtw_write8(padapter, REG_ACMHWCTRL, hwctrl);
+		}
+		break;
+
+	case HW_VAR_AMPDU_FACTOR:
+		{
+			u32 AMPDULen =  (*((u8 *)val));
+
+			if (AMPDULen < HT_AGG_SIZE_32K)
+				AMPDULen = (0x2000 << (*((u8 *)val)))-1;
+			else
+				AMPDULen = 0x7fff;
+
+			rtw_write32(padapter, REG_AMPDU_MAX_LENGTH_8723B, AMPDULen);
+		}
+		break;
+
+	case HW_VAR_H2C_FW_PWRMODE:
+		{
+			u8 psmode = *val;
+
+			/*  Forece leave RF low power mode for 1T1R to prevent conficting setting in Fw power */
+			/*  saving sequence. 2010.06.07. Added by tynli. Suggested by SD3 yschang. */
+			if (psmode != PS_MODE_ACTIVE) {
+				ODM_RF_Saving(&pHalData->odmpriv, true);
+			}
+
+			/* if (psmode != PS_MODE_ACTIVE)	{ */
+			/* 	rtl8723b_set_lowpwr_lps_cmd(padapter, true); */
+			/*  else { */
+			/* 	rtl8723b_set_lowpwr_lps_cmd(padapter, false); */
+			/*  */
+			rtl8723b_set_FwPwrMode_cmd(padapter, psmode);
+		}
+		break;
+	case HW_VAR_H2C_PS_TUNE_PARAM:
+		rtl8723b_set_FwPsTuneParam_cmd(padapter);
+		break;
+
+	case HW_VAR_H2C_FW_JOINBSSRPT:
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, *val);
+		break;
+
+	case HW_VAR_INITIAL_GAIN:
+		{
+			struct dig_t *pDigTable = &pHalData->odmpriv.DM_DigTable;
+			u32 rx_gain = *(u32 *)val;
+
+			if (rx_gain == 0xff) {/* restore rx gain */
+				ODM_Write_DIG(&pHalData->odmpriv, pDigTable->BackupIGValue);
+			} else {
+				pDigTable->BackupIGValue = pDigTable->CurIGValue;
+				ODM_Write_DIG(&pHalData->odmpriv, rx_gain);
+			}
+		}
+		break;
+
+	case HW_VAR_EFUSE_USAGE:
+		pHalData->EfuseUsedPercentage = *val;
+		break;
+
+	case HW_VAR_EFUSE_BYTES:
+		pHalData->EfuseUsedBytes = *((u16 *)val);
+		break;
+
+	case HW_VAR_EFUSE_BT_USAGE:
+#ifdef HAL_EFUSE_MEMORY
+		pHalData->EfuseHal.BTEfuseUsedPercentage = *val;
+#endif
+		break;
+
+	case HW_VAR_EFUSE_BT_BYTES:
+#ifdef HAL_EFUSE_MEMORY
+		pHalData->EfuseHal.BTEfuseUsedBytes = *((u16 *)val);
+#else
+		BTEfuseUsedBytes = *((u16 *)val);
+#endif
+		break;
+
+	case HW_VAR_FIFO_CLEARN_UP:
+		{
+			#define RW_RELEASE_EN		BIT(18)
+			#define RXDMA_IDLE			BIT(17)
+
+			struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+			u8 trycnt = 100;
+
+			/*  pause tx */
+			rtw_write8(padapter, REG_TXPAUSE, 0xff);
+
+			/*  keep sn */
+			padapter->xmitpriv.nqos_ssn = rtw_read16(padapter, REG_NQOS_SEQ);
+
+			if (!pwrpriv->bkeepfwalive) {
+				/* RX DMA stop */
+				val32 = rtw_read32(padapter, REG_RXPKT_NUM);
+				val32 |= RW_RELEASE_EN;
+				rtw_write32(padapter, REG_RXPKT_NUM, val32);
+				do {
+					val32 = rtw_read32(padapter, REG_RXPKT_NUM);
+					val32 &= RXDMA_IDLE;
+					if (val32)
+						break;
+				} while (--trycnt);
+
+				/*  RQPN Load 0 */
+				rtw_write16(padapter, REG_RQPN_NPQ, 0);
+				rtw_write32(padapter, REG_RQPN, 0x80000000);
+				mdelay(2);
+			}
+		}
+		break;
+
+	case HW_VAR_APFM_ON_MAC:
+		pHalData->bMacPwrCtrlOn = *val;
+		break;
+
+	case HW_VAR_NAV_UPPER:
+		{
+			u32 usNavUpper = *((u32 *)val);
+
+			if (usNavUpper > HAL_NAV_UPPER_UNIT_8723B * 0xFF)
+				break;
+
+			usNavUpper = DIV_ROUND_UP(usNavUpper,
+						  HAL_NAV_UPPER_UNIT_8723B);
+			rtw_write8(padapter, REG_NAV_UPPER, (u8)usNavUpper);
+		}
+		break;
+
+	case HW_VAR_H2C_MEDIA_STATUS_RPT:
+		{
+			u16 mstatus_rpt = (*(u16 *)val);
+			u8 mstatus, macId;
+
+			mstatus = (u8) (mstatus_rpt & 0xFF);
+			macId = (u8)(mstatus_rpt >> 8);
+			rtl8723b_set_FwMediaStatusRpt_cmd(padapter, mstatus, macId);
+		}
+		break;
+	case HW_VAR_BCN_VALID:
+		{
+			/*  BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2, write 1 to clear, Clear by sw */
+			val8 = rtw_read8(padapter, REG_TDECTRL+2);
+			val8 |= BIT(0);
+			rtw_write8(padapter, REG_TDECTRL+2, val8);
+		}
+		break;
+
+	case HW_VAR_DL_BCN_SEL:
+		{
+			/*  SW_BCN_SEL - Port0 */
+			val8 = rtw_read8(padapter, REG_DWBCN1_CTRL_8723B+2);
+			val8 &= ~BIT(4);
+			rtw_write8(padapter, REG_DWBCN1_CTRL_8723B+2, val8);
+		}
+		break;
+
+	case HW_VAR_DO_IQK:
+		pHalData->bNeedIQK = true;
+		break;
+
+	case HW_VAR_DL_RSVD_PAGE:
+		if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)
+			rtl8723b_download_BTCoex_AP_mode_rsvd_page(padapter);
+		else
+			rtl8723b_download_rsvd_page(padapter, RT_MEDIA_CONNECT);
+		break;
+
+	case HW_VAR_MACID_SLEEP:
+		/*  Input is MACID */
+		val32 = *(u32 *)val;
+		if (val32 > 31)
+			break;
+
+		val8 = (u8)val32; /*  macid is between 0~31 */
+
+		val32 = rtw_read32(padapter, REG_MACID_SLEEP);
+		if (val32 & BIT(val8))
+			break;
+		val32 |= BIT(val8);
+		rtw_write32(padapter, REG_MACID_SLEEP, val32);
+		break;
+
+	case HW_VAR_MACID_WAKEUP:
+		/*  Input is MACID */
+		val32 = *(u32 *)val;
+		if (val32 > 31)
+			break;
+
+		val8 = (u8)val32; /*  macid is between 0~31 */
+
+		val32 = rtw_read32(padapter, REG_MACID_SLEEP);
+		if (!(val32 & BIT(val8)))
+			break;
+		val32 &= ~BIT(val8);
+		rtw_write32(padapter, REG_MACID_SLEEP, val32);
+		break;
+
+	default:
+		SetHwReg(padapter, variable, val);
+		break;
+	}
+}
+
+void GetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 val8;
+	u16 val16;
+
+	switch (variable) {
+	case HW_VAR_TXPAUSE:
+		*val = rtw_read8(padapter, REG_TXPAUSE);
+		break;
+
+	case HW_VAR_BCN_VALID:
+		{
+			/*  BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2 */
+			val8 = rtw_read8(padapter, REG_TDECTRL+2);
+			*val = (BIT(0) & val8) ? true : false;
+		}
+		break;
+
+	case HW_VAR_FWLPS_RF_ON:
+		{
+			/*  When we halt NIC, we should check if FW LPS is leave. */
+			u32 valRCR;
+
+			if (
+				padapter->bSurpriseRemoved  ||
+				(adapter_to_pwrctl(padapter)->rf_pwrstate == rf_off)
+			) {
+				/*  If it is in HW/SW Radio OFF or IPS state, we do not check Fw LPS Leave, */
+				/*  because Fw is unload. */
+				*val = true;
+			} else {
+				valRCR = rtw_read32(padapter, REG_RCR);
+				valRCR &= 0x00070000;
+				if (valRCR)
+					*val = false;
+				else
+					*val = true;
+			}
+		}
+		break;
+
+	case HW_VAR_EFUSE_USAGE:
+		*val = pHalData->EfuseUsedPercentage;
+		break;
+
+	case HW_VAR_EFUSE_BYTES:
+		*((u16 *)val) = pHalData->EfuseUsedBytes;
+		break;
+
+	case HW_VAR_EFUSE_BT_USAGE:
+#ifdef HAL_EFUSE_MEMORY
+		*val = pHalData->EfuseHal.BTEfuseUsedPercentage;
+#endif
+		break;
+
+	case HW_VAR_EFUSE_BT_BYTES:
+#ifdef HAL_EFUSE_MEMORY
+		*((u16 *)val) = pHalData->EfuseHal.BTEfuseUsedBytes;
+#else
+		*((u16 *)val) = BTEfuseUsedBytes;
+#endif
+		break;
+
+	case HW_VAR_APFM_ON_MAC:
+		*val = pHalData->bMacPwrCtrlOn;
+		break;
+	case HW_VAR_CHK_HI_QUEUE_EMPTY:
+		val16 = rtw_read16(padapter, REG_TXPKT_EMPTY);
+		*val = (val16 & BIT(10)) ? true:false;
+		break;
+	default:
+		GetHwReg(padapter, variable, val);
+		break;
+	}
+}
+
+/* Description:
+ *	Change default setting of specified variable.
+ */
+u8 SetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable, void *pval)
+{
+	u8 bResult;
+
+	bResult = _SUCCESS;
+
+	switch (variable) {
+	default:
+		bResult = SetHalDefVar(padapter, variable, pval);
+		break;
+	}
+
+	return bResult;
+}
+
+/* Description:
+ *	Query setting of specified variable.
+ */
+u8 GetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable, void *pval)
+{
+	u8 bResult;
+
+	bResult = _SUCCESS;
+
+	switch (variable) {
+	case HAL_DEF_MAX_RECVBUF_SZ:
+		*((u32 *)pval) = MAX_RECVBUF_SZ;
+		break;
+
+	case HAL_DEF_RX_PACKET_OFFSET:
+		*((u32 *)pval) = RXDESC_SIZE + DRVINFO_SZ*8;
+		break;
+
+	case HW_VAR_MAX_RX_AMPDU_FACTOR:
+		/*  Stanley@BB.SD3 suggests 16K can get stable performance */
+		/*  The experiment was done on SDIO interface */
+		/*  coding by Lucas@20130730 */
+		*(u32 *)pval = IEEE80211_HT_MAX_AMPDU_16K;
+		break;
+	case HAL_DEF_TX_LDPC:
+	case HAL_DEF_RX_LDPC:
+		*((u8 *)pval) = false;
+		break;
+	case HAL_DEF_TX_STBC:
+		*((u8 *)pval) = 0;
+		break;
+	case HAL_DEF_RX_STBC:
+		*((u8 *)pval) = 1;
+		break;
+	case HAL_DEF_EXPLICIT_BEAMFORMER:
+	case HAL_DEF_EXPLICIT_BEAMFORMEE:
+		*((u8 *)pval) = false;
+		break;
+
+	case HW_DEF_RA_INFO_DUMP:
+		{
+			u8 mac_id = *(u8 *)pval;
+			u32 cmd;
+
+			cmd = 0x40000100 | mac_id;
+			rtw_write32(padapter, REG_HMEBOX_DBG_2_8723B, cmd);
+			msleep(10);
+			rtw_read32(padapter, 0x2F0);	// info 1
+
+			cmd = 0x40000400 | mac_id;
+			rtw_write32(padapter, REG_HMEBOX_DBG_2_8723B, cmd);
+			msleep(10);
+			rtw_read32(padapter, 0x2F0);	// info 1
+			rtw_read32(padapter, 0x2F4);	// info 2
+			rtw_read32(padapter, 0x2F8);	// rate mask 1
+			rtw_read32(padapter, 0x2FC);	// rate mask 2
+		}
+		break;
+
+	case HAL_DEF_TX_PAGE_BOUNDARY:
+		if (!padapter->registrypriv.wifi_spec) {
+			*(u8 *)pval = TX_PAGE_BOUNDARY_8723B;
+		} else {
+			*(u8 *)pval = WMM_NORMAL_TX_PAGE_BOUNDARY_8723B;
+		}
+		break;
+
+	case HAL_DEF_MACID_SLEEP:
+		*(u8 *)pval = true; /*  support macid sleep */
+		break;
+
+	default:
+		bResult = GetHalDefVar(padapter, variable, pval);
+		break;
+	}
+
+	return bResult;
+}
+
+void rtl8723b_start_thread(struct adapter *padapter)
+{
+	struct xmit_priv *xmitpriv = &padapter->xmitpriv;
+
+	xmitpriv->SdioXmitThread = kthread_run(rtl8723bs_xmit_thread, padapter, "RTWHALXT");
+}
+
+void rtl8723b_stop_thread(struct adapter *padapter)
+{
+	struct xmit_priv *xmitpriv = &padapter->xmitpriv;
+
+	/*  stop xmit_buf_thread */
+	if (xmitpriv->SdioXmitThread) {
+		complete(&xmitpriv->SdioXmitStart);
+		wait_for_completion(&xmitpriv->SdioXmitTerminate);
+		xmitpriv->SdioXmitThread = NULL;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
new file mode 100644
index 000000000..a3bff27af
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
@@ -0,0 +1,794 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+/**
+ * phy_CalculateBitShift - Get shifted position of the BitMask.
+ * @BitMask: Bitmask.
+ *
+ * Return:	Return the shift bit position of the mask
+ */
+static	u32 phy_CalculateBitShift(u32 BitMask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((BitMask>>i) &  0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+
+/**
+ * PHY_QueryBBReg_8723B - Read "specific bits" from BB register.
+ * @Adapter:
+ * @RegAddr:	The target address to be readback
+ * @BitMask:	The target bit position in the target address
+ *				to be readback
+ *
+ * Return:	The readback register value
+ *
+ * .. Note::	This function is equal to "GetRegSetting" in PHY programming
+ *			guide
+ */
+u32 PHY_QueryBBReg_8723B(struct adapter *Adapter, u32 RegAddr, u32 BitMask)
+{
+	u32 OriginalValue, BitShift;
+
+	OriginalValue = rtw_read32(Adapter, RegAddr);
+	BitShift = phy_CalculateBitShift(BitMask);
+
+	return (OriginalValue & BitMask) >> BitShift;
+
+}
+
+
+/**
+ * PHY_SetBBReg_8723B - Write "Specific bits" to BB register (page 8~).
+ * @Adapter:
+ * @RegAddr:	The target address to be modified
+ * @BitMask:	The target bit position in the target address
+ *				to be modified
+ * @Data:		The new register value in the target bit position
+ *				of the target address
+ *
+ * .. Note::	This function is equal to "PutRegSetting" in PHY programming
+ *			guide
+ */
+
+void PHY_SetBBReg_8723B(
+	struct adapter *Adapter,
+	u32 RegAddr,
+	u32 BitMask,
+	u32 Data
+)
+{
+	/* u16 BBWaitCounter	= 0; */
+	u32 OriginalValue, BitShift;
+
+	if (BitMask != bMaskDWord) { /* if not "double word" write */
+		OriginalValue = rtw_read32(Adapter, RegAddr);
+		BitShift = phy_CalculateBitShift(BitMask);
+		Data = ((OriginalValue & (~BitMask)) | ((Data << BitShift) & BitMask));
+	}
+
+	rtw_write32(Adapter, RegAddr, Data);
+
+}
+
+
+/*  */
+/*  2. RF register R/W API */
+/*  */
+
+static u32 phy_RFSerialRead_8723B(
+	struct adapter *Adapter, enum rf_path eRFPath, u32 Offset
+)
+{
+	u32 retValue = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct bb_register_def *pPhyReg = &pHalData->PHYRegDef[eRFPath];
+	u32 NewOffset;
+	u32 tmplong2;
+	u8 RfPiEnable = 0;
+	u32 MaskforPhySet = 0;
+	int i = 0;
+
+	/*  */
+	/*  Make sure RF register offset is correct */
+	/*  */
+	Offset &= 0xff;
+
+	NewOffset = Offset;
+
+	if (eRFPath == RF_PATH_A) {
+		tmplong2 = PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter2|MaskforPhySet, bMaskDWord);
+		tmplong2 = (tmplong2 & (~bLSSIReadAddress)) | (NewOffset<<23) | bLSSIReadEdge;	/* T65 RF */
+		PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2|MaskforPhySet, bMaskDWord, tmplong2&(~bLSSIReadEdge));
+	} else {
+		tmplong2 = PHY_QueryBBReg(Adapter, rFPGA0_XB_HSSIParameter2|MaskforPhySet, bMaskDWord);
+		tmplong2 = (tmplong2 & (~bLSSIReadAddress)) | (NewOffset<<23) | bLSSIReadEdge;	/* T65 RF */
+		PHY_SetBBReg(Adapter, rFPGA0_XB_HSSIParameter2|MaskforPhySet, bMaskDWord, tmplong2&(~bLSSIReadEdge));
+	}
+
+	tmplong2 = PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter2|MaskforPhySet, bMaskDWord);
+	PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2|MaskforPhySet, bMaskDWord, tmplong2 & (~bLSSIReadEdge));
+	PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2|MaskforPhySet, bMaskDWord, tmplong2 | bLSSIReadEdge);
+
+	udelay(10);
+
+	for (i = 0; i < 2; i++)
+		udelay(MAX_STALL_TIME);
+	udelay(10);
+
+	if (eRFPath == RF_PATH_A)
+		RfPiEnable = (u8)PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter1|MaskforPhySet, BIT8);
+	else if (eRFPath == RF_PATH_B)
+		RfPiEnable = (u8)PHY_QueryBBReg(Adapter, rFPGA0_XB_HSSIParameter1|MaskforPhySet, BIT8);
+
+	if (RfPiEnable) {
+		/*  Read from BBreg8b8, 12 bits for 8190, 20bits for T65 RF */
+		retValue = PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBackPi|MaskforPhySet, bLSSIReadBackData);
+	} else {
+		/* Read from BBreg8a0, 12 bits for 8190, 20 bits for T65 RF */
+		retValue = PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBack|MaskforPhySet, bLSSIReadBackData);
+	}
+	return retValue;
+
+}
+
+/**
+ * phy_RFSerialWrite_8723B - Write data to RF register (page 8~).
+ * @Adapter:
+ * @eRFPath:	Radio path of A/B/C/D
+ * @Offset:	The target address to be read
+ * @Data:	The new register Data in the target bit position
+ *			of the target to be read
+ *
+ * .. Note::	Threre are three types of serial operations:
+ *		1. Software serial write
+ *		2. Hardware LSSI-Low Speed Serial Interface
+ *		3. Hardware HSSI-High speed
+ *		serial write. Driver need to implement (1) and (2).
+ *		This function is equal to the combination of RF_ReadReg() and  RFLSSIRead()
+ *
+ * .. Note::		  For RF8256 only
+ *		 The total count of RTL8256(Zebra4) register is around 36 bit it only employs
+ *		 4-bit RF address. RTL8256 uses "register mode control bit" (Reg00[12], Reg00[10])
+ *		 to access register address bigger than 0xf. See "Appendix-4 in PHY Configuration
+ *		 programming guide" for more details.
+ *		 Thus, we define a sub-finction for RTL8526 register address conversion
+ *	       ===========================================================
+ *		 Register Mode		RegCTL[1]		RegCTL[0]		Note
+ *							(Reg00[12])		(Reg00[10])
+ *	       ===========================================================
+ *		 Reg_Mode0				0				x			Reg 0 ~15(0x0 ~ 0xf)
+ *	       ------------------------------------------------------------------
+ *		 Reg_Mode1				1				0			Reg 16 ~30(0x1 ~ 0xf)
+ *	       ------------------------------------------------------------------
+ *		 Reg_Mode2				1				1			Reg 31 ~ 45(0x1 ~ 0xf)
+ *	       ------------------------------------------------------------------
+ *
+ *2008/09/02	MH	Add 92S RF definition
+ *
+ *
+ *
+ */
+static void phy_RFSerialWrite_8723B(
+	struct adapter *Adapter,
+	enum rf_path eRFPath,
+	u32 Offset,
+	u32 Data
+)
+{
+	u32 DataAndAddr = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct bb_register_def *pPhyReg = &pHalData->PHYRegDef[eRFPath];
+	u32 NewOffset;
+
+	Offset &= 0xff;
+
+	/*  */
+	/*  Switch page for 8256 RF IC */
+	/*  */
+	NewOffset = Offset;
+
+	/*  */
+	/*  Put write addr in [5:0]  and write data in [31:16] */
+	/*  */
+	DataAndAddr = ((NewOffset<<20) | (Data&0x000fffff)) & 0x0fffffff;	/*  T65 RF */
+	/*  */
+	/*  Write Operation */
+	/*  */
+	PHY_SetBBReg(Adapter, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
+}
+
+
+/**
+ * PHY_QueryRFReg_8723B - Query "Specific bits" to RF register (page 8~).
+ * @Adapter:
+ * @eRFPath:	Radio path of A/B/C/D
+ * @RegAddr:	The target address to be read
+ * @BitMask:	The target bit position in the target address
+ *				to be read
+ *
+ * Return:	Readback value
+ *
+ * .. Note::	This function is equal to "GetRFRegSetting" in PHY
+ *			programming guide
+ */
+u32 PHY_QueryRFReg_8723B(
+	struct adapter *Adapter,
+	u8 eRFPath,
+	u32 RegAddr,
+	u32 BitMask
+)
+{
+	u32 Original_Value, BitShift;
+
+	Original_Value = phy_RFSerialRead_8723B(Adapter, eRFPath, RegAddr);
+	BitShift =  phy_CalculateBitShift(BitMask);
+
+	return (Original_Value & BitMask) >> BitShift;
+}
+
+/**
+ * PHY_SetRFReg_8723B - Write "Specific bits" to RF register (page 8~).
+ * @Adapter:
+ * @eRFPath:	Radio path of A/B/C/D
+ * @RegAddr:	The target address to be modified
+ * @BitMask:	The target bit position in the target address
+ *				to be modified
+ * @Data:	The new register Data in the target bit position
+ *								of the target address
+ *
+ * .. Note::	This function is equal to "PutRFRegSetting" in PHY
+ *			programming guide.
+ */
+void PHY_SetRFReg_8723B(
+	struct adapter *Adapter,
+	u8 eRFPath,
+	u32 RegAddr,
+	u32 BitMask,
+	u32 Data
+)
+{
+	u32 Original_Value, BitShift;
+
+	/*  RF data is 12 bits only */
+	if (BitMask != bRFRegOffsetMask) {
+		Original_Value = phy_RFSerialRead_8723B(Adapter, eRFPath, RegAddr);
+		BitShift =  phy_CalculateBitShift(BitMask);
+		Data = ((Original_Value & (~BitMask)) | (Data<<BitShift));
+	}
+
+	phy_RFSerialWrite_8723B(Adapter, eRFPath, RegAddr, Data);
+}
+
+
+/*  */
+/*  3. Initial MAC/BB/RF config by reading MAC/BB/RF txt. */
+/*  */
+
+
+/*-----------------------------------------------------------------------------
+ * PHY_MACConfig8192C - Condig MAC by header file or parameter file.
+ *
+ * Revised History:
+ *  When		Who		Remark
+ *  08/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------
+ */
+s32 PHY_MACConfig8723B(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	ODM_ReadAndConfig_MP_8723B_MAC_REG(&pHalData->odmpriv);
+	return _SUCCESS;
+}
+
+/**
+ * phy_InitBBRFRegisterDefinition - Initialize Register definition offset for
+ *									Radio Path A/B/C/D
+ * @Adapter:
+ *
+ * .. Note::		The initialization value is constant and it should never be changes
+ */
+static void phy_InitBBRFRegisterDefinition(struct adapter *Adapter)
+{
+	struct hal_com_data		*pHalData = GET_HAL_DATA(Adapter);
+
+	/*  RF Interface Sowrtware Control */
+	pHalData->PHYRegDef[RF_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 LSBs if read 32-bit from 0x870 */
+	pHalData->PHYRegDef[RF_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 MSBs if read 32-bit from 0x870 (16-bit for 0x872) */
+
+	/*  RF Interface Output (and Enable) */
+	pHalData->PHYRegDef[RF_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x860 */
+	pHalData->PHYRegDef[RF_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x864 */
+
+	/*  RF Interface (Output and)  Enable */
+	pHalData->PHYRegDef[RF_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x860 (16-bit for 0x862) */
+	pHalData->PHYRegDef[RF_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x864 (16-bit for 0x866) */
+
+	pHalData->PHYRegDef[RF_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter; /* LSSI Parameter */
+	pHalData->PHYRegDef[RF_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
+
+	pHalData->PHYRegDef[RF_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;  /* wire control parameter2 */
+	pHalData->PHYRegDef[RF_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;  /* wire control parameter2 */
+
+	/*  Tranceiver Readback LSSI/HSPI mode */
+	pHalData->PHYRegDef[RF_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	pHalData->PHYRegDef[RF_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	pHalData->PHYRegDef[RF_PATH_A].rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
+	pHalData->PHYRegDef[RF_PATH_B].rfLSSIReadBackPi = TransceiverB_HSPI_Readback;
+
+}
+
+static int phy_BB8723b_Config_ParaFile(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	/*  Read Tx Power Limit File */
+	PHY_InitTxPowerLimit(Adapter);
+	if (
+		Adapter->registrypriv.RegEnableTxPowerLimit == 1 ||
+		(Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory == 1)
+	) {
+		ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv,
+					   CONFIG_RF_TXPWR_LMT, 0);
+	}
+
+	/*  */
+	/*  1. Read PHY_REG.TXT BB INIT!! */
+	/*  */
+	ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG);
+
+	/*  If EEPROM or EFUSE autoload OK, We must config by PHY_REG_PG.txt */
+	PHY_InitTxPowerByRate(Adapter);
+	if (
+		Adapter->registrypriv.RegEnableTxPowerByRate == 1 ||
+		(Adapter->registrypriv.RegEnableTxPowerByRate == 2 && pHalData->EEPROMRegulatory != 2)
+	) {
+		ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv,
+					   CONFIG_BB_PHY_REG_PG);
+
+		if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE)
+			PHY_TxPowerByRateConfiguration(Adapter);
+
+		if (
+			Adapter->registrypriv.RegEnableTxPowerLimit == 1 ||
+			(Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory == 1)
+		)
+			PHY_ConvertTxPowerLimitToPowerIndex(Adapter);
+	}
+
+	/*  */
+	/*  2. Read BB AGC table Initialization */
+	/*  */
+	ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_AGC_TAB);
+
+	return _SUCCESS;
+}
+
+
+int PHY_BBConfig8723B(struct adapter *Adapter)
+{
+	int	rtStatus = _SUCCESS;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u32 RegVal;
+	u8 CrystalCap;
+
+	phy_InitBBRFRegisterDefinition(Adapter);
+
+	/*  Enable BB and RF */
+	RegVal = rtw_read16(Adapter, REG_SYS_FUNC_EN);
+	rtw_write16(Adapter, REG_SYS_FUNC_EN, (u16)(RegVal|BIT13|BIT0|BIT1));
+
+	rtw_write32(Adapter, 0x948, 0x280);	/*  Others use Antenna S1 */
+
+	rtw_write8(Adapter, REG_RF_CTRL, RF_EN|RF_RSTB|RF_SDMRSTB);
+
+	msleep(1);
+
+	PHY_SetRFReg(Adapter, RF_PATH_A, 0x1, 0xfffff, 0x780);
+
+	rtw_write8(Adapter, REG_SYS_FUNC_EN, FEN_PPLL|FEN_PCIEA|FEN_DIO_PCIE|FEN_BB_GLB_RSTn|FEN_BBRSTB);
+
+	rtw_write8(Adapter, REG_AFE_XTAL_CTRL+1, 0x80);
+
+	/*  */
+	/*  Config BB and AGC */
+	/*  */
+	rtStatus = phy_BB8723b_Config_ParaFile(Adapter);
+
+	/*  0x2C[23:18] = 0x2C[17:12] = CrystalCap */
+	CrystalCap = pHalData->CrystalCap & 0x3F;
+	PHY_SetBBReg(Adapter, REG_MAC_PHY_CTRL, 0xFFF000, (CrystalCap | (CrystalCap << 6)));
+
+	return rtStatus;
+}
+
+static void phy_LCK_8723B(struct adapter *Adapter)
+{
+	PHY_SetRFReg(Adapter, RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFBE0);
+	PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, 0x8C01);
+	mdelay(200);
+	PHY_SetRFReg(Adapter, RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFFE0);
+}
+
+int PHY_RFConfig8723B(struct adapter *Adapter)
+{
+	int rtStatus = _SUCCESS;
+
+	/*  */
+	/*  RF config */
+	/*  */
+	rtStatus = PHY_RF6052_Config8723B(Adapter);
+
+	phy_LCK_8723B(Adapter);
+
+	return rtStatus;
+}
+
+/**************************************************************************************************************
+ *   Description:
+ *       The low-level interface to set TxAGC , called by both MP and Normal Driver.
+ *
+ *                                                                                    <20120830, Kordan>
+ **************************************************************************************************************/
+
+void PHY_SetTxPowerIndex(
+	struct adapter *Adapter,
+	u32 PowerIndex,
+	u8 RFPath,
+	u8 Rate
+)
+{
+	if (RFPath == RF_PATH_A || RFPath == RF_PATH_B) {
+		switch (Rate) {
+		case MGN_1M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_CCK1_Mcs32, bMaskByte1, PowerIndex);
+			break;
+		case MGN_2M:
+			PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte1, PowerIndex);
+			break;
+		case MGN_5_5M:
+			PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte2, PowerIndex);
+			break;
+		case MGN_11M:
+			PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte3, PowerIndex);
+			break;
+
+		case MGN_6M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate18_06, bMaskByte0, PowerIndex);
+			break;
+		case MGN_9M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate18_06, bMaskByte1, PowerIndex);
+			break;
+		case MGN_12M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate18_06, bMaskByte2, PowerIndex);
+			break;
+		case MGN_18M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate18_06, bMaskByte3, PowerIndex);
+			break;
+
+		case MGN_24M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate54_24, bMaskByte0, PowerIndex);
+			break;
+		case MGN_36M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate54_24, bMaskByte1, PowerIndex);
+			break;
+		case MGN_48M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate54_24, bMaskByte2, PowerIndex);
+			break;
+		case MGN_54M:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Rate54_24, bMaskByte3, PowerIndex);
+			break;
+
+		case MGN_MCS0:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs03_Mcs00, bMaskByte0, PowerIndex);
+			break;
+		case MGN_MCS1:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs03_Mcs00, bMaskByte1, PowerIndex);
+			break;
+		case MGN_MCS2:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs03_Mcs00, bMaskByte2, PowerIndex);
+			break;
+		case MGN_MCS3:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs03_Mcs00, bMaskByte3, PowerIndex);
+			break;
+
+		case MGN_MCS4:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs07_Mcs04, bMaskByte0, PowerIndex);
+			break;
+		case MGN_MCS5:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs07_Mcs04, bMaskByte1, PowerIndex);
+			break;
+		case MGN_MCS6:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs07_Mcs04, bMaskByte2, PowerIndex);
+			break;
+		case MGN_MCS7:
+			PHY_SetBBReg(Adapter, rTxAGC_A_Mcs07_Mcs04, bMaskByte3, PowerIndex);
+			break;
+
+		default:
+			break;
+		}
+	}
+}
+
+u8 PHY_GetTxPowerIndex(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum channel_width BandWidth,
+	u8 Channel
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	s8 txPower = 0, powerDiffByRate = 0, limit = 0;
+
+	txPower = (s8) PHY_GetTxPowerIndexBase(padapter, RFPath, Rate, BandWidth, Channel);
+	powerDiffByRate = PHY_GetTxPowerByRate(padapter, RF_PATH_A, Rate);
+
+	limit = phy_get_tx_pwr_lmt(
+		padapter,
+		padapter->registrypriv.RegPwrTblSel,
+		pHalData->CurrentChannelBW,
+		RFPath,
+		Rate,
+		pHalData->CurrentChannel
+	);
+
+	powerDiffByRate = powerDiffByRate > limit ? limit : powerDiffByRate;
+	txPower += powerDiffByRate;
+
+	txPower += PHY_GetTxPowerTrackingOffset(padapter, RFPath, Rate);
+
+	if (txPower > MAX_POWER_INDEX)
+		txPower = MAX_POWER_INDEX;
+
+	return (u8) txPower;
+}
+
+void PHY_SetTxPowerLevel8723B(struct adapter *Adapter, u8 Channel)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
+	struct fat_t *pDM_FatTable = &pDM_Odm->DM_FatTable;
+	u8 RFPath = RF_PATH_A;
+
+	if (pHalData->AntDivCfg) {/*  antenna diversity Enable */
+		RFPath = ((pDM_FatTable->RxIdleAnt == MAIN_ANT) ? RF_PATH_A : RF_PATH_B);
+	} else { /*  antenna diversity disable */
+		RFPath = pHalData->ant_path;
+	}
+
+	PHY_SetTxPowerLevelByPath(Adapter, Channel, RFPath);
+}
+
+void PHY_GetTxPowerLevel8723B(struct adapter *Adapter, s32 *powerlevel)
+{
+}
+
+static void phy_SetRegBW_8723B(
+	struct adapter *Adapter, enum channel_width CurrentBW
+)
+{
+	u16 RegRfMod_BW, u2tmp = 0;
+	RegRfMod_BW = rtw_read16(Adapter, REG_TRXPTCL_CTL_8723B);
+
+	switch (CurrentBW) {
+	case CHANNEL_WIDTH_20:
+		rtw_write16(Adapter, REG_TRXPTCL_CTL_8723B, (RegRfMod_BW & 0xFE7F)); /*  BIT 7 = 0, BIT 8 = 0 */
+		break;
+
+	case CHANNEL_WIDTH_40:
+		u2tmp = RegRfMod_BW | BIT7;
+		rtw_write16(Adapter, REG_TRXPTCL_CTL_8723B, (u2tmp & 0xFEFF)); /*  BIT 7 = 1, BIT 8 = 0 */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static u8 phy_GetSecondaryChnl_8723B(struct adapter *Adapter)
+{
+	u8 SCSettingOf40 = 0, SCSettingOf20 = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+			SCSettingOf20 = HT_DATA_SC_20_UPPER_OF_40MHZ;
+		else if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+			SCSettingOf20 = HT_DATA_SC_20_LOWER_OF_40MHZ;
+	}
+
+	return  (SCSettingOf40 << 4) | SCSettingOf20;
+}
+
+static void phy_PostSetBwMode8723B(struct adapter *Adapter)
+{
+	u8 SubChnlNum = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+
+	/* 3 Set Reg668 Reg440 BW */
+	phy_SetRegBW_8723B(Adapter, pHalData->CurrentChannelBW);
+
+	/* 3 Set Reg483 */
+	SubChnlNum = phy_GetSecondaryChnl_8723B(Adapter);
+	rtw_write8(Adapter, REG_DATA_SC_8723B, SubChnlNum);
+
+	/* 3 */
+	/* 3<2>Set PHY related register */
+	/* 3 */
+	switch (pHalData->CurrentChannelBW) {
+	/* 20 MHz channel*/
+	case CHANNEL_WIDTH_20:
+		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x0);
+
+		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x0);
+
+		PHY_SetBBReg(Adapter, rOFDM0_TxPseudoNoiseWgt, (BIT31|BIT30), 0x0);
+		break;
+
+	/* 40 MHz channel*/
+	case CHANNEL_WIDTH_40:
+		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x1);
+
+		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x1);
+
+		/*  Set Control channel to upper or lower. These settings are required only for 40MHz */
+		PHY_SetBBReg(Adapter, rCCK0_System, bCCKSideBand, (pHalData->nCur40MhzPrimeSC>>1));
+
+		PHY_SetBBReg(Adapter, rOFDM1_LSTF, 0xC00, pHalData->nCur40MhzPrimeSC);
+
+		PHY_SetBBReg(Adapter, 0x818, (BIT26|BIT27), (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		break;
+	}
+
+	/* 3<3>Set RF related register */
+	PHY_RF6052SetBandwidth8723B(Adapter, pHalData->CurrentChannelBW);
+}
+
+static void phy_SwChnl8723B(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 channelToSW = pHalData->CurrentChannel;
+
+	if (pHalData->rf_chip == RF_PSEUDO_11N)
+		return;
+	pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff00) | channelToSW);
+	PHY_SetRFReg(padapter, RF_PATH_A, RF_CHNLBW, 0x3FF, pHalData->RfRegChnlVal[0]);
+	PHY_SetRFReg(padapter, RF_PATH_B, RF_CHNLBW, 0x3FF, pHalData->RfRegChnlVal[0]);
+}
+
+static void phy_SwChnlAndSetBwMode8723B(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	if (Adapter->bDriverStopped || Adapter->bSurpriseRemoved)
+		return;
+
+	if (pHalData->bSwChnl) {
+		phy_SwChnl8723B(Adapter);
+		pHalData->bSwChnl = false;
+	}
+
+	if (pHalData->bSetChnlBW) {
+		phy_PostSetBwMode8723B(Adapter);
+		pHalData->bSetChnlBW = false;
+	}
+
+	PHY_SetTxPowerLevel8723B(Adapter, pHalData->CurrentChannel);
+}
+
+static void PHY_HandleSwChnlAndSetBW8723B(
+	struct adapter *Adapter,
+	bool bSwitchChannel,
+	bool bSetBandWidth,
+	u8 ChannelNum,
+	enum channel_width ChnlWidth,
+	enum extchnl_offset ExtChnlOffsetOf40MHz,
+	enum extchnl_offset ExtChnlOffsetOf80MHz,
+	u8 CenterFrequencyIndex1
+)
+{
+	/* static bool		bInitialzed = false; */
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u8 tmpChannel = pHalData->CurrentChannel;
+	enum channel_width tmpBW = pHalData->CurrentChannelBW;
+	u8 tmpnCur40MhzPrimeSC = pHalData->nCur40MhzPrimeSC;
+	u8 tmpnCur80MhzPrimeSC = pHalData->nCur80MhzPrimeSC;
+	u8 tmpCenterFrequencyIndex1 = pHalData->CurrentCenterFrequencyIndex1;
+
+	/* check is swchnl or setbw */
+	if (!bSwitchChannel && !bSetBandWidth)
+		return;
+
+	/* skip change for channel or bandwidth is the same */
+	if (bSwitchChannel) {
+		{
+			if (HAL_IsLegalChannel(Adapter, ChannelNum))
+				pHalData->bSwChnl = true;
+		}
+	}
+
+	if (bSetBandWidth)
+		pHalData->bSetChnlBW = true;
+
+	if (!pHalData->bSetChnlBW && !pHalData->bSwChnl)
+		return;
+
+
+	if (pHalData->bSwChnl) {
+		pHalData->CurrentChannel = ChannelNum;
+		pHalData->CurrentCenterFrequencyIndex1 = ChannelNum;
+	}
+
+
+	if (pHalData->bSetChnlBW) {
+		pHalData->CurrentChannelBW = ChnlWidth;
+		pHalData->nCur40MhzPrimeSC = ExtChnlOffsetOf40MHz;
+		pHalData->nCur80MhzPrimeSC = ExtChnlOffsetOf80MHz;
+		pHalData->CurrentCenterFrequencyIndex1 = CenterFrequencyIndex1;
+	}
+
+	/* Switch workitem or set timer to do switch channel or setbandwidth operation */
+	if ((!Adapter->bDriverStopped) && (!Adapter->bSurpriseRemoved)) {
+		phy_SwChnlAndSetBwMode8723B(Adapter);
+	} else {
+		if (pHalData->bSwChnl) {
+			pHalData->CurrentChannel = tmpChannel;
+			pHalData->CurrentCenterFrequencyIndex1 = tmpChannel;
+		}
+
+		if (pHalData->bSetChnlBW) {
+			pHalData->CurrentChannelBW = tmpBW;
+			pHalData->nCur40MhzPrimeSC = tmpnCur40MhzPrimeSC;
+			pHalData->nCur80MhzPrimeSC = tmpnCur80MhzPrimeSC;
+			pHalData->CurrentCenterFrequencyIndex1 = tmpCenterFrequencyIndex1;
+		}
+	}
+}
+
+void PHY_SetBWMode8723B(
+	struct adapter *Adapter,
+	enum channel_width Bandwidth, /*  20M or 40M */
+	unsigned char Offset /*  Upper, Lower, or Don't care */
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	PHY_HandleSwChnlAndSetBW8723B(Adapter, false, true, pHalData->CurrentChannel, Bandwidth, Offset, Offset, pHalData->CurrentChannel);
+}
+
+/*  Call after initialization */
+void PHY_SwChnl8723B(struct adapter *Adapter, u8 channel)
+{
+	PHY_HandleSwChnlAndSetBW8723B(Adapter, true, false, channel, 0, 0, 0, channel);
+}
+
+void PHY_SetSwChnlBWMode8723B(
+	struct adapter *Adapter,
+	u8 channel,
+	enum channel_width Bandwidth,
+	u8 Offset40,
+	u8 Offset80
+)
+{
+	PHY_HandleSwChnlAndSetBW8723B(Adapter, true, true, channel, Bandwidth, Offset40, Offset80, channel);
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
new file mode 100644
index 000000000..ffb35e1ac
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
@@ -0,0 +1,168 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/******************************************************************************
+ *
+ *
+ * Module:	rtl8192c_rf6052.c	(Source C File)
+ *
+ * Note:	Provide RF 6052 series relative API.
+ *
+ * Function:
+ *
+ * Export:
+ *
+ * Abbrev:
+ *
+ * History:
+ * Data			Who		Remark
+ *
+ * 09/25/2008	MHC		Create initial version.
+ * 11/05/2008	MHC		Add API for tw power setting.
+ *
+ *
+******************************************************************************/
+
+#include <rtl8723b_hal.h>
+
+/*---------------------------Define Local Constant---------------------------*/
+/*---------------------------Define Local Constant---------------------------*/
+
+
+/*------------------------Define global variable-----------------------------*/
+/*------------------------Define global variable-----------------------------*/
+
+
+/*------------------------Define local variable------------------------------*/
+/*  2008/11/20 MH For Debug only, RF */
+/*------------------------Define local variable------------------------------*/
+
+/*-----------------------------------------------------------------------------
+ * Function:    PHY_RF6052SetBandwidth()
+ *
+ * Overview:    This function is called by SetBWModeCallback8190Pci() only
+ *
+ * Input:       struct adapter *			Adapter
+ *		WIRELESS_BANDWIDTH_E	Bandwidth	20M or 40M
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Note:		For RF type 0222D
+ *---------------------------------------------------------------------------*/
+void PHY_RF6052SetBandwidth8723B(
+	struct adapter *Adapter, enum channel_width Bandwidth
+) /* 20M or 40M */
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	switch (Bandwidth) {
+	case CHANNEL_WIDTH_20:
+		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff) | BIT10 | BIT11);
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		PHY_SetRFReg(Adapter, RF_PATH_B, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		break;
+
+	case CHANNEL_WIDTH_40:
+		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff) | BIT10);
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		PHY_SetRFReg(Adapter, RF_PATH_B, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		break;
+
+	default:
+		break;
+	}
+
+}
+
+static int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
+{
+	u32 u4RegValue = 0;
+	u8 eRFPath;
+	struct bb_register_def *pPhyReg;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	/* 3----------------------------------------------------------------- */
+	/* 3 <2> Initialize RF */
+	/* 3----------------------------------------------------------------- */
+	/* for (eRFPath = RF_PATH_A; eRFPath <pHalData->NumTotalRFPath; eRFPath++) */
+	for (eRFPath = 0; eRFPath < pHalData->NumTotalRFPath; eRFPath++) {
+
+		pPhyReg = &pHalData->PHYRegDef[eRFPath];
+
+		/*----Store original RFENV control type----*/
+		switch (eRFPath) {
+		case RF_PATH_A:
+			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV);
+			break;
+		case RF_PATH_B:
+			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV << 16);
+			break;
+		}
+
+		/*----Set RF_ENV enable----*/
+		PHY_SetBBReg(Adapter, pPhyReg->rfintfe, bRFSI_RFENV << 16, 0x1);
+		udelay(1);/* PlatformStallExecution(1); */
+
+		/*----Set RF_ENV output high----*/
+		PHY_SetBBReg(Adapter, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
+		udelay(1);/* PlatformStallExecution(1); */
+
+		/* Set bit number of Address and Data for RF register */
+		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/*  Set 1 to 4 bits for 8255 */
+		udelay(1);/* PlatformStallExecution(1); */
+
+		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/*  Set 0 to 12  bits for 8255 */
+		udelay(1);/* PlatformStallExecution(1); */
+
+		/*----Initialize RF fom connfiguration file----*/
+		switch (eRFPath) {
+		case RF_PATH_A:
+		case RF_PATH_B:
+			ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv,
+						   CONFIG_RF_RADIO, eRFPath);
+			break;
+		}
+
+		/*----Restore RFENV control type----*/
+		switch (eRFPath) {
+		case RF_PATH_A:
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
+			break;
+		case RF_PATH_B:
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV << 16, u4RegValue);
+			break;
+		}
+	}
+
+	/* 3 ----------------------------------------------------------------- */
+	/* 3 Configuration of Tx Power Tracking */
+	/* 3 ----------------------------------------------------------------- */
+
+	ODM_ConfigRFWithTxPwrTrackHeaderFile(&pHalData->odmpriv);
+
+	return _SUCCESS;
+}
+
+
+int PHY_RF6052_Config8723B(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	/*  */
+	/*  Initialize general global value */
+	/*  */
+	pHalData->NumTotalRFPath = 1;
+
+	/*  */
+	/*  Config BB and RF */
+	/*  */
+	return phy_RF6052_Config_ParaFile(Adapter);
+
+}
+
+/* End of HalRf6052.c */
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_rxdesc.c b/drivers/staging/rtl8723bs/hal/rtl8723b_rxdesc.c
new file mode 100644
index 000000000..717faebf8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_rxdesc.c
@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <rtl8723b_hal.h>
+
+static void process_rssi(struct adapter *padapter, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct signal_stat *signal_stat = &padapter->recvpriv.signal_strength_data;
+
+	/* if (pRfd->Status.bPacketToSelf || pRfd->Status.bPacketBeacon) */
+	{
+		if (signal_stat->update_req) {
+			signal_stat->total_num = 0;
+			signal_stat->total_val = 0;
+			signal_stat->update_req = 0;
+		}
+
+		signal_stat->total_num++;
+		signal_stat->total_val  += pattrib->phy_info.SignalStrength;
+		signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+	}
+
+} /*  Process_UI_RSSI_8192C */
+
+static void process_link_qual(struct adapter *padapter, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib;
+	struct signal_stat *signal_stat;
+
+	if (!prframe || !padapter)
+		return;
+
+	pattrib = &prframe->u.hdr.attrib;
+	signal_stat = &padapter->recvpriv.signal_qual_data;
+
+	if (signal_stat->update_req) {
+		signal_stat->total_num = 0;
+		signal_stat->total_val = 0;
+		signal_stat->update_req = 0;
+	}
+
+	signal_stat->total_num++;
+	signal_stat->total_val  += pattrib->phy_info.SignalQuality;
+	signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+} /*  Process_UiLinkQuality8192S */
+
+
+void rtl8723b_process_phy_info(struct adapter *padapter, void *prframe)
+{
+	union recv_frame *precvframe = prframe;
+	/*  */
+	/*  Check RSSI */
+	/*  */
+	process_rssi(padapter, precvframe);
+	/*  */
+	/*  Check PWDB. */
+	/*  */
+	/* process_PWDB(padapter, precvframe); */
+
+	/* UpdateRxSignalStatistics8192C(Adapter, pRfd); */
+	/*  */
+	/*  Check EVM */
+	/*  */
+	process_link_qual(padapter,  precvframe);
+	#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	rtw_store_phy_info(padapter, prframe);
+	#endif
+
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c b/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
new file mode 100644
index 000000000..74e75dc97
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
@@ -0,0 +1,479 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+static void initrecvbuf(struct recv_buf *precvbuf, struct adapter *padapter)
+{
+	INIT_LIST_HEAD(&precvbuf->list);
+	spin_lock_init(&precvbuf->recvbuf_lock);
+
+	precvbuf->adapter = padapter;
+}
+
+static void update_recvframe_attrib(struct adapter *padapter,
+				    union recv_frame *precvframe,
+				    struct recv_stat *prxstat)
+{
+	struct rx_pkt_attrib *pattrib;
+	struct recv_stat report;
+	struct rxreport_8723b *prxreport = (struct rxreport_8723b *)&report;
+
+	report.rxdw0 = prxstat->rxdw0;
+	report.rxdw1 = prxstat->rxdw1;
+	report.rxdw2 = prxstat->rxdw2;
+	report.rxdw3 = prxstat->rxdw3;
+	report.rxdw4 = prxstat->rxdw4;
+	report.rxdw5 = prxstat->rxdw5;
+
+	pattrib = &precvframe->u.hdr.attrib;
+	memset(pattrib, 0, sizeof(struct rx_pkt_attrib));
+
+	/*  update rx report to recv_frame attribute */
+	pattrib->pkt_rpt_type = prxreport->c2h_ind ? C2H_PACKET : NORMAL_RX;
+
+	if (pattrib->pkt_rpt_type == NORMAL_RX) {
+		/*  Normal rx packet */
+		/*  update rx report to recv_frame attribute */
+		pattrib->pkt_len = (u16)prxreport->pktlen;
+		pattrib->drvinfo_sz = (u8)(prxreport->drvinfosize << 3);
+		pattrib->physt = (u8)prxreport->physt;
+
+		pattrib->crc_err = (u8)prxreport->crc32;
+		pattrib->icv_err = (u8)prxreport->icverr;
+
+		pattrib->bdecrypted = (u8)(prxreport->swdec ? 0 : 1);
+		pattrib->encrypt = (u8)prxreport->security;
+
+		pattrib->qos = (u8)prxreport->qos;
+		pattrib->priority = (u8)prxreport->tid;
+
+		pattrib->amsdu = (u8)prxreport->amsdu;
+
+		pattrib->seq_num = (u16)prxreport->seq;
+		pattrib->frag_num = (u8)prxreport->frag;
+		pattrib->mfrag = (u8)prxreport->mf;
+		pattrib->mdata = (u8)prxreport->md;
+
+		pattrib->data_rate = (u8)prxreport->rx_rate;
+	} else {
+		pattrib->pkt_len = (u16)prxreport->pktlen;
+	}
+}
+
+/*
+ * Notice:
+ *Before calling this function,
+ *precvframe->u.hdr.rx_data should be ready!
+ */
+static void update_recvframe_phyinfo(union recv_frame *precvframe,
+				     struct phy_stat *pphy_status)
+{
+	struct adapter *padapter = precvframe->u.hdr.adapter;
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+	struct hal_com_data *p_hal_data = GET_HAL_DATA(padapter);
+	struct odm_phy_info *p_phy_info =
+		(struct odm_phy_info *)(&pattrib->phy_info);
+
+	u8 *wlanhdr = precvframe->u.hdr.rx_data;
+	u8 *my_bssid;
+	u8 *rx_bssid;
+	u8 *rx_ra;
+	u8 *my_hwaddr;
+	u8 *sa = NULL;
+
+	struct odm_packet_info pkt_info = {
+		.data_rate   = 0x00,
+		.station_id  = 0x00,
+		.bssid_match = false,
+		.to_self     = false,
+		.is_beacon   = false,
+	};
+
+	/* unsigned long		irqL; */
+	struct sta_priv *pstapriv;
+	struct sta_info *psta;
+
+	my_bssid = get_bssid(&padapter->mlmepriv);
+	rx_bssid = get_hdr_bssid(wlanhdr);
+	pkt_info.bssid_match = ((!IsFrameTypeCtrl(wlanhdr)) &&
+				!pattrib->icv_err && !pattrib->crc_err &&
+				ether_addr_equal(rx_bssid, my_bssid));
+
+	rx_ra = rtl8723bs_get_ra(wlanhdr);
+	my_hwaddr = myid(&padapter->eeprompriv);
+	pkt_info.to_self = pkt_info.bssid_match &&
+		ether_addr_equal(rx_ra, my_hwaddr);
+
+
+	pkt_info.is_beacon = pkt_info.bssid_match &&
+		(GetFrameSubType(wlanhdr) == WIFI_BEACON);
+
+	sa = get_ta(wlanhdr);
+
+	pkt_info.station_id = 0xFF;
+
+	pstapriv = &padapter->stapriv;
+	psta = rtw_get_stainfo(pstapriv, sa);
+	if (psta)
+		pkt_info.station_id = psta->mac_id;
+
+	pkt_info.data_rate = pattrib->data_rate;
+
+	/* rtl8723b_query_rx_phy_status(precvframe, pphy_status); */
+	/* spin_lock_bh(&p_hal_data->odm_stainfo_lock); */
+	odm_phy_status_query(&p_hal_data->odmpriv, p_phy_info,
+			   (u8 *)pphy_status, &(pkt_info));
+	if (psta)
+		psta->rssi = pattrib->phy_info.RecvSignalPower;
+	/* spin_unlock_bh(&p_hal_data->odm_stainfo_lock); */
+	precvframe->u.hdr.psta = NULL;
+	if (
+		pkt_info.bssid_match &&
+		(check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)
+	) {
+		if (psta) {
+			precvframe->u.hdr.psta = psta;
+			rtl8723b_process_phy_info(padapter, precvframe);
+		}
+	} else if (pkt_info.to_self || pkt_info.is_beacon) {
+		u32 adhoc_state = WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE;
+		if (check_fwstate(&padapter->mlmepriv, adhoc_state))
+			if (psta)
+				precvframe->u.hdr.psta = psta;
+		rtl8723b_process_phy_info(padapter, precvframe);
+	}
+}
+
+static void rtl8723bs_c2h_packet_handler(struct adapter *padapter,
+					 u8 *pbuf, u16 length)
+{
+	u8 *tmp = NULL;
+	u8 res = false;
+
+	if (length == 0)
+		return;
+
+	tmp = rtw_zmalloc(length);
+	if (!tmp)
+		return;
+
+	memcpy(tmp, pbuf, length);
+
+	res = rtw_c2h_packet_wk_cmd(padapter, tmp, length);
+
+	if (!res)
+		kfree(tmp);
+}
+
+static inline union recv_frame *try_alloc_recvframe(struct recv_priv *precvpriv,
+						    struct recv_buf *precvbuf)
+{
+	union recv_frame *precvframe;
+
+	precvframe = rtw_alloc_recvframe(&precvpriv->free_recv_queue);
+	if (!precvframe) {
+		rtw_enqueue_recvbuf_to_head(precvbuf,
+					    &precvpriv->recv_buf_pending_queue);
+
+		/*  The case of can't allocate recvframe should be temporary, */
+		/*  schedule again and hope recvframe is available next time. */
+		tasklet_schedule(&precvpriv->recv_tasklet);
+	}
+
+	return precvframe;
+}
+
+static inline bool rx_crc_err(struct recv_priv *precvpriv,
+			      struct hal_com_data *p_hal_data,
+			      struct rx_pkt_attrib *pattrib,
+			      union recv_frame *precvframe)
+{
+	/*  fix Hardware RX data error, drop whole recv_buffer */
+	if ((!(p_hal_data->ReceiveConfig & RCR_ACRC32)) && pattrib->crc_err) {
+		rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+		return true;
+	}
+
+	return false;
+}
+
+static inline bool pkt_exceeds_tail(struct recv_priv *precvpriv,
+				    u8 *end, u8 *tail,
+				    union recv_frame *precvframe)
+{
+	if (end > tail) {
+		rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+		return true;
+	}
+
+	return false;
+}
+
+static void rtl8723bs_recv_tasklet(struct tasklet_struct *t)
+{
+	struct adapter *padapter = from_tasklet(padapter, t,
+						recvpriv.recv_tasklet);
+	struct hal_com_data *p_hal_data;
+	struct recv_priv *precvpriv;
+	struct recv_buf *precvbuf;
+	union recv_frame *precvframe;
+	struct rx_pkt_attrib *pattrib;
+	struct __queue *recv_buf_queue;
+	u8 *ptr;
+	u32 pkt_offset, skb_len, alloc_sz;
+	struct sk_buff *pkt_copy = NULL;
+	u8 shift_sz = 0, rx_report_sz = 0;
+
+	p_hal_data = GET_HAL_DATA(padapter);
+	precvpriv = &padapter->recvpriv;
+	recv_buf_queue = &precvpriv->recv_buf_pending_queue;
+
+	do {
+		precvbuf = rtw_dequeue_recvbuf(recv_buf_queue);
+		if (!precvbuf)
+			break;
+
+		ptr = precvbuf->pdata;
+
+		while (ptr < precvbuf->ptail) {
+			precvframe = try_alloc_recvframe(precvpriv, precvbuf);
+			if (!precvframe)
+				return;
+
+			/* rx desc parsing */
+			update_recvframe_attrib(padapter, precvframe,
+						(struct recv_stat *)ptr);
+
+			pattrib = &precvframe->u.hdr.attrib;
+
+			if (rx_crc_err(precvpriv, p_hal_data,
+				       pattrib, precvframe))
+				break;
+
+			rx_report_sz = RXDESC_SIZE + pattrib->drvinfo_sz;
+			pkt_offset = rx_report_sz +
+				pattrib->shift_sz +
+				pattrib->pkt_len;
+
+			if (pkt_exceeds_tail(precvpriv, ptr + pkt_offset,
+					     precvbuf->ptail, precvframe))
+				break;
+
+			if ((pattrib->crc_err) || (pattrib->icv_err)) {
+				rtw_free_recvframe(precvframe,
+						   &precvpriv->free_recv_queue);
+			} else {
+				/* 	Modified by Albert 20101213 */
+				/* 	For 8 bytes IP header alignment. */
+				if (pattrib->qos)	/* 	Qos data, wireless lan header length is 26 */
+					shift_sz = 6;
+				else
+					shift_sz = 0;
+
+				skb_len = pattrib->pkt_len;
+
+				/*  for first fragment packet, driver need allocate 1536+drvinfo_sz+RXDESC_SIZE to defrag packet. */
+				/*  modify alloc_sz for recvive crc error packet by thomas 2011-06-02 */
+				if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+					if (skb_len <= 1650)
+						alloc_sz = 1664;
+					else
+						alloc_sz = skb_len + 14;
+				} else {
+					alloc_sz = skb_len;
+					/* 	6 is for IP header 8 bytes alignment in QoS packet case. */
+					/* 	8 is for skb->data 4 bytes alignment. */
+					alloc_sz += 14;
+				}
+
+				pkt_copy = rtw_skb_alloc(alloc_sz);
+				if (!pkt_copy) {
+					rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+					break;
+				}
+
+				pkt_copy->dev = padapter->pnetdev;
+				precvframe->u.hdr.pkt = pkt_copy;
+				skb_reserve(pkt_copy, 8 - ((SIZE_PTR)(pkt_copy->data) & 7));/* force pkt_copy->data at 8-byte alignment address */
+				skb_reserve(pkt_copy, shift_sz);/* force ip_hdr at 8-byte alignment address according to shift_sz. */
+				memcpy(pkt_copy->data, (ptr + rx_report_sz + pattrib->shift_sz), skb_len);
+				precvframe->u.hdr.rx_head = pkt_copy->head;
+				precvframe->u.hdr.rx_data = precvframe->u.hdr.rx_tail = pkt_copy->data;
+				precvframe->u.hdr.rx_end = skb_end_pointer(pkt_copy);
+
+				recvframe_put(precvframe, skb_len);
+				/* recvframe_pull(precvframe, drvinfo_sz + RXDESC_SIZE); */
+
+				if (p_hal_data->ReceiveConfig & RCR_APPFCS)
+					recvframe_pull_tail(precvframe, IEEE80211_FCS_LEN);
+
+				/*  move to drv info position */
+				ptr += RXDESC_SIZE;
+
+				/*  update drv info */
+				if (p_hal_data->ReceiveConfig & RCR_APP_BA_SSN) {
+					/* rtl8723s_update_bassn(padapter, pdrvinfo); */
+					ptr += 4;
+				}
+
+				if (pattrib->pkt_rpt_type == NORMAL_RX) { /* Normal rx packet */
+					if (pattrib->physt)
+						update_recvframe_phyinfo(precvframe, (struct phy_stat *)ptr);
+
+					rtw_recv_entry(precvframe);
+				} else if (pattrib->pkt_rpt_type == C2H_PACKET) {
+					struct c2h_evt_hdr_t	C2hEvent;
+
+					u16 len_c2h = pattrib->pkt_len;
+					u8 *pbuf_c2h = precvframe->u.hdr.rx_data;
+					u8 *pdata_c2h;
+
+					C2hEvent.CmdID = pbuf_c2h[0];
+					C2hEvent.CmdSeq = pbuf_c2h[1];
+					C2hEvent.CmdLen = (len_c2h-2);
+					pdata_c2h = pbuf_c2h+2;
+
+					if (C2hEvent.CmdID == C2H_CCX_TX_RPT)
+						CCX_FwC2HTxRpt_8723b(padapter, pdata_c2h, C2hEvent.CmdLen);
+					else
+						rtl8723bs_c2h_packet_handler(padapter, precvframe->u.hdr.rx_data, pattrib->pkt_len);
+
+					rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+				}
+			}
+
+			pkt_offset = round_up(pkt_offset, 8);
+			precvbuf->pdata += pkt_offset;
+			ptr = precvbuf->pdata;
+			precvframe = NULL;
+			pkt_copy = NULL;
+		}
+
+		rtw_enqueue_recvbuf(precvbuf, &precvpriv->free_recv_buf_queue);
+	} while (1);
+}
+
+/*
+ * Initialize recv private variable for hardware dependent
+ * 1. recv buf
+ * 2. recv tasklet
+ *
+ */
+s32 rtl8723bs_init_recv_priv(struct adapter *padapter)
+{
+	s32 res;
+	u32 i, n;
+	struct recv_priv *precvpriv;
+	struct recv_buf *precvbuf;
+
+	res = _SUCCESS;
+	precvpriv = &padapter->recvpriv;
+
+	/* 3 1. init recv buffer */
+	INIT_LIST_HEAD(&precvpriv->free_recv_buf_queue.queue);
+	spin_lock_init(&precvpriv->free_recv_buf_queue.lock);
+	INIT_LIST_HEAD(&precvpriv->recv_buf_pending_queue.queue);
+	spin_lock_init(&precvpriv->recv_buf_pending_queue.lock);
+
+	n = NR_RECVBUFF * sizeof(struct recv_buf) + 4;
+	precvpriv->pallocated_recv_buf = rtw_zmalloc(n);
+	if (!precvpriv->pallocated_recv_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	precvpriv->precv_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_recv_buf), 4);
+
+	/*  init each recv buffer */
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	for (i = 0; i < NR_RECVBUFF; i++) {
+		initrecvbuf(precvbuf, padapter);
+
+		if (!precvbuf->pskb) {
+			SIZE_PTR tmpaddr = 0;
+			SIZE_PTR alignment = 0;
+
+			precvbuf->pskb = rtw_skb_alloc(MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
+
+			if (precvbuf->pskb) {
+				precvbuf->pskb->dev = padapter->pnetdev;
+
+				tmpaddr = (SIZE_PTR)precvbuf->pskb->data;
+				alignment = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
+				skb_reserve(precvbuf->pskb, (RECVBUFF_ALIGN_SZ - alignment));
+			}
+		}
+
+		list_add_tail(&precvbuf->list, &precvpriv->free_recv_buf_queue.queue);
+
+		precvbuf++;
+	}
+	precvpriv->free_recv_buf_queue_cnt = i;
+
+	if (res == _FAIL)
+		goto initbuferror;
+
+	/* 3 2. init tasklet */
+	tasklet_setup(&precvpriv->recv_tasklet, rtl8723bs_recv_tasklet);
+
+	goto exit;
+
+initbuferror:
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	if (precvbuf) {
+		n = precvpriv->free_recv_buf_queue_cnt;
+		precvpriv->free_recv_buf_queue_cnt = 0;
+		for (i = 0; i < n ; i++) {
+			list_del_init(&precvbuf->list);
+			rtw_os_recvbuf_resource_free(padapter, precvbuf);
+			precvbuf++;
+		}
+		precvpriv->precv_buf = NULL;
+	}
+
+	kfree(precvpriv->pallocated_recv_buf);
+	precvpriv->pallocated_recv_buf = NULL;
+
+exit:
+	return res;
+}
+
+/*
+ * Free recv private variable of hardware dependent
+ * 1. recv buf
+ * 2. recv tasklet
+ *
+ */
+void rtl8723bs_free_recv_priv(struct adapter *padapter)
+{
+	u32 i;
+	struct recv_priv *precvpriv;
+	struct recv_buf *precvbuf;
+
+	precvpriv = &padapter->recvpriv;
+
+	/* 3 1. kill tasklet */
+	tasklet_kill(&precvpriv->recv_tasklet);
+
+	/* 3 2. free all recv buffers */
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+	if (precvbuf) {
+		precvpriv->free_recv_buf_queue_cnt = 0;
+		for (i = 0; i < NR_RECVBUFF; i++) {
+			list_del_init(&precvbuf->list);
+			rtw_os_recvbuf_resource_free(padapter, precvbuf);
+			precvbuf++;
+		}
+		precvpriv->precv_buf = NULL;
+	}
+
+	kfree(precvpriv->pallocated_recv_buf);
+	precvpriv->pallocated_recv_buf = NULL;
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c b/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
new file mode 100644
index 000000000..15810438a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
@@ -0,0 +1,594 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+static u8 rtw_sdio_wait_enough_TxOQT_space(struct adapter *padapter, u8 agg_num)
+{
+	u32 n = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	while (pHalData->SdioTxOQTFreeSpace < agg_num) {
+		if (
+			(padapter->bSurpriseRemoved) ||
+			(padapter->bDriverStopped)
+		)
+			return false;
+
+		HalQueryTxOQTBufferStatus8723BSdio(padapter);
+
+		if ((++n % 60) == 0) {
+			msleep(1);
+			/* yield(); */
+		}
+	}
+
+	pHalData->SdioTxOQTFreeSpace -= agg_num;
+
+	return true;
+}
+
+static s32 rtl8723_dequeue_writeport(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	struct xmit_buf *pxmitbuf;
+	struct adapter *pri_padapter = padapter;
+	s32 ret = 0;
+	u8 PageIdx = 0;
+	u32 deviceId;
+	u8 bUpdatePageNum = false;
+
+	ret = ret || check_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	if (ret)
+		pxmitbuf = dequeue_pending_xmitbuf_under_survey(pxmitpriv);
+	else
+		pxmitbuf = dequeue_pending_xmitbuf(pxmitpriv);
+
+	if (!pxmitbuf)
+		return true;
+
+	deviceId = ffaddr2deviceId(pdvobjpriv, pxmitbuf->ff_hwaddr);
+
+	/*  translate fifo addr to queue index */
+	switch (deviceId) {
+	case WLAN_TX_HIQ_DEVICE_ID:
+		PageIdx = HI_QUEUE_IDX;
+		break;
+
+	case WLAN_TX_MIQ_DEVICE_ID:
+		PageIdx = MID_QUEUE_IDX;
+		break;
+
+	case WLAN_TX_LOQ_DEVICE_ID:
+		PageIdx = LOW_QUEUE_IDX;
+		break;
+	}
+
+query_free_page:
+	/*  check if hardware tx fifo page is enough */
+	if (!rtw_hal_sdio_query_tx_freepage(pri_padapter, PageIdx, pxmitbuf->pg_num)) {
+		if (!bUpdatePageNum) {
+			/*  Total number of page is NOT available, so update current FIFO status */
+			HalQueryTxBufferStatus8723BSdio(padapter);
+			bUpdatePageNum = true;
+			goto query_free_page;
+		} else {
+			bUpdatePageNum = false;
+			enqueue_pending_xmitbuf_to_head(pxmitpriv, pxmitbuf);
+			return true;
+		}
+	}
+
+	if (
+		(padapter->bSurpriseRemoved) ||
+		(padapter->bDriverStopped)
+	)
+		goto free_xmitbuf;
+
+	if (rtw_sdio_wait_enough_TxOQT_space(padapter, pxmitbuf->agg_num) == false)
+		goto free_xmitbuf;
+
+	traffic_check_for_leave_lps(padapter, true, pxmitbuf->agg_num);
+
+	rtw_write_port(padapter, deviceId, pxmitbuf->len, (u8 *)pxmitbuf);
+
+	rtw_hal_sdio_update_tx_freepage(pri_padapter, PageIdx, pxmitbuf->pg_num);
+
+free_xmitbuf:
+	/* rtw_free_xmitframe(pxmitpriv, pframe); */
+	/* pxmitbuf->priv_data = NULL; */
+	rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+
+	return _FAIL;
+}
+
+/*
+ * Description
+ *Transmit xmitbuf to hardware tx fifo
+ *
+ * Return
+ *_SUCCESS	ok
+ *_FAIL		something error
+ */
+s32 rtl8723bs_xmit_buf_handler(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv;
+	u8 queue_empty, queue_pending;
+	s32 ret;
+
+
+	pxmitpriv = &padapter->xmitpriv;
+
+	if (wait_for_completion_interruptible(&pxmitpriv->xmit_comp)) {
+		netdev_emerg(padapter->pnetdev,
+			     "%s: down SdioXmitBufSema fail!\n", __func__);
+		return _FAIL;
+	}
+
+	ret = (padapter->bDriverStopped) || (padapter->bSurpriseRemoved);
+	if (ret)
+		return _FAIL;
+
+	queue_pending = check_pending_xmitbuf(pxmitpriv);
+
+	if (!queue_pending)
+		return _SUCCESS;
+
+	ret = rtw_register_tx_alive(padapter);
+	if (ret != _SUCCESS) {
+		return _SUCCESS;
+	}
+
+	do {
+		queue_empty = rtl8723_dequeue_writeport(padapter);
+/* 	dump secondary adapter xmitbuf */
+	} while (!queue_empty);
+
+	rtw_unregister_tx_alive(padapter);
+
+	return _SUCCESS;
+}
+
+/*
+ * Description:
+ *Aggregation packets and send to hardware
+ *
+ * Return:
+ *0	Success
+ *-1	Hardware resource(TX FIFO) not ready
+ *-2	Software resource(xmitbuf) not ready
+ */
+static s32 xmit_xmitframes(struct adapter *padapter, struct xmit_priv *pxmitpriv)
+{
+	s32 err, ret;
+	u32 k = 0;
+	struct hw_xmit *hwxmits, *phwxmit;
+	u8 idx, hwentry;
+	struct tx_servq *ptxservq;
+	struct list_head *sta_plist, *sta_phead, *frame_plist, *frame_phead, *tmp;
+	struct xmit_frame *pxmitframe;
+	struct __queue *pframe_queue;
+	struct xmit_buf *pxmitbuf;
+	u32 txlen, max_xmit_len;
+	u8 txdesc_size = TXDESC_SIZE;
+	int inx[4];
+
+	err = 0;
+	hwxmits = pxmitpriv->hwxmits;
+	hwentry = pxmitpriv->hwxmit_entry;
+	ptxservq = NULL;
+	pxmitframe = NULL;
+	pframe_queue = NULL;
+	pxmitbuf = NULL;
+
+	if (padapter->registrypriv.wifi_spec == 1) {
+		for (idx = 0; idx < 4; idx++)
+			inx[idx] = pxmitpriv->wmm_para_seq[idx];
+	} else {
+		inx[0] = 0;
+		inx[1] = 1;
+		inx[2] = 2;
+		inx[3] = 3;
+	}
+
+	/*  0(VO), 1(VI), 2(BE), 3(BK) */
+	for (idx = 0; idx < hwentry; idx++) {
+		phwxmit = hwxmits + inx[idx];
+
+		if (
+			(check_pending_xmitbuf(pxmitpriv)) &&
+			(padapter->mlmepriv.LinkDetectInfo.bHigherBusyTxTraffic)
+		) {
+			if ((phwxmit->accnt > 0) && (phwxmit->accnt < 5)) {
+				err = -2;
+				break;
+			}
+		}
+
+		max_xmit_len = rtw_hal_get_sdio_tx_max_length(padapter, inx[idx]);
+
+		spin_lock_bh(&pxmitpriv->lock);
+
+		sta_phead = get_list_head(phwxmit->sta_queue);
+		/* because stop_sta_xmit may delete sta_plist at any time */
+		/* so we should add lock here, or while loop can not exit */
+		list_for_each_safe(sta_plist, tmp, sta_phead) {
+			ptxservq = list_entry(sta_plist, struct tx_servq,
+					      tx_pending);
+
+			pframe_queue = &ptxservq->sta_pending;
+
+			frame_phead = get_list_head(pframe_queue);
+
+			while (list_empty(frame_phead) == false) {
+				frame_plist = get_next(frame_phead);
+				pxmitframe = container_of(frame_plist, struct xmit_frame, list);
+
+				/*  check xmit_buf size enough or not */
+				txlen = txdesc_size + rtw_wlan_pkt_size(pxmitframe);
+				if (!pxmitbuf ||
+					((_RND(pxmitbuf->len, 8) + txlen) > max_xmit_len) ||
+					(k >= (rtw_hal_sdio_max_txoqt_free_space(padapter) - 1))
+				) {
+					if (pxmitbuf) {
+						/* pxmitbuf->priv_data will be NULL, and will crash here */
+						if (pxmitbuf->len > 0 &&
+						    pxmitbuf->priv_data) {
+							struct xmit_frame *pframe;
+							pframe = (struct xmit_frame *)pxmitbuf->priv_data;
+							pframe->agg_num = k;
+							pxmitbuf->agg_num = k;
+							rtl8723b_update_txdesc(pframe, pframe->buf_addr);
+							rtw_free_xmitframe(pxmitpriv, pframe);
+							pxmitbuf->priv_data = NULL;
+							enqueue_pending_xmitbuf(pxmitpriv, pxmitbuf);
+							/* can not yield under lock */
+							/* yield(); */
+						} else
+							rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+					}
+
+					pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+					if (!pxmitbuf) {
+#ifdef DBG_XMIT_BUF
+						netdev_err(padapter->pnetdev,
+							   "%s: xmit_buf is not enough!\n",
+							   __func__);
+#endif
+						err = -2;
+						complete(&(pxmitpriv->xmit_comp));
+						break;
+					}
+					k = 0;
+				}
+
+				/*  ok to send, remove frame from queue */
+				if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)
+					if (
+						(pxmitframe->attrib.psta->state & WIFI_SLEEP_STATE) &&
+						(pxmitframe->attrib.triggered == 0)
+					)
+						break;
+
+				list_del_init(&pxmitframe->list);
+				ptxservq->qcnt--;
+				phwxmit->accnt--;
+
+				if (k == 0) {
+					pxmitbuf->ff_hwaddr = rtw_get_ff_hwaddr(pxmitframe);
+					pxmitbuf->priv_data = (u8 *)pxmitframe;
+				}
+
+				/*  coalesce the xmitframe to xmitbuf */
+				pxmitframe->pxmitbuf = pxmitbuf;
+				pxmitframe->buf_addr = pxmitbuf->ptail;
+
+				ret = rtw_xmitframe_coalesce(padapter, pxmitframe->pkt, pxmitframe);
+				if (ret == _FAIL) {
+					netdev_err(padapter->pnetdev,
+						   "%s: coalesce FAIL!",
+						   __func__);
+					/*  Todo: error handler */
+				} else {
+					k++;
+					if (k != 1)
+						rtl8723b_update_txdesc(pxmitframe, pxmitframe->buf_addr);
+					rtw_count_tx_stats(padapter, pxmitframe, pxmitframe->attrib.last_txcmdsz);
+
+					txlen = txdesc_size + pxmitframe->attrib.last_txcmdsz;
+					pxmitframe->pg_num = (txlen + 127) / 128;
+					pxmitbuf->pg_num += (txlen + 127) / 128;
+					pxmitbuf->ptail += _RND(txlen, 8); /*  round to 8 bytes alignment */
+					pxmitbuf->len = _RND(pxmitbuf->len, 8) + txlen;
+				}
+
+				if (k != 1)
+					rtw_free_xmitframe(pxmitpriv, pxmitframe);
+				pxmitframe = NULL;
+			}
+
+			if (list_empty(&pframe_queue->queue))
+				list_del_init(&ptxservq->tx_pending);
+
+			if (err)
+				break;
+		}
+		spin_unlock_bh(&pxmitpriv->lock);
+
+		/*  dump xmit_buf to hw tx fifo */
+		if (pxmitbuf) {
+			if (pxmitbuf->len > 0) {
+				struct xmit_frame *pframe;
+				pframe = (struct xmit_frame *)pxmitbuf->priv_data;
+				pframe->agg_num = k;
+				pxmitbuf->agg_num = k;
+				rtl8723b_update_txdesc(pframe, pframe->buf_addr);
+				rtw_free_xmitframe(pxmitpriv, pframe);
+				pxmitbuf->priv_data = NULL;
+				enqueue_pending_xmitbuf(pxmitpriv, pxmitbuf);
+				yield();
+			} else
+				rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+			pxmitbuf = NULL;
+		}
+
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+/*
+ * Description
+ *Transmit xmitframe from queue
+ *
+ * Return
+ *_SUCCESS	ok
+ *_FAIL		something error
+ */
+static s32 rtl8723bs_xmit_handler(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv;
+	s32 ret;
+
+
+	pxmitpriv = &padapter->xmitpriv;
+
+	if (wait_for_completion_interruptible(&pxmitpriv->SdioXmitStart)) {
+		netdev_emerg(padapter->pnetdev, "%s: SdioXmitStart fail!\n",
+			     __func__);
+		return _FAIL;
+	}
+
+next:
+	if (
+		(padapter->bDriverStopped) ||
+		(padapter->bSurpriseRemoved)
+	)
+		return _FAIL;
+
+	spin_lock_bh(&pxmitpriv->lock);
+	ret = rtw_txframes_pending(padapter);
+	spin_unlock_bh(&pxmitpriv->lock);
+	if (ret == 0) {
+		return _SUCCESS;
+	}
+
+	/*  dequeue frame and write to hardware */
+
+	ret = xmit_xmitframes(padapter, pxmitpriv);
+	if (ret == -2) {
+		/* here sleep 1ms will cause big TP loss of TX */
+		/* from 50+ to 40+ */
+		if (padapter->registrypriv.wifi_spec)
+			msleep(1);
+		else
+			yield();
+		goto next;
+	}
+
+	spin_lock_bh(&pxmitpriv->lock);
+	ret = rtw_txframes_pending(padapter);
+	spin_unlock_bh(&pxmitpriv->lock);
+	if (ret == 1) {
+		goto next;
+	}
+
+	return _SUCCESS;
+}
+
+int rtl8723bs_xmit_thread(void *context)
+{
+	s32 ret;
+	struct adapter *padapter;
+	struct xmit_priv *pxmitpriv;
+	u8 thread_name[20];
+
+	ret = _SUCCESS;
+	padapter = context;
+	pxmitpriv = &padapter->xmitpriv;
+
+	rtw_sprintf(thread_name, 20, "RTWHALXT-%s", ADPT_ARG(padapter));
+	thread_enter(thread_name);
+
+	do {
+		ret = rtl8723bs_xmit_handler(padapter);
+		if (signal_pending(current)) {
+			flush_signals(current);
+		}
+	} while (_SUCCESS == ret);
+
+	complete(&pxmitpriv->SdioXmitTerminate);
+
+	return 0;
+}
+
+s32 rtl8723bs_mgnt_xmit(
+	struct adapter *padapter, struct xmit_frame *pmgntframe
+)
+{
+	s32 ret = _SUCCESS;
+	struct pkt_attrib *pattrib;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	u8 *pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	u8 txdesc_size = TXDESC_SIZE;
+
+	pattrib = &pmgntframe->attrib;
+	pxmitbuf = pmgntframe->pxmitbuf;
+
+	rtl8723b_update_txdesc(pmgntframe, pmgntframe->buf_addr);
+
+	pxmitbuf->len = txdesc_size + pattrib->last_txcmdsz;
+	pxmitbuf->pg_num = (pxmitbuf->len + 127) / 128; /*  128 is tx page size */
+	pxmitbuf->ptail = pmgntframe->buf_addr + pxmitbuf->len;
+	pxmitbuf->ff_hwaddr = rtw_get_ff_hwaddr(pmgntframe);
+
+	rtw_count_tx_stats(padapter, pmgntframe, pattrib->last_txcmdsz);
+
+	rtw_free_xmitframe(pxmitpriv, pmgntframe);
+
+	pxmitbuf->priv_data = NULL;
+
+	if (GetFrameSubType(pframe) == WIFI_BEACON) { /* dump beacon directly */
+		ret = rtw_write_port(padapter, pdvobjpriv->Queue2Pipe[pxmitbuf->ff_hwaddr], pxmitbuf->len, (u8 *)pxmitbuf);
+		if (ret != _SUCCESS)
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_WRITE_PORT_ERR);
+
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+	} else
+		enqueue_pending_xmitbuf(pxmitpriv, pxmitbuf);
+
+	return ret;
+}
+
+/*
+ * Description:
+ *Handle xmitframe(packet) come from rtw_xmit()
+ *
+ * Return:
+ *true	dump packet directly ok
+ *false	enqueue, temporary can't transmit packets to hardware
+ */
+s32 rtl8723bs_hal_xmit(
+	struct adapter *padapter, struct xmit_frame *pxmitframe
+)
+{
+	struct xmit_priv *pxmitpriv;
+	s32 err;
+
+
+	pxmitframe->attrib.qsel = pxmitframe->attrib.priority;
+	pxmitpriv = &padapter->xmitpriv;
+
+	if (
+		(pxmitframe->frame_tag == DATA_FRAMETAG) &&
+		(pxmitframe->attrib.ether_type != 0x0806) &&
+		(pxmitframe->attrib.ether_type != 0x888e) &&
+		(pxmitframe->attrib.dhcp_pkt != 1)
+	) {
+		if (padapter->mlmepriv.LinkDetectInfo.bBusyTraffic)
+			rtw_issue_addbareq_cmd(padapter, pxmitframe);
+	}
+
+	spin_lock_bh(&pxmitpriv->lock);
+	err = rtw_xmitframe_enqueue(padapter, pxmitframe);
+	spin_unlock_bh(&pxmitpriv->lock);
+	if (err != _SUCCESS) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		pxmitpriv->tx_drop++;
+		return true;
+	}
+
+	complete(&pxmitpriv->SdioXmitStart);
+
+	return false;
+}
+
+s32	rtl8723bs_hal_xmitframe_enqueue(
+	struct adapter *padapter, struct xmit_frame *pxmitframe
+)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	s32 err;
+
+	err = rtw_xmitframe_enqueue(padapter, pxmitframe);
+	if (err != _SUCCESS) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		pxmitpriv->tx_drop++;
+	} else {
+		complete(&pxmitpriv->SdioXmitStart);
+	}
+
+	return err;
+
+}
+
+/*
+ * Return
+ *_SUCCESS	start thread ok
+ *_FAIL		start thread fail
+ *
+ */
+s32 rtl8723bs_init_xmit_priv(struct adapter *padapter)
+{
+	struct xmit_priv *xmitpriv = &padapter->xmitpriv;
+	struct hal_com_data *phal;
+
+
+	phal = GET_HAL_DATA(padapter);
+
+	spin_lock_init(&phal->SdioTxFIFOFreePageLock);
+	init_completion(&xmitpriv->SdioXmitStart);
+	init_completion(&xmitpriv->SdioXmitTerminate);
+
+	return _SUCCESS;
+}
+
+void rtl8723bs_free_xmit_priv(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv;
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+	struct list_head *plist, *phead;
+	struct list_head tmplist;
+
+
+	pxmitpriv = &padapter->xmitpriv;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+	phead = get_list_head(pqueue);
+	INIT_LIST_HEAD(&tmplist);
+
+	spin_lock_bh(&pqueue->lock);
+	if (!list_empty(&pqueue->queue)) {
+		/*  Insert tmplist to end of queue, and delete phead */
+		/*  then tmplist become head of queue. */
+		list_add_tail(&tmplist, phead);
+		list_del_init(phead);
+	}
+	spin_unlock_bh(&pqueue->lock);
+
+	phead = &tmplist;
+	while (list_empty(phead) == false) {
+		plist = get_next(phead);
+		list_del_init(plist);
+
+		pxmitbuf = container_of(plist, struct xmit_buf, list);
+		rtw_free_xmitframe(pxmitpriv, (struct xmit_frame *)pxmitbuf->priv_data);
+		pxmitbuf->priv_data = NULL;
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/sdio_halinit.c b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
new file mode 100644
index 000000000..c9cd6578f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
@@ -0,0 +1,1301 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+#include "hal_com_h2c.h"
+/*
+ * Description:
+ *Call power on sequence to enable card
+ *
+ * Return:
+ *_SUCCESS	enable success
+ *_FAIL		enable fail
+ */
+static u8 CardEnable(struct adapter *padapter)
+{
+	u8 bMacPwrCtrlOn;
+	u8 ret = _FAIL;
+
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (!bMacPwrCtrlOn) {
+		/*  RSV_CTRL 0x1C[7:0] = 0x00 */
+		/*  unlock ISO/CLK/Power control register */
+		rtw_write8(padapter, REG_RSV_CTRL, 0x0);
+
+		ret = HalPwrSeqCmdParsing(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, rtl8723B_card_enable_flow);
+		if (ret == _SUCCESS) {
+			u8 bMacPwrCtrlOn = true;
+			rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+		}
+	} else
+		ret = _SUCCESS;
+
+	return ret;
+}
+
+static
+u8 _InitPowerOn_8723BS(struct adapter *padapter)
+{
+	u8 value8;
+	u16 value16;
+	u32 value32;
+	u8 ret;
+/* 	u8 bMacPwrCtrlOn; */
+
+
+	/*  all of these MUST be configured before power on */
+
+	/*  only cmd52 can be used before power on(card enable) */
+	ret = CardEnable(padapter);
+	if (!ret)
+		return _FAIL;
+
+	/*  Radio-Off Pin Trigger */
+	value8 = rtw_read8(padapter, REG_GPIO_INTM + 1);
+	value8 |= BIT(1); /*  Enable falling edge triggering interrupt */
+	rtw_write8(padapter, REG_GPIO_INTM + 1, value8);
+	value8 = rtw_read8(padapter, REG_GPIO_IO_SEL_2 + 1);
+	value8 |= BIT(1);
+	rtw_write8(padapter, REG_GPIO_IO_SEL_2 + 1, value8);
+
+	/*  Enable power down and GPIO interrupt */
+	value16 = rtw_read16(padapter, REG_APS_FSMCO);
+	value16 |= EnPDN; /*  Enable HW power down and RF on */
+	rtw_write16(padapter, REG_APS_FSMCO, value16);
+
+	/*  Enable CMD53 R/W Operation */
+/* 	bMacPwrCtrlOn = true; */
+/* 	rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn); */
+
+	rtw_write8(padapter, REG_CR, 0x00);
+	/*  Enable MAC DMA/WMAC/SCHEDULE/SEC block */
+	value16 = rtw_read16(padapter, REG_CR);
+	value16 |= (
+		HCI_TXDMA_EN |
+		HCI_RXDMA_EN |
+		TXDMA_EN |
+		RXDMA_EN |
+		PROTOCOL_EN |
+		SCHEDULE_EN |
+		ENSEC |
+		CALTMR_EN
+	);
+	rtw_write16(padapter, REG_CR, value16);
+
+	hal_btcoex_PowerOnSetting(padapter);
+
+	/*  external switch to S1 */
+	/*  0x38[11] = 0x1 */
+	/*  0x4c[23] = 0x1 */
+	/*  0x64[0] = 0 */
+	value16 = rtw_read16(padapter, REG_PWR_DATA);
+	/*  Switch the control of EESK, EECS to RFC for DPDT or Antenna switch */
+	value16 |= BIT(11); /*  BIT_EEPRPAD_RFE_CTRL_EN */
+	rtw_write16(padapter, REG_PWR_DATA, value16);
+
+	value32 = rtw_read32(padapter, REG_LEDCFG0);
+	value32 |= BIT(23); /*  DPDT_SEL_EN, 1 for SW control */
+	rtw_write32(padapter, REG_LEDCFG0, value32);
+
+	value8 = rtw_read8(padapter, REG_PAD_CTRL1_8723B);
+	value8 &= ~BIT(0); /*  BIT_SW_DPDT_SEL_DATA, DPDT_SEL default configuration */
+	rtw_write8(padapter, REG_PAD_CTRL1_8723B, value8);
+
+	return _SUCCESS;
+}
+
+/* Tx Page FIFO threshold */
+static void _init_available_page_threshold(struct adapter *padapter, u8 numHQ, u8 numNQ, u8 numLQ, u8 numPubQ)
+{
+	u16 HQ_threshold, NQ_threshold, LQ_threshold;
+
+	HQ_threshold = (numPubQ + numHQ + 1) >> 1;
+	HQ_threshold |= (HQ_threshold << 8);
+
+	NQ_threshold = (numPubQ + numNQ + 1) >> 1;
+	NQ_threshold |= (NQ_threshold << 8);
+
+	LQ_threshold = (numPubQ + numLQ + 1) >> 1;
+	LQ_threshold |= (LQ_threshold << 8);
+
+	rtw_write16(padapter, 0x218, HQ_threshold);
+	rtw_write16(padapter, 0x21A, NQ_threshold);
+	rtw_write16(padapter, 0x21C, LQ_threshold);
+}
+
+static void _InitQueueReservedPage(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	u32 numHQ = 0;
+	u32 numLQ = 0;
+	u32 numNQ = 0;
+	u32 numPubQ;
+	u32 value32;
+	u8 value8;
+	bool bWiFiConfig	= pregistrypriv->wifi_spec;
+
+	if (pHalData->OutEpQueueSel & TX_SELE_HQ)
+		numHQ = bWiFiConfig ? WMM_NORMAL_PAGE_NUM_HPQ_8723B : NORMAL_PAGE_NUM_HPQ_8723B;
+
+	if (pHalData->OutEpQueueSel & TX_SELE_LQ)
+		numLQ = bWiFiConfig ? WMM_NORMAL_PAGE_NUM_LPQ_8723B : NORMAL_PAGE_NUM_LPQ_8723B;
+
+	/*  NOTE: This step shall be proceed before writing REG_RQPN. */
+	if (pHalData->OutEpQueueSel & TX_SELE_NQ)
+		numNQ = bWiFiConfig ? WMM_NORMAL_PAGE_NUM_NPQ_8723B : NORMAL_PAGE_NUM_NPQ_8723B;
+
+	numPubQ = TX_TOTAL_PAGE_NUMBER_8723B - numHQ - numLQ - numNQ;
+
+	value8 = (u8)_NPQ(numNQ);
+	rtw_write8(padapter, REG_RQPN_NPQ, value8);
+
+	/*  TX DMA */
+	value32 = _HPQ(numHQ) | _LPQ(numLQ) | _PUBQ(numPubQ) | LD_RQPN;
+	rtw_write32(padapter, REG_RQPN, value32);
+
+	rtw_hal_set_sdio_tx_max_length(padapter, numHQ, numNQ, numLQ, numPubQ);
+
+	_init_available_page_threshold(padapter, numHQ, numNQ, numLQ, numPubQ);
+}
+
+static void _InitTxBufferBoundary(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	/* u16 txdmactrl; */
+	u8 txpktbuf_bndy;
+
+	if (!pregistrypriv->wifi_spec) {
+		txpktbuf_bndy = TX_PAGE_BOUNDARY_8723B;
+	} else {
+		/* for WMM */
+		txpktbuf_bndy = WMM_NORMAL_TX_PAGE_BOUNDARY_8723B;
+	}
+
+	rtw_write8(padapter, REG_TXPKTBUF_BCNQ_BDNY_8723B, txpktbuf_bndy);
+	rtw_write8(padapter, REG_TXPKTBUF_MGQ_BDNY_8723B, txpktbuf_bndy);
+	rtw_write8(padapter, REG_TXPKTBUF_WMAC_LBK_BF_HD_8723B, txpktbuf_bndy);
+	rtw_write8(padapter, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtw_write8(padapter, REG_TDECTRL + 1, txpktbuf_bndy);
+}
+
+static void _InitNormalChipRegPriority(
+	struct adapter *Adapter,
+	u16 beQ,
+	u16 bkQ,
+	u16 viQ,
+	u16 voQ,
+	u16 mgtQ,
+	u16 hiQ
+)
+{
+	u16 value16 = (rtw_read16(Adapter, REG_TRXDMA_CTRL) & 0x7);
+
+	value16 |=
+		_TXDMA_BEQ_MAP(beQ)  |
+		_TXDMA_BKQ_MAP(bkQ)  |
+		_TXDMA_VIQ_MAP(viQ)  |
+		_TXDMA_VOQ_MAP(voQ)  |
+		_TXDMA_MGQ_MAP(mgtQ) |
+		_TXDMA_HIQ_MAP(hiQ);
+
+	rtw_write16(Adapter, REG_TRXDMA_CTRL, value16);
+}
+
+static void _InitNormalChipOneOutEpPriority(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	u16 value = 0;
+	switch (pHalData->OutEpQueueSel) {
+	case TX_SELE_HQ:
+		value = QUEUE_HIGH;
+		break;
+	case TX_SELE_LQ:
+		value = QUEUE_LOW;
+		break;
+	case TX_SELE_NQ:
+		value = QUEUE_NORMAL;
+		break;
+	default:
+		break;
+	}
+
+	_InitNormalChipRegPriority(
+		Adapter, value, value, value, value, value, value
+	);
+
+}
+
+static void _InitNormalChipTwoOutEpPriority(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
+	u16 beQ, bkQ, viQ, voQ, mgtQ, hiQ;
+
+
+	u16 valueHi = 0;
+	u16 valueLow = 0;
+
+	switch (pHalData->OutEpQueueSel) {
+	case (TX_SELE_HQ | TX_SELE_LQ):
+		valueHi = QUEUE_HIGH;
+		valueLow = QUEUE_LOW;
+		break;
+	case (TX_SELE_NQ | TX_SELE_LQ):
+		valueHi = QUEUE_NORMAL;
+		valueLow = QUEUE_LOW;
+		break;
+	case (TX_SELE_HQ | TX_SELE_NQ):
+		valueHi = QUEUE_HIGH;
+		valueLow = QUEUE_NORMAL;
+		break;
+	default:
+		break;
+	}
+
+	if (!pregistrypriv->wifi_spec) {
+		beQ = valueLow;
+		bkQ = valueLow;
+		viQ = valueHi;
+		voQ = valueHi;
+		mgtQ = valueHi;
+		hiQ = valueHi;
+	} else {
+		/* for WMM , CONFIG_OUT_EP_WIFI_MODE */
+		beQ = valueLow;
+		bkQ = valueHi;
+		viQ = valueHi;
+		voQ = valueLow;
+		mgtQ = valueHi;
+		hiQ = valueHi;
+	}
+
+	_InitNormalChipRegPriority(Adapter, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
+
+}
+
+static void _InitNormalChipThreeOutEpPriority(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	u16 beQ, bkQ, viQ, voQ, mgtQ, hiQ;
+
+	if (!pregistrypriv->wifi_spec) {
+		/*  typical setting */
+		beQ = QUEUE_LOW;
+		bkQ = QUEUE_LOW;
+		viQ = QUEUE_NORMAL;
+		voQ = QUEUE_HIGH;
+		mgtQ = QUEUE_HIGH;
+		hiQ = QUEUE_HIGH;
+	} else {
+		/*  for WMM */
+		beQ = QUEUE_LOW;
+		bkQ = QUEUE_NORMAL;
+		viQ = QUEUE_NORMAL;
+		voQ = QUEUE_HIGH;
+		mgtQ = QUEUE_HIGH;
+		hiQ = QUEUE_HIGH;
+	}
+	_InitNormalChipRegPriority(padapter, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
+}
+
+static void _InitQueuePriority(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	switch (pHalData->OutEpNumber) {
+	case 1:
+		_InitNormalChipOneOutEpPriority(Adapter);
+		break;
+	case 2:
+		_InitNormalChipTwoOutEpPriority(Adapter);
+		break;
+	case 3:
+		_InitNormalChipThreeOutEpPriority(Adapter);
+		break;
+	default:
+		break;
+	}
+
+
+}
+
+static void _InitPageBoundary(struct adapter *padapter)
+{
+	/*  RX Page Boundary */
+	u16 rxff_bndy = RX_DMA_BOUNDARY_8723B;
+
+	rtw_write16(padapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
+}
+
+static void _InitTransferPageSize(struct adapter *padapter)
+{
+	/*  Tx page size is always 128. */
+
+	u8 value8;
+	value8 = _PSRX(PBP_128) | _PSTX(PBP_128);
+	rtw_write8(padapter, REG_PBP, value8);
+}
+
+static void _InitDriverInfoSize(struct adapter *padapter, u8 drvInfoSize)
+{
+	rtw_write8(padapter, REG_RX_DRVINFO_SZ, drvInfoSize);
+}
+
+static void _InitNetworkType(struct adapter *padapter)
+{
+	u32 value32;
+
+	value32 = rtw_read32(padapter, REG_CR);
+
+	/*  TODO: use the other function to set network type */
+/* 	value32 = (value32 & ~MASK_NETTYPE) | _NETTYPE(NT_LINK_AD_HOC); */
+	value32 = (value32 & ~MASK_NETTYPE) | _NETTYPE(NT_LINK_AP);
+
+	rtw_write32(padapter, REG_CR, value32);
+}
+
+static void _InitWMACSetting(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	u16 value16;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->ReceiveConfig = 0;
+	pHalData->ReceiveConfig |= RCR_APM | RCR_AM | RCR_AB;
+	pHalData->ReceiveConfig |= RCR_CBSSID_DATA | RCR_CBSSID_BCN | RCR_AMF;
+	pHalData->ReceiveConfig |= RCR_HTC_LOC_CTRL;
+	pHalData->ReceiveConfig |= RCR_APP_PHYST_RXFF | RCR_APP_ICV | RCR_APP_MIC;
+	rtw_write32(padapter, REG_RCR, pHalData->ReceiveConfig);
+
+	/*  Accept all multicast address */
+	rtw_write32(padapter, REG_MAR, 0xFFFFFFFF);
+	rtw_write32(padapter, REG_MAR + 4, 0xFFFFFFFF);
+
+	/*  Accept all data frames */
+	value16 = 0xFFFF;
+	rtw_write16(padapter, REG_RXFLTMAP2, value16);
+
+	/*  2010.09.08 hpfan */
+	/*  Since ADF is removed from RCR, ps-poll will not be indicate to driver, */
+	/*  RxFilterMap should mask ps-poll to gurantee AP mode can rx ps-poll. */
+	value16 = 0x400;
+	rtw_write16(padapter, REG_RXFLTMAP1, value16);
+
+	/*  Accept all management frames */
+	value16 = 0xFFFF;
+	rtw_write16(padapter, REG_RXFLTMAP0, value16);
+}
+
+static void _InitAdaptiveCtrl(struct adapter *padapter)
+{
+	u16 value16;
+	u32 value32;
+
+	/*  Response Rate Set */
+	value32 = rtw_read32(padapter, REG_RRSR);
+	value32 &= ~RATE_BITMAP_ALL;
+	value32 |= RATE_RRSR_CCK_ONLY_1M;
+	rtw_write32(padapter, REG_RRSR, value32);
+
+	/*  CF-END Threshold */
+	/* m_spIoBase->rtw_write8(REG_CFEND_TH, 0x1); */
+
+	/*  SIFS (used in NAV) */
+	value16 = _SPEC_SIFS_CCK(0x10) | _SPEC_SIFS_OFDM(0x10);
+	rtw_write16(padapter, REG_SPEC_SIFS, value16);
+
+	/*  Retry Limit */
+	value16 = _LRL(0x30) | _SRL(0x30);
+	rtw_write16(padapter, REG_RL, value16);
+}
+
+static void _InitEDCA(struct adapter *padapter)
+{
+	/*  Set Spec SIFS (used in NAV) */
+	rtw_write16(padapter, REG_SPEC_SIFS, 0x100a);
+	rtw_write16(padapter, REG_MAC_SPEC_SIFS, 0x100a);
+
+	/*  Set SIFS for CCK */
+	rtw_write16(padapter, REG_SIFS_CTX, 0x100a);
+
+	/*  Set SIFS for OFDM */
+	rtw_write16(padapter, REG_SIFS_TRX, 0x100a);
+
+	/*  TXOP */
+	rtw_write32(padapter, REG_EDCA_BE_PARAM, 0x005EA42B);
+	rtw_write32(padapter, REG_EDCA_BK_PARAM, 0x0000A44F);
+	rtw_write32(padapter, REG_EDCA_VI_PARAM, 0x005EA324);
+	rtw_write32(padapter, REG_EDCA_VO_PARAM, 0x002FA226);
+}
+
+static void _InitRetryFunction(struct adapter *padapter)
+{
+	u8 value8;
+
+	value8 = rtw_read8(padapter, REG_FWHW_TXQ_CTRL);
+	value8 |= EN_AMPDU_RTY_NEW;
+	rtw_write8(padapter, REG_FWHW_TXQ_CTRL, value8);
+
+	/*  Set ACK timeout */
+	rtw_write8(padapter, REG_ACKTO, 0x40);
+}
+
+static void HalRxAggr8723BSdio(struct adapter *padapter)
+{
+	u8 valueDMATimeout;
+	u8 valueDMAPageCount;
+
+	valueDMATimeout = 0x06;
+	valueDMAPageCount = 0x06;
+
+	rtw_write8(padapter, REG_RXDMA_AGG_PG_TH + 1, valueDMATimeout);
+	rtw_write8(padapter, REG_RXDMA_AGG_PG_TH, valueDMAPageCount);
+}
+
+static void sdio_AggSettingRxUpdate(struct adapter *padapter)
+{
+	u8 valueDMA;
+	u8 valueRxAggCtrl = 0;
+	u8 aggBurstNum = 3;  /* 0:1, 1:2, 2:3, 3:4 */
+	u8 aggBurstSize = 0;  /* 0:1K, 1:512Byte, 2:256Byte... */
+
+	valueDMA = rtw_read8(padapter, REG_TRXDMA_CTRL);
+	valueDMA |= RXDMA_AGG_EN;
+	rtw_write8(padapter, REG_TRXDMA_CTRL, valueDMA);
+
+	valueRxAggCtrl |= RXDMA_AGG_MODE_EN;
+	valueRxAggCtrl |= ((aggBurstNum << 2) & 0x0C);
+	valueRxAggCtrl |= ((aggBurstSize << 4) & 0x30);
+	rtw_write8(padapter, REG_RXDMA_MODE_CTRL_8723B, valueRxAggCtrl);/* RxAggLowThresh = 4*1K */
+}
+
+static void _initSdioAggregationSetting(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	/*  Tx aggregation setting */
+/* 	sdio_AggSettingTxUpdate(padapter); */
+
+	/*  Rx aggregation setting */
+	HalRxAggr8723BSdio(padapter);
+
+	sdio_AggSettingRxUpdate(padapter);
+
+	/*  201/12/10 MH Add for USB agg mode dynamic switch. */
+	pHalData->UsbRxHighSpeedMode = false;
+}
+
+static void _InitOperationMode(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext;
+	u8 regBwOpMode = 0;
+
+	pmlmeext = &padapter->mlmeextpriv;
+
+	/* 1 This part need to modified according to the rate set we filtered!! */
+	/*  */
+	/*  Set RRSR, RATR, and REG_BWOPMODE registers */
+	/*  */
+	switch (pmlmeext->cur_wireless_mode) {
+	case WIRELESS_MODE_B:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		break;
+	case WIRELESS_MODE_G:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		break;
+	case WIRELESS_MODE_AUTO:
+		regBwOpMode = BW_OPMODE_20MHZ;
+		break;
+	case WIRELESS_MODE_N_24G:
+		/*  It support CCK rate by default. */
+		/*  CCK rate will be filtered out only when associated AP does not support it. */
+		regBwOpMode = BW_OPMODE_20MHZ;
+		break;
+
+	default: /* for MacOSX compiler warning. */
+		break;
+	}
+
+	rtw_write8(padapter, REG_BWOPMODE, regBwOpMode);
+
+}
+
+static void _InitInterrupt(struct adapter *padapter)
+{
+	/*  HISR - turn all off */
+	rtw_write32(padapter, REG_HISR, 0);
+
+	/*  HIMR - turn all off */
+	rtw_write32(padapter, REG_HIMR, 0);
+
+	/*  */
+	/*  Initialize and enable SDIO Host Interrupt. */
+	/*  */
+	InitInterrupt8723BSdio(padapter);
+
+	/*  */
+	/*  Initialize system Host Interrupt. */
+	/*  */
+	InitSysInterrupt8723BSdio(padapter);
+}
+
+static void _InitRFType(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->rf_chip	= RF_6052;
+}
+
+static void _RfPowerSave(struct adapter *padapter)
+{
+/* YJ, TODO */
+}
+
+/*  */
+/*  2010/08/09 MH Add for power down check. */
+/*  */
+static bool HalDetectPwrDownMode(struct adapter *Adapter)
+{
+	u8 tmpvalue;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(Adapter);
+
+
+	EFUSE_ShadowRead(Adapter, 1, 0x7B/*EEPROM_RF_OPT3_92C*/, (u32 *)&tmpvalue);
+
+	/*  2010/08/25 MH INF priority > PDN Efuse value. */
+	if (tmpvalue & BIT4 && pwrctrlpriv->reg_pdnmode)
+		pHalData->pwrdown = true;
+	else
+		pHalData->pwrdown = false;
+
+	return pHalData->pwrdown;
+}	/*  HalDetectPwrDownMode */
+
+static u32 rtl8723bs_hal_init(struct adapter *padapter)
+{
+	s32 ret;
+	struct hal_com_data *pHalData;
+	struct pwrctrl_priv *pwrctrlpriv;
+	u32 NavUpper = WiFiNavUpperUs;
+	u8 u1bTmp;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (
+		adapter_to_pwrctl(padapter)->bips_processing == true &&
+		adapter_to_pwrctl(padapter)->pre_ips_type == 0
+	) {
+		unsigned long start_time;
+		u8 cpwm_orig, cpwm_now;
+		u8 val8, bMacPwrCtrlOn = true;
+
+		/* for polling cpwm */
+		cpwm_orig = 0;
+		rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
+
+		/* ser rpwm */
+		val8 = rtw_read8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1);
+		val8 &= 0x80;
+		val8 += 0x80;
+		val8 |= BIT(6);
+		rtw_write8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1, val8);
+		adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80;
+
+		/* do polling cpwm */
+		start_time = jiffies;
+		do {
+
+			mdelay(1);
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now);
+			if ((cpwm_orig ^ cpwm_now) & 0x80)
+				break;
+
+			if (jiffies_to_msecs(jiffies - start_time) > 100)
+				break;
+
+		} while (1);
+
+		rtl8723b_set_FwPwrModeInIPS_cmd(padapter, 0);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+
+		hal_btcoex_InitHwConfig(padapter, false);
+
+		return _SUCCESS;
+	}
+
+	/*  Disable Interrupt first. */
+/* 	rtw_hal_disable_interrupt(padapter); */
+
+	ret = _InitPowerOn_8723BS(padapter);
+	if (ret == _FAIL)
+		return _FAIL;
+
+	rtw_write8(padapter, REG_EARLY_MODE_CONTROL, 0);
+
+	ret = rtl8723b_FirmwareDownload(padapter, false);
+	if (ret != _SUCCESS) {
+		padapter->bFWReady = false;
+		pHalData->fw_ractrl = false;
+		return ret;
+	} else {
+		padapter->bFWReady = true;
+		pHalData->fw_ractrl = true;
+	}
+
+	rtl8723b_InitializeFirmwareVars(padapter);
+
+/* 	SIC_Init(padapter); */
+
+	if (pwrctrlpriv->reg_rfoff)
+		pwrctrlpriv->rf_pwrstate = rf_off;
+
+	/*  2010/08/09 MH We need to check if we need to turnon or off RF after detecting */
+	/*  HW GPIO pin. Before PHY_RFConfig8192C. */
+	HalDetectPwrDownMode(padapter);
+
+	/*  Set RF type for BB/RF configuration */
+	_InitRFType(padapter);
+
+	/*  Save target channel */
+	/*  <Roger_Notes> Current Channel will be updated again later. */
+	pHalData->CurrentChannel = 6;
+
+	ret = PHY_MACConfig8723B(padapter);
+	if (ret != _SUCCESS)
+		return ret;
+	/*  */
+	/* d. Initialize BB related configurations. */
+	/*  */
+	ret = PHY_BBConfig8723B(padapter);
+	if (ret != _SUCCESS)
+		return ret;
+
+	/*  If RF is on, we need to init RF. Otherwise, skip the procedure. */
+	/*  We need to follow SU method to change the RF cfg.txt. Default disable RF TX/RX mode. */
+	/* if (pHalData->eRFPowerState == eRfOn) */
+	{
+		ret = PHY_RFConfig8723B(padapter);
+		if (ret != _SUCCESS)
+			return ret;
+	}
+
+	/*  */
+	/*  Joseph Note: Keep RfRegChnlVal for later use. */
+	/*  */
+	pHalData->RfRegChnlVal[0] =
+		PHY_QueryRFReg(padapter, (enum rf_path)0, RF_CHNLBW, bRFRegOffsetMask);
+	pHalData->RfRegChnlVal[1] =
+		PHY_QueryRFReg(padapter, (enum rf_path)1, RF_CHNLBW, bRFRegOffsetMask);
+
+
+	/* if (!pHalData->bMACFuncEnable) { */
+	_InitQueueReservedPage(padapter);
+	_InitTxBufferBoundary(padapter);
+
+	/*  init LLT after tx buffer boundary is defined */
+	ret = rtl8723b_InitLLTTable(padapter);
+	if (ret != _SUCCESS)
+		return _FAIL;
+
+	/*  */
+	_InitQueuePriority(padapter);
+	_InitPageBoundary(padapter);
+	_InitTransferPageSize(padapter);
+
+	/*  Get Rx PHY status in order to report RSSI and others. */
+	_InitDriverInfoSize(padapter, DRVINFO_SZ);
+	hal_init_macaddr(padapter);
+	_InitNetworkType(padapter);
+	_InitWMACSetting(padapter);
+	_InitAdaptiveCtrl(padapter);
+	_InitEDCA(padapter);
+	_InitRetryFunction(padapter);
+	_initSdioAggregationSetting(padapter);
+	_InitOperationMode(padapter);
+	rtl8723b_InitBeaconParameters(padapter);
+	_InitInterrupt(padapter);
+	_InitBurstPktLen_8723BS(padapter);
+
+	/* YJ, TODO */
+	rtw_write8(padapter, REG_SECONDARY_CCA_CTRL_8723B, 0x3);	/*  CCA */
+	rtw_write8(padapter, 0x976, 0);	/*  hpfan_todo: 2nd CCA related */
+
+	rtw_write16(padapter, REG_PKT_VO_VI_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
+	rtw_write16(padapter, REG_PKT_BE_BK_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
+
+	invalidate_cam_all(padapter);
+
+	rtw_hal_set_chnl_bw(padapter, padapter->registrypriv.channel,
+		CHANNEL_WIDTH_20, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HAL_PRIME_CHNL_OFFSET_DONT_CARE);
+
+	/*  Record original value for template. This is arough data, we can only use the data */
+	/*  for power adjust. The value can not be adjustde according to different power!!! */
+/* 	pHalData->OriginalCckTxPwrIdx = pHalData->CurrentCckTxPwrIdx; */
+/* 	pHalData->OriginalOfdm24GTxPwrIdx = pHalData->CurrentOfdm24GTxPwrIdx; */
+
+	rtl8723b_InitAntenna_Selection(padapter);
+
+	/*  */
+	/*  Disable BAR, suggested by Scott */
+	/*  2010.04.09 add by hpfan */
+	/*  */
+	rtw_write32(padapter, REG_BAR_MODE_CTRL, 0x0201ffff);
+
+	/*  HW SEQ CTRL */
+	/*  set 0x0 to 0xFF by tynli. Default enable HW SEQ NUM. */
+	rtw_write8(padapter, REG_HWSEQ_CTRL, 0xFF);
+
+
+	/*  */
+	/*  Configure SDIO TxRx Control to enable Rx DMA timer masking. */
+	/*  2010.02.24. */
+	/*  */
+	rtw_write32(padapter, SDIO_LOCAL_BASE | SDIO_REG_TX_CTRL, 0);
+
+	_RfPowerSave(padapter);
+
+
+	rtl8723b_InitHalDm(padapter);
+
+	/*  */
+	/*  Update current Tx FIFO page status. */
+	/*  */
+	HalQueryTxBufferStatus8723BSdio(padapter);
+	HalQueryTxOQTBufferStatus8723BSdio(padapter);
+	pHalData->SdioTxOQTMaxFreeSpace = pHalData->SdioTxOQTFreeSpace;
+
+	/*  Enable MACTXEN/MACRXEN block */
+	u1bTmp = rtw_read8(padapter, REG_CR);
+	u1bTmp |= (MACTXEN | MACRXEN);
+	rtw_write8(padapter, REG_CR, u1bTmp);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_NAV_UPPER, (u8 *)&NavUpper);
+
+	/* ack for xmit mgmt frames. */
+	rtw_write32(padapter, REG_FWHW_TXQ_CTRL, rtw_read32(padapter, REG_FWHW_TXQ_CTRL) | BIT(12));
+
+/* 	pHalData->PreRpwmVal = SdioLocalCmd52Read1Byte(padapter, SDIO_REG_HRPWM1) & 0x80; */
+
+	{
+		pwrctrlpriv->rf_pwrstate = rf_on;
+
+		if (pwrctrlpriv->rf_pwrstate == rf_on) {
+			struct pwrctrl_priv *pwrpriv;
+			unsigned long start_time;
+			u8 restore_iqk_rst;
+			u8 b2Ant;
+			u8 h2cCmdBuf;
+
+			pwrpriv = adapter_to_pwrctl(padapter);
+
+			PHY_LCCalibrate_8723B(&pHalData->odmpriv);
+
+			/* Inform WiFi FW that it is the beginning of IQK */
+			h2cCmdBuf = 1;
+			FillH2CCmd8723B(padapter, H2C_8723B_BT_WLAN_CALIBRATION, 1, &h2cCmdBuf);
+
+			start_time = jiffies;
+			do {
+				if (rtw_read8(padapter, 0x1e7) & 0x01)
+					break;
+
+				msleep(50);
+			} while (jiffies_to_msecs(jiffies - start_time) <= 400);
+
+			hal_btcoex_IQKNotify(padapter, true);
+
+			restore_iqk_rst = pwrpriv->bips_processing;
+			b2Ant = pHalData->EEPROMBluetoothAntNum == Ant_x2;
+			PHY_IQCalibrate_8723B(padapter, false, restore_iqk_rst, b2Ant, pHalData->ant_path);
+			pHalData->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
+
+			hal_btcoex_IQKNotify(padapter, false);
+
+			/* Inform WiFi FW that it is the finish of IQK */
+			h2cCmdBuf = 0;
+			FillH2CCmd8723B(padapter, H2C_8723B_BT_WLAN_CALIBRATION, 1, &h2cCmdBuf);
+
+			ODM_TXPowerTrackingCheck(&pHalData->odmpriv);
+		}
+	}
+
+	/*  Init BT hw config. */
+	hal_btcoex_InitHwConfig(padapter, false);
+
+	return _SUCCESS;
+}
+
+/*  */
+/*  Description: */
+/* 	RTL8723e card disable power sequence v003 which suggested by Scott. */
+/*  */
+/*  First created by tynli. 2011.01.28. */
+/*  */
+static void CardDisableRTL8723BSdio(struct adapter *padapter)
+{
+	u8 u1bTmp;
+	u8 bMacPwrCtrlOn;
+
+	/*  Run LPS WL RFOFF flow */
+	HalPwrSeqCmdParsing(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, rtl8723B_enter_lps_flow);
+
+	/* 	==== Reset digital sequence   ====== */
+
+	u1bTmp = rtw_read8(padapter, REG_MCUFWDL);
+	if ((u1bTmp & RAM_DL_SEL) && padapter->bFWReady) /* 8051 RAM code */
+		rtl8723b_FirmwareSelfReset(padapter);
+
+	/*  Reset MCU 0x2[10]= 0. Suggested by Filen. 2011.01.26. by tynli. */
+	u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
+	u1bTmp &= ~BIT(2);	/*  0x2[10], FEN_CPUEN */
+	rtw_write8(padapter, REG_SYS_FUNC_EN + 1, u1bTmp);
+
+	/*  MCUFWDL 0x80[1:0]= 0 */
+	/*  reset MCU ready status */
+	rtw_write8(padapter, REG_MCUFWDL, 0);
+
+	/*  Reset MCU IO Wrapper, added by Roger, 2011.08.30 */
+	u1bTmp = rtw_read8(padapter, REG_RSV_CTRL + 1);
+	u1bTmp &= ~BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL + 1, u1bTmp);
+	u1bTmp = rtw_read8(padapter, REG_RSV_CTRL + 1);
+	u1bTmp |= BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL+1, u1bTmp);
+
+	/* 	==== Reset digital sequence end ====== */
+
+	bMacPwrCtrlOn = false;	/*  Disable CMD53 R/W */
+	rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	HalPwrSeqCmdParsing(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, rtl8723B_card_disable_flow);
+}
+
+static u32 rtl8723bs_hal_deinit(struct adapter *padapter)
+{
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (padapter->hw_init_completed) {
+		if (adapter_to_pwrctl(padapter)->bips_processing) {
+			if (padapter->netif_up) {
+				int cnt = 0;
+				u8 val8 = 0;
+
+				rtl8723b_set_FwPwrModeInIPS_cmd(padapter, 0x3);
+				/* poll 0x1cc to make sure H2C command already finished by FW; MAC_0x1cc = 0 means H2C done by FW. */
+				do {
+					val8 = rtw_read8(padapter, REG_HMETFR);
+					cnt++;
+					mdelay(10);
+				} while (cnt < 100 && (val8 != 0));
+				/* H2C done, enter 32k */
+				if (val8 == 0) {
+					/* ser rpwm to enter 32k */
+					val8 = rtw_read8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1);
+					val8 += 0x80;
+					val8 |= BIT(0);
+					rtw_write8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1, val8);
+					adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80;
+					cnt = val8 = 0;
+					do {
+						val8 = rtw_read8(padapter, REG_CR);
+						cnt++;
+						mdelay(10);
+					} while (cnt < 100 && (val8 != 0xEA));
+				}
+
+				adapter_to_pwrctl(padapter)->pre_ips_type = 0;
+
+			} else {
+				pdbgpriv->dbg_carddisable_cnt++;
+				CardDisableRTL8723BSdio(padapter);
+
+				adapter_to_pwrctl(padapter)->pre_ips_type = 1;
+			}
+
+		} else {
+			pdbgpriv->dbg_carddisable_cnt++;
+			CardDisableRTL8723BSdio(padapter);
+		}
+	} else
+		pdbgpriv->dbg_deinit_fail_cnt++;
+
+	return _SUCCESS;
+}
+
+static u32 rtl8723bs_inirp_init(struct adapter *padapter)
+{
+	return _SUCCESS;
+}
+
+static u32 rtl8723bs_inirp_deinit(struct adapter *padapter)
+{
+	return _SUCCESS;
+}
+
+static void rtl8723bs_init_default_value(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	rtl8723b_init_default_value(padapter);
+
+	/*  interface related variable */
+	pHalData->SdioRxFIFOCnt = 0;
+}
+
+static void rtl8723bs_interface_configure(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	bool bWiFiConfig = pregistrypriv->wifi_spec;
+
+
+	pdvobjpriv->RtOutPipe[0] = WLAN_TX_HIQ_DEVICE_ID;
+	pdvobjpriv->RtOutPipe[1] = WLAN_TX_MIQ_DEVICE_ID;
+	pdvobjpriv->RtOutPipe[2] = WLAN_TX_LOQ_DEVICE_ID;
+
+	if (bWiFiConfig)
+		pHalData->OutEpNumber = 2;
+	else
+		pHalData->OutEpNumber = SDIO_MAX_TX_QUEUE;
+
+	switch (pHalData->OutEpNumber) {
+	case 3:
+		pHalData->OutEpQueueSel = TX_SELE_HQ | TX_SELE_LQ | TX_SELE_NQ;
+		break;
+	case 2:
+		pHalData->OutEpQueueSel = TX_SELE_HQ | TX_SELE_NQ;
+		break;
+	case 1:
+		pHalData->OutEpQueueSel = TX_SELE_HQ;
+		break;
+	default:
+		break;
+	}
+
+	Hal_MappingOutPipe(padapter, pHalData->OutEpNumber);
+}
+
+/*  */
+/* 	Description: */
+/* 		We should set Efuse cell selection to WiFi cell in default. */
+/*  */
+/* 	Assumption: */
+/* 		PASSIVE_LEVEL */
+/*  */
+/* 	Added by Roger, 2010.11.23. */
+/*  */
+static void _EfuseCellSel(struct adapter *padapter)
+{
+	u32 value32;
+
+	value32 = rtw_read32(padapter, EFUSE_TEST);
+	value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
+	rtw_write32(padapter, EFUSE_TEST, value32);
+}
+
+static void _ReadRFType(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	pHalData->rf_chip = RF_6052;
+}
+
+
+static void Hal_EfuseParseMACAddr_8723BS(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	u16 i;
+	u8 sMacAddr[6] = {0x00, 0xE0, 0x4C, 0xb7, 0x23, 0x00};
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	if (AutoLoadFail) {
+/* 		sMacAddr[5] = (u8)GetRandomNumber(1, 254); */
+		for (i = 0; i < 6; i++)
+			pEEPROM->mac_addr[i] = sMacAddr[i];
+	} else {
+		/* Read Permanent MAC address */
+		memcpy(pEEPROM->mac_addr, &hwinfo[EEPROM_MAC_ADDR_8723BS], ETH_ALEN);
+	}
+}
+
+static void Hal_EfuseParseBoardType_8723BS(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	if (!AutoLoadFail) {
+		pHalData->BoardType = (hwinfo[EEPROM_RF_BOARD_OPTION_8723B] & 0xE0) >> 5;
+		if (pHalData->BoardType == 0xFF)
+			pHalData->BoardType = (EEPROM_DEFAULT_BOARD_OPTION & 0xE0) >> 5;
+	} else
+		pHalData->BoardType = 0;
+}
+
+static void _ReadEfuseInfo8723BS(struct adapter *padapter)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	u8 *hwinfo = NULL;
+
+	/*  */
+	/*  This part read and parse the eeprom/efuse content */
+	/*  */
+
+	hwinfo = pEEPROM->efuse_eeprom_data;
+
+	Hal_InitPGData(padapter, hwinfo);
+
+	Hal_EfuseParseIDCode(padapter, hwinfo);
+	Hal_EfuseParseEEPROMVer_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	Hal_EfuseParseMACAddr_8723BS(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	Hal_EfuseParseTxPowerInfo_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseBoardType_8723BS(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	/*  */
+	/*  Read Bluetooth co-exist and initialize */
+	/*  */
+	Hal_EfuseParsePackageType_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseBTCoexistInfo_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseChnlPlan_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseXtal_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseThermalMeter_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseAntennaDiversity_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+	Hal_EfuseParseCustomerID_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	Hal_EfuseParseVoltage_8723B(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	Hal_ReadRFGainOffset(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+}
+
+static void _ReadPROMContent(struct adapter *padapter)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	u8 	eeValue;
+
+	eeValue = rtw_read8(padapter, REG_9346CR);
+	/*  To check system boot selection. */
+	pEEPROM->EepromOrEfuse = (eeValue & BOOT_FROM_EEPROM) ? true : false;
+	pEEPROM->bautoload_fail_flag = (eeValue & EEPROM_EN) ? false : true;
+
+/* 	pHalData->EEType = IS_BOOT_FROM_EEPROM(Adapter) ? EEPROM_93C46 : EEPROM_BOOT_EFUSE; */
+
+	_ReadEfuseInfo8723BS(padapter);
+}
+
+static void _InitOtherVariable(struct adapter *Adapter)
+{
+}
+
+/*  */
+/* 	Description: */
+/* 		Read HW adapter information by E-Fuse or EEPROM according CR9346 reported. */
+/*  */
+/* 	Assumption: */
+/* 		PASSIVE_LEVEL (SDIO interface) */
+/*  */
+/*  */
+static s32 _ReadAdapterInfo8723BS(struct adapter *padapter)
+{
+	u8 val8;
+
+	/*  before access eFuse, make sure card enable has been called */
+	if (!padapter->hw_init_completed)
+		_InitPowerOn_8723BS(padapter);
+
+
+	val8 = rtw_read8(padapter, 0x4e);
+	val8 |= BIT(6);
+	rtw_write8(padapter, 0x4e, val8);
+
+	_EfuseCellSel(padapter);
+	_ReadRFType(padapter);
+	_ReadPROMContent(padapter);
+	_InitOtherVariable(padapter);
+
+	if (!padapter->hw_init_completed) {
+		rtw_write8(padapter, 0x67, 0x00); /*  for BT, Switch Ant control to BT */
+		CardDisableRTL8723BSdio(padapter);/* for the power consumption issue,  wifi ko module is loaded during booting, but wifi GUI is off */
+	}
+
+	return _SUCCESS;
+}
+
+static void ReadAdapterInfo8723BS(struct adapter *padapter)
+{
+	/*  Read EEPROM size before call any EEPROM function */
+	padapter->EepromAddressSize = GetEEPROMSize8723B(padapter);
+
+	_ReadAdapterInfo8723BS(padapter);
+}
+
+/*
+ * If variable not handled here,
+ * some variables will be processed in SetHwReg8723B()
+ */
+static void SetHwReg8723BS(struct adapter *padapter, u8 variable, u8 *val)
+{
+	u8 val8;
+
+	switch (variable) {
+	case HW_VAR_SET_RPWM:
+		/*  rpwm value only use BIT0(clock bit) , BIT6(Ack bit), and BIT7(Toggle bit) */
+		/*  BIT0 value - 1: 32k, 0:40MHz. */
+		/*  BIT6 value - 1: report cpwm value after success set, 0:do not report. */
+		/*  BIT7 value - Toggle bit change. */
+		{
+			val8 = *val;
+			val8 &= 0xC1;
+			rtw_write8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1, val8);
+		}
+		break;
+	case HW_VAR_SET_REQ_FW_PS:
+		{
+			u8 req_fw_ps = 0;
+			req_fw_ps = rtw_read8(padapter, 0x8f);
+			req_fw_ps |= 0x10;
+			rtw_write8(padapter, 0x8f, req_fw_ps);
+		}
+		break;
+	case HW_VAR_RXDMA_AGG_PG_TH:
+		val8 = *val;
+		break;
+
+	case HW_VAR_DM_IN_LPS:
+		rtl8723b_hal_dm_in_lps(padapter);
+		break;
+	default:
+		SetHwReg8723B(padapter, variable, val);
+		break;
+	}
+}
+
+/*
+ * If variable not handled here,
+ * some variables will be processed in GetHwReg8723B()
+ */
+static void GetHwReg8723BS(struct adapter *padapter, u8 variable, u8 *val)
+{
+	switch (variable) {
+	case HW_VAR_CPWM:
+		*val = rtw_read8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HCPWM1_8723B);
+		break;
+
+	case HW_VAR_FW_PS_STATE:
+		{
+			/* 3. read dword 0x88               driver read fw ps state */
+			*((u16 *)val) = rtw_read16(padapter, 0x88);
+		}
+		break;
+	default:
+		GetHwReg8723B(padapter, variable, val);
+		break;
+	}
+}
+
+static void SetHwRegWithBuf8723B(struct adapter *padapter, u8 variable, u8 *pbuf, int len)
+{
+	switch (variable) {
+	case HW_VAR_C2H_HANDLE:
+		C2HPacketHandler_8723B(padapter, pbuf, len);
+		break;
+	default:
+		break;
+	}
+}
+
+/*  */
+/* 	Description: */
+/* 		Query setting of specified variable. */
+/*  */
+static u8 GetHalDefVar8723BSDIO(
+	struct adapter *Adapter, enum hal_def_variable eVariable, void *pValue
+)
+{
+	u8 	bResult = _SUCCESS;
+
+	switch (eVariable) {
+	case HAL_DEF_IS_SUPPORT_ANT_DIV:
+		break;
+	case HAL_DEF_CURRENT_ANTENNA:
+		break;
+	case HW_VAR_MAX_RX_AMPDU_FACTOR:
+		/*  Stanley@BB.SD3 suggests 16K can get stable performance */
+		/*  coding by Lucas@20130730 */
+		*(u32 *)pValue = IEEE80211_HT_MAX_AMPDU_16K;
+		break;
+	default:
+		bResult = GetHalDefVar8723B(Adapter, eVariable, pValue);
+		break;
+	}
+
+	return bResult;
+}
+
+/*  */
+/* 	Description: */
+/* 		Change default setting of specified variable. */
+/*  */
+static u8 SetHalDefVar8723BSDIO(struct adapter *Adapter,
+				enum hal_def_variable eVariable, void *pValue)
+{
+	return SetHalDefVar8723B(Adapter, eVariable, pValue);
+}
+
+void rtl8723bs_set_hal_ops(struct adapter *padapter)
+{
+	struct hal_ops *pHalFunc = &padapter->HalFunc;
+
+	rtl8723b_set_hal_ops(pHalFunc);
+
+	pHalFunc->hal_init = &rtl8723bs_hal_init;
+	pHalFunc->hal_deinit = &rtl8723bs_hal_deinit;
+
+	pHalFunc->inirp_init = &rtl8723bs_inirp_init;
+	pHalFunc->inirp_deinit = &rtl8723bs_inirp_deinit;
+
+	pHalFunc->init_xmit_priv = &rtl8723bs_init_xmit_priv;
+	pHalFunc->free_xmit_priv = &rtl8723bs_free_xmit_priv;
+
+	pHalFunc->init_recv_priv = &rtl8723bs_init_recv_priv;
+	pHalFunc->free_recv_priv = &rtl8723bs_free_recv_priv;
+
+	pHalFunc->init_default_value = &rtl8723bs_init_default_value;
+	pHalFunc->intf_chip_configure = &rtl8723bs_interface_configure;
+	pHalFunc->read_adapter_info = &ReadAdapterInfo8723BS;
+
+	pHalFunc->enable_interrupt = &EnableInterrupt8723BSdio;
+	pHalFunc->disable_interrupt = &DisableInterrupt8723BSdio;
+	pHalFunc->check_ips_status = &CheckIPSStatus;
+	pHalFunc->SetHwRegHandler = &SetHwReg8723BS;
+	pHalFunc->GetHwRegHandler = &GetHwReg8723BS;
+	pHalFunc->SetHwRegHandlerWithBuf = &SetHwRegWithBuf8723B;
+	pHalFunc->GetHalDefVarHandler = &GetHalDefVar8723BSDIO;
+	pHalFunc->SetHalDefVarHandler = &SetHalDefVar8723BSDIO;
+
+	pHalFunc->hal_xmit = &rtl8723bs_hal_xmit;
+	pHalFunc->mgnt_xmit = &rtl8723bs_mgnt_xmit;
+	pHalFunc->hal_xmitframe_enqueue = &rtl8723bs_hal_xmitframe_enqueue;
+}
diff --git a/drivers/staging/rtl8723bs/hal/sdio_ops.c b/drivers/staging/rtl8723bs/hal/sdio_ops.c
new file mode 100644
index 000000000..107f427ee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/sdio_ops.c
@@ -0,0 +1,1012 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ *******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+/*  */
+/*  Description: */
+/*	The following mapping is for SDIO host local register space. */
+/*  */
+/*  Creadted by Roger, 2011.01.31. */
+/*  */
+static void hal_sdio_get_cmd_addr_8723b(
+	struct adapter *adapter,
+	u8 device_id,
+	u32 addr,
+	u32 *cmdaddr
+)
+{
+	switch (device_id) {
+	case SDIO_LOCAL_DEVICE_ID:
+		*cmdaddr = ((SDIO_LOCAL_DEVICE_ID << 13) | (addr & SDIO_LOCAL_MSK));
+		break;
+
+	case WLAN_IOREG_DEVICE_ID:
+		*cmdaddr = ((WLAN_IOREG_DEVICE_ID << 13) | (addr & WLAN_IOREG_MSK));
+		break;
+
+	case WLAN_TX_HIQ_DEVICE_ID:
+		*cmdaddr = ((WLAN_TX_HIQ_DEVICE_ID << 13) | (addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_TX_MIQ_DEVICE_ID:
+		*cmdaddr = ((WLAN_TX_MIQ_DEVICE_ID << 13) | (addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_TX_LOQ_DEVICE_ID:
+		*cmdaddr = ((WLAN_TX_LOQ_DEVICE_ID << 13) | (addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_RX0FF_DEVICE_ID:
+		*cmdaddr = ((WLAN_RX0FF_DEVICE_ID << 13) | (addr & WLAN_RX0FF_MSK));
+		break;
+
+	default:
+		break;
+	}
+}
+
+static u8 get_deviceid(u32 addr)
+{
+	u8 devide_id;
+	u16 pseudo_id;
+
+	pseudo_id = (u16)(addr >> 16);
+	switch (pseudo_id) {
+	case 0x1025:
+		devide_id = SDIO_LOCAL_DEVICE_ID;
+		break;
+
+	case 0x1026:
+		devide_id = WLAN_IOREG_DEVICE_ID;
+		break;
+
+	case 0x1031:
+		devide_id = WLAN_TX_HIQ_DEVICE_ID;
+		break;
+
+	case 0x1032:
+		devide_id = WLAN_TX_MIQ_DEVICE_ID;
+		break;
+
+	case 0x1033:
+		devide_id = WLAN_TX_LOQ_DEVICE_ID;
+		break;
+
+	case 0x1034:
+		devide_id = WLAN_RX0FF_DEVICE_ID;
+		break;
+
+	default:
+		devide_id = WLAN_IOREG_DEVICE_ID;
+		break;
+	}
+
+	return devide_id;
+}
+
+static u32 _cvrt2ftaddr(const u32 addr, u8 *pdevice_id, u16 *poffset)
+{
+	u8 device_id;
+	u16 offset;
+	u32 ftaddr;
+
+	device_id = get_deviceid(addr);
+	offset = 0;
+
+	switch (device_id) {
+	case SDIO_LOCAL_DEVICE_ID:
+		offset = addr & SDIO_LOCAL_MSK;
+		break;
+
+	case WLAN_TX_HIQ_DEVICE_ID:
+	case WLAN_TX_MIQ_DEVICE_ID:
+	case WLAN_TX_LOQ_DEVICE_ID:
+		offset = addr & WLAN_FIFO_MSK;
+		break;
+
+	case WLAN_RX0FF_DEVICE_ID:
+		offset = addr & WLAN_RX0FF_MSK;
+		break;
+
+	case WLAN_IOREG_DEVICE_ID:
+	default:
+		device_id = WLAN_IOREG_DEVICE_ID;
+		offset = addr & WLAN_IOREG_MSK;
+		break;
+	}
+	ftaddr = (device_id << 13) | offset;
+
+	if (pdevice_id)
+		*pdevice_id = device_id;
+	if (poffset)
+		*poffset = offset;
+
+	return ftaddr;
+}
+
+static u8 sdio_read8(struct intf_hdl *intfhdl, u32 addr)
+{
+	u32 ftaddr;
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+
+	return sd_read8(intfhdl, ftaddr, NULL);
+}
+
+static u16 sdio_read16(struct intf_hdl *intfhdl, u32 addr)
+{
+	u32 ftaddr;
+	__le16 le_tmp;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	sd_cmd52_read(intfhdl, ftaddr, 2, (u8 *)&le_tmp);
+
+	return le16_to_cpu(le_tmp);
+}
+
+static u32 sdio_read32(struct intf_hdl *intfhdl, u32 addr)
+{
+	struct adapter *adapter;
+	u8 mac_pwr_ctrl_on;
+	u8 device_id;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	u32 val;
+	s32 __maybe_unused err;
+	__le32 le_tmp;
+
+	adapter = intfhdl->padapter;
+	ftaddr = _cvrt2ftaddr(addr, &device_id, &offset);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		((device_id == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	) {
+		err = sd_cmd52_read(intfhdl, ftaddr, 4, (u8 *)&le_tmp);
+		return le32_to_cpu(le_tmp);
+	}
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		val = sd_read32(intfhdl, ftaddr, NULL);
+	} else {
+		u8 *tmpbuf;
+
+		tmpbuf = rtw_malloc(8);
+		if (!tmpbuf)
+			return SDIO_ERR_VAL32;
+
+		ftaddr &= ~(u16)0x3;
+		sd_read(intfhdl, ftaddr, 8, tmpbuf);
+		memcpy(&le_tmp, tmpbuf + shift, 4);
+		val = le32_to_cpu(le_tmp);
+
+		kfree(tmpbuf);
+	}
+	return val;
+}
+
+static s32 sdio_readN(struct intf_hdl *intfhdl, u32 addr, u32 cnt, u8 *buf)
+{
+	struct adapter *adapter;
+	u8 mac_pwr_ctrl_on;
+	u8 device_id;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+
+	adapter = intfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &device_id, &offset);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		((device_id == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	)
+		return sd_cmd52_read(intfhdl, ftaddr, cnt, buf);
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		err = sd_read(intfhdl, ftaddr, cnt, buf);
+	} else {
+		u8 *tmpbuf;
+		u32 n;
+
+		ftaddr &= ~(u16)0x3;
+		n = cnt + shift;
+		tmpbuf = rtw_malloc(n);
+		if (!tmpbuf)
+			return -1;
+
+		err = sd_read(intfhdl, ftaddr, n, tmpbuf);
+		if (!err)
+			memcpy(buf, tmpbuf + shift, cnt);
+		kfree(tmpbuf);
+	}
+	return err;
+}
+
+static s32 sdio_write8(struct intf_hdl *intfhdl, u32 addr, u8 val)
+{
+	u32 ftaddr;
+	s32 err;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	sd_write8(intfhdl, ftaddr, val, &err);
+
+	return err;
+}
+
+static s32 sdio_write16(struct intf_hdl *intfhdl, u32 addr, u16 val)
+{
+	u32 ftaddr;
+	__le16 le_tmp;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	le_tmp = cpu_to_le16(val);
+	return sd_cmd52_write(intfhdl, ftaddr, 2, (u8 *)&le_tmp);
+}
+
+static s32 sdio_write32(struct intf_hdl *intfhdl, u32 addr, u32 val)
+{
+	struct adapter *adapter;
+	u8 mac_pwr_ctrl_on;
+	u8 device_id;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+	__le32 le_tmp;
+
+	adapter = intfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &device_id, &offset);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		((device_id == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	) {
+		le_tmp = cpu_to_le32(val);
+
+		return sd_cmd52_write(intfhdl, ftaddr, 4, (u8 *)&le_tmp);
+	}
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		sd_write32(intfhdl, ftaddr, val, &err);
+	} else {
+		le_tmp = cpu_to_le32(val);
+		err = sd_cmd52_write(intfhdl, ftaddr, 4, (u8 *)&le_tmp);
+	}
+	return err;
+}
+
+static s32 sdio_writeN(struct intf_hdl *intfhdl, u32 addr, u32 cnt, u8 *buf)
+{
+	struct adapter *adapter;
+	u8 mac_pwr_ctrl_on;
+	u8 device_id;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+
+	adapter = intfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &device_id, &offset);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		((device_id == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	)
+		return sd_cmd52_write(intfhdl, ftaddr, cnt, buf);
+
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		err = sd_write(intfhdl, ftaddr, cnt, buf);
+	} else {
+		u8 *tmpbuf;
+		u32 n;
+
+		ftaddr &= ~(u16)0x3;
+		n = cnt + shift;
+		tmpbuf = rtw_malloc(n);
+		if (!tmpbuf)
+			return -1;
+		err = sd_read(intfhdl, ftaddr, 4, tmpbuf);
+		if (err) {
+			kfree(tmpbuf);
+			return err;
+		}
+		memcpy(tmpbuf + shift, buf, cnt);
+		err = sd_write(intfhdl, ftaddr, n, tmpbuf);
+		kfree(tmpbuf);
+	}
+	return err;
+}
+
+static void sdio_read_mem(
+	struct intf_hdl *intfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *rmem
+)
+{
+	sdio_readN(intfhdl, addr, cnt, rmem);
+}
+
+static void sdio_write_mem(
+	struct intf_hdl *intfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *wmem
+)
+{
+	sdio_writeN(intfhdl, addr, cnt, wmem);
+}
+
+/*
+ * Description:
+ *Read from RX FIFO
+ *Round read size to block size,
+ *and make sure data transfer will be done in one command.
+ *
+ * Parameters:
+ *intfhdl	a pointer of intf_hdl
+ *addr		port ID
+ *cnt			size to read
+ *rmem		address to put data
+ *
+ * Return:
+ *_SUCCESS(1)		Success
+ *_FAIL(0)		Fail
+ */
+static u32 sdio_read_port(
+	struct intf_hdl *intfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *mem
+)
+{
+	struct adapter *adapter;
+	struct sdio_data *psdio;
+	struct hal_com_data *hal;
+	s32 err;
+
+	adapter = intfhdl->padapter;
+	psdio = &adapter_to_dvobj(adapter)->intf_data;
+	hal = GET_HAL_DATA(adapter);
+
+	hal_sdio_get_cmd_addr_8723b(adapter, addr, hal->SdioRxFIFOCnt++, &addr);
+
+	if (cnt > psdio->block_transfer_len)
+		cnt = _RND(cnt, psdio->block_transfer_len);
+
+	err = _sd_read(intfhdl, addr, cnt, mem);
+
+	if (err)
+		return _FAIL;
+	return _SUCCESS;
+}
+
+/*
+ * Description:
+ *Write to TX FIFO
+ *Align write size block size,
+ *and make sure data could be written in one command.
+ *
+ * Parameters:
+ *intfhdl	a pointer of intf_hdl
+ *addr		port ID
+ *cnt			size to write
+ *wmem		data pointer to write
+ *
+ * Return:
+ *_SUCCESS(1)		Success
+ *_FAIL(0)		Fail
+ */
+static u32 sdio_write_port(
+	struct intf_hdl *intfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *mem
+)
+{
+	struct adapter *adapter;
+	struct sdio_data *psdio;
+	s32 err;
+	struct xmit_buf *xmitbuf = (struct xmit_buf *)mem;
+
+	adapter = intfhdl->padapter;
+	psdio = &adapter_to_dvobj(adapter)->intf_data;
+
+	if (!adapter->hw_init_completed)
+		return _FAIL;
+
+	cnt = round_up(cnt, 4);
+	hal_sdio_get_cmd_addr_8723b(adapter, addr, cnt >> 2, &addr);
+
+	if (cnt > psdio->block_transfer_len)
+		cnt = _RND(cnt, psdio->block_transfer_len);
+
+	err = sd_write(intfhdl, addr, cnt, xmitbuf->pdata);
+
+	rtw_sctx_done_err(
+		&xmitbuf->sctx,
+		err ? RTW_SCTX_DONE_WRITE_PORT_ERR : RTW_SCTX_DONE_SUCCESS
+	);
+
+	if (err)
+		return _FAIL;
+	return _SUCCESS;
+}
+
+void sdio_set_intf_ops(struct adapter *adapter, struct _io_ops *ops)
+{
+	ops->_read8 = &sdio_read8;
+	ops->_read16 = &sdio_read16;
+	ops->_read32 = &sdio_read32;
+	ops->_read_mem = &sdio_read_mem;
+	ops->_read_port = &sdio_read_port;
+
+	ops->_write8 = &sdio_write8;
+	ops->_write16 = &sdio_write16;
+	ops->_write32 = &sdio_write32;
+	ops->_writeN = &sdio_writeN;
+	ops->_write_mem = &sdio_write_mem;
+	ops->_write_port = &sdio_write_port;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+static s32 _sdio_local_read(
+	struct adapter *adapter,
+	u32 addr,
+	u32 cnt,
+	u8 *buf
+)
+{
+	struct intf_hdl *intfhdl;
+	u8 mac_pwr_ctrl_on;
+	s32 err;
+	u8 *tmpbuf;
+	u32 n;
+
+	intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (!mac_pwr_ctrl_on)
+		return _sd_cmd52_read(intfhdl, addr, cnt, buf);
+
+	n = round_up(cnt, 4);
+	tmpbuf = rtw_malloc(n);
+	if (!tmpbuf)
+		return -1;
+
+	err = _sd_read(intfhdl, addr, n, tmpbuf);
+	if (!err)
+		memcpy(buf, tmpbuf, cnt);
+
+	kfree(tmpbuf);
+
+	return err;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+s32 sdio_local_read(
+	struct adapter *adapter,
+	u32 addr,
+	u32 cnt,
+	u8 *buf
+)
+{
+	struct intf_hdl *intfhdl;
+	u8 mac_pwr_ctrl_on;
+	s32 err;
+	u8 *tmpbuf;
+	u32 n;
+
+	intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	)
+		return sd_cmd52_read(intfhdl, addr, cnt, buf);
+
+	n = round_up(cnt, 4);
+	tmpbuf = rtw_malloc(n);
+	if (!tmpbuf)
+		return -1;
+
+	err = sd_read(intfhdl, addr, n, tmpbuf);
+	if (!err)
+		memcpy(buf, tmpbuf, cnt);
+
+	kfree(tmpbuf);
+
+	return err;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+s32 sdio_local_write(
+	struct adapter *adapter,
+	u32 addr,
+	u32 cnt,
+	u8 *buf
+)
+{
+	struct intf_hdl *intfhdl;
+	u8 mac_pwr_ctrl_on;
+	s32 err;
+	u8 *tmpbuf;
+
+	intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (
+		(!mac_pwr_ctrl_on) ||
+		(adapter_to_pwrctl(adapter)->fw_current_in_ps_mode)
+	)
+		return sd_cmd52_write(intfhdl, addr, cnt, buf);
+
+	tmpbuf = rtw_malloc(cnt);
+	if (!tmpbuf)
+		return -1;
+
+	memcpy(tmpbuf, buf, cnt);
+
+	err = sd_write(intfhdl, addr, cnt, tmpbuf);
+
+	kfree(tmpbuf);
+
+	return err;
+}
+
+u8 SdioLocalCmd52Read1Byte(struct adapter *adapter, u32 addr)
+{
+	u8 val = 0;
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_read(intfhdl, addr, 1, &val);
+
+	return val;
+}
+
+static u16 sdio_local_cmd52_read2byte(struct adapter *adapter, u32 addr)
+{
+	__le16 val = 0;
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_read(intfhdl, addr, 2, (u8 *)&val);
+
+	return le16_to_cpu(val);
+}
+
+static u32 sdio_local_cmd53_read4byte(struct adapter *adapter, u32 addr)
+{
+
+	u8 mac_pwr_ctrl_on;
+	u32 val = 0;
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+	__le32 le_tmp;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	rtw_hal_get_hwreg(adapter, HW_VAR_APFM_ON_MAC, &mac_pwr_ctrl_on);
+	if (!mac_pwr_ctrl_on || adapter_to_pwrctl(adapter)->fw_current_in_ps_mode) {
+		sd_cmd52_read(intfhdl, addr, 4, (u8 *)&le_tmp);
+		val = le32_to_cpu(le_tmp);
+	} else {
+		val = sd_read32(intfhdl, addr, NULL);
+	}
+	return val;
+}
+
+void SdioLocalCmd52Write1Byte(struct adapter *adapter, u32 addr, u8 v)
+{
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_write(intfhdl, addr, 1, &v);
+}
+
+static void sdio_local_cmd52_write4byte(struct adapter *adapter, u32 addr, u32 v)
+{
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+	__le32 le_tmp;
+
+	hal_sdio_get_cmd_addr_8723b(adapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	le_tmp = cpu_to_le32(v);
+	sd_cmd52_write(intfhdl, addr, 4, (u8 *)&le_tmp);
+}
+
+static s32 read_interrupt_8723b_sdio(struct adapter *adapter, u32 *phisr)
+{
+	u32 hisr, himr;
+	u8 val8, hisr_len;
+
+	if (!phisr)
+		return false;
+
+	himr = GET_HAL_DATA(adapter)->sdio_himr;
+
+	/*  decide how many bytes need to be read */
+	hisr_len = 0;
+	while (himr) {
+		hisr_len++;
+		himr >>= 8;
+	}
+
+	hisr = 0;
+	while (hisr_len != 0) {
+		hisr_len--;
+		val8 = SdioLocalCmd52Read1Byte(adapter, SDIO_REG_HISR + hisr_len);
+		hisr |= (val8 << (8 * hisr_len));
+	}
+
+	*phisr = hisr;
+
+	return true;
+}
+
+/*  */
+/*	Description: */
+/*		Initialize SDIO Host Interrupt Mask configuration variables for future use. */
+/*  */
+/*	Assumption: */
+/*		Using SDIO Local register ONLY for configuration. */
+/*  */
+/*	Created by Roger, 2011.02.11. */
+/*  */
+void InitInterrupt8723BSdio(struct adapter *adapter)
+{
+	struct hal_com_data *haldata;
+
+	haldata = GET_HAL_DATA(adapter);
+	haldata->sdio_himr = (u32)(SDIO_HIMR_RX_REQUEST_MSK	|
+				   SDIO_HIMR_AVAL_MSK		|
+				   0);
+}
+
+/*  */
+/*	Description: */
+/*		Initialize System Host Interrupt Mask configuration variables for future use. */
+/*  */
+/*	Created by Roger, 2011.08.03. */
+/*  */
+void InitSysInterrupt8723BSdio(struct adapter *adapter)
+{
+	struct hal_com_data *haldata;
+
+	haldata = GET_HAL_DATA(adapter);
+
+	haldata->SysIntrMask = (0);
+}
+
+/*  */
+/*	Description: */
+/*		Enalbe SDIO Host Interrupt Mask configuration on SDIO local domain. */
+/*  */
+/*	Assumption: */
+/*		1. Using SDIO Local register ONLY for configuration. */
+/*		2. PASSIVE LEVEL */
+/*  */
+/*	Created by Roger, 2011.02.11. */
+/*  */
+void EnableInterrupt8723BSdio(struct adapter *adapter)
+{
+	struct hal_com_data *haldata;
+	__le32 himr;
+	u32 tmp;
+
+	haldata = GET_HAL_DATA(adapter);
+
+	himr = cpu_to_le32(haldata->sdio_himr);
+	sdio_local_write(adapter, SDIO_REG_HIMR, 4, (u8 *)&himr);
+
+	/*  Update current system IMR settings */
+	tmp = rtw_read32(adapter, REG_HSIMR);
+	rtw_write32(adapter, REG_HSIMR, tmp | haldata->SysIntrMask);
+
+	/*  */
+	/*  <Roger_Notes> There are some C2H CMDs have been sent before system interrupt is enabled, e.g., C2H, CPWM. */
+	/*  So we need to clear all C2H events that FW has notified, otherwise FW won't schedule any commands anymore. */
+	/*  2011.10.19. */
+	/*  */
+	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+}
+
+/*  */
+/*	Description: */
+/*		Disable SDIO Host IMR configuration to mask unnecessary interrupt service. */
+/*  */
+/*	Assumption: */
+/*		Using SDIO Local register ONLY for configuration. */
+/*  */
+/*	Created by Roger, 2011.02.11. */
+/*  */
+void DisableInterrupt8723BSdio(struct adapter *adapter)
+{
+	__le32 himr;
+
+	himr = cpu_to_le32(SDIO_HIMR_DISABLED);
+	sdio_local_write(adapter, SDIO_REG_HIMR, 4, (u8 *)&himr);
+}
+
+/*  */
+/*	Description: */
+/*		Using 0x100 to check the power status of FW. */
+/*  */
+/*	Assumption: */
+/*		Using SDIO Local register ONLY for configuration. */
+/*  */
+/*	Created by Isaac, 2013.09.10. */
+/*  */
+u8 CheckIPSStatus(struct adapter *adapter)
+{
+	if (rtw_read8(adapter, 0x100) == 0xEA)
+		return true;
+	else
+		return false;
+}
+
+static struct recv_buf *sd_recv_rxfifo(struct adapter *adapter, u32 size)
+{
+	u32 readsize, ret;
+	u8 *readbuf;
+	struct recv_priv *recv_priv;
+	struct recv_buf	*recvbuf;
+
+	/*  Patch for some SDIO Host 4 bytes issue */
+	/*  ex. RK3188 */
+	readsize = round_up(size, 4);
+
+	/* 3 1. alloc recvbuf */
+	recv_priv = &adapter->recvpriv;
+	recvbuf = rtw_dequeue_recvbuf(&recv_priv->free_recv_buf_queue);
+	if (!recvbuf) {
+		netdev_err(adapter->pnetdev, "%s: alloc recvbuf FAIL!\n",
+			   __func__);
+		return NULL;
+	}
+
+	/* 3 2. alloc skb */
+	if (!recvbuf->pskb) {
+		SIZE_PTR tmpaddr = 0;
+		SIZE_PTR alignment = 0;
+
+		recvbuf->pskb = rtw_skb_alloc(MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
+		if (!recvbuf->pskb)
+			return NULL;
+
+		recvbuf->pskb->dev = adapter->pnetdev;
+
+		tmpaddr = (SIZE_PTR)recvbuf->pskb->data;
+		alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+		skb_reserve(recvbuf->pskb, (RECVBUFF_ALIGN_SZ - alignment));
+	}
+
+	/* 3 3. read data from rxfifo */
+	readbuf = recvbuf->pskb->data;
+	ret = sdio_read_port(&adapter->iopriv.intf, WLAN_RX0FF_DEVICE_ID, readsize, readbuf);
+	if (ret == _FAIL)
+		return NULL;
+
+	/* 3 4. init recvbuf */
+	recvbuf->len = size;
+	recvbuf->phead = recvbuf->pskb->head;
+	recvbuf->pdata = recvbuf->pskb->data;
+	skb_set_tail_pointer(recvbuf->pskb, size);
+	recvbuf->ptail = skb_tail_pointer(recvbuf->pskb);
+	recvbuf->pend = skb_end_pointer(recvbuf->pskb);
+
+	return recvbuf;
+}
+
+static void sd_rxhandler(struct adapter *adapter, struct recv_buf *recvbuf)
+{
+	struct recv_priv *recv_priv;
+	struct __queue *pending_queue;
+
+	recv_priv = &adapter->recvpriv;
+	pending_queue = &recv_priv->recv_buf_pending_queue;
+
+	/* 3 1. enqueue recvbuf */
+	rtw_enqueue_recvbuf(recvbuf, pending_queue);
+
+	/* 3 2. schedule tasklet */
+	tasklet_schedule(&recv_priv->recv_tasklet);
+}
+
+void sd_int_dpc(struct adapter *adapter)
+{
+	struct hal_com_data *hal;
+	struct dvobj_priv *dvobj;
+	struct intf_hdl *intfhdl = &adapter->iopriv.intf;
+	struct pwrctrl_priv *pwrctl;
+
+	hal = GET_HAL_DATA(adapter);
+	dvobj = adapter_to_dvobj(adapter);
+	pwrctl = dvobj_to_pwrctl(dvobj);
+
+	if (hal->sdio_hisr & SDIO_HISR_AVAL) {
+		u8 freepage[4];
+
+		_sdio_local_read(adapter, SDIO_REG_FREE_TXPG, 4, freepage);
+		complete(&(adapter->xmitpriv.xmit_comp));
+	}
+
+	if (hal->sdio_hisr & SDIO_HISR_CPWM1) {
+		del_timer_sync(&(pwrctl->pwr_rpwm_timer));
+
+		SdioLocalCmd52Read1Byte(adapter, SDIO_REG_HCPWM1_8723B);
+
+		_set_workitem(&(pwrctl->cpwm_event));
+	}
+
+	if (hal->sdio_hisr & SDIO_HISR_TXERR) {
+		u8 *status;
+		u32 addr;
+
+		status = rtw_malloc(4);
+		if (status) {
+			addr = REG_TXDMA_STATUS;
+			hal_sdio_get_cmd_addr_8723b(adapter, WLAN_IOREG_DEVICE_ID, addr, &addr);
+			_sd_read(intfhdl, addr, 4, status);
+			_sd_write(intfhdl, addr, 4, status);
+			kfree(status);
+		}
+	}
+
+	if (hal->sdio_hisr & SDIO_HISR_C2HCMD) {
+		struct c2h_evt_hdr_88xx *c2h_evt;
+
+		c2h_evt = rtw_zmalloc(16);
+		if (c2h_evt) {
+			if (c2h_evt_read_88xx(adapter, (u8 *)c2h_evt) == _SUCCESS) {
+				if (c2h_id_filter_ccx_8723b((u8 *)c2h_evt)) {
+					/* Handle CCX report here */
+					rtw_hal_c2h_handler(adapter, (u8 *)c2h_evt);
+					kfree(c2h_evt);
+				} else {
+					rtw_c2h_wk_cmd(adapter, (u8 *)c2h_evt);
+				}
+			} else {
+				kfree(c2h_evt);
+			}
+		} else {
+			/* Error handling for malloc fail */
+			rtw_cbuf_push(adapter->evtpriv.c2h_queue, NULL);
+			_set_workitem(&adapter->evtpriv.c2h_wk);
+		}
+	}
+
+	if (hal->sdio_hisr & SDIO_HISR_RX_REQUEST) {
+		struct recv_buf *recvbuf;
+		int alloc_fail_time = 0;
+		u32 hisr;
+
+		hal->sdio_hisr ^= SDIO_HISR_RX_REQUEST;
+		do {
+			hal->SdioRxFIFOSize = sdio_local_cmd52_read2byte(adapter, SDIO_REG_RX0_REQ_LEN);
+			if (hal->SdioRxFIFOSize != 0) {
+				recvbuf = sd_recv_rxfifo(adapter, hal->SdioRxFIFOSize);
+				if (recvbuf)
+					sd_rxhandler(adapter, recvbuf);
+				else {
+					alloc_fail_time++;
+					if (alloc_fail_time >= 10)
+						break;
+				}
+				hal->SdioRxFIFOSize = 0;
+			} else
+				break;
+
+			hisr = 0;
+			read_interrupt_8723b_sdio(adapter, &hisr);
+			hisr &= SDIO_HISR_RX_REQUEST;
+			if (!hisr)
+				break;
+		} while (1);
+	}
+}
+
+void sd_int_hdl(struct adapter *adapter)
+{
+	struct hal_com_data *hal;
+
+	if (
+		(adapter->bDriverStopped) || (adapter->bSurpriseRemoved)
+	)
+		return;
+
+	hal = GET_HAL_DATA(adapter);
+
+	hal->sdio_hisr = 0;
+	read_interrupt_8723b_sdio(adapter, &hal->sdio_hisr);
+
+	if (hal->sdio_hisr & hal->sdio_himr) {
+		u32 v32;
+
+		hal->sdio_hisr &= hal->sdio_himr;
+
+		/*  clear HISR */
+		v32 = hal->sdio_hisr & MASK_SDIO_HISR_CLEAR;
+		if (v32)
+			sdio_local_cmd52_write4byte(adapter, SDIO_REG_HISR, v32);
+
+		sd_int_dpc(adapter);
+	}
+}
+
+/*  */
+/*	Description: */
+/*		Query SDIO Local register to query current the number of Free TxPacketBuffer page. */
+/*  */
+/*	Assumption: */
+/*		1. Running at PASSIVE_LEVEL */
+/*		2. RT_TX_SPINLOCK is NOT acquired. */
+/*  */
+/*	Created by Roger, 2011.01.28. */
+/*  */
+u8 HalQueryTxBufferStatus8723BSdio(struct adapter *adapter)
+{
+	struct hal_com_data *hal;
+	u32 numof_free_page;
+
+	hal = GET_HAL_DATA(adapter);
+
+	numof_free_page = sdio_local_cmd53_read4byte(adapter, SDIO_REG_FREE_TXPG);
+
+	memcpy(hal->SdioTxFIFOFreePage, &numof_free_page, 4);
+
+	return true;
+}
+
+/*  */
+/*	Description: */
+/*		Query SDIO Local register to get the current number of TX OQT Free Space. */
+/*  */
+void HalQueryTxOQTBufferStatus8723BSdio(struct adapter *adapter)
+{
+	struct hal_com_data *haldata = GET_HAL_DATA(adapter);
+
+	haldata->SdioTxOQTFreeSpace = SdioLocalCmd52Read1Byte(adapter, SDIO_REG_OQT_FREE_PG);
+}
+
+
diff --git a/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h b/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h
new file mode 100644
index 000000000..586a3dabc
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h
@@ -0,0 +1,1112 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*****************************************************************************
+ *
+ * Module:	__INC_HAL8192CPHYREG_H
+ *
+ *
+ * Note:	1. Define PMAC/BB register map
+ *		2. Define RF register map
+ *		3. PMAC/BB register bit mask.
+ *		4. RF reg bit mask.
+ *		5. Other BB/RF relative definition.
+ *
+ *
+ * Export:	Constants, macro, functions(API), global variables(None).
+ *
+ * Abbrev:
+ *
+ * History:
+ *	Data		Who		Remark
+ *      08/07/2007  MHC		1. Porting from 9x series PHYCFG.h.
+ *						2. Reorganize code architecture.
+ *09/25/2008	MH		1. Add RL6052 register definition
+ *
+ *****************************************************************************/
+#ifndef __INC_HAL8192CPHYREG_H
+#define __INC_HAL8192CPHYREG_H
+
+
+/*--------------------------Define Parameters-------------------------------*/
+
+/*  */
+/*        8192S Register offset definition */
+/*  */
+
+/*  */
+/*  BB-PHY register PMAC 0x100 PHY 0x800 - 0xEFF */
+/*  1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF */
+/*  2. 0x800/0x900/0xA00/0xC00/0xD00/0xE00 */
+/*  3. RF register 0x00-2E */
+/*  4. Bit Mask for BB/RF register */
+/*  5. Other definition for BB/RF R/W */
+/*  */
+
+
+/*  */
+/*  1. PMAC duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF */
+/*  1. Page1(0x100) */
+/*  */
+#define		rPMAC_Reset					0x100
+#define		rPMAC_TxStart					0x104
+#define		rPMAC_TxLegacySIG				0x108
+#define		rPMAC_TxHTSIG1				0x10c
+#define		rPMAC_TxHTSIG2				0x110
+#define		rPMAC_PHYDebug				0x114
+#define		rPMAC_TxPacketNum				0x118
+#define		rPMAC_TxIdle					0x11c
+#define		rPMAC_TxMACHeader0			0x120
+#define		rPMAC_TxMACHeader1			0x124
+#define		rPMAC_TxMACHeader2			0x128
+#define		rPMAC_TxMACHeader3			0x12c
+#define		rPMAC_TxMACHeader4			0x130
+#define		rPMAC_TxMACHeader5			0x134
+#define		rPMAC_TxDataType				0x138
+#define		rPMAC_TxRandomSeed			0x13c
+#define		rPMAC_CCKPLCPPreamble			0x140
+#define		rPMAC_CCKPLCPHeader			0x144
+#define		rPMAC_CCKCRC16				0x148
+#define		rPMAC_OFDMRxCRC32OK			0x170
+#define		rPMAC_OFDMRxCRC32Er			0x174
+#define		rPMAC_OFDMRxParityEr			0x178
+#define		rPMAC_OFDMRxCRC8Er			0x17c
+#define		rPMAC_CCKCRxRC16Er			0x180
+#define		rPMAC_CCKCRxRC32Er			0x184
+#define		rPMAC_CCKCRxRC32OK			0x188
+#define		rPMAC_TxStatus					0x18c
+
+/*  */
+/*  2. Page2(0x200) */
+/*  */
+/*  The following two definition are only used for USB interface. */
+#define		RF_BB_CMD_ADDR				0x02c0	/*  RF/BB read/write command address. */
+#define		RF_BB_CMD_DATA				0x02c4	/*  RF/BB read/write command data. */
+
+/*  */
+/*  3. Page8(0x800) */
+/*  */
+#define		rFPGA0_RFMOD				0x800	/* RF mode & CCK TxSC  RF BW Setting?? */
+
+#define		rFPGA0_TxInfo				0x804	/*  Status report?? */
+#define		rFPGA0_PSDFunction			0x808
+
+#define		rFPGA0_TxGainStage			0x80c	/*  Set TX PWR init gain? */
+
+#define		rFPGA0_RFTiming1			0x810	/*  Useless now */
+#define		rFPGA0_RFTiming2			0x814
+
+#define		rFPGA0_XA_HSSIParameter1		0x820	/*  RF 3 wire register */
+#define		rFPGA0_XA_HSSIParameter2		0x824
+#define		rFPGA0_XB_HSSIParameter1		0x828
+#define		rFPGA0_XB_HSSIParameter2		0x82c
+#define		rTxAGC_B_Rate18_06				0x830
+#define		rTxAGC_B_Rate54_24				0x834
+#define		rTxAGC_B_CCK1_55_Mcs32		0x838
+#define		rTxAGC_B_Mcs03_Mcs00			0x83c
+
+#define		rTxAGC_B_Mcs07_Mcs04			0x848
+
+#define		rFPGA0_XA_LSSIParameter		0x840
+#define		rFPGA0_XB_LSSIParameter		0x844
+
+#define		rFPGA0_RFWakeUpParameter		0x850	/*  Useless now */
+#define		rFPGA0_RFSleepUpParameter		0x854
+
+#define		rFPGA0_XAB_SwitchControl		0x858	/*  RF Channel switch */
+#define		rFPGA0_XCD_SwitchControl		0x85c
+
+#define		rFPGA0_XA_RFInterfaceOE		0x860	/*  RF Channel switch */
+#define		rFPGA0_XB_RFInterfaceOE		0x864
+
+#define		rTxAGC_B_CCK11_A_CCK2_11		0x86c
+
+#define		rFPGA0_XAB_RFInterfaceSW		0x870	/*  RF Interface Software Control */
+#define		rFPGA0_XCD_RFInterfaceSW		0x874
+
+#define		rFPGA0_XAB_RFParameter		0x878	/*  RF Parameter */
+#define		rFPGA0_XCD_RFParameter		0x87c
+
+#define		rFPGA0_AnalogParameter1		0x880	/*  Crystal cap setting RF-R/W protection for parameter4?? */
+#define		rFPGA0_AnalogParameter2		0x884
+#define		rFPGA0_AnalogParameter3		0x888	/*  Useless now */
+#define		rFPGA0_AnalogParameter4		0x88c
+
+#define		rFPGA0_XA_LSSIReadBack		0x8a0	/*  Transceiver LSSI Readback */
+#define		rFPGA0_XB_LSSIReadBack		0x8a4
+#define		rFPGA0_XC_LSSIReadBack		0x8a8
+#define		rFPGA0_XD_LSSIReadBack		0x8ac
+
+#define		rFPGA0_PSDReport				0x8b4	/*  Useless now */
+#define		TransceiverA_HSPI_Readback	0x8b8	/*  Transceiver A HSPI Readback */
+#define		TransceiverB_HSPI_Readback	0x8bc	/*  Transceiver B HSPI Readback */
+#define		rFPGA0_XAB_RFInterfaceRB		0x8e0	/*  Useless now  RF Interface Readback Value */
+#define		rFPGA0_XCD_RFInterfaceRB		0x8e4	/*  Useless now */
+
+/*  */
+/*  4. Page9(0x900) */
+/*  */
+#define		rFPGA1_RFMOD				0x900	/* RF mode & OFDM TxSC  RF BW Setting?? */
+
+#define		rFPGA1_TxBlock				0x904	/*  Useless now */
+#define		rFPGA1_DebugSelect			0x908	/*  Useless now */
+#define		rFPGA1_TxInfo				0x90c	/*  Useless now  Status report?? */
+#define		rS0S1_PathSwitch			0x948
+
+/*  */
+/*  5. PageA(0xA00) */
+/*  */
+/*  Set Control channel to upper or lower. These settings are required only for 40MHz */
+#define		rCCK0_System				0xa00
+
+#define		rCCK0_AFESetting			0xa04	/*  Disable init gain now Select RX path by RSSI */
+#define		rCCK0_CCA					0xa08	/*  Disable init gain now Init gain */
+
+#define		rCCK0_RxAGC1				0xa0c	/* AGC default value, saturation level Antenna Diversity, RX AGC, LNA Threshold, RX LNA Threshold useless now. Not the same as 90 series */
+#define		rCCK0_RxAGC2				0xa10	/* AGC & DAGC */
+
+#define		rCCK0_RxHP					0xa14
+
+#define		rCCK0_DSPParameter1		0xa18	/* Timing recovery & Channel estimation threshold */
+#define		rCCK0_DSPParameter2		0xa1c	/* SQ threshold */
+
+#define		rCCK0_TxFilter1				0xa20
+#define		rCCK0_TxFilter2				0xa24
+#define		rCCK0_DebugPort			0xa28	/* debug port and Tx filter3 */
+#define		rCCK0_FalseAlarmReport		0xa2c	/* 0xa2d	useless now 0xa30-a4f channel report */
+#define		rCCK0_TRSSIReport			0xa50
+#define		rCCK0_RxReport				0xa54  /* 0xa57 */
+#define		rCCK0_FACounterLower		0xa5c  /* 0xa5b */
+#define		rCCK0_FACounterUpper		0xa58  /* 0xa5c */
+/*  */
+/*  PageB(0xB00) */
+/*  */
+#define		rPdp_AntA				0xb00
+#define		rPdp_AntA_4				0xb04
+#define		rConfig_Pmpd_AntA			0xb28
+#define		rConfig_AntA				0xb68
+#define		rConfig_AntB				0xb6c
+#define		rPdp_AntB					0xb70
+#define		rPdp_AntB_4				0xb74
+#define		rConfig_Pmpd_AntB			0xb98
+#define		rAPK						0xbd8
+
+/*  */
+/*  6. PageC(0xC00) */
+/*  */
+#define		rOFDM0_LSTF				0xc00
+
+#define		rOFDM0_TRxPathEnable		0xc04
+#define		rOFDM0_TRMuxPar			0xc08
+#define		rOFDM0_TRSWIsolation		0xc0c
+
+#define		rOFDM0_XARxAFE			0xc10  /* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define		rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
+#define		rOFDM0_XBRxAFE				0xc18
+#define		rOFDM0_XBRxIQImbalance		0xc1c
+#define		rOFDM0_XCRxAFE				0xc20
+#define		rOFDM0_XCRxIQImbalance		0xc24
+#define		rOFDM0_XDRxAFE				0xc28
+#define		rOFDM0_XDRxIQImbalance		0xc2c
+
+#define		rOFDM0_RxDetector1			0xc30  /* PD, BW & SBD	DM tune init gain */
+#define		rOFDM0_RxDetector2			0xc34  /* SBD & Fame Sync. */
+#define		rOFDM0_RxDetector3			0xc38  /* Frame Sync. */
+#define		rOFDM0_RxDetector4			0xc3c  /* PD, SBD, Frame Sync & Short-GI */
+
+#define		rOFDM0_RxDSP				0xc40  /* Rx Sync Path */
+#define		rOFDM0_CFOandDAGC		0xc44  /* CFO & DAGC */
+#define		rOFDM0_CCADropThreshold	0xc48 /* CCA Drop threshold */
+#define		rOFDM0_ECCAThreshold		0xc4c /*  energy CCA */
+
+#define		rOFDM0_XAAGCCore1			0xc50	/*  DIG */
+#define		rOFDM0_XAAGCCore2			0xc54
+#define		rOFDM0_XBAGCCore1			0xc58
+#define		rOFDM0_XBAGCCore2			0xc5c
+#define		rOFDM0_XCAGCCore1			0xc60
+#define		rOFDM0_XCAGCCore2			0xc64
+#define		rOFDM0_XDAGCCore1			0xc68
+#define		rOFDM0_XDAGCCore2			0xc6c
+
+#define		rOFDM0_AGCParameter1			0xc70
+#define		rOFDM0_AGCParameter2			0xc74
+#define		rOFDM0_AGCRSSITable			0xc78
+#define		rOFDM0_HTSTFAGC				0xc7c
+
+#define		rOFDM0_XATxIQImbalance		0xc80	/*  TX PWR TRACK and DIG */
+#define		rOFDM0_XATxAFE				0xc84
+#define		rOFDM0_XBTxIQImbalance		0xc88
+#define		rOFDM0_XBTxAFE				0xc8c
+#define		rOFDM0_XCTxIQImbalance		0xc90
+#define		rOFDM0_XCTxAFE					0xc94
+#define		rOFDM0_XDTxIQImbalance		0xc98
+#define		rOFDM0_XDTxAFE				0xc9c
+
+#define		rOFDM0_RxIQExtAnta			0xca0
+#define		rOFDM0_TxCoeff1				0xca4
+#define		rOFDM0_TxCoeff2				0xca8
+#define		rOFDM0_TxCoeff3				0xcac
+#define		rOFDM0_TxCoeff4				0xcb0
+#define		rOFDM0_TxCoeff5				0xcb4
+#define		rOFDM0_TxCoeff6				0xcb8
+#define		rOFDM0_RxHPParameter			0xce0
+#define		rOFDM0_TxPseudoNoiseWgt		0xce4
+#define		rOFDM0_FrameSync				0xcf0
+#define		rOFDM0_DFSReport				0xcf4
+
+/*  */
+/*  7. PageD(0xD00) */
+/*  */
+#define		rOFDM1_LSTF					0xd00
+#define		rOFDM1_TRxPathEnable			0xd04
+
+#define		rOFDM1_CFO						0xd08	/*  No setting now */
+#define		rOFDM1_CSI1					0xd10
+#define		rOFDM1_SBD						0xd14
+#define		rOFDM1_CSI2					0xd18
+#define		rOFDM1_CFOTracking			0xd2c
+#define		rOFDM1_TRxMesaure1			0xd34
+#define		rOFDM1_IntfDet					0xd3c
+#define		rOFDM1_PseudoNoiseStateAB		0xd50
+#define		rOFDM1_PseudoNoiseStateCD		0xd54
+#define		rOFDM1_RxPseudoNoiseWgt		0xd58
+
+#define		rOFDM_PHYCounter1				0xda0  /* cca, parity fail */
+#define		rOFDM_PHYCounter2				0xda4  /* rate illegal, crc8 fail */
+#define		rOFDM_PHYCounter3				0xda8  /* MCS not support */
+
+#define		rOFDM_ShortCFOAB				0xdac	/*  No setting now */
+#define		rOFDM_ShortCFOCD				0xdb0
+#define		rOFDM_LongCFOAB				0xdb4
+#define		rOFDM_LongCFOCD				0xdb8
+#define		rOFDM_TailCFOAB				0xdbc
+#define		rOFDM_TailCFOCD				0xdc0
+#define		rOFDM_PWMeasure1			0xdc4
+#define		rOFDM_PWMeasure2			0xdc8
+#define		rOFDM_BWReport				0xdcc
+#define		rOFDM_AGCReport				0xdd0
+#define		rOFDM_RxSNR					0xdd4
+#define		rOFDM_RxEVMCSI				0xdd8
+#define		rOFDM_SIGReport				0xddc
+
+
+/*  */
+/*  8. PageE(0xE00) */
+/*  */
+#define		rTxAGC_A_Rate18_06			0xe00
+#define		rTxAGC_A_Rate54_24			0xe04
+#define		rTxAGC_A_CCK1_Mcs32			0xe08
+#define		rTxAGC_A_Mcs03_Mcs00			0xe10
+#define		rTxAGC_A_Mcs07_Mcs04			0xe14
+
+#define		rFPGA0_IQK					0xe28
+#define		rTx_IQK_Tone_A				0xe30
+#define		rRx_IQK_Tone_A				0xe34
+#define		rTx_IQK_PI_A					0xe38
+#define		rRx_IQK_PI_A					0xe3c
+
+#define		rTx_IQK							0xe40
+#define		rRx_IQK						0xe44
+#define		rIQK_AGC_Pts					0xe48
+#define		rIQK_AGC_Rsp					0xe4c
+#define		rTx_IQK_Tone_B				0xe50
+#define		rRx_IQK_Tone_B				0xe54
+#define		rTx_IQK_PI_B					0xe58
+#define		rRx_IQK_PI_B					0xe5c
+#define		rIQK_AGC_Cont				0xe60
+
+#define		rBlue_Tooth					0xe6c
+#define		rRx_Wait_CCA					0xe70
+#define		rTx_CCK_RFON					0xe74
+#define		rTx_CCK_BBON				0xe78
+#define		rTx_OFDM_RFON				0xe7c
+#define		rTx_OFDM_BBON				0xe80
+#define		rTx_To_Rx					0xe84
+#define		rTx_To_Tx					0xe88
+#define		rRx_CCK						0xe8c
+
+#define		rTx_Power_Before_IQK_A		0xe94
+#define		rTx_Power_After_IQK_A			0xe9c
+
+#define		rRx_Power_Before_IQK_A		0xea0
+#define		rRx_Power_Before_IQK_A_2		0xea4
+#define		rRx_Power_After_IQK_A			0xea8
+#define		rRx_Power_After_IQK_A_2		0xeac
+
+#define		rTx_Power_Before_IQK_B		0xeb4
+#define		rTx_Power_After_IQK_B			0xebc
+
+#define		rRx_Power_Before_IQK_B		0xec0
+#define		rRx_Power_Before_IQK_B_2		0xec4
+#define		rRx_Power_After_IQK_B			0xec8
+#define		rRx_Power_After_IQK_B_2		0xecc
+
+#define		rRx_OFDM					0xed0
+#define		rRx_Wait_RIFS				0xed4
+#define		rRx_TO_Rx					0xed8
+#define		rStandby						0xedc
+#define		rSleep						0xee0
+#define		rPMPD_ANAEN				0xeec
+
+/*  */
+/*  7. RF Register 0x00-0x2E (RF 8256) */
+/*     RF-0222D 0x00-3F */
+/*  */
+/* Zebra1 */
+#define		rZebra1_HSSIEnable				0x0	/*  Useless now */
+#define		rZebra1_TRxEnable1				0x1
+#define		rZebra1_TRxEnable2				0x2
+#define		rZebra1_AGC					0x4
+#define		rZebra1_ChargePump			0x5
+#define		rZebra1_Channel				0x7	/*  RF channel switch */
+
+/* endif */
+#define		rZebra1_TxGain					0x8	/*  Useless now */
+#define		rZebra1_TxLPF					0x9
+#define		rZebra1_RxLPF					0xb
+#define		rZebra1_RxHPFCorner			0xc
+
+/* Zebra4 */
+#define		rGlobalCtrl						0	/*  Useless now */
+#define		rRTL8256_TxLPF					19
+#define		rRTL8256_RxLPF					11
+
+/* RTL8258 */
+#define		rRTL8258_TxLPF					0x11	/*  Useless now */
+#define		rRTL8258_RxLPF					0x13
+#define		rRTL8258_RSSILPF				0xa
+
+/*  */
+/*  RL6052 Register definition */
+/*  */
+#define		RF_AC						0x00	/*  */
+
+#define		RF_IQADJ_G1				0x01	/*  */
+#define		RF_IQADJ_G2				0x02	/*  */
+#define		RF_BS_PA_APSET_G1_G4		0x03
+#define		RF_BS_PA_APSET_G5_G8		0x04
+#define		RF_POW_TRSW				0x05	/*  */
+
+#define		RF_GAIN_RX					0x06	/*  */
+#define		RF_GAIN_TX					0x07	/*  */
+
+#define		RF_TXM_IDAC				0x08	/*  */
+#define		RF_IPA_G					0x09	/*  */
+#define		RF_TXBIAS_G				0x0A
+#define		RF_TXPA_AG					0x0B
+#define		RF_IPA_A					0x0C	/*  */
+#define		RF_TXBIAS_A				0x0D
+#define		RF_BS_PA_APSET_G9_G11	0x0E
+#define		RF_BS_IQGEN				0x0F	/*  */
+
+#define		RF_MODE1					0x10	/*  */
+#define		RF_MODE2					0x11	/*  */
+
+#define		RF_RX_AGC_HP				0x12	/*  */
+#define		RF_TX_AGC					0x13	/*  */
+#define		RF_BIAS						0x14	/*  */
+#define		RF_IPA						0x15	/*  */
+#define		RF_TXBIAS					0x16 /*  */
+#define		RF_POW_ABILITY			0x17	/*  */
+#define		RF_MODE_AG				0x18	/*  */
+#define		rRfChannel					0x18	/*  RF channel and BW switch */
+#define		RF_CHNLBW					0x18	/*  RF channel and BW switch */
+#define		RF_TOP						0x19	/*  */
+
+#define		RF_RX_G1					0x1A	/*  */
+#define		RF_RX_G2					0x1B	/*  */
+
+#define		RF_RX_BB2					0x1C	/*  */
+#define		RF_RX_BB1					0x1D	/*  */
+
+#define		RF_RCK1					0x1E	/*  */
+#define		RF_RCK2					0x1F	/*  */
+
+#define		RF_TX_G1					0x20	/*  */
+#define		RF_TX_G2					0x21	/*  */
+#define		RF_TX_G3					0x22	/*  */
+
+#define		RF_TX_BB1					0x23	/*  */
+
+#define		RF_T_METER					0x24	/*  */
+
+#define		RF_SYN_G1					0x25	/*  RF TX Power control */
+#define		RF_SYN_G2					0x26	/*  RF TX Power control */
+#define		RF_SYN_G3					0x27	/*  RF TX Power control */
+#define		RF_SYN_G4					0x28	/*  RF TX Power control */
+#define		RF_SYN_G5					0x29	/*  RF TX Power control */
+#define		RF_SYN_G6					0x2A	/*  RF TX Power control */
+#define		RF_SYN_G7					0x2B	/*  RF TX Power control */
+#define		RF_SYN_G8					0x2C	/*  RF TX Power control */
+
+#define		RF_RCK_OS					0x30	/*  RF TX PA control */
+
+#define		RF_TXPA_G1					0x31	/*  RF TX PA control */
+#define		RF_TXPA_G2					0x32	/*  RF TX PA control */
+#define		RF_TXPA_G3					0x33	/*  RF TX PA control */
+#define		RF_TX_BIAS_A				0x35
+#define		RF_TX_BIAS_D				0x36
+#define		RF_LOBF_9					0x38
+#define		RF_RXRF_A3					0x3C	/*  */
+#define		RF_TRSW						0x3F
+
+#define		RF_TXRF_A2					0x41
+#define		RF_TXPA_G4					0x46
+#define		RF_TXPA_A4					0x4B
+#define		RF_0x52						0x52
+#define		RF_WE_LUT					0xEF
+#define		RF_S0S1						0xB0
+
+/*  */
+/* Bit Mask */
+/*  */
+/*  1. Page1(0x100) */
+#define		bBBResetB						0x100	/*  Useless now? */
+#define		bGlobalResetB					0x200
+#define		bOFDMTxStart					0x4
+#define		bCCKTxStart						0x8
+#define		bCRC32Debug					0x100
+#define		bPMACLoopback					0x10
+#define		bTxLSIG							0xffffff
+#define		bOFDMTxRate					0xf
+#define		bOFDMTxReserved				0x10
+#define		bOFDMTxLength					0x1ffe0
+#define		bOFDMTxParity					0x20000
+#define		bTxHTSIG1						0xffffff
+#define		bTxHTMCSRate					0x7f
+#define		bTxHTBW						0x80
+#define		bTxHTLength					0xffff00
+#define		bTxHTSIG2						0xffffff
+#define		bTxHTSmoothing					0x1
+#define		bTxHTSounding					0x2
+#define		bTxHTReserved					0x4
+#define		bTxHTAggreation				0x8
+#define		bTxHTSTBC						0x30
+#define		bTxHTAdvanceCoding			0x40
+#define		bTxHTShortGI					0x80
+#define		bTxHTNumberHT_LTF			0x300
+#define		bTxHTCRC8						0x3fc00
+#define		bCounterReset					0x10000
+#define		bNumOfOFDMTx					0xffff
+#define		bNumOfCCKTx					0xffff0000
+#define		bTxIdleInterval					0xffff
+#define		bOFDMService					0xffff0000
+#define		bTxMACHeader					0xffffffff
+#define		bTxDataInit						0xff
+#define		bTxHTMode						0x100
+#define		bTxDataType					0x30000
+#define		bTxRandomSeed					0xffffffff
+#define		bCCKTxPreamble					0x1
+#define		bCCKTxSFD						0xffff0000
+#define		bCCKTxSIG						0xff
+#define		bCCKTxService					0xff00
+#define		bCCKLengthExt					0x8000
+#define		bCCKTxLength					0xffff0000
+#define		bCCKTxCRC16					0xffff
+#define		bCCKTxStatus					0x1
+#define		bOFDMTxStatus					0x2
+
+#define			IS_BB_REG_OFFSET_92S(_Offset)		((_Offset >= 0x800) && (_Offset <= 0xfff))
+
+/*  2. Page8(0x800) */
+#define		bRFMOD							0x1	/*  Reg 0x800 rFPGA0_RFMOD */
+#define		bJapanMode						0x2
+#define		bCCKTxSC						0x30
+#define		bCCKEn							0x1000000
+#define		bOFDMEn						0x2000000
+
+#define		bOFDMRxADCPhase				0x10000	/*  Useless now */
+#define		bOFDMTxDACPhase				0x40000
+#define		bXATxAGC					0x3f
+
+#define		bAntennaSelect				0x0300
+
+#define		bXBTxAGC					0xf00	/*  Reg 80c rFPGA0_TxGainStage */
+#define		bXCTxAGC					0xf000
+#define		bXDTxAGC					0xf0000
+
+#define		bPAStart					0xf0000000	/*  Useless now */
+#define		bTRStart					0x00f00000
+#define		bRFStart					0x0000f000
+#define		bBBStart					0x000000f0
+#define		bBBCCKStart				0x0000000f
+#define		bPAEnd						0xf          /* Reg0x814 */
+#define		bTREnd						0x0f000000
+#define		bRFEnd						0x000f0000
+#define		bCCAMask					0x000000f0   /* T2R */
+#define		bR2RCCAMask				0x00000f00
+#define		bHSSI_R2TDelay				0xf8000000
+#define		bHSSI_T2RDelay				0xf80000
+#define		bContTxHSSI				0x400     /* chane gain at continue Tx */
+#define		bIGFromCCK				0x200
+#define		bAGCAddress				0x3f
+#define		bRxHPTx						0x7000
+#define		bRxHPT2R					0x38000
+#define		bRxHPCCKIni				0xc0000
+#define		bAGCTxCode				0xc00000
+#define		bAGCRxCode				0x300000
+
+#define		b3WireDataLength			0x800	/*  Reg 0x820~84f rFPGA0_XA_HSSIParameter1 */
+#define		b3WireAddressLength			0x400
+
+#define		b3WireRFPowerDown			0x1	/*  Useless now */
+/* define bHWSISelect				0x8 */
+#define		b2GPAPEPolarity				0x80000000
+#define		bRFSW_TxDefaultAnt			0x3
+#define		bRFSW_TxOptionAnt			0x30
+#define		bRFSW_RxDefaultAnt			0x300
+#define		bRFSW_RxOptionAnt			0x3000
+#define		bRFSI_3WireData				0x1
+#define		bRFSI_3WireClock			0x2
+#define		bRFSI_3WireLoad				0x4
+#define		bRFSI_3WireRW				0x8
+#define		bRFSI_3Wire					0xf
+
+#define		bRFSI_RFENV				0x10	/*  Reg 0x870 rFPGA0_XAB_RFInterfaceSW */
+
+#define		bRFSI_TRSW				0x20	/*  Useless now */
+#define		bRFSI_TRSWB				0x40
+#define		bRFSI_ANTSW				0x100
+#define		bRFSI_ANTSWB				0x200
+#define		bRFSI_PAPE					0x400
+#define		bBandSelect					0x1
+#define		bHTSIG2_GI					0x80
+#define		bHTSIG2_Smoothing			0x01
+#define		bHTSIG2_Sounding			0x02
+#define		bHTSIG2_Aggreaton			0x08
+#define		bHTSIG2_STBC				0x30
+#define		bHTSIG2_AdvCoding			0x40
+#define		bHTSIG2_NumOfHTLTF		0x300
+#define		bHTSIG2_CRC8				0x3fc
+#define		bHTSIG1_MCS				0x7f
+#define		bHTSIG1_BandWidth			0x80
+#define		bHTSIG1_HTLength			0xffff
+#define		bLSIG_Rate					0xf
+#define		bLSIG_Reserved				0x10
+#define		bLSIG_Length				0x1fffe
+#define		bLSIG_Parity					0x20
+#define		bCCKRxPhase				0x4
+
+#define		bLSSIReadAddress			0x7f800000   /*  T65 RF */
+
+#define		bLSSIReadEdge				0x80000000   /* LSSI "Read" edge signal */
+
+#define		bLSSIReadBackData			0xfffff		/*  T65 RF */
+
+#define		bLSSIReadOKFlag				0x1000	/*  Useless now */
+#define		bCCKSampleRate				0x8       /* 0: 44MHz, 1:88MHz */
+#define		bRegulator0Standby			0x1
+#define		bRegulatorPLLStandby			0x2
+#define		bRegulator1Standby			0x4
+#define		bPLLPowerUp				0x8
+#define		bDPLLPowerUp				0x10
+#define		bDA10PowerUp				0x20
+#define		bAD7PowerUp				0x200
+#define		bDA6PowerUp				0x2000
+#define		bXtalPowerUp				0x4000
+#define		b40MDClkPowerUP				0x8000
+#define		bDA6DebugMode				0x20000
+#define		bDA6Swing					0x380000
+
+#define		bADClkPhase				0x4000000	/*  Reg 0x880 rFPGA0_AnalogParameter1 20/40 CCK support switch 40/80 BB MHZ */
+
+#define		b80MClkDelay				0x18000000	/*  Useless */
+#define		bAFEWatchDogEnable			0x20000000
+
+#define		bXtalCap01					0xc0000000	/*  Reg 0x884 rFPGA0_AnalogParameter2 Crystal cap */
+#define		bXtalCap23					0x3
+#define		bXtalCap92x					0x0f000000
+#define			bXtalCap					0x0f000000
+
+#define		bIntDifClkEnable			0x400	/*  Useless */
+#define		bExtSigClkEnable			0x800
+#define		bBandgapMbiasPowerUp		0x10000
+#define		bAD11SHGain				0xc0000
+#define		bAD11InputRange				0x700000
+#define		bAD11OPCurrent				0x3800000
+#define		bIPathLoopback				0x4000000
+#define		bQPathLoopback				0x8000000
+#define		bAFELoopback				0x10000000
+#define		bDA10Swing				0x7e0
+#define		bDA10Reverse				0x800
+#define		bDAClkSource				0x1000
+#define		bAD7InputRange				0x6000
+#define		bAD7Gain					0x38000
+#define		bAD7OutputCMMode			0x40000
+#define		bAD7InputCMMode				0x380000
+#define		bAD7Current					0xc00000
+#define		bRegulatorAdjust			0x7000000
+#define		bAD11PowerUpAtTx			0x1
+#define		bDA10PSAtTx				0x10
+#define		bAD11PowerUpAtRx			0x100
+#define		bDA10PSAtRx				0x1000
+#define		bCCKRxAGCFormat				0x200
+#define		bPSDFFTSamplepPoint			0xc000
+#define		bPSDAverageNum				0x3000
+#define		bIQPathControl				0xc00
+#define		bPSDFreq					0x3ff
+#define		bPSDAntennaPath				0x30
+#define		bPSDIQSwitch				0x40
+#define		bPSDRxTrigger				0x400000
+#define		bPSDTxTrigger				0x80000000
+#define		bPSDSineToneScale			0x7f000000
+#define		bPSDReport					0xffff
+
+/*  3. Page9(0x900) */
+#define		bOFDMTxSC				0x30000000	/*  Useless */
+#define		bCCKTxOn					0x1
+#define		bOFDMTxOn				0x2
+#define		bDebugPage				0xfff  /* reset debug page and also HWord, LWord */
+#define		bDebugItem				0xff   /* reset debug page and LWord */
+#define		bAntL					0x10
+#define		bAntNonHT					0x100
+#define		bAntHT1					0x1000
+#define		bAntHT2						0x10000
+#define		bAntHT1S1					0x100000
+#define		bAntNonHTS1				0x1000000
+
+/*  4. PageA(0xA00) */
+#define		bCCKBBMode				0x3	/*  Useless */
+#define		bCCKTxPowerSaving		0x80
+#define		bCCKRxPowerSaving		0x40
+
+#define		bCCKSideBand			0x10	/*  Reg 0xa00 rCCK0_System 20/40 switch */
+
+#define		bCCKScramble			0x8	/*  Useless */
+#define		bCCKAntDiversity		0x8000
+#define		bCCKCarrierRecovery		0x4000
+#define		bCCKTxRate				0x3000
+#define		bCCKDCCancel			0x0800
+#define		bCCKISICancel			0x0400
+#define		bCCKMatchFilter			0x0200
+#define		bCCKEqualizer			0x0100
+#define		bCCKPreambleDetect		0x800000
+#define		bCCKFastFalseCCA		0x400000
+#define		bCCKChEstStart			0x300000
+#define		bCCKCCACount			0x080000
+#define		bCCKcs_lim				0x070000
+#define		bCCKBistMode			0x80000000
+#define		bCCKCCAMask			0x40000000
+#define		bCCKTxDACPhase		0x4
+#define		bCCKRxADCPhase		0x20000000   /* r_rx_clk */
+#define		bCCKr_cp_mode0		0x0100
+#define		bCCKTxDCOffset			0xf0
+#define		bCCKRxDCOffset			0xf
+#define		bCCKCCAMode			0xc000
+#define		bCCKFalseCS_lim			0x3f00
+#define		bCCKCS_ratio			0xc00000
+#define		bCCKCorgBit_sel			0x300000
+#define		bCCKPD_lim				0x0f0000
+#define		bCCKNewCCA			0x80000000
+#define		bCCKRxHPofIG			0x8000
+#define		bCCKRxIG				0x7f00
+#define		bCCKLNAPolarity			0x800000
+#define		bCCKRx1stGain			0x7f0000
+#define		bCCKRFExtend			0x20000000 /* CCK Rx Iinital gain polarity */
+#define		bCCKRxAGCSatLevel		0x1f000000
+#define		bCCKRxAGCSatCount		0xe0
+#define		bCCKRxRFSettle			0x1f       /* AGCsamp_dly */
+#define		bCCKFixedRxAGC			0x8000
+#define		bCCKAntennaPolarity		0x2000
+#define		bCCKTxFilterType		0x0c00
+#define		bCCKRxAGCReportType	0x0300
+#define		bCCKRxDAGCEn			0x80000000
+#define		bCCKRxDAGCPeriod		0x20000000
+#define		bCCKRxDAGCSatLevel		0x1f000000
+#define		bCCKTimingRecovery		0x800000
+#define		bCCKTxC0				0x3f0000
+#define		bCCKTxC1				0x3f000000
+#define		bCCKTxC2				0x3f
+#define		bCCKTxC3				0x3f00
+#define		bCCKTxC4				0x3f0000
+#define		bCCKTxC5				0x3f000000
+#define		bCCKTxC6				0x3f
+#define		bCCKTxC7				0x3f00
+#define		bCCKDebugPort			0xff0000
+#define		bCCKDACDebug			0x0f000000
+#define		bCCKFalseAlarmEnable	0x8000
+#define		bCCKFalseAlarmRead		0x4000
+#define		bCCKTRSSI				0x7f
+#define		bCCKRxAGCReport		0xfe
+#define		bCCKRxReport_AntSel	0x80000000
+#define		bCCKRxReport_MFOff		0x40000000
+#define		bCCKRxRxReport_SQLoss	0x20000000
+#define		bCCKRxReport_Pktloss	0x10000000
+#define		bCCKRxReport_Lockedbit	0x08000000
+#define		bCCKRxReport_RateError	0x04000000
+#define		bCCKRxReport_RxRate	0x03000000
+#define		bCCKRxFACounterLower	0xff
+#define		bCCKRxFACounterUpper	0xff000000
+#define		bCCKRxHPAGCStart		0xe000
+#define		bCCKRxHPAGCFinal		0x1c00
+#define		bCCKRxFalseAlarmEnable	0x8000
+#define		bCCKFACounterFreeze	0x4000
+#define		bCCKTxPathSel			0x10000000
+#define		bCCKDefaultRxPath		0xc000000
+#define		bCCKOptionRxPath		0x3000000
+
+/*  5. PageC(0xC00) */
+#define		bNumOfSTF				0x3	/*  Useless */
+#define		bShift_L					0xc0
+#define		bGI_TH					0xc
+#define		bRxPathA				0x1
+#define		bRxPathB				0x2
+#define		bRxPathC				0x4
+#define		bRxPathD				0x8
+#define		bTxPathA				0x1
+#define		bTxPathB				0x2
+#define		bTxPathC				0x4
+#define		bTxPathD				0x8
+#define		bTRSSIFreq				0x200
+#define		bADCBackoff				0x3000
+#define		bDFIRBackoff			0xc000
+#define		bTRSSILatchPhase		0x10000
+#define		bRxIDCOffset			0xff
+#define		bRxQDCOffset			0xff00
+#define		bRxDFIRMode			0x1800000
+#define		bRxDCNFType			0xe000000
+#define		bRXIQImb_A				0x3ff
+#define		bRXIQImb_B				0xfc00
+#define		bRXIQImb_C				0x3f0000
+#define		bRXIQImb_D				0xffc00000
+#define		bDC_dc_Notch			0x60000
+#define		bRxNBINotch			0x1f000000
+#define		bPD_TH					0xf
+#define		bPD_TH_Opt2			0xc000
+#define		bPWED_TH				0x700
+#define		bIfMF_Win_L			0x800
+#define		bPD_Option				0x1000
+#define		bMF_Win_L				0xe000
+#define		bBW_Search_L			0x30000
+#define		bwin_enh_L				0xc0000
+#define		bBW_TH					0x700000
+#define		bED_TH2				0x3800000
+#define		bBW_option				0x4000000
+#define		bRatio_TH				0x18000000
+#define		bWindow_L				0xe0000000
+#define		bSBD_Option				0x1
+#define		bFrame_TH				0x1c
+#define		bFS_Option				0x60
+#define		bDC_Slope_check		0x80
+#define		bFGuard_Counter_DC_L	0xe00
+#define		bFrame_Weight_Short	0x7000
+#define		bSub_Tune				0xe00000
+#define		bFrame_DC_Length		0xe000000
+#define		bSBD_start_offset		0x30000000
+#define		bFrame_TH_2			0x7
+#define		bFrame_GI2_TH			0x38
+#define		bGI2_Sync_en			0x40
+#define		bSarch_Short_Early		0x300
+#define		bSarch_Short_Late		0xc00
+#define		bSarch_GI2_Late		0x70000
+#define		bCFOAntSum				0x1
+#define		bCFOAcc				0x2
+#define		bCFOStartOffset			0xc
+#define		bCFOLookBack			0x70
+#define		bCFOSumWeight			0x80
+#define		bDAGCEnable			0x10000
+#define		bTXIQImb_A				0x3ff
+#define		bTXIQImb_B				0xfc00
+#define		bTXIQImb_C				0x3f0000
+#define		bTXIQImb_D				0xffc00000
+#define		bTxIDCOffset			0xff
+#define		bTxQDCOffset			0xff00
+#define		bTxDFIRMode			0x10000
+#define		bTxPesudoNoiseOn		0x4000000
+#define		bTxPesudoNoise_A		0xff
+#define		bTxPesudoNoise_B		0xff00
+#define		bTxPesudoNoise_C		0xff0000
+#define		bTxPesudoNoise_D		0xff000000
+#define		bCCADropOption			0x20000
+#define		bCCADropThres			0xfff00000
+#define		bEDCCA_H				0xf
+#define		bEDCCA_L				0xf0
+#define		bLambda_ED			0x300
+#define		bRxInitialGain			0x7f
+#define		bRxAntDivEn				0x80
+#define		bRxAGCAddressForLNA	0x7f00
+#define		bRxHighPowerFlow		0x8000
+#define		bRxAGCFreezeThres		0xc0000
+#define		bRxFreezeStep_AGC1	0x300000
+#define		bRxFreezeStep_AGC2	0xc00000
+#define		bRxFreezeStep_AGC3	0x3000000
+#define		bRxFreezeStep_AGC0	0xc000000
+#define		bRxRssi_Cmp_En			0x10000000
+#define		bRxQuickAGCEn			0x20000000
+#define		bRxAGCFreezeThresMode	0x40000000
+#define		bRxOverFlowCheckType	0x80000000
+#define		bRxAGCShift				0x7f
+#define		bTRSW_Tri_Only			0x80
+#define		bPowerThres			0x300
+#define		bRxAGCEn				0x1
+#define		bRxAGCTogetherEn		0x2
+#define		bRxAGCMin				0x4
+#define		bRxHP_Ini				0x7
+#define		bRxHP_TRLNA			0x70
+#define		bRxHP_RSSI				0x700
+#define		bRxHP_BBP1				0x7000
+#define		bRxHP_BBP2				0x70000
+#define		bRxHP_BBP3				0x700000
+#define		bRSSI_H					0x7f0000     /* the threshold for high power */
+#define		bRSSI_Gen				0x7f000000   /* the threshold for ant diversity */
+#define		bRxSettle_TRSW			0x7
+#define		bRxSettle_LNA			0x38
+#define		bRxSettle_RSSI			0x1c0
+#define		bRxSettle_BBP			0xe00
+#define		bRxSettle_RxHP			0x7000
+#define		bRxSettle_AntSW_RSSI	0x38000
+#define		bRxSettle_AntSW		0xc0000
+#define		bRxProcessTime_DAGC	0x300000
+#define		bRxSettle_HSSI			0x400000
+#define		bRxProcessTime_BBPPW	0x800000
+#define		bRxAntennaPowerShift	0x3000000
+#define		bRSSITableSelect		0xc000000
+#define		bRxHP_Final				0x7000000
+#define		bRxHTSettle_BBP			0x7
+#define		bRxHTSettle_HSSI		0x8
+#define		bRxHTSettle_RxHP		0x70
+#define		bRxHTSettle_BBPPW		0x80
+#define		bRxHTSettle_Idle		0x300
+#define		bRxHTSettle_Reserved	0x1c00
+#define		bRxHTRxHPEn			0x8000
+#define		bRxHTAGCFreezeThres	0x30000
+#define		bRxHTAGCTogetherEn	0x40000
+#define		bRxHTAGCMin			0x80000
+#define		bRxHTAGCEn				0x100000
+#define		bRxHTDAGCEn			0x200000
+#define		bRxHTRxHP_BBP			0x1c00000
+#define		bRxHTRxHP_Final		0xe0000000
+#define		bRxPWRatioTH			0x3
+#define		bRxPWRatioEn			0x4
+#define		bRxMFHold				0x3800
+#define		bRxPD_Delay_TH1		0x38
+#define		bRxPD_Delay_TH2		0x1c0
+#define		bRxPD_DC_COUNT_MAX	0x600
+/* define bRxMF_Hold               0x3800 */
+#define		bRxPD_Delay_TH			0x8000
+#define		bRxProcess_Delay		0xf0000
+#define		bRxSearchrange_GI2_Early	0x700000
+#define		bRxFrame_Guard_Counter_L	0x3800000
+#define		bRxSGI_Guard_L			0xc000000
+#define		bRxSGI_Search_L		0x30000000
+#define		bRxSGI_TH				0xc0000000
+#define		bDFSCnt0				0xff
+#define		bDFSCnt1				0xff00
+#define		bDFSFlag				0xf0000
+#define		bMFWeightSum			0x300000
+#define		bMinIdxTH				0x7f000000
+#define		bDAFormat				0x40000
+#define		bTxChEmuEnable		0x01000000
+#define		bTRSWIsolation_A		0x7f
+#define		bTRSWIsolation_B		0x7f00
+#define		bTRSWIsolation_C		0x7f0000
+#define		bTRSWIsolation_D		0x7f000000
+#define		bExtLNAGain				0x7c00
+
+/*  6. PageE(0xE00) */
+#define		bSTBCEn				0x4	/*  Useless */
+#define		bAntennaMapping		0x10
+#define		bNss					0x20
+#define		bCFOAntSumD			0x200
+#define		bPHYCounterReset		0x8000000
+#define		bCFOReportGet			0x4000000
+#define		bOFDMContinueTx		0x10000000
+#define		bOFDMSingleCarrier		0x20000000
+#define		bOFDMSingleTone		0x40000000
+/* define bRxPath1                 0x01 */
+/* define bRxPath2                 0x02 */
+/* define bRxPath3                 0x04 */
+/* define bRxPath4                 0x08 */
+/* define bTxPath1                 0x10 */
+/* define bTxPath2                 0x20 */
+#define		bHTDetect			0x100
+#define		bCFOEn				0x10000
+#define		bCFOValue			0xfff00000
+#define		bSigTone_Re		0x3f
+#define		bSigTone_Im		0x7f00
+#define		bCounter_CCA		0xffff
+#define		bCounter_ParityFail	0xffff0000
+#define		bCounter_RateIllegal		0xffff
+#define		bCounter_CRC8Fail	0xffff0000
+#define		bCounter_MCSNoSupport	0xffff
+#define		bCounter_FastSync	0xffff
+#define		bShortCFO			0xfff
+#define		bShortCFOTLength	12   /* total */
+#define		bShortCFOFLength	11   /* fraction */
+#define		bLongCFO			0x7ff
+#define		bLongCFOTLength	11
+#define		bLongCFOFLength	11
+#define		bTailCFO			0x1fff
+#define		bTailCFOTLength		13
+#define		bTailCFOFLength		12
+#define		bmax_en_pwdB		0xffff
+#define		bCC_power_dB		0xffff0000
+#define		bnoise_pwdB		0xffff
+#define		bPowerMeasTLength	10
+#define		bPowerMeasFLength	3
+#define		bRx_HT_BW			0x1
+#define		bRxSC				0x6
+#define		bRx_HT				0x8
+#define		bNB_intf_det_on		0x1
+#define		bIntf_win_len_cfg	0x30
+#define		bNB_Intf_TH_cfg		0x1c0
+#define		bRFGain				0x3f
+#define		bTableSel			0x40
+#define		bTRSW				0x80
+#define		bRxSNR_A			0xff
+#define		bRxSNR_B			0xff00
+#define		bRxSNR_C			0xff0000
+#define		bRxSNR_D			0xff000000
+#define		bSNREVMTLength		8
+#define		bSNREVMFLength		1
+#define		bCSI1st				0xff
+#define		bCSI2nd				0xff00
+#define		bRxEVM1st			0xff0000
+#define		bRxEVM2nd			0xff000000
+#define		bSIGEVM			0xff
+#define		bPWDB				0xff00
+#define		bSGIEN				0x10000
+
+#define		bSFactorQAM1		0xf	/*  Useless */
+#define		bSFactorQAM2		0xf0
+#define		bSFactorQAM3		0xf00
+#define		bSFactorQAM4		0xf000
+#define		bSFactorQAM5		0xf0000
+#define		bSFactorQAM6		0xf0000
+#define		bSFactorQAM7		0xf00000
+#define		bSFactorQAM8		0xf000000
+#define		bSFactorQAM9		0xf0000000
+#define		bCSIScheme			0x100000
+
+#define		bNoiseLvlTopSet		0x3	/*  Useless */
+#define		bChSmooth			0x4
+#define		bChSmoothCfg1		0x38
+#define		bChSmoothCfg2		0x1c0
+#define		bChSmoothCfg3		0xe00
+#define		bChSmoothCfg4		0x7000
+#define		bMRCMode			0x800000
+#define		bTHEVMCfg			0x7000000
+
+#define		bLoopFitType		0x1	/*  Useless */
+#define		bUpdCFO			0x40
+#define		bUpdCFOOffData		0x80
+#define		bAdvUpdCFO			0x100
+#define		bAdvTimeCtrl		0x800
+#define		bUpdClko			0x1000
+#define		bFC					0x6000
+#define		bTrackingMode		0x8000
+#define		bPhCmpEnable		0x10000
+#define		bUpdClkoLTF		0x20000
+#define		bComChCFO			0x40000
+#define		bCSIEstiMode		0x80000
+#define		bAdvUpdEqz			0x100000
+#define		bUChCfg				0x7000000
+#define		bUpdEqz			0x8000000
+
+/* Rx Pseduo noise */
+#define		bRxPesudoNoiseOn		0x20000000	/*  Useless */
+#define		bRxPesudoNoise_A		0xff
+#define		bRxPesudoNoise_B		0xff00
+#define		bRxPesudoNoise_C		0xff0000
+#define		bRxPesudoNoise_D		0xff000000
+#define		bPesudoNoiseState_A	0xffff
+#define		bPesudoNoiseState_B	0xffff0000
+#define		bPesudoNoiseState_C	0xffff
+#define		bPesudoNoiseState_D	0xffff0000
+
+/* 7. RF Register */
+/* Zebra1 */
+#define		bZebra1_HSSIEnable		0x8		/*  Useless */
+#define		bZebra1_TRxControl		0xc00
+#define		bZebra1_TRxGainSetting	0x07f
+#define		bZebra1_RxCorner		0xc00
+#define		bZebra1_TxChargePump	0x38
+#define		bZebra1_RxChargePump	0x7
+#define		bZebra1_ChannelNum	0xf80
+#define		bZebra1_TxLPFBW		0x400
+#define		bZebra1_RxLPFBW		0x600
+
+/* Zebra4 */
+#define		bRTL8256RegModeCtrl1	0x100	/*  Useless */
+#define		bRTL8256RegModeCtrl0	0x40
+#define		bRTL8256_TxLPFBW		0x18
+#define		bRTL8256_RxLPFBW		0x600
+
+/* RTL8258 */
+#define		bRTL8258_TxLPFBW		0xc	/*  Useless */
+#define		bRTL8258_RxLPFBW		0xc00
+#define		bRTL8258_RSSILPFBW	0xc0
+
+
+/*  */
+/*  Other Definition */
+/*  */
+
+/* byte endable for sb_write */
+#define		bByte0				0x1	/*  Useless */
+#define		bByte1				0x2
+#define		bByte2				0x4
+#define		bByte3				0x8
+#define		bWord0				0x3
+#define		bWord1				0xc
+#define		bDWord				0xf
+
+/* for PutRegsetting & GetRegSetting BitMask */
+#define		bMaskByte0			0xff	/*  Reg 0xc50 rOFDM0_XAAGCCore~0xC6f */
+#define		bMaskByte1			0xff00
+#define		bMaskByte2			0xff0000
+#define		bMaskByte3			0xff000000
+#define		bMaskHWord		0xffff0000
+#define		bMaskLWord			0x0000ffff
+#define		bMaskDWord		0xffffffff
+#define		bMaskH3Bytes		0xffffff00
+#define		bMask12Bits			0xfff
+#define		bMaskH4Bits			0xf0000000
+#define		bMaskOFDM_D		0xffc00000
+#define		bMaskCCK			0x3f3f3f3f
+
+
+#define		bEnable			0x1	/*  Useless */
+#define		bDisable		0x0
+
+#define		LeftAntenna		0x0	/*  Useless */
+#define		RightAntenna	0x1
+
+#define		tCheckTxStatus		500   /* 500ms Useless */
+#define		tUpdateRxCounter	100   /* 100ms */
+
+#define		rateCCK		0	/*  Useless */
+#define		rateOFDM	1
+#define		rateHT		2
+
+/* define Register-End */
+#define		bPMAC_End			0x1ff	/*  Useless */
+#define		bFPGAPHY0_End		0x8ff
+#define		bFPGAPHY1_End		0x9ff
+#define		bCCKPHY0_End		0xaff
+#define		bOFDMPHY0_End		0xcff
+#define		bOFDMPHY1_End		0xdff
+
+/* define max debug item in each debug page */
+/* define bMaxItem_FPGA_PHY0        0x9 */
+/* define bMaxItem_FPGA_PHY1        0x3 */
+/* define bMaxItem_PHY_11B          0x16 */
+/* define bMaxItem_OFDM_PHY0        0x29 */
+/* define bMaxItem_OFDM_PHY1        0x0 */
+
+#define		bPMACControl		0x0		/*  Useless */
+#define		bWMACControl		0x1
+#define		bWNICControl		0x2
+
+#define		PathA			0x0	/*  Useless */
+#define		PathB			0x1
+#define		PathC			0x2
+#define		PathD			0x3
+
+/*--------------------------Define Parameters-------------------------------*/
+
+
+#endif	/* __INC_HAL8192SPHYREG_H */
diff --git a/drivers/staging/rtl8723bs/include/HalPwrSeqCmd.h b/drivers/staging/rtl8723bs/include/HalPwrSeqCmd.h
new file mode 100644
index 000000000..e30071935
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/HalPwrSeqCmd.h
@@ -0,0 +1,122 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HALPWRSEQCMD_H__
+#define __HALPWRSEQCMD_H__
+
+#include <drv_types.h>
+
+/*---------------------------------------------*/
+/* 3 The value of cmd: 4 bits */
+/*---------------------------------------------*/
+#define PWR_CMD_READ			0x00
+     /*  offset: the read register offset */
+     /*  msk: the mask of the read value */
+     /*  value: N/A, left by 0 */
+     /*  note: dirver shall implement this function by read & msk */
+
+#define PWR_CMD_WRITE			0x01
+     /*  offset: the read register offset */
+     /*  msk: the mask of the write bits */
+     /*  value: write value */
+     /*  note: driver shall implement this cmd by read & msk after write */
+
+#define PWR_CMD_POLLING			0x02
+     /*  offset: the read register offset */
+     /*  msk: the mask of the polled value */
+     /*  value: the value to be polled, masked by the msd field. */
+     /*  note: driver shall implement this cmd by */
+     /*  do{ */
+     /*  if ((Read(offset) & msk) == (value & msk)) */
+     /*  break; */
+     /*  } while (not timeout); */
+
+#define PWR_CMD_DELAY			0x03
+     /*  offset: the value to delay */
+     /*  msk: N/A */
+     /*  value: the unit of delay, 0: us, 1: ms */
+
+#define PWR_CMD_END				0x04
+     /*  offset: N/A */
+     /*  msk: N/A */
+     /*  value: N/A */
+
+/*---------------------------------------------*/
+/* 3 The value of base: 4 bits */
+/*---------------------------------------------*/
+   /*  define the base address of each block */
+#define PWR_BASEADDR_MAC		0x00
+#define PWR_BASEADDR_USB		0x01
+#define PWR_BASEADDR_PCIE		0x02
+#define PWR_BASEADDR_SDIO		0x03
+
+/*---------------------------------------------*/
+/* 3 The value of interface_msk: 4 bits */
+/*---------------------------------------------*/
+#define	PWR_INTF_SDIO_MSK		BIT(0)
+#define	PWR_INTF_USB_MSK		BIT(1)
+#define	PWR_INTF_PCI_MSK		BIT(2)
+#define	PWR_INTF_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+/*---------------------------------------------*/
+/* 3 The value of fab_msk: 4 bits */
+/*---------------------------------------------*/
+#define	PWR_FAB_TSMC_MSK		BIT(0)
+#define	PWR_FAB_UMC_MSK			BIT(1)
+#define	PWR_FAB_ALL_MSK			(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+/*---------------------------------------------*/
+/* 3 The value of cut_msk: 8 bits */
+/*---------------------------------------------*/
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK			BIT(1)
+#define	PWR_CUT_B_MSK			BIT(2)
+#define	PWR_CUT_C_MSK			BIT(3)
+#define	PWR_CUT_D_MSK			BIT(4)
+#define	PWR_CUT_E_MSK			BIT(5)
+#define	PWR_CUT_F_MSK			BIT(6)
+#define	PWR_CUT_G_MSK			BIT(7)
+#define	PWR_CUT_ALL_MSK			0xFF
+
+
+enum {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+
+#define GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
+#define GET_PWR_CFG_CUT_MASK(__PWR_CMD)		__PWR_CMD.cut_msk
+#define GET_PWR_CFG_FAB_MASK(__PWR_CMD)		__PWR_CMD.fab_msk
+#define GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
+#define GET_PWR_CFG_BASE(__PWR_CMD)			__PWR_CMD.base
+#define GET_PWR_CFG_CMD(__PWR_CMD)			__PWR_CMD.cmd
+#define GET_PWR_CFG_MASK(__PWR_CMD)			__PWR_CMD.msk
+#define GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
+
+
+/*  */
+/* 	Prototype of protected function. */
+/*  */
+u8 HalPwrSeqCmdParsing(
+	struct adapter *padapter,
+	u8 		CutVersion,
+	u8 		FabVersion,
+	u8 		InterfaceType,
+	struct wlan_pwr_cfg	PwrCfgCmd[]);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/HalVerDef.h b/drivers/staging/rtl8723bs/include/HalVerDef.h
new file mode 100644
index 000000000..d0ce21ccc
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/HalVerDef.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_VERSION_DEF_H__
+#define __HAL_VERSION_DEF_H__
+
+/*  hal_ic_type_e */
+enum hal_ic_type_e { /* tag_HAL_IC_Type_Definition */
+	CHIP_8723B	=	8,
+};
+
+/* hal_chip_type_e */
+enum hal_chip_type_e { /* tag_HAL_CHIP_Type_Definition */
+	TEST_CHIP		=	0,
+	NORMAL_CHIP	=	1,
+	FPGA			=	2,
+};
+
+/* hal_cut_version_e */
+enum hal_cut_version_e { /* tag_HAL_Cut_Version_Definition */
+	A_CUT_VERSION		=	0,
+	B_CUT_VERSION		=	1,
+	C_CUT_VERSION		=	2,
+	D_CUT_VERSION		=	3,
+	E_CUT_VERSION		=	4,
+	F_CUT_VERSION		=	5,
+	G_CUT_VERSION		=	6,
+	H_CUT_VERSION		=	7,
+	I_CUT_VERSION		=	8,
+	J_CUT_VERSION		=	9,
+	K_CUT_VERSION		=	10,
+};
+
+/*  HAL_Manufacturer */
+enum hal_vendor_e { /* tag_HAL_Manufacturer_Version_Definition */
+	CHIP_VENDOR_TSMC	=	0,
+	CHIP_VENDOR_UMC		=	1,
+	CHIP_VENDOR_SMIC	=	2,
+};
+
+struct hal_version { /* tag_HAL_VERSION */
+	enum hal_ic_type_e		ICType;
+	enum hal_chip_type_e		ChipType;
+	enum hal_cut_version_e	CUTVersion;
+	enum hal_vendor_e		VendorType;
+	u8 			ROMVer;
+};
+
+/* hal_version			VersionID; */
+
+/*  Get element */
+#define GET_CVID_IC_TYPE(version)			((enum hal_ic_type_e)((version).ICType))
+#define GET_CVID_CHIP_TYPE(version)			((enum hal_chip_type_e)((version).ChipType))
+#define GET_CVID_MANUFACTUER(version)		((enum hal_vendor_e)((version).VendorType))
+#define GET_CVID_CUT_VERSION(version)		((enum hal_cut_version_e)((version).CUTVersion))
+#define GET_CVID_ROM_VERSION(version)		(((version).ROMVer) & ROM_VERSION_MASK)
+
+/*  */
+/* Common Macro. -- */
+/*  */
+/* hal_version VersionID */
+
+/* hal_chip_type_e */
+#define IS_TEST_CHIP(version)			((GET_CVID_CHIP_TYPE(version) == TEST_CHIP) ? true : false)
+#define IS_NORMAL_CHIP(version)			((GET_CVID_CHIP_TYPE(version) == NORMAL_CHIP) ? true : false)
+
+/* hal_cut_version_e */
+#define IS_A_CUT(version)				((GET_CVID_CUT_VERSION(version) == A_CUT_VERSION) ? true : false)
+#define IS_B_CUT(version)				((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? true : false)
+#define IS_C_CUT(version)				((GET_CVID_CUT_VERSION(version) == C_CUT_VERSION) ? true : false)
+#define IS_D_CUT(version)				((GET_CVID_CUT_VERSION(version) == D_CUT_VERSION) ? true : false)
+#define IS_E_CUT(version)				((GET_CVID_CUT_VERSION(version) == E_CUT_VERSION) ? true : false)
+#define IS_I_CUT(version)				((GET_CVID_CUT_VERSION(version) == I_CUT_VERSION) ? true : false)
+#define IS_J_CUT(version)				((GET_CVID_CUT_VERSION(version) == J_CUT_VERSION) ? true : false)
+#define IS_K_CUT(version)				((GET_CVID_CUT_VERSION(version) == K_CUT_VERSION) ? true : false)
+
+/* hal_vendor_e */
+#define IS_CHIP_VENDOR_TSMC(version)	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_TSMC) ? true : false)
+#define IS_CHIP_VENDOR_UMC(version)	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_UMC) ? true : false)
+#define IS_CHIP_VENDOR_SMIC(version)	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_SMIC) ? true : false)
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/basic_types.h b/drivers/staging/rtl8723bs/include/basic_types.h
new file mode 100644
index 000000000..57bb71732
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/basic_types.h
@@ -0,0 +1,199 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __BASIC_TYPES_H__
+#define __BASIC_TYPES_H__
+
+
+#define SUCCESS	0
+#define FAIL	(-1)
+
+#include <linux/types.h>
+
+#define FIELD_OFFSET(s, field)	((__kernel_ssize_t)&((s *)(0))->field)
+
+#define SIZE_PTR __kernel_size_t
+#define SSIZE_PTR __kernel_ssize_t
+
+/* port from fw by thomas */
+/*  TODO: Belows are Sync from SD7-Driver. It is necessary to check correctness */
+
+/*
+ *Call endian free function when
+ *	1. Read/write packet content.
+ *	2. Before write integer to IO.
+ *	3. After read integer from IO.
+*/
+
+/*  */
+/*  Byte Swapping routine. */
+/*  */
+#define EF1Byte	(u8)
+#define EF2Byte		le16_to_cpu
+#define EF4Byte	le32_to_cpu
+
+/* Convert little data endian to host ordering */
+#define EF1BYTE(_val)		\
+	((u8)(_val))
+#define EF2BYTE(_val)		\
+	(le16_to_cpu(_val))
+#define EF4BYTE(_val)		\
+	(le32_to_cpu(_val))
+
+/* Read data from memory */
+#define READEF1BYTE(_ptr)	\
+	EF1BYTE(*((u8 *)(_ptr)))
+/* Read le16 data from memory and convert to host ordering */
+#define READEF2BYTE(_ptr)	\
+	EF2BYTE(*(_ptr))
+#define READEF4BYTE(_ptr)	\
+	EF4BYTE(*(_ptr))
+
+/* Write data to memory */
+#define WRITEEF1BYTE(_ptr, _val)			\
+	do {						\
+		(*((u8 *)(_ptr))) = EF1BYTE(_val);	\
+	} while (0)
+/* Write le data to memory in host ordering */
+#define WRITEEF2BYTE(_ptr, _val)			\
+	do {						\
+		(*((u16 *)(_ptr))) = EF2BYTE(_val);	\
+	} while (0)
+
+#define WRITEEF4BYTE(_ptr, _val)			\
+	do {						\
+		(*((u32 *)(_ptr))) = EF2BYTE(_val);	\
+	} while (0)
+
+/* Create a bit mask
+ * Examples:
+ * BIT_LEN_MASK_32(0) => 0x00000000
+ * BIT_LEN_MASK_32(1) => 0x00000001
+ * BIT_LEN_MASK_32(2) => 0x00000003
+ * BIT_LEN_MASK_32(32) => 0xFFFFFFFF
+ */
+#define BIT_LEN_MASK_32(__bitlen)	 \
+	(0xFFFFFFFF >> (32 - (__bitlen)))
+#define BIT_LEN_MASK_16(__bitlen)	 \
+	(0xFFFF >> (16 - (__bitlen)))
+#define BIT_LEN_MASK_8(__bitlen) \
+	(0xFF >> (8 - (__bitlen)))
+
+/* Create an offset bit mask
+ * Examples:
+ * BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
+ * BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000
+ */
+#define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_16(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
+
+/*Description:
+ * Return 4-byte value in host byte ordering from
+ * 4-byte pointer in little-endian system.
+ */
+#define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
+	(EF4BYTE(*((__le32 *)(__pstart))))
+#define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
+	(EF2BYTE(*((__le16 *)(__pstart))))
+#define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
+	(EF1BYTE(*((u8 *)(__pstart))))
+
+/*  */
+/* 	Description: */
+/* 		Translate subfield (continuous bits in little-endian) of 4-byte value in litten byte to */
+/* 		4-byte value in host byte ordering. */
+/*  */
+#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		(LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset))  & \
+		BIT_LEN_MASK_32(__bitlen) \
+	)
+#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		(LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset)) & \
+		BIT_LEN_MASK_16(__bitlen) \
+	)
+#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		(LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset)) & \
+		BIT_LEN_MASK_8(__bitlen) \
+	)
+
+/*  */
+/* 	Description: */
+/* 		Mask subfield (continuous bits in little-endian) of 4-byte value in litten byte oredering */
+/* 		and return the result in 4-byte value in host byte ordering. */
+/*  */
+#define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
+		(~BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen)) \
+	)
+#define LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		LE_P2BYTE_TO_HOST_2BYTE(__pstart) & \
+		(~BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen)) \
+	)
+#define LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	(\
+		LE_P1BYTE_TO_HOST_1BYTE(__pstart) & \
+		(~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen)) \
+	)
+
+/*  */
+/* 	Description: */
+/* 		Set subfield of little-endian 4-byte value to specified value. */
+/*  */
+#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
+		*((u32 *)(__pstart)) =				\
+		(						\
+		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset)) \
+		)
+
+#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
+		*((u16 *)(__pstart)) =				\
+		(					\
+		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset)) \
+		);
+
+#define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
+		*((u8 *)(__pstart)) = EF1BYTE			\
+		(					\
+		LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u8)__val) & BIT_LEN_MASK_8(__bitlen)) << (__bitoffset)) \
+		)
+
+#define LE_BITS_CLEARED_TO_1BYTE_8BIT(__pStart, __BitOffset, __BitLen) \
+	(\
+		LE_P1BYTE_TO_HOST_1BYTE(__pStart) \
+	)
+
+#define SET_BITS_TO_LE_1BYTE_8BIT(__pStart, __BitOffset, __BitLen, __Value) \
+{ \
+	*((u8 *)(__pStart)) = \
+		EF1Byte(\
+			LE_BITS_CLEARED_TO_1BYTE_8BIT(__pStart, __BitOffset, __BitLen) \
+			| \
+			((u8)__Value) \
+		); \
+}
+
+/*  Get the N-bytes alignent offset from the current length */
+#define N_BYTE_ALIGMENT(__Value, __Aligment) ((__Aligment == 1) ? (__Value) : (((__Value + __Aligment - 1) / __Aligment) * __Aligment))
+
+#define TEST_FLAG(__Flag, __testFlag)		(((__Flag) & (__testFlag)) != 0)
+#define SET_FLAG(__Flag, __setFlag)			((__Flag) |= __setFlag)
+#define CLEAR_FLAG(__Flag, __clearFlag)		((__Flag) &= ~(__clearFlag))
+#define CLEAR_FLAGS(__Flag)					((__Flag) = 0)
+#define TEST_FLAGS(__Flag, __testFlags)		(((__Flag) & (__testFlags)) == (__testFlags))
+
+#endif /* __BASIC_TYPES_H__ */
diff --git a/drivers/staging/rtl8723bs/include/cmd_osdep.h b/drivers/staging/rtl8723bs/include/cmd_osdep.h
new file mode 100644
index 000000000..5506f513d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/cmd_osdep.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __CMD_OSDEP_H_
+#define __CMD_OSDEP_H_
+
+
+int rtw_init_cmd_priv(struct	cmd_priv *pcmdpriv);
+int rtw_init_evt_priv(struct evt_priv *pevtpriv);
+extern void _rtw_free_evt_priv(struct	evt_priv *pevtpriv);
+extern void _rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv);
+int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj);
+extern struct	cmd_obj	*_rtw_dequeue_cmd(struct __queue *queue);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/drv_types.h b/drivers/staging/rtl8723bs/include/drv_types.h
new file mode 100644
index 000000000..82159e1c7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/drv_types.h
@@ -0,0 +1,502 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+/*-------------------------------------------------------------------------------
+
+	For type defines and data structure defines
+
+--------------------------------------------------------------------------------*/
+
+
+#ifndef __DRV_TYPES_H__
+#define __DRV_TYPES_H__
+
+#include <linux/sched/signal.h>
+#include <basic_types.h>
+#include <osdep_service.h>
+#include <rtw_byteorder.h>
+#include <wlan_bssdef.h>
+#include <wifi.h>
+#include <ieee80211.h>
+
+#include <rtw_rf.h>
+
+#include <rtw_ht.h>
+
+#include <rtw_cmd.h>
+#include <cmd_osdep.h>
+#include <rtw_security.h>
+#include <rtw_xmit.h>
+#include <xmit_osdep.h>
+#include <rtw_recv.h>
+
+#include <recv_osdep.h>
+#include <rtw_efuse.h>
+#include <hal_intf.h>
+#include <hal_com.h>
+#include <rtw_qos.h>
+#include <rtw_pwrctrl.h>
+#include <rtw_mlme.h>
+#include <mlme_osdep.h>
+#include <rtw_io.h>
+#include <rtw_ioctl_set.h>
+#include <osdep_intf.h>
+#include <rtw_eeprom.h>
+#include <sta_info.h>
+#include <rtw_event.h>
+#include <rtw_mlme_ext.h>
+#include <rtw_ap.h>
+#include <rtw_version.h>
+
+#include "ioctl_cfg80211.h"
+
+#include <linux/ip.h>
+#include <linux/if_ether.h>
+
+#define SPEC_DEV_ID_NONE BIT(0)
+#define SPEC_DEV_ID_DISABLE_HT BIT(1)
+#define SPEC_DEV_ID_ENABLE_PS BIT(2)
+#define SPEC_DEV_ID_RF_CONFIG_1T1R BIT(3)
+#define SPEC_DEV_ID_RF_CONFIG_2T2R BIT(4)
+#define SPEC_DEV_ID_ASSIGN_IFNAME BIT(5)
+
+struct registry_priv {
+	u8 chip_version;
+	u8 rfintfs;
+	u8 lbkmode;
+	u8 hci;
+	struct ndis_802_11_ssid	ssid;
+	u8 network_mode;	/* infra, ad-hoc, auto */
+	u8 channel;/* ad-hoc support requirement */
+	u8 wireless_mode;/* A, B, G, auto */
+	u8 scan_mode;/* active, passive */
+	u8 radio_enable;
+	u8 preamble;/* long, short, auto */
+	u8 vrtl_carrier_sense;/* Enable, Disable, Auto */
+	u8 vcs_type;/* RTS/CTS, CTS-to-self */
+	u16 rts_thresh;
+	u16  frag_thresh;
+	u8 adhoc_tx_pwr;
+	u8 soft_ap;
+	u8 power_mgnt;
+	u8 ips_mode;
+	u8 smart_ps;
+	u8   usb_rxagg_mode;
+	u8 long_retry_lmt;
+	u8 short_retry_lmt;
+	u16 busy_thresh;
+	u8 ack_policy;
+	u8  mp_dm;
+	u8 software_encrypt;
+	u8 software_decrypt;
+	u8 acm_method;
+	  /* UAPSD */
+	u8 wmm_enable;
+	u8 uapsd_enable;
+	u8 uapsd_max_sp;
+	u8 uapsd_acbk_en;
+	u8 uapsd_acbe_en;
+	u8 uapsd_acvi_en;
+	u8 uapsd_acvo_en;
+
+	struct wlan_bssid_ex    dev_network;
+
+	u8 ht_enable;
+	/*
+	 * 0: 20 MHz, 1: 40 MHz
+	 * 2.4G use bit 0 ~ 3
+	 * 0x01 means enable 2.4G 40MHz
+	 */
+	u8 bw_mode;
+	u8 ampdu_enable;/* for tx */
+	u8 rx_stbc;
+	u8 ampdu_amsdu;/* A-MPDU Supports A-MSDU is permitted */
+	/*  Short GI support Bit Map */
+	/*  BIT0 - 20MHz, 1: support, 0: non-support */
+	/*  BIT1 - 40MHz, 1: support, 0: non-support */
+	/*  BIT2 - 80MHz, 1: support, 0: non-support */
+	/*  BIT3 - 160MHz, 1: support, 0: non-support */
+	u8 short_gi;
+	/*  BIT0: Enable VHT LDPC Rx, BIT1: Enable VHT LDPC Tx, BIT4: Enable HT LDPC Rx, BIT5: Enable HT LDPC Tx */
+	u8 ldpc_cap;
+	/*  BIT0: Enable VHT STBC Rx, BIT1: Enable VHT STBC Tx, BIT4: Enable HT STBC Rx, BIT5: Enable HT STBC Tx */
+	u8 stbc_cap;
+	/*  BIT0: Enable VHT Beamformer, BIT1: Enable VHT Beamformee, BIT4: Enable HT Beamformer, BIT5: Enable HT Beamformee */
+	u8 beamform_cap;
+
+	u8 lowrate_two_xmit;
+
+	u8 low_power;
+
+	u8 wifi_spec;/*  !turbo_mode */
+
+	u8 channel_plan;
+
+	s8	ant_num;
+
+	/* false:Reject AP's Add BA req, true:accept AP's Add BA req */
+	bool	accept_addba_req;
+
+	u8 antdiv_cfg;
+	u8 antdiv_type;
+
+	u8 usbss_enable;/* 0:disable, 1:enable */
+	u8 hwpdn_mode;/* 0:disable, 1:enable, 2:decide by EFUSE config */
+	u8 hwpwrp_detect;/* 0:disable, 1:enable */
+
+	u8 hw_wps_pbc;/* 0:disable, 1:enable */
+
+	u8 max_roaming_times; /*  the max number driver will try to roaming */
+
+	u8 enable80211d;
+
+	u8 ifname[16];
+
+	u8 notch_filter;
+
+	/* define for tx power adjust */
+	u8 RegEnableTxPowerLimit;
+	u8 RegEnableTxPowerByRate;
+	u8 RegPowerBase;
+	u8 RegPwrTblSel;
+	s8	TxBBSwing_2G;
+	u8 AmplifierType_2G;
+	u8 bEn_RFE;
+	u8 RFE_Type;
+	u8  check_fw_ps;
+
+	u8 qos_opt_enable;
+
+	u8 hiq_filter;
+};
+
+
+/* For registry parameters */
+#define RGTRY_OFT(field) ((u32)FIELD_OFFSET(struct registry_priv, field))
+#define RGTRY_SZ(field)   sizeof(((struct registry_priv *)0)->field)
+#define BSSID_OFT(field) ((u32)FIELD_OFFSET(struct wlan_bssid_ex, field))
+#define BSSID_SZ(field)   sizeof(((struct wlan_bssid_ex *) 0)->field)
+
+#include <drv_types_sdio.h>
+
+#define is_primary_adapter(adapter) (1)
+#define get_iface_type(adapter) (IFACE_PORT0)
+#define GET_PRIMARY_ADAPTER(padapter) (((struct adapter *)padapter)->dvobj->if1)
+#define GET_IFACE_NUMS(padapter) (((struct adapter *)padapter)->dvobj->iface_nums)
+#define GET_ADAPTER(padapter, iface_id) (((struct adapter *)padapter)->dvobj->padapters[iface_id])
+
+struct debug_priv {
+	u32 dbg_sdio_free_irq_error_cnt;
+	u32 dbg_sdio_alloc_irq_error_cnt;
+	u32 dbg_sdio_free_irq_cnt;
+	u32 dbg_sdio_alloc_irq_cnt;
+	u32 dbg_sdio_deinit_error_cnt;
+	u32 dbg_sdio_init_error_cnt;
+	u32 dbg_suspend_error_cnt;
+	u32 dbg_suspend_cnt;
+	u32 dbg_resume_cnt;
+	u32 dbg_resume_error_cnt;
+	u32 dbg_deinit_fail_cnt;
+	u32 dbg_carddisable_cnt;
+	u32 dbg_carddisable_error_cnt;
+	u32 dbg_ps_insuspend_cnt;
+	u32 dbg_dev_unload_inIPS_cnt;
+	u32 dbg_wow_leave_ps_fail_cnt;
+	u32 dbg_scan_pwr_state_cnt;
+	u32 dbg_downloadfw_pwr_state_cnt;
+	u32 dbg_fw_read_ps_state_fail_cnt;
+	u32 dbg_leave_ips_fail_cnt;
+	u32 dbg_leave_lps_fail_cnt;
+	u32 dbg_h2c_leave32k_fail_cnt;
+	u32 dbg_diswow_dload_fw_fail_cnt;
+	u32 dbg_enwow_dload_fw_fail_cnt;
+	u32 dbg_ips_drvopen_fail_cnt;
+	u32 dbg_poll_fail_cnt;
+	u32 dbg_rpwm_toggle_cnt;
+	u32 dbg_rpwm_timeout_fail_cnt;
+	u64 dbg_rx_fifo_last_overflow;
+	u64 dbg_rx_fifo_curr_overflow;
+	u64 dbg_rx_fifo_diff_overflow;
+	u64 dbg_rx_ampdu_drop_count;
+	u64 dbg_rx_ampdu_forced_indicate_count;
+	u64 dbg_rx_ampdu_loss_count;
+	u64 dbg_rx_dup_mgt_frame_drop_count;
+	u64 dbg_rx_ampdu_window_shift_cnt;
+};
+
+struct rtw_traffic_statistics {
+	/*  tx statistics */
+	u64	tx_bytes;
+	u64	tx_pkts;
+	u64	tx_drop;
+	u64	cur_tx_bytes;
+	u64	last_tx_bytes;
+	u32 cur_tx_tp; /*  Tx throughput in MBps. */
+
+	/*  rx statistics */
+	u64	rx_bytes;
+	u64	rx_pkts;
+	u64	rx_drop;
+	u64	cur_rx_bytes;
+	u64	last_rx_bytes;
+	u32 cur_rx_tp; /*  Rx throughput in MBps. */
+};
+
+struct cam_ctl_t {
+	spinlock_t lock;
+	u64 bitmap;
+};
+
+struct cam_entry_cache {
+	u16 ctrl;
+	u8 mac[ETH_ALEN];
+	u8 key[16];
+};
+
+struct dvobj_priv {
+	/*-------- below is common data --------*/
+	struct adapter *if1; /* PRIMARY_ADAPTER */
+
+	s32	processing_dev_remove;
+
+	struct debug_priv drv_dbg;
+
+	/* for local/global synchronization */
+	/*  */
+	spinlock_t	lock;
+	int macid[NUM_STA];
+
+	struct mutex hw_init_mutex;
+	struct mutex h2c_fwcmd_mutex;
+	struct mutex setch_mutex;
+	struct mutex setbw_mutex;
+
+	unsigned char oper_channel; /* saved channel info when call set_channel_bw */
+	unsigned char oper_bwmode;
+	unsigned char oper_ch_offset;/* PRIME_CHNL_OFFSET */
+	unsigned long on_oper_ch_time;
+
+	struct adapter *padapters;
+
+	struct cam_ctl_t cam_ctl;
+	struct cam_entry_cache cam_cache[TOTAL_CAM_ENTRY];
+
+	/* In /Out Pipe information */
+	int	RtInPipe[2];
+	int	RtOutPipe[4];
+	u8 Queue2Pipe[HW_QUEUE_ENTRY];/* for out pipe mapping */
+
+	u8 irq_alloc;
+	atomic_t continual_io_error;
+
+	atomic_t disable_func;
+
+	struct pwrctrl_priv pwrctl_priv;
+
+	struct rtw_traffic_statistics	traffic_stat;
+
+/*-------- below is for SDIO INTERFACE --------*/
+
+struct sdio_data intf_data;
+
+};
+
+#define dvobj_to_pwrctl(dvobj) (&(dvobj->pwrctl_priv))
+#define pwrctl_to_dvobj(pwrctl) container_of(pwrctl, struct dvobj_priv, pwrctl_priv)
+
+static inline struct device *dvobj_to_dev(struct dvobj_priv *dvobj)
+{
+	/* todo: get interface type from dvobj and the return the dev accordingly */
+#ifdef RTW_DVOBJ_CHIP_HW_TYPE
+#endif
+
+	return &dvobj->intf_data.func->dev;
+}
+
+struct adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj);
+
+enum {
+	IFACE_PORT0, /* mapping to port0 for C/D series chips */
+	IFACE_PORT1, /* mapping to port1 for C/D series chip */
+	MAX_IFACE_PORT,
+};
+
+enum {
+	DRIVER_NORMAL = 0,
+	DRIVER_DISAPPEAR = 1,
+	DRIVER_REPLACE_DONGLE = 2,
+};
+
+struct adapter {
+	int	DriverState;/*  for disable driver using module, use dongle to replace module. */
+	int	pid[3];/* process id from UI, 0:wps, 1:hostapd, 2:dhcpcd */
+	int	bDongle;/* build-in module or external dongle */
+
+	struct dvobj_priv *dvobj;
+	struct	mlme_priv mlmepriv;
+	struct	mlme_ext_priv mlmeextpriv;
+	struct	cmd_priv cmdpriv;
+	struct	evt_priv evtpriv;
+	/* struct	io_queue	*pio_queue; */
+	struct	io_priv iopriv;
+	struct	xmit_priv xmitpriv;
+	struct	recv_priv recvpriv;
+	struct	sta_priv stapriv;
+	struct	security_priv securitypriv;
+	spinlock_t   security_key_mutex; /*  add for CONFIG_IEEE80211W, none 11w also can use */
+	struct	registry_priv registrypriv;
+	struct	eeprom_priv eeprompriv;
+
+	struct	hostapd_priv *phostapdpriv;
+
+	u32 setband;
+
+	void *HalData;
+	u32 hal_data_sz;
+	struct hal_ops	HalFunc;
+
+	s32	bDriverStopped;
+	s32	bSurpriseRemoved;
+	s32  bCardDisableWOHSM;
+
+	u32 IsrContent;
+	u32 ImrContent;
+
+	u8 EepromAddressSize;
+	u8 hw_init_completed;
+	u8 bDriverIsGoingToUnload;
+	u8 init_adpt_in_progress;
+	u8 bHaltInProgress;
+
+	void *cmdThread;
+	void *evtThread;
+	void *xmitThread;
+	void *recvThread;
+
+	u32 (*intf_init)(struct dvobj_priv *dvobj);
+	void (*intf_deinit)(struct dvobj_priv *dvobj);
+	int (*intf_alloc_irq)(struct dvobj_priv *dvobj);
+	void (*intf_free_irq)(struct dvobj_priv *dvobj);
+
+
+	void (*intf_start)(struct adapter *adapter);
+	void (*intf_stop)(struct adapter *adapter);
+
+	struct net_device *pnetdev;
+	char old_ifname[IFNAMSIZ];
+
+	/*  used by rtw_rereg_nd_name related function */
+	struct rereg_nd_name_data {
+		struct net_device *old_pnetdev;
+		char old_ifname[IFNAMSIZ];
+		u8 old_bRegUseLed;
+	} rereg_nd_name_priv;
+
+	int bup;
+	struct net_device_stats stats;
+	struct iw_statistics iwstats;
+
+	struct wireless_dev *rtw_wdev;
+	struct rtw_wdev_priv wdev_data;
+
+	int net_closed;
+
+	u8 netif_up;
+
+	u8 bFWReady;
+	u8 bBTFWReady;
+	u8 bLinkInfoDump;
+	u8 bRxRSSIDisplay;
+	/* 	Added by Albert 2012/10/26 */
+	/* 	The driver will show up the desired channel number when this flag is 1. */
+	u8 bNotifyChannelChange;
+
+	/* pbuddystruct adapter is used only in two interface case, (iface_nums =2 in struct dvobj_priv) */
+	/* PRIMARY ADAPTER's buddy is SECONDARY_ADAPTER */
+	/* SECONDARY_ADAPTER's buddy is PRIMARY_ADAPTER */
+	/* for iface_id > SECONDARY_ADAPTER(IFACE_ID1), refer to padapters[iface_id]  in struct dvobj_priv */
+	/* and their pbuddystruct adapter is PRIMARY_ADAPTER. */
+	/* for PRIMARY_ADAPTER(IFACE_ID0) can directly refer to if1 in struct dvobj_priv */
+	struct adapter *pbuddy_adapter;
+
+	/* extend to support multi interface */
+       /* IFACE_ID0 is equals to PRIMARY_ADAPTER */
+       /* IFACE_ID1 is equals to SECONDARY_ADAPTER */
+	u8 iface_id;
+
+	/* for debug purpose */
+	u8 fix_rate;
+	u8 driver_vcs_en; /* Enable = 1, Disable = 0 driver control vrtl_carrier_sense for tx */
+	u8 driver_vcs_type;/* force 0:disable VCS, 1:RTS-CTS, 2:CTS-to-self when vcs_en = 1. */
+	u8 driver_ampdu_spacing;/* driver control AMPDU Density for peer sta's rx */
+	u8 driver_rx_ampdu_factor;/* 0xff: disable drv ctrl, 0:8k, 1:16k, 2:32k, 3:64k; */
+
+	unsigned char     in_cta_test;
+};
+
+#define adapter_to_dvobj(adapter) (adapter->dvobj)
+#define adapter_to_pwrctl(adapter) (dvobj_to_pwrctl(adapter->dvobj))
+#define adapter_wdev_data(adapter) (&((adapter)->wdev_data))
+
+/*  */
+/*  Function disabled. */
+/*  */
+#define DF_TX_BIT		BIT0
+#define DF_RX_BIT		BIT1
+#define DF_IO_BIT		BIT2
+
+/* define RTW_DISABLE_FUNC(padapter, func) (atomic_add(&adapter_to_dvobj(padapter)->disable_func, (func))) */
+/* define RTW_ENABLE_FUNC(padapter, func) (atomic_sub(&adapter_to_dvobj(padapter)->disable_func, (func))) */
+static inline void RTW_DISABLE_FUNC(struct adapter *padapter, int func_bit)
+{
+	int	df = atomic_read(&adapter_to_dvobj(padapter)->disable_func);
+	df |= func_bit;
+	atomic_set(&adapter_to_dvobj(padapter)->disable_func, df);
+}
+
+static inline void RTW_ENABLE_FUNC(struct adapter *padapter, int func_bit)
+{
+	int	df = atomic_read(&adapter_to_dvobj(padapter)->disable_func);
+	df &= ~(func_bit);
+	atomic_set(&adapter_to_dvobj(padapter)->disable_func, df);
+}
+
+#define RTW_IS_FUNC_DISABLED(padapter, func_bit) (atomic_read(&adapter_to_dvobj(padapter)->disable_func) & (func_bit))
+
+#define RTW_CANNOT_IO(padapter) \
+			((padapter)->bSurpriseRemoved || \
+			 RTW_IS_FUNC_DISABLED((padapter), DF_IO_BIT))
+
+#define RTW_CANNOT_RX(padapter) \
+			((padapter)->bDriverStopped || \
+			 (padapter)->bSurpriseRemoved || \
+			 RTW_IS_FUNC_DISABLED((padapter), DF_RX_BIT))
+
+#define RTW_CANNOT_TX(padapter) \
+			((padapter)->bDriverStopped || \
+			 (padapter)->bSurpriseRemoved || \
+			 RTW_IS_FUNC_DISABLED((padapter), DF_TX_BIT))
+
+static inline u8 *myid(struct eeprom_priv *peepriv)
+{
+	return peepriv->mac_addr;
+}
+
+/*  HCI Related header file */
+#include <sdio_osintf.h>
+#include <sdio_ops.h>
+#include <sdio_hal.h>
+
+#include <rtw_btcoex.h>
+
+extern char *rtw_initmac;
+extern int rtw_mc2u_disable;
+extern int rtw_ht_enable;
+extern u32 g_wait_hiq_empty;
+extern u8 g_fwdl_wintint_rdy_fail;
+extern u8 g_fwdl_chksum_fail;
+
+#endif /* __DRV_TYPES_H__ */
diff --git a/drivers/staging/rtl8723bs/include/drv_types_sdio.h b/drivers/staging/rtl8723bs/include/drv_types_sdio.h
new file mode 100644
index 000000000..25b3c3a39
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/drv_types_sdio.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __DRV_TYPES_SDIO_H__
+#define __DRV_TYPES_SDIO_H__
+
+/*  SDIO Header Files */
+	#include <linux/mmc/sdio_func.h>
+	#include <linux/mmc/sdio_ids.h>
+
+struct sdio_data {
+	u8  func_number;
+
+	u8  tx_block_mode;
+	u8  rx_block_mode;
+	u32 block_transfer_len;
+
+	struct sdio_func	 *func;
+	void *sys_sdio_irq_thd;
+};
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_btcoex.h b/drivers/staging/rtl8723bs/include/hal_btcoex.h
new file mode 100644
index 000000000..fb167642d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_btcoex.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_BTCOEX_H__
+#define __HAL_BTCOEX_H__
+
+#include <drv_types.h>
+
+#define LPS_RPWM_WAIT_MS	300
+
+/*  Some variables can't get from outsrc BT-Coex, */
+/*  so we need to save here */
+struct bt_coexist {
+	u8 bBtExist;
+	u8 btTotalAntNum;
+	u8 btChipType;
+	u8 bInitlized;
+};
+
+void hal_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist);
+bool hal_btcoex_IsBtExist(struct adapter *padapter);
+bool hal_btcoex_IsBtDisabled(struct adapter *);
+void hal_btcoex_SetChipType(struct adapter *padapter, u8 chipType);
+void hal_btcoex_SetPgAntNum(struct adapter *padapter, u8 antNum);
+void hal_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath);
+
+void hal_btcoex_Initialize(void *padapter);
+void hal_btcoex_PowerOnSetting(struct adapter *padapter);
+void hal_btcoex_InitHwConfig(struct adapter *padapter, u8 bWifiOnly);
+
+void hal_btcoex_IpsNotify(struct adapter *padapter, u8 type);
+void hal_btcoex_LpsNotify(struct adapter *padapter, u8 type);
+void hal_btcoex_ScanNotify(struct adapter *padapter, u8 type);
+void hal_btcoex_ConnectNotify(struct adapter *padapter, u8 action);
+void hal_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus);
+void hal_btcoex_SpecialPacketNotify(struct adapter *padapter, u8 pktType);
+void hal_btcoex_IQKNotify(struct adapter *padapter, u8 state);
+void hal_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf);
+void hal_btcoex_SuspendNotify(struct adapter *padapter, u8 state);
+void hal_btcoex_HaltNotify(struct adapter *padapter);
+
+void hal_btcoex_Handler(struct adapter *padapter);
+
+s32 hal_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter);
+bool hal_btcoex_IsBtControlLps(struct adapter *padapter);
+bool hal_btcoex_IsLpsOn(struct adapter *padapter);
+u8 hal_btcoex_RpwmVal(struct adapter *);
+u8 hal_btcoex_LpsVal(struct adapter *);
+u32 hal_btcoex_GetRaMask(struct adapter *);
+void hal_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen);
+
+#endif /*  !__HAL_BTCOEX_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_com.h b/drivers/staging/rtl8723bs/include/hal_com.h
new file mode 100644
index 000000000..6356b8c2e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_COMMON_H__
+#define __HAL_COMMON_H__
+
+#include "HalVerDef.h"
+#include "hal_pg.h"
+#include "hal_phy.h"
+#include "hal_phy_reg.h"
+#include "hal_com_reg.h"
+#include "hal_com_phycfg.h"
+
+/*------------------------------ Tx Desc definition Macro ------------------------*/
+/* pragma mark -- Tx Desc related definition. -- */
+/*  */
+/*  */
+/* 	Rate */
+/*  */
+/*  CCK Rates, TxHT = 0 */
+#define DESC_RATE1M					0x00
+#define DESC_RATE2M					0x01
+#define DESC_RATE5_5M				0x02
+#define DESC_RATE11M				0x03
+
+/*  OFDM Rates, TxHT = 0 */
+#define DESC_RATE6M					0x04
+#define DESC_RATE9M					0x05
+#define DESC_RATE12M				0x06
+#define DESC_RATE18M				0x07
+#define DESC_RATE24M				0x08
+#define DESC_RATE36M				0x09
+#define DESC_RATE48M				0x0a
+#define DESC_RATE54M				0x0b
+
+/*  MCS Rates, TxHT = 1 */
+#define DESC_RATEMCS0				0x0c
+#define DESC_RATEMCS1				0x0d
+#define DESC_RATEMCS2				0x0e
+#define DESC_RATEMCS3				0x0f
+#define DESC_RATEMCS4				0x10
+#define DESC_RATEMCS5				0x11
+#define DESC_RATEMCS6				0x12
+#define DESC_RATEMCS7				0x13
+
+#define HDATA_RATE(rate)\
+(rate == DESC_RATE1M) ? "CCK_1M" : \
+(rate == DESC_RATE2M) ? "CCK_2M" : \
+(rate == DESC_RATE5_5M) ? "CCK5_5M" : \
+(rate == DESC_RATE11M) ? "CCK_11M" : \
+(rate == DESC_RATE6M) ? "OFDM_6M" : \
+(rate == DESC_RATE9M) ? "OFDM_9M" : \
+(rate == DESC_RATE12M) ? "OFDM_12M" : \
+(rate == DESC_RATE18M) ? "OFDM_18M" : \
+(rate == DESC_RATE24M) ? "OFDM_24M" : \
+(rate == DESC_RATE36M) ? "OFDM_36M" : \
+(rate == DESC_RATE48M) ? "OFDM_48M" : \
+(rate == DESC_RATE54M) ? "OFDM_54M" : \
+(rate == DESC_RATEMCS0) ? "MCS0" : \
+(rate == DESC_RATEMCS1) ? "MCS1" : \
+(rate == DESC_RATEMCS2) ? "MCS2" : \
+(rate == DESC_RATEMCS3) ? "MCS3" : \
+(rate == DESC_RATEMCS4) ? "MCS4" : \
+(rate == DESC_RATEMCS5) ? "MCS5" : \
+(rate == DESC_RATEMCS6) ? "MCS6" : \
+(rate == DESC_RATEMCS7) ? "MCS7" : "UNKNOWN"
+
+enum{
+	UP_LINK,
+	DOWN_LINK,
+};
+enum rt_media_status {
+	RT_MEDIA_DISCONNECT = 0,
+	RT_MEDIA_CONNECT       = 1
+};
+
+#define MAX_DLFW_PAGE_SIZE			4096	/*  @ page : 4k bytes */
+
+/*  BK, BE, VI, VO, HCCA, MANAGEMENT, COMMAND, HIGH, BEACON. */
+/* define MAX_TX_QUEUE		9 */
+
+#define TX_SELE_HQ			BIT(0)		/*  High Queue */
+#define TX_SELE_LQ			BIT(1)		/*  Low Queue */
+#define TX_SELE_NQ			BIT(2)		/*  Normal Queue */
+#define TX_SELE_EQ			BIT(3)		/*  Extern Queue */
+
+#define PageNum_128(_Len)		((u32)(((_Len) >> 7) + ((_Len) & 0x7F ? 1 : 0)))
+
+u8 rtw_hal_data_init(struct adapter *padapter);
+void rtw_hal_data_deinit(struct adapter *padapter);
+
+void dump_chip_info(struct hal_version	ChipVersion);
+
+u8 /* return the final channel plan decision */
+hal_com_config_channel_plan(
+struct adapter *padapter,
+u8 	hw_channel_plan,	/* channel plan from HW (efuse/eeprom) */
+u8 	sw_channel_plan,	/* channel plan from SW (registry/module param) */
+u8 	def_channel_plan,	/* channel plan used when the former two is invalid */
+bool		AutoLoadFail
+	);
+
+bool
+HAL_IsLegalChannel(
+struct adapter *Adapter,
+u32 		Channel
+	);
+
+u8 MRateToHwRate(u8 rate);
+
+u8 HwRateToMRate(u8 rate);
+
+void HalSetBrateCfg(
+	struct adapter *Adapter,
+	u8 *mBratesOS,
+	u16	*pBrateCfg);
+
+bool
+Hal_MappingOutPipe(
+struct adapter *padapter,
+u8 NumOutPipe
+	);
+
+void hal_init_macaddr(struct adapter *adapter);
+
+void rtw_init_hal_com_default_value(struct adapter *Adapter);
+
+void c2h_evt_clear(struct adapter *adapter);
+s32 c2h_evt_read_88xx(struct adapter *adapter, u8 *buf);
+
+u8 rtw_get_mgntframe_raid(struct adapter *adapter, unsigned char network_type);
+void rtw_hal_update_sta_rate_mask(struct adapter *padapter, struct sta_info *psta);
+
+void hw_var_port_switch(struct adapter *adapter);
+
+void SetHwReg(struct adapter *padapter, u8 variable, u8 *val);
+void GetHwReg(struct adapter *padapter, u8 variable, u8 *val);
+void rtw_hal_check_rxfifo_full(struct adapter *adapter);
+
+u8 SetHalDefVar(struct adapter *adapter, enum hal_def_variable variable,
+		void *value);
+u8 GetHalDefVar(struct adapter *adapter, enum hal_def_variable variable,
+		void *value);
+
+bool eqNByte(u8 *str1, u8 *str2, u32 num);
+
+u32 MapCharToHexDigit(char chTmp);
+
+bool ParseQualifiedString(char *In, u32 *Start, char *Out, char LeftQualifier,
+			  char RightQualifier);
+
+bool GetU1ByteIntegerFromStringInDecimal(char *str, u8 *in);
+
+bool isAllSpaceOrTab(u8 *data, u8 size);
+
+void linked_info_dump(struct adapter *padapter, u8 benable);
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+void rtw_get_raw_rssi_info(void *sel, struct adapter *padapter);
+void rtw_store_phy_info(struct adapter *padapter, union recv_frame *prframe);
+void rtw_dump_raw_rssi_info(struct adapter *padapter);
+#endif
+
+#define		HWSET_MAX_SIZE			512
+
+void rtw_bb_rf_gain_offset(struct adapter *padapter);
+
+void GetHalODMVar(struct adapter *Adapter,
+	enum hal_odm_variable		eVariable,
+	void *pValue1,
+	void *pValue2);
+void SetHalODMVar(
+	struct adapter *Adapter,
+	enum hal_odm_variable		eVariable,
+	void *pValue1,
+	bool					bSet);
+#endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_com_h2c.h b/drivers/staging/rtl8723bs/include/hal_com_h2c.h
new file mode 100644
index 000000000..24cd9415f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_h2c.h
@@ -0,0 +1,104 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __COMMON_H2C_H__
+#define __COMMON_H2C_H__
+
+#define H2C_RSVDPAGE_LOC_LEN		5
+#define H2C_MEDIA_STATUS_RPT_LEN		3
+#define H2C_KEEP_ALIVE_CTRL_LEN	2
+#define H2C_DISCON_DECISION_LEN		3
+#define H2C_AP_OFFLOAD_LEN		3
+#define H2C_AP_WOW_GPIO_CTRL_LEN	4
+#define H2C_AP_PS_LEN			2
+#define H2C_PWRMODE_LEN			7
+#define H2C_PSTUNEPARAM_LEN			4
+#define H2C_MACID_CFG_LEN		7
+#define H2C_BTMP_OPER_LEN			4
+#define H2C_WOWLAN_LEN			4
+#define H2C_REMOTE_WAKE_CTRL_LEN	3
+#define H2C_AOAC_GLOBAL_INFO_LEN	2
+#define H2C_AOAC_RSVDPAGE_LOC_LEN	7
+#define H2C_SCAN_OFFLOAD_CTRL_LEN	4
+#define H2C_BT_FW_PATCH_LEN			6
+#define H2C_RSSI_SETTING_LEN		4
+#define H2C_AP_REQ_TXRPT_LEN		2
+#define H2C_FORCE_BT_TXPWR_LEN		3
+#define H2C_BCN_RSVDPAGE_LEN		5
+#define H2C_PROBERSP_RSVDPAGE_LEN	5
+
+/* _RSVDPAGE_LOC_CMD_0x00 */
+#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(__pH2CCmd, __Value)SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+4, 0, 8, __Value)
+
+/* _MEDIA_STATUS_RPT_PARM_CMD_0x01 */
+#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_END(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+2, 0, 8, __Value)
+
+/* _KEEP_ALIVE_CMD_0x03 */
+#define SET_H2CCMD_KEEPALIVE_PARM_ENABLE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_KEEPALIVE_PARM_ADOPT(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_KEEPALIVE_PARM_PKT_TYPE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_H2CCMD_KEEPALIVE_PARM_CHECK_PERIOD(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+
+/* _DISCONNECT_DECISION_CMD_0x04 */
+#define SET_H2CCMD_DISCONDECISION_PARM_ENABLE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_DISCONDECISION_PARM_ADOPT(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_DISCONDECISION_PARM_CHECK_PERIOD(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+#define SET_H2CCMD_DISCONDECISION_PARM_TRY_PKT_NUM(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+2, 0, 8, __Value)
+
+/*  _WoWLAN PARAM_CMD_0x80 */
+#define SET_H2CCMD_WOWLAN_FUNC_ENABLE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
+#define SET_H2CCMD_WOWLAN_ALL_PKT_DROP(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 1, __Value)
+#define SET_H2CCMD_WOWLAN_GPIO_ACTIVE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 5, 1, __Value)
+#define SET_H2CCMD_WOWLAN_REKEY_WAKE_UP(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 6, 1, __Value)
+#define SET_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 7, 1, __Value)
+#define SET_H2CCMD_WOWLAN_GPIONUM(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 7, __Value)
+#define SET_H2CCMD_WOWLAN_DATAPIN_WAKE_UP(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 7, 1, __Value)
+#define SET_H2CCMD_WOWLAN_GPIO_DURATION(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+/* define SET_H2CCMD_WOWLAN_GPIO_PULSE_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 1, __Value) */
+#define SET_H2CCMD_WOWLAN_GPIO_PULSE_COUNT(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+
+/*  _REMOTE_WAKEUP_CMD_0x81 */
+#define SET_H2CCMD_REMOTE_WAKECTRL_ENABLE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_NLO_OFFLOAD_EN(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_FW_UNICAST_EN(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 7, 1, __Value)
+#define SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_ACTION(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 1, __Value)
+
+/*  AOAC_GLOBAL_INFO_0x82 */
+#define SET_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+
+/*  AOAC_RSVDPAGE_LOC_0x83 */
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd), 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+4, 0, 8, __Value)
+
+/*  */
+/*     Structure    -------------------------------------------------- */
+/*  */
+struct rsvdpage_loc {
+	u8 LocProbeRsp;
+	u8 LocPsPoll;
+	u8 LocNullData;
+	u8 LocQosNull;
+	u8 LocBTQosNull;
+};
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_com_phycfg.h b/drivers/staging/rtl8723bs/include/hal_com_phycfg.h
new file mode 100644
index 000000000..cb7c7ed74
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_phycfg.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_COM_PHYCFG_H__
+#define __HAL_COM_PHYCFG_H__
+
+#define		PathA		0x0	/*  Useless */
+#define		PathB		0x1
+#define		PathC		0x2
+#define		PathD		0x3
+
+enum rate_section {
+	CCK = 0,
+	OFDM,
+	HT_MCS0_MCS7,
+};
+
+#define MAX_POWER_INDEX			0x3F
+
+enum {
+	TXPWR_LMT_FCC = 0,
+	TXPWR_LMT_MKK,
+	TXPWR_LMT_ETSI,
+	TXPWR_LMT_WW,
+	TXPWR_LMT_MAX_REGULATION_NUM,
+};
+
+/*------------------------------Define structure----------------------------*/
+struct bb_register_def {
+	u32 rfintfs;			/*  set software control: */
+					/* 	0x870~0x877[8 bytes] */
+
+	u32 rfintfo;			/*  output data: */
+					/* 	0x860~0x86f [16 bytes] */
+
+	u32 rfintfe;			/*  output enable: */
+					/* 	0x860~0x86f [16 bytes] */
+
+	u32 rf3wireOffset;		/*  LSSI data: */
+					/* 	0x840~0x84f [16 bytes] */
+
+	u32 rfHSSIPara2;		/*  wire parameter control2 : */
+					/* 	0x824~0x827, 0x82c~0x82f,
+					 *	0x834~0x837, 0x83c~0x83f
+					 */
+	u32 rfLSSIReadBack;		/* LSSI RF readback data SI mode */
+					/* 	0x8a0~0x8af [16 bytes] */
+
+	u32 rfLSSIReadBackPi;		/* LSSI RF readback data PI mode
+					 *	0x8b8-8bc for Path A and B */
+
+};
+
+u8 PHY_GetTxPowerByRateBase(struct adapter *Adapter, u8 RfPath,
+			    enum rate_section RateSection);
+
+u8 PHY_GetRateSectionIndexOfTxPowerByRate(struct adapter *padapter, u32	RegAddr,
+					  u32 BitMask);
+
+void PHY_GetRateValuesOfTxPowerByRate(struct adapter *padapter, u32 RegAddr,
+				      u32 BitMask, u32 Value, u8 *RateIndex,
+				      s8 *PwrByRateVal, u8 *RateNum);
+
+u8 PHY_GetRateIndexOfTxPowerByRate(u8 Rate);
+
+void PHY_SetTxPowerIndexByRateSection(struct adapter *padapter, u8 RFPath, u8 Channel,
+				      u8 RateSection);
+
+s8 PHY_GetTxPowerByRate(struct adapter *padapter, u8 RFPath, u8 RateIndex);
+
+void PHY_SetTxPowerByRate(struct adapter *padapter, u8 RFPath, u8 Rate,
+			  s8 Value);
+
+void PHY_SetTxPowerLevelByPath(struct adapter *Adapter, u8 channel, u8 path);
+
+void PHY_SetTxPowerIndexByRateArray(struct adapter *padapter, u8 RFPath,
+				    enum channel_width BandWidth, u8 Channel,
+				    u8 *Rates, u8 RateArraySize);
+
+void PHY_InitTxPowerByRate(struct adapter *padapter);
+
+void PHY_StoreTxPowerByRate(struct adapter *padapter, u32 RfPath,
+			    u32	RegAddr, u32 BitMask, u32 Data);
+
+void PHY_TxPowerByRateConfiguration(struct adapter *padapter);
+
+u8 PHY_GetTxPowerIndexBase(struct adapter *padapter, u8 RFPath, u8 Rate,
+			   enum channel_width BandWidth, u8 Channel);
+
+s8 phy_get_tx_pwr_lmt(struct adapter *adapter, u32 RegPwrTblSel,
+		      enum channel_width Bandwidth, u8 RfPath, u8 DataRate,
+		      u8 Channel);
+
+void PHY_SetTxPowerLimit(struct adapter *Adapter, u8 *Regulation, u8 *Bandwidth,
+			 u8 *RateSection, u8 *RfPath, u8 *Channel, u8 *PowerLimit);
+
+void PHY_ConvertTxPowerLimitToPowerIndex(struct adapter *Adapter);
+
+void PHY_InitTxPowerLimit(struct adapter *Adapter);
+
+s8 PHY_GetTxPowerTrackingOffset(struct adapter *padapter, u8 Rate, u8 RFPath);
+
+void Hal_ChannelPlanToRegulation(struct adapter *Adapter, u16 ChannelPlan);
+
+#endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_com_reg.h b/drivers/staging/rtl8723bs/include/hal_com_reg.h
new file mode 100644
index 000000000..d8d03752d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_reg.h
@@ -0,0 +1,1394 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_COMMON_REG_H__
+#define __HAL_COMMON_REG_H__
+
+
+#define MAC_ADDR_LEN				6
+
+#define HAL_NAV_UPPER_UNIT		128		/*  micro-second */
+
+/*  8188E PKT_BUFF_ACCESS_CTRL value */
+#define TXPKT_BUF_SELECT				0x69
+#define RXPKT_BUF_SELECT				0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+
+/*  */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/* 	0x0000h ~ 0x00FFh	System Configuration */
+/*  */
+/*  */
+#define REG_SYS_ISO_CTRL				0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO					0x0004
+#define REG_SYS_CLKR					0x0008
+#define REG_9346CR						0x000A
+#define REG_SYS_EEPROM_CTRL			0x000A
+#define REG_EE_VPD						0x000C
+#define REG_AFE_MISC					0x0010
+#define REG_SPS0_CTRL					0x0011
+#define REG_SPS0_CTRL_6					0x0016
+#define REG_POWER_OFF_IN_PROCESS		0x0017
+#define REG_SPS_OCP_CFG				0x0018
+#define REG_RSV_CTRL					0x001C
+#define REG_RF_CTRL						0x001F
+#define REG_LDOA15_CTRL				0x0020
+#define REG_LDOV12D_CTRL				0x0021
+#define REG_LDOHCI12_CTRL				0x0022
+#define REG_LPLDO_CTRL					0x0023
+#define REG_AFE_XTAL_CTRL				0x0024
+#define REG_AFE_LDO_CTRL				0x0027 /*  1.5v for 8188EE test chip, 1.4v for MP chip */
+#define REG_AFE_PLL_CTRL				0x0028
+#define REG_MAC_PHY_CTRL				0x002c /* for 92d, DMDP, SMSP, DMSP contrl */
+#define REG_APE_PLL_CTRL_EXT			0x002c
+#define REG_EFUSE_CTRL					0x0030
+#define REG_EFUSE_TEST					0x0034
+#define REG_PWR_DATA					0x0038
+#define REG_CAL_TIMER					0x003C
+#define REG_ACLK_MON					0x003E
+#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_IO_SEL					0x0042
+#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_GPIO_PIN_CTRL				0x0044
+#define REG_GPIO_INTM					0x0048
+#define REG_LEDCFG0						0x004C
+#define REG_LEDCFG1						0x004D
+#define REG_LEDCFG2						0x004E
+#define REG_LEDCFG3						0x004F
+#define REG_FSIMR						0x0050
+#define REG_FSISR						0x0054
+#define REG_HSIMR						0x0058
+#define REG_HSISR						0x005c
+#define REG_GPIO_PIN_CTRL_2			0x0060 /*  RTL8723 WIFI/BT/GPS Multi-Function GPIO Pin Control. */
+#define REG_GPIO_IO_SEL_2				0x0062 /*  RTL8723 WIFI/BT/GPS Multi-Function GPIO Select. */
+#define REG_MULTI_FUNC_CTRL			0x0068 /*  RTL8723 WIFI/BT/GPS Multi-Function control source. */
+#define REG_GSSR						0x006c
+#define REG_AFE_XTAL_CTRL_EXT			0x0078 /* RTL8188E */
+#define REG_MCUFWDL					0x0080
+#define REG_MCUTSTCFG					0x0084
+#define REG_FDHM0						0x0088
+#define REG_EFUSE_ACCESS				0x00CF	/*  Efuse access protection for RTL8723 */
+#define REG_BIST_SCAN					0x00D0
+#define REG_BIST_RPT					0x00D4
+#define REG_BIST_ROM_RPT				0x00D8
+#define REG_USB_SIE_INTF				0x00E0
+#define REG_PCIE_MIO_INTF				0x00E4
+#define REG_PCIE_MIO_INTD				0x00E8
+#define REG_HPON_FSM					0x00EC
+#define REG_SYS_CFG						0x00F0
+#define REG_GPIO_OUTSTS				0x00F4	/*  For RTL8723 only. */
+#define REG_TYPE_ID						0x00FC
+
+/*  */
+/*  2010/12/29 MH Add for 92D */
+/*  */
+#define REG_MAC_PHY_CTRL_NORMAL		0x00f8
+
+
+/*  */
+/*  */
+/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+#define REG_CR							0x0100
+#define REG_PBP							0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY					0x0114
+#define REG_TRXFF_STATUS				0x0118
+#define REG_RXFF_PTR					0x011C
+#define REG_HIMR						0x0120
+#define REG_HISR						0x0124
+#define REG_HIMRE						0x0128
+#define REG_HISRE						0x012C
+#define REG_CPWM						0x012F
+#define REG_FWIMR						0x0130
+#define REG_FWISR						0x0134
+#define REG_FTIMR						0x0138
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_RXPKTBUF_CTRL				(REG_PKTBUF_DBG_CTRL+2)
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H		0x0148
+
+#define REG_TC0_CTRL					0x0150
+#define REG_TC1_CTRL					0x0154
+#define REG_TC2_CTRL					0x0158
+#define REG_TC3_CTRL					0x015C
+#define REG_TC4_CTRL					0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_MBIST_START				0x0174
+#define REG_MBIST_DONE					0x0178
+#define REG_MBIST_FAIL					0x017C
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
+#define REG_C2HEVT_CLEAR				0x01AF
+#define REG_MCUTST_1					0x01c0
+#define REG_FMETHR						0x01C8
+#define REG_HMETFR						0x01CC
+#define REG_HMEBOX_0					0x01D0
+#define REG_HMEBOX_1					0x01D4
+#define REG_HMEBOX_2					0x01D8
+#define REG_HMEBOX_3					0x01DC
+#define REG_LLT_INIT					0x01E0
+
+
+/*  */
+/*  */
+/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+#define REG_RQPN						0x0200
+#define REG_FIFOPAGE					0x0204
+#define REG_TDECTRL						0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS				0x0210
+#define REG_RQPN_NPQ					0x0214
+#define REG_AUTO_LLT					0x0224
+
+
+/*  */
+/*  */
+/* 	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*  */
+/*  */
+#define REG_RXDMA_AGG_PG_TH			0x0280
+#define REG_RXPKT_NUM					0x0284
+#define REG_RXDMA_STATUS				0x0288
+
+/*  */
+/*  */
+/* 	0x0300h ~ 0x03FFh	PCIe */
+/*  */
+/*  */
+#define REG_PCIE_CTRL_REG				0x0300
+#define REG_INT_MIG						0x0304	/*  Interrupt Migration */
+#define REG_BCNQ_DESA					0x0308	/*  TX Beacon Descriptor Address */
+#define REG_HQ_DESA					0x0310	/*  TX High Queue Descriptor Address */
+#define REG_MGQ_DESA					0x0318	/*  TX Manage Queue Descriptor Address */
+#define REG_VOQ_DESA					0x0320	/*  TX VO Queue Descriptor Address */
+#define REG_VIQ_DESA					0x0328	/*  TX VI Queue Descriptor Address */
+#define REG_BEQ_DESA					0x0330	/*  TX BE Queue Descriptor Address */
+#define REG_BKQ_DESA					0x0338	/*  TX BK Queue Descriptor Address */
+#define REG_RX_DESA						0x0340	/*  RX Queue	Descriptor Address */
+/* sherry added for DBI Read/Write  20091126 */
+#define REG_DBI_WDATA					0x0348	/*  Backdoor REG for Access Configuration */
+#define REG_DBI_RDATA				0x034C	/* Backdoor REG for Access Configuration */
+#define REG_DBI_CTRL					0x0350	/* Backdoor REG for Access Configuration */
+#define REG_DBI_FLAG					0x0352	/* Backdoor REG for Access Configuration */
+#define REG_MDIO						0x0354	/*  MDIO for Access PCIE PHY */
+#define REG_DBG_SEL						0x0360	/*  Debug Selection Register */
+#define REG_PCIE_HRPWM					0x0361	/* PCIe RPWM */
+#define REG_PCIE_HCPWM					0x0363	/* PCIe CPWM */
+#define REG_WATCH_DOG					0x0368
+
+/*  RTL8723 series ------------------------------- */
+#define REG_PCIE_HISR_EN				0x0394	/* PCIE Local Interrupt Enable Register */
+#define REG_PCIE_HISR					0x03A0
+#define REG_PCIE_HISRE					0x03A4
+#define REG_PCIE_HIMR					0x03A8
+#define REG_PCIE_HIMRE					0x03AC
+
+#define REG_USB_HIMR					0xFE38
+#define REG_USB_HIMRE					0xFE3C
+#define REG_USB_HISR					0xFE78
+#define REG_USB_HISRE					0xFE7C
+
+
+/*  */
+/*  */
+/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+#define REG_CPU_MGQ_INFORMATION		0x041C
+#define REG_FWHW_TXQ_CTRL				0x0420
+#define REG_HWSEQ_CTRL					0x0423
+#define REG_BCNQ_BDNY					0x0424
+#define REG_MGQ_BDNY					0x0425
+#define REG_LIFETIME_CTRL				0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS					0x0428
+#define REG_RL							0x042A
+#define REG_DARFRC						0x0430
+#define REG_RARFRC						0x0438
+#define REG_RRSR						0x0440
+#define REG_ARFR0						0x0444
+#define REG_ARFR1						0x0448
+#define REG_ARFR2						0x044C
+#define REG_ARFR3						0x0450
+#define REG_BCNQ1_BDNY					0x0457
+
+#define REG_AGGLEN_LMT					0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_WMAC_LBK_BF_HD			0x045D
+#define REG_FAST_EDCA_CTRL				0x0460
+#define REG_RD_RESP_PKT_TH				0x0463
+
+#define REG_INIRTS_RATE_SEL				0x0480
+#define REG_INIDATA_RATE_SEL			0x0484
+
+#define REG_POWER_STAGE1				0x04B4
+#define REG_POWER_STAGE2				0x04B8
+#define REG_PKT_VO_VI_LIFE_TIME		0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME		0x04C2
+#define REG_STBC_SETTING				0x04C4
+#define REG_QUEUE_CTRL					0x04C6
+#define REG_SINGLE_AMPDU_CTRL			0x04c7
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM				0x04CA
+#define REG_RTS_MAX_AGGR_NUM			0x04CB
+#define REG_BAR_MODE_CTRL				0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
+#define REG_EARLY_MODE_CONTROL		0x04D0
+#define REG_MACID_SLEEP				0x04D4
+#define REG_NQOS_SEQ					0x04DC
+#define REG_QOS_SEQ					0x04DE
+#define REG_NEED_CPU_HANDLE			0x04E0
+#define REG_PKT_LOSE_RPT				0x04E1
+#define REG_PTCL_ERR_STATUS			0x04E2
+#define REG_TX_RPT_CTRL					0x04EC
+#define REG_TX_RPT_TIME					0x04F0	/*  2 byte */
+#define REG_DUMMY						0x04FC
+
+/*  */
+/*  */
+/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+#define REG_EDCA_VO_PARAM				0x0500
+#define REG_EDCA_VI_PARAM				0x0504
+#define REG_EDCA_BE_PARAM				0x0508
+#define REG_EDCA_BK_PARAM				0x050C
+#define REG_BCNTCFG						0x0510
+#define REG_PIFS							0x0512
+#define REG_RDG_PIFS					0x0513
+#define REG_SIFS_CTX					0x0514
+#define REG_SIFS_TRX					0x0516
+#define REG_TSFTR_SYN_OFFSET			0x0518
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT						0x051B
+#define REG_TX_PTCL_CTRL				0x0520
+#define REG_TXPAUSE						0x0522
+#define REG_DIS_TXREQ_CLR				0x0523
+#define REG_RD_CTRL						0x0524
+/*  */
+/*  Format for offset 540h-542h: */
+/* 	[3:0]:   TBTT prohibit setup in unit of 32us. The time for HW getting beacon content before TBTT. */
+/* 	[7:4]:   Reserved. */
+/* 	[19:8]:  TBTT prohibit hold in unit of 32us. The time for HW holding to send the beacon packet. */
+/* 	[23:20]: Reserved */
+/*  Description: */
+/* 	              | */
+/*      |<--Setup--|--Hold------------>| */
+/* 	--------------|---------------------- */
+/*                 | */
+/*                TBTT */
+/*  Note: We cannot update beacon content to HW or send any AC packets during the time between Setup and Hold. */
+/*  Described by Designer Tim and Bruce, 2011-01-14. */
+/*  */
+#define REG_TBTT_PROHIBIT				0x0540
+#define REG_RD_NAV_NXT					0x0544
+#define REG_NAV_PROT_LEN				0x0546
+#define REG_BCN_CTRL					0x0550
+#define REG_BCN_CTRL_1					0x0551
+#define REG_MBID_NUM					0x0552
+#define REG_DUAL_TSF_RST				0x0553
+#define REG_BCN_INTERVAL				0x0554	/*  The same as REG_MBSSID_BCN_SPACE */
+#define REG_DRVERLYINT					0x0558
+#define REG_BCNDMATIM					0x0559
+#define REG_ATIMWND					0x055A
+#define REG_USTIME_TSF					0x055C
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK			0x055E
+#define REG_RXTSF_OFFSET_OFDM			0x055F
+#define REG_TSFTR						0x0560
+#define REG_TSFTR1						0x0568	/*  HW Port 1 TSF Register */
+#define REG_ATIMWND_1					0x0570
+#define REG_P2P_CTWIN					0x0572 /*  1 Byte long (in unit of TU) */
+#define REG_PSTIMER						0x0580
+#define REG_TIMER0						0x0584
+#define REG_TIMER1						0x0588
+#define REG_ACMHWCTRL					0x05C0
+#define REG_NOA_DESC_SEL				0x05CF
+#define REG_NOA_DESC_DURATION		0x05E0
+#define REG_NOA_DESC_INTERVAL			0x05E4
+#define REG_NOA_DESC_START			0x05E8
+#define REG_NOA_DESC_COUNT			0x05EC
+
+#define REG_DMC							0x05F0	/* Dual MAC Co-Existence Register */
+#define REG_SCH_TX_CMD					0x05F8
+
+#define REG_FW_RESET_TSF_CNT_1		0x05FC
+#define REG_FW_RESET_TSF_CNT_0		0x05FD
+#define REG_FW_BCN_DIS_CNT			0x05FE
+
+/*  */
+/*  */
+/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*  */
+/*  */
+#define REG_APSD_CTRL					0x0600
+#define REG_BWOPMODE					0x0603
+#define REG_TCR							0x0604
+#define REG_RCR							0x0608
+#define REG_RX_PKT_LIMIT				0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ				0x060F
+
+#define REG_MACID						0x0610
+#define REG_BSSID						0x0618
+#define REG_MAR							0x0620
+#define REG_MBIDCAMCFG					0x0628
+
+#define REG_PNO_STATUS					0x0631
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS				0x063A
+/*  20100719 Joseph: Hardware register definition change. (HW datasheet v54) */
+#define REG_RESP_SIFS_CCK				0x063C	/*  [15:8]SIFS_R2T_OFDM, [7:0]SIFS_R2T_CCK */
+#define REG_RESP_SIFS_OFDM                    0x063E	/*  [15:8]SIFS_T2T_OFDM, [7:0]SIFS_T2T_CCK */
+
+#define REG_ACKTO						0x0640
+#define REG_CTS2TO						0x0641
+#define REG_EIFS							0x0642
+
+
+/* RXERR_RPT */
+#define RXERR_TYPE_OFDM_PPDU			0
+#define RXERR_TYPE_OFDMfalse_ALARM	1
+#define RXERR_TYPE_OFDM_MPDU_OK			2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
+#define RXERR_TYPE_CCK_PPDU			4
+#define RXERR_TYPE_CCKfalse_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL		7
+#define RXERR_TYPE_HT_PPDU				8
+#define RXERR_TYPE_HTfalse_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL		10
+#define RXERR_TYPE_HT_MPDU_OK			11
+#define RXERR_TYPE_HT_MPDU_FAIL			12
+#define RXERR_TYPE_RX_FULL_DROP			15
+
+#define RXERR_COUNTER_MASK			0xFFFFF
+#define RXERR_RPT_RST					BIT(27)
+#define _RXERR_RPT_SEL(type)			((type) << 28)
+
+/*  */
+/*  Note: */
+/* 	The NAV upper value is very important to WiFi 11n 5.2.3 NAV test. The default value is */
+/* 	always too small, but the WiFi TestPlan test by 25, 000 microseconds of NAV through sending */
+/* 	CTS in the air. We must update this value greater than 25, 000 microseconds to pass the item. */
+/* 	The offset of NAV_UPPER in 8192C Spec is incorrect, and the offset should be 0x0652. Commented */
+/* 	by SD1 Scott. */
+/*  By Bruce, 2011-07-18. */
+/*  */
+#define REG_NAV_UPPER					0x0652	/*  unit of 128 */
+
+/* WMA, BA, CCX */
+#define REG_NAV_CTRL					0x0650
+#define REG_BACAMCMD					0x0654
+#define REG_BACAMCONTENT				0x0658
+#define REG_LBDLY						0x0660
+#define REG_FWDLY						0x0661
+#define REG_RXERR_RPT					0x0664
+#define REG_WMAC_TRXPTCL_CTL			0x0668
+
+/*  Security */
+#define REG_CAMCMD						0x0670
+#define REG_CAMWRITE					0x0674
+#define REG_CAMREAD					0x0678
+#define REG_CAMDBG						0x067C
+#define REG_SECCFG						0x0680
+
+/*  Power */
+#define REG_WOW_CTRL					0x0690
+#define REG_PS_RX_INFO					0x0692
+#define REG_UAPSD_TID					0x0693
+#define REG_WKFMCAM_CMD				0x0698
+#define REG_WKFMCAM_NUM				REG_WKFMCAM_CMD
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0					0x06A0
+#define REG_RXFLTMAP1					0x06A2
+#define REG_RXFLTMAP2					0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_BT_COEX_TABLE				0x06C0
+
+/*  Hardware Port 2 */
+#define REG_MACID1						0x0700
+#define REG_BSSID1						0x0708
+
+
+/*  */
+/*  */
+/* 	0xFE00h ~ 0xFE55h	USB Configuration */
+/*  */
+/*  */
+#define REG_USB_INFO					0xFE17
+#define REG_USB_SPECIAL_OPTION		0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO					0xFE5C
+#define REG_USB_AGG_TH					0xFE5D
+
+#define REG_USB_HRPWM					0xFE58
+#define REG_USB_HCPWM					0xFE57
+
+/*  for 92DU high_Queue low_Queue Normal_Queue select */
+#define REG_USB_High_NORMAL_Queue_Select_MAC0	0xFE44
+/* define REG_USB_LOW_Queue_Select_MAC0		0xFE45 */
+#define REG_USB_High_NORMAL_Queue_Select_MAC1	0xFE47
+/* define REG_USB_LOW_Queue_Select_MAC1		0xFE48 */
+
+/*  For test chip */
+#define REG_TEST_USB_TXQS				0xFE48
+#define REG_TEST_SIE_VID				0xFE60		/*  0xFE60~0xFE61 */
+#define REG_TEST_SIE_PID				0xFE62		/*  0xFE62~0xFE63 */
+#define REG_TEST_SIE_OPTIONAL			0xFE64
+#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_PHY				0xFE66		/*  0xFE66~0xFE6B */
+#define REG_TEST_SIE_MAC_ADDR			0xFE70		/*  0xFE70~0xFE75 */
+#define REG_TEST_SIE_STRING			0xFE80		/*  0xFE80~0xFEB9 */
+
+
+/*  For normal chip */
+#define REG_NORMAL_SIE_VID				0xFE60		/*  0xFE60~0xFE61 */
+#define REG_NORMAL_SIE_PID				0xFE62		/*  0xFE62~0xFE63 */
+#define REG_NORMAL_SIE_OPTIONAL		0xFE64
+#define REG_NORMAL_SIE_EP				0xFE65		/*  0xFE65~0xFE67 */
+#define REG_NORMAL_SIE_PHY			0xFE68		/*  0xFE68~0xFE6B */
+#define REG_NORMAL_SIE_OPTIONAL2		0xFE6C
+#define REG_NORMAL_SIE_GPS_EP			0xFE6D		/*  0xFE6D, for RTL8723 only. */
+#define REG_NORMAL_SIE_MAC_ADDR		0xFE70		/*  0xFE70~0xFE75 */
+#define REG_NORMAL_SIE_STRING			0xFE80		/*  0xFE80~0xFEDF */
+
+
+/*  */
+/*  */
+/* 	Redifine 8192C register definition for compatibility */
+/*  */
+/*  */
+
+/*  TODO: use these definition when using REG_xxx naming rule. */
+/*  NOTE: DO NOT Remove these definition. Use later. */
+
+#define EFUSE_CTRL				REG_EFUSE_CTRL		/*  E-Fuse Control. */
+#define EFUSE_TEST				REG_EFUSE_TEST		/*  E-Fuse Test. */
+#define MSR						(REG_CR + 2)		/*  Media Status register */
+/* define ISR						REG_HISR */
+
+#define TSFR						REG_TSFTR			/*  Timing Sync Function Timer Register. */
+#define TSFR1					REG_TSFTR1			/*  HW Port 1 TSF Register */
+
+#define PBP						REG_PBP
+
+/*  Redifine MACID register, to compatible prior ICs. */
+#define IDR0						REG_MACID			/*  MAC ID Register, Offset 0x0050-0x0053 */
+#define IDR4						(REG_MACID + 4)		/*  MAC ID Register, Offset 0x0054-0x0055 */
+
+
+/*  */
+/*  9. Security Control Registers	(Offset:) */
+/*  */
+#define RWCAM					REG_CAMCMD		/* IN 8190 Data Sheet is called CAMcmd */
+#define WCAMI					REG_CAMWRITE	/*  Software write CAM input content */
+#define RCAMO					REG_CAMREAD		/*  Software read/write CAM config */
+#define CAMDBG					REG_CAMDBG
+#define SECR						REG_SECCFG		/* Security Configuration Register */
+
+/*  Unused register */
+#define UnusedRegister			0x1BF
+#define DCAM					UnusedRegister
+#define PSR						UnusedRegister
+#define BBAddr					UnusedRegister
+#define PhyDataR					UnusedRegister
+
+/*  Min Spacing related settings. */
+#define MAX_MSS_DENSITY_2T			0x13
+#define MAX_MSS_DENSITY_1T			0x0A
+
+/*  */
+/*        8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+/*  */
+#define HSISR_GPIO12_0_INT				BIT0
+#define HSISR_SPS_OCP_INT				BIT5
+#define HSISR_RON_INT					BIT6
+#define HSISR_PDNINT					BIT7
+#define HSISR_GPIO9_INT					BIT25
+
+/*  */
+/*        USB INTR CONTENT */
+/*  */
+#define USB_C2H_CMDID_OFFSET					0
+#define USB_C2H_SEQ_OFFSET					1
+#define USB_C2H_EVENT_OFFSET					2
+#define USB_INTR_CPWM_OFFSET					16
+#define USB_INTR_CONTENT_C2H_OFFSET			0
+#define USB_INTR_CONTENT_CPWM1_OFFSET		16
+#define USB_INTR_CONTENT_CPWM2_OFFSET		20
+#define USB_INTR_CONTENT_HISR_OFFSET			48
+#define USB_INTR_CONTENT_HISRE_OFFSET		52
+#define USB_INTR_CONTENT_LENGTH				56
+
+/*  */
+/*        Response Rate Set Register	(offset 0x440, 24bits) */
+/*  */
+#define RRSR_1M					BIT0
+#define RRSR_2M					BIT1
+#define RRSR_5_5M				BIT2
+#define RRSR_11M				BIT3
+#define RRSR_6M					BIT4
+#define RRSR_9M					BIT5
+#define RRSR_12M				BIT6
+#define RRSR_18M				BIT7
+#define RRSR_24M				BIT8
+#define RRSR_36M				BIT9
+#define RRSR_48M				BIT10
+#define RRSR_54M				BIT11
+#define RRSR_MCS0				BIT12
+#define RRSR_MCS1				BIT13
+#define RRSR_MCS2				BIT14
+#define RRSR_MCS3				BIT15
+#define RRSR_MCS4				BIT16
+#define RRSR_MCS5				BIT17
+#define RRSR_MCS6				BIT18
+#define RRSR_MCS7				BIT19
+
+#define RRSR_CCK_RATES (RRSR_11M|RRSR_5_5M|RRSR_2M|RRSR_1M)
+#define RRSR_OFDM_RATES (RRSR_54M|RRSR_48M|RRSR_36M|RRSR_24M|RRSR_18M|RRSR_12M|RRSR_9M|RRSR_6M)
+
+/*  WOL bit information */
+#define HAL92C_WOL_PTK_UPDATE_EVENT		BIT0
+#define HAL92C_WOL_GTK_UPDATE_EVENT		BIT1
+#define HAL92C_WOL_DISASSOC_EVENT		BIT2
+#define HAL92C_WOL_DEAUTH_EVENT			BIT3
+#define HAL92C_WOL_FW_DISCONNECT_EVENT	BIT4
+
+/*  */
+/*        Rate Definition */
+/*  */
+/* CCK */
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M					0x00000004
+#define	RATR_11M					0x00000008
+/* OFDM */
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M					0x00000040
+#define	RATR_18M					0x00000080
+#define	RATR_24M					0x00000100
+#define	RATR_36M					0x00000200
+#define	RATR_48M					0x00000400
+#define	RATR_54M					0x00000800
+/* MCS 1 Spatial Stream */
+#define	RATR_MCS0					0x00001000
+#define	RATR_MCS1					0x00002000
+#define	RATR_MCS2					0x00004000
+#define	RATR_MCS3					0x00008000
+#define	RATR_MCS4					0x00010000
+#define	RATR_MCS5					0x00020000
+#define	RATR_MCS6					0x00040000
+#define	RATR_MCS7					0x00080000
+
+/* CCK */
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+/* OFDM */
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+/* MCS 1 Spatial Stream */
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+
+/*  ALL CCK Rate */
+#define RATE_BITMAP_ALL			0xFFFFF
+
+/*  Only use CCK 1M rate for ACK */
+#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
+#define RATE_RRSR_WITHOUT_CCK		0xFFFF0
+
+/*  */
+/*        BW_OPMODE bits				(Offset 0x603, 8bit) */
+/*  */
+#define BW_OPMODE_20MHZ			BIT2
+
+/*  */
+/*        CAM Config Setting (offset 0x680, 1 byte) */
+/*  */
+#define CAM_VALID				BIT15
+#define CAM_NOTVALID			0x0000
+#define CAM_USEDK				BIT5
+
+#define CAM_CONTENT_COUNT	8
+
+#define CAM_NONE				0x0
+#define CAM_WEP40				0x01
+#define CAM_TKIP				0x02
+#define CAM_AES					0x04
+#define CAM_WEP104				0x05
+#define CAM_SMS4				0x6
+
+#define TOTAL_CAM_ENTRY		32
+#define HALF_CAM_ENTRY			16
+
+#define CAM_CONFIG_USEDK		true
+#define CAM_CONFIG_NO_USEDK	false
+
+#define CAM_WRITE				BIT16
+#define CAM_READ				0x00000000
+#define CAM_POLLINIG			BIT31
+
+/*  */
+/*  10. Power Save Control Registers */
+/*  */
+#define WOW_PMEN				BIT0 /*  Power management Enable. */
+#define WOW_WOMEN				BIT1 /*  WoW function on or off. */
+#define WOW_MAGIC				BIT2 /*  Magic packet */
+#define WOW_UWF				BIT3 /*  Unicast Wakeup frame. */
+
+/*  */
+/*  12. Host Interrupt Status Registers */
+/*  */
+/*  */
+/*       8190 IMR/ISR bits */
+/*  */
+#define IMR8190_DISABLED		0x0
+#define IMR_DISABLED			0x0
+/*  IMR DW0 Bit 0-31 */
+#define IMR_BCNDMAINT6			BIT31		/*  Beacon DMA Interrupt 6 */
+#define IMR_BCNDMAINT5			BIT30		/*  Beacon DMA Interrupt 5 */
+#define IMR_BCNDMAINT4			BIT29		/*  Beacon DMA Interrupt 4 */
+#define IMR_BCNDMAINT3			BIT28		/*  Beacon DMA Interrupt 3 */
+#define IMR_BCNDMAINT2			BIT27		/*  Beacon DMA Interrupt 2 */
+#define IMR_BCNDMAINT1			BIT26		/*  Beacon DMA Interrupt 1 */
+#define IMR_BCNDOK8				BIT25		/*  Beacon Queue DMA OK Interrupt 8 */
+#define IMR_BCNDOK7				BIT24		/*  Beacon Queue DMA OK Interrupt 7 */
+#define IMR_BCNDOK6				BIT23		/*  Beacon Queue DMA OK Interrupt 6 */
+#define IMR_BCNDOK5				BIT22		/*  Beacon Queue DMA OK Interrupt 5 */
+#define IMR_BCNDOK4				BIT21		/*  Beacon Queue DMA OK Interrupt 4 */
+#define IMR_BCNDOK3				BIT20		/*  Beacon Queue DMA OK Interrupt 3 */
+#define IMR_BCNDOK2				BIT19		/*  Beacon Queue DMA OK Interrupt 2 */
+#define IMR_BCNDOK1				BIT18		/*  Beacon Queue DMA OK Interrupt 1 */
+#define IMR_TIMEOUT2			BIT17		/*  Timeout interrupt 2 */
+#define IMR_TIMEOUT1			BIT16		/*  Timeout interrupt 1 */
+#define IMR_TXFOVW				BIT15		/*  Transmit FIFO Overflow */
+#define IMR_PSTIMEOUT			BIT14		/*  Power save time out interrupt */
+#define IMR_BcnInt				BIT13		/*  Beacon DMA Interrupt 0 */
+#define IMR_RXFOVW				BIT12		/*  Receive FIFO Overflow */
+#define IMR_RDU					BIT11		/*  Receive Descriptor Unavailable */
+#define IMR_ATIMEND				BIT10		/*  For 92C, ATIM Window End Interrupt. For 8723 and later ICs, it also means P2P CTWin End interrupt. */
+#define IMR_BDOK				BIT9		/*  Beacon Queue DMA OK Interrupt */
+#define IMR_HIGHDOK				BIT8		/*  High Queue DMA OK Interrupt */
+#define IMR_TBDOK				BIT7		/*  Transmit Beacon OK interrupt */
+#define IMR_MGNTDOK			BIT6		/*  Management Queue DMA OK Interrupt */
+#define IMR_TBDER				BIT5		/*  For 92C, Transmit Beacon Error Interrupt */
+#define IMR_BKDOK				BIT4		/*  AC_BK DMA OK Interrupt */
+#define IMR_BEDOK				BIT3		/*  AC_BE DMA OK Interrupt */
+#define IMR_VIDOK				BIT2		/*  AC_VI DMA OK Interrupt */
+#define IMR_VODOK				BIT1		/*  AC_VO DMA Interrupt */
+#define IMR_ROK					BIT0		/*  Receive DMA OK Interrupt */
+
+/*  13. Host Interrupt Status Extension Register	 (Offset: 0x012C-012Eh) */
+#define IMR_TSF_BIT32_TOGGLE	BIT15
+#define IMR_BcnInt_E				BIT12
+#define IMR_TXERR				BIT11
+#define IMR_RXERR				BIT10
+#define IMR_C2HCMD				BIT9
+#define IMR_CPWM				BIT8
+/* RSVD [2-7] */
+#define IMR_OCPINT				BIT1
+#define IMR_WLANOFF			BIT0
+
+/*  */
+/*        8192C (RCR) Receive Configuration Register	(Offset 0x608, 32 bits) */
+/*  */
+#define RCR_APPFCS				BIT31	/*  WMAC append FCS after pauload */
+#define RCR_APP_MIC				BIT30	/*  MACRX will retain the MIC at the bottom of the packet. */
+#define RCR_APP_ICV				BIT29	/*  MACRX will retain the ICV at the bottom of the packet. */
+#define RCR_APP_PHYST_RXFF		BIT28	/*  PHY Status is appended before RX packet in RXFF */
+#define RCR_APP_BA_SSN			BIT27	/*  SSN of previous TXBA is appended as after original RXDESC as the 4-th DW of RXDESC. */
+#define RCR_NONQOS_VHT			BIT26	/*  Reserved */
+#define RCR_RSVD_BIT25			BIT25	/*  Reserved */
+#define RCR_ENMBID				BIT24	/*  Enable Multiple BssId. Only response ACK to the packets whose DID(A1) matching to the addresses in the MBSSID CAM Entries. */
+#define RCR_LSIGEN				BIT23	/*  Enable LSIG TXOP Protection function. Search KEYCAM for each rx packet to check if LSIGEN bit is set. */
+#define RCR_MFBEN				BIT22	/*  Enable immediate MCS Feedback function. When Rx packet with MRQ = 1'b1, then search KEYCAM to find sender's MCS Feedback function and send response. */
+#define RCR_RSVD_BIT21			BIT21	/*  Reserved */
+#define RCR_RSVD_BIT20			BIT20	/*  Reserved */
+#define RCR_RSVD_BIT19			BIT19	/*  Reserved */
+#define RCR_TIM_PARSER_EN		BIT18	/*  RX Beacon TIM Parser. */
+#define RCR_BM_DATA_EN			BIT17	/*  Broadcast data packet interrupt enable. */
+#define RCR_UC_DATA_EN			BIT16	/*  Unicast data packet interrupt enable. */
+#define RCR_RSVD_BIT15			BIT15	/*  Reserved */
+#define RCR_HTC_LOC_CTRL		BIT14	/*  MFC<--HTC = 1 MFC-->HTC = 0 */
+#define RCR_AMF					BIT13	/*  Accept management type frame */
+#define RCR_ACF					BIT12	/*  Accept control type frame. Control frames BA, BAR, and PS-Poll (when in AP mode) are not controlled by this bit. They are controlled by ADF. */
+#define RCR_ADF					BIT11	/*  Accept data type frame. This bit also regulates BA, BAR, and PS-Poll (AP mode only). */
+#define RCR_RSVD_BIT10			BIT10	/*  Reserved */
+#define RCR_AICV					BIT9		/*  Accept ICV error packet */
+#define RCR_ACRC32				BIT8		/*  Accept CRC32 error packet */
+#define RCR_CBSSID_BCN			BIT7		/*  Accept BSSID match packet (Rx beacon, probe rsp) */
+#define RCR_CBSSID_DATA		BIT6		/*  Accept BSSID match packet (Data) */
+#define RCR_CBSSID				RCR_CBSSID_DATA	/*  Accept BSSID match packet */
+#define RCR_APWRMGT			BIT5		/*  Accept power management packet */
+#define RCR_ADD3				BIT4		/*  Accept address 3 match packet */
+#define RCR_AB					BIT3		/*  Accept broadcast packet */
+#define RCR_AM					BIT2		/*  Accept multicast packet */
+#define RCR_APM					BIT1		/*  Accept physical match packet */
+#define RCR_AAP					BIT0		/*  Accept all unicast packet */
+
+
+/*  */
+/*  */
+/* 	0x0000h ~ 0x00FFh	System Configuration */
+/*  */
+/*  */
+
+/* 2 SYS_ISO_CTRL */
+#define ISO_MD2PP				BIT(0)
+#define ISO_UA2USB				BIT(1)
+#define ISO_UD2CORE				BIT(2)
+#define ISO_PA2PCIE				BIT(3)
+#define ISO_PD2CORE				BIT(4)
+#define ISO_IP2MAC				BIT(5)
+#define ISO_DIOP					BIT(6)
+#define ISO_DIOE					BIT(7)
+#define ISO_EB2CORE				BIT(8)
+#define ISO_DIOR					BIT(9)
+#define PWC_EV12V				BIT(15)
+
+
+/* 2 SYS_FUNC_EN */
+#define FEN_BBRSTB				BIT(0)
+#define FEN_BB_GLB_RSTn		BIT(1)
+#define FEN_USBA				BIT(2)
+#define FEN_UPLL				BIT(3)
+#define FEN_USBD				BIT(4)
+#define FEN_DIO_PCIE			BIT(5)
+#define FEN_PCIEA				BIT(6)
+#define FEN_PPLL					BIT(7)
+#define FEN_PCIED				BIT(8)
+#define FEN_DIOE				BIT(9)
+#define FEN_CPUEN				BIT(10)
+#define FEN_DCORE				BIT(11)
+#define FEN_ELDR				BIT(12)
+#define FEN_EN_25_1				BIT(13)
+#define FEN_HWPDN				BIT(14)
+#define FEN_MREGEN				BIT(15)
+
+/* 2 APS_FSMCO */
+#define PFM_LDALL				BIT(0)
+#define PFM_ALDN				BIT(1)
+#define PFM_LDKP				BIT(2)
+#define PFM_WOWL				BIT(3)
+#define EnPDN					BIT(4)
+#define PDN_PL					BIT(5)
+#define APFM_ONMAC				BIT(8)
+#define APFM_OFF				BIT(9)
+#define APFM_RSM				BIT(10)
+#define AFSM_HSUS				BIT(11)
+#define AFSM_PCIE				BIT(12)
+#define APDM_MAC				BIT(13)
+#define APDM_HOST				BIT(14)
+#define APDM_HPDN				BIT(15)
+#define RDY_MACON				BIT(16)
+#define SUS_HOST				BIT(17)
+#define ROP_ALD					BIT(20)
+#define ROP_PWR					BIT(21)
+#define ROP_SPS					BIT(22)
+#define SOP_MRST				BIT(25)
+#define SOP_FUSE				BIT(26)
+#define SOP_ABG					BIT(27)
+#define SOP_AMB					BIT(28)
+#define SOP_RCK					BIT(29)
+#define SOP_A8M					BIT(30)
+#define XOP_BTCK				BIT(31)
+
+/* 2 SYS_CLKR */
+#define ANAD16V_EN				BIT(0)
+#define ANA8M					BIT(1)
+#define MACSLP					BIT(4)
+#define LOADER_CLK_EN			BIT(5)
+
+
+/* 2 9346CR /REG_SYS_EEPROM_CTRL */
+#define BOOT_FROM_EEPROM		BIT(4)
+#define EEPROMSEL				BIT(4)
+#define EEPROM_EN				BIT(5)
+
+
+/* 2 RF_CTRL */
+#define RF_EN					BIT(0)
+#define RF_RSTB					BIT(1)
+#define RF_SDMRSTB				BIT(2)
+
+
+/* 2 LDOV12D_CTRL */
+#define LDV12_EN				BIT(0)
+#define LDV12_SDBY				BIT(1)
+#define LPLDO_HSM				BIT(2)
+#define LPLDO_LSM_DIS			BIT(3)
+#define _LDV12_VADJ(x)			(((x) & 0xF) << 4)
+
+
+
+/* 2 EFUSE_TEST (For RTL8723 partially) */
+#define EF_TRPT					BIT(7)
+#define EF_CELL_SEL				(BIT(8)|BIT(9)) /*  00: Wifi Efuse, 01: BT Efuse0, 10: BT Efuse1, 11: BT Efuse2 */
+#define LDOE25_EN				BIT(31)
+#define EFUSE_SEL(x)				(((x) & 0x3) << 8)
+#define EFUSE_SEL_MASK			0x300
+#define EFUSE_WIFI_SEL_0		0x0
+#define EFUSE_BT_SEL_0			0x1
+#define EFUSE_BT_SEL_1			0x2
+#define EFUSE_BT_SEL_2			0x3
+
+
+/* 2 8051FWDL */
+/* 2 MCUFWDL */
+#define MCUFWDL_EN				BIT(0)
+#define MCUFWDL_RDY			BIT(1)
+#define FWDL_ChkSum_rpt		BIT(2)
+#define MACINI_RDY				BIT(3)
+#define BBINI_RDY				BIT(4)
+#define RFINI_RDY				BIT(5)
+#define WINTINI_RDY				BIT(6)
+#define RAM_DL_SEL				BIT(7)
+#define ROM_DLEN				BIT(19)
+#define CPRST					BIT(23)
+
+
+/* 2 REG_SYS_CFG */
+#define XCLK_VLD				BIT(0)
+#define ACLK_VLD				BIT(1)
+#define UCLK_VLD				BIT(2)
+#define PCLK_VLD				BIT(3)
+#define PCIRSTB					BIT(4)
+#define V15_VLD					BIT(5)
+#define SW_OFFLOAD_EN			BIT(7)
+#define SIC_IDLE					BIT(8)
+#define BD_MAC2					BIT(9)
+#define BD_MAC1					BIT(10)
+#define IC_MACPHY_MODE		BIT(11)
+#define CHIP_VER				(BIT(12)|BIT(13)|BIT(14)|BIT(15))
+#define BT_FUNC					BIT(16)
+#define VENDOR_ID				BIT(19)
+#define EXT_VENDOR_ID			(BIT(18)|BIT(19)) /* Currently only for RTL8723B */
+#define PAD_HWPD_IDN			BIT(22)
+#define TRP_VAUX_EN				BIT(23)	/*  RTL ID */
+#define TRP_BT_EN				BIT(24)
+#define BD_PKG_SEL				BIT(25)
+#define BD_HCI_SEL				BIT(26)
+#define TYPE_ID					BIT(27)
+#define RF_TYPE_ID				BIT(27)
+
+#define RTL_ID					BIT(23) /*  TestChip ID, 1:Test(RLE); 0:MP(RL) */
+#define SPS_SEL					BIT(24) /*  1:LDO regulator mode; 0:Switching regulator mode */
+
+
+#define CHIP_VER_RTL_MASK		0xF000	/* Bit 12 ~ 15 */
+#define CHIP_VER_RTL_SHIFT		12
+#define EXT_VENDOR_ID_SHIFT	18
+
+/* 2 REG_GPIO_OUTSTS (For RTL8723 only) */
+#define EFS_HCI_SEL				(BIT(0)|BIT(1))
+#define PAD_HCI_SEL				(BIT(2)|BIT(3))
+#define HCI_SEL					(BIT(4)|BIT(5))
+#define PKG_SEL_HCI				BIT(6)
+#define FEN_GPS					BIT(7)
+#define FEN_BT					BIT(8)
+#define FEN_WL					BIT(9)
+#define FEN_PCI					BIT(10)
+#define FEN_USB					BIT(11)
+#define BTRF_HWPDN_N			BIT(12)
+#define WLRF_HWPDN_N			BIT(13)
+#define PDN_BT_N				BIT(14)
+#define PDN_GPS_N				BIT(15)
+#define BT_CTL_HWPDN			BIT(16)
+#define GPS_CTL_HWPDN			BIT(17)
+#define PPHY_SUSB				BIT(20)
+#define UPHY_SUSB				BIT(21)
+#define PCI_SUSEN				BIT(22)
+#define USB_SUSEN				BIT(23)
+#define RF_RL_ID					(BIT(31)|BIT(30)|BIT(29)|BIT(28))
+
+
+/*  */
+/*  */
+/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+
+/* 2 Function Enable Registers */
+/* 2 CR */
+#define HCI_TXDMA_EN			BIT(0)
+#define HCI_RXDMA_EN			BIT(1)
+#define TXDMA_EN				BIT(2)
+#define RXDMA_EN				BIT(3)
+#define PROTOCOL_EN				BIT(4)
+#define SCHEDULE_EN				BIT(5)
+#define MACTXEN					BIT(6)
+#define MACRXEN					BIT(7)
+#define ENSWBCN					BIT(8)
+#define ENSEC					BIT(9)
+#define CALTMR_EN				BIT(10)	/*  32k CAL TMR enable */
+
+/*  Network type */
+#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define MASK_NETTYPE			0x30000
+#define NT_NO_LINK				0x0
+#define NT_LINK_AD_HOC			0x1
+#define NT_LINK_AP				0x2
+#define NT_AS_AP				0x3
+
+/* 2 PBP - Page Size Register */
+#define GET_RX_PAGE_SIZE(value)			((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)			(((value) & 0xF0) >> 4)
+#define _PSRX_MASK				0xF
+#define _PSTX_MASK				0xF0
+#define _PSRX(x)				(x)
+#define _PSTX(x)				((x) << 4)
+
+#define PBP_64					0x0
+#define PBP_128					0x1
+#define PBP_256					0x2
+#define PBP_512					0x3
+#define PBP_1024				0x4
+
+
+/* 2 TX/RXDMA */
+#define RXDMA_ARBBW_EN		BIT(0)
+#define RXSHFT_EN				BIT(1)
+#define RXDMA_AGG_EN			BIT(2)
+#define QS_VO_QUEUE			BIT(8)
+#define QS_VI_QUEUE				BIT(9)
+#define QS_BE_QUEUE			BIT(10)
+#define QS_BK_QUEUE			BIT(11)
+#define QS_MANAGER_QUEUE		BIT(12)
+#define QS_HIGH_QUEUE			BIT(13)
+
+#define HQSEL_VOQ				BIT(0)
+#define HQSEL_VIQ				BIT(1)
+#define HQSEL_BEQ				BIT(2)
+#define HQSEL_BKQ				BIT(3)
+#define HQSEL_MGTQ				BIT(4)
+#define HQSEL_HIQ				BIT(5)
+
+/*  For normal driver, 0x10C */
+#define _TXDMA_CMQ_MAP(x)			(((x)&0x3) << 16)
+#define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x)			(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x)			(((x)&0x3) << 8)
+#define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
+#define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
+
+#define QUEUE_EXTRA				0
+#define QUEUE_LOW				1
+#define QUEUE_NORMAL			2
+#define QUEUE_HIGH				3
+
+
+/* 2 TRXFF_BNDY */
+
+
+/* 2 LLT_INIT */
+#define _LLT_NO_ACTIVE				0x0
+#define _LLT_WRITE_ACCESS			0x1
+#define _LLT_READ_ACCESS			0x2
+
+#define _LLT_INIT_DATA(x)			((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
+#define _LLT_OP(x)					(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
+
+
+/*  */
+/*  */
+/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+/* 2 RQPN */
+#define _HPQ(x)					((x) & 0xFF)
+#define _LPQ(x)					(((x) & 0xFF) << 8)
+#define _PUBQ(x)					(((x) & 0xFF) << 16)
+#define _NPQ(x)					((x) & 0xFF)			/*  NOTE: in RQPN_NPQ register */
+#define _EPQ(x)					(((x) & 0xFF) << 16)	/*  NOTE: in RQPN_EPQ register */
+
+
+#define HPQ_PUBLIC_DIS			BIT(24)
+#define LPQ_PUBLIC_DIS			BIT(25)
+#define LD_RQPN					BIT(31)
+
+
+/* 2 TDECTL */
+#define BLK_DESC_NUM_SHIFT			4
+#define BLK_DESC_NUM_MASK			0xF
+
+
+/* 2 TXDMA_OFFSET_CHK */
+#define DROP_DATA_EN				BIT(9)
+
+/* 2 AUTO_LLT */
+#define BIT_SHIFT_TXPKTNUM 24
+#define BIT_MASK_TXPKTNUM 0xff
+#define BIT_TXPKTNUM(x) (((x) & BIT_MASK_TXPKTNUM) << BIT_SHIFT_TXPKTNUM)
+
+#define BIT_TDE_DBG_SEL BIT(23)
+#define BIT_AUTO_INIT_LLT BIT(16)
+
+#define BIT_SHIFT_Tx_OQT_free_space 8
+#define BIT_MASK_Tx_OQT_free_space 0xff
+#define BIT_Tx_OQT_free_space(x) (((x) & BIT_MASK_Tx_OQT_free_space) << BIT_SHIFT_Tx_OQT_free_space)
+
+
+/*  */
+/*  */
+/* 	0x0280h ~ 0x028Bh	RX DMA Configuration */
+/*  */
+/*  */
+
+/* 2 REG_RXDMA_CONTROL, 0x0286h */
+/*  Write only. When this bit is set, RXDMA will decrease RX PKT counter by one. Before */
+/*  this bit is polled, FW shall update RXFF_RD_PTR first. This register is write pulse and auto clear. */
+/* define RXPKT_RELEASE_POLL			BIT(0) */
+/*  Read only. When RXMA finishes on-going DMA operation, RXMDA will report idle state in */
+/*  this bit. FW can start releasing packets after RXDMA entering idle mode. */
+/* define RXDMA_IDLE					BIT(1) */
+/*  When this bit is set, RXDMA will enter this mode after on-going RXDMA packet to host */
+/*  completed, and stop DMA packet to host. RXDMA will then report Default: 0; */
+/* define RW_RELEASE_EN				BIT(2) */
+
+/* 2 REG_RXPKT_NUM, 0x0284 */
+#define		RXPKT_RELEASE_POLL	BIT(16)
+#define	RXDMA_IDLE				BIT(17)
+#define	RW_RELEASE_EN			BIT(18)
+
+/*  */
+/*  */
+/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+/* 2 FWHW_TXQ_CTRL */
+#define EN_AMPDU_RTY_NEW			BIT(7)
+
+
+/* 2 SPEC SIFS */
+#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
+
+/* 2 RL */
+#define	RETRY_LIMIT_SHORT_SHIFT			8
+#define	RETRY_LIMIT_LONG_SHIFT			0
+
+/*  */
+/*  */
+/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+
+/* 2 EDCA setting */
+#define AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define AC_PARAM_ECW_MAX_OFFSET			12
+#define AC_PARAM_ECW_MIN_OFFSET			8
+#define AC_PARAM_AIFS_OFFSET				0
+
+
+#define _LRL(x)					((x) & 0x3F)
+#define _SRL(x)					(((x) & 0x3F) << 8)
+
+
+/* 2 BCN_CTRL */
+#define EN_TXBCN_RPT			BIT(2)
+#define EN_BCN_FUNCTION		BIT(3)
+#define STOP_BCNQ				BIT(6)
+#define DIS_RX_BSSID_FIT		BIT(6)
+
+#define DIS_ATIM					BIT(0)
+#define DIS_BCNQ_SUB			BIT(1)
+#define DIS_TSF_UDT				BIT(4)
+
+/*  The same function but different bit field. */
+#define DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define DIS_TSF_UDT0_TEST_CHIP	BIT(5)
+
+
+/* 2 ACMHWCTRL */
+#define AcmHw_HwEn				BIT(0)
+#define AcmHw_BeqEn			BIT(1)
+#define AcmHw_ViqEn				BIT(2)
+#define AcmHw_VoqEn			BIT(3)
+#define AcmHw_BeqStatus		BIT(4)
+#define AcmHw_ViqStatus			BIT(5)
+#define AcmHw_VoqStatus		BIT(6)
+
+/* 2 REG_DUAL_TSF_RST (0x553) */
+#define DUAL_TSF_RST_P2P		BIT(4)
+
+/* 2  REG_NOA_DESC_SEL (0x5CF) */
+#define NOA_DESC_SEL_0			0
+#define NOA_DESC_SEL_1			BIT(4)
+
+/*  */
+/*  */
+/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*  */
+/*  */
+
+/* 2 APSD_CTRL */
+#define APSDOFF					BIT(6)
+
+/* 2 TCR */
+#define TSFRST					BIT(0)
+#define DIS_GCLK					BIT(1)
+#define PAD_SEL					BIT(2)
+#define PWR_ST					BIT(6)
+#define PWRBIT_OW_EN			BIT(7)
+#define ACRC						BIT(8)
+#define CFENDFORM				BIT(9)
+#define ICV						BIT(10)
+
+
+/* 2 RCR */
+#define AAP						BIT(0)
+#define APM						BIT(1)
+#define AM						BIT(2)
+#define AB						BIT(3)
+#define ADD3						BIT(4)
+#define APWRMGT				BIT(5)
+#define CBSSID					BIT(6)
+#define CBSSID_DATA				BIT(6)
+#define CBSSID_BCN				BIT(7)
+#define ACRC32					BIT(8)
+#define AICV						BIT(9)
+#define ADF						BIT(11)
+#define ACF						BIT(12)
+#define AMF						BIT(13)
+#define HTC_LOC_CTRL			BIT(14)
+#define UC_DATA_EN				BIT(16)
+#define BM_DATA_EN				BIT(17)
+#define MFBEN					BIT(22)
+#define LSIGEN					BIT(23)
+#define EnMBID					BIT(24)
+#define FORCEACK				BIT(26)
+#define APP_BASSN				BIT(27)
+#define APP_PHYSTS				BIT(28)
+#define APP_ICV					BIT(29)
+#define APP_MIC					BIT(30)
+#define APP_FCS					BIT(31)
+
+
+/* 2 SECCFG */
+#define SCR_TxUseDK				BIT(0)			/* Force Tx Use Default Key */
+#define SCR_RxUseDK				BIT(1)			/* Force Rx Use Default Key */
+#define SCR_TxEncEnable			BIT(2)			/* Enable Tx Encryption */
+#define SCR_RxDecEnable			BIT(3)			/* Enable Rx Decryption */
+#define SCR_SKByA2				BIT(4)			/* Search kEY BY A2 */
+#define SCR_NoSKMC				BIT(5)			/* No Key Search Multicast */
+#define SCR_TXBCUSEDK			BIT(6)			/*  Force Tx Broadcast packets Use Default Key */
+#define SCR_RXBCUSEDK			BIT(7)			/*  Force Rx Broadcast packets Use Default Key */
+#define SCR_CHK_KEYID			BIT(8)
+
+/*  */
+/*  */
+/* 	SDIO Bus Specification */
+/*  */
+/*  */
+
+/*  I/O bus domain address mapping */
+#define SDIO_LOCAL_BASE		0x10250000
+#define WLAN_IOREG_BASE		0x10260000
+#define FIRMWARE_FIFO_BASE	0x10270000
+#define TX_HIQ_BASE				0x10310000
+#define TX_MIQ_BASE				0x10320000
+#define TX_LOQ_BASE				0x10330000
+#define TX_EPQ_BASE				0x10350000
+#define RX_RX0FF_BASE			0x10340000
+
+/* SDIO host local register space mapping. */
+#define SDIO_LOCAL_MSK				0x0FFF
+#define WLAN_IOREG_MSK			0x7FFF
+#define WLAN_FIFO_MSK				0x1FFF	/*  Aggregation Length[12:0] */
+#define WLAN_RX0FF_MSK				0x0003
+
+#define SDIO_WITHOUT_REF_DEVICE_ID	0	/*  Without reference to the SDIO Device ID */
+#define SDIO_LOCAL_DEVICE_ID			0	/*  0b[16], 000b[15:13] */
+#define WLAN_TX_HIQ_DEVICE_ID			4	/*  0b[16], 100b[15:13] */
+#define WLAN_TX_MIQ_DEVICE_ID		5	/*  0b[16], 101b[15:13] */
+#define WLAN_TX_LOQ_DEVICE_ID		6	/*  0b[16], 110b[15:13] */
+#define WLAN_TX_EXQ_DEVICE_ID		3	/*  0b[16], 011b[15:13] */
+#define WLAN_RX0FF_DEVICE_ID			7	/*  0b[16], 111b[15:13] */
+#define WLAN_IOREG_DEVICE_ID			8	/*  1b[16] */
+
+/* SDIO Tx Free Page Index */
+#define HI_QUEUE_IDX				0
+#define MID_QUEUE_IDX				1
+#define LOW_QUEUE_IDX				2
+#define PUBLIC_QUEUE_IDX			3
+
+#define SDIO_MAX_TX_QUEUE			3		/*  HIQ, MIQ and LOQ */
+#define SDIO_MAX_RX_QUEUE			1
+
+#define SDIO_REG_TX_CTRL			0x0000 /*  SDIO Tx Control */
+#define SDIO_REG_HIMR				0x0014 /*  SDIO Host Interrupt Mask */
+#define SDIO_REG_HISR				0x0018 /*  SDIO Host Interrupt Service Routine */
+#define SDIO_REG_HCPWM			0x0019 /*  HCI Current Power Mode */
+#define SDIO_REG_RX0_REQ_LEN		0x001C /*  RXDMA Request Length */
+#define SDIO_REG_OQT_FREE_PG		0x001E /*  OQT Free Page */
+#define SDIO_REG_FREE_TXPG			0x0020 /*  Free Tx Buffer Page */
+#define SDIO_REG_HCPWM1			0x0024 /*  HCI Current Power Mode 1 */
+#define SDIO_REG_HCPWM2			0x0026 /*  HCI Current Power Mode 2 */
+#define SDIO_REG_FREE_TXPG_SEQ	0x0028 /*  Free Tx Page Sequence */
+#define SDIO_REG_HTSFR_INFO		0x0030 /*  HTSF Informaion */
+#define SDIO_REG_HRPWM1			0x0080 /*  HCI Request Power Mode 1 */
+#define SDIO_REG_HRPWM2			0x0082 /*  HCI Request Power Mode 2 */
+#define SDIO_REG_HPS_CLKR			0x0084 /*  HCI Power Save Clock */
+#define SDIO_REG_HSUS_CTRL			0x0086 /*  SDIO HCI Suspend Control */
+#define SDIO_REG_HIMR_ON			0x0090 /* SDIO Host Extension Interrupt Mask Always */
+#define SDIO_REG_HISR_ON			0x0091 /* SDIO Host Extension Interrupt Status Always */
+
+#define SDIO_HIMR_DISABLED			0
+
+/*  RTL8723/RTL8188E SDIO Host Interrupt Mask Register */
+#define SDIO_HIMR_RX_REQUEST_MSK		BIT0
+#define SDIO_HIMR_AVAL_MSK			BIT1
+#define SDIO_HIMR_TXERR_MSK			BIT2
+#define SDIO_HIMR_RXERR_MSK			BIT3
+#define SDIO_HIMR_TXFOVW_MSK			BIT4
+#define SDIO_HIMR_RXFOVW_MSK			BIT5
+#define SDIO_HIMR_TXBCNOK_MSK			BIT6
+#define SDIO_HIMR_TXBCNERR_MSK		BIT7
+#define SDIO_HIMR_BCNERLY_INT_MSK		BIT16
+#define SDIO_HIMR_C2HCMD_MSK			BIT17
+#define SDIO_HIMR_CPWM1_MSK			BIT18
+#define SDIO_HIMR_CPWM2_MSK			BIT19
+#define SDIO_HIMR_HSISR_IND_MSK		BIT20
+#define SDIO_HIMR_GTINT3_IND_MSK		BIT21
+#define SDIO_HIMR_GTINT4_IND_MSK		BIT22
+#define SDIO_HIMR_PSTIMEOUT_MSK		BIT23
+#define SDIO_HIMR_OCPINT_MSK			BIT24
+#define SDIO_HIMR_ATIMEND_MSK			BIT25
+#define SDIO_HIMR_ATIMEND_E_MSK		BIT26
+#define SDIO_HIMR_CTWEND_MSK			BIT27
+
+/*  SDIO Host Interrupt Service Routine */
+#define SDIO_HISR_RX_REQUEST			BIT0
+#define SDIO_HISR_AVAL					BIT1
+#define SDIO_HISR_TXERR					BIT2
+#define SDIO_HISR_RXERR					BIT3
+#define SDIO_HISR_TXFOVW				BIT4
+#define SDIO_HISR_RXFOVW				BIT5
+#define SDIO_HISR_TXBCNOK				BIT6
+#define SDIO_HISR_TXBCNERR				BIT7
+#define SDIO_HISR_BCNERLY_INT			BIT16
+#define SDIO_HISR_C2HCMD				BIT17
+#define SDIO_HISR_CPWM1				BIT18
+#define SDIO_HISR_CPWM2				BIT19
+#define SDIO_HISR_HSISR_IND			BIT20
+#define SDIO_HISR_GTINT3_IND			BIT21
+#define SDIO_HISR_GTINT4_IND			BIT22
+#define SDIO_HISR_PSTIMEOUT			BIT23
+#define SDIO_HISR_OCPINT				BIT24
+#define SDIO_HISR_ATIMEND				BIT25
+#define SDIO_HISR_ATIMEND_E			BIT26
+#define SDIO_HISR_CTWEND				BIT27
+
+#define MASK_SDIO_HISR_CLEAR		(SDIO_HISR_TXERR |\
+									SDIO_HISR_RXERR |\
+									SDIO_HISR_TXFOVW |\
+									SDIO_HISR_RXFOVW |\
+									SDIO_HISR_TXBCNOK |\
+									SDIO_HISR_TXBCNERR |\
+									SDIO_HISR_C2HCMD |\
+									SDIO_HISR_CPWM1 |\
+									SDIO_HISR_CPWM2 |\
+									SDIO_HISR_HSISR_IND |\
+									SDIO_HISR_GTINT3_IND |\
+									SDIO_HISR_GTINT4_IND |\
+									SDIO_HISR_PSTIMEOUT |\
+									SDIO_HISR_OCPINT)
+
+/*  SDIO HCI Suspend Control Register */
+#define HCI_RESUME_PWR_RDY			BIT1
+#define HCI_SUS_CTRL					BIT0
+
+/*  SDIO Tx FIFO related */
+#define SDIO_TX_FREE_PG_QUEUE			4	/*  The number of Tx FIFO free page */
+#define SDIO_TX_FIFO_PAGE_SZ			128
+
+#define MAX_TX_AGG_PACKET_NUMBER	0x8
+
+/*  */
+/*  */
+/* 	0xFE00h ~ 0xFE55h	USB Configuration */
+/*  */
+/*  */
+
+/* 2 USB Information (0xFE17) */
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK	0xF
+#define USB_NORMAL_SIE_EP_SHIFT	4
+
+/* 2 Special Option */
+#define USB_AGG_EN				BIT(3)
+
+/*  0; Use interrupt endpoint to upload interrupt pkt */
+/*  1; Use bulk endpoint to upload interrupt pkt, */
+#define INT_BULK_SEL			BIT(4)
+
+/* 2REG_C2HEVT_CLEAR */
+#define C2H_EVT_HOST_CLOSE		0x00	/*  Set by driver and notify FW that the driver has read the C2H command message */
+#define C2H_EVT_FW_CLOSE		0xFF	/*  Set by FW indicating that FW had set the C2H command message and it's not yet read by driver. */
+
+
+/* 2REG_MULTI_FUNC_CTRL(For RTL8723 Only) */
+#define WL_HWPDN_EN			BIT0	/*  Enable GPIO[9] as WiFi HW PDn source */
+#define WL_HWPDN_SL			BIT1	/*  WiFi HW PDn polarity control */
+#define WL_FUNC_EN				BIT2	/*  WiFi function enable */
+#define WL_HWROF_EN			BIT3	/*  Enable GPIO[9] as WiFi RF HW PDn source */
+#define BT_HWPDN_EN			BIT16	/*  Enable GPIO[11] as BT HW PDn source */
+#define BT_HWPDN_SL			BIT17	/*  BT HW PDn polarity control */
+#define BT_FUNC_EN				BIT18	/*  BT function enable */
+#define BT_HWROF_EN			BIT19	/*  Enable GPIO[11] as BT/GPS RF HW PDn source */
+#define GPS_HWPDN_EN			BIT20	/*  Enable GPIO[10] as GPS HW PDn source */
+#define GPS_HWPDN_SL			BIT21	/*  GPS HW PDn polarity control */
+#define GPS_FUNC_EN			BIT22	/*  GPS function enable */
+
+/*  */
+/*  General definitions */
+/*  */
+
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_8723B		255
+
+#define POLLING_LLT_THRESHOLD				20
+#define POLLING_READY_TIMEOUT_COUNT		1000
+
+#endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_data.h b/drivers/staging/rtl8723bs/include/hal_data.h
new file mode 100644
index 000000000..b87c90f69
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_data.h
@@ -0,0 +1,400 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_DATA_H__
+#define __HAL_DATA_H__
+
+#include "odm_precomp.h"
+#include <hal_btcoex.h>
+
+#include <hal_sdio.h>
+
+/*  */
+/*  <Roger_Notes> For RTL8723 WiFi/BT/GPS multi-function configuration. 2010.10.06. */
+/*  */
+enum rt_multi_func {
+	RT_MULTI_FUNC_NONE	= 0x00,
+	RT_MULTI_FUNC_WIFI	= 0x01,
+	RT_MULTI_FUNC_BT		= 0x02,
+	RT_MULTI_FUNC_GPS	= 0x04,
+};
+/*  */
+/*  <Roger_Notes> For RTL8723 WiFi PDn/GPIO polarity control configuration. 2010.10.08. */
+/*  */
+enum rt_polarity_ctl {
+	RT_POLARITY_LOW_ACT	= 0,
+	RT_POLARITY_HIGH_ACT	= 1,
+};
+
+/*  For RTL8723 regulator mode. by tynli. 2011.01.14. */
+enum rt_regulator_mode {
+	RT_SWITCHING_REGULATOR	= 0,
+	RT_LDO_REGULATOR	= 1,
+};
+
+enum rt_ampdu_burst {
+	RT_AMPDU_BURST_NONE	= 0,
+	RT_AMPDU_BURST_92D	= 1,
+	RT_AMPDU_BURST_88E	= 2,
+	RT_AMPDU_BURST_8812_4	= 3,
+	RT_AMPDU_BURST_8812_8	= 4,
+	RT_AMPDU_BURST_8812_12	= 5,
+	RT_AMPDU_BURST_8812_15	= 6,
+	RT_AMPDU_BURST_8723B	= 7,
+};
+
+#define CHANNEL_MAX_NUMBER		(14)	/*  14 is the max channel number */
+#define CHANNEL_MAX_NUMBER_2G		14
+#define MAX_PG_GROUP			13
+
+/*  Tx Power Limit Table Size */
+#define MAX_REGULATION_NUM			4
+#define MAX_2_4G_BANDWIDTH_NUM			2
+#define MAX_RATE_SECTION_NUM			3 /* CCK:1, OFDM:1, HT:1 */
+
+/*  duplicate code, will move to ODM ######### */
+/* define IQK_MAC_REG_NUM		4 */
+/* define IQK_ADDA_REG_NUM		16 */
+
+/* define IQK_BB_REG_NUM			10 */
+
+/* define HP_THERMAL_NUM		8 */
+/*  duplicate code, will move to ODM ######### */
+
+enum {
+	SINGLEMAC_SINGLEPHY,	/* SMSP */
+	DUALMAC_DUALPHY,		/* DMDP */
+	DUALMAC_SINGLEPHY,	/* DMSP */
+};
+
+#define PAGE_SIZE_128	128
+#define PAGE_SIZE_256	256
+#define PAGE_SIZE_512	512
+
+struct dm_priv {
+	u8 DM_Type;
+
+#define DYNAMIC_FUNC_BT BIT0
+
+	u8 DMFlag;
+	u8 InitDMFlag;
+	/* u8   RSVD_1; */
+
+	u32 InitODMFlag;
+	/*  Upper and Lower Signal threshold for Rate Adaptive */
+	int	UndecoratedSmoothedPWDB;
+	int	UndecoratedSmoothedCCK;
+	int	EntryMinUndecoratedSmoothedPWDB;
+	int	EntryMaxUndecoratedSmoothedPWDB;
+	int	MinUndecoratedPWDBForDM;
+	int	LastMinUndecoratedPWDBForDM;
+
+	s32	UndecoratedSmoothedBeacon;
+
+/*  duplicate code, will move to ODM ######### */
+	/* for High Power */
+	u8 bDynamicTxPowerEnable;
+	u8 LastDTPLvl;
+	u8 DynamicTxHighPowerLvl;/* Add by Jacken Tx Power Control for Near/Far Range 2008/03/06 */
+
+	/* for tx power tracking */
+	u8 bTXPowerTracking;
+	u8 TXPowercount;
+	u8 bTXPowerTrackingInit;
+	u8 TxPowerTrackControl;	/* for mp mode, turn off txpwrtracking as default */
+	u8 TM_Trigger;
+
+	u8 ThermalMeter[2];				/*  ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
+	u8 ThermalValue;
+	u8 ThermalValue_LCK;
+	u8 ThermalValue_IQK;
+	u8 ThermalValue_DPK;
+	u8 bRfPiEnable;
+	/* u8   RSVD_2; */
+
+	/* for APK */
+	u32 APKoutput[2][2];	/* path A/B; output1_1a/output1_2a */
+	u8 bAPKdone;
+	u8 bAPKThermalMeterIgnore;
+	u8 bDPdone;
+	u8 bDPPathAOK;
+	u8 bDPPathBOK;
+	/* u8   RSVD_3; */
+	/* u8   RSVD_4; */
+	/* u8   RSVD_5; */
+
+	/* for IQK */
+	u32 ADDA_backup[IQK_ADDA_REG_NUM];
+	u32 IQK_MAC_backup[IQK_MAC_REG_NUM];
+	u32 IQK_BB_backup_recover[9];
+	u32 IQK_BB_backup[IQK_BB_REG_NUM];
+
+	u8 PowerIndex_backup[6];
+	u8 OFDM_index[2];
+
+	u8 bCCKinCH14;
+	u8 CCK_index;
+	u8 bDoneTxpower;
+	u8 CCK_index_HP;
+
+	u8 OFDM_index_HP[2];
+	u8 ThermalValue_HP[HP_THERMAL_NUM];
+	u8 ThermalValue_HP_index;
+	/* u8   RSVD_6; */
+
+	/* for TxPwrTracking2 */
+	s32	RegE94;
+	s32  RegE9C;
+	s32	RegEB4;
+	s32	RegEBC;
+
+	u32 TXPowerTrackingCallbackCnt;	/* cosa add for debug */
+
+	u32 prv_traffic_idx; /*  edca turbo */
+/*  duplicate code, will move to ODM ######### */
+
+	/*  Add for Reading Initial Data Rate SEL Register 0x484 during watchdog. Using for fill tx desc. 2011.3.21 by Thomas */
+	u8 INIDATA_RATE[32];
+};
+
+
+struct hal_com_data {
+	struct hal_version VersionID;
+	enum rt_multi_func MultiFunc; /*  For multi-function consideration. */
+	enum rt_polarity_ctl PolarityCtl; /*  For Wifi PDn Polarity control. */
+	enum rt_regulator_mode	RegulatorMode; /*  switching regulator or LDO */
+
+	u16 FirmwareVersion;
+	u16 FirmwareVersionRev;
+	u16 FirmwareSubVersion;
+	u16 FirmwareSignature;
+
+	/* current WIFI_PHY values */
+	enum wireless_mode CurrentWirelessMode;
+	enum channel_width CurrentChannelBW;
+	u8 CurrentChannel;
+	u8 CurrentCenterFrequencyIndex1;
+	u8 nCur40MhzPrimeSC;/*  Control channel sub-carrier */
+	u8 nCur80MhzPrimeSC;   /* used for primary 40MHz of 80MHz mode */
+
+	u16 CustomerID;
+	u16 BasicRateSet;
+	u16 ForcedDataRate;/*  Force Data Rate. 0: Auto, 0x02: 1M ~ 0x6C: 54M. */
+	u32 ReceiveConfig;
+
+	/* rf_ctrl */
+	u8 rf_chip;
+	u8 PackageType;
+	u8 NumTotalRFPath;
+
+	u8 InterfaceSel;
+	u8 framesync;
+	u32 framesyncC34;
+	u8 framesyncMonitor;
+	u8 DefaultInitialGain[4];
+	/*  EEPROM setting. */
+	u16 EEPROMVID;
+	u16 EEPROMSVID;
+
+	u8 EEPROMCustomerID;
+	u8 EEPROMSubCustomerID;
+	u8 EEPROMVersion;
+	u8 EEPROMRegulatory;
+	u8 EEPROMThermalMeter;
+	u8 EEPROMBluetoothCoexist;
+	u8 EEPROMBluetoothType;
+	u8 EEPROMBluetoothAntNum;
+	u8 EEPROMBluetoothAntIsolation;
+	u8 EEPROMBluetoothRadioShared;
+	u8 bTXPowerDataReadFromEEPORM;
+	u8 bAPKThermalMeterIgnore;
+	u8 bDisableSWChannelPlan; /*  flag of disable software change channel plan */
+
+	bool		EepromOrEfuse;
+	u8 		EfuseUsedPercentage;
+	u16 			EfuseUsedBytes;
+	struct efuse_hal		EfuseHal;
+
+	/* 3 [2.4G] */
+	u8 Index24G_CCK_Base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	u8 Index24G_BW40_Base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	/* If only one tx, only BW20 and OFDM are used. */
+	s8	CCK_24G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	OFDM_24G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	BW20_24G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	BW40_24G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+
+	u8 Regulation2_4G;
+
+	u8 TxPwrInPercentage;
+
+	u8 TxPwrCalibrateRate;
+	/*  TX power by rate table */
+	/*  RF: at most 2 = AB = 0/1 */
+	/*  CCK = 0 OFDM = 1 HT-MCS 0-7 = 2 */
+	u8 TxPwrByRateTable;
+	u8 TxPwrByRateBand;
+	s8 TxPwrByRateOffset[MAX_RF_PATH_NUM][TX_PWR_BY_RATE_NUM_RATE];
+	/*  */
+
+	/* 2 Power Limit Table */
+	u8 TxPwrLevelCck[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];
+	u8 TxPwrLevelHT40_1S[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];	/*  For HT 40MHZ pwr */
+	u8 TxPwrLevelHT40_2S[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];	/*  For HT 40MHZ pwr */
+	s8	TxPwrHt20Diff[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];/*  HT 20<->40 Pwr diff */
+	u8 TxPwrLegacyHtDiff[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];/*  For HT<->legacy pwr diff */
+
+	/*  Power Limit Table for 2.4G */
+	s8	TxPwrLimit_2_4G[MAX_REGULATION_NUM]
+						[MAX_2_4G_BANDWIDTH_NUM]
+	                                [MAX_RATE_SECTION_NUM]
+	                                [CHANNEL_MAX_NUMBER_2G]
+						[MAX_RF_PATH_NUM];
+
+	/*  Store the original power by rate value of the base of each rate section of rf path A & B */
+	u8 TxPwrByRateBase2_4G[MAX_RF_PATH_NUM][MAX_RATE_SECTION_NUM];
+
+	/*  For power group */
+	u8 PwrGroupHT20[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];
+	u8 PwrGroupHT40[RF_PATH_MAX_92C_88E][CHANNEL_MAX_NUMBER];
+
+
+
+
+	u8 PGMaxGroup;
+	u8 LegacyHTTxPowerDiff;/*  Legacy to HT rate power diff */
+	/*  The current Tx Power Level */
+	u8 CurrentCckTxPwrIdx;
+	u8 CurrentOfdm24GTxPwrIdx;
+	u8 CurrentBW2024GTxPwrIdx;
+	u8 CurrentBW4024GTxPwrIdx;
+
+	/*  Read/write are allow for following hardware information variables */
+	u8 pwrGroupCnt;
+	u32 MCSTxPowerLevelOriginalOffset[MAX_PG_GROUP][16];
+	u32 CCKTxPowerLevelOriginalOffset;
+
+	u8 CrystalCap;
+	u32 AntennaTxPath;					/*  Antenna path Tx */
+	u32 AntennaRxPath;					/*  Antenna path Rx */
+
+	u8 PAType_2G;
+	u8 LNAType_2G;
+	u8 ExternalPA_2G;
+	u8 ExternalLNA_2G;
+	u8 TypeGLNA;
+	u8 TypeGPA;
+	u8 TypeALNA;
+	u8 TypeAPA;
+	u8 RFEType;
+	u8 BoardType;
+	u8 ExternalPA;
+	u8 bIQKInitialized;
+	bool		bLCKInProgress;
+
+	bool		bSwChnl;
+	bool		bSetChnlBW;
+	bool		bChnlBWInitialized;
+	bool		bNeedIQK;
+
+	u8 bLedOpenDrain; /*  Support Open-drain arrangement for controlling the LED. Added by Roger, 2009.10.16. */
+	u8 TxPowerTrackControl; /* for mp mode, turn off txpwrtracking as default */
+	u8 b1x1RecvCombine;	/*  for 1T1R receive combining */
+
+	u32 AcParam_BE; /* Original parameter for BE, use for EDCA turbo. */
+
+	struct bb_register_def PHYRegDef[4];	/* Radio A/B/C/D */
+
+	u32 RfRegChnlVal[2];
+
+	/* RDG enable */
+	bool	 bRDGEnable;
+
+	/* for host message to fw */
+	u8 LastHMEBoxNum;
+
+	u8 fw_ractrl;
+	u8 RegTxPause;
+	/*  Beacon function related global variable. */
+	u8 RegBcnCtrlVal;
+	u8 RegFwHwTxQCtrl;
+	u8 RegReg542;
+	u8 RegCR_1;
+	u8 Reg837;
+	u8 RegRFPathS1;
+	u16 RegRRSR;
+
+	u8 CurAntenna;
+	u8 AntDivCfg;
+	u8 AntDetection;
+	u8 TRxAntDivType;
+	u8 ant_path; /* for 8723B s0/s1 selection */
+
+	u8 u1ForcedIgiLb;			/*  forced IGI lower bound */
+
+	u8 bDumpRxPkt;/* for debug */
+	u8 bDumpTxPkt;/* for debug */
+	u8 FwRsvdPageStartOffset; /* 2010.06.23. Added by tynli. Reserve page start offset except beacon in TxQ. */
+
+	/*  2010/08/09 MH Add CU power down mode. */
+	bool		pwrdown;
+
+	/*  Add for dual MAC  0--Mac0 1--Mac1 */
+	u32 interfaceIndex;
+
+	u8 OutEpQueueSel;
+	u8 OutEpNumber;
+
+	/*  2010/12/10 MH Add for USB aggregation mode dynamic scheme. */
+	bool		UsbRxHighSpeedMode;
+
+	/*  2010/11/22 MH Add for slim combo debug mode selective. */
+	/*  This is used for fix the drawback of CU TSMC-A/UMC-A cut. HW auto suspend ability. Close BT clock. */
+	bool		SlimComboDbg;
+
+	/* u8 AMPDUDensity; */
+
+	/*  Auto FSM to Turn On, include clock, isolation, power control for MAC only */
+	u8 bMacPwrCtrlOn;
+
+	u8 RegIQKFWOffload;
+	struct submit_ctx	iqk_sctx;
+
+	enum rt_ampdu_burst	AMPDUBurstMode; /* 92C maybe not use, but for compile successfully */
+
+	u32 		sdio_himr;
+	u32 		sdio_hisr;
+
+	/*  SDIO Tx FIFO related. */
+	/*  HIQ, MID, LOW, PUB free pages; padapter->xmitpriv.free_txpg */
+	u8 	SdioTxFIFOFreePage[SDIO_TX_FREE_PG_QUEUE];
+	spinlock_t		SdioTxFIFOFreePageLock;
+	u8 	SdioTxOQTMaxFreeSpace;
+	u8 	SdioTxOQTFreeSpace;
+
+
+	/*  SDIO Rx FIFO related. */
+	u8 	SdioRxFIFOCnt;
+	u16 		SdioRxFIFOSize;
+
+	u32 		sdio_tx_max_len[SDIO_MAX_TX_QUEUE];/*  H, N, L, used for sdio tx aggregation max length per queue */
+
+	struct dm_priv dmpriv;
+	struct dm_odm_t		odmpriv;
+
+	/*  For bluetooth co-existance */
+	struct bt_coexist		bt_coexist;
+
+	/*  Interrupt related register information. */
+	u32 		SysIntrStatus;
+	u32 		SysIntrMask;
+};
+
+#define GET_HAL_DATA(__padapter)	((struct hal_com_data *)((__padapter)->HalData))
+#define GET_HAL_RFPATH_NUM(__padapter) (((struct hal_com_data *)((__padapter)->HalData))->NumTotalRFPath)
+#define RT_GetInterfaceSelection(_Adapter)	(GET_HAL_DATA(_Adapter)->InterfaceSel)
+
+#endif /* __HAL_DATA_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_intf.h b/drivers/staging/rtl8723bs/include/hal_intf.h
new file mode 100644
index 000000000..5cffab2d0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_intf.h
@@ -0,0 +1,361 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_INTF_H__
+#define __HAL_INTF_H__
+
+
+enum {
+	RTW_PCIE	= BIT0,
+	RTW_USB		= BIT1,
+	RTW_SDIO	= BIT2,
+	RTW_GSPI	= BIT3,
+};
+
+enum {
+	HW_VAR_MEDIA_STATUS,
+	HW_VAR_MEDIA_STATUS1,
+	HW_VAR_SET_OPMODE,
+	HW_VAR_MAC_ADDR,
+	HW_VAR_BSSID,
+	HW_VAR_INIT_RTS_RATE,
+	HW_VAR_BASIC_RATE,
+	HW_VAR_TXPAUSE,
+	HW_VAR_BCN_FUNC,
+	HW_VAR_CORRECT_TSF,
+	HW_VAR_CHECK_BSSID,
+	HW_VAR_MLME_DISCONNECT,
+	HW_VAR_MLME_SITESURVEY,
+	HW_VAR_MLME_JOIN,
+	HW_VAR_ON_RCR_AM,
+	HW_VAR_OFF_RCR_AM,
+	HW_VAR_BEACON_INTERVAL,
+	HW_VAR_SLOT_TIME,
+	HW_VAR_RESP_SIFS,
+	HW_VAR_ACK_PREAMBLE,
+	HW_VAR_SEC_CFG,
+	HW_VAR_SEC_DK_CFG,
+	HW_VAR_BCN_VALID,
+	HW_VAR_RF_TYPE,
+	HW_VAR_DM_FLAG,
+	HW_VAR_DM_FUNC_OP,
+	HW_VAR_DM_FUNC_SET,
+	HW_VAR_DM_FUNC_CLR,
+	HW_VAR_CAM_EMPTY_ENTRY,
+	HW_VAR_CAM_INVALID_ALL,
+	HW_VAR_CAM_WRITE,
+	HW_VAR_CAM_READ,
+	HW_VAR_AC_PARAM_VO,
+	HW_VAR_AC_PARAM_VI,
+	HW_VAR_AC_PARAM_BE,
+	HW_VAR_AC_PARAM_BK,
+	HW_VAR_ACM_CTRL,
+	HW_VAR_AMPDU_MIN_SPACE,
+	HW_VAR_AMPDU_FACTOR,
+	HW_VAR_RXDMA_AGG_PG_TH,
+	HW_VAR_SET_RPWM,
+	HW_VAR_CPWM,
+	HW_VAR_H2C_FW_PWRMODE,
+	HW_VAR_H2C_PS_TUNE_PARAM,
+	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_FWLPS_RF_ON,
+	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+	HW_VAR_TDLS_WRCR,
+	HW_VAR_TDLS_INIT_CH_SEN,
+	HW_VAR_TDLS_RS_RCR,
+	HW_VAR_TDLS_DONE_CH_SEN,
+	HW_VAR_INITIAL_GAIN,
+	HW_VAR_TRIGGER_GPIO_0,
+	HW_VAR_BT_SET_COEXIST,
+	HW_VAR_BT_ISSUE_DELBA,
+	HW_VAR_CURRENT_ANTENNA,
+	HW_VAR_ANTENNA_DIVERSITY_LINK,
+	HW_VAR_ANTENNA_DIVERSITY_SELECT,
+	HW_VAR_SWITCH_EPHY_WoWLAN,
+	HW_VAR_EFUSE_USAGE,
+	HW_VAR_EFUSE_BYTES,
+	HW_VAR_EFUSE_BT_USAGE,
+	HW_VAR_EFUSE_BT_BYTES,
+	HW_VAR_FIFO_CLEARN_UP,
+	HW_VAR_CHECK_TXBUF,
+	HW_VAR_PCIE_STOP_TX_DMA,
+	HW_VAR_APFM_ON_MAC, /* Auto FSM to Turn On, include clock, isolation, power control for MAC only */
+	/*  The valid upper nav range for the HW updating, if the true value is larger than the upper range, the HW won't update it. */
+	/*  Unit in microsecond. 0 means disable this function. */
+	HW_VAR_SYS_CLKR,
+	HW_VAR_NAV_UPPER,
+	HW_VAR_C2H_HANDLE,
+	HW_VAR_RPT_TIMER_SETTING,
+	HW_VAR_TX_RPT_MAX_MACID,
+	HW_VAR_H2C_MEDIA_STATUS_RPT,
+	HW_VAR_CHK_HI_QUEUE_EMPTY,
+	HW_VAR_DL_BCN_SEL,
+	HW_VAR_AMPDU_MAX_TIME,
+	HW_VAR_WIRELESS_MODE,
+	HW_VAR_USB_MODE,
+	HW_VAR_PORT_SWITCH,
+	HW_VAR_DO_IQK,
+	HW_VAR_DM_IN_LPS,
+	HW_VAR_SET_REQ_FW_PS,
+	HW_VAR_FW_PS_STATE,
+	HW_VAR_SOUNDING_ENTER,
+	HW_VAR_SOUNDING_LEAVE,
+	HW_VAR_SOUNDING_RATE,
+	HW_VAR_SOUNDING_STATUS,
+	HW_VAR_SOUNDING_FW_NDPA,
+	HW_VAR_SOUNDING_CLK,
+	HW_VAR_DL_RSVD_PAGE,
+	HW_VAR_MACID_SLEEP,
+	HW_VAR_MACID_WAKEUP,
+};
+
+enum hal_def_variable {
+	HAL_DEF_UNDERCORATEDSMOOTHEDPWDB,
+	HAL_DEF_IS_SUPPORT_ANT_DIV,
+	HAL_DEF_CURRENT_ANTENNA,
+	HAL_DEF_DRVINFO_SZ,
+	HAL_DEF_MAX_RECVBUF_SZ,
+	HAL_DEF_RX_PACKET_OFFSET,
+	HAL_DEF_DBG_DUMP_RXPKT,/* for dbg */
+	HAL_DEF_DBG_DM_FUNC,/* for dbg */
+	HAL_DEF_RA_DECISION_RATE,
+	HAL_DEF_RA_SGI,
+	HAL_DEF_PT_PWR_STATUS,
+	HAL_DEF_TX_LDPC,				/*  LDPC support */
+	HAL_DEF_RX_LDPC,				/*  LDPC support */
+	HAL_DEF_TX_STBC,				/*  TX STBC support */
+	HAL_DEF_RX_STBC,				/*  RX STBC support */
+	HAL_DEF_EXPLICIT_BEAMFORMER,/*  Explicit  Compressed Steering Capable */
+	HAL_DEF_EXPLICIT_BEAMFORMEE,/*  Explicit Compressed Beamforming Feedback Capable */
+	HW_VAR_MAX_RX_AMPDU_FACTOR,
+	HW_DEF_RA_INFO_DUMP,
+	HAL_DEF_DBG_DUMP_TXPKT,
+	HW_DEF_FA_CNT_DUMP,
+	HW_DEF_ODM_DBG_FLAG,
+	HW_DEF_ODM_DBG_LEVEL,
+	HAL_DEF_TX_PAGE_SIZE,
+	HAL_DEF_TX_PAGE_BOUNDARY,
+	HAL_DEF_TX_PAGE_BOUNDARY_WOWLAN,
+	HAL_DEF_ANT_DETECT,/* to do for 8723a */
+	HAL_DEF_PCI_SUUPORT_L1_BACKDOOR, /*  Determine if the L1 Backdoor setting is turned on. */
+	HAL_DEF_PCI_AMD_L1_SUPPORT,
+	HAL_DEF_PCI_ASPM_OSC, /*  Support for ASPM OSC, added by Roger, 2013.03.27. */
+	HAL_DEF_MACID_SLEEP, /*  Support for MACID sleep */
+	HAL_DEF_DBG_RX_INFO_DUMP,
+};
+
+enum hal_odm_variable {
+	HAL_ODM_STA_INFO,
+	HAL_ODM_P2P_STATE,
+	HAL_ODM_WIFI_DISPLAY_STATE,
+	HAL_ODM_NOISE_MONITOR,
+};
+
+enum hal_intf_ps_func {
+	HAL_USB_SELECT_SUSPEND,
+	HAL_MAX_ID,
+};
+
+typedef s32 (*c2h_id_filter)(u8 *c2h_evt);
+
+struct hal_ops {
+	u32 (*hal_power_on)(struct adapter *padapter);
+	void (*hal_power_off)(struct adapter *padapter);
+	u32 (*hal_init)(struct adapter *padapter);
+	u32 (*hal_deinit)(struct adapter *padapter);
+
+	void (*free_hal_data)(struct adapter *padapter);
+
+	u32 (*inirp_init)(struct adapter *padapter);
+	u32 (*inirp_deinit)(struct adapter *padapter);
+	void (*irp_reset)(struct adapter *padapter);
+
+	s32	(*init_xmit_priv)(struct adapter *padapter);
+	void (*free_xmit_priv)(struct adapter *padapter);
+
+	s32	(*init_recv_priv)(struct adapter *padapter);
+	void (*free_recv_priv)(struct adapter *padapter);
+
+	void (*dm_init)(struct adapter *padapter);
+	void (*dm_deinit)(struct adapter *padapter);
+	void (*read_chip_version)(struct adapter *padapter);
+
+	void (*init_default_value)(struct adapter *padapter);
+
+	void (*intf_chip_configure)(struct adapter *padapter);
+
+	void (*read_adapter_info)(struct adapter *padapter);
+
+	void (*enable_interrupt)(struct adapter *padapter);
+	void (*disable_interrupt)(struct adapter *padapter);
+	u8 (*check_ips_status)(struct adapter *padapter);
+	s32		(*interrupt_handler)(struct adapter *padapter);
+	void    (*clear_interrupt)(struct adapter *padapter);
+	void (*set_bwmode_handler)(struct adapter *padapter, enum channel_width Bandwidth, u8 Offset);
+	void (*set_channel_handler)(struct adapter *padapter, u8 channel);
+	void (*set_chnl_bw_handler)(struct adapter *padapter, u8 channel, enum channel_width Bandwidth, u8 Offset40, u8 Offset80);
+
+	void (*set_tx_power_level_handler)(struct adapter *padapter, u8 channel);
+	void (*get_tx_power_level_handler)(struct adapter *padapter, s32 *powerlevel);
+
+	void (*hal_dm_watchdog)(struct adapter *padapter);
+	void (*hal_dm_watchdog_in_lps)(struct adapter *padapter);
+
+
+	void (*SetHwRegHandler)(struct adapter *padapter, u8 variable, u8 *val);
+	void (*GetHwRegHandler)(struct adapter *padapter, u8 variable, u8 *val);
+
+	void (*SetHwRegHandlerWithBuf)(struct adapter *padapter, u8 variable, u8 *pbuf, int len);
+
+	u8 (*GetHalDefVarHandler)(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
+	u8 (*SetHalDefVarHandler)(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
+
+	void (*GetHalODMVarHandler)(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, void *pValue2);
+	void (*SetHalODMVarHandler)(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet);
+
+	void (*UpdateRAMaskHandler)(struct adapter *padapter, u32 mac_id, u8 rssi_level);
+	void (*SetBeaconRelatedRegistersHandler)(struct adapter *padapter);
+
+	void (*Add_RateATid)(struct adapter *padapter, u32 bitmap, u8 *arg, u8 rssi_level);
+
+	void (*run_thread)(struct adapter *padapter);
+	void (*cancel_thread)(struct adapter *padapter);
+
+	u8 (*interface_ps_func)(struct adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val);
+
+	s32	(*hal_xmit)(struct adapter *padapter, struct xmit_frame *pxmitframe);
+	/*
+	 * mgnt_xmit should be implemented to run in interrupt context
+	 */
+	s32 (*mgnt_xmit)(struct adapter *padapter, struct xmit_frame *pmgntframe);
+	s32	(*hal_xmitframe_enqueue)(struct adapter *padapter, struct xmit_frame *pxmitframe);
+
+	u32 (*read_bbreg)(struct adapter *padapter, u32 RegAddr, u32 BitMask);
+	void (*write_bbreg)(struct adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data);
+	u32 (*read_rfreg)(struct adapter *padapter, u8 eRFPath, u32 RegAddr, u32 BitMask);
+	void (*write_rfreg)(struct adapter *padapter, u8 eRFPath, u32 RegAddr, u32 BitMask, u32 Data);
+
+	void (*EfusePowerSwitch)(struct adapter *padapter, u8 bWrite, u8 PwrState);
+	void (*BTEfusePowerSwitch)(struct adapter *padapter, u8 bWrite, u8 PwrState);
+	void (*ReadEFuse)(struct adapter *padapter, u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf, bool bPseudoTest);
+	void (*EFUSEGetEfuseDefinition)(struct adapter *padapter, u8 efuseType, u8 type, void *pOut, bool bPseudoTest);
+	u16 (*EfuseGetCurrentSize)(struct adapter *padapter, u8 efuseType, bool bPseudoTest);
+	int	(*Efuse_PgPacketRead)(struct adapter *padapter, u8 offset, u8 *data, bool bPseudoTest);
+	int	(*Efuse_PgPacketWrite)(struct adapter *padapter, u8 offset, u8 word_en, u8 *data, bool bPseudoTest);
+	u8 (*Efuse_WordEnableDataWrite)(struct adapter *padapter, u16 efuse_addr, u8 word_en, u8 *data, bool bPseudoTest);
+	bool	(*Efuse_PgPacketWrite_BT)(struct adapter *padapter, u8 offset, u8 word_en, u8 *data, bool bPseudoTest);
+
+	s32 (*xmit_thread_handler)(struct adapter *padapter);
+	void (*hal_notch_filter)(struct adapter *adapter, bool enable);
+	void (*hal_reset_security_engine)(struct adapter *adapter);
+	s32 (*c2h_handler)(struct adapter *padapter, u8 *c2h_evt);
+	c2h_id_filter c2h_id_filter_ccx;
+
+	s32 (*fill_h2c_cmd)(struct adapter *, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);
+};
+
+#define RF_CHANGE_BY_INIT	0
+#define RF_CHANGE_BY_IPS	BIT28
+#define RF_CHANGE_BY_PS		BIT29
+#define RF_CHANGE_BY_HW		BIT30
+#define RF_CHANGE_BY_SW		BIT31
+
+#define GET_EEPROM_EFUSE_PRIV(adapter) (&adapter->eeprompriv)
+#define is_boot_from_eeprom(adapter) (adapter->eeprompriv.EepromOrEfuse)
+
+#define Rx_Pairwisekey			0x01
+#define Rx_GTK					0x02
+#define Rx_DisAssoc				0x04
+#define Rx_DeAuth				0x08
+#define Rx_ARPReq				0x09
+#define FWDecisionDisconnect	0x10
+#define Rx_MagicPkt				0x21
+#define Rx_UnicastPkt			0x22
+#define Rx_PatternPkt			0x23
+#define	RX_PNOWakeUp			0x55
+#define	AP_WakeUp			0x66
+
+void rtw_hal_def_value_init(struct adapter *padapter);
+
+void rtw_hal_free_data(struct adapter *padapter);
+
+void rtw_hal_dm_init(struct adapter *padapter);
+void rtw_hal_dm_deinit(struct adapter *padapter);
+
+uint rtw_hal_init(struct adapter *padapter);
+uint rtw_hal_deinit(struct adapter *padapter);
+void rtw_hal_stop(struct adapter *padapter);
+void rtw_hal_set_hwreg(struct adapter *padapter, u8 variable, u8 *val);
+void rtw_hal_get_hwreg(struct adapter *padapter, u8 variable, u8 *val);
+
+void rtw_hal_set_hwreg_with_buf(struct adapter *padapter, u8 variable, u8 *pbuf, int len);
+
+void rtw_hal_chip_configure(struct adapter *padapter);
+void rtw_hal_read_chip_info(struct adapter *padapter);
+void rtw_hal_read_chip_version(struct adapter *padapter);
+
+u8 rtw_hal_set_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
+u8 rtw_hal_get_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
+
+void rtw_hal_set_odm_var(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet);
+void rtw_hal_get_odm_var(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, void *pValue2);
+
+void rtw_hal_enable_interrupt(struct adapter *padapter);
+void rtw_hal_disable_interrupt(struct adapter *padapter);
+
+u8 rtw_hal_check_ips_status(struct adapter *padapter);
+
+s32	rtw_hal_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe);
+s32	rtw_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe);
+s32	rtw_hal_mgnt_xmit(struct adapter *padapter, struct xmit_frame *pmgntframe);
+
+s32	rtw_hal_init_xmit_priv(struct adapter *padapter);
+void rtw_hal_free_xmit_priv(struct adapter *padapter);
+
+s32	rtw_hal_init_recv_priv(struct adapter *padapter);
+void rtw_hal_free_recv_priv(struct adapter *padapter);
+
+void rtw_hal_update_ra_mask(struct sta_info *psta, u8 rssi_level);
+void rtw_hal_add_ra_tid(struct adapter *padapter, u32 bitmap, u8 *arg, u8 rssi_level);
+
+void rtw_hal_start_thread(struct adapter *padapter);
+void rtw_hal_stop_thread(struct adapter *padapter);
+
+void beacon_timing_control(struct adapter *padapter);
+
+u32 rtw_hal_read_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask);
+void rtw_hal_write_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data);
+u32 rtw_hal_read_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask);
+void rtw_hal_write_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data);
+
+#define PHY_QueryBBReg(Adapter, RegAddr, BitMask) rtw_hal_read_bbreg((Adapter), (RegAddr), (BitMask))
+#define PHY_SetBBReg(Adapter, RegAddr, BitMask, Data) rtw_hal_write_bbreg((Adapter), (RegAddr), (BitMask), (Data))
+#define PHY_QueryRFReg(Adapter, eRFPath, RegAddr, BitMask) rtw_hal_read_rfreg((Adapter), (eRFPath), (RegAddr), (BitMask))
+#define PHY_SetRFReg(Adapter, eRFPath, RegAddr, BitMask, Data) rtw_hal_write_rfreg((Adapter), (eRFPath), (RegAddr), (BitMask), (Data))
+
+#define PHY_SetMacReg	PHY_SetBBReg
+#define PHY_QueryMacReg PHY_QueryBBReg
+
+void rtw_hal_set_chan(struct adapter *padapter, u8 channel);
+void rtw_hal_set_chnl_bw(struct adapter *padapter, u8 channel, enum channel_width Bandwidth, u8 Offset40, u8 Offset80);
+void rtw_hal_dm_watchdog(struct adapter *padapter);
+void rtw_hal_dm_watchdog_in_lps(struct adapter *padapter);
+
+s32 rtw_hal_xmit_thread_handler(struct adapter *padapter);
+
+void rtw_hal_notch_filter(struct adapter *adapter, bool enable);
+void rtw_hal_reset_security_engine(struct adapter *adapter);
+
+bool rtw_hal_c2h_valid(struct adapter *adapter, u8 *buf);
+s32 rtw_hal_c2h_handler(struct adapter *adapter, u8 *c2h_evt);
+c2h_id_filter rtw_hal_c2h_id_filter_ccx(struct adapter *adapter);
+
+s32 rtw_hal_macid_sleep(struct adapter *padapter, u32 macid);
+s32 rtw_hal_macid_wakeup(struct adapter *padapter, u32 macid);
+
+s32 rtw_hal_fill_h2c_cmd(struct adapter *, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);
+
+#endif /* __HAL_INTF_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_pg.h b/drivers/staging/rtl8723bs/include/hal_pg.h
new file mode 100644
index 000000000..7cb9c441f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_pg.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __HAL_PG_H__
+#define __HAL_PG_H__
+
+#define	MAX_RF_PATH				4
+/* MAX_TX_COUNT must always be set to 4, otherwise the read efuse table
+ * sequence will be wrong.
+ */
+#define MAX_TX_COUNT				4
+
+/*  TX power by rate table. */
+/*  RF: = AB = 0/1 */
+/*  CCK = 0 OFDM = 1 HT-MCS 0-7 = 2 */
+#define TX_PWR_BY_RATE_NUM_RATE			84
+#define MAX_RF_PATH_NUM				2
+#define	MAX_CHNL_GROUP_24G			6
+#define EEPROM_DEFAULT_BOARD_OPTION		0x00
+
+/* EEPROM/Efuse PG Offset for 8723BE/8723BU/8723BS */
+/*  0x10 ~ 0x63 = TX power area. */
+#define	EEPROM_TX_PWR_INX_8723B			0x10
+/* New EFUSE default value */
+#define EEPROM_DEFAULT_24G_INDEX		0x2D
+#define EEPROM_DEFAULT_24G_HT20_DIFF		0X02
+#define EEPROM_DEFAULT_24G_OFDM_DIFF		0X04
+#define	EEPROM_Default_ThermalMeter_8723B	0x18
+#define EEPROM_Default_CrystalCap_8723B		0x20
+
+#define	EEPROM_ChannelPlan_8723B		0xB8
+#define	EEPROM_XTAL_8723B			0xB9
+#define	EEPROM_THERMAL_METER_8723B		0xBA
+
+#define	EEPROM_RF_BOARD_OPTION_8723B		0xC1
+#define	EEPROM_RF_BT_SETTING_8723B		0xC3
+#define	EEPROM_VERSION_8723B			0xC4
+#define	EEPROM_CustomID_8723B			0xC5
+#define EEPROM_DEFAULT_DIFF			0XFE
+
+/* RTL8723BS */
+#define	EEPROM_MAC_ADDR_8723BS			0x11A
+#define EEPROM_Voltage_ADDR_8723B		0x8
+#define RTL_EEPROM_ID				0x8129
+
+struct TxPowerInfo24G {
+	u8 IndexCCK_Base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	u8 IndexBW40_Base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	/* If only one tx, only BW20 and OFDM are used. */
+	s8 CCK_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8 OFDM_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8 BW20_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8 BW40_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+enum {
+	Ant_x2	= 0,
+	Ant_x1	= 1
+};
+
+enum {
+	BT_RTL8723B = 8,
+};
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_phy.h b/drivers/staging/rtl8723bs/include/hal_phy.h
new file mode 100644
index 000000000..3d71a4f41
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_PHY_H__
+#define __HAL_PHY_H__
+/*  */
+/*  Antenna detection method, i.e., using single tone detection or RSSI reported from each antenna detected. */
+/*  Added by Roger, 2013.05.22. */
+/*  */
+#define ANT_DETECT_BY_SINGLE_TONE	BIT0
+#define ANT_DETECT_BY_RSSI				BIT1
+#define IS_ANT_DETECT_SUPPORT_SINGLE_TONE(__Adapter)		((GET_HAL_DATA(__Adapter)->AntDetection) & ANT_DETECT_BY_SINGLE_TONE)
+#define IS_ANT_DETECT_SUPPORT_RSSI(__Adapter)		((GET_HAL_DATA(__Adapter)->AntDetection) & ANT_DETECT_BY_RSSI)
+
+
+/*--------------------------Define Parameters-------------------------------*/
+enum {
+	RF_TYPE_MIN = 0,	/*  0 */
+	RF_8225 = 1,		/*  1 11b/g RF for verification only */
+	RF_8256 = 2,		/*  2 11b/g/n */
+	RF_8258 = 3,		/*  3 11a/b/g/n RF */
+	RF_6052 = 4,		/*  4 11b/g/n RF */
+	RF_PSEUDO_11N = 5,	/*  5, It is a temporality RF. */
+	RF_TYPE_MAX
+};
+
+enum rf_path {
+	RF_PATH_A = 0,
+	RF_PATH_B,
+	RF_PATH_MAX
+};
+
+#define	TX_1S			0
+#define	TX_2S			1
+#define	TX_3S			2
+#define	TX_4S			3
+
+#define	RF_PATH_MAX_92C_88E		2
+#define	RF_PATH_MAX_90_8812		4	/* Max RF number 90 support */
+
+enum wireless_mode {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+	WIRELESS_MODE_N_24G = 0x10,
+	WIRELESS_MODE_AC_24G  = 0x80,
+	WIRELESS_MODE_AC_ONLY  = 0x100,
+};
+
+enum SwChnlCmdID {
+	CmdID_End,
+	CmdID_SetTxPowerLevel,
+	CmdID_BBRegWrite10,
+	CmdID_WritePortUlong,
+	CmdID_WritePortUshort,
+	CmdID_WritePortUchar,
+	CmdID_RF_WriteReg,
+};
+
+struct SwChnlCmd {
+	enum SwChnlCmdID	CmdID;
+	u32 			Para1;
+	u32 			Para2;
+	u32 			msDelay;
+};
+
+/*--------------------------Exported Function prototype---------------------*/
+
+#endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_phy_cfg.h b/drivers/staging/rtl8723bs/include/hal_phy_cfg.h
new file mode 100644
index 000000000..ea494bcf8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy_cfg.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __INC_HAL8723BPHYCFG_H__
+#define __INC_HAL8723BPHYCFG_H__
+
+/*--------------------------Define Parameters-------------------------------*/
+#define LOOP_LIMIT		5
+#define MAX_STALL_TIME		50	/* us */
+#define AntennaDiversityValue	0x80	/* Adapter->bSoftwareAntennaDiversity ? 0x00:0x80) */
+#define MAX_TXPWR_IDX_NMODE_92S	63
+#define Reset_Cnt_Limit		3
+
+#define MAX_AGGR_NUM	0x07
+
+
+/*--------------------------Define Parameters End-------------------------------*/
+
+
+/*------------------------------Define structure----------------------------*/
+
+/*------------------------------Define structure End----------------------------*/
+
+/*--------------------------Exported Function prototype---------------------*/
+u32 PHY_QueryBBReg_8723B(struct adapter *Adapter, u32 RegAddr, u32 BitMask);
+
+void PHY_SetBBReg_8723B(struct adapter *Adapter, u32 RegAddr,
+			u32 BitMask, u32 Data);
+
+u32 PHY_QueryRFReg_8723B(struct adapter *Adapter, u8 eRFPath,
+			 u32 RegAddr, u32 BitMask);
+
+void PHY_SetRFReg_8723B(struct adapter *Adapter, u8 eRFPath,
+			u32 RegAddr, u32 BitMask, u32 Data);
+
+/* MAC/BB/RF HAL config */
+int PHY_BBConfig8723B(struct adapter *Adapter);
+
+int PHY_RFConfig8723B(struct adapter *Adapter);
+
+s32 PHY_MACConfig8723B(struct adapter *padapter);
+
+void PHY_SetTxPowerIndex(struct adapter *Adapter, u32 PowerIndex,
+			 u8 RFPath, u8 Rate);
+
+u8 PHY_GetTxPowerIndex(struct adapter *padapter, u8 RFPath, u8 Rate,
+			enum channel_width BandWidth, u8 Channel);
+
+void PHY_GetTxPowerLevel8723B(struct adapter *Adapter, s32 *powerlevel);
+
+void PHY_SetTxPowerLevel8723B(struct adapter *Adapter, u8 channel);
+
+void PHY_SetBWMode8723B(struct adapter *Adapter, enum channel_width Bandwidth,
+			unsigned char Offset);
+
+/*  Call after initialization */
+void PHY_SwChnl8723B(struct adapter *Adapter, u8 channel);
+
+void PHY_SetSwChnlBWMode8723B(struct adapter *Adapter, u8 channel,
+				enum channel_width Bandwidth,
+				u8 Offset40, u8 Offset80);
+
+/*--------------------------Exported Function prototype End---------------------*/
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_phy_reg.h b/drivers/staging/rtl8723bs/include/hal_phy_reg.h
new file mode 100644
index 000000000..682cdd665
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy_reg.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_PHY_REG_H__
+#define __HAL_PHY_REG_H__
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+/* if (RTL92SE_FPGA_VERIFY == 1) */
+/* define		bRFRegOffsetMask	0xfff */
+/* else */
+#define			bRFRegOffsetMask	0xfffff
+/* endif */
+
+#endif /* __HAL_PHY_REG_H__ */
diff --git a/drivers/staging/rtl8723bs/include/hal_phy_reg_8723b.h b/drivers/staging/rtl8723bs/include/hal_phy_reg_8723b.h
new file mode 100644
index 000000000..b0b1ac109
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy_reg_8723b.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __INC_HAL8723BPHYREG_H__
+#define __INC_HAL8723BPHYREG_H__
+
+#include <Hal8192CPhyReg.h>
+
+/*  BB Register Definition */
+/*  */
+/*  4. Page9(0x900) */
+/*  */
+#define rDPDT_control				0x92c
+#define rfe_ctrl_anta_src				0x930
+#define rS0S1_PathSwitch			0x948
+#define AGC_table_select				0xb2c
+
+/*  */
+/*  PageB(0xB00) */
+/*  */
+#define rPdp_AntA						0xb00
+#define rPdp_AntA_4						0xb04
+#define rPdp_AntA_8						0xb08
+#define rPdp_AntA_C						0xb0c
+#define rPdp_AntA_10					0xb10
+#define rPdp_AntA_14					0xb14
+#define rPdp_AntA_18					0xb18
+#define rPdp_AntA_1C					0xb1c
+#define rPdp_AntA_20					0xb20
+#define rPdp_AntA_24					0xb24
+
+#define rConfig_Pmpd_AntA				0xb28
+#define rConfig_ram64x16				0xb2c
+
+#define rBndA							0xb30
+#define rHssiPar						0xb34
+
+#define rConfig_AntA					0xb68
+#define rConfig_AntB					0xb6c
+
+#define rPdp_AntB						0xb70
+#define rPdp_AntB_4						0xb74
+#define rPdp_AntB_8						0xb78
+#define rPdp_AntB_C						0xb7c
+#define rPdp_AntB_10					0xb80
+#define rPdp_AntB_14					0xb84
+#define rPdp_AntB_18					0xb88
+#define rPdp_AntB_1C					0xb8c
+#define rPdp_AntB_20					0xb90
+#define rPdp_AntB_24					0xb94
+
+#define rConfig_Pmpd_AntB				0xb98
+
+#define rBndB							0xba0
+
+#define rAPK							0xbd8
+#define rPm_Rx0_AntA					0xbdc
+#define rPm_Rx1_AntA					0xbe0
+#define rPm_Rx2_AntA					0xbe4
+#define rPm_Rx3_AntA					0xbe8
+#define rPm_Rx0_AntB					0xbec
+#define rPm_Rx1_AntB					0xbf0
+#define rPm_Rx2_AntB					0xbf4
+#define rPm_Rx3_AntB					0xbf8
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_pwr_seq.h b/drivers/staging/rtl8723bs/include/hal_pwr_seq.h
new file mode 100644
index 000000000..0a2e60770
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_pwr_seq.h
@@ -0,0 +1,233 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef REALTEK_POWER_SEQUENCE_8723B
+#define REALTEK_POWER_SEQUENCE_8723B
+
+#include "HalPwrSeqCmd.h"
+
+/*
+	Check document WM-20130815-JackieLau-RTL8723B_Power_Architecture v08.vsd
+	There are 6 HW Power States:
+	0: POFF--Power Off
+	1: PDN--Power Down
+	2: CARDEMU--Card Emulation
+	3: ACT--Active Mode
+	4: LPS--Low Power State
+	5: SUS--Suspend
+
+	The transition from different states are defined below
+	TRANS_CARDEMU_TO_ACT
+	TRANS_ACT_TO_CARDEMU
+	TRANS_CARDEMU_TO_SUS
+	TRANS_SUS_TO_CARDEMU
+	TRANS_CARDEMU_TO_PDN
+	TRANS_ACT_TO_LPS
+	TRANS_LPS_TO_ACT
+
+	TRANS_END
+*/
+#define	RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS	26
+#define	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8723B_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS	15
+#define	RTL8723B_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8723B_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8723B_TRANS_ACT_TO_SWLPS_STEPS		22
+#define	RTL8723B_TRANS_SWLPS_TO_ACT_STEPS		15
+#define	RTL8723B_TRANS_END_STEPS		1
+
+
+#define RTL8723B_TRANS_CARDEMU_TO_ACT														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/								\
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0}, /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/   \
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x67[0] = 0 to disable BT_GPS_SEL pins*/	\
+	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS},/*Delay 1ms*/   \
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, 0}, /*0x00[5] = 1b'0 release analog Ips to digital , 1:isolation*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT4|BIT3|BIT2), 0},/* disable SW LPS 0x04[10]= 0 and WLSUS_EN 0x04[11]= 0*/	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* Disable USB suspend */	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/* Enable USB suspend */	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* release WLON reset  0x04[16]= 1*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]= 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT4|BIT3), 0},/* disable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0},/**/	\
+	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6, BIT6},/* Enable WL control XTAL setting*/	\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},/*Enable falling edge triggering interrupt*/\
+	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},/*Enable GPIO9 interrupt mode*/\
+	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*Enable GPIO9 input mode*/\
+	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*Enable HSISR GPIO[C:0] interrupt*/\
+	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},/*Enable HSISR GPIO9 interrupt*/\
+	{0x0068, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, BIT3},/*For GPIO9 internal pull high setting by test chip*/\
+	{0x0069, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6, BIT6},/*For GPIO9 internal pull high setting*/\
+
+
+#define RTL8723B_TRANS_ACT_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/								\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/	\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*Enable rising edge triggering interrupt*/ \
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* release WLON reset  0x04[16]= 1*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
+	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6, 0},/* Enable BT control XTAL setting*/\
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5}, /*0x00[5] = 1b'1 analog Ips to digital , 1:isolation*/   \
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/\
+
+
+#define RTL8723B_TRANS_CARDEMU_TO_SUS													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4|BIT3, (BIT4|BIT3)}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+
+#define RTL8723B_TRANS_SUS_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
+
+#define RTL8723B_TRANS_CARDEMU_TO_CARDDIS													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, omments here*/								\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07 = 0x20 , SOP option to disable BG/MB*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, BIT2}, /*0x04[10] = 1, enable SW LPS*/	\
+        {0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1}, /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+
+#define RTL8723B_TRANS_CARDDIS_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
+        {0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0}, /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
+
+
+#define RTL8723B_TRANS_CARDEMU_TO_PDN												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK|PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/   \
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/* 0x04[16] = 0*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
+
+#define RTL8723B_TRANS_PDN_TO_CARDEMU												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
+
+#define RTL8723B_TRANS_ACT_TO_LPS														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},/*Tx Pause*/	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled, and clock are gated*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*Whole BB is reset*/	\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/	\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00},/*When driver enter Sus/ Disable, enable LOP for BT*/	\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/	\
+
+
+#define RTL8723B_TRANS_LPS_TO_ACT															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]= 0  TSF in 40M*/\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+
+ #define RTL8723B_TRANS_ACT_TO_SWLPS														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0194, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*enable 32 K source*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled, and clock are gated*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1},/*CCK and OFDM are enable*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled, and clock are gated*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1},/*CCK and OFDM are enable*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled, and clock are gated*/	\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*disable security engine*/	\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x40},/*When driver enter Sus/ Disable, enable LOP for BT*/	\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5},/*reset dual TSF*/	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0},/*Reset CPU*/	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*Reset MCUFWDL register*/	\
+	{0x001D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*Reset CPU IO Wrapper*/	\
+	{0x001D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1},/*Reset CPU IO Wrapper*/	\
+	{0x0287, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*polling RXFF packet number = 0 */	\
+	{0x0286, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1},/*polling RXDMA idle */	\
+	{0x013D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*Clear FW RPWM interrupt */\
+	{0x0139, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*Set FW RPWM interrupt source*/\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4},/*switch TSF to 32K*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},/*polling TSF stable*/\
+	{0x0090, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*Set FW LPS*/	\
+	{0x0090, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0},/*polling FW LPS ready */
+
+
+#define RTL8723B_TRANS_SWLPS_TO_ACT															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0},/*switch TSF to 32K*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/*polling TSF stable*/\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1, enable security engine*/\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
+	{0x06B7, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x09}, /*.	reset MAC rx state machine*/\
+	{0x06B4, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x86}, /*.	reset MAC rx state machine*/\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},/* set CPU RAM code ready*/	\
+	{0x001D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*Reset CPU IO Wrapper*/	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0},/* Enable CPU*/	\
+	{0x001D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/*enable CPU IO Wrapper*/	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, BIT2},/* Enable CPU*/	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, BIT7},/*polling FW init ready */	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT6, BIT6},/*polling FW init ready */	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+#define RTL8723B_TRANS_END															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 0, PWR_CMD_END, 0, 0}, 
+
+
+extern struct wlan_pwr_cfg rtl8723B_power_on_flow[RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_radio_off_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_card_disable_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_card_enable_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_suspend_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_resume_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_hwpdn_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_enter_lps_flow[RTL8723B_TRANS_ACT_TO_LPS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_leave_lps_flow[RTL8723B_TRANS_LPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_enter_swlps_flow[RTL8723B_TRANS_ACT_TO_SWLPS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_leave_swlps_flow[RTL8723B_TRANS_SWLPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS];
+#endif
diff --git a/drivers/staging/rtl8723bs/include/hal_sdio.h b/drivers/staging/rtl8723bs/include/hal_sdio.h
new file mode 100644
index 000000000..3fc8acb43
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_sdio.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __HAL_SDIO_H_
+#define __HAL_SDIO_H_
+
+#define ffaddr2deviceId(pdvobj, addr)	(pdvobj->Queue2Pipe[addr])
+
+u8 rtw_hal_sdio_max_txoqt_free_space(struct adapter *padapter);
+u8 rtw_hal_sdio_query_tx_freepage(struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum);
+void rtw_hal_sdio_update_tx_freepage(struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum);
+void rtw_hal_set_sdio_tx_max_length(struct adapter *padapter, u8 numHQ, u8 numNQ, u8 numLQ, u8 numPubQ);
+u32 rtw_hal_get_sdio_tx_max_length(struct adapter *padapter, u8 queue_idx);
+
+#endif /* __RTW_LED_H_ */
diff --git a/drivers/staging/rtl8723bs/include/ieee80211.h b/drivers/staging/rtl8723bs/include/ieee80211.h
new file mode 100644
index 000000000..9041d8dc5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/ieee80211.h
@@ -0,0 +1,786 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __IEEE80211_H
+#define __IEEE80211_H
+
+#include <linux/ieee80211.h>
+
+#define MGMT_QUEUE_NUM 5
+
+#define ETH_ALEN	6
+#define ETH_TYPE_LEN		2
+#define PAYLOAD_TYPE_LEN	1
+
+#define RTL_IOCTL_HOSTAPD (SIOCIWFIRSTPRIV + 28)
+
+/* RTL871X_IOCTL_HOSTAPD ioctl() cmd: */
+enum {
+	RTL871X_HOSTAPD_FLUSH = 1,
+	RTL871X_HOSTAPD_ADD_STA = 2,
+	RTL871X_HOSTAPD_REMOVE_STA = 3,
+	RTL871X_HOSTAPD_GET_INFO_STA = 4,
+	/* REMOVED: PRISM2_HOSTAPD_RESET_TXEXC_STA = 5, */
+	RTL871X_HOSTAPD_GET_WPAIE_STA = 5,
+	RTL871X_SET_ENCRYPTION = 6,
+	RTL871X_GET_ENCRYPTION = 7,
+	RTL871X_HOSTAPD_SET_FLAGS_STA = 8,
+	RTL871X_HOSTAPD_GET_RID = 9,
+	RTL871X_HOSTAPD_SET_RID = 10,
+	RTL871X_HOSTAPD_SET_ASSOC_AP_ADDR = 11,
+	RTL871X_HOSTAPD_SET_GENERIC_ELEMENT = 12,
+	RTL871X_HOSTAPD_MLME = 13,
+	RTL871X_HOSTAPD_SCAN_REQ = 14,
+	RTL871X_HOSTAPD_STA_CLEAR_STATS = 15,
+	RTL871X_HOSTAPD_SET_BEACON = 16,
+	RTL871X_HOSTAPD_SET_WPS_BEACON = 17,
+	RTL871X_HOSTAPD_SET_WPS_PROBE_RESP = 18,
+	RTL871X_HOSTAPD_SET_WPS_ASSOC_RESP = 19,
+	RTL871X_HOSTAPD_SET_HIDDEN_SSID = 20,
+	RTL871X_HOSTAPD_SET_MACADDR_ACL = 21,
+	RTL871X_HOSTAPD_ACL_ADD_STA = 22,
+	RTL871X_HOSTAPD_ACL_REMOVE_STA = 23,
+};
+
+/* STA flags */
+#define WLAN_STA_AUTH BIT(0)
+#define WLAN_STA_ASSOC BIT(1)
+#define WLAN_STA_PS BIT(2)
+#define WLAN_STA_TIM BIT(3)
+#define WLAN_STA_PERM BIT(4)
+#define WLAN_STA_AUTHORIZED BIT(5)
+#define WLAN_STA_PENDING_POLL BIT(6) /* pending activity poll not ACKed */
+#define WLAN_STA_SHORT_PREAMBLE BIT(7)
+#define WLAN_STA_PREAUTH BIT(8)
+#define WLAN_STA_WME BIT(9)
+#define WLAN_STA_MFP BIT(10)
+#define WLAN_STA_HT BIT(11)
+#define WLAN_STA_WPS BIT(12)
+#define WLAN_STA_MAYBE_WPS BIT(13)
+#define WLAN_STA_NONERP BIT(31)
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define IEEE_CMD_SET_WPA_IE				2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME						4
+
+#define IEEE_PARAM_WPA_ENABLED				1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED			3
+#define IEEE_PARAM_PRIVACY_INVOKED			4
+#define IEEE_PARAM_AUTH_ALGS					5
+#define IEEE_PARAM_IEEE_802_1X				6
+#define IEEE_PARAM_WPAX_SELECT				7
+
+#define IEEE_MLME_STA_DEAUTH				1
+#define IEEE_MLME_STA_DISASSOC			2
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG			2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR			3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED		4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED			5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED		6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#define WPA_CIPHER_NONE		BIT(0)
+#define WPA_CIPHER_WEP40	BIT(1)
+#define WPA_CIPHER_WEP104 BIT(2)
+#define WPA_CIPHER_TKIP		BIT(3)
+#define WPA_CIPHER_CCMP		BIT(4)
+
+
+
+#define WPA_SELECTOR_LEN 4
+extern u8 RTW_WPA_OUI_TYPE[];
+extern u16 RTW_WPA_VERSION;
+extern u8 WPA_AUTH_KEY_MGMT_NONE[];
+extern u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[];
+extern u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[];
+extern u8 WPA_CIPHER_SUITE_NONE[];
+extern u8 WPA_CIPHER_SUITE_WEP40[];
+extern u8 WPA_CIPHER_SUITE_TKIP[];
+extern u8 WPA_CIPHER_SUITE_WRAP[];
+extern u8 WPA_CIPHER_SUITE_CCMP[];
+extern u8 WPA_CIPHER_SUITE_WEP104[];
+
+
+#define RSN_HEADER_LEN 4
+#define RSN_SELECTOR_LEN 4
+
+extern u16 RSN_VERSION_BSD;
+extern u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[];
+extern u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[];
+extern u8 RSN_CIPHER_SUITE_NONE[];
+extern u8 RSN_CIPHER_SUITE_WEP40[];
+extern u8 RSN_CIPHER_SUITE_TKIP[];
+extern u8 RSN_CIPHER_SUITE_WRAP[];
+extern u8 RSN_CIPHER_SUITE_CCMP[];
+extern u8 RSN_CIPHER_SUITE_WEP104[];
+
+
+enum {
+	RATEID_IDX_BGN_40M_2SS = 0,
+	RATEID_IDX_BGN_40M_1SS = 1,
+	RATEID_IDX_BGN_20M_2SS_BN = 2,
+	RATEID_IDX_BGN_20M_1SS_BN = 3,
+	RATEID_IDX_GN_N2SS = 4,
+	RATEID_IDX_GN_N1SS = 5,
+	RATEID_IDX_BG = 6,
+	RATEID_IDX_G = 7,
+	RATEID_IDX_B = 8,
+};
+
+enum network_type {
+	WIRELESS_INVALID = 0,
+	/* Sub-Element */
+	WIRELESS_11B = BIT(0), /*  tx: cck only , rx: cck only, hw: cck */
+	WIRELESS_11G = BIT(1), /*  tx: ofdm only, rx: ofdm & cck, hw: cck & ofdm */
+	WIRELESS_11_24N = BIT(3), /*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
+	WIRELESS_AUTO = BIT(5),
+
+	/* Combination */
+	/* Type for current wireless mode */
+	WIRELESS_11BG = (WIRELESS_11B|WIRELESS_11G), /*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
+	WIRELESS_11G_24N = (WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
+	WIRELESS_11B_24N = (WIRELESS_11B|WIRELESS_11_24N), /*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
+	WIRELESS_11BG_24N = (WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
+};
+
+#define SUPPORTED_24G_NETTYPE_MSK (WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N)
+
+#define is_legacy_only(net_type)  ((net_type) == ((net_type) & (WIRELESS_11BG)))
+
+#define is_supported_24g(net_type) ((net_type) & SUPPORTED_24G_NETTYPE_MSK ? true : false)
+
+#define is_supported_tx_cck(net_type) (((net_type) & (WIRELESS_11B)) ? true : false)
+#define is_supported_ht(net_type) (((net_type) & (WIRELESS_11_24N)) ? true : false)
+
+struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[];
+		} wpa_ie;
+	        struct{
+			int command;
+			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[];
+		} crypt;
+		struct {
+			u16 aid;
+			u16 capability;
+			int flags;
+			u8 tx_supp_rates[16];
+			struct ieee80211_ht_cap ht_cap;
+		} add_sta;
+		struct {
+			u8 reserved[2];/* for set max_num_sta */
+			u8 buf[];
+		} bcn_ie;
+	} u;
+};
+
+struct ieee_param_ex {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	u8 data[];
+};
+
+struct sta_data {
+	u16 aid;
+	u16 capability;
+	int flags;
+	u32 sta_set;
+	u8 tx_supp_rates[16];
+	u32 tx_supp_rates_len;
+	struct ieee80211_ht_cap ht_cap;
+	u64	rx_pkts;
+	u64	rx_bytes;
+	u64	rx_drops;
+	u64	tx_pkts;
+	u64	tx_bytes;
+	u64	tx_drops;
+};
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __attribute__ ((packed));
+
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* sequence control field */
+#define RTW_IEEE80211_SCTL_FRAG	0x000F
+#define RTW_IEEE80211_SCTL_SEQ	0xFFF0
+
+
+#define RTW_ERP_INFO_NON_ERP_PRESENT BIT(0)
+#define RTW_ERP_INFO_USE_PROTECTION BIT(1)
+#define RTW_ERP_INFO_BARKER_PREAMBLE_MODE BIT(2)
+
+/* QoS, QOS */
+#define NORMAL_ACK			0
+#define NO_ACK				1
+#define NON_EXPLICIT_ACK	2
+#define BLOCK_ACK			3
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
+#endif /* ETH_P_PAE */
+
+#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
+
+#define ETH_P_ECONET	0x0018
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+        u8    dsap;   /* always 0xAA */
+        u8    ssap;   /* always 0xAA */
+        u8    ctrl;   /* always 0x03 */
+        u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+} __attribute__ ((packed));
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+
+#define WLAN_QC_GET_TID(qc) ((qc) & 0x0f)
+
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTW_IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & RTW_IEEE80211_SCTL_SEQ)
+
+/* Reason codes */
+#define WLAN_REASON_ACTIVE_ROAM 65533
+#define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
+#define WLAN_REASON_EXPIRATION_CHK 65535
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+
+#define IEEE80211_STATMASK_SIGNAL (1<<0)
+#define IEEE80211_STATMASK_RSSI (1<<1)
+#define IEEE80211_STATMASK_NOISE (1<<2)
+#define IEEE80211_STATMASK_RATE (1<<3)
+#define IEEE80211_STATMASK_WEMASK 0x7
+
+
+#define IEEE80211_CCK_MODULATION    (1<<0)
+#define IEEE80211_OFDM_MODULATION   (1<<1)
+
+#define IEEE80211_24GHZ_BAND     (1<<0)
+#define IEEE80211_52GHZ_BAND     (1<<1)
+
+#define IEEE80211_CCK_RATE_LEN			4
+#define IEEE80211_NUM_OFDM_RATESLEN	8
+
+
+#define IEEE80211_CCK_RATE_1MB		        0x02
+#define IEEE80211_CCK_RATE_2MB		        0x04
+#define IEEE80211_CCK_RATE_5MB		        0x0B
+#define IEEE80211_CCK_RATE_11MB		        0x16
+#define IEEE80211_OFDM_RATE_LEN			8
+#define IEEE80211_OFDM_RATE_6MB		        0x0C
+#define IEEE80211_OFDM_RATE_9MB		        0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
+#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
+#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
+#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
+#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
+#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
+#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
+#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
+#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
+#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
+#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
+#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
+
+#define IEEE80211_CCK_RATES_MASK	        0x0000000F
+#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
+	IEEE80211_CCK_RATE_2MB_MASK)
+#define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
+        IEEE80211_CCK_RATE_5MB_MASK | \
+        IEEE80211_CCK_RATE_11MB_MASK)
+
+#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
+#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
+	IEEE80211_OFDM_RATE_12MB_MASK | \
+	IEEE80211_OFDM_RATE_24MB_MASK)
+#define IEEE80211_OFDM_DEFAULT_RATES_MASK	(IEEE80211_OFDM_BASIC_RATES_MASK | \
+	IEEE80211_OFDM_RATE_9MB_MASK  | \
+	IEEE80211_OFDM_RATE_18MB_MASK | \
+	IEEE80211_OFDM_RATE_36MB_MASK | \
+	IEEE80211_OFDM_RATE_48MB_MASK | \
+	IEEE80211_OFDM_RATE_54MB_MASK)
+#define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
+                                IEEE80211_CCK_DEFAULT_RATES_MASK)
+
+#define IEEE80211_NUM_OFDM_RATES	    8
+#define IEEE80211_NUM_CCK_RATES	            4
+#define IEEE80211_OFDM_SHIFT_MASK_A         4
+
+
+enum {
+	MGN_1M		= 0x02,
+	MGN_2M		= 0x04,
+	MGN_5_5M	= 0x0B,
+	MGN_6M		= 0x0C,
+	MGN_9M		= 0x12,
+	MGN_11M		= 0x16,
+	MGN_12M	= 0x18,
+	MGN_18M	= 0x24,
+	MGN_24M	= 0x30,
+	MGN_36M	= 0x48,
+	MGN_48M	= 0x60,
+	MGN_54M	= 0x6C,
+	MGN_MCS32	= 0x7F,
+	MGN_MCS0,
+	MGN_MCS1,
+	MGN_MCS2,
+	MGN_MCS3,
+	MGN_MCS4,
+	MGN_MCS5,
+	MGN_MCS6,
+	MGN_MCS7,
+	MGN_UNKNOWN
+};
+
+#define IS_HT_RATE(_rate)				(_rate >= MGN_MCS0 && _rate <= MGN_MCS31)
+#define IS_CCK_RATE(_rate)				(MGN_1M == _rate || _rate == MGN_2M || _rate == MGN_5_5M || _rate == MGN_11M)
+#define IS_OFDM_RATE(_rate)				(MGN_6M <= _rate && _rate <= MGN_54M  && _rate != MGN_11M)
+
+
+/* NOTE: This data is for statistical purposes; not all hardware provides this
+ *       information for frames received.  Not setting these will not cause
+ *       any adverse affects. */
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
+#define IEEE80211_FRAG_CACHE_LEN 4
+
+#define SEC_KEY_1         (1<<0)
+#define SEC_KEY_2         (1<<1)
+#define SEC_KEY_3         (1<<2)
+#define SEC_KEY_4         (1<<3)
+#define SEC_ACTIVE_KEY    (1<<4)
+#define SEC_AUTH_MODE     (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL         (1<<7)
+#define SEC_ENABLED       (1<<8)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define WEP_KEYS 4
+#define WEP_KEY_LEN 13
+
+#define BIP_MAX_KEYID 5
+#define BIP_AAD_SIZE  20
+
+/*
+
+ 802.11 data frame from AP
+
+      ,-------------------------------------------------------------------.
+Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+      |------|------|---------|---------|---------|------|---------|------|
+Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+      |      | tion | (BSSID) |         |         | ence |  data   |      |
+      `-------------------------------------------------------------------'
+
+Total: 28-2340 bytes
+
+*/
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID       0
+#define MFIE_TYPE_RATES      1
+#define MFIE_TYPE_FH_SET     2
+#define MFIE_TYPE_DS_SET     3
+#define MFIE_TYPE_CF_SET     4
+#define MFIE_TYPE_TIM        5
+#define MFIE_TYPE_IBSS_SET   6
+#define MFIE_TYPE_CHALLENGE  16
+#define MFIE_TYPE_ERP        42
+#define MFIE_TYPE_RSN	     48
+#define MFIE_TYPE_RATES_EX   50
+#define MFIE_TYPE_GENERIC    221
+
+/* SWEEP TABLE ENTRIES NUMBER*/
+#define MAX_SWEEP_TAB_ENTRIES		  42
+#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element... */
+#define MAX_RATES_LENGTH                  ((u8)12)
+#define MAX_RATES_EX_LENGTH               ((u8)16)
+#define MAX_NETWORK_COUNT                  128
+#define MAX_CHANNEL_NUMBER                 161
+#define IEEE80211_SOFTMAC_SCAN_TIME	  400
+/* HZ / 2) */
+#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define CRC_LENGTH                 4U
+
+#define MAX_WPA_IE_LEN (256)
+#define MAX_WPS_IE_LEN (512)
+#define MAX_P2P_IE_LEN (256)
+#define MAX_WFD_IE_LEN (128)
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+#define IEEE80211_DTIM_MBCAST 4
+#define IEEE80211_DTIM_UCAST 2
+#define IEEE80211_DTIM_VALID 1
+#define IEEE80211_DTIM_INVALID 0
+
+#define IEEE80211_PS_DISABLED 0
+#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
+#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
+#define IW_ESSID_MAX_SIZE 32
+/*
+join_res:
+-1: authentication fail
+-2: association fail
+> 0: TID
+*/
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+#define MAC_ARG(x) (x)
+#define IP_ARG(x) (x)
+
+static inline int is_multicast_mac_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+
+static inline int is_broadcast_mac_addr(const u8 *addr)
+{
+	return ((addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&   \
+		(addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff));
+}
+
+static inline int is_zero_mac_addr(const u8 *addr)
+{
+	return ((addr[0] == 0x00) && (addr[1] == 0x00) && (addr[2] == 0x00) &&   \
+		(addr[3] == 0x00) && (addr[4] == 0x00) && (addr[5] == 0x00));
+}
+
+#define CFG_IEEE80211_RESERVE_FCS (1<<0)
+#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
+
+#define MAXTID	16
+
+#define IEEE_A            (1<<0)
+#define IEEE_B            (1<<1)
+#define IEEE_G            (1<<2)
+#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
+
+/* Action category code */
+enum {
+	RTW_WLAN_CATEGORY_SPECTRUM_MGMT = 0,
+	RTW_WLAN_CATEGORY_QOS = 1,
+	RTW_WLAN_CATEGORY_DLS = 2,
+	RTW_WLAN_CATEGORY_BACK = 3,
+	RTW_WLAN_CATEGORY_PUBLIC = 4, /* IEEE 802.11 public action frames */
+	RTW_WLAN_CATEGORY_RADIO_MEASUREMENT  = 5,
+	RTW_WLAN_CATEGORY_FT = 6,
+	RTW_WLAN_CATEGORY_HT = 7,
+	RTW_WLAN_CATEGORY_SA_QUERY = 8,
+	RTW_WLAN_CATEGORY_UNPROTECTED_WNM = 11, /*  add for CONFIG_IEEE80211W, none 11w also can use */
+	RTW_WLAN_CATEGORY_TDLS = 12,
+	RTW_WLAN_CATEGORY_SELF_PROTECTED = 15, /*  add for CONFIG_IEEE80211W, none 11w also can use */
+	RTW_WLAN_CATEGORY_WMM = 17,
+	RTW_WLAN_CATEGORY_P2P = 0x7f,/* P2P action frames */
+};
+
+enum {
+	ACT_PUBLIC_BSSCOEXIST = 0, /*  20/40 BSS Coexistence */
+	ACT_PUBLIC_DSE_ENABLE = 1,
+	ACT_PUBLIC_DSE_DEENABLE = 2,
+	ACT_PUBLIC_DSE_REG_LOCATION = 3,
+	ACT_PUBLIC_EXT_CHL_SWITCH = 4,
+	ACT_PUBLIC_DSE_MSR_REQ = 5,
+	ACT_PUBLIC_DSE_MSR_RPRT = 6,
+	ACT_PUBLIC_MP = 7, /*  Measurement Pilot */
+	ACT_PUBLIC_DSE_PWR_CONSTRAINT = 8,
+	ACT_PUBLIC_VENDOR = 9, /*  for WIFI_DIRECT */
+	ACT_PUBLIC_GAS_INITIAL_REQ = 10,
+	ACT_PUBLIC_GAS_INITIAL_RSP = 11,
+	ACT_PUBLIC_GAS_COMEBACK_REQ = 12,
+	ACT_PUBLIC_GAS_COMEBACK_RSP = 13,
+	ACT_PUBLIC_TDLS_DISCOVERY_RSP = 14,
+	ACT_PUBLIC_LOCATION_TRACK = 15,
+	ACT_PUBLIC_MAX
+};
+
+#define OUI_MICROSOFT 0x0050f2 /* Microsoft (also used in Wi-Fi specs)
+				* 00:50:F2 */
+#define WME_OUI_TYPE 2
+#define WME_OUI_SUBTYPE_INFORMATION_ELEMENT 0
+#define WME_OUI_SUBTYPE_PARAMETER_ELEMENT 1
+#define WME_OUI_SUBTYPE_TSPEC_ELEMENT 2
+#define WME_VERSION 1
+
+#define WME_ACTION_CODE_SETUP_REQUEST 0
+#define WME_ACTION_CODE_SETUP_RESPONSE 1
+#define WME_ACTION_CODE_TEARDOWN 2
+
+#define WME_SETUP_RESPONSE_STATUS_ADMISSION_ACCEPTED 0
+#define WME_SETUP_RESPONSE_STATUS_INVALID_PARAMETERS 1
+#define WME_SETUP_RESPONSE_STATUS_REFUSED 3
+
+#define WME_TSPEC_DIRECTION_UPLINK 0
+#define WME_TSPEC_DIRECTION_DOWNLINK 1
+#define WME_TSPEC_DIRECTION_BI_DIRECTIONAL 3
+
+
+#define OUI_BROADCOM 0x00904c /* Broadcom (Epigram) */
+
+#define VENDOR_HT_CAPAB_OUI_TYPE 0x33 /* 00-90-4c:0x33 */
+
+/**
+ * enum rtw_ieee80211_channel_flags - channel flags
+ *
+ * Channel flags set by the regulatory control code.
+ *
+ * @RTW_IEEE80211_CHAN_DISABLED: This channel is disabled.
+ * @RTW_IEEE80211_CHAN_PASSIVE_SCAN: Only passive scanning is permitted
+ *      on this channel.
+ * @RTW_IEEE80211_CHAN_NO_IBSS: IBSS is not allowed on this channel.
+ * @RTW_IEEE80211_CHAN_RADAR: Radar detection is required on this channel.
+ * @RTW_IEEE80211_CHAN_NO_HT40PLUS: extension channel above this channel
+ *      is not permitted.
+ * @RTW_IEEE80211_CHAN_NO_HT40MINUS: extension channel below this channel
+ *      is not permitted.
+ */
+  enum rtw_ieee80211_channel_flags {
+          RTW_IEEE80211_CHAN_DISABLED         = 1<<0,
+          RTW_IEEE80211_CHAN_PASSIVE_SCAN     = 1<<1,
+          RTW_IEEE80211_CHAN_NO_IBSS          = 1<<2,
+          RTW_IEEE80211_CHAN_RADAR            = 1<<3,
+          RTW_IEEE80211_CHAN_NO_HT40PLUS      = 1<<4,
+          RTW_IEEE80211_CHAN_NO_HT40MINUS     = 1<<5,
+  };
+
+  #define RTW_IEEE80211_CHAN_NO_HT40 \
+          (RTW_IEEE80211_CHAN_NO_HT40PLUS | RTW_IEEE80211_CHAN_NO_HT40MINUS)
+
+/* Represent channel details, subset of ieee80211_channel */
+struct rtw_ieee80211_channel {
+	/* enum nl80211_band band; */
+	/* u16 center_freq; */
+	u16 hw_value;
+	u32 flags;
+	/* int max_antenna_gain; */
+	/* int max_power; */
+	/* int max_reg_power; */
+	/* bool beacon_found; */
+	/* u32 orig_flags; */
+	/* int orig_mag; */
+	/* int orig_mpwr; */
+};
+
+#define CHAN_FMT \
+	/*"band:%d, "*/ \
+	/*"center_freq:%u, "*/ \
+	"hw_value:%u, " \
+	"flags:0x%08x" \
+	/*"max_antenna_gain:%d\n"*/ \
+	/*"max_power:%d\n"*/ \
+	/*"max_reg_power:%d\n"*/ \
+	/*"beacon_found:%u\n"*/ \
+	/*"orig_flags:0x%08x\n"*/ \
+	/*"orig_mag:%d\n"*/ \
+	/*"orig_mpwr:%d\n"*/
+
+#define CHAN_ARG(channel) \
+	/*(channel)->band*/ \
+	/*, (channel)->center_freq*/ \
+	(channel)->hw_value \
+	, (channel)->flags \
+	/*, (channel)->max_antenna_gain*/ \
+	/*, (channel)->max_power*/ \
+	/*, (channel)->max_reg_power*/ \
+	/*, (channel)->beacon_found*/ \
+	/*, (channel)->orig_flags*/ \
+	/*, (channel)->orig_mag*/ \
+	/*, (channel)->orig_mpwr*/ \
+
+/* Parsed Information Elements */
+struct rtw_ieee802_11_elems {
+	u8 *ssid;
+	u8 ssid_len;
+	u8 *supp_rates;
+	u8 supp_rates_len;
+	u8 *fh_params;
+	u8 fh_params_len;
+	u8 *ds_params;
+	u8 ds_params_len;
+	u8 *cf_params;
+	u8 cf_params_len;
+	u8 *tim;
+	u8 tim_len;
+	u8 *ibss_params;
+	u8 ibss_params_len;
+	u8 *challenge;
+	u8 challenge_len;
+	u8 *erp_info;
+	u8 erp_info_len;
+	u8 *ext_supp_rates;
+	u8 ext_supp_rates_len;
+	u8 *wpa_ie;
+	u8 wpa_ie_len;
+	u8 *rsn_ie;
+	u8 rsn_ie_len;
+	u8 *wme;
+	u8 wme_len;
+	u8 *wme_tspec;
+	u8 wme_tspec_len;
+	u8 *wps_ie;
+	u8 wps_ie_len;
+	u8 *power_cap;
+	u8 power_cap_len;
+	u8 *supp_channels;
+	u8 supp_channels_len;
+	u8 *mdie;
+	u8 mdie_len;
+	u8 *ftie;
+	u8 ftie_len;
+	u8 *timeout_int;
+	u8 timeout_int_len;
+	u8 *ht_capabilities;
+	u8 ht_capabilities_len;
+	u8 *ht_operation;
+	u8 ht_operation_len;
+	u8 *vendor_ht_cap;
+	u8 vendor_ht_cap_len;
+	u8 *vht_capabilities;
+	u8 vht_capabilities_len;
+	u8 *vht_operation;
+	u8 vht_operation_len;
+	u8 *vht_op_mode_notify;
+	u8 vht_op_mode_notify_len;
+};
+
+enum ParseRes { ParseOK = 0, ParseUnknown = 1, ParseFailed = -1 };
+
+enum ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				struct rtw_ieee802_11_elems *elems,
+				int show_errors);
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source, unsigned int *frlen);
+u8 *rtw_set_ie(u8 *pbuf, signed int index, uint len, u8 *source, uint *frlen);
+
+u8 *rtw_get_ie(u8 *pbuf, signed int index, signed int *len, signed int limit);
+u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen);
+int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len);
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode);
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit);
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit);
+int rtw_get_wpa_cipher_suite(u8 *s);
+int rtw_get_wpa2_cipher_suite(u8 *s);
+int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len);
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
+int rtw_parse_wpa2_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
+
+void rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len);
+
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen);
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr);
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content);
+
+/**
+ * for_each_ie - iterate over continuous IEs
+ * @ie:
+ * @buf:
+ * @buf_len:
+ */
+#define for_each_ie(ie, buf, buf_len) \
+	for (ie = (void *)buf; (((u8 *)ie) - ((u8 *)buf) + 1) < buf_len; \
+		ie = (void *)(((u8 *)ie) + *(((u8 *)ie) + 1) + 2))
+
+uint	rtw_get_rateset_len(u8 *rateset);
+
+struct registry_priv;
+int rtw_generate_ie(struct registry_priv *pregistrypriv);
+
+
+int rtw_get_bit_value_from_ieee_value(u8 val);
+
+bool rtw_is_cckrates_included(u8 *rate);
+
+bool rtw_is_cckratesonly_included(u8 *rate);
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel);
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork);
+
+void rtw_macaddr_cfg(struct device *dev, u8 *mac_addr);
+
+u16 rtw_mcs_rate(u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate);
+
+int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action);
+const char *action_public_str(u8 action);
+
+#endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8723bs/include/ioctl_cfg80211.h b/drivers/staging/rtl8723bs/include/ioctl_cfg80211.h
new file mode 100644
index 000000000..993a7b3c3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/ioctl_cfg80211.h
@@ -0,0 +1,117 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __IOCTL_CFG80211_H__
+#define __IOCTL_CFG80211_H__
+
+struct rtw_wdev_invit_info {
+	u8 state; /* 0: req, 1:rep */
+	u8 peer_mac[ETH_ALEN];
+	u8 active;
+	u8 token;
+	u8 flags;
+	u8 status;
+	u8 req_op_ch;
+	u8 rsp_op_ch;
+};
+
+#define rtw_wdev_invit_info_init(invit_info) \
+	do { \
+		(invit_info)->state = 0xff; \
+		memset((invit_info)->peer_mac, 0, ETH_ALEN); \
+		(invit_info)->active = 0xff; \
+		(invit_info)->token = 0; \
+		(invit_info)->flags = 0x00; \
+		(invit_info)->status = 0xff; \
+		(invit_info)->req_op_ch = 0; \
+		(invit_info)->rsp_op_ch = 0; \
+	} while (0)
+
+struct rtw_wdev_nego_info {
+	u8 state; /* 0: req, 1:rep, 2:conf */
+	u8 peer_mac[ETH_ALEN];
+	u8 active;
+	u8 token;
+	u8 status;
+	u8 req_intent;
+	u8 req_op_ch;
+	u8 req_listen_ch;
+	u8 rsp_intent;
+	u8 rsp_op_ch;
+	u8 conf_op_ch;
+};
+
+#define rtw_wdev_nego_info_init(nego_info) \
+	do { \
+		(nego_info)->state = 0xff; \
+		memset((nego_info)->peer_mac, 0, ETH_ALEN); \
+		(nego_info)->active = 0xff; \
+		(nego_info)->token = 0; \
+		(nego_info)->status = 0xff; \
+		(nego_info)->req_intent = 0xff; \
+		(nego_info)->req_op_ch = 0; \
+		(nego_info)->req_listen_ch = 0; \
+		(nego_info)->rsp_intent = 0xff; \
+		(nego_info)->rsp_op_ch = 0; \
+		(nego_info)->conf_op_ch = 0; \
+	} while (0)
+
+struct rtw_wdev_priv {
+	struct wireless_dev *rtw_wdev;
+
+	struct adapter *padapter;
+
+	struct cfg80211_scan_request *scan_request;
+	spinlock_t scan_req_lock;
+
+	struct net_device *pmon_ndev;/* for monitor interface */
+	char ifname_mon[IFNAMSIZ + 1]; /* interface name for monitor interface */
+
+	u8 p2p_enabled;
+
+	u8 provdisc_req_issued;
+
+	struct rtw_wdev_invit_info invit_info;
+	struct rtw_wdev_nego_info nego_info;
+
+	u8 bandroid_scan;
+	bool block;
+	bool power_mgmt;
+};
+
+#define wiphy_to_adapter(x) (*((struct adapter **)wiphy_priv(x)))
+
+#define wdev_to_ndev(w) ((w)->netdev)
+
+int rtw_wdev_alloc(struct adapter *padapter, struct device *dev);
+void rtw_wdev_free(struct wireless_dev *wdev);
+void rtw_wdev_unregister(struct wireless_dev *wdev);
+
+void rtw_cfg80211_init_wiphy(struct adapter *padapter);
+
+void rtw_cfg80211_unlink_bss(struct adapter *padapter, struct wlan_network *pnetwork);
+void rtw_cfg80211_surveydone_event_callback(struct adapter *padapter);
+struct cfg80211_bss *rtw_cfg80211_inform_bss(struct adapter *padapter, struct wlan_network *pnetwork);
+int rtw_cfg80211_check_bss(struct adapter *padapter);
+void rtw_cfg80211_ibss_indicate_connect(struct adapter *padapter);
+void rtw_cfg80211_indicate_connect(struct adapter *padapter);
+void rtw_cfg80211_indicate_disconnect(struct adapter *padapter);
+void rtw_cfg80211_indicate_scan_done(struct adapter *adapter, bool aborted);
+
+void rtw_cfg80211_indicate_sta_assoc(struct adapter *padapter, u8 *pmgmt_frame, uint frame_len);
+void rtw_cfg80211_indicate_sta_disassoc(struct adapter *padapter, unsigned char *da, unsigned short reason);
+
+void rtw_cfg80211_rx_action(struct adapter *adapter, u8 *frame, uint frame_len, const char *msg);
+
+bool rtw_cfg80211_pwr_mgmt(struct adapter *adapter);
+
+#define rtw_cfg80211_rx_mgmt(adapter, freq, sig_dbm, buf, len, gfp) cfg80211_rx_mgmt((adapter)->rtw_wdev, freq, sig_dbm, buf, len, 0)
+#define rtw_cfg80211_send_rx_assoc(adapter, bss, buf, len) cfg80211_send_rx_assoc((adapter)->pnetdev, bss, buf, len)
+#define rtw_cfg80211_mgmt_tx_status(adapter, cookie, buf, len, ack, gfp) cfg80211_mgmt_tx_status((adapter)->rtw_wdev, cookie, buf, len, ack, gfp)
+#define rtw_cfg80211_ready_on_channel(adapter, cookie, chan, channel_type, duration, gfp)  cfg80211_ready_on_channel((adapter)->rtw_wdev, cookie, chan, duration, gfp)
+#define rtw_cfg80211_remain_on_channel_expired(adapter, cookie, chan, chan_type, gfp) cfg80211_remain_on_channel_expired((adapter)->rtw_wdev, cookie, chan, gfp)
+
+#endif /* __IOCTL_CFG80211_H__ */
diff --git a/drivers/staging/rtl8723bs/include/mlme_osdep.h b/drivers/staging/rtl8723bs/include/mlme_osdep.h
new file mode 100644
index 000000000..f0d19637f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/mlme_osdep.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef	__MLME_OSDEP_H_
+#define __MLME_OSDEP_H_
+
+
+extern void rtw_init_mlme_timer(struct adapter *padapter);
+extern void rtw_os_indicate_disconnect(struct adapter *adapter);
+extern void rtw_os_indicate_connect(struct adapter *adapter);
+void rtw_os_indicate_scan_done(struct adapter *padapter, bool aborted);
+extern void rtw_report_sec_ie(struct adapter *adapter, u8 authmode, u8 *sec_ie);
+
+void rtw_reset_securitypriv(struct adapter *adapter);
+
+#endif	/* _MLME_OSDEP_H_ */
diff --git a/drivers/staging/rtl8723bs/include/osdep_intf.h b/drivers/staging/rtl8723bs/include/osdep_intf.h
new file mode 100644
index 000000000..111e01797
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_intf.h
@@ -0,0 +1,74 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef __OSDEP_INTF_H_
+#define __OSDEP_INTF_H_
+
+
+struct intf_priv {
+
+	u8 *intf_dev;
+	u32 max_iosz;	/* USB2.0: 128, USB1.1: 64, SDIO:64 */
+	u32 max_xmitsz; /* USB2.0: unlimited, SDIO:512 */
+	u32 max_recvsz; /* USB2.0: unlimited, SDIO:512 */
+
+	volatile u8 *io_rwmem;
+	volatile u8 *allocated_io_rwmem;
+	u32 io_wsz; /* unit: 4bytes */
+	u32 io_rsz;/* unit: 4bytes */
+	u8 intf_status;
+
+	void (*_bus_io)(u8 *priv);
+
+/*
+Under Sync. IRP (SDIO/USB)
+A protection mechanism is necessary for the io_rwmem(read/write protocol)
+
+Under Async. IRP (SDIO/USB)
+The protection mechanism is through the pending queue.
+*/
+
+	struct mutex ioctl_mutex;
+};
+
+struct dvobj_priv *devobj_init(void);
+void devobj_deinit(struct dvobj_priv *pdvobj);
+
+u8 rtw_init_drv_sw(struct adapter *padapter);
+u8 rtw_free_drv_sw(struct adapter *padapter);
+void rtw_reset_drv_sw(struct adapter *padapter);
+void rtw_dev_unload(struct adapter *padapter);
+
+u32 rtw_start_drv_threads(struct adapter *padapter);
+void rtw_stop_drv_threads(struct adapter *padapter);
+void rtw_cancel_all_timer(struct adapter *padapter);
+
+int rtw_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+
+int rtw_init_netdev_name(struct net_device *pnetdev, const char *ifname);
+struct net_device *rtw_init_netdev(struct adapter *padapter);
+void rtw_unregister_netdevs(struct dvobj_priv *dvobj);
+
+u16 rtw_recv_select_queue(struct sk_buff *skb);
+
+int rtw_ndev_notifier_register(void);
+void rtw_ndev_notifier_unregister(void);
+
+void rtw_ips_dev_unload(struct adapter *padapter);
+
+int rtw_ips_pwr_up(struct adapter *padapter);
+void rtw_ips_pwr_down(struct adapter *padapter);
+
+int rtw_drv_register_netdev(struct adapter *padapter);
+void rtw_ndev_destructor(struct net_device *ndev);
+
+void rtw_suspend_common(struct adapter *padapter);
+int rtw_resume_common(struct adapter *padapter);
+
+int netdev_open(struct net_device *pnetdev);
+
+#endif	/* _OSDEP_INTF_H_ */
diff --git a/drivers/staging/rtl8723bs/include/osdep_service.h b/drivers/staging/rtl8723bs/include/osdep_service.h
new file mode 100644
index 000000000..cf96b5f7a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_service.h
@@ -0,0 +1,131 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __OSDEP_SERVICE_H_
+#define __OSDEP_SERVICE_H_
+
+
+#define _FAIL		0
+#define _SUCCESS	1
+#define RTW_RX_HANDLED 2
+
+#include <osdep_service_linux.h>
+
+#define BIT0	0x00000001
+#define BIT1	0x00000002
+#define BIT2	0x00000004
+#define BIT3	0x00000008
+#define BIT4	0x00000010
+#define BIT5	0x00000020
+#define BIT6	0x00000040
+#define BIT7	0x00000080
+#define BIT8	0x00000100
+#define BIT9	0x00000200
+#define BIT10	0x00000400
+#define BIT11	0x00000800
+#define BIT12	0x00001000
+#define BIT13	0x00002000
+#define BIT14	0x00004000
+#define BIT15	0x00008000
+#define BIT16	0x00010000
+#define BIT17	0x00020000
+#define BIT18	0x00040000
+#define BIT19	0x00080000
+#define BIT20	0x00100000
+#define BIT21	0x00200000
+#define BIT22	0x00400000
+#define BIT23	0x00800000
+#define BIT24	0x01000000
+#define BIT25	0x02000000
+#define BIT26	0x04000000
+#define BIT27	0x08000000
+#define BIT28	0x10000000
+#define BIT29	0x20000000
+#define BIT30	0x40000000
+#define BIT31	0x80000000
+#define BIT32	0x0100000000
+#define BIT33	0x0200000000
+#define BIT34	0x0400000000
+#define BIT35	0x0800000000
+#define BIT36	0x1000000000
+
+extern int RTW_STATUS_CODE(int error_code);
+
+void *_rtw_zmalloc(u32 sz);
+void *_rtw_malloc(u32 sz);
+void _kfree(u8 *pbuf, u32 sz);
+
+struct sk_buff *_rtw_skb_alloc(u32 sz);
+struct sk_buff *_rtw_skb_copy(const struct sk_buff *skb);
+int _rtw_netif_rx(struct net_device *ndev, struct sk_buff *skb);
+
+#define rtw_malloc(sz)			_rtw_malloc((sz))
+#define rtw_zmalloc(sz)			_rtw_zmalloc((sz))
+
+#define rtw_skb_alloc(size) _rtw_skb_alloc((size))
+#define rtw_skb_alloc_f(size, mstat_f)	_rtw_skb_alloc((size))
+#define rtw_skb_copy(skb)	_rtw_skb_copy((skb))
+#define rtw_skb_copy_f(skb, mstat_f)	_rtw_skb_copy((skb))
+#define rtw_netif_rx(ndev, skb) _rtw_netif_rx(ndev, skb)
+
+extern void _rtw_init_queue(struct __queue	*pqueue);
+
+static inline void thread_enter(char *name)
+{
+	allow_signal(SIGTERM);
+}
+
+static inline void flush_signals_thread(void)
+{
+	if (signal_pending(current))
+	{
+		flush_signals(current);
+	}
+}
+
+#define rtw_warn_on(condition) WARN_ON(condition)
+
+static inline int rtw_bug_check(void *parg1, void *parg2, void *parg3, void *parg4)
+{
+	int ret = true;
+
+	return ret;
+
+}
+
+#define _RND(sz, r) ((((sz)+((r)-1))/(r))*(r))
+
+#ifndef MAC_ARG
+#define MAC_ARG(x) (x)
+#endif
+
+extern void rtw_free_netdev(struct net_device * netdev);
+
+/* Macros for handling unaligned memory accesses */
+
+void rtw_buf_free(u8 **buf, u32 *buf_len);
+void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len);
+
+struct rtw_cbuf {
+	u32 write;
+	u32 read;
+	u32 size;
+	void *bufs[];
+};
+
+bool rtw_cbuf_full(struct rtw_cbuf *cbuf);
+bool rtw_cbuf_empty(struct rtw_cbuf *cbuf);
+bool rtw_cbuf_push(struct rtw_cbuf *cbuf, void *buf);
+void *rtw_cbuf_pop(struct rtw_cbuf *cbuf);
+struct rtw_cbuf *rtw_cbuf_alloc(u32 size);
+
+/*  String handler */
+/*
+ * Write formatted output to sized buffer
+ */
+#define rtw_sprintf(buf, size, format, arg...)	snprintf(buf, size, format, ##arg)
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/osdep_service_linux.h b/drivers/staging/rtl8723bs/include/osdep_service_linux.h
new file mode 100644
index 000000000..188ed7e26
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_service_linux.h
@@ -0,0 +1,123 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __OSDEP_LINUX_SERVICE_H_
+#define __OSDEP_LINUX_SERVICE_H_
+
+	#include <linux/spinlock.h>
+	#include <linux/compiler.h>
+	#include <linux/kernel.h>
+	#include <linux/errno.h>
+	#include <linux/init.h>
+	#include <linux/slab.h>
+	#include <linux/module.h>
+	#include <linux/kref.h>
+	/* include <linux/smp_lock.h> */
+	#include <linux/netdevice.h>
+	#include <linux/skbuff.h>
+	#include <linux/uaccess.h>
+	#include <asm/byteorder.h>
+	#include <linux/atomic.h>
+	#include <linux/io.h>
+	#include <linux/sem.h>
+	#include <linux/sched.h>
+	#include <linux/etherdevice.h>
+	#include <linux/wireless.h>
+	#include <net/iw_handler.h>
+	#include <linux/if_arp.h>
+	#include <linux/rtnetlink.h>
+	#include <linux/delay.h>
+	#include <linux/interrupt.h>	/*  for struct tasklet_struct */
+	#include <linux/ip.h>
+	#include <linux/kthread.h>
+	#include <linux/list.h>
+	#include <linux/vmalloc.h>
+
+/* 	#include <linux/ieee80211.h> */
+        #include <net/ieee80211_radiotap.h>
+	#include <net/cfg80211.h>
+
+	struct	__queue	{
+		struct	list_head	queue;
+		spinlock_t	lock;
+	};
+
+static inline struct list_head *get_next(struct list_head	*list)
+{
+	return list->next;
+}
+
+static inline struct list_head	*get_list_head(struct __queue	*queue)
+{
+	return (&(queue->queue));
+}
+
+static inline void _set_timer(struct timer_list *ptimer, u32 delay_time)
+{
+	mod_timer(ptimer, (jiffies + (delay_time * HZ / 1000)));
+}
+
+static inline void _init_workitem(struct work_struct *pwork, void *pfunc, void *cntx)
+{
+	INIT_WORK(pwork, pfunc);
+}
+
+static inline void _set_workitem(struct work_struct *pwork)
+{
+	schedule_work(pwork);
+}
+
+static inline void _cancel_workitem_sync(struct work_struct *pwork)
+{
+	cancel_work_sync(pwork);
+}
+
+static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
+{
+	return (netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 0)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 1)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 2)) &&
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3)));
+}
+
+static inline void rtw_netif_wake_queue(struct net_device *pnetdev)
+{
+	netif_tx_wake_all_queues(pnetdev);
+}
+
+static inline void rtw_netif_start_queue(struct net_device *pnetdev)
+{
+	netif_tx_start_all_queues(pnetdev);
+}
+
+static inline void rtw_netif_stop_queue(struct net_device *pnetdev)
+{
+	netif_tx_stop_all_queues(pnetdev);
+}
+
+#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)), (sig), 1)
+
+#define NDEV_ARG(ndev) ndev->name
+#define ADPT_ARG(adapter) adapter->pnetdev->name
+#define FUNC_NDEV_FMT "%s(%s)"
+#define FUNC_NDEV_ARG(ndev) __func__, ndev->name
+#define FUNC_ADPT_FMT "%s(%s)"
+#define FUNC_ADPT_ARG(adapter) __func__, adapter->pnetdev->name
+
+struct rtw_netdev_priv_indicator {
+	void *priv;
+	u32 sizeof_priv;
+};
+
+static inline struct adapter *rtw_netdev_priv(struct net_device *netdev)
+{
+	return ((struct rtw_netdev_priv_indicator *)netdev_priv(netdev))->priv;
+}
+
+struct net_device *rtw_alloc_etherdev_with_old_priv(int sizeof_priv, void *old_priv);
+extern struct net_device *rtw_alloc_etherdev(int sizeof_priv);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/recv_osdep.h b/drivers/staging/rtl8723bs/include/recv_osdep.h
new file mode 100644
index 000000000..83330ea98
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/recv_osdep.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RECV_OSDEP_H_
+#define __RECV_OSDEP_H_
+
+
+extern signed int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter);
+extern void _rtw_free_recv_priv(struct recv_priv *precvpriv);
+
+
+extern s32  rtw_recv_entry(union recv_frame *precv_frame);
+extern int rtw_recv_indicatepkt(struct adapter *adapter, union recv_frame *precv_frame);
+extern void rtw_recv_returnpacket(struct net_device *cnxt, struct sk_buff *preturnedpkt);
+
+extern void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup);
+
+int	rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter);
+void rtw_free_recv_priv(struct recv_priv *precvpriv);
+
+
+void rtw_os_recv_resource_alloc(struct adapter *padapter, union recv_frame *precvframe);
+void rtw_os_recv_resource_free(struct recv_priv *precvpriv);
+
+
+void rtw_os_free_recvframe(union recv_frame *precvframe);
+
+
+void rtw_os_recvbuf_resource_free(struct adapter *padapter, struct recv_buf *precvbuf);
+
+struct sk_buff *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata);
+void rtw_os_recv_indicate_pkt(struct adapter *padapter, struct sk_buff *pkt, struct rx_pkt_attrib *pattrib);
+
+void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl);
+
+
+#endif /*  */
diff --git a/drivers/staging/rtl8723bs/include/rtl8192c_recv.h b/drivers/staging/rtl8723bs/include/rtl8192c_recv.h
new file mode 100644
index 000000000..9664758e2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8192c_recv.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTL8192C_RECV_H_
+#define _RTL8192C_RECV_H_
+
+#define RECV_BLK_SZ 512
+#define RECV_BLK_CNT 16
+#define RECV_BLK_TH RECV_BLK_CNT
+
+#define MAX_RECVBUF_SZ (10240)
+
+struct phy_stat {
+	unsigned int phydw0;
+
+	unsigned int phydw1;
+
+	unsigned int phydw2;
+
+	unsigned int phydw3;
+
+	unsigned int phydw4;
+
+	unsigned int phydw5;
+
+	unsigned int phydw6;
+
+	unsigned int phydw7;
+};
+
+/*  Rx smooth factor */
+#define	Rx_Smooth_Factor (20)
+
+
+void rtl8192c_translate_rx_signal_stuff(union recv_frame *precvframe, struct phy_stat *pphy_status);
+void rtl8192c_query_rx_desc_status(union recv_frame *precvframe, struct recv_stat *pdesc);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h b/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h
new file mode 100644
index 000000000..dbcf01bbf
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h
@@ -0,0 +1,185 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_CMD_H__
+#define __RTL8723B_CMD_H__
+
+/*  */
+/*     H2C CMD DEFINITION    ------------------------------------------------ */
+/*  */
+
+enum {
+	/* Common Class: 000 */
+	H2C_8723B_RSVD_PAGE = 0x00,
+	H2C_8723B_MEDIA_STATUS_RPT = 0x01,
+	H2C_8723B_SCAN_ENABLE = 0x02,
+	H2C_8723B_KEEP_ALIVE = 0x03,
+	H2C_8723B_DISCON_DECISION = 0x04,
+	H2C_8723B_PSD_OFFLOAD = 0x05,
+	H2C_8723B_AP_OFFLOAD = 0x08,
+	H2C_8723B_BCN_RSVDPAGE = 0x09,
+	H2C_8723B_PROBERSP_RSVDPAGE = 0x0A,
+	H2C_8723B_FCS_RSVDPAGE = 0x10,
+	H2C_8723B_FCS_INFO = 0x11,
+	H2C_8723B_AP_WOW_GPIO_CTRL = 0x13,
+
+	/* PoweSave Class: 001 */
+	H2C_8723B_SET_PWR_MODE = 0x20,
+	H2C_8723B_PS_TUNING_PARA = 0x21,
+	H2C_8723B_PS_TUNING_PARA2 = 0x22,
+	H2C_8723B_P2P_LPS_PARAM = 0x23,
+	H2C_8723B_P2P_PS_OFFLOAD = 0x24,
+	H2C_8723B_PS_SCAN_ENABLE = 0x25,
+	H2C_8723B_SAP_PS_ = 0x26,
+	H2C_8723B_INACTIVE_PS_ = 0x27, /* Inactive_PS */
+	H2C_8723B_FWLPS_IN_IPS_ = 0x28,
+
+	/* Dynamic Mechanism Class: 010 */
+	H2C_8723B_MACID_CFG = 0x40,
+	H2C_8723B_TXBF = 0x41,
+	H2C_8723B_RSSI_SETTING = 0x42,
+	H2C_8723B_AP_REQ_TXRPT = 0x43,
+	H2C_8723B_INIT_RATE_COLLECT = 0x44,
+
+	/* BT Class: 011 */
+	H2C_8723B_B_TYPE_TDMA = 0x60,
+	H2C_8723B_BT_INFO = 0x61,
+	H2C_8723B_FORCE_BT_TXPWR = 0x62,
+	H2C_8723B_BT_IGNORE_WLANACT = 0x63,
+	H2C_8723B_DAC_SWING_VALUE = 0x64,
+	H2C_8723B_ANT_SEL_RSV = 0x65,
+	H2C_8723B_WL_OPMODE = 0x66,
+	H2C_8723B_BT_MP_OPER = 0x67,
+	H2C_8723B_BT_CONTROL = 0x68,
+	H2C_8723B_BT_WIFI_CTRL = 0x69,
+	H2C_8723B_BT_FW_PATCH = 0x6A,
+	H2C_8723B_BT_WLAN_CALIBRATION = 0x6D,
+
+	/* WOWLAN Class: 100 */
+	H2C_8723B_WOWLAN = 0x80,
+	H2C_8723B_REMOTE_WAKE_CTRL = 0x81,
+	H2C_8723B_AOAC_GLOBAL_INFO = 0x82,
+	H2C_8723B_AOAC_RSVD_PAGE = 0x83,
+	H2C_8723B_AOAC_RSVD_PAGE2 = 0x84,
+	H2C_8723B_D0_SCAN_OFFLOAD_CTRL = 0x85,
+	H2C_8723B_D0_SCAN_OFFLOAD_INFO = 0x86,
+	H2C_8723B_CHNL_SWITCH_OFFLOAD = 0x87,
+
+	H2C_8723B_RESET_TSF = 0xC0,
+	H2C_8723B_MAXID,
+};
+/*  */
+/*     H2C CMD CONTENT    -------------------------------------------------- */
+/*  */
+/* _RSVDPAGE_LOC_CMD_0x00 */
+#define SET_8723B_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_RSVDPAGE_LOC_PSPOLL(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_8723B_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_8723B_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_8723B_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+4, 0, 8, __Value)
+
+/* _MEDIA_STATUS_RPT_PARM_CMD_0x01 */
+#define SET_8723B_H2CCMD_MSRRPT_PARM_OPMODE(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_8723B_H2CCMD_MSRRPT_PARM_MACID_IND(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8723B_H2CCMD_MSRRPT_PARM_MACID(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+#define SET_8723B_H2CCMD_MSRRPT_PARM_MACID_END(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+2, 0, 8, __Value)
+
+/* _KEEP_ALIVE_CMD_0x03 */
+#define SET_8723B_H2CCMD_KEEPALIVE_PARM_ENABLE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_8723B_H2CCMD_KEEPALIVE_PARM_ADOPT(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8723B_H2CCMD_KEEPALIVE_PARM_PKT_TYPE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_8723B_H2CCMD_KEEPALIVE_PARM_CHECK_PERIOD(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+
+/* _DISCONNECT_DECISION_CMD_0x04 */
+#define SET_8723B_H2CCMD_DISCONDECISION_PARM_ENABLE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_8723B_H2CCMD_DISCONDECISION_PARM_ADOPT(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8723B_H2CCMD_DISCONDECISION_PARM_CHECK_PERIOD(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+1, 0, 8, __Value)
+#define SET_8723B_H2CCMD_DISCONDECISION_PARM_TRY_PKT_NUM(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd+2, 0, 8, __Value)
+
+/*  _PWR_MOD_CMD_0x20 */
+#define SET_8723B_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_RLBM(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 4, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_SMART_PS(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 4, 4, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_PWR_STATE(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+4, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PWRMODE_PARM_BYTE5(__pH2CCmd, __Value)				SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+5, 0, 8, __Value)
+
+#define GET_8723B_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd)					LE_BITS_TO_1BYTE(__pH2CCmd, 0, 8)
+
+/*  _PS_TUNE_PARAM_CMD_0x21 */
+#define SET_8723B_H2CCMD_PSTUNE_PARM_BCN_TO_LIMIT(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_TIMEOUT(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 8, __Value)
+#define SET_8723B_H2CCMD_PSTUNE_PARM_ADOPT(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 1, __Value)
+#define SET_8723B_H2CCMD_PSTUNE_PARM_PS_TIMEOUT(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 1, 7, __Value)
+#define SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_PERIOD(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd+3, 0, 8, __Value)
+
+/* _MACID_CFG_CMD_0x40 */
+#define SET_8723B_H2CCMD_MACID_CFG_MACID(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_RAID(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 5, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_SGI_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 7, 1, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_BW(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 2, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_NO_UPDATE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 3, 1, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_VHT_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 4, 2, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_DISPT(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 6, 1, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_DISRA(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 7, 1, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_RATE_MASK0(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+3, 0, 8, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_RATE_MASK1(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+4, 0, 8, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_RATE_MASK2(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+5, 0, 8, __Value)
+#define SET_8723B_H2CCMD_MACID_CFG_RATE_MASK3(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+6, 0, 8, __Value)
+
+/* _RSSI_SETTING_CMD_0x42 */
+#define SET_8723B_H2CCMD_RSSI_SETTING_MACID(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_RSSI_SETTING_RSSI(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 7, __Value)
+#define SET_8723B_H2CCMD_RSSI_SETTING_ULDL_STATE(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+3, 0, 8, __Value)
+
+/*  _AP_REQ_TXRPT_CMD_0x43 */
+#define SET_8723B_H2CCMD_APREQRPT_PARM_MACID1(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_8723B_H2CCMD_APREQRPT_PARM_MACID2(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 8, __Value)
+
+/*  _FORCE_BT_TXPWR_CMD_0x62 */
+#define SET_8723B_H2CCMD_BT_PWR_IDX(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+
+/*  _FORCE_BT_MP_OPER_CMD_0x67 */
+#define SET_8723B_H2CCMD_BT_MPOPER_VER(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 4, __Value)
+#define SET_8723B_H2CCMD_BT_MPOPER_REQNUM(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 4, __Value)
+#define SET_8723B_H2CCMD_BT_MPOPER_IDX(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_MPOPER_PARAM1(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_MPOPER_PARAM2(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd+3, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_MPOPER_PARAM3(__pH2CCmd, __Value)							SET_BITS_TO_LE_1BYTE(__pH2CCmd+4, 0, 8, __Value)
+
+/*  _BT_FW_PATCH_0x6A */
+#define SET_8723B_H2CCMD_BT_FW_PATCH_SIZE(__pH2CCmd, __Value)					SET_BITS_TO_LE_2BYTE((u8 *)(__pH2CCmd), 0, 16, __Value)
+#define SET_8723B_H2CCMD_BT_FW_PATCH_ADDR0(__pH2CCmd, __Value)					SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_FW_PATCH_ADDR1(__pH2CCmd, __Value)					SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_FW_PATCH_ADDR2(__pH2CCmd, __Value)					SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __Value)
+#define SET_8723B_H2CCMD_BT_FW_PATCH_ADDR3(__pH2CCmd, __Value)					SET_BITS_TO_LE_1BYTE((__pH2CCmd)+5, 0, 8, __Value)
+
+/*  */
+/*     Function Statement     -------------------------------------------------- */
+/*  */
+
+/*  host message to firmware cmd */
+void rtl8723b_set_FwPwrMode_cmd(struct adapter *padapter, u8 Mode);
+void rtl8723b_set_FwJoinBssRpt_cmd(struct adapter *padapter, u8 mstatus);
+void rtl8723b_set_rssi_cmd(struct adapter *padapter, u8 *param);
+void rtl8723b_Add_RateATid(struct adapter *padapter, u32 bitmap, u8 *arg, u8 rssi_level);
+void rtl8723b_fw_try_ap_cmd(struct adapter *padapter, u32 need_ack);
+/* s32 rtl8723b_set_lowpwr_lps_cmd(struct adapter *padapter, u8 enable); */
+void rtl8723b_set_FwPsTuneParam_cmd(struct adapter *padapter);
+void rtl8723b_set_FwMacIdConfig_cmd(struct adapter *padapter, u8 mac_id, u8 raid, u8 bw, u8 sgi, u32 mask);
+void rtl8723b_set_FwMediaStatusRpt_cmd(struct adapter *padapter, u8 mstatus, u8 macid);
+void rtl8723b_download_rsvd_page(struct adapter *padapter, u8 mstatus);
+void rtl8723b_download_BTCoex_AP_mode_rsvd_page(struct adapter *padapter);
+
+void CheckFwRsvdPageContent(struct adapter *padapter);
+
+void rtl8723b_set_FwPwrModeInIPS_cmd(struct adapter *padapter, u8 cmd_param);
+
+s32 FillH2CCmd8723B(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);
+
+#define FillH2CCmd FillH2CCmd8723B
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_dm.h b/drivers/staging/rtl8723bs/include/rtl8723b_dm.h
new file mode 100644
index 000000000..1d2da5286
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_dm.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_DM_H__
+#define __RTL8723B_DM_H__
+/*  */
+/*  Description: */
+/*  */
+/*  This file is for 8723B dynamic mechanism only */
+/*  */
+/*  */
+/*  */
+
+/*  */
+/*  structure and define */
+/*  */
+
+/*  */
+/*  function prototype */
+/*  */
+
+void rtl8723b_init_dm_priv(struct adapter *padapter);
+
+void rtl8723b_InitHalDm(struct adapter *padapter);
+void rtl8723b_HalDmWatchDog(struct adapter *padapter);
+void rtl8723b_HalDmWatchDog_in_LPS(struct adapter *padapter);
+void rtl8723b_hal_dm_in_lps(struct adapter *padapter);
+
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_hal.h b/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
new file mode 100644
index 000000000..c1d7249e3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
@@ -0,0 +1,254 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_HAL_H__
+#define __RTL8723B_HAL_H__
+
+#include "hal_data.h"
+
+#include "rtl8723b_spec.h"
+#include "rtl8723b_rf.h"
+#include "rtl8723b_dm.h"
+#include "rtl8723b_recv.h"
+#include "rtl8723b_xmit.h"
+#include "rtl8723b_cmd.h"
+#include "rtw_mp.h"
+#include "hal_pwr_seq.h"
+#include "hal_phy_reg_8723b.h"
+#include "hal_phy_cfg.h"
+
+/*  */
+/* RTL8723B From header */
+/*  */
+
+#define FW_8723B_SIZE          0x8000
+#define FW_8723B_START_ADDRESS 0x1000
+#define FW_8723B_END_ADDRESS   0x1FFF /* 0x5FFF */
+
+#define IS_FW_HEADER_EXIST_8723B(fw_hdr) \
+	((le16_to_cpu(fw_hdr->signature) & 0xFFF0) == 0x5300)
+
+struct rt_firmware {
+	u32 fw_length;
+	u8 *fw_buffer_sz;
+};
+
+/* This structure must be carefully byte-ordered. */
+struct rt_firmware_hdr {
+	/*  8-byte alinment required */
+
+	/*  LONG WORD 0 ---- */
+	__le16 signature;  /* 92C0: test chip; 92C, 88C0: test chip;
+			    * 88C1: MP A-cut; 92C1: MP A-cut
+			    */
+	u8 category;	   /* AP/NIC and USB/PCI */
+	u8 function;	   /* Reserved for different FW function indications,
+			    * for further use when driver needs to download
+			    * different FW in different conditions.
+			    */
+	__le16 version;    /* FW Version */
+	__le16 subversion; /* FW Subversion, default 0x00 */
+
+	/*  LONG WORD 1 ---- */
+	u8 month;  /* Release time Month field */
+	u8 date;   /* Release time Date field */
+	u8 hour;   /* Release time Hour field */
+	u8 minute; /* Release time Minute field */
+
+	__le16 ram_code_size; /* The size of RAM code */
+	__le16 rsvd2;
+
+	/*  LONG WORD 2 ---- */
+	__le32 svn_idx;	/* The SVN entry index */
+	__le32 rsvd3;
+
+	/*  LONG WORD 3 ---- */
+	__le32 rsvd4;
+	__le32 rsvd5;
+};
+
+#define DRIVER_EARLY_INT_TIME_8723B  0x05
+#define BCN_DMA_ATIME_INT_TIME_8723B 0x02
+
+/* for 8723B */
+/* TX 32K, RX 16K, Page size 128B for TX, 8B for RX */
+#define PAGE_SIZE_TX_8723B 128
+#define PAGE_SIZE_RX_8723B 8
+
+#define RX_DMA_SIZE_8723B          0x4000 /* 16K */
+#define RX_DMA_RESERVED_SIZE_8723B 0x80   /* 128B, reserved for tx report */
+#define RX_DMA_BOUNDARY_8723B \
+	(RX_DMA_SIZE_8723B - RX_DMA_RESERVED_SIZE_8723B - 1)
+
+/* Note: We will divide number of pages equally for each queue other than the
+ * public queue!
+ */
+
+/* For General Reserved Page Number(Beacon Queue is reserved page) */
+/* Beacon:2, PS-Poll:1, Null Data:1, Qos Null Data:1, BT Qos Null Data:1 */
+#define BCNQ_PAGE_NUM_8723B  0x08
+#define BCNQ1_PAGE_NUM_8723B 0x00
+
+#define MAX_RX_DMA_BUFFER_SIZE_8723B 0x2800 /* RX 10K */
+
+/* For WoWLan, more reserved page */
+/* ARP Rsp:1, RWC:1, GTK Info:1, GTK RSP:2, GTK EXT MEM:2, PNO: 6 */
+#define WOWLAN_PAGE_NUM_8723B 0x00
+
+#define TX_TOTAL_PAGE_NUMBER_8723B     \
+	(0xFF - BCNQ_PAGE_NUM_8723B  - \
+		BCNQ1_PAGE_NUM_8723B - \
+		WOWLAN_PAGE_NUM_8723B)
+#define TX_PAGE_BOUNDARY_8723B (TX_TOTAL_PAGE_NUMBER_8723B + 1)
+
+#define WMM_NORMAL_TX_TOTAL_PAGE_NUMBER_8723B TX_TOTAL_PAGE_NUMBER_8723B
+#define WMM_NORMAL_TX_PAGE_BOUNDARY_8723B \
+	(WMM_NORMAL_TX_TOTAL_PAGE_NUMBER_8723B + 1)
+
+/* For Normal Chip Setting */
+/* (HPQ + LPQ + NPQ + PUBQ) shall be TX_TOTAL_PAGE_NUMBER_8723B */
+#define NORMAL_PAGE_NUM_HPQ_8723B 0x0C
+#define NORMAL_PAGE_NUM_LPQ_8723B 0x02
+#define NORMAL_PAGE_NUM_NPQ_8723B 0x02
+
+/*  Note: For Normal Chip Setting, modify later */
+#define WMM_NORMAL_PAGE_NUM_HPQ_8723B 0x30
+#define WMM_NORMAL_PAGE_NUM_LPQ_8723B 0x20
+#define WMM_NORMAL_PAGE_NUM_NPQ_8723B 0x20
+
+#include "HalVerDef.h"
+#include "hal_com.h"
+
+#define EFUSE_OOB_PROTECT_BYTES 15
+
+#define HAL_EFUSE_MEMORY
+
+#define HWSET_MAX_SIZE_8723B         512
+#define EFUSE_REAL_CONTENT_LEN_8723B 512
+#define EFUSE_MAP_LEN_8723B          512
+#define EFUSE_MAX_SECTION_8723B      64
+
+#define EFUSE_IC_ID_OFFSET 506 /* For some inferiority IC purpose.
+				* Added by Roger, 2009.09.02.
+				*/
+#define AVAILABLE_EFUSE_ADDR(addr) (addr < EFUSE_REAL_CONTENT_LEN_8723B)
+
+#define EFUSE_ACCESS_ON  0x69 /* For RTL8723 only. */
+#define EFUSE_ACCESS_OFF 0x00 /* For RTL8723 only. */
+
+/*  */
+/* EFUSE for BT definition */
+/*  */
+#define EFUSE_BT_REAL_BANK_CONTENT_LEN 512
+#define EFUSE_BT_REAL_CONTENT_LEN      1536 /* 512*3 */
+#define EFUSE_BT_MAP_LEN               1024 /* 1k bytes */
+#define EFUSE_BT_MAX_SECTION           128  /* 1024/8 */
+
+#define EFUSE_PROTECT_BYTES_BANK 16
+
+/* Description: Determine the types of C2H events that are the same in driver
+ * and FW; First constructed by tynli. 2009.10.09.
+ */
+enum {
+	C2H_DBG = 0,
+	C2H_TSF = 1,
+	C2H_AP_RPT_RSP = 2,
+	C2H_CCX_TX_RPT = 3, /* The FW notify the report
+			     * of the specific tx packet.
+			     */
+	C2H_BT_RSSI = 4,
+	C2H_BT_OP_MODE = 5,
+	C2H_EXT_RA_RPT = 6,
+	C2H_8723B_BT_INFO = 9,
+	C2H_HW_INFO_EXCH = 10,
+	C2H_8723B_BT_MP_INFO = 11,
+	MAX_C2HEVENT
+};
+
+struct c2h_evt_hdr_t {
+	u8 CmdID;
+	u8 CmdLen;
+	u8 CmdSeq;
+} __attribute__((__packed__));
+
+enum { /* tag_Package_Definition */
+	PACKAGE_DEFAULT,
+	PACKAGE_QFN68,
+	PACKAGE_TFBGA90,
+	PACKAGE_TFBGA80,
+	PACKAGE_TFBGA79
+};
+
+#define INCLUDE_MULTI_FUNC_BT(_Adapter)  \
+	(GET_HAL_DATA(_Adapter)->MultiFunc & RT_MULTI_FUNC_BT)
+#define INCLUDE_MULTI_FUNC_GPS(_Adapter) \
+	(GET_HAL_DATA(_Adapter)->MultiFunc & RT_MULTI_FUNC_GPS)
+
+/*  rtl8723a_hal_init.c */
+s32 rtl8723b_FirmwareDownload(struct adapter *padapter, bool  bUsedWoWLANFw);
+void rtl8723b_FirmwareSelfReset(struct adapter *padapter);
+void rtl8723b_InitializeFirmwareVars(struct adapter *padapter);
+
+void rtl8723b_InitAntenna_Selection(struct adapter *padapter);
+void rtl8723b_init_default_value(struct adapter *padapter);
+
+s32 rtl8723b_InitLLTTable(struct adapter *padapter);
+
+/*  EFuse */
+u8 GetEEPROMSize8723B(struct adapter *padapter);
+void Hal_InitPGData(struct adapter *padapter, u8 *PROMContent);
+void Hal_EfuseParseIDCode(struct adapter *padapter, u8 *hwinfo);
+void Hal_EfuseParseTxPowerInfo_8723B(struct adapter *padapter, u8 *PROMContent,
+				     bool AutoLoadFail);
+void Hal_EfuseParseBTCoexistInfo_8723B(struct adapter *padapter, u8 *hwinfo,
+				       bool AutoLoadFail);
+void Hal_EfuseParseEEPROMVer_8723B(struct adapter *padapter, u8 *hwinfo,
+				   bool AutoLoadFail);
+void Hal_EfuseParseChnlPlan_8723B(struct adapter *padapter, u8 *hwinfo,
+				  bool AutoLoadFail);
+void Hal_EfuseParseCustomerID_8723B(struct adapter *padapter, u8 *hwinfo,
+				    bool AutoLoadFail);
+void Hal_EfuseParseAntennaDiversity_8723B(struct adapter *padapter, u8 *hwinfo,
+					  bool AutoLoadFail);
+void Hal_EfuseParseXtal_8723B(struct adapter *padapter, u8 *hwinfo,
+			      bool AutoLoadFail);
+void Hal_EfuseParseThermalMeter_8723B(struct adapter *padapter, u8 *hwinfo,
+				      u8 AutoLoadFail);
+void Hal_EfuseParsePackageType_8723B(struct adapter *padapter, u8 *hwinfo,
+				     bool AutoLoadFail);
+void Hal_EfuseParseVoltage_8723B(struct adapter *padapter, u8 *hwinfo,
+				 bool AutoLoadFail);
+
+void C2HPacketHandler_8723B(struct adapter *padapter, u8 *pbuffer, u16 length);
+
+void rtl8723b_set_hal_ops(struct hal_ops *pHalFunc);
+void SetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val);
+void GetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val);
+u8 SetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable,
+		     void *pval);
+u8 GetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable,
+		     void *pval);
+
+/*  register */
+void rtl8723b_InitBeaconParameters(struct adapter *padapter);
+void _InitBurstPktLen_8723BS(struct adapter *adapter);
+void _8051Reset8723(struct adapter *padapter);
+
+void rtl8723b_start_thread(struct adapter *padapter);
+void rtl8723b_stop_thread(struct adapter *padapter);
+
+int FirmwareDownloadBT(struct adapter *adapter, struct rt_firmware *firmware);
+
+void CCX_FwC2HTxRpt_8723b(struct adapter *padapter, u8 *pdata, u8 len);
+s32 c2h_id_filter_ccx_8723b(u8 *buf);
+s32 c2h_handler_8723b(struct adapter *padapter, u8 *pC2hEvent);
+u8 MRateToHwRate8723B(u8 rate);
+u8 HwRateToMRate8723B(u8 rate);
+
+void Hal_ReadRFGainOffset(struct adapter *padapter, u8 *hwinfo,
+			  bool AutoLoadFail);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_recv.h b/drivers/staging/rtl8723bs/include/rtl8723b_recv.h
new file mode 100644
index 000000000..a108ce89b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_recv.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_RECV_H__
+#define __RTL8723B_RECV_H__
+
+#include <rtl8192c_recv.h>
+
+struct rxreport_8723b {
+	/* DWORD 0 */
+	u32 pktlen:14;
+	u32 crc32:1;
+	u32 icverr:1;
+	u32 drvinfosize:4;
+	u32 security:3;
+	u32 qos:1;
+	u32 shift:2;
+	u32 physt:1;
+	u32 swdec:1;
+	u32 rsvd0028:2;
+	u32 eor:1;
+	u32 rsvd0031:1;
+
+	/* DWORD 1 */
+	u32 macid:7;
+	u32 rsvd0407:1;
+	u32 tid:4;
+	u32 macid_vld:1;
+	u32 amsdu:1;
+	u32 rxid_match:1;
+	u32 paggr:1;
+	u32 a1fit:4;
+	u32 chkerr:1;  /* 20 */
+	u32 rx_ipv:1;
+	u32 rx_is_tcp_udp:1;
+	u32 chk_vld:1;   /* 23 */
+	u32 pam:1;
+	u32 pwr:1;
+	u32 md:1;
+	u32 mf:1;
+	u32 type:2;
+	u32 mc:1;
+	u32 bc:1;
+
+	/* DWORD 2 */
+	u32 seq:12;
+	u32 frag:4;
+	u32 rx_is_qos:1;
+	u32 rsvd0817:1;
+	u32 wlanhd_iv_len:6;
+	u32 hwrsvd0824:4;
+	u32 c2h_ind:1;
+	u32 rsvd0829:2;
+	u32 fcs_ok:1;
+
+	/* DWORD 3 */
+	u32 rx_rate:7;
+	u32 rsvd1207:3;
+	u32 htc:1;
+	u32 esop:1;
+	u32 bssid_fit:2;
+	u32 rsvd1214:2;
+	u32 dma_agg_num:8;
+	u32 rsvd1224:5;
+	u32 patternmatch:1;
+	u32 unicastwake:1;
+	u32 magicwake:1;
+
+	/* DWORD 4 */
+	u32 splcp:1;	/* Ofdm sgi or cck_splcp */
+	u32 ldpc:1;
+	u32 stbc:1;
+	u32 not_sounding:1;
+	u32 bw:2;
+	u32 rsvd1606:26;
+
+	/* DWORD 5 */
+	u32 tsfl;
+};
+
+s32 rtl8723bs_init_recv_priv(struct adapter *padapter);
+void rtl8723bs_free_recv_priv(struct adapter *padapter);
+
+void rtl8723b_query_rx_phy_status(union recv_frame *prframe, struct phy_stat *pphy_stat);
+void rtl8723b_process_phy_info(struct adapter *padapter, void *prframe);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_rf.h b/drivers/staging/rtl8723bs/include/rtl8723b_rf.h
new file mode 100644
index 000000000..525eb2fac
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_rf.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_RF_H__
+#define __RTL8723B_RF_H__
+
+
+int	PHY_RF6052_Config8723B(struct adapter *Adapter);
+
+void
+PHY_RF6052SetBandwidth8723B(struct adapter *Adapter,
+	enum channel_width		Bandwidth);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_spec.h b/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
new file mode 100644
index 000000000..6816040a6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
@@ -0,0 +1,237 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ *******************************************************************************/
+#ifndef __RTL8723B_SPEC_H__
+#define __RTL8723B_SPEC_H__
+
+#define HAL_NAV_UPPER_UNIT_8723B		128		/*  micro-second */
+
+/*  */
+/*  */
+/*	0x0000h ~ 0x00FFh	System Configuration */
+/*  */
+/*  */
+#define REG_RSV_CTRL_8723B				0x001C	/*  3 Byte */
+#define REG_BT_WIFI_ANTENNA_SWITCH_8723B	0x0038
+#define REG_HSISR_8723B					0x005c
+#define REG_PAD_CTRL1_8723B		0x0064
+#define REG_AFE_CTRL_4_8723B		0x0078
+#define REG_HMEBOX_DBG_0_8723B	0x0088
+#define REG_HMEBOX_DBG_1_8723B	0x008A
+#define REG_HMEBOX_DBG_2_8723B	0x008C
+#define REG_HMEBOX_DBG_3_8723B	0x008E
+#define REG_HIMR0_8723B					0x00B0
+#define REG_HISR0_8723B					0x00B4
+#define REG_HIMR1_8723B					0x00B8
+#define REG_HISR1_8723B					0x00BC
+#define REG_PMC_DBG_CTRL2_8723B			0x00CC
+
+/*  */
+/*  */
+/*	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+#define REG_C2HEVT_CMD_ID_8723B	0x01A0
+#define REG_C2HEVT_CMD_LEN_8723B	0x01AE
+#define REG_WOWLAN_WAKE_REASON 0x01C7
+#define REG_WOWLAN_GTK_DBG1	0x630
+#define REG_WOWLAN_GTK_DBG2	0x634
+
+#define REG_HMEBOX_EXT0_8723B			0x01F0
+#define REG_HMEBOX_EXT1_8723B			0x01F4
+#define REG_HMEBOX_EXT2_8723B			0x01F8
+#define REG_HMEBOX_EXT3_8723B			0x01FC
+
+/*  */
+/*  */
+/*	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*  */
+/*  */
+#define REG_RXDMA_CONTROL_8723B		0x0286 /*  Control the RX DMA. */
+#define REG_RXDMA_MODE_CTRL_8723B		0x0290
+
+/*  */
+/*  */
+/*	0x0300h ~ 0x03FFh	PCIe */
+/*  */
+/*  */
+#define	REG_PCIE_CTRL_REG_8723B		0x0300
+#define	REG_INT_MIG_8723B				0x0304	/*  Interrupt Migration */
+#define	REG_BCNQ_DESA_8723B			0x0308	/*  TX Beacon Descriptor Address */
+#define	REG_HQ_DESA_8723B				0x0310	/*  TX High Queue Descriptor Address */
+#define	REG_MGQ_DESA_8723B			0x0318	/*  TX Manage Queue Descriptor Address */
+#define	REG_VOQ_DESA_8723B			0x0320	/*  TX VO Queue Descriptor Address */
+#define	REG_VIQ_DESA_8723B				0x0328	/*  TX VI Queue Descriptor Address */
+#define	REG_BEQ_DESA_8723B			0x0330	/*  TX BE Queue Descriptor Address */
+#define	REG_BKQ_DESA_8723B			0x0338	/*  TX BK Queue Descriptor Address */
+#define	REG_RX_DESA_8723B				0x0340	/*  RX Queue	Descriptor Address */
+#define	REG_DBI_WDATA_8723B			0x0348	/*  DBI Write Data */
+#define	REG_DBI_RDATA_8723B			0x034C	/*  DBI Read Data */
+#define	REG_DBI_ADDR_8723B				0x0350	/*  DBI Address */
+#define	REG_DBI_FLAG_8723B				0x0352	/*  DBI Read/Write Flag */
+#define	REG_MDIO_WDATA_8723B		0x0354	/*  MDIO for Write PCIE PHY */
+#define	REG_MDIO_RDATA_8723B			0x0356	/*  MDIO for Reads PCIE PHY */
+#define	REG_MDIO_CTL_8723B			0x0358	/*  MDIO for Control */
+#define	REG_DBG_SEL_8723B				0x0360	/*  Debug Selection Register */
+#define	REG_PCIE_HRPWM_8723B			0x0361	/* PCIe RPWM */
+#define	REG_PCIE_HCPWM_8723B			0x0363	/* PCIe CPWM */
+#define	REG_PCIE_MULTIFET_CTRL_8723B	0x036A	/* PCIE Multi-Fethc Control */
+
+/*  */
+/*  */
+/*	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+#define REG_TXPKTBUF_BCNQ_BDNY_8723B	0x0424
+#define REG_TXPKTBUF_MGQ_BDNY_8723B	0x0425
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD_8723B	0x045D
+#define REG_AMPDU_BURST_MODE_8723B	0x04BC
+
+/*  */
+/*  */
+/*	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+#define REG_SECONDARY_CCA_CTRL_8723B	0x0577
+
+/*  */
+/*  */
+/*	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*  */
+/*  */
+
+/*  */
+/*  SDIO Bus Specification */
+/*  */
+
+/*  */
+/*  SDIO CMD Address Mapping */
+/*  */
+
+/*  */
+/*  I/O bus domain (Host) */
+/*  */
+
+/*  */
+/*  SDIO register */
+/*  */
+#define SDIO_REG_HCPWM1_8723B	0x025 /*  HCI Current Power Mode 1 */
+
+/*  */
+/*	8723 Register Bit and Content definition */
+/*  */
+
+/* 2 HSISR */
+/*  interrupt mask which needs to clear */
+#define MASK_HSISR_CLEAR		(HSISR_GPIO12_0_INT |\
+								HSISR_SPS_OCP_INT |\
+								HSISR_RON_INT |\
+								HSISR_PDNINT |\
+								HSISR_GPIO9_INT)
+
+/*  */
+/*  */
+/*	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*  */
+/*  */
+#define BIT_USB_RXDMA_AGG_EN	BIT(31)
+#define RXDMA_AGG_MODE_EN		BIT(1)
+
+/*  */
+/*  */
+/*	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+
+/*  */
+/*        8723B REG_CCK_CHECK						(offset 0x454) */
+/*  */
+#define BIT_BCN_PORT_SEL		BIT5
+
+/*  */
+/*  */
+/*	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/*	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*  */
+/*  */
+#define EEPROM_RF_GAIN_OFFSET			0xC1
+#define EEPROM_RF_GAIN_VAL			0x1F6
+
+/*  */
+/*        8195 IMR/ISR bits						(offset 0xB0,  8bits) */
+/*  */
+#define	IMR_DISABLED_8723B					0
+/*  IMR DW0(0x00B0-00B3) Bit 0-31 */
+#define	IMR_TIMER2_8723B					BIT31		/*  Timeout interrupt 2 */
+#define	IMR_TIMER1_8723B					BIT30		/*  Timeout interrupt 1 */
+#define	IMR_PSTIMEOUT_8723B				BIT29		/*  Power Save Time Out Interrupt */
+#define	IMR_GTINT4_8723B					BIT28		/*  When GTIMER4 expires, this bit is set to 1 */
+#define	IMR_GTINT3_8723B					BIT27		/*  When GTIMER3 expires, this bit is set to 1 */
+#define	IMR_TXBCN0ERR_8723B				BIT26		/*  Transmit Beacon0 Error */
+#define	IMR_TXBCN0OK_8723B				BIT25		/*  Transmit Beacon0 OK */
+#define	IMR_TSF_BIT32_TOGGLE_8723B		BIT24		/*  TSF Timer BIT32 toggle indication interrupt */
+#define	IMR_BCNDMAINT0_8723B				BIT20		/*  Beacon DMA Interrupt 0 */
+#define	IMR_BCNDERR0_8723B				BIT16		/*  Beacon Queue DMA OK0 */
+#define	IMR_HSISR_IND_ON_INT_8723B		BIT15		/*  HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define	IMR_BCNDMAINT_E_8723B			BIT14		/*  Beacon DMA Interrupt Extension for Win7 */
+#define	IMR_ATIMEND_8723B				BIT12		/*  CTWidnow End or ATIM Window End */
+#define	IMR_C2HCMD_8723B					BIT10		/*  CPU to Host Command INT Status, Write 1 clear */
+#define	IMR_CPWM2_8723B					BIT9			/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_CPWM_8723B					BIT8			/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_HIGHDOK_8723B				BIT7			/*  High Queue DMA OK */
+#define	IMR_MGNTDOK_8723B				BIT6			/*  Management Queue DMA OK */
+#define	IMR_BKDOK_8723B					BIT5			/*  AC_BK DMA OK */
+#define	IMR_BEDOK_8723B					BIT4			/*  AC_BE DMA OK */
+#define	IMR_VIDOK_8723B					BIT3			/*  AC_VI DMA OK */
+#define	IMR_VODOK_8723B					BIT2			/*  AC_VO DMA OK */
+#define	IMR_RDU_8723B					BIT1			/*  Rx Descriptor Unavailable */
+#define	IMR_ROK_8723B					BIT0			/*  Receive DMA OK */
+
+/*  IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7_8723B				BIT27		/*  Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6_8723B				BIT26		/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5_8723B				BIT25		/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4_8723B				BIT24		/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3_8723B				BIT23		/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2_8723B				BIT22		/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1_8723B				BIT21		/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK7_8723B					BIT20		/*  Beacon Queue DMA OK Interrupt 7 */
+#define	IMR_BCNDOK6_8723B					BIT19		/*  Beacon Queue DMA OK Interrupt 6 */
+#define	IMR_BCNDOK5_8723B					BIT18		/*  Beacon Queue DMA OK Interrupt 5 */
+#define	IMR_BCNDOK4_8723B					BIT17		/*  Beacon Queue DMA OK Interrupt 4 */
+#define	IMR_BCNDOK3_8723B					BIT16		/*  Beacon Queue DMA OK Interrupt 3 */
+#define	IMR_BCNDOK2_8723B					BIT15		/*  Beacon Queue DMA OK Interrupt 2 */
+#define	IMR_BCNDOK1_8723B					BIT14		/*  Beacon Queue DMA OK Interrupt 1 */
+#define	IMR_ATIMEND_E_8723B				BIT13		/*  ATIM Window End Extension for Win7 */
+#define	IMR_TXERR_8723B					BIT11		/*  Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_RXERR_8723B					BIT10		/*  Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_TXFOVW_8723B					BIT9			/*  Transmit FIFO Overflow */
+#define	IMR_RXFOVW_8723B					BIT8			/*  Receive FIFO Overflow */
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_xmit.h b/drivers/staging/rtl8723bs/include/rtl8723b_xmit.h
new file mode 100644
index 000000000..ad2542d0c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_xmit.h
@@ -0,0 +1,420 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTL8723B_XMIT_H__
+#define __RTL8723B_XMIT_H__
+
+/*  */
+/*  Queue Select Value in TxDesc */
+/*  */
+#define QSLT_BK							0x2/* 0x01 */
+#define QSLT_BE							0x0
+#define QSLT_VI							0x5/* 0x4 */
+#define QSLT_VO							0x7/* 0x6 */
+#define QSLT_BEACON						0x10
+#define QSLT_HIGH						0x11
+#define QSLT_MGNT						0x12
+#define QSLT_CMD						0x13
+
+#define MAX_TID (15)
+
+/* OFFSET 0 */
+#define OFFSET_SZ	0
+#define OFFSET_SHT	16
+#define BMC		BIT(24)
+#define LSG		BIT(26)
+#define FSG		BIT(27)
+#define OWN		BIT(31)
+
+
+/* OFFSET 4 */
+#define PKT_OFFSET_SZ	0
+#define BK		BIT(6)
+#define QSEL_SHT	8
+#define Rate_ID_SHT	16
+#define NAVUSEHDR	BIT(20)
+#define PKT_OFFSET_SHT	26
+#define HWPC		BIT(31)
+
+/* OFFSET 8 */
+#define AGG_EN		BIT(29)
+
+/* OFFSET 12 */
+#define SEQ_SHT		16
+
+/* OFFSET 16 */
+#define QoS		BIT(6)
+#define HW_SEQ_EN	BIT(7)
+#define USERATE		BIT(8)
+#define DISDATAFB	BIT(10)
+#define DATA_SHORT	BIT(24)
+#define DATA_BW		BIT(25)
+
+/* OFFSET 20 */
+#define SGI		BIT(6)
+
+/*  */
+/* defined for TX DESC Operation */
+/*  */
+struct txdesc_8723b {
+	/*  Offset 0 */
+	u32 pktlen:16;
+	u32 offset:8;
+	u32 bmc:1;
+	u32 htc:1;
+	u32 rsvd0026:1;
+	u32 rsvd0027:1;
+	u32 linip:1;
+	u32 noacm:1;
+	u32 gf:1;
+	u32 rsvd0031:1;
+
+	/*  Offset 4 */
+	u32 macid:7;
+	u32 rsvd0407:1;
+	u32 qsel:5;
+	u32 rdg_nav_ext:1;
+	u32 lsig_txop_en:1;
+	u32 pifs:1;
+	u32 rate_id:5;
+	u32 en_desc_id:1;
+	u32 sectype:2;
+	u32 pkt_offset:5; /*  unit: 8 bytes */
+	u32 moredata:1;
+	u32 txop_ps_cap:1;
+	u32 txop_ps_mode:1;
+
+	/*  Offset 8 */
+	u32 p_aid:9;
+	u32 rsvd0809:1;
+	u32 cca_rts:2;
+	u32 agg_en:1;
+	u32 rdg_en:1;
+	u32 null_0:1;
+	u32 null_1:1;
+	u32 bk:1;
+	u32 morefrag:1;
+	u32 raw:1;
+	u32 spe_rpt:1;
+	u32 ampdu_density:3;
+	u32 bt_null:1;
+	u32 g_id:6;
+	u32 rsvd0830:2;
+
+	/*  Offset 12 */
+	u32 wheader_len:4;
+	u32 chk_en:1;
+	u32 early_rate:1;
+	u32 hw_ssn_sel:2;
+	u32 userate:1;
+	u32 disrtsfb:1;
+	u32 disdatafb:1;
+	u32 cts2self:1;
+	u32 rtsen:1;
+	u32 hw_rts_en:1;
+	u32 port_id:1;
+	u32 navusehdr:1;
+	u32 use_max_len:1;
+	u32 max_agg_num:5;
+	u32 ndpa:2;
+	u32 ampdu_max_time:8;
+
+	/*  Offset 16 */
+	u32 datarate:7;
+	u32 try_rate:1;
+	u32 data_ratefb_lmt:5;
+	u32 rts_ratefb_lmt:4;
+	u32 rty_lmt_en:1;
+	u32 data_rt_lmt:6;
+	u32 rtsrate:5;
+	u32 pcts_en:1;
+	u32 pcts_mask_idx:2;
+
+	/*  Offset 20 */
+	u32 data_sc:4;
+	u32 data_short:1;
+	u32 data_bw:2;
+	u32 data_ldpc:1;
+	u32 data_stbc:2;
+	u32 vcs_stbc:2;
+	u32 rts_short:1;
+	u32 rts_sc:4;
+	u32 rsvd2016:7;
+	u32 tx_ant:4;
+	u32 txpwr_offset:3;
+	u32 rsvd2031:1;
+
+	/*  Offset 24 */
+	u32 sw_define:12;
+	u32 mbssid:4;
+	u32 antsel_A:3;
+	u32 antsel_B:3;
+	u32 antsel_C:3;
+	u32 antsel_D:3;
+	u32 rsvd2428:4;
+
+	/*  Offset 28 */
+	u32 checksum:16;
+	u32 rsvd2816:8;
+	u32 usb_txagg_num:8;
+
+	/*  Offset 32 */
+	u32 rts_rc:6;
+	u32 bar_rty_th:2;
+	u32 data_rc:6;
+	u32 rsvd3214:1;
+	u32 en_hwseq:1;
+	u32 nextneadpage:8;
+	u32 tailpage:8;
+
+	/*  Offset 36 */
+	u32 padding_len:11;
+	u32 txbf_path:1;
+	u32 seq:12;
+	u32 final_data_rate:8;
+};
+
+#ifndef __INC_HAL8723BDESC_H
+#define __INC_HAL8723BDESC_H
+
+#define RX_STATUS_DESC_SIZE_8723B		24
+#define RX_DRV_INFO_SIZE_UNIT_8723B 8
+
+
+/* DWORD 0 */
+#define SET_RX_STATUS_DESC_PKT_LEN_8723B(__pRxStatusDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pRxStatusDesc, 0, 14, __Value)
+#define SET_RX_STATUS_DESC_EOR_8723B(__pRxStatusDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pRxStatusDesc, 30, 1, __Value)
+#define SET_RX_STATUS_DESC_OWN_8723B(__pRxStatusDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pRxStatusDesc, 31, 1, __Value)
+
+#define GET_RX_STATUS_DESC_PKT_LEN_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 0, 14)
+#define GET_RX_STATUS_DESC_CRC32_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 14, 1)
+#define GET_RX_STATUS_DESC_ICV_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc, 15, 1)
+#define GET_RX_STATUS_DESC_DRVINFO_SIZE_8723B(__pRxStatusDesc)		LE_BITS_TO_4BYTE(__pRxStatusDesc, 16, 4)
+#define GET_RX_STATUS_DESC_SECURITY_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 20, 3)
+#define GET_RX_STATUS_DESC_QOS_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc, 23, 1)
+#define GET_RX_STATUS_DESC_SHIFT_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 24, 2)
+#define GET_RX_STATUS_DESC_PHY_STATUS_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 26, 1)
+#define GET_RX_STATUS_DESC_SWDEC_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 27, 1)
+#define GET_RX_STATUS_DESC_LAST_SEG_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc, 28, 1)
+#define GET_RX_STATUS_DESC_FIRST_SEG_8723B(__pRxStatusDesc)		LE_BITS_TO_4BYTE(__pRxStatusDesc, 29, 1)
+#define GET_RX_STATUS_DESC_EOR_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc, 30, 1)
+#define GET_RX_STATUS_DESC_OWN_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc, 31, 1)
+
+/* DWORD 1 */
+#define GET_RX_STATUS_DESC_MACID_8723B(__pRxDesc)					LE_BITS_TO_4BYTE(__pRxDesc+4, 0, 7)
+#define GET_RX_STATUS_DESC_TID_8723B(__pRxDesc)						LE_BITS_TO_4BYTE(__pRxDesc+4, 8, 4)
+#define GET_RX_STATUS_DESC_AMSDU_8723B(__pRxDesc)					LE_BITS_TO_4BYTE(__pRxDesc+4, 13, 1)
+#define GET_RX_STATUS_DESC_RXID_MATCH_8723B(__pRxDesc)		LE_BITS_TO_4BYTE(__pRxDesc+4, 14, 1)
+#define GET_RX_STATUS_DESC_PAGGR_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 15, 1)
+#define GET_RX_STATUS_DESC_A1_FIT_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 16, 4)
+#define GET_RX_STATUS_DESC_CHKERR_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 20, 1)
+#define GET_RX_STATUS_DESC_IPVER_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+4, 21, 1)
+#define GET_RX_STATUS_DESC_IS_TCPUDP__8723B(__pRxDesc)		LE_BITS_TO_4BYTE(__pRxDesc+4, 22, 1)
+#define GET_RX_STATUS_DESC_CHK_VLD_8723B(__pRxDesc)	LE_BITS_TO_4BYTE(__pRxDesc+4, 23, 1)
+#define GET_RX_STATUS_DESC_PAM_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 24, 1)
+#define GET_RX_STATUS_DESC_PWR_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 25, 1)
+#define GET_RX_STATUS_DESC_MORE_DATA_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+4, 26, 1)
+#define GET_RX_STATUS_DESC_MORE_FRAG_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+4, 27, 1)
+#define GET_RX_STATUS_DESC_TYPE_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+4, 28, 2)
+#define GET_RX_STATUS_DESC_MC_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 30, 1)
+#define GET_RX_STATUS_DESC_BC_8723B(__pRxDesc)				LE_BITS_TO_4BYTE(__pRxDesc+4, 31, 1)
+
+/* DWORD 2 */
+#define GET_RX_STATUS_DESC_SEQ_8723B(__pRxStatusDesc)					LE_BITS_TO_4BYTE(__pRxStatusDesc+8, 0, 12)
+#define GET_RX_STATUS_DESC_FRAG_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc+8, 12, 4)
+#define GET_RX_STATUS_DESC_RX_IS_QOS_8723B(__pRxStatusDesc)		LE_BITS_TO_4BYTE(__pRxStatusDesc+8, 16, 1)
+#define GET_RX_STATUS_DESC_WLANHD_IV_LEN_8723B(__pRxStatusDesc)		LE_BITS_TO_4BYTE(__pRxStatusDesc+8, 18, 6)
+#define GET_RX_STATUS_DESC_RPT_SEL_8723B(__pRxStatusDesc)			LE_BITS_TO_4BYTE(__pRxStatusDesc+8, 28, 1)
+
+/* DWORD 3 */
+#define GET_RX_STATUS_DESC_RX_RATE_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc+12, 0, 7)
+#define GET_RX_STATUS_DESC_HTC_8723B(__pRxStatusDesc)					LE_BITS_TO_4BYTE(__pRxStatusDesc+12, 10, 1)
+#define GET_RX_STATUS_DESC_EOSP_8723B(__pRxStatusDesc)					LE_BITS_TO_4BYTE(__pRxStatusDesc+12, 11, 1)
+#define GET_RX_STATUS_DESC_BSSID_FIT_8723B(__pRxStatusDesc)		LE_BITS_TO_4BYTE(__pRxStatusDesc+12, 12, 2)
+#define GET_RX_STATUS_DESC_PATTERN_MATCH_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+12, 29, 1)
+#define GET_RX_STATUS_DESC_UNICAST_MATCH_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+12, 30, 1)
+#define GET_RX_STATUS_DESC_MAGIC_MATCH_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+12, 31, 1)
+
+/* DWORD 6 */
+#define GET_RX_STATUS_DESC_SPLCP_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+16, 0, 1)
+#define GET_RX_STATUS_DESC_LDPC_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+16, 1, 1)
+#define GET_RX_STATUS_DESC_STBC_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+16, 2, 1)
+#define GET_RX_STATUS_DESC_BW_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+16, 4, 2)
+
+/* DWORD 5 */
+#define GET_RX_STATUS_DESC_TSFL_8723B(__pRxStatusDesc)				LE_BITS_TO_4BYTE(__pRxStatusDesc+20, 0, 32)
+
+#define GET_RX_STATUS_DESC_BUFF_ADDR_8723B(__pRxDesc)		LE_BITS_TO_4BYTE(__pRxDesc+24, 0, 32)
+#define GET_RX_STATUS_DESC_BUFF_ADDR64_8723B(__pRxDesc)			LE_BITS_TO_4BYTE(__pRxDesc+28, 0, 32)
+
+#define SET_RX_STATUS_DESC_BUFF_ADDR_8723B(__pRxDesc, __Value)	SET_BITS_TO_LE_4BYTE(__pRxDesc+24, 0, 32, __Value)
+
+
+/*  Dword 0 */
+#define GET_TX_DESC_OWN_8723B(__pTxDesc)				LE_BITS_TO_4BYTE(__pTxDesc, 31, 1)
+
+#define SET_TX_DESC_PKT_SIZE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 0, 16, __Value)
+#define SET_TX_DESC_OFFSET_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 16, 8, __Value)
+#define SET_TX_DESC_BMC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 24, 1, __Value)
+#define SET_TX_DESC_HTC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 25, 1, __Value)
+#define SET_TX_DESC_LAST_SEG_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 26, 1, __Value)
+#define SET_TX_DESC_FIRST_SEG_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 27, 1, __Value)
+#define SET_TX_DESC_LINIP_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 28, 1, __Value)
+#define SET_TX_DESC_NO_ACM_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 29, 1, __Value)
+#define SET_TX_DESC_GF_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 30, 1, __Value)
+#define SET_TX_DESC_OWN_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc, 31, 1, __Value)
+
+/*  Dword 1 */
+#define SET_TX_DESC_MACID_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 0, 7, __Value)
+#define SET_TX_DESC_QUEUE_SEL_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 8, 5, __Value)
+#define SET_TX_DESC_RDG_NAV_EXT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 13, 1, __Value)
+#define SET_TX_DESC_LSIG_TXOP_EN_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 14, 1, __Value)
+#define SET_TX_DESC_PIFS_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 15, 1, __Value)
+#define SET_TX_DESC_RATE_ID_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 16, 5, __Value)
+#define SET_TX_DESC_EN_DESC_ID_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 21, 1, __Value)
+#define SET_TX_DESC_SEC_TYPE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 22, 2, __Value)
+#define SET_TX_DESC_PKT_OFFSET_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+4, 24, 5, __Value)
+
+
+/*  Dword 2 */
+#define SET_TX_DESC_PAID_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 0,  9, __Value)
+#define SET_TX_DESC_CCA_RTS_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 10, 2, __Value)
+#define SET_TX_DESC_AGG_ENABLE_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 12, 1, __Value)
+#define SET_TX_DESC_RDG_ENABLE_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 13, 1, __Value)
+#define SET_TX_DESC_AGG_BREAK_8723B(__pTxDesc, __Value)					SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 16, 1, __Value)
+#define SET_TX_DESC_MORE_FRAG_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 17, 1, __Value)
+#define SET_TX_DESC_RAW_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 18, 1, __Value)
+#define SET_TX_DESC_SPE_RPT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 19, 1, __Value)
+#define SET_TX_DESC_AMPDU_DENSITY_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 20, 3, __Value)
+#define SET_TX_DESC_BT_INT_8723B(__pTxDesc, __Value)			SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 23, 1, __Value)
+#define SET_TX_DESC_GID_8723B(__pTxDesc, __Value)			SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 6, __Value)
+
+
+/*  Dword 3 */
+#define SET_TX_DESC_WHEADER_LEN_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 0, 4, __Value)
+#define SET_TX_DESC_CHK_EN_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 4, 1, __Value)
+#define SET_TX_DESC_EARLY_MODE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 5, 1, __Value)
+#define SET_TX_DESC_HWSEQ_SEL_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 6, 2, __Value)
+#define SET_TX_DESC_USE_RATE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 8, 1, __Value)
+#define SET_TX_DESC_DISABLE_RTS_FB_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 9, 1, __Value)
+#define SET_TX_DESC_DISABLE_FB_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 10, 1, __Value)
+#define SET_TX_DESC_CTS2SELF_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 11, 1, __Value)
+#define SET_TX_DESC_RTS_ENABLE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 12, 1, __Value)
+#define SET_TX_DESC_HW_RTS_ENABLE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 13, 1, __Value)
+#define SET_TX_DESC_NAV_USE_HDR_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 15, 1, __Value)
+#define SET_TX_DESC_USE_MAX_LEN_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 16, 1, __Value)
+#define SET_TX_DESC_MAX_AGG_NUM_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 17, 5, __Value)
+#define SET_TX_DESC_NDPA_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 22, 2, __Value)
+#define SET_TX_DESC_AMPDU_MAX_TIME_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+12, 24, 8, __Value)
+
+/*  Dword 4 */
+#define SET_TX_DESC_TX_RATE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 0, 7, __Value)
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 8, 5, __Value)
+#define SET_TX_DESC_RTS_RATE_FB_LIMIT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 13, 4, __Value)
+#define SET_TX_DESC_RETRY_LIMIT_ENABLE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 17, 1, __Value)
+#define SET_TX_DESC_DATA_RETRY_LIMIT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 18, 6, __Value)
+#define SET_TX_DESC_RTS_RATE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 24, 5, __Value)
+
+
+/*  Dword 5 */
+#define SET_TX_DESC_DATA_SC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 0, 4, __Value)
+#define SET_TX_DESC_DATA_SHORT_8723B(__pTxDesc, __Value)	SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 4, 1, __Value)
+#define SET_TX_DESC_DATA_BW_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 5, 2, __Value)
+#define SET_TX_DESC_DATA_LDPC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 7, 1, __Value)
+#define SET_TX_DESC_DATA_STBC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 8, 2, __Value)
+#define SET_TX_DESC_CTROL_STBC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 10, 2, __Value)
+#define SET_TX_DESC_RTS_SHORT_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 12, 1, __Value)
+#define SET_TX_DESC_RTS_SC_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+20, 13, 4, __Value)
+
+
+/*  Dword 6 */
+#define SET_TX_DESC_SW_DEFINE_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+24, 0, 12, __Value)
+#define SET_TX_DESC_ANTSEL_A_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+24, 16, 3, __Value)
+#define SET_TX_DESC_ANTSEL_B_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+24, 19, 3, __Value)
+#define SET_TX_DESC_ANTSEL_C_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+24, 22, 3, __Value)
+#define SET_TX_DESC_ANTSEL_D_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+24, 25, 3, __Value)
+
+/*  Dword 7 */
+#define SET_TX_DESC_TX_DESC_CHECKSUM_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 0, 16, __Value)
+#define SET_TX_DESC_USB_TXAGG_NUM_8723B(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 24, 8, __Value)
+#define SET_TX_DESC_SDIO_TXSEQ_8723B(__pTxDesc, __Value)			SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 16, 8, __Value)
+
+/*  Dword 8 */
+#define SET_TX_DESC_HWSEQ_EN_8723B(__pTxDesc, __Value)			SET_BITS_TO_LE_4BYTE(__pTxDesc+32, 15, 1, __Value)
+
+/*  Dword 9 */
+#define SET_TX_DESC_SEQ_8723B(__pTxDesc, __Value)					SET_BITS_TO_LE_4BYTE(__pTxDesc+36, 12, 12, __Value)
+
+/*  Dword 10 */
+#define SET_TX_DESC_TX_BUFFER_ADDRESS_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+40, 0, 32, __Value)
+#define GET_TX_DESC_TX_BUFFER_ADDRESS_8723B(__pTxDesc)	LE_BITS_TO_4BYTE(__pTxDesc+40, 0, 32)
+
+/*  Dword 11 */
+#define SET_TX_DESC_NEXT_DESC_ADDRESS_8723B(__pTxDesc, __Value)		SET_BITS_TO_LE_4BYTE(__pTxDesc+48, 0, 32, __Value)
+
+
+#define SET_EARLYMODE_PKTNUM_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr, 0, 4, __Value)
+#define SET_EARLYMODE_LEN0_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr, 4, 15, __Value)
+#define SET_EARLYMODE_LEN1_1_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr, 19, 13, __Value)
+#define SET_EARLYMODE_LEN1_2_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr+4, 0, 2, __Value)
+#define SET_EARLYMODE_LEN2_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr+4, 2, 15,	__Value)
+#define SET_EARLYMODE_LEN3_8723B(__pAddr, __Value)					SET_BITS_TO_LE_4BYTE(__pAddr+4, 17, 15, __Value)
+
+#endif
+/*  */
+/*  */
+/* 	Rate */
+/*  */
+/*  */
+/*  CCK Rates, TxHT = 0 */
+#define DESC8723B_RATE1M				0x00
+#define DESC8723B_RATE2M				0x01
+#define DESC8723B_RATE5_5M				0x02
+#define DESC8723B_RATE11M				0x03
+
+/*  OFDM Rates, TxHT = 0 */
+#define DESC8723B_RATE6M				0x04
+#define DESC8723B_RATE9M				0x05
+#define DESC8723B_RATE12M				0x06
+#define DESC8723B_RATE18M				0x07
+#define DESC8723B_RATE24M				0x08
+#define DESC8723B_RATE36M				0x09
+#define DESC8723B_RATE48M				0x0a
+#define DESC8723B_RATE54M				0x0b
+
+/*  MCS Rates, TxHT = 1 */
+#define DESC8723B_RATEMCS0				0x0c
+#define DESC8723B_RATEMCS1				0x0d
+#define DESC8723B_RATEMCS2				0x0e
+#define DESC8723B_RATEMCS3				0x0f
+#define DESC8723B_RATEMCS4				0x10
+#define DESC8723B_RATEMCS5				0x11
+#define DESC8723B_RATEMCS6				0x12
+#define DESC8723B_RATEMCS7				0x13
+
+#define		RX_HAL_IS_CCK_RATE_8723B(pDesc)\
+			(GET_RX_STATUS_DESC_RX_RATE_8723B(pDesc) == DESC8723B_RATE1M ||\
+			GET_RX_STATUS_DESC_RX_RATE_8723B(pDesc) == DESC8723B_RATE2M ||\
+			GET_RX_STATUS_DESC_RX_RATE_8723B(pDesc) == DESC8723B_RATE5_5M ||\
+			GET_RX_STATUS_DESC_RX_RATE_8723B(pDesc) == DESC8723B_RATE11M)
+
+
+void rtl8723b_update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem);
+void rtl8723b_fill_fake_txdesc(struct adapter *padapter, u8 *pDesc, u32 BufferLen, u8 IsPsPoll, u8 IsBTQosNull, u8 bDataFrame);
+
+s32 rtl8723bs_init_xmit_priv(struct adapter *padapter);
+void rtl8723bs_free_xmit_priv(struct adapter *padapter);
+s32 rtl8723bs_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe);
+s32 rtl8723bs_mgnt_xmit(struct adapter *padapter, struct xmit_frame *pmgntframe);
+s32	rtl8723bs_hal_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe);
+s32 rtl8723bs_xmit_buf_handler(struct adapter *padapter);
+int rtl8723bs_xmit_thread(void *context);
+#define hal_xmit_handler rtl8723bs_xmit_buf_handler
+
+u8 BWMapping_8723B(struct adapter *Adapter, struct pkt_attrib *pattrib);
+u8 SCMapping_8723B(struct adapter *Adapter, struct pkt_attrib *pattrib);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_ap.h b/drivers/staging/rtl8723bs/include/rtw_ap.h
new file mode 100644
index 000000000..7a735e691
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ap.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_AP_H_
+#define __RTW_AP_H_
+
+void init_mlme_ap_info(struct adapter *padapter);
+void free_mlme_ap_info(struct adapter *padapter);
+/* void update_BCNTIM(struct adapter *padapter); */
+void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx);
+void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level);
+void expire_timeout_chk(struct adapter *padapter);
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta);
+void start_bss_network(struct adapter *padapter);
+int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len);
+void rtw_ap_restore_network(struct adapter *padapter);
+void rtw_set_macaddr_acl(struct adapter *padapter, int mode);
+int rtw_acl_add_sta(struct adapter *padapter, u8 *addr);
+void rtw_acl_remove_sta(struct adapter *padapter, u8 *addr);
+
+u8 rtw_ap_set_pairwise_key(struct adapter *padapter, struct sta_info *psta);
+int rtw_ap_set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid);
+int rtw_ap_set_wep_key(struct adapter *padapter, u8 *key, u8 keylen, int keyid, u8 set_tx);
+
+void associated_clients_update(struct adapter *padapter, u8 updated);
+void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta);
+u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta);
+void sta_info_update(struct adapter *padapter, struct sta_info *psta);
+void ap_sta_info_defer_update(struct adapter *padapter, struct sta_info *psta);
+u8 ap_free_sta(struct adapter *padapter, struct sta_info *psta, bool active, u16 reason);
+void rtw_sta_flush(struct adapter *padapter);
+void start_ap_mode(struct adapter *padapter);
+void stop_ap_mode(struct adapter *padapter);
+
+#endif
+void update_bmc_sta(struct adapter *padapter);
diff --git a/drivers/staging/rtl8723bs/include/rtw_btcoex.h b/drivers/staging/rtl8723bs/include/rtw_btcoex.h
new file mode 100644
index 000000000..19764c80b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_btcoex.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_BTCOEX_H__
+#define __RTW_BTCOEX_H__
+
+#include <drv_types.h>
+
+
+#define	PACKET_NORMAL			0
+#define	PACKET_DHCP				1
+#define	PACKET_ARP				2
+#define	PACKET_EAPOL			3
+
+void rtw_btcoex_MediaStatusNotify(struct adapter *, u8 mediaStatus);
+void rtw_btcoex_HaltNotify(struct adapter *);
+
+/*  ================================================== */
+/*  Below Functions are called by BT-Coex */
+/*  ================================================== */
+void rtw_btcoex_RejectApAggregatedPacket(struct adapter *, u8 enable);
+void rtw_btcoex_LPS_Enter(struct adapter *);
+void rtw_btcoex_LPS_Leave(struct adapter *);
+
+#endif /*  __RTW_BTCOEX_H__ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_byteorder.h b/drivers/staging/rtl8723bs/include/rtw_byteorder.h
new file mode 100644
index 000000000..c3a65f971
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_byteorder.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTL871X_BYTEORDER_H_
+#define _RTL871X_BYTEORDER_H_
+
+#if defined(__LITTLE_ENDIAN)
+#include <linux/byteorder/little_endian.h>
+#else
+#  include <linux/byteorder/big_endian.h>
+#endif
+
+#endif /* _RTL871X_BYTEORDER_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_cmd.h b/drivers/staging/rtl8723bs/include/rtw_cmd.h
new file mode 100644
index 000000000..fe1b03101
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_cmd.h
@@ -0,0 +1,719 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_CMD_H_
+#define __RTW_CMD_H_
+
+#include <linux/completion.h>
+
+#define C2H_MEM_SZ (16*1024)
+
+	#define FREE_CMDOBJ_SZ	128
+
+	#define MAX_CMDSZ	1024
+	#define MAX_RSPSZ	512
+	#define MAX_EVTSZ	1024
+
+	#define CMDBUFF_ALIGN_SZ 512
+
+	struct cmd_obj {
+		struct adapter *padapter;
+		u16 cmdcode;
+		u8 res;
+		u8 *parmbuf;
+		u32 cmdsz;
+		u8 *rsp;
+		u32 rspsz;
+		struct submit_ctx *sctx;
+		struct list_head	list;
+	};
+
+	/* cmd flags */
+	enum {
+		RTW_CMDF_DIRECTLY = BIT0,
+		RTW_CMDF_WAIT_ACK = BIT1,
+	};
+
+	struct cmd_priv {
+		struct completion cmd_queue_comp;
+		struct completion terminate_cmdthread_comp;
+		struct __queue	cmd_queue;
+		u8 cmd_seq;
+		u8 *cmd_buf;	/* shall be non-paged, and 4 bytes aligned */
+		u8 *cmd_allocated_buf;
+		u8 *rsp_buf;	/* shall be non-paged, and 4 bytes aligned */
+		u8 *rsp_allocated_buf;
+		u32 cmd_issued_cnt;
+		u32 cmd_done_cnt;
+		u32 rsp_cnt;
+		atomic_t cmdthd_running;
+		/* u8 cmdthd_running; */
+		u8 stop_req;
+		struct adapter *padapter;
+		struct mutex sctx_mutex;
+	};
+
+	struct	evt_priv {
+		struct work_struct c2h_wk;
+		bool c2h_wk_alive;
+		struct rtw_cbuf *c2h_queue;
+		#define C2H_QUEUE_MAX_LEN 10
+
+		atomic_t event_seq;
+		u8 *evt_buf;	/* shall be non-paged, and 4 bytes aligned */
+		u8 *evt_allocated_buf;
+		u32 evt_done_cnt;
+		u8 *c2h_mem;
+		u8 *allocated_c2h_mem;
+	};
+
+#define init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code) \
+do {\
+	INIT_LIST_HEAD(&pcmd->list);\
+	pcmd->cmdcode = code;\
+	pcmd->parmbuf = (u8 *)(pparm);\
+	pcmd->cmdsz = sizeof(*pparm);\
+	pcmd->rsp = NULL;\
+	pcmd->rspsz = 0;\
+} while (0)
+
+#define init_h2fwcmd_w_parm_no_parm_rsp(pcmd, code) \
+do {\
+	INIT_LIST_HEAD(&pcmd->list);\
+	pcmd->cmdcode = code;\
+	pcmd->parmbuf = NULL;\
+	pcmd->cmdsz = 0;\
+	pcmd->rsp = NULL;\
+	pcmd->rspsz = 0;\
+} while (0)
+
+struct c2h_evt_hdr {
+	u8 id:4;
+	u8 plen:4;
+	u8 seq;
+	u8 payload[];
+};
+
+struct c2h_evt_hdr_88xx {
+	u8 id;
+	u8 seq;
+	u8 payload[12];
+	u8 plen;
+	u8 trigger;
+};
+
+#define c2h_evt_valid(c2h_evt) ((c2h_evt)->id || (c2h_evt)->plen)
+
+int rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+extern struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv);
+extern void rtw_free_cmd_obj(struct cmd_obj *pcmd);
+
+void rtw_stop_cmd_thread(struct adapter *adapter);
+int rtw_cmd_thread(void *context);
+
+extern void rtw_free_cmd_priv(struct cmd_priv *pcmdpriv);
+
+extern void rtw_free_evt_priv(struct evt_priv *pevtpriv);
+extern void rtw_evt_notify_isr(struct evt_priv *pevtpriv);
+
+enum {
+	NONE_WK_CID,
+	DYNAMIC_CHK_WK_CID,
+	DM_CTRL_WK_CID,
+	PBC_POLLING_WK_CID,
+	POWER_SAVING_CTRL_WK_CID,/* IPS, AUTOSuspend */
+	LPS_CTRL_WK_CID,
+	ANT_SELECT_WK_CID,
+	P2P_PS_WK_CID,
+	P2P_PROTO_WK_CID,
+	CHECK_HIQ_WK_CID,/* for softap mode, check hi queue if empty */
+	INTEl_WIDI_WK_CID,
+	C2H_WK_CID,
+	RTP_TIMER_CFG_WK_CID,
+	RESET_SECURITYPRIV, /*  add for CONFIG_IEEE80211W, none 11w also can use */
+	FREE_ASSOC_RESOURCES, /*  add for CONFIG_IEEE80211W, none 11w also can use */
+	DM_IN_LPS_WK_CID,
+	DM_RA_MSK_WK_CID, /* add for STA update RAMask when bandwidth change. */
+	BEAMFORMING_WK_CID,
+	LPS_CHANGE_DTIM_CID,
+	BTINFO_WK_CID,
+	MAX_WK_CID
+};
+
+enum {
+	LPS_CTRL_SCAN = 0,
+	LPS_CTRL_JOINBSS = 1,
+	LPS_CTRL_CONNECT = 2,
+	LPS_CTRL_DISCONNECT = 3,
+	LPS_CTRL_SPECIAL_PACKET = 4,
+	LPS_CTRL_LEAVE = 5,
+	LPS_CTRL_TRAFFIC_BUSY = 6,
+};
+
+enum {
+	SWSI,
+	HWSI,
+	HWPI,
+};
+
+/*
+Caller Mode: Infra, Ad-HoC
+
+Notes: To join a known BSS.
+
+Command-Event Mode
+
+*/
+
+/*
+Caller Mode: Infra, Ad-Hoc
+
+Notes: To join the specified bss
+
+Command Event Mode
+
+*/
+struct joinbss_parm {
+	struct wlan_bssid_ex network;
+};
+
+/*
+Caller Mode: Infra, Ad-HoC(C)
+
+Notes: To disconnect the current associated BSS
+
+Command Mode
+
+*/
+struct disconnect_parm {
+	u32 deauth_timeout_ms;
+};
+
+/*
+Caller Mode: AP, Ad-HoC(M)
+
+Notes: To create a BSS
+
+Command Mode
+*/
+struct createbss_parm {
+	struct wlan_bssid_ex network;
+};
+
+/*
+Caller Mode: AP, Ad-HoC, Infra
+
+Notes: To set the NIC mode of RTL8711
+
+Command Mode
+
+The definition of mode:
+
+#define IW_MODE_AUTO	0	 Let the driver decides which AP to join
+#define IW_MODE_ADHOC	1	 Single cell network (Ad-Hoc Clients)
+#define IW_MODE_INFRA	2	 Multi cell network, roaming, ..
+#define IW_MODE_MASTER	3	 Synchronisation master or Access Point
+#define IW_MODE_REPEAT	4	 Wireless Repeater (forwarder)
+#define IW_MODE_SECOND	5	 Secondary master/repeater (backup)
+#define IW_MODE_MONITOR	6	 Passive monitor (listen only)
+
+*/
+struct	setopmode_parm {
+	u8 mode;
+	u8 rsvd[3];
+};
+
+/*
+Caller Mode: AP, Ad-HoC, Infra
+
+Notes: To ask RTL8711 performing site-survey
+
+Command-Event Mode
+
+*/
+
+#define RTW_SSID_SCAN_AMOUNT 9 /*  for WEXT_CSCAN_AMOUNT 9 */
+#define RTW_CHANNEL_SCAN_AMOUNT (14+37)
+struct sitesurvey_parm {
+	signed int scan_mode;	/* active: 1, passive: 0 */
+	u8 ssid_num;
+	u8 ch_num;
+	struct ndis_802_11_ssid ssid[RTW_SSID_SCAN_AMOUNT];
+	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To set the auth type of RTL8711. open/shared/802.1x
+
+Command Mode
+
+*/
+struct setauth_parm {
+	u8 mode;  /* 0: legacy open, 1: legacy shared 2: 802.1x */
+	u8 _1x;   /* 0: PSK, 1: TLS */
+	u8 rsvd[2];
+};
+
+/*
+Caller Mode: Infra
+
+a. algorithm: wep40, wep104, tkip & aes
+b. keytype: grp key/unicast key
+c. key contents
+
+when shared key ==> keyid is the camid
+when 802.1x ==> keyid [0:1] ==> grp key
+when 802.1x ==> keyid > 2 ==> unicast key
+
+*/
+struct setkey_parm {
+	u8 algorithm;	/*  encryption algorithm, could be none, wep40, TKIP, CCMP, wep104 */
+	u8 keyid;
+	u8 grpkey;		/*  1: this is the grpkey for 802.1x. 0: this is the unicast key for 802.1x */
+	u8 set_tx;		/*  1: main tx key for wep. 0: other key. */
+	u8 key[16];	/*  this could be 40 or 104 */
+};
+
+/*
+When in AP or Ad-Hoc mode, this is used to
+allocate an sw/hw entry for a newly associated sta.
+
+Command
+
+when shared key ==> algorithm/keyid
+
+*/
+struct set_stakey_parm {
+	u8 addr[ETH_ALEN];
+	u8 algorithm;
+	u8 keyid;
+	u8 key[16];
+};
+
+struct set_stakey_rsp {
+	u8 addr[ETH_ALEN];
+	u8 keyid;
+	u8 rsvd;
+};
+
+/*
+Caller Ad-Hoc/AP
+
+Command -Rsp(AID == CAMID) mode
+
+This is to force fw to add an sta_data entry per driver's request.
+
+FW will write an cam entry associated with it.
+
+*/
+struct set_assocsta_parm {
+	u8 addr[ETH_ALEN];
+};
+
+struct set_assocsta_rsp {
+	u8 cam_id;
+	u8 rsvd[3];
+};
+
+/*
+	Caller Ad-Hoc/AP
+
+	Command mode
+
+	This is to force fw to del an sta_data entry per driver's request
+
+	FW will invalidate the cam entry associated with it.
+
+*/
+struct del_assocsta_parm {
+	u8 addr[ETH_ALEN];
+};
+
+/*
+Caller Mode: AP/Ad-HoC(M)
+
+Notes: To notify fw that given staid has changed its power state
+
+Command Mode
+
+*/
+struct setstapwrstate_parm {
+	u8 staid;
+	u8 status;
+	u8 hwaddr[6];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To setup the basic rate of RTL8711
+
+Command Mode
+
+*/
+struct	setbasicrate_parm {
+	u8 basicrates[NumRates];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To read the current basic rate
+
+Command-Rsp Mode
+
+*/
+struct getbasicrate_parm {
+	u32 rsvd;
+};
+
+/*
+Caller Mode: Any
+
+Notes: To setup the data rate of RTL8711
+
+Command Mode
+
+*/
+struct setdatarate_parm {
+	u8 mac_id;
+	u8 datarates[NumRates];
+};
+
+/*
+Caller Mode: Any
+
+Notes: To read the current data rate
+
+Command-Rsp Mode
+
+*/
+struct getdatarate_parm {
+	u32 rsvd;
+
+};
+
+/*
+Caller Mode: Any
+AP: AP can use the info for the contents of beacon frame
+Infra: STA can use the info when sitesurveying
+Ad-HoC(M): Like AP
+Ad-HoC(C): Like STA
+
+
+Notes: To set the phy capability of the NIC
+
+Command Mode
+
+*/
+
+struct	setphyinfo_parm {
+	struct regulatory_class class_sets[NUM_REGULATORYS];
+	u8 status;
+};
+
+struct	getphyinfo_parm {
+	u32 rsvd;
+};
+
+/*
+Caller Mode: Any
+
+Notes: To set the channel/modem/band
+This command will be used when channel/modem/band is changed.
+
+Command Mode
+
+*/
+struct	setphy_parm {
+	u8 rfchannel;
+	u8 modem;
+};
+
+/*
+Caller Mode: Any
+
+Notes: To get the current setting of channel/modem/band
+
+Command-Rsp Mode
+
+*/
+struct	getphy_parm {
+	u32 rsvd;
+
+};
+
+struct Tx_Beacon_param {
+	struct wlan_bssid_ex network;
+};
+
+/*
+	Notes: This command is used for H2C/C2H loopback testing
+
+	mac[0] == 0
+	==> CMD mode, return H2C_SUCCESS.
+	The following condition must be true under CMD mode
+		mac[1] == mac[4], mac[2] == mac[3], mac[0]=mac[5]= 0;
+		s0 == 0x1234, s1 == 0xabcd, w0 == 0x78563412, w1 == 0x5aa5def7;
+		s2 == (b1 << 8 | b0);
+
+	mac[0] == 1
+	==> CMD_RSP mode, return H2C_SUCCESS_RSP
+
+	The rsp layout shall be:
+	rsp:			parm:
+		mac[0]  =   mac[5];
+		mac[1]  =   mac[4];
+		mac[2]  =   mac[3];
+		mac[3]  =   mac[2];
+		mac[4]  =   mac[1];
+		mac[5]  =   mac[0];
+		s0		=   s1;
+		s1		=   swap16(s0);
+		w0		=	swap32(w1);
+		b0		=	b1
+		s2		=	s0 + s1
+		b1		=	b0
+		w1		=	w0
+
+	mac[0] ==	2
+	==> CMD_EVENT mode, return	H2C_SUCCESS
+	The event layout shall be:
+	event:			parm:
+		mac[0]  =   mac[5];
+		mac[1]  =   mac[4];
+		mac[2]  =   event's sequence number, starting from 1 to parm's marc[3]
+		mac[3]  =   mac[2];
+		mac[4]  =   mac[1];
+		mac[5]  =   mac[0];
+		s0		=   swap16(s0) - event.mac[2];
+		s1		=   s1 + event.mac[2];
+		w0		=	swap32(w0);
+		b0		=	b1
+		s2		=	s0 + event.mac[2]
+		b1		=	b0
+		w1		=	swap32(w1) - event.mac[2];
+
+		parm->mac[3] is the total event counts that host requested.
+
+
+	event will be the same with the cmd's param.
+
+*/
+
+/*  CMD param Formart for driver extra cmd handler */
+struct drvextra_cmd_parm {
+	int ec_id; /* extra cmd id */
+	int type; /*  Can use this field as the type id or command size */
+	int size; /* buffer size */
+	unsigned char *pbuf;
+};
+
+/*------------------- Below are used for RF/BB tuning ---------------------*/
+
+struct	getcountjudge_rsp {
+	u8 count_judge[MAX_RATES_LENGTH];
+};
+
+struct addBaReq_parm {
+	unsigned int tid;
+	u8 addr[ETH_ALEN];
+};
+
+/*H2C Handler index: 46 */
+struct set_ch_parm {
+	u8 ch;
+	u8 bw;
+	u8 ch_offset;
+};
+
+/*H2C Handler index: 59 */
+struct SetChannelPlan_param {
+	u8 channel_plan;
+};
+
+/*H2C Handler index: 61 */
+struct SetChannelSwitch_param {
+	u8 new_ch_no;
+};
+
+/*H2C Handler index: 62 */
+struct TDLSoption_param {
+	u8 addr[ETH_ALEN];
+	u8 option;
+};
+
+/*H2C Handler index: 64 */
+struct RunInThread_param {
+	void (*func)(void *);
+	void *context;
+};
+
+
+#define GEN_CMD_CODE(cmd)	cmd ## _CMD_
+
+
+/*
+
+Result:
+0x00: success
+0x01: success, and check Response.
+0x02: cmd ignored due to duplicated sequcne number
+0x03: cmd dropped due to invalid cmd code
+0x04: reserved.
+
+*/
+
+#define H2C_RSP_OFFSET			512
+
+#define H2C_SUCCESS			0x00
+#define H2C_SUCCESS_RSP			0x01
+#define H2C_DUPLICATED			0x02
+#define H2C_DROPPED			0x03
+#define H2C_PARAMETERS_ERROR		0x04
+#define H2C_REJECTED			0x05
+#define H2C_CMD_OVERFLOW		0x06
+#define H2C_RESERVED			0x07
+
+u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num, struct rtw_ieee80211_channel *ch, int ch_num);
+extern u8 rtw_createbss_cmd(struct adapter  *padapter);
+int rtw_startbss_cmd(struct adapter  *padapter, int flags);
+
+struct sta_info;
+extern u8 rtw_setstakey_cmd(struct adapter  *padapter, struct sta_info *sta, u8 unicast_key, bool enqueue);
+extern u8 rtw_clearstakey_cmd(struct adapter *padapter, struct sta_info *sta, u8 enqueue);
+
+extern u8 rtw_joinbss_cmd(struct adapter *padapter, struct wlan_network *pnetwork);
+u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
+extern u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infrastructure networktype, bool enqueue);
+extern u8 rtw_setrfintfs_cmd(struct adapter  *padapter, u8 mode);
+
+extern u8 rtw_gettssi_cmd(struct adapter  *padapter, u8 offset, u8 *pval);
+extern u8 rtw_setfwdig_cmd(struct adapter *padapter, u8 type);
+extern u8 rtw_setfwra_cmd(struct adapter *padapter, u8 type);
+
+extern u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr);
+/*  add for CONFIG_IEEE80211W, none 11w also can use */
+extern u8 rtw_reset_securitypriv_cmd(struct adapter *padapter);
+extern u8 rtw_free_assoc_resources_cmd(struct adapter *padapter);
+extern u8 rtw_dynamic_chk_wk_cmd(struct adapter *adapter);
+
+u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue);
+u8 rtw_dm_in_lps_wk_cmd(struct adapter *padapter);
+
+u8 rtw_dm_ra_mask_wk_cmd(struct adapter *padapter, u8 *psta);
+
+extern u8 rtw_ps_cmd(struct adapter *padapter);
+
+u8 rtw_chk_hi_queue_cmd(struct adapter *padapter);
+
+extern u8 rtw_c2h_packet_wk_cmd(struct adapter *padapter, u8 *pbuf, u16 length);
+extern u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt);
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf);
+
+extern void rtw_survey_cmd_callback(struct adapter  *padapter, struct cmd_obj *pcmd);
+extern void rtw_disassoc_cmd_callback(struct adapter  *padapter, struct cmd_obj *pcmd);
+extern void rtw_joinbss_cmd_callback(struct adapter  *padapter, struct cmd_obj *pcmd);
+extern void rtw_createbss_cmd_callback(struct adapter  *padapter, struct cmd_obj *pcmd);
+extern void rtw_getbbrfreg_cmdrsp_callback(struct adapter  *padapter, struct cmd_obj *pcmd);
+
+extern void rtw_setstaKey_cmdrsp_callback(struct adapter  *padapter,  struct cmd_obj *pcmd);
+extern void rtw_setassocsta_cmdrsp_callback(struct adapter  *padapter,  struct cmd_obj *pcmd);
+extern void rtw_getrttbl_cmdrsp_callback(struct adapter  *padapter,  struct cmd_obj *pcmd);
+
+
+struct _cmd_callback {
+	u32 cmd_code;
+	void (*callback)(struct adapter  *padapter, struct cmd_obj *cmd);
+};
+
+enum {
+	GEN_CMD_CODE(_Read_MACREG),	/*0*/
+	GEN_CMD_CODE(_Write_MACREG),
+	GEN_CMD_CODE(_Read_BBREG),
+	GEN_CMD_CODE(_Write_BBREG),
+	GEN_CMD_CODE(_Read_RFREG),
+	GEN_CMD_CODE(_Write_RFREG), /*5*/
+	GEN_CMD_CODE(_Read_EEPROM),
+	GEN_CMD_CODE(_Write_EEPROM),
+	GEN_CMD_CODE(_Read_EFUSE),
+	GEN_CMD_CODE(_Write_EFUSE),
+
+	GEN_CMD_CODE(_Read_CAM),	/*10*/
+	GEN_CMD_CODE(_Write_CAM),
+	GEN_CMD_CODE(_setBCNITV),
+	GEN_CMD_CODE(_setMBIDCFG),
+	GEN_CMD_CODE(_JoinBss),   /*14*/
+	GEN_CMD_CODE(_DisConnect), /*15*/
+	GEN_CMD_CODE(_CreateBss),
+	GEN_CMD_CODE(_SetOpMode),
+	GEN_CMD_CODE(_SiteSurvey),  /*18*/
+	GEN_CMD_CODE(_SetAuth),
+
+	GEN_CMD_CODE(_SetKey),	/*20*/
+	GEN_CMD_CODE(_SetStaKey),
+	GEN_CMD_CODE(_SetAssocSta),
+	GEN_CMD_CODE(_DelAssocSta),
+	GEN_CMD_CODE(_SetStaPwrState),
+	GEN_CMD_CODE(_SetBasicRate), /*25*/
+	GEN_CMD_CODE(_GetBasicRate),
+	GEN_CMD_CODE(_SetDataRate),
+	GEN_CMD_CODE(_GetDataRate),
+	GEN_CMD_CODE(_SetPhyInfo),
+
+	GEN_CMD_CODE(_GetPhyInfo),	/*30*/
+	GEN_CMD_CODE(_SetPhy),
+	GEN_CMD_CODE(_GetPhy),
+	GEN_CMD_CODE(_readRssi),
+	GEN_CMD_CODE(_readGain),
+	GEN_CMD_CODE(_SetAtim), /*35*/
+	GEN_CMD_CODE(_SetPwrMode),
+	GEN_CMD_CODE(_JoinbssRpt),
+	GEN_CMD_CODE(_SetRaTable),
+	GEN_CMD_CODE(_GetRaTable),
+
+	GEN_CMD_CODE(_GetCCXReport), /*40*/
+	GEN_CMD_CODE(_GetDTMReport),
+	GEN_CMD_CODE(_GetTXRateStatistics),
+	GEN_CMD_CODE(_SetUsbSuspend),
+	GEN_CMD_CODE(_SetH2cLbk),
+	GEN_CMD_CODE(_AddBAReq), /*45*/
+	GEN_CMD_CODE(_SetChannel), /*46*/
+	GEN_CMD_CODE(_SetTxPower),
+	GEN_CMD_CODE(_SwitchAntenna),
+	GEN_CMD_CODE(_SetCrystalCap),
+	GEN_CMD_CODE(_SetSingleCarrierTx), /*50*/
+
+	GEN_CMD_CODE(_SetSingleToneTx),/*51*/
+	GEN_CMD_CODE(_SetCarrierSuppressionTx),
+	GEN_CMD_CODE(_SetContinuousTx),
+	GEN_CMD_CODE(_SwitchBandwidth), /*54*/
+	GEN_CMD_CODE(_TX_Beacon), /*55*/
+
+	GEN_CMD_CODE(_Set_MLME_EVT), /*56*/
+	GEN_CMD_CODE(_Set_Drv_Extra), /*57*/
+	GEN_CMD_CODE(_Set_H2C_MSG), /*58*/
+
+	GEN_CMD_CODE(_SetChannelPlan), /*59*/
+
+	GEN_CMD_CODE(_SetChannelSwitch), /*60*/
+	GEN_CMD_CODE(_TDLS), /*61*/
+	GEN_CMD_CODE(_ChkBMCSleepq), /*62*/
+
+	GEN_CMD_CODE(_RunInThreadCMD), /*63*/
+
+	MAX_H2CCMD
+};
+
+#define _GetBBReg_CMD_		_Read_BBREG_CMD_
+#define _SetBBReg_CMD_		_Write_BBREG_CMD_
+#define _GetRFReg_CMD_		_Read_RFREG_CMD_
+#define _SetRFReg_CMD_		_Write_RFREG_CMD_
+
+#endif /*  _CMD_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_debug.h b/drivers/staging/rtl8723bs/include/rtw_debug.h
new file mode 100644
index 000000000..7f96ff669
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_debug.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_DEBUG_H__
+#define __RTW_DEBUG_H__
+
+void mac_reg_dump(struct adapter *adapter);
+void bb_reg_dump(struct adapter *adapter);
+void rf_reg_dump(struct adapter *adapter);
+
+#endif	/* __RTW_DEBUG_H__ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_eeprom.h b/drivers/staging/rtl8723bs/include/rtw_eeprom.h
new file mode 100644
index 000000000..9b84105af
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_eeprom.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_EEPROM_H__
+#define __RTW_EEPROM_H__
+
+
+#define	RTL8712_EEPROM_ID			0x8712
+/* define	EEPROM_MAX_SIZE			256 */
+
+#define	HWSET_MAX_SIZE_128		128
+#define	HWSET_MAX_SIZE_256		256
+#define	HWSET_MAX_SIZE_512		512
+
+#define	EEPROM_MAX_SIZE			HWSET_MAX_SIZE_512
+
+#define	CLOCK_RATE					50			/* 100us */
+
+/*  EEPROM opcodes */
+#define EEPROM_READ_OPCODE		06
+#define EEPROM_WRITE_OPCODE		05
+#define EEPROM_ERASE_OPCODE		07
+#define EEPROM_EWEN_OPCODE		19      /*  Erase/write enable */
+#define EEPROM_EWDS_OPCODE		16      /*  Erase/write disable */
+
+/* Country codes */
+#define USA							0x555320
+#define EUROPE						0x1 /* temp, should be provided later */
+#define JAPAN						0x2 /* temp, should be provided later */
+
+#define eeprom_cis0_sz	17
+#define eeprom_cis1_sz	50
+
+/*  */
+/*  Customer ID, note that: */
+/*  This variable is initiailzed through EEPROM or registry, */
+/*  however, its definition may be different with that in EEPROM for */
+/*  EEPROM size consideration. So, we have to perform proper translation between them. */
+/*  Besides, CustomerID of registry has precedence of that of EEPROM. */
+/*  defined below. 060703, by rcnjko. */
+/*  */
+enum {
+	RT_CID_DEFAULT = 0,
+	RT_CID_8187_ALPHA0 = 1,
+	RT_CID_8187_SERCOMM_PS = 2,
+	RT_CID_8187_HW_LED = 3,
+	RT_CID_8187_NETGEAR = 4,
+	RT_CID_WHQL = 5,
+	RT_CID_819x_CAMEO  = 6,
+	RT_CID_819x_RUNTOP = 7,
+	RT_CID_819x_Senao = 8,
+	RT_CID_TOSHIBA = 9,	/*  Merge by Jacken, 2008/01/31. */
+	RT_CID_819x_Netcore = 10,
+	RT_CID_Nettronix = 11,
+	RT_CID_DLINK = 12,
+	RT_CID_PRONET = 13,
+	RT_CID_COREGA = 14,
+	RT_CID_CHINA_MOBILE = 15,
+	RT_CID_819x_ALPHA = 16,
+	RT_CID_819x_Sitecom = 17,
+	RT_CID_CCX = 18, /*  It's set under CCX logo test and isn't demanded for CCX functions, but for test behavior like retry limit and tx report. By Bruce, 2009-02-17. */
+	RT_CID_819x_Lenovo = 19,
+	RT_CID_819x_QMI = 20,
+	RT_CID_819x_Edimax_Belkin = 21,
+	RT_CID_819x_Sercomm_Belkin = 22,
+	RT_CID_819x_CAMEO1 = 23,
+	RT_CID_819x_MSI = 24,
+	RT_CID_819x_Acer = 25,
+	RT_CID_819x_AzWave_ASUS = 26,
+	RT_CID_819x_AzWave = 27, /*  For AzWave in PCIe, The ID is AzWave use and not only Asus */
+	RT_CID_819x_HP = 28,
+	RT_CID_819x_WNC_COREGA = 29,
+	RT_CID_819x_Arcadyan_Belkin = 30,
+	RT_CID_819x_SAMSUNG = 31,
+	RT_CID_819x_CLEVO = 32,
+	RT_CID_819x_DELL = 33,
+	RT_CID_819x_PRONETS = 34,
+	RT_CID_819x_Edimax_ASUS = 35,
+	RT_CID_NETGEAR = 36,
+	RT_CID_PLANEX = 37,
+	RT_CID_CC_C = 38,
+	RT_CID_819x_Xavi = 39,
+	RT_CID_LENOVO_CHINA = 40,
+	RT_CID_INTEL_CHINA = 41,
+	RT_CID_TPLINK_HPWR = 42,
+	RT_CID_819x_Sercomm_Netgear = 43,
+	RT_CID_819x_ALPHA_Dlink = 44,/* add by ylb 20121012 for customer led for alpha */
+	RT_CID_WNC_NEC = 45,/* add by page for NEC */
+	RT_CID_DNI_BUFFALO = 46,/* add by page for NEC */
+};
+
+struct eeprom_priv {
+	u8 bautoload_fail_flag;
+	u8 bloadfile_fail_flag;
+	u8 bloadmac_fail_flag;
+	u8 EepromOrEfuse;
+
+	u8 mac_addr[6];	/* PermanentAddress */
+
+	u16 	channel_plan;
+	u16 	CustomerID;
+
+	u8 efuse_eeprom_data[EEPROM_MAX_SIZE]; /* 92C:256bytes, 88E:512bytes, we use union set (512bytes) */
+	u8 adjuseVoltageVal;
+
+	u8 EEPROMRFGainOffset;
+	u8 EEPROMRFGainVal;
+
+	u8 sdio_setting;
+	u32 	ocr;
+	u8 cis0[eeprom_cis0_sz];
+	u8 cis1[eeprom_cis1_sz];
+};
+
+#endif  /* __RTL871X_EEPROM_H__ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_efuse.h b/drivers/staging/rtl8723bs/include/rtw_efuse.h
new file mode 100644
index 000000000..5938a6bfb
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_efuse.h
@@ -0,0 +1,124 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_EFUSE_H__
+#define __RTW_EFUSE_H__
+
+
+#define	EFUSE_ERROE_HANDLE		1
+
+#define	PG_STATE_HEADER			0x01
+#define	PG_STATE_WORD_0		0x02
+#define	PG_STATE_WORD_1		0x04
+#define	PG_STATE_WORD_2		0x08
+#define	PG_STATE_WORD_3		0x10
+#define	PG_STATE_DATA			0x20
+
+#define	PG_SWBYTE_H			0x01
+#define	PG_SWBYTE_L			0x02
+
+#define	PGPKT_DATA_SIZE		8
+
+#define	EFUSE_WIFI				0
+#define	EFUSE_BT				1
+
+enum {
+	TYPE_EFUSE_MAX_SECTION				= 0,
+	TYPE_EFUSE_REAL_CONTENT_LEN			= 1,
+	TYPE_AVAILABLE_EFUSE_BYTES_BANK		= 2,
+	TYPE_AVAILABLE_EFUSE_BYTES_TOTAL	= 3,
+	TYPE_EFUSE_MAP_LEN					= 4,
+	TYPE_EFUSE_PROTECT_BYTES_BANK		= 5,
+	TYPE_EFUSE_CONTENT_LEN_BANK			= 6,
+};
+
+#define		EFUSE_MAX_MAP_LEN		512
+
+#define		EFUSE_MAX_HW_SIZE		512
+#define		EFUSE_MAX_SECTION_BASE	16
+
+#define EXT_HEADER(header) ((header & 0x1F) == 0x0F)
+#define ALL_WORDS_DISABLED(wde)	((wde & 0x0F) == 0x0F)
+#define GET_HDR_OFFSET_2_0(header) ((header & 0xE0) >> 5)
+
+#define		EFUSE_REPEAT_THRESHOLD_			3
+
+/*  */
+/* 	The following is for BT Efuse definition */
+/*  */
+#define		EFUSE_BT_MAX_MAP_LEN		1024
+#define		EFUSE_MAX_BANK			4
+#define		EFUSE_MAX_BT_BANK		(EFUSE_MAX_BANK-1)
+/*  */
+/*--------------------------Define Parameters-------------------------------*/
+#define		EFUSE_MAX_WORD_UNIT			4
+
+/*------------------------------Define structure----------------------------*/
+struct pgpkt_struct {
+	u8 offset;
+	u8 word_en;
+	u8 data[8];
+	u8 word_cnts;
+};
+
+/*------------------------------Define structure----------------------------*/
+struct efuse_hal {
+	u8 fakeEfuseBank;
+	u32 fakeEfuseUsedBytes;
+	u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE];
+	u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN];
+	u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN];
+
+	u16 BTEfuseUsedBytes;
+	u8 BTEfuseUsedPercentage;
+	u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+	u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN];
+	u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN];
+
+	u16 fakeBTEfuseUsedBytes;
+	u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+	u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN];
+	u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN];
+};
+
+
+/*------------------------Export global variable----------------------------*/
+extern u8 fakeEfuseBank;
+extern u32 fakeEfuseUsedBytes;
+extern u8 fakeEfuseContent[];
+extern u8 fakeEfuseInitMap[];
+extern u8 fakeEfuseModifiedMap[];
+
+extern u32 BTEfuseUsedBytes;
+extern u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+extern u8 BTEfuseInitMap[];
+extern u8 BTEfuseModifiedMap[];
+
+extern u32 fakeBTEfuseUsedBytes;
+extern u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+extern u8 fakeBTEfuseInitMap[];
+extern u8 fakeBTEfuseModifiedMap[];
+/*------------------------Export global variable----------------------------*/
+
+u16 Efuse_GetCurrentSize(struct adapter *padapter, u8 efuseType, bool bPseudoTest);
+u8 Efuse_CalculateWordCnts(u8 word_en);
+void EFUSE_GetEfuseDefinition(struct adapter *padapter, u8 efuseType, u8 type, void *pOut, bool bPseudoTest);
+u8 efuse_OneByteRead(struct adapter *padapter, u16 addr, u8 *data, bool	 bPseudoTest);
+u8 efuse_OneByteWrite(struct adapter *padapter, u16 addr, u8 data, bool	 bPseudoTest);
+
+void Efuse_PowerSwitch(struct adapter *padapter, u8 bWrite, u8  PwrState);
+int	Efuse_PgPacketRead(struct adapter *padapter, u8 offset, u8 *data, bool bPseudoTest);
+int	Efuse_PgPacketWrite(struct adapter *padapter, u8 offset, u8 word_en, u8 *data, bool bPseudoTest);
+void efuse_WordEnableDataRead(u8 word_en, u8 *sourdata, u8 *targetdata);
+u8 Efuse_WordEnableDataWrite(struct adapter *padapter, u16 efuse_addr, u8 word_en, u8 *data, bool bPseudoTest);
+
+u8 EFUSE_Read1Byte(struct adapter *padapter, u16 Address);
+void EFUSE_ShadowMapUpdate(struct adapter *padapter, u8 efuseType, bool bPseudoTest);
+void EFUSE_ShadowRead(struct adapter *padapter, u8 Type, u16 Offset, u32 *Value);
+void Rtw_Hal_ReadMACAddrFromFile(struct adapter *padapter);
+u32 Rtw_Hal_readPGDataFromConfigFile(struct adapter *padapter);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_event.h b/drivers/staging/rtl8723bs/include/rtw_event.h
new file mode 100644
index 000000000..d48bae541
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_event.h
@@ -0,0 +1,96 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTW_EVENT_H_
+#define _RTW_EVENT_H_
+
+/*
+Used to report a bss has been scanned
+
+*/
+struct survey_event	{
+	struct wlan_bssid_ex bss;
+};
+
+/*
+Used to report that the requested site survey has been done.
+
+bss_cnt indicates the number of bss that has been reported.
+
+
+*/
+struct surveydone_event {
+	unsigned int	bss_cnt;
+
+};
+
+/*
+Used to report the link result of joinning the given bss
+
+
+join_res:
+-1: authentication fail
+-2: association fail
+> 0: TID
+
+*/
+struct joinbss_event {
+	struct	wlan_network	network;
+};
+
+/*
+Used to report a given STA has joinned the created BSS.
+It is used in AP/Ad-HoC(M) mode.
+
+
+*/
+struct stassoc_event {
+	unsigned char macaddr[6];
+	unsigned char rsvd[2];
+	int    cam_id;
+
+};
+
+struct stadel_event {
+ unsigned char macaddr[6];
+ unsigned char rsvd[2]; /* for reason */
+ int mac_id;
+};
+
+struct wmm_event {
+	unsigned char wmm;
+};
+
+#define GEN_EVT_CODE(event)	event ## _EVT_
+
+
+
+struct fwevent {
+	u32 parmsize;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+};
+
+
+#define C2HEVENT_SZ			32
+
+struct event_node {
+	unsigned char *node;
+	unsigned char evt_code;
+	unsigned short evt_sz;
+	volatile int	*caller_ff_tail;
+	int	caller_ff_sz;
+};
+
+#define NETWORK_QUEUE_SZ	4
+
+struct network_queue {
+	volatile int	head;
+	volatile int	tail;
+	struct wlan_bssid_ex networks[NETWORK_QUEUE_SZ];
+};
+
+
+#endif /*  _WLANEVENT_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_ht.h b/drivers/staging/rtl8723bs/include/rtw_ht.h
new file mode 100644
index 000000000..1527d8be2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ht.h
@@ -0,0 +1,105 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTW_HT_H_
+#define _RTW_HT_H_
+
+
+struct ht_priv {
+	u8 ht_option;
+	u8 ampdu_enable;/* for enable Tx A-MPDU */
+	u8 tx_amsdu_enable;/* for enable Tx A-MSDU */
+	u8 bss_coexist;/* for 20/40 Bss coexist */
+
+	/* u8 baddbareq_issued[16]; */
+	u32 tx_amsdu_maxlen; /*  1: 8k, 0:4k ; default:8k, for tx */
+	u32 rx_ampdu_maxlen; /* for rx reordering ctrl win_sz, updated when join_callback. */
+
+	u8 rx_ampdu_min_spacing;
+
+	u8 ch_offset;/* PRIME_CHNL_OFFSET */
+	u8 sgi_20m;
+	u8 sgi_40m;
+
+	/* for processing Tx A-MPDU */
+	u8 agg_enable_bitmap;
+	/* u8 ADDBA_retry_count; */
+	u8 candidate_tid_bitmap;
+
+	u8 ldpc_cap;
+	u8 stbc_cap;
+	u8 beamform_cap;
+
+	struct ieee80211_ht_cap ht_cap;
+
+};
+
+enum {
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+};
+
+enum {
+	RT_HT_CAP_USE_TURBO_AGGR = 0x01,
+	RT_HT_CAP_USE_LONG_PREAMBLE = 0x02,
+	RT_HT_CAP_USE_AMPDU = 0x04,
+	RT_HT_CAP_USE_WOW = 0x8,
+	RT_HT_CAP_USE_SOFTAP = 0x10,
+	RT_HT_CAP_USE_92SE = 0x20,
+	RT_HT_CAP_USE_88C_92C = 0x40,
+	RT_HT_CAP_USE_AP_CLIENT_MODE = 0x80,	/*  AP team request to reserve this bit, by Emily */
+};
+
+enum {
+	RT_HT_CAP_USE_VIDEO_CLIENT = 0x01,
+	RT_HT_CAP_USE_JAGUAR_BCUT = 0x02,
+	RT_HT_CAP_USE_JAGUAR_CCUT = 0x04,
+};
+
+#define	LDPC_HT_ENABLE_RX			BIT0
+#define	LDPC_HT_ENABLE_TX			BIT1
+#define	LDPC_HT_TEST_TX_ENABLE		BIT2
+#define	LDPC_HT_CAP_TX				BIT3
+
+#define	STBC_HT_ENABLE_RX			BIT0
+#define	STBC_HT_ENABLE_TX			BIT1
+#define	STBC_HT_TEST_TX_ENABLE		BIT2
+#define	STBC_HT_CAP_TX				BIT3
+
+#define	BEAMFORMING_HT_BEAMFORMER_ENABLE	BIT0	/*  Declare our NIC supports beamformer */
+#define	BEAMFORMING_HT_BEAMFORMEE_ENABLE	BIT1	/*  Declare our NIC supports beamformee */
+#define	BEAMFORMING_HT_BEAMFORMER_TEST		BIT2	/*  Transmiting Beamforming no matter the target supports it or not */
+
+/*  */
+/*  The HT Control field */
+/*  */
+#define SET_HT_CTRL_CSI_STEERING(_pEleStart, _val)					SET_BITS_TO_LE_1BYTE((_pEleStart)+2, 6, 2, _val)
+#define SET_HT_CTRL_NDP_ANNOUNCEMENT(_pEleStart, _val)			SET_BITS_TO_LE_1BYTE((_pEleStart)+3, 0, 1, _val)
+#define GET_HT_CTRL_NDP_ANNOUNCEMENT(_pEleStart)					LE_BITS_TO_1BYTE((_pEleStart)+3, 0, 1)
+
+/*  20/40 BSS Coexist */
+#define SET_EXT_CAPABILITY_ELE_BSS_COEXIST(_pEleStart, _val)			SET_BITS_TO_LE_1BYTE((_pEleStart), 0, 1, _val)
+#define GET_EXT_CAPABILITY_ELE_BSS_COEXIST(_pEleStart)				LE_BITS_TO_1BYTE((_pEleStart), 0, 1)
+
+
+#define GET_HT_CAPABILITY_ELE_LDPC_CAP(_pEleStart)				LE_BITS_TO_1BYTE(_pEleStart, 0, 1)
+#define GET_HT_CAPABILITY_ELE_TX_STBC(_pEleStart)					LE_BITS_TO_1BYTE(_pEleStart, 7, 1)
+
+#define GET_HT_CAPABILITY_ELE_RX_STBC(_pEleStart)					LE_BITS_TO_1BYTE((_pEleStart)+1, 0, 2)
+
+/* TXBF Capabilities */
+#define SET_HT_CAP_TXBF_RECEIVE_NDP_CAP(_pEleStart, _val)					SET_BITS_TO_LE_4BYTE(((u8 *)(_pEleStart))+21, 3, 1, ((u8)_val))
+#define SET_HT_CAP_TXBF_TRANSMIT_NDP_CAP(_pEleStart, _val)				SET_BITS_TO_LE_4BYTE(((u8 *)(_pEleStart))+21, 4, 1, ((u8)_val))
+#define SET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(_pEleStart, _val)		SET_BITS_TO_LE_4BYTE(((u8 *)(_pEleStart))+21, 10, 1, ((u8)_val))
+#define SET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(_pEleStart, _val)		SET_BITS_TO_LE_4BYTE(((u8 *)(_pEleStart))+21, 15, 2, ((u8)_val))
+#define SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(_pEleStart, _val)	SET_BITS_TO_LE_4BYTE(((u8 *)(_pEleStart))+21, 23, 2, ((u8)_val))
+
+#define GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(_pEleStart)			LE_BITS_TO_4BYTE((_pEleStart)+21, 10, 1)
+#define GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(_pEleStart)			LE_BITS_TO_4BYTE((_pEleStart)+21, 15, 2)
+
+#endif	/* _RTL871X_HT_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_io.h b/drivers/staging/rtl8723bs/include/rtw_io.h
new file mode 100644
index 000000000..e98083a07
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_io.h
@@ -0,0 +1,223 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#ifndef _RTW_IO_H_
+#define _RTW_IO_H_
+
+#define NUM_IOREQ		8
+
+#define MAX_PROT_SZ	(64-16)
+
+#define _IOREADY			0
+#define _IO_WAIT_COMPLETE   1
+#define _IO_WAIT_RSP        2
+
+/*  IO COMMAND TYPE */
+#define _IOSZ_MASK_		(0x7F)
+#define _IO_WRITE_		BIT(7)
+#define _IO_FIXED_		BIT(8)
+#define _IO_BURST_		BIT(9)
+#define _IO_BYTE_		BIT(10)
+#define _IO_HW_			BIT(11)
+#define _IO_WORD_		BIT(12)
+#define _IO_SYNC_		BIT(13)
+#define _IO_CMDMASK_	(0x1F80)
+
+
+/*
+	For prompt mode accessing, caller shall free io_req
+	Otherwise, io_handler will free io_req
+*/
+
+
+
+/*  IO STATUS TYPE */
+#define _IO_ERR_		BIT(2)
+#define _IO_SUCCESS_	BIT(1)
+#define _IO_DONE_		BIT(0)
+
+
+#define IO_RD32			(_IO_SYNC_ | _IO_WORD_)
+#define IO_RD16			(_IO_SYNC_ | _IO_HW_)
+#define IO_RD8			(_IO_SYNC_ | _IO_BYTE_)
+
+#define IO_RD32_ASYNC	(_IO_WORD_)
+#define IO_RD16_ASYNC	(_IO_HW_)
+#define IO_RD8_ASYNC	(_IO_BYTE_)
+
+#define IO_WR32			(_IO_WRITE_ | _IO_SYNC_ | _IO_WORD_)
+#define IO_WR16			(_IO_WRITE_ | _IO_SYNC_ | _IO_HW_)
+#define IO_WR8			(_IO_WRITE_ | _IO_SYNC_ | _IO_BYTE_)
+
+#define IO_WR32_ASYNC	(_IO_WRITE_ | _IO_WORD_)
+#define IO_WR16_ASYNC	(_IO_WRITE_ | _IO_HW_)
+#define IO_WR8_ASYNC	(_IO_WRITE_ | _IO_BYTE_)
+
+/*
+
+	Only Sync. burst accessing is provided.
+
+*/
+
+#define IO_WR_BURST(x)		(_IO_WRITE_ | _IO_SYNC_ | _IO_BURST_ | ((x) & _IOSZ_MASK_))
+#define IO_RD_BURST(x)		(_IO_SYNC_ | _IO_BURST_ | ((x) & _IOSZ_MASK_))
+
+
+
+/* below is for the intf_option bit defition... */
+
+#define _INTF_ASYNC_	BIT(0)	/* support async io */
+
+struct intf_priv;
+struct intf_hdl;
+struct io_queue;
+
+struct _io_ops {
+		u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+		u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+		u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+
+		int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+		int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+		int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+		int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
+
+		int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+		int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+		int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+
+		void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+		void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+
+		void (*_sync_irp_protocol_rw)(struct io_queue *pio_q);
+
+		u32 (*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
+
+		u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+		u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+
+		u32 (*_write_scsi)(struct intf_hdl *pintfhdl, u32 cnt, u8 *pmem);
+
+		void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
+		void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
+};
+
+struct io_req {
+	struct list_head	list;
+	u32 addr;
+	volatile u32 val;
+	u32 command;
+	u32 status;
+	u8 *pbuf;
+
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt);
+	u8 *cnxt;
+};
+
+struct	intf_hdl {
+	struct adapter *padapter;
+	struct dvobj_priv *pintf_dev;/* 	pointer to &(padapter->dvobjpriv); */
+
+	struct _io_ops	io_ops;
+};
+
+#define SD_IO_TRY_CNT (8)
+#define MAX_CONTINUAL_IO_ERR SD_IO_TRY_CNT
+
+int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj);
+void rtw_reset_continual_io_error(struct dvobj_priv *dvobj);
+
+/*
+Below is the data structure used by _io_handler
+
+*/
+
+struct io_queue {
+	spinlock_t	lock;
+	struct list_head	free_ioreqs;
+	struct list_head		pending;		/* The io_req list that will be served in the single protocol read/write. */
+	struct list_head		processing;
+	u8 *free_ioreqs_buf; /*  4-byte aligned */
+	u8 *pallocated_free_ioreqs_buf;
+	struct	intf_hdl	intf;
+};
+
+struct io_priv {
+
+	struct adapter *padapter;
+
+	struct intf_hdl intf;
+
+};
+
+extern uint ioreq_flush(struct adapter *adapter, struct io_queue *ioqueue);
+extern void sync_ioreq_enqueue(struct io_req *preq, struct io_queue *ioqueue);
+extern uint sync_ioreq_flush(struct adapter *adapter, struct io_queue *ioqueue);
+
+
+extern uint free_ioreq(struct io_req *preq, struct io_queue *pio_queue);
+extern struct io_req *alloc_ioreq(struct io_queue *pio_q);
+
+extern uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl);
+extern void unregister_intf_hdl(struct intf_hdl *pintfhdl);
+
+extern void _rtw_attrib_read(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+extern void _rtw_attrib_write(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+extern u8 rtw_read8(struct adapter *adapter, u32 addr);
+extern u16 rtw_read16(struct adapter *adapter, u32 addr);
+extern u32 rtw_read32(struct adapter *adapter, u32 addr);
+
+extern int rtw_write8(struct adapter *adapter, u32 addr, u8 val);
+extern int rtw_write16(struct adapter *adapter, u32 addr, u16 val);
+extern int rtw_write32(struct adapter *adapter, u32 addr, u32 val);
+
+extern u32 rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+extern void rtw_write_scsi(struct adapter *adapter, u32 cnt, u8 *pmem);
+
+/* ioreq */
+extern void ioreq_read8(struct adapter *adapter, u32 addr, u8 *pval);
+extern void ioreq_read16(struct adapter *adapter, u32 addr, u16 *pval);
+extern void ioreq_read32(struct adapter *adapter, u32 addr, u32 *pval);
+extern void ioreq_write8(struct adapter *adapter, u32 addr, u8 val);
+extern void ioreq_write16(struct adapter *adapter, u32 addr, u16 val);
+extern void ioreq_write32(struct adapter *adapter, u32 addr, u32 val);
+
+
+extern uint async_read8(struct adapter *adapter, u32 addr, u8 *pbuff,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+extern uint async_read16(struct adapter *adapter, u32 addr,  u8 *pbuff,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+extern uint async_read32(struct adapter *adapter, u32 addr,  u8 *pbuff,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+
+extern void async_read_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+extern void async_read_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+extern void async_write8(struct adapter *adapter, u32 addr, u8 val,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+extern void async_write16(struct adapter *adapter, u32 addr, u16 val,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+extern void async_write32(struct adapter *adapter, u32 addr, u32 val,
+	void (*_async_io_callback)(struct adapter *padater, struct io_req *pio_req, u8 *cnxt), u8 *cnxt);
+
+extern void async_write_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+extern void async_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+
+int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct adapter *padapter, struct _io_ops *pops));
+
+
+extern uint alloc_io_queue(struct adapter *adapter);
+extern void free_io_queue(struct adapter *adapter);
+extern void async_bus_io(struct io_queue *pio_q);
+extern void bus_sync_io(struct io_queue *pio_q);
+extern u32 _ioreq2rwmem(struct io_queue *pio_q);
+extern void dev_power_down(struct adapter *Adapter, u8 bpwrup);
+
+#endif	/* _RTL8711_IO_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_ioctl_set.h b/drivers/staging/rtl8723bs/include/rtw_ioctl_set.h
new file mode 100644
index 000000000..ab349de73
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ioctl_set.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_IOCTL_SET_H_
+#define __RTW_IOCTL_SET_H_
+
+
+typedef u8 NDIS_802_11_PMKID_VALUE[16];
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *pdapter, enum ndis_802_11_authentication_mode authmode);
+u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct ndis_802_11_wep *wep);
+u8 rtw_set_802_11_disassociate(struct adapter *padapter);
+u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num);
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter, enum ndis_802_11_network_infrastructure networktype);
+u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid);
+u8 rtw_set_802_11_connect(struct adapter *padapter, u8 *bssid, struct ndis_802_11_ssid *ssid);
+
+u8 rtw_validate_bssid(u8 *bssid);
+u8 rtw_validate_ssid(struct ndis_802_11_ssid *ssid);
+
+u8 rtw_do_join(struct adapter *padapter);
+
+u16 rtw_get_cur_max_rate(struct adapter *adapter);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_mlme.h b/drivers/staging/rtl8723bs/include/rtw_mlme.h
new file mode 100644
index 000000000..1b343b434
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme.h
@@ -0,0 +1,603 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_MLME_H_
+#define __RTW_MLME_H_
+
+
+#define	MAX_BSS_CNT	128
+/* define   MAX_JOIN_TIMEOUT	2000 */
+/* define   MAX_JOIN_TIMEOUT	2500 */
+#define   MAX_JOIN_TIMEOUT	6500
+
+/* 	Commented by Albert 20101105 */
+/* 	Increase the scanning timeout because of increasing the SURVEY_TO value. */
+
+#define		SCANNING_TIMEOUT	8000
+
+#ifdef PALTFORM_OS_WINCE
+#define	SCANQUEUE_LIFETIME 12000000 /*  unit:us */
+#else
+#define	SCANQUEUE_LIFETIME 20000 /*  20sec, unit:msec */
+#endif
+
+#define WIFI_NULL_STATE		0x00000000
+#define WIFI_ASOC_STATE		0x00000001		/*  Under Linked state... */
+#define WIFI_REASOC_STATE	0x00000002
+#define WIFI_SLEEP_STATE	0x00000004
+#define WIFI_STATION_STATE	0x00000008
+#define	WIFI_AP_STATE			0x00000010
+#define	WIFI_ADHOC_STATE		0x00000020
+#define WIFI_ADHOC_MASTER_STATE	0x00000040
+#define WIFI_UNDER_LINKING	0x00000080
+
+#define WIFI_UNDER_WPS			0x00000100
+/* define	WIFI_UNDER_CMD			0x00000200 */
+/* define	WIFI_UNDER_P2P			0x00000400 */
+#define	WIFI_STA_ALIVE_CHK_STATE	0x00000400
+#define	WIFI_SITE_MONITOR			0x00000800		/* to indicate the station is under site surveying */
+#ifdef WDS
+#define	WIFI_WDS				0x00001000
+#define	WIFI_WDS_RX_BEACON	0x00002000		/*  already rx WDS AP beacon */
+#endif
+#ifdef AUTO_CONFIG
+#define	WIFI_AUTOCONF			0x00004000
+#define	WIFI_AUTOCONF_IND	0x00008000
+#endif
+
+/**
+*  ========== P2P Section Start ===============
+#define	WIFI_P2P_LISTEN_STATE		0x00010000
+#define	WIFI_P2P_GROUP_FORMATION_STATE		0x00020000
+  ========== P2P Section End ===============
+*/
+
+/* ifdef UNDER_MPTEST */
+#define	WIFI_MP_STATE							0x00010000
+#define	WIFI_MP_CTX_BACKGROUND				0x00020000	/*  in continuous tx background */
+#define	WIFI_MP_CTX_ST						0x00040000	/*  in continuous tx with single-tone */
+#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000	/*  pending in continuous tx background due to out of skb */
+#define	WIFI_MP_CTX_CCK_HW					0x00100000	/*  in continuous tx */
+#define	WIFI_MP_CTX_CCK_CS					0x00200000	/*  in continuous tx with carrier suppression */
+#define   WIFI_MP_LPBK_STATE					0x00400000
+/* endif */
+
+/* define _FW_UNDER_CMD		WIFI_UNDER_CMD */
+#define _FW_UNDER_LINKING	WIFI_UNDER_LINKING
+#define _FW_LINKED			WIFI_ASOC_STATE
+#define _FW_UNDER_SURVEY	WIFI_SITE_MONITOR
+
+
+enum {
+ dot11AuthAlgrthm_Open = 0,
+ dot11AuthAlgrthm_Shared,
+ dot11AuthAlgrthm_8021X,
+ dot11AuthAlgrthm_Auto,
+ dot11AuthAlgrthm_WAPI,
+ dot11AuthAlgrthm_MaxNum
+};
+
+/*  Scan type including active and passive scan. */
+enum rt_scan_type {
+	SCAN_PASSIVE,
+	SCAN_ACTIVE,
+	SCAN_MIX,
+};
+
+enum {
+	GHZ24_50 = 0,
+	GHZ_50,
+	GHZ_24,
+	GHZ_MAX,
+};
+
+#define rtw_band_valid(band) ((band) >= GHZ24_50 && (band) < GHZ_MAX)
+
+/*
+
+there are several "locks" in mlme_priv,
+since mlme_priv is a shared resource between many threads,
+like ISR/Call-Back functions, the OID handlers, and even timer functions.
+
+Each struct __queue has its own locks, already.
+Other items in mlme_priv are protected by mlme_priv.lock, while items in
+xmit_priv are protected by xmit_priv.lock.
+
+To avoid possible dead lock, any thread trying to modifiying mlme_priv
+SHALL not lock up more than one locks at a time!
+
+The only exception is that queue functions which take the __queue.lock
+may be called with the xmit_priv.lock held. In this case the order
+MUST always be first lock xmit_priv.lock and then call any queue functions
+which take __queue.lock.
+*/
+
+
+#define traffic_threshold	10
+#define	traffic_scan_period	500
+
+struct sitesurvey_ctrl {
+	u64	last_tx_pkts;
+	uint	last_rx_pkts;
+	signed int	traffic_busy;
+	struct timer_list	sitesurvey_ctrl_timer;
+};
+
+struct rt_link_detect_t {
+	u32 			NumTxOkInPeriod;
+	u32 			NumRxOkInPeriod;
+	u32 			NumRxUnicastOkInPeriod;
+	bool			bBusyTraffic;
+	bool			bTxBusyTraffic;
+	bool			bRxBusyTraffic;
+	bool			bHigherBusyTraffic; /*  For interrupt migration purpose. */
+	bool			bHigherBusyRxTraffic; /*  We may disable Tx interrupt according as Rx traffic. */
+	bool			bHigherBusyTxTraffic; /*  We may disable Tx interrupt according as Tx traffic. */
+	/* u8 TrafficBusyState; */
+	u8 TrafficTransitionCount;
+	u32 LowPowerTransitionCount;
+};
+
+struct profile_info {
+	u8 ssidlen;
+	u8 ssid[WLAN_SSID_MAXLEN];
+	u8 peermac[ETH_ALEN];
+};
+
+struct tx_invite_req_info {
+	u8 			token;
+	u8 			benable;
+	u8			go_ssid[WLAN_SSID_MAXLEN];
+	u8 			ssidlen;
+	u8			go_bssid[ETH_ALEN];
+	u8			peer_macaddr[ETH_ALEN];
+	u8 			operating_ch;	/* 	This information will be set by using the p2p_set op_ch =x */
+	u8 			peer_ch;		/* 	The listen channel for peer P2P device */
+
+};
+
+struct tx_invite_resp_info {
+	u8 			token;	/* 	Used to record the dialog token of p2p invitation request frame. */
+};
+
+struct tx_provdisc_req_info {
+	u16 				wps_config_method_request;	/* 	Used when sending the provisioning request frame */
+	u16 				peer_channel_num[2];		/* 	The channel number which the receiver stands. */
+	struct ndis_802_11_ssid	ssid;
+	u8			peerDevAddr[ETH_ALEN];		/*	Peer device address */
+	u8			peerIFAddr[ETH_ALEN];		/*	Peer interface address */
+	u8 			benable;					/* 	This provision discovery request frame is trigger to send or not */
+};
+
+struct rx_provdisc_req_info {	/* When peer device issue prov_disc_req first, we should store the following information */
+	u8			peerDevAddr[ETH_ALEN];		/*	Peer device address */
+	u8 			strconfig_method_desc_of_prov_disc_req[4];	/* 	description for the config method located in the provisioning discovery request frame. */
+																	/* 	The UI must know this information to know which config method the remote p2p device is requiring. */
+};
+
+struct tx_nego_req_info {
+	u16 				peer_channel_num[2];		/* 	The channel number which the receiver stands. */
+	u8			peerDevAddr[ETH_ALEN];		/*	Peer device address */
+	u8 			benable;					/* 	This negotiation request frame is trigger to send or not */
+};
+
+struct group_id_info {
+	u8			go_device_addr[ETH_ALEN];	/*	The GO's device address of this P2P group */
+	u8			ssid[WLAN_SSID_MAXLEN];	/*	The SSID of this P2P group */
+};
+
+struct scan_limit_info {
+	u8 			scan_op_ch_only;			/* 	When this flag is set, the driver should just scan the operation channel */
+	u8 			operation_ch[2];				/* 	Store the operation channel of invitation request frame */
+};
+
+struct wifidirect_info {
+	struct adapter				*padapter;
+	struct timer_list					find_phase_timer;
+	struct timer_list					restore_p2p_state_timer;
+
+	/* 	Used to do the scanning. After confirming the peer is availalble, the driver transmits the P2P frame to peer. */
+	struct timer_list					pre_tx_scan_timer;
+	struct timer_list					reset_ch_sitesurvey;
+	struct timer_list					reset_ch_sitesurvey2;	/* 	Just for resetting the scan limit function by using p2p nego */
+	struct tx_provdisc_req_info tx_prov_disc_info;
+	struct rx_provdisc_req_info rx_prov_disc_info;
+	struct tx_invite_req_info invitereq_info;
+	struct profile_info		profileinfo[P2P_MAX_PERSISTENT_GROUP_NUM];	/*	Store the profile information of persistent group */
+	struct tx_invite_resp_info inviteresp_info;
+	struct tx_nego_req_info nego_req_info;
+	struct group_id_info 	groupid_info;	/* 	Store the group id information when doing the group negotiation handshake. */
+	struct scan_limit_info 	rx_invitereq_info;	/* 	Used for get the limit scan channel from the Invitation procedure */
+	struct scan_limit_info 	p2p_info;		/* 	Used for get the limit scan channel from the P2P negotiation handshake */
+	enum p2p_role			role;
+	enum p2p_state			pre_p2p_state;
+	enum p2p_state			p2p_state;
+	u8 				device_addr[ETH_ALEN];	/* 	The device address should be the mac address of this device. */
+	u8 				interface_addr[ETH_ALEN];
+	u8 				social_chan[4];
+	u8 				listen_channel;
+	u8 				operating_channel;
+	u8 				listen_dwell;		/* 	This value should be between 1 and 3 */
+	u8 				support_rate[8];
+	u8 				p2p_wildcard_ssid[P2P_WILDCARD_SSID_LEN];
+	u8 				intent;		/* 	should only include the intent value. */
+	u8				p2p_peer_interface_addr[ETH_ALEN];
+	u8				p2p_peer_device_addr[ETH_ALEN];
+	u8 				peer_intent;	/* 	Included the intent value and tie breaker value. */
+	u8				device_name[WPS_MAX_DEVICE_NAME_LEN];	/*	Device name for displaying on searching device screen */
+	u8 				device_name_len;
+	u8 				profileindex;	/* 	Used to point to the index of profileinfo array */
+	u8 				peer_operating_ch;
+	u8 				find_phase_state_exchange_cnt;
+	u16 					device_password_id_for_nego;	/* 	The device password ID for group negotiation */
+	u8 				negotiation_dialog_token;
+	u8				nego_ssid[WLAN_SSID_MAXLEN];	/*	SSID information for group negotiation */
+	u8 				nego_ssidlen;
+	u8 				p2p_group_ssid[WLAN_SSID_MAXLEN];
+	u8 				p2p_group_ssid_len;
+	u8 				persistent_supported;		/* 	Flag to know the persistent function should be supported or not. */
+														/* 	In the Sigma test, the Sigma will provide this enable from the sta_set_p2p CAPI. */
+														/* 	0: disable */
+														/* 	1: enable */
+	u8 				session_available;			/* 	Flag to set the WFD session available to enable or disable "by Sigma" */
+														/* 	In the Sigma test, the Sigma will disable the session available by using the sta_preset CAPI. */
+														/* 	0: disable */
+														/* 	1: enable */
+
+	u8 				wfd_tdls_enable;			/* 	Flag to enable or disable the TDLS by WFD Sigma */
+														/* 	0: disable */
+														/* 	1: enable */
+	u8 				wfd_tdls_weaksec;			/* 	Flag to enable or disable the weak security function for TDLS by WFD Sigma */
+														/* 	0: disable */
+														/* 	In this case, the driver can't issue the tdsl setup request frame. */
+														/* 	1: enable */
+														/* 	In this case, the driver can issue the tdls setup request frame */
+														/* 	even the current security is weak security. */
+
+	enum	p2p_wpsinfo		ui_got_wps_info;			/* 	This field will store the WPS value (PIN value or PBC) that UI had got from the user. */
+	u16 					supported_wps_cm;			/* 	This field describes the WPS config method which this driver supported. */
+														/* 	The value should be the combination of config method defined in page104 of WPS v2.0 spec. */
+	u8 				external_uuid;				/*  UUID flag */
+	u8 				uuid[16];					/*  UUID */
+	uint						channel_list_attr_len;		/* 	This field will contain the length of body of P2P Channel List attribute of group negotitation response frame. */
+	u8 				channel_list_attr[100];		/* 	This field will contain the body of P2P Channel List attribute of group negotitation response frame. */
+														/* 	We will use the channel_cnt and channel_list fields when constructing the group negotitation confirm frame. */
+	u8 				driver_interface;			/* 	Indicate DRIVER_WEXT or DRIVER_CFG80211 */
+};
+
+struct tdls_ss_record {	/* signal strength record */
+	u8 macaddr[ETH_ALEN];
+	u8 rx_pwd_ba11;
+	u8 is_tdls_sta;	/*  true: direct link sta, false: else */
+};
+
+/* used for mlme_priv.roam_flags */
+enum {
+	RTW_ROAM_ON_EXPIRED = BIT0,
+	RTW_ROAM_ON_RESUME = BIT1,
+	RTW_ROAM_ACTIVE = BIT2,
+};
+
+struct mlme_priv {
+
+	spinlock_t	lock;
+	signed int	fw_state;	/* shall we protect this variable? maybe not necessarily... */
+	u8 bScanInProcess;
+	u8 to_join; /* flag */
+
+	u8 to_roam; /* roaming trying times */
+	struct wlan_network *roam_network; /* the target of active roam */
+	u8 roam_flags;
+	u8 roam_rssi_diff_th; /* rssi difference threshold for active scan candidate selection */
+	u32 roam_scan_int_ms; /* scan interval for active roam */
+	u32 roam_scanr_exp_ms; /* scan result expire time in ms  for roam */
+	u8 roam_tgt_addr[ETH_ALEN]; /* request to roam to speicific target without other consideration */
+
+	u8 *nic_hdl;
+
+	u8 not_indic_disco;
+	struct list_head		*pscanned;
+	struct __queue	free_bss_pool;
+	struct __queue	scanned_queue;
+	u8 *free_bss_buf;
+	u32 num_of_scanned;
+
+	struct ndis_802_11_ssid	assoc_ssid;
+	u8 assoc_bssid[6];
+
+	struct wlan_network	cur_network;
+	struct wlan_network *cur_network_scanned;
+
+	/* uint wireless_mode; no used, remove it */
+
+	u32 auto_scan_int_ms;
+
+	struct timer_list assoc_timer;
+
+	uint assoc_by_bssid;
+	uint assoc_by_rssi;
+
+	struct timer_list scan_to_timer; /*  driver itself handles scan_timeout status. */
+	unsigned long scan_start_time; /*  used to evaluate the time spent in scanning */
+
+	struct timer_list set_scan_deny_timer;
+	atomic_t set_scan_deny; /* 0: allowed, 1: deny */
+
+	struct qos_priv qospriv;
+
+	/* Number of non-HT AP/stations */
+	int num_sta_no_ht;
+
+	/* Number of HT AP/stations 20 MHz */
+	/* int num_sta_ht_20mhz; */
+
+
+	int num_FortyMHzIntolerant;
+
+	struct ht_priv htpriv;
+
+	struct rt_link_detect_t	LinkDetectInfo;
+	struct timer_list	dynamic_chk_timer; /* dynamic/periodic check timer */
+
+	u8 acm_mask; /*  for wmm acm mask */
+	u8 ChannelPlan;
+	enum rt_scan_type	scan_mode; /*  active: 1, passive: 0 */
+
+	u8 *wps_probe_req_ie;
+	u32 wps_probe_req_ie_len;
+
+	/* Number of associated Non-ERP stations (i.e., stations using 802.11b
+	 * in 802.11g BSS) */
+	int num_sta_non_erp;
+
+	/* Number of associated stations that do not support Short Slot Time */
+	int num_sta_no_short_slot_time;
+
+	/* Number of associated stations that do not support Short Preamble */
+	int num_sta_no_short_preamble;
+
+	int olbc; /* Overlapping Legacy BSS Condition */
+
+	/* Number of HT associated stations that do not support greenfield */
+	int num_sta_ht_no_gf;
+
+	/* Number of associated non-HT stations */
+	/* int num_sta_no_ht; */
+
+	/* Number of HT associated stations 20 MHz */
+	int num_sta_ht_20mhz;
+
+	/* Overlapping BSS information */
+	int olbc_ht;
+
+	u16 ht_op_mode;
+
+	u8 *assoc_req;
+	u32 assoc_req_len;
+	u8 *assoc_rsp;
+	u32 assoc_rsp_len;
+
+	u8 *wps_beacon_ie;
+	/* u8 *wps_probe_req_ie; */
+	u8 *wps_probe_resp_ie;
+	u8 *wps_assoc_resp_ie; /*  for CONFIG_IOCTL_CFG80211, this IE could include p2p ie / wfd ie */
+
+	u32 wps_beacon_ie_len;
+	/* u32 wps_probe_req_ie_len; */
+	u32 wps_probe_resp_ie_len;
+	u32 wps_assoc_resp_ie_len; /*  for CONFIG_IOCTL_CFG80211, this IE len could include p2p ie / wfd ie */
+
+	u8 *p2p_beacon_ie;
+	u8 *p2p_probe_req_ie;
+	u8 *p2p_probe_resp_ie;
+	u8 *p2p_go_probe_resp_ie; /* for GO */
+	u8 *p2p_assoc_req_ie;
+
+	u32 p2p_beacon_ie_len;
+	u32 p2p_probe_req_ie_len;
+	u32 p2p_probe_resp_ie_len;
+	u32 p2p_go_probe_resp_ie_len; /* for GO */
+	u32 p2p_assoc_req_ie_len;
+
+	spinlock_t	bcn_update_lock;
+	u8 update_bcn;
+
+	u8 NumOfBcnInfoChkFail;
+	unsigned long	timeBcnInfoChkStart;
+};
+
+#define rtw_mlme_set_auto_scan_int(adapter, ms) \
+	do { \
+		adapter->mlmepriv.auto_scan_int_ms = ms; \
+	while (0)
+
+void rtw_mlme_reset_auto_scan_int(struct adapter *adapter);
+
+struct hostapd_priv {
+	struct adapter *padapter;
+};
+
+extern int hostapd_mode_init(struct adapter *padapter);
+extern void hostapd_mode_unload(struct adapter *padapter);
+
+extern void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf);
+extern void rtw_survey_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_surveydone_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_atimdone_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_cpwm_event_callback(struct adapter *adapter, u8 *pbuf);
+extern void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf);
+
+extern void rtw_join_timeout_handler(struct timer_list *t);
+extern void _rtw_scan_timeout_handler(struct timer_list *t);
+
+int event_thread(void *context);
+
+extern void rtw_free_network_queue(struct adapter *adapter, u8 isfreeall);
+extern int rtw_init_mlme_priv(struct adapter *adapter);/*  (struct mlme_priv *pmlmepriv); */
+
+extern void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv);
+
+
+extern signed int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv);
+extern signed int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, signed int keyid, u8 set_tx, bool enqueue);
+extern signed int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv);
+
+static inline u8 *get_bssid(struct mlme_priv *pmlmepriv)
+{	/* if sta_mode:pmlmepriv->cur_network.network.mac_address => bssid */
+	/*  if adhoc_mode:pmlmepriv->cur_network.network.mac_address => ibss mac address */
+	return pmlmepriv->cur_network.network.mac_address;
+}
+
+static inline signed int check_fwstate(struct mlme_priv *pmlmepriv, signed int state)
+{
+	if (pmlmepriv->fw_state & state)
+		return true;
+
+	return false;
+}
+
+static inline signed int get_fwstate(struct mlme_priv *pmlmepriv)
+{
+	return pmlmepriv->fw_state;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ *
+ * ### NOTE:#### (!!!!)
+ * MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+static inline void set_fwstate(struct mlme_priv *pmlmepriv, signed int state)
+{
+	pmlmepriv->fw_state |= state;
+	/* FOR HW integration */
+	if (state == _FW_UNDER_SURVEY)
+		pmlmepriv->bScanInProcess = true;
+}
+
+static inline void _clr_fwstate_(struct mlme_priv *pmlmepriv, signed int state)
+{
+	pmlmepriv->fw_state &= ~state;
+	/* FOR HW integration */
+	if (state == _FW_UNDER_SURVEY)
+		pmlmepriv->bScanInProcess = false;
+}
+
+/*
+ * No Limit on the calling context,
+ * therefore set it to be the critical section...
+ */
+static inline void clr_fwstate(struct mlme_priv *pmlmepriv, signed int state)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	if (check_fwstate(pmlmepriv, state) == true)
+		pmlmepriv->fw_state ^= state;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+static inline void set_scanned_network_val(struct mlme_priv *pmlmepriv, signed int val)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	pmlmepriv->num_of_scanned = val;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+extern u16 rtw_get_capability(struct wlan_bssid_ex *bss);
+extern void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target);
+extern void rtw_disconnect_hdl_under_linked(struct adapter *adapter, struct sta_info *psta, u8 free_assoc);
+extern void rtw_generate_random_ibss(u8 *pibss);
+extern struct wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr);
+extern struct wlan_network *rtw_get_oldest_wlan_network(struct __queue *scanned_queue);
+struct wlan_network *_rtw_find_same_network(struct __queue *scanned_queue, struct wlan_network *network);
+
+extern void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue);
+extern void rtw_indicate_disconnect(struct adapter *adapter);
+extern void rtw_indicate_connect(struct adapter *adapter);
+void rtw_indicate_scan_done(struct adapter *padapter, bool aborted);
+void rtw_scan_abort(struct adapter *adapter);
+
+extern int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len);
+extern int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len);
+extern void rtw_init_registrypriv_dev_network(struct adapter *adapter);
+
+extern void rtw_update_registrypriv_dev_network(struct adapter *adapter);
+
+extern void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter);
+
+extern void _rtw_join_timeout_handler(struct timer_list *t);
+extern void rtw_scan_timeout_handler(struct timer_list *t);
+
+extern void rtw_dynamic_check_timer_handler(struct adapter *adapter);
+bool rtw_is_scan_deny(struct adapter *adapter);
+void rtw_clear_scan_deny(struct adapter *adapter);
+void rtw_set_scan_deny(struct adapter *adapter, u32 ms);
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv);
+
+extern void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv);
+
+/* extern struct wlan_network* _rtw_dequeue_network(struct __queue *queue); */
+
+extern struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv);
+
+
+extern void _rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall);
+extern void _rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork);
+
+
+extern struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr);
+
+extern signed int rtw_if_up(struct adapter *padapter);
+
+signed int rtw_linked_check(struct adapter *padapter);
+
+u8 *rtw_get_capability_from_ie(u8 *ie);
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie);
+
+
+void rtw_joinbss_reset(struct adapter *padapter);
+
+void rtw_ht_use_default_setting(struct adapter *padapter);
+void rtw_build_wmm_ie_ht(struct adapter *padapter, u8 *out_ie, uint *pout_len);
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel);
+void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len, u8 channel);
+void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe);
+void rtw_append_exented_cap(struct adapter *padapter, u8 *out_ie, uint *pout_len);
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork);
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst, u8 feature);
+
+#define rtw_roam_flags(adapter) ((adapter)->mlmepriv.roam_flags)
+#define rtw_chk_roam_flags(adapter, flags) ((adapter)->mlmepriv.roam_flags & flags)
+#define rtw_clr_roam_flags(adapter, flags) \
+	do { \
+		((adapter)->mlmepriv.roam_flags &= ~flags); \
+	} while (0)
+
+#define rtw_set_roam_flags(adapter, flags) \
+	do { \
+		((adapter)->mlmepriv.roam_flags |= flags); \
+	} while (0)
+
+#define rtw_assign_roam_flags(adapter, flags) \
+	do { \
+		((adapter)->mlmepriv.roam_flags = flags); \
+	} while (0)
+
+void _rtw_roaming(struct adapter *adapter, struct wlan_network *tgt_network);
+void rtw_roaming(struct adapter *adapter, struct wlan_network *tgt_network);
+void rtw_set_to_roam(struct adapter *adapter, u8 to_roam);
+u8 rtw_dec_to_roam(struct adapter *adapter);
+u8 rtw_to_roam(struct adapter *adapter);
+int rtw_select_roaming_candidate(struct mlme_priv *pmlmepriv);
+
+void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta, u32 mstatus);
+
+#endif /* __RTL871X_MLME_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h b/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
new file mode 100644
index 000000000..65e138a52
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
@@ -0,0 +1,761 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_MLME_EXT_H_
+#define __RTW_MLME_EXT_H_
+
+
+/* 	Commented by Albert 20101105 */
+/* 	Increase the SURVEY_TO value from 100 to 150  (100ms to 150ms) */
+/* 	The Realtek 8188CE SoftAP will spend around 100ms to send the probe response after receiving the probe request. */
+/* 	So, this driver tried to extend the dwell time for each scanning channel. */
+/* 	This will increase the chance to receive the probe response from SoftAP. */
+
+#define SURVEY_TO		(100)
+#define REAUTH_TO		(300) /* 50) */
+#define REASSOC_TO		(300) /* 50) */
+/* define DISCONNECT_TO	(3000) */
+#define ADDBA_TO			(2000)
+
+#define LINKED_TO (1) /* unit:2 sec, 1x2 =2 sec */
+
+#define REAUTH_LIMIT	(4)
+#define REASSOC_LIMIT	(4)
+#define READDBA_LIMIT	(2)
+
+#define ROAMING_LIMIT	8
+/* define	IOCMD_REG0		0x10250370 */
+/* define	IOCMD_REG1		0x10250374 */
+/* define	IOCMD_REG2		0x10250378 */
+
+/* define	FW_DYNAMIC_FUN_SWITCH	0x10250364 */
+
+/* define	WRITE_BB_CMD		0xF0000001 */
+/* define	SET_CHANNEL_CMD	0xF3000000 */
+/* define	UPDATE_RA_CMD	0xFD0000A2 */
+
+#define DYNAMIC_FUNC_DISABLE		(0x0)
+
+/*  ====== ODM_ABILITY_E ======== */
+/*  BB ODM section BIT 0-15 */
+#define DYNAMIC_BB_DIG				BIT0 /* ODM_BB_DIG */
+#define DYNAMIC_BB_RA_MASK			BIT1 /* ODM_BB_RA_MASK */
+#define DYNAMIC_BB_DYNAMIC_TXPWR	BIT2 /* ODM_BB_DYNAMIC_TXPWR */
+#define DYNAMIC_BB_BB_FA_CNT		BIT3 /* ODM_BB_FA_CNT */
+#define DYNAMIC_BB_RSSI_MONITOR		BIT4 /* ODM_BB_RSSI_MONITOR */
+#define DYNAMIC_BB_CCK_PD			BIT5 /* ODM_BB_CCK_PD */
+#define DYNAMIC_BB_ANT_DIV			BIT6 /* ODM_BB_ANT_DIV */
+#define DYNAMIC_BB_PWR_SAVE			BIT7 /* ODM_BB_PWR_SAVE */
+#define DYNAMIC_BB_PWR_TRAIN		BIT8 /* ODM_BB_PWR_TRAIN */
+#define DYNAMIC_BB_RATE_ADAPTIVE	BIT9 /* ODM_BB_RATE_ADAPTIVE */
+#define DYNAMIC_BB_PATH_DIV			BIT10/* ODM_BB_PATH_DIV */
+#define DYNAMIC_BB_PSD				BIT11/* ODM_BB_PSD */
+#define DYNAMIC_BB_RXHP				BIT12/* ODM_BB_RXHP */
+#define DYNAMIC_BB_ADAPTIVITY		BIT13/* ODM_BB_ADAPTIVITY */
+#define DYNAMIC_BB_DYNAMIC_ATC		BIT14/* ODM_BB_DYNAMIC_ATC */
+
+/*  MAC DM section BIT 16-23 */
+#define DYNAMIC_MAC_EDCA_TURBO		BIT16/* ODM_MAC_EDCA_TURBO */
+#define DYNAMIC_MAC_EARLY_MODE		BIT17/* ODM_MAC_EARLY_MODE */
+
+/*  RF ODM section BIT 24-31 */
+#define DYNAMIC_RF_TX_PWR_TRACK		BIT24/* ODM_RF_TX_PWR_TRACK */
+#define DYNAMIC_RF_RX_GAIN_TRACK	BIT25/* ODM_RF_RX_GAIN_TRACK */
+#define DYNAMIC_RF_CALIBRATION		BIT26/* ODM_RF_CALIBRATION */
+
+#define DYNAMIC_ALL_FUNC_ENABLE		0xFFFFFFF
+
+#define _HW_STATE_NOLINK_		0x00
+#define _HW_STATE_ADHOC_		0x01
+#define _HW_STATE_STATION_	0x02
+#define _HW_STATE_AP_			0x03
+
+
+#define		_1M_RATE_	0
+#define		_2M_RATE_	1
+#define		_5M_RATE_	2
+#define		_11M_RATE_	3
+#define		_6M_RATE_	4
+#define		_9M_RATE_	5
+#define		_12M_RATE_	6
+#define		_18M_RATE_	7
+#define		_24M_RATE_	8
+#define		_36M_RATE_	9
+#define		_48M_RATE_	10
+#define		_54M_RATE_	11
+
+/********************************************************
+MCS rate definitions
+*********************************************************/
+#define MCS_RATE_1R	(0x000000ff)
+#define MCS_RATE_2R	(0x0000ffff)
+#define MCS_RATE_3R	(0x00ffffff)
+#define MCS_RATE_4R	(0xffffffff)
+#define MCS_RATE_2R_13TO15_OFF	(0x00001fff)
+
+
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WMM_OUI[];
+extern unsigned char WPS_OUI[];
+extern unsigned char WFD_OUI[];
+extern unsigned char P2P_OUI[];
+
+extern unsigned char WMM_INFO_OUI[];
+extern unsigned char WMM_PARA_OUI[];
+
+
+/*  */
+/*  Channel Plan Type. */
+/*  Note: */
+/* 	We just add new channel plan when the new channel plan is different from any of the following */
+/* 	channel plan. */
+/* 	If you just want to customize the actions(scan period or join actions) about one of the channel plan, */
+/* 	customize them in rt_channel_info in the RT_CHANNEL_LIST. */
+/*  */
+enum {
+	/*  old channel plan mapping ===== */
+	RT_CHANNEL_DOMAIN_FCC = 0x00,
+	RT_CHANNEL_DOMAIN_IC = 0x01,
+	RT_CHANNEL_DOMAIN_ETSI = 0x02,
+	RT_CHANNEL_DOMAIN_SPAIN = 0x03,
+	RT_CHANNEL_DOMAIN_FRANCE = 0x04,
+	RT_CHANNEL_DOMAIN_MKK = 0x05,
+	RT_CHANNEL_DOMAIN_MKK1 = 0x06,
+	RT_CHANNEL_DOMAIN_ISRAEL = 0x07,
+	RT_CHANNEL_DOMAIN_TELEC = 0x08,
+	RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN = 0x09,
+	RT_CHANNEL_DOMAIN_WORLD_WIDE_13 = 0x0A,
+	RT_CHANNEL_DOMAIN_TAIWAN = 0x0B,
+	RT_CHANNEL_DOMAIN_CHINA = 0x0C,
+	RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO = 0x0D,
+	RT_CHANNEL_DOMAIN_KOREA = 0x0E,
+	RT_CHANNEL_DOMAIN_TURKEY = 0x0F,
+	RT_CHANNEL_DOMAIN_JAPAN = 0x10,
+	RT_CHANNEL_DOMAIN_FCC_NO_DFS = 0x11,
+	RT_CHANNEL_DOMAIN_JAPAN_NO_DFS = 0x12,
+	RT_CHANNEL_DOMAIN_WORLD_WIDE_5G = 0x13,
+	RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS = 0x14,
+
+	/*  new channel plan mapping, (2GDOMAIN_5GDOMAIN) ===== */
+	RT_CHANNEL_DOMAIN_WORLD_NULL = 0x20,
+	RT_CHANNEL_DOMAIN_ETSI1_NULL = 0x21,
+	RT_CHANNEL_DOMAIN_FCC1_NULL = 0x22,
+	RT_CHANNEL_DOMAIN_MKK1_NULL = 0x23,
+	RT_CHANNEL_DOMAIN_ETSI2_NULL = 0x24,
+	RT_CHANNEL_DOMAIN_FCC1_FCC1 = 0x25,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI1 = 0x26,
+	RT_CHANNEL_DOMAIN_MKK1_MKK1 = 0x27,
+	RT_CHANNEL_DOMAIN_WORLD_KCC1 = 0x28,
+	RT_CHANNEL_DOMAIN_WORLD_FCC2 = 0x29,
+	RT_CHANNEL_DOMAIN_WORLD_FCC3 = 0x30,
+	RT_CHANNEL_DOMAIN_WORLD_FCC4 = 0x31,
+	RT_CHANNEL_DOMAIN_WORLD_FCC5 = 0x32,
+	RT_CHANNEL_DOMAIN_WORLD_FCC6 = 0x33,
+	RT_CHANNEL_DOMAIN_FCC1_FCC7 = 0x34,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI2 = 0x35,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI3 = 0x36,
+	RT_CHANNEL_DOMAIN_MKK1_MKK2 = 0x37,
+	RT_CHANNEL_DOMAIN_MKK1_MKK3 = 0x38,
+	RT_CHANNEL_DOMAIN_FCC1_NCC1 = 0x39,
+	RT_CHANNEL_DOMAIN_FCC1_NCC2 = 0x40,
+	RT_CHANNEL_DOMAIN_GLOBAL_NULL = 0x41,
+	RT_CHANNEL_DOMAIN_ETSI1_ETSI4 = 0x42,
+	RT_CHANNEL_DOMAIN_FCC1_FCC2 = 0x43,
+	RT_CHANNEL_DOMAIN_FCC1_NCC3 = 0x44,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI5 = 0x45,
+	RT_CHANNEL_DOMAIN_FCC1_FCC8 = 0x46,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI6 = 0x47,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI7 = 0x48,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI8 = 0x49,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI9 = 0x50,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI10 = 0x51,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI11 = 0x52,
+	RT_CHANNEL_DOMAIN_FCC1_NCC4 = 0x53,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI12 = 0x54,
+	RT_CHANNEL_DOMAIN_FCC1_FCC9 = 0x55,
+	RT_CHANNEL_DOMAIN_WORLD_ETSI13 = 0x56,
+	RT_CHANNEL_DOMAIN_FCC1_FCC10 = 0x57,
+	/*  Add new channel plan above this line =============== */
+	RT_CHANNEL_DOMAIN_MAX,
+	RT_CHANNEL_DOMAIN_REALTEK_DEFINE = 0x7F,
+};
+
+enum {
+	RT_CHANNEL_DOMAIN_2G_WORLD = 0x00,		/* Worldwird 13 */
+	RT_CHANNEL_DOMAIN_2G_ETSI1 = 0x01,		/* Europe */
+	RT_CHANNEL_DOMAIN_2G_FCC1 = 0x02,		/* US */
+	RT_CHANNEL_DOMAIN_2G_MKK1 = 0x03,		/* Japan */
+	RT_CHANNEL_DOMAIN_2G_ETSI2 = 0x04,		/* France */
+	RT_CHANNEL_DOMAIN_2G_GLOBAL = 0x05,		/* Global domain */
+	RT_CHANNEL_DOMAIN_2G_NULL = 0x06,
+	/*  Add new channel plan above this line =============== */
+	RT_CHANNEL_DOMAIN_2G_MAX,
+};
+
+#define rtw_is_channel_plan_valid(chplan) (chplan < RT_CHANNEL_DOMAIN_MAX || chplan == RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+
+struct rt_channel_plan {
+	unsigned char Channel[MAX_CHANNEL_NUM];
+	unsigned char Len;
+};
+
+struct rt_channel_plan_2g {
+	unsigned char Channel[MAX_CHANNEL_NUM_2G];
+	unsigned char Len;
+};
+
+struct rt_channel_plan_map {
+	unsigned char Index2G;
+};
+
+enum {
+	HT_IOT_PEER_UNKNOWN			= 0,
+	HT_IOT_PEER_REALTEK			= 1,
+	HT_IOT_PEER_REALTEK_92SE		= 2,
+	HT_IOT_PEER_BROADCOM		= 3,
+	HT_IOT_PEER_RALINK			= 4,
+	HT_IOT_PEER_ATHEROS			= 5,
+	HT_IOT_PEER_CISCO				= 6,
+	HT_IOT_PEER_MERU				= 7,
+	HT_IOT_PEER_MARVELL			= 8,
+	HT_IOT_PEER_REALTEK_SOFTAP	= 9,/*  peer is RealTek SOFT_AP, by Bohn, 2009.12.17 */
+	HT_IOT_PEER_SELF_SOFTAP			= 10, /*  Self is SoftAP */
+	HT_IOT_PEER_AIRGO				= 11,
+	HT_IOT_PEER_INTEL				= 12,
+	HT_IOT_PEER_RTK_APCLIENT		= 13,
+	HT_IOT_PEER_REALTEK_81XX		= 14,
+	HT_IOT_PEER_REALTEK_WOW			= 15,
+	HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP = 16,
+	HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP = 17,
+	HT_IOT_PEER_MAX					= 18
+};
+
+
+enum {
+	SCAN_DISABLE = 0,
+	SCAN_START = 1,
+	SCAN_TXNULL = 2,
+	SCAN_PROCESS = 3,
+	SCAN_COMPLETE = 4,
+	SCAN_STATE_MAX,
+};
+
+struct mlme_handler {
+	unsigned int   num;
+	char *str;
+	unsigned int (*func)(struct adapter *padapter, union recv_frame *precv_frame);
+};
+
+struct action_handler {
+	unsigned int   num;
+	char *str;
+	unsigned int (*func)(struct adapter *padapter, union recv_frame *precv_frame);
+};
+
+struct	ss_res {
+	int	state;
+	int	bss_cnt;
+	int	channel_idx;
+	int	scan_mode;
+	u8 ssid_num;
+	u8 ch_num;
+	struct ndis_802_11_ssid ssid[RTW_SSID_SCAN_AMOUNT];
+	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
+};
+
+/* define AP_MODE				0x0C */
+/* define STATION_MODE	0x08 */
+/* define AD_HOC_MODE		0x04 */
+/* define NO_LINK_MODE	0x00 */
+
+#define		WIFI_FW_NULL_STATE			_HW_STATE_NOLINK_
+#define	WIFI_FW_STATION_STATE		_HW_STATE_STATION_
+#define	WIFI_FW_AP_STATE				_HW_STATE_AP_
+#define	WIFI_FW_ADHOC_STATE			_HW_STATE_ADHOC_
+
+#define	WIFI_FW_AUTH_NULL			0x00000100
+#define	WIFI_FW_AUTH_STATE			0x00000200
+#define	WIFI_FW_AUTH_SUCCESS			0x00000400
+
+#define	WIFI_FW_ASSOC_STATE			0x00002000
+#define	WIFI_FW_ASSOC_SUCCESS		0x00004000
+
+#define	WIFI_FW_LINKING_STATE		(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE | WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)
+
+struct FW_Sta_Info {
+	struct sta_info *psta;
+	u32 status;
+	u32 rx_pkt;
+	u32 retry;
+	NDIS_802_11_RATES_EX  SupportedRates;
+};
+
+/*
+ * Usage:
+ * When one iface acted as AP mode and the other iface is STA mode and scanning,
+ * it should switch back to AP's operating channel periodically.
+ * Parameters info:
+ * When the driver scanned RTW_SCAN_NUM_OF_CH channels, it would switch back to AP's operating channel for
+ * RTW_STAY_AP_CH_MILLISECOND * SURVEY_TO milliseconds.
+ * Example:
+ * For chip supports 2.4G and AP mode is operating in channel 1,
+ * RTW_SCAN_NUM_OF_CH is 8, RTW_STAY_AP_CH_MILLISECOND is 3 and SURVEY_TO is 100.
+ * When it's STA mode gets set_scan command,
+ * it would
+ * 1. Doing the scan on channel 1.2.3.4.5.6.7.8
+ * 2. Back to channel 1 for 300 milliseconds
+ * 3. Go through doing site survey on channel 9.10.11
+ * 4. Back to channel 1 for 300 milliseconds
+ * 5. ... and so on, till survey done.
+ */
+struct mlme_ext_info {
+	u32 state;
+	u32 reauth_count;
+	u32 reassoc_count;
+	u32 link_count;
+	u32 auth_seq;
+	u32 auth_algo;	/*  802.11 auth, could be open, shared, auto */
+	u32 authModeToggle;
+	u32 enc_algo;/* encrypt algorithm; */
+	u32 key_index;	/*  this is only valid for legendary wep, 0~3 for key id. */
+	u32 iv;
+	u8 chg_txt[128];
+	u16 aid;
+	u16 bcn_interval;
+	u16 capability;
+	u8 assoc_AP_vendor;
+	u8 slotTime;
+	u8 preamble_mode;
+	u8 WMM_enable;
+	u8 ERP_enable;
+	u8 ERP_IE;
+	u8 HT_enable;
+	u8 HT_caps_enable;
+	u8 HT_info_enable;
+	u8 HT_protection;
+	u8 turboMode_cts2self;
+	u8 turboMode_rtsen;
+	u8 SM_PS;
+	u8 agg_enable_bitmap;
+	u8 ADDBA_retry_count;
+	u8 candidate_tid_bitmap;
+	u8 dialogToken;
+	/*  Accept ADDBA Request */
+	bool accept_addba_req;
+	u8 bwmode_updated;
+	u8 hidden_ssid_mode;
+	u8 VHT_enable;
+
+	struct ADDBA_request		ADDBA_req;
+	struct WMM_para_element	WMM_param;
+	struct HT_caps_element	HT_caps;
+	struct HT_info_element		HT_info;
+	struct wlan_bssid_ex			network;/* join network or bss_network, if in ap mode, it is the same to cur_network.network */
+	struct FW_Sta_Info		FW_sta_info[NUM_STA];
+};
+
+/*  The channel information about this channel including joining, scanning, and power constraints. */
+struct rt_channel_info {
+	u8 		ChannelNum;		/*  The channel number. */
+	enum rt_scan_type	ScanType;		/*  Scan type such as passive or active scan. */
+};
+
+int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch);
+
+/*  P2P_MAX_REG_CLASSES - Maximum number of regulatory classes */
+#define P2P_MAX_REG_CLASSES 10
+
+/*  P2P_MAX_REG_CLASS_CHANNELS - Maximum number of channels per regulatory class */
+#define P2P_MAX_REG_CLASS_CHANNELS 20
+
+/*   struct p2p_channels - List of supported channels */
+struct p2p_channels {
+	/*  struct p2p_reg_class - Supported regulatory class */
+	struct p2p_reg_class {
+		/*  reg_class - Regulatory class (IEEE 802.11-2007, Annex J) */
+		u8 reg_class;
+
+		/*  channel - Supported channels */
+		u8 channel[P2P_MAX_REG_CLASS_CHANNELS];
+
+		/*  channels - Number of channel entries in use */
+		size_t channels;
+	} reg_class[P2P_MAX_REG_CLASSES];
+
+	/*  reg_classes - Number of reg_class entries in use */
+	size_t reg_classes;
+};
+
+struct p2p_oper_class_map {
+	enum hw_mode {IEEE80211G, IEEE80211A} mode;
+	u8 op_class;
+	u8 min_chan;
+	u8 max_chan;
+	u8 inc;
+	enum { BW20, BW40PLUS, BW40MINUS } bw;
+};
+
+struct mlme_ext_priv {
+	struct adapter	*padapter;
+	u8 mlmeext_init;
+	atomic_t		event_seq;
+	u16 mgnt_seq;
+	u16 sa_query_seq;
+	u64 mgnt_80211w_IPN;
+	u64 mgnt_80211w_IPN_rx;
+	/* struct fw_priv fwpriv; */
+
+	unsigned char cur_channel;
+	unsigned char cur_bwmode;
+	unsigned char cur_ch_offset;/* PRIME_CHNL_OFFSET */
+	unsigned char cur_wireless_mode;	/*  NETWORK_TYPE */
+
+	unsigned char max_chan_nums;
+	struct rt_channel_info		channel_set[MAX_CHANNEL_NUM];
+	struct p2p_channels channel_list;
+	unsigned char basicrate[NumRates];
+	unsigned char datarate[NumRates];
+	unsigned char default_supported_mcs_set[16];
+
+	struct ss_res		sitesurvey_res;
+	struct mlme_ext_info mlmext_info;/* for sta/adhoc mode, including current scanning/connecting/connected related info. */
+                                                     /* for ap mode, network includes ap's cap_info */
+	struct timer_list		survey_timer;
+	struct timer_list		link_timer;
+	struct timer_list		sa_query_timer;
+	/* struct timer_list		ADDBA_timer; */
+	u16 		chan_scan_time;
+	unsigned long last_scan_time;
+	u8 scan_abort;
+	u8 tx_rate; /*  TXRATE when USERATE is set. */
+
+	u32 retry; /* retry for issue probereq */
+
+	u64 TSFValue;
+
+	/* for LPS-32K to adaptive bcn early and timeout */
+	u8 adaptive_tsf_done;
+	u32 bcn_delay_cnt[9];
+	u32 bcn_delay_ratio[9];
+	u32 bcn_cnt;
+	u8 DrvBcnEarly;
+	u8 DrvBcnTimeOut;
+
+	unsigned char bstart_bss;
+
+	u8 update_channel_plan_by_ap_done;
+
+	/* recv_decache check for Action_public frame */
+	u8 action_public_dialog_token;
+	u16  action_public_rxseq;
+
+	u8 active_keep_alive_check;
+#ifdef DBG_FIXED_CHAN
+	u8 fixed_chan;
+#endif
+
+};
+
+void init_mlme_default_rate_set(struct adapter *padapter);
+void init_mlme_ext_priv(struct adapter *padapter);
+int init_hw_mlme_ext(struct adapter *padapter);
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext);
+extern void init_mlme_ext_timer(struct adapter *padapter);
+extern void init_addba_retry_timer(struct adapter *padapter, struct sta_info *psta);
+extern struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv);
+
+/* void fill_fwpriv(struct adapter *padapter, struct fw_priv *pfwpriv); */
+
+u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta);
+
+void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len);
+void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask);
+void UpdateBrateTbl(struct adapter *padapter, u8 *mBratesOS);
+void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen);
+
+void Save_DM_Func_Flag(struct adapter *padapter);
+void Restore_DM_Func_Flag(struct adapter *padapter);
+void Switch_DM_Func(struct adapter *padapter, u32 mode, u8 enable);
+
+void Set_MSR(struct adapter *padapter, u8 type);
+
+u8 rtw_get_oper_ch(struct adapter *adapter);
+void rtw_set_oper_ch(struct adapter *adapter, u8 ch);
+u8 rtw_get_oper_bw(struct adapter *adapter);
+void rtw_set_oper_bw(struct adapter *adapter, u8 bw);
+u8 rtw_get_oper_choffset(struct adapter *adapter);
+void rtw_set_oper_choffset(struct adapter *adapter, u8 offset);
+u8 rtw_get_center_ch(u8 channel, u8 chnl_bw, u8 chnl_offset);
+unsigned long rtw_get_on_cur_ch_time(struct adapter *adapter);
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode);
+void SelectChannel(struct adapter *padapter, unsigned char channel);
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval);
+
+void read_cam(struct adapter *padapter, u8 entry, u8 *get_key);
+
+/* modify HW only */
+void _write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key);
+void _clear_cam_entry(struct adapter *padapter, u8 entry);
+
+/* modify both HW and cache */
+void write_cam(struct adapter *padapter, u8 id, u16 ctrl, u8 *mac, u8 *key);
+void clear_cam_entry(struct adapter *padapter, u8 id);
+
+/* modify cache only */
+void write_cam_cache(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key);
+void clear_cam_cache(struct adapter *adapter, u8 id);
+
+void invalidate_cam_all(struct adapter *padapter);
+
+
+int allocate_fw_sta_entry(struct adapter *padapter);
+void flush_all_cam_entry(struct adapter *padapter);
+
+void site_survey(struct adapter *padapter);
+u8 collect_bss_info(struct adapter *padapter, union recv_frame *precv_frame, struct wlan_bssid_ex *bssid);
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src, struct adapter *padapter, bool update_ie);
+
+u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork);
+u16 get_beacon_interval(struct wlan_bssid_ex *bss);
+
+int is_client_associated_to_ap(struct adapter *padapter);
+int is_client_associated_to_ibss(struct adapter *padapter);
+int is_IBSS_empty(struct adapter *padapter);
+
+unsigned char check_assoc_AP(u8 *pframe, uint len);
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE);
+void WMMOnAssocRsp(struct adapter *padapter);
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE);
+void HT_info_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE);
+void HTOnAssocRsp(struct adapter *padapter);
+
+void ERP_IE_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE);
+void VCS_update(struct adapter *padapter, struct sta_info *psta);
+void update_ldpc_stbc_cap(struct sta_info *psta);
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint len, struct sta_info *psta);
+int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len);
+void update_IOT_info(struct adapter *padapter);
+void update_capinfo(struct adapter *Adapter, u16 updateCap);
+void update_wireless_mode(struct adapter *padapter);
+void update_sta_basic_rate(struct sta_info *psta, u8 wireless_mode);
+int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx);
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta);
+void Update_RA_Entry(struct adapter *padapter, struct sta_info *psta);
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta);
+
+unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason);
+
+unsigned char get_highest_rate_idx(u32 mask);
+int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps, u8 bwmode);
+unsigned int is_ap_in_tkip(struct adapter *padapter);
+
+s16 rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid);
+s16 rtw_camid_alloc(struct adapter *adapter, struct sta_info *sta, u8 kid);
+void rtw_camid_free(struct adapter *adapter, u8 cam_id);
+
+extern void rtw_alloc_macid(struct adapter *padapter, struct sta_info *psta);
+extern void rtw_release_macid(struct adapter *padapter, struct sta_info *psta);
+extern u8 rtw_search_max_mac_id(struct adapter *padapter);
+
+void report_join_res(struct adapter *padapter, int res);
+void report_survey_event(struct adapter *padapter, union recv_frame *precv_frame);
+void report_surveydone_event(struct adapter *padapter);
+void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason);
+void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx);
+void report_wmm_edca_update(struct adapter *padapter);
+
+u8 chk_bmc_sleepq_cmd(struct adapter *padapter);
+extern u8 set_tx_beacon_cmd(struct adapter *padapter);
+unsigned int setup_beacon_frame(struct adapter *padapter, unsigned char *beacon_frame);
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate);
+void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib);
+void update_mgntframe_attrib_addr(struct adapter *padapter, struct xmit_frame *pmgntframe);
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe);
+s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms);
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe);
+
+void issue_beacon(struct adapter *padapter, int timeout_ms);
+void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq);
+void issue_assocreq(struct adapter *padapter);
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type);
+void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status);
+void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da);
+s32 issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, u8 ch, bool append_wps, int try_cnt, int wait_ms);
+int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms);
+s32 issue_nulldata_in_interrupt(struct adapter *padapter, u8 *da);
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms);
+int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason);
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt, int wait_ms);
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status);
+void issue_action_SA_Query(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid);
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr);
+unsigned int send_beacon(struct adapter *padapter);
+
+void start_clnt_assoc(struct adapter *padapter);
+void start_clnt_auth(struct adapter *padapter);
+void start_clnt_join(struct adapter *padapter);
+void start_create_ibss(struct adapter *padapter);
+
+unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int DoReserved(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAtim(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnDisassoc(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAuth(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnDeAuth(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAction(struct adapter *padapter, union recv_frame *precv_frame);
+
+unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAction_back(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int on_action_public(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAction_ht(struct adapter *padapter, union recv_frame *precv_frame);
+unsigned int OnAction_sa_query(struct adapter *padapter, union recv_frame *precv_frame);
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res);
+void mlmeext_sta_del_event_callback(struct adapter *padapter);
+void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta);
+
+void linked_status_chk(struct adapter *padapter);
+
+void _linked_info_dump(struct adapter *padapter);
+
+void survey_timer_hdl(struct timer_list *t);
+void link_timer_hdl(struct timer_list *t);
+void addba_timer_hdl(struct timer_list *t);
+void sa_query_timer_hdl(struct timer_list *t);
+/* void reauth_timer_hdl(struct adapter *padapter); */
+/* void reassoc_timer_hdl(struct adapter *padapter); */
+
+#define set_survey_timer(mlmeext, ms) \
+	do { \
+		_set_timer(&(mlmeext)->survey_timer, (ms)); \
+	} while (0)
+
+#define set_link_timer(mlmeext, ms) \
+	do { \
+		_set_timer(&(mlmeext)->link_timer, (ms)); \
+	} while (0)
+#define set_sa_query_timer(mlmeext, ms) \
+	do { \
+		_set_timer(&(mlmeext)->sa_query_timer, (ms)); \
+	} while (0)
+
+extern void process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr);
+
+extern void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len);
+extern void correct_TSF(struct adapter *padapter, struct mlme_ext_priv *pmlmeext);
+extern void adaptive_early_32k(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len);
+extern u8 traffic_status_watchdog(struct adapter *padapter, u8 from_timer);
+
+int rtw_chk_start_clnt_join(struct adapter *padapter, u8 *ch, u8 *bw, u8 *offset);
+
+struct cmd_hdl {
+	uint	parmsize;
+	u8 (*h2cfuns)(struct adapter *padapter, u8 *pbuf);
+};
+
+
+u8 read_macreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_macreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 read_bbreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_bbreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 read_rfreg_hdl(struct adapter *padapter, u8 *pbuf);
+u8 write_rfreg_hdl(struct adapter *padapter, u8 *pbuf);
+
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf);
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf);
+u8 disconnect_hdl(struct adapter *padapter, u8 *pbuf);
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf);
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setauth_hdl(struct adapter *padapter, u8 *pbuf);
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_assocsta_hdl(struct adapter *padapter, u8 *pbuf);
+u8 del_assocsta_hdl(struct adapter *padapter, u8 *pbuf);
+u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf);
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 chk_bmc_sleepq_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf);
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf);	/* Kurt: Handling DFS channel switch announcement ie. */
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf);
+u8 run_in_thread_hdl(struct adapter *padapter, u8 *pbuf);
+
+
+#define GEN_DRV_CMD_HANDLER(size, cmd)	{size, &cmd ## _hdl},
+#define GEN_MLME_EXT_HANDLER(size, cmd)	{size, cmd},
+
+struct C2HEvent_Header {
+
+#ifdef __LITTLE_ENDIAN
+
+	unsigned int len:16;
+	unsigned int ID:8;
+	unsigned int seq:8;
+#else
+	unsigned int seq:8;
+	unsigned int ID:8;
+	unsigned int len:16;
+#endif
+	unsigned int rsvd;
+};
+
+void rtw_dummy_event_callback(struct adapter *adapter, u8 *pbuf);
+void rtw_fwdbg_event_callback(struct adapter *adapter, u8 *pbuf);
+
+enum {
+	GEN_EVT_CODE(_Read_MACREG) = 0, /*0*/
+	GEN_EVT_CODE(_Read_BBREG),
+	GEN_EVT_CODE(_Read_RFREG),
+	GEN_EVT_CODE(_Read_EEPROM),
+	GEN_EVT_CODE(_Read_EFUSE),
+	GEN_EVT_CODE(_Read_CAM),			/*5*/
+	GEN_EVT_CODE(_Get_BasicRate),
+	GEN_EVT_CODE(_Get_DataRate),
+	GEN_EVT_CODE(_Survey),	 /*8*/
+	GEN_EVT_CODE(_SurveyDone),	 /*9*/
+
+	GEN_EVT_CODE(_JoinBss), /*10*/
+	GEN_EVT_CODE(_AddSTA),
+	GEN_EVT_CODE(_DelSTA),
+	GEN_EVT_CODE(_AtimDone),
+	GEN_EVT_CODE(_TX_Report),
+	GEN_EVT_CODE(_CCX_Report),			/*15*/
+	GEN_EVT_CODE(_DTM_Report),
+	GEN_EVT_CODE(_TX_Rate_Statistics),
+	GEN_EVT_CODE(_C2HLBK),
+	GEN_EVT_CODE(_FWDBG),
+	GEN_EVT_CODE(_C2HFEEDBACK),               /*20*/
+	GEN_EVT_CODE(_ADDBA),
+	GEN_EVT_CODE(_C2HBCN),
+	GEN_EVT_CODE(_ReportPwrState),		/* filen: only for PCIE, USB */
+	GEN_EVT_CODE(_CloseRF),				/* filen: only for PCIE, work around ASPM */
+	GEN_EVT_CODE(_WMM),					/*25*/
+	MAX_C2HEVT
+};
+
+
+#ifdef _RTW_MLME_EXT_C_
+
+#endif/* _RTL8192C_CMD_C_ */
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_mp.h b/drivers/staging/rtl8723bs/include/rtw_mp.h
new file mode 100644
index 000000000..ea3abee32
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mp.h
@@ -0,0 +1,374 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTW_MP_H_
+#define _RTW_MP_H_
+
+#define MAX_MP_XMITBUF_SZ	2048
+#define NR_MP_XMITFRAME		8
+
+struct mp_xmit_frame {
+	struct list_head	list;
+
+	struct pkt_attrib attrib;
+
+	struct sk_buff *pkt;
+
+	int frame_tag;
+
+	struct adapter *padapter;
+
+	uint mem[(MAX_MP_XMITBUF_SZ >> 2)];
+};
+
+struct mp_wiparam {
+	u32 bcompleted;
+	u32 act_type;
+	u32 io_offset;
+	u32 io_value;
+};
+
+struct mp_tx {
+	u8 stop;
+	u32 count, sended;
+	u8 payload;
+	struct pkt_attrib attrib;
+	/* struct tx_desc desc; */
+	/* u8 resvdtx[7]; */
+	u8 desc[TXDESC_SIZE];
+	u8 *pallocated_buf;
+	u8 *buf;
+	u32 buf_size, write_size;
+	void *PktTxThread;
+};
+
+#define MP_MAX_LINES		1000
+#define MP_MAX_LINES_BYTES	256
+
+typedef void (*MPT_WORK_ITEM_HANDLER)(void *Adapter);
+struct mpt_context {
+	/*  Indicate if we have started Mass Production Test. */
+	bool			bMassProdTest;
+
+	/*  Indicate if the driver is unloading or unloaded. */
+	bool			bMptDrvUnload;
+
+	struct timer_list			MPh2c_timeout_timer;
+/*  Event used to sync H2c for BT control */
+
+	bool		MptH2cRspEvent;
+	bool		MptBtC2hEvent;
+	bool		bMPh2c_timeout;
+
+	/* 8190 PCI does not support NDIS_WORK_ITEM. */
+	/*  Work Item for Mass Production Test. */
+	/* NDIS_WORK_ITEM	MptWorkItem; */
+/* 	RT_WORK_ITEM		MptWorkItem; */
+	/*  Event used to sync the case unloading driver and MptWorkItem is still in progress. */
+/* 	NDIS_EVENT		MptWorkItemEvent; */
+	/*  To protect the following variables. */
+/* 	NDIS_SPIN_LOCK		MptWorkItemSpinLock; */
+	/*  Indicate a MptWorkItem is scheduled and not yet finished. */
+	bool			bMptWorkItemInProgress;
+	/*  An instance which implements function and context of MptWorkItem. */
+	MPT_WORK_ITEM_HANDLER	CurrMptAct;
+
+	/*  1 =Start, 0 =Stop from UI. */
+	u32 		MptTestStart;
+	/*  _TEST_MODE, defined in MPT_Req2.h */
+	u32 		MptTestItem;
+	/*  Variable needed in each implementation of CurrMptAct. */
+	u32 		MptActType;	/*  Type of action performed in CurrMptAct. */
+	/*  The Offset of IO operation is depend of MptActType. */
+	u32 		MptIoOffset;
+	/*  The Value of IO operation is depend of MptActType. */
+	u32 		MptIoValue;
+	/*  The RfPath of IO operation is depend of MptActType. */
+	u32 		MptRfPath;
+
+	enum wireless_mode		MptWirelessModeToSw;	/*  Wireless mode to switch. */
+	u8 	MptChannelToSw;		/*  Channel to switch. */
+	u8 	MptInitGainToSet;	/*  Initial gain to set. */
+	u32 		MptBandWidth;		/*  bandwidth to switch. */
+	u32 		MptRateIndex;		/*  rate index. */
+	/*  Register value kept for Single Carrier Tx test. */
+	u8 	btMpCckTxPower;
+	/*  Register value kept for Single Carrier Tx test. */
+	u8 	btMpOfdmTxPower;
+	/*  For MP Tx Power index */
+	u8 	TxPwrLevel[2];	/*  rf-A, rf-B */
+	u32 		RegTxPwrLimit;
+	/*  Content of RCR Register for Mass Production Test. */
+	u32 		MptRCR;
+	/*  true if we only receive packets with specific pattern. */
+	bool			bMptFilterPattern;
+	/*  Rx OK count, statistics used in Mass Production Test. */
+	u32 		MptRxOkCnt;
+	/*  Rx CRC32 error count, statistics used in Mass Production Test. */
+	u32 		MptRxCrcErrCnt;
+
+	bool			bCckContTx;	/*  true if we are in CCK Continuous Tx test. */
+	bool			bOfdmContTx;	/*  true if we are in OFDM Continuous Tx test. */
+	bool			bStartContTx;	/*  true if we have start Continuous Tx test. */
+	/*  true if we are in Single Carrier Tx test. */
+	bool			bSingleCarrier;
+	/*  true if we are in Carrier Suppression Tx Test. */
+	bool			bCarrierSuppression;
+	/* true if we are in Single Tone Tx test. */
+	bool			bSingleTone;
+
+	/*  ACK counter asked by K.Y.. */
+	bool			bMptEnableAckCounter;
+	u32 		MptAckCounter;
+
+	/*  SD3 Willis For 8192S to save 1T/2T RF table for ACUT	Only fro ACUT delete later ~~~! */
+	/* s8		BufOfLines[2][MAX_LINES_HWCONFIG_TXT][MAX_BYTES_LINE_HWCONFIG_TXT]; */
+	/* s8			BufOfLines[2][MP_MAX_LINES][MP_MAX_LINES_BYTES]; */
+	/* s32			RfReadLine[2]; */
+
+	u8 APK_bound[2];	/* for APK	path A/path B */
+	bool		bMptIndexEven;
+
+	u8 backup0xc50;
+	u8 backup0xc58;
+	u8 backup0xc30;
+	u8 backup0x52_RF_A;
+	u8 backup0x52_RF_B;
+
+	u32 		backup0x58_RF_A;
+	u32 		backup0x58_RF_B;
+
+	u8 	h2cReqNum;
+	u8 	c2hBuf[32];
+
+    u8          btInBuf[100];
+	u32 		mptOutLen;
+    u8          mptOutBuf[100];
+
+};
+/* endif */
+
+/* E-Fuse */
+#define EFUSE_MAP_SIZE		512
+
+#define EFUSE_MAX_SIZE		512
+/* end of E-Fuse */
+
+/* define RTPRIV_IOCTL_MP					(SIOCIWFIRSTPRIV + 0x17) */
+enum {
+	WRITE_REG = 1,
+	READ_REG,
+	WRITE_RF,
+	READ_RF,
+	MP_START,
+	MP_STOP,
+	MP_RATE,
+	MP_CHANNEL,
+	MP_BANDWIDTH,
+	MP_TXPOWER,
+	MP_ANT_TX,
+	MP_ANT_RX,
+	MP_CTX,
+	MP_QUERY,
+	MP_ARX,
+	MP_PSD,
+	MP_PWRTRK,
+	MP_THER,
+	MP_IOCTL,
+	EFUSE_GET,
+	EFUSE_SET,
+	MP_RESET_STATS,
+	MP_DUMP,
+	MP_PHYPARA,
+	MP_SetRFPathSwh,
+	MP_QueryDrvStats,
+	MP_SetBT,
+	CTA_TEST,
+	MP_DISABLE_BT_COEXIST,
+	MP_PwrCtlDM,
+	MP_NULL,
+	MP_GET_TXPOWER_INX,
+};
+
+struct mp_priv {
+	struct adapter *papdater;
+
+	/* Testing Flag */
+	u32 mode;/* 0 for normal type packet, 1 for loopback packet (16bytes TXCMD) */
+
+	u32 prev_fw_state;
+
+	/* OID cmd handler */
+	struct mp_wiparam workparam;
+/* 	u8 act_in_progress; */
+
+	/* Tx Section */
+	u8 TID;
+	u32 tx_pktcount;
+	u32 pktInterval;
+	struct mp_tx tx;
+
+	/* Rx Section */
+	u32 rx_bssidpktcount;
+	u32 rx_pktcount;
+	u32 rx_pktcount_filter_out;
+	u32 rx_crcerrpktcount;
+	u32 rx_pktloss;
+	bool  rx_bindicatePkt;
+	struct recv_stat rxstat;
+
+	/* RF/BB relative */
+	u8 channel;
+	u8 bandwidth;
+	u8 prime_channel_offset;
+	u8 txpoweridx;
+	u8 txpoweridx_b;
+	u8 rateidx;
+	u32 preamble;
+/* 	u8 modem; */
+	u32 CrystalCap;
+/* 	u32 curr_crystalcap; */
+
+	u16 antenna_tx;
+	u16 antenna_rx;
+/* 	u8 curr_rfpath; */
+
+	u8 check_mp_pkt;
+
+	u8 bSetTxPower;
+/* 	uint ForcedDataRate; */
+	u8 mp_dm;
+	u8 mac_filter[ETH_ALEN];
+	u8 bmac_filter;
+
+	struct wlan_network mp_network;
+	NDIS_802_11_MAC_ADDRESS network_macaddr;
+
+	u8 *pallocated_mp_xmitframe_buf;
+	u8 *pmp_xmtframe_buf;
+	struct __queue free_mp_xmitqueue;
+	u32 free_mp_xmitframe_cnt;
+	bool bSetRxBssid;
+	bool bTxBufCkFail;
+
+	struct mpt_context MptCtx;
+
+	u8 *TXradomBuffer;
+};
+
+#define LOWER	true
+#define RAISE	false
+
+/* Hardware Registers */
+#define BB_REG_BASE_ADDR		0x800
+
+#define MAX_RF_PATH_NUMS	RF_PATH_MAX
+
+extern u8 mpdatarate[NumRates];
+
+#define MAX_TX_PWR_INDEX_N_MODE 64	/*  0x3F */
+
+#define RX_PKT_BROADCAST	1
+#define RX_PKT_DEST_ADDR	2
+#define RX_PKT_PHY_MATCH	3
+
+#define Mac_OFDM_OK			0x00000000
+#define Mac_OFDM_Fail			0x10000000
+#define Mac_OFDM_FasleAlarm	0x20000000
+#define Mac_CCK_OK				0x30000000
+#define Mac_CCK_Fail			0x40000000
+#define Mac_CCK_FasleAlarm		0x50000000
+#define Mac_HT_OK				0x60000000
+#define Mac_HT_Fail			0x70000000
+#define Mac_HT_FasleAlarm		0x90000000
+#define Mac_DropPacket			0xA0000000
+
+#define		REG_RF_BB_GAIN_OFFSET	0x7f
+#define		RF_GAIN_OFFSET_MASK	0xfffff
+
+/*  */
+/* struct mp_xmit_frame *alloc_mp_xmitframe(struct mp_priv *pmp_priv); */
+/* int free_mp_xmitframe(struct xmit_priv *pxmitpriv, struct mp_xmit_frame *pmp_xmitframe); */
+
+s32 init_mp_priv(struct adapter *padapter);
+void free_mp_priv(struct mp_priv *pmp_priv);
+s32 MPT_InitializeAdapter(struct adapter *padapter, u8 Channel);
+void MPT_DeInitAdapter(struct adapter *padapter);
+s32 mp_start_test(struct adapter *padapter);
+void mp_stop_test(struct adapter *padapter);
+
+u32 _read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask);
+void _write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val);
+
+u32 read_macreg(struct adapter *padapter, u32 addr, u32 sz);
+void write_macreg(struct adapter *padapter, u32 addr, u32 val, u32 sz);
+u32 read_bbreg(struct adapter *padapter, u32 addr, u32 bitmask);
+void write_bbreg(struct adapter *padapter, u32 addr, u32 bitmask, u32 val);
+u32 read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr);
+void write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 val);
+
+void SetChannel(struct adapter *padapter);
+void SetBandwidth(struct adapter *padapter);
+int SetTxPower(struct adapter *padapter);
+void SetAntennaPathPower(struct adapter *padapter);
+void SetDataRate(struct adapter *padapter);
+
+void SetAntenna(struct adapter *padapter);
+
+s32 SetThermalMeter(struct adapter *padapter, u8 target_ther);
+void GetThermalMeter(struct adapter *padapter, u8 *value);
+
+void SetContinuousTx(struct adapter *padapter, u8 bStart);
+void SetSingleCarrierTx(struct adapter *padapter, u8 bStart);
+void SetSingleToneTx(struct adapter *padapter, u8 bStart);
+void SetCarrierSuppressionTx(struct adapter *padapter, u8 bStart);
+void PhySetTxPowerLevel(struct adapter *padapter);
+
+void fill_txdesc_for_mp(struct adapter *padapter, u8 *ptxdesc);
+void SetPacketTx(struct adapter *padapter);
+void SetPacketRx(struct adapter *padapter, u8 bStartRx);
+
+void ResetPhyRxPktCount(struct adapter *padapter);
+u32 GetPhyRxPktReceived(struct adapter *padapter);
+u32 GetPhyRxPktCRC32Error(struct adapter *padapter);
+
+s32	SetPowerTracking(struct adapter *padapter, u8 enable);
+void GetPowerTracking(struct adapter *padapter, u8 *enable);
+
+u32 mp_query_psd(struct adapter *padapter, u8 *data);
+
+void Hal_SetAntenna(struct adapter *padapter);
+void Hal_SetBandwidth(struct adapter *padapter);
+
+void Hal_SetTxPower(struct adapter *padapter);
+void Hal_SetCarrierSuppressionTx(struct adapter *padapter, u8 bStart);
+void Hal_SetSingleToneTx(struct adapter *padapter, u8 bStart);
+void Hal_SetSingleCarrierTx(struct adapter *padapter, u8 bStart);
+void Hal_SetContinuousTx(struct adapter *padapter, u8 bStart);
+
+void Hal_SetDataRate(struct adapter *padapter);
+void Hal_SetChannel(struct adapter *padapter);
+void Hal_SetAntennaPathPower(struct adapter *padapter);
+s32 Hal_SetThermalMeter(struct adapter *padapter, u8 target_ther);
+s32 Hal_SetPowerTracking(struct adapter *padapter, u8 enable);
+void Hal_GetPowerTracking(struct adapter *padapter, u8 *enable);
+void Hal_GetThermalMeter(struct adapter *padapter, u8 *value);
+void Hal_mpt_SwitchRfSetting(struct adapter *padapter);
+void Hal_MPT_CCKTxPowerAdjust(struct adapter *Adapter, bool bInCH14);
+void Hal_MPT_CCKTxPowerAdjustbyIndex(struct adapter *padapter, bool beven);
+void Hal_SetCCKTxPower(struct adapter *padapter, u8 *TxPower);
+void Hal_SetOFDMTxPower(struct adapter *padapter, u8 *TxPower);
+void Hal_TriggerRFThermalMeter(struct adapter *padapter);
+u8 Hal_ReadRFThermalMeter(struct adapter *padapter);
+void Hal_SetCCKContinuousTx(struct adapter *padapter, u8 bStart);
+void Hal_SetOFDMContinuousTx(struct adapter *padapter, u8 bStart);
+void Hal_ProSetCrystalCap(struct adapter *padapter, u32 CrystalCapVal);
+void MP_PHY_SetRFPathSwitch(struct adapter *padapter, bool bMain);
+u32 mpt_ProQueryCalTxPower(struct adapter *padapter, u8 RfPath);
+void MPT_PwrCtlDM(struct adapter *padapter, u32 bstart);
+u8 MptToMgntRate(u32 MptRateIdx);
+
+#endif /* _RTW_MP_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h b/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h
new file mode 100644
index 000000000..0767dbb84
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h
@@ -0,0 +1,292 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_PWRCTRL_H_
+#define __RTW_PWRCTRL_H_
+
+#include <linux/mutex.h>
+
+#define FW_PWR0	0
+#define FW_PWR1		1
+#define FW_PWR2		2
+#define FW_PWR3		3
+
+
+#define HW_PWR0	7
+#define HW_PWR1		6
+#define HW_PWR2		2
+#define HW_PWR3	0
+#define HW_PWR4	8
+
+#define FW_PWRMSK	0x7
+
+
+#define XMIT_ALIVE	BIT(0)
+#define RECV_ALIVE	BIT(1)
+#define CMD_ALIVE	BIT(2)
+#define EVT_ALIVE	BIT(3)
+#define BTCOEX_ALIVE	BIT(4)
+
+
+enum {
+	PS_MODE_ACTIVE	= 0,
+	PS_MODE_MIN,
+	PS_MODE_MAX,
+	PS_MODE_DTIM,	/* PS_MODE_SELF_DEFINED */
+	PS_MODE_VOIP,
+	PS_MODE_UAPSD_WMM,
+	PS_MODE_UAPSD,
+	PS_MODE_IBSS,
+	PS_MODE_WWLAN,
+	PM_Radio_Off,
+	PM_Card_Disable,
+	PS_MODE_NUM,
+};
+
+/*
+	BIT[2:0] = HW state
+	BIT[3] = Protocol PS state,   0: register active state , 1: register sleep state
+	BIT[4] = sub-state
+*/
+
+#define PS_DPS				BIT(0)
+#define PS_LCLK				(PS_DPS)
+#define PS_RF_OFF			BIT(1)
+#define PS_ALL_ON			BIT(2)
+#define PS_ST_ACTIVE		BIT(3)
+
+#define PS_ISR_ENABLE		BIT(4)
+#define PS_IMR_ENABLE		BIT(5)
+#define PS_ACK				BIT(6)
+#define PS_TOGGLE			BIT(7)
+
+#define PS_STATE_MASK		(0x0F)
+#define PS_STATE_HW_MASK	(0x07)
+#define PS_SEQ_MASK			(0xc0)
+
+#define PS_STATE(x)		(PS_STATE_MASK & (x))
+#define PS_STATE_HW(x)	(PS_STATE_HW_MASK & (x))
+#define PS_SEQ(x)		(PS_SEQ_MASK & (x))
+
+#define PS_STATE_S0		(PS_DPS)
+#define PS_STATE_S1		(PS_LCLK)
+#define PS_STATE_S2		(PS_RF_OFF)
+#define PS_STATE_S3		(PS_ALL_ON)
+#define PS_STATE_S4		((PS_ST_ACTIVE) | (PS_ALL_ON))
+
+
+#define PS_IS_RF_ON(x)	((x) & (PS_ALL_ON))
+#define PS_IS_ACTIVE(x)	((x) & (PS_ST_ACTIVE))
+#define CLR_PS_STATE(x)	((x) = ((x) & (0xF0)))
+
+
+struct reportpwrstate_parm {
+	unsigned char mode;
+	unsigned char state; /* the CPWM value */
+	unsigned short rsvd;
+};
+
+#define LPS_DELAY_TIME	(1 * HZ) /*  1 sec */
+
+#define EXE_PWR_NONE	0x01
+#define EXE_PWR_IPS		0x02
+#define EXE_PWR_LPS		0x04
+
+/*  RF state. */
+enum rt_rf_power_state {
+	rf_on,		/*  RF is on after RFSleep or RFOff */
+	rf_sleep,	/*  802.11 Power Save mode */
+	rf_off,		/*  HW/SW Radio OFF or Inactive Power Save */
+	/* Add the new RF state above this line ===== */
+	rf_max
+};
+
+/*  RF Off Level for IPS or HW/SW radio off */
+#define	RT_RF_OFF_LEVL_ASPM			BIT(0)	/*  PCI ASPM */
+#define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*  PCI clock request */
+#define	RT_RF_OFF_LEVL_PCI_D3			BIT(2)	/*  PCI D3 mode */
+#define	RT_RF_OFF_LEVL_HALT_NIC		BIT(3)	/*  NIC halt, re-initialize hw parameters */
+#define	RT_RF_OFF_LEVL_FREE_FW		BIT(4)	/*  FW free, re-download the FW */
+#define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*  FW in 32k */
+#define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)	/*  Always enable ASPM and Clock Req in initialization. */
+#define	RT_RF_LPS_DISALBE_2R			BIT(30)	/*  When LPS is on, disable 2R if no packet is received or transmitted. */
+#define	RT_RF_LPS_LEVEL_ASPM			BIT(31)	/*  LPS with ASPM */
+
+#define	RT_IN_PS_LEVEL(ppsc, _PS_FLAG)		((ppsc->cur_ps_level & _PS_FLAG) ? true : false)
+#define	RT_CLEAR_PS_LEVEL(ppsc, _PS_FLAG)	(ppsc->cur_ps_level &= (~(_PS_FLAG)))
+#define	RT_SET_PS_LEVEL(ppsc, _PS_FLAG)		(ppsc->cur_ps_level |= _PS_FLAG)
+
+/*  ASPM OSC Control bit, added by Roger, 2013.03.29. */
+#define	RT_PCI_ASPM_OSC_IGNORE		0	 /*  PCI ASPM ignore OSC control in default */
+#define	RT_PCI_ASPM_OSC_ENABLE		BIT0 /*  PCI ASPM controlled by OS according to ACPI Spec 5.0 */
+#define	RT_PCI_ASPM_OSC_DISABLE		BIT1 /*  PCI ASPM controlled by driver or BIOS, i.e., force enable ASPM */
+
+enum {
+	PSBBREG_RF0 = 0,
+	PSBBREG_RF1,
+	PSBBREG_RF2,
+	PSBBREG_AFE0,
+	PSBBREG_TOTALCNT
+};
+
+enum { /*  for ips_mode */
+	IPS_NONE = 0,
+	IPS_NORMAL,
+	IPS_LEVEL_2,
+	IPS_NUM
+};
+
+/*  Design for pwrctrl_priv.ips_deny, 32 bits for 32 reasons at most */
+enum ps_deny_reason {
+	PS_DENY_DRV_INITIAL = 0,
+	PS_DENY_SCAN,
+	PS_DENY_JOIN,
+	PS_DENY_DISCONNECT,
+	PS_DENY_SUSPEND,
+	PS_DENY_IOCTL,
+	PS_DENY_MGNT_TX,
+	PS_DENY_DRV_REMOVE = 30,
+	PS_DENY_OTHERS = 31
+};
+
+struct pwrctrl_priv {
+	struct mutex lock;
+	volatile u8 rpwm; /*  requested power state for fw */
+	volatile u8 cpwm; /*  fw current power state. updated when 1. read from HCPWM 2. driver lowers power level */
+	volatile u8 tog; /*  toggling */
+	volatile u8 cpwm_tog; /*  toggling */
+
+	u8 pwr_mode;
+	u8 smart_ps;
+	u8 bcn_ant_mode;
+	u8 dtim;
+
+	u32 alives;
+	struct work_struct cpwm_event;
+	u8 brpwmtimeout;
+	struct work_struct rpwmtimeoutwi;
+	struct timer_list pwr_rpwm_timer;
+	u8 bpower_saving; /* for LPS/IPS */
+
+	u8 b_hw_radio_off;
+	u8 reg_rfoff;
+	u8 reg_pdnmode; /* powerdown mode */
+	u32 rfoff_reason;
+
+	/* RF OFF Level */
+	u32 cur_ps_level;
+	u32 reg_rfps_level;
+
+	uint	ips_enter_cnts;
+	uint	ips_leave_cnts;
+
+	u8 ips_mode;
+	u8 ips_org_mode;
+	u8 ips_mode_req; /*  used to accept the mode setting request, will update to ipsmode later */
+	bool bips_processing;
+	unsigned long ips_deny_time; /* will deny IPS when system time is smaller than this */
+	u8 pre_ips_type;/*  0: default flow, 1: carddisbale flow */
+
+	/*  ps_deny: if 0, power save is free to go; otherwise deny all kinds of power save. */
+	/*  Use enum ps_deny_reason to decide reason. */
+	/*  Don't access this variable directly without control function, */
+	/*  and this variable should be protected by lock. */
+	u32 ps_deny;
+
+	u8 ps_processing; /* temporarily used to mark whether in rtw_ps_processor */
+
+	u8 fw_psmode_iface_id;
+	u8 bLeisurePs;
+	u8 LpsIdleCount;
+	u8 power_mgnt;
+	u8 org_power_mgnt;
+	bool fw_current_in_ps_mode;
+	unsigned long	DelayLPSLastTimeStamp;
+	s32		pnp_current_pwr_state;
+	u8 pnp_bstop_trx;
+
+
+	u8 bInternalAutoSuspend;
+	u8 bInSuspend;
+
+	u8 bAutoResume;
+	u8 autopm_cnt;
+
+	u8 bSupportRemoteWakeup;
+	u8 wowlan_wake_reason;
+	u8 wowlan_ap_mode;
+	u8 wowlan_mode;
+	struct timer_list	pwr_state_check_timer;
+	struct adapter *adapter;
+	int		pwr_state_check_interval;
+	u8 pwr_state_check_cnts;
+
+	int		ps_flag; /* used by autosuspend */
+
+	enum rt_rf_power_state	rf_pwrstate;/* cur power state, only for IPS */
+	/* rt_rf_power_state	current_rfpwrstate; */
+	enum rt_rf_power_state	change_rfpwrstate;
+
+	u8 bHWPowerdown; /* power down mode selection. 0:radio off, 1:power down */
+	u8 bHWPwrPindetect; /* come from registrypriv.hwpwrp_detect. enable power down function. 0:disable, 1:enable */
+	u8 bkeepfwalive;
+	u8 brfoffbyhw;
+	unsigned long PS_BBRegBackup[PSBBREG_TOTALCNT];
+};
+
+#define rtw_ips_mode_req(pwrctl, ips_mode) \
+	((pwrctl)->ips_mode_req = (ips_mode))
+
+#define RTW_PWR_STATE_CHK_INTERVAL 2000
+
+#define _rtw_set_pwr_state_check_timer(pwrctl, ms) \
+	do { \
+		_set_timer(&(pwrctl)->pwr_state_check_timer, (ms)); \
+	} while (0)
+
+#define rtw_set_pwr_state_check_timer(pwrctl) \
+	_rtw_set_pwr_state_check_timer((pwrctl), (pwrctl)->pwr_state_check_interval)
+
+extern void rtw_init_pwrctrl_priv(struct adapter *adapter);
+extern void rtw_free_pwrctrl_priv(struct adapter *adapter);
+
+s32 rtw_register_task_alive(struct adapter *, u32 task);
+void rtw_unregister_task_alive(struct adapter *, u32 task);
+extern s32 rtw_register_tx_alive(struct adapter *padapter);
+extern void rtw_unregister_tx_alive(struct adapter *padapter);
+extern s32 rtw_register_cmd_alive(struct adapter *padapter);
+extern void rtw_unregister_cmd_alive(struct adapter *padapter);
+extern void cpwm_int_hdl(struct adapter *padapter, struct reportpwrstate_parm *preportpwrstate);
+extern void LPS_Leave_check(struct adapter *padapter);
+
+extern void LeaveAllPowerSaveMode(struct adapter *Adapter);
+extern void LeaveAllPowerSaveModeDirect(struct adapter *Adapter);
+void _ips_enter(struct adapter *padapter);
+void ips_enter(struct adapter *padapter);
+int _ips_leave(struct adapter *padapter);
+int ips_leave(struct adapter *padapter);
+
+void rtw_ps_processor(struct adapter *padapter);
+
+s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms);
+void LPS_Enter(struct adapter *padapter, const char *msg);
+void LPS_Leave(struct adapter *padapter, const char *msg);
+void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets);
+void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg);
+void rtw_set_rpwm(struct adapter *padapter, u8 val8);
+
+void rtw_set_ips_deny(struct adapter *padapter, u32 ms);
+int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller);
+#define rtw_pwr_wakeup(adapter) _rtw_pwr_wakeup(adapter, RTW_PWR_STATE_CHK_INTERVAL, __func__)
+#define rtw_pwr_wakeup_ex(adapter, ips_deffer_ms) _rtw_pwr_wakeup(adapter, ips_deffer_ms, __func__)
+int rtw_pm_set_ips(struct adapter *padapter, u8 mode);
+int rtw_pm_set_lps(struct adapter *padapter, u8 mode);
+
+void rtw_ps_deny(struct adapter *padapter, enum ps_deny_reason reason);
+void rtw_ps_deny_cancel(struct adapter *padapter, enum ps_deny_reason reason);
+u32 rtw_ps_deny_get(struct adapter *padapter);
+
+#endif  /* __RTL871X_PWRCTRL_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_qos.h b/drivers/staging/rtl8723bs/include/rtw_qos.h
new file mode 100644
index 000000000..1f28837f6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_qos.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+
+#ifndef _RTW_QOS_H_
+#define _RTW_QOS_H_
+
+
+
+struct	qos_priv {
+	unsigned int	  qos_option;	/* bit mask option: u-apsd, s-apsd, ts, block ack... */
+};
+
+
+#endif	/* _RTL871X_QOS_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_recv.h b/drivers/staging/rtl8723bs/include/rtw_recv.h
new file mode 100644
index 000000000..44f671035
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_recv.h
@@ -0,0 +1,500 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTW_RECV_H_
+#define _RTW_RECV_H_
+
+#define NR_RECVBUFF (8)
+
+#define NR_PREALLOC_RECV_SKB (8)
+
+#define NR_RECVFRAME 256
+
+#define RXFRAME_ALIGN	8
+#define RXFRAME_ALIGN_SZ	(1<<RXFRAME_ALIGN)
+
+#define DRVINFO_SZ	4 /*  unit is 8bytes */
+
+#define MAX_RXFRAME_CNT	512
+#define MAX_RX_NUMBLKS		(32)
+#define RECVFRAME_HDR_ALIGN 128
+
+
+#define PHY_RSSI_SLID_WIN_MAX				100
+#define PHY_LINKQUALITY_SLID_WIN_MAX		20
+
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define RX_MPDU_QUEUE				0
+#define RX_CMD_QUEUE				1
+#define RX_MAX_QUEUE				2
+
+#define MAX_SUBFRAME_COUNT	64
+
+#define LLC_HEADER_LENGTH	6
+
+/* for Rx reordering buffer control */
+struct recv_reorder_ctrl {
+	struct adapter	*padapter;
+	u8 enable;
+	u16 indicate_seq;/* wstart_b, init_value = 0xffff */
+	u16 wend_b;
+	u8 wsize_b;
+	struct __queue pending_recvframe_queue;
+	struct timer_list reordering_ctrl_timer;
+};
+
+struct	stainfo_rxcache	{
+	u16 tid_rxseq[16];
+/*
+	unsigned short	tid0_rxseq;
+	unsigned short	tid1_rxseq;
+	unsigned short	tid2_rxseq;
+	unsigned short	tid3_rxseq;
+	unsigned short	tid4_rxseq;
+	unsigned short	tid5_rxseq;
+	unsigned short	tid6_rxseq;
+	unsigned short	tid7_rxseq;
+	unsigned short	tid8_rxseq;
+	unsigned short	tid9_rxseq;
+	unsigned short	tid10_rxseq;
+	unsigned short	tid11_rxseq;
+	unsigned short	tid12_rxseq;
+	unsigned short	tid13_rxseq;
+	unsigned short	tid14_rxseq;
+	unsigned short	tid15_rxseq;
+*/
+};
+
+
+struct signal_stat {
+	u8 update_req;		/* used to indicate */
+	u8 avg_val;		/* avg of valid elements */
+	u32 total_num;		/* num of valid elements */
+	u32 total_val;		/* sum of valid elements */
+};
+
+struct phy_info {
+	u8 rx_pwd_ba11;
+
+	u8 SignalQuality;	 /*  in 0-100 index. */
+	s8		rx_mimo_signal_quality[4];	/* per-path's EVM */
+	u8 RxMIMOEVMdbm[4];		/* per-path's EVM dbm */
+
+	u8 rx_mimo_signal_strength[4];/*  in 0~100 index */
+
+	u16 	Cfo_short[4];			/*  per-path's Cfo_short */
+	u16 	Cfo_tail[4];			/*  per-path's Cfo_tail */
+
+	s8		RxPower; /*  in dBm Translate from PWdB */
+	s8		RecvSignalPower;/*  Real power in dBm for this packet, no beautification and aggregation. Keep this raw info to be used for the other procedures. */
+	u8 bt_rx_rssi_percentage;
+	u8 SignalStrength; /*  in 0-100 index. */
+
+	s8		RxPwr[4];				/* per-path's pwdb */
+	u8 RxSNR[4];				/* per-path's SNR */
+	u8 BandWidth;
+	u8 btCoexPwrAdjust;
+};
+
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+struct rx_raw_rssi {
+	u8 data_rate;
+	u8 pwdball;
+	s8 pwr_all;
+
+	u8 mimo_signal_strength[4];/*  in 0~100 index */
+	u8 mimo_signal_quality[4];
+
+	s8 ofdm_pwr[4];
+	u8 ofdm_snr[4];
+
+};
+#endif
+
+struct rx_pkt_attrib	{
+	u16 pkt_len;
+	u8 physt;
+	u8 drvinfo_sz;
+	u8 shift_sz;
+	u8 hdrlen; /* the WLAN Header Len */
+	u8 to_fr_ds;
+	u8 amsdu;
+	u8 qos;
+	u8 priority;
+	u8 pw_save;
+	u8 mdata;
+	u16 seq_num;
+	u8 frag_num;
+	u8 mfrag;
+	u8 order;
+	u8 privacy; /* in frame_ctrl field */
+	u8 bdecrypted;
+	u8 encrypt; /* when 0 indicates no encryption; when non-zero, indicates the encryption algorithm */
+	u8 iv_len;
+	u8 icv_len;
+	u8 crc_err;
+	u8 icv_err;
+
+	u16 eth_type;
+
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 ta[ETH_ALEN];
+	u8 ra[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+
+	u8 ack_policy;
+
+	u8 key_index;
+
+	u8 data_rate;
+	u8 sgi;
+	u8 pkt_rpt_type;
+	u32 MacIDValidEntry[2];	/*  64 bits present 64 entry. */
+
+/*
+	u8 signal_qual;
+	s8	rx_mimo_signal_qual[2];
+	u8 signal_strength;
+	u32 rx_pwd_ba11;
+	s32	RecvSignalPower;
+*/
+	struct phy_info phy_info;
+};
+
+
+/* These definition is used for Rx packet reordering. */
+#define SN_LESS(a, b)		(((a - b) & 0x800) != 0)
+#define SN_EQUAL(a, b)	(a == b)
+/* define REORDER_WIN_SIZE	128 */
+/* define REORDER_ENTRY_NUM	128 */
+#define REORDER_WAIT_TIME	(50) /*  (ms) */
+
+#define RECVBUFF_ALIGN_SZ 8
+
+#define RXDESC_SIZE	24
+#define RXDESC_OFFSET RXDESC_SIZE
+
+struct recv_stat {
+	__le32 rxdw0;
+	__le32 rxdw1;
+	__le32 rxdw2;
+	__le32 rxdw3;
+#ifndef BUF_DESC_ARCH
+	__le32 rxdw4;
+	__le32 rxdw5;
+#endif /* if BUF_DESC_ARCH is defined, rx_buf_desc occupy 4 double words */
+};
+
+#define EOR BIT(30)
+
+/*
+accesser of recv_priv: rtw_recv_entry(dispatch / passive level); recv_thread(passive) ; returnpkt(dispatch)
+; halt(passive) ;
+
+using enter_critical section to protect
+*/
+struct recv_priv {
+	spinlock_t	lock;
+	struct __queue	free_recv_queue;
+	struct __queue	recv_pending_queue;
+	struct __queue	uc_swdec_pending_queue;
+	u8 *pallocated_frame_buf;
+	u8 *precv_frame_buf;
+	uint free_recvframe_cnt;
+	struct adapter	*adapter;
+	u32 bIsAnyNonBEPkts;
+	u64	rx_bytes;
+	u64	rx_pkts;
+	u64	rx_drop;
+	uint  rx_icv_err;
+	uint  rx_largepacket_crcerr;
+	uint  rx_smallpacket_crcerr;
+	uint  rx_middlepacket_crcerr;
+
+	struct tasklet_struct irq_prepare_beacon_tasklet;
+	struct tasklet_struct recv_tasklet;
+	struct sk_buff_head free_recv_skb_queue;
+	struct sk_buff_head rx_skb_queue;
+
+	u8 *pallocated_recv_buf;
+	u8 *precv_buf;    /*  4 alignment */
+	struct __queue	free_recv_buf_queue;
+	u32 free_recv_buf_queue_cnt;
+
+	struct __queue	recv_buf_pending_queue;
+
+	/* For display the phy information */
+	u8 is_signal_dbg;	/*  for debug */
+	u8 signal_strength_dbg;	/*  for debug */
+
+	u8 signal_strength;
+	u8 signal_qual;
+	s8 rssi;	/* translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength); */
+	#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	struct rx_raw_rssi raw_rssi_info;
+	#endif
+	/* s8 rxpwdb; */
+	s16 noise;
+	/* int RxSNRdB[2]; */
+	/* s8 RxRssi[2]; */
+	/* int FalseAlmCnt_all; */
+
+
+	struct timer_list signal_stat_timer;
+	u32 signal_stat_sampling_interval;
+	/* u32 signal_stat_converging_constant; */
+	struct signal_stat signal_qual_data;
+	struct signal_stat signal_strength_data;
+};
+
+#define rtw_set_signal_stat_timer(recvpriv) _set_timer(&(recvpriv)->signal_stat_timer, (recvpriv)->signal_stat_sampling_interval)
+
+struct sta_recv_priv {
+
+	spinlock_t	lock;
+	signed int	option;
+
+	/* struct __queue	blk_strms[MAX_RX_NUMBLKS]; */
+	struct __queue defrag_q;	 /* keeping the fragment frame until defrag */
+
+	struct	stainfo_rxcache rxcache;
+
+	/* uint	sta_rx_bytes; */
+	/* uint	sta_rx_pkts; */
+	/* uint	sta_rx_fail; */
+
+};
+
+
+struct recv_buf {
+	struct list_head list;
+
+	spinlock_t recvbuf_lock;
+
+	u32 ref_cnt;
+
+	struct adapter *adapter;
+
+	u8 *pbuf;
+	u8 *pallocated_buf;
+
+	u32 len;
+	u8 *phead;
+	u8 *pdata;
+	u8 *ptail;
+	u8 *pend;
+
+	struct sk_buff	*pskb;
+	u8 reuse;
+};
+
+
+/*
+	head  ----->
+
+		data  ----->
+
+			payload
+
+		tail  ----->
+
+
+	end   ----->
+
+	len = (unsigned int)(tail - data);
+
+*/
+struct recv_frame_hdr {
+	struct list_head	list;
+	struct sk_buff	 *pkt;
+	struct sk_buff	 *pkt_newalloc;
+
+	struct adapter  *adapter;
+
+	u8 fragcnt;
+
+	int frame_tag;
+
+	struct rx_pkt_attrib attrib;
+
+	uint  len;
+	u8 *rx_head;
+	u8 *rx_data;
+	u8 *rx_tail;
+	u8 *rx_end;
+
+	void *precvbuf;
+
+
+	/*  */
+	struct sta_info *psta;
+
+	/* for A-MPDU Rx reordering buffer control */
+	struct recv_reorder_ctrl *preorder_ctrl;
+};
+
+
+union recv_frame {
+	union{
+		struct list_head list;
+		struct recv_frame_hdr hdr;
+		uint mem[RECVFRAME_HDR_ALIGN>>2];
+	} u;
+
+	/* uint mem[MAX_RXSZ>>2]; */
+
+};
+
+enum {
+	NORMAL_RX,/* Normal rx packet */
+	TX_REPORT1,/* CCX */
+	TX_REPORT2,/* TX RPT */
+	HIS_REPORT,/*  USB HISR RPT */
+	C2H_PACKET
+};
+
+extern union recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue);  /* get a free recv_frame from pfree_recv_queue */
+extern union recv_frame *rtw_alloc_recvframe(struct __queue *pfree_recv_queue);  /* get a free recv_frame from pfree_recv_queue */
+extern int	 rtw_free_recvframe(union recv_frame *precvframe, struct __queue *pfree_recv_queue);
+
+#define rtw_dequeue_recvframe(queue) rtw_alloc_recvframe(queue)
+extern int _rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue);
+extern int rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue);
+
+extern void rtw_free_recvframe_queue(struct __queue *pframequeue,  struct __queue *pfree_recv_queue);
+u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter);
+
+signed int rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue);
+signed int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue);
+struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue);
+
+void rtw_reordering_ctrl_timeout_handler(struct timer_list *t);
+
+static inline u8 *get_rxmem(union recv_frame *precvframe)
+{
+	/* always return rx_head... */
+	if (precvframe == NULL)
+		return NULL;
+
+	return precvframe->u.hdr.rx_head;
+}
+
+static inline u8 *recvframe_pull(union recv_frame *precvframe, signed int sz)
+{
+	/*  rx_data += sz; move rx_data sz bytes  hereafter */
+
+	/* used for extract sz bytes from rx_data, update rx_data and return the updated rx_data to the caller */
+
+
+	if (precvframe == NULL)
+		return NULL;
+
+
+	precvframe->u.hdr.rx_data += sz;
+
+	if (precvframe->u.hdr.rx_data > precvframe->u.hdr.rx_tail)
+	{
+		precvframe->u.hdr.rx_data -= sz;
+		return NULL;
+	}
+
+	precvframe->u.hdr.len -= sz;
+
+	return precvframe->u.hdr.rx_data;
+
+}
+
+static inline u8 *recvframe_put(union recv_frame *precvframe, signed int sz)
+{
+	/*  rx_tai += sz; move rx_tail sz bytes  hereafter */
+
+	/* used for append sz bytes from ptr to rx_tail, update rx_tail and return the updated rx_tail to the caller */
+	/* after putting, rx_tail must be still larger than rx_end. */
+	unsigned char *prev_rx_tail;
+
+	if (precvframe == NULL)
+		return NULL;
+
+	prev_rx_tail = precvframe->u.hdr.rx_tail;
+
+	precvframe->u.hdr.rx_tail += sz;
+
+	if (precvframe->u.hdr.rx_tail > precvframe->u.hdr.rx_end)
+	{
+		precvframe->u.hdr.rx_tail = prev_rx_tail;
+		return NULL;
+	}
+
+	precvframe->u.hdr.len += sz;
+
+	return precvframe->u.hdr.rx_tail;
+
+}
+
+
+
+static inline u8 *recvframe_pull_tail(union recv_frame *precvframe, signed int sz)
+{
+	/*  rmv data from rx_tail (by yitsen) */
+
+	/* used for extract sz bytes from rx_end, update rx_end and return the updated rx_end to the caller */
+	/* after pulling, rx_end must be still larger than rx_data. */
+
+	if (precvframe == NULL)
+		return NULL;
+
+	precvframe->u.hdr.rx_tail -= sz;
+
+	if (precvframe->u.hdr.rx_tail < precvframe->u.hdr.rx_data)
+	{
+		precvframe->u.hdr.rx_tail += sz;
+		return NULL;
+	}
+
+	precvframe->u.hdr.len -= sz;
+
+	return precvframe->u.hdr.rx_tail;
+
+}
+
+static inline union recv_frame *rxmem_to_recvframe(u8 *rxmem)
+{
+	/* due to the design of 2048 bytes alignment of recv_frame, we can reference the union recv_frame */
+	/* from any given member of recv_frame. */
+	/*  rxmem indicates the any member/address in recv_frame */
+
+	return (union recv_frame *)(((SIZE_PTR)rxmem >> RXFRAME_ALIGN) << RXFRAME_ALIGN);
+
+}
+
+static inline signed int get_recvframe_len(union recv_frame *precvframe)
+{
+	return precvframe->u.hdr.len;
+}
+
+
+static inline s32 translate_percentage_to_dbm(u32 SignalStrengthIndex)
+{
+	s32	SignalPower; /*  in dBm. */
+
+	/*  Translate to dBm (x = 0.5y-95). */
+	SignalPower = (s32)((SignalStrengthIndex + 1) >> 1);
+	SignalPower -= 95;
+
+	return SignalPower;
+}
+
+
+struct sta_info;
+
+extern void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv);
+
+extern void  mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_rf.h b/drivers/staging/rtl8723bs/include/rtw_rf.h
new file mode 100644
index 000000000..718275ee4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_rf.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef	__RTW_RF_H_
+#define __RTW_RF_H_
+
+
+#define OFDM_PHY		1
+#define MIXED_PHY		2
+#define CCK_PHY			3
+
+#define NumRates		13
+
+/*  slot time for 11g */
+#define SHORT_SLOT_TIME		9
+#define NON_SHORT_SLOT_TIME	20
+
+#define RTL8711_RF_MAX_SENS	 6
+#define RTL8711_RF_DEF_SENS	 4
+
+/*
+ * We now define the following channels as the max channels in each channel plan.
+ * 2G, total 14 chnls
+ * {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}
+ */
+#define	MAX_CHANNEL_NUM_2G	14
+#define	MAX_CHANNEL_NUM		14
+
+#define NUM_REGULATORYS	1
+
+/* Country codes */
+#define USA			0x555320
+#define EUROPE			0x1 /* temp, should be provided later */
+#define JAPAN			0x2 /* temp, should be provided later */
+
+struct	regulatory_class {
+	u32 starting_freq;			/* MHz, */
+	u8 channel_set[MAX_CHANNEL_NUM];
+	u8 channel_cck_power[MAX_CHANNEL_NUM];/* dbm */
+	u8 channel_ofdm_power[MAX_CHANNEL_NUM];/* dbm */
+	u8 txpower_limit;			/* dbm */
+	u8 channel_spacing;			/* MHz */
+	u8 modem;
+};
+
+enum {
+	cESS			= 0x0001,
+	cIBSS			= 0x0002,
+	cPollable		= 0x0004,
+	cPollReq		= 0x0008,
+	cPrivacy		= 0x0010,
+	cShortPreamble		= 0x0020,
+	cPBCC			= 0x0040,
+	cChannelAgility		= 0x0080,
+	cSpectrumMgnt		= 0x0100,
+	cQos			= 0x0200,	/*  For HCCA, use with CF-Pollable and CF-PollReq */
+	cShortSlotTime		= 0x0400,
+	cAPSD			= 0x0800,
+	cRM			= 0x1000,	/*  RRM (Radio Request Measurement) */
+	cDSSS_OFDM		= 0x2000,
+	cDelayedBA		= 0x4000,
+	cImmediateBA		= 0x8000,
+};
+
+enum {
+	PREAMBLE_LONG	= 1,
+	PREAMBLE_AUTO	= 2,
+	PREAMBLE_SHORT	= 3,
+};
+
+/*  Bandwidth Offset */
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE	0
+#define HAL_PRIME_CHNL_OFFSET_LOWER	1
+#define HAL_PRIME_CHNL_OFFSET_UPPER	2
+
+/*  Represent Channel Width in HT Capabilities */
+enum channel_width {
+	CHANNEL_WIDTH_20 = 0,
+	CHANNEL_WIDTH_40 = 1,
+};
+
+/*  Represent Extension Channel Offset in HT Capabilities */
+/*  This is available only in 40Mhz mode. */
+enum extchnl_offset {
+	EXTCHNL_OFFSET_NO_EXT = 0,
+	EXTCHNL_OFFSET_UPPER = 1,
+	EXTCHNL_OFFSET_NO_DEF = 2,
+	EXTCHNL_OFFSET_LOWER = 3,
+};
+
+enum {
+	HT_DATA_SC_DONOT_CARE = 0,
+	HT_DATA_SC_20_UPPER_OF_40MHZ = 1,
+	HT_DATA_SC_20_LOWER_OF_40MHZ = 2,
+};
+
+u32 rtw_ch2freq(u32 ch);
+
+#endif /* _RTL8711_RF_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_security.h b/drivers/staging/rtl8723bs/include/rtw_security.h
new file mode 100644
index 000000000..7587fa888
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_security.h
@@ -0,0 +1,278 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __RTW_SECURITY_H_
+#define __RTW_SECURITY_H_
+
+#include <crypto/arc4.h>
+
+#define _NO_PRIVACY_		0x0
+#define _WEP40_				0x1
+#define _TKIP_				0x2
+#define _TKIP_WTMIC_		0x3
+#define _AES_				0x4
+#define _WEP104_			0x5
+#define _WEP_WPA_MIXED_	0x07  /*  WEP + WPA */
+#define _SMS4_				0x06
+#define _BIP_				0x8
+#define is_wep_enc(alg) (((alg) == _WEP40_) || ((alg) == _WEP104_))
+
+const char *security_type_str(u8 value);
+
+#define SHA256_MAC_LEN 32
+#define AES_BLOCK_SIZE 16
+#define AES_PRIV_SIZE (4 * 44)
+
+#define RTW_KEK_LEN 16
+#define RTW_KCK_LEN 16
+#define RTW_REPLAY_CTR_LEN 8
+
+enum {
+	ENCRYP_PROTOCOL_OPENSYS,   /* open system */
+	ENCRYP_PROTOCOL_WEP,       /* WEP */
+	ENCRYP_PROTOCOL_WPA,       /* WPA */
+	ENCRYP_PROTOCOL_WPA2,      /* WPA2 */
+	ENCRYP_PROTOCOL_WAPI,      /* WAPI: Not support in this version */
+	ENCRYP_PROTOCOL_MAX
+};
+
+
+#ifndef Ndis802_11AuthModeWPA2
+#define Ndis802_11AuthModeWPA2 (Ndis802_11AuthModeWPANone + 1)
+#endif
+
+#ifndef Ndis802_11AuthModeWPA2PSK
+#define Ndis802_11AuthModeWPA2PSK (Ndis802_11AuthModeWPANone + 2)
+#endif
+
+union pn48	{
+
+	u64	val;
+
+#ifdef __LITTLE_ENDIAN
+
+struct {
+  u8 TSC0;
+  u8 TSC1;
+  u8 TSC2;
+  u8 TSC3;
+  u8 TSC4;
+  u8 TSC5;
+  u8 TSC6;
+  u8 TSC7;
+} _byte_;
+#else
+struct {
+  u8 TSC7;
+  u8 TSC6;
+  u8 TSC5;
+  u8 TSC4;
+  u8 TSC3;
+  u8 TSC2;
+  u8 TSC1;
+  u8 TSC0;
+} _byte_;
+#endif
+
+};
+
+union Keytype {
+        u8   skey[16];
+        u32    lkey[4];
+};
+
+
+struct rt_pmkid_list {
+	u8 				bUsed;
+	u8 				Bssid[6];
+	u8 				PMKID[16];
+	u8 				SsidBuf[33];
+	u8 *ssid_octet;
+	u16 					ssid_length;
+};
+
+
+struct security_priv {
+	u32   dot11AuthAlgrthm;		/*  802.11 auth, could be open, shared, 8021x and authswitch */
+	u32   dot11PrivacyAlgrthm;	/*  This specify the privacy for shared auth. algorithm. */
+
+	/* WEP */
+	u32   dot11PrivacyKeyIndex;	/*  this is only valid for legendary wep, 0~3 for key id. (tx key index) */
+	union Keytype dot11DefKey[4];	/*  this is only valid for def. key */
+	u32 dot11DefKeylen[4];
+	u8 key_mask; /* use to restore wep key after hal_init */
+
+	u32 dot118021XGrpPrivacy;	/*  This specify the privacy algthm. used for Grp key */
+	u32 dot118021XGrpKeyid;		/*  key id used for Grp Key (tx key index) */
+	union Keytype	dot118021XGrpKey[BIP_MAX_KEYID + 1];	/*  802.1x Group Key, for inx0 and inx1 */
+	union Keytype	dot118021XGrptxmickey[BIP_MAX_KEYID + 1];
+	union Keytype	dot118021XGrprxmickey[BIP_MAX_KEYID + 1];
+	union pn48		dot11Grptxpn;			/*  PN48 used for Grp Key xmit. */
+	union pn48		dot11Grprxpn;			/*  PN48 used for Grp Key recv. */
+	u32 dot11wBIPKeyid;						/*  key id used for BIP Key (tx key index) */
+	union Keytype	dot11wBIPKey[BIP_MAX_KEYID + 1];	/*  BIP Key, for index4 and index5 */
+	union pn48		dot11wBIPtxpn;			/*  PN48 used for Grp Key xmit. */
+	union pn48		dot11wBIPrxpn;			/*  PN48 used for Grp Key recv. */
+
+	/* extend security capabilities for AP_MODE */
+	unsigned int dot8021xalg;/* 0:disable, 1:psk, 2:802.1x */
+	unsigned int wpa_psk;/* 0:disable, bit(0): WPA, bit(1):WPA2 */
+	unsigned int wpa_group_cipher;
+	unsigned int wpa2_group_cipher;
+	unsigned int wpa_pairwise_cipher;
+	unsigned int wpa2_pairwise_cipher;
+
+	u8 wps_ie[MAX_WPS_IE_LEN];/* added in assoc req */
+	int wps_ie_len;
+
+	struct arc4_ctx xmit_arc4_ctx;
+	struct arc4_ctx recv_arc4_ctx;
+
+	u8 binstallGrpkey;
+	u8 binstallBIPkey;
+	u8 busetkipkey;
+	/* _timer tkip_timer; */
+	u8 bcheck_grpkey;
+	u8 bgrpkey_handshake;
+
+	s32	sw_encrypt;/* from registry_priv */
+	s32	sw_decrypt;/* from registry_priv */
+
+	s32	hw_decrypted;/* if the rx packets is hw_decrypted ==false, it means the hw has not been ready. */
+
+
+	/* keeps the auth_type & enc_status from upper layer ioctl(wpa_supplicant or wzc) */
+	u32 ndisauthtype;	/*  enum ndis_802_11_authentication_mode */
+	u32 ndisencryptstatus;	/*  NDIS_802_11_ENCRYPTION_STATUS */
+
+	struct wlan_bssid_ex sec_bss;  /* for joinbss (h2c buffer) usage */
+
+	struct ndis_802_11_wep ndiswep;
+
+	u8 assoc_info[600];
+	u8 szofcapability[256]; /* for wpa2 usage */
+	u8 oidassociation[512]; /* for wpa/wpa2 usage */
+	u8 authenticator_ie[256];  /* store ap security information element */
+	u8 supplicant_ie[256];  /* store sta security information element */
+
+
+	/* for tkip countermeasure */
+	unsigned long last_mic_err_time;
+	u8 btkip_countermeasure;
+	u8 btkip_wait_report;
+	u32 btkip_countermeasure_time;
+
+	/*  For WPA2 Pre-Authentication. */
+	struct rt_pmkid_list		PMKIDList[NUM_PMKID_CACHE];	/*  Renamed from PreAuthKey[NUM_PRE_AUTH_KEY]. Annie, 2006-10-13. */
+	u8 		PMKIDIndex;
+
+	u8 bWepDefaultKeyIdxSet;
+
+};
+
+#define GET_ENCRY_ALGO(psecuritypriv, psta, encry_algo, bmcst)\
+do {\
+	switch (psecuritypriv->dot11AuthAlgrthm)\
+	{\
+		case dot11AuthAlgrthm_Open:\
+		case dot11AuthAlgrthm_Shared:\
+		case dot11AuthAlgrthm_Auto:\
+			encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;\
+			break;\
+		case dot11AuthAlgrthm_8021X:\
+			if (bmcst)\
+				encry_algo = (u8)psecuritypriv->dot118021XGrpPrivacy;\
+			else\
+				encry_algo = (u8)psta->dot118021XPrivacy;\
+			break;\
+	     case dot11AuthAlgrthm_WAPI:\
+		     encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;\
+		     break;\
+	} \
+} while (0)
+
+#define SET_ICE_IV_LEN(iv_len, icv_len, encrypt)\
+do {\
+	switch (encrypt)\
+	{\
+		case _WEP40_:\
+		case _WEP104_:\
+			iv_len = 4;\
+			icv_len = 4;\
+			break;\
+		case _TKIP_:\
+			iv_len = 8;\
+			icv_len = 4;\
+			break;\
+		case _AES_:\
+			iv_len = 8;\
+			icv_len = 8;\
+			break;\
+		case _SMS4_:\
+			iv_len = 18;\
+			icv_len = 16;\
+			break;\
+		default:\
+			iv_len = 0;\
+			icv_len = 0;\
+			break;\
+	} \
+} while (0)
+
+
+#define GET_TKIP_PN(iv, dot11txpn)\
+do {\
+	dot11txpn._byte_.TSC0 = iv[2];\
+	dot11txpn._byte_.TSC1 = iv[0];\
+	dot11txpn._byte_.TSC2 = iv[4];\
+	dot11txpn._byte_.TSC3 = iv[5];\
+	dot11txpn._byte_.TSC4 = iv[6];\
+	dot11txpn._byte_.TSC5 = iv[7];\
+} while (0)
+
+
+#define ROL32(A, n)	(((A) << (n)) | (((A)>>(32-(n)))  & ((1UL << (n)) - 1)))
+#define ROR32(A, n)	ROL32((A), 32-(n))
+
+struct mic_data {
+	u32  K0, K1;         /*  Key */
+	u32  L, R;           /*  Current state */
+	u32  M;              /*  Message accumulator (single word) */
+	u32     nBytesInM;      /*  # bytes in M */
+};
+
+/* ===== start - public domain SHA256 implementation ===== */
+
+/* This is based on SHA256 implementation in LibTomCrypt that was released into
+ * public domain by Tom St Denis. */
+
+int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac);
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key);
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b);
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nBytes);
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst);
+
+void rtw_seccalctkipmic(
+	u8 *key,
+	u8 *header,
+	u8 *data,
+	u32 data_len,
+	u8 *Miccode,
+	u8   priority);
+
+u32 rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe);
+u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe);
+void rtw_wep_encrypt(struct adapter *padapter, u8  *pxmitframe);
+
+u32 rtw_aes_decrypt(struct adapter *padapter, u8  *precvframe);
+u32 rtw_tkip_decrypt(struct adapter *padapter, u8  *precvframe);
+void rtw_wep_decrypt(struct adapter *padapter, u8  *precvframe);
+u32 rtw_BIP_verify(struct adapter *padapter, u8 *precvframe);
+
+void rtw_sec_restore_wep_key(struct adapter *adapter);
+u8 rtw_handle_tkip_countermeasure(struct adapter *adapter, const char *caller);
+
+#endif	/* __RTL871X_SECURITY_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_version.h b/drivers/staging/rtl8723bs/include/rtw_version.h
new file mode 100644
index 000000000..55e907b09
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_version.h
@@ -0,0 +1,3 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#define DRIVERVERSION	"v4.3.5.5_12290.20140916_BTCOEX20140507-4E40"
+#define BTCOEXVERSION	"BTCOEX20140507-4E40"
diff --git a/drivers/staging/rtl8723bs/include/rtw_wifi_regd.h b/drivers/staging/rtl8723bs/include/rtw_wifi_regd.h
new file mode 100644
index 000000000..e611651cb
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_wifi_regd.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ *****************************************************************************/
+
+#ifndef __RTW_WIFI_REGD_H__
+#define __RTW_WIFI_REGD_H__
+
+void rtw_regd_init(struct wiphy *wiphy,
+		   void (*reg_notifier)(struct wiphy *wiphy,
+					struct regulatory_request *request));
+void rtw_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
+
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_xmit.h b/drivers/staging/rtl8723bs/include/rtw_xmit.h
new file mode 100644
index 000000000..676ead037
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_xmit.h
@@ -0,0 +1,501 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _RTW_XMIT_H_
+#define _RTW_XMIT_H_
+
+#include <linux/completion.h>
+
+#define MAX_XMITBUF_SZ	(20480)	/*  20k */
+
+#define NR_XMITBUFF	(16)
+
+#define XMITBUF_ALIGN_SZ 512
+
+/*  xmit extension buff defination */
+#define MAX_XMIT_EXTBUF_SZ	(1536)
+#define NR_XMIT_EXTBUFF	(32)
+
+#define MAX_CMDBUF_SZ	(5120)	/* 4096) */
+
+#define MAX_NUMBLKS		(1)
+
+#define XMIT_VO_QUEUE (0)
+#define XMIT_VI_QUEUE (1)
+#define XMIT_BE_QUEUE (2)
+#define XMIT_BK_QUEUE (3)
+
+#define VO_QUEUE_INX		0
+#define VI_QUEUE_INX		1
+#define BE_QUEUE_INX		2
+#define BK_QUEUE_INX		3
+#define BCN_QUEUE_INX		4
+#define MGT_QUEUE_INX		5
+#define HIGH_QUEUE_INX		6
+#define TXCMD_QUEUE_INX	7
+
+#define HW_QUEUE_ENTRY	8
+
+#define WEP_IV(pattrib_iv, dot11txpn, keyidx)\
+do {\
+	pattrib_iv[0] = dot11txpn._byte_.TSC0;\
+	pattrib_iv[1] = dot11txpn._byte_.TSC1;\
+	pattrib_iv[2] = dot11txpn._byte_.TSC2;\
+	pattrib_iv[3] = ((keyidx & 0x3)<<6);\
+	dot11txpn.val = (dot11txpn.val == 0xffffff) ? 0 : (dot11txpn.val + 1);\
+} while (0)
+
+
+#define TKIP_IV(pattrib_iv, dot11txpn, keyidx)\
+do {\
+	pattrib_iv[0] = dot11txpn._byte_.TSC1;\
+	pattrib_iv[1] = (dot11txpn._byte_.TSC1 | 0x20) & 0x7f;\
+	pattrib_iv[2] = dot11txpn._byte_.TSC0;\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3)<<6);\
+	pattrib_iv[4] = dot11txpn._byte_.TSC2;\
+	pattrib_iv[5] = dot11txpn._byte_.TSC3;\
+	pattrib_iv[6] = dot11txpn._byte_.TSC4;\
+	pattrib_iv[7] = dot11txpn._byte_.TSC5;\
+	dot11txpn.val = dot11txpn.val == 0xffffffffffffULL ? 0 : (dot11txpn.val + 1);\
+} while (0)
+
+#define AES_IV(pattrib_iv, dot11txpn, keyidx)\
+do {\
+	pattrib_iv[0] = dot11txpn._byte_.TSC0;\
+	pattrib_iv[1] = dot11txpn._byte_.TSC1;\
+	pattrib_iv[2] = 0;\
+	pattrib_iv[3] = BIT(5) | ((keyidx & 0x3)<<6);\
+	pattrib_iv[4] = dot11txpn._byte_.TSC2;\
+	pattrib_iv[5] = dot11txpn._byte_.TSC3;\
+	pattrib_iv[6] = dot11txpn._byte_.TSC4;\
+	pattrib_iv[7] = dot11txpn._byte_.TSC5;\
+	dot11txpn.val = dot11txpn.val == 0xffffffffffffULL ? 0 : (dot11txpn.val + 1);\
+} while (0)
+
+
+#define HWXMIT_ENTRY	4
+
+/*  For Buffer Descriptor ring architecture */
+#define TXDESC_SIZE 40
+
+#define TXDESC_OFFSET TXDESC_SIZE
+
+#define TXDESC_40_BYTES
+
+struct tx_desc {
+	__le32 txdw0;
+	__le32 txdw1;
+	__le32 txdw2;
+	__le32 txdw3;
+	__le32 txdw4;
+	__le32 txdw5;
+	__le32 txdw6;
+	__le32 txdw7;
+
+#if defined(TXDESC_40_BYTES) || defined(TXDESC_64_BYTES)
+	__le32 txdw8;
+	__le32 txdw9;
+#endif /*  TXDESC_40_BYTES */
+
+#ifdef TXDESC_64_BYTES
+	__le32 txdw10;
+	__le32 txdw11;
+
+	/*  2008/05/15 MH Because PCIE HW memory R/W 4K limit. And now,  our descriptor */
+	/*  size is 40 bytes. If you use more than 102 descriptor(103*40>4096), HW will execute */
+	/*  memoryR/W CRC error. And then all DMA fetch will fail. We must decrease descriptor */
+	/*  number or enlarge descriptor size as 64 bytes. */
+	__le32 txdw12;
+	__le32 txdw13;
+	__le32 txdw14;
+	__le32 txdw15;
+#endif
+};
+
+union txdesc {
+	struct tx_desc txdesc;
+	unsigned int value[TXDESC_SIZE>>2];
+};
+
+struct	hw_xmit	{
+	/* spinlock_t xmit_lock; */
+	/* struct list_head	pending; */
+	struct __queue *sta_queue;
+	/* struct hw_txqueue *phwtxqueue; */
+	/* signed int	txcmdcnt; */
+	int	accnt;
+};
+
+/* reduce size */
+struct pkt_attrib {
+	u8 type;
+	u8 subtype;
+	u8 bswenc;
+	u8 dhcp_pkt;
+	u16 ether_type;
+	u16 seqnum;
+	u16 pkt_hdrlen;	/* the original 802.3 pkt header len */
+	u16 hdrlen;		/* the WLAN Header Len */
+	u32 pktlen;		/* the original 802.3 pkt raw_data len (not include ether_hdr data) */
+	u32 last_txcmdsz;
+	u8 nr_frags;
+	u8 encrypt;	/* when 0 indicates no encryption; when non-zero, indicates the encryption algorithm */
+	u8 iv_len;
+	u8 icv_len;
+	u8 iv[18];
+	u8 icv[16];
+	u8 priority;
+	u8 ack_policy;
+	u8 mac_id;
+	u8 vcs_mode;	/* virtual carrier sense method */
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 ta[ETH_ALEN];
+	u8 ra[ETH_ALEN];
+	u8 key_idx;
+	u8 qos_en;
+	u8 ht_en;
+	u8 raid;/* rate adpative id */
+	u8 bwmode;
+	u8 ch_offset;/* PRIME_CHNL_OFFSET */
+	u8 sgi;/* short GI */
+	u8 ampdu_en;/* tx ampdu enable */
+	u8 ampdu_spacing; /* ampdu_min_spacing for peer sta's rx */
+	u8 mdata;/* more data bit */
+	u8 pctrl;/* per packet txdesc control enable */
+	u8 triggered;/* for ap mode handling Power Saving sta */
+	u8 qsel;
+	u8 order;/* order bit */
+	u8 eosp;
+	u8 rate;
+	u8 intel_proxim;
+	u8 retry_ctrl;
+	u8   mbssid;
+	u8 ldpc;
+	u8 stbc;
+	struct sta_info *psta;
+
+	u8 rtsen;
+	u8 cts2self;
+	union Keytype	dot11tkiptxmickey;
+	/* union Keytype	dot11tkiprxmickey; */
+	union Keytype	dot118021x_UncstKey;
+
+	u8 icmp_pkt;
+
+};
+
+#define WLANHDR_OFFSET	64
+
+#define NULL_FRAMETAG		(0x0)
+#define DATA_FRAMETAG		0x01
+#define L2_FRAMETAG		0x02
+#define MGNT_FRAMETAG		0x03
+#define AMSDU_FRAMETAG	0x04
+
+#define EII_FRAMETAG		0x05
+#define IEEE8023_FRAMETAG  0x06
+
+#define MP_FRAMETAG		0x07
+
+#define TXAGG_FRAMETAG	0x08
+
+enum {
+	XMITBUF_DATA = 0,
+	XMITBUF_MGNT = 1,
+	XMITBUF_CMD = 2,
+};
+
+struct  submit_ctx {
+	unsigned long submit_time; /* */
+	u32 timeout_ms; /* <0: not synchronous, 0: wait forever, >0: up to ms waiting */
+	int status; /* status for operation */
+	struct completion done;
+};
+
+enum {
+	RTW_SCTX_SUBMITTED = -1,
+	RTW_SCTX_DONE_SUCCESS = 0,
+	RTW_SCTX_DONE_UNKNOWN,
+	RTW_SCTX_DONE_TIMEOUT,
+	RTW_SCTX_DONE_BUF_ALLOC,
+	RTW_SCTX_DONE_BUF_FREE,
+	RTW_SCTX_DONE_WRITE_PORT_ERR,
+	RTW_SCTX_DONE_TX_DESC_NA,
+	RTW_SCTX_DONE_TX_DENY,
+	RTW_SCTX_DONE_CCX_PKT_FAIL,
+	RTW_SCTX_DONE_DRV_STOP,
+	RTW_SCTX_DONE_DEV_REMOVE,
+	RTW_SCTX_DONE_CMD_ERROR,
+};
+
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms);
+int rtw_sctx_wait(struct submit_ctx *sctx);
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status);
+void rtw_sctx_done(struct submit_ctx **sctx);
+
+struct xmit_buf {
+	struct list_head	list;
+
+	struct adapter *padapter;
+
+	u8 *pallocated_buf;
+
+	u8 *pbuf;
+
+	void *priv_data;
+
+	u16 buf_tag; /*  0: Normal xmitbuf, 1: extension xmitbuf, 2:cmd xmitbuf */
+	u16 flags;
+	u32 alloc_sz;
+
+	u32  len;
+
+	struct submit_ctx *sctx;
+
+	u8 *phead;
+	u8 *pdata;
+	u8 *ptail;
+	u8 *pend;
+	u32 ff_hwaddr;
+	u8 pg_num;
+	u8 agg_num;
+
+#if defined(DBG_XMIT_BUF) || defined(DBG_XMIT_BUF_EXT)
+	u8 no;
+#endif
+
+};
+
+
+struct xmit_frame {
+	struct list_head	list;
+
+	struct pkt_attrib attrib;
+
+	struct sk_buff *pkt;
+
+	int	frame_tag;
+
+	struct adapter *padapter;
+
+	u8 *buf_addr;
+
+	struct xmit_buf *pxmitbuf;
+
+	u8 pg_num;
+	u8 agg_num;
+
+	u8 ack_report;
+
+	u8 *alloc_addr; /* the actual address this xmitframe allocated */
+	u8 ext_tag; /* 0:data, 1:mgmt */
+
+};
+
+struct tx_servq {
+	struct list_head	tx_pending;
+	struct __queue	sta_pending;
+	int qcnt;
+};
+
+
+struct sta_xmit_priv {
+	spinlock_t	lock;
+	signed int	option;
+	signed int	apsd_setting;	/* When bit mask is on, the associated edca queue supports APSD. */
+
+
+	/* struct tx_servq blk_q[MAX_NUMBLKS]; */
+	struct tx_servq	be_q;			/* priority == 0, 3 */
+	struct tx_servq	bk_q;			/* priority == 1, 2 */
+	struct tx_servq	vi_q;			/* priority == 4, 5 */
+	struct tx_servq	vo_q;			/* priority == 6, 7 */
+	struct list_head	legacy_dz;
+	struct list_head  apsd;
+
+	u16 txseq_tid[16];
+
+	/* uint	sta_tx_bytes; */
+	/* u64	sta_tx_pkts; */
+	/* uint	sta_tx_fail; */
+
+
+};
+
+
+struct	hw_txqueue	{
+	volatile signed int	head;
+	volatile signed int	tail;
+	volatile signed int	free_sz;	/* in units of 64 bytes */
+	volatile signed int      free_cmdsz;
+	volatile signed int	 txsz[8];
+	uint	ff_hwaddr;
+	uint	cmd_hwaddr;
+	signed int	ac_tag;
+};
+
+enum cmdbuf_type {
+	CMDBUF_BEACON = 0x00,
+	CMDBUF_RSVD,
+	CMDBUF_MAX
+};
+
+struct	xmit_priv {
+
+	spinlock_t	lock;
+
+	struct completion xmit_comp;
+	struct completion terminate_xmitthread_comp;
+
+	/* struct __queue	blk_strms[MAX_NUMBLKS]; */
+	struct __queue	be_pending;
+	struct __queue	bk_pending;
+	struct __queue	vi_pending;
+	struct __queue	vo_pending;
+	struct __queue	bm_pending;
+
+	/* struct __queue	legacy_dz_queue; */
+	/* struct __queue	apsd_queue; */
+
+	u8 *pallocated_frame_buf;
+	u8 *pxmit_frame_buf;
+	uint free_xmitframe_cnt;
+	struct __queue	free_xmit_queue;
+
+	/* uint mapping_addr; */
+	/* uint pkt_sz; */
+
+	u8 *xframe_ext_alloc_addr;
+	u8 *xframe_ext;
+	uint free_xframe_ext_cnt;
+	struct __queue free_xframe_ext_queue;
+
+	/* struct	hw_txqueue	be_txqueue; */
+	/* struct	hw_txqueue	bk_txqueue; */
+	/* struct	hw_txqueue	vi_txqueue; */
+	/* struct	hw_txqueue	vo_txqueue; */
+	/* struct	hw_txqueue	bmc_txqueue; */
+
+	uint	frag_len;
+
+	struct adapter	*adapter;
+
+	u8   vcs_setting;
+	u8 vcs;
+	u8 vcs_type;
+	/* u16  rts_thresh; */
+
+	u64	tx_bytes;
+	u64	tx_pkts;
+	u64	tx_drop;
+	u64	last_tx_pkts;
+
+	struct hw_xmit *hwxmits;
+	u8 hwxmit_entry;
+
+	u8 wmm_para_seq[4];/* sequence for wmm ac parameter strength from large to small. it's value is 0->vo, 1->vi, 2->be, 3->bk. */
+
+	void *SdioXmitThread;
+	struct completion SdioXmitStart;
+	struct completion SdioXmitTerminate;
+
+	struct __queue free_xmitbuf_queue;
+	struct __queue pending_xmitbuf_queue;
+	u8 *pallocated_xmitbuf;
+	u8 *pxmitbuf;
+	uint free_xmitbuf_cnt;
+
+	struct __queue free_xmit_extbuf_queue;
+	u8 *pallocated_xmit_extbuf;
+	u8 *pxmit_extbuf;
+	uint free_xmit_extbuf_cnt;
+
+	struct xmit_buf	pcmd_xmitbuf[CMDBUF_MAX];
+
+	u16 nqos_ssn;
+
+	int	ack_tx;
+	struct mutex ack_tx_mutex;
+	struct submit_ctx ack_tx_ops;
+	u8 seq_no;
+	spinlock_t lock_sctx;
+};
+
+extern struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv,
+		enum cmdbuf_type buf_type);
+#define rtw_alloc_cmdxmitframe(p) __rtw_alloc_cmdxmitframe(p, CMDBUF_RSVD)
+#define rtw_alloc_bcnxmitframe(p) __rtw_alloc_cmdxmitframe(p, CMDBUF_BEACON)
+
+extern struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv);
+extern s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+
+extern struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv);
+extern s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+
+void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz);
+extern void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len);
+extern s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib);
+extern s32 rtw_put_snap(u8 *data, u16 h_proto);
+
+extern struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv);
+struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv);
+struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv);
+extern s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe);
+extern void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue);
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, signed int up, u8 *ac);
+extern s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe);
+
+extern s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe);
+extern u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib);
+#define rtw_wlan_pkt_size(f) rtw_calculate_wlan_pkt_size_by_attribue(&f->attrib)
+extern s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe);
+extern s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe);
+s32 _rtw_init_hw_txqueue(struct hw_txqueue *phw_txqueue, u8 ac_tag);
+void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv);
+
+
+s32 rtw_txframes_pending(struct adapter *padapter);
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, signed int entry);
+
+
+s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter);
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv);
+
+
+s32 rtw_alloc_hwxmits(struct adapter *padapter);
+void rtw_free_hwxmits(struct adapter *padapter);
+
+
+s32 rtw_xmit(struct adapter *padapter, struct sk_buff **pkt);
+bool xmitframe_hiq_filter(struct xmit_frame *xmitframe);
+
+signed int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe);
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta);
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta);
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta);
+
+u8 query_ra_short_GI(struct sta_info *psta);
+
+u8 qos_acm(u8 acm_mask, u8 priority);
+
+void enqueue_pending_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+void enqueue_pending_xmitbuf_to_head(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+struct xmit_buf *dequeue_pending_xmitbuf(struct xmit_priv *pxmitpriv);
+struct xmit_buf *dequeue_pending_xmitbuf_under_survey(struct xmit_priv *pxmitpriv);
+signed int	check_pending_xmitbuf(struct xmit_priv *pxmitpriv);
+int	rtw_xmit_thread(void *context);
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame	*pxmitframe);
+
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms);
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status);
+
+/* include after declaring struct xmit_buf, in order to avoid warning */
+#include <xmit_osdep.h>
+
+#endif	/* _RTL871X_XMIT_H_ */
diff --git a/drivers/staging/rtl8723bs/include/sdio_hal.h b/drivers/staging/rtl8723bs/include/sdio_hal.h
new file mode 100644
index 000000000..6fae19dd0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_hal.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __SDIO_HAL_H__
+#define __SDIO_HAL_H__
+
+
+extern u8 sd_hal_bus_init(struct adapter *padapter);
+extern u8 sd_hal_bus_deinit(struct adapter *padapter);
+
+u8 sd_int_isr(struct adapter *padapter);
+void sd_int_dpc(struct adapter *padapter);
+void rtw_set_hal_ops(struct adapter *padapter);
+
+void rtl8723bs_set_hal_ops(struct adapter *padapter);
+
+#endif /* __SDIO_HAL_H__ */
diff --git a/drivers/staging/rtl8723bs/include/sdio_ops.h b/drivers/staging/rtl8723bs/include/sdio_ops.h
new file mode 100644
index 000000000..c7559a884
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_ops.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __SDIO_OPS_H__
+#define __SDIO_OPS_H__
+
+
+#include <sdio_ops_linux.h>
+
+extern void sdio_set_intf_ops(struct adapter *padapter, struct _io_ops *pops);
+
+/* extern void sdio_func1cmd52_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem); */
+/* extern void sdio_func1cmd52_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem); */
+extern u8 SdioLocalCmd52Read1Byte(struct adapter *padapter, u32 addr);
+extern void SdioLocalCmd52Write1Byte(struct adapter *padapter, u32 addr, u8 v);
+extern s32 sdio_local_read(struct adapter *padapter, u32 addr, u32 cnt, u8 *pbuf);
+extern s32 sdio_local_write(struct adapter *padapter, u32 addr, u32 cnt, u8 *pbuf);
+
+u32 _sdio_read32(struct adapter *padapter, u32 addr);
+s32 _sdio_write32(struct adapter *padapter, u32 addr, u32 val);
+
+extern void sd_int_hdl(struct adapter *padapter);
+extern u8 CheckIPSStatus(struct adapter *padapter);
+
+extern void InitInterrupt8723BSdio(struct adapter *padapter);
+extern void InitSysInterrupt8723BSdio(struct adapter *padapter);
+extern void EnableInterrupt8723BSdio(struct adapter *padapter);
+extern void DisableInterrupt8723BSdio(struct adapter *padapter);
+extern u8 HalQueryTxBufferStatus8723BSdio(struct adapter *padapter);
+extern void HalQueryTxOQTBufferStatus8723BSdio(struct adapter *padapter);
+#endif /*  !__SDIO_OPS_H__ */
diff --git a/drivers/staging/rtl8723bs/include/sdio_ops_linux.h b/drivers/staging/rtl8723bs/include/sdio_ops_linux.h
new file mode 100644
index 000000000..18830dd18
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_ops_linux.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __SDIO_OPS_LINUX_H__
+#define __SDIO_OPS_LINUX_H__
+
+#define SDIO_ERR_VAL8	0xEA
+#define SDIO_ERR_VAL16	0xEAEA
+#define SDIO_ERR_VAL32	0xEAEAEAEA
+
+s32 _sd_cmd52_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata);
+s32 _sd_cmd52_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata);
+s32 sd_cmd52_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata);
+s32 sd_cmd52_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata);
+
+u8 sd_read8(struct intf_hdl *pintfhdl, u32 addr, s32 *err);
+u32 sd_read32(struct intf_hdl *pintfhdl, u32 addr, s32 *err);
+s32 _sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata);
+s32 sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata);
+void sd_write8(struct intf_hdl *pintfhdl, u32 addr, u8 v, s32 *err);
+void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err);
+s32 _sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata);
+s32 sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata);
+
+
+void rtw_sdio_set_irq_thd(struct dvobj_priv *dvobj, void *thd_hdl);
+#endif
diff --git a/drivers/staging/rtl8723bs/include/sdio_osintf.h b/drivers/staging/rtl8723bs/include/sdio_osintf.h
new file mode 100644
index 000000000..146b44f95
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_osintf.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __SDIO_OSINTF_H__
+#define __SDIO_OSINTF_H__
+
+
+
+u8 sd_hal_bus_init(struct adapter *padapter);
+u8 sd_hal_bus_deinit(struct adapter *padapter);
+void sd_c2h_hdl(struct adapter *padapter);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/sta_info.h b/drivers/staging/rtl8723bs/include/sta_info.h
new file mode 100644
index 000000000..69c377eee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sta_info.h
@@ -0,0 +1,374 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __STA_INFO_H_
+#define __STA_INFO_H_
+
+
+#define IBSS_START_MAC_ID	2
+#define NUM_STA 32
+#define NUM_ACL 16
+
+
+/* if mode == 0, then the sta is allowed once the addr is hit. */
+/* if mode == 1, then the sta is rejected once the addr is non-hit. */
+struct rtw_wlan_acl_node {
+        struct list_head		        list;
+        u8       addr[ETH_ALEN];
+        u8       valid;
+};
+
+/* mode = 0, disable */
+/* mode = 1, accept unless in deny list */
+/* mode =2, deny unless in accept list */
+struct wlan_acl_pool {
+	int mode;
+	int num;
+	struct rtw_wlan_acl_node aclnode[NUM_ACL];
+	struct __queue	acl_node_q;
+};
+
+struct rssi_sta {
+	s32	UndecoratedSmoothedPWDB;
+	s32	UndecoratedSmoothedCCK;
+	s32	UndecoratedSmoothedOFDM;
+	u64	PacketMap;
+	u8 ValidBit;
+};
+
+struct	stainfo_stats	{
+
+	u64 rx_mgnt_pkts;
+		u64 rx_beacon_pkts;
+		u64 rx_probereq_pkts;
+		u64 rx_probersp_pkts;
+		u64 rx_probersp_bm_pkts;
+		u64 rx_probersp_uo_pkts;
+	u64 rx_ctrl_pkts;
+	u64 rx_data_pkts;
+
+	u64	last_rx_mgnt_pkts;
+		u64 last_rx_beacon_pkts;
+		u64 last_rx_probereq_pkts;
+		u64 last_rx_probersp_pkts;
+		u64 last_rx_probersp_bm_pkts;
+		u64 last_rx_probersp_uo_pkts;
+	u64	last_rx_ctrl_pkts;
+	u64	last_rx_data_pkts;
+
+	u64	rx_bytes;
+	u64	rx_drops;
+
+	u64	tx_pkts;
+	u64	tx_bytes;
+	u64  tx_drops;
+};
+
+struct sta_info {
+
+	spinlock_t	lock;
+	struct list_head	list; /* free_sta_queue */
+	struct list_head	hash_list; /* sta_hash */
+	struct adapter *padapter;
+
+	struct sta_xmit_priv sta_xmitpriv;
+	struct sta_recv_priv sta_recvpriv;
+
+	struct __queue sleep_q;
+	unsigned int sleepq_len;
+
+	uint state;
+	uint aid;
+	uint mac_id;
+	uint qos_option;
+	u8 hwaddr[ETH_ALEN];
+
+	uint	ieee8021x_blocked;	/* 0: allowed, 1:blocked */
+	uint	dot118021XPrivacy; /* aes, tkip... */
+	union Keytype	dot11tkiptxmickey;
+	union Keytype	dot11tkiprxmickey;
+	union Keytype	dot118021x_UncstKey;
+	union pn48		dot11txpn;			/*  PN48 used for Unicast xmit */
+	union pn48		dot11wtxpn;			/*  PN48 used for Unicast mgmt xmit. */
+	union pn48		dot11rxpn;			/*  PN48 used for Unicast recv. */
+
+
+	u8 bssrateset[16];
+	u32 bssratelen;
+	s32  rssi;
+	s32	signal_quality;
+
+	u8 cts2self;
+	u8 rtsen;
+
+	u8 raid;
+	u8 init_rate;
+	u32 ra_mask;
+	u8 wireless_mode;	/*  NETWORK_TYPE */
+	u8 bw_mode;
+
+	u8 ldpc;
+	u8 stbc;
+
+	struct stainfo_stats sta_stats;
+
+	/* for A-MPDU TX, ADDBA timeout check */
+	struct timer_list addba_retry_timer;
+
+	/* for A-MPDU Rx reordering buffer control */
+	struct recv_reorder_ctrl recvreorder_ctrl[16];
+
+	/* for A-MPDU Tx */
+	/* unsigned char 	ampdu_txen_bitmap; */
+	u16 BA_starting_seqctrl[16];
+
+
+	struct ht_priv htpriv;
+
+	/* Notes: */
+	/* STA_Mode: */
+	/* curr_network(mlme_priv/security_priv/qos/ht) + sta_info: (STA & AP) CAP/INFO */
+	/* scan_q: AP CAP/INFO */
+
+	/* AP_Mode: */
+	/* curr_network(mlme_priv/security_priv/qos/ht) : AP CAP/INFO */
+	/* sta_info: (AP & STA) CAP/INFO */
+
+	struct list_head asoc_list;
+	struct list_head auth_list;
+
+	unsigned int expire_to;
+	unsigned int auth_seq;
+	unsigned int authalg;
+	unsigned char chg_txt[128];
+
+	u16 capability;
+	int flags;
+
+	int dot8021xalg;/* 0:disable, 1:psk, 2:802.1x */
+	int wpa_psk;/* 0:disable, bit(0): WPA, bit(1):WPA2 */
+	int wpa_group_cipher;
+	int wpa2_group_cipher;
+	int wpa_pairwise_cipher;
+	int wpa2_pairwise_cipher;
+
+	u8 bpairwise_key_installed;
+
+	u8 wpa_ie[32];
+
+	u8 nonerp_set;
+	u8 no_short_slot_time_set;
+	u8 no_short_preamble_set;
+	u8 no_ht_gf_set;
+	u8 no_ht_set;
+	u8 ht_20mhz_set;
+
+	unsigned int tx_ra_bitmap;
+	u8 qos_info;
+
+	u8 max_sp_len;
+	u8 uapsd_bk;/* BIT(0): Delivery enabled, BIT(1): Trigger enabled */
+	u8 uapsd_be;
+	u8 uapsd_vi;
+	u8 uapsd_vo;
+
+	u8 has_legacy_ac;
+	unsigned int sleepq_ac_len;
+
+	u8 under_exist_checking;
+
+	u8 keep_alive_trycnt;
+
+	u8 *passoc_req;
+	u32 assoc_req_len;
+
+	/* for DM */
+	struct rssi_sta	 rssi_stat;
+
+	/* ODM_STA_INFO_T */
+	/*  ================ODM Relative Info ======================= */
+	/*  Please be care, dont declare too much structure here. It will cost memory * STA support num. */
+	/*  */
+	/*  */
+	/*  2011/10/20 MH Add for ODM STA info. */
+	/*  */
+	/*  Driver Write */
+	u8 bValid;				/*  record the sta status link or not? */
+	u8 IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
+	/*  ODM Write */
+	/* 1 PHY_STATUS_INFO */
+	u8 RSSI_Path[4];		/*  */
+	u8 RSSI_Ave;
+	u8 RXEVM[4];
+	u8 RXSNR[4];
+
+	u8 rssi_level;			/* for Refresh RA mask */
+	/*  ODM Write */
+	/* 1 TX_INFO (may changed by IC) */
+	/* TX_INFO_T		pTxInfo;		 Define in IC folder. Move lower layer. */
+	/*  */
+	/*  ================ODM Relative Info ======================= */
+	/*  */
+
+	/* To store the sequence number of received management frame */
+	u16 RxMgmtFrameSeqNum;
+};
+
+#define sta_rx_pkts(sta) \
+	(sta->sta_stats.rx_mgnt_pkts \
+	+ sta->sta_stats.rx_ctrl_pkts \
+	+ sta->sta_stats.rx_data_pkts)
+
+#define sta_last_rx_pkts(sta) \
+	(sta->sta_stats.last_rx_mgnt_pkts \
+	+ sta->sta_stats.last_rx_ctrl_pkts \
+	+ sta->sta_stats.last_rx_data_pkts)
+
+#define sta_rx_data_pkts(sta) \
+	(sta->sta_stats.rx_data_pkts)
+
+#define sta_last_rx_data_pkts(sta) \
+	(sta->sta_stats.last_rx_data_pkts)
+
+#define sta_rx_mgnt_pkts(sta) \
+	(sta->sta_stats.rx_mgnt_pkts)
+
+#define sta_last_rx_mgnt_pkts(sta) \
+	(sta->sta_stats.last_rx_mgnt_pkts)
+
+#define sta_rx_beacon_pkts(sta) \
+	(sta->sta_stats.rx_beacon_pkts)
+
+#define sta_last_rx_beacon_pkts(sta) \
+	(sta->sta_stats.last_rx_beacon_pkts)
+
+#define sta_rx_probereq_pkts(sta) \
+	(sta->sta_stats.rx_probereq_pkts)
+
+#define sta_last_rx_probereq_pkts(sta) \
+	(sta->sta_stats.last_rx_probereq_pkts)
+
+#define sta_rx_probersp_pkts(sta) \
+	(sta->sta_stats.rx_probersp_pkts)
+
+#define sta_last_rx_probersp_pkts(sta) \
+	(sta->sta_stats.last_rx_probersp_pkts)
+
+#define sta_rx_probersp_bm_pkts(sta) \
+	(sta->sta_stats.rx_probersp_bm_pkts)
+
+#define sta_last_rx_probersp_bm_pkts(sta) \
+	(sta->sta_stats.last_rx_probersp_bm_pkts)
+
+#define sta_rx_probersp_uo_pkts(sta) \
+	(sta->sta_stats.rx_probersp_uo_pkts)
+
+#define sta_last_rx_probersp_uo_pkts(sta) \
+	(sta->sta_stats.last_rx_probersp_uo_pkts)
+
+#define sta_update_last_rx_pkts(sta) \
+	do { \
+		sta->sta_stats.last_rx_mgnt_pkts = sta->sta_stats.rx_mgnt_pkts; \
+		sta->sta_stats.last_rx_beacon_pkts = sta->sta_stats.rx_beacon_pkts; \
+		sta->sta_stats.last_rx_probereq_pkts = sta->sta_stats.rx_probereq_pkts; \
+		sta->sta_stats.last_rx_probersp_pkts = sta->sta_stats.rx_probersp_pkts; \
+		sta->sta_stats.last_rx_probersp_bm_pkts = sta->sta_stats.rx_probersp_bm_pkts; \
+		sta->sta_stats.last_rx_probersp_uo_pkts = sta->sta_stats.rx_probersp_uo_pkts; \
+		sta->sta_stats.last_rx_ctrl_pkts = sta->sta_stats.rx_ctrl_pkts; \
+		sta->sta_stats.last_rx_data_pkts = sta->sta_stats.rx_data_pkts; \
+	} while (0)
+
+#define STA_RX_PKTS_ARG(sta) \
+	sta->sta_stats.rx_mgnt_pkts \
+	, sta->sta_stats.rx_ctrl_pkts \
+	, sta->sta_stats.rx_data_pkts
+
+#define STA_LAST_RX_PKTS_ARG(sta) \
+	sta->sta_stats.last_rx_mgnt_pkts \
+	, sta->sta_stats.last_rx_ctrl_pkts \
+	, sta->sta_stats.last_rx_data_pkts
+
+#define STA_RX_PKTS_DIFF_ARG(sta) \
+	sta->sta_stats.rx_mgnt_pkts - sta->sta_stats.last_rx_mgnt_pkts \
+	, sta->sta_stats.rx_ctrl_pkts - sta->sta_stats.last_rx_ctrl_pkts \
+	, sta->sta_stats.rx_data_pkts - sta->sta_stats.last_rx_data_pkts
+
+#define STA_PKTS_FMT "(m:%llu, c:%llu, d:%llu)"
+
+struct	sta_priv {
+
+	u8 *pallocated_stainfo_buf;
+	u8 *pstainfo_buf;
+	struct __queue	free_sta_queue;
+
+	spinlock_t sta_hash_lock;
+	struct list_head   sta_hash[NUM_STA];
+	int asoc_sta_count;
+	struct __queue sleep_q;
+	struct __queue wakeup_q;
+
+	struct adapter *padapter;
+
+	struct list_head asoc_list;
+	struct list_head auth_list;
+	spinlock_t asoc_list_lock;
+	spinlock_t auth_list_lock;
+	u8 asoc_list_cnt;
+	u8 auth_list_cnt;
+
+	unsigned int auth_to;  /* sec, time to expire in authenticating. */
+	unsigned int assoc_to; /* sec, time to expire before associating. */
+	unsigned int expire_to; /* sec , time to expire after associated. */
+
+	/* pointers to STA info; based on allocated AID or NULL if AID free
+	 * AID is in the range 1-2007, so sta_aid[0] corresponders to AID 1
+	 * and so on
+	 */
+	struct sta_info *sta_aid[NUM_STA];
+
+	u16 sta_dz_bitmap;/* only support for 15 stations, aid bitmap for sleeping stations. */
+	u16 tim_bitmap;/* only support 15 stations, aid = 0~15 mapping bit0~bit15 */
+
+	u16 max_num_sta;
+
+	struct wlan_acl_pool acl_list;
+};
+
+
+static inline u32 wifi_mac_hash(u8 *mac)
+{
+        u32 x;
+
+        x = mac[0];
+        x = (x << 2) ^ mac[1];
+        x = (x << 2) ^ mac[2];
+        x = (x << 2) ^ mac[3];
+        x = (x << 2) ^ mac[4];
+        x = (x << 2) ^ mac[5];
+
+        x ^= x >> 8;
+        x  = x & (NUM_STA - 1);
+
+        return x;
+}
+
+
+extern u32 _rtw_init_sta_priv(struct sta_priv *pstapriv);
+extern u32 _rtw_free_sta_priv(struct sta_priv *pstapriv);
+
+#define stainfo_offset_valid(offset) (offset < NUM_STA && offset >= 0)
+int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta);
+struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset);
+
+extern struct sta_info *rtw_alloc_stainfo(struct	sta_priv *pstapriv, u8 *hwaddr);
+extern u32 rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta);
+extern void rtw_free_all_stainfo(struct adapter *padapter);
+extern struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr);
+extern u32 rtw_init_bcmc_stainfo(struct adapter *padapter);
+extern struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter);
+extern u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr);
+
+#endif /* _STA_INFO_H_ */
diff --git a/drivers/staging/rtl8723bs/include/wifi.h b/drivers/staging/rtl8723bs/include/wifi.h
new file mode 100644
index 000000000..f03e26818
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/wifi.h
@@ -0,0 +1,778 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef _WIFI_H_
+#define _WIFI_H_
+
+#define WLAN_ETHHDR_LEN		14
+#define WLAN_ETHADDR_LEN	6
+#define WLAN_IEEE_OUI_LEN	3
+#define WLAN_ADDR_LEN		6
+#define WLAN_CRC_LEN		4
+#define WLAN_BSSID_LEN		6
+#define WLAN_BSS_TS_LEN		8
+#define WLAN_HDR_A3_LEN		24
+#define WLAN_HDR_A4_LEN		30
+#define WLAN_HDR_A3_QOS_LEN	26
+#define WLAN_HDR_A4_QOS_LEN	32
+#define WLAN_SSID_MAXLEN	32
+#define WLAN_DATA_MAXLEN	2312
+
+#define WLAN_A3_PN_OFFSET	24
+#define WLAN_A4_PN_OFFSET	30
+
+#define WLAN_MIN_ETHFRM_LEN	60
+#define WLAN_MAX_ETHFRM_LEN	1514
+#define WLAN_ETHHDR_LEN		14
+#define WLAN_WMM_LEN		24
+
+#define P80211CAPTURE_VERSION	0x80211001
+
+/*  This value is tested by WiFi 11n Test Plan 5.2.3. */
+/*  This test verifies the WLAN NIC can update the NAV through sending the CTS with large duration. */
+#define	WiFiNavUpperUs				30000	/*  30 ms */
+
+enum {
+	WIFI_MGT_TYPE  =	(0),
+	WIFI_CTRL_TYPE =	(BIT(2)),
+	WIFI_DATA_TYPE =	(BIT(3)),
+	WIFI_QOS_DATA_TYPE	= (BIT(7)|BIT(3)),	/*  QoS Data */
+};
+
+enum {
+
+    /*  below is for mgt frame */
+    WIFI_ASSOCREQ       = (0 | WIFI_MGT_TYPE),
+    WIFI_ASSOCRSP       = (BIT(4) | WIFI_MGT_TYPE),
+    WIFI_REASSOCREQ     = (BIT(5) | WIFI_MGT_TYPE),
+    WIFI_REASSOCRSP     = (BIT(5) | BIT(4) | WIFI_MGT_TYPE),
+    WIFI_PROBEREQ       = (BIT(6) | WIFI_MGT_TYPE),
+    WIFI_PROBERSP       = (BIT(6) | BIT(4) | WIFI_MGT_TYPE),
+    WIFI_BEACON         = (BIT(7) | WIFI_MGT_TYPE),
+    WIFI_ATIM           = (BIT(7) | BIT(4) | WIFI_MGT_TYPE),
+    WIFI_DISASSOC       = (BIT(7) | BIT(5) | WIFI_MGT_TYPE),
+    WIFI_AUTH           = (BIT(7) | BIT(5) | BIT(4) | WIFI_MGT_TYPE),
+    WIFI_DEAUTH         = (BIT(7) | BIT(6) | WIFI_MGT_TYPE),
+    WIFI_ACTION         = (BIT(7) | BIT(6) | BIT(4) | WIFI_MGT_TYPE),
+    WIFI_ACTION_NOACK = (BIT(7) | BIT(6) | BIT(5) | WIFI_MGT_TYPE),
+
+    /*  below is for control frame */
+    WIFI_NDPA         = (BIT(6) | BIT(4) | WIFI_CTRL_TYPE),
+    WIFI_PSPOLL         = (BIT(7) | BIT(5) | WIFI_CTRL_TYPE),
+    WIFI_RTS            = (BIT(7) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
+    WIFI_CTS            = (BIT(7) | BIT(6) | WIFI_CTRL_TYPE),
+    WIFI_ACK            = (BIT(7) | BIT(6) | BIT(4) | WIFI_CTRL_TYPE),
+    WIFI_CFEND          = (BIT(7) | BIT(6) | BIT(5) | WIFI_CTRL_TYPE),
+    WIFI_CFEND_CFACK    = (BIT(7) | BIT(6) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
+
+    /*  below is for data frame */
+    WIFI_DATA           = (0 | WIFI_DATA_TYPE),
+    WIFI_DATA_CFACK     = (BIT(4) | WIFI_DATA_TYPE),
+    WIFI_DATA_CFPOLL    = (BIT(5) | WIFI_DATA_TYPE),
+    WIFI_DATA_CFACKPOLL = (BIT(5) | BIT(4) | WIFI_DATA_TYPE),
+    WIFI_DATA_NULL      = (BIT(6) | WIFI_DATA_TYPE),
+    WIFI_CF_ACK         = (BIT(6) | BIT(4) | WIFI_DATA_TYPE),
+    WIFI_CF_POLL        = (BIT(6) | BIT(5) | WIFI_DATA_TYPE),
+    WIFI_CF_ACKPOLL     = (BIT(6) | BIT(5) | BIT(4) | WIFI_DATA_TYPE),
+    WIFI_QOS_DATA_NULL	= (BIT(6) | WIFI_QOS_DATA_TYPE),
+};
+
+#define _TO_DS_		BIT(8)
+#define _FROM_DS_	BIT(9)
+#define _MORE_FRAG_	BIT(10)
+#define _RETRY_		BIT(11)
+#define _PWRMGT_	BIT(12)
+#define _MORE_DATA_	BIT(13)
+#define _PRIVACY_	BIT(14)
+#define _ORDER_			BIT(15)
+
+#define SetToDs(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_TO_DS_))
+
+#define GetToDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_TO_DS_)) != 0)
+
+#define SetFrDs(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_FROM_DS_))
+
+#define GetFrDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_FROM_DS_)) != 0)
+
+#define get_tofr_ds(pframe)	((GetToDs(pframe) << 1) | GetFrDs(pframe))
+
+#define SetMFrag(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_MORE_FRAG_))
+
+#define GetMFrag(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_MORE_FRAG_)) != 0)
+
+#define ClearMFrag(pbuf)	\
+	(*(__le16 *)(pbuf) &= (~cpu_to_le16(_MORE_FRAG_)))
+
+#define GetRetry(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_RETRY_)) != 0)
+
+#define ClearRetry(pbuf)	\
+	(*(__le16 *)(pbuf) &= (~cpu_to_le16(_RETRY_)))
+
+#define SetPwrMgt(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_PWRMGT_))
+
+#define GetPwrMgt(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_PWRMGT_)) != 0)
+
+#define ClearPwrMgt(pbuf)	\
+	(*(__le16 *)(pbuf) &= (~cpu_to_le16(_PWRMGT_)))
+
+#define SetMData(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_MORE_DATA_))
+
+#define GetMData(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_MORE_DATA_)) != 0)
+
+#define ClearMData(pbuf)	\
+	(*(__le16 *)(pbuf) &= (~cpu_to_le16(_MORE_DATA_)))
+
+#define SetPrivacy(pbuf)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(_PRIVACY_))
+
+#define GetPrivacy(pbuf)					\
+	(((*(__le16 *)(pbuf)) & cpu_to_le16(_PRIVACY_)) != 0)
+
+#define GetOrder(pbuf)					\
+	(((*(__le16 *)(pbuf)) & cpu_to_le16(_ORDER_)) != 0)
+
+#define GetFrameType(pbuf)				\
+	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(3) | BIT(2)))
+
+#define SetFrameType(pbuf, type)	\
+	do {	\
+		*(unsigned short *)(pbuf) &= cpu_to_le16(~(BIT(3) | BIT(2))); \
+		*(unsigned short *)(pbuf) |= cpu_to_le16(type); \
+	} while (0)
+
+#define GetFrameSubType(pbuf)	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(7) |\
+	 BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2)))
+
+#define SetFrameSubType(pbuf, type) \
+	do {    \
+		*(__le16 *)(pbuf) &= cpu_to_le16(~(BIT(7) | BIT(6) |	\
+		 BIT(5) | BIT(4) | BIT(3) | BIT(2))); \
+		*(__le16 *)(pbuf) |= cpu_to_le16(type); \
+	} while (0)
+
+#define GetSequence(pbuf)			\
+	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 22)) >> 4)
+
+#define GetFragNum(pbuf)			\
+	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 22)) & 0x0f)
+
+#define GetTupleCache(pbuf)			\
+	(cpu_to_le16(*(unsigned short *)((size_t)(pbuf) + 22)))
+
+#define SetFragNum(pbuf, num) \
+	do {    \
+		*(unsigned short *)((size_t)(pbuf) + 22) = \
+			((*(unsigned short *)((size_t)(pbuf) + 22)) &	\
+			le16_to_cpu(~(0x000f))) | \
+			cpu_to_le16(0x0f & (num));     \
+	} while (0)
+
+#define SetSeqNum(pbuf, num) \
+	do {    \
+		*(__le16 *)((size_t)(pbuf) + 22) = \
+			((*(__le16 *)((size_t)(pbuf) + 22)) & cpu_to_le16((unsigned short)0x000f)) | \
+			cpu_to_le16((unsigned short)(0xfff0 & (num << 4))); \
+	} while (0)
+
+#define SetDuration(pbuf, dur) \
+	(*(__le16 *)((size_t)(pbuf) + 2) = cpu_to_le16(0xffff & (dur)))
+
+
+#define SetPriority(pbuf, tid)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16(tid & 0xf))
+
+#define GetPriority(pbuf)	((le16_to_cpu(*(__le16 *)(pbuf))) & 0xf)
+
+#define SetEOSP(pbuf, eosp)	\
+		(*(__le16 *)(pbuf) |= cpu_to_le16((eosp & 1) << 4))
+
+#define SetAckpolicy(pbuf, ack)	\
+	(*(__le16 *)(pbuf) |= cpu_to_le16((ack & 3) << 5))
+
+#define GetAckpolicy(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 5) & 0x3)
+
+#define GetAMsdu(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 7) & 0x1)
+
+#define GetAid(pbuf)	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 2)) & 0x3fff)
+
+#define GetAddr1Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 4))
+
+#define GetAddr2Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 10))
+
+#define GetAddr3Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 16))
+
+#define GetAddr4Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 24))
+
+#define MacAddr_isBcst(addr) \
+	(\
+	((addr[0] == 0xff) && (addr[1] == 0xff) && \
+	(addr[2] == 0xff) && (addr[3] == 0xff) && \
+	(addr[4] == 0xff) && (addr[5] == 0xff))  ? true : false \
+)
+
+static inline int IS_MCAST(unsigned char *da)
+{
+	if ((*da) & 0x01)
+		return true;
+	else
+		return false;
+}
+
+static inline unsigned char *rtl8723bs_get_ra(unsigned char *pframe)
+{
+	unsigned char *ra;
+	ra = GetAddr1Ptr(pframe);
+	return ra;
+}
+static inline unsigned char *get_ta(unsigned char *pframe)
+{
+	unsigned char *ta;
+	ta = GetAddr2Ptr(pframe);
+	return ta;
+}
+
+static inline unsigned char *get_da(unsigned char *pframe)
+{
+	unsigned char *da;
+	unsigned int	to_fr_ds	= (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs = 0, FromDs = 0 */
+		da = GetAddr1Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs = 0, FromDs = 1 */
+		da = GetAddr1Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs = 1, FromDs = 0 */
+		da = GetAddr3Ptr(pframe);
+		break;
+	default:	/*  ToDs = 1, FromDs = 1 */
+		da = GetAddr3Ptr(pframe);
+		break;
+	}
+
+	return da;
+}
+
+
+static inline unsigned char *get_sa(unsigned char *pframe)
+{
+	unsigned char *sa;
+	unsigned int	to_fr_ds	= (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs = 0, FromDs = 0 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs = 0, FromDs = 1 */
+		sa = GetAddr3Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs = 1, FromDs = 0 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	default:	/*  ToDs = 1, FromDs = 1 */
+		sa = GetAddr4Ptr(pframe);
+		break;
+	}
+
+	return sa;
+}
+
+static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
+{
+	unsigned char *sa = NULL;
+	unsigned int	to_fr_ds	= (GetToDs(pframe) << 1) | GetFrDs(pframe);
+
+	switch (to_fr_ds) {
+	case 0x00:	/*  ToDs = 0, FromDs = 0 */
+		sa = GetAddr3Ptr(pframe);
+		break;
+	case 0x01:	/*  ToDs = 0, FromDs = 1 */
+		sa = GetAddr2Ptr(pframe);
+		break;
+	case 0x02:	/*  ToDs = 1, FromDs = 0 */
+		sa = GetAddr1Ptr(pframe);
+		break;
+	case 0x03:	/*  ToDs = 1, FromDs = 1 */
+		sa = GetAddr1Ptr(pframe);
+		break;
+	}
+
+	return sa;
+}
+
+
+static inline int IsFrameTypeCtrl(unsigned char *pframe)
+{
+	if (WIFI_CTRL_TYPE == GetFrameType(pframe))
+		return true;
+	else
+		return false;
+}
+/*-----------------------------------------------------------------------------
+			Below is for the security related definition
+------------------------------------------------------------------------------*/
+#define _RESERVED_FRAME_TYPE_	0
+#define _SKB_FRAME_TYPE_		2
+#define _PRE_ALLOCMEM_			1
+#define _PRE_ALLOCHDR_			3
+#define _PRE_ALLOCLLCHDR_		4
+#define _PRE_ALLOCICVHDR_		5
+#define _PRE_ALLOCMICHDR_		6
+
+#define _ACKCTSLNG_				14	/* 14 bytes long, including crclng */
+#define _CRCLNG_				4
+
+#define _ASOCREQ_IE_OFFSET_		4	/*  excluding wlan_hdr */
+#define	_ASOCRSP_IE_OFFSET_		6
+#define _REASOCREQ_IE_OFFSET_	10
+#define _REASOCRSP_IE_OFFSET_	6
+#define _PROBEREQ_IE_OFFSET_	0
+#define	_PROBERSP_IE_OFFSET_	12
+#define _AUTH_IE_OFFSET_		6
+#define _DEAUTH_IE_OFFSET_		0
+#define _BEACON_IE_OFFSET_		12
+#define _PUBLIC_ACTION_IE_OFFSET_	8
+
+#define _FIXED_IE_LENGTH_			_BEACON_IE_OFFSET_
+
+/* ---------------------------------------------------------------------------
+					Below is the fixed elements...
+-----------------------------------------------------------------------------*/
+#define _AUTH_ALGM_NUM_			2
+#define _AUTH_SEQ_NUM_			2
+#define _BEACON_ITERVAL_		2
+#define _CAPABILITY_			2
+#define _CURRENT_APADDR_		6
+#define _LISTEN_INTERVAL_		2
+#define _RSON_CODE_				2
+#define _ASOC_ID_				2
+#define _STATUS_CODE_			2
+#define _TIMESTAMP_				8
+
+#define AUTH_ODD_TO				0
+#define AUTH_EVEN_TO			1
+
+#define WLAN_ETHCONV_ENCAP		1
+#define WLAN_ETHCONV_RFC1042	2
+#define WLAN_ETHCONV_8021h		3
+
+/*-----------------------------------------------------------------------------
+				Below is the definition for 802.11i / 802.1x
+------------------------------------------------------------------------------*/
+#define _IEEE8021X_MGT_			1		/*  WPA */
+#define _IEEE8021X_PSK_			2		/*  WPA with pre-shared key */
+
+#define _MME_IE_LENGTH_  18
+/*-----------------------------------------------------------------------------
+				Below is the definition for WMM
+------------------------------------------------------------------------------*/
+#define _WMM_IE_Length_				7  /*  for WMM STA */
+#define _WMM_Para_Element_Length_		24
+
+
+/*-----------------------------------------------------------------------------
+				Below is the definition for 802.11n
+------------------------------------------------------------------------------*/
+
+#define SetOrderBit(pbuf)	\
+	do	{	\
+		*(unsigned short *)(pbuf) |= cpu_to_le16(_ORDER_); \
+	} while (0)
+
+#define GetOrderBit(pbuf)	(((*(unsigned short *)(pbuf)) & cpu_to_le16(_ORDER_)) != 0)
+
+#define ACT_CAT_VENDOR				0x7F/* 127 */
+
+/**
+ * struct rtw_ieee80211_ht_cap - HT additional information
+ *
+ * This structure refers to "HT information element" as
+ * described in 802.11n draft section 7.3.2.53
+ */
+struct ieee80211_ht_addt_info {
+	unsigned char control_chan;
+	unsigned char 	ht_param;
+	__le16	operation_mode;
+	__le16	stbc_param;
+	unsigned char 	basic_set[16];
+} __attribute__ ((packed));
+
+
+struct HT_caps_element {
+	union {
+		struct {
+			__le16	HT_caps_info;
+			unsigned char AMPDU_para;
+			unsigned char MCS_rate[16];
+			__le16	HT_ext_caps;
+			__le16	Beamforming_caps;
+			unsigned char ASEL_caps;
+		} HT_cap_element;
+		unsigned char HT_cap[26];
+	} u;
+} __attribute__ ((packed));
+
+struct HT_info_element {
+	unsigned char primary_channel;
+	unsigned char infos[5];
+	unsigned char MCS_rate[16];
+}  __attribute__ ((packed));
+
+struct AC_param {
+	unsigned char 	ACI_AIFSN;
+	unsigned char 	CW;
+	__le16	TXOP_limit;
+}  __attribute__ ((packed));
+
+struct WMM_para_element {
+	unsigned char 	QoS_info;
+	unsigned char 	reserved;
+	struct AC_param	ac_param[4];
+}  __attribute__ ((packed));
+
+struct ADDBA_request {
+	unsigned char 	dialog_token;
+	__le16	BA_para_set;
+	__le16	BA_timeout_value;
+	__le16	BA_starting_seqctrl;
+}  __attribute__ ((packed));
+
+/* 802.11n HT capabilities masks */
+#define IEEE80211_HT_CAP_LDPC_CODING		0x0001
+#define IEEE80211_HT_CAP_SUP_WIDTH		0x0002
+#define IEEE80211_HT_CAP_SM_PS			0x000C
+#define IEEE80211_HT_CAP_GRN_FLD		0x0010
+#define IEEE80211_HT_CAP_SGI_20			0x0020
+#define IEEE80211_HT_CAP_SGI_40			0x0040
+#define IEEE80211_HT_CAP_TX_STBC			0x0080
+#define IEEE80211_HT_CAP_RX_STBC_1R		0x0100
+#define IEEE80211_HT_CAP_RX_STBC_2R		0x0200
+#define IEEE80211_HT_CAP_RX_STBC_3R		0x0300
+#define IEEE80211_HT_CAP_DELAY_BA		0x0400
+#define IEEE80211_HT_CAP_MAX_AMSDU		0x0800
+#define IEEE80211_HT_CAP_DSSSCCK40		0x1000
+/* 802.11n HT capability AMPDU settings */
+#define IEEE80211_HT_CAP_AMPDU_FACTOR		0x03
+#define IEEE80211_HT_CAP_AMPDU_DENSITY		0x1C
+/* 802.11n HT capability MSC set */
+#define IEEE80211_SUPP_MCS_SET_UEQM		4
+#define IEEE80211_HT_CAP_MAX_STREAMS		4
+#define IEEE80211_SUPP_MCS_SET_LEN		10
+/* maximum streams the spec allows */
+#define IEEE80211_HT_CAP_MCS_TX_DEFINED		0x01
+#define IEEE80211_HT_CAP_MCS_TX_RX_DIFF		0x02
+#define IEEE80211_HT_CAP_MCS_TX_STREAMS		0x0C
+#define IEEE80211_HT_CAP_MCS_TX_UEQM		0x10
+/* 802.11n HT capability TXBF capability */
+#define IEEE80211_HT_CAP_TXBF_RX_NDP		0x00000008
+#define IEEE80211_HT_CAP_TXBF_TX_NDP		0x00000010
+#define IEEE80211_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP	0x00000400
+
+/* endif */
+
+/* 	===============WPS Section =============== */
+/* 	For WPSv1.0 */
+#define WPSOUI							0x0050f204
+/* 	WPS attribute ID */
+#define WPS_ATTR_VER1					0x104A
+#define WPS_ATTR_SIMPLE_CONF_STATE	0x1044
+#define WPS_ATTR_RESP_TYPE			0x103B
+#define WPS_ATTR_UUID_E				0x1047
+#define WPS_ATTR_MANUFACTURER		0x1021
+#define WPS_ATTR_MODEL_NAME			0x1023
+#define WPS_ATTR_MODEL_NUMBER		0x1024
+#define WPS_ATTR_SERIAL_NUMBER		0x1042
+#define WPS_ATTR_PRIMARY_DEV_TYPE	0x1054
+#define WPS_ATTR_SEC_DEV_TYPE_LIST	0x1055
+#define WPS_ATTR_DEVICE_NAME			0x1011
+#define WPS_ATTR_CONF_METHOD			0x1008
+#define WPS_ATTR_RF_BANDS				0x103C
+#define WPS_ATTR_DEVICE_PWID			0x1012
+#define WPS_ATTR_REQUEST_TYPE			0x103A
+#define WPS_ATTR_ASSOCIATION_STATE	0x1002
+#define WPS_ATTR_CONFIG_ERROR			0x1009
+#define WPS_ATTR_VENDOR_EXT			0x1049
+#define WPS_ATTR_SELECTED_REGISTRAR	0x1041
+
+/* 	Value of WPS attribute "WPS_ATTR_DEVICE_NAME */
+#define WPS_MAX_DEVICE_NAME_LEN		32
+
+/* 	Value of WPS Request Type Attribute */
+#define WPS_REQ_TYPE_ENROLLEE_INFO_ONLY			0x00
+#define WPS_REQ_TYPE_ENROLLEE_OPEN_8021X		0x01
+#define WPS_REQ_TYPE_REGISTRAR					0x02
+#define WPS_REQ_TYPE_WLAN_MANAGER_REGISTRAR	0x03
+
+/* 	Value of WPS Response Type Attribute */
+#define WPS_RESPONSE_TYPE_INFO_ONLY	0x00
+#define WPS_RESPONSE_TYPE_8021X		0x01
+#define WPS_RESPONSE_TYPE_REGISTRAR	0x02
+#define WPS_RESPONSE_TYPE_AP			0x03
+
+/* 	Value of WPS WiFi Simple Configuration State Attribute */
+#define WPS_WSC_STATE_NOT_CONFIG	0x01
+#define WPS_WSC_STATE_CONFIG			0x02
+
+/* 	Value of WPS Version Attribute */
+#define WPS_VERSION_1					0x10
+
+/* 	Value of WPS Configuration Method Attribute */
+#define WPS_CONFIG_METHOD_FLASH		0x0001
+#define WPS_CONFIG_METHOD_ETHERNET	0x0002
+#define WPS_CONFIG_METHOD_LABEL		0x0004
+#define WPS_CONFIG_METHOD_DISPLAY	0x0008
+#define WPS_CONFIG_METHOD_E_NFC		0x0010
+#define WPS_CONFIG_METHOD_I_NFC		0x0020
+#define WPS_CONFIG_METHOD_NFC		0x0040
+#define WPS_CONFIG_METHOD_PBC		0x0080
+#define WPS_CONFIG_METHOD_KEYPAD	0x0100
+#define WPS_CONFIG_METHOD_VPBC		0x0280
+#define WPS_CONFIG_METHOD_PPBC		0x0480
+#define WPS_CONFIG_METHOD_VDISPLAY	0x2008
+#define WPS_CONFIG_METHOD_PDISPLAY	0x4008
+
+/* 	Value of Category ID of WPS Primary Device Type Attribute */
+#define WPS_PDT_CID_DISPLAYS			0x0007
+#define WPS_PDT_CID_MULIT_MEDIA		0x0008
+#define WPS_PDT_CID_RTK_WIDI			WPS_PDT_CID_MULIT_MEDIA
+
+/* 	Value of Sub Category ID of WPS Primary Device Type Attribute */
+#define WPS_PDT_SCID_MEDIA_SERVER	0x0005
+#define WPS_PDT_SCID_RTK_DMP			WPS_PDT_SCID_MEDIA_SERVER
+
+/* 	Value of Device Password ID */
+#define WPS_DPID_PIN					0x0000
+#define WPS_DPID_USER_SPEC			0x0001
+#define WPS_DPID_MACHINE_SPEC			0x0002
+#define WPS_DPID_REKEY					0x0003
+#define WPS_DPID_PBC					0x0004
+#define WPS_DPID_REGISTRAR_SPEC		0x0005
+
+/* 	Value of WPS RF Bands Attribute */
+#define WPS_RF_BANDS_2_4_GHZ		0x01
+#define WPS_RF_BANDS_5_GHZ		0x02
+
+/* 	Value of WPS Association State Attribute */
+#define WPS_ASSOC_STATE_NOT_ASSOCIATED			0x00
+#define WPS_ASSOC_STATE_CONNECTION_SUCCESS		0x01
+#define WPS_ASSOC_STATE_CONFIGURATION_FAILURE	0x02
+#define WPS_ASSOC_STATE_ASSOCIATION_FAILURE		0x03
+#define WPS_ASSOC_STATE_IP_FAILURE				0x04
+
+/* 	=====================P2P Section ===================== */
+/* 	For P2P */
+#define	P2POUI							0x506F9A09
+
+/* 	P2P Attribute ID */
+#define	P2P_ATTR_STATUS					0x00
+#define	P2P_ATTR_MINOR_REASON_CODE		0x01
+#define	P2P_ATTR_CAPABILITY				0x02
+#define	P2P_ATTR_DEVICE_ID				0x03
+#define	P2P_ATTR_GO_INTENT				0x04
+#define	P2P_ATTR_CONF_TIMEOUT			0x05
+#define	P2P_ATTR_LISTEN_CH				0x06
+#define	P2P_ATTR_GROUP_BSSID				0x07
+#define	P2P_ATTR_EX_LISTEN_TIMING		0x08
+#define	P2P_ATTR_INTENTED_IF_ADDR		0x09
+#define	P2P_ATTR_MANAGEABILITY			0x0A
+#define	P2P_ATTR_CH_LIST					0x0B
+#define	P2P_ATTR_NOA						0x0C
+#define	P2P_ATTR_DEVICE_INFO				0x0D
+#define	P2P_ATTR_GROUP_INFO				0x0E
+#define	P2P_ATTR_GROUP_ID					0x0F
+#define	P2P_ATTR_INTERFACE				0x10
+#define	P2P_ATTR_OPERATING_CH			0x11
+#define	P2P_ATTR_INVITATION_FLAGS		0x12
+
+/* 	Value of Status Attribute */
+#define	P2P_STATUS_SUCCESS						0x00
+#define	P2P_STATUS_FAIL_INFO_UNAVAILABLE		0x01
+#define	P2P_STATUS_FAIL_INCOMPATIBLE_PARAM		0x02
+#define	P2P_STATUS_FAIL_LIMIT_REACHED			0x03
+#define	P2P_STATUS_FAIL_INVALID_PARAM			0x04
+#define	P2P_STATUS_FAIL_REQUEST_UNABLE			0x05
+#define	P2P_STATUS_FAIL_PREVOUS_PROTO_ERR		0x06
+#define	P2P_STATUS_FAIL_NO_COMMON_CH			0x07
+#define	P2P_STATUS_FAIL_UNKNOWN_P2PGROUP		0x08
+#define	P2P_STATUS_FAIL_BOTH_GOINTENT_15		0x09
+#define	P2P_STATUS_FAIL_INCOMPATIBLE_PROVSION	0x0A
+#define	P2P_STATUS_FAIL_USER_REJECT				0x0B
+
+/* 	Value of Invitation Flags Attribute */
+#define	P2P_INVITATION_FLAGS_PERSISTENT			BIT(0)
+
+#define	DMP_P2P_DEVCAP_SUPPORT	(P2P_DEVCAP_SERVICE_DISCOVERY | \
+									P2P_DEVCAP_CLIENT_DISCOVERABILITY | \
+									P2P_DEVCAP_CONCURRENT_OPERATION | \
+									P2P_DEVCAP_INVITATION_PROC)
+
+#define	DMP_P2P_GRPCAP_SUPPORT	(P2P_GRPCAP_INTRABSS)
+
+/* 	Value of Device Capability Bitmap */
+#define	P2P_DEVCAP_SERVICE_DISCOVERY		BIT(0)
+#define	P2P_DEVCAP_CLIENT_DISCOVERABILITY	BIT(1)
+#define	P2P_DEVCAP_CONCURRENT_OPERATION	BIT(2)
+#define	P2P_DEVCAP_INFRA_MANAGED			BIT(3)
+#define	P2P_DEVCAP_DEVICE_LIMIT				BIT(4)
+#define	P2P_DEVCAP_INVITATION_PROC			BIT(5)
+
+/* 	Value of Group Capability Bitmap */
+#define	P2P_GRPCAP_GO							BIT(0)
+#define	P2P_GRPCAP_PERSISTENT_GROUP			BIT(1)
+#define	P2P_GRPCAP_GROUP_LIMIT				BIT(2)
+#define	P2P_GRPCAP_INTRABSS					BIT(3)
+#define	P2P_GRPCAP_CROSS_CONN				BIT(4)
+#define	P2P_GRPCAP_PERSISTENT_RECONN		BIT(5)
+#define	P2P_GRPCAP_GROUP_FORMATION			BIT(6)
+
+/* 	P2P Public Action Frame (Management Frame) */
+#define	P2P_PUB_ACTION_ACTION				0x09
+
+/* 	P2P Public Action Frame Type */
+#define	P2P_GO_NEGO_REQ						0
+#define	P2P_GO_NEGO_RESP						1
+#define	P2P_GO_NEGO_CONF						2
+#define	P2P_INVIT_REQ							3
+#define	P2P_INVIT_RESP							4
+#define	P2P_DEVDISC_REQ						5
+#define	P2P_DEVDISC_RESP						6
+#define	P2P_PROVISION_DISC_REQ				7
+#define	P2P_PROVISION_DISC_RESP				8
+
+/* 	P2P Action Frame Type */
+#define	P2P_NOTICE_OF_ABSENCE	0
+#define	P2P_PRESENCE_REQUEST		1
+#define	P2P_PRESENCE_RESPONSE	2
+#define	P2P_GO_DISC_REQUEST		3
+
+
+#define	P2P_MAX_PERSISTENT_GROUP_NUM		10
+
+#define	P2P_PROVISIONING_SCAN_CNT			3
+
+#define	P2P_WILDCARD_SSID_LEN				7
+
+#define	P2P_FINDPHASE_EX_NONE				0	/*  default value, used when: (1)p2p disabled or (2)p2p enabled but only do 1 scan phase */
+#define	P2P_FINDPHASE_EX_FULL				1	/*  used when p2p enabled and want to do 1 scan phase and P2P_FINDPHASE_EX_MAX-1 find phase */
+#define	P2P_FINDPHASE_EX_SOCIAL_FIRST		(P2P_FINDPHASE_EX_FULL+1)
+#define	P2P_FINDPHASE_EX_MAX					4
+#define	P2P_FINDPHASE_EX_SOCIAL_LAST		P2P_FINDPHASE_EX_MAX
+
+#define	P2P_PROVISION_TIMEOUT				5000	/* 	5 seconds timeout for sending the provision discovery request */
+#define	P2P_CONCURRENT_PROVISION_TIMEOUT	3000	/* 	3 seconds timeout for sending the provision discovery request under concurrent mode */
+#define	P2P_GO_NEGO_TIMEOUT					5000	/* 	5 seconds timeout for receiving the group negotiation response */
+#define	P2P_CONCURRENT_GO_NEGO_TIMEOUT		3000	/* 	3 seconds timeout for sending the negotiation request under concurrent mode */
+#define	P2P_TX_PRESCAN_TIMEOUT				100		/* 	100ms */
+#define	P2P_INVITE_TIMEOUT					5000	/* 	5 seconds timeout for sending the invitation request */
+#define	P2P_CONCURRENT_INVITE_TIMEOUT		3000	/* 	3 seconds timeout for sending the invitation request under concurrent mode */
+#define	P2P_RESET_SCAN_CH						25000	/* 	25 seconds timeout to reset the scan channel (based on channel plan) */
+#define	P2P_MAX_INTENT						15
+
+#define	P2P_MAX_NOA_NUM						2
+
+/* 	WPS Configuration Method */
+#define	WPS_CM_NONE							0x0000
+#define	WPS_CM_LABEL							0x0004
+#define	WPS_CM_DISPLYA						0x0008
+#define	WPS_CM_EXTERNAL_NFC_TOKEN			0x0010
+#define	WPS_CM_INTEGRATED_NFC_TOKEN		0x0020
+#define	WPS_CM_NFC_INTERFACE					0x0040
+#define	WPS_CM_PUSH_BUTTON					0x0080
+#define	WPS_CM_KEYPAD						0x0100
+#define	WPS_CM_SW_PUHS_BUTTON				0x0280
+#define	WPS_CM_HW_PUHS_BUTTON				0x0480
+#define	WPS_CM_SW_DISPLAY_PIN				0x2008
+#define	WPS_CM_LCD_DISPLAY_PIN				0x4008
+
+enum p2p_role {
+	P2P_ROLE_DISABLE = 0,
+	P2P_ROLE_DEVICE = 1,
+	P2P_ROLE_CLIENT = 2,
+	P2P_ROLE_GO = 3
+};
+
+enum p2p_state {
+	P2P_STATE_NONE = 0,							/* 	P2P disable */
+	P2P_STATE_IDLE = 1,								/* 	P2P had enabled and do nothing */
+	P2P_STATE_LISTEN = 2,							/* 	In pure listen state */
+	P2P_STATE_SCAN = 3,							/* 	In scan phase */
+	P2P_STATE_FIND_PHASE_LISTEN = 4,				/* 	In the listen state of find phase */
+	P2P_STATE_FIND_PHASE_SEARCH = 5,				/* 	In the search state of find phase */
+	P2P_STATE_TX_PROVISION_DIS_REQ = 6,			/* 	In P2P provisioning discovery */
+	P2P_STATE_RX_PROVISION_DIS_RSP = 7,
+	P2P_STATE_RX_PROVISION_DIS_REQ = 8,
+	P2P_STATE_GONEGO_ING = 9,						/* 	Doing the group owner negotiation handshake */
+	P2P_STATE_GONEGO_OK = 10,						/* 	finish the group negotiation handshake with success */
+	P2P_STATE_GONEGO_FAIL = 11,					/* 	finish the group negotiation handshake with failure */
+	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,		/* 	receiving the P2P Invitation request and match with the profile. */
+	P2P_STATE_PROVISIONING_ING = 13,				/* 	Doing the P2P WPS */
+	P2P_STATE_PROVISIONING_DONE = 14,			/* 	Finish the P2P WPS */
+	P2P_STATE_TX_INVITE_REQ = 15,					/* 	Transmit the P2P Invitation request */
+	P2P_STATE_RX_INVITE_RESP_OK = 16,				/* 	Receiving the P2P Invitation response */
+	P2P_STATE_RECV_INVITE_REQ_DISMATCH = 17,	/* 	receiving the P2P Invitation request and mismatch with the profile. */
+	P2P_STATE_RECV_INVITE_REQ_GO = 18,			/* 	receiving the P2P Invitation request and this wifi is GO. */
+	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,			/* 	receiving the P2P Invitation request to join an existing P2P Group. */
+	P2P_STATE_RX_INVITE_RESP_FAIL = 20,			/* 	recveing the P2P Invitation response with failure */
+	P2P_STATE_RX_INFOR_NOREADY = 21,			/*  receiving p2p negotiation response with information is not available */
+	P2P_STATE_TX_INFOR_NOREADY = 22,			/*  sending p2p negotiation response with information is not available */
+};
+
+enum p2p_wpsinfo {
+	P2P_NO_WPSINFO						= 0,
+	P2P_GOT_WPSINFO_PEER_DISPLAY_PIN	= 1,
+	P2P_GOT_WPSINFO_SELF_DISPLAY_PIN	= 2,
+	P2P_GOT_WPSINFO_PBC					= 3,
+};
+
+#define	P2P_PRIVATE_IOCTL_SET_LEN		64
+
+/* 	=====================WFD Section ===================== */
+/* 	For Wi-Fi Display */
+#define	WFD_ATTR_DEVICE_INFO			0x00
+#define	WFD_ATTR_ASSOC_BSSID			0x01
+#define	WFD_ATTR_COUPLED_SINK_INFO	0x06
+#define	WFD_ATTR_LOCAL_IP_ADDR		0x08
+#define	WFD_ATTR_SESSION_INFO		0x09
+#define	WFD_ATTR_ALTER_MAC			0x0a
+
+/* 	For WFD Device Information Attribute */
+#define	WFD_DEVINFO_SOURCE					0x0000
+#define	WFD_DEVINFO_PSINK					0x0001
+#define	WFD_DEVINFO_SSINK					0x0002
+#define	WFD_DEVINFO_DUAL					0x0003
+
+#define	WFD_DEVINFO_SESSION_AVAIL			0x0010
+#define	WFD_DEVINFO_WSD						0x0040
+#define	WFD_DEVINFO_PC_TDLS					0x0080
+#define	WFD_DEVINFO_HDCP_SUPPORT			0x0100
+
+#define IP_MCAST_MAC(mac)		((mac[0] == 0x01) && (mac[1] == 0x00) && (mac[2] == 0x5e))
+#define ICMPV6_MCAST_MAC(mac)	((mac[0] == 0x33) && (mac[1] == 0x33) && (mac[2] != 0xff))
+
+/* Regulatroy Domain */
+struct regd_pair_mapping {
+	u16 reg_dmnenum;
+	u16 reg_2ghz_ctl;
+};
+
+struct rtw_regulatory {
+	char alpha2[2];
+	u16 country_code;
+	u16 max_power_level;
+	u32 tp_scale;
+	u16 current_rd;
+	u16 current_rd_ext;
+	int16_t power_limit;
+	struct regd_pair_mapping *regpair;
+};
+
+#endif /*  _WIFI_H_ */
diff --git a/drivers/staging/rtl8723bs/include/wlan_bssdef.h b/drivers/staging/rtl8723bs/include/wlan_bssdef.h
new file mode 100644
index 000000000..eb38594c8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/wlan_bssdef.h
@@ -0,0 +1,216 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __WLAN_BSSDEF_H__
+#define __WLAN_BSSDEF_H__
+
+
+#define MAX_IE_SZ	768
+
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+
+typedef unsigned char   NDIS_802_11_MAC_ADDRESS[6];
+typedef unsigned char   NDIS_802_11_RATES[NDIS_802_11_LENGTH_RATES];        /*  Set of 8 data rates */
+typedef unsigned char   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];  /*  Set of 16 data rates */
+
+struct ndis_802_11_ssid {
+	u32  ssid_length;
+	u8  ssid[32];
+};
+
+enum ndis_802_11_network_type {
+	Ndis802_11FH,
+	Ndis802_11DS,
+	Ndis802_11OFDM5,
+	Ndis802_11OFDM24,
+	Ndis802_11NetworkTypeMax    /*  not a real type, defined as an upper bound */
+};
+
+/*
+	FW will only save the channel number in DSConfig.
+	ODI Handler will convert the channel number to freq. number.
+*/
+struct ndis_802_11_conf {
+	u32 length;             /*  Length of structure */
+	u32 beacon_period;       /*  units are Kusec */
+	u32 atim_window;         /*  units are Kusec */
+	u32 ds_config;           /*  Frequency, units are kHz */
+};
+
+enum ndis_802_11_network_infrastructure {
+	Ndis802_11IBSS,
+	Ndis802_11Infrastructure,
+	Ndis802_11AutoUnknown,
+	Ndis802_11InfrastructureMax,     /*  Not a real value, defined as upper bound */
+	Ndis802_11APMode,
+};
+
+struct ndis_802_11_fix_ie {
+	u8  time_stamp[8];
+	u16  beacon_interval;
+	u16  capabilities;
+};
+
+struct ndis_80211_var_ie {
+	u8  element_id;
+	u8  length;
+	u8  data[];
+};
+
+/* Length is the 4 bytes multiples of the sum of
+ * sizeof (NDIS_802_11_MAC_ADDRESS) + 2 +
+ * sizeof (struct ndis_802_11_ssid) + sizeof (u32) +
+ * sizeof (long) + sizeof (enum ndis_802_11_network_type) +
+ * sizeof (struct ndis_802_11_conf) + sizeof (NDIS_802_11_RATES_EX) + ie_length
+ *
+ * Except for ie_length, all other fields are fixed length. Therefore, we can
+ * define a macro to present the partial sum.
+ */
+enum ndis_802_11_authentication_mode {
+	Ndis802_11AuthModeOpen,
+	Ndis802_11AuthModeShared,
+	Ndis802_11AuthModeAutoSwitch,
+	Ndis802_11AuthModeWPA,
+	Ndis802_11AuthModeWPAPSK,
+	Ndis802_11AuthModeWPANone,
+	Ndis802_11AuthModeWAPI,
+	Ndis802_11AuthModeMax   /*  Not a real mode, defined as upper bound */
+};
+
+enum {
+	Ndis802_11WEPEnabled,
+	Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+	Ndis802_11WEPDisabled,
+	Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+	Ndis802_11WEPKeyAbsent,
+	Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+	Ndis802_11WEPNotSupported,
+	Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+	Ndis802_11Encryption2Enabled,
+	Ndis802_11Encryption2KeyAbsent,
+	Ndis802_11Encryption3Enabled,
+	Ndis802_11Encryption3KeyAbsent,
+	Ndis802_11_EncrypteionWAPI
+};
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+/*  Key mapping keys require a BSSID */
+
+struct ndis_802_11_wep {
+	u32 length;        /*  Length of this structure */
+	u32 key_index;      /*  0 is the per-client key, 1-N are the global keys */
+	u32 key_length;     /*  length of key in bytes */
+	u8 key_material[16];/*  variable length depending on above field */
+};
+
+/*  mask for authentication/integrity fields */
+#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
+#define NDIS_802_11_AUTH_REQUEST_REAUTH			0x01
+#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE		0x02
+#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR		0x06
+#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR		0x0E
+
+/*  MIC check time, 60 seconds. */
+#define MIC_CHECK_TIME	60000000
+
+#ifndef Ndis802_11APMode
+#define Ndis802_11APMode (Ndis802_11InfrastructureMax + 1)
+#endif
+
+struct wlan_phy_info {
+	u8 signal_strength;/* in percentage) */
+	u8 signal_quality;/* in percentage) */
+	u8 optimum_antenna;  /* for Antenna diversity */
+	u8 reserved_0;
+};
+
+struct wlan_bcn_info {
+	/* these infor get from rtw_get_encrypt_info when
+	 * * translate scan to UI */
+	u8 encryp_protocol;/* ENCRYP_PROTOCOL_E: OPEN/WEP/WPA/WPA2/WAPI */
+	int group_cipher; /* WPA/WPA2 group cipher */
+	int pairwise_cipher;/* WPA/WPA2/WEP pairwise cipher */
+	int is_8021x;
+
+	/* bwmode 20/40 and ch_offset UP/LOW */
+	unsigned short	ht_cap_info;
+	unsigned char ht_info_infos_0;
+};
+
+/* temporally add #pragma pack for structure alignment issue of
+*   struct wlan_bssid_ex and get_wlan_bssid_ex_sz()
+*/
+struct wlan_bssid_ex {
+	u32  length;
+	NDIS_802_11_MAC_ADDRESS  mac_address;
+	u8  reserved[2];/* 0]: IS beacon frame */
+	struct ndis_802_11_ssid  ssid;
+	u32  privacy;
+	long  rssi;/* in dBM, raw data , get from PHY) */
+	enum ndis_802_11_network_type  network_type_in_use;
+	struct ndis_802_11_conf  configuration;
+	enum ndis_802_11_network_infrastructure  infrastructure_mode;
+	NDIS_802_11_RATES_EX  supported_rates;
+	struct wlan_phy_info phy_info;
+	u32  ie_length;
+	u8  ies[MAX_IE_SZ];	/* timestamp, beacon interval, and capability information) */
+} __packed;
+
+static inline uint get_wlan_bssid_ex_sz(struct wlan_bssid_ex *bss)
+{
+	return (sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + bss->ie_length);
+}
+
+struct	wlan_network {
+	struct list_head	list;
+	int	network_type;	/* refer to ieee80211.h for WIRELESS_11A/B/G */
+	int	fixed;			/*  set to fixed when not to be removed as site-surveying */
+	unsigned long	last_scanned; /* timestamp for the network */
+	int	aid;			/* will only be valid when a BSS is joinned. */
+	int	join_res;
+	struct wlan_bssid_ex	network; /* must be the last item */
+	struct wlan_bcn_info	bcn_info;
+};
+
+enum {
+	DISABLE_VCS,
+	ENABLE_VCS,
+	AUTO_VCS
+};
+
+enum {
+	NONE_VCS,
+	RTS_CTS,
+	CTS_TO_SELF
+};
+
+#define PWR_CAM 0
+#define PWR_MINPS 1
+#define PWR_MAXPS 2
+#define PWR_UAPSD 3
+#define PWR_VOIP 4
+
+enum {
+	NO_LIMIT,
+	TWO_MSDU,
+	FOUR_MSDU,
+	SIX_MSDU
+};
+
+#define NUM_PRE_AUTH_KEY 16
+#define NUM_PMKID_CACHE NUM_PRE_AUTH_KEY
+
+#endif /* ifndef WLAN_BSSDEF_H_ */
diff --git a/drivers/staging/rtl8723bs/include/xmit_osdep.h b/drivers/staging/rtl8723bs/include/xmit_osdep.h
new file mode 100644
index 000000000..8704dced5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/xmit_osdep.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#ifndef __XMIT_OSDEP_H_
+#define __XMIT_OSDEP_H_
+
+
+struct pkt_file {
+	struct sk_buff *pkt;
+	__kernel_size_t pkt_len;	 /* the remainder length of the open_file */
+	unsigned char *cur_buffer;
+	u8 *buf_start;
+	u8 *cur_addr;
+	__kernel_size_t buf_len;
+};
+
+#define NR_XMITFRAME	256
+
+struct xmit_priv;
+struct pkt_attrib;
+struct sta_xmit_priv;
+struct xmit_frame;
+struct xmit_buf;
+
+extern void _rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev);
+extern netdev_tx_t rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev);
+
+void rtw_os_xmit_schedule(struct adapter *padapter);
+
+int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 alloc_sz, u8 flag);
+void rtw_os_xmit_resource_free(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 free_sz, u8 flag);
+
+extern uint rtw_remainder_len(struct pkt_file *pfile);
+extern void _rtw_open_pktfile(struct sk_buff *pkt, struct pkt_file *pfile);
+extern uint _rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen);
+extern signed int rtw_endofpktfile(struct pkt_file *pfile);
+
+extern void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt);
+extern void rtw_os_xmit_complete(struct adapter *padapter, struct xmit_frame *pxframe);
+
+#endif /* __XMIT_OSDEP_H_ */
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
new file mode 100644
index 000000000..5c7380113
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -0,0 +1,2883 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+
+#include <rtw_wifi_regd.h>
+
+#define RTW_MAX_MGMT_TX_CNT (8)
+
+#define RTW_SCAN_IE_LEN_MAX      2304
+#define RTW_MAX_REMAIN_ON_CHANNEL_DURATION 5000 /* ms */
+#define RTW_MAX_NUM_PMKIDS 4
+
+static const u32 rtw_cipher_suites[] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104,
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_AES_CMAC,
+};
+
+#define RATETAB_ENT(_rate, _rateid, _flags) \
+	{								\
+		.bitrate	= (_rate),				\
+		.hw_value	= (_rateid),				\
+		.flags		= (_flags),				\
+	}
+
+#define CHAN2G(_channel, _freq, _flags) {			\
+	.band			= NL80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+/* if wowlan is not supported, kernel generate a disconnect at each suspend
+ * cf: /net/wireless/sysfs.c, so register a stub wowlan.
+ * Moreover wowlan has to be enabled via a the nl80211_set_wowlan callback.
+ * (from user space, e.g. iw phy0 wowlan enable)
+ */
+static __maybe_unused const struct wiphy_wowlan_support wowlan_stub = {
+	.flags = WIPHY_WOWLAN_ANY,
+	.n_patterns = 0,
+	.pattern_max_len = 0,
+	.pattern_min_len = 0,
+	.max_pkt_offset = 0,
+};
+
+static struct ieee80211_rate rtw_rates[] = {
+	RATETAB_ENT(10,  0x1,   0),
+	RATETAB_ENT(20,  0x2,   0),
+	RATETAB_ENT(55,  0x4,   0),
+	RATETAB_ENT(110, 0x8,   0),
+	RATETAB_ENT(60,  0x10,  0),
+	RATETAB_ENT(90,  0x20,  0),
+	RATETAB_ENT(120, 0x40,  0),
+	RATETAB_ENT(180, 0x80,  0),
+	RATETAB_ENT(240, 0x100, 0),
+	RATETAB_ENT(360, 0x200, 0),
+	RATETAB_ENT(480, 0x400, 0),
+	RATETAB_ENT(540, 0x800, 0),
+};
+
+#define rtw_g_rates		(rtw_rates + 0)
+#define RTW_G_RATES_NUM	12
+
+#define RTW_2G_CHANNELS_NUM 14
+
+static struct ieee80211_channel rtw_2ghz_channels[] = {
+	CHAN2G(1, 2412, 0),
+	CHAN2G(2, 2417, 0),
+	CHAN2G(3, 2422, 0),
+	CHAN2G(4, 2427, 0),
+	CHAN2G(5, 2432, 0),
+	CHAN2G(6, 2437, 0),
+	CHAN2G(7, 2442, 0),
+	CHAN2G(8, 2447, 0),
+	CHAN2G(9, 2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+static void rtw_2g_channels_init(struct ieee80211_channel *channels)
+{
+	memcpy((void *)channels, (void *)rtw_2ghz_channels,
+		sizeof(struct ieee80211_channel)*RTW_2G_CHANNELS_NUM
+	);
+}
+
+static void rtw_2g_rates_init(struct ieee80211_rate *rates)
+{
+	memcpy(rates, rtw_g_rates,
+		sizeof(struct ieee80211_rate)*RTW_G_RATES_NUM
+	);
+}
+
+static struct ieee80211_supported_band *rtw_spt_band_alloc(
+	enum nl80211_band band
+	)
+{
+	struct ieee80211_supported_band *spt_band = NULL;
+	int n_channels, n_bitrates;
+
+	if (band == NL80211_BAND_2GHZ) {
+		n_channels = RTW_2G_CHANNELS_NUM;
+		n_bitrates = RTW_G_RATES_NUM;
+	} else {
+		goto exit;
+	}
+
+	spt_band = rtw_zmalloc(sizeof(struct ieee80211_supported_band) +
+			       sizeof(struct ieee80211_channel) * n_channels +
+			       sizeof(struct ieee80211_rate) * n_bitrates);
+	if (!spt_band)
+		goto exit;
+
+	spt_band->channels = (struct ieee80211_channel *)(((u8 *)spt_band)+sizeof(struct ieee80211_supported_band));
+	spt_band->bitrates = (struct ieee80211_rate *)(((u8 *)spt_band->channels)+sizeof(struct ieee80211_channel)*n_channels);
+	spt_band->band = band;
+	spt_band->n_channels = n_channels;
+	spt_band->n_bitrates = n_bitrates;
+
+	if (band == NL80211_BAND_2GHZ) {
+		rtw_2g_channels_init(spt_band->channels);
+		rtw_2g_rates_init(spt_band->bitrates);
+	}
+
+	/* spt_band.ht_cap */
+
+exit:
+
+	return spt_band;
+}
+
+static const struct ieee80211_txrx_stypes
+rtw_cfg80211_default_mgmt_stypes[NUM_NL80211_IFTYPES] = {
+	[NL80211_IFTYPE_ADHOC] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_STATION] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_AP] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_AP_VLAN] = {
+		/* copy AP */
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_P2P_CLIENT] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_P2P_GO] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+};
+
+static int rtw_ieee80211_channel_to_frequency(int chan, int band)
+{
+	if (band == NL80211_BAND_2GHZ) {
+		if (chan == 14)
+			return 2484;
+		else if (chan < 14)
+			return 2407 + chan * 5;
+	}
+
+	return 0; /* not supported */
+}
+
+#define MAX_BSSINFO_LEN 1000
+struct cfg80211_bss *rtw_cfg80211_inform_bss(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	struct ieee80211_channel *notify_channel;
+	struct cfg80211_bss *bss = NULL;
+	/* struct ieee80211_supported_band *band; */
+	u16 channel;
+	u32 freq;
+	u64 notify_timestamp;
+	s32 notify_signal;
+	u8 *buf = NULL, *pbuf;
+	size_t len, bssinf_len = 0;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+
+	struct wireless_dev *wdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = wdev->wiphy;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	bssinf_len = pnetwork->network.ie_length + sizeof(struct ieee80211_hdr_3addr);
+	if (bssinf_len > MAX_BSSINFO_LEN)
+		goto exit;
+
+	{
+		u16 wapi_len = 0;
+
+		if (rtw_get_wapi_ie(pnetwork->network.ies, pnetwork->network.ie_length, NULL, &wapi_len) > 0) {
+			if (wapi_len > 0)
+				goto exit;
+		}
+	}
+
+	/* To reduce PBC Overlap rate */
+	/* spin_lock_bh(&pwdev_priv->scan_req_lock); */
+	if (adapter_wdev_data(padapter)->scan_request) {
+		u8 *psr = NULL, sr = 0;
+		struct ndis_802_11_ssid *pssid = &pnetwork->network.ssid;
+		struct cfg80211_scan_request *request = adapter_wdev_data(padapter)->scan_request;
+		struct cfg80211_ssid *ssids = request->ssids;
+		u32 wpsielen = 0;
+		u8 *wpsie = NULL;
+
+		wpsie = rtw_get_wps_ie(pnetwork->network.ies+_FIXED_IE_LENGTH_, pnetwork->network.ie_length-_FIXED_IE_LENGTH_, NULL, &wpsielen);
+
+		if (wpsie && wpsielen > 0)
+			psr = rtw_get_wps_attr_content(wpsie,  wpsielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+
+		if (sr != 0) {
+			/* it means under processing WPS */
+			if (request->n_ssids == 1 && request->n_channels == 1) {
+				if (ssids[0].ssid_len != 0 &&
+				    (pssid->ssid_length != ssids[0].ssid_len ||
+				     memcmp(pssid->ssid, ssids[0].ssid, ssids[0].ssid_len))) {
+					if (psr)
+						*psr = 0; /* clear sr */
+				}
+			}
+		}
+	}
+	/* spin_unlock_bh(&pwdev_priv->scan_req_lock); */
+
+
+	channel = pnetwork->network.configuration.ds_config;
+	freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	notify_timestamp = ktime_to_us(ktime_get_boottime());
+
+	/* We've set wiphy's signal_type as CFG80211_SIGNAL_TYPE_MBM: signal strength in mBm (100*dBm) */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+		is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) {
+		notify_signal = 100*translate_percentage_to_dbm(padapter->recvpriv.signal_strength);/* dbm */
+	} else {
+		notify_signal = 100*translate_percentage_to_dbm(pnetwork->network.phy_info.signal_strength);/* dbm */
+	}
+
+	buf = kzalloc(MAX_BSSINFO_LEN, GFP_ATOMIC);
+	if (!buf)
+		goto exit;
+	pbuf = buf;
+
+	pwlanhdr = (struct ieee80211_hdr *)pbuf;
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+
+	if (pnetwork->network.reserved[0] == 1) { /*  WIFI_BEACON */
+		eth_broadcast_addr(pwlanhdr->addr1);
+		SetFrameSubType(pbuf, WIFI_BEACON);
+	} else {
+		memcpy(pwlanhdr->addr1, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		SetFrameSubType(pbuf, WIFI_PROBERSP);
+	}
+
+	memcpy(pwlanhdr->addr2, pnetwork->network.mac_address, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pnetwork->network.mac_address, ETH_ALEN);
+
+
+	pbuf += sizeof(struct ieee80211_hdr_3addr);
+	len = sizeof(struct ieee80211_hdr_3addr);
+
+	memcpy(pbuf, pnetwork->network.ies, pnetwork->network.ie_length);
+	len += pnetwork->network.ie_length;
+
+	*((__le64 *)pbuf) = cpu_to_le64(notify_timestamp);
+
+	bss = cfg80211_inform_bss_frame(wiphy, notify_channel, (struct ieee80211_mgmt *)buf,
+		len, notify_signal, GFP_ATOMIC);
+
+	if (unlikely(!bss))
+		goto exit;
+
+	cfg80211_put_bss(wiphy, bss);
+	kfree(buf);
+
+exit:
+	return bss;
+
+}
+
+/*
+	Check the given bss is valid by kernel API cfg80211_get_bss()
+	@padapter : the given adapter
+
+	return true if bss is valid,  false for not found.
+*/
+int rtw_cfg80211_check_bss(struct adapter *padapter)
+{
+	struct wlan_bssid_ex  *pnetwork = &(padapter->mlmeextpriv.mlmext_info.network);
+	struct cfg80211_bss *bss = NULL;
+	struct ieee80211_channel *notify_channel = NULL;
+	u32 freq;
+
+	if (!(pnetwork) || !(padapter->rtw_wdev))
+		return false;
+
+	freq = rtw_ieee80211_channel_to_frequency(pnetwork->configuration.ds_config, NL80211_BAND_2GHZ);
+
+	notify_channel = ieee80211_get_channel(padapter->rtw_wdev->wiphy, freq);
+	bss = cfg80211_get_bss(padapter->rtw_wdev->wiphy, notify_channel,
+			pnetwork->mac_address, pnetwork->ssid.ssid,
+			pnetwork->ssid.ssid_length,
+			IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY);
+
+	cfg80211_put_bss(padapter->rtw_wdev->wiphy, bss);
+
+	return	(bss != NULL);
+}
+
+void rtw_cfg80211_ibss_indicate_connect(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = pwdev->wiphy;
+	int freq = (int)cur_network->network.configuration.ds_config;
+	struct ieee80211_channel *chan;
+
+	if (pwdev->iftype != NL80211_IFTYPE_ADHOC)
+		return;
+
+	if (!rtw_cfg80211_check_bss(padapter)) {
+		struct wlan_bssid_ex  *pnetwork = &(padapter->mlmeextpriv.mlmext_info.network);
+		struct wlan_network *scanned = pmlmepriv->cur_network_scanned;
+
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+
+			memcpy(&cur_network->network, pnetwork, sizeof(struct wlan_bssid_ex));
+			rtw_cfg80211_inform_bss(padapter, cur_network);
+		} else {
+			if (!scanned) {
+				rtw_warn_on(1);
+				return;
+			}
+			if (!memcmp(&(scanned->network.ssid), &(pnetwork->ssid), sizeof(struct ndis_802_11_ssid))
+				&& !memcmp(scanned->network.mac_address, pnetwork->mac_address, sizeof(NDIS_802_11_MAC_ADDRESS))
+			)
+				rtw_cfg80211_inform_bss(padapter, scanned);
+			else
+				rtw_warn_on(1);
+		}
+
+		if (!rtw_cfg80211_check_bss(padapter))
+			netdev_dbg(padapter->pnetdev,
+				   FUNC_ADPT_FMT " BSS not found !!\n",
+				   FUNC_ADPT_ARG(padapter));
+	}
+	/* notify cfg80211 that device joined an IBSS */
+	chan = ieee80211_get_channel(wiphy, freq);
+	cfg80211_ibss_joined(padapter->pnetdev, cur_network->network.mac_address, chan, GFP_ATOMIC);
+}
+
+void rtw_cfg80211_indicate_connect(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+
+	if (pwdev->iftype != NL80211_IFTYPE_STATION
+		&& pwdev->iftype != NL80211_IFTYPE_P2P_CLIENT
+	) {
+		return;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		return;
+
+	{
+		struct wlan_bssid_ex  *pnetwork = &(padapter->mlmeextpriv.mlmext_info.network);
+		struct wlan_network *scanned = pmlmepriv->cur_network_scanned;
+
+		if (!scanned) {
+			rtw_warn_on(1);
+			goto check_bss;
+		}
+
+		if (!memcmp(scanned->network.mac_address, pnetwork->mac_address, sizeof(NDIS_802_11_MAC_ADDRESS))
+			&& !memcmp(&(scanned->network.ssid), &(pnetwork->ssid), sizeof(struct ndis_802_11_ssid))
+		)
+			rtw_cfg80211_inform_bss(padapter, scanned);
+		else
+			rtw_warn_on(1);
+	}
+
+check_bss:
+	if (!rtw_cfg80211_check_bss(padapter))
+		netdev_dbg(padapter->pnetdev,
+			   FUNC_ADPT_FMT " BSS not found !!\n",
+			   FUNC_ADPT_ARG(padapter));
+
+	if (rtw_to_roam(padapter) > 0) {
+		struct wiphy *wiphy = pwdev->wiphy;
+		struct ieee80211_channel *notify_channel;
+		u32 freq;
+		u16 channel = cur_network->network.configuration.ds_config;
+		struct cfg80211_roam_info roam_info = {};
+
+		freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+		notify_channel = ieee80211_get_channel(wiphy, freq);
+
+		roam_info.links[0].channel = notify_channel;
+		roam_info.links[0].bssid = cur_network->network.mac_address;
+		roam_info.req_ie =
+			pmlmepriv->assoc_req+sizeof(struct ieee80211_hdr_3addr)+2;
+		roam_info.req_ie_len =
+			pmlmepriv->assoc_req_len-sizeof(struct ieee80211_hdr_3addr)-2;
+		roam_info.resp_ie =
+			pmlmepriv->assoc_rsp+sizeof(struct ieee80211_hdr_3addr)+6;
+		roam_info.resp_ie_len =
+			pmlmepriv->assoc_rsp_len-sizeof(struct ieee80211_hdr_3addr)-6;
+		cfg80211_roamed(padapter->pnetdev, &roam_info, GFP_ATOMIC);
+	} else {
+		cfg80211_connect_result(padapter->pnetdev, cur_network->network.mac_address
+			, pmlmepriv->assoc_req+sizeof(struct ieee80211_hdr_3addr)+2
+			, pmlmepriv->assoc_req_len-sizeof(struct ieee80211_hdr_3addr)-2
+			, pmlmepriv->assoc_rsp+sizeof(struct ieee80211_hdr_3addr)+6
+			, pmlmepriv->assoc_rsp_len-sizeof(struct ieee80211_hdr_3addr)-6
+			, WLAN_STATUS_SUCCESS, GFP_ATOMIC);
+	}
+}
+
+void rtw_cfg80211_indicate_disconnect(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+
+	if (pwdev->iftype != NL80211_IFTYPE_STATION
+		&& pwdev->iftype != NL80211_IFTYPE_P2P_CLIENT
+	) {
+		return;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		return;
+
+	if (!padapter->mlmepriv.not_indic_disco) {
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+			cfg80211_disconnected(padapter->pnetdev, 0,
+					      NULL, 0, true, GFP_ATOMIC);
+		} else {
+			cfg80211_connect_result(padapter->pnetdev, NULL, NULL, 0, NULL, 0,
+				WLAN_STATUS_UNSPECIFIED_FAILURE, GFP_ATOMIC/*GFP_KERNEL*/);
+		}
+	}
+}
+
+
+static int rtw_cfg80211_ap_set_encryption(struct net_device *dev, struct ieee_param *param, u32 param_len)
+{
+	int ret = 0;
+	u32 wep_key_idx, wep_key_len;
+	struct sta_info *psta = NULL, *pbcmc_sta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv =  &(padapter->securitypriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	char *grpkey = padapter->securitypriv.dot118021XGrpKey[param->u.crypt.idx].skey;
+	char *txkey = padapter->securitypriv.dot118021XGrptxmickey[param->u.crypt.idx].skey;
+	char *rxkey = padapter->securitypriv.dot118021XGrprxmickey[param->u.crypt.idx].skey;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len !=  sizeof(struct ieee_param) + param->u.crypt.key_len) {
+		ret =  -EINVAL;
+		goto exit;
+	}
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		if (param->u.crypt.idx >= WEP_KEYS) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	} else {
+		psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+		if (!psta)
+			/* ret = -EINVAL; */
+			goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0 && !psta)
+		goto exit;
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0 && !psta) {
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		if ((wep_key_idx >= WEP_KEYS) || (wep_key_len <= 0)) {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		if (wep_key_len > 0)
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+
+		if (psecuritypriv->bWepDefaultKeyIdxSet == 0) {
+			/* wep default key has not been set, so use this key index as default key. */
+
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+			psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+
+			if (wep_key_len == 13) {
+				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+			}
+
+			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
+		}
+
+		memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), param->u.crypt.key, wep_key_len);
+
+		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+
+		rtw_ap_set_wep_key(padapter, param->u.crypt.key, wep_key_len, wep_key_idx, 1);
+
+		goto exit;
+
+	}
+
+	/* group key */
+	if (!psta && check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* group key */
+		if (param->u.crypt.set_tx == 0) {
+			if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+				if (param->u.crypt.key_len == 13)
+						psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+
+			} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+				/* set mic key */
+				memcpy(txkey, &(param->u.crypt.key[16]), 8);
+				memcpy(rxkey, &(param->u.crypt.key[24]), 8);
+
+				psecuritypriv->busetkipkey = true;
+
+			} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+			} else {
+				psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+			}
+
+			psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
+
+			psecuritypriv->binstallGrpkey = true;
+
+			psecuritypriv->dot11PrivacyAlgrthm = psecuritypriv->dot118021XGrpPrivacy;/*  */
+
+			rtw_ap_set_group_key(padapter, param->u.crypt.key, psecuritypriv->dot118021XGrpPrivacy, param->u.crypt.idx);
+
+			pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+			if (pbcmc_sta) {
+				pbcmc_sta->ieee8021x_blocked = false;
+				pbcmc_sta->dot118021XPrivacy = psecuritypriv->dot118021XGrpPrivacy;/* rx will use bmc_sta's dot118021XPrivacy */
+			}
+
+		}
+
+		goto exit;
+
+	}
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X && psta) { /*  psk/802_1x */
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+			if (param->u.crypt.set_tx == 1) { /* pairwise key */
+				memcpy(psta->dot118021x_UncstKey.skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+					psta->dot118021XPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+						psta->dot118021XPrivacy = _WEP104_;
+				} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+					psta->dot118021XPrivacy = _TKIP_;
+
+					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+					/* set mic key */
+					memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
+					memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+
+					psecuritypriv->busetkipkey = true;
+
+				} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+
+					psta->dot118021XPrivacy = _AES_;
+				} else {
+					psta->dot118021XPrivacy = _NO_PRIVACY_;
+				}
+
+				rtw_ap_set_pairwise_key(padapter, psta);
+
+				psta->ieee8021x_blocked = false;
+
+				psta->bpairwise_key_installed = true;
+
+			} else { /* group key??? */
+				if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+						psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+					psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+					/* set mic key */
+					memcpy(txkey, &(param->u.crypt.key[16]), 8);
+					memcpy(rxkey, &(param->u.crypt.key[24]), 8);
+
+					psecuritypriv->busetkipkey = true;
+
+				} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+					psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+				} else {
+					psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+				}
+
+				psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
+
+				psecuritypriv->binstallGrpkey = true;
+
+				psecuritypriv->dot11PrivacyAlgrthm = psecuritypriv->dot118021XGrpPrivacy;/*  */
+
+				rtw_ap_set_group_key(padapter, param->u.crypt.key, psecuritypriv->dot118021XGrpPrivacy, param->u.crypt.idx);
+
+				pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+				if (pbcmc_sta) {
+					pbcmc_sta->ieee8021x_blocked = false;
+					pbcmc_sta->dot118021XPrivacy = psecuritypriv->dot118021XGrpPrivacy;/* rx will use bmc_sta's dot118021XPrivacy */
+				}
+
+			}
+
+		}
+
+	}
+
+exit:
+
+	return ret;
+
+}
+
+static int rtw_cfg80211_set_encryption(struct net_device *dev, struct ieee_param *param, u32 param_len)
+{
+	int ret = 0;
+	u8 max_idx;
+	u32 wep_key_idx, wep_key_len;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len < (u32) ((u8 *) param->u.crypt.key - (u8 *) param) + param->u.crypt.key_len) {
+		ret =  -EINVAL;
+		goto exit;
+	}
+
+	if (param->sta_addr[0] != 0xff || param->sta_addr[1] != 0xff ||
+	    param->sta_addr[2] != 0xff || param->sta_addr[3] != 0xff ||
+	    param->sta_addr[4] != 0xff || param->sta_addr[5] != 0xff) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0)
+		max_idx = WEP_KEYS - 1;
+	else
+		max_idx = BIP_MAX_KEYID;
+
+	if (param->u.crypt.idx > max_idx) {
+		netdev_err(dev, "Error crypt.idx %d > %d\n", param->u.crypt.idx, max_idx);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		if (wep_key_len <= 0) {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		if (psecuritypriv->bWepDefaultKeyIdxSet == 0) {
+			/* wep default key has not been set, so use this key index as default key. */
+
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+
+			psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+
+			if (wep_key_len == 13) {
+				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+			}
+
+			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
+		}
+
+		memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), param->u.crypt.key, wep_key_len);
+
+		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+
+		rtw_set_key(padapter, psecuritypriv, wep_key_idx, 0, true);
+
+		goto exit;
+	}
+
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { /*  802_1x */
+		struct sta_info *psta, *pbcmc_sta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_MP_STATE) == true) { /* sta mode */
+			psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+			if (psta) {
+				/* Jeff: don't disable ieee8021x_blocked while clearing key */
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					psta->ieee8021x_blocked = false;
+
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+						(padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled)) {
+					psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+				}
+
+				if (param->u.crypt.set_tx == 1) { /* pairwise key */
+
+					memcpy(psta->dot118021x_UncstKey.skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					if (strcmp(param->u.crypt.alg, "TKIP") == 0) { /* set mic key */
+						/* DEBUG_ERR(("\nset key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+						memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
+						memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+
+						padapter->securitypriv.busetkipkey = false;
+						/* _set_timer(&padapter->securitypriv.tkip_timer, 50); */
+					}
+
+					rtw_setstakey_cmd(padapter, psta, true, true);
+				} else { /* group key */
+					if (strcmp(param->u.crypt.alg, "TKIP") == 0 || strcmp(param->u.crypt.alg, "CCMP") == 0) {
+						memcpy(padapter->securitypriv.dot118021XGrpKey[param->u.crypt.idx].skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+						memcpy(padapter->securitypriv.dot118021XGrptxmickey[param->u.crypt.idx].skey, &(param->u.crypt.key[16]), 8);
+						memcpy(padapter->securitypriv.dot118021XGrprxmickey[param->u.crypt.idx].skey, &(param->u.crypt.key[24]), 8);
+						padapter->securitypriv.binstallGrpkey = true;
+
+						padapter->securitypriv.dot118021XGrpKeyid = param->u.crypt.idx;
+						rtw_set_key(padapter, &padapter->securitypriv, param->u.crypt.idx, 1, true);
+					} else if (strcmp(param->u.crypt.alg, "BIP") == 0) {
+						/* save the IGTK key, length 16 bytes */
+						memcpy(padapter->securitypriv.dot11wBIPKey[param->u.crypt.idx].skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+						/*
+						for (no = 0;no<16;no++)
+							printk(" %02x ", padapter->securitypriv.dot11wBIPKey[param->u.crypt.idx].skey[no]);
+						*/
+						padapter->securitypriv.dot11wBIPKeyid = param->u.crypt.idx;
+						padapter->securitypriv.binstallBIPkey = true;
+					}
+				}
+			}
+
+			pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+			if (!pbcmc_sta) {
+				/* DEBUG_ERR(("Set OID_802_11_ADD_KEY: bcmc stainfo is null\n")); */
+			} else {
+				/* Jeff: don't disable ieee8021x_blocked while clearing key */
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					pbcmc_sta->ieee8021x_blocked = false;
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+						(padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled)) {
+					pbcmc_sta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+				}
+			}
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) { /* adhoc mode */
+		}
+	}
+
+exit:
+
+	return ret;
+}
+
+static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
+				int link_id, u8 key_index, bool pairwise,
+				const u8 *mac_addr, struct key_params *params)
+{
+	char *alg_name;
+	u32 param_len;
+	struct ieee_param *param = NULL;
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	param_len = sizeof(struct ieee_param) + params->key_len;
+	param = rtw_malloc(param_len);
+	if (!param)
+		return -1;
+
+	memset(param, 0, param_len);
+
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	eth_broadcast_addr(param->sta_addr);
+
+	switch (params->cipher) {
+	case IW_AUTH_CIPHER_NONE:
+		/* todo: remove key */
+		/* remove = 1; */
+		alg_name = "none";
+		break;
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		alg_name = "WEP";
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		alg_name = "TKIP";
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		alg_name = "CCMP";
+		break;
+	case WLAN_CIPHER_SUITE_AES_CMAC:
+		alg_name = "BIP";
+		break;
+	default:
+		ret = -ENOTSUPP;
+		goto addkey_end;
+	}
+
+	strncpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
+
+
+	if (!mac_addr || is_broadcast_ether_addr(mac_addr))
+		param->u.crypt.set_tx = 0; /* for wpa/wpa2 group key */
+	else
+		param->u.crypt.set_tx = 1; /* for wpa/wpa2 pairwise key */
+
+	param->u.crypt.idx = key_index;
+
+	if (params->seq_len && params->seq)
+		memcpy(param->u.crypt.seq, (u8 *)params->seq, params->seq_len);
+
+	if (params->key_len && params->key) {
+		param->u.crypt.key_len = params->key_len;
+		memcpy(param->u.crypt.key, (u8 *)params->key, params->key_len);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+		ret =  rtw_cfg80211_set_encryption(ndev, param, param_len);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		if (mac_addr)
+			memcpy(param->sta_addr, (void *)mac_addr, ETH_ALEN);
+
+		ret = rtw_cfg80211_ap_set_encryption(ndev, param, param_len);
+	} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true
+		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+		ret =  rtw_cfg80211_set_encryption(ndev, param, param_len);
+	}
+
+addkey_end:
+	kfree(param);
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_get_key(struct wiphy *wiphy, struct net_device *ndev,
+				int link_id, u8 key_index, bool pairwise,
+				const u8 *mac_addr, void *cookie,
+				void (*callback)(void *cookie,
+						 struct key_params*))
+{
+	return 0;
+}
+
+static int cfg80211_rtw_del_key(struct wiphy *wiphy, struct net_device *ndev,
+				int link_id, u8 key_index, bool pairwise,
+				const u8 *mac_addr)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if (key_index == psecuritypriv->dot11PrivacyKeyIndex) {
+		/* clear the flag of wep default key set. */
+		psecuritypriv->bWepDefaultKeyIdxSet = 0;
+	}
+
+	return 0;
+}
+
+static int cfg80211_rtw_set_default_key(struct wiphy *wiphy,
+	struct net_device *ndev, int link_id, u8 key_index
+	, bool unicast, bool multicast
+	)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if ((key_index < WEP_KEYS) && ((psecuritypriv->dot11PrivacyAlgrthm == _WEP40_) || (psecuritypriv->dot11PrivacyAlgrthm == _WEP104_))) { /* set wep default key */
+		psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+
+		psecuritypriv->dot11PrivacyKeyIndex = key_index;
+
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+		if (psecuritypriv->dot11DefKeylen[key_index] == 13) {
+			psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+			psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+		}
+
+		psecuritypriv->bWepDefaultKeyIdxSet = 1; /* set the flag to represent that wep default key has been set */
+	}
+
+	return 0;
+
+}
+
+static int cfg80211_rtw_get_station(struct wiphy *wiphy,
+				    struct net_device *ndev,
+				const u8 *mac,
+				struct station_info *sinfo)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	sinfo->filled = 0;
+
+	if (!mac) {
+		ret = -ENOENT;
+		goto exit;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, (u8 *)mac);
+	if (!psta) {
+		ret = -ENOENT;
+		goto exit;
+	}
+
+	/* for infra./P2PClient mode */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+		&& check_fwstate(pmlmepriv, _FW_LINKED)) {
+		struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+
+		if (memcmp((u8 *)mac, cur_network->network.mac_address, ETH_ALEN)) {
+			ret = -ENOENT;
+			goto exit;
+		}
+
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_SIGNAL);
+		sinfo->signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);
+
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_BITRATE);
+		sinfo->txrate.legacy = rtw_get_cur_max_rate(padapter);
+
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_RX_PACKETS);
+		sinfo->rx_packets = sta_rx_data_pkts(psta);
+
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_PACKETS);
+		sinfo->tx_packets = psta->sta_stats.tx_pkts;
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_FAILED);
+	}
+
+	/* for Ad-Hoc/AP mode */
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)
+ || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)
+ || check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		&& check_fwstate(pmlmepriv, _FW_LINKED)) {
+		/* TODO: should acquire station info... */
+	}
+
+exit:
+	return ret;
+}
+
+static int cfg80211_rtw_change_iface(struct wiphy *wiphy,
+				     struct net_device *ndev,
+				     enum nl80211_iftype type,
+				     struct vif_params *params)
+{
+	enum nl80211_iftype old_type;
+	enum ndis_802_11_network_infrastructure networkType;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct wireless_dev *rtw_wdev = padapter->rtw_wdev;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	int ret = 0;
+
+	if (adapter_to_dvobj(padapter)->processing_dev_remove == true) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	{
+		if (netdev_open(ndev) != 0) {
+			ret = -EPERM;
+			goto exit;
+		}
+	}
+
+	if (rtw_pwr_wakeup(padapter) == _FAIL) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	old_type = rtw_wdev->iftype;
+
+	if (old_type != type) {
+		pmlmeext->action_public_rxseq = 0xffff;
+		pmlmeext->action_public_dialog_token = 0xff;
+	}
+
+	switch (type) {
+	case NL80211_IFTYPE_ADHOC:
+		networkType = Ndis802_11IBSS;
+		break;
+	case NL80211_IFTYPE_STATION:
+		networkType = Ndis802_11Infrastructure;
+		break;
+	case NL80211_IFTYPE_AP:
+		networkType = Ndis802_11APMode;
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		goto exit;
+	}
+
+	rtw_wdev->iftype = type;
+
+	if (rtw_set_802_11_infrastructure_mode(padapter, networkType) == false) {
+		rtw_wdev->iftype = old_type;
+		ret = -EPERM;
+		goto exit;
+	}
+
+	rtw_setopmode_cmd(padapter, networkType, true);
+
+exit:
+
+	return ret;
+}
+
+void rtw_cfg80211_indicate_scan_done(struct adapter *adapter, bool aborted)
+{
+	struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter);
+	struct cfg80211_scan_info info = {
+		.aborted = aborted
+	};
+
+	spin_lock_bh(&pwdev_priv->scan_req_lock);
+	if (pwdev_priv->scan_request) {
+		/* avoid WARN_ON(request != wiphy_to_dev(request->wiphy)->scan_req); */
+		if (pwdev_priv->scan_request->wiphy == pwdev_priv->rtw_wdev->wiphy)
+			cfg80211_scan_done(pwdev_priv->scan_request, &info);
+
+		pwdev_priv->scan_request = NULL;
+	}
+	spin_unlock_bh(&pwdev_priv->scan_req_lock);
+}
+
+void rtw_cfg80211_unlink_bss(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = pwdev->wiphy;
+	struct cfg80211_bss *bss = NULL;
+	struct wlan_bssid_ex *select_network = &pnetwork->network;
+
+	bss = cfg80211_get_bss(wiphy, NULL/*notify_channel*/,
+		select_network->mac_address, select_network->ssid.ssid,
+		select_network->ssid.ssid_length, IEEE80211_BSS_TYPE_ANY,
+		IEEE80211_PRIVACY_ANY);
+
+	if (bss) {
+		cfg80211_unlink_bss(wiphy, bss);
+		cfg80211_put_bss(padapter->rtw_wdev->wiphy, bss);
+	}
+}
+
+void rtw_cfg80211_surveydone_event_callback(struct adapter *padapter)
+{
+	struct list_head					*plist, *phead;
+	struct	mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct __queue *queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	phead = get_list_head(queue);
+	list_for_each(plist, phead)
+	{
+		pnetwork = list_entry(plist, struct wlan_network, list);
+
+		/* report network only if the current channel set contains the channel to which this network belongs */
+		if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, pnetwork->network.configuration.ds_config) >= 0
+			&& true == rtw_validate_ssid(&(pnetwork->network.ssid))) {
+			/* ev =translate_scan(padapter, a, pnetwork, ev, stop); */
+			rtw_cfg80211_inform_bss(padapter, pnetwork);
+		}
+
+	}
+
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+}
+
+static int rtw_cfg80211_set_probe_req_wpsp2pie(struct adapter *padapter, char *buf, int len)
+{
+	int ret = 0;
+	uint wps_ielen = 0;
+	u8 *wps_ie;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	if (len > 0) {
+		wps_ie = rtw_get_wps_ie(buf, len, NULL, &wps_ielen);
+		if (wps_ie) {
+			if (pmlmepriv->wps_probe_req_ie) {
+				pmlmepriv->wps_probe_req_ie_len = 0;
+				kfree(pmlmepriv->wps_probe_req_ie);
+				pmlmepriv->wps_probe_req_ie = NULL;
+			}
+
+			pmlmepriv->wps_probe_req_ie = rtw_malloc(wps_ielen);
+			if (!pmlmepriv->wps_probe_req_ie)
+				return -EINVAL;
+
+			memcpy(pmlmepriv->wps_probe_req_ie, wps_ie, wps_ielen);
+			pmlmepriv->wps_probe_req_ie_len = wps_ielen;
+		}
+	}
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_scan(struct wiphy *wiphy
+	, struct cfg80211_scan_request *request)
+{
+	struct net_device *ndev = wdev_to_ndev(request->wdev);
+	int i;
+	u8 _status = false;
+	int ret = 0;
+	struct ndis_802_11_ssid *ssid = NULL;
+	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
+	u8 survey_times = 3;
+	u8 survey_times_for_one_ch = 6;
+	struct cfg80211_ssid *ssids = request->ssids;
+	int j = 0;
+	bool need_indicate_scan_done = false;
+
+	struct adapter *padapter;
+	struct rtw_wdev_priv *pwdev_priv;
+	struct mlme_priv *pmlmepriv;
+
+	if (!ndev) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	padapter = rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+/* endif */
+
+	spin_lock_bh(&pwdev_priv->scan_req_lock);
+	pwdev_priv->scan_request = request;
+	spin_unlock_bh(&pwdev_priv->scan_req_lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS|_FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) {
+			need_indicate_scan_done = true;
+			goto check_need_indicate_scan_done;
+		}
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_SCAN);
+	if (rtw_pwr_wakeup(padapter) == _FAIL) {
+		need_indicate_scan_done = true;
+		goto check_need_indicate_scan_done;
+	}
+
+	if (request->ie && request->ie_len > 0)
+		rtw_cfg80211_set_probe_req_wpsp2pie(padapter, (u8 *)request->ie, request->ie_len);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		need_indicate_scan_done = true;
+		goto check_need_indicate_scan_done;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		ret = -EBUSY;
+		goto check_need_indicate_scan_done;
+	}
+
+	if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true) {
+		static unsigned long lastscantime;
+		unsigned long passtime;
+
+		passtime = jiffies_to_msecs(jiffies - lastscantime);
+		lastscantime = jiffies;
+		if (passtime > 12000) {
+			need_indicate_scan_done = true;
+			goto check_need_indicate_scan_done;
+		}
+	}
+
+	if (rtw_is_scan_deny(padapter)) {
+		need_indicate_scan_done = true;
+		goto check_need_indicate_scan_done;
+	}
+
+	ssid = kzalloc(RTW_SSID_SCAN_AMOUNT * sizeof(struct ndis_802_11_ssid),
+		       GFP_KERNEL);
+	if (!ssid) {
+		ret = -ENOMEM;
+		goto check_need_indicate_scan_done;
+	}
+
+	/* parsing request ssids, n_ssids */
+	for (i = 0; i < request->n_ssids && i < RTW_SSID_SCAN_AMOUNT; i++) {
+		memcpy(ssid[i].ssid, ssids[i].ssid, ssids[i].ssid_len);
+		ssid[i].ssid_length = ssids[i].ssid_len;
+	}
+
+	/* parsing channels, n_channels */
+	memset(ch, 0, sizeof(struct rtw_ieee80211_channel)*RTW_CHANNEL_SCAN_AMOUNT);
+	for (i = 0; i < request->n_channels && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
+		ch[i].hw_value = request->channels[i]->hw_value;
+		ch[i].flags = request->channels[i]->flags;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+	if (request->n_channels == 1) {
+		for (i = 1; i < survey_times_for_one_ch; i++)
+			memcpy(&ch[i], &ch[0], sizeof(struct rtw_ieee80211_channel));
+		_status = rtw_sitesurvey_cmd(padapter, ssid, RTW_SSID_SCAN_AMOUNT, ch, survey_times_for_one_ch);
+	} else if (request->n_channels <= 4) {
+		for (j = request->n_channels - 1; j >= 0; j--)
+			for (i = 0; i < survey_times; i++)
+			memcpy(&ch[j*survey_times+i], &ch[j], sizeof(struct rtw_ieee80211_channel));
+		_status = rtw_sitesurvey_cmd(padapter, ssid, RTW_SSID_SCAN_AMOUNT, ch, survey_times * request->n_channels);
+	} else {
+		_status = rtw_sitesurvey_cmd(padapter, ssid, RTW_SSID_SCAN_AMOUNT, NULL, 0);
+	}
+	spin_unlock_bh(&pmlmepriv->lock);
+
+
+	if (_status == false)
+		ret = -1;
+
+check_need_indicate_scan_done:
+	kfree(ssid);
+	if (need_indicate_scan_done) {
+		rtw_cfg80211_surveydone_event_callback(padapter);
+		rtw_cfg80211_indicate_scan_done(padapter, false);
+	}
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_SCAN);
+
+exit:
+	return ret;
+
+}
+
+static int cfg80211_rtw_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+	return 0;
+}
+
+static int rtw_cfg80211_set_wpa_version(struct security_priv *psecuritypriv, u32 wpa_version)
+{
+	if (!wpa_version) {
+		psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
+		return 0;
+	}
+
+
+	if (wpa_version & (NL80211_WPA_VERSION_1 | NL80211_WPA_VERSION_2))
+		psecuritypriv->ndisauthtype = Ndis802_11AuthModeWPAPSK;
+
+	return 0;
+
+}
+
+static int rtw_cfg80211_set_auth_type(struct security_priv *psecuritypriv,
+			     enum nl80211_auth_type sme_auth_type)
+{
+	switch (sme_auth_type) {
+	case NL80211_AUTHTYPE_AUTOMATIC:
+
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+
+		break;
+	case NL80211_AUTHTYPE_OPEN_SYSTEM:
+
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+
+		if (psecuritypriv->ndisauthtype > Ndis802_11AuthModeWPA)
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+		break;
+	case NL80211_AUTHTYPE_SHARED_KEY:
+
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Shared;
+
+		psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+
+
+		break;
+	default:
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		/* return -ENOTSUPP; */
+	}
+
+	return 0;
+
+}
+
+static int rtw_cfg80211_set_cipher(struct security_priv *psecuritypriv, u32 cipher, bool ucast)
+{
+	u32 ndisencryptstatus = Ndis802_11EncryptionDisabled;
+
+	u32 *profile_cipher = ucast ? &psecuritypriv->dot11PrivacyAlgrthm :
+		&psecuritypriv->dot118021XGrpPrivacy;
+
+
+	if (!cipher) {
+		*profile_cipher = _NO_PRIVACY_;
+		psecuritypriv->ndisencryptstatus = ndisencryptstatus;
+		return 0;
+	}
+
+	switch (cipher) {
+	case IW_AUTH_CIPHER_NONE:
+		*profile_cipher = _NO_PRIVACY_;
+		ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		break;
+	case WLAN_CIPHER_SUITE_WEP40:
+		*profile_cipher = _WEP40_;
+		ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	case WLAN_CIPHER_SUITE_WEP104:
+		*profile_cipher = _WEP104_;
+		ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		*profile_cipher = _TKIP_;
+		ndisencryptstatus = Ndis802_11Encryption2Enabled;
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		*profile_cipher = _AES_;
+		ndisencryptstatus = Ndis802_11Encryption3Enabled;
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	if (ucast) {
+		psecuritypriv->ndisencryptstatus = ndisencryptstatus;
+
+		/* if (psecuritypriv->dot11PrivacyAlgrthm >= _AES_) */
+		/*	psecuritypriv->ndisauthtype = Ndis802_11AuthModeWPA2PSK; */
+	}
+
+	return 0;
+}
+
+static int rtw_cfg80211_set_key_mgt(struct security_priv *psecuritypriv, u32 key_mgt)
+{
+	if (key_mgt == WLAN_AKM_SUITE_8021X)
+		/* auth_type = UMAC_AUTH_TYPE_8021X; */
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+	else if (key_mgt == WLAN_AKM_SUITE_PSK) {
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+	}
+
+	return 0;
+}
+
+static int rtw_cfg80211_set_wpa_ie(struct adapter *padapter, u8 *pie, size_t ielen)
+{
+	u8 *buf = NULL;
+	int group_cipher = 0, pairwise_cipher = 0;
+	int ret = 0;
+	int wpa_ielen = 0;
+	int wpa2_ielen = 0;
+	u8 *pwpa, *pwpa2;
+	u8 null_addr[] = {0, 0, 0, 0, 0, 0};
+
+	if (!pie || !ielen) {
+		/* Treat this as normal case, but need to clear WIFI_UNDER_WPS */
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		goto exit;
+	}
+
+	if (ielen > MAX_WPA_IE_LEN+MAX_WPS_IE_LEN+MAX_P2P_IE_LEN) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	buf = rtw_zmalloc(ielen);
+	if (!buf) {
+		ret =  -ENOMEM;
+		goto exit;
+	}
+
+	memcpy(buf, pie, ielen);
+
+	if (ielen < RSN_HEADER_LEN) {
+		ret  = -1;
+		goto exit;
+	}
+
+	pwpa = rtw_get_wpa_ie(buf, &wpa_ielen, ielen);
+	if (pwpa && wpa_ielen > 0) {
+		if (rtw_parse_wpa_ie(pwpa, wpa_ielen+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPAPSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &pwpa[0], wpa_ielen+2);
+		}
+	}
+
+	pwpa2 = rtw_get_wpa2_ie(buf, &wpa2_ielen, ielen);
+	if (pwpa2 && wpa2_ielen > 0) {
+		if (rtw_parse_wpa2_ie(pwpa2, wpa2_ielen+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPA2PSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &pwpa2[0], wpa2_ielen+2);
+		}
+	}
+
+	if (group_cipher == 0)
+		group_cipher = WPA_CIPHER_NONE;
+
+	if (pairwise_cipher == 0)
+		pairwise_cipher = WPA_CIPHER_NONE;
+
+	switch (group_cipher) {
+	case WPA_CIPHER_NONE:
+		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		break;
+	case WPA_CIPHER_WEP40:
+		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	case WPA_CIPHER_TKIP:
+		padapter->securitypriv.dot118021XGrpPrivacy = _TKIP_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+		break;
+	case WPA_CIPHER_CCMP:
+		padapter->securitypriv.dot118021XGrpPrivacy = _AES_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+		break;
+	case WPA_CIPHER_WEP104:
+		padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	}
+
+	switch (pairwise_cipher) {
+	case WPA_CIPHER_NONE:
+		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		break;
+	case WPA_CIPHER_WEP40:
+		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	case WPA_CIPHER_TKIP:
+		padapter->securitypriv.dot11PrivacyAlgrthm = _TKIP_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+		break;
+	case WPA_CIPHER_CCMP:
+		padapter->securitypriv.dot11PrivacyAlgrthm = _AES_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+		break;
+	case WPA_CIPHER_WEP104:
+		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		break;
+	}
+
+	{/* handle wps_ie */
+		uint wps_ielen;
+		u8 *wps_ie;
+
+		wps_ie = rtw_get_wps_ie(buf, ielen, NULL, &wps_ielen);
+		if (wps_ie && wps_ielen > 0) {
+			padapter->securitypriv.wps_ie_len = wps_ielen < MAX_WPS_IE_LEN ? wps_ielen : MAX_WPS_IE_LEN;
+			memcpy(padapter->securitypriv.wps_ie, wps_ie, padapter->securitypriv.wps_ie_len);
+			set_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		} else {
+			_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		}
+	}
+
+	/* TKIP and AES disallow multicast packets until installing group key */
+	if (padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_
+		|| padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_WTMIC_
+		|| padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		/* WPS open need to enable multicast */
+		/*  check_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS) == true) */
+		rtw_hal_set_hwreg(padapter, HW_VAR_OFF_RCR_AM, null_addr);
+
+exit:
+	kfree(buf);
+	if (ret)
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+	return ret;
+}
+
+static int cfg80211_rtw_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
+				  struct cfg80211_ibss_params *params)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct ndis_802_11_ssid ndis_ssid;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	int ret = 0;
+
+	if (rtw_pwr_wakeup(padapter) == _FAIL) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	if (!params->ssid || !params->ssid_len) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (params->ssid_len > IW_ESSID_MAX_SIZE) {
+
+		ret = -E2BIG;
+		goto exit;
+	}
+
+	memset(&ndis_ssid, 0, sizeof(struct ndis_802_11_ssid));
+	ndis_ssid.ssid_length = params->ssid_len;
+	memcpy(ndis_ssid.ssid, (u8 *)params->ssid, params->ssid_len);
+
+	psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
+	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
+
+	ret = rtw_cfg80211_set_auth_type(psecuritypriv, NL80211_AUTHTYPE_OPEN_SYSTEM);
+	rtw_set_802_11_authentication_mode(padapter, psecuritypriv->ndisauthtype);
+
+	if (rtw_set_802_11_ssid(padapter, &ndis_ssid) == false) {
+		ret = -1;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+static int cfg80211_rtw_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct wireless_dev *rtw_wdev = padapter->rtw_wdev;
+	enum nl80211_iftype old_type;
+	int ret = 0;
+
+	old_type = rtw_wdev->iftype;
+
+	rtw_set_to_roam(padapter, 0);
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		rtw_scan_abort(padapter);
+		LeaveAllPowerSaveMode(padapter);
+
+		rtw_wdev->iftype = NL80211_IFTYPE_STATION;
+
+		if (rtw_set_802_11_infrastructure_mode(padapter, Ndis802_11Infrastructure) == false) {
+			rtw_wdev->iftype = old_type;
+			ret = -EPERM;
+			goto leave_ibss;
+		}
+		rtw_setopmode_cmd(padapter, Ndis802_11Infrastructure, true);
+	}
+
+leave_ibss:
+	return ret;
+}
+
+static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
+				 struct cfg80211_connect_params *sme)
+{
+	int ret = 0;
+	enum ndis_802_11_authentication_mode authmode;
+	struct ndis_802_11_ssid ndis_ssid;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	padapter->mlmepriv.not_indic_disco = true;
+
+
+	if (adapter_wdev_data(padapter)->block == true) {
+		ret = -EBUSY;
+		goto exit;
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_JOIN);
+	if (rtw_pwr_wakeup(padapter) == _FAIL) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	if (!sme->ssid || !sme->ssid_len) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (sme->ssid_len > IW_ESSID_MAX_SIZE) {
+
+		ret = -E2BIG;
+		goto exit;
+	}
+
+	memset(&ndis_ssid, 0, sizeof(struct ndis_802_11_ssid));
+	ndis_ssid.ssid_length = sme->ssid_len;
+	memcpy(ndis_ssid.ssid, (u8 *)sme->ssid, sme->ssid_len);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		ret = -EBUSY;
+		goto exit;
+	}
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		rtw_scan_abort(padapter);
+
+	psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
+	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
+
+	ret = rtw_cfg80211_set_wpa_version(psecuritypriv, sme->crypto.wpa_versions);
+	if (ret < 0)
+		goto exit;
+
+	ret = rtw_cfg80211_set_auth_type(psecuritypriv, sme->auth_type);
+
+	if (ret < 0)
+		goto exit;
+
+	ret = rtw_cfg80211_set_wpa_ie(padapter, (u8 *)sme->ie, sme->ie_len);
+	if (ret < 0)
+		goto exit;
+
+	if (sme->crypto.n_ciphers_pairwise) {
+		ret = rtw_cfg80211_set_cipher(psecuritypriv, sme->crypto.ciphers_pairwise[0], true);
+		if (ret < 0)
+			goto exit;
+	}
+
+	/* For WEP Shared auth */
+	if ((psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Shared ||
+	    psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Auto) && sme->key) {
+		u32 wep_key_idx, wep_key_len, wep_total_len;
+		struct ndis_802_11_wep	 *pwep = NULL;
+
+		wep_key_idx = sme->key_idx;
+		wep_key_len = sme->key_len;
+
+		if (sme->key_idx > WEP_KEYS) {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		if (wep_key_len > 0) {
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+			wep_total_len = wep_key_len + FIELD_OFFSET(struct ndis_802_11_wep, key_material);
+			pwep = rtw_malloc(wep_total_len);
+			if (!pwep) {
+				ret = -ENOMEM;
+				goto exit;
+			}
+
+			memset(pwep, 0, wep_total_len);
+
+			pwep->key_length = wep_key_len;
+			pwep->length = wep_total_len;
+
+			if (wep_key_len == 13) {
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+				padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+			}
+		} else {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		pwep->key_index = wep_key_idx;
+		pwep->key_index |= 0x80000000;
+
+		memcpy(pwep->key_material,  (void *)sme->key, pwep->key_length);
+
+		if (rtw_set_802_11_add_wep(padapter, pwep) == (u8)_FAIL)
+			ret = -EOPNOTSUPP;
+
+		kfree(pwep);
+
+		if (ret < 0)
+			goto exit;
+	}
+
+	ret = rtw_cfg80211_set_cipher(psecuritypriv, sme->crypto.cipher_group, false);
+	if (ret < 0)
+		return ret;
+
+	if (sme->crypto.n_akm_suites) {
+		ret = rtw_cfg80211_set_key_mgt(psecuritypriv, sme->crypto.akm_suites[0]);
+		if (ret < 0)
+			goto exit;
+	}
+
+	authmode = psecuritypriv->ndisauthtype;
+	rtw_set_802_11_authentication_mode(padapter, authmode);
+
+	/* rtw_set_802_11_encryption_mode(padapter, padapter->securitypriv.ndisencryptstatus); */
+
+	if (rtw_set_802_11_connect(padapter, (u8 *)sme->bssid, &ndis_ssid) == false) {
+		ret = -1;
+		goto exit;
+	}
+
+exit:
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_JOIN);
+
+	padapter->mlmepriv.not_indic_disco = false;
+
+	return ret;
+}
+
+static int cfg80211_rtw_disconnect(struct wiphy *wiphy, struct net_device *ndev,
+				   u16 reason_code)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+
+	rtw_set_to_roam(padapter, 0);
+
+	rtw_scan_abort(padapter);
+	LeaveAllPowerSaveMode(padapter);
+	rtw_disassoc_cmd(padapter, 500, false);
+
+	rtw_indicate_disconnect(padapter);
+
+	rtw_free_assoc_resources(padapter, 1);
+	rtw_pwr_wakeup(padapter);
+
+	return 0;
+}
+
+static int cfg80211_rtw_set_txpower(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	enum nl80211_tx_power_setting type, int mbm)
+{
+	return 0;
+}
+
+static int cfg80211_rtw_get_txpower(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	int *dbm)
+{
+	*dbm = (12);
+
+	return 0;
+}
+
+inline bool rtw_cfg80211_pwr_mgmt(struct adapter *adapter)
+{
+	struct rtw_wdev_priv *rtw_wdev_priv = adapter_wdev_data(adapter);
+
+	return rtw_wdev_priv->power_mgmt;
+}
+
+static int cfg80211_rtw_set_power_mgmt(struct wiphy *wiphy,
+				       struct net_device *ndev,
+				       bool enabled, int timeout)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct rtw_wdev_priv *rtw_wdev_priv = adapter_wdev_data(padapter);
+
+	rtw_wdev_priv->power_mgmt = enabled;
+
+	if (!enabled)
+		LPS_Leave(padapter, "CFG80211_PWRMGMT");
+
+	return 0;
+}
+
+static int cfg80211_rtw_set_pmksa(struct wiphy *wiphy,
+				  struct net_device *ndev,
+				  struct cfg80211_pmksa *pmksa)
+{
+	u8 index, blInserted = false;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8 strZeroMacAddress[ETH_ALEN] = { 0x00 };
+
+	if (!memcmp((u8 *)pmksa->bssid, strZeroMacAddress, ETH_ALEN))
+		return -EINVAL;
+
+	blInserted = false;
+
+	/* overwrite PMKID */
+	for (index = 0 ; index < NUM_PMKID_CACHE; index++) {
+		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid, (u8 *)pmksa->bssid, ETH_ALEN)) {
+
+			memcpy(psecuritypriv->PMKIDList[index].PMKID, (u8 *)pmksa->pmkid, WLAN_PMKID_LEN);
+			psecuritypriv->PMKIDList[index].bUsed = true;
+			psecuritypriv->PMKIDIndex = index+1;
+			blInserted = true;
+			break;
+		}
+	}
+
+	if (!blInserted) {
+
+		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].Bssid, (u8 *)pmksa->bssid, ETH_ALEN);
+		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].PMKID, (u8 *)pmksa->pmkid, WLAN_PMKID_LEN);
+
+		psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].bUsed = true;
+		psecuritypriv->PMKIDIndex++;
+		if (psecuritypriv->PMKIDIndex == 16)
+			psecuritypriv->PMKIDIndex = 0;
+	}
+
+	return 0;
+}
+
+static int cfg80211_rtw_del_pmksa(struct wiphy *wiphy,
+				  struct net_device *ndev,
+				  struct cfg80211_pmksa *pmksa)
+{
+	u8 index, bMatched = false;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	for (index = 0 ; index < NUM_PMKID_CACHE; index++) {
+		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid, (u8 *)pmksa->bssid, ETH_ALEN)) {
+			/*
+			 * BSSID is matched, the same AP => Remove this PMKID information
+			 * and reset it.
+			 */
+			eth_zero_addr(psecuritypriv->PMKIDList[index].Bssid);
+			memset(psecuritypriv->PMKIDList[index].PMKID, 0x00, WLAN_PMKID_LEN);
+			psecuritypriv->PMKIDList[index].bUsed = false;
+			bMatched = true;
+			break;
+		}
+	}
+
+	if (!bMatched)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int cfg80211_rtw_flush_pmksa(struct wiphy *wiphy,
+				    struct net_device *ndev)
+{
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	memset(&psecuritypriv->PMKIDList[0], 0x00, sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+	psecuritypriv->PMKIDIndex = 0;
+
+	return 0;
+}
+
+void rtw_cfg80211_indicate_sta_assoc(struct adapter *padapter, u8 *pmgmt_frame, uint frame_len)
+{
+	struct net_device *ndev = padapter->pnetdev;
+
+	{
+		struct station_info sinfo = {};
+		u8 ie_offset;
+
+		if (GetFrameSubType(pmgmt_frame) == WIFI_ASSOCREQ)
+			ie_offset = _ASOCREQ_IE_OFFSET_;
+		else /*  WIFI_REASSOCREQ */
+			ie_offset = _REASOCREQ_IE_OFFSET_;
+
+		sinfo.filled = 0;
+		sinfo.assoc_req_ies = pmgmt_frame + WLAN_HDR_A3_LEN + ie_offset;
+		sinfo.assoc_req_ies_len = frame_len - WLAN_HDR_A3_LEN - ie_offset;
+		cfg80211_new_sta(ndev, GetAddr2Ptr(pmgmt_frame), &sinfo, GFP_ATOMIC);
+	}
+}
+
+void rtw_cfg80211_indicate_sta_disassoc(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	struct net_device *ndev = padapter->pnetdev;
+
+	cfg80211_del_sta(ndev, da, GFP_ATOMIC);
+}
+
+static u8 rtw_get_chan_type(struct adapter *adapter)
+{
+	struct mlme_ext_priv *mlme_ext = &adapter->mlmeextpriv;
+
+	switch (mlme_ext->cur_bwmode) {
+	case CHANNEL_WIDTH_20:
+		if (is_supported_ht(adapter->registrypriv.wireless_mode))
+			return NL80211_CHAN_HT20;
+		else
+			return NL80211_CHAN_NO_HT;
+	case CHANNEL_WIDTH_40:
+		if (mlme_ext->cur_ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+			return NL80211_CHAN_HT40PLUS;
+		else
+			return NL80211_CHAN_HT40MINUS;
+	default:
+		return NL80211_CHAN_HT20;
+	}
+
+	return NL80211_CHAN_HT20;
+}
+
+static int cfg80211_rtw_get_channel(struct wiphy *wiphy, struct wireless_dev *wdev,
+				    unsigned int link_id,
+				    struct cfg80211_chan_def *chandef)
+{
+	struct adapter *adapter = wiphy_to_adapter(wiphy);
+	struct registry_priv *registrypriv = &adapter->registrypriv;
+	enum nl80211_channel_type chan_type;
+	struct ieee80211_channel *chan = NULL;
+	int channel;
+	int freq;
+
+	if (!adapter->rtw_wdev)
+		return -ENODEV;
+
+	channel = rtw_get_oper_ch(adapter);
+	if (!channel)
+		return -ENODATA;
+
+	freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+	chan = ieee80211_get_channel(adapter->rtw_wdev->wiphy, freq);
+
+	if (registrypriv->ht_enable) {
+		chan_type = rtw_get_chan_type(adapter);
+		cfg80211_chandef_create(chandef, chan, chan_type);
+	} else {
+		cfg80211_chandef_create(chandef, chan, NL80211_CHAN_NO_HT);
+	}
+
+	return 0;
+}
+
+static netdev_tx_t rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb, struct net_device *ndev)
+{
+	int rtap_len;
+	int qos_len = 0;
+	int dot11_hdr_len = 24;
+	int snap_len = 6;
+	unsigned char *pdata;
+	u16 frame_control;
+	unsigned char src_mac_addr[6];
+	unsigned char dst_mac_addr[6];
+	struct ieee80211_hdr *dot11_hdr;
+	struct ieee80211_radiotap_header *rtap_hdr;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+
+	if (!skb)
+		goto fail;
+
+	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
+		goto fail;
+
+	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
+	if (unlikely(rtap_hdr->it_version))
+		goto fail;
+
+	rtap_len = ieee80211_get_radiotap_len(skb->data);
+	if (unlikely(skb->len < rtap_len))
+		goto fail;
+
+	if (rtap_len != 14)
+		goto fail;
+
+	/* Skip the ratio tap header */
+	skb_pull(skb, rtap_len);
+
+	dot11_hdr = (struct ieee80211_hdr *)skb->data;
+	frame_control = le16_to_cpu(dot11_hdr->frame_control);
+	/* Check if the QoS bit is set */
+	if ((frame_control & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
+		/* Check if this ia a Wireless Distribution System (WDS) frame
+		 * which has 4 MAC addresses
+		 */
+		if (frame_control & 0x0080)
+			qos_len = 2;
+		if ((frame_control & 0x0300) == 0x0300)
+			dot11_hdr_len += 6;
+
+		memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
+		memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
+
+		/* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
+		 * two MAC addresses
+		 */
+		skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
+		pdata = (unsigned char *)skb->data;
+		memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
+		memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
+
+		/* Use the real net device to transmit the packet */
+		_rtw_xmit_entry(skb, padapter->pnetdev);
+		return NETDEV_TX_OK;
+
+	} else if ((frame_control & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) ==
+		   (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)) {
+		/* only for action frames */
+		struct xmit_frame		*pmgntframe;
+		struct pkt_attrib	*pattrib;
+		unsigned char *pframe;
+		/* u8 category, action, OUI_Subtype, dialogToken = 0; */
+		/* unsigned char *frame_body; */
+		struct ieee80211_hdr *pwlanhdr;
+		struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+		struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+		u8 *buf = skb->data;
+		u32 len = skb->len;
+		u8 category, action;
+
+		if (rtw_action_frame_parse(buf, len, &category, &action) == false)
+			goto fail;
+
+		/* starting alloc mgmt frame to dump it */
+		pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+		if (!pmgntframe)
+			goto fail;
+
+		/* update attribute */
+		pattrib = &pmgntframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->retry_ctrl = false;
+
+		memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+		memcpy(pframe, (void *)buf, len);
+		pattrib->pktlen = len;
+
+		pwlanhdr = (struct ieee80211_hdr *)pframe;
+		/* update seq number */
+		pmlmeext->mgnt_seq = GetSequence(pwlanhdr);
+		pattrib->seqnum = pmlmeext->mgnt_seq;
+		pmlmeext->mgnt_seq++;
+
+
+		pattrib->last_txcmdsz = pattrib->pktlen;
+
+		dump_mgntframe(padapter, pmgntframe);
+
+	}
+
+fail:
+
+	dev_kfree_skb_any(skb);
+
+	return NETDEV_TX_OK;
+
+}
+
+
+
+static const struct net_device_ops rtw_cfg80211_monitor_if_ops = {
+	.ndo_start_xmit = rtw_cfg80211_monitor_if_xmit_entry,
+};
+
+static int rtw_cfg80211_add_monitor_if(struct adapter *padapter, char *name, struct net_device **ndev)
+{
+	int ret = 0;
+	struct net_device *mon_ndev = NULL;
+	struct wireless_dev *mon_wdev = NULL;
+	struct rtw_netdev_priv_indicator *pnpi;
+	struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter);
+
+	if (!name) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (pwdev_priv->pmon_ndev) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mon_ndev = alloc_etherdev(sizeof(struct rtw_netdev_priv_indicator));
+	if (!mon_ndev) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mon_ndev->type = ARPHRD_IEEE80211_RADIOTAP;
+	strncpy(mon_ndev->name, name, IFNAMSIZ);
+	mon_ndev->name[IFNAMSIZ - 1] = 0;
+	mon_ndev->needs_free_netdev = true;
+	mon_ndev->priv_destructor = rtw_ndev_destructor;
+
+	mon_ndev->netdev_ops = &rtw_cfg80211_monitor_if_ops;
+
+	pnpi = netdev_priv(mon_ndev);
+	pnpi->priv = padapter;
+	pnpi->sizeof_priv = sizeof(struct adapter);
+
+	/*  wdev */
+	mon_wdev = rtw_zmalloc(sizeof(struct wireless_dev));
+	if (!mon_wdev) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mon_wdev->wiphy = padapter->rtw_wdev->wiphy;
+	mon_wdev->netdev = mon_ndev;
+	mon_wdev->iftype = NL80211_IFTYPE_MONITOR;
+	mon_ndev->ieee80211_ptr = mon_wdev;
+
+	ret = cfg80211_register_netdevice(mon_ndev);
+	if (ret)
+		goto out;
+
+	*ndev = pwdev_priv->pmon_ndev = mon_ndev;
+	memcpy(pwdev_priv->ifname_mon, name, IFNAMSIZ+1);
+
+out:
+	if (ret && mon_wdev) {
+		kfree(mon_wdev);
+		mon_wdev = NULL;
+	}
+
+	if (ret && mon_ndev) {
+		free_netdev(mon_ndev);
+		*ndev = mon_ndev = NULL;
+	}
+
+	return ret;
+}
+
+static struct wireless_dev *
+	cfg80211_rtw_add_virtual_intf(
+		struct wiphy *wiphy,
+		const char *name,
+		unsigned char name_assign_type,
+		enum nl80211_iftype type, struct vif_params *params)
+{
+	int ret = 0;
+	struct net_device *ndev = NULL;
+	struct adapter *padapter = wiphy_to_adapter(wiphy);
+
+	switch (type) {
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_AP_VLAN:
+	case NL80211_IFTYPE_WDS:
+	case NL80211_IFTYPE_MESH_POINT:
+		ret = -ENODEV;
+		break;
+	case NL80211_IFTYPE_MONITOR:
+		ret = rtw_cfg80211_add_monitor_if(padapter, (char *)name, &ndev);
+		break;
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		ret = -ENODEV;
+		break;
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
+		ret = -ENODEV;
+		break;
+	default:
+		ret = -ENODEV;
+		break;
+	}
+
+	return ndev ? ndev->ieee80211_ptr : ERR_PTR(ret);
+}
+
+static int cfg80211_rtw_del_virtual_intf(struct wiphy *wiphy,
+	struct wireless_dev *wdev
+)
+{
+	struct net_device *ndev = wdev_to_ndev(wdev);
+	int ret = 0;
+	struct adapter *adapter;
+	struct rtw_wdev_priv *pwdev_priv;
+
+	if (!ndev) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	adapter = rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(adapter);
+
+	cfg80211_unregister_netdevice(ndev);
+
+	if (ndev == pwdev_priv->pmon_ndev) {
+		pwdev_priv->pmon_ndev = NULL;
+		pwdev_priv->ifname_mon[0] = '\0';
+	}
+
+exit:
+	return ret;
+}
+
+static int rtw_add_beacon(struct adapter *adapter, const u8 *head, size_t head_len, const u8 *tail, size_t tail_len)
+{
+	int ret = 0;
+	u8 *pbuf = NULL;
+	uint len, wps_ielen = 0;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	if (head_len < 24)
+		return -EINVAL;
+
+	pbuf = rtw_zmalloc(head_len+tail_len);
+	if (!pbuf)
+		return -ENOMEM;
+
+	memcpy(pbuf, (void *)head+24, head_len-24);/*  24 =beacon header len. */
+	memcpy(pbuf+head_len-24, (void *)tail, tail_len);
+
+	len = head_len+tail_len-24;
+
+	/* check wps ie if inclued */
+	rtw_get_wps_ie(pbuf + _FIXED_IE_LENGTH_, len - _FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+	/* pbss_network->ies will not include p2p_ie, wfd ie */
+	rtw_ies_remove_ie(pbuf, &len, _BEACON_IE_OFFSET_, WLAN_EID_VENDOR_SPECIFIC, P2P_OUI, 4);
+	rtw_ies_remove_ie(pbuf, &len, _BEACON_IE_OFFSET_, WLAN_EID_VENDOR_SPECIFIC, WFD_OUI, 4);
+
+	if (rtw_check_beacon_data(adapter, pbuf,  len) == _SUCCESS)
+		ret = 0;
+	else
+		ret = -EINVAL;
+
+
+	kfree(pbuf);
+
+	return ret;
+}
+
+static int cfg80211_rtw_start_ap(struct wiphy *wiphy, struct net_device *ndev,
+								struct cfg80211_ap_settings *settings)
+{
+	int ret = 0;
+	struct adapter *adapter = rtw_netdev_priv(ndev);
+
+	ret = rtw_add_beacon(adapter, settings->beacon.head, settings->beacon.head_len,
+		settings->beacon.tail, settings->beacon.tail_len);
+
+	adapter->mlmeextpriv.mlmext_info.hidden_ssid_mode = settings->hidden_ssid;
+
+	if (settings->ssid && settings->ssid_len) {
+		struct wlan_bssid_ex *pbss_network = &adapter->mlmepriv.cur_network.network;
+		struct wlan_bssid_ex *pbss_network_ext = &adapter->mlmeextpriv.mlmext_info.network;
+
+		memcpy(pbss_network->ssid.ssid, (void *)settings->ssid, settings->ssid_len);
+		pbss_network->ssid.ssid_length = settings->ssid_len;
+		memcpy(pbss_network_ext->ssid.ssid, (void *)settings->ssid, settings->ssid_len);
+		pbss_network_ext->ssid.ssid_length = settings->ssid_len;
+	}
+
+	return ret;
+}
+
+static int cfg80211_rtw_change_beacon(struct wiphy *wiphy, struct net_device *ndev,
+		struct cfg80211_beacon_data *info)
+{
+	struct adapter *adapter = rtw_netdev_priv(ndev);
+
+	return rtw_add_beacon(adapter, info->head, info->head_len, info->tail, info->tail_len);
+}
+
+static int cfg80211_rtw_stop_ap(struct wiphy *wiphy, struct net_device *ndev,
+				unsigned int link_id)
+{
+	return 0;
+}
+
+static int	cfg80211_rtw_add_station(struct wiphy *wiphy, struct net_device *ndev,
+				const u8 *mac,
+			struct station_parameters *params)
+{
+	return 0;
+}
+
+static int cfg80211_rtw_del_station(struct wiphy *wiphy, struct net_device *ndev,
+				    struct station_del_parameters *params)
+{
+	int ret = 0;
+	struct list_head *phead, *plist, *tmp;
+	u8 updated = false;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	const u8 *mac = params->mac;
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED | WIFI_AP_STATE)) != true)
+		return -EINVAL;
+
+	if (!mac) {
+		flush_all_cam_entry(padapter);	/* clear CAM */
+
+		rtw_sta_flush(padapter);
+
+		return 0;
+	}
+
+	if (mac[0] == 0xff && mac[1] == 0xff &&
+	    mac[2] == 0xff && mac[3] == 0xff &&
+	    mac[4] == 0xff && mac[5] == 0xff) {
+		return -EINVAL;
+	}
+
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	/* check asoc_queue */
+	list_for_each_safe(plist, tmp, phead) {
+		psta = list_entry(plist, struct sta_info, asoc_list);
+
+		if (!memcmp((u8 *)mac, psta->hwaddr, ETH_ALEN)) {
+			if (psta->dot8021xalg != 1 || psta->bpairwise_key_installed) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+
+				updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+
+				psta = NULL;
+
+				break;
+			}
+
+		}
+
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	associated_clients_update(padapter, updated);
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_change_station(struct wiphy *wiphy, struct net_device *ndev,
+				  const u8 *mac, struct station_parameters *params)
+{
+	return 0;
+}
+
+static struct sta_info *rtw_sta_info_get_by_idx(const int idx, struct sta_priv *pstapriv)
+
+{
+	struct list_head	*phead, *plist;
+	struct sta_info *psta = NULL;
+	int i = 0;
+
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* check asoc_queue */
+	while (phead != plist) {
+		if (idx == i)
+			psta = container_of(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+		i++;
+	}
+	return psta;
+}
+
+static int	cfg80211_rtw_dump_station(struct wiphy *wiphy, struct net_device *ndev,
+			       int idx, u8 *mac, struct station_info *sinfo)
+{
+
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(ndev);
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	psta = rtw_sta_info_get_by_idx(idx, pstapriv);
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+	if (psta == NULL) {
+		ret = -ENOENT;
+		goto exit;
+	}
+	memcpy(mac, psta->hwaddr, ETH_ALEN);
+	sinfo->filled = BIT_ULL(NL80211_STA_INFO_SIGNAL);
+	sinfo->signal = psta->rssi;
+
+exit:
+	return ret;
+}
+
+static int	cfg80211_rtw_change_bss(struct wiphy *wiphy, struct net_device *ndev,
+			      struct bss_parameters *params)
+{
+	return 0;
+}
+
+void rtw_cfg80211_rx_action(struct adapter *adapter, u8 *frame, uint frame_len, const char *msg)
+{
+	s32 freq;
+	int channel;
+	u8 category, action;
+
+	channel = rtw_get_oper_ch(adapter);
+
+	rtw_action_frame_parse(frame, frame_len, &category, &action);
+
+	freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+	rtw_cfg80211_rx_mgmt(adapter, freq, 0, frame, frame_len, GFP_ATOMIC);
+}
+
+static int _cfg80211_rtw_mgmt_tx(struct adapter *padapter, u8 tx_ch, const u8 *buf, size_t len)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char *pframe;
+	int ret = _FAIL;
+	bool __maybe_unused ack = true;
+	struct ieee80211_hdr *pwlanhdr;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	rtw_set_scan_deny(padapter, 1000);
+
+	rtw_scan_abort(padapter);
+	if (tx_ch != rtw_get_oper_ch(padapter)) {
+		if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED))
+			pmlmeext->cur_channel = tx_ch;
+		set_channel_bwmode(padapter, tx_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+	}
+
+	/* starting alloc mgmt frame to dump it */
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe) {
+		/* ret = -ENOMEM; */
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+	memcpy(pframe, (void *)buf, len);
+	pattrib->pktlen = len;
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	/* update seq number */
+	pmlmeext->mgnt_seq = GetSequence(pwlanhdr);
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+	pmlmeext->mgnt_seq++;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (dump_mgntframe_and_wait_ack(padapter, pmgntframe) != _SUCCESS) {
+		ack = false;
+		ret = _FAIL;
+
+	} else {
+		msleep(50);
+
+		ret = _SUCCESS;
+	}
+
+exit:
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_mgmt_tx(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	struct cfg80211_mgmt_tx_params *params,
+	u64 *cookie)
+{
+	struct net_device *ndev = wdev_to_ndev(wdev);
+	struct ieee80211_channel *chan = params->chan;
+	const u8 *buf = params->buf;
+	size_t len = params->len;
+	int ret = 0;
+	int tx_ret;
+	u32 dump_limit = RTW_MAX_MGMT_TX_CNT;
+	u32 dump_cnt = 0;
+	bool ack = true;
+	u8 tx_ch = (u8)ieee80211_frequency_to_channel(chan->center_freq);
+	u8 category, action;
+	int type = (-1);
+	struct adapter *padapter;
+	struct rtw_wdev_priv *pwdev_priv;
+
+	if (!ndev) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	padapter = rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(padapter);
+
+	/* cookie generation */
+	*cookie = (unsigned long) buf;
+
+	/* indicate ack before issue frame to avoid racing with rsp frame */
+	rtw_cfg80211_mgmt_tx_status(padapter, *cookie, buf, len, ack, GFP_KERNEL);
+
+	if (rtw_action_frame_parse(buf, len, &category, &action) == false)
+		goto exit;
+
+	rtw_ps_deny(padapter, PS_DENY_MGNT_TX);
+	if (rtw_pwr_wakeup(padapter) == _FAIL) {
+		ret = -EFAULT;
+		goto cancel_ps_deny;
+	}
+
+	do {
+		dump_cnt++;
+		tx_ret = _cfg80211_rtw_mgmt_tx(padapter, tx_ch, buf, len);
+	} while (dump_cnt < dump_limit && tx_ret != _SUCCESS);
+
+	switch (type) {
+	case P2P_GO_NEGO_CONF:
+		rtw_clear_scan_deny(padapter);
+		break;
+	case P2P_INVIT_RESP:
+		if (pwdev_priv->invit_info.flags & BIT(0) && pwdev_priv->invit_info.status == 0) {
+			rtw_set_scan_deny(padapter, 5000);
+			rtw_pwr_wakeup_ex(padapter, 5000);
+			rtw_clear_scan_deny(padapter);
+		}
+		break;
+	}
+
+cancel_ps_deny:
+	rtw_ps_deny_cancel(padapter, PS_DENY_MGNT_TX);
+exit:
+	return ret;
+}
+
+static void rtw_cfg80211_init_ht_capab(struct ieee80211_sta_ht_cap *ht_cap, enum nl80211_band band)
+{
+
+#define MAX_BIT_RATE_40MHZ_MCS15	300	/* Mbps */
+#define MAX_BIT_RATE_40MHZ_MCS7		150	/* Mbps */
+
+	ht_cap->ht_supported = true;
+
+	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+					IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_SGI_20 |
+					IEEE80211_HT_CAP_DSSSCCK40 | IEEE80211_HT_CAP_MAX_AMSDU;
+
+	/*
+	 *Maximum length of AMPDU that the STA can receive.
+	 *Length = 2 ^ (13 + max_ampdu_length_exp) - 1 (octets)
+	 */
+	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+
+	/*Minimum MPDU start spacing , */
+	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
+
+	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+
+	/*
+	 *hw->wiphy->bands[NL80211_BAND_2GHZ]
+	 *base on ant_num
+	 *rx_mask: RX mask
+	 *if rx_ant = 1 rx_mask[0]= 0xff;==>MCS0-MCS7
+	 *if rx_ant =2 rx_mask[1]= 0xff;==>MCS8-MCS15
+	 *if rx_ant >=3 rx_mask[2]= 0xff;
+	 *if BW_40 rx_mask[4]= 0x01;
+	 *highest supported RX rate
+	 */
+	ht_cap->mcs.rx_mask[0] = 0xFF;
+	ht_cap->mcs.rx_mask[1] = 0x00;
+	ht_cap->mcs.rx_mask[4] = 0x01;
+
+	ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
+}
+
+void rtw_cfg80211_init_wiphy(struct adapter *padapter)
+{
+	struct ieee80211_supported_band *bands;
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = pwdev->wiphy;
+
+	{
+		bands = wiphy->bands[NL80211_BAND_2GHZ];
+		if (bands)
+			rtw_cfg80211_init_ht_capab(&bands->ht_cap, NL80211_BAND_2GHZ);
+	}
+
+	/* copy mac_addr to wiphy */
+	memcpy(wiphy->perm_addr, padapter->eeprompriv.mac_addr, ETH_ALEN);
+
+}
+
+static void rtw_cfg80211_preinit_wiphy(struct adapter *padapter, struct wiphy *wiphy)
+{
+
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+
+	wiphy->max_scan_ssids = RTW_SSID_SCAN_AMOUNT;
+	wiphy->max_scan_ie_len = RTW_SCAN_IE_LEN_MAX;
+	wiphy->max_num_pmkids = RTW_MAX_NUM_PMKIDS;
+
+	wiphy->max_remain_on_channel_duration = RTW_MAX_REMAIN_ON_CHANNEL_DURATION;
+
+	wiphy->interface_modes =	BIT(NL80211_IFTYPE_STATION)
+								| BIT(NL80211_IFTYPE_ADHOC)
+								| BIT(NL80211_IFTYPE_AP)
+								| BIT(NL80211_IFTYPE_MONITOR)
+								;
+
+	wiphy->mgmt_stypes = rtw_cfg80211_default_mgmt_stypes;
+
+	wiphy->software_iftypes |= BIT(NL80211_IFTYPE_MONITOR);
+
+	wiphy->cipher_suites = rtw_cipher_suites;
+	wiphy->n_cipher_suites = ARRAY_SIZE(rtw_cipher_suites);
+
+	/* if (padapter->registrypriv.wireless_mode & WIRELESS_11G) */
+	wiphy->bands[NL80211_BAND_2GHZ] = rtw_spt_band_alloc(NL80211_BAND_2GHZ);
+
+	wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+	wiphy->flags |= WIPHY_FLAG_OFFCHAN_TX | WIPHY_FLAG_HAVE_AP_SME;
+
+#if defined(CONFIG_PM)
+	wiphy->max_sched_scan_reqs = 1;
+#endif
+
+#if defined(CONFIG_PM)
+	wiphy->wowlan = &wowlan_stub;
+#endif
+
+	if (padapter->registrypriv.power_mgnt != PS_MODE_ACTIVE)
+		wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
+	else
+		wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+}
+
+static struct cfg80211_ops rtw_cfg80211_ops = {
+	.change_virtual_intf = cfg80211_rtw_change_iface,
+	.add_key = cfg80211_rtw_add_key,
+	.get_key = cfg80211_rtw_get_key,
+	.del_key = cfg80211_rtw_del_key,
+	.set_default_key = cfg80211_rtw_set_default_key,
+	.get_station = cfg80211_rtw_get_station,
+	.scan = cfg80211_rtw_scan,
+	.set_wiphy_params = cfg80211_rtw_set_wiphy_params,
+	.connect = cfg80211_rtw_connect,
+	.disconnect = cfg80211_rtw_disconnect,
+	.join_ibss = cfg80211_rtw_join_ibss,
+	.leave_ibss = cfg80211_rtw_leave_ibss,
+	.set_tx_power = cfg80211_rtw_set_txpower,
+	.get_tx_power = cfg80211_rtw_get_txpower,
+	.set_power_mgmt = cfg80211_rtw_set_power_mgmt,
+	.set_pmksa = cfg80211_rtw_set_pmksa,
+	.del_pmksa = cfg80211_rtw_del_pmksa,
+	.flush_pmksa = cfg80211_rtw_flush_pmksa,
+	.get_channel = cfg80211_rtw_get_channel,
+	.add_virtual_intf = cfg80211_rtw_add_virtual_intf,
+	.del_virtual_intf = cfg80211_rtw_del_virtual_intf,
+
+	.start_ap = cfg80211_rtw_start_ap,
+	.change_beacon = cfg80211_rtw_change_beacon,
+	.stop_ap = cfg80211_rtw_stop_ap,
+
+	.add_station = cfg80211_rtw_add_station,
+	.del_station = cfg80211_rtw_del_station,
+	.change_station = cfg80211_rtw_change_station,
+	.dump_station = cfg80211_rtw_dump_station,
+	.change_bss = cfg80211_rtw_change_bss,
+
+	.mgmt_tx = cfg80211_rtw_mgmt_tx,
+};
+
+int rtw_wdev_alloc(struct adapter *padapter, struct device *dev)
+{
+	int ret = 0;
+	struct wiphy *wiphy;
+	struct wireless_dev *wdev;
+	struct rtw_wdev_priv *pwdev_priv;
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	/* wiphy */
+	wiphy = wiphy_new(&rtw_cfg80211_ops, sizeof(struct adapter *));
+	if (!wiphy) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+	set_wiphy_dev(wiphy, dev);
+	*((struct adapter **)wiphy_priv(wiphy)) = padapter;
+	rtw_cfg80211_preinit_wiphy(padapter, wiphy);
+
+	/* init regulary domain */
+	rtw_regd_init(wiphy, rtw_reg_notifier);
+
+	ret = wiphy_register(wiphy);
+	if (ret < 0)
+		goto free_wiphy;
+
+	/*  wdev */
+	wdev = rtw_zmalloc(sizeof(struct wireless_dev));
+	if (!wdev) {
+		ret = -ENOMEM;
+		goto unregister_wiphy;
+	}
+	wdev->wiphy = wiphy;
+	wdev->netdev = pnetdev;
+
+	wdev->iftype = NL80211_IFTYPE_STATION; /*  will be init in rtw_hal_init() */
+	                                       /*  Must sync with _rtw_init_mlme_priv() */
+					   /*  pmlmepriv->fw_state = WIFI_STATION_STATE */
+	padapter->rtw_wdev = wdev;
+	pnetdev->ieee80211_ptr = wdev;
+
+	/* init pwdev_priv */
+	pwdev_priv = adapter_wdev_data(padapter);
+	pwdev_priv->rtw_wdev = wdev;
+	pwdev_priv->pmon_ndev = NULL;
+	pwdev_priv->ifname_mon[0] = '\0';
+	pwdev_priv->padapter = padapter;
+	pwdev_priv->scan_request = NULL;
+	spin_lock_init(&pwdev_priv->scan_req_lock);
+
+	pwdev_priv->p2p_enabled = false;
+	pwdev_priv->provdisc_req_issued = false;
+	rtw_wdev_invit_info_init(&pwdev_priv->invit_info);
+	rtw_wdev_nego_info_init(&pwdev_priv->nego_info);
+
+	pwdev_priv->bandroid_scan = false;
+
+	if (padapter->registrypriv.power_mgnt != PS_MODE_ACTIVE)
+		pwdev_priv->power_mgmt = true;
+	else
+		pwdev_priv->power_mgmt = false;
+
+	return ret;
+
+unregister_wiphy:
+	wiphy_unregister(wiphy);
+ free_wiphy:
+	wiphy_free(wiphy);
+exit:
+	return ret;
+
+}
+
+void rtw_wdev_free(struct wireless_dev *wdev)
+{
+	if (!wdev)
+		return;
+
+	kfree(wdev->wiphy->bands[NL80211_BAND_2GHZ]);
+
+	wiphy_free(wdev->wiphy);
+
+	kfree(wdev);
+}
+
+void rtw_wdev_unregister(struct wireless_dev *wdev)
+{
+	struct net_device *ndev;
+	struct adapter *adapter;
+	struct rtw_wdev_priv *pwdev_priv;
+
+	if (!wdev)
+		return;
+	ndev = wdev_to_ndev(wdev);
+	if (!ndev)
+		return;
+
+	adapter = rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(adapter);
+
+	rtw_cfg80211_indicate_scan_done(adapter, true);
+
+	if (pwdev_priv->pmon_ndev)
+		unregister_netdev(pwdev_priv->pmon_ndev);
+
+	wiphy_unregister(wdev->wiphy);
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
new file mode 100644
index 000000000..40a3157fb
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
@@ -0,0 +1,1345 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_mp.h>
+#include <hal_btcoex.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+
+#define RTL_IOCTL_WPA_SUPPLICANT	(SIOCIWFIRSTPRIV+30)
+
+static int wpa_set_auth_algs(struct net_device *dev, u32 value)
+{
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	int ret = 0;
+
+	if ((value & IW_AUTH_ALG_SHARED_KEY) && (value & IW_AUTH_ALG_OPEN_SYSTEM)) {
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeAutoSwitch;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+	} else if (value & IW_AUTH_ALG_SHARED_KEY)	{
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeShared;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Shared;
+	} else if (value & IW_AUTH_ALG_OPEN_SYSTEM) {
+		/* padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled; */
+		if (padapter->securitypriv.ndisauthtype < Ndis802_11AuthModeWPAPSK) {
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		}
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+
+}
+
+static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param, u32 param_len)
+{
+	int ret = 0;
+	u8 max_idx;
+	u32 wep_key_idx, wep_key_len, wep_total_len;
+	struct ndis_802_11_wep	 *pwep = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len < (u32)((u8 *)param->u.crypt.key - (u8 *)param) + param->u.crypt.key_len) {
+		ret =  -EINVAL;
+		goto exit;
+	}
+
+	if (param->sta_addr[0] != 0xff || param->sta_addr[1] != 0xff ||
+	    param->sta_addr[2] != 0xff || param->sta_addr[3] != 0xff ||
+	    param->sta_addr[4] != 0xff || param->sta_addr[5] != 0xff) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0)
+		max_idx = WEP_KEYS - 1;
+	else
+		max_idx = BIP_MAX_KEYID;
+
+	if (param->u.crypt.idx > max_idx) {
+		netdev_err(dev, "Error crypt.idx %d > %d\n", param->u.crypt.idx, max_idx);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		if (wep_key_len > 0) {
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+			wep_total_len = wep_key_len + FIELD_OFFSET(struct ndis_802_11_wep, key_material);
+			/* Allocate a full structure to avoid potentially running off the end. */
+			pwep = kzalloc(sizeof(*pwep), GFP_KERNEL);
+			if (!pwep) {
+				ret = -ENOMEM;
+				goto exit;
+			}
+
+			pwep->key_length = wep_key_len;
+			pwep->length = wep_total_len;
+
+			if (wep_key_len == 13) {
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+				padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+			}
+		} else {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		pwep->key_index = wep_key_idx;
+		pwep->key_index |= 0x80000000;
+
+		memcpy(pwep->key_material,  param->u.crypt.key, pwep->key_length);
+
+		if (param->u.crypt.set_tx) {
+			if (rtw_set_802_11_add_wep(padapter, pwep) == (u8)_FAIL)
+				ret = -EOPNOTSUPP;
+		} else {
+			/* don't update "psecuritypriv->dot11PrivacyAlgrthm" and */
+			/* psecuritypriv->dot11PrivacyKeyIndex =keyid", but can rtw_set_key to fw/cam */
+
+			if (wep_key_idx >= WEP_KEYS) {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			}
+
+			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->key_material, pwep->key_length);
+			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->key_length;
+			rtw_set_key(padapter, psecuritypriv, wep_key_idx, 0, true);
+		}
+
+		goto exit;
+	}
+
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { /*  802_1x */
+		struct sta_info *psta, *pbcmc_sta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_MP_STATE) == true) { /* sta mode */
+			psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+			if (!psta) {
+				/* DEBUG_ERR(("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
+			} else {
+				/* Jeff: don't disable ieee8021x_blocked while clearing key */
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					psta->ieee8021x_blocked = false;
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+						(padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled)) {
+					psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+				}
+
+				if (param->u.crypt.set_tx == 1) { /* pairwise key */
+					memcpy(psta->dot118021x_UncstKey.skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					if (strcmp(param->u.crypt.alg, "TKIP") == 0) { /* set mic key */
+						/* DEBUG_ERR(("\nset key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+						memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
+						memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+
+						padapter->securitypriv.busetkipkey = false;
+						/* _set_timer(&padapter->securitypriv.tkip_timer, 50); */
+					}
+
+					rtw_setstakey_cmd(padapter, psta, true, true);
+				} else { /* group key */
+					if (strcmp(param->u.crypt.alg, "TKIP") == 0 || strcmp(param->u.crypt.alg, "CCMP") == 0) {
+						memcpy(padapter->securitypriv.dot118021XGrpKey[param->u.crypt.idx].skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+						/* only TKIP group key need to install this */
+						if (param->u.crypt.key_len > 16) {
+							memcpy(padapter->securitypriv.dot118021XGrptxmickey[param->u.crypt.idx].skey, &(param->u.crypt.key[16]), 8);
+							memcpy(padapter->securitypriv.dot118021XGrprxmickey[param->u.crypt.idx].skey, &(param->u.crypt.key[24]), 8);
+						}
+						padapter->securitypriv.binstallGrpkey = true;
+
+						padapter->securitypriv.dot118021XGrpKeyid = param->u.crypt.idx;
+
+						rtw_set_key(padapter, &padapter->securitypriv, param->u.crypt.idx, 1, true);
+					} else if (strcmp(param->u.crypt.alg, "BIP") == 0) {
+						/* printk("BIP key_len =%d , index =%d @@@@@@@@@@@@@@@@@@\n", param->u.crypt.key_len, param->u.crypt.idx); */
+						/* save the IGTK key, length 16 bytes */
+						memcpy(padapter->securitypriv.dot11wBIPKey[param->u.crypt.idx].skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+						/*printk("IGTK key below:\n");
+						for (no = 0;no<16;no++)
+							printk(" %02x ", padapter->securitypriv.dot11wBIPKey[param->u.crypt.idx].skey[no]);
+						printk("\n");*/
+						padapter->securitypriv.dot11wBIPKeyid = param->u.crypt.idx;
+						padapter->securitypriv.binstallBIPkey = true;
+					}
+				}
+			}
+
+			pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+			if (!pbcmc_sta) {
+				/* DEBUG_ERR(("Set OID_802_11_ADD_KEY: bcmc stainfo is null\n")); */
+			} else {
+				/* Jeff: don't disable ieee8021x_blocked while clearing key */
+				if (strcmp(param->u.crypt.alg, "none") != 0)
+					pbcmc_sta->ieee8021x_blocked = false;
+
+				if ((padapter->securitypriv.ndisencryptstatus == Ndis802_11Encryption2Enabled) ||
+						(padapter->securitypriv.ndisencryptstatus ==  Ndis802_11Encryption3Enabled)) {
+					pbcmc_sta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+				}
+			}
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			/* adhoc mode */
+		}
+	}
+
+exit:
+
+	kfree(pwep);
+	return ret;
+}
+
+static int rtw_set_wpa_ie(struct adapter *padapter, char *pie, unsigned short ielen)
+{
+	u8 *buf = NULL;
+	int group_cipher = 0, pairwise_cipher = 0;
+	int ret = 0;
+	u8 null_addr[] = {0, 0, 0, 0, 0, 0};
+
+	if (ielen > MAX_WPA_IE_LEN || !pie) {
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		if (!pie)
+			return ret;
+		else
+			return -EINVAL;
+	}
+
+	if (ielen) {
+		buf = rtw_zmalloc(ielen);
+		if (!buf) {
+			ret =  -ENOMEM;
+			goto exit;
+		}
+
+		memcpy(buf, pie, ielen);
+
+		if (ielen < RSN_HEADER_LEN) {
+			ret  = -1;
+			goto exit;
+		}
+
+		if (rtw_parse_wpa_ie(buf, ielen, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPAPSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &buf[0], ielen);
+		}
+
+		if (rtw_parse_wpa2_ie(buf, ielen, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPA2PSK;
+			memcpy(padapter->securitypriv.supplicant_ie, &buf[0], ielen);
+		}
+
+		if (group_cipher == 0)
+			group_cipher = WPA_CIPHER_NONE;
+		if (pairwise_cipher == 0)
+			pairwise_cipher = WPA_CIPHER_NONE;
+
+		switch (group_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.dot118021XGrpPrivacy = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.dot118021XGrpPrivacy = _AES_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		}
+
+		switch (pairwise_cipher) {
+		case WPA_CIPHER_NONE:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
+			break;
+		case WPA_CIPHER_WEP40:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		case WPA_CIPHER_TKIP:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _TKIP_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+			break;
+		case WPA_CIPHER_CCMP:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _AES_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+			break;
+		case WPA_CIPHER_WEP104:
+			padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			break;
+		}
+
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		{/* set wps_ie */
+			u16 cnt = 0;
+			u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+			while (cnt < ielen) {
+				eid = buf[cnt];
+
+				if ((eid == WLAN_EID_VENDOR_SPECIFIC) && (!memcmp(&buf[cnt+2], wps_oui, 4))) {
+					padapter->securitypriv.wps_ie_len = ((buf[cnt+1]+2) < MAX_WPS_IE_LEN) ? (buf[cnt+1]+2):MAX_WPS_IE_LEN;
+
+					memcpy(padapter->securitypriv.wps_ie, &buf[cnt], padapter->securitypriv.wps_ie_len);
+
+					set_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS);
+
+					cnt += buf[cnt+1]+2;
+
+					break;
+				} else {
+					cnt += buf[cnt+1]+2; /* goto next */
+				}
+			}
+		}
+	}
+
+	/* TKIP and AES disallow multicast packets until installing group key */
+	if (padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_ ||
+		padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_WTMIC_ ||
+		padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		/* WPS open need to enable multicast */
+		/*  check_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS) == true) */
+		rtw_hal_set_hwreg(padapter, HW_VAR_OFF_RCR_AM, null_addr);
+
+exit:
+
+	kfree(buf);
+
+	return ret;
+}
+
+static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
+{
+	uint ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+
+	switch (name) {
+	case IEEE_PARAM_WPA_ENABLED:
+
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; /* 802.1x */
+
+		/* ret = ieee80211_wpa_enable(ieee, value); */
+
+		switch ((value)&0xff) {
+		case 1: /* WPA */
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPAPSK; /* WPA_PSK */
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption2Enabled;
+			break;
+		case 2: /* WPA2 */
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeWPA2PSK; /* WPA2_PSK */
+			padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption3Enabled;
+			break;
+		}
+
+		break;
+
+	case IEEE_PARAM_TKIP_COUNTERMEASURES:
+		/* ieee->tkip_countermeasures =value; */
+		break;
+
+	case IEEE_PARAM_DROP_UNENCRYPTED:
+	{
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+		break;
+
+	}
+	case IEEE_PARAM_PRIVACY_INVOKED:
+
+		/* ieee->privacy_invoked =value; */
+
+		break;
+
+	case IEEE_PARAM_AUTH_ALGS:
+
+		ret = wpa_set_auth_algs(dev, value);
+
+		break;
+
+	case IEEE_PARAM_IEEE_802_1X:
+
+		/* ieee->ieee802_1x =value; */
+
+		break;
+
+	case IEEE_PARAM_WPAX_SELECT:
+
+		/*  added for WPA2 mixed mode */
+		/*
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock, flags);
+		ieee->wpax_type_set = 1;
+		ieee->wpax_type_notify = value;
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock, flags);
+		*/
+
+		break;
+
+	default:
+
+
+
+		ret = -EOPNOTSUPP;
+
+
+		break;
+
+	}
+
+	return ret;
+
+}
+
+static int wpa_mlme(struct net_device *dev, u32 command, u32 reason)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+
+	switch (command) {
+	case IEEE_MLME_STA_DEAUTH:
+
+		if (!rtw_set_802_11_disassociate(padapter))
+			ret = -1;
+
+		break;
+
+	case IEEE_MLME_STA_DISASSOC:
+
+		if (!rtw_set_802_11_disassociate(padapter))
+			ret = -1;
+
+		break;
+
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
+
+	return ret;
+
+}
+
+static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
+{
+	struct ieee_param *param;
+	uint ret = 0;
+
+	/* down(&ieee->wx_sem); */
+
+	if (!p->pointer || p->length != sizeof(struct ieee_param))
+		return -EINVAL;
+
+	param = rtw_malloc(p->length);
+	if (!param)
+		return -ENOMEM;
+
+	if (copy_from_user(param, p->pointer, p->length)) {
+		kfree(param);
+		return -EFAULT;
+	}
+
+	switch (param->cmd) {
+
+	case IEEE_CMD_SET_WPA_PARAM:
+		ret = wpa_set_param(dev, param->u.wpa_param.name, param->u.wpa_param.value);
+		break;
+
+	case IEEE_CMD_SET_WPA_IE:
+		/* ret = wpa_set_wpa_ie(dev, param, p->length); */
+		ret =  rtw_set_wpa_ie(rtw_netdev_priv(dev), (char *)param->u.wpa_ie.data, (u16)param->u.wpa_ie.len);
+		break;
+
+	case IEEE_CMD_SET_ENCRYPTION:
+		ret = wpa_set_encryption(dev, param, p->length);
+		break;
+
+	case IEEE_CMD_MLME:
+		ret = wpa_mlme(dev, param->u.mlme.command, param->u.mlme.reason_code);
+		break;
+
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+
+	}
+
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+
+	kfree(param);
+
+	/* up(&ieee->wx_sem); */
+	return ret;
+}
+
+static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param, u32 param_len)
+{
+	int ret = 0;
+	u32 wep_key_idx, wep_key_len, wep_total_len;
+	struct ndis_802_11_wep	 *pwep = NULL;
+	struct sta_info *psta = NULL, *pbcmc_sta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	char *txkey = padapter->securitypriv.dot118021XGrptxmickey[param->u.crypt.idx].skey;
+	char *rxkey = padapter->securitypriv.dot118021XGrprxmickey[param->u.crypt.idx].skey;
+	char *grpkey = psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey;
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	/* sizeof(struct ieee_param) = 64 bytes; */
+	/* if (param_len !=  (u32) ((u8 *) param->u.crypt.key - (u8 *) param) + param->u.crypt.key_len) */
+	if (param_len !=  sizeof(struct ieee_param) + param->u.crypt.key_len) {
+		ret =  -EINVAL;
+		goto exit;
+	}
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		if (param->u.crypt.idx >= WEP_KEYS) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	} else {
+		psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+		if (!psta)
+			/* ret = -EINVAL; */
+			goto exit;
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0 && !psta) {
+		/* todo:clear default encryption keys */
+
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+
+		goto exit;
+	}
+
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0 && !psta) {
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		if ((wep_key_idx >= WEP_KEYS) || (wep_key_len <= 0)) {
+			ret = -EINVAL;
+			goto exit;
+		}
+
+
+		if (wep_key_len > 0) {
+			wep_key_len = wep_key_len <= 5 ? 5 : 13;
+			wep_total_len = wep_key_len + FIELD_OFFSET(struct ndis_802_11_wep, key_material);
+			/* Allocate a full structure to avoid potentially running off the end. */
+			pwep = kzalloc(sizeof(*pwep), GFP_KERNEL);
+			if (!pwep)
+				goto exit;
+
+			pwep->key_length = wep_key_len;
+			pwep->length = wep_total_len;
+
+		}
+
+		pwep->key_index = wep_key_idx;
+
+		memcpy(pwep->key_material,  param->u.crypt.key, pwep->key_length);
+
+		if (param->u.crypt.set_tx) {
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+			psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+
+			if (pwep->key_length == 13) {
+				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+			}
+
+
+			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
+
+			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->key_material, pwep->key_length);
+
+			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->key_length;
+
+			rtw_ap_set_wep_key(padapter, pwep->key_material, pwep->key_length, wep_key_idx, 1);
+		} else {
+			/* don't update "psecuritypriv->dot11PrivacyAlgrthm" and */
+			/* psecuritypriv->dot11PrivacyKeyIndex =keyid", but can rtw_set_key to cam */
+
+			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->key_material, pwep->key_length);
+
+			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->key_length;
+
+			rtw_ap_set_wep_key(padapter, pwep->key_material, pwep->key_length, wep_key_idx, 0);
+		}
+
+		goto exit;
+
+	}
+
+
+	if (!psta && check_fwstate(pmlmepriv, WIFI_AP_STATE)) { /*  group key */
+		if (param->u.crypt.set_tx == 1) {
+			if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+				if (param->u.crypt.key_len == 13)
+						psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+
+			} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+				/* set mic key */
+				memcpy(txkey, &(param->u.crypt.key[16]), 8);
+				memcpy(psecuritypriv->dot118021XGrprxmickey[param->u.crypt.idx].skey, &(param->u.crypt.key[24]), 8);
+
+				psecuritypriv->busetkipkey = true;
+
+			}
+			else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+				memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+			} else {
+				psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+			}
+
+			psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
+
+			psecuritypriv->binstallGrpkey = true;
+
+			psecuritypriv->dot11PrivacyAlgrthm = psecuritypriv->dot118021XGrpPrivacy;/*  */
+
+			rtw_ap_set_group_key(padapter, param->u.crypt.key, psecuritypriv->dot118021XGrpPrivacy, param->u.crypt.idx);
+
+			pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+			if (pbcmc_sta) {
+				pbcmc_sta->ieee8021x_blocked = false;
+				pbcmc_sta->dot118021XPrivacy = psecuritypriv->dot118021XGrpPrivacy;/* rx will use bmc_sta's dot118021XPrivacy */
+			}
+		}
+
+		goto exit;
+
+	}
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X && psta) { /*  psk/802_1x */
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+			if (param->u.crypt.set_tx == 1)	{
+				memcpy(psta->dot118021x_UncstKey.skey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+				if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+					psta->dot118021XPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+						psta->dot118021XPrivacy = _WEP104_;
+				} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+					psta->dot118021XPrivacy = _TKIP_;
+
+					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+					/* set mic key */
+					memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
+					memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+
+					psecuritypriv->busetkipkey = true;
+
+				} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+
+					psta->dot118021XPrivacy = _AES_;
+				} else {
+					psta->dot118021XPrivacy = _NO_PRIVACY_;
+				}
+
+				rtw_ap_set_pairwise_key(padapter, psta);
+
+				psta->ieee8021x_blocked = false;
+
+			} else { /* group key??? */
+				if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+						psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+					psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+
+					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
+					/* set mic key */
+					memcpy(txkey, &(param->u.crypt.key[16]), 8);
+					memcpy(rxkey, &(param->u.crypt.key[24]), 8);
+
+					psecuritypriv->busetkipkey = true;
+
+				} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+					psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+					memcpy(grpkey, param->u.crypt.key, (param->u.crypt.key_len > 16 ? 16 : param->u.crypt.key_len));
+				} else {
+					psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+				}
+
+				psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
+
+				psecuritypriv->binstallGrpkey = true;
+
+				psecuritypriv->dot11PrivacyAlgrthm = psecuritypriv->dot118021XGrpPrivacy;/*  */
+
+				rtw_ap_set_group_key(padapter, param->u.crypt.key, psecuritypriv->dot118021XGrpPrivacy, param->u.crypt.idx);
+
+				pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+				if (pbcmc_sta) {
+					pbcmc_sta->ieee8021x_blocked = false;
+					pbcmc_sta->dot118021XPrivacy = psecuritypriv->dot118021XGrpPrivacy;/* rx will use bmc_sta's dot118021XPrivacy */
+				}
+			}
+		}
+	}
+
+exit:
+	kfree(pwep);
+
+	return ret;
+
+}
+
+static int rtw_set_beacon(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned char *pbuf = param->u.bcn_ie.buf;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	memcpy(&pstapriv->max_num_sta, param->u.bcn_ie.reserved, 2);
+
+	if ((pstapriv->max_num_sta > NUM_STA) || (pstapriv->max_num_sta <= 0))
+		pstapriv->max_num_sta = NUM_STA;
+
+
+	if (rtw_check_beacon_data(padapter, pbuf,  (len-12-2)) == _SUCCESS)/*  12 = param header, 2:no packed */
+		ret = 0;
+	else
+		ret = -EINVAL;
+
+
+	return ret;
+
+}
+
+static void rtw_hostapd_sta_flush(struct net_device *dev)
+{
+	/* _irqL irqL; */
+	/* struct list_head	*phead, *plist; */
+	/* struct sta_info *psta = NULL; */
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	/* struct sta_priv *pstapriv = &padapter->stapriv; */
+
+	flush_all_cam_entry(padapter);	/* clear CAM */
+
+	rtw_sta_flush(padapter);
+}
+
+static int rtw_add_sta(struct net_device *dev, struct ieee_param *param)
+{
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)) != true)
+		return -EINVAL;
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+/*
+	psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+	if (psta)
+	{
+		rtw_free_stainfo(padapter,  psta);
+
+		psta = NULL;
+	}
+*/
+	/* psta = rtw_alloc_stainfo(pstapriv, param->sta_addr); */
+	psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+	if (psta) {
+		int flags = param->u.add_sta.flags;
+
+		psta->aid = param->u.add_sta.aid;/* aid = 1~2007 */
+
+		memcpy(psta->bssrateset, param->u.add_sta.tx_supp_rates, 16);
+
+
+		/* check wmm cap. */
+		if (WLAN_STA_WME&flags)
+			psta->qos_option = 1;
+		else
+			psta->qos_option = 0;
+
+		if (pmlmepriv->qospriv.qos_option == 0)
+			psta->qos_option = 0;
+
+		/* chec 802.11n ht cap. */
+		if (WLAN_STA_HT&flags) {
+			psta->htpriv.ht_option = true;
+			psta->qos_option = 1;
+			memcpy((void *)&psta->htpriv.ht_cap, (void *)&param->u.add_sta.ht_cap, sizeof(struct ieee80211_ht_cap));
+		} else {
+			psta->htpriv.ht_option = false;
+		}
+
+		if (pmlmepriv->htpriv.ht_option == false)
+			psta->htpriv.ht_option = false;
+
+		update_sta_info_apmode(padapter, psta);
+
+
+	} else {
+		ret = -ENOMEM;
+	}
+
+	return ret;
+
+}
+
+static int rtw_del_sta(struct net_device *dev, struct ieee_param *param)
+{
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)) != true)
+		return -EINVAL;
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+	if (psta) {
+		u8 updated = false;
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		if (list_empty(&psta->asoc_list) == false) {
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		associated_clients_update(padapter, updated);
+
+		psta = NULL;
+
+	}
+
+	return ret;
+
+}
+
+static int rtw_ioctl_get_sta_data(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct ieee_param_ex *param_ex = (struct ieee_param_ex *)param;
+	struct sta_data *psta_data = (struct sta_data *)param_ex->data;
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)) != true)
+		return -EINVAL;
+
+	if (param_ex->sta_addr[0] == 0xff && param_ex->sta_addr[1] == 0xff &&
+	    param_ex->sta_addr[2] == 0xff && param_ex->sta_addr[3] == 0xff &&
+	    param_ex->sta_addr[4] == 0xff && param_ex->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, param_ex->sta_addr);
+	if (psta) {
+		psta_data->aid = (u16)psta->aid;
+		psta_data->capability = psta->capability;
+		psta_data->flags = psta->flags;
+
+/*
+		nonerp_set : BIT(0)
+		no_short_slot_time_set : BIT(1)
+		no_short_preamble_set : BIT(2)
+		no_ht_gf_set : BIT(3)
+		no_ht_set : BIT(4)
+		ht_20mhz_set : BIT(5)
+*/
+
+		psta_data->sta_set = ((psta->nonerp_set) |
+							 (psta->no_short_slot_time_set << 1) |
+							 (psta->no_short_preamble_set << 2) |
+							 (psta->no_ht_gf_set << 3) |
+							 (psta->no_ht_set << 4) |
+							 (psta->ht_20mhz_set << 5));
+
+		psta_data->tx_supp_rates_len =  psta->bssratelen;
+		memcpy(psta_data->tx_supp_rates, psta->bssrateset, psta->bssratelen);
+		memcpy(&psta_data->ht_cap, &psta->htpriv.ht_cap, sizeof(struct ieee80211_ht_cap));
+		psta_data->rx_pkts = psta->sta_stats.rx_data_pkts;
+		psta_data->rx_bytes = psta->sta_stats.rx_bytes;
+		psta_data->rx_drops = psta->sta_stats.rx_drops;
+
+		psta_data->tx_pkts = psta->sta_stats.tx_pkts;
+		psta_data->tx_bytes = psta->sta_stats.tx_bytes;
+		psta_data->tx_drops = psta->sta_stats.tx_drops;
+
+
+	} else {
+		ret = -1;
+	}
+
+	return ret;
+
+}
+
+static int rtw_get_sta_wpaie(struct net_device *dev, struct ieee_param *param)
+{
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)) != true)
+		return -EINVAL;
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, param->sta_addr);
+	if (psta) {
+		if ((psta->wpa_ie[0] == WLAN_EID_RSN) || (psta->wpa_ie[0] == WLAN_EID_VENDOR_SPECIFIC)) {
+			int wpa_ie_len;
+			int copy_len;
+
+			wpa_ie_len = psta->wpa_ie[1];
+
+			copy_len = ((wpa_ie_len+2) > sizeof(psta->wpa_ie)) ? (sizeof(psta->wpa_ie)):(wpa_ie_len+2);
+
+			param->u.wpa_ie.len = copy_len;
+
+			memcpy(param->u.wpa_ie.reserved, psta->wpa_ie, copy_len);
+		}
+	} else {
+		ret = -1;
+	}
+
+	return ret;
+
+}
+
+static int rtw_set_wps_beacon(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	unsigned char wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	int ie_len;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	kfree(pmlmepriv->wps_beacon_ie);
+	pmlmepriv->wps_beacon_ie = NULL;
+
+	if (ie_len > 0) {
+		pmlmepriv->wps_beacon_ie = rtw_malloc(ie_len);
+		pmlmepriv->wps_beacon_ie_len = ie_len;
+		if (!pmlmepriv->wps_beacon_ie)
+			return -EINVAL;
+
+		memcpy(pmlmepriv->wps_beacon_ie, param->u.bcn_ie.buf, ie_len);
+
+		update_beacon(padapter, WLAN_EID_VENDOR_SPECIFIC, wps_oui, true);
+
+		pmlmeext->bstart_bss = true;
+	}
+
+
+	return ret;
+
+}
+
+static int rtw_set_wps_probe_resp(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	int ie_len;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	kfree(pmlmepriv->wps_probe_resp_ie);
+	pmlmepriv->wps_probe_resp_ie = NULL;
+
+	if (ie_len > 0) {
+		pmlmepriv->wps_probe_resp_ie = rtw_malloc(ie_len);
+		pmlmepriv->wps_probe_resp_ie_len = ie_len;
+		if (!pmlmepriv->wps_probe_resp_ie)
+			return -EINVAL;
+
+		memcpy(pmlmepriv->wps_probe_resp_ie, param->u.bcn_ie.buf, ie_len);
+	}
+
+
+	return ret;
+
+}
+
+static int rtw_set_wps_assoc_resp(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	int ie_len;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	kfree(pmlmepriv->wps_assoc_resp_ie);
+	pmlmepriv->wps_assoc_resp_ie = NULL;
+
+	if (ie_len > 0) {
+		pmlmepriv->wps_assoc_resp_ie = rtw_malloc(ie_len);
+		pmlmepriv->wps_assoc_resp_ie_len = ie_len;
+		if (!pmlmepriv->wps_assoc_resp_ie)
+			return -EINVAL;
+
+		memcpy(pmlmepriv->wps_assoc_resp_ie, param->u.bcn_ie.buf, ie_len);
+	}
+
+
+	return ret;
+
+}
+
+static int rtw_set_hidden_ssid(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *adapter = rtw_netdev_priv(dev);
+	struct mlme_priv *mlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	struct mlme_ext_info *mlmeinfo = &(mlmeext->mlmext_info);
+	int ie_len;
+	u8 *ssid_ie;
+	char ssid[NDIS_802_11_LENGTH_SSID + 1];
+	signed int ssid_len;
+	u8 ignore_broadcast_ssid;
+
+	if (check_fwstate(mlmepriv, WIFI_AP_STATE) != true)
+		return -EPERM;
+
+	if (param->u.bcn_ie.reserved[0] != 0xea)
+		return -EINVAL;
+
+	mlmeinfo->hidden_ssid_mode = ignore_broadcast_ssid = param->u.bcn_ie.reserved[1];
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+	ssid_ie = rtw_get_ie(param->u.bcn_ie.buf,  WLAN_EID_SSID, &ssid_len, ie_len);
+
+	if (ssid_ie && ssid_len > 0 && ssid_len <= NDIS_802_11_LENGTH_SSID) {
+		struct wlan_bssid_ex *pbss_network = &mlmepriv->cur_network.network;
+		struct wlan_bssid_ex *pbss_network_ext = &mlmeinfo->network;
+
+		memcpy(ssid, ssid_ie+2, ssid_len);
+		ssid[ssid_len] = 0x0;
+
+		memcpy(pbss_network->ssid.ssid, (void *)ssid, ssid_len);
+		pbss_network->ssid.ssid_length = ssid_len;
+		memcpy(pbss_network_ext->ssid.ssid, (void *)ssid, ssid_len);
+		pbss_network_ext->ssid.ssid_length = ssid_len;
+	}
+
+	return ret;
+}
+
+static int rtw_ioctl_acl_remove_sta(struct net_device *dev, struct ieee_param *param, int len)
+{
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+	rtw_acl_remove_sta(padapter, param->sta_addr);
+	return 0;
+
+}
+
+static int rtw_ioctl_acl_add_sta(struct net_device *dev, struct ieee_param *param, int len)
+{
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		return -EINVAL;
+	}
+
+	return rtw_acl_add_sta(padapter, param->sta_addr);
+
+}
+
+static int rtw_ioctl_set_macaddr_acl(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	rtw_set_macaddr_acl(padapter, param->u.mlme.command);
+
+	return ret;
+}
+
+static int rtw_hostapd_ioctl(struct net_device *dev, struct iw_point *p)
+{
+	struct ieee_param *param;
+	int ret = 0;
+	struct adapter *padapter = rtw_netdev_priv(dev);
+
+	/*
+	* this function is expect to call in master mode, which allows no power saving
+	* so, we just check hw_init_completed
+	*/
+
+	if (!padapter->hw_init_completed)
+		return -EPERM;
+
+	if (!p->pointer || p->length != sizeof(*param))
+		return -EINVAL;
+
+	param = rtw_malloc(p->length);
+	if (!param)
+		return -ENOMEM;
+
+	if (copy_from_user(param, p->pointer, p->length)) {
+		kfree(param);
+		return -EFAULT;
+	}
+
+	switch (param->cmd) {
+	case RTL871X_HOSTAPD_FLUSH:
+
+		rtw_hostapd_sta_flush(dev);
+
+		break;
+
+	case RTL871X_HOSTAPD_ADD_STA:
+
+		ret = rtw_add_sta(dev, param);
+
+		break;
+
+	case RTL871X_HOSTAPD_REMOVE_STA:
+
+		ret = rtw_del_sta(dev, param);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_BEACON:
+
+		ret = rtw_set_beacon(dev, param, p->length);
+
+		break;
+
+	case RTL871X_SET_ENCRYPTION:
+
+		ret = rtw_set_encryption(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_GET_WPAIE_STA:
+
+		ret = rtw_get_sta_wpaie(dev, param);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_WPS_BEACON:
+
+		ret = rtw_set_wps_beacon(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_WPS_PROBE_RESP:
+
+		ret = rtw_set_wps_probe_resp(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_WPS_ASSOC_RESP:
+
+		ret = rtw_set_wps_assoc_resp(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_HIDDEN_SSID:
+
+		ret = rtw_set_hidden_ssid(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_GET_INFO_STA:
+
+		ret = rtw_ioctl_get_sta_data(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_SET_MACADDR_ACL:
+
+		ret = rtw_ioctl_set_macaddr_acl(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_ACL_ADD_STA:
+
+		ret = rtw_ioctl_acl_add_sta(dev, param, p->length);
+
+		break;
+
+	case RTL871X_HOSTAPD_ACL_REMOVE_STA:
+
+		ret = rtw_ioctl_acl_remove_sta(dev, param, p->length);
+
+		break;
+
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+
+	}
+
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+
+	kfree(param);
+	return ret;
+}
+
+/*  copy from net/wireless/wext.c end */
+
+int rtw_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	struct iwreq *wrq = (struct iwreq *)rq;
+	int ret = 0;
+
+	switch (cmd) {
+	case RTL_IOCTL_WPA_SUPPLICANT:
+		ret = wpa_supplicant_ioctl(dev, &wrq->u.data);
+		break;
+	case RTL_IOCTL_HOSTAPD:
+		ret = rtw_hostapd_ioctl(dev, &wrq->u.data);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
+
+	return ret;
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/mlme_linux.c b/drivers/staging/rtl8723bs/os_dep/mlme_linux.c
new file mode 100644
index 000000000..1341801e5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/mlme_linux.c
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+static void _dynamic_check_timer_handler(struct timer_list *t)
+{
+	struct adapter *adapter =
+		from_timer(adapter, t, mlmepriv.dynamic_chk_timer);
+
+	rtw_dynamic_check_timer_handler(adapter);
+
+	_set_timer(&adapter->mlmepriv.dynamic_chk_timer, 2000);
+}
+
+static void _rtw_set_scan_deny_timer_hdl(struct timer_list *t)
+{
+	struct adapter *adapter =
+		from_timer(adapter, t, mlmepriv.set_scan_deny_timer);
+
+	rtw_clear_scan_deny(adapter);
+}
+
+void rtw_init_mlme_timer(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	timer_setup(&pmlmepriv->assoc_timer, _rtw_join_timeout_handler, 0);
+	timer_setup(&pmlmepriv->scan_to_timer, rtw_scan_timeout_handler, 0);
+	timer_setup(&pmlmepriv->dynamic_chk_timer,
+		    _dynamic_check_timer_handler, 0);
+	timer_setup(&pmlmepriv->set_scan_deny_timer,
+		    _rtw_set_scan_deny_timer_hdl, 0);
+}
+
+void rtw_os_indicate_connect(struct adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+		rtw_cfg80211_ibss_indicate_connect(adapter);
+	} else {
+		rtw_cfg80211_indicate_connect(adapter);
+	}
+
+	netif_carrier_on(adapter->pnetdev);
+
+	if (adapter->pid[2] != 0)
+		rtw_signal_process(adapter->pid[2], SIGALRM);
+}
+
+void rtw_os_indicate_scan_done(struct adapter *padapter, bool aborted)
+{
+	rtw_cfg80211_indicate_scan_done(padapter, aborted);
+}
+
+static struct rt_pmkid_list   backupPMKIDList[NUM_PMKID_CACHE];
+void rtw_reset_securitypriv(struct adapter *adapter)
+{
+	u8 backupPMKIDIndex = 0;
+	u8 backupTKIPCountermeasure = 0x00;
+	u32 backupTKIPcountermeasure_time = 0;
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+
+	spin_lock_bh(&adapter->security_key_mutex);
+
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+		/* 802.1x */
+		/*  Added by Albert 2009/02/18 */
+		/*  We have to backup the PMK information for WiFi PMK Caching test item. */
+		/*  */
+		/*  Backup the btkip_countermeasure information. */
+		/*  When the countermeasure is trigger, the driver have to disconnect with AP for 60 seconds. */
+
+		memcpy(&backupPMKIDList[0], &adapter->securitypriv.PMKIDList[0], sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+		backupPMKIDIndex = adapter->securitypriv.PMKIDIndex;
+		backupTKIPCountermeasure = adapter->securitypriv.btkip_countermeasure;
+		backupTKIPcountermeasure_time = adapter->securitypriv.btkip_countermeasure_time;
+
+		/* reset RX BIP packet number */
+		pmlmeext->mgnt_80211w_IPN_rx = 0;
+
+		memset((unsigned char *)&adapter->securitypriv, 0, sizeof(struct security_priv));
+
+		/*  Added by Albert 2009/02/18 */
+		/*  Restore the PMK information to securitypriv structure for the following connection. */
+		memcpy(&adapter->securitypriv.PMKIDList[0], &backupPMKIDList[0], sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
+		adapter->securitypriv.PMKIDIndex = backupPMKIDIndex;
+		adapter->securitypriv.btkip_countermeasure = backupTKIPCountermeasure;
+		adapter->securitypriv.btkip_countermeasure_time = backupTKIPcountermeasure_time;
+
+		adapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+		adapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+
+	} else {
+		/* reset values in securitypriv */
+		/* if (adapter->mlmepriv.fw_state & WIFI_STATION_STATE) */
+		/*  */
+		struct security_priv *psec_priv = &adapter->securitypriv;
+
+		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;  /* open system */
+		psec_priv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psec_priv->dot11PrivacyKeyIndex = 0;
+
+		psec_priv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+		psec_priv->dot118021XGrpKeyid = 1;
+
+		psec_priv->ndisauthtype = Ndis802_11AuthModeOpen;
+		psec_priv->ndisencryptstatus = Ndis802_11WEPDisabled;
+		/*  */
+	}
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+	spin_unlock_bh(&adapter->security_key_mutex);
+}
+
+void rtw_os_indicate_disconnect(struct adapter *adapter)
+{
+	/* struct rt_pmkid_list   backupPMKIDList[ NUM_PMKID_CACHE ]; */
+
+	netif_carrier_off(adapter->pnetdev); /*  Do it first for tx broadcast pkt after disconnection issue! */
+
+	rtw_cfg80211_indicate_disconnect(adapter);
+
+	/* modify for CONFIG_IEEE80211W, none 11w also can use the same command */
+	rtw_reset_securitypriv_cmd(adapter);
+}
+
+void rtw_report_sec_ie(struct adapter *adapter, u8 authmode, u8 *sec_ie)
+{
+	uint	len;
+	u8 *buff, *p, i;
+	union iwreq_data wrqu;
+
+	buff = NULL;
+	if (authmode == WLAN_EID_VENDOR_SPECIFIC) {
+		buff = rtw_zmalloc(IW_CUSTOM_MAX);
+		if (!buff)
+			return;
+
+		p = buff;
+
+		p += scnprintf(p, IW_CUSTOM_MAX - (p - buff), "ASSOCINFO(ReqIEs =");
+
+		len = sec_ie[1] + 2;
+		len = (len < IW_CUSTOM_MAX) ? len : IW_CUSTOM_MAX;
+
+		for (i = 0; i < len; i++)
+			p += scnprintf(p, IW_CUSTOM_MAX - (p - buff), "%02x", sec_ie[i]);
+
+		p += scnprintf(p, IW_CUSTOM_MAX - (p - buff), ")");
+
+		memset(&wrqu, 0, sizeof(wrqu));
+
+		wrqu.data.length = p - buff;
+
+		wrqu.data.length = (wrqu.data.length < IW_CUSTOM_MAX) ? wrqu.data.length : IW_CUSTOM_MAX;
+
+		kfree(buff);
+	}
+}
+
+void init_addba_retry_timer(struct adapter *padapter, struct sta_info *psta)
+{
+	timer_setup(&psta->addba_retry_timer, addba_timer_hdl, 0);
+}
+
+void init_mlme_ext_timer(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	timer_setup(&pmlmeext->survey_timer, survey_timer_hdl, 0);
+	timer_setup(&pmlmeext->link_timer, link_timer_hdl, 0);
+	timer_setup(&pmlmeext->sa_query_timer, sa_query_timer_hdl, 0);
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/os_intfs.c b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
new file mode 100644
index 000000000..68bba3c0e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
@@ -0,0 +1,1254 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek Wireless Lan Driver");
+MODULE_AUTHOR("Realtek Semiconductor Corp.");
+MODULE_VERSION(DRIVERVERSION);
+
+/* module param defaults */
+static int rtw_chip_version;
+static int rtw_rfintfs = HWPI;
+static int rtw_lbkmode;/* RTL8712_AIR_TRX; */
+
+
+static int rtw_network_mode = Ndis802_11IBSS;/* Ndis802_11Infrastructure;infra, ad-hoc, auto */
+/* struct ndis_802_11_ssid	ssid; */
+static int rtw_channel = 1;/* ad-hoc support requirement */
+static int rtw_wireless_mode = WIRELESS_11BG_24N;
+static int rtw_vrtl_carrier_sense = AUTO_VCS;
+static int rtw_vcs_type = RTS_CTS;/*  */
+static int rtw_rts_thresh = 2347;/*  */
+static int rtw_frag_thresh = 2346;/*  */
+static int rtw_preamble = PREAMBLE_LONG;/* long, short, auto */
+static int rtw_scan_mode = 1;/* active, passive */
+static int rtw_adhoc_tx_pwr = 1;
+static int rtw_soft_ap;
+/* int smart_ps = 1; */
+static int rtw_power_mgnt = 1;
+static int rtw_ips_mode = IPS_NORMAL;
+module_param(rtw_ips_mode, int, 0644);
+MODULE_PARM_DESC(rtw_ips_mode, "The default IPS mode");
+
+static int rtw_smart_ps = 2;
+
+static int rtw_check_fw_ps = 1;
+
+static int rtw_usb_rxagg_mode = 2;/* USB_RX_AGG_DMA = 1, USB_RX_AGG_USB =2 */
+module_param(rtw_usb_rxagg_mode, int, 0644);
+
+static int rtw_radio_enable = 1;
+static int rtw_long_retry_lmt = 7;
+static int rtw_short_retry_lmt = 7;
+static int rtw_busy_thresh = 40;
+/* int qos_enable = 0; */
+static int rtw_ack_policy = NORMAL_ACK;
+
+static int rtw_software_encrypt;
+static int rtw_software_decrypt;
+
+static int rtw_acm_method;/*  0:By SW 1:By HW. */
+
+static int rtw_wmm_enable = 1;/*  default is set to enable the wmm. */
+static int rtw_uapsd_enable;
+static int rtw_uapsd_max_sp = NO_LIMIT;
+static int rtw_uapsd_acbk_en;
+static int rtw_uapsd_acbe_en;
+static int rtw_uapsd_acvi_en;
+static int rtw_uapsd_acvo_en;
+
+int rtw_ht_enable = 1;
+/*
+ * 0: 20 MHz, 1: 40 MHz
+ * 2.4G use bit 0 ~ 3
+ * 0x01 means enable 2.4G 40MHz
+ */
+static int rtw_bw_mode = 0x01;
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu ,0: disable, 0x1:enable (but wifi_spec should be 0), 0x2: force enable (don't care wifi_spec) */
+static int rtw_rx_stbc = 1;/*  0: disable, 1:enable 2.4g */
+static int rtw_ampdu_amsdu;/*  0: disabled, 1:enabled, 2:auto . There is an IOT issu with DLINK DIR-629 when the flag turn on */
+/*  Short GI support Bit Map */
+/*  BIT0 - 20MHz, 0: non-support, 1: support */
+/*  BIT1 - 40MHz, 0: non-support, 1: support */
+/*  BIT2 - 80MHz, 0: non-support, 1: support */
+/*  BIT3 - 160MHz, 0: non-support, 1: support */
+static int rtw_short_gi = 0xf;
+/*  BIT0: Enable VHT LDPC Rx, BIT1: Enable VHT LDPC Tx, BIT4: Enable HT LDPC Rx, BIT5: Enable HT LDPC Tx */
+static int rtw_ldpc_cap = 0x33;
+/*  BIT0: Enable VHT STBC Rx, BIT1: Enable VHT STBC Tx, BIT4: Enable HT STBC Rx, BIT5: Enable HT STBC Tx */
+static int rtw_stbc_cap = 0x13;
+/*  BIT0: Enable VHT Beamformer, BIT1: Enable VHT Beamformee, BIT4: Enable HT Beamformer, BIT5: Enable HT Beamformee */
+static int rtw_beamform_cap = 0x2;
+
+static int rtw_lowrate_two_xmit = 1;/* Use 2 path Tx to transmit MCS0~7 and legacy mode */
+
+static int rtw_low_power;
+static int rtw_wifi_spec;
+static int rtw_channel_plan = RT_CHANNEL_DOMAIN_MAX;
+
+static int rtw_ant_num = -1; /*  <0: undefined, >0: Antenna number */
+module_param(rtw_ant_num, int, 0644);
+MODULE_PARM_DESC(rtw_ant_num, "Antenna number setting");
+
+static int rtw_antdiv_cfg = 1; /*  0:OFF , 1:ON, 2:decide by Efuse config */
+static int rtw_antdiv_type; /* 0:decide by efuse  1: for 88EE, 1Tx and 1RxCG are diversity.(2 Ant with SPDT), 2:  for 88EE, 1Tx and 2Rx are diversity.(2 Ant, Tx and RxCG are both on aux port, RxCS is on main port), 3: for 88EE, 1Tx and 1RxCG are fixed.(1Ant, Tx and RxCG are both on aux port) */
+
+
+
+static int rtw_hw_wps_pbc;
+
+int rtw_mc2u_disable;
+
+static int rtw_80211d;
+
+static int rtw_qos_opt_enable;/* 0: disable, 1:enable */
+module_param(rtw_qos_opt_enable, int, 0644);
+
+static char *ifname = "wlan%d";
+module_param(ifname, charp, 0644);
+MODULE_PARM_DESC(ifname, "The default name to allocate for first interface");
+
+char *rtw_initmac;  /*  temp mac address if users want to use instead of the mac address in Efuse */
+
+module_param(rtw_initmac, charp, 0644);
+module_param(rtw_channel_plan, int, 0644);
+module_param(rtw_chip_version, int, 0644);
+module_param(rtw_rfintfs, int, 0644);
+module_param(rtw_lbkmode, int, 0644);
+module_param(rtw_network_mode, int, 0644);
+module_param(rtw_channel, int, 0644);
+module_param(rtw_wmm_enable, int, 0644);
+module_param(rtw_vrtl_carrier_sense, int, 0644);
+module_param(rtw_vcs_type, int, 0644);
+module_param(rtw_busy_thresh, int, 0644);
+
+module_param(rtw_ht_enable, int, 0644);
+module_param(rtw_bw_mode, int, 0644);
+module_param(rtw_ampdu_enable, int, 0644);
+module_param(rtw_rx_stbc, int, 0644);
+module_param(rtw_ampdu_amsdu, int, 0644);
+
+module_param(rtw_lowrate_two_xmit, int, 0644);
+
+module_param(rtw_power_mgnt, int, 0644);
+module_param(rtw_smart_ps, int, 0644);
+module_param(rtw_low_power, int, 0644);
+module_param(rtw_wifi_spec, int, 0644);
+
+module_param(rtw_antdiv_cfg, int, 0644);
+module_param(rtw_antdiv_type, int, 0644);
+
+
+module_param(rtw_hw_wps_pbc, int, 0644);
+
+static uint rtw_max_roaming_times = 2;
+module_param(rtw_max_roaming_times, uint, 0644);
+MODULE_PARM_DESC(rtw_max_roaming_times, "The max roaming times to try");
+
+module_param(rtw_mc2u_disable, int, 0644);
+
+module_param(rtw_80211d, int, 0644);
+MODULE_PARM_DESC(rtw_80211d, "Enable 802.11d mechanism");
+
+static uint rtw_notch_filter;
+module_param(rtw_notch_filter, uint, 0644);
+MODULE_PARM_DESC(rtw_notch_filter, "0:Disable, 1:Enable, 2:Enable only for P2P");
+
+#define CONFIG_RTW_HIQ_FILTER 1
+
+static uint rtw_hiq_filter = CONFIG_RTW_HIQ_FILTER;
+module_param(rtw_hiq_filter, uint, 0644);
+MODULE_PARM_DESC(rtw_hiq_filter, "0:allow all, 1:allow special, 2:deny all");
+
+static int rtw_tx_pwr_lmt_enable;
+static int rtw_tx_pwr_by_rate;
+
+module_param(rtw_tx_pwr_lmt_enable, int, 0644);
+MODULE_PARM_DESC(rtw_tx_pwr_lmt_enable, "0:Disable, 1:Enable, 2: Depend on efuse");
+
+module_param(rtw_tx_pwr_by_rate, int, 0644);
+MODULE_PARM_DESC(rtw_tx_pwr_by_rate, "0:Disable, 1:Enable, 2: Depend on efuse");
+
+static int netdev_close(struct net_device *pnetdev);
+
+static void loadparam(struct adapter *padapter, struct net_device *pnetdev)
+{
+	struct registry_priv  *registry_par = &padapter->registrypriv;
+
+	registry_par->chip_version = (u8)rtw_chip_version;
+	registry_par->rfintfs = (u8)rtw_rfintfs;
+	registry_par->lbkmode = (u8)rtw_lbkmode;
+	/* registry_par->hci = (u8)hci; */
+	registry_par->network_mode  = (u8)rtw_network_mode;
+
+	memcpy(registry_par->ssid.ssid, "ANY", 3);
+	registry_par->ssid.ssid_length = 3;
+
+	registry_par->channel = (u8)rtw_channel;
+	registry_par->wireless_mode = (u8)rtw_wireless_mode;
+
+	registry_par->vrtl_carrier_sense = (u8)rtw_vrtl_carrier_sense;
+	registry_par->vcs_type = (u8)rtw_vcs_type;
+	registry_par->rts_thresh = (u16)rtw_rts_thresh;
+	registry_par->frag_thresh = (u16)rtw_frag_thresh;
+	registry_par->preamble = (u8)rtw_preamble;
+	registry_par->scan_mode = (u8)rtw_scan_mode;
+	registry_par->adhoc_tx_pwr = (u8)rtw_adhoc_tx_pwr;
+	registry_par->soft_ap =  (u8)rtw_soft_ap;
+	registry_par->smart_ps =  (u8)rtw_smart_ps;
+	registry_par->check_fw_ps = (u8)rtw_check_fw_ps;
+	registry_par->power_mgnt = (u8)rtw_power_mgnt;
+	registry_par->ips_mode = (u8)rtw_ips_mode;
+	registry_par->radio_enable = (u8)rtw_radio_enable;
+	registry_par->long_retry_lmt = (u8)rtw_long_retry_lmt;
+	registry_par->short_retry_lmt = (u8)rtw_short_retry_lmt;
+	registry_par->busy_thresh = (u16)rtw_busy_thresh;
+	/* registry_par->qos_enable = (u8)rtw_qos_enable; */
+	registry_par->ack_policy = (u8)rtw_ack_policy;
+	registry_par->software_encrypt = (u8)rtw_software_encrypt;
+	registry_par->software_decrypt = (u8)rtw_software_decrypt;
+
+	registry_par->acm_method = (u8)rtw_acm_method;
+	registry_par->usb_rxagg_mode = (u8)rtw_usb_rxagg_mode;
+
+	 /* UAPSD */
+	registry_par->wmm_enable = (u8)rtw_wmm_enable;
+	registry_par->uapsd_enable = (u8)rtw_uapsd_enable;
+	registry_par->uapsd_max_sp = (u8)rtw_uapsd_max_sp;
+	registry_par->uapsd_acbk_en = (u8)rtw_uapsd_acbk_en;
+	registry_par->uapsd_acbe_en = (u8)rtw_uapsd_acbe_en;
+	registry_par->uapsd_acvi_en = (u8)rtw_uapsd_acvi_en;
+	registry_par->uapsd_acvo_en = (u8)rtw_uapsd_acvo_en;
+
+	registry_par->ht_enable = (u8)rtw_ht_enable;
+	registry_par->bw_mode = (u8)rtw_bw_mode;
+	registry_par->ampdu_enable = (u8)rtw_ampdu_enable;
+	registry_par->rx_stbc = (u8)rtw_rx_stbc;
+	registry_par->ampdu_amsdu = (u8)rtw_ampdu_amsdu;
+	registry_par->short_gi = (u8)rtw_short_gi;
+	registry_par->ldpc_cap = (u8)rtw_ldpc_cap;
+	registry_par->stbc_cap = (u8)rtw_stbc_cap;
+	registry_par->beamform_cap = (u8)rtw_beamform_cap;
+
+	registry_par->lowrate_two_xmit = (u8)rtw_lowrate_two_xmit;
+	registry_par->low_power = (u8)rtw_low_power;
+
+
+	registry_par->wifi_spec = (u8)rtw_wifi_spec;
+
+	registry_par->channel_plan = (u8)rtw_channel_plan;
+
+	registry_par->ant_num = (s8)rtw_ant_num;
+
+	registry_par->accept_addba_req = true;
+
+	registry_par->antdiv_cfg = (u8)rtw_antdiv_cfg;
+	registry_par->antdiv_type = (u8)rtw_antdiv_type;
+
+	registry_par->hw_wps_pbc = (u8)rtw_hw_wps_pbc;
+
+	registry_par->max_roaming_times = (u8)rtw_max_roaming_times;
+
+	registry_par->enable80211d = (u8)rtw_80211d;
+
+	snprintf(registry_par->ifname, 16, "%s", ifname);
+
+	registry_par->notch_filter = (u8)rtw_notch_filter;
+
+	registry_par->RegEnableTxPowerLimit = (u8)rtw_tx_pwr_lmt_enable;
+	registry_par->RegEnableTxPowerByRate = (u8)rtw_tx_pwr_by_rate;
+
+	registry_par->RegPowerBase = 14;
+	registry_par->TxBBSwing_2G = 0xFF;
+	registry_par->bEn_RFE = 1;
+	registry_par->RFE_Type = 64;
+
+	registry_par->qos_opt_enable = (u8)rtw_qos_opt_enable;
+
+	registry_par->hiq_filter = (u8)rtw_hiq_filter;
+}
+
+static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
+{
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct sockaddr *addr = p;
+
+	if (!padapter->bup) {
+		/* addr->sa_data[4], addr->sa_data[5]); */
+		memcpy(padapter->eeprompriv.mac_addr, addr->sa_data, ETH_ALEN);
+		/* eth_hw_addr_set(pnetdev, addr->sa_data); */
+		/* padapter->bset_hwaddr = true; */
+	}
+
+	return 0;
+}
+
+static struct net_device_stats *rtw_net_get_stats(struct net_device *pnetdev)
+{
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct recv_priv *precvpriv = &(padapter->recvpriv);
+
+	padapter->stats.tx_packets = pxmitpriv->tx_pkts;/* pxmitpriv->tx_pkts++; */
+	padapter->stats.rx_packets = precvpriv->rx_pkts;/* precvpriv->rx_pkts++; */
+	padapter->stats.tx_dropped = pxmitpriv->tx_drop;
+	padapter->stats.rx_dropped = precvpriv->rx_drop;
+	padapter->stats.tx_bytes = pxmitpriv->tx_bytes;
+	padapter->stats.rx_bytes = precvpriv->rx_bytes;
+
+	return &padapter->stats;
+}
+
+/*
+ * AC to queue mapping
+ *
+ * AC_VO -> queue 0
+ * AC_VI -> queue 1
+ * AC_BE -> queue 2
+ * AC_BK -> queue 3
+ */
+static const u16 rtw_1d_to_queue[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
+
+/* Given a data frame determine the 802.1p/1d tag to use. */
+static unsigned int rtw_classify8021d(struct sk_buff *skb)
+{
+	unsigned int dscp;
+
+	/* skb->priority values from 256->263 are magic values to
+	 * directly indicate a specific 802.1d priority.  This is used
+	 * to allow 802.1d priority to be passed directly in from VLAN
+	 * tags, etc.
+	 */
+	if (skb->priority >= 256 && skb->priority <= 263)
+		return skb->priority - 256;
+
+	switch (skb->protocol) {
+	case htons(ETH_P_IP):
+		dscp = ip_hdr(skb)->tos & 0xfc;
+		break;
+	default:
+		return 0;
+	}
+
+	return dscp >> 5;
+}
+
+
+static u16 rtw_select_queue(struct net_device *dev, struct sk_buff *skb,
+			    struct net_device *sb_dev)
+{
+	struct adapter	*padapter = rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	skb->priority = rtw_classify8021d(skb);
+
+	if (pmlmepriv->acm_mask != 0)
+		skb->priority = qos_acm(pmlmepriv->acm_mask, skb->priority);
+
+	return rtw_1d_to_queue[skb->priority];
+}
+
+u16 rtw_recv_select_queue(struct sk_buff *skb)
+{
+	struct iphdr *piphdr;
+	unsigned int dscp;
+	__be16	eth_type;
+	u32 priority;
+	u8 *pdata = skb->data;
+
+	memcpy(&eth_type, pdata + (ETH_ALEN << 1), 2);
+
+	switch (be16_to_cpu(eth_type)) {
+	case ETH_P_IP:
+
+		piphdr = (struct iphdr *)(pdata + ETH_HLEN);
+
+		dscp = piphdr->tos & 0xfc;
+
+		priority = dscp >> 5;
+
+		break;
+	default:
+		priority = 0;
+	}
+
+	return rtw_1d_to_queue[priority];
+}
+
+static int rtw_ndev_notifier_call(struct notifier_block *nb, unsigned long state, void *ptr)
+{
+	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
+
+	if (dev->netdev_ops->ndo_do_ioctl != rtw_ioctl)
+		return NOTIFY_DONE;
+
+	netdev_dbg(dev, FUNC_NDEV_FMT " state:%lu\n", FUNC_NDEV_ARG(dev),
+		    state);
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block rtw_ndev_notifier = {
+	.notifier_call = rtw_ndev_notifier_call,
+};
+
+int rtw_ndev_notifier_register(void)
+{
+	return register_netdevice_notifier(&rtw_ndev_notifier);
+}
+
+void rtw_ndev_notifier_unregister(void)
+{
+	unregister_netdevice_notifier(&rtw_ndev_notifier);
+}
+
+
+static int rtw_ndev_init(struct net_device *dev)
+{
+	struct adapter *adapter = rtw_netdev_priv(dev);
+
+	netdev_dbg(dev, FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(adapter));
+	strncpy(adapter->old_ifname, dev->name, IFNAMSIZ);
+
+	return 0;
+}
+
+static void rtw_ndev_uninit(struct net_device *dev)
+{
+	struct adapter *adapter = rtw_netdev_priv(dev);
+
+	netdev_dbg(dev, FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(adapter));
+}
+
+static const struct net_device_ops rtw_netdev_ops = {
+	.ndo_init = rtw_ndev_init,
+	.ndo_uninit = rtw_ndev_uninit,
+	.ndo_open = netdev_open,
+	.ndo_stop = netdev_close,
+	.ndo_start_xmit = rtw_xmit_entry,
+	.ndo_select_queue	= rtw_select_queue,
+	.ndo_set_mac_address = rtw_net_set_mac_address,
+	.ndo_get_stats = rtw_net_get_stats,
+	.ndo_do_ioctl = rtw_ioctl,
+};
+
+int rtw_init_netdev_name(struct net_device *pnetdev, const char *ifname)
+{
+	if (dev_alloc_name(pnetdev, ifname) < 0) {
+		pr_err("dev_alloc_name, fail for %s\n", ifname);
+		return 1;
+	}
+	netif_carrier_off(pnetdev);
+	/* rtw_netif_stop_queue(pnetdev); */
+
+	return 0;
+}
+
+struct net_device *rtw_init_netdev(struct adapter *old_padapter)
+{
+	struct adapter *padapter;
+	struct net_device *pnetdev;
+
+	if (old_padapter)
+		pnetdev = rtw_alloc_etherdev_with_old_priv(sizeof(struct adapter), (void *)old_padapter);
+	else
+		pnetdev = rtw_alloc_etherdev(sizeof(struct adapter));
+
+	pr_info("pnetdev = %p\n", pnetdev);
+	if (!pnetdev)
+		return NULL;
+
+	padapter = rtw_netdev_priv(pnetdev);
+	padapter->pnetdev = pnetdev;
+
+	/* pnetdev->init = NULL; */
+
+	pnetdev->netdev_ops = &rtw_netdev_ops;
+
+	/* pnetdev->tx_timeout = NULL; */
+	pnetdev->watchdog_timeo = HZ * 3; /* 3 second timeout */
+
+	/* step 2. */
+	loadparam(padapter, pnetdev);
+
+	return pnetdev;
+}
+
+void rtw_unregister_netdevs(struct dvobj_priv *dvobj)
+{
+	struct adapter *padapter = NULL;
+	struct net_device *pnetdev = NULL;
+
+	padapter = dvobj->padapters;
+
+	if (!padapter)
+		return;
+
+	pnetdev = padapter->pnetdev;
+
+	if ((padapter->DriverState != DRIVER_DISAPPEAR) && pnetdev)
+		unregister_netdev(pnetdev); /* will call netdev_close() */
+	rtw_wdev_unregister(padapter->rtw_wdev);
+}
+
+u32 rtw_start_drv_threads(struct adapter *padapter)
+{
+	u32 _status = _SUCCESS;
+
+	padapter->xmitThread = kthread_run(rtw_xmit_thread, padapter, "RTW_XMIT_THREAD");
+	if (IS_ERR(padapter->xmitThread))
+		_status = _FAIL;
+
+	padapter->cmdThread = kthread_run(rtw_cmd_thread, padapter, "RTW_CMD_THREAD");
+	if (IS_ERR(padapter->cmdThread))
+		_status = _FAIL;
+	else
+		wait_for_completion(&padapter->cmdpriv.terminate_cmdthread_comp); /* wait for cmd_thread to run */
+
+	rtw_hal_start_thread(padapter);
+	return _status;
+}
+
+void rtw_stop_drv_threads(struct adapter *padapter)
+{
+	rtw_stop_cmd_thread(padapter);
+
+	/*  Below is to termindate tx_thread... */
+	complete(&padapter->xmitpriv.xmit_comp);
+	wait_for_completion(&padapter->xmitpriv.terminate_xmitthread_comp);
+
+	rtw_hal_stop_thread(padapter);
+}
+
+static void rtw_init_default_value(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	/* xmit_priv */
+	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
+	pxmitpriv->vcs = pregistrypriv->vcs_type;
+	pxmitpriv->vcs_type = pregistrypriv->vcs_type;
+	/* pxmitpriv->rts_thresh = pregistrypriv->rts_thresh; */
+	pxmitpriv->frag_len = pregistrypriv->frag_thresh;
+
+	/* recv_priv */
+
+	/* mlme_priv */
+	pmlmepriv->scan_mode = SCAN_ACTIVE;
+
+	/* qos_priv */
+	/* pmlmepriv->qospriv.qos_option = pregistrypriv->wmm_enable; */
+
+	/* ht_priv */
+	pmlmepriv->htpriv.ampdu_enable = false;/* set to disabled */
+
+	/* security_priv */
+	/* rtw_get_encrypt_decrypt_from_registrypriv(padapter); */
+	psecuritypriv->binstallGrpkey = _FAIL;
+	psecuritypriv->sw_encrypt = pregistrypriv->software_encrypt;
+	psecuritypriv->sw_decrypt = pregistrypriv->software_decrypt;
+
+	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+
+	psecuritypriv->dot11PrivacyKeyIndex = 0;
+
+	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpKeyid = 1;
+
+	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
+	psecuritypriv->ndisencryptstatus = Ndis802_11WEPDisabled;
+
+	/* registry_priv */
+	rtw_init_registrypriv_dev_network(padapter);
+	rtw_update_registrypriv_dev_network(padapter);
+
+	/* hal_priv */
+	rtw_hal_def_value_init(padapter);
+
+	/* misc. */
+	RTW_ENABLE_FUNC(padapter, DF_RX_BIT);
+	RTW_ENABLE_FUNC(padapter, DF_TX_BIT);
+	padapter->bLinkInfoDump = 0;
+	padapter->bNotifyChannelChange = 0;
+
+	/* for debug purpose */
+	padapter->fix_rate = 0xFF;
+	padapter->driver_ampdu_spacing = 0xFF;
+	padapter->driver_rx_ampdu_factor =  0xFF;
+
+}
+
+struct dvobj_priv *devobj_init(void)
+{
+	struct dvobj_priv *pdvobj = NULL;
+
+	pdvobj = rtw_zmalloc(sizeof(*pdvobj));
+	if (!pdvobj)
+		return NULL;
+
+	mutex_init(&pdvobj->hw_init_mutex);
+	mutex_init(&pdvobj->h2c_fwcmd_mutex);
+	mutex_init(&pdvobj->setch_mutex);
+	mutex_init(&pdvobj->setbw_mutex);
+
+	spin_lock_init(&pdvobj->lock);
+
+	pdvobj->macid[1] = true; /* macid = 1 for bc/mc stainfo */
+
+	pdvobj->processing_dev_remove = false;
+
+	atomic_set(&pdvobj->disable_func, 0);
+
+	spin_lock_init(&pdvobj->cam_ctl.lock);
+
+	return pdvobj;
+}
+
+void devobj_deinit(struct dvobj_priv *pdvobj)
+{
+	if (!pdvobj)
+		return;
+
+	mutex_destroy(&pdvobj->hw_init_mutex);
+	mutex_destroy(&pdvobj->h2c_fwcmd_mutex);
+	mutex_destroy(&pdvobj->setch_mutex);
+	mutex_destroy(&pdvobj->setbw_mutex);
+
+	kfree(pdvobj);
+}
+
+void rtw_reset_drv_sw(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	/* hal_priv */
+	if (is_primary_adapter(padapter))
+		rtw_hal_def_value_init(padapter);
+
+	RTW_ENABLE_FUNC(padapter, DF_RX_BIT);
+	RTW_ENABLE_FUNC(padapter, DF_TX_BIT);
+	padapter->bLinkInfoDump = 0;
+
+	padapter->xmitpriv.tx_pkts = 0;
+	padapter->recvpriv.rx_pkts = 0;
+
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+
+	/* pmlmepriv->LinkDetectInfo.TrafficBusyState = false; */
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING);
+
+	pwrctrlpriv->pwr_state_check_cnts = 0;
+
+	/* mlmeextpriv */
+	padapter->mlmeextpriv.sitesurvey_res.state = SCAN_DISABLE;
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+}
+
+
+u8 rtw_init_drv_sw(struct adapter *padapter)
+{
+	rtw_init_default_value(padapter);
+
+	rtw_init_hal_com_default_value(padapter);
+
+	if (rtw_init_cmd_priv(&padapter->cmdpriv))
+		return _FAIL;
+
+	padapter->cmdpriv.padapter = padapter;
+
+	if (rtw_init_evt_priv(&padapter->evtpriv))
+		goto free_cmd_priv;
+
+	if (rtw_init_mlme_priv(padapter) == _FAIL)
+		goto free_evt_priv;
+
+	init_mlme_ext_priv(padapter);
+
+	if (_rtw_init_xmit_priv(&padapter->xmitpriv, padapter) == _FAIL)
+		goto free_mlme_ext;
+
+	if (_rtw_init_recv_priv(&padapter->recvpriv, padapter) == _FAIL)
+		goto free_xmit_priv;
+	/*  add for CONFIG_IEEE80211W, none 11w also can use */
+	spin_lock_init(&padapter->security_key_mutex);
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((unsigned char *)&padapter->securitypriv, 0, sizeof (struct security_priv)); */
+
+	if (_rtw_init_sta_priv(&padapter->stapriv) == _FAIL)
+		goto free_recv_priv;
+
+	padapter->stapriv.padapter = padapter;
+	padapter->setband = GHZ24_50;
+	padapter->fix_rate = 0xFF;
+	rtw_init_bcmc_stainfo(padapter);
+
+	rtw_init_pwrctrl_priv(padapter);
+
+	rtw_hal_dm_init(padapter);
+
+	return _SUCCESS;
+
+free_recv_priv:
+	_rtw_free_recv_priv(&padapter->recvpriv);
+
+free_xmit_priv:
+	_rtw_free_xmit_priv(&padapter->xmitpriv);
+
+free_mlme_ext:
+	free_mlme_ext_priv(&padapter->mlmeextpriv);
+
+	rtw_free_mlme_priv(&padapter->mlmepriv);
+
+free_evt_priv:
+	rtw_free_evt_priv(&padapter->evtpriv);
+
+free_cmd_priv:
+	rtw_free_cmd_priv(&padapter->cmdpriv);
+
+	return _FAIL;
+}
+
+void rtw_cancel_all_timer(struct adapter *padapter)
+{
+	del_timer_sync(&padapter->mlmepriv.assoc_timer);
+
+	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
+
+	del_timer_sync(&padapter->mlmepriv.dynamic_chk_timer);
+
+	del_timer_sync(&(adapter_to_pwrctl(padapter)->pwr_state_check_timer));
+
+	del_timer_sync(&padapter->mlmepriv.set_scan_deny_timer);
+	rtw_clear_scan_deny(padapter);
+
+	del_timer_sync(&padapter->recvpriv.signal_stat_timer);
+
+	/* cancel dm timer */
+	rtw_hal_dm_deinit(padapter);
+}
+
+u8 rtw_free_drv_sw(struct adapter *padapter)
+{
+	free_mlme_ext_priv(&padapter->mlmeextpriv);
+
+	rtw_free_cmd_priv(&padapter->cmdpriv);
+
+	rtw_free_evt_priv(&padapter->evtpriv);
+
+	rtw_free_mlme_priv(&padapter->mlmepriv);
+
+	/* free_io_queue(padapter); */
+
+	_rtw_free_xmit_priv(&padapter->xmitpriv);
+
+	_rtw_free_sta_priv(&padapter->stapriv); /* will free bcmc_stainfo here */
+
+	_rtw_free_recv_priv(&padapter->recvpriv);
+
+	rtw_free_pwrctrl_priv(padapter);
+
+	/* kfree((void *)padapter); */
+
+	rtw_hal_free_data(padapter);
+
+	/* free the old_pnetdev */
+	if (padapter->rereg_nd_name_priv.old_pnetdev) {
+		free_netdev(padapter->rereg_nd_name_priv.old_pnetdev);
+		padapter->rereg_nd_name_priv.old_pnetdev = NULL;
+	}
+
+	/*  clear pbuddystruct adapter to avoid access wrong pointer. */
+	if (padapter->pbuddy_adapter)
+		padapter->pbuddy_adapter->pbuddy_adapter = NULL;
+
+	return _SUCCESS;
+}
+
+static int _rtw_drv_register_netdev(struct adapter *padapter, char *name)
+{
+	int ret = _SUCCESS;
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	/* alloc netdev name */
+	if (rtw_init_netdev_name(pnetdev, name))
+		return _FAIL;
+
+	eth_hw_addr_set(pnetdev, padapter->eeprompriv.mac_addr);
+
+	/* Tell the network stack we exist */
+	if (register_netdev(pnetdev) != 0) {
+		ret = _FAIL;
+		goto error_register_netdev;
+	}
+
+	return ret;
+
+error_register_netdev:
+
+	rtw_free_drv_sw(padapter);
+
+	rtw_free_netdev(pnetdev);
+
+	return ret;
+}
+
+int rtw_drv_register_netdev(struct adapter *if1)
+{
+	struct dvobj_priv *dvobj = if1->dvobj;
+	struct adapter *padapter = dvobj->padapters;
+	char *name = if1->registrypriv.ifname;
+
+	return _rtw_drv_register_netdev(padapter, name);
+}
+
+static int _netdev_open(struct net_device *pnetdev)
+{
+	uint status;
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	padapter->netif_up = true;
+
+	if (pwrctrlpriv->ps_flag) {
+		padapter->net_closed = false;
+		goto netdev_open_normal_process;
+	}
+
+	if (!padapter->bup) {
+		padapter->bDriverStopped = false;
+		padapter->bSurpriseRemoved = false;
+		padapter->bCardDisableWOHSM = false;
+
+		status = rtw_hal_init(padapter);
+		if (status == _FAIL)
+			goto netdev_open_error;
+
+		status = rtw_start_drv_threads(padapter);
+		if (status == _FAIL)
+			goto netdev_open_error;
+
+		if (padapter->intf_start)
+			padapter->intf_start(padapter);
+
+		rtw_cfg80211_init_wiphy(padapter);
+
+		padapter->bup = true;
+		pwrctrlpriv->bips_processing = false;
+	}
+	padapter->net_closed = false;
+
+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+
+	if (!rtw_netif_queue_stopped(pnetdev))
+		rtw_netif_start_queue(pnetdev);
+	else
+		rtw_netif_wake_queue(pnetdev);
+
+netdev_open_normal_process:
+
+	return 0;
+
+netdev_open_error:
+
+	padapter->bup = false;
+
+	netif_carrier_off(pnetdev);
+	rtw_netif_stop_queue(pnetdev);
+
+	return (-1);
+}
+
+int netdev_open(struct net_device *pnetdev)
+{
+	int ret;
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (pwrctrlpriv->bInSuspend)
+		return 0;
+
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->hw_init_mutex)))
+		return -1;
+
+	ret = _netdev_open(pnetdev);
+	mutex_unlock(&(adapter_to_dvobj(padapter)->hw_init_mutex));
+
+	return ret;
+}
+
+static int  ips_netdrv_open(struct adapter *padapter)
+{
+	int status = _SUCCESS;
+	/* struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); */
+
+	padapter->net_closed = false;
+
+	padapter->bDriverStopped = false;
+	padapter->bCardDisableWOHSM = false;
+	/* padapter->bup = true; */
+
+	status = rtw_hal_init(padapter);
+	if (status == _FAIL)
+		goto netdev_open_error;
+
+	if (padapter->intf_start)
+		padapter->intf_start(padapter);
+
+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+
+	return _SUCCESS;
+
+netdev_open_error:
+
+	return _FAIL;
+}
+
+
+int rtw_ips_pwr_up(struct adapter *padapter)
+{
+	return ips_netdrv_open(padapter);
+}
+
+void rtw_ips_pwr_down(struct adapter *padapter)
+{
+	padapter->bCardDisableWOHSM = true;
+	padapter->net_closed = true;
+
+	rtw_ips_dev_unload(padapter);
+	padapter->bCardDisableWOHSM = false;
+}
+
+void rtw_ips_dev_unload(struct adapter *padapter)
+{
+
+	if (!padapter->bSurpriseRemoved)
+		rtw_hal_deinit(padapter);
+}
+
+static int pm_netdev_open(struct net_device *pnetdev, u8 bnormal)
+{
+	int status = -1;
+
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+
+	if (bnormal) {
+		if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->hw_init_mutex)) == 0) {
+			status = _netdev_open(pnetdev);
+			mutex_unlock(&(adapter_to_dvobj(padapter)->hw_init_mutex));
+		}
+	} else {
+		status =  (_SUCCESS == ips_netdrv_open(padapter)) ? (0) : (-1);
+	}
+
+	return status;
+}
+
+static int netdev_close(struct net_device *pnetdev)
+{
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	if (pwrctl->bInternalAutoSuspend) {
+		/* rtw_pwr_wakeup(padapter); */
+		if (pwrctl->rf_pwrstate == rf_off)
+			pwrctl->ps_flag = true;
+	}
+	padapter->net_closed = true;
+	padapter->netif_up = false;
+
+/*if (!padapter->hw_init_completed)
+	{
+
+		padapter->bDriverStopped = true;
+
+		rtw_dev_unload(padapter);
+	}
+	else*/
+	if (pwrctl->rf_pwrstate == rf_on) {
+		/* s1. */
+		if (pnetdev) {
+			if (!rtw_netif_queue_stopped(pnetdev))
+				rtw_netif_stop_queue(pnetdev);
+		}
+
+		/* s2. */
+		LeaveAllPowerSaveMode(padapter);
+		rtw_disassoc_cmd(padapter, 500, false);
+		/* s2-2.  indicate disconnect to os */
+		rtw_indicate_disconnect(padapter);
+		/* s2-3. */
+		rtw_free_assoc_resources(padapter, 1);
+		/* s2-4. */
+		rtw_free_network_queue(padapter, true);
+	}
+
+	rtw_scan_abort(padapter);
+	adapter_wdev_data(padapter)->bandroid_scan = false;
+
+	return 0;
+}
+
+void rtw_ndev_destructor(struct net_device *ndev)
+{
+	kfree(ndev->ieee80211_ptr);
+}
+
+void rtw_dev_unload(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *pobjpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &pobjpriv->drv_dbg;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 cnt = 0;
+
+	if (padapter->bup) {
+
+		padapter->bDriverStopped = true;
+		if (padapter->xmitpriv.ack_tx)
+			rtw_ack_tx_done(&padapter->xmitpriv, RTW_SCTX_DONE_DRV_STOP);
+
+		if (padapter->intf_stop)
+			padapter->intf_stop(padapter);
+
+		if (!pwrctl->bInternalAutoSuspend)
+			rtw_stop_drv_threads(padapter);
+
+		while (atomic_read(&pcmdpriv->cmdthd_running)) {
+			if (cnt > 5) {
+				break;
+			} else {
+				cnt++;
+				msleep(10);
+			}
+		}
+
+		/* check the status of IPS */
+		if (rtw_hal_check_ips_status(padapter) || pwrctl->rf_pwrstate == rf_off) {
+			/* check HW status and SW state */
+			netdev_dbg(padapter->pnetdev,
+				   "%s: driver in IPS-FWLPS\n", __func__);
+			pdbgpriv->dbg_dev_unload_inIPS_cnt++;
+			LeaveAllPowerSaveMode(padapter);
+		} else {
+			netdev_dbg(padapter->pnetdev,
+				   "%s: driver not in IPS\n", __func__);
+		}
+
+		if (!padapter->bSurpriseRemoved) {
+			hal_btcoex_IpsNotify(padapter, pwrctl->ips_mode_req);
+
+			/* amy modify 20120221 for power seq is different between driver open and ips */
+			rtw_hal_deinit(padapter);
+
+			padapter->bSurpriseRemoved = true;
+		}
+
+		padapter->bup = false;
+
+	}
+}
+
+static int rtw_suspend_free_assoc_resource(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME)) {
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+			&& check_fwstate(pmlmepriv, _FW_LINKED)) {
+			rtw_set_to_roam(padapter, 1);
+		}
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtw_disassoc_cmd(padapter, 0, false);
+		/* s2-2.  indicate disconnect to os */
+		rtw_indicate_disconnect(padapter);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		rtw_sta_flush(padapter);
+	}
+
+	/* s2-3. */
+	rtw_free_assoc_resources(padapter, 1);
+
+	/* s2-4. */
+	rtw_free_network_queue(padapter, true);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		rtw_indicate_scan_done(padapter, 1);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		netdev_dbg(padapter->pnetdev, "%s: fw_under_linking\n",
+			   __func__);
+		rtw_indicate_disconnect(padapter);
+	}
+
+	return _SUCCESS;
+}
+
+static void rtw_suspend_normal(struct adapter *padapter)
+{
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	if (pnetdev) {
+		netif_carrier_off(pnetdev);
+		rtw_netif_stop_queue(pnetdev);
+	}
+
+	rtw_suspend_free_assoc_resource(padapter);
+
+	if ((rtw_hal_check_ips_status(padapter)) || (adapter_to_pwrctl(padapter)->rf_pwrstate == rf_off))
+		netdev_dbg(padapter->pnetdev,
+			   "%s: ### ERROR #### driver in IPS ####ERROR###!!!\n",
+			   __func__);
+
+	rtw_dev_unload(padapter);
+
+	/* sdio_deinit(adapter_to_dvobj(padapter)); */
+	if (padapter->intf_deinit)
+		padapter->intf_deinit(adapter_to_dvobj(padapter));
+}
+
+void rtw_suspend_common(struct adapter *padapter)
+{
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(psdpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	unsigned long start_time = jiffies;
+
+	netdev_dbg(padapter->pnetdev, " suspend start\n");
+	pdbgpriv->dbg_suspend_cnt++;
+
+	pwrpriv->bInSuspend = true;
+
+	while (pwrpriv->bips_processing)
+		msleep(1);
+
+	if ((!padapter->bup) || (padapter->bDriverStopped) || (padapter->bSurpriseRemoved)) {
+		pdbgpriv->dbg_suspend_error_cnt++;
+		goto exit;
+	}
+	rtw_ps_deny(padapter, PS_DENY_SUSPEND);
+
+	rtw_cancel_all_timer(padapter);
+
+	LeaveAllPowerSaveModeDirect(padapter);
+
+	rtw_stop_cmd_thread(padapter);
+
+	/*  wait for the latest FW to remove this condition. */
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		hal_btcoex_SuspendNotify(padapter, 0);
+	else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		hal_btcoex_SuspendNotify(padapter, 1);
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_SUSPEND);
+
+	rtw_suspend_normal(padapter);
+
+	netdev_dbg(padapter->pnetdev, "rtw suspend success in %d ms\n",
+		   jiffies_to_msecs(jiffies - start_time));
+
+exit:
+
+	return;
+}
+
+static int rtw_resume_process_normal(struct adapter *padapter)
+{
+	struct net_device *pnetdev;
+	struct pwrctrl_priv *pwrpriv;
+	struct mlme_priv *pmlmepriv;
+	struct dvobj_priv *psdpriv;
+	struct debug_priv *pdbgpriv;
+
+	int ret = _SUCCESS;
+
+	if (!padapter) {
+		ret = -1;
+		goto exit;
+	}
+
+	pnetdev = padapter->pnetdev;
+	pwrpriv = adapter_to_pwrctl(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+	psdpriv = padapter->dvobj;
+	pdbgpriv = &psdpriv->drv_dbg;
+	/*  interface init */
+	/* if (sdio_init(adapter_to_dvobj(padapter)) != _SUCCESS) */
+	if ((padapter->intf_init) && (padapter->intf_init(adapter_to_dvobj(padapter)) != _SUCCESS)) {
+		ret = -1;
+		goto exit;
+	}
+	rtw_hal_disable_interrupt(padapter);
+	/* if (sdio_alloc_irq(adapter_to_dvobj(padapter)) != _SUCCESS) */
+	if ((padapter->intf_alloc_irq) && (padapter->intf_alloc_irq(adapter_to_dvobj(padapter)) != _SUCCESS)) {
+		ret = -1;
+		goto exit;
+	}
+
+	rtw_reset_drv_sw(padapter);
+	pwrpriv->bkeepfwalive = false;
+
+	if (pm_netdev_open(pnetdev, true) != 0) {
+		ret = -1;
+		pdbgpriv->dbg_resume_error_cnt++;
+		goto exit;
+	}
+
+	netif_device_attach(pnetdev);
+	netif_carrier_on(pnetdev);
+
+	if (padapter->pid[1] != 0)
+		rtw_signal_process(padapter->pid[1], SIGUSR2);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME))
+			rtw_roaming(padapter, NULL);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		rtw_ap_restore_network(padapter);
+	}
+
+exit:
+	return ret;
+}
+
+int rtw_resume_common(struct adapter *padapter)
+{
+	int ret = 0;
+	unsigned long start_time = jiffies;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	netdev_dbg(padapter->pnetdev, "resume start\n");
+
+	rtw_resume_process_normal(padapter);
+
+	hal_btcoex_SuspendNotify(padapter, 0);
+
+	if (pwrpriv) {
+		pwrpriv->bInSuspend = false;
+	}
+	netdev_dbg(padapter->pnetdev, "%s:%d in %d ms\n", __func__, ret,
+		   jiffies_to_msecs(jiffies - start_time));
+
+	return ret;
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/osdep_service.c b/drivers/staging/rtl8723bs/os_dep/osdep_service.c
new file mode 100644
index 000000000..f09c1324c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/osdep_service.c
@@ -0,0 +1,235 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+/*
+* Translate the OS dependent @param error_code to OS independent RTW_STATUS_CODE
+* @return: one of RTW_STATUS_CODE
+*/
+inline int RTW_STATUS_CODE(int error_code)
+{
+	if (error_code >= 0)
+		return _SUCCESS;
+	return _FAIL;
+}
+
+void *_rtw_malloc(u32 sz)
+{
+	return kmalloc(sz, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+}
+
+void *_rtw_zmalloc(u32 sz)
+{
+	void *pbuf = _rtw_malloc(sz);
+
+	if (pbuf)
+		memset(pbuf, 0, sz);
+
+	return pbuf;
+}
+
+inline struct sk_buff *_rtw_skb_alloc(u32 sz)
+{
+	return __dev_alloc_skb(sz, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+}
+
+inline struct sk_buff *_rtw_skb_copy(const struct sk_buff *skb)
+{
+	return skb_copy(skb, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+}
+
+inline int _rtw_netif_rx(struct net_device *ndev, struct sk_buff *skb)
+{
+	skb->dev = ndev;
+	return netif_rx(skb);
+}
+
+struct net_device *rtw_alloc_etherdev_with_old_priv(int sizeof_priv, void *old_priv)
+{
+	struct net_device *pnetdev;
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	pnetdev = alloc_etherdev_mq(sizeof(struct rtw_netdev_priv_indicator), 4);
+	if (!pnetdev)
+		goto RETURN;
+
+	pnpi = netdev_priv(pnetdev);
+	pnpi->priv = old_priv;
+	pnpi->sizeof_priv = sizeof_priv;
+
+RETURN:
+	return pnetdev;
+}
+
+struct net_device *rtw_alloc_etherdev(int sizeof_priv)
+{
+	struct net_device *pnetdev;
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	pnetdev = alloc_etherdev_mq(sizeof(struct rtw_netdev_priv_indicator), 4);
+	if (!pnetdev)
+		goto RETURN;
+
+	pnpi = netdev_priv(pnetdev);
+
+	pnpi->priv = vzalloc(sizeof_priv);
+	if (!pnpi->priv) {
+		free_netdev(pnetdev);
+		pnetdev = NULL;
+		goto RETURN;
+	}
+
+	pnpi->sizeof_priv = sizeof_priv;
+RETURN:
+	return pnetdev;
+}
+
+void rtw_free_netdev(struct net_device *netdev)
+{
+	struct rtw_netdev_priv_indicator *pnpi;
+
+	if (!netdev)
+		goto RETURN;
+
+	pnpi = netdev_priv(netdev);
+
+	if (!pnpi->priv)
+		goto RETURN;
+
+	vfree(pnpi->priv);
+	free_netdev(netdev);
+
+RETURN:
+	return;
+}
+
+void rtw_buf_free(u8 **buf, u32 *buf_len)
+{
+	if (!buf || !buf_len)
+		return;
+
+	if (*buf) {
+		*buf_len = 0;
+		kfree(*buf);
+		*buf = NULL;
+	}
+}
+
+void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len)
+{
+	u32 ori_len = 0, dup_len = 0;
+	u8 *ori = NULL;
+	u8 *dup = NULL;
+
+	if (!buf || !buf_len)
+		return;
+
+	if (!src || !src_len)
+		goto keep_ori;
+
+	/* duplicate src */
+	dup = rtw_malloc(src_len);
+	if (dup) {
+		dup_len = src_len;
+		memcpy(dup, src, dup_len);
+	}
+
+keep_ori:
+	ori = *buf;
+	ori_len = *buf_len;
+
+	/* replace buf with dup */
+	*buf_len = 0;
+	*buf = dup;
+	*buf_len = dup_len;
+
+	/* free ori */
+	if (ori && ori_len > 0)
+		kfree(ori);
+}
+
+
+/**
+ * rtw_cbuf_full - test if cbuf is full
+ * @cbuf: pointer of struct rtw_cbuf
+ *
+ * Returns: true if cbuf is full
+ */
+inline bool rtw_cbuf_full(struct rtw_cbuf *cbuf)
+{
+	return (cbuf->write == cbuf->read - 1) ? true : false;
+}
+
+/**
+ * rtw_cbuf_empty - test if cbuf is empty
+ * @cbuf: pointer of struct rtw_cbuf
+ *
+ * Returns: true if cbuf is empty
+ */
+inline bool rtw_cbuf_empty(struct rtw_cbuf *cbuf)
+{
+	return (cbuf->write == cbuf->read) ? true : false;
+}
+
+/**
+ * rtw_cbuf_push - push a pointer into cbuf
+ * @cbuf: pointer of struct rtw_cbuf
+ * @buf: pointer to push in
+ *
+ * Lock free operation, be careful of the use scheme
+ * Returns: true push success
+ */
+bool rtw_cbuf_push(struct rtw_cbuf *cbuf, void *buf)
+{
+	if (rtw_cbuf_full(cbuf))
+		return _FAIL;
+
+	cbuf->bufs[cbuf->write] = buf;
+	cbuf->write = (cbuf->write + 1) % cbuf->size;
+
+	return _SUCCESS;
+}
+
+/**
+ * rtw_cbuf_pop - pop a pointer from cbuf
+ * @cbuf: pointer of struct rtw_cbuf
+ *
+ * Lock free operation, be careful of the use scheme
+ * Returns: pointer popped out
+ */
+void *rtw_cbuf_pop(struct rtw_cbuf *cbuf)
+{
+	void *buf;
+	if (rtw_cbuf_empty(cbuf))
+		return NULL;
+
+	buf = cbuf->bufs[cbuf->read];
+	cbuf->read = (cbuf->read + 1) % cbuf->size;
+
+	return buf;
+}
+
+/**
+ * rtw_cbuf_alloc - allocate a rtw_cbuf with given size and do initialization
+ * @size: size of pointer
+ *
+ * Returns: pointer of srtuct rtw_cbuf, NULL for allocation failure
+ */
+struct rtw_cbuf *rtw_cbuf_alloc(u32 size)
+{
+	struct rtw_cbuf *cbuf;
+
+	cbuf = rtw_malloc(struct_size(cbuf, bufs, size));
+
+	if (cbuf) {
+		cbuf->write = cbuf->read = 0;
+		cbuf->size = size;
+	}
+
+	return cbuf;
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/recv_linux.c b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
new file mode 100644
index 000000000..88a69c7ca
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
@@ -0,0 +1,229 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+#include <net/cfg80211.h>
+#include <asm/unaligned.h>
+
+void rtw_os_free_recvframe(union recv_frame *precvframe)
+{
+	if (precvframe->u.hdr.pkt) {
+		dev_kfree_skb_any(precvframe->u.hdr.pkt);/* free skb by driver */
+
+		precvframe->u.hdr.pkt = NULL;
+	}
+}
+
+/* alloc os related resource in union recv_frame */
+void rtw_os_recv_resource_alloc(struct adapter *padapter, union recv_frame *precvframe)
+{
+	precvframe->u.hdr.pkt_newalloc = precvframe->u.hdr.pkt = NULL;
+}
+
+/* free os related resource in union recv_frame */
+void rtw_os_recv_resource_free(struct recv_priv *precvpriv)
+{
+	signed int i;
+	union recv_frame *precvframe;
+
+	precvframe = (union recv_frame *) precvpriv->precv_frame_buf;
+
+	for (i = 0; i < NR_RECVFRAME; i++) {
+		if (precvframe->u.hdr.pkt) {
+			/* free skb by driver */
+			dev_kfree_skb_any(precvframe->u.hdr.pkt);
+			precvframe->u.hdr.pkt = NULL;
+		}
+		precvframe++;
+	}
+}
+
+/* free os related resource in struct recv_buf */
+void rtw_os_recvbuf_resource_free(struct adapter *padapter, struct recv_buf *precvbuf)
+{
+	if (precvbuf->pskb) {
+		dev_kfree_skb_any(precvbuf->pskb);
+	}
+}
+
+struct sk_buff *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata)
+{
+	u16 eth_type;
+	struct sk_buff *sub_skb;
+	struct rx_pkt_attrib *pattrib;
+
+	pattrib = &prframe->u.hdr.attrib;
+
+	sub_skb = rtw_skb_alloc(nSubframe_Length + 12);
+	if (!sub_skb)
+		return NULL;
+
+	skb_reserve(sub_skb, 12);
+	skb_put_data(sub_skb, (pdata + ETH_HLEN), nSubframe_Length);
+
+	eth_type = get_unaligned_be16(&sub_skb->data[6]);
+
+	if (sub_skb->len >= 8 &&
+		((!memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) &&
+		  eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+		 !memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE))) {
+		/*
+		 * remove RFC1042 or Bridge-Tunnel encapsulation and replace
+		 * EtherType
+		 */
+		skb_pull(sub_skb, SNAP_SIZE);
+		memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+		memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+	} else {
+		__be16 len;
+		/* Leave Ethernet header part of hdr and full payload */
+		len = htons(sub_skb->len);
+		memcpy(skb_push(sub_skb, 2), &len, 2);
+		memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+		memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+	}
+
+	return sub_skb;
+}
+
+void rtw_os_recv_indicate_pkt(struct adapter *padapter, struct sk_buff *pkt, struct rx_pkt_attrib *pattrib)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* Indicate the packets to upper layer */
+	if (pkt) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+			struct sk_buff *pskb2 = NULL;
+			struct sta_info *psta = NULL;
+			struct sta_priv *pstapriv = &padapter->stapriv;
+			int bmcast = IS_MCAST(pattrib->dst);
+
+			if (memcmp(pattrib->dst, myid(&padapter->eeprompriv), ETH_ALEN)) {
+				if (bmcast) {
+					psta = rtw_get_bcmc_stainfo(padapter);
+					pskb2 = skb_clone(pkt, GFP_ATOMIC);
+				} else {
+					psta = rtw_get_stainfo(pstapriv, pattrib->dst);
+				}
+
+				if (psta) {
+					struct net_device *pnetdev = (struct net_device *)padapter->pnetdev;
+					/* skb->ip_summed = CHECKSUM_NONE; */
+					pkt->dev = pnetdev;
+					skb_set_queue_mapping(pkt, rtw_recv_select_queue(pkt));
+
+					_rtw_xmit_entry(pkt, pnetdev);
+
+					if (bmcast && pskb2)
+						pkt = pskb2;
+					else
+						return;
+				}
+			} else {
+				/*  to APself */
+			}
+		}
+
+		pkt->protocol = eth_type_trans(pkt, padapter->pnetdev);
+		pkt->dev = padapter->pnetdev;
+
+		pkt->ip_summed = CHECKSUM_NONE;
+
+		rtw_netif_rx(padapter->pnetdev, pkt);
+	}
+}
+
+void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
+{
+	enum nl80211_key_type key_type = 0;
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure    ev;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	unsigned long cur_time = 0;
+
+	if (psecuritypriv->last_mic_err_time == 0) {
+		psecuritypriv->last_mic_err_time = jiffies;
+	} else {
+		cur_time = jiffies;
+
+		if (cur_time - psecuritypriv->last_mic_err_time < 60*HZ) {
+			psecuritypriv->btkip_countermeasure = true;
+			psecuritypriv->last_mic_err_time = 0;
+			psecuritypriv->btkip_countermeasure_time = cur_time;
+		} else {
+			psecuritypriv->last_mic_err_time = jiffies;
+		}
+	}
+
+	if (bgroup) {
+		key_type |= NL80211_KEYTYPE_GROUP;
+	} else {
+		key_type |= NL80211_KEYTYPE_PAIRWISE;
+	}
+
+	cfg80211_michael_mic_failure(padapter->pnetdev, (u8 *)&pmlmepriv->assoc_bssid[0], key_type, -1,
+		NULL, GFP_ATOMIC);
+
+	memset(&ev, 0x00, sizeof(ev));
+	if (bgroup) {
+		ev.flags |= IW_MICFAILURE_GROUP;
+	} else {
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+	}
+
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	memcpy(ev.src_addr.sa_data, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+
+	memset(&wrqu, 0x00, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+}
+
+int rtw_recv_indicatepkt(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct recv_priv *precvpriv;
+	struct __queue	*pfree_recv_queue;
+	struct sk_buff *skb;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+
+	precvpriv = &(padapter->recvpriv);
+	pfree_recv_queue = &(precvpriv->free_recv_queue);
+
+	skb = precv_frame->u.hdr.pkt;
+	if (!skb)
+		goto _recv_indicatepkt_drop;
+
+	skb->data = precv_frame->u.hdr.rx_data;
+
+	skb_set_tail_pointer(skb, precv_frame->u.hdr.len);
+
+	skb->len = precv_frame->u.hdr.len;
+
+	rtw_os_recv_indicate_pkt(padapter, skb, pattrib);
+
+	/* pointers to NULL before rtw_free_recvframe() */
+	precv_frame->u.hdr.pkt = NULL;
+
+	rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	return _SUCCESS;
+
+_recv_indicatepkt_drop:
+
+	/* enqueue back to free_recv_queue */
+	rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	return _FAIL;
+}
+
+void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
+{
+	timer_setup(&preorder_ctrl->reordering_ctrl_timer,
+		    rtw_reordering_ctrl_timeout_handler, 0);
+
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/sdio_intf.c b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
new file mode 100644
index 000000000..490431484
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
@@ -0,0 +1,506 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <linux/jiffies.h>
+
+#ifndef dev_to_sdio_func
+#define dev_to_sdio_func(d)     container_of(d, struct sdio_func, dev)
+#endif
+
+static const struct sdio_device_id sdio_ids[] = {
+	{ SDIO_DEVICE(0x024c, 0x0523), },
+	{ SDIO_DEVICE(0x024c, 0x0525), },
+	{ SDIO_DEVICE(0x024c, 0x0623), },
+	{ SDIO_DEVICE(0x024c, 0x0626), },
+	{ SDIO_DEVICE(0x024c, 0x0627), },
+	{ SDIO_DEVICE(0x024c, 0xb723), },
+	{ /* end: all zeroes */				},
+};
+MODULE_DEVICE_TABLE(sdio, sdio_ids);
+
+static int rtw_drv_init(struct sdio_func *func, const struct sdio_device_id *id);
+static void rtw_dev_remove(struct sdio_func *func);
+static int rtw_sdio_resume(struct device *dev);
+static int rtw_sdio_suspend(struct device *dev);
+
+static const struct dev_pm_ops rtw_sdio_pm_ops = {
+	.suspend	= rtw_sdio_suspend,
+	.resume	= rtw_sdio_resume,
+};
+
+static struct sdio_driver rtl8723bs_sdio_driver = {
+	.probe = rtw_drv_init,
+	.remove = rtw_dev_remove,
+	.name = "rtl8723bs",
+	.id_table = sdio_ids,
+	.drv = {
+		.pm = &rtw_sdio_pm_ops,
+	}
+};
+
+static void sd_sync_int_hdl(struct sdio_func *func)
+{
+	struct dvobj_priv *psdpriv;
+
+
+	psdpriv = sdio_get_drvdata(func);
+
+	if (!psdpriv->if1)
+		return;
+
+	rtw_sdio_set_irq_thd(psdpriv, current);
+	sd_int_hdl(psdpriv->if1);
+	rtw_sdio_set_irq_thd(psdpriv, NULL);
+}
+
+static int sdio_alloc_irq(struct dvobj_priv *dvobj)
+{
+	struct sdio_data *psdio_data;
+	struct sdio_func *func;
+	int err;
+
+	psdio_data = &dvobj->intf_data;
+	func = psdio_data->func;
+
+	sdio_claim_host(func);
+
+	err = sdio_claim_irq(func, &sd_sync_int_hdl);
+	if (err) {
+		dvobj->drv_dbg.dbg_sdio_alloc_irq_error_cnt++;
+		printk(KERN_CRIT "%s: sdio_claim_irq FAIL(%d)!\n", __func__, err);
+	} else {
+		dvobj->drv_dbg.dbg_sdio_alloc_irq_cnt++;
+		dvobj->irq_alloc = 1;
+	}
+
+	sdio_release_host(func);
+
+	return err?_FAIL:_SUCCESS;
+}
+
+static void sdio_free_irq(struct dvobj_priv *dvobj)
+{
+	struct sdio_data *psdio_data;
+	struct sdio_func *func;
+	int err;
+
+	if (dvobj->irq_alloc) {
+		psdio_data = &dvobj->intf_data;
+		func = psdio_data->func;
+
+		if (func) {
+			sdio_claim_host(func);
+			err = sdio_release_irq(func);
+			if (err) {
+				dvobj->drv_dbg.dbg_sdio_free_irq_error_cnt++;
+				netdev_err(dvobj->if1->pnetdev,
+					   "%s: sdio_release_irq FAIL(%d)!\n",
+					   __func__, err);
+			} else
+				dvobj->drv_dbg.dbg_sdio_free_irq_cnt++;
+			sdio_release_host(func);
+		}
+		dvobj->irq_alloc = 0;
+	}
+}
+
+static u32 sdio_init(struct dvobj_priv *dvobj)
+{
+	struct sdio_data *psdio_data;
+	struct sdio_func *func;
+	int err;
+
+	psdio_data = &dvobj->intf_data;
+	func = psdio_data->func;
+
+	/* 3 1. init SDIO bus */
+	sdio_claim_host(func);
+
+	err = sdio_enable_func(func);
+	if (err) {
+		dvobj->drv_dbg.dbg_sdio_init_error_cnt++;
+		goto release;
+	}
+
+	err = sdio_set_block_size(func, 512);
+	if (err) {
+		dvobj->drv_dbg.dbg_sdio_init_error_cnt++;
+		goto release;
+	}
+	psdio_data->block_transfer_len = 512;
+	psdio_data->tx_block_mode = 1;
+	psdio_data->rx_block_mode = 1;
+
+release:
+	sdio_release_host(func);
+
+	if (err)
+		return _FAIL;
+	return _SUCCESS;
+}
+
+static void sdio_deinit(struct dvobj_priv *dvobj)
+{
+	struct sdio_func *func;
+	int err;
+
+	func = dvobj->intf_data.func;
+
+	if (func) {
+		sdio_claim_host(func);
+		err = sdio_disable_func(func);
+		if (err)
+			dvobj->drv_dbg.dbg_sdio_deinit_error_cnt++;
+
+		if (dvobj->irq_alloc) {
+			err = sdio_release_irq(func);
+			if (err)
+				dvobj->drv_dbg.dbg_sdio_free_irq_error_cnt++;
+			else
+				dvobj->drv_dbg.dbg_sdio_free_irq_cnt++;
+		}
+
+		sdio_release_host(func);
+	}
+}
+static struct dvobj_priv *sdio_dvobj_init(struct sdio_func *func)
+{
+	int status = _FAIL;
+	struct dvobj_priv *dvobj = NULL;
+	struct sdio_data *psdio;
+
+	dvobj = devobj_init();
+	if (!dvobj)
+		goto exit;
+
+	sdio_set_drvdata(func, dvobj);
+
+	psdio = &dvobj->intf_data;
+	psdio->func = func;
+
+	if (sdio_init(dvobj) != _SUCCESS)
+		goto free_dvobj;
+
+	rtw_reset_continual_io_error(dvobj);
+	status = _SUCCESS;
+
+free_dvobj:
+	if (status != _SUCCESS && dvobj) {
+		sdio_set_drvdata(func, NULL);
+
+		devobj_deinit(dvobj);
+
+		dvobj = NULL;
+	}
+exit:
+	return dvobj;
+}
+
+static void sdio_dvobj_deinit(struct sdio_func *func)
+{
+	struct dvobj_priv *dvobj = sdio_get_drvdata(func);
+
+	sdio_set_drvdata(func, NULL);
+	if (dvobj) {
+		sdio_deinit(dvobj);
+		devobj_deinit(dvobj);
+	}
+}
+
+void rtw_set_hal_ops(struct adapter *padapter)
+{
+	/* alloc memory for HAL DATA */
+	rtw_hal_data_init(padapter);
+
+	rtl8723bs_set_hal_ops(padapter);
+}
+
+static void sd_intf_start(struct adapter *padapter)
+{
+	if (!padapter)
+		return;
+
+	/*  hal dep */
+	rtw_hal_enable_interrupt(padapter);
+}
+
+static void sd_intf_stop(struct adapter *padapter)
+{
+	if (!padapter)
+		return;
+
+	/*  hal dep */
+	rtw_hal_disable_interrupt(padapter);
+}
+
+
+static struct adapter *rtw_sdio_if1_init(struct dvobj_priv *dvobj, const struct sdio_device_id  *pdid)
+{
+	int status = _FAIL;
+	struct net_device *pnetdev;
+	struct adapter *padapter = NULL;
+	struct sdio_data *psdio = &dvobj->intf_data;
+
+	padapter = vzalloc(sizeof(*padapter));
+	if (!padapter)
+		goto exit;
+
+	padapter->dvobj = dvobj;
+	dvobj->if1 = padapter;
+
+	padapter->bDriverStopped = true;
+
+	dvobj->padapters = padapter;
+	padapter->iface_id = 0;
+
+	/* 3 1. init network device data */
+	pnetdev = rtw_init_netdev(padapter);
+	if (!pnetdev)
+		goto free_adapter;
+
+	SET_NETDEV_DEV(pnetdev, dvobj_to_dev(dvobj));
+
+	padapter = rtw_netdev_priv(pnetdev);
+
+	/* 3 3. init driver special setting, interface, OS and hardware relative */
+
+	/* 4 3.1 set hardware operation functions */
+	rtw_set_hal_ops(padapter);
+
+
+	/* 3 5. initialize Chip version */
+	padapter->intf_start = &sd_intf_start;
+	padapter->intf_stop = &sd_intf_stop;
+
+	padapter->intf_init = &sdio_init;
+	padapter->intf_deinit = &sdio_deinit;
+	padapter->intf_alloc_irq = &sdio_alloc_irq;
+	padapter->intf_free_irq = &sdio_free_irq;
+
+	if (rtw_init_io_priv(padapter, sdio_set_intf_ops) == _FAIL)
+		goto free_hal_data;
+
+	rtw_hal_read_chip_version(padapter);
+
+	rtw_hal_chip_configure(padapter);
+
+	hal_btcoex_Initialize((void *) padapter);
+
+	/* 3 6. read efuse/eeprom data */
+	rtw_hal_read_chip_info(padapter);
+
+	/* 3 7. init driver common data */
+	if (rtw_init_drv_sw(padapter) == _FAIL)
+		goto free_hal_data;
+
+	rtw_wdev_alloc(padapter, dvobj_to_dev(dvobj));
+
+	/* 3 8. get WLan MAC address */
+	/*  set mac addr */
+	rtw_macaddr_cfg(&psdio->func->dev, padapter->eeprompriv.mac_addr);
+
+	rtw_hal_disable_interrupt(padapter);
+
+	status = _SUCCESS;
+
+free_hal_data:
+	if (status != _SUCCESS && padapter->HalData)
+		kfree(padapter->HalData);
+
+	if (status != _SUCCESS) {
+		rtw_wdev_unregister(padapter->rtw_wdev);
+		rtw_wdev_free(padapter->rtw_wdev);
+	}
+
+free_adapter:
+	if (status != _SUCCESS) {
+		if (pnetdev)
+			rtw_free_netdev(pnetdev);
+		else
+			vfree((u8 *)padapter);
+		padapter = NULL;
+	}
+exit:
+	return padapter;
+}
+
+static void rtw_sdio_if1_deinit(struct adapter *if1)
+{
+	struct net_device *pnetdev = if1->pnetdev;
+	struct mlme_priv *pmlmepriv = &if1->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		rtw_disassoc_cmd(if1, 0, false);
+
+	free_mlme_ap_info(if1);
+
+	rtw_cancel_all_timer(if1);
+
+	rtw_dev_unload(if1);
+
+	if (if1->rtw_wdev)
+		rtw_wdev_free(if1->rtw_wdev);
+
+	rtw_free_drv_sw(if1);
+
+	if (pnetdev)
+		rtw_free_netdev(pnetdev);
+}
+
+/*
+ * drv_init() - a device potentially for us
+ *
+ * notes: drv_init() is called when the bus driver has located a card for us to support.
+ *        We accept the new device by returning 0.
+ */
+static int rtw_drv_init(
+	struct sdio_func *func,
+	const struct sdio_device_id *id)
+{
+	int status = _FAIL;
+	struct adapter *if1 = NULL;
+	struct dvobj_priv *dvobj;
+
+	dvobj = sdio_dvobj_init(func);
+	if (!dvobj)
+		goto exit;
+
+	if1 = rtw_sdio_if1_init(dvobj, id);
+	if (!if1)
+		goto free_dvobj;
+
+	/* dev_alloc_name && register_netdev */
+	status = rtw_drv_register_netdev(if1);
+	if (status != _SUCCESS)
+		goto free_if1;
+
+	if (sdio_alloc_irq(dvobj) != _SUCCESS)
+		goto free_if1;
+
+	rtw_ndev_notifier_register();
+	status = _SUCCESS;
+
+free_if1:
+	if (status != _SUCCESS && if1)
+		rtw_sdio_if1_deinit(if1);
+
+free_dvobj:
+	if (status != _SUCCESS)
+		sdio_dvobj_deinit(func);
+exit:
+	return status == _SUCCESS ? 0 : -ENODEV;
+}
+
+static void rtw_dev_remove(struct sdio_func *func)
+{
+	struct dvobj_priv *dvobj = sdio_get_drvdata(func);
+	struct adapter *padapter = dvobj->if1;
+
+	dvobj->processing_dev_remove = true;
+
+	rtw_unregister_netdevs(dvobj);
+
+	if (!padapter->bSurpriseRemoved) {
+		int err;
+
+		/* test surprise remove */
+		sdio_claim_host(func);
+		sdio_readb(func, 0, &err);
+		sdio_release_host(func);
+		if (err == -ENOMEDIUM)
+			padapter->bSurpriseRemoved = true;
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_DRV_REMOVE);
+
+	rtw_pm_set_ips(padapter, IPS_NONE);
+	rtw_pm_set_lps(padapter, PS_MODE_ACTIVE);
+
+	LeaveAllPowerSaveMode(padapter);
+
+	rtw_btcoex_HaltNotify(padapter);
+
+	rtw_sdio_if1_deinit(padapter);
+
+	sdio_dvobj_deinit(func);
+}
+
+static int rtw_sdio_suspend(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct dvobj_priv *psdpriv = sdio_get_drvdata(func);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(psdpriv);
+	struct adapter *padapter = psdpriv->if1;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (padapter->bDriverStopped)
+		return 0;
+
+	if (pwrpriv->bInSuspend) {
+		pdbgpriv->dbg_suspend_error_cnt++;
+		return 0;
+	}
+
+	rtw_suspend_common(padapter);
+
+	return 0;
+}
+
+static int rtw_resume_process(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (!pwrpriv->bInSuspend) {
+		pdbgpriv->dbg_resume_error_cnt++;
+		return -1;
+	}
+
+	return rtw_resume_common(padapter);
+}
+
+static int rtw_sdio_resume(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct dvobj_priv *psdpriv = sdio_get_drvdata(func);
+	struct adapter *padapter = psdpriv->if1;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	int ret = 0;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	pdbgpriv->dbg_resume_cnt++;
+
+	ret = rtw_resume_process(padapter);
+
+	pmlmeext->last_scan_time = jiffies;
+	return ret;
+}
+
+static int __init rtw_drv_entry(void)
+{
+	int ret;
+
+	ret = sdio_register_driver(&rtl8723bs_sdio_driver);
+	if (ret != 0)
+		rtw_ndev_notifier_unregister();
+
+	return ret;
+}
+
+static void __exit rtw_drv_halt(void)
+{
+	sdio_unregister_driver(&rtl8723bs_sdio_driver);
+
+	rtw_ndev_notifier_unregister();
+}
+
+
+module_init(rtw_drv_entry);
+module_exit(rtw_drv_halt);
diff --git a/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c b/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
new file mode 100644
index 000000000..0a0b04088
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
@@ -0,0 +1,497 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ *******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+static bool rtw_sdio_claim_host_needed(struct sdio_func *func)
+{
+	struct dvobj_priv *dvobj = sdio_get_drvdata(func);
+	struct sdio_data *sdio_data = &dvobj->intf_data;
+
+	if (sdio_data->sys_sdio_irq_thd && sdio_data->sys_sdio_irq_thd == current)
+		return false;
+	return true;
+}
+
+inline void rtw_sdio_set_irq_thd(struct dvobj_priv *dvobj, void *thd_hdl)
+{
+	struct sdio_data *sdio_data = &dvobj->intf_data;
+
+	sdio_data->sys_sdio_irq_thd = thd_hdl;
+}
+
+/*
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 _sd_cmd52_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	int err = 0, i;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+
+	for (i = 0; i < cnt; i++) {
+		pdata[i] = sdio_readb(func, addr + i, &err);
+		if (err)
+			break;
+	}
+	return err;
+}
+
+/*
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 sd_cmd52_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	int err = 0;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	err = _sd_cmd52_read(pintfhdl, addr, cnt, pdata);
+	if (claim_needed)
+		sdio_release_host(func);
+	return err;
+}
+
+/*
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 _sd_cmd52_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	int err = 0, i;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+
+	for (i = 0; i < cnt; i++) {
+		sdio_writeb(func, pdata[i], addr + i, &err);
+		if (err)
+			break;
+	}
+	return err;
+}
+
+/*
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 sd_cmd52_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	int err = 0;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	err = _sd_cmd52_write(pintfhdl, addr, cnt, pdata);
+	if (claim_needed)
+		sdio_release_host(func);
+	return err;
+}
+
+u8 sd_read8(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	u8 v = 0;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return v;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	v = sdio_readb(func, addr, err);
+	if (claim_needed)
+		sdio_release_host(func);
+	return v;
+}
+
+u32 sd_read32(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+	u32 v = 0;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return v;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	v = sdio_readl(func, addr, err);
+	if (claim_needed)
+		sdio_release_host(func);
+
+	if (err && *err) {
+		int i;
+
+		*err = 0;
+		for (i = 0; i < SD_IO_TRY_CNT; i++) {
+			if (claim_needed)
+				sdio_claim_host(func);
+			v = sdio_readl(func, addr, err);
+			if (claim_needed)
+				sdio_release_host(func);
+
+			if (*err == 0) {
+				rtw_reset_continual_io_error(psdiodev);
+				break;
+			} else {
+				if ((-ESHUTDOWN == *err) || (-ENODEV == *err))
+					padapter->bSurpriseRemoved = true;
+
+				if (rtw_inc_and_chk_continual_io_error(psdiodev) == true) {
+					padapter->bSurpriseRemoved = true;
+					break;
+				}
+			}
+		}
+	}
+	return  v;
+}
+
+void sd_write8(struct intf_hdl *pintfhdl, u32 addr, u8 v, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	sdio_writeb(func, v, addr, err);
+	if (claim_needed)
+		sdio_release_host(func);
+}
+
+void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	sdio_writel(func, v, addr, err);
+	if (claim_needed)
+		sdio_release_host(func);
+
+	if (err && *err) {
+		int i;
+
+		*err = 0;
+		for (i = 0; i < SD_IO_TRY_CNT; i++) {
+			if (claim_needed)
+				sdio_claim_host(func);
+			sdio_writel(func, v, addr, err);
+			if (claim_needed)
+				sdio_release_host(func);
+			if (*err == 0) {
+				rtw_reset_continual_io_error(psdiodev);
+				break;
+			} else {
+				if ((-ESHUTDOWN == *err) || (-ENODEV == *err))
+					padapter->bSurpriseRemoved = true;
+
+				if (rtw_inc_and_chk_continual_io_error(psdiodev) == true) {
+					padapter->bSurpriseRemoved = true;
+					break;
+				}
+			}
+		}
+
+	}
+}
+
+/*
+ * Use CMD53 to read data from SDIO device.
+ * This function MUST be called after sdio_claim_host() or
+ * in SDIO ISR(host had been claimed).
+ *
+ * Parameters:
+ *psdio	pointer of SDIO_DATA
+ *addr	address to read
+ *cnt		amount to read
+ *pdata	pointer to put data, this should be a "DMA:able scratch buffer"!
+ *
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 _sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	int err = -EPERM;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+
+	if (unlikely((cnt == 1) || (cnt == 2))) {
+		int i;
+		u8 *pbuf = pdata;
+
+		for (i = 0; i < cnt; i++) {
+			*(pbuf + i) = sdio_readb(func, addr + i, &err);
+
+			if (err)
+				break;
+		}
+		return err;
+	}
+
+	err = sdio_memcpy_fromio(func, pdata, addr, cnt);
+
+	return err;
+}
+
+/*
+ * Use CMD53 to read data from SDIO device.
+ *
+ * Parameters:
+ *psdio	pointer of SDIO_DATA
+ *addr	address to read
+ *cnt		amount to read
+ *pdata	pointer to put data, this should be a "DMA:able scratch buffer"!
+ *
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	struct sdio_func *func;
+	bool claim_needed;
+	s32 err = -EPERM;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	err = _sd_read(pintfhdl, addr, cnt, pdata);
+	if (claim_needed)
+		sdio_release_host(func);
+	return err;
+}
+
+/*
+ * Use CMD53 to write data to SDIO device.
+ * This function MUST be called after sdio_claim_host() or
+ * in SDIO ISR(host had been claimed).
+ *
+ * Parameters:
+ *psdio	pointer of SDIO_DATA
+ *addr	address to write
+ *cnt		amount to write
+ *pdata	data pointer, this should be a "DMA:able scratch buffer"!
+ *
+ * Return:
+ *0		Success
+ *others	Fail
+ */
+s32 _sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+
+	struct sdio_func *func;
+	u32 size;
+	s32 err =  -EPERM;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+/*	size = sdio_align_size(func, cnt); */
+
+	if (unlikely((cnt == 1) || (cnt == 2))) {
+		int i;
+		u8 *pbuf = pdata;
+
+		for (i = 0; i < cnt; i++) {
+			sdio_writeb(func, *(pbuf + i), addr + i, &err);
+			if (err)
+				break;
+		}
+
+		return err;
+	}
+
+	size = cnt;
+	err = sdio_memcpy_toio(func, addr, pdata, size);
+
+	return err;
+}
+
+/*
+ * Use CMD53 to write data to SDIO device.
+ *
+ * Parameters:
+ *  psdio	pointer of SDIO_DATA
+ *  addr	address to write
+ *  cnt		amount to write
+ *  pdata	data pointer, this should be a "DMA:able scratch buffer"!
+ *
+ * Return:
+ *  0		Success
+ *  others	Fail
+ */
+s32 sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	struct sdio_data *psdio;
+	struct sdio_func *func;
+	bool claim_needed;
+	s32 err =  -EPERM;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved)
+		return err;
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	err = _sd_write(pintfhdl, addr, cnt, pdata);
+	if (claim_needed)
+		sdio_release_host(func);
+	return err;
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/wifi_regd.c b/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
new file mode 100644
index 000000000..5eef1d68c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
@@ -0,0 +1,150 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ *****************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+#include <rtw_wifi_regd.h>
+
+/*
+ * REG_RULE(freq start, freq end, bandwidth, max gain, eirp, reg_flags)
+ */
+
+/*
+ * Only these channels all allow active
+ * scan on all world regulatory domains
+ */
+
+/* 2G chan 01 - chan 11 */
+#define RTW_2GHZ_CH01_11	\
+	REG_RULE(2412 - 10, 2462 + 10, 40, 0, 20, 0)
+
+/*
+ * We enable active scan on these a case
+ * by case basis by regulatory domain
+ */
+
+/* 2G chan 12 - chan 13, PASSIV SCAN */
+#define RTW_2GHZ_CH12_13	\
+	REG_RULE(2467 - 10, 2472 + 10, 40, 0, 20,	\
+	NL80211_RRF_PASSIVE_SCAN)
+
+static const struct ieee80211_regdomain rtw_regdom_rd = {
+	.n_reg_rules = 2,
+	.alpha2 = "99",
+	.reg_rules = {
+		RTW_2GHZ_CH01_11,
+		RTW_2GHZ_CH12_13,
+	}
+};
+
+static int rtw_ieee80211_channel_to_frequency(int chan, int band)
+{
+	/* NL80211_BAND_2GHZ */
+	if (chan == 14)
+		return 2484;
+	else if (chan < 14)
+		return 2407 + chan * 5;
+	else
+		return 0;	/* not supported */
+}
+
+static void _rtw_reg_apply_flags(struct wiphy *wiphy)
+{
+	struct adapter *padapter = wiphy_to_adapter(wiphy);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct rt_channel_info *channel_set = pmlmeext->channel_set;
+	u8 max_chan_nums = pmlmeext->max_chan_nums;
+
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i, j;
+	u16 channel;
+	u32 freq;
+
+	/* all channels disable */
+	for (i = 0; i < NUM_NL80211_BANDS; i++) {
+		sband = wiphy->bands[i];
+
+		if (sband) {
+			for (j = 0; j < sband->n_channels; j++) {
+				ch = &sband->channels[j];
+
+				if (ch)
+					ch->flags = IEEE80211_CHAN_DISABLED;
+			}
+		}
+	}
+
+	/* channels apply by channel plans. */
+	for (i = 0; i < max_chan_nums; i++) {
+		channel = channel_set[i].ChannelNum;
+		freq =
+		    rtw_ieee80211_channel_to_frequency(channel,
+						       NL80211_BAND_2GHZ);
+
+		ch = ieee80211_get_channel(wiphy, freq);
+		if (ch) {
+			if (channel_set[i].ScanType == SCAN_PASSIVE)
+				ch->flags = IEEE80211_CHAN_NO_IR;
+			else
+				ch->flags = 0;
+		}
+	}
+}
+
+static int _rtw_reg_notifier_apply(struct wiphy *wiphy,
+				   struct regulatory_request *request,
+				   struct rtw_regulatory *reg)
+{
+	/* Hard code flags */
+	_rtw_reg_apply_flags(wiphy);
+	return 0;
+}
+
+static const struct ieee80211_regdomain *_rtw_regdomain_select(struct
+							       rtw_regulatory
+							       *reg)
+{
+	return &rtw_regdom_rd;
+}
+
+static void _rtw_regd_init_wiphy(struct rtw_regulatory *reg,
+				 struct wiphy *wiphy,
+				 void (*reg_notifier)(struct wiphy *wiphy,
+						      struct
+						      regulatory_request *
+						      request))
+{
+	const struct ieee80211_regdomain *regd;
+
+	wiphy->reg_notifier = reg_notifier;
+
+	wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
+	wiphy->regulatory_flags &= ~REGULATORY_STRICT_REG;
+	wiphy->regulatory_flags &= ~REGULATORY_DISABLE_BEACON_HINTS;
+
+	regd = _rtw_regdomain_select(reg);
+	wiphy_apply_custom_regulatory(wiphy, regd);
+
+	/* Hard code flags */
+	_rtw_reg_apply_flags(wiphy);
+}
+
+void rtw_regd_init(struct wiphy *wiphy,
+		   void (*reg_notifier)(struct wiphy *wiphy,
+					struct regulatory_request *request))
+{
+	_rtw_regd_init_wiphy(NULL, wiphy, reg_notifier);
+}
+
+void rtw_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
+{
+	struct rtw_regulatory *reg = NULL;
+
+	_rtw_reg_notifier_apply(wiphy, request, reg);
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/xmit_linux.c b/drivers/staging/rtl8723bs/os_dep/xmit_linux.c
new file mode 100644
index 000000000..1eeabfffd
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/xmit_linux.c
@@ -0,0 +1,226 @@
+// SPDX-License-Identifier: GPL-2.0
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+
+uint rtw_remainder_len(struct pkt_file *pfile)
+{
+	return (pfile->buf_len - ((SIZE_PTR)(pfile->cur_addr) - (SIZE_PTR)(pfile->buf_start)));
+}
+
+void _rtw_open_pktfile(struct sk_buff *pktptr, struct pkt_file *pfile)
+{
+	pfile->pkt = pktptr;
+	pfile->cur_addr = pfile->buf_start = pktptr->data;
+	pfile->pkt_len = pfile->buf_len = pktptr->len;
+
+	pfile->cur_buffer = pfile->buf_start;
+}
+
+uint _rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen)
+{
+	uint	len = 0;
+
+	len =  rtw_remainder_len(pfile);
+	len = (rlen > len) ? len : rlen;
+
+	if (rmem)
+		skb_copy_bits(pfile->pkt, pfile->buf_len - pfile->pkt_len, rmem, len);
+
+	pfile->cur_addr += len;
+	pfile->pkt_len -= len;
+	return len;
+}
+
+signed int rtw_endofpktfile(struct pkt_file *pfile)
+{
+	if (pfile->pkt_len == 0)
+		return true;
+	return false;
+}
+
+int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 alloc_sz, u8 flag)
+{
+	if (alloc_sz > 0) {
+		pxmitbuf->pallocated_buf = rtw_zmalloc(alloc_sz);
+		if (!pxmitbuf->pallocated_buf)
+			return _FAIL;
+
+		pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), XMITBUF_ALIGN_SZ);
+	}
+
+	return _SUCCESS;
+}
+
+void rtw_os_xmit_resource_free(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 free_sz, u8 flag)
+{
+	if (free_sz > 0)
+		kfree(pxmitbuf->pallocated_buf);
+}
+
+#define WMM_XMIT_THRESHOLD	(NR_XMITFRAME * 2 / 5)
+
+void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
+{
+	u16 queue;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	queue = skb_get_queue_mapping(pkt);
+	if (padapter->registrypriv.wifi_spec) {
+		if (__netif_subqueue_stopped(padapter->pnetdev, queue) &&
+		    (pxmitpriv->hwxmits[queue].accnt < WMM_XMIT_THRESHOLD))
+			netif_wake_subqueue(padapter->pnetdev, queue);
+	} else {
+		if (__netif_subqueue_stopped(padapter->pnetdev, queue))
+			netif_wake_subqueue(padapter->pnetdev, queue);
+	}
+
+	dev_kfree_skb_any(pkt);
+}
+
+void rtw_os_xmit_complete(struct adapter *padapter, struct xmit_frame *pxframe)
+{
+	if (pxframe->pkt)
+		rtw_os_pkt_complete(padapter, pxframe->pkt);
+
+	pxframe->pkt = NULL;
+}
+
+void rtw_os_xmit_schedule(struct adapter *padapter)
+{
+	struct adapter *pri_adapter = padapter;
+
+	if (!padapter)
+		return;
+
+	if (!list_empty(&padapter->xmitpriv.pending_xmitbuf_queue.queue))
+		complete(&pri_adapter->xmitpriv.xmit_comp);
+}
+
+static void rtw_check_xmit_resource(struct adapter *padapter, struct sk_buff *pkt)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	u16 queue;
+
+	queue = skb_get_queue_mapping(pkt);
+	if (padapter->registrypriv.wifi_spec) {
+		/* No free space for Tx, tx_worker is too slow */
+		if (pxmitpriv->hwxmits[queue].accnt > WMM_XMIT_THRESHOLD)
+			netif_stop_subqueue(padapter->pnetdev, queue);
+	} else {
+		if (pxmitpriv->free_xmitframe_cnt <= 4) {
+			if (!netif_tx_queue_stopped(netdev_get_tx_queue(padapter->pnetdev, queue)))
+				netif_stop_subqueue(padapter->pnetdev, queue);
+		}
+	}
+}
+
+static int rtw_mlcst2unicst(struct adapter *padapter, struct sk_buff *skb)
+{
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct list_head	*phead, *plist;
+	struct sk_buff *newskb;
+	struct sta_info *psta = NULL;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	u8 bc_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 null_addr[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	int i;
+	s32	res;
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	/* free sta asoc_queue */
+	list_for_each(plist, phead) {
+		int stainfo_offset;
+
+		psta = list_entry(plist, struct sta_info, asoc_list);
+
+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+		if (stainfo_offset_valid(stainfo_offset)) {
+			chk_alive_list[chk_alive_num++] = stainfo_offset;
+		}
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	for (i = 0; i < chk_alive_num; i++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+		if (!(psta->state & _FW_LINKED))
+			continue;
+
+		/* avoid come from STA1 and send back STA1 */
+		if (!memcmp(psta->hwaddr, &skb->data[6], 6) ||
+		    !memcmp(psta->hwaddr, null_addr, 6) ||
+		    !memcmp(psta->hwaddr, bc_addr, 6))
+			continue;
+
+		newskb = rtw_skb_copy(skb);
+
+		if (newskb) {
+			memcpy(newskb->data, psta->hwaddr, 6);
+			res = rtw_xmit(padapter, &newskb);
+			if (res < 0) {
+				pxmitpriv->tx_drop++;
+				dev_kfree_skb_any(newskb);
+			}
+		} else {
+			pxmitpriv->tx_drop++;
+			/* dev_kfree_skb_any(skb); */
+			return false;	/*  Caller shall tx this multicast frame via normal way. */
+		}
+	}
+
+	dev_kfree_skb_any(skb);
+	return true;
+}
+
+void _rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev)
+{
+	struct adapter *padapter = rtw_netdev_priv(pnetdev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	s32 res = 0;
+
+	if (rtw_if_up(padapter) == false)
+		goto drop_packet;
+
+	rtw_check_xmit_resource(padapter, pkt);
+
+	if (!rtw_mc2u_disable
+		&& check_fwstate(pmlmepriv, WIFI_AP_STATE) == true
+		&& (IP_MCAST_MAC(pkt->data)
+			|| ICMPV6_MCAST_MAC(pkt->data)
+			)
+		&& padapter->registrypriv.wifi_spec == 0) {
+		if (pxmitpriv->free_xmitframe_cnt > (NR_XMITFRAME / 4)) {
+			res = rtw_mlcst2unicst(padapter, pkt);
+			if (res)
+				return;
+		}
+	}
+
+	res = rtw_xmit(padapter, &pkt);
+	if (res < 0)
+		goto drop_packet;
+
+	return;
+
+drop_packet:
+	pxmitpriv->tx_drop++;
+	dev_kfree_skb_any(pkt);
+}
+
+netdev_tx_t rtw_xmit_entry(struct sk_buff *pkt, struct net_device *pnetdev)
+{
+	if (pkt)
+		_rtw_xmit_entry(pkt, pnetdev);
+
+	return NETDEV_TX_OK;
+}
diff --git a/drivers/staging/rts5208/Kconfig b/drivers/staging/rts5208/Kconfig
new file mode 100644
index 000000000..b864023d3
--- /dev/null
+++ b/drivers/staging/rts5208/Kconfig
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+config RTS5208
+	tristate "Realtek PCI-E Card Reader RTS5208/5288 support"
+	depends on PCI && SCSI
+	help
+	  Say Y here to include driver code to support the Realtek
+	  PCI-E card reader rts5208/rts5288.
+
+	  If this driver is compiled as a module, it will be named rts5208.
diff --git a/drivers/staging/rts5208/Makefile b/drivers/staging/rts5208/Makefile
new file mode 100644
index 000000000..3c9e9797d
--- /dev/null
+++ b/drivers/staging/rts5208/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_RTS5208) := rts5208.o
+
+rts5208-y := rtsx.o rtsx_chip.o rtsx_transport.o rtsx_scsi.o \
+	rtsx_card.o general.o sd.o xd.o ms.o spi.o
diff --git a/drivers/staging/rts5208/TODO b/drivers/staging/rts5208/TODO
new file mode 100644
index 000000000..9cec0d8dd
--- /dev/null
+++ b/drivers/staging/rts5208/TODO
@@ -0,0 +1,7 @@
+TODO:
+- use kernel coding style
+- checkpatch.pl fixes
+- We will use the stack in drivers/mmc to implement
+  rts5208/5288 in the future
+
+Micky Ching <micky_ching@realsil.com.cn>
diff --git a/drivers/staging/rts5208/general.c b/drivers/staging/rts5208/general.c
new file mode 100644
index 000000000..0f912b011
--- /dev/null
+++ b/drivers/staging/rts5208/general.c
@@ -0,0 +1,25 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include "general.h"
+
+int bit1cnt_long(u32 data)
+{
+	int i, cnt = 0;
+
+	for (i = 0; i < 32; i++) {
+		if (data & 0x01)
+			cnt++;
+		data >>= 1;
+	}
+	return cnt;
+}
+
diff --git a/drivers/staging/rts5208/general.h b/drivers/staging/rts5208/general.h
new file mode 100644
index 000000000..53e2dbabf
--- /dev/null
+++ b/drivers/staging/rts5208/general.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __RTSX_GENERAL_H
+#define __RTSX_GENERAL_H
+
+#include "rtsx.h"
+
+int bit1cnt_long(u32 data);
+
+#endif /* __RTSX_GENERAL_H */
diff --git a/drivers/staging/rts5208/ms.c b/drivers/staging/rts5208/ms.c
new file mode 100644
index 000000000..14449f8af
--- /dev/null
+++ b/drivers/staging/rts5208/ms.c
@@ -0,0 +1,4309 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include "rtsx.h"
+#include "ms.h"
+
+static inline void ms_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+
+	ms_card->err_code = err_code;
+}
+
+static inline int ms_check_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+
+	return (ms_card->err_code == err_code);
+}
+
+static int ms_parse_err_code(struct rtsx_chip *chip)
+{
+	return STATUS_FAIL;
+}
+
+static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
+			   u8 tpc, u8 cnt, u8 cfg)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	u8 *ptr;
+
+	dev_dbg(rtsx_dev(chip), "%s: tpc = 0x%x\n", __func__, tpc);
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER,
+		     0xFF, MS_TRANSFER_START | trans_mode);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+		     MS_TRANSFER_END, MS_TRANSFER_END);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		rtsx_clear_ms_error(chip);
+		ms_set_err_code(chip, MS_TO_ERROR);
+		return ms_parse_err_code(chip);
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if (!(tpc & 0x08)) {		/* Read Packet */
+		if (*ptr & MS_CRC16_ERR) {
+			ms_set_err_code(chip, MS_CRC16_ERROR);
+			return ms_parse_err_code(chip);
+		}
+	} else {			/* Write Packet */
+		if (CHK_MSPRO(ms_card) && !(*ptr & 0x80)) {
+			if (*ptr & (MS_INT_ERR | MS_INT_CMDNK)) {
+				ms_set_err_code(chip, MS_CMD_NK);
+				return ms_parse_err_code(chip);
+			}
+		}
+	}
+
+	if (*ptr & MS_RDY_TIMEOUT) {
+		rtsx_clear_ms_error(chip);
+		ms_set_err_code(chip, MS_TO_ERROR);
+		return ms_parse_err_code(chip);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
+			    u8 tpc, u16 sec_cnt, u8 cfg, bool mode_2k,
+			    int use_sg, void *buf, int buf_len)
+{
+	int retval;
+	u8 val, err_code = 0;
+	enum dma_data_direction dir;
+
+	if (!buf || !buf_len)
+		return STATUS_FAIL;
+
+	if (trans_mode == MS_TM_AUTO_READ) {
+		dir = DMA_FROM_DEVICE;
+		err_code = MS_FLASH_READ_ERROR;
+	} else if (trans_mode == MS_TM_AUTO_WRITE) {
+		dir = DMA_TO_DEVICE;
+		err_code = MS_FLASH_WRITE_ERROR;
+	} else {
+		return STATUS_FAIL;
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD,
+		     MS_SECTOR_CNT_H, 0xFF, (u8)(sec_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF, (u8)sec_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+
+	if (mode_2k) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD,
+			     MS_CFG, MS_2K_SECTOR_MODE, MS_2K_SECTOR_MODE);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE, 0);
+	}
+
+	trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD,
+		     MS_TRANSFER, 0xFF, MS_TRANSFER_START | trans_mode);
+	rtsx_add_cmd(chip, CHECK_REG_CMD,
+		     MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
+
+	rtsx_send_cmd_no_wait(chip);
+
+	retval = rtsx_transfer_data(chip, MS_CARD, buf, buf_len,
+				    use_sg, dir, chip->mspro_timeout);
+	if (retval < 0) {
+		ms_set_err_code(chip, err_code);
+		if (retval == -ETIMEDOUT)
+			retval = STATUS_TIMEDOUT;
+		else
+			retval = STATUS_FAIL;
+
+		return retval;
+	}
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval)
+		return retval;
+
+	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_bytes(struct rtsx_chip *chip,
+			  u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+
+	if (!data || data_len < cnt)
+		return STATUS_ERROR;
+
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < cnt; i++) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD,
+			     PPBUF_BASE2 + i, 0xFF, data[i]);
+	}
+	if (cnt % 2)
+		rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF, 0xFF);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD,
+		     MS_TRANSFER, 0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD,
+		     MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		u8 val = 0;
+
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		dev_dbg(rtsx_dev(chip), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		rtsx_clear_ms_error(chip);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				return ms_parse_err_code(chip);
+			}
+		} else {
+			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
+					ms_set_err_code(chip, MS_CMD_NK);
+					return ms_parse_err_code(chip);
+				}
+			}
+		}
+
+		if (val & MS_RDY_TIMEOUT) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			return ms_parse_err_code(chip);
+		}
+
+		ms_set_err_code(chip, MS_TO_ERROR);
+		return ms_parse_err_code(chip);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_bytes(struct rtsx_chip *chip,
+			 u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 *ptr;
+
+	if (!data)
+		return STATUS_ERROR;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+		     MS_TRANSFER_START | MS_TM_READ_BYTES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+		     MS_TRANSFER_END, MS_TRANSFER_END);
+
+	for (i = 0; i < data_len - 1; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
+
+	if (data_len % 2)
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0, 0);
+	else
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
+			     0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval < 0) {
+		u8 val = 0;
+
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		rtsx_clear_ms_error(chip);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				return ms_parse_err_code(chip);
+			}
+		} else {
+			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
+					ms_set_err_code(chip, MS_CMD_NK);
+					return ms_parse_err_code(chip);
+				}
+			}
+		}
+
+		if (val & MS_RDY_TIMEOUT) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			return ms_parse_err_code(chip);
+		}
+
+		ms_set_err_code(chip, MS_TO_ERROR);
+		return ms_parse_err_code(chip);
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	for (i = 0; i < data_len; i++)
+		data[i] = ptr[i];
+
+	if (tpc == PRO_READ_SHORT_DATA && data_len == 8) {
+		dev_dbg(rtsx_dev(chip), "Read format progress:\n");
+		print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, ptr,
+				     cnt);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_set_rw_reg_addr(struct rtsx_chip *chip, u8 read_start,
+			      u8 read_cnt, u8 write_start, u8 write_cnt)
+{
+	int retval, i;
+	u8 data[4];
+
+	data[0] = read_start;
+	data[1] = read_cnt;
+	data[2] = write_start;
+	data[3] = write_cnt;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, SET_RW_REG_ADRS, 4,
+					NO_WAIT_INT, data, 4);
+		if (retval == STATUS_SUCCESS)
+			return STATUS_SUCCESS;
+		rtsx_clear_ms_error(chip);
+	}
+
+	return STATUS_FAIL;
+}
+
+static int ms_send_cmd(struct rtsx_chip *chip, u8 cmd, u8 cfg)
+{
+	u8 data[2];
+
+	data[0] = cmd;
+	data[1] = 0;
+
+	return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
+}
+
+static int ms_set_init_para(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+	if (CHK_HG8BIT(ms_card)) {
+		if (chip->asic_code)
+			ms_card->ms_clock = chip->asic_ms_hg_clk;
+		else
+			ms_card->ms_clock = chip->fpga_ms_hg_clk;
+
+	} else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
+		if (chip->asic_code)
+			ms_card->ms_clock = chip->asic_ms_4bit_clk;
+		else
+			ms_card->ms_clock = chip->fpga_ms_4bit_clk;
+
+	} else {
+		if (chip->asic_code)
+			ms_card->ms_clock = chip->asic_ms_1bit_clk;
+		else
+			ms_card->ms_clock = chip->fpga_ms_1bit_clk;
+	}
+
+	retval = switch_clock(chip, ms_card->ms_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_clock(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = switch_clock(chip, ms_card->ms_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (CHECK_PID(chip, 0x5208)) {
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     MS_D1_PD | MS_D2_PD | MS_CLK_PD |
+					     MS_D6_PD);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     MS_D3_PD | MS_D0_PD | MS_BS_PD |
+					     XD_D4_PD);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     MS_D7_PD | XD_CE_PD | XD_CLE_PD |
+					     XD_CD_PU);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD | SD_D3_PD | SD_D2_PD |
+					     XD_ALE_PD);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU | SD_WP_PD | SD_CD_PU |
+					     SD_CMD_PD);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval)
+			return retval;
+
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval)
+				return retval;
+
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval)
+				return retval;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_pull_ctl_enable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	if (CHECK_PID(chip, 0x5208)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
+			     MS_D1_PD | MS_D2_PD | MS_CLK_NP | MS_D6_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
+			     MS_D3_PD | MS_D0_PD | MS_BS_NP | XD_D4_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
+			     MS_D7_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
+			     XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
+			     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
+			     MS_D5_PD | MS_D4_PD);
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD,
+				     CARD_PULL_CTL1, 0xFF, 0x55);
+			rtsx_add_cmd(chip, WRITE_REG_CMD,
+				     CARD_PULL_CTL2, 0xFF, 0x45);
+			rtsx_add_cmd(chip, WRITE_REG_CMD,
+				     CARD_PULL_CTL3, 0xFF, 0x4B);
+			rtsx_add_cmd(chip, WRITE_REG_CMD,
+				     CARD_PULL_CTL4, 0xFF, 0x29);
+		}
+	}
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_prepare_reset(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	u8 oc_mask = 0;
+
+	ms_card->ms_type = 0;
+	ms_card->check_ms_flow = 0;
+	ms_card->switch_8bit_fail = 0;
+	ms_card->delay_write.delay_write_flag = 0;
+
+	ms_card->pro_under_formatting = 0;
+
+	retval = ms_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (!chip->ft2_fast_mode)
+		wait_timeout(250);
+
+	retval = enable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (chip->asic_code) {
+		retval = ms_pull_ctl_enable(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_MS_PULL_CTL_BIT | 0x20, 0);
+		if (retval)
+			return retval;
+	}
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_on(chip, MS_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		wait_timeout(150);
+
+#ifdef SUPPORT_OCP
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+			oc_mask = MS_OC_NOW | MS_OC_EVER;
+		else
+			oc_mask = SD_OC_NOW | SD_OC_EVER;
+
+		if (chip->ocp_stat & oc_mask) {
+			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+				chip->ocp_stat);
+			return STATUS_FAIL;
+		}
+#endif
+	}
+
+	retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
+				     MS_OUTPUT_EN);
+	if (retval)
+		return retval;
+
+	if (chip->asic_code) {
+		retval = rtsx_write_register(chip, MS_CFG, 0xFF,
+					     SAMPLE_TIME_RISING |
+					     PUSH_TIME_DEFAULT |
+					     NO_EXTEND_TOGGLE |
+					     MS_BUS_WIDTH_1);
+		if (retval)
+			return retval;
+
+	} else {
+		retval = rtsx_write_register(chip, MS_CFG, 0xFF,
+					     SAMPLE_TIME_FALLING |
+					     PUSH_TIME_DEFAULT |
+					     NO_EXTEND_TOGGLE |
+					     MS_BUS_WIDTH_1);
+		if (retval)
+			return retval;
+	}
+	retval = rtsx_write_register(chip, MS_TRANS_CFG, 0xFF,
+				     NO_WAIT_INT | NO_AUTO_READ_INT_REG);
+	if (retval)
+		return retval;
+
+	retval = rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
+				     MS_STOP | MS_CLR_ERR);
+	if (retval)
+		return retval;
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 val;
+
+	retval = ms_set_rw_reg_addr(chip, PRO_STATUS_REG, 6, SYSTEM_PARAM, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG,
+					 6, NO_WAIT_INT);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 2, &val);
+	if (retval)
+		return retval;
+
+	dev_dbg(rtsx_dev(chip), "Type register: 0x%x\n", val);
+	if (val != 0x01) {
+		if (val != 0x02)
+			ms_card->check_ms_flow = 1;
+
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 4, &val);
+	if (retval)
+		return retval;
+
+	dev_dbg(rtsx_dev(chip), "Category register: 0x%x\n", val);
+	if (val != 0) {
+		ms_card->check_ms_flow = 1;
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 5, &val);
+	if (retval)
+		return retval;
+
+	dev_dbg(rtsx_dev(chip), "Class register: 0x%x\n", val);
+	if (val == 0) {
+		retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+		if (retval)
+			return retval;
+
+		if (val & WRT_PRTCT)
+			chip->card_wp |= MS_CARD;
+		else
+			chip->card_wp &= ~MS_CARD;
+
+	} else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
+		chip->card_wp |= MS_CARD;
+	} else {
+		ms_card->check_ms_flow = 1;
+		return STATUS_FAIL;
+	}
+
+	ms_card->ms_type |= TYPE_MSPRO;
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 3, &val);
+	if (retval)
+		return retval;
+
+	dev_dbg(rtsx_dev(chip), "IF Mode register: 0x%x\n", val);
+	if (val == 0) {
+		ms_card->ms_type &= 0x0F;
+	} else if (val == 7) {
+		if (switch_8bit_bus)
+			ms_card->ms_type |= MS_HG;
+		else
+			ms_card->ms_type &= 0x0F;
+
+	} else {
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
+{
+	int retval, i, k;
+	u8 val;
+
+	/* Confirm CPU StartUp */
+	k = 0;
+	do {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+			retval = ms_read_bytes(chip, GET_INT, 1,
+					       NO_WAIT_INT, &val, 1);
+			if (retval == STATUS_SUCCESS)
+				break;
+		}
+		if (i == MS_MAX_RETRY_COUNT)
+			return STATUS_FAIL;
+
+		if (k > 100)
+			return STATUS_FAIL;
+
+		k++;
+		wait_timeout(100);
+	} while (!(val & INT_REG_CED));
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_ERR) {
+		if (val & INT_REG_CMDNK)
+			chip->card_wp |= (MS_CARD);
+		else
+			return STATUS_FAIL;
+	}
+	/* --  end confirm CPU startup */
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_parallel_bus(struct rtsx_chip *chip)
+{
+	int retval, i;
+	u8 data[2];
+
+	data[0] = PARALLEL_4BIT_IF;
+	data[1] = 0;
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT,
+					data, 2);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_switch_8bit_bus(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 data[2];
+
+	data[0] = PARALLEL_8BIT_IF;
+	data[1] = 0;
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, WRITE_REG, 1,
+					NO_WAIT_INT, data, 2);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, MS_CFG, 0x98,
+				     MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
+	if (retval)
+		return retval;
+
+	ms_card->ms_type |= MS_8BIT;
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT,
+					 1, NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+
+	for (i = 0; i < 3; i++) {
+		retval = ms_prepare_reset(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_identify_media_type(chip, switch_8bit_bus);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_confirm_cpu_startup(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_switch_parallel_bus(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				ms_set_err_code(chip, MS_NO_CARD);
+				return STATUS_FAIL;
+			}
+			continue;
+		} else {
+			break;
+		}
+	}
+
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	/* Switch MS-PRO into Parallel mode */
+	retval = rtsx_write_register(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
+	if (retval)
+		return retval;
+
+	retval = rtsx_write_register(chip, MS_CFG, PUSH_TIME_ODD,
+				     PUSH_TIME_ODD);
+	if (retval)
+		return retval;
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	/* If MSPro HG Card, We shall try to switch to 8-bit bus */
+	if (CHK_MSHG(ms_card) && chip->support_ms_8bit && switch_8bit_bus) {
+		retval = ms_switch_8bit_bus(chip);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->switch_8bit_fail = 1;
+			return STATUS_FAIL;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef XC_POWERCLASS
+static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
+{
+	int retval;
+	u8 buf[6];
+
+	ms_cleanup_work(chip);
+
+	retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_DATA_COUNT1, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf[0] = 0;
+	buf[1] = mode;
+	buf[2] = 0;
+	buf[3] = 0;
+	buf[4] = 0;
+	buf[5] = 0;
+
+	retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, buf);
+	if (retval)
+		return retval;
+
+	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int ms_read_attribute_info(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 val, *buf, class_code, device_type, sub_class, data[16];
+	u16 total_blk = 0, blk_size = 0;
+#ifdef SUPPORT_MSXC
+	u32 xc_total_blk = 0, xc_blk_size = 0;
+#endif
+	u32 sys_info_addr = 0, sys_info_size;
+#ifdef SUPPORT_PCGL_1P18
+	u32 model_name_addr = 0, model_name_size;
+	int found_sys_info = 0, found_model_name = 0;
+#endif
+
+	retval = ms_set_rw_reg_addr(chip, PRO_INT_REG, 2, PRO_SYSTEM_PARAM, 7);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_MS8BIT(ms_card))
+		data[0] = PARALLEL_8BIT_IF;
+	else
+		data[0] = PARALLEL_4BIT_IF;
+
+	data[1] = 0;
+
+	data[2] = 0x40;
+	data[3] = 0;
+	data[4] = 0;
+	data[5] = 0;
+	data[6] = 0;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT,
+					data, 8);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf = kmalloc(64 * 512, GFP_KERNEL);
+	if (!buf)
+		return STATUS_ERROR;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			continue;
+
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		if (retval != STATUS_SUCCESS) {
+			kfree(buf);
+			return STATUS_FAIL;
+		}
+		if (!(val & MS_INT_BREQ)) {
+			kfree(buf);
+			return STATUS_FAIL;
+		}
+		retval = ms_transfer_data(chip, MS_TM_AUTO_READ,
+					  PRO_READ_LONG_DATA, 0x40, WAIT_INT,
+					  0, 0, buf, 64 * 512);
+		if (retval == STATUS_SUCCESS)
+			break;
+
+		rtsx_clear_ms_error(chip);
+	}
+	if (retval != STATUS_SUCCESS) {
+		kfree(buf);
+		return STATUS_FAIL;
+	}
+
+	i = 0;
+	do {
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		if (retval != STATUS_SUCCESS) {
+			kfree(buf);
+			return STATUS_FAIL;
+		}
+
+		if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
+			break;
+
+		retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
+					 PRO_READ_LONG_DATA, 0, WAIT_INT);
+		if (retval != STATUS_SUCCESS) {
+			kfree(buf);
+			return STATUS_FAIL;
+		}
+
+		i++;
+	} while (i < 1024);
+
+	if (buf[0] != 0xa5 && buf[1] != 0xc3) {
+		/* Signature code is wrong */
+		kfree(buf);
+		return STATUS_FAIL;
+	}
+
+	if (buf[4] < 1 || buf[4] > 12) {
+		kfree(buf);
+		return STATUS_FAIL;
+	}
+
+	for (i = 0; i < buf[4]; i++) {
+		int cur_addr_off = 16 + i * 12;
+
+#ifdef SUPPORT_MSXC
+		if (buf[cur_addr_off + 8] == 0x10 ||
+		    buf[cur_addr_off + 8] == 0x13) {
+#else
+		if (buf[cur_addr_off + 8] == 0x10) {
+#endif
+			sys_info_addr = ((u32)buf[cur_addr_off + 0] << 24) |
+				((u32)buf[cur_addr_off + 1] << 16) |
+				((u32)buf[cur_addr_off + 2] << 8) |
+				buf[cur_addr_off + 3];
+			sys_info_size = ((u32)buf[cur_addr_off + 4] << 24) |
+				((u32)buf[cur_addr_off + 5] << 16) |
+				((u32)buf[cur_addr_off + 6] << 8) |
+				buf[cur_addr_off + 7];
+			dev_dbg(rtsx_dev(chip), "sys_info_addr = 0x%x, sys_info_size = 0x%x\n",
+				sys_info_addr, sys_info_size);
+			if (sys_info_size != 96)  {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+			if (sys_info_addr < 0x1A0) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+			if ((sys_info_size + sys_info_addr) > 0x8000) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+#ifdef SUPPORT_MSXC
+			if (buf[cur_addr_off + 8] == 0x13)
+				ms_card->ms_type |= MS_XC;
+#endif
+#ifdef SUPPORT_PCGL_1P18
+			found_sys_info = 1;
+#else
+			break;
+#endif
+		}
+#ifdef SUPPORT_PCGL_1P18
+		if (buf[cur_addr_off + 8] == 0x15) {
+			model_name_addr = ((u32)buf[cur_addr_off + 0] << 24) |
+				((u32)buf[cur_addr_off + 1] << 16) |
+				((u32)buf[cur_addr_off + 2] << 8) |
+				buf[cur_addr_off + 3];
+			model_name_size = ((u32)buf[cur_addr_off + 4] << 24) |
+				((u32)buf[cur_addr_off + 5] << 16) |
+				((u32)buf[cur_addr_off + 6] << 8) |
+				buf[cur_addr_off + 7];
+			dev_dbg(rtsx_dev(chip), "model_name_addr = 0x%x, model_name_size = 0x%x\n",
+				model_name_addr, model_name_size);
+			if (model_name_size != 48)  {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+			if (model_name_addr < 0x1A0) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+			if ((model_name_size + model_name_addr) > 0x8000) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			found_model_name = 1;
+		}
+
+		if (found_sys_info && found_model_name)
+			break;
+#endif
+	}
+
+	if (i == buf[4]) {
+		kfree(buf);
+		return STATUS_FAIL;
+	}
+
+	class_code =  buf[sys_info_addr + 0];
+	device_type = buf[sys_info_addr + 56];
+	sub_class = buf[sys_info_addr + 46];
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		xc_total_blk = ((u32)buf[sys_info_addr + 6] << 24) |
+				((u32)buf[sys_info_addr + 7] << 16) |
+				((u32)buf[sys_info_addr + 8] << 8) |
+				buf[sys_info_addr + 9];
+		xc_blk_size = ((u32)buf[sys_info_addr + 32] << 24) |
+				((u32)buf[sys_info_addr + 33] << 16) |
+				((u32)buf[sys_info_addr + 34] << 8) |
+				buf[sys_info_addr + 35];
+		dev_dbg(rtsx_dev(chip), "xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
+			xc_total_blk, xc_blk_size);
+	} else {
+		total_blk = ((u16)buf[sys_info_addr + 6] << 8) |
+			buf[sys_info_addr + 7];
+		blk_size = ((u16)buf[sys_info_addr + 2] << 8) |
+			buf[sys_info_addr + 3];
+		dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
+			total_blk, blk_size);
+	}
+#else
+	total_blk = ((u16)buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
+	blk_size = ((u16)buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
+	dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
+		total_blk, blk_size);
+#endif
+
+	dev_dbg(rtsx_dev(chip), "class_code = 0x%x, device_type = 0x%x, sub_class = 0x%x\n",
+		class_code, device_type, sub_class);
+
+	memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
+#ifdef SUPPORT_PCGL_1P18
+	memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
+#endif
+
+	kfree(buf);
+
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		if (class_code != 0x03)
+			return STATUS_FAIL;
+	} else {
+		if (class_code != 0x02)
+			return STATUS_FAIL;
+	}
+#else
+	if (class_code != 0x02)
+		return STATUS_FAIL;
+#endif
+
+	if (device_type != 0x00) {
+		if (device_type == 0x01 || device_type == 0x02 ||
+		    device_type == 0x03) {
+			chip->card_wp |= MS_CARD;
+		} else {
+			return STATUS_FAIL;
+		}
+	}
+
+	if (sub_class & 0xC0)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
+		class_code, device_type, sub_class);
+
+#ifdef SUPPORT_MSXC
+	if (CHK_MSXC(ms_card)) {
+		chip->capacity[chip->card2lun[MS_CARD]] =
+			ms_card->capacity = xc_total_blk * xc_blk_size;
+	} else {
+		chip->capacity[chip->card2lun[MS_CARD]] =
+			ms_card->capacity = total_blk * blk_size;
+	}
+#else
+	ms_card->capacity = total_blk * blk_size;
+	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef SUPPORT_MAGIC_GATE
+static int mg_set_tpc_para_sub(struct rtsx_chip *chip,
+			       int type, u8 mg_entry_num);
+#endif
+
+static int reset_ms_pro(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+#ifdef XC_POWERCLASS
+	u8 change_power_class;
+
+	if (chip->ms_power_class_en & 0x02)
+		change_power_class = 2;
+	else if (chip->ms_power_class_en & 0x01)
+		change_power_class = 1;
+	else
+		change_power_class = 0;
+#endif
+
+#ifdef XC_POWERCLASS
+retry:
+#endif
+	retval = ms_pro_reset_flow(chip, 1);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->switch_8bit_fail) {
+			retval = ms_pro_reset_flow(chip, 0);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		} else {
+			return STATUS_FAIL;
+		}
+	}
+
+	retval = ms_read_attribute_info(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+#ifdef XC_POWERCLASS
+	if (CHK_HG8BIT(ms_card))
+		change_power_class = 0;
+
+	if (change_power_class && CHK_MSXC(ms_card)) {
+		u8 power_class_en = chip->ms_power_class_en;
+
+		dev_dbg(rtsx_dev(chip), "power_class_en = 0x%x\n",
+			power_class_en);
+		dev_dbg(rtsx_dev(chip), "change_power_class = %d\n",
+			change_power_class);
+
+		if (change_power_class)
+			power_class_en &= (1 << (change_power_class - 1));
+		else
+			power_class_en = 0;
+
+		if (power_class_en) {
+			u8 power_class_mode =
+				(ms_card->raw_sys_info[46] & 0x18) >> 3;
+			dev_dbg(rtsx_dev(chip), "power_class_mode = 0x%x",
+				power_class_mode);
+			if (change_power_class > power_class_mode)
+				change_power_class = power_class_mode;
+			if (change_power_class) {
+				retval = msxc_change_power(chip,
+							   change_power_class);
+				if (retval != STATUS_SUCCESS) {
+					change_power_class--;
+					goto retry;
+				}
+			}
+		}
+	}
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	retval = mg_set_tpc_para_sub(chip, 0, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+#endif
+
+	if (CHK_HG8BIT(ms_card))
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
+	else
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_status_reg(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val[2];
+
+	retval = ms_set_rw_reg_addr(chip, STATUS_REG0, 2, 0, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
+		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_extra_data(struct rtsx_chip *chip,
+			      u16 block_addr, u8 page_num, u8 *buf, int buf_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 val, data[10];
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_MS4BIT(ms_card)) {
+		/* Parallel interface */
+		data[0] = 0x88;
+	} else {
+		/* Serial interface */
+		data[0] = 0x80;
+	}
+	data[1] = 0;
+	data[2] = (u8)(block_addr >> 8);
+	data[3] = (u8)block_addr;
+	data[4] = 0x40;
+	data[5] = page_num;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
+					data, 6);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		return STATUS_FAIL;
+	}
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			retval = ms_read_status_reg(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
+						    MS_EXTRA_SIZE, SYSTEM_PARAM,
+						    6);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		}
+	}
+
+	retval = ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT,
+			       data, MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (buf && buf_len) {
+		if (buf_len > MS_EXTRA_SIZE)
+			buf_len = MS_EXTRA_SIZE;
+		memcpy(buf, data, buf_len);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_extra_data(struct rtsx_chip *chip, u16 block_addr,
+			       u8 page_num, u8 *buf, int buf_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 val, data[16];
+
+	if (!buf || buf_len < MS_EXTRA_SIZE)
+		return STATUS_FAIL;
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 6 + MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(block_addr >> 8);
+	data[3] = (u8)block_addr;
+	data[4] = 0x40;
+	data[5] = page_num;
+
+	for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
+		data[i] = buf[i - 6];
+
+	retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
+				NO_WAIT_INT, data, 16);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		return STATUS_FAIL;
+	}
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			return STATUS_FAIL;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	u8 val, data[6];
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(block_addr >> 8);
+	data[3] = (u8)block_addr;
+	data[4] = 0x20;
+	data[5] = page_num;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		return STATUS_FAIL;
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip,  MS_FLASH_READ_ERROR);
+				return STATUS_FAIL;
+			}
+			retval = ms_read_status_reg(chip);
+			if (retval != STATUS_SUCCESS)
+				ms_set_err_code(chip,  MS_FLASH_WRITE_ERROR);
+
+		} else {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				return STATUS_FAIL;
+			}
+		}
+	}
+
+	retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA,
+				 0, NO_WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	u8 val, data[8], extra[MS_EXTRA_SIZE];
+
+	retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 7);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(phy_blk >> 8);
+	data[3] = (u8)phy_blk;
+	data[4] = 0x80;
+	data[5] = 0;
+	data[6] = extra[0] & 0x7F;
+	data[7] = 0xFF;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 7);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_CMDNK) {
+		ms_set_err_code(chip, MS_CMD_NK);
+		return STATUS_FAIL;
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			return STATUS_FAIL;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i = 0;
+	u8 val, data[6];
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(phy_blk >> 8);
+	data[3] = (u8)phy_blk;
+	data[4] = 0;
+	data[5] = 0;
+
+	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+ERASE_RTY:
+	retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (val & INT_REG_CMDNK) {
+		if (i < 3) {
+			i++;
+			goto ERASE_RTY;
+		}
+
+		ms_set_err_code(chip, MS_CMD_NK);
+		ms_set_bad_block(chip, phy_blk);
+		return STATUS_FAIL;
+	}
+
+	if (val & INT_REG_CED) {
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			return STATUS_FAIL;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
+{
+	if (!extra || extra_len < MS_EXTRA_SIZE)
+		return;
+
+	memset(extra, 0xFF, MS_EXTRA_SIZE);
+
+	if (type == set_PS_NG) {
+		/* set page status as 1:NG,and block status keep 1:OK */
+		extra[0] = 0xB8;
+	} else {
+		/* set page status as 0:Data Error,and block status keep 1:OK */
+		extra[0] = 0x98;
+	}
+
+	extra[2] = (u8)(log_blk >> 8);
+	extra[3] = (u8)log_blk;
+}
+
+static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk,
+			u8 start_page, u8 end_page)
+{
+	int retval;
+	u8 extra[MS_EXTRA_SIZE], i;
+
+	memset(extra, 0xff, MS_EXTRA_SIZE);
+
+	extra[0] = 0xf8;	/* Block, page OK, data erased */
+	extra[1] = 0xff;
+	extra[2] = (u8)(log_blk >> 8);
+	extra[3] = (u8)log_blk;
+
+	for (i = start_page; i < end_page; i++) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = ms_write_extra_data(chip, phy_blk, i,
+					     extra, MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			u16 log_blk, u8 start_page, u8 end_page)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	bool uncorrect_flag = false;
+	int retval, rty_cnt;
+	u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
+
+	dev_dbg(rtsx_dev(chip), "Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
+		old_blk, new_blk, log_blk);
+	dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d\n",
+		start_page, end_page);
+
+	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_read_status_reg(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+	if (retval)
+		return retval;
+
+	if (val & BUF_FULL) {
+		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (!(val & INT_REG_CED)) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			return STATUS_FAIL;
+		}
+	}
+
+	for (i = start_page; i < end_page; i++) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = ms_read_extra_data(chip, old_blk, i, extra,
+					    MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
+					    MS_EXTRA_SIZE, SYSTEM_PARAM, 6);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (CHK_MS4BIT(ms_card))
+			data[0] = 0x88;
+		else
+			data[0] = 0x80;
+
+		data[1] = 0;
+		data[2] = (u8)(old_blk >> 8);
+		data[3] = (u8)old_blk;
+		data[4] = 0x20;
+		data[5] = i;
+
+		retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
+					data, 6);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			return STATUS_FAIL;
+		}
+
+		if (val & INT_REG_CED) {
+			if (val & INT_REG_ERR) {
+				retval = ms_read_status_reg(chip);
+				if (retval != STATUS_SUCCESS) {
+					uncorrect_flag = true;
+					dev_dbg(rtsx_dev(chip), "Uncorrectable error\n");
+				} else {
+					uncorrect_flag = false;
+				}
+
+				retval = ms_transfer_tpc(chip,
+							 MS_TM_NORMAL_READ,
+							 READ_PAGE_DATA,
+							 0, NO_WAIT_INT);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+
+				if (uncorrect_flag) {
+					ms_set_page_status(log_blk, set_PS_NG,
+							   extra,
+							   MS_EXTRA_SIZE);
+					if (i == 0)
+						extra[0] &= 0xEF;
+
+					ms_write_extra_data(chip, old_blk, i,
+							    extra,
+							    MS_EXTRA_SIZE);
+					dev_dbg(rtsx_dev(chip), "page %d : extra[0] = 0x%x\n",
+						i, extra[0]);
+					MS_SET_BAD_BLOCK_FLG(ms_card);
+
+					ms_set_page_status(log_blk,
+							   set_PS_error, extra,
+							   MS_EXTRA_SIZE);
+					ms_write_extra_data(chip, new_blk, i,
+							    extra,
+							    MS_EXTRA_SIZE);
+					continue;
+				}
+
+				for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT;
+				     rty_cnt++) {
+					retval = ms_transfer_tpc(chip,
+								 MS_TM_NORMAL_WRITE,
+								 WRITE_PAGE_DATA,
+								 0, NO_WAIT_INT);
+					if (retval == STATUS_SUCCESS)
+						break;
+				}
+				if (rty_cnt == MS_MAX_RETRY_COUNT)
+					return STATUS_FAIL;
+			}
+
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				return STATUS_FAIL;
+			}
+		}
+
+		retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+					    SYSTEM_PARAM, (6 + MS_EXTRA_SIZE));
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (CHK_MS4BIT(ms_card))
+			data[0] = 0x88;
+		else
+			data[0] = 0x80;
+
+		data[1] = 0;
+		data[2] = (u8)(new_blk >> 8);
+		data[3] = (u8)new_blk;
+		data[4] = 0x20;
+		data[5] = i;
+
+		if ((extra[0] & 0x60) != 0x60)
+			data[6] = extra[0];
+		else
+			data[6] = 0xF8;
+
+		data[6 + 1] = 0xFF;
+		data[6 + 2] = (u8)(log_blk >> 8);
+		data[6 + 3] = (u8)log_blk;
+
+		for (j = 4; j <= MS_EXTRA_SIZE; j++)
+			data[6 + j] = 0xFF;
+
+		retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
+					NO_WAIT_INT, data, 16);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			return STATUS_FAIL;
+		}
+
+		if (val & INT_REG_CED) {
+			if (val & INT_REG_ERR) {
+				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+				return STATUS_FAIL;
+			}
+		}
+
+		if (i == 0) {
+			retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
+						    MS_EXTRA_SIZE, SYSTEM_PARAM,
+						    7);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			ms_set_err_code(chip, MS_NO_ERROR);
+
+			if (CHK_MS4BIT(ms_card))
+				data[0] = 0x88;
+			else
+				data[0] = 0x80;
+
+			data[1] = 0;
+			data[2] = (u8)(old_blk >> 8);
+			data[3] = (u8)old_blk;
+			data[4] = 0x80;
+			data[5] = 0;
+			data[6] = 0xEF;
+			data[7] = 0xFF;
+
+			retval = ms_write_bytes(chip, WRITE_REG, 7,
+						NO_WAIT_INT, data, 8);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			ms_set_err_code(chip, MS_NO_ERROR);
+			retval = ms_read_bytes(chip, GET_INT, 1,
+					       NO_WAIT_INT, &val, 1);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			if (val & INT_REG_CMDNK) {
+				ms_set_err_code(chip, MS_CMD_NK);
+				return STATUS_FAIL;
+			}
+
+			if (val & INT_REG_CED) {
+				if (val & INT_REG_ERR) {
+					ms_set_err_code(chip,
+							MS_FLASH_WRITE_ERROR);
+					return STATUS_FAIL;
+				}
+			}
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_ms(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	u16 i, reg_addr, block_size;
+	u8 val, extra[MS_EXTRA_SIZE], j, *ptr;
+#ifndef SUPPORT_MAGIC_GATE
+	u16 eblock_cnt;
+#endif
+
+	retval = ms_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_card->ms_type |= TYPE_MS;
+
+	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_read_status_reg(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+	if (retval)
+		return retval;
+
+	if (val & WRT_PRTCT)
+		chip->card_wp |= MS_CARD;
+	else
+		chip->card_wp &= ~MS_CARD;
+
+	i = 0;
+
+RE_SEARCH:
+	/* Search Boot Block */
+	while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = ms_read_extra_data(chip, i, 0, extra, MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			i++;
+			continue;
+		}
+
+		if (extra[0] & BLOCK_OK) {
+			if (!(extra[1] & NOT_BOOT_BLOCK)) {
+				ms_card->boot_block = i;
+				break;
+			}
+		}
+		i++;
+	}
+
+	if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
+		dev_dbg(rtsx_dev(chip), "No boot block found!");
+		return STATUS_FAIL;
+	}
+
+	for (j = 0; j < 3; j++) {
+		retval = ms_read_page(chip, ms_card->boot_block, j);
+		if (retval != STATUS_SUCCESS) {
+			if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
+				i = ms_card->boot_block + 1;
+				ms_set_err_code(chip, MS_NO_ERROR);
+				goto RE_SEARCH;
+			}
+		}
+	}
+
+	retval = ms_read_page(chip, ms_card->boot_block, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	/* Read MS system information as sys_info */
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < 96; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	ptr = rtsx_get_cmd_data(chip);
+	memcpy(ms_card->raw_sys_info, ptr, 96);
+
+	/* Read useful block contents */
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
+
+	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
+	     reg_addr++)
+		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
+		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_device_type, 0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_support, 0, 0);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	ptr = rtsx_get_cmd_data(chip);
+
+	dev_dbg(rtsx_dev(chip), "Boot block data:\n");
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 16, ptr);
+
+	/* Block ID error
+	 * HEADER_ID0, HEADER_ID1
+	 */
+	if (ptr[0] != 0x00 || ptr[1] != 0x01) {
+		i = ms_card->boot_block + 1;
+		goto RE_SEARCH;
+	}
+
+	/* Page size error
+	 * PAGE_SIZE_0, PAGE_SIZE_1
+	 */
+	if (ptr[12] != 0x02 || ptr[13] != 0x00) {
+		i = ms_card->boot_block + 1;
+		goto RE_SEARCH;
+	}
+
+	if (ptr[14] == 1 || ptr[14] == 3)
+		chip->card_wp |= MS_CARD;
+
+	/* BLOCK_SIZE_0, BLOCK_SIZE_1 */
+	block_size = ((u16)ptr[6] << 8) | ptr[7];
+	if (block_size == 0x0010) {
+		/* Block size 16KB */
+		ms_card->block_shift = 5;
+		ms_card->page_off = 0x1F;
+	} else if (block_size == 0x0008) {
+		/* Block size 8KB */
+		ms_card->block_shift = 4;
+		ms_card->page_off = 0x0F;
+	}
+
+	/* BLOCK_COUNT_0, BLOCK_COUNT_1 */
+	ms_card->total_block = ((u16)ptr[8] << 8) | ptr[9];
+
+#ifdef SUPPORT_MAGIC_GATE
+	j = ptr[10];
+
+	if (ms_card->block_shift == 4)  { /* 4MB or 8MB */
+		if (j < 2)  { /* Effective block for 4MB: 0x1F0 */
+			ms_card->capacity = 0x1EE0;
+		} else { /* Effective block for 8MB: 0x3E0 */
+			ms_card->capacity = 0x3DE0;
+		}
+	} else  { /* 16MB, 32MB, 64MB or 128MB */
+		if (j < 5)  { /* Effective block for 16MB: 0x3E0 */
+			ms_card->capacity = 0x7BC0;
+		} else if (j < 0xA) { /* Effective block for 32MB: 0x7C0 */
+			ms_card->capacity = 0xF7C0;
+		} else if (j < 0x11) { /* Effective block for 64MB: 0xF80 */
+			ms_card->capacity = 0x1EF80;
+		} else { /* Effective block for 128MB: 0x1F00 */
+			ms_card->capacity = 0x3DF00;
+		}
+	}
+#else
+	/* EBLOCK_COUNT_0, EBLOCK_COUNT_1 */
+	eblock_cnt = ((u16)ptr[10] << 8) | ptr[11];
+
+	ms_card->capacity = ((u32)eblock_cnt - 2) << ms_card->block_shift;
+#endif
+
+	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
+
+	/* Switch I/F Mode */
+	if (ptr[15]) {
+		retval = ms_set_rw_reg_addr(chip, 0, 0, SYSTEM_PARAM, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = rtsx_write_register(chip, PPBUF_BASE2, 0xFF, 0x88);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, PPBUF_BASE2 + 1, 0xFF, 0);
+		if (retval)
+			return retval;
+
+		retval = ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
+					 NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = rtsx_write_register(chip, MS_CFG,
+					     0x58 | MS_NO_CHECK_INT,
+					     MS_BUS_WIDTH_4 |
+					     PUSH_TIME_ODD |
+					     MS_NO_CHECK_INT);
+		if (retval)
+			return retval;
+
+		ms_card->ms_type |= MS_4BIT;
+	}
+
+	if (CHK_MS4BIT(ms_card))
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
+	else
+		chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_init_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int size, i, seg_no, retval;
+	u16 defect_block, reg_addr;
+	u8 val1, val2;
+
+	ms_card->segment_cnt = ms_card->total_block >> 9;
+	dev_dbg(rtsx_dev(chip), "ms_card->segment_cnt = %d\n",
+		ms_card->segment_cnt);
+
+	size = ms_card->segment_cnt * sizeof(struct zone_entry);
+	ms_card->segment = vzalloc(size);
+	if (!ms_card->segment)
+		return STATUS_FAIL;
+
+	retval = ms_read_page(chip, ms_card->boot_block, 1);
+	if (retval != STATUS_SUCCESS)
+		goto INIT_FAIL;
+
+	reg_addr = PPBUF_BASE2;
+	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
+		int block_no;
+
+		retval = rtsx_read_register(chip, reg_addr++, &val1);
+		if (retval != STATUS_SUCCESS)
+			goto INIT_FAIL;
+
+		retval = rtsx_read_register(chip, reg_addr++, &val2);
+		if (retval != STATUS_SUCCESS)
+			goto INIT_FAIL;
+
+		defect_block = ((u16)val1 << 8) | val2;
+		if (defect_block == 0xFFFF)
+			break;
+
+		seg_no = defect_block / 512;
+
+		block_no = ms_card->segment[seg_no].disable_count++;
+		ms_card->segment[seg_no].defect_list[block_no] = defect_block;
+	}
+
+	for (i = 0; i < ms_card->segment_cnt; i++) {
+		ms_card->segment[i].build_flag = 0;
+		ms_card->segment[i].l2p_table = NULL;
+		ms_card->segment[i].free_table = NULL;
+		ms_card->segment[i].get_index = 0;
+		ms_card->segment[i].set_index = 0;
+		ms_card->segment[i].unused_blk_cnt = 0;
+
+		dev_dbg(rtsx_dev(chip), "defective block count of segment %d is %d\n",
+			i, ms_card->segment[i].disable_count);
+	}
+
+	return STATUS_SUCCESS;
+
+INIT_FAIL:
+	vfree(ms_card->segment);
+	ms_card->segment = NULL;
+
+	return STATUS_FAIL;
+}
+
+static u16 ms_get_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+
+	if (!ms_card->segment)
+		return 0xFFFF;
+
+	segment = &ms_card->segment[seg_no];
+
+	if (segment->l2p_table)
+		return segment->l2p_table[log_off];
+
+	return 0xFFFF;
+}
+
+static void ms_set_l2p_tbl(struct rtsx_chip *chip,
+			   int seg_no, u16 log_off, u16 phy_blk)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+
+	if (!ms_card->segment)
+		return;
+
+	segment = &ms_card->segment[seg_no];
+	if (segment->l2p_table)
+		segment->l2p_table[log_off] = phy_blk;
+}
+
+static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+	int seg_no;
+
+	seg_no = (int)phy_blk >> 9;
+	segment = &ms_card->segment[seg_no];
+
+	segment->free_table[segment->set_index++] = phy_blk;
+	if (segment->set_index >= MS_FREE_TABLE_CNT)
+		segment->set_index = 0;
+
+	segment->unused_blk_cnt++;
+}
+
+static u16 ms_get_unused_block(struct rtsx_chip *chip, int seg_no)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+	u16 phy_blk;
+
+	segment = &ms_card->segment[seg_no];
+
+	if (segment->unused_blk_cnt <= 0)
+		return 0xFFFF;
+
+	phy_blk = segment->free_table[segment->get_index];
+	segment->free_table[segment->get_index++] = 0xFFFF;
+	if (segment->get_index >= MS_FREE_TABLE_CNT)
+		segment->get_index = 0;
+
+	segment->unused_blk_cnt--;
+
+	return phy_blk;
+}
+
+static const unsigned short ms_start_idx[] = {0, 494, 990, 1486, 1982, 2478,
+					      2974, 3470, 3966, 4462, 4958,
+					      5454, 5950, 6446, 6942, 7438,
+					      7934};
+
+static int ms_arbitrate_l2p(struct rtsx_chip *chip, u16 phy_blk,
+			    u16 log_off, u8 us1, u8 us2)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+	int seg_no;
+	u16 tmp_blk;
+
+	seg_no = (int)phy_blk >> 9;
+	segment = &ms_card->segment[seg_no];
+	tmp_blk = segment->l2p_table[log_off];
+
+	if (us1 != us2) {
+		if (us1 == 0) {
+			if (!(chip->card_wp & MS_CARD))
+				ms_erase_block(chip, tmp_blk);
+
+			ms_set_unused_block(chip, tmp_blk);
+			segment->l2p_table[log_off] = phy_blk;
+		} else {
+			if (!(chip->card_wp & MS_CARD))
+				ms_erase_block(chip, phy_blk);
+
+			ms_set_unused_block(chip, phy_blk);
+		}
+	} else {
+		if (phy_blk < tmp_blk) {
+			if (!(chip->card_wp & MS_CARD))
+				ms_erase_block(chip, phy_blk);
+
+			ms_set_unused_block(chip, phy_blk);
+		} else {
+			if (!(chip->card_wp & MS_CARD))
+				ms_erase_block(chip, tmp_blk);
+
+			ms_set_unused_block(chip, tmp_blk);
+			segment->l2p_table[log_off] = phy_blk;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct zone_entry *segment;
+	bool defect_flag;
+	int retval, table_size, disable_cnt, i;
+	u16 start, end, phy_blk, log_blk, tmp_blk, idx;
+	u8 extra[MS_EXTRA_SIZE], us1, us2;
+
+	dev_dbg(rtsx_dev(chip), "%s: %d\n", __func__, seg_no);
+
+	if (!ms_card->segment) {
+		retval = ms_init_l2p_tbl(chip);
+		if (retval != STATUS_SUCCESS)
+			return retval;
+	}
+
+	if (ms_card->segment[seg_no].build_flag) {
+		dev_dbg(rtsx_dev(chip), "l2p table of segment %d has been built\n",
+			seg_no);
+		return STATUS_SUCCESS;
+	}
+
+	if (seg_no == 0)
+		table_size = 494;
+	else
+		table_size = 496;
+
+	segment = &ms_card->segment[seg_no];
+
+	if (!segment->l2p_table) {
+		segment->l2p_table = vmalloc(array_size(table_size, 2));
+		if (!segment->l2p_table)
+			goto BUILD_FAIL;
+	}
+	memset((u8 *)(segment->l2p_table), 0xff, array_size(table_size, 2));
+
+	if (!segment->free_table) {
+		segment->free_table = vmalloc(array_size(MS_FREE_TABLE_CNT, 2));
+		if (!segment->free_table)
+			goto BUILD_FAIL;
+	}
+	memset((u8 *)(segment->free_table), 0xff, array_size(MS_FREE_TABLE_CNT, 2));
+
+	start = (u16)seg_no << 9;
+	end = (u16)(seg_no + 1) << 9;
+
+	disable_cnt = segment->disable_count;
+
+	segment->get_index = 0;
+	segment->set_index = 0;
+	segment->unused_blk_cnt = 0;
+
+	for (phy_blk = start; phy_blk < end; phy_blk++) {
+		if (disable_cnt) {
+			defect_flag = false;
+			for (i = 0; i < segment->disable_count; i++) {
+				if (phy_blk == segment->defect_list[i]) {
+					defect_flag = true;
+					break;
+				}
+			}
+			if (defect_flag) {
+				disable_cnt--;
+				continue;
+			}
+		}
+
+		retval = ms_read_extra_data(chip, phy_blk, 0,
+					    extra, MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS) {
+			dev_dbg(rtsx_dev(chip), "read extra data fail\n");
+			ms_set_bad_block(chip, phy_blk);
+			continue;
+		}
+
+		if (seg_no == ms_card->segment_cnt - 1) {
+			if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
+				if (!(chip->card_wp & MS_CARD)) {
+					retval = ms_erase_block(chip, phy_blk);
+					if (retval != STATUS_SUCCESS)
+						continue;
+					extra[2] = 0xff;
+					extra[3] = 0xff;
+				}
+			}
+		}
+
+		if (!(extra[0] & BLOCK_OK))
+			continue;
+		if (!(extra[1] & NOT_BOOT_BLOCK))
+			continue;
+		if ((extra[0] & PAGE_OK) != PAGE_OK)
+			continue;
+
+		log_blk = ((u16)extra[2] << 8) | extra[3];
+
+		if (log_blk == 0xFFFF) {
+			if (!(chip->card_wp & MS_CARD)) {
+				retval = ms_erase_block(chip, phy_blk);
+				if (retval != STATUS_SUCCESS)
+					continue;
+			}
+			ms_set_unused_block(chip, phy_blk);
+			continue;
+		}
+
+		if (log_blk < ms_start_idx[seg_no] ||
+		    log_blk >= ms_start_idx[seg_no + 1]) {
+			if (!(chip->card_wp & MS_CARD)) {
+				retval = ms_erase_block(chip, phy_blk);
+				if (retval != STATUS_SUCCESS)
+					continue;
+			}
+			ms_set_unused_block(chip, phy_blk);
+			continue;
+		}
+
+		idx = log_blk - ms_start_idx[seg_no];
+
+		if (segment->l2p_table[idx] == 0xFFFF) {
+			segment->l2p_table[idx] = phy_blk;
+			continue;
+		}
+
+		us1 = extra[0] & 0x10;
+		tmp_blk = segment->l2p_table[idx];
+		retval = ms_read_extra_data(chip, tmp_blk, 0,
+					    extra, MS_EXTRA_SIZE);
+		if (retval != STATUS_SUCCESS)
+			continue;
+		us2 = extra[0] & 0x10;
+
+		(void)ms_arbitrate_l2p(chip, phy_blk,
+				log_blk - ms_start_idx[seg_no], us1, us2);
+	}
+
+	segment->build_flag = 1;
+
+	dev_dbg(rtsx_dev(chip), "unused block count: %d\n",
+		segment->unused_blk_cnt);
+
+	/* Logical Address Confirmation Process */
+	if (seg_no == ms_card->segment_cnt - 1) {
+		if (segment->unused_blk_cnt < 2)
+			chip->card_wp |= MS_CARD;
+	} else {
+		if (segment->unused_blk_cnt < 1)
+			chip->card_wp |= MS_CARD;
+	}
+
+	if (chip->card_wp & MS_CARD)
+		return STATUS_SUCCESS;
+
+	for (log_blk = ms_start_idx[seg_no];
+	     log_blk < ms_start_idx[seg_no + 1]; log_blk++) {
+		idx = log_blk - ms_start_idx[seg_no];
+		if (segment->l2p_table[idx] == 0xFFFF) {
+			phy_blk = ms_get_unused_block(chip, seg_no);
+			if (phy_blk == 0xFFFF) {
+				chip->card_wp |= MS_CARD;
+				return STATUS_SUCCESS;
+			}
+			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
+			if (retval != STATUS_SUCCESS)
+				goto BUILD_FAIL;
+
+			segment->l2p_table[idx] = phy_blk;
+			if (seg_no == ms_card->segment_cnt - 1) {
+				if (segment->unused_blk_cnt < 2) {
+					chip->card_wp |= MS_CARD;
+					return STATUS_SUCCESS;
+				}
+			} else {
+				if (segment->unused_blk_cnt < 1) {
+					chip->card_wp |= MS_CARD;
+					return STATUS_SUCCESS;
+				}
+			}
+		}
+	}
+
+	/* Make boot block be the first normal block */
+	if (seg_no == 0) {
+		for (log_blk = 0; log_blk < 494; log_blk++) {
+			tmp_blk = segment->l2p_table[log_blk];
+			if (tmp_blk < ms_card->boot_block) {
+				dev_dbg(rtsx_dev(chip), "Boot block is not the first normal block.\n");
+
+				if (chip->card_wp & MS_CARD)
+					break;
+
+				phy_blk = ms_get_unused_block(chip, 0);
+				retval = ms_copy_page(chip, tmp_blk, phy_blk,
+						      log_blk, 0,
+						      ms_card->page_off + 1);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+
+				segment->l2p_table[log_blk] = phy_blk;
+
+				retval = ms_set_bad_block(chip, tmp_blk);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+			}
+		}
+	}
+
+	return STATUS_SUCCESS;
+
+BUILD_FAIL:
+	segment->build_flag = 0;
+	vfree(segment->l2p_table);
+	segment->l2p_table = NULL;
+	vfree(segment->free_table);
+	segment->free_table = NULL;
+
+	return STATUS_FAIL;
+}
+
+int reset_ms_card(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int seg_no = ms_card->total_block / 512 - 1;
+	int retval;
+
+	memset(ms_card, 0, sizeof(struct ms_info));
+
+	retval = enable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = select_card(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_card->ms_type = 0;
+
+	retval = reset_ms_pro(chip);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->check_ms_flow) {
+			retval = reset_ms(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		} else {
+			return STATUS_FAIL;
+		}
+	}
+
+	retval = ms_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (!CHK_MSPRO(ms_card)) {
+		/* Build table for the last segment,
+		 * to check if L2P table block exists, erasing it
+		 */
+		retval = ms_build_l2p_tbl(chip, seg_no);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	dev_dbg(rtsx_dev(chip), "ms_card->ms_type = 0x%x\n", ms_card->ms_type);
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_set_rw_cmd(struct rtsx_chip *chip,
+			    u32 start_sec, u16 sec_cnt, u8 cmd)
+{
+	int retval, i;
+	u8 data[8];
+
+	data[0] = cmd;
+	data[1] = (u8)(sec_cnt >> 8);
+	data[2] = (u8)sec_cnt;
+	data[3] = (u8)(start_sec >> 24);
+	data[4] = (u8)(start_sec >> 16);
+	data[5] = (u8)(start_sec >> 8);
+	data[6] = (u8)start_sec;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7,
+					WAIT_INT, data, 8);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+void mspro_stop_seq_mode(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+	if (ms_card->seq_mode) {
+		retval = ms_switch_clock(chip);
+		if (retval != STATUS_SUCCESS)
+			return;
+
+		ms_card->seq_mode = 0;
+		ms_card->total_sec_cnt = 0;
+		ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+
+		rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+	}
+}
+
+static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+	if (chip->asic_code) {
+		if (ms_card->ms_clock > 30)
+			ms_card->ms_clock -= 20;
+	} else {
+		if (ms_card->ms_clock == CLK_80)
+			ms_card->ms_clock = CLK_60;
+		else if (ms_card->ms_clock == CLK_60)
+			ms_card->ms_clock = CLK_40;
+	}
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
+				 struct rtsx_chip *chip, u32 start_sector,
+				 u16 sector_cnt)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	bool mode_2k = false;
+	int retval;
+	u16 count;
+	u8 val, trans_mode, rw_tpc, rw_cmd;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	ms_card->cleanup_counter = 0;
+
+	if (CHK_MSHG(ms_card)) {
+		if ((start_sector % 4) || (sector_cnt % 4)) {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+				rw_tpc = PRO_READ_LONG_DATA;
+				rw_cmd = PRO_READ_DATA;
+			} else {
+				rw_tpc = PRO_WRITE_LONG_DATA;
+				rw_cmd = PRO_WRITE_DATA;
+			}
+		} else {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+				rw_tpc = PRO_READ_QUAD_DATA;
+				rw_cmd = PRO_READ_2K_DATA;
+			} else {
+				rw_tpc = PRO_WRITE_QUAD_DATA;
+				rw_cmd = PRO_WRITE_2K_DATA;
+			}
+			mode_2k = true;
+		}
+	} else {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			rw_tpc = PRO_READ_LONG_DATA;
+			rw_cmd = PRO_READ_DATA;
+		} else {
+			rw_tpc = PRO_WRITE_LONG_DATA;
+			rw_cmd = PRO_WRITE_DATA;
+		}
+	}
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
+		trans_mode = MS_TM_AUTO_READ;
+	else
+		trans_mode = MS_TM_AUTO_WRITE;
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval)
+		return retval;
+
+	if (ms_card->seq_mode) {
+		if (ms_card->pre_dir != srb->sc_data_direction ||
+		    ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
+		     start_sector) ||
+		    (mode_2k && (ms_card->seq_mode & MODE_512_SEQ)) ||
+		    (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ)) ||
+		    !(val & MS_INT_BREQ) ||
+		    ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
+			ms_card->seq_mode = 0;
+			ms_card->total_sec_cnt = 0;
+			if (val & MS_INT_BREQ) {
+				retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+
+				rtsx_write_register(chip, RBCTL, RB_FLUSH,
+						    RB_FLUSH);
+			}
+		}
+	}
+
+	if (!ms_card->seq_mode) {
+		ms_card->total_sec_cnt = 0;
+		if (sector_cnt >= SEQ_START_CRITERIA) {
+			if ((ms_card->capacity - start_sector) > 0xFE00)
+				count = 0xFE00;
+			else
+				count = (u16)(ms_card->capacity - start_sector);
+
+			if (count > sector_cnt) {
+				if (mode_2k)
+					ms_card->seq_mode = MODE_2K_SEQ;
+				else
+					ms_card->seq_mode = MODE_512_SEQ;
+			}
+		} else {
+			count = sector_cnt;
+		}
+		retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->seq_mode = 0;
+			return STATUS_FAIL;
+		}
+	}
+
+	retval = ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt,
+				  WAIT_INT, mode_2k, scsi_sg_count(srb),
+				  scsi_sglist(srb), scsi_bufflen(srb));
+	if (retval != STATUS_SUCCESS) {
+		ms_card->seq_mode = 0;
+		rtsx_read_register(chip, MS_TRANS_CFG, &val);
+		rtsx_clear_ms_error(chip);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			chip->rw_need_retry = 0;
+			dev_dbg(rtsx_dev(chip), "No card exist, exit %s\n",
+				__func__);
+			return STATUS_FAIL;
+		}
+
+		if (val & MS_INT_BREQ)
+			ms_send_cmd(chip, PRO_STOP, WAIT_INT);
+
+		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+			dev_dbg(rtsx_dev(chip), "MSPro CRC error, tune clock!\n");
+			chip->rw_need_retry = 1;
+			ms_auto_tune_clock(chip);
+		}
+
+		return retval;
+	}
+
+	if (ms_card->seq_mode) {
+		ms_card->pre_sec_addr = start_sector;
+		ms_card->pre_sec_cnt = sector_cnt;
+		ms_card->pre_dir = srb->sc_data_direction;
+		ms_card->total_sec_cnt += sector_cnt;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mspro_read_format_progress(struct rtsx_chip *chip,
+				      const int short_data_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u32 total_progress, cur_progress;
+	u8 cnt, tmp;
+	u8 data[8];
+
+	dev_dbg(rtsx_dev(chip), "%s, short_data_len = %d\n", __func__,
+		short_data_len);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	if (!(tmp & MS_INT_BREQ)) {
+		if ((tmp & (MS_INT_CED | MS_INT_BREQ | MS_INT_CMDNK |
+			    MS_INT_ERR)) == MS_INT_CED) {
+			ms_card->format_status = FORMAT_SUCCESS;
+			return STATUS_SUCCESS;
+		}
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	if (short_data_len >= 256)
+		cnt = 0;
+	else
+		cnt = (u8)short_data_len;
+
+	retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT,
+				     MS_NO_CHECK_INT);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT,
+			       data, 8);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	total_progress = (data[0] << 24) | (data[1] << 16) |
+		(data[2] << 8) | data[3];
+	cur_progress = (data[4] << 24) | (data[5] << 16) |
+		(data[6] << 8) | data[7];
+
+	dev_dbg(rtsx_dev(chip), "total_progress = %d, cur_progress = %d\n",
+		total_progress, cur_progress);
+
+	if (total_progress == 0) {
+		ms_card->progress = 0;
+	} else {
+		u64 ulltmp = (u64)cur_progress * (u64)65535;
+
+		do_div(ulltmp, total_progress);
+		ms_card->progress = (u16)ulltmp;
+	}
+	dev_dbg(rtsx_dev(chip), "progress = %d\n", ms_card->progress);
+
+	for (i = 0; i < 5000; i++) {
+		retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+		if (retval != STATUS_SUCCESS) {
+			ms_card->format_status = FORMAT_FAIL;
+			return STATUS_FAIL;
+		}
+		if (tmp & (MS_INT_CED | MS_INT_CMDNK |
+				MS_INT_BREQ | MS_INT_ERR))
+			break;
+
+		wait_timeout(1);
+	}
+
+	retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, 0);
+	if (retval != STATUS_SUCCESS) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	if (i == 5000) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
+		ms_card->format_status = FORMAT_FAIL;
+		return STATUS_FAIL;
+	}
+
+	if (tmp & MS_INT_CED) {
+		ms_card->format_status = FORMAT_SUCCESS;
+		ms_card->pro_under_formatting = 0;
+	} else if (tmp & MS_INT_BREQ) {
+		ms_card->format_status = FORMAT_IN_PROGRESS;
+	} else {
+		ms_card->format_status = FORMAT_FAIL;
+		ms_card->pro_under_formatting = 0;
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void mspro_polling_format_status(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int i;
+
+	if (ms_card->pro_under_formatting &&
+	    (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
+		rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+		for (i = 0; i < 65535; i++) {
+			mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
+			if (ms_card->format_status != FORMAT_IN_PROGRESS)
+				break;
+		}
+	}
+}
+
+int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		 int short_data_len, bool quick_format)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 buf[8], tmp;
+	u16 para;
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, PRO_TPC_PARM, 0x01);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	memset(buf, 0, 2);
+	switch (short_data_len) {
+	case 32:
+		buf[0] = 0;
+		break;
+	case 64:
+		buf[0] = 1;
+		break;
+	case 128:
+		buf[0] = 2;
+		break;
+	case 256:
+	default:
+		buf[0] = 3;
+		break;
+	}
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, PRO_WRITE_REG, 1,
+					NO_WAIT_INT, buf, 2);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	if (quick_format)
+		para = 0x0000;
+	else
+		para = 0x0001;
+
+	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+	if (retval)
+		return retval;
+
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
+		return STATUS_FAIL;
+
+	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
+		ms_card->pro_under_formatting = 1;
+		ms_card->progress = 0;
+		ms_card->format_status = FORMAT_IN_PROGRESS;
+		return STATUS_SUCCESS;
+	}
+
+	if (tmp & MS_INT_CED) {
+		ms_card->pro_under_formatting = 0;
+		ms_card->progress = 0;
+		ms_card->format_status = FORMAT_SUCCESS;
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
+		return STATUS_SUCCESS;
+	}
+
+	return STATUS_FAIL;
+}
+
+static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
+				  u16 log_blk, u8 start_page, u8 end_page,
+				  u8 *buf, unsigned int *index,
+				  unsigned int *offset)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
+	u8 *ptr;
+
+	retval = ms_read_extra_data(chip, phy_blk, start_page,
+				    extra, MS_EXTRA_SIZE);
+	if (retval == STATUS_SUCCESS) {
+		if ((extra[1] & 0x30) != 0x30) {
+			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+			return STATUS_FAIL;
+		}
+	}
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(phy_blk >> 8);
+	data[3] = (u8)phy_blk;
+	data[4] = 0;
+	data[5] = start_page;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
+					data, 6);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ptr = buf;
+
+	for (page_addr = start_page; page_addr < end_page; page_addr++) {
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			return STATUS_FAIL;
+		}
+		if (val & INT_REG_ERR) {
+			if (val & INT_REG_BREQ) {
+				retval = ms_read_status_reg(chip);
+				if (retval != STATUS_SUCCESS) {
+					if (!(chip->card_wp & MS_CARD)) {
+						reset_ms(chip);
+						ms_set_page_status
+							(log_blk, set_PS_NG,
+							 extra,
+							 MS_EXTRA_SIZE);
+						ms_write_extra_data
+							(chip, phy_blk,
+							 page_addr, extra,
+							 MS_EXTRA_SIZE);
+					}
+					ms_set_err_code(chip,
+							MS_FLASH_READ_ERROR);
+					return STATUS_FAIL;
+				}
+			} else {
+				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+				return STATUS_FAIL;
+			}
+		} else {
+			if (!(val & INT_REG_BREQ)) {
+				ms_set_err_code(chip, MS_BREQ_ERROR);
+				return STATUS_FAIL;
+			}
+		}
+
+		if (page_addr == (end_page - 1)) {
+			if (!(val & INT_REG_CED)) {
+				retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+			}
+
+			retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
+					       &val, 1);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			if (!(val & INT_REG_CED)) {
+				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
+				return STATUS_FAIL;
+			}
+
+			trans_cfg = NO_WAIT_INT;
+		} else {
+			trans_cfg = WAIT_INT;
+		}
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, READ_PAGE_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
+			     0xFF, trans_cfg);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			     0x01, RING_BUFFER);
+
+		trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START |  MS_TM_NORMAL_READ);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
+						    scsi_sg_count(chip->srb),
+						    index, offset,
+						    DMA_FROM_DEVICE,
+						    chip->ms_timeout);
+		if (retval < 0) {
+			if (retval == -ETIMEDOUT) {
+				ms_set_err_code(chip, MS_TO_ERROR);
+				rtsx_clear_ms_error(chip);
+				return STATUS_TIMEDOUT;
+			}
+
+			retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+			if (retval != STATUS_SUCCESS) {
+				ms_set_err_code(chip, MS_TO_ERROR);
+				rtsx_clear_ms_error(chip);
+				return STATUS_TIMEDOUT;
+			}
+			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+				ms_set_err_code(chip, MS_CRC16_ERROR);
+				rtsx_clear_ms_error(chip);
+				return STATUS_FAIL;
+			}
+		}
+
+		if (scsi_sg_count(chip->srb) == 0)
+			ptr += 512;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
+				   u16 new_blk, u16 log_blk, u8 start_page,
+				u8 end_page, u8 *buf, unsigned int *index,
+				unsigned int *offset)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, i;
+	u8 page_addr, val, data[16];
+	u8 *ptr;
+
+	if (!start_page) {
+		retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+					    SYSTEM_PARAM, 7);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (CHK_MS4BIT(ms_card))
+			data[0] = 0x88;
+		else
+			data[0] = 0x80;
+
+		data[1] = 0;
+		data[2] = (u8)(old_blk >> 8);
+		data[3] = (u8)old_blk;
+		data[4] = 0x80;
+		data[5] = 0;
+		data[6] = 0xEF;
+		data[7] = 0xFF;
+
+		retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
+					data, 8);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		ms_set_err_code(chip, MS_NO_ERROR);
+		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
+					 NO_WAIT_INT);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
+				    SYSTEM_PARAM, (6 + MS_EXTRA_SIZE));
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	if (CHK_MS4BIT(ms_card))
+		data[0] = 0x88;
+	else
+		data[0] = 0x80;
+
+	data[1] = 0;
+	data[2] = (u8)(new_blk >> 8);
+	data[3] = (u8)new_blk;
+	if ((end_page - start_page) == 1)
+		data[4] = 0x20;
+	else
+		data[4] = 0;
+
+	data[5] = start_page;
+	data[6] = 0xF8;
+	data[7] = 0xFF;
+	data[8] = (u8)(log_blk >> 8);
+	data[9] = (u8)log_blk;
+
+	for (i = 0x0A; i < 0x10; i++)
+		data[i] = 0xFF;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
+					NO_WAIT_INT, data, 16);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	ptr = buf;
+	for (page_addr = start_page; page_addr < end_page; page_addr++) {
+		ms_set_err_code(chip, MS_NO_ERROR);
+
+		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+			ms_set_err_code(chip, MS_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		if (val & INT_REG_CMDNK) {
+			ms_set_err_code(chip, MS_CMD_NK);
+			return STATUS_FAIL;
+		}
+		if (val & INT_REG_ERR) {
+			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+			return STATUS_FAIL;
+		}
+		if (!(val & INT_REG_BREQ)) {
+			ms_set_err_code(chip, MS_BREQ_ERROR);
+			return STATUS_FAIL;
+		}
+
+		udelay(30);
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
+			     0xFF, WRITE_PAGE_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
+			     0xFF, WAIT_INT);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			     0x01, RING_BUFFER);
+
+		trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr,	512,
+						    scsi_sg_count(chip->srb),
+						    index, offset,
+						    DMA_TO_DEVICE,
+						    chip->ms_timeout);
+		if (retval < 0) {
+			ms_set_err_code(chip, MS_TO_ERROR);
+			rtsx_clear_ms_error(chip);
+
+			if (retval == -ETIMEDOUT)
+				return STATUS_TIMEDOUT;
+			return STATUS_FAIL;
+		}
+
+		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if ((end_page - start_page) == 1) {
+			if (!(val & INT_REG_CED)) {
+				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
+				return STATUS_FAIL;
+			}
+		} else {
+			if (page_addr == (end_page - 1)) {
+				if (!(val & INT_REG_CED)) {
+					retval = ms_send_cmd(chip, BLOCK_END,
+							     WAIT_INT);
+					if (retval != STATUS_SUCCESS)
+						return STATUS_FAIL;
+				}
+
+				retval = ms_read_bytes(chip, GET_INT, 1,
+						       NO_WAIT_INT, &val, 1);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+			}
+
+			if (page_addr == (end_page - 1) ||
+			    page_addr == ms_card->page_off) {
+				if (!(val & INT_REG_CED)) {
+					ms_set_err_code(chip,
+							MS_FLASH_WRITE_ERROR);
+					return STATUS_FAIL;
+				}
+			}
+		}
+
+		if (scsi_sg_count(chip->srb) == 0)
+			ptr += 512;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			   u16 log_blk, u8 page_off)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval, seg_no;
+
+	retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
+			      page_off, ms_card->page_off + 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	seg_no = old_blk >> 9;
+
+	if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
+		MS_CLR_BAD_BLOCK_FLG(ms_card);
+		ms_set_bad_block(chip, old_blk);
+	} else {
+		retval = ms_erase_block(chip, old_blk);
+		if (retval == STATUS_SUCCESS)
+			ms_set_unused_block(chip, old_blk);
+	}
+
+	ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
+
+	return STATUS_SUCCESS;
+}
+
+static int ms_prepare_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
+			    u16 log_blk, u8 start_page)
+{
+	int retval;
+
+	if (start_page) {
+		retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
+				      0, start_page);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef MS_DELAY_WRITE
+int ms_delay_write(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
+	int retval;
+
+	if (delay_write->delay_write_flag) {
+		retval = ms_set_init_para(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		delay_write->delay_write_flag = 0;
+		retval = ms_finish_write(chip,
+					 delay_write->old_phyblock,
+					delay_write->new_phyblock,
+					delay_write->logblock,
+					delay_write->pageoff);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static inline void ms_rw_fail(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+	else
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+}
+
+static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+			      u32 start_sector, u16 sector_cnt)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, seg_no;
+	unsigned int index = 0, offset = 0;
+	u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
+	u8 start_page, end_page = 0, page_cnt;
+	u8 *ptr;
+#ifdef MS_DELAY_WRITE
+	struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
+#endif
+
+	ms_set_err_code(chip, MS_NO_ERROR);
+
+	ms_card->cleanup_counter = 0;
+
+	ptr = (u8 *)scsi_sglist(srb);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		ms_rw_fail(srb, chip);
+		return STATUS_FAIL;
+	}
+
+	log_blk = (u16)(start_sector >> ms_card->block_shift);
+	start_page = (u8)(start_sector & ms_card->page_off);
+
+	for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
+		if (log_blk < ms_start_idx[seg_no + 1])
+			break;
+	}
+
+	if (ms_card->segment[seg_no].build_flag == 0) {
+		retval = ms_build_l2p_tbl(chip, seg_no);
+		if (retval != STATUS_SUCCESS) {
+			chip->card_fail |= MS_CARD;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return STATUS_FAIL;
+		}
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+#ifdef MS_DELAY_WRITE
+		if (delay_write->delay_write_flag &&
+		    delay_write->logblock == log_blk &&
+		    start_page > delay_write->pageoff) {
+			delay_write->delay_write_flag = 0;
+			retval = ms_copy_page(chip,
+					      delay_write->old_phyblock,
+					      delay_write->new_phyblock,
+					      log_blk,
+					      delay_write->pageoff, start_page);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else if (delay_write->delay_write_flag &&
+				(delay_write->logblock == log_blk) &&
+				(start_page == delay_write->pageoff)) {
+			delay_write->delay_write_flag = 0;
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else {
+			retval = ms_delay_write(chip);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+#endif
+			old_blk = ms_get_l2p_tbl
+					(chip, seg_no,
+					 log_blk - ms_start_idx[seg_no]);
+			new_blk  = ms_get_unused_block(chip, seg_no);
+			if (old_blk == 0xFFFF || new_blk == 0xFFFF) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+
+			retval = ms_prepare_write(chip, old_blk, new_blk,
+						  log_blk, start_page);
+			if (retval != STATUS_SUCCESS) {
+				if (detect_card_cd(chip, MS_CARD) !=
+				    STATUS_SUCCESS) {
+					set_sense_type
+						(chip, lun,
+						SENSE_TYPE_MEDIA_NOT_PRESENT);
+					return STATUS_FAIL;
+				}
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+#ifdef MS_DELAY_WRITE
+		}
+#endif
+	} else {
+#ifdef MS_DELAY_WRITE
+		retval = ms_delay_write(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return STATUS_FAIL;
+		}
+#endif
+		old_blk = ms_get_l2p_tbl(chip, seg_no,
+					 log_blk - ms_start_idx[seg_no]);
+		if (old_blk == 0xFFFF) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return STATUS_FAIL;
+		}
+	}
+
+	dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
+		seg_no, old_blk, new_blk);
+
+	while (total_sec_cnt) {
+		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
+			end_page = ms_card->page_off + 1;
+		else
+			end_page = start_page + (u8)total_sec_cnt;
+
+		page_cnt = end_page - start_page;
+
+		dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d, page_cnt = %d\n",
+			start_page, end_page, page_cnt);
+
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			retval = ms_read_multiple_pages(chip,
+							old_blk, log_blk,
+							start_page, end_page,
+							ptr, &index, &offset);
+		} else {
+			retval = ms_write_multiple_pages(chip, old_blk, new_blk,
+							 log_blk, start_page,
+							 end_page, ptr, &index,
+							 &offset);
+		}
+
+		if (retval != STATUS_SUCCESS) {
+			toggle_gpio(chip, 1);
+			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+			ms_rw_fail(srb, chip);
+			return STATUS_FAIL;
+		}
+
+		if (srb->sc_data_direction == DMA_TO_DEVICE) {
+			if (end_page == (ms_card->page_off + 1)) {
+				retval = ms_erase_block(chip, old_blk);
+				if (retval == STATUS_SUCCESS)
+					ms_set_unused_block(chip, old_blk);
+
+				ms_set_l2p_tbl(chip, seg_no,
+					       log_blk - ms_start_idx[seg_no],
+					       new_blk);
+			}
+		}
+
+		total_sec_cnt -= page_cnt;
+		if (scsi_sg_count(srb) == 0)
+			ptr += page_cnt * 512;
+
+		if (total_sec_cnt == 0)
+			break;
+
+		log_blk++;
+
+		for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
+				seg_no++) {
+			if (log_blk < ms_start_idx[seg_no + 1])
+				break;
+		}
+
+		if (ms_card->segment[seg_no].build_flag == 0) {
+			retval = ms_build_l2p_tbl(chip, seg_no);
+			if (retval != STATUS_SUCCESS) {
+				chip->card_fail |= MS_CARD;
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+		}
+
+		old_blk = ms_get_l2p_tbl(chip, seg_no,
+					 log_blk - ms_start_idx[seg_no]);
+		if (old_blk == 0xFFFF) {
+			ms_rw_fail(srb, chip);
+			return STATUS_FAIL;
+		}
+
+		if (srb->sc_data_direction == DMA_TO_DEVICE) {
+			new_blk = ms_get_unused_block(chip, seg_no);
+			if (new_blk == 0xFFFF) {
+				ms_rw_fail(srb, chip);
+				return STATUS_FAIL;
+			}
+		}
+
+		dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
+			seg_no, old_blk, new_blk);
+
+		start_page = 0;
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+		if (end_page < (ms_card->page_off + 1)) {
+#ifdef MS_DELAY_WRITE
+			delay_write->delay_write_flag = 1;
+			delay_write->old_phyblock = old_blk;
+			delay_write->new_phyblock = new_blk;
+			delay_write->logblock = log_blk;
+			delay_write->pageoff = end_page;
+#else
+			retval = ms_finish_write(chip, old_blk, new_blk,
+						 log_blk, end_page);
+			if (retval != STATUS_SUCCESS) {
+				if (detect_card_cd(chip, MS_CARD) !=
+				    STATUS_SUCCESS) {
+					set_sense_type
+						(chip, lun,
+						SENSE_TYPE_MEDIA_NOT_PRESENT);
+					return STATUS_FAIL;
+				}
+
+				ms_rw_fail(srb, chip);
+				return STATUS_FAIL;
+			}
+#endif
+		}
+	}
+
+	scsi_set_resid(srb, 0);
+
+	return STATUS_SUCCESS;
+}
+
+int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	  u32 start_sector, u16 sector_cnt)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+	if (CHK_MSPRO(ms_card))
+		retval = mspro_rw_multi_sector(srb, chip, start_sector,
+					       sector_cnt);
+	else
+		retval = ms_rw_multi_sector(srb, chip, start_sector,
+					    sector_cnt);
+
+	return retval;
+}
+
+void ms_free_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int i = 0;
+
+	if (ms_card->segment) {
+		for (i = 0; i < ms_card->segment_cnt; i++) {
+			vfree(ms_card->segment[i].l2p_table);
+			ms_card->segment[i].l2p_table = NULL;
+			vfree(ms_card->segment[i].free_table);
+			ms_card->segment[i].free_table = NULL;
+		}
+		vfree(ms_card->segment);
+		ms_card->segment = NULL;
+	}
+}
+
+#ifdef SUPPORT_MAGIC_GATE
+
+#ifdef READ_BYTES_WAIT_INT
+static int ms_poll_int(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANS_CFG, MS_INT_CED, MS_INT_CED);
+
+	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	val = *rtsx_get_cmd_data(chip);
+	if (val & MS_INT_ERR)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+#ifdef MS_SAMPLE_INT_ERR
+static int check_ms_err(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	retval = rtsx_read_register(chip, MS_TRANSFER, &val);
+	if (retval != STATUS_SUCCESS)
+		return 1;
+	if (val & MS_TRANSFER_ERR)
+		return 1;
+
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval != STATUS_SUCCESS)
+		return 1;
+
+	if (val & (MS_INT_ERR | MS_INT_CMDNK))
+		return 1;
+
+	return 0;
+}
+#else
+static int check_ms_err(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val;
+
+	retval = rtsx_read_register(chip, MS_TRANSFER, &val);
+	if (retval != STATUS_SUCCESS)
+		return 1;
+	if (val & MS_TRANSFER_ERR)
+		return 1;
+
+	return 0;
+}
+#endif
+
+static int mg_send_ex_cmd(struct rtsx_chip *chip, u8 cmd, u8 entry_num)
+{
+	int retval, i;
+	u8 data[8];
+
+	data[0] = cmd;
+	data[1] = 0;
+	data[2] = 0;
+	data[3] = 0;
+	data[4] = 0;
+	data[5] = 0;
+	data[6] = entry_num;
+	data[7] = 0;
+
+	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
+		retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT,
+					data, 8);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (i == MS_MAX_RETRY_COUNT)
+		return STATUS_FAIL;
+
+	if (check_ms_err(chip)) {
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
+			       u8 mg_entry_num)
+{
+	int retval;
+	u8 buf[6];
+
+	if (type == 0)
+		retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_TPC_PARM, 1);
+	else
+		retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_DATA_COUNT1, 6);
+
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf[0] = 0;
+	buf[1] = 0;
+	if (type == 1) {
+		buf[2] = 0;
+		buf[3] = 0;
+		buf[4] = 0;
+		buf[5] = mg_entry_num;
+	}
+	retval = ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
+				NO_WAIT_INT, buf, 6);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	int i;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf1[32], buf2[12];
+
+	if (scsi_bufflen(srb) < 12) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return STATUS_FAIL;
+	}
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		return STATUS_FAIL;
+	}
+
+	memset(buf1, 0, 32);
+	rtsx_stor_get_xfer_buf(buf2, min_t(int, 12, scsi_bufflen(srb)), srb);
+	for (i = 0; i < 8; i++)
+		buf1[8 + i] = buf2[4 + i];
+
+	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
+				buf1, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		return STATUS_FAIL;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf = kmalloc(1540, GFP_KERNEL);
+	if (!buf)
+		return STATUS_ERROR;
+
+	buf[0] = 0x04;
+	buf[1] = 0x1A;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+
+	retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		goto free_buffer;
+	}
+
+	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
+				  3, WAIT_INT, 0, 0, buf + 4, 1536);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		goto free_buffer;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		retval = STATUS_FAIL;
+		goto free_buffer;
+	}
+
+	bufflen = min_t(int, 1052, scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf, bufflen, srb);
+
+free_buffer:
+	kfree(buf);
+	return retval;
+}
+
+int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	int bufflen;
+	int i;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf[32];
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return STATUS_FAIL;
+	}
+
+	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
+			       buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return STATUS_FAIL;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	memcpy(ms_card->magic_gate_id, buf, 16);
+
+#ifdef READ_BYTES_WAIT_INT
+	retval = ms_poll_int(chip);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return STATUS_FAIL;
+	}
+#endif
+
+	retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return STATUS_FAIL;
+	}
+
+	bufflen = min_t(int, 12, scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	for (i = 0; i < 8; i++)
+		buf[i] = buf[4 + i];
+
+	for (i = 0; i < 24; i++)
+		buf[8 + i] = 0;
+
+	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA,
+				32, WAIT_INT, buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return STATUS_FAIL;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	ms_card->mg_auth = 0;
+
+	return STATUS_SUCCESS;
+}
+
+int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf1[32], buf2[36];
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		return STATUS_FAIL;
+	}
+
+	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
+			       buf1, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		return STATUS_FAIL;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	buf2[0] = 0x00;
+	buf2[1] = 0x22;
+	buf2[2] = 0x00;
+	buf2[3] = 0x00;
+
+	memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
+	memcpy(buf2 + 20, buf1, 16);
+
+	bufflen = min_t(int, 36, scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf2, bufflen, srb);
+
+#ifdef READ_BYTES_WAIT_INT
+	retval = ms_poll_int(chip);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		return STATUS_FAIL;
+	}
+#endif
+
+	return STATUS_SUCCESS;
+}
+
+int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	int i;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 buf[32];
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		return STATUS_FAIL;
+	}
+
+	bufflen = min_t(int, 12, scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	for (i = 0; i < 8; i++)
+		buf[i] = buf[4 + i];
+
+	for (i = 0; i < 24; i++)
+		buf[8 + i] = 0;
+
+	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
+				buf, 32);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		return STATUS_FAIL;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
+		rtsx_clear_ms_error(chip);
+		return STATUS_FAIL;
+	}
+
+	ms_card->mg_auth = 1;
+
+	return STATUS_SUCCESS;
+}
+
+int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	int bufflen;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf = kmalloc(1028, GFP_KERNEL);
+	if (!buf)
+		return STATUS_ERROR;
+
+	buf[0] = 0x04;
+	buf[1] = 0x02;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+
+	retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		goto free_buffer;
+	}
+
+	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
+				  2, WAIT_INT, 0, 0, buf + 4, 1024);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		rtsx_clear_ms_error(chip);
+		goto free_buffer;
+	}
+	if (check_ms_err(chip)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		rtsx_clear_ms_error(chip);
+		retval = STATUS_FAIL;
+		goto free_buffer;
+	}
+
+	bufflen = min_t(int, 1028, scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf, bufflen, srb);
+
+free_buffer:
+	kfree(buf);
+	return retval;
+}
+
+int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	int bufflen;
+#ifdef MG_SET_ICV_SLOW
+	int i;
+#endif
+	unsigned int lun = SCSI_LUN(srb);
+	u8 *buf = NULL;
+
+	ms_cleanup_work(chip);
+
+	retval = ms_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	buf = kmalloc(1028, GFP_KERNEL);
+	if (!buf)
+		return STATUS_ERROR;
+
+	bufflen = min_t(int, 1028, scsi_bufflen(srb));
+	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
+
+	retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
+	if (retval != STATUS_SUCCESS) {
+		if (ms_card->mg_auth == 0) {
+			if ((buf[5] & 0xC0) != 0)
+				set_sense_type
+					(chip, lun,
+					SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+			else
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
+		}
+		goto set_ICV_finish;
+	}
+
+#ifdef MG_SET_ICV_SLOW
+	for (i = 0; i < 2; i++) {
+		udelay(50);
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
+			     0xFF, PRO_WRITE_LONG_DATA);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, WAIT_INT);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			     0x01, RING_BUFFER);
+
+		trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
+			     MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
+			     MS_TRANSFER_END, MS_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data(chip, MS_CARD, buf + 4 + i * 512,
+					    512, 0, DMA_TO_DEVICE, 3000);
+		if (retval < 0 || check_ms_err(chip)) {
+			rtsx_clear_ms_error(chip);
+			if (ms_card->mg_auth == 0) {
+				if ((buf[5] & 0xC0) != 0)
+					set_sense_type
+					    (chip, lun,
+					     SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+				else
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MG_WRITE_ERR);
+			} else {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+			}
+			retval = STATUS_FAIL;
+			goto set_ICV_finish;
+		}
+	}
+#else
+	retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
+				  2, WAIT_INT, 0, 0, buf + 4, 1024);
+	if (retval != STATUS_SUCCESS || check_ms_err(chip)) {
+		rtsx_clear_ms_error(chip);
+		if (ms_card->mg_auth == 0) {
+			if ((buf[5] & 0xC0) != 0)
+				set_sense_type
+				    (chip, lun,
+				     SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
+			else
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MG_WRITE_ERR);
+		} else {
+			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
+		}
+		goto set_ICV_finish;
+	}
+#endif
+
+set_ICV_finish:
+	kfree(buf);
+	return retval;
+}
+
+#endif /* SUPPORT_MAGIC_GATE */
+
+void ms_cleanup_work(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+
+	if (CHK_MSPRO(ms_card)) {
+		if (ms_card->seq_mode) {
+			dev_dbg(rtsx_dev(chip), "MS Pro: stop transmission\n");
+			mspro_stop_seq_mode(chip);
+			ms_card->cleanup_counter = 0;
+		}
+		if (CHK_MSHG(ms_card)) {
+			rtsx_write_register(chip, MS_CFG,
+					    MS_2K_SECTOR_MODE, 0x00);
+		}
+	}
+#ifdef MS_DELAY_WRITE
+	else if ((!CHK_MSPRO(ms_card)) &&
+		 ms_card->delay_write.delay_write_flag) {
+		dev_dbg(rtsx_dev(chip), "MS: delay write\n");
+		ms_delay_write(chip);
+		ms_card->cleanup_counter = 0;
+	}
+#endif
+}
+
+int ms_power_off_card3v3(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = disable_card_clock(chip, MS_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (chip->asic_code) {
+		retval = ms_pull_ctl_disable(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_MS_PULL_CTL_BIT | 0x20,
+					     FPGA_MS_PULL_CTL_BIT);
+		if (retval)
+			return retval;
+	}
+	retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+	if (retval)
+		return retval;
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_off(chip, MS_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int release_ms_card(struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+
+#ifdef MS_DELAY_WRITE
+	ms_card->delay_write.delay_write_flag = 0;
+#endif
+	ms_card->pro_under_formatting = 0;
+
+	chip->card_ready &= ~MS_CARD;
+	chip->card_fail &= ~MS_CARD;
+	chip->card_wp &= ~MS_CARD;
+
+	ms_free_l2p_tbl(chip);
+
+	memset(ms_card->raw_sys_info, 0, 96);
+#ifdef SUPPORT_PCGL_1P18
+	memset(ms_card->raw_model_name, 0, 48);
+#endif
+
+	retval = ms_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5208/ms.h b/drivers/staging/rts5208/ms.h
new file mode 100644
index 000000000..33bda9ce3
--- /dev/null
+++ b/drivers/staging/rts5208/ms.h
@@ -0,0 +1,214 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_MS_H
+#define __REALTEK_RTSX_MS_H
+
+#define MS_DELAY_WRITE
+
+#define	MS_MAX_RETRY_COUNT	3
+
+#define	MS_EXTRA_SIZE		0x9
+
+#define	WRT_PRTCT		0x01
+
+/* Error Code */
+#define	MS_NO_ERROR				0x00
+#define	MS_CRC16_ERROR				0x80
+#define	MS_TO_ERROR				0x40
+#define	MS_NO_CARD				0x20
+#define	MS_NO_MEMORY				0x10
+#define	MS_CMD_NK				0x08
+#define	MS_FLASH_READ_ERROR			0x04
+#define	MS_FLASH_WRITE_ERROR			0x02
+#define	MS_BREQ_ERROR				0x01
+#define	MS_NOT_FOUND				0x03
+
+/* Transfer Protocol Command */
+#define READ_PAGE_DATA				0x02
+#define READ_REG				0x04
+#define	GET_INT					0x07
+#define WRITE_PAGE_DATA				0x0D
+#define WRITE_REG				0x0B
+#define SET_RW_REG_ADRS				0x08
+#define SET_CMD					0x0E
+
+#define	PRO_READ_LONG_DATA			0x02
+#define	PRO_READ_SHORT_DATA			0x03
+#define PRO_READ_REG				0x04
+#define	PRO_READ_QUAD_DATA			0x05
+#define PRO_GET_INT				0x07
+#define	PRO_WRITE_LONG_DATA			0x0D
+#define	PRO_WRITE_SHORT_DATA			0x0C
+#define	PRO_WRITE_QUAD_DATA			0x0A
+#define PRO_WRITE_REG				0x0B
+#define PRO_SET_RW_REG_ADRS			0x08
+#define PRO_SET_CMD				0x0E
+#define PRO_EX_SET_CMD				0x09
+
+#ifdef SUPPORT_MAGIC_GATE
+
+#define MG_GET_ID		0x40
+#define MG_SET_LID		0x41
+#define MG_GET_LEKB		0x42
+#define MG_SET_RD		0x43
+#define MG_MAKE_RMS		0x44
+#define MG_MAKE_KSE		0x45
+#define MG_SET_IBD		0x46
+#define MG_GET_IBD		0x47
+
+#endif
+
+#ifdef XC_POWERCLASS
+#define XC_CHG_POWER		0x16
+#endif
+
+#define BLOCK_READ	0xAA
+#define	BLOCK_WRITE	0x55
+#define BLOCK_END	0x33
+#define BLOCK_ERASE	0x99
+#define FLASH_STOP	0xCC
+
+#define SLEEP		0x5A
+#define CLEAR_BUF	0xC3
+#define MS_RESET	0x3C
+
+#define PRO_READ_DATA		0x20
+#define	PRO_WRITE_DATA		0x21
+#define PRO_READ_ATRB		0x24
+#define PRO_STOP		0x25
+#define PRO_ERASE		0x26
+#define	PRO_READ_2K_DATA	0x27
+#define	PRO_WRITE_2K_DATA	0x28
+
+#define PRO_FORMAT		0x10
+#define PRO_SLEEP		0x11
+
+#define INT_REG			0x01
+#define STATUS_REG0		0x02
+#define STATUS_REG1		0x03
+
+#define SYSTEM_PARAM		0x10
+#define BLOCK_ADRS		0x11
+#define CMD_PARM		0x14
+#define PAGE_ADRS		0x15
+
+#define OVERWRITE_FLAG		0x16
+#define MANAGEMEN_FLAG		0x17
+#define LOGICAL_ADRS		0x18
+#define RESERVE_AREA		0x1A
+
+#define PRO_INT_REG		0x01
+#define PRO_STATUS_REG		0x02
+#define PRO_TYPE_REG		0x04
+#define PRO_IF_mode_REG		0x05
+#define PRO_CATEGORY_REG	0x06
+#define PRO_CLASS_REG		0x07
+
+#define PRO_SYSTEM_PARAM		0x10
+#define PRO_DATA_COUNT1		0x11
+#define PRO_DATA_COUNT0		0x12
+#define PRO_DATA_ADDR3		0x13
+#define PRO_DATA_ADDR2		0x14
+#define PRO_DATA_ADDR1		0x15
+#define PRO_DATA_ADDR0		0x16
+
+#define PRO_TPC_PARM		0x17
+#define PRO_CMD_PARM		0x18
+
+#define	INT_REG_CED		0x80
+#define	INT_REG_ERR		0x40
+#define	INT_REG_BREQ		0x20
+#define	INT_REG_CMDNK		0x01
+
+#define	BLOCK_BOOT		0xC0
+#define	BLOCK_OK		0x80
+#define	PAGE_OK			0x60
+#define	DATA_COMPL		0x10
+
+#define	NOT_BOOT_BLOCK		0x4
+#define	NOT_TRANSLATION_TABLE	0x8
+
+#define	HEADER_ID0		PPBUF_BASE2
+#define	HEADER_ID1		(PPBUF_BASE2 + 1)
+#define	DISABLED_BLOCK0		(PPBUF_BASE2 + 0x170 + 4)
+#define	DISABLED_BLOCK1		(PPBUF_BASE2 + 0x170 + 5)
+#define	DISABLED_BLOCK2		(PPBUF_BASE2 + 0x170 + 6)
+#define	DISABLED_BLOCK3		(PPBUF_BASE2 + 0x170 + 7)
+#define	BLOCK_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 2)
+#define	BLOCK_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 3)
+#define	BLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 4)
+#define	BLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 5)
+#define	EBLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 6)
+#define	EBLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 7)
+#define	PAGE_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 8)
+#define	PAGE_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 9)
+
+#define MS_device_type		(PPBUF_BASE2 + 0x1D8)
+
+#define MS_4bit_support		(PPBUF_BASE2 + 0x1D3)
+
+#define set_PS_NG	1
+#define set_PS_error	0
+
+#define	PARALLEL_8BIT_IF	0x40
+#define	PARALLEL_4BIT_IF	0x00
+#define	SERIAL_IF		0x80
+
+#define BUF_FULL	0x10
+#define BUF_EMPTY	0x20
+
+#define	MEDIA_BUSY	0x80
+#define	FLASH_BUSY	0x40
+#define	DATA_ERROR	0x20
+#define	STS_UCDT	0x10
+#define	EXTRA_ERROR	0x08
+#define	STS_UCEX	0x04
+#define	FLAG_ERROR	0x02
+#define	STS_UCFG	0x01
+
+#define MS_SHORT_DATA_LEN	32
+
+#define FORMAT_SUCCESS		0
+#define FORMAT_FAIL		1
+#define FORMAT_IN_PROGRESS	2
+
+#define	MS_SET_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag |= 0x80)
+#define MS_CLR_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag &= 0x7F)
+#define MS_TST_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag & 0x80)
+
+void mspro_polling_format_status(struct rtsx_chip *chip);
+
+void mspro_stop_seq_mode(struct rtsx_chip *chip);
+int reset_ms_card(struct rtsx_chip *chip);
+int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	  u32 start_sector, u16 sector_cnt);
+int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+		 int short_data_len, bool quick_format);
+void ms_free_l2p_tbl(struct rtsx_chip *chip);
+void ms_cleanup_work(struct rtsx_chip *chip);
+int ms_power_off_card3v3(struct rtsx_chip *chip);
+int release_ms_card(struct rtsx_chip *chip);
+#ifdef MS_DELAY_WRITE
+int ms_delay_write(struct rtsx_chip *chip);
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+#endif
+
+#endif  /* __REALTEK_RTSX_MS_H */
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
new file mode 100644
index 000000000..2284a96ab
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx.c
@@ -0,0 +1,993 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+
+#include "rtsx.h"
+#include "ms.h"
+#include "sd.h"
+#include "xd.h"
+
+MODULE_DESCRIPTION("Realtek PCI-Express card reader rts5208/rts5288 driver");
+MODULE_LICENSE("GPL");
+
+static unsigned int delay_use = 1;
+module_param(delay_use, uint, 0644);
+MODULE_PARM_DESC(delay_use, "seconds to delay before using a new device");
+
+static int ss_en;
+module_param(ss_en, int, 0644);
+MODULE_PARM_DESC(ss_en, "enable selective suspend");
+
+static int ss_interval = 50;
+module_param(ss_interval, int, 0644);
+MODULE_PARM_DESC(ss_interval, "Interval to enter ss state in seconds");
+
+static int auto_delink_en;
+module_param(auto_delink_en, int, 0644);
+MODULE_PARM_DESC(auto_delink_en, "enable auto delink");
+
+static unsigned char aspm_l0s_l1_en;
+module_param(aspm_l0s_l1_en, byte, 0644);
+MODULE_PARM_DESC(aspm_l0s_l1_en, "enable device aspm");
+
+static int msi_en;
+module_param(msi_en, int, 0644);
+MODULE_PARM_DESC(msi_en, "enable msi");
+
+static irqreturn_t rtsx_interrupt(int irq, void *dev_id);
+
+/***********************************************************************
+ * Host functions
+ ***********************************************************************/
+
+static const char *host_info(struct Scsi_Host *host)
+{
+	return "SCSI emulation for PCI-Express Mass Storage devices";
+}
+
+static int slave_alloc(struct scsi_device *sdev)
+{
+	/*
+	 * Set the INQUIRY transfer length to 36.  We don't use any of
+	 * the extra data and many devices choke if asked for more or
+	 * less than 36 bytes.
+	 */
+	sdev->inquiry_len = 36;
+	return 0;
+}
+
+static int slave_configure(struct scsi_device *sdev)
+{
+	/*
+	 * Scatter-gather buffers (all but the last) must have a length
+	 * divisible by the bulk maxpacket size.  Otherwise a data packet
+	 * would end up being short, causing a premature end to the data
+	 * transfer.  Since high-speed bulk pipes have a maxpacket size
+	 * of 512, we'll use that as the scsi device queue's DMA alignment
+	 * mask.  Guaranteeing proper alignment of the first buffer will
+	 * have the desired effect because, except at the beginning and
+	 * the end, scatter-gather buffers follow page boundaries.
+	 */
+	blk_queue_dma_alignment(sdev->request_queue, (512 - 1));
+
+	/* Set the SCSI level to at least 2.  We'll leave it at 3 if that's
+	 * what is originally reported.  We need this to avoid confusing
+	 * the SCSI layer with devices that report 0 or 1, but need 10-byte
+	 * commands (ala ATAPI devices behind certain bridges, or devices
+	 * which simply have broken INQUIRY data).
+	 *
+	 * NOTE: This means /dev/sg programs (ala cdrecord) will get the
+	 * actual information.  This seems to be the preference for
+	 * programs like that.
+	 *
+	 * NOTE: This also means that /proc/scsi/scsi and sysfs may report
+	 * the actual value or the modified one, depending on where the
+	 * data comes from.
+	 */
+	if (sdev->scsi_level < SCSI_2) {
+		sdev->scsi_level = SCSI_2;
+		sdev->sdev_target->scsi_level = SCSI_2;
+	}
+
+	return 0;
+}
+
+/***********************************************************************
+ * /proc/scsi/ functions
+ ***********************************************************************/
+
+/* we use this macro to help us write into the buffer */
+#undef SPRINTF
+#define SPRINTF(args...) \
+	do { \
+		if (pos < buffer + length) \
+			pos += sprintf(pos, ## args); \
+	} while (0)
+
+/* queue a command */
+/* This is always called with scsi_lock(host) held */
+static int queuecommand_lck(struct scsi_cmnd *srb)
+{
+	void (*done)(struct scsi_cmnd *) = scsi_done;
+	struct rtsx_dev *dev = host_to_rtsx(srb->device->host);
+	struct rtsx_chip *chip = dev->chip;
+
+	/* check for state-transition errors */
+	if (chip->srb) {
+		dev_err(&dev->pci->dev, "Error: chip->srb = %p\n",
+			chip->srb);
+		return SCSI_MLQUEUE_HOST_BUSY;
+	}
+
+	/* fail the command if we are disconnecting */
+	if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+		dev_info(&dev->pci->dev, "Fail command during disconnect\n");
+		srb->result = DID_NO_CONNECT << 16;
+		done(srb);
+		return 0;
+	}
+
+	/* enqueue the command and wake up the control thread */
+	chip->srb = srb;
+	complete(&dev->cmnd_ready);
+
+	return 0;
+}
+
+static DEF_SCSI_QCMD(queuecommand)
+
+/***********************************************************************
+ * Error handling functions
+ ***********************************************************************/
+
+/* Command timeout and abort */
+static int command_abort(struct scsi_cmnd *srb)
+{
+	struct Scsi_Host *host = srb->device->host;
+	struct rtsx_dev *dev = host_to_rtsx(host);
+	struct rtsx_chip *chip = dev->chip;
+
+	scsi_lock(host);
+
+	/* Is this command still active? */
+	if (chip->srb != srb) {
+		scsi_unlock(host);
+		dev_info(&dev->pci->dev, "-- nothing to abort\n");
+		return FAILED;
+	}
+
+	rtsx_set_stat(chip, RTSX_STAT_ABORT);
+
+	scsi_unlock(host);
+
+	/* Wait for the aborted command to finish */
+	wait_for_completion(&dev->notify);
+
+	return SUCCESS;
+}
+
+/*
+ * This invokes the transport reset mechanism to reset the state of the
+ * device
+ */
+static int device_reset(struct scsi_cmnd *srb)
+{
+	return SUCCESS;
+}
+
+/*
+ * this defines our host template, with which we'll allocate hosts
+ */
+
+static struct scsi_host_template rtsx_host_template = {
+	/* basic userland interface stuff */
+	.name =				CR_DRIVER_NAME,
+	.proc_name =			CR_DRIVER_NAME,
+	.info =				host_info,
+
+	/* command interface -- queued only */
+	.queuecommand =			queuecommand,
+
+	/* error and abort handlers */
+	.eh_abort_handler =		command_abort,
+	.eh_device_reset_handler =	device_reset,
+
+	/* queue commands only, only one command per LUN */
+	.can_queue =			1,
+
+	/* unknown initiator id */
+	.this_id =			-1,
+
+	.slave_alloc =			slave_alloc,
+	.slave_configure =		slave_configure,
+
+	/* lots of sg segments can be handled */
+	.sg_tablesize =			SG_ALL,
+
+	/* limit the total size of a transfer to 120 KB */
+	.max_sectors =                  240,
+
+	/* emulated HBA */
+	.emulated =			1,
+
+	/* we do our own delay after a device or bus reset */
+	.skip_settle_delay =		1,
+
+	/* module management */
+	.module =			THIS_MODULE
+};
+
+static int rtsx_acquire_irq(struct rtsx_dev *dev)
+{
+	struct rtsx_chip *chip = dev->chip;
+
+	dev_info(&dev->pci->dev, "%s: chip->msi_en = %d, pci->irq = %d\n",
+		 __func__, chip->msi_en, dev->pci->irq);
+
+	if (request_irq(dev->pci->irq, rtsx_interrupt,
+			chip->msi_en ? 0 : IRQF_SHARED,
+			CR_DRIVER_NAME, dev)) {
+		dev_err(&dev->pci->dev,
+			"rtsx: unable to grab IRQ %d, disabling device\n",
+			dev->pci->irq);
+		return -1;
+	}
+
+	dev->irq = dev->pci->irq;
+	pci_intx(dev->pci, !chip->msi_en);
+
+	return 0;
+}
+
+/*
+ * power management
+ */
+static int __maybe_unused rtsx_suspend(struct device *dev_d)
+{
+	struct pci_dev *pci = to_pci_dev(dev_d);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	if (!dev)
+		return 0;
+
+	/* lock the device pointers */
+	mutex_lock(&dev->dev_mutex);
+
+	chip = dev->chip;
+
+	rtsx_do_before_power_down(chip, PM_S3);
+
+	if (dev->irq >= 0) {
+		free_irq(dev->irq, (void *)dev);
+		dev->irq = -1;
+	}
+
+	if (chip->msi_en)
+		pci_free_irq_vectors(pci);
+
+	device_wakeup_enable(dev_d);
+
+	/* unlock the device pointers */
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+
+static int __maybe_unused rtsx_resume(struct device *dev_d)
+{
+	struct pci_dev *pci = to_pci_dev(dev_d);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	if (!dev)
+		return 0;
+
+	chip = dev->chip;
+
+	/* lock the device pointers */
+	mutex_lock(&dev->dev_mutex);
+
+	pci_set_master(pci);
+
+	if (chip->msi_en) {
+		if (pci_alloc_irq_vectors(pci, 1, 1, PCI_IRQ_MSI) < 0)
+			chip->msi_en = 0;
+	}
+
+	if (rtsx_acquire_irq(dev) < 0) {
+		/* unlock the device pointers */
+		mutex_unlock(&dev->dev_mutex);
+		return -EIO;
+	}
+
+	rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+	rtsx_init_chip(chip);
+
+	/* unlock the device pointers */
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+
+static void rtsx_shutdown(struct pci_dev *pci)
+{
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
+	struct rtsx_chip *chip;
+
+	if (!dev)
+		return;
+
+	chip = dev->chip;
+
+	rtsx_do_before_power_down(chip, PM_S1);
+
+	if (dev->irq >= 0) {
+		free_irq(dev->irq, (void *)dev);
+		dev->irq = -1;
+	}
+
+	if (chip->msi_en)
+		pci_free_irq_vectors(pci);
+
+	pci_disable_device(pci);
+}
+
+static int rtsx_control_thread(void *__dev)
+{
+	struct rtsx_dev *dev = __dev;
+	struct rtsx_chip *chip = dev->chip;
+	struct Scsi_Host *host = rtsx_to_host(dev);
+
+	for (;;) {
+		if (wait_for_completion_interruptible(&dev->cmnd_ready))
+			break;
+
+		/* lock the device pointers */
+		mutex_lock(&dev->dev_mutex);
+
+		/* if the device has disconnected, we are free to exit */
+		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+			dev_info(&dev->pci->dev, "-- rtsx-control exiting\n");
+			mutex_unlock(&dev->dev_mutex);
+			break;
+		}
+
+		/* lock access to the state */
+		scsi_lock(host);
+
+		/* has the command aborted ? */
+		if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+			chip->srb->result = DID_ABORT << 16;
+			goto skip_for_abort;
+		}
+
+		scsi_unlock(host);
+
+		/* reject the command if the direction indicator
+		 * is UNKNOWN
+		 */
+		if (chip->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
+			dev_err(&dev->pci->dev, "UNKNOWN data direction\n");
+			chip->srb->result = DID_ERROR << 16;
+		}
+
+		/* reject if target != 0 or if LUN is higher than
+		 * the maximum known LUN
+		 */
+		else if (chip->srb->device->id) {
+			dev_err(&dev->pci->dev, "Bad target number (%d:%d)\n",
+				chip->srb->device->id,
+				(u8)chip->srb->device->lun);
+			chip->srb->result = DID_BAD_TARGET << 16;
+		}
+
+		else if (chip->srb->device->lun > chip->max_lun) {
+			dev_err(&dev->pci->dev, "Bad LUN (%d:%d)\n",
+				chip->srb->device->id,
+				(u8)chip->srb->device->lun);
+			chip->srb->result = DID_BAD_TARGET << 16;
+		}
+
+		/* we've got a command, let's do it! */
+		else {
+			scsi_show_command(chip);
+			rtsx_invoke_transport(chip->srb, chip);
+		}
+
+		/* lock access to the state */
+		scsi_lock(host);
+
+		/* did the command already complete because of a disconnect? */
+		if (!chip->srb)
+			;		/* nothing to do */
+
+		/* indicate that the command is done */
+		else if (chip->srb->result != DID_ABORT << 16) {
+			scsi_done(chip->srb);
+		} else {
+skip_for_abort:
+			dev_err(&dev->pci->dev, "scsi command aborted\n");
+		}
+
+		if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+			complete(&dev->notify);
+
+			rtsx_set_stat(chip, RTSX_STAT_IDLE);
+		}
+
+		/* finished working on this command */
+		chip->srb = NULL;
+		scsi_unlock(host);
+
+		/* unlock the device pointers */
+		mutex_unlock(&dev->dev_mutex);
+	} /* for (;;) */
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * kthread_complete_and_exit() goes even further than this --
+	 * it is safe in the case that the thread of the caller is going away
+	 * (not just the structure) -- this is necessary for the module-remove
+	 * case.  This is important in preemption kernels, which transfer the
+	 * flow of execution immediately upon a complete().
+	 */
+	kthread_complete_and_exit(&dev->control_exit, 0);
+}
+
+static int rtsx_polling_thread(void *__dev)
+{
+	struct rtsx_dev *dev = __dev;
+	struct rtsx_chip *chip = dev->chip;
+	struct sd_info *sd_card = &chip->sd_card;
+	struct xd_info *xd_card = &chip->xd_card;
+	struct ms_info *ms_card = &chip->ms_card;
+
+	sd_card->cleanup_counter = 0;
+	xd_card->cleanup_counter = 0;
+	ms_card->cleanup_counter = 0;
+
+	/* Wait until SCSI scan finished */
+	wait_timeout((delay_use + 5) * 1000);
+
+	for (;;) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(msecs_to_jiffies(POLLING_INTERVAL));
+
+		/* lock the device pointers */
+		mutex_lock(&dev->dev_mutex);
+
+		/* if the device has disconnected, we are free to exit */
+		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+			dev_info(&dev->pci->dev, "-- rtsx-polling exiting\n");
+			mutex_unlock(&dev->dev_mutex);
+			break;
+		}
+
+		mutex_unlock(&dev->dev_mutex);
+
+		mspro_polling_format_status(chip);
+
+		/* lock the device pointers */
+		mutex_lock(&dev->dev_mutex);
+
+		rtsx_polling_func(chip);
+
+		/* unlock the device pointers */
+		mutex_unlock(&dev->dev_mutex);
+	}
+
+	kthread_complete_and_exit(&dev->polling_exit, 0);
+}
+
+/*
+ * interrupt handler
+ */
+static irqreturn_t rtsx_interrupt(int irq, void *dev_id)
+{
+	struct rtsx_dev *dev = dev_id;
+	struct rtsx_chip *chip;
+	int retval;
+	u32 status;
+
+	if (dev)
+		chip = dev->chip;
+	else
+		return IRQ_NONE;
+
+	if (!chip)
+		return IRQ_NONE;
+
+	spin_lock(&dev->reg_lock);
+
+	retval = rtsx_pre_handle_interrupt(chip);
+	if (retval == STATUS_FAIL) {
+		spin_unlock(&dev->reg_lock);
+		if (chip->int_reg == 0xFFFFFFFF)
+			return IRQ_HANDLED;
+		return IRQ_NONE;
+	}
+
+	status = chip->int_reg;
+
+	if (dev->check_card_cd) {
+		if (!(dev->check_card_cd & status)) {
+			/* card not exist, return TRANS_RESULT_FAIL */
+			dev->trans_result = TRANS_RESULT_FAIL;
+			if (dev->done)
+				complete(dev->done);
+			goto exit;
+		}
+	}
+
+	if (status & (NEED_COMPLETE_INT | DELINK_INT)) {
+		if (status & (TRANS_FAIL_INT | DELINK_INT)) {
+			if (status & DELINK_INT)
+				RTSX_SET_DELINK(chip);
+			dev->trans_result = TRANS_RESULT_FAIL;
+			if (dev->done)
+				complete(dev->done);
+		} else if (status & TRANS_OK_INT) {
+			dev->trans_result = TRANS_RESULT_OK;
+			if (dev->done)
+				complete(dev->done);
+		} else if (status & DATA_DONE_INT) {
+			dev->trans_result = TRANS_NOT_READY;
+			if (dev->done && dev->trans_state == STATE_TRANS_SG)
+				complete(dev->done);
+		}
+	}
+
+exit:
+	spin_unlock(&dev->reg_lock);
+	return IRQ_HANDLED;
+}
+
+/* Release all our dynamic resources */
+static void rtsx_release_resources(struct rtsx_dev *dev)
+{
+	dev_info(&dev->pci->dev, "-- %s\n", __func__);
+
+	/* Tell the control thread to exit.  The SCSI host must
+	 * already have been removed so it won't try to queue
+	 * any more commands.
+	 */
+	dev_info(&dev->pci->dev, "-- sending exit command to thread\n");
+	complete(&dev->cmnd_ready);
+	if (dev->ctl_thread)
+		wait_for_completion(&dev->control_exit);
+	if (dev->polling_thread)
+		wait_for_completion(&dev->polling_exit);
+
+	wait_timeout(200);
+
+	if (dev->rtsx_resv_buf) {
+		dev->chip->host_cmds_ptr = NULL;
+		dev->chip->host_sg_tbl_ptr = NULL;
+	}
+
+	if (dev->irq > 0)
+		free_irq(dev->irq, (void *)dev);
+	if (dev->chip->msi_en)
+		pci_free_irq_vectors(dev->pci);
+	if (dev->remap_addr)
+		iounmap(dev->remap_addr);
+
+	rtsx_release_chip(dev->chip);
+	kfree(dev->chip);
+}
+
+/*
+ * First stage of disconnect processing: stop all commands and remove
+ * the host
+ */
+static void quiesce_and_remove_host(struct rtsx_dev *dev)
+{
+	struct Scsi_Host *host = rtsx_to_host(dev);
+	struct rtsx_chip *chip = dev->chip;
+
+	/*
+	 * Prevent new transfers, stop the current command, and
+	 * interrupt a SCSI-scan or device-reset delay
+	 */
+	mutex_lock(&dev->dev_mutex);
+	scsi_lock(host);
+	rtsx_set_stat(chip, RTSX_STAT_DISCONNECT);
+	scsi_unlock(host);
+	mutex_unlock(&dev->dev_mutex);
+	wake_up(&dev->delay_wait);
+	wait_for_completion(&dev->scanning_done);
+
+	/* Wait some time to let other threads exist */
+	wait_timeout(100);
+
+	/*
+	 * queuecommand won't accept any new commands and the control
+	 * thread won't execute a previously-queued command.  If there
+	 * is such a command pending, complete it with an error.
+	 */
+	mutex_lock(&dev->dev_mutex);
+	if (chip->srb) {
+		chip->srb->result = DID_NO_CONNECT << 16;
+		scsi_lock(host);
+		scsi_done(dev->chip->srb);
+		chip->srb = NULL;
+		scsi_unlock(host);
+	}
+	mutex_unlock(&dev->dev_mutex);
+
+	/* Now we own no commands so it's safe to remove the SCSI host */
+	scsi_remove_host(host);
+}
+
+/* Second stage of disconnect processing: deallocate all resources */
+static void release_everything(struct rtsx_dev *dev)
+{
+	rtsx_release_resources(dev);
+
+	/*
+	 * Drop our reference to the host; the SCSI core will free it
+	 * when the refcount becomes 0.
+	 */
+	scsi_host_put(rtsx_to_host(dev));
+}
+
+/* Thread to carry out delayed SCSI-device scanning */
+static int rtsx_scan_thread(void *__dev)
+{
+	struct rtsx_dev *dev = __dev;
+	struct rtsx_chip *chip = dev->chip;
+
+	/* Wait for the timeout to expire or for a disconnect */
+	if (delay_use > 0) {
+		dev_info(&dev->pci->dev,
+			 "%s: waiting for device to settle before scanning\n",
+			 CR_DRIVER_NAME);
+		wait_event_interruptible_timeout
+			(dev->delay_wait,
+			 rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT),
+			 delay_use * HZ);
+	}
+
+	/* If the device is still connected, perform the scanning */
+	if (!rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
+		scsi_scan_host(rtsx_to_host(dev));
+		dev_info(&dev->pci->dev, "%s: device scan complete\n",
+			 CR_DRIVER_NAME);
+
+		/* Should we unbind if no devices were detected? */
+	}
+
+	kthread_complete_and_exit(&dev->scanning_done, 0);
+}
+
+static void rtsx_init_options(struct rtsx_chip *chip)
+{
+	chip->vendor_id = chip->rtsx->pci->vendor;
+	chip->product_id = chip->rtsx->pci->device;
+	chip->adma_mode = 1;
+	chip->lun_mc = 0;
+	chip->driver_first_load = 1;
+#ifdef HW_AUTO_SWITCH_SD_BUS
+	chip->sdio_in_charge = 0;
+#endif
+
+	chip->mspro_formatter_enable = 1;
+	chip->ignore_sd = 0;
+	chip->use_hw_setting = 0;
+	chip->lun_mode = DEFAULT_SINGLE;
+	chip->auto_delink_en = auto_delink_en;
+	chip->ss_en = ss_en;
+	chip->ss_idle_period = ss_interval * 1000;
+	chip->remote_wakeup_en = 0;
+	chip->aspm_l0s_l1_en = aspm_l0s_l1_en;
+	chip->dynamic_aspm = 1;
+	chip->fpga_sd_sdr104_clk = CLK_200;
+	chip->fpga_sd_ddr50_clk = CLK_100;
+	chip->fpga_sd_sdr50_clk = CLK_100;
+	chip->fpga_sd_hs_clk = CLK_100;
+	chip->fpga_mmc_52m_clk = CLK_80;
+	chip->fpga_ms_hg_clk = CLK_80;
+	chip->fpga_ms_4bit_clk = CLK_80;
+	chip->fpga_ms_1bit_clk = CLK_40;
+	chip->asic_sd_sdr104_clk = 203;
+	chip->asic_sd_sdr50_clk = 98;
+	chip->asic_sd_ddr50_clk = 98;
+	chip->asic_sd_hs_clk = 98;
+	chip->asic_mmc_52m_clk = 98;
+	chip->asic_ms_hg_clk = 117;
+	chip->asic_ms_4bit_clk = 78;
+	chip->asic_ms_1bit_clk = 39;
+	chip->ssc_depth_sd_sdr104 = SSC_DEPTH_2M;
+	chip->ssc_depth_sd_sdr50 = SSC_DEPTH_2M;
+	chip->ssc_depth_sd_ddr50 = SSC_DEPTH_1M;
+	chip->ssc_depth_sd_hs = SSC_DEPTH_1M;
+	chip->ssc_depth_mmc_52m = SSC_DEPTH_1M;
+	chip->ssc_depth_ms_hg = SSC_DEPTH_1M;
+	chip->ssc_depth_ms_4bit = SSC_DEPTH_512K;
+	chip->ssc_depth_low_speed = SSC_DEPTH_512K;
+	chip->ssc_en = 1;
+	chip->sd_speed_prior = 0x01040203;
+	chip->sd_current_prior = 0x00010203;
+	chip->sd_ctl = SD_PUSH_POINT_AUTO |
+		       SD_SAMPLE_POINT_AUTO |
+		       SUPPORT_MMC_DDR_MODE;
+	chip->sd_ddr_tx_phase = 0;
+	chip->mmc_ddr_tx_phase = 1;
+	chip->sd_default_tx_phase = 15;
+	chip->sd_default_rx_phase = 15;
+	chip->pmos_pwr_on_interval = 200;
+	chip->sd_voltage_switch_delay = 1000;
+	chip->ms_power_class_en = 3;
+
+	chip->sd_400mA_ocp_thd = 1;
+	chip->sd_800mA_ocp_thd = 5;
+	chip->ms_ocp_thd = 2;
+
+	chip->card_drive_sel = 0x55;
+	chip->sd30_drive_sel_1v8 = 0x03;
+	chip->sd30_drive_sel_3v3 = 0x01;
+
+	chip->do_delink_before_power_down = 1;
+	chip->auto_power_down = 1;
+	chip->polling_config = 0;
+
+	chip->force_clkreq_0 = 1;
+	chip->ft2_fast_mode = 0;
+
+	chip->sdio_retry_cnt = 1;
+
+	chip->xd_timeout = 2000;
+	chip->sd_timeout = 10000;
+	chip->ms_timeout = 2000;
+	chip->mspro_timeout = 15000;
+
+	chip->power_down_in_ss = 1;
+
+	chip->sdr104_en = 1;
+	chip->sdr50_en = 1;
+	chip->ddr50_en = 1;
+
+	chip->delink_stage1_step = 100;
+	chip->delink_stage2_step = 40;
+	chip->delink_stage3_step = 20;
+
+	chip->auto_delink_in_L1 = 1;
+	chip->blink_led = 1;
+	chip->msi_en = msi_en;
+	chip->hp_watch_bios_hotplug = 0;
+	chip->max_payload = 0;
+	chip->phy_voltage = 0;
+
+	chip->support_ms_8bit = 1;
+	chip->s3_pwr_off_delay = 1000;
+}
+
+static int rtsx_probe(struct pci_dev *pci,
+		      const struct pci_device_id *pci_id)
+{
+	struct Scsi_Host *host;
+	struct rtsx_dev *dev;
+	int err = 0;
+	struct task_struct *th;
+
+	dev_dbg(&pci->dev, "Realtek PCI-E card reader detected\n");
+
+	err = pcim_enable_device(pci);
+	if (err < 0) {
+		dev_err(&pci->dev, "PCI enable device failed!\n");
+		return err;
+	}
+
+	err = pci_request_regions(pci, CR_DRIVER_NAME);
+	if (err < 0) {
+		dev_err(&pci->dev, "PCI request regions for %s failed!\n",
+			CR_DRIVER_NAME);
+		return err;
+	}
+
+	/*
+	 * Ask the SCSI layer to allocate a host structure, with extra
+	 * space at the end for our private rtsx_dev structure.
+	 */
+	host = scsi_host_alloc(&rtsx_host_template, sizeof(*dev));
+	if (!host) {
+		dev_err(&pci->dev, "Unable to allocate the scsi host\n");
+		err = -ENOMEM;
+		goto scsi_host_alloc_fail;
+	}
+
+	dev = host_to_rtsx(host);
+	memset(dev, 0, sizeof(struct rtsx_dev));
+
+	dev->chip = kzalloc(sizeof(*dev->chip), GFP_KERNEL);
+	if (!dev->chip) {
+		err = -ENOMEM;
+		goto chip_alloc_fail;
+	}
+
+	spin_lock_init(&dev->reg_lock);
+	mutex_init(&dev->dev_mutex);
+	init_completion(&dev->cmnd_ready);
+	init_completion(&dev->control_exit);
+	init_completion(&dev->polling_exit);
+	init_completion(&dev->notify);
+	init_completion(&dev->scanning_done);
+	init_waitqueue_head(&dev->delay_wait);
+
+	dev->pci = pci;
+	dev->irq = -1;
+
+	dev_info(&pci->dev, "Resource length: 0x%x\n",
+		 (unsigned int)pci_resource_len(pci, 0));
+	dev->addr = pci_resource_start(pci, 0);
+	dev->remap_addr = ioremap(dev->addr, pci_resource_len(pci, 0));
+	if (!dev->remap_addr) {
+		dev_err(&pci->dev, "ioremap error\n");
+		err = -ENXIO;
+		goto ioremap_fail;
+	}
+
+	/*
+	 * Using "unsigned long" cast here to eliminate gcc warning in
+	 * 64-bit system
+	 */
+	dev_info(&pci->dev, "Original address: 0x%lx, remapped address: 0x%lx\n",
+		 (unsigned long)(dev->addr), (unsigned long)(dev->remap_addr));
+
+	dev->rtsx_resv_buf = dmam_alloc_coherent(&pci->dev, RTSX_RESV_BUF_LEN,
+						 &dev->rtsx_resv_buf_addr,
+						 GFP_KERNEL);
+	if (!dev->rtsx_resv_buf) {
+		dev_err(&pci->dev, "alloc dma buffer fail\n");
+		err = -ENXIO;
+		goto dma_alloc_fail;
+	}
+	dev->chip->host_cmds_ptr = dev->rtsx_resv_buf;
+	dev->chip->host_cmds_addr = dev->rtsx_resv_buf_addr;
+	dev->chip->host_sg_tbl_ptr = dev->rtsx_resv_buf + HOST_CMDS_BUF_LEN;
+	dev->chip->host_sg_tbl_addr = dev->rtsx_resv_buf_addr +
+				      HOST_CMDS_BUF_LEN;
+
+	dev->chip->rtsx = dev;
+
+	rtsx_init_options(dev->chip);
+
+	dev_info(&pci->dev, "pci->irq = %d\n", pci->irq);
+
+	if (dev->chip->msi_en) {
+		if (pci_alloc_irq_vectors(pci, 1, 1, PCI_IRQ_MSI) < 0)
+			dev->chip->msi_en = 0;
+	}
+
+	if (rtsx_acquire_irq(dev) < 0) {
+		err = -EBUSY;
+		goto irq_acquire_fail;
+	}
+
+	pci_set_master(pci);
+	synchronize_irq(dev->irq);
+
+	rtsx_init_chip(dev->chip);
+
+	/*
+	 * set the supported max_lun and max_id for the scsi host
+	 * NOTE: the minimal value of max_id is 1
+	 */
+	host->max_id = 1;
+	host->max_lun = dev->chip->max_lun;
+
+	/* Start up our control thread */
+	th = kthread_run(rtsx_control_thread, dev, CR_DRIVER_NAME);
+	if (IS_ERR(th)) {
+		dev_err(&pci->dev, "Unable to start control thread\n");
+		err = PTR_ERR(th);
+		goto control_thread_fail;
+	}
+	dev->ctl_thread = th;
+
+	err = scsi_add_host(host, &pci->dev);
+	if (err) {
+		dev_err(&pci->dev, "Unable to add the scsi host\n");
+		goto scsi_add_host_fail;
+	}
+
+	/* Start up the thread for delayed SCSI-device scanning */
+	th = kthread_run(rtsx_scan_thread, dev, "rtsx-scan");
+	if (IS_ERR(th)) {
+		dev_err(&pci->dev, "Unable to start the device-scanning thread\n");
+		complete(&dev->scanning_done);
+		err = PTR_ERR(th);
+		goto scan_thread_fail;
+	}
+
+	/* Start up the thread for polling thread */
+	th = kthread_run(rtsx_polling_thread, dev, "rtsx-polling");
+	if (IS_ERR(th)) {
+		dev_err(&pci->dev, "Unable to start the device-polling thread\n");
+		err = PTR_ERR(th);
+		goto scan_thread_fail;
+	}
+	dev->polling_thread = th;
+
+	pci_set_drvdata(pci, dev);
+
+	return 0;
+
+	/* We come here if there are any problems */
+scan_thread_fail:
+	quiesce_and_remove_host(dev);
+scsi_add_host_fail:
+	complete(&dev->cmnd_ready);
+	wait_for_completion(&dev->control_exit);
+control_thread_fail:
+	free_irq(dev->irq, (void *)dev);
+	rtsx_release_chip(dev->chip);
+irq_acquire_fail:
+	dev->chip->host_cmds_ptr = NULL;
+	dev->chip->host_sg_tbl_ptr = NULL;
+	if (dev->chip->msi_en)
+		pci_free_irq_vectors(dev->pci);
+dma_alloc_fail:
+	iounmap(dev->remap_addr);
+ioremap_fail:
+	kfree(dev->chip);
+chip_alloc_fail:
+	dev_err(&pci->dev, "%s failed\n", __func__);
+	scsi_host_put(host);
+scsi_host_alloc_fail:
+	pci_release_regions(pci);
+	return err;
+}
+
+static void rtsx_remove(struct pci_dev *pci)
+{
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
+
+	quiesce_and_remove_host(dev);
+	release_everything(dev);
+	pci_release_regions(pci);
+}
+
+/* PCI IDs */
+static const struct pci_device_id rtsx_ids[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5208),
+		PCI_CLASS_OTHERS << 16, 0xFF0000 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5288),
+		PCI_CLASS_OTHERS << 16, 0xFF0000 },
+	{ 0, },
+};
+
+MODULE_DEVICE_TABLE(pci, rtsx_ids);
+
+static SIMPLE_DEV_PM_OPS(rtsx_pm_ops, rtsx_suspend, rtsx_resume);
+
+/* pci_driver definition */
+static struct pci_driver rtsx_driver = {
+	.name = CR_DRIVER_NAME,
+	.id_table = rtsx_ids,
+	.probe = rtsx_probe,
+	.remove = rtsx_remove,
+	.driver.pm = &rtsx_pm_ops,
+	.shutdown = rtsx_shutdown,
+};
+
+module_pci_driver(rtsx_driver);
diff --git a/drivers/staging/rts5208/rtsx.h b/drivers/staging/rts5208/rtsx.h
new file mode 100644
index 000000000..2e101da83
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx.h
@@ -0,0 +1,150 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_H
+#define __REALTEK_RTSX_H
+
+#include <linux/io.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+#include <linux/mutex.h>
+#include <linux/cdrom.h>
+#include <linux/workqueue.h>
+#include <linux/timer.h>
+#include <linux/time64.h>
+
+#include <scsi/scsi.h>
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
+#include <scsi/scsi_devinfo.h>
+#include <scsi/scsi_eh.h>
+#include <scsi/scsi_host.h>
+
+#define CR_DRIVER_NAME		"rts5208"
+
+/*
+ * macros for easy use
+ */
+#define rtsx_writel(chip, reg, value) \
+	iowrite32(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readl(chip, reg) \
+	ioread32((chip)->rtsx->remap_addr + reg)
+#define rtsx_writew(chip, reg, value) \
+	iowrite16(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readw(chip, reg) \
+	ioread16((chip)->rtsx->remap_addr + reg)
+#define rtsx_writeb(chip, reg, value) \
+	iowrite8(value, (chip)->rtsx->remap_addr + reg)
+#define rtsx_readb(chip, reg) \
+	ioread8((chip)->rtsx->remap_addr + reg)
+
+#define rtsx_read_config_byte(chip, where, val) \
+	pci_read_config_byte((chip)->rtsx->pci, where, val)
+
+#define rtsx_write_config_byte(chip, where, val) \
+	pci_write_config_byte((chip)->rtsx->pci, where, val)
+
+#define wait_timeout_x(task_state, msecs)	\
+do {						\
+	set_current_state((task_state));	\
+	schedule_timeout((msecs) * HZ / 1000);	\
+} while (0)
+#define wait_timeout(msecs)	wait_timeout_x(TASK_INTERRUPTIBLE, (msecs))
+
+#define STATE_TRANS_NONE	0
+#define STATE_TRANS_CMD		1
+#define STATE_TRANS_BUF		2
+#define STATE_TRANS_SG		3
+
+#define TRANS_NOT_READY		0
+#define TRANS_RESULT_OK		1
+#define TRANS_RESULT_FAIL	2
+
+#define SCSI_LUN(srb)		((srb)->device->lun)
+
+struct rtsx_chip;
+
+struct rtsx_dev {
+	struct pci_dev *pci;
+
+	/* pci resources */
+	unsigned long		addr;
+	void __iomem		*remap_addr;
+	int irq;
+
+	/* locks */
+	spinlock_t		reg_lock;
+
+	struct task_struct	*ctl_thread;	 /* the control thread   */
+	struct task_struct	*polling_thread; /* the polling thread   */
+
+	/* mutual exclusion and synchronization structures */
+	struct completion	cmnd_ready;	 /* to sleep thread on	    */
+	struct completion	control_exit;	 /* control thread exit	    */
+	struct completion	polling_exit;	 /* polling thread exit	    */
+	struct completion	notify;		 /* thread begin/end	    */
+	struct completion	scanning_done;	 /* wait for scan thread    */
+
+	wait_queue_head_t	delay_wait;	 /* wait during scan, reset */
+	struct mutex		dev_mutex;
+
+	/* host reserved buffer */
+	void			*rtsx_resv_buf;
+	dma_addr_t		rtsx_resv_buf_addr;
+
+	char			trans_result;
+	char			trans_state;
+
+	struct completion	*done;
+	/* Whether interrupt handler should care card cd info */
+	u32			check_card_cd;
+
+	struct rtsx_chip	*chip;
+};
+
+/* Convert between rtsx_dev and the corresponding Scsi_Host */
+static inline struct Scsi_Host *rtsx_to_host(struct rtsx_dev *dev)
+{
+	return container_of((void *)dev, struct Scsi_Host, hostdata);
+}
+
+static inline struct rtsx_dev *host_to_rtsx(struct Scsi_Host *host)
+{
+	return (struct rtsx_dev *)host->hostdata;
+}
+
+/*
+ * The scsi_lock() and scsi_unlock() macros protect the sm_state and the
+ * single queue element srb for write access
+ */
+#define scsi_unlock(host)	spin_unlock_irq(host->host_lock)
+#define scsi_lock(host)		spin_lock_irq(host->host_lock)
+
+#define lock_state(chip)	spin_lock_irq(&((chip)->rtsx->reg_lock))
+#define unlock_state(chip)	spin_unlock_irq(&((chip)->rtsx->reg_lock))
+
+/* struct scsi_cmnd transfer buffer access utilities */
+enum xfer_buf_dir	{TO_XFER_BUF, FROM_XFER_BUF};
+
+#include "rtsx_chip.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "rtsx_sys.h"
+#include "general.h"
+
+#endif  /* __REALTEK_RTSX_H */
diff --git a/drivers/staging/rts5208/rtsx_card.c b/drivers/staging/rts5208/rtsx_card.c
new file mode 100644
index 000000000..326b04756
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_card.c
@@ -0,0 +1,1151 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+#include <linux/kernel.h>
+
+#include "rtsx.h"
+#include "sd.h"
+#include "xd.h"
+#include "ms.h"
+
+void do_remaining_work(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+#ifdef XD_DELAY_WRITE
+	struct xd_info *xd_card = &chip->xd_card;
+#endif
+	struct ms_info *ms_card = &chip->ms_card;
+
+	if (chip->card_ready & SD_CARD) {
+		if (sd_card->seq_mode) {
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+			sd_card->cleanup_counter++;
+		} else {
+			sd_card->cleanup_counter = 0;
+		}
+	}
+
+#ifdef XD_DELAY_WRITE
+	if (chip->card_ready & XD_CARD) {
+		if (xd_card->delay_write.delay_write_flag) {
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+			xd_card->cleanup_counter++;
+		} else {
+			xd_card->cleanup_counter = 0;
+		}
+	}
+#endif
+
+	if (chip->card_ready & MS_CARD) {
+		if (CHK_MSPRO(ms_card)) {
+			if (ms_card->seq_mode) {
+				rtsx_set_stat(chip, RTSX_STAT_RUN);
+				ms_card->cleanup_counter++;
+			} else {
+				ms_card->cleanup_counter = 0;
+			}
+		} else {
+#ifdef MS_DELAY_WRITE
+			if (ms_card->delay_write.delay_write_flag) {
+				rtsx_set_stat(chip, RTSX_STAT_RUN);
+				ms_card->cleanup_counter++;
+			} else {
+				ms_card->cleanup_counter = 0;
+			}
+#endif
+		}
+	}
+
+	if (sd_card->cleanup_counter > POLLING_WAIT_CNT)
+		sd_cleanup_work(chip);
+
+	if (xd_card->cleanup_counter > POLLING_WAIT_CNT)
+		xd_cleanup_work(chip);
+
+	if (ms_card->cleanup_counter > POLLING_WAIT_CNT)
+		ms_cleanup_work(chip);
+}
+
+void try_to_switch_sdio_ctrl(struct rtsx_chip *chip)
+{
+	u8 reg1 = 0, reg2 = 0;
+
+	rtsx_read_register(chip, 0xFF34, &reg1);
+	rtsx_read_register(chip, 0xFF38, &reg2);
+	dev_dbg(rtsx_dev(chip), "reg 0xFF34: 0x%x, reg 0xFF38: 0x%x\n",
+		reg1, reg2);
+	if ((reg1 & 0xC0) && (reg2 & 0xC0)) {
+		chip->sd_int = 1;
+		rtsx_write_register(chip, SDIO_CTRL, 0xFF,
+				    SDIO_BUS_CTRL | SDIO_CD_CTRL);
+		rtsx_write_register(chip, PWR_GATE_CTRL,
+				    LDO3318_PWR_MASK, LDO_ON);
+	}
+}
+
+#ifdef SUPPORT_SDIO_ASPM
+void dynamic_configure_sdio_aspm(struct rtsx_chip *chip)
+{
+	u8 buf[12], reg;
+	int i;
+
+	for (i = 0; i < 12; i++)
+		rtsx_read_register(chip, 0xFF08 + i, &buf[i]);
+	rtsx_read_register(chip, 0xFF25, &reg);
+	if ((memcmp(buf, chip->sdio_raw_data, 12) != 0) || (reg & 0x03)) {
+		chip->sdio_counter = 0;
+		chip->sdio_idle = 0;
+	} else {
+		if (!chip->sdio_idle) {
+			chip->sdio_counter++;
+			if (chip->sdio_counter >= SDIO_IDLE_COUNT) {
+				chip->sdio_counter = 0;
+				chip->sdio_idle = 1;
+			}
+		}
+	}
+	memcpy(chip->sdio_raw_data, buf, 12);
+
+	if (chip->sdio_idle) {
+		if (!chip->sdio_aspm) {
+			dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
+			rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFC,
+					    0x30 | (chip->aspm_level[1] << 2));
+			chip->sdio_aspm = 1;
+		}
+	} else {
+		if (chip->sdio_aspm) {
+			dev_dbg(rtsx_dev(chip), "SDIO exit ASPM!\n");
+			rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFC, 0x30);
+			chip->sdio_aspm = 0;
+		}
+	}
+}
+#endif
+
+void do_reset_sd_card(struct rtsx_chip *chip)
+{
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "%s: %d, card2lun = 0x%x\n", __func__,
+		chip->sd_reset_counter, chip->card2lun[SD_CARD]);
+
+	if (chip->card2lun[SD_CARD] >= MAX_ALLOWED_LUN_CNT) {
+		clear_bit(SD_NR, &chip->need_reset);
+		chip->sd_reset_counter = 0;
+		chip->sd_show_cnt = 0;
+		return;
+	}
+
+	chip->rw_fail_cnt[chip->card2lun[SD_CARD]] = 0;
+
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+	rtsx_write_register(chip, SDIO_CTRL, 0xFF, 0);
+
+	retval = reset_sd_card(chip);
+	if (chip->need_release & SD_CARD)
+		return;
+	if (retval == STATUS_SUCCESS) {
+		clear_bit(SD_NR, &chip->need_reset);
+		chip->sd_reset_counter = 0;
+		chip->sd_show_cnt = 0;
+		chip->card_ready |= SD_CARD;
+		chip->card_fail &= ~SD_CARD;
+		chip->rw_card[chip->card2lun[SD_CARD]] = sd_rw;
+	} else {
+		if (chip->sd_io || chip->sd_reset_counter >= MAX_RESET_CNT) {
+			clear_bit(SD_NR, &chip->need_reset);
+			chip->sd_reset_counter = 0;
+			chip->sd_show_cnt = 0;
+		} else {
+			chip->sd_reset_counter++;
+		}
+		chip->card_ready &= ~SD_CARD;
+		chip->card_fail |= SD_CARD;
+		chip->capacity[chip->card2lun[SD_CARD]] = 0;
+		chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
+
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		if (!chip->ft2_fast_mode)
+			card_power_off(chip, SD_CARD);
+		if (chip->sd_io) {
+			chip->sd_int = 0;
+			try_to_switch_sdio_ctrl(chip);
+		} else {
+			disable_card_clock(chip, SD_CARD);
+		}
+	}
+}
+
+void do_reset_xd_card(struct rtsx_chip *chip)
+{
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "%s: %d, card2lun = 0x%x\n", __func__,
+		chip->xd_reset_counter, chip->card2lun[XD_CARD]);
+
+	if (chip->card2lun[XD_CARD] >= MAX_ALLOWED_LUN_CNT) {
+		clear_bit(XD_NR, &chip->need_reset);
+		chip->xd_reset_counter = 0;
+		chip->xd_show_cnt = 0;
+		return;
+	}
+
+	chip->rw_fail_cnt[chip->card2lun[XD_CARD]] = 0;
+
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+	rtsx_write_register(chip, SDIO_CTRL, 0xFF, 0);
+
+	retval = reset_xd_card(chip);
+	if (chip->need_release & XD_CARD)
+		return;
+	if (retval == STATUS_SUCCESS) {
+		clear_bit(XD_NR, &chip->need_reset);
+		chip->xd_reset_counter = 0;
+		chip->card_ready |= XD_CARD;
+		chip->card_fail &= ~XD_CARD;
+		chip->rw_card[chip->card2lun[XD_CARD]] = xd_rw;
+	} else {
+		if (chip->xd_reset_counter >= MAX_RESET_CNT) {
+			clear_bit(XD_NR, &chip->need_reset);
+			chip->xd_reset_counter = 0;
+			chip->xd_show_cnt = 0;
+		} else {
+			chip->xd_reset_counter++;
+		}
+		chip->card_ready &= ~XD_CARD;
+		chip->card_fail |= XD_CARD;
+		chip->capacity[chip->card2lun[XD_CARD]] = 0;
+		chip->rw_card[chip->card2lun[XD_CARD]] = NULL;
+
+		rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+		if (!chip->ft2_fast_mode)
+			card_power_off(chip, XD_CARD);
+		disable_card_clock(chip, XD_CARD);
+	}
+}
+
+void do_reset_ms_card(struct rtsx_chip *chip)
+{
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "%s: %d, card2lun = 0x%x\n", __func__,
+		chip->ms_reset_counter, chip->card2lun[MS_CARD]);
+
+	if (chip->card2lun[MS_CARD] >= MAX_ALLOWED_LUN_CNT) {
+		clear_bit(MS_NR, &chip->need_reset);
+		chip->ms_reset_counter = 0;
+		chip->ms_show_cnt = 0;
+		return;
+	}
+
+	chip->rw_fail_cnt[chip->card2lun[MS_CARD]] = 0;
+
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+	rtsx_write_register(chip, SDIO_CTRL, 0xFF, 0);
+
+	retval = reset_ms_card(chip);
+	if (chip->need_release & MS_CARD)
+		return;
+	if (retval == STATUS_SUCCESS) {
+		clear_bit(MS_NR, &chip->need_reset);
+		chip->ms_reset_counter = 0;
+		chip->card_ready |= MS_CARD;
+		chip->card_fail &= ~MS_CARD;
+		chip->rw_card[chip->card2lun[MS_CARD]] = ms_rw;
+	} else {
+		if (chip->ms_reset_counter >= MAX_RESET_CNT) {
+			clear_bit(MS_NR, &chip->need_reset);
+			chip->ms_reset_counter = 0;
+			chip->ms_show_cnt = 0;
+		} else {
+			chip->ms_reset_counter++;
+		}
+		chip->card_ready &= ~MS_CARD;
+		chip->card_fail |= MS_CARD;
+		chip->capacity[chip->card2lun[MS_CARD]] = 0;
+		chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
+
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		if (!chip->ft2_fast_mode)
+			card_power_off(chip, MS_CARD);
+		disable_card_clock(chip, MS_CARD);
+	}
+}
+
+static void release_sdio(struct rtsx_chip *chip)
+{
+	if (chip->sd_io) {
+		rtsx_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+				    SD_STOP | SD_CLR_ERR);
+
+		if (chip->chip_insert_with_sdio) {
+			chip->chip_insert_with_sdio = 0;
+
+			if (CHECK_PID(chip, 0x5288))
+				rtsx_write_register(chip, 0xFE5A, 0x08, 0x00);
+			else
+				rtsx_write_register(chip, 0xFE70, 0x80, 0x00);
+		}
+
+		rtsx_write_register(chip, SDIO_CTRL, SDIO_CD_CTRL, 0);
+		chip->sd_io = 0;
+	}
+}
+
+void rtsx_power_off_card(struct rtsx_chip *chip)
+{
+	if ((chip->card_ready & SD_CARD) || chip->sd_io) {
+		sd_cleanup_work(chip);
+		sd_power_off_card3v3(chip);
+	}
+
+	if (chip->card_ready & XD_CARD) {
+		xd_cleanup_work(chip);
+		xd_power_off_card3v3(chip);
+	}
+
+	if (chip->card_ready & MS_CARD) {
+		ms_cleanup_work(chip);
+		ms_power_off_card3v3(chip);
+	}
+}
+
+void rtsx_release_cards(struct rtsx_chip *chip)
+{
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if ((chip->card_ready & SD_CARD) || chip->sd_io) {
+		if (chip->int_reg & SD_EXIST)
+			sd_cleanup_work(chip);
+		release_sd_card(chip);
+	}
+
+	if (chip->card_ready & XD_CARD) {
+		if (chip->int_reg & XD_EXIST)
+			xd_cleanup_work(chip);
+		release_xd_card(chip);
+	}
+
+	if (chip->card_ready & MS_CARD) {
+		if (chip->int_reg & MS_EXIST)
+			ms_cleanup_work(chip);
+		release_ms_card(chip);
+	}
+}
+
+void rtsx_reset_cards(struct rtsx_chip *chip)
+{
+	if (!chip->need_reset)
+		return;
+
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+
+	rtsx_disable_aspm(chip);
+
+	if ((chip->need_reset & SD_CARD) && chip->chip_insert_with_sdio)
+		clear_bit(SD_NR, &chip->need_reset);
+
+	if (chip->need_reset & XD_CARD) {
+		chip->card_exist |= XD_CARD;
+
+		if (chip->xd_show_cnt >= MAX_SHOW_CNT)
+			do_reset_xd_card(chip);
+		else
+			chip->xd_show_cnt++;
+	}
+	if (CHECK_PID(chip, 0x5288) && CHECK_BARO_PKG(chip, QFN)) {
+		if (chip->card_exist & XD_CARD) {
+			clear_bit(SD_NR, &chip->need_reset);
+			clear_bit(MS_NR, &chip->need_reset);
+		}
+	}
+	if (chip->need_reset & SD_CARD) {
+		chip->card_exist |= SD_CARD;
+
+		if (chip->sd_show_cnt >= MAX_SHOW_CNT) {
+			rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+			do_reset_sd_card(chip);
+		} else {
+			chip->sd_show_cnt++;
+		}
+	}
+	if (chip->need_reset & MS_CARD) {
+		chip->card_exist |= MS_CARD;
+
+		if (chip->ms_show_cnt >= MAX_SHOW_CNT)
+			do_reset_ms_card(chip);
+		else
+			chip->ms_show_cnt++;
+	}
+}
+
+void rtsx_reinit_cards(struct rtsx_chip *chip, int reset_chip)
+{
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+
+	if (reset_chip)
+		rtsx_reset_chip(chip);
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if ((chip->int_reg & SD_EXIST) && (chip->need_reinit & SD_CARD)) {
+		release_sdio(chip);
+		release_sd_card(chip);
+
+		wait_timeout(100);
+
+		chip->card_exist |= SD_CARD;
+		do_reset_sd_card(chip);
+	}
+
+	if ((chip->int_reg & XD_EXIST) && (chip->need_reinit & XD_CARD)) {
+		release_xd_card(chip);
+
+		wait_timeout(100);
+
+		chip->card_exist |= XD_CARD;
+		do_reset_xd_card(chip);
+	}
+
+	if ((chip->int_reg & MS_EXIST) && (chip->need_reinit & MS_CARD)) {
+		release_ms_card(chip);
+
+		wait_timeout(100);
+
+		chip->card_exist |= MS_CARD;
+		do_reset_ms_card(chip);
+	}
+
+	chip->need_reinit = 0;
+}
+
+#ifdef DISABLE_CARD_INT
+void card_cd_debounce(struct rtsx_chip *chip, unsigned long *need_reset,
+		      unsigned long *need_release)
+{
+	u8 release_map = 0, reset_map = 0;
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if (chip->card_exist) {
+		if (chip->card_exist & XD_CARD) {
+			if (!(chip->int_reg & XD_EXIST))
+				release_map |= XD_CARD;
+		} else if (chip->card_exist & SD_CARD) {
+			if (!(chip->int_reg & SD_EXIST))
+				release_map |= SD_CARD;
+		} else if (chip->card_exist & MS_CARD) {
+			if (!(chip->int_reg & MS_EXIST))
+				release_map |= MS_CARD;
+		}
+	} else {
+		if (chip->int_reg & XD_EXIST)
+			reset_map |= XD_CARD;
+		else if (chip->int_reg & SD_EXIST)
+			reset_map |= SD_CARD;
+		else if (chip->int_reg & MS_EXIST)
+			reset_map |= MS_CARD;
+	}
+
+	if (reset_map) {
+		int xd_cnt = 0, sd_cnt = 0, ms_cnt = 0;
+		int i;
+
+		for (i = 0; i < (DEBOUNCE_CNT); i++) {
+			chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+			if (chip->int_reg & XD_EXIST)
+				xd_cnt++;
+			else
+				xd_cnt = 0;
+
+			if (chip->int_reg & SD_EXIST)
+				sd_cnt++;
+			else
+				sd_cnt = 0;
+
+			if (chip->int_reg & MS_EXIST)
+				ms_cnt++;
+			else
+				ms_cnt = 0;
+
+			wait_timeout(30);
+		}
+
+		reset_map = 0;
+		if (!(chip->card_exist & XD_CARD) &&
+		    (xd_cnt > (DEBOUNCE_CNT - 1)))
+			reset_map |= XD_CARD;
+		if (!(chip->card_exist & SD_CARD) &&
+		    (sd_cnt > (DEBOUNCE_CNT - 1)))
+			reset_map |= SD_CARD;
+		if (!(chip->card_exist & MS_CARD) &&
+		    (ms_cnt > (DEBOUNCE_CNT - 1)))
+			reset_map |= MS_CARD;
+	}
+
+	if (CHECK_PID(chip, 0x5288) && CHECK_BARO_PKG(chip, QFN))
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0xC0, 0x00);
+
+	if (need_reset)
+		*need_reset = reset_map;
+	if (need_release)
+		*need_release = release_map;
+}
+#endif
+
+void rtsx_init_cards(struct rtsx_chip *chip)
+{
+	if (RTSX_TST_DELINK(chip) && (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
+		dev_dbg(rtsx_dev(chip), "Reset chip in polling thread!\n");
+		rtsx_reset_chip(chip);
+		RTSX_CLR_DELINK(chip);
+	}
+
+#ifdef DISABLE_CARD_INT
+	card_cd_debounce(chip, &chip->need_reset, &chip->need_release);
+#endif
+
+	if (chip->need_release) {
+		if (CHECK_PID(chip, 0x5288) && CHECK_BARO_PKG(chip, QFN)) {
+			if (chip->int_reg & XD_EXIST) {
+				clear_bit(SD_NR, &chip->need_release);
+				clear_bit(MS_NR, &chip->need_release);
+			}
+		}
+
+		if (!(chip->card_exist & SD_CARD) && !chip->sd_io)
+			clear_bit(SD_NR, &chip->need_release);
+		if (!(chip->card_exist & XD_CARD))
+			clear_bit(XD_NR, &chip->need_release);
+		if (!(chip->card_exist & MS_CARD))
+			clear_bit(MS_NR, &chip->need_release);
+
+		dev_dbg(rtsx_dev(chip), "chip->need_release = 0x%x\n",
+			(unsigned int)(chip->need_release));
+
+#ifdef SUPPORT_OCP
+		if (chip->need_release) {
+			if (chip->ocp_stat & (CARD_OC_NOW | CARD_OC_EVER))
+				rtsx_write_register(chip, OCPCLR,
+						    CARD_OC_INT_CLR |
+						    CARD_OC_CLR,
+						    CARD_OC_INT_CLR |
+						    CARD_OC_CLR);
+			chip->ocp_stat = 0;
+		}
+#endif
+		if (chip->need_release) {
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+			rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+		}
+
+		if (chip->need_release & SD_CARD) {
+			clear_bit(SD_NR, &chip->need_release);
+			chip->card_exist &= ~SD_CARD;
+			chip->card_ejected &= ~SD_CARD;
+			chip->card_fail &= ~SD_CARD;
+			CLR_BIT(chip->lun_mc, chip->card2lun[SD_CARD]);
+			chip->rw_fail_cnt[chip->card2lun[SD_CARD]] = 0;
+			rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+
+			release_sdio(chip);
+			release_sd_card(chip);
+		}
+
+		if (chip->need_release & XD_CARD) {
+			clear_bit(XD_NR, &chip->need_release);
+			chip->card_exist &= ~XD_CARD;
+			chip->card_ejected &= ~XD_CARD;
+			chip->card_fail &= ~XD_CARD;
+			CLR_BIT(chip->lun_mc, chip->card2lun[XD_CARD]);
+			chip->rw_fail_cnt[chip->card2lun[XD_CARD]] = 0;
+
+			release_xd_card(chip);
+
+			if (CHECK_PID(chip, 0x5288) &&
+			    CHECK_BARO_PKG(chip, QFN))
+				rtsx_write_register(chip, HOST_SLEEP_STATE,
+						    0xC0, 0xC0);
+		}
+
+		if (chip->need_release & MS_CARD) {
+			clear_bit(MS_NR, &chip->need_release);
+			chip->card_exist &= ~MS_CARD;
+			chip->card_ejected &= ~MS_CARD;
+			chip->card_fail &= ~MS_CARD;
+			CLR_BIT(chip->lun_mc, chip->card2lun[MS_CARD]);
+			chip->rw_fail_cnt[chip->card2lun[MS_CARD]] = 0;
+
+			release_ms_card(chip);
+		}
+
+		dev_dbg(rtsx_dev(chip), "chip->card_exist = 0x%x\n",
+			chip->card_exist);
+
+		if (!chip->card_exist)
+			turn_off_led(chip, LED_GPIO);
+	}
+
+	if (chip->need_reset) {
+		dev_dbg(rtsx_dev(chip), "chip->need_reset = 0x%x\n",
+			(unsigned int)(chip->need_reset));
+
+		rtsx_reset_cards(chip);
+	}
+
+	if (chip->need_reinit) {
+		dev_dbg(rtsx_dev(chip), "chip->need_reinit = 0x%x\n",
+			(unsigned int)(chip->need_reinit));
+
+		rtsx_reinit_cards(chip, 0);
+	}
+}
+
+int switch_ssc_clock(struct rtsx_chip *chip, int clk)
+{
+	int retval;
+	u8 n = (u8)(clk - 2), min_n, max_n;
+	u8 mcu_cnt, div, max_div, ssc_depth, ssc_depth_mask;
+	int sd_vpclk_phase_reset = 0;
+
+	if (chip->cur_clk == clk)
+		return STATUS_SUCCESS;
+
+	min_n = 60;
+	max_n = 120;
+	max_div = CLK_DIV_4;
+
+	dev_dbg(rtsx_dev(chip), "Switch SSC clock to %dMHz (cur_clk = %d)\n",
+		clk, chip->cur_clk);
+
+	if (clk <= 2 || n > max_n)
+		return STATUS_FAIL;
+
+	mcu_cnt = (u8)(125 / clk + 3);
+	if (mcu_cnt > 7)
+		mcu_cnt = 7;
+
+	div = CLK_DIV_1;
+	while ((n < min_n) && (div < max_div)) {
+		n = (n + 2) * 2 - 2;
+		div++;
+	}
+	dev_dbg(rtsx_dev(chip), "n = %d, div = %d\n", n, div);
+
+	if (chip->ssc_en) {
+		ssc_depth = 0x01;
+		n -= 2;
+	} else {
+		ssc_depth = 0;
+	}
+
+	ssc_depth_mask = 0x03;
+
+	dev_dbg(rtsx_dev(chip), "ssc_depth = %d\n", ssc_depth);
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CLK_CTL, CLK_LOW_FREQ, CLK_LOW_FREQ);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0xFF, (div << 4) | mcu_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL2, ssc_depth_mask, ssc_depth);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_DIV_N_0, 0xFF, n);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, SSC_RSTB);
+	if (sd_vpclk_phase_reset) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
+			     PHASE_NOT_RESET, 0);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
+			     PHASE_NOT_RESET, PHASE_NOT_RESET);
+	}
+
+	retval = rtsx_send_cmd(chip, 0, WAIT_TIME);
+	if (retval < 0)
+		return STATUS_ERROR;
+
+	udelay(10);
+	retval = rtsx_write_register(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+	if (retval)
+		return retval;
+
+	chip->cur_clk = clk;
+
+	return STATUS_SUCCESS;
+}
+
+int switch_normal_clock(struct rtsx_chip *chip, int clk)
+{
+	int retval;
+	u8 sel, div, mcu_cnt;
+	int sd_vpclk_phase_reset = 0;
+
+	if (chip->cur_clk == clk)
+		return STATUS_SUCCESS;
+
+	switch (clk) {
+	case CLK_20:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 20MHz\n");
+		sel = SSC_80;
+		div = CLK_DIV_4;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_30:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 30MHz\n");
+		sel = SSC_120;
+		div = CLK_DIV_4;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_40:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 40MHz\n");
+		sel = SSC_80;
+		div = CLK_DIV_2;
+		mcu_cnt = 7;
+		break;
+
+	case CLK_50:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 50MHz\n");
+		sel = SSC_100;
+		div = CLK_DIV_2;
+		mcu_cnt = 6;
+		break;
+
+	case CLK_60:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 60MHz\n");
+		sel = SSC_120;
+		div = CLK_DIV_2;
+		mcu_cnt = 6;
+		break;
+
+	case CLK_80:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 80MHz\n");
+		sel = SSC_80;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_100:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 100MHz\n");
+		sel = SSC_100;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_120:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 120MHz\n");
+		sel = SSC_120;
+		div = CLK_DIV_1;
+		mcu_cnt = 5;
+		break;
+
+	case CLK_150:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 150MHz\n");
+		sel = SSC_150;
+		div = CLK_DIV_1;
+		mcu_cnt = 4;
+		break;
+
+	case CLK_200:
+		dev_dbg(rtsx_dev(chip), "Switch clock to 200MHz\n");
+		sel = SSC_200;
+		div = CLK_DIV_1;
+		mcu_cnt = 4;
+		break;
+
+	default:
+		dev_dbg(rtsx_dev(chip), "Try to switch to an illegal clock (%d)\n",
+			clk);
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, CLK_CTL, 0xFF, CLK_LOW_FREQ);
+	if (retval)
+		return retval;
+	if (sd_vpclk_phase_reset) {
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SD_VPCLK1_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval)
+			return retval;
+	}
+	retval = rtsx_write_register(chip, CLK_DIV, 0xFF,
+				     (div << 4) | mcu_cnt);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, CLK_SEL, 0xFF, sel);
+	if (retval)
+		return retval;
+
+	if (sd_vpclk_phase_reset) {
+		udelay(200);
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SD_VPCLK1_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval)
+			return retval;
+		udelay(200);
+	}
+	retval = rtsx_write_register(chip, CLK_CTL, 0xFF, 0);
+	if (retval)
+		return retval;
+
+	chip->cur_clk = clk;
+
+	return STATUS_SUCCESS;
+}
+
+void trans_dma_enable(enum dma_data_direction dir, struct rtsx_chip *chip,
+		      u32 byte_cnt, u8 pack_size)
+{
+	if (pack_size > DMA_1024)
+		pack_size = DMA_512;
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, IRQSTAT0, DMA_DONE_INT, DMA_DONE_INT);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC3, 0xFF, (u8)(byte_cnt >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC2, 0xFF, (u8)(byte_cnt >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC1, 0xFF, (u8)(byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, DMATC0, 0xFF, (u8)byte_cnt);
+
+	if (dir == DMA_FROM_DEVICE) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, DMACTL,
+			     0x03 | DMA_PACK_SIZE_MASK,
+			     DMA_DIR_FROM_CARD | DMA_EN | pack_size);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, DMACTL,
+			     0x03 | DMA_PACK_SIZE_MASK,
+			     DMA_DIR_TO_CARD | DMA_EN | pack_size);
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+}
+
+int enable_card_clock(struct rtsx_chip *chip, u8 card)
+{
+	int retval;
+	u8 clk_en = 0;
+
+	if (card & XD_CARD)
+		clk_en |= XD_CLK_EN;
+	if (card & SD_CARD)
+		clk_en |= SD_CLK_EN;
+	if (card & MS_CARD)
+		clk_en |= MS_CLK_EN;
+
+	retval = rtsx_write_register(chip, CARD_CLK_EN, clk_en, clk_en);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int disable_card_clock(struct rtsx_chip *chip, u8 card)
+{
+	int retval;
+	u8 clk_en = 0;
+
+	if (card & XD_CARD)
+		clk_en |= XD_CLK_EN;
+	if (card & SD_CARD)
+		clk_en |= SD_CLK_EN;
+	if (card & MS_CARD)
+		clk_en |= MS_CLK_EN;
+
+	retval = rtsx_write_register(chip, CARD_CLK_EN, clk_en, 0);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int card_power_on(struct rtsx_chip *chip, u8 card)
+{
+	int retval;
+	u8 mask, val1, val2;
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN) && card == MS_CARD) {
+		mask = MS_POWER_MASK;
+		val1 = MS_PARTIAL_POWER_ON;
+		val2 = MS_POWER_ON;
+	} else {
+		mask = SD_POWER_MASK;
+		val1 = SD_PARTIAL_POWER_ON;
+		val2 = SD_POWER_ON;
+	}
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val1);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	udelay(chip->pmos_pwr_on_interval);
+
+	rtsx_init_cmd(chip);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val2);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int card_power_off(struct rtsx_chip *chip, u8 card)
+{
+	int retval;
+	u8 mask, val;
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN) && card == MS_CARD) {
+		mask = MS_POWER_MASK;
+		val = MS_POWER_OFF;
+	} else {
+		mask = SD_POWER_MASK;
+		val = SD_POWER_OFF;
+	}
+
+	retval = rtsx_write_register(chip, CARD_PWR_CTL, mask, val);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	    u32 sec_addr, u16 sec_cnt)
+{
+	int retval;
+	unsigned int lun = SCSI_LUN(srb);
+	int i;
+
+	if (!chip->rw_card[lun])
+		return STATUS_FAIL;
+
+	for (i = 0; i < 3; i++) {
+		chip->rw_need_retry = 0;
+
+		retval = chip->rw_card[lun](srb, chip, sec_addr, sec_cnt);
+		if (retval != STATUS_SUCCESS) {
+			if (rtsx_check_chip_exist(chip) != STATUS_SUCCESS) {
+				rtsx_release_chip(chip);
+				return STATUS_FAIL;
+			}
+			if (detect_card_cd(chip, chip->cur_card) !=
+							STATUS_SUCCESS) {
+				return STATUS_FAIL;
+			}
+
+			if (!chip->rw_need_retry) {
+				dev_dbg(rtsx_dev(chip), "RW fail, but no need to retry\n");
+				break;
+			}
+		} else {
+			chip->rw_need_retry = 0;
+			break;
+		}
+
+		dev_dbg(rtsx_dev(chip), "Retry RW, (i = %d)\n", i);
+	}
+
+	return retval;
+}
+
+int card_share_mode(struct rtsx_chip *chip, int card)
+{
+	int retval;
+	u8 mask, value;
+
+	if (CHECK_PID(chip, 0x5208)) {
+		mask = CARD_SHARE_MASK;
+		if (card == SD_CARD)
+			value = CARD_SHARE_48_SD;
+		else if (card == MS_CARD)
+			value = CARD_SHARE_48_MS;
+		else if (card == XD_CARD)
+			value = CARD_SHARE_48_XD;
+		else
+			return STATUS_FAIL;
+
+	} else if (CHECK_PID(chip, 0x5288)) {
+		mask = 0x03;
+		if (card == SD_CARD)
+			value = CARD_SHARE_BAROSSA_SD;
+		else if (card == MS_CARD)
+			value = CARD_SHARE_BAROSSA_MS;
+		else if (card == XD_CARD)
+			value = CARD_SHARE_BAROSSA_XD;
+		else
+			return STATUS_FAIL;
+
+	} else {
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, CARD_SHARE_MODE, mask, value);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int select_card(struct rtsx_chip *chip, int card)
+{
+	int retval;
+
+	if (chip->cur_card != card) {
+		u8 mod;
+
+		if (card == SD_CARD)
+			mod = SD_MOD_SEL;
+		else if (card == MS_CARD)
+			mod = MS_MOD_SEL;
+		else if (card == XD_CARD)
+			mod = XD_MOD_SEL;
+		else if (card == SPI_CARD)
+			mod = SPI_MOD_SEL;
+		else
+			return STATUS_FAIL;
+
+		retval = rtsx_write_register(chip, CARD_SELECT, 0x07, mod);
+		if (retval)
+			return retval;
+		chip->cur_card = card;
+
+		retval =  card_share_mode(chip, card);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+void toggle_gpio(struct rtsx_chip *chip, u8 gpio)
+{
+	u8 temp_reg;
+
+	rtsx_read_register(chip, CARD_GPIO, &temp_reg);
+	temp_reg ^= (0x01 << gpio);
+	rtsx_write_register(chip, CARD_GPIO, 0xFF, temp_reg);
+}
+
+void turn_on_led(struct rtsx_chip *chip, u8 gpio)
+{
+	if (CHECK_PID(chip, 0x5288))
+		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio),
+				    (u8)(1 << gpio));
+	else
+		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), 0);
+}
+
+void turn_off_led(struct rtsx_chip *chip, u8 gpio)
+{
+	if (CHECK_PID(chip, 0x5288))
+		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), 0);
+	else
+		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio),
+				    (u8)(1 << gpio));
+}
+
+int detect_card_cd(struct rtsx_chip *chip, int card)
+{
+	u32 card_cd, status;
+
+	if (card == SD_CARD) {
+		card_cd = SD_EXIST;
+	} else if (card == MS_CARD) {
+		card_cd = MS_EXIST;
+	} else if (card == XD_CARD) {
+		card_cd = XD_EXIST;
+	} else {
+		dev_dbg(rtsx_dev(chip), "Wrong card type: 0x%x\n", card);
+		return STATUS_FAIL;
+	}
+
+	status = rtsx_readl(chip, RTSX_BIPR);
+	if (!(status & card_cd))
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int check_card_exist(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_exist & chip->lun2card[lun])
+		return 1;
+
+	return 0;
+}
+
+int check_card_ready(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_ready & chip->lun2card[lun])
+		return 1;
+
+	return 0;
+}
+
+int check_card_wp(struct rtsx_chip *chip, unsigned int lun)
+{
+	if (chip->card_wp & chip->lun2card[lun])
+		return 1;
+
+	return 0;
+}
+
+u8 get_lun_card(struct rtsx_chip *chip, unsigned int lun)
+{
+	if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD)
+		return (u8)XD_CARD;
+	else if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD)
+		return (u8)SD_CARD;
+	else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD)
+		return (u8)MS_CARD;
+
+	return 0;
+}
+
+void eject_card(struct rtsx_chip *chip, unsigned int lun)
+{
+	do_remaining_work(chip);
+
+	if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
+		release_sd_card(chip);
+		chip->card_ejected |= SD_CARD;
+		chip->card_ready &= ~SD_CARD;
+		chip->capacity[lun] = 0;
+	} else if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD) {
+		release_xd_card(chip);
+		chip->card_ejected |= XD_CARD;
+		chip->card_ready &= ~XD_CARD;
+		chip->capacity[lun] = 0;
+	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
+		release_ms_card(chip);
+		chip->card_ejected |= MS_CARD;
+		chip->card_ready &= ~MS_CARD;
+		chip->capacity[lun] = 0;
+	}
+}
diff --git a/drivers/staging/rts5208/rtsx_card.h b/drivers/staging/rts5208/rtsx_card.h
new file mode 100644
index 000000000..39727371c
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_card.h
@@ -0,0 +1,1087 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_CARD_H
+#define __REALTEK_RTSX_CARD_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+#include "rtsx_transport.h"
+#include "sd.h"
+
+#define SSC_POWER_DOWN		0x01
+#define SD_OC_POWER_DOWN	0x02
+#define MS_OC_POWER_DOWN	0x04
+#define ALL_POWER_DOWN		0x07
+#define OC_POWER_DOWN		0x06
+
+#define PMOS_STRG_MASK		0x10
+#define PMOS_STRG_800mA		0x10
+#define PMOS_STRG_400mA		0x00
+
+#define POWER_OFF		0x03
+#define PARTIAL_POWER_ON	0x01
+#define POWER_ON		0x00
+
+#define MS_POWER_OFF		0x0C
+#define MS_PARTIAL_POWER_ON	0x04
+#define MS_POWER_ON		0x00
+#define MS_POWER_MASK		0x0C
+
+#define SD_POWER_OFF		0x03
+#define SD_PARTIAL_POWER_ON	0x01
+#define SD_POWER_ON		0x00
+#define SD_POWER_MASK		0x03
+
+#define XD_OUTPUT_EN		0x02
+#define SD_OUTPUT_EN		0x04
+#define MS_OUTPUT_EN		0x08
+#define SPI_OUTPUT_EN		0x10
+
+#define CLK_LOW_FREQ		0x01
+
+#define CLK_DIV_1		0x01
+#define CLK_DIV_2		0x02
+#define CLK_DIV_4		0x03
+#define CLK_DIV_8		0x04
+
+#define SSC_80			0
+#define SSC_100			1
+#define SSC_120			2
+#define SSC_150			3
+#define SSC_200			4
+
+#define XD_CLK_EN		0x02
+#define SD_CLK_EN		0x04
+#define MS_CLK_EN		0x08
+#define SPI_CLK_EN		0x10
+
+#define XD_MOD_SEL		1
+#define SD_MOD_SEL		2
+#define MS_MOD_SEL		3
+#define SPI_MOD_SEL		4
+
+#define CHANGE_CLK		0x01
+
+#define	SD_CRC7_ERR			0x80
+#define	SD_CRC16_ERR			0x40
+#define	SD_CRC_WRITE_ERR		0x20
+#define	SD_CRC_WRITE_ERR_MASK		0x1C
+#define	GET_CRC_TIME_OUT		0x02
+#define	SD_TUNING_COMPARE_ERR		0x01
+
+#define	SD_RSP_80CLK_TIMEOUT		0x01
+
+#define	SD_CLK_TOGGLE_EN		0x80
+#define	SD_CLK_FORCE_STOP		0x40
+#define	SD_DAT3_STATUS			0x10
+#define	SD_DAT2_STATUS			0x08
+#define	SD_DAT1_STATUS			0x04
+#define	SD_DAT0_STATUS			0x02
+#define	SD_CMD_STATUS			0x01
+
+#define	SD_IO_USING_1V8			0x80
+#define	SD_IO_USING_3V3			0x7F
+#define	TYPE_A_DRIVING			0x00
+#define	TYPE_B_DRIVING			0x01
+#define	TYPE_C_DRIVING			0x02
+#define	TYPE_D_DRIVING			0x03
+
+#define	DDR_FIX_RX_DAT			0x00
+#define	DDR_VAR_RX_DAT			0x80
+#define	DDR_FIX_RX_DAT_EDGE		0x00
+#define	DDR_FIX_RX_DAT_14_DELAY		0x40
+#define	DDR_FIX_RX_CMD			0x00
+#define	DDR_VAR_RX_CMD			0x20
+#define	DDR_FIX_RX_CMD_POS_EDGE		0x00
+#define	DDR_FIX_RX_CMD_14_DELAY		0x10
+#define	SD20_RX_POS_EDGE		0x00
+#define	SD20_RX_14_DELAY		0x08
+#define SD20_RX_SEL_MASK		0x08
+
+#define	DDR_FIX_TX_CMD_DAT		0x00
+#define	DDR_VAR_TX_CMD_DAT		0x80
+#define	DDR_FIX_TX_DAT_14_TSU		0x00
+#define	DDR_FIX_TX_DAT_12_TSU		0x40
+#define	DDR_FIX_TX_CMD_NEG_EDGE		0x00
+#define	DDR_FIX_TX_CMD_14_AHEAD		0x20
+#define	SD20_TX_NEG_EDGE		0x00
+#define	SD20_TX_14_AHEAD		0x10
+#define SD20_TX_SEL_MASK		0x10
+#define	DDR_VAR_SDCLK_POL_SWAP		0x01
+
+#define	SD_TRANSFER_START		0x80
+#define	SD_TRANSFER_END			0x40
+#define SD_STAT_IDLE			0x20
+#define	SD_TRANSFER_ERR			0x10
+#define	SD_TM_NORMAL_WRITE		0x00
+#define	SD_TM_AUTO_WRITE_3		0x01
+#define	SD_TM_AUTO_WRITE_4		0x02
+#define	SD_TM_AUTO_READ_3		0x05
+#define	SD_TM_AUTO_READ_4		0x06
+#define	SD_TM_CMD_RSP			0x08
+#define	SD_TM_AUTO_WRITE_1		0x09
+#define	SD_TM_AUTO_WRITE_2		0x0A
+#define	SD_TM_NORMAL_READ		0x0C
+#define	SD_TM_AUTO_READ_1		0x0D
+#define	SD_TM_AUTO_READ_2		0x0E
+#define	SD_TM_AUTO_TUNING		0x0F
+
+#define PHASE_CHANGE			0x80
+#define PHASE_NOT_RESET			0x40
+
+#define DCMPS_CHANGE			0x80
+#define DCMPS_CHANGE_DONE		0x40
+#define DCMPS_ERROR			0x20
+#define DCMPS_CURRENT_PHASE		0x1F
+
+#define SD_CLK_DIVIDE_0			0x00
+#define	SD_CLK_DIVIDE_256		0xC0
+#define	SD_CLK_DIVIDE_128		0x80
+#define	SD_BUS_WIDTH_1			0x00
+#define	SD_BUS_WIDTH_4			0x01
+#define	SD_BUS_WIDTH_8			0x02
+#define	SD_ASYNC_FIFO_NOT_RST		0x10
+#define	SD_20_MODE			0x00
+#define	SD_DDR_MODE			0x04
+#define	SD_30_MODE			0x08
+
+#define SD_CLK_DIVIDE_MASK		0xC0
+
+#define SD_CMD_IDLE			0x80
+
+#define SD_DATA_IDLE			0x80
+
+#define DCM_RESET			0x08
+#define DCM_LOCKED			0x04
+#define DCM_208M			0x00
+#define DCM_TX				0x01
+#define DCM_RX				0x02
+
+#define DRP_START			0x80
+#define DRP_DONE			0x40
+
+#define DRP_WRITE			0x80
+#define DRP_READ			0x00
+#define DCM_WRITE_ADDRESS_50		0x50
+#define DCM_WRITE_ADDRESS_51		0x51
+#define DCM_READ_ADDRESS_00		0x00
+#define DCM_READ_ADDRESS_51		0x51
+
+#define	SD_CALCULATE_CRC7		0x00
+#define	SD_NO_CALCULATE_CRC7		0x80
+#define	SD_CHECK_CRC16			0x00
+#define	SD_NO_CHECK_CRC16		0x40
+#define SD_NO_CHECK_WAIT_CRC_TO		0x20
+#define	SD_WAIT_BUSY_END		0x08
+#define	SD_NO_WAIT_BUSY_END		0x00
+#define	SD_CHECK_CRC7			0x00
+#define	SD_NO_CHECK_CRC7		0x04
+#define	SD_RSP_LEN_0			0x00
+#define	SD_RSP_LEN_6			0x01
+#define	SD_RSP_LEN_17			0x02
+#define	SD_RSP_TYPE_R0			0x04
+#define	SD_RSP_TYPE_R1			0x01
+#define	SD_RSP_TYPE_R1b			0x09
+#define	SD_RSP_TYPE_R2			0x02
+#define	SD_RSP_TYPE_R3			0x05
+#define	SD_RSP_TYPE_R4			0x05
+#define	SD_RSP_TYPE_R5			0x01
+#define	SD_RSP_TYPE_R6			0x01
+#define	SD_RSP_TYPE_R7			0x01
+
+#define	SD_RSP_80CLK_TIMEOUT_EN		0x01
+
+#define	SAMPLE_TIME_RISING		0x00
+#define	SAMPLE_TIME_FALLING		0x80
+#define	PUSH_TIME_DEFAULT		0x00
+#define	PUSH_TIME_ODD			0x40
+#define	NO_EXTEND_TOGGLE		0x00
+#define	EXTEND_TOGGLE_CHK		0x20
+#define	MS_BUS_WIDTH_1			0x00
+#define	MS_BUS_WIDTH_4			0x10
+#define	MS_BUS_WIDTH_8			0x18
+#define	MS_2K_SECTOR_MODE		0x04
+#define	MS_512_SECTOR_MODE		0x00
+#define	MS_TOGGLE_TIMEOUT_EN		0x00
+#define	MS_TOGGLE_TIMEOUT_DISEN		0x01
+#define MS_NO_CHECK_INT			0x02
+
+#define	WAIT_INT			0x80
+#define	NO_WAIT_INT			0x00
+#define	NO_AUTO_READ_INT_REG		0x00
+#define	AUTO_READ_INT_REG		0x40
+#define	MS_CRC16_ERR			0x20
+#define	MS_RDY_TIMEOUT			0x10
+#define	MS_INT_CMDNK			0x08
+#define	MS_INT_BREQ			0x04
+#define	MS_INT_ERR			0x02
+#define	MS_INT_CED			0x01
+
+#define	MS_TRANSFER_START		0x80
+#define	MS_TRANSFER_END			0x40
+#define	MS_TRANSFER_ERR			0x20
+#define	MS_BS_STATE			0x10
+#define	MS_TM_READ_BYTES		0x00
+#define	MS_TM_NORMAL_READ		0x01
+#define	MS_TM_WRITE_BYTES		0x04
+#define	MS_TM_NORMAL_WRITE		0x05
+#define	MS_TM_AUTO_READ			0x08
+#define	MS_TM_AUTO_WRITE		0x0C
+
+#define CARD_SHARE_MASK			0x0F
+#define CARD_SHARE_MULTI_LUN		0x00
+#define	CARD_SHARE_NORMAL		0x00
+#define	CARD_SHARE_48_XD		0x02
+#define	CARD_SHARE_48_SD		0x04
+#define	CARD_SHARE_48_MS		0x08
+#define CARD_SHARE_BAROSSA_XD		0x00
+#define CARD_SHARE_BAROSSA_SD		0x01
+#define CARD_SHARE_BAROSSA_MS		0x02
+
+#define	MS_DRIVE_8			0x00
+#define	MS_DRIVE_4			0x40
+#define	MS_DRIVE_12			0x80
+#define	SD_DRIVE_8			0x00
+#define	SD_DRIVE_4			0x10
+#define	SD_DRIVE_12			0x20
+#define	XD_DRIVE_8			0x00
+#define	XD_DRIVE_4			0x04
+#define	XD_DRIVE_12			0x08
+
+#define SPI_STOP		0x01
+#define XD_STOP			0x02
+#define SD_STOP			0x04
+#define MS_STOP			0x08
+#define SPI_CLR_ERR		0x10
+#define XD_CLR_ERR		0x20
+#define SD_CLR_ERR		0x40
+#define MS_CLR_ERR		0x80
+
+#define CRC_FIX_CLK		(0x00 << 0)
+#define CRC_VAR_CLK0		(0x01 << 0)
+#define CRC_VAR_CLK1		(0x02 << 0)
+#define SD30_FIX_CLK		(0x00 << 2)
+#define SD30_VAR_CLK0		(0x01 << 2)
+#define SD30_VAR_CLK1		(0x02 << 2)
+#define SAMPLE_FIX_CLK		(0x00 << 4)
+#define SAMPLE_VAR_CLK0		(0x01 << 4)
+#define SAMPLE_VAR_CLK1		(0x02 << 4)
+
+#define SDIO_VER_20		0x80
+#define SDIO_VER_10		0x00
+#define SDIO_VER_CHG		0x40
+#define SDIO_BUS_AUTO_SWITCH	0x10
+
+#define PINGPONG_BUFFER		0x01
+#define RING_BUFFER		0x00
+
+#define RB_FLUSH		0x80
+
+#define DMA_DONE_INT_EN			0x80
+#define SUSPEND_INT_EN			0x40
+#define LINK_RDY_INT_EN			0x20
+#define LINK_DOWN_INT_EN		0x10
+
+#define DMA_DONE_INT			0x80
+#define SUSPEND_INT			0x40
+#define LINK_RDY_INT			0x20
+#define LINK_DOWN_INT			0x10
+
+#define MRD_ERR_INT_EN			0x40
+#define MWR_ERR_INT_EN			0x20
+#define SCSI_CMD_INT_EN			0x10
+#define TLP_RCV_INT_EN			0x08
+#define TLP_TRSMT_INT_EN		0x04
+#define MRD_COMPLETE_INT_EN		0x02
+#define MWR_COMPLETE_INT_EN		0x01
+
+#define MRD_ERR_INT			0x40
+#define MWR_ERR_INT			0x20
+#define SCSI_CMD_INT			0x10
+#define TLP_RX_INT			0x08
+#define TLP_TX_INT			0x04
+#define MRD_COMPLETE_INT		0x02
+#define MWR_COMPLETE_INT		0x01
+
+#define MSG_RX_INT_EN			0x08
+#define MRD_RX_INT_EN			0x04
+#define MWR_RX_INT_EN			0x02
+#define CPLD_RX_INT_EN			0x01
+
+#define MSG_RX_INT			0x08
+#define MRD_RX_INT			0x04
+#define MWR_RX_INT			0x02
+#define CPLD_RX_INT			0x01
+
+#define MSG_TX_INT_EN			0x08
+#define MRD_TX_INT_EN			0x04
+#define MWR_TX_INT_EN			0x02
+#define CPLD_TX_INT_EN			0x01
+
+#define MSG_TX_INT			0x08
+#define MRD_TX_INT			0x04
+#define MWR_TX_INT			0x02
+#define CPLD_TX_INT			0x01
+
+#define DMA_RST				0x80
+#define DMA_BUSY			0x04
+#define DMA_DIR_TO_CARD			0x00
+#define DMA_DIR_FROM_CARD		0x02
+#define DMA_EN				0x01
+#define DMA_128				(0 << 4)
+#define DMA_256				(1 << 4)
+#define DMA_512				(2 << 4)
+#define DMA_1024			(3 << 4)
+#define DMA_PACK_SIZE_MASK		0x30
+
+#define	XD_PWR_OFF_DELAY0		0x00
+#define	XD_PWR_OFF_DELAY1		0x02
+#define	XD_PWR_OFF_DELAY2		0x04
+#define	XD_PWR_OFF_DELAY3		0x06
+#define	XD_AUTO_PWR_OFF_EN		0xF7
+#define	XD_NO_AUTO_PWR_OFF		0x08
+
+#define	XD_TIME_RWN_1			0x00
+#define	XD_TIME_RWN_STEP		0x20
+#define	XD_TIME_RW_1			0x00
+#define	XD_TIME_RW_STEP			0x04
+#define	XD_TIME_SETUP_1			0x00
+#define	XD_TIME_SETUP_STEP		0x01
+
+#define	XD_ECC2_UNCORRECTABLE		0x80
+#define	XD_ECC2_ERROR			0x40
+#define	XD_ECC1_UNCORRECTABLE		0x20
+#define	XD_ECC1_ERROR			0x10
+#define	XD_RDY				0x04
+#define	XD_CE_EN			0xFD
+#define	XD_CE_DISEN			0x02
+#define	XD_WP_EN			0xFE
+#define	XD_WP_DISEN			0x01
+
+#define	XD_TRANSFER_START		0x80
+#define	XD_TRANSFER_END			0x40
+#define	XD_PPB_EMPTY			0x20
+#define	XD_RESET			0x00
+#define	XD_ERASE			0x01
+#define	XD_READ_STATUS			0x02
+#define	XD_READ_ID			0x03
+#define	XD_READ_REDUNDANT		0x04
+#define	XD_READ_PAGES			0x05
+#define	XD_SET_CMD			0x06
+#define	XD_NORMAL_READ			0x07
+#define	XD_WRITE_PAGES			0x08
+#define	XD_NORMAL_WRITE			0x09
+#define	XD_WRITE_REDUNDANT		0x0A
+#define	XD_SET_ADDR			0x0B
+
+#define	XD_PPB_TO_SIE			0x80
+#define	XD_TO_PPB_ONLY			0x00
+#define	XD_BA_TRANSFORM			0x40
+#define	XD_BA_NO_TRANSFORM		0x00
+#define	XD_NO_CALC_ECC			0x20
+#define	XD_CALC_ECC			0x00
+#define	XD_IGNORE_ECC			0x10
+#define	XD_CHECK_ECC			0x00
+#define	XD_DIRECT_TO_RB			0x08
+#define	XD_ADDR_LENGTH_0		0x00
+#define	XD_ADDR_LENGTH_1		0x01
+#define	XD_ADDR_LENGTH_2		0x02
+#define	XD_ADDR_LENGTH_3		0x03
+#define	XD_ADDR_LENGTH_4		0x04
+
+#define	XD_GPG				0xFF
+#define	XD_BPG				0x00
+
+#define	XD_GBLK				0xFF
+#define	XD_LATER_BBLK			0xF0
+
+#define	XD_ECC2_ALL1			0x80
+#define	XD_ECC1_ALL1			0x40
+#define	XD_BA2_ALL0			0x20
+#define	XD_BA1_ALL0			0x10
+#define	XD_BA1_BA2_EQL			0x04
+#define	XD_BA2_VALID			0x02
+#define	XD_BA1_VALID			0x01
+
+#define	XD_PGSTS_ZEROBIT_OVER4		0x00
+#define	XD_PGSTS_NOT_FF			0x02
+#define	XD_AUTO_CHK_DATA_STATUS		0x01
+
+#define	RSTB_MODE_DETECT		0x80
+#define	MODE_OUT_VLD			0x40
+#define	MODE_OUT_0_NONE			0x00
+#define	MODE_OUT_10_NONE		0x04
+#define	MODE_OUT_10_47			0x05
+#define	MODE_OUT_10_180			0x06
+#define	MODE_OUT_10_680			0x07
+#define	MODE_OUT_16_NONE		0x08
+#define	MODE_OUT_16_47			0x09
+#define	MODE_OUT_16_180			0x0A
+#define	MODE_OUT_16_680			0x0B
+#define	MODE_OUT_NONE_NONE		0x0C
+#define	MODE_OUT_NONE_47		0x0D
+#define	MODE_OUT_NONE_180		0x0E
+#define	MODE_OUT_NONE_680		0x0F
+
+#define	CARD_OC_INT_EN			0x20
+#define	CARD_DETECT_EN			0x08
+
+#define MS_DETECT_EN			0x80
+#define MS_OCP_INT_EN			0x40
+#define MS_OCP_INT_CLR			0x20
+#define MS_OC_CLR			0x10
+#define SD_DETECT_EN			0x08
+#define SD_OCP_INT_EN			0x04
+#define SD_OCP_INT_CLR			0x02
+#define SD_OC_CLR			0x01
+
+#define	CARD_OCP_DETECT			0x80
+#define	CARD_OC_NOW			0x08
+#define	CARD_OC_EVER			0x04
+
+#define MS_OCP_DETECT			0x80
+#define MS_OC_NOW			0x40
+#define MS_OC_EVER			0x20
+#define SD_OCP_DETECT			0x08
+#define SD_OC_NOW			0x04
+#define SD_OC_EVER			0x02
+
+#define	CARD_OC_INT_CLR			0x08
+#define	CARD_OC_CLR			0x02
+
+#define SD_OCP_GLITCH_MASK		0x07
+#define SD_OCP_GLITCH_6_4		0x00
+#define SD_OCP_GLITCH_64		0x01
+#define SD_OCP_GLITCH_640		0x02
+#define SD_OCP_GLITCH_1000		0x03
+#define SD_OCP_GLITCH_2000		0x04
+#define SD_OCP_GLITCH_4000		0x05
+#define SD_OCP_GLITCH_8000		0x06
+#define SD_OCP_GLITCH_10000		0x07
+
+#define MS_OCP_GLITCH_MASK		0x70
+#define MS_OCP_GLITCH_6_4		(0x00 << 4)
+#define MS_OCP_GLITCH_64		(0x01 << 4)
+#define MS_OCP_GLITCH_640		(0x02 << 4)
+#define MS_OCP_GLITCH_1000		(0x03 << 4)
+#define MS_OCP_GLITCH_2000		(0x04 << 4)
+#define MS_OCP_GLITCH_4000		(0x05 << 4)
+#define MS_OCP_GLITCH_8000		(0x06 << 4)
+#define MS_OCP_GLITCH_10000		(0x07 << 4)
+
+#define OCP_TIME_60			0x00
+#define OCP_TIME_100			(0x01 << 3)
+#define OCP_TIME_200			(0x02 << 3)
+#define OCP_TIME_400			(0x03 << 3)
+#define OCP_TIME_600			(0x04 << 3)
+#define OCP_TIME_800			(0x05 << 3)
+#define OCP_TIME_1100			(0x06 << 3)
+#define OCP_TIME_MASK			0x38
+
+#define MS_OCP_TIME_60			0x00
+#define MS_OCP_TIME_100			(0x01 << 4)
+#define MS_OCP_TIME_200			(0x02 << 4)
+#define MS_OCP_TIME_400			(0x03 << 4)
+#define MS_OCP_TIME_600			(0x04 << 4)
+#define MS_OCP_TIME_800			(0x05 << 4)
+#define MS_OCP_TIME_1100		(0x06 << 4)
+#define MS_OCP_TIME_MASK		0x70
+
+#define SD_OCP_TIME_60			0x00
+#define SD_OCP_TIME_100			0x01
+#define SD_OCP_TIME_200			0x02
+#define SD_OCP_TIME_400			0x03
+#define SD_OCP_TIME_600			0x04
+#define SD_OCP_TIME_800			0x05
+#define SD_OCP_TIME_1100		0x06
+#define SD_OCP_TIME_MASK		0x07
+
+#define OCP_THD_315_417			0x00
+#define OCP_THD_283_783			(0x01 << 6)
+#define OCP_THD_244_946			(0x02 << 6)
+#define OCP_THD_191_1080		(0x03 << 6)
+#define OCP_THD_MASK			0xC0
+
+#define MS_OCP_THD_450			0x00
+#define MS_OCP_THD_550			(0x01 << 4)
+#define MS_OCP_THD_650			(0x02 << 4)
+#define MS_OCP_THD_750			(0x03 << 4)
+#define MS_OCP_THD_850			(0x04 << 4)
+#define MS_OCP_THD_950			(0x05 << 4)
+#define MS_OCP_THD_1050			(0x06 << 4)
+#define MS_OCP_THD_1150			(0x07 << 4)
+#define MS_OCP_THD_MASK			0x70
+
+#define SD_OCP_THD_450			0x00
+#define SD_OCP_THD_550			0x01
+#define SD_OCP_THD_650			0x02
+#define SD_OCP_THD_750			0x03
+#define SD_OCP_THD_850			0x04
+#define SD_OCP_THD_950			0x05
+#define SD_OCP_THD_1050			0x06
+#define SD_OCP_THD_1150			0x07
+#define SD_OCP_THD_MASK			0x07
+
+#define FPGA_MS_PULL_CTL_EN		0xEF
+#define FPGA_SD_PULL_CTL_EN		0xF7
+#define FPGA_XD_PULL_CTL_EN1		0xFE
+#define FPGA_XD_PULL_CTL_EN2		0xFD
+#define FPGA_XD_PULL_CTL_EN3		0xFB
+
+#define FPGA_MS_PULL_CTL_BIT		0x10
+#define FPGA_SD_PULL_CTL_BIT		0x08
+
+#define BLINK_EN			0x08
+#define LED_GPIO0			(0 << 4)
+#define LED_GPIO1			(1 << 4)
+#define LED_GPIO2			(2 << 4)
+
+#define SDIO_BUS_CTRL		0x01
+#define SDIO_CD_CTRL		0x02
+
+#define SSC_RSTB		0x80
+#define SSC_8X_EN		0x40
+#define SSC_FIX_FRAC		0x20
+#define SSC_SEL_1M		0x00
+#define SSC_SEL_2M		0x08
+#define SSC_SEL_4M		0x10
+#define SSC_SEL_8M		0x18
+
+#define SSC_DEPTH_MASK		0x07
+#define SSC_DEPTH_DISALBE	0x00
+#define SSC_DEPTH_4M		0x01
+#define SSC_DEPTH_2M		0x02
+#define SSC_DEPTH_1M		0x03
+#define SSC_DEPTH_512K		0x04
+#define SSC_DEPTH_256K		0x05
+#define SSC_DEPTH_128K		0x06
+#define SSC_DEPTH_64K		0x07
+
+#define XD_D3_NP		0x00
+#define XD_D3_PD		(0x01 << 6)
+#define XD_D3_PU		(0x02 << 6)
+#define XD_D2_NP		0x00
+#define XD_D2_PD		(0x01 << 4)
+#define XD_D2_PU		(0x02 << 4)
+#define XD_D1_NP		0x00
+#define XD_D1_PD		(0x01 << 2)
+#define XD_D1_PU		(0x02 << 2)
+#define XD_D0_NP		0x00
+#define XD_D0_PD		0x01
+#define XD_D0_PU		0x02
+
+#define SD_D7_NP		0x00
+#define SD_D7_PD		(0x01 << 4)
+#define SD_DAT7_PU		(0x02 << 4)
+#define SD_CLK_NP		0x00
+#define SD_CLK_PD		(0x01 << 2)
+#define SD_CLK_PU		(0x02 << 2)
+#define SD_D5_NP		0x00
+#define SD_D5_PD		0x01
+#define SD_D5_PU		0x02
+
+#define MS_D1_NP		0x00
+#define MS_D1_PD		(0x01 << 6)
+#define MS_D1_PU		(0x02 << 6)
+#define MS_D2_NP		0x00
+#define MS_D2_PD		(0x01 << 4)
+#define MS_D2_PU		(0x02 << 4)
+#define MS_CLK_NP		0x00
+#define MS_CLK_PD		(0x01 << 2)
+#define MS_CLK_PU		(0x02 << 2)
+#define MS_D6_NP		0x00
+#define MS_D6_PD		0x01
+#define MS_D6_PU		0x02
+
+#define XD_D7_NP		0x00
+#define XD_D7_PD		(0x01 << 6)
+#define XD_D7_PU		(0x02 << 6)
+#define XD_D6_NP		0x00
+#define XD_D6_PD		(0x01 << 4)
+#define XD_D6_PU		(0x02 << 4)
+#define XD_D5_NP		0x00
+#define XD_D5_PD		(0x01 << 2)
+#define XD_D5_PU		(0x02 << 2)
+#define XD_D4_NP		0x00
+#define XD_D4_PD		0x01
+#define XD_D4_PU		0x02
+
+#define SD_D6_NP		0x00
+#define SD_D6_PD		(0x01 << 6)
+#define SD_D6_PU		(0x02 << 6)
+#define SD_D0_NP		0x00
+#define SD_D0_PD		(0x01 << 4)
+#define SD_D0_PU		(0x02 << 4)
+#define SD_D1_NP		0x00
+#define SD_D1_PD		0x01
+#define SD_D1_PU		0x02
+
+#define MS_D3_NP		0x00
+#define MS_D3_PD		(0x01 << 6)
+#define MS_D3_PU		(0x02 << 6)
+#define MS_D0_NP		0x00
+#define MS_D0_PD		(0x01 << 4)
+#define MS_D0_PU		(0x02 << 4)
+#define MS_BS_NP		0x00
+#define MS_BS_PD		(0x01 << 2)
+#define MS_BS_PU		(0x02 << 2)
+
+#define XD_WP_NP		0x00
+#define XD_WP_PD		(0x01 << 6)
+#define XD_WP_PU		(0x02 << 6)
+#define XD_CE_NP		0x00
+#define XD_CE_PD		(0x01 << 3)
+#define XD_CE_PU		(0x02 << 3)
+#define XD_CLE_NP		0x00
+#define XD_CLE_PD		(0x01 << 1)
+#define XD_CLE_PU		(0x02 << 1)
+#define XD_CD_PD		0x00
+#define XD_CD_PU		0x01
+
+#define SD_D4_NP		0x00
+#define SD_D4_PD		(0x01 << 6)
+#define SD_D4_PU		(0x02 << 6)
+
+#define MS_D7_NP		0x00
+#define MS_D7_PD		(0x01 << 6)
+#define MS_D7_PU		(0x02 << 6)
+
+#define XD_RDY_NP		0x00
+#define XD_RDY_PD		(0x01 << 6)
+#define XD_RDY_PU		(0x02 << 6)
+#define XD_WE_NP		0x00
+#define XD_WE_PD		(0x01 << 4)
+#define XD_WE_PU		(0x02 << 4)
+#define XD_RE_NP		0x00
+#define XD_RE_PD		(0x01 << 2)
+#define XD_RE_PU		(0x02 << 2)
+#define XD_ALE_NP		0x00
+#define XD_ALE_PD		0x01
+#define XD_ALE_PU		0x02
+
+#define SD_D3_NP		0x00
+#define SD_D3_PD		(0x01 << 4)
+#define SD_D3_PU		(0x02 << 4)
+#define SD_D2_NP		0x00
+#define SD_D2_PD		(0x01 << 2)
+#define SD_D2_PU		(0x02 << 2)
+
+#define MS_INS_PD		0x00
+#define MS_INS_PU		(0x01 << 7)
+#define SD_WP_NP		0x00
+#define SD_WP_PD		(0x01 << 5)
+#define SD_WP_PU		(0x02 << 5)
+#define SD_CD_PD		0x00
+#define SD_CD_PU		(0x01 << 4)
+#define SD_CMD_NP		0x00
+#define SD_CMD_PD		(0x01 << 2)
+#define SD_CMD_PU		(0x02 << 2)
+
+#define MS_D5_NP		0x00
+#define MS_D5_PD		(0x01 << 2)
+#define MS_D5_PU		(0x02 << 2)
+#define MS_D4_NP		0x00
+#define MS_D4_PD		0x01
+#define MS_D4_PU		0x02
+
+#define FORCE_PM_CLOCK		0x10
+#define EN_CLOCK_PM		0x01
+
+#define HOST_ENTER_S3		0x02
+#define HOST_ENTER_S1		0x01
+
+#define AUX_PWR_DETECTED	0x01
+
+#define PHY_DEBUG_MODE		0x01
+
+#define SPI_COMMAND_BIT_8	0xE0
+#define SPI_ADDRESS_BIT_24	0x17
+#define SPI_ADDRESS_BIT_32	0x1F
+
+#define SPI_TRANSFER0_START	0x80
+#define SPI_TRANSFER0_END	0x40
+#define SPI_C_MODE0		0x00
+#define SPI_CA_MODE0		0x01
+#define SPI_CDO_MODE0		0x02
+#define SPI_CDI_MODE0		0x03
+#define SPI_CADO_MODE0		0x04
+#define SPI_CADI_MODE0		0x05
+#define SPI_POLLING_MODE0	0x06
+
+#define SPI_TRANSFER1_START	0x80
+#define SPI_TRANSFER1_END	0x40
+#define SPI_DO_MODE1		0x00
+#define SPI_DI_MODE1		0x01
+
+#define CS_POLARITY_HIGH	0x40
+#define CS_POLARITY_LOW		0x00
+#define DTO_MSB_FIRST		0x00
+#define DTO_LSB_FIRST		0x20
+#define SPI_MASTER		0x00
+#define SPI_SLAVE		0x10
+#define SPI_MODE0		0x00
+#define SPI_MODE1		0x04
+#define SPI_MODE2		0x08
+#define SPI_MODE3		0x0C
+#define SPI_MANUAL		0x00
+#define SPI_HALF_AUTO		0x01
+#define SPI_AUTO		0x02
+#define SPI_EEPROM_AUTO		0x03
+
+#define EDO_TIMING_MASK		0x03
+#define SAMPLE_RISING		0x00
+#define SAMPLE_DELAY_HALF	0x01
+#define SAMPLE_DELAY_ONE	0x02
+#define SAPMLE_DELAY_ONE_HALF	0x03
+#define TCS_MASK		0x0C
+
+#define NOT_BYPASS_SD		0x02
+#define DISABLE_SDIO_FUNC	0x04
+#define SELECT_1LUN		0x08
+
+#define PWR_GATE_EN		0x01
+#define LDO3318_PWR_MASK	0x06
+#define LDO_ON			0x00
+#define LDO_SUSPEND		0x04
+#define LDO_OFF			0x06
+
+#define SD_CFG1			0xFDA0
+#define SD_CFG2			0xFDA1
+#define SD_CFG3			0xFDA2
+#define SD_STAT1		0xFDA3
+#define SD_STAT2		0xFDA4
+#define SD_BUS_STAT		0xFDA5
+#define SD_PAD_CTL		0xFDA6
+#define SD_SAMPLE_POINT_CTL	0xFDA7
+#define SD_PUSH_POINT_CTL	0xFDA8
+#define SD_CMD0			0xFDA9
+#define SD_CMD1			0xFDAA
+#define SD_CMD2			0xFDAB
+#define SD_CMD3			0xFDAC
+#define SD_CMD4			0xFDAD
+#define SD_CMD5			0xFDAE
+#define SD_BYTE_CNT_L		0xFDAF
+#define SD_BYTE_CNT_H		0xFDB0
+#define SD_BLOCK_CNT_L		0xFDB1
+#define SD_BLOCK_CNT_H		0xFDB2
+#define SD_TRANSFER		0xFDB3
+#define SD_CMD_STATE		0xFDB5
+#define SD_DATA_STATE		0xFDB6
+
+#define	DCM_DRP_CTL		0xFC23
+#define	DCM_DRP_TRIG		0xFC24
+#define	DCM_DRP_CFG		0xFC25
+#define	DCM_DRP_WR_DATA_L	0xFC26
+#define	DCM_DRP_WR_DATA_H	0xFC27
+#define	DCM_DRP_RD_DATA_L	0xFC28
+#define	DCM_DRP_RD_DATA_H	0xFC29
+#define SD_VPCLK0_CTL		0xFC2A
+#define SD_VPCLK1_CTL		0xFC2B
+#define SD_DCMPS0_CTL		0xFC2C
+#define SD_DCMPS1_CTL		0xFC2D
+#define SD_VPTX_CTL		SD_VPCLK0_CTL
+#define SD_VPRX_CTL		SD_VPCLK1_CTL
+#define SD_DCMPS_TX_CTL		SD_DCMPS0_CTL
+#define SD_DCMPS_RX_CTL		SD_DCMPS1_CTL
+
+#define CARD_CLK_SOURCE		0xFC2E
+
+#define CARD_PWR_CTL		0xFD50
+#define CARD_CLK_SWITCH		0xFD51
+#define CARD_SHARE_MODE		0xFD52
+#define CARD_DRIVE_SEL		0xFD53
+#define CARD_STOP		0xFD54
+#define CARD_OE			0xFD55
+#define CARD_AUTO_BLINK		0xFD56
+#define CARD_GPIO_DIR		0xFD57
+#define CARD_GPIO		0xFD58
+
+#define CARD_DATA_SOURCE	0xFD5B
+#define CARD_SELECT		0xFD5C
+#define SD30_DRIVE_SEL		0xFD5E
+
+#define CARD_CLK_EN		0xFD69
+
+#define SDIO_CTRL		0xFD6B
+
+#define FPDCTL			0xFC00
+#define PDINFO			0xFC01
+
+#define CLK_CTL			0xFC02
+#define CLK_DIV			0xFC03
+#define CLK_SEL			0xFC04
+
+#define SSC_DIV_N_0		0xFC0F
+#define SSC_DIV_N_1		0xFC10
+
+#define RCCTL			0xFC14
+
+#define FPGA_PULL_CTL		0xFC1D
+
+#define CARD_PULL_CTL1		0xFD60
+#define CARD_PULL_CTL2		0xFD61
+#define CARD_PULL_CTL3		0xFD62
+#define CARD_PULL_CTL4		0xFD63
+#define CARD_PULL_CTL5		0xFD64
+#define CARD_PULL_CTL6		0xFD65
+
+#define IRQEN0				0xFE20
+#define IRQSTAT0			0xFE21
+#define IRQEN1				0xFE22
+#define IRQSTAT1			0xFE23
+#define TLPRIEN				0xFE24
+#define TLPRISTAT			0xFE25
+#define TLPTIEN				0xFE26
+#define TLPTISTAT			0xFE27
+#define DMATC0				0xFE28
+#define DMATC1				0xFE29
+#define DMATC2				0xFE2A
+#define DMATC3				0xFE2B
+#define DMACTL				0xFE2C
+#define BCTL				0xFE2D
+#define RBBC0				0xFE2E
+#define RBBC1				0xFE2F
+#define RBDAT				0xFE30
+#define RBCTL				0xFE34
+#define CFGADDR0			0xFE35
+#define CFGADDR1			0xFE36
+#define CFGDATA0			0xFE37
+#define CFGDATA1			0xFE38
+#define CFGDATA2			0xFE39
+#define CFGDATA3			0xFE3A
+#define CFGRWCTL			0xFE3B
+#define PHYRWCTL			0xFE3C
+#define PHYDATA0			0xFE3D
+#define PHYDATA1			0xFE3E
+#define PHYADDR				0xFE3F
+#define MSGRXDATA0			0xFE40
+#define MSGRXDATA1			0xFE41
+#define MSGRXDATA2			0xFE42
+#define MSGRXDATA3			0xFE43
+#define MSGTXDATA0			0xFE44
+#define MSGTXDATA1			0xFE45
+#define MSGTXDATA2			0xFE46
+#define MSGTXDATA3			0xFE47
+#define MSGTXCTL			0xFE48
+#define PETXCFG				0xFE49
+
+#define CDRESUMECTL			0xFE52
+#define WAKE_SEL_CTL			0xFE54
+#define PME_FORCE_CTL			0xFE56
+#define ASPM_FORCE_CTL			0xFE57
+#define PM_CLK_FORCE_CTL		0xFE58
+#define PERST_GLITCH_WIDTH		0xFE5C
+#define CHANGE_LINK_STATE		0xFE5B
+#define RESET_LOAD_REG			0xFE5E
+#define HOST_SLEEP_STATE		0xFE60
+#define MAIN_PWR_OFF_CTL		0xFE70	/* RTS5208 */
+
+#define NFTS_TX_CTRL			0xFE72
+
+#define PWR_GATE_CTRL			0xFE75
+#define PWD_SUSPEND_EN			0xFE76
+
+#define EFUSE_CONTENT			0xFE5F
+
+#define XD_INIT				0xFD10
+#define XD_DTCTL			0xFD11
+#define XD_CTL				0xFD12
+#define XD_TRANSFER			0xFD13
+#define XD_CFG				0xFD14
+#define XD_ADDRESS0			0xFD15
+#define XD_ADDRESS1			0xFD16
+#define XD_ADDRESS2			0xFD17
+#define XD_ADDRESS3			0xFD18
+#define XD_ADDRESS4			0xFD19
+#define XD_DAT				0xFD1A
+#define XD_PAGE_CNT			0xFD1B
+#define XD_PAGE_STATUS			0xFD1C
+#define XD_BLOCK_STATUS			0xFD1D
+#define XD_BLOCK_ADDR1_L		0xFD1E
+#define XD_BLOCK_ADDR1_H		0xFD1F
+#define XD_BLOCK_ADDR2_L		0xFD20
+#define XD_BLOCK_ADDR2_H		0xFD21
+#define XD_BYTE_CNT_L			0xFD22
+#define XD_BYTE_CNT_H			0xFD23
+#define	XD_PARITY			0xFD24
+#define XD_ECC_BIT1			0xFD25
+#define XD_ECC_BYTE1			0xFD26
+#define XD_ECC_BIT2			0xFD27
+#define XD_ECC_BYTE2			0xFD28
+#define XD_RESERVED0			0xFD29
+#define XD_RESERVED1			0xFD2A
+#define XD_RESERVED2			0xFD2B
+#define XD_RESERVED3			0xFD2C
+#define XD_CHK_DATA_STATUS		0xFD2D
+#define XD_CATCTL			0xFD2E
+
+#define MS_CFG				0xFD40
+#define MS_TPC				0xFD41
+#define MS_TRANS_CFG			0xFD42
+#define MS_TRANSFER			0xFD43
+#define MS_INT_REG			0xFD44
+#define MS_BYTE_CNT			0xFD45
+#define MS_SECTOR_CNT_L			0xFD46
+#define MS_SECTOR_CNT_H			0xFD47
+#define MS_DBUS_H			0xFD48
+
+#define SSC_CTL1			0xFC11
+#define SSC_CTL2			0xFC12
+
+#define OCPCTL				0xFC15
+#define OCPSTAT				0xFC16
+#define OCPCLR				0xFC17	/* 5208 */
+#define OCPPARA1			0xFC18
+#define OCPPARA2			0xFC19
+
+#define EFUSE_OP			0xFC20
+#define EFUSE_CTRL			0xFC21
+#define EFUSE_DATA			0xFC22
+
+#define	SPI_COMMAND			0xFD80
+#define	SPI_ADDR0			0xFD81
+#define	SPI_ADDR1			0xFD82
+#define	SPI_ADDR2			0xFD83
+#define	SPI_ADDR3			0xFD84
+#define	SPI_CA_NUMBER			0xFD85
+#define	SPI_LENGTH0			0xFD86
+#define	SPI_LENGTH1			0xFD87
+#define	SPI_DATA			0xFD88
+#define SPI_DATA_NUMBER			0xFD89
+#define	SPI_TRANSFER0			0xFD90
+#define	SPI_TRANSFER1			0xFD91
+#define	SPI_CONTROL			0xFD92
+#define	SPI_SIG				0xFD93
+#define	SPI_TCTL			0xFD94
+#define	SPI_SLAVE_NUM			0xFD95
+#define	SPI_CLK_DIVIDER0		0xFD96
+#define	SPI_CLK_DIVIDER1		0xFD97
+
+#define SRAM_BASE			0xE600
+#define RBUF_BASE			0xF400
+#define PPBUF_BASE1			0xF800
+#define PPBUF_BASE2			0xFA00
+#define IMAGE_FLAG_ADDR0		0xCE80
+#define IMAGE_FLAG_ADDR1		0xCE81
+
+#define READ_OP			1
+#define WRITE_OP		2
+
+#define LCTLR		0x80
+
+#define POLLING_WAIT_CNT	1
+#define IDLE_MAX_COUNT		10
+#define SDIO_IDLE_COUNT		10
+
+#define DEBOUNCE_CNT			5
+
+void do_remaining_work(struct rtsx_chip *chip);
+void try_to_switch_sdio_ctrl(struct rtsx_chip *chip);
+void do_reset_sd_card(struct rtsx_chip *chip);
+void do_reset_xd_card(struct rtsx_chip *chip);
+void do_reset_ms_card(struct rtsx_chip *chip);
+void rtsx_power_off_card(struct rtsx_chip *chip);
+void rtsx_release_cards(struct rtsx_chip *chip);
+void rtsx_reset_cards(struct rtsx_chip *chip);
+void rtsx_reinit_cards(struct rtsx_chip *chip, int reset_chip);
+void rtsx_init_cards(struct rtsx_chip *chip);
+int switch_ssc_clock(struct rtsx_chip *chip, int clk);
+int switch_normal_clock(struct rtsx_chip *chip, int clk);
+int enable_card_clock(struct rtsx_chip *chip, u8 card);
+int disable_card_clock(struct rtsx_chip *chip, u8 card);
+int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	    u32 sec_addr, u16 sec_cnt);
+void trans_dma_enable(enum dma_data_direction dir,
+		      struct rtsx_chip *chip, u32 byte_cnt, u8 pack_size);
+void toggle_gpio(struct rtsx_chip *chip, u8 gpio);
+void turn_on_led(struct rtsx_chip *chip, u8 gpio);
+void turn_off_led(struct rtsx_chip *chip, u8 gpio);
+
+int card_share_mode(struct rtsx_chip *chip, int card);
+int select_card(struct rtsx_chip *chip, int card);
+int detect_card_cd(struct rtsx_chip *chip, int card);
+int check_card_exist(struct rtsx_chip *chip, unsigned int lun);
+int check_card_ready(struct rtsx_chip *chip, unsigned int lun);
+int check_card_wp(struct rtsx_chip *chip, unsigned int lun);
+void eject_card(struct rtsx_chip *chip, unsigned int lun);
+u8 get_lun_card(struct rtsx_chip *chip, unsigned int lun);
+
+static inline u32 get_card_size(struct rtsx_chip *chip, unsigned int lun)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &chip->sd_card;
+
+	if ((get_lun_card(chip, lun) == SD_CARD) &&
+	    (sd_card->sd_lock_status & SD_LOCKED))
+		return 0;
+
+	return chip->capacity[lun];
+#else
+	return chip->capacity[lun];
+#endif
+}
+
+static inline int switch_clock(struct rtsx_chip *chip, int clk)
+{
+	int retval = 0;
+
+	if (chip->asic_code)
+		retval = switch_ssc_clock(chip, clk);
+	else
+		retval = switch_normal_clock(chip, clk);
+
+	return retval;
+}
+
+int card_power_on(struct rtsx_chip *chip, u8 card);
+int card_power_off(struct rtsx_chip *chip, u8 card);
+
+static inline int card_power_off_all(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = rtsx_write_register(chip, CARD_PWR_CTL, 0x0F, 0x0F);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static inline void rtsx_clear_xd_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, XD_STOP | XD_CLR_ERR,
+			    XD_STOP | XD_CLR_ERR);
+}
+
+static inline void rtsx_clear_sd_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+			    SD_STOP | SD_CLR_ERR);
+}
+
+static inline void rtsx_clear_ms_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
+			    MS_STOP | MS_CLR_ERR);
+}
+
+static inline void rtsx_clear_spi_error(struct rtsx_chip *chip)
+{
+	rtsx_write_register(chip, CARD_STOP, SPI_STOP | SPI_CLR_ERR,
+			    SPI_STOP | SPI_CLR_ERR);
+}
+
+#ifdef SUPPORT_SDIO_ASPM
+void dynamic_configure_sdio_aspm(struct rtsx_chip *chip);
+#endif
+
+#endif  /* __REALTEK_RTSX_CARD_H */
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
new file mode 100644
index 000000000..637503291
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -0,0 +1,2161 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+#include <linux/vmalloc.h>
+
+#include "rtsx.h"
+#include "sd.h"
+#include "xd.h"
+#include "ms.h"
+
+static void rtsx_calibration(struct rtsx_chip *chip)
+{
+	rtsx_write_phy_register(chip, 0x1B, 0x135E);
+	wait_timeout(10);
+	rtsx_write_phy_register(chip, 0x00, 0x0280);
+	rtsx_write_phy_register(chip, 0x01, 0x7112);
+	rtsx_write_phy_register(chip, 0x01, 0x7110);
+	rtsx_write_phy_register(chip, 0x01, 0x7112);
+	rtsx_write_phy_register(chip, 0x01, 0x7113);
+	rtsx_write_phy_register(chip, 0x00, 0x0288);
+}
+
+void rtsx_enable_card_int(struct rtsx_chip *chip)
+{
+	u32 reg = rtsx_readl(chip, RTSX_BIER);
+	int i;
+
+	for (i = 0; i <= chip->max_lun; i++) {
+		if (chip->lun2card[i] & XD_CARD)
+			reg |= XD_INT_EN;
+		if (chip->lun2card[i] & SD_CARD)
+			reg |= SD_INT_EN;
+		if (chip->lun2card[i] & MS_CARD)
+			reg |= MS_INT_EN;
+	}
+	if (chip->hw_bypass_sd)
+		reg &= ~((u32)SD_INT_EN);
+
+	rtsx_writel(chip, RTSX_BIER, reg);
+}
+
+void rtsx_enable_bus_int(struct rtsx_chip *chip)
+{
+	u32 reg = 0;
+#ifndef DISABLE_CARD_INT
+	int i;
+#endif
+
+	reg = TRANS_OK_INT_EN | TRANS_FAIL_INT_EN;
+
+#ifndef DISABLE_CARD_INT
+	for (i = 0; i <= chip->max_lun; i++) {
+		dev_dbg(rtsx_dev(chip), "lun2card[%d] = 0x%02x\n",
+			i, chip->lun2card[i]);
+
+		if (chip->lun2card[i] & XD_CARD)
+			reg |= XD_INT_EN;
+		if (chip->lun2card[i] & SD_CARD)
+			reg |= SD_INT_EN;
+		if (chip->lun2card[i] & MS_CARD)
+			reg |= MS_INT_EN;
+	}
+	if (chip->hw_bypass_sd)
+		reg &= ~((u32)SD_INT_EN);
+#endif
+
+	if (chip->ic_version >= IC_VER_C)
+		reg |= DELINK_INT_EN;
+#ifdef SUPPORT_OCP
+	reg |= OC_INT_EN;
+#endif
+	if (!chip->adma_mode)
+		reg |= DATA_DONE_INT_EN;
+
+	/* Enable Bus Interrupt */
+	rtsx_writel(chip, RTSX_BIER, reg);
+
+	dev_dbg(rtsx_dev(chip), "RTSX_BIER: 0x%08x\n", reg);
+}
+
+void rtsx_disable_bus_int(struct rtsx_chip *chip)
+{
+	rtsx_writel(chip, RTSX_BIER, 0);
+}
+
+static int rtsx_pre_handle_sdio_old(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (chip->ignore_sd && CHK_SDIO_EXIST(chip)) {
+		if (chip->asic_code) {
+			retval = rtsx_write_register(chip, CARD_PULL_CTL5,
+						     0xFF,
+						     MS_INS_PU | SD_WP_PU |
+						     SD_CD_PU | SD_CMD_PU);
+			if (retval)
+				return retval;
+		} else {
+			retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+						     0xFF,
+						     FPGA_SD_PULL_CTL_EN);
+			if (retval)
+				return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_SHARE_MODE, 0xFF,
+					     CARD_SHARE_48_SD);
+		if (retval)
+			return retval;
+
+		/* Enable SDIO internal clock */
+		retval = rtsx_write_register(chip, 0xFF2C, 0x01, 0x01);
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, SDIO_CTRL, 0xFF,
+					     SDIO_BUS_CTRL | SDIO_CD_CTRL);
+		if (retval)
+			return retval;
+
+		chip->sd_int = 1;
+		chip->sd_io = 1;
+	} else {
+		chip->need_reset |= SD_CARD;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef HW_AUTO_SWITCH_SD_BUS
+static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
+{
+	u8 tmp;
+	bool sw_bypass_sd = false;
+	int retval;
+
+	if (chip->driver_first_load) {
+		if (CHECK_PID(chip, 0x5288)) {
+			retval = rtsx_read_register(chip, 0xFE5A, &tmp);
+			if (retval)
+				return retval;
+			if (tmp & 0x08)
+				sw_bypass_sd = true;
+		} else if (CHECK_PID(chip, 0x5208)) {
+			retval = rtsx_read_register(chip, 0xFE70, &tmp);
+			if (retval)
+				return retval;
+			if (tmp & 0x80)
+				sw_bypass_sd = true;
+		}
+	} else {
+		if (chip->sdio_in_charge)
+			sw_bypass_sd = true;
+	}
+	dev_dbg(rtsx_dev(chip), "chip->sdio_in_charge = %d\n",
+		chip->sdio_in_charge);
+	dev_dbg(rtsx_dev(chip), "chip->driver_first_load = %d\n",
+		chip->driver_first_load);
+	dev_dbg(rtsx_dev(chip), "sw_bypass_sd = %d\n",
+		sw_bypass_sd);
+
+	if (sw_bypass_sd) {
+		u8 cd_toggle_mask = 0;
+
+		retval = rtsx_read_register(chip, TLPTISTAT, &tmp);
+		if (retval)
+			return retval;
+		cd_toggle_mask = 0x08;
+
+		if (tmp & cd_toggle_mask) {
+			/* Disable sdio_bus_auto_switch */
+			if (CHECK_PID(chip, 0x5288)) {
+				retval = rtsx_write_register(chip, 0xFE5A,
+							     0x08, 0x00);
+				if (retval)
+					return retval;
+			} else if (CHECK_PID(chip, 0x5208)) {
+				retval = rtsx_write_register(chip, 0xFE70,
+							     0x80, 0x00);
+				if (retval)
+					return retval;
+			}
+
+			retval = rtsx_write_register(chip, TLPTISTAT, 0xFF,
+						     tmp);
+			if (retval)
+				return retval;
+
+			chip->need_reset |= SD_CARD;
+		} else {
+			dev_dbg(rtsx_dev(chip), "Chip inserted with SDIO!\n");
+
+			if (chip->asic_code) {
+				retval = sd_pull_ctl_enable(chip);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+			} else {
+				retval = rtsx_write_register
+						(chip, FPGA_PULL_CTL,
+						 FPGA_SD_PULL_CTL_BIT | 0x20,
+						 0);
+				if (retval)
+					return retval;
+			}
+			retval = card_share_mode(chip, SD_CARD);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			/* Enable sdio_bus_auto_switch */
+			if (CHECK_PID(chip, 0x5288)) {
+				retval = rtsx_write_register(chip, 0xFE5A,
+							     0x08, 0x08);
+				if (retval)
+					return retval;
+			} else if (CHECK_PID(chip, 0x5208)) {
+				retval = rtsx_write_register(chip, 0xFE70,
+							     0x80, 0x80);
+				if (retval)
+					return retval;
+			}
+
+			chip->chip_insert_with_sdio = 1;
+			chip->sd_io = 1;
+		}
+	} else {
+		retval = rtsx_write_register(chip, TLPTISTAT, 0x08, 0x08);
+		if (retval)
+			return retval;
+
+		chip->need_reset |= SD_CARD;
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int rtsx_reset_aspm(struct rtsx_chip *chip)
+{
+	int ret;
+
+	if (chip->dynamic_aspm) {
+		if (!CHK_SDIO_EXIST(chip) || !CHECK_PID(chip, 0x5288))
+			return STATUS_SUCCESS;
+
+		ret = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF,
+					chip->aspm_l0s_l1_en);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		return STATUS_SUCCESS;
+	}
+
+	if (CHECK_PID(chip, 0x5208)) {
+		ret = rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFF, 0x3F);
+		if (ret)
+			return ret;
+	}
+	ret = rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
+	if (ret != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	chip->aspm_level[0] = chip->aspm_l0s_l1_en;
+	if (CHK_SDIO_EXIST(chip)) {
+		chip->aspm_level[1] = chip->aspm_l0s_l1_en;
+		ret = rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+					0xC0, 0xFF, chip->aspm_l0s_l1_en);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	chip->aspm_enabled = 1;
+
+	return STATUS_SUCCESS;
+}
+
+static int rtsx_enable_pcie_intr(struct rtsx_chip *chip)
+{
+	int ret;
+
+	if (!chip->asic_code || !CHECK_PID(chip, 0x5208)) {
+		rtsx_enable_bus_int(chip);
+		return STATUS_SUCCESS;
+	}
+
+	if (chip->phy_debug_mode) {
+		ret = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0);
+		if (ret)
+			return ret;
+		rtsx_disable_bus_int(chip);
+	} else {
+		rtsx_enable_bus_int(chip);
+	}
+
+	if (chip->ic_version >= IC_VER_D) {
+		u16 reg;
+
+		ret = rtsx_read_phy_register(chip, 0x00, &reg);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		reg &= 0xFE7F;
+		reg |= 0x80;
+		ret = rtsx_write_phy_register(chip, 0x00, reg);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		ret = rtsx_read_phy_register(chip, 0x1C, &reg);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		reg &= 0xFFF7;
+		ret = rtsx_write_phy_register(chip, 0x1C, reg);
+		if (ret != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	if (chip->driver_first_load && chip->ic_version < IC_VER_C)
+		rtsx_calibration(chip);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_reset_chip(struct rtsx_chip *chip)
+{
+	int retval;
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	rtsx_disable_aspm(chip);
+
+	retval = rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+	if (retval)
+		return retval;
+
+	/* Disable card clock */
+	retval = rtsx_write_register(chip, CARD_CLK_EN, 0x1E, 0);
+	if (retval)
+		return retval;
+
+#ifdef SUPPORT_OCP
+	/* SSC power on, OCD power on */
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN, 0);
+		if (retval)
+			return retval;
+	} else {
+		retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN,
+					     MS_OC_POWER_DOWN);
+		if (retval)
+			return retval;
+	}
+
+	retval = rtsx_write_register(chip, OCPPARA1, OCP_TIME_MASK,
+				     OCP_TIME_800);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, OCPPARA2, OCP_THD_MASK,
+				     OCP_THD_244_946);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, OCPCTL, 0xFF,
+				     CARD_OC_INT_EN | CARD_DETECT_EN);
+	if (retval)
+		return retval;
+#else
+	/* OC power down */
+	retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN,
+				     OC_POWER_DOWN);
+	if (retval)
+		return retval;
+#endif
+
+	if (!CHECK_PID(chip, 0x5288)) {
+		retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0xFF, 0x03);
+		if (retval)
+			return retval;
+	}
+
+	/* Turn off LED */
+	retval = rtsx_write_register(chip, CARD_GPIO, 0xFF, 0x03);
+	if (retval)
+		return retval;
+
+	/* Reset delink mode */
+	retval = rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0);
+	if (retval)
+		return retval;
+
+	/* Card driving select */
+	retval = rtsx_write_register(chip, CARD_DRIVE_SEL, 0xFF,
+				     chip->card_drive_sel);
+	if (retval)
+		return retval;
+
+#ifdef LED_AUTO_BLINK
+	retval = rtsx_write_register(chip, CARD_AUTO_BLINK, 0xFF,
+				     LED_BLINK_SPEED | BLINK_EN | LED_GPIO0);
+	if (retval)
+		return retval;
+#endif
+
+	if (chip->asic_code) {
+		/* Enable SSC Clock */
+		retval = rtsx_write_register(chip, SSC_CTL1, 0xFF,
+					     SSC_8X_EN | SSC_SEL_4M);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SSC_CTL2, 0xFF, 0x12);
+		if (retval)
+			return retval;
+	}
+
+	/*
+	 * Disable cd_pwr_save (u_force_rst_core_en=0, u_cd_rst_core_en=0)
+	 *    0xFE5B
+	 *    bit[1]    u_cd_rst_core_en	rst_value = 0
+	 *    bit[2]    u_force_rst_core_en	rst_value = 0
+	 *    bit[5]    u_mac_phy_rst_n_dbg	rst_value = 1
+	 *    bit[4]	u_non_sticky_rst_n_dbg	rst_value = 0
+	 */
+	retval = rtsx_write_register(chip, CHANGE_LINK_STATE, 0x16, 0x10);
+	if (retval)
+		return retval;
+
+	/* Enable ASPM */
+	if (chip->aspm_l0s_l1_en) {
+		retval = rtsx_reset_aspm(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
+			retval = rtsx_write_phy_register(chip, 0x07, 0x0129);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		}
+		retval = rtsx_write_config_byte(chip, LCTLR,
+						chip->aspm_l0s_l1_en);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_config_byte(chip, 0x81, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_SDIO_EXIST(chip)) {
+		retval = rtsx_write_cfg_dw(chip,
+					   CHECK_PID(chip, 0x5288) ? 2 : 1,
+					   0xC0, 0xFF00, 0x0100);
+
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	if (CHECK_PID(chip, 0x5288) && !CHK_SDIO_EXIST(chip)) {
+		retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF, 0x0103);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = rtsx_write_cfg_dw(chip, 2, 0x84, 0xFF, 0x03);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, IRQSTAT0, LINK_RDY_INT,
+				     LINK_RDY_INT);
+	if (retval)
+		return retval;
+
+	retval = rtsx_write_register(chip, PERST_GLITCH_WIDTH, 0xFF, 0x80);
+	if (retval)
+		return retval;
+
+	retval = rtsx_enable_pcie_intr(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	chip->need_reset = 0;
+
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if (chip->hw_bypass_sd)
+		goto nextcard;
+	dev_dbg(rtsx_dev(chip), "In %s, chip->int_reg = 0x%x\n", __func__,
+		chip->int_reg);
+	if (chip->int_reg & SD_EXIST) {
+#ifdef HW_AUTO_SWITCH_SD_BUS
+		if (CHECK_PID(chip, 0x5208) && chip->ic_version < IC_VER_C)
+			retval = rtsx_pre_handle_sdio_old(chip);
+		else
+			retval = rtsx_pre_handle_sdio_new(chip);
+
+		dev_dbg(rtsx_dev(chip), "chip->need_reset = 0x%x (%s)\n",
+			(unsigned int)(chip->need_reset), __func__);
+#else  /* HW_AUTO_SWITCH_SD_BUS */
+		retval = rtsx_pre_handle_sdio_old(chip);
+#endif  /* HW_AUTO_SWITCH_SD_BUS */
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+	} else {
+		chip->sd_io = 0;
+		retval = rtsx_write_register(chip, SDIO_CTRL,
+					     SDIO_BUS_CTRL | SDIO_CD_CTRL, 0);
+		if (retval)
+			return retval;
+	}
+
+nextcard:
+	if (chip->int_reg & XD_EXIST)
+		chip->need_reset |= XD_CARD;
+	if (chip->int_reg & MS_EXIST)
+		chip->need_reset |= MS_CARD;
+	if (chip->int_reg & CARD_EXIST) {
+		retval = rtsx_write_register(chip, SSC_CTL1, SSC_RSTB,
+					     SSC_RSTB);
+		if (retval)
+			return retval;
+	}
+
+	dev_dbg(rtsx_dev(chip), "In %s, chip->need_reset = 0x%x\n", __func__,
+		(unsigned int)(chip->need_reset));
+
+	retval = rtsx_write_register(chip, RCCTL, 0x01, 0x00);
+	if (retval)
+		return retval;
+
+	if (CHECK_PID(chip, 0x5208) || CHECK_PID(chip, 0x5288)) {
+		/* Turn off main power when entering S3/S4 state */
+		retval = rtsx_write_register(chip, MAIN_PWR_OFF_CTL, 0x03,
+					     0x03);
+		if (retval)
+			return retval;
+	}
+
+	if (chip->remote_wakeup_en && !chip->auto_delink_en) {
+		retval = rtsx_write_register(chip, WAKE_SEL_CTL, 0x07, 0x07);
+		if (retval)
+			return retval;
+		if (chip->aux_pwr_exist) {
+			retval = rtsx_write_register(chip, PME_FORCE_CTL,
+						     0xFF, 0x33);
+			if (retval)
+				return retval;
+		}
+	} else {
+		retval = rtsx_write_register(chip, WAKE_SEL_CTL, 0x07, 0x04);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, PME_FORCE_CTL, 0xFF, 0x30);
+		if (retval)
+			return retval;
+	}
+
+	if (CHECK_PID(chip, 0x5208) && chip->ic_version >= IC_VER_D) {
+		retval = rtsx_write_register(chip, PETXCFG, 0x1C, 0x14);
+		if (retval)
+			return retval;
+	}
+
+	if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
+		retval = rtsx_clr_phy_reg_bit(chip, 0x1C, 2);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	if (chip->ft2_fast_mode) {
+		retval = rtsx_write_register(chip, CARD_PWR_CTL, 0xFF,
+					     MS_PARTIAL_POWER_ON |
+					     SD_PARTIAL_POWER_ON);
+		if (retval)
+			return retval;
+		udelay(chip->pmos_pwr_on_interval);
+		retval = rtsx_write_register(chip, CARD_PWR_CTL, 0xFF,
+					     MS_POWER_ON | SD_POWER_ON);
+		if (retval)
+			return retval;
+
+		wait_timeout(200);
+	}
+
+	/* Reset card */
+	rtsx_reset_detected_cards(chip, 0);
+
+	chip->driver_first_load = 0;
+
+	return STATUS_SUCCESS;
+}
+
+static inline int valid_sd_speed_prior(u32 sd_speed_prior)
+{
+	bool valid_para = true;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		u8 tmp = (u8)(sd_speed_prior >> (i * 8));
+
+		if (tmp < 0x01 || tmp > 0x04) {
+			valid_para = false;
+			break;
+		}
+	}
+
+	return valid_para;
+}
+
+static inline int valid_sd_current_prior(u32 sd_current_prior)
+{
+	bool valid_para = true;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		u8 tmp = (u8)(sd_current_prior >> (i * 8));
+
+		if (tmp > 0x03) {
+			valid_para = false;
+			break;
+		}
+	}
+
+	return valid_para;
+}
+
+static int rts5208_init(struct rtsx_chip *chip)
+{
+	int retval;
+	u16 reg = 0;
+	u8 val = 0;
+
+	retval = rtsx_write_register(chip, CLK_SEL, 0x03, 0x03);
+	if (retval)
+		return retval;
+	retval = rtsx_read_register(chip, CLK_SEL, &val);
+	if (retval)
+		return retval;
+	chip->asic_code = val == 0 ? 1 : 0;
+
+	if (chip->asic_code) {
+		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		dev_dbg(rtsx_dev(chip), "Value of phy register 0x1C is 0x%x\n",
+			reg);
+		chip->ic_version = (reg >> 4) & 0x07;
+		chip->phy_debug_mode = reg & PHY_DEBUG_MODE ? 1 : 0;
+
+	} else {
+		retval = rtsx_read_register(chip, 0xFE80, &val);
+		if (retval)
+			return retval;
+		chip->ic_version = val;
+		chip->phy_debug_mode = 0;
+	}
+
+	retval = rtsx_read_register(chip, PDINFO, &val);
+	if (retval)
+		return retval;
+	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
+	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
+
+	retval = rtsx_read_register(chip, 0xFE50, &val);
+	if (retval)
+		return retval;
+	chip->hw_bypass_sd = val & 0x01 ? 1 : 0;
+
+	rtsx_read_config_byte(chip, 0x0E, &val);
+	if (val & 0x80)
+		SET_SDIO_EXIST(chip);
+	else
+		CLR_SDIO_EXIST(chip);
+
+	if (chip->use_hw_setting) {
+		retval = rtsx_read_register(chip, CHANGE_LINK_STATE, &val);
+		if (retval)
+			return retval;
+		chip->auto_delink_en = val & 0x80 ? 1 : 0;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int rts5288_init(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 val = 0, max_func;
+	u32 lval = 0;
+
+	retval = rtsx_write_register(chip, CLK_SEL, 0x03, 0x03);
+	if (retval)
+		return retval;
+	retval = rtsx_read_register(chip, CLK_SEL, &val);
+	if (retval)
+		return retval;
+	chip->asic_code = val == 0 ? 1 : 0;
+
+	chip->ic_version = 0;
+	chip->phy_debug_mode = 0;
+
+	retval = rtsx_read_register(chip, PDINFO, &val);
+	if (retval)
+		return retval;
+	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
+	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
+
+	retval = rtsx_read_register(chip, CARD_SHARE_MODE, &val);
+	if (retval)
+		return retval;
+	dev_dbg(rtsx_dev(chip), "CARD_SHARE_MODE: 0x%x\n", val);
+	chip->baro_pkg = val & 0x04 ? QFN : LQFP;
+
+	retval = rtsx_read_register(chip, 0xFE5A, &val);
+	if (retval)
+		return retval;
+	chip->hw_bypass_sd = val & 0x10 ? 1 : 0;
+
+	retval = rtsx_read_cfg_dw(chip, 0, 0x718, &lval);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	max_func = (u8)((lval >> 29) & 0x07);
+	dev_dbg(rtsx_dev(chip), "Max function number: %d\n", max_func);
+	if (max_func == 0x02)
+		SET_SDIO_EXIST(chip);
+	else
+		CLR_SDIO_EXIST(chip);
+
+	if (chip->use_hw_setting) {
+		retval = rtsx_read_register(chip, CHANGE_LINK_STATE, &val);
+		if (retval)
+			return retval;
+		chip->auto_delink_en = val & 0x80 ? 1 : 0;
+
+		if (CHECK_BARO_PKG(chip, LQFP))
+			chip->lun_mode = SD_MS_1LUN;
+		else
+			chip->lun_mode = DEFAULT_SINGLE;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_init_chip(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	struct xd_info *xd_card = &chip->xd_card;
+	struct ms_info *ms_card = &chip->ms_card;
+	int retval;
+	unsigned int i;
+
+	dev_dbg(rtsx_dev(chip), "Vendor ID: 0x%04x, Product ID: 0x%04x\n",
+		chip->vendor_id, chip->product_id);
+
+	chip->ic_version = 0;
+
+	memset(xd_card, 0, sizeof(struct xd_info));
+	memset(sd_card, 0, sizeof(struct sd_info));
+	memset(ms_card, 0, sizeof(struct ms_info));
+
+	chip->xd_reset_counter = 0;
+	chip->sd_reset_counter = 0;
+	chip->ms_reset_counter = 0;
+
+	chip->xd_show_cnt = MAX_SHOW_CNT;
+	chip->sd_show_cnt = MAX_SHOW_CNT;
+	chip->ms_show_cnt = MAX_SHOW_CNT;
+
+	chip->sd_io = 0;
+	chip->auto_delink_cnt = 0;
+	chip->auto_delink_allowed = 1;
+	rtsx_set_stat(chip, RTSX_STAT_INIT);
+
+	chip->aspm_enabled = 0;
+	chip->chip_insert_with_sdio = 0;
+	chip->sdio_aspm = 0;
+	chip->sdio_idle = 0;
+	chip->sdio_counter = 0;
+	chip->cur_card = 0;
+	chip->phy_debug_mode = 0;
+	chip->sdio_func_exist = 0;
+	memset(chip->sdio_raw_data, 0, 12);
+
+	for (i = 0; i < MAX_ALLOWED_LUN_CNT; i++) {
+		set_sense_type(chip, i, SENSE_TYPE_NO_SENSE);
+		chip->rw_fail_cnt[i] = 0;
+	}
+
+	if (!valid_sd_speed_prior(chip->sd_speed_prior))
+		chip->sd_speed_prior = 0x01040203;
+
+	dev_dbg(rtsx_dev(chip), "sd_speed_prior = 0x%08x\n",
+		chip->sd_speed_prior);
+
+	if (!valid_sd_current_prior(chip->sd_current_prior))
+		chip->sd_current_prior = 0x00010203;
+
+	dev_dbg(rtsx_dev(chip), "sd_current_prior = 0x%08x\n",
+		chip->sd_current_prior);
+
+	if (chip->sd_ddr_tx_phase > 31 || chip->sd_ddr_tx_phase < 0)
+		chip->sd_ddr_tx_phase = 0;
+
+	if (chip->mmc_ddr_tx_phase > 31 || chip->mmc_ddr_tx_phase < 0)
+		chip->mmc_ddr_tx_phase = 0;
+
+	retval = rtsx_write_register(chip, FPDCTL, SSC_POWER_DOWN, 0);
+	if (retval)
+		return retval;
+	wait_timeout(200);
+	retval = rtsx_write_register(chip, CLK_DIV, 0x07, 0x07);
+	if (retval)
+		return retval;
+	dev_dbg(rtsx_dev(chip), "chip->use_hw_setting = %d\n",
+		chip->use_hw_setting);
+
+	if (CHECK_PID(chip, 0x5208)) {
+		retval = rts5208_init(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+	} else if (CHECK_PID(chip, 0x5288)) {
+		retval = rts5288_init(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	if (chip->ss_en == 2)
+		chip->ss_en = 0;
+
+	dev_dbg(rtsx_dev(chip), "chip->asic_code = %d\n", chip->asic_code);
+	dev_dbg(rtsx_dev(chip), "chip->ic_version = 0x%x\n", chip->ic_version);
+	dev_dbg(rtsx_dev(chip), "chip->phy_debug_mode = %d\n",
+		chip->phy_debug_mode);
+	dev_dbg(rtsx_dev(chip), "chip->aux_pwr_exist = %d\n",
+		chip->aux_pwr_exist);
+	dev_dbg(rtsx_dev(chip), "chip->sdio_func_exist = %d\n",
+		chip->sdio_func_exist);
+	dev_dbg(rtsx_dev(chip), "chip->hw_bypass_sd = %d\n",
+		chip->hw_bypass_sd);
+	dev_dbg(rtsx_dev(chip), "chip->aspm_l0s_l1_en = %d\n",
+		chip->aspm_l0s_l1_en);
+	dev_dbg(rtsx_dev(chip), "chip->lun_mode = %d\n", chip->lun_mode);
+	dev_dbg(rtsx_dev(chip), "chip->auto_delink_en = %d\n",
+		chip->auto_delink_en);
+	dev_dbg(rtsx_dev(chip), "chip->ss_en = %d\n", chip->ss_en);
+	dev_dbg(rtsx_dev(chip), "chip->baro_pkg = %d\n", chip->baro_pkg);
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		chip->card2lun[SD_CARD] = 0;
+		chip->card2lun[MS_CARD] = 1;
+		chip->card2lun[XD_CARD] = 0xFF;
+		chip->lun2card[0] = SD_CARD;
+		chip->lun2card[1] = MS_CARD;
+		chip->max_lun = 1;
+		SET_SDIO_IGNORED(chip);
+	} else if (CHECK_LUN_MODE(chip, SD_MS_1LUN)) {
+		chip->card2lun[SD_CARD] = 0;
+		chip->card2lun[MS_CARD] = 0;
+		chip->card2lun[XD_CARD] = 0xFF;
+		chip->lun2card[0] = SD_CARD | MS_CARD;
+		chip->max_lun = 0;
+	} else {
+		chip->card2lun[XD_CARD] = 0;
+		chip->card2lun[SD_CARD] = 0;
+		chip->card2lun[MS_CARD] = 0;
+		chip->lun2card[0] = XD_CARD | SD_CARD | MS_CARD;
+		chip->max_lun = 0;
+	}
+
+	retval = rtsx_reset_chip(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+void rtsx_release_chip(struct rtsx_chip *chip)
+{
+	xd_free_l2p_tbl(chip);
+	ms_free_l2p_tbl(chip);
+	chip->card_exist = 0;
+	chip->card_ready = 0;
+}
+
+#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
+static inline void rtsx_blink_led(struct rtsx_chip *chip)
+{
+	if (chip->card_exist && chip->blink_led) {
+		if (chip->led_toggle_counter < LED_TOGGLE_INTERVAL) {
+			chip->led_toggle_counter++;
+		} else {
+			chip->led_toggle_counter = 0;
+			toggle_gpio(chip, LED_GPIO);
+		}
+	}
+}
+#endif
+
+static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
+{
+	bool reg_changed, maybe_support_aspm;
+	u32 tmp = 0;
+	u8 reg0 = 0, reg1 = 0;
+
+	maybe_support_aspm = false;
+	reg_changed = false;
+	rtsx_read_config_byte(chip, LCTLR, &reg0);
+	if (chip->aspm_level[0] != reg0) {
+		reg_changed = true;
+		chip->aspm_level[0] = reg0;
+	}
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+		rtsx_read_cfg_dw(chip, 1, 0xC0, &tmp);
+		reg1 = (u8)tmp;
+		if (chip->aspm_level[1] != reg1) {
+			reg_changed = true;
+			chip->aspm_level[1] = reg1;
+		}
+
+		if ((reg0 & 0x03) && (reg1 & 0x03))
+			maybe_support_aspm = true;
+
+	} else {
+		if (reg0 & 0x03)
+			maybe_support_aspm = true;
+	}
+
+	if (reg_changed) {
+		if (maybe_support_aspm)
+			chip->aspm_l0s_l1_en = 0x03;
+
+		dev_dbg(rtsx_dev(chip),
+			"aspm_level[0] = 0x%02x, aspm_level[1] = 0x%02x\n",
+			chip->aspm_level[0], chip->aspm_level[1]);
+
+		if (chip->aspm_l0s_l1_en) {
+			chip->aspm_enabled = 1;
+		} else {
+			chip->aspm_enabled = 0;
+			chip->sdio_aspm = 0;
+		}
+		rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFF,
+				    0x30 | chip->aspm_level[0] |
+				    (chip->aspm_level[1] << 2));
+	}
+}
+
+static void rtsx_manage_ocp(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!chip->ocp_int)
+		return;
+
+	rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD)
+		sd_power_off_card3v3(chip);
+	else if (chip->card_exist & MS_CARD)
+		ms_power_off_card3v3(chip);
+	else if (chip->card_exist & XD_CARD)
+		xd_power_off_card3v3(chip);
+
+	chip->ocp_int = 0;
+#endif
+}
+
+static void rtsx_manage_sd_lock(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &chip->sd_card;
+	u8 val;
+
+	if (!sd_card->sd_erase_status)
+		return;
+
+	if (chip->card_exist & SD_CARD) {
+		rtsx_read_register(chip, 0xFD30, &val);
+		if (val & 0x02) {
+			sd_card->sd_erase_status = SD_NOT_ERASE;
+			sd_card->sd_lock_notify = 1;
+			chip->need_reinit |= SD_CARD;
+		}
+	} else {
+		sd_card->sd_erase_status = SD_NOT_ERASE;
+	}
+#endif
+}
+
+static bool rtsx_is_ss_allowed(struct rtsx_chip *chip)
+{
+	u32 val;
+
+	if (!chip->ss_en || CHECK_PID(chip, 0x5288))
+		return false;
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+		rtsx_read_cfg_dw(chip, 1, 0x04, &val);
+		if (val & 0x07)
+			return false;
+	}
+
+	return true;
+}
+
+static void rtsx_manage_ss(struct rtsx_chip *chip)
+{
+	if (!rtsx_is_ss_allowed(chip) || chip->sd_io)
+		return;
+
+	if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
+		chip->ss_counter = 0;
+		return;
+	}
+
+	if (chip->ss_counter < (chip->ss_idle_period / POLLING_INTERVAL))
+		chip->ss_counter++;
+	else
+		rtsx_exclusive_enter_ss(chip);
+}
+
+static void rtsx_manage_aspm(struct rtsx_chip *chip)
+{
+	u8 data;
+
+	if (!CHECK_PID(chip, 0x5208))
+		return;
+
+	rtsx_monitor_aspm_config(chip);
+
+#ifdef SUPPORT_SDIO_ASPM
+	if (!CHK_SDIO_EXIST(chip) || CHK_SDIO_IGNORED(chip) ||
+	    !chip->aspm_l0s_l1_en || !chip->dynamic_aspm)
+		return;
+
+	if (chip->sd_io) {
+		dynamic_configure_sdio_aspm(chip);
+		return;
+	}
+
+	if (chip->sdio_aspm)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
+	data = 0x30 | (chip->aspm_level[1] << 2);
+	rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFC, data);
+	chip->sdio_aspm = 1;
+#endif
+}
+
+static void rtsx_manage_idle(struct rtsx_chip *chip)
+{
+	if (chip->idle_counter < IDLE_MAX_COUNT) {
+		chip->idle_counter++;
+		return;
+	}
+
+	if (rtsx_get_stat(chip) == RTSX_STAT_IDLE)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "Idle state!\n");
+	rtsx_set_stat(chip, RTSX_STAT_IDLE);
+
+#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
+	chip->led_toggle_counter = 0;
+#endif
+	rtsx_force_power_on(chip, SSC_PDCTL);
+
+	turn_off_led(chip, LED_GPIO);
+
+	if (chip->auto_power_down && !chip->card_ready && !chip->sd_io)
+		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+}
+
+static void rtsx_manage_2lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	u8 sd_oc, ms_oc;
+
+	sd_oc = chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER);
+	ms_oc = chip->ocp_stat & (MS_OC_NOW | MS_OC_EVER);
+
+	if (sd_oc || ms_oc)
+		dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+			chip->ocp_stat);
+
+	if (sd_oc && (chip->card_exist & SD_CARD)) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		card_power_off(chip, SD_CARD);
+		chip->card_fail |= SD_CARD;
+	}
+
+	if (ms_oc && (chip->card_exist & MS_CARD)) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		card_power_off(chip, MS_CARD);
+		chip->card_fail |= MS_CARD;
+	}
+#endif
+}
+
+static void rtsx_manage_1lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!(chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)))
+		return;
+
+	dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+		chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		chip->card_fail |= SD_CARD;
+	} else if (chip->card_exist & MS_CARD) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		chip->card_fail |= MS_CARD;
+	} else if (chip->card_exist & XD_CARD) {
+		rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+		chip->card_fail |= XD_CARD;
+	}
+	card_power_off(chip, SD_CARD);
+#endif
+}
+
+static void rtsx_delink_stage1(struct rtsx_chip *chip, int enter_L1,
+			       int stage3_cnt)
+{
+	u8 val;
+
+	rtsx_set_stat(chip, RTSX_STAT_DELINK);
+
+	if (chip->asic_code && CHECK_PID(chip, 0x5208))
+		rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+	if (chip->card_exist)
+		dev_dbg(rtsx_dev(chip), "False card inserted, do force delink\n");
+	else
+		dev_dbg(rtsx_dev(chip), "No card inserted, do delink\n");
+
+	if (enter_L1)
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
+
+	if (chip->card_exist)
+		val = 0x02;
+	else
+		val = 0x0A;
+
+	rtsx_write_register(chip, CHANGE_LINK_STATE, val, val);
+
+	if (enter_L1)
+		rtsx_enter_L1(chip);
+
+	if (chip->card_exist)
+		chip->auto_delink_cnt = stage3_cnt + 1;
+}
+
+static void rtsx_delink_stage(struct rtsx_chip *chip)
+{
+	int delink_stage1_cnt, delink_stage2_cnt, delink_stage3_cnt;
+	int enter_L1;
+
+	if (!chip->auto_delink_en || !chip->auto_delink_allowed ||
+	    chip->card_ready || chip->card_ejected || chip->sd_io) {
+		chip->auto_delink_cnt = 0;
+		return;
+	}
+
+	enter_L1 = chip->auto_delink_in_L1 &&
+		(chip->aspm_l0s_l1_en || chip->ss_en);
+
+	delink_stage1_cnt = chip->delink_stage1_step;
+	delink_stage2_cnt = delink_stage1_cnt + chip->delink_stage2_step;
+	delink_stage3_cnt = delink_stage2_cnt + chip->delink_stage3_step;
+
+	if (chip->auto_delink_cnt > delink_stage3_cnt)
+		return;
+
+	if (chip->auto_delink_cnt == delink_stage1_cnt)
+		rtsx_delink_stage1(chip, enter_L1, delink_stage3_cnt);
+
+	if (chip->auto_delink_cnt == delink_stage2_cnt) {
+		dev_dbg(rtsx_dev(chip), "Try to do force delink\n");
+
+		if (enter_L1)
+			rtsx_exit_L1(chip);
+
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x0A);
+	}
+
+	chip->auto_delink_cnt++;
+}
+
+void rtsx_polling_func(struct rtsx_chip *chip)
+{
+	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
+		return;
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
+		goto delink_stage;
+
+	if (chip->polling_config) {
+		u8 val;
+
+		rtsx_read_config_byte(chip, 0, &val);
+	}
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_SS))
+		return;
+
+	rtsx_manage_ocp(chip);
+
+	rtsx_manage_sd_lock(chip);
+
+	rtsx_init_cards(chip);
+
+	rtsx_manage_ss(chip);
+
+	rtsx_manage_aspm(chip);
+
+	rtsx_manage_idle(chip);
+
+	switch (rtsx_get_stat(chip)) {
+	case RTSX_STAT_RUN:
+#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
+		rtsx_blink_led(chip);
+#endif
+		do_remaining_work(chip);
+		break;
+
+	case RTSX_STAT_IDLE:
+		if (chip->sd_io && !chip->sd_int)
+			try_to_switch_sdio_ctrl(chip);
+
+		rtsx_enable_aspm(chip);
+		break;
+
+	default:
+		break;
+	}
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+		rtsx_manage_2lun_mode(chip);
+	else
+		rtsx_manage_1lun_mode(chip);
+
+delink_stage:
+	rtsx_delink_stage(chip);
+}
+
+/**
+ * rtsx_stop_cmd - stop command transfer and DMA transfer
+ * @chip: Realtek's card reader chip
+ * @card: flash card type
+ *
+ * Stop command transfer and DMA transfer.
+ * This function is called in error handler.
+ */
+void rtsx_stop_cmd(struct rtsx_chip *chip, int card)
+{
+	int i;
+
+	for (i = 0; i <= 8; i++) {
+		int addr = RTSX_HCBAR + i * 4;
+		u32 reg;
+
+		reg = rtsx_readl(chip, addr);
+		dev_dbg(rtsx_dev(chip), "BAR (0x%02x): 0x%08x\n", addr, reg);
+	}
+	rtsx_writel(chip, RTSX_HCBCTLR, STOP_CMD);
+	rtsx_writel(chip, RTSX_HDBCTLR, STOP_DMA);
+
+	for (i = 0; i < 16; i++) {
+		u16 addr = 0xFE20 + (u16)i;
+		u8 val;
+
+		rtsx_read_register(chip, addr, &val);
+		dev_dbg(rtsx_dev(chip), "0x%04X: 0x%02x\n", addr, val);
+	}
+
+	rtsx_write_register(chip, DMACTL, 0x80, 0x80);
+	rtsx_write_register(chip, RBCTL, 0x80, 0x80);
+}
+
+#define MAX_RW_REG_CNT		1024
+
+int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data)
+{
+	int i;
+	u32 val = 3 << 30;
+
+	val |= (u32)(addr & 0x3FFF) << 16;
+	val |= (u32)mask << 8;
+	val |= (u32)data;
+
+	rtsx_writel(chip, RTSX_HAIMR, val);
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		val = rtsx_readl(chip, RTSX_HAIMR);
+		if ((val & BIT(31)) == 0) {
+			if (data != (u8)val)
+				return STATUS_FAIL;
+
+			return STATUS_SUCCESS;
+		}
+	}
+
+	return STATUS_TIMEDOUT;
+}
+
+int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
+{
+	u32 val = 2 << 30;
+	int i;
+
+	if (data)
+		*data = 0;
+
+	val |= (u32)(addr & 0x3FFF) << 16;
+
+	rtsx_writel(chip, RTSX_HAIMR, val);
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		val = rtsx_readl(chip, RTSX_HAIMR);
+		if ((val & BIT(31)) == 0)
+			break;
+	}
+
+	if (i >= MAX_RW_REG_CNT)
+		return STATUS_TIMEDOUT;
+
+	if (data)
+		*data = (u8)(val & 0xFF);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
+		      u32 val)
+{
+	int retval;
+	u8 mode = 0, tmp;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		if (mask & 0xFF) {
+			retval = rtsx_write_register(chip, CFGDATA0 + i,
+						     0xFF,
+						     (u8)(val & mask & 0xFF));
+			if (retval)
+				return retval;
+			mode |= (1 << i);
+		}
+		mask >>= 8;
+		val >>= 8;
+	}
+
+	if (mode) {
+		retval = rtsx_write_register(chip, CFGADDR0, 0xFF, (u8)addr);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CFGADDR1, 0xFF,
+					     (u8)(addr >> 8));
+		if (retval)
+			return retval;
+
+		retval = rtsx_write_register(chip, CFGRWCTL, 0xFF,
+					     0x80 | mode |
+					     ((func_no & 0x03) << 4));
+		if (retval)
+			return retval;
+
+		for (i = 0; i < MAX_RW_REG_CNT; i++) {
+			retval = rtsx_read_register(chip, CFGRWCTL, &tmp);
+			if (retval)
+				return retval;
+			if ((tmp & 0x80) == 0)
+				break;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val)
+{
+	int retval;
+	int i;
+	u8 tmp;
+	u32 data = 0;
+
+	retval = rtsx_write_register(chip, CFGADDR0, 0xFF, (u8)addr);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, CFGADDR1, 0xFF, (u8)(addr >> 8));
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, CFGRWCTL, 0xFF,
+				     0x80 | ((func_no & 0x03) << 4));
+	if (retval)
+		return retval;
+
+	for (i = 0; i < MAX_RW_REG_CNT; i++) {
+		retval = rtsx_read_register(chip, CFGRWCTL, &tmp);
+		if (retval)
+			return retval;
+		if ((tmp & 0x80) == 0)
+			break;
+	}
+
+	for (i = 0; i < 4; i++) {
+		retval = rtsx_read_register(chip, CFGDATA0 + i, &tmp);
+		if (retval)
+			return retval;
+		data |= (u32)tmp << (i * 8);
+	}
+
+	if (val)
+		*val = data;
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
+		       int len)
+{
+	u32 *data, *mask;
+	u16 offset = addr % 4;
+	u16 aligned_addr = addr - offset;
+	int dw_len, i, j;
+	int retval;
+	size_t size;
+
+	if (!buf)
+		return STATUS_NOMEM;
+
+	if ((len + offset) % 4)
+		dw_len = (len + offset) / 4 + 1;
+	else
+		dw_len = (len + offset) / 4;
+
+	dev_dbg(rtsx_dev(chip), "dw_len = %d\n", dw_len);
+
+	size = array_size(dw_len, 4);
+	data = vzalloc(size);
+	if (!data)
+		return STATUS_NOMEM;
+
+	mask = vzalloc(size);
+	if (!mask) {
+		vfree(data);
+		return STATUS_NOMEM;
+	}
+
+	j = 0;
+	for (i = 0; i < len; i++) {
+		mask[j] |= 0xFF << (offset * 8);
+		data[j] |= buf[i] << (offset * 8);
+		if (++offset == 4) {
+			j++;
+			offset = 0;
+		}
+	}
+
+	print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, mask, size);
+	print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, data, size);
+
+	for (i = 0; i < dw_len; i++) {
+		retval = rtsx_write_cfg_dw(chip, func, aligned_addr + i * 4,
+					   mask[i], data[i]);
+		if (retval != STATUS_SUCCESS) {
+			vfree(data);
+			vfree(mask);
+			return STATUS_FAIL;
+		}
+	}
+
+	vfree(data);
+	vfree(mask);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
+		      int len)
+{
+	u32 *data;
+	u16 offset = addr % 4;
+	u16 aligned_addr = addr - offset;
+	int dw_len, i, j;
+	int retval;
+
+	if ((len + offset) % 4)
+		dw_len = (len + offset) / 4 + 1;
+	else
+		dw_len = (len + offset) / 4;
+
+	dev_dbg(rtsx_dev(chip), "dw_len = %d\n", dw_len);
+
+	data = vmalloc(array_size(dw_len, 4));
+	if (!data)
+		return STATUS_NOMEM;
+
+	for (i = 0; i < dw_len; i++) {
+		retval = rtsx_read_cfg_dw(chip, func, aligned_addr + i * 4,
+					  data + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(data);
+			return STATUS_FAIL;
+		}
+	}
+
+	if (buf) {
+		j = 0;
+
+		for (i = 0; i < len; i++) {
+			buf[i] = (u8)(data[j] >> (offset * 8));
+			if (++offset == 4) {
+				j++;
+				offset = 0;
+			}
+		}
+	}
+
+	vfree(data);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val)
+{
+	int retval;
+	bool finished = false;
+	int i;
+	u8 tmp;
+
+	retval = rtsx_write_register(chip, PHYDATA0, 0xFF, (u8)val);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, PHYDATA1, 0xFF, (u8)(val >> 8));
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, PHYADDR, 0xFF, addr);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, PHYRWCTL, 0xFF, 0x81);
+	if (retval)
+		return retval;
+
+	for (i = 0; i < 100000; i++) {
+		retval = rtsx_read_register(chip, PHYRWCTL, &tmp);
+		if (retval)
+			return retval;
+		if (!(tmp & 0x80)) {
+			finished = true;
+			break;
+		}
+	}
+
+	if (!finished)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 *val)
+{
+	int retval;
+	bool finished = false;
+	int i;
+	u16 data = 0;
+	u8 tmp;
+
+	retval = rtsx_write_register(chip, PHYADDR, 0xFF, addr);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, PHYRWCTL, 0xFF, 0x80);
+	if (retval)
+		return retval;
+
+	for (i = 0; i < 100000; i++) {
+		retval = rtsx_read_register(chip, PHYRWCTL, &tmp);
+		if (retval)
+			return retval;
+		if (!(tmp & 0x80)) {
+			finished = true;
+			break;
+		}
+	}
+
+	if (!finished)
+		return STATUS_FAIL;
+
+	retval = rtsx_read_register(chip, PHYDATA0, &tmp);
+	if (retval)
+		return retval;
+	data = tmp;
+	retval = rtsx_read_register(chip, PHYDATA1, &tmp);
+	if (retval)
+		return retval;
+	data |= (u16)tmp << 8;
+
+	if (val)
+		*val = data;
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_read_efuse(struct rtsx_chip *chip, u8 addr, u8 *val)
+{
+	int retval;
+	int i;
+	u8 data = 0;
+
+	retval = rtsx_write_register(chip, EFUSE_CTRL, 0xFF, 0x80 | addr);
+	if (retval)
+		return retval;
+
+	for (i = 0; i < 100; i++) {
+		retval = rtsx_read_register(chip, EFUSE_CTRL, &data);
+		if (retval)
+			return retval;
+		if (!(data & 0x80))
+			break;
+		udelay(1);
+	}
+
+	if (data & 0x80)
+		return STATUS_TIMEDOUT;
+
+	retval = rtsx_read_register(chip, EFUSE_DATA, &data);
+	if (retval)
+		return retval;
+	if (val)
+		*val = data;
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_efuse(struct rtsx_chip *chip, u8 addr, u8 val)
+{
+	int retval;
+	int i, j;
+	u8 data = 0, tmp = 0xFF;
+
+	for (i = 0; i < 8; i++) {
+		if (val & (u8)(1 << i))
+			continue;
+
+		tmp &= (~(u8)(1 << i));
+		dev_dbg(rtsx_dev(chip), "Write 0x%x to 0x%x\n", tmp, addr);
+
+		retval = rtsx_write_register(chip, EFUSE_DATA, 0xFF, tmp);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, EFUSE_CTRL, 0xFF,
+					     0xA0 | addr);
+		if (retval)
+			return retval;
+
+		for (j = 0; j < 100; j++) {
+			retval = rtsx_read_register(chip, EFUSE_CTRL, &data);
+			if (retval)
+				return retval;
+			if (!(data & 0x80))
+				break;
+			wait_timeout(3);
+		}
+
+		if (data & 0x80)
+			return STATUS_TIMEDOUT;
+
+		wait_timeout(5);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_clr_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
+{
+	int retval;
+	u16 value;
+
+	retval = rtsx_read_phy_register(chip, reg, &value);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (value & (1 << bit)) {
+		value &= ~(1 << bit);
+		retval = rtsx_write_phy_register(chip, reg, value);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
+{
+	int retval;
+	u16 value;
+
+	retval = rtsx_read_phy_register(chip, reg, &value);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if ((value & (1 << bit)) == 0) {
+		value |= (1 << bit);
+		retval = rtsx_write_phy_register(chip, reg, value);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void rtsx_handle_pm_dstate(struct rtsx_chip *chip, u8 dstate)
+{
+	u32 ultmp;
+
+	dev_dbg(rtsx_dev(chip), "%04x set pm_dstate to %d\n",
+		chip->product_id, dstate);
+
+	if (CHK_SDIO_EXIST(chip)) {
+		u8 func_no;
+
+		if (CHECK_PID(chip, 0x5288))
+			func_no = 2;
+		else
+			func_no = 1;
+
+		rtsx_read_cfg_dw(chip, func_no, 0x84, &ultmp);
+		dev_dbg(rtsx_dev(chip), "pm_dstate of function %d: 0x%x\n",
+			(int)func_no, ultmp);
+		rtsx_write_cfg_dw(chip, func_no, 0x84, 0xFF, dstate);
+	}
+
+	rtsx_write_config_byte(chip, 0x44, dstate);
+	rtsx_write_config_byte(chip, 0x45, 0);
+}
+
+void rtsx_enter_L1(struct rtsx_chip *chip)
+{
+	rtsx_handle_pm_dstate(chip, 2);
+}
+
+void rtsx_exit_L1(struct rtsx_chip *chip)
+{
+	rtsx_write_config_byte(chip, 0x44, 0);
+	rtsx_write_config_byte(chip, 0x45, 0);
+}
+
+void rtsx_enter_ss(struct rtsx_chip *chip)
+{
+	dev_dbg(rtsx_dev(chip), "Enter Selective Suspend State!\n");
+
+	rtsx_write_register(chip, IRQSTAT0, LINK_RDY_INT, LINK_RDY_INT);
+
+	if (chip->power_down_in_ss) {
+		rtsx_power_off_card(chip);
+		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+	}
+
+	if (CHK_SDIO_EXIST(chip))
+		rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+				  0xC0, 0xFF00, 0x0100);
+
+	if (chip->auto_delink_en) {
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x01, 0x01);
+	} else {
+		if (!chip->phy_debug_mode) {
+			u32 tmp;
+
+			tmp = rtsx_readl(chip, RTSX_BIER);
+			tmp |= CARD_INT;
+			rtsx_writel(chip, RTSX_BIER, tmp);
+		}
+
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 0);
+	}
+
+	rtsx_enter_L1(chip);
+
+	RTSX_CLR_DELINK(chip);
+	rtsx_set_stat(chip, RTSX_STAT_SS);
+}
+
+void rtsx_exit_ss(struct rtsx_chip *chip)
+{
+	dev_dbg(rtsx_dev(chip), "Exit Selective Suspend State!\n");
+
+	rtsx_exit_L1(chip);
+
+	if (chip->power_down_in_ss) {
+		rtsx_force_power_on(chip, SSC_PDCTL | OC_PDCTL);
+		udelay(1000);
+	}
+
+	if (RTSX_TST_DELINK(chip)) {
+		chip->need_reinit = SD_CARD | MS_CARD | XD_CARD;
+		rtsx_reinit_cards(chip, 1);
+		RTSX_CLR_DELINK(chip);
+	} else if (chip->power_down_in_ss) {
+		chip->need_reinit = SD_CARD | MS_CARD | XD_CARD;
+		rtsx_reinit_cards(chip, 0);
+	}
+}
+
+int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
+{
+	u32 status, int_enable;
+	bool exit_ss = false;
+#ifdef SUPPORT_OCP
+	u32 ocp_int = 0;
+
+	ocp_int = OC_INT;
+#endif
+
+	if (chip->ss_en) {
+		chip->ss_counter = 0;
+		if (rtsx_get_stat(chip) == RTSX_STAT_SS) {
+			exit_ss = true;
+			rtsx_exit_L1(chip);
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+		}
+	}
+
+	int_enable = rtsx_readl(chip, RTSX_BIER);
+	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
+
+	if (((chip->int_reg & int_enable) == 0) ||
+	    chip->int_reg == 0xFFFFFFFF)
+		return STATUS_FAIL;
+
+	status = chip->int_reg &= (int_enable | 0x7FFFFF);
+
+	if (status & CARD_INT) {
+		chip->auto_delink_cnt = 0;
+
+		if (status & SD_INT) {
+			if (status & SD_EXIST) {
+				set_bit(SD_NR, &chip->need_reset);
+			} else {
+				set_bit(SD_NR, &chip->need_release);
+				chip->sd_reset_counter = 0;
+				chip->sd_show_cnt = 0;
+				clear_bit(SD_NR, &chip->need_reset);
+			}
+		} else {
+			/*
+			 * If multi-luns, it's possible that
+			 * when plugging/unplugging one card
+			 * there is another card which still
+			 * exists in the slot. In this case,
+			 * all existed cards should be reset.
+			 */
+			if (exit_ss && (status & SD_EXIST))
+				set_bit(SD_NR, &chip->need_reinit);
+		}
+		if (!CHECK_PID(chip, 0x5288) || CHECK_BARO_PKG(chip, QFN)) {
+			if (status & XD_INT) {
+				if (status & XD_EXIST) {
+					set_bit(XD_NR, &chip->need_reset);
+				} else {
+					set_bit(XD_NR, &chip->need_release);
+					chip->xd_reset_counter = 0;
+					chip->xd_show_cnt = 0;
+					clear_bit(XD_NR, &chip->need_reset);
+				}
+			} else {
+				if (exit_ss && (status & XD_EXIST))
+					set_bit(XD_NR, &chip->need_reinit);
+			}
+		}
+		if (status & MS_INT) {
+			if (status & MS_EXIST) {
+				set_bit(MS_NR, &chip->need_reset);
+			} else {
+				set_bit(MS_NR, &chip->need_release);
+				chip->ms_reset_counter = 0;
+				chip->ms_show_cnt = 0;
+				clear_bit(MS_NR, &chip->need_reset);
+			}
+		} else {
+			if (exit_ss && (status & MS_EXIST))
+				set_bit(MS_NR, &chip->need_reinit);
+		}
+	}
+
+#ifdef SUPPORT_OCP
+	chip->ocp_int = ocp_int & status;
+#endif
+
+	if (chip->sd_io && (chip->int_reg & DATA_DONE_INT))
+		chip->int_reg &= ~(u32)DATA_DONE_INT;
+
+	return STATUS_SUCCESS;
+}
+
+void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat)
+{
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "%s, pm_stat = %d\n", __func__, pm_stat);
+
+	rtsx_set_stat(chip, RTSX_STAT_SUSPEND);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS)
+		return;
+
+	rtsx_release_cards(chip);
+	rtsx_disable_bus_int(chip);
+	turn_off_led(chip, LED_GPIO);
+
+#ifdef HW_AUTO_SWITCH_SD_BUS
+	if (chip->sd_io) {
+		chip->sdio_in_charge = 1;
+		if (CHECK_PID(chip, 0x5208)) {
+			rtsx_write_register(chip, TLPTISTAT, 0x08, 0x08);
+			/* Enable sdio_bus_auto_switch */
+			rtsx_write_register(chip, 0xFE70, 0x80, 0x80);
+		} else if (CHECK_PID(chip, 0x5288)) {
+			rtsx_write_register(chip, TLPTISTAT, 0x08, 0x08);
+			/* Enable sdio_bus_auto_switch */
+			rtsx_write_register(chip, 0xFE5A, 0x08, 0x08);
+		}
+	}
+#endif
+
+	if (CHECK_PID(chip, 0x5208) && chip->ic_version >= IC_VER_D) {
+		/* u_force_clkreq_0 */
+		rtsx_write_register(chip, PETXCFG, 0x08, 0x08);
+	}
+
+	if (pm_stat == PM_S1) {
+		dev_dbg(rtsx_dev(chip), "Host enter S1\n");
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
+				    HOST_ENTER_S1);
+	} else if (pm_stat == PM_S3) {
+		if (chip->s3_pwr_off_delay > 0)
+			wait_timeout(chip->s3_pwr_off_delay);
+
+		dev_dbg(rtsx_dev(chip), "Host enter S3\n");
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
+				    HOST_ENTER_S3);
+	}
+
+	if (chip->do_delink_before_power_down && chip->auto_delink_en)
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 2);
+
+	rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+
+	chip->cur_clk = 0;
+	chip->cur_card = 0;
+	chip->card_exist = 0;
+}
+
+void rtsx_enable_aspm(struct rtsx_chip *chip)
+{
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm && !chip->aspm_enabled) {
+		dev_dbg(rtsx_dev(chip), "Try to enable ASPM\n");
+		chip->aspm_enabled = 1;
+
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_write_phy_register(chip, 0x07, 0);
+		if (CHECK_PID(chip, 0x5208)) {
+			rtsx_write_register(chip, ASPM_FORCE_CTL, 0xF3,
+					    0x30 | chip->aspm_level[0]);
+		} else {
+			rtsx_write_config_byte(chip, LCTLR,
+					       chip->aspm_l0s_l1_en);
+		}
+
+		if (CHK_SDIO_EXIST(chip)) {
+			u16 val = chip->aspm_l0s_l1_en | 0x0100;
+
+			rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+					  0xC0, 0xFFF, val);
+		}
+	}
+}
+
+void rtsx_disable_aspm(struct rtsx_chip *chip)
+{
+	if (CHECK_PID(chip, 0x5208))
+		rtsx_monitor_aspm_config(chip);
+
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm && chip->aspm_enabled) {
+		dev_dbg(rtsx_dev(chip), "Try to disable ASPM\n");
+		chip->aspm_enabled = 0;
+
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_write_phy_register(chip, 0x07, 0x0129);
+		if (CHECK_PID(chip, 0x5208))
+			rtsx_write_register(chip, ASPM_FORCE_CTL,
+					    0xF3, 0x30);
+		else
+			rtsx_write_config_byte(chip, LCTLR, 0x00);
+
+		wait_timeout(1);
+	}
+}
+
+int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
+{
+	int retval;
+	int i, j;
+	u16 reg_addr;
+	u8 *ptr;
+
+	if (!buf)
+		return STATUS_ERROR;
+
+	ptr = buf;
+	reg_addr = PPBUF_BASE2;
+	for (i = 0; i < buf_len / 256; i++) {
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < 256; j++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0)
+			return STATUS_FAIL;
+
+		memcpy(ptr, rtsx_get_cmd_data(chip), 256);
+		ptr += 256;
+	}
+
+	if (buf_len % 256) {
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < buf_len % 256; j++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0)
+			return STATUS_FAIL;
+	}
+
+	memcpy(ptr, rtsx_get_cmd_data(chip), buf_len % 256);
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
+{
+	int retval;
+	int i, j;
+	u16 reg_addr;
+	u8 *ptr;
+
+	if (!buf)
+		return STATUS_ERROR;
+
+	ptr = buf;
+	reg_addr = PPBUF_BASE2;
+	for (i = 0; i < buf_len / 256; i++) {
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < 256; j++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
+				     *ptr);
+			ptr++;
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0)
+			return STATUS_FAIL;
+	}
+
+	if (buf_len % 256) {
+		rtsx_init_cmd(chip);
+
+		for (j = 0; j < buf_len % 256; j++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
+				     *ptr);
+			ptr++;
+		}
+
+		retval = rtsx_send_cmd(chip, 0, 250);
+		if (retval < 0)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_check_chip_exist(struct rtsx_chip *chip)
+{
+	if (rtsx_readl(chip, 0) == 0xFFFFFFFF)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl)
+{
+	int retval;
+	u8 mask = 0;
+
+	if (ctl & SSC_PDCTL)
+		mask |= SSC_POWER_DOWN;
+
+#ifdef SUPPORT_OCP
+	if (ctl & OC_PDCTL) {
+		mask |= SD_OC_POWER_DOWN;
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+			mask |= MS_OC_POWER_DOWN;
+	}
+#endif
+
+	if (mask) {
+		retval = rtsx_write_register(chip, FPDCTL, mask, 0);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (CHECK_PID(chip, 0x5288))
+			wait_timeout(200);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int rtsx_force_power_down(struct rtsx_chip *chip, u8 ctl)
+{
+	int retval;
+	u8 mask = 0, val = 0;
+
+	if (ctl & SSC_PDCTL)
+		mask |= SSC_POWER_DOWN;
+
+#ifdef SUPPORT_OCP
+	if (ctl & OC_PDCTL) {
+		mask |= SD_OC_POWER_DOWN;
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+			mask |= MS_OC_POWER_DOWN;
+	}
+#endif
+
+	if (mask) {
+		val = mask;
+		retval = rtsx_write_register(chip, FPDCTL, mask, val);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5208/rtsx_chip.h b/drivers/staging/rts5208/rtsx_chip.h
new file mode 100644
index 000000000..bac65784d
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_chip.h
@@ -0,0 +1,987 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_CHIP_H
+#define __REALTEK_RTSX_CHIP_H
+
+#include "rtsx.h"
+
+#define SUPPORT_CPRM
+#define SUPPORT_OCP
+#define SUPPORT_SDIO_ASPM
+#define SUPPORT_MAGIC_GATE
+#define SUPPORT_MSXC
+#define SUPPORT_SD_LOCK
+/* Hardware switch bus_ctl and cd_ctl automatically */
+#define HW_AUTO_SWITCH_SD_BUS
+/* Enable hardware interrupt write clear */
+#define HW_INT_WRITE_CLR
+/* #define LED_AUTO_BLINK */
+/* #define DISABLE_CARD_INT */
+
+#ifdef SUPPORT_MAGIC_GATE
+	/* Using NORMAL_WRITE instead of AUTO_WRITE to set ICV */
+	#define MG_SET_ICV_SLOW
+	/* HW may miss ERR/CMDNK signal when sampling INT status. */
+	#define MS_SAMPLE_INT_ERR
+	/*
+	 * HW DO NOT support Wait_INT function
+	 * during READ_BYTES transfer mode
+	 */
+	#define READ_BYTES_WAIT_INT
+#endif
+
+#ifdef SUPPORT_MSXC
+#define XC_POWERCLASS
+#define SUPPORT_PCGL_1P18
+#endif
+
+#ifndef LED_AUTO_BLINK
+#define REGULAR_BLINK
+#endif
+
+#define LED_BLINK_SPEED		5
+#define LED_TOGGLE_INTERVAL	6
+#define	GPIO_TOGGLE_THRESHOLD   1024
+#define LED_GPIO		0
+
+#define POLLING_INTERVAL	30
+
+#define TRACE_ITEM_CNT		64
+
+#ifndef STATUS_SUCCESS
+#define STATUS_SUCCESS		0
+#endif
+#ifndef STATUS_FAIL
+#define STATUS_FAIL		1
+#endif
+#ifndef STATUS_TIMEDOUT
+#define STATUS_TIMEDOUT		2
+#endif
+#ifndef STATUS_NOMEM
+#define STATUS_NOMEM		3
+#endif
+#ifndef STATUS_READ_FAIL
+#define STATUS_READ_FAIL	4
+#endif
+#ifndef STATUS_WRITE_FAIL
+#define STATUS_WRITE_FAIL	5
+#endif
+#ifndef STATUS_ERROR
+#define STATUS_ERROR		10
+#endif
+
+#define PM_S1			1
+#define PM_S3			3
+
+/*
+ * Transport return codes
+ */
+
+#define TRANSPORT_GOOD		0   /* Transport good, command good	   */
+#define TRANSPORT_FAILED	1   /* Transport good, command failed   */
+#define TRANSPORT_NO_SENSE	2  /* Command failed, no auto-sense    */
+#define TRANSPORT_ERROR		3   /* Transport bad (i.e. device dead) */
+
+/*
+ * Start-Stop-Unit
+ */
+#define STOP_MEDIUM			0x00    /* access disable         */
+#define MAKE_MEDIUM_READY		0x01    /* access enable          */
+#define UNLOAD_MEDIUM			0x02    /* unload                 */
+#define LOAD_MEDIUM			0x03    /* load                   */
+
+/*
+ * STANDARD_INQUIRY
+ */
+#define QULIFIRE                0x00
+#define AENC_FNC                0x00
+#define TRML_IOP                0x00
+#define REL_ADR                 0x00
+#define WBUS_32                 0x00
+#define WBUS_16                 0x00
+#define SYNC                    0x00
+#define LINKED                  0x00
+#define CMD_QUE                 0x00
+#define SFT_RE                  0x00
+
+#define VEN_ID_LEN              8               /* Vendor ID Length         */
+#define PRDCT_ID_LEN            16              /* Product ID Length        */
+#define PRDCT_REV_LEN           4               /* Product LOT Length       */
+
+/* Dynamic flag definitions: used in set_bit() etc. */
+/* 0x00040000 transfer is active */
+#define RTSX_FLIDX_TRANS_ACTIVE		18
+/* 0x00100000 abort is in progress */
+#define RTSX_FLIDX_ABORTING		20
+/* 0x00200000 disconnect in progress */
+#define RTSX_FLIDX_DISCONNECTING	21
+
+#define ABORTING_OR_DISCONNECTING	((1UL << US_FLIDX_ABORTING) | \
+					 (1UL << US_FLIDX_DISCONNECTING))
+
+/* 0x00400000 device reset in progress */
+#define RTSX_FLIDX_RESETTING		22
+/* 0x00800000 SCSI midlayer timed out  */
+#define RTSX_FLIDX_TIMED_OUT		23
+#define DRCT_ACCESS_DEV         0x00    /* Direct Access Device      */
+#define RMB_DISC                0x80    /* The Device is Removable   */
+#define ANSI_SCSI2              0x02    /* Based on ANSI-SCSI2       */
+
+#define SCSI                    0x00    /* Interface ID              */
+
+#define	WRITE_PROTECTED_MEDIA 0x07
+
+/*---- sense key ----*/
+#define ILI                     0x20    /* ILI bit is on                    */
+
+#define NO_SENSE                0x00    /* not exist sense key              */
+#define RECOVER_ERR             0x01    /* Target/Logical unit is recoverd  */
+#define NOT_READY               0x02    /* Logical unit is not ready        */
+#define MEDIA_ERR               0x03    /* medium/data error                */
+#define HARDWARE_ERR            0x04    /* hardware error                   */
+#define ILGAL_REQ               0x05    /* CDB/parameter/identify msg error */
+#define UNIT_ATTENTION          0x06    /* unit attention condition occur   */
+#define DAT_PRTCT               0x07    /* read/write is desable            */
+#define BLNC_CHK                0x08    /* find blank/DOF in read           */
+					/* write to unblank area            */
+#define CPY_ABRT                0x0a    /* Copy/Compare/Copy&Verify illegal */
+#define ABRT_CMD                0x0b    /* Target make the command in error */
+#define EQUAL                   0x0c    /* Search Data end with Equal       */
+#define VLM_OVRFLW              0x0d    /* Some data are left in buffer     */
+#define MISCMP                  0x0e    /* find inequality                  */
+
+#define READ_ERR                -1
+#define WRITE_ERR               -2
+
+#define	FIRST_RESET		0x01
+#define	USED_EXIST		0x02
+
+/*
+ * SENSE_DATA
+ */
+/*---- valid ----*/
+#define SENSE_VALID             0x80    /* Sense data is valid as SCSI2     */
+#define SENSE_INVALID           0x00    /* Sense data is invalid as SCSI2   */
+
+/*---- error code ----*/
+#define CUR_ERR                 0x70    /* current error                    */
+#define DEF_ERR                 0x71    /* specific command error           */
+
+/*---- sense key Information ----*/
+#define SNSKEYINFO_LEN          3       /* length of sense key information   */
+
+#define SKSV                    0x80
+#define CDB_ILLEGAL             0x40
+#define DAT_ILLEGAL             0x00
+#define BPV                     0x08
+#define BIT_ILLEGAL0            0       /* bit0 is illegal                  */
+#define BIT_ILLEGAL1            1       /* bit1 is illegal                  */
+#define BIT_ILLEGAL2            2       /* bit2 is illegal                  */
+#define BIT_ILLEGAL3            3       /* bit3 is illegal                  */
+#define BIT_ILLEGAL4            4       /* bit4 is illegal                  */
+#define BIT_ILLEGAL5            5       /* bit5 is illegal                  */
+#define BIT_ILLEGAL6            6       /* bit6 is illegal                  */
+#define BIT_ILLEGAL7            7       /* bit7 is illegal                  */
+
+/*---- ASC ----*/
+#define ASC_NO_INFO             0x00
+#define ASC_MISCMP              0x1d
+#define ASC_INVLD_CDB           0x24
+#define ASC_INVLD_PARA          0x26
+#define ASC_LU_NOT_READY	0x04
+#define ASC_WRITE_ERR           0x0c
+#define ASC_READ_ERR            0x11
+#define ASC_LOAD_EJCT_ERR       0x53
+#define	ASC_MEDIA_NOT_PRESENT	0x3A
+#define	ASC_MEDIA_CHANGED	0x28
+#define	ASC_MEDIA_IN_PROCESS	0x04
+#define	ASC_WRITE_PROTECT	0x27
+#define ASC_LUN_NOT_SUPPORTED	0x25
+
+/*---- ASQC ----*/
+#define ASCQ_NO_INFO            0x00
+#define	ASCQ_MEDIA_IN_PROCESS	0x01
+#define ASCQ_MISCMP             0x00
+#define ASCQ_INVLD_CDB          0x00
+#define ASCQ_INVLD_PARA         0x02
+#define ASCQ_LU_NOT_READY	0x02
+#define ASCQ_WRITE_ERR          0x02
+#define ASCQ_READ_ERR           0x00
+#define ASCQ_LOAD_EJCT_ERR      0x00
+#define	ASCQ_WRITE_PROTECT	0x00
+
+struct sense_data_t {
+	unsigned char   err_code;	/* error code */
+	/* bit7 : valid */
+	/*   (1 : SCSI2) */
+	/*   (0 : Vendor * specific) */
+	/* bit6-0 : error * code */
+	/*  (0x70 : current * error) */
+	/*  (0x71 : specific command error) */
+	unsigned char   seg_no;		/* segment No.                      */
+	unsigned char   sense_key;	/* byte5 : ILI                      */
+	/* bit3-0 : sense key              */
+	unsigned char   info[4];	/* information                       */
+	unsigned char   ad_sense_len;	/* additional sense data length     */
+	unsigned char   cmd_info[4];	/* command specific information      */
+	unsigned char   asc;		/* ASC                              */
+	unsigned char   ascq;		/* ASCQ                             */
+	unsigned char   rfu;		/* FRU                              */
+	unsigned char   sns_key_info[3];/* sense key specific information    */
+};
+
+/* PCI Operation Register Address */
+#define RTSX_HCBAR		0x00
+#define RTSX_HCBCTLR		0x04
+#define RTSX_HDBAR		0x08
+#define RTSX_HDBCTLR		0x0C
+#define RTSX_HAIMR		0x10
+#define RTSX_BIPR		0x14
+#define RTSX_BIER		0x18
+
+/* Host command buffer control register */
+#define STOP_CMD		(0x01 << 28)
+
+/* Host data buffer control register */
+#define SDMA_MODE		0x00
+#define ADMA_MODE		(0x02 << 26)
+#define STOP_DMA		(0x01 << 28)
+#define TRIG_DMA		(0x01 << 31)
+
+/* Bus interrupt pending register */
+#define CMD_DONE_INT		BIT(31)
+#define DATA_DONE_INT		BIT(30)
+#define TRANS_OK_INT		BIT(29)
+#define TRANS_FAIL_INT		BIT(28)
+#define XD_INT			BIT(27)
+#define MS_INT			BIT(26)
+#define SD_INT			BIT(25)
+#define GPIO0_INT		BIT(24)
+#define OC_INT			BIT(23)
+#define SD_WRITE_PROTECT	BIT(19)
+#define XD_EXIST		BIT(18)
+#define MS_EXIST		BIT(17)
+#define SD_EXIST		BIT(16)
+#define DELINK_INT		GPIO0_INT
+#define MS_OC_INT		BIT(23)
+#define SD_OC_INT		BIT(22)
+
+#define CARD_INT		(XD_INT | MS_INT | SD_INT)
+#define NEED_COMPLETE_INT	(DATA_DONE_INT | TRANS_OK_INT | TRANS_FAIL_INT)
+#define RTSX_INT		(CMD_DONE_INT | NEED_COMPLETE_INT | CARD_INT | \
+				 GPIO0_INT | OC_INT)
+
+#define CARD_EXIST		(XD_EXIST | MS_EXIST | SD_EXIST)
+
+/* Bus interrupt enable register */
+#define CMD_DONE_INT_EN		BIT(31)
+#define DATA_DONE_INT_EN	BIT(30)
+#define TRANS_OK_INT_EN		BIT(29)
+#define TRANS_FAIL_INT_EN	BIT(28)
+#define XD_INT_EN		BIT(27)
+#define MS_INT_EN		BIT(26)
+#define SD_INT_EN		BIT(25)
+#define GPIO0_INT_EN		BIT(24)
+#define OC_INT_EN		BIT(23)
+#define DELINK_INT_EN		GPIO0_INT_EN
+#define MS_OC_INT_EN		BIT(23)
+#define SD_OC_INT_EN		BIT(22)
+
+#define READ_REG_CMD		0
+#define WRITE_REG_CMD		1
+#define CHECK_REG_CMD		2
+
+#define HOST_TO_DEVICE		0
+#define DEVICE_TO_HOST		1
+
+#define RTSX_RESV_BUF_LEN	4096
+#define HOST_CMDS_BUF_LEN	1024
+#define HOST_SG_TBL_BUF_LEN	(RTSX_RESV_BUF_LEN - HOST_CMDS_BUF_LEN)
+
+#define SD_NR		2
+#define MS_NR		3
+#define XD_NR		4
+#define SPI_NR		7
+#define SD_CARD		BIT(SD_NR)
+#define MS_CARD		BIT(MS_NR)
+#define XD_CARD		BIT(XD_NR)
+#define SPI_CARD	BIT(SPI_NR)
+
+#define MAX_ALLOWED_LUN_CNT	8
+
+#define XD_FREE_TABLE_CNT	1200
+#define MS_FREE_TABLE_CNT	512
+
+/* Bit Operation */
+#define SET_BIT(data, idx)	((data) |= 1 << (idx))
+#define CLR_BIT(data, idx)	((data) &= ~(1 << (idx)))
+#define CHK_BIT(data, idx)	((data) & (1 << (idx)))
+
+/* SG descriptor */
+#define RTSX_SG_INT		0x04
+#define RTSX_SG_END		0x02
+#define RTSX_SG_VALID		0x01
+
+#define RTSX_SG_NO_OP		0x00
+#define RTSX_SG_TRANS_DATA	(0x02 << 4)
+#define RTSX_SG_LINK_DESC	(0x03 << 4)
+
+struct rtsx_chip;
+
+typedef int (*card_rw_func)(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+			u32 sec_addr, u16 sec_cnt);
+
+/* Supported Clock */
+enum card_clock	{CLK_20 = 1, CLK_30, CLK_40, CLK_50, CLK_60,
+		 CLK_80, CLK_100, CLK_120, CLK_150, CLK_200};
+
+enum RTSX_STAT	{RTSX_STAT_INIT, RTSX_STAT_IDLE, RTSX_STAT_RUN, RTSX_STAT_SS,
+		 RTSX_STAT_DELINK, RTSX_STAT_SUSPEND,
+		 RTSX_STAT_ABORT, RTSX_STAT_DISCONNECT};
+enum IC_VER	{IC_VER_AB, IC_VER_C = 2, IC_VER_D = 3};
+
+#define MAX_RESET_CNT		3
+
+/* For MS Card */
+#define MAX_DEFECTIVE_BLOCK     10
+
+struct zone_entry {
+	u16 *l2p_table;
+	u16 *free_table;
+	u16 defect_list[MAX_DEFECTIVE_BLOCK];  /* For MS card only */
+	int set_index;
+	int get_index;
+	int unused_blk_cnt;
+	int disable_count;
+	/* To indicate whether the L2P table of this zone has been built. */
+	int build_flag;
+};
+
+#define TYPE_SD			0x0000
+#define TYPE_MMC		0x0001
+
+/* TYPE_SD */
+#define SD_HS			0x0100
+#define SD_SDR50		0x0200
+#define SD_DDR50		0x0400
+#define SD_SDR104		0x0800
+#define SD_HCXC			0x1000
+
+/* TYPE_MMC */
+#define MMC_26M			0x0100
+#define MMC_52M			0x0200
+#define MMC_4BIT		0x0400
+#define MMC_8BIT		0x0800
+#define MMC_SECTOR_MODE		0x1000
+#define MMC_DDR52		0x2000
+
+/* SD card */
+#define CHK_SD(sd_card)			(((sd_card)->sd_type & 0xFF) == TYPE_SD)
+#define CHK_SD_HS(sd_card)		(CHK_SD(sd_card) && \
+					 ((sd_card)->sd_type & SD_HS))
+#define CHK_SD_SDR50(sd_card)		(CHK_SD(sd_card) && \
+					 ((sd_card)->sd_type & SD_SDR50))
+#define CHK_SD_DDR50(sd_card)		(CHK_SD(sd_card) && \
+					 ((sd_card)->sd_type & SD_DDR50))
+#define CHK_SD_SDR104(sd_card)		(CHK_SD(sd_card) && \
+					 ((sd_card)->sd_type & SD_SDR104))
+#define CHK_SD_HCXC(sd_card)		(CHK_SD(sd_card) && \
+					 ((sd_card)->sd_type & SD_HCXC))
+#define CHK_SD_HC(sd_card)		(CHK_SD_HCXC(sd_card) && \
+					 ((sd_card)->capacity <= 0x4000000))
+#define CHK_SD_XC(sd_card)		(CHK_SD_HCXC(sd_card) && \
+					 ((sd_card)->capacity > 0x4000000))
+#define CHK_SD30_SPEED(sd_card)		(CHK_SD_SDR50(sd_card) || \
+					 CHK_SD_DDR50(sd_card) || \
+					 CHK_SD_SDR104(sd_card))
+
+#define SET_SD(sd_card)			((sd_card)->sd_type = TYPE_SD)
+#define SET_SD_HS(sd_card)		((sd_card)->sd_type |= SD_HS)
+#define SET_SD_SDR50(sd_card)		((sd_card)->sd_type |= SD_SDR50)
+#define SET_SD_DDR50(sd_card)		((sd_card)->sd_type |= SD_DDR50)
+#define SET_SD_SDR104(sd_card)		((sd_card)->sd_type |= SD_SDR104)
+#define SET_SD_HCXC(sd_card)		((sd_card)->sd_type |= SD_HCXC)
+
+#define CLR_SD_HS(sd_card)		((sd_card)->sd_type &= ~SD_HS)
+#define CLR_SD_SDR50(sd_card)		((sd_card)->sd_type &= ~SD_SDR50)
+#define CLR_SD_DDR50(sd_card)		((sd_card)->sd_type &= ~SD_DDR50)
+#define CLR_SD_SDR104(sd_card)		((sd_card)->sd_type &= ~SD_SDR104)
+#define CLR_SD_HCXC(sd_card)		((sd_card)->sd_type &= ~SD_HCXC)
+
+/* MMC card */
+#define CHK_MMC(sd_card)		(((sd_card)->sd_type & 0xFF) == \
+					 TYPE_MMC)
+#define CHK_MMC_26M(sd_card)		(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_26M))
+#define CHK_MMC_52M(sd_card)		(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_52M))
+#define CHK_MMC_4BIT(sd_card)		(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_4BIT))
+#define CHK_MMC_8BIT(sd_card)		(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_8BIT))
+#define CHK_MMC_SECTOR_MODE(sd_card)	(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_SECTOR_MODE))
+#define CHK_MMC_DDR52(sd_card)		(CHK_MMC(sd_card) && \
+					 ((sd_card)->sd_type & MMC_DDR52))
+
+#define SET_MMC(sd_card)		((sd_card)->sd_type = TYPE_MMC)
+#define SET_MMC_26M(sd_card)		((sd_card)->sd_type |= MMC_26M)
+#define SET_MMC_52M(sd_card)		((sd_card)->sd_type |= MMC_52M)
+#define SET_MMC_4BIT(sd_card)		((sd_card)->sd_type |= MMC_4BIT)
+#define SET_MMC_8BIT(sd_card)		((sd_card)->sd_type |= MMC_8BIT)
+#define SET_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type |= MMC_SECTOR_MODE)
+#define SET_MMC_DDR52(sd_card)		((sd_card)->sd_type |= MMC_DDR52)
+
+#define CLR_MMC_26M(sd_card)		((sd_card)->sd_type &= ~MMC_26M)
+#define CLR_MMC_52M(sd_card)		((sd_card)->sd_type &= ~MMC_52M)
+#define CLR_MMC_4BIT(sd_card)		((sd_card)->sd_type &= ~MMC_4BIT)
+#define CLR_MMC_8BIT(sd_card)		((sd_card)->sd_type &= ~MMC_8BIT)
+#define CLR_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type &= ~MMC_SECTOR_MODE)
+#define CLR_MMC_DDR52(sd_card)		((sd_card)->sd_type &= ~MMC_DDR52)
+
+#define CHK_MMC_HS(sd_card)		(CHK_MMC_52M(sd_card) && \
+					 CHK_MMC_26M(sd_card))
+#define CLR_MMC_HS(sd_card)			\
+do {						\
+	CLR_MMC_DDR52(sd_card);			\
+	CLR_MMC_52M(sd_card);			\
+	CLR_MMC_26M(sd_card);			\
+} while (0)
+
+#define SD_SUPPORT_CLASS_TEN		0x01
+#define SD_SUPPORT_1V8			0x02
+
+#define SD_SET_CLASS_TEN(sd_card)	((sd_card)->sd_setting |= \
+					 SD_SUPPORT_CLASS_TEN)
+#define SD_CHK_CLASS_TEN(sd_card)	((sd_card)->sd_setting & \
+					 SD_SUPPORT_CLASS_TEN)
+#define SD_CLR_CLASS_TEN(sd_card)	((sd_card)->sd_setting &= \
+					 ~SD_SUPPORT_CLASS_TEN)
+#define SD_SET_1V8(sd_card)		((sd_card)->sd_setting |= \
+					 SD_SUPPORT_1V8)
+#define SD_CHK_1V8(sd_card)		((sd_card)->sd_setting & \
+					 SD_SUPPORT_1V8)
+#define SD_CLR_1V8(sd_card)		((sd_card)->sd_setting &= \
+					 ~SD_SUPPORT_1V8)
+
+struct sd_info {
+	u16 sd_type;
+	u8 err_code;
+	u8 sd_data_buf_ready;
+	u32 sd_addr;
+	u32 capacity;
+
+	u8 raw_csd[16];
+	u8 raw_scr[8];
+
+	/* Sequential RW */
+	int seq_mode;
+	enum dma_data_direction pre_dir;
+	u32 pre_sec_addr;
+	u16 pre_sec_cnt;
+
+	int cleanup_counter;
+
+	int sd_clock;
+
+	int mmc_dont_switch_bus;
+
+#ifdef SUPPORT_CPRM
+	int sd_pass_thru_en;
+	int pre_cmd_err;
+	u8 last_rsp_type;
+	u8 rsp[17];
+#endif
+
+	u8 func_group1_mask;
+	u8 func_group2_mask;
+	u8 func_group3_mask;
+	u8 func_group4_mask;
+
+	u8 sd_switch_fail;
+	u8 sd_read_phase;
+
+#ifdef SUPPORT_SD_LOCK
+	u8 sd_lock_status;
+	u8 sd_erase_status;
+	u8 sd_lock_notify;
+#endif
+	int need_retune;
+};
+
+struct xd_delay_write_tag {
+	u32 old_phyblock;
+	u32 new_phyblock;
+	u32 logblock;
+	u8 pageoff;
+	u8 delay_write_flag;
+};
+
+struct xd_info {
+	u8 maker_code;
+	u8 device_code;
+	u8 block_shift;
+	u8 page_off;
+	u8 addr_cycle;
+	u16 cis_block;
+	u8 multi_flag;
+	u8 err_code;
+	u32 capacity;
+
+	struct zone_entry *zone;
+	int zone_cnt;
+
+	struct xd_delay_write_tag delay_write;
+	int cleanup_counter;
+
+	int xd_clock;
+};
+
+#define MODE_512_SEQ		0x01
+#define MODE_2K_SEQ		0x02
+
+#define TYPE_MS			0x0000
+#define TYPE_MSPRO		0x0001
+
+#define MS_4BIT			0x0100
+#define MS_8BIT			0x0200
+#define MS_HG			0x0400
+#define MS_XC			0x0800
+
+#define HG8BIT			(MS_HG | MS_8BIT)
+
+#define CHK_MSPRO(ms_card)	(((ms_card)->ms_type & 0xFF) == TYPE_MSPRO)
+#define CHK_HG8BIT(ms_card)	(CHK_MSPRO(ms_card) && \
+				 (((ms_card)->ms_type & HG8BIT) == HG8BIT))
+#define CHK_MSXC(ms_card)	(CHK_MSPRO(ms_card) && \
+				 ((ms_card)->ms_type & MS_XC))
+#define CHK_MSHG(ms_card)	(CHK_MSPRO(ms_card) && \
+				 ((ms_card)->ms_type & MS_HG))
+
+#define CHK_MS8BIT(ms_card)	(((ms_card)->ms_type & MS_8BIT))
+#define CHK_MS4BIT(ms_card)	(((ms_card)->ms_type & MS_4BIT))
+
+struct ms_delay_write_tag {
+	u16 old_phyblock;
+	u16 new_phyblock;
+	u16 logblock;
+	u8 pageoff;
+	u8 delay_write_flag;
+};
+
+struct ms_info {
+	u16 ms_type;
+	u8 block_shift;
+	u8 page_off;
+	u16 total_block;
+	u16 boot_block;
+	u32 capacity;
+
+	u8 check_ms_flow;
+	u8 switch_8bit_fail;
+	u8 err_code;
+
+	struct zone_entry *segment;
+	int segment_cnt;
+
+	int pro_under_formatting;
+	int format_status;
+	u16 progress;
+	u8 raw_sys_info[96];
+#ifdef SUPPORT_PCGL_1P18
+	u8 raw_model_name[48];
+#endif
+
+	u8 multi_flag;
+
+	/* Sequential RW */
+	u8 seq_mode;
+	enum dma_data_direction pre_dir;
+	u32 pre_sec_addr;
+	u16 pre_sec_cnt;
+	u32 total_sec_cnt;
+
+	struct ms_delay_write_tag delay_write;
+
+	int cleanup_counter;
+
+	int ms_clock;
+
+#ifdef SUPPORT_MAGIC_GATE
+	u8 magic_gate_id[16];
+	u8 mg_entry_num;
+	int mg_auth;    /* flag to indicate authentication process */
+#endif
+};
+
+struct spi_info {
+	u8 use_clk;
+	u8 write_en;
+	u16 clk_div;
+	u8 err_code;
+
+	int spi_clock;
+};
+
+/************/
+/* LUN mode */
+/************/
+/* Single LUN, support xD/SD/MS */
+#define DEFAULT_SINGLE		0
+/* 2 LUN mode, support SD/MS */
+#define SD_MS_2LUN		1
+/* Single LUN, but only support SD/MS, for Barossa LQFP */
+#define SD_MS_1LUN		2
+
+#define LAST_LUN_MODE		2
+
+/* Barossa package */
+#define QFN		0
+#define LQFP		1
+
+/******************/
+/* sd_ctl bit map */
+/******************/
+/* SD push point control, bit 0, 1 */
+#define SD_PUSH_POINT_CTL_MASK		0x03
+#define SD_PUSH_POINT_DELAY		0x01
+#define SD_PUSH_POINT_AUTO		0x02
+/* SD sample point control, bit 2, 3 */
+#define SD_SAMPLE_POINT_CTL_MASK	0x0C
+#define SD_SAMPLE_POINT_DELAY		0x04
+#define SD_SAMPLE_POINT_AUTO		0x08
+/* SD DDR Tx phase set by user, bit 4 */
+#define SD_DDR_TX_PHASE_SET_BY_USER	0x10
+/* MMC DDR Tx phase set by user, bit 5 */
+#define MMC_DDR_TX_PHASE_SET_BY_USER	0x20
+/* Support MMC DDR mode, bit 6 */
+#define SUPPORT_MMC_DDR_MODE		0x40
+/* Reset MMC at first */
+#define RESET_MMC_FIRST			0x80
+
+#define SEQ_START_CRITERIA		0x20
+
+/* MS Power Class En */
+#define POWER_CLASS_2_EN		0x02
+#define POWER_CLASS_1_EN		0x01
+
+#define MAX_SHOW_CNT			10
+#define MAX_RESET_CNT			3
+
+#define SDIO_EXIST			0x01
+#define SDIO_IGNORED			0x02
+
+#define CHK_SDIO_EXIST(chip)		((chip)->sdio_func_exist & SDIO_EXIST)
+#define SET_SDIO_EXIST(chip)		((chip)->sdio_func_exist |= SDIO_EXIST)
+#define CLR_SDIO_EXIST(chip)		((chip)->sdio_func_exist &= ~SDIO_EXIST)
+
+#define CHK_SDIO_IGNORED(chip)		((chip)->sdio_func_exist & SDIO_IGNORED)
+#define SET_SDIO_IGNORED(chip)		((chip)->sdio_func_exist |= \
+					 SDIO_IGNORED)
+#define CLR_SDIO_IGNORED(chip)		((chip)->sdio_func_exist &= \
+					 ~SDIO_IGNORED)
+
+struct rtsx_chip {
+	struct rtsx_dev	*rtsx;
+
+	u32		int_reg; /* Bus interrupt pending register */
+	char		max_lun;
+	void		*context;
+
+	void		*host_cmds_ptr;	/* host commands buffer pointer */
+	dma_addr_t	host_cmds_addr;
+	int		ci;			/* Command Index */
+
+	void		*host_sg_tbl_ptr;	/* SG descriptor table */
+	dma_addr_t	host_sg_tbl_addr;
+	int		sgi;			/* SG entry index */
+
+	struct scsi_cmnd	*srb;			/* current srb */
+	struct sense_data_t	sense_buffer[MAX_ALLOWED_LUN_CNT];
+
+	int			cur_clk;		/* current card clock */
+
+	/* Current accessed card */
+	int			cur_card;
+
+	unsigned long	need_release;		/* need release bit map */
+	unsigned long	need_reset;		/* need reset bit map */
+	/*
+	 * Flag to indicate that this card is just resumed from SS state,
+	 * and need released before being resetted
+	 */
+	unsigned long		need_reinit;
+
+	int			rw_need_retry;
+
+#ifdef SUPPORT_OCP
+	u32			ocp_int;
+	u8			ocp_stat;
+#endif
+
+	u8	card_exist;	/* card exist bit map (physical exist) */
+	u8	card_ready;	/* card ready bit map (reset successfully) */
+	u8	card_fail;	/* card reset fail bit map */
+	u8	card_ejected;	/* card ejected bit map */
+	u8	card_wp;	/* card write protected bit map */
+
+	u8	lun_mc;		/*
+				 * flag to indicate whether to answer
+				 * MediaChange
+				 */
+
+#ifndef LED_AUTO_BLINK
+	int			led_toggle_counter;
+#endif
+
+	int			sd_reset_counter;
+	int			xd_reset_counter;
+	int			ms_reset_counter;
+
+	/* card bus width */
+	u8			card_bus_width[MAX_ALLOWED_LUN_CNT];
+	/* card capacity */
+	u32			capacity[MAX_ALLOWED_LUN_CNT];
+	/* read/write card function pointer */
+	card_rw_func		rw_card[MAX_ALLOWED_LUN_CNT];
+	/* read/write capacity, used for GPIO Toggle */
+	u32			rw_cap[MAX_ALLOWED_LUN_CNT];
+	/* card to lun mapping table */
+	u8			card2lun[32];
+	/* lun to card mapping table */
+	u8			lun2card[MAX_ALLOWED_LUN_CNT];
+
+	int			rw_fail_cnt[MAX_ALLOWED_LUN_CNT];
+
+	int			sd_show_cnt;
+	int			xd_show_cnt;
+	int			ms_show_cnt;
+
+	/* card information */
+	struct sd_info		sd_card;
+	struct xd_info		xd_card;
+	struct ms_info		ms_card;
+
+	struct spi_info		spi;
+
+	int			auto_delink_cnt;
+	int			auto_delink_allowed;
+
+	int			aspm_enabled;
+
+	int			sdio_aspm;
+	int			sdio_idle;
+	int			sdio_counter;
+	u8			sdio_raw_data[12];
+
+	u8			sd_io;
+	u8			sd_int;
+
+	u8			rtsx_flag;
+
+	int			ss_counter;
+	int			idle_counter;
+	enum RTSX_STAT		rtsx_stat;
+
+	u16			vendor_id;
+	u16			product_id;
+	u8			ic_version;
+
+	int			driver_first_load;
+
+#ifdef HW_AUTO_SWITCH_SD_BUS
+	int			sdio_in_charge;
+#endif
+
+	u8			aspm_level[2];
+
+	int			chip_insert_with_sdio;
+
+	/* Options */
+
+	int adma_mode;
+
+	int auto_delink_en;
+	int ss_en;
+	u8 lun_mode;
+	u8 aspm_l0s_l1_en;
+
+	int power_down_in_ss;
+
+	int sdr104_en;
+	int ddr50_en;
+	int sdr50_en;
+
+	int baro_pkg;
+
+	int asic_code;
+	int phy_debug_mode;
+	int hw_bypass_sd;
+	int sdio_func_exist;
+	int aux_pwr_exist;
+	u8 ms_power_class_en;
+
+	int mspro_formatter_enable;
+
+	int remote_wakeup_en;
+
+	int ignore_sd;
+	int use_hw_setting;
+
+	int ss_idle_period;
+
+	int dynamic_aspm;
+
+	int fpga_sd_sdr104_clk;
+	int fpga_sd_ddr50_clk;
+	int fpga_sd_sdr50_clk;
+	int fpga_sd_hs_clk;
+	int fpga_mmc_52m_clk;
+	int fpga_ms_hg_clk;
+	int fpga_ms_4bit_clk;
+	int fpga_ms_1bit_clk;
+
+	int asic_sd_sdr104_clk;
+	int asic_sd_ddr50_clk;
+	int asic_sd_sdr50_clk;
+	int asic_sd_hs_clk;
+	int asic_mmc_52m_clk;
+	int asic_ms_hg_clk;
+	int asic_ms_4bit_clk;
+	int asic_ms_1bit_clk;
+
+	u8 ssc_depth_sd_sdr104;
+	u8 ssc_depth_sd_ddr50;
+	u8 ssc_depth_sd_sdr50;
+	u8 ssc_depth_sd_hs;
+	u8 ssc_depth_mmc_52m;
+	u8 ssc_depth_ms_hg;
+	u8 ssc_depth_ms_4bit;
+	u8 ssc_depth_low_speed;
+
+	u8 card_drive_sel;
+	u8 sd30_drive_sel_1v8;
+	u8 sd30_drive_sel_3v3;
+
+	u8 sd_400mA_ocp_thd;
+	u8 sd_800mA_ocp_thd;
+	u8 ms_ocp_thd;
+
+	int ssc_en;
+	int msi_en;
+
+	int xd_timeout;
+	int sd_timeout;
+	int ms_timeout;
+	int mspro_timeout;
+
+	int auto_power_down;
+
+	int sd_ddr_tx_phase;
+	int mmc_ddr_tx_phase;
+	int sd_default_tx_phase;
+	int sd_default_rx_phase;
+
+	int pmos_pwr_on_interval;
+	int sd_voltage_switch_delay;
+	int s3_pwr_off_delay;
+
+	int force_clkreq_0;
+	int ft2_fast_mode;
+
+	int do_delink_before_power_down;
+	int polling_config;
+	int sdio_retry_cnt;
+
+	int delink_stage1_step;
+	int delink_stage2_step;
+	int delink_stage3_step;
+
+	int auto_delink_in_L1;
+	int hp_watch_bios_hotplug;
+	int support_ms_8bit;
+
+	u8 blink_led;
+	u8 phy_voltage;
+	u8 max_payload;
+
+	u32 sd_speed_prior;
+	u32 sd_current_prior;
+	u32 sd_ctl;
+};
+
+static inline struct device *rtsx_dev(const struct rtsx_chip *chip)
+{
+	return &chip->rtsx->pci->dev;
+}
+
+#define rtsx_set_stat(chip, stat)				\
+do {								\
+	if ((stat) != RTSX_STAT_IDLE) {				\
+		(chip)->idle_counter = 0;			\
+	}							\
+	(chip)->rtsx_stat = (enum RTSX_STAT)(stat);		\
+} while (0)
+#define rtsx_get_stat(chip)		((chip)->rtsx_stat)
+#define rtsx_chk_stat(chip, stat)	((chip)->rtsx_stat == (stat))
+
+#define RTSX_SET_DELINK(chip)	((chip)->rtsx_flag |= 0x01)
+#define RTSX_CLR_DELINK(chip)	((chip)->rtsx_flag &= 0xFE)
+#define RTSX_TST_DELINK(chip)	((chip)->rtsx_flag & 0x01)
+
+#define CHECK_PID(chip, pid)		((chip)->product_id == (pid))
+#define CHECK_BARO_PKG(chip, pkg)	((chip)->baro_pkg == (pkg))
+#define CHECK_LUN_MODE(chip, mode)	((chip)->lun_mode == (mode))
+
+/* Power down control */
+#define SSC_PDCTL		0x01
+#define OC_PDCTL		0x02
+
+int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl);
+int rtsx_force_power_down(struct rtsx_chip *chip, u8 ctl);
+
+void rtsx_enable_card_int(struct rtsx_chip *chip);
+void rtsx_enable_bus_int(struct rtsx_chip *chip);
+void rtsx_disable_bus_int(struct rtsx_chip *chip);
+int rtsx_reset_chip(struct rtsx_chip *chip);
+int rtsx_init_chip(struct rtsx_chip *chip);
+void rtsx_release_chip(struct rtsx_chip *chip);
+void rtsx_polling_func(struct rtsx_chip *chip);
+void rtsx_stop_cmd(struct rtsx_chip *chip, int card);
+int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data);
+int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data);
+int rtsx_write_cfg_dw(struct rtsx_chip *chip,
+		      u8 func_no, u16 addr, u32 mask, u32 val);
+int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val);
+int rtsx_write_cfg_seq(struct rtsx_chip *chip,
+		       u8 func, u16 addr, u8 *buf, int len);
+int rtsx_read_cfg_seq(struct rtsx_chip *chip,
+		      u8 func, u16 addr, u8 *buf, int len);
+int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val);
+int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 *val);
+int rtsx_read_efuse(struct rtsx_chip *chip, u8 addr, u8 *val);
+int rtsx_write_efuse(struct rtsx_chip *chip, u8 addr, u8 val);
+int rtsx_clr_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit);
+int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit);
+void rtsx_enter_ss(struct rtsx_chip *chip);
+void rtsx_exit_ss(struct rtsx_chip *chip);
+int rtsx_pre_handle_interrupt(struct rtsx_chip *chip);
+void rtsx_enter_L1(struct rtsx_chip *chip);
+void rtsx_exit_L1(struct rtsx_chip *chip);
+void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat);
+void rtsx_enable_aspm(struct rtsx_chip *chip);
+void rtsx_disable_aspm(struct rtsx_chip *chip);
+int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len);
+int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len);
+int rtsx_check_chip_exist(struct rtsx_chip *chip);
+
+#endif  /* __REALTEK_RTSX_CHIP_H */
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
new file mode 100644
index 000000000..08bd768ad
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -0,0 +1,3279 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include "rtsx.h"
+#include "sd.h"
+#include "ms.h"
+#include "spi.h"
+
+void scsi_show_command(struct rtsx_chip *chip)
+{
+	struct scsi_cmnd *srb = chip->srb;
+	char *what = NULL;
+	bool unknown_cmd = false;
+	int len;
+
+	switch (srb->cmnd[0]) {
+	case TEST_UNIT_READY:
+		what = "TEST_UNIT_READY";
+		break;
+	case REZERO_UNIT:
+		what = "REZERO_UNIT";
+		break;
+	case REQUEST_SENSE:
+		what = "REQUEST_SENSE";
+		break;
+	case FORMAT_UNIT:
+		what = "FORMAT_UNIT";
+		break;
+	case READ_BLOCK_LIMITS:
+		what = "READ_BLOCK_LIMITS";
+		break;
+	case REASSIGN_BLOCKS:
+		what = "REASSIGN_BLOCKS";
+		break;
+	case READ_6:
+		what = "READ_6";
+		break;
+	case WRITE_6:
+		what = "WRITE_6";
+		break;
+	case SEEK_6:
+		what = "SEEK_6";
+		break;
+	case READ_REVERSE:
+		what = "READ_REVERSE";
+		break;
+	case WRITE_FILEMARKS:
+		what = "WRITE_FILEMARKS";
+		break;
+	case SPACE:
+		what = "SPACE";
+		break;
+	case INQUIRY:
+		what = "INQUIRY";
+		break;
+	case RECOVER_BUFFERED_DATA:
+		what = "RECOVER_BUFFERED_DATA";
+		break;
+	case MODE_SELECT:
+		what = "MODE_SELECT";
+		break;
+	case RESERVE:
+		what = "RESERVE";
+		break;
+	case RELEASE:
+		what = "RELEASE";
+		break;
+	case COPY:
+		what = "COPY";
+		break;
+	case ERASE:
+		what = "ERASE";
+		break;
+	case MODE_SENSE:
+		what = "MODE_SENSE";
+		break;
+	case START_STOP:
+		what = "START_STOP";
+		break;
+	case RECEIVE_DIAGNOSTIC:
+		what = "RECEIVE_DIAGNOSTIC";
+		break;
+	case SEND_DIAGNOSTIC:
+		what = "SEND_DIAGNOSTIC";
+		break;
+	case ALLOW_MEDIUM_REMOVAL:
+		what = "ALLOW_MEDIUM_REMOVAL";
+		break;
+	case SET_WINDOW:
+		what = "SET_WINDOW";
+		break;
+	case READ_CAPACITY:
+		what = "READ_CAPACITY";
+		break;
+	case READ_10:
+		what = "READ_10";
+		break;
+	case WRITE_10:
+		what = "WRITE_10";
+		break;
+	case SEEK_10:
+		what = "SEEK_10";
+		break;
+	case WRITE_VERIFY:
+		what = "WRITE_VERIFY";
+		break;
+	case VERIFY:
+		what = "VERIFY";
+		break;
+	case SEARCH_HIGH:
+		what = "SEARCH_HIGH";
+		break;
+	case SEARCH_EQUAL:
+		what = "SEARCH_EQUAL";
+		break;
+	case SEARCH_LOW:
+		what = "SEARCH_LOW";
+		break;
+	case SET_LIMITS:
+		what = "SET_LIMITS";
+		break;
+	case READ_POSITION:
+		what = "READ_POSITION";
+		break;
+	case SYNCHRONIZE_CACHE:
+		what = "SYNCHRONIZE_CACHE";
+		break;
+	case LOCK_UNLOCK_CACHE:
+		what = "LOCK_UNLOCK_CACHE";
+		break;
+	case READ_DEFECT_DATA:
+		what = "READ_DEFECT_DATA";
+		break;
+	case MEDIUM_SCAN:
+		what = "MEDIUM_SCAN";
+		break;
+	case COMPARE:
+		what = "COMPARE";
+		break;
+	case COPY_VERIFY:
+		what = "COPY_VERIFY";
+		break;
+	case WRITE_BUFFER:
+		what = "WRITE_BUFFER";
+		break;
+	case READ_BUFFER:
+		what = "READ_BUFFER";
+		break;
+	case UPDATE_BLOCK:
+		what = "UPDATE_BLOCK";
+		break;
+	case READ_LONG:
+		what = "READ_LONG";
+		break;
+	case WRITE_LONG:
+		what = "WRITE_LONG";
+		break;
+	case CHANGE_DEFINITION:
+		what = "CHANGE_DEFINITION";
+		break;
+	case WRITE_SAME:
+		what = "WRITE_SAME";
+		break;
+	case GPCMD_READ_SUBCHANNEL:
+		what = "READ SUBCHANNEL";
+		break;
+	case READ_TOC:
+		what = "READ_TOC";
+		break;
+	case GPCMD_READ_HEADER:
+		what = "READ HEADER";
+		break;
+	case GPCMD_PLAY_AUDIO_10:
+		what = "PLAY AUDIO (10)";
+		break;
+	case GPCMD_PLAY_AUDIO_MSF:
+		what = "PLAY AUDIO MSF";
+		break;
+	case GPCMD_GET_EVENT_STATUS_NOTIFICATION:
+		what = "GET EVENT/STATUS NOTIFICATION";
+		break;
+	case GPCMD_PAUSE_RESUME:
+		what = "PAUSE/RESUME";
+		break;
+	case LOG_SELECT:
+		what = "LOG_SELECT";
+		break;
+	case LOG_SENSE:
+		what = "LOG_SENSE";
+		break;
+	case GPCMD_STOP_PLAY_SCAN:
+		what = "STOP PLAY/SCAN";
+		break;
+	case GPCMD_READ_DISC_INFO:
+		what = "READ DISC INFORMATION";
+		break;
+	case GPCMD_READ_TRACK_RZONE_INFO:
+		what = "READ TRACK INFORMATION";
+		break;
+	case GPCMD_RESERVE_RZONE_TRACK:
+		what = "RESERVE TRACK";
+		break;
+	case GPCMD_SEND_OPC:
+		what = "SEND OPC";
+		break;
+	case MODE_SELECT_10:
+		what = "MODE_SELECT_10";
+		break;
+	case GPCMD_REPAIR_RZONE_TRACK:
+		what = "REPAIR TRACK";
+		break;
+	case 0x59:
+		what = "READ MASTER CUE";
+		break;
+	case MODE_SENSE_10:
+		what = "MODE_SENSE_10";
+		break;
+	case GPCMD_CLOSE_TRACK:
+		what = "CLOSE TRACK/SESSION";
+		break;
+	case 0x5C:
+		what = "READ BUFFER CAPACITY";
+		break;
+	case 0x5D:
+		what = "SEND CUE SHEET";
+		break;
+	case GPCMD_BLANK:
+		what = "BLANK";
+		break;
+	case REPORT_LUNS:
+		what = "REPORT LUNS";
+		break;
+	case MOVE_MEDIUM:
+		what = "MOVE_MEDIUM or PLAY AUDIO (12)";
+		break;
+	case READ_12:
+		what = "READ_12";
+		break;
+	case WRITE_12:
+		what = "WRITE_12";
+		break;
+	case WRITE_VERIFY_12:
+		what = "WRITE_VERIFY_12";
+		break;
+	case SEARCH_HIGH_12:
+		what = "SEARCH_HIGH_12";
+		break;
+	case SEARCH_EQUAL_12:
+		what = "SEARCH_EQUAL_12";
+		break;
+	case SEARCH_LOW_12:
+		what = "SEARCH_LOW_12";
+		break;
+	case SEND_VOLUME_TAG:
+		what = "SEND_VOLUME_TAG";
+		break;
+	case READ_ELEMENT_STATUS:
+		what = "READ_ELEMENT_STATUS";
+		break;
+	case GPCMD_READ_CD_MSF:
+		what = "READ CD MSF";
+		break;
+	case GPCMD_SCAN:
+		what = "SCAN";
+		break;
+	case GPCMD_SET_SPEED:
+		what = "SET CD SPEED";
+		break;
+	case GPCMD_MECHANISM_STATUS:
+		what = "MECHANISM STATUS";
+		break;
+	case GPCMD_READ_CD:
+		what = "READ CD";
+		break;
+	case 0xE1:
+		what = "WRITE CONTINUE";
+		break;
+	case WRITE_LONG_2:
+		what = "WRITE_LONG_2";
+		break;
+	case VENDOR_CMND:
+		what = "Realtek's vendor command";
+		break;
+	default:
+		what = "(unknown command)";
+		unknown_cmd = true;
+		break;
+	}
+
+	if (srb->cmnd[0] != TEST_UNIT_READY)
+		dev_dbg(rtsx_dev(chip), "Command %s (%d bytes)\n",
+			what, srb->cmd_len);
+
+	if (unknown_cmd) {
+		len = min_t(unsigned short, srb->cmd_len, 16);
+		dev_dbg(rtsx_dev(chip), "%*ph\n", len, srb->cmnd);
+	}
+}
+
+void set_sense_type(struct rtsx_chip *chip, unsigned int lun, int sense_type)
+{
+	switch (sense_type) {
+	case SENSE_TYPE_MEDIA_CHANGE:
+		set_sense_data(chip, lun, CUR_ERR, 0x06, 0, 0x28, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_NOT_PRESENT:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x3A, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_LBA_OVER_RANGE:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x21, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x25, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_WRITE_PROTECT:
+		set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x27, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x11, 0, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_WRITE_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x02, 0, 0);
+		break;
+
+	case SENSE_TYPE_MEDIA_INVALID_CMD_FIELD:
+		set_sense_data(chip, lun, CUR_ERR, ILGAL_REQ, 0,
+			       ASC_INVLD_CDB, ASCQ_INVLD_CDB, CDB_ILLEGAL, 1);
+		break;
+
+	case SENSE_TYPE_FORMAT_IN_PROGRESS:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04, 0, 0);
+		break;
+
+	case SENSE_TYPE_FORMAT_CMD_FAILED:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x31, 0x01, 0, 0);
+		break;
+
+#ifdef SUPPORT_MAGIC_GATE
+	case SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x02, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN:
+		set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x00, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM:
+		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x30, 0x00, 0, 0);
+		break;
+
+	case SENSE_TYPE_MG_WRITE_ERR:
+		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x00, 0, 0);
+		break;
+#endif
+
+#ifdef SUPPORT_SD_LOCK
+	case SENSE_TYPE_MEDIA_READ_FORBIDDEN:
+		set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x11, 0x13, 0, 0);
+		break;
+#endif
+
+	case SENSE_TYPE_NO_SENSE:
+	default:
+		set_sense_data(chip, lun, CUR_ERR, 0, 0, 0, 0, 0, 0);
+		break;
+	}
+}
+
+void set_sense_data(struct rtsx_chip *chip, unsigned int lun, u8 err_code,
+		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
+		u16 sns_key_info1)
+{
+	struct sense_data_t *sense = &chip->sense_buffer[lun];
+
+	sense->err_code = err_code;
+	sense->sense_key = sense_key;
+	sense->info[0] = (u8)(info >> 24);
+	sense->info[1] = (u8)(info >> 16);
+	sense->info[2] = (u8)(info >> 8);
+	sense->info[3] = (u8)info;
+
+	sense->ad_sense_len = sizeof(struct sense_data_t) - 8;
+	sense->asc = asc;
+	sense->ascq = ascq;
+	if (sns_key_info0 != 0) {
+		sense->sns_key_info[0] = SKSV | sns_key_info0;
+		sense->sns_key_info[1] = (sns_key_info1 & 0xf0) >> 4;
+		sense->sns_key_info[2] = sns_key_info1 & 0x0f;
+	}
+}
+
+static int test_unit_ready(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	if (get_lun_card(chip, SCSI_LUN(srb)) == SD_CARD) {
+		struct sd_info *sd_card = &chip->sd_card;
+
+		if (sd_card->sd_lock_notify) {
+			sd_card->sd_lock_notify = 0;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+			return TRANSPORT_FAILED;
+		} else if (sd_card->sd_lock_status & SD_LOCKED) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+			return TRANSPORT_FAILED;
+		}
+	}
+#endif
+
+	return TRANSPORT_GOOD;
+}
+
+static unsigned char formatter_inquiry_str[20] = {
+	'M', 'E', 'M', 'O', 'R', 'Y', 'S', 'T', 'I', 'C', 'K',
+#ifdef SUPPORT_MAGIC_GATE
+	'-', 'M', 'G', /* Byte[47:49] */
+#else
+	0x20, 0x20, 0x20,  /* Byte[47:49] */
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	0x0B,  /* Byte[50]: MG, MS, MSPro, MSXC */
+#else
+	0x09,  /* Byte[50]: MS, MSPro, MSXC */
+#endif
+	0x00,  /* Byte[51]: Category Specific Commands */
+	0x00,  /* Byte[52]: Access Control and feature */
+	0x20, 0x20, 0x20, /* Byte[53:55] */
+};
+
+static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	char *inquiry_default = (char *)"Generic-xD/SD/M.S.      1.00 ";
+	char *inquiry_sdms =    (char *)"Generic-SD/MemoryStick  1.00 ";
+	char *inquiry_sd =      (char *)"Generic-SD/MMC          1.00 ";
+	char *inquiry_ms =      (char *)"Generic-MemoryStick     1.00 ";
+	char *inquiry_string;
+	unsigned char sendbytes;
+	unsigned char *buf;
+	u8 card = get_lun_card(chip, lun);
+	bool pro_formatter_flag = false;
+	unsigned char inquiry_buf[] = {
+		QULIFIRE | DRCT_ACCESS_DEV,
+		RMB_DISC | 0x0D,
+		0x00,
+		0x01,
+		0x1f,
+		0x02,
+		0,
+		REL_ADR | WBUS_32 | WBUS_16 | SYNC | LINKED | CMD_QUE | SFT_RE,
+	};
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (chip->lun2card[lun] == SD_CARD)
+			inquiry_string = inquiry_sd;
+		else
+			inquiry_string = inquiry_ms;
+
+	} else if (CHECK_LUN_MODE(chip, SD_MS_1LUN)) {
+		inquiry_string = inquiry_sdms;
+	} else {
+		inquiry_string = inquiry_default;
+	}
+
+	buf = vmalloc(scsi_bufflen(srb));
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+#ifdef SUPPORT_MAGIC_GATE
+	if (chip->mspro_formatter_enable &&
+	    (chip->lun2card[lun] & MS_CARD))
+#else
+	if (chip->mspro_formatter_enable)
+#endif
+		if (!card || card == MS_CARD)
+			pro_formatter_flag = true;
+
+	if (pro_formatter_flag) {
+		if (scsi_bufflen(srb) < 56)
+			sendbytes = (unsigned char)(scsi_bufflen(srb));
+		else
+			sendbytes = 56;
+
+	} else {
+		if (scsi_bufflen(srb) < 36)
+			sendbytes = (unsigned char)(scsi_bufflen(srb));
+		else
+			sendbytes = 36;
+	}
+
+	if (sendbytes > 8) {
+		memcpy(buf, inquiry_buf, 8);
+		strncpy(buf + 8, inquiry_string, sendbytes - 8);
+		if (pro_formatter_flag) {
+			/* Additional Length */
+			buf[4] = 0x33;
+		}
+	} else {
+		memcpy(buf, inquiry_buf, sendbytes);
+	}
+
+	if (pro_formatter_flag) {
+		if (sendbytes > 36)
+			memcpy(buf + 36, formatter_inquiry_str, sendbytes - 36);
+	}
+
+	scsi_set_resid(srb, 0);
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int start_stop_unit(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	scsi_set_resid(srb, scsi_bufflen(srb));
+
+	if (srb->cmnd[1] == 1)
+		return TRANSPORT_GOOD;
+
+	switch (srb->cmnd[0x4]) {
+	case STOP_MEDIUM:
+		/* Media disabled */
+		return TRANSPORT_GOOD;
+
+	case UNLOAD_MEDIUM:
+		/* Media shall be unload */
+		if (check_card_ready(chip, lun))
+			eject_card(chip, lun);
+		return TRANSPORT_GOOD;
+
+	case MAKE_MEDIUM_READY:
+	case LOAD_MEDIUM:
+		if (check_card_ready(chip, lun))
+			return TRANSPORT_GOOD;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+
+		break;
+	}
+
+	return TRANSPORT_ERROR;
+}
+
+static int allow_medium_removal(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int prevent;
+
+	prevent = srb->cmnd[4] & 0x1;
+
+	scsi_set_resid(srb, 0);
+
+	if (prevent) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int request_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sense_data_t *sense;
+	unsigned int lun = SCSI_LUN(srb);
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned char *tmp, *buf;
+
+	sense = &chip->sense_buffer[lun];
+
+	if ((get_lun_card(chip, lun) == MS_CARD) &&
+	    ms_card->pro_under_formatting) {
+		if (ms_card->format_status == FORMAT_SUCCESS) {
+			set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+			ms_card->pro_under_formatting = 0;
+			ms_card->progress = 0;
+		} else if (ms_card->format_status == FORMAT_IN_PROGRESS) {
+			/* Logical Unit Not Ready Format in Progress */
+			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
+				       0, (u16)(ms_card->progress));
+		} else {
+			/* Format Command Failed */
+			set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
+			ms_card->pro_under_formatting = 0;
+			ms_card->progress = 0;
+		}
+
+		rtsx_set_stat(chip, RTSX_STAT_RUN);
+	}
+
+	buf = vmalloc(scsi_bufflen(srb));
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	tmp = (unsigned char *)sense;
+	memcpy(buf, tmp, scsi_bufflen(srb));
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	vfree(buf);
+
+	scsi_set_resid(srb, 0);
+	/* Reset Sense Data */
+	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+	return TRANSPORT_GOOD;
+}
+
+static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
+			  int lun, u8 *buf, int buf_len)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	int sys_info_offset;
+	int data_size = buf_len;
+	bool support_format = false;
+	int i = 0;
+
+	if (cmd == MODE_SENSE) {
+		sys_info_offset = 8;
+		if (data_size > 0x68)
+			data_size = 0x68;
+
+		buf[i++] = 0x67;  /* Mode Data Length */
+	} else {
+		sys_info_offset = 12;
+		if (data_size > 0x6C)
+			data_size = 0x6C;
+
+		buf[i++] = 0x00;  /* Mode Data Length (MSB) */
+		buf[i++] = 0x6A;  /* Mode Data Length (LSB) */
+	}
+
+	/* Medium Type Code */
+	if (check_card_ready(chip, lun)) {
+		if (CHK_MSXC(ms_card)) {
+			support_format = true;
+			buf[i++] = 0x40;
+		} else if (CHK_MSPRO(ms_card)) {
+			support_format = true;
+			buf[i++] = 0x20;
+		} else {
+			buf[i++] = 0x10;
+		}
+
+		/* WP */
+		if (check_card_wp(chip, lun))
+			buf[i++] = 0x80;
+		else
+			buf[i++] = 0x00;
+
+	} else {
+		buf[i++] = 0x00;	/* MediaType */
+		buf[i++] = 0x00;	/* WP */
+	}
+
+	buf[i++] = 0x00;		/* Reserved */
+
+	if (cmd == MODE_SENSE_10) {
+		buf[i++] = 0x00;  /* Reserved */
+		buf[i++] = 0x00;  /* Block descriptor length(MSB) */
+		buf[i++] = 0x00;  /* Block descriptor length(LSB) */
+
+		/* The Following Data is the content of "Page 0x20" */
+		if (data_size >= 9)
+			buf[i++] = 0x20;		/* Page Code */
+		if (data_size >= 10)
+			buf[i++] = 0x62;		/* Page Length */
+		if (data_size >= 11)
+			buf[i++] = 0x00;		/* No Access Control */
+		if (data_size >= 12) {
+			if (support_format)
+				buf[i++] = 0xC0;	/* SF, SGM */
+			else
+				buf[i++] = 0x00;
+		}
+	} else {
+		/* The Following Data is the content of "Page 0x20" */
+		if (data_size >= 5)
+			buf[i++] = 0x20;		/* Page Code */
+		if (data_size >= 6)
+			buf[i++] = 0x62;		/* Page Length */
+		if (data_size >= 7)
+			buf[i++] = 0x00;		/* No Access Control */
+		if (data_size >= 8) {
+			if (support_format)
+				buf[i++] = 0xC0;	/* SF, SGM */
+			else
+				buf[i++] = 0x00;
+		}
+	}
+
+	if (data_size > sys_info_offset) {
+		/* 96 Bytes Attribute Data */
+		int len = data_size - sys_info_offset;
+
+		len = (len < 96) ? len : 96;
+
+		memcpy(buf + sys_info_offset, ms_card->raw_sys_info, len);
+	}
+}
+
+static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	unsigned int data_size;
+	int status;
+	bool pro_formatter_flag;
+	unsigned char page_code, *buf;
+	u8 card = get_lun_card(chip, lun);
+
+#ifndef SUPPORT_MAGIC_GATE
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		scsi_set_resid(srb, scsi_bufflen(srb));
+		return TRANSPORT_FAILED;
+	}
+#endif
+
+	pro_formatter_flag = false;
+	data_size = 8;
+#ifdef SUPPORT_MAGIC_GATE
+	if ((chip->lun2card[lun] & MS_CARD)) {
+		if (!card || card == MS_CARD) {
+			data_size = 108;
+			if (chip->mspro_formatter_enable)
+				pro_formatter_flag = true;
+		}
+	}
+#else
+	if (card == MS_CARD) {
+		if (chip->mspro_formatter_enable) {
+			pro_formatter_flag = true;
+			data_size = 108;
+		}
+	}
+#endif
+
+	buf = kmalloc(data_size, GFP_KERNEL);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	page_code = srb->cmnd[2] & 0x3f;
+
+	if (page_code == 0x3F || page_code == 0x1C ||
+	    page_code == 0x00 ||
+		(pro_formatter_flag && page_code == 0x20)) {
+		if (srb->cmnd[0] == MODE_SENSE) {
+			if (page_code == 0x3F || page_code == 0x20) {
+				ms_mode_sense(chip, srb->cmnd[0],
+					      lun, buf, data_size);
+			} else {
+				data_size = 4;
+				buf[0] = 0x03;
+				buf[1] = 0x00;
+				if (check_card_wp(chip, lun))
+					buf[2] = 0x80;
+				else
+					buf[2] = 0x00;
+
+				buf[3] = 0x00;
+			}
+		} else {
+			if (page_code == 0x3F || page_code == 0x20) {
+				ms_mode_sense(chip, srb->cmnd[0],
+					      lun, buf, data_size);
+			} else {
+				data_size = 8;
+				buf[0] = 0x00;
+				buf[1] = 0x06;
+				buf[2] = 0x00;
+				if (check_card_wp(chip, lun))
+					buf[3] = 0x80;
+				else
+					buf[3] = 0x00;
+				buf[4] = 0x00;
+				buf[5] = 0x00;
+				buf[6] = 0x00;
+				buf[7] = 0x00;
+			}
+		}
+		status = TRANSPORT_GOOD;
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		scsi_set_resid(srb, scsi_bufflen(srb));
+		status = TRANSPORT_FAILED;
+	}
+
+	if (status == TRANSPORT_GOOD) {
+		unsigned int len = min_t(unsigned int, scsi_bufflen(srb),
+					data_size);
+		rtsx_stor_set_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+	}
+	kfree(buf);
+
+	return status;
+}
+
+static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &chip->sd_card;
+#endif
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u32 start_sec;
+	u16 sec_cnt;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	if (!check_card_ready(chip, lun) || (get_card_size(chip, lun) == 0)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_erase_status) {
+		/* Accessing to any card is forbidden
+		 * until the erase procedure of SD is completed
+		 */
+		dev_dbg(rtsx_dev(chip), "SD card being erased!\n");
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+		return TRANSPORT_FAILED;
+	}
+
+	if (get_lun_card(chip, lun) == SD_CARD) {
+		if (sd_card->sd_lock_status & SD_LOCKED) {
+			dev_dbg(rtsx_dev(chip), "SD card locked!\n");
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
+			return TRANSPORT_FAILED;
+		}
+	}
+#endif
+
+	if (srb->cmnd[0] == READ_10 || srb->cmnd[0] == WRITE_10) {
+		start_sec = ((u32)srb->cmnd[2] << 24) |
+			((u32)srb->cmnd[3] << 16) |
+			((u32)srb->cmnd[4] << 8) | ((u32)srb->cmnd[5]);
+		sec_cnt = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
+	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
+		start_sec = ((u32)(srb->cmnd[1] & 0x1F) << 16) |
+			((u32)srb->cmnd[2] << 8) | ((u32)srb->cmnd[3]);
+		sec_cnt = srb->cmnd[4];
+		if (sec_cnt == 0)
+			sec_cnt = 256;
+	} else if ((srb->cmnd[0] == VENDOR_CMND) &&
+		(srb->cmnd[1] == SCSI_APP_CMD) &&
+		((srb->cmnd[2] == PP_READ10) || (srb->cmnd[2] == PP_WRITE10))) {
+		start_sec = ((u32)srb->cmnd[4] << 24) |
+			((u32)srb->cmnd[5] << 16) |
+			((u32)srb->cmnd[6] << 8) | ((u32)srb->cmnd[7]);
+		sec_cnt = ((u16)(srb->cmnd[9]) << 8) | srb->cmnd[10];
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	/* In some test, we will receive a start_sec like 0xFFFFFFFF.
+	 * In this situation, start_sec + sec_cnt will overflow, so we
+	 * need to judge start_sec at first
+	 */
+	if (start_sec > get_card_size(chip, lun) ||
+	    ((start_sec + sec_cnt) > get_card_size(chip, lun))) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LBA_OVER_RANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	if (sec_cnt == 0) {
+		scsi_set_resid(srb, 0);
+		return TRANSPORT_GOOD;
+	}
+
+	if (chip->rw_fail_cnt[lun] == 3) {
+		dev_dbg(rtsx_dev(chip), "read/write fail three times in succession\n");
+		if (srb->sc_data_direction == DMA_FROM_DEVICE)
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		else
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+		if (check_card_wp(chip, lun)) {
+			dev_dbg(rtsx_dev(chip), "Write protected card!\n");
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_WRITE_PROTECT);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	retval = card_rw(srb, chip, start_sec, sec_cnt);
+	if (retval != STATUS_SUCCESS) {
+		if (chip->need_release & chip->lun2card[lun]) {
+			chip->rw_fail_cnt[lun] = 0;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		} else {
+			chip->rw_fail_cnt[lun]++;
+			if (srb->sc_data_direction == DMA_FROM_DEVICE)
+				set_sense_type
+					(chip, lun,
+					 SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			else
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+		}
+		retval = TRANSPORT_FAILED;
+		goto exit;
+	} else {
+		chip->rw_fail_cnt[lun] = 0;
+		retval = TRANSPORT_GOOD;
+	}
+
+	scsi_set_resid(srb, 0);
+
+exit:
+	return retval;
+}
+
+static int read_format_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *buf;
+	unsigned int lun = SCSI_LUN(srb);
+	unsigned int buf_len;
+	u8 card = get_lun_card(chip, lun);
+	u32 card_size;
+	int desc_cnt;
+	int i = 0;
+
+	if (!check_card_ready(chip, lun)) {
+		if (!chip->mspro_formatter_enable) {
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
+
+	buf = kmalloc(buf_len, GFP_KERNEL);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	buf[i++] = 0;
+	buf[i++] = 0;
+	buf[i++] = 0;
+
+	/* Capacity List Length */
+	if (buf_len > 12 && chip->mspro_formatter_enable &&
+	    (chip->lun2card[lun] & MS_CARD) &&
+	    (!card || card == MS_CARD)) {
+		buf[i++] = 0x10;
+		desc_cnt = 2;
+	} else {
+		buf[i++] = 0x08;
+		desc_cnt = 1;
+	}
+
+	while (desc_cnt) {
+		if (check_card_ready(chip, lun)) {
+			card_size = get_card_size(chip, lun);
+			buf[i++] = (unsigned char)(card_size >> 24);
+			buf[i++] = (unsigned char)(card_size >> 16);
+			buf[i++] = (unsigned char)(card_size >> 8);
+			buf[i++] = (unsigned char)card_size;
+
+			if (desc_cnt == 2)
+				buf[i++] = 2;
+			else
+				buf[i++] = 0;
+		} else {
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+			buf[i++] = 0xFF;
+
+			if (desc_cnt == 2)
+				buf[i++] = 3;
+			else
+				buf[i++] = 0;
+		}
+
+		buf[i++] = 0x00;
+		buf[i++] = 0x02;
+		buf[i++] = 0x00;
+
+		desc_cnt--;
+	}
+
+	buf_len = min_t(unsigned int, scsi_bufflen(srb), buf_len);
+	rtsx_stor_set_xfer_buf(buf, buf_len, srb);
+	kfree(buf);
+
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned char *buf;
+	unsigned int lun = SCSI_LUN(srb);
+	u32 card_size;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	buf = kmalloc(8, GFP_KERNEL);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	card_size = get_card_size(chip, lun);
+	buf[0] = (unsigned char)((card_size - 1) >> 24);
+	buf[1] = (unsigned char)((card_size - 1) >> 16);
+	buf[2] = (unsigned char)((card_size - 1) >> 8);
+	buf[3] = (unsigned char)(card_size - 1);
+
+	buf[4] = 0x00;
+	buf[5] = 0x00;
+	buf[6] = 0x02;
+	buf[7] = 0x00;
+
+	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+	kfree(buf);
+
+	scsi_set_resid(srb, 0);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = spi_read_eeprom(chip, i, buf + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	if (len == 511) {
+		retval = spi_erase_eeprom_chip(chip);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+	} else {
+		len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb),
+					len);
+		buf = vmalloc(len);
+		if (!buf)
+			return TRANSPORT_ERROR;
+
+		rtsx_stor_get_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+		for (i = 0; i < len; i++) {
+			retval = spi_write_eeprom(chip, i, buf[i]);
+			if (retval != STATUS_SUCCESS) {
+				vfree(buf);
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return TRANSPORT_FAILED;
+			}
+		}
+
+		vfree(buf);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[2] << 8) | srb->cmnd[3];
+	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	if (addr < 0xFC00) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_read_register(chip, addr + i, buf + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[2] << 8) | srb->cmnd[3];
+	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	if (addr < 0xFC00) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_write_register(chip, addr + i, 0xFF, buf[i]);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_sd_csd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (get_lun_card(chip, lun) != SD_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	rtsx_stor_set_xfer_buf(sd_card->raw_csd, scsi_bufflen(srb), srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static int toggle_gpio_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 gpio = srb->cmnd[2];
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	if (gpio > 3)
+		gpio = 1;
+	toggle_gpio(chip, gpio);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_host_reg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 addr, buf[4];
+	u32 val;
+	unsigned int len;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+
+	val = rtsx_readl(chip, addr);
+	dev_dbg(rtsx_dev(chip), "Host register (0x%x): 0x%x\n", addr, val);
+
+	buf[0] = (u8)(val >> 24);
+	buf[1] = (u8)(val >> 16);
+	buf[2] = (u8)(val >> 8);
+	buf[3] = (u8)val;
+
+	len = min_t(unsigned int, scsi_bufflen(srb), 4);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_host_reg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 addr, buf[4];
+	u32 val;
+	unsigned int len;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+
+	len = min_t(unsigned int, scsi_bufflen(srb), 4);
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	val = ((u32)buf[0] << 24) | ((u32)buf[1] << 16) | ((u32)buf[2]
+							<< 8) | buf[3];
+
+	rtsx_writel(chip, addr, val);
+
+	return TRANSPORT_GOOD;
+}
+
+static int set_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (srb->cmnd[3] == 1) {
+		/* Variable Clock */
+		struct xd_info *xd_card = &chip->xd_card;
+		struct sd_info *sd_card = &chip->sd_card;
+		struct ms_info *ms_card = &chip->ms_card;
+
+		switch (srb->cmnd[4]) {
+		case XD_CARD:
+			xd_card->xd_clock = srb->cmnd[5];
+			break;
+
+		case SD_CARD:
+			sd_card->sd_clock = srb->cmnd[5];
+			break;
+
+		case MS_CARD:
+			ms_card->ms_clock = srb->cmnd[5];
+			break;
+
+		default:
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+	} else if (srb->cmnd[3] == 2) {
+		if (srb->cmnd[4]) {
+			chip->blink_led = 1;
+		} else {
+			int retval;
+
+			chip->blink_led = 0;
+
+			rtsx_disable_aspm(chip);
+
+			if (chip->ss_en &&
+			    (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+				rtsx_exit_ss(chip);
+				wait_timeout(100);
+			}
+			rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+			retval = rtsx_force_power_on(chip, SSC_PDCTL);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return TRANSPORT_FAILED;
+			}
+
+			turn_off_led(chip, LED_GPIO);
+		}
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+
+	if (srb->cmnd[3] == 1) {
+		struct xd_info *xd_card = &chip->xd_card;
+		struct sd_info *sd_card = &chip->sd_card;
+		struct ms_info *ms_card = &chip->ms_card;
+		u8 tmp;
+
+		switch (srb->cmnd[4]) {
+		case XD_CARD:
+			tmp = (u8)(xd_card->xd_clock);
+			break;
+
+		case SD_CARD:
+			tmp = (u8)(sd_card->sd_clock);
+			break;
+
+		case MS_CARD:
+			tmp = (u8)(ms_card->ms_clock);
+			break;
+
+		default:
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+
+		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
+	} else if (srb->cmnd[3] == 2) {
+		u8 tmp = chip->blink_led;
+
+		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int dma_access_ring_buffer(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	unsigned int lun = SCSI_LUN(srb);
+	u16 len;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	len = ((u16)(srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = min_t(u16, len, scsi_bufflen(srb));
+
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
+		dev_dbg(rtsx_dev(chip), "Read from device\n");
+	else
+		dev_dbg(rtsx_dev(chip), "Write to device\n");
+
+	retval = rtsx_transfer_data(chip, 0, scsi_sglist(srb), len,
+				    scsi_sg_count(srb), srb->sc_data_direction,
+				    1000);
+	if (retval < 0) {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE)
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		else
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+
+		return TRANSPORT_FAILED;
+	}
+	scsi_set_resid(srb, 0);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_dev_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	struct ms_info *ms_card = &chip->ms_card;
+	int buf_len;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 card = get_lun_card(chip, lun);
+	u8 status[32];
+#ifdef SUPPORT_OCP
+	u8 oc_now_mask = 0, oc_ever_mask = 0;
+#endif
+
+	memset(status, 0, 32);
+
+	status[0] = (u8)(chip->product_id);
+	status[1] = chip->ic_version;
+
+	if (chip->auto_delink_en)
+		status[2] = 0x10;
+	else
+		status[2] = 0x00;
+
+	status[3] = 20;
+	status[4] = 10;
+	status[5] = 05;
+	status[6] = 21;
+
+	if (chip->card_wp)
+		status[7] = 0x20;
+	else
+		status[7] = 0x00;
+
+#ifdef SUPPORT_OCP
+	status[8] = 0;
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN) &&
+	    chip->lun2card[lun] == MS_CARD) {
+		oc_now_mask = MS_OC_NOW;
+		oc_ever_mask = MS_OC_EVER;
+	} else {
+		oc_now_mask = SD_OC_NOW;
+		oc_ever_mask = SD_OC_EVER;
+	}
+
+	if (chip->ocp_stat & oc_now_mask)
+		status[8] |= 0x02;
+
+	if (chip->ocp_stat & oc_ever_mask)
+		status[8] |= 0x01;
+#endif
+
+	if (card == SD_CARD) {
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_HCXC(sd_card)) {
+				if (sd_card->capacity > 0x4000000)
+					status[0x0E] = 0x02;
+				else
+					status[0x0E] = 0x01;
+			} else {
+				status[0x0E] = 0x00;
+			}
+
+			if (CHK_SD_SDR104(sd_card))
+				status[0x0F] = 0x03;
+			else if (CHK_SD_DDR50(sd_card))
+				status[0x0F] = 0x04;
+			else if (CHK_SD_SDR50(sd_card))
+				status[0x0F] = 0x02;
+			else if (CHK_SD_HS(sd_card))
+				status[0x0F] = 0x01;
+			else
+				status[0x0F] = 0x00;
+		} else {
+			if (CHK_MMC_SECTOR_MODE(sd_card))
+				status[0x0E] = 0x01;
+			else
+				status[0x0E] = 0x00;
+
+			if (CHK_MMC_DDR52(sd_card))
+				status[0x0F] = 0x03;
+			else if (CHK_MMC_52M(sd_card))
+				status[0x0F] = 0x02;
+			else if (CHK_MMC_26M(sd_card))
+				status[0x0F] = 0x01;
+			else
+				status[0x0F] = 0x00;
+		}
+	} else if (card == MS_CARD) {
+		if (CHK_MSPRO(ms_card)) {
+			if (CHK_MSXC(ms_card))
+				status[0x0E] = 0x01;
+			else
+				status[0x0E] = 0x00;
+
+			if (CHK_HG8BIT(ms_card))
+				status[0x0F] = 0x01;
+			else
+				status[0x0F] = 0x00;
+		}
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	if (card == SD_CARD) {
+		status[0x17] = 0x80;
+		if (sd_card->sd_erase_status)
+			status[0x17] |= 0x01;
+		if (sd_card->sd_lock_status & SD_LOCKED) {
+			status[0x17] |= 0x02;
+			status[0x07] |= 0x40;
+		}
+		if (sd_card->sd_lock_status & SD_PWD_EXIST)
+			status[0x17] |= 0x04;
+	} else {
+		status[0x17] = 0x00;
+	}
+
+	dev_dbg(rtsx_dev(chip), "status[0x17] = 0x%x\n", status[0x17]);
+#endif
+
+	status[0x18] = 0x8A;
+	status[0x1A] = 0x28;
+#ifdef SUPPORT_SD_LOCK
+	status[0x1F] = 0x01;
+#endif
+
+	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(status));
+	rtsx_stor_set_xfer_buf(status, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int set_chip_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int phy_debug_mode;
+	int retval;
+	u16 reg;
+
+	if (!CHECK_PID(chip, 0x5208)) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	phy_debug_mode = (int)(srb->cmnd[3]);
+
+	if (phy_debug_mode) {
+		chip->phy_debug_mode = 1;
+		retval = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+
+		rtsx_disable_bus_int(chip);
+
+		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+
+		reg |= 0x0001;
+		retval = rtsx_write_phy_register(chip, 0x1C, reg);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+	} else {
+		chip->phy_debug_mode = 0;
+		retval = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0x77);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+
+		rtsx_enable_bus_int(chip);
+
+		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+
+		reg &= 0xFFFE;
+		retval = rtsx_write_phy_register(chip, 0x1C, reg);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval =  STATUS_SUCCESS;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 cmd_type, mask, value, idx;
+	u16 addr;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	switch (srb->cmnd[3]) {
+	case INIT_BATCHCMD:
+		rtsx_init_cmd(chip);
+		break;
+
+	case ADD_BATCHCMD:
+		cmd_type = srb->cmnd[4];
+		if (cmd_type > 2) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		addr = (srb->cmnd[5] << 8) | srb->cmnd[6];
+		mask = srb->cmnd[7];
+		value = srb->cmnd[8];
+		rtsx_add_cmd(chip, cmd_type, addr, mask, value);
+		break;
+
+	case SEND_BATCHCMD:
+		retval = rtsx_send_cmd(chip, 0, 1000);
+		break;
+
+	case GET_BATCHRSP:
+		idx = srb->cmnd[4];
+		value = *(rtsx_get_cmd_data(chip) + idx);
+		if (scsi_bufflen(srb) < 1) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		rtsx_stor_set_xfer_buf(&value, 1, srb);
+		scsi_set_resid(srb, 0);
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int suit_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	switch (srb->cmnd[3]) {
+	case INIT_BATCHCMD:
+	case ADD_BATCHCMD:
+	case SEND_BATCHCMD:
+	case GET_BATCHRSP:
+		return rw_mem_cmd_buf(srb, chip);
+	default:
+		return TRANSPORT_ERROR;
+	}
+}
+
+static int read_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+	u16 val;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	if (len % 2)
+		len -= len % 2;
+
+	if (len) {
+		buf = vmalloc(len);
+		if (!buf)
+			return TRANSPORT_ERROR;
+
+		retval = rtsx_force_power_on(chip, SSC_PDCTL);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return TRANSPORT_FAILED;
+		}
+
+		for (i = 0; i < len / 2; i++) {
+			retval = rtsx_read_phy_register(chip, addr + i, &val);
+			if (retval != STATUS_SUCCESS) {
+				vfree(buf);
+				set_sense_type
+					(chip, SCSI_LUN(srb),
+					 SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+				return TRANSPORT_FAILED;
+			}
+
+			buf[2 * i] = (u8)(val >> 8);
+			buf[2 * i + 1] = (u8)val;
+		}
+
+		len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb),
+					len);
+		rtsx_stor_set_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+		vfree(buf);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+	u16 val;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	if (len % 2)
+		len -= len % 2;
+
+	if (len) {
+		len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb),
+					len);
+
+		buf = vmalloc(len);
+		if (!buf)
+			return TRANSPORT_ERROR;
+
+		rtsx_stor_get_xfer_buf(buf, len, srb);
+		scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+		retval = rtsx_force_power_on(chip, SSC_PDCTL);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+
+		for (i = 0; i < len / 2; i++) {
+			val = ((u16)buf[2 * i] << 8) | buf[2 * i + 1];
+			retval = rtsx_write_phy_register(chip, addr + i, val);
+			if (retval != STATUS_SUCCESS) {
+				vfree(buf);
+				set_sense_type(chip, SCSI_LUN(srb),
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return TRANSPORT_FAILED;
+			}
+		}
+
+		vfree(buf);
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int erase_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr;
+	int retval;
+	u8 mode;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	mode = srb->cmnd[3];
+	addr = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+
+	if (mode == 0) {
+		retval = spi_erase_eeprom_chip(chip);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+	} else if (mode == 1) {
+		retval = spi_erase_eeprom_byte(chip, addr);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+	} else {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = spi_read_eeprom(chip, addr + i, buf + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned short addr, len, i;
+	int retval;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
+	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = spi_write_eeprom(chip, addr + i, buf[i]);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int read_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 addr, len, i;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+	len = srb->cmnd[5];
+
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_read_efuse(chip, addr + i, buf + i);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	len = (u8)min_t(unsigned int, scsi_bufflen(srb), len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval, result = TRANSPORT_GOOD;
+	u16 val;
+	u8 addr, len, i;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	addr = srb->cmnd[4];
+	len = srb->cmnd[5];
+
+	len = (u8)min_t(unsigned int, scsi_bufflen(srb), len);
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	retval = rtsx_force_power_on(chip, SSC_PDCTL);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		return TRANSPORT_ERROR;
+	}
+
+	if (chip->asic_code) {
+		retval = rtsx_read_phy_register(chip, 0x08, &val);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			return TRANSPORT_ERROR;
+		}
+
+		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
+					     LDO3318_PWR_MASK, LDO_OFF);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			return TRANSPORT_ERROR;
+		}
+
+		wait_timeout(600);
+
+		retval = rtsx_write_phy_register(chip, 0x08,
+						 0x4C00 | chip->phy_voltage);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			return TRANSPORT_ERROR;
+		}
+
+		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
+					     LDO3318_PWR_MASK, LDO_ON);
+		if (retval != STATUS_SUCCESS) {
+			vfree(buf);
+			return TRANSPORT_ERROR;
+		}
+
+		wait_timeout(600);
+	}
+
+	retval = card_power_on(chip, SPI_CARD);
+	if (retval != STATUS_SUCCESS) {
+		vfree(buf);
+		return TRANSPORT_ERROR;
+	}
+
+	wait_timeout(50);
+
+	for (i = 0; i < len; i++) {
+		retval = rtsx_write_efuse(chip, addr + i, buf[i]);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, SCSI_LUN(srb),
+				       SENSE_TYPE_MEDIA_WRITE_ERR);
+			result = TRANSPORT_FAILED;
+			goto exit;
+		}
+	}
+
+exit:
+	vfree(buf);
+
+	retval = card_power_off(chip, SPI_CARD);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_ERROR;
+
+	if (chip->asic_code) {
+		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
+					     LDO3318_PWR_MASK, LDO_OFF);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_ERROR;
+
+		wait_timeout(600);
+
+		retval = rtsx_write_phy_register(chip, 0x08, val);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_ERROR;
+
+		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
+					     LDO3318_PWR_MASK, LDO_ON);
+		if (retval != STATUS_SUCCESS)
+			return TRANSPORT_ERROR;
+	}
+
+	return result;
+}
+
+static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	bool func_max;
+	u8 func;
+	u16 addr, len;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	func = srb->cmnd[3];
+	addr = ((u16)(srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = ((u16)(srb->cmnd[6]) << 8) | srb->cmnd[7];
+
+	dev_dbg(rtsx_dev(chip), "%s: func = %d, addr = 0x%x, len = %d\n",
+		__func__, func, addr, len);
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
+		func_max = true;
+	else
+		func_max = false;
+
+	if (func > func_max) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	retval = rtsx_read_cfg_seq(chip, func, addr, buf, len);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		vfree(buf);
+		return TRANSPORT_FAILED;
+	}
+
+	len = (u16)min_t(unsigned int, scsi_bufflen(srb), len);
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	bool func_max;
+	u8 func;
+	u16 addr, len;
+	u8 *buf;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	func = srb->cmnd[3];
+	addr = ((u16)(srb->cmnd[4]) << 8) | srb->cmnd[5];
+	len = ((u16)(srb->cmnd[6]) << 8) | srb->cmnd[7];
+
+	dev_dbg(rtsx_dev(chip), "%s: func = %d, addr = 0x%x\n",
+		__func__, func, addr);
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
+		func_max = true;
+	else
+		func_max = false;
+
+	if (func > func_max) {
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
+	buf = vmalloc(len);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	rtsx_stor_get_xfer_buf(buf, len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - len);
+
+	retval = rtsx_write_cfg_seq(chip, func, addr, buf, len);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
+		vfree(buf);
+		return TRANSPORT_FAILED;
+	}
+
+	vfree(buf);
+
+	return TRANSPORT_GOOD;
+}
+
+static int app_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	switch (srb->cmnd[2]) {
+	case PP_READ10:
+	case PP_WRITE10:
+		result = read_write(srb, chip);
+		break;
+
+	case READ_HOST_REG:
+		result = read_host_reg(srb, chip);
+		break;
+
+	case WRITE_HOST_REG:
+		result = write_host_reg(srb, chip);
+		break;
+
+	case GET_VAR:
+		result = get_variable(srb, chip);
+		break;
+
+	case SET_VAR:
+		result = set_variable(srb, chip);
+		break;
+
+	case DMA_READ:
+	case DMA_WRITE:
+		result = dma_access_ring_buffer(srb, chip);
+		break;
+
+	case READ_PHY:
+		result = read_phy_register(srb, chip);
+		break;
+
+	case WRITE_PHY:
+		result = write_phy_register(srb, chip);
+		break;
+
+	case ERASE_EEPROM2:
+		result = erase_eeprom2(srb, chip);
+		break;
+
+	case READ_EEPROM2:
+		result = read_eeprom2(srb, chip);
+		break;
+
+	case WRITE_EEPROM2:
+		result = write_eeprom2(srb, chip);
+		break;
+
+	case READ_EFUSE:
+		result = read_efuse(srb, chip);
+		break;
+
+	case WRITE_EFUSE:
+		result = write_efuse(srb, chip);
+		break;
+
+	case READ_CFG:
+		result = read_cfg_byte(srb, chip);
+		break;
+
+	case WRITE_CFG:
+		result = write_cfg_byte(srb, chip);
+		break;
+
+	case SET_CHIP_MODE:
+		result = set_chip_mode(srb, chip);
+		break;
+
+	case SUIT_CMD:
+		result = suit_cmd(srb, chip);
+		break;
+
+	case GET_DEV_STATUS:
+		result = get_dev_status(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return result;
+}
+
+static int read_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	u8 rtsx_status[16];
+	int buf_len;
+	unsigned int lun = SCSI_LUN(srb);
+
+	rtsx_status[0] = (u8)(chip->vendor_id >> 8);
+	rtsx_status[1] = (u8)(chip->vendor_id);
+
+	rtsx_status[2] = (u8)(chip->product_id >> 8);
+	rtsx_status[3] = (u8)(chip->product_id);
+
+	rtsx_status[4] = (u8)lun;
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (chip->lun2card[lun] == SD_CARD)
+			rtsx_status[5] = 2;
+		else
+			rtsx_status[5] = 3;
+	} else {
+		if (chip->card_exist) {
+			if (chip->card_exist & XD_CARD)
+				rtsx_status[5] = 4;
+			else if (chip->card_exist & SD_CARD)
+				rtsx_status[5] = 2;
+			else if (chip->card_exist & MS_CARD)
+				rtsx_status[5] = 3;
+			else
+				rtsx_status[5] = 7;
+		} else {
+			rtsx_status[5] = 7;
+		}
+	}
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+		rtsx_status[6] = 2;
+	else
+		rtsx_status[6] = 1;
+
+	rtsx_status[7] = (u8)(chip->product_id);
+	rtsx_status[8] = chip->ic_version;
+
+	if (check_card_exist(chip, lun))
+		rtsx_status[9] = 1;
+	else
+		rtsx_status[9] = 0;
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+		rtsx_status[10] = 0;
+	else
+		rtsx_status[10] = 1;
+
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (chip->lun2card[lun] == SD_CARD)
+			rtsx_status[11] = SD_CARD;
+		else
+			rtsx_status[11] = MS_CARD;
+	} else {
+		rtsx_status[11] = XD_CARD | SD_CARD | MS_CARD;
+	}
+
+	if (check_card_ready(chip, lun))
+		rtsx_status[12] = 1;
+	else
+		rtsx_status[12] = 0;
+
+	if (get_lun_card(chip, lun) == XD_CARD) {
+		rtsx_status[13] = 0x40;
+	} else if (get_lun_card(chip, lun) == SD_CARD) {
+		struct sd_info *sd_card = &chip->sd_card;
+
+		rtsx_status[13] = 0x20;
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_HCXC(sd_card))
+				rtsx_status[13] |= 0x04;
+			if (CHK_SD_HS(sd_card))
+				rtsx_status[13] |= 0x02;
+		} else {
+			rtsx_status[13] |= 0x08;
+			if (CHK_MMC_52M(sd_card))
+				rtsx_status[13] |= 0x02;
+			if (CHK_MMC_SECTOR_MODE(sd_card))
+				rtsx_status[13] |= 0x04;
+		}
+	} else if (get_lun_card(chip, lun) == MS_CARD) {
+		struct ms_info *ms_card = &chip->ms_card;
+
+		if (CHK_MSPRO(ms_card)) {
+			rtsx_status[13] = 0x38;
+			if (CHK_HG8BIT(ms_card))
+				rtsx_status[13] |= 0x04;
+#ifdef SUPPORT_MSXC
+			if (CHK_MSXC(ms_card))
+				rtsx_status[13] |= 0x01;
+#endif
+		} else {
+			rtsx_status[13] = 0x30;
+		}
+	} else {
+		if (CHECK_LUN_MODE(chip, DEFAULT_SINGLE)) {
+#ifdef SUPPORT_SDIO
+			if (chip->sd_io && chip->sd_int)
+				rtsx_status[13] = 0x60;
+			else
+				rtsx_status[13] = 0x70;
+#else
+			rtsx_status[13] = 0x70;
+#endif
+		} else {
+			if (chip->lun2card[lun] == SD_CARD)
+				rtsx_status[13] = 0x20;
+			else
+				rtsx_status[13] = 0x30;
+		}
+	}
+
+	rtsx_status[14] = 0x78;
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
+		rtsx_status[15] = 0x83;
+	else
+		rtsx_status[15] = 0x82;
+
+	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(rtsx_status));
+	rtsx_stor_set_xfer_buf(rtsx_status, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	return TRANSPORT_GOOD;
+}
+
+static int get_card_bus_width(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	u8 card, bus_width;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	card = get_lun_card(chip, lun);
+	if (card == SD_CARD || card == MS_CARD) {
+		bus_width = chip->card_bus_width[lun];
+	} else {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	rtsx_stor_set_xfer_buf(&bus_width, scsi_bufflen(srb), srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static int spi_vendor_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 gpio_dir;
+
+	if (CHECK_PID(chip, 0x5208) || CHECK_PID(chip, 0x5288)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	rtsx_force_power_on(chip, SSC_PDCTL);
+
+	rtsx_read_register(chip, CARD_GPIO_DIR, &gpio_dir);
+	rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir & 0x06);
+
+	switch (srb->cmnd[2]) {
+	case SCSI_SPI_GETSTATUS:
+		result = spi_get_status(srb, chip);
+		break;
+
+	case SCSI_SPI_SETPARAMETER:
+		result = spi_set_parameter(srb, chip);
+		break;
+
+	case SCSI_SPI_READFALSHID:
+		result = spi_read_flash_id(srb, chip);
+		break;
+
+	case SCSI_SPI_READFLASH:
+		result = spi_read_flash(srb, chip);
+		break;
+
+	case SCSI_SPI_WRITEFLASH:
+		result = spi_write_flash(srb, chip);
+		break;
+
+	case SCSI_SPI_WRITEFLASHSTATUS:
+		result = spi_write_flash_status(srb, chip);
+		break;
+
+	case SCSI_SPI_ERASEFLASH:
+		result = spi_erase_flash(srb, chip);
+		break;
+
+	default:
+		rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir);
+
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir);
+
+	if (result != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+	return TRANSPORT_GOOD;
+}
+
+static int vendor_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	switch (srb->cmnd[1]) {
+	case READ_STATUS:
+		result = read_status(srb, chip);
+		break;
+
+	case READ_MEM:
+		result = read_mem(srb, chip);
+		break;
+
+	case WRITE_MEM:
+		result = write_mem(srb, chip);
+		break;
+
+	case READ_EEPROM:
+		result = read_eeprom(srb, chip);
+		break;
+
+	case WRITE_EEPROM:
+		result = write_eeprom(srb, chip);
+		break;
+
+	case TOGGLE_GPIO:
+		result = toggle_gpio_cmd(srb, chip);
+		break;
+
+	case GET_SD_CSD:
+		result = get_sd_csd(srb, chip);
+		break;
+
+	case GET_BUS_WIDTH:
+		result = get_card_bus_width(srb, chip);
+		break;
+
+	case SCSI_APP_CMD:
+		result = app_cmd(srb, chip);
+		break;
+
+	case SPI_VENDOR_COMMAND:
+		result = spi_vendor_cmd(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, SCSI_LUN(srb),
+			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return result;
+}
+
+#if !defined(LED_AUTO_BLINK) && !defined(REGULAR_BLINK)
+void led_shine(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	u16 sec_cnt;
+
+	if (srb->cmnd[0] == READ_10 || srb->cmnd[0] == WRITE_10) {
+		sec_cnt = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
+	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
+		sec_cnt = srb->cmnd[4];
+		if (sec_cnt == 0)
+			sec_cnt = 256;
+	} else {
+		return;
+	}
+
+	if (chip->rw_cap[lun] >= GPIO_TOGGLE_THRESHOLD) {
+		toggle_gpio(chip, LED_GPIO);
+		chip->rw_cap[lun] = 0;
+	} else {
+		chip->rw_cap[lun] += sec_cnt;
+	}
+}
+#endif
+
+static int ms_format_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	bool quick_format;
+	int retval;
+
+	if (get_lun_card(chip, lun) != MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[3] != 0x4D || srb->cmnd[4] != 0x47 ||
+	    srb->cmnd[5] != 0x66 || srb->cmnd[6] != 0x6D ||
+		srb->cmnd[7] != 0x74) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+
+		if (!check_card_ready(chip, lun) ||
+		    (get_card_size(chip, lun) == 0)) {
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return TRANSPORT_FAILED;
+		}
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	if (srb->cmnd[8] & 0x01)
+		quick_format = false;
+	else
+		quick_format = true;
+
+	if (!(chip->card_ready & MS_CARD)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (chip->card_wp & MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	retval = mspro_format(srb, chip, MS_SHORT_DATA_LEN, quick_format);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+
+#ifdef SUPPORT_PCGL_1P18
+static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	u8 dev_info_id, data_len;
+	u8 *buf;
+	unsigned int buf_len;
+	int i;
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+	if (get_lun_card(chip, lun) != MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[2] != 0xB0 || srb->cmnd[4] != 0x4D ||
+	    srb->cmnd[5] != 0x53 || srb->cmnd[6] != 0x49 ||
+	    srb->cmnd[7] != 0x44) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	dev_info_id = srb->cmnd[3];
+	if ((CHK_MSXC(ms_card) && dev_info_id == 0x10) ||
+	    (!CHK_MSXC(ms_card) && dev_info_id == 0x13) ||
+	    !CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (dev_info_id == 0x15) {
+		buf_len = 0x3C;
+		data_len = 0x3A;
+	} else {
+		buf_len = 0x6C;
+		data_len = 0x6A;
+	}
+
+	buf = kmalloc(buf_len, GFP_KERNEL);
+	if (!buf)
+		return TRANSPORT_ERROR;
+
+	i = 0;
+	/*  GET Memory Stick Media Information Response Header */
+	buf[i++] = 0x00;		/* Data length MSB */
+	buf[i++] = data_len;		/* Data length LSB */
+	/* Device Information Type Code */
+	if (CHK_MSXC(ms_card))
+		buf[i++] = 0x03;
+	else
+		buf[i++] = 0x02;
+
+	/* SGM bit */
+	buf[i++] = 0x01;
+	/* Reserved */
+	buf[i++] = 0x00;
+	buf[i++] = 0x00;
+	buf[i++] = 0x00;
+	/* Number of Device Information */
+	buf[i++] = 0x01;
+
+	/*  Device Information Body */
+
+	/* Device Information ID Number */
+	buf[i++] = dev_info_id;
+	/* Device Information Length */
+	if (dev_info_id == 0x15)
+		data_len = 0x31;
+	else
+		data_len = 0x61;
+
+	buf[i++] = 0x00;		/* Data length MSB */
+	buf[i++] = data_len;		/* Data length LSB */
+	/* Valid Bit */
+	buf[i++] = 0x80;
+	if (dev_info_id == 0x10 || dev_info_id == 0x13) {
+		/* System Information */
+		memcpy(buf + i, ms_card->raw_sys_info, 96);
+	} else {
+		/* Model Name */
+		memcpy(buf + i, ms_card->raw_model_name, 48);
+	}
+
+	rtsx_stor_set_xfer_buf(buf, buf_len, srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
+
+	kfree(buf);
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int ms_sp_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval = TRANSPORT_ERROR;
+
+	if (srb->cmnd[2] == MS_FORMAT)
+		retval = ms_format_cmnd(srb, chip);
+#ifdef SUPPORT_PCGL_1P18
+	else if (srb->cmnd[2] == GET_MS_INFORMATION)
+		retval = get_ms_information(srb, chip);
+#endif
+
+	return retval;
+}
+
+#ifdef SUPPORT_CPRM
+static int sd_extension_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	unsigned int lun = SCSI_LUN(srb);
+	int result;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	sd_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+	if (get_lun_card(chip, lun) != SD_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	switch (srb->cmnd[0]) {
+	case SD_PASS_THRU_MODE:
+		result = sd_pass_thru_mode(srb, chip);
+		break;
+
+	case SD_EXECUTE_NO_DATA:
+		result = sd_execute_no_data(srb, chip);
+		break;
+
+	case SD_EXECUTE_READ:
+		result = sd_execute_read_data(srb, chip);
+		break;
+
+	case SD_EXECUTE_WRITE:
+		result = sd_execute_write_data(srb, chip);
+		break;
+
+	case SD_GET_RSP:
+		result = sd_get_cmd_rsp(srb, chip);
+		break;
+
+	case SD_HW_RST:
+		result = sd_hw_rst(srb, chip);
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	return result;
+}
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+static int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u8 key_format;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	ms_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+	if (get_lun_card(chip, lun) != MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[7] != KC_MG_R_PRO) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return TRANSPORT_FAILED;
+	}
+
+	key_format = srb->cmnd[10] & 0x3F;
+	dev_dbg(rtsx_dev(chip), "key_format = 0x%x\n", key_format);
+
+	switch (key_format) {
+	case KF_GET_LOC_EKB:
+		if ((scsi_bufflen(srb) == 0x41C) &&
+		    srb->cmnd[8] == 0x04 &&
+		    srb->cmnd[9] == 0x1C) {
+			retval = mg_get_local_EKB(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	case KF_RSP_CHG:
+		if ((scsi_bufflen(srb) == 0x24) &&
+		    srb->cmnd[8] == 0x00 &&
+		    srb->cmnd[9] == 0x24) {
+			retval = mg_get_rsp_chg(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	case KF_GET_ICV:
+		ms_card->mg_entry_num = srb->cmnd[5];
+		if ((scsi_bufflen(srb) == 0x404) &&
+		    srb->cmnd[8] == 0x04 &&
+		    srb->cmnd[9] == 0x04 &&
+		    srb->cmnd[2] == 0x00 &&
+		    srb->cmnd[3] == 0x00 &&
+		    srb->cmnd[4] == 0x00 &&
+		    srb->cmnd[5] < 32) {
+			retval = mg_get_ICV(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+
+static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+	u8 key_format;
+
+	rtsx_disable_aspm(chip);
+
+	if (chip->ss_en && (rtsx_get_stat(chip) == RTSX_STAT_SS)) {
+		rtsx_exit_ss(chip);
+		wait_timeout(100);
+	}
+	rtsx_set_stat(chip, RTSX_STAT_RUN);
+
+	ms_cleanup_work(chip);
+
+	if (!check_card_ready(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return TRANSPORT_FAILED;
+	}
+	if (check_card_wp(chip, lun)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
+		return TRANSPORT_FAILED;
+	}
+	if (get_lun_card(chip, lun) != MS_CARD) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[7] != KC_MG_R_PRO) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (!CHK_MSPRO(ms_card)) {
+		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
+		return TRANSPORT_FAILED;
+	}
+
+	key_format = srb->cmnd[10] & 0x3F;
+	dev_dbg(rtsx_dev(chip), "key_format = 0x%x\n", key_format);
+
+	switch (key_format) {
+	case KF_SET_LEAF_ID:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    srb->cmnd[8] == 0x00 &&
+		    srb->cmnd[9] == 0x0C) {
+			retval = mg_set_leaf_id(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	case KF_CHG_HOST:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    srb->cmnd[8] == 0x00 &&
+		    srb->cmnd[9] == 0x0C) {
+			retval = mg_chg(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	case KF_RSP_HOST:
+		if ((scsi_bufflen(srb) == 0x0C) &&
+		    srb->cmnd[8] == 0x00 &&
+		    srb->cmnd[9] == 0x0C) {
+			retval = mg_rsp(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	case KF_SET_ICV:
+		ms_card->mg_entry_num = srb->cmnd[5];
+		if ((scsi_bufflen(srb) == 0x404) &&
+		    srb->cmnd[8] == 0x04 &&
+		    srb->cmnd[9] == 0x04 &&
+		    srb->cmnd[2] == 0x00 &&
+		    srb->cmnd[3] == 0x00 &&
+		    srb->cmnd[4] == 0x00 &&
+		    srb->cmnd[5] < 32) {
+			retval = mg_set_ICV(srb, chip);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+#endif
+
+int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_SD_LOCK
+	struct sd_info *sd_card = &chip->sd_card;
+#endif
+	struct ms_info *ms_card = &chip->ms_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int result;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_erase_status) {
+		/* Block all SCSI command except for
+		 * REQUEST_SENSE and rs_ppstatus
+		 */
+		if (!(srb->cmnd[0] == VENDOR_CMND &&
+		      srb->cmnd[1] == SCSI_APP_CMD &&
+		      srb->cmnd[2] == GET_DEV_STATUS) &&
+			srb->cmnd[0] != REQUEST_SENSE) {
+			/* Logical Unit Not Ready Format in Progress */
+			set_sense_data(chip, lun, CUR_ERR,
+				       0x02, 0, 0x04, 0x04, 0, 0);
+			return TRANSPORT_FAILED;
+		}
+	}
+#endif
+
+	if ((get_lun_card(chip, lun) == MS_CARD) &&
+	    ms_card->format_status == FORMAT_IN_PROGRESS) {
+		if (srb->cmnd[0] != REQUEST_SENSE &&
+		    srb->cmnd[0] != INQUIRY) {
+			/* Logical Unit Not Ready Format in Progress */
+			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
+				       0, (u16)(ms_card->progress));
+			return TRANSPORT_FAILED;
+		}
+	}
+
+	switch (srb->cmnd[0]) {
+	case READ_10:
+	case WRITE_10:
+	case READ_6:
+	case WRITE_6:
+		result = read_write(srb, chip);
+#if !defined(LED_AUTO_BLINK) && !defined(REGULAR_BLINK)
+		led_shine(srb, chip);
+#endif
+		break;
+
+	case TEST_UNIT_READY:
+		result = test_unit_ready(srb, chip);
+		break;
+
+	case INQUIRY:
+		result = inquiry(srb, chip);
+		break;
+
+	case READ_CAPACITY:
+		result = read_capacity(srb, chip);
+		break;
+
+	case START_STOP:
+		result = start_stop_unit(srb, chip);
+		break;
+
+	case ALLOW_MEDIUM_REMOVAL:
+		result = allow_medium_removal(srb, chip);
+		break;
+
+	case REQUEST_SENSE:
+		result = request_sense(srb, chip);
+		break;
+
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+		result = mode_sense(srb, chip);
+		break;
+
+	case 0x23:
+		result = read_format_capacity(srb, chip);
+		break;
+
+	case VENDOR_CMND:
+		result = vendor_cmnd(srb, chip);
+		break;
+
+	case MS_SP_CMND:
+		result = ms_sp_cmnd(srb, chip);
+		break;
+
+#ifdef SUPPORT_CPRM
+	case SD_PASS_THRU_MODE:
+	case SD_EXECUTE_NO_DATA:
+	case SD_EXECUTE_READ:
+	case SD_EXECUTE_WRITE:
+	case SD_GET_RSP:
+	case SD_HW_RST:
+		result = sd_extension_cmnd(srb, chip);
+		break;
+#endif
+
+#ifdef SUPPORT_MAGIC_GATE
+	case CMD_MSPRO_MG_RKEY:
+		result = mg_report_key(srb, chip);
+		break;
+
+	case CMD_MSPRO_MG_SKEY:
+		result = mg_send_key(srb, chip);
+		break;
+#endif
+
+	case FORMAT_UNIT:
+	case MODE_SELECT:
+	case VERIFY:
+		result = TRANSPORT_GOOD;
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		result = TRANSPORT_FAILED;
+	}
+
+	return result;
+}
diff --git a/drivers/staging/rts5208/rtsx_scsi.h b/drivers/staging/rts5208/rtsx_scsi.h
new file mode 100644
index 000000000..df6138c97
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_scsi.h
@@ -0,0 +1,131 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_SCSI_H
+#define __REALTEK_RTSX_SCSI_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+
+#define MS_SP_CMND		0xFA
+#define MS_FORMAT		0xA0
+#define GET_MS_INFORMATION	0xB0
+
+#define VENDOR_CMND		0xF0
+
+#define READ_STATUS		0x09
+
+#define READ_EEPROM		0x04
+#define WRITE_EEPROM		0x05
+#define READ_MEM		0x0D
+#define WRITE_MEM		0x0E
+#define GET_BUS_WIDTH		0x13
+#define GET_SD_CSD		0x14
+#define TOGGLE_GPIO		0x15
+#define TRACE_MSG		0x18
+
+#define SCSI_APP_CMD		0x10
+
+#define PP_READ10		0x1A
+#define PP_WRITE10		0x0A
+#define READ_HOST_REG		0x1D
+#define WRITE_HOST_REG		0x0D
+#define SET_VAR			0x05
+#define GET_VAR			0x15
+#define DMA_READ		0x16
+#define DMA_WRITE		0x06
+#define GET_DEV_STATUS		0x10
+#define SET_CHIP_MODE		0x27
+#define SUIT_CMD		0xE0
+#define WRITE_PHY		0x07
+#define READ_PHY		0x17
+#define WRITE_EEPROM2		0x03
+#define READ_EEPROM2		0x13
+#define ERASE_EEPROM2		0x23
+#define WRITE_EFUSE		0x04
+#define READ_EFUSE		0x14
+#define WRITE_CFG		0x0E
+#define READ_CFG		0x1E
+
+#define SPI_VENDOR_COMMAND		0x1C
+
+#define	SCSI_SPI_GETSTATUS		0x00
+#define	SCSI_SPI_SETPARAMETER		0x01
+#define	SCSI_SPI_READFALSHID		0x02
+#define	SCSI_SPI_READFLASH		0x03
+#define	SCSI_SPI_WRITEFLASH		0x04
+#define	SCSI_SPI_WRITEFLASHSTATUS	0x05
+#define	SCSI_SPI_ERASEFLASH		0x06
+
+#define INIT_BATCHCMD		0x41
+#define ADD_BATCHCMD		0x42
+#define SEND_BATCHCMD		0x43
+#define GET_BATCHRSP		0x44
+
+#define CHIP_NORMALMODE		0x00
+#define CHIP_DEBUGMODE		0x01
+
+/* SD Pass Through Command Extension */
+#define SD_PASS_THRU_MODE	0xD0
+#define SD_EXECUTE_NO_DATA	0xD1
+#define SD_EXECUTE_READ		0xD2
+#define SD_EXECUTE_WRITE	0xD3
+#define SD_GET_RSP		0xD4
+#define SD_HW_RST		0xD6
+
+#ifdef SUPPORT_MAGIC_GATE
+#define CMD_MSPRO_MG_RKEY	0xA4   /* Report Key Command */
+#define CMD_MSPRO_MG_SKEY	0xA3   /* Send Key Command */
+
+/* CBWCB field: key class */
+#define KC_MG_R_PRO		0xBE   /* MG-R PRO*/
+
+/* CBWCB field: key format */
+#define KF_SET_LEAF_ID		0x31   /* Set Leaf ID */
+#define KF_GET_LOC_EKB		0x32   /* Get Local EKB */
+#define KF_CHG_HOST		0x33   /* Challenge (host) */
+#define KF_RSP_CHG		0x34   /* Response and Challenge (device)  */
+#define KF_RSP_HOST		0x35   /* Response (host) */
+#define KF_GET_ICV		0x36   /* Get ICV */
+#define KF_SET_ICV		0x37   /* SSet ICV */
+#endif
+
+/* Sense type */
+#define	SENSE_TYPE_NO_SENSE				0
+#define	SENSE_TYPE_MEDIA_CHANGE				1
+#define	SENSE_TYPE_MEDIA_NOT_PRESENT			2
+#define	SENSE_TYPE_MEDIA_LBA_OVER_RANGE			3
+#define	SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT		4
+#define	SENSE_TYPE_MEDIA_WRITE_PROTECT			5
+#define	SENSE_TYPE_MEDIA_INVALID_CMD_FIELD		6
+#define	SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR		7
+#define	SENSE_TYPE_MEDIA_WRITE_ERR			8
+#define SENSE_TYPE_FORMAT_IN_PROGRESS			9
+#define SENSE_TYPE_FORMAT_CMD_FAILED			10
+#ifdef SUPPORT_MAGIC_GATE
+#define SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB		0x0b
+#define SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN		0x0c
+#define SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM		0x0d
+#define SENSE_TYPE_MG_WRITE_ERR				0x0e
+#endif
+#ifdef SUPPORT_SD_LOCK
+/* FOR Locked SD card*/
+#define SENSE_TYPE_MEDIA_READ_FORBIDDEN			0x10
+#endif
+
+void scsi_show_command(struct rtsx_chip *chip);
+void set_sense_type(struct rtsx_chip *chip, unsigned int lun, int sense_type);
+void set_sense_data(struct rtsx_chip *chip, unsigned int lun, u8 err_code,
+		    u8 sense_key, u32 info, u8 asc, u8 ascq,
+		    u8 sns_key_info0, u16 sns_key_info1);
+int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+
+#endif   /* __REALTEK_RTSX_SCSI_H */
diff --git a/drivers/staging/rts5208/rtsx_sys.h b/drivers/staging/rts5208/rtsx_sys.h
new file mode 100644
index 000000000..77094809c
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_sys.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __RTSX_SYS_H
+#define __RTSX_SYS_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+#include "rtsx_card.h"
+
+static inline void rtsx_exclusive_enter_ss(struct rtsx_chip *chip)
+{
+	struct rtsx_dev *dev = chip->rtsx;
+
+	spin_lock(&dev->reg_lock);
+	rtsx_enter_ss(chip);
+	spin_unlock(&dev->reg_lock);
+}
+
+static inline void rtsx_reset_detected_cards(struct rtsx_chip *chip, int flag)
+{
+	rtsx_reset_cards(chip);
+}
+
+#define RTSX_MSG_IN_INT(x)
+
+#endif  /* __RTSX_SYS_H */
+
diff --git a/drivers/staging/rts5208/rtsx_transport.c b/drivers/staging/rts5208/rtsx_transport.c
new file mode 100644
index 000000000..d5ad49de4
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_transport.c
@@ -0,0 +1,768 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+
+/***********************************************************************
+ * Scatter-gather transfer buffer access routines
+ ***********************************************************************/
+
+/*
+ * Copy a buffer of length buflen to/from the srb's transfer buffer.
+ * (Note: for scatter-gather transfers (srb->use_sg > 0), srb->request_buffer
+ * points to a list of s-g entries and we ignore srb->request_bufflen.
+ * For non-scatter-gather transfers, srb->request_buffer points to the
+ * transfer buffer itself and srb->request_bufflen is the buffer's length.)
+ * Update the *index and *offset variables so that the next copy will
+ * pick up from where this one left off.
+ */
+
+unsigned int rtsx_stor_access_xfer_buf(unsigned char *buffer,
+				       unsigned int buflen,
+				       struct scsi_cmnd *srb,
+				       unsigned int *index,
+				       unsigned int *offset,
+				       enum xfer_buf_dir dir)
+{
+	unsigned int cnt;
+
+	/* If not using scatter-gather, just transfer the data directly. */
+	if (scsi_sg_count(srb) == 0) {
+		unsigned char *sgbuffer;
+
+		if (*offset >= scsi_bufflen(srb))
+			return 0;
+		cnt = min(buflen, scsi_bufflen(srb) - *offset);
+
+		sgbuffer = (unsigned char *)scsi_sglist(srb) + *offset;
+
+		if (dir == TO_XFER_BUF)
+			memcpy(sgbuffer, buffer, cnt);
+		else
+			memcpy(buffer, sgbuffer, cnt);
+		*offset += cnt;
+
+	/*
+	 * Using scatter-gather. We have to go through the list one entry
+	 * at a time. Each s-g entry contains some number of pages which
+	 * have to be copied one at a time.
+	 */
+	} else {
+		struct scatterlist *sg =
+				(struct scatterlist *)scsi_sglist(srb)
+				+ *index;
+
+		/*
+		 * This loop handles a single s-g list entry, which may
+		 * include multiple pages.  Find the initial page structure
+		 * and the starting offset within the page, and update
+		 * the *offset and *index values for the next loop.
+		 */
+		cnt = 0;
+		while (cnt < buflen && *index < scsi_sg_count(srb)) {
+			struct page *page = sg_page(sg) +
+					((sg->offset + *offset) >> PAGE_SHIFT);
+			unsigned int poff = (sg->offset + *offset) &
+					    (PAGE_SIZE - 1);
+			unsigned int sglen = sg->length - *offset;
+
+			if (sglen > buflen - cnt) {
+				/* Transfer ends within this s-g entry */
+				sglen = buflen - cnt;
+				*offset += sglen;
+			} else {
+				/* Transfer continues to next s-g entry */
+				*offset = 0;
+				++*index;
+				++sg;
+			}
+
+			while (sglen > 0) {
+				unsigned int plen = min(sglen, (unsigned int)
+						PAGE_SIZE - poff);
+
+				if (dir == TO_XFER_BUF)
+					memcpy_to_page(page, poff, buffer + cnt, plen);
+				else
+					memcpy_from_page(buffer + cnt, page, poff, plen);
+
+				/* Start at the beginning of the next page */
+				poff = 0;
+				++page;
+				cnt += plen;
+				sglen -= plen;
+			}
+		}
+	}
+
+	/* Return the amount actually transferred */
+	return cnt;
+}
+
+/*
+ * Store the contents of buffer into srb's transfer buffer and set the
+ * SCSI residue.
+ */
+void rtsx_stor_set_xfer_buf(unsigned char *buffer,
+			    unsigned int buflen, struct scsi_cmnd *srb)
+{
+	unsigned int index = 0, offset = 0;
+
+	rtsx_stor_access_xfer_buf(buffer, buflen, srb, &index, &offset,
+				  TO_XFER_BUF);
+	if (buflen < scsi_bufflen(srb))
+		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
+}
+
+void rtsx_stor_get_xfer_buf(unsigned char *buffer,
+			    unsigned int buflen, struct scsi_cmnd *srb)
+{
+	unsigned int index = 0, offset = 0;
+
+	rtsx_stor_access_xfer_buf(buffer, buflen, srb, &index, &offset,
+				  FROM_XFER_BUF);
+	if (buflen < scsi_bufflen(srb))
+		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
+}
+
+/***********************************************************************
+ * Transport routines
+ ***********************************************************************/
+
+/*
+ * Invoke the transport and basic error-handling/recovery methods
+ *
+ * This is used to send the message to the device and receive the response.
+ */
+void rtsx_invoke_transport(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int result;
+
+	result = rtsx_scsi_handler(srb, chip);
+
+	/*
+	 * if the command gets aborted by the higher layers, we need to
+	 * short-circuit all other processing.
+	 */
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
+		dev_dbg(rtsx_dev(chip), "-- command was aborted\n");
+		srb->result = DID_ABORT << 16;
+		goto handle_errors;
+	}
+
+	/* if there is a transport error, reset and don't auto-sense */
+	if (result == TRANSPORT_ERROR) {
+		dev_dbg(rtsx_dev(chip), "-- transport indicates error, resetting\n");
+		srb->result = DID_ERROR << 16;
+		goto handle_errors;
+	}
+
+	srb->result = SAM_STAT_GOOD;
+
+	/*
+	 * If we have a failure, we're going to do a REQUEST_SENSE
+	 * automatically.  Note that we differentiate between a command
+	 * "failure" and an "error" in the transport mechanism.
+	 */
+	if (result == TRANSPORT_FAILED) {
+		/* set the result so the higher layers expect this data */
+		srb->result = SAM_STAT_CHECK_CONDITION;
+		memcpy(srb->sense_buffer,
+		       (unsigned char *)&chip->sense_buffer[SCSI_LUN(srb)],
+		       sizeof(struct sense_data_t));
+	}
+
+	return;
+
+handle_errors:
+	return;
+}
+
+void rtsx_add_cmd(struct rtsx_chip *chip,
+		  u8 cmd_type, u16 reg_addr, u8 mask, u8 data)
+{
+	__le32 *cb = (__le32 *)(chip->host_cmds_ptr);
+	u32 val = 0;
+
+	val |= (u32)(cmd_type & 0x03) << 30;
+	val |= (u32)(reg_addr & 0x3FFF) << 16;
+	val |= (u32)mask << 8;
+	val |= (u32)data;
+
+	spin_lock_irq(&chip->rtsx->reg_lock);
+	if (chip->ci < (HOST_CMDS_BUF_LEN / 4))
+		cb[(chip->ci)++] = cpu_to_le32(val);
+
+	spin_unlock_irq(&chip->rtsx->reg_lock);
+}
+
+void rtsx_send_cmd_no_wait(struct rtsx_chip *chip)
+{
+	u32 val = BIT(31);
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	val |= (u32)(chip->ci * 4) & 0x00FFFFFF;
+	/* Hardware Auto Response */
+	val |= 0x40000000;
+	rtsx_writel(chip, RTSX_HCBCTLR, val);
+}
+
+int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u32 val = BIT(31);
+	long timeleft;
+	int err = 0;
+
+	if (card == SD_CARD)
+		rtsx->check_card_cd = SD_EXIST;
+	else if (card == MS_CARD)
+		rtsx->check_card_cd = MS_EXIST;
+	else if (card == XD_CARD)
+		rtsx->check_card_cd = XD_EXIST;
+	else
+		rtsx->check_card_cd = 0;
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	/* set up data structures for the wakeup system */
+	rtsx->done = &trans_done;
+	rtsx->trans_result = TRANS_NOT_READY;
+	init_completion(&trans_done);
+	rtsx->trans_state = STATE_TRANS_CMD;
+
+	rtsx_writel(chip, RTSX_HCBAR, chip->host_cmds_addr);
+
+	val |= (u32)(chip->ci * 4) & 0x00FFFFFF;
+	/* Hardware Auto Response */
+	val |= 0x40000000;
+	rtsx_writel(chip, RTSX_HCBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	/* Wait for TRANS_OK_INT */
+	timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+							     msecs_to_jiffies(timeout));
+	if (timeleft <= 0) {
+		dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+			chip->int_reg);
+		err = -ETIMEDOUT;
+		goto finish_send_cmd;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
+		err = -EIO;
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
+		err = 0;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+finish_send_cmd:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+
+	if (err < 0)
+		rtsx_stop_cmd(chip, card);
+
+	return err;
+}
+
+static inline void rtsx_add_sg_tbl(struct rtsx_chip *chip,
+				   u32 addr, u32 len, u8 option)
+{
+	__le64 *sgb = (__le64 *)(chip->host_sg_tbl_ptr);
+	u64 val = 0;
+	u32 temp_len = 0;
+	u8  temp_opt = 0;
+
+	do {
+		if (len > 0x80000) {
+			temp_len = 0x80000;
+			temp_opt = option & (~RTSX_SG_END);
+		} else {
+			temp_len = len;
+			temp_opt = option;
+		}
+		val = ((u64)addr << 32) | ((u64)temp_len << 12) | temp_opt;
+
+		if (chip->sgi < (HOST_SG_TBL_BUF_LEN / 8))
+			sgb[(chip->sgi)++] = cpu_to_le64(val);
+
+		len -= temp_len;
+		addr += temp_len;
+	} while (len);
+}
+
+static int rtsx_transfer_sglist_adma_partial(struct rtsx_chip *chip, u8 card,
+					     struct scatterlist *sg, int num_sg,
+					     unsigned int *index,
+					     unsigned int *offset, int size,
+					     enum dma_data_direction dma_dir,
+					     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u8 dir;
+	int sg_cnt, i, resid;
+	int err = 0;
+	long timeleft;
+	struct scatterlist *sg_ptr;
+	u32 val = TRIG_DMA;
+
+	if (!sg || num_sg <= 0 || !offset || !index)
+		return -EIO;
+
+	if (dma_dir == DMA_TO_DEVICE)
+		dir = HOST_TO_DEVICE;
+	else if (dma_dir == DMA_FROM_DEVICE)
+		dir = DEVICE_TO_HOST;
+	else
+		return -ENXIO;
+
+	if (card == SD_CARD)
+		rtsx->check_card_cd = SD_EXIST;
+	else if (card == MS_CARD)
+		rtsx->check_card_cd = MS_EXIST;
+	else if (card == XD_CARD)
+		rtsx->check_card_cd = XD_EXIST;
+	else
+		rtsx->check_card_cd = 0;
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	/* set up data structures for the wakeup system */
+	rtsx->done = &trans_done;
+
+	rtsx->trans_state = STATE_TRANS_SG;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	sg_cnt = dma_map_sg(&rtsx->pci->dev, sg, num_sg, dma_dir);
+
+	resid = size;
+	sg_ptr = sg;
+	chip->sgi = 0;
+	/*
+	 * Usually the next entry will be @sg@ + 1, but if this sg element
+	 * is part of a chained scatterlist, it could jump to the start of
+	 * a new scatterlist array. So here we use sg_next to move to
+	 * the proper sg.
+	 */
+	for (i = 0; i < *index; i++)
+		sg_ptr = sg_next(sg_ptr);
+	for (i = *index; i < sg_cnt; i++) {
+		dma_addr_t addr;
+		unsigned int len;
+		u8 option;
+
+		addr = sg_dma_address(sg_ptr);
+		len = sg_dma_len(sg_ptr);
+
+		dev_dbg(rtsx_dev(chip), "DMA addr: 0x%x, Len: 0x%x\n",
+			(unsigned int)addr, len);
+		dev_dbg(rtsx_dev(chip), "*index = %d, *offset = %d\n",
+			*index, *offset);
+
+		addr += *offset;
+
+		if ((len - *offset) > resid) {
+			*offset += resid;
+			len = resid;
+			resid = 0;
+		} else {
+			resid -= (len - *offset);
+			len -= *offset;
+			*offset = 0;
+			*index = *index + 1;
+		}
+		option = RTSX_SG_VALID | RTSX_SG_TRANS_DATA;
+		if ((i == sg_cnt - 1) || !resid)
+			option |= RTSX_SG_END;
+
+		rtsx_add_sg_tbl(chip, (u32)addr, (u32)len, option);
+
+		if (!resid)
+			break;
+
+		sg_ptr = sg_next(sg_ptr);
+	}
+
+	dev_dbg(rtsx_dev(chip), "SG table count = %d\n", chip->sgi);
+
+	val |= (u32)(dir & 0x01) << 29;
+	val |= ADMA_MODE;
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	init_completion(&trans_done);
+
+	rtsx_writel(chip, RTSX_HDBAR, chip->host_sg_tbl_addr);
+	rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+							     msecs_to_jiffies(timeout));
+	if (timeleft <= 0) {
+		dev_dbg(rtsx_dev(chip), "Timeout (%s %d)\n",
+			__func__, __LINE__);
+		dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+			chip->int_reg);
+		err = -ETIMEDOUT;
+		goto out;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+		err = -EIO;
+		spin_unlock_irq(&rtsx->reg_lock);
+		goto out;
+	}
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	/* Wait for TRANS_OK_INT */
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_NOT_READY) {
+		init_completion(&trans_done);
+		spin_unlock_irq(&rtsx->reg_lock);
+		timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+								     msecs_to_jiffies(timeout));
+		if (timeleft <= 0) {
+			dev_dbg(rtsx_dev(chip), "Timeout (%s %d)\n",
+				__func__, __LINE__);
+			dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+				chip->int_reg);
+			err = -ETIMEDOUT;
+			goto out;
+		}
+	} else {
+		spin_unlock_irq(&rtsx->reg_lock);
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
+		err = -EIO;
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
+		err = 0;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_sg(&rtsx->pci->dev, sg, num_sg, dma_dir);
+
+	if (err < 0)
+		rtsx_stop_cmd(chip, card);
+
+	return err;
+}
+
+static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
+				     struct scatterlist *sg, int num_sg,
+				     enum dma_data_direction dma_dir,
+				     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	u8 dir;
+	int buf_cnt, i;
+	int err = 0;
+	long timeleft;
+	struct scatterlist *sg_ptr;
+
+	if (!sg || num_sg <= 0)
+		return -EIO;
+
+	if (dma_dir == DMA_TO_DEVICE)
+		dir = HOST_TO_DEVICE;
+	else if (dma_dir == DMA_FROM_DEVICE)
+		dir = DEVICE_TO_HOST;
+	else
+		return -ENXIO;
+
+	if (card == SD_CARD)
+		rtsx->check_card_cd = SD_EXIST;
+	else if (card == MS_CARD)
+		rtsx->check_card_cd = MS_EXIST;
+	else if (card == XD_CARD)
+		rtsx->check_card_cd = XD_EXIST;
+	else
+		rtsx->check_card_cd = 0;
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	/* set up data structures for the wakeup system */
+	rtsx->done = &trans_done;
+
+	rtsx->trans_state = STATE_TRANS_SG;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	buf_cnt = dma_map_sg(&rtsx->pci->dev, sg, num_sg, dma_dir);
+
+	sg_ptr = sg;
+
+	for (i = 0; i <= buf_cnt / (HOST_SG_TBL_BUF_LEN / 8); i++) {
+		u32 val = TRIG_DMA;
+		int sg_cnt, j;
+
+		if (i == buf_cnt / (HOST_SG_TBL_BUF_LEN / 8))
+			sg_cnt = buf_cnt % (HOST_SG_TBL_BUF_LEN / 8);
+		else
+			sg_cnt = HOST_SG_TBL_BUF_LEN / 8;
+
+		chip->sgi = 0;
+		for (j = 0; j < sg_cnt; j++) {
+			dma_addr_t addr = sg_dma_address(sg_ptr);
+			unsigned int len = sg_dma_len(sg_ptr);
+			u8 option;
+
+			dev_dbg(rtsx_dev(chip), "DMA addr: 0x%x, Len: 0x%x\n",
+				(unsigned int)addr, len);
+
+			option = RTSX_SG_VALID | RTSX_SG_TRANS_DATA;
+			if (j == (sg_cnt - 1))
+				option |= RTSX_SG_END;
+
+			rtsx_add_sg_tbl(chip, (u32)addr, (u32)len, option);
+
+			sg_ptr = sg_next(sg_ptr);
+		}
+
+		dev_dbg(rtsx_dev(chip), "SG table count = %d\n", chip->sgi);
+
+		val |= (u32)(dir & 0x01) << 29;
+		val |= ADMA_MODE;
+
+		spin_lock_irq(&rtsx->reg_lock);
+
+		init_completion(&trans_done);
+
+		rtsx_writel(chip, RTSX_HDBAR, chip->host_sg_tbl_addr);
+		rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+		spin_unlock_irq(&rtsx->reg_lock);
+
+		timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+								     msecs_to_jiffies(timeout));
+		if (timeleft <= 0) {
+			dev_dbg(rtsx_dev(chip), "Timeout (%s %d)\n",
+				__func__, __LINE__);
+			dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+				chip->int_reg);
+			err = -ETIMEDOUT;
+			goto out;
+		}
+
+		spin_lock_irq(&rtsx->reg_lock);
+		if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+			err = -EIO;
+			spin_unlock_irq(&rtsx->reg_lock);
+			goto out;
+		}
+		spin_unlock_irq(&rtsx->reg_lock);
+
+		sg_ptr += sg_cnt;
+	}
+
+	/* Wait for TRANS_OK_INT */
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_NOT_READY) {
+		init_completion(&trans_done);
+		spin_unlock_irq(&rtsx->reg_lock);
+		timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+								     msecs_to_jiffies(timeout));
+		if (timeleft <= 0) {
+			dev_dbg(rtsx_dev(chip), "Timeout (%s %d)\n",
+				__func__, __LINE__);
+			dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+				chip->int_reg);
+			err = -ETIMEDOUT;
+			goto out;
+		}
+	} else {
+		spin_unlock_irq(&rtsx->reg_lock);
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
+		err = -EIO;
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
+		err = 0;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_sg(&rtsx->pci->dev, sg, num_sg, dma_dir);
+
+	if (err < 0)
+		rtsx_stop_cmd(chip, card);
+
+	return err;
+}
+
+static int rtsx_transfer_buf(struct rtsx_chip *chip, u8 card, void *buf,
+			     size_t len, enum dma_data_direction dma_dir,
+			     int timeout)
+{
+	struct rtsx_dev *rtsx = chip->rtsx;
+	struct completion trans_done;
+	dma_addr_t addr;
+	u8 dir;
+	int err = 0;
+	u32 val = BIT(31);
+	long timeleft;
+
+	if (!buf || len <= 0)
+		return -EIO;
+
+	if (dma_dir == DMA_TO_DEVICE)
+		dir = HOST_TO_DEVICE;
+	else if (dma_dir == DMA_FROM_DEVICE)
+		dir = DEVICE_TO_HOST;
+	else
+		return -ENXIO;
+
+	addr = dma_map_single(&rtsx->pci->dev, buf, len, dma_dir);
+	if (dma_mapping_error(&rtsx->pci->dev, addr))
+		return -ENOMEM;
+
+	if (card == SD_CARD)
+		rtsx->check_card_cd = SD_EXIST;
+	else if (card == MS_CARD)
+		rtsx->check_card_cd = MS_EXIST;
+	else if (card == XD_CARD)
+		rtsx->check_card_cd = XD_EXIST;
+	else
+		rtsx->check_card_cd = 0;
+
+	val |= (u32)(dir & 0x01) << 29;
+	val |= (u32)(len & 0x00FFFFFF);
+
+	spin_lock_irq(&rtsx->reg_lock);
+
+	/* set up data structures for the wakeup system */
+	rtsx->done = &trans_done;
+
+	init_completion(&trans_done);
+
+	rtsx->trans_state = STATE_TRANS_BUF;
+	rtsx->trans_result = TRANS_NOT_READY;
+
+	rtsx_writel(chip, RTSX_HDBAR, addr);
+	rtsx_writel(chip, RTSX_HDBCTLR, val);
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+	/* Wait for TRANS_OK_INT */
+	timeleft = wait_for_completion_interruptible_timeout(&trans_done,
+							     msecs_to_jiffies(timeout));
+	if (timeleft <= 0) {
+		dev_dbg(rtsx_dev(chip), "Timeout (%s %d)\n",
+			__func__, __LINE__);
+		dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
+			chip->int_reg);
+		err = -ETIMEDOUT;
+		goto out;
+	}
+
+	spin_lock_irq(&rtsx->reg_lock);
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
+		err = -EIO;
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
+		err = 0;
+
+	spin_unlock_irq(&rtsx->reg_lock);
+
+out:
+	rtsx->done = NULL;
+	rtsx->trans_state = STATE_TRANS_NONE;
+	dma_unmap_single(&rtsx->pci->dev, addr, len, dma_dir);
+
+	if (err < 0)
+		rtsx_stop_cmd(chip, card);
+
+	return err;
+}
+
+int rtsx_transfer_data_partial(struct rtsx_chip *chip, u8 card,
+			       void *buf, size_t len, int use_sg,
+			       unsigned int *index, unsigned int *offset,
+			       enum dma_data_direction dma_dir, int timeout)
+{
+	int err = 0;
+
+	/* don't transfer data during abort processing */
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT))
+		return -EIO;
+
+	if (use_sg) {
+		struct scatterlist *sg = buf;
+
+		err = rtsx_transfer_sglist_adma_partial(chip, card, sg, use_sg,
+							index, offset, (int)len,
+							dma_dir, timeout);
+	} else {
+		err = rtsx_transfer_buf(chip, card,
+					buf, len, dma_dir, timeout);
+	}
+	if (err < 0) {
+		if (RTSX_TST_DELINK(chip)) {
+			RTSX_CLR_DELINK(chip);
+			chip->need_reinit = SD_CARD | MS_CARD | XD_CARD;
+			rtsx_reinit_cards(chip, 1);
+		}
+	}
+
+	return err;
+}
+
+int rtsx_transfer_data(struct rtsx_chip *chip, u8 card, void *buf, size_t len,
+		       int use_sg, enum dma_data_direction dma_dir, int timeout)
+{
+	int err = 0;
+
+	dev_dbg(rtsx_dev(chip), "use_sg = %d\n", use_sg);
+
+	/* don't transfer data during abort processing */
+	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT))
+		return -EIO;
+
+	if (use_sg) {
+		err = rtsx_transfer_sglist_adma(chip, card, buf,
+						use_sg, dma_dir, timeout);
+	} else {
+		err = rtsx_transfer_buf(chip, card, buf, len, dma_dir, timeout);
+	}
+
+	if (err < 0) {
+		if (RTSX_TST_DELINK(chip)) {
+			RTSX_CLR_DELINK(chip);
+			chip->need_reinit = SD_CARD | MS_CARD | XD_CARD;
+			rtsx_reinit_cards(chip, 1);
+		}
+	}
+
+	return err;
+}
+
diff --git a/drivers/staging/rts5208/rtsx_transport.h b/drivers/staging/rts5208/rtsx_transport.h
new file mode 100644
index 000000000..097efed24
--- /dev/null
+++ b/drivers/staging/rts5208/rtsx_transport.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_TRANSPORT_H
+#define __REALTEK_RTSX_TRANSPORT_H
+
+#include "rtsx.h"
+#include "rtsx_chip.h"
+
+#define WAIT_TIME	2000
+
+unsigned int rtsx_stor_access_xfer_buf(unsigned char *buffer,
+				       unsigned int buflen,
+				       struct scsi_cmnd *srb,
+				       unsigned int *index,
+				       unsigned int *offset,
+				       enum xfer_buf_dir dir);
+void rtsx_stor_set_xfer_buf(unsigned char *buffer, unsigned int buflen,
+			    struct scsi_cmnd *srb);
+void rtsx_stor_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
+			    struct scsi_cmnd *srb);
+void rtsx_invoke_transport(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+
+#define rtsx_init_cmd(chip)			((chip)->ci = 0)
+
+void rtsx_add_cmd(struct rtsx_chip *chip, u8 cmd_type, u16 reg_addr, u8 mask,
+		  u8 data);
+void rtsx_send_cmd_no_wait(struct rtsx_chip *chip);
+int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout);
+
+static inline u8 *rtsx_get_cmd_data(struct rtsx_chip *chip)
+{
+#ifdef CMD_USING_SG
+	return (u8 *)(chip->host_sg_tbl_ptr);
+#else
+	return (u8 *)(chip->host_cmds_ptr);
+#endif
+}
+
+int rtsx_transfer_data(struct rtsx_chip *chip, u8 card, void *buf, size_t len,
+		       int use_sg, enum dma_data_direction dma_dir,
+		       int timeout);
+
+int rtsx_transfer_data_partial(struct rtsx_chip *chip, u8 card,	void *buf,
+			       size_t len, int use_sg, unsigned int *index,
+			       unsigned int *offset,
+			       enum dma_data_direction dma_dir, int timeout);
+
+#endif   /* __REALTEK_RTSX_TRANSPORT_H */
diff --git a/drivers/staging/rts5208/sd.c b/drivers/staging/rts5208/sd.c
new file mode 100644
index 000000000..d1fafd530
--- /dev/null
+++ b/drivers/staging/rts5208/sd.c
@@ -0,0 +1,4717 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "sd.h"
+
+#define SD_MAX_RETRY_COUNT	3
+
+static u16 REG_SD_CFG1;
+static u16 REG_SD_CFG2;
+static u16 REG_SD_CFG3;
+static u16 REG_SD_STAT1;
+static u16 REG_SD_STAT2;
+static u16 REG_SD_BUS_STAT;
+static u16 REG_SD_PAD_CTL;
+static u16 REG_SD_SAMPLE_POINT_CTL;
+static u16 REG_SD_PUSH_POINT_CTL;
+static u16 REG_SD_CMD0;
+static u16 REG_SD_CMD1;
+static u16 REG_SD_CMD2;
+static u16 REG_SD_CMD3;
+static u16 REG_SD_CMD4;
+static u16 REG_SD_CMD5;
+static u16 REG_SD_BYTE_CNT_L;
+static u16 REG_SD_BYTE_CNT_H;
+static u16 REG_SD_BLOCK_CNT_L;
+static u16 REG_SD_BLOCK_CNT_H;
+static u16 REG_SD_TRANSFER;
+static u16 REG_SD_VPCLK0_CTL;
+static u16 REG_SD_VPCLK1_CTL;
+static u16 REG_SD_DCMPS0_CTL;
+static u16 REG_SD_DCMPS1_CTL;
+
+static inline void sd_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+
+	sd_card->err_code |= err_code;
+}
+
+static inline void sd_clr_err_code(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+
+	sd_card->err_code = 0;
+}
+
+static inline int sd_check_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+
+	return sd_card->err_code & err_code;
+}
+
+static void sd_init_reg_addr(struct rtsx_chip *chip)
+{
+	REG_SD_CFG1 = 0xFD31;
+	REG_SD_CFG2 = 0xFD33;
+	REG_SD_CFG3 = 0xFD3E;
+	REG_SD_STAT1 = 0xFD30;
+	REG_SD_STAT2 = 0;
+	REG_SD_BUS_STAT = 0;
+	REG_SD_PAD_CTL = 0;
+	REG_SD_SAMPLE_POINT_CTL = 0;
+	REG_SD_PUSH_POINT_CTL = 0;
+	REG_SD_CMD0 = 0xFD34;
+	REG_SD_CMD1 = 0xFD35;
+	REG_SD_CMD2 = 0xFD36;
+	REG_SD_CMD3 = 0xFD37;
+	REG_SD_CMD4 = 0xFD38;
+	REG_SD_CMD5 = 0xFD5A;
+	REG_SD_BYTE_CNT_L = 0xFD39;
+	REG_SD_BYTE_CNT_H = 0xFD3A;
+	REG_SD_BLOCK_CNT_L = 0xFD3B;
+	REG_SD_BLOCK_CNT_H = 0xFD3C;
+	REG_SD_TRANSFER = 0xFD32;
+	REG_SD_VPCLK0_CTL = 0;
+	REG_SD_VPCLK1_CTL = 0;
+	REG_SD_DCMPS0_CTL = 0;
+	REG_SD_DCMPS1_CTL = 0;
+}
+
+static int sd_check_data0_status(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 stat;
+
+	retval = rtsx_read_register(chip, REG_SD_STAT1, &stat);
+	if (retval)
+		return retval;
+
+	if (!(stat & SD_DAT0_STATUS)) {
+		sd_set_err_code(chip, SD_BUSY);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
+			       u32 arg, u8 rsp_type, u8 *rsp, int rsp_len)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int timeout = 100;
+	u16 reg_addr;
+	u8 *ptr;
+	int stat_idx = 0;
+	int rty_cnt = 0;
+
+	sd_clr_err_code(chip);
+
+	dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d, arg = 0x%08x\n", cmd_idx, arg);
+
+	if (rsp_type == SD_RSP_TYPE_R1b)
+		timeout = 3000;
+
+RTY_SEND_CMD:
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF, 0x40 | cmd_idx);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD1, 0xFF, (u8)(arg >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD2, 0xFF, (u8)(arg >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD3, 0xFF, (u8)(arg >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD4, 0xFF, (u8)arg);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, rsp_type);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER,
+		     0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+		     SD_TRANSFER_END | SD_STAT_IDLE, SD_TRANSFER_END |
+		     SD_STAT_IDLE);
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+		stat_idx = 16;
+	} else if (rsp_type != SD_RSP_TYPE_R0) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+		stat_idx = 5;
+	}
+
+	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_STAT1, 0, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		u8 val;
+
+		rtsx_read_register(chip, REG_SD_STAT1, &val);
+		dev_dbg(rtsx_dev(chip), "SD_STAT1: 0x%x\n", val);
+
+		rtsx_read_register(chip, REG_SD_CFG3, &val);
+		dev_dbg(rtsx_dev(chip), "SD_CFG3: 0x%x\n", val);
+
+		if (retval == -ETIMEDOUT) {
+			if (rsp_type & SD_WAIT_BUSY_END) {
+				retval = sd_check_data0_status(chip);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_clear_sd_error(chip);
+					return retval;
+				}
+			} else {
+				sd_set_err_code(chip, SD_TO_ERR);
+			}
+			retval = STATUS_TIMEDOUT;
+		} else {
+			retval = STATUS_FAIL;
+		}
+		rtsx_clear_sd_error(chip);
+
+		return retval;
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R0)
+		return STATUS_SUCCESS;
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if ((ptr[0] & 0xC0) != 0) {
+		sd_set_err_code(chip, SD_STS_ERR);
+		return STATUS_FAIL;
+	}
+
+	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
+		if (ptr[stat_idx] & SD_CRC7_ERR) {
+			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				return STATUS_FAIL;
+			}
+			if (rty_cnt < SD_MAX_RETRY_COUNT) {
+				wait_timeout(20);
+				rty_cnt++;
+				goto RTY_SEND_CMD;
+			} else {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				return STATUS_FAIL;
+			}
+		}
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R1 || rsp_type == SD_RSP_TYPE_R1b) {
+		if (cmd_idx != SEND_RELATIVE_ADDR &&
+		    cmd_idx != SEND_IF_COND) {
+			if (cmd_idx != STOP_TRANSMISSION) {
+				if (ptr[1] & 0x80)
+					return STATUS_FAIL;
+			}
+#ifdef SUPPORT_SD_LOCK
+			if (ptr[1] & 0x7D) {
+#else
+			if (ptr[1] & 0x7F) {
+#endif
+				dev_dbg(rtsx_dev(chip), "ptr[1]: 0x%02x\n",
+					ptr[1]);
+				return STATUS_FAIL;
+			}
+			if (ptr[2] & 0xFF) {
+				dev_dbg(rtsx_dev(chip), "ptr[2]: 0x%02x\n",
+					ptr[2]);
+				return STATUS_FAIL;
+			}
+			if (ptr[3] & 0x80) {
+				dev_dbg(rtsx_dev(chip), "ptr[3]: 0x%02x\n",
+					ptr[3]);
+				return STATUS_FAIL;
+			}
+			if (ptr[3] & 0x01)
+				sd_card->sd_data_buf_ready = 1;
+			else
+				sd_card->sd_data_buf_ready = 0;
+		}
+	}
+
+	if (rsp && rsp_len)
+		memcpy(rsp, ptr, rsp_len);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_read_data(struct rtsx_chip *chip,
+			u8 trans_mode, u8 *cmd, int cmd_len, u16 byte_cnt,
+			u16 blk_cnt, u8 bus_width, u8 *buf, int buf_len,
+			int timeout)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i;
+
+	sd_clr_err_code(chip);
+
+	if (!buf)
+		buf_len = 0;
+
+	if (buf_len > 512)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	if (cmd_len) {
+		dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n", cmd[0] - 0x40);
+		for (i = 0; i < (min(cmd_len, 6)); i++)
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0 + i,
+				     0xFF, cmd[i]);
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF,
+		     (u8)byte_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF,
+		     (u8)(byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF,
+		     (u8)blk_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF,
+		     (u8)(blk_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, bus_width);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
+		     SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	if (trans_mode != SD_TM_AUTO_TUNING)
+		rtsx_add_cmd(chip, WRITE_REG_CMD,
+			     CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+		     trans_mode | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		if (retval == -ETIMEDOUT) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					    SD_RSP_TYPE_R1, NULL, 0);
+		}
+
+		return STATUS_FAIL;
+	}
+
+	if (buf && buf_len) {
+		retval = rtsx_read_ppbuf(chip, buf, buf_len);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_write_data(struct rtsx_chip *chip, u8 trans_mode,
+			 u8 *cmd, int cmd_len, u16 byte_cnt, u16 blk_cnt,
+			 u8 bus_width, u8 *buf, int buf_len, int timeout)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i;
+
+	sd_clr_err_code(chip);
+
+	if (!buf)
+		buf_len = 0;
+
+	if (buf_len > 512) {
+		/* This function can't write data more than one page */
+		return STATUS_FAIL;
+	}
+
+	if (buf && buf_len) {
+		retval = rtsx_write_ppbuf(chip, buf, buf_len);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	rtsx_init_cmd(chip);
+
+	if (cmd_len) {
+		dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n", cmd[0] - 0x40);
+		for (i = 0; i < (min(cmd_len, 6)); i++) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD,
+				     REG_SD_CMD0 + i, 0xFF, cmd[i]);
+		}
+	}
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF,
+		     (u8)byte_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF,
+		     (u8)(byte_cnt >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF,
+		     (u8)blk_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF,
+		     (u8)(blk_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, bus_width);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
+		     SD_CHECK_CRC7 | SD_RSP_LEN_6);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+		     trans_mode | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		if (retval == -ETIMEDOUT) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					    SD_RSP_TYPE_R1, NULL, 0);
+		}
+
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i;
+	u8 csd_ver, trans_speed;
+	u8 rsp[16];
+
+	for (i = 0; i < 6; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_CSD, sd_card->sd_addr,
+					     SD_RSP_TYPE_R2, rsp, 16);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+
+	if (i == 6)
+		return STATUS_FAIL;
+
+	memcpy(sd_card->raw_csd, rsp + 1, 15);
+
+	dev_dbg(rtsx_dev(chip), "CSD Response:\n");
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 16, sd_card->raw_csd);
+
+	csd_ver = (rsp[1] & 0xc0) >> 6;
+	dev_dbg(rtsx_dev(chip), "csd_ver = %d\n", csd_ver);
+
+	trans_speed = rsp[4];
+	if ((trans_speed & 0x07) == 0x02) {
+		if ((trans_speed & 0xf8) >= 0x30) {
+			if (chip->asic_code)
+				sd_card->sd_clock = 47;
+			else
+				sd_card->sd_clock = CLK_50;
+
+		} else if ((trans_speed & 0xf8) == 0x28) {
+			if (chip->asic_code)
+				sd_card->sd_clock = 39;
+			else
+				sd_card->sd_clock = CLK_40;
+
+		} else if ((trans_speed & 0xf8) == 0x20) {
+			if (chip->asic_code)
+				sd_card->sd_clock = 29;
+			else
+				sd_card->sd_clock = CLK_30;
+
+		} else if ((trans_speed & 0xf8) >= 0x10) {
+			if (chip->asic_code)
+				sd_card->sd_clock = 23;
+			else
+				sd_card->sd_clock = CLK_20;
+
+		} else if ((trans_speed & 0x08) >= 0x08) {
+			if (chip->asic_code)
+				sd_card->sd_clock = 19;
+			else
+				sd_card->sd_clock = CLK_20;
+		} else {
+			return STATUS_FAIL;
+		}
+	} else {
+		return STATUS_FAIL;
+	}
+
+	if (CHK_MMC_SECTOR_MODE(sd_card)) {
+		sd_card->capacity = 0;
+	} else {
+		if ((!CHK_SD_HCXC(sd_card)) || csd_ver == 0) {
+			u8 blk_size, c_size_mult;
+			u16 c_size;
+
+			blk_size = rsp[6] & 0x0F;
+			c_size =  ((u16)(rsp[7] & 0x03) << 10)
+					+ ((u16)rsp[8] << 2)
+					+ ((u16)(rsp[9] & 0xC0) >> 6);
+			c_size_mult = (u8)((rsp[10] & 0x03) << 1);
+			c_size_mult += (rsp[11] & 0x80) >> 7;
+			sd_card->capacity = (((u32)(c_size + 1)) *
+					(1 << (c_size_mult + 2)))
+				<< (blk_size - 9);
+		} else {
+			u32 total_sector = 0;
+
+			total_sector = (((u32)rsp[8] & 0x3f) << 16) |
+				((u32)rsp[9] << 8) | (u32)rsp[10];
+			sd_card->capacity = (total_sector + 1) << 10;
+		}
+	}
+
+	if (check_wp) {
+		if (rsp[15] & 0x30)
+			chip->card_wp |= SD_CARD;
+
+		dev_dbg(rtsx_dev(chip), "CSD WP Status: 0x%x\n", rsp[15]);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_set_sample_push_timing(struct rtsx_chip *chip)
+{
+	int retval;
+	struct sd_info *sd_card = &chip->sd_card;
+	u8 val = 0;
+
+	if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY)
+		val |= 0x10;
+
+	if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) == SD_SAMPLE_POINT_AUTO) {
+		if (chip->asic_code) {
+			if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card)) {
+				if (val & 0x10)
+					val |= 0x04;
+				else
+					val |= 0x08;
+			}
+		} else {
+			if (val & 0x10)
+				val |= 0x04;
+			else
+				val |= 0x08;
+		}
+	} else if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
+		SD_SAMPLE_POINT_DELAY) {
+		if (val & 0x10)
+			val |= 0x04;
+		else
+			val |= 0x08;
+	}
+
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x1C, val);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static void sd_choose_proper_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+
+	if (CHK_SD_SDR104(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = chip->asic_sd_sdr104_clk;
+		else
+			sd_card->sd_clock = chip->fpga_sd_sdr104_clk;
+
+	} else if (CHK_SD_DDR50(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = chip->asic_sd_ddr50_clk;
+		else
+			sd_card->sd_clock = chip->fpga_sd_ddr50_clk;
+
+	} else if (CHK_SD_SDR50(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = chip->asic_sd_sdr50_clk;
+		else
+			sd_card->sd_clock = chip->fpga_sd_sdr50_clk;
+
+	} else if (CHK_SD_HS(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = chip->asic_sd_hs_clk;
+		else
+			sd_card->sd_clock = chip->fpga_sd_hs_clk;
+
+	} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = chip->asic_mmc_52m_clk;
+		else
+			sd_card->sd_clock = chip->fpga_mmc_52m_clk;
+
+	} else if (CHK_MMC_26M(sd_card)) {
+		if (chip->asic_code)
+			sd_card->sd_clock = 48;
+		else
+			sd_card->sd_clock = CLK_50;
+	}
+}
+
+static int sd_set_clock_divider(struct rtsx_chip *chip, u8 clk_div)
+{
+	int retval;
+	u8 mask = 0, val = 0;
+
+	mask = 0x60;
+	if (clk_div == SD_CLK_DIVIDE_0)
+		val = 0x00;
+	else if (clk_div == SD_CLK_DIVIDE_128)
+		val = 0x40;
+	else if (clk_div == SD_CLK_DIVIDE_256)
+		val = 0x20;
+
+	retval = rtsx_write_register(chip, REG_SD_CFG1, mask, val);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_set_init_para(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	retval = sd_set_sample_push_timing(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	sd_choose_proper_clock(chip);
+
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int sd_select_card(struct rtsx_chip *chip, int select)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd_idx, cmd_type;
+	u32 addr;
+
+	if (select) {
+		cmd_idx = SELECT_CARD;
+		cmd_type = SD_RSP_TYPE_R1;
+		addr = sd_card->sd_addr;
+	} else {
+		cmd_idx = DESELECT_CARD;
+		cmd_type = SD_RSP_TYPE_R0;
+		addr = 0;
+	}
+
+	retval = sd_send_cmd_get_rsp(chip, cmd_idx, addr, cmd_type, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+#ifdef SUPPORT_SD_LOCK
+static int sd_update_lock_status(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 rsp[5];
+
+	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, rsp, 5);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (rsp[1] & 0x02)
+		sd_card->sd_lock_status |= SD_LOCKED;
+	else
+		sd_card->sd_lock_status &= ~SD_LOCKED;
+
+	dev_dbg(rtsx_dev(chip), "sd_card->sd_lock_status = 0x%x\n",
+		sd_card->sd_lock_status);
+
+	if (rsp[1] & 0x01)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+static int sd_wait_state_data_ready(struct rtsx_chip *chip, u8 state,
+				    u8 data_ready, int polling_cnt)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval, i;
+	u8 rsp[5];
+
+	for (i = 0; i < polling_cnt; i++) {
+		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					     sd_card->sd_addr, SD_RSP_TYPE_R1,
+					     rsp, 5);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (((rsp[3] & 0x1E) == state) &&
+		    ((rsp[3] & 0x01) == data_ready))
+			return STATUS_SUCCESS;
+	}
+
+	return STATUS_FAIL;
+}
+
+static int sd_change_bank_voltage(struct rtsx_chip *chip, u8 voltage)
+{
+	int retval;
+
+	if (voltage == SD_IO_3V3) {
+		if (chip->asic_code) {
+			retval = rtsx_write_phy_register(chip, 0x08,
+							 0x4FC0 |
+							 chip->phy_voltage);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		} else {
+			retval = rtsx_write_register(chip, SD_PAD_CTL,
+						     SD_IO_USING_1V8, 0);
+			if (retval)
+				return retval;
+		}
+	} else if (voltage == SD_IO_1V8) {
+		if (chip->asic_code) {
+			retval = rtsx_write_phy_register(chip, 0x08,
+							 0x4C40 |
+							 chip->phy_voltage);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		} else {
+			retval = rtsx_write_register(chip, SD_PAD_CTL,
+						     SD_IO_USING_1V8,
+						     SD_IO_USING_1V8);
+			if (retval)
+				return retval;
+		}
+	} else {
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_voltage_switch(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 stat;
+
+	retval = rtsx_write_register(chip, SD_BUS_STAT,
+				     SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+				     SD_CLK_TOGGLE_EN);
+	if (retval)
+		return retval;
+
+	retval = sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	udelay(chip->sd_voltage_switch_delay);
+
+	retval = rtsx_read_register(chip, SD_BUS_STAT, &stat);
+	if (retval)
+		return retval;
+	if (stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+				SD_DAT1_STATUS | SD_DAT0_STATUS)) {
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, SD_BUS_STAT, 0xFF,
+				     SD_CLK_FORCE_STOP);
+	if (retval)
+		return retval;
+	retval = sd_change_bank_voltage(chip, SD_IO_1V8);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	wait_timeout(50);
+
+	retval = rtsx_write_register(chip, SD_BUS_STAT, 0xFF,
+				     SD_CLK_TOGGLE_EN);
+	if (retval)
+		return retval;
+	wait_timeout(10);
+
+	retval = rtsx_read_register(chip, SD_BUS_STAT, &stat);
+	if (retval)
+		return retval;
+	if ((stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+				SD_DAT1_STATUS | SD_DAT0_STATUS)) !=
+			(SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
+				SD_DAT1_STATUS | SD_DAT0_STATUS)) {
+		dev_dbg(rtsx_dev(chip), "SD_BUS_STAT: 0x%x\n", stat);
+		rtsx_write_register(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN |
+				    SD_CLK_FORCE_STOP, 0);
+		rtsx_write_register(chip, CARD_CLK_EN, 0xFF, 0);
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, SD_BUS_STAT,
+				     SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_reset_dcm(struct rtsx_chip *chip, u8 tune_dir)
+{
+	int retval;
+
+	if (tune_dir == TUNE_RX) {
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF,
+					     DCM_RESET | DCM_RX);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF, DCM_RX);
+		if (retval)
+			return retval;
+	} else {
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF,
+					     DCM_RESET | DCM_TX);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF, DCM_TX);
+		if (retval)
+			return retval;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	u16 SD_VP_CTL, SD_DCMPS_CTL;
+	u8 val;
+	int retval;
+	bool ddr_rx = false;
+
+	dev_dbg(rtsx_dev(chip), "%s (sample_point = %d, tune_dir = %d)\n",
+		__func__, sample_point, tune_dir);
+
+	if (tune_dir == TUNE_RX) {
+		SD_VP_CTL = SD_VPRX_CTL;
+		SD_DCMPS_CTL = SD_DCMPS_RX_CTL;
+		if (CHK_SD_DDR50(sd_card))
+			ddr_rx = true;
+	} else {
+		SD_VP_CTL = SD_VPTX_CTL;
+		SD_DCMPS_CTL = SD_DCMPS_TX_CTL;
+	}
+
+	if (chip->asic_code) {
+		retval = rtsx_write_register(chip, CLK_CTL, CHANGE_CLK,
+					     CHANGE_CLK);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SD_VP_CTL, 0x1F,
+					     sample_point);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CLK_CTL, CHANGE_CLK, 0);
+		if (retval)
+			return retval;
+	} else {
+		rtsx_read_register(chip, SD_VP_CTL, &val);
+		dev_dbg(rtsx_dev(chip), "SD_VP_CTL: 0x%x\n", val);
+		rtsx_read_register(chip, SD_DCMPS_CTL, &val);
+		dev_dbg(rtsx_dev(chip), "SD_DCMPS_CTL: 0x%x\n", val);
+
+		if (ddr_rx) {
+			retval = rtsx_write_register(chip, SD_VP_CTL,
+						     PHASE_CHANGE,
+						     PHASE_CHANGE);
+			if (retval)
+				return retval;
+			udelay(50);
+			retval = rtsx_write_register(chip, SD_VP_CTL, 0xFF,
+						     PHASE_CHANGE |
+						     PHASE_NOT_RESET |
+						     sample_point);
+			if (retval)
+				return retval;
+		} else {
+			retval = rtsx_write_register(chip, CLK_CTL,
+						     CHANGE_CLK, CHANGE_CLK);
+			if (retval)
+				return retval;
+			udelay(50);
+			retval = rtsx_write_register(chip, SD_VP_CTL, 0xFF,
+						     PHASE_NOT_RESET |
+						     sample_point);
+			if (retval)
+				return retval;
+		}
+		udelay(100);
+
+		rtsx_init_cmd(chip);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_DCMPS_CTL, DCMPS_CHANGE,
+			     DCMPS_CHANGE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, SD_DCMPS_CTL,
+			     DCMPS_CHANGE_DONE, DCMPS_CHANGE_DONE);
+		retval = rtsx_send_cmd(chip, SD_CARD, 100);
+		if (retval != STATUS_SUCCESS)
+			goto fail;
+
+		val = *rtsx_get_cmd_data(chip);
+		if (val & DCMPS_ERROR)
+			goto fail;
+
+		if ((val & DCMPS_CURRENT_PHASE) != sample_point)
+			goto fail;
+
+		retval = rtsx_write_register(chip, SD_DCMPS_CTL,
+					     DCMPS_CHANGE, 0);
+		if (retval)
+			return retval;
+		if (ddr_rx) {
+			retval = rtsx_write_register(chip, SD_VP_CTL,
+						     PHASE_CHANGE, 0);
+			if (retval)
+				return retval;
+		} else {
+			retval = rtsx_write_register(chip, CLK_CTL,
+						     CHANGE_CLK, 0);
+			if (retval)
+				return retval;
+		}
+
+		udelay(50);
+	}
+
+	retval = rtsx_write_register(chip, SD_CFG1, SD_ASYNC_FIFO_NOT_RST, 0);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+
+fail:
+	rtsx_read_register(chip, SD_VP_CTL, &val);
+	dev_dbg(rtsx_dev(chip), "SD_VP_CTL: 0x%x\n", val);
+	rtsx_read_register(chip, SD_DCMPS_CTL, &val);
+	dev_dbg(rtsx_dev(chip), "SD_DCMPS_CTL: 0x%x\n", val);
+
+	rtsx_write_register(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
+	rtsx_write_register(chip, SD_VP_CTL, PHASE_CHANGE, 0);
+	mdelay(10);
+	sd_reset_dcm(chip, tune_dir);
+	return STATUS_FAIL;
+}
+
+static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5], buf[8];
+
+	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	cmd[0] = 0x40 | SEND_SCR;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 8, 1, bus_width,
+			      buf, 8, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	memcpy(sd_card->raw_scr, buf, 8);
+
+	if ((buf[0] & 0x0F) == 0)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_query_switch_result(struct rtsx_chip *chip, u8 func_group,
+				  u8 func_to_switch, u8 *buf, int buf_len)
+{
+	u8 support_mask = 0, query_switch = 0, switch_busy = 0;
+	int support_offset = 0, query_switch_offset = 0, check_busy_offset = 0;
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		support_offset = FUNCTION_GROUP1_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP1_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP1_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case HS_SUPPORT:
+			support_mask = HS_SUPPORT_MASK;
+			query_switch = HS_QUERY_SWITCH_OK;
+			switch_busy = HS_SWITCH_BUSY;
+			break;
+
+		case SDR50_SUPPORT:
+			support_mask = SDR50_SUPPORT_MASK;
+			query_switch = SDR50_QUERY_SWITCH_OK;
+			switch_busy = SDR50_SWITCH_BUSY;
+			break;
+
+		case SDR104_SUPPORT:
+			support_mask = SDR104_SUPPORT_MASK;
+			query_switch = SDR104_QUERY_SWITCH_OK;
+			switch_busy = SDR104_SWITCH_BUSY;
+			break;
+
+		case DDR50_SUPPORT:
+			support_mask = DDR50_SUPPORT_MASK;
+			query_switch = DDR50_QUERY_SWITCH_OK;
+			switch_busy = DDR50_SWITCH_BUSY;
+			break;
+
+		default:
+			return STATUS_FAIL;
+		}
+	} else if (func_group == SD_FUNC_GROUP_3) {
+		support_offset = FUNCTION_GROUP3_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP3_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP3_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case DRIVING_TYPE_A:
+			support_mask = DRIVING_TYPE_A_MASK;
+			query_switch = TYPE_A_QUERY_SWITCH_OK;
+			switch_busy = TYPE_A_SWITCH_BUSY;
+			break;
+
+		case DRIVING_TYPE_C:
+			support_mask = DRIVING_TYPE_C_MASK;
+			query_switch = TYPE_C_QUERY_SWITCH_OK;
+			switch_busy = TYPE_C_SWITCH_BUSY;
+			break;
+
+		case DRIVING_TYPE_D:
+			support_mask = DRIVING_TYPE_D_MASK;
+			query_switch = TYPE_D_QUERY_SWITCH_OK;
+			switch_busy = TYPE_D_SWITCH_BUSY;
+			break;
+
+		default:
+			return STATUS_FAIL;
+		}
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		support_offset = FUNCTION_GROUP4_SUPPORT_OFFSET;
+		query_switch_offset = FUNCTION_GROUP4_QUERY_SWITCH_OFFSET;
+		check_busy_offset = FUNCTION_GROUP4_CHECK_BUSY_OFFSET;
+
+		switch (func_to_switch) {
+		case CURRENT_LIMIT_400:
+			support_mask = CURRENT_LIMIT_400_MASK;
+			query_switch = CURRENT_LIMIT_400_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_400_SWITCH_BUSY;
+			break;
+
+		case CURRENT_LIMIT_600:
+			support_mask = CURRENT_LIMIT_600_MASK;
+			query_switch = CURRENT_LIMIT_600_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_600_SWITCH_BUSY;
+			break;
+
+		case CURRENT_LIMIT_800:
+			support_mask = CURRENT_LIMIT_800_MASK;
+			query_switch = CURRENT_LIMIT_800_QUERY_SWITCH_OK;
+			switch_busy = CURRENT_LIMIT_800_SWITCH_BUSY;
+			break;
+
+		default:
+			return STATUS_FAIL;
+		}
+	} else {
+		return STATUS_FAIL;
+	}
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		if (!(buf[support_offset] & support_mask) ||
+		    ((buf[query_switch_offset] & 0x0F) != query_switch)) {
+			return STATUS_FAIL;
+		}
+	}
+
+	/* Check 'Busy Status' */
+	if (buf[DATA_STRUCTURE_VER_OFFSET] == 0x01 &&
+	    ((buf[check_busy_offset] & switch_busy) == switch_busy)) {
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode, u8 func_group,
+				u8 func_to_switch, u8 bus_width)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5], buf[64];
+
+	dev_dbg(rtsx_dev(chip), "%s (mode = %d, func_group = %d, func_to_switch = %d)\n",
+		__func__, mode, func_group, func_to_switch);
+
+	cmd[0] = 0x40 | SWITCH;
+	cmd[1] = mode;
+
+	if (func_group == SD_FUNC_GROUP_1) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0xFF;
+		cmd[4] = 0xF0 + func_to_switch;
+	} else if (func_group == SD_FUNC_GROUP_3) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0xF0 + func_to_switch;
+		cmd[4] = 0xFF;
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		cmd[2] = 0xFF;
+		cmd[3] = 0x0F + (func_to_switch << 4);
+		cmd[4] = 0xFF;
+	} else {
+		cmd[1] = SD_CHECK_MODE;
+		cmd[2] = 0xFF;
+		cmd[3] = 0xFF;
+		cmd[4] = 0xFF;
+	}
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1, bus_width,
+			      buf, 64, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 64, buf);
+
+	if (func_group == NO_ARGUMENT) {
+		sd_card->func_group1_mask = buf[0x0D];
+		sd_card->func_group2_mask = buf[0x0B];
+		sd_card->func_group3_mask = buf[0x09];
+		sd_card->func_group4_mask = buf[0x07];
+
+		dev_dbg(rtsx_dev(chip), "func_group1_mask = 0x%02x\n",
+			buf[0x0D]);
+		dev_dbg(rtsx_dev(chip), "func_group2_mask = 0x%02x\n",
+			buf[0x0B]);
+		dev_dbg(rtsx_dev(chip), "func_group3_mask = 0x%02x\n",
+			buf[0x09]);
+		dev_dbg(rtsx_dev(chip), "func_group4_mask = 0x%02x\n",
+			buf[0x07]);
+	} else {
+		/* Maximum current consumption, check whether current is
+		 * acceptable; bit[511:496] = 0x0000 means some error happened.
+		 */
+		u16 cc = ((u16)buf[0] << 8) | buf[1];
+
+		dev_dbg(rtsx_dev(chip), "Maximum current consumption: %dmA\n",
+			cc);
+		if (cc == 0 || cc > 800)
+			return STATUS_FAIL;
+
+		retval = sd_query_switch_result(chip, func_group,
+						func_to_switch, buf, 64);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (cc > 400 || func_to_switch > CURRENT_LIMIT_400) {
+			retval = rtsx_write_register(chip, OCPPARA2,
+						     SD_OCP_THD_MASK,
+						     chip->sd_800mA_ocp_thd);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PWR_CTL,
+						     PMOS_STRG_MASK,
+						     PMOS_STRG_800mA);
+			if (retval)
+				return retval;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static u8 downgrade_switch_mode(u8 func_group, u8 func_to_switch)
+{
+	if (func_group == SD_FUNC_GROUP_1) {
+		if (func_to_switch > HS_SUPPORT)
+			func_to_switch--;
+
+	} else if (func_group == SD_FUNC_GROUP_4) {
+		if (func_to_switch > CURRENT_LIMIT_200)
+			func_to_switch--;
+	}
+
+	return func_to_switch;
+}
+
+static int sd_check_switch(struct rtsx_chip *chip,
+			   u8 func_group, u8 func_to_switch, u8 bus_width)
+{
+	int retval;
+	int i;
+	bool switch_good = false;
+
+	for (i = 0; i < 3; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = sd_check_switch_mode(chip, SD_CHECK_MODE, func_group,
+					      func_to_switch, bus_width);
+		if (retval == STATUS_SUCCESS) {
+			u8 stat;
+
+			retval = sd_check_switch_mode(chip, SD_SWITCH_MODE,
+						      func_group,
+						      func_to_switch,
+						      bus_width);
+			if (retval == STATUS_SUCCESS) {
+				switch_good = true;
+				break;
+			}
+
+			retval = rtsx_read_register(chip, SD_STAT1, &stat);
+			if (retval)
+				return retval;
+			if (stat & SD_CRC16_ERR) {
+				dev_dbg(rtsx_dev(chip), "SD CRC16 error when switching mode\n");
+				return STATUS_FAIL;
+			}
+		}
+
+		func_to_switch = downgrade_switch_mode(func_group,
+						       func_to_switch);
+
+		wait_timeout(20);
+	}
+
+	if (!switch_good)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i;
+	u8 func_to_switch = 0;
+
+	/* Get supported functions */
+	retval = sd_check_switch_mode(chip, SD_CHECK_MODE, NO_ARGUMENT,
+				      NO_ARGUMENT, bus_width);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
+
+	/* Function Group 1: Access Mode */
+	for (i = 0; i < 4; i++) {
+		switch ((u8)(chip->sd_speed_prior >> (i * 8))) {
+		case SDR104_SUPPORT:
+			if ((sd_card->func_group1_mask & SDR104_SUPPORT_MASK) &&
+			    chip->sdr104_en) {
+				func_to_switch = SDR104_SUPPORT;
+			}
+			break;
+
+		case DDR50_SUPPORT:
+			if ((sd_card->func_group1_mask & DDR50_SUPPORT_MASK) &&
+			    chip->ddr50_en) {
+				func_to_switch = DDR50_SUPPORT;
+			}
+			break;
+
+		case SDR50_SUPPORT:
+			if ((sd_card->func_group1_mask & SDR50_SUPPORT_MASK) &&
+			    chip->sdr50_en) {
+				func_to_switch = SDR50_SUPPORT;
+			}
+			break;
+
+		case HS_SUPPORT:
+			if (sd_card->func_group1_mask & HS_SUPPORT_MASK)
+				func_to_switch = HS_SUPPORT;
+
+			break;
+
+		default:
+			continue;
+		}
+
+		if (func_to_switch)
+			break;
+	}
+	dev_dbg(rtsx_dev(chip), "SD_FUNC_GROUP_1: func_to_switch = 0x%02x",
+		func_to_switch);
+
+#ifdef SUPPORT_SD_LOCK
+	if ((sd_card->sd_lock_status & SD_SDR_RST) &&
+	    func_to_switch == DDR50_SUPPORT &&
+	    (sd_card->func_group1_mask & SDR50_SUPPORT_MASK)) {
+		func_to_switch = SDR50_SUPPORT;
+		dev_dbg(rtsx_dev(chip), "Using SDR50 instead of DDR50 for SD Lock\n");
+	}
+#endif
+
+	if (func_to_switch) {
+		retval = sd_check_switch(chip, SD_FUNC_GROUP_1, func_to_switch,
+					 bus_width);
+		if (retval != STATUS_SUCCESS) {
+			if (func_to_switch == SDR104_SUPPORT) {
+				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK;
+			} else if (func_to_switch == DDR50_SUPPORT) {
+				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK |
+					DDR50_SUPPORT_MASK;
+			} else if (func_to_switch == SDR50_SUPPORT) {
+				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK |
+					DDR50_SUPPORT_MASK | SDR50_SUPPORT_MASK;
+			}
+			return STATUS_FAIL;
+		}
+
+		if (func_to_switch == SDR104_SUPPORT)
+			SET_SD_SDR104(sd_card);
+		else if (func_to_switch == DDR50_SUPPORT)
+			SET_SD_DDR50(sd_card);
+		else if (func_to_switch == SDR50_SUPPORT)
+			SET_SD_SDR50(sd_card);
+		else
+			SET_SD_HS(sd_card);
+	}
+
+	if (CHK_SD_DDR50(sd_card)) {
+		retval = rtsx_write_register(chip, SD_PUSH_POINT_CTL, 0x06,
+					     0x04);
+		if (retval)
+			return retval;
+		retval = sd_set_sample_push_timing(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	if (!func_to_switch || func_to_switch == HS_SUPPORT) {
+		/* Do not try to switch current limit if the card doesn't
+		 * support UHS mode or we don't want it to support UHS mode
+		 */
+		return STATUS_SUCCESS;
+	}
+
+	/* Function Group 4: Current Limit */
+	func_to_switch = 0xFF;
+
+	for (i = 0; i < 4; i++) {
+		switch ((u8)(chip->sd_current_prior >> (i * 8))) {
+		case CURRENT_LIMIT_800:
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_800_MASK)
+				func_to_switch = CURRENT_LIMIT_800;
+
+			break;
+
+		case CURRENT_LIMIT_600:
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_600_MASK)
+				func_to_switch = CURRENT_LIMIT_600;
+
+			break;
+
+		case CURRENT_LIMIT_400:
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_400_MASK)
+				func_to_switch = CURRENT_LIMIT_400;
+
+			break;
+
+		case CURRENT_LIMIT_200:
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_200_MASK)
+				func_to_switch = CURRENT_LIMIT_200;
+
+			break;
+
+		default:
+			continue;
+		}
+
+		if (func_to_switch != 0xFF)
+			break;
+	}
+
+	dev_dbg(rtsx_dev(chip), "SD_FUNC_GROUP_4: func_to_switch = 0x%02x",
+		func_to_switch);
+
+	if (func_to_switch <= CURRENT_LIMIT_800) {
+		retval = sd_check_switch(chip, SD_FUNC_GROUP_4, func_to_switch,
+					 bus_width);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_NO_CARD))
+				return STATUS_FAIL;
+		}
+		dev_dbg(rtsx_dev(chip), "Switch current limit finished! (%d)\n",
+			retval);
+	}
+
+	if (CHK_SD_DDR50(sd_card)) {
+		retval = rtsx_write_register(chip, SD_PUSH_POINT_CTL, 0x06, 0);
+		if (retval)
+			return retval;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_wait_data_idle(struct rtsx_chip *chip)
+{
+	int retval = STATUS_TIMEDOUT;
+	int i;
+	u8 val = 0;
+
+	for (i = 0; i < 100; i++) {
+		retval = rtsx_read_register(chip, SD_DATA_STATE, &val);
+		if (retval)
+			return retval;
+		if (val & SD_DATA_IDLE) {
+			retval = STATUS_SUCCESS;
+			break;
+		}
+		udelay(100);
+	}
+	dev_dbg(rtsx_dev(chip), "SD_DATA_STATE: 0x%02x\n", val);
+
+	return retval;
+}
+
+static int sd_sdr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	int retval;
+	u8 cmd[5];
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_AUTO_TUNING, cmd, 5, 0x40, 1,
+			      SD_BUS_WIDTH_4, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5];
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "sd ddr tuning rx\n");
+
+	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	cmd[0] = 0x40 | SD_STATUS;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1,
+			      SD_BUS_WIDTH_4, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mmc_ddr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5], bus_width;
+
+	if (CHK_MMC_8BIT(sd_card))
+		bus_width = SD_BUS_WIDTH_8;
+	else if (CHK_MMC_4BIT(sd_card))
+		bus_width = SD_BUS_WIDTH_4;
+	else
+		bus_width = SD_BUS_WIDTH_1;
+
+	retval = sd_change_phase(chip, sample_point, TUNE_RX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "mmc ddr tuning rx\n");
+
+	cmd[0] = 0x40 | SEND_EXT_CSD;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 0x200, 1,
+			      bus_width, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		(void)sd_wait_data_idle(chip);
+
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_sdr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	retval = sd_change_phase(chip, sample_point, TUNE_TX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval)
+		return retval;
+
+	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS) {
+		if (sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
+			rtsx_write_register(chip, SD_CFG3,
+					    SD_RSP_80CLK_TIMEOUT_EN, 0);
+			return STATUS_FAIL;
+		}
+	}
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5], bus_width;
+
+	retval = sd_change_phase(chip, sample_point, TUNE_TX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (CHK_SD(sd_card)) {
+		bus_width = SD_BUS_WIDTH_4;
+	} else {
+		if (CHK_MMC_8BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_8;
+		else if (CHK_MMC_4BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_4;
+		else
+			bus_width = SD_BUS_WIDTH_1;
+	}
+
+	retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval)
+		return retval;
+
+	cmd[0] = 0x40 | PROGRAM_CSD;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_2, cmd, 5, 16, 1,
+			       bus_width, sd_card->raw_csd, 16, 100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval)
+		return retval;
+
+	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
+			    NULL, 0);
+
+	return STATUS_SUCCESS;
+}
+
+static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
+				u8 tune_dir)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	struct timing_phase_path path[MAX_PHASE + 1];
+	int i, j, cont_path_cnt;
+	bool new_block;
+	int max_len, final_path_idx;
+	u8 final_phase = 0xFF;
+
+	if (phase_map == 0xFFFFFFFF) {
+		if (tune_dir == TUNE_RX)
+			final_phase = (u8)chip->sd_default_rx_phase;
+		else
+			final_phase = (u8)chip->sd_default_tx_phase;
+
+		goto search_finish;
+	}
+
+	cont_path_cnt = 0;
+	new_block = true;
+	j = 0;
+	for (i = 0; i < MAX_PHASE + 1; i++) {
+		if (phase_map & (1 << i)) {
+			if (new_block) {
+				new_block = false;
+				j = cont_path_cnt++;
+				path[j].start = i;
+				path[j].end = i;
+			} else {
+				path[j].end = i;
+			}
+		} else {
+			new_block = true;
+			if (cont_path_cnt) {
+				int idx = cont_path_cnt - 1;
+
+				path[idx].len = path[idx].end -
+					path[idx].start + 1;
+				path[idx].mid = path[idx].start +
+					path[idx].len / 2;
+			}
+		}
+	}
+
+	if (cont_path_cnt == 0) {
+		dev_dbg(rtsx_dev(chip), "No continuous phase path\n");
+		goto search_finish;
+	} else {
+		int idx = cont_path_cnt - 1;
+
+		path[idx].len = path[idx].end - path[idx].start + 1;
+		path[idx].mid = path[idx].start + path[idx].len / 2;
+	}
+
+	if (path[0].start == 0 &&
+	    path[cont_path_cnt - 1].end == MAX_PHASE) {
+		path[0].start = path[cont_path_cnt - 1].start - MAX_PHASE - 1;
+		path[0].len += path[cont_path_cnt - 1].len;
+		path[0].mid = path[0].start + path[0].len / 2;
+		if (path[0].mid < 0)
+			path[0].mid += MAX_PHASE + 1;
+
+		cont_path_cnt--;
+	}
+
+	max_len = 0;
+	final_phase = 0;
+	final_path_idx = 0;
+	for (i = 0; i < cont_path_cnt; i++) {
+		if (path[i].len > max_len) {
+			max_len = path[i].len;
+			final_phase = (u8)path[i].mid;
+			final_path_idx = i;
+		}
+
+		dev_dbg(rtsx_dev(chip), "path[%d].start = %d\n",
+			i, path[i].start);
+		dev_dbg(rtsx_dev(chip), "path[%d].end = %d\n", i, path[i].end);
+		dev_dbg(rtsx_dev(chip), "path[%d].len = %d\n", i, path[i].len);
+		dev_dbg(rtsx_dev(chip), "path[%d].mid = %d\n", i, path[i].mid);
+		dev_dbg(rtsx_dev(chip), "\n");
+	}
+
+	if (tune_dir == TUNE_TX) {
+		if (CHK_SD_SDR104(sd_card)) {
+			if (max_len > 15) {
+				int temp_mid = (max_len - 16) / 2;
+				int temp_final_phase =
+					path[final_path_idx].end -
+					(max_len - (6 + temp_mid));
+
+				if (temp_final_phase < 0)
+					final_phase = (u8)(temp_final_phase +
+							MAX_PHASE + 1);
+				else
+					final_phase = (u8)temp_final_phase;
+			}
+		} else if (CHK_SD_SDR50(sd_card)) {
+			if (max_len > 12) {
+				int temp_mid = (max_len - 13) / 2;
+				int temp_final_phase =
+					path[final_path_idx].end -
+					(max_len - (3 + temp_mid));
+
+				if (temp_final_phase < 0)
+					final_phase = (u8)(temp_final_phase +
+							MAX_PHASE + 1);
+				else
+					final_phase = (u8)temp_final_phase;
+			}
+		}
+	}
+
+search_finish:
+	dev_dbg(rtsx_dev(chip), "Final chosen phase: %d\n", final_phase);
+	return final_phase;
+}
+
+static int sd_tuning_rx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i, j;
+	u32 raw_phase_map[3], phase_map;
+	u8 final_phase;
+	int (*tuning_cmd)(struct rtsx_chip *chip, u8 sample_point);
+
+	if (CHK_SD(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
+			tuning_cmd = sd_ddr_tuning_rx_cmd;
+		else
+			tuning_cmd = sd_sdr_tuning_rx_cmd;
+
+	} else {
+		if (CHK_MMC_DDR52(sd_card))
+			tuning_cmd = mmc_ddr_tuning_rx_cmd;
+		else
+			return STATUS_FAIL;
+	}
+
+	for (i = 0; i < 3; i++) {
+		raw_phase_map[i] = 0;
+		for (j = MAX_PHASE; j >= 0; j--) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				return STATUS_FAIL;
+			}
+
+			retval = tuning_cmd(chip, (u8)j);
+			if (retval == STATUS_SUCCESS)
+				raw_phase_map[i] |= 1 << j;
+		}
+	}
+
+	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
+	for (i = 0; i < 3; i++)
+		dev_dbg(rtsx_dev(chip), "RX raw_phase_map[%d] = 0x%08x\n",
+			i, raw_phase_map[i]);
+
+	dev_dbg(rtsx_dev(chip), "RX phase_map = 0x%08x\n", phase_map);
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
+	if (final_phase == 0xFF)
+		return STATUS_FAIL;
+
+	retval = sd_change_phase(chip, final_phase, TUNE_RX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i;
+	u32 phase_map;
+	u8 final_phase;
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval)
+		return retval;
+
+	phase_map = 0;
+	for (i = MAX_PHASE; i >= 0; i--) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			rtsx_write_register(chip, SD_CFG3,
+					    SD_RSP_80CLK_TIMEOUT_EN, 0);
+			return STATUS_FAIL;
+		}
+
+		retval = sd_change_phase(chip, (u8)i, TUNE_TX);
+		if (retval != STATUS_SUCCESS)
+			continue;
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					     sd_card->sd_addr, SD_RSP_TYPE_R1,
+					     NULL, 0);
+		if (retval == STATUS_SUCCESS ||
+		    !sd_check_err_code(chip, SD_RSP_TIMEOUT))
+			phase_map |= 1 << i;
+	}
+
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval)
+		return retval;
+
+	dev_dbg(rtsx_dev(chip), "DDR TX pre tune phase_map = 0x%08x\n",
+		phase_map);
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
+	if (final_phase == 0xFF)
+		return STATUS_FAIL;
+
+	retval = sd_change_phase(chip, final_phase, TUNE_TX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "DDR TX pre tune phase: %d\n",
+		(int)final_phase);
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_tuning_tx(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int i, j;
+	u32 raw_phase_map[3], phase_map;
+	u8 final_phase;
+	int (*tuning_cmd)(struct rtsx_chip *chip, u8 sample_point);
+
+	if (CHK_SD(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
+			tuning_cmd = sd_ddr_tuning_tx_cmd;
+		else
+			tuning_cmd = sd_sdr_tuning_tx_cmd;
+
+	} else {
+		if (CHK_MMC_DDR52(sd_card))
+			tuning_cmd = sd_ddr_tuning_tx_cmd;
+		else
+			return STATUS_FAIL;
+	}
+
+	for (i = 0; i < 3; i++) {
+		raw_phase_map[i] = 0;
+		for (j = MAX_PHASE; j >= 0; j--) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				rtsx_write_register(chip, SD_CFG3,
+						    SD_RSP_80CLK_TIMEOUT_EN, 0);
+				return STATUS_FAIL;
+			}
+
+			retval = tuning_cmd(chip, (u8)j);
+			if (retval == STATUS_SUCCESS)
+				raw_phase_map[i] |= 1 << j;
+		}
+	}
+
+	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
+	for (i = 0; i < 3; i++)
+		dev_dbg(rtsx_dev(chip), "TX raw_phase_map[%d] = 0x%08x\n",
+			i, raw_phase_map[i]);
+
+	dev_dbg(rtsx_dev(chip), "TX phase_map = 0x%08x\n", phase_map);
+
+	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
+	if (final_phase == 0xFF)
+		return STATUS_FAIL;
+
+	retval = sd_change_phase(chip, final_phase, TUNE_TX);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_sdr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = sd_tuning_tx(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_ddr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_ddr_pre_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = sd_change_phase(chip, (u8)chip->sd_ddr_tx_phase,
+					 TUNE_TX);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int mmc_ddr_tuning(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_ddr_pre_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = sd_change_phase(chip, (u8)chip->mmc_ddr_tx_phase,
+					 TUNE_TX);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	retval = sd_tuning_rx(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
+		retval = sd_tuning_tx(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_switch_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	int re_tuning = 0;
+
+	retval = select_card(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (re_tuning) {
+		if (CHK_SD(sd_card)) {
+			if (CHK_SD_DDR50(sd_card))
+				retval = sd_ddr_tuning(chip);
+			else
+				retval = sd_sdr_tuning(chip);
+		} else {
+			if (CHK_MMC_DDR52(sd_card))
+				retval = mmc_ddr_tuning(chip);
+		}
+
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_prepare_reset(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	if (chip->asic_code)
+		sd_card->sd_clock = 29;
+	else
+		sd_card->sd_clock = CLK_30;
+
+	sd_card->sd_type = 0;
+	sd_card->seq_mode = 0;
+	sd_card->sd_data_buf_ready = 0;
+	sd_card->capacity = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status = 0;
+	sd_card->sd_erase_status = 0;
+#endif
+
+	chip->capacity[chip->card2lun[SD_CARD]] = 0;
+	chip->sd_io = 0;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return retval;
+
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0xFF, 0x40);
+	if (retval)
+		return retval;
+
+	retval = rtsx_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+				     SD_STOP | SD_CLR_ERR);
+	if (retval)
+		return retval;
+
+	retval = select_card(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (CHECK_PID(chip, 0x5208)) {
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     XD_D3_PD | SD_D7_PD | SD_CLK_PD |
+					     SD_D5_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     SD_D6_PD | SD_D0_PD | SD_D1_PD |
+					     XD_D5_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     SD_D4_PD | XD_CE_PD | XD_CLE_PD |
+					     XD_CD_PU);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD | SD_D3_PD | SD_D2_PD |
+					     XD_ALE_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU | SD_WP_PD | SD_CD_PU |
+					     SD_CMD_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval)
+			return retval;
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval)
+				return retval;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_pull_ctl_enable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	if (CHECK_PID(chip, 0x5208)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
+			     XD_D3_PD | SD_DAT7_PU | SD_CLK_NP | SD_D5_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
+			     SD_D6_PU | SD_D0_PU | SD_D1_PU | XD_D5_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
+			     SD_D4_PU | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
+			     XD_RDY_PD | SD_D3_PU | SD_D2_PU | XD_ALE_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
+			     MS_INS_PU | SD_WP_PU | SD_CD_PU | SD_CMD_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
+			     MS_D5_PD | MS_D4_PD);
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
+				     0xA8);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
+				     0x5A);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
+				     0x95);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
+				     0xAA);
+		}
+	}
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_init_power(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = sd_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (!chip->ft2_fast_mode)
+		wait_timeout(250);
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (chip->asic_code) {
+		retval = sd_pull_ctl_enable(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_SD_PULL_CTL_BIT | 0x20, 0);
+		if (retval)
+			return retval;
+	}
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_on(chip, SD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		wait_timeout(260);
+
+#ifdef SUPPORT_OCP
+		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
+			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+				chip->ocp_stat);
+			return STATUS_FAIL;
+		}
+#endif
+	}
+
+	retval = rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
+				     SD_OUTPUT_EN);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_dummy_clock(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x01, 0x01);
+	if (retval)
+		return retval;
+	wait_timeout(5);
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x01, 0);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_read_lba0(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 cmd[5], bus_width;
+
+	cmd[0] = 0x40 | READ_SINGLE_BLOCK;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	if (CHK_SD(sd_card)) {
+		bus_width = SD_BUS_WIDTH_4;
+	} else {
+		if (CHK_MMC_8BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_8;
+		else if (CHK_MMC_4BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_4;
+		else
+			bus_width = SD_BUS_WIDTH_1;
+	}
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 512, 1,
+			      bus_width, NULL, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sd_check_wp_state(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u32 val;
+	u16 sd_card_type;
+	u8 cmd[5], buf[64];
+
+	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	cmd[0] = 0x40 | SD_STATUS;
+	cmd[1] = 0;
+	cmd[2] = 0;
+	cmd[3] = 0;
+	cmd[4] = 0;
+
+	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1,
+			      SD_BUS_WIDTH_4, buf, 64, 250);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+		return STATUS_FAIL;
+	}
+
+	dev_dbg(rtsx_dev(chip), "ACMD13:\n");
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 64, buf);
+
+	sd_card_type = ((u16)buf[2] << 8) | buf[3];
+	dev_dbg(rtsx_dev(chip), "sd_card_type = 0x%04x\n", sd_card_type);
+	if (sd_card_type == 0x0001 || sd_card_type == 0x0002) {
+		/* ROM card or OTP */
+		chip->card_wp |= SD_CARD;
+	}
+
+	/* Check SD Machanical Write-Protect Switch */
+	val = rtsx_readl(chip, RTSX_BIPR);
+	if (val & SD_WRITE_PROTECT)
+		chip->card_wp |= SD_CARD;
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_sd(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	bool hi_cap_flow = false;
+	int retval, i = 0, j = 0, k = 0;
+	bool sd_dont_switch = false;
+	bool support_1v8 = false;
+	bool try_sdio = true;
+	u8 rsp[16];
+	u8 switch_bus_width;
+	u32 voltage = 0;
+	bool sd20_mode = false;
+
+	SET_SD(sd_card);
+
+switch_fail:
+
+	i = 0;
+	j = 0;
+	k = 0;
+	hi_cap_flow = false;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON)
+		goto SD_UNLOCK_ENTRY;
+#endif
+
+	retval = sd_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	retval = sd_dummy_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) && try_sdio) {
+		int rty_cnt = 0;
+
+		for (; rty_cnt < chip->sdio_retry_cnt; rty_cnt++) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				goto status_fail;
+			}
+
+			retval = sd_send_cmd_get_rsp(chip, IO_SEND_OP_COND, 0,
+						     SD_RSP_TYPE_R4, rsp, 5);
+			if (retval == STATUS_SUCCESS) {
+				int func_num = (rsp[1] >> 4) & 0x07;
+
+				if (func_num) {
+					dev_dbg(rtsx_dev(chip), "SD_IO card (Function number: %d)!\n",
+						func_num);
+					chip->sd_io = 1;
+					goto status_fail;
+				}
+
+				break;
+			}
+
+			sd_init_power(chip);
+
+			sd_dummy_clock(chip);
+		}
+
+		dev_dbg(rtsx_dev(chip), "Normal card!\n");
+	}
+
+	/* Start Initialization Process of SD Card */
+RTY_SD_RST:
+	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	wait_timeout(20);
+
+	retval = sd_send_cmd_get_rsp(chip, SEND_IF_COND, 0x000001AA,
+				     SD_RSP_TYPE_R7, rsp, 5);
+	if (retval == STATUS_SUCCESS) {
+		if (rsp[4] == 0xAA && ((rsp[3] & 0x0f) == 0x01)) {
+			hi_cap_flow = true;
+			voltage = SUPPORT_VOLTAGE | 0x40000000;
+		}
+	}
+
+	if (!hi_cap_flow) {
+		voltage = SUPPORT_VOLTAGE;
+
+		retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0,
+					     SD_RSP_TYPE_R0, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		wait_timeout(20);
+	}
+
+	do {
+		retval = sd_send_cmd_get_rsp(chip, APP_CMD, 0, SD_RSP_TYPE_R1,
+					     NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+				sd_set_err_code(chip, SD_NO_CARD);
+				goto status_fail;
+			}
+
+			j++;
+			if (j < 3)
+				goto RTY_SD_RST;
+			else
+				goto status_fail;
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage,
+					     SD_RSP_TYPE_R3, rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			k++;
+			if (k < 3)
+				goto RTY_SD_RST;
+			else
+				goto status_fail;
+		}
+
+		i++;
+		wait_timeout(20);
+	} while (!(rsp[1] & 0x80) && (i < 255));
+
+	if (i == 255)
+		goto status_fail;
+
+	if (hi_cap_flow) {
+		if (rsp[1] & 0x40)
+			SET_SD_HCXC(sd_card);
+		else
+			CLR_SD_HCXC(sd_card);
+
+		support_1v8 = false;
+	} else {
+		CLR_SD_HCXC(sd_card);
+		support_1v8 = false;
+	}
+	dev_dbg(rtsx_dev(chip), "support_1v8 = %d\n", support_1v8);
+
+	if (support_1v8) {
+		retval = sd_voltage_switch(chip);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+	}
+
+	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	for (i = 0; i < 3; i++) {
+		retval = sd_send_cmd_get_rsp(chip, SEND_RELATIVE_ADDR, 0,
+					     SD_RSP_TYPE_R6, rsp, 5);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		sd_card->sd_addr = (u32)rsp[1] << 24;
+		sd_card->sd_addr += (u32)rsp[2] << 16;
+
+		if (sd_card->sd_addr)
+			break;
+	}
+
+	retval = sd_check_csd(chip, 1);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	retval = sd_select_card(chip, 1);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+#ifdef SUPPORT_SD_LOCK
+SD_UNLOCK_ENTRY:
+	retval = sd_update_lock_status(chip);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	if (sd_card->sd_lock_status & SD_LOCKED) {
+		sd_card->sd_lock_status |= (SD_LOCK_1BIT_MODE | SD_PWD_EXIST);
+		return STATUS_SUCCESS;
+	} else if (!(sd_card->sd_lock_status & SD_UNLOCK_POW_ON)) {
+		sd_card->sd_lock_status &= ~SD_PWD_EXIST;
+	}
+#endif
+
+	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	retval = sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0,
+				     SD_RSP_TYPE_R1, NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	if (support_1v8) {
+		retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+					     SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
+					     SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		switch_bus_width = SD_BUS_WIDTH_4;
+	} else {
+		switch_bus_width = SD_BUS_WIDTH_1;
+	}
+
+	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	if (!(sd_card->raw_csd[4] & 0x40))
+		sd_dont_switch = true;
+
+	if (!sd_dont_switch) {
+		if (sd20_mode) {
+			/* Set sd_switch_fail here, because we needn't
+			 * switch to UHS mode
+			 */
+			sd_card->sd_switch_fail = SDR104_SUPPORT_MASK |
+				DDR50_SUPPORT_MASK | SDR50_SUPPORT_MASK;
+		}
+
+		/* Check the card whether follow SD1.1 spec or higher */
+		retval = sd_check_spec(chip, switch_bus_width);
+		if (retval == STATUS_SUCCESS) {
+			retval = sd_switch_function(chip, switch_bus_width);
+			if (retval != STATUS_SUCCESS) {
+				sd_init_power(chip);
+				sd_dont_switch = true;
+				try_sdio = false;
+
+				goto switch_fail;
+			}
+		} else {
+			if (support_1v8) {
+				sd_init_power(chip);
+				sd_dont_switch = true;
+				try_sdio = false;
+
+				goto switch_fail;
+			}
+		}
+	}
+
+	if (!support_1v8) {
+		retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
+					     SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
+					     SD_RSP_TYPE_R1, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+
+	if (!sd20_mode && CHK_SD30_SPEED(sd_card)) {
+		int read_lba0 = 1;
+
+		retval = rtsx_write_register(chip, SD30_DRIVE_SEL, 0x07,
+					     chip->sd30_drive_sel_1v8);
+		if (retval)
+			return retval;
+
+		retval = sd_set_init_para(chip);
+		if (retval != STATUS_SUCCESS)
+			goto status_fail;
+
+		if (CHK_SD_DDR50(sd_card))
+			retval = sd_ddr_tuning(chip);
+		else
+			retval = sd_sdr_tuning(chip);
+
+		if (retval != STATUS_SUCCESS) {
+			retval = sd_init_power(chip);
+			if (retval != STATUS_SUCCESS)
+				goto status_fail;
+
+			try_sdio = false;
+			sd20_mode = true;
+			goto switch_fail;
+		}
+
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+
+		if (CHK_SD_DDR50(sd_card)) {
+			retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+			if (retval != STATUS_SUCCESS)
+				read_lba0 = 0;
+		}
+
+		if (read_lba0) {
+			retval = sd_read_lba0(chip);
+			if (retval != STATUS_SUCCESS) {
+				retval = sd_init_power(chip);
+				if (retval != STATUS_SUCCESS)
+					goto status_fail;
+
+				try_sdio = false;
+				sd20_mode = true;
+				goto switch_fail;
+			}
+		}
+	}
+
+	retval = sd_check_wp_state(chip);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_H, 0xFF,
+					     0x02);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_L, 0xFF,
+					     0x00);
+		if (retval)
+			return retval;
+	}
+#endif
+
+	return STATUS_SUCCESS;
+
+status_fail:
+	return STATUS_FAIL;
+}
+
+static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 buf[8] = {0}, bus_width, *ptr;
+	u16 byte_cnt;
+	int len;
+
+	retval = sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL,
+				     0);
+	if (retval != STATUS_SUCCESS)
+		return SWITCH_FAIL;
+
+	if (width == MMC_8BIT_BUS) {
+		buf[0] = 0x55;
+		buf[1] = 0xAA;
+		len = 8;
+		byte_cnt = 8;
+		bus_width = SD_BUS_WIDTH_8;
+	} else {
+		buf[0] = 0x5A;
+		len = 4;
+		byte_cnt = 4;
+		bus_width = SD_BUS_WIDTH_4;
+	}
+
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0x02);
+	if (retval != STATUS_SUCCESS)
+		return SWITCH_ERR;
+
+	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_3, NULL, 0, byte_cnt, 1,
+			       bus_width, buf, len, 100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_sd_error(chip);
+		rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0);
+		return SWITCH_ERR;
+	}
+
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0);
+	if (retval != STATUS_SUCCESS)
+		return SWITCH_ERR;
+
+	dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n", BUSTEST_R);
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF, 0x40 | BUSTEST_R);
+
+	if (width == MMC_8BIT_BUS)
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L,
+			     0xFF, 0x08);
+	else
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L,
+			     0xFF, 0x04);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, SD_CALCULATE_CRC7 |
+		     SD_NO_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
+		     SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+		     SD_TM_NORMAL_READ | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2, 0, 0);
+	if (width == MMC_8BIT_BUS)
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 1, 0, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 100);
+	if (retval < 0) {
+		rtsx_clear_sd_error(chip);
+		return SWITCH_ERR;
+	}
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if (width == MMC_8BIT_BUS) {
+		dev_dbg(rtsx_dev(chip), "BUSTEST_R [8bits]: 0x%02x 0x%02x\n",
+			ptr[0], ptr[1]);
+		if (ptr[0] == 0xAA && ptr[1] == 0x55) {
+			u8 rsp[5];
+			u32 arg;
+
+			if (CHK_MMC_DDR52(sd_card))
+				arg = 0x03B70600;
+			else
+				arg = 0x03B70200;
+
+			retval = sd_send_cmd_get_rsp(chip, SWITCH, arg,
+						     SD_RSP_TYPE_R1b, rsp, 5);
+			if (retval == STATUS_SUCCESS &&
+			    !(rsp[4] & MMC_SWITCH_ERR))
+				return SWITCH_SUCCESS;
+		}
+	} else {
+		dev_dbg(rtsx_dev(chip), "BUSTEST_R [4bits]: 0x%02x\n", ptr[0]);
+		if (ptr[0] == 0xA5) {
+			u8 rsp[5];
+			u32 arg;
+
+			if (CHK_MMC_DDR52(sd_card))
+				arg = 0x03B70500;
+			else
+				arg = 0x03B70100;
+
+			retval = sd_send_cmd_get_rsp(chip, SWITCH, arg,
+						     SD_RSP_TYPE_R1b, rsp, 5);
+			if (retval == STATUS_SUCCESS &&
+			    !(rsp[4] & MMC_SWITCH_ERR))
+				return SWITCH_SUCCESS;
+		}
+	}
+
+	return SWITCH_FAIL;
+}
+
+static int mmc_switch_timing_bus(struct rtsx_chip *chip, bool switch_ddr)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+	u8 *ptr, card_type, card_type_mask = 0;
+
+	CLR_MMC_HS(sd_card);
+
+	dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n", SEND_EXT_CSD);
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF,
+		     0x40 | SEND_EXT_CSD);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD1, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD2, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD3, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD4, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
+		     SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
+		     SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+		     SD_TM_NORMAL_READ | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 196, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 212, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 213, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 214, 0xFF, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 215, 0xFF, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 1000);
+	if (retval < 0) {
+		if (retval == -ETIMEDOUT) {
+			rtsx_clear_sd_error(chip);
+			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					    SD_RSP_TYPE_R1, NULL, 0);
+		}
+		return STATUS_FAIL;
+	}
+
+	ptr = rtsx_get_cmd_data(chip);
+	if (ptr[0] & SD_TRANSFER_ERR) {
+		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+				    SD_RSP_TYPE_R1, NULL, 0);
+		return STATUS_FAIL;
+	}
+
+	if (CHK_MMC_SECTOR_MODE(sd_card)) {
+		sd_card->capacity = ((u32)ptr[5] << 24) | ((u32)ptr[4] << 16) |
+			((u32)ptr[3] << 8) | ((u32)ptr[2]);
+	}
+
+	card_type_mask = 0x03;
+	card_type = ptr[1] & card_type_mask;
+	if (card_type) {
+		u8 rsp[5];
+
+		if (card_type & 0x04) {
+			if (switch_ddr)
+				SET_MMC_DDR52(sd_card);
+			else
+				SET_MMC_52M(sd_card);
+		} else if (card_type & 0x02) {
+			SET_MMC_52M(sd_card);
+		} else {
+			SET_MMC_26M(sd_card);
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, SWITCH, 0x03B90100,
+					     SD_RSP_TYPE_R1b, rsp, 5);
+		if (retval != STATUS_SUCCESS || (rsp[4] & MMC_SWITCH_ERR))
+			CLR_MMC_HS(sd_card);
+	}
+
+	sd_choose_proper_clock(chip);
+	retval = switch_clock(chip, sd_card->sd_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	/* Test Bus Procedure */
+	retval = mmc_test_switch_bus(chip, MMC_8BIT_BUS);
+	if (retval == SWITCH_SUCCESS) {
+		SET_MMC_8BIT(sd_card);
+		chip->card_bus_width[chip->card2lun[SD_CARD]] = 8;
+#ifdef SUPPORT_SD_LOCK
+		sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+	} else if (retval == SWITCH_FAIL) {
+		retval = mmc_test_switch_bus(chip, MMC_4BIT_BUS);
+		if (retval == SWITCH_SUCCESS) {
+			SET_MMC_4BIT(sd_card);
+			chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
+#ifdef SUPPORT_SD_LOCK
+			sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
+#endif
+		} else if (retval == SWITCH_FAIL) {
+			CLR_MMC_8BIT(sd_card);
+			CLR_MMC_4BIT(sd_card);
+		} else {
+			return STATUS_FAIL;
+		}
+	} else {
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_mmc(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval, i = 0, j = 0, k = 0;
+	bool switch_ddr = true;
+	u8 rsp[16];
+	u8 spec_ver = 0;
+	u32 temp;
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON)
+		goto MMC_UNLOCK_ENTRY;
+#endif
+
+switch_fail:
+	retval = sd_prepare_reset(chip);
+	if (retval != STATUS_SUCCESS)
+		return retval;
+
+	SET_MMC(sd_card);
+
+RTY_MMC_RST:
+	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	do {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_OP_COND,
+					     (SUPPORT_VOLTAGE | 0x40000000),
+					     SD_RSP_TYPE_R3, rsp, 5);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_BUSY) ||
+			    sd_check_err_code(chip, SD_TO_ERR)) {
+				k++;
+				if (k < 20) {
+					sd_clr_err_code(chip);
+					goto RTY_MMC_RST;
+				} else {
+					return STATUS_FAIL;
+				}
+			} else {
+				j++;
+				if (j < 100) {
+					sd_clr_err_code(chip);
+					goto RTY_MMC_RST;
+				} else {
+					return STATUS_FAIL;
+				}
+			}
+		}
+
+		wait_timeout(20);
+		i++;
+	} while (!(rsp[1] & 0x80) && (i < 255));
+
+	if (i == 255)
+		return STATUS_FAIL;
+
+	if ((rsp[1] & 0x60) == 0x40)
+		SET_MMC_SECTOR_MODE(sd_card);
+	else
+		CLR_MMC_SECTOR_MODE(sd_card);
+
+	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	sd_card->sd_addr = 0x00100000;
+	retval = sd_send_cmd_get_rsp(chip, SET_RELATIVE_ADDR, sd_card->sd_addr,
+				     SD_RSP_TYPE_R6, rsp, 5);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = sd_check_csd(chip, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
+
+	retval = sd_select_card(chip, 1);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
+				     NULL, 0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+#ifdef SUPPORT_SD_LOCK
+MMC_UNLOCK_ENTRY:
+	retval = sd_update_lock_status(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+#endif
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
+
+	if (!sd_card->mmc_dont_switch_bus) {
+		if (spec_ver == 4) {
+			/* MMC 4.x Cards */
+			retval = mmc_switch_timing_bus(chip, switch_ddr);
+			if (retval != STATUS_SUCCESS) {
+				retval = sd_init_power(chip);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+				sd_card->mmc_dont_switch_bus = 1;
+				goto switch_fail;
+			}
+		}
+
+		if (CHK_MMC_SECTOR_MODE(sd_card) && sd_card->capacity == 0)
+			return STATUS_FAIL;
+
+		if (switch_ddr && CHK_MMC_DDR52(sd_card)) {
+			retval = sd_set_init_para(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			retval = mmc_ddr_tuning(chip);
+			if (retval != STATUS_SUCCESS) {
+				retval = sd_init_power(chip);
+				if (retval != STATUS_SUCCESS)
+					return STATUS_FAIL;
+
+				switch_ddr = false;
+				goto switch_fail;
+			}
+
+			retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+			if (retval == STATUS_SUCCESS) {
+				retval = sd_read_lba0(chip);
+				if (retval != STATUS_SUCCESS) {
+					retval = sd_init_power(chip);
+					if (retval != STATUS_SUCCESS)
+						return STATUS_FAIL;
+
+					switch_ddr = false;
+					goto switch_fail;
+				}
+			}
+		}
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_H, 0xFF,
+					     0x02);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_L, 0xFF,
+					     0x00);
+		if (retval)
+			return retval;
+	}
+#endif
+
+	temp = rtsx_readl(chip, RTSX_BIPR);
+	if (temp & SD_WRITE_PROTECT)
+		chip->card_wp |= SD_CARD;
+
+	return STATUS_SUCCESS;
+}
+
+int reset_sd_card(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	sd_init_reg_addr(chip);
+
+	memset(sd_card, 0, sizeof(struct sd_info));
+	chip->capacity[chip->card2lun[SD_CARD]] = 0;
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (chip->ignore_sd && CHK_SDIO_EXIST(chip) &&
+	    !CHK_SDIO_IGNORED(chip)) {
+		if (chip->asic_code) {
+			retval = sd_pull_ctl_enable(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		} else {
+			retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+						     FPGA_SD_PULL_CTL_BIT |
+						     0x20, 0);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		}
+		retval = card_share_mode(chip, SD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		chip->sd_io = 1;
+		return STATUS_FAIL;
+	}
+
+	retval = sd_init_power(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (chip->sd_ctl & RESET_MMC_FIRST) {
+		retval = reset_mmc(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_NO_CARD))
+				return STATUS_FAIL;
+
+			retval = reset_sd(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		}
+	} else {
+		retval = reset_sd(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (sd_check_err_code(chip, SD_NO_CARD))
+				return STATUS_FAIL;
+
+			if (chip->sd_io)
+				return STATUS_FAIL;
+			retval = reset_mmc(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+		}
+	}
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	if (retval)
+		return retval;
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "sd_card->sd_type = 0x%x\n", sd_card->sd_type);
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_mmc_only(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	sd_card->sd_type = 0;
+	sd_card->seq_mode = 0;
+	sd_card->sd_data_buf_ready = 0;
+	sd_card->capacity = 0;
+	sd_card->sd_switch_fail = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status = 0;
+	sd_card->sd_erase_status = 0;
+#endif
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity = 0;
+
+	retval = enable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = sd_init_power(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = reset_mmc(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	if (retval)
+		return retval;
+
+	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
+
+	retval = sd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	dev_dbg(rtsx_dev(chip), "In %s, sd_card->sd_type = 0x%x\n",
+		__func__, sd_card->sd_type);
+
+	return STATUS_SUCCESS;
+}
+
+#define WAIT_DATA_READY_RTY_CNT		255
+
+static int wait_data_buf_ready(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int i, retval;
+
+	for (i = 0; i < WAIT_DATA_READY_RTY_CNT; i++) {
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		sd_card->sd_data_buf_ready = 0;
+
+		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
+					     sd_card->sd_addr, SD_RSP_TYPE_R1,
+					     NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		if (sd_card->sd_data_buf_ready) {
+			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
+				sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
+		}
+	}
+
+	sd_set_err_code(chip, SD_TO_ERR);
+
+	return STATUS_FAIL;
+}
+
+void sd_stop_seq_mode(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	if (sd_card->seq_mode) {
+		retval = sd_switch_clock(chip);
+		if (retval != STATUS_SUCCESS)
+			return;
+
+		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+					     SD_RSP_TYPE_R1b, NULL, 0);
+		if (retval != STATUS_SUCCESS)
+			sd_set_err_code(chip, SD_STS_ERR);
+
+		retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
+		if (retval != STATUS_SUCCESS)
+			sd_set_err_code(chip, SD_STS_ERR);
+
+		sd_card->seq_mode = 0;
+
+		rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+	}
+}
+
+static inline int sd_auto_tune_clock(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	if (chip->asic_code) {
+		if (sd_card->sd_clock > 30)
+			sd_card->sd_clock -= 20;
+	} else {
+		switch (sd_card->sd_clock) {
+		case CLK_200:
+			sd_card->sd_clock = CLK_150;
+			break;
+
+		case CLK_150:
+			sd_card->sd_clock = CLK_120;
+			break;
+
+		case CLK_120:
+			sd_card->sd_clock = CLK_100;
+			break;
+
+		case CLK_100:
+			sd_card->sd_clock = CLK_80;
+			break;
+
+		case CLK_80:
+			sd_card->sd_clock = CLK_60;
+			break;
+
+		case CLK_60:
+			sd_card->sd_clock = CLK_50;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
+	  u16 sector_cnt)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	u32 data_addr;
+	u8 cfg2;
+	int retval;
+
+	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		dev_dbg(rtsx_dev(chip), "%s: Read %d %s from 0x%x\n", __func__,
+			sector_cnt, (sector_cnt > 1) ? "sectors" : "sector",
+			start_sector);
+	} else {
+		dev_dbg(rtsx_dev(chip), "%s: Write %d %s to 0x%x\n", __func__,
+			sector_cnt, (sector_cnt > 1) ? "sectors" : "sector",
+			start_sector);
+	}
+
+	sd_card->cleanup_counter = 0;
+
+	if (!(chip->card_ready & SD_CARD)) {
+		sd_card->seq_mode = 0;
+
+		retval = reset_sd_card(chip);
+		if (retval == STATUS_SUCCESS) {
+			chip->card_ready |= SD_CARD;
+			chip->card_fail &= ~SD_CARD;
+		} else {
+			chip->card_ready &= ~SD_CARD;
+			chip->card_fail |= SD_CARD;
+			chip->capacity[chip->card2lun[SD_CARD]] = 0;
+			chip->rw_need_retry = 1;
+			return STATUS_FAIL;
+		}
+	}
+
+	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card))
+		data_addr = start_sector << 9;
+	else
+		data_addr = start_sector;
+
+	sd_clr_err_code(chip);
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS) {
+		sd_set_err_code(chip, SD_IO_ERR);
+		goto RW_FAIL;
+	}
+
+	if (sd_card->seq_mode &&
+	    (sd_card->pre_dir != srb->sc_data_direction ||
+	    ((sd_card->pre_sec_addr + sd_card->pre_sec_cnt) !=
+	    start_sector))) {
+		if (sd_card->pre_sec_cnt < 0x80 &&
+		    sd_card->pre_dir == DMA_FROM_DEVICE &&
+		    !CHK_SD30_SPEED(sd_card) &&
+		    !CHK_SD_HS(sd_card) &&
+		    !CHK_MMC_HS(sd_card)) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					    SD_RSP_TYPE_R1, NULL, 0);
+		}
+
+		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+					     SD_RSP_TYPE_R1b, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			chip->rw_need_retry = 1;
+			sd_set_err_code(chip, SD_STS_ERR);
+			goto RW_FAIL;
+		}
+
+		sd_card->seq_mode = 0;
+
+		retval = rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
+		if (retval != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_IO_ERR);
+			goto RW_FAIL;
+		}
+
+		if (sd_card->pre_sec_cnt < 0x80 &&
+		    !CHK_SD30_SPEED(sd_card) &&
+		    !CHK_SD_HS(sd_card) &&
+		    !CHK_MMC_HS(sd_card)) {
+			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
+					    SD_RSP_TYPE_R1, NULL, 0);
+		}
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF, 0x00);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF, 0x02);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF,
+		     (u8)sector_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF,
+		     (u8)(sector_cnt >> 8));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+
+	if (CHK_MMC_8BIT(sd_card))
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1,
+			     0x03, SD_BUS_WIDTH_8);
+	else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card))
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1,
+			     0x03, SD_BUS_WIDTH_4);
+	else
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1,
+			     0x03, SD_BUS_WIDTH_1);
+
+	if (sd_card->seq_mode) {
+		cfg2 = SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+			SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 |
+			SD_RSP_LEN_0;
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, cfg2);
+
+		trans_dma_enable(srb->sc_data_direction, chip, sector_cnt * 512,
+				 DMA_512);
+
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+				     SD_TM_AUTO_READ_3 | SD_TRANSFER_START);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+				     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+		}
+
+		rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+	} else {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n",
+				READ_MULTIPLE_BLOCK);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF,
+				     0x40 | READ_MULTIPLE_BLOCK);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD1, 0xFF,
+				     (u8)(data_addr >> 24));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD2, 0xFF,
+				     (u8)(data_addr >> 16));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD3, 0xFF,
+				     (u8)(data_addr >> 8));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD4, 0xFF,
+				     (u8)data_addr);
+
+			cfg2 = SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+				SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 |
+				SD_RSP_LEN_6;
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
+				     cfg2);
+
+			trans_dma_enable(srb->sc_data_direction, chip,
+					 sector_cnt * 512, DMA_512);
+
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+				     SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
+			rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+				     SD_TRANSFER_END, SD_TRANSFER_END);
+
+			rtsx_send_cmd_no_wait(chip);
+		} else {
+			retval = rtsx_send_cmd(chip, SD_CARD, 50);
+			if (retval < 0) {
+				rtsx_clear_sd_error(chip);
+
+				chip->rw_need_retry = 1;
+				sd_set_err_code(chip, SD_TO_ERR);
+				goto RW_FAIL;
+			}
+
+			retval = wait_data_buf_ready(chip);
+			if (retval != STATUS_SUCCESS) {
+				chip->rw_need_retry = 1;
+				sd_set_err_code(chip, SD_TO_ERR);
+				goto RW_FAIL;
+			}
+
+			retval = sd_send_cmd_get_rsp(chip, WRITE_MULTIPLE_BLOCK,
+						     data_addr, SD_RSP_TYPE_R1,
+						     NULL, 0);
+			if (retval != STATUS_SUCCESS) {
+				chip->rw_need_retry = 1;
+				goto RW_FAIL;
+			}
+
+			rtsx_init_cmd(chip);
+
+			cfg2 = SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+				SD_NO_WAIT_BUSY_END |
+				SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
+				     cfg2);
+
+			trans_dma_enable(srb->sc_data_direction, chip,
+					 sector_cnt * 512, DMA_512);
+
+			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+				     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+			rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+				     SD_TRANSFER_END, SD_TRANSFER_END);
+
+			rtsx_send_cmd_no_wait(chip);
+		}
+
+		sd_card->seq_mode = 1;
+	}
+
+	retval = rtsx_transfer_data(chip, SD_CARD, scsi_sglist(srb),
+				    scsi_bufflen(srb), scsi_sg_count(srb),
+				srb->sc_data_direction, chip->sd_timeout);
+	if (retval < 0) {
+		u8 stat = 0;
+		int err;
+
+		sd_card->seq_mode = 0;
+
+		if (retval == -ETIMEDOUT)
+			err = STATUS_TIMEDOUT;
+		else
+			err = STATUS_FAIL;
+
+		rtsx_read_register(chip, REG_SD_STAT1, &stat);
+		rtsx_clear_sd_error(chip);
+		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+			chip->rw_need_retry = 0;
+			dev_dbg(rtsx_dev(chip), "No card exist, exit %s\n",
+				__func__);
+			return STATUS_FAIL;
+		}
+
+		chip->rw_need_retry = 1;
+
+		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+					     SD_RSP_TYPE_R1b, NULL, 0);
+		if (retval != STATUS_SUCCESS) {
+			sd_set_err_code(chip, SD_STS_ERR);
+			goto RW_FAIL;
+		}
+
+		if (stat & (SD_CRC7_ERR | SD_CRC16_ERR | SD_CRC_WRITE_ERR)) {
+			dev_dbg(rtsx_dev(chip), "SD CRC error, tune clock!\n");
+			sd_set_err_code(chip, SD_CRC_ERR);
+			goto RW_FAIL;
+		}
+
+		if (err == STATUS_TIMEDOUT) {
+			sd_set_err_code(chip, SD_TO_ERR);
+			goto RW_FAIL;
+		}
+
+		return err;
+	}
+
+	sd_card->pre_sec_addr = start_sector;
+	sd_card->pre_sec_cnt = sector_cnt;
+	sd_card->pre_dir = srb->sc_data_direction;
+
+	return STATUS_SUCCESS;
+
+RW_FAIL:
+	sd_card->seq_mode = 0;
+
+	if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
+		chip->rw_need_retry = 0;
+		dev_dbg(rtsx_dev(chip), "No card exist, exit %s\n", __func__);
+		return STATUS_FAIL;
+	}
+
+	if (sd_check_err_code(chip, SD_CRC_ERR)) {
+		if (CHK_MMC_4BIT(sd_card) || CHK_MMC_8BIT(sd_card)) {
+			sd_card->mmc_dont_switch_bus = 1;
+			reset_mmc_only(chip);
+			sd_card->mmc_dont_switch_bus = 0;
+		} else {
+			sd_card->need_retune = 1;
+			sd_auto_tune_clock(chip);
+		}
+	} else if (sd_check_err_code(chip, SD_TO_ERR | SD_STS_ERR)) {
+		retval = reset_sd_card(chip);
+		if (retval != STATUS_SUCCESS) {
+			chip->card_ready &= ~SD_CARD;
+			chip->card_fail |= SD_CARD;
+			chip->capacity[chip->card2lun[SD_CARD]] = 0;
+		}
+	}
+
+	return STATUS_FAIL;
+}
+
+#ifdef SUPPORT_CPRM
+int ext_sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx, u32 arg,
+			    u8 rsp_type, u8 *rsp, int rsp_len,
+			    bool special_check)
+{
+	int retval;
+	int timeout = 100;
+	u16 reg_addr;
+	u8 *ptr;
+	int stat_idx = 0;
+	int rty_cnt = 0;
+
+	dev_dbg(rtsx_dev(chip), "EXT SD/MMC CMD %d\n", cmd_idx);
+
+	if (rsp_type == SD_RSP_TYPE_R1b)
+		timeout = 3000;
+
+RTY_SEND_CMD:
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF, 0x40 | cmd_idx);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD1, 0xFF, (u8)(arg >> 24));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD2, 0xFF, (u8)(arg >> 16));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD3, 0xFF, (u8)(arg >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD4, 0xFF, (u8)arg);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, rsp_type);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER,
+		     0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END,
+		     SD_TRANSFER_END);
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+		stat_idx = 17;
+	} else if (rsp_type != SD_RSP_TYPE_R0) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
+
+		stat_idx = 6;
+	}
+	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_CMD5, 0, 0);
+
+	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_STAT1, 0, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, timeout);
+	if (retval < 0) {
+		if (retval == -ETIMEDOUT) {
+			rtsx_clear_sd_error(chip);
+
+			if (rsp_type & SD_WAIT_BUSY_END) {
+				retval = sd_check_data0_status(chip);
+				if (retval != STATUS_SUCCESS)
+					return retval;
+			} else {
+				sd_set_err_code(chip, SD_TO_ERR);
+			}
+		}
+		return STATUS_FAIL;
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R0)
+		return STATUS_SUCCESS;
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+
+	if ((ptr[0] & 0xC0) != 0) {
+		sd_set_err_code(chip, SD_STS_ERR);
+		return STATUS_FAIL;
+	}
+
+	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
+		if (ptr[stat_idx] & SD_CRC7_ERR) {
+			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				return STATUS_FAIL;
+			}
+			if (rty_cnt < SD_MAX_RETRY_COUNT) {
+				wait_timeout(20);
+				rty_cnt++;
+				goto RTY_SEND_CMD;
+			} else {
+				sd_set_err_code(chip, SD_CRC_ERR);
+				return STATUS_FAIL;
+			}
+		}
+	}
+
+	if (cmd_idx == SELECT_CARD || cmd_idx == APP_CMD ||
+	    cmd_idx == SEND_STATUS || cmd_idx == STOP_TRANSMISSION) {
+		if (cmd_idx != STOP_TRANSMISSION && !special_check) {
+			if (ptr[1] & 0x80)
+				return STATUS_FAIL;
+		}
+#ifdef SUPPORT_SD_LOCK
+		if (ptr[1] & 0x7D) {
+#else
+		if (ptr[1] & 0x7F) {
+#endif
+			return STATUS_FAIL;
+		}
+		if (ptr[2] & 0xF8)
+			return STATUS_FAIL;
+
+		if (cmd_idx == SELECT_CARD) {
+			if (rsp_type == SD_RSP_TYPE_R2) {
+				if ((ptr[3] & 0x1E) != 0x04)
+					return STATUS_FAIL;
+			}
+		}
+	}
+
+	if (rsp && rsp_len)
+		memcpy(rsp, ptr, rsp_len);
+
+	return STATUS_SUCCESS;
+}
+
+int ext_sd_get_rsp(struct rtsx_chip *chip, int len, u8 *rsp, u8 rsp_type)
+{
+	int retval, rsp_len;
+	u16 reg_addr;
+
+	if (rsp_type == SD_RSP_TYPE_R0)
+		return STATUS_SUCCESS;
+
+	rtsx_init_cmd(chip);
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
+
+		rsp_len = 17;
+	} else if (rsp_type != SD_RSP_TYPE_R0) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4;
+		     reg_addr++)
+			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
+
+		rsp_len = 6;
+	}
+	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_CMD5, 0xFF, 0);
+
+	retval = rtsx_send_cmd(chip, SD_CARD, 100);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (rsp) {
+		int min_len = (rsp_len < len) ? rsp_len : len;
+
+		memcpy(rsp, rtsx_get_cmd_data(chip), min_len);
+
+		dev_dbg(rtsx_dev(chip), "min_len = %d\n", min_len);
+		dev_dbg(rtsx_dev(chip), "Response in cmd buf: 0x%x 0x%x 0x%x 0x%x\n",
+			rsp[0], rsp[1], rsp[2], rsp[3]);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int len;
+	u8 buf[18] = {
+		0x00,
+		0x00,
+		0x00,
+		0x0E,
+		0x00,
+		0x00,
+		0x00,
+		0x00,
+		0x53,
+		0x44,
+		0x20,
+		0x43,
+		0x61,
+		0x72,
+		0x64,
+		0x00,
+		0x00,
+		0x00,
+	};
+
+	sd_card->pre_cmd_err = 0;
+
+	if (!(CHK_BIT(chip->lun_mc, lun))) {
+		SET_BIT(chip->lun_mc, lun);
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[2] != 0x53 || srb->cmnd[3] != 0x44 ||
+	    srb->cmnd[4] != 0x20 || srb->cmnd[5] != 0x43 ||
+	    srb->cmnd[6] != 0x61 || srb->cmnd[7] != 0x72 ||
+	    srb->cmnd[8] != 0x64) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	switch (srb->cmnd[1] & 0x0F) {
+	case 0:
+		sd_card->sd_pass_thru_en = 0;
+		break;
+
+	case 1:
+		sd_card->sd_pass_thru_en = 1;
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	buf[5] = (CHK_SD(sd_card) == 1) ? 0x01 : 0x02;
+	if (chip->card_wp & SD_CARD)
+		buf[5] |= 0x80;
+
+	buf[6] = (u8)(sd_card->sd_addr >> 16);
+	buf[7] = (u8)(sd_card->sd_addr >> 24);
+
+	buf[15] = chip->max_lun;
+
+	len = min_t(int, 18, scsi_bufflen(srb));
+	rtsx_stor_set_xfer_buf(buf, len, srb);
+
+	return TRANSPORT_GOOD;
+}
+
+static inline int get_rsp_type(struct scsi_cmnd *srb, u8 *rsp_type,
+			       int *rsp_len)
+{
+	if (!rsp_type || !rsp_len)
+		return STATUS_FAIL;
+
+	switch (srb->cmnd[10]) {
+	case 0x03:
+		*rsp_type = SD_RSP_TYPE_R0;
+		*rsp_len = 0;
+		break;
+
+	case 0x04:
+		*rsp_type = SD_RSP_TYPE_R1;
+		*rsp_len = 6;
+		break;
+
+	case 0x05:
+		*rsp_type = SD_RSP_TYPE_R1b;
+		*rsp_len = 6;
+		break;
+
+	case 0x06:
+		*rsp_type = SD_RSP_TYPE_R2;
+		*rsp_len = 17;
+		break;
+
+	case 0x07:
+		*rsp_type = SD_RSP_TYPE_R3;
+		*rsp_len = 6;
+		break;
+
+	default:
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, rsp_len;
+	u8 cmd_idx, rsp_type;
+	bool standby = false, acmd = false;
+	u32 arg;
+
+	if (!sd_card->sd_pass_thru_en) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+	if (sd_card->pre_cmd_err) {
+		sd_card->pre_cmd_err = 0;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	cmd_idx = srb->cmnd[2] & 0x3F;
+	if (srb->cmnd[1] & 0x02)
+		standby = true;
+
+	if (srb->cmnd[1] & 0x01)
+		acmd = true;
+
+	arg = ((u32)srb->cmnd[3] << 24) | ((u32)srb->cmnd[4] << 16) |
+		((u32)srb->cmnd[5] << 8) | srb->cmnd[6];
+
+	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+	sd_card->last_rsp_type = rsp_type;
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+#ifdef SUPPORT_SD_LOCK
+	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
+		if (CHK_MMC_8BIT(sd_card)) {
+			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
+						     SD_BUS_WIDTH_8);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
+			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
+						     SD_BUS_WIDTH_4);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+		}
+	}
+#else
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+#endif
+
+	if (standby) {
+		retval = sd_select_card(chip, 0);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_cmd_failed;
+	}
+
+	if (acmd) {
+		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
+						 sd_card->sd_addr,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_cmd_failed;
+	}
+
+	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
+					 sd_card->rsp, rsp_len, false);
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_cmd_failed;
+
+	if (standby) {
+		retval = sd_select_card(chip, 1);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_cmd_failed;
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	retval = sd_update_lock_status(chip);
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_cmd_failed;
+#endif
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+
+sd_execute_cmd_failed:
+	sd_card->pre_cmd_err = 1;
+	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+	release_sd_card(chip);
+	do_reset_sd_card(chip);
+	if (!(chip->card_ready & SD_CARD))
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+
+	return TRANSPORT_FAILED;
+}
+
+int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, rsp_len, i;
+	bool read_err = false, cmd13_checkbit = false;
+	u8 cmd_idx, rsp_type, bus_width;
+	bool standby = false, send_cmd12 = false, acmd = false;
+	u32 data_len;
+
+	if (!sd_card->sd_pass_thru_en) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (sd_card->pre_cmd_err) {
+		sd_card->pre_cmd_err = 0;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+	cmd_idx = srb->cmnd[2] & 0x3F;
+	if (srb->cmnd[1] & 0x04)
+		send_cmd12 = true;
+
+	if (srb->cmnd[1] & 0x02)
+		standby = true;
+
+	if (srb->cmnd[1] & 0x01)
+		acmd = true;
+
+	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8]
+						<< 8) | srb->cmnd[9];
+
+	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+	sd_card->last_rsp_type = rsp_type;
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+#ifdef SUPPORT_SD_LOCK
+	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
+		if (CHK_MMC_8BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_8;
+		else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card))
+			bus_width = SD_BUS_WIDTH_4;
+		else
+			bus_width = SD_BUS_WIDTH_1;
+	} else {
+		bus_width = SD_BUS_WIDTH_4;
+	}
+	dev_dbg(rtsx_dev(chip), "bus_width = %d\n", bus_width);
+#else
+	bus_width = SD_BUS_WIDTH_4;
+#endif
+
+	if (data_len < 512) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if (standby) {
+		retval = sd_select_card(chip, 0);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if (acmd) {
+		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
+						 sd_card->sd_addr,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if (data_len <= 512) {
+		int min_len;
+		u8 *buf;
+		u16 byte_cnt, blk_cnt;
+		u8 cmd[5];
+
+		byte_cnt = ((u16)(srb->cmnd[8] & 0x03) << 8) | srb->cmnd[9];
+		blk_cnt = 1;
+
+		cmd[0] = 0x40 | cmd_idx;
+		cmd[1] = srb->cmnd[3];
+		cmd[2] = srb->cmnd[4];
+		cmd[3] = srb->cmnd[5];
+		cmd[4] = srb->cmnd[6];
+
+		buf = kmalloc(data_len, GFP_KERNEL);
+		if (!buf)
+			return TRANSPORT_ERROR;
+
+		retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, byte_cnt,
+				      blk_cnt, bus_width, buf, data_len, 2000);
+		if (retval != STATUS_SUCCESS) {
+			read_err = true;
+			kfree(buf);
+			rtsx_clear_sd_error(chip);
+			goto sd_execute_read_cmd_failed;
+		}
+
+		min_len = min(data_len, scsi_bufflen(srb));
+		rtsx_stor_set_xfer_buf(buf, min_len, srb);
+
+		kfree(buf);
+	} else if (!(data_len & 0x1FF)) {
+		rtsx_init_cmd(chip);
+
+		trans_dma_enable(DMA_FROM_DEVICE, chip, data_len, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF,
+			     0x02);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF,
+			     0x00);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H,
+			     0xFF, (srb->cmnd[7] & 0xFE) >> 1);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L,
+			     0xFF, (u8)((data_len & 0x0001FE00) >> 9));
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF,
+			     0x40 | cmd_idx);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD1, 0xFF,
+			     srb->cmnd[3]);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD2, 0xFF,
+			     srb->cmnd[4]);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD3, 0xFF,
+			     srb->cmnd[5]);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD4, 0xFF,
+			     srb->cmnd[6]);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, bus_width);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, rsp_type);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER,
+			     0xFF, SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data(chip, SD_CARD, scsi_sglist(srb),
+					    scsi_bufflen(srb),
+					    scsi_sg_count(srb),
+					    DMA_FROM_DEVICE, 10000);
+		if (retval < 0) {
+			read_err = true;
+			rtsx_clear_sd_error(chip);
+			goto sd_execute_read_cmd_failed;
+		}
+
+	} else {
+		goto sd_execute_read_cmd_failed;
+	}
+
+	retval = ext_sd_get_rsp(chip, rsp_len, sd_card->rsp, rsp_type);
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_read_cmd_failed;
+
+	if (standby) {
+		retval = sd_select_card(chip, 1);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if (send_cmd12) {
+		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+						 SD_RSP_TYPE_R1b, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if (data_len < 512) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+
+		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+
+		retval = rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_read_cmd_failed;
+	}
+
+	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
+		cmd13_checkbit = true;
+
+	for (i = 0; i < 3; i++) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS,
+						 sd_card->sd_addr,
+						SD_RSP_TYPE_R1, NULL, 0,
+						cmd13_checkbit);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_read_cmd_failed;
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+
+sd_execute_read_cmd_failed:
+	sd_card->pre_cmd_err = 1;
+	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+	if (read_err)
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+
+	release_sd_card(chip);
+	do_reset_sd_card(chip);
+	if (!(chip->card_ready & SD_CARD))
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+
+	return TRANSPORT_FAILED;
+}
+
+int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval, rsp_len, i;
+	bool write_err = false, cmd13_checkbit = false;
+	u8 cmd_idx, rsp_type;
+	bool standby = false, send_cmd12 = false, acmd = false;
+	u32 data_len, arg;
+#ifdef SUPPORT_SD_LOCK
+	int lock_cmd_fail = 0;
+	u8 sd_lock_state = 0;
+	u8 lock_cmd_type = 0;
+#endif
+
+	if (!sd_card->sd_pass_thru_en) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (sd_card->pre_cmd_err) {
+		sd_card->pre_cmd_err = 0;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+	cmd_idx = srb->cmnd[2] & 0x3F;
+	if (srb->cmnd[1] & 0x04)
+		send_cmd12 = true;
+
+	if (srb->cmnd[1] & 0x02)
+		standby = true;
+
+	if (srb->cmnd[1] & 0x01)
+		acmd = true;
+
+	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8]
+						<< 8) | srb->cmnd[9];
+	arg = ((u32)srb->cmnd[3] << 24) | ((u32)srb->cmnd[4] << 16) |
+		((u32)srb->cmnd[5] << 8) | srb->cmnd[6];
+
+#ifdef SUPPORT_SD_LOCK
+	if (cmd_idx == LOCK_UNLOCK) {
+		sd_lock_state = sd_card->sd_lock_status;
+		sd_lock_state &= SD_LOCKED;
+	}
+#endif
+
+	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
+	if (retval != STATUS_SUCCESS) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+	sd_card->last_rsp_type = rsp_type;
+
+	retval = sd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+
+#ifdef SUPPORT_SD_LOCK
+	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
+		if (CHK_MMC_8BIT(sd_card)) {
+			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
+						     SD_BUS_WIDTH_8);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+
+		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
+			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
+						     SD_BUS_WIDTH_4);
+			if (retval != STATUS_SUCCESS)
+				return TRANSPORT_FAILED;
+		}
+	}
+#else
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
+	if (retval != STATUS_SUCCESS)
+		return TRANSPORT_FAILED;
+#endif
+
+	if (data_len < 512) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	if (standby) {
+		retval = sd_select_card(chip, 0);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	if (acmd) {
+		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
+						 sd_card->sd_addr,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
+					 sd_card->rsp, rsp_len, false);
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_write_cmd_failed;
+
+	if (data_len <= 512) {
+		u16 i;
+		u8 *buf;
+
+		buf = kmalloc(data_len, GFP_KERNEL);
+		if (!buf)
+			return TRANSPORT_ERROR;
+
+		rtsx_stor_get_xfer_buf(buf, data_len, srb);
+
+#ifdef SUPPORT_SD_LOCK
+		if (cmd_idx == LOCK_UNLOCK)
+			lock_cmd_type = buf[0] & 0x0F;
+#endif
+
+		if (data_len > 256) {
+			rtsx_init_cmd(chip);
+			for (i = 0; i < 256; i++) {
+				rtsx_add_cmd(chip, WRITE_REG_CMD,
+					     PPBUF_BASE2 + i, 0xFF, buf[i]);
+			}
+			retval = rtsx_send_cmd(chip, 0, 250);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				goto sd_execute_write_cmd_failed;
+			}
+
+			rtsx_init_cmd(chip);
+			for (i = 256; i < data_len; i++) {
+				rtsx_add_cmd(chip, WRITE_REG_CMD,
+					     PPBUF_BASE2 + i, 0xFF, buf[i]);
+			}
+			retval = rtsx_send_cmd(chip, 0, 250);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				goto sd_execute_write_cmd_failed;
+			}
+		} else {
+			rtsx_init_cmd(chip);
+			for (i = 0; i < data_len; i++) {
+				rtsx_add_cmd(chip, WRITE_REG_CMD,
+					     PPBUF_BASE2 + i, 0xFF, buf[i]);
+			}
+			retval = rtsx_send_cmd(chip, 0, 250);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				goto sd_execute_write_cmd_failed;
+			}
+		}
+
+		kfree(buf);
+
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF,
+			     srb->cmnd[8] & 0x03);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF,
+			     srb->cmnd[9]);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF,
+			     0x00);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF,
+			     0x01);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+			     PINGPONG_BUFFER);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+			     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		retval = rtsx_send_cmd(chip, SD_CARD, 250);
+	} else if (!(data_len & 0x1FF)) {
+		rtsx_init_cmd(chip);
+
+		trans_dma_enable(DMA_TO_DEVICE, chip, data_len, DMA_512);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF,
+			     0x02);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF,
+			     0x00);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H,
+			     0xFF, (srb->cmnd[7] & 0xFE) >> 1);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L,
+			     0xFF, (u8)((data_len & 0x0001FE00) >> 9));
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF,
+			     SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER,
+			     SD_TRANSFER_END, SD_TRANSFER_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data(chip, SD_CARD, scsi_sglist(srb),
+					    scsi_bufflen(srb),
+					    scsi_sg_count(srb),
+					    DMA_TO_DEVICE, 10000);
+
+	} else {
+		goto sd_execute_write_cmd_failed;
+	}
+
+	if (retval < 0) {
+		write_err = true;
+		rtsx_clear_sd_error(chip);
+		goto sd_execute_write_cmd_failed;
+	}
+
+#ifdef SUPPORT_SD_LOCK
+	if (cmd_idx == LOCK_UNLOCK) {
+		if (lock_cmd_type == SD_ERASE) {
+			sd_card->sd_erase_status = SD_UNDER_ERASING;
+			scsi_set_resid(srb, 0);
+			return TRANSPORT_GOOD;
+		}
+
+		rtsx_init_cmd(chip);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, 0xFD30, 0x02, 0x02);
+
+		retval = rtsx_send_cmd(chip, SD_CARD, 250);
+		if (retval < 0) {
+			write_err = true;
+			rtsx_clear_sd_error(chip);
+			goto sd_execute_write_cmd_failed;
+		}
+
+		retval = sd_update_lock_status(chip);
+		if (retval != STATUS_SUCCESS) {
+			dev_dbg(rtsx_dev(chip), "Lock command fail!\n");
+			lock_cmd_fail = 1;
+		}
+	}
+#endif /* SUPPORT_SD_LOCK */
+
+	if (standby) {
+		retval = sd_select_card(chip, 1);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	if (send_cmd12) {
+		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
+						 SD_RSP_TYPE_R1b, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	if (data_len < 512) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 false);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+
+		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+
+		retval = rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
+		if (retval != STATUS_SUCCESS)
+			goto sd_execute_write_cmd_failed;
+	}
+
+	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
+		cmd13_checkbit = true;
+
+	for (i = 0; i < 3; i++) {
+		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS,
+						 sd_card->sd_addr,
+						 SD_RSP_TYPE_R1, NULL, 0,
+						 cmd13_checkbit);
+		if (retval == STATUS_SUCCESS)
+			break;
+	}
+	if (retval != STATUS_SUCCESS)
+		goto sd_execute_write_cmd_failed;
+
+#ifdef SUPPORT_SD_LOCK
+	if (cmd_idx == LOCK_UNLOCK) {
+		if (!lock_cmd_fail) {
+			dev_dbg(rtsx_dev(chip), "lock_cmd_type = 0x%x\n",
+				lock_cmd_type);
+			if (lock_cmd_type & SD_CLR_PWD)
+				sd_card->sd_lock_status &= ~SD_PWD_EXIST;
+
+			if (lock_cmd_type & SD_SET_PWD)
+				sd_card->sd_lock_status |= SD_PWD_EXIST;
+		}
+
+		dev_dbg(rtsx_dev(chip), "sd_lock_state = 0x%x, sd_card->sd_lock_status = 0x%x\n",
+			sd_lock_state, sd_card->sd_lock_status);
+		if (sd_lock_state ^ (sd_card->sd_lock_status & SD_LOCKED)) {
+			sd_card->sd_lock_notify = 1;
+			if (sd_lock_state &&
+			    (sd_card->sd_lock_status & SD_LOCK_1BIT_MODE)) {
+				sd_card->sd_lock_status |= (
+					SD_UNLOCK_POW_ON | SD_SDR_RST);
+				if (CHK_SD(sd_card)) {
+					retval = reset_sd(chip);
+					if (retval != STATUS_SUCCESS) {
+						sd_card->sd_lock_status &= ~(SD_UNLOCK_POW_ON | SD_SDR_RST);
+						goto sd_execute_write_cmd_failed;
+					}
+				}
+
+				sd_card->sd_lock_status &= ~(SD_UNLOCK_POW_ON | SD_SDR_RST);
+			}
+		}
+	}
+
+	if (lock_cmd_fail) {
+		scsi_set_resid(srb, 0);
+		set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+		return TRANSPORT_FAILED;
+	}
+#endif  /* SUPPORT_SD_LOCK */
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+
+sd_execute_write_cmd_failed:
+	sd_card->pre_cmd_err = 1;
+	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
+	if (write_err)
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+
+	release_sd_card(chip);
+	do_reset_sd_card(chip);
+	if (!(chip->card_ready & SD_CARD))
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+
+	return TRANSPORT_FAILED;
+}
+
+int sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int count;
+	u16 data_len;
+
+	if (!sd_card->sd_pass_thru_en) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (sd_card->pre_cmd_err) {
+		sd_card->pre_cmd_err = 0;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	data_len = ((u16)srb->cmnd[7] << 8) | srb->cmnd[8];
+
+	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	} else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2) {
+		count = (data_len < 17) ? data_len : 17;
+	} else {
+		count = (data_len < 6) ? data_len : 6;
+	}
+	rtsx_stor_set_xfer_buf(sd_card->rsp, count, srb);
+
+	dev_dbg(rtsx_dev(chip), "Response length: %d\n", data_len);
+	dev_dbg(rtsx_dev(chip), "Response: 0x%x 0x%x 0x%x 0x%x\n",
+		sd_card->rsp[0], sd_card->rsp[1],
+		sd_card->rsp[2], sd_card->rsp[3]);
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+
+int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	unsigned int lun = SCSI_LUN(srb);
+	int retval;
+
+	if (!sd_card->sd_pass_thru_en) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	if (sd_card->pre_cmd_err) {
+		sd_card->pre_cmd_err = 0;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
+		return TRANSPORT_FAILED;
+	}
+
+	if (srb->cmnd[2] != 0x53 || srb->cmnd[3] != 0x44 ||
+	    srb->cmnd[4] != 0x20 || srb->cmnd[5] != 0x43 ||
+	    srb->cmnd[6] != 0x61 || srb->cmnd[7] != 0x72 ||
+	    srb->cmnd[8] != 0x64) {
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	switch (srb->cmnd[1] & 0x0F) {
+	case 0:
+#ifdef SUPPORT_SD_LOCK
+		if (srb->cmnd[9] == 0x64)
+			sd_card->sd_lock_status |= SD_SDR_RST;
+#endif
+		retval = reset_sd_card(chip);
+		if (retval != STATUS_SUCCESS) {
+#ifdef SUPPORT_SD_LOCK
+			sd_card->sd_lock_status &= ~SD_SDR_RST;
+#endif
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			sd_card->pre_cmd_err = 1;
+			return TRANSPORT_FAILED;
+		}
+#ifdef SUPPORT_SD_LOCK
+		sd_card->sd_lock_status &= ~SD_SDR_RST;
+#endif
+		break;
+
+	case 1:
+		retval = reset_sd(chip);
+		if (retval != STATUS_SUCCESS) {
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			sd_card->pre_cmd_err = 1;
+			return TRANSPORT_FAILED;
+		}
+		break;
+
+	default:
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
+		return TRANSPORT_FAILED;
+	}
+
+	scsi_set_resid(srb, 0);
+	return TRANSPORT_GOOD;
+}
+#endif
+
+void sd_cleanup_work(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+
+	if (sd_card->seq_mode) {
+		dev_dbg(rtsx_dev(chip), "SD: stop transmission\n");
+		sd_stop_seq_mode(chip);
+		sd_card->cleanup_counter = 0;
+	}
+}
+
+int sd_power_off_card3v3(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = disable_card_clock(chip, SD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+	if (retval)
+		return retval;
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_off(chip, SD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		mdelay(50);
+	}
+
+	if (chip->asic_code) {
+		retval = sd_pull_ctl_disable(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_SD_PULL_CTL_BIT | 0x20,
+					     FPGA_SD_PULL_CTL_BIT);
+		if (retval)
+			return retval;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int release_sd_card(struct rtsx_chip *chip)
+{
+	struct sd_info *sd_card = &chip->sd_card;
+	int retval;
+
+	chip->card_ready &= ~SD_CARD;
+	chip->card_fail &= ~SD_CARD;
+	chip->card_wp &= ~SD_CARD;
+
+	chip->sd_io = 0;
+	chip->sd_int = 0;
+
+#ifdef SUPPORT_SD_LOCK
+	sd_card->sd_lock_status = 0;
+	sd_card->sd_erase_status = 0;
+#endif
+
+	memset(sd_card->raw_csd, 0, 16);
+	memset(sd_card->raw_scr, 0, 8);
+
+	retval = sd_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5208/sd.h b/drivers/staging/rts5208/sd.h
new file mode 100644
index 000000000..f4ff62653
--- /dev/null
+++ b/drivers/staging/rts5208/sd.h
@@ -0,0 +1,289 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_SD_H
+#define __REALTEK_RTSX_SD_H
+
+#include "rtsx_chip.h"
+
+#define SUPPORT_VOLTAGE	0x003C0000
+
+/* Error Code */
+#define	SD_NO_ERROR		0x0
+#define	SD_CRC_ERR		0x80
+#define	SD_TO_ERR		0x40
+#define	SD_NO_CARD		0x20
+#define SD_BUSY			0x10
+#define	SD_STS_ERR		0x08
+#define SD_RSP_TIMEOUT		0x04
+#define SD_IO_ERR		0x02
+
+/* Return code for MMC switch bus */
+#define SWITCH_SUCCESS		0
+#define SWITCH_ERR		1
+#define SWITCH_FAIL		2
+
+/* MMC/SD Command Index */
+/* Basic command (class 0) */
+#define GO_IDLE_STATE		0
+#define	SEND_OP_COND		1
+#define	ALL_SEND_CID		2
+#define	SET_RELATIVE_ADDR	3
+#define	SEND_RELATIVE_ADDR	3
+#define	SET_DSR			4
+#define IO_SEND_OP_COND		5
+#define	SWITCH			6
+#define	SELECT_CARD		7
+#define	DESELECT_CARD		7
+/* CMD8 is "SEND_EXT_CSD" for MMC4.x Spec
+ * while is "SEND_IF_COND" for SD 2.0
+ */
+#define	SEND_EXT_CSD		8
+#define	SEND_IF_COND		8
+
+#define	SEND_CSD		9
+#define	SEND_CID		10
+#define	VOLTAGE_SWITCH		11
+#define	READ_DAT_UTIL_STOP	11
+#define	STOP_TRANSMISSION	12
+#define	SEND_STATUS		13
+#define	GO_INACTIVE_STATE	15
+
+#define	SET_BLOCKLEN		16
+#define	READ_SINGLE_BLOCK	17
+#define	READ_MULTIPLE_BLOCK	18
+#define	SEND_TUNING_PATTERN	19
+
+#define	BUSTEST_R		14
+#define	BUSTEST_W		19
+
+#define	WRITE_BLOCK		24
+#define	WRITE_MULTIPLE_BLOCK	25
+#define	PROGRAM_CSD		27
+
+#define	ERASE_WR_BLK_START	32
+#define	ERASE_WR_BLK_END	33
+#define	ERASE_CMD		38
+
+#define LOCK_UNLOCK		42
+#define	IO_RW_DIRECT		52
+
+#define	APP_CMD			55
+#define	GEN_CMD			56
+
+#define	SET_BUS_WIDTH		6
+#define	SD_STATUS		13
+#define	SEND_NUM_WR_BLOCKS	22
+#define	SET_WR_BLK_ERASE_COUNT	23
+#define	SD_APP_OP_COND		41
+#define	SET_CLR_CARD_DETECT	42
+#define	SEND_SCR		51
+
+#define	SD_READ_COMPLETE	0x00
+#define	SD_READ_TO		0x01
+#define	SD_READ_ADVENCE		0x02
+
+#define	SD_CHECK_MODE		0x00
+#define	SD_SWITCH_MODE		0x80
+#define	SD_FUNC_GROUP_1		0x01
+#define	SD_FUNC_GROUP_2		0x02
+#define	SD_FUNC_GROUP_3		0x03
+#define	SD_FUNC_GROUP_4		0x04
+#define	SD_CHECK_SPEC_V1_1	0xFF
+
+#define	NO_ARGUMENT				0x00
+#define	CHECK_PATTERN				0x000000AA
+#define	VOLTAGE_SUPPLY_RANGE			0x00000100
+#define	SUPPORT_HIGH_AND_EXTENDED_CAPACITY	0x40000000
+#define	SUPPORT_MAX_POWER_PERMANCE		0x10000000
+#define	SUPPORT_1V8				0x01000000
+
+#define	SWITCH_NO_ERR		0x00
+#define	CARD_NOT_EXIST		0x01
+#define	SPEC_NOT_SUPPORT	0x02
+#define	CHECK_MODE_ERR		0x03
+#define	CHECK_NOT_READY		0x04
+#define	SWITCH_CRC_ERR		0x05
+#define	SWITCH_MODE_ERR		0x06
+#define	SWITCH_PASS		0x07
+
+#ifdef SUPPORT_SD_LOCK
+#define SD_ERASE		0x08
+#define SD_LOCK			0x04
+#define SD_UNLOCK		0x00
+#define SD_CLR_PWD		0x02
+#define SD_SET_PWD		0x01
+
+#define SD_PWD_LEN		0x10
+
+#define SD_LOCKED		0x80
+#define SD_LOCK_1BIT_MODE	0x40
+#define SD_PWD_EXIST		0x20
+#define SD_UNLOCK_POW_ON	0x01
+#define SD_SDR_RST		0x02
+
+#define SD_NOT_ERASE		0x00
+#define SD_UNDER_ERASING	0x01
+#define SD_COMPLETE_ERASE	0x02
+
+#define SD_RW_FORBIDDEN		0x0F
+
+#endif
+
+#define	HS_SUPPORT			0x01
+#define	SDR50_SUPPORT			0x02
+#define	SDR104_SUPPORT			0x03
+#define	DDR50_SUPPORT			0x04
+
+#define	HS_SUPPORT_MASK			0x02
+#define	SDR50_SUPPORT_MASK		0x04
+#define	SDR104_SUPPORT_MASK		0x08
+#define	DDR50_SUPPORT_MASK		0x10
+
+#define	HS_QUERY_SWITCH_OK		0x01
+#define	SDR50_QUERY_SWITCH_OK		0x02
+#define	SDR104_QUERY_SWITCH_OK		0x03
+#define	DDR50_QUERY_SWITCH_OK		0x04
+
+#define	HS_SWITCH_BUSY			0x02
+#define	SDR50_SWITCH_BUSY		0x04
+#define	SDR104_SWITCH_BUSY		0x08
+#define	DDR50_SWITCH_BUSY		0x10
+
+#define	FUNCTION_GROUP1_SUPPORT_OFFSET       0x0D
+#define FUNCTION_GROUP1_QUERY_SWITCH_OFFSET  0x10
+#define FUNCTION_GROUP1_CHECK_BUSY_OFFSET    0x1D
+
+#define	DRIVING_TYPE_A		0x01
+#define	DRIVING_TYPE_B		    0x00
+#define	DRIVING_TYPE_C		    0x02
+#define	DRIVING_TYPE_D		0x03
+
+#define	DRIVING_TYPE_A_MASK	    0x02
+#define	DRIVING_TYPE_B_MASK	    0x01
+#define	DRIVING_TYPE_C_MASK	    0x04
+#define	DRIVING_TYPE_D_MASK	    0x08
+
+#define	TYPE_A_QUERY_SWITCH_OK	0x01
+#define	TYPE_B_QUERY_SWITCH_OK	0x00
+#define	TYPE_C_QUERY_SWITCH_OK  0x02
+#define	TYPE_D_QUERY_SWITCH_OK  0x03
+
+#define	TYPE_A_SWITCH_BUSY	    0x02
+#define	TYPE_B_SWITCH_BUSY	    0x01
+#define	TYPE_C_SWITCH_BUSY      0x04
+#define	TYPE_D_SWITCH_BUSY      0x08
+
+#define	FUNCTION_GROUP3_SUPPORT_OFFSET       0x09
+#define FUNCTION_GROUP3_QUERY_SWITCH_OFFSET  0x0F
+#define FUNCTION_GROUP3_CHECK_BUSY_OFFSET    0x19
+
+#define	CURRENT_LIMIT_200	    0x00
+#define	CURRENT_LIMIT_400	    0x01
+#define	CURRENT_LIMIT_600	    0x02
+#define	CURRENT_LIMIT_800	    0x03
+
+#define	CURRENT_LIMIT_200_MASK	0x01
+#define	CURRENT_LIMIT_400_MASK	0x02
+#define	CURRENT_LIMIT_600_MASK	0x04
+#define	CURRENT_LIMIT_800_MASK	0x08
+
+#define	CURRENT_LIMIT_200_QUERY_SWITCH_OK    0x00
+#define	CURRENT_LIMIT_400_QUERY_SWITCH_OK    0x01
+#define	CURRENT_LIMIT_600_QUERY_SWITCH_OK    0x02
+#define	CURRENT_LIMIT_800_QUERY_SWITCH_OK    0x03
+
+#define	CURRENT_LIMIT_200_SWITCH_BUSY        0x01
+#define	CURRENT_LIMIT_400_SWITCH_BUSY	     0x02
+#define	CURRENT_LIMIT_600_SWITCH_BUSY        0x04
+#define	CURRENT_LIMIT_800_SWITCH_BUSY        0x08
+
+#define	FUNCTION_GROUP4_SUPPORT_OFFSET       0x07
+#define FUNCTION_GROUP4_QUERY_SWITCH_OFFSET  0x0F
+#define FUNCTION_GROUP4_CHECK_BUSY_OFFSET    0x17
+
+#define	DATA_STRUCTURE_VER_OFFSET	0x11
+
+#define MAX_PHASE			31
+
+#define MMC_8BIT_BUS			0x0010
+#define MMC_4BIT_BUS			0x0020
+
+#define MMC_SWITCH_ERR			0x80
+
+#define SD_IO_3V3		0
+#define SD_IO_1V8		1
+
+#define TUNE_TX    0x00
+#define TUNE_RX	   0x01
+
+#define CHANGE_TX  0x00
+#define CHANGE_RX  0x01
+
+#define DCM_HIGH_FREQUENCY_MODE  0x00
+#define DCM_LOW_FREQUENCY_MODE   0x01
+
+#define DCM_HIGH_FREQUENCY_MODE_SET  0x0C
+#define DCM_LOW_FREQUENCY_MODE_SET   0x00
+
+#define MULTIPLY_BY_1    0x00
+#define MULTIPLY_BY_2    0x01
+#define MULTIPLY_BY_3    0x02
+#define MULTIPLY_BY_4    0x03
+#define MULTIPLY_BY_5    0x04
+#define MULTIPLY_BY_6    0x05
+#define MULTIPLY_BY_7    0x06
+#define MULTIPLY_BY_8    0x07
+#define MULTIPLY_BY_9    0x08
+#define MULTIPLY_BY_10   0x09
+
+#define DIVIDE_BY_2      0x01
+#define DIVIDE_BY_3      0x02
+#define DIVIDE_BY_4      0x03
+#define DIVIDE_BY_5      0x04
+#define DIVIDE_BY_6      0x05
+#define DIVIDE_BY_7      0x06
+#define DIVIDE_BY_8      0x07
+#define DIVIDE_BY_9      0x08
+#define DIVIDE_BY_10     0x09
+
+struct timing_phase_path {
+	int start;
+	int end;
+	int mid;
+	int len;
+};
+
+int sd_select_card(struct rtsx_chip *chip, int select);
+int sd_pull_ctl_enable(struct rtsx_chip *chip);
+int reset_sd_card(struct rtsx_chip *chip);
+int sd_switch_clock(struct rtsx_chip *chip);
+void sd_stop_seq_mode(struct rtsx_chip *chip);
+int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	  u32 start_sector, u16 sector_cnt);
+void sd_cleanup_work(struct rtsx_chip *chip);
+int sd_power_off_card3v3(struct rtsx_chip *chip);
+int release_sd_card(struct rtsx_chip *chip);
+#ifdef SUPPORT_CPRM
+int ext_sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
+			    u32 arg, u8 rsp_type, u8 *rsp, int rsp_len,
+			    bool special_check);
+int ext_sd_get_rsp(struct rtsx_chip *chip, int len, u8 *rsp, u8 rsp_type);
+
+int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+#endif
+
+#endif  /* __REALTEK_RTSX_SD_H */
diff --git a/drivers/staging/rts5208/spi.c b/drivers/staging/rts5208/spi.c
new file mode 100644
index 000000000..e88fe1a99
--- /dev/null
+++ b/drivers/staging/rts5208/spi.c
@@ -0,0 +1,906 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+
+#include "rtsx.h"
+#include "spi.h"
+
+static inline void spi_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct spi_info *spi = &chip->spi;
+
+	spi->err_code = err_code;
+}
+
+static int spi_init(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = rtsx_write_register(chip, SPI_CONTROL, 0xFF,
+				     CS_POLARITY_LOW | DTO_MSB_FIRST
+				     | SPI_MASTER | SPI_MODE0 | SPI_AUTO);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, SPI_TCTL, EDO_TIMING_MASK,
+				     SAMPLE_DELAY_HALF);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int spi_set_init_para(struct rtsx_chip *chip)
+{
+	struct spi_info *spi = &chip->spi;
+	int retval;
+
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER1, 0xFF,
+				     (u8)(spi->clk_div >> 8));
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER0, 0xFF,
+				     (u8)(spi->clk_div));
+	if (retval)
+		return retval;
+
+	retval = switch_clock(chip, spi->spi_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = select_card(chip, SPI_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_CLK_EN, SPI_CLK_EN,
+				     SPI_CLK_EN);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, CARD_OE, SPI_OUTPUT_EN,
+				     SPI_OUTPUT_EN);
+	if (retval)
+		return retval;
+
+	wait_timeout(10);
+
+	retval = spi_init(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int sf_polling_status(struct rtsx_chip *chip, int msec)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, SPI_RDSR);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_POLLING_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, msec);
+	if (retval < 0) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_BUSY_ERR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sf_enable_write(struct rtsx_chip *chip, u8 ins)
+{
+	struct spi_info *spi = &chip->spi;
+	int retval;
+
+	if (!spi->write_en)
+		return STATUS_SUCCESS;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_C_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int sf_disable_write(struct rtsx_chip *chip, u8 ins)
+{
+	struct spi_info *spi = &chip->spi;
+	int retval;
+
+	if (!spi->write_en)
+		return STATUS_SUCCESS;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_C_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void sf_program(struct rtsx_chip *chip, u8 ins, u8 addr_mode, u32 addr,
+		       u16 len)
+{
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH0, 0xFF, (u8)len);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH1, 0xFF, (u8)(len >> 8));
+	if (addr_mode) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, (u8)addr);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF,
+			     (u8)(addr >> 8));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF,
+			     (u8)(addr >> 16));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+			     SPI_TRANSFER0_START | SPI_CADO_MODE0);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+			     SPI_TRANSFER0_START | SPI_CDO_MODE0);
+	}
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+}
+
+static int sf_erase(struct rtsx_chip *chip, u8 ins, u8 addr_mode, u32 addr)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	if (addr_mode) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, (u8)addr);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF,
+			     (u8)(addr >> 8));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF,
+			     (u8)(addr >> 16));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+			     SPI_TRANSFER0_START | SPI_CA_MODE0);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+			     SPI_TRANSFER0_START | SPI_C_MODE0);
+	}
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int spi_init_eeprom(struct rtsx_chip *chip)
+{
+	int retval;
+	int clk;
+
+	if (chip->asic_code)
+		clk = 30;
+	else
+		clk = CLK_30;
+
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER1, 0xFF, 0x00);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER0, 0xFF, 0x27);
+	if (retval)
+		return retval;
+
+	retval = switch_clock(chip, clk);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = select_card(chip, SPI_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_CLK_EN, SPI_CLK_EN,
+				     SPI_CLK_EN);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, CARD_OE, SPI_OUTPUT_EN,
+				     SPI_OUTPUT_EN);
+	if (retval)
+		return retval;
+
+	wait_timeout(10);
+
+	retval = rtsx_write_register(chip, SPI_CONTROL, 0xFF,
+				     CS_POLARITY_HIGH | SPI_EEPROM_AUTO);
+	if (retval)
+		return retval;
+	retval = rtsx_write_register(chip, SPI_TCTL, EDO_TIMING_MASK,
+				     SAMPLE_DELAY_HALF);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+static int spi_eeprom_program_enable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF, 0x86);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, 0x13);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CA_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+int spi_erase_eeprom_chip(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = spi_init_eeprom(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = spi_eeprom_program_enable(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO_DIR, 0x01, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, 0x12);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF, 0x84);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CA_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int spi_erase_eeprom_byte(struct rtsx_chip *chip, u16 addr)
+{
+	int retval;
+
+	retval = spi_init_eeprom(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = spi_eeprom_program_enable(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO_DIR, 0x01, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, 0x07);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, (u8)addr);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF, (u8)(addr >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF, 0x46);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CA_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int spi_read_eeprom(struct rtsx_chip *chip, u16 addr, u8 *val)
+{
+	int retval;
+	u8 data;
+
+	retval = spi_init_eeprom(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO_DIR, 0x01, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, 0x06);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, (u8)addr);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF, (u8)(addr >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF, 0x46);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH0, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CADI_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	wait_timeout(5);
+	retval = rtsx_read_register(chip, SPI_DATA, &data);
+	if (retval)
+		return retval;
+
+	if (val)
+		*val = data;
+
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int spi_write_eeprom(struct rtsx_chip *chip, u16 addr, u8 val)
+{
+	int retval;
+
+	retval = spi_init_eeprom(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = spi_eeprom_program_enable(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO_DIR, 0x01, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, 0x05);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, val);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF, (u8)addr);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF, (u8)(addr >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF, 0x4E);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CA_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval)
+		return retval;
+
+	return STATUS_SUCCESS;
+}
+
+int spi_get_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct spi_info *spi = &chip->spi;
+
+	dev_dbg(rtsx_dev(chip), "%s: err_code = 0x%x\n", __func__,
+		spi->err_code);
+	rtsx_stor_set_xfer_buf(&spi->err_code,
+			       min_t(int, scsi_bufflen(srb), 1), srb);
+	scsi_set_resid(srb, scsi_bufflen(srb) - 1);
+
+	return STATUS_SUCCESS;
+}
+
+int spi_set_parameter(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	struct spi_info *spi = &chip->spi;
+
+	spi_set_err_code(chip, SPI_NO_ERR);
+
+	if (chip->asic_code)
+		spi->spi_clock = ((u16)(srb->cmnd[8]) << 8) | srb->cmnd[9];
+	else
+		spi->spi_clock = srb->cmnd[3];
+
+	spi->clk_div = ((u16)(srb->cmnd[4]) << 8) | srb->cmnd[5];
+	spi->write_en = srb->cmnd[6];
+
+	dev_dbg(rtsx_dev(chip), "spi_clock = %d, clk_div = %d, write_en = %d\n",
+		spi->spi_clock, spi->clk_div, spi->write_en);
+
+	return STATUS_SUCCESS;
+}
+
+int spi_read_flash_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u16 len;
+	u8 *buf;
+
+	spi_set_err_code(chip, SPI_NO_ERR);
+
+	len = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
+	if (len > 512) {
+		spi_set_err_code(chip, SPI_INVALID_COMMAND);
+		return STATUS_FAIL;
+	}
+
+	retval = spi_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, srb->cmnd[3]);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF, srb->cmnd[4]);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF, srb->cmnd[5]);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF, srb->cmnd[6]);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH1, 0xFF, srb->cmnd[7]);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH0, 0xFF, srb->cmnd[8]);
+
+	if (len == 0) {
+		if (srb->cmnd[9]) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0,
+				     0xFF, SPI_TRANSFER0_START | SPI_CA_MODE0);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0,
+				     0xFF, SPI_TRANSFER0_START | SPI_C_MODE0);
+		}
+	} else {
+		if (srb->cmnd[9]) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+				     SPI_TRANSFER0_START | SPI_CADI_MODE0);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+				     SPI_TRANSFER0_START | SPI_CDI_MODE0);
+		}
+	}
+
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval < 0) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	if (len) {
+		buf = kmalloc(len, GFP_KERNEL);
+		if (!buf)
+			return STATUS_ERROR;
+
+		retval = rtsx_read_ppbuf(chip, buf, len);
+		if (retval != STATUS_SUCCESS) {
+			spi_set_err_code(chip, SPI_READ_ERR);
+			kfree(buf);
+			return STATUS_FAIL;
+		}
+
+		rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
+		scsi_set_resid(srb, 0);
+
+		kfree(buf);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int spi_read_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	unsigned int index = 0, offset = 0;
+	u8 ins, slow_read;
+	u32 addr;
+	u16 len;
+	u8 *buf;
+
+	spi_set_err_code(chip, SPI_NO_ERR);
+
+	ins = srb->cmnd[3];
+	addr = ((u32)(srb->cmnd[4]) << 16) | ((u32)(srb->cmnd[5])
+					<< 8) | srb->cmnd[6];
+	len = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
+	slow_read = srb->cmnd[9];
+
+	retval = spi_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	buf = kmalloc(SF_PAGE_LEN, GFP_KERNEL);
+	if (!buf)
+		return STATUS_ERROR;
+
+	while (len) {
+		u16 pagelen = SF_PAGE_LEN - (u8)addr;
+
+		if (pagelen > len)
+			pagelen = len;
+
+		rtsx_init_cmd(chip);
+
+		trans_dma_enable(DMA_FROM_DEVICE, chip, 256, DMA_256);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+
+		if (slow_read) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR0, 0xFF,
+				     (u8)addr);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF,
+				     (u8)(addr >> 8));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF,
+				     (u8)(addr >> 16));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+				     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR1, 0xFF,
+				     (u8)addr);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR2, 0xFF,
+				     (u8)(addr >> 8));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_ADDR3, 0xFF,
+				     (u8)(addr >> 16));
+			rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+				     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_32);
+		}
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH1, 0xFF,
+			     (u8)(pagelen >> 8));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH0, 0xFF,
+			     (u8)pagelen);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+			     SPI_TRANSFER0_START | SPI_CADI_MODE0);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0,
+			     SPI_TRANSFER0_END, SPI_TRANSFER0_END);
+
+		rtsx_send_cmd_no_wait(chip);
+
+		retval = rtsx_transfer_data(chip, 0, buf, pagelen, 0,
+					    DMA_FROM_DEVICE, 10000);
+		if (retval < 0) {
+			kfree(buf);
+			rtsx_clear_spi_error(chip);
+			spi_set_err_code(chip, SPI_HW_ERR);
+			return STATUS_FAIL;
+		}
+
+		rtsx_stor_access_xfer_buf(buf, pagelen, srb, &index, &offset,
+					  TO_XFER_BUF);
+
+		addr += pagelen;
+		len -= pagelen;
+	}
+
+	scsi_set_resid(srb, 0);
+	kfree(buf);
+
+	return STATUS_SUCCESS;
+}
+
+int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 ins, program_mode;
+	u32 addr;
+	u16 len;
+	u8 *buf;
+	unsigned int index = 0, offset = 0;
+
+	spi_set_err_code(chip, SPI_NO_ERR);
+
+	ins = srb->cmnd[3];
+	addr = ((u32)(srb->cmnd[4]) << 16) | ((u32)(srb->cmnd[5])
+					<< 8) | srb->cmnd[6];
+	len = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
+	program_mode = srb->cmnd[9];
+
+	retval = spi_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	if (program_mode == BYTE_PROGRAM) {
+		buf = kmalloc(4, GFP_KERNEL);
+		if (!buf)
+			return STATUS_ERROR;
+
+		while (len) {
+			retval = sf_enable_write(chip, SPI_WREN);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			rtsx_stor_access_xfer_buf(buf, 1, srb, &index, &offset,
+						  FROM_XFER_BUF);
+
+			rtsx_init_cmd(chip);
+
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+				     0x01, PINGPONG_BUFFER);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2, 0xFF,
+				     buf[0]);
+			sf_program(chip, ins, 1, addr, 1);
+
+			retval = rtsx_send_cmd(chip, 0, 100);
+			if (retval < 0) {
+				kfree(buf);
+				rtsx_clear_spi_error(chip);
+				spi_set_err_code(chip, SPI_HW_ERR);
+				return STATUS_FAIL;
+			}
+
+			retval = sf_polling_status(chip, 100);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			addr++;
+			len--;
+		}
+
+		kfree(buf);
+
+	} else if (program_mode == AAI_PROGRAM) {
+		int first_byte = 1;
+
+		retval = sf_enable_write(chip, SPI_WREN);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		buf = kmalloc(4, GFP_KERNEL);
+		if (!buf)
+			return STATUS_ERROR;
+
+		while (len) {
+			rtsx_stor_access_xfer_buf(buf, 1, srb, &index, &offset,
+						  FROM_XFER_BUF);
+
+			rtsx_init_cmd(chip);
+
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+				     0x01, PINGPONG_BUFFER);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2, 0xFF,
+				     buf[0]);
+			if (first_byte) {
+				sf_program(chip, ins, 1, addr, 1);
+				first_byte = 0;
+			} else {
+				sf_program(chip, ins, 0, 0, 1);
+			}
+
+			retval = rtsx_send_cmd(chip, 0, 100);
+			if (retval < 0) {
+				kfree(buf);
+				rtsx_clear_spi_error(chip);
+				spi_set_err_code(chip, SPI_HW_ERR);
+				return STATUS_FAIL;
+			}
+
+			retval = sf_polling_status(chip, 100);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			len--;
+		}
+
+		kfree(buf);
+
+		retval = sf_disable_write(chip, SPI_WRDI);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = sf_polling_status(chip, 100);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else if (program_mode == PAGE_PROGRAM) {
+		buf = kmalloc(SF_PAGE_LEN, GFP_KERNEL);
+		if (!buf)
+			return STATUS_NOMEM;
+
+		while (len) {
+			u16 pagelen = SF_PAGE_LEN - (u8)addr;
+
+			if (pagelen > len)
+				pagelen = len;
+
+			retval = sf_enable_write(chip, SPI_WREN);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			rtsx_init_cmd(chip);
+
+			trans_dma_enable(DMA_TO_DEVICE, chip, 256, DMA_256);
+			sf_program(chip, ins, 1, addr, pagelen);
+
+			rtsx_send_cmd_no_wait(chip);
+
+			rtsx_stor_access_xfer_buf(buf, pagelen, srb, &index,
+						  &offset, FROM_XFER_BUF);
+
+			retval = rtsx_transfer_data(chip, 0, buf, pagelen, 0,
+						    DMA_TO_DEVICE, 100);
+			if (retval < 0) {
+				kfree(buf);
+				rtsx_clear_spi_error(chip);
+				spi_set_err_code(chip, SPI_HW_ERR);
+				return STATUS_FAIL;
+			}
+
+			retval = sf_polling_status(chip, 100);
+			if (retval != STATUS_SUCCESS) {
+				kfree(buf);
+				return STATUS_FAIL;
+			}
+
+			addr += pagelen;
+			len -= pagelen;
+		}
+
+		kfree(buf);
+	} else {
+		spi_set_err_code(chip, SPI_INVALID_COMMAND);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int spi_erase_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 ins, erase_mode;
+	u32 addr;
+
+	spi_set_err_code(chip, SPI_NO_ERR);
+
+	ins = srb->cmnd[3];
+	addr = ((u32)(srb->cmnd[4]) << 16) | ((u32)(srb->cmnd[5])
+					<< 8) | srb->cmnd[6];
+	erase_mode = srb->cmnd[9];
+
+	retval = spi_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	if (erase_mode == PAGE_ERASE) {
+		retval = sf_enable_write(chip, SPI_WREN);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = sf_erase(chip, ins, 1, addr);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else if (erase_mode == CHIP_ERASE) {
+		retval = sf_enable_write(chip, SPI_WREN);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		retval = sf_erase(chip, ins, 0, 0);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		spi_set_err_code(chip, SPI_INVALID_COMMAND);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int spi_write_flash_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
+{
+	int retval;
+	u8 ins, status, ewsr;
+
+	ins = srb->cmnd[3];
+	status = srb->cmnd[4];
+	ewsr = srb->cmnd[5];
+
+	retval = spi_set_init_para(chip);
+	if (retval != STATUS_SUCCESS) {
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	retval = sf_enable_write(chip, ewsr);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
+		     PINGPONG_BUFFER);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_COMMAND, 0xFF, ins);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_CA_NUMBER, 0xFF,
+		     SPI_COMMAND_BIT_8 | SPI_ADDRESS_BIT_24);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH1, 0xFF, 0);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_LENGTH0, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2, 0xFF, status);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, SPI_TRANSFER0, 0xFF,
+		     SPI_TRANSFER0_START | SPI_CDO_MODE0);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, SPI_TRANSFER0, SPI_TRANSFER0_END,
+		     SPI_TRANSFER0_END);
+
+	retval = rtsx_send_cmd(chip, 0, 100);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_clear_spi_error(chip);
+		spi_set_err_code(chip, SPI_HW_ERR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5208/spi.h b/drivers/staging/rts5208/spi.h
new file mode 100644
index 000000000..dcf93c80b
--- /dev/null
+++ b/drivers/staging/rts5208/spi.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_SPI_H
+#define __REALTEK_RTSX_SPI_H
+
+/* SPI operation error */
+#define SPI_NO_ERR		0x00
+#define SPI_HW_ERR		0x01
+#define SPI_INVALID_COMMAND	0x02
+#define SPI_READ_ERR		0x03
+#define SPI_WRITE_ERR		0x04
+#define SPI_ERASE_ERR		0x05
+#define SPI_BUSY_ERR		0x06
+
+/* Serial flash instruction */
+#define SPI_READ		0x03
+#define SPI_FAST_READ		0x0B
+#define SPI_WREN		0x06
+#define SPI_WRDI		0x04
+#define SPI_RDSR		0x05
+
+#define SF_PAGE_LEN		256
+
+#define BYTE_PROGRAM		0
+#define AAI_PROGRAM		1
+#define PAGE_PROGRAM		2
+
+#define PAGE_ERASE		0
+#define CHIP_ERASE		1
+
+int spi_erase_eeprom_chip(struct rtsx_chip *chip);
+int spi_erase_eeprom_byte(struct rtsx_chip *chip, u16 addr);
+int spi_read_eeprom(struct rtsx_chip *chip, u16 addr, u8 *val);
+int spi_write_eeprom(struct rtsx_chip *chip, u16 addr, u8 val);
+int spi_get_status(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_set_parameter(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_read_flash_id(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_read_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_erase_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+int spi_write_flash_status(struct scsi_cmnd *srb, struct rtsx_chip *chip);
+
+#endif  /* __REALTEK_RTSX_SPI_H */
diff --git a/drivers/staging/rts5208/xd.c b/drivers/staging/rts5208/xd.c
new file mode 100644
index 000000000..42cab9398
--- /dev/null
+++ b/drivers/staging/rts5208/xd.c
@@ -0,0 +1,2152 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#include <linux/blkdev.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include "rtsx.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "xd.h"
+
+static int xd_build_l2p_tbl(struct rtsx_chip *chip, int zone_no);
+static int xd_init_page(struct rtsx_chip *chip, u32 phy_blk, u16 logoff,
+			u8 start_page, u8 end_page);
+
+static inline void xd_set_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+
+	xd_card->err_code = err_code;
+}
+
+static inline int xd_check_err_code(struct rtsx_chip *chip, u8 err_code)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+
+	return (xd_card->err_code == err_code);
+}
+
+static int xd_set_init_para(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+
+	if (chip->asic_code)
+		xd_card->xd_clock = 47;
+	else
+		xd_card->xd_clock = CLK_50;
+
+	retval = switch_clock(chip, xd_card->xd_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_switch_clock(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+
+	retval = select_card(chip, XD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = switch_clock(chip, xd_card->xd_clock);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_read_id(struct rtsx_chip *chip, u8 id_cmd, u8 *id_buf, u8 buf_len)
+{
+	int retval, i;
+	u8 *ptr;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, id_cmd);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+		     XD_TRANSFER_START | XD_READ_ID);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
+		     XD_TRANSFER_END);
+
+	for (i = 0; i < 4; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, (u16)(XD_ADDRESS1 + i), 0, 0);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 20);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+	if (id_buf && buf_len) {
+		if (buf_len > 4)
+			buf_len = 4;
+		memcpy(id_buf, ptr, buf_len);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void xd_assign_phy_addr(struct rtsx_chip *chip, u32 addr, u8 mode)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+
+	switch (mode) {
+	case XD_RW_ADDR:
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF, 0);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1, 0xFF, (u8)addr);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2,
+			     0xFF, (u8)(addr >> 8));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS3,
+			     0xFF, (u8)(addr >> 16));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
+			     xd_card->addr_cycle |
+			     XD_CALC_ECC |
+			     XD_BA_NO_TRANSFORM);
+		break;
+
+	case XD_ERASE_ADDR:
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF, (u8)addr);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1,
+			     0xFF, (u8)(addr >> 8));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2,
+			     0xFF, (u8)(addr >> 16));
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
+			     (xd_card->addr_cycle - 1) | XD_CALC_ECC |
+			XD_BA_NO_TRANSFORM);
+		break;
+
+	default:
+		break;
+	}
+}
+
+static int xd_read_redundant(struct rtsx_chip *chip, u32 page_addr,
+			     u8 *buf, int buf_len)
+{
+	int retval, i;
+
+	rtsx_init_cmd(chip);
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER,
+		     0xFF, XD_TRANSFER_START | XD_READ_REDUNDANT);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+
+	for (i = 0; i < 6; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, (u16)(XD_PAGE_STATUS + i),
+			     0, 0);
+	for (i = 0; i < 4; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, (u16)(XD_RESERVED0 + i),
+			     0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, XD_PARITY, 0, 0);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 500);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	if (buf && buf_len) {
+		u8 *ptr = rtsx_get_cmd_data(chip) + 1;
+
+		if (buf_len > 11)
+			buf_len = 11;
+		memcpy(buf, ptr, buf_len);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_read_data_from_ppb(struct rtsx_chip *chip, int offset,
+				 u8 *buf, int buf_len)
+{
+	int retval, i;
+
+	if (!buf || buf_len < 0)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	for (i = 0; i < buf_len; i++)
+		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + offset + i,
+			     0, 0);
+
+	retval = rtsx_send_cmd(chip, 0, 250);
+	if (retval < 0) {
+		rtsx_clear_xd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	memcpy(buf, rtsx_get_cmd_data(chip), buf_len);
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_read_cis(struct rtsx_chip *chip, u32 page_addr, u8 *buf,
+		       int buf_len)
+{
+	int retval;
+	u8 reg;
+
+	if (!buf || buf_len < 10)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
+		     0x01, PINGPONG_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
+		     XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+		     XD_TRANSFER_START | XD_READ_PAGES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
+		     XD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 250);
+	if (retval == -ETIMEDOUT) {
+		rtsx_clear_xd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_read_register(chip, XD_PAGE_STATUS, &reg);
+	if (retval)
+		return retval;
+	if (reg != XD_GPG) {
+		rtsx_clear_xd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	retval = rtsx_read_register(chip, XD_CTL, &reg);
+	if (retval)
+		return retval;
+	if (!(reg & XD_ECC1_ERROR) || !(reg & XD_ECC1_UNCORRECTABLE)) {
+		retval = xd_read_data_from_ppb(chip, 0, buf, buf_len);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+		if (reg & XD_ECC1_ERROR) {
+			u8 ecc_bit, ecc_byte;
+
+			retval = rtsx_read_register(chip, XD_ECC_BIT1,
+						    &ecc_bit);
+			if (retval)
+				return retval;
+			retval = rtsx_read_register(chip, XD_ECC_BYTE1,
+						    &ecc_byte);
+			if (retval)
+				return retval;
+
+			dev_dbg(rtsx_dev(chip), "ECC_BIT1 = 0x%x, ECC_BYTE1 = 0x%x\n",
+				ecc_bit, ecc_byte);
+			if (ecc_byte < buf_len) {
+				dev_dbg(rtsx_dev(chip), "Before correct: 0x%x\n",
+					buf[ecc_byte]);
+				buf[ecc_byte] ^= (1 << ecc_bit);
+				dev_dbg(rtsx_dev(chip), "After correct: 0x%x\n",
+					buf[ecc_byte]);
+			}
+		}
+	} else if (!(reg & XD_ECC2_ERROR) || !(reg & XD_ECC2_UNCORRECTABLE)) {
+		rtsx_clear_xd_error(chip);
+
+		retval = xd_read_data_from_ppb(chip, 256, buf, buf_len);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+		if (reg & XD_ECC2_ERROR) {
+			u8 ecc_bit, ecc_byte;
+
+			retval = rtsx_read_register(chip, XD_ECC_BIT2,
+						    &ecc_bit);
+			if (retval)
+				return retval;
+			retval = rtsx_read_register(chip, XD_ECC_BYTE2,
+						    &ecc_byte);
+			if (retval)
+				return retval;
+
+			dev_dbg(rtsx_dev(chip), "ECC_BIT2 = 0x%x, ECC_BYTE2 = 0x%x\n",
+				ecc_bit, ecc_byte);
+			if (ecc_byte < buf_len) {
+				dev_dbg(rtsx_dev(chip), "Before correct: 0x%x\n",
+					buf[ecc_byte]);
+				buf[ecc_byte] ^= (1 << ecc_bit);
+				dev_dbg(rtsx_dev(chip), "After correct: 0x%x\n",
+					buf[ecc_byte]);
+			}
+		}
+	} else {
+		rtsx_clear_xd_error(chip);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void xd_fill_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	if (CHECK_PID(chip, 0x5208)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
+			     XD_D3_PD | XD_D2_PD | XD_D1_PD | XD_D0_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
+			     XD_D7_PD | XD_D6_PD | XD_D5_PD | XD_D4_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
+			     XD_WP_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
+			     XD_RDY_PD | XD_WE_PD | XD_RE_PD | XD_ALE_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
+			     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
+			     MS_D5_PD | MS_D4_PD);
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1,
+				     0xFF, 0x55);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2,
+				     0xFF, 0x55);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3,
+				     0xFF, 0x4B);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4,
+				     0xFF, 0x69);
+		}
+	}
+}
+
+static void xd_fill_pull_ctl_stage1_barossa(struct rtsx_chip *chip)
+{
+	if (CHECK_BARO_PKG(chip, QFN)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x4B);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	}
+}
+
+static void xd_fill_pull_ctl_enable(struct rtsx_chip *chip)
+{
+	if (CHECK_PID(chip, 0x5208)) {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
+			     XD_D3_PD | XD_D2_PD | XD_D1_PD | XD_D0_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
+			     XD_D7_PD | XD_D6_PD | XD_D5_PD | XD_D4_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
+			     XD_WP_PD | XD_CE_PU | XD_CLE_PD | XD_CD_PU);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
+			     XD_RDY_PU | XD_WE_PU | XD_RE_PU | XD_ALE_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
+			     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
+			     MS_D5_PD | MS_D4_PD);
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1,
+				     0xFF, 0x55);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2,
+				     0xFF, 0x55);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3,
+				     0xFF, 0x53);
+			rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4,
+				     0xFF, 0xA9);
+		}
+	}
+}
+
+static int xd_pull_ctl_disable(struct rtsx_chip *chip)
+{
+	int retval;
+
+	if (CHECK_PID(chip, 0x5208)) {
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     XD_D3_PD |
+					     XD_D2_PD |
+					     XD_D1_PD |
+					     XD_D0_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     XD_D7_PD |
+					     XD_D6_PD |
+					     XD_D5_PD |
+					     XD_D4_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     XD_WP_PD |
+					     XD_CE_PD |
+					     XD_CLE_PD |
+					     XD_CD_PU);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD |
+					     XD_WE_PD |
+					     XD_RE_PD |
+					     XD_ALE_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU |
+					     SD_WP_PD |
+					     SD_CD_PU |
+					     SD_CMD_PD);
+		if (retval)
+			return retval;
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval)
+			return retval;
+	} else if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_BARO_PKG(chip, QFN)) {
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval)
+				return retval;
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval)
+				return retval;
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int reset_xd(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval, i, j;
+	u8 *ptr, id_buf[4], redunt[11];
+
+	retval = select_card(chip, XD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS, 0xFF,
+		     XD_PGSTS_NOT_FF);
+	if (chip->asic_code) {
+		if (!CHECK_PID(chip, 0x5288))
+			xd_fill_pull_ctl_disable(chip);
+		else
+			xd_fill_pull_ctl_stage1_barossa(chip);
+	} else {
+		rtsx_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
+			     (FPGA_XD_PULL_CTL_EN1 & FPGA_XD_PULL_CTL_EN3) |
+			     0x20);
+	}
+
+	if (!chip->ft2_fast_mode)
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_INIT,
+			     XD_NO_AUTO_PWR_OFF, 0);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_off(chip, XD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		wait_timeout(250);
+
+		rtsx_init_cmd(chip);
+
+		if (chip->asic_code) {
+			xd_fill_pull_ctl_enable(chip);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
+				     (FPGA_XD_PULL_CTL_EN1 &
+				      FPGA_XD_PULL_CTL_EN2) |
+				     0x20);
+		}
+
+		retval = rtsx_send_cmd(chip, XD_CARD, 100);
+		if (retval < 0)
+			return STATUS_FAIL;
+
+		retval = card_power_on(chip, XD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+#ifdef SUPPORT_OCP
+		wait_timeout(50);
+		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
+			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+				chip->ocp_stat);
+			return STATUS_FAIL;
+		}
+#endif
+	}
+
+	rtsx_init_cmd(chip);
+
+	if (chip->ft2_fast_mode) {
+		if (chip->asic_code) {
+			xd_fill_pull_ctl_enable(chip);
+		} else {
+			rtsx_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
+				     (FPGA_XD_PULL_CTL_EN1 &
+				      FPGA_XD_PULL_CTL_EN2) |
+				     0x20);
+		}
+	}
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, XD_OUTPUT_EN);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CTL, XD_CE_DISEN, XD_CE_DISEN);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	if (!chip->ft2_fast_mode)
+		wait_timeout(200);
+
+	retval = xd_set_init_para(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	/* Read ID to check if the timing setting is right */
+	for (i = 0; i < 4; i++) {
+		rtsx_init_cmd(chip);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_DTCTL, 0xFF,
+			     XD_TIME_SETUP_STEP * 3 +
+			     XD_TIME_RW_STEP * (2 + i) + XD_TIME_RWN_STEP * i);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CATCTL, 0xFF,
+			     XD_TIME_SETUP_STEP * 3 +
+			     XD_TIME_RW_STEP * (4 + i) +
+			     XD_TIME_RWN_STEP * (3 + i));
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+			     XD_TRANSFER_START | XD_RESET);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+			     XD_TRANSFER_END, XD_TRANSFER_END);
+
+		rtsx_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
+		rtsx_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
+
+		retval = rtsx_send_cmd(chip, XD_CARD, 100);
+		if (retval < 0)
+			return STATUS_FAIL;
+
+		ptr = rtsx_get_cmd_data(chip) + 1;
+
+		dev_dbg(rtsx_dev(chip), "XD_DAT: 0x%x, XD_CTL: 0x%x\n",
+			ptr[0], ptr[1]);
+
+		if (((ptr[0] & READY_FLAG) != READY_STATE) ||
+		    !(ptr[1] & XD_RDY))
+			continue;
+
+		retval = xd_read_id(chip, READ_ID, id_buf, 4);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		dev_dbg(rtsx_dev(chip), "READ_ID: 0x%x 0x%x 0x%x 0x%x\n",
+			id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
+
+		xd_card->device_code = id_buf[1];
+
+		/* Check if the xD card is supported */
+		switch (xd_card->device_code) {
+		case XD_4M_X8_512_1:
+		case XD_4M_X8_512_2:
+			xd_card->block_shift = 4;
+			xd_card->page_off = 0x0F;
+			xd_card->addr_cycle = 3;
+			xd_card->zone_cnt = 1;
+			xd_card->capacity = 8000;
+			XD_SET_4MB(xd_card);
+			break;
+		case XD_8M_X8_512:
+			xd_card->block_shift = 4;
+			xd_card->page_off = 0x0F;
+			xd_card->addr_cycle = 3;
+			xd_card->zone_cnt = 1;
+			xd_card->capacity = 16000;
+			break;
+		case XD_16M_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 3;
+			xd_card->zone_cnt = 1;
+			xd_card->capacity = 32000;
+			break;
+		case XD_32M_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 3;
+			xd_card->zone_cnt = 2;
+			xd_card->capacity = 64000;
+			break;
+		case XD_64M_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 4;
+			xd_card->capacity = 128000;
+			break;
+		case XD_128M_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 8;
+			xd_card->capacity = 256000;
+			break;
+		case XD_256M_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 16;
+			xd_card->capacity = 512000;
+			break;
+		case XD_512M_X8:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 32;
+			xd_card->capacity = 1024000;
+			break;
+		case XD_1G_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 64;
+			xd_card->capacity = 2048000;
+			break;
+		case XD_2G_X8_512:
+			XD_PAGE_512(xd_card);
+			xd_card->addr_cycle = 4;
+			xd_card->zone_cnt = 128;
+			xd_card->capacity = 4096000;
+			break;
+		default:
+			continue;
+		}
+
+		/* Confirm timing setting */
+		for (j = 0; j < 10; j++) {
+			retval = xd_read_id(chip, READ_ID, id_buf, 4);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			if (id_buf[1] != xd_card->device_code)
+				break;
+		}
+
+		if (j == 10)
+			break;
+	}
+
+	if (i == 4) {
+		xd_card->block_shift = 0;
+		xd_card->page_off = 0;
+		xd_card->addr_cycle = 0;
+		xd_card->capacity = 0;
+
+		return STATUS_FAIL;
+	}
+
+	retval = xd_read_id(chip, READ_XD_ID, id_buf, 4);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+	dev_dbg(rtsx_dev(chip), "READ_XD_ID: 0x%x 0x%x 0x%x 0x%x\n",
+		id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
+	if (id_buf[2] != XD_ID_CODE)
+		return STATUS_FAIL;
+
+	/* Search CIS block */
+	for (i = 0; i < 24; i++) {
+		u32 page_addr;
+
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		page_addr = (u32)i << xd_card->block_shift;
+
+		for (j = 0; j < 3; j++) {
+			retval = xd_read_redundant(chip, page_addr, redunt, 11);
+			if (retval == STATUS_SUCCESS)
+				break;
+		}
+		if (j == 3)
+			continue;
+
+		if (redunt[BLOCK_STATUS] != XD_GBLK)
+			continue;
+
+		j = 0;
+		if (redunt[PAGE_STATUS] != XD_GPG) {
+			for (j = 1; j <= 8; j++) {
+				retval = xd_read_redundant(chip, page_addr + j,
+							   redunt, 11);
+				if (retval == STATUS_SUCCESS) {
+					if (redunt[PAGE_STATUS] == XD_GPG)
+						break;
+				}
+			}
+
+			if (j == 9)
+				break;
+		}
+
+		/* Check CIS data */
+		if (redunt[BLOCK_STATUS] == XD_GBLK &&
+		    (redunt[PARITY] & XD_BA1_ALL0)) {
+			u8 buf[10];
+
+			page_addr += j;
+
+			retval = xd_read_cis(chip, page_addr, buf, 10);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+
+			if (buf[0] == 0x01 && buf[1] == 0x03 &&
+			    buf[2] == 0xD9 &&
+			    buf[3] == 0x01 && buf[4] == 0xFF &&
+			    buf[5] == 0x18 && buf[6] == 0x02 &&
+			    buf[7] == 0xDF && buf[8] == 0x01 &&
+			    buf[9] == 0x20) {
+				xd_card->cis_block = (u16)i;
+			}
+		}
+
+		break;
+	}
+
+	dev_dbg(rtsx_dev(chip), "CIS block: 0x%x\n", xd_card->cis_block);
+	if (xd_card->cis_block == 0xFFFF)
+		return STATUS_FAIL;
+
+	chip->capacity[chip->card2lun[XD_CARD]] = xd_card->capacity;
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_check_data_blank(u8 *redunt)
+{
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		if (redunt[PAGE_STATUS + i] != 0xFF)
+			return 0;
+	}
+
+	if ((redunt[PARITY] & (XD_ECC1_ALL1 | XD_ECC2_ALL1))
+		!= (XD_ECC1_ALL1 | XD_ECC2_ALL1))
+		return 0;
+
+	for (i = 0; i < 4; i++) {
+		if (redunt[RESERVED0 + i] != 0xFF)
+			return 0;
+	}
+
+	return 1;
+}
+
+static u16 xd_load_log_block_addr(u8 *redunt)
+{
+	u16 addr = 0xFFFF;
+
+	if (redunt[PARITY] & XD_BA1_BA2_EQL)
+		addr = ((u16)redunt[BLOCK_ADDR1_H] << 8) |
+			redunt[BLOCK_ADDR1_L];
+	else if (redunt[PARITY] & XD_BA1_VALID)
+		addr = ((u16)redunt[BLOCK_ADDR1_H] << 8) |
+			redunt[BLOCK_ADDR1_L];
+	else if (redunt[PARITY] & XD_BA2_VALID)
+		addr = ((u16)redunt[BLOCK_ADDR2_H] << 8) |
+			redunt[BLOCK_ADDR2_L];
+
+	return addr;
+}
+
+static int xd_init_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int size, i;
+
+	dev_dbg(rtsx_dev(chip), "%s: zone_cnt = %d\n", __func__,
+		xd_card->zone_cnt);
+
+	if (xd_card->zone_cnt < 1)
+		return STATUS_FAIL;
+
+	size = xd_card->zone_cnt * sizeof(struct zone_entry);
+	dev_dbg(rtsx_dev(chip), "Buffer size for l2p table is %d\n", size);
+
+	xd_card->zone = vmalloc(size);
+	if (!xd_card->zone)
+		return STATUS_ERROR;
+
+	for (i = 0; i < xd_card->zone_cnt; i++) {
+		xd_card->zone[i].build_flag = 0;
+		xd_card->zone[i].l2p_table = NULL;
+		xd_card->zone[i].free_table = NULL;
+		xd_card->zone[i].get_index = 0;
+		xd_card->zone[i].set_index = 0;
+		xd_card->zone[i].unused_blk_cnt = 0;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static inline void free_zone(struct zone_entry *zone)
+{
+	if (!zone)
+		return;
+
+	zone->build_flag = 0;
+	zone->set_index = 0;
+	zone->get_index = 0;
+	zone->unused_blk_cnt = 0;
+	vfree(zone->l2p_table);
+	zone->l2p_table = NULL;
+	vfree(zone->free_table);
+	zone->free_table = NULL;
+}
+
+static void xd_set_unused_block(struct rtsx_chip *chip, u32 phy_blk)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct zone_entry *zone;
+	int zone_no;
+
+	zone_no = (int)phy_blk >> 10;
+	if (zone_no >= xd_card->zone_cnt) {
+		dev_dbg(rtsx_dev(chip), "Set unused block to invalid zone (zone_no = %d, zone_cnt = %d)\n",
+			zone_no, xd_card->zone_cnt);
+		return;
+	}
+	zone = &xd_card->zone[zone_no];
+
+	if (!zone->free_table) {
+		if (xd_build_l2p_tbl(chip, zone_no) != STATUS_SUCCESS)
+			return;
+	}
+
+	if (zone->set_index >= XD_FREE_TABLE_CNT ||
+	    zone->set_index < 0) {
+		free_zone(zone);
+		dev_dbg(rtsx_dev(chip), "Set unused block fail, invalid set_index\n");
+		return;
+	}
+
+	dev_dbg(rtsx_dev(chip), "Set unused block to index %d\n",
+		zone->set_index);
+
+	zone->free_table[zone->set_index++] = (u16)(phy_blk & 0x3ff);
+	if (zone->set_index >= XD_FREE_TABLE_CNT)
+		zone->set_index = 0;
+	zone->unused_blk_cnt++;
+}
+
+static u32 xd_get_unused_block(struct rtsx_chip *chip, int zone_no)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct zone_entry *zone;
+	u32 phy_blk;
+
+	if (zone_no >= xd_card->zone_cnt) {
+		dev_dbg(rtsx_dev(chip), "Get unused block from invalid zone (zone_no = %d, zone_cnt = %d)\n",
+			zone_no, xd_card->zone_cnt);
+		return BLK_NOT_FOUND;
+	}
+	zone = &xd_card->zone[zone_no];
+
+	if (zone->unused_blk_cnt == 0 ||
+	    zone->set_index == zone->get_index) {
+		free_zone(zone);
+		dev_dbg(rtsx_dev(chip), "Get unused block fail, no unused block available\n");
+		return BLK_NOT_FOUND;
+	}
+	if (zone->get_index >= XD_FREE_TABLE_CNT || zone->get_index < 0) {
+		free_zone(zone);
+		dev_dbg(rtsx_dev(chip), "Get unused block fail, invalid get_index\n");
+		return BLK_NOT_FOUND;
+	}
+
+	dev_dbg(rtsx_dev(chip), "Get unused block from index %d\n",
+		zone->get_index);
+
+	phy_blk = zone->free_table[zone->get_index];
+	zone->free_table[zone->get_index++] = 0xFFFF;
+	if (zone->get_index >= XD_FREE_TABLE_CNT)
+		zone->get_index = 0;
+	zone->unused_blk_cnt--;
+
+	phy_blk += ((u32)(zone_no) << 10);
+	return phy_blk;
+}
+
+static void xd_set_l2p_tbl(struct rtsx_chip *chip,
+			   int zone_no, u16 log_off, u16 phy_off)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct zone_entry *zone;
+
+	zone = &xd_card->zone[zone_no];
+	zone->l2p_table[log_off] = phy_off;
+}
+
+static u32 xd_get_l2p_tbl(struct rtsx_chip *chip, int zone_no, u16 log_off)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct zone_entry *zone;
+	int retval;
+
+	zone = &xd_card->zone[zone_no];
+	if (zone->l2p_table[log_off] == 0xFFFF) {
+		u32 phy_blk = 0;
+		int i;
+
+#ifdef XD_DELAY_WRITE
+		retval = xd_delay_write(chip);
+		if (retval != STATUS_SUCCESS) {
+			dev_dbg(rtsx_dev(chip), "In %s, delay write fail!\n",
+				__func__);
+			return BLK_NOT_FOUND;
+		}
+#endif
+
+		if (zone->unused_blk_cnt <= 0) {
+			dev_dbg(rtsx_dev(chip), "No unused block!\n");
+			return BLK_NOT_FOUND;
+		}
+
+		for (i = 0; i < zone->unused_blk_cnt; i++) {
+			phy_blk = xd_get_unused_block(chip, zone_no);
+			if (phy_blk == BLK_NOT_FOUND) {
+				dev_dbg(rtsx_dev(chip), "No unused block available!\n");
+				return BLK_NOT_FOUND;
+			}
+
+			retval = xd_init_page(chip, phy_blk, log_off,
+					      0, xd_card->page_off + 1);
+			if (retval == STATUS_SUCCESS)
+				break;
+		}
+		if (i >= zone->unused_blk_cnt) {
+			dev_dbg(rtsx_dev(chip), "No good unused block available!\n");
+			return BLK_NOT_FOUND;
+		}
+
+		xd_set_l2p_tbl(chip, zone_no, log_off, (u16)(phy_blk & 0x3FF));
+		return phy_blk;
+	}
+
+	return (u32)zone->l2p_table[log_off] + ((u32)(zone_no) << 10);
+}
+
+int reset_xd_card(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+
+	memset(xd_card, 0, sizeof(struct xd_info));
+
+	xd_card->block_shift = 0;
+	xd_card->page_off = 0;
+	xd_card->addr_cycle = 0;
+	xd_card->capacity = 0;
+	xd_card->zone_cnt = 0;
+	xd_card->cis_block = 0xFFFF;
+	xd_card->delay_write.delay_write_flag = 0;
+
+	retval = enable_card_clock(chip, XD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = reset_xd(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = xd_init_l2p_tbl(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_mark_bad_block(struct rtsx_chip *chip, u32 phy_blk)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+	u32 page_addr;
+	u8 reg = 0;
+
+	dev_dbg(rtsx_dev(chip), "mark block 0x%x as bad block\n", phy_blk);
+
+	if (phy_blk == BLK_NOT_FOUND)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, XD_LATER_BBLK);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_H, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_L, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED0, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED1, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED2, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED3, 0xFF, 0xFF);
+
+	page_addr = phy_blk << xd_card->block_shift;
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF,
+		     xd_card->page_off + 1);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+		     XD_TRANSFER_START | XD_WRITE_REDUNDANT);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 500);
+	if (retval < 0) {
+		rtsx_clear_xd_error(chip);
+		rtsx_read_register(chip, XD_DAT, &reg);
+		if (reg & PROGRAM_ERROR)
+			xd_set_err_code(chip, XD_PRG_ERROR);
+		else
+			xd_set_err_code(chip, XD_TO_ERROR);
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_init_page(struct rtsx_chip *chip, u32 phy_blk,
+			u16 logoff, u8 start_page, u8 end_page)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+	u32 page_addr;
+	u8 reg = 0;
+
+	dev_dbg(rtsx_dev(chip), "Init block 0x%x\n", phy_blk);
+
+	if (start_page > end_page)
+		return STATUS_FAIL;
+	if (phy_blk == BLK_NOT_FOUND)
+		return STATUS_FAIL;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, 0xFF);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H,
+		     0xFF, (u8)(logoff >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF, (u8)logoff);
+
+	page_addr = (phy_blk << xd_card->block_shift) + start_page;
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CFG,
+		     XD_BA_TRANSFORM, XD_BA_TRANSFORM);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT,
+		     0xFF, (end_page - start_page));
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER,
+		     0xFF, XD_TRANSFER_START | XD_WRITE_REDUNDANT);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 500);
+	if (retval < 0) {
+		rtsx_clear_xd_error(chip);
+		rtsx_read_register(chip, XD_DAT, &reg);
+		if (reg & PROGRAM_ERROR) {
+			xd_mark_bad_block(chip, phy_blk);
+			xd_set_err_code(chip, XD_PRG_ERROR);
+		} else {
+			xd_set_err_code(chip, XD_TO_ERROR);
+		}
+		return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
+			u8 start_page, u8 end_page)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	u32 old_page, new_page;
+	u8 i, reg = 0;
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "Copy page from block 0x%x to block 0x%x\n",
+		old_blk, new_blk);
+
+	if (start_page > end_page)
+		return STATUS_FAIL;
+
+	if (old_blk == BLK_NOT_FOUND || new_blk == BLK_NOT_FOUND)
+		return STATUS_FAIL;
+
+	old_page = (old_blk << xd_card->block_shift) + start_page;
+	new_page = (new_blk << xd_card->block_shift) + start_page;
+
+	XD_CLR_BAD_NEWBLK(xd_card);
+
+	retval = rtsx_write_register(chip, CARD_DATA_SOURCE, 0x01,
+				     PINGPONG_BUFFER);
+	if (retval)
+		return retval;
+
+	for (i = start_page; i < end_page; i++) {
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+			rtsx_clear_xd_error(chip);
+			xd_set_err_code(chip, XD_NO_CARD);
+			return STATUS_FAIL;
+		}
+
+		rtsx_init_cmd(chip);
+
+		xd_assign_phy_addr(chip, old_page, XD_RW_ADDR);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
+			     XD_AUTO_CHK_DATA_STATUS, 0);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+			     XD_TRANSFER_START | XD_READ_PAGES);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+			     XD_TRANSFER_END, XD_TRANSFER_END);
+
+		retval = rtsx_send_cmd(chip, XD_CARD, 500);
+		if (retval < 0) {
+			rtsx_clear_xd_error(chip);
+			reg = 0;
+			rtsx_read_register(chip, XD_CTL, &reg);
+			if (reg & (XD_ECC1_ERROR | XD_ECC2_ERROR)) {
+				mdelay(100);
+
+				if (detect_card_cd(chip,
+						   XD_CARD) != STATUS_SUCCESS) {
+					xd_set_err_code(chip, XD_NO_CARD);
+					return STATUS_FAIL;
+				}
+
+				if (((reg & XD_ECC1_ERROR) &&
+				     (reg & XD_ECC1_UNCORRECTABLE)) ||
+				    ((reg & XD_ECC2_ERROR) &&
+				     (reg & XD_ECC2_UNCORRECTABLE))) {
+					rtsx_write_register(chip,
+							    XD_PAGE_STATUS,
+							    0xFF,
+							    XD_BPG);
+					rtsx_write_register(chip,
+							    XD_BLOCK_STATUS,
+							    0xFF,
+							    XD_GBLK);
+					XD_SET_BAD_OLDBLK(xd_card);
+					dev_dbg(rtsx_dev(chip), "old block 0x%x ecc error\n",
+						old_blk);
+				}
+			} else {
+				xd_set_err_code(chip, XD_TO_ERROR);
+				return STATUS_FAIL;
+			}
+		}
+
+		if (XD_CHK_BAD_OLDBLK(xd_card))
+			rtsx_clear_xd_error(chip);
+
+		rtsx_init_cmd(chip);
+
+		xd_assign_phy_addr(chip, new_page, XD_RW_ADDR);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+			     XD_TRANSFER_START | XD_WRITE_PAGES);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+			     XD_TRANSFER_END, XD_TRANSFER_END);
+
+		retval = rtsx_send_cmd(chip, XD_CARD, 300);
+		if (retval < 0) {
+			rtsx_clear_xd_error(chip);
+			reg = 0;
+			rtsx_read_register(chip, XD_DAT, &reg);
+			if (reg & PROGRAM_ERROR) {
+				xd_mark_bad_block(chip, new_blk);
+				xd_set_err_code(chip, XD_PRG_ERROR);
+				XD_SET_BAD_NEWBLK(xd_card);
+			} else {
+				xd_set_err_code(chip, XD_TO_ERROR);
+			}
+			return STATUS_FAIL;
+		}
+
+		old_page++;
+		new_page++;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_reset_cmd(struct rtsx_chip *chip)
+{
+	int retval;
+	u8 *ptr;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER,
+		     0xFF, XD_TRANSFER_START | XD_RESET);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+	rtsx_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
+	rtsx_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 100);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	ptr = rtsx_get_cmd_data(chip) + 1;
+	if (((ptr[0] & READY_FLAG) == READY_STATE) && (ptr[1] & XD_RDY))
+		return STATUS_SUCCESS;
+
+	return STATUS_FAIL;
+}
+
+static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	u32 page_addr;
+	u8 reg = 0, *ptr;
+	int i, retval;
+
+	if (phy_blk == BLK_NOT_FOUND)
+		return STATUS_FAIL;
+
+	page_addr = phy_blk << xd_card->block_shift;
+
+	for (i = 0; i < 3; i++) {
+		rtsx_init_cmd(chip);
+
+		xd_assign_phy_addr(chip, page_addr, XD_ERASE_ADDR);
+
+		rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+			     XD_TRANSFER_START | XD_ERASE);
+		rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+			     XD_TRANSFER_END, XD_TRANSFER_END);
+		rtsx_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
+
+		retval = rtsx_send_cmd(chip, XD_CARD, 250);
+		if (retval < 0) {
+			rtsx_clear_xd_error(chip);
+			rtsx_read_register(chip, XD_DAT, &reg);
+			if (reg & PROGRAM_ERROR) {
+				xd_mark_bad_block(chip, phy_blk);
+				xd_set_err_code(chip, XD_PRG_ERROR);
+				return STATUS_FAIL;
+			}
+			xd_set_err_code(chip, XD_ERASE_FAIL);
+			retval = xd_reset_cmd(chip);
+			if (retval != STATUS_SUCCESS)
+				return STATUS_FAIL;
+			continue;
+		}
+
+		ptr = rtsx_get_cmd_data(chip) + 1;
+		if (*ptr & PROGRAM_ERROR) {
+			xd_mark_bad_block(chip, phy_blk);
+			xd_set_err_code(chip, XD_PRG_ERROR);
+			return STATUS_FAIL;
+		}
+
+		return STATUS_SUCCESS;
+	}
+
+	xd_mark_bad_block(chip, phy_blk);
+	xd_set_err_code(chip, XD_ERASE_FAIL);
+	return STATUS_FAIL;
+}
+
+static int xd_build_l2p_tbl(struct rtsx_chip *chip, int zone_no)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct zone_entry *zone;
+	int retval;
+	u32 start, end, i;
+	u16 max_logoff, cur_fst_page_logoff;
+	u16 cur_lst_page_logoff, ent_lst_page_logoff;
+	u8 redunt[11];
+
+	dev_dbg(rtsx_dev(chip), "%s: %d\n", __func__, zone_no);
+
+	if (!xd_card->zone) {
+		retval = xd_init_l2p_tbl(chip);
+		if (retval != STATUS_SUCCESS)
+			return retval;
+	}
+
+	if (xd_card->zone[zone_no].build_flag) {
+		dev_dbg(rtsx_dev(chip), "l2p table of zone %d has been built\n",
+			zone_no);
+		return STATUS_SUCCESS;
+	}
+
+	zone = &xd_card->zone[zone_no];
+
+	if (!zone->l2p_table) {
+		zone->l2p_table = vmalloc(2000);
+		if (!zone->l2p_table)
+			goto build_fail;
+	}
+	memset((u8 *)(zone->l2p_table), 0xff, 2000);
+
+	if (!zone->free_table) {
+		zone->free_table = vmalloc(XD_FREE_TABLE_CNT * 2);
+		if (!zone->free_table)
+			goto build_fail;
+	}
+	memset((u8 *)(zone->free_table), 0xff, XD_FREE_TABLE_CNT * 2);
+
+	if (zone_no == 0) {
+		if (xd_card->cis_block == 0xFFFF)
+			start = 0;
+		else
+			start = xd_card->cis_block + 1;
+		if (XD_CHK_4MB(xd_card)) {
+			end = 0x200;
+			max_logoff = 499;
+		} else {
+			end = 0x400;
+			max_logoff = 999;
+		}
+	} else {
+		start = (u32)(zone_no) << 10;
+		end = (u32)(zone_no + 1) << 10;
+		max_logoff = 999;
+	}
+
+	dev_dbg(rtsx_dev(chip), "start block 0x%x, end block 0x%x\n",
+		start, end);
+
+	zone->set_index = 0;
+	zone->get_index = 0;
+	zone->unused_blk_cnt = 0;
+
+	for (i = start; i < end; i++) {
+		u32 page_addr = i << xd_card->block_shift;
+		u32 phy_block;
+
+		retval = xd_read_redundant(chip, page_addr, redunt, 11);
+		if (retval != STATUS_SUCCESS)
+			continue;
+
+		if (redunt[BLOCK_STATUS] != 0xFF) {
+			dev_dbg(rtsx_dev(chip), "bad block\n");
+			continue;
+		}
+
+		if (xd_check_data_blank(redunt)) {
+			dev_dbg(rtsx_dev(chip), "blank block\n");
+			xd_set_unused_block(chip, i);
+			continue;
+		}
+
+		cur_fst_page_logoff = xd_load_log_block_addr(redunt);
+		if (cur_fst_page_logoff == 0xFFFF ||
+		    cur_fst_page_logoff > max_logoff) {
+			retval = xd_erase_block(chip, i);
+			if (retval == STATUS_SUCCESS)
+				xd_set_unused_block(chip, i);
+			continue;
+		}
+
+		if (zone_no == 0 && cur_fst_page_logoff == 0 &&
+		    redunt[PAGE_STATUS] != XD_GPG)
+			XD_SET_MBR_FAIL(xd_card);
+
+		if (zone->l2p_table[cur_fst_page_logoff] == 0xFFFF) {
+			zone->l2p_table[cur_fst_page_logoff] = (u16)(i & 0x3FF);
+			continue;
+		}
+
+		phy_block = zone->l2p_table[cur_fst_page_logoff] +
+			((u32)((zone_no) << 10));
+
+		page_addr = ((i + 1) << xd_card->block_shift) - 1;
+
+		retval = xd_read_redundant(chip, page_addr, redunt, 11);
+		if (retval != STATUS_SUCCESS)
+			continue;
+
+		cur_lst_page_logoff = xd_load_log_block_addr(redunt);
+		if (cur_lst_page_logoff == cur_fst_page_logoff) {
+			int m;
+
+			page_addr = ((phy_block + 1) <<
+				xd_card->block_shift) - 1;
+
+			for (m = 0; m < 3; m++) {
+				retval = xd_read_redundant(chip, page_addr,
+							   redunt, 11);
+				if (retval == STATUS_SUCCESS)
+					break;
+			}
+
+			if (m == 3) {
+				zone->l2p_table[cur_fst_page_logoff] =
+					(u16)(i & 0x3FF);
+				retval = xd_erase_block(chip, phy_block);
+				if (retval == STATUS_SUCCESS)
+					xd_set_unused_block(chip, phy_block);
+				continue;
+			}
+
+			ent_lst_page_logoff = xd_load_log_block_addr(redunt);
+			if (ent_lst_page_logoff != cur_fst_page_logoff) {
+				zone->l2p_table[cur_fst_page_logoff] =
+					(u16)(i & 0x3FF);
+				retval = xd_erase_block(chip, phy_block);
+				if (retval == STATUS_SUCCESS)
+					xd_set_unused_block(chip, phy_block);
+				continue;
+			} else {
+				retval = xd_erase_block(chip, i);
+				if (retval == STATUS_SUCCESS)
+					xd_set_unused_block(chip, i);
+			}
+		} else {
+			retval = xd_erase_block(chip, i);
+			if (retval == STATUS_SUCCESS)
+				xd_set_unused_block(chip, i);
+		}
+	}
+
+	if (XD_CHK_4MB(xd_card))
+		end = 500;
+	else
+		end = 1000;
+
+	i = 0;
+	for (start = 0; start < end; start++) {
+		if (zone->l2p_table[start] == 0xFFFF)
+			i++;
+	}
+
+	dev_dbg(rtsx_dev(chip), "Block count %d, invalid L2P entry %d\n",
+		end, i);
+	dev_dbg(rtsx_dev(chip), "Total unused block: %d\n",
+		zone->unused_blk_cnt);
+
+	if ((zone->unused_blk_cnt - i) < 1)
+		chip->card_wp |= XD_CARD;
+
+	zone->build_flag = 1;
+
+	return STATUS_SUCCESS;
+
+build_fail:
+	vfree(zone->l2p_table);
+	zone->l2p_table = NULL;
+	vfree(zone->free_table);
+	zone->free_table = NULL;
+
+	return STATUS_FAIL;
+}
+
+static int xd_send_cmd(struct rtsx_chip *chip, u8 cmd)
+{
+	int retval;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, cmd);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+		     XD_TRANSFER_START | XD_SET_CMD);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+
+	retval = rtsx_send_cmd(chip, XD_CARD, 200);
+	if (retval < 0)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_read_multiple_pages(struct rtsx_chip *chip, u32 phy_blk,
+				  u32 log_blk, u8 start_page, u8 end_page,
+				  u8 *buf, unsigned int *index,
+				  unsigned int *offset)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	u32 page_addr, new_blk;
+	u16 log_off;
+	u8 reg_val, page_cnt;
+	int zone_no, retval, i;
+
+	if (start_page > end_page)
+		goto status_fail;
+
+	page_cnt = end_page - start_page;
+	zone_no = (int)(log_blk / 1000);
+	log_off = (u16)(log_blk % 1000);
+
+	if ((phy_blk & 0x3FF) == 0x3FF) {
+		for (i = 0; i < 256; i++) {
+			page_addr = ((u32)i) << xd_card->block_shift;
+
+			retval = xd_read_redundant(chip, page_addr, NULL, 0);
+			if (retval == STATUS_SUCCESS)
+				break;
+
+			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+				xd_set_err_code(chip, XD_NO_CARD);
+				goto status_fail;
+			}
+		}
+	}
+
+	page_addr = (phy_blk << xd_card->block_shift) + start_page;
+
+	rtsx_init_cmd(chip);
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_PPB_TO_SIE, XD_PPB_TO_SIE);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
+		     XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
+
+	trans_dma_enable(chip->srb->sc_data_direction, chip,
+			 page_cnt * 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
+		     XD_TRANSFER_START | XD_READ_PAGES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END | XD_PPB_EMPTY,
+		     XD_TRANSFER_END | XD_PPB_EMPTY);
+
+	rtsx_send_cmd_no_wait(chip);
+
+	retval = rtsx_transfer_data_partial(chip, XD_CARD, buf, page_cnt * 512,
+					    scsi_sg_count(chip->srb),
+					    index, offset, DMA_FROM_DEVICE,
+					    chip->xd_timeout);
+	if (retval < 0) {
+		rtsx_clear_xd_error(chip);
+
+		if (retval == -ETIMEDOUT) {
+			xd_set_err_code(chip, XD_TO_ERROR);
+			goto status_fail;
+		} else {
+			goto fail;
+		}
+	}
+
+	return STATUS_SUCCESS;
+
+fail:
+	retval = rtsx_read_register(chip, XD_PAGE_STATUS, &reg_val);
+	if (retval)
+		return retval;
+
+	if (reg_val !=  XD_GPG)
+		xd_set_err_code(chip, XD_PRG_ERROR);
+
+	retval = rtsx_read_register(chip, XD_CTL, &reg_val);
+	if (retval)
+		return retval;
+
+	if (((reg_val & (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE)) ==
+				(XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE)) ||
+		((reg_val & (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE)) ==
+			(XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))) {
+		wait_timeout(100);
+
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+			xd_set_err_code(chip, XD_NO_CARD);
+			goto status_fail;
+		}
+
+		xd_set_err_code(chip, XD_ECC_ERROR);
+
+		new_blk = xd_get_unused_block(chip, zone_no);
+		if (new_blk == NO_NEW_BLK) {
+			XD_CLR_BAD_OLDBLK(xd_card);
+			goto status_fail;
+		}
+
+		retval = xd_copy_page(chip, phy_blk, new_blk, 0,
+				      xd_card->page_off + 1);
+		if (retval != STATUS_SUCCESS) {
+			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
+				retval = xd_erase_block(chip, new_blk);
+				if (retval == STATUS_SUCCESS)
+					xd_set_unused_block(chip, new_blk);
+			} else {
+				XD_CLR_BAD_NEWBLK(xd_card);
+			}
+			XD_CLR_BAD_OLDBLK(xd_card);
+			goto status_fail;
+		}
+		xd_set_l2p_tbl(chip, zone_no, log_off, (u16)(new_blk & 0x3FF));
+		xd_erase_block(chip, phy_blk);
+		xd_mark_bad_block(chip, phy_blk);
+		XD_CLR_BAD_OLDBLK(xd_card);
+	}
+
+status_fail:
+	return STATUS_FAIL;
+}
+
+static int xd_finish_write(struct rtsx_chip *chip,
+			   u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval, zone_no;
+	u16 log_off;
+
+	dev_dbg(rtsx_dev(chip), "old_blk = 0x%x, ", old_blk);
+	dev_dbg(rtsx_dev(chip),	"new_blk = 0x%x, ", new_blk);
+	dev_dbg(rtsx_dev(chip), "log_blk = 0x%x\n", log_blk);
+
+	if (page_off > xd_card->page_off)
+		return STATUS_FAIL;
+
+	zone_no = (int)(log_blk / 1000);
+	log_off = (u16)(log_blk % 1000);
+
+	if (old_blk == BLK_NOT_FOUND) {
+		retval = xd_init_page(chip, new_blk, log_off,
+				      page_off, xd_card->page_off + 1);
+		if (retval != STATUS_SUCCESS) {
+			retval = xd_erase_block(chip, new_blk);
+			if (retval == STATUS_SUCCESS)
+				xd_set_unused_block(chip, new_blk);
+			return STATUS_FAIL;
+		}
+	} else {
+		retval = xd_copy_page(chip, old_blk, new_blk,
+				      page_off, xd_card->page_off + 1);
+		if (retval != STATUS_SUCCESS) {
+			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
+				retval = xd_erase_block(chip, new_blk);
+				if (retval == STATUS_SUCCESS)
+					xd_set_unused_block(chip, new_blk);
+			}
+			XD_CLR_BAD_NEWBLK(xd_card);
+			return STATUS_FAIL;
+		}
+
+		retval = xd_erase_block(chip, old_blk);
+		if (retval == STATUS_SUCCESS) {
+			if (XD_CHK_BAD_OLDBLK(xd_card)) {
+				xd_mark_bad_block(chip, old_blk);
+				XD_CLR_BAD_OLDBLK(xd_card);
+			} else {
+				xd_set_unused_block(chip, old_blk);
+			}
+		} else {
+			xd_set_err_code(chip, XD_NO_ERROR);
+			XD_CLR_BAD_OLDBLK(xd_card);
+		}
+	}
+
+	xd_set_l2p_tbl(chip, zone_no, log_off, (u16)(new_blk & 0x3FF));
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_prepare_write(struct rtsx_chip *chip,
+			    u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
+{
+	int retval;
+
+	dev_dbg(rtsx_dev(chip), "%s, old_blk = 0x%x, new_blk = 0x%x, log_blk = 0x%x, page_off = %d\n",
+		__func__, old_blk, new_blk, log_blk, (int)page_off);
+
+	if (page_off) {
+		retval = xd_copy_page(chip, old_blk, new_blk, 0, page_off);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int xd_write_multiple_pages(struct rtsx_chip *chip, u32 old_blk,
+				   u32 new_blk, u32 log_blk, u8 start_page,
+				   u8 end_page, u8 *buf, unsigned int *index,
+				   unsigned int *offset)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	u32 page_addr;
+	int zone_no, retval;
+	u16 log_off;
+	u8 page_cnt, reg_val;
+
+	dev_dbg(rtsx_dev(chip), "%s, old_blk = 0x%x, new_blk = 0x%x, log_blk = 0x%x\n",
+		__func__, old_blk, new_blk, log_blk);
+
+	if (start_page > end_page)
+		goto status_fail;
+
+	page_cnt = end_page - start_page;
+	zone_no = (int)(log_blk / 1000);
+	log_off = (u16)(log_blk % 1000);
+
+	page_addr = (new_blk << xd_card->block_shift) + start_page;
+
+	retval = xd_send_cmd(chip, READ1_1);
+	if (retval != STATUS_SUCCESS)
+		goto status_fail;
+
+	rtsx_init_cmd(chip);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H,
+		     0xFF, (u8)(log_off >> 8));
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF, (u8)log_off);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, XD_GBLK);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
+
+	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_BA_TRANSFORM,
+		     XD_BA_TRANSFORM);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
+	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
+
+	trans_dma_enable(chip->srb->sc_data_direction, chip,
+			 page_cnt * 512, DMA_512);
+
+	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER,
+		     0xFF, XD_TRANSFER_START | XD_WRITE_PAGES);
+	rtsx_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
+		     XD_TRANSFER_END, XD_TRANSFER_END);
+
+	rtsx_send_cmd_no_wait(chip);
+
+	retval = rtsx_transfer_data_partial(chip, XD_CARD, buf, page_cnt * 512,
+					    scsi_sg_count(chip->srb),
+					    index, offset, DMA_TO_DEVICE, chip->xd_timeout);
+	if (retval < 0) {
+		rtsx_clear_xd_error(chip);
+
+		if (retval == -ETIMEDOUT) {
+			xd_set_err_code(chip, XD_TO_ERROR);
+			goto status_fail;
+		} else {
+			goto fail;
+		}
+	}
+
+	if (end_page == (xd_card->page_off + 1)) {
+		xd_card->delay_write.delay_write_flag = 0;
+
+		if (old_blk != BLK_NOT_FOUND) {
+			retval = xd_erase_block(chip, old_blk);
+			if (retval == STATUS_SUCCESS) {
+				if (XD_CHK_BAD_OLDBLK(xd_card)) {
+					xd_mark_bad_block(chip, old_blk);
+					XD_CLR_BAD_OLDBLK(xd_card);
+				} else {
+					xd_set_unused_block(chip, old_blk);
+				}
+			} else {
+				xd_set_err_code(chip, XD_NO_ERROR);
+				XD_CLR_BAD_OLDBLK(xd_card);
+			}
+		}
+		xd_set_l2p_tbl(chip, zone_no, log_off, (u16)(new_blk & 0x3FF));
+	}
+
+	return STATUS_SUCCESS;
+
+fail:
+	retval = rtsx_read_register(chip, XD_DAT, &reg_val);
+	if (retval)
+		return retval;
+	if (reg_val & PROGRAM_ERROR) {
+		xd_set_err_code(chip, XD_PRG_ERROR);
+		xd_mark_bad_block(chip, new_blk);
+	}
+
+status_fail:
+	return STATUS_FAIL;
+}
+
+#ifdef XD_DELAY_WRITE
+int xd_delay_write(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	struct xd_delay_write_tag *delay_write = &xd_card->delay_write;
+	int retval;
+
+	if (delay_write->delay_write_flag) {
+		retval = xd_switch_clock(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		delay_write->delay_write_flag = 0;
+		retval = xd_finish_write(chip,
+					 delay_write->old_phyblock,
+					 delay_write->new_phyblock,
+					 delay_write->logblock,
+					 delay_write->pageoff);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	}
+
+	return STATUS_SUCCESS;
+}
+#endif
+
+int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	  u32 start_sector, u16 sector_cnt)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	unsigned int lun = SCSI_LUN(srb);
+#ifdef XD_DELAY_WRITE
+	struct xd_delay_write_tag *delay_write = &xd_card->delay_write;
+#endif
+	int retval, zone_no;
+	unsigned int index = 0, offset = 0;
+	u32 log_blk, old_blk = 0, new_blk = 0;
+	u16 log_off, total_sec_cnt = sector_cnt;
+	u8 start_page, end_page = 0, page_cnt;
+	u8 *ptr;
+
+	xd_set_err_code(chip, XD_NO_ERROR);
+
+	xd_card->cleanup_counter = 0;
+
+	dev_dbg(rtsx_dev(chip), "%s: scsi_sg_count = %d\n", __func__,
+		scsi_sg_count(srb));
+
+	ptr = (u8 *)scsi_sglist(srb);
+
+	retval = xd_switch_clock(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+		chip->card_fail |= XD_CARD;
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		return STATUS_FAIL;
+	}
+
+	log_blk = start_sector >> xd_card->block_shift;
+	start_page = (u8)start_sector & xd_card->page_off;
+	zone_no = (int)(log_blk / 1000);
+	log_off = (u16)(log_blk % 1000);
+
+	if (xd_card->zone[zone_no].build_flag == 0) {
+		retval = xd_build_l2p_tbl(chip, zone_no);
+		if (retval != STATUS_SUCCESS) {
+			chip->card_fail |= XD_CARD;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return STATUS_FAIL;
+		}
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE) {
+#ifdef XD_DELAY_WRITE
+		if (delay_write->delay_write_flag &&
+		    delay_write->logblock == log_blk &&
+		    start_page > delay_write->pageoff) {
+			delay_write->delay_write_flag = 0;
+			if (delay_write->old_phyblock != BLK_NOT_FOUND) {
+				retval = xd_copy_page(chip,
+						      delay_write->old_phyblock,
+						      delay_write->new_phyblock,
+						      delay_write->pageoff,
+						      start_page);
+				if (retval != STATUS_SUCCESS) {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MEDIA_WRITE_ERR);
+					return STATUS_FAIL;
+				}
+			}
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else if (delay_write->delay_write_flag &&
+				(delay_write->logblock == log_blk) &&
+				(start_page == delay_write->pageoff)) {
+			delay_write->delay_write_flag = 0;
+			old_blk = delay_write->old_phyblock;
+			new_blk = delay_write->new_phyblock;
+		} else {
+			retval = xd_delay_write(chip);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+#endif
+			old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
+			new_blk  = xd_get_unused_block(chip, zone_no);
+			if (old_blk == BLK_NOT_FOUND ||
+			    new_blk == BLK_NOT_FOUND) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+
+			retval = xd_prepare_write(chip, old_blk, new_blk,
+						  log_blk, start_page);
+			if (retval != STATUS_SUCCESS) {
+				if (detect_card_cd(chip, XD_CARD) !=
+					STATUS_SUCCESS) {
+					set_sense_type(chip, lun,
+						       SENSE_TYPE_MEDIA_NOT_PRESENT);
+					return STATUS_FAIL;
+				}
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+#ifdef XD_DELAY_WRITE
+		}
+#endif
+	} else {
+#ifdef XD_DELAY_WRITE
+		retval = xd_delay_write(chip);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return STATUS_FAIL;
+		}
+#endif
+
+		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
+		if (old_blk == BLK_NOT_FOUND) {
+			set_sense_type(chip, lun,
+				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			return STATUS_FAIL;
+		}
+	}
+
+	dev_dbg(rtsx_dev(chip), "old_blk = 0x%x\n", old_blk);
+
+	while (total_sec_cnt) {
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+			chip->card_fail |= XD_CARD;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return STATUS_FAIL;
+		}
+
+		if ((start_page + total_sec_cnt) > (xd_card->page_off + 1))
+			end_page = xd_card->page_off + 1;
+		else
+			end_page = start_page + (u8)total_sec_cnt;
+
+		page_cnt = end_page - start_page;
+		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			retval = xd_read_multiple_pages(chip, old_blk, log_blk,
+							start_page, end_page,
+							ptr, &index, &offset);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+				return STATUS_FAIL;
+			}
+		} else {
+			retval = xd_write_multiple_pages(chip, old_blk,
+							 new_blk, log_blk,
+							 start_page, end_page,
+							 ptr, &index, &offset);
+			if (retval != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+		}
+
+		total_sec_cnt -= page_cnt;
+		if (scsi_sg_count(srb) == 0)
+			ptr += page_cnt * 512;
+
+		if (total_sec_cnt == 0)
+			break;
+
+		log_blk++;
+		zone_no = (int)(log_blk / 1000);
+		log_off = (u16)(log_blk % 1000);
+
+		if (xd_card->zone[zone_no].build_flag == 0) {
+			retval = xd_build_l2p_tbl(chip, zone_no);
+			if (retval != STATUS_SUCCESS) {
+				chip->card_fail |= XD_CARD;
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+		}
+
+		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
+		if (old_blk == BLK_NOT_FOUND) {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE)
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
+			else
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+
+			return STATUS_FAIL;
+		}
+
+		if (srb->sc_data_direction == DMA_TO_DEVICE) {
+			new_blk = xd_get_unused_block(chip, zone_no);
+			if (new_blk == BLK_NOT_FOUND) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_WRITE_ERR);
+				return STATUS_FAIL;
+			}
+		}
+
+		start_page = 0;
+	}
+
+	if (srb->sc_data_direction == DMA_TO_DEVICE &&
+	    (end_page != (xd_card->page_off + 1))) {
+#ifdef XD_DELAY_WRITE
+		delay_write->delay_write_flag = 1;
+		delay_write->old_phyblock = old_blk;
+		delay_write->new_phyblock = new_blk;
+		delay_write->logblock = log_blk;
+		delay_write->pageoff = end_page;
+#else
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+			chip->card_fail |= XD_CARD;
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+			return STATUS_FAIL;
+		}
+
+		retval = xd_finish_write(chip, old_blk, new_blk,
+					 log_blk, end_page);
+		if (retval != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+				set_sense_type(chip, lun,
+					       SENSE_TYPE_MEDIA_NOT_PRESENT);
+				return STATUS_FAIL;
+			}
+			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
+			return STATUS_FAIL;
+		}
+#endif
+	}
+
+	scsi_set_resid(srb, 0);
+
+	return STATUS_SUCCESS;
+}
+
+void xd_free_l2p_tbl(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int i = 0;
+
+	if (xd_card->zone) {
+		for (i = 0; i < xd_card->zone_cnt; i++) {
+			vfree(xd_card->zone[i].l2p_table);
+			xd_card->zone[i].l2p_table = NULL;
+			vfree(xd_card->zone[i].free_table);
+			xd_card->zone[i].free_table = NULL;
+		}
+		vfree(xd_card->zone);
+		xd_card->zone = NULL;
+	}
+}
+
+void xd_cleanup_work(struct rtsx_chip *chip)
+{
+#ifdef XD_DELAY_WRITE
+	struct xd_info *xd_card = &chip->xd_card;
+
+	if (xd_card->delay_write.delay_write_flag) {
+		dev_dbg(rtsx_dev(chip), "xD: delay write\n");
+		xd_delay_write(chip);
+		xd_card->cleanup_counter = 0;
+	}
+#endif
+}
+
+int xd_power_off_card3v3(struct rtsx_chip *chip)
+{
+	int retval;
+
+	retval = disable_card_clock(chip, XD_CARD);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	retval = rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+	if (retval)
+		return retval;
+
+	if (!chip->ft2_fast_mode) {
+		retval = card_power_off(chip, XD_CARD);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+
+		wait_timeout(50);
+	}
+
+	if (chip->asic_code) {
+		retval = xd_pull_ctl_disable(chip);
+		if (retval != STATUS_SUCCESS)
+			return STATUS_FAIL;
+	} else {
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL, 0xFF, 0xDF);
+		if (retval)
+			return retval;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+int release_xd_card(struct rtsx_chip *chip)
+{
+	struct xd_info *xd_card = &chip->xd_card;
+	int retval;
+
+	chip->card_ready &= ~XD_CARD;
+	chip->card_fail &= ~XD_CARD;
+	chip->card_wp &= ~XD_CARD;
+
+	xd_card->delay_write.delay_write_flag = 0;
+
+	xd_free_l2p_tbl(chip);
+
+	retval = xd_power_off_card3v3(chip);
+	if (retval != STATUS_SUCCESS)
+		return STATUS_FAIL;
+
+	return STATUS_SUCCESS;
+}
diff --git a/drivers/staging/rts5208/xd.h b/drivers/staging/rts5208/xd.h
new file mode 100644
index 000000000..98c00f268
--- /dev/null
+++ b/drivers/staging/rts5208/xd.h
@@ -0,0 +1,176 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Author:
+ *   Wei WANG (wei_wang@realsil.com.cn)
+ *   Micky Ching (micky_ching@realsil.com.cn)
+ */
+
+#ifndef __REALTEK_RTSX_XD_H
+#define __REALTEK_RTSX_XD_H
+
+#define	XD_DELAY_WRITE
+
+/* Error Codes */
+#define	XD_NO_ERROR			0x00
+#define	XD_NO_MEMORY			0x80
+#define	XD_PRG_ERROR			0x40
+#define	XD_NO_CARD			0x20
+#define	XD_READ_FAIL			0x10
+#define	XD_ERASE_FAIL			0x08
+#define	XD_WRITE_FAIL			0x04
+#define	XD_ECC_ERROR			0x02
+#define	XD_TO_ERROR			0x01
+
+/* XD Commands */
+#define	READ1_1				0x00
+#define	READ1_2				0x01
+#define	READ2				0x50
+#define READ_ID				0x90
+#define RESET				0xff
+#define PAGE_PRG_1			0x80
+#define PAGE_PRG_2			0x10
+#define	BLK_ERASE_1			0x60
+#define	BLK_ERASE_2			0xD0
+#define READ_STS			0x70
+#define READ_XD_ID			0x9A
+#define	COPY_BACK_512			0x8A
+#define	COPY_BACK_2K			0x85
+#define	READ1_1_2			0x30
+#define	READ1_1_3			0x35
+#define	CHG_DAT_OUT_1			0x05
+#define RDM_DAT_OUT_1			0x05
+#define	CHG_DAT_OUT_2			0xE0
+#define RDM_DAT_OUT_2			0xE0
+#define	CHG_DAT_OUT_2			0xE0
+#define	CHG_DAT_IN_1			0x85
+#define	CACHE_PRG			0x15
+
+/* Redundant Area Related */
+#define XD_EXTRA_SIZE			0x10
+#define XD_2K_EXTRA_SIZE		0x40
+
+#define	NOT_WRITE_PROTECTED		0x80
+#define	READY_STATE			0x40
+#define	PROGRAM_ERROR			0x01
+#define	PROGRAM_ERROR_N_1		0x02
+#define	INTERNAL_READY			0x20
+#define	READY_FLAG			0x5F
+
+#define	XD_8M_X8_512			0xE6
+#define	XD_16M_X8_512			0x73
+#define	XD_32M_X8_512			0x75
+#define	XD_64M_X8_512			0x76
+#define	XD_128M_X8_512			0x79
+#define	XD_256M_X8_512			0x71
+#define	XD_128M_X8_2048			0xF1
+#define	XD_256M_X8_2048			0xDA
+#define	XD_512M_X8			0xDC
+#define	XD_128M_X16_2048		0xC1
+#define	XD_4M_X8_512_1			0xE3
+#define	XD_4M_X8_512_2			0xE5
+#define	XD_1G_X8_512			0xD3
+#define	XD_2G_X8_512			0xD5
+
+#define	XD_ID_CODE			0xB5
+
+#define	VENDOR_BLOCK			0xEFFF
+#define	CIS_BLOCK			0xDFFF
+
+#define	BLK_NOT_FOUND			0xFFFFFFFF
+
+#define	NO_NEW_BLK			0xFFFFFFFF
+
+#define	PAGE_CORRECTABLE		0x0
+#define	PAGE_NOTCORRECTABLE		0x1
+
+#define	NO_OFFSET			0x0
+#define	WITH_OFFSET			0x1
+
+#define	SECT_PER_PAGE			4
+#define	XD_ADDR_MODE_2C			XD_ADDR_MODE_2A
+
+#define ZONE0_BAD_BLOCK			23
+#define NOT_ZONE0_BAD_BLOCK		24
+
+#define	XD_RW_ADDR			0x01
+#define	XD_ERASE_ADDR			0x02
+
+#define	XD_PAGE_512(xd_card)		\
+do {					\
+	(xd_card)->block_shift = 5;	\
+	(xd_card)->page_off = 0x1F;	\
+} while (0)
+
+#define	XD_SET_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag |= 0x01)
+#define	XD_CLR_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag &= ~0x01)
+#define	XD_CHK_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag & 0x01)
+
+#define	XD_SET_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag |= 0x02)
+#define	XD_CLR_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag &= ~0x02)
+#define	XD_CHK_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag & 0x02)
+
+#define	XD_SET_MBR_FAIL(xd_card)	((xd_card)->multi_flag |= 0x04)
+#define	XD_CLR_MBR_FAIL(xd_card)	((xd_card)->multi_flag &= ~0x04)
+#define	XD_CHK_MBR_FAIL(xd_card)	((xd_card)->multi_flag & 0x04)
+
+#define	XD_SET_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag |= 0x08)
+#define	XD_CLR_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag &= ~0x08)
+#define	XD_CHK_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag & 0x08)
+
+#define	XD_SET_4MB(xd_card)		((xd_card)->multi_flag |= 0x10)
+#define	XD_CLR_4MB(xd_card)		((xd_card)->multi_flag &= ~0x10)
+#define	XD_CHK_4MB(xd_card)		((xd_card)->multi_flag & 0x10)
+
+#define	XD_SET_ECC_ERR(xd_card)		((xd_card)->multi_flag |= 0x40)
+#define	XD_CLR_ECC_ERR(xd_card)		((xd_card)->multi_flag &= ~0x40)
+#define	XD_CHK_ECC_ERR(xd_card)		((xd_card)->multi_flag & 0x40)
+
+#define PAGE_STATUS		0
+#define BLOCK_STATUS		1
+#define BLOCK_ADDR1_L		2
+#define BLOCK_ADDR1_H		3
+#define BLOCK_ADDR2_L		4
+#define BLOCK_ADDR2_H		5
+#define RESERVED0		6
+#define RESERVED1		7
+#define RESERVED2		8
+#define RESERVED3		9
+#define PARITY			10
+
+#define	CIS0_0			0
+#define	CIS0_1			1
+#define	CIS0_2			2
+#define	CIS0_3			3
+#define	CIS0_4			4
+#define	CIS0_5			5
+#define	CIS0_6			6
+#define	CIS0_7			7
+#define	CIS0_8			8
+#define	CIS0_9			9
+#define	CIS1_0			256
+#define	CIS1_1			(256 + 1)
+#define	CIS1_2			(256 + 2)
+#define	CIS1_3			(256 + 3)
+#define	CIS1_4			(256 + 4)
+#define	CIS1_5			(256 + 5)
+#define	CIS1_6			(256 + 6)
+#define	CIS1_7			(256 + 7)
+#define	CIS1_8			(256 + 8)
+#define	CIS1_9			(256 + 9)
+
+int reset_xd_card(struct rtsx_chip *chip);
+#ifdef XD_DELAY_WRITE
+int xd_delay_write(struct rtsx_chip *chip);
+#endif
+int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
+	  u32 start_sector, u16 sector_cnt);
+void xd_free_l2p_tbl(struct rtsx_chip *chip);
+void xd_cleanup_work(struct rtsx_chip *chip);
+int xd_power_off_card3v3(struct rtsx_chip *chip);
+int release_xd_card(struct rtsx_chip *chip);
+
+#endif  /* __REALTEK_RTSX_XD_H */
diff --git a/drivers/staging/sm750fb/Kconfig b/drivers/staging/sm750fb/Kconfig
new file mode 100644
index 000000000..8c0d8a873
--- /dev/null
+++ b/drivers/staging/sm750fb/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0
+config FB_SM750
+	tristate "Silicon Motion SM750 framebuffer support"
+	depends on FB && PCI
+	select FB_MODE_HELPERS
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  Frame buffer driver for the Silicon Motion SM750 chip
+	  with 2D accelearion and dual head support.
+
+	  This driver is also available as a module. The module will be
+	  called sm750fb. If you want to compile it as a module, say M
+	  here and read <file:Documentation/kbuild/modules.rst>.
diff --git a/drivers/staging/sm750fb/Makefile b/drivers/staging/sm750fb/Makefile
new file mode 100644
index 000000000..b89aa4c12
--- /dev/null
+++ b/drivers/staging/sm750fb/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_FB_SM750)	+= sm750fb.o
+
+sm750fb-objs		:= sm750.o sm750_hw.o sm750_accel.o sm750_cursor.o \
+			   ddk750_chip.o ddk750_power.o ddk750_mode.o \
+			   ddk750_display.o ddk750_swi2c.o ddk750_sii164.o \
+			   ddk750_dvi.o ddk750_hwi2c.o
diff --git a/drivers/staging/sm750fb/TODO b/drivers/staging/sm750fb/TODO
new file mode 100644
index 000000000..481409eb3
--- /dev/null
+++ b/drivers/staging/sm750fb/TODO
@@ -0,0 +1,19 @@
+TODO:
+- lots of checkpatch cleanup
+- use kernel coding style
+- refine the code and remove unused code
+- Implement hardware acceleration for imageblit if image->depth > 1
+- check on hardware effects of removal of USE_HW_I2C and USE_DVICHIP (these two
+	are supposed to be sample code which is given here if someone wants to
+	use those functionalities)
+- must be ported to the atomic kms framework in the drm subsystem (which will
+  give you a basic fbdev driver for free)
+
+Note:
+- This driver will be removed from staging after the drm driver is ready
+- The drm driver is getting ready at https://gitlab.com/sudipm/sm750/tree/sm750
+
+Please send any patches to
+	Greg Kroah-Hartman <greg@kroah.com>
+	Sudip Mukherjee <sudipm.mukherjee@gmail.com>
+	Teddy Wang <teddy.wang@siliconmotion.com>
diff --git a/drivers/staging/sm750fb/ddk750.h b/drivers/staging/sm750fb/ddk750.h
new file mode 100644
index 000000000..64ef4d258
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+ *
+ *  RegSC.h --- SM718 SDK
+ *  This file contains the definitions for the System Configuration registers.
+ */
+
+#ifndef DDK750_H__
+#define DDK750_H__
+
+#include "ddk750_reg.h"
+#include "ddk750_mode.h"
+#include "ddk750_chip.h"
+#include "ddk750_display.h"
+#include "ddk750_power.h"
+#ifdef USE_HW_I2C
+#include "ddk750_hwi2c.h"
+#endif
+#include "ddk750_swi2c.h"
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_chip.c b/drivers/staging/sm750fb/ddk750_chip.c
new file mode 100644
index 000000000..02860d3ec
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_chip.c
@@ -0,0 +1,407 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#include "ddk750_reg.h"
+#include "ddk750_chip.h"
+#include "ddk750_power.h"
+
+#define MHz(x) ((x) * 1000000)
+
+static enum logical_chip_type chip;
+
+enum logical_chip_type sm750_get_chip_type(void)
+{
+	return chip;
+}
+
+void sm750_set_chip_type(unsigned short dev_id, u8 rev_id)
+{
+	if (dev_id == 0x718) {
+		chip = SM718;
+	} else if (dev_id == 0x750) {
+		chip = SM750;
+		/* SM750 and SM750LE are different in their revision ID only. */
+		if (rev_id == SM750LE_REVISION_ID) {
+			chip = SM750LE;
+			pr_info("found sm750le\n");
+		}
+	} else {
+		chip = SM_UNKNOWN;
+	}
+}
+
+static unsigned int get_mxclk_freq(void)
+{
+	unsigned int pll_reg;
+	unsigned int M, N, OD, POD;
+
+	if (sm750_get_chip_type() == SM750LE)
+		return MHz(130);
+
+	pll_reg = peek32(MXCLK_PLL_CTRL);
+	M = (pll_reg & PLL_CTRL_M_MASK) >> PLL_CTRL_M_SHIFT;
+	N = (pll_reg & PLL_CTRL_N_MASK) >> PLL_CTRL_N_SHIFT;
+	OD = (pll_reg & PLL_CTRL_OD_MASK) >> PLL_CTRL_OD_SHIFT;
+	POD = (pll_reg & PLL_CTRL_POD_MASK) >> PLL_CTRL_POD_SHIFT;
+
+	return DEFAULT_INPUT_CLOCK * M / N / BIT(OD) / BIT(POD);
+}
+
+/*
+ * This function set up the main chip clock.
+ *
+ * Input: Frequency to be set.
+ */
+static void set_chip_clock(unsigned int frequency)
+{
+	struct pll_value pll;
+
+	/* Cheok_0509: For SM750LE, the chip clock is fixed. Nothing to set. */
+	if (sm750_get_chip_type() == SM750LE)
+		return;
+
+	if (frequency) {
+		/*
+		 * Set up PLL structure to hold the value to be set in clocks.
+		 */
+		pll.input_freq = DEFAULT_INPUT_CLOCK; /* Defined in CLOCK.H */
+		pll.clock_type = MXCLK_PLL;
+
+		/*
+		 * Call sm750_calc_pll_value() to fill the other fields
+		 * of the PLL structure. Sometimes, the chip cannot set
+		 * up the exact clock required by the User.
+		 * Return value of sm750_calc_pll_value gives the actual
+		 * possible clock.
+		 */
+		sm750_calc_pll_value(frequency, &pll);
+
+		/* Master Clock Control: MXCLK_PLL */
+		poke32(MXCLK_PLL_CTRL, sm750_format_pll_reg(&pll));
+	}
+}
+
+static void set_memory_clock(unsigned int frequency)
+{
+	unsigned int reg, divisor;
+
+	/*
+	 * Cheok_0509: For SM750LE, the memory clock is fixed.
+	 * Nothing to set.
+	 */
+	if (sm750_get_chip_type() == SM750LE)
+		return;
+
+	if (frequency) {
+		/*
+		 * Set the frequency to the maximum frequency
+		 * that the DDR Memory can take which is 336MHz.
+		 */
+		if (frequency > MHz(336))
+			frequency = MHz(336);
+
+		/* Calculate the divisor */
+		divisor = DIV_ROUND_CLOSEST(get_mxclk_freq(), frequency);
+
+		/* Set the corresponding divisor in the register. */
+		reg = peek32(CURRENT_GATE) & ~CURRENT_GATE_M2XCLK_MASK;
+		switch (divisor) {
+		default:
+		case 1:
+			reg |= CURRENT_GATE_M2XCLK_DIV_1;
+			break;
+		case 2:
+			reg |= CURRENT_GATE_M2XCLK_DIV_2;
+			break;
+		case 3:
+			reg |= CURRENT_GATE_M2XCLK_DIV_3;
+			break;
+		case 4:
+			reg |= CURRENT_GATE_M2XCLK_DIV_4;
+			break;
+		}
+
+		sm750_set_current_gate(reg);
+	}
+}
+
+/*
+ * This function set up the master clock (MCLK).
+ *
+ * Input: Frequency to be set.
+ *
+ * NOTE:
+ *      The maximum frequency the engine can run is 168MHz.
+ */
+static void set_master_clock(unsigned int frequency)
+{
+	unsigned int reg, divisor;
+
+	/*
+	 * Cheok_0509: For SM750LE, the memory clock is fixed.
+	 * Nothing to set.
+	 */
+	if (sm750_get_chip_type() == SM750LE)
+		return;
+
+	if (frequency) {
+		/*
+		 * Set the frequency to the maximum frequency
+		 * that the SM750 engine can run, which is about 190 MHz.
+		 */
+		if (frequency > MHz(190))
+			frequency = MHz(190);
+
+		/* Calculate the divisor */
+		divisor = DIV_ROUND_CLOSEST(get_mxclk_freq(), frequency);
+
+		/* Set the corresponding divisor in the register. */
+		reg = peek32(CURRENT_GATE) & ~CURRENT_GATE_MCLK_MASK;
+		switch (divisor) {
+		default:
+		case 3:
+			reg |= CURRENT_GATE_MCLK_DIV_3;
+			break;
+		case 4:
+			reg |= CURRENT_GATE_MCLK_DIV_4;
+			break;
+		case 6:
+			reg |= CURRENT_GATE_MCLK_DIV_6;
+			break;
+		case 8:
+			reg |= CURRENT_GATE_MCLK_DIV_8;
+			break;
+		}
+
+		sm750_set_current_gate(reg);
+	}
+}
+
+unsigned int ddk750_get_vm_size(void)
+{
+	unsigned int reg;
+	unsigned int data;
+
+	/* sm750le only use 64 mb memory*/
+	if (sm750_get_chip_type() == SM750LE)
+		return SZ_64M;
+
+	/* for 750,always use power mode0*/
+	reg = peek32(MODE0_GATE);
+	reg |= MODE0_GATE_GPIO;
+	poke32(MODE0_GATE, reg);
+
+	/* get frame buffer size from GPIO */
+	reg = peek32(MISC_CTRL) & MISC_CTRL_LOCALMEM_SIZE_MASK;
+	switch (reg) {
+	case MISC_CTRL_LOCALMEM_SIZE_8M:
+		data = SZ_8M;  break; /* 8  Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_16M:
+		data = SZ_16M; break; /* 16 Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_32M:
+		data = SZ_32M; break; /* 32 Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_64M:
+		data = SZ_64M; break; /* 64 Mega byte */
+	default:
+		data = 0;
+		break;
+	}
+	return data;
+}
+
+int ddk750_init_hw(struct initchip_param *p_init_param)
+{
+	unsigned int reg;
+
+	if (p_init_param->power_mode != 0)
+		p_init_param->power_mode = 0;
+	sm750_set_power_mode(p_init_param->power_mode);
+
+	/* Enable display power gate & LOCALMEM power gate*/
+	reg = peek32(CURRENT_GATE);
+	reg |= (CURRENT_GATE_DISPLAY | CURRENT_GATE_LOCALMEM);
+	sm750_set_current_gate(reg);
+
+	if (sm750_get_chip_type() != SM750LE) {
+		/* set panel pll and graphic mode via mmio_88 */
+		reg = peek32(VGA_CONFIGURATION);
+		reg |= (VGA_CONFIGURATION_PLL | VGA_CONFIGURATION_MODE);
+		poke32(VGA_CONFIGURATION, reg);
+	} else {
+#if defined(__i386__) || defined(__x86_64__)
+		/* set graphic mode via IO method */
+		outb_p(0x88, 0x3d4);
+		outb_p(0x06, 0x3d5);
+#endif
+	}
+
+	/* Set the Main Chip Clock */
+	set_chip_clock(MHz((unsigned int)p_init_param->chip_clock));
+
+	/* Set up memory clock. */
+	set_memory_clock(MHz(p_init_param->mem_clock));
+
+	/* Set up master clock */
+	set_master_clock(MHz(p_init_param->master_clock));
+
+	/*
+	 * Reset the memory controller.
+	 * If the memory controller is not reset in SM750,
+	 * the system might hang when sw accesses the memory.
+	 * The memory should be resetted after changing the MXCLK.
+	 */
+	if (p_init_param->reset_memory == 1) {
+		reg = peek32(MISC_CTRL);
+		reg &= ~MISC_CTRL_LOCALMEM_RESET;
+		poke32(MISC_CTRL, reg);
+
+		reg |= MISC_CTRL_LOCALMEM_RESET;
+		poke32(MISC_CTRL, reg);
+	}
+
+	if (p_init_param->set_all_eng_off == 1) {
+		sm750_enable_2d_engine(0);
+
+		/* Disable Overlay, if a former application left it on */
+		reg = peek32(VIDEO_DISPLAY_CTRL);
+		reg &= ~DISPLAY_CTRL_PLANE;
+		poke32(VIDEO_DISPLAY_CTRL, reg);
+
+		/* Disable video alpha, if a former application left it on */
+		reg = peek32(VIDEO_ALPHA_DISPLAY_CTRL);
+		reg &= ~DISPLAY_CTRL_PLANE;
+		poke32(VIDEO_ALPHA_DISPLAY_CTRL, reg);
+
+		/* Disable alpha plane, if a former application left it on */
+		reg = peek32(ALPHA_DISPLAY_CTRL);
+		reg &= ~DISPLAY_CTRL_PLANE;
+		poke32(ALPHA_DISPLAY_CTRL, reg);
+
+		/* Disable DMA Channel, if a former application left it on */
+		reg = peek32(DMA_ABORT_INTERRUPT);
+		reg |= DMA_ABORT_INTERRUPT_ABORT_1;
+		poke32(DMA_ABORT_INTERRUPT, reg);
+
+		/* Disable DMA Power, if a former application left it on */
+		sm750_enable_dma(0);
+	}
+
+	/* We can add more initialization as needed. */
+
+	return 0;
+}
+
+/*
+ * monk liu @ 4/6/2011:
+ *	re-write the calculatePLL function of ddk750.
+ *	the original version function does not use
+ *	some mathematics tricks and shortcut
+ *	when it doing the calculation of the best N,M,D combination
+ *	I think this version gives a little upgrade in speed
+ *
+ * 750 pll clock formular:
+ * Request Clock = (Input Clock * M )/(N * X)
+ *
+ * Input Clock = 14318181 hz
+ * X = 2 power D
+ * D ={0,1,2,3,4,5,6}
+ * M = {1,...,255}
+ * N = {2,...,15}
+ */
+unsigned int sm750_calc_pll_value(unsigned int request_orig,
+				  struct pll_value *pll)
+{
+	/*
+	 * as sm750 register definition,
+	 * N located in 2,15 and M located in 1,255
+	 */
+	int N, M, X, d;
+	int mini_diff;
+	unsigned int RN, quo, rem, fl_quo;
+	unsigned int input, request;
+	unsigned int tmp_clock, ret;
+	const int max_OD = 3;
+	int max_d = 6;
+
+	if (sm750_get_chip_type() == SM750LE) {
+		/*
+		 * SM750LE don't have
+		 * programmable PLL and M/N values to work on.
+		 * Just return the requested clock.
+		 */
+		return request_orig;
+	}
+
+	ret = 0;
+	mini_diff = ~0;
+	request = request_orig / 1000;
+	input = pll->input_freq / 1000;
+
+	/*
+	 * for MXCLK register,
+	 * no POD provided, so need be treated differently
+	 */
+	if (pll->clock_type == MXCLK_PLL)
+		max_d = 3;
+
+	for (N = 15; N > 1; N--) {
+		/*
+		 * RN will not exceed maximum long
+		 * if @request <= 285 MHZ (for 32bit cpu)
+		 */
+		RN = N * request;
+		quo = RN / input;
+		rem = RN % input;/* rem always small than 14318181 */
+		fl_quo = rem * 10000 / input;
+
+		for (d = max_d; d >= 0; d--) {
+			X = BIT(d);
+			M = quo * X;
+			M += fl_quo * X / 10000;
+			/* round step */
+			M += (fl_quo * X % 10000) > 5000 ? 1 : 0;
+			if (M < 256 && M > 0) {
+				unsigned int diff;
+
+				tmp_clock = pll->input_freq * M / N / X;
+				diff = abs(tmp_clock - request_orig);
+				if (diff < mini_diff) {
+					pll->M = M;
+					pll->N = N;
+					pll->POD = 0;
+					if (d > max_OD)
+						pll->POD = d - max_OD;
+					pll->OD = d - pll->POD;
+					mini_diff = diff;
+					ret = tmp_clock;
+				}
+			}
+		}
+	}
+	return ret;
+}
+
+unsigned int sm750_format_pll_reg(struct pll_value *p_PLL)
+{
+#ifndef VALIDATION_CHIP
+	unsigned int POD = p_PLL->POD;
+#endif
+	unsigned int OD = p_PLL->OD;
+	unsigned int M = p_PLL->M;
+	unsigned int N = p_PLL->N;
+
+	/*
+	 * Note that all PLL's have the same format. Here, we just use
+	 * Panel PLL parameter to work out the bit fields in the
+	 * register. On returning a 32 bit number, the value can be
+	 * applied to any PLL in the calling function.
+	 */
+	return PLL_CTRL_POWER |
+#ifndef VALIDATION_CHIP
+		((POD << PLL_CTRL_POD_SHIFT) & PLL_CTRL_POD_MASK) |
+#endif
+		((OD << PLL_CTRL_OD_SHIFT) & PLL_CTRL_OD_MASK) |
+		((N << PLL_CTRL_N_SHIFT) & PLL_CTRL_N_MASK) |
+		((M << PLL_CTRL_M_SHIFT) & PLL_CTRL_M_MASK);
+}
diff --git a/drivers/staging/sm750fb/ddk750_chip.h b/drivers/staging/sm750fb/ddk750_chip.h
new file mode 100644
index 000000000..ee2e9d90f
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_chip.h
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_CHIP_H__
+#define DDK750_CHIP_H__
+#define DEFAULT_INPUT_CLOCK 14318181 /* Default reference clock */
+#ifndef SM750LE_REVISION_ID
+#define SM750LE_REVISION_ID ((unsigned char)0xfe)
+#endif
+
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/uaccess.h>
+
+extern void __iomem *mmio750;
+
+/* software control endianness */
+static inline u32 peek32(u32 addr)
+{
+	return readl(addr + mmio750);
+}
+
+static inline void poke32(u32 addr, u32 data)
+{
+	writel(data, addr + mmio750);
+}
+
+/* This is all the chips recognized by this library */
+enum logical_chip_type {
+	SM_UNKNOWN,
+	SM718,
+	SM750,
+	SM750LE,
+};
+
+enum clock_type {
+	MXCLK_PLL,
+	PRIMARY_PLL,
+	SECONDARY_PLL,
+	VGA0_PLL,
+	VGA1_PLL,
+};
+
+struct pll_value {
+	enum clock_type clock_type;
+	unsigned long input_freq; /* Input clock frequency to the PLL */
+
+	/* Use this when clockType = PANEL_PLL */
+	unsigned long M;
+	unsigned long N;
+	unsigned long OD;
+	unsigned long POD;
+};
+
+/* input struct to initChipParam() function */
+struct initchip_param {
+	/* Use power mode 0 or 1 */
+	unsigned short power_mode;
+
+	/*
+	 * Speed of main chip clock in MHz unit
+	 * 0 = keep the current clock setting
+	 * Others = the new main chip clock
+	 */
+	unsigned short chip_clock;
+
+	/*
+	 * Speed of memory clock in MHz unit
+	 * 0 = keep the current clock setting
+	 * Others = the new memory clock
+	 */
+	unsigned short mem_clock;
+
+	/*
+	 * Speed of master clock in MHz unit
+	 * 0 = keep the current clock setting
+	 * Others = the new master clock
+	 */
+	unsigned short master_clock;
+
+	/*
+	 * 0 = leave all engine state untouched.
+	 * 1 = make sure they are off: 2D, Overlay,
+	 * video alpha, alpha, hardware cursors
+	 */
+	unsigned short set_all_eng_off;
+
+	/*
+	 * 0 = Do not reset the memory controller
+	 * 1 = Reset the memory controller
+	 */
+	unsigned char reset_memory;
+
+	/* More initialization parameter can be added if needed */
+};
+
+enum logical_chip_type sm750_get_chip_type(void);
+void sm750_set_chip_type(unsigned short dev_id, u8 rev_id);
+unsigned int sm750_calc_pll_value(unsigned int request, struct  pll_value *pll);
+unsigned int sm750_format_pll_reg(struct pll_value *p_PLL);
+unsigned int ddk750_get_vm_size(void);
+int ddk750_init_hw(struct initchip_param *pinit_param);
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_display.c b/drivers/staging/sm750fb/ddk750_display.c
new file mode 100644
index 000000000..172624ff9
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_display.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "ddk750_reg.h"
+#include "ddk750_chip.h"
+#include "ddk750_display.h"
+#include "ddk750_power.h"
+#include "ddk750_dvi.h"
+
+static void set_display_control(int ctrl, int disp_state)
+{
+	/* state != 0 means turn on both timing & plane en_bit */
+	unsigned long reg, val, reserved;
+	int cnt = 0;
+
+	if (!ctrl) {
+		reg = PANEL_DISPLAY_CTRL;
+		reserved = PANEL_DISPLAY_CTRL_RESERVED_MASK;
+	} else {
+		reg = CRT_DISPLAY_CTRL;
+		reserved = CRT_DISPLAY_CTRL_RESERVED_MASK;
+	}
+
+	val = peek32(reg);
+	if (disp_state) {
+		/*
+		 * Timing should be enabled first before enabling the
+		 * plane because changing at the same time does not
+		 * guarantee that the plane will also enabled or
+		 * disabled.
+		 */
+		val |= DISPLAY_CTRL_TIMING;
+		poke32(reg, val);
+
+		val |= DISPLAY_CTRL_PLANE;
+
+		/*
+		 * Somehow the register value on the plane is not set
+		 * until a few delay. Need to write and read it a
+		 * couple times
+		 */
+		do {
+			cnt++;
+			poke32(reg, val);
+		} while ((peek32(reg) & ~reserved) != (val & ~reserved));
+		pr_debug("Set Plane enbit:after tried %d times\n", cnt);
+	} else {
+		/*
+		 * When turning off, there is no rule on the
+		 * programming sequence since whenever the clock is
+		 * off, then it does not matter whether the plane is
+		 * enabled or disabled.  Note: Modifying the plane bit
+		 * will take effect on the next vertical sync. Need to
+		 * find out if it is necessary to wait for 1 vsync
+		 * before modifying the timing enable bit.
+		 */
+		val &= ~DISPLAY_CTRL_PLANE;
+		poke32(reg, val);
+
+		val &= ~DISPLAY_CTRL_TIMING;
+		poke32(reg, val);
+	}
+}
+
+static void primary_wait_vertical_sync(int delay)
+{
+	unsigned int status;
+
+	/*
+	 * Do not wait when the Primary PLL is off or display control is
+	 * already off. This will prevent the software to wait forever.
+	 */
+	if (!(peek32(PANEL_PLL_CTRL) & PLL_CTRL_POWER) ||
+	    !(peek32(PANEL_DISPLAY_CTRL) & DISPLAY_CTRL_TIMING))
+		return;
+
+	while (delay-- > 0) {
+		/* Wait for end of vsync. */
+		do {
+			status = peek32(SYSTEM_CTRL);
+		} while (status & SYSTEM_CTRL_PANEL_VSYNC_ACTIVE);
+
+		/* Wait for start of vsync. */
+		do {
+			status = peek32(SYSTEM_CTRL);
+		} while (!(status & SYSTEM_CTRL_PANEL_VSYNC_ACTIVE));
+	}
+}
+
+static void sw_panel_power_sequence(int disp, int delay)
+{
+	unsigned int reg;
+
+	/* disp should be 1 to open sequence */
+	reg = peek32(PANEL_DISPLAY_CTRL);
+	reg |= (disp ? PANEL_DISPLAY_CTRL_FPEN : 0);
+	poke32(PANEL_DISPLAY_CTRL, reg);
+	primary_wait_vertical_sync(delay);
+
+	reg = peek32(PANEL_DISPLAY_CTRL);
+	reg |= (disp ? PANEL_DISPLAY_CTRL_DATA : 0);
+	poke32(PANEL_DISPLAY_CTRL, reg);
+	primary_wait_vertical_sync(delay);
+
+	reg = peek32(PANEL_DISPLAY_CTRL);
+	reg |= (disp ? PANEL_DISPLAY_CTRL_VBIASEN : 0);
+	poke32(PANEL_DISPLAY_CTRL, reg);
+	primary_wait_vertical_sync(delay);
+
+	reg = peek32(PANEL_DISPLAY_CTRL);
+	reg |= (disp ? PANEL_DISPLAY_CTRL_FPEN : 0);
+	poke32(PANEL_DISPLAY_CTRL, reg);
+	primary_wait_vertical_sync(delay);
+}
+
+void ddk750_set_logical_disp_out(enum disp_output output)
+{
+	unsigned int reg;
+
+	if (output & PNL_2_USAGE) {
+		/* set panel path controller select */
+		reg = peek32(PANEL_DISPLAY_CTRL);
+		reg &= ~PANEL_DISPLAY_CTRL_SELECT_MASK;
+		reg |= (((output & PNL_2_MASK) >> PNL_2_OFFSET) <<
+			PANEL_DISPLAY_CTRL_SELECT_SHIFT);
+		poke32(PANEL_DISPLAY_CTRL, reg);
+	}
+
+	if (output & CRT_2_USAGE) {
+		/* set crt path controller select */
+		reg = peek32(CRT_DISPLAY_CTRL);
+		reg &= ~CRT_DISPLAY_CTRL_SELECT_MASK;
+		reg |= (((output & CRT_2_MASK) >> CRT_2_OFFSET) <<
+			CRT_DISPLAY_CTRL_SELECT_SHIFT);
+		/*se blank off */
+		reg &= ~CRT_DISPLAY_CTRL_BLANK;
+		poke32(CRT_DISPLAY_CTRL, reg);
+	}
+
+	if (output & PRI_TP_USAGE) {
+		/* set primary timing and plane en_bit */
+		set_display_control(0, (output & PRI_TP_MASK) >> PRI_TP_OFFSET);
+	}
+
+	if (output & SEC_TP_USAGE) {
+		/* set secondary timing and plane en_bit*/
+		set_display_control(1, (output & SEC_TP_MASK) >> SEC_TP_OFFSET);
+	}
+
+	if (output & PNL_SEQ_USAGE) {
+		/* set  panel sequence */
+		sw_panel_power_sequence((output & PNL_SEQ_MASK) >>
+					PNL_SEQ_OFFSET, 4);
+	}
+
+	if (output & DAC_USAGE)
+		set_DAC((output & DAC_MASK) >> DAC_OFFSET);
+
+	if (output & DPMS_USAGE)
+		ddk750_set_dpms((output & DPMS_MASK) >> DPMS_OFFSET);
+}
diff --git a/drivers/staging/sm750fb/ddk750_display.h b/drivers/staging/sm750fb/ddk750_display.h
new file mode 100644
index 000000000..7f713906d
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_display.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_DISPLAY_H__
+#define DDK750_DISPLAY_H__
+
+/*
+ * panel path select
+ *	80000[29:28]
+ */
+
+#define PNL_2_OFFSET 0
+#define PNL_2_MASK (3 << PNL_2_OFFSET)
+#define PNL_2_USAGE	(PNL_2_MASK << 16)
+#define PNL_2_PRI	((0 << PNL_2_OFFSET) | PNL_2_USAGE)
+#define PNL_2_SEC	((2 << PNL_2_OFFSET) | PNL_2_USAGE)
+
+/*
+ * primary timing & plane enable bit
+ *	1: 80000[8] & 80000[2] on
+ *	0: both off
+ */
+#define PRI_TP_OFFSET 4
+#define PRI_TP_MASK BIT(PRI_TP_OFFSET)
+#define PRI_TP_USAGE (PRI_TP_MASK << 16)
+#define PRI_TP_ON ((0x1 << PRI_TP_OFFSET) | PRI_TP_USAGE)
+#define PRI_TP_OFF ((0x0 << PRI_TP_OFFSET) | PRI_TP_USAGE)
+
+/*
+ * panel sequency status
+ *	80000[27:24]
+ */
+#define PNL_SEQ_OFFSET 6
+#define PNL_SEQ_MASK BIT(PNL_SEQ_OFFSET)
+#define PNL_SEQ_USAGE (PNL_SEQ_MASK << 16)
+#define PNL_SEQ_ON (BIT(PNL_SEQ_OFFSET) | PNL_SEQ_USAGE)
+#define PNL_SEQ_OFF ((0 << PNL_SEQ_OFFSET) | PNL_SEQ_USAGE)
+
+/*
+ * dual digital output
+ *	80000[19]
+ */
+#define DUAL_TFT_OFFSET 8
+#define DUAL_TFT_MASK BIT(DUAL_TFT_OFFSET)
+#define DUAL_TFT_USAGE (DUAL_TFT_MASK << 16)
+#define DUAL_TFT_ON (BIT(DUAL_TFT_OFFSET) | DUAL_TFT_USAGE)
+#define DUAL_TFT_OFF ((0 << DUAL_TFT_OFFSET) | DUAL_TFT_USAGE)
+
+/*
+ * secondary timing & plane enable bit
+ *	1:80200[8] & 80200[2] on
+ *	0: both off
+ */
+#define SEC_TP_OFFSET 5
+#define SEC_TP_MASK BIT(SEC_TP_OFFSET)
+#define SEC_TP_USAGE (SEC_TP_MASK << 16)
+#define SEC_TP_ON  ((0x1 << SEC_TP_OFFSET) | SEC_TP_USAGE)
+#define SEC_TP_OFF ((0x0 << SEC_TP_OFFSET) | SEC_TP_USAGE)
+
+/*
+ * crt path select
+ *	80200[19:18]
+ */
+#define CRT_2_OFFSET 2
+#define CRT_2_MASK (3 << CRT_2_OFFSET)
+#define CRT_2_USAGE (CRT_2_MASK << 16)
+#define CRT_2_PRI ((0x0 << CRT_2_OFFSET) | CRT_2_USAGE)
+#define CRT_2_SEC ((0x2 << CRT_2_OFFSET) | CRT_2_USAGE)
+
+/*
+ * DAC affect both DVI and DSUB
+ *	4[20]
+ */
+#define DAC_OFFSET 7
+#define DAC_MASK BIT(DAC_OFFSET)
+#define DAC_USAGE (DAC_MASK << 16)
+#define DAC_ON ((0x0 << DAC_OFFSET) | DAC_USAGE)
+#define DAC_OFF ((0x1 << DAC_OFFSET) | DAC_USAGE)
+
+/*
+ * DPMS only affect D-SUB head
+ *	0[31:30]
+ */
+#define DPMS_OFFSET 9
+#define DPMS_MASK (3 << DPMS_OFFSET)
+#define DPMS_USAGE (DPMS_MASK << 16)
+#define DPMS_OFF ((3 << DPMS_OFFSET) | DPMS_USAGE)
+#define DPMS_ON ((0 << DPMS_OFFSET) | DPMS_USAGE)
+
+/*
+ * LCD1 means panel path TFT1  & panel path DVI (so enable DAC)
+ * CRT means crt path DSUB
+ */
+enum disp_output {
+	do_LCD1_PRI = PNL_2_PRI | PRI_TP_ON | PNL_SEQ_ON | DAC_ON,
+	do_LCD1_SEC = PNL_2_SEC | SEC_TP_ON | PNL_SEQ_ON | DAC_ON,
+	do_LCD2_PRI = CRT_2_PRI | PRI_TP_ON | DUAL_TFT_ON,
+	do_LCD2_SEC = CRT_2_SEC | SEC_TP_ON | DUAL_TFT_ON,
+	/*
+	 * do_DSUB_PRI = CRT_2_PRI | PRI_TP_ON | DPMS_ON|DAC_ON,
+	 * do_DSUB_SEC = CRT_2_SEC | SEC_TP_ON | DPMS_ON|DAC_ON,
+	 */
+	do_CRT_PRI = CRT_2_PRI | PRI_TP_ON | DPMS_ON | DAC_ON,
+	do_CRT_SEC = CRT_2_SEC | SEC_TP_ON | DPMS_ON | DAC_ON,
+};
+
+void ddk750_set_logical_disp_out(enum disp_output output);
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_dvi.c b/drivers/staging/sm750fb/ddk750_dvi.c
new file mode 100644
index 000000000..e0c7ff335
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_dvi.c
@@ -0,0 +1,62 @@
+// SPDX-License-Identifier: GPL-2.0
+#define USE_DVICHIP
+#ifdef USE_DVICHIP
+#include "ddk750_chip.h"
+#include "ddk750_reg.h"
+#include "ddk750_dvi.h"
+#include "ddk750_sii164.h"
+
+/*
+ * This global variable contains all the supported driver and its corresponding
+ * function API. Please set the function pointer to NULL whenever the function
+ * is not supported.
+ */
+static struct dvi_ctrl_device dcft_supported_dvi_controller[] = {
+#ifdef DVI_CTRL_SII164
+	{
+		.init = sii164InitChip,
+		.get_vendor_id = sii164_get_vendor_id,
+		.get_device_id = sii164GetDeviceID,
+#ifdef SII164_FULL_FUNCTIONS
+		.reset_chip = sii164ResetChip,
+		.get_chip_string = sii164GetChipString,
+		.set_power = sii164SetPower,
+		.enable_hot_plug_detection = sii164EnableHotPlugDetection,
+		.is_connected = sii164IsConnected,
+		.check_interrupt = sii164CheckInterrupt,
+		.clear_interrupt = sii164ClearInterrupt,
+#endif
+	},
+#endif
+};
+
+int dvi_init(unsigned char edge_select,
+	     unsigned char bus_select,
+	     unsigned char dual_edge_clk_select,
+	     unsigned char hsync_enable,
+	     unsigned char vsync_enable,
+	     unsigned char deskew_enable,
+	     unsigned char deskew_setting,
+	     unsigned char continuous_sync_enable,
+	     unsigned char pll_filter_enable,
+	     unsigned char pll_filter_value)
+{
+	struct dvi_ctrl_device *current_dvi_ctrl;
+
+	current_dvi_ctrl = dcft_supported_dvi_controller;
+	if (current_dvi_ctrl->init) {
+		return current_dvi_ctrl->init(edge_select,
+					      bus_select,
+					      dual_edge_clk_select,
+					      hsync_enable,
+					      vsync_enable,
+					      deskew_enable,
+					      deskew_setting,
+					      continuous_sync_enable,
+					      pll_filter_enable,
+					      pll_filter_value);
+	}
+	return -1; /* error */
+}
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_dvi.h b/drivers/staging/sm750fb/ddk750_dvi.h
new file mode 100644
index 000000000..c2518b73b
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_dvi.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_DVI_H__
+#define DDK750_DVI_H__
+
+/* dvi chip stuffs structros */
+
+typedef long (*PFN_DVICTRL_INIT)(unsigned char edge_select,
+				 unsigned char bus_select,
+				 unsigned char dual_edge_clk_select,
+				 unsigned char hsync_enable,
+				 unsigned char vsync_enable,
+				 unsigned char deskew_enable,
+				 unsigned char deskew_setting,
+				 unsigned char continuous_sync_enable,
+				 unsigned char pll_filter_enable,
+				 unsigned char pll_filter_value);
+
+typedef void (*PFN_DVICTRL_RESETCHIP)(void);
+typedef char* (*PFN_DVICTRL_GETCHIPSTRING)(void);
+typedef unsigned short (*PFN_DVICTRL_GETVENDORID)(void);
+typedef unsigned short (*PFN_DVICTRL_GETDEVICEID)(void);
+typedef void (*PFN_DVICTRL_SETPOWER)(unsigned char power_up);
+typedef void (*PFN_DVICTRL_HOTPLUGDETECTION)(unsigned char enable_hot_plug);
+typedef unsigned char (*PFN_DVICTRL_ISCONNECTED)(void);
+typedef unsigned char (*PFN_DVICTRL_CHECKINTERRUPT)(void);
+typedef void (*PFN_DVICTRL_CLEARINTERRUPT)(void);
+
+/* Structure to hold all the function pointer to the DVI Controller. */
+struct dvi_ctrl_device {
+	PFN_DVICTRL_INIT		init;
+	PFN_DVICTRL_RESETCHIP		reset_chip;
+	PFN_DVICTRL_GETCHIPSTRING	get_chip_string;
+	PFN_DVICTRL_GETVENDORID		get_vendor_id;
+	PFN_DVICTRL_GETDEVICEID		get_device_id;
+	PFN_DVICTRL_SETPOWER		set_power;
+	PFN_DVICTRL_HOTPLUGDETECTION	enable_hot_plug_detection;
+	PFN_DVICTRL_ISCONNECTED		is_connected;
+	PFN_DVICTRL_CHECKINTERRUPT	check_interrupt;
+	PFN_DVICTRL_CLEARINTERRUPT	clear_interrupt;
+};
+
+#define DVI_CTRL_SII164
+
+/* dvi functions prototype */
+int dvi_init(unsigned char edge_select,
+	     unsigned char bus_select,
+	     unsigned char dual_edge_clk_select,
+	     unsigned char hsync_enable,
+	     unsigned char vsync_enable,
+	     unsigned char deskew_enable,
+	     unsigned char deskew_setting,
+	     unsigned char continuous_sync_enable,
+	     unsigned char pll_filter_enable,
+	     unsigned char pll_filter_value);
+
+#endif
+
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.c b/drivers/staging/sm750fb/ddk750_hwi2c.c
new file mode 100644
index 000000000..8482689b6
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0
+#define USE_HW_I2C
+#ifdef USE_HW_I2C
+#include "ddk750_chip.h"
+#include "ddk750_reg.h"
+#include "ddk750_hwi2c.h"
+#include "ddk750_power.h"
+
+#define MAX_HWI2C_FIFO                  16
+#define HWI2C_WAIT_TIMEOUT              0xF0000
+
+int sm750_hw_i2c_init(unsigned char bus_speed_mode)
+{
+	unsigned int value;
+
+	/* Enable GPIO 30 & 31 as IIC clock & data */
+	value = peek32(GPIO_MUX);
+
+	value |= (GPIO_MUX_30 | GPIO_MUX_31);
+	poke32(GPIO_MUX, value);
+
+	/*
+	 * Enable Hardware I2C power.
+	 * TODO: Check if we need to enable GPIO power?
+	 */
+	sm750_enable_i2c(1);
+
+	/* Enable the I2C Controller and set the bus speed mode */
+	value = peek32(I2C_CTRL) & ~(I2C_CTRL_MODE | I2C_CTRL_EN);
+	if (bus_speed_mode)
+		value |= I2C_CTRL_MODE;
+	value |= I2C_CTRL_EN;
+	poke32(I2C_CTRL, value);
+
+	return 0;
+}
+
+void sm750_hw_i2c_close(void)
+{
+	unsigned int value;
+
+	/* Disable I2C controller */
+	value = peek32(I2C_CTRL) & ~I2C_CTRL_EN;
+	poke32(I2C_CTRL, value);
+
+	/* Disable I2C Power */
+	sm750_enable_i2c(0);
+
+	/* Set GPIO 30 & 31 back as GPIO pins */
+	value = peek32(GPIO_MUX);
+	value &= ~GPIO_MUX_30;
+	value &= ~GPIO_MUX_31;
+	poke32(GPIO_MUX, value);
+}
+
+static long hw_i2c_wait_tx_done(void)
+{
+	unsigned int timeout;
+
+	/* Wait until the transfer is completed. */
+	timeout = HWI2C_WAIT_TIMEOUT;
+	while (!(peek32(I2C_STATUS) & I2C_STATUS_TX) && (timeout != 0))
+		timeout--;
+
+	if (timeout == 0)
+		return -1;
+
+	return 0;
+}
+
+/*
+ *  This function writes data to the i2c slave device registers.
+ *
+ *  Parameters:
+ *      addr            - i2c Slave device address
+ *      length          - Total number of bytes to be written to the device
+ *      buf             - The buffer that contains the data to be written to the
+ *                     i2c device.
+ *
+ *  Return Value:
+ *      Total number of bytes those are actually written.
+ */
+static unsigned int hw_i2c_write_data(unsigned char addr,
+				      unsigned int length,
+				      unsigned char *buf)
+{
+	unsigned char count, i;
+	unsigned int total_bytes = 0;
+
+	/* Set the Device Address */
+	poke32(I2C_SLAVE_ADDRESS, addr & ~0x01);
+
+	/*
+	 * Write data.
+	 * Note:
+	 *      Only 16 byte can be accessed per i2c start instruction.
+	 */
+	do {
+		/*
+		 * Reset I2C by writing 0 to I2C_RESET register to
+		 * clear the previous status.
+		 */
+		poke32(I2C_RESET, 0);
+
+		/* Set the number of bytes to be written */
+		if (length < MAX_HWI2C_FIFO)
+			count = length - 1;
+		else
+			count = MAX_HWI2C_FIFO - 1;
+		poke32(I2C_BYTE_COUNT, count);
+
+		/* Move the data to the I2C data register */
+		for (i = 0; i <= count; i++)
+			poke32(I2C_DATA0 + i, *buf++);
+
+		/* Start the I2C */
+		poke32(I2C_CTRL, peek32(I2C_CTRL) | I2C_CTRL_CTRL);
+
+		/* Wait until the transfer is completed. */
+		if (hw_i2c_wait_tx_done() != 0)
+			break;
+
+		/* Subtract length */
+		length -= (count + 1);
+
+		/* Total byte written */
+		total_bytes += (count + 1);
+
+	} while (length > 0);
+
+	return total_bytes;
+}
+
+/*
+ *  This function reads data from the slave device and stores them
+ *  in the given buffer
+ *
+ *  Parameters:
+ *      addr            - i2c Slave device address
+ *      length          - Total number of bytes to be read
+ *      buf             - Pointer to a buffer to be filled with the data read
+ *                     from the slave device. It has to be the same size as the
+ *                     length to make sure that it can keep all the data read.
+ *
+ *  Return Value:
+ *      Total number of actual bytes read from the slave device
+ */
+static unsigned int hw_i2c_read_data(unsigned char addr,
+				     unsigned int length,
+				     unsigned char *buf)
+{
+	unsigned char count, i;
+	unsigned int total_bytes = 0;
+
+	/* Set the Device Address */
+	poke32(I2C_SLAVE_ADDRESS, addr | 0x01);
+
+	/*
+	 * Read data and save them to the buffer.
+	 * Note:
+	 *      Only 16 byte can be accessed per i2c start instruction.
+	 */
+	do {
+		/*
+		 * Reset I2C by writing 0 to I2C_RESET register to
+		 * clear all the status.
+		 */
+		poke32(I2C_RESET, 0);
+
+		/* Set the number of bytes to be read */
+		if (length <= MAX_HWI2C_FIFO)
+			count = length - 1;
+		else
+			count = MAX_HWI2C_FIFO - 1;
+		poke32(I2C_BYTE_COUNT, count);
+
+		/* Start the I2C */
+		poke32(I2C_CTRL, peek32(I2C_CTRL) | I2C_CTRL_CTRL);
+
+		/* Wait until transaction done. */
+		if (hw_i2c_wait_tx_done() != 0)
+			break;
+
+		/* Save the data to the given buffer */
+		for (i = 0; i <= count; i++)
+			*buf++ = peek32(I2C_DATA0 + i);
+
+		/* Subtract length by 16 */
+		length -= (count + 1);
+
+		/* Number of bytes read. */
+		total_bytes += (count + 1);
+
+	} while (length > 0);
+
+	return total_bytes;
+}
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be read from
+ *      registerIndex   - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char sm750_hw_i2c_read_reg(unsigned char addr, unsigned char reg)
+{
+	unsigned char value = 0xFF;
+
+	if (hw_i2c_write_data(addr, 1, &reg) == 1)
+		hw_i2c_read_data(addr, 1, &value);
+
+	return value;
+}
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be written
+ *      registerIndex   - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+int sm750_hw_i2c_write_reg(unsigned char addr,
+			   unsigned char reg,
+			   unsigned char data)
+{
+	unsigned char value[2];
+
+	value[0] = reg;
+	value[1] = data;
+	if (hw_i2c_write_data(addr, 2, value) == 2)
+		return 0;
+
+	return -1;
+}
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.h b/drivers/staging/sm750fb/ddk750_hwi2c.h
new file mode 100644
index 000000000..337c6493c
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_HWI2C_H__
+#define DDK750_HWI2C_H__
+
+/* hwi2c functions */
+int sm750_hw_i2c_init(unsigned char bus_speed_mode);
+void sm750_hw_i2c_close(void);
+
+unsigned char sm750_hw_i2c_read_reg(unsigned char addr, unsigned char reg);
+int sm750_hw_i2c_write_reg(unsigned char addr, unsigned char reg,
+			   unsigned char data);
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_mode.c b/drivers/staging/sm750fb/ddk750_mode.c
new file mode 100644
index 000000000..e00a6cb31
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_mode.c
@@ -0,0 +1,225 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include "ddk750_reg.h"
+#include "ddk750_mode.h"
+#include "ddk750_chip.h"
+
+/*
+ * SM750LE only:
+ * This function takes care extra registers and bit fields required to set
+ * up a mode in SM750LE
+ *
+ * Explanation about Display Control register:
+ * HW only supports 7 predefined pixel clocks, and clock select is
+ * in bit 29:27 of Display Control register.
+ */
+static unsigned long
+displayControlAdjust_SM750LE(struct mode_parameter *pModeParam,
+			     unsigned long dispControl)
+{
+	unsigned long x, y;
+
+	x = pModeParam->horizontal_display_end;
+	y = pModeParam->vertical_display_end;
+
+	/*
+	 * SM750LE has to set up the top-left and bottom-right
+	 * registers as well.
+	 * Note that normal SM750/SM718 only use those two register for
+	 * auto-centering mode.
+	 */
+	poke32(CRT_AUTO_CENTERING_TL, 0);
+
+	poke32(CRT_AUTO_CENTERING_BR,
+	       (((y - 1) << CRT_AUTO_CENTERING_BR_BOTTOM_SHIFT) &
+		CRT_AUTO_CENTERING_BR_BOTTOM_MASK) |
+	       ((x - 1) & CRT_AUTO_CENTERING_BR_RIGHT_MASK));
+
+	/*
+	 * Assume common fields in dispControl have been properly set before
+	 * calling this function.
+	 * This function only sets the extra fields in dispControl.
+	 */
+
+	/* Clear bit 29:27 of display control register */
+	dispControl &= ~CRT_DISPLAY_CTRL_CLK_MASK;
+
+	/* Set bit 29:27 of display control register for the right clock */
+	/* Note that SM750LE only need to supported 7 resolutions. */
+	if (x == 800 && y == 600)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL41;
+	else if (x == 1024 && y == 768)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL65;
+	else if (x == 1152 && y == 864)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL80;
+	else if (x == 1280 && y == 768)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL80;
+	else if (x == 1280 && y == 720)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL74;
+	else if (x == 1280 && y == 960)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL108;
+	else if (x == 1280 && y == 1024)
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL108;
+	else /* default to VGA clock */
+		dispControl |= CRT_DISPLAY_CTRL_CLK_PLL25;
+
+	/* Set bit 25:24 of display controller */
+	dispControl |= (CRT_DISPLAY_CTRL_CRTSELECT | CRT_DISPLAY_CTRL_RGBBIT);
+
+	/* Set bit 14 of display controller */
+	dispControl |= DISPLAY_CTRL_CLOCK_PHASE;
+
+	poke32(CRT_DISPLAY_CTRL, dispControl);
+
+	return dispControl;
+}
+
+/* only timing related registers will be  programed */
+static int programModeRegisters(struct mode_parameter *pModeParam,
+				struct pll_value *pll)
+{
+	int ret = 0;
+	int cnt = 0;
+	unsigned int tmp, reg;
+
+	if (pll->clock_type == SECONDARY_PLL) {
+		/* programe secondary pixel clock */
+		poke32(CRT_PLL_CTRL, sm750_format_pll_reg(pll));
+
+		tmp = ((pModeParam->horizontal_total - 1) <<
+		       CRT_HORIZONTAL_TOTAL_TOTAL_SHIFT) &
+		     CRT_HORIZONTAL_TOTAL_TOTAL_MASK;
+		tmp |= (pModeParam->horizontal_display_end - 1) &
+		      CRT_HORIZONTAL_TOTAL_DISPLAY_END_MASK;
+
+		poke32(CRT_HORIZONTAL_TOTAL, tmp);
+
+		tmp = (pModeParam->horizontal_sync_width <<
+		       CRT_HORIZONTAL_SYNC_WIDTH_SHIFT) &
+		     CRT_HORIZONTAL_SYNC_WIDTH_MASK;
+		tmp |= (pModeParam->horizontal_sync_start - 1) &
+		      CRT_HORIZONTAL_SYNC_START_MASK;
+
+		poke32(CRT_HORIZONTAL_SYNC, tmp);
+
+		tmp = ((pModeParam->vertical_total - 1) <<
+		       CRT_VERTICAL_TOTAL_TOTAL_SHIFT) &
+		     CRT_VERTICAL_TOTAL_TOTAL_MASK;
+		tmp |= (pModeParam->vertical_display_end - 1) &
+		      CRT_VERTICAL_TOTAL_DISPLAY_END_MASK;
+
+		poke32(CRT_VERTICAL_TOTAL, tmp);
+
+		tmp = ((pModeParam->vertical_sync_height <<
+		       CRT_VERTICAL_SYNC_HEIGHT_SHIFT)) &
+		     CRT_VERTICAL_SYNC_HEIGHT_MASK;
+		tmp |= (pModeParam->vertical_sync_start - 1) &
+		      CRT_VERTICAL_SYNC_START_MASK;
+
+		poke32(CRT_VERTICAL_SYNC, tmp);
+
+		tmp = DISPLAY_CTRL_TIMING | DISPLAY_CTRL_PLANE;
+		if (pModeParam->vertical_sync_polarity)
+			tmp |= DISPLAY_CTRL_VSYNC_PHASE;
+		if (pModeParam->horizontal_sync_polarity)
+			tmp |= DISPLAY_CTRL_HSYNC_PHASE;
+
+		if (sm750_get_chip_type() == SM750LE) {
+			displayControlAdjust_SM750LE(pModeParam, tmp);
+		} else {
+			reg = peek32(CRT_DISPLAY_CTRL) &
+				~(DISPLAY_CTRL_VSYNC_PHASE |
+				  DISPLAY_CTRL_HSYNC_PHASE |
+				  DISPLAY_CTRL_TIMING | DISPLAY_CTRL_PLANE);
+
+			poke32(CRT_DISPLAY_CTRL, tmp | reg);
+		}
+
+	} else if (pll->clock_type == PRIMARY_PLL) {
+		unsigned int reserved;
+
+		poke32(PANEL_PLL_CTRL, sm750_format_pll_reg(pll));
+
+		reg = ((pModeParam->horizontal_total - 1) <<
+			PANEL_HORIZONTAL_TOTAL_TOTAL_SHIFT) &
+			PANEL_HORIZONTAL_TOTAL_TOTAL_MASK;
+		reg |= ((pModeParam->horizontal_display_end - 1) &
+			PANEL_HORIZONTAL_TOTAL_DISPLAY_END_MASK);
+		poke32(PANEL_HORIZONTAL_TOTAL, reg);
+
+		poke32(PANEL_HORIZONTAL_SYNC,
+		       ((pModeParam->horizontal_sync_width <<
+			 PANEL_HORIZONTAL_SYNC_WIDTH_SHIFT) &
+			PANEL_HORIZONTAL_SYNC_WIDTH_MASK) |
+		       ((pModeParam->horizontal_sync_start - 1) &
+			PANEL_HORIZONTAL_SYNC_START_MASK));
+
+		poke32(PANEL_VERTICAL_TOTAL,
+		       (((pModeParam->vertical_total - 1) <<
+			 PANEL_VERTICAL_TOTAL_TOTAL_SHIFT) &
+			PANEL_VERTICAL_TOTAL_TOTAL_MASK) |
+		       ((pModeParam->vertical_display_end - 1) &
+			PANEL_VERTICAL_TOTAL_DISPLAY_END_MASK));
+
+		poke32(PANEL_VERTICAL_SYNC,
+		       ((pModeParam->vertical_sync_height <<
+			 PANEL_VERTICAL_SYNC_HEIGHT_SHIFT) &
+			PANEL_VERTICAL_SYNC_HEIGHT_MASK) |
+		       ((pModeParam->vertical_sync_start - 1) &
+			PANEL_VERTICAL_SYNC_START_MASK));
+
+		tmp = DISPLAY_CTRL_TIMING | DISPLAY_CTRL_PLANE;
+		if (pModeParam->vertical_sync_polarity)
+			tmp |= DISPLAY_CTRL_VSYNC_PHASE;
+		if (pModeParam->horizontal_sync_polarity)
+			tmp |= DISPLAY_CTRL_HSYNC_PHASE;
+		if (pModeParam->clock_phase_polarity)
+			tmp |= DISPLAY_CTRL_CLOCK_PHASE;
+
+		reserved = PANEL_DISPLAY_CTRL_RESERVED_MASK |
+			PANEL_DISPLAY_CTRL_VSYNC;
+
+		reg = (peek32(PANEL_DISPLAY_CTRL) & ~reserved) &
+			~(DISPLAY_CTRL_CLOCK_PHASE | DISPLAY_CTRL_VSYNC_PHASE |
+			  DISPLAY_CTRL_HSYNC_PHASE | DISPLAY_CTRL_TIMING |
+			  DISPLAY_CTRL_PLANE);
+
+		/*
+		 * May a hardware bug or just my test chip (not confirmed).
+		 * PANEL_DISPLAY_CTRL register seems requiring few writes
+		 * before a value can be successfully written in.
+		 * Added some masks to mask out the reserved bits.
+		 * Note: This problem happens by design. The hardware will wait
+		 *       for the next vertical sync to turn on/off the plane.
+		 */
+		poke32(PANEL_DISPLAY_CTRL, tmp | reg);
+
+		while ((peek32(PANEL_DISPLAY_CTRL) & ~reserved) !=
+			(tmp | reg)) {
+			cnt++;
+			if (cnt > 1000)
+				break;
+			poke32(PANEL_DISPLAY_CTRL, tmp | reg);
+		}
+	} else {
+		ret = -1;
+	}
+	return ret;
+}
+
+int ddk750_setModeTiming(struct mode_parameter *parm, enum clock_type clock)
+{
+	struct pll_value pll;
+
+	pll.input_freq = DEFAULT_INPUT_CLOCK;
+	pll.clock_type = clock;
+
+	sm750_calc_pll_value(parm->pixel_clock, &pll);
+	if (sm750_get_chip_type() == SM750LE) {
+		/* set graphic mode via IO method */
+		outb_p(0x88, 0x3d4);
+		outb_p(0x06, 0x3d5);
+	}
+	programModeRegisters(parm, &pll);
+	return 0;
+}
diff --git a/drivers/staging/sm750fb/ddk750_mode.h b/drivers/staging/sm750fb/ddk750_mode.h
new file mode 100644
index 000000000..2df78a093
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_mode.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_MODE_H__
+#define DDK750_MODE_H__
+
+#include "ddk750_chip.h"
+
+enum spolarity {
+	POS = 0, /* positive */
+	NEG, /* negative */
+};
+
+struct mode_parameter {
+	/* Horizontal timing. */
+	unsigned long horizontal_total;
+	unsigned long horizontal_display_end;
+	unsigned long horizontal_sync_start;
+	unsigned long horizontal_sync_width;
+	enum spolarity horizontal_sync_polarity;
+
+	/* Vertical timing. */
+	unsigned long vertical_total;
+	unsigned long vertical_display_end;
+	unsigned long vertical_sync_start;
+	unsigned long vertical_sync_height;
+	enum spolarity vertical_sync_polarity;
+
+	/* Refresh timing. */
+	unsigned long pixel_clock;
+	unsigned long horizontal_frequency;
+	unsigned long vertical_frequency;
+
+	/* Clock Phase. This clock phase only applies to Panel. */
+	enum spolarity clock_phase_polarity;
+};
+
+int ddk750_setModeTiming(struct mode_parameter *parm, enum clock_type clock);
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_power.c b/drivers/staging/sm750fb/ddk750_power.c
new file mode 100644
index 000000000..12834f78e
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_power.c
@@ -0,0 +1,145 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "ddk750_chip.h"
+#include "ddk750_reg.h"
+#include "ddk750_power.h"
+
+void ddk750_set_dpms(enum dpms state)
+{
+	unsigned int value;
+
+	if (sm750_get_chip_type() == SM750LE) {
+		value = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_DPMS_MASK;
+		value |= (state << CRT_DISPLAY_CTRL_DPMS_SHIFT);
+		poke32(CRT_DISPLAY_CTRL, value);
+	} else {
+		value = peek32(SYSTEM_CTRL);
+		value = (value & ~SYSTEM_CTRL_DPMS_MASK) | state;
+		poke32(SYSTEM_CTRL, value);
+	}
+}
+
+static unsigned int get_power_mode(void)
+{
+	if (sm750_get_chip_type() == SM750LE)
+		return 0;
+	return peek32(POWER_MODE_CTRL) & POWER_MODE_CTRL_MODE_MASK;
+}
+
+/*
+ * SM50x can operate in one of three modes: 0, 1 or Sleep.
+ * On hardware reset, power mode 0 is default.
+ */
+void sm750_set_power_mode(unsigned int mode)
+{
+	unsigned int ctrl = 0;
+
+	ctrl = peek32(POWER_MODE_CTRL) & ~POWER_MODE_CTRL_MODE_MASK;
+
+	if (sm750_get_chip_type() == SM750LE)
+		return;
+
+	switch (mode) {
+	case POWER_MODE_CTRL_MODE_MODE0:
+		ctrl |= POWER_MODE_CTRL_MODE_MODE0;
+		break;
+
+	case POWER_MODE_CTRL_MODE_MODE1:
+		ctrl |= POWER_MODE_CTRL_MODE_MODE1;
+		break;
+
+	case POWER_MODE_CTRL_MODE_SLEEP:
+		ctrl |= POWER_MODE_CTRL_MODE_SLEEP;
+		break;
+
+	default:
+		break;
+	}
+
+	/* Set up other fields in Power Control Register */
+	if (mode == POWER_MODE_CTRL_MODE_SLEEP) {
+		ctrl &= ~POWER_MODE_CTRL_OSC_INPUT;
+#ifdef VALIDATION_CHIP
+		ctrl &= ~POWER_MODE_CTRL_336CLK;
+#endif
+	} else {
+		ctrl |= POWER_MODE_CTRL_OSC_INPUT;
+#ifdef VALIDATION_CHIP
+		ctrl |= POWER_MODE_CTRL_336CLK;
+#endif
+	}
+
+	/* Program new power mode. */
+	poke32(POWER_MODE_CTRL, ctrl);
+}
+
+void sm750_set_current_gate(unsigned int gate)
+{
+	if (get_power_mode() == POWER_MODE_CTRL_MODE_MODE1)
+		poke32(MODE1_GATE, gate);
+	else
+		poke32(MODE0_GATE, gate);
+}
+
+/*
+ * This function enable/disable the 2D engine.
+ */
+void sm750_enable_2d_engine(unsigned int enable)
+{
+	u32 gate;
+
+	gate = peek32(CURRENT_GATE);
+	if (enable)
+		gate |= (CURRENT_GATE_DE | CURRENT_GATE_CSC);
+	else
+		gate &= ~(CURRENT_GATE_DE | CURRENT_GATE_CSC);
+
+	sm750_set_current_gate(gate);
+}
+
+void sm750_enable_dma(unsigned int enable)
+{
+	u32 gate;
+
+	/* Enable DMA Gate */
+	gate = peek32(CURRENT_GATE);
+	if (enable)
+		gate |= CURRENT_GATE_DMA;
+	else
+		gate &= ~CURRENT_GATE_DMA;
+
+	sm750_set_current_gate(gate);
+}
+
+/*
+ * This function enable/disable the GPIO Engine
+ */
+void sm750_enable_gpio(unsigned int enable)
+{
+	u32 gate;
+
+	/* Enable GPIO Gate */
+	gate = peek32(CURRENT_GATE);
+	if (enable)
+		gate |= CURRENT_GATE_GPIO;
+	else
+		gate &= ~CURRENT_GATE_GPIO;
+
+	sm750_set_current_gate(gate);
+}
+
+/*
+ * This function enable/disable the I2C Engine
+ */
+void sm750_enable_i2c(unsigned int enable)
+{
+	u32 gate;
+
+	/* Enable I2C Gate */
+	gate = peek32(CURRENT_GATE);
+	if (enable)
+		gate |= CURRENT_GATE_I2C;
+	else
+		gate &= ~CURRENT_GATE_I2C;
+
+	sm750_set_current_gate(gate);
+}
diff --git a/drivers/staging/sm750fb/ddk750_power.h b/drivers/staging/sm750fb/ddk750_power.h
new file mode 100644
index 000000000..63c9e8b6f
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_power.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_POWER_H__
+#define DDK750_POWER_H__
+
+enum dpms {
+	crtDPMS_ON = 0x0,
+	crtDPMS_STANDBY = 0x1,
+	crtDPMS_SUSPEND = 0x2,
+	crtDPMS_OFF = 0x3,
+};
+
+#define set_DAC(off) {							\
+	poke32(MISC_CTRL,						\
+	       (peek32(MISC_CTRL) & ~MISC_CTRL_DAC_POWER_OFF) | (off)); \
+}
+
+void ddk750_set_dpms(enum dpms state);
+void sm750_set_power_mode(unsigned int mode);
+void sm750_set_current_gate(unsigned int gate);
+
+/*
+ * This function enable/disable the 2D engine.
+ */
+void sm750_enable_2d_engine(unsigned int enable);
+
+/*
+ * This function enable/disable the DMA Engine
+ */
+void sm750_enable_dma(unsigned int enable);
+
+/*
+ * This function enable/disable the GPIO Engine
+ */
+void sm750_enable_gpio(unsigned int enable);
+
+/*
+ * This function enable/disable the I2C Engine
+ */
+void sm750_enable_i2c(unsigned int enable);
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_reg.h b/drivers/staging/sm750fb/ddk750_reg.h
new file mode 100644
index 000000000..fe412ead7
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_reg.h
@@ -0,0 +1,1455 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_REG_H__
+#define DDK750_REG_H__
+
+/* New register for SM750LE */
+#define DE_STATE1                                        0x100054
+#define DE_STATE1_DE_ABORT                               BIT(0)
+
+#define DE_STATE2                                        0x100058
+#define DE_STATE2_DE_FIFO_EMPTY                          BIT(3)
+#define DE_STATE2_DE_STATUS_BUSY                         BIT(2)
+#define DE_STATE2_DE_MEM_FIFO_EMPTY                      BIT(1)
+
+#define SYSTEM_CTRL                                   0x000000
+#define SYSTEM_CTRL_DPMS_MASK                         (0x3 << 30)
+#define SYSTEM_CTRL_DPMS_VPHP                         (0x0 << 30)
+#define SYSTEM_CTRL_DPMS_VPHN                         (0x1 << 30)
+#define SYSTEM_CTRL_DPMS_VNHP                         (0x2 << 30)
+#define SYSTEM_CTRL_DPMS_VNHN                         (0x3 << 30)
+#define SYSTEM_CTRL_PCI_BURST                         BIT(29)
+#define SYSTEM_CTRL_PCI_MASTER                        BIT(25)
+#define SYSTEM_CTRL_LATENCY_TIMER_OFF                 BIT(24)
+#define SYSTEM_CTRL_DE_FIFO_EMPTY                     BIT(23)
+#define SYSTEM_CTRL_DE_STATUS_BUSY                    BIT(22)
+#define SYSTEM_CTRL_DE_MEM_FIFO_EMPTY                 BIT(21)
+#define SYSTEM_CTRL_CSC_STATUS_BUSY                   BIT(20)
+#define SYSTEM_CTRL_CRT_VSYNC_ACTIVE                  BIT(19)
+#define SYSTEM_CTRL_PANEL_VSYNC_ACTIVE                BIT(18)
+#define SYSTEM_CTRL_CURRENT_BUFFER_FLIP_PENDING       BIT(17)
+#define SYSTEM_CTRL_DMA_STATUS_BUSY                   BIT(16)
+#define SYSTEM_CTRL_PCI_BURST_READ                    BIT(15)
+#define SYSTEM_CTRL_DE_ABORT                          BIT(13)
+#define SYSTEM_CTRL_PCI_SUBSYS_ID_LOCK                BIT(11)
+#define SYSTEM_CTRL_PCI_RETRY_OFF                     BIT(7)
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_MASK    (0x3 << 4)
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_1       (0x0 << 4)
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_2       (0x1 << 4)
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_4       (0x2 << 4)
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_8       (0x3 << 4)
+#define SYSTEM_CTRL_CRT_TRISTATE                      BIT(3)
+#define SYSTEM_CTRL_PCIMEM_TRISTATE                   BIT(2)
+#define SYSTEM_CTRL_LOCALMEM_TRISTATE                 BIT(1)
+#define SYSTEM_CTRL_PANEL_TRISTATE                    BIT(0)
+
+#define MISC_CTRL                                     0x000004
+#define MISC_CTRL_DRAM_RERESH_COUNT                   BIT(27)
+#define MISC_CTRL_DRAM_REFRESH_TIME_MASK              (0x3 << 25)
+#define MISC_CTRL_DRAM_REFRESH_TIME_8                 (0x0 << 25)
+#define MISC_CTRL_DRAM_REFRESH_TIME_16                (0x1 << 25)
+#define MISC_CTRL_DRAM_REFRESH_TIME_32                (0x2 << 25)
+#define MISC_CTRL_DRAM_REFRESH_TIME_64                (0x3 << 25)
+#define MISC_CTRL_INT_OUTPUT_INVERT                   BIT(24)
+#define MISC_CTRL_PLL_CLK_COUNT                       BIT(23)
+#define MISC_CTRL_DAC_POWER_OFF                       BIT(20)
+#define MISC_CTRL_CLK_SELECT_TESTCLK                  BIT(16)
+#define MISC_CTRL_DRAM_COLUMN_SIZE_MASK               (0x3 << 14)
+#define MISC_CTRL_DRAM_COLUMN_SIZE_256                (0x0 << 14)
+#define MISC_CTRL_DRAM_COLUMN_SIZE_512                (0x1 << 14)
+#define MISC_CTRL_DRAM_COLUMN_SIZE_1024               (0x2 << 14)
+#define MISC_CTRL_LOCALMEM_SIZE_MASK                  (0x3 << 12)
+#define MISC_CTRL_LOCALMEM_SIZE_8M                    (0x3 << 12)
+#define MISC_CTRL_LOCALMEM_SIZE_16M                   (0x0 << 12)
+#define MISC_CTRL_LOCALMEM_SIZE_32M                   (0x1 << 12)
+#define MISC_CTRL_LOCALMEM_SIZE_64M                   (0x2 << 12)
+#define MISC_CTRL_DRAM_TWTR                           BIT(11)
+#define MISC_CTRL_DRAM_TWR                            BIT(10)
+#define MISC_CTRL_DRAM_TRP                            BIT(9)
+#define MISC_CTRL_DRAM_TRFC                           BIT(8)
+#define MISC_CTRL_DRAM_TRAS                           BIT(7)
+#define MISC_CTRL_LOCALMEM_RESET                      BIT(6)
+#define MISC_CTRL_LOCALMEM_STATE_INACTIVE             BIT(5)
+#define MISC_CTRL_CPU_CAS_LATENCY                     BIT(4)
+#define MISC_CTRL_DLL_OFF                             BIT(3)
+#define MISC_CTRL_DRAM_OUTPUT_HIGH                    BIT(2)
+#define MISC_CTRL_LOCALMEM_BUS_SIZE                   BIT(1)
+#define MISC_CTRL_EMBEDDED_LOCALMEM_OFF               BIT(0)
+
+#define GPIO_MUX                                      0x000008
+#define GPIO_MUX_31                                   BIT(31)
+#define GPIO_MUX_30                                   BIT(30)
+#define GPIO_MUX_29                                   BIT(29)
+#define GPIO_MUX_28                                   BIT(28)
+#define GPIO_MUX_27                                   BIT(27)
+#define GPIO_MUX_26                                   BIT(26)
+#define GPIO_MUX_25                                   BIT(25)
+#define GPIO_MUX_24                                   BIT(24)
+#define GPIO_MUX_23                                   BIT(23)
+#define GPIO_MUX_22                                   BIT(22)
+#define GPIO_MUX_21                                   BIT(21)
+#define GPIO_MUX_20                                   BIT(20)
+#define GPIO_MUX_19                                   BIT(19)
+#define GPIO_MUX_18                                   BIT(18)
+#define GPIO_MUX_17                                   BIT(17)
+#define GPIO_MUX_16                                   BIT(16)
+#define GPIO_MUX_15                                   BIT(15)
+#define GPIO_MUX_14                                   BIT(14)
+#define GPIO_MUX_13                                   BIT(13)
+#define GPIO_MUX_12                                   BIT(12)
+#define GPIO_MUX_11                                   BIT(11)
+#define GPIO_MUX_10                                   BIT(10)
+#define GPIO_MUX_9                                    BIT(9)
+#define GPIO_MUX_8                                    BIT(8)
+#define GPIO_MUX_7                                    BIT(7)
+#define GPIO_MUX_6                                    BIT(6)
+#define GPIO_MUX_5                                    BIT(5)
+#define GPIO_MUX_4                                    BIT(4)
+#define GPIO_MUX_3                                    BIT(3)
+#define GPIO_MUX_2                                    BIT(2)
+#define GPIO_MUX_1                                    BIT(1)
+#define GPIO_MUX_0                                    BIT(0)
+
+#define LOCALMEM_ARBITRATION                          0x00000C
+#define LOCALMEM_ARBITRATION_ROTATE                   BIT(28)
+#define LOCALMEM_ARBITRATION_VGA_MASK                 (0x7 << 24)
+#define LOCALMEM_ARBITRATION_VGA_OFF                  (0x0 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_1           (0x1 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_2           (0x2 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_3           (0x3 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_4           (0x4 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_5           (0x5 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_6           (0x6 << 24)
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_7           (0x7 << 24)
+#define LOCALMEM_ARBITRATION_DMA_MASK                 (0x7 << 20)
+#define LOCALMEM_ARBITRATION_DMA_OFF                  (0x0 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_1           (0x1 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_2           (0x2 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_3           (0x3 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_4           (0x4 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_5           (0x5 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_6           (0x6 << 20)
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_7           (0x7 << 20)
+#define LOCALMEM_ARBITRATION_ZVPORT1_MASK             (0x7 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_OFF              (0x0 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_1       (0x1 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_2       (0x2 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_3       (0x3 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_4       (0x4 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_5       (0x5 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_6       (0x6 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_7       (0x7 << 16)
+#define LOCALMEM_ARBITRATION_ZVPORT0_MASK             (0x7 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_OFF              (0x0 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_1       (0x1 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_2       (0x2 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_3       (0x3 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_4       (0x4 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_5       (0x5 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_6       (0x6 << 12)
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_7       (0x7 << 12)
+#define LOCALMEM_ARBITRATION_VIDEO_MASK               (0x7 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_OFF                (0x0 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_1         (0x1 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_2         (0x2 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_3         (0x3 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_4         (0x4 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_5         (0x5 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_6         (0x6 << 8)
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_7         (0x7 << 8)
+#define LOCALMEM_ARBITRATION_PANEL_MASK               (0x7 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_OFF                (0x0 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_1         (0x1 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_2         (0x2 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_3         (0x3 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_4         (0x4 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_5         (0x5 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_6         (0x6 << 4)
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_7         (0x7 << 4)
+#define LOCALMEM_ARBITRATION_CRT_MASK                 0x7
+#define LOCALMEM_ARBITRATION_CRT_OFF                  0x0
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_1           0x1
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_2           0x2
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_3           0x3
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_4           0x4
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_5           0x5
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_6           0x6
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_7           0x7
+
+#define PCIMEM_ARBITRATION                            0x000010
+#define PCIMEM_ARBITRATION_ROTATE                     BIT(28)
+#define PCIMEM_ARBITRATION_VGA_MASK                   (0x7 << 24)
+#define PCIMEM_ARBITRATION_VGA_OFF                    (0x0 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_1             (0x1 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_2             (0x2 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_3             (0x3 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_4             (0x4 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_5             (0x5 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_6             (0x6 << 24)
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_7             (0x7 << 24)
+#define PCIMEM_ARBITRATION_DMA_MASK                   (0x7 << 20)
+#define PCIMEM_ARBITRATION_DMA_OFF                    (0x0 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_1             (0x1 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_2             (0x2 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_3             (0x3 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_4             (0x4 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_5             (0x5 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_6             (0x6 << 20)
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_7             (0x7 << 20)
+#define PCIMEM_ARBITRATION_ZVPORT1_MASK               (0x7 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_OFF                (0x0 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_1         (0x1 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_2         (0x2 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_3         (0x3 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_4         (0x4 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_5         (0x5 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_6         (0x6 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_7         (0x7 << 16)
+#define PCIMEM_ARBITRATION_ZVPORT0_MASK               (0x7 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_OFF                (0x0 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_1         (0x1 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_2         (0x2 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_3         (0x3 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_4         (0x4 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_5         (0x5 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_6         (0x6 << 12)
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_7         (0x7 << 12)
+#define PCIMEM_ARBITRATION_VIDEO_MASK                 (0x7 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_OFF                  (0x0 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_1           (0x1 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_2           (0x2 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_3           (0x3 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_4           (0x4 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_5           (0x5 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_6           (0x6 << 8)
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_7           (0x7 << 8)
+#define PCIMEM_ARBITRATION_PANEL_MASK                 (0x7 << 4)
+#define PCIMEM_ARBITRATION_PANEL_OFF                  (0x0 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_1           (0x1 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_2           (0x2 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_3           (0x3 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_4           (0x4 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_5           (0x5 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_6           (0x6 << 4)
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_7           (0x7 << 4)
+#define PCIMEM_ARBITRATION_CRT_MASK                   0x7
+#define PCIMEM_ARBITRATION_CRT_OFF                    0x0
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_1             0x1
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_2             0x2
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_3             0x3
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_4             0x4
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_5             0x5
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_6             0x6
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_7             0x7
+
+#define RAW_INT                                       0x000020
+#define RAW_INT_ZVPORT1_VSYNC                         BIT(4)
+#define RAW_INT_ZVPORT0_VSYNC                         BIT(3)
+#define RAW_INT_CRT_VSYNC                             BIT(2)
+#define RAW_INT_PANEL_VSYNC                           BIT(1)
+#define RAW_INT_VGA_VSYNC                             BIT(0)
+
+#define INT_STATUS                                    0x000024
+#define INT_STATUS_GPIO31                             BIT(31)
+#define INT_STATUS_GPIO30                             BIT(30)
+#define INT_STATUS_GPIO29                             BIT(29)
+#define INT_STATUS_GPIO28                             BIT(28)
+#define INT_STATUS_GPIO27                             BIT(27)
+#define INT_STATUS_GPIO26                             BIT(26)
+#define INT_STATUS_GPIO25                             BIT(25)
+#define INT_STATUS_I2C                                BIT(12)
+#define INT_STATUS_PWM                                BIT(11)
+#define INT_STATUS_DMA1                               BIT(10)
+#define INT_STATUS_DMA0                               BIT(9)
+#define INT_STATUS_PCI                                BIT(8)
+#define INT_STATUS_SSP1                               BIT(7)
+#define INT_STATUS_SSP0                               BIT(6)
+#define INT_STATUS_DE                                 BIT(5)
+#define INT_STATUS_ZVPORT1_VSYNC                      BIT(4)
+#define INT_STATUS_ZVPORT0_VSYNC                      BIT(3)
+#define INT_STATUS_CRT_VSYNC                          BIT(2)
+#define INT_STATUS_PANEL_VSYNC                        BIT(1)
+#define INT_STATUS_VGA_VSYNC                          BIT(0)
+
+#define INT_MASK                                      0x000028
+#define INT_MASK_GPIO31                               BIT(31)
+#define INT_MASK_GPIO30                               BIT(30)
+#define INT_MASK_GPIO29                               BIT(29)
+#define INT_MASK_GPIO28                               BIT(28)
+#define INT_MASK_GPIO27                               BIT(27)
+#define INT_MASK_GPIO26                               BIT(26)
+#define INT_MASK_GPIO25                               BIT(25)
+#define INT_MASK_I2C                                  BIT(12)
+#define INT_MASK_PWM                                  BIT(11)
+#define INT_MASK_DMA1                                 BIT(10)
+#define INT_MASK_DMA                                  BIT(9)
+#define INT_MASK_PCI                                  BIT(8)
+#define INT_MASK_SSP1                                 BIT(7)
+#define INT_MASK_SSP0                                 BIT(6)
+#define INT_MASK_DE                                   BIT(5)
+#define INT_MASK_ZVPORT1_VSYNC                        BIT(4)
+#define INT_MASK_ZVPORT0_VSYNC                        BIT(3)
+#define INT_MASK_CRT_VSYNC                            BIT(2)
+#define INT_MASK_PANEL_VSYNC                          BIT(1)
+#define INT_MASK_VGA_VSYNC                            BIT(0)
+
+#define CURRENT_GATE                                  0x000040
+#define CURRENT_GATE_MCLK_MASK                        (0x3 << 14)
+#ifdef VALIDATION_CHIP
+    #define CURRENT_GATE_MCLK_112MHZ                  (0x0 << 14)
+    #define CURRENT_GATE_MCLK_84MHZ                   (0x1 << 14)
+    #define CURRENT_GATE_MCLK_56MHZ                   (0x2 << 14)
+    #define CURRENT_GATE_MCLK_42MHZ                   (0x3 << 14)
+#else
+    #define CURRENT_GATE_MCLK_DIV_3                   (0x0 << 14)
+    #define CURRENT_GATE_MCLK_DIV_4                   (0x1 << 14)
+    #define CURRENT_GATE_MCLK_DIV_6                   (0x2 << 14)
+    #define CURRENT_GATE_MCLK_DIV_8                   (0x3 << 14)
+#endif
+#define CURRENT_GATE_M2XCLK_MASK                      (0x3 << 12)
+#ifdef VALIDATION_CHIP
+    #define CURRENT_GATE_M2XCLK_336MHZ                (0x0 << 12)
+    #define CURRENT_GATE_M2XCLK_168MHZ                (0x1 << 12)
+    #define CURRENT_GATE_M2XCLK_112MHZ                (0x2 << 12)
+    #define CURRENT_GATE_M2XCLK_84MHZ                 (0x3 << 12)
+#else
+    #define CURRENT_GATE_M2XCLK_DIV_1                 (0x0 << 12)
+    #define CURRENT_GATE_M2XCLK_DIV_2                 (0x1 << 12)
+    #define CURRENT_GATE_M2XCLK_DIV_3                 (0x2 << 12)
+    #define CURRENT_GATE_M2XCLK_DIV_4                 (0x3 << 12)
+#endif
+#define CURRENT_GATE_VGA                              BIT(10)
+#define CURRENT_GATE_PWM                              BIT(9)
+#define CURRENT_GATE_I2C                              BIT(8)
+#define CURRENT_GATE_SSP                              BIT(7)
+#define CURRENT_GATE_GPIO                             BIT(6)
+#define CURRENT_GATE_ZVPORT                           BIT(5)
+#define CURRENT_GATE_CSC                              BIT(4)
+#define CURRENT_GATE_DE                               BIT(3)
+#define CURRENT_GATE_DISPLAY                          BIT(2)
+#define CURRENT_GATE_LOCALMEM                         BIT(1)
+#define CURRENT_GATE_DMA                              BIT(0)
+
+#define MODE0_GATE                                    0x000044
+#define MODE0_GATE_MCLK_MASK                          (0x3 << 14)
+#define MODE0_GATE_MCLK_112MHZ                        (0x0 << 14)
+#define MODE0_GATE_MCLK_84MHZ                         (0x1 << 14)
+#define MODE0_GATE_MCLK_56MHZ                         (0x2 << 14)
+#define MODE0_GATE_MCLK_42MHZ                         (0x3 << 14)
+#define MODE0_GATE_M2XCLK_MASK                        (0x3 << 12)
+#define MODE0_GATE_M2XCLK_336MHZ                      (0x0 << 12)
+#define MODE0_GATE_M2XCLK_168MHZ                      (0x1 << 12)
+#define MODE0_GATE_M2XCLK_112MHZ                      (0x2 << 12)
+#define MODE0_GATE_M2XCLK_84MHZ                       (0x3 << 12)
+#define MODE0_GATE_VGA                                BIT(10)
+#define MODE0_GATE_PWM                                BIT(9)
+#define MODE0_GATE_I2C                                BIT(8)
+#define MODE0_GATE_SSP                                BIT(7)
+#define MODE0_GATE_GPIO                               BIT(6)
+#define MODE0_GATE_ZVPORT                             BIT(5)
+#define MODE0_GATE_CSC                                BIT(4)
+#define MODE0_GATE_DE                                 BIT(3)
+#define MODE0_GATE_DISPLAY                            BIT(2)
+#define MODE0_GATE_LOCALMEM                           BIT(1)
+#define MODE0_GATE_DMA                                BIT(0)
+
+#define MODE1_GATE                                    0x000048
+#define MODE1_GATE_MCLK_MASK                          (0x3 << 14)
+#define MODE1_GATE_MCLK_112MHZ                        (0x0 << 14)
+#define MODE1_GATE_MCLK_84MHZ                         (0x1 << 14)
+#define MODE1_GATE_MCLK_56MHZ                         (0x2 << 14)
+#define MODE1_GATE_MCLK_42MHZ                         (0x3 << 14)
+#define MODE1_GATE_M2XCLK_MASK                        (0x3 << 12)
+#define MODE1_GATE_M2XCLK_336MHZ                      (0x0 << 12)
+#define MODE1_GATE_M2XCLK_168MHZ                      (0x1 << 12)
+#define MODE1_GATE_M2XCLK_112MHZ                      (0x2 << 12)
+#define MODE1_GATE_M2XCLK_84MHZ                       (0x3 << 12)
+#define MODE1_GATE_VGA                                BIT(10)
+#define MODE1_GATE_PWM                                BIT(9)
+#define MODE1_GATE_I2C                                BIT(8)
+#define MODE1_GATE_SSP                                BIT(7)
+#define MODE1_GATE_GPIO                               BIT(6)
+#define MODE1_GATE_ZVPORT                             BIT(5)
+#define MODE1_GATE_CSC                                BIT(4)
+#define MODE1_GATE_DE                                 BIT(3)
+#define MODE1_GATE_DISPLAY                            BIT(2)
+#define MODE1_GATE_LOCALMEM                           BIT(1)
+#define MODE1_GATE_DMA                                BIT(0)
+
+#define POWER_MODE_CTRL                               0x00004C
+#ifdef VALIDATION_CHIP
+    #define POWER_MODE_CTRL_336CLK                    BIT(4)
+#endif
+#define POWER_MODE_CTRL_OSC_INPUT                     BIT(3)
+#define POWER_MODE_CTRL_ACPI                          BIT(2)
+#define POWER_MODE_CTRL_MODE_MASK                     (0x3 << 0)
+#define POWER_MODE_CTRL_MODE_MODE0                    (0x0 << 0)
+#define POWER_MODE_CTRL_MODE_MODE1                    (0x1 << 0)
+#define POWER_MODE_CTRL_MODE_SLEEP                    (0x2 << 0)
+
+#define PCI_MASTER_BASE                               0x000050
+#define PCI_MASTER_BASE_ADDRESS_MASK                  0xff
+
+#define DEVICE_ID                                     0x000054
+#define DEVICE_ID_DEVICE_ID_MASK                      (0xffff << 16)
+#define DEVICE_ID_REVISION_ID_MASK                    0xff
+
+#define PLL_CLK_COUNT                                 0x000058
+#define PLL_CLK_COUNT_COUNTER_MASK                    0xffff
+
+#define PANEL_PLL_CTRL                                0x00005C
+#define PLL_CTRL_BYPASS                               BIT(18)
+#define PLL_CTRL_POWER                                BIT(17)
+#define PLL_CTRL_INPUT                                BIT(16)
+#ifdef VALIDATION_CHIP
+    #define PLL_CTRL_OD_SHIFT                         14
+    #define PLL_CTRL_OD_MASK                          (0x3 << 14)
+#else
+    #define PLL_CTRL_POD_SHIFT                        14
+    #define PLL_CTRL_POD_MASK                         (0x3 << 14)
+    #define PLL_CTRL_OD_SHIFT                         12
+    #define PLL_CTRL_OD_MASK                          (0x3 << 12)
+#endif
+#define PLL_CTRL_N_SHIFT                              8
+#define PLL_CTRL_N_MASK                               (0xf << 8)
+#define PLL_CTRL_M_SHIFT                              0
+#define PLL_CTRL_M_MASK                               0xff
+
+#define CRT_PLL_CTRL                                  0x000060
+
+#define VGA_PLL0_CTRL                                 0x000064
+
+#define VGA_PLL1_CTRL                                 0x000068
+
+#define SCRATCH_DATA                                  0x00006c
+
+#ifndef VALIDATION_CHIP
+
+#define MXCLK_PLL_CTRL                                0x000070
+
+#define VGA_CONFIGURATION                             0x000088
+#define VGA_CONFIGURATION_USER_DEFINE_MASK            (0x3 << 4)
+#define VGA_CONFIGURATION_PLL                         BIT(2)
+#define VGA_CONFIGURATION_MODE                        BIT(1)
+
+#endif
+
+#define GPIO_DATA                                       0x010000
+#define GPIO_DATA_31                                    BIT(31)
+#define GPIO_DATA_30                                    BIT(30)
+#define GPIO_DATA_29                                    BIT(29)
+#define GPIO_DATA_28                                    BIT(28)
+#define GPIO_DATA_27                                    BIT(27)
+#define GPIO_DATA_26                                    BIT(26)
+#define GPIO_DATA_25                                    BIT(25)
+#define GPIO_DATA_24                                    BIT(24)
+#define GPIO_DATA_23                                    BIT(23)
+#define GPIO_DATA_22                                    BIT(22)
+#define GPIO_DATA_21                                    BIT(21)
+#define GPIO_DATA_20                                    BIT(20)
+#define GPIO_DATA_19                                    BIT(19)
+#define GPIO_DATA_18                                    BIT(18)
+#define GPIO_DATA_17                                    BIT(17)
+#define GPIO_DATA_16                                    BIT(16)
+#define GPIO_DATA_15                                    BIT(15)
+#define GPIO_DATA_14                                    BIT(14)
+#define GPIO_DATA_13                                    BIT(13)
+#define GPIO_DATA_12                                    BIT(12)
+#define GPIO_DATA_11                                    BIT(11)
+#define GPIO_DATA_10                                    BIT(10)
+#define GPIO_DATA_9                                     BIT(9)
+#define GPIO_DATA_8                                     BIT(8)
+#define GPIO_DATA_7                                     BIT(7)
+#define GPIO_DATA_6                                     BIT(6)
+#define GPIO_DATA_5                                     BIT(5)
+#define GPIO_DATA_4                                     BIT(4)
+#define GPIO_DATA_3                                     BIT(3)
+#define GPIO_DATA_2                                     BIT(2)
+#define GPIO_DATA_1                                     BIT(1)
+#define GPIO_DATA_0                                     BIT(0)
+
+#define GPIO_DATA_DIRECTION                             0x010004
+#define GPIO_DATA_DIRECTION_31                          BIT(31)
+#define GPIO_DATA_DIRECTION_30                          BIT(30)
+#define GPIO_DATA_DIRECTION_29                          BIT(29)
+#define GPIO_DATA_DIRECTION_28                          BIT(28)
+#define GPIO_DATA_DIRECTION_27                          BIT(27)
+#define GPIO_DATA_DIRECTION_26                          BIT(26)
+#define GPIO_DATA_DIRECTION_25                          BIT(25)
+#define GPIO_DATA_DIRECTION_24                          BIT(24)
+#define GPIO_DATA_DIRECTION_23                          BIT(23)
+#define GPIO_DATA_DIRECTION_22                          BIT(22)
+#define GPIO_DATA_DIRECTION_21                          BIT(21)
+#define GPIO_DATA_DIRECTION_20                          BIT(20)
+#define GPIO_DATA_DIRECTION_19                          BIT(19)
+#define GPIO_DATA_DIRECTION_18                          BIT(18)
+#define GPIO_DATA_DIRECTION_17                          BIT(17)
+#define GPIO_DATA_DIRECTION_16                          BIT(16)
+#define GPIO_DATA_DIRECTION_15                          BIT(15)
+#define GPIO_DATA_DIRECTION_14                          BIT(14)
+#define GPIO_DATA_DIRECTION_13                          BIT(13)
+#define GPIO_DATA_DIRECTION_12                          BIT(12)
+#define GPIO_DATA_DIRECTION_11                          BIT(11)
+#define GPIO_DATA_DIRECTION_10                          BIT(10)
+#define GPIO_DATA_DIRECTION_9                           BIT(9)
+#define GPIO_DATA_DIRECTION_8                           BIT(8)
+#define GPIO_DATA_DIRECTION_7                           BIT(7)
+#define GPIO_DATA_DIRECTION_6                           BIT(6)
+#define GPIO_DATA_DIRECTION_5                           BIT(5)
+#define GPIO_DATA_DIRECTION_4                           BIT(4)
+#define GPIO_DATA_DIRECTION_3                           BIT(3)
+#define GPIO_DATA_DIRECTION_2                           BIT(2)
+#define GPIO_DATA_DIRECTION_1                           BIT(1)
+#define GPIO_DATA_DIRECTION_0                           BIT(0)
+
+#define GPIO_INTERRUPT_SETUP                            0x010008
+#define GPIO_INTERRUPT_SETUP_TRIGGER_31                 BIT(22)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_30                 BIT(21)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_29                 BIT(20)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_28                 BIT(19)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_27                 BIT(18)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_26                 BIT(17)
+#define GPIO_INTERRUPT_SETUP_TRIGGER_25                 BIT(16)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_31                  BIT(14)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_30                  BIT(13)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_29                  BIT(12)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_28                  BIT(11)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_27                  BIT(10)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_26                  BIT(9)
+#define GPIO_INTERRUPT_SETUP_ACTIVE_25                  BIT(8)
+#define GPIO_INTERRUPT_SETUP_ENABLE_31                  BIT(6)
+#define GPIO_INTERRUPT_SETUP_ENABLE_30                  BIT(5)
+#define GPIO_INTERRUPT_SETUP_ENABLE_29                  BIT(4)
+#define GPIO_INTERRUPT_SETUP_ENABLE_28                  BIT(3)
+#define GPIO_INTERRUPT_SETUP_ENABLE_27                  BIT(2)
+#define GPIO_INTERRUPT_SETUP_ENABLE_26                  BIT(1)
+#define GPIO_INTERRUPT_SETUP_ENABLE_25                  BIT(0)
+
+#define GPIO_INTERRUPT_STATUS                           0x01000C
+#define GPIO_INTERRUPT_STATUS_31                        BIT(22)
+#define GPIO_INTERRUPT_STATUS_30                        BIT(21)
+#define GPIO_INTERRUPT_STATUS_29                        BIT(20)
+#define GPIO_INTERRUPT_STATUS_28                        BIT(19)
+#define GPIO_INTERRUPT_STATUS_27                        BIT(18)
+#define GPIO_INTERRUPT_STATUS_26                        BIT(17)
+#define GPIO_INTERRUPT_STATUS_25                        BIT(16)
+
+#define PANEL_DISPLAY_CTRL                            0x080000
+#define PANEL_DISPLAY_CTRL_RESERVED_MASK              0xc0f08000
+#define PANEL_DISPLAY_CTRL_SELECT_SHIFT               28
+#define PANEL_DISPLAY_CTRL_SELECT_MASK                (0x3 << 28)
+#define PANEL_DISPLAY_CTRL_SELECT_PANEL               (0x0 << 28)
+#define PANEL_DISPLAY_CTRL_SELECT_VGA                 (0x1 << 28)
+#define PANEL_DISPLAY_CTRL_SELECT_CRT                 (0x2 << 28)
+#define PANEL_DISPLAY_CTRL_FPEN                       BIT(27)
+#define PANEL_DISPLAY_CTRL_VBIASEN                    BIT(26)
+#define PANEL_DISPLAY_CTRL_DATA                       BIT(25)
+#define PANEL_DISPLAY_CTRL_FPVDDEN                    BIT(24)
+#define PANEL_DISPLAY_CTRL_DUAL_DISPLAY               BIT(19)
+#define PANEL_DISPLAY_CTRL_DOUBLE_PIXEL               BIT(18)
+#define PANEL_DISPLAY_CTRL_FIFO                       (0x3 << 16)
+#define PANEL_DISPLAY_CTRL_FIFO_1                     (0x0 << 16)
+#define PANEL_DISPLAY_CTRL_FIFO_3                     (0x1 << 16)
+#define PANEL_DISPLAY_CTRL_FIFO_7                     (0x2 << 16)
+#define PANEL_DISPLAY_CTRL_FIFO_11                    (0x3 << 16)
+#define DISPLAY_CTRL_CLOCK_PHASE                      BIT(14)
+#define DISPLAY_CTRL_VSYNC_PHASE                      BIT(13)
+#define DISPLAY_CTRL_HSYNC_PHASE                      BIT(12)
+#define PANEL_DISPLAY_CTRL_VSYNC                      BIT(11)
+#define PANEL_DISPLAY_CTRL_CAPTURE_TIMING             BIT(10)
+#define PANEL_DISPLAY_CTRL_COLOR_KEY                  BIT(9)
+#define DISPLAY_CTRL_TIMING                           BIT(8)
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_DIR           BIT(7)
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN               BIT(6)
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_DIR         BIT(5)
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN             BIT(4)
+#define DISPLAY_CTRL_GAMMA                            BIT(3)
+#define DISPLAY_CTRL_PLANE                            BIT(2)
+#define PANEL_DISPLAY_CTRL_FORMAT                     (0x3 << 0)
+#define PANEL_DISPLAY_CTRL_FORMAT_8                   (0x0 << 0)
+#define PANEL_DISPLAY_CTRL_FORMAT_16                  (0x1 << 0)
+#define PANEL_DISPLAY_CTRL_FORMAT_32                  (0x2 << 0)
+
+#define PANEL_PAN_CTRL                                0x080004
+#define PANEL_PAN_CTRL_VERTICAL_PAN_MASK              (0xff << 24)
+#define PANEL_PAN_CTRL_VERTICAL_VSYNC_MASK            (0x3f << 16)
+#define PANEL_PAN_CTRL_HORIZONTAL_PAN_MASK            (0xff << 8)
+#define PANEL_PAN_CTRL_HORIZONTAL_VSYNC_MASK          0x3f
+
+#define PANEL_COLOR_KEY                               0x080008
+#define PANEL_COLOR_KEY_MASK_MASK                     (0xffff << 16)
+#define PANEL_COLOR_KEY_VALUE_MASK                    0xffff
+
+#define PANEL_FB_ADDRESS                              0x08000C
+#define PANEL_FB_ADDRESS_STATUS                       BIT(31)
+#define PANEL_FB_ADDRESS_EXT                          BIT(27)
+#define PANEL_FB_ADDRESS_ADDRESS_MASK                 0x1ffffff
+
+#define PANEL_FB_WIDTH                                0x080010
+#define PANEL_FB_WIDTH_WIDTH_SHIFT                    16
+#define PANEL_FB_WIDTH_WIDTH_MASK                     (0x3fff << 16)
+#define PANEL_FB_WIDTH_OFFSET_MASK                    0x3fff
+
+#define PANEL_WINDOW_WIDTH                            0x080014
+#define PANEL_WINDOW_WIDTH_WIDTH_SHIFT                16
+#define PANEL_WINDOW_WIDTH_WIDTH_MASK                 (0xfff << 16)
+#define PANEL_WINDOW_WIDTH_X_MASK                     0xfff
+
+#define PANEL_WINDOW_HEIGHT                           0x080018
+#define PANEL_WINDOW_HEIGHT_HEIGHT_SHIFT              16
+#define PANEL_WINDOW_HEIGHT_HEIGHT_MASK               (0xfff << 16)
+#define PANEL_WINDOW_HEIGHT_Y_MASK                    0xfff
+
+#define PANEL_PLANE_TL                                0x08001C
+#define PANEL_PLANE_TL_TOP_SHIFT                      16
+#define PANEL_PLANE_TL_TOP_MASK                       (0x7ff << 16)
+#define PANEL_PLANE_TL_LEFT_MASK                      0x7ff
+
+#define PANEL_PLANE_BR                                0x080020
+#define PANEL_PLANE_BR_BOTTOM_SHIFT                   16
+#define PANEL_PLANE_BR_BOTTOM_MASK                    (0x7ff << 16)
+#define PANEL_PLANE_BR_RIGHT_MASK                     0x7ff
+
+#define PANEL_HORIZONTAL_TOTAL                        0x080024
+#define PANEL_HORIZONTAL_TOTAL_TOTAL_SHIFT            16
+#define PANEL_HORIZONTAL_TOTAL_TOTAL_MASK             (0xfff << 16)
+#define PANEL_HORIZONTAL_TOTAL_DISPLAY_END_MASK       0xfff
+
+#define PANEL_HORIZONTAL_SYNC                         0x080028
+#define PANEL_HORIZONTAL_SYNC_WIDTH_SHIFT             16
+#define PANEL_HORIZONTAL_SYNC_WIDTH_MASK              (0xff << 16)
+#define PANEL_HORIZONTAL_SYNC_START_MASK              0xfff
+
+#define PANEL_VERTICAL_TOTAL                          0x08002C
+#define PANEL_VERTICAL_TOTAL_TOTAL_SHIFT              16
+#define PANEL_VERTICAL_TOTAL_TOTAL_MASK               (0x7ff << 16)
+#define PANEL_VERTICAL_TOTAL_DISPLAY_END_MASK         0x7ff
+
+#define PANEL_VERTICAL_SYNC                           0x080030
+#define PANEL_VERTICAL_SYNC_HEIGHT_SHIFT              16
+#define PANEL_VERTICAL_SYNC_HEIGHT_MASK               (0x3f << 16)
+#define PANEL_VERTICAL_SYNC_START_MASK                0x7ff
+
+#define PANEL_CURRENT_LINE                            0x080034
+#define PANEL_CURRENT_LINE_LINE_MASK                  0x7ff
+
+/* Video Control */
+
+#define VIDEO_DISPLAY_CTRL                              0x080040
+#define VIDEO_DISPLAY_CTRL_LINE_BUFFER                  BIT(18)
+#define VIDEO_DISPLAY_CTRL_FIFO_MASK                    (0x3 << 16)
+#define VIDEO_DISPLAY_CTRL_FIFO_1                       (0x0 << 16)
+#define VIDEO_DISPLAY_CTRL_FIFO_3                       (0x1 << 16)
+#define VIDEO_DISPLAY_CTRL_FIFO_7                       (0x2 << 16)
+#define VIDEO_DISPLAY_CTRL_FIFO_11                      (0x3 << 16)
+#define VIDEO_DISPLAY_CTRL_BUFFER                       BIT(15)
+#define VIDEO_DISPLAY_CTRL_CAPTURE                      BIT(14)
+#define VIDEO_DISPLAY_CTRL_DOUBLE_BUFFER                BIT(13)
+#define VIDEO_DISPLAY_CTRL_BYTE_SWAP                    BIT(12)
+#define VIDEO_DISPLAY_CTRL_VERTICAL_SCALE               BIT(11)
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_SCALE             BIT(10)
+#define VIDEO_DISPLAY_CTRL_VERTICAL_MODE                BIT(9)
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_MODE              BIT(8)
+#define VIDEO_DISPLAY_CTRL_PIXEL_MASK                   (0xf << 4)
+#define VIDEO_DISPLAY_CTRL_GAMMA                        BIT(3)
+#define VIDEO_DISPLAY_CTRL_FORMAT_MASK                  0x3
+#define VIDEO_DISPLAY_CTRL_FORMAT_8                     0x0
+#define VIDEO_DISPLAY_CTRL_FORMAT_16                    0x1
+#define VIDEO_DISPLAY_CTRL_FORMAT_32                    0x2
+#define VIDEO_DISPLAY_CTRL_FORMAT_YUV                   0x3
+
+#define VIDEO_FB_0_ADDRESS                            0x080044
+#define VIDEO_FB_0_ADDRESS_STATUS                     BIT(31)
+#define VIDEO_FB_0_ADDRESS_EXT                        BIT(27)
+#define VIDEO_FB_0_ADDRESS_ADDRESS_MASK               0x3ffffff
+
+#define VIDEO_FB_WIDTH                                0x080048
+#define VIDEO_FB_WIDTH_WIDTH_MASK                     (0x3fff << 16)
+#define VIDEO_FB_WIDTH_OFFSET_MASK                    0x3fff
+
+#define VIDEO_FB_0_LAST_ADDRESS                       0x08004C
+#define VIDEO_FB_0_LAST_ADDRESS_EXT                   BIT(27)
+#define VIDEO_FB_0_LAST_ADDRESS_ADDRESS_MASK          0x3ffffff
+
+#define VIDEO_PLANE_TL                                0x080050
+#define VIDEO_PLANE_TL_TOP_MASK                       (0x7ff << 16)
+#define VIDEO_PLANE_TL_LEFT_MASK                      0x7ff
+
+#define VIDEO_PLANE_BR                                0x080054
+#define VIDEO_PLANE_BR_BOTTOM_MASK                    (0x7ff << 16)
+#define VIDEO_PLANE_BR_RIGHT_MASK                     0x7ff
+
+#define VIDEO_SCALE                                   0x080058
+#define VIDEO_SCALE_VERTICAL_MODE                     BIT(31)
+#define VIDEO_SCALE_VERTICAL_SCALE_MASK               (0xfff << 16)
+#define VIDEO_SCALE_HORIZONTAL_MODE                   BIT(15)
+#define VIDEO_SCALE_HORIZONTAL_SCALE_MASK             0xfff
+
+#define VIDEO_INITIAL_SCALE                           0x08005C
+#define VIDEO_INITIAL_SCALE_FB_1_MASK                 (0xfff << 16)
+#define VIDEO_INITIAL_SCALE_FB_0_MASK                 0xfff
+
+#define VIDEO_YUV_CONSTANTS                           0x080060
+#define VIDEO_YUV_CONSTANTS_Y_MASK                    (0xff << 24)
+#define VIDEO_YUV_CONSTANTS_R_MASK                    (0xff << 16)
+#define VIDEO_YUV_CONSTANTS_G_MASK                    (0xff << 8)
+#define VIDEO_YUV_CONSTANTS_B_MASK                    0xff
+
+#define VIDEO_FB_1_ADDRESS                            0x080064
+#define VIDEO_FB_1_ADDRESS_STATUS                     BIT(31)
+#define VIDEO_FB_1_ADDRESS_EXT                        BIT(27)
+#define VIDEO_FB_1_ADDRESS_ADDRESS_MASK               0x3ffffff
+
+#define VIDEO_FB_1_LAST_ADDRESS                       0x080068
+#define VIDEO_FB_1_LAST_ADDRESS_EXT                   BIT(27)
+#define VIDEO_FB_1_LAST_ADDRESS_ADDRESS_MASK          0x3ffffff
+
+/* Video Alpha Control */
+
+#define VIDEO_ALPHA_DISPLAY_CTRL                        0x080080
+#define VIDEO_ALPHA_DISPLAY_CTRL_SELECT                 BIT(28)
+#define VIDEO_ALPHA_DISPLAY_CTRL_ALPHA_MASK             (0xf << 24)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_MASK              (0x3 << 16)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_1                 (0x0 << 16)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_3                 (0x1 << 16)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_7                 (0x2 << 16)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_11                (0x3 << 16)
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_SCALE             BIT(11)
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_SCALE             BIT(10)
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_MODE              BIT(9)
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_MODE              BIT(8)
+#define VIDEO_ALPHA_DISPLAY_CTRL_PIXEL_MASK             (0xf << 4)
+#define VIDEO_ALPHA_DISPLAY_CTRL_CHROMA_KEY             BIT(3)
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_MASK            0x3
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_8               0x0
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_16              0x1
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4       0x2
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4_4_4   0x3
+
+#define VIDEO_ALPHA_FB_ADDRESS                        0x080084
+#define VIDEO_ALPHA_FB_ADDRESS_STATUS                 BIT(31)
+#define VIDEO_ALPHA_FB_ADDRESS_EXT                    BIT(27)
+#define VIDEO_ALPHA_FB_ADDRESS_ADDRESS_MASK           0x3ffffff
+
+#define VIDEO_ALPHA_FB_WIDTH                          0x080088
+#define VIDEO_ALPHA_FB_WIDTH_WIDTH_MASK               (0x3fff << 16)
+#define VIDEO_ALPHA_FB_WIDTH_OFFSET_MASK              0x3fff
+
+#define VIDEO_ALPHA_FB_LAST_ADDRESS                   0x08008C
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_EXT               BIT(27)
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_ADDRESS_MASK      0x3ffffff
+
+#define VIDEO_ALPHA_PLANE_TL                          0x080090
+#define VIDEO_ALPHA_PLANE_TL_TOP_MASK                 (0x7ff << 16)
+#define VIDEO_ALPHA_PLANE_TL_LEFT_MASK                0x7ff
+
+#define VIDEO_ALPHA_PLANE_BR                          0x080094
+#define VIDEO_ALPHA_PLANE_BR_BOTTOM_MASK              (0x7ff << 16)
+#define VIDEO_ALPHA_PLANE_BR_RIGHT_MASK               0x7ff
+
+#define VIDEO_ALPHA_SCALE                             0x080098
+#define VIDEO_ALPHA_SCALE_VERTICAL_MODE               BIT(31)
+#define VIDEO_ALPHA_SCALE_VERTICAL_SCALE_MASK         (0xfff << 16)
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_MODE             BIT(15)
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_SCALE_MASK       0xfff
+
+#define VIDEO_ALPHA_INITIAL_SCALE                     0x08009C
+#define VIDEO_ALPHA_INITIAL_SCALE_VERTICAL_MASK       (0xfff << 16)
+#define VIDEO_ALPHA_INITIAL_SCALE_HORIZONTAL_MASK     0xfff
+
+#define VIDEO_ALPHA_CHROMA_KEY                        0x0800A0
+#define VIDEO_ALPHA_CHROMA_KEY_MASK_MASK              (0xffff << 16)
+#define VIDEO_ALPHA_CHROMA_KEY_VALUE_MASK             0xffff
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_01                   0x0800A4
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_23                   0x0800A8
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_45                   0x0800AC
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_67                   0x0800B0
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_89                   0x0800B4
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB                   0x0800B8
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD                   0x0800BC
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_BLUE_MASK       0x1f
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF                   0x0800C0
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_MASK            (0xffff << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_RED_MASK        (0x1f << 27)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_GREEN_MASK      (0x3f << 21)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_BLUE_MASK       (0x1f << 16)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_MASK            0xffff
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_RED_MASK        (0x1f << 11)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_GREEN_MASK      (0x3f << 5)
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_BLUE_MASK       0x1f
+
+/* Panel Cursor Control */
+
+#define PANEL_HWC_ADDRESS                             0x0800F0
+#define PANEL_HWC_ADDRESS_ENABLE                      BIT(31)
+#define PANEL_HWC_ADDRESS_EXT                         BIT(27)
+#define PANEL_HWC_ADDRESS_ADDRESS_MASK                0x3ffffff
+
+#define PANEL_HWC_LOCATION                            0x0800F4
+#define PANEL_HWC_LOCATION_TOP                        BIT(27)
+#define PANEL_HWC_LOCATION_Y_MASK                     (0x7ff << 16)
+#define PANEL_HWC_LOCATION_LEFT                       BIT(11)
+#define PANEL_HWC_LOCATION_X_MASK                     0x7ff
+
+#define PANEL_HWC_COLOR_12                            0x0800F8
+#define PANEL_HWC_COLOR_12_2_RGB565_MASK              (0xffff << 16)
+#define PANEL_HWC_COLOR_12_1_RGB565_MASK              0xffff
+
+#define PANEL_HWC_COLOR_3                             0x0800FC
+#define PANEL_HWC_COLOR_3_RGB565_MASK                 0xffff
+
+/* Old Definitions +++ */
+#define PANEL_HWC_COLOR_01                            0x0800F8
+#define PANEL_HWC_COLOR_01_1_RED_MASK                 (0x1f << 27)
+#define PANEL_HWC_COLOR_01_1_GREEN_MASK               (0x3f << 21)
+#define PANEL_HWC_COLOR_01_1_BLUE_MASK                (0x1f << 16)
+#define PANEL_HWC_COLOR_01_0_RED_MASK                 (0x1f << 11)
+#define PANEL_HWC_COLOR_01_0_GREEN_MASK               (0x3f << 5)
+#define PANEL_HWC_COLOR_01_0_BLUE_MASK                0x1f
+
+#define PANEL_HWC_COLOR_2                             0x0800FC
+#define PANEL_HWC_COLOR_2_RED_MASK                    (0x1f << 11)
+#define PANEL_HWC_COLOR_2_GREEN_MASK                  (0x3f << 5)
+#define PANEL_HWC_COLOR_2_BLUE_MASK                   0x1f
+/* Old Definitions --- */
+
+/* Alpha Control */
+
+#define ALPHA_DISPLAY_CTRL                            0x080100
+#define ALPHA_DISPLAY_CTRL_SELECT                     BIT(28)
+#define ALPHA_DISPLAY_CTRL_ALPHA_MASK                 (0xf << 24)
+#define ALPHA_DISPLAY_CTRL_FIFO_MASK                  (0x3 << 16)
+#define ALPHA_DISPLAY_CTRL_FIFO_1                     (0x0 << 16)
+#define ALPHA_DISPLAY_CTRL_FIFO_3                     (0x1 << 16)
+#define ALPHA_DISPLAY_CTRL_FIFO_7                     (0x2 << 16)
+#define ALPHA_DISPLAY_CTRL_FIFO_11                    (0x3 << 16)
+#define ALPHA_DISPLAY_CTRL_PIXEL_MASK                 (0xf << 4)
+#define ALPHA_DISPLAY_CTRL_CHROMA_KEY                 BIT(3)
+#define ALPHA_DISPLAY_CTRL_FORMAT_MASK                0x3
+#define ALPHA_DISPLAY_CTRL_FORMAT_16                  0x1
+#define ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4           0x2
+#define ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4_4_4       0x3
+
+#define ALPHA_FB_ADDRESS                              0x080104
+#define ALPHA_FB_ADDRESS_STATUS                       BIT(31)
+#define ALPHA_FB_ADDRESS_EXT                          BIT(27)
+#define ALPHA_FB_ADDRESS_ADDRESS_MASK                 0x3ffffff
+
+#define ALPHA_FB_WIDTH                                0x080108
+#define ALPHA_FB_WIDTH_WIDTH_MASK                     (0x3fff << 16)
+#define ALPHA_FB_WIDTH_OFFSET_MASK                    0x3fff
+
+#define ALPHA_PLANE_TL                                0x08010C
+#define ALPHA_PLANE_TL_TOP_MASK                       (0x7ff << 16)
+#define ALPHA_PLANE_TL_LEFT_MASK                      0x7ff
+
+#define ALPHA_PLANE_BR                                0x080110
+#define ALPHA_PLANE_BR_BOTTOM_MASK                    (0x7ff << 16)
+#define ALPHA_PLANE_BR_RIGHT_MASK                     0x7ff
+
+#define ALPHA_CHROMA_KEY                              0x080114
+#define ALPHA_CHROMA_KEY_MASK_MASK                    (0xffff << 16)
+#define ALPHA_CHROMA_KEY_VALUE_MASK                   0xffff
+
+#define ALPHA_COLOR_LOOKUP_01                         0x080118
+#define ALPHA_COLOR_LOOKUP_01_1_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_01_1_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_01_1_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_01_1_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_01_0_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_01_0_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_01_0_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_01_0_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_23                         0x08011C
+#define ALPHA_COLOR_LOOKUP_23_3_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_23_3_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_23_3_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_23_3_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_23_2_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_23_2_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_23_2_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_23_2_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_45                         0x080120
+#define ALPHA_COLOR_LOOKUP_45_5_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_45_5_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_45_5_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_45_5_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_45_4_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_45_4_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_45_4_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_45_4_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_67                         0x080124
+#define ALPHA_COLOR_LOOKUP_67_7_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_67_7_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_67_7_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_67_7_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_67_6_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_67_6_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_67_6_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_67_6_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_89                         0x080128
+#define ALPHA_COLOR_LOOKUP_89_9_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_89_9_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_89_9_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_89_9_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_89_8_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_89_8_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_89_8_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_89_8_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_AB                         0x08012C
+#define ALPHA_COLOR_LOOKUP_AB_B_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_AB_B_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_AB_B_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_AB_B_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_AB_A_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_AB_A_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_AB_A_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_AB_A_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_CD                         0x080130
+#define ALPHA_COLOR_LOOKUP_CD_D_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_CD_D_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_CD_D_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_CD_D_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_CD_C_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_CD_C_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_CD_C_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_CD_C_BLUE_MASK             0x1f
+
+#define ALPHA_COLOR_LOOKUP_EF                         0x080134
+#define ALPHA_COLOR_LOOKUP_EF_F_MASK                  (0xffff << 16)
+#define ALPHA_COLOR_LOOKUP_EF_F_RED_MASK              (0x1f << 27)
+#define ALPHA_COLOR_LOOKUP_EF_F_GREEN_MASK            (0x3f << 21)
+#define ALPHA_COLOR_LOOKUP_EF_F_BLUE_MASK             (0x1f << 16)
+#define ALPHA_COLOR_LOOKUP_EF_E_MASK                  0xffff
+#define ALPHA_COLOR_LOOKUP_EF_E_RED_MASK              (0x1f << 11)
+#define ALPHA_COLOR_LOOKUP_EF_E_GREEN_MASK            (0x3f << 5)
+#define ALPHA_COLOR_LOOKUP_EF_E_BLUE_MASK             0x1f
+
+/* CRT Graphics Control */
+
+#define CRT_DISPLAY_CTRL                              0x080200
+#define CRT_DISPLAY_CTRL_RESERVED_MASK                0xfb008200
+
+/* SM750LE definition */
+#define CRT_DISPLAY_CTRL_DPMS_SHIFT                   30
+#define CRT_DISPLAY_CTRL_DPMS_MASK                    (0x3 << 30)
+#define CRT_DISPLAY_CTRL_DPMS_0                       (0x0 << 30)
+#define CRT_DISPLAY_CTRL_DPMS_1                       (0x1 << 30)
+#define CRT_DISPLAY_CTRL_DPMS_2                       (0x2 << 30)
+#define CRT_DISPLAY_CTRL_DPMS_3                       (0x3 << 30)
+#define CRT_DISPLAY_CTRL_CLK_MASK                     (0x7 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL25                    (0x0 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL41                    (0x1 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL62                    (0x2 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL65                    (0x3 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL74                    (0x4 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL80                    (0x5 << 27)
+#define CRT_DISPLAY_CTRL_CLK_PLL108                   (0x6 << 27)
+#define CRT_DISPLAY_CTRL_CLK_RESERVED                 (0x7 << 27)
+#define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC                BIT(26)
+
+/* SM750LE definition */
+#define CRT_DISPLAY_CTRL_CRTSELECT                    BIT(25)
+#define CRT_DISPLAY_CTRL_RGBBIT                       BIT(24)
+
+#ifndef VALIDATION_CHIP
+    #define CRT_DISPLAY_CTRL_CENTERING                BIT(24)
+#endif
+#define CRT_DISPLAY_CTRL_LOCK_TIMING                  BIT(23)
+#define CRT_DISPLAY_CTRL_EXPANSION                    BIT(22)
+#define CRT_DISPLAY_CTRL_VERTICAL_MODE                BIT(21)
+#define CRT_DISPLAY_CTRL_HORIZONTAL_MODE              BIT(20)
+#define CRT_DISPLAY_CTRL_SELECT_SHIFT                 18
+#define CRT_DISPLAY_CTRL_SELECT_MASK                  (0x3 << 18)
+#define CRT_DISPLAY_CTRL_SELECT_PANEL                 (0x0 << 18)
+#define CRT_DISPLAY_CTRL_SELECT_VGA                   (0x1 << 18)
+#define CRT_DISPLAY_CTRL_SELECT_CRT                   (0x2 << 18)
+#define CRT_DISPLAY_CTRL_FIFO_MASK                    (0x3 << 16)
+#define CRT_DISPLAY_CTRL_FIFO_1                       (0x0 << 16)
+#define CRT_DISPLAY_CTRL_FIFO_3                       (0x1 << 16)
+#define CRT_DISPLAY_CTRL_FIFO_7                       (0x2 << 16)
+#define CRT_DISPLAY_CTRL_FIFO_11                      (0x3 << 16)
+#define CRT_DISPLAY_CTRL_BLANK                        BIT(10)
+#define CRT_DISPLAY_CTRL_PIXEL_MASK                   (0xf << 4)
+#define CRT_DISPLAY_CTRL_FORMAT_MASK                  (0x3 << 0)
+#define CRT_DISPLAY_CTRL_FORMAT_8                     (0x0 << 0)
+#define CRT_DISPLAY_CTRL_FORMAT_16                    (0x1 << 0)
+#define CRT_DISPLAY_CTRL_FORMAT_32                    (0x2 << 0)
+
+#define CRT_FB_ADDRESS                                0x080204
+#define CRT_FB_ADDRESS_STATUS                         BIT(31)
+#define CRT_FB_ADDRESS_EXT                            BIT(27)
+#define CRT_FB_ADDRESS_ADDRESS_MASK                   0x3ffffff
+
+#define CRT_FB_WIDTH                                  0x080208
+#define CRT_FB_WIDTH_WIDTH_SHIFT                      16
+#define CRT_FB_WIDTH_WIDTH_MASK                       (0x3fff << 16)
+#define CRT_FB_WIDTH_OFFSET_MASK                      0x3fff
+
+#define CRT_HORIZONTAL_TOTAL                          0x08020C
+#define CRT_HORIZONTAL_TOTAL_TOTAL_SHIFT              16
+#define CRT_HORIZONTAL_TOTAL_TOTAL_MASK               (0xfff << 16)
+#define CRT_HORIZONTAL_TOTAL_DISPLAY_END_MASK         0xfff
+
+#define CRT_HORIZONTAL_SYNC                           0x080210
+#define CRT_HORIZONTAL_SYNC_WIDTH_SHIFT               16
+#define CRT_HORIZONTAL_SYNC_WIDTH_MASK                (0xff << 16)
+#define CRT_HORIZONTAL_SYNC_START_MASK                0xfff
+
+#define CRT_VERTICAL_TOTAL                            0x080214
+#define CRT_VERTICAL_TOTAL_TOTAL_SHIFT                16
+#define CRT_VERTICAL_TOTAL_TOTAL_MASK                 (0x7ff << 16)
+#define CRT_VERTICAL_TOTAL_DISPLAY_END_MASK           (0x7ff)
+
+#define CRT_VERTICAL_SYNC                             0x080218
+#define CRT_VERTICAL_SYNC_HEIGHT_SHIFT                16
+#define CRT_VERTICAL_SYNC_HEIGHT_MASK                 (0x3f << 16)
+#define CRT_VERTICAL_SYNC_START_MASK                  0x7ff
+
+#define CRT_SIGNATURE_ANALYZER                        0x08021C
+#define CRT_SIGNATURE_ANALYZER_STATUS_MASK            (0xffff << 16)
+#define CRT_SIGNATURE_ANALYZER_ENABLE                 BIT(3)
+#define CRT_SIGNATURE_ANALYZER_RESET                  BIT(2)
+#define CRT_SIGNATURE_ANALYZER_SOURCE_MASK            0x3
+#define CRT_SIGNATURE_ANALYZER_SOURCE_RED             0
+#define CRT_SIGNATURE_ANALYZER_SOURCE_GREEN           1
+#define CRT_SIGNATURE_ANALYZER_SOURCE_BLUE            2
+
+#define CRT_CURRENT_LINE                              0x080220
+#define CRT_CURRENT_LINE_LINE_MASK                    0x7ff
+
+#define CRT_MONITOR_DETECT                            0x080224
+#define CRT_MONITOR_DETECT_VALUE                      BIT(25)
+#define CRT_MONITOR_DETECT_ENABLE                     BIT(24)
+#define CRT_MONITOR_DETECT_RED_MASK                   (0xff << 16)
+#define CRT_MONITOR_DETECT_GREEN_MASK                 (0xff << 8)
+#define CRT_MONITOR_DETECT_BLUE_MASK                  0xff
+
+#define CRT_SCALE                                     0x080228
+#define CRT_SCALE_VERTICAL_MODE                       BIT(31)
+#define CRT_SCALE_VERTICAL_SCALE_MASK                 (0xfff << 16)
+#define CRT_SCALE_HORIZONTAL_MODE                     BIT(15)
+#define CRT_SCALE_HORIZONTAL_SCALE_MASK               0xfff
+
+/* CRT Cursor Control */
+
+#define CRT_HWC_ADDRESS                               0x080230
+#define CRT_HWC_ADDRESS_ENABLE                        BIT(31)
+#define CRT_HWC_ADDRESS_EXT                           BIT(27)
+#define CRT_HWC_ADDRESS_ADDRESS_MASK                  0x3ffffff
+
+#define CRT_HWC_LOCATION                              0x080234
+#define CRT_HWC_LOCATION_TOP                          BIT(27)
+#define CRT_HWC_LOCATION_Y_MASK                       (0x7ff << 16)
+#define CRT_HWC_LOCATION_LEFT                         BIT(11)
+#define CRT_HWC_LOCATION_X_MASK                       0x7ff
+
+#define CRT_HWC_COLOR_12                              0x080238
+#define CRT_HWC_COLOR_12_2_RGB565_MASK                (0xffff << 16)
+#define CRT_HWC_COLOR_12_1_RGB565_MASK                0xffff
+
+#define CRT_HWC_COLOR_3                               0x08023C
+#define CRT_HWC_COLOR_3_RGB565_MASK                   0xffff
+
+/* This vertical expansion below start at 0x080240 ~ 0x080264 */
+#define CRT_VERTICAL_EXPANSION                        0x080240
+#ifndef VALIDATION_CHIP
+    #define CRT_VERTICAL_CENTERING_VALUE_MASK         (0xff << 24)
+#endif
+#define CRT_VERTICAL_EXPANSION_COMPARE_VALUE_MASK     (0xff << 16)
+#define CRT_VERTICAL_EXPANSION_LINE_BUFFER_MASK       (0xf << 12)
+#define CRT_VERTICAL_EXPANSION_SCALE_FACTOR_MASK      0xfff
+
+/* This horizontal expansion below start at 0x080268 ~ 0x08027C */
+#define CRT_HORIZONTAL_EXPANSION                      0x080268
+#ifndef VALIDATION_CHIP
+    #define CRT_HORIZONTAL_CENTERING_VALUE_MASK       (0xff << 24)
+#endif
+#define CRT_HORIZONTAL_EXPANSION_COMPARE_VALUE_MASK   (0xff << 16)
+#define CRT_HORIZONTAL_EXPANSION_SCALE_FACTOR_MASK    0xfff
+
+#ifndef VALIDATION_CHIP
+    /* Auto Centering */
+    #define CRT_AUTO_CENTERING_TL                     0x080280
+    #define CRT_AUTO_CENTERING_TL_TOP_MASK            (0x7ff << 16)
+    #define CRT_AUTO_CENTERING_TL_LEFT_MASK           0x7ff
+
+    #define CRT_AUTO_CENTERING_BR                     0x080284
+    #define CRT_AUTO_CENTERING_BR_BOTTOM_MASK         (0x7ff << 16)
+    #define CRT_AUTO_CENTERING_BR_BOTTOM_SHIFT        16
+    #define CRT_AUTO_CENTERING_BR_RIGHT_MASK          0x7ff
+#endif
+
+/* sm750le new register to control panel output */
+#define DISPLAY_CONTROL_750LE			      0x80288
+/* Palette RAM */
+
+/* Panel Palette register starts at 0x080400 ~ 0x0807FC */
+#define PANEL_PALETTE_RAM                             0x080400
+
+/* Panel Palette register starts at 0x080C00 ~ 0x080FFC */
+#define CRT_PALETTE_RAM                               0x080C00
+
+/* Color Space Conversion registers. */
+
+#define CSC_Y_SOURCE_BASE                               0x1000C8
+#define CSC_Y_SOURCE_BASE_EXT                           BIT(27)
+#define CSC_Y_SOURCE_BASE_CS                            BIT(26)
+#define CSC_Y_SOURCE_BASE_ADDRESS_MASK                  0x3ffffff
+
+#define CSC_CONSTANTS                                   0x1000CC
+#define CSC_CONSTANTS_Y_MASK                            (0xff << 24)
+#define CSC_CONSTANTS_R_MASK                            (0xff << 16)
+#define CSC_CONSTANTS_G_MASK                            (0xff << 8)
+#define CSC_CONSTANTS_B_MASK                            0xff
+
+#define CSC_Y_SOURCE_X                                  0x1000D0
+#define CSC_Y_SOURCE_X_INTEGER_MASK                     (0x7ff << 16)
+#define CSC_Y_SOURCE_X_FRACTION_MASK                    (0x1fff << 3)
+
+#define CSC_Y_SOURCE_Y                                  0x1000D4
+#define CSC_Y_SOURCE_Y_INTEGER_MASK                     (0xfff << 16)
+#define CSC_Y_SOURCE_Y_FRACTION_MASK                    (0x1fff << 3)
+
+#define CSC_U_SOURCE_BASE                               0x1000D8
+#define CSC_U_SOURCE_BASE_EXT                           BIT(27)
+#define CSC_U_SOURCE_BASE_CS                            BIT(26)
+#define CSC_U_SOURCE_BASE_ADDRESS_MASK                  0x3ffffff
+
+#define CSC_V_SOURCE_BASE                               0x1000DC
+#define CSC_V_SOURCE_BASE_EXT                           BIT(27)
+#define CSC_V_SOURCE_BASE_CS                            BIT(26)
+#define CSC_V_SOURCE_BASE_ADDRESS_MASK                  0x3ffffff
+
+#define CSC_SOURCE_DIMENSION                            0x1000E0
+#define CSC_SOURCE_DIMENSION_X_MASK                     (0xffff << 16)
+#define CSC_SOURCE_DIMENSION_Y_MASK                     0xffff
+
+#define CSC_SOURCE_PITCH                                0x1000E4
+#define CSC_SOURCE_PITCH_Y_MASK                         (0xffff << 16)
+#define CSC_SOURCE_PITCH_UV_MASK                        0xffff
+
+#define CSC_DESTINATION                                 0x1000E8
+#define CSC_DESTINATION_WRAP                            BIT(31)
+#define CSC_DESTINATION_X_MASK                          (0xfff << 16)
+#define CSC_DESTINATION_Y_MASK                          0xfff
+
+#define CSC_DESTINATION_DIMENSION                       0x1000EC
+#define CSC_DESTINATION_DIMENSION_X_MASK                (0xffff << 16)
+#define CSC_DESTINATION_DIMENSION_Y_MASK                0xffff
+
+#define CSC_DESTINATION_PITCH                           0x1000F0
+#define CSC_DESTINATION_PITCH_X_MASK                    (0xffff << 16)
+#define CSC_DESTINATION_PITCH_Y_MASK                    0xffff
+
+#define CSC_SCALE_FACTOR                                0x1000F4
+#define CSC_SCALE_FACTOR_HORIZONTAL_MASK                (0xffff << 16)
+#define CSC_SCALE_FACTOR_VERTICAL_MASK                  0xffff
+
+#define CSC_DESTINATION_BASE                            0x1000F8
+#define CSC_DESTINATION_BASE_EXT                        BIT(27)
+#define CSC_DESTINATION_BASE_CS                         BIT(26)
+#define CSC_DESTINATION_BASE_ADDRESS_MASK               0x3ffffff
+
+#define CSC_CONTROL                                     0x1000FC
+#define CSC_CONTROL_STATUS                              BIT(31)
+#define CSC_CONTROL_SOURCE_FORMAT_MASK                  (0x7 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_YUV422                (0x0 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_YUV420I               (0x1 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_YUV420                (0x2 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_YVU9                  (0x3 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_IYU1                  (0x4 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_IYU2                  (0x5 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_RGB565                (0x6 << 28)
+#define CSC_CONTROL_SOURCE_FORMAT_RGB8888               (0x7 << 28)
+#define CSC_CONTROL_DESTINATION_FORMAT_MASK             (0x3 << 26)
+#define CSC_CONTROL_DESTINATION_FORMAT_RGB565           (0x0 << 26)
+#define CSC_CONTROL_DESTINATION_FORMAT_RGB8888          (0x1 << 26)
+#define CSC_CONTROL_HORIZONTAL_FILTER                   BIT(25)
+#define CSC_CONTROL_VERTICAL_FILTER                     BIT(24)
+#define CSC_CONTROL_BYTE_ORDER                          BIT(23)
+
+#define DE_DATA_PORT                                    0x110000
+
+#define I2C_BYTE_COUNT                                  0x010040
+#define I2C_BYTE_COUNT_COUNT_MASK                       0xf
+
+#define I2C_CTRL                                        0x010041
+#define I2C_CTRL_INT                                    BIT(4)
+#define I2C_CTRL_DIR                                    BIT(3)
+#define I2C_CTRL_CTRL                                   BIT(2)
+#define I2C_CTRL_MODE                                   BIT(1)
+#define I2C_CTRL_EN                                     BIT(0)
+
+#define I2C_STATUS                                      0x010042
+#define I2C_STATUS_TX                                   BIT(3)
+#define I2C_STATUS_ERR                                  BIT(2)
+#define I2C_STATUS_ACK                                  BIT(1)
+#define I2C_STATUS_BSY                                  BIT(0)
+
+#define I2C_RESET                                       0x010042
+#define I2C_RESET_BUS_ERROR                             BIT(2)
+
+#define I2C_SLAVE_ADDRESS                               0x010043
+#define I2C_SLAVE_ADDRESS_ADDRESS_MASK                  (0x7f << 1)
+#define I2C_SLAVE_ADDRESS_RW                            BIT(0)
+
+#define I2C_DATA0                                       0x010044
+#define I2C_DATA1                                       0x010045
+#define I2C_DATA2                                       0x010046
+#define I2C_DATA3                                       0x010047
+#define I2C_DATA4                                       0x010048
+#define I2C_DATA5                                       0x010049
+#define I2C_DATA6                                       0x01004A
+#define I2C_DATA7                                       0x01004B
+#define I2C_DATA8                                       0x01004C
+#define I2C_DATA9                                       0x01004D
+#define I2C_DATA10                                      0x01004E
+#define I2C_DATA11                                      0x01004F
+#define I2C_DATA12                                      0x010050
+#define I2C_DATA13                                      0x010051
+#define I2C_DATA14                                      0x010052
+#define I2C_DATA15                                      0x010053
+
+#define ZV0_CAPTURE_CTRL                                0x090000
+#define ZV0_CAPTURE_CTRL_FIELD_INPUT                    BIT(27)
+#define ZV0_CAPTURE_CTRL_SCAN                           BIT(26)
+#define ZV0_CAPTURE_CTRL_CURRENT_BUFFER                 BIT(25)
+#define ZV0_CAPTURE_CTRL_VERTICAL_SYNC                  BIT(24)
+#define ZV0_CAPTURE_CTRL_ADJ                            BIT(19)
+#define ZV0_CAPTURE_CTRL_HA                             BIT(18)
+#define ZV0_CAPTURE_CTRL_VSK                            BIT(17)
+#define ZV0_CAPTURE_CTRL_HSK                            BIT(16)
+#define ZV0_CAPTURE_CTRL_FD                             BIT(15)
+#define ZV0_CAPTURE_CTRL_VP                             BIT(14)
+#define ZV0_CAPTURE_CTRL_HP                             BIT(13)
+#define ZV0_CAPTURE_CTRL_CP                             BIT(12)
+#define ZV0_CAPTURE_CTRL_UVS                            BIT(11)
+#define ZV0_CAPTURE_CTRL_BS                             BIT(10)
+#define ZV0_CAPTURE_CTRL_CS                             BIT(9)
+#define ZV0_CAPTURE_CTRL_CF                             BIT(8)
+#define ZV0_CAPTURE_CTRL_FS                             BIT(7)
+#define ZV0_CAPTURE_CTRL_WEAVE                          BIT(6)
+#define ZV0_CAPTURE_CTRL_BOB                            BIT(5)
+#define ZV0_CAPTURE_CTRL_DB                             BIT(4)
+#define ZV0_CAPTURE_CTRL_CC                             BIT(3)
+#define ZV0_CAPTURE_CTRL_RGB                            BIT(2)
+#define ZV0_CAPTURE_CTRL_656                            BIT(1)
+#define ZV0_CAPTURE_CTRL_CAP                            BIT(0)
+
+#define ZV0_CAPTURE_CLIP                                0x090004
+#define ZV0_CAPTURE_CLIP_EYCLIP_MASK                    (0x3ff << 16)
+#define ZV0_CAPTURE_CLIP_XCLIP_MASK                     0x3ff
+
+#define ZV0_CAPTURE_SIZE                                0x090008
+#define ZV0_CAPTURE_SIZE_HEIGHT_MASK                    (0x7ff << 16)
+#define ZV0_CAPTURE_SIZE_WIDTH_MASK                     0x7ff
+
+#define ZV0_CAPTURE_BUF0_ADDRESS                        0x09000C
+#define ZV0_CAPTURE_BUF0_ADDRESS_STATUS                 BIT(31)
+#define ZV0_CAPTURE_BUF0_ADDRESS_EXT                    BIT(27)
+#define ZV0_CAPTURE_BUF0_ADDRESS_CS                     BIT(26)
+#define ZV0_CAPTURE_BUF0_ADDRESS_ADDRESS_MASK           0x3ffffff
+
+#define ZV0_CAPTURE_BUF1_ADDRESS                        0x090010
+#define ZV0_CAPTURE_BUF1_ADDRESS_STATUS                 BIT(31)
+#define ZV0_CAPTURE_BUF1_ADDRESS_EXT                    BIT(27)
+#define ZV0_CAPTURE_BUF1_ADDRESS_CS                     BIT(26)
+#define ZV0_CAPTURE_BUF1_ADDRESS_ADDRESS_MASK           0x3ffffff
+
+#define ZV0_CAPTURE_BUF_OFFSET                          0x090014
+#ifndef VALIDATION_CHIP
+    #define ZV0_CAPTURE_BUF_OFFSET_YCLIP_ODD_FIELD      (0x3ff << 16)
+#endif
+#define ZV0_CAPTURE_BUF_OFFSET_OFFSET_MASK              0xffff
+
+#define ZV0_CAPTURE_FIFO_CTRL                           0x090018
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_MASK                 0x7
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_0                    0
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_1                    1
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_2                    2
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_3                    3
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_4                    4
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_5                    5
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_6                    6
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_7                    7
+
+#define ZV0_CAPTURE_YRGB_CONST                          0x09001C
+#define ZV0_CAPTURE_YRGB_CONST_Y_MASK                   (0xff << 24)
+#define ZV0_CAPTURE_YRGB_CONST_R_MASK                   (0xff << 16)
+#define ZV0_CAPTURE_YRGB_CONST_G_MASK                   (0xff << 8)
+#define ZV0_CAPTURE_YRGB_CONST_B_MASK                   0xff
+
+#define ZV0_CAPTURE_LINE_COMP                           0x090020
+#define ZV0_CAPTURE_LINE_COMP_LC_MASK                   0x7ff
+
+/* ZV1 */
+
+#define ZV1_CAPTURE_CTRL                                0x098000
+#define ZV1_CAPTURE_CTRL_FIELD_INPUT                    BIT(27)
+#define ZV1_CAPTURE_CTRL_SCAN                           BIT(26)
+#define ZV1_CAPTURE_CTRL_CURRENT_BUFFER                 BIT(25)
+#define ZV1_CAPTURE_CTRL_VERTICAL_SYNC                  BIT(24)
+#define ZV1_CAPTURE_CTRL_PANEL                          BIT(20)
+#define ZV1_CAPTURE_CTRL_ADJ                            BIT(19)
+#define ZV1_CAPTURE_CTRL_HA                             BIT(18)
+#define ZV1_CAPTURE_CTRL_VSK                            BIT(17)
+#define ZV1_CAPTURE_CTRL_HSK                            BIT(16)
+#define ZV1_CAPTURE_CTRL_FD                             BIT(15)
+#define ZV1_CAPTURE_CTRL_VP                             BIT(14)
+#define ZV1_CAPTURE_CTRL_HP                             BIT(13)
+#define ZV1_CAPTURE_CTRL_CP                             BIT(12)
+#define ZV1_CAPTURE_CTRL_UVS                            BIT(11)
+#define ZV1_CAPTURE_CTRL_BS                             BIT(10)
+#define ZV1_CAPTURE_CTRL_CS                             BIT(9)
+#define ZV1_CAPTURE_CTRL_CF                             BIT(8)
+#define ZV1_CAPTURE_CTRL_FS                             BIT(7)
+#define ZV1_CAPTURE_CTRL_WEAVE                          BIT(6)
+#define ZV1_CAPTURE_CTRL_BOB                            BIT(5)
+#define ZV1_CAPTURE_CTRL_DB                             BIT(4)
+#define ZV1_CAPTURE_CTRL_CC                             BIT(3)
+#define ZV1_CAPTURE_CTRL_RGB                            BIT(2)
+#define ZV1_CAPTURE_CTRL_656                            BIT(1)
+#define ZV1_CAPTURE_CTRL_CAP                            BIT(0)
+
+#define ZV1_CAPTURE_CLIP                                0x098004
+#define ZV1_CAPTURE_CLIP_YCLIP_MASK                     (0x3ff << 16)
+#define ZV1_CAPTURE_CLIP_XCLIP_MASK                     0x3ff
+
+#define ZV1_CAPTURE_SIZE                                0x098008
+#define ZV1_CAPTURE_SIZE_HEIGHT_MASK                    (0x7ff << 16)
+#define ZV1_CAPTURE_SIZE_WIDTH_MASK                     0x7ff
+
+#define ZV1_CAPTURE_BUF0_ADDRESS                        0x09800C
+#define ZV1_CAPTURE_BUF0_ADDRESS_STATUS                 BIT(31)
+#define ZV1_CAPTURE_BUF0_ADDRESS_EXT                    BIT(27)
+#define ZV1_CAPTURE_BUF0_ADDRESS_CS                     BIT(26)
+#define ZV1_CAPTURE_BUF0_ADDRESS_ADDRESS_MASK           0x3ffffff
+
+#define ZV1_CAPTURE_BUF1_ADDRESS                        0x098010
+#define ZV1_CAPTURE_BUF1_ADDRESS_STATUS                 BIT(31)
+#define ZV1_CAPTURE_BUF1_ADDRESS_EXT                    BIT(27)
+#define ZV1_CAPTURE_BUF1_ADDRESS_CS                     BIT(26)
+#define ZV1_CAPTURE_BUF1_ADDRESS_ADDRESS_MASK           0x3ffffff
+
+#define ZV1_CAPTURE_BUF_OFFSET                          0x098014
+#define ZV1_CAPTURE_BUF_OFFSET_OFFSET_MASK              0xffff
+
+#define ZV1_CAPTURE_FIFO_CTRL                           0x098018
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_MASK                 0x7
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_0                    0
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_1                    1
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_2                    2
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_3                    3
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_4                    4
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_5                    5
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_6                    6
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_7                    7
+
+#define ZV1_CAPTURE_YRGB_CONST                          0x09801C
+#define ZV1_CAPTURE_YRGB_CONST_Y_MASK                   (0xff << 24)
+#define ZV1_CAPTURE_YRGB_CONST_R_MASK                   (0xff << 16)
+#define ZV1_CAPTURE_YRGB_CONST_G_MASK                   (0xff << 8)
+#define ZV1_CAPTURE_YRGB_CONST_B_MASK                   0xff
+
+#define DMA_1_SOURCE                                    0x0D0010
+#define DMA_1_SOURCE_ADDRESS_EXT                        BIT(27)
+#define DMA_1_SOURCE_ADDRESS_CS                         BIT(26)
+#define DMA_1_SOURCE_ADDRESS_MASK                       0x3ffffff
+
+#define DMA_1_DESTINATION                               0x0D0014
+#define DMA_1_DESTINATION_ADDRESS_EXT                   BIT(27)
+#define DMA_1_DESTINATION_ADDRESS_CS                    BIT(26)
+#define DMA_1_DESTINATION_ADDRESS_MASK                  0x3ffffff
+
+#define DMA_1_SIZE_CONTROL                              0x0D0018
+#define DMA_1_SIZE_CONTROL_STATUS                       BIT(31)
+#define DMA_1_SIZE_CONTROL_SIZE_MASK                    0xffffff
+
+#define DMA_ABORT_INTERRUPT                             0x0D0020
+#define DMA_ABORT_INTERRUPT_ABORT_1                     BIT(5)
+#define DMA_ABORT_INTERRUPT_ABORT_0                     BIT(4)
+#define DMA_ABORT_INTERRUPT_INT_1                       BIT(1)
+#define DMA_ABORT_INTERRUPT_INT_0                       BIT(0)
+
+/* Default i2c CLK and Data GPIO. These are the default i2c pins */
+#define DEFAULT_I2C_SCL                     30
+#define DEFAULT_I2C_SDA                     31
+
+#define GPIO_DATA_SM750LE                               0x020018
+#define GPIO_DATA_SM750LE_1                             BIT(1)
+#define GPIO_DATA_SM750LE_0                             BIT(0)
+
+#define GPIO_DATA_DIRECTION_SM750LE                     0x02001C
+#define GPIO_DATA_DIRECTION_SM750LE_1                   BIT(1)
+#define GPIO_DATA_DIRECTION_SM750LE_0                   BIT(0)
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_sii164.c b/drivers/staging/sm750fb/ddk750_sii164.c
new file mode 100644
index 000000000..3da1796cd
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_sii164.c
@@ -0,0 +1,408 @@
+// SPDX-License-Identifier: GPL-2.0
+#define USE_DVICHIP
+#ifdef USE_DVICHIP
+
+#include "ddk750_sii164.h"
+#include "ddk750_hwi2c.h"
+
+/* I2C Address of each SII164 chip */
+#define SII164_I2C_ADDRESS                  0x70
+
+/* Define this definition to use hardware i2c. */
+#define USE_HW_I2C
+
+#ifdef USE_HW_I2C
+    #define i2cWriteReg sm750_hw_i2c_write_reg
+    #define i2cReadReg  sm750_hw_i2c_read_reg
+#else
+    #define i2cWriteReg sm750_sw_i2c_write_reg
+    #define i2cReadReg  sm750_sw_i2c_read_reg
+#endif
+
+/* SII164 Vendor and Device ID */
+#define SII164_VENDOR_ID                    0x0001
+#define SII164_DEVICE_ID                    0x0006
+
+#ifdef SII164_FULL_FUNCTIONS
+/* Name of the DVI Controller chip */
+static char *gDviCtrlChipName = "Silicon Image SiI 164";
+#endif
+
+/*
+ *  sii164_get_vendor_id
+ *      This function gets the vendor ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Vendor ID
+ */
+unsigned short sii164_get_vendor_id(void)
+{
+	unsigned short vendorID;
+
+	vendorID = ((unsigned short)i2cReadReg(SII164_I2C_ADDRESS,
+					       SII164_VENDOR_ID_HIGH) << 8) |
+		   (unsigned short)i2cReadReg(SII164_I2C_ADDRESS,
+					      SII164_VENDOR_ID_LOW);
+
+	return vendorID;
+}
+
+/*
+ *  sii164GetDeviceID
+ *      This function gets the device ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Device ID
+ */
+unsigned short sii164GetDeviceID(void)
+{
+	unsigned short deviceID;
+
+	deviceID = ((unsigned short)i2cReadReg(SII164_I2C_ADDRESS,
+					       SII164_DEVICE_ID_HIGH) << 8) |
+		   (unsigned short)i2cReadReg(SII164_I2C_ADDRESS,
+					      SII164_DEVICE_ID_LOW);
+
+	return deviceID;
+}
+
+/*
+ *  DVI.C will handle all SiI164 chip stuffs and try its best to make code
+ *  minimal and useful
+ */
+
+/*
+ *  sii164InitChip
+ *      This function initialize and detect the DVI controller chip.
+ *
+ *  Input:
+ *      edge_select           - Edge Select:
+ *                               0 = Input data is falling edge latched (falling
+ *                                   edge latched first in dual edge mode)
+ *                               1 = Input data is rising edge latched (rising
+ *                                   edge latched first in dual edge mode)
+ *      bus_select            - Input Bus Select:
+ *                               0 = Input data bus is 12-bits wide
+ *                               1 = Input data bus is 24-bits wide
+ *      dual_edge_clk_select  - Dual Edge Clock Select
+ *                               0 = Input data is single edge latched
+ *                               1 = Input data is dual edge latched
+ *      hsync_enable          - Horizontal Sync Enable:
+ *                               0 = HSYNC input is transmitted as fixed LOW
+ *                               1 = HSYNC input is transmitted as is
+ *      vsync_enable          - Vertical Sync Enable:
+ *                               0 = VSYNC input is transmitted as fixed LOW
+ *                               1 = VSYNC input is transmitted as is
+ *      deskew_enable         - De-skewing Enable:
+ *                               0 = De-skew disabled
+ *                               1 = De-skew enabled
+ *      deskew_setting        - De-skewing Setting (increment of 260psec)
+ *                               0 = 1 step --> minimum setup / maximum hold
+ *                               1 = 2 step
+ *                               2 = 3 step
+ *                               3 = 4 step
+ *                               4 = 5 step
+ *                               5 = 6 step
+ *                               6 = 7 step
+ *                               7 = 8 step --> maximum setup / minimum hold
+ *      continuous_sync_enable- SYNC Continuous:
+ *                               0 = Disable
+ *                               1 = Enable
+ *      pll_filter_enable     - PLL Filter Enable
+ *                               0 = Disable PLL Filter
+ *                               1 = Enable PLL Filter
+ *      pll_filter_value      - PLL Filter characteristics:
+ *                               0~7 (recommended value is 4)
+ *
+ *  Output:
+ *      0   - Success
+ *     -1   - Fail.
+ */
+long sii164InitChip(unsigned char edge_select,
+		    unsigned char bus_select,
+		    unsigned char dual_edge_clk_select,
+		    unsigned char hsync_enable,
+		    unsigned char vsync_enable,
+		    unsigned char deskew_enable,
+		    unsigned char deskew_setting,
+		    unsigned char continuous_sync_enable,
+		    unsigned char pll_filter_enable,
+		    unsigned char pll_filter_value)
+{
+	unsigned char config;
+
+	/* Initialize the i2c bus */
+#ifdef USE_HW_I2C
+	/* Use fast mode. */
+	sm750_hw_i2c_init(1);
+#else
+	sm750_sw_i2c_init(DEFAULT_I2C_SCL, DEFAULT_I2C_SDA);
+#endif
+
+	/* Check if SII164 Chip exists */
+	if ((sii164_get_vendor_id() == SII164_VENDOR_ID) &&
+	    (sii164GetDeviceID() == SII164_DEVICE_ID)) {
+		/*
+		 *  Initialize SII164 controller chip.
+		 */
+
+		/* Select the edge */
+		if (edge_select == 0)
+			config = SII164_CONFIGURATION_LATCH_FALLING;
+		else
+			config = SII164_CONFIGURATION_LATCH_RISING;
+
+		/* Select bus wide */
+		if (bus_select == 0)
+			config |= SII164_CONFIGURATION_BUS_12BITS;
+		else
+			config |= SII164_CONFIGURATION_BUS_24BITS;
+
+		/* Select Dual/Single Edge Clock */
+		if (dual_edge_clk_select == 0)
+			config |= SII164_CONFIGURATION_CLOCK_SINGLE;
+		else
+			config |= SII164_CONFIGURATION_CLOCK_DUAL;
+
+		/* Select HSync Enable */
+		if (hsync_enable == 0)
+			config |= SII164_CONFIGURATION_HSYNC_FORCE_LOW;
+		else
+			config |= SII164_CONFIGURATION_HSYNC_AS_IS;
+
+		/* Select VSync Enable */
+		if (vsync_enable == 0)
+			config |= SII164_CONFIGURATION_VSYNC_FORCE_LOW;
+		else
+			config |= SII164_CONFIGURATION_VSYNC_AS_IS;
+
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+
+		/*
+		 * De-skew enabled with default 111b value.
+		 * This fixes some artifacts problem in some mode on board 2.2.
+		 * Somehow this fix does not affect board 2.1.
+		 */
+		if (deskew_enable == 0)
+			config = SII164_DESKEW_DISABLE;
+		else
+			config = SII164_DESKEW_ENABLE;
+
+		switch (deskew_setting) {
+		case 0:
+			config |= SII164_DESKEW_1_STEP;
+			break;
+		case 1:
+			config |= SII164_DESKEW_2_STEP;
+			break;
+		case 2:
+			config |= SII164_DESKEW_3_STEP;
+			break;
+		case 3:
+			config |= SII164_DESKEW_4_STEP;
+			break;
+		case 4:
+			config |= SII164_DESKEW_5_STEP;
+			break;
+		case 5:
+			config |= SII164_DESKEW_6_STEP;
+			break;
+		case 6:
+			config |= SII164_DESKEW_7_STEP;
+			break;
+		case 7:
+			config |= SII164_DESKEW_8_STEP;
+			break;
+		}
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_DESKEW, config);
+
+		/* Enable/Disable Continuous Sync. */
+		if (continuous_sync_enable == 0)
+			config = SII164_PLL_FILTER_SYNC_CONTINUOUS_DISABLE;
+		else
+			config = SII164_PLL_FILTER_SYNC_CONTINUOUS_ENABLE;
+
+		/* Enable/Disable PLL Filter */
+		if (pll_filter_enable == 0)
+			config |= SII164_PLL_FILTER_DISABLE;
+		else
+			config |= SII164_PLL_FILTER_ENABLE;
+
+		/* Set the PLL Filter value */
+		config |= ((pll_filter_value & 0x07) << 1);
+
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_PLL, config);
+
+		/* Recover from Power Down and enable output. */
+		config = i2cReadReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION);
+		config |= SII164_CONFIGURATION_POWER_NORMAL;
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+
+		return 0;
+	}
+
+	/* Return -1 if initialization fails. */
+	return -1;
+}
+
+/* below sii164 function is not necessary */
+
+#ifdef SII164_FULL_FUNCTIONS
+
+/*
+ *  sii164ResetChip
+ *      This function resets the DVI Controller Chip.
+ */
+void sii164ResetChip(void)
+{
+	/* Power down */
+	sii164SetPower(0);
+	sii164SetPower(1);
+}
+
+/*
+ * sii164GetChipString
+ *      This function returns a char string name of the current DVI Controller
+ *      chip.
+ *
+ *      It's convenient for application need to display the chip name.
+ */
+char *sii164GetChipString(void)
+{
+	return gDviCtrlChipName;
+}
+
+/*
+ *  sii164SetPower
+ *      This function sets the power configuration of the DVI Controller Chip.
+ *
+ *  Input:
+ *      powerUp - Flag to set the power down or up
+ */
+void sii164SetPower(unsigned char powerUp)
+{
+	unsigned char config;
+
+	config = i2cReadReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION);
+	if (powerUp == 1) {
+		/* Power up the chip */
+		config &= ~SII164_CONFIGURATION_POWER_MASK;
+		config |= SII164_CONFIGURATION_POWER_NORMAL;
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+	} else {
+		/* Power down the chip */
+		config &= ~SII164_CONFIGURATION_POWER_MASK;
+		config |= SII164_CONFIGURATION_POWER_DOWN;
+		i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+	}
+}
+
+/*
+ *  sii164SelectHotPlugDetectionMode
+ *      This function selects the mode of the hot plug detection.
+ */
+static
+void sii164SelectHotPlugDetectionMode(enum sii164_hot_plug_mode hotPlugMode)
+{
+	unsigned char detectReg;
+
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		    ~SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG;
+	switch (hotPlugMode) {
+	case SII164_HOTPLUG_DISABLE:
+		detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_HIGH;
+		break;
+	case SII164_HOTPLUG_USE_MDI:
+		detectReg &= ~SII164_DETECT_INTERRUPT_MASK;
+		detectReg |= SII164_DETECT_INTERRUPT_BY_HTPLG_PIN;
+		detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_MDI;
+		break;
+	case SII164_HOTPLUG_USE_RSEN:
+		detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_RSEN;
+		break;
+	case SII164_HOTPLUG_USE_HTPLG:
+		detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_HTPLG;
+		break;
+	}
+
+	i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT, detectReg);
+}
+
+/*
+ *  sii164EnableHotPlugDetection
+ *      This function enables the Hot Plug detection.
+ *
+ *  enableHotPlug   - Enable (=1) / disable (=0) Hot Plug detection
+ */
+void sii164EnableHotPlugDetection(unsigned char enableHotPlug)
+{
+	unsigned char detectReg;
+
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT);
+
+	/* Depending on each DVI controller, need to enable the hot plug based
+	 * on each individual chip design.
+	 */
+	if (enableHotPlug != 0)
+		sii164SelectHotPlugDetectionMode(SII164_HOTPLUG_USE_MDI);
+	else
+		sii164SelectHotPlugDetectionMode(SII164_HOTPLUG_DISABLE);
+}
+
+/*
+ *  sii164IsConnected
+ *      Check if the DVI Monitor is connected.
+ *
+ *  Output:
+ *      0   - Not Connected
+ *      1   - Connected
+ */
+unsigned char sii164IsConnected(void)
+{
+	unsigned char hotPlugValue;
+
+	hotPlugValue = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		       SII164_DETECT_HOT_PLUG_STATUS_MASK;
+	if (hotPlugValue == SII164_DETECT_HOT_PLUG_STATUS_ON)
+		return 1;
+	else
+		return 0;
+}
+
+/*
+ *  sii164CheckInterrupt
+ *      Checks if interrupt has occurred.
+ *
+ *  Output:
+ *      0   - No interrupt
+ *      1   - Interrupt occurs
+ */
+unsigned char sii164CheckInterrupt(void)
+{
+	unsigned char detectReg;
+
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		    SII164_DETECT_MONITOR_STATE_MASK;
+	if (detectReg == SII164_DETECT_MONITOR_STATE_CHANGE)
+		return 1;
+	else
+		return 0;
+}
+
+/*
+ *  sii164ClearInterrupt
+ *      Clear the hot plug interrupt.
+ */
+void sii164ClearInterrupt(void)
+{
+	unsigned char detectReg;
+
+	/* Clear the MDI interrupt */
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT);
+	i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT,
+		    detectReg | SII164_DETECT_MONITOR_STATE_CLEAR);
+}
+
+#endif
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_sii164.h b/drivers/staging/sm750fb/ddk750_sii164.h
new file mode 100644
index 000000000..ca330f6a4
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_sii164.h
@@ -0,0 +1,174 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef DDK750_SII164_H__
+#define DDK750_SII164_H__
+
+#define USE_DVICHIP
+
+/* Hot Plug detection mode structure */
+enum sii164_hot_plug_mode {
+	SII164_HOTPLUG_DISABLE = 0,	/* Disable Hot Plug output bit
+					 * (always high).
+					 */
+
+	SII164_HOTPLUG_USE_MDI,         /* Use Monitor Detect Interrupt bit. */
+	SII164_HOTPLUG_USE_RSEN,        /* Use Receiver Sense detect bit. */
+	SII164_HOTPLUG_USE_HTPLG        /* Use Hot Plug detect bit. */
+};
+
+/* Silicon Image SiI164 chip prototype */
+long sii164InitChip(unsigned char edgeSelect,
+		    unsigned char busSelect,
+		    unsigned char dualEdgeClkSelect,
+		    unsigned char hsyncEnable,
+		    unsigned char vsyncEnable,
+		    unsigned char deskewEnable,
+		    unsigned char deskewSetting,
+		    unsigned char continuousSyncEnable,
+		    unsigned char pllFilterEnable,
+		    unsigned char pllFilterValue);
+
+unsigned short sii164_get_vendor_id(void);
+unsigned short sii164GetDeviceID(void);
+
+#ifdef SII164_FULL_FUNCTIONS
+void sii164ResetChip(void);
+char *sii164GetChipString(void);
+void sii164SetPower(unsigned char powerUp);
+void sii164EnableHotPlugDetection(unsigned char enableHotPlug);
+unsigned char sii164IsConnected(void);
+unsigned char sii164CheckInterrupt(void);
+void sii164ClearInterrupt(void);
+#endif
+/*
+ * below register definition is used for
+ * Silicon Image SiI164 DVI controller chip
+ */
+/*
+ * Vendor ID registers
+ */
+#define SII164_VENDOR_ID_LOW                        0x00
+#define SII164_VENDOR_ID_HIGH                       0x01
+
+/*
+ * Device ID registers
+ */
+#define SII164_DEVICE_ID_LOW                        0x02
+#define SII164_DEVICE_ID_HIGH                       0x03
+
+/*
+ * Device Revision
+ */
+#define SII164_DEVICE_REVISION                      0x04
+
+/*
+ * Frequency Limitation registers
+ */
+#define SII164_FREQUENCY_LIMIT_LOW                  0x06
+#define SII164_FREQUENCY_LIMIT_HIGH                 0x07
+
+/*
+ * Power Down and Input Signal Configuration registers
+ */
+#define SII164_CONFIGURATION                        0x08
+
+/* Power down (PD) */
+#define SII164_CONFIGURATION_POWER_DOWN             0x00
+#define SII164_CONFIGURATION_POWER_NORMAL           0x01
+#define SII164_CONFIGURATION_POWER_MASK             0x01
+
+/* Input Edge Latch Select (EDGE) */
+#define SII164_CONFIGURATION_LATCH_FALLING          0x00
+#define SII164_CONFIGURATION_LATCH_RISING           0x02
+
+/* Bus Select (BSEL) */
+#define SII164_CONFIGURATION_BUS_12BITS             0x00
+#define SII164_CONFIGURATION_BUS_24BITS             0x04
+
+/* Dual Edge Clock Select (DSEL) */
+#define SII164_CONFIGURATION_CLOCK_SINGLE           0x00
+#define SII164_CONFIGURATION_CLOCK_DUAL             0x08
+
+/* Horizontal Sync Enable (HEN) */
+#define SII164_CONFIGURATION_HSYNC_FORCE_LOW        0x00
+#define SII164_CONFIGURATION_HSYNC_AS_IS            0x10
+
+/* Vertical Sync Enable (VEN) */
+#define SII164_CONFIGURATION_VSYNC_FORCE_LOW        0x00
+#define SII164_CONFIGURATION_VSYNC_AS_IS            0x20
+
+/*
+ * Detection registers
+ */
+#define SII164_DETECT                               0x09
+
+/* Monitor Detect Interrupt (MDI) */
+#define SII164_DETECT_MONITOR_STATE_CHANGE          0x00
+#define SII164_DETECT_MONITOR_STATE_NO_CHANGE       0x01
+#define SII164_DETECT_MONITOR_STATE_CLEAR           0x01
+#define SII164_DETECT_MONITOR_STATE_MASK            0x01
+
+/* Hot Plug detect Input (HTPLG) */
+#define SII164_DETECT_HOT_PLUG_STATUS_OFF           0x00
+#define SII164_DETECT_HOT_PLUG_STATUS_ON            0x02
+#define SII164_DETECT_HOT_PLUG_STATUS_MASK          0x02
+
+/* Receiver Sense (RSEN) */
+#define SII164_DETECT_RECEIVER_SENSE_NOT_DETECTED   0x00
+#define SII164_DETECT_RECEIVER_SENSE_DETECTED       0x04
+
+/* Interrupt Generation Method (TSEL) */
+#define SII164_DETECT_INTERRUPT_BY_RSEN_PIN         0x00
+#define SII164_DETECT_INTERRUPT_BY_HTPLG_PIN        0x08
+#define SII164_DETECT_INTERRUPT_MASK                0x08
+
+/* Monitor Sense Output (MSEN) */
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_HIGH     0x00
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_MDI      0x10
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_RSEN     0x20
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_HTPLG    0x30
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG     0x30
+
+/*
+ * Skewing registers
+ */
+#define SII164_DESKEW                               0x0A
+
+/* General Purpose Input (CTL[3:1]) */
+#define SII164_DESKEW_GENERAL_PURPOSE_INPUT_MASK    0x0E
+
+/* De-skewing Enable bit (DKEN) */
+#define SII164_DESKEW_DISABLE                       0x00
+#define SII164_DESKEW_ENABLE                        0x10
+
+/* De-skewing Setting (DK[3:1])*/
+#define SII164_DESKEW_1_STEP                        0x00
+#define SII164_DESKEW_2_STEP                        0x20
+#define SII164_DESKEW_3_STEP                        0x40
+#define SII164_DESKEW_4_STEP                        0x60
+#define SII164_DESKEW_5_STEP                        0x80
+#define SII164_DESKEW_6_STEP                        0xA0
+#define SII164_DESKEW_7_STEP                        0xC0
+#define SII164_DESKEW_8_STEP                        0xE0
+
+/*
+ * User Configuration Data registers (CFG 7:0)
+ */
+#define SII164_USER_CONFIGURATION                   0x0B
+
+/*
+ * PLL registers
+ */
+#define SII164_PLL                                  0x0C
+
+/* PLL Filter Value (PLLF) */
+#define SII164_PLL_FILTER_VALUE_MASK                0x0E
+
+/* PLL Filter Enable (PFEN) */
+#define SII164_PLL_FILTER_DISABLE                   0x00
+#define SII164_PLL_FILTER_ENABLE                    0x01
+
+/* Sync Continuous (SCNT) */
+#define SII164_PLL_FILTER_SYNC_CONTINUOUS_DISABLE   0x00
+#define SII164_PLL_FILTER_SYNC_CONTINUOUS_ENABLE    0x80
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.c b/drivers/staging/sm750fb/ddk750_swi2c.c
new file mode 100644
index 000000000..0ef8d4ff2
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_swi2c.c
@@ -0,0 +1,504 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ *         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+ *
+ *  swi2c.c --- SM750/SM718 DDK
+ *  This file contains the source code for I2C using software
+ *  implementation.
+ */
+
+#include "ddk750_chip.h"
+#include "ddk750_reg.h"
+#include "ddk750_swi2c.h"
+#include "ddk750_power.h"
+
+/*
+ * I2C Software Master Driver:
+ * ===========================
+ * Each i2c cycle is split into 4 sections. Each of these section marks
+ * a point in time where the SCL or SDA may be changed.
+ *
+ * 1 Cycle == |  Section I. |  Section 2. |  Section 3. |  Section 4. |
+ *            +-------------+-------------+-------------+-------------+
+ *            | SCL set LOW |SCL no change| SCL set HIGH|SCL no change|
+ *
+ *                                          ____________ _____________
+ * SCL == XXXX _____________ ____________ /
+ *
+ * I.e. the SCL may only be changed in section 1. and section 3. while
+ * the SDA may only be changed in section 2. and section 4. The table
+ * below gives the changes for these 2 lines in the varios sections.
+ *
+ * Section changes Table:
+ * ======================
+ * blank = no change, L = set bit LOW, H = set bit HIGH
+ *
+ *                                | 1.| 2.| 3.| 4.|
+ *                 ---------------+---+---+---+---+
+ *                 Tx Start   SDA |   | H |   | L |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx Stop    SDA |   | L |   | H |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx bit H   SDA |   | H |   |   |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx bit L   SDA |   | L |   |   |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *
+ */
+
+/* GPIO pins used for this I2C. It ranges from 0 to 63. */
+static unsigned char sw_i2c_clk_gpio = DEFAULT_I2C_SCL;
+static unsigned char sw_i2c_data_gpio = DEFAULT_I2C_SDA;
+
+/*
+ *  Below is the variable declaration for the GPIO pin register usage
+ *  for the i2c Clock and i2c Data.
+ *
+ *  Note:
+ *      Notice that the GPIO usage for the i2c clock and i2c Data are
+ *      separated. This is to make this code flexible enough when
+ *      two separate GPIO pins for the clock and data are located
+ *      in two different GPIO register set (worst case).
+ */
+
+/* i2c Clock GPIO Register usage */
+static unsigned long sw_i2c_clk_gpio_mux_reg = GPIO_MUX;
+static unsigned long sw_i2c_clk_gpio_data_reg = GPIO_DATA;
+static unsigned long sw_i2c_clk_gpio_data_dir_reg = GPIO_DATA_DIRECTION;
+
+/* i2c Data GPIO Register usage */
+static unsigned long sw_i2c_data_gpio_mux_reg = GPIO_MUX;
+static unsigned long sw_i2c_data_gpio_data_reg = GPIO_DATA;
+static unsigned long sw_i2c_data_gpio_data_dir_reg = GPIO_DATA_DIRECTION;
+
+/*
+ *  This function puts a delay between command
+ */
+static void sw_i2c_wait(void)
+{
+	/* find a bug:
+	 * peekIO method works well before suspend/resume
+	 * but after suspend, peekIO(0x3ce,0x61) & 0x10
+	 * always be non-zero,which makes the while loop
+	 * never finish.
+	 * use non-ultimate for loop below is safe
+	 */
+
+    /* Change wait algorithm to use PCI bus clock,
+     * it's more reliable than counter loop ..
+     * write 0x61 to 0x3ce and read from 0x3cf
+     */
+	int i, tmp;
+
+	for (i = 0; i < 600; i++) {
+		tmp = i;
+		tmp += i;
+	}
+}
+
+/*
+ *  This function set/reset the SCL GPIO pin
+ *
+ *  Parameters:
+ *      value    - Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ *
+ *  Notes:
+ *      When setting SCL to high, just set the GPIO as input where the pull up
+ *      resistor will pull the signal up. Do not use software to pull up the
+ *      signal because the i2c will fail when other device try to drive the
+ *      signal due to SM50x will drive the signal to always high.
+ */
+static void sw_i2c_scl(unsigned char value)
+{
+	unsigned long gpio_data;
+	unsigned long gpio_dir;
+
+	gpio_dir = peek32(sw_i2c_clk_gpio_data_dir_reg);
+	if (value) {    /* High */
+		/*
+		 * Set direction as input. This will automatically
+		 * pull the signal up.
+		 */
+		gpio_dir &= ~(1 << sw_i2c_clk_gpio);
+		poke32(sw_i2c_clk_gpio_data_dir_reg, gpio_dir);
+	} else {        /* Low */
+		/* Set the signal down */
+		gpio_data = peek32(sw_i2c_clk_gpio_data_reg);
+		gpio_data &= ~(1 << sw_i2c_clk_gpio);
+		poke32(sw_i2c_clk_gpio_data_reg, gpio_data);
+
+		/* Set direction as output */
+		gpio_dir |= (1 << sw_i2c_clk_gpio);
+		poke32(sw_i2c_clk_gpio_data_dir_reg, gpio_dir);
+	}
+}
+
+/*
+ *  This function set/reset the SDA GPIO pin
+ *
+ *  Parameters:
+ *      value    - Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ *
+ *  Notes:
+ *      When setting SCL to high, just set the GPIO as input where the pull up
+ *      resistor will pull the signal up. Do not use software to pull up the
+ *      signal because the i2c will fail when other device try to drive the
+ *      signal due to SM50x will drive the signal to always high.
+ */
+static void sw_i2c_sda(unsigned char value)
+{
+	unsigned long gpio_data;
+	unsigned long gpio_dir;
+
+	gpio_dir = peek32(sw_i2c_data_gpio_data_dir_reg);
+	if (value) {    /* High */
+		/*
+		 * Set direction as input. This will automatically
+		 * pull the signal up.
+		 */
+		gpio_dir &= ~(1 << sw_i2c_data_gpio);
+		poke32(sw_i2c_data_gpio_data_dir_reg, gpio_dir);
+	} else {        /* Low */
+		/* Set the signal down */
+		gpio_data = peek32(sw_i2c_data_gpio_data_reg);
+		gpio_data &= ~(1 << sw_i2c_data_gpio);
+		poke32(sw_i2c_data_gpio_data_reg, gpio_data);
+
+		/* Set direction as output */
+		gpio_dir |= (1 << sw_i2c_data_gpio);
+		poke32(sw_i2c_data_gpio_data_dir_reg, gpio_dir);
+	}
+}
+
+/*
+ *  This function read the data from the SDA GPIO pin
+ *
+ *  Return Value:
+ *      The SDA data bit sent by the Slave
+ */
+static unsigned char sw_i2c_read_sda(void)
+{
+	unsigned long gpio_dir;
+	unsigned long gpio_data;
+	unsigned long dir_mask = 1 << sw_i2c_data_gpio;
+
+	/* Make sure that the direction is input (High) */
+	gpio_dir = peek32(sw_i2c_data_gpio_data_dir_reg);
+	if ((gpio_dir & dir_mask) != ~dir_mask) {
+		gpio_dir &= ~(1 << sw_i2c_data_gpio);
+		poke32(sw_i2c_data_gpio_data_dir_reg, gpio_dir);
+	}
+
+	/* Now read the SDA line */
+	gpio_data = peek32(sw_i2c_data_gpio_data_reg);
+	if (gpio_data & (1 << sw_i2c_data_gpio))
+		return 1;
+	else
+		return 0;
+}
+
+/*
+ *  This function sends ACK signal
+ */
+static void sw_i2c_ack(void)
+{
+	return;  /* Single byte read is ok without it. */
+}
+
+/*
+ *  This function sends the start command to the slave device
+ */
+static void sw_i2c_start(void)
+{
+	/* Start I2C */
+	sw_i2c_sda(1);
+	sw_i2c_scl(1);
+	sw_i2c_sda(0);
+}
+
+/*
+ *  This function sends the stop command to the slave device
+ */
+static void sw_i2c_stop(void)
+{
+	/* Stop the I2C */
+	sw_i2c_scl(1);
+	sw_i2c_sda(0);
+	sw_i2c_sda(1);
+}
+
+/*
+ *  This function writes one byte to the slave device
+ *
+ *  Parameters:
+ *      data    - Data to be write to the slave device
+ *
+ *  Return Value:
+ *       0   - Success
+ *      -1   - Fail to write byte
+ */
+static long sw_i2c_write_byte(unsigned char data)
+{
+	unsigned char value = data;
+	int i;
+
+	/* Sending the data bit by bit */
+	for (i = 0; i < 8; i++) {
+		/* Set SCL to low */
+		sw_i2c_scl(0);
+
+		/* Send data bit */
+		if ((value & 0x80) != 0)
+			sw_i2c_sda(1);
+		else
+			sw_i2c_sda(0);
+
+		sw_i2c_wait();
+
+		/* Toggle clk line to one */
+		sw_i2c_scl(1);
+		sw_i2c_wait();
+
+		/* Shift byte to be sent */
+		value = value << 1;
+	}
+
+	/* Set the SCL Low and SDA High (prepare to get input) */
+	sw_i2c_scl(0);
+	sw_i2c_sda(1);
+
+	/* Set the SCL High for ack */
+	sw_i2c_wait();
+	sw_i2c_scl(1);
+	sw_i2c_wait();
+
+	/* Read SDA, until SDA==0 */
+	for (i = 0; i < 0xff; i++) {
+		if (!sw_i2c_read_sda())
+			break;
+
+		sw_i2c_scl(0);
+		sw_i2c_wait();
+		sw_i2c_scl(1);
+		sw_i2c_wait();
+	}
+
+	/* Set the SCL Low and SDA High */
+	sw_i2c_scl(0);
+	sw_i2c_sda(1);
+
+	if (i < 0xff)
+		return 0;
+	else
+		return -1;
+}
+
+/*
+ *  This function reads one byte from the slave device
+ *
+ *  Parameters:
+ *      ack    - Flag to indicate either to send the acknowledge
+ *            message to the slave device or not
+ *
+ *  Return Value:
+ *      One byte data read from the Slave device
+ */
+static unsigned char sw_i2c_read_byte(unsigned char ack)
+{
+	int i;
+	unsigned char data = 0;
+
+	for (i = 7; i >= 0; i--) {
+		/* Set the SCL to Low and SDA to High (Input) */
+		sw_i2c_scl(0);
+		sw_i2c_sda(1);
+		sw_i2c_wait();
+
+		/* Set the SCL High */
+		sw_i2c_scl(1);
+		sw_i2c_wait();
+
+		/* Read data bits from SDA */
+		data |= (sw_i2c_read_sda() << i);
+	}
+
+	if (ack)
+		sw_i2c_ack();
+
+	/* Set the SCL Low and SDA High */
+	sw_i2c_scl(0);
+	sw_i2c_sda(1);
+
+	return data;
+}
+
+/*
+ * This function initializes GPIO port for SW I2C communication.
+ *
+ * Parameters:
+ *      clk_gpio      - The GPIO pin to be used as i2c SCL
+ *      data_gpio     - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+static long sm750le_i2c_init(unsigned char clk_gpio, unsigned char data_gpio)
+{
+	int i;
+
+	/* Initialize the GPIO pin for the i2c Clock Register */
+	sw_i2c_clk_gpio_data_reg = GPIO_DATA_SM750LE;
+	sw_i2c_clk_gpio_data_dir_reg = GPIO_DATA_DIRECTION_SM750LE;
+
+	/* Initialize the Clock GPIO Offset */
+	sw_i2c_clk_gpio = clk_gpio;
+
+	/* Initialize the GPIO pin for the i2c Data Register */
+	sw_i2c_data_gpio_data_reg = GPIO_DATA_SM750LE;
+	sw_i2c_data_gpio_data_dir_reg = GPIO_DATA_DIRECTION_SM750LE;
+
+	/* Initialize the Data GPIO Offset */
+	sw_i2c_data_gpio = data_gpio;
+
+	/* Note that SM750LE don't have GPIO MUX and power is always on */
+
+	/* Clear the i2c lines. */
+	for (i = 0; i < 9; i++)
+		sw_i2c_stop();
+
+	return 0;
+}
+
+/*
+ * This function initializes the i2c attributes and bus
+ *
+ * Parameters:
+ *      clk_gpio      - The GPIO pin to be used as i2c SCL
+ *      data_gpio     - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+long sm750_sw_i2c_init(unsigned char clk_gpio, unsigned char data_gpio)
+{
+	int i;
+
+	/*
+	 * Return 0 if the GPIO pins to be used is out of range. The
+	 * range is only from [0..63]
+	 */
+	if ((clk_gpio > 31) || (data_gpio > 31))
+		return -1;
+
+	if (sm750_get_chip_type() == SM750LE)
+		return sm750le_i2c_init(clk_gpio, data_gpio);
+
+	/* Initialize the GPIO pin for the i2c Clock Register */
+	sw_i2c_clk_gpio_mux_reg = GPIO_MUX;
+	sw_i2c_clk_gpio_data_reg = GPIO_DATA;
+	sw_i2c_clk_gpio_data_dir_reg = GPIO_DATA_DIRECTION;
+
+	/* Initialize the Clock GPIO Offset */
+	sw_i2c_clk_gpio = clk_gpio;
+
+	/* Initialize the GPIO pin for the i2c Data Register */
+	sw_i2c_data_gpio_mux_reg = GPIO_MUX;
+	sw_i2c_data_gpio_data_reg = GPIO_DATA;
+	sw_i2c_data_gpio_data_dir_reg = GPIO_DATA_DIRECTION;
+
+	/* Initialize the Data GPIO Offset */
+	sw_i2c_data_gpio = data_gpio;
+
+	/* Enable the GPIO pins for the i2c Clock and Data (GPIO MUX) */
+	poke32(sw_i2c_clk_gpio_mux_reg,
+	       peek32(sw_i2c_clk_gpio_mux_reg) & ~(1 << sw_i2c_clk_gpio));
+	poke32(sw_i2c_data_gpio_mux_reg,
+	       peek32(sw_i2c_data_gpio_mux_reg) & ~(1 << sw_i2c_data_gpio));
+
+	/* Enable GPIO power */
+	sm750_enable_gpio(1);
+
+	/* Clear the i2c lines. */
+	for (i = 0; i < 9; i++)
+		sw_i2c_stop();
+
+	return 0;
+}
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      addr   - i2c Slave device address which register
+ *                        to be read from
+ *      reg    - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char sm750_sw_i2c_read_reg(unsigned char addr, unsigned char reg)
+{
+	unsigned char data;
+
+	/* Send the Start signal */
+	sw_i2c_start();
+
+	/* Send the device address */
+	sw_i2c_write_byte(addr);
+
+	/* Send the register index */
+	sw_i2c_write_byte(reg);
+
+	/* Get the bus again and get the data from the device read address */
+	sw_i2c_start();
+	sw_i2c_write_byte(addr + 1);
+	data = sw_i2c_read_byte(1);
+
+	/* Stop swI2C and release the bus */
+	sw_i2c_stop();
+
+	return data;
+}
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      addr            - i2c Slave device address which register
+ *                        to be written
+ *      reg             - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+long sm750_sw_i2c_write_reg(unsigned char addr,
+			    unsigned char reg,
+			    unsigned char data)
+{
+	long ret = 0;
+
+	/* Send the Start signal */
+	sw_i2c_start();
+
+	/* Send the device address and read the data. All should return success
+	 * in order for the writing processed to be successful
+	 */
+	if ((sw_i2c_write_byte(addr) != 0) ||
+	    (sw_i2c_write_byte(reg) != 0) ||
+	    (sw_i2c_write_byte(data) != 0)) {
+		ret = -1;
+	}
+
+	/* Stop i2c and release the bus */
+	sw_i2c_stop();
+
+	return ret;
+}
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.h b/drivers/staging/sm750fb/ddk750_swi2c.h
new file mode 100644
index 000000000..dfa166060
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_swi2c.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+ *
+ *  swi2c.h --- SM750/SM718 DDK
+ *  This file contains the definitions for i2c using software
+ *  implementation.
+ *
+ */
+
+#ifndef _SWI2C_H_
+#define _SWI2C_H_
+
+/* Default i2c CLK and Data GPIO. These are the default i2c pins */
+#define DEFAULT_I2C_SCL                     30
+#define DEFAULT_I2C_SDA                     31
+
+/*
+ * This function initializes the i2c attributes and bus
+ *
+ * Parameters:
+ *      i2cClkGPIO  - The GPIO pin to be used as i2c SCL
+ *      i2cDataGPIO - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+long sm750_sw_i2c_init(unsigned char clk_gpio, unsigned char data_gpio);
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      addr   - i2c Slave device address which register
+ *                        to be read from
+ *      reg    - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char sm750_sw_i2c_read_reg(unsigned char addr, unsigned char reg);
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      addr            - i2c Slave device address which register
+ *                        to be written
+ *      reg             - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+long sm750_sw_i2c_write_reg(unsigned char addr,
+			    unsigned char reg,
+			    unsigned char data);
+
+#endif  /* _SWI2C_H_ */
diff --git a/drivers/staging/sm750fb/readme b/drivers/staging/sm750fb/readme
new file mode 100644
index 000000000..cfa45958b
--- /dev/null
+++ b/drivers/staging/sm750fb/readme
@@ -0,0 +1,38 @@
+Introduction:
+	SM750 of Silicon MOtion is pci express display controller device.
+	The SM750 embedded graphics features include:
+	- dual display
+	- 2D acceleration
+	- 16MB integrated video memory
+
+About the kernel module parameter of driver:
+
+	Use 1280,8bpp index color and 60 hz mode:
+	insmod ./sm750fb.ko g_option="1280x1024-8@60"
+
+	Disable MTRR,Disable 2d acceleration,Disable hardware cursor,
+	and use a 800x600 mode :
+	insmod ./sm750fb.ko g_option="noaccel:nomtrr:nohwc:800x600"
+
+	dual frame buffer for driver with "dual" parameter
+	insmod ./sm750fb.ko g_option="dual,800x600:1024x768"
+	it will create fb0 and fb1 (or fb1,fb2 if fb0 already exist) under /dev
+	and user can use con2fb to link fbX and ttyX
+
+	Notes:
+	1) if you build the driver with built-in method, the parameter
+		you edited in the grub config file will be also the
+		same format as above modular method,but additionally add
+		"video=sm750fb:"
+		ahead of parameters,so,it looks like:
+		video=sm750fb:noaccel,1280x1024@60,otherparam,etc...
+		it equal to modular method with below command:
+		insmod ./sm750fb.ko g_option="noaccel:1280x1024@60:otherparm:etc..."
+
+	2) if you put 800x600 into the parameter without bpp and
+		refresh rate, kernel driver will defaulty use 16bpp and 60hz
+
+Important:
+	if you have vesafb enabled in your config then /dev/fb0 will be created by vesafb
+	and this driver will use fb1, fb2. In that case, you need to configure your X-server
+	to use fb1. Another simple althernative is to disable vesafb from your config.
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
new file mode 100644
index 000000000..168ae2e90
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750.c
@@ -0,0 +1,1204 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/aperture.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/mm_types.h>
+#include <linux/vmalloc.h>
+#include <linux/pagemap.h>
+#include <linux/screen_info.h>
+#include <linux/console.h>
+#include <asm/fb.h>
+#include "sm750.h"
+#include "sm750_accel.h"
+#include "sm750_cursor.h"
+
+/*
+ * #ifdef __BIG_ENDIAN
+ * ssize_t lynxfb_ops_write(struct fb_info *info, const char __user *buf,
+ * size_t count, loff_t *ppos);
+ * ssize_t lynxfb_ops_read(struct fb_info *info, char __user *buf,
+ * size_t count, loff_t *ppos);
+ * #endif
+ */
+
+/* common var for all device */
+static int g_hwcursor = 1;
+static int g_noaccel;
+static int g_nomtrr;
+static const char *g_fbmode[] = {NULL, NULL};
+static const char *g_def_fbmode = "1024x768-32@60";
+static char *g_settings;
+static int g_dualview;
+static char *g_option;
+
+static const struct fb_videomode lynx750_ext[] = {
+	/*	1024x600-60 VESA	[1.71:1] */
+	{NULL,  60, 1024, 600, 20423, 144,  40, 18, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-70 VESA */
+	{NULL,  70, 1024, 600, 17211, 152,  48, 21, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-75 VESA */
+	{NULL,  75, 1024, 600, 15822, 160,  56, 23, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-85 VESA */
+	{NULL,  85, 1024, 600, 13730, 168,  56, 26, 1, 112, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	720x480	*/
+	{NULL, 60,  720,  480,  37427, 88,   16, 13, 1,   72,  3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1280x720		[1.78:1]	*/
+	{NULL, 60,  1280,  720,  13426, 162, 86, 22, 1,  136, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1280x768@60 */
+	{NULL, 60, 1280, 768, 12579, 192, 64, 20, 3, 128, 7,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1360 x 768	[1.77083:1]	*/
+	{NULL,  60, 1360, 768, 11804, 208,  64, 23, 1, 144, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1368 x 768      [1.78:1]	*/
+	{NULL, 60,  1368,  768,  11647, 216, 72, 23, 1,  144, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1440 x 900		[16:10]	*/
+	{NULL, 60, 1440, 900, 9392, 232, 80, 28, 1, 152, 3,
+	 FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1440x960		[15:10]	*/
+	{NULL, 60, 1440, 960, 8733, 240, 88, 30, 1, 152, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1920x1080	[16:9]	*/
+	{NULL, 60, 1920, 1080, 6734, 148, 88, 41, 1, 44, 3,
+	 FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+};
+
+/* no hardware cursor supported under version 2.6.10, kernel bug */
+static int lynxfb_ops_cursor(struct fb_info *info, struct fb_cursor *fbcursor)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynx_cursor *cursor;
+
+	par = info->par;
+	crtc = &par->crtc;
+	cursor = &crtc->cursor;
+
+	if (fbcursor->image.width > cursor->max_w ||
+	    fbcursor->image.height > cursor->max_h ||
+	    fbcursor->image.depth > 1) {
+		return -ENXIO;
+	}
+
+	sm750_hw_cursor_disable(cursor);
+	if (fbcursor->set & FB_CUR_SETSIZE)
+		sm750_hw_cursor_setSize(cursor,
+					fbcursor->image.width,
+					fbcursor->image.height);
+
+	if (fbcursor->set & FB_CUR_SETPOS)
+		sm750_hw_cursor_setPos(cursor,
+				       fbcursor->image.dx - info->var.xoffset,
+				       fbcursor->image.dy - info->var.yoffset);
+
+	if (fbcursor->set & FB_CUR_SETCMAP) {
+		/* get the 16bit color of kernel means */
+		u16 fg, bg;
+
+		fg = ((info->cmap.red[fbcursor->image.fg_color] & 0xf800)) |
+		     ((info->cmap.green[fbcursor->image.fg_color] & 0xfc00) >> 5) |
+		     ((info->cmap.blue[fbcursor->image.fg_color] & 0xf800) >> 11);
+
+		bg = ((info->cmap.red[fbcursor->image.bg_color] & 0xf800)) |
+		     ((info->cmap.green[fbcursor->image.bg_color] & 0xfc00) >> 5) |
+		     ((info->cmap.blue[fbcursor->image.bg_color] & 0xf800) >> 11);
+
+		sm750_hw_cursor_setColor(cursor, fg, bg);
+	}
+
+	if (fbcursor->set & (FB_CUR_SETSHAPE | FB_CUR_SETIMAGE)) {
+		sm750_hw_cursor_setData(cursor,
+					fbcursor->rop,
+					fbcursor->image.data,
+					fbcursor->mask);
+	}
+
+	if (fbcursor->enable)
+		sm750_hw_cursor_enable(cursor);
+
+	return 0;
+}
+
+static void lynxfb_ops_fillrect(struct fb_info *info,
+				const struct fb_fillrect *region)
+{
+	struct lynxfb_par *par;
+	struct sm750_dev *sm750_dev;
+	unsigned int base, pitch, Bpp, rop;
+	u32 color;
+
+	if (info->state != FBINFO_STATE_RUNNING)
+		return;
+
+	par = info->par;
+	sm750_dev = par->dev;
+
+	/*
+	 * each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place
+	 */
+	base = par->crtc.o_screen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	color = (Bpp == 1) ? region->color :
+		((u32 *)info->pseudo_palette)[region->color];
+	rop = (region->rop != ROP_COPY) ? HW_ROP2_XOR : HW_ROP2_COPY;
+
+	/*
+	 * If not use spin_lock, system will die if user load driver
+	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
+	 */
+	spin_lock(&sm750_dev->slock);
+
+	sm750_dev->accel.de_fillrect(&sm750_dev->accel,
+				     base, pitch, Bpp,
+				     region->dx, region->dy,
+				     region->width, region->height,
+				     color, rop);
+	spin_unlock(&sm750_dev->slock);
+}
+
+static void lynxfb_ops_copyarea(struct fb_info *info,
+				const struct fb_copyarea *region)
+{
+	struct lynxfb_par *par;
+	struct sm750_dev *sm750_dev;
+	unsigned int base, pitch, Bpp;
+
+	par = info->par;
+	sm750_dev = par->dev;
+
+	/*
+	 * each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place
+	 */
+	base = par->crtc.o_screen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	/*
+	 * If not use spin_lock, system will die if user load driver
+	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
+	 */
+	spin_lock(&sm750_dev->slock);
+
+	sm750_dev->accel.de_copyarea(&sm750_dev->accel,
+				     base, pitch, region->sx, region->sy,
+				     base, pitch, Bpp, region->dx, region->dy,
+				     region->width, region->height,
+				     HW_ROP2_COPY);
+	spin_unlock(&sm750_dev->slock);
+}
+
+static void lynxfb_ops_imageblit(struct fb_info *info,
+				 const struct fb_image *image)
+{
+	unsigned int base, pitch, Bpp;
+	unsigned int fgcol, bgcol;
+	struct lynxfb_par *par;
+	struct sm750_dev *sm750_dev;
+
+	par = info->par;
+	sm750_dev = par->dev;
+	/*
+	 * each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place
+	 */
+	base = par->crtc.o_screen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	/* TODO: Implement hardware acceleration for image->depth > 1 */
+	if (image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
+		fgcol = ((u32 *)info->pseudo_palette)[image->fg_color];
+		bgcol = ((u32 *)info->pseudo_palette)[image->bg_color];
+	} else {
+		fgcol = image->fg_color;
+		bgcol = image->bg_color;
+	}
+
+	/*
+	 * If not use spin_lock, system will die if user load driver
+	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
+	 */
+	spin_lock(&sm750_dev->slock);
+
+	sm750_dev->accel.de_imageblit(&sm750_dev->accel,
+				      image->data, image->width >> 3, 0,
+				      base, pitch, Bpp,
+				      image->dx, image->dy,
+				      image->width, image->height,
+				      fgcol, bgcol, HW_ROP2_COPY);
+	spin_unlock(&sm750_dev->slock);
+}
+
+static int lynxfb_ops_pan_display(struct fb_var_screeninfo *var,
+				  struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+
+	if (!info)
+		return -EINVAL;
+
+	par = info->par;
+	crtc = &par->crtc;
+	return hw_sm750_pan_display(crtc, var, info);
+}
+
+static inline void lynxfb_set_visual_mode(struct fb_info *info)
+{
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+		break;
+	case 16:
+	case 24:
+	case 32:
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	default:
+		break;
+	}
+}
+
+static inline int lynxfb_set_color_offsets(struct fb_info *info)
+{
+	lynxfb_set_visual_mode(info);
+
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		info->var.red.offset = 0;
+		info->var.red.length = 8;
+		info->var.green.offset = 0;
+		info->var.green.length = 8;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 8;
+		info->var.transp.length = 0;
+		info->var.transp.offset = 0;
+		break;
+	case 16:
+		info->var.red.offset = 11;
+		info->var.red.length = 5;
+		info->var.green.offset = 5;
+		info->var.green.length = 6;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 5;
+		info->var.transp.length = 0;
+		info->var.transp.offset = 0;
+		break;
+	case 24:
+	case 32:
+		info->var.red.offset = 16;
+		info->var.red.length = 8;
+		info->var.green.offset = 8;
+		info->var.green.length = 8;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 8;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int lynxfb_ops_set_par(struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynxfb_output *output;
+	struct fb_var_screeninfo *var;
+	struct fb_fix_screeninfo *fix;
+	int ret;
+	unsigned int line_length;
+
+	if (!info)
+		return -EINVAL;
+
+	ret = 0;
+	par = info->par;
+	crtc = &par->crtc;
+	output = &par->output;
+	var = &info->var;
+	fix = &info->fix;
+
+	/* fix structure is not so FIX ... */
+	line_length = var->xres_virtual * var->bits_per_pixel / 8;
+	line_length = ALIGN(line_length, crtc->line_pad);
+	fix->line_length = line_length;
+	pr_info("fix->line_length = %d\n", fix->line_length);
+
+	/*
+	 * var->red,green,blue,transp are need to be set by driver
+	 * and these data should be set before setcolreg routine
+	 */
+
+	ret = lynxfb_set_color_offsets(info);
+
+	var->height = -1;
+	var->width = -1;
+	var->accel_flags = 0;/*FB_ACCELF_TEXT;*/
+
+	if (ret) {
+		pr_err("bpp %d not supported\n", var->bits_per_pixel);
+		return ret;
+	}
+	ret = hw_sm750_crtc_setMode(crtc, var, fix);
+	if (!ret)
+		ret = hw_sm750_output_setMode(output, var, fix);
+	return ret;
+}
+
+static inline unsigned int chan_to_field(unsigned int chan,
+					 struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int __maybe_unused lynxfb_suspend(struct device *dev)
+{
+	struct fb_info *info;
+	struct sm750_dev *sm750_dev;
+
+	sm750_dev = dev_get_drvdata(dev);
+
+	console_lock();
+	info = sm750_dev->fbinfo[0];
+	if (info)
+		/* 1 means do suspend */
+		fb_set_suspend(info, 1);
+	info = sm750_dev->fbinfo[1];
+	if (info)
+		/* 1 means do suspend */
+		fb_set_suspend(info, 1);
+
+	console_unlock();
+	return 0;
+}
+
+static int __maybe_unused lynxfb_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct fb_info *info;
+	struct sm750_dev *sm750_dev;
+
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynx_cursor *cursor;
+
+	sm750_dev = pci_get_drvdata(pdev);
+
+	console_lock();
+
+	hw_sm750_inithw(sm750_dev, pdev);
+
+	info = sm750_dev->fbinfo[0];
+
+	if (info) {
+		par = info->par;
+		crtc = &par->crtc;
+		cursor = &crtc->cursor;
+		memset_io(cursor->vstart, 0x0, cursor->size);
+		memset_io(crtc->v_screen, 0x0, crtc->vidmem_size);
+		lynxfb_ops_set_par(info);
+		fb_set_suspend(info, 0);
+	}
+
+	info = sm750_dev->fbinfo[1];
+
+	if (info) {
+		par = info->par;
+		crtc = &par->crtc;
+		cursor = &crtc->cursor;
+		memset_io(cursor->vstart, 0x0, cursor->size);
+		memset_io(crtc->v_screen, 0x0, crtc->vidmem_size);
+		lynxfb_ops_set_par(info);
+		fb_set_suspend(info, 0);
+	}
+
+	pdev->dev.power.power_state.event = PM_EVENT_RESUME;
+
+	console_unlock();
+	return 0;
+}
+
+static int lynxfb_ops_check_var(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	int ret;
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	resource_size_t request;
+
+	ret = 0;
+	par = info->par;
+	crtc = &par->crtc;
+
+	pr_debug("check var:%dx%d-%d\n",
+		 var->xres,
+		 var->yres,
+		 var->bits_per_pixel);
+
+	ret = lynxfb_set_color_offsets(info);
+
+	if (ret) {
+		pr_err("bpp %d not supported\n", var->bits_per_pixel);
+		return ret;
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->accel_flags = 0;/* FB_ACCELF_TEXT; */
+
+	/* check if current fb's video memory big enough to hold the onscreen*/
+	request = var->xres_virtual * (var->bits_per_pixel >> 3);
+	/* defaulty crtc->channel go with par->index */
+
+	request = ALIGN(request, crtc->line_pad);
+	request = request * var->yres_virtual;
+	if (crtc->vidmem_size < request) {
+		pr_err("not enough video memory for mode\n");
+		return -ENOMEM;
+	}
+
+	return hw_sm750_crtc_checkMode(crtc, var);
+}
+
+static int lynxfb_ops_setcolreg(unsigned int regno,
+				unsigned int red,
+				unsigned int green,
+				unsigned int blue,
+				unsigned int transp,
+				struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct fb_var_screeninfo *var;
+	int ret;
+
+	par = info->par;
+	crtc = &par->crtc;
+	var = &info->var;
+	ret = 0;
+
+	if (regno > 256) {
+		pr_err("regno = %d\n", regno);
+		return -EINVAL;
+	}
+
+	if (info->var.grayscale)
+		red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+
+	if (var->bits_per_pixel == 8 &&
+	    info->fix.visual == FB_VISUAL_PSEUDOCOLOR) {
+		red >>= 8;
+		green >>= 8;
+		blue >>= 8;
+		ret = hw_sm750_setColReg(crtc, regno, red, green, blue);
+		goto exit;
+	}
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR && regno < 256) {
+		u32 val;
+
+		if (var->bits_per_pixel == 16 ||
+		    var->bits_per_pixel == 32 ||
+		    var->bits_per_pixel == 24) {
+			val = chan_to_field(red, &var->red);
+			val |= chan_to_field(green, &var->green);
+			val |= chan_to_field(blue, &var->blue);
+			par->pseudo_palette[regno] = val;
+			goto exit;
+		}
+	}
+
+	ret = -EINVAL;
+
+exit:
+	return ret;
+}
+
+static int lynxfb_ops_blank(int blank, struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_output *output;
+
+	pr_debug("blank = %d.\n", blank);
+	par = info->par;
+	output = &par->output;
+	return output->proc_setBLANK(output, blank);
+}
+
+static int sm750fb_set_drv(struct lynxfb_par *par)
+{
+	int ret;
+	struct sm750_dev *sm750_dev;
+	struct lynxfb_output *output;
+	struct lynxfb_crtc *crtc;
+
+	ret = 0;
+
+	sm750_dev = par->dev;
+	output = &par->output;
+	crtc = &par->crtc;
+
+	crtc->vidmem_size = sm750_dev->vidmem_size;
+	if (sm750_dev->fb_count > 1)
+		crtc->vidmem_size >>= 1;
+
+	/* setup crtc and output member */
+	sm750_dev->hwCursor = g_hwcursor;
+
+	crtc->line_pad = 16;
+	crtc->xpanstep = 8;
+	crtc->ypanstep = 1;
+	crtc->ywrapstep = 0;
+
+	output->proc_setBLANK = (sm750_dev->revid == SM750LE_REVISION_ID) ?
+				 hw_sm750le_setBLANK : hw_sm750_setBLANK;
+	/* chip specific phase */
+	sm750_dev->accel.de_wait = (sm750_dev->revid == SM750LE_REVISION_ID) ?
+				    hw_sm750le_deWait : hw_sm750_deWait;
+	switch (sm750_dev->dataflow) {
+	case sm750_simul_pri:
+		output->paths = sm750_pnc;
+		crtc->channel = sm750_primary;
+		crtc->o_screen = 0;
+		crtc->v_screen = sm750_dev->pvMem;
+		pr_info("use simul primary mode\n");
+		break;
+	case sm750_simul_sec:
+		output->paths = sm750_pnc;
+		crtc->channel = sm750_secondary;
+		crtc->o_screen = 0;
+		crtc->v_screen = sm750_dev->pvMem;
+		break;
+	case sm750_dual_normal:
+		if (par->index == 0) {
+			output->paths = sm750_panel;
+			crtc->channel = sm750_primary;
+			crtc->o_screen = 0;
+			crtc->v_screen = sm750_dev->pvMem;
+		} else {
+			output->paths = sm750_crt;
+			crtc->channel = sm750_secondary;
+			/* not consider of padding stuffs for o_screen,need fix */
+			crtc->o_screen = sm750_dev->vidmem_size >> 1;
+			crtc->v_screen = sm750_dev->pvMem + crtc->o_screen;
+		}
+		break;
+	case sm750_dual_swap:
+		if (par->index == 0) {
+			output->paths = sm750_panel;
+			crtc->channel = sm750_secondary;
+			crtc->o_screen = 0;
+			crtc->v_screen = sm750_dev->pvMem;
+		} else {
+			output->paths = sm750_crt;
+			crtc->channel = sm750_primary;
+			/* not consider of padding stuffs for o_screen,
+			 * need fix
+			 */
+			crtc->o_screen = sm750_dev->vidmem_size >> 1;
+			crtc->v_screen = sm750_dev->pvMem + crtc->o_screen;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static struct fb_ops lynxfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var =  lynxfb_ops_check_var,
+	.fb_set_par = lynxfb_ops_set_par,
+	.fb_setcolreg = lynxfb_ops_setcolreg,
+	.fb_blank = lynxfb_ops_blank,
+	.fb_fillrect = cfb_fillrect,
+	.fb_imageblit = cfb_imageblit,
+	.fb_copyarea = cfb_copyarea,
+	/* cursor */
+	.fb_cursor = lynxfb_ops_cursor,
+};
+
+static int lynxfb_set_fbinfo(struct fb_info *info, int index)
+{
+	int i;
+	struct lynxfb_par *par;
+	struct sm750_dev *sm750_dev;
+	struct lynxfb_crtc *crtc;
+	struct lynxfb_output *output;
+	struct fb_var_screeninfo *var;
+	struct fb_fix_screeninfo *fix;
+
+	const struct fb_videomode *pdb[] = {
+		lynx750_ext, NULL, vesa_modes,
+	};
+	int cdb[] = {ARRAY_SIZE(lynx750_ext), 0, VESA_MODEDB_SIZE};
+	static const char * const mdb_desc[] = {
+		"driver prepared modes",
+		"kernel prepared default modedb",
+		"kernel HELPERS prepared vesa_modes",
+	};
+
+	static const char *fixId[2] = {
+		"sm750_fb1", "sm750_fb2",
+	};
+
+	int ret, line_length;
+
+	ret = 0;
+	par = (struct lynxfb_par *)info->par;
+	sm750_dev = par->dev;
+	crtc = &par->crtc;
+	output = &par->output;
+	var = &info->var;
+	fix = &info->fix;
+
+	/* set index */
+	par->index = index;
+	output->channel = &crtc->channel;
+	sm750fb_set_drv(par);
+	lynxfb_ops.fb_pan_display = lynxfb_ops_pan_display;
+
+	/*
+	 * set current cursor variable and proc pointer,
+	 * must be set after crtc member initialized
+	 */
+	crtc->cursor.offset = crtc->o_screen + crtc->vidmem_size - 1024;
+	crtc->cursor.mmio = sm750_dev->pvReg +
+		0x800f0 + (int)crtc->channel * 0x140;
+
+	pr_info("crtc->cursor.mmio = %p\n", crtc->cursor.mmio);
+	crtc->cursor.max_h = 64;
+	crtc->cursor.max_w = 64;
+	crtc->cursor.size = crtc->cursor.max_h * crtc->cursor.max_w * 2 / 8;
+	crtc->cursor.vstart = sm750_dev->pvMem + crtc->cursor.offset;
+
+	memset_io(crtc->cursor.vstart, 0, crtc->cursor.size);
+	if (!g_hwcursor) {
+		lynxfb_ops.fb_cursor = NULL;
+		sm750_hw_cursor_disable(&crtc->cursor);
+	}
+
+	/* set info->fbops, must be set before fb_find_mode */
+	if (!sm750_dev->accel_off) {
+		/* use 2d acceleration */
+		lynxfb_ops.fb_fillrect = lynxfb_ops_fillrect;
+		lynxfb_ops.fb_copyarea = lynxfb_ops_copyarea;
+		lynxfb_ops.fb_imageblit = lynxfb_ops_imageblit;
+	}
+	info->fbops = &lynxfb_ops;
+
+	if (!g_fbmode[index]) {
+		g_fbmode[index] = g_def_fbmode;
+		if (index)
+			g_fbmode[index] = g_fbmode[0];
+	}
+
+	for (i = 0; i < 3; i++) {
+		ret = fb_find_mode(var, info, g_fbmode[index],
+				   pdb[i], cdb[i], NULL, 8);
+
+		if (ret == 1) {
+			pr_info("success! use specified mode:%s in %s\n",
+				g_fbmode[index],
+				mdb_desc[i]);
+			break;
+		} else if (ret == 2) {
+			pr_warn("use specified mode:%s in %s,with an ignored refresh rate\n",
+				g_fbmode[index],
+				mdb_desc[i]);
+			break;
+		} else if (ret == 3) {
+			pr_warn("wanna use default mode\n");
+			/*break;*/
+		} else if (ret == 4) {
+			pr_warn("fall back to any valid mode\n");
+		} else {
+			pr_warn("ret = %d,fb_find_mode failed,with %s\n",
+				ret,
+				mdb_desc[i]);
+		}
+	}
+
+	/* some member of info->var had been set by fb_find_mode */
+
+	pr_info("Member of info->var is :\n"
+		"xres=%d\n"
+		"yres=%d\n"
+		"xres_virtual=%d\n"
+		"yres_virtual=%d\n"
+		"xoffset=%d\n"
+		"yoffset=%d\n"
+		"bits_per_pixel=%d\n"
+		" ...\n",
+		var->xres,
+		var->yres,
+		var->xres_virtual,
+		var->yres_virtual,
+		var->xoffset,
+		var->yoffset,
+		var->bits_per_pixel);
+
+	/* set par */
+	par->info = info;
+
+	/* set info */
+	line_length = ALIGN((var->xres_virtual * var->bits_per_pixel / 8),
+			    crtc->line_pad);
+
+	info->pseudo_palette = &par->pseudo_palette[0];
+	info->screen_base = crtc->v_screen;
+	pr_debug("screen_base vaddr = %p\n", info->screen_base);
+	info->screen_size = line_length * var->yres_virtual;
+	info->flags = FBINFO_FLAG_DEFAULT | 0;
+
+	/* set info->fix */
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->type_aux = 0;
+	fix->xpanstep = crtc->xpanstep;
+	fix->ypanstep = crtc->ypanstep;
+	fix->ywrapstep = crtc->ywrapstep;
+	fix->accel = FB_ACCEL_SMI;
+
+	strscpy(fix->id, fixId[index], sizeof(fix->id));
+
+	fix->smem_start = crtc->o_screen + sm750_dev->vidmem_start;
+	pr_info("fix->smem_start = %lx\n", fix->smem_start);
+	/*
+	 * according to mmap experiment from user space application,
+	 * fix->mmio_len should not larger than virtual size
+	 * (xres_virtual x yres_virtual x ByPP)
+	 * Below line maybe buggy when user mmap fb dev node and write
+	 * data into the bound over virtual size
+	 */
+	fix->smem_len = crtc->vidmem_size;
+	pr_info("fix->smem_len = %x\n", fix->smem_len);
+	info->screen_size = fix->smem_len;
+	fix->line_length = line_length;
+	fix->mmio_start = sm750_dev->vidreg_start;
+	pr_info("fix->mmio_start = %lx\n", fix->mmio_start);
+	fix->mmio_len = sm750_dev->vidreg_size;
+	pr_info("fix->mmio_len = %x\n", fix->mmio_len);
+
+	lynxfb_set_visual_mode(info);
+
+	/* set var */
+	var->activate = FB_ACTIVATE_NOW;
+	var->accel_flags = 0;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	pr_debug("#1 show info->cmap :\nstart=%d,len=%d,red=%p,green=%p,blue=%p,transp=%p\n",
+		 info->cmap.start, info->cmap.len,
+		 info->cmap.red, info->cmap.green, info->cmap.blue,
+		 info->cmap.transp);
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret < 0) {
+		pr_err("Could not allocate memory for cmap.\n");
+		goto exit;
+	}
+
+	pr_debug("#2 show info->cmap :\nstart=%d,len=%d,red=%p,green=%p,blue=%p,transp=%p\n",
+		 info->cmap.start, info->cmap.len,
+		 info->cmap.red, info->cmap.green, info->cmap.blue,
+		 info->cmap.transp);
+
+exit:
+	lynxfb_ops_check_var(var, info);
+	return ret;
+}
+
+/*	chip specific g_option configuration routine */
+static void sm750fb_setup(struct sm750_dev *sm750_dev, char *src)
+{
+	char *opt;
+	int swap;
+
+	swap = 0;
+
+	sm750_dev->initParm.chip_clk = 0;
+	sm750_dev->initParm.mem_clk = 0;
+	sm750_dev->initParm.master_clk = 0;
+	sm750_dev->initParm.powerMode = 0;
+	sm750_dev->initParm.setAllEngOff = 0;
+	sm750_dev->initParm.resetMemory = 1;
+
+	/* defaultly turn g_hwcursor on for both view */
+	g_hwcursor = 3;
+
+	if (!src || !*src) {
+		dev_warn(&sm750_dev->pdev->dev, "no specific g_option.\n");
+		goto NO_PARAM;
+	}
+
+	while ((opt = strsep(&src, ":")) != NULL && *opt != 0) {
+		dev_info(&sm750_dev->pdev->dev, "opt=%s\n", opt);
+		dev_info(&sm750_dev->pdev->dev, "src=%s\n", src);
+
+		if (!strncmp(opt, "swap", strlen("swap"))) {
+			swap = 1;
+		} else if (!strncmp(opt, "nocrt", strlen("nocrt"))) {
+			sm750_dev->nocrt = 1;
+		} else if (!strncmp(opt, "36bit", strlen("36bit"))) {
+			sm750_dev->pnltype = sm750_doubleTFT;
+		} else if (!strncmp(opt, "18bit", strlen("18bit"))) {
+			sm750_dev->pnltype = sm750_dualTFT;
+		} else if (!strncmp(opt, "24bit", strlen("24bit"))) {
+			sm750_dev->pnltype = sm750_24TFT;
+		} else if (!strncmp(opt, "nohwc0", strlen("nohwc0"))) {
+			g_hwcursor &= ~0x1;
+		} else if (!strncmp(opt, "nohwc1", strlen("nohwc1"))) {
+			g_hwcursor &= ~0x2;
+		} else if (!strncmp(opt, "nohwc", strlen("nohwc"))) {
+			g_hwcursor = 0;
+		} else {
+			if (!g_fbmode[0]) {
+				g_fbmode[0] = opt;
+				dev_info(&sm750_dev->pdev->dev,
+					 "find fbmode0 : %s\n", g_fbmode[0]);
+			} else if (!g_fbmode[1]) {
+				g_fbmode[1] = opt;
+				dev_info(&sm750_dev->pdev->dev,
+					 "find fbmode1 : %s\n", g_fbmode[1]);
+			} else {
+				dev_warn(&sm750_dev->pdev->dev, "How many view you wann set?\n");
+			}
+		}
+	}
+
+NO_PARAM:
+	if (sm750_dev->revid != SM750LE_REVISION_ID) {
+		if (sm750_dev->fb_count > 1) {
+			if (swap)
+				sm750_dev->dataflow = sm750_dual_swap;
+			else
+				sm750_dev->dataflow = sm750_dual_normal;
+		} else {
+			if (swap)
+				sm750_dev->dataflow = sm750_simul_sec;
+			else
+				sm750_dev->dataflow = sm750_simul_pri;
+		}
+	} else {
+		/* SM750LE only have one crt channel */
+		sm750_dev->dataflow = sm750_simul_sec;
+		/* sm750le do not have complex attributes */
+		sm750_dev->nocrt = 0;
+	}
+}
+
+static void sm750fb_framebuffer_release(struct sm750_dev *sm750_dev)
+{
+	struct fb_info *fb_info;
+
+	while (sm750_dev->fb_count) {
+		fb_info = sm750_dev->fbinfo[sm750_dev->fb_count - 1];
+		unregister_framebuffer(fb_info);
+		framebuffer_release(fb_info);
+		sm750_dev->fb_count--;
+	}
+}
+
+static int sm750fb_framebuffer_alloc(struct sm750_dev *sm750_dev, int fbidx)
+{
+	struct fb_info *fb_info;
+	struct lynxfb_par *par;
+	int err;
+
+	fb_info = framebuffer_alloc(sizeof(struct lynxfb_par),
+				    &sm750_dev->pdev->dev);
+	if (!fb_info)
+		return -ENOMEM;
+
+	sm750_dev->fbinfo[fbidx] = fb_info;
+	par = fb_info->par;
+	par->dev = sm750_dev;
+
+	err = lynxfb_set_fbinfo(fb_info, fbidx);
+	if (err)
+		goto release_fb;
+
+	err = register_framebuffer(fb_info);
+	if (err < 0)
+		goto release_fb;
+
+	sm750_dev->fb_count++;
+
+	return 0;
+
+release_fb:
+	framebuffer_release(fb_info);
+	return err;
+}
+
+static int lynxfb_kick_out_firmware_fb(struct pci_dev *pdev)
+{
+	resource_size_t base = pci_resource_start(pdev, 0);
+	resource_size_t size = pci_resource_len(pdev, 0);
+	bool primary = false;
+
+#ifdef CONFIG_X86
+	primary = pdev->resource[PCI_ROM_RESOURCE].flags &
+					IORESOURCE_ROM_SHADOW;
+#endif
+
+	return aperture_remove_conflicting_devices(base, size, primary, "sm750_fb1");
+}
+
+static int lynxfb_pci_probe(struct pci_dev *pdev,
+			    const struct pci_device_id *ent)
+{
+	struct sm750_dev *sm750_dev = NULL;
+	int max_fb;
+	int fbidx;
+	int err;
+
+	err = lynxfb_kick_out_firmware_fb(pdev);
+	if (err)
+		return err;
+
+	/* enable device */
+	err = pcim_enable_device(pdev);
+	if (err)
+		return err;
+
+	err = -ENOMEM;
+	sm750_dev = devm_kzalloc(&pdev->dev, sizeof(*sm750_dev), GFP_KERNEL);
+	if (!sm750_dev)
+		return err;
+
+	sm750_dev->fbinfo[0] = NULL;
+	sm750_dev->fbinfo[1] = NULL;
+	sm750_dev->devid = pdev->device;
+	sm750_dev->revid = pdev->revision;
+	sm750_dev->pdev = pdev;
+	sm750_dev->mtrr_off = g_nomtrr;
+	sm750_dev->mtrr.vram = 0;
+	sm750_dev->accel_off = g_noaccel;
+	spin_lock_init(&sm750_dev->slock);
+
+	if (!sm750_dev->accel_off) {
+		/*
+		 * hook deInit and 2d routines, notes that below hw_xxx
+		 * routine can work on most of lynx chips
+		 * if some chip need specific function,
+		 * please hook it in smXXX_set_drv routine
+		 */
+		sm750_dev->accel.de_init = sm750_hw_de_init;
+		sm750_dev->accel.de_fillrect = sm750_hw_fillrect;
+		sm750_dev->accel.de_copyarea = sm750_hw_copyarea;
+		sm750_dev->accel.de_imageblit = sm750_hw_imageblit;
+	}
+
+	/* call chip specific setup routine  */
+	sm750fb_setup(sm750_dev, g_settings);
+
+	/* call chip specific mmap routine */
+	err = hw_sm750_map(sm750_dev, pdev);
+	if (err)
+		return err;
+
+	if (!sm750_dev->mtrr_off)
+		sm750_dev->mtrr.vram = arch_phys_wc_add(sm750_dev->vidmem_start,
+							sm750_dev->vidmem_size);
+
+	memset_io(sm750_dev->pvMem, 0, sm750_dev->vidmem_size);
+
+	pci_set_drvdata(pdev, sm750_dev);
+
+	/* call chipInit routine */
+	hw_sm750_inithw(sm750_dev, pdev);
+
+	/* allocate frame buffer info structures according to g_dualview */
+	max_fb = g_dualview ? 2 : 1;
+	for (fbidx = 0; fbidx < max_fb; fbidx++) {
+		err = sm750fb_framebuffer_alloc(sm750_dev, fbidx);
+		if (err)
+			goto release_fb;
+	}
+
+	return 0;
+
+release_fb:
+	sm750fb_framebuffer_release(sm750_dev);
+	return err;
+}
+
+static void lynxfb_pci_remove(struct pci_dev *pdev)
+{
+	struct sm750_dev *sm750_dev;
+
+	sm750_dev = pci_get_drvdata(pdev);
+
+	sm750fb_framebuffer_release(sm750_dev);
+	arch_phys_wc_del(sm750_dev->mtrr.vram);
+
+	iounmap(sm750_dev->pvReg);
+	iounmap(sm750_dev->pvMem);
+	kfree(g_settings);
+}
+
+static int __init lynxfb_setup(char *options)
+{
+	int len;
+	char *opt, *tmp;
+
+	if (!options || !*options) {
+		pr_warn("no options.\n");
+		return 0;
+	}
+
+	pr_info("options:%s\n", options);
+
+	len = strlen(options) + 1;
+	g_settings = kzalloc(len, GFP_KERNEL);
+	if (!g_settings)
+		return -ENOMEM;
+
+	tmp = g_settings;
+
+	/*
+	 * Notes:
+	 * char * strsep(char **s,const char * ct);
+	 * @s: the string to be searched
+	 * @ct :the characters to search for
+	 *
+	 * strsep() updates @options to pointer after the first found token
+	 * it also returns the pointer ahead the token.
+	 */
+	while ((opt = strsep(&options, ":")) != NULL) {
+		/* options that mean for any lynx chips are configured here */
+		if (!strncmp(opt, "noaccel", strlen("noaccel"))) {
+			g_noaccel = 1;
+		} else if (!strncmp(opt, "nomtrr", strlen("nomtrr"))) {
+			g_nomtrr = 1;
+		} else if (!strncmp(opt, "dual", strlen("dual"))) {
+			g_dualview = 1;
+		} else {
+			strcat(tmp, opt);
+			tmp += strlen(opt);
+			if (options)
+				*tmp++ = ':';
+			else
+				*tmp++ = 0;
+		}
+	}
+
+	/* misc g_settings are transport to chip specific routines */
+	pr_info("parameter left for chip specific analysis:%s\n", g_settings);
+	return 0;
+}
+
+static const struct pci_device_id smi_pci_table[] = {
+	{ PCI_DEVICE(0x126f, 0x0750), },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, smi_pci_table);
+
+static SIMPLE_DEV_PM_OPS(lynxfb_pm_ops, lynxfb_suspend, lynxfb_resume);
+
+static struct pci_driver lynxfb_driver = {
+	.name =		"sm750fb",
+	.id_table =	smi_pci_table,
+	.probe =	lynxfb_pci_probe,
+	.remove =	lynxfb_pci_remove,
+	.driver.pm =	&lynxfb_pm_ops,
+};
+
+static int __init lynxfb_init(void)
+{
+	char *option;
+
+#ifdef MODULE
+	option = g_option;
+#else
+	if (fb_get_options("sm750fb", &option))
+		return -ENODEV;
+#endif
+
+	lynxfb_setup(option);
+	return pci_register_driver(&lynxfb_driver);
+}
+module_init(lynxfb_init);
+
+static void __exit lynxfb_exit(void)
+{
+	pci_unregister_driver(&lynxfb_driver);
+}
+module_exit(lynxfb_exit);
+
+module_param(g_option, charp, 0444);
+
+MODULE_PARM_DESC(g_option,
+		 "\n\t\tCommon options:\n"
+		 "\t\tnoaccel:disable 2d capabilities\n"
+		 "\t\tnomtrr:disable MTRR attribute for video memory\n"
+		 "\t\tdualview:dual frame buffer feature enabled\n"
+		 "\t\tnohwc:disable hardware cursor\n"
+		 "\t\tUsual example:\n"
+		 "\t\tinsmod ./sm750fb.ko g_option=\"noaccel,nohwc,1280x1024-8@60\"\n"
+		 );
+
+MODULE_AUTHOR("monk liu <monk.liu@siliconmotion.com>");
+MODULE_AUTHOR("Sudip Mukherjee <sudip@vectorindia.org>");
+MODULE_DESCRIPTION("Frame buffer driver for SM750 chipset");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
new file mode 100644
index 000000000..aff69661c
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750.h
@@ -0,0 +1,220 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef LYNXDRV_H_
+#define LYNXDRV_H_
+
+#define FB_ACCEL_SMI 0xab
+
+#define MHZ(x) ((x) * 1000000)
+
+#define DEFAULT_SM750_CHIP_CLOCK	290
+#define DEFAULT_SM750LE_CHIP_CLOCK	333
+#ifndef SM750LE_REVISION_ID
+#define SM750LE_REVISION_ID ((unsigned char)0xfe)
+#endif
+
+enum sm750_pnltype {
+	sm750_24TFT = 0,	/* 24bit tft */
+	sm750_dualTFT = 2,	/* dual 18 bit tft */
+	sm750_doubleTFT = 1,	/* 36 bit double pixel tft */
+};
+
+/* vga channel is not concerned  */
+enum sm750_dataflow {
+	sm750_simul_pri,	/* primary => all head */
+	sm750_simul_sec,	/* secondary => all head */
+	sm750_dual_normal,	/* primary => panel head and secondary => crt */
+	sm750_dual_swap,	/* primary => crt head and secondary => panel */
+};
+
+enum sm750_channel {
+	sm750_primary = 0,
+	/* enum value equal to the register filed data */
+	sm750_secondary = 1,
+};
+
+enum sm750_path {
+	sm750_panel = 1,
+	sm750_crt = 2,
+	sm750_pnc = 3,	/* panel and crt */
+};
+
+struct init_status {
+	ushort powerMode;
+	/* below three clocks are in unit of MHZ*/
+	ushort chip_clk;
+	ushort mem_clk;
+	ushort master_clk;
+	ushort setAllEngOff;
+	ushort resetMemory;
+};
+
+struct lynx_accel {
+	/* base virtual address of DPR registers */
+	volatile unsigned char __iomem *dprBase;
+	/* base virtual address of de data port */
+	volatile unsigned char __iomem *dpPortBase;
+
+	/* function pointers */
+	void (*de_init)(struct lynx_accel *accel);
+
+	int (*de_wait)(void);/* see if hardware ready to work */
+
+	int (*de_fillrect)(struct lynx_accel *accel,
+			   u32 base, u32 pitch, u32 bpp,
+			   u32 x, u32 y, u32 width, u32 height,
+			   u32 color, u32 rop);
+
+	int (*de_copyarea)(struct lynx_accel *accel,
+			   u32 s_base, u32 s_pitch,
+			   u32 sx, u32 sy,
+			   u32 d_base, u32 d_pitch,
+			   u32 bpp, u32 dx, u32 dy,
+			   u32 width, u32 height,
+			   u32 rop2);
+
+	int (*de_imageblit)(struct lynx_accel *accel, const char *p_srcbuf,
+			    u32 src_delta, u32 start_bit, u32 d_base, u32 d_pitch,
+			    u32 byte_per_pixel, u32 dx, u32 dy, u32 width,
+			    u32 height, u32 f_color, u32 b_color, u32 rop2);
+
+};
+
+struct sm750_dev {
+	/* common members */
+	u16 devid;
+	u8 revid;
+	struct pci_dev *pdev;
+	struct fb_info *fbinfo[2];
+	struct lynx_accel accel;
+	int accel_off;
+	int fb_count;
+	int mtrr_off;
+	struct{
+		int vram;
+	} mtrr;
+	/* all smi graphic adaptor got below attributes */
+	unsigned long vidmem_start;
+	unsigned long vidreg_start;
+	__u32 vidmem_size;
+	__u32 vidreg_size;
+	void __iomem *pvReg;
+	unsigned char __iomem *pvMem;
+	/* locks*/
+	spinlock_t slock;
+
+	struct init_status initParm;
+	enum sm750_pnltype pnltype;
+	enum sm750_dataflow dataflow;
+	int nocrt;
+
+	/*
+	 * 0: no hardware cursor
+	 * 1: primary crtc hw cursor enabled,
+	 * 2: secondary crtc hw cursor enabled
+	 * 3: both ctrc hw cursor enabled
+	 */
+	int hwCursor;
+};
+
+struct lynx_cursor {
+	/* cursor width ,height and size */
+	int w;
+	int h;
+	int size;
+	/* hardware limitation */
+	int max_w;
+	int max_h;
+	/* base virtual address and offset  of cursor image */
+	char __iomem *vstart;
+	int offset;
+	/* mmio addr of hw cursor */
+	volatile char __iomem *mmio;
+};
+
+struct lynxfb_crtc {
+	unsigned char __iomem *v_cursor; /* virtual address of cursor */
+	unsigned char __iomem *v_screen; /* virtual address of on_screen */
+	int o_cursor; /* cursor address offset in vidmem */
+	int o_screen; /* onscreen address offset in vidmem */
+	int channel;/* which channel this crtc stands for*/
+	resource_size_t vidmem_size;/* this view's video memory max size */
+
+	/* below attributes belong to info->fix, their value depends on specific adaptor*/
+	u16 line_pad;/* padding information:0,1,2,4,8,16,... */
+	u16 xpanstep;
+	u16 ypanstep;
+	u16 ywrapstep;
+
+	void *priv;
+
+	/* cursor information */
+	struct lynx_cursor cursor;
+};
+
+struct lynxfb_output {
+	int dpms;
+	int paths;
+	/*
+	 * which paths(s) this output stands for,for sm750:
+	 * paths=1:means output for panel paths
+	 * paths=2:means output for crt paths
+	 * paths=3:means output for both panel and crt paths
+	 */
+
+	int *channel;
+	/*
+	 * which channel these outputs linked with,for sm750:
+	 * *channel=0 means primary channel
+	 * *channel=1 means secondary channel
+	 * output->channel ==> &crtc->channel
+	 */
+	void *priv;
+
+	int (*proc_setBLANK)(struct lynxfb_output *output, int blank);
+};
+
+struct lynxfb_par {
+	/* either 0 or 1 for dual head adaptor,0 is the older one registered */
+	int index;
+	unsigned int pseudo_palette[256];
+	struct lynxfb_crtc crtc;
+	struct lynxfb_output output;
+	struct fb_info *info;
+	struct sm750_dev *dev;
+};
+
+static inline unsigned long ps_to_hz(unsigned int psvalue)
+{
+	unsigned long long numerator = 1000 * 1000 * 1000 * 1000ULL;
+	/* 10^12 / picosecond period gives frequency in Hz */
+	do_div(numerator, psvalue);
+	return (unsigned long)numerator;
+}
+
+int hw_sm750_map(struct sm750_dev *sm750_dev, struct pci_dev *pdev);
+int hw_sm750_inithw(struct sm750_dev *sm750_dev, struct pci_dev *pdev);
+void hw_sm750_initAccel(struct sm750_dev *sm750_dev);
+int hw_sm750_deWait(void);
+int hw_sm750le_deWait(void);
+
+int hw_sm750_output_setMode(struct lynxfb_output *output,
+			    struct fb_var_screeninfo *var,
+			    struct fb_fix_screeninfo *fix);
+
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc *crtc,
+			    struct fb_var_screeninfo *var);
+
+int hw_sm750_crtc_setMode(struct lynxfb_crtc *crtc,
+			  struct fb_var_screeninfo *var,
+			  struct fb_fix_screeninfo *fix);
+
+int hw_sm750_setColReg(struct lynxfb_crtc *crtc, ushort index,
+		       ushort red, ushort green, ushort blue);
+
+int hw_sm750_setBLANK(struct lynxfb_output *output, int blank);
+int hw_sm750le_setBLANK(struct lynxfb_output *output, int blank);
+int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
+			 const struct fb_var_screeninfo *var,
+			 const struct fb_info *info);
+
+#endif
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
new file mode 100644
index 000000000..24b9077a6
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -0,0 +1,414 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/pagemap.h>
+#include <linux/console.h>
+#include <linux/platform_device.h>
+#include <linux/screen_info.h>
+
+#include "sm750.h"
+#include "sm750_accel.h"
+static inline void write_dpr(struct lynx_accel *accel, int offset, u32 regValue)
+{
+	writel(regValue, accel->dprBase + offset);
+}
+
+static inline u32 read_dpr(struct lynx_accel *accel, int offset)
+{
+	return readl(accel->dprBase + offset);
+}
+
+static inline void write_dpPort(struct lynx_accel *accel, u32 data)
+{
+	writel(data, accel->dpPortBase);
+}
+
+void sm750_hw_de_init(struct lynx_accel *accel)
+{
+	/* setup 2d engine registers */
+	u32 reg, clr;
+
+	write_dpr(accel, DE_MASKS, 0xFFFFFFFF);
+
+	/* dpr1c */
+	reg =  0x3;
+
+	clr = DE_STRETCH_FORMAT_PATTERN_XY |
+	      DE_STRETCH_FORMAT_PATTERN_Y_MASK |
+	      DE_STRETCH_FORMAT_PATTERN_X_MASK |
+	      DE_STRETCH_FORMAT_ADDRESSING_MASK |
+	      DE_STRETCH_FORMAT_SOURCE_HEIGHT_MASK;
+
+	/* DE_STRETCH bpp format need be initialized in setMode routine */
+	write_dpr(accel, DE_STRETCH_FORMAT,
+		  (read_dpr(accel, DE_STRETCH_FORMAT) & ~clr) | reg);
+
+	/* disable clipping and transparent */
+	write_dpr(accel, DE_CLIP_TL, 0); /* dpr2c */
+	write_dpr(accel, DE_CLIP_BR, 0); /* dpr30 */
+
+	write_dpr(accel, DE_COLOR_COMPARE_MASK, 0); /* dpr24 */
+	write_dpr(accel, DE_COLOR_COMPARE, 0);
+
+	clr = DE_CONTROL_TRANSPARENCY | DE_CONTROL_TRANSPARENCY_MATCH |
+		DE_CONTROL_TRANSPARENCY_SELECT;
+
+	/* dpr0c */
+	write_dpr(accel, DE_CONTROL, read_dpr(accel, DE_CONTROL) & ~clr);
+}
+
+/*
+ * set2dformat only be called from setmode functions
+ * but if you need dual framebuffer driver,need call set2dformat
+ * every time you use 2d function
+ */
+
+void sm750_hw_set2dformat(struct lynx_accel *accel, int fmt)
+{
+	u32 reg;
+
+	/* fmt=0,1,2 for 8,16,32,bpp on sm718/750/502 */
+	reg = read_dpr(accel, DE_STRETCH_FORMAT);
+	reg &= ~DE_STRETCH_FORMAT_PIXEL_FORMAT_MASK;
+	reg |= ((fmt << DE_STRETCH_FORMAT_PIXEL_FORMAT_SHIFT) &
+		DE_STRETCH_FORMAT_PIXEL_FORMAT_MASK);
+	write_dpr(accel, DE_STRETCH_FORMAT, reg);
+}
+
+int sm750_hw_fillrect(struct lynx_accel *accel,
+		      u32 base, u32 pitch, u32 Bpp,
+		      u32 x, u32 y, u32 width, u32 height,
+		      u32 color, u32 rop)
+{
+	u32 deCtrl;
+
+	if (accel->de_wait() != 0) {
+		/*
+		 * int time wait and always busy,seems hardware
+		 * got something error
+		 */
+		pr_debug("De engine always busy\n");
+		return -1;
+	}
+
+	write_dpr(accel, DE_WINDOW_DESTINATION_BASE, base); /* dpr40 */
+	write_dpr(accel, DE_PITCH,
+		  ((pitch / Bpp << DE_PITCH_DESTINATION_SHIFT) &
+		   DE_PITCH_DESTINATION_MASK) |
+		  (pitch / Bpp & DE_PITCH_SOURCE_MASK)); /* dpr10 */
+
+	write_dpr(accel, DE_WINDOW_WIDTH,
+		  ((pitch / Bpp << DE_WINDOW_WIDTH_DST_SHIFT) &
+		   DE_WINDOW_WIDTH_DST_MASK) |
+		   (pitch / Bpp & DE_WINDOW_WIDTH_SRC_MASK)); /* dpr44 */
+
+	write_dpr(accel, DE_FOREGROUND, color); /* DPR14 */
+
+	write_dpr(accel, DE_DESTINATION,
+		  ((x << DE_DESTINATION_X_SHIFT) & DE_DESTINATION_X_MASK) |
+		  (y & DE_DESTINATION_Y_MASK)); /* dpr4 */
+
+	write_dpr(accel, DE_DIMENSION,
+		  ((width << DE_DIMENSION_X_SHIFT) & DE_DIMENSION_X_MASK) |
+		  (height & DE_DIMENSION_Y_ET_MASK)); /* dpr8 */
+
+	deCtrl = DE_CONTROL_STATUS | DE_CONTROL_LAST_PIXEL |
+		DE_CONTROL_COMMAND_RECTANGLE_FILL | DE_CONTROL_ROP_SELECT |
+		(rop & DE_CONTROL_ROP_MASK); /* dpr0xc */
+
+	write_dpr(accel, DE_CONTROL, deCtrl);
+	return 0;
+}
+
+/**
+ * sm750_hw_copyarea
+ * @accel: Acceleration device data
+ * @sBase: Address of source: offset in frame buffer
+ * @sPitch: Pitch value of source surface in BYTE
+ * @sx: Starting x coordinate of source surface
+ * @sy: Starting y coordinate of source surface
+ * @dBase: Address of destination: offset in frame buffer
+ * @dPitch: Pitch value of destination surface in BYTE
+ * @Bpp: Color depth of destination surface
+ * @dx: Starting x coordinate of destination surface
+ * @dy: Starting y coordinate of destination surface
+ * @width: width of rectangle in pixel value
+ * @height: height of rectangle in pixel value
+ * @rop2: ROP value
+ */
+int sm750_hw_copyarea(struct lynx_accel *accel,
+		      unsigned int sBase, unsigned int sPitch,
+		      unsigned int sx, unsigned int sy,
+		      unsigned int dBase, unsigned int dPitch,
+		      unsigned int Bpp, unsigned int dx, unsigned int dy,
+		      unsigned int width, unsigned int height,
+		      unsigned int rop2)
+{
+	unsigned int nDirection, de_ctrl;
+
+	nDirection = LEFT_TO_RIGHT;
+	/* Direction of ROP2 operation: 1 = Left to Right, (-1) = Right to Left */
+	de_ctrl = 0;
+
+	/* If source and destination are the same surface, need to check for overlay cases */
+	if (sBase == dBase && sPitch == dPitch) {
+		/* Determine direction of operation */
+		if (sy < dy) {
+			/*  +----------+
+			 *  |S         |
+			 *  |   +----------+
+			 *  |   |      |   |
+			 *  |   |      |   |
+			 *  +---|------+   |
+			 *	|         D|
+			 *	+----------+
+			 */
+
+			nDirection = BOTTOM_TO_TOP;
+		} else if (sy > dy) {
+			/*  +----------+
+			 *  |D         |
+			 *  |   +----------+
+			 *  |   |      |   |
+			 *  |   |      |   |
+			 *  +---|------+   |
+			 *	|         S|
+			 *	+----------+
+			 */
+
+			nDirection = TOP_TO_BOTTOM;
+		} else {
+			/* sy == dy */
+
+			if (sx <= dx) {
+				/* +------+---+------+
+				 * |S     |   |     D|
+				 * |      |   |      |
+				 * |      |   |      |
+				 * |      |   |      |
+				 * +------+---+------+
+				 */
+
+				nDirection = RIGHT_TO_LEFT;
+			} else {
+			/* sx > dx */
+
+				/* +------+---+------+
+				 * |D     |   |     S|
+				 * |      |   |      |
+				 * |      |   |      |
+				 * |      |   |      |
+				 * +------+---+------+
+				 */
+
+				nDirection = LEFT_TO_RIGHT;
+			}
+		}
+	}
+
+	if ((nDirection == BOTTOM_TO_TOP) || (nDirection == RIGHT_TO_LEFT)) {
+		sx += width - 1;
+		sy += height - 1;
+		dx += width - 1;
+		dy += height - 1;
+	}
+
+	/*
+	 * Note:
+	 * DE_FOREGROUND and DE_BACKGROUND are don't care.
+	 * DE_COLOR_COMPARE and DE_COLOR_COMPARE_MAKS
+	 * are set by set deSetTransparency().
+	 */
+
+	/*
+	 * 2D Source Base.
+	 * It is an address offset (128 bit aligned)
+	 * from the beginning of frame buffer.
+	 */
+	write_dpr(accel, DE_WINDOW_SOURCE_BASE, sBase); /* dpr40 */
+
+	/*
+	 * 2D Destination Base.
+	 * It is an address offset (128 bit aligned)
+	 * from the beginning of frame buffer.
+	 */
+	write_dpr(accel, DE_WINDOW_DESTINATION_BASE, dBase); /* dpr44 */
+
+	/*
+	 * Program pitch (distance between the 1st points of two adjacent lines).
+	 * Note that input pitch is BYTE value, but the 2D Pitch register uses
+	 * pixel values. Need Byte to pixel conversion.
+	 */
+	write_dpr(accel, DE_PITCH,
+		  ((dPitch / Bpp << DE_PITCH_DESTINATION_SHIFT) &
+		   DE_PITCH_DESTINATION_MASK) |
+		  (sPitch / Bpp & DE_PITCH_SOURCE_MASK)); /* dpr10 */
+
+	/*
+	 * Screen Window width in Pixels.
+	 * 2D engine uses this value to calculate the linear address in frame buffer
+	 * for a given point.
+	 */
+	write_dpr(accel, DE_WINDOW_WIDTH,
+		  ((dPitch / Bpp << DE_WINDOW_WIDTH_DST_SHIFT) &
+		   DE_WINDOW_WIDTH_DST_MASK) |
+		  (sPitch / Bpp & DE_WINDOW_WIDTH_SRC_MASK)); /* dpr3c */
+
+	if (accel->de_wait() != 0)
+		return -1;
+
+	write_dpr(accel, DE_SOURCE,
+		  ((sx << DE_SOURCE_X_K1_SHIFT) & DE_SOURCE_X_K1_MASK) |
+		  (sy & DE_SOURCE_Y_K2_MASK)); /* dpr0 */
+	write_dpr(accel, DE_DESTINATION,
+		  ((dx << DE_DESTINATION_X_SHIFT) & DE_DESTINATION_X_MASK) |
+		  (dy & DE_DESTINATION_Y_MASK)); /* dpr04 */
+	write_dpr(accel, DE_DIMENSION,
+		  ((width << DE_DIMENSION_X_SHIFT) & DE_DIMENSION_X_MASK) |
+		  (height & DE_DIMENSION_Y_ET_MASK)); /* dpr08 */
+
+	de_ctrl = (rop2 & DE_CONTROL_ROP_MASK) | DE_CONTROL_ROP_SELECT |
+		((nDirection == RIGHT_TO_LEFT) ? DE_CONTROL_DIRECTION : 0) |
+		DE_CONTROL_COMMAND_BITBLT | DE_CONTROL_STATUS;
+	write_dpr(accel, DE_CONTROL, de_ctrl); /* dpr0c */
+
+	return 0;
+}
+
+static unsigned int deGetTransparency(struct lynx_accel *accel)
+{
+	unsigned int de_ctrl;
+
+	de_ctrl = read_dpr(accel, DE_CONTROL);
+
+	de_ctrl &= (DE_CONTROL_TRANSPARENCY_MATCH |
+		    DE_CONTROL_TRANSPARENCY_SELECT | DE_CONTROL_TRANSPARENCY);
+
+	return de_ctrl;
+}
+
+/**
+ * sm750_hw_imageblit
+ * @accel: Acceleration device data
+ * @pSrcbuf: pointer to start of source buffer in system memory
+ * @srcDelta: Pitch value (in bytes) of the source buffer, +ive means top down
+ *	      and -ive mean button up
+ * @startBit: Mono data can start at any bit in a byte, this value should be
+ *	      0 to 7
+ * @dBase: Address of destination: offset in frame buffer
+ * @dPitch: Pitch value of destination surface in BYTE
+ * @bytePerPixel: Color depth of destination surface
+ * @dx: Starting x coordinate of destination surface
+ * @dy: Starting y coordinate of destination surface
+ * @width: width of rectangle in pixel value
+ * @height: height of rectangle in pixel value
+ * @fColor: Foreground color (corresponding to a 1 in the monochrome data
+ * @bColor: Background color (corresponding to a 0 in the monochrome data
+ * @rop2: ROP value
+ */
+int sm750_hw_imageblit(struct lynx_accel *accel, const char *pSrcbuf,
+		       u32 srcDelta, u32 startBit, u32 dBase, u32 dPitch,
+		       u32 bytePerPixel, u32 dx, u32 dy, u32 width,
+		       u32 height, u32 fColor, u32 bColor, u32 rop2)
+{
+	unsigned int ulBytesPerScan;
+	unsigned int ul4BytesPerScan;
+	unsigned int ulBytesRemain;
+	unsigned int de_ctrl = 0;
+	unsigned char ajRemain[4];
+	int i, j;
+
+	startBit &= 7; /* Just make sure the start bit is within legal range */
+	ulBytesPerScan = (width + startBit + 7) / 8;
+	ul4BytesPerScan = ulBytesPerScan & ~3;
+	ulBytesRemain = ulBytesPerScan & 3;
+
+	if (accel->de_wait() != 0)
+		return -1;
+
+	/*
+	 * 2D Source Base.
+	 * Use 0 for HOST Blt.
+	 */
+	write_dpr(accel, DE_WINDOW_SOURCE_BASE, 0);
+
+	/* 2D Destination Base.
+	 * It is an address offset (128 bit aligned)
+	 * from the beginning of frame buffer.
+	 */
+	write_dpr(accel, DE_WINDOW_DESTINATION_BASE, dBase);
+
+	/*
+	 * Program pitch (distance between the 1st points of two adjacent
+	 * lines). Note that input pitch is BYTE value, but the 2D Pitch
+	 * register uses pixel values. Need Byte to pixel conversion.
+	 */
+	write_dpr(accel, DE_PITCH,
+		  ((dPitch / bytePerPixel << DE_PITCH_DESTINATION_SHIFT) &
+		   DE_PITCH_DESTINATION_MASK) |
+		  (dPitch / bytePerPixel & DE_PITCH_SOURCE_MASK)); /* dpr10 */
+
+	/*
+	 * Screen Window width in Pixels.
+	 * 2D engine uses this value to calculate the linear address
+	 * in frame buffer for a given point.
+	 */
+	write_dpr(accel, DE_WINDOW_WIDTH,
+		  ((dPitch / bytePerPixel << DE_WINDOW_WIDTH_DST_SHIFT) &
+		   DE_WINDOW_WIDTH_DST_MASK) |
+		  (dPitch / bytePerPixel & DE_WINDOW_WIDTH_SRC_MASK));
+
+	 /*
+	  * Note: For 2D Source in Host Write, only X_K1_MONO field is needed,
+	  * and Y_K2 field is not used.
+	  * For mono bitmap, use startBit for X_K1.
+	  */
+	write_dpr(accel, DE_SOURCE,
+		  (startBit << DE_SOURCE_X_K1_SHIFT) &
+		  DE_SOURCE_X_K1_MONO_MASK); /* dpr00 */
+
+	write_dpr(accel, DE_DESTINATION,
+		  ((dx << DE_DESTINATION_X_SHIFT) & DE_DESTINATION_X_MASK) |
+		  (dy & DE_DESTINATION_Y_MASK)); /* dpr04 */
+
+	write_dpr(accel, DE_DIMENSION,
+		  ((width << DE_DIMENSION_X_SHIFT) & DE_DIMENSION_X_MASK) |
+		  (height & DE_DIMENSION_Y_ET_MASK)); /* dpr08 */
+
+	write_dpr(accel, DE_FOREGROUND, fColor);
+	write_dpr(accel, DE_BACKGROUND, bColor);
+
+	de_ctrl = (rop2 & DE_CONTROL_ROP_MASK) |
+		DE_CONTROL_ROP_SELECT | DE_CONTROL_COMMAND_HOST_WRITE |
+		DE_CONTROL_HOST | DE_CONTROL_STATUS;
+
+	write_dpr(accel, DE_CONTROL, de_ctrl | deGetTransparency(accel));
+
+	/* Write MONO data (line by line) to 2D Engine data port */
+	for (i = 0; i < height; i++) {
+		/* For each line, send the data in chunks of 4 bytes */
+		for (j = 0; j < (ul4BytesPerScan / 4); j++)
+			write_dpPort(accel, *(unsigned int *)(pSrcbuf + (j * 4)));
+
+		if (ulBytesRemain) {
+			memcpy(ajRemain, pSrcbuf + ul4BytesPerScan,
+			       ulBytesRemain);
+			write_dpPort(accel, *(unsigned int *)ajRemain);
+		}
+
+		pSrcbuf += srcDelta;
+	}
+
+	return 0;
+}
+
diff --git a/drivers/staging/sm750fb/sm750_accel.h b/drivers/staging/sm750fb/sm750_accel.h
new file mode 100644
index 000000000..2c79cb730
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_accel.h
@@ -0,0 +1,243 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef ACCEL_H__
+#define ACCEL_H__
+
+#define HW_ROP2_COPY 0xc
+#define HW_ROP2_XOR 0x6
+
+/* notes: below address are the offset value from de_base_address (0x100000)*/
+
+/* for sm718/750/502 de_base is at mmreg_1mb*/
+#define DE_BASE_ADDR_TYPE1 0x100000
+/* for sm712,de_base is at mmreg_32kb */
+#define DE_BASE_ADDR_TYPE2  0x8000
+/* for sm722,de_base is at mmreg_0 */
+#define DE_BASE_ADDR_TYPE3 0
+
+/* type1 data port address is at mmreg_0x110000*/
+#define DE_PORT_ADDR_TYPE1 0x110000
+/* for sm712,data port address is at mmreg_0 */
+#define DE_PORT_ADDR_TYPE2 0x100000
+/* for sm722,data port address is at mmreg_1mb */
+#define DE_PORT_ADDR_TYPE3 0x100000
+
+#define DE_SOURCE                                       0x0
+#define DE_SOURCE_WRAP                                  BIT(31)
+#define DE_SOURCE_X_K1_SHIFT                            16
+#define DE_SOURCE_X_K1_MASK                             (0x3fff << 16)
+#define DE_SOURCE_X_K1_MONO_MASK			(0x1f << 16)
+#define DE_SOURCE_Y_K2_MASK                             0xffff
+
+#define DE_DESTINATION                                  0x4
+#define DE_DESTINATION_WRAP                             BIT(31)
+#define DE_DESTINATION_X_SHIFT                          16
+#define DE_DESTINATION_X_MASK                           (0x1fff << 16)
+#define DE_DESTINATION_Y_MASK                           0xffff
+
+#define DE_DIMENSION                                    0x8
+#define DE_DIMENSION_X_SHIFT                            16
+#define DE_DIMENSION_X_MASK                             (0x1fff << 16)
+#define DE_DIMENSION_Y_ET_MASK                          0x1fff
+
+#define DE_CONTROL                                      0xC
+#define DE_CONTROL_STATUS                               BIT(31)
+#define DE_CONTROL_PATTERN                              BIT(30)
+#define DE_CONTROL_UPDATE_DESTINATION_X                 BIT(29)
+#define DE_CONTROL_QUICK_START                          BIT(28)
+#define DE_CONTROL_DIRECTION                            BIT(27)
+#define DE_CONTROL_MAJOR                                BIT(26)
+#define DE_CONTROL_STEP_X                               BIT(25)
+#define DE_CONTROL_STEP_Y                               BIT(24)
+#define DE_CONTROL_STRETCH                              BIT(23)
+#define DE_CONTROL_HOST                                 BIT(22)
+#define DE_CONTROL_LAST_PIXEL                           BIT(21)
+#define DE_CONTROL_COMMAND_SHIFT                        16
+#define DE_CONTROL_COMMAND_MASK                         (0x1f << 16)
+#define DE_CONTROL_COMMAND_BITBLT                       (0x0 << 16)
+#define DE_CONTROL_COMMAND_RECTANGLE_FILL               (0x1 << 16)
+#define DE_CONTROL_COMMAND_DE_TILE                      (0x2 << 16)
+#define DE_CONTROL_COMMAND_TRAPEZOID_FILL               (0x3 << 16)
+#define DE_CONTROL_COMMAND_ALPHA_BLEND                  (0x4 << 16)
+#define DE_CONTROL_COMMAND_RLE_STRIP                    (0x5 << 16)
+#define DE_CONTROL_COMMAND_SHORT_STROKE                 (0x6 << 16)
+#define DE_CONTROL_COMMAND_LINE_DRAW                    (0x7 << 16)
+#define DE_CONTROL_COMMAND_HOST_WRITE                   (0x8 << 16)
+#define DE_CONTROL_COMMAND_HOST_READ                    (0x9 << 16)
+#define DE_CONTROL_COMMAND_HOST_WRITE_BOTTOM_UP         (0xa << 16)
+#define DE_CONTROL_COMMAND_ROTATE                       (0xb << 16)
+#define DE_CONTROL_COMMAND_FONT                         (0xc << 16)
+#define DE_CONTROL_COMMAND_TEXTURE_LOAD                 (0xe << 16)
+#define DE_CONTROL_ROP_SELECT                           BIT(15)
+#define DE_CONTROL_ROP2_SOURCE                          BIT(14)
+#define DE_CONTROL_MONO_DATA_SHIFT                      12
+#define DE_CONTROL_MONO_DATA_MASK                       (0x3 << 12)
+#define DE_CONTROL_MONO_DATA_NOT_PACKED                 (0x0 << 12)
+#define DE_CONTROL_MONO_DATA_8_PACKED                   (0x1 << 12)
+#define DE_CONTROL_MONO_DATA_16_PACKED                  (0x2 << 12)
+#define DE_CONTROL_MONO_DATA_32_PACKED                  (0x3 << 12)
+#define DE_CONTROL_REPEAT_ROTATE                        BIT(11)
+#define DE_CONTROL_TRANSPARENCY_MATCH                   BIT(10)
+#define DE_CONTROL_TRANSPARENCY_SELECT                  BIT(9)
+#define DE_CONTROL_TRANSPARENCY                         BIT(8)
+#define DE_CONTROL_ROP_MASK                             0xff
+
+/* Pseudo fields. */
+
+#define DE_CONTROL_SHORT_STROKE_DIR_MASK                (0xf << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_225                 (0x0 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_135                 (0x1 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_315                 (0x2 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_45                  (0x3 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_270                 (0x4 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_90                  (0x5 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_180                 (0x8 << 24)
+#define DE_CONTROL_SHORT_STROKE_DIR_0                   (0xa << 24)
+#define DE_CONTROL_ROTATION_MASK                        (0x3 << 24)
+#define DE_CONTROL_ROTATION_0                           (0x0 << 24)
+#define DE_CONTROL_ROTATION_270                         (0x1 << 24)
+#define DE_CONTROL_ROTATION_90                          (0x2 << 24)
+#define DE_CONTROL_ROTATION_180                         (0x3 << 24)
+
+#define DE_PITCH                                        0x000010
+#define DE_PITCH_DESTINATION_SHIFT                      16
+#define DE_PITCH_DESTINATION_MASK                       (0x1fff << 16)
+#define DE_PITCH_SOURCE_MASK                            0x1fff
+
+#define DE_FOREGROUND                                   0x000014
+#define DE_FOREGROUND_COLOR_MASK                        0xffffffff
+
+#define DE_BACKGROUND                                   0x000018
+#define DE_BACKGROUND_COLOR_MASK                        0xffffffff
+
+#define DE_STRETCH_FORMAT                               0x00001C
+#define DE_STRETCH_FORMAT_PATTERN_XY                    BIT(30)
+#define DE_STRETCH_FORMAT_PATTERN_Y_SHIFT               27
+#define DE_STRETCH_FORMAT_PATTERN_Y_MASK                (0x7 << 27)
+#define DE_STRETCH_FORMAT_PATTERN_X_SHIFT               23
+#define DE_STRETCH_FORMAT_PATTERN_X_MASK                (0x7 << 23)
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_SHIFT            20
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_MASK             (0x3 << 20)
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_8                (0x0 << 20)
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_16               (0x1 << 20)
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_32               (0x2 << 20)
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_24               (0x3 << 20)
+#define DE_STRETCH_FORMAT_ADDRESSING_SHIFT              16
+#define DE_STRETCH_FORMAT_ADDRESSING_MASK               (0xf << 16)
+#define DE_STRETCH_FORMAT_ADDRESSING_XY                 (0x0 << 16)
+#define DE_STRETCH_FORMAT_ADDRESSING_LINEAR             (0xf << 16)
+#define DE_STRETCH_FORMAT_SOURCE_HEIGHT_MASK            0xfff
+
+#define DE_COLOR_COMPARE                                0x000020
+#define DE_COLOR_COMPARE_COLOR_MASK                     0xffffff
+
+#define DE_COLOR_COMPARE_MASK                           0x000024
+#define DE_COLOR_COMPARE_MASK_MASK                      0xffffff
+
+#define DE_MASKS                                        0x000028
+#define DE_MASKS_BYTE_MASK                              (0xffff << 16)
+#define DE_MASKS_BIT_MASK                               0xffff
+
+#define DE_CLIP_TL                                      0x00002C
+#define DE_CLIP_TL_TOP_MASK                             (0xffff << 16)
+#define DE_CLIP_TL_STATUS                               BIT(13)
+#define DE_CLIP_TL_INHIBIT                              BIT(12)
+#define DE_CLIP_TL_LEFT_MASK                            0xfff
+
+#define DE_CLIP_BR                                      0x000030
+#define DE_CLIP_BR_BOTTOM_MASK                          (0xffff << 16)
+#define DE_CLIP_BR_RIGHT_MASK                           0x1fff
+
+#define DE_MONO_PATTERN_LOW                             0x000034
+#define DE_MONO_PATTERN_LOW_PATTERN_MASK                0xffffffff
+
+#define DE_MONO_PATTERN_HIGH                            0x000038
+#define DE_MONO_PATTERN_HIGH_PATTERN_MASK               0xffffffff
+
+#define DE_WINDOW_WIDTH                                 0x00003C
+#define DE_WINDOW_WIDTH_DST_SHIFT                       16
+#define DE_WINDOW_WIDTH_DST_MASK                        (0x1fff << 16)
+#define DE_WINDOW_WIDTH_SRC_MASK                        0x1fff
+
+#define DE_WINDOW_SOURCE_BASE                           0x000040
+#define DE_WINDOW_SOURCE_BASE_EXT                       BIT(27)
+#define DE_WINDOW_SOURCE_BASE_CS                        BIT(26)
+#define DE_WINDOW_SOURCE_BASE_ADDRESS_MASK              0x3ffffff
+
+#define DE_WINDOW_DESTINATION_BASE                      0x000044
+#define DE_WINDOW_DESTINATION_BASE_EXT                  BIT(27)
+#define DE_WINDOW_DESTINATION_BASE_CS                   BIT(26)
+#define DE_WINDOW_DESTINATION_BASE_ADDRESS_MASK         0x3ffffff
+
+#define DE_ALPHA                                        0x000048
+#define DE_ALPHA_VALUE_MASK                             0xff
+
+#define DE_WRAP                                         0x00004C
+#define DE_WRAP_X_MASK                                  (0xffff << 16)
+#define DE_WRAP_Y_MASK                                  0xffff
+
+#define DE_STATUS                                       0x000050
+#define DE_STATUS_CSC                                   BIT(1)
+#define DE_STATUS_2D                                    BIT(0)
+
+/* blt direction */
+#define TOP_TO_BOTTOM 0
+#define LEFT_TO_RIGHT 0
+#define BOTTOM_TO_TOP 1
+#define RIGHT_TO_LEFT 1
+
+void sm750_hw_set2dformat(struct lynx_accel *accel, int fmt);
+
+void sm750_hw_de_init(struct lynx_accel *accel);
+
+int sm750_hw_fillrect(struct lynx_accel *accel,
+		      u32 base, u32 pitch, u32 Bpp,
+		      u32 x, u32 y, u32 width, u32 height,
+		      u32 color, u32 rop);
+
+/**
+ * sm750_hm_copyarea
+ * @sBase: Address of source: offset in frame buffer
+ * @sPitch: Pitch value of source surface in BYTE
+ * @sx: Starting x coordinate of source surface
+ * @sy: Starting y coordinate of source surface
+ * @dBase: Address of destination: offset in frame buffer
+ * @dPitch: Pitch value of destination surface in BYTE
+ * @Bpp: Color depth of destination surface
+ * @dx: Starting x coordinate of destination surface
+ * @dy: Starting y coordinate of destination surface
+ * @width: width of rectangle in pixel value
+ * @height: height of rectangle in pixel value
+ * @rop2: ROP value
+ */
+int sm750_hw_copyarea(struct lynx_accel *accel,
+		      unsigned int sBase, unsigned int sPitch,
+		      unsigned int sx, unsigned int sy,
+		      unsigned int dBase, unsigned int dPitch,
+		      unsigned int Bpp, unsigned int dx, unsigned int dy,
+		      unsigned int width, unsigned int height,
+		      unsigned int rop2);
+
+/**
+ * sm750_hw_imageblit
+ * @pSrcbuf: pointer to start of source buffer in system memory
+ * @srcDelta: Pitch value (in bytes) of the source buffer, +ive means top down
+ *>-----      and -ive mean button up
+ * @startBit: Mono data can start at any bit in a byte, this value should be
+ *>-----      0 to 7
+ * @dBase: Address of destination: offset in frame buffer
+ * @dPitch: Pitch value of destination surface in BYTE
+ * @bytePerPixel: Color depth of destination surface
+ * @dx: Starting x coordinate of destination surface
+ * @dy: Starting y coordinate of destination surface
+ * @width: width of rectangle in pixel value
+ * @height: height of rectangle in pixel value
+ * @fColor: Foreground color (corresponding to a 1 in the monochrome data
+ * @bColor: Background color (corresponding to a 0 in the monochrome data
+ * @rop2: ROP value
+ */
+int sm750_hw_imageblit(struct lynx_accel *accel, const char *pSrcbuf,
+		       u32 srcDelta, u32 startBit, u32 dBase, u32 dPitch,
+		       u32 bytePerPixel, u32 dx, u32 dy, u32 width,
+		       u32 height, u32 fColor, u32 bColor, u32 rop2);
+
+#endif
diff --git a/drivers/staging/sm750fb/sm750_cursor.c b/drivers/staging/sm750fb/sm750_cursor.c
new file mode 100644
index 000000000..43e6f52c2
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_cursor.c
@@ -0,0 +1,176 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/pagemap.h>
+#include <linux/console.h>
+#include <linux/platform_device.h>
+#include <linux/screen_info.h>
+
+#include "sm750.h"
+#include "sm750_cursor.h"
+
+#define poke32(addr, data) \
+writel((data), cursor->mmio + (addr))
+
+/* cursor control for voyager and 718/750*/
+#define HWC_ADDRESS                         0x0
+#define HWC_ADDRESS_ENABLE                  BIT(31)
+#define HWC_ADDRESS_EXT                     BIT(27)
+#define HWC_ADDRESS_CS                      BIT(26)
+#define HWC_ADDRESS_ADDRESS_MASK            0x3ffffff
+
+#define HWC_LOCATION                        0x4
+#define HWC_LOCATION_TOP                    BIT(27)
+#define HWC_LOCATION_Y_SHIFT                16
+#define HWC_LOCATION_Y_MASK                 (0x7ff << 16)
+#define HWC_LOCATION_LEFT                   BIT(11)
+#define HWC_LOCATION_X_MASK                 0x7ff
+
+#define HWC_COLOR_12                        0x8
+#define HWC_COLOR_12_2_RGB565_SHIFT         16
+#define HWC_COLOR_12_2_RGB565_MASK          (0xffff << 16)
+#define HWC_COLOR_12_1_RGB565_MASK          0xffff
+
+#define HWC_COLOR_3                         0xC
+#define HWC_COLOR_3_RGB565_MASK             0xffff
+
+/* hw_cursor_xxx works for voyager,718 and 750 */
+void sm750_hw_cursor_enable(struct lynx_cursor *cursor)
+{
+	u32 reg;
+
+	reg = (cursor->offset & HWC_ADDRESS_ADDRESS_MASK) | HWC_ADDRESS_ENABLE;
+	poke32(HWC_ADDRESS, reg);
+}
+
+void sm750_hw_cursor_disable(struct lynx_cursor *cursor)
+{
+	poke32(HWC_ADDRESS, 0);
+}
+
+void sm750_hw_cursor_setSize(struct lynx_cursor *cursor, int w, int h)
+{
+	cursor->w = w;
+	cursor->h = h;
+}
+
+void sm750_hw_cursor_setPos(struct lynx_cursor *cursor, int x, int y)
+{
+	u32 reg;
+
+	reg = ((y << HWC_LOCATION_Y_SHIFT) & HWC_LOCATION_Y_MASK) |
+	       (x & HWC_LOCATION_X_MASK);
+	poke32(HWC_LOCATION, reg);
+}
+
+void sm750_hw_cursor_setColor(struct lynx_cursor *cursor, u32 fg, u32 bg)
+{
+	u32 reg = (fg << HWC_COLOR_12_2_RGB565_SHIFT) &
+		HWC_COLOR_12_2_RGB565_MASK;
+
+	poke32(HWC_COLOR_12, reg | (bg & HWC_COLOR_12_1_RGB565_MASK));
+	poke32(HWC_COLOR_3, 0xffe0);
+}
+
+void sm750_hw_cursor_setData(struct lynx_cursor *cursor, u16 rop,
+			     const u8 *pcol, const u8 *pmsk)
+{
+	int i, j, count, pitch, offset;
+	u8 color, mask, opr;
+	u16 data;
+	void __iomem *pbuffer, *pstart;
+
+	/*  in byte*/
+	pitch = cursor->w >> 3;
+
+	/* in byte	*/
+	count = pitch * cursor->h;
+
+	/* in byte */
+	offset = cursor->max_w * 2 / 8;
+
+	data = 0;
+	pstart = cursor->vstart;
+	pbuffer = pstart;
+
+	for (i = 0; i < count; i++) {
+		color = *pcol++;
+		mask = *pmsk++;
+		data = 0;
+
+		for (j = 0; j < 8; j++) {
+			if (mask & (0x80 >> j)) {
+				if (rop == ROP_XOR)
+					opr = mask ^ color;
+				else
+					opr = mask & color;
+
+				/* 2 stands for forecolor and 1 for backcolor */
+				data |= ((opr & (0x80 >> j)) ? 2 : 1) << (j * 2);
+			}
+		}
+		iowrite16(data, pbuffer);
+
+		/* assume pitch is 1,2,4,8,...*/
+		if ((i + 1) % pitch == 0) {
+			/* need a return */
+			pstart += offset;
+			pbuffer = pstart;
+		} else {
+			pbuffer += sizeof(u16);
+		}
+	}
+}
+
+void sm750_hw_cursor_setData2(struct lynx_cursor *cursor, u16 rop,
+			      const u8 *pcol, const u8 *pmsk)
+{
+	int i, j, count, pitch, offset;
+	u8 color, mask;
+	u16 data;
+	void __iomem *pbuffer, *pstart;
+
+	/*  in byte*/
+	pitch = cursor->w >> 3;
+
+	/* in byte	*/
+	count = pitch * cursor->h;
+
+	/* in byte */
+	offset = cursor->max_w * 2 / 8;
+
+	data = 0;
+	pstart = cursor->vstart;
+	pbuffer = pstart;
+
+	for (i = 0; i < count; i++) {
+		color = *pcol++;
+		mask = *pmsk++;
+		data = 0;
+
+		for (j = 0; j < 8; j++) {
+			if (mask & (1 << j))
+				data |= ((color & (1 << j)) ? 1 : 2) << (j * 2);
+		}
+		iowrite16(data, pbuffer);
+
+		/* assume pitch is 1,2,4,8,...*/
+		if (!(i & (pitch - 1))) {
+			/* need a return */
+			pstart += offset;
+			pbuffer = pstart;
+		} else {
+			pbuffer += sizeof(u16);
+		}
+	}
+}
diff --git a/drivers/staging/sm750fb/sm750_cursor.h b/drivers/staging/sm750fb/sm750_cursor.h
new file mode 100644
index 000000000..b59643dd6
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_cursor.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef LYNX_CURSOR_H__
+#define LYNX_CURSOR_H__
+
+/* hw_cursor_xxx works for voyager,718 and 750 */
+void sm750_hw_cursor_enable(struct lynx_cursor *cursor);
+void sm750_hw_cursor_disable(struct lynx_cursor *cursor);
+void sm750_hw_cursor_setSize(struct lynx_cursor *cursor, int w, int h);
+void sm750_hw_cursor_setPos(struct lynx_cursor *cursor, int x, int y);
+void sm750_hw_cursor_setColor(struct lynx_cursor *cursor, u32 fg, u32 bg);
+void sm750_hw_cursor_setData(struct lynx_cursor *cursor, u16 rop,
+			     const u8 *data, const u8 *mask);
+void sm750_hw_cursor_setData2(struct lynx_cursor *cursor, u16 rop,
+			      const u8 *data, const u8 *mask);
+#endif
diff --git a/drivers/staging/sm750fb/sm750_hw.c b/drivers/staging/sm750fb/sm750_hw.c
new file mode 100644
index 000000000..55cb00e8b
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_hw.c
@@ -0,0 +1,569 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/pagemap.h>
+#include <linux/console.h>
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+#include <linux/platform_device.h>
+#include <linux/screen_info.h>
+#include <linux/sizes.h>
+
+#include "sm750.h"
+#include "ddk750.h"
+#include "sm750_accel.h"
+
+void __iomem *mmio750;
+
+int hw_sm750_map(struct sm750_dev *sm750_dev, struct pci_dev *pdev)
+{
+	int ret;
+
+	ret = 0;
+
+	sm750_dev->vidreg_start = pci_resource_start(pdev, 1);
+	sm750_dev->vidreg_size = SZ_2M;
+
+	pr_info("mmio phyAddr = %lx\n", sm750_dev->vidreg_start);
+
+	/*
+	 * reserve the vidreg space of smi adaptor
+	 * if you do this, you need to add release region code
+	 * in lynxfb_remove, or memory will not be mapped again
+	 * successfully
+	 */
+	ret = pci_request_region(pdev, 1, "sm750fb");
+	if (ret) {
+		pr_err("Can not request PCI regions.\n");
+		goto exit;
+	}
+
+	/* now map mmio and vidmem */
+	sm750_dev->pvReg =
+		ioremap(sm750_dev->vidreg_start, sm750_dev->vidreg_size);
+	if (!sm750_dev->pvReg) {
+		pr_err("mmio failed\n");
+		ret = -EFAULT;
+		goto exit;
+	} else {
+		pr_info("mmio virtual addr = %p\n", sm750_dev->pvReg);
+	}
+
+	sm750_dev->accel.dprBase = sm750_dev->pvReg + DE_BASE_ADDR_TYPE1;
+	sm750_dev->accel.dpPortBase = sm750_dev->pvReg + DE_PORT_ADDR_TYPE1;
+
+	mmio750 = sm750_dev->pvReg;
+	sm750_set_chip_type(sm750_dev->devid, sm750_dev->revid);
+
+	sm750_dev->vidmem_start = pci_resource_start(pdev, 0);
+	/*
+	 * don't use pdev_resource[x].end - resource[x].start to
+	 * calculate the resource size, it's only the maximum available
+	 * size but not the actual size, using
+	 * @ddk750_get_vm_size function can be safe.
+	 */
+	sm750_dev->vidmem_size = ddk750_get_vm_size();
+	pr_info("video memory phyAddr = %lx, size = %u bytes\n",
+		sm750_dev->vidmem_start, sm750_dev->vidmem_size);
+
+	/* reserve the vidmem space of smi adaptor */
+	sm750_dev->pvMem =
+		ioremap_wc(sm750_dev->vidmem_start, sm750_dev->vidmem_size);
+	if (!sm750_dev->pvMem) {
+		iounmap(sm750_dev->pvReg);
+		pr_err("Map video memory failed\n");
+		ret = -EFAULT;
+		goto exit;
+	} else {
+		pr_info("video memory vaddr = %p\n", sm750_dev->pvMem);
+	}
+exit:
+	return ret;
+}
+
+int hw_sm750_inithw(struct sm750_dev *sm750_dev, struct pci_dev *pdev)
+{
+	struct init_status *parm;
+
+	parm = &sm750_dev->initParm;
+	if (parm->chip_clk == 0)
+		parm->chip_clk = (sm750_get_chip_type() == SM750LE) ?
+					       DEFAULT_SM750LE_CHIP_CLOCK :
+					       DEFAULT_SM750_CHIP_CLOCK;
+
+	if (parm->mem_clk == 0)
+		parm->mem_clk = parm->chip_clk;
+	if (parm->master_clk == 0)
+		parm->master_clk = parm->chip_clk / 3;
+
+	ddk750_init_hw((struct initchip_param *)&sm750_dev->initParm);
+	/* for sm718, open pci burst */
+	if (sm750_dev->devid == 0x718) {
+		poke32(SYSTEM_CTRL,
+		       peek32(SYSTEM_CTRL) | SYSTEM_CTRL_PCI_BURST);
+	}
+
+	if (sm750_get_chip_type() != SM750LE) {
+		unsigned int val;
+		/* does user need CRT? */
+		if (sm750_dev->nocrt) {
+			poke32(MISC_CTRL,
+			       peek32(MISC_CTRL) | MISC_CTRL_DAC_POWER_OFF);
+			/* shut off dpms */
+			val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
+			val |= SYSTEM_CTRL_DPMS_VPHN;
+			poke32(SYSTEM_CTRL, val);
+		} else {
+			poke32(MISC_CTRL,
+			       peek32(MISC_CTRL) & ~MISC_CTRL_DAC_POWER_OFF);
+			/* turn on dpms */
+			val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
+			val |= SYSTEM_CTRL_DPMS_VPHP;
+			poke32(SYSTEM_CTRL, val);
+		}
+
+		val = peek32(PANEL_DISPLAY_CTRL) &
+		      ~(PANEL_DISPLAY_CTRL_DUAL_DISPLAY |
+			PANEL_DISPLAY_CTRL_DOUBLE_PIXEL);
+		switch (sm750_dev->pnltype) {
+		case sm750_24TFT:
+			break;
+		case sm750_doubleTFT:
+			val |= PANEL_DISPLAY_CTRL_DOUBLE_PIXEL;
+			break;
+		case sm750_dualTFT:
+			val |= PANEL_DISPLAY_CTRL_DUAL_DISPLAY;
+			break;
+		}
+		poke32(PANEL_DISPLAY_CTRL, val);
+	} else {
+		/*
+		 * for 750LE, no DVI chip initialization
+		 * makes Monitor no signal
+		 *
+		 * Set up GPIO for software I2C to program DVI chip in the
+		 * Xilinx SP605 board, in order to have video signal.
+		 */
+		sm750_sw_i2c_init(0, 1);
+
+		/*
+		 * Customer may NOT use CH7301 DVI chip, which has to be
+		 * initialized differently.
+		 */
+		if (sm750_sw_i2c_read_reg(0xec, 0x4a) == 0x95) {
+			/*
+			 * The following register values for CH7301 are from
+			 * Chrontel app note and our experiment.
+			 */
+			pr_info("yes,CH7301 DVI chip found\n");
+			sm750_sw_i2c_write_reg(0xec, 0x1d, 0x16);
+			sm750_sw_i2c_write_reg(0xec, 0x21, 0x9);
+			sm750_sw_i2c_write_reg(0xec, 0x49, 0xC0);
+			pr_info("okay,CH7301 DVI chip setup done\n");
+		}
+	}
+
+	/* init 2d engine */
+	if (!sm750_dev->accel_off)
+		hw_sm750_initAccel(sm750_dev);
+
+	return 0;
+}
+
+int hw_sm750_output_setMode(struct lynxfb_output *output,
+			    struct fb_var_screeninfo *var,
+			    struct fb_fix_screeninfo *fix)
+{
+	int ret;
+	enum disp_output disp_set;
+	int channel;
+
+	ret = 0;
+	disp_set = 0;
+	channel = *output->channel;
+
+	if (sm750_get_chip_type() != SM750LE) {
+		if (channel == sm750_primary) {
+			pr_info("primary channel\n");
+			if (output->paths & sm750_panel)
+				disp_set |= do_LCD1_PRI;
+			if (output->paths & sm750_crt)
+				disp_set |= do_CRT_PRI;
+
+		} else {
+			pr_info("secondary channel\n");
+			if (output->paths & sm750_panel)
+				disp_set |= do_LCD1_SEC;
+			if (output->paths & sm750_crt)
+				disp_set |= do_CRT_SEC;
+		}
+		ddk750_set_logical_disp_out(disp_set);
+	} else {
+		/* just open DISPLAY_CONTROL_750LE register bit 3:0 */
+		u32 reg;
+
+		reg = peek32(DISPLAY_CONTROL_750LE);
+		reg |= 0xf;
+		poke32(DISPLAY_CONTROL_750LE, reg);
+	}
+
+	pr_info("ddk setlogicdispout done\n");
+	return ret;
+}
+
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc *crtc,
+			    struct fb_var_screeninfo *var)
+{
+	struct sm750_dev *sm750_dev;
+	struct lynxfb_par *par = container_of(crtc, struct lynxfb_par, crtc);
+
+	sm750_dev = par->dev;
+
+	switch (var->bits_per_pixel) {
+	case 8:
+	case 16:
+		break;
+	case 32:
+		if (sm750_dev->revid == SM750LE_REVISION_ID) {
+			pr_debug("750le do not support 32bpp\n");
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* set the controller's mode for @crtc charged with @var and @fix parameters */
+int hw_sm750_crtc_setMode(struct lynxfb_crtc *crtc,
+			  struct fb_var_screeninfo *var,
+			  struct fb_fix_screeninfo *fix)
+{
+	int ret, fmt;
+	u32 reg;
+	struct mode_parameter modparm;
+	enum clock_type clock;
+	struct sm750_dev *sm750_dev;
+	struct lynxfb_par *par;
+
+	ret = 0;
+	par = container_of(crtc, struct lynxfb_par, crtc);
+	sm750_dev = par->dev;
+
+	if (!sm750_dev->accel_off) {
+		/* set 2d engine pixel format according to mode bpp */
+		switch (var->bits_per_pixel) {
+		case 8:
+			fmt = 0;
+			break;
+		case 16:
+			fmt = 1;
+			break;
+		case 32:
+		default:
+			fmt = 2;
+			break;
+		}
+		sm750_hw_set2dformat(&sm750_dev->accel, fmt);
+	}
+
+	/* set timing */
+	modparm.pixel_clock = ps_to_hz(var->pixclock);
+	modparm.vertical_sync_polarity =
+		(var->sync & FB_SYNC_HOR_HIGH_ACT) ? POS : NEG;
+	modparm.horizontal_sync_polarity =
+		(var->sync & FB_SYNC_VERT_HIGH_ACT) ? POS : NEG;
+	modparm.clock_phase_polarity =
+		(var->sync & FB_SYNC_COMP_HIGH_ACT) ? POS : NEG;
+	modparm.horizontal_display_end = var->xres;
+	modparm.horizontal_sync_width = var->hsync_len;
+	modparm.horizontal_sync_start = var->xres + var->right_margin;
+	modparm.horizontal_total = var->xres + var->left_margin +
+				   var->right_margin + var->hsync_len;
+	modparm.vertical_display_end = var->yres;
+	modparm.vertical_sync_height = var->vsync_len;
+	modparm.vertical_sync_start = var->yres + var->lower_margin;
+	modparm.vertical_total = var->yres + var->upper_margin +
+				 var->lower_margin + var->vsync_len;
+
+	/* choose pll */
+	if (crtc->channel != sm750_secondary)
+		clock = PRIMARY_PLL;
+	else
+		clock = SECONDARY_PLL;
+
+	pr_debug("Request pixel clock = %lu\n", modparm.pixel_clock);
+	ret = ddk750_setModeTiming(&modparm, clock);
+	if (ret) {
+		pr_err("Set mode timing failed\n");
+		goto exit;
+	}
+
+	if (crtc->channel != sm750_secondary) {
+		/* set pitch, offset, width, start address, etc... */
+		poke32(PANEL_FB_ADDRESS,
+		       crtc->o_screen & PANEL_FB_ADDRESS_ADDRESS_MASK);
+
+		reg = var->xres * (var->bits_per_pixel >> 3);
+		/*
+		 * crtc->channel is not equal to par->index on numeric,
+		 * be aware of that
+		 */
+		reg = ALIGN(reg, crtc->line_pad);
+		reg = (reg << PANEL_FB_WIDTH_WIDTH_SHIFT) &
+		      PANEL_FB_WIDTH_WIDTH_MASK;
+		reg |= (fix->line_length & PANEL_FB_WIDTH_OFFSET_MASK);
+		poke32(PANEL_FB_WIDTH, reg);
+
+		reg = ((var->xres - 1) << PANEL_WINDOW_WIDTH_WIDTH_SHIFT) &
+		      PANEL_WINDOW_WIDTH_WIDTH_MASK;
+		reg |= (var->xoffset & PANEL_WINDOW_WIDTH_X_MASK);
+		poke32(PANEL_WINDOW_WIDTH, reg);
+
+		reg = (var->yres_virtual - 1)
+		      << PANEL_WINDOW_HEIGHT_HEIGHT_SHIFT;
+		reg &= PANEL_WINDOW_HEIGHT_HEIGHT_MASK;
+		reg |= (var->yoffset & PANEL_WINDOW_HEIGHT_Y_MASK);
+		poke32(PANEL_WINDOW_HEIGHT, reg);
+
+		poke32(PANEL_PLANE_TL, 0);
+
+		reg = ((var->yres - 1) << PANEL_PLANE_BR_BOTTOM_SHIFT) &
+		      PANEL_PLANE_BR_BOTTOM_MASK;
+		reg |= ((var->xres - 1) & PANEL_PLANE_BR_RIGHT_MASK);
+		poke32(PANEL_PLANE_BR, reg);
+
+		/* set pixel format */
+		reg = peek32(PANEL_DISPLAY_CTRL);
+		poke32(PANEL_DISPLAY_CTRL, reg | (var->bits_per_pixel >> 4));
+	} else {
+		/* not implemented now */
+		poke32(CRT_FB_ADDRESS, crtc->o_screen);
+		reg = var->xres * (var->bits_per_pixel >> 3);
+		/*
+		 * crtc->channel is not equal to par->index on numeric,
+		 * be aware of that
+		 */
+		reg = ALIGN(reg, crtc->line_pad) << CRT_FB_WIDTH_WIDTH_SHIFT;
+		reg &= CRT_FB_WIDTH_WIDTH_MASK;
+		reg |= (fix->line_length & CRT_FB_WIDTH_OFFSET_MASK);
+		poke32(CRT_FB_WIDTH, reg);
+
+		/* SET PIXEL FORMAT */
+		reg = peek32(CRT_DISPLAY_CTRL);
+		reg |= ((var->bits_per_pixel >> 4) &
+			CRT_DISPLAY_CTRL_FORMAT_MASK);
+		poke32(CRT_DISPLAY_CTRL, reg);
+	}
+
+exit:
+	return ret;
+}
+
+int hw_sm750_setColReg(struct lynxfb_crtc *crtc, ushort index, ushort red,
+		       ushort green, ushort blue)
+{
+	static unsigned int add[] = { PANEL_PALETTE_RAM, CRT_PALETTE_RAM };
+
+	poke32(add[crtc->channel] + index * 4,
+	       (red << 16) | (green << 8) | blue);
+	return 0;
+}
+
+int hw_sm750le_setBLANK(struct lynxfb_output *output, int blank)
+{
+	int dpms, crtdb;
+
+	switch (blank) {
+	case FB_BLANK_UNBLANK:
+		dpms = CRT_DISPLAY_CTRL_DPMS_0;
+		crtdb = 0;
+		break;
+	case FB_BLANK_NORMAL:
+		dpms = CRT_DISPLAY_CTRL_DPMS_0;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_VSYNC_SUSPEND:
+		dpms = CRT_DISPLAY_CTRL_DPMS_2;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_HSYNC_SUSPEND:
+		dpms = CRT_DISPLAY_CTRL_DPMS_1;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_POWERDOWN:
+		dpms = CRT_DISPLAY_CTRL_DPMS_3;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (output->paths & sm750_crt) {
+		unsigned int val;
+
+		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_DPMS_MASK;
+		poke32(CRT_DISPLAY_CTRL, val | dpms);
+
+		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_BLANK;
+		poke32(CRT_DISPLAY_CTRL, val | crtdb);
+	}
+	return 0;
+}
+
+int hw_sm750_setBLANK(struct lynxfb_output *output, int blank)
+{
+	unsigned int dpms, pps, crtdb;
+
+	dpms = 0;
+	pps = 0;
+	crtdb = 0;
+
+	switch (blank) {
+	case FB_BLANK_UNBLANK:
+		pr_debug("flag = FB_BLANK_UNBLANK\n");
+		dpms = SYSTEM_CTRL_DPMS_VPHP;
+		pps = PANEL_DISPLAY_CTRL_DATA;
+		break;
+	case FB_BLANK_NORMAL:
+		pr_debug("flag = FB_BLANK_NORMAL\n");
+		dpms = SYSTEM_CTRL_DPMS_VPHP;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_VSYNC_SUSPEND:
+		dpms = SYSTEM_CTRL_DPMS_VNHP;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_HSYNC_SUSPEND:
+		dpms = SYSTEM_CTRL_DPMS_VPHN;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	case FB_BLANK_POWERDOWN:
+		dpms = SYSTEM_CTRL_DPMS_VNHN;
+		crtdb = CRT_DISPLAY_CTRL_BLANK;
+		break;
+	}
+
+	if (output->paths & sm750_crt) {
+		unsigned int val = peek32(SYSTEM_CTRL) & ~SYSTEM_CTRL_DPMS_MASK;
+
+		poke32(SYSTEM_CTRL, val | dpms);
+
+		val = peek32(CRT_DISPLAY_CTRL) & ~CRT_DISPLAY_CTRL_BLANK;
+		poke32(CRT_DISPLAY_CTRL, val | crtdb);
+	}
+
+	if (output->paths & sm750_panel) {
+		unsigned int val = peek32(PANEL_DISPLAY_CTRL);
+
+		val &= ~PANEL_DISPLAY_CTRL_DATA;
+		val |= pps;
+		poke32(PANEL_DISPLAY_CTRL, val);
+	}
+
+	return 0;
+}
+
+void hw_sm750_initAccel(struct sm750_dev *sm750_dev)
+{
+	u32 reg;
+
+	sm750_enable_2d_engine(1);
+
+	if (sm750_get_chip_type() == SM750LE) {
+		reg = peek32(DE_STATE1);
+		reg |= DE_STATE1_DE_ABORT;
+		poke32(DE_STATE1, reg);
+
+		reg = peek32(DE_STATE1);
+		reg &= ~DE_STATE1_DE_ABORT;
+		poke32(DE_STATE1, reg);
+
+	} else {
+		/* engine reset */
+		reg = peek32(SYSTEM_CTRL);
+		reg |= SYSTEM_CTRL_DE_ABORT;
+		poke32(SYSTEM_CTRL, reg);
+
+		reg = peek32(SYSTEM_CTRL);
+		reg &= ~SYSTEM_CTRL_DE_ABORT;
+		poke32(SYSTEM_CTRL, reg);
+	}
+
+	/* call 2d init */
+	sm750_dev->accel.de_init(&sm750_dev->accel);
+}
+
+int hw_sm750le_deWait(void)
+{
+	int i = 0x10000000;
+	unsigned int mask = DE_STATE2_DE_STATUS_BUSY | DE_STATE2_DE_FIFO_EMPTY |
+			    DE_STATE2_DE_MEM_FIFO_EMPTY;
+
+	while (i--) {
+		unsigned int val = peek32(DE_STATE2);
+
+		if ((val & mask) ==
+		    (DE_STATE2_DE_FIFO_EMPTY | DE_STATE2_DE_MEM_FIFO_EMPTY))
+			return 0;
+	}
+	/* timeout error */
+	return -1;
+}
+
+int hw_sm750_deWait(void)
+{
+	int i = 0x10000000;
+	unsigned int mask = SYSTEM_CTRL_DE_STATUS_BUSY |
+			    SYSTEM_CTRL_DE_FIFO_EMPTY |
+			    SYSTEM_CTRL_DE_MEM_FIFO_EMPTY;
+
+	while (i--) {
+		unsigned int val = peek32(SYSTEM_CTRL);
+
+		if ((val & mask) ==
+		    (SYSTEM_CTRL_DE_FIFO_EMPTY | SYSTEM_CTRL_DE_MEM_FIFO_EMPTY))
+			return 0;
+	}
+	/* timeout error */
+	return -1;
+}
+
+int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
+			 const struct fb_var_screeninfo *var,
+			 const struct fb_info *info)
+{
+	u32 total;
+	/* check params */
+	if ((var->xoffset + var->xres > var->xres_virtual) ||
+	    (var->yoffset + var->yres > var->yres_virtual)) {
+		return -EINVAL;
+	}
+
+	total = var->yoffset * info->fix.line_length +
+		((var->xoffset * var->bits_per_pixel) >> 3);
+	total += crtc->o_screen;
+	if (crtc->channel == sm750_primary) {
+		poke32(PANEL_FB_ADDRESS,
+		       peek32(PANEL_FB_ADDRESS) |
+			       (total & PANEL_FB_ADDRESS_ADDRESS_MASK));
+	} else {
+		poke32(CRT_FB_ADDRESS,
+		       peek32(CRT_FB_ADDRESS) |
+			       (total & CRT_FB_ADDRESS_ADDRESS_MASK));
+	}
+	return 0;
+}
diff --git a/drivers/staging/vc04_services/Kconfig b/drivers/staging/vc04_services/Kconfig
new file mode 100644
index 000000000..31e58c9d1
--- /dev/null
+++ b/drivers/staging/vc04_services/Kconfig
@@ -0,0 +1,50 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig BCM_VIDEOCORE
+	tristate "Broadcom VideoCore support"
+	depends on OF
+	depends on RASPBERRYPI_FIRMWARE || (COMPILE_TEST && !RASPBERRYPI_FIRMWARE)
+	default y
+	help
+		Support for Broadcom VideoCore services including
+		the BCM2835 family of products which is used
+		by the Raspberry PI.
+
+if BCM_VIDEOCORE
+
+config BCM2835_VCHIQ
+	tristate "BCM2835 VCHIQ"
+	depends on HAS_DMA
+	imply VCHIQ_CDEV
+	help
+		Broadcom BCM2835 and similar SoCs have a VPU called VideoCore. This config
+		enables the VCHIQ driver, which implements a messaging interface between
+		the kernel and the firmware running on VideoCore. Other drivers use this
+		interface to communicate to the VPU. More specifically, the VCHIQ driver is
+		used by audio/video and camera drivers as well as for implementing MMAL
+		API, which is in turn used by several multimedia services on the BCM2835
+		family of SoCs.
+		Defaults to Y when the Broadcom Videocore services are included in
+		the build, N otherwise.
+
+if BCM2835_VCHIQ
+
+config VCHIQ_CDEV
+	bool "VCHIQ Character Driver"
+	help
+		Enable the creation of VCHIQ character driver. The cdev exposes ioctls used
+		by userspace libraries and testing tools to interact with VideoCore, via
+		the VCHIQ core driver (Check BCM2835_VCHIQ for more info).
+		This can be set to 'N' if the VideoCore communication is not needed by
+		userspace but only by other kernel modules (like bcm2835-audio).  If not
+		sure, set this to 'Y'.
+
+endif
+
+source "drivers/staging/vc04_services/bcm2835-audio/Kconfig"
+
+source "drivers/staging/vc04_services/bcm2835-camera/Kconfig"
+
+source "drivers/staging/vc04_services/vchiq-mmal/Kconfig"
+
+endif
+
diff --git a/drivers/staging/vc04_services/Makefile b/drivers/staging/vc04_services/Makefile
new file mode 100644
index 000000000..1fd191e2e
--- /dev/null
+++ b/drivers/staging/vc04_services/Makefile
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_BCM2835_VCHIQ)	+= vchiq.o
+
+vchiq-objs := \
+   interface/vchiq_arm/vchiq_core.o  \
+   interface/vchiq_arm/vchiq_arm.o \
+   interface/vchiq_arm/vchiq_debugfs.o \
+   interface/vchiq_arm/vchiq_connected.o \
+
+ifdef CONFIG_VCHIQ_CDEV
+vchiq-objs += interface/vchiq_arm/vchiq_dev.o
+endif
+
+obj-$(CONFIG_SND_BCM2835)		+= bcm2835-audio/
+obj-$(CONFIG_VIDEO_BCM2835)		+= bcm2835-camera/
+obj-$(CONFIG_BCM2835_VCHIQ_MMAL)	+= vchiq-mmal/
+
+ccflags-y += -I $(srctree)/$(src)/include
+
diff --git a/drivers/staging/vc04_services/bcm2835-audio/Kconfig b/drivers/staging/vc04_services/bcm2835-audio/Kconfig
new file mode 100644
index 000000000..7f22f6c85
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/Kconfig
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+config SND_BCM2835
+	tristate "BCM2835 Audio"
+	depends on (ARCH_BCM2835 || COMPILE_TEST) && SND
+	select SND_PCM
+	select BCM2835_VCHIQ if HAS_DMA
+	help
+	  Say Y or M if you want to support BCM2835 built in audio.
+	  This driver handles both 3.5mm and HDMI audio, by leveraging
+	  the VCHIQ messaging interface between the kernel and the firmware
+	  running on VideoCore.
\ No newline at end of file
diff --git a/drivers/staging/vc04_services/bcm2835-audio/Makefile b/drivers/staging/vc04_services/bcm2835-audio/Makefile
new file mode 100644
index 000000000..d59fe4dde
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_SND_BCM2835)	+= snd-bcm2835.o
+snd-bcm2835-objs		:= bcm2835.o bcm2835-ctl.o bcm2835-pcm.o bcm2835-vchiq.o
+
+ccflags-y += -I $(srctree)/$(src)/../include -D__VCCOREVER__=0x04000000
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c
new file mode 100644
index 000000000..1c1f04012
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c
@@ -0,0 +1,232 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#include <sound/core.h>
+#include <sound/control.h>
+#include <sound/tlv.h>
+#include <sound/asoundef.h>
+
+#include "bcm2835.h"
+
+/* volume maximum and minimum in terms of 0.01dB */
+#define CTRL_VOL_MAX 400
+#define CTRL_VOL_MIN -10239 /* originally -10240 */
+
+static int bcm2835_audio_set_chip_ctls(struct bcm2835_chip *chip)
+{
+	int i, err = 0;
+
+	/* change ctls for all substreams */
+	for (i = 0; i < MAX_SUBSTREAMS; i++) {
+		if (chip->alsa_stream[i]) {
+			err = bcm2835_audio_set_ctls(chip->alsa_stream[i]);
+			if (err < 0)
+				break;
+		}
+	}
+	return err;
+}
+
+static int snd_bcm2835_ctl_info(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_info *uinfo)
+{
+	if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) {
+		uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+		uinfo->count = 1;
+		uinfo->value.integer.min = CTRL_VOL_MIN;
+		uinfo->value.integer.max = CTRL_VOL_MAX; /* 2303 */
+	} else if (kcontrol->private_value == PCM_PLAYBACK_MUTE) {
+		uinfo->type = SNDRV_CTL_ELEM_TYPE_BOOLEAN;
+		uinfo->count = 1;
+		uinfo->value.integer.min = 0;
+		uinfo->value.integer.max = 1;
+	} else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE) {
+		uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+		uinfo->count = 1;
+		uinfo->value.integer.min = 0;
+		uinfo->value.integer.max = AUDIO_DEST_MAX - 1;
+	}
+	return 0;
+}
+
+static int snd_bcm2835_ctl_get(struct snd_kcontrol *kcontrol,
+			       struct snd_ctl_elem_value *ucontrol)
+{
+	struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol);
+
+	mutex_lock(&chip->audio_mutex);
+
+	if (kcontrol->private_value == PCM_PLAYBACK_VOLUME)
+		ucontrol->value.integer.value[0] = chip->volume;
+	else if (kcontrol->private_value == PCM_PLAYBACK_MUTE)
+		ucontrol->value.integer.value[0] = chip->mute;
+	else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE)
+		ucontrol->value.integer.value[0] = chip->dest;
+
+	mutex_unlock(&chip->audio_mutex);
+	return 0;
+}
+
+static int snd_bcm2835_ctl_put(struct snd_kcontrol *kcontrol,
+			       struct snd_ctl_elem_value *ucontrol)
+{
+	struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol);
+	int val, *valp;
+	int changed = 0;
+
+	if (kcontrol->private_value == PCM_PLAYBACK_VOLUME)
+		valp = &chip->volume;
+	else if (kcontrol->private_value == PCM_PLAYBACK_MUTE)
+		valp = &chip->mute;
+	else if (kcontrol->private_value == PCM_PLAYBACK_DEVICE)
+		valp = &chip->dest;
+	else
+		return -EINVAL;
+
+	val = ucontrol->value.integer.value[0];
+	mutex_lock(&chip->audio_mutex);
+	if (val != *valp) {
+		*valp = val;
+		changed = 1;
+		if (bcm2835_audio_set_chip_ctls(chip))
+			dev_err(chip->card->dev, "Failed to set ALSA controls..\n");
+	}
+	mutex_unlock(&chip->audio_mutex);
+	return changed;
+}
+
+static DECLARE_TLV_DB_SCALE(snd_bcm2835_db_scale, CTRL_VOL_MIN, 1, 1);
+
+static const struct snd_kcontrol_new snd_bcm2835_ctl[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "PCM Playback Volume",
+		.access = SNDRV_CTL_ELEM_ACCESS_READWRITE | SNDRV_CTL_ELEM_ACCESS_TLV_READ,
+		.private_value = PCM_PLAYBACK_VOLUME,
+		.info = snd_bcm2835_ctl_info,
+		.get = snd_bcm2835_ctl_get,
+		.put = snd_bcm2835_ctl_put,
+		.tlv = {.p = snd_bcm2835_db_scale}
+	},
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "PCM Playback Switch",
+		.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+		.private_value = PCM_PLAYBACK_MUTE,
+		.info = snd_bcm2835_ctl_info,
+		.get = snd_bcm2835_ctl_get,
+		.put = snd_bcm2835_ctl_put,
+	},
+};
+
+static int snd_bcm2835_spdif_default_info(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_IEC958;
+	uinfo->count = 1;
+	return 0;
+}
+
+static int snd_bcm2835_spdif_default_get(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *ucontrol)
+{
+	struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol);
+	int i;
+
+	mutex_lock(&chip->audio_mutex);
+
+	for (i = 0; i < 4; i++)
+		ucontrol->value.iec958.status[i] =
+			(chip->spdif_status >> (i * 8)) & 0xff;
+
+	mutex_unlock(&chip->audio_mutex);
+	return 0;
+}
+
+static int snd_bcm2835_spdif_default_put(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *ucontrol)
+{
+	struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol);
+	unsigned int val = 0;
+	int i, change;
+
+	mutex_lock(&chip->audio_mutex);
+
+	for (i = 0; i < 4; i++)
+		val |= (unsigned int)ucontrol->value.iec958.status[i] << (i * 8);
+
+	change = val != chip->spdif_status;
+	chip->spdif_status = val;
+
+	mutex_unlock(&chip->audio_mutex);
+	return change;
+}
+
+static int snd_bcm2835_spdif_mask_info(struct snd_kcontrol *kcontrol,
+				       struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_IEC958;
+	uinfo->count = 1;
+	return 0;
+}
+
+static int snd_bcm2835_spdif_mask_get(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *ucontrol)
+{
+	/*
+	 * bcm2835 supports only consumer mode and sets all other format flags
+	 * automatically. So the only thing left is signalling non-audio content
+	 */
+	ucontrol->value.iec958.status[0] = IEC958_AES0_NONAUDIO;
+	return 0;
+}
+
+static const struct snd_kcontrol_new snd_bcm2835_spdif[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_PCM,
+		.name = SNDRV_CTL_NAME_IEC958("", PLAYBACK, DEFAULT),
+		.info = snd_bcm2835_spdif_default_info,
+		.get = snd_bcm2835_spdif_default_get,
+		.put = snd_bcm2835_spdif_default_put
+	},
+	{
+		.access = SNDRV_CTL_ELEM_ACCESS_READ,
+		.iface = SNDRV_CTL_ELEM_IFACE_PCM,
+		.name = SNDRV_CTL_NAME_IEC958("", PLAYBACK, CON_MASK),
+		.info = snd_bcm2835_spdif_mask_info,
+		.get = snd_bcm2835_spdif_mask_get,
+	},
+};
+
+static int create_ctls(struct bcm2835_chip *chip, size_t size,
+		       const struct snd_kcontrol_new *kctls)
+{
+	int i, err;
+
+	for (i = 0; i < size; i++) {
+		err = snd_ctl_add(chip->card, snd_ctl_new1(&kctls[i], chip));
+		if (err < 0)
+			return err;
+	}
+	return 0;
+}
+
+int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip)
+{
+	strscpy(chip->card->mixername, "Broadcom Mixer", sizeof(chip->card->mixername));
+	return create_ctls(chip, ARRAY_SIZE(snd_bcm2835_ctl),
+			   snd_bcm2835_ctl);
+}
+
+int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip)
+{
+	int err;
+
+	strscpy(chip->card->mixername, "Broadcom Mixer", sizeof(chip->card->mixername));
+	err = create_ctls(chip, ARRAY_SIZE(snd_bcm2835_ctl), snd_bcm2835_ctl);
+	if (err < 0)
+		return err;
+	return create_ctls(chip, ARRAY_SIZE(snd_bcm2835_spdif),
+			   snd_bcm2835_spdif);
+}
+
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
new file mode 100644
index 000000000..68e8d491a
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
@@ -0,0 +1,355 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+
+#include <sound/asoundef.h>
+
+#include "bcm2835.h"
+
+/* hardware definition */
+static const struct snd_pcm_hardware snd_bcm2835_playback_hw = {
+	.info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		 SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID |
+		 SNDRV_PCM_INFO_SYNC_APPLPTR | SNDRV_PCM_INFO_BATCH),
+	.formats = SNDRV_PCM_FMTBIT_U8 | SNDRV_PCM_FMTBIT_S16_LE,
+	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_8000_192000,
+	.rate_min = 8000,
+	.rate_max = 192000,
+	.channels_min = 1,
+	.channels_max = 8,
+	.buffer_bytes_max = 512 * 1024,
+	.period_bytes_min = 1 * 1024,
+	.period_bytes_max = 512 * 1024,
+	.periods_min = 1,
+	.periods_max = 128,
+};
+
+static const struct snd_pcm_hardware snd_bcm2835_playback_spdif_hw = {
+	.info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		 SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID |
+		 SNDRV_PCM_INFO_SYNC_APPLPTR | SNDRV_PCM_INFO_BATCH),
+	.formats = SNDRV_PCM_FMTBIT_S16_LE,
+	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_44100 |
+	SNDRV_PCM_RATE_48000,
+	.rate_min = 44100,
+	.rate_max = 48000,
+	.channels_min = 2,
+	.channels_max = 2,
+	.buffer_bytes_max = 128 * 1024,
+	.period_bytes_min = 1 * 1024,
+	.period_bytes_max = 128 * 1024,
+	.periods_min = 1,
+	.periods_max = 128,
+};
+
+static void snd_bcm2835_playback_free(struct snd_pcm_runtime *runtime)
+{
+	kfree(runtime->private_data);
+}
+
+void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream,
+			   unsigned int bytes)
+{
+	struct snd_pcm_substream *substream = alsa_stream->substream;
+	unsigned int pos;
+
+	if (!alsa_stream->period_size)
+		return;
+
+	if (bytes >= alsa_stream->buffer_size) {
+		snd_pcm_stream_lock(substream);
+		snd_pcm_stop(substream,
+			     alsa_stream->draining ?
+			     SNDRV_PCM_STATE_SETUP :
+			     SNDRV_PCM_STATE_XRUN);
+		snd_pcm_stream_unlock(substream);
+		return;
+	}
+
+	pos = atomic_read(&alsa_stream->pos);
+	pos += bytes;
+	pos %= alsa_stream->buffer_size;
+	atomic_set(&alsa_stream->pos, pos);
+
+	alsa_stream->period_offset += bytes;
+	alsa_stream->interpolate_start = ktime_get();
+	if (alsa_stream->period_offset >= alsa_stream->period_size) {
+		alsa_stream->period_offset %= alsa_stream->period_size;
+		snd_pcm_period_elapsed(substream);
+	}
+}
+
+/* open callback */
+static int snd_bcm2835_playback_open_generic(struct snd_pcm_substream *substream, int spdif)
+{
+	struct bcm2835_chip *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream;
+	int idx;
+	int err;
+
+	mutex_lock(&chip->audio_mutex);
+	idx = substream->number;
+
+	if (spdif && chip->opened) {
+		err = -EBUSY;
+		goto out;
+	} else if (!spdif && (chip->opened & (1 << idx))) {
+		err = -EBUSY;
+		goto out;
+	}
+	if (idx >= MAX_SUBSTREAMS) {
+		dev_err(chip->dev,
+			"substream(%d) device doesn't exist max(%d) substreams allowed\n",
+			idx, MAX_SUBSTREAMS);
+		err = -ENODEV;
+		goto out;
+	}
+
+	alsa_stream = kzalloc(sizeof(*alsa_stream), GFP_KERNEL);
+	if (!alsa_stream) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	/* Initialise alsa_stream */
+	alsa_stream->chip = chip;
+	alsa_stream->substream = substream;
+	alsa_stream->idx = idx;
+
+	err = bcm2835_audio_open(alsa_stream);
+	if (err) {
+		kfree(alsa_stream);
+		goto out;
+	}
+	runtime->private_data = alsa_stream;
+	runtime->private_free = snd_bcm2835_playback_free;
+	if (spdif) {
+		runtime->hw = snd_bcm2835_playback_spdif_hw;
+	} else {
+		/* clear spdif status, as we are not in spdif mode */
+		chip->spdif_status = 0;
+		runtime->hw = snd_bcm2835_playback_hw;
+	}
+	/* minimum 16 bytes alignment (for vchiq bulk transfers) */
+	snd_pcm_hw_constraint_step(runtime,
+				   0,
+				   SNDRV_PCM_HW_PARAM_PERIOD_BYTES,
+				   16);
+
+	/* position update is in 10ms order */
+	snd_pcm_hw_constraint_minmax(runtime,
+				     SNDRV_PCM_HW_PARAM_PERIOD_TIME,
+				     10 * 1000, UINT_MAX);
+
+	chip->alsa_stream[idx] = alsa_stream;
+
+	chip->opened |= (1 << idx);
+
+out:
+	mutex_unlock(&chip->audio_mutex);
+
+	return err;
+}
+
+static int snd_bcm2835_playback_open(struct snd_pcm_substream *substream)
+{
+	return snd_bcm2835_playback_open_generic(substream, 0);
+}
+
+static int snd_bcm2835_playback_spdif_open(struct snd_pcm_substream *substream)
+{
+	return snd_bcm2835_playback_open_generic(substream, 1);
+}
+
+static int snd_bcm2835_playback_close(struct snd_pcm_substream *substream)
+{
+	struct bcm2835_alsa_stream *alsa_stream;
+	struct snd_pcm_runtime *runtime;
+	struct bcm2835_chip *chip;
+
+	chip = snd_pcm_substream_chip(substream);
+	mutex_lock(&chip->audio_mutex);
+	runtime = substream->runtime;
+	alsa_stream = runtime->private_data;
+
+	alsa_stream->period_size = 0;
+	alsa_stream->buffer_size = 0;
+
+	bcm2835_audio_close(alsa_stream);
+	alsa_stream->chip->alsa_stream[alsa_stream->idx] = NULL;
+	/*
+	 * Do not free up alsa_stream here, it will be freed up by
+	 * runtime->private_free callback we registered in *_open above
+	 */
+
+	chip->opened &= ~(1 << substream->number);
+
+	mutex_unlock(&chip->audio_mutex);
+
+	return 0;
+}
+
+static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream)
+{
+	struct bcm2835_chip *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
+	int channels;
+	int err;
+
+	/* notify the vchiq that it should enter spdif passthrough mode by
+	 * setting channels=0 (see
+	 * https://github.com/raspberrypi/linux/issues/528)
+	 */
+	if (chip->spdif_status & IEC958_AES0_NONAUDIO)
+		channels = 0;
+	else
+		channels = runtime->channels;
+
+	err = bcm2835_audio_set_params(alsa_stream, channels,
+				       runtime->rate,
+				       snd_pcm_format_width(runtime->format));
+	if (err < 0)
+		return err;
+
+	memset(&alsa_stream->pcm_indirect, 0, sizeof(alsa_stream->pcm_indirect));
+
+	alsa_stream->pcm_indirect.hw_buffer_size =
+		alsa_stream->pcm_indirect.sw_buffer_size =
+		snd_pcm_lib_buffer_bytes(substream);
+
+	alsa_stream->buffer_size = snd_pcm_lib_buffer_bytes(substream);
+	alsa_stream->period_size = snd_pcm_lib_period_bytes(substream);
+	atomic_set(&alsa_stream->pos, 0);
+	alsa_stream->period_offset = 0;
+	alsa_stream->draining = false;
+	alsa_stream->interpolate_start = ktime_get();
+
+	return 0;
+}
+
+static void snd_bcm2835_pcm_transfer(struct snd_pcm_substream *substream,
+				     struct snd_pcm_indirect *rec, size_t bytes)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
+	void *src = (void *)(substream->runtime->dma_area + rec->sw_data);
+
+	bcm2835_audio_write(alsa_stream, bytes, src);
+}
+
+static int snd_bcm2835_pcm_ack(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
+	struct snd_pcm_indirect *pcm_indirect = &alsa_stream->pcm_indirect;
+
+	return snd_pcm_indirect_playback_transfer(substream, pcm_indirect,
+						  snd_bcm2835_pcm_transfer);
+}
+
+/* trigger callback */
+static int snd_bcm2835_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+		return bcm2835_audio_start(alsa_stream);
+	case SNDRV_PCM_TRIGGER_DRAIN:
+		alsa_stream->draining = true;
+		return bcm2835_audio_drain(alsa_stream);
+	case SNDRV_PCM_TRIGGER_STOP:
+		return bcm2835_audio_stop(alsa_stream);
+	default:
+		return -EINVAL;
+	}
+}
+
+/* pointer callback */
+static snd_pcm_uframes_t
+snd_bcm2835_pcm_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
+	ktime_t now = ktime_get();
+
+	/* Give userspace better delay reporting by interpolating between GPU
+	 * notifications, assuming audio speed is close enough to the clock
+	 * used for ktime
+	 */
+
+	if ((ktime_to_ns(alsa_stream->interpolate_start)) &&
+	    (ktime_compare(alsa_stream->interpolate_start, now) < 0)) {
+		u64 interval =
+			(ktime_to_ns(ktime_sub(now,
+				alsa_stream->interpolate_start)));
+		u64 frames_output_in_interval =
+			div_u64((interval * runtime->rate), 1000000000);
+		snd_pcm_sframes_t frames_output_in_interval_sized =
+			-frames_output_in_interval;
+		runtime->delay = frames_output_in_interval_sized;
+	}
+
+	return snd_pcm_indirect_playback_pointer(substream,
+		&alsa_stream->pcm_indirect,
+		atomic_read(&alsa_stream->pos));
+}
+
+/* operators */
+static const struct snd_pcm_ops snd_bcm2835_playback_ops = {
+	.open = snd_bcm2835_playback_open,
+	.close = snd_bcm2835_playback_close,
+	.prepare = snd_bcm2835_pcm_prepare,
+	.trigger = snd_bcm2835_pcm_trigger,
+	.pointer = snd_bcm2835_pcm_pointer,
+	.ack = snd_bcm2835_pcm_ack,
+};
+
+static const struct snd_pcm_ops snd_bcm2835_playback_spdif_ops = {
+	.open = snd_bcm2835_playback_spdif_open,
+	.close = snd_bcm2835_playback_close,
+	.prepare = snd_bcm2835_pcm_prepare,
+	.trigger = snd_bcm2835_pcm_trigger,
+	.pointer = snd_bcm2835_pcm_pointer,
+	.ack = snd_bcm2835_pcm_ack,
+};
+
+/* create a pcm device */
+int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name,
+			int idx, enum snd_bcm2835_route route,
+			u32 numchannels, bool spdif)
+{
+	struct snd_pcm *pcm;
+	int err;
+
+	err = snd_pcm_new(chip->card, name, idx, numchannels, 0, &pcm);
+	if (err)
+		return err;
+
+	pcm->private_data = chip;
+	pcm->nonatomic = true;
+	strscpy(pcm->name, name, sizeof(pcm->name));
+	if (!spdif) {
+		chip->dest = route;
+		chip->volume = 0;
+		chip->mute = CTRL_VOL_UNMUTE;
+	}
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK,
+			spdif ? &snd_bcm2835_playback_spdif_ops :
+			&snd_bcm2835_playback_ops);
+
+	snd_pcm_set_managed_buffer_all(pcm, SNDRV_DMA_TYPE_DEV,
+				       chip->card->dev, 128 * 1024, 128 * 1024);
+
+	if (spdif)
+		chip->pcm_spdif = pcm;
+	else
+		chip->pcm = pcm;
+	return 0;
+}
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
new file mode 100644
index 000000000..f4c2c9506
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
@@ -0,0 +1,378 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/completion.h>
+#include "bcm2835.h"
+#include "vc_vchi_audioserv_defs.h"
+
+struct bcm2835_audio_instance {
+	struct device *dev;
+	unsigned int service_handle;
+	struct completion msg_avail_comp;
+	struct mutex vchi_mutex; /* Serialize vchiq access */
+	struct bcm2835_alsa_stream *alsa_stream;
+	int result;
+	unsigned int max_packet;
+	short peer_version;
+};
+
+static bool force_bulk;
+module_param(force_bulk, bool, 0444);
+MODULE_PARM_DESC(force_bulk, "Force use of vchiq bulk for audio");
+
+static void bcm2835_audio_lock(struct bcm2835_audio_instance *instance)
+{
+	mutex_lock(&instance->vchi_mutex);
+	vchiq_use_service(instance->alsa_stream->chip->vchi_ctx->instance,
+			  instance->service_handle);
+}
+
+static void bcm2835_audio_unlock(struct bcm2835_audio_instance *instance)
+{
+	vchiq_release_service(instance->alsa_stream->chip->vchi_ctx->instance,
+			      instance->service_handle);
+	mutex_unlock(&instance->vchi_mutex);
+}
+
+static int bcm2835_audio_send_msg_locked(struct bcm2835_audio_instance *instance,
+					 struct vc_audio_msg *m, bool wait)
+{
+	int status;
+
+	if (wait) {
+		instance->result = -1;
+		init_completion(&instance->msg_avail_comp);
+	}
+
+	status = vchiq_queue_kernel_message(instance->alsa_stream->chip->vchi_ctx->instance,
+					    instance->service_handle, m, sizeof(*m));
+	if (status) {
+		dev_err(instance->dev,
+			"vchi message queue failed: %d, msg=%d\n",
+			status, m->type);
+		return -EIO;
+	}
+
+	if (wait) {
+		if (!wait_for_completion_timeout(&instance->msg_avail_comp,
+						 msecs_to_jiffies(10 * 1000))) {
+			dev_err(instance->dev,
+				"vchi message timeout, msg=%d\n", m->type);
+			return -ETIMEDOUT;
+		} else if (instance->result) {
+			dev_err(instance->dev,
+				"vchi message response error:%d, msg=%d\n",
+				instance->result, m->type);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int bcm2835_audio_send_msg(struct bcm2835_audio_instance *instance,
+				  struct vc_audio_msg *m, bool wait)
+{
+	int err;
+
+	bcm2835_audio_lock(instance);
+	err = bcm2835_audio_send_msg_locked(instance, m, wait);
+	bcm2835_audio_unlock(instance);
+	return err;
+}
+
+static int bcm2835_audio_send_simple(struct bcm2835_audio_instance *instance,
+				     int type, bool wait)
+{
+	struct vc_audio_msg m = { .type = type };
+
+	return bcm2835_audio_send_msg(instance, &m, wait);
+}
+
+static enum vchiq_status audio_vchi_callback(struct vchiq_instance *vchiq_instance,
+					     enum vchiq_reason reason,
+					     struct vchiq_header *header,
+					     unsigned int handle, void *userdata)
+{
+	struct bcm2835_audio_instance *instance = vchiq_get_service_userdata(vchiq_instance,
+									     handle);
+	struct vc_audio_msg *m;
+
+	if (reason != VCHIQ_MESSAGE_AVAILABLE)
+		return VCHIQ_SUCCESS;
+
+	m = (void *)header->data;
+	if (m->type == VC_AUDIO_MSG_TYPE_RESULT) {
+		instance->result = m->result.success;
+		complete(&instance->msg_avail_comp);
+	} else if (m->type == VC_AUDIO_MSG_TYPE_COMPLETE) {
+		if (m->complete.cookie1 != VC_AUDIO_WRITE_COOKIE1 ||
+		    m->complete.cookie2 != VC_AUDIO_WRITE_COOKIE2)
+			dev_err(instance->dev, "invalid cookie\n");
+		else
+			bcm2835_playback_fifo(instance->alsa_stream,
+					      m->complete.count);
+	} else {
+		dev_err(instance->dev, "unexpected callback type=%d\n", m->type);
+	}
+
+	vchiq_release_message(vchiq_instance, instance->service_handle, header);
+	return VCHIQ_SUCCESS;
+}
+
+static int
+vc_vchi_audio_init(struct vchiq_instance *vchiq_instance,
+		   struct bcm2835_audio_instance *instance)
+{
+	struct vchiq_service_params_kernel params = {
+		.version		= VC_AUDIOSERV_VER,
+		.version_min		= VC_AUDIOSERV_MIN_VER,
+		.fourcc			= VCHIQ_MAKE_FOURCC('A', 'U', 'D', 'S'),
+		.callback		= audio_vchi_callback,
+		.userdata		= instance,
+	};
+	int status;
+
+	/* Open the VCHI service connections */
+	status = vchiq_open_service(vchiq_instance, &params,
+				    &instance->service_handle);
+
+	if (status) {
+		dev_err(instance->dev,
+			"failed to open VCHI service connection (status=%d)\n",
+			status);
+		return -EPERM;
+	}
+
+	/* Finished with the service for now */
+	vchiq_release_service(instance->alsa_stream->chip->vchi_ctx->instance,
+			      instance->service_handle);
+
+	return 0;
+}
+
+static void vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
+{
+	int status;
+
+	mutex_lock(&instance->vchi_mutex);
+	vchiq_use_service(instance->alsa_stream->chip->vchi_ctx->instance,
+			  instance->service_handle);
+
+	/* Close all VCHI service connections */
+	status = vchiq_close_service(instance->alsa_stream->chip->vchi_ctx->instance,
+				     instance->service_handle);
+	if (status) {
+		dev_err(instance->dev,
+			"failed to close VCHI service connection (status=%d)\n",
+			status);
+	}
+
+	mutex_unlock(&instance->vchi_mutex);
+}
+
+int bcm2835_new_vchi_ctx(struct device *dev, struct bcm2835_vchi_ctx *vchi_ctx)
+{
+	int ret;
+
+	/* Initialize and create a VCHI connection */
+	ret = vchiq_initialise(&vchi_ctx->instance);
+	if (ret) {
+		dev_err(dev, "failed to initialise VCHI instance (ret=%d)\n",
+			ret);
+		return -EIO;
+	}
+
+	ret = vchiq_connect(vchi_ctx->instance);
+	if (ret) {
+		dev_dbg(dev, "failed to connect VCHI instance (ret=%d)\n",
+			ret);
+
+		kfree(vchi_ctx->instance);
+		vchi_ctx->instance = NULL;
+
+		return -EIO;
+	}
+
+	return 0;
+}
+
+void bcm2835_free_vchi_ctx(struct bcm2835_vchi_ctx *vchi_ctx)
+{
+	/* Close the VCHI connection - it will also free vchi_ctx->instance */
+	WARN_ON(vchiq_shutdown(vchi_ctx->instance));
+
+	vchi_ctx->instance = NULL;
+}
+
+int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream)
+{
+	struct bcm2835_vchi_ctx *vchi_ctx = alsa_stream->chip->vchi_ctx;
+	struct bcm2835_audio_instance *instance;
+	int err;
+
+	/* Allocate memory for this instance */
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance)
+		return -ENOMEM;
+	mutex_init(&instance->vchi_mutex);
+	instance->dev = alsa_stream->chip->dev;
+	instance->alsa_stream = alsa_stream;
+	alsa_stream->instance = instance;
+
+	err = vc_vchi_audio_init(vchi_ctx->instance,
+				 instance);
+	if (err < 0)
+		goto free_instance;
+
+	err = bcm2835_audio_send_simple(instance, VC_AUDIO_MSG_TYPE_OPEN,
+					false);
+	if (err < 0)
+		goto deinit;
+
+	bcm2835_audio_lock(instance);
+	vchiq_get_peer_version(vchi_ctx->instance, instance->service_handle,
+			       &instance->peer_version);
+	bcm2835_audio_unlock(instance);
+	if (instance->peer_version < 2 || force_bulk)
+		instance->max_packet = 0; /* bulk transfer */
+	else
+		instance->max_packet = 4000;
+
+	return 0;
+
+ deinit:
+	vc_vchi_audio_deinit(instance);
+ free_instance:
+	alsa_stream->instance = NULL;
+	kfree(instance);
+	return err;
+}
+
+int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream)
+{
+	struct bcm2835_chip *chip = alsa_stream->chip;
+	struct vc_audio_msg m = {};
+
+	m.type = VC_AUDIO_MSG_TYPE_CONTROL;
+	m.control.dest = chip->dest;
+	if (!chip->mute)
+		m.control.volume = CHIP_MIN_VOLUME;
+	else
+		m.control.volume = alsa2chip(chip->volume);
+
+	return bcm2835_audio_send_msg(alsa_stream->instance, &m, true);
+}
+
+int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream,
+			     unsigned int channels, unsigned int samplerate,
+			     unsigned int bps)
+{
+	struct vc_audio_msg m = {
+		 .type = VC_AUDIO_MSG_TYPE_CONFIG,
+		 .config.channels = channels,
+		 .config.samplerate = samplerate,
+		 .config.bps = bps,
+	};
+	int err;
+
+	/* resend ctls - alsa_stream may not have been open when first send */
+	err = bcm2835_audio_set_ctls(alsa_stream);
+	if (err)
+		return err;
+
+	return bcm2835_audio_send_msg(alsa_stream->instance, &m, true);
+}
+
+int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream)
+{
+	return bcm2835_audio_send_simple(alsa_stream->instance,
+					 VC_AUDIO_MSG_TYPE_START, false);
+}
+
+int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream)
+{
+	return bcm2835_audio_send_simple(alsa_stream->instance,
+					 VC_AUDIO_MSG_TYPE_STOP, false);
+}
+
+/* FIXME: this doesn't seem working as expected for "draining" */
+int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream)
+{
+	struct vc_audio_msg m = {
+		.type = VC_AUDIO_MSG_TYPE_STOP,
+		.stop.draining = 1,
+	};
+
+	return bcm2835_audio_send_msg(alsa_stream->instance, &m, false);
+}
+
+int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream)
+{
+	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+	int err;
+
+	err = bcm2835_audio_send_simple(alsa_stream->instance,
+					VC_AUDIO_MSG_TYPE_CLOSE, true);
+
+	/* Stop the audio service */
+	vc_vchi_audio_deinit(instance);
+	alsa_stream->instance = NULL;
+	kfree(instance);
+
+	return err;
+}
+
+int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
+			unsigned int size, void *src)
+{
+	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+	struct bcm2835_vchi_ctx *vchi_ctx = alsa_stream->chip->vchi_ctx;
+	struct vchiq_instance *vchiq_instance = vchi_ctx->instance;
+	struct vc_audio_msg m = {
+		.type = VC_AUDIO_MSG_TYPE_WRITE,
+		.write.count = size,
+		.write.max_packet = instance->max_packet,
+		.write.cookie1 = VC_AUDIO_WRITE_COOKIE1,
+		.write.cookie2 = VC_AUDIO_WRITE_COOKIE2,
+	};
+	unsigned int count;
+	int err, status;
+
+	if (!size)
+		return 0;
+
+	bcm2835_audio_lock(instance);
+	err = bcm2835_audio_send_msg_locked(instance, &m, false);
+	if (err < 0)
+		goto unlock;
+
+	count = size;
+	if (!instance->max_packet) {
+		/* Send the message to the videocore */
+		status = vchiq_bulk_transmit(vchiq_instance, instance->service_handle, src, count,
+					     NULL, VCHIQ_BULK_MODE_BLOCKING);
+	} else {
+		while (count > 0) {
+			int bytes = min(instance->max_packet, count);
+
+			status = vchiq_queue_kernel_message(vchiq_instance,
+							    instance->service_handle, src, bytes);
+			src += bytes;
+			count -= bytes;
+		}
+	}
+
+	if (status) {
+		dev_err(instance->dev,
+			"failed on %d bytes transfer (status=%d)\n",
+			size, status);
+		err = -EIO;
+	}
+
+ unlock:
+	bcm2835_audio_unlock(instance);
+	return err;
+}
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
new file mode 100644
index 000000000..00bc898b0
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
@@ -0,0 +1,323 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#include <linux/platform_device.h>
+
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+
+#include "bcm2835.h"
+
+static bool enable_hdmi;
+static bool enable_headphones = true;
+static int num_channels = MAX_SUBSTREAMS;
+
+module_param(enable_hdmi, bool, 0444);
+MODULE_PARM_DESC(enable_hdmi, "Enables HDMI virtual audio device");
+module_param(enable_headphones, bool, 0444);
+MODULE_PARM_DESC(enable_headphones, "Enables Headphones virtual audio device");
+module_param(num_channels, int, 0644);
+MODULE_PARM_DESC(num_channels, "Number of audio channels (default: 8)");
+
+static void bcm2835_devm_free_vchi_ctx(struct device *dev, void *res)
+{
+	struct bcm2835_vchi_ctx *vchi_ctx = res;
+
+	bcm2835_free_vchi_ctx(vchi_ctx);
+}
+
+static int bcm2835_devm_add_vchi_ctx(struct device *dev)
+{
+	struct bcm2835_vchi_ctx *vchi_ctx;
+	int ret;
+
+	vchi_ctx = devres_alloc(bcm2835_devm_free_vchi_ctx, sizeof(*vchi_ctx),
+				GFP_KERNEL);
+	if (!vchi_ctx)
+		return -ENOMEM;
+
+	ret = bcm2835_new_vchi_ctx(dev, vchi_ctx);
+	if (ret) {
+		devres_free(vchi_ctx);
+		return ret;
+	}
+
+	devres_add(dev, vchi_ctx);
+
+	return 0;
+}
+
+struct bcm2835_audio_driver {
+	struct device_driver driver;
+	const char *shortname;
+	const char *longname;
+	int minchannels;
+	int (*newpcm)(struct bcm2835_chip *chip, const char *name,
+		      enum snd_bcm2835_route route, u32 numchannels);
+	int (*newctl)(struct bcm2835_chip *chip);
+	enum snd_bcm2835_route route;
+};
+
+static int bcm2835_audio_dual_newpcm(struct bcm2835_chip *chip,
+				     const char *name,
+				     enum snd_bcm2835_route route,
+				     u32 numchannels)
+{
+	int err;
+
+	err = snd_bcm2835_new_pcm(chip, name, 0, route,
+				  numchannels, false);
+
+	if (err)
+		return err;
+
+	err = snd_bcm2835_new_pcm(chip, "IEC958", 1, route, 1, true);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int bcm2835_audio_simple_newpcm(struct bcm2835_chip *chip,
+				       const char *name,
+				       enum snd_bcm2835_route route,
+				       u32 numchannels)
+{
+	return snd_bcm2835_new_pcm(chip, name, 0, route, numchannels, false);
+}
+
+static struct bcm2835_audio_driver bcm2835_audio_hdmi = {
+	.driver = {
+		.name = "bcm2835_hdmi",
+		.owner = THIS_MODULE,
+	},
+	.shortname = "bcm2835 HDMI",
+	.longname  = "bcm2835 HDMI",
+	.minchannels = 1,
+	.newpcm = bcm2835_audio_dual_newpcm,
+	.newctl = snd_bcm2835_new_hdmi_ctl,
+	.route = AUDIO_DEST_HDMI
+};
+
+static struct bcm2835_audio_driver bcm2835_audio_headphones = {
+	.driver = {
+		.name = "bcm2835_headphones",
+		.owner = THIS_MODULE,
+	},
+	.shortname = "bcm2835 Headphones",
+	.longname  = "bcm2835 Headphones",
+	.minchannels = 1,
+	.newpcm = bcm2835_audio_simple_newpcm,
+	.newctl = snd_bcm2835_new_headphones_ctl,
+	.route = AUDIO_DEST_HEADPHONES
+};
+
+struct bcm2835_audio_drivers {
+	struct bcm2835_audio_driver *audio_driver;
+	const bool *is_enabled;
+};
+
+static struct bcm2835_audio_drivers children_devices[] = {
+	{
+		.audio_driver = &bcm2835_audio_hdmi,
+		.is_enabled = &enable_hdmi,
+	},
+	{
+		.audio_driver = &bcm2835_audio_headphones,
+		.is_enabled = &enable_headphones,
+	},
+};
+
+static void bcm2835_card_free(void *data)
+{
+	snd_card_free(data);
+}
+
+static int snd_add_child_device(struct device *dev,
+				struct bcm2835_audio_driver *audio_driver,
+				u32 numchans)
+{
+	struct bcm2835_chip *chip;
+	struct snd_card *card;
+	int err;
+
+	err = snd_card_new(dev, -1, NULL, THIS_MODULE, sizeof(*chip), &card);
+	if (err < 0) {
+		dev_err(dev, "Failed to create card");
+		return err;
+	}
+
+	chip = card->private_data;
+	chip->card = card;
+	chip->dev = dev;
+	mutex_init(&chip->audio_mutex);
+
+	chip->vchi_ctx = devres_find(dev,
+				     bcm2835_devm_free_vchi_ctx, NULL, NULL);
+	if (!chip->vchi_ctx) {
+		err = -ENODEV;
+		goto error;
+	}
+
+	strscpy(card->driver, audio_driver->driver.name, sizeof(card->driver));
+	strscpy(card->shortname, audio_driver->shortname, sizeof(card->shortname));
+	strscpy(card->longname, audio_driver->longname, sizeof(card->longname));
+
+	err = audio_driver->newpcm(chip, audio_driver->shortname,
+		audio_driver->route,
+		numchans);
+	if (err) {
+		dev_err(dev, "Failed to create pcm, error %d\n", err);
+		goto error;
+	}
+
+	err = audio_driver->newctl(chip);
+	if (err) {
+		dev_err(dev, "Failed to create controls, error %d\n", err);
+		goto error;
+	}
+
+	err = snd_card_register(card);
+	if (err) {
+		dev_err(dev, "Failed to register card, error %d\n", err);
+		goto error;
+	}
+
+	dev_set_drvdata(dev, chip);
+
+	err = devm_add_action(dev, bcm2835_card_free, card);
+	if (err < 0) {
+		dev_err(dev, "Failed to add devm action, err %d\n", err);
+		goto error;
+	}
+
+	dev_info(dev, "card created with %d channels\n", numchans);
+	return 0;
+
+ error:
+	snd_card_free(card);
+	return err;
+}
+
+static int snd_add_child_devices(struct device *device, u32 numchans)
+{
+	int extrachannels_per_driver = 0;
+	int extrachannels_remainder = 0;
+	int count_devices = 0;
+	int extrachannels = 0;
+	int minchannels = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(children_devices); i++)
+		if (*children_devices[i].is_enabled)
+			count_devices++;
+
+	if (!count_devices)
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(children_devices); i++)
+		if (*children_devices[i].is_enabled)
+			minchannels +=
+				children_devices[i].audio_driver->minchannels;
+
+	if (minchannels < numchans) {
+		extrachannels = numchans - minchannels;
+		extrachannels_per_driver = extrachannels / count_devices;
+		extrachannels_remainder = extrachannels % count_devices;
+	}
+
+	dev_dbg(device, "minchannels %d\n", minchannels);
+	dev_dbg(device, "extrachannels %d\n", extrachannels);
+	dev_dbg(device, "extrachannels_per_driver %d\n",
+		extrachannels_per_driver);
+	dev_dbg(device, "extrachannels_remainder %d\n",
+		extrachannels_remainder);
+
+	for (i = 0; i < ARRAY_SIZE(children_devices); i++) {
+		struct bcm2835_audio_driver *audio_driver;
+		int numchannels_this_device;
+		int err;
+
+		if (!*children_devices[i].is_enabled)
+			continue;
+
+		audio_driver = children_devices[i].audio_driver;
+
+		if (audio_driver->minchannels > numchans) {
+			dev_err(device,
+				"Out of channels, needed %d but only %d left\n",
+				audio_driver->minchannels,
+				numchans);
+			continue;
+		}
+
+		numchannels_this_device =
+			audio_driver->minchannels + extrachannels_per_driver +
+			extrachannels_remainder;
+		extrachannels_remainder = 0;
+
+		numchans -= numchannels_this_device;
+
+		err = snd_add_child_device(device, audio_driver,
+					   numchannels_this_device);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int snd_bcm2835_alsa_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	int err;
+
+	if (num_channels <= 0 || num_channels > MAX_SUBSTREAMS) {
+		num_channels = MAX_SUBSTREAMS;
+		dev_warn(dev, "Illegal num_channels value, will use %u\n",
+			 num_channels);
+	}
+
+	err = bcm2835_devm_add_vchi_ctx(dev);
+	if (err)
+		return err;
+
+	err = snd_add_child_devices(dev, num_channels);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int snd_bcm2835_alsa_suspend(struct platform_device *pdev,
+				    pm_message_t state)
+{
+	return 0;
+}
+
+static int snd_bcm2835_alsa_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+#endif
+
+static struct platform_driver bcm2835_alsa_driver = {
+	.probe = snd_bcm2835_alsa_probe,
+#ifdef CONFIG_PM
+	.suspend = snd_bcm2835_alsa_suspend,
+	.resume = snd_bcm2835_alsa_resume,
+#endif
+	.driver = {
+		.name = "bcm2835_audio",
+	},
+};
+module_platform_driver(bcm2835_alsa_driver);
+
+MODULE_AUTHOR("Dom Cobley");
+MODULE_DESCRIPTION("Alsa driver for BCM2835 chip");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:bcm2835_audio");
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
new file mode 100644
index 000000000..38b7451d7
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
@@ -0,0 +1,113 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#ifndef __SOUND_ARM_BCM2835_H
+#define __SOUND_ARM_BCM2835_H
+
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/raspberrypi/vchiq.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm-indirect.h>
+
+#define MAX_SUBSTREAMS   (8)
+#define AVAIL_SUBSTREAMS_MASK  (0xff)
+
+enum {
+	CTRL_VOL_MUTE,
+	CTRL_VOL_UNMUTE
+};
+
+/* macros for alsa2chip and chip2alsa, instead of functions */
+
+// convert alsa to chip volume (defined as macro rather than function call)
+#define alsa2chip(vol) ((uint)(-(((vol) << 8) / 100)))
+
+// convert chip to alsa volume
+#define chip2alsa(vol) -(((vol) * 100) >> 8)
+
+#define CHIP_MIN_VOLUME		26214 /* minimum level aka mute */
+
+/* Some constants for values .. */
+enum snd_bcm2835_route {
+	AUDIO_DEST_AUTO = 0,
+	AUDIO_DEST_HEADPHONES = 1,
+	AUDIO_DEST_HDMI = 2,
+	AUDIO_DEST_MAX,
+};
+
+enum snd_bcm2835_ctrl {
+	PCM_PLAYBACK_VOLUME,
+	PCM_PLAYBACK_MUTE,
+	PCM_PLAYBACK_DEVICE,
+};
+
+struct bcm2835_vchi_ctx {
+	struct vchiq_instance *instance;
+};
+
+/* definition of the chip-specific record */
+struct bcm2835_chip {
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_pcm *pcm_spdif;
+	struct device *dev;
+	struct bcm2835_alsa_stream *alsa_stream[MAX_SUBSTREAMS];
+
+	int volume;
+	int dest;
+	int mute;
+
+	unsigned int opened;
+	unsigned int spdif_status;
+	struct mutex audio_mutex; /* Serialize chip data access */
+
+	struct bcm2835_vchi_ctx *vchi_ctx;
+};
+
+struct bcm2835_alsa_stream {
+	struct bcm2835_chip *chip;
+	struct snd_pcm_substream *substream;
+	struct snd_pcm_indirect pcm_indirect;
+
+	int draining;
+
+	atomic_t pos;
+	unsigned int period_offset;
+	unsigned int buffer_size;
+	unsigned int period_size;
+	ktime_t interpolate_start;
+
+	struct bcm2835_audio_instance *instance;
+	int idx;
+};
+
+int snd_bcm2835_new_ctl(struct bcm2835_chip *chip);
+int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name,
+			int idx, enum snd_bcm2835_route route,
+			u32 numchannels, bool spdif);
+
+int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip);
+int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip);
+
+int bcm2835_new_vchi_ctx(struct device *dev, struct bcm2835_vchi_ctx *vchi_ctx);
+void bcm2835_free_vchi_ctx(struct bcm2835_vchi_ctx *vchi_ctx);
+
+int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream,
+			     unsigned int channels, unsigned int samplerate,
+			     unsigned int bps);
+int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream);
+int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
+			unsigned int count,
+			void *src);
+void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream,
+			   unsigned int size);
+unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream);
+
+#endif /* __SOUND_ARM_BCM2835_H */
diff --git a/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h
new file mode 100644
index 000000000..b4fa239c5
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright 2011 Broadcom Corporation.  All rights reserved. */
+
+#ifndef _VC_AUDIO_DEFS_H_
+#define _VC_AUDIO_DEFS_H_
+
+#define VC_AUDIOSERV_MIN_VER 1
+#define VC_AUDIOSERV_VER 2
+
+/* FourCC codes used for VCHI communication */
+#define VC_AUDIO_WRITE_COOKIE1 VCHIQ_MAKE_FOURCC('B', 'C', 'M', 'A')
+#define VC_AUDIO_WRITE_COOKIE2 VCHIQ_MAKE_FOURCC('D', 'A', 'T', 'A')
+
+/*
+ *  List of screens that are currently supported
+ *  All message types supported for HOST->VC direction
+ */
+
+enum vc_audio_msg_type {
+	VC_AUDIO_MSG_TYPE_RESULT, // Generic result
+	VC_AUDIO_MSG_TYPE_COMPLETE, // Generic result
+	VC_AUDIO_MSG_TYPE_CONFIG, // Configure audio
+	VC_AUDIO_MSG_TYPE_CONTROL, // Configure audio
+	VC_AUDIO_MSG_TYPE_OPEN, // Configure audio
+	VC_AUDIO_MSG_TYPE_CLOSE, // Configure audio
+	VC_AUDIO_MSG_TYPE_START, // Configure audio
+	VC_AUDIO_MSG_TYPE_STOP, // Configure audio
+	VC_AUDIO_MSG_TYPE_WRITE, // Configure audio
+	VC_AUDIO_MSG_TYPE_MAX
+};
+
+/* configure the audio */
+
+struct vc_audio_config {
+	u32 channels;
+	u32 samplerate;
+	u32 bps;
+};
+
+struct vc_audio_control {
+	u32 volume;
+	u32 dest;
+};
+
+struct vc_audio_open {
+	u32 dummy;
+};
+
+struct vc_audio_close {
+	u32 dummy;
+};
+
+struct vc_audio_start {
+	u32 dummy;
+};
+
+struct vc_audio_stop {
+	u32 draining;
+};
+
+/* configure the write audio samples */
+struct vc_audio_write {
+	u32 count; // in bytes
+	u32 cookie1;
+	u32 cookie2;
+	s16 silence;
+	s16 max_packet;
+};
+
+/* Generic result for a request (VC->HOST) */
+struct vc_audio_result {
+	s32 success; // Success value
+};
+
+/* Generic result for a request (VC->HOST) */
+struct vc_audio_complete {
+	s32 count; // Success value
+	u32 cookie1;
+	u32 cookie2;
+};
+
+/* Message header for all messages in HOST->VC direction */
+struct vc_audio_msg {
+	s32 type; /* Message type (VC_AUDIO_MSG_TYPE) */
+	union {
+		struct vc_audio_config config;
+		struct vc_audio_control control;
+		struct vc_audio_open open;
+		struct vc_audio_close close;
+		struct vc_audio_start start;
+		struct vc_audio_stop stop;
+		struct vc_audio_write write;
+		struct vc_audio_result result;
+		struct vc_audio_complete complete;
+	};
+};
+
+#endif /* _VC_AUDIO_DEFS_H_ */
diff --git a/drivers/staging/vc04_services/bcm2835-camera/Kconfig b/drivers/staging/vc04_services/bcm2835-camera/Kconfig
new file mode 100644
index 000000000..870c9afb2
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_BCM2835
+	tristate "BCM2835 Camera"
+	depends on MEDIA_SUPPORT
+	depends on VIDEO_DEV && (ARCH_BCM2835 || COMPILE_TEST)
+	select BCM2835_VCHIQ if HAS_DMA
+	select BCM2835_VCHIQ_MMAL if HAS_DMA
+	select VIDEOBUF2_VMALLOC
+	select BTREE
+	help
+	  Say Y here to enable camera host interface devices for
+	  Broadcom BCM2835 SoC. This operates over the VCHIQ interface
+	  to a service running on VideoCore.
diff --git a/drivers/staging/vc04_services/bcm2835-camera/Makefile b/drivers/staging/vc04_services/bcm2835-camera/Makefile
new file mode 100644
index 000000000..3a76d6ade
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/Makefile
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+bcm2835-v4l2-$(CONFIG_VIDEO_BCM2835) := \
+	bcm2835-camera.o \
+	controls.o
+
+obj-$(CONFIG_VIDEO_BCM2835) += bcm2835-v4l2.o
+
+ccflags-y += \
+	-I $(srctree)/$(src)/.. \
+	-I $(srctree)/$(src)/../vchiq-mmal/ \
+	-D__VCCOREVER__=0x04000000
diff --git a/drivers/staging/vc04_services/bcm2835-camera/TODO b/drivers/staging/vc04_services/bcm2835-camera/TODO
new file mode 100644
index 000000000..6c2b4ffe4
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/TODO
@@ -0,0 +1,17 @@
+1) Support dma-buf memory management.
+
+In order to zero-copy import camera images into the 3D or display
+pipelines, we need to export our buffers through dma-buf so that the
+vc4 driver can import them.  This may involve bringing in the VCSM
+driver (which allows long-term management of regions of memory in the
+space that the VPU reserved and Linux otherwise doesn't have access
+to), or building some new protocol that allows VCSM-style management
+of Linux's CMA memory.
+
+2) Avoid extra copies for padding of images.
+
+We expose V4L2_PIX_FMT_* formats that have a specified stride/height
+padding in the V4L2 spec, but that padding doesn't match what the
+hardware can do.  If we exposed the native padding requirements
+through the V4L2 "multiplanar" formats, the firmware would have one
+less copy it needed to do.
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
new file mode 100644
index 000000000..fd456d1f7
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
@@ -0,0 +1,2006 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.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 <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include "mmal-common.h"
+#include "mmal-encodings.h"
+#include "mmal-vchiq.h"
+#include "mmal-msg.h"
+#include "mmal-parameters.h"
+#include "bcm2835-camera.h"
+
+#define MIN_WIDTH 32
+#define MIN_HEIGHT 32
+#define MIN_BUFFER_SIZE (80 * 1024)
+
+#define MAX_VIDEO_MODE_WIDTH 1280
+#define MAX_VIDEO_MODE_HEIGHT 720
+
+#define MAX_BCM2835_CAMERAS 2
+
+int bcm2835_v4l2_debug;
+module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
+MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
+
+#define UNSET (-1)
+static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
+module_param_array(video_nr, int, NULL, 0644);
+MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
+
+static int max_video_width = MAX_VIDEO_MODE_WIDTH;
+static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
+module_param(max_video_width, int, 0644);
+MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
+module_param(max_video_height, int, 0644);
+MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
+
+/* camera instance counter */
+static atomic_t camera_instance = ATOMIC_INIT(0);
+
+/* global device data array */
+static struct bcm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
+
+#define FPS_MIN 1
+#define FPS_MAX 90
+
+/* timeperframe: min/max and default */
+static const struct v4l2_fract
+	tpf_min     = {.numerator = 1,		.denominator = FPS_MAX},
+	tpf_max     = {.numerator = 1,	        .denominator = FPS_MIN},
+	tpf_default = {.numerator = 1000,	.denominator = 30000};
+
+/* Container for MMAL and VB2 buffers*/
+struct vb2_mmal_buffer {
+	struct vb2_v4l2_buffer	vb;
+	struct mmal_buffer	mmal;
+};
+
+/* video formats */
+static struct mmal_fmt formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_YUV420,
+		.mmal = MMAL_ENCODING_I420,
+		.depth = 12,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 1,
+		.remove_padding = 1,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUYV,
+		.mmal = MMAL_ENCODING_YUYV,
+		.depth = 16,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 2,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_RGB24,
+		.mmal = MMAL_ENCODING_RGB24,
+		.depth = 24,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 3,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_JPEG,
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+		.mmal = MMAL_ENCODING_JPEG,
+		.depth = 8,
+		.mmal_component = COMP_IMAGE_ENCODE,
+		.ybbp = 0,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_H264,
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+		.mmal = MMAL_ENCODING_H264,
+		.depth = 8,
+		.mmal_component = COMP_VIDEO_ENCODE,
+		.ybbp = 0,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_MJPEG,
+		.flags = V4L2_FMT_FLAG_COMPRESSED,
+		.mmal = MMAL_ENCODING_MJPEG,
+		.depth = 8,
+		.mmal_component = COMP_VIDEO_ENCODE,
+		.ybbp = 0,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YVYU,
+		.mmal = MMAL_ENCODING_YVYU,
+		.depth = 16,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 2,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_VYUY,
+		.mmal = MMAL_ENCODING_VYUY,
+		.depth = 16,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 2,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_UYVY,
+		.mmal = MMAL_ENCODING_UYVY,
+		.depth = 16,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 2,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV12,
+		.mmal = MMAL_ENCODING_NV12,
+		.depth = 12,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 1,
+		.remove_padding = 1,
+	}, {
+		.fourcc = V4L2_PIX_FMT_BGR24,
+		.mmal = MMAL_ENCODING_BGR24,
+		.depth = 24,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 3,
+		.remove_padding = 0,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YVU420,
+		.mmal = MMAL_ENCODING_YV12,
+		.depth = 12,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 1,
+		.remove_padding = 1,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV21,
+		.mmal = MMAL_ENCODING_NV21,
+		.depth = 12,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 1,
+		.remove_padding = 1,
+	}, {
+		.fourcc = V4L2_PIX_FMT_BGR32,
+		.mmal = MMAL_ENCODING_BGRA,
+		.depth = 32,
+		.mmal_component = COMP_CAMERA,
+		.ybbp = 4,
+		.remove_padding = 0,
+	},
+};
+
+static struct mmal_fmt *get_format(struct v4l2_format *f)
+{
+	struct mmal_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		fmt = &formats[k];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+/* ------------------------------------------------------------------
+ *	Videobuf queue operations
+ * ------------------------------------------------------------------
+ */
+
+static int queue_setup(struct vb2_queue *vq,
+		       unsigned int *nbuffers, unsigned int *nplanes,
+		       unsigned int sizes[], struct device *alloc_ctxs[])
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
+	unsigned long size;
+
+	/* refuse queue setup if port is not configured */
+	if (!dev->capture.port) {
+		v4l2_err(&dev->v4l2_dev,
+			 "%s: capture port not configured\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Handle CREATE_BUFS situation - *nplanes != 0 */
+	if (*nplanes) {
+		if (*nplanes != 1 ||
+		    sizes[0] < dev->capture.port->current_buffer.size) {
+			v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+				 "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n",
+				 __func__, dev, sizes[0],
+				 dev->capture.port->current_buffer.size,
+				 *nplanes);
+			return -EINVAL;
+		} else {
+			return 0;
+		}
+	}
+
+	/* Handle REQBUFS situation */
+	size = dev->capture.port->current_buffer.size;
+	if (size == 0) {
+		v4l2_err(&dev->v4l2_dev,
+			 "%s: capture port buffer size is zero\n", __func__);
+		return -EINVAL;
+	}
+
+	if (*nbuffers < dev->capture.port->minimum_buffer.num)
+		*nbuffers = dev->capture.port->minimum_buffer.num;
+
+	dev->capture.port->current_buffer.num = *nbuffers;
+
+	*nplanes = 1;
+
+	sizes[0] = size;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
+		 __func__, dev);
+
+	return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb)
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
+	struct vb2_mmal_buffer *buf =
+				container_of(vb2, struct vb2_mmal_buffer, vb);
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
+		 __func__, dev, vb);
+	buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+	buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
+
+	return mmal_vchi_buffer_init(dev->instance, &buf->mmal);
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
+		 __func__, dev, vb);
+
+	if (!dev->capture.port || !dev->capture.fmt)
+		return -ENODEV;
+
+	size = dev->capture.stride * dev->capture.height;
+	if (vb2_plane_size(vb, 0) < size) {
+		v4l2_err(&dev->v4l2_dev,
+			 "%s data will not fit into plane (%lu < %lu)\n",
+			 __func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void buffer_cleanup(struct vb2_buffer *vb)
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
+	struct vb2_mmal_buffer *buf =
+				container_of(vb2, struct vb2_mmal_buffer, vb);
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
+		 __func__, dev, vb);
+
+	mmal_vchi_buffer_cleanup(&buf->mmal);
+}
+
+static inline bool is_capturing(struct bcm2835_mmal_dev *dev)
+{
+	return dev->capture.camera_port ==
+	    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
+}
+
+static void buffer_cb(struct vchiq_mmal_instance *instance,
+		      struct vchiq_mmal_port *port,
+		      int status,
+		      struct mmal_buffer *mmal_buf)
+{
+	struct bcm2835_mmal_dev *dev = port->cb_ctx;
+	struct vb2_mmal_buffer *buf =
+			container_of(mmal_buf, struct vb2_mmal_buffer, mmal);
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
+		 __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags,
+		 mmal_buf->pts);
+
+	if (status) {
+		/* error in transfer */
+		if (buf) {
+			/* there was a buffer with the error so return it */
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		}
+		return;
+	}
+
+	if (mmal_buf->length == 0) {
+		/* stream ended */
+		if (dev->capture.frame_count) {
+			/* empty buffer whilst capturing - expected to be an
+			 * EOS, so grab another frame
+			 */
+			if (is_capturing(dev)) {
+				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+					 "Grab another frame");
+				vchiq_mmal_port_parameter_set(
+					instance,
+					dev->capture.camera_port,
+					MMAL_PARAMETER_CAPTURE,
+					&dev->capture.frame_count,
+					sizeof(dev->capture.frame_count));
+			}
+			if (vchiq_mmal_submit_buffer(instance, port,
+						     &buf->mmal))
+				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+					 "Failed to return EOS buffer");
+		} else {
+			/* stopping streaming.
+			 * return buffer, and signal frame completion
+			 */
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+			complete(&dev->capture.frame_cmplt);
+		}
+		return;
+	}
+
+	if (!dev->capture.frame_count) {
+		/* signal frame completion */
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		complete(&dev->capture.frame_cmplt);
+		return;
+	}
+
+	if (dev->capture.vc_start_timestamp != -1 && mmal_buf->pts) {
+		ktime_t timestamp;
+		s64 runtime_us = mmal_buf->pts -
+		    dev->capture.vc_start_timestamp;
+		timestamp = ktime_add_us(dev->capture.kernel_start_ts,
+					 runtime_us);
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Convert start time %llu and %llu with offset %llu to %llu\n",
+			 ktime_to_ns(dev->capture.kernel_start_ts),
+			 dev->capture.vc_start_timestamp, mmal_buf->pts,
+			 ktime_to_ns(timestamp));
+		buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
+	} else {
+		buf->vb.vb2_buf.timestamp = ktime_get_ns();
+	}
+	buf->vb.sequence = dev->capture.sequence++;
+	buf->vb.field = V4L2_FIELD_NONE;
+
+	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length);
+	if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
+		buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME;
+
+	vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+	if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
+	    is_capturing(dev)) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Grab another frame as buffer has EOS");
+		vchiq_mmal_port_parameter_set(
+			instance,
+			dev->capture.camera_port,
+			MMAL_PARAMETER_CAPTURE,
+			&dev->capture.frame_count,
+			sizeof(dev->capture.frame_count));
+	}
+}
+
+static int enable_camera(struct bcm2835_mmal_dev *dev)
+{
+	int ret;
+
+	if (!dev->camera_use_count) {
+		ret = vchiq_mmal_port_parameter_set(
+			dev->instance,
+			&dev->component[COMP_CAMERA]->control,
+			MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
+			sizeof(dev->camera_num));
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed setting camera num, ret %d\n", ret);
+			return -EINVAL;
+		}
+
+		ret = vchiq_mmal_component_enable(dev->instance,
+						  dev->component[COMP_CAMERA]);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed enabling camera, ret %d\n", ret);
+			return -EINVAL;
+		}
+	}
+	dev->camera_use_count++;
+	v4l2_dbg(1, bcm2835_v4l2_debug,
+		 &dev->v4l2_dev, "enabled camera (refcount %d)\n",
+			dev->camera_use_count);
+	return 0;
+}
+
+static int disable_camera(struct bcm2835_mmal_dev *dev)
+{
+	int ret;
+
+	if (!dev->camera_use_count) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Disabled the camera when already disabled\n");
+		return -EINVAL;
+	}
+	dev->camera_use_count--;
+	if (!dev->camera_use_count) {
+		unsigned int i = 0xFFFFFFFF;
+
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Disabling camera\n");
+		ret = vchiq_mmal_component_disable(dev->instance,
+						   dev->component[COMP_CAMERA]);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed disabling camera, ret %d\n", ret);
+			return -EINVAL;
+		}
+		vchiq_mmal_port_parameter_set(
+			dev->instance,
+			&dev->component[COMP_CAMERA]->control,
+			MMAL_PARAMETER_CAMERA_NUM, &i,
+			sizeof(i));
+	}
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Camera refcount now %d\n", dev->camera_use_count);
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
+	struct vb2_mmal_buffer *buf =
+				container_of(vb2, struct vb2_mmal_buffer, vb);
+	int ret;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "%s: dev:%p buf:%p, idx %u\n",
+		 __func__, dev, buf, vb2->vb2_buf.index);
+
+	ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port,
+				       &buf->mmal);
+	if (ret < 0)
+		v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
+			 __func__);
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
+	int ret;
+	u32 parameter_size;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
+		 __func__, dev);
+
+	/* ensure a format has actually been set */
+	if (!dev->capture.port)
+		return -EINVAL;
+
+	if (enable_camera(dev) < 0) {
+		v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
+		return -EINVAL;
+	}
+
+	/*init_completion(&dev->capture.frame_cmplt); */
+
+	/* enable frame capture */
+	dev->capture.frame_count = 1;
+
+	/* reset sequence number */
+	dev->capture.sequence = 0;
+
+	/* if the preview is not already running, wait for a few frames for AGC
+	 * to settle down.
+	 */
+	if (!dev->component[COMP_PREVIEW]->enabled)
+		msleep(300);
+
+	/* enable the connection from camera to encoder (if applicable) */
+	if (dev->capture.camera_port != dev->capture.port &&
+	    dev->capture.camera_port) {
+		ret = vchiq_mmal_port_enable(dev->instance,
+					     dev->capture.camera_port, NULL);
+		if (ret) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed to enable encode tunnel - error %d\n",
+				 ret);
+			return -1;
+		}
+	}
+
+	/* Get VC timestamp at this point in time */
+	parameter_size = sizeof(dev->capture.vc_start_timestamp);
+	if (vchiq_mmal_port_parameter_get(dev->instance,
+					  dev->capture.camera_port,
+					  MMAL_PARAMETER_SYSTEM_TIME,
+					  &dev->capture.vc_start_timestamp,
+					  &parameter_size)) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to get VC start time - update your VC f/w\n");
+
+		/* Flag to indicate just to rely on kernel timestamps */
+		dev->capture.vc_start_timestamp = -1;
+	} else {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Start time %lld size %d\n",
+			 dev->capture.vc_start_timestamp, parameter_size);
+	}
+
+	dev->capture.kernel_start_ts = ktime_get();
+
+	/* enable the camera port */
+	dev->capture.port->cb_ctx = dev;
+	ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port,
+				     buffer_cb);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to enable capture port - error %d. Disabling camera port again\n",
+			 ret);
+
+		vchiq_mmal_port_disable(dev->instance,
+					dev->capture.camera_port);
+		if (disable_camera(dev) < 0) {
+			v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
+			return -EINVAL;
+		}
+		return -1;
+	}
+
+	/* capture the first frame */
+	vchiq_mmal_port_parameter_set(dev->instance,
+				      dev->capture.camera_port,
+				      MMAL_PARAMETER_CAPTURE,
+				      &dev->capture.frame_count,
+				      sizeof(dev->capture.frame_count));
+	return 0;
+}
+
+/* abort streaming and wait for last buffer */
+static void stop_streaming(struct vb2_queue *vq)
+{
+	int ret;
+	unsigned long timeout;
+	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
+	struct vchiq_mmal_port *port = dev->capture.port;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
+		 __func__, dev);
+
+	init_completion(&dev->capture.frame_cmplt);
+	dev->capture.frame_count = 0;
+
+	/* ensure a format has actually been set */
+	if (!port) {
+		v4l2_err(&dev->v4l2_dev,
+			 "no capture port - stream not started?\n");
+		return;
+	}
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
+
+	/* stop capturing frames */
+	vchiq_mmal_port_parameter_set(dev->instance,
+				      dev->capture.camera_port,
+				      MMAL_PARAMETER_CAPTURE,
+				      &dev->capture.frame_count,
+				      sizeof(dev->capture.frame_count));
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "disabling connection\n");
+
+	/* disable the connection from camera to encoder */
+	ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
+	if (!ret && dev->capture.camera_port != port) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "disabling port\n");
+		ret = vchiq_mmal_port_disable(dev->instance, port);
+	} else if (dev->capture.camera_port != port) {
+		v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
+			 ret);
+	}
+
+	/* wait for all buffers to be returned */
+	while (atomic_read(&port->buffers_with_vpu)) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s: Waiting for buffers to be returned - %d outstanding\n",
+			 __func__, atomic_read(&port->buffers_with_vpu));
+		timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt,
+						      HZ);
+		if (timeout == 0) {
+			v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
+				 __func__,
+				 atomic_read(&port->buffers_with_vpu));
+			break;
+		}
+	}
+
+	if (disable_camera(dev) < 0)
+		v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
+}
+
+static const struct vb2_ops bcm2835_mmal_video_qops = {
+	.queue_setup = queue_setup,
+	.buf_init = buffer_init,
+	.buf_prepare = buffer_prepare,
+	.buf_cleanup = buffer_cleanup,
+	.buf_queue = buffer_queue,
+	.start_streaming = start_streaming,
+	.stop_streaming = stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+/* ------------------------------------------------------------------
+ *	IOCTL operations
+ * ------------------------------------------------------------------
+ */
+
+static int set_overlay_params(struct bcm2835_mmal_dev *dev,
+			      struct vchiq_mmal_port *port)
+{
+	struct mmal_parameter_displayregion prev_config = {
+		.set =	MMAL_DISPLAY_SET_LAYER |
+			MMAL_DISPLAY_SET_ALPHA |
+			MMAL_DISPLAY_SET_DEST_RECT |
+			MMAL_DISPLAY_SET_FULLSCREEN,
+		.layer = 2,
+		.alpha = dev->overlay.global_alpha,
+		.fullscreen = 0,
+		.dest_rect = {
+			.x = dev->overlay.w.left,
+			.y = dev->overlay.w.top,
+			.width = dev->overlay.w.width,
+			.height = dev->overlay.w.height,
+		},
+	};
+	return vchiq_mmal_port_parameter_set(dev->instance, port,
+					     MMAL_PARAMETER_DISPLAYREGION,
+					     &prev_config, sizeof(prev_config));
+}
+
+/* overlay ioctl */
+static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
+				       struct v4l2_fmtdesc *f)
+{
+	struct mmal_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	fmt = &formats[f->index];
+
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+
+	f->fmt.win = dev->overlay;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+
+	f->fmt.win.field = V4L2_FIELD_NONE;
+	f->fmt.win.chromakey = 0;
+	f->fmt.win.clips = NULL;
+	f->fmt.win.clipcount = 0;
+	f->fmt.win.bitmap = NULL;
+
+	v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1,
+			      &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height,
+			      1, 0);
+	v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1,
+			      &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height,
+			      1, 0);
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Overlay: Now w/h %dx%d l/t %dx%d\n",
+		f->fmt.win.w.width, f->fmt.win.w.height,
+		f->fmt.win.w.left, f->fmt.win.w.top);
+
+	v4l2_dump_win_format(1,
+			     bcm2835_v4l2_debug,
+			     &dev->v4l2_dev,
+			     &f->fmt.win,
+			     __func__);
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+
+	vidioc_try_fmt_vid_overlay(file, priv, f);
+
+	dev->overlay = f->fmt.win;
+	if (dev->component[COMP_PREVIEW]->enabled) {
+		set_overlay_params(dev,
+				   &dev->component[COMP_PREVIEW]->input[0]);
+	}
+
+	return 0;
+}
+
+static int vidioc_overlay(struct file *file, void *f, unsigned int on)
+{
+	int ret;
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	struct vchiq_mmal_port *src;
+	struct vchiq_mmal_port *dst;
+
+	if ((on && dev->component[COMP_PREVIEW]->enabled) ||
+	    (!on && !dev->component[COMP_PREVIEW]->enabled))
+		return 0;	/* already in requested state */
+
+	src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
+
+	if (!on) {
+		/* disconnect preview ports and disable component */
+		ret = vchiq_mmal_port_disable(dev->instance, src);
+		if (!ret)
+			ret = vchiq_mmal_port_connect_tunnel(dev->instance, src,
+							     NULL);
+		if (ret >= 0)
+			ret = vchiq_mmal_component_disable(
+					dev->instance,
+					dev->component[COMP_PREVIEW]);
+
+		disable_camera(dev);
+		return ret;
+	}
+
+	/* set preview port format and connect it to output */
+	dst = &dev->component[COMP_PREVIEW]->input[0];
+
+	ret = vchiq_mmal_port_set_format(dev->instance, src);
+	if (ret < 0)
+		return ret;
+
+	ret = set_overlay_params(dev, dst);
+	if (ret < 0)
+		return ret;
+
+	if (enable_camera(dev) < 0)
+		return -EINVAL;
+
+	ret = vchiq_mmal_component_enable(dev->instance,
+					  dev->component[COMP_PREVIEW]);
+	if (ret < 0)
+		return ret;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n",
+		 src, dst);
+	ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst);
+	if (ret)
+		return ret;
+
+	return vchiq_mmal_port_enable(dev->instance, src, NULL);
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh,
+			 struct v4l2_framebuffer *a)
+{
+	/* The video overlay must stay within the framebuffer and can't be
+	 * positioned independently.
+	 */
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	struct vchiq_mmal_port *preview_port =
+		&dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
+
+	a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+			V4L2_FBUF_CAP_GLOBAL_ALPHA;
+	a->flags = V4L2_FBUF_FLAG_OVERLAY;
+	a->fmt.width = preview_port->es.video.width;
+	a->fmt.height = preview_port->es.video.height;
+	a->fmt.pixelformat = V4L2_PIX_FMT_YUV420;
+	a->fmt.bytesperline = preview_port->es.video.width;
+	a->fmt.sizeimage = (preview_port->es.video.width *
+			       preview_port->es.video.height * 3) >> 1;
+	a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+/* input ioctls */
+static int vidioc_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *inp)
+{
+	/* only a single camera input */
+	if (inp->index)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	sprintf((char *)inp->name, "Camera %u", inp->index);
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i)
+		return -EINVAL;
+
+	return 0;
+}
+
+/* capture ioctls */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	u32 major;
+	u32 minor;
+
+	vchiq_mmal_version(dev->instance, &major, &minor);
+
+	strscpy(cap->driver, "bcm2835 mmal", sizeof(cap->driver));
+	snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d", major, minor);
+
+	snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name);
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	struct mmal_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	fmt = &formats[f->index];
+
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+
+	f->fmt.pix.width = dev->capture.width;
+	f->fmt.pix.height = dev->capture.height;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat = dev->capture.fmt->fourcc;
+	f->fmt.pix.bytesperline = dev->capture.stride;
+	f->fmt.pix.sizeimage = dev->capture.buffersize;
+
+	if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+	else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	else
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+
+	v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
+			     __func__);
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	struct mmal_fmt *mfmt;
+
+	mfmt = get_format(f);
+	if (!mfmt) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Fourcc format (0x%08x) unknown.\n",
+			 f->fmt.pix.pixelformat);
+		f->fmt.pix.pixelformat = formats[0].fourcc;
+		mfmt = get_format(f);
+	}
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Clipping/aligning %dx%d format %08X\n",
+		 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
+
+	v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1,
+			      &f->fmt.pix.height, MIN_HEIGHT, dev->max_height,
+			      1, 0);
+	f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp;
+	if (!mfmt->remove_padding) {
+		if (mfmt->depth == 24) {
+			/*
+			 * 24bpp is a pain as we can't use simple masking.
+			 * Min stride is width aligned to 16, times 24bpp.
+			 */
+			f->fmt.pix.bytesperline =
+				((f->fmt.pix.width + 15) & ~15) * 3;
+		} else {
+			/*
+			 * GPU isn't removing padding, so stride is aligned to
+			 * 32
+			 */
+			int align_mask = ((32 * mfmt->depth) >> 3) - 1;
+
+			f->fmt.pix.bytesperline =
+				(f->fmt.pix.bytesperline + align_mask) &
+							~align_mask;
+		}
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Not removing padding, so bytes/line = %d\n",
+			 f->fmt.pix.bytesperline);
+	}
+
+	/* Image buffer has to be padded to allow for alignment, even though
+	 * we sometimes then remove that padding before delivering the buffer.
+	 */
+	f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) *
+			(((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3;
+
+	if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) &&
+	    f->fmt.pix.sizeimage < MIN_BUFFER_SIZE)
+		f->fmt.pix.sizeimage = MIN_BUFFER_SIZE;
+
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+	else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	else
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Now %dx%d format %08X\n",
+		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
+
+	v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
+			     __func__);
+	return 0;
+}
+
+
+static int mmal_setup_video_component(struct bcm2835_mmal_dev *dev,
+				      struct v4l2_format *f)
+{
+	bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled;
+	struct vchiq_mmal_port *preview_port;
+	int ret;
+
+	preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
+
+	/* Preview and encode ports need to match on resolution */
+	if (overlay_enabled) {
+		/* Need to disable the overlay before we can update
+		 * the resolution
+		 */
+		ret = vchiq_mmal_port_disable(dev->instance, preview_port);
+		if (!ret) {
+			ret = vchiq_mmal_port_connect_tunnel(dev->instance,
+							     preview_port,
+							     NULL);
+		}
+	}
+	preview_port->es.video.width = f->fmt.pix.width;
+	preview_port->es.video.height = f->fmt.pix.height;
+	preview_port->es.video.crop.x = 0;
+	preview_port->es.video.crop.y = 0;
+	preview_port->es.video.crop.width = f->fmt.pix.width;
+	preview_port->es.video.crop.height = f->fmt.pix.height;
+	preview_port->es.video.frame_rate.numerator =
+				  dev->capture.timeperframe.denominator;
+	preview_port->es.video.frame_rate.denominator =
+				  dev->capture.timeperframe.numerator;
+	ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
+
+	if (overlay_enabled) {
+		ret = vchiq_mmal_port_connect_tunnel(dev->instance,
+				preview_port,
+				&dev->component[COMP_PREVIEW]->input[0]);
+		if (ret)
+			return ret;
+
+		ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL);
+	}
+
+	return ret;
+}
+
+static int mmal_setup_encode_component(struct bcm2835_mmal_dev *dev,
+				       struct v4l2_format *f,
+				       struct vchiq_mmal_port *port,
+				       struct vchiq_mmal_port *camera_port,
+				       struct vchiq_mmal_component *component)
+{
+	struct mmal_fmt *mfmt = get_format(f);
+	int ret;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "vid_cap - set up encode comp\n");
+
+	/* configure buffering */
+	camera_port->current_buffer.size = camera_port->recommended_buffer.size;
+	camera_port->current_buffer.num = camera_port->recommended_buffer.num;
+
+	ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port,
+					     &component->input[0]);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s failed to create connection\n", __func__);
+		/* ensure capture is not going to be tried */
+		dev->capture.port = NULL;
+		return ret;
+	}
+
+	port->es.video.width = f->fmt.pix.width;
+	port->es.video.height = f->fmt.pix.height;
+	port->es.video.crop.x = 0;
+	port->es.video.crop.y = 0;
+	port->es.video.crop.width = f->fmt.pix.width;
+	port->es.video.crop.height = f->fmt.pix.height;
+	port->es.video.frame_rate.numerator =
+		  dev->capture.timeperframe.denominator;
+	port->es.video.frame_rate.denominator =
+		  dev->capture.timeperframe.numerator;
+
+	port->format.encoding = mfmt->mmal;
+	port->format.encoding_variant = 0;
+	/* Set any encoding specific parameters */
+	switch (mfmt->mmal_component) {
+	case COMP_VIDEO_ENCODE:
+		port->format.bitrate = dev->capture.encode_bitrate;
+		break;
+	case COMP_IMAGE_ENCODE:
+		/* Could set EXIF parameters here */
+		break;
+	default:
+		break;
+	}
+
+	ret = vchiq_mmal_port_set_format(dev->instance, port);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s failed to set format %dx%d fmt %08X\n",
+			 __func__,
+			 f->fmt.pix.width,
+			 f->fmt.pix.height,
+			 f->fmt.pix.pixelformat);
+		return ret;
+	}
+
+	ret = vchiq_mmal_component_enable(dev->instance, component);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s Failed to enable encode components\n", __func__);
+		return ret;
+	}
+
+	/* configure buffering */
+	port->current_buffer.num = 1;
+	port->current_buffer.size = f->fmt.pix.sizeimage;
+	if (port->format.encoding == MMAL_ENCODING_JPEG) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "JPG - buf size now %d was %d\n",
+			 f->fmt.pix.sizeimage,
+			 port->current_buffer.size);
+		port->current_buffer.size =
+		    (f->fmt.pix.sizeimage < (100 << 10)) ?
+		    (100 << 10) : f->fmt.pix.sizeimage;
+	}
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage);
+	port->current_buffer.alignment = 0;
+
+	return 0;
+}
+
+static int mmal_setup_components(struct bcm2835_mmal_dev *dev,
+				 struct v4l2_format *f)
+{
+	int ret;
+	struct vchiq_mmal_port *port = NULL, *camera_port = NULL;
+	struct vchiq_mmal_component *encode_component = NULL;
+	struct mmal_fmt *mfmt = get_format(f);
+	u32 remove_padding;
+
+	if (!mfmt)
+		return -EINVAL;
+
+	if (dev->capture.encode_component) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "vid_cap - disconnect previous tunnel\n");
+
+		/* Disconnect any previous connection */
+		vchiq_mmal_port_connect_tunnel(dev->instance,
+					       dev->capture.camera_port, NULL);
+		dev->capture.camera_port = NULL;
+		ret = vchiq_mmal_component_disable(dev->instance,
+						   dev->capture.encode_component);
+		if (ret)
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed to disable encode component %d\n",
+				 ret);
+
+		dev->capture.encode_component = NULL;
+	}
+	/* format dependent port setup */
+	switch (mfmt->mmal_component) {
+	case COMP_CAMERA:
+		/* Make a further decision on port based on resolution */
+		if (f->fmt.pix.width <= max_video_width &&
+		    f->fmt.pix.height <= max_video_height)
+			camera_port =
+			    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
+		else
+			camera_port =
+			    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
+		port = camera_port;
+		break;
+	case COMP_IMAGE_ENCODE:
+		encode_component = dev->component[COMP_IMAGE_ENCODE];
+		port = &dev->component[COMP_IMAGE_ENCODE]->output[0];
+		camera_port =
+		    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
+		break;
+	case COMP_VIDEO_ENCODE:
+		encode_component = dev->component[COMP_VIDEO_ENCODE];
+		port = &dev->component[COMP_VIDEO_ENCODE]->output[0];
+		camera_port =
+		    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
+		break;
+	default:
+		break;
+	}
+
+	if (!port)
+		return -EINVAL;
+
+	if (encode_component)
+		camera_port->format.encoding = MMAL_ENCODING_OPAQUE;
+	else
+		camera_port->format.encoding = mfmt->mmal;
+
+	if (dev->rgb_bgr_swapped) {
+		if (camera_port->format.encoding == MMAL_ENCODING_RGB24)
+			camera_port->format.encoding = MMAL_ENCODING_BGR24;
+		else if (camera_port->format.encoding == MMAL_ENCODING_BGR24)
+			camera_port->format.encoding = MMAL_ENCODING_RGB24;
+	}
+
+	remove_padding = mfmt->remove_padding;
+	vchiq_mmal_port_parameter_set(dev->instance, camera_port,
+				      MMAL_PARAMETER_NO_IMAGE_PADDING,
+				      &remove_padding, sizeof(remove_padding));
+
+	camera_port->format.encoding_variant = 0;
+	camera_port->es.video.width = f->fmt.pix.width;
+	camera_port->es.video.height = f->fmt.pix.height;
+	camera_port->es.video.crop.x = 0;
+	camera_port->es.video.crop.y = 0;
+	camera_port->es.video.crop.width = f->fmt.pix.width;
+	camera_port->es.video.crop.height = f->fmt.pix.height;
+	camera_port->es.video.frame_rate.numerator = 0;
+	camera_port->es.video.frame_rate.denominator = 1;
+	camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF;
+
+	ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
+
+	if (!ret &&
+	    camera_port ==
+	    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
+		ret = mmal_setup_video_component(dev, f);
+	}
+
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s failed to set format %dx%d %08X\n", __func__,
+			 f->fmt.pix.width, f->fmt.pix.height,
+			 f->fmt.pix.pixelformat);
+		/* ensure capture is not going to be tried */
+		dev->capture.port = NULL;
+		return ret;
+	}
+
+	if (encode_component) {
+		ret = mmal_setup_encode_component(dev, f, port,
+						  camera_port,
+						  encode_component);
+
+		if (ret)
+			return ret;
+	} else {
+		/* configure buffering */
+		camera_port->current_buffer.num = 1;
+		camera_port->current_buffer.size = f->fmt.pix.sizeimage;
+		camera_port->current_buffer.alignment = 0;
+	}
+
+	dev->capture.fmt = mfmt;
+	dev->capture.stride = f->fmt.pix.bytesperline;
+	dev->capture.width = camera_port->es.video.crop.width;
+	dev->capture.height = camera_port->es.video.crop.height;
+	dev->capture.buffersize = port->current_buffer.size;
+
+	/* select port for capture */
+	dev->capture.port = port;
+	dev->capture.camera_port = camera_port;
+	dev->capture.encode_component = encode_component;
+	v4l2_dbg(1, bcm2835_v4l2_debug,
+		 &dev->v4l2_dev,
+		"Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
+		port->format.encoding,
+		dev->capture.width, dev->capture.height,
+		dev->capture.stride, dev->capture.buffersize);
+
+	/* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
+	return ret;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	struct mmal_fmt *mfmt;
+
+	/* try the format to set valid parameters */
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "vid_cap - vidioc_try_fmt_vid_cap failed\n");
+		return ret;
+	}
+
+	/* if a capture is running refuse to set format */
+	if (vb2_is_busy(&dev->capture.vb_vidq)) {
+		v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	/* If the format is unsupported v4l2 says we should switch to
+	 * a supported one and not return an error.
+	 */
+	mfmt = get_format(f);
+	if (!mfmt) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Fourcc format (0x%08x) unknown.\n",
+			 f->fmt.pix.pixelformat);
+		f->fmt.pix.pixelformat = formats[0].fourcc;
+		mfmt = get_format(f);
+	}
+
+	ret = mmal_setup_components(dev, f);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "%s: failed to setup mmal components: %d\n",
+			 __func__, ret);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int vidioc_enum_framesizes(struct file *file, void *fh,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	static const struct v4l2_frmsize_stepwise sizes = {
+		MIN_WIDTH, 0, 2,
+		MIN_HEIGHT, 0, 2
+	};
+	int i;
+
+	if (fsize->index)
+		return -EINVAL;
+	for (i = 0; i < ARRAY_SIZE(formats); i++)
+		if (formats[i].fourcc == fsize->pixel_format)
+			break;
+	if (i == ARRAY_SIZE(formats))
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise = sizes;
+	fsize->stepwise.max_width = dev->max_width;
+	fsize->stepwise.max_height = dev->max_height;
+	return 0;
+}
+
+/* timeperframe is arbitrary and continuous */
+static int vidioc_enum_frameintervals(struct file *file, void *priv,
+				      struct v4l2_frmivalenum *fival)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	int i;
+
+	if (fival->index)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++)
+		if (formats[i].fourcc == fival->pixel_format)
+			break;
+	if (i == ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	/* regarding width & height - we support any within range */
+	if (fival->width < MIN_WIDTH || fival->width > dev->max_width ||
+	    fival->height < MIN_HEIGHT || fival->height > dev->max_height)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
+
+	/* fill in stepwise (step=1.0 is required by V4L2 spec) */
+	fival->stepwise.min  = tpf_min;
+	fival->stepwise.max  = tpf_max;
+	fival->stepwise.step = (struct v4l2_fract) {1, 1};
+
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = dev->capture.timeperframe;
+	parm->parm.capture.readbuffers  = 1;
+	return 0;
+}
+
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct bcm2835_mmal_dev *dev = video_drvdata(file);
+	struct v4l2_fract tpf;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	tpf = parm->parm.capture.timeperframe;
+
+	/* tpf: {*, 0} resets timing; clip to [min, max]*/
+	tpf = tpf.denominator ? tpf : tpf_default;
+	tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf;
+	tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf;
+
+	dev->capture.timeperframe = tpf;
+	parm->parm.capture.timeperframe = tpf;
+	parm->parm.capture.readbuffers  = 1;
+	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
+
+	set_framerate_params(dev);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
+	/* overlay */
+	.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_overlay = vidioc_overlay,
+	.vidioc_g_fbuf = vidioc_g_fbuf,
+
+	/* inputs */
+	.vidioc_enum_input = vidioc_enum_input,
+	.vidioc_g_input = vidioc_g_input,
+	.vidioc_s_input = vidioc_s_input,
+
+	/* capture */
+	.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,
+
+	/* buffer management */
+	.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_enum_framesizes = vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+	.vidioc_g_parm        = vidioc_g_parm,
+	.vidioc_s_parm        = vidioc_s_parm,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+/* ------------------------------------------------------------------
+ *	Driver init/finalise
+ * ------------------------------------------------------------------
+ */
+
+static const struct v4l2_file_operations camera0_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.read = vb2_fop_read,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,	/* V4L2 ioctl handler */
+	.mmap = vb2_fop_mmap,
+};
+
+static const struct video_device vdev_template = {
+	.name = "camera0",
+	.fops = &camera0_fops,
+	.ioctl_ops = &camera0_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
+		       V4L2_CAP_STREAMING | V4L2_CAP_READWRITE,
+};
+
+/* Returns the number of cameras, and also the max resolution supported
+ * by those cameras.
+ */
+static int get_num_cameras(struct vchiq_mmal_instance *instance,
+			   unsigned int resolutions[][2], int num_resolutions)
+{
+	int ret;
+	struct vchiq_mmal_component  *cam_info_component;
+	struct mmal_parameter_camera_info cam_info = {0};
+	u32 param_size = sizeof(cam_info);
+	int i;
+
+	/* create a camera_info component */
+	ret = vchiq_mmal_component_init(instance, "camera_info",
+					&cam_info_component);
+	if (ret < 0)
+		/* Unusual failure - let's guess one camera. */
+		return 1;
+
+	if (vchiq_mmal_port_parameter_get(instance,
+					  &cam_info_component->control,
+					  MMAL_PARAMETER_CAMERA_INFO,
+					  &cam_info,
+					  &param_size)) {
+		pr_info("Failed to get camera info\n");
+	}
+	for (i = 0;
+	     i < min_t(unsigned int, cam_info.num_cameras, num_resolutions);
+	     i++) {
+		resolutions[i][0] = cam_info.cameras[i].max_width;
+		resolutions[i][1] = cam_info.cameras[i].max_height;
+	}
+
+	vchiq_mmal_component_finalise(instance,
+				      cam_info_component);
+
+	return cam_info.num_cameras;
+}
+
+static int set_camera_parameters(struct vchiq_mmal_instance *instance,
+				 struct vchiq_mmal_component *camera,
+				 struct bcm2835_mmal_dev *dev)
+{
+	struct mmal_parameter_camera_config cam_config = {
+		.max_stills_w = dev->max_width,
+		.max_stills_h = dev->max_height,
+		.stills_yuv422 = 1,
+		.one_shot_stills = 1,
+		.max_preview_video_w = (max_video_width > 1920) ?
+						max_video_width : 1920,
+		.max_preview_video_h = (max_video_height > 1088) ?
+						max_video_height : 1088,
+		.num_preview_video_frames = 3,
+		.stills_capture_circular_buffer_height = 0,
+		.fast_preview_resume = 0,
+		.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
+	};
+
+	return vchiq_mmal_port_parameter_set(instance, &camera->control,
+					    MMAL_PARAMETER_CAMERA_CONFIG,
+					    &cam_config, sizeof(cam_config));
+}
+
+#define MAX_SUPPORTED_ENCODINGS 20
+
+/* MMAL instance and component init */
+static int mmal_init(struct bcm2835_mmal_dev *dev)
+{
+	int ret;
+	struct mmal_es_format_local *format;
+	u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
+	u32 param_size;
+	struct vchiq_mmal_component  *camera;
+
+	ret = vchiq_mmal_init(&dev->instance);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n",
+			 __func__, ret);
+		return ret;
+	}
+
+	/* get the camera component ready */
+	ret = vchiq_mmal_component_init(dev->instance, "ril.camera",
+					&dev->component[COMP_CAMERA]);
+	if (ret < 0)
+		goto unreg_mmal;
+
+	camera = dev->component[COMP_CAMERA];
+	if (camera->outputs < CAM_PORT_COUNT) {
+		v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n",
+			 __func__, camera->outputs, CAM_PORT_COUNT);
+		ret = -EINVAL;
+		goto unreg_camera;
+	}
+
+	ret = set_camera_parameters(dev->instance,
+				    camera,
+				    dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n",
+			 __func__, ret);
+		goto unreg_camera;
+	}
+
+	/* There was an error in the firmware that meant the camera component
+	 * produced BGR instead of RGB.
+	 * This is now fixed, but in order to support the old firmwares, we
+	 * have to check.
+	 */
+	dev->rgb_bgr_swapped = true;
+	param_size = sizeof(supported_encodings);
+	ret = vchiq_mmal_port_parameter_get(dev->instance,
+					    &camera->output[CAM_PORT_CAPTURE],
+					    MMAL_PARAMETER_SUPPORTED_ENCODINGS,
+					    &supported_encodings,
+					    &param_size);
+	if (ret == 0) {
+		int i;
+
+		for (i = 0; i < param_size / sizeof(u32); i++) {
+			if (supported_encodings[i] == MMAL_ENCODING_BGR24) {
+				/* Found BGR24 first - old firmware. */
+				break;
+			}
+			if (supported_encodings[i] == MMAL_ENCODING_RGB24) {
+				/* Found RGB24 first
+				 * new firmware, so use RGB24.
+				 */
+				dev->rgb_bgr_swapped = false;
+			break;
+			}
+		}
+	}
+	format = &camera->output[CAM_PORT_PREVIEW].format;
+
+	format->encoding = MMAL_ENCODING_OPAQUE;
+	format->encoding_variant = MMAL_ENCODING_I420;
+
+	format->es->video.width = 1024;
+	format->es->video.height = 768;
+	format->es->video.crop.x = 0;
+	format->es->video.crop.y = 0;
+	format->es->video.crop.width = 1024;
+	format->es->video.crop.height = 768;
+	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
+	format->es->video.frame_rate.denominator = 1;
+
+	format = &camera->output[CAM_PORT_VIDEO].format;
+
+	format->encoding = MMAL_ENCODING_OPAQUE;
+	format->encoding_variant = MMAL_ENCODING_I420;
+
+	format->es->video.width = 1024;
+	format->es->video.height = 768;
+	format->es->video.crop.x = 0;
+	format->es->video.crop.y = 0;
+	format->es->video.crop.width = 1024;
+	format->es->video.crop.height = 768;
+	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
+	format->es->video.frame_rate.denominator = 1;
+
+	format = &camera->output[CAM_PORT_CAPTURE].format;
+
+	format->encoding = MMAL_ENCODING_OPAQUE;
+
+	format->es->video.width = 2592;
+	format->es->video.height = 1944;
+	format->es->video.crop.x = 0;
+	format->es->video.crop.y = 0;
+	format->es->video.crop.width = 2592;
+	format->es->video.crop.height = 1944;
+	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
+	format->es->video.frame_rate.denominator = 1;
+
+	dev->capture.width = format->es->video.width;
+	dev->capture.height = format->es->video.height;
+	dev->capture.fmt = &formats[0];
+	dev->capture.encode_component = NULL;
+	dev->capture.timeperframe = tpf_default;
+	dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
+	dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
+
+	/* get the preview component ready */
+	ret = vchiq_mmal_component_init(dev->instance, "ril.video_render",
+					&dev->component[COMP_PREVIEW]);
+	if (ret < 0)
+		goto unreg_camera;
+
+	if (dev->component[COMP_PREVIEW]->inputs < 1) {
+		ret = -EINVAL;
+		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
+			 __func__, dev->component[COMP_PREVIEW]->inputs, 1);
+		goto unreg_preview;
+	}
+
+	/* get the image encoder component ready */
+	ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode",
+					&dev->component[COMP_IMAGE_ENCODE]);
+	if (ret < 0)
+		goto unreg_preview;
+
+	if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) {
+		ret = -EINVAL;
+		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
+			 __func__, dev->component[COMP_IMAGE_ENCODE]->inputs,
+			 1);
+		goto unreg_image_encoder;
+	}
+
+	/* get the video encoder component ready */
+	ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode",
+					&dev->component[COMP_VIDEO_ENCODE]);
+	if (ret < 0)
+		goto unreg_image_encoder;
+
+	if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) {
+		ret = -EINVAL;
+		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
+			 __func__, dev->component[COMP_VIDEO_ENCODE]->inputs,
+			 1);
+		goto unreg_vid_encoder;
+	}
+
+	{
+		struct vchiq_mmal_port *encoder_port =
+			&dev->component[COMP_VIDEO_ENCODE]->output[0];
+		encoder_port->format.encoding = MMAL_ENCODING_H264;
+		ret = vchiq_mmal_port_set_format(dev->instance,
+						 encoder_port);
+	}
+
+	{
+		unsigned int enable = 1;
+
+		vchiq_mmal_port_parameter_set(
+			dev->instance,
+			&dev->component[COMP_VIDEO_ENCODE]->control,
+			MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
+			&enable, sizeof(enable));
+
+		vchiq_mmal_port_parameter_set(dev->instance,
+					      &dev->component[COMP_VIDEO_ENCODE]->control,
+					      MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
+					      &enable,
+					      sizeof(enable));
+	}
+	ret = bcm2835_mmal_set_all_camera_controls(dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n",
+			 __func__, ret);
+		goto unreg_vid_encoder;
+	}
+
+	return 0;
+
+unreg_vid_encoder:
+	pr_err("Cleanup: Destroy video encoder\n");
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_VIDEO_ENCODE]);
+
+unreg_image_encoder:
+	pr_err("Cleanup: Destroy image encoder\n");
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_IMAGE_ENCODE]);
+
+unreg_preview:
+	pr_err("Cleanup: Destroy video render\n");
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_PREVIEW]);
+
+unreg_camera:
+	pr_err("Cleanup: Destroy camera\n");
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_CAMERA]);
+
+unreg_mmal:
+	vchiq_mmal_finalise(dev->instance);
+	return ret;
+}
+
+static int bcm2835_mmal_init_device(struct bcm2835_mmal_dev *dev, struct video_device *vfd)
+{
+	int ret;
+
+	*vfd = vdev_template;
+
+	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	vfd->lock = &dev->mutex;
+
+	vfd->queue = &dev->capture.vb_vidq;
+
+	/* video device needs to be able to access instance data */
+	video_set_drvdata(vfd, dev);
+
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO,
+				    video_nr[dev->camera_num]);
+	if (ret < 0)
+		return ret;
+
+	v4l2_info(vfd->v4l2_dev,
+		  "V4L2 device registered as %s - stills mode > %dx%d\n",
+		  video_device_node_name(vfd),
+		  max_video_width, max_video_height);
+
+	return 0;
+}
+
+static void bcm2835_cleanup_instance(struct bcm2835_mmal_dev *dev)
+{
+	if (!dev)
+		return;
+
+	v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+		  video_device_node_name(&dev->vdev));
+
+	video_unregister_device(&dev->vdev);
+
+	if (dev->capture.encode_component) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "mmal_exit - disconnect tunnel\n");
+		vchiq_mmal_port_connect_tunnel(dev->instance,
+					       dev->capture.camera_port, NULL);
+		vchiq_mmal_component_disable(dev->instance,
+					     dev->capture.encode_component);
+	}
+	vchiq_mmal_component_disable(dev->instance,
+				     dev->component[COMP_CAMERA]);
+
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_VIDEO_ENCODE]);
+
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_IMAGE_ENCODE]);
+
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_PREVIEW]);
+
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_CAMERA]);
+
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	kfree(dev);
+}
+
+static struct v4l2_format default_v4l2_format = {
+	.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
+	.fmt.pix.width = 1024,
+	.fmt.pix.bytesperline = 0,
+	.fmt.pix.height = 768,
+	.fmt.pix.sizeimage = 1024 * 768,
+};
+
+static int bcm2835_mmal_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct bcm2835_mmal_dev *dev;
+	struct vb2_queue *q;
+	int camera;
+	unsigned int num_cameras;
+	struct vchiq_mmal_instance *instance;
+	unsigned int resolutions[MAX_BCM2835_CAMERAS][2];
+	int i;
+
+	ret = vchiq_mmal_init(&instance);
+	if (ret < 0)
+		return ret;
+
+	num_cameras = get_num_cameras(instance,
+				      resolutions,
+				      MAX_BCM2835_CAMERAS);
+
+	if (num_cameras < 1) {
+		ret = -ENODEV;
+		goto cleanup_mmal;
+	}
+
+	if (num_cameras > MAX_BCM2835_CAMERAS)
+		num_cameras = MAX_BCM2835_CAMERAS;
+
+	for (camera = 0; camera < num_cameras; camera++) {
+		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto cleanup_gdev;
+		}
+
+		/* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
+		mutex_init(&dev->mutex);
+		dev->max_width = resolutions[camera][0];
+		dev->max_height = resolutions[camera][1];
+
+		/* setup device defaults */
+		dev->overlay.w.left = 150;
+		dev->overlay.w.top = 50;
+		dev->overlay.w.width = 1024;
+		dev->overlay.w.height = 768;
+		dev->overlay.clipcount = 0;
+		dev->overlay.field = V4L2_FIELD_NONE;
+		dev->overlay.global_alpha = 255;
+
+		dev->capture.fmt = &formats[3]; /* JPEG */
+
+		/* v4l device registration */
+		dev->camera_num = v4l2_device_set_name(&dev->v4l2_dev, KBUILD_MODNAME,
+						       &camera_instance);
+		ret = v4l2_device_register(NULL, &dev->v4l2_dev);
+		if (ret) {
+			dev_err(&pdev->dev, "%s: could not register V4L2 device: %d\n",
+				__func__, ret);
+			goto free_dev;
+		}
+
+		/* setup v4l controls */
+		ret = bcm2835_mmal_init_controls(dev, &dev->ctrl_handler);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n",
+				 __func__, ret);
+			goto unreg_dev;
+		}
+		dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
+
+		/* mmal init */
+		dev->instance = instance;
+		ret = mmal_init(dev);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n",
+				 __func__, ret);
+			goto unreg_dev;
+		}
+		/* initialize queue */
+		q = &dev->capture.vb_vidq;
+		memset(q, 0, sizeof(*q));
+		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vb2_mmal_buffer);
+		q->ops = &bcm2835_mmal_video_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->lock = &dev->mutex;
+		ret = vb2_queue_init(q);
+		if (ret < 0)
+			goto unreg_dev;
+
+		/* initialise video devices */
+		ret = bcm2835_mmal_init_device(dev, &dev->vdev);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n",
+				 __func__, ret);
+			goto unreg_dev;
+		}
+
+		/* Really want to call vidioc_s_fmt_vid_cap with the default
+		 * format, but currently the APIs don't join up.
+		 */
+		ret = mmal_setup_components(dev, &default_v4l2_format);
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n",
+				 __func__, ret);
+			goto unreg_dev;
+		}
+
+		v4l2_info(&dev->v4l2_dev, "Broadcom 2835 MMAL video capture loaded.\n");
+
+		gdev[camera] = dev;
+	}
+	return 0;
+
+unreg_dev:
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+free_dev:
+	kfree(dev);
+
+cleanup_gdev:
+	for (i = 0; i < camera; i++) {
+		bcm2835_cleanup_instance(gdev[i]);
+		gdev[i] = NULL;
+	}
+
+cleanup_mmal:
+	vchiq_mmal_finalise(instance);
+
+	return ret;
+}
+
+static int bcm2835_mmal_remove(struct platform_device *pdev)
+{
+	int camera;
+	struct vchiq_mmal_instance *instance = gdev[0]->instance;
+
+	for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
+		bcm2835_cleanup_instance(gdev[camera]);
+		gdev[camera] = NULL;
+	}
+	vchiq_mmal_finalise(instance);
+
+	return 0;
+}
+
+static struct platform_driver bcm2835_camera_driver = {
+	.probe		= bcm2835_mmal_probe,
+	.remove		= bcm2835_mmal_remove,
+	.driver		= {
+		.name	= "bcm2835-camera",
+	},
+};
+
+module_platform_driver(bcm2835_camera_driver)
+
+MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
+MODULE_AUTHOR("Vincent Sanders");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:bcm2835-camera");
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
new file mode 100644
index 000000000..0f0c6f7a3
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
@@ -0,0 +1,142 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ *
+ * core driver device
+ */
+
+#define V4L2_CTRL_COUNT 29 /* number of v4l controls */
+
+enum {
+	COMP_CAMERA = 0,
+	COMP_PREVIEW,
+	COMP_IMAGE_ENCODE,
+	COMP_VIDEO_ENCODE,
+	COMP_COUNT
+};
+
+enum {
+	CAM_PORT_PREVIEW = 0,
+	CAM_PORT_VIDEO,
+	CAM_PORT_CAPTURE,
+	CAM_PORT_COUNT
+};
+
+extern int bcm2835_v4l2_debug;
+
+struct bcm2835_mmal_dev {
+	/* v4l2 devices */
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	struct mutex mutex;
+
+	/* controls */
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *ctrls[V4L2_CTRL_COUNT];
+	enum v4l2_scene_mode scene_mode;
+	struct mmal_colourfx colourfx;
+	int hflip;
+	int vflip;
+	int red_gain;
+	int blue_gain;
+	enum mmal_parameter_exposuremode exposure_mode_user;
+	enum v4l2_exposure_auto_type exposure_mode_v4l2_user;
+	/* active exposure mode may differ if selected via a scene mode */
+	enum mmal_parameter_exposuremode exposure_mode_active;
+	enum mmal_parameter_exposuremeteringmode metering_mode;
+	unsigned int manual_shutter_speed;
+	bool exp_auto_priority;
+	bool manual_iso_enabled;
+	u32 iso;
+
+	/* allocated mmal instance and components */
+	struct vchiq_mmal_instance *instance;
+	struct vchiq_mmal_component *component[COMP_COUNT];
+	int camera_use_count;
+
+	struct v4l2_window overlay;
+
+	struct {
+		unsigned int width;  /* width */
+		unsigned int height;  /* height */
+		unsigned int stride;  /* stride */
+		unsigned int buffersize; /* buffer size with padding */
+		struct mmal_fmt *fmt;
+		struct v4l2_fract timeperframe;
+
+		/* H264 encode bitrate */
+		int encode_bitrate;
+		/* H264 bitrate mode. CBR/VBR */
+		int encode_bitrate_mode;
+		/* H264 profile */
+		enum v4l2_mpeg_video_h264_profile enc_profile;
+		/* H264 level */
+		enum v4l2_mpeg_video_h264_level enc_level;
+		/* JPEG Q-factor */
+		int q_factor;
+
+		struct vb2_queue vb_vidq;
+
+		/* VC start timestamp for streaming */
+		s64 vc_start_timestamp;
+		/* Kernel start timestamp for streaming */
+		ktime_t kernel_start_ts;
+		/* Sequence number of last buffer */
+		u32 sequence;
+
+		struct vchiq_mmal_port *port; /* port being used for capture */
+		/* camera port being used for capture */
+		struct vchiq_mmal_port *camera_port;
+		/* component being used for encode */
+		struct vchiq_mmal_component *encode_component;
+		/* number of frames remaining which driver should capture */
+		unsigned int frame_count;
+		/* last frame completion */
+		struct completion frame_cmplt;
+
+	} capture;
+
+	unsigned int camera_num;
+	unsigned int max_width;
+	unsigned int max_height;
+	unsigned int rgb_bgr_swapped;
+};
+
+int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl);
+
+int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev);
+int set_framerate_params(struct bcm2835_mmal_dev *dev);
+
+/* Debug helpers */
+
+#define v4l2_dump_pix_format(level, debug, dev, pix_fmt, desc)	\
+{	\
+	v4l2_dbg(level, debug, dev,	\
+"%s: w %u h %u field %u pfmt 0x%x bpl %u sz_img %u colorspace 0x%x priv %u\n", \
+		desc,	\
+		(pix_fmt)->width, (pix_fmt)->height, (pix_fmt)->field,	\
+		(pix_fmt)->pixelformat, (pix_fmt)->bytesperline,	\
+		(pix_fmt)->sizeimage, (pix_fmt)->colorspace, (pix_fmt)->priv); \
+}
+
+#define v4l2_dump_win_format(level, debug, dev, win_fmt, desc)	\
+{	\
+	v4l2_dbg(level, debug, dev,	\
+"%s: w %u h %u l %u t %u  field %u chromakey %06X clip %p " \
+"clipcount %u bitmap %p\n", \
+		desc,	\
+		(win_fmt)->w.width, (win_fmt)->w.height, \
+		(win_fmt)->w.left, (win_fmt)->w.top, \
+		(win_fmt)->field,	\
+		(win_fmt)->chromakey,	\
+		(win_fmt)->clips, (win_fmt)->clipcount,	\
+		(win_fmt)->bitmap); \
+}
diff --git a/drivers/staging/vc04_services/bcm2835-camera/controls.c b/drivers/staging/vc04_services/bcm2835-camera/controls.c
new file mode 100644
index 000000000..5644d1d45
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-camera/controls.c
@@ -0,0 +1,1401 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.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 "mmal-common.h"
+#include "mmal-vchiq.h"
+#include "mmal-parameters.h"
+#include "bcm2835-camera.h"
+
+/* The supported V4L2_CID_AUTO_EXPOSURE_BIAS values are from -4.0 to +4.0.
+ * MMAL values are in 1/6th increments so the MMAL range is -24 to +24.
+ * V4L2 docs say value "is expressed in terms of EV, drivers should interpret
+ * the values as 0.001 EV units, where the value 1000 stands for +1 EV."
+ * V4L2 is limited to a max of 32 values in a menu, so count in 1/3rds from
+ * -4 to +4
+ */
+static const s64 ev_bias_qmenu[] = {
+	-4000, -3667, -3333,
+	-3000, -2667, -2333,
+	-2000, -1667, -1333,
+	-1000,  -667,  -333,
+	    0,   333,   667,
+	 1000,  1333,  1667,
+	 2000,  2333,  2667,
+	 3000,  3333,  3667,
+	 4000
+};
+
+/* Supported ISO values (*1000)
+ * ISOO = auto ISO
+ */
+static const s64 iso_qmenu[] = {
+	0, 100000, 200000, 400000, 800000,
+};
+
+static const u32 iso_values[] = {
+	0, 100, 200, 400, 800,
+};
+
+enum bcm2835_mmal_ctrl_type {
+	MMAL_CONTROL_TYPE_STD,
+	MMAL_CONTROL_TYPE_STD_MENU,
+	MMAL_CONTROL_TYPE_INT_MENU,
+	MMAL_CONTROL_TYPE_CLUSTER, /* special cluster entry */
+};
+
+struct bcm2835_mmal_v4l2_ctrl {
+	u32 id; /* v4l2 control identifier */
+	enum bcm2835_mmal_ctrl_type type;
+	/* control minimum value or
+	 * mask for MMAL_CONTROL_TYPE_STD_MENU
+	 */
+	s64 min;
+	s64 max; /* maximum value of control */
+	s64 def;  /* default value of control */
+	u64 step; /* step size of the control */
+	const s64 *imenu; /* integer menu array */
+	u32 mmal_id; /* mmal parameter id */
+	int (*setter)(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl *ctrl,
+		      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl);
+};
+
+struct v4l2_to_mmal_effects_setting {
+	u32 v4l2_effect;
+	u32 mmal_effect;
+	s32 col_fx_enable;
+	s32 col_fx_fixed_cbcr;
+	u32 u;
+	u32 v;
+	u32 num_effect_params;
+	u32 effect_params[MMAL_MAX_IMAGEFX_PARAMETERS];
+};
+
+static const struct v4l2_to_mmal_effects_setting
+	v4l2_to_mmal_effects_values[] = {
+	{  V4L2_COLORFX_NONE,         MMAL_PARAM_IMAGEFX_NONE,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_BW,           MMAL_PARAM_IMAGEFX_NONE,
+		1,   0,    128,  128, 0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SEPIA,        MMAL_PARAM_IMAGEFX_NONE,
+		1,   0,    87,   151, 0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_NEGATIVE,     MMAL_PARAM_IMAGEFX_NEGATIVE,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_EMBOSS,       MMAL_PARAM_IMAGEFX_EMBOSS,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SKETCH,       MMAL_PARAM_IMAGEFX_SKETCH,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SKY_BLUE,     MMAL_PARAM_IMAGEFX_PASTEL,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_GRASS_GREEN,  MMAL_PARAM_IMAGEFX_WATERCOLOUR,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SKIN_WHITEN,  MMAL_PARAM_IMAGEFX_WASHEDOUT,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_VIVID,        MMAL_PARAM_IMAGEFX_SATURATION,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_AQUA,         MMAL_PARAM_IMAGEFX_NONE,
+		1,   0,    171,  121, 0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_ART_FREEZE,   MMAL_PARAM_IMAGEFX_HATCH,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SILHOUETTE,   MMAL_PARAM_IMAGEFX_FILM,
+		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
+	{  V4L2_COLORFX_SOLARIZATION, MMAL_PARAM_IMAGEFX_SOLARIZE,
+		0,   0,    0,    0,   5, {1, 128, 160, 160, 48} },
+	{  V4L2_COLORFX_ANTIQUE,      MMAL_PARAM_IMAGEFX_COLOURBALANCE,
+		0,   0,    0,    0,   3, {108, 274, 238, 0, 0} },
+	{  V4L2_COLORFX_SET_CBCR,     MMAL_PARAM_IMAGEFX_NONE,
+		1,   1,    0,    0,   0, {0, 0, 0, 0, 0} }
+};
+
+struct v4l2_mmal_scene_config {
+	enum v4l2_scene_mode v4l2_scene;
+	enum mmal_parameter_exposuremode exposure_mode;
+	enum mmal_parameter_exposuremeteringmode metering_mode;
+};
+
+static const struct v4l2_mmal_scene_config scene_configs[] = {
+	/* V4L2_SCENE_MODE_NONE automatically added */
+	{
+		V4L2_SCENE_MODE_NIGHT,
+		MMAL_PARAM_EXPOSUREMODE_NIGHT,
+		MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE
+	},
+	{
+		V4L2_SCENE_MODE_SPORTS,
+		MMAL_PARAM_EXPOSUREMODE_SPORTS,
+		MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE
+	},
+};
+
+/* control handlers*/
+
+static int ctrl_set_rational(struct bcm2835_mmal_dev *dev,
+			     struct v4l2_ctrl *ctrl,
+			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	struct s32_fract rational_value;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	rational_value.numerator = ctrl->val;
+	rational_value.denominator = 100;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &rational_value,
+					     sizeof(rational_value));
+}
+
+static int ctrl_set_value(struct bcm2835_mmal_dev *dev,
+			  struct v4l2_ctrl *ctrl,
+			  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	u32_value = ctrl->val;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_iso(struct bcm2835_mmal_dev *dev,
+			struct v4l2_ctrl *ctrl,
+			const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *control;
+
+	if (ctrl->val > mmal_ctrl->max || ctrl->val < mmal_ctrl->min)
+		return 1;
+
+	if (ctrl->id == V4L2_CID_ISO_SENSITIVITY)
+		dev->iso = iso_values[ctrl->val];
+	else if (ctrl->id == V4L2_CID_ISO_SENSITIVITY_AUTO)
+		dev->manual_iso_enabled =
+				(ctrl->val == V4L2_ISO_SENSITIVITY_MANUAL);
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	if (dev->manual_iso_enabled)
+		u32_value = dev->iso;
+	else
+		u32_value = 0;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     MMAL_PARAMETER_ISO,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_value_ev(struct bcm2835_mmal_dev *dev,
+			     struct v4l2_ctrl *ctrl,
+			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	s32 s32_value;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	s32_value = (ctrl->val - 12) * 2;	/* Convert from index to 1/6ths */
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &s32_value, sizeof(s32_value));
+}
+
+static int ctrl_set_rotate(struct bcm2835_mmal_dev *dev,
+			   struct v4l2_ctrl *ctrl,
+			   const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret;
+	u32 u32_value;
+	struct vchiq_mmal_component *camera;
+
+	camera = dev->component[COMP_CAMERA];
+
+	u32_value = ((ctrl->val % 360) / 90) * 90;
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+	if (ret < 0)
+		return ret;
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+	if (ret < 0)
+		return ret;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_flip(struct bcm2835_mmal_dev *dev,
+			 struct v4l2_ctrl *ctrl,
+			 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret;
+	u32 u32_value;
+	struct vchiq_mmal_component *camera;
+
+	if (ctrl->id == V4L2_CID_HFLIP)
+		dev->hflip = ctrl->val;
+	else
+		dev->vflip = ctrl->val;
+
+	camera = dev->component[COMP_CAMERA];
+
+	if (dev->hflip && dev->vflip)
+		u32_value = MMAL_PARAM_MIRROR_BOTH;
+	else if (dev->hflip)
+		u32_value = MMAL_PARAM_MIRROR_HORIZONTAL;
+	else if (dev->vflip)
+		u32_value = MMAL_PARAM_MIRROR_VERTICAL;
+	else
+		u32_value = MMAL_PARAM_MIRROR_NONE;
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+	if (ret < 0)
+		return ret;
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+	if (ret < 0)
+		return ret;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2],
+					    mmal_ctrl->mmal_id,
+					    &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_exposure(struct bcm2835_mmal_dev *dev,
+			     struct v4l2_ctrl *ctrl,
+			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	enum mmal_parameter_exposuremode exp_mode = dev->exposure_mode_user;
+	u32 shutter_speed = 0;
+	struct vchiq_mmal_port *control;
+	int ret = 0;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	if (mmal_ctrl->mmal_id == MMAL_PARAMETER_SHUTTER_SPEED)	{
+		/* V4L2 is in 100usec increments.
+		 * MMAL is 1usec.
+		 */
+		dev->manual_shutter_speed = ctrl->val * 100;
+	} else if (mmal_ctrl->mmal_id == MMAL_PARAMETER_EXPOSURE_MODE) {
+		switch (ctrl->val) {
+		case V4L2_EXPOSURE_AUTO:
+			exp_mode = MMAL_PARAM_EXPOSUREMODE_AUTO;
+			break;
+
+		case V4L2_EXPOSURE_MANUAL:
+			exp_mode = MMAL_PARAM_EXPOSUREMODE_OFF;
+			break;
+		}
+		dev->exposure_mode_user = exp_mode;
+		dev->exposure_mode_v4l2_user = ctrl->val;
+	} else if (mmal_ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) {
+		dev->exp_auto_priority = ctrl->val;
+	}
+
+	if (dev->scene_mode == V4L2_SCENE_MODE_NONE) {
+		if (exp_mode == MMAL_PARAM_EXPOSUREMODE_OFF)
+			shutter_speed = dev->manual_shutter_speed;
+
+		ret = vchiq_mmal_port_parameter_set(dev->instance,
+						    control,
+						    MMAL_PARAMETER_SHUTTER_SPEED,
+						    &shutter_speed,
+						    sizeof(shutter_speed));
+		ret += vchiq_mmal_port_parameter_set(dev->instance,
+						     control,
+						     MMAL_PARAMETER_EXPOSURE_MODE,
+						     &exp_mode,
+						     sizeof(u32));
+		dev->exposure_mode_active = exp_mode;
+	}
+	/* exposure_dynamic_framerate (V4L2_CID_EXPOSURE_AUTO_PRIORITY) should
+	 * always apply irrespective of scene mode.
+	 */
+	ret += set_framerate_params(dev);
+
+	return ret;
+}
+
+static int ctrl_set_metering_mode(struct bcm2835_mmal_dev *dev,
+				  struct v4l2_ctrl *ctrl,
+				  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	switch (ctrl->val) {
+	case V4L2_EXPOSURE_METERING_AVERAGE:
+		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
+		break;
+
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT;
+		break;
+
+	case V4L2_EXPOSURE_METERING_SPOT:
+		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT;
+		break;
+
+	case V4L2_EXPOSURE_METERING_MATRIX:
+		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX;
+		break;
+	}
+
+	if (dev->scene_mode == V4L2_SCENE_MODE_NONE) {
+		struct vchiq_mmal_port *control;
+		u32 u32_value = dev->metering_mode;
+
+		control = &dev->component[COMP_CAMERA]->control;
+
+		return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+	} else {
+		return 0;
+	}
+}
+
+static int ctrl_set_flicker_avoidance(struct bcm2835_mmal_dev *dev,
+				      struct v4l2_ctrl *ctrl,
+				      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	switch (ctrl->val) {
+	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+		u32_value = MMAL_PARAM_FLICKERAVOID_OFF;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+		u32_value = MMAL_PARAM_FLICKERAVOID_50HZ;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+		u32_value = MMAL_PARAM_FLICKERAVOID_60HZ;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
+		u32_value = MMAL_PARAM_FLICKERAVOID_AUTO;
+		break;
+	}
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_awb_mode(struct bcm2835_mmal_dev *dev,
+			     struct v4l2_ctrl *ctrl,
+			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	switch (ctrl->val) {
+	case V4L2_WHITE_BALANCE_MANUAL:
+		u32_value = MMAL_PARAM_AWBMODE_OFF;
+		break;
+
+	case V4L2_WHITE_BALANCE_AUTO:
+		u32_value = MMAL_PARAM_AWBMODE_AUTO;
+		break;
+
+	case V4L2_WHITE_BALANCE_INCANDESCENT:
+		u32_value = MMAL_PARAM_AWBMODE_INCANDESCENT;
+		break;
+
+	case V4L2_WHITE_BALANCE_FLUORESCENT:
+		u32_value = MMAL_PARAM_AWBMODE_FLUORESCENT;
+		break;
+
+	case V4L2_WHITE_BALANCE_FLUORESCENT_H:
+		u32_value = MMAL_PARAM_AWBMODE_TUNGSTEN;
+		break;
+
+	case V4L2_WHITE_BALANCE_HORIZON:
+		u32_value = MMAL_PARAM_AWBMODE_HORIZON;
+		break;
+
+	case V4L2_WHITE_BALANCE_DAYLIGHT:
+		u32_value = MMAL_PARAM_AWBMODE_SUNLIGHT;
+		break;
+
+	case V4L2_WHITE_BALANCE_FLASH:
+		u32_value = MMAL_PARAM_AWBMODE_FLASH;
+		break;
+
+	case V4L2_WHITE_BALANCE_CLOUDY:
+		u32_value = MMAL_PARAM_AWBMODE_CLOUDY;
+		break;
+
+	case V4L2_WHITE_BALANCE_SHADE:
+		u32_value = MMAL_PARAM_AWBMODE_SHADE;
+		break;
+	}
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_awb_gains(struct bcm2835_mmal_dev *dev,
+			      struct v4l2_ctrl *ctrl,
+			      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	struct vchiq_mmal_port *control;
+	struct mmal_parameter_awbgains gains;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	if (ctrl->id == V4L2_CID_RED_BALANCE)
+		dev->red_gain = ctrl->val;
+	else if (ctrl->id == V4L2_CID_BLUE_BALANCE)
+		dev->blue_gain = ctrl->val;
+
+	gains.r_gain.numerator = dev->red_gain;
+	gains.r_gain.denominator = 1000;
+	gains.b_gain.numerator = dev->blue_gain;
+	gains.b_gain.denominator = 1000;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, control,
+					     mmal_ctrl->mmal_id,
+					     &gains, sizeof(gains));
+}
+
+static int ctrl_set_image_effect(struct bcm2835_mmal_dev *dev,
+				 struct v4l2_ctrl *ctrl,
+				 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret = -EINVAL;
+	int i, j;
+	struct vchiq_mmal_port *control;
+	struct mmal_parameter_imagefx_parameters imagefx;
+
+	for (i = 0; i < ARRAY_SIZE(v4l2_to_mmal_effects_values); i++) {
+		if (ctrl->val != v4l2_to_mmal_effects_values[i].v4l2_effect)
+			continue;
+
+		imagefx.effect =
+			v4l2_to_mmal_effects_values[i].mmal_effect;
+		imagefx.num_effect_params =
+			v4l2_to_mmal_effects_values[i].num_effect_params;
+
+		if (imagefx.num_effect_params > MMAL_MAX_IMAGEFX_PARAMETERS)
+			imagefx.num_effect_params = MMAL_MAX_IMAGEFX_PARAMETERS;
+
+		for (j = 0; j < imagefx.num_effect_params; j++)
+			imagefx.effect_parameter[j] =
+				v4l2_to_mmal_effects_values[i].effect_params[j];
+
+		dev->colourfx.enable =
+			v4l2_to_mmal_effects_values[i].col_fx_enable;
+		if (!v4l2_to_mmal_effects_values[i].col_fx_fixed_cbcr) {
+			dev->colourfx.u = v4l2_to_mmal_effects_values[i].u;
+			dev->colourfx.v = v4l2_to_mmal_effects_values[i].v;
+		}
+
+		control = &dev->component[COMP_CAMERA]->control;
+
+		ret = vchiq_mmal_port_parameter_set(
+				dev->instance, control,
+				MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
+				&imagefx, sizeof(imagefx));
+		if (ret)
+			goto exit;
+
+		ret = vchiq_mmal_port_parameter_set(
+				dev->instance, control,
+				MMAL_PARAMETER_COLOUR_EFFECT,
+				&dev->colourfx, sizeof(dev->colourfx));
+	}
+
+exit:
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "mmal_ctrl:%p ctrl id:0x%x ctrl val:%d imagefx:0x%x color_effect:%s u:%d v:%d ret %d(%d)\n",
+				mmal_ctrl, ctrl->id, ctrl->val, imagefx.effect,
+				dev->colourfx.enable ? "true" : "false",
+				dev->colourfx.u, dev->colourfx.v,
+				ret, (ret == 0 ? 0 : -EINVAL));
+	return (ret == 0 ? 0 : -EINVAL);
+}
+
+static int ctrl_set_colfx(struct bcm2835_mmal_dev *dev,
+			  struct v4l2_ctrl *ctrl,
+			  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret;
+	struct vchiq_mmal_port *control;
+
+	control = &dev->component[COMP_CAMERA]->control;
+
+	dev->colourfx.u = (ctrl->val & 0xff00) >> 8;
+	dev->colourfx.v = ctrl->val & 0xff;
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, control,
+					    MMAL_PARAMETER_COLOUR_EFFECT,
+					    &dev->colourfx,
+					    sizeof(dev->colourfx));
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
+			__func__, mmal_ctrl, ctrl->id, ctrl->val, ret,
+			(ret == 0 ? 0 : -EINVAL));
+	return (ret == 0 ? 0 : -EINVAL);
+}
+
+static int ctrl_set_bitrate(struct bcm2835_mmal_dev *dev,
+			    struct v4l2_ctrl *ctrl,
+			    const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret;
+	struct vchiq_mmal_port *encoder_out;
+
+	dev->capture.encode_bitrate = ctrl->val;
+
+	encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0];
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance, encoder_out,
+					    mmal_ctrl->mmal_id, &ctrl->val,
+					    sizeof(ctrl->val));
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
+		 __func__, mmal_ctrl, ctrl->id, ctrl->val, ret,
+		 (ret == 0 ? 0 : -EINVAL));
+
+	/*
+	 * Older firmware versions (pre July 2019) have a bug in handling
+	 * MMAL_PARAMETER_VIDEO_BIT_RATE that result in the call
+	 * returning -MMAL_MSG_STATUS_EINVAL. So ignore errors from this call.
+	 */
+	return 0;
+}
+
+static int ctrl_set_bitrate_mode(struct bcm2835_mmal_dev *dev,
+				 struct v4l2_ctrl *ctrl,
+				 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 bitrate_mode;
+	struct vchiq_mmal_port *encoder_out;
+
+	encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0];
+
+	dev->capture.encode_bitrate_mode = ctrl->val;
+	switch (ctrl->val) {
+	default:
+	case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
+		bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE;
+		break;
+	case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
+		bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT;
+		break;
+	}
+
+	vchiq_mmal_port_parameter_set(dev->instance, encoder_out,
+				      mmal_ctrl->mmal_id,
+					     &bitrate_mode,
+					     sizeof(bitrate_mode));
+	return 0;
+}
+
+static int ctrl_set_image_encode_output(struct bcm2835_mmal_dev *dev,
+					struct v4l2_ctrl *ctrl,
+					const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *jpeg_out;
+
+	jpeg_out = &dev->component[COMP_IMAGE_ENCODE]->output[0];
+
+	u32_value = ctrl->val;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, jpeg_out,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_video_encode_param_output(struct bcm2835_mmal_dev *dev,
+					      struct v4l2_ctrl *ctrl,
+					      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	u32 u32_value;
+	struct vchiq_mmal_port *vid_enc_ctl;
+
+	vid_enc_ctl = &dev->component[COMP_VIDEO_ENCODE]->output[0];
+
+	u32_value = ctrl->val;
+
+	return vchiq_mmal_port_parameter_set(dev->instance, vid_enc_ctl,
+					     mmal_ctrl->mmal_id,
+					     &u32_value, sizeof(u32_value));
+}
+
+static int ctrl_set_video_encode_profile_level(struct bcm2835_mmal_dev *dev,
+					       struct v4l2_ctrl *ctrl,
+					       const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	struct mmal_parameter_video_profile param;
+	int ret = 0;
+
+	if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_PROFILE) {
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			dev->capture.enc_profile = ctrl->val;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+	} else if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_LEVEL) {
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			dev->capture.enc_level = ctrl->val;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+	}
+
+	if (!ret) {
+		switch (dev->capture.enc_profile) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+			param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+			param.profile =
+				MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+			param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
+			break;
+		default:
+			/* Should never get here */
+			break;
+		}
+
+		switch (dev->capture.enc_level) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_1;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+			param.level = MMAL_VIDEO_LEVEL_H264_1b;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_11;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_12;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+			param.level = MMAL_VIDEO_LEVEL_H264_13;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_2;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_21;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_22;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_3;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_31;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_32;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_4;
+			break;
+		default:
+			/* Should never get here */
+			break;
+		}
+
+		ret = vchiq_mmal_port_parameter_set(dev->instance,
+						    &dev->component[COMP_VIDEO_ENCODE]->output[0],
+			mmal_ctrl->mmal_id,
+			&param, sizeof(param));
+	}
+	return ret;
+}
+
+static int ctrl_set_scene_mode(struct bcm2835_mmal_dev *dev,
+			       struct v4l2_ctrl *ctrl,
+			       const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	int ret = 0;
+	int shutter_speed;
+	struct vchiq_mmal_port *control;
+
+	v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "scene mode selected %d, was %d\n", ctrl->val,
+		 dev->scene_mode);
+	control = &dev->component[COMP_CAMERA]->control;
+
+	if (ctrl->val == dev->scene_mode)
+		return 0;
+
+	if (ctrl->val == V4L2_SCENE_MODE_NONE) {
+		/* Restore all user selections */
+		dev->scene_mode = V4L2_SCENE_MODE_NONE;
+
+		if (dev->exposure_mode_user == MMAL_PARAM_EXPOSUREMODE_OFF)
+			shutter_speed = dev->manual_shutter_speed;
+		else
+			shutter_speed = 0;
+
+		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n",
+			 __func__, shutter_speed, dev->exposure_mode_user,
+			 dev->metering_mode);
+		ret = vchiq_mmal_port_parameter_set(dev->instance,
+						    control,
+						    MMAL_PARAMETER_SHUTTER_SPEED,
+						    &shutter_speed,
+						    sizeof(shutter_speed));
+		ret += vchiq_mmal_port_parameter_set(dev->instance,
+						     control,
+						     MMAL_PARAMETER_EXPOSURE_MODE,
+						     &dev->exposure_mode_user,
+						     sizeof(u32));
+		dev->exposure_mode_active = dev->exposure_mode_user;
+		ret += vchiq_mmal_port_parameter_set(dev->instance,
+						     control,
+						     MMAL_PARAMETER_EXP_METERING_MODE,
+						     &dev->metering_mode,
+						     sizeof(u32));
+		ret += set_framerate_params(dev);
+	} else {
+		/* Set up scene mode */
+		int i;
+		const struct v4l2_mmal_scene_config *scene = NULL;
+		int shutter_speed;
+		enum mmal_parameter_exposuremode exposure_mode;
+		enum mmal_parameter_exposuremeteringmode metering_mode;
+
+		for (i = 0; i < ARRAY_SIZE(scene_configs); i++) {
+			if (scene_configs[i].v4l2_scene == ctrl->val) {
+				scene = &scene_configs[i];
+				break;
+			}
+		}
+		if (!scene)
+			return -EINVAL;
+		if (i >= ARRAY_SIZE(scene_configs))
+			return -EINVAL;
+
+		/* Set all the values */
+		dev->scene_mode = ctrl->val;
+
+		if (scene->exposure_mode == MMAL_PARAM_EXPOSUREMODE_OFF)
+			shutter_speed = dev->manual_shutter_speed;
+		else
+			shutter_speed = 0;
+		exposure_mode = scene->exposure_mode;
+		metering_mode = scene->metering_mode;
+
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n",
+			 __func__, shutter_speed, exposure_mode, metering_mode);
+
+		ret = vchiq_mmal_port_parameter_set(dev->instance, control,
+						    MMAL_PARAMETER_SHUTTER_SPEED,
+						    &shutter_speed,
+						    sizeof(shutter_speed));
+		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
+						     MMAL_PARAMETER_EXPOSURE_MODE,
+						     &exposure_mode,
+						     sizeof(u32));
+		dev->exposure_mode_active = exposure_mode;
+		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
+						     MMAL_PARAMETER_EXPOSURE_MODE,
+						     &exposure_mode,
+						     sizeof(u32));
+		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
+						     MMAL_PARAMETER_EXP_METERING_MODE,
+						     &metering_mode,
+						     sizeof(u32));
+		ret += set_framerate_params(dev);
+	}
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s: Setting scene to %d, ret=%d\n",
+			 __func__, ctrl->val, ret);
+		ret = -EINVAL;
+	}
+	return 0;
+}
+
+static int bcm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct bcm2835_mmal_dev *dev = container_of(ctrl->handler, struct bcm2835_mmal_dev,
+						    ctrl_handler);
+	const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl = ctrl->priv;
+	int ret;
+
+	if (!mmal_ctrl || mmal_ctrl->id != ctrl->id || !mmal_ctrl->setter) {
+		pr_warn("mmal_ctrl:%p ctrl id:%d\n", mmal_ctrl, ctrl->id);
+		return -EINVAL;
+	}
+
+	ret = mmal_ctrl->setter(dev, ctrl, mmal_ctrl);
+	if (ret)
+		pr_warn("ctrl id:%d/MMAL param %08X- returned ret %d\n",
+			ctrl->id, mmal_ctrl->mmal_id, ret);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops bcm2835_mmal_ctrl_ops = {
+	.s_ctrl = bcm2835_mmal_s_ctrl,
+};
+
+static const struct bcm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
+	{
+		.id = V4L2_CID_SATURATION,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = -100,
+		.max = 100,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_SATURATION,
+		.setter = ctrl_set_rational,
+	},
+	{
+		.id = V4L2_CID_SHARPNESS,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = -100,
+		.max = 100,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_SHARPNESS,
+		.setter = ctrl_set_rational,
+	},
+	{
+		.id = V4L2_CID_CONTRAST,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = -100,
+		.max = 100,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_CONTRAST,
+		.setter = ctrl_set_rational,
+	},
+	{
+		.id = V4L2_CID_BRIGHTNESS,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 100,
+		.def = 50,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_BRIGHTNESS,
+		.setter = ctrl_set_rational,
+	},
+	{
+		.id = V4L2_CID_ISO_SENSITIVITY,
+		.type = MMAL_CONTROL_TYPE_INT_MENU,
+		.min = 0,
+		.max = ARRAY_SIZE(iso_qmenu) - 1,
+		.def = 0,
+		.step = 1,
+		.imenu = iso_qmenu,
+		.mmal_id = MMAL_PARAMETER_ISO,
+		.setter = ctrl_set_iso,
+	},
+	{
+		.id = V4L2_CID_ISO_SENSITIVITY_AUTO,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = 0,
+		.max = V4L2_ISO_SENSITIVITY_AUTO,
+		.def = V4L2_ISO_SENSITIVITY_AUTO,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_ISO,
+		.setter = ctrl_set_iso,
+	},
+	{
+		.id = V4L2_CID_IMAGE_STABILIZATION,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 1,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_VIDEO_STABILISATION,
+		.setter = ctrl_set_value,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_AUTO,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = ~0x03,
+		.max = V4L2_EXPOSURE_APERTURE_PRIORITY,
+		.def = V4L2_EXPOSURE_AUTO,
+		.step = 0,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_EXPOSURE_MODE,
+		.setter = ctrl_set_exposure,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.type = MMAL_CONTROL_TYPE_STD,
+		/* Units of 100usecs */
+		.min = 1,
+		.max = 1 * 1000 * 10,
+		.def = 100 * 10,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_SHUTTER_SPEED,
+		.setter = ctrl_set_exposure,
+	},
+	{
+		.id = V4L2_CID_AUTO_EXPOSURE_BIAS,
+		.type = MMAL_CONTROL_TYPE_INT_MENU,
+		.min = 0,
+		.max = ARRAY_SIZE(ev_bias_qmenu) - 1,
+		.def = (ARRAY_SIZE(ev_bias_qmenu) + 1) / 2 - 1,
+		.step = 0,
+		.imenu = ev_bias_qmenu,
+		.mmal_id = MMAL_PARAMETER_EXPOSURE_COMP,
+		.setter = ctrl_set_value_ev,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_AUTO_PRIORITY,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 1,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		/* Dummy MMAL ID as it gets mapped into FPS range */
+		.mmal_id = 0,
+		.setter = ctrl_set_exposure,
+	},
+	{
+		.id = V4L2_CID_EXPOSURE_METERING,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = ~0xf,
+		.max = V4L2_EXPOSURE_METERING_MATRIX,
+		.def = V4L2_EXPOSURE_METERING_AVERAGE,
+		.step = 0,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_EXP_METERING_MODE,
+		.setter = ctrl_set_metering_mode,
+	},
+	{
+		.id = V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = ~0x3ff,
+		.max = V4L2_WHITE_BALANCE_SHADE,
+		.def = V4L2_WHITE_BALANCE_AUTO,
+		.step = 0,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_AWB_MODE,
+		.setter = ctrl_set_awb_mode,
+	},
+	{
+		.id = V4L2_CID_RED_BALANCE,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 1,
+		.max = 7999,
+		.def = 1000,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS,
+		.setter = ctrl_set_awb_gains,
+	},
+	{
+		.id = V4L2_CID_BLUE_BALANCE,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 1,
+		.max = 7999,
+		.def = 1000,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS,
+		.setter = ctrl_set_awb_gains,
+	},
+	{
+		.id = V4L2_CID_COLORFX,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = 0,
+		.max = V4L2_COLORFX_SET_CBCR,
+		.def = V4L2_COLORFX_NONE,
+		.step = 0,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_IMAGE_EFFECT,
+		.setter = ctrl_set_image_effect,
+	},
+	{
+		.id = V4L2_CID_COLORFX_CBCR,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 0xffff,
+		.def = 0x8080,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_COLOUR_EFFECT,
+		.setter = ctrl_set_colfx,
+	},
+	{
+		.id = V4L2_CID_ROTATE,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 360,
+		.def = 0,
+		.step = 90,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_ROTATION,
+		.setter = ctrl_set_rotate,
+	},
+	{
+		.id = V4L2_CID_HFLIP,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 1,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_MIRROR,
+		.setter = ctrl_set_flip,
+	},
+	{
+		.id = V4L2_CID_VFLIP,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 1,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_MIRROR,
+		.setter = ctrl_set_flip,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = 0,
+		.max = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+		.def = 0,
+		.step = 0,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_RATECONTROL,
+		.setter = ctrl_set_bitrate_mode,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_BITRATE,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 25 * 1000,
+		.max = 25 * 1000 * 1000,
+		.def = 10 * 1000 * 1000,
+		.step = 25 * 1000,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_VIDEO_BIT_RATE,
+		.setter = ctrl_set_bitrate,
+	},
+	{
+		.id = V4L2_CID_JPEG_COMPRESSION_QUALITY,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 1,
+		.max = 100,
+		.def = 30,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_JPEG_Q_FACTOR,
+		.setter = ctrl_set_image_encode_output,
+	},
+	{
+		.id = V4L2_CID_POWER_LINE_FREQUENCY,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = 0,
+		.max = V4L2_CID_POWER_LINE_FREQUENCY_AUTO,
+		.def = 1,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_FLICKER_AVOID,
+		.setter = ctrl_set_flicker_avoidance,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 1,
+		.def = 0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER,
+		.setter = ctrl_set_video_encode_param_output,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
+		.max = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		.def = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_PROFILE,
+		.setter = ctrl_set_video_encode_profile_level,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		.min = ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
+			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0)),
+		.max = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
+		.def = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_PROFILE,
+		.setter = ctrl_set_video_encode_profile_level,
+	},
+	{
+		.id = V4L2_CID_SCENE_MODE,
+		.type = MMAL_CONTROL_TYPE_STD_MENU,
+		/* mask is computed at runtime */
+		.min = -1,
+		.max = V4L2_SCENE_MODE_TEXT,
+		.def = V4L2_SCENE_MODE_NONE,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_PROFILE,
+		.setter = ctrl_set_scene_mode,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
+		.type = MMAL_CONTROL_TYPE_STD,
+		.min = 0,
+		.max = 0x7FFFFFFF,
+		.def = 60,
+		.step = 1,
+		.imenu = NULL,
+		.mmal_id = MMAL_PARAMETER_INTRAPERIOD,
+		.setter = ctrl_set_video_encode_param_output,
+	},
+};
+
+int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev)
+{
+	int c;
+	int ret = 0;
+
+	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
+		if ((dev->ctrls[c]) && (v4l2_ctrls[c].setter)) {
+			ret = v4l2_ctrls[c].setter(dev, dev->ctrls[c],
+						   &v4l2_ctrls[c]);
+			if (ret) {
+				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+					 "Failed when setting default values for ctrl %d\n",
+					 c);
+				break;
+			}
+		}
+	}
+	return ret;
+}
+
+int set_framerate_params(struct bcm2835_mmal_dev *dev)
+{
+	struct mmal_parameter_fps_range fps_range;
+	int ret;
+
+	fps_range.fps_high.numerator = dev->capture.timeperframe.denominator;
+	fps_range.fps_high.denominator = dev->capture.timeperframe.numerator;
+
+	if ((dev->exposure_mode_active != MMAL_PARAM_EXPOSUREMODE_OFF) &&
+	    (dev->exp_auto_priority)) {
+		/* Variable FPS. Define min FPS as 1fps. */
+		fps_range.fps_low.numerator = 1;
+		fps_range.fps_low.denominator = 1;
+	} else {
+		/* Fixed FPS - set min and max to be the same */
+		fps_range.fps_low.numerator = fps_range.fps_high.numerator;
+		fps_range.fps_low.denominator = fps_range.fps_high.denominator;
+	}
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Set fps range to %d/%d to %d/%d\n",
+		 fps_range.fps_low.numerator,
+		 fps_range.fps_low.denominator,
+		 fps_range.fps_high.numerator,
+		 fps_range.fps_high.denominator);
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance,
+					    &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW],
+					    MMAL_PARAMETER_FPS_RANGE,
+					    &fps_range, sizeof(fps_range));
+	ret += vchiq_mmal_port_parameter_set(dev->instance,
+					     &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO],
+					     MMAL_PARAMETER_FPS_RANGE,
+					     &fps_range, sizeof(fps_range));
+	ret += vchiq_mmal_port_parameter_set(dev->instance,
+					     &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE],
+					     MMAL_PARAMETER_FPS_RANGE,
+					     &fps_range, sizeof(fps_range));
+	if (ret)
+		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Failed to set fps ret %d\n", ret);
+
+	return ret;
+}
+
+int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl)
+{
+	int c;
+	const struct bcm2835_mmal_v4l2_ctrl *ctrl;
+
+	v4l2_ctrl_handler_init(hdl, V4L2_CTRL_COUNT);
+
+	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
+		ctrl = &v4l2_ctrls[c];
+
+		switch (ctrl->type) {
+		case MMAL_CONTROL_TYPE_STD:
+			dev->ctrls[c] = v4l2_ctrl_new_std(hdl, &bcm2835_mmal_ctrl_ops,
+							  ctrl->id, ctrl->min, ctrl->max,
+							  ctrl->step, ctrl->def);
+			break;
+
+		case MMAL_CONTROL_TYPE_STD_MENU:
+		{
+			u64 mask = ctrl->min;
+
+			if (ctrl->id == V4L2_CID_SCENE_MODE) {
+				/* Special handling to work out the mask
+				 * value based on the scene_configs array
+				 * at runtime. Reduces the chance of
+				 * mismatches.
+				 */
+				int i;
+
+				mask = BIT(V4L2_SCENE_MODE_NONE);
+				for (i = 0;
+				     i < ARRAY_SIZE(scene_configs);
+				     i++) {
+					mask |= BIT(scene_configs[i].v4l2_scene);
+				}
+				mask = ~mask;
+			}
+
+			dev->ctrls[c] = v4l2_ctrl_new_std_menu(hdl, &bcm2835_mmal_ctrl_ops,
+							       ctrl->id, ctrl->max, mask,
+							       ctrl->def);
+			break;
+		}
+
+		case MMAL_CONTROL_TYPE_INT_MENU:
+			dev->ctrls[c] = v4l2_ctrl_new_int_menu(hdl, &bcm2835_mmal_ctrl_ops,
+							       ctrl->id, ctrl->max,
+							       ctrl->def, ctrl->imenu);
+			break;
+
+		case MMAL_CONTROL_TYPE_CLUSTER:
+			/* skip this entry when constructing controls */
+			continue;
+		}
+
+		if (hdl->error)
+			break;
+
+		dev->ctrls[c]->priv = (void *)ctrl;
+	}
+
+	if (hdl->error) {
+		pr_err("error adding control %d/%d id 0x%x\n", c,
+		       V4L2_CTRL_COUNT, ctrl->id);
+		return hdl->error;
+	}
+
+	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
+		ctrl = &v4l2_ctrls[c];
+
+		switch (ctrl->type) {
+		case MMAL_CONTROL_TYPE_CLUSTER:
+			v4l2_ctrl_auto_cluster(ctrl->min,
+					       &dev->ctrls[c + 1],
+					       ctrl->max,
+					       ctrl->def);
+			break;
+
+		case MMAL_CONTROL_TYPE_STD:
+		case MMAL_CONTROL_TYPE_STD_MENU:
+		case MMAL_CONTROL_TYPE_INT_MENU:
+			break;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h b/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h
new file mode 100644
index 000000000..690ab7165
--- /dev/null
+++ b/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_H
+#define VCHIQ_H
+
+#define VCHIQ_MAKE_FOURCC(x0, x1, x2, x3) \
+			(((x0) << 24) | ((x1) << 16) | ((x2) << 8) | (x3))
+
+enum vchiq_reason {
+	VCHIQ_SERVICE_OPENED,         /* service, -, -             */
+	VCHIQ_SERVICE_CLOSED,         /* service, -, -             */
+	VCHIQ_MESSAGE_AVAILABLE,      /* service, header, -        */
+	VCHIQ_BULK_TRANSMIT_DONE,     /* service, -, bulk_userdata */
+	VCHIQ_BULK_RECEIVE_DONE,      /* service, -, bulk_userdata */
+	VCHIQ_BULK_TRANSMIT_ABORTED,  /* service, -, bulk_userdata */
+	VCHIQ_BULK_RECEIVE_ABORTED    /* service, -, bulk_userdata */
+};
+
+enum vchiq_status {
+	VCHIQ_ERROR   = -1,
+	VCHIQ_SUCCESS = 0,
+	VCHIQ_RETRY   = 1
+};
+
+enum vchiq_bulk_mode {
+	VCHIQ_BULK_MODE_CALLBACK,
+	VCHIQ_BULK_MODE_BLOCKING,
+	VCHIQ_BULK_MODE_NOCALLBACK,
+	VCHIQ_BULK_MODE_WAITING		/* Reserved for internal use */
+};
+
+enum vchiq_service_option {
+	VCHIQ_SERVICE_OPTION_AUTOCLOSE,
+	VCHIQ_SERVICE_OPTION_SLOT_QUOTA,
+	VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA,
+	VCHIQ_SERVICE_OPTION_SYNCHRONOUS,
+	VCHIQ_SERVICE_OPTION_TRACE
+};
+
+struct vchiq_header {
+	/* The message identifier - opaque to applications. */
+	int msgid;
+
+	/* Size of message data. */
+	unsigned int size;
+
+	char data[];           /* message */
+};
+
+struct vchiq_element {
+	const void __user *data;
+	unsigned int size;
+};
+
+struct vchiq_instance;
+
+struct vchiq_service_base {
+	int fourcc;
+	enum vchiq_status (*callback)(struct vchiq_instance *instance,
+				      enum vchiq_reason reason,
+				      struct vchiq_header *header,
+				      unsigned int handle,
+				      void *bulk_userdata);
+	void *userdata;
+};
+
+struct vchiq_completion_data_kernel {
+	enum vchiq_reason reason;
+	struct vchiq_header *header;
+	void *service_userdata;
+	void *bulk_userdata;
+};
+
+struct vchiq_service_params_kernel {
+	int fourcc;
+	enum vchiq_status (*callback)(struct vchiq_instance *instance,
+				      enum vchiq_reason reason,
+				      struct vchiq_header *header,
+				      unsigned int handle,
+				      void *bulk_userdata);
+	void *userdata;
+	short version;       /* Increment for non-trivial changes */
+	short version_min;   /* Update for incompatible changes */
+};
+
+struct vchiq_instance;
+
+extern int vchiq_initialise(struct vchiq_instance **pinstance);
+extern enum vchiq_status vchiq_shutdown(struct vchiq_instance *instance);
+extern enum vchiq_status vchiq_connect(struct vchiq_instance *instance);
+extern enum vchiq_status vchiq_open_service(struct vchiq_instance *instance,
+	const struct vchiq_service_params_kernel *params,
+	unsigned int *pservice);
+extern enum vchiq_status vchiq_close_service(struct vchiq_instance *instance,
+					     unsigned int service);
+extern enum vchiq_status vchiq_use_service(struct vchiq_instance *instance, unsigned int service);
+extern enum vchiq_status vchiq_release_service(struct vchiq_instance *instance,
+					       unsigned int service);
+extern void vchiq_msg_queue_push(struct vchiq_instance *instance, unsigned int handle,
+				 struct vchiq_header *header);
+extern void vchiq_release_message(struct vchiq_instance *instance, unsigned int service,
+				  struct vchiq_header *header);
+extern int vchiq_queue_kernel_message(struct vchiq_instance *instance, unsigned int handle,
+				      void *data, unsigned int size);
+extern enum vchiq_status vchiq_bulk_transmit(struct vchiq_instance *instance, unsigned int service,
+					     const void *data, unsigned int size, void *userdata,
+					     enum vchiq_bulk_mode mode);
+extern enum vchiq_status vchiq_bulk_receive(struct vchiq_instance *instance, unsigned int service,
+					    void *data, unsigned int size, void *userdata,
+					    enum vchiq_bulk_mode mode);
+extern void *vchiq_get_service_userdata(struct vchiq_instance *instance, unsigned int service);
+extern enum vchiq_status vchiq_get_peer_version(struct vchiq_instance *instance,
+						unsigned int handle,
+						short *peer_version);
+extern struct vchiq_header *vchiq_msg_hold(struct vchiq_instance *instance, unsigned int handle);
+
+#endif /* VCHIQ_H */
diff --git a/drivers/staging/vc04_services/interface/TESTING b/drivers/staging/vc04_services/interface/TESTING
new file mode 100644
index 000000000..a6d63efcb
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/TESTING
@@ -0,0 +1,82 @@
+This document contains some hints to test the function of the VCHIQ driver
+without having additional hardware to the Raspberry Pi.
+
+* Requirements & limitations
+
+Testing the VCHIQ driver requires a Raspberry Pi with one of the following SoC:
+  - BCM2835 ( e.g. Raspberry Pi Zero W )
+  - BCM2836 ( e.g. Raspberry Pi 2 )
+  - BCM2837 ( e.g. Raspberry Pi 3 B+ )
+
+The BCM2711 used in the Raspberry Pi 4 is currently not supported in the
+mainline kernel.
+
+There are no specific requirements to the VideoCore firmware to get VCHIQ
+working.
+
+The test scenarios described in this document based on the tool vchiq_test.
+Its source code is available here: https://github.com/raspberrypi/userland
+
+* Configuration
+
+Here are the most common kernel configurations:
+
+ 1. BCM2835 target SoC (ARM 32 bit)
+
+    Just use bcm2835_defconfig which already has VCHIQ enabled.
+
+ 2. BCM2836/7 target SoC (ARM 32 bit)
+
+    Use the multi_v7_defconfig as a base and then enable all VCHIQ options.
+
+ 3. BCM2837 target SoC (ARM 64 bit)
+
+    Use the defconfig as a base and then enable all VCHIQ options.
+
+* Scenarios
+
+ * Initial test
+
+   Check the driver is probed and /dev/vchiq is created
+
+ * Functional test
+
+   Command: vchiq_test -f 10
+
+   Expected output:
+   Functional test - iters:10
+   ======== iteration 1 ========
+   Testing bulk transfer for alignment.
+   Testing bulk transfer at PAGE_SIZE.
+   ...
+
+ * Ping test
+
+   Command: vchiq_test -p 1
+
+   Expected output:
+   Ping test - service:echo, iters:1, version 3
+   vchi ping (size 0) -> 57.000000us
+   vchi ping (size 0, 0 async, 0 oneway) -> 122.000000us
+   vchi bulk (size 0, 0 async, 0 oneway) -> 546.000000us
+   vchi bulk (size 0, 0 oneway) -> 230.000000us
+   vchi ping (size 0) -> 49.000000us
+   vchi ping (size 0, 0 async, 0 oneway) -> 70.000000us
+   vchi bulk (size 0, 0 async, 0 oneway) -> 296.000000us
+   vchi bulk (size 0, 0 oneway) -> 266.000000us
+   vchi ping (size 0, 1 async, 0 oneway) -> 65.000000us
+   vchi bulk (size 0, 0 oneway) -> 456.000000us
+   vchi ping (size 0, 2 async, 0 oneway) -> 74.000000us
+   vchi bulk (size 0, 0 oneway) -> 640.000000us
+   vchi ping (size 0, 10 async, 0 oneway) -> 125.000000us
+   vchi bulk (size 0, 0 oneway) -> 2309.000000us
+   vchi ping (size 0, 0 async, 1 oneway) -> 70.000000us
+   vchi ping (size 0, 0 async, 2 oneway) -> 76.000000us
+   vchi ping (size 0, 0 async, 10 oneway) -> 105.000000us
+   vchi ping (size 0, 10 async, 10 oneway) -> 165.000000us
+   vchi ping (size 0, 100 async, 0 oneway) -> nanus
+   vchi bulk (size 0, 0 oneway) -> nanus
+   vchi ping (size 0, 0 async, 100 oneway) -> nanus
+   vchi ping (size 0, 100 async, 100 oneway) -> infus
+   vchi ping (size 0, 200 async, 0 oneway) -> infus
+   ...
diff --git a/drivers/staging/vc04_services/interface/TODO b/drivers/staging/vc04_services/interface/TODO
new file mode 100644
index 000000000..97085a0b3
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/TODO
@@ -0,0 +1,73 @@
+* Import drivers using VCHI.
+
+VCHI is just a tool to let drivers talk to the firmware.  Here are
+some of the ones we want:
+
+  - vc_mem (https://github.com/raspberrypi/linux/blob/rpi-4.4.y/drivers/char/broadcom/vc_mem.c)
+
+  This driver is what the vcdbg userspace program uses to set up its
+  requests to the firmware, which are transmitted across VCHIQ.  vcdbg
+  is really useful for debugging firmware interactions.
+
+  - VCSM (https://github.com/raspberrypi/linux/tree/rpi-4.4.y/drivers/char/broadcom/vc_sm)
+
+  This driver is used for talking about regions of VC memory across
+  firmware protocols including VCHI.  We'll want to extend this driver
+  to manage these buffers as dmabufs so that we can zero-copy import
+  camera images into vc4 for rendering/display.
+
+* Fix kernel module support
+
+Even the VPU firmware doesn't support a VCHI re-connect, the driver
+should properly handle a module unload. This also includes that all
+resources must be freed (kthreads, debugfs entries, ...) and global
+variables avoided.
+
+* Cleanup logging mechanism
+
+The driver should probably be using the standard kernel logging mechanisms
+such as dev_info, dev_dbg, and friends.
+
+* Documentation
+
+A short top-down description of this driver's architecture (function of
+kthreads, userspace, limitations) could be very helpful for reviewers.
+
+* Review and comment memory barriers
+
+There is a heavy use of memory barriers in this driver, it would be very
+beneficial to go over all of them and, if correct, comment on their merits.
+Extra points to whomever confidently reviews the remote_event_*() family of
+functions.
+
+* Get rid of custom function return values
+
+Most functions use a custom set of return values, we should force proper Linux
+error numbers. Special care is needed for VCHIQ_RETRY.
+
+* Reformat core code with more sane indentations
+
+The code follows the 80 characters limitation yet tends to go 3 or 4 levels of
+indentation deep making it very unpleasant to read. This is specially relevant
+in the character driver ioctl code and in the core thread functions.
+
+* Get rid of all non essential global structures and create a proper per
+device structure
+
+The first thing one generally sees in a probe function is a memory allocation
+for all the device specific data. This structure is then passed all over the
+driver. This is good practice since it makes the driver work regardless of the
+number of devices probed.
+
+* Clean up Sparse warnings from __user annotations. See
+vchiq_irq_queue_bulk_tx_rx(). Ensure that the address of "&waiter->bulk_waiter"
+is never disclosed to userspace.
+
+* Fix behavior of message handling
+
+The polling behavior of vchiq_bulk_transmit(), vchiq_bulk_receive() and
+vchiq_queue_kernel_message() looks broken. A possible signal should be
+propagated back to user space to let the calling task handle it before
+retrying. Hopefully these msleep(1) shouldn't be necessary anymore.
+
+https://lore.kernel.org/linux-staging/CAK8P3a3HGm1cPo4sW9fOY4E8AN8yAq3tevXxU5m8bmtmsU8WKw@mail.gmail.com/
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
new file mode 100644
index 000000000..705c5e283
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -0,0 +1,1885 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched/signal.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/highmem.h>
+#include <linux/pagemap.h>
+#include <linux/bug.h>
+#include <linux/completion.h>
+#include <linux/list.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/compat.h>
+#include <linux/dma-mapping.h>
+#include <linux/rcupdate.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <soc/bcm2835/raspberrypi-firmware.h>
+
+#include "vchiq_core.h"
+#include "vchiq_ioctl.h"
+#include "vchiq_arm.h"
+#include "vchiq_debugfs.h"
+#include "vchiq_connected.h"
+#include "vchiq_pagelist.h"
+
+#define DEVICE_NAME "vchiq"
+
+#define TOTAL_SLOTS (VCHIQ_SLOT_ZERO_SLOTS + 2 * 32)
+
+#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2)
+
+#define VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX 0
+#define VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX  1
+
+#define BELL0	0x00
+#define BELL2	0x08
+
+#define ARM_DS_ACTIVE	BIT(2)
+
+/* Override the default prefix, which would be vchiq_arm (from the filename) */
+#undef MODULE_PARAM_PREFIX
+#define MODULE_PARAM_PREFIX DEVICE_NAME "."
+
+#define KEEPALIVE_VER 1
+#define KEEPALIVE_VER_MIN KEEPALIVE_VER
+
+/* Run time control of log level, based on KERN_XXX level. */
+int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT;
+int vchiq_susp_log_level = VCHIQ_LOG_ERROR;
+
+DEFINE_SPINLOCK(msg_queue_spinlock);
+struct vchiq_state g_state;
+
+static struct platform_device *bcm2835_camera;
+static struct platform_device *bcm2835_audio;
+
+struct vchiq_drvdata {
+	const unsigned int cache_line_size;
+	struct rpi_firmware *fw;
+};
+
+static struct vchiq_drvdata bcm2835_drvdata = {
+	.cache_line_size = 32,
+};
+
+static struct vchiq_drvdata bcm2836_drvdata = {
+	.cache_line_size = 64,
+};
+
+struct vchiq_arm_state {
+	/* Keepalive-related data */
+	struct task_struct *ka_thread;
+	struct completion ka_evt;
+	atomic_t ka_use_count;
+	atomic_t ka_use_ack_count;
+	atomic_t ka_release_count;
+
+	rwlock_t susp_res_lock;
+
+	struct vchiq_state *state;
+
+	/*
+	 * Global use count for videocore.
+	 * This is equal to the sum of the use counts for all services.  When
+	 * this hits zero the videocore suspend procedure will be initiated.
+	 */
+	int videocore_use_count;
+
+	/*
+	 * Use count to track requests from videocore peer.
+	 * This use count is not associated with a service, so needs to be
+	 * tracked separately with the state.
+	 */
+	int peer_use_count;
+
+	/*
+	 * Flag to indicate that the first vchiq connect has made it through.
+	 * This means that both sides should be fully ready, and we should
+	 * be able to suspend after this point.
+	 */
+	int first_connect;
+};
+
+struct vchiq_2835_state {
+	int inited;
+	struct vchiq_arm_state arm_state;
+};
+
+struct vchiq_pagelist_info {
+	struct pagelist *pagelist;
+	size_t pagelist_buffer_size;
+	dma_addr_t dma_addr;
+	enum dma_data_direction dma_dir;
+	unsigned int num_pages;
+	unsigned int pages_need_release;
+	struct page **pages;
+	struct scatterlist *scatterlist;
+	unsigned int scatterlist_mapped;
+};
+
+static void __iomem *g_regs;
+/* This value is the size of the L2 cache lines as understood by the
+ * VPU firmware, which determines the required alignment of the
+ * offsets/sizes in pagelists.
+ *
+ * Modern VPU firmware looks for a DT "cache-line-size" property in
+ * the VCHIQ node and will overwrite it with the actual L2 cache size,
+ * which the kernel must then respect.  That property was rejected
+ * upstream, so we have to use the VPU firmware's compatibility value
+ * of 32.
+ */
+static unsigned int g_cache_line_size = 32;
+static unsigned int g_fragments_size;
+static char *g_fragments_base;
+static char *g_free_fragments;
+static struct semaphore g_free_fragments_sema;
+
+static DEFINE_SEMAPHORE(g_free_fragments_mutex);
+
+static enum vchiq_status
+vchiq_blocking_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *data,
+			     unsigned int size, enum vchiq_bulk_dir dir);
+
+static irqreturn_t
+vchiq_doorbell_irq(int irq, void *dev_id)
+{
+	struct vchiq_state *state = dev_id;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int status;
+
+	/* Read (and clear) the doorbell */
+	status = readl(g_regs + BELL0);
+
+	if (status & ARM_DS_ACTIVE) {  /* Was the doorbell rung? */
+		remote_event_pollall(state);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static void
+cleanup_pagelistinfo(struct vchiq_instance *instance, struct vchiq_pagelist_info *pagelistinfo)
+{
+	if (pagelistinfo->scatterlist_mapped) {
+		dma_unmap_sg(instance->state->dev, pagelistinfo->scatterlist,
+			     pagelistinfo->num_pages, pagelistinfo->dma_dir);
+	}
+
+	if (pagelistinfo->pages_need_release)
+		unpin_user_pages(pagelistinfo->pages, pagelistinfo->num_pages);
+
+	dma_free_coherent(instance->state->dev, pagelistinfo->pagelist_buffer_size,
+			  pagelistinfo->pagelist, pagelistinfo->dma_addr);
+}
+
+static inline bool
+is_adjacent_block(u32 *addrs, u32 addr, unsigned int k)
+{
+	u32 tmp;
+
+	if (!k)
+		return false;
+
+	tmp = (addrs[k - 1] & PAGE_MASK) +
+	      (((addrs[k - 1] & ~PAGE_MASK) + 1) << PAGE_SHIFT);
+
+	return tmp == (addr & PAGE_MASK);
+}
+
+/* There is a potential problem with partial cache lines (pages?)
+ * at the ends of the block when reading. If the CPU accessed anything in
+ * the same line (page?) then it may have pulled old data into the cache,
+ * obscuring the new data underneath. We can solve this by transferring the
+ * partial cache lines separately, and allowing the ARM to copy into the
+ * cached area.
+ */
+
+static struct vchiq_pagelist_info *
+create_pagelist(struct vchiq_instance *instance, char *buf, char __user *ubuf,
+		size_t count, unsigned short type)
+{
+	struct pagelist *pagelist;
+	struct vchiq_pagelist_info *pagelistinfo;
+	struct page **pages;
+	u32 *addrs;
+	unsigned int num_pages, offset, i, k;
+	int actual_pages;
+	size_t pagelist_size;
+	struct scatterlist *scatterlist, *sg;
+	int dma_buffers;
+	dma_addr_t dma_addr;
+
+	if (count >= INT_MAX - PAGE_SIZE)
+		return NULL;
+
+	if (buf)
+		offset = (uintptr_t)buf & (PAGE_SIZE - 1);
+	else
+		offset = (uintptr_t)ubuf & (PAGE_SIZE - 1);
+	num_pages = DIV_ROUND_UP(count + offset, PAGE_SIZE);
+
+	if ((size_t)num_pages > (SIZE_MAX - sizeof(struct pagelist) -
+			 sizeof(struct vchiq_pagelist_info)) /
+			(sizeof(u32) + sizeof(pages[0]) +
+			 sizeof(struct scatterlist)))
+		return NULL;
+
+	pagelist_size = sizeof(struct pagelist) +
+			(num_pages * sizeof(u32)) +
+			(num_pages * sizeof(pages[0]) +
+			(num_pages * sizeof(struct scatterlist))) +
+			sizeof(struct vchiq_pagelist_info);
+
+	/* Allocate enough storage to hold the page pointers and the page
+	 * list
+	 */
+	pagelist = dma_alloc_coherent(instance->state->dev, pagelist_size, &dma_addr,
+				      GFP_KERNEL);
+
+	vchiq_log_trace(vchiq_arm_log_level, "%s - %pK", __func__, pagelist);
+
+	if (!pagelist)
+		return NULL;
+
+	addrs		= pagelist->addrs;
+	pages		= (struct page **)(addrs + num_pages);
+	scatterlist	= (struct scatterlist *)(pages + num_pages);
+	pagelistinfo	= (struct vchiq_pagelist_info *)
+			  (scatterlist + num_pages);
+
+	pagelist->length = count;
+	pagelist->type = type;
+	pagelist->offset = offset;
+
+	/* Populate the fields of the pagelistinfo structure */
+	pagelistinfo->pagelist = pagelist;
+	pagelistinfo->pagelist_buffer_size = pagelist_size;
+	pagelistinfo->dma_addr = dma_addr;
+	pagelistinfo->dma_dir =  (type == PAGELIST_WRITE) ?
+				  DMA_TO_DEVICE : DMA_FROM_DEVICE;
+	pagelistinfo->num_pages = num_pages;
+	pagelistinfo->pages_need_release = 0;
+	pagelistinfo->pages = pages;
+	pagelistinfo->scatterlist = scatterlist;
+	pagelistinfo->scatterlist_mapped = 0;
+
+	if (buf) {
+		unsigned long length = count;
+		unsigned int off = offset;
+
+		for (actual_pages = 0; actual_pages < num_pages;
+		     actual_pages++) {
+			struct page *pg =
+				vmalloc_to_page((buf +
+						 (actual_pages * PAGE_SIZE)));
+			size_t bytes = PAGE_SIZE - off;
+
+			if (!pg) {
+				cleanup_pagelistinfo(instance, pagelistinfo);
+				return NULL;
+			}
+
+			if (bytes > length)
+				bytes = length;
+			pages[actual_pages] = pg;
+			length -= bytes;
+			off = 0;
+		}
+		/* do not try and release vmalloc pages */
+	} else {
+		actual_pages = pin_user_pages_fast((unsigned long)ubuf & PAGE_MASK, num_pages,
+						   type == PAGELIST_READ, pages);
+
+		if (actual_pages != num_pages) {
+			vchiq_log_info(vchiq_arm_log_level,
+				       "%s - only %d/%d pages locked",
+				       __func__, actual_pages, num_pages);
+
+			/* This is probably due to the process being killed */
+			if (actual_pages > 0)
+				unpin_user_pages(pages, actual_pages);
+			cleanup_pagelistinfo(instance, pagelistinfo);
+			return NULL;
+		}
+		 /* release user pages */
+		pagelistinfo->pages_need_release = 1;
+	}
+
+	/*
+	 * Initialize the scatterlist so that the magic cookie
+	 *  is filled if debugging is enabled
+	 */
+	sg_init_table(scatterlist, num_pages);
+	/* Now set the pages for each scatterlist */
+	for (i = 0; i < num_pages; i++)	{
+		unsigned int len = PAGE_SIZE - offset;
+
+		if (len > count)
+			len = count;
+		sg_set_page(scatterlist + i, pages[i], len, offset);
+		offset = 0;
+		count -= len;
+	}
+
+	dma_buffers = dma_map_sg(instance->state->dev,
+				 scatterlist,
+				 num_pages,
+				 pagelistinfo->dma_dir);
+
+	if (dma_buffers == 0) {
+		cleanup_pagelistinfo(instance, pagelistinfo);
+		return NULL;
+	}
+
+	pagelistinfo->scatterlist_mapped = 1;
+
+	/* Combine adjacent blocks for performance */
+	k = 0;
+	for_each_sg(scatterlist, sg, dma_buffers, i) {
+		u32 len = sg_dma_len(sg);
+		u32 addr = sg_dma_address(sg);
+
+		/* Note: addrs is the address + page_count - 1
+		 * The firmware expects blocks after the first to be page-
+		 * aligned and a multiple of the page size
+		 */
+		WARN_ON(len == 0);
+		WARN_ON(i && (i != (dma_buffers - 1)) && (len & ~PAGE_MASK));
+		WARN_ON(i && (addr & ~PAGE_MASK));
+		if (is_adjacent_block(addrs, addr, k))
+			addrs[k - 1] += ((len + PAGE_SIZE - 1) >> PAGE_SHIFT);
+		else
+			addrs[k++] = (addr & PAGE_MASK) |
+				(((len + PAGE_SIZE - 1) >> PAGE_SHIFT) - 1);
+	}
+
+	/* Partial cache lines (fragments) require special measures */
+	if ((type == PAGELIST_READ) &&
+	    ((pagelist->offset & (g_cache_line_size - 1)) ||
+	    ((pagelist->offset + pagelist->length) &
+	    (g_cache_line_size - 1)))) {
+		char *fragments;
+
+		if (down_interruptible(&g_free_fragments_sema)) {
+			cleanup_pagelistinfo(instance, pagelistinfo);
+			return NULL;
+		}
+
+		WARN_ON(!g_free_fragments);
+
+		down(&g_free_fragments_mutex);
+		fragments = g_free_fragments;
+		WARN_ON(!fragments);
+		g_free_fragments = *(char **)g_free_fragments;
+		up(&g_free_fragments_mutex);
+		pagelist->type = PAGELIST_READ_WITH_FRAGMENTS +
+			(fragments - g_fragments_base) / g_fragments_size;
+	}
+
+	return pagelistinfo;
+}
+
+static void
+free_pagelist(struct vchiq_instance *instance, struct vchiq_pagelist_info *pagelistinfo,
+	      int actual)
+{
+	struct pagelist *pagelist = pagelistinfo->pagelist;
+	struct page **pages = pagelistinfo->pages;
+	unsigned int num_pages = pagelistinfo->num_pages;
+
+	vchiq_log_trace(vchiq_arm_log_level, "%s - %pK, %d",
+			__func__, pagelistinfo->pagelist, actual);
+
+	/*
+	 * NOTE: dma_unmap_sg must be called before the
+	 * cpu can touch any of the data/pages.
+	 */
+	dma_unmap_sg(instance->state->dev, pagelistinfo->scatterlist,
+		     pagelistinfo->num_pages, pagelistinfo->dma_dir);
+	pagelistinfo->scatterlist_mapped = 0;
+
+	/* Deal with any partial cache lines (fragments) */
+	if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS && g_fragments_base) {
+		char *fragments = g_fragments_base +
+			(pagelist->type - PAGELIST_READ_WITH_FRAGMENTS) *
+			g_fragments_size;
+		int head_bytes, tail_bytes;
+
+		head_bytes = (g_cache_line_size - pagelist->offset) &
+			(g_cache_line_size - 1);
+		tail_bytes = (pagelist->offset + actual) &
+			(g_cache_line_size - 1);
+
+		if ((actual >= 0) && (head_bytes != 0)) {
+			if (head_bytes > actual)
+				head_bytes = actual;
+
+			memcpy_to_page(pages[0],
+				pagelist->offset,
+				fragments,
+				head_bytes);
+		}
+		if ((actual >= 0) && (head_bytes < actual) &&
+		    (tail_bytes != 0))
+			memcpy_to_page(pages[num_pages - 1],
+				(pagelist->offset + actual) &
+				(PAGE_SIZE - 1) & ~(g_cache_line_size - 1),
+				fragments + g_cache_line_size,
+				tail_bytes);
+
+		down(&g_free_fragments_mutex);
+		*(char **)fragments = g_free_fragments;
+		g_free_fragments = fragments;
+		up(&g_free_fragments_mutex);
+		up(&g_free_fragments_sema);
+	}
+
+	/* Need to mark all the pages dirty. */
+	if (pagelist->type != PAGELIST_WRITE &&
+	    pagelistinfo->pages_need_release) {
+		unsigned int i;
+
+		for (i = 0; i < num_pages; i++)
+			set_page_dirty(pages[i]);
+	}
+
+	cleanup_pagelistinfo(instance, pagelistinfo);
+}
+
+static int vchiq_platform_init(struct platform_device *pdev, struct vchiq_state *state)
+{
+	struct device *dev = &pdev->dev;
+	struct vchiq_drvdata *drvdata = platform_get_drvdata(pdev);
+	struct rpi_firmware *fw = drvdata->fw;
+	struct vchiq_slot_zero *vchiq_slot_zero;
+	void *slot_mem;
+	dma_addr_t slot_phys;
+	u32 channelbase;
+	int slot_mem_size, frag_mem_size;
+	int err, irq, i;
+
+	/*
+	 * VCHI messages between the CPU and firmware use
+	 * 32-bit bus addresses.
+	 */
+	err = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
+
+	if (err < 0)
+		return err;
+
+	g_cache_line_size = drvdata->cache_line_size;
+	g_fragments_size = 2 * g_cache_line_size;
+
+	/* Allocate space for the channels in coherent memory */
+	slot_mem_size = PAGE_ALIGN(TOTAL_SLOTS * VCHIQ_SLOT_SIZE);
+	frag_mem_size = PAGE_ALIGN(g_fragments_size * MAX_FRAGMENTS);
+
+	slot_mem = dmam_alloc_coherent(dev, slot_mem_size + frag_mem_size,
+				       &slot_phys, GFP_KERNEL);
+	if (!slot_mem) {
+		dev_err(dev, "could not allocate DMA memory\n");
+		return -ENOMEM;
+	}
+
+	WARN_ON(((unsigned long)slot_mem & (PAGE_SIZE - 1)) != 0);
+
+	vchiq_slot_zero = vchiq_init_slots(slot_mem, slot_mem_size);
+	if (!vchiq_slot_zero)
+		return -EINVAL;
+
+	vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX] =
+		(int)slot_phys + slot_mem_size;
+	vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX] =
+		MAX_FRAGMENTS;
+
+	g_fragments_base = (char *)slot_mem + slot_mem_size;
+
+	g_free_fragments = g_fragments_base;
+	for (i = 0; i < (MAX_FRAGMENTS - 1); i++) {
+		*(char **)&g_fragments_base[i * g_fragments_size] =
+			&g_fragments_base[(i + 1) * g_fragments_size];
+	}
+	*(char **)&g_fragments_base[i * g_fragments_size] = NULL;
+	sema_init(&g_free_fragments_sema, MAX_FRAGMENTS);
+
+	err = vchiq_init_state(state, vchiq_slot_zero, dev);
+	if (err)
+		return err;
+
+	g_regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(g_regs))
+		return PTR_ERR(g_regs);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0)
+		return irq;
+
+	err = devm_request_irq(dev, irq, vchiq_doorbell_irq, IRQF_IRQPOLL,
+			       "VCHIQ doorbell", state);
+	if (err) {
+		dev_err(dev, "failed to register irq=%d\n", irq);
+		return err;
+	}
+
+	/* Send the base address of the slots to VideoCore */
+	channelbase = slot_phys;
+	err = rpi_firmware_property(fw, RPI_FIRMWARE_VCHIQ_INIT,
+				    &channelbase, sizeof(channelbase));
+	if (err || channelbase) {
+		dev_err(dev, "failed to set channelbase\n");
+		return err ? : -ENXIO;
+	}
+
+	vchiq_log_info(vchiq_arm_log_level, "vchiq_init - done (slots %pK, phys %pad)",
+		       vchiq_slot_zero, &slot_phys);
+
+	vchiq_call_connected_callbacks();
+
+	return 0;
+}
+
+static void
+vchiq_arm_init_state(struct vchiq_state *state,
+		     struct vchiq_arm_state *arm_state)
+{
+	if (arm_state) {
+		rwlock_init(&arm_state->susp_res_lock);
+
+		init_completion(&arm_state->ka_evt);
+		atomic_set(&arm_state->ka_use_count, 0);
+		atomic_set(&arm_state->ka_use_ack_count, 0);
+		atomic_set(&arm_state->ka_release_count, 0);
+
+		arm_state->state = state;
+		arm_state->first_connect = 0;
+	}
+}
+
+int
+vchiq_platform_init_state(struct vchiq_state *state)
+{
+	struct vchiq_2835_state *platform_state;
+
+	state->platform_state = kzalloc(sizeof(*platform_state), GFP_KERNEL);
+	if (!state->platform_state)
+		return -ENOMEM;
+
+	platform_state = (struct vchiq_2835_state *)state->platform_state;
+
+	platform_state->inited = 1;
+	vchiq_arm_init_state(state, &platform_state->arm_state);
+
+	return 0;
+}
+
+static struct vchiq_arm_state *vchiq_platform_get_arm_state(struct vchiq_state *state)
+{
+	struct vchiq_2835_state *platform_state;
+
+	platform_state   = (struct vchiq_2835_state *)state->platform_state;
+
+	WARN_ON_ONCE(!platform_state->inited);
+
+	return &platform_state->arm_state;
+}
+
+void
+remote_event_signal(struct remote_event *event)
+{
+	/*
+	 * Ensure that all writes to shared data structures have completed
+	 * before signalling the peer.
+	 */
+	wmb();
+
+	event->fired = 1;
+
+	dsb(sy);         /* data barrier operation */
+
+	if (event->armed)
+		writel(0, g_regs + BELL2); /* trigger vc interrupt */
+}
+
+int
+vchiq_prepare_bulk_data(struct vchiq_instance *instance, struct vchiq_bulk *bulk, void *offset,
+			void __user *uoffset, int size, int dir)
+{
+	struct vchiq_pagelist_info *pagelistinfo;
+
+	pagelistinfo = create_pagelist(instance, offset, uoffset, size,
+				       (dir == VCHIQ_BULK_RECEIVE)
+				       ? PAGELIST_READ
+				       : PAGELIST_WRITE);
+
+	if (!pagelistinfo)
+		return -ENOMEM;
+
+	bulk->data = pagelistinfo->dma_addr;
+
+	/*
+	 * Store the pagelistinfo address in remote_data,
+	 * which isn't used by the slave.
+	 */
+	bulk->remote_data = pagelistinfo;
+
+	return 0;
+}
+
+void
+vchiq_complete_bulk(struct vchiq_instance *instance, struct vchiq_bulk *bulk)
+{
+	if (bulk && bulk->remote_data && bulk->actual)
+		free_pagelist(instance, (struct vchiq_pagelist_info *)bulk->remote_data,
+			      bulk->actual);
+}
+
+int vchiq_dump_platform_state(void *dump_context)
+{
+	char buf[80];
+	int len;
+
+	len = snprintf(buf, sizeof(buf), "  Platform: 2835 (VC master)");
+	return vchiq_dump(dump_context, buf, len + 1);
+}
+
+#define VCHIQ_INIT_RETRIES 10
+int vchiq_initialise(struct vchiq_instance **instance_out)
+{
+	struct vchiq_state *state;
+	struct vchiq_instance *instance = NULL;
+	int i, ret;
+
+	/*
+	 * VideoCore may not be ready due to boot up timing.
+	 * It may never be ready if kernel and firmware are mismatched,so don't
+	 * block forever.
+	 */
+	for (i = 0; i < VCHIQ_INIT_RETRIES; i++) {
+		state = vchiq_get_state();
+		if (state)
+			break;
+		usleep_range(500, 600);
+	}
+	if (i == VCHIQ_INIT_RETRIES) {
+		vchiq_log_error(vchiq_core_log_level, "%s: videocore not initialized\n", __func__);
+		ret = -ENOTCONN;
+		goto failed;
+	} else if (i > 0) {
+		vchiq_log_warning(vchiq_core_log_level,
+				  "%s: videocore initialized after %d retries\n", __func__, i);
+	}
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance) {
+		vchiq_log_error(vchiq_core_log_level,
+				"%s: error allocating vchiq instance\n", __func__);
+		ret = -ENOMEM;
+		goto failed;
+	}
+
+	instance->connected = 0;
+	instance->state = state;
+	mutex_init(&instance->bulk_waiter_list_mutex);
+	INIT_LIST_HEAD(&instance->bulk_waiter_list);
+
+	*instance_out = instance;
+
+	ret = 0;
+
+failed:
+	vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL(vchiq_initialise);
+
+void free_bulk_waiter(struct vchiq_instance *instance)
+{
+	struct bulk_waiter_node *waiter, *next;
+
+	list_for_each_entry_safe(waiter, next,
+				 &instance->bulk_waiter_list, list) {
+		list_del(&waiter->list);
+		vchiq_log_info(vchiq_arm_log_level, "bulk_waiter - cleaned up %pK for pid %d",
+			       waiter, waiter->pid);
+		kfree(waiter);
+	}
+}
+
+enum vchiq_status vchiq_shutdown(struct vchiq_instance *instance)
+{
+	enum vchiq_status status = VCHIQ_SUCCESS;
+	struct vchiq_state *state = instance->state;
+
+	if (mutex_lock_killable(&state->mutex))
+		return VCHIQ_RETRY;
+
+	/* Remove all services */
+	vchiq_shutdown_internal(state, instance);
+
+	mutex_unlock(&state->mutex);
+
+	vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status);
+
+	free_bulk_waiter(instance);
+	kfree(instance);
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_shutdown);
+
+static int vchiq_is_connected(struct vchiq_instance *instance)
+{
+	return instance->connected;
+}
+
+enum vchiq_status vchiq_connect(struct vchiq_instance *instance)
+{
+	enum vchiq_status status;
+	struct vchiq_state *state = instance->state;
+
+	if (mutex_lock_killable(&state->mutex)) {
+		vchiq_log_trace(vchiq_core_log_level, "%s: call to mutex_lock failed", __func__);
+		status = VCHIQ_RETRY;
+		goto failed;
+	}
+	status = vchiq_connect_internal(state, instance);
+
+	if (status == VCHIQ_SUCCESS)
+		instance->connected = 1;
+
+	mutex_unlock(&state->mutex);
+
+failed:
+	vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status);
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_connect);
+
+static enum vchiq_status
+vchiq_add_service(struct vchiq_instance *instance,
+		  const struct vchiq_service_params_kernel *params,
+		  unsigned int *phandle)
+{
+	enum vchiq_status status;
+	struct vchiq_state *state = instance->state;
+	struct vchiq_service *service = NULL;
+	int srvstate;
+
+	*phandle = VCHIQ_SERVICE_HANDLE_INVALID;
+
+	srvstate = vchiq_is_connected(instance)
+		? VCHIQ_SRVSTATE_LISTENING
+		: VCHIQ_SRVSTATE_HIDDEN;
+
+	service = vchiq_add_service_internal(state, params, srvstate, instance, NULL);
+
+	if (service) {
+		*phandle = service->handle;
+		status = VCHIQ_SUCCESS;
+	} else {
+		status = VCHIQ_ERROR;
+	}
+
+	vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status);
+
+	return status;
+}
+
+enum vchiq_status
+vchiq_open_service(struct vchiq_instance *instance,
+		   const struct vchiq_service_params_kernel *params,
+		   unsigned int *phandle)
+{
+	enum vchiq_status   status = VCHIQ_ERROR;
+	struct vchiq_state   *state = instance->state;
+	struct vchiq_service *service = NULL;
+
+	*phandle = VCHIQ_SERVICE_HANDLE_INVALID;
+
+	if (!vchiq_is_connected(instance))
+		goto failed;
+
+	service = vchiq_add_service_internal(state, params, VCHIQ_SRVSTATE_OPENING, instance, NULL);
+
+	if (service) {
+		*phandle = service->handle;
+		status = vchiq_open_service_internal(service, current->pid);
+		if (status != VCHIQ_SUCCESS) {
+			vchiq_remove_service(instance, service->handle);
+			*phandle = VCHIQ_SERVICE_HANDLE_INVALID;
+		}
+	}
+
+failed:
+	vchiq_log_trace(vchiq_core_log_level, "%s(%p): returning %d", __func__, instance, status);
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_open_service);
+
+enum vchiq_status
+vchiq_bulk_transmit(struct vchiq_instance *instance, unsigned int handle, const void *data,
+		    unsigned int size, void *userdata, enum vchiq_bulk_mode mode)
+{
+	enum vchiq_status status;
+
+	while (1) {
+		switch (mode) {
+		case VCHIQ_BULK_MODE_NOCALLBACK:
+		case VCHIQ_BULK_MODE_CALLBACK:
+			status = vchiq_bulk_transfer(instance, handle,
+						     (void *)data, NULL,
+						     size, userdata, mode,
+						     VCHIQ_BULK_TRANSMIT);
+			break;
+		case VCHIQ_BULK_MODE_BLOCKING:
+			status = vchiq_blocking_bulk_transfer(instance, handle, (void *)data, size,
+							      VCHIQ_BULK_TRANSMIT);
+			break;
+		default:
+			return VCHIQ_ERROR;
+		}
+
+		/*
+		 * vchiq_*_bulk_transfer() may return VCHIQ_RETRY, so we need
+		 * to implement a retry mechanism since this function is
+		 * supposed to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
+
+		msleep(1);
+	}
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_bulk_transmit);
+
+enum vchiq_status vchiq_bulk_receive(struct vchiq_instance *instance, unsigned int handle,
+				     void *data, unsigned int size, void *userdata,
+				     enum vchiq_bulk_mode mode)
+{
+	enum vchiq_status status;
+
+	while (1) {
+		switch (mode) {
+		case VCHIQ_BULK_MODE_NOCALLBACK:
+		case VCHIQ_BULK_MODE_CALLBACK:
+			status = vchiq_bulk_transfer(instance, handle, data, NULL,
+						     size, userdata,
+						     mode, VCHIQ_BULK_RECEIVE);
+			break;
+		case VCHIQ_BULK_MODE_BLOCKING:
+			status = vchiq_blocking_bulk_transfer(instance, handle, (void *)data, size,
+							      VCHIQ_BULK_RECEIVE);
+			break;
+		default:
+			return VCHIQ_ERROR;
+		}
+
+		/*
+		 * vchiq_*_bulk_transfer() may return VCHIQ_RETRY, so we need
+		 * to implement a retry mechanism since this function is
+		 * supposed to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
+
+		msleep(1);
+	}
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_bulk_receive);
+
+static enum vchiq_status
+vchiq_blocking_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *data,
+			     unsigned int size, enum vchiq_bulk_dir dir)
+{
+	struct vchiq_service *service;
+	enum vchiq_status status;
+	struct bulk_waiter_node *waiter = NULL, *iter;
+
+	service = find_service_by_handle(instance, handle);
+	if (!service)
+		return VCHIQ_ERROR;
+
+	vchiq_service_put(service);
+
+	mutex_lock(&instance->bulk_waiter_list_mutex);
+	list_for_each_entry(iter, &instance->bulk_waiter_list, list) {
+		if (iter->pid == current->pid) {
+			list_del(&iter->list);
+			waiter = iter;
+			break;
+		}
+	}
+	mutex_unlock(&instance->bulk_waiter_list_mutex);
+
+	if (waiter) {
+		struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk;
+
+		if (bulk) {
+			/* This thread has an outstanding bulk transfer. */
+			/* FIXME: why compare a dma address to a pointer? */
+			if ((bulk->data != (dma_addr_t)(uintptr_t)data) || (bulk->size != size)) {
+				/*
+				 * This is not a retry of the previous one.
+				 * Cancel the signal when the transfer completes.
+				 */
+				spin_lock(&bulk_waiter_spinlock);
+				bulk->userdata = NULL;
+				spin_unlock(&bulk_waiter_spinlock);
+			}
+		}
+	} else {
+		waiter = kzalloc(sizeof(*waiter), GFP_KERNEL);
+		if (!waiter) {
+			vchiq_log_error(vchiq_core_log_level, "%s - out of memory", __func__);
+			return VCHIQ_ERROR;
+		}
+	}
+
+	status = vchiq_bulk_transfer(instance, handle, data, NULL, size,
+				     &waiter->bulk_waiter,
+				     VCHIQ_BULK_MODE_BLOCKING, dir);
+	if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) || !waiter->bulk_waiter.bulk) {
+		struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk;
+
+		if (bulk) {
+			/* Cancel the signal when the transfer completes. */
+			spin_lock(&bulk_waiter_spinlock);
+			bulk->userdata = NULL;
+			spin_unlock(&bulk_waiter_spinlock);
+		}
+		kfree(waiter);
+	} else {
+		waiter->pid = current->pid;
+		mutex_lock(&instance->bulk_waiter_list_mutex);
+		list_add(&waiter->list, &instance->bulk_waiter_list);
+		mutex_unlock(&instance->bulk_waiter_list_mutex);
+		vchiq_log_info(vchiq_arm_log_level, "saved bulk_waiter %pK for pid %d", waiter,
+			       current->pid);
+	}
+
+	return status;
+}
+
+static enum vchiq_status
+add_completion(struct vchiq_instance *instance, enum vchiq_reason reason,
+	       struct vchiq_header *header, struct user_service *user_service,
+	       void *bulk_userdata)
+{
+	struct vchiq_completion_data_kernel *completion;
+	int insert;
+
+	DEBUG_INITIALISE(g_state.local);
+
+	insert = instance->completion_insert;
+	while ((insert - instance->completion_remove) >= MAX_COMPLETIONS) {
+		/* Out of space - wait for the client */
+		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+		vchiq_log_trace(vchiq_arm_log_level, "%s - completion queue full", __func__);
+		DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
+		if (wait_for_completion_interruptible(&instance->remove_event)) {
+			vchiq_log_info(vchiq_arm_log_level, "service_callback interrupted");
+			return VCHIQ_RETRY;
+		} else if (instance->closing) {
+			vchiq_log_info(vchiq_arm_log_level, "service_callback closing");
+			return VCHIQ_SUCCESS;
+		}
+		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+	}
+
+	completion = &instance->completions[insert & (MAX_COMPLETIONS - 1)];
+
+	completion->header = header;
+	completion->reason = reason;
+	/* N.B. service_userdata is updated while processing AWAIT_COMPLETION */
+	completion->service_userdata = user_service->service;
+	completion->bulk_userdata = bulk_userdata;
+
+	if (reason == VCHIQ_SERVICE_CLOSED) {
+		/*
+		 * Take an extra reference, to be held until
+		 * this CLOSED notification is delivered.
+		 */
+		vchiq_service_get(user_service->service);
+		if (instance->use_close_delivered)
+			user_service->close_pending = 1;
+	}
+
+	/*
+	 * A write barrier is needed here to ensure that the entire completion
+	 * record is written out before the insert point.
+	 */
+	wmb();
+
+	if (reason == VCHIQ_MESSAGE_AVAILABLE)
+		user_service->message_available_pos = insert;
+
+	insert++;
+	instance->completion_insert = insert;
+
+	complete(&instance->insert_event);
+
+	return VCHIQ_SUCCESS;
+}
+
+enum vchiq_status
+service_callback(struct vchiq_instance *instance, enum vchiq_reason reason,
+		 struct vchiq_header *header, unsigned int handle, void *bulk_userdata)
+{
+	/*
+	 * How do we ensure the callback goes to the right client?
+	 * The service_user data points to a user_service record
+	 * containing the original callback and the user state structure, which
+	 * contains a circular buffer for completion records.
+	 */
+	struct user_service *user_service;
+	struct vchiq_service *service;
+	bool skip_completion = false;
+
+	DEBUG_INITIALISE(g_state.local);
+
+	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	if (WARN_ON(!service)) {
+		rcu_read_unlock();
+		return VCHIQ_SUCCESS;
+	}
+
+	user_service = (struct user_service *)service->base.userdata;
+
+	if (!instance || instance->closing) {
+		rcu_read_unlock();
+		return VCHIQ_SUCCESS;
+	}
+
+	/*
+	 * As hopping around different synchronization mechanism,
+	 * taking an extra reference results in simpler implementation.
+	 */
+	vchiq_service_get(service);
+	rcu_read_unlock();
+
+	vchiq_log_trace(vchiq_arm_log_level,
+			"%s - service %lx(%d,%p), reason %d, header %lx, instance %lx, bulk_userdata %lx",
+			__func__, (unsigned long)user_service, service->localport,
+			user_service->userdata, reason, (unsigned long)header,
+			(unsigned long)instance, (unsigned long)bulk_userdata);
+
+	if (header && user_service->is_vchi) {
+		spin_lock(&msg_queue_spinlock);
+		while (user_service->msg_insert ==
+			(user_service->msg_remove + MSG_QUEUE_SIZE)) {
+			spin_unlock(&msg_queue_spinlock);
+			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+			DEBUG_COUNT(MSG_QUEUE_FULL_COUNT);
+			vchiq_log_trace(vchiq_arm_log_level, "%s - msg queue full", __func__);
+			/*
+			 * If there is no MESSAGE_AVAILABLE in the completion
+			 * queue, add one
+			 */
+			if ((user_service->message_available_pos -
+				instance->completion_remove) < 0) {
+				enum vchiq_status status;
+
+				vchiq_log_info(vchiq_arm_log_level,
+					       "Inserting extra MESSAGE_AVAILABLE");
+				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+				status = add_completion(instance, reason, NULL, user_service,
+							bulk_userdata);
+				if (status != VCHIQ_SUCCESS) {
+					DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+					vchiq_service_put(service);
+					return status;
+				}
+			}
+
+			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+			if (wait_for_completion_interruptible(&user_service->remove_event)) {
+				vchiq_log_info(vchiq_arm_log_level, "%s interrupted", __func__);
+				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+				vchiq_service_put(service);
+				return VCHIQ_RETRY;
+			} else if (instance->closing) {
+				vchiq_log_info(vchiq_arm_log_level, "%s closing", __func__);
+				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+				vchiq_service_put(service);
+				return VCHIQ_ERROR;
+			}
+			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+			spin_lock(&msg_queue_spinlock);
+		}
+
+		user_service->msg_queue[user_service->msg_insert &
+			(MSG_QUEUE_SIZE - 1)] = header;
+		user_service->msg_insert++;
+
+		/*
+		 * If there is a thread waiting in DEQUEUE_MESSAGE, or if
+		 * there is a MESSAGE_AVAILABLE in the completion queue then
+		 * bypass the completion queue.
+		 */
+		if (((user_service->message_available_pos -
+			instance->completion_remove) >= 0) ||
+			user_service->dequeue_pending) {
+			user_service->dequeue_pending = 0;
+			skip_completion = true;
+		}
+
+		spin_unlock(&msg_queue_spinlock);
+		complete(&user_service->insert_event);
+
+		header = NULL;
+	}
+	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+	vchiq_service_put(service);
+
+	if (skip_completion)
+		return VCHIQ_SUCCESS;
+
+	return add_completion(instance, reason, header, user_service,
+		bulk_userdata);
+}
+
+int vchiq_dump(void *dump_context, const char *str, int len)
+{
+	struct dump_context *context = (struct dump_context *)dump_context;
+	int copy_bytes;
+
+	if (context->actual >= context->space)
+		return 0;
+
+	if (context->offset > 0) {
+		int skip_bytes = min_t(int, len, context->offset);
+
+		str += skip_bytes;
+		len -= skip_bytes;
+		context->offset -= skip_bytes;
+		if (context->offset > 0)
+			return 0;
+	}
+	copy_bytes = min_t(int, len, context->space - context->actual);
+	if (copy_bytes == 0)
+		return 0;
+	if (copy_to_user(context->buf + context->actual, str,
+			 copy_bytes))
+		return -EFAULT;
+	context->actual += copy_bytes;
+	len -= copy_bytes;
+
+	/*
+	 * If the terminating NUL is included in the length, then it
+	 * marks the end of a line and should be replaced with a
+	 * carriage return.
+	 */
+	if ((len == 0) && (str[copy_bytes - 1] == '\0')) {
+		char cr = '\n';
+
+		if (copy_to_user(context->buf + context->actual - 1,
+				 &cr, 1))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vchiq_dump_platform_instances(void *dump_context)
+{
+	struct vchiq_state *state = vchiq_get_state();
+	char buf[80];
+	int len;
+	int i;
+
+	if (!state)
+		return -ENOTCONN;
+
+	/*
+	 * There is no list of instances, so instead scan all services,
+	 * marking those that have been dumped.
+	 */
+
+	rcu_read_lock();
+	for (i = 0; i < state->unused_service; i++) {
+		struct vchiq_service *service;
+		struct vchiq_instance *instance;
+
+		service = rcu_dereference(state->services[i]);
+		if (!service || service->base.callback != service_callback)
+			continue;
+
+		instance = service->instance;
+		if (instance)
+			instance->mark = 0;
+	}
+	rcu_read_unlock();
+
+	for (i = 0; i < state->unused_service; i++) {
+		struct vchiq_service *service;
+		struct vchiq_instance *instance;
+		int err;
+
+		rcu_read_lock();
+		service = rcu_dereference(state->services[i]);
+		if (!service || service->base.callback != service_callback) {
+			rcu_read_unlock();
+			continue;
+		}
+
+		instance = service->instance;
+		if (!instance || instance->mark) {
+			rcu_read_unlock();
+			continue;
+		}
+		rcu_read_unlock();
+
+		len = snprintf(buf, sizeof(buf),
+			       "Instance %pK: pid %d,%s completions %d/%d",
+			       instance, instance->pid,
+			       instance->connected ? " connected, " :
+			       "",
+			       instance->completion_insert -
+			       instance->completion_remove,
+			       MAX_COMPLETIONS);
+		err = vchiq_dump(dump_context, buf, len + 1);
+		if (err)
+			return err;
+		instance->mark = 1;
+	}
+	return 0;
+}
+
+int vchiq_dump_platform_service_state(void *dump_context,
+				      struct vchiq_service *service)
+{
+	struct user_service *user_service =
+			(struct user_service *)service->base.userdata;
+	char buf[80];
+	int len;
+
+	len = scnprintf(buf, sizeof(buf), "  instance %pK", service->instance);
+
+	if ((service->base.callback == service_callback) && user_service->is_vchi) {
+		len += scnprintf(buf + len, sizeof(buf) - len, ", %d/%d messages",
+				 user_service->msg_insert - user_service->msg_remove,
+				 MSG_QUEUE_SIZE);
+
+		if (user_service->dequeue_pending)
+			len += scnprintf(buf + len, sizeof(buf) - len,
+				" (dequeue pending)");
+	}
+
+	return vchiq_dump(dump_context, buf, len + 1);
+}
+
+struct vchiq_state *
+vchiq_get_state(void)
+{
+	if (!g_state.remote) {
+		pr_err("%s: g_state.remote == NULL\n", __func__);
+		return NULL;
+	}
+
+	if (g_state.remote->initialised != 1) {
+		pr_notice("%s: g_state.remote->initialised != 1 (%d)\n",
+			  __func__, g_state.remote->initialised);
+		return NULL;
+	}
+
+	return &g_state;
+}
+
+/*
+ * Autosuspend related functionality
+ */
+
+static enum vchiq_status
+vchiq_keepalive_vchiq_callback(struct vchiq_instance *instance,
+			       enum vchiq_reason reason,
+			       struct vchiq_header *header,
+			       unsigned int service_user, void *bulk_user)
+{
+	vchiq_log_error(vchiq_susp_log_level, "%s callback reason %d", __func__, reason);
+	return 0;
+}
+
+static int
+vchiq_keepalive_thread_func(void *v)
+{
+	struct vchiq_state *state = (struct vchiq_state *)v;
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+
+	enum vchiq_status status;
+	struct vchiq_instance *instance;
+	unsigned int ka_handle;
+	int ret;
+
+	struct vchiq_service_params_kernel params = {
+		.fourcc      = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'),
+		.callback    = vchiq_keepalive_vchiq_callback,
+		.version     = KEEPALIVE_VER,
+		.version_min = KEEPALIVE_VER_MIN
+	};
+
+	ret = vchiq_initialise(&instance);
+	if (ret) {
+		vchiq_log_error(vchiq_susp_log_level, "%s vchiq_initialise failed %d", __func__,
+				ret);
+		goto exit;
+	}
+
+	status = vchiq_connect(instance);
+	if (status != VCHIQ_SUCCESS) {
+		vchiq_log_error(vchiq_susp_log_level, "%s vchiq_connect failed %d", __func__,
+				status);
+		goto shutdown;
+	}
+
+	status = vchiq_add_service(instance, &params, &ka_handle);
+	if (status != VCHIQ_SUCCESS) {
+		vchiq_log_error(vchiq_susp_log_level, "%s vchiq_open_service failed %d", __func__,
+				status);
+		goto shutdown;
+	}
+
+	while (1) {
+		long rc = 0, uc = 0;
+
+		if (wait_for_completion_interruptible(&arm_state->ka_evt)) {
+			vchiq_log_error(vchiq_susp_log_level, "%s interrupted", __func__);
+			flush_signals(current);
+			continue;
+		}
+
+		/*
+		 * read and clear counters.  Do release_count then use_count to
+		 * prevent getting more releases than uses
+		 */
+		rc = atomic_xchg(&arm_state->ka_release_count, 0);
+		uc = atomic_xchg(&arm_state->ka_use_count, 0);
+
+		/*
+		 * Call use/release service the requisite number of times.
+		 * Process use before release so use counts don't go negative
+		 */
+		while (uc--) {
+			atomic_inc(&arm_state->ka_use_ack_count);
+			status = vchiq_use_service(instance, ka_handle);
+			if (status != VCHIQ_SUCCESS) {
+				vchiq_log_error(vchiq_susp_log_level,
+						"%s vchiq_use_service error %d", __func__, status);
+			}
+		}
+		while (rc--) {
+			status = vchiq_release_service(instance, ka_handle);
+			if (status != VCHIQ_SUCCESS) {
+				vchiq_log_error(vchiq_susp_log_level,
+						"%s vchiq_release_service error %d", __func__,
+						status);
+			}
+		}
+	}
+
+shutdown:
+	vchiq_shutdown(instance);
+exit:
+	return 0;
+}
+
+int
+vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service,
+		   enum USE_TYPE_E use_type)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+	int ret = 0;
+	char entity[16];
+	int *entity_uc;
+	int local_uc;
+
+	if (!arm_state) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (use_type == USE_TYPE_VCHIQ) {
+		sprintf(entity, "VCHIQ:   ");
+		entity_uc = &arm_state->peer_use_count;
+	} else if (service) {
+		sprintf(entity, "%c%c%c%c:%03d",
+			VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+			service->client_id);
+		entity_uc = &service->service_use_count;
+	} else {
+		vchiq_log_error(vchiq_susp_log_level, "%s null service ptr", __func__);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	write_lock_bh(&arm_state->susp_res_lock);
+	local_uc = ++arm_state->videocore_use_count;
+	++(*entity_uc);
+
+	vchiq_log_trace(vchiq_susp_log_level, "%s %s count %d, state count %d", __func__, entity,
+			*entity_uc, local_uc);
+
+	write_unlock_bh(&arm_state->susp_res_lock);
+
+	if (!ret) {
+		enum vchiq_status status = VCHIQ_SUCCESS;
+		long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0);
+
+		while (ack_cnt && (status == VCHIQ_SUCCESS)) {
+			/* Send the use notify to videocore */
+			status = vchiq_send_remote_use_active(state);
+			if (status == VCHIQ_SUCCESS)
+				ack_cnt--;
+			else
+				atomic_add(ack_cnt, &arm_state->ka_use_ack_count);
+		}
+	}
+
+out:
+	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
+	return ret;
+}
+
+int
+vchiq_release_internal(struct vchiq_state *state, struct vchiq_service *service)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+	int ret = 0;
+	char entity[16];
+	int *entity_uc;
+
+	if (!arm_state) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (service) {
+		sprintf(entity, "%c%c%c%c:%03d",
+			VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+			service->client_id);
+		entity_uc = &service->service_use_count;
+	} else {
+		sprintf(entity, "PEER:   ");
+		entity_uc = &arm_state->peer_use_count;
+	}
+
+	write_lock_bh(&arm_state->susp_res_lock);
+	if (!arm_state->videocore_use_count || !(*entity_uc)) {
+		/* Don't use BUG_ON - don't allow user thread to crash kernel */
+		WARN_ON(!arm_state->videocore_use_count);
+		WARN_ON(!(*entity_uc));
+		ret = -EINVAL;
+		goto unlock;
+	}
+	--arm_state->videocore_use_count;
+	--(*entity_uc);
+
+	vchiq_log_trace(vchiq_susp_log_level, "%s %s count %d, state count %d", __func__, entity,
+			*entity_uc, arm_state->videocore_use_count);
+
+unlock:
+	write_unlock_bh(&arm_state->susp_res_lock);
+
+out:
+	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
+	return ret;
+}
+
+void
+vchiq_on_remote_use(struct vchiq_state *state)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+
+	atomic_inc(&arm_state->ka_use_count);
+	complete(&arm_state->ka_evt);
+}
+
+void
+vchiq_on_remote_release(struct vchiq_state *state)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+
+	atomic_inc(&arm_state->ka_release_count);
+	complete(&arm_state->ka_evt);
+}
+
+int
+vchiq_use_service_internal(struct vchiq_service *service)
+{
+	return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE);
+}
+
+int
+vchiq_release_service_internal(struct vchiq_service *service)
+{
+	return vchiq_release_internal(service->state, service);
+}
+
+struct vchiq_debugfs_node *
+vchiq_instance_get_debugfs_node(struct vchiq_instance *instance)
+{
+	return &instance->debugfs_node;
+}
+
+int
+vchiq_instance_get_use_count(struct vchiq_instance *instance)
+{
+	struct vchiq_service *service;
+	int use_count = 0, i;
+
+	i = 0;
+	rcu_read_lock();
+	while ((service = __next_service_by_instance(instance->state,
+						     instance, &i)))
+		use_count += service->service_use_count;
+	rcu_read_unlock();
+	return use_count;
+}
+
+int
+vchiq_instance_get_pid(struct vchiq_instance *instance)
+{
+	return instance->pid;
+}
+
+int
+vchiq_instance_get_trace(struct vchiq_instance *instance)
+{
+	return instance->trace;
+}
+
+void
+vchiq_instance_set_trace(struct vchiq_instance *instance, int trace)
+{
+	struct vchiq_service *service;
+	int i;
+
+	i = 0;
+	rcu_read_lock();
+	while ((service = __next_service_by_instance(instance->state,
+						     instance, &i)))
+		service->trace = trace;
+	rcu_read_unlock();
+	instance->trace = (trace != 0);
+}
+
+enum vchiq_status
+vchiq_use_service(struct vchiq_instance *instance, unsigned int handle)
+{
+	enum vchiq_status ret = VCHIQ_ERROR;
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+
+	if (service) {
+		ret = vchiq_use_internal(service->state, service, USE_TYPE_SERVICE);
+		vchiq_service_put(service);
+	}
+	return ret;
+}
+EXPORT_SYMBOL(vchiq_use_service);
+
+enum vchiq_status
+vchiq_release_service(struct vchiq_instance *instance, unsigned int handle)
+{
+	enum vchiq_status ret = VCHIQ_ERROR;
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+
+	if (service) {
+		ret = vchiq_release_internal(service->state, service);
+		vchiq_service_put(service);
+	}
+	return ret;
+}
+EXPORT_SYMBOL(vchiq_release_service);
+
+struct service_data_struct {
+	int fourcc;
+	int clientid;
+	int use_count;
+};
+
+void
+vchiq_dump_service_use_state(struct vchiq_state *state)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+	struct service_data_struct *service_data;
+	int i, found = 0;
+	/*
+	 * If there's more than 64 services, only dump ones with
+	 * non-zero counts
+	 */
+	int only_nonzero = 0;
+	static const char *nz = "<-- preventing suspend";
+
+	int peer_count;
+	int vc_use_count;
+	int active_services;
+
+	if (!arm_state)
+		return;
+
+	service_data = kmalloc_array(MAX_SERVICES, sizeof(*service_data),
+				     GFP_KERNEL);
+	if (!service_data)
+		return;
+
+	read_lock_bh(&arm_state->susp_res_lock);
+	peer_count = arm_state->peer_use_count;
+	vc_use_count = arm_state->videocore_use_count;
+	active_services = state->unused_service;
+	if (active_services > MAX_SERVICES)
+		only_nonzero = 1;
+
+	rcu_read_lock();
+	for (i = 0; i < active_services; i++) {
+		struct vchiq_service *service_ptr =
+			rcu_dereference(state->services[i]);
+
+		if (!service_ptr)
+			continue;
+
+		if (only_nonzero && !service_ptr->service_use_count)
+			continue;
+
+		if (service_ptr->srvstate == VCHIQ_SRVSTATE_FREE)
+			continue;
+
+		service_data[found].fourcc = service_ptr->base.fourcc;
+		service_data[found].clientid = service_ptr->client_id;
+		service_data[found].use_count = service_ptr->service_use_count;
+		found++;
+		if (found >= MAX_SERVICES)
+			break;
+	}
+	rcu_read_unlock();
+
+	read_unlock_bh(&arm_state->susp_res_lock);
+
+	if (only_nonzero)
+		vchiq_log_warning(vchiq_susp_log_level, "Too many active services (%d). Only dumping up to first %d services with non-zero use-count",
+				  active_services, found);
+
+	for (i = 0; i < found; i++) {
+		vchiq_log_warning(vchiq_susp_log_level, "----- %c%c%c%c:%d service count %d %s",
+				  VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc),
+				  service_data[i].clientid, service_data[i].use_count,
+				  service_data[i].use_count ? nz : "");
+	}
+	vchiq_log_warning(vchiq_susp_log_level, "----- VCHIQ use count %d", peer_count);
+	vchiq_log_warning(vchiq_susp_log_level, "--- Overall vchiq instance use count %d",
+			  vc_use_count);
+
+	kfree(service_data);
+}
+
+enum vchiq_status
+vchiq_check_service(struct vchiq_service *service)
+{
+	struct vchiq_arm_state *arm_state;
+	enum vchiq_status ret = VCHIQ_ERROR;
+
+	if (!service || !service->state)
+		goto out;
+
+	arm_state = vchiq_platform_get_arm_state(service->state);
+
+	read_lock_bh(&arm_state->susp_res_lock);
+	if (service->service_use_count)
+		ret = VCHIQ_SUCCESS;
+	read_unlock_bh(&arm_state->susp_res_lock);
+
+	if (ret == VCHIQ_ERROR) {
+		vchiq_log_error(vchiq_susp_log_level,
+				"%s ERROR - %c%c%c%c:%d service count %d, state count %d", __func__,
+				VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), service->client_id,
+				service->service_use_count, arm_state->videocore_use_count);
+		vchiq_dump_service_use_state(service->state);
+	}
+out:
+	return ret;
+}
+
+void vchiq_platform_conn_state_changed(struct vchiq_state *state,
+				       enum vchiq_connstate oldstate,
+				       enum vchiq_connstate newstate)
+{
+	struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
+	char threadname[16];
+
+	vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id,
+		       get_conn_state_name(oldstate), get_conn_state_name(newstate));
+	if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED)
+		return;
+
+	write_lock_bh(&arm_state->susp_res_lock);
+	if (arm_state->first_connect) {
+		write_unlock_bh(&arm_state->susp_res_lock);
+		return;
+	}
+
+	arm_state->first_connect = 1;
+	write_unlock_bh(&arm_state->susp_res_lock);
+	snprintf(threadname, sizeof(threadname), "vchiq-keep/%d",
+		 state->id);
+	arm_state->ka_thread = kthread_create(&vchiq_keepalive_thread_func,
+					      (void *)state,
+					      threadname);
+	if (IS_ERR(arm_state->ka_thread)) {
+		vchiq_log_error(vchiq_susp_log_level,
+				"vchiq: FATAL: couldn't create thread %s",
+				threadname);
+	} else {
+		wake_up_process(arm_state->ka_thread);
+	}
+}
+
+static const struct of_device_id vchiq_of_match[] = {
+	{ .compatible = "brcm,bcm2835-vchiq", .data = &bcm2835_drvdata },
+	{ .compatible = "brcm,bcm2836-vchiq", .data = &bcm2836_drvdata },
+	{},
+};
+MODULE_DEVICE_TABLE(of, vchiq_of_match);
+
+static struct platform_device *
+vchiq_register_child(struct platform_device *pdev, const char *name)
+{
+	struct platform_device_info pdevinfo;
+	struct platform_device *child;
+
+	memset(&pdevinfo, 0, sizeof(pdevinfo));
+
+	pdevinfo.parent = &pdev->dev;
+	pdevinfo.name = name;
+	pdevinfo.id = PLATFORM_DEVID_NONE;
+	pdevinfo.dma_mask = DMA_BIT_MASK(32);
+
+	child = platform_device_register_full(&pdevinfo);
+	if (IS_ERR(child)) {
+		dev_warn(&pdev->dev, "%s not registered\n", name);
+		child = NULL;
+	}
+
+	return child;
+}
+
+static int vchiq_probe(struct platform_device *pdev)
+{
+	struct device_node *fw_node;
+	const struct of_device_id *of_id;
+	struct vchiq_drvdata *drvdata;
+	int err;
+
+	of_id = of_match_node(vchiq_of_match, pdev->dev.of_node);
+	drvdata = (struct vchiq_drvdata *)of_id->data;
+	if (!drvdata)
+		return -EINVAL;
+
+	fw_node = of_find_compatible_node(NULL, NULL,
+					  "raspberrypi,bcm2835-firmware");
+	if (!fw_node) {
+		dev_err(&pdev->dev, "Missing firmware node\n");
+		return -ENOENT;
+	}
+
+	drvdata->fw = devm_rpi_firmware_get(&pdev->dev, fw_node);
+	of_node_put(fw_node);
+	if (!drvdata->fw)
+		return -EPROBE_DEFER;
+
+	platform_set_drvdata(pdev, drvdata);
+
+	err = vchiq_platform_init(pdev, &g_state);
+	if (err)
+		goto failed_platform_init;
+
+	vchiq_debugfs_init();
+
+	vchiq_log_info(vchiq_arm_log_level,
+		       "vchiq: platform initialised - version %d (min %d)",
+		       VCHIQ_VERSION, VCHIQ_VERSION_MIN);
+
+	/*
+	 * Simply exit on error since the function handles cleanup in
+	 * cases of failure.
+	 */
+	err = vchiq_register_chrdev(&pdev->dev);
+	if (err) {
+		vchiq_log_warning(vchiq_arm_log_level,
+				  "Failed to initialize vchiq cdev");
+		goto error_exit;
+	}
+
+	bcm2835_camera = vchiq_register_child(pdev, "bcm2835-camera");
+	bcm2835_audio = vchiq_register_child(pdev, "bcm2835_audio");
+
+	return 0;
+
+failed_platform_init:
+	vchiq_log_warning(vchiq_arm_log_level, "could not initialize vchiq platform");
+error_exit:
+	return err;
+}
+
+static int vchiq_remove(struct platform_device *pdev)
+{
+	platform_device_unregister(bcm2835_audio);
+	platform_device_unregister(bcm2835_camera);
+	vchiq_debugfs_deinit();
+	vchiq_deregister_chrdev();
+
+	return 0;
+}
+
+static struct platform_driver vchiq_driver = {
+	.driver = {
+		.name = "bcm2835_vchiq",
+		.of_match_table = vchiq_of_match,
+	},
+	.probe = vchiq_probe,
+	.remove = vchiq_remove,
+};
+
+static int __init vchiq_driver_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&vchiq_driver);
+	if (ret)
+		pr_err("Failed to register vchiq driver\n");
+
+	return ret;
+}
+module_init(vchiq_driver_init);
+
+static void __exit vchiq_driver_exit(void)
+{
+	platform_driver_unregister(&vchiq_driver);
+}
+module_exit(vchiq_driver_exit);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Videocore VCHIQ driver");
+MODULE_AUTHOR("Broadcom Corporation");
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h
new file mode 100644
index 000000000..cd20eb18f
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h
@@ -0,0 +1,147 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/*
+ * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ */
+
+#ifndef VCHIQ_ARM_H
+#define VCHIQ_ARM_H
+
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <linux/atomic.h>
+#include "vchiq_core.h"
+#include "vchiq_debugfs.h"
+
+/* Some per-instance constants */
+#define MAX_COMPLETIONS 128
+#define MAX_SERVICES 64
+#define MAX_ELEMENTS 8
+#define MSG_QUEUE_SIZE 128
+
+enum USE_TYPE_E {
+	USE_TYPE_SERVICE,
+	USE_TYPE_VCHIQ
+};
+
+struct user_service {
+	struct vchiq_service *service;
+	void __user *userdata;
+	struct vchiq_instance *instance;
+	char is_vchi;
+	char dequeue_pending;
+	char close_pending;
+	int message_available_pos;
+	int msg_insert;
+	int msg_remove;
+	struct completion insert_event;
+	struct completion remove_event;
+	struct completion close_event;
+	struct vchiq_header *msg_queue[MSG_QUEUE_SIZE];
+};
+
+struct bulk_waiter_node {
+	struct bulk_waiter bulk_waiter;
+	int pid;
+	struct list_head list;
+};
+
+struct vchiq_instance {
+	struct vchiq_state *state;
+	struct vchiq_completion_data_kernel completions[MAX_COMPLETIONS];
+	int completion_insert;
+	int completion_remove;
+	struct completion insert_event;
+	struct completion remove_event;
+	struct mutex completion_mutex;
+
+	int connected;
+	int closing;
+	int pid;
+	int mark;
+	int use_close_delivered;
+	int trace;
+
+	struct list_head bulk_waiter_list;
+	struct mutex bulk_waiter_list_mutex;
+
+	struct vchiq_debugfs_node debugfs_node;
+};
+
+struct dump_context {
+	char __user *buf;
+	size_t actual;
+	size_t space;
+	loff_t offset;
+};
+
+extern int vchiq_arm_log_level;
+extern int vchiq_susp_log_level;
+
+extern spinlock_t msg_queue_spinlock;
+extern struct vchiq_state g_state;
+
+extern struct vchiq_state *
+vchiq_get_state(void);
+
+enum vchiq_status
+vchiq_use_service(struct vchiq_instance *instance, unsigned int handle);
+
+extern enum vchiq_status
+vchiq_release_service(struct vchiq_instance *instance, unsigned int handle);
+
+extern enum vchiq_status
+vchiq_check_service(struct vchiq_service *service);
+
+extern void
+vchiq_dump_platform_use_state(struct vchiq_state *state);
+
+extern void
+vchiq_dump_service_use_state(struct vchiq_state *state);
+
+extern int
+vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service,
+		   enum USE_TYPE_E use_type);
+extern int
+vchiq_release_internal(struct vchiq_state *state,
+		       struct vchiq_service *service);
+
+extern struct vchiq_debugfs_node *
+vchiq_instance_get_debugfs_node(struct vchiq_instance *instance);
+
+extern int
+vchiq_instance_get_use_count(struct vchiq_instance *instance);
+
+extern int
+vchiq_instance_get_pid(struct vchiq_instance *instance);
+
+extern int
+vchiq_instance_get_trace(struct vchiq_instance *instance);
+
+extern void
+vchiq_instance_set_trace(struct vchiq_instance *instance, int trace);
+
+#if IS_ENABLED(CONFIG_VCHIQ_CDEV)
+
+extern void
+vchiq_deregister_chrdev(void);
+
+extern int
+vchiq_register_chrdev(struct device *parent);
+
+#else
+
+static inline void vchiq_deregister_chrdev(void) { }
+static inline int vchiq_register_chrdev(struct device *parent) { return 0; }
+
+#endif /* IS_ENABLED(CONFIG_VCHIQ_CDEV) */
+
+extern enum vchiq_status
+service_callback(struct vchiq_instance *vchiq_instance, enum vchiq_reason reason,
+		 struct vchiq_header *header, unsigned int handle, void *bulk_userdata);
+
+extern void
+free_bulk_waiter(struct vchiq_instance *instance);
+
+#endif /* VCHIQ_ARM_H */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h
new file mode 100644
index 000000000..a16d02999
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2014 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_CFG_H
+#define VCHIQ_CFG_H
+
+#define VCHIQ_MAGIC              VCHIQ_MAKE_FOURCC('V', 'C', 'H', 'I')
+/* The version of VCHIQ - change with any non-trivial change */
+#define VCHIQ_VERSION            8
+/*
+ * The minimum compatible version - update to match VCHIQ_VERSION with any
+ * incompatible change
+ */
+#define VCHIQ_VERSION_MIN        3
+
+/* The version that introduced the VCHIQ_IOC_LIB_VERSION ioctl */
+#define VCHIQ_VERSION_LIB_VERSION 7
+
+/* The version that introduced the VCHIQ_IOC_CLOSE_DELIVERED ioctl */
+#define VCHIQ_VERSION_CLOSE_DELIVERED 7
+
+/* The version that made it safe to use SYNCHRONOUS mode */
+#define VCHIQ_VERSION_SYNCHRONOUS_MODE 8
+
+#define VCHIQ_MAX_STATES         1
+#define VCHIQ_MAX_SERVICES       4096
+#define VCHIQ_MAX_SLOTS          128
+#define VCHIQ_MAX_SLOTS_PER_SIDE 64
+
+#define VCHIQ_NUM_CURRENT_BULKS        32
+#define VCHIQ_NUM_SERVICE_BULKS        4
+
+#ifndef VCHIQ_ENABLE_DEBUG
+#define VCHIQ_ENABLE_DEBUG             1
+#endif
+
+#ifndef VCHIQ_ENABLE_STATS
+#define VCHIQ_ENABLE_STATS             1
+#endif
+
+#endif /* VCHIQ_CFG_H */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
new file mode 100644
index 000000000..bdb0ab617
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#include "vchiq_connected.h"
+#include "vchiq_core.h"
+#include <linux/module.h>
+#include <linux/mutex.h>
+
+#define  MAX_CALLBACKS  10
+
+static   int                        g_connected;
+static   int                        g_num_deferred_callbacks;
+static   void (*g_deferred_callback[MAX_CALLBACKS])(void);
+static   int                        g_once_init;
+static   DEFINE_MUTEX(g_connected_mutex);
+
+/* Function to initialize our lock */
+static void connected_init(void)
+{
+	if (!g_once_init)
+		g_once_init = 1;
+}
+
+/*
+ * This function is used to defer initialization until the vchiq stack is
+ * initialized. If the stack is already initialized, then the callback will
+ * be made immediately, otherwise it will be deferred until
+ * vchiq_call_connected_callbacks is called.
+ */
+void vchiq_add_connected_callback(void (*callback)(void))
+{
+	connected_init();
+
+	if (mutex_lock_killable(&g_connected_mutex))
+		return;
+
+	if (g_connected) {
+		/* We're already connected. Call the callback immediately. */
+		callback();
+	} else {
+		if (g_num_deferred_callbacks >= MAX_CALLBACKS) {
+			vchiq_log_error(vchiq_core_log_level,
+					"There already %d callback registered - please increase MAX_CALLBACKS",
+					g_num_deferred_callbacks);
+		} else {
+			g_deferred_callback[g_num_deferred_callbacks] =
+				callback;
+			g_num_deferred_callbacks++;
+		}
+	}
+	mutex_unlock(&g_connected_mutex);
+}
+EXPORT_SYMBOL(vchiq_add_connected_callback);
+
+/*
+ * This function is called by the vchiq stack once it has been connected to
+ * the videocore and clients can start to use the stack.
+ */
+void vchiq_call_connected_callbacks(void)
+{
+	int i;
+
+	connected_init();
+
+	if (mutex_lock_killable(&g_connected_mutex))
+		return;
+
+	for (i = 0; i <  g_num_deferred_callbacks; i++)
+		g_deferred_callback[i]();
+
+	g_num_deferred_callbacks = 0;
+	g_connected = 1;
+	mutex_unlock(&g_connected_mutex);
+}
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h
new file mode 100644
index 000000000..4caf5e300
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h
@@ -0,0 +1,10 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_CONNECTED_H
+#define VCHIQ_CONNECTED_H
+
+void vchiq_add_connected_callback(void (*callback)(void));
+void vchiq_call_connected_callbacks(void);
+
+#endif /* VCHIQ_CONNECTED_H */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
new file mode 100644
index 000000000..45ed30bfd
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -0,0 +1,3700 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#include <linux/types.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/bitops.h>
+#include <linux/kthread.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/kref.h>
+#include <linux/rcupdate.h>
+#include <linux/sched/signal.h>
+
+#include "vchiq_arm.h"
+#include "vchiq_core.h"
+
+#define VCHIQ_SLOT_HANDLER_STACK 8192
+
+#define VCHIQ_MSG_PADDING            0  /* -                                 */
+#define VCHIQ_MSG_CONNECT            1  /* -                                 */
+#define VCHIQ_MSG_OPEN               2  /* + (srcport, -), fourcc, client_id */
+#define VCHIQ_MSG_OPENACK            3  /* + (srcport, dstport)              */
+#define VCHIQ_MSG_CLOSE              4  /* + (srcport, dstport)              */
+#define VCHIQ_MSG_DATA               5  /* + (srcport, dstport)              */
+#define VCHIQ_MSG_BULK_RX            6  /* + (srcport, dstport), data, size  */
+#define VCHIQ_MSG_BULK_TX            7  /* + (srcport, dstport), data, size  */
+#define VCHIQ_MSG_BULK_RX_DONE       8  /* + (srcport, dstport), actual      */
+#define VCHIQ_MSG_BULK_TX_DONE       9  /* + (srcport, dstport), actual      */
+#define VCHIQ_MSG_PAUSE             10  /* -                                 */
+#define VCHIQ_MSG_RESUME            11  /* -                                 */
+#define VCHIQ_MSG_REMOTE_USE        12  /* -                                 */
+#define VCHIQ_MSG_REMOTE_RELEASE    13  /* -                                 */
+#define VCHIQ_MSG_REMOTE_USE_ACTIVE 14  /* -                                 */
+
+#define TYPE_SHIFT 24
+
+#define VCHIQ_PORT_MAX                 (VCHIQ_MAX_SERVICES - 1)
+#define VCHIQ_PORT_FREE                0x1000
+#define VCHIQ_PORT_IS_VALID(port)      ((port) < VCHIQ_PORT_FREE)
+#define VCHIQ_MAKE_MSG(type, srcport, dstport) \
+	(((type) << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0))
+#define VCHIQ_MSG_TYPE(msgid)          ((unsigned int)(msgid) >> TYPE_SHIFT)
+#define VCHIQ_MSG_SRCPORT(msgid) \
+	(unsigned short)(((unsigned int)(msgid) >> 12) & 0xfff)
+#define VCHIQ_MSG_DSTPORT(msgid) \
+	((unsigned short)(msgid) & 0xfff)
+
+#define MAKE_CONNECT			(VCHIQ_MSG_CONNECT << TYPE_SHIFT)
+#define MAKE_OPEN(srcport) \
+	((VCHIQ_MSG_OPEN << TYPE_SHIFT) | ((srcport) << 12))
+#define MAKE_OPENACK(srcport, dstport) \
+	((VCHIQ_MSG_OPENACK << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0))
+#define MAKE_CLOSE(srcport, dstport) \
+	((VCHIQ_MSG_CLOSE << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0))
+#define MAKE_DATA(srcport, dstport) \
+	((VCHIQ_MSG_DATA << TYPE_SHIFT) | ((srcport) << 12) | ((dstport) << 0))
+#define MAKE_PAUSE			(VCHIQ_MSG_PAUSE << TYPE_SHIFT)
+#define MAKE_RESUME			(VCHIQ_MSG_RESUME << TYPE_SHIFT)
+#define MAKE_REMOTE_USE			(VCHIQ_MSG_REMOTE_USE << TYPE_SHIFT)
+#define MAKE_REMOTE_USE_ACTIVE		(VCHIQ_MSG_REMOTE_USE_ACTIVE << TYPE_SHIFT)
+
+/* Ensure the fields are wide enough */
+static_assert(VCHIQ_MSG_SRCPORT(VCHIQ_MAKE_MSG(0, 0, VCHIQ_PORT_MAX))
+	== 0);
+static_assert(VCHIQ_MSG_TYPE(VCHIQ_MAKE_MSG(0, VCHIQ_PORT_MAX, 0)) == 0);
+static_assert((unsigned int)VCHIQ_PORT_MAX <
+	(unsigned int)VCHIQ_PORT_FREE);
+
+#define VCHIQ_MSGID_PADDING            VCHIQ_MAKE_MSG(VCHIQ_MSG_PADDING, 0, 0)
+#define VCHIQ_MSGID_CLAIMED            0x40000000
+
+#define VCHIQ_FOURCC_INVALID           0x00000000
+#define VCHIQ_FOURCC_IS_LEGAL(fourcc)  ((fourcc) != VCHIQ_FOURCC_INVALID)
+
+#define VCHIQ_BULK_ACTUAL_ABORTED -1
+
+#if VCHIQ_ENABLE_STATS
+#define VCHIQ_STATS_INC(state, stat) (state->stats. stat++)
+#define VCHIQ_SERVICE_STATS_INC(service, stat) (service->stats. stat++)
+#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) \
+	(service->stats. stat += addend)
+#else
+#define VCHIQ_STATS_INC(state, stat) ((void)0)
+#define VCHIQ_SERVICE_STATS_INC(service, stat) ((void)0)
+#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) ((void)0)
+#endif
+
+#define HANDLE_STATE_SHIFT 12
+
+#define SLOT_INFO_FROM_INDEX(state, index) (state->slot_info + (index))
+#define SLOT_DATA_FROM_INDEX(state, index) (state->slot_data + (index))
+#define SLOT_INDEX_FROM_DATA(state, data) \
+	(((unsigned int)((char *)data - (char *)state->slot_data)) / \
+	VCHIQ_SLOT_SIZE)
+#define SLOT_INDEX_FROM_INFO(state, info) \
+	((unsigned int)(info - state->slot_info))
+#define SLOT_QUEUE_INDEX_FROM_POS(pos) \
+	((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE))
+#define SLOT_QUEUE_INDEX_FROM_POS_MASKED(pos) \
+	(SLOT_QUEUE_INDEX_FROM_POS(pos) & VCHIQ_SLOT_QUEUE_MASK)
+
+#define BULK_INDEX(x) ((x) & (VCHIQ_NUM_SERVICE_BULKS - 1))
+
+#define SRVTRACE_LEVEL(srv) \
+	(((srv) && (srv)->trace) ? VCHIQ_LOG_TRACE : vchiq_core_msg_log_level)
+#define SRVTRACE_ENABLED(srv, lev) \
+	(((srv) && (srv)->trace) || (vchiq_core_msg_log_level >= (lev)))
+
+#define NO_CLOSE_RECVD	0
+#define CLOSE_RECVD	1
+
+#define NO_RETRY_POLL	0
+#define RETRY_POLL	1
+
+struct vchiq_open_payload {
+	int fourcc;
+	int client_id;
+	short version;
+	short version_min;
+};
+
+struct vchiq_openack_payload {
+	short version;
+};
+
+enum {
+	QMFLAGS_IS_BLOCKING     = BIT(0),
+	QMFLAGS_NO_MUTEX_LOCK   = BIT(1),
+	QMFLAGS_NO_MUTEX_UNLOCK = BIT(2)
+};
+
+enum {
+	VCHIQ_POLL_TERMINATE,
+	VCHIQ_POLL_REMOVE,
+	VCHIQ_POLL_TXNOTIFY,
+	VCHIQ_POLL_RXNOTIFY,
+	VCHIQ_POLL_COUNT
+};
+
+/* we require this for consistency between endpoints */
+static_assert(sizeof(struct vchiq_header) == 8);
+static_assert(VCHIQ_VERSION >= VCHIQ_VERSION_MIN);
+
+static inline void check_sizes(void)
+{
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_SLOT_SIZE);
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SLOTS);
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SLOTS_PER_SIDE);
+	BUILD_BUG_ON_NOT_POWER_OF_2(sizeof(struct vchiq_header));
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_NUM_CURRENT_BULKS);
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_NUM_SERVICE_BULKS);
+	BUILD_BUG_ON_NOT_POWER_OF_2(VCHIQ_MAX_SERVICES);
+}
+
+/* Run time control of log level, based on KERN_XXX level. */
+int vchiq_core_log_level = VCHIQ_LOG_DEFAULT;
+int vchiq_core_msg_log_level = VCHIQ_LOG_DEFAULT;
+int vchiq_sync_log_level = VCHIQ_LOG_DEFAULT;
+
+DEFINE_SPINLOCK(bulk_waiter_spinlock);
+static DEFINE_SPINLOCK(quota_spinlock);
+
+static unsigned int handle_seq;
+
+static const char *const srvstate_names[] = {
+	"FREE",
+	"HIDDEN",
+	"LISTENING",
+	"OPENING",
+	"OPEN",
+	"OPENSYNC",
+	"CLOSESENT",
+	"CLOSERECVD",
+	"CLOSEWAIT",
+	"CLOSED"
+};
+
+static const char *const reason_names[] = {
+	"SERVICE_OPENED",
+	"SERVICE_CLOSED",
+	"MESSAGE_AVAILABLE",
+	"BULK_TRANSMIT_DONE",
+	"BULK_RECEIVE_DONE",
+	"BULK_TRANSMIT_ABORTED",
+	"BULK_RECEIVE_ABORTED"
+};
+
+static const char *const conn_state_names[] = {
+	"DISCONNECTED",
+	"CONNECTING",
+	"CONNECTED",
+	"PAUSING",
+	"PAUSE_SENT",
+	"PAUSED",
+	"RESUMING",
+	"PAUSE_TIMEOUT",
+	"RESUME_TIMEOUT"
+};
+
+static void
+release_message_sync(struct vchiq_state *state, struct vchiq_header *header);
+
+static const char *msg_type_str(unsigned int msg_type)
+{
+	switch (msg_type) {
+	case VCHIQ_MSG_PADDING:			return "PADDING";
+	case VCHIQ_MSG_CONNECT:			return "CONNECT";
+	case VCHIQ_MSG_OPEN:			return "OPEN";
+	case VCHIQ_MSG_OPENACK:			return "OPENACK";
+	case VCHIQ_MSG_CLOSE:			return "CLOSE";
+	case VCHIQ_MSG_DATA:			return "DATA";
+	case VCHIQ_MSG_BULK_RX:			return "BULK_RX";
+	case VCHIQ_MSG_BULK_TX:			return "BULK_TX";
+	case VCHIQ_MSG_BULK_RX_DONE:		return "BULK_RX_DONE";
+	case VCHIQ_MSG_BULK_TX_DONE:		return "BULK_TX_DONE";
+	case VCHIQ_MSG_PAUSE:			return "PAUSE";
+	case VCHIQ_MSG_RESUME:			return "RESUME";
+	case VCHIQ_MSG_REMOTE_USE:		return "REMOTE_USE";
+	case VCHIQ_MSG_REMOTE_RELEASE:		return "REMOTE_RELEASE";
+	case VCHIQ_MSG_REMOTE_USE_ACTIVE:	return "REMOTE_USE_ACTIVE";
+	}
+	return "???";
+}
+
+static inline void
+set_service_state(struct vchiq_service *service, int newstate)
+{
+	vchiq_log_info(vchiq_core_log_level, "%d: srv:%d %s->%s",
+		       service->state->id, service->localport,
+		       srvstate_names[service->srvstate],
+		       srvstate_names[newstate]);
+	service->srvstate = newstate;
+}
+
+struct vchiq_service *handle_to_service(struct vchiq_instance *instance, unsigned int handle)
+{
+	int idx = handle & (VCHIQ_MAX_SERVICES - 1);
+
+	return rcu_dereference(instance->state->services[idx]);
+}
+struct vchiq_service *
+find_service_by_handle(struct vchiq_instance *instance, unsigned int handle)
+{
+	struct vchiq_service *service;
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	if (service && service->srvstate != VCHIQ_SRVSTATE_FREE &&
+	    service->handle == handle &&
+	    kref_get_unless_zero(&service->ref_count)) {
+		service = rcu_pointer_handoff(service);
+		rcu_read_unlock();
+		return service;
+	}
+	rcu_read_unlock();
+	vchiq_log_info(vchiq_core_log_level,
+		       "Invalid service handle 0x%x", handle);
+	return NULL;
+}
+
+struct vchiq_service *
+find_service_by_port(struct vchiq_state *state, unsigned int localport)
+{
+	if (localport <= VCHIQ_PORT_MAX) {
+		struct vchiq_service *service;
+
+		rcu_read_lock();
+		service = rcu_dereference(state->services[localport]);
+		if (service && service->srvstate != VCHIQ_SRVSTATE_FREE &&
+		    kref_get_unless_zero(&service->ref_count)) {
+			service = rcu_pointer_handoff(service);
+			rcu_read_unlock();
+			return service;
+		}
+		rcu_read_unlock();
+	}
+	vchiq_log_info(vchiq_core_log_level,
+		       "Invalid port %u", localport);
+	return NULL;
+}
+
+struct vchiq_service *
+find_service_for_instance(struct vchiq_instance *instance, unsigned int handle)
+{
+	struct vchiq_service *service;
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	if (service && service->srvstate != VCHIQ_SRVSTATE_FREE &&
+	    service->handle == handle &&
+	    service->instance == instance &&
+	    kref_get_unless_zero(&service->ref_count)) {
+		service = rcu_pointer_handoff(service);
+		rcu_read_unlock();
+		return service;
+	}
+	rcu_read_unlock();
+	vchiq_log_info(vchiq_core_log_level,
+		       "Invalid service handle 0x%x", handle);
+	return NULL;
+}
+
+struct vchiq_service *
+find_closed_service_for_instance(struct vchiq_instance *instance, unsigned int handle)
+{
+	struct vchiq_service *service;
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	if (service &&
+	    (service->srvstate == VCHIQ_SRVSTATE_FREE ||
+	     service->srvstate == VCHIQ_SRVSTATE_CLOSED) &&
+	    service->handle == handle &&
+	    service->instance == instance &&
+	    kref_get_unless_zero(&service->ref_count)) {
+		service = rcu_pointer_handoff(service);
+		rcu_read_unlock();
+		return service;
+	}
+	rcu_read_unlock();
+	vchiq_log_info(vchiq_core_log_level,
+		       "Invalid service handle 0x%x", handle);
+	return service;
+}
+
+struct vchiq_service *
+__next_service_by_instance(struct vchiq_state *state,
+			   struct vchiq_instance *instance,
+			   int *pidx)
+{
+	struct vchiq_service *service = NULL;
+	int idx = *pidx;
+
+	while (idx < state->unused_service) {
+		struct vchiq_service *srv;
+
+		srv = rcu_dereference(state->services[idx]);
+		idx++;
+		if (srv && srv->srvstate != VCHIQ_SRVSTATE_FREE &&
+		    srv->instance == instance) {
+			service = srv;
+			break;
+		}
+	}
+
+	*pidx = idx;
+	return service;
+}
+
+struct vchiq_service *
+next_service_by_instance(struct vchiq_state *state,
+			 struct vchiq_instance *instance,
+			 int *pidx)
+{
+	struct vchiq_service *service;
+
+	rcu_read_lock();
+	while (1) {
+		service = __next_service_by_instance(state, instance, pidx);
+		if (!service)
+			break;
+		if (kref_get_unless_zero(&service->ref_count)) {
+			service = rcu_pointer_handoff(service);
+			break;
+		}
+	}
+	rcu_read_unlock();
+	return service;
+}
+
+void
+vchiq_service_get(struct vchiq_service *service)
+{
+	if (!service) {
+		WARN(1, "%s service is NULL\n", __func__);
+		return;
+	}
+	kref_get(&service->ref_count);
+}
+
+static void service_release(struct kref *kref)
+{
+	struct vchiq_service *service =
+		container_of(kref, struct vchiq_service, ref_count);
+	struct vchiq_state *state = service->state;
+
+	WARN_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
+	rcu_assign_pointer(state->services[service->localport], NULL);
+	if (service->userdata_term)
+		service->userdata_term(service->base.userdata);
+	kfree_rcu(service, rcu);
+}
+
+void
+vchiq_service_put(struct vchiq_service *service)
+{
+	if (!service) {
+		WARN(1, "%s: service is NULL\n", __func__);
+		return;
+	}
+	kref_put(&service->ref_count, service_release);
+}
+
+int
+vchiq_get_client_id(struct vchiq_instance *instance, unsigned int handle)
+{
+	struct vchiq_service *service;
+	int id;
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	id = service ? service->client_id : 0;
+	rcu_read_unlock();
+	return id;
+}
+
+void *
+vchiq_get_service_userdata(struct vchiq_instance *instance, unsigned int handle)
+{
+	void *userdata;
+	struct vchiq_service *service;
+
+	rcu_read_lock();
+	service = handle_to_service(instance, handle);
+	userdata = service ? service->base.userdata : NULL;
+	rcu_read_unlock();
+	return userdata;
+}
+EXPORT_SYMBOL(vchiq_get_service_userdata);
+
+static void
+mark_service_closing_internal(struct vchiq_service *service, int sh_thread)
+{
+	struct vchiq_state *state = service->state;
+	struct vchiq_service_quota *quota;
+
+	service->closing = 1;
+
+	/* Synchronise with other threads. */
+	mutex_lock(&state->recycle_mutex);
+	mutex_unlock(&state->recycle_mutex);
+	if (!sh_thread || (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT)) {
+		/*
+		 * If we're pausing then the slot_mutex is held until resume
+		 * by the slot handler.  Therefore don't try to acquire this
+		 * mutex if we're the slot handler and in the pause sent state.
+		 * We don't need to in this case anyway.
+		 */
+		mutex_lock(&state->slot_mutex);
+		mutex_unlock(&state->slot_mutex);
+	}
+
+	/* Unblock any sending thread. */
+	quota = &state->service_quotas[service->localport];
+	complete(&quota->quota_event);
+}
+
+static void
+mark_service_closing(struct vchiq_service *service)
+{
+	mark_service_closing_internal(service, 0);
+}
+
+static inline enum vchiq_status
+make_service_callback(struct vchiq_service *service, enum vchiq_reason reason,
+		      struct vchiq_header *header, void *bulk_userdata)
+{
+	enum vchiq_status status;
+
+	vchiq_log_trace(vchiq_core_log_level, "%d: callback:%d (%s, %pK, %pK)",
+			service->state->id, service->localport, reason_names[reason],
+			header, bulk_userdata);
+	status = service->base.callback(service->instance, reason, header, service->handle,
+					bulk_userdata);
+	if (status == VCHIQ_ERROR) {
+		vchiq_log_warning(vchiq_core_log_level,
+				  "%d: ignoring ERROR from callback to service %x",
+				  service->state->id, service->handle);
+		status = VCHIQ_SUCCESS;
+	}
+
+	if (reason != VCHIQ_MESSAGE_AVAILABLE)
+		vchiq_release_message(service->instance, service->handle, header);
+
+	return status;
+}
+
+inline void
+vchiq_set_conn_state(struct vchiq_state *state, enum vchiq_connstate newstate)
+{
+	enum vchiq_connstate oldstate = state->conn_state;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id, conn_state_names[oldstate],
+		       conn_state_names[newstate]);
+	state->conn_state = newstate;
+	vchiq_platform_conn_state_changed(state, oldstate, newstate);
+}
+
+static inline void
+remote_event_create(wait_queue_head_t *wq, struct remote_event *event)
+{
+	event->armed = 0;
+	/*
+	 * Don't clear the 'fired' flag because it may already have been set
+	 * by the other side.
+	 */
+	init_waitqueue_head(wq);
+}
+
+/*
+ * All the event waiting routines in VCHIQ used a custom semaphore
+ * implementation that filtered most signals. This achieved a behaviour similar
+ * to the "killable" family of functions. While cleaning up this code all the
+ * routines where switched to the "interruptible" family of functions, as the
+ * former was deemed unjustified and the use "killable" set all VCHIQ's
+ * threads in D state.
+ */
+static inline int
+remote_event_wait(wait_queue_head_t *wq, struct remote_event *event)
+{
+	if (!event->fired) {
+		event->armed = 1;
+		dsb(sy);
+		if (wait_event_interruptible(*wq, event->fired)) {
+			event->armed = 0;
+			return 0;
+		}
+		event->armed = 0;
+		/* Ensure that the peer sees that we are not waiting (armed == 0). */
+		wmb();
+	}
+
+	event->fired = 0;
+	return 1;
+}
+
+static inline void
+remote_event_signal_local(wait_queue_head_t *wq, struct remote_event *event)
+{
+	event->fired = 1;
+	event->armed = 0;
+	wake_up_all(wq);
+}
+
+static inline void
+remote_event_poll(wait_queue_head_t *wq, struct remote_event *event)
+{
+	if (event->fired && event->armed)
+		remote_event_signal_local(wq, event);
+}
+
+void
+remote_event_pollall(struct vchiq_state *state)
+{
+	remote_event_poll(&state->sync_trigger_event, &state->local->sync_trigger);
+	remote_event_poll(&state->sync_release_event, &state->local->sync_release);
+	remote_event_poll(&state->trigger_event, &state->local->trigger);
+	remote_event_poll(&state->recycle_event, &state->local->recycle);
+}
+
+/*
+ * Round up message sizes so that any space at the end of a slot is always big
+ * enough for a header. This relies on header size being a power of two, which
+ * has been verified earlier by a static assertion.
+ */
+
+static inline size_t
+calc_stride(size_t size)
+{
+	/* Allow room for the header */
+	size += sizeof(struct vchiq_header);
+
+	/* Round up */
+	return (size + sizeof(struct vchiq_header) - 1) &
+		~(sizeof(struct vchiq_header) - 1);
+}
+
+/* Called by the slot handler thread */
+static struct vchiq_service *
+get_listening_service(struct vchiq_state *state, int fourcc)
+{
+	int i;
+
+	WARN_ON(fourcc == VCHIQ_FOURCC_INVALID);
+
+	rcu_read_lock();
+	for (i = 0; i < state->unused_service; i++) {
+		struct vchiq_service *service;
+
+		service = rcu_dereference(state->services[i]);
+		if (service &&
+		    service->public_fourcc == fourcc &&
+		    (service->srvstate == VCHIQ_SRVSTATE_LISTENING ||
+		     (service->srvstate == VCHIQ_SRVSTATE_OPEN &&
+		      service->remoteport == VCHIQ_PORT_FREE)) &&
+		    kref_get_unless_zero(&service->ref_count)) {
+			service = rcu_pointer_handoff(service);
+			rcu_read_unlock();
+			return service;
+		}
+	}
+	rcu_read_unlock();
+	return NULL;
+}
+
+/* Called by the slot handler thread */
+static struct vchiq_service *
+get_connected_service(struct vchiq_state *state, unsigned int port)
+{
+	int i;
+
+	rcu_read_lock();
+	for (i = 0; i < state->unused_service; i++) {
+		struct vchiq_service *service =
+			rcu_dereference(state->services[i]);
+
+		if (service && service->srvstate == VCHIQ_SRVSTATE_OPEN &&
+		    service->remoteport == port &&
+		    kref_get_unless_zero(&service->ref_count)) {
+			service = rcu_pointer_handoff(service);
+			rcu_read_unlock();
+			return service;
+		}
+	}
+	rcu_read_unlock();
+	return NULL;
+}
+
+inline void
+request_poll(struct vchiq_state *state, struct vchiq_service *service,
+	     int poll_type)
+{
+	u32 value;
+	int index;
+
+	if (!service)
+		goto skip_service;
+
+	do {
+		value = atomic_read(&service->poll_flags);
+	} while (atomic_cmpxchg(&service->poll_flags, value,
+		 value | BIT(poll_type)) != value);
+
+	index = BITSET_WORD(service->localport);
+	do {
+		value = atomic_read(&state->poll_services[index]);
+	} while (atomic_cmpxchg(&state->poll_services[index],
+		 value, value | BIT(service->localport & 0x1f)) != value);
+
+skip_service:
+	state->poll_needed = 1;
+	/* Ensure the slot handler thread sees the poll_needed flag. */
+	wmb();
+
+	/* ... and ensure the slot handler runs. */
+	remote_event_signal_local(&state->trigger_event, &state->local->trigger);
+}
+
+/*
+ * Called from queue_message, by the slot handler and application threads,
+ * with slot_mutex held
+ */
+static struct vchiq_header *
+reserve_space(struct vchiq_state *state, size_t space, int is_blocking)
+{
+	struct vchiq_shared_state *local = state->local;
+	int tx_pos = state->local_tx_pos;
+	int slot_space = VCHIQ_SLOT_SIZE - (tx_pos & VCHIQ_SLOT_MASK);
+
+	if (space > slot_space) {
+		struct vchiq_header *header;
+		/* Fill the remaining space with padding */
+		WARN_ON(!state->tx_data);
+		header = (struct vchiq_header *)
+			(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
+		header->msgid = VCHIQ_MSGID_PADDING;
+		header->size = slot_space - sizeof(struct vchiq_header);
+
+		tx_pos += slot_space;
+	}
+
+	/* If necessary, get the next slot. */
+	if ((tx_pos & VCHIQ_SLOT_MASK) == 0) {
+		int slot_index;
+
+		/* If there is no free slot... */
+
+		if (!try_wait_for_completion(&state->slot_available_event)) {
+			/* ...wait for one. */
+
+			VCHIQ_STATS_INC(state, slot_stalls);
+
+			/* But first, flush through the last slot. */
+			state->local_tx_pos = tx_pos;
+			local->tx_pos = tx_pos;
+			remote_event_signal(&state->remote->trigger);
+
+			if (!is_blocking ||
+			    (wait_for_completion_interruptible(&state->slot_available_event)))
+				return NULL; /* No space available */
+		}
+
+		if (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE)) {
+			complete(&state->slot_available_event);
+			pr_warn("%s: invalid tx_pos: %d\n", __func__, tx_pos);
+			return NULL;
+		}
+
+		slot_index = local->slot_queue[SLOT_QUEUE_INDEX_FROM_POS_MASKED(tx_pos)];
+		state->tx_data =
+			(char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+	}
+
+	state->local_tx_pos = tx_pos + space;
+
+	return (struct vchiq_header *)(state->tx_data +
+						(tx_pos & VCHIQ_SLOT_MASK));
+}
+
+static void
+process_free_data_message(struct vchiq_state *state, u32 *service_found,
+			  struct vchiq_header *header)
+{
+	int msgid = header->msgid;
+	int port = VCHIQ_MSG_SRCPORT(msgid);
+	struct vchiq_service_quota *quota = &state->service_quotas[port];
+	int count;
+
+	spin_lock(&quota_spinlock);
+	count = quota->message_use_count;
+	if (count > 0)
+		quota->message_use_count = count - 1;
+	spin_unlock(&quota_spinlock);
+
+	if (count == quota->message_quota) {
+		/*
+		 * Signal the service that it
+		 * has dropped below its quota
+		 */
+		complete(&quota->quota_event);
+	} else if (count == 0) {
+		vchiq_log_error(vchiq_core_log_level,
+				"service %d message_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)",
+				port, quota->message_use_count, header, msgid, header->msgid,
+				header->size);
+		WARN(1, "invalid message use count\n");
+	}
+	if (!BITSET_IS_SET(service_found, port)) {
+		/* Set the found bit for this service */
+		BITSET_SET(service_found, port);
+
+		spin_lock(&quota_spinlock);
+		count = quota->slot_use_count;
+		if (count > 0)
+			quota->slot_use_count = count - 1;
+		spin_unlock(&quota_spinlock);
+
+		if (count > 0) {
+			/*
+			 * Signal the service in case
+			 * it has dropped below its quota
+			 */
+			complete(&quota->quota_event);
+			vchiq_log_trace(vchiq_core_log_level, "%d: pfq:%d %x@%pK - slot_use->%d",
+					state->id, port, header->size, header, count - 1);
+		} else {
+			vchiq_log_error(vchiq_core_log_level,
+					"service %d slot_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)",
+					port, count, header, msgid, header->msgid, header->size);
+			WARN(1, "bad slot use count\n");
+		}
+	}
+}
+
+/* Called by the recycle thread. */
+static void
+process_free_queue(struct vchiq_state *state, u32 *service_found,
+		   size_t length)
+{
+	struct vchiq_shared_state *local = state->local;
+	int slot_queue_available;
+
+	/*
+	 * Find slots which have been freed by the other side, and return them
+	 * to the available queue.
+	 */
+	slot_queue_available = state->slot_queue_available;
+
+	/*
+	 * Use a memory barrier to ensure that any state that may have been
+	 * modified by another thread is not masked by stale prefetched
+	 * values.
+	 */
+	mb();
+
+	while (slot_queue_available != local->slot_queue_recycle) {
+		unsigned int pos;
+		int slot_index = local->slot_queue[slot_queue_available &
+			VCHIQ_SLOT_QUEUE_MASK];
+		char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+		int data_found = 0;
+
+		slot_queue_available++;
+		/*
+		 * Beware of the address dependency - data is calculated
+		 * using an index written by the other side.
+		 */
+		rmb();
+
+		vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%pK %x %x",
+				state->id, slot_index, data, local->slot_queue_recycle,
+				slot_queue_available);
+
+		/* Initialise the bitmask for services which have used this slot */
+		memset(service_found, 0, length);
+
+		pos = 0;
+
+		while (pos < VCHIQ_SLOT_SIZE) {
+			struct vchiq_header *header =
+				(struct vchiq_header *)(data + pos);
+			int msgid = header->msgid;
+
+			if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
+				process_free_data_message(state, service_found,
+							  header);
+				data_found = 1;
+			}
+
+			pos += calc_stride(header->size);
+			if (pos > VCHIQ_SLOT_SIZE) {
+				vchiq_log_error(vchiq_core_log_level,
+						"pfq - pos %x: header %pK, msgid %x, header->msgid %x, header->size %x",
+						pos, header, msgid, header->msgid, header->size);
+				WARN(1, "invalid slot position\n");
+			}
+		}
+
+		if (data_found) {
+			int count;
+
+			spin_lock(&quota_spinlock);
+			count = state->data_use_count;
+			if (count > 0)
+				state->data_use_count = count - 1;
+			spin_unlock(&quota_spinlock);
+			if (count == state->data_quota)
+				complete(&state->data_quota_event);
+		}
+
+		/*
+		 * Don't allow the slot to be reused until we are no
+		 * longer interested in it.
+		 */
+		mb();
+
+		state->slot_queue_available = slot_queue_available;
+		complete(&state->slot_available_event);
+	}
+}
+
+static ssize_t
+memcpy_copy_callback(void *context, void *dest, size_t offset, size_t maxsize)
+{
+	memcpy(dest + offset, context + offset, maxsize);
+	return maxsize;
+}
+
+static ssize_t
+copy_message_data(ssize_t (*copy_callback)(void *context, void *dest, size_t offset,
+					   size_t maxsize),
+	void *context,
+	void *dest,
+	size_t size)
+{
+	size_t pos = 0;
+
+	while (pos < size) {
+		ssize_t callback_result;
+		size_t max_bytes = size - pos;
+
+		callback_result = copy_callback(context, dest + pos, pos,
+						max_bytes);
+
+		if (callback_result < 0)
+			return callback_result;
+
+		if (!callback_result)
+			return -EIO;
+
+		if (callback_result > max_bytes)
+			return -EIO;
+
+		pos += callback_result;
+	}
+
+	return size;
+}
+
+/* Called by the slot handler and application threads */
+static enum vchiq_status
+queue_message(struct vchiq_state *state, struct vchiq_service *service,
+	      int msgid,
+	      ssize_t (*copy_callback)(void *context, void *dest,
+				       size_t offset, size_t maxsize),
+	      void *context, size_t size, int flags)
+{
+	struct vchiq_shared_state *local;
+	struct vchiq_service_quota *quota = NULL;
+	struct vchiq_header *header;
+	int type = VCHIQ_MSG_TYPE(msgid);
+
+	size_t stride;
+
+	local = state->local;
+
+	stride = calc_stride(size);
+
+	WARN_ON(stride > VCHIQ_SLOT_SIZE);
+
+	if (!(flags & QMFLAGS_NO_MUTEX_LOCK) &&
+	    mutex_lock_killable(&state->slot_mutex))
+		return VCHIQ_RETRY;
+
+	if (type == VCHIQ_MSG_DATA) {
+		int tx_end_index;
+
+		if (!service) {
+			WARN(1, "%s: service is NULL\n", __func__);
+			mutex_unlock(&state->slot_mutex);
+			return VCHIQ_ERROR;
+		}
+
+		WARN_ON(flags & (QMFLAGS_NO_MUTEX_LOCK |
+				 QMFLAGS_NO_MUTEX_UNLOCK));
+
+		if (service->closing) {
+			/* The service has been closed */
+			mutex_unlock(&state->slot_mutex);
+			return VCHIQ_ERROR;
+		}
+
+		quota = &state->service_quotas[service->localport];
+
+		spin_lock(&quota_spinlock);
+
+		/*
+		 * Ensure this service doesn't use more than its quota of
+		 * messages or slots
+		 */
+		tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1);
+
+		/*
+		 * Ensure data messages don't use more than their quota of
+		 * slots
+		 */
+		while ((tx_end_index != state->previous_data_index) &&
+		       (state->data_use_count == state->data_quota)) {
+			VCHIQ_STATS_INC(state, data_stalls);
+			spin_unlock(&quota_spinlock);
+			mutex_unlock(&state->slot_mutex);
+
+			if (wait_for_completion_interruptible(&state->data_quota_event))
+				return VCHIQ_RETRY;
+
+			mutex_lock(&state->slot_mutex);
+			spin_lock(&quota_spinlock);
+			tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1);
+			if ((tx_end_index == state->previous_data_index) ||
+			    (state->data_use_count < state->data_quota)) {
+				/* Pass the signal on to other waiters */
+				complete(&state->data_quota_event);
+				break;
+			}
+		}
+
+		while ((quota->message_use_count == quota->message_quota) ||
+		       ((tx_end_index != quota->previous_tx_index) &&
+			(quota->slot_use_count == quota->slot_quota))) {
+			spin_unlock(&quota_spinlock);
+			vchiq_log_trace(vchiq_core_log_level,
+					"%d: qm:%d %s,%zx - quota stall (msg %d, slot %d)",
+					state->id, service->localport, msg_type_str(type), size,
+					quota->message_use_count, quota->slot_use_count);
+			VCHIQ_SERVICE_STATS_INC(service, quota_stalls);
+			mutex_unlock(&state->slot_mutex);
+			if (wait_for_completion_interruptible(&quota->quota_event))
+				return VCHIQ_RETRY;
+			if (service->closing)
+				return VCHIQ_ERROR;
+			if (mutex_lock_killable(&state->slot_mutex))
+				return VCHIQ_RETRY;
+			if (service->srvstate != VCHIQ_SRVSTATE_OPEN) {
+				/* The service has been closed */
+				mutex_unlock(&state->slot_mutex);
+				return VCHIQ_ERROR;
+			}
+			spin_lock(&quota_spinlock);
+			tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1);
+		}
+
+		spin_unlock(&quota_spinlock);
+	}
+
+	header = reserve_space(state, stride, flags & QMFLAGS_IS_BLOCKING);
+
+	if (!header) {
+		if (service)
+			VCHIQ_SERVICE_STATS_INC(service, slot_stalls);
+		/*
+		 * In the event of a failure, return the mutex to the
+		 * state it was in
+		 */
+		if (!(flags & QMFLAGS_NO_MUTEX_LOCK))
+			mutex_unlock(&state->slot_mutex);
+		return VCHIQ_RETRY;
+	}
+
+	if (type == VCHIQ_MSG_DATA) {
+		ssize_t callback_result;
+		int tx_end_index;
+		int slot_use_count;
+
+		vchiq_log_info(vchiq_core_log_level, "%d: qm %s@%pK,%zx (%d->%d)", state->id,
+			       msg_type_str(VCHIQ_MSG_TYPE(msgid)), header, size,
+			       VCHIQ_MSG_SRCPORT(msgid), VCHIQ_MSG_DSTPORT(msgid));
+
+		WARN_ON(flags & (QMFLAGS_NO_MUTEX_LOCK |
+				 QMFLAGS_NO_MUTEX_UNLOCK));
+
+		callback_result =
+			copy_message_data(copy_callback, context,
+					  header->data, size);
+
+		if (callback_result < 0) {
+			mutex_unlock(&state->slot_mutex);
+			VCHIQ_SERVICE_STATS_INC(service, error_count);
+			return VCHIQ_ERROR;
+		}
+
+		if (SRVTRACE_ENABLED(service,
+				     VCHIQ_LOG_INFO))
+			vchiq_log_dump_mem("Sent", 0,
+					   header->data,
+					   min_t(size_t, 16, callback_result));
+
+		spin_lock(&quota_spinlock);
+		quota->message_use_count++;
+
+		tx_end_index =
+			SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1);
+
+		/*
+		 * If this transmission can't fit in the last slot used by any
+		 * service, the data_use_count must be increased.
+		 */
+		if (tx_end_index != state->previous_data_index) {
+			state->previous_data_index = tx_end_index;
+			state->data_use_count++;
+		}
+
+		/*
+		 * If this isn't the same slot last used by this service,
+		 * the service's slot_use_count must be increased.
+		 */
+		if (tx_end_index != quota->previous_tx_index) {
+			quota->previous_tx_index = tx_end_index;
+			slot_use_count = ++quota->slot_use_count;
+		} else {
+			slot_use_count = 0;
+		}
+
+		spin_unlock(&quota_spinlock);
+
+		if (slot_use_count)
+			vchiq_log_trace(vchiq_core_log_level,
+					"%d: qm:%d %s,%zx - slot_use->%d (hdr %p)", state->id,
+					service->localport, msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+					size, slot_use_count, header);
+
+		VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
+		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
+	} else {
+		vchiq_log_info(vchiq_core_log_level, "%d: qm %s@%pK,%zx (%d->%d)", state->id,
+			       msg_type_str(VCHIQ_MSG_TYPE(msgid)), header, size,
+			       VCHIQ_MSG_SRCPORT(msgid), VCHIQ_MSG_DSTPORT(msgid));
+		if (size != 0) {
+			/*
+			 * It is assumed for now that this code path
+			 * only happens from calls inside this file.
+			 *
+			 * External callers are through the vchiq_queue_message
+			 * path which always sets the type to be VCHIQ_MSG_DATA
+			 *
+			 * At first glance this appears to be correct but
+			 * more review is needed.
+			 */
+			copy_message_data(copy_callback, context,
+					  header->data, size);
+		}
+		VCHIQ_STATS_INC(state, ctrl_tx_count);
+	}
+
+	header->msgid = msgid;
+	header->size = size;
+
+	{
+		int svc_fourcc;
+
+		svc_fourcc = service
+			? service->base.fourcc
+			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+
+		vchiq_log_info(SRVTRACE_LEVEL(service),
+			       "Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%zu",
+			       msg_type_str(VCHIQ_MSG_TYPE(msgid)), VCHIQ_MSG_TYPE(msgid),
+			       VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), VCHIQ_MSG_SRCPORT(msgid),
+			       VCHIQ_MSG_DSTPORT(msgid), size);
+	}
+
+	/* Make sure the new header is visible to the peer. */
+	wmb();
+
+	/* Make the new tx_pos visible to the peer. */
+	local->tx_pos = state->local_tx_pos;
+	wmb();
+
+	if (service && (type == VCHIQ_MSG_CLOSE))
+		set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
+
+	if (!(flags & QMFLAGS_NO_MUTEX_UNLOCK))
+		mutex_unlock(&state->slot_mutex);
+
+	remote_event_signal(&state->remote->trigger);
+
+	return VCHIQ_SUCCESS;
+}
+
+/* Called by the slot handler and application threads */
+static enum vchiq_status
+queue_message_sync(struct vchiq_state *state, struct vchiq_service *service,
+		   int msgid,
+		   ssize_t (*copy_callback)(void *context, void *dest,
+					    size_t offset, size_t maxsize),
+		   void *context, int size, int is_blocking)
+{
+	struct vchiq_shared_state *local;
+	struct vchiq_header *header;
+	ssize_t callback_result;
+
+	local = state->local;
+
+	if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME &&
+	    mutex_lock_killable(&state->sync_mutex))
+		return VCHIQ_RETRY;
+
+	remote_event_wait(&state->sync_release_event, &local->sync_release);
+
+	/* Ensure that reads don't overtake the remote_event_wait. */
+	rmb();
+
+	header = (struct vchiq_header *)SLOT_DATA_FROM_INDEX(state,
+		local->slot_sync);
+
+	{
+		int oldmsgid = header->msgid;
+
+		if (oldmsgid != VCHIQ_MSGID_PADDING)
+			vchiq_log_error(vchiq_core_log_level, "%d: qms - msgid %x, not PADDING",
+					state->id, oldmsgid);
+	}
+
+	vchiq_log_info(vchiq_sync_log_level,
+		       "%d: qms %s@%pK,%x (%d->%d)", state->id,
+		       msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+		       header, size, VCHIQ_MSG_SRCPORT(msgid),
+		       VCHIQ_MSG_DSTPORT(msgid));
+
+	callback_result =
+		copy_message_data(copy_callback, context,
+				  header->data, size);
+
+	if (callback_result < 0) {
+		mutex_unlock(&state->slot_mutex);
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		return VCHIQ_ERROR;
+	}
+
+	if (service) {
+		if (SRVTRACE_ENABLED(service,
+				     VCHIQ_LOG_INFO))
+			vchiq_log_dump_mem("Sent", 0,
+					   header->data,
+					   min_t(size_t, 16, callback_result));
+
+		VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
+		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
+	} else {
+		VCHIQ_STATS_INC(state, ctrl_tx_count);
+	}
+
+	header->size = size;
+	header->msgid = msgid;
+
+	if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
+		int svc_fourcc;
+
+		svc_fourcc = service
+			? service->base.fourcc
+			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+
+		vchiq_log_trace(vchiq_sync_log_level,
+				"Sent Sync Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
+				msg_type_str(VCHIQ_MSG_TYPE(msgid)), VCHIQ_MSG_TYPE(msgid),
+				VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), VCHIQ_MSG_SRCPORT(msgid),
+				VCHIQ_MSG_DSTPORT(msgid), size);
+	}
+
+	remote_event_signal(&state->remote->sync_trigger);
+
+	if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE)
+		mutex_unlock(&state->sync_mutex);
+
+	return VCHIQ_SUCCESS;
+}
+
+static inline void
+claim_slot(struct vchiq_slot_info *slot)
+{
+	slot->use_count++;
+}
+
+static void
+release_slot(struct vchiq_state *state, struct vchiq_slot_info *slot_info,
+	     struct vchiq_header *header, struct vchiq_service *service)
+{
+	mutex_lock(&state->recycle_mutex);
+
+	if (header) {
+		int msgid = header->msgid;
+
+		if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) || (service && service->closing)) {
+			mutex_unlock(&state->recycle_mutex);
+			return;
+		}
+
+		/* Rewrite the message header to prevent a double release */
+		header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED;
+	}
+
+	slot_info->release_count++;
+
+	if (slot_info->release_count == slot_info->use_count) {
+		int slot_queue_recycle;
+		/* Add to the freed queue */
+
+		/*
+		 * A read barrier is necessary here to prevent speculative
+		 * fetches of remote->slot_queue_recycle from overtaking the
+		 * mutex.
+		 */
+		rmb();
+
+		slot_queue_recycle = state->remote->slot_queue_recycle;
+		state->remote->slot_queue[slot_queue_recycle &
+			VCHIQ_SLOT_QUEUE_MASK] =
+			SLOT_INDEX_FROM_INFO(state, slot_info);
+		state->remote->slot_queue_recycle = slot_queue_recycle + 1;
+		vchiq_log_info(vchiq_core_log_level, "%d: %s %d - recycle->%x", state->id, __func__,
+			       SLOT_INDEX_FROM_INFO(state, slot_info),
+			       state->remote->slot_queue_recycle);
+
+		/*
+		 * A write barrier is necessary, but remote_event_signal
+		 * contains one.
+		 */
+		remote_event_signal(&state->remote->recycle);
+	}
+
+	mutex_unlock(&state->recycle_mutex);
+}
+
+static inline enum vchiq_reason
+get_bulk_reason(struct vchiq_bulk *bulk)
+{
+	if (bulk->dir == VCHIQ_BULK_TRANSMIT) {
+		if (bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED)
+			return VCHIQ_BULK_TRANSMIT_ABORTED;
+
+		return VCHIQ_BULK_TRANSMIT_DONE;
+	}
+
+	if (bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED)
+		return VCHIQ_BULK_RECEIVE_ABORTED;
+
+	return VCHIQ_BULK_RECEIVE_DONE;
+}
+
+/* Called by the slot handler - don't hold the bulk mutex */
+static enum vchiq_status
+notify_bulks(struct vchiq_service *service, struct vchiq_bulk_queue *queue,
+	     int retry_poll)
+{
+	enum vchiq_status status = VCHIQ_SUCCESS;
+
+	vchiq_log_trace(vchiq_core_log_level, "%d: nb:%d %cx - p=%x rn=%x r=%x", service->state->id,
+			service->localport, (queue == &service->bulk_tx) ? 't' : 'r',
+			queue->process, queue->remote_notify, queue->remove);
+
+	queue->remote_notify = queue->process;
+
+	while (queue->remove != queue->remote_notify) {
+		struct vchiq_bulk *bulk =
+			&queue->bulks[BULK_INDEX(queue->remove)];
+
+		/*
+		 * Only generate callbacks for non-dummy bulk
+		 * requests, and non-terminated services
+		 */
+		if (bulk->data && service->instance) {
+			if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED) {
+				if (bulk->dir == VCHIQ_BULK_TRANSMIT) {
+					VCHIQ_SERVICE_STATS_INC(service, bulk_tx_count);
+					VCHIQ_SERVICE_STATS_ADD(service, bulk_tx_bytes,
+								bulk->actual);
+				} else {
+					VCHIQ_SERVICE_STATS_INC(service, bulk_rx_count);
+					VCHIQ_SERVICE_STATS_ADD(service, bulk_rx_bytes,
+								bulk->actual);
+				}
+			} else {
+				VCHIQ_SERVICE_STATS_INC(service, bulk_aborted_count);
+			}
+			if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING) {
+				struct bulk_waiter *waiter;
+
+				spin_lock(&bulk_waiter_spinlock);
+				waiter = bulk->userdata;
+				if (waiter) {
+					waiter->actual = bulk->actual;
+					complete(&waiter->event);
+				}
+				spin_unlock(&bulk_waiter_spinlock);
+			} else if (bulk->mode == VCHIQ_BULK_MODE_CALLBACK) {
+				enum vchiq_reason reason =
+						get_bulk_reason(bulk);
+				status = make_service_callback(service, reason,	NULL,
+							       bulk->userdata);
+				if (status == VCHIQ_RETRY)
+					break;
+			}
+		}
+
+		queue->remove++;
+		complete(&service->bulk_remove_event);
+	}
+	if (!retry_poll)
+		status = VCHIQ_SUCCESS;
+
+	if (status == VCHIQ_RETRY)
+		request_poll(service->state, service, (queue == &service->bulk_tx) ?
+			     VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
+
+	return status;
+}
+
+static void
+poll_services_of_group(struct vchiq_state *state, int group)
+{
+	u32 flags = atomic_xchg(&state->poll_services[group], 0);
+	int i;
+
+	for (i = 0; flags; i++) {
+		struct vchiq_service *service;
+		u32 service_flags;
+
+		if ((flags & BIT(i)) == 0)
+			continue;
+
+		service = find_service_by_port(state, (group << 5) + i);
+		flags &= ~BIT(i);
+
+		if (!service)
+			continue;
+
+		service_flags = atomic_xchg(&service->poll_flags, 0);
+		if (service_flags & BIT(VCHIQ_POLL_REMOVE)) {
+			vchiq_log_info(vchiq_core_log_level, "%d: ps - remove %d<->%d",
+				       state->id, service->localport,
+				       service->remoteport);
+
+			/*
+			 * Make it look like a client, because
+			 * it must be removed and not left in
+			 * the LISTENING state.
+			 */
+			service->public_fourcc = VCHIQ_FOURCC_INVALID;
+
+			if (vchiq_close_service_internal(service, NO_CLOSE_RECVD) !=
+							 VCHIQ_SUCCESS)
+				request_poll(state, service, VCHIQ_POLL_REMOVE);
+		} else if (service_flags & BIT(VCHIQ_POLL_TERMINATE)) {
+			vchiq_log_info(vchiq_core_log_level, "%d: ps - terminate %d<->%d",
+				       state->id, service->localport, service->remoteport);
+			if (vchiq_close_service_internal(service, NO_CLOSE_RECVD) != VCHIQ_SUCCESS)
+				request_poll(state, service, VCHIQ_POLL_TERMINATE);
+		}
+		if (service_flags & BIT(VCHIQ_POLL_TXNOTIFY))
+			notify_bulks(service, &service->bulk_tx, RETRY_POLL);
+		if (service_flags & BIT(VCHIQ_POLL_RXNOTIFY))
+			notify_bulks(service, &service->bulk_rx, RETRY_POLL);
+		vchiq_service_put(service);
+	}
+}
+
+/* Called by the slot handler thread */
+static void
+poll_services(struct vchiq_state *state)
+{
+	int group;
+
+	for (group = 0; group < BITSET_SIZE(state->unused_service); group++)
+		poll_services_of_group(state, group);
+}
+
+/* Called with the bulk_mutex held */
+static void
+abort_outstanding_bulks(struct vchiq_service *service,
+			struct vchiq_bulk_queue *queue)
+{
+	int is_tx = (queue == &service->bulk_tx);
+
+	vchiq_log_trace(vchiq_core_log_level, "%d: aob:%d %cx - li=%x ri=%x p=%x",
+			service->state->id, service->localport, is_tx ? 't' : 'r',
+			queue->local_insert, queue->remote_insert, queue->process);
+
+	WARN_ON((int)(queue->local_insert - queue->process) < 0);
+	WARN_ON((int)(queue->remote_insert - queue->process) < 0);
+
+	while ((queue->process != queue->local_insert) ||
+	       (queue->process != queue->remote_insert)) {
+		struct vchiq_bulk *bulk = &queue->bulks[BULK_INDEX(queue->process)];
+
+		if (queue->process == queue->remote_insert) {
+			/* fabricate a matching dummy bulk */
+			bulk->remote_data = NULL;
+			bulk->remote_size = 0;
+			queue->remote_insert++;
+		}
+
+		if (queue->process != queue->local_insert) {
+			vchiq_complete_bulk(service->instance, bulk);
+
+			vchiq_log_info(SRVTRACE_LEVEL(service),
+				       "%s %c%c%c%c d:%d ABORTED - tx len:%d, rx len:%d",
+				       is_tx ? "Send Bulk to" : "Recv Bulk from",
+				       VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+				       service->remoteport, bulk->size, bulk->remote_size);
+		} else {
+			/* fabricate a matching dummy bulk */
+			bulk->data = 0;
+			bulk->size = 0;
+			bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+			bulk->dir = is_tx ? VCHIQ_BULK_TRANSMIT :
+				VCHIQ_BULK_RECEIVE;
+			queue->local_insert++;
+		}
+
+		queue->process++;
+	}
+}
+
+static int
+parse_open(struct vchiq_state *state, struct vchiq_header *header)
+{
+	const struct vchiq_open_payload *payload;
+	struct vchiq_service *service = NULL;
+	int msgid, size;
+	unsigned int localport, remoteport, fourcc;
+	short version, version_min;
+
+	msgid = header->msgid;
+	size = header->size;
+	localport = VCHIQ_MSG_DSTPORT(msgid);
+	remoteport = VCHIQ_MSG_SRCPORT(msgid);
+	if (size < sizeof(struct vchiq_open_payload))
+		goto fail_open;
+
+	payload = (struct vchiq_open_payload *)header->data;
+	fourcc = payload->fourcc;
+	vchiq_log_info(vchiq_core_log_level, "%d: prs OPEN@%pK (%d->'%c%c%c%c')",
+		       state->id, header, localport, VCHIQ_FOURCC_AS_4CHARS(fourcc));
+
+	service = get_listening_service(state, fourcc);
+	if (!service)
+		goto fail_open;
+
+	/* A matching service exists */
+	version = payload->version;
+	version_min = payload->version_min;
+
+	if ((service->version < version_min) || (version < service->version_min)) {
+		/* Version mismatch */
+		vchiq_loud_error_header();
+		vchiq_loud_error("%d: service %d (%c%c%c%c) version mismatch - local (%d, min %d) vs. remote (%d, min %d)",
+				 state->id, service->localport, VCHIQ_FOURCC_AS_4CHARS(fourcc),
+				 service->version, service->version_min, version, version_min);
+		vchiq_loud_error_footer();
+		vchiq_service_put(service);
+		service = NULL;
+		goto fail_open;
+	}
+	service->peer_version = version;
+
+	if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
+		struct vchiq_openack_payload ack_payload = {
+			service->version
+		};
+		int openack_id = MAKE_OPENACK(service->localport, remoteport);
+
+		if (state->version_common <
+		    VCHIQ_VERSION_SYNCHRONOUS_MODE)
+			service->sync = 0;
+
+		/* Acknowledge the OPEN */
+		if (service->sync) {
+			if (queue_message_sync(state, NULL, openack_id, memcpy_copy_callback,
+					       &ack_payload, sizeof(ack_payload), 0) == VCHIQ_RETRY)
+				goto bail_not_ready;
+
+			/* The service is now open */
+			set_service_state(service, VCHIQ_SRVSTATE_OPENSYNC);
+		} else {
+			if (queue_message(state, NULL, openack_id, memcpy_copy_callback,
+					  &ack_payload, sizeof(ack_payload), 0) == VCHIQ_RETRY)
+				goto bail_not_ready;
+
+			/* The service is now open */
+			set_service_state(service, VCHIQ_SRVSTATE_OPEN);
+		}
+	}
+
+	/* Success - the message has been dealt with */
+	vchiq_service_put(service);
+	return 1;
+
+fail_open:
+	/* No available service, or an invalid request - send a CLOSE */
+	if (queue_message(state, NULL, MAKE_CLOSE(0, VCHIQ_MSG_SRCPORT(msgid)),
+			  NULL, NULL, 0, 0) == VCHIQ_RETRY)
+		goto bail_not_ready;
+
+	return 1;
+
+bail_not_ready:
+	if (service)
+		vchiq_service_put(service);
+
+	return 0;
+}
+
+/**
+ * parse_message() - parses a single message from the rx slot
+ * @state:  vchiq state struct
+ * @header: message header
+ *
+ * Context: Process context
+ *
+ * Return:
+ * * >= 0     - size of the parsed message payload (without header)
+ * * -EINVAL  - fatal error occurred, bail out is required
+ */
+static int
+parse_message(struct vchiq_state *state, struct vchiq_header *header)
+{
+	struct vchiq_service *service = NULL;
+	unsigned int localport, remoteport;
+	int msgid, size, type, ret = -EINVAL;
+
+	DEBUG_INITIALISE(state->local);
+
+	DEBUG_VALUE(PARSE_HEADER, (int)(long)header);
+	msgid = header->msgid;
+	DEBUG_VALUE(PARSE_MSGID, msgid);
+	size = header->size;
+	type = VCHIQ_MSG_TYPE(msgid);
+	localport = VCHIQ_MSG_DSTPORT(msgid);
+	remoteport = VCHIQ_MSG_SRCPORT(msgid);
+
+	if (type != VCHIQ_MSG_DATA)
+		VCHIQ_STATS_INC(state, ctrl_rx_count);
+
+	switch (type) {
+	case VCHIQ_MSG_OPENACK:
+	case VCHIQ_MSG_CLOSE:
+	case VCHIQ_MSG_DATA:
+	case VCHIQ_MSG_BULK_RX:
+	case VCHIQ_MSG_BULK_TX:
+	case VCHIQ_MSG_BULK_RX_DONE:
+	case VCHIQ_MSG_BULK_TX_DONE:
+		service = find_service_by_port(state, localport);
+		if ((!service ||
+		     ((service->remoteport != remoteport) &&
+		      (service->remoteport != VCHIQ_PORT_FREE))) &&
+		    (localport == 0) &&
+		    (type == VCHIQ_MSG_CLOSE)) {
+			/*
+			 * This could be a CLOSE from a client which
+			 * hadn't yet received the OPENACK - look for
+			 * the connected service
+			 */
+			if (service)
+				vchiq_service_put(service);
+			service = get_connected_service(state, remoteport);
+			if (service)
+				vchiq_log_warning(vchiq_core_log_level,
+						  "%d: prs %s@%pK (%d->%d) - found connected service %d",
+						  state->id, msg_type_str(type), header,
+						  remoteport, localport, service->localport);
+		}
+
+		if (!service) {
+			vchiq_log_error(vchiq_core_log_level,
+					"%d: prs %s@%pK (%d->%d) - invalid/closed service %d",
+					state->id, msg_type_str(type), header, remoteport,
+					localport, localport);
+			goto skip_message;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) {
+		int svc_fourcc;
+
+		svc_fourcc = service
+			? service->base.fourcc
+			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+		vchiq_log_info(SRVTRACE_LEVEL(service),
+			       "Rcvd Msg %s(%u) from %c%c%c%c s:%d d:%d len:%d",
+			       msg_type_str(type), type, VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+			       remoteport, localport, size);
+		if (size > 0)
+			vchiq_log_dump_mem("Rcvd", 0, header->data, min(16, size));
+	}
+
+	if (((unsigned long)header & VCHIQ_SLOT_MASK) +
+	    calc_stride(size) > VCHIQ_SLOT_SIZE) {
+		vchiq_log_error(vchiq_core_log_level,
+				"header %pK (msgid %x) - size %x too big for slot",
+				header, (unsigned int)msgid, (unsigned int)size);
+		WARN(1, "oversized for slot\n");
+	}
+
+	switch (type) {
+	case VCHIQ_MSG_OPEN:
+		WARN_ON(VCHIQ_MSG_DSTPORT(msgid));
+		if (!parse_open(state, header))
+			goto bail_not_ready;
+		break;
+	case VCHIQ_MSG_OPENACK:
+		if (size >= sizeof(struct vchiq_openack_payload)) {
+			const struct vchiq_openack_payload *payload =
+				(struct vchiq_openack_payload *)
+				header->data;
+			service->peer_version = payload->version;
+		}
+		vchiq_log_info(vchiq_core_log_level, "%d: prs OPENACK@%pK,%x (%d->%d) v:%d",
+			       state->id, header, size, remoteport, localport,
+			       service->peer_version);
+		if (service->srvstate == VCHIQ_SRVSTATE_OPENING) {
+			service->remoteport = remoteport;
+			set_service_state(service, VCHIQ_SRVSTATE_OPEN);
+			complete(&service->remove_event);
+		} else {
+			vchiq_log_error(vchiq_core_log_level, "OPENACK received in state %s",
+					srvstate_names[service->srvstate]);
+		}
+		break;
+	case VCHIQ_MSG_CLOSE:
+		WARN_ON(size); /* There should be no data */
+
+		vchiq_log_info(vchiq_core_log_level, "%d: prs CLOSE@%pK (%d->%d)",
+			       state->id, header, remoteport, localport);
+
+		mark_service_closing_internal(service, 1);
+
+		if (vchiq_close_service_internal(service, CLOSE_RECVD) == VCHIQ_RETRY)
+			goto bail_not_ready;
+
+		vchiq_log_info(vchiq_core_log_level, "Close Service %c%c%c%c s:%u d:%d",
+			       VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+			       service->localport, service->remoteport);
+		break;
+	case VCHIQ_MSG_DATA:
+		vchiq_log_info(vchiq_core_log_level, "%d: prs DATA@%pK,%x (%d->%d)",
+			       state->id, header, size, remoteport, localport);
+
+		if ((service->remoteport == remoteport) &&
+		    (service->srvstate == VCHIQ_SRVSTATE_OPEN)) {
+			header->msgid = msgid | VCHIQ_MSGID_CLAIMED;
+			claim_slot(state->rx_info);
+			DEBUG_TRACE(PARSE_LINE);
+			if (make_service_callback(service, VCHIQ_MESSAGE_AVAILABLE, header,
+						  NULL) == VCHIQ_RETRY) {
+				DEBUG_TRACE(PARSE_LINE);
+				goto bail_not_ready;
+			}
+			VCHIQ_SERVICE_STATS_INC(service, ctrl_rx_count);
+			VCHIQ_SERVICE_STATS_ADD(service, ctrl_rx_bytes, size);
+		} else {
+			VCHIQ_STATS_INC(state, error_count);
+		}
+		break;
+	case VCHIQ_MSG_CONNECT:
+		vchiq_log_info(vchiq_core_log_level, "%d: prs CONNECT@%pK", state->id, header);
+		state->version_common =	((struct vchiq_slot_zero *)
+					 state->slot_data)->version;
+		complete(&state->connect);
+		break;
+	case VCHIQ_MSG_BULK_RX:
+	case VCHIQ_MSG_BULK_TX:
+		/*
+		 * We should never receive a bulk request from the
+		 * other side since we're not setup to perform as the
+		 * master.
+		 */
+		WARN_ON(1);
+		break;
+	case VCHIQ_MSG_BULK_RX_DONE:
+	case VCHIQ_MSG_BULK_TX_DONE:
+		if ((service->remoteport == remoteport) &&
+		    (service->srvstate != VCHIQ_SRVSTATE_FREE)) {
+			struct vchiq_bulk_queue *queue;
+			struct vchiq_bulk *bulk;
+
+			queue = (type == VCHIQ_MSG_BULK_RX_DONE) ?
+				&service->bulk_rx : &service->bulk_tx;
+
+			DEBUG_TRACE(PARSE_LINE);
+			if (mutex_lock_killable(&service->bulk_mutex)) {
+				DEBUG_TRACE(PARSE_LINE);
+				goto bail_not_ready;
+			}
+			if ((int)(queue->remote_insert -
+				queue->local_insert) >= 0) {
+				vchiq_log_error(vchiq_core_log_level,
+						"%d: prs %s@%pK (%d->%d) unexpected (ri=%d,li=%d)",
+						state->id, msg_type_str(type), header, remoteport,
+						localport, queue->remote_insert,
+						queue->local_insert);
+				mutex_unlock(&service->bulk_mutex);
+				break;
+			}
+			if (queue->process != queue->remote_insert) {
+				pr_err("%s: p %x != ri %x\n",
+				       __func__,
+				       queue->process,
+				       queue->remote_insert);
+				mutex_unlock(&service->bulk_mutex);
+				goto bail_not_ready;
+			}
+
+			bulk = &queue->bulks[BULK_INDEX(queue->remote_insert)];
+			bulk->actual = *(int *)header->data;
+			queue->remote_insert++;
+
+			vchiq_log_info(vchiq_core_log_level, "%d: prs %s@%pK (%d->%d) %x@%pad",
+				       state->id, msg_type_str(type), header, remoteport, localport,
+				       bulk->actual, &bulk->data);
+
+			vchiq_log_trace(vchiq_core_log_level, "%d: prs:%d %cx li=%x ri=%x p=%x",
+					state->id, localport,
+					(type == VCHIQ_MSG_BULK_RX_DONE) ? 'r' : 't',
+					queue->local_insert, queue->remote_insert, queue->process);
+
+			DEBUG_TRACE(PARSE_LINE);
+			WARN_ON(queue->process == queue->local_insert);
+			vchiq_complete_bulk(service->instance, bulk);
+			queue->process++;
+			mutex_unlock(&service->bulk_mutex);
+			DEBUG_TRACE(PARSE_LINE);
+			notify_bulks(service, queue, RETRY_POLL);
+			DEBUG_TRACE(PARSE_LINE);
+		}
+		break;
+	case VCHIQ_MSG_PADDING:
+		vchiq_log_trace(vchiq_core_log_level, "%d: prs PADDING@%pK,%x",
+				state->id, header, size);
+		break;
+	case VCHIQ_MSG_PAUSE:
+		/* If initiated, signal the application thread */
+		vchiq_log_trace(vchiq_core_log_level, "%d: prs PAUSE@%pK,%x",
+				state->id, header, size);
+		if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) {
+			vchiq_log_error(vchiq_core_log_level, "%d: PAUSE received in state PAUSED",
+					state->id);
+			break;
+		}
+		if (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT) {
+			/* Send a PAUSE in response */
+			if (queue_message(state, NULL, MAKE_PAUSE, NULL, NULL, 0,
+					  QMFLAGS_NO_MUTEX_UNLOCK) == VCHIQ_RETRY)
+				goto bail_not_ready;
+		}
+		/* At this point slot_mutex is held */
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSED);
+		break;
+	case VCHIQ_MSG_RESUME:
+		vchiq_log_trace(vchiq_core_log_level, "%d: prs RESUME@%pK,%x",
+				state->id, header, size);
+		/* Release the slot mutex */
+		mutex_unlock(&state->slot_mutex);
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+		break;
+
+	case VCHIQ_MSG_REMOTE_USE:
+		vchiq_on_remote_use(state);
+		break;
+	case VCHIQ_MSG_REMOTE_RELEASE:
+		vchiq_on_remote_release(state);
+		break;
+	case VCHIQ_MSG_REMOTE_USE_ACTIVE:
+		break;
+
+	default:
+		vchiq_log_error(vchiq_core_log_level, "%d: prs invalid msgid %x@%pK,%x",
+				state->id, msgid, header, size);
+		WARN(1, "invalid message\n");
+		break;
+	}
+
+skip_message:
+	ret = size;
+
+bail_not_ready:
+	if (service)
+		vchiq_service_put(service);
+
+	return ret;
+}
+
+/* Called by the slot handler thread */
+static void
+parse_rx_slots(struct vchiq_state *state)
+{
+	struct vchiq_shared_state *remote = state->remote;
+	int tx_pos;
+
+	DEBUG_INITIALISE(state->local);
+
+	tx_pos = remote->tx_pos;
+
+	while (state->rx_pos != tx_pos) {
+		struct vchiq_header *header;
+		int size;
+
+		DEBUG_TRACE(PARSE_LINE);
+		if (!state->rx_data) {
+			int rx_index;
+
+			WARN_ON(state->rx_pos & VCHIQ_SLOT_MASK);
+			rx_index = remote->slot_queue[
+				SLOT_QUEUE_INDEX_FROM_POS_MASKED(state->rx_pos)];
+			state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state,
+				rx_index);
+			state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index);
+
+			/*
+			 * Initialise use_count to one, and increment
+			 * release_count at the end of the slot to avoid
+			 * releasing the slot prematurely.
+			 */
+			state->rx_info->use_count = 1;
+			state->rx_info->release_count = 0;
+		}
+
+		header = (struct vchiq_header *)(state->rx_data +
+			(state->rx_pos & VCHIQ_SLOT_MASK));
+		size = parse_message(state, header);
+		if (size < 0)
+			return;
+
+		state->rx_pos += calc_stride(size);
+
+		DEBUG_TRACE(PARSE_LINE);
+		/*
+		 * Perform some housekeeping when the end of the slot is
+		 * reached.
+		 */
+		if ((state->rx_pos & VCHIQ_SLOT_MASK) == 0) {
+			/* Remove the extra reference count. */
+			release_slot(state, state->rx_info, NULL, NULL);
+			state->rx_data = NULL;
+		}
+	}
+}
+
+/**
+ * handle_poll() - handle service polling and other rare conditions
+ * @state:  vchiq state struct
+ *
+ * Context: Process context
+ *
+ * Return:
+ * * 0        - poll handled successful
+ * * -EAGAIN  - retry later
+ */
+static int
+handle_poll(struct vchiq_state *state)
+{
+	switch (state->conn_state) {
+	case VCHIQ_CONNSTATE_CONNECTED:
+		/* Poll the services as requested */
+		poll_services(state);
+		break;
+
+	case VCHIQ_CONNSTATE_PAUSING:
+		if (queue_message(state, NULL, MAKE_PAUSE, NULL, NULL, 0,
+				  QMFLAGS_NO_MUTEX_UNLOCK) != VCHIQ_RETRY) {
+			vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSE_SENT);
+		} else {
+			/* Retry later */
+			return -EAGAIN;
+		}
+		break;
+
+	case VCHIQ_CONNSTATE_RESUMING:
+		if (queue_message(state, NULL, MAKE_RESUME, NULL, NULL, 0,
+				  QMFLAGS_NO_MUTEX_LOCK) != VCHIQ_RETRY) {
+			vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+		} else {
+			/*
+			 * This should really be impossible,
+			 * since the PAUSE should have flushed
+			 * through outstanding messages.
+			 */
+			vchiq_log_error(vchiq_core_log_level, "Failed to send RESUME message");
+		}
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/* Called by the slot handler thread */
+static int
+slot_handler_func(void *v)
+{
+	struct vchiq_state *state = v;
+	struct vchiq_shared_state *local = state->local;
+
+	DEBUG_INITIALISE(local);
+
+	while (1) {
+		DEBUG_COUNT(SLOT_HANDLER_COUNT);
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		remote_event_wait(&state->trigger_event, &local->trigger);
+
+		/* Ensure that reads don't overtake the remote_event_wait. */
+		rmb();
+
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		if (state->poll_needed) {
+			state->poll_needed = 0;
+
+			/*
+			 * Handle service polling and other rare conditions here
+			 * out of the mainline code
+			 */
+			if (handle_poll(state) == -EAGAIN)
+				state->poll_needed = 1;
+		}
+
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		parse_rx_slots(state);
+	}
+	return 0;
+}
+
+/* Called by the recycle thread */
+static int
+recycle_func(void *v)
+{
+	struct vchiq_state *state = v;
+	struct vchiq_shared_state *local = state->local;
+	u32 *found;
+	size_t length;
+
+	length = sizeof(*found) * BITSET_SIZE(VCHIQ_MAX_SERVICES);
+
+	found = kmalloc_array(BITSET_SIZE(VCHIQ_MAX_SERVICES), sizeof(*found),
+			      GFP_KERNEL);
+	if (!found)
+		return -ENOMEM;
+
+	while (1) {
+		remote_event_wait(&state->recycle_event, &local->recycle);
+
+		process_free_queue(state, found, length);
+	}
+	return 0;
+}
+
+/* Called by the sync thread */
+static int
+sync_func(void *v)
+{
+	struct vchiq_state *state = v;
+	struct vchiq_shared_state *local = state->local;
+	struct vchiq_header *header =
+		(struct vchiq_header *)SLOT_DATA_FROM_INDEX(state,
+			state->remote->slot_sync);
+
+	while (1) {
+		struct vchiq_service *service;
+		int msgid, size;
+		int type;
+		unsigned int localport, remoteport;
+
+		remote_event_wait(&state->sync_trigger_event, &local->sync_trigger);
+
+		/* Ensure that reads don't overtake the remote_event_wait. */
+		rmb();
+
+		msgid = header->msgid;
+		size = header->size;
+		type = VCHIQ_MSG_TYPE(msgid);
+		localport = VCHIQ_MSG_DSTPORT(msgid);
+		remoteport = VCHIQ_MSG_SRCPORT(msgid);
+
+		service = find_service_by_port(state, localport);
+
+		if (!service) {
+			vchiq_log_error(vchiq_sync_log_level,
+					"%d: sf %s@%pK (%d->%d) - invalid/closed service %d",
+					state->id, msg_type_str(type), header,
+					remoteport, localport, localport);
+			release_message_sync(state, header);
+			continue;
+		}
+
+		if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
+			int svc_fourcc;
+
+			svc_fourcc = service
+				? service->base.fourcc
+				: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+			vchiq_log_trace(vchiq_sync_log_level,
+					"Rcvd Msg %s from %c%c%c%c s:%d d:%d len:%d",
+					msg_type_str(type), VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+					remoteport, localport, size);
+			if (size > 0)
+				vchiq_log_dump_mem("Rcvd", 0, header->data, min(16, size));
+		}
+
+		switch (type) {
+		case VCHIQ_MSG_OPENACK:
+			if (size >= sizeof(struct vchiq_openack_payload)) {
+				const struct vchiq_openack_payload *payload =
+					(struct vchiq_openack_payload *)
+					header->data;
+				service->peer_version = payload->version;
+			}
+			vchiq_log_info(vchiq_sync_log_level, "%d: sf OPENACK@%pK,%x (%d->%d) v:%d",
+				       state->id, header, size, remoteport, localport,
+				       service->peer_version);
+			if (service->srvstate == VCHIQ_SRVSTATE_OPENING) {
+				service->remoteport = remoteport;
+				set_service_state(service, VCHIQ_SRVSTATE_OPENSYNC);
+				service->sync = 1;
+				complete(&service->remove_event);
+			}
+			release_message_sync(state, header);
+			break;
+
+		case VCHIQ_MSG_DATA:
+			vchiq_log_trace(vchiq_sync_log_level, "%d: sf DATA@%pK,%x (%d->%d)",
+					state->id, header, size, remoteport, localport);
+
+			if ((service->remoteport == remoteport) &&
+			    (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC)) {
+				if (make_service_callback(service, VCHIQ_MESSAGE_AVAILABLE, header,
+							  NULL) == VCHIQ_RETRY)
+					vchiq_log_error(vchiq_sync_log_level,
+							"synchronous callback to service %d returns VCHIQ_RETRY",
+							localport);
+			}
+			break;
+
+		default:
+			vchiq_log_error(vchiq_sync_log_level, "%d: sf unexpected msgid %x@%pK,%x",
+					state->id, msgid, header, size);
+			release_message_sync(state, header);
+			break;
+		}
+
+		vchiq_service_put(service);
+	}
+
+	return 0;
+}
+
+inline const char *
+get_conn_state_name(enum vchiq_connstate conn_state)
+{
+	return conn_state_names[conn_state];
+}
+
+struct vchiq_slot_zero *
+vchiq_init_slots(void *mem_base, int mem_size)
+{
+	int mem_align =
+		(int)((VCHIQ_SLOT_SIZE - (long)mem_base) & VCHIQ_SLOT_MASK);
+	struct vchiq_slot_zero *slot_zero =
+		(struct vchiq_slot_zero *)(mem_base + mem_align);
+	int num_slots = (mem_size - mem_align) / VCHIQ_SLOT_SIZE;
+	int first_data_slot = VCHIQ_SLOT_ZERO_SLOTS;
+
+	check_sizes();
+
+	/* Ensure there is enough memory to run an absolutely minimum system */
+	num_slots -= first_data_slot;
+
+	if (num_slots < 4) {
+		vchiq_log_error(vchiq_core_log_level, "%s - insufficient memory %x bytes",
+				__func__, mem_size);
+		return NULL;
+	}
+
+	memset(slot_zero, 0, sizeof(struct vchiq_slot_zero));
+
+	slot_zero->magic = VCHIQ_MAGIC;
+	slot_zero->version = VCHIQ_VERSION;
+	slot_zero->version_min = VCHIQ_VERSION_MIN;
+	slot_zero->slot_zero_size = sizeof(struct vchiq_slot_zero);
+	slot_zero->slot_size = VCHIQ_SLOT_SIZE;
+	slot_zero->max_slots = VCHIQ_MAX_SLOTS;
+	slot_zero->max_slots_per_side = VCHIQ_MAX_SLOTS_PER_SIDE;
+
+	slot_zero->master.slot_sync = first_data_slot;
+	slot_zero->master.slot_first = first_data_slot + 1;
+	slot_zero->master.slot_last = first_data_slot + (num_slots / 2) - 1;
+	slot_zero->slave.slot_sync = first_data_slot + (num_slots / 2);
+	slot_zero->slave.slot_first = first_data_slot + (num_slots / 2) + 1;
+	slot_zero->slave.slot_last = first_data_slot + num_slots - 1;
+
+	return slot_zero;
+}
+
+int
+vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero, struct device *dev)
+{
+	struct vchiq_shared_state *local;
+	struct vchiq_shared_state *remote;
+	char threadname[16];
+	int i, ret;
+
+	local = &slot_zero->slave;
+	remote = &slot_zero->master;
+
+	if (local->initialised) {
+		vchiq_loud_error_header();
+		if (remote->initialised)
+			vchiq_loud_error("local state has already been initialised");
+		else
+			vchiq_loud_error("master/slave mismatch two slaves");
+		vchiq_loud_error_footer();
+		return -EINVAL;
+	}
+
+	memset(state, 0, sizeof(struct vchiq_state));
+
+	state->dev = dev;
+
+	/*
+	 * initialize shared state pointers
+	 */
+
+	state->local = local;
+	state->remote = remote;
+	state->slot_data = (struct vchiq_slot *)slot_zero;
+
+	/*
+	 * initialize events and mutexes
+	 */
+
+	init_completion(&state->connect);
+	mutex_init(&state->mutex);
+	mutex_init(&state->slot_mutex);
+	mutex_init(&state->recycle_mutex);
+	mutex_init(&state->sync_mutex);
+	mutex_init(&state->bulk_transfer_mutex);
+
+	init_completion(&state->slot_available_event);
+	init_completion(&state->slot_remove_event);
+	init_completion(&state->data_quota_event);
+
+	state->slot_queue_available = 0;
+
+	for (i = 0; i < VCHIQ_MAX_SERVICES; i++) {
+		struct vchiq_service_quota *quota = &state->service_quotas[i];
+		init_completion(&quota->quota_event);
+	}
+
+	for (i = local->slot_first; i <= local->slot_last; i++) {
+		local->slot_queue[state->slot_queue_available] = i;
+		state->slot_queue_available++;
+		complete(&state->slot_available_event);
+	}
+
+	state->default_slot_quota = state->slot_queue_available / 2;
+	state->default_message_quota =
+		min_t(unsigned short, state->default_slot_quota * 256, ~0);
+
+	state->previous_data_index = -1;
+	state->data_use_count = 0;
+	state->data_quota = state->slot_queue_available - 1;
+
+	remote_event_create(&state->trigger_event, &local->trigger);
+	local->tx_pos = 0;
+	remote_event_create(&state->recycle_event, &local->recycle);
+	local->slot_queue_recycle = state->slot_queue_available;
+	remote_event_create(&state->sync_trigger_event, &local->sync_trigger);
+	remote_event_create(&state->sync_release_event, &local->sync_release);
+
+	/* At start-of-day, the slot is empty and available */
+	((struct vchiq_header *)
+		SLOT_DATA_FROM_INDEX(state, local->slot_sync))->msgid =
+							VCHIQ_MSGID_PADDING;
+	remote_event_signal_local(&state->sync_release_event, &local->sync_release);
+
+	local->debug[DEBUG_ENTRIES] = DEBUG_MAX;
+
+	ret = vchiq_platform_init_state(state);
+	if (ret)
+		return ret;
+
+	/*
+	 * bring up slot handler thread
+	 */
+	snprintf(threadname, sizeof(threadname), "vchiq-slot/%d", state->id);
+	state->slot_handler_thread = kthread_create(&slot_handler_func, (void *)state, threadname);
+
+	if (IS_ERR(state->slot_handler_thread)) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		return PTR_ERR(state->slot_handler_thread);
+	}
+	set_user_nice(state->slot_handler_thread, -19);
+
+	snprintf(threadname, sizeof(threadname), "vchiq-recy/%d", state->id);
+	state->recycle_thread = kthread_create(&recycle_func, (void *)state, threadname);
+	if (IS_ERR(state->recycle_thread)) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		ret = PTR_ERR(state->recycle_thread);
+		goto fail_free_handler_thread;
+	}
+	set_user_nice(state->recycle_thread, -19);
+
+	snprintf(threadname, sizeof(threadname), "vchiq-sync/%d", state->id);
+	state->sync_thread = kthread_create(&sync_func, (void *)state, threadname);
+	if (IS_ERR(state->sync_thread)) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		ret = PTR_ERR(state->sync_thread);
+		goto fail_free_recycle_thread;
+	}
+	set_user_nice(state->sync_thread, -20);
+
+	wake_up_process(state->slot_handler_thread);
+	wake_up_process(state->recycle_thread);
+	wake_up_process(state->sync_thread);
+
+	/* Indicate readiness to the other side */
+	local->initialised = 1;
+
+	return 0;
+
+fail_free_recycle_thread:
+	kthread_stop(state->recycle_thread);
+fail_free_handler_thread:
+	kthread_stop(state->slot_handler_thread);
+
+	return ret;
+}
+
+void vchiq_msg_queue_push(struct vchiq_instance *instance, unsigned int handle,
+			  struct vchiq_header *header)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	int pos;
+
+	if (!service)
+		return;
+
+	while (service->msg_queue_write == service->msg_queue_read +
+		VCHIQ_MAX_SLOTS) {
+		if (wait_for_completion_interruptible(&service->msg_queue_pop))
+			flush_signals(current);
+	}
+
+	pos = service->msg_queue_write & (VCHIQ_MAX_SLOTS - 1);
+	service->msg_queue_write++;
+	service->msg_queue[pos] = header;
+
+	complete(&service->msg_queue_push);
+}
+EXPORT_SYMBOL(vchiq_msg_queue_push);
+
+struct vchiq_header *vchiq_msg_hold(struct vchiq_instance *instance, unsigned int handle)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	struct vchiq_header *header;
+	int pos;
+
+	if (!service)
+		return NULL;
+
+	if (service->msg_queue_write == service->msg_queue_read)
+		return NULL;
+
+	while (service->msg_queue_write == service->msg_queue_read) {
+		if (wait_for_completion_interruptible(&service->msg_queue_push))
+			flush_signals(current);
+	}
+
+	pos = service->msg_queue_read & (VCHIQ_MAX_SLOTS - 1);
+	service->msg_queue_read++;
+	header = service->msg_queue[pos];
+
+	complete(&service->msg_queue_pop);
+
+	return header;
+}
+EXPORT_SYMBOL(vchiq_msg_hold);
+
+static int vchiq_validate_params(const struct vchiq_service_params_kernel *params)
+{
+	if (!params->callback || !params->fourcc) {
+		vchiq_loud_error("Can't add service, invalid params\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Called from application thread when a client or server service is created. */
+struct vchiq_service *
+vchiq_add_service_internal(struct vchiq_state *state,
+			   const struct vchiq_service_params_kernel *params,
+			   int srvstate, struct vchiq_instance *instance,
+			   void (*userdata_term)(void *userdata))
+{
+	struct vchiq_service *service;
+	struct vchiq_service __rcu **pservice = NULL;
+	struct vchiq_service_quota *quota;
+	int ret;
+	int i;
+
+	ret = vchiq_validate_params(params);
+	if (ret)
+		return NULL;
+
+	service = kzalloc(sizeof(*service), GFP_KERNEL);
+	if (!service)
+		return service;
+
+	service->base.fourcc   = params->fourcc;
+	service->base.callback = params->callback;
+	service->base.userdata = params->userdata;
+	service->handle        = VCHIQ_SERVICE_HANDLE_INVALID;
+	kref_init(&service->ref_count);
+	service->srvstate      = VCHIQ_SRVSTATE_FREE;
+	service->userdata_term = userdata_term;
+	service->localport     = VCHIQ_PORT_FREE;
+	service->remoteport    = VCHIQ_PORT_FREE;
+
+	service->public_fourcc = (srvstate == VCHIQ_SRVSTATE_OPENING) ?
+		VCHIQ_FOURCC_INVALID : params->fourcc;
+	service->auto_close    = 1;
+	atomic_set(&service->poll_flags, 0);
+	service->version       = params->version;
+	service->version_min   = params->version_min;
+	service->state         = state;
+	service->instance      = instance;
+	init_completion(&service->remove_event);
+	init_completion(&service->bulk_remove_event);
+	init_completion(&service->msg_queue_pop);
+	init_completion(&service->msg_queue_push);
+	mutex_init(&service->bulk_mutex);
+
+	/*
+	 * Although it is perfectly possible to use a spinlock
+	 * to protect the creation of services, it is overkill as it
+	 * disables interrupts while the array is searched.
+	 * The only danger is of another thread trying to create a
+	 * service - service deletion is safe.
+	 * Therefore it is preferable to use state->mutex which,
+	 * although slower to claim, doesn't block interrupts while
+	 * it is held.
+	 */
+
+	mutex_lock(&state->mutex);
+
+	/* Prepare to use a previously unused service */
+	if (state->unused_service < VCHIQ_MAX_SERVICES)
+		pservice = &state->services[state->unused_service];
+
+	if (srvstate == VCHIQ_SRVSTATE_OPENING) {
+		for (i = 0; i < state->unused_service; i++) {
+			if (!rcu_access_pointer(state->services[i])) {
+				pservice = &state->services[i];
+				break;
+			}
+		}
+	} else {
+		rcu_read_lock();
+		for (i = (state->unused_service - 1); i >= 0; i--) {
+			struct vchiq_service *srv;
+
+			srv = rcu_dereference(state->services[i]);
+			if (!srv) {
+				pservice = &state->services[i];
+			} else if ((srv->public_fourcc == params->fourcc) &&
+				   ((srv->instance != instance) ||
+				   (srv->base.callback != params->callback))) {
+				/*
+				 * There is another server using this
+				 * fourcc which doesn't match.
+				 */
+				pservice = NULL;
+				break;
+			}
+		}
+		rcu_read_unlock();
+	}
+
+	if (pservice) {
+		service->localport = (pservice - state->services);
+		if (!handle_seq)
+			handle_seq = VCHIQ_MAX_STATES *
+				 VCHIQ_MAX_SERVICES;
+		service->handle = handle_seq |
+			(state->id * VCHIQ_MAX_SERVICES) |
+			service->localport;
+		handle_seq += VCHIQ_MAX_STATES * VCHIQ_MAX_SERVICES;
+		rcu_assign_pointer(*pservice, service);
+		if (pservice == &state->services[state->unused_service])
+			state->unused_service++;
+	}
+
+	mutex_unlock(&state->mutex);
+
+	if (!pservice) {
+		kfree(service);
+		return NULL;
+	}
+
+	quota = &state->service_quotas[service->localport];
+	quota->slot_quota = state->default_slot_quota;
+	quota->message_quota = state->default_message_quota;
+	if (quota->slot_use_count == 0)
+		quota->previous_tx_index =
+			SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos)
+			- 1;
+
+	/* Bring this service online */
+	set_service_state(service, srvstate);
+
+	vchiq_log_info(vchiq_core_msg_log_level, "%s Service %c%c%c%c SrcPort:%d",
+		       (srvstate == VCHIQ_SRVSTATE_OPENING) ? "Open" : "Add",
+		       VCHIQ_FOURCC_AS_4CHARS(params->fourcc), service->localport);
+
+	/* Don't unlock the service - leave it with a ref_count of 1. */
+
+	return service;
+}
+
+enum vchiq_status
+vchiq_open_service_internal(struct vchiq_service *service, int client_id)
+{
+	struct vchiq_open_payload payload = {
+		service->base.fourcc,
+		client_id,
+		service->version,
+		service->version_min
+	};
+	enum vchiq_status status = VCHIQ_SUCCESS;
+
+	service->client_id = client_id;
+	vchiq_use_service_internal(service);
+	status = queue_message(service->state,
+			       NULL, MAKE_OPEN(service->localport),
+			       memcpy_copy_callback,
+			       &payload,
+			       sizeof(payload),
+			       QMFLAGS_IS_BLOCKING);
+
+	if (status != VCHIQ_SUCCESS)
+		return status;
+
+	/* Wait for the ACK/NAK */
+	if (wait_for_completion_interruptible(&service->remove_event)) {
+		status = VCHIQ_RETRY;
+		vchiq_release_service_internal(service);
+	} else if ((service->srvstate != VCHIQ_SRVSTATE_OPEN) &&
+		   (service->srvstate != VCHIQ_SRVSTATE_OPENSYNC)) {
+		if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT)
+			vchiq_log_error(vchiq_core_log_level,
+					"%d: osi - srvstate = %s (ref %u)",
+					service->state->id,
+					srvstate_names[service->srvstate],
+					kref_read(&service->ref_count));
+		status = VCHIQ_ERROR;
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		vchiq_release_service_internal(service);
+	}
+
+	return status;
+}
+
+static void
+release_service_messages(struct vchiq_service *service)
+{
+	struct vchiq_state *state = service->state;
+	int slot_last = state->remote->slot_last;
+	int i;
+
+	/* Release any claimed messages aimed at this service */
+
+	if (service->sync) {
+		struct vchiq_header *header =
+			(struct vchiq_header *)SLOT_DATA_FROM_INDEX(state,
+						state->remote->slot_sync);
+		if (VCHIQ_MSG_DSTPORT(header->msgid) == service->localport)
+			release_message_sync(state, header);
+
+		return;
+	}
+
+	for (i = state->remote->slot_first; i <= slot_last; i++) {
+		struct vchiq_slot_info *slot_info =
+			SLOT_INFO_FROM_INDEX(state, i);
+		unsigned int pos, end;
+		char *data;
+
+		if (slot_info->release_count == slot_info->use_count)
+			continue;
+
+		data = (char *)SLOT_DATA_FROM_INDEX(state, i);
+		end = VCHIQ_SLOT_SIZE;
+		if (data == state->rx_data)
+			/*
+			 * This buffer is still being read from - stop
+			 * at the current read position
+			 */
+			end = state->rx_pos & VCHIQ_SLOT_MASK;
+
+		pos = 0;
+
+		while (pos < end) {
+			struct vchiq_header *header =
+				(struct vchiq_header *)(data + pos);
+			int msgid = header->msgid;
+			int port = VCHIQ_MSG_DSTPORT(msgid);
+
+			if ((port == service->localport) && (msgid & VCHIQ_MSGID_CLAIMED)) {
+				vchiq_log_info(vchiq_core_log_level, "  fsi - hdr %pK", header);
+				release_slot(state, slot_info, header, NULL);
+			}
+			pos += calc_stride(header->size);
+			if (pos > VCHIQ_SLOT_SIZE) {
+				vchiq_log_error(vchiq_core_log_level,
+						"fsi - pos %x: header %pK, msgid %x, header->msgid %x, header->size %x",
+						pos, header, msgid, header->msgid, header->size);
+				WARN(1, "invalid slot position\n");
+			}
+		}
+	}
+}
+
+static int
+do_abort_bulks(struct vchiq_service *service)
+{
+	enum vchiq_status status;
+
+	/* Abort any outstanding bulk transfers */
+	if (mutex_lock_killable(&service->bulk_mutex))
+		return 0;
+	abort_outstanding_bulks(service, &service->bulk_tx);
+	abort_outstanding_bulks(service, &service->bulk_rx);
+	mutex_unlock(&service->bulk_mutex);
+
+	status = notify_bulks(service, &service->bulk_tx, NO_RETRY_POLL);
+	if (status != VCHIQ_SUCCESS)
+		return 0;
+
+	status = notify_bulks(service, &service->bulk_rx, NO_RETRY_POLL);
+	return (status == VCHIQ_SUCCESS);
+}
+
+static enum vchiq_status
+close_service_complete(struct vchiq_service *service, int failstate)
+{
+	enum vchiq_status status;
+	int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
+	int newstate;
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPEN:
+	case VCHIQ_SRVSTATE_CLOSESENT:
+	case VCHIQ_SRVSTATE_CLOSERECVD:
+		if (is_server) {
+			if (service->auto_close) {
+				service->client_id = 0;
+				service->remoteport = VCHIQ_PORT_FREE;
+				newstate = VCHIQ_SRVSTATE_LISTENING;
+			} else {
+				newstate = VCHIQ_SRVSTATE_CLOSEWAIT;
+			}
+		} else {
+			newstate = VCHIQ_SRVSTATE_CLOSED;
+		}
+		set_service_state(service, newstate);
+		break;
+	case VCHIQ_SRVSTATE_LISTENING:
+		break;
+	default:
+		vchiq_log_error(vchiq_core_log_level, "%s(%x) called in state %s", __func__,
+				service->handle, srvstate_names[service->srvstate]);
+		WARN(1, "%s in unexpected state\n", __func__);
+		return VCHIQ_ERROR;
+	}
+
+	status = make_service_callback(service, VCHIQ_SERVICE_CLOSED, NULL, NULL);
+
+	if (status != VCHIQ_RETRY) {
+		int uc = service->service_use_count;
+		int i;
+		/* Complete the close process */
+		for (i = 0; i < uc; i++)
+			/*
+			 * cater for cases where close is forced and the
+			 * client may not close all it's handles
+			 */
+			vchiq_release_service_internal(service);
+
+		service->client_id = 0;
+		service->remoteport = VCHIQ_PORT_FREE;
+
+		if (service->srvstate == VCHIQ_SRVSTATE_CLOSED) {
+			vchiq_free_service_internal(service);
+		} else if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) {
+			if (is_server)
+				service->closing = 0;
+
+			complete(&service->remove_event);
+		}
+	} else {
+		set_service_state(service, failstate);
+	}
+
+	return status;
+}
+
+/* Called by the slot handler */
+enum vchiq_status
+vchiq_close_service_internal(struct vchiq_service *service, int close_recvd)
+{
+	struct vchiq_state *state = service->state;
+	enum vchiq_status status = VCHIQ_SUCCESS;
+	int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
+	int close_id = MAKE_CLOSE(service->localport,
+				  VCHIQ_MSG_DSTPORT(service->remoteport));
+
+	vchiq_log_info(vchiq_core_log_level, "%d: csi:%d,%d (%s)", service->state->id,
+		       service->localport, close_recvd, srvstate_names[service->srvstate]);
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_CLOSED:
+	case VCHIQ_SRVSTATE_HIDDEN:
+	case VCHIQ_SRVSTATE_LISTENING:
+	case VCHIQ_SRVSTATE_CLOSEWAIT:
+		if (close_recvd) {
+			vchiq_log_error(vchiq_core_log_level, "%s(1) called in state %s",
+					__func__, srvstate_names[service->srvstate]);
+		} else if (is_server) {
+			if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
+				status = VCHIQ_ERROR;
+			} else {
+				service->client_id = 0;
+				service->remoteport = VCHIQ_PORT_FREE;
+				if (service->srvstate == VCHIQ_SRVSTATE_CLOSEWAIT)
+					set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
+			}
+			complete(&service->remove_event);
+		} else {
+			vchiq_free_service_internal(service);
+		}
+		break;
+	case VCHIQ_SRVSTATE_OPENING:
+		if (close_recvd) {
+			/* The open was rejected - tell the user */
+			set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT);
+			complete(&service->remove_event);
+		} else {
+			/* Shutdown mid-open - let the other side know */
+			status = queue_message(state, service, close_id, NULL, NULL, 0, 0);
+		}
+		break;
+
+	case VCHIQ_SRVSTATE_OPENSYNC:
+		mutex_lock(&state->sync_mutex);
+		fallthrough;
+	case VCHIQ_SRVSTATE_OPEN:
+		if (close_recvd) {
+			if (!do_abort_bulks(service))
+				status = VCHIQ_RETRY;
+		}
+
+		release_service_messages(service);
+
+		if (status == VCHIQ_SUCCESS)
+			status = queue_message(state, service, close_id, NULL,
+					       NULL, 0, QMFLAGS_NO_MUTEX_UNLOCK);
+
+		if (status != VCHIQ_SUCCESS) {
+			if (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC)
+				mutex_unlock(&state->sync_mutex);
+			break;
+		}
+
+		if (!close_recvd) {
+			/* Change the state while the mutex is still held */
+			set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
+			mutex_unlock(&state->slot_mutex);
+			if (service->sync)
+				mutex_unlock(&state->sync_mutex);
+			break;
+		}
+
+		/* Change the state while the mutex is still held */
+		set_service_state(service, VCHIQ_SRVSTATE_CLOSERECVD);
+		mutex_unlock(&state->slot_mutex);
+		if (service->sync)
+			mutex_unlock(&state->sync_mutex);
+
+		status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	case VCHIQ_SRVSTATE_CLOSESENT:
+		if (!close_recvd)
+			/* This happens when a process is killed mid-close */
+			break;
+
+		if (!do_abort_bulks(service)) {
+			status = VCHIQ_RETRY;
+			break;
+		}
+
+		if (status == VCHIQ_SUCCESS)
+			status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	case VCHIQ_SRVSTATE_CLOSERECVD:
+		if (!close_recvd && is_server)
+			/* Force into LISTENING mode */
+			set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
+		status = close_service_complete(service, VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	default:
+		vchiq_log_error(vchiq_core_log_level, "%s(%d) called in state %s", __func__,
+				close_recvd, srvstate_names[service->srvstate]);
+		break;
+	}
+
+	return status;
+}
+
+/* Called from the application process upon process death */
+void
+vchiq_terminate_service_internal(struct vchiq_service *service)
+{
+	struct vchiq_state *state = service->state;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: tsi - (%d<->%d)", state->id,
+		       service->localport, service->remoteport);
+
+	mark_service_closing(service);
+
+	/* Mark the service for removal by the slot handler */
+	request_poll(state, service, VCHIQ_POLL_REMOVE);
+}
+
+/* Called from the slot handler */
+void
+vchiq_free_service_internal(struct vchiq_service *service)
+{
+	struct vchiq_state *state = service->state;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: fsi - (%d)", state->id, service->localport);
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPENING:
+	case VCHIQ_SRVSTATE_CLOSED:
+	case VCHIQ_SRVSTATE_HIDDEN:
+	case VCHIQ_SRVSTATE_LISTENING:
+	case VCHIQ_SRVSTATE_CLOSEWAIT:
+		break;
+	default:
+		vchiq_log_error(vchiq_core_log_level, "%d: fsi - (%d) in state %s", state->id,
+				service->localport, srvstate_names[service->srvstate]);
+		return;
+	}
+
+	set_service_state(service, VCHIQ_SRVSTATE_FREE);
+
+	complete(&service->remove_event);
+
+	/* Release the initial lock */
+	vchiq_service_put(service);
+}
+
+enum vchiq_status
+vchiq_connect_internal(struct vchiq_state *state, struct vchiq_instance *instance)
+{
+	struct vchiq_service *service;
+	int i;
+
+	/* Find all services registered to this client and enable them. */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance, &i)) != NULL) {
+		if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)
+			set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
+		vchiq_service_put(service);
+	}
+
+	if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) {
+		if (queue_message(state, NULL, MAKE_CONNECT, NULL, NULL, 0,
+				  QMFLAGS_IS_BLOCKING) == VCHIQ_RETRY)
+			return VCHIQ_RETRY;
+
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTING);
+	}
+
+	if (state->conn_state == VCHIQ_CONNSTATE_CONNECTING) {
+		if (wait_for_completion_interruptible(&state->connect))
+			return VCHIQ_RETRY;
+
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+		complete(&state->connect);
+	}
+
+	return VCHIQ_SUCCESS;
+}
+
+void
+vchiq_shutdown_internal(struct vchiq_state *state, struct vchiq_instance *instance)
+{
+	struct vchiq_service *service;
+	int i;
+
+	/* Find all services registered to this client and remove them. */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance, &i)) != NULL) {
+		(void)vchiq_remove_service(instance, service->handle);
+		vchiq_service_put(service);
+	}
+}
+
+enum vchiq_status
+vchiq_close_service(struct vchiq_instance *instance, unsigned int handle)
+{
+	/* Unregister the service */
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	enum vchiq_status status = VCHIQ_SUCCESS;
+
+	if (!service)
+		return VCHIQ_ERROR;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: close_service:%d",
+		       service->state->id, service->localport);
+
+	if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+	    (service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+	    (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) {
+		vchiq_service_put(service);
+		return VCHIQ_ERROR;
+	}
+
+	mark_service_closing(service);
+
+	if (current == service->state->slot_handler_thread) {
+		status = vchiq_close_service_internal(service, NO_CLOSE_RECVD);
+		WARN_ON(status == VCHIQ_RETRY);
+	} else {
+		/* Mark the service for termination by the slot handler */
+		request_poll(service->state, service, VCHIQ_POLL_TERMINATE);
+	}
+
+	while (1) {
+		if (wait_for_completion_interruptible(&service->remove_event)) {
+			status = VCHIQ_RETRY;
+			break;
+		}
+
+		if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+		    (service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+		    (service->srvstate == VCHIQ_SRVSTATE_OPEN))
+			break;
+
+		vchiq_log_warning(vchiq_core_log_level,
+				  "%d: close_service:%d - waiting in state %s",
+				  service->state->id, service->localport,
+				  srvstate_names[service->srvstate]);
+	}
+
+	if ((status == VCHIQ_SUCCESS) &&
+	    (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
+	    (service->srvstate != VCHIQ_SRVSTATE_LISTENING))
+		status = VCHIQ_ERROR;
+
+	vchiq_service_put(service);
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_close_service);
+
+enum vchiq_status
+vchiq_remove_service(struct vchiq_instance *instance, unsigned int handle)
+{
+	/* Unregister the service */
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	enum vchiq_status status = VCHIQ_SUCCESS;
+
+	if (!service)
+		return VCHIQ_ERROR;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: remove_service:%d",
+		       service->state->id, service->localport);
+
+	if (service->srvstate == VCHIQ_SRVSTATE_FREE) {
+		vchiq_service_put(service);
+		return VCHIQ_ERROR;
+	}
+
+	mark_service_closing(service);
+
+	if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
+	    (current == service->state->slot_handler_thread)) {
+		/*
+		 * Make it look like a client, because it must be removed and
+		 * not left in the LISTENING state.
+		 */
+		service->public_fourcc = VCHIQ_FOURCC_INVALID;
+
+		status = vchiq_close_service_internal(service, NO_CLOSE_RECVD);
+		WARN_ON(status == VCHIQ_RETRY);
+	} else {
+		/* Mark the service for removal by the slot handler */
+		request_poll(service->state, service, VCHIQ_POLL_REMOVE);
+	}
+	while (1) {
+		if (wait_for_completion_interruptible(&service->remove_event)) {
+			status = VCHIQ_RETRY;
+			break;
+		}
+
+		if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+		    (service->srvstate == VCHIQ_SRVSTATE_OPEN))
+			break;
+
+		vchiq_log_warning(vchiq_core_log_level,
+				  "%d: remove_service:%d - waiting in state %s",
+				  service->state->id, service->localport,
+				  srvstate_names[service->srvstate]);
+	}
+
+	if ((status == VCHIQ_SUCCESS) &&
+	    (service->srvstate != VCHIQ_SRVSTATE_FREE))
+		status = VCHIQ_ERROR;
+
+	vchiq_service_put(service);
+
+	return status;
+}
+
+/*
+ * This function may be called by kernel threads or user threads.
+ * User threads may receive VCHIQ_RETRY to indicate that a signal has been
+ * received and the call should be retried after being returned to user
+ * context.
+ * When called in blocking mode, the userdata field points to a bulk_waiter
+ * structure.
+ */
+enum vchiq_status vchiq_bulk_transfer(struct vchiq_instance *instance, unsigned int handle,
+				      void *offset, void __user *uoffset, int size, void *userdata,
+				      enum vchiq_bulk_mode mode, enum vchiq_bulk_dir dir)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	struct vchiq_bulk_queue *queue;
+	struct vchiq_bulk *bulk;
+	struct vchiq_state *state;
+	struct bulk_waiter *bulk_waiter = NULL;
+	const char dir_char = (dir == VCHIQ_BULK_TRANSMIT) ? 't' : 'r';
+	const int dir_msgtype = (dir == VCHIQ_BULK_TRANSMIT) ?
+		VCHIQ_MSG_BULK_TX : VCHIQ_MSG_BULK_RX;
+	enum vchiq_status status = VCHIQ_ERROR;
+	int payload[2];
+
+	if (!service)
+		goto error_exit;
+
+	if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
+		goto error_exit;
+
+	if (!offset && !uoffset)
+		goto error_exit;
+
+	if (vchiq_check_service(service) != VCHIQ_SUCCESS)
+		goto error_exit;
+
+	switch (mode) {
+	case VCHIQ_BULK_MODE_NOCALLBACK:
+	case VCHIQ_BULK_MODE_CALLBACK:
+		break;
+	case VCHIQ_BULK_MODE_BLOCKING:
+		bulk_waiter = userdata;
+		init_completion(&bulk_waiter->event);
+		bulk_waiter->actual = 0;
+		bulk_waiter->bulk = NULL;
+		break;
+	case VCHIQ_BULK_MODE_WAITING:
+		bulk_waiter = userdata;
+		bulk = bulk_waiter->bulk;
+		goto waiting;
+	default:
+		goto error_exit;
+	}
+
+	state = service->state;
+
+	queue = (dir == VCHIQ_BULK_TRANSMIT) ?
+		&service->bulk_tx : &service->bulk_rx;
+
+	if (mutex_lock_killable(&service->bulk_mutex)) {
+		status = VCHIQ_RETRY;
+		goto error_exit;
+	}
+
+	if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS) {
+		VCHIQ_SERVICE_STATS_INC(service, bulk_stalls);
+		do {
+			mutex_unlock(&service->bulk_mutex);
+			if (wait_for_completion_interruptible(&service->bulk_remove_event)) {
+				status = VCHIQ_RETRY;
+				goto error_exit;
+			}
+			if (mutex_lock_killable(&service->bulk_mutex)) {
+				status = VCHIQ_RETRY;
+				goto error_exit;
+			}
+		} while (queue->local_insert == queue->remove +
+				VCHIQ_NUM_SERVICE_BULKS);
+	}
+
+	bulk = &queue->bulks[BULK_INDEX(queue->local_insert)];
+
+	bulk->mode = mode;
+	bulk->dir = dir;
+	bulk->userdata = userdata;
+	bulk->size = size;
+	bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+
+	if (vchiq_prepare_bulk_data(instance, bulk, offset, uoffset, size, dir))
+		goto unlock_error_exit;
+
+	/*
+	 * Ensure that the bulk data record is visible to the peer
+	 * before proceeding.
+	 */
+	wmb();
+
+	vchiq_log_info(vchiq_core_log_level, "%d: bt (%d->%d) %cx %x@%pad %pK",
+		       state->id, service->localport, service->remoteport,
+		       dir_char, size, &bulk->data, userdata);
+
+	/*
+	 * The slot mutex must be held when the service is being closed, so
+	 * claim it here to ensure that isn't happening
+	 */
+	if (mutex_lock_killable(&state->slot_mutex)) {
+		status = VCHIQ_RETRY;
+		goto cancel_bulk_error_exit;
+	}
+
+	if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
+		goto unlock_both_error_exit;
+
+	payload[0] = lower_32_bits(bulk->data);
+	payload[1] = bulk->size;
+	status = queue_message(state,
+			       NULL,
+			       VCHIQ_MAKE_MSG(dir_msgtype,
+					      service->localport,
+					      service->remoteport),
+			       memcpy_copy_callback,
+			       &payload,
+			       sizeof(payload),
+			       QMFLAGS_IS_BLOCKING |
+			       QMFLAGS_NO_MUTEX_LOCK |
+			       QMFLAGS_NO_MUTEX_UNLOCK);
+	if (status != VCHIQ_SUCCESS)
+		goto unlock_both_error_exit;
+
+	queue->local_insert++;
+
+	mutex_unlock(&state->slot_mutex);
+	mutex_unlock(&service->bulk_mutex);
+
+	vchiq_log_trace(vchiq_core_log_level, "%d: bt:%d %cx li=%x ri=%x p=%x",
+			state->id, service->localport, dir_char, queue->local_insert,
+			queue->remote_insert, queue->process);
+
+waiting:
+	vchiq_service_put(service);
+
+	status = VCHIQ_SUCCESS;
+
+	if (bulk_waiter) {
+		bulk_waiter->bulk = bulk;
+		if (wait_for_completion_interruptible(&bulk_waiter->event))
+			status = VCHIQ_RETRY;
+		else if (bulk_waiter->actual == VCHIQ_BULK_ACTUAL_ABORTED)
+			status = VCHIQ_ERROR;
+	}
+
+	return status;
+
+unlock_both_error_exit:
+	mutex_unlock(&state->slot_mutex);
+cancel_bulk_error_exit:
+	vchiq_complete_bulk(service->instance, bulk);
+unlock_error_exit:
+	mutex_unlock(&service->bulk_mutex);
+
+error_exit:
+	if (service)
+		vchiq_service_put(service);
+	return status;
+}
+
+enum vchiq_status
+vchiq_queue_message(struct vchiq_instance *instance, unsigned int handle,
+		    ssize_t (*copy_callback)(void *context, void *dest,
+					     size_t offset, size_t maxsize),
+		    void *context,
+		    size_t size)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	enum vchiq_status status = VCHIQ_ERROR;
+	int data_id;
+
+	if (!service)
+		goto error_exit;
+
+	if (vchiq_check_service(service) != VCHIQ_SUCCESS)
+		goto error_exit;
+
+	if (!size) {
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		goto error_exit;
+	}
+
+	if (size > VCHIQ_MAX_MSG_SIZE) {
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		goto error_exit;
+	}
+
+	data_id = MAKE_DATA(service->localport, service->remoteport);
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPEN:
+		status = queue_message(service->state, service, data_id,
+				       copy_callback, context, size, 1);
+		break;
+	case VCHIQ_SRVSTATE_OPENSYNC:
+		status = queue_message_sync(service->state, service, data_id,
+					    copy_callback, context, size, 1);
+		break;
+	default:
+		status = VCHIQ_ERROR;
+		break;
+	}
+
+error_exit:
+	if (service)
+		vchiq_service_put(service);
+
+	return status;
+}
+
+int vchiq_queue_kernel_message(struct vchiq_instance *instance, unsigned int handle, void *data,
+			       unsigned int size)
+{
+	enum vchiq_status status;
+
+	while (1) {
+		status = vchiq_queue_message(instance, handle, memcpy_copy_callback,
+					     data, size);
+
+		/*
+		 * vchiq_queue_message() may return VCHIQ_RETRY, so we need to
+		 * implement a retry mechanism since this function is supposed
+		 * to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
+
+		msleep(1);
+	}
+
+	return status;
+}
+EXPORT_SYMBOL(vchiq_queue_kernel_message);
+
+void
+vchiq_release_message(struct vchiq_instance *instance, unsigned int handle,
+		      struct vchiq_header *header)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	struct vchiq_shared_state *remote;
+	struct vchiq_state *state;
+	int slot_index;
+
+	if (!service)
+		return;
+
+	state = service->state;
+	remote = state->remote;
+
+	slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header);
+
+	if ((slot_index >= remote->slot_first) &&
+	    (slot_index <= remote->slot_last)) {
+		int msgid = header->msgid;
+
+		if (msgid & VCHIQ_MSGID_CLAIMED) {
+			struct vchiq_slot_info *slot_info =
+				SLOT_INFO_FROM_INDEX(state, slot_index);
+
+			release_slot(state, slot_info, header, service);
+		}
+	} else if (slot_index == remote->slot_sync) {
+		release_message_sync(state, header);
+	}
+
+	vchiq_service_put(service);
+}
+EXPORT_SYMBOL(vchiq_release_message);
+
+static void
+release_message_sync(struct vchiq_state *state, struct vchiq_header *header)
+{
+	header->msgid = VCHIQ_MSGID_PADDING;
+	remote_event_signal(&state->remote->sync_release);
+}
+
+enum vchiq_status
+vchiq_get_peer_version(struct vchiq_instance *instance, unsigned int handle, short *peer_version)
+{
+	enum vchiq_status status = VCHIQ_ERROR;
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+
+	if (!service)
+		goto exit;
+
+	if (vchiq_check_service(service) != VCHIQ_SUCCESS)
+		goto exit;
+
+	if (!peer_version)
+		goto exit;
+
+	*peer_version = service->peer_version;
+	status = VCHIQ_SUCCESS;
+
+exit:
+	if (service)
+		vchiq_service_put(service);
+	return status;
+}
+EXPORT_SYMBOL(vchiq_get_peer_version);
+
+void vchiq_get_config(struct vchiq_config *config)
+{
+	config->max_msg_size           = VCHIQ_MAX_MSG_SIZE;
+	config->bulk_threshold         = VCHIQ_MAX_MSG_SIZE;
+	config->max_outstanding_bulks  = VCHIQ_NUM_SERVICE_BULKS;
+	config->max_services           = VCHIQ_MAX_SERVICES;
+	config->version                = VCHIQ_VERSION;
+	config->version_min            = VCHIQ_VERSION_MIN;
+}
+
+int
+vchiq_set_service_option(struct vchiq_instance *instance, unsigned int handle,
+			 enum vchiq_service_option option, int value)
+{
+	struct vchiq_service *service = find_service_by_handle(instance, handle);
+	struct vchiq_service_quota *quota;
+	int ret = -EINVAL;
+
+	if (!service)
+		return -EINVAL;
+
+	switch (option) {
+	case VCHIQ_SERVICE_OPTION_AUTOCLOSE:
+		service->auto_close = value;
+		ret = 0;
+		break;
+
+	case VCHIQ_SERVICE_OPTION_SLOT_QUOTA:
+		quota = &service->state->service_quotas[service->localport];
+		if (value == 0)
+			value = service->state->default_slot_quota;
+		if ((value >= quota->slot_use_count) &&
+		    (value < (unsigned short)~0)) {
+			quota->slot_quota = value;
+			if ((value >= quota->slot_use_count) &&
+			    (quota->message_quota >= quota->message_use_count))
+				/*
+				 * Signal the service that it may have
+				 * dropped below its quota
+				 */
+				complete(&quota->quota_event);
+			ret = 0;
+		}
+		break;
+
+	case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA:
+		quota = &service->state->service_quotas[service->localport];
+		if (value == 0)
+			value = service->state->default_message_quota;
+		if ((value >= quota->message_use_count) &&
+		    (value < (unsigned short)~0)) {
+			quota->message_quota = value;
+			if ((value >= quota->message_use_count) &&
+			    (quota->slot_quota >= quota->slot_use_count))
+				/*
+				 * Signal the service that it may have
+				 * dropped below its quota
+				 */
+				complete(&quota->quota_event);
+			ret = 0;
+		}
+		break;
+
+	case VCHIQ_SERVICE_OPTION_SYNCHRONOUS:
+		if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
+		    (service->srvstate == VCHIQ_SRVSTATE_LISTENING)) {
+			service->sync = value;
+			ret = 0;
+		}
+		break;
+
+	case VCHIQ_SERVICE_OPTION_TRACE:
+		service->trace = value;
+		ret = 0;
+		break;
+
+	default:
+		break;
+	}
+	vchiq_service_put(service);
+
+	return ret;
+}
+
+static int
+vchiq_dump_shared_state(void *dump_context, struct vchiq_state *state,
+			struct vchiq_shared_state *shared, const char *label)
+{
+	static const char *const debug_names[] = {
+		"<entries>",
+		"SLOT_HANDLER_COUNT",
+		"SLOT_HANDLER_LINE",
+		"PARSE_LINE",
+		"PARSE_HEADER",
+		"PARSE_MSGID",
+		"AWAIT_COMPLETION_LINE",
+		"DEQUEUE_MESSAGE_LINE",
+		"SERVICE_CALLBACK_LINE",
+		"MSG_QUEUE_FULL_COUNT",
+		"COMPLETION_QUEUE_FULL_COUNT"
+	};
+	int i;
+	char buf[80];
+	int len;
+	int err;
+
+	len = scnprintf(buf, sizeof(buf), "  %s: slots %d-%d tx_pos=%x recycle=%x",
+			label, shared->slot_first, shared->slot_last,
+			shared->tx_pos, shared->slot_queue_recycle);
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	len = scnprintf(buf, sizeof(buf), "    Slots claimed:");
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	for (i = shared->slot_first; i <= shared->slot_last; i++) {
+		struct vchiq_slot_info slot_info =
+						*SLOT_INFO_FROM_INDEX(state, i);
+		if (slot_info.use_count != slot_info.release_count) {
+			len = scnprintf(buf, sizeof(buf), "      %d: %d/%d", i, slot_info.use_count,
+					slot_info.release_count);
+			err = vchiq_dump(dump_context, buf, len + 1);
+			if (err)
+				return err;
+		}
+	}
+
+	for (i = 1; i < shared->debug[DEBUG_ENTRIES]; i++) {
+		len = scnprintf(buf, sizeof(buf), "    DEBUG: %s = %d(%x)",
+				debug_names[i], shared->debug[i], shared->debug[i]);
+		err = vchiq_dump(dump_context, buf, len + 1);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+int vchiq_dump_state(void *dump_context, struct vchiq_state *state)
+{
+	char buf[80];
+	int len;
+	int i;
+	int err;
+
+	len = scnprintf(buf, sizeof(buf), "State %d: %s", state->id,
+			conn_state_names[state->conn_state]);
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	len = scnprintf(buf, sizeof(buf), "  tx_pos=%x(@%pK), rx_pos=%x(@%pK)",
+			state->local->tx_pos,
+			state->tx_data + (state->local_tx_pos & VCHIQ_SLOT_MASK),
+			state->rx_pos,
+			state->rx_data + (state->rx_pos & VCHIQ_SLOT_MASK));
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	len = scnprintf(buf, sizeof(buf), "  Version: %d (min %d)",
+			VCHIQ_VERSION, VCHIQ_VERSION_MIN);
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	if (VCHIQ_ENABLE_STATS) {
+		len = scnprintf(buf, sizeof(buf),
+				"  Stats: ctrl_tx_count=%d, ctrl_rx_count=%d, error_count=%d",
+				state->stats.ctrl_tx_count, state->stats.ctrl_rx_count,
+				state->stats.error_count);
+		err = vchiq_dump(dump_context, buf, len + 1);
+		if (err)
+			return err;
+	}
+
+	len = scnprintf(buf, sizeof(buf),
+			"  Slots: %d available (%d data), %d recyclable, %d stalls (%d data)",
+			((state->slot_queue_available * VCHIQ_SLOT_SIZE) -
+			state->local_tx_pos) / VCHIQ_SLOT_SIZE,
+			state->data_quota - state->data_use_count,
+			state->local->slot_queue_recycle - state->slot_queue_available,
+			state->stats.slot_stalls, state->stats.data_stalls);
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	err = vchiq_dump_platform_state(dump_context);
+	if (err)
+		return err;
+
+	err = vchiq_dump_shared_state(dump_context,
+				      state,
+				      state->local,
+				      "Local");
+	if (err)
+		return err;
+	err = vchiq_dump_shared_state(dump_context,
+				      state,
+				      state->remote,
+				      "Remote");
+	if (err)
+		return err;
+
+	err = vchiq_dump_platform_instances(dump_context);
+	if (err)
+		return err;
+
+	for (i = 0; i < state->unused_service; i++) {
+		struct vchiq_service *service = find_service_by_port(state, i);
+
+		if (service) {
+			err = vchiq_dump_service_state(dump_context, service);
+			vchiq_service_put(service);
+			if (err)
+				return err;
+		}
+	}
+	return 0;
+}
+
+int vchiq_dump_service_state(void *dump_context, struct vchiq_service *service)
+{
+	char buf[80];
+	int len;
+	int err;
+	unsigned int ref_count;
+
+	/*Don't include the lock just taken*/
+	ref_count = kref_read(&service->ref_count) - 1;
+	len = scnprintf(buf, sizeof(buf), "Service %u: %s (ref %u)",
+			service->localport, srvstate_names[service->srvstate],
+			ref_count);
+
+	if (service->srvstate != VCHIQ_SRVSTATE_FREE) {
+		char remoteport[30];
+		struct vchiq_service_quota *quota =
+			&service->state->service_quotas[service->localport];
+		int fourcc = service->base.fourcc;
+		int tx_pending, rx_pending;
+
+		if (service->remoteport != VCHIQ_PORT_FREE) {
+			int len2 = scnprintf(remoteport, sizeof(remoteport),
+				"%u", service->remoteport);
+
+			if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
+				scnprintf(remoteport + len2, sizeof(remoteport) - len2,
+					  " (client %x)", service->client_id);
+		} else {
+			strscpy(remoteport, "n/a", sizeof(remoteport));
+		}
+
+		len += scnprintf(buf + len, sizeof(buf) - len,
+				 " '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)",
+				 VCHIQ_FOURCC_AS_4CHARS(fourcc), remoteport,
+				 quota->message_use_count, quota->message_quota,
+				 quota->slot_use_count, quota->slot_quota);
+
+		err = vchiq_dump(dump_context, buf, len + 1);
+		if (err)
+			return err;
+
+		tx_pending = service->bulk_tx.local_insert -
+			service->bulk_tx.remote_insert;
+
+		rx_pending = service->bulk_rx.local_insert -
+			service->bulk_rx.remote_insert;
+
+		len = scnprintf(buf, sizeof(buf),
+				"  Bulk: tx_pending=%d (size %d), rx_pending=%d (size %d)",
+				tx_pending,
+				tx_pending ?
+				service->bulk_tx.bulks[BULK_INDEX(service->bulk_tx.remove)].size :
+				0, rx_pending, rx_pending ?
+				service->bulk_rx.bulks[BULK_INDEX(service->bulk_rx.remove)].size :
+				0);
+
+		if (VCHIQ_ENABLE_STATS) {
+			err = vchiq_dump(dump_context, buf, len + 1);
+			if (err)
+				return err;
+
+			len = scnprintf(buf, sizeof(buf),
+					"  Ctrl: tx_count=%d, tx_bytes=%llu, rx_count=%d, rx_bytes=%llu",
+					service->stats.ctrl_tx_count, service->stats.ctrl_tx_bytes,
+					service->stats.ctrl_rx_count, service->stats.ctrl_rx_bytes);
+			err = vchiq_dump(dump_context, buf, len + 1);
+			if (err)
+				return err;
+
+			len = scnprintf(buf, sizeof(buf),
+					"  Bulk: tx_count=%d, tx_bytes=%llu, rx_count=%d, rx_bytes=%llu",
+					service->stats.bulk_tx_count, service->stats.bulk_tx_bytes,
+					service->stats.bulk_rx_count, service->stats.bulk_rx_bytes);
+			err = vchiq_dump(dump_context, buf, len + 1);
+			if (err)
+				return err;
+
+			len = scnprintf(buf, sizeof(buf),
+					"  %d quota stalls, %d slot stalls, %d bulk stalls, %d aborted, %d errors",
+					service->stats.quota_stalls, service->stats.slot_stalls,
+					service->stats.bulk_stalls,
+					service->stats.bulk_aborted_count,
+					service->stats.error_count);
+		}
+	}
+
+	err = vchiq_dump(dump_context, buf, len + 1);
+	if (err)
+		return err;
+
+	if (service->srvstate != VCHIQ_SRVSTATE_FREE)
+		err = vchiq_dump_platform_service_state(dump_context, service);
+	return err;
+}
+
+void
+vchiq_loud_error_header(void)
+{
+	vchiq_log_error(vchiq_core_log_level,
+			"============================================================================");
+	vchiq_log_error(vchiq_core_log_level,
+			"============================================================================");
+	vchiq_log_error(vchiq_core_log_level, "=====");
+}
+
+void
+vchiq_loud_error_footer(void)
+{
+	vchiq_log_error(vchiq_core_log_level, "=====");
+	vchiq_log_error(vchiq_core_log_level,
+			"============================================================================");
+	vchiq_log_error(vchiq_core_log_level,
+			"============================================================================");
+}
+
+enum vchiq_status vchiq_send_remote_use(struct vchiq_state *state)
+{
+	if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED)
+		return VCHIQ_RETRY;
+
+	return queue_message(state, NULL, MAKE_REMOTE_USE, NULL, NULL, 0, 0);
+}
+
+enum vchiq_status vchiq_send_remote_use_active(struct vchiq_state *state)
+{
+	if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED)
+		return VCHIQ_RETRY;
+
+	return queue_message(state, NULL, MAKE_REMOTE_USE_ACTIVE,
+			     NULL, NULL, 0, 0);
+}
+
+void vchiq_log_dump_mem(const char *label, u32 addr, const void *void_mem, size_t num_bytes)
+{
+	const u8 *mem = void_mem;
+	size_t offset;
+	char line_buf[100];
+	char *s;
+
+	while (num_bytes > 0) {
+		s = line_buf;
+
+		for (offset = 0; offset < 16; offset++) {
+			if (offset < num_bytes)
+				s += scnprintf(s, 4, "%02x ", mem[offset]);
+			else
+				s += scnprintf(s, 4, "   ");
+		}
+
+		for (offset = 0; offset < 16; offset++) {
+			if (offset < num_bytes) {
+				u8 ch = mem[offset];
+
+				if ((ch < ' ') || (ch > '~'))
+					ch = '.';
+				*s++ = (char)ch;
+			}
+		}
+		*s++ = '\0';
+
+		if (label && (*label != '\0'))
+			vchiq_log_trace(VCHIQ_LOG_TRACE, "%s: %08x: %s", label, addr, line_buf);
+		else
+			vchiq_log_trace(VCHIQ_LOG_TRACE, "%08x: %s", addr, line_buf);
+
+		addr += 16;
+		mem += 16;
+		if (num_bytes > 16)
+			num_bytes -= 16;
+		else
+			num_bytes = 0;
+	}
+}
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
new file mode 100644
index 000000000..8b4a38f5b
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
@@ -0,0 +1,596 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_CORE_H
+#define VCHIQ_CORE_H
+
+#include <linux/mutex.h>
+#include <linux/completion.h>
+#include <linux/kthread.h>
+#include <linux/kref.h>
+#include <linux/rcupdate.h>
+#include <linux/wait.h>
+#include <linux/raspberrypi/vchiq.h>
+
+#include "vchiq_cfg.h"
+
+/* Do this so that we can test-build the code on non-rpi systems */
+#if IS_ENABLED(CONFIG_RASPBERRYPI_FIRMWARE)
+
+#else
+
+#ifndef dsb
+#define dsb(a)
+#endif
+
+#endif	/* IS_ENABLED(CONFIG_RASPBERRYPI_FIRMWARE) */
+
+#define VCHIQ_SERVICE_HANDLE_INVALID 0
+
+#define VCHIQ_SLOT_SIZE     4096
+#define VCHIQ_MAX_MSG_SIZE  (VCHIQ_SLOT_SIZE - sizeof(struct vchiq_header))
+
+/* Run time control of log level, based on KERN_XXX level. */
+#define VCHIQ_LOG_DEFAULT  4
+#define VCHIQ_LOG_ERROR    3
+#define VCHIQ_LOG_WARNING  4
+#define VCHIQ_LOG_INFO     6
+#define VCHIQ_LOG_TRACE    7
+
+#define VCHIQ_LOG_PREFIX   KERN_INFO "vchiq: "
+
+#ifndef vchiq_log_error
+#define vchiq_log_error(cat, fmt, ...) \
+	do { if (cat >= VCHIQ_LOG_ERROR) \
+		printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0)
+#endif
+#ifndef vchiq_log_warning
+#define vchiq_log_warning(cat, fmt, ...) \
+	do { if (cat >= VCHIQ_LOG_WARNING) \
+		 printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0)
+#endif
+#ifndef vchiq_log_info
+#define vchiq_log_info(cat, fmt, ...) \
+	do { if (cat >= VCHIQ_LOG_INFO) \
+		printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0)
+#endif
+#ifndef vchiq_log_trace
+#define vchiq_log_trace(cat, fmt, ...) \
+	do { if (cat >= VCHIQ_LOG_TRACE) \
+		printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0)
+#endif
+
+#define vchiq_loud_error(...) \
+	vchiq_log_error(vchiq_core_log_level, "===== " __VA_ARGS__)
+
+#define VCHIQ_SLOT_MASK        (VCHIQ_SLOT_SIZE - 1)
+#define VCHIQ_SLOT_QUEUE_MASK  (VCHIQ_MAX_SLOTS_PER_SIDE - 1)
+#define VCHIQ_SLOT_ZERO_SLOTS  DIV_ROUND_UP(sizeof(struct vchiq_slot_zero), \
+					    VCHIQ_SLOT_SIZE)
+
+#define VCHIQ_FOURCC_AS_4CHARS(fourcc)	\
+	((fourcc) >> 24) & 0xff, \
+	((fourcc) >> 16) & 0xff, \
+	((fourcc) >>  8) & 0xff, \
+	(fourcc) & 0xff
+
+#define BITSET_SIZE(b)        ((b + 31) >> 5)
+#define BITSET_WORD(b)        (b >> 5)
+#define BITSET_BIT(b)         (1 << (b & 31))
+#define BITSET_IS_SET(bs, b)  (bs[BITSET_WORD(b)] & BITSET_BIT(b))
+#define BITSET_SET(bs, b)     (bs[BITSET_WORD(b)] |= BITSET_BIT(b))
+
+enum {
+	DEBUG_ENTRIES,
+#if VCHIQ_ENABLE_DEBUG
+	DEBUG_SLOT_HANDLER_COUNT,
+	DEBUG_SLOT_HANDLER_LINE,
+	DEBUG_PARSE_LINE,
+	DEBUG_PARSE_HEADER,
+	DEBUG_PARSE_MSGID,
+	DEBUG_AWAIT_COMPLETION_LINE,
+	DEBUG_DEQUEUE_MESSAGE_LINE,
+	DEBUG_SERVICE_CALLBACK_LINE,
+	DEBUG_MSG_QUEUE_FULL_COUNT,
+	DEBUG_COMPLETION_QUEUE_FULL_COUNT,
+#endif
+	DEBUG_MAX
+};
+
+#if VCHIQ_ENABLE_DEBUG
+
+#define DEBUG_INITIALISE(local) int *debug_ptr = (local)->debug
+#define DEBUG_TRACE(d) \
+	do { debug_ptr[DEBUG_ ## d] = __LINE__; dsb(sy); } while (0)
+#define DEBUG_VALUE(d, v) \
+	do { debug_ptr[DEBUG_ ## d] = (v); dsb(sy); } while (0)
+#define DEBUG_COUNT(d) \
+	do { debug_ptr[DEBUG_ ## d]++; dsb(sy); } while (0)
+
+#else /* VCHIQ_ENABLE_DEBUG */
+
+#define DEBUG_INITIALISE(local)
+#define DEBUG_TRACE(d)
+#define DEBUG_VALUE(d, v)
+#define DEBUG_COUNT(d)
+
+#endif /* VCHIQ_ENABLE_DEBUG */
+
+enum vchiq_connstate {
+	VCHIQ_CONNSTATE_DISCONNECTED,
+	VCHIQ_CONNSTATE_CONNECTING,
+	VCHIQ_CONNSTATE_CONNECTED,
+	VCHIQ_CONNSTATE_PAUSING,
+	VCHIQ_CONNSTATE_PAUSE_SENT,
+	VCHIQ_CONNSTATE_PAUSED,
+	VCHIQ_CONNSTATE_RESUMING,
+	VCHIQ_CONNSTATE_PAUSE_TIMEOUT,
+	VCHIQ_CONNSTATE_RESUME_TIMEOUT
+};
+
+enum {
+	VCHIQ_SRVSTATE_FREE,
+	VCHIQ_SRVSTATE_HIDDEN,
+	VCHIQ_SRVSTATE_LISTENING,
+	VCHIQ_SRVSTATE_OPENING,
+	VCHIQ_SRVSTATE_OPEN,
+	VCHIQ_SRVSTATE_OPENSYNC,
+	VCHIQ_SRVSTATE_CLOSESENT,
+	VCHIQ_SRVSTATE_CLOSERECVD,
+	VCHIQ_SRVSTATE_CLOSEWAIT,
+	VCHIQ_SRVSTATE_CLOSED
+};
+
+enum vchiq_bulk_dir {
+	VCHIQ_BULK_TRANSMIT,
+	VCHIQ_BULK_RECEIVE
+};
+
+struct vchiq_bulk {
+	short mode;
+	short dir;
+	void *userdata;
+	dma_addr_t data;
+	int size;
+	void *remote_data;
+	int remote_size;
+	int actual;
+};
+
+struct vchiq_bulk_queue {
+	int local_insert;  /* Where to insert the next local bulk */
+	int remote_insert; /* Where to insert the next remote bulk (master) */
+	int process;       /* Bulk to transfer next */
+	int remote_notify; /* Bulk to notify the remote client of next (mstr) */
+	int remove;        /* Bulk to notify the local client of, and remove, next */
+	struct vchiq_bulk bulks[VCHIQ_NUM_SERVICE_BULKS];
+};
+
+struct remote_event {
+	int armed;
+	int fired;
+	u32 __unused;
+};
+
+struct opaque_platform_state;
+
+struct vchiq_slot {
+	char data[VCHIQ_SLOT_SIZE];
+};
+
+struct vchiq_slot_info {
+	/* Use two counters rather than one to avoid the need for a mutex. */
+	short use_count;
+	short release_count;
+};
+
+struct vchiq_service {
+	struct vchiq_service_base base;
+	unsigned int handle;
+	struct kref ref_count;
+	struct rcu_head rcu;
+	int srvstate;
+	void (*userdata_term)(void *userdata);
+	unsigned int localport;
+	unsigned int remoteport;
+	int public_fourcc;
+	int client_id;
+	char auto_close;
+	char sync;
+	char closing;
+	char trace;
+	atomic_t poll_flags;
+	short version;
+	short version_min;
+	short peer_version;
+
+	struct vchiq_state *state;
+	struct vchiq_instance *instance;
+
+	int service_use_count;
+
+	struct vchiq_bulk_queue bulk_tx;
+	struct vchiq_bulk_queue bulk_rx;
+
+	struct completion remove_event;
+	struct completion bulk_remove_event;
+	struct mutex bulk_mutex;
+
+	struct service_stats_struct {
+		int quota_stalls;
+		int slot_stalls;
+		int bulk_stalls;
+		int error_count;
+		int ctrl_tx_count;
+		int ctrl_rx_count;
+		int bulk_tx_count;
+		int bulk_rx_count;
+		int bulk_aborted_count;
+		u64 ctrl_tx_bytes;
+		u64 ctrl_rx_bytes;
+		u64 bulk_tx_bytes;
+		u64 bulk_rx_bytes;
+	} stats;
+
+	int msg_queue_read;
+	int msg_queue_write;
+	struct completion msg_queue_pop;
+	struct completion msg_queue_push;
+	struct vchiq_header *msg_queue[VCHIQ_MAX_SLOTS];
+};
+
+/*
+ * The quota information is outside struct vchiq_service so that it can
+ * be statically allocated, since for accounting reasons a service's slot
+ * usage is carried over between users of the same port number.
+ */
+struct vchiq_service_quota {
+	unsigned short slot_quota;
+	unsigned short slot_use_count;
+	unsigned short message_quota;
+	unsigned short message_use_count;
+	struct completion quota_event;
+	int previous_tx_index;
+};
+
+struct vchiq_shared_state {
+	/* A non-zero value here indicates that the content is valid. */
+	int initialised;
+
+	/* The first and last (inclusive) slots allocated to the owner. */
+	int slot_first;
+	int slot_last;
+
+	/* The slot allocated to synchronous messages from the owner. */
+	int slot_sync;
+
+	/*
+	 * Signalling this event indicates that owner's slot handler thread
+	 * should run.
+	 */
+	struct remote_event trigger;
+
+	/*
+	 * Indicates the byte position within the stream where the next message
+	 * will be written. The least significant bits are an index into the
+	 * slot. The next bits are the index of the slot in slot_queue.
+	 */
+	int tx_pos;
+
+	/* This event should be signalled when a slot is recycled. */
+	struct remote_event recycle;
+
+	/* The slot_queue index where the next recycled slot will be written. */
+	int slot_queue_recycle;
+
+	/* This event should be signalled when a synchronous message is sent. */
+	struct remote_event sync_trigger;
+
+	/*
+	 * This event should be signalled when a synchronous message has been
+	 * released.
+	 */
+	struct remote_event sync_release;
+
+	/* A circular buffer of slot indexes. */
+	int slot_queue[VCHIQ_MAX_SLOTS_PER_SIDE];
+
+	/* Debugging state */
+	int debug[DEBUG_MAX];
+};
+
+struct vchiq_slot_zero {
+	int magic;
+	short version;
+	short version_min;
+	int slot_zero_size;
+	int slot_size;
+	int max_slots;
+	int max_slots_per_side;
+	int platform_data[2];
+	struct vchiq_shared_state master;
+	struct vchiq_shared_state slave;
+	struct vchiq_slot_info slots[VCHIQ_MAX_SLOTS];
+};
+
+struct vchiq_state {
+	struct device *dev;
+	int id;
+	int initialised;
+	enum vchiq_connstate conn_state;
+	short version_common;
+
+	struct vchiq_shared_state *local;
+	struct vchiq_shared_state *remote;
+	struct vchiq_slot *slot_data;
+
+	unsigned short default_slot_quota;
+	unsigned short default_message_quota;
+
+	/* Event indicating connect message received */
+	struct completion connect;
+
+	/* Mutex protecting services */
+	struct mutex mutex;
+	struct vchiq_instance **instance;
+
+	/* Processes incoming messages */
+	struct task_struct *slot_handler_thread;
+
+	/* Processes recycled slots */
+	struct task_struct *recycle_thread;
+
+	/* Processes synchronous messages */
+	struct task_struct *sync_thread;
+
+	/* Local implementation of the trigger remote event */
+	wait_queue_head_t trigger_event;
+
+	/* Local implementation of the recycle remote event */
+	wait_queue_head_t recycle_event;
+
+	/* Local implementation of the sync trigger remote event */
+	wait_queue_head_t sync_trigger_event;
+
+	/* Local implementation of the sync release remote event */
+	wait_queue_head_t sync_release_event;
+
+	char *tx_data;
+	char *rx_data;
+	struct vchiq_slot_info *rx_info;
+
+	struct mutex slot_mutex;
+
+	struct mutex recycle_mutex;
+
+	struct mutex sync_mutex;
+
+	struct mutex bulk_transfer_mutex;
+
+	/*
+	 * Indicates the byte position within the stream from where the next
+	 * message will be read. The least significant bits are an index into
+	 * the slot.The next bits are the index of the slot in
+	 * remote->slot_queue.
+	 */
+	int rx_pos;
+
+	/*
+	 * A cached copy of local->tx_pos. Only write to local->tx_pos, and read
+	 * from remote->tx_pos.
+	 */
+	int local_tx_pos;
+
+	/* The slot_queue index of the slot to become available next. */
+	int slot_queue_available;
+
+	/* A flag to indicate if any poll has been requested */
+	int poll_needed;
+
+	/* Ths index of the previous slot used for data messages. */
+	int previous_data_index;
+
+	/* The number of slots occupied by data messages. */
+	unsigned short data_use_count;
+
+	/* The maximum number of slots to be occupied by data messages. */
+	unsigned short data_quota;
+
+	/* An array of bit sets indicating which services must be polled. */
+	atomic_t poll_services[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
+
+	/* The number of the first unused service */
+	int unused_service;
+
+	/* Signalled when a free slot becomes available. */
+	struct completion slot_available_event;
+
+	struct completion slot_remove_event;
+
+	/* Signalled when a free data slot becomes available. */
+	struct completion data_quota_event;
+
+	struct state_stats_struct {
+		int slot_stalls;
+		int data_stalls;
+		int ctrl_tx_count;
+		int ctrl_rx_count;
+		int error_count;
+	} stats;
+
+	struct vchiq_service __rcu *services[VCHIQ_MAX_SERVICES];
+	struct vchiq_service_quota service_quotas[VCHIQ_MAX_SERVICES];
+	struct vchiq_slot_info slot_info[VCHIQ_MAX_SLOTS];
+
+	struct opaque_platform_state *platform_state;
+};
+
+struct bulk_waiter {
+	struct vchiq_bulk *bulk;
+	struct completion event;
+	int actual;
+};
+
+struct vchiq_config {
+	unsigned int max_msg_size;
+	unsigned int bulk_threshold;	/* The message size above which it
+					 * is better to use a bulk transfer
+					 * (<= max_msg_size)
+					 */
+	unsigned int max_outstanding_bulks;
+	unsigned int max_services;
+	short version;      /* The version of VCHIQ */
+	short version_min;  /* The minimum compatible version of VCHIQ */
+};
+
+extern spinlock_t bulk_waiter_spinlock;
+
+extern int vchiq_core_log_level;
+extern int vchiq_core_msg_log_level;
+extern int vchiq_sync_log_level;
+
+extern const char *
+get_conn_state_name(enum vchiq_connstate conn_state);
+
+extern struct vchiq_slot_zero *
+vchiq_init_slots(void *mem_base, int mem_size);
+
+extern int
+vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero, struct device *dev);
+
+extern enum vchiq_status
+vchiq_connect_internal(struct vchiq_state *state, struct vchiq_instance *instance);
+
+struct vchiq_service *
+vchiq_add_service_internal(struct vchiq_state *state,
+			   const struct vchiq_service_params_kernel *params,
+			   int srvstate, struct vchiq_instance *instance,
+			   void (*userdata_term)(void *userdata));
+
+extern enum vchiq_status
+vchiq_open_service_internal(struct vchiq_service *service, int client_id);
+
+extern enum vchiq_status
+vchiq_close_service_internal(struct vchiq_service *service, int close_recvd);
+
+extern void
+vchiq_terminate_service_internal(struct vchiq_service *service);
+
+extern void
+vchiq_free_service_internal(struct vchiq_service *service);
+
+extern void
+vchiq_shutdown_internal(struct vchiq_state *state, struct vchiq_instance *instance);
+
+extern void
+remote_event_pollall(struct vchiq_state *state);
+
+extern enum vchiq_status
+vchiq_bulk_transfer(struct vchiq_instance *instance, unsigned int handle, void *offset,
+		    void __user *uoffset, int size, void *userdata, enum vchiq_bulk_mode mode,
+		    enum vchiq_bulk_dir dir);
+
+extern int
+vchiq_dump_state(void *dump_context, struct vchiq_state *state);
+
+extern int
+vchiq_dump_service_state(void *dump_context, struct vchiq_service *service);
+
+extern void
+vchiq_loud_error_header(void);
+
+extern void
+vchiq_loud_error_footer(void);
+
+extern void
+request_poll(struct vchiq_state *state, struct vchiq_service *service,
+	     int poll_type);
+
+struct vchiq_service *handle_to_service(struct vchiq_instance *instance, unsigned int handle);
+
+extern struct vchiq_service *
+find_service_by_handle(struct vchiq_instance *instance, unsigned int handle);
+
+extern struct vchiq_service *
+find_service_by_port(struct vchiq_state *state, unsigned int localport);
+
+extern struct vchiq_service *
+find_service_for_instance(struct vchiq_instance *instance, unsigned int handle);
+
+extern struct vchiq_service *
+find_closed_service_for_instance(struct vchiq_instance *instance, unsigned int handle);
+
+extern struct vchiq_service *
+__next_service_by_instance(struct vchiq_state *state,
+			   struct vchiq_instance *instance,
+			   int *pidx);
+
+extern struct vchiq_service *
+next_service_by_instance(struct vchiq_state *state,
+			 struct vchiq_instance *instance,
+			 int *pidx);
+
+extern void
+vchiq_service_get(struct vchiq_service *service);
+
+extern void
+vchiq_service_put(struct vchiq_service *service);
+
+extern enum vchiq_status
+vchiq_queue_message(struct vchiq_instance *instance, unsigned int handle,
+		    ssize_t (*copy_callback)(void *context, void *dest,
+					     size_t offset, size_t maxsize),
+		    void *context,
+		    size_t size);
+
+int vchiq_prepare_bulk_data(struct vchiq_instance *instance, struct vchiq_bulk *bulk, void *offset,
+			    void __user *uoffset, int size, int dir);
+
+void vchiq_complete_bulk(struct vchiq_instance *instance, struct vchiq_bulk *bulk);
+
+void remote_event_signal(struct remote_event *event);
+
+int vchiq_dump(void *dump_context, const char *str, int len);
+
+int vchiq_dump_platform_state(void *dump_context);
+
+int vchiq_dump_platform_instances(void *dump_context);
+
+int vchiq_dump_platform_service_state(void *dump_context, struct vchiq_service *service);
+
+int vchiq_use_service_internal(struct vchiq_service *service);
+
+int vchiq_release_service_internal(struct vchiq_service *service);
+
+void vchiq_on_remote_use(struct vchiq_state *state);
+
+void vchiq_on_remote_release(struct vchiq_state *state);
+
+int vchiq_platform_init_state(struct vchiq_state *state);
+
+enum vchiq_status vchiq_check_service(struct vchiq_service *service);
+
+void vchiq_on_remote_use_active(struct vchiq_state *state);
+
+enum vchiq_status vchiq_send_remote_use(struct vchiq_state *state);
+
+enum vchiq_status vchiq_send_remote_use_active(struct vchiq_state *state);
+
+void vchiq_platform_conn_state_changed(struct vchiq_state *state,
+				       enum vchiq_connstate oldstate,
+				  enum vchiq_connstate newstate);
+
+void vchiq_set_conn_state(struct vchiq_state *state, enum vchiq_connstate newstate);
+
+void vchiq_log_dump_mem(const char *label, u32 addr, const void *void_mem, size_t num_bytes);
+
+enum vchiq_status vchiq_remove_service(struct vchiq_instance *instance, unsigned int service);
+
+int vchiq_get_client_id(struct vchiq_instance *instance, unsigned int service);
+
+void vchiq_get_config(struct vchiq_config *config);
+
+int vchiq_set_service_option(struct vchiq_instance *instance, unsigned int service,
+			     enum vchiq_service_option option, int value);
+
+#endif
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
new file mode 100644
index 000000000..dc667afd1
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ */
+
+#include <linux/debugfs.h>
+#include "vchiq_core.h"
+#include "vchiq_arm.h"
+#include "vchiq_debugfs.h"
+
+#ifdef CONFIG_DEBUG_FS
+
+#define DEBUGFS_WRITE_BUF_SIZE 256
+
+#define VCHIQ_LOG_ERROR_STR   "error"
+#define VCHIQ_LOG_WARNING_STR "warning"
+#define VCHIQ_LOG_INFO_STR    "info"
+#define VCHIQ_LOG_TRACE_STR   "trace"
+
+/* Global 'vchiq' debugfs and clients entry used by all instances */
+static struct dentry *vchiq_dbg_dir;
+static struct dentry *vchiq_dbg_clients;
+
+/* Log category debugfs entries */
+struct vchiq_debugfs_log_entry {
+	const char *name;
+	void *plevel;
+};
+
+static struct vchiq_debugfs_log_entry vchiq_debugfs_log_entries[] = {
+	{ "core", &vchiq_core_log_level },
+	{ "msg",  &vchiq_core_msg_log_level },
+	{ "sync", &vchiq_sync_log_level },
+	{ "susp", &vchiq_susp_log_level },
+	{ "arm",  &vchiq_arm_log_level },
+};
+
+static int debugfs_log_show(struct seq_file *f, void *offset)
+{
+	int *levp = f->private;
+	char *log_value = NULL;
+
+	switch (*levp) {
+	case VCHIQ_LOG_ERROR:
+		log_value = VCHIQ_LOG_ERROR_STR;
+		break;
+	case VCHIQ_LOG_WARNING:
+		log_value = VCHIQ_LOG_WARNING_STR;
+		break;
+	case VCHIQ_LOG_INFO:
+		log_value = VCHIQ_LOG_INFO_STR;
+		break;
+	case VCHIQ_LOG_TRACE:
+		log_value = VCHIQ_LOG_TRACE_STR;
+		break;
+	default:
+		break;
+	}
+
+	seq_printf(f, "%s\n", log_value ? log_value : "(null)");
+
+	return 0;
+}
+
+static int debugfs_log_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, debugfs_log_show, inode->i_private);
+}
+
+static ssize_t debugfs_log_write(struct file *file,
+	const char __user *buffer,
+	size_t count, loff_t *ppos)
+{
+	struct seq_file *f = (struct seq_file *)file->private_data;
+	int *levp = f->private;
+	char kbuf[DEBUGFS_WRITE_BUF_SIZE + 1];
+
+	memset(kbuf, 0, DEBUGFS_WRITE_BUF_SIZE + 1);
+	if (count >= DEBUGFS_WRITE_BUF_SIZE)
+		count = DEBUGFS_WRITE_BUF_SIZE;
+
+	if (copy_from_user(kbuf, buffer, count))
+		return -EFAULT;
+	kbuf[count - 1] = 0;
+
+	if (strncmp("error", kbuf, strlen("error")) == 0)
+		*levp = VCHIQ_LOG_ERROR;
+	else if (strncmp("warning", kbuf, strlen("warning")) == 0)
+		*levp = VCHIQ_LOG_WARNING;
+	else if (strncmp("info", kbuf, strlen("info")) == 0)
+		*levp = VCHIQ_LOG_INFO;
+	else if (strncmp("trace", kbuf, strlen("trace")) == 0)
+		*levp = VCHIQ_LOG_TRACE;
+	else
+		*levp = VCHIQ_LOG_DEFAULT;
+
+	*ppos += count;
+
+	return count;
+}
+
+static const struct file_operations debugfs_log_fops = {
+	.owner		= THIS_MODULE,
+	.open		= debugfs_log_open,
+	.write		= debugfs_log_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int debugfs_usecount_show(struct seq_file *f, void *offset)
+{
+	struct vchiq_instance *instance = f->private;
+	int use_count;
+
+	use_count = vchiq_instance_get_use_count(instance);
+	seq_printf(f, "%d\n", use_count);
+
+	return 0;
+}
+DEFINE_SHOW_ATTRIBUTE(debugfs_usecount);
+
+static int debugfs_trace_show(struct seq_file *f, void *offset)
+{
+	struct vchiq_instance *instance = f->private;
+	int trace;
+
+	trace = vchiq_instance_get_trace(instance);
+	seq_printf(f, "%s\n", trace ? "Y" : "N");
+
+	return 0;
+}
+
+static int debugfs_trace_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, debugfs_trace_show, inode->i_private);
+}
+
+static ssize_t debugfs_trace_write(struct file *file,
+	const char __user *buffer,
+	size_t count, loff_t *ppos)
+{
+	struct seq_file *f = (struct seq_file *)file->private_data;
+	struct vchiq_instance *instance = f->private;
+	char firstchar;
+
+	if (copy_from_user(&firstchar, buffer, 1))
+		return -EFAULT;
+
+	switch (firstchar) {
+	case 'Y':
+	case 'y':
+	case '1':
+		vchiq_instance_set_trace(instance, 1);
+		break;
+	case 'N':
+	case 'n':
+	case '0':
+		vchiq_instance_set_trace(instance, 0);
+		break;
+	default:
+		break;
+	}
+
+	*ppos += count;
+
+	return count;
+}
+
+static const struct file_operations debugfs_trace_fops = {
+	.owner		= THIS_MODULE,
+	.open		= debugfs_trace_open,
+	.write		= debugfs_trace_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+/* add an instance (process) to the debugfs entries */
+void vchiq_debugfs_add_instance(struct vchiq_instance *instance)
+{
+	char pidstr[16];
+	struct dentry *top;
+
+	snprintf(pidstr, sizeof(pidstr), "%d",
+		 vchiq_instance_get_pid(instance));
+
+	top = debugfs_create_dir(pidstr, vchiq_dbg_clients);
+
+	debugfs_create_file("use_count", 0444, top, instance,
+			    &debugfs_usecount_fops);
+	debugfs_create_file("trace", 0644, top, instance, &debugfs_trace_fops);
+
+	vchiq_instance_get_debugfs_node(instance)->dentry = top;
+}
+
+void vchiq_debugfs_remove_instance(struct vchiq_instance *instance)
+{
+	struct vchiq_debugfs_node *node =
+				vchiq_instance_get_debugfs_node(instance);
+
+	debugfs_remove_recursive(node->dentry);
+}
+
+void vchiq_debugfs_init(void)
+{
+	struct dentry *dir;
+	int i;
+
+	vchiq_dbg_dir = debugfs_create_dir("vchiq", NULL);
+	vchiq_dbg_clients = debugfs_create_dir("clients", vchiq_dbg_dir);
+
+	/* create an entry under <debugfs>/vchiq/log for each log category */
+	dir = debugfs_create_dir("log", vchiq_dbg_dir);
+
+	for (i = 0; i < ARRAY_SIZE(vchiq_debugfs_log_entries); i++)
+		debugfs_create_file(vchiq_debugfs_log_entries[i].name, 0644,
+				    dir, vchiq_debugfs_log_entries[i].plevel,
+				    &debugfs_log_fops);
+}
+
+/* remove all the debugfs entries */
+void vchiq_debugfs_deinit(void)
+{
+	debugfs_remove_recursive(vchiq_dbg_dir);
+}
+
+#else /* CONFIG_DEBUG_FS */
+
+void vchiq_debugfs_init(void)
+{
+}
+
+void vchiq_debugfs_deinit(void)
+{
+}
+
+void vchiq_debugfs_add_instance(struct vchiq_instance *instance)
+{
+}
+
+void vchiq_debugfs_remove_instance(struct vchiq_instance *instance)
+{
+}
+
+#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h
new file mode 100644
index 000000000..e9bf055a4
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved. */
+
+#ifndef VCHIQ_DEBUGFS_H
+#define VCHIQ_DEBUGFS_H
+
+#include "vchiq_core.h"
+
+struct vchiq_debugfs_node {
+	struct dentry *dentry;
+};
+
+void vchiq_debugfs_init(void);
+
+void vchiq_debugfs_deinit(void);
+
+void vchiq_debugfs_add_instance(struct vchiq_instance *instance);
+
+void vchiq_debugfs_remove_instance(struct vchiq_instance *instance);
+
+#endif /* VCHIQ_DEBUGFS_H */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c
new file mode 100644
index 000000000..7e2974944
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c
@@ -0,0 +1,1370 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ */
+
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/compat.h>
+#include <linux/miscdevice.h>
+
+#include "vchiq_core.h"
+#include "vchiq_ioctl.h"
+#include "vchiq_arm.h"
+#include "vchiq_debugfs.h"
+
+static const char *const ioctl_names[] = {
+	"CONNECT",
+	"SHUTDOWN",
+	"CREATE_SERVICE",
+	"REMOVE_SERVICE",
+	"QUEUE_MESSAGE",
+	"QUEUE_BULK_TRANSMIT",
+	"QUEUE_BULK_RECEIVE",
+	"AWAIT_COMPLETION",
+	"DEQUEUE_MESSAGE",
+	"GET_CLIENT_ID",
+	"GET_CONFIG",
+	"CLOSE_SERVICE",
+	"USE_SERVICE",
+	"RELEASE_SERVICE",
+	"SET_SERVICE_OPTION",
+	"DUMP_PHYS_MEM",
+	"LIB_VERSION",
+	"CLOSE_DELIVERED"
+};
+
+static_assert(ARRAY_SIZE(ioctl_names) == (VCHIQ_IOC_MAX + 1));
+
+static void
+user_service_free(void *userdata)
+{
+	kfree(userdata);
+}
+
+static void close_delivered(struct user_service *user_service)
+{
+	vchiq_log_info(vchiq_arm_log_level,
+		       "%s(handle=%x)",
+		       __func__, user_service->service->handle);
+
+	if (user_service->close_pending) {
+		/* Allow the underlying service to be culled */
+		vchiq_service_put(user_service->service);
+
+		/* Wake the user-thread blocked in close_ or remove_service */
+		complete(&user_service->close_event);
+
+		user_service->close_pending = 0;
+	}
+}
+
+struct vchiq_io_copy_callback_context {
+	struct vchiq_element *element;
+	size_t element_offset;
+	unsigned long elements_to_go;
+};
+
+static ssize_t vchiq_ioc_copy_element_data(void *context, void *dest,
+					   size_t offset, size_t maxsize)
+{
+	struct vchiq_io_copy_callback_context *cc = context;
+	size_t total_bytes_copied = 0;
+	size_t bytes_this_round;
+
+	while (total_bytes_copied < maxsize) {
+		if (!cc->elements_to_go)
+			return total_bytes_copied;
+
+		if (!cc->element->size) {
+			cc->elements_to_go--;
+			cc->element++;
+			cc->element_offset = 0;
+			continue;
+		}
+
+		bytes_this_round = min(cc->element->size - cc->element_offset,
+				       maxsize - total_bytes_copied);
+
+		if (copy_from_user(dest + total_bytes_copied,
+				   cc->element->data + cc->element_offset,
+				   bytes_this_round))
+			return -EFAULT;
+
+		cc->element_offset += bytes_this_round;
+		total_bytes_copied += bytes_this_round;
+
+		if (cc->element_offset == cc->element->size) {
+			cc->elements_to_go--;
+			cc->element++;
+			cc->element_offset = 0;
+		}
+	}
+
+	return maxsize;
+}
+
+static int
+vchiq_ioc_queue_message(struct vchiq_instance *instance, unsigned int handle,
+			struct vchiq_element *elements, unsigned long count)
+{
+	struct vchiq_io_copy_callback_context context;
+	enum vchiq_status status = VCHIQ_SUCCESS;
+	unsigned long i;
+	size_t total_size = 0;
+
+	context.element = elements;
+	context.element_offset = 0;
+	context.elements_to_go = count;
+
+	for (i = 0; i < count; i++) {
+		if (!elements[i].data && elements[i].size != 0)
+			return -EFAULT;
+
+		total_size += elements[i].size;
+	}
+
+	status = vchiq_queue_message(instance, handle, vchiq_ioc_copy_element_data,
+				     &context, total_size);
+
+	if (status == VCHIQ_ERROR)
+		return -EIO;
+	else if (status == VCHIQ_RETRY)
+		return -EINTR;
+	return 0;
+}
+
+static int vchiq_ioc_create_service(struct vchiq_instance *instance,
+				    struct vchiq_create_service *args)
+{
+	struct user_service *user_service = NULL;
+	struct vchiq_service *service;
+	enum vchiq_status status = VCHIQ_SUCCESS;
+	struct vchiq_service_params_kernel params;
+	int srvstate;
+
+	if (args->is_open && !instance->connected)
+		return -ENOTCONN;
+
+	user_service = kmalloc(sizeof(*user_service), GFP_KERNEL);
+	if (!user_service)
+		return -ENOMEM;
+
+	if (args->is_open) {
+		srvstate = VCHIQ_SRVSTATE_OPENING;
+	} else {
+		srvstate = instance->connected ?
+			 VCHIQ_SRVSTATE_LISTENING : VCHIQ_SRVSTATE_HIDDEN;
+	}
+
+	params = (struct vchiq_service_params_kernel) {
+		.fourcc   = args->params.fourcc,
+		.callback = service_callback,
+		.userdata = user_service,
+		.version  = args->params.version,
+		.version_min = args->params.version_min,
+	};
+	service = vchiq_add_service_internal(instance->state, &params,
+					     srvstate, instance,
+					     user_service_free);
+	if (!service) {
+		kfree(user_service);
+		return -EEXIST;
+	}
+
+	user_service->service = service;
+	user_service->userdata = args->params.userdata;
+	user_service->instance = instance;
+	user_service->is_vchi = (args->is_vchi != 0);
+	user_service->dequeue_pending = 0;
+	user_service->close_pending = 0;
+	user_service->message_available_pos = instance->completion_remove - 1;
+	user_service->msg_insert = 0;
+	user_service->msg_remove = 0;
+	init_completion(&user_service->insert_event);
+	init_completion(&user_service->remove_event);
+	init_completion(&user_service->close_event);
+
+	if (args->is_open) {
+		status = vchiq_open_service_internal(service, instance->pid);
+		if (status != VCHIQ_SUCCESS) {
+			vchiq_remove_service(instance, service->handle);
+			return (status == VCHIQ_RETRY) ?
+				-EINTR : -EIO;
+		}
+	}
+	args->handle = service->handle;
+
+	return 0;
+}
+
+static int vchiq_ioc_dequeue_message(struct vchiq_instance *instance,
+				     struct vchiq_dequeue_message *args)
+{
+	struct user_service *user_service;
+	struct vchiq_service *service;
+	struct vchiq_header *header;
+	int ret;
+
+	DEBUG_INITIALISE(g_state.local);
+	DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+	service = find_service_for_instance(instance, args->handle);
+	if (!service)
+		return -EINVAL;
+
+	user_service = (struct user_service *)service->base.userdata;
+	if (user_service->is_vchi == 0) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	spin_lock(&msg_queue_spinlock);
+	if (user_service->msg_remove == user_service->msg_insert) {
+		if (!args->blocking) {
+			spin_unlock(&msg_queue_spinlock);
+			DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+			ret = -EWOULDBLOCK;
+			goto out;
+		}
+		user_service->dequeue_pending = 1;
+		ret = 0;
+		do {
+			spin_unlock(&msg_queue_spinlock);
+			DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+			if (wait_for_completion_interruptible(&user_service->insert_event)) {
+				vchiq_log_info(vchiq_arm_log_level,
+					       "DEQUEUE_MESSAGE interrupted");
+				ret = -EINTR;
+				break;
+			}
+			spin_lock(&msg_queue_spinlock);
+		} while (user_service->msg_remove == user_service->msg_insert);
+
+		if (ret)
+			goto out;
+	}
+
+	if (WARN_ON_ONCE((int)(user_service->msg_insert -
+			 user_service->msg_remove) < 0)) {
+		spin_unlock(&msg_queue_spinlock);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	header = user_service->msg_queue[user_service->msg_remove &
+		(MSG_QUEUE_SIZE - 1)];
+	user_service->msg_remove++;
+	spin_unlock(&msg_queue_spinlock);
+
+	complete(&user_service->remove_event);
+	if (!header) {
+		ret = -ENOTCONN;
+	} else if (header->size <= args->bufsize) {
+		/* Copy to user space if msgbuf is not NULL */
+		if (!args->buf || (copy_to_user(args->buf, header->data, header->size) == 0)) {
+			ret = header->size;
+			vchiq_release_message(instance, service->handle, header);
+		} else {
+			ret = -EFAULT;
+		}
+	} else {
+		vchiq_log_error(vchiq_arm_log_level,
+				"header %pK: bufsize %x < size %x",
+				header, args->bufsize, header->size);
+		WARN(1, "invalid size\n");
+		ret = -EMSGSIZE;
+	}
+	DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+out:
+	vchiq_service_put(service);
+	return ret;
+}
+
+static int vchiq_irq_queue_bulk_tx_rx(struct vchiq_instance *instance,
+				      struct vchiq_queue_bulk_transfer *args,
+				      enum vchiq_bulk_dir dir,
+				      enum vchiq_bulk_mode __user *mode)
+{
+	struct vchiq_service *service;
+	struct bulk_waiter_node *waiter = NULL, *iter;
+	void *userdata;
+	int status = 0;
+	int ret;
+
+	service = find_service_for_instance(instance, args->handle);
+	if (!service)
+		return -EINVAL;
+
+	if (args->mode == VCHIQ_BULK_MODE_BLOCKING) {
+		waiter = kzalloc(sizeof(*waiter), GFP_KERNEL);
+		if (!waiter) {
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		userdata = &waiter->bulk_waiter;
+	} else if (args->mode == VCHIQ_BULK_MODE_WAITING) {
+		mutex_lock(&instance->bulk_waiter_list_mutex);
+		list_for_each_entry(iter, &instance->bulk_waiter_list,
+				    list) {
+			if (iter->pid == current->pid) {
+				list_del(&iter->list);
+				waiter = iter;
+				break;
+			}
+		}
+		mutex_unlock(&instance->bulk_waiter_list_mutex);
+		if (!waiter) {
+			vchiq_log_error(vchiq_arm_log_level,
+					"no bulk_waiter found for pid %d", current->pid);
+			ret = -ESRCH;
+			goto out;
+		}
+		vchiq_log_info(vchiq_arm_log_level,
+			       "found bulk_waiter %pK for pid %d", waiter, current->pid);
+		userdata = &waiter->bulk_waiter;
+	} else {
+		userdata = args->userdata;
+	}
+
+	status = vchiq_bulk_transfer(instance, args->handle, NULL, args->data, args->size,
+				     userdata, args->mode, dir);
+
+	if (!waiter) {
+		ret = 0;
+		goto out;
+	}
+
+	if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
+	    !waiter->bulk_waiter.bulk) {
+		if (waiter->bulk_waiter.bulk) {
+			/* Cancel the signal when the transfer completes. */
+			spin_lock(&bulk_waiter_spinlock);
+			waiter->bulk_waiter.bulk->userdata = NULL;
+			spin_unlock(&bulk_waiter_spinlock);
+		}
+		kfree(waiter);
+		ret = 0;
+	} else {
+		const enum vchiq_bulk_mode mode_waiting =
+			VCHIQ_BULK_MODE_WAITING;
+		waiter->pid = current->pid;
+		mutex_lock(&instance->bulk_waiter_list_mutex);
+		list_add(&waiter->list, &instance->bulk_waiter_list);
+		mutex_unlock(&instance->bulk_waiter_list_mutex);
+		vchiq_log_info(vchiq_arm_log_level,
+			       "saved bulk_waiter %pK for pid %d", waiter, current->pid);
+
+		ret = put_user(mode_waiting, mode);
+	}
+out:
+	vchiq_service_put(service);
+	if (ret)
+		return ret;
+	else if (status == VCHIQ_ERROR)
+		return -EIO;
+	else if (status == VCHIQ_RETRY)
+		return -EINTR;
+	return 0;
+}
+
+/* read a user pointer value from an array pointers in user space */
+static inline int vchiq_get_user_ptr(void __user **buf, void __user *ubuf, int index)
+{
+	int ret;
+
+	if (in_compat_syscall()) {
+		compat_uptr_t ptr32;
+		compat_uptr_t __user *uptr = ubuf;
+
+		ret = get_user(ptr32, uptr + index);
+		if (ret)
+			return ret;
+
+		*buf = compat_ptr(ptr32);
+	} else {
+		uintptr_t ptr, __user *uptr = ubuf;
+
+		ret = get_user(ptr, uptr + index);
+
+		if (ret)
+			return ret;
+
+		*buf = (void __user *)ptr;
+	}
+
+	return 0;
+}
+
+struct vchiq_completion_data32 {
+	enum vchiq_reason reason;
+	compat_uptr_t header;
+	compat_uptr_t service_userdata;
+	compat_uptr_t bulk_userdata;
+};
+
+static int vchiq_put_completion(struct vchiq_completion_data __user *buf,
+				struct vchiq_completion_data *completion,
+				int index)
+{
+	struct vchiq_completion_data32 __user *buf32 = (void __user *)buf;
+
+	if (in_compat_syscall()) {
+		struct vchiq_completion_data32 tmp = {
+			.reason		  = completion->reason,
+			.header		  = ptr_to_compat(completion->header),
+			.service_userdata = ptr_to_compat(completion->service_userdata),
+			.bulk_userdata	  = ptr_to_compat(completion->bulk_userdata),
+		};
+		if (copy_to_user(&buf32[index], &tmp, sizeof(tmp)))
+			return -EFAULT;
+	} else {
+		if (copy_to_user(&buf[index], completion, sizeof(*completion)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int vchiq_ioc_await_completion(struct vchiq_instance *instance,
+				      struct vchiq_await_completion *args,
+				      int __user *msgbufcountp)
+{
+	int msgbufcount;
+	int remove;
+	int ret;
+
+	DEBUG_INITIALISE(g_state.local);
+
+	DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+	if (!instance->connected)
+		return -ENOTCONN;
+
+	mutex_lock(&instance->completion_mutex);
+
+	DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+	while ((instance->completion_remove == instance->completion_insert) && !instance->closing) {
+		int rc;
+
+		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+		mutex_unlock(&instance->completion_mutex);
+		rc = wait_for_completion_interruptible(&instance->insert_event);
+		mutex_lock(&instance->completion_mutex);
+		if (rc) {
+			DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+			vchiq_log_info(vchiq_arm_log_level,
+				       "AWAIT_COMPLETION interrupted");
+			ret = -EINTR;
+			goto out;
+		}
+	}
+	DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+
+	msgbufcount = args->msgbufcount;
+	remove = instance->completion_remove;
+
+	for (ret = 0; ret < args->count; ret++) {
+		struct vchiq_completion_data_kernel *completion;
+		struct vchiq_completion_data user_completion;
+		struct vchiq_service *service;
+		struct user_service *user_service;
+		struct vchiq_header *header;
+
+		if (remove == instance->completion_insert)
+			break;
+
+		completion = &instance->completions[remove & (MAX_COMPLETIONS - 1)];
+
+		/*
+		 * A read memory barrier is needed to stop
+		 * prefetch of a stale completion record
+		 */
+		rmb();
+
+		service = completion->service_userdata;
+		user_service = service->base.userdata;
+
+		memset(&user_completion, 0, sizeof(user_completion));
+		user_completion = (struct vchiq_completion_data) {
+			.reason = completion->reason,
+			.service_userdata = user_service->userdata,
+		};
+
+		header = completion->header;
+		if (header) {
+			void __user *msgbuf;
+			int msglen;
+
+			msglen = header->size + sizeof(struct vchiq_header);
+			/* This must be a VCHIQ-style service */
+			if (args->msgbufsize < msglen) {
+				vchiq_log_error(vchiq_arm_log_level,
+						"header %pK: msgbufsize %x < msglen %x",
+						header, args->msgbufsize, msglen);
+						WARN(1, "invalid message size\n");
+				if (ret == 0)
+					ret = -EMSGSIZE;
+				break;
+			}
+			if (msgbufcount <= 0)
+				/* Stall here for lack of a buffer for the message. */
+				break;
+			/* Get the pointer from user space */
+			msgbufcount--;
+			if (vchiq_get_user_ptr(&msgbuf, args->msgbufs,
+					       msgbufcount)) {
+				if (ret == 0)
+					ret = -EFAULT;
+				break;
+			}
+
+			/* Copy the message to user space */
+			if (copy_to_user(msgbuf, header, msglen)) {
+				if (ret == 0)
+					ret = -EFAULT;
+				break;
+			}
+
+			/* Now it has been copied, the message can be released. */
+			vchiq_release_message(instance, service->handle, header);
+
+			/* The completion must point to the msgbuf. */
+			user_completion.header = msgbuf;
+		}
+
+		if ((completion->reason == VCHIQ_SERVICE_CLOSED) &&
+		    !instance->use_close_delivered)
+			vchiq_service_put(service);
+
+		/*
+		 * FIXME: address space mismatch, does bulk_userdata
+		 * actually point to user or kernel memory?
+		 */
+		user_completion.bulk_userdata = completion->bulk_userdata;
+
+		if (vchiq_put_completion(args->buf, &user_completion, ret)) {
+			if (ret == 0)
+				ret = -EFAULT;
+			break;
+		}
+
+		/*
+		 * Ensure that the above copy has completed
+		 * before advancing the remove pointer.
+		 */
+		mb();
+		remove++;
+		instance->completion_remove = remove;
+	}
+
+	if (msgbufcount != args->msgbufcount) {
+		if (put_user(msgbufcount, msgbufcountp))
+			ret = -EFAULT;
+	}
+out:
+	if (ret)
+		complete(&instance->remove_event);
+	mutex_unlock(&instance->completion_mutex);
+	DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+
+	return ret;
+}
+
+static long
+vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct vchiq_instance *instance = file->private_data;
+	enum vchiq_status status = VCHIQ_SUCCESS;
+	struct vchiq_service *service = NULL;
+	long ret = 0;
+	int i, rc;
+
+	vchiq_log_trace(vchiq_arm_log_level,
+			"%s - instance %pK, cmd %s, arg %lx", __func__, instance,
+			((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) && (_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ?
+			ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg);
+
+	switch (cmd) {
+	case VCHIQ_IOC_SHUTDOWN:
+		if (!instance->connected)
+			break;
+
+		/* Remove all services */
+		i = 0;
+		while ((service = next_service_by_instance(instance->state,
+							   instance, &i))) {
+			status = vchiq_remove_service(instance, service->handle);
+			vchiq_service_put(service);
+			if (status != VCHIQ_SUCCESS)
+				break;
+		}
+		service = NULL;
+
+		if (status == VCHIQ_SUCCESS) {
+			/* Wake the completion thread and ask it to exit */
+			instance->closing = 1;
+			complete(&instance->insert_event);
+		}
+
+		break;
+
+	case VCHIQ_IOC_CONNECT:
+		if (instance->connected) {
+			ret = -EINVAL;
+			break;
+		}
+		rc = mutex_lock_killable(&instance->state->mutex);
+		if (rc) {
+			vchiq_log_error(vchiq_arm_log_level,
+					"vchiq: connect: could not lock mutex for state %d: %d",
+					instance->state->id, rc);
+			ret = -EINTR;
+			break;
+		}
+		status = vchiq_connect_internal(instance->state, instance);
+		mutex_unlock(&instance->state->mutex);
+
+		if (status == VCHIQ_SUCCESS)
+			instance->connected = 1;
+		else
+			vchiq_log_error(vchiq_arm_log_level,
+					"vchiq: could not connect: %d", status);
+		break;
+
+	case VCHIQ_IOC_CREATE_SERVICE: {
+		struct vchiq_create_service __user *argp;
+		struct vchiq_create_service args;
+
+		argp = (void __user *)arg;
+		if (copy_from_user(&args, argp, sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = vchiq_ioc_create_service(instance, &args);
+		if (ret < 0)
+			break;
+
+		if (put_user(args.handle, &argp->handle)) {
+			vchiq_remove_service(instance, args.handle);
+			ret = -EFAULT;
+		}
+	} break;
+
+	case VCHIQ_IOC_CLOSE_SERVICE:
+	case VCHIQ_IOC_REMOVE_SERVICE: {
+		unsigned int handle = (unsigned int)arg;
+		struct user_service *user_service;
+
+		service = find_service_for_instance(instance, handle);
+		if (!service) {
+			ret = -EINVAL;
+			break;
+		}
+
+		user_service = service->base.userdata;
+
+		/*
+		 * close_pending is false on first entry, and when the
+		 * wait in vchiq_close_service has been interrupted.
+		 */
+		if (!user_service->close_pending) {
+			status = (cmd == VCHIQ_IOC_CLOSE_SERVICE) ?
+				 vchiq_close_service(instance, service->handle) :
+				 vchiq_remove_service(instance, service->handle);
+			if (status != VCHIQ_SUCCESS)
+				break;
+		}
+
+		/*
+		 * close_pending is true once the underlying service
+		 * has been closed until the client library calls the
+		 * CLOSE_DELIVERED ioctl, signalling close_event.
+		 */
+		if (user_service->close_pending &&
+		    wait_for_completion_interruptible(&user_service->close_event))
+			status = VCHIQ_RETRY;
+		break;
+	}
+
+	case VCHIQ_IOC_USE_SERVICE:
+	case VCHIQ_IOC_RELEASE_SERVICE:	{
+		unsigned int handle = (unsigned int)arg;
+
+		service = find_service_for_instance(instance, handle);
+		if (service) {
+			ret = (cmd == VCHIQ_IOC_USE_SERVICE) ?
+				vchiq_use_service_internal(service) :
+				vchiq_release_service_internal(service);
+			if (ret) {
+				vchiq_log_error(vchiq_susp_log_level,
+						"%s: cmd %s returned error %ld for service %c%c%c%c:%03d",
+						__func__, (cmd == VCHIQ_IOC_USE_SERVICE) ?
+						"VCHIQ_IOC_USE_SERVICE" :
+						"VCHIQ_IOC_RELEASE_SERVICE",
+					ret,
+					VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+					service->client_id);
+			}
+		} else {
+			ret = -EINVAL;
+		}
+	} break;
+
+	case VCHIQ_IOC_QUEUE_MESSAGE: {
+		struct vchiq_queue_message args;
+
+		if (copy_from_user(&args, (const void __user *)arg,
+				   sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		service = find_service_for_instance(instance, args.handle);
+
+		if (service && (args.count <= MAX_ELEMENTS)) {
+			/* Copy elements into kernel space */
+			struct vchiq_element elements[MAX_ELEMENTS];
+
+			if (copy_from_user(elements, args.elements,
+					   args.count * sizeof(struct vchiq_element)) == 0)
+				ret = vchiq_ioc_queue_message(instance, args.handle, elements,
+							      args.count);
+			else
+				ret = -EFAULT;
+		} else {
+			ret = -EINVAL;
+		}
+	} break;
+
+	case VCHIQ_IOC_QUEUE_BULK_TRANSMIT:
+	case VCHIQ_IOC_QUEUE_BULK_RECEIVE: {
+		struct vchiq_queue_bulk_transfer args;
+		struct vchiq_queue_bulk_transfer __user *argp;
+
+		enum vchiq_bulk_dir dir =
+			(cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
+			VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
+
+		argp = (void __user *)arg;
+		if (copy_from_user(&args, argp, sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = vchiq_irq_queue_bulk_tx_rx(instance, &args,
+						 dir, &argp->mode);
+	} break;
+
+	case VCHIQ_IOC_AWAIT_COMPLETION: {
+		struct vchiq_await_completion args;
+		struct vchiq_await_completion __user *argp;
+
+		argp = (void __user *)arg;
+		if (copy_from_user(&args, argp, sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = vchiq_ioc_await_completion(instance, &args,
+						 &argp->msgbufcount);
+	} break;
+
+	case VCHIQ_IOC_DEQUEUE_MESSAGE: {
+		struct vchiq_dequeue_message args;
+
+		if (copy_from_user(&args, (const void __user *)arg,
+				   sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = vchiq_ioc_dequeue_message(instance, &args);
+	} break;
+
+	case VCHIQ_IOC_GET_CLIENT_ID: {
+		unsigned int handle = (unsigned int)arg;
+
+		ret = vchiq_get_client_id(instance, handle);
+	} break;
+
+	case VCHIQ_IOC_GET_CONFIG: {
+		struct vchiq_get_config args;
+		struct vchiq_config config;
+
+		if (copy_from_user(&args, (const void __user *)arg,
+				   sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+		if (args.config_size > sizeof(config)) {
+			ret = -EINVAL;
+			break;
+		}
+
+		vchiq_get_config(&config);
+		if (copy_to_user(args.pconfig, &config, args.config_size)) {
+			ret = -EFAULT;
+			break;
+		}
+	} break;
+
+	case VCHIQ_IOC_SET_SERVICE_OPTION: {
+		struct vchiq_set_service_option args;
+
+		if (copy_from_user(&args, (const void __user *)arg,
+				   sizeof(args))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		service = find_service_for_instance(instance, args.handle);
+		if (!service) {
+			ret = -EINVAL;
+			break;
+		}
+
+		ret = vchiq_set_service_option(instance, args.handle, args.option,
+					       args.value);
+	} break;
+
+	case VCHIQ_IOC_LIB_VERSION: {
+		unsigned int lib_version = (unsigned int)arg;
+
+		if (lib_version < VCHIQ_VERSION_MIN)
+			ret = -EINVAL;
+		else if (lib_version >= VCHIQ_VERSION_CLOSE_DELIVERED)
+			instance->use_close_delivered = 1;
+	} break;
+
+	case VCHIQ_IOC_CLOSE_DELIVERED: {
+		unsigned int handle = (unsigned int)arg;
+
+		service = find_closed_service_for_instance(instance, handle);
+		if (service) {
+			struct user_service *user_service =
+				(struct user_service *)service->base.userdata;
+			close_delivered(user_service);
+		} else {
+			ret = -EINVAL;
+		}
+	} break;
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	if (service)
+		vchiq_service_put(service);
+
+	if (ret == 0) {
+		if (status == VCHIQ_ERROR)
+			ret = -EIO;
+		else if (status == VCHIQ_RETRY)
+			ret = -EINTR;
+	}
+
+	if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) && (ret != -EWOULDBLOCK))
+		vchiq_log_info(vchiq_arm_log_level,
+			       "  ioctl instance %pK, cmd %s -> status %d, %ld",
+			       instance, (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
+			       ioctl_names[_IOC_NR(cmd)] : "<invalid>", status, ret);
+	else
+		vchiq_log_trace(vchiq_arm_log_level,
+				"  ioctl instance %pK, cmd %s -> status %d, %ld",
+				instance, (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
+				ioctl_names[_IOC_NR(cmd)] : "<invalid>", status, ret);
+
+	return ret;
+}
+
+#if defined(CONFIG_COMPAT)
+
+struct vchiq_service_params32 {
+	int fourcc;
+	compat_uptr_t callback;
+	compat_uptr_t userdata;
+	short version; /* Increment for non-trivial changes */
+	short version_min; /* Update for incompatible changes */
+};
+
+struct vchiq_create_service32 {
+	struct vchiq_service_params32 params;
+	int is_open;
+	int is_vchi;
+	unsigned int handle; /* OUT */
+};
+
+#define VCHIQ_IOC_CREATE_SERVICE32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service32)
+
+static long
+vchiq_compat_ioctl_create_service(struct file *file, unsigned int cmd,
+				  struct vchiq_create_service32 __user *ptrargs32)
+{
+	struct vchiq_create_service args;
+	struct vchiq_create_service32 args32;
+	struct vchiq_instance *instance = file->private_data;
+	long ret;
+
+	if (copy_from_user(&args32, ptrargs32, sizeof(args32)))
+		return -EFAULT;
+
+	args = (struct vchiq_create_service) {
+		.params = {
+			.fourcc	     = args32.params.fourcc,
+			.callback    = compat_ptr(args32.params.callback),
+			.userdata    = compat_ptr(args32.params.userdata),
+			.version     = args32.params.version,
+			.version_min = args32.params.version_min,
+		},
+		.is_open = args32.is_open,
+		.is_vchi = args32.is_vchi,
+		.handle  = args32.handle,
+	};
+
+	ret = vchiq_ioc_create_service(instance, &args);
+	if (ret < 0)
+		return ret;
+
+	if (put_user(args.handle, &ptrargs32->handle)) {
+		vchiq_remove_service(instance, args.handle);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+struct vchiq_element32 {
+	compat_uptr_t data;
+	unsigned int size;
+};
+
+struct vchiq_queue_message32 {
+	unsigned int handle;
+	unsigned int count;
+	compat_uptr_t elements;
+};
+
+#define VCHIQ_IOC_QUEUE_MESSAGE32 \
+	_IOW(VCHIQ_IOC_MAGIC,  4, struct vchiq_queue_message32)
+
+static long
+vchiq_compat_ioctl_queue_message(struct file *file,
+				 unsigned int cmd,
+				 struct vchiq_queue_message32 __user *arg)
+{
+	struct vchiq_queue_message args;
+	struct vchiq_queue_message32 args32;
+	struct vchiq_service *service;
+	struct vchiq_instance *instance = file->private_data;
+	int ret;
+
+	if (copy_from_user(&args32, arg, sizeof(args32)))
+		return -EFAULT;
+
+	args = (struct vchiq_queue_message) {
+		.handle   = args32.handle,
+		.count    = args32.count,
+		.elements = compat_ptr(args32.elements),
+	};
+
+	if (args32.count > MAX_ELEMENTS)
+		return -EINVAL;
+
+	service = find_service_for_instance(instance, args.handle);
+	if (!service)
+		return -EINVAL;
+
+	if (args32.elements && args32.count) {
+		struct vchiq_element32 element32[MAX_ELEMENTS];
+		struct vchiq_element elements[MAX_ELEMENTS];
+		unsigned int count;
+
+		if (copy_from_user(&element32, args.elements,
+				   sizeof(element32))) {
+			vchiq_service_put(service);
+			return -EFAULT;
+		}
+
+		for (count = 0; count < args32.count; count++) {
+			elements[count].data =
+				compat_ptr(element32[count].data);
+			elements[count].size = element32[count].size;
+		}
+		ret = vchiq_ioc_queue_message(instance, args.handle, elements,
+					      args.count);
+	} else {
+		ret = -EINVAL;
+	}
+	vchiq_service_put(service);
+
+	return ret;
+}
+
+struct vchiq_queue_bulk_transfer32 {
+	unsigned int handle;
+	compat_uptr_t data;
+	unsigned int size;
+	compat_uptr_t userdata;
+	enum vchiq_bulk_mode mode;
+};
+
+#define VCHIQ_IOC_QUEUE_BULK_TRANSMIT32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer32)
+#define VCHIQ_IOC_QUEUE_BULK_RECEIVE32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer32)
+
+static long
+vchiq_compat_ioctl_queue_bulk(struct file *file,
+			      unsigned int cmd,
+			      struct vchiq_queue_bulk_transfer32 __user *argp)
+{
+	struct vchiq_queue_bulk_transfer32 args32;
+	struct vchiq_queue_bulk_transfer args;
+	enum vchiq_bulk_dir dir = (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT32) ?
+				  VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
+
+	if (copy_from_user(&args32, argp, sizeof(args32)))
+		return -EFAULT;
+
+	args = (struct vchiq_queue_bulk_transfer) {
+		.handle   = args32.handle,
+		.data	  = compat_ptr(args32.data),
+		.size	  = args32.size,
+		.userdata = compat_ptr(args32.userdata),
+		.mode	  = args32.mode,
+	};
+
+	return vchiq_irq_queue_bulk_tx_rx(file->private_data, &args,
+					  dir, &argp->mode);
+}
+
+struct vchiq_await_completion32 {
+	unsigned int count;
+	compat_uptr_t buf;
+	unsigned int msgbufsize;
+	unsigned int msgbufcount; /* IN/OUT */
+	compat_uptr_t msgbufs;
+};
+
+#define VCHIQ_IOC_AWAIT_COMPLETION32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion32)
+
+static long
+vchiq_compat_ioctl_await_completion(struct file *file,
+				    unsigned int cmd,
+				    struct vchiq_await_completion32 __user *argp)
+{
+	struct vchiq_await_completion args;
+	struct vchiq_await_completion32 args32;
+
+	if (copy_from_user(&args32, argp, sizeof(args32)))
+		return -EFAULT;
+
+	args = (struct vchiq_await_completion) {
+		.count		= args32.count,
+		.buf		= compat_ptr(args32.buf),
+		.msgbufsize	= args32.msgbufsize,
+		.msgbufcount	= args32.msgbufcount,
+		.msgbufs	= compat_ptr(args32.msgbufs),
+	};
+
+	return vchiq_ioc_await_completion(file->private_data, &args,
+					  &argp->msgbufcount);
+}
+
+struct vchiq_dequeue_message32 {
+	unsigned int handle;
+	int blocking;
+	unsigned int bufsize;
+	compat_uptr_t buf;
+};
+
+#define VCHIQ_IOC_DEQUEUE_MESSAGE32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message32)
+
+static long
+vchiq_compat_ioctl_dequeue_message(struct file *file,
+				   unsigned int cmd,
+				   struct vchiq_dequeue_message32 __user *arg)
+{
+	struct vchiq_dequeue_message32 args32;
+	struct vchiq_dequeue_message args;
+
+	if (copy_from_user(&args32, arg, sizeof(args32)))
+		return -EFAULT;
+
+	args = (struct vchiq_dequeue_message) {
+		.handle		= args32.handle,
+		.blocking	= args32.blocking,
+		.bufsize	= args32.bufsize,
+		.buf		= compat_ptr(args32.buf),
+	};
+
+	return vchiq_ioc_dequeue_message(file->private_data, &args);
+}
+
+struct vchiq_get_config32 {
+	unsigned int config_size;
+	compat_uptr_t pconfig;
+};
+
+#define VCHIQ_IOC_GET_CONFIG32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config32)
+
+static long
+vchiq_compat_ioctl_get_config(struct file *file,
+			      unsigned int cmd,
+			      struct vchiq_get_config32 __user *arg)
+{
+	struct vchiq_get_config32 args32;
+	struct vchiq_config config;
+	void __user *ptr;
+
+	if (copy_from_user(&args32, arg, sizeof(args32)))
+		return -EFAULT;
+	if (args32.config_size > sizeof(config))
+		return -EINVAL;
+
+	vchiq_get_config(&config);
+	ptr = compat_ptr(args32.pconfig);
+	if (copy_to_user(ptr, &config, args32.config_size))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long
+vchiq_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	void __user *argp = compat_ptr(arg);
+
+	switch (cmd) {
+	case VCHIQ_IOC_CREATE_SERVICE32:
+		return vchiq_compat_ioctl_create_service(file, cmd, argp);
+	case VCHIQ_IOC_QUEUE_MESSAGE32:
+		return vchiq_compat_ioctl_queue_message(file, cmd, argp);
+	case VCHIQ_IOC_QUEUE_BULK_TRANSMIT32:
+	case VCHIQ_IOC_QUEUE_BULK_RECEIVE32:
+		return vchiq_compat_ioctl_queue_bulk(file, cmd, argp);
+	case VCHIQ_IOC_AWAIT_COMPLETION32:
+		return vchiq_compat_ioctl_await_completion(file, cmd, argp);
+	case VCHIQ_IOC_DEQUEUE_MESSAGE32:
+		return vchiq_compat_ioctl_dequeue_message(file, cmd, argp);
+	case VCHIQ_IOC_GET_CONFIG32:
+		return vchiq_compat_ioctl_get_config(file, cmd, argp);
+	default:
+		return vchiq_ioctl(file, cmd, (unsigned long)argp);
+	}
+}
+
+#endif
+
+static int vchiq_open(struct inode *inode, struct file *file)
+{
+	struct vchiq_state *state = vchiq_get_state();
+	struct vchiq_instance *instance;
+
+	vchiq_log_info(vchiq_arm_log_level, "vchiq_open");
+
+	if (!state) {
+		vchiq_log_error(vchiq_arm_log_level,
+				"vchiq has no connection to VideoCore");
+		return -ENOTCONN;
+	}
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance)
+		return -ENOMEM;
+
+	instance->state = state;
+	instance->pid = current->tgid;
+
+	vchiq_debugfs_add_instance(instance);
+
+	init_completion(&instance->insert_event);
+	init_completion(&instance->remove_event);
+	mutex_init(&instance->completion_mutex);
+	mutex_init(&instance->bulk_waiter_list_mutex);
+	INIT_LIST_HEAD(&instance->bulk_waiter_list);
+
+	file->private_data = instance;
+
+	return 0;
+}
+
+static int vchiq_release(struct inode *inode, struct file *file)
+{
+	struct vchiq_instance *instance = file->private_data;
+	struct vchiq_state *state = vchiq_get_state();
+	struct vchiq_service *service;
+	int ret = 0;
+	int i;
+
+	vchiq_log_info(vchiq_arm_log_level, "%s: instance=%lx", __func__,
+		       (unsigned long)instance);
+
+	if (!state) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	/* Ensure videocore is awake to allow termination. */
+	vchiq_use_internal(instance->state, NULL, USE_TYPE_VCHIQ);
+
+	mutex_lock(&instance->completion_mutex);
+
+	/* Wake the completion thread and ask it to exit */
+	instance->closing = 1;
+	complete(&instance->insert_event);
+
+	mutex_unlock(&instance->completion_mutex);
+
+	/* Wake the slot handler if the completion queue is full. */
+	complete(&instance->remove_event);
+
+	/* Mark all services for termination... */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance, &i))) {
+		struct user_service *user_service = service->base.userdata;
+
+		/* Wake the slot handler if the msg queue is full. */
+		complete(&user_service->remove_event);
+
+		vchiq_terminate_service_internal(service);
+		vchiq_service_put(service);
+	}
+
+	/* ...and wait for them to die */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance, &i))) {
+		struct user_service *user_service = service->base.userdata;
+
+		wait_for_completion(&service->remove_event);
+
+		if (WARN_ON(service->srvstate != VCHIQ_SRVSTATE_FREE)) {
+			vchiq_service_put(service);
+			break;
+		}
+
+		spin_lock(&msg_queue_spinlock);
+
+		while (user_service->msg_remove != user_service->msg_insert) {
+			struct vchiq_header *header;
+			int m = user_service->msg_remove & (MSG_QUEUE_SIZE - 1);
+
+			header = user_service->msg_queue[m];
+			user_service->msg_remove++;
+			spin_unlock(&msg_queue_spinlock);
+
+			if (header)
+				vchiq_release_message(instance, service->handle, header);
+			spin_lock(&msg_queue_spinlock);
+		}
+
+		spin_unlock(&msg_queue_spinlock);
+
+		vchiq_service_put(service);
+	}
+
+	/* Release any closed services */
+	while (instance->completion_remove != instance->completion_insert) {
+		struct vchiq_completion_data_kernel *completion;
+		struct vchiq_service *service;
+
+		completion = &instance->completions[instance->completion_remove
+						    & (MAX_COMPLETIONS - 1)];
+		service = completion->service_userdata;
+		if (completion->reason == VCHIQ_SERVICE_CLOSED) {
+			struct user_service *user_service =
+							service->base.userdata;
+
+			/* Wake any blocked user-thread */
+			if (instance->use_close_delivered)
+				complete(&user_service->close_event);
+			vchiq_service_put(service);
+		}
+		instance->completion_remove++;
+	}
+
+	/* Release the PEER service count. */
+	vchiq_release_internal(instance->state, NULL);
+
+	free_bulk_waiter(instance);
+
+	vchiq_debugfs_remove_instance(instance);
+
+	kfree(instance);
+	file->private_data = NULL;
+
+out:
+	return ret;
+}
+
+static ssize_t
+vchiq_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+{
+	struct dump_context context;
+	int err;
+
+	context.buf = buf;
+	context.actual = 0;
+	context.space = count;
+	context.offset = *ppos;
+
+	err = vchiq_dump_state(&context, &g_state);
+	if (err)
+		return err;
+
+	*ppos += context.actual;
+
+	return context.actual;
+}
+
+static const struct file_operations
+vchiq_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = vchiq_ioctl,
+#if defined(CONFIG_COMPAT)
+	.compat_ioctl = vchiq_compat_ioctl,
+#endif
+	.open = vchiq_open,
+	.release = vchiq_release,
+	.read = vchiq_read
+};
+
+static struct miscdevice vchiq_miscdev = {
+	.fops = &vchiq_fops,
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "vchiq",
+
+};
+
+/**
+ *	vchiq_register_chrdev - Register the char driver for vchiq
+ *				and create the necessary class and
+ *				device files in userspace.
+ *	@parent		The parent of the char device.
+ *
+ *	Returns 0 on success else returns the error code.
+ */
+int vchiq_register_chrdev(struct device *parent)
+{
+	vchiq_miscdev.parent = parent;
+
+	return misc_register(&vchiq_miscdev);
+}
+
+/**
+ *	vchiq_deregister_chrdev	- Deregister and cleanup the vchiq char
+ *				  driver and device files
+ */
+void vchiq_deregister_chrdev(void)
+{
+	misc_deregister(&vchiq_miscdev);
+}
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
new file mode 100644
index 000000000..86d77f2ee
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
@@ -0,0 +1,112 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_IOCTLS_H
+#define VCHIQ_IOCTLS_H
+
+#include <linux/ioctl.h>
+#include <linux/raspberrypi/vchiq.h>
+
+#define VCHIQ_IOC_MAGIC 0xc4
+#define VCHIQ_INVALID_HANDLE (~0)
+
+struct vchiq_service_params {
+	int fourcc;
+	enum vchiq_status __user (*callback)(enum vchiq_reason reason,
+				      struct vchiq_header *header,
+				      unsigned int handle,
+				      void *bulk_userdata);
+	void __user *userdata;
+	short version;       /* Increment for non-trivial changes */
+	short version_min;   /* Update for incompatible changes */
+};
+
+struct vchiq_create_service {
+	struct vchiq_service_params params;
+	int is_open;
+	int is_vchi;
+	unsigned int handle;       /* OUT */
+};
+
+struct vchiq_queue_message {
+	unsigned int handle;
+	unsigned int count;
+	const struct vchiq_element __user *elements;
+};
+
+struct vchiq_queue_bulk_transfer {
+	unsigned int handle;
+	void __user *data;
+	unsigned int size;
+	void __user *userdata;
+	enum vchiq_bulk_mode mode;
+};
+
+struct vchiq_completion_data {
+	enum vchiq_reason reason;
+	struct vchiq_header __user *header;
+	void __user *service_userdata;
+	void __user *bulk_userdata;
+};
+
+struct vchiq_await_completion {
+	unsigned int count;
+	struct vchiq_completion_data __user *buf;
+	unsigned int msgbufsize;
+	unsigned int msgbufcount; /* IN/OUT */
+	void * __user *msgbufs;
+};
+
+struct vchiq_dequeue_message {
+	unsigned int handle;
+	int blocking;
+	unsigned int bufsize;
+	void __user *buf;
+};
+
+struct vchiq_get_config {
+	unsigned int config_size;
+	struct vchiq_config __user *pconfig;
+};
+
+struct vchiq_set_service_option {
+	unsigned int handle;
+	enum vchiq_service_option option;
+	int value;
+};
+
+struct vchiq_dump_mem {
+	void     __user *virt_addr;
+	size_t    num_bytes;
+};
+
+#define VCHIQ_IOC_CONNECT              _IO(VCHIQ_IOC_MAGIC,   0)
+#define VCHIQ_IOC_SHUTDOWN             _IO(VCHIQ_IOC_MAGIC,   1)
+#define VCHIQ_IOC_CREATE_SERVICE \
+	_IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service)
+#define VCHIQ_IOC_REMOVE_SERVICE       _IO(VCHIQ_IOC_MAGIC,   3)
+#define VCHIQ_IOC_QUEUE_MESSAGE \
+	_IOW(VCHIQ_IOC_MAGIC,  4, struct vchiq_queue_message)
+#define VCHIQ_IOC_QUEUE_BULK_TRANSMIT \
+	_IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer)
+#define VCHIQ_IOC_QUEUE_BULK_RECEIVE \
+	_IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer)
+#define VCHIQ_IOC_AWAIT_COMPLETION \
+	_IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion)
+#define VCHIQ_IOC_DEQUEUE_MESSAGE \
+	_IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message)
+#define VCHIQ_IOC_GET_CLIENT_ID        _IO(VCHIQ_IOC_MAGIC,   9)
+#define VCHIQ_IOC_GET_CONFIG \
+	_IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config)
+#define VCHIQ_IOC_CLOSE_SERVICE        _IO(VCHIQ_IOC_MAGIC,   11)
+#define VCHIQ_IOC_USE_SERVICE          _IO(VCHIQ_IOC_MAGIC,   12)
+#define VCHIQ_IOC_RELEASE_SERVICE      _IO(VCHIQ_IOC_MAGIC,   13)
+#define VCHIQ_IOC_SET_SERVICE_OPTION \
+	_IOW(VCHIQ_IOC_MAGIC,  14, struct vchiq_set_service_option)
+#define VCHIQ_IOC_DUMP_PHYS_MEM \
+	_IOW(VCHIQ_IOC_MAGIC,  15, struct vchiq_dump_mem)
+#define VCHIQ_IOC_LIB_VERSION          _IO(VCHIQ_IOC_MAGIC,   16)
+#define VCHIQ_IOC_CLOSE_DELIVERED      _IO(VCHIQ_IOC_MAGIC,   17)
+#define VCHIQ_IOC_MAX                  17
+
+#endif
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
new file mode 100644
index 000000000..ebd12bfab
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
+
+#ifndef VCHIQ_PAGELIST_H
+#define VCHIQ_PAGELIST_H
+
+#define PAGELIST_WRITE 0
+#define PAGELIST_READ 1
+#define PAGELIST_READ_WITH_FRAGMENTS 2
+
+struct pagelist {
+	u32 length;
+	u16 type;
+	u16 offset;
+	u32 addrs[1];	/* N.B. 12 LSBs hold the number
+			 * of following pages at consecutive
+			 * addresses.
+			 */
+};
+
+#endif /* VCHIQ_PAGELIST_H */
diff --git a/drivers/staging/vc04_services/vchiq-mmal/Kconfig b/drivers/staging/vc04_services/vchiq-mmal/Kconfig
new file mode 100644
index 000000000..c99525a0b
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/Kconfig
@@ -0,0 +1,7 @@
+config BCM2835_VCHIQ_MMAL
+	tristate "BCM2835 MMAL VCHIQ service"
+	depends on BCM2835_VCHIQ
+	help
+	  Enables the MMAL API over VCHIQ interface as used for the
+	  majority of the multimedia services on VideoCore.
+	  Defaults to Y when the Broadcomd BCM2835 camera host is selected.
diff --git a/drivers/staging/vc04_services/vchiq-mmal/Makefile b/drivers/staging/vc04_services/vchiq-mmal/Makefile
new file mode 100644
index 000000000..b2a830f48
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/Makefile
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+bcm2835-mmal-vchiq-objs := mmal-vchiq.o
+
+obj-$(CONFIG_BCM2835_VCHIQ_MMAL) += bcm2835-mmal-vchiq.o
+
+ccflags-y += \
+	-I$(srctree)/$(src)/.. \
+	-I$(srctree)/$(src)/../include \
+	-D__VCCOREVER__=0x04000000
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h
new file mode 100644
index 000000000..b33129403
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ *
+ * MMAL structures
+ *
+ */
+#ifndef MMAL_COMMON_H
+#define MMAL_COMMON_H
+
+#define MMAL_FOURCC(a, b, c, d) ((a) | (b << 8) | (c << 16) | (d << 24))
+#define MMAL_MAGIC MMAL_FOURCC('m', 'm', 'a', 'l')
+
+/** Special value signalling that time is not known */
+#define MMAL_TIME_UNKNOWN BIT_ULL(63)
+
+struct mmal_msg_context;
+
+/* mapping between v4l and mmal video modes */
+struct mmal_fmt {
+	u32 fourcc;          /* v4l2 format id */
+	int flags;           /* v4l2 flags field */
+	u32 mmal;
+	int depth;
+	u32 mmal_component;  /* MMAL component index to be used to encode */
+	u32 ybbp;            /* depth of first Y plane for planar formats */
+	bool remove_padding;   /* Does the GPU have to remove padding,
+				* or can we do hide padding via bytesperline.
+				*/
+};
+
+/* buffer for one video frame */
+struct mmal_buffer {
+	/* v4l buffer data -- must be first */
+	struct vb2_v4l2_buffer vb;
+
+	/* list of buffers available */
+	struct list_head list;
+
+	void *buffer; /* buffer pointer */
+	unsigned long buffer_size; /* size of allocated buffer */
+
+	struct mmal_msg_context *msg_context;
+
+	unsigned long length;
+	u32 mmal_flags;
+	s64 dts;
+	s64 pts;
+};
+
+/* */
+struct mmal_colourfx {
+	s32 enable;
+	u32 u;
+	u32 v;
+};
+#endif
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h
new file mode 100644
index 000000000..e15ae7b24
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h
@@ -0,0 +1,124 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+#ifndef MMAL_ENCODINGS_H
+#define MMAL_ENCODINGS_H
+
+#define MMAL_ENCODING_H264             MMAL_FOURCC('H', '2', '6', '4')
+#define MMAL_ENCODING_H263             MMAL_FOURCC('H', '2', '6', '3')
+#define MMAL_ENCODING_MP4V             MMAL_FOURCC('M', 'P', '4', 'V')
+#define MMAL_ENCODING_MP2V             MMAL_FOURCC('M', 'P', '2', 'V')
+#define MMAL_ENCODING_MP1V             MMAL_FOURCC('M', 'P', '1', 'V')
+#define MMAL_ENCODING_WMV3             MMAL_FOURCC('W', 'M', 'V', '3')
+#define MMAL_ENCODING_WMV2             MMAL_FOURCC('W', 'M', 'V', '2')
+#define MMAL_ENCODING_WMV1             MMAL_FOURCC('W', 'M', 'V', '1')
+#define MMAL_ENCODING_WVC1             MMAL_FOURCC('W', 'V', 'C', '1')
+#define MMAL_ENCODING_VP8              MMAL_FOURCC('V', 'P', '8', ' ')
+#define MMAL_ENCODING_VP7              MMAL_FOURCC('V', 'P', '7', ' ')
+#define MMAL_ENCODING_VP6              MMAL_FOURCC('V', 'P', '6', ' ')
+#define MMAL_ENCODING_THEORA           MMAL_FOURCC('T', 'H', 'E', 'O')
+#define MMAL_ENCODING_SPARK            MMAL_FOURCC('S', 'P', 'R', 'K')
+#define MMAL_ENCODING_MJPEG            MMAL_FOURCC('M', 'J', 'P', 'G')
+
+#define MMAL_ENCODING_JPEG             MMAL_FOURCC('J', 'P', 'E', 'G')
+#define MMAL_ENCODING_GIF              MMAL_FOURCC('G', 'I', 'F', ' ')
+#define MMAL_ENCODING_PNG              MMAL_FOURCC('P', 'N', 'G', ' ')
+#define MMAL_ENCODING_PPM              MMAL_FOURCC('P', 'P', 'M', ' ')
+#define MMAL_ENCODING_TGA              MMAL_FOURCC('T', 'G', 'A', ' ')
+#define MMAL_ENCODING_BMP              MMAL_FOURCC('B', 'M', 'P', ' ')
+
+#define MMAL_ENCODING_I420             MMAL_FOURCC('I', '4', '2', '0')
+#define MMAL_ENCODING_I420_SLICE       MMAL_FOURCC('S', '4', '2', '0')
+#define MMAL_ENCODING_YV12             MMAL_FOURCC('Y', 'V', '1', '2')
+#define MMAL_ENCODING_I422             MMAL_FOURCC('I', '4', '2', '2')
+#define MMAL_ENCODING_I422_SLICE       MMAL_FOURCC('S', '4', '2', '2')
+#define MMAL_ENCODING_YUYV             MMAL_FOURCC('Y', 'U', 'Y', 'V')
+#define MMAL_ENCODING_YVYU             MMAL_FOURCC('Y', 'V', 'Y', 'U')
+#define MMAL_ENCODING_UYVY             MMAL_FOURCC('U', 'Y', 'V', 'Y')
+#define MMAL_ENCODING_VYUY             MMAL_FOURCC('V', 'Y', 'U', 'Y')
+#define MMAL_ENCODING_NV12             MMAL_FOURCC('N', 'V', '1', '2')
+#define MMAL_ENCODING_NV21             MMAL_FOURCC('N', 'V', '2', '1')
+#define MMAL_ENCODING_ARGB             MMAL_FOURCC('A', 'R', 'G', 'B')
+#define MMAL_ENCODING_RGBA             MMAL_FOURCC('R', 'G', 'B', 'A')
+#define MMAL_ENCODING_ABGR             MMAL_FOURCC('A', 'B', 'G', 'R')
+#define MMAL_ENCODING_BGRA             MMAL_FOURCC('B', 'G', 'R', 'A')
+#define MMAL_ENCODING_RGB16            MMAL_FOURCC('R', 'G', 'B', '2')
+#define MMAL_ENCODING_RGB24            MMAL_FOURCC('R', 'G', 'B', '3')
+#define MMAL_ENCODING_RGB32            MMAL_FOURCC('R', 'G', 'B', '4')
+#define MMAL_ENCODING_BGR16            MMAL_FOURCC('B', 'G', 'R', '2')
+#define MMAL_ENCODING_BGR24            MMAL_FOURCC('B', 'G', 'R', '3')
+#define MMAL_ENCODING_BGR32            MMAL_FOURCC('B', 'G', 'R', '4')
+
+/** SAND Video (YUVUV128) format, native format understood by VideoCore.
+ * This format is *not* opaque - if requested you will receive full frames
+ * of YUV_UV video.
+ */
+#define MMAL_ENCODING_YUVUV128         MMAL_FOURCC('S', 'A', 'N', 'D')
+
+/** VideoCore opaque image format, image handles are returned to
+ * the host but not the actual image data.
+ */
+#define MMAL_ENCODING_OPAQUE           MMAL_FOURCC('O', 'P', 'Q', 'V')
+
+/** An EGL image handle
+ */
+#define MMAL_ENCODING_EGL_IMAGE        MMAL_FOURCC('E', 'G', 'L', 'I')
+
+/* }@ */
+
+/** \name Pre-defined audio encodings */
+/* @{ */
+#define MMAL_ENCODING_PCM_UNSIGNED_BE  MMAL_FOURCC('P', 'C', 'M', 'U')
+#define MMAL_ENCODING_PCM_UNSIGNED_LE  MMAL_FOURCC('p', 'c', 'm', 'u')
+#define MMAL_ENCODING_PCM_SIGNED_BE    MMAL_FOURCC('P', 'C', 'M', 'S')
+#define MMAL_ENCODING_PCM_SIGNED_LE    MMAL_FOURCC('p', 'c', 'm', 's')
+#define MMAL_ENCODING_PCM_FLOAT_BE     MMAL_FOURCC('P', 'C', 'M', 'F')
+#define MMAL_ENCODING_PCM_FLOAT_LE     MMAL_FOURCC('p', 'c', 'm', 'f')
+
+/* Pre-defined H264 encoding variants */
+
+/** ISO 14496-10 Annex B byte stream format */
+#define MMAL_ENCODING_VARIANT_H264_DEFAULT   0
+/** ISO 14496-15 AVC stream format */
+#define MMAL_ENCODING_VARIANT_H264_AVC1      MMAL_FOURCC('A', 'V', 'C', '1')
+/** Implicitly delineated NAL units without emulation prevention */
+#define MMAL_ENCODING_VARIANT_H264_RAW       MMAL_FOURCC('R', 'A', 'W', ' ')
+
+/** \defgroup MmalColorSpace List of pre-defined video color spaces
+ * This defines a list of common color spaces. This list isn't exhaustive and
+ * is only provided as a convenience to avoid clients having to use FourCC
+ * codes directly. However components are allowed to define and use their own
+ * FourCC codes.
+ */
+/* @{ */
+
+/** Unknown color space */
+#define MMAL_COLOR_SPACE_UNKNOWN       0
+/** ITU-R BT.601-5 [SDTV] */
+#define MMAL_COLOR_SPACE_ITUR_BT601    MMAL_FOURCC('Y', '6', '0', '1')
+/** ITU-R BT.709-3 [HDTV] */
+#define MMAL_COLOR_SPACE_ITUR_BT709    MMAL_FOURCC('Y', '7', '0', '9')
+/** JPEG JFIF */
+#define MMAL_COLOR_SPACE_JPEG_JFIF     MMAL_FOURCC('Y', 'J', 'F', 'I')
+/** Title 47 Code of Federal Regulations (2003) 73.682 (a) (20) */
+#define MMAL_COLOR_SPACE_FCC           MMAL_FOURCC('Y', 'F', 'C', 'C')
+/** Society of Motion Picture and Television Engineers 240M (1999) */
+#define MMAL_COLOR_SPACE_SMPTE240M     MMAL_FOURCC('Y', '2', '4', '0')
+/** ITU-R BT.470-2 System M */
+#define MMAL_COLOR_SPACE_BT470_2_M     MMAL_FOURCC('Y', '_', '_', 'M')
+/** ITU-R BT.470-2 System BG */
+#define MMAL_COLOR_SPACE_BT470_2_BG    MMAL_FOURCC('Y', '_', 'B', 'G')
+/** JPEG JFIF, but with 16..255 luma */
+#define MMAL_COLOR_SPACE_JFIF_Y16_255  MMAL_FOURCC('Y', 'Y', '1', '6')
+/* @} MmalColorSpace List */
+
+#endif /* MMAL_ENCODINGS_H */
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h
new file mode 100644
index 000000000..492d4c5dc
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+#ifndef MMAL_MSG_COMMON_H
+#define MMAL_MSG_COMMON_H
+
+#include <linux/types.h>
+
+enum mmal_msg_status {
+	MMAL_MSG_STATUS_SUCCESS = 0, /**< Success */
+	MMAL_MSG_STATUS_ENOMEM,      /**< Out of memory */
+	MMAL_MSG_STATUS_ENOSPC,      /**< Out of resources other than memory */
+	MMAL_MSG_STATUS_EINVAL,      /**< Argument is invalid */
+	MMAL_MSG_STATUS_ENOSYS,      /**< Function not implemented */
+	MMAL_MSG_STATUS_ENOENT,      /**< No such file or directory */
+	MMAL_MSG_STATUS_ENXIO,       /**< No such device or address */
+	MMAL_MSG_STATUS_EIO,         /**< I/O error */
+	MMAL_MSG_STATUS_ESPIPE,      /**< Illegal seek */
+	MMAL_MSG_STATUS_ECORRUPT,    /**< Data is corrupt \attention */
+	MMAL_MSG_STATUS_ENOTREADY,   /**< Component is not ready */
+	MMAL_MSG_STATUS_ECONFIG,     /**< Component is not configured */
+	MMAL_MSG_STATUS_EISCONN,     /**< Port is already connected */
+	MMAL_MSG_STATUS_ENOTCONN,    /**< Port is disconnected */
+	MMAL_MSG_STATUS_EAGAIN,      /**< Resource temporarily unavailable. */
+	MMAL_MSG_STATUS_EFAULT,      /**< Bad address */
+};
+
+struct mmal_rect {
+	s32 x;      /**< x coordinate (from left) */
+	s32 y;      /**< y coordinate (from top) */
+	s32 width;  /**< width */
+	s32 height; /**< height */
+};
+
+#endif /* MMAL_MSG_COMMON_H */
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h
new file mode 100644
index 000000000..5569876d8
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+#ifndef MMAL_MSG_FORMAT_H
+#define MMAL_MSG_FORMAT_H
+
+#include <linux/math.h>
+
+#include "mmal-msg-common.h"
+
+/* MMAL_ES_FORMAT_T */
+
+struct mmal_audio_format {
+	u32 channels;		/* Number of audio channels */
+	u32 sample_rate;	/* Sample rate */
+
+	u32 bits_per_sample;	/* Bits per sample */
+	u32 block_align;	/* Size of a block of data */
+};
+
+struct mmal_video_format {
+	u32 width;		/* Width of frame in pixels */
+	u32 height;		/* Height of frame in rows of pixels */
+	struct mmal_rect crop;	/* Visible region of the frame */
+	struct s32_fract frame_rate;	/* Frame rate */
+	struct s32_fract par;		/* Pixel aspect ratio */
+
+	/*
+	 * FourCC specifying the color space of the video stream. See the
+	 * MmalColorSpace "pre-defined color spaces" for some examples.
+	 */
+	u32 color_space;
+};
+
+struct mmal_subpicture_format {
+	u32 x_offset;
+	u32 y_offset;
+};
+
+union mmal_es_specific_format {
+	struct mmal_audio_format audio;
+	struct mmal_video_format video;
+	struct mmal_subpicture_format subpicture;
+};
+
+/* Definition of an elementary stream format (MMAL_ES_FORMAT_T) */
+struct mmal_es_format_local {
+	u32 type;	/* enum mmal_es_type */
+
+	u32 encoding;	/* FourCC specifying encoding of the elementary
+			 * stream.
+			 */
+	u32 encoding_variant;	/* FourCC specifying the specific
+				 * encoding variant of the elementary
+				 * stream.
+				 */
+
+	union mmal_es_specific_format *es;	/* Type specific
+						 * information for the
+						 * elementary stream
+						 */
+
+	u32 bitrate;	/* Bitrate in bits per second */
+	u32 flags;	/* Flags describing properties of the elementary
+			 * stream.
+			 */
+
+	u32 extradata_size;	/* Size of the codec specific data */
+	u8  *extradata;		/* Codec specific data */
+};
+
+/* Remote definition of an elementary stream format (MMAL_ES_FORMAT_T) */
+struct mmal_es_format {
+	u32 type;	/* enum mmal_es_type */
+
+	u32 encoding;	/* FourCC specifying encoding of the elementary
+			 * stream.
+			 */
+	u32 encoding_variant;	/* FourCC specifying the specific
+				 * encoding variant of the elementary
+				 * stream.
+				 */
+
+	u32 es;	/* Type specific
+		 * information for the
+		 * elementary stream
+		 */
+
+	u32 bitrate;	/* Bitrate in bits per second */
+	u32 flags;	/* Flags describing properties of the elementary
+			 * stream.
+			 */
+
+	u32 extradata_size;	/* Size of the codec specific data */
+	u32 extradata;		/* Codec specific data */
+};
+
+#endif /* MMAL_MSG_FORMAT_H */
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h
new file mode 100644
index 000000000..6ee4c1ed7
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h
@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+/* MMAL_PORT_TYPE_T */
+enum mmal_port_type {
+	MMAL_PORT_TYPE_UNKNOWN = 0,	/* Unknown port type */
+	MMAL_PORT_TYPE_CONTROL,		/* Control port */
+	MMAL_PORT_TYPE_INPUT,		/* Input port */
+	MMAL_PORT_TYPE_OUTPUT,		/* Output port */
+	MMAL_PORT_TYPE_CLOCK,		/* Clock port */
+};
+
+/* The port is pass-through and doesn't need buffer headers allocated */
+#define MMAL_PORT_CAPABILITY_PASSTHROUGH                       0x01
+/*
+ *The port wants to allocate the buffer payloads.
+ * This signals a preference that payload allocation should be done
+ * on this port for efficiency reasons.
+ */
+#define MMAL_PORT_CAPABILITY_ALLOCATION                        0x02
+/*
+ * The port supports format change events.
+ * This applies to input ports and is used to let the client know
+ * whether the port supports being reconfigured via a format
+ * change event (i.e. without having to disable the port).
+ */
+#define MMAL_PORT_CAPABILITY_SUPPORTS_EVENT_FORMAT_CHANGE      0x04
+
+/*
+ * mmal port structure (MMAL_PORT_T)
+ *
+ * most elements are informational only, the pointer values for
+ * interogation messages are generally provided as additional
+ * structures within the message. When used to set values only the
+ * buffer_num, buffer_size and userdata parameters are writable.
+ */
+struct mmal_port {
+	u32 priv;	/* Private member used by the framework */
+	u32 name;	/* Port name. Used for debugging purposes (RO) */
+
+	u32 type;	/* Type of the port (RO) enum mmal_port_type */
+	u16 index;	/* Index of the port in its type list (RO) */
+	u16 index_all;	/* Index of the port in the list of all ports (RO) */
+
+	u32 is_enabled;	/* Indicates whether the port is enabled or not (RO) */
+	u32 format;	/* Format of the elementary stream */
+
+	u32 buffer_num_min;	/* Minimum number of buffers the port
+				 *   requires (RO).  This is set by the
+				 *   component.
+				 */
+
+	u32 buffer_size_min;	/* Minimum size of buffers the port
+				 * requires (RO).  This is set by the
+				 * component.
+				 */
+
+	u32 buffer_alignment_min;/* Minimum alignment requirement for
+				  * the buffers (RO).  A value of
+				  * zero means no special alignment
+				  * requirements.  This is set by the
+				  * component.
+				  */
+
+	u32 buffer_num_recommended;	/* Number of buffers the port
+					 * recommends for optimal
+					 * performance (RO).  A value of
+					 * zero means no special
+					 * recommendation.  This is set
+					 * by the component.
+					 */
+
+	u32 buffer_size_recommended;	/* Size of buffers the port
+					 * recommends for optimal
+					 * performance (RO).  A value of
+					 * zero means no special
+					 * recommendation.  This is set
+					 * by the component.
+					 */
+
+	u32 buffer_num;	/* Actual number of buffers the port will use.
+			 * This is set by the client.
+			 */
+
+	u32 buffer_size; /* Actual maximum size of the buffers that
+			  * will be sent to the port. This is set by
+			  * the client.
+			  */
+
+	u32 component;	/* Component this port belongs to (Read Only) */
+
+	u32 userdata;	/* Field reserved for use by the client */
+
+	u32 capabilities;	/* Flags describing the capabilities of a
+				 * port (RO).  Bitwise combination of \ref
+				 * portcapabilities "Port capabilities"
+				 * values.
+				 */
+};
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h
new file mode 100644
index 000000000..471413248
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h
@@ -0,0 +1,406 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+/*
+ * all the data structures which serialise the MMAL protocol. note
+ * these are directly mapped onto the recived message data.
+ *
+ * BEWARE: They seem to *assume* pointers are u32 and that there is no
+ * structure padding!
+ *
+ * NOTE: this implementation uses kernel types to ensure sizes. Rather
+ * than assigning values to enums to force their size the
+ * implementation uses fixed size types and not the enums (though the
+ * comments have the actual enum type
+ */
+#ifndef MMAL_MSG_H
+#define MMAL_MSG_H
+
+#define VC_MMAL_VER 15
+#define VC_MMAL_MIN_VER 10
+
+/* max total message size is 512 bytes */
+#define MMAL_MSG_MAX_SIZE 512
+/* with six 32bit header elements max payload is therefore 488 bytes */
+#define MMAL_MSG_MAX_PAYLOAD 488
+
+#include "mmal-msg-common.h"
+#include "mmal-msg-format.h"
+#include "mmal-msg-port.h"
+#include "mmal-vchiq.h"
+
+enum mmal_msg_type {
+	MMAL_MSG_TYPE_QUIT = 1,
+	MMAL_MSG_TYPE_SERVICE_CLOSED,
+	MMAL_MSG_TYPE_GET_VERSION,
+	MMAL_MSG_TYPE_COMPONENT_CREATE,
+	MMAL_MSG_TYPE_COMPONENT_DESTROY,	/* 5 */
+	MMAL_MSG_TYPE_COMPONENT_ENABLE,
+	MMAL_MSG_TYPE_COMPONENT_DISABLE,
+	MMAL_MSG_TYPE_PORT_INFO_GET,
+	MMAL_MSG_TYPE_PORT_INFO_SET,
+	MMAL_MSG_TYPE_PORT_ACTION,		/* 10 */
+	MMAL_MSG_TYPE_BUFFER_FROM_HOST,
+	MMAL_MSG_TYPE_BUFFER_TO_HOST,
+	MMAL_MSG_TYPE_GET_STATS,
+	MMAL_MSG_TYPE_PORT_PARAMETER_SET,
+	MMAL_MSG_TYPE_PORT_PARAMETER_GET,	/* 15 */
+	MMAL_MSG_TYPE_EVENT_TO_HOST,
+	MMAL_MSG_TYPE_GET_CORE_STATS_FOR_PORT,
+	MMAL_MSG_TYPE_OPAQUE_ALLOCATOR,
+	MMAL_MSG_TYPE_CONSUME_MEM,
+	MMAL_MSG_TYPE_LMK,			/* 20 */
+	MMAL_MSG_TYPE_OPAQUE_ALLOCATOR_DESC,
+	MMAL_MSG_TYPE_DRM_GET_LHS32,
+	MMAL_MSG_TYPE_DRM_GET_TIME,
+	MMAL_MSG_TYPE_BUFFER_FROM_HOST_ZEROLEN,
+	MMAL_MSG_TYPE_PORT_FLUSH,		/* 25 */
+	MMAL_MSG_TYPE_HOST_LOG,
+	MMAL_MSG_TYPE_MSG_LAST
+};
+
+/* port action request messages differ depending on the action type */
+enum mmal_msg_port_action_type {
+	MMAL_MSG_PORT_ACTION_TYPE_UNKNOWN = 0,	/* Unknown action */
+	MMAL_MSG_PORT_ACTION_TYPE_ENABLE,	/* Enable a port */
+	MMAL_MSG_PORT_ACTION_TYPE_DISABLE,	/* Disable a port */
+	MMAL_MSG_PORT_ACTION_TYPE_FLUSH,	/* Flush a port */
+	MMAL_MSG_PORT_ACTION_TYPE_CONNECT,	/* Connect ports */
+	MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT,	/* Disconnect ports */
+	MMAL_MSG_PORT_ACTION_TYPE_SET_REQUIREMENTS, /* Set buffer requirements*/
+};
+
+struct mmal_msg_header {
+	u32 magic;
+	u32 type;	/* enum mmal_msg_type */
+
+	/* Opaque handle to the control service */
+	u32 control_service;
+
+	u32 context;	/* a u32 per message context */
+	u32 status;	/* The status of the vchiq operation */
+	u32 padding;
+};
+
+/* Send from VC to host to report version */
+struct mmal_msg_version {
+	u32 flags;
+	u32 major;
+	u32 minor;
+	u32 minimum;
+};
+
+/* request to VC to create component */
+struct mmal_msg_component_create {
+	u32 client_component;	/* component context */
+	char name[128];
+	u32 pid;		/* For debug */
+};
+
+/* reply from VC to component creation request */
+struct mmal_msg_component_create_reply {
+	u32 status;	/* enum mmal_msg_status - how does this differ to
+			 * the one in the header?
+			 */
+	u32 component_handle; /* VideoCore handle for component */
+	u32 input_num;        /* Number of input ports */
+	u32 output_num;       /* Number of output ports */
+	u32 clock_num;        /* Number of clock ports */
+};
+
+/* request to VC to destroy a component */
+struct mmal_msg_component_destroy {
+	u32 component_handle;
+};
+
+struct mmal_msg_component_destroy_reply {
+	u32 status; /* The component destruction status */
+};
+
+/* request and reply to VC to enable a component */
+struct mmal_msg_component_enable {
+	u32 component_handle;
+};
+
+struct mmal_msg_component_enable_reply {
+	u32 status; /* The component enable status */
+};
+
+/* request and reply to VC to disable a component */
+struct mmal_msg_component_disable {
+	u32 component_handle;
+};
+
+struct mmal_msg_component_disable_reply {
+	u32 status; /* The component disable status */
+};
+
+/* request to VC to get port information */
+struct mmal_msg_port_info_get {
+	u32 component_handle;  /* component handle port is associated with */
+	u32 port_type;         /* enum mmal_msg_port_type */
+	u32 index;             /* port index to query */
+};
+
+/* reply from VC to get port info request */
+struct mmal_msg_port_info_get_reply {
+	u32 status;		/* enum mmal_msg_status */
+	u32 component_handle;	/* component handle port is associated with */
+	u32 port_type;		/* enum mmal_msg_port_type */
+	u32 port_index;		/* port indexed in query */
+	s32 found;		/* unused */
+	u32 port_handle;	/* Handle to use for this port */
+	struct mmal_port port;
+	struct mmal_es_format format; /* elementary stream format */
+	union mmal_es_specific_format es; /* es type specific data */
+	u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE]; /* es extra data */
+};
+
+/* request to VC to set port information */
+struct mmal_msg_port_info_set {
+	u32 component_handle;
+	u32 port_type;		/* enum mmal_msg_port_type */
+	u32 port_index;		/* port indexed in query */
+	struct mmal_port port;
+	struct mmal_es_format format;
+	union mmal_es_specific_format es;
+	u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE];
+};
+
+/* reply from VC to port info set request */
+struct mmal_msg_port_info_set_reply {
+	u32 status;
+	u32 component_handle;	/* component handle port is associated with */
+	u32 port_type;		/* enum mmal_msg_port_type */
+	u32 index;		/* port indexed in query */
+	s32 found;		/* unused */
+	u32 port_handle;	/* Handle to use for this port */
+	struct mmal_port port;
+	struct mmal_es_format format;
+	union mmal_es_specific_format es;
+	u8 extradata[MMAL_FORMAT_EXTRADATA_MAX_SIZE];
+};
+
+/* port action requests that take a mmal_port as a parameter */
+struct mmal_msg_port_action_port {
+	u32 component_handle;
+	u32 port_handle;
+	u32 action;		/* enum mmal_msg_port_action_type */
+	struct mmal_port port;
+};
+
+/* port action requests that take handles as a parameter */
+struct mmal_msg_port_action_handle {
+	u32 component_handle;
+	u32 port_handle;
+	u32 action;		/* enum mmal_msg_port_action_type */
+	u32 connect_component_handle;
+	u32 connect_port_handle;
+};
+
+struct mmal_msg_port_action_reply {
+	u32 status;	/* The port action operation status */
+};
+
+/* MMAL buffer transfer */
+
+/* Size of space reserved in a buffer message for short messages. */
+#define MMAL_VC_SHORT_DATA 128
+
+/* Signals that the current payload is the end of the stream of data */
+#define MMAL_BUFFER_HEADER_FLAG_EOS                    BIT(0)
+/* Signals that the start of the current payload starts a frame */
+#define MMAL_BUFFER_HEADER_FLAG_FRAME_START            BIT(1)
+/* Signals that the end of the current payload ends a frame */
+#define MMAL_BUFFER_HEADER_FLAG_FRAME_END              BIT(2)
+/* Signals that the current payload contains only complete frames (>1) */
+#define MMAL_BUFFER_HEADER_FLAG_FRAME                  \
+	(MMAL_BUFFER_HEADER_FLAG_FRAME_START | \
+	 MMAL_BUFFER_HEADER_FLAG_FRAME_END)
+/* Signals that the current payload is a keyframe (i.e. self decodable) */
+#define MMAL_BUFFER_HEADER_FLAG_KEYFRAME               BIT(3)
+/*
+ * Signals a discontinuity in the stream of data (e.g. after a seek).
+ * Can be used for instance by a decoder to reset its state
+ */
+#define MMAL_BUFFER_HEADER_FLAG_DISCONTINUITY          BIT(4)
+/*
+ * Signals a buffer containing some kind of config data for the component
+ * (e.g. codec config data)
+ */
+#define MMAL_BUFFER_HEADER_FLAG_CONFIG                 BIT(5)
+/* Signals an encrypted payload */
+#define MMAL_BUFFER_HEADER_FLAG_ENCRYPTED              BIT(6)
+/* Signals a buffer containing side information */
+#define MMAL_BUFFER_HEADER_FLAG_CODECSIDEINFO          BIT(7)
+/*
+ * Signals a buffer which is the snapshot/postview image from a stills
+ * capture
+ */
+#define MMAL_BUFFER_HEADER_FLAGS_SNAPSHOT              BIT(8)
+/* Signals a buffer which contains data known to be corrupted */
+#define MMAL_BUFFER_HEADER_FLAG_CORRUPTED              BIT(9)
+/* Signals that a buffer failed to be transmitted */
+#define MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED    BIT(10)
+
+struct mmal_driver_buffer {
+	u32 magic;
+	u32 component_handle;
+	u32 port_handle;
+	u32 client_context;
+};
+
+/* buffer header */
+struct mmal_buffer_header {
+	u32 next;	/* next header */
+	u32 priv;	/* framework private data */
+	u32 cmd;
+	u32 data;
+	u32 alloc_size;
+	u32 length;
+	u32 offset;
+	u32 flags;
+	s64 pts;
+	s64 dts;
+	u32 type;
+	u32 user_data;
+};
+
+struct mmal_buffer_header_type_specific {
+	union {
+		struct {
+		u32 planes;
+		u32 offset[4];
+		u32 pitch[4];
+		u32 flags;
+		} video;
+	} u;
+};
+
+struct mmal_msg_buffer_from_host {
+	/*
+	 *The front 32 bytes of the buffer header are copied
+	 * back to us in the reply to allow for context. This
+	 * area is used to store two mmal_driver_buffer structures to
+	 * allow for multiple concurrent service users.
+	 */
+	/* control data */
+	struct mmal_driver_buffer drvbuf;
+
+	/* referenced control data for passthrough buffer management */
+	struct mmal_driver_buffer drvbuf_ref;
+	struct mmal_buffer_header buffer_header; /* buffer header itself */
+	struct mmal_buffer_header_type_specific buffer_header_type_specific;
+	s32 is_zero_copy;
+	s32 has_reference;
+
+	/* allows short data to be xfered in control message */
+	u32 payload_in_message;
+	u8 short_data[MMAL_VC_SHORT_DATA];
+};
+
+/* port parameter setting */
+
+#define MMAL_WORKER_PORT_PARAMETER_SPACE      96
+
+struct mmal_msg_port_parameter_set {
+	u32 component_handle;	/* component */
+	u32 port_handle;	/* port */
+	u32 id;			/* Parameter ID  */
+	u32 size;		/* Parameter size */
+	u32 value[MMAL_WORKER_PORT_PARAMETER_SPACE];
+};
+
+struct mmal_msg_port_parameter_set_reply {
+	u32 status;	/* enum mmal_msg_status todo: how does this
+			 * differ to the one in the header?
+			 */
+};
+
+/* port parameter getting */
+
+struct mmal_msg_port_parameter_get {
+	u32 component_handle;	/* component */
+	u32 port_handle;	/* port */
+	u32 id;			/* Parameter ID  */
+	u32 size;		/* Parameter size */
+};
+
+struct mmal_msg_port_parameter_get_reply {
+	u32 status;		/* Status of mmal_port_parameter_get call */
+	u32 id;			/* Parameter ID  */
+	u32 size;		/* Parameter size */
+	u32 value[MMAL_WORKER_PORT_PARAMETER_SPACE];
+};
+
+/* event messages */
+#define MMAL_WORKER_EVENT_SPACE 256
+
+struct mmal_msg_event_to_host {
+	u32 client_component;	/* component context */
+
+	u32 port_type;
+	u32 port_num;
+
+	u32 cmd;
+	u32 length;
+	u8 data[MMAL_WORKER_EVENT_SPACE];
+	u32 delayed_buffer;
+};
+
+/* all mmal messages are serialised through this structure */
+struct mmal_msg {
+	/* header */
+	struct mmal_msg_header h;
+	/* payload */
+	union {
+		struct mmal_msg_version version;
+
+		struct mmal_msg_component_create component_create;
+		struct mmal_msg_component_create_reply component_create_reply;
+
+		struct mmal_msg_component_destroy component_destroy;
+		struct mmal_msg_component_destroy_reply component_destroy_reply;
+
+		struct mmal_msg_component_enable component_enable;
+		struct mmal_msg_component_enable_reply component_enable_reply;
+
+		struct mmal_msg_component_disable component_disable;
+		struct mmal_msg_component_disable_reply component_disable_reply;
+
+		struct mmal_msg_port_info_get port_info_get;
+		struct mmal_msg_port_info_get_reply port_info_get_reply;
+
+		struct mmal_msg_port_info_set port_info_set;
+		struct mmal_msg_port_info_set_reply port_info_set_reply;
+
+		struct mmal_msg_port_action_port port_action_port;
+		struct mmal_msg_port_action_handle port_action_handle;
+		struct mmal_msg_port_action_reply port_action_reply;
+
+		struct mmal_msg_buffer_from_host buffer_from_host;
+
+		struct mmal_msg_port_parameter_set port_parameter_set;
+		struct mmal_msg_port_parameter_set_reply
+			port_parameter_set_reply;
+		struct mmal_msg_port_parameter_get
+			port_parameter_get;
+		struct mmal_msg_port_parameter_get_reply
+			port_parameter_get_reply;
+
+		struct mmal_msg_event_to_host event_to_host;
+
+		u8 payload[MMAL_MSG_MAX_PAYLOAD];
+	} u;
+};
+#endif
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
new file mode 100644
index 000000000..a0cdd2810
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
@@ -0,0 +1,752 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ */
+
+/* common parameters */
+
+/** @name Parameter groups
+ * Parameters are divided into groups, and then allocated sequentially within
+ * a group using an enum.
+ * @{
+ */
+
+#ifndef MMAL_PARAMETERS_H
+#define MMAL_PARAMETERS_H
+
+#include <linux/math.h>
+
+/** Common parameter ID group, used with many types of component. */
+#define MMAL_PARAMETER_GROUP_COMMON		(0 << 16)
+/** Camera-specific parameter ID group. */
+#define MMAL_PARAMETER_GROUP_CAMERA		(1 << 16)
+/** Video-specific parameter ID group. */
+#define MMAL_PARAMETER_GROUP_VIDEO		(2 << 16)
+/** Audio-specific parameter ID group. */
+#define MMAL_PARAMETER_GROUP_AUDIO		(3 << 16)
+/** Clock-specific parameter ID group. */
+#define MMAL_PARAMETER_GROUP_CLOCK		(4 << 16)
+/** Miracast-specific parameter ID group. */
+#define MMAL_PARAMETER_GROUP_MIRACAST		(5 << 16)
+
+/* Common parameters */
+enum mmal_parameter_common_type {
+	/**< Never a valid parameter ID */
+	MMAL_PARAMETER_UNUSED = MMAL_PARAMETER_GROUP_COMMON,
+
+		/**< MMAL_PARAMETER_ENCODING_T */
+	MMAL_PARAMETER_SUPPORTED_ENCODINGS,
+		/**< MMAL_PARAMETER_URI_T */
+	MMAL_PARAMETER_URI,
+		/** MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T */
+	MMAL_PARAMETER_CHANGE_EVENT_REQUEST,
+		/** MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_ZERO_COPY,
+		/**< MMAL_PARAMETER_BUFFER_REQUIREMENTS_T */
+	MMAL_PARAMETER_BUFFER_REQUIREMENTS,
+		/**< MMAL_PARAMETER_STATISTICS_T */
+	MMAL_PARAMETER_STATISTICS,
+		/**< MMAL_PARAMETER_CORE_STATISTICS_T */
+	MMAL_PARAMETER_CORE_STATISTICS,
+		/**< MMAL_PARAMETER_MEM_USAGE_T */
+	MMAL_PARAMETER_MEM_USAGE,
+		/**< MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_BUFFER_FLAG_FILTER,
+		/**< MMAL_PARAMETER_SEEK_T */
+	MMAL_PARAMETER_SEEK,
+		/**< MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_POWERMON_ENABLE,
+		/**< MMAL_PARAMETER_LOGGING_T */
+	MMAL_PARAMETER_LOGGING,
+		/**< MMAL_PARAMETER_UINT64_T */
+	MMAL_PARAMETER_SYSTEM_TIME,
+		/**< MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_NO_IMAGE_PADDING,
+};
+
+/* camera parameters */
+
+enum mmal_parameter_camera_type {
+	/* 0 */
+		/** @ref MMAL_PARAMETER_THUMBNAIL_CONFIG_T */
+	MMAL_PARAMETER_THUMBNAIL_CONFIGURATION =
+		MMAL_PARAMETER_GROUP_CAMERA,
+		/**< Unused? */
+	MMAL_PARAMETER_CAPTURE_QUALITY,
+		/**< @ref MMAL_PARAMETER_INT32_T */
+	MMAL_PARAMETER_ROTATION,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_EXIF_DISABLE,
+		/**< @ref MMAL_PARAMETER_EXIF_T */
+	MMAL_PARAMETER_EXIF,
+		/**< @ref MMAL_PARAM_AWBMODE_T */
+	MMAL_PARAMETER_AWB_MODE,
+		/**< @ref MMAL_PARAMETER_IMAGEFX_T */
+	MMAL_PARAMETER_IMAGE_EFFECT,
+		/**< @ref MMAL_PARAMETER_COLOURFX_T */
+	MMAL_PARAMETER_COLOUR_EFFECT,
+		/**< @ref MMAL_PARAMETER_FLICKERAVOID_T */
+	MMAL_PARAMETER_FLICKER_AVOID,
+		/**< @ref MMAL_PARAMETER_FLASH_T */
+	MMAL_PARAMETER_FLASH,
+		/**< @ref MMAL_PARAMETER_REDEYE_T */
+	MMAL_PARAMETER_REDEYE,
+		/**< @ref MMAL_PARAMETER_FOCUS_T */
+	MMAL_PARAMETER_FOCUS,
+		/**< Unused? */
+	MMAL_PARAMETER_FOCAL_LENGTHS,
+		/**< @ref MMAL_PARAMETER_INT32_T */
+	MMAL_PARAMETER_EXPOSURE_COMP,
+		/**< @ref MMAL_PARAMETER_SCALEFACTOR_T */
+	MMAL_PARAMETER_ZOOM,
+		/**< @ref MMAL_PARAMETER_MIRROR_T */
+	MMAL_PARAMETER_MIRROR,
+
+	/* 0x10 */
+		/**< @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_CAMERA_NUM,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_CAPTURE,
+		/**< @ref MMAL_PARAMETER_EXPOSUREMODE_T */
+	MMAL_PARAMETER_EXPOSURE_MODE,
+		/**< @ref MMAL_PARAMETER_EXPOSUREMETERINGMODE_T */
+	MMAL_PARAMETER_EXP_METERING_MODE,
+		/**< @ref MMAL_PARAMETER_FOCUS_STATUS_T */
+	MMAL_PARAMETER_FOCUS_STATUS,
+		/**< @ref MMAL_PARAMETER_CAMERA_CONFIG_T */
+	MMAL_PARAMETER_CAMERA_CONFIG,
+		/**< @ref MMAL_PARAMETER_CAPTURE_STATUS_T */
+	MMAL_PARAMETER_CAPTURE_STATUS,
+		/**< @ref MMAL_PARAMETER_FACE_TRACK_T */
+	MMAL_PARAMETER_FACE_TRACK,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_DRAW_BOX_FACES_AND_FOCUS,
+		/**< @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_JPEG_Q_FACTOR,
+		/**< @ref MMAL_PARAMETER_FRAME_RATE_T */
+	MMAL_PARAMETER_FRAME_RATE,
+		/**< @ref MMAL_PARAMETER_CAMERA_STC_MODE_T */
+	MMAL_PARAMETER_USE_STC,
+		/**< @ref MMAL_PARAMETER_CAMERA_INFO_T */
+	MMAL_PARAMETER_CAMERA_INFO,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_STABILISATION,
+		/**< @ref MMAL_PARAMETER_FACE_TRACK_RESULTS_T */
+	MMAL_PARAMETER_FACE_TRACK_RESULTS,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_ENABLE_RAW_CAPTURE,
+
+	/* 0x20 */
+		/**< @ref MMAL_PARAMETER_URI_T */
+	MMAL_PARAMETER_DPF_FILE,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_ENABLE_DPF_FILE,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_DPF_FAIL_IS_FATAL,
+		/**< @ref MMAL_PARAMETER_CAPTUREMODE_T */
+	MMAL_PARAMETER_CAPTURE_MODE,
+		/**< @ref MMAL_PARAMETER_FOCUS_REGIONS_T */
+	MMAL_PARAMETER_FOCUS_REGIONS,
+		/**< @ref MMAL_PARAMETER_INPUT_CROP_T */
+	MMAL_PARAMETER_INPUT_CROP,
+		/**< @ref MMAL_PARAMETER_SENSOR_INFORMATION_T */
+	MMAL_PARAMETER_SENSOR_INFORMATION,
+		/**< @ref MMAL_PARAMETER_FLASH_SELECT_T */
+	MMAL_PARAMETER_FLASH_SELECT,
+		/**< @ref MMAL_PARAMETER_FIELD_OF_VIEW_T */
+	MMAL_PARAMETER_FIELD_OF_VIEW,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_HIGH_DYNAMIC_RANGE,
+		/**< @ref MMAL_PARAMETER_DRC_T */
+	MMAL_PARAMETER_DYNAMIC_RANGE_COMPRESSION,
+		/**< @ref MMAL_PARAMETER_ALGORITHM_CONTROL_T */
+	MMAL_PARAMETER_ALGORITHM_CONTROL,
+		/**< @ref MMAL_PARAMETER_RATIONAL_T */
+	MMAL_PARAMETER_SHARPNESS,
+		/**< @ref MMAL_PARAMETER_RATIONAL_T */
+	MMAL_PARAMETER_CONTRAST,
+		/**< @ref MMAL_PARAMETER_RATIONAL_T */
+	MMAL_PARAMETER_BRIGHTNESS,
+		/**< @ref MMAL_PARAMETER_RATIONAL_T */
+	MMAL_PARAMETER_SATURATION,
+
+	/* 0x30 */
+		/**< @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_ISO,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_ANTISHAKE,
+		/** @ref MMAL_PARAMETER_IMAGEFX_PARAMETERS_T */
+	MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
+		/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_CAMERA_BURST_CAPTURE,
+		/** @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_CAMERA_MIN_ISO,
+		/** @ref MMAL_PARAMETER_CAMERA_USE_CASE_T */
+	MMAL_PARAMETER_CAMERA_USE_CASE,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_CAPTURE_STATS_PASS,
+		/** @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG,
+		/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_ENABLE_REGISTER_FILE,
+		/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_REGISTER_FAIL_IS_FATAL,
+		/** @ref MMAL_PARAMETER_CONFIGFILE_T */
+	MMAL_PARAMETER_CONFIGFILE_REGISTERS,
+		/** @ref MMAL_PARAMETER_CONFIGFILE_CHUNK_T */
+	MMAL_PARAMETER_CONFIGFILE_CHUNK_REGISTERS,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_JPEG_ATTACH_LOG,
+		/**< @ref MMAL_PARAMETER_ZEROSHUTTERLAG_T */
+	MMAL_PARAMETER_ZERO_SHUTTER_LAG,
+		/**< @ref MMAL_PARAMETER_FPS_RANGE_T */
+	MMAL_PARAMETER_FPS_RANGE,
+		/**< @ref MMAL_PARAMETER_INT32_T */
+	MMAL_PARAMETER_CAPTURE_EXPOSURE_COMP,
+
+	/* 0x40 */
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_SW_SHARPEN_DISABLE,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_FLASH_REQUIRED,
+		/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_SW_SATURATION_DISABLE,
+		/**< Takes a @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_SHUTTER_SPEED,
+		/**< Takes a @ref MMAL_PARAMETER_AWB_GAINS_T */
+	MMAL_PARAMETER_CUSTOM_AWB_GAINS,
+};
+
+enum mmal_parameter_camera_config_timestamp_mode {
+	MMAL_PARAM_TIMESTAMP_MODE_ZERO = 0, /* Always timestamp frames as 0 */
+	MMAL_PARAM_TIMESTAMP_MODE_RAW_STC,  /* Use the raw STC value
+					     * for the frame timestamp
+					     */
+	MMAL_PARAM_TIMESTAMP_MODE_RESET_STC, /* Use the STC timestamp
+					      * but subtract the
+					      * timestamp of the first
+					      * frame sent to give a
+					      * zero based timestamp.
+					      */
+};
+
+struct mmal_parameter_fps_range {
+	/**< Low end of the permitted framerate range */
+	struct s32_fract	fps_low;
+	/**< High end of the permitted framerate range */
+	struct s32_fract	fps_high;
+};
+
+/* camera configuration parameter */
+struct mmal_parameter_camera_config {
+	/* Parameters for setting up the image pools */
+	u32 max_stills_w; /* Max size of stills capture */
+	u32 max_stills_h;
+	u32 stills_yuv422; /* Allow YUV422 stills capture */
+	u32 one_shot_stills; /* Continuous or one shot stills captures. */
+
+	u32 max_preview_video_w; /* Max size of the preview or video
+				  * capture frames
+				  */
+	u32 max_preview_video_h;
+	u32 num_preview_video_frames;
+
+	/** Sets the height of the circular buffer for stills capture. */
+	u32 stills_capture_circular_buffer_height;
+
+	/** Allows preview/encode to resume as fast as possible after the stills
+	 * input frame has been received, and then processes the still frame in
+	 * the background whilst preview/encode has resumed.
+	 * Actual mode is controlled by MMAL_PARAMETER_CAPTURE_MODE.
+	 */
+	u32 fast_preview_resume;
+
+	/** Selects algorithm for timestamping frames if
+	 * there is no clock component connected.
+	 * enum mmal_parameter_camera_config_timestamp_mode
+	 */
+	s32 use_stc_timestamp;
+};
+
+enum mmal_parameter_exposuremode {
+	MMAL_PARAM_EXPOSUREMODE_OFF,
+	MMAL_PARAM_EXPOSUREMODE_AUTO,
+	MMAL_PARAM_EXPOSUREMODE_NIGHT,
+	MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
+	MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
+	MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
+	MMAL_PARAM_EXPOSUREMODE_SPORTS,
+	MMAL_PARAM_EXPOSUREMODE_SNOW,
+	MMAL_PARAM_EXPOSUREMODE_BEACH,
+	MMAL_PARAM_EXPOSUREMODE_VERYLONG,
+	MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
+	MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
+	MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
+};
+
+enum mmal_parameter_exposuremeteringmode {
+	MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE,
+	MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT,
+	MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT,
+	MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX,
+};
+
+enum mmal_parameter_awbmode {
+	MMAL_PARAM_AWBMODE_OFF,
+	MMAL_PARAM_AWBMODE_AUTO,
+	MMAL_PARAM_AWBMODE_SUNLIGHT,
+	MMAL_PARAM_AWBMODE_CLOUDY,
+	MMAL_PARAM_AWBMODE_SHADE,
+	MMAL_PARAM_AWBMODE_TUNGSTEN,
+	MMAL_PARAM_AWBMODE_FLUORESCENT,
+	MMAL_PARAM_AWBMODE_INCANDESCENT,
+	MMAL_PARAM_AWBMODE_FLASH,
+	MMAL_PARAM_AWBMODE_HORIZON,
+};
+
+enum mmal_parameter_imagefx {
+	MMAL_PARAM_IMAGEFX_NONE,
+	MMAL_PARAM_IMAGEFX_NEGATIVE,
+	MMAL_PARAM_IMAGEFX_SOLARIZE,
+	MMAL_PARAM_IMAGEFX_POSTERIZE,
+	MMAL_PARAM_IMAGEFX_WHITEBOARD,
+	MMAL_PARAM_IMAGEFX_BLACKBOARD,
+	MMAL_PARAM_IMAGEFX_SKETCH,
+	MMAL_PARAM_IMAGEFX_DENOISE,
+	MMAL_PARAM_IMAGEFX_EMBOSS,
+	MMAL_PARAM_IMAGEFX_OILPAINT,
+	MMAL_PARAM_IMAGEFX_HATCH,
+	MMAL_PARAM_IMAGEFX_GPEN,
+	MMAL_PARAM_IMAGEFX_PASTEL,
+	MMAL_PARAM_IMAGEFX_WATERCOLOUR,
+	MMAL_PARAM_IMAGEFX_FILM,
+	MMAL_PARAM_IMAGEFX_BLUR,
+	MMAL_PARAM_IMAGEFX_SATURATION,
+	MMAL_PARAM_IMAGEFX_COLOURSWAP,
+	MMAL_PARAM_IMAGEFX_WASHEDOUT,
+	MMAL_PARAM_IMAGEFX_POSTERISE,
+	MMAL_PARAM_IMAGEFX_COLOURPOINT,
+	MMAL_PARAM_IMAGEFX_COLOURBALANCE,
+	MMAL_PARAM_IMAGEFX_CARTOON,
+};
+
+enum MMAL_PARAM_FLICKERAVOID {
+	MMAL_PARAM_FLICKERAVOID_OFF,
+	MMAL_PARAM_FLICKERAVOID_AUTO,
+	MMAL_PARAM_FLICKERAVOID_50HZ,
+	MMAL_PARAM_FLICKERAVOID_60HZ,
+	MMAL_PARAM_FLICKERAVOID_MAX = 0x7FFFFFFF
+};
+
+struct mmal_parameter_awbgains {
+	struct s32_fract r_gain;	/**< Red gain */
+	struct s32_fract b_gain;	/**< Blue gain */
+};
+
+/** Manner of video rate control */
+enum mmal_parameter_rate_control_mode {
+	MMAL_VIDEO_RATECONTROL_DEFAULT,
+	MMAL_VIDEO_RATECONTROL_VARIABLE,
+	MMAL_VIDEO_RATECONTROL_CONSTANT,
+	MMAL_VIDEO_RATECONTROL_VARIABLE_SKIP_FRAMES,
+	MMAL_VIDEO_RATECONTROL_CONSTANT_SKIP_FRAMES
+};
+
+enum mmal_video_profile {
+	MMAL_VIDEO_PROFILE_H263_BASELINE,
+	MMAL_VIDEO_PROFILE_H263_H320CODING,
+	MMAL_VIDEO_PROFILE_H263_BACKWARDCOMPATIBLE,
+	MMAL_VIDEO_PROFILE_H263_ISWV2,
+	MMAL_VIDEO_PROFILE_H263_ISWV3,
+	MMAL_VIDEO_PROFILE_H263_HIGHCOMPRESSION,
+	MMAL_VIDEO_PROFILE_H263_INTERNET,
+	MMAL_VIDEO_PROFILE_H263_INTERLACE,
+	MMAL_VIDEO_PROFILE_H263_HIGHLATENCY,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLESCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_CORE,
+	MMAL_VIDEO_PROFILE_MP4V_MAIN,
+	MMAL_VIDEO_PROFILE_MP4V_NBIT,
+	MMAL_VIDEO_PROFILE_MP4V_SCALABLETEXTURE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLEFACE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLEFBA,
+	MMAL_VIDEO_PROFILE_MP4V_BASICANIMATED,
+	MMAL_VIDEO_PROFILE_MP4V_HYBRID,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDREALTIME,
+	MMAL_VIDEO_PROFILE_MP4V_CORESCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCODING,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCORE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSIMPLE,
+	MMAL_VIDEO_PROFILE_H264_BASELINE,
+	MMAL_VIDEO_PROFILE_H264_MAIN,
+	MMAL_VIDEO_PROFILE_H264_EXTENDED,
+	MMAL_VIDEO_PROFILE_H264_HIGH,
+	MMAL_VIDEO_PROFILE_H264_HIGH10,
+	MMAL_VIDEO_PROFILE_H264_HIGH422,
+	MMAL_VIDEO_PROFILE_H264_HIGH444,
+	MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE,
+	MMAL_VIDEO_PROFILE_DUMMY = 0x7FFFFFFF
+};
+
+enum mmal_video_level {
+	MMAL_VIDEO_LEVEL_H263_10,
+	MMAL_VIDEO_LEVEL_H263_20,
+	MMAL_VIDEO_LEVEL_H263_30,
+	MMAL_VIDEO_LEVEL_H263_40,
+	MMAL_VIDEO_LEVEL_H263_45,
+	MMAL_VIDEO_LEVEL_H263_50,
+	MMAL_VIDEO_LEVEL_H263_60,
+	MMAL_VIDEO_LEVEL_H263_70,
+	MMAL_VIDEO_LEVEL_MP4V_0,
+	MMAL_VIDEO_LEVEL_MP4V_0b,
+	MMAL_VIDEO_LEVEL_MP4V_1,
+	MMAL_VIDEO_LEVEL_MP4V_2,
+	MMAL_VIDEO_LEVEL_MP4V_3,
+	MMAL_VIDEO_LEVEL_MP4V_4,
+	MMAL_VIDEO_LEVEL_MP4V_4a,
+	MMAL_VIDEO_LEVEL_MP4V_5,
+	MMAL_VIDEO_LEVEL_MP4V_6,
+	MMAL_VIDEO_LEVEL_H264_1,
+	MMAL_VIDEO_LEVEL_H264_1b,
+	MMAL_VIDEO_LEVEL_H264_11,
+	MMAL_VIDEO_LEVEL_H264_12,
+	MMAL_VIDEO_LEVEL_H264_13,
+	MMAL_VIDEO_LEVEL_H264_2,
+	MMAL_VIDEO_LEVEL_H264_21,
+	MMAL_VIDEO_LEVEL_H264_22,
+	MMAL_VIDEO_LEVEL_H264_3,
+	MMAL_VIDEO_LEVEL_H264_31,
+	MMAL_VIDEO_LEVEL_H264_32,
+	MMAL_VIDEO_LEVEL_H264_4,
+	MMAL_VIDEO_LEVEL_H264_41,
+	MMAL_VIDEO_LEVEL_H264_42,
+	MMAL_VIDEO_LEVEL_H264_5,
+	MMAL_VIDEO_LEVEL_H264_51,
+	MMAL_VIDEO_LEVEL_DUMMY = 0x7FFFFFFF
+};
+
+struct mmal_parameter_video_profile {
+	enum mmal_video_profile profile;
+	enum mmal_video_level level;
+};
+
+/* video parameters */
+
+enum mmal_parameter_video_type {
+	/** @ref MMAL_DISPLAYREGION_T */
+	MMAL_PARAMETER_DISPLAYREGION = MMAL_PARAMETER_GROUP_VIDEO,
+
+	/** @ref MMAL_PARAMETER_VIDEO_PROFILE_T */
+	MMAL_PARAMETER_SUPPORTED_PROFILES,
+
+	/** @ref MMAL_PARAMETER_VIDEO_PROFILE_T */
+	MMAL_PARAMETER_PROFILE,
+
+	/** @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_INTRAPERIOD,
+
+	/** @ref MMAL_PARAMETER_VIDEO_RATECONTROL_T */
+	MMAL_PARAMETER_RATECONTROL,
+
+	/** @ref MMAL_PARAMETER_VIDEO_NALUNITFORMAT_T */
+	MMAL_PARAMETER_NALUNITFORMAT,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
+
+	/** @ref MMAL_PARAMETER_UINT32_T.
+	 * Setting the value to zero resets to the default (one slice per
+	 * frame).
+	 */
+	MMAL_PARAMETER_MB_ROWS_PER_SLICE,
+
+	/** @ref MMAL_PARAMETER_VIDEO_LEVEL_EXTENSION_T */
+	MMAL_PARAMETER_VIDEO_LEVEL_EXTENSION,
+
+	/** @ref MMAL_PARAMETER_VIDEO_EEDE_ENABLE_T */
+	MMAL_PARAMETER_VIDEO_EEDE_ENABLE,
+
+	/** @ref MMAL_PARAMETER_VIDEO_EEDE_LOSSRATE_T */
+	MMAL_PARAMETER_VIDEO_EEDE_LOSSRATE,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. Request an I-frame. */
+	MMAL_PARAMETER_VIDEO_REQUEST_I_FRAME,
+	/** @ref MMAL_PARAMETER_VIDEO_INTRA_REFRESH_T */
+	MMAL_PARAMETER_VIDEO_INTRA_REFRESH,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. */
+	MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. Run-time bit rate control */
+	MMAL_PARAMETER_VIDEO_BIT_RATE,
+
+	/** @ref MMAL_PARAMETER_FRAME_RATE_T */
+	MMAL_PARAMETER_VIDEO_FRAME_RATE,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_MIN_QUANT,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_MAX_QUANT,
+
+	/** @ref MMAL_PARAMETER_VIDEO_ENCODE_RC_MODEL_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_RC_MODEL,
+
+	MMAL_PARAMETER_EXTRA_BUFFERS, /**< @ref MMAL_PARAMETER_UINT32_T. */
+	/** @ref MMAL_PARAMETER_UINT32_T.
+	 * Changing this parameter from the default can reduce frame rate
+	 * because image buffers need to be re-pitched.
+	 */
+	MMAL_PARAMETER_VIDEO_ALIGN_HORIZ,
+
+	/** @ref MMAL_PARAMETER_UINT32_T.
+	 * Changing this parameter from the default can reduce frame rate
+	 * because image buffers need to be re-pitched.
+	 */
+	MMAL_PARAMETER_VIDEO_ALIGN_VERT,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. */
+	MMAL_PARAMETER_VIDEO_DROPPABLE_PFRAMES,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_INITIAL_QUANT,
+
+	/**< @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_QP_P,
+
+	/**< @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_RC_SLICE_DQUANT,
+
+	/** @ref MMAL_PARAMETER_UINT32_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_FRAME_LIMIT_BITS,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_PEAK_RATE,
+
+	/* H264 specific parameters */
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_DISABLE_CABAC,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_LOW_LATENCY,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_AU_DELIMITERS,
+
+	/** @ref MMAL_PARAMETER_UINT32_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_DEBLOCK_IDC,
+
+	/** @ref MMAL_PARAMETER_VIDEO_ENCODER_H264_MB_INTRA_MODES_T. */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_MB_INTRA_MODE,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_HEADER_ON_OPEN,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_PRECODE_FOR_QP,
+
+	/** @ref MMAL_PARAMETER_VIDEO_DRM_INIT_INFO_T. */
+	MMAL_PARAMETER_VIDEO_DRM_INIT_INFO,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_TIMESTAMP_FIFO,
+
+	/** @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_DECODE_ERROR_CONCEALMENT,
+
+	/** @ref MMAL_PARAMETER_VIDEO_DRM_PROTECT_BUFFER_T. */
+	MMAL_PARAMETER_VIDEO_DRM_PROTECT_BUFFER,
+
+	/** @ref MMAL_PARAMETER_BYTES_T */
+	MMAL_PARAMETER_VIDEO_DECODE_CONFIG_VD3,
+
+	/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_VCL_HRD_PARAMETERS,
+
+	/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_H264_LOW_DELAY_HRD_FLAG,
+
+	/**< @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER
+};
+
+/** Valid mirror modes */
+enum mmal_parameter_mirror {
+	MMAL_PARAM_MIRROR_NONE,
+	MMAL_PARAM_MIRROR_VERTICAL,
+	MMAL_PARAM_MIRROR_HORIZONTAL,
+	MMAL_PARAM_MIRROR_BOTH,
+};
+
+enum mmal_parameter_displaytransform {
+	MMAL_DISPLAY_ROT0 = 0,
+	MMAL_DISPLAY_MIRROR_ROT0 = 1,
+	MMAL_DISPLAY_MIRROR_ROT180 = 2,
+	MMAL_DISPLAY_ROT180 = 3,
+	MMAL_DISPLAY_MIRROR_ROT90 = 4,
+	MMAL_DISPLAY_ROT270 = 5,
+	MMAL_DISPLAY_ROT90 = 6,
+	MMAL_DISPLAY_MIRROR_ROT270 = 7,
+};
+
+enum mmal_parameter_displaymode {
+	MMAL_DISPLAY_MODE_FILL = 0,
+	MMAL_DISPLAY_MODE_LETTERBOX = 1,
+};
+
+enum mmal_parameter_displayset {
+	MMAL_DISPLAY_SET_NONE = 0,
+	MMAL_DISPLAY_SET_NUM = 1,
+	MMAL_DISPLAY_SET_FULLSCREEN = 2,
+	MMAL_DISPLAY_SET_TRANSFORM = 4,
+	MMAL_DISPLAY_SET_DEST_RECT = 8,
+	MMAL_DISPLAY_SET_SRC_RECT = 0x10,
+	MMAL_DISPLAY_SET_MODE = 0x20,
+	MMAL_DISPLAY_SET_PIXEL = 0x40,
+	MMAL_DISPLAY_SET_NOASPECT = 0x80,
+	MMAL_DISPLAY_SET_LAYER = 0x100,
+	MMAL_DISPLAY_SET_COPYPROTECT = 0x200,
+	MMAL_DISPLAY_SET_ALPHA = 0x400,
+};
+
+/* rectangle, used lots so it gets its own struct */
+struct vchiq_mmal_rect {
+	s32 x;
+	s32 y;
+	s32 width;
+	s32 height;
+};
+
+struct mmal_parameter_displayregion {
+	/** Bitfield that indicates which fields are set and should be
+	 * used. All other fields will maintain their current value.
+	 * \ref MMAL_DISPLAYSET_T defines the bits that can be
+	 * combined.
+	 */
+	u32 set;
+
+	/** Describes the display output device, with 0 typically
+	 * being a directly connected LCD display.  The actual values
+	 * will depend on the hardware.  Code using hard-wired numbers
+	 * (e.g. 2) is certain to fail.
+	 */
+
+	u32 display_num;
+	/** Indicates that we are using the full device screen area,
+	 * rather than a window of the display.  If zero, then
+	 * dest_rect is used to specify a region of the display to
+	 * use.
+	 */
+
+	s32 fullscreen;
+	/** Indicates any rotation or flipping used to map frames onto
+	 * the natural display orientation.
+	 */
+	u32 transform; /* enum mmal_parameter_displaytransform */
+
+	/** Where to display the frame within the screen, if
+	 * fullscreen is zero.
+	 */
+	struct vchiq_mmal_rect dest_rect;
+
+	/** Indicates which area of the frame to display. If all
+	 * values are zero, the whole frame will be used.
+	 */
+	struct vchiq_mmal_rect src_rect;
+
+	/** If set to non-zero, indicates that any display scaling
+	 * should disregard the aspect ratio of the frame region being
+	 * displayed.
+	 */
+	s32 noaspect;
+
+	/** Indicates how the image should be scaled to fit the
+	 * display. \code MMAL_DISPLAY_MODE_FILL \endcode indicates
+	 * that the image should fill the screen by potentially
+	 * cropping the frames.  Setting \code mode \endcode to \code
+	 * MMAL_DISPLAY_MODE_LETTERBOX \endcode indicates that all the
+	 * source region should be displayed and black bars added if
+	 * necessary.
+	 */
+	u32 mode; /* enum mmal_parameter_displaymode */
+
+	/** If non-zero, defines the width of a source pixel relative
+	 * to \code pixel_y \endcode.  If zero, then pixels default to
+	 * being square.
+	 */
+	u32 pixel_x;
+
+	/** If non-zero, defines the height of a source pixel relative
+	 * to \code pixel_x \endcode.  If zero, then pixels default to
+	 * being square.
+	 */
+	u32 pixel_y;
+
+	/** Sets the relative depth of the images, with greater values
+	 * being in front of smaller values.
+	 */
+	u32 layer;
+
+	/** Set to non-zero to ensure copy protection is used on
+	 * output.
+	 */
+	s32 copyprotect_required;
+
+	/** Level of opacity of the layer, where zero is fully
+	 * transparent and 255 is fully opaque.
+	 */
+	u32 alpha;
+};
+
+#define MMAL_MAX_IMAGEFX_PARAMETERS 5
+
+struct mmal_parameter_imagefx_parameters {
+	enum mmal_parameter_imagefx effect;
+	u32 num_effect_params;
+	u32 effect_parameter[MMAL_MAX_IMAGEFX_PARAMETERS];
+};
+
+#define MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS 4
+#define MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES 2
+#define MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN 16
+
+struct mmal_parameter_camera_info_camera {
+	u32 port_id;
+	u32 max_width;
+	u32 max_height;
+	u32 lens_present;
+	u8 camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN];
+};
+
+enum mmal_parameter_camera_info_flash_type {
+	/* Make values explicit to ensure they match values in config ini */
+	MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_XENON = 0,
+	MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_LED   = 1,
+	MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_OTHER = 2,
+	MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_MAX = 0x7FFFFFFF
+};
+
+struct mmal_parameter_camera_info_flash {
+	enum mmal_parameter_camera_info_flash_type flash_type;
+};
+
+struct mmal_parameter_camera_info {
+	u32 num_cameras;
+	u32 num_flashes;
+	struct mmal_parameter_camera_info_camera
+		cameras[MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS];
+	struct mmal_parameter_camera_info_flash
+				flashes[MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES];
+};
+
+#endif
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c
new file mode 100644
index 000000000..cb921c949
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c
@@ -0,0 +1,1945 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ *
+ * V4L2 driver MMAL vchiq interface code
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/completion.h>
+#include <linux/vmalloc.h>
+#include <linux/raspberrypi/vchiq.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "mmal-common.h"
+#include "mmal-vchiq.h"
+#include "mmal-msg.h"
+
+/*
+ * maximum number of components supported.
+ * This matches the maximum permitted by default on the VPU
+ */
+#define VCHIQ_MMAL_MAX_COMPONENTS 64
+
+/*
+ * Timeout for synchronous msg responses in seconds.
+ * Helpful to increase this if stopping in the VPU debugger.
+ */
+#define SYNC_MSG_TIMEOUT       3
+
+/*#define FULL_MSG_DUMP 1*/
+
+#ifdef DEBUG
+static const char *const msg_type_names[] = {
+	"UNKNOWN",
+	"QUIT",
+	"SERVICE_CLOSED",
+	"GET_VERSION",
+	"COMPONENT_CREATE",
+	"COMPONENT_DESTROY",
+	"COMPONENT_ENABLE",
+	"COMPONENT_DISABLE",
+	"PORT_INFO_GET",
+	"PORT_INFO_SET",
+	"PORT_ACTION",
+	"BUFFER_FROM_HOST",
+	"BUFFER_TO_HOST",
+	"GET_STATS",
+	"PORT_PARAMETER_SET",
+	"PORT_PARAMETER_GET",
+	"EVENT_TO_HOST",
+	"GET_CORE_STATS_FOR_PORT",
+	"OPAQUE_ALLOCATOR",
+	"CONSUME_MEM",
+	"LMK",
+	"OPAQUE_ALLOCATOR_DESC",
+	"DRM_GET_LHS32",
+	"DRM_GET_TIME",
+	"BUFFER_FROM_HOST_ZEROLEN",
+	"PORT_FLUSH",
+	"HOST_LOG",
+};
+#endif
+
+static const char *const port_action_type_names[] = {
+	"UNKNOWN",
+	"ENABLE",
+	"DISABLE",
+	"FLUSH",
+	"CONNECT",
+	"DISCONNECT",
+	"SET_REQUIREMENTS",
+};
+
+#if defined(DEBUG)
+#if defined(FULL_MSG_DUMP)
+#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)				\
+	do {								\
+		pr_debug(TITLE" type:%s(%d) length:%d\n",		\
+			 msg_type_names[(MSG)->h.type],			\
+			 (MSG)->h.type, (MSG_LEN));			\
+		print_hex_dump(KERN_DEBUG, "<<h: ", DUMP_PREFIX_OFFSET,	\
+			       16, 4, (MSG),				\
+			       sizeof(struct mmal_msg_header), 1);	\
+		print_hex_dump(KERN_DEBUG, "<<p: ", DUMP_PREFIX_OFFSET,	\
+			       16, 4,					\
+			       ((u8 *)(MSG)) + sizeof(struct mmal_msg_header),\
+			       (MSG_LEN) - sizeof(struct mmal_msg_header), 1); \
+	} while (0)
+#else
+#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)				\
+	{								\
+		pr_debug(TITLE" type:%s(%d) length:%d\n",		\
+			 msg_type_names[(MSG)->h.type],			\
+			 (MSG)->h.type, (MSG_LEN));			\
+	}
+#endif
+#else
+#define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)
+#endif
+
+struct vchiq_mmal_instance;
+
+/* normal message context */
+struct mmal_msg_context {
+	struct vchiq_mmal_instance *instance;
+
+	/* Index in the context_map idr so that we can find the
+	 * mmal_msg_context again when servicing the VCHI reply.
+	 */
+	int handle;
+
+	union {
+		struct {
+			/* work struct for buffer_cb callback */
+			struct work_struct work;
+			/* work struct for deferred callback */
+			struct work_struct buffer_to_host_work;
+			/* mmal instance */
+			struct vchiq_mmal_instance *instance;
+			/* mmal port */
+			struct vchiq_mmal_port *port;
+			/* actual buffer used to store bulk reply */
+			struct mmal_buffer *buffer;
+			/* amount of buffer used */
+			unsigned long buffer_used;
+			/* MMAL buffer flags */
+			u32 mmal_flags;
+			/* Presentation and Decode timestamps */
+			s64 pts;
+			s64 dts;
+
+			int status;	/* context status */
+
+		} bulk;		/* bulk data */
+
+		struct {
+			/* message handle to release */
+			struct vchiq_header *msg_handle;
+			/* pointer to received message */
+			struct mmal_msg *msg;
+			/* received message length */
+			u32 msg_len;
+			/* completion upon reply */
+			struct completion cmplt;
+		} sync;		/* synchronous response */
+	} u;
+
+};
+
+struct vchiq_mmal_instance {
+	unsigned int service_handle;
+
+	/* ensure serialised access to service */
+	struct mutex vchiq_mutex;
+
+	struct idr context_map;
+	/* protect accesses to context_map */
+	struct mutex context_map_lock;
+
+	struct vchiq_mmal_component component[VCHIQ_MMAL_MAX_COMPONENTS];
+
+	/* ordered workqueue to process all bulk operations */
+	struct workqueue_struct *bulk_wq;
+
+	/* handle for a vchiq instance */
+	struct vchiq_instance *vchiq_instance;
+};
+
+static struct mmal_msg_context *
+get_msg_context(struct vchiq_mmal_instance *instance)
+{
+	struct mmal_msg_context *msg_context;
+	int handle;
+
+	/* todo: should this be allocated from a pool to avoid kzalloc */
+	msg_context = kzalloc(sizeof(*msg_context), GFP_KERNEL);
+
+	if (!msg_context)
+		return ERR_PTR(-ENOMEM);
+
+	/* Create an ID that will be passed along with our message so
+	 * that when we service the VCHI reply, we can look up what
+	 * message is being replied to.
+	 */
+	mutex_lock(&instance->context_map_lock);
+	handle = idr_alloc(&instance->context_map, msg_context,
+			   0, 0, GFP_KERNEL);
+	mutex_unlock(&instance->context_map_lock);
+
+	if (handle < 0) {
+		kfree(msg_context);
+		return ERR_PTR(handle);
+	}
+
+	msg_context->instance = instance;
+	msg_context->handle = handle;
+
+	return msg_context;
+}
+
+static struct mmal_msg_context *
+lookup_msg_context(struct vchiq_mmal_instance *instance, int handle)
+{
+	return idr_find(&instance->context_map, handle);
+}
+
+static void
+release_msg_context(struct mmal_msg_context *msg_context)
+{
+	struct vchiq_mmal_instance *instance = msg_context->instance;
+
+	mutex_lock(&instance->context_map_lock);
+	idr_remove(&instance->context_map, msg_context->handle);
+	mutex_unlock(&instance->context_map_lock);
+	kfree(msg_context);
+}
+
+/* deals with receipt of event to host message */
+static void event_to_host_cb(struct vchiq_mmal_instance *instance,
+			     struct mmal_msg *msg, u32 msg_len)
+{
+	pr_debug("unhandled event\n");
+	pr_debug("component:%u port type:%d num:%d cmd:0x%x length:%d\n",
+		 msg->u.event_to_host.client_component,
+		 msg->u.event_to_host.port_type,
+		 msg->u.event_to_host.port_num,
+		 msg->u.event_to_host.cmd, msg->u.event_to_host.length);
+}
+
+/* workqueue scheduled callback
+ *
+ * we do this because it is important we do not call any other vchiq
+ * sync calls from witin the message delivery thread
+ */
+static void buffer_work_cb(struct work_struct *work)
+{
+	struct mmal_msg_context *msg_context =
+		container_of(work, struct mmal_msg_context, u.bulk.work);
+	struct mmal_buffer *buffer = msg_context->u.bulk.buffer;
+
+	if (!buffer) {
+		pr_err("%s: ctx: %p, No mmal buffer to pass details\n",
+		       __func__, msg_context);
+		return;
+	}
+
+	buffer->length = msg_context->u.bulk.buffer_used;
+	buffer->mmal_flags = msg_context->u.bulk.mmal_flags;
+	buffer->dts = msg_context->u.bulk.dts;
+	buffer->pts = msg_context->u.bulk.pts;
+
+	atomic_dec(&msg_context->u.bulk.port->buffers_with_vpu);
+
+	msg_context->u.bulk.port->buffer_cb(msg_context->u.bulk.instance,
+					    msg_context->u.bulk.port,
+					    msg_context->u.bulk.status,
+					    msg_context->u.bulk.buffer);
+}
+
+/* workqueue scheduled callback to handle receiving buffers
+ *
+ * VCHI will allow up to 4 bulk receives to be scheduled before blocking.
+ * If we block in the service_callback context then we can't process the
+ * VCHI_CALLBACK_BULK_RECEIVED message that would otherwise allow the blocked
+ * vchiq_bulk_receive() call to complete.
+ */
+static void buffer_to_host_work_cb(struct work_struct *work)
+{
+	struct mmal_msg_context *msg_context =
+		container_of(work, struct mmal_msg_context,
+			     u.bulk.buffer_to_host_work);
+	struct vchiq_mmal_instance *instance = msg_context->instance;
+	unsigned long len = msg_context->u.bulk.buffer_used;
+	int ret;
+
+	if (!len)
+		/* Dummy receive to ensure the buffers remain in order */
+		len = 8;
+	/* queue the bulk submission */
+	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
+	ret = vchiq_bulk_receive(instance->vchiq_instance, instance->service_handle,
+				 msg_context->u.bulk.buffer->buffer,
+				 /* Actual receive needs to be a multiple
+				  * of 4 bytes
+				  */
+				(len + 3) & ~3,
+				msg_context,
+				VCHIQ_BULK_MODE_CALLBACK);
+
+	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
+
+	if (ret != 0)
+		pr_err("%s: ctx: %p, vchiq_bulk_receive failed %d\n",
+		       __func__, msg_context, ret);
+}
+
+/* enqueue a bulk receive for a given message context */
+static int bulk_receive(struct vchiq_mmal_instance *instance,
+			struct mmal_msg *msg,
+			struct mmal_msg_context *msg_context)
+{
+	unsigned long rd_len;
+
+	rd_len = msg->u.buffer_from_host.buffer_header.length;
+
+	if (!msg_context->u.bulk.buffer) {
+		pr_err("bulk.buffer not configured - error in buffer_from_host\n");
+
+		/* todo: this is a serious error, we should never have
+		 * committed a buffer_to_host operation to the mmal
+		 * port without the buffer to back it up (underflow
+		 * handling) and there is no obvious way to deal with
+		 * this - how is the mmal servie going to react when
+		 * we fail to do the xfer and reschedule a buffer when
+		 * it arrives? perhaps a starved flag to indicate a
+		 * waiting bulk receive?
+		 */
+
+		return -EINVAL;
+	}
+
+	/* ensure we do not overrun the available buffer */
+	if (rd_len > msg_context->u.bulk.buffer->buffer_size) {
+		rd_len = msg_context->u.bulk.buffer->buffer_size;
+		pr_warn("short read as not enough receive buffer space\n");
+		/* todo: is this the correct response, what happens to
+		 * the rest of the message data?
+		 */
+	}
+
+	/* store length */
+	msg_context->u.bulk.buffer_used = rd_len;
+	msg_context->u.bulk.dts = msg->u.buffer_from_host.buffer_header.dts;
+	msg_context->u.bulk.pts = msg->u.buffer_from_host.buffer_header.pts;
+
+	queue_work(msg_context->instance->bulk_wq,
+		   &msg_context->u.bulk.buffer_to_host_work);
+
+	return 0;
+}
+
+/* data in message, memcpy from packet into output buffer */
+static int inline_receive(struct vchiq_mmal_instance *instance,
+			  struct mmal_msg *msg,
+			  struct mmal_msg_context *msg_context)
+{
+	memcpy(msg_context->u.bulk.buffer->buffer,
+	       msg->u.buffer_from_host.short_data,
+	       msg->u.buffer_from_host.payload_in_message);
+
+	msg_context->u.bulk.buffer_used =
+	    msg->u.buffer_from_host.payload_in_message;
+
+	return 0;
+}
+
+/* queue the buffer availability with MMAL_MSG_TYPE_BUFFER_FROM_HOST */
+static int
+buffer_from_host(struct vchiq_mmal_instance *instance,
+		 struct vchiq_mmal_port *port, struct mmal_buffer *buf)
+{
+	struct mmal_msg_context *msg_context;
+	struct mmal_msg m;
+	int ret;
+
+	if (!port->enabled)
+		return -EINVAL;
+
+	pr_debug("instance:%u buffer:%p\n", instance->service_handle, buf);
+
+	/* get context */
+	if (!buf->msg_context) {
+		pr_err("%s: msg_context not allocated, buf %p\n", __func__,
+		       buf);
+		return -EINVAL;
+	}
+	msg_context = buf->msg_context;
+
+	/* store bulk message context for when data arrives */
+	msg_context->u.bulk.instance = instance;
+	msg_context->u.bulk.port = port;
+	msg_context->u.bulk.buffer = buf;
+	msg_context->u.bulk.buffer_used = 0;
+
+	/* initialise work structure ready to schedule callback */
+	INIT_WORK(&msg_context->u.bulk.work, buffer_work_cb);
+	INIT_WORK(&msg_context->u.bulk.buffer_to_host_work,
+		  buffer_to_host_work_cb);
+
+	atomic_inc(&port->buffers_with_vpu);
+
+	/* prep the buffer from host message */
+	memset(&m, 0xbc, sizeof(m));	/* just to make debug clearer */
+
+	m.h.type = MMAL_MSG_TYPE_BUFFER_FROM_HOST;
+	m.h.magic = MMAL_MAGIC;
+	m.h.context = msg_context->handle;
+	m.h.status = 0;
+
+	/* drvbuf is our private data passed back */
+	m.u.buffer_from_host.drvbuf.magic = MMAL_MAGIC;
+	m.u.buffer_from_host.drvbuf.component_handle = port->component->handle;
+	m.u.buffer_from_host.drvbuf.port_handle = port->handle;
+	m.u.buffer_from_host.drvbuf.client_context = msg_context->handle;
+
+	/* buffer header */
+	m.u.buffer_from_host.buffer_header.cmd = 0;
+	m.u.buffer_from_host.buffer_header.data =
+		(u32)(unsigned long)buf->buffer;
+	m.u.buffer_from_host.buffer_header.alloc_size = buf->buffer_size;
+	m.u.buffer_from_host.buffer_header.length = 0;	/* nothing used yet */
+	m.u.buffer_from_host.buffer_header.offset = 0;	/* no offset */
+	m.u.buffer_from_host.buffer_header.flags = 0;	/* no flags */
+	m.u.buffer_from_host.buffer_header.pts = MMAL_TIME_UNKNOWN;
+	m.u.buffer_from_host.buffer_header.dts = MMAL_TIME_UNKNOWN;
+
+	/* clear buffer type specific data */
+	memset(&m.u.buffer_from_host.buffer_header_type_specific, 0,
+	       sizeof(m.u.buffer_from_host.buffer_header_type_specific));
+
+	/* no payload in message */
+	m.u.buffer_from_host.payload_in_message = 0;
+
+	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
+
+	ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, &m,
+					 sizeof(struct mmal_msg_header) +
+					 sizeof(m.u.buffer_from_host));
+	if (ret)
+		atomic_dec(&port->buffers_with_vpu);
+
+	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
+
+	return ret;
+}
+
+/* deals with receipt of buffer to host message */
+static void buffer_to_host_cb(struct vchiq_mmal_instance *instance,
+			      struct mmal_msg *msg, u32 msg_len)
+{
+	struct mmal_msg_context *msg_context;
+	u32 handle;
+
+	pr_debug("%s: instance:%p msg:%p msg_len:%d\n",
+		 __func__, instance, msg, msg_len);
+
+	if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) {
+		handle = msg->u.buffer_from_host.drvbuf.client_context;
+		msg_context = lookup_msg_context(instance, handle);
+
+		if (!msg_context) {
+			pr_err("drvbuf.client_context(%u) is invalid\n",
+			       handle);
+			return;
+		}
+	} else {
+		pr_err("MMAL_MSG_TYPE_BUFFER_TO_HOST with bad magic\n");
+		return;
+	}
+
+	msg_context->u.bulk.mmal_flags =
+				msg->u.buffer_from_host.buffer_header.flags;
+
+	if (msg->h.status != MMAL_MSG_STATUS_SUCCESS) {
+		/* message reception had an error */
+		pr_warn("error %d in reply\n", msg->h.status);
+
+		msg_context->u.bulk.status = msg->h.status;
+
+	} else if (msg->u.buffer_from_host.buffer_header.length == 0) {
+		/* empty buffer */
+		if (msg->u.buffer_from_host.buffer_header.flags &
+		    MMAL_BUFFER_HEADER_FLAG_EOS) {
+			msg_context->u.bulk.status =
+			    bulk_receive(instance, msg, msg_context);
+			if (msg_context->u.bulk.status == 0)
+				return;	/* successful bulk submission, bulk
+					 * completion will trigger callback
+					 */
+		} else {
+			/* do callback with empty buffer - not EOS though */
+			msg_context->u.bulk.status = 0;
+			msg_context->u.bulk.buffer_used = 0;
+		}
+	} else if (msg->u.buffer_from_host.payload_in_message == 0) {
+		/* data is not in message, queue a bulk receive */
+		msg_context->u.bulk.status =
+		    bulk_receive(instance, msg, msg_context);
+		if (msg_context->u.bulk.status == 0)
+			return;	/* successful bulk submission, bulk
+				 * completion will trigger callback
+				 */
+
+		/* failed to submit buffer, this will end badly */
+		pr_err("error %d on bulk submission\n",
+		       msg_context->u.bulk.status);
+
+	} else if (msg->u.buffer_from_host.payload_in_message <=
+		   MMAL_VC_SHORT_DATA) {
+		/* data payload within message */
+		msg_context->u.bulk.status = inline_receive(instance, msg,
+							    msg_context);
+	} else {
+		pr_err("message with invalid short payload\n");
+
+		/* signal error */
+		msg_context->u.bulk.status = -EINVAL;
+		msg_context->u.bulk.buffer_used =
+		    msg->u.buffer_from_host.payload_in_message;
+	}
+
+	/* schedule the port callback */
+	schedule_work(&msg_context->u.bulk.work);
+}
+
+static void bulk_receive_cb(struct vchiq_mmal_instance *instance,
+			    struct mmal_msg_context *msg_context)
+{
+	msg_context->u.bulk.status = 0;
+
+	/* schedule the port callback */
+	schedule_work(&msg_context->u.bulk.work);
+}
+
+static void bulk_abort_cb(struct vchiq_mmal_instance *instance,
+			  struct mmal_msg_context *msg_context)
+{
+	pr_err("%s: bulk ABORTED msg_context:%p\n", __func__, msg_context);
+
+	msg_context->u.bulk.status = -EINTR;
+
+	schedule_work(&msg_context->u.bulk.work);
+}
+
+/* incoming event service callback */
+static enum vchiq_status service_callback(struct vchiq_instance *vchiq_instance,
+					  enum vchiq_reason reason,
+					  struct vchiq_header *header,
+					  unsigned int handle, void *bulk_ctx)
+{
+	struct vchiq_mmal_instance *instance = vchiq_get_service_userdata(vchiq_instance, handle);
+	u32 msg_len;
+	struct mmal_msg *msg;
+	struct mmal_msg_context *msg_context;
+
+	if (!instance) {
+		pr_err("Message callback passed NULL instance\n");
+		return VCHIQ_SUCCESS;
+	}
+
+	switch (reason) {
+	case VCHIQ_MESSAGE_AVAILABLE:
+		msg = (void *)header->data;
+		msg_len = header->size;
+
+		DBG_DUMP_MSG(msg, msg_len, "<<< reply message");
+
+		/* handling is different for buffer messages */
+		switch (msg->h.type) {
+		case MMAL_MSG_TYPE_BUFFER_FROM_HOST:
+			vchiq_release_message(vchiq_instance, handle, header);
+			break;
+
+		case MMAL_MSG_TYPE_EVENT_TO_HOST:
+			event_to_host_cb(instance, msg, msg_len);
+			vchiq_release_message(vchiq_instance, handle, header);
+
+			break;
+
+		case MMAL_MSG_TYPE_BUFFER_TO_HOST:
+			buffer_to_host_cb(instance, msg, msg_len);
+			vchiq_release_message(vchiq_instance, handle, header);
+			break;
+
+		default:
+			/* messages dependent on header context to complete */
+			if (!msg->h.context) {
+				pr_err("received message context was null!\n");
+				vchiq_release_message(vchiq_instance, handle, header);
+				break;
+			}
+
+			msg_context = lookup_msg_context(instance,
+							 msg->h.context);
+			if (!msg_context) {
+				pr_err("received invalid message context %u!\n",
+				       msg->h.context);
+				vchiq_release_message(vchiq_instance, handle, header);
+				break;
+			}
+
+			/* fill in context values */
+			msg_context->u.sync.msg_handle = header;
+			msg_context->u.sync.msg = msg;
+			msg_context->u.sync.msg_len = msg_len;
+
+			/* todo: should this check (completion_done()
+			 * == 1) for no one waiting? or do we need a
+			 * flag to tell us the completion has been
+			 * interrupted so we can free the message and
+			 * its context. This probably also solves the
+			 * message arriving after interruption todo
+			 * below
+			 */
+
+			/* complete message so caller knows it happened */
+			complete(&msg_context->u.sync.cmplt);
+			break;
+		}
+
+		break;
+
+	case VCHIQ_BULK_RECEIVE_DONE:
+		bulk_receive_cb(instance, bulk_ctx);
+		break;
+
+	case VCHIQ_BULK_RECEIVE_ABORTED:
+		bulk_abort_cb(instance, bulk_ctx);
+		break;
+
+	case VCHIQ_SERVICE_CLOSED:
+		/* TODO: consider if this requires action if received when
+		 * driver is not explicitly closing the service
+		 */
+		break;
+
+	default:
+		pr_err("Received unhandled message reason %d\n", reason);
+		break;
+	}
+
+	return VCHIQ_SUCCESS;
+}
+
+static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance,
+				     struct mmal_msg *msg,
+				     unsigned int payload_len,
+				     struct mmal_msg **msg_out,
+				     struct vchiq_header **msg_handle)
+{
+	struct mmal_msg_context *msg_context;
+	int ret;
+	unsigned long timeout;
+
+	/* payload size must not cause message to exceed max size */
+	if (payload_len >
+	    (MMAL_MSG_MAX_SIZE - sizeof(struct mmal_msg_header))) {
+		pr_err("payload length %d exceeds max:%d\n", payload_len,
+		       (int)(MMAL_MSG_MAX_SIZE -
+			    sizeof(struct mmal_msg_header)));
+		return -EINVAL;
+	}
+
+	msg_context = get_msg_context(instance);
+	if (IS_ERR(msg_context))
+		return PTR_ERR(msg_context);
+
+	init_completion(&msg_context->u.sync.cmplt);
+
+	msg->h.magic = MMAL_MAGIC;
+	msg->h.context = msg_context->handle;
+	msg->h.status = 0;
+
+	DBG_DUMP_MSG(msg, (sizeof(struct mmal_msg_header) + payload_len),
+		     ">>> sync message");
+
+	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
+
+	ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, msg,
+					 sizeof(struct mmal_msg_header) +
+					 payload_len);
+
+	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
+
+	if (ret) {
+		pr_err("error %d queuing message\n", ret);
+		release_msg_context(msg_context);
+		return ret;
+	}
+
+	timeout = wait_for_completion_timeout(&msg_context->u.sync.cmplt,
+					      SYNC_MSG_TIMEOUT * HZ);
+	if (timeout == 0) {
+		pr_err("timed out waiting for sync completion\n");
+		ret = -ETIME;
+		/* todo: what happens if the message arrives after aborting */
+		release_msg_context(msg_context);
+		return ret;
+	}
+
+	*msg_out = msg_context->u.sync.msg;
+	*msg_handle = msg_context->u.sync.msg_handle;
+	release_msg_context(msg_context);
+
+	return 0;
+}
+
+static void dump_port_info(struct vchiq_mmal_port *port)
+{
+	pr_debug("port handle:0x%x enabled:%d\n", port->handle, port->enabled);
+
+	pr_debug("buffer minimum num:%d size:%d align:%d\n",
+		 port->minimum_buffer.num,
+		 port->minimum_buffer.size, port->minimum_buffer.alignment);
+
+	pr_debug("buffer recommended num:%d size:%d align:%d\n",
+		 port->recommended_buffer.num,
+		 port->recommended_buffer.size,
+		 port->recommended_buffer.alignment);
+
+	pr_debug("buffer current values num:%d size:%d align:%d\n",
+		 port->current_buffer.num,
+		 port->current_buffer.size, port->current_buffer.alignment);
+
+	pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n",
+		 port->format.type,
+		 port->format.encoding, port->format.encoding_variant);
+
+	pr_debug("		    bitrate:%d flags:0x%x\n",
+		 port->format.bitrate, port->format.flags);
+
+	if (port->format.type == MMAL_ES_TYPE_VIDEO) {
+		pr_debug
+		    ("es video format: width:%d height:%d colourspace:0x%x\n",
+		     port->es.video.width, port->es.video.height,
+		     port->es.video.color_space);
+
+		pr_debug("		 : crop xywh %d,%d,%d,%d\n",
+			 port->es.video.crop.x,
+			 port->es.video.crop.y,
+			 port->es.video.crop.width, port->es.video.crop.height);
+		pr_debug("		 : framerate %d/%d  aspect %d/%d\n",
+			 port->es.video.frame_rate.numerator,
+			 port->es.video.frame_rate.denominator,
+			 port->es.video.par.numerator, port->es.video.par.denominator);
+	}
+}
+
+static void port_to_mmal_msg(struct vchiq_mmal_port *port, struct mmal_port *p)
+{
+	/* todo do readonly fields need setting at all? */
+	p->type = port->type;
+	p->index = port->index;
+	p->index_all = 0;
+	p->is_enabled = port->enabled;
+	p->buffer_num_min = port->minimum_buffer.num;
+	p->buffer_size_min = port->minimum_buffer.size;
+	p->buffer_alignment_min = port->minimum_buffer.alignment;
+	p->buffer_num_recommended = port->recommended_buffer.num;
+	p->buffer_size_recommended = port->recommended_buffer.size;
+
+	/* only three writable fields in a port */
+	p->buffer_num = port->current_buffer.num;
+	p->buffer_size = port->current_buffer.size;
+	p->userdata = (u32)(unsigned long)port;
+}
+
+static int port_info_set(struct vchiq_mmal_instance *instance,
+			 struct vchiq_mmal_port *port)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	pr_debug("setting port info port %p\n", port);
+	if (!port)
+		return -1;
+	dump_port_info(port);
+
+	m.h.type = MMAL_MSG_TYPE_PORT_INFO_SET;
+
+	m.u.port_info_set.component_handle = port->component->handle;
+	m.u.port_info_set.port_type = port->type;
+	m.u.port_info_set.port_index = port->index;
+
+	port_to_mmal_msg(port, &m.u.port_info_set.port);
+
+	/* elementary stream format setup */
+	m.u.port_info_set.format.type = port->format.type;
+	m.u.port_info_set.format.encoding = port->format.encoding;
+	m.u.port_info_set.format.encoding_variant =
+	    port->format.encoding_variant;
+	m.u.port_info_set.format.bitrate = port->format.bitrate;
+	m.u.port_info_set.format.flags = port->format.flags;
+
+	memcpy(&m.u.port_info_set.es, &port->es,
+	       sizeof(union mmal_es_specific_format));
+
+	m.u.port_info_set.format.extradata_size = port->format.extradata_size;
+	memcpy(&m.u.port_info_set.extradata, port->format.extradata,
+	       port->format.extradata_size);
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.port_info_set),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_SET) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	/* return operation status */
+	ret = -rmsg->u.port_info_get_reply.status;
+
+	pr_debug("%s:result:%d component:0x%x port:%d\n", __func__, ret,
+		 port->component->handle, port->handle);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* use port info get message to retrieve port information */
+static int port_info_get(struct vchiq_mmal_instance *instance,
+			 struct vchiq_mmal_port *port)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	/* port info time */
+	m.h.type = MMAL_MSG_TYPE_PORT_INFO_GET;
+	m.u.port_info_get.component_handle = port->component->handle;
+	m.u.port_info_get.port_type = port->type;
+	m.u.port_info_get.index = port->index;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.port_info_get),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_GET) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	/* return operation status */
+	ret = -rmsg->u.port_info_get_reply.status;
+	if (ret != MMAL_MSG_STATUS_SUCCESS)
+		goto release_msg;
+
+	if (rmsg->u.port_info_get_reply.port.is_enabled == 0)
+		port->enabled = 0;
+	else
+		port->enabled = 1;
+
+	/* copy the values out of the message */
+	port->handle = rmsg->u.port_info_get_reply.port_handle;
+
+	/* port type and index cached to use on port info set because
+	 * it does not use a port handle
+	 */
+	port->type = rmsg->u.port_info_get_reply.port_type;
+	port->index = rmsg->u.port_info_get_reply.port_index;
+
+	port->minimum_buffer.num =
+	    rmsg->u.port_info_get_reply.port.buffer_num_min;
+	port->minimum_buffer.size =
+	    rmsg->u.port_info_get_reply.port.buffer_size_min;
+	port->minimum_buffer.alignment =
+	    rmsg->u.port_info_get_reply.port.buffer_alignment_min;
+
+	port->recommended_buffer.alignment =
+	    rmsg->u.port_info_get_reply.port.buffer_alignment_min;
+	port->recommended_buffer.num =
+	    rmsg->u.port_info_get_reply.port.buffer_num_recommended;
+
+	port->current_buffer.num = rmsg->u.port_info_get_reply.port.buffer_num;
+	port->current_buffer.size =
+	    rmsg->u.port_info_get_reply.port.buffer_size;
+
+	/* stream format */
+	port->format.type = rmsg->u.port_info_get_reply.format.type;
+	port->format.encoding = rmsg->u.port_info_get_reply.format.encoding;
+	port->format.encoding_variant =
+	    rmsg->u.port_info_get_reply.format.encoding_variant;
+	port->format.bitrate = rmsg->u.port_info_get_reply.format.bitrate;
+	port->format.flags = rmsg->u.port_info_get_reply.format.flags;
+
+	/* elementary stream format */
+	memcpy(&port->es,
+	       &rmsg->u.port_info_get_reply.es,
+	       sizeof(union mmal_es_specific_format));
+	port->format.es = &port->es;
+
+	port->format.extradata_size =
+	    rmsg->u.port_info_get_reply.format.extradata_size;
+	memcpy(port->format.extradata,
+	       rmsg->u.port_info_get_reply.extradata,
+	       port->format.extradata_size);
+
+	pr_debug("received port info\n");
+	dump_port_info(port);
+
+release_msg:
+
+	pr_debug("%s:result:%d component:0x%x port:%d\n",
+		 __func__, ret, port->component->handle, port->handle);
+
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* create component on vc */
+static int create_component(struct vchiq_mmal_instance *instance,
+			    struct vchiq_mmal_component *component,
+			    const char *name)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	/* build component create message */
+	m.h.type = MMAL_MSG_TYPE_COMPONENT_CREATE;
+	m.u.component_create.client_component = component->client_component;
+	strncpy(m.u.component_create.name, name,
+		sizeof(m.u.component_create.name));
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.component_create),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != m.h.type) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.component_create_reply.status;
+	if (ret != MMAL_MSG_STATUS_SUCCESS)
+		goto release_msg;
+
+	/* a valid component response received */
+	component->handle = rmsg->u.component_create_reply.component_handle;
+	component->inputs = rmsg->u.component_create_reply.input_num;
+	component->outputs = rmsg->u.component_create_reply.output_num;
+	component->clocks = rmsg->u.component_create_reply.clock_num;
+
+	pr_debug("Component handle:0x%x in:%d out:%d clock:%d\n",
+		 component->handle,
+		 component->inputs, component->outputs, component->clocks);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* destroys a component on vc */
+static int destroy_component(struct vchiq_mmal_instance *instance,
+			     struct vchiq_mmal_component *component)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_COMPONENT_DESTROY;
+	m.u.component_destroy.component_handle = component->handle;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.component_destroy),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != m.h.type) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.component_destroy_reply.status;
+
+release_msg:
+
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* enable a component on vc */
+static int enable_component(struct vchiq_mmal_instance *instance,
+			    struct vchiq_mmal_component *component)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_COMPONENT_ENABLE;
+	m.u.component_enable.component_handle = component->handle;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.component_enable),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != m.h.type) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.component_enable_reply.status;
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* disable a component on vc */
+static int disable_component(struct vchiq_mmal_instance *instance,
+			     struct vchiq_mmal_component *component)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_COMPONENT_DISABLE;
+	m.u.component_disable.component_handle = component->handle;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.component_disable),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != m.h.type) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.component_disable_reply.status;
+
+release_msg:
+
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* get version of mmal implementation */
+static int get_version(struct vchiq_mmal_instance *instance,
+		       u32 *major_out, u32 *minor_out)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_GET_VERSION;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.version),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != m.h.type) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	*major_out = rmsg->u.version.major;
+	*minor_out = rmsg->u.version.minor;
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* do a port action with a port as a parameter */
+static int port_action_port(struct vchiq_mmal_instance *instance,
+			    struct vchiq_mmal_port *port,
+			    enum mmal_msg_port_action_type action_type)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
+	m.u.port_action_port.component_handle = port->component->handle;
+	m.u.port_action_port.port_handle = port->handle;
+	m.u.port_action_port.action = action_type;
+
+	port_to_mmal_msg(port, &m.u.port_action_port.port);
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.port_action_port),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.port_action_reply.status;
+
+	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d)\n",
+		 __func__,
+		 ret, port->component->handle, port->handle,
+		 port_action_type_names[action_type], action_type);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* do a port action with handles as parameters */
+static int port_action_handle(struct vchiq_mmal_instance *instance,
+			      struct vchiq_mmal_port *port,
+			      enum mmal_msg_port_action_type action_type,
+			      u32 connect_component_handle,
+			      u32 connect_port_handle)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
+
+	m.u.port_action_handle.component_handle = port->component->handle;
+	m.u.port_action_handle.port_handle = port->handle;
+	m.u.port_action_handle.action = action_type;
+
+	m.u.port_action_handle.connect_component_handle =
+	    connect_component_handle;
+	m.u.port_action_handle.connect_port_handle = connect_port_handle;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(m.u.port_action_handle),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.port_action_reply.status;
+
+	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n",
+		 __func__,
+		 ret, port->component->handle, port->handle,
+		 port_action_type_names[action_type],
+		 action_type, connect_component_handle, connect_port_handle);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+static int port_parameter_set(struct vchiq_mmal_instance *instance,
+			      struct vchiq_mmal_port *port,
+			      u32 parameter_id, void *value, u32 value_size)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_SET;
+
+	m.u.port_parameter_set.component_handle = port->component->handle;
+	m.u.port_parameter_set.port_handle = port->handle;
+	m.u.port_parameter_set.id = parameter_id;
+	m.u.port_parameter_set.size = (2 * sizeof(u32)) + value_size;
+	memcpy(&m.u.port_parameter_set.value, value, value_size);
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					(4 * sizeof(u32)) + value_size,
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_SET) {
+		/* got an unexpected message type in reply */
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = -rmsg->u.port_parameter_set_reply.status;
+
+	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n",
+		 __func__,
+		 ret, port->component->handle, port->handle, parameter_id);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+static int port_parameter_get(struct vchiq_mmal_instance *instance,
+			      struct vchiq_mmal_port *port,
+			      u32 parameter_id, void *value, u32 *value_size)
+{
+	int ret;
+	struct mmal_msg m;
+	struct mmal_msg *rmsg;
+	struct vchiq_header *rmsg_handle;
+
+	m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_GET;
+
+	m.u.port_parameter_get.component_handle = port->component->handle;
+	m.u.port_parameter_get.port_handle = port->handle;
+	m.u.port_parameter_get.id = parameter_id;
+	m.u.port_parameter_get.size = (2 * sizeof(u32)) + *value_size;
+
+	ret = send_synchronous_mmal_msg(instance, &m,
+					sizeof(struct
+					       mmal_msg_port_parameter_get),
+					&rmsg, &rmsg_handle);
+	if (ret)
+		return ret;
+
+	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_GET) {
+		/* got an unexpected message type in reply */
+		pr_err("Incorrect reply type %d\n", rmsg->h.type);
+		ret = -EINVAL;
+		goto release_msg;
+	}
+
+	ret = rmsg->u.port_parameter_get_reply.status;
+
+	/* port_parameter_get_reply.size includes the header,
+	 * whilst *value_size doesn't.
+	 */
+	rmsg->u.port_parameter_get_reply.size -= (2 * sizeof(u32));
+
+	if (ret || rmsg->u.port_parameter_get_reply.size > *value_size) {
+		/* Copy only as much as we have space for
+		 * but report true size of parameter
+		 */
+		memcpy(value, &rmsg->u.port_parameter_get_reply.value,
+		       *value_size);
+	} else {
+		memcpy(value, &rmsg->u.port_parameter_get_reply.value,
+		       rmsg->u.port_parameter_get_reply.size);
+	}
+	/* Always report the size of the returned parameter to the caller */
+	*value_size = rmsg->u.port_parameter_get_reply.size;
+
+	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", __func__,
+		 ret, port->component->handle, port->handle, parameter_id);
+
+release_msg:
+	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
+
+	return ret;
+}
+
+/* disables a port and drains buffers from it */
+static int port_disable(struct vchiq_mmal_instance *instance,
+			struct vchiq_mmal_port *port)
+{
+	int ret;
+	struct list_head *q, *buf_head;
+	unsigned long flags = 0;
+
+	if (!port->enabled)
+		return 0;
+
+	port->enabled = 0;
+
+	ret = port_action_port(instance, port,
+			       MMAL_MSG_PORT_ACTION_TYPE_DISABLE);
+	if (ret == 0) {
+		/*
+		 * Drain all queued buffers on port. This should only
+		 * apply to buffers that have been queued before the port
+		 * has been enabled. If the port has been enabled and buffers
+		 * passed, then the buffers should have been removed from this
+		 * list, and we should get the relevant callbacks via VCHIQ
+		 * to release the buffers.
+		 */
+		spin_lock_irqsave(&port->slock, flags);
+
+		list_for_each_safe(buf_head, q, &port->buffers) {
+			struct mmal_buffer *mmalbuf;
+
+			mmalbuf = list_entry(buf_head, struct mmal_buffer,
+					     list);
+			list_del(buf_head);
+			if (port->buffer_cb) {
+				mmalbuf->length = 0;
+				mmalbuf->mmal_flags = 0;
+				mmalbuf->dts = MMAL_TIME_UNKNOWN;
+				mmalbuf->pts = MMAL_TIME_UNKNOWN;
+				port->buffer_cb(instance,
+						port, 0, mmalbuf);
+			}
+		}
+
+		spin_unlock_irqrestore(&port->slock, flags);
+
+		ret = port_info_get(instance, port);
+	}
+
+	return ret;
+}
+
+/* enable a port */
+static int port_enable(struct vchiq_mmal_instance *instance,
+		       struct vchiq_mmal_port *port)
+{
+	unsigned int hdr_count;
+	struct list_head *q, *buf_head;
+	int ret;
+
+	if (port->enabled)
+		return 0;
+
+	ret = port_action_port(instance, port,
+			       MMAL_MSG_PORT_ACTION_TYPE_ENABLE);
+	if (ret)
+		goto done;
+
+	port->enabled = 1;
+
+	if (port->buffer_cb) {
+		/* send buffer headers to videocore */
+		hdr_count = 1;
+		list_for_each_safe(buf_head, q, &port->buffers) {
+			struct mmal_buffer *mmalbuf;
+
+			mmalbuf = list_entry(buf_head, struct mmal_buffer,
+					     list);
+			ret = buffer_from_host(instance, port, mmalbuf);
+			if (ret)
+				goto done;
+
+			list_del(buf_head);
+			hdr_count++;
+			if (hdr_count > port->current_buffer.num)
+				break;
+		}
+	}
+
+	ret = port_info_get(instance, port);
+
+done:
+	return ret;
+}
+
+/* ------------------------------------------------------------------
+ * Exported API
+ *------------------------------------------------------------------
+ */
+
+int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance,
+			       struct vchiq_mmal_port *port)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	ret = port_info_set(instance, port);
+	if (ret)
+		goto release_unlock;
+
+	/* read what has actually been set */
+	ret = port_info_get(instance, port);
+
+release_unlock:
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_set_format);
+
+int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance,
+				  struct vchiq_mmal_port *port,
+				  u32 parameter, void *value, u32 value_size)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	ret = port_parameter_set(instance, port, parameter, value, value_size);
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_set);
+
+int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance,
+				  struct vchiq_mmal_port *port,
+				  u32 parameter, void *value, u32 *value_size)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	ret = port_parameter_get(instance, port, parameter, value, value_size);
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_get);
+
+/* enable a port
+ *
+ * enables a port and queues buffers for satisfying callbacks if we
+ * provide a callback handler
+ */
+int vchiq_mmal_port_enable(struct vchiq_mmal_instance *instance,
+			   struct vchiq_mmal_port *port,
+			   vchiq_mmal_buffer_cb buffer_cb)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	/* already enabled - noop */
+	if (port->enabled) {
+		ret = 0;
+		goto unlock;
+	}
+
+	port->buffer_cb = buffer_cb;
+
+	ret = port_enable(instance, port);
+
+unlock:
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_enable);
+
+int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance,
+			    struct vchiq_mmal_port *port)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	if (!port->enabled) {
+		mutex_unlock(&instance->vchiq_mutex);
+		return 0;
+	}
+
+	ret = port_disable(instance, port);
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_disable);
+
+/* ports will be connected in a tunneled manner so data buffers
+ * are not handled by client.
+ */
+int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
+				   struct vchiq_mmal_port *src,
+				   struct vchiq_mmal_port *dst)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	/* disconnect ports if connected */
+	if (src->connected) {
+		ret = port_disable(instance, src);
+		if (ret) {
+			pr_err("failed disabling src port(%d)\n", ret);
+			goto release_unlock;
+		}
+
+		/* do not need to disable the destination port as they
+		 * are connected and it is done automatically
+		 */
+
+		ret = port_action_handle(instance, src,
+					 MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT,
+					 src->connected->component->handle,
+					 src->connected->handle);
+		if (ret < 0) {
+			pr_err("failed disconnecting src port\n");
+			goto release_unlock;
+		}
+		src->connected->enabled = 0;
+		src->connected = NULL;
+	}
+
+	if (!dst) {
+		/* do not make new connection */
+		ret = 0;
+		pr_debug("not making new connection\n");
+		goto release_unlock;
+	}
+
+	/* copy src port format to dst */
+	dst->format.encoding = src->format.encoding;
+	dst->es.video.width = src->es.video.width;
+	dst->es.video.height = src->es.video.height;
+	dst->es.video.crop.x = src->es.video.crop.x;
+	dst->es.video.crop.y = src->es.video.crop.y;
+	dst->es.video.crop.width = src->es.video.crop.width;
+	dst->es.video.crop.height = src->es.video.crop.height;
+	dst->es.video.frame_rate.numerator = src->es.video.frame_rate.numerator;
+	dst->es.video.frame_rate.denominator = src->es.video.frame_rate.denominator;
+
+	/* set new format */
+	ret = port_info_set(instance, dst);
+	if (ret) {
+		pr_debug("setting port info failed\n");
+		goto release_unlock;
+	}
+
+	/* read what has actually been set */
+	ret = port_info_get(instance, dst);
+	if (ret) {
+		pr_debug("read back port info failed\n");
+		goto release_unlock;
+	}
+
+	/* connect two ports together */
+	ret = port_action_handle(instance, src,
+				 MMAL_MSG_PORT_ACTION_TYPE_CONNECT,
+				 dst->component->handle, dst->handle);
+	if (ret < 0) {
+		pr_debug("connecting port %d:%d to %d:%d failed\n",
+			 src->component->handle, src->handle,
+			 dst->component->handle, dst->handle);
+		goto release_unlock;
+	}
+	src->connected = dst;
+
+release_unlock:
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_port_connect_tunnel);
+
+int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance,
+			     struct vchiq_mmal_port *port,
+			     struct mmal_buffer *buffer)
+{
+	unsigned long flags = 0;
+	int ret;
+
+	ret = buffer_from_host(instance, port, buffer);
+	if (ret == -EINVAL) {
+		/* Port is disabled. Queue for when it is enabled. */
+		spin_lock_irqsave(&port->slock, flags);
+		list_add_tail(&buffer->list, &port->buffers);
+		spin_unlock_irqrestore(&port->slock, flags);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_submit_buffer);
+
+int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance,
+			  struct mmal_buffer *buf)
+{
+	struct mmal_msg_context *msg_context = get_msg_context(instance);
+
+	if (IS_ERR(msg_context))
+		return (PTR_ERR(msg_context));
+
+	buf->msg_context = msg_context;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mmal_vchi_buffer_init);
+
+int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf)
+{
+	struct mmal_msg_context *msg_context = buf->msg_context;
+
+	if (msg_context)
+		release_msg_context(msg_context);
+	buf->msg_context = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mmal_vchi_buffer_cleanup);
+
+/* Initialise a mmal component and its ports
+ *
+ */
+int vchiq_mmal_component_init(struct vchiq_mmal_instance *instance,
+			      const char *name,
+			      struct vchiq_mmal_component **component_out)
+{
+	int ret;
+	int idx;		/* port index */
+	struct vchiq_mmal_component *component = NULL;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	for (idx = 0; idx < VCHIQ_MMAL_MAX_COMPONENTS; idx++) {
+		if (!instance->component[idx].in_use) {
+			component = &instance->component[idx];
+			component->in_use = 1;
+			break;
+		}
+	}
+
+	if (!component) {
+		ret = -EINVAL;	/* todo is this correct error? */
+		goto unlock;
+	}
+
+	/* We need a handle to reference back to our component structure.
+	 * Use the array index in instance->component rather than rolling
+	 * another IDR.
+	 */
+	component->client_component = idx;
+
+	ret = create_component(instance, component, name);
+	if (ret < 0) {
+		pr_err("%s: failed to create component %d (Not enough GPU mem?)\n",
+		       __func__, ret);
+		goto unlock;
+	}
+
+	/* ports info needs gathering */
+	component->control.type = MMAL_PORT_TYPE_CONTROL;
+	component->control.index = 0;
+	component->control.component = component;
+	spin_lock_init(&component->control.slock);
+	INIT_LIST_HEAD(&component->control.buffers);
+	ret = port_info_get(instance, &component->control);
+	if (ret < 0)
+		goto release_component;
+
+	for (idx = 0; idx < component->inputs; idx++) {
+		component->input[idx].type = MMAL_PORT_TYPE_INPUT;
+		component->input[idx].index = idx;
+		component->input[idx].component = component;
+		spin_lock_init(&component->input[idx].slock);
+		INIT_LIST_HEAD(&component->input[idx].buffers);
+		ret = port_info_get(instance, &component->input[idx]);
+		if (ret < 0)
+			goto release_component;
+	}
+
+	for (idx = 0; idx < component->outputs; idx++) {
+		component->output[idx].type = MMAL_PORT_TYPE_OUTPUT;
+		component->output[idx].index = idx;
+		component->output[idx].component = component;
+		spin_lock_init(&component->output[idx].slock);
+		INIT_LIST_HEAD(&component->output[idx].buffers);
+		ret = port_info_get(instance, &component->output[idx]);
+		if (ret < 0)
+			goto release_component;
+	}
+
+	for (idx = 0; idx < component->clocks; idx++) {
+		component->clock[idx].type = MMAL_PORT_TYPE_CLOCK;
+		component->clock[idx].index = idx;
+		component->clock[idx].component = component;
+		spin_lock_init(&component->clock[idx].slock);
+		INIT_LIST_HEAD(&component->clock[idx].buffers);
+		ret = port_info_get(instance, &component->clock[idx]);
+		if (ret < 0)
+			goto release_component;
+	}
+
+	*component_out = component;
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return 0;
+
+release_component:
+	destroy_component(instance, component);
+unlock:
+	if (component)
+		component->in_use = 0;
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_component_init);
+
+/*
+ * cause a mmal component to be destroyed
+ */
+int vchiq_mmal_component_finalise(struct vchiq_mmal_instance *instance,
+				  struct vchiq_mmal_component *component)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	if (component->enabled)
+		ret = disable_component(instance, component);
+
+	ret = destroy_component(instance, component);
+
+	component->in_use = 0;
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_component_finalise);
+
+/*
+ * cause a mmal component to be enabled
+ */
+int vchiq_mmal_component_enable(struct vchiq_mmal_instance *instance,
+				struct vchiq_mmal_component *component)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	if (component->enabled) {
+		mutex_unlock(&instance->vchiq_mutex);
+		return 0;
+	}
+
+	ret = enable_component(instance, component);
+	if (ret == 0)
+		component->enabled = true;
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_component_enable);
+
+/*
+ * cause a mmal component to be enabled
+ */
+int vchiq_mmal_component_disable(struct vchiq_mmal_instance *instance,
+				 struct vchiq_mmal_component *component)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	if (!component->enabled) {
+		mutex_unlock(&instance->vchiq_mutex);
+		return 0;
+	}
+
+	ret = disable_component(instance, component);
+	if (ret == 0)
+		component->enabled = 0;
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_component_disable);
+
+int vchiq_mmal_version(struct vchiq_mmal_instance *instance,
+		       u32 *major_out, u32 *minor_out)
+{
+	int ret;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	ret = get_version(instance, major_out, minor_out);
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_version);
+
+int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance)
+{
+	int status = 0;
+
+	if (!instance)
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&instance->vchiq_mutex))
+		return -EINTR;
+
+	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
+
+	status = vchiq_close_service(instance->vchiq_instance, instance->service_handle);
+	if (status != 0)
+		pr_err("mmal-vchiq: VCHIQ close failed\n");
+
+	mutex_unlock(&instance->vchiq_mutex);
+
+	vchiq_shutdown(instance->vchiq_instance);
+	destroy_workqueue(instance->bulk_wq);
+
+	idr_destroy(&instance->context_map);
+
+	kfree(instance);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_finalise);
+
+int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance)
+{
+	int status;
+	int err = -ENODEV;
+	struct vchiq_mmal_instance *instance;
+	struct vchiq_instance *vchiq_instance;
+	struct vchiq_service_params_kernel params = {
+		.version		= VC_MMAL_VER,
+		.version_min		= VC_MMAL_MIN_VER,
+		.fourcc			= VCHIQ_MAKE_FOURCC('m', 'm', 'a', 'l'),
+		.callback		= service_callback,
+		.userdata		= NULL,
+	};
+
+	/* compile time checks to ensure structure size as they are
+	 * directly (de)serialised from memory.
+	 */
+
+	/* ensure the header structure has packed to the correct size */
+	BUILD_BUG_ON(sizeof(struct mmal_msg_header) != 24);
+
+	/* ensure message structure does not exceed maximum length */
+	BUILD_BUG_ON(sizeof(struct mmal_msg) > MMAL_MSG_MAX_SIZE);
+
+	/* mmal port struct is correct size */
+	BUILD_BUG_ON(sizeof(struct mmal_port) != 64);
+
+	/* create a vchi instance */
+	status = vchiq_initialise(&vchiq_instance);
+	if (status) {
+		pr_err("Failed to initialise VCHI instance (status=%d)\n",
+		       status);
+		return -EIO;
+	}
+
+	status = vchiq_connect(vchiq_instance);
+	if (status) {
+		pr_err("Failed to connect VCHI instance (status=%d)\n", status);
+		err = -EIO;
+		goto err_shutdown_vchiq;
+	}
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+
+	if (!instance) {
+		err = -ENOMEM;
+		goto err_shutdown_vchiq;
+	}
+
+	mutex_init(&instance->vchiq_mutex);
+
+	instance->vchiq_instance = vchiq_instance;
+
+	mutex_init(&instance->context_map_lock);
+	idr_init_base(&instance->context_map, 1);
+
+	params.userdata = instance;
+
+	instance->bulk_wq = alloc_ordered_workqueue("mmal-vchiq",
+						    WQ_MEM_RECLAIM);
+	if (!instance->bulk_wq)
+		goto err_free;
+
+	status = vchiq_open_service(vchiq_instance, &params,
+				    &instance->service_handle);
+	if (status) {
+		pr_err("Failed to open VCHI service connection (status=%d)\n",
+		       status);
+		goto err_close_services;
+	}
+
+	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
+
+	*out_instance = instance;
+
+	return 0;
+
+err_close_services:
+	vchiq_close_service(instance->vchiq_instance, instance->service_handle);
+	destroy_workqueue(instance->bulk_wq);
+err_free:
+	kfree(instance);
+err_shutdown_vchiq:
+	vchiq_shutdown(vchiq_instance);
+	return err;
+}
+EXPORT_SYMBOL_GPL(vchiq_mmal_init);
+
+MODULE_DESCRIPTION("BCM2835 MMAL VCHIQ interface");
+MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h
new file mode 100644
index 000000000..6006e2923
--- /dev/null
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h
@@ -0,0 +1,168 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Broadcom BCM2835 V4L2 driver
+ *
+ * Copyright © 2013 Raspberry Pi (Trading) Ltd.
+ *
+ * Authors: Vincent Sanders @ Collabora
+ *          Dave Stevenson @ Broadcom
+ *		(now dave.stevenson@raspberrypi.org)
+ *          Simon Mellor @ Broadcom
+ *          Luke Diamand @ Broadcom
+ *
+ * MMAL interface to VCHIQ message passing
+ */
+
+#ifndef MMAL_VCHIQ_H
+#define MMAL_VCHIQ_H
+
+#include "mmal-common.h"
+#include "mmal-msg-format.h"
+
+#define MAX_PORT_COUNT 4
+
+/* Maximum size of the format extradata. */
+#define MMAL_FORMAT_EXTRADATA_MAX_SIZE 128
+
+struct vchiq_mmal_instance;
+
+enum vchiq_mmal_es_type {
+	MMAL_ES_TYPE_UNKNOWN,     /**< Unknown elementary stream type */
+	MMAL_ES_TYPE_CONTROL,     /**< Elementary stream of control commands */
+	MMAL_ES_TYPE_AUDIO,       /**< Audio elementary stream */
+	MMAL_ES_TYPE_VIDEO,       /**< Video elementary stream */
+	MMAL_ES_TYPE_SUBPICTURE   /**< Sub-picture elementary stream */
+};
+
+struct vchiq_mmal_port_buffer {
+	unsigned int num; /* number of buffers */
+	u32 size; /* size of buffers */
+	u32 alignment; /* alignment of buffers */
+};
+
+struct vchiq_mmal_port;
+
+typedef void (*vchiq_mmal_buffer_cb)(
+		struct vchiq_mmal_instance  *instance,
+		struct vchiq_mmal_port *port,
+		int status, struct mmal_buffer *buffer);
+
+struct vchiq_mmal_port {
+	u32 enabled:1;
+	u32 handle;
+	u32 type; /* port type, cached to use on port info set */
+	u32 index; /* port index, cached to use on port info set */
+
+	/* component port belongs to, allows simple deref */
+	struct vchiq_mmal_component *component;
+
+	struct vchiq_mmal_port *connected; /* port connected to */
+
+	/* buffer info */
+	struct vchiq_mmal_port_buffer minimum_buffer;
+	struct vchiq_mmal_port_buffer recommended_buffer;
+	struct vchiq_mmal_port_buffer current_buffer;
+
+	/* stream format */
+	struct mmal_es_format_local format;
+	/* elementary stream format */
+	union mmal_es_specific_format es;
+
+	/* data buffers to fill */
+	struct list_head buffers;
+	/* lock to serialise adding and removing buffers from list */
+	spinlock_t slock;
+
+	/* Count of buffers the VPU has yet to return */
+	atomic_t buffers_with_vpu;
+	/* callback on buffer completion */
+	vchiq_mmal_buffer_cb buffer_cb;
+	/* callback context */
+	void *cb_ctx;
+};
+
+struct vchiq_mmal_component {
+	u32 in_use:1;
+	u32 enabled:1;
+	u32 handle;  /* VideoCore handle for component */
+	u32 inputs;  /* Number of input ports */
+	u32 outputs; /* Number of output ports */
+	u32 clocks;  /* Number of clock ports */
+	struct vchiq_mmal_port control; /* control port */
+	struct vchiq_mmal_port input[MAX_PORT_COUNT]; /* input ports */
+	struct vchiq_mmal_port output[MAX_PORT_COUNT]; /* output ports */
+	struct vchiq_mmal_port clock[MAX_PORT_COUNT]; /* clock ports */
+	u32 client_component;	/* Used to ref back to client struct */
+};
+
+int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance);
+int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance);
+
+/* Initialise a mmal component and its ports
+ *
+ */
+int vchiq_mmal_component_init(
+		struct vchiq_mmal_instance *instance,
+		const char *name,
+		struct vchiq_mmal_component **component_out);
+
+int vchiq_mmal_component_finalise(
+		struct vchiq_mmal_instance *instance,
+		struct vchiq_mmal_component *component);
+
+int vchiq_mmal_component_enable(
+		struct vchiq_mmal_instance *instance,
+		struct vchiq_mmal_component *component);
+
+int vchiq_mmal_component_disable(
+		struct vchiq_mmal_instance *instance,
+		struct vchiq_mmal_component *component);
+
+/* enable a mmal port
+ *
+ * enables a port and if a buffer callback provided enque buffer
+ * headers as appropriate for the port.
+ */
+int vchiq_mmal_port_enable(
+		struct vchiq_mmal_instance *instance,
+		struct vchiq_mmal_port *port,
+		vchiq_mmal_buffer_cb buffer_cb);
+
+/* disable a port
+ *
+ * disable a port will dequeue any pending buffers
+ */
+int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance,
+			    struct vchiq_mmal_port *port);
+
+int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance,
+				  struct vchiq_mmal_port *port,
+				  u32 parameter,
+				  void *value,
+				  u32 value_size);
+
+int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance,
+				  struct vchiq_mmal_port *port,
+				  u32 parameter,
+				  void *value,
+				  u32 *value_size);
+
+int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance,
+			       struct vchiq_mmal_port *port);
+
+int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
+				   struct vchiq_mmal_port *src,
+				   struct vchiq_mmal_port *dst);
+
+int vchiq_mmal_version(struct vchiq_mmal_instance *instance,
+		       u32 *major_out,
+		       u32 *minor_out);
+
+int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance,
+			     struct vchiq_mmal_port *port,
+			     struct mmal_buffer *buf);
+
+int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance,
+			  struct mmal_buffer *buf);
+int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf);
+#endif /* MMAL_VCHIQ_H */
diff --git a/drivers/staging/vme_user/Kconfig b/drivers/staging/vme_user/Kconfig
new file mode 100644
index 000000000..c8eabf8f4
--- /dev/null
+++ b/drivers/staging/vme_user/Kconfig
@@ -0,0 +1,40 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig VME_BUS
+	bool "VME bridge support"
+	depends on STAGING && PCI
+	help
+	  If you say Y here you get support for the VME bridge Framework.
+
+if VME_BUS
+
+comment "VME Bridge Drivers"
+
+config VME_TSI148
+	tristate "Tempe"
+	depends on HAS_DMA
+	help
+	 If you say Y here you get support for the Tundra TSI148 VME bridge
+	 chip.
+
+config VME_FAKE
+	tristate "Fake"
+	help
+	 If you say Y here you get support for the fake VME bridge. This
+	 provides a virtualised VME Bus for devices with no VME bridge. This
+	 is mainly useful for VME development (in the absence of VME
+	 hardware).
+
+comment "VME Device Drivers"
+
+config VME_USER
+	tristate "VME user space access driver"
+	depends on STAGING && VME_BUS
+	help
+	  If you say Y here you want to be able to access a limited number of
+	  VME windows in a manner at least semi-compatible with the interface
+	  provided with the original driver at <http://www.vmelinux.org/>.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called vme_user. If unsure, say N.
+
+endif
diff --git a/drivers/staging/vme_user/Makefile b/drivers/staging/vme_user/Makefile
new file mode 100644
index 000000000..8dcc6938c
--- /dev/null
+++ b/drivers/staging/vme_user/Makefile
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the VME device drivers.
+#
+
+obj-$(CONFIG_VME_BUS)		+= vme.o
+obj-$(CONFIG_VME_USER)		+= vme_user.o
+obj-$(CONFIG_VME_TSI148)	+= vme_tsi148.o
+obj-$(CONFIG_VME_FAKE)		+= vme_fake.o
diff --git a/drivers/staging/vme_user/vme.c b/drivers/staging/vme_user/vme.c
new file mode 100644
index 000000000..b5555683a
--- /dev/null
+++ b/drivers/staging/vme_user/vme.c
@@ -0,0 +1,2015 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * VME Bridge Framework
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2008 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * Based on work by Tom Armistead and Ajit Prem
+ * Copyright 2004 Motorola Inc.
+ */
+
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/mm.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/pci.h>
+#include <linux/poll.h>
+#include <linux/highmem.h>
+#include <linux/interrupt.h>
+#include <linux/pagemap.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/syscalls.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+
+#include "vme.h"
+#include "vme_bridge.h"
+
+/* Bitmask and list of registered buses both protected by common mutex */
+static unsigned int vme_bus_numbers;
+static LIST_HEAD(vme_bus_list);
+static DEFINE_MUTEX(vme_buses_lock);
+
+static int __init vme_init(void);
+
+static struct vme_dev *dev_to_vme_dev(struct device *dev)
+{
+	return container_of(dev, struct vme_dev, dev);
+}
+
+/*
+ * Find the bridge that the resource is associated with.
+ */
+static struct vme_bridge *find_bridge(struct vme_resource *resource)
+{
+	/* Get list to search */
+	switch (resource->type) {
+	case VME_MASTER:
+		return list_entry(resource->entry, struct vme_master_resource,
+			list)->parent;
+	case VME_SLAVE:
+		return list_entry(resource->entry, struct vme_slave_resource,
+			list)->parent;
+	case VME_DMA:
+		return list_entry(resource->entry, struct vme_dma_resource,
+			list)->parent;
+	case VME_LM:
+		return list_entry(resource->entry, struct vme_lm_resource,
+			list)->parent;
+	default:
+		printk(KERN_ERR "Unknown resource type\n");
+		return NULL;
+	}
+}
+
+/**
+ * vme_alloc_consistent - Allocate contiguous memory.
+ * @resource: Pointer to VME resource.
+ * @size: Size of allocation required.
+ * @dma: Pointer to variable to store physical address of allocation.
+ *
+ * Allocate a contiguous block of memory for use by the driver. This is used to
+ * create the buffers for the slave windows.
+ *
+ * Return: Virtual address of allocation on success, NULL on failure.
+ */
+void *vme_alloc_consistent(struct vme_resource *resource, size_t size,
+	dma_addr_t *dma)
+{
+	struct vme_bridge *bridge;
+
+	if (!resource) {
+		printk(KERN_ERR "No resource\n");
+		return NULL;
+	}
+
+	bridge = find_bridge(resource);
+	if (!bridge) {
+		printk(KERN_ERR "Can't find bridge\n");
+		return NULL;
+	}
+
+	if (!bridge->parent) {
+		printk(KERN_ERR "Dev entry NULL for bridge %s\n", bridge->name);
+		return NULL;
+	}
+
+	if (!bridge->alloc_consistent) {
+		printk(KERN_ERR "alloc_consistent not supported by bridge %s\n",
+		       bridge->name);
+		return NULL;
+	}
+
+	return bridge->alloc_consistent(bridge->parent, size, dma);
+}
+EXPORT_SYMBOL(vme_alloc_consistent);
+
+/**
+ * vme_free_consistent - Free previously allocated memory.
+ * @resource: Pointer to VME resource.
+ * @size: Size of allocation to free.
+ * @vaddr: Virtual address of allocation.
+ * @dma: Physical address of allocation.
+ *
+ * Free previously allocated block of contiguous memory.
+ */
+void vme_free_consistent(struct vme_resource *resource, size_t size,
+	void *vaddr, dma_addr_t dma)
+{
+	struct vme_bridge *bridge;
+
+	if (!resource) {
+		printk(KERN_ERR "No resource\n");
+		return;
+	}
+
+	bridge = find_bridge(resource);
+	if (!bridge) {
+		printk(KERN_ERR "Can't find bridge\n");
+		return;
+	}
+
+	if (!bridge->parent) {
+		printk(KERN_ERR "Dev entry NULL for bridge %s\n", bridge->name);
+		return;
+	}
+
+	if (!bridge->free_consistent) {
+		printk(KERN_ERR "free_consistent not supported by bridge %s\n",
+		       bridge->name);
+		return;
+	}
+
+	bridge->free_consistent(bridge->parent, size, vaddr, dma);
+}
+EXPORT_SYMBOL(vme_free_consistent);
+
+/**
+ * vme_get_size - Helper function returning size of a VME window
+ * @resource: Pointer to VME slave or master resource.
+ *
+ * Determine the size of the VME window provided. This is a helper
+ * function, wrappering the call to vme_master_get or vme_slave_get
+ * depending on the type of window resource handed to it.
+ *
+ * Return: Size of the window on success, zero on failure.
+ */
+size_t vme_get_size(struct vme_resource *resource)
+{
+	int enabled, retval;
+	unsigned long long base, size;
+	dma_addr_t buf_base;
+	u32 aspace, cycle, dwidth;
+
+	switch (resource->type) {
+	case VME_MASTER:
+		retval = vme_master_get(resource, &enabled, &base, &size,
+			&aspace, &cycle, &dwidth);
+		if (retval)
+			return 0;
+
+		return size;
+	case VME_SLAVE:
+		retval = vme_slave_get(resource, &enabled, &base, &size,
+			&buf_base, &aspace, &cycle);
+		if (retval)
+			return 0;
+
+		return size;
+	case VME_DMA:
+		return 0;
+	default:
+		printk(KERN_ERR "Unknown resource type\n");
+		return 0;
+	}
+}
+EXPORT_SYMBOL(vme_get_size);
+
+int vme_check_window(u32 aspace, unsigned long long vme_base,
+		     unsigned long long size)
+{
+	int retval = 0;
+
+	if (vme_base + size < size)
+		return -EINVAL;
+
+	switch (aspace) {
+	case VME_A16:
+		if (vme_base + size > VME_A16_MAX)
+			retval = -EFAULT;
+		break;
+	case VME_A24:
+		if (vme_base + size > VME_A24_MAX)
+			retval = -EFAULT;
+		break;
+	case VME_A32:
+		if (vme_base + size > VME_A32_MAX)
+			retval = -EFAULT;
+		break;
+	case VME_A64:
+		/* The VME_A64_MAX limit is actually U64_MAX + 1 */
+		break;
+	case VME_CRCSR:
+		if (vme_base + size > VME_CRCSR_MAX)
+			retval = -EFAULT;
+		break;
+	case VME_USER1:
+	case VME_USER2:
+	case VME_USER3:
+	case VME_USER4:
+		/* User Defined */
+		break;
+	default:
+		printk(KERN_ERR "Invalid address space\n");
+		retval = -EINVAL;
+		break;
+	}
+
+	return retval;
+}
+EXPORT_SYMBOL(vme_check_window);
+
+static u32 vme_get_aspace(int am)
+{
+	switch (am) {
+	case 0x29:
+	case 0x2D:
+		return VME_A16;
+	case 0x38:
+	case 0x39:
+	case 0x3A:
+	case 0x3B:
+	case 0x3C:
+	case 0x3D:
+	case 0x3E:
+	case 0x3F:
+		return VME_A24;
+	case 0x8:
+	case 0x9:
+	case 0xA:
+	case 0xB:
+	case 0xC:
+	case 0xD:
+	case 0xE:
+	case 0xF:
+		return VME_A32;
+	case 0x0:
+	case 0x1:
+	case 0x3:
+		return VME_A64;
+	}
+
+	return 0;
+}
+
+/**
+ * vme_slave_request - Request a VME slave window resource.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @address: Required VME address space.
+ * @cycle: Required VME data transfer cycle type.
+ *
+ * Request use of a VME window resource capable of being set for the requested
+ * address space and data transfer cycle.
+ *
+ * Return: Pointer to VME resource on success, NULL on failure.
+ */
+struct vme_resource *vme_slave_request(struct vme_dev *vdev, u32 address,
+	u32 cycle)
+{
+	struct vme_bridge *bridge;
+	struct list_head *slave_pos = NULL;
+	struct vme_slave_resource *allocated_image = NULL;
+	struct vme_slave_resource *slave_image = NULL;
+	struct vme_resource *resource = NULL;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		goto err_bus;
+	}
+
+	/* Loop through slave resources */
+	list_for_each(slave_pos, &bridge->slave_resources) {
+		slave_image = list_entry(slave_pos,
+			struct vme_slave_resource, list);
+
+		if (!slave_image) {
+			printk(KERN_ERR "Registered NULL Slave resource\n");
+			continue;
+		}
+
+		/* Find an unlocked and compatible image */
+		mutex_lock(&slave_image->mtx);
+		if (((slave_image->address_attr & address) == address) &&
+			((slave_image->cycle_attr & cycle) == cycle) &&
+			(slave_image->locked == 0)) {
+
+			slave_image->locked = 1;
+			mutex_unlock(&slave_image->mtx);
+			allocated_image = slave_image;
+			break;
+		}
+		mutex_unlock(&slave_image->mtx);
+	}
+
+	/* No free image */
+	if (!allocated_image)
+		goto err_image;
+
+	resource = kmalloc(sizeof(*resource), GFP_KERNEL);
+	if (!resource)
+		goto err_alloc;
+
+	resource->type = VME_SLAVE;
+	resource->entry = &allocated_image->list;
+
+	return resource;
+
+err_alloc:
+	/* Unlock image */
+	mutex_lock(&slave_image->mtx);
+	slave_image->locked = 0;
+	mutex_unlock(&slave_image->mtx);
+err_image:
+err_bus:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_slave_request);
+
+/**
+ * vme_slave_set - Set VME slave window configuration.
+ * @resource: Pointer to VME slave resource.
+ * @enabled: State to which the window should be configured.
+ * @vme_base: Base address for the window.
+ * @size: Size of the VME window.
+ * @buf_base: Based address of buffer used to provide VME slave window storage.
+ * @aspace: VME address space for the VME window.
+ * @cycle: VME data transfer cycle type for the VME window.
+ *
+ * Set configuration for provided VME slave window.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device, if an invalid resource has been provided or invalid
+ *         attributes are provided. Hardware specific errors may also be
+ *         returned.
+ */
+int vme_slave_set(struct vme_resource *resource, int enabled,
+	unsigned long long vme_base, unsigned long long size,
+	dma_addr_t buf_base, u32 aspace, u32 cycle)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_slave_resource *image;
+	int retval;
+
+	if (resource->type != VME_SLAVE) {
+		printk(KERN_ERR "Not a slave resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_slave_resource, list);
+
+	if (!bridge->slave_set) {
+		printk(KERN_ERR "Function not supported\n");
+		return -ENOSYS;
+	}
+
+	if (!(((image->address_attr & aspace) == aspace) &&
+		((image->cycle_attr & cycle) == cycle))) {
+		printk(KERN_ERR "Invalid attributes\n");
+		return -EINVAL;
+	}
+
+	retval = vme_check_window(aspace, vme_base, size);
+	if (retval)
+		return retval;
+
+	return bridge->slave_set(image, enabled, vme_base, size, buf_base,
+		aspace, cycle);
+}
+EXPORT_SYMBOL(vme_slave_set);
+
+/**
+ * vme_slave_get - Retrieve VME slave window configuration.
+ * @resource: Pointer to VME slave resource.
+ * @enabled: Pointer to variable for storing state.
+ * @vme_base: Pointer to variable for storing window base address.
+ * @size: Pointer to variable for storing window size.
+ * @buf_base: Pointer to variable for storing slave buffer base address.
+ * @aspace: Pointer to variable for storing VME address space.
+ * @cycle: Pointer to variable for storing VME data transfer cycle type.
+ *
+ * Return configuration for provided VME slave window.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device or if an invalid resource has been provided.
+ */
+int vme_slave_get(struct vme_resource *resource, int *enabled,
+	unsigned long long *vme_base, unsigned long long *size,
+	dma_addr_t *buf_base, u32 *aspace, u32 *cycle)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_slave_resource *image;
+
+	if (resource->type != VME_SLAVE) {
+		printk(KERN_ERR "Not a slave resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_slave_resource, list);
+
+	if (!bridge->slave_get) {
+		printk(KERN_ERR "vme_slave_get not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->slave_get(image, enabled, vme_base, size, buf_base,
+		aspace, cycle);
+}
+EXPORT_SYMBOL(vme_slave_get);
+
+/**
+ * vme_slave_free - Free VME slave window
+ * @resource: Pointer to VME slave resource.
+ *
+ * Free the provided slave resource so that it may be reallocated.
+ */
+void vme_slave_free(struct vme_resource *resource)
+{
+	struct vme_slave_resource *slave_image;
+
+	if (resource->type != VME_SLAVE) {
+		printk(KERN_ERR "Not a slave resource\n");
+		return;
+	}
+
+	slave_image = list_entry(resource->entry, struct vme_slave_resource,
+		list);
+	if (!slave_image) {
+		printk(KERN_ERR "Can't find slave resource\n");
+		return;
+	}
+
+	/* Unlock image */
+	mutex_lock(&slave_image->mtx);
+	if (slave_image->locked == 0)
+		printk(KERN_ERR "Image is already free\n");
+
+	slave_image->locked = 0;
+	mutex_unlock(&slave_image->mtx);
+
+	/* Free up resource memory */
+	kfree(resource);
+}
+EXPORT_SYMBOL(vme_slave_free);
+
+/**
+ * vme_master_request - Request a VME master window resource.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @address: Required VME address space.
+ * @cycle: Required VME data transfer cycle type.
+ * @dwidth: Required VME data transfer width.
+ *
+ * Request use of a VME window resource capable of being set for the requested
+ * address space, data transfer cycle and width.
+ *
+ * Return: Pointer to VME resource on success, NULL on failure.
+ */
+struct vme_resource *vme_master_request(struct vme_dev *vdev, u32 address,
+	u32 cycle, u32 dwidth)
+{
+	struct vme_bridge *bridge;
+	struct list_head *master_pos = NULL;
+	struct vme_master_resource *allocated_image = NULL;
+	struct vme_master_resource *master_image = NULL;
+	struct vme_resource *resource = NULL;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		goto err_bus;
+	}
+
+	/* Loop through master resources */
+	list_for_each(master_pos, &bridge->master_resources) {
+		master_image = list_entry(master_pos,
+			struct vme_master_resource, list);
+
+		if (!master_image) {
+			printk(KERN_WARNING "Registered NULL master resource\n");
+			continue;
+		}
+
+		/* Find an unlocked and compatible image */
+		spin_lock(&master_image->lock);
+		if (((master_image->address_attr & address) == address) &&
+			((master_image->cycle_attr & cycle) == cycle) &&
+			((master_image->width_attr & dwidth) == dwidth) &&
+			(master_image->locked == 0)) {
+
+			master_image->locked = 1;
+			spin_unlock(&master_image->lock);
+			allocated_image = master_image;
+			break;
+		}
+		spin_unlock(&master_image->lock);
+	}
+
+	/* Check to see if we found a resource */
+	if (!allocated_image) {
+		printk(KERN_ERR "Can't find a suitable resource\n");
+		goto err_image;
+	}
+
+	resource = kmalloc(sizeof(*resource), GFP_KERNEL);
+	if (!resource)
+		goto err_alloc;
+
+	resource->type = VME_MASTER;
+	resource->entry = &allocated_image->list;
+
+	return resource;
+
+err_alloc:
+	/* Unlock image */
+	spin_lock(&master_image->lock);
+	master_image->locked = 0;
+	spin_unlock(&master_image->lock);
+err_image:
+err_bus:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_master_request);
+
+/**
+ * vme_master_set - Set VME master window configuration.
+ * @resource: Pointer to VME master resource.
+ * @enabled: State to which the window should be configured.
+ * @vme_base: Base address for the window.
+ * @size: Size of the VME window.
+ * @aspace: VME address space for the VME window.
+ * @cycle: VME data transfer cycle type for the VME window.
+ * @dwidth: VME data transfer width for the VME window.
+ *
+ * Set configuration for provided VME master window.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device, if an invalid resource has been provided or invalid
+ *         attributes are provided. Hardware specific errors may also be
+ *         returned.
+ */
+int vme_master_set(struct vme_resource *resource, int enabled,
+	unsigned long long vme_base, unsigned long long size, u32 aspace,
+	u32 cycle, u32 dwidth)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_master_resource *image;
+	int retval;
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+
+	if (!bridge->master_set) {
+		printk(KERN_WARNING "vme_master_set not supported\n");
+		return -EINVAL;
+	}
+
+	if (!(((image->address_attr & aspace) == aspace) &&
+		((image->cycle_attr & cycle) == cycle) &&
+		((image->width_attr & dwidth) == dwidth))) {
+		printk(KERN_WARNING "Invalid attributes\n");
+		return -EINVAL;
+	}
+
+	retval = vme_check_window(aspace, vme_base, size);
+	if (retval)
+		return retval;
+
+	return bridge->master_set(image, enabled, vme_base, size, aspace,
+		cycle, dwidth);
+}
+EXPORT_SYMBOL(vme_master_set);
+
+/**
+ * vme_master_get - Retrieve VME master window configuration.
+ * @resource: Pointer to VME master resource.
+ * @enabled: Pointer to variable for storing state.
+ * @vme_base: Pointer to variable for storing window base address.
+ * @size: Pointer to variable for storing window size.
+ * @aspace: Pointer to variable for storing VME address space.
+ * @cycle: Pointer to variable for storing VME data transfer cycle type.
+ * @dwidth: Pointer to variable for storing VME data transfer width.
+ *
+ * Return configuration for provided VME master window.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device or if an invalid resource has been provided.
+ */
+int vme_master_get(struct vme_resource *resource, int *enabled,
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_master_resource *image;
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+
+	if (!bridge->master_get) {
+		printk(KERN_WARNING "%s not supported\n", __func__);
+		return -EINVAL;
+	}
+
+	return bridge->master_get(image, enabled, vme_base, size, aspace,
+		cycle, dwidth);
+}
+EXPORT_SYMBOL(vme_master_get);
+
+/**
+ * vme_master_read - Read data from VME space into a buffer.
+ * @resource: Pointer to VME master resource.
+ * @buf: Pointer to buffer where data should be transferred.
+ * @count: Number of bytes to transfer.
+ * @offset: Offset into VME master window at which to start transfer.
+ *
+ * Perform read of count bytes of data from location on VME bus which maps into
+ * the VME master window at offset to buf.
+ *
+ * Return: Number of bytes read, -EINVAL if resource is not a VME master
+ *         resource or read operation is not supported. -EFAULT returned if
+ *         invalid offset is provided. Hardware specific errors may also be
+ *         returned.
+ */
+ssize_t vme_master_read(struct vme_resource *resource, void *buf, size_t count,
+	loff_t offset)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_master_resource *image;
+	size_t length;
+
+	if (!bridge->master_read) {
+		printk(KERN_WARNING "Reading from resource not supported\n");
+		return -EINVAL;
+	}
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+
+	length = vme_get_size(resource);
+
+	if (offset > length) {
+		printk(KERN_WARNING "Invalid Offset\n");
+		return -EFAULT;
+	}
+
+	if ((offset + count) > length)
+		count = length - offset;
+
+	return bridge->master_read(image, buf, count, offset);
+
+}
+EXPORT_SYMBOL(vme_master_read);
+
+/**
+ * vme_master_write - Write data out to VME space from a buffer.
+ * @resource: Pointer to VME master resource.
+ * @buf: Pointer to buffer holding data to transfer.
+ * @count: Number of bytes to transfer.
+ * @offset: Offset into VME master window at which to start transfer.
+ *
+ * Perform write of count bytes of data from buf to location on VME bus which
+ * maps into the VME master window at offset.
+ *
+ * Return: Number of bytes written, -EINVAL if resource is not a VME master
+ *         resource or write operation is not supported. -EFAULT returned if
+ *         invalid offset is provided. Hardware specific errors may also be
+ *         returned.
+ */
+ssize_t vme_master_write(struct vme_resource *resource, void *buf,
+	size_t count, loff_t offset)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_master_resource *image;
+	size_t length;
+
+	if (!bridge->master_write) {
+		printk(KERN_WARNING "Writing to resource not supported\n");
+		return -EINVAL;
+	}
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+
+	length = vme_get_size(resource);
+
+	if (offset > length) {
+		printk(KERN_WARNING "Invalid Offset\n");
+		return -EFAULT;
+	}
+
+	if ((offset + count) > length)
+		count = length - offset;
+
+	return bridge->master_write(image, buf, count, offset);
+}
+EXPORT_SYMBOL(vme_master_write);
+
+/**
+ * vme_master_rmw - Perform read-modify-write cycle.
+ * @resource: Pointer to VME master resource.
+ * @mask: Bits to be compared and swapped in operation.
+ * @compare: Bits to be compared with data read from offset.
+ * @swap: Bits to be swapped in data read from offset.
+ * @offset: Offset into VME master window at which to perform operation.
+ *
+ * Perform read-modify-write cycle on provided location:
+ * - Location on VME bus is read.
+ * - Bits selected by mask are compared with compare.
+ * - Where a selected bit matches that in compare and are selected in swap,
+ * the bit is swapped.
+ * - Result written back to location on VME bus.
+ *
+ * Return: Bytes written on success, -EINVAL if resource is not a VME master
+ *         resource or RMW operation is not supported. Hardware specific
+ *         errors may also be returned.
+ */
+unsigned int vme_master_rmw(struct vme_resource *resource, unsigned int mask,
+	unsigned int compare, unsigned int swap, loff_t offset)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_master_resource *image;
+
+	if (!bridge->master_rmw) {
+		printk(KERN_WARNING "Writing to resource not supported\n");
+		return -EINVAL;
+	}
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+
+	return bridge->master_rmw(image, mask, compare, swap, offset);
+}
+EXPORT_SYMBOL(vme_master_rmw);
+
+/**
+ * vme_master_mmap - Mmap region of VME master window.
+ * @resource: Pointer to VME master resource.
+ * @vma: Pointer to definition of user mapping.
+ *
+ * Memory map a region of the VME master window into user space.
+ *
+ * Return: Zero on success, -EINVAL if resource is not a VME master
+ *         resource or -EFAULT if map exceeds window size. Other generic mmap
+ *         errors may also be returned.
+ */
+int vme_master_mmap(struct vme_resource *resource, struct vm_area_struct *vma)
+{
+	struct vme_master_resource *image;
+	phys_addr_t phys_addr;
+	unsigned long vma_size;
+
+	if (resource->type != VME_MASTER) {
+		pr_err("Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+	phys_addr = image->bus_resource.start + (vma->vm_pgoff << PAGE_SHIFT);
+	vma_size = vma->vm_end - vma->vm_start;
+
+	if (phys_addr + vma_size > image->bus_resource.end + 1) {
+		pr_err("Map size cannot exceed the window size\n");
+		return -EFAULT;
+	}
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	return vm_iomap_memory(vma, phys_addr, vma->vm_end - vma->vm_start);
+}
+EXPORT_SYMBOL(vme_master_mmap);
+
+/**
+ * vme_master_free - Free VME master window
+ * @resource: Pointer to VME master resource.
+ *
+ * Free the provided master resource so that it may be reallocated.
+ */
+void vme_master_free(struct vme_resource *resource)
+{
+	struct vme_master_resource *master_image;
+
+	if (resource->type != VME_MASTER) {
+		printk(KERN_ERR "Not a master resource\n");
+		return;
+	}
+
+	master_image = list_entry(resource->entry, struct vme_master_resource,
+		list);
+	if (!master_image) {
+		printk(KERN_ERR "Can't find master resource\n");
+		return;
+	}
+
+	/* Unlock image */
+	spin_lock(&master_image->lock);
+	if (master_image->locked == 0)
+		printk(KERN_ERR "Image is already free\n");
+
+	master_image->locked = 0;
+	spin_unlock(&master_image->lock);
+
+	/* Free up resource memory */
+	kfree(resource);
+}
+EXPORT_SYMBOL(vme_master_free);
+
+/**
+ * vme_dma_request - Request a DMA controller.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @route: Required src/destination combination.
+ *
+ * Request a VME DMA controller with capability to perform transfers bewteen
+ * requested source/destination combination.
+ *
+ * Return: Pointer to VME DMA resource on success, NULL on failure.
+ */
+struct vme_resource *vme_dma_request(struct vme_dev *vdev, u32 route)
+{
+	struct vme_bridge *bridge;
+	struct list_head *dma_pos = NULL;
+	struct vme_dma_resource *allocated_ctrlr = NULL;
+	struct vme_dma_resource *dma_ctrlr = NULL;
+	struct vme_resource *resource = NULL;
+
+	/* XXX Not checking resource attributes */
+	printk(KERN_ERR "No VME resource Attribute tests done\n");
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		goto err_bus;
+	}
+
+	/* Loop through DMA resources */
+	list_for_each(dma_pos, &bridge->dma_resources) {
+		dma_ctrlr = list_entry(dma_pos,
+			struct vme_dma_resource, list);
+		if (!dma_ctrlr) {
+			printk(KERN_ERR "Registered NULL DMA resource\n");
+			continue;
+		}
+
+		/* Find an unlocked and compatible controller */
+		mutex_lock(&dma_ctrlr->mtx);
+		if (((dma_ctrlr->route_attr & route) == route) &&
+			(dma_ctrlr->locked == 0)) {
+
+			dma_ctrlr->locked = 1;
+			mutex_unlock(&dma_ctrlr->mtx);
+			allocated_ctrlr = dma_ctrlr;
+			break;
+		}
+		mutex_unlock(&dma_ctrlr->mtx);
+	}
+
+	/* Check to see if we found a resource */
+	if (!allocated_ctrlr)
+		goto err_ctrlr;
+
+	resource = kmalloc(sizeof(*resource), GFP_KERNEL);
+	if (!resource)
+		goto err_alloc;
+
+	resource->type = VME_DMA;
+	resource->entry = &allocated_ctrlr->list;
+
+	return resource;
+
+err_alloc:
+	/* Unlock image */
+	mutex_lock(&dma_ctrlr->mtx);
+	dma_ctrlr->locked = 0;
+	mutex_unlock(&dma_ctrlr->mtx);
+err_ctrlr:
+err_bus:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_dma_request);
+
+/**
+ * vme_new_dma_list - Create new VME DMA list.
+ * @resource: Pointer to VME DMA resource.
+ *
+ * Create a new VME DMA list. It is the responsibility of the user to free
+ * the list once it is no longer required with vme_dma_list_free().
+ *
+ * Return: Pointer to new VME DMA list, NULL on allocation failure or invalid
+ *         VME DMA resource.
+ */
+struct vme_dma_list *vme_new_dma_list(struct vme_resource *resource)
+{
+	struct vme_dma_list *dma_list;
+
+	if (resource->type != VME_DMA) {
+		printk(KERN_ERR "Not a DMA resource\n");
+		return NULL;
+	}
+
+	dma_list = kmalloc(sizeof(*dma_list), GFP_KERNEL);
+	if (!dma_list)
+		return NULL;
+
+	INIT_LIST_HEAD(&dma_list->entries);
+	dma_list->parent = list_entry(resource->entry,
+				      struct vme_dma_resource,
+				      list);
+	mutex_init(&dma_list->mtx);
+
+	return dma_list;
+}
+EXPORT_SYMBOL(vme_new_dma_list);
+
+/**
+ * vme_dma_pattern_attribute - Create "Pattern" type VME DMA list attribute.
+ * @pattern: Value to use used as pattern
+ * @type: Type of pattern to be written.
+ *
+ * Create VME DMA list attribute for pattern generation. It is the
+ * responsibility of the user to free used attributes using
+ * vme_dma_free_attribute().
+ *
+ * Return: Pointer to VME DMA attribute, NULL on failure.
+ */
+struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern, u32 type)
+{
+	struct vme_dma_attr *attributes;
+	struct vme_dma_pattern *pattern_attr;
+
+	attributes = kmalloc(sizeof(*attributes), GFP_KERNEL);
+	if (!attributes)
+		goto err_attr;
+
+	pattern_attr = kmalloc(sizeof(*pattern_attr), GFP_KERNEL);
+	if (!pattern_attr)
+		goto err_pat;
+
+	attributes->type = VME_DMA_PATTERN;
+	attributes->private = (void *)pattern_attr;
+
+	pattern_attr->pattern = pattern;
+	pattern_attr->type = type;
+
+	return attributes;
+
+err_pat:
+	kfree(attributes);
+err_attr:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_dma_pattern_attribute);
+
+/**
+ * vme_dma_pci_attribute - Create "PCI" type VME DMA list attribute.
+ * @address: PCI base address for DMA transfer.
+ *
+ * Create VME DMA list attribute pointing to a location on PCI for DMA
+ * transfers. It is the responsibility of the user to free used attributes
+ * using vme_dma_free_attribute().
+ *
+ * Return: Pointer to VME DMA attribute, NULL on failure.
+ */
+struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t address)
+{
+	struct vme_dma_attr *attributes;
+	struct vme_dma_pci *pci_attr;
+
+	/* XXX Run some sanity checks here */
+
+	attributes = kmalloc(sizeof(*attributes), GFP_KERNEL);
+	if (!attributes)
+		goto err_attr;
+
+	pci_attr = kmalloc(sizeof(*pci_attr), GFP_KERNEL);
+	if (!pci_attr)
+		goto err_pci;
+
+	attributes->type = VME_DMA_PCI;
+	attributes->private = (void *)pci_attr;
+
+	pci_attr->address = address;
+
+	return attributes;
+
+err_pci:
+	kfree(attributes);
+err_attr:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_dma_pci_attribute);
+
+/**
+ * vme_dma_vme_attribute - Create "VME" type VME DMA list attribute.
+ * @address: VME base address for DMA transfer.
+ * @aspace: VME address space to use for DMA transfer.
+ * @cycle: VME bus cycle to use for DMA transfer.
+ * @dwidth: VME data width to use for DMA transfer.
+ *
+ * Create VME DMA list attribute pointing to a location on the VME bus for DMA
+ * transfers. It is the responsibility of the user to free used attributes
+ * using vme_dma_free_attribute().
+ *
+ * Return: Pointer to VME DMA attribute, NULL on failure.
+ */
+struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long address,
+	u32 aspace, u32 cycle, u32 dwidth)
+{
+	struct vme_dma_attr *attributes;
+	struct vme_dma_vme *vme_attr;
+
+	attributes = kmalloc(sizeof(*attributes), GFP_KERNEL);
+	if (!attributes)
+		goto err_attr;
+
+	vme_attr = kmalloc(sizeof(*vme_attr), GFP_KERNEL);
+	if (!vme_attr)
+		goto err_vme;
+
+	attributes->type = VME_DMA_VME;
+	attributes->private = (void *)vme_attr;
+
+	vme_attr->address = address;
+	vme_attr->aspace = aspace;
+	vme_attr->cycle = cycle;
+	vme_attr->dwidth = dwidth;
+
+	return attributes;
+
+err_vme:
+	kfree(attributes);
+err_attr:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_dma_vme_attribute);
+
+/**
+ * vme_dma_free_attribute - Free DMA list attribute.
+ * @attributes: Pointer to DMA list attribute.
+ *
+ * Free VME DMA list attribute. VME DMA list attributes can be safely freed
+ * once vme_dma_list_add() has returned.
+ */
+void vme_dma_free_attribute(struct vme_dma_attr *attributes)
+{
+	kfree(attributes->private);
+	kfree(attributes);
+}
+EXPORT_SYMBOL(vme_dma_free_attribute);
+
+/**
+ * vme_dma_list_add - Add enty to a VME DMA list.
+ * @list: Pointer to VME list.
+ * @src: Pointer to DMA list attribute to use as source.
+ * @dest: Pointer to DMA list attribute to use as destination.
+ * @count: Number of bytes to transfer.
+ *
+ * Add an entry to the provided VME DMA list. Entry requires pointers to source
+ * and destination DMA attributes and a count.
+ *
+ * Please note, the attributes supported as source and destinations for
+ * transfers are hardware dependent.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device or if the link list has already been submitted for execution.
+ *         Hardware specific errors also possible.
+ */
+int vme_dma_list_add(struct vme_dma_list *list, struct vme_dma_attr *src,
+	struct vme_dma_attr *dest, size_t count)
+{
+	struct vme_bridge *bridge = list->parent->parent;
+	int retval;
+
+	if (!bridge->dma_list_add) {
+		printk(KERN_WARNING "Link List DMA generation not supported\n");
+		return -EINVAL;
+	}
+
+	if (!mutex_trylock(&list->mtx)) {
+		printk(KERN_ERR "Link List already submitted\n");
+		return -EINVAL;
+	}
+
+	retval = bridge->dma_list_add(list, src, dest, count);
+
+	mutex_unlock(&list->mtx);
+
+	return retval;
+}
+EXPORT_SYMBOL(vme_dma_list_add);
+
+/**
+ * vme_dma_list_exec - Queue a VME DMA list for execution.
+ * @list: Pointer to VME list.
+ *
+ * Queue the provided VME DMA list for execution. The call will return once the
+ * list has been executed.
+ *
+ * Return: Zero on success, -EINVAL if operation is not supported on this
+ *         device. Hardware specific errors also possible.
+ */
+int vme_dma_list_exec(struct vme_dma_list *list)
+{
+	struct vme_bridge *bridge = list->parent->parent;
+	int retval;
+
+	if (!bridge->dma_list_exec) {
+		printk(KERN_ERR "Link List DMA execution not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&list->mtx);
+
+	retval = bridge->dma_list_exec(list);
+
+	mutex_unlock(&list->mtx);
+
+	return retval;
+}
+EXPORT_SYMBOL(vme_dma_list_exec);
+
+/**
+ * vme_dma_list_free - Free a VME DMA list.
+ * @list: Pointer to VME list.
+ *
+ * Free the provided DMA list and all its entries.
+ *
+ * Return: Zero on success, -EINVAL on invalid VME resource, -EBUSY if resource
+ *         is still in use. Hardware specific errors also possible.
+ */
+int vme_dma_list_free(struct vme_dma_list *list)
+{
+	struct vme_bridge *bridge = list->parent->parent;
+	int retval;
+
+	if (!bridge->dma_list_empty) {
+		printk(KERN_WARNING "Emptying of Link Lists not supported\n");
+		return -EINVAL;
+	}
+
+	if (!mutex_trylock(&list->mtx)) {
+		printk(KERN_ERR "Link List in use\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Empty out all of the entries from the DMA list. We need to go to the
+	 * low level driver as DMA entries are driver specific.
+	 */
+	retval = bridge->dma_list_empty(list);
+	if (retval) {
+		printk(KERN_ERR "Unable to empty link-list entries\n");
+		mutex_unlock(&list->mtx);
+		return retval;
+	}
+	mutex_unlock(&list->mtx);
+	kfree(list);
+
+	return retval;
+}
+EXPORT_SYMBOL(vme_dma_list_free);
+
+/**
+ * vme_dma_free - Free a VME DMA resource.
+ * @resource: Pointer to VME DMA resource.
+ *
+ * Free the provided DMA resource so that it may be reallocated.
+ *
+ * Return: Zero on success, -EINVAL on invalid VME resource, -EBUSY if resource
+ *         is still active.
+ */
+int vme_dma_free(struct vme_resource *resource)
+{
+	struct vme_dma_resource *ctrlr;
+
+	if (resource->type != VME_DMA) {
+		printk(KERN_ERR "Not a DMA resource\n");
+		return -EINVAL;
+	}
+
+	ctrlr = list_entry(resource->entry, struct vme_dma_resource, list);
+
+	if (!mutex_trylock(&ctrlr->mtx)) {
+		printk(KERN_ERR "Resource busy, can't free\n");
+		return -EBUSY;
+	}
+
+	if (!(list_empty(&ctrlr->pending) && list_empty(&ctrlr->running))) {
+		printk(KERN_WARNING "Resource still processing transfers\n");
+		mutex_unlock(&ctrlr->mtx);
+		return -EBUSY;
+	}
+
+	ctrlr->locked = 0;
+
+	mutex_unlock(&ctrlr->mtx);
+
+	kfree(resource);
+
+	return 0;
+}
+EXPORT_SYMBOL(vme_dma_free);
+
+void vme_bus_error_handler(struct vme_bridge *bridge,
+			   unsigned long long address, int am)
+{
+	struct list_head *handler_pos = NULL;
+	struct vme_error_handler *handler;
+	int handler_triggered = 0;
+	u32 aspace = vme_get_aspace(am);
+
+	list_for_each(handler_pos, &bridge->vme_error_handlers) {
+		handler = list_entry(handler_pos, struct vme_error_handler,
+				     list);
+		if ((aspace == handler->aspace) &&
+		    (address >= handler->start) &&
+		    (address < handler->end)) {
+			if (!handler->num_errors)
+				handler->first_error = address;
+			if (handler->num_errors != UINT_MAX)
+				handler->num_errors++;
+			handler_triggered = 1;
+		}
+	}
+
+	if (!handler_triggered)
+		dev_err(bridge->parent,
+			"Unhandled VME access error at address 0x%llx\n",
+			address);
+}
+EXPORT_SYMBOL(vme_bus_error_handler);
+
+struct vme_error_handler *vme_register_error_handler(
+	struct vme_bridge *bridge, u32 aspace,
+	unsigned long long address, size_t len)
+{
+	struct vme_error_handler *handler;
+
+	handler = kmalloc(sizeof(*handler), GFP_ATOMIC);
+	if (!handler)
+		return NULL;
+
+	handler->aspace = aspace;
+	handler->start = address;
+	handler->end = address + len;
+	handler->num_errors = 0;
+	handler->first_error = 0;
+	list_add_tail(&handler->list, &bridge->vme_error_handlers);
+
+	return handler;
+}
+EXPORT_SYMBOL(vme_register_error_handler);
+
+void vme_unregister_error_handler(struct vme_error_handler *handler)
+{
+	list_del(&handler->list);
+	kfree(handler);
+}
+EXPORT_SYMBOL(vme_unregister_error_handler);
+
+void vme_irq_handler(struct vme_bridge *bridge, int level, int statid)
+{
+	void (*call)(int, int, void *);
+	void *priv_data;
+
+	call = bridge->irq[level - 1].callback[statid].func;
+	priv_data = bridge->irq[level - 1].callback[statid].priv_data;
+	if (call)
+		call(level, statid, priv_data);
+	else
+		printk(KERN_WARNING "Spurious VME interrupt, level:%x, vector:%x\n",
+		       level, statid);
+}
+EXPORT_SYMBOL(vme_irq_handler);
+
+/**
+ * vme_irq_request - Request a specific VME interrupt.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @level: Interrupt priority being requested.
+ * @statid: Interrupt vector being requested.
+ * @callback: Pointer to callback function called when VME interrupt/vector
+ *            received.
+ * @priv_data: Generic pointer that will be passed to the callback function.
+ *
+ * Request callback to be attached as a handler for VME interrupts with provided
+ * level and statid.
+ *
+ * Return: Zero on success, -EINVAL on invalid vme device, level or if the
+ *         function is not supported, -EBUSY if the level/statid combination is
+ *         already in use. Hardware specific errors also possible.
+ */
+int vme_irq_request(struct vme_dev *vdev, int level, int statid,
+	void (*callback)(int, int, void *),
+	void *priv_data)
+{
+	struct vme_bridge *bridge;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		return -EINVAL;
+	}
+
+	if ((level < 1) || (level > 7)) {
+		printk(KERN_ERR "Invalid interrupt level\n");
+		return -EINVAL;
+	}
+
+	if (!bridge->irq_set) {
+		printk(KERN_ERR "Configuring interrupts not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&bridge->irq_mtx);
+
+	if (bridge->irq[level - 1].callback[statid].func) {
+		mutex_unlock(&bridge->irq_mtx);
+		printk(KERN_WARNING "VME Interrupt already taken\n");
+		return -EBUSY;
+	}
+
+	bridge->irq[level - 1].count++;
+	bridge->irq[level - 1].callback[statid].priv_data = priv_data;
+	bridge->irq[level - 1].callback[statid].func = callback;
+
+	/* Enable IRQ level */
+	bridge->irq_set(bridge, level, 1, 1);
+
+	mutex_unlock(&bridge->irq_mtx);
+
+	return 0;
+}
+EXPORT_SYMBOL(vme_irq_request);
+
+/**
+ * vme_irq_free - Free a VME interrupt.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @level: Interrupt priority of interrupt being freed.
+ * @statid: Interrupt vector of interrupt being freed.
+ *
+ * Remove previously attached callback from VME interrupt priority/vector.
+ */
+void vme_irq_free(struct vme_dev *vdev, int level, int statid)
+{
+	struct vme_bridge *bridge;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		return;
+	}
+
+	if ((level < 1) || (level > 7)) {
+		printk(KERN_ERR "Invalid interrupt level\n");
+		return;
+	}
+
+	if (!bridge->irq_set) {
+		printk(KERN_ERR "Configuring interrupts not supported\n");
+		return;
+	}
+
+	mutex_lock(&bridge->irq_mtx);
+
+	bridge->irq[level - 1].count--;
+
+	/* Disable IRQ level if no more interrupts attached at this level*/
+	if (bridge->irq[level - 1].count == 0)
+		bridge->irq_set(bridge, level, 0, 1);
+
+	bridge->irq[level - 1].callback[statid].func = NULL;
+	bridge->irq[level - 1].callback[statid].priv_data = NULL;
+
+	mutex_unlock(&bridge->irq_mtx);
+}
+EXPORT_SYMBOL(vme_irq_free);
+
+/**
+ * vme_irq_generate - Generate VME interrupt.
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ * @level: Interrupt priority at which to assert the interrupt.
+ * @statid: Interrupt vector to associate with the interrupt.
+ *
+ * Generate a VME interrupt of the provided level and with the provided
+ * statid.
+ *
+ * Return: Zero on success, -EINVAL on invalid vme device, level or if the
+ *         function is not supported. Hardware specific errors also possible.
+ */
+int vme_irq_generate(struct vme_dev *vdev, int level, int statid)
+{
+	struct vme_bridge *bridge;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		return -EINVAL;
+	}
+
+	if ((level < 1) || (level > 7)) {
+		printk(KERN_WARNING "Invalid interrupt level\n");
+		return -EINVAL;
+	}
+
+	if (!bridge->irq_generate) {
+		printk(KERN_WARNING "Interrupt generation not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->irq_generate(bridge, level, statid);
+}
+EXPORT_SYMBOL(vme_irq_generate);
+
+/**
+ * vme_lm_request - Request a VME location monitor
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ *
+ * Allocate a location monitor resource to the driver. A location monitor
+ * allows the driver to monitor accesses to a contiguous number of
+ * addresses on the VME bus.
+ *
+ * Return: Pointer to a VME resource on success or NULL on failure.
+ */
+struct vme_resource *vme_lm_request(struct vme_dev *vdev)
+{
+	struct vme_bridge *bridge;
+	struct list_head *lm_pos = NULL;
+	struct vme_lm_resource *allocated_lm = NULL;
+	struct vme_lm_resource *lm = NULL;
+	struct vme_resource *resource = NULL;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		goto err_bus;
+	}
+
+	/* Loop through LM resources */
+	list_for_each(lm_pos, &bridge->lm_resources) {
+		lm = list_entry(lm_pos,
+			struct vme_lm_resource, list);
+		if (!lm) {
+			printk(KERN_ERR "Registered NULL Location Monitor resource\n");
+			continue;
+		}
+
+		/* Find an unlocked controller */
+		mutex_lock(&lm->mtx);
+		if (lm->locked == 0) {
+			lm->locked = 1;
+			mutex_unlock(&lm->mtx);
+			allocated_lm = lm;
+			break;
+		}
+		mutex_unlock(&lm->mtx);
+	}
+
+	/* Check to see if we found a resource */
+	if (!allocated_lm)
+		goto err_lm;
+
+	resource = kmalloc(sizeof(*resource), GFP_KERNEL);
+	if (!resource)
+		goto err_alloc;
+
+	resource->type = VME_LM;
+	resource->entry = &allocated_lm->list;
+
+	return resource;
+
+err_alloc:
+	/* Unlock image */
+	mutex_lock(&lm->mtx);
+	lm->locked = 0;
+	mutex_unlock(&lm->mtx);
+err_lm:
+err_bus:
+	return NULL;
+}
+EXPORT_SYMBOL(vme_lm_request);
+
+/**
+ * vme_lm_count - Determine number of VME Addresses monitored
+ * @resource: Pointer to VME location monitor resource.
+ *
+ * The number of contiguous addresses monitored is hardware dependent.
+ * Return the number of contiguous addresses monitored by the
+ * location monitor.
+ *
+ * Return: Count of addresses monitored or -EINVAL when provided with an
+ *	   invalid location monitor resource.
+ */
+int vme_lm_count(struct vme_resource *resource)
+{
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return -EINVAL;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	return lm->monitors;
+}
+EXPORT_SYMBOL(vme_lm_count);
+
+/**
+ * vme_lm_set - Configure location monitor
+ * @resource: Pointer to VME location monitor resource.
+ * @lm_base: Base address to monitor.
+ * @aspace: VME address space to monitor.
+ * @cycle: VME bus cycle type to monitor.
+ *
+ * Set the base address, address space and cycle type of accesses to be
+ * monitored by the location monitor.
+ *
+ * Return: Zero on success, -EINVAL when provided with an invalid location
+ *	   monitor resource or function is not supported. Hardware specific
+ *	   errors may also be returned.
+ */
+int vme_lm_set(struct vme_resource *resource, unsigned long long lm_base,
+	u32 aspace, u32 cycle)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return -EINVAL;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	if (!bridge->lm_set) {
+		printk(KERN_ERR "vme_lm_set not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->lm_set(lm, lm_base, aspace, cycle);
+}
+EXPORT_SYMBOL(vme_lm_set);
+
+/**
+ * vme_lm_get - Retrieve location monitor settings
+ * @resource: Pointer to VME location monitor resource.
+ * @lm_base: Pointer used to output the base address monitored.
+ * @aspace: Pointer used to output the address space monitored.
+ * @cycle: Pointer used to output the VME bus cycle type monitored.
+ *
+ * Retrieve the base address, address space and cycle type of accesses to
+ * be monitored by the location monitor.
+ *
+ * Return: Zero on success, -EINVAL when provided with an invalid location
+ *	   monitor resource or function is not supported. Hardware specific
+ *	   errors may also be returned.
+ */
+int vme_lm_get(struct vme_resource *resource, unsigned long long *lm_base,
+	u32 *aspace, u32 *cycle)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return -EINVAL;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	if (!bridge->lm_get) {
+		printk(KERN_ERR "vme_lm_get not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->lm_get(lm, lm_base, aspace, cycle);
+}
+EXPORT_SYMBOL(vme_lm_get);
+
+/**
+ * vme_lm_attach - Provide callback for location monitor address
+ * @resource: Pointer to VME location monitor resource.
+ * @monitor: Offset to which callback should be attached.
+ * @callback: Pointer to callback function called when triggered.
+ * @data: Generic pointer that will be passed to the callback function.
+ *
+ * Attach a callback to the specificed offset into the location monitors
+ * monitored addresses. A generic pointer is provided to allow data to be
+ * passed to the callback when called.
+ *
+ * Return: Zero on success, -EINVAL when provided with an invalid location
+ *	   monitor resource or function is not supported. Hardware specific
+ *	   errors may also be returned.
+ */
+int vme_lm_attach(struct vme_resource *resource, int monitor,
+	void (*callback)(void *), void *data)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return -EINVAL;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	if (!bridge->lm_attach) {
+		printk(KERN_ERR "vme_lm_attach not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->lm_attach(lm, monitor, callback, data);
+}
+EXPORT_SYMBOL(vme_lm_attach);
+
+/**
+ * vme_lm_detach - Remove callback for location monitor address
+ * @resource: Pointer to VME location monitor resource.
+ * @monitor: Offset to which callback should be removed.
+ *
+ * Remove the callback associated with the specificed offset into the
+ * location monitors monitored addresses.
+ *
+ * Return: Zero on success, -EINVAL when provided with an invalid location
+ *	   monitor resource or function is not supported. Hardware specific
+ *	   errors may also be returned.
+ */
+int vme_lm_detach(struct vme_resource *resource, int monitor)
+{
+	struct vme_bridge *bridge = find_bridge(resource);
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return -EINVAL;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	if (!bridge->lm_detach) {
+		printk(KERN_ERR "vme_lm_detach not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->lm_detach(lm, monitor);
+}
+EXPORT_SYMBOL(vme_lm_detach);
+
+/**
+ * vme_lm_free - Free allocated VME location monitor
+ * @resource: Pointer to VME location monitor resource.
+ *
+ * Free allocation of a VME location monitor.
+ *
+ * WARNING: This function currently expects that any callbacks that have
+ *          been attached to the location monitor have been removed.
+ *
+ * Return: Zero on success, -EINVAL when provided with an invalid location
+ *	   monitor resource.
+ */
+void vme_lm_free(struct vme_resource *resource)
+{
+	struct vme_lm_resource *lm;
+
+	if (resource->type != VME_LM) {
+		printk(KERN_ERR "Not a Location Monitor resource\n");
+		return;
+	}
+
+	lm = list_entry(resource->entry, struct vme_lm_resource, list);
+
+	mutex_lock(&lm->mtx);
+
+	/* XXX
+	 * Check to see that there aren't any callbacks still attached, if
+	 * there are we should probably be detaching them!
+	 */
+
+	lm->locked = 0;
+
+	mutex_unlock(&lm->mtx);
+
+	kfree(resource);
+}
+EXPORT_SYMBOL(vme_lm_free);
+
+/**
+ * vme_slot_num - Retrieve slot ID
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ *
+ * Retrieve the slot ID associated with the provided VME device.
+ *
+ * Return: The slot ID on success, -EINVAL if VME bridge cannot be determined
+ *         or the function is not supported. Hardware specific errors may also
+ *         be returned.
+ */
+int vme_slot_num(struct vme_dev *vdev)
+{
+	struct vme_bridge *bridge;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		printk(KERN_ERR "Can't find VME bus\n");
+		return -EINVAL;
+	}
+
+	if (!bridge->slot_get) {
+		printk(KERN_WARNING "vme_slot_num not supported\n");
+		return -EINVAL;
+	}
+
+	return bridge->slot_get(bridge);
+}
+EXPORT_SYMBOL(vme_slot_num);
+
+/**
+ * vme_bus_num - Retrieve bus number
+ * @vdev: Pointer to VME device struct vme_dev assigned to driver instance.
+ *
+ * Retrieve the bus enumeration associated with the provided VME device.
+ *
+ * Return: The bus number on success, -EINVAL if VME bridge cannot be
+ *         determined.
+ */
+int vme_bus_num(struct vme_dev *vdev)
+{
+	struct vme_bridge *bridge;
+
+	bridge = vdev->bridge;
+	if (!bridge) {
+		pr_err("Can't find VME bus\n");
+		return -EINVAL;
+	}
+
+	return bridge->num;
+}
+EXPORT_SYMBOL(vme_bus_num);
+
+/* - Bridge Registration --------------------------------------------------- */
+
+static void vme_dev_release(struct device *dev)
+{
+	kfree(dev_to_vme_dev(dev));
+}
+
+/* Common bridge initialization */
+struct vme_bridge *vme_init_bridge(struct vme_bridge *bridge)
+{
+	INIT_LIST_HEAD(&bridge->vme_error_handlers);
+	INIT_LIST_HEAD(&bridge->master_resources);
+	INIT_LIST_HEAD(&bridge->slave_resources);
+	INIT_LIST_HEAD(&bridge->dma_resources);
+	INIT_LIST_HEAD(&bridge->lm_resources);
+	mutex_init(&bridge->irq_mtx);
+
+	return bridge;
+}
+EXPORT_SYMBOL(vme_init_bridge);
+
+int vme_register_bridge(struct vme_bridge *bridge)
+{
+	int i;
+	int ret = -1;
+
+	mutex_lock(&vme_buses_lock);
+	for (i = 0; i < sizeof(vme_bus_numbers) * 8; i++) {
+		if ((vme_bus_numbers & (1 << i)) == 0) {
+			vme_bus_numbers |= (1 << i);
+			bridge->num = i;
+			INIT_LIST_HEAD(&bridge->devices);
+			list_add_tail(&bridge->bus_list, &vme_bus_list);
+			ret = 0;
+			break;
+		}
+	}
+	mutex_unlock(&vme_buses_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(vme_register_bridge);
+
+void vme_unregister_bridge(struct vme_bridge *bridge)
+{
+	struct vme_dev *vdev;
+	struct vme_dev *tmp;
+
+	mutex_lock(&vme_buses_lock);
+	vme_bus_numbers &= ~(1 << bridge->num);
+	list_for_each_entry_safe(vdev, tmp, &bridge->devices, bridge_list) {
+		list_del(&vdev->drv_list);
+		list_del(&vdev->bridge_list);
+		device_unregister(&vdev->dev);
+	}
+	list_del(&bridge->bus_list);
+	mutex_unlock(&vme_buses_lock);
+}
+EXPORT_SYMBOL(vme_unregister_bridge);
+
+/* - Driver Registration --------------------------------------------------- */
+
+static int __vme_register_driver_bus(struct vme_driver *drv,
+	struct vme_bridge *bridge, unsigned int ndevs)
+{
+	int err;
+	unsigned int i;
+	struct vme_dev *vdev;
+	struct vme_dev *tmp;
+
+	for (i = 0; i < ndevs; i++) {
+		vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+		if (!vdev) {
+			err = -ENOMEM;
+			goto err_devalloc;
+		}
+		vdev->num = i;
+		vdev->bridge = bridge;
+		vdev->dev.platform_data = drv;
+		vdev->dev.release = vme_dev_release;
+		vdev->dev.parent = bridge->parent;
+		vdev->dev.bus = &vme_bus_type;
+		dev_set_name(&vdev->dev, "%s.%u-%u", drv->name, bridge->num,
+			vdev->num);
+
+		err = device_register(&vdev->dev);
+		if (err)
+			goto err_reg;
+
+		if (vdev->dev.platform_data) {
+			list_add_tail(&vdev->drv_list, &drv->devices);
+			list_add_tail(&vdev->bridge_list, &bridge->devices);
+		} else
+			device_unregister(&vdev->dev);
+	}
+	return 0;
+
+err_reg:
+	put_device(&vdev->dev);
+err_devalloc:
+	list_for_each_entry_safe(vdev, tmp, &drv->devices, drv_list) {
+		list_del(&vdev->drv_list);
+		list_del(&vdev->bridge_list);
+		device_unregister(&vdev->dev);
+	}
+	return err;
+}
+
+static int __vme_register_driver(struct vme_driver *drv, unsigned int ndevs)
+{
+	struct vme_bridge *bridge;
+	int err = 0;
+
+	mutex_lock(&vme_buses_lock);
+	list_for_each_entry(bridge, &vme_bus_list, bus_list) {
+		/*
+		 * This cannot cause trouble as we already have vme_buses_lock
+		 * and if the bridge is removed, it will have to go through
+		 * vme_unregister_bridge() to do it (which calls remove() on
+		 * the bridge which in turn tries to acquire vme_buses_lock and
+		 * will have to wait).
+		 */
+		err = __vme_register_driver_bus(drv, bridge, ndevs);
+		if (err)
+			break;
+	}
+	mutex_unlock(&vme_buses_lock);
+	return err;
+}
+
+/**
+ * vme_register_driver - Register a VME driver
+ * @drv: Pointer to VME driver structure to register.
+ * @ndevs: Maximum number of devices to allow to be enumerated.
+ *
+ * Register a VME device driver with the VME subsystem.
+ *
+ * Return: Zero on success, error value on registration failure.
+ */
+int vme_register_driver(struct vme_driver *drv, unsigned int ndevs)
+{
+	int err;
+
+	drv->driver.name = drv->name;
+	drv->driver.bus = &vme_bus_type;
+	INIT_LIST_HEAD(&drv->devices);
+
+	err = driver_register(&drv->driver);
+	if (err)
+		return err;
+
+	err = __vme_register_driver(drv, ndevs);
+	if (err)
+		driver_unregister(&drv->driver);
+
+	return err;
+}
+EXPORT_SYMBOL(vme_register_driver);
+
+/**
+ * vme_unregister_driver - Unregister a VME driver
+ * @drv: Pointer to VME driver structure to unregister.
+ *
+ * Unregister a VME device driver from the VME subsystem.
+ */
+void vme_unregister_driver(struct vme_driver *drv)
+{
+	struct vme_dev *dev, *dev_tmp;
+
+	mutex_lock(&vme_buses_lock);
+	list_for_each_entry_safe(dev, dev_tmp, &drv->devices, drv_list) {
+		list_del(&dev->drv_list);
+		list_del(&dev->bridge_list);
+		device_unregister(&dev->dev);
+	}
+	mutex_unlock(&vme_buses_lock);
+
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL(vme_unregister_driver);
+
+/* - Bus Registration ------------------------------------------------------ */
+
+static int vme_bus_match(struct device *dev, struct device_driver *drv)
+{
+	struct vme_driver *vme_drv;
+
+	vme_drv = container_of(drv, struct vme_driver, driver);
+
+	if (dev->platform_data == vme_drv) {
+		struct vme_dev *vdev = dev_to_vme_dev(dev);
+
+		if (vme_drv->match && vme_drv->match(vdev))
+			return 1;
+
+		dev->platform_data = NULL;
+	}
+	return 0;
+}
+
+static int vme_bus_probe(struct device *dev)
+{
+	struct vme_driver *driver;
+	struct vme_dev *vdev = dev_to_vme_dev(dev);
+
+	driver = dev->platform_data;
+	if (driver->probe)
+		return driver->probe(vdev);
+
+	return -ENODEV;
+}
+
+static void vme_bus_remove(struct device *dev)
+{
+	struct vme_driver *driver;
+	struct vme_dev *vdev = dev_to_vme_dev(dev);
+
+	driver = dev->platform_data;
+	if (driver->remove)
+		driver->remove(vdev);
+}
+
+struct bus_type vme_bus_type = {
+	.name = "vme",
+	.match = vme_bus_match,
+	.probe = vme_bus_probe,
+	.remove = vme_bus_remove,
+};
+EXPORT_SYMBOL(vme_bus_type);
+
+static int __init vme_init(void)
+{
+	return bus_register(&vme_bus_type);
+}
+subsys_initcall(vme_init);
diff --git a/drivers/staging/vme_user/vme.h b/drivers/staging/vme_user/vme.h
new file mode 100644
index 000000000..b204a9b4b
--- /dev/null
+++ b/drivers/staging/vme_user/vme.h
@@ -0,0 +1,190 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _VME_H_
+#define _VME_H_
+
+/* Resource Type */
+enum vme_resource_type {
+	VME_MASTER,
+	VME_SLAVE,
+	VME_DMA,
+	VME_LM
+};
+
+/* VME Address Spaces */
+#define VME_A16		0x1
+#define VME_A24		0x2
+#define	VME_A32		0x4
+#define VME_A64		0x8
+#define VME_CRCSR	0x10
+#define VME_USER1	0x20
+#define VME_USER2	0x40
+#define VME_USER3	0x80
+#define VME_USER4	0x100
+
+#define VME_A16_MAX	0x10000ULL
+#define VME_A24_MAX	0x1000000ULL
+#define VME_A32_MAX	0x100000000ULL
+#define VME_A64_MAX	0x10000000000000000ULL
+#define VME_CRCSR_MAX	0x1000000ULL
+
+
+/* VME Cycle Types */
+#define VME_SCT		0x1
+#define VME_BLT		0x2
+#define VME_MBLT	0x4
+#define VME_2eVME	0x8
+#define VME_2eSST	0x10
+#define VME_2eSSTB	0x20
+
+#define VME_2eSST160	0x100
+#define VME_2eSST267	0x200
+#define VME_2eSST320	0x400
+
+#define	VME_SUPER	0x1000
+#define	VME_USER	0x2000
+#define	VME_PROG	0x4000
+#define	VME_DATA	0x8000
+
+/* VME Data Widths */
+#define VME_D8		0x1
+#define VME_D16		0x2
+#define VME_D32		0x4
+#define VME_D64		0x8
+
+/* Arbitration Scheduling Modes */
+#define VME_R_ROBIN_MODE	0x1
+#define VME_PRIORITY_MODE	0x2
+
+#define VME_DMA_PATTERN			(1<<0)
+#define VME_DMA_PCI			(1<<1)
+#define VME_DMA_VME			(1<<2)
+
+#define VME_DMA_PATTERN_BYTE		(1<<0)
+#define VME_DMA_PATTERN_WORD		(1<<1)
+#define VME_DMA_PATTERN_INCREMENT	(1<<2)
+
+#define VME_DMA_VME_TO_MEM		(1<<0)
+#define VME_DMA_MEM_TO_VME		(1<<1)
+#define VME_DMA_VME_TO_VME		(1<<2)
+#define VME_DMA_MEM_TO_MEM		(1<<3)
+#define VME_DMA_PATTERN_TO_VME		(1<<4)
+#define VME_DMA_PATTERN_TO_MEM		(1<<5)
+
+struct vme_dma_attr {
+	u32 type;
+	void *private;
+};
+
+struct vme_resource {
+	enum vme_resource_type type;
+	struct list_head *entry;
+};
+
+extern struct bus_type vme_bus_type;
+
+/* Number of VME interrupt vectors */
+#define VME_NUM_STATUSID	256
+
+/* VME_MAX_BRIDGES comes from the type of vme_bus_numbers */
+#define VME_MAX_BRIDGES		(sizeof(unsigned int)*8)
+#define VME_MAX_SLOTS		32
+
+#define VME_SLOT_CURRENT	-1
+#define VME_SLOT_ALL		-2
+
+/**
+ * struct vme_dev - Structure representing a VME device
+ * @num: The device number
+ * @bridge: Pointer to the bridge device this device is on
+ * @dev: Internal device structure
+ * @drv_list: List of devices (per driver)
+ * @bridge_list: List of devices (per bridge)
+ */
+struct vme_dev {
+	int num;
+	struct vme_bridge *bridge;
+	struct device dev;
+	struct list_head drv_list;
+	struct list_head bridge_list;
+};
+
+/**
+ * struct vme_driver - Structure representing a VME driver
+ * @name: Driver name, should be unique among VME drivers and usually the same
+ *        as the module name.
+ * @match: Callback used to determine whether probe should be run.
+ * @probe: Callback for device binding, called when new device is detected.
+ * @remove: Callback, called on device removal.
+ * @driver: Underlying generic device driver structure.
+ * @devices: List of VME devices (struct vme_dev) associated with this driver.
+ */
+struct vme_driver {
+	const char *name;
+	int (*match)(struct vme_dev *);
+	int (*probe)(struct vme_dev *);
+	void (*remove)(struct vme_dev *);
+	struct device_driver driver;
+	struct list_head devices;
+};
+
+void *vme_alloc_consistent(struct vme_resource *, size_t, dma_addr_t *);
+void vme_free_consistent(struct vme_resource *, size_t,  void *,
+	dma_addr_t);
+
+size_t vme_get_size(struct vme_resource *);
+int vme_check_window(u32 aspace, unsigned long long vme_base,
+		     unsigned long long size);
+
+struct vme_resource *vme_slave_request(struct vme_dev *, u32, u32);
+int vme_slave_set(struct vme_resource *, int, unsigned long long,
+	unsigned long long, dma_addr_t, u32, u32);
+int vme_slave_get(struct vme_resource *, int *, unsigned long long *,
+	unsigned long long *, dma_addr_t *, u32 *, u32 *);
+void vme_slave_free(struct vme_resource *);
+
+struct vme_resource *vme_master_request(struct vme_dev *, u32, u32, u32);
+int vme_master_set(struct vme_resource *, int, unsigned long long,
+	unsigned long long, u32, u32, u32);
+int vme_master_get(struct vme_resource *, int *, unsigned long long *,
+	unsigned long long *, u32 *, u32 *, u32 *);
+ssize_t vme_master_read(struct vme_resource *, void *, size_t, loff_t);
+ssize_t vme_master_write(struct vme_resource *, void *, size_t, loff_t);
+unsigned int vme_master_rmw(struct vme_resource *, unsigned int, unsigned int,
+	unsigned int, loff_t);
+int vme_master_mmap(struct vme_resource *resource, struct vm_area_struct *vma);
+void vme_master_free(struct vme_resource *);
+
+struct vme_resource *vme_dma_request(struct vme_dev *, u32);
+struct vme_dma_list *vme_new_dma_list(struct vme_resource *);
+struct vme_dma_attr *vme_dma_pattern_attribute(u32, u32);
+struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t);
+struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long, u32, u32, u32);
+void vme_dma_free_attribute(struct vme_dma_attr *);
+int vme_dma_list_add(struct vme_dma_list *, struct vme_dma_attr *,
+	struct vme_dma_attr *, size_t);
+int vme_dma_list_exec(struct vme_dma_list *);
+int vme_dma_list_free(struct vme_dma_list *);
+int vme_dma_free(struct vme_resource *);
+
+int vme_irq_request(struct vme_dev *, int, int,
+	void (*callback)(int, int, void *), void *);
+void vme_irq_free(struct vme_dev *, int, int);
+int vme_irq_generate(struct vme_dev *, int, int);
+
+struct vme_resource *vme_lm_request(struct vme_dev *);
+int vme_lm_count(struct vme_resource *);
+int vme_lm_set(struct vme_resource *, unsigned long long, u32, u32);
+int vme_lm_get(struct vme_resource *, unsigned long long *, u32 *, u32 *);
+int vme_lm_attach(struct vme_resource *, int, void (*callback)(void *), void *);
+int vme_lm_detach(struct vme_resource *, int);
+void vme_lm_free(struct vme_resource *);
+
+int vme_slot_num(struct vme_dev *);
+int vme_bus_num(struct vme_dev *);
+
+int vme_register_driver(struct vme_driver *, unsigned int);
+void vme_unregister_driver(struct vme_driver *);
+
+
+#endif /* _VME_H_ */
+
diff --git a/drivers/staging/vme_user/vme_bridge.h b/drivers/staging/vme_user/vme_bridge.h
new file mode 100644
index 000000000..0bbefe985
--- /dev/null
+++ b/drivers/staging/vme_user/vme_bridge.h
@@ -0,0 +1,190 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _VME_BRIDGE_H_
+#define _VME_BRIDGE_H_
+
+#include "vme.h"
+
+#define VME_CRCSR_BUF_SIZE (508*1024)
+/*
+ * Resource structures
+ */
+struct vme_master_resource {
+	struct list_head list;
+	struct vme_bridge *parent;
+	/*
+	 * We are likely to need to access the VME bus in interrupt context, so
+	 * protect master routines with a spinlock rather than a mutex.
+	 */
+	spinlock_t lock;
+	int locked;
+	int number;
+	u32 address_attr;
+	u32 cycle_attr;
+	u32 width_attr;
+	struct resource bus_resource;
+	void __iomem *kern_base;
+};
+
+struct vme_slave_resource {
+	struct list_head list;
+	struct vme_bridge *parent;
+	struct mutex mtx;
+	int locked;
+	int number;
+	u32 address_attr;
+	u32 cycle_attr;
+};
+
+struct vme_dma_pattern {
+	u32 pattern;
+	u32 type;
+};
+
+struct vme_dma_pci {
+	dma_addr_t address;
+};
+
+struct vme_dma_vme {
+	unsigned long long address;
+	u32 aspace;
+	u32 cycle;
+	u32 dwidth;
+};
+
+struct vme_dma_list {
+	struct list_head list;
+	struct vme_dma_resource *parent;
+	struct list_head entries;
+	struct mutex mtx;
+};
+
+struct vme_dma_resource {
+	struct list_head list;
+	struct vme_bridge *parent;
+	struct mutex mtx;
+	int locked;
+	int number;
+	struct list_head pending;
+	struct list_head running;
+	u32 route_attr;
+};
+
+struct vme_lm_resource {
+	struct list_head list;
+	struct vme_bridge *parent;
+	struct mutex mtx;
+	int locked;
+	int number;
+	int monitors;
+};
+
+struct vme_error_handler {
+	struct list_head list;
+	unsigned long long start;	/* Beginning of error window */
+	unsigned long long end;		/* End of error window */
+	unsigned long long first_error;	/* Address of the first error */
+	u32 aspace;			/* Address space of error window*/
+	unsigned num_errors;		/* Number of errors */
+};
+
+struct vme_callback {
+	void (*func)(int, int, void*);
+	void *priv_data;
+};
+
+struct vme_irq {
+	int count;
+	struct vme_callback callback[VME_NUM_STATUSID];
+};
+
+/* Allow 16 characters for name (including null character) */
+#define VMENAMSIZ 16
+
+/* This structure stores all the information about one bridge
+ * The structure should be dynamically allocated by the driver and one instance
+ * of the structure should be present for each VME chip present in the system.
+ */
+struct vme_bridge {
+	char name[VMENAMSIZ];
+	int num;
+	struct list_head master_resources;
+	struct list_head slave_resources;
+	struct list_head dma_resources;
+	struct list_head lm_resources;
+
+	/* List for registered errors handlers */
+	struct list_head vme_error_handlers;
+	/* List of devices on this bridge */
+	struct list_head devices;
+
+	/* Bridge Info - XXX Move to private structure? */
+	struct device *parent;	/* Parent device (eg. pdev->dev for PCI) */
+	void *driver_priv;	/* Private pointer for the bridge driver */
+	struct list_head bus_list; /* list of VME buses */
+
+	/* Interrupt callbacks */
+	struct vme_irq irq[7];
+	/* Locking for VME irq callback configuration */
+	struct mutex irq_mtx;
+
+	/* Slave Functions */
+	int (*slave_get) (struct vme_slave_resource *, int *,
+		unsigned long long *, unsigned long long *, dma_addr_t *,
+		u32 *, u32 *);
+	int (*slave_set) (struct vme_slave_resource *, int, unsigned long long,
+		unsigned long long, dma_addr_t, u32, u32);
+
+	/* Master Functions */
+	int (*master_get) (struct vme_master_resource *, int *,
+		unsigned long long *, unsigned long long *, u32 *, u32 *,
+		u32 *);
+	int (*master_set) (struct vme_master_resource *, int,
+		unsigned long long, unsigned long long,  u32, u32, u32);
+	ssize_t (*master_read) (struct vme_master_resource *, void *, size_t,
+		loff_t);
+	ssize_t (*master_write) (struct vme_master_resource *, void *, size_t,
+		loff_t);
+	unsigned int (*master_rmw) (struct vme_master_resource *, unsigned int,
+		unsigned int, unsigned int, loff_t);
+
+	/* DMA Functions */
+	int (*dma_list_add) (struct vme_dma_list *, struct vme_dma_attr *,
+		struct vme_dma_attr *, size_t);
+	int (*dma_list_exec) (struct vme_dma_list *);
+	int (*dma_list_empty) (struct vme_dma_list *);
+
+	/* Interrupt Functions */
+	void (*irq_set) (struct vme_bridge *, int, int, int);
+	int (*irq_generate) (struct vme_bridge *, int, int);
+
+	/* Location monitor functions */
+	int (*lm_set) (struct vme_lm_resource *, unsigned long long, u32, u32);
+	int (*lm_get) (struct vme_lm_resource *, unsigned long long *, u32 *,
+		u32 *);
+	int (*lm_attach)(struct vme_lm_resource *, int,
+			 void (*callback)(void *), void *);
+	int (*lm_detach) (struct vme_lm_resource *, int);
+
+	/* CR/CSR space functions */
+	int (*slot_get) (struct vme_bridge *);
+
+	/* Bridge parent interface */
+	void *(*alloc_consistent)(struct device *dev, size_t size,
+		dma_addr_t *dma);
+	void (*free_consistent)(struct device *dev, size_t size,
+		void *vaddr, dma_addr_t dma);
+};
+
+void vme_bus_error_handler(struct vme_bridge *bridge,
+			   unsigned long long address, int am);
+void vme_irq_handler(struct vme_bridge *, int, int);
+
+struct vme_bridge *vme_init_bridge(struct vme_bridge *);
+int vme_register_bridge(struct vme_bridge *);
+void vme_unregister_bridge(struct vme_bridge *);
+struct vme_error_handler *vme_register_error_handler(
+	struct vme_bridge *bridge, u32 aspace,
+	unsigned long long address, size_t len);
+void vme_unregister_error_handler(struct vme_error_handler *handler);
+
+#endif /* _VME_BRIDGE_H_ */
diff --git a/drivers/staging/vme_user/vme_fake.c b/drivers/staging/vme_user/vme_fake.c
new file mode 100644
index 000000000..1ee432c22
--- /dev/null
+++ b/drivers/staging/vme_user/vme_fake.c
@@ -0,0 +1,1307 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Fake VME bridge support.
+ *
+ * This drive provides a fake VME bridge chip, this enables debugging of the
+ * VME framework in the absence of a VME system.
+ *
+ * This driver has to do a number of things in software that would be driven
+ * by hardware if it was available, it will also result in extra overhead at
+ * times when compared with driving actual hardware.
+ *
+ * Author: Martyn Welch <martyn@welches.me.uk>
+ * Copyright (c) 2014 Martyn Welch
+ *
+ * Based on vme_tsi148.c:
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2008 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * Based on work by Tom Armistead and Ajit Prem
+ * Copyright 2004 Motorola Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include "vme.h"
+#include "vme_bridge.h"
+
+/*
+ *  Define the number of each that the fake driver supports.
+ */
+#define FAKE_MAX_MASTER		8	/* Max Master Windows */
+#define FAKE_MAX_SLAVE		8	/* Max Slave Windows */
+
+/* Structures to hold information normally held in device registers */
+struct fake_slave_window {
+	int enabled;
+	unsigned long long vme_base;
+	unsigned long long size;
+	void *buf_base;
+	u32 aspace;
+	u32 cycle;
+};
+
+struct fake_master_window {
+	int enabled;
+	unsigned long long vme_base;
+	unsigned long long size;
+	u32 aspace;
+	u32 cycle;
+	u32 dwidth;
+};
+
+/* Structure used to hold driver specific information */
+struct fake_driver {
+	struct vme_bridge *parent;
+	struct fake_slave_window slaves[FAKE_MAX_SLAVE];
+	struct fake_master_window masters[FAKE_MAX_MASTER];
+	u32 lm_enabled;
+	unsigned long long lm_base;
+	u32 lm_aspace;
+	u32 lm_cycle;
+	void (*lm_callback[4])(void *);
+	void *lm_data[4];
+	struct tasklet_struct int_tasklet;
+	int int_level;
+	int int_statid;
+	void *crcsr_kernel;
+	dma_addr_t crcsr_bus;
+	/* Only one VME interrupt can be generated at a time, provide locking */
+	struct mutex vme_int;
+};
+
+/* Module parameter */
+static int geoid;
+
+static const char driver_name[] = "vme_fake";
+
+static struct vme_bridge *exit_pointer;
+
+static struct device *vme_root;
+
+/*
+ * Calling VME bus interrupt callback if provided.
+ */
+static void fake_VIRQ_tasklet(unsigned long data)
+{
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = (struct vme_bridge *) data;
+	bridge = fake_bridge->driver_priv;
+
+	vme_irq_handler(fake_bridge, bridge->int_level, bridge->int_statid);
+}
+
+/*
+ * Configure VME interrupt
+ */
+static void fake_irq_set(struct vme_bridge *fake_bridge, int level,
+		int state, int sync)
+{
+	/* Nothing to do */
+}
+
+static void *fake_pci_to_ptr(dma_addr_t addr)
+{
+	return (void *)(uintptr_t)addr;
+}
+
+static dma_addr_t fake_ptr_to_pci(void *addr)
+{
+	return (dma_addr_t)(uintptr_t)addr;
+}
+
+/*
+ * Generate a VME bus interrupt at the requested level & vector. Wait for
+ * interrupt to be acked.
+ */
+static int fake_irq_generate(struct vme_bridge *fake_bridge, int level,
+		int statid)
+{
+	struct fake_driver *bridge;
+
+	bridge = fake_bridge->driver_priv;
+
+	mutex_lock(&bridge->vme_int);
+
+	bridge->int_level = level;
+
+	bridge->int_statid = statid;
+
+	/*
+	 * Schedule tasklet to run VME handler to emulate normal VME interrupt
+	 * handler behaviour.
+	 */
+	tasklet_schedule(&bridge->int_tasklet);
+
+	mutex_unlock(&bridge->vme_int);
+
+	return 0;
+}
+
+/*
+ * Initialize a slave window with the requested attributes.
+ */
+static int fake_slave_set(struct vme_slave_resource *image, int enabled,
+		unsigned long long vme_base, unsigned long long size,
+		dma_addr_t buf_base, u32 aspace, u32 cycle)
+{
+	unsigned int i, granularity = 0;
+	unsigned long long vme_bound;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = image->parent;
+	bridge = fake_bridge->driver_priv;
+
+	i = image->number;
+
+	switch (aspace) {
+	case VME_A16:
+		granularity = 0x10;
+		break;
+	case VME_A24:
+		granularity = 0x1000;
+		break;
+	case VME_A32:
+		granularity = 0x10000;
+		break;
+	case VME_A64:
+		granularity = 0x10000;
+		break;
+	case VME_CRCSR:
+	case VME_USER1:
+	case VME_USER2:
+	case VME_USER3:
+	case VME_USER4:
+	default:
+		pr_err("Invalid address space\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Bound address is a valid address for the window, adjust
+	 * accordingly
+	 */
+	vme_bound = vme_base + size - granularity;
+
+	if (vme_base & (granularity - 1)) {
+		pr_err("Invalid VME base alignment\n");
+		return -EINVAL;
+	}
+	if (vme_bound & (granularity - 1)) {
+		pr_err("Invalid VME bound alignment\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&image->mtx);
+
+	bridge->slaves[i].enabled = enabled;
+	bridge->slaves[i].vme_base = vme_base;
+	bridge->slaves[i].size = size;
+	bridge->slaves[i].buf_base = fake_pci_to_ptr(buf_base);
+	bridge->slaves[i].aspace = aspace;
+	bridge->slaves[i].cycle = cycle;
+
+	mutex_unlock(&image->mtx);
+
+	return 0;
+}
+
+/*
+ * Get slave window configuration.
+ */
+static int fake_slave_get(struct vme_slave_resource *image, int *enabled,
+		unsigned long long *vme_base, unsigned long long *size,
+		dma_addr_t *buf_base, u32 *aspace, u32 *cycle)
+{
+	unsigned int i;
+	struct fake_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	i = image->number;
+
+	mutex_lock(&image->mtx);
+
+	*enabled = bridge->slaves[i].enabled;
+	*vme_base = bridge->slaves[i].vme_base;
+	*size = bridge->slaves[i].size;
+	*buf_base = fake_ptr_to_pci(bridge->slaves[i].buf_base);
+	*aspace = bridge->slaves[i].aspace;
+	*cycle = bridge->slaves[i].cycle;
+
+	mutex_unlock(&image->mtx);
+
+	return 0;
+}
+
+/*
+ * Set the attributes of an outbound window.
+ */
+static int fake_master_set(struct vme_master_resource *image, int enabled,
+		unsigned long long vme_base, unsigned long long size,
+		u32 aspace, u32 cycle, u32 dwidth)
+{
+	int retval = 0;
+	unsigned int i;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = image->parent;
+
+	bridge = fake_bridge->driver_priv;
+
+	/* Verify input data */
+	if (vme_base & 0xFFFF) {
+		pr_err("Invalid VME Window alignment\n");
+		retval = -EINVAL;
+		goto err_window;
+	}
+
+	if (size & 0xFFFF) {
+		pr_err("Invalid size alignment\n");
+		retval = -EINVAL;
+		goto err_window;
+	}
+
+	if ((size == 0) && (enabled != 0)) {
+		pr_err("Size must be non-zero for enabled windows\n");
+		retval = -EINVAL;
+		goto err_window;
+	}
+
+	/* Setup data width */
+	switch (dwidth) {
+	case VME_D8:
+	case VME_D16:
+	case VME_D32:
+		break;
+	default:
+		pr_err("Invalid data width\n");
+		retval = -EINVAL;
+		goto err_dwidth;
+	}
+
+	/* Setup address space */
+	switch (aspace) {
+	case VME_A16:
+	case VME_A24:
+	case VME_A32:
+	case VME_A64:
+	case VME_CRCSR:
+	case VME_USER1:
+	case VME_USER2:
+	case VME_USER3:
+	case VME_USER4:
+		break;
+	default:
+		pr_err("Invalid address space\n");
+		retval = -EINVAL;
+		goto err_aspace;
+	}
+
+	spin_lock(&image->lock);
+
+	i = image->number;
+
+	bridge->masters[i].enabled = enabled;
+	bridge->masters[i].vme_base = vme_base;
+	bridge->masters[i].size = size;
+	bridge->masters[i].aspace = aspace;
+	bridge->masters[i].cycle = cycle;
+	bridge->masters[i].dwidth = dwidth;
+
+	spin_unlock(&image->lock);
+
+	return 0;
+
+err_aspace:
+err_dwidth:
+err_window:
+	return retval;
+
+}
+
+/*
+ * Set the attributes of an outbound window.
+ */
+static int __fake_master_get(struct vme_master_resource *image, int *enabled,
+		unsigned long long *vme_base, unsigned long long *size,
+		u32 *aspace, u32 *cycle, u32 *dwidth)
+{
+	unsigned int i;
+	struct fake_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	i = image->number;
+
+	*enabled = bridge->masters[i].enabled;
+	*vme_base = bridge->masters[i].vme_base;
+	*size = bridge->masters[i].size;
+	*aspace = bridge->masters[i].aspace;
+	*cycle = bridge->masters[i].cycle;
+	*dwidth = bridge->masters[i].dwidth;
+
+	return 0;
+}
+
+
+static int fake_master_get(struct vme_master_resource *image, int *enabled,
+		unsigned long long *vme_base, unsigned long long *size,
+		u32 *aspace, u32 *cycle, u32 *dwidth)
+{
+	int retval;
+
+	spin_lock(&image->lock);
+
+	retval = __fake_master_get(image, enabled, vme_base, size, aspace,
+			cycle, dwidth);
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+
+static void fake_lm_check(struct fake_driver *bridge, unsigned long long addr,
+			  u32 aspace, u32 cycle)
+{
+	struct vme_bridge *fake_bridge;
+	unsigned long long lm_base;
+	u32 lm_aspace, lm_cycle;
+	int i;
+	struct vme_lm_resource *lm;
+	struct list_head *pos = NULL, *n;
+
+	/* Get vme_bridge */
+	fake_bridge = bridge->parent;
+
+	/* Loop through each location monitor resource */
+	list_for_each_safe(pos, n, &fake_bridge->lm_resources) {
+		lm = list_entry(pos, struct vme_lm_resource, list);
+
+		/* If disabled, we're done */
+		if (bridge->lm_enabled == 0)
+			return;
+
+		lm_base = bridge->lm_base;
+		lm_aspace = bridge->lm_aspace;
+		lm_cycle = bridge->lm_cycle;
+
+		/* First make sure that the cycle and address space match */
+		if ((lm_aspace == aspace) && (lm_cycle == cycle)) {
+			for (i = 0; i < lm->monitors; i++) {
+				/* Each location monitor covers 8 bytes */
+				if (((lm_base + (8 * i)) <= addr) &&
+				    ((lm_base + (8 * i) + 8) > addr)) {
+					if (bridge->lm_callback[i])
+						bridge->lm_callback[i](
+							bridge->lm_data[i]);
+				}
+			}
+		}
+	}
+}
+
+static noinline_for_stack u8 fake_vmeread8(struct fake_driver *bridge,
+					   unsigned long long addr,
+					   u32 aspace, u32 cycle)
+{
+	u8 retval = 0xff;
+	int i;
+	unsigned long long start, end, offset;
+	u8 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		if ((addr >= start) && (addr < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u8 *)(bridge->slaves[i].buf_base + offset);
+			retval = *loc;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+	return retval;
+}
+
+static noinline_for_stack u16 fake_vmeread16(struct fake_driver *bridge,
+					     unsigned long long addr,
+					     u32 aspace, u32 cycle)
+{
+	u16 retval = 0xffff;
+	int i;
+	unsigned long long start, end, offset;
+	u16 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if ((addr >= start) && ((addr + 1) < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u16 *)(bridge->slaves[i].buf_base + offset);
+			retval = *loc;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+	return retval;
+}
+
+static noinline_for_stack u32 fake_vmeread32(struct fake_driver *bridge,
+					     unsigned long long addr,
+					     u32 aspace, u32 cycle)
+{
+	u32 retval = 0xffffffff;
+	int i;
+	unsigned long long start, end, offset;
+	u32 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if ((addr >= start) && ((addr + 3) < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u32 *)(bridge->slaves[i].buf_base + offset);
+			retval = *loc;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+	return retval;
+}
+
+static ssize_t fake_master_read(struct vme_master_resource *image, void *buf,
+		size_t count, loff_t offset)
+{
+	int retval;
+	u32 aspace, cycle, dwidth;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *priv;
+	int i;
+	unsigned long long addr;
+	unsigned int done = 0;
+	unsigned int count32;
+
+	fake_bridge = image->parent;
+
+	priv = fake_bridge->driver_priv;
+
+	i = image->number;
+
+	addr = (unsigned long long)priv->masters[i].vme_base + offset;
+	aspace = priv->masters[i].aspace;
+	cycle = priv->masters[i].cycle;
+	dwidth = priv->masters[i].dwidth;
+
+	spin_lock(&image->lock);
+
+	/* The following code handles VME address alignment. We cannot use
+	 * memcpy_xxx here because it may cut data transfers in to 8-bit
+	 * cycles when D16 or D32 cycles are required on the VME bus.
+	 * On the other hand, the bridge itself assures that the maximum data
+	 * cycle configured for the transfer is used and splits it
+	 * automatically for non-aligned addresses, so we don't want the
+	 * overhead of needlessly forcing small transfers for the entire cycle.
+	 */
+	if (addr & 0x1) {
+		*(u8 *)buf = fake_vmeread8(priv, addr, aspace, cycle);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+	if ((dwidth == VME_D16) || (dwidth == VME_D32)) {
+		if ((addr + done) & 0x2) {
+			if ((count - done) < 2) {
+				*(u8 *)(buf + done) = fake_vmeread8(priv,
+						addr + done, aspace, cycle);
+				done += 1;
+				goto out;
+			} else {
+				*(u16 *)(buf + done) = fake_vmeread16(priv,
+						addr + done, aspace, cycle);
+				done += 2;
+			}
+		}
+	}
+
+	if (dwidth == VME_D32) {
+		count32 = (count - done) & ~0x3;
+		while (done < count32) {
+			*(u32 *)(buf + done) = fake_vmeread32(priv, addr + done,
+					aspace, cycle);
+			done += 4;
+		}
+	} else if (dwidth == VME_D16) {
+		count32 = (count - done) & ~0x3;
+		while (done < count32) {
+			*(u16 *)(buf + done) = fake_vmeread16(priv, addr + done,
+					aspace, cycle);
+			done += 2;
+		}
+	} else if (dwidth == VME_D8) {
+		count32 = (count - done);
+		while (done < count32) {
+			*(u8 *)(buf + done) = fake_vmeread8(priv, addr + done,
+					aspace, cycle);
+			done += 1;
+		}
+
+	}
+
+	if ((dwidth == VME_D16) || (dwidth == VME_D32)) {
+		if ((count - done) & 0x2) {
+			*(u16 *)(buf + done) = fake_vmeread16(priv, addr + done,
+					aspace, cycle);
+			done += 2;
+		}
+	}
+	if ((count - done) & 0x1) {
+		*(u8 *)(buf + done) = fake_vmeread8(priv, addr + done, aspace,
+				cycle);
+		done += 1;
+	}
+
+out:
+	retval = count;
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+static noinline_for_stack void fake_vmewrite8(struct fake_driver *bridge,
+					      u8 *buf, unsigned long long addr,
+					      u32 aspace, u32 cycle)
+{
+	int i;
+	unsigned long long start, end, offset;
+	u8 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if ((addr >= start) && (addr < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u8 *)((void *)bridge->slaves[i].buf_base + offset);
+			*loc = *buf;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+}
+
+static noinline_for_stack void fake_vmewrite16(struct fake_driver *bridge,
+					       u16 *buf, unsigned long long addr,
+					       u32 aspace, u32 cycle)
+{
+	int i;
+	unsigned long long start, end, offset;
+	u16 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if ((addr >= start) && ((addr + 1) < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u16 *)((void *)bridge->slaves[i].buf_base + offset);
+			*loc = *buf;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+}
+
+static noinline_for_stack void fake_vmewrite32(struct fake_driver *bridge,
+					       u32 *buf, unsigned long long addr,
+					       u32 aspace, u32 cycle)
+{
+	int i;
+	unsigned long long start, end, offset;
+	u32 *loc;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		if (aspace != bridge->slaves[i].aspace)
+			continue;
+
+		if (cycle != bridge->slaves[i].cycle)
+			continue;
+
+		start = bridge->slaves[i].vme_base;
+		end = bridge->slaves[i].vme_base + bridge->slaves[i].size;
+
+		if ((addr >= start) && ((addr + 3) < end)) {
+			offset = addr - bridge->slaves[i].vme_base;
+			loc = (u32 *)((void *)bridge->slaves[i].buf_base + offset);
+			*loc = *buf;
+
+			break;
+		}
+	}
+
+	fake_lm_check(bridge, addr, aspace, cycle);
+
+}
+
+static ssize_t fake_master_write(struct vme_master_resource *image, void *buf,
+		size_t count, loff_t offset)
+{
+	int retval = 0;
+	u32 aspace, cycle, dwidth;
+	unsigned long long addr;
+	int i;
+	unsigned int done = 0;
+	unsigned int count32;
+
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = image->parent;
+
+	bridge = fake_bridge->driver_priv;
+
+	i = image->number;
+
+	addr = bridge->masters[i].vme_base + offset;
+	aspace = bridge->masters[i].aspace;
+	cycle = bridge->masters[i].cycle;
+	dwidth = bridge->masters[i].dwidth;
+
+	spin_lock(&image->lock);
+
+	/* Here we apply for the same strategy we do in master_read
+	 * function in order to assure the correct cycles.
+	 */
+	if (addr & 0x1) {
+		fake_vmewrite8(bridge, (u8 *)buf, addr, aspace, cycle);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+
+	if ((dwidth == VME_D16) || (dwidth == VME_D32)) {
+		if ((addr + done) & 0x2) {
+			if ((count - done) < 2) {
+				fake_vmewrite8(bridge, (u8 *)(buf + done),
+						addr + done, aspace, cycle);
+				done += 1;
+				goto out;
+			} else {
+				fake_vmewrite16(bridge, (u16 *)(buf + done),
+						addr + done, aspace, cycle);
+				done += 2;
+			}
+		}
+	}
+
+	if (dwidth == VME_D32) {
+		count32 = (count - done) & ~0x3;
+		while (done < count32) {
+			fake_vmewrite32(bridge, (u32 *)(buf + done),
+					addr + done, aspace, cycle);
+			done += 4;
+		}
+	} else if (dwidth == VME_D16) {
+		count32 = (count - done) & ~0x3;
+		while (done < count32) {
+			fake_vmewrite16(bridge, (u16 *)(buf + done),
+					addr + done, aspace, cycle);
+			done += 2;
+		}
+	} else if (dwidth == VME_D8) {
+		count32 = (count - done);
+		while (done < count32) {
+			fake_vmewrite8(bridge, (u8 *)(buf + done), addr + done,
+					aspace, cycle);
+			done += 1;
+		}
+
+	}
+
+	if ((dwidth == VME_D16) || (dwidth == VME_D32)) {
+		if ((count - done) & 0x2) {
+			fake_vmewrite16(bridge, (u16 *)(buf + done),
+					addr + done, aspace, cycle);
+			done += 2;
+		}
+	}
+
+	if ((count - done) & 0x1) {
+		fake_vmewrite8(bridge, (u8 *)(buf + done), addr + done, aspace,
+				cycle);
+		done += 1;
+	}
+
+out:
+	retval = count;
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+/*
+ * Perform an RMW cycle on the VME bus.
+ *
+ * Requires a previously configured master window, returns final value.
+ */
+static unsigned int fake_master_rmw(struct vme_master_resource *image,
+		unsigned int mask, unsigned int compare, unsigned int swap,
+		loff_t offset)
+{
+	u32 tmp, base;
+	u32 aspace, cycle;
+	int i;
+	struct fake_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	/* Find the PCI address that maps to the desired VME address */
+	i = image->number;
+
+	base = bridge->masters[i].vme_base;
+	aspace = bridge->masters[i].aspace;
+	cycle = bridge->masters[i].cycle;
+
+	/* Lock image */
+	spin_lock(&image->lock);
+
+	/* Read existing value */
+	tmp = fake_vmeread32(bridge, base + offset, aspace, cycle);
+
+	/* Perform check */
+	if ((tmp && mask) == (compare && mask)) {
+		tmp = tmp | (mask | swap);
+		tmp = tmp & (~mask | swap);
+
+		/* Write back */
+		fake_vmewrite32(bridge, &tmp, base + offset, aspace, cycle);
+	}
+
+	/* Unlock image */
+	spin_unlock(&image->lock);
+
+	return tmp;
+}
+
+/*
+ * All 4 location monitors reside at the same base - this is therefore a
+ * system wide configuration.
+ *
+ * This does not enable the LM monitor - that should be done when the first
+ * callback is attached and disabled when the last callback is removed.
+ */
+static int fake_lm_set(struct vme_lm_resource *lm, unsigned long long lm_base,
+		u32 aspace, u32 cycle)
+{
+	int i;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = lm->parent;
+
+	bridge = fake_bridge->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* If we already have a callback attached, we can't move it! */
+	for (i = 0; i < lm->monitors; i++) {
+		if (bridge->lm_callback[i]) {
+			mutex_unlock(&lm->mtx);
+			pr_err("Location monitor callback attached, can't reset\n");
+			return -EBUSY;
+		}
+	}
+
+	switch (aspace) {
+	case VME_A16:
+	case VME_A24:
+	case VME_A32:
+	case VME_A64:
+		break;
+	default:
+		mutex_unlock(&lm->mtx);
+		pr_err("Invalid address space\n");
+		return -EINVAL;
+	}
+
+	bridge->lm_base = lm_base;
+	bridge->lm_aspace = aspace;
+	bridge->lm_cycle = cycle;
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/* Get configuration of the callback monitor and return whether it is enabled
+ * or disabled.
+ */
+static int fake_lm_get(struct vme_lm_resource *lm,
+		unsigned long long *lm_base, u32 *aspace, u32 *cycle)
+{
+	struct fake_driver *bridge;
+
+	bridge = lm->parent->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	*lm_base = bridge->lm_base;
+	*aspace = bridge->lm_aspace;
+	*cycle = bridge->lm_cycle;
+
+	mutex_unlock(&lm->mtx);
+
+	return bridge->lm_enabled;
+}
+
+/*
+ * Attach a callback to a specific location monitor.
+ *
+ * Callback will be passed the monitor triggered.
+ */
+static int fake_lm_attach(struct vme_lm_resource *lm, int monitor,
+		void (*callback)(void *), void *data)
+{
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = lm->parent;
+
+	bridge = fake_bridge->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* Ensure that the location monitor is configured - need PGM or DATA */
+	if (bridge->lm_cycle == 0) {
+		mutex_unlock(&lm->mtx);
+		pr_err("Location monitor not properly configured\n");
+		return -EINVAL;
+	}
+
+	/* Check that a callback isn't already attached */
+	if (bridge->lm_callback[monitor]) {
+		mutex_unlock(&lm->mtx);
+		pr_err("Existing callback attached\n");
+		return -EBUSY;
+	}
+
+	/* Attach callback */
+	bridge->lm_callback[monitor] = callback;
+	bridge->lm_data[monitor] = data;
+
+	/* Ensure that global Location Monitor Enable set */
+	bridge->lm_enabled = 1;
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/*
+ * Detach a callback function forn a specific location monitor.
+ */
+static int fake_lm_detach(struct vme_lm_resource *lm, int monitor)
+{
+	u32 tmp;
+	int i;
+	struct fake_driver *bridge;
+
+	bridge = lm->parent->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* Detach callback */
+	bridge->lm_callback[monitor] = NULL;
+	bridge->lm_data[monitor] = NULL;
+
+	/* If all location monitors disabled, disable global Location Monitor */
+	tmp = 0;
+	for (i = 0; i < lm->monitors; i++) {
+		if (bridge->lm_callback[i])
+			tmp = 1;
+	}
+
+	if (tmp == 0)
+		bridge->lm_enabled = 0;
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/*
+ * Determine Geographical Addressing
+ */
+static int fake_slot_get(struct vme_bridge *fake_bridge)
+{
+	return geoid;
+}
+
+static void *fake_alloc_consistent(struct device *parent, size_t size,
+		dma_addr_t *dma)
+{
+	void *alloc = kmalloc(size, GFP_KERNEL);
+
+	if (alloc)
+		*dma = fake_ptr_to_pci(alloc);
+
+	return alloc;
+}
+
+static void fake_free_consistent(struct device *parent, size_t size,
+		void *vaddr, dma_addr_t dma)
+{
+	kfree(vaddr);
+/*
+	dma_free_coherent(parent, size, vaddr, dma);
+*/
+}
+
+/*
+ * Configure CR/CSR space
+ *
+ * Access to the CR/CSR can be configured at power-up. The location of the
+ * CR/CSR registers in the CR/CSR address space is determined by the boards
+ * Geographic address.
+ *
+ * Each board has a 512kB window, with the highest 4kB being used for the
+ * boards registers, this means there is a fix length 508kB window which must
+ * be mapped onto PCI memory.
+ */
+static int fake_crcsr_init(struct vme_bridge *fake_bridge)
+{
+	u32 vstat;
+	struct fake_driver *bridge;
+
+	bridge = fake_bridge->driver_priv;
+
+	/* Allocate mem for CR/CSR image */
+	bridge->crcsr_kernel = kzalloc(VME_CRCSR_BUF_SIZE, GFP_KERNEL);
+	bridge->crcsr_bus = fake_ptr_to_pci(bridge->crcsr_kernel);
+	if (!bridge->crcsr_kernel)
+		return -ENOMEM;
+
+	vstat = fake_slot_get(fake_bridge);
+
+	pr_info("CR/CSR Offset: %d\n", vstat);
+
+	return 0;
+}
+
+static void fake_crcsr_exit(struct vme_bridge *fake_bridge)
+{
+	struct fake_driver *bridge;
+
+	bridge = fake_bridge->driver_priv;
+
+	kfree(bridge->crcsr_kernel);
+}
+
+
+static int __init fake_init(void)
+{
+	int retval, i;
+	struct list_head *pos = NULL, *n;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *fake_device;
+	struct vme_master_resource *master_image;
+	struct vme_slave_resource *slave_image;
+	struct vme_lm_resource *lm;
+
+	/* We need a fake parent device */
+	vme_root = __root_device_register("vme", THIS_MODULE);
+	if (IS_ERR(vme_root))
+		return PTR_ERR(vme_root);
+
+	/* If we want to support more than one bridge at some point, we need to
+	 * dynamically allocate this so we get one per device.
+	 */
+	fake_bridge = kzalloc(sizeof(*fake_bridge), GFP_KERNEL);
+	if (!fake_bridge) {
+		retval = -ENOMEM;
+		goto err_struct;
+	}
+
+	fake_device = kzalloc(sizeof(*fake_device), GFP_KERNEL);
+	if (!fake_device) {
+		retval = -ENOMEM;
+		goto err_driver;
+	}
+
+	fake_bridge->driver_priv = fake_device;
+
+	fake_bridge->parent = vme_root;
+
+	fake_device->parent = fake_bridge;
+
+	/* Initialize wait queues & mutual exclusion flags */
+	mutex_init(&fake_device->vme_int);
+	mutex_init(&fake_bridge->irq_mtx);
+	tasklet_init(&fake_device->int_tasklet, fake_VIRQ_tasklet,
+			(unsigned long) fake_bridge);
+
+	strcpy(fake_bridge->name, driver_name);
+
+	/* Add master windows to list */
+	INIT_LIST_HEAD(&fake_bridge->master_resources);
+	for (i = 0; i < FAKE_MAX_MASTER; i++) {
+		master_image = kmalloc(sizeof(*master_image), GFP_KERNEL);
+		if (!master_image) {
+			retval = -ENOMEM;
+			goto err_master;
+		}
+		master_image->parent = fake_bridge;
+		spin_lock_init(&master_image->lock);
+		master_image->locked = 0;
+		master_image->number = i;
+		master_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
+			VME_A64;
+		master_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
+			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
+			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
+			VME_PROG | VME_DATA;
+		master_image->width_attr = VME_D16 | VME_D32;
+		memset(&master_image->bus_resource, 0,
+				sizeof(struct resource));
+		master_image->kern_base  = NULL;
+		list_add_tail(&master_image->list,
+				&fake_bridge->master_resources);
+	}
+
+	/* Add slave windows to list */
+	INIT_LIST_HEAD(&fake_bridge->slave_resources);
+	for (i = 0; i < FAKE_MAX_SLAVE; i++) {
+		slave_image = kmalloc(sizeof(*slave_image), GFP_KERNEL);
+		if (!slave_image) {
+			retval = -ENOMEM;
+			goto err_slave;
+		}
+		slave_image->parent = fake_bridge;
+		mutex_init(&slave_image->mtx);
+		slave_image->locked = 0;
+		slave_image->number = i;
+		slave_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
+			VME_A64 | VME_CRCSR | VME_USER1 | VME_USER2 |
+			VME_USER3 | VME_USER4;
+		slave_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
+			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
+			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
+			VME_PROG | VME_DATA;
+		list_add_tail(&slave_image->list,
+				&fake_bridge->slave_resources);
+	}
+
+	/* Add location monitor to list */
+	INIT_LIST_HEAD(&fake_bridge->lm_resources);
+	lm = kmalloc(sizeof(*lm), GFP_KERNEL);
+	if (!lm) {
+		retval = -ENOMEM;
+		goto err_lm;
+	}
+	lm->parent = fake_bridge;
+	mutex_init(&lm->mtx);
+	lm->locked = 0;
+	lm->number = 1;
+	lm->monitors = 4;
+	list_add_tail(&lm->list, &fake_bridge->lm_resources);
+
+	fake_bridge->slave_get = fake_slave_get;
+	fake_bridge->slave_set = fake_slave_set;
+	fake_bridge->master_get = fake_master_get;
+	fake_bridge->master_set = fake_master_set;
+	fake_bridge->master_read = fake_master_read;
+	fake_bridge->master_write = fake_master_write;
+	fake_bridge->master_rmw = fake_master_rmw;
+	fake_bridge->irq_set = fake_irq_set;
+	fake_bridge->irq_generate = fake_irq_generate;
+	fake_bridge->lm_set = fake_lm_set;
+	fake_bridge->lm_get = fake_lm_get;
+	fake_bridge->lm_attach = fake_lm_attach;
+	fake_bridge->lm_detach = fake_lm_detach;
+	fake_bridge->slot_get = fake_slot_get;
+	fake_bridge->alloc_consistent = fake_alloc_consistent;
+	fake_bridge->free_consistent = fake_free_consistent;
+
+	pr_info("Board is%s the VME system controller\n",
+			(geoid == 1) ? "" : " not");
+
+	pr_info("VME geographical address is set to %d\n", geoid);
+
+	retval = fake_crcsr_init(fake_bridge);
+	if (retval) {
+		pr_err("CR/CSR configuration failed.\n");
+		goto err_crcsr;
+	}
+
+	retval = vme_register_bridge(fake_bridge);
+	if (retval != 0) {
+		pr_err("Chip Registration failed.\n");
+		goto err_reg;
+	}
+
+	exit_pointer = fake_bridge;
+
+	return 0;
+
+err_reg:
+	fake_crcsr_exit(fake_bridge);
+err_crcsr:
+err_lm:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &fake_bridge->lm_resources) {
+		lm = list_entry(pos, struct vme_lm_resource, list);
+		list_del(pos);
+		kfree(lm);
+	}
+err_slave:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &fake_bridge->slave_resources) {
+		slave_image = list_entry(pos, struct vme_slave_resource, list);
+		list_del(pos);
+		kfree(slave_image);
+	}
+err_master:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &fake_bridge->master_resources) {
+		master_image = list_entry(pos, struct vme_master_resource,
+				list);
+		list_del(pos);
+		kfree(master_image);
+	}
+
+	kfree(fake_device);
+err_driver:
+	kfree(fake_bridge);
+err_struct:
+	return retval;
+
+}
+
+
+static void __exit fake_exit(void)
+{
+	struct list_head *pos = NULL;
+	struct list_head *tmplist;
+	struct vme_master_resource *master_image;
+	struct vme_slave_resource *slave_image;
+	int i;
+	struct vme_bridge *fake_bridge;
+	struct fake_driver *bridge;
+
+	fake_bridge = exit_pointer;
+
+	bridge = fake_bridge->driver_priv;
+
+	pr_debug("Driver is being unloaded.\n");
+
+	/*
+	 *  Shutdown all inbound and outbound windows.
+	 */
+	for (i = 0; i < FAKE_MAX_MASTER; i++)
+		bridge->masters[i].enabled = 0;
+
+	for (i = 0; i < FAKE_MAX_SLAVE; i++)
+		bridge->slaves[i].enabled = 0;
+
+	/*
+	 *  Shutdown Location monitor.
+	 */
+	bridge->lm_enabled = 0;
+
+	vme_unregister_bridge(fake_bridge);
+
+	fake_crcsr_exit(fake_bridge);
+	/* resources are stored in link list */
+	list_for_each_safe(pos, tmplist, &fake_bridge->slave_resources) {
+		slave_image = list_entry(pos, struct vme_slave_resource, list);
+		list_del(pos);
+		kfree(slave_image);
+	}
+
+	/* resources are stored in link list */
+	list_for_each_safe(pos, tmplist, &fake_bridge->master_resources) {
+		master_image = list_entry(pos, struct vme_master_resource,
+				list);
+		list_del(pos);
+		kfree(master_image);
+	}
+
+	kfree(fake_bridge->driver_priv);
+
+	kfree(fake_bridge);
+
+	root_device_unregister(vme_root);
+}
+
+
+MODULE_PARM_DESC(geoid, "Set geographical addressing");
+module_param(geoid, int, 0);
+
+MODULE_DESCRIPTION("Fake VME bridge driver");
+MODULE_LICENSE("GPL");
+
+module_init(fake_init);
+module_exit(fake_exit);
diff --git a/drivers/staging/vme_user/vme_tsi148.c b/drivers/staging/vme_user/vme_tsi148.c
new file mode 100644
index 000000000..0171f46d1
--- /dev/null
+++ b/drivers/staging/vme_user/vme_tsi148.c
@@ -0,0 +1,2644 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Support for the Tundra TSI148 VME-PCI Bridge Chip
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2008 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * Based on work by Tom Armistead and Ajit Prem
+ * Copyright 2004 Motorola Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/mm.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/proc_fs.h>
+#include <linux/pci.h>
+#include <linux/poll.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/byteorder/generic.h>
+
+#include "vme.h"
+#include "vme_bridge.h"
+#include "vme_tsi148.h"
+
+static int tsi148_probe(struct pci_dev *, const struct pci_device_id *);
+static void tsi148_remove(struct pci_dev *);
+
+
+/* Module parameter */
+static bool err_chk;
+static int geoid;
+
+static const char driver_name[] = "vme_tsi148";
+
+static const struct pci_device_id tsi148_ids[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_TUNDRA, PCI_DEVICE_ID_TUNDRA_TSI148) },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(pci, tsi148_ids);
+
+static struct pci_driver tsi148_driver = {
+	.name = driver_name,
+	.id_table = tsi148_ids,
+	.probe = tsi148_probe,
+	.remove = tsi148_remove,
+};
+
+static void reg_join(unsigned int high, unsigned int low,
+	unsigned long long *variable)
+{
+	*variable = (unsigned long long)high << 32;
+	*variable |= (unsigned long long)low;
+}
+
+static void reg_split(unsigned long long variable, unsigned int *high,
+	unsigned int *low)
+{
+	*low = (unsigned int)variable & 0xFFFFFFFF;
+	*high = (unsigned int)(variable >> 32);
+}
+
+/*
+ * Wakes up DMA queue.
+ */
+static u32 tsi148_DMA_irqhandler(struct tsi148_driver *bridge,
+	int channel_mask)
+{
+	u32 serviced = 0;
+
+	if (channel_mask & TSI148_LCSR_INTS_DMA0S) {
+		wake_up(&bridge->dma_queue[0]);
+		serviced |= TSI148_LCSR_INTC_DMA0C;
+	}
+	if (channel_mask & TSI148_LCSR_INTS_DMA1S) {
+		wake_up(&bridge->dma_queue[1]);
+		serviced |= TSI148_LCSR_INTC_DMA1C;
+	}
+
+	return serviced;
+}
+
+/*
+ * Wake up location monitor queue
+ */
+static u32 tsi148_LM_irqhandler(struct tsi148_driver *bridge, u32 stat)
+{
+	int i;
+	u32 serviced = 0;
+
+	for (i = 0; i < 4; i++) {
+		if (stat & TSI148_LCSR_INTS_LMS[i]) {
+			/* We only enable interrupts if the callback is set */
+			bridge->lm_callback[i](bridge->lm_data[i]);
+			serviced |= TSI148_LCSR_INTC_LMC[i];
+		}
+	}
+
+	return serviced;
+}
+
+/*
+ * Wake up mail box queue.
+ *
+ * XXX This functionality is not exposed up though API.
+ */
+static u32 tsi148_MB_irqhandler(struct vme_bridge *tsi148_bridge, u32 stat)
+{
+	int i;
+	u32 val;
+	u32 serviced = 0;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	for (i = 0; i < 4; i++) {
+		if (stat & TSI148_LCSR_INTS_MBS[i]) {
+			val = ioread32be(bridge->base +	TSI148_GCSR_MBOX[i]);
+			dev_err(tsi148_bridge->parent, "VME Mailbox %d received: 0x%x\n",
+				i, val);
+			serviced |= TSI148_LCSR_INTC_MBC[i];
+		}
+	}
+
+	return serviced;
+}
+
+/*
+ * Display error & status message when PERR (PCI) exception interrupt occurs.
+ */
+static u32 tsi148_PERR_irqhandler(struct vme_bridge *tsi148_bridge)
+{
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	dev_err(tsi148_bridge->parent, "PCI Exception at address: 0x%08x:%08x, attributes: %08x\n",
+		ioread32be(bridge->base + TSI148_LCSR_EDPAU),
+		ioread32be(bridge->base + TSI148_LCSR_EDPAL),
+		ioread32be(bridge->base + TSI148_LCSR_EDPAT));
+
+	dev_err(tsi148_bridge->parent, "PCI-X attribute reg: %08x, PCI-X split completion reg: %08x\n",
+		ioread32be(bridge->base + TSI148_LCSR_EDPXA),
+		ioread32be(bridge->base + TSI148_LCSR_EDPXS));
+
+	iowrite32be(TSI148_LCSR_EDPAT_EDPCL, bridge->base + TSI148_LCSR_EDPAT);
+
+	return TSI148_LCSR_INTC_PERRC;
+}
+
+/*
+ * Save address and status when VME error interrupt occurs.
+ */
+static u32 tsi148_VERR_irqhandler(struct vme_bridge *tsi148_bridge)
+{
+	unsigned int error_addr_high, error_addr_low;
+	unsigned long long error_addr;
+	u32 error_attrib;
+	int error_am;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	error_addr_high = ioread32be(bridge->base + TSI148_LCSR_VEAU);
+	error_addr_low = ioread32be(bridge->base + TSI148_LCSR_VEAL);
+	error_attrib = ioread32be(bridge->base + TSI148_LCSR_VEAT);
+	error_am = (error_attrib & TSI148_LCSR_VEAT_AM_M) >> 8;
+
+	reg_join(error_addr_high, error_addr_low, &error_addr);
+
+	/* Check for exception register overflow (we have lost error data) */
+	if (error_attrib & TSI148_LCSR_VEAT_VEOF)
+		dev_err(tsi148_bridge->parent, "VME Bus Exception Overflow Occurred\n");
+
+	if (err_chk)
+		vme_bus_error_handler(tsi148_bridge, error_addr, error_am);
+	else
+		dev_err(tsi148_bridge->parent,
+			"VME Bus Error at address: 0x%llx, attributes: %08x\n",
+			error_addr, error_attrib);
+
+	/* Clear Status */
+	iowrite32be(TSI148_LCSR_VEAT_VESCL, bridge->base + TSI148_LCSR_VEAT);
+
+	return TSI148_LCSR_INTC_VERRC;
+}
+
+/*
+ * Wake up IACK queue.
+ */
+static u32 tsi148_IACK_irqhandler(struct tsi148_driver *bridge)
+{
+	wake_up(&bridge->iack_queue);
+
+	return TSI148_LCSR_INTC_IACKC;
+}
+
+/*
+ * Calling VME bus interrupt callback if provided.
+ */
+static u32 tsi148_VIRQ_irqhandler(struct vme_bridge *tsi148_bridge,
+	u32 stat)
+{
+	int vec, i, serviced = 0;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	for (i = 7; i > 0; i--) {
+		if (stat & (1 << i)) {
+			/*
+			 * Note: Even though the registers are defined as
+			 * 32-bits in the spec, we only want to issue 8-bit
+			 * IACK cycles on the bus, read from offset 3.
+			 */
+			vec = ioread8(bridge->base + TSI148_LCSR_VIACK[i] + 3);
+
+			vme_irq_handler(tsi148_bridge, i, vec);
+
+			serviced |= (1 << i);
+		}
+	}
+
+	return serviced;
+}
+
+/*
+ * Top level interrupt handler.  Clears appropriate interrupt status bits and
+ * then calls appropriate sub handler(s).
+ */
+static irqreturn_t tsi148_irqhandler(int irq, void *ptr)
+{
+	u32 stat, enable, serviced = 0;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	tsi148_bridge = ptr;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	/* Determine which interrupts are unmasked and set */
+	enable = ioread32be(bridge->base + TSI148_LCSR_INTEO);
+	stat = ioread32be(bridge->base + TSI148_LCSR_INTS);
+
+	/* Only look at unmasked interrupts */
+	stat &= enable;
+
+	if (unlikely(!stat))
+		return IRQ_NONE;
+
+	/* Call subhandlers as appropriate */
+	/* DMA irqs */
+	if (stat & (TSI148_LCSR_INTS_DMA1S | TSI148_LCSR_INTS_DMA0S))
+		serviced |= tsi148_DMA_irqhandler(bridge, stat);
+
+	/* Location monitor irqs */
+	if (stat & (TSI148_LCSR_INTS_LM3S | TSI148_LCSR_INTS_LM2S |
+			TSI148_LCSR_INTS_LM1S | TSI148_LCSR_INTS_LM0S))
+		serviced |= tsi148_LM_irqhandler(bridge, stat);
+
+	/* Mail box irqs */
+	if (stat & (TSI148_LCSR_INTS_MB3S | TSI148_LCSR_INTS_MB2S |
+			TSI148_LCSR_INTS_MB1S | TSI148_LCSR_INTS_MB0S))
+		serviced |= tsi148_MB_irqhandler(tsi148_bridge, stat);
+
+	/* PCI bus error */
+	if (stat & TSI148_LCSR_INTS_PERRS)
+		serviced |= tsi148_PERR_irqhandler(tsi148_bridge);
+
+	/* VME bus error */
+	if (stat & TSI148_LCSR_INTS_VERRS)
+		serviced |= tsi148_VERR_irqhandler(tsi148_bridge);
+
+	/* IACK irq */
+	if (stat & TSI148_LCSR_INTS_IACKS)
+		serviced |= tsi148_IACK_irqhandler(bridge);
+
+	/* VME bus irqs */
+	if (stat & (TSI148_LCSR_INTS_IRQ7S | TSI148_LCSR_INTS_IRQ6S |
+			TSI148_LCSR_INTS_IRQ5S | TSI148_LCSR_INTS_IRQ4S |
+			TSI148_LCSR_INTS_IRQ3S | TSI148_LCSR_INTS_IRQ2S |
+			TSI148_LCSR_INTS_IRQ1S))
+		serviced |= tsi148_VIRQ_irqhandler(tsi148_bridge, stat);
+
+	/* Clear serviced interrupts */
+	iowrite32be(serviced, bridge->base + TSI148_LCSR_INTC);
+
+	return IRQ_HANDLED;
+}
+
+static int tsi148_irq_init(struct vme_bridge *tsi148_bridge)
+{
+	int result;
+	unsigned int tmp;
+	struct pci_dev *pdev;
+	struct tsi148_driver *bridge;
+
+	pdev = to_pci_dev(tsi148_bridge->parent);
+
+	bridge = tsi148_bridge->driver_priv;
+
+	result = request_irq(pdev->irq,
+			     tsi148_irqhandler,
+			     IRQF_SHARED,
+			     driver_name, tsi148_bridge);
+	if (result) {
+		dev_err(tsi148_bridge->parent, "Can't get assigned pci irq vector %02X\n",
+			pdev->irq);
+		return result;
+	}
+
+	/* Enable and unmask interrupts */
+	tmp = TSI148_LCSR_INTEO_DMA1EO | TSI148_LCSR_INTEO_DMA0EO |
+		TSI148_LCSR_INTEO_MB3EO | TSI148_LCSR_INTEO_MB2EO |
+		TSI148_LCSR_INTEO_MB1EO | TSI148_LCSR_INTEO_MB0EO |
+		TSI148_LCSR_INTEO_PERREO | TSI148_LCSR_INTEO_VERREO |
+		TSI148_LCSR_INTEO_IACKEO;
+
+	/* This leaves the following interrupts masked.
+	 * TSI148_LCSR_INTEO_VIEEO
+	 * TSI148_LCSR_INTEO_SYSFLEO
+	 * TSI148_LCSR_INTEO_ACFLEO
+	 */
+
+	/* Don't enable Location Monitor interrupts here - they will be
+	 * enabled when the location monitors are properly configured and
+	 * a callback has been attached.
+	 * TSI148_LCSR_INTEO_LM0EO
+	 * TSI148_LCSR_INTEO_LM1EO
+	 * TSI148_LCSR_INTEO_LM2EO
+	 * TSI148_LCSR_INTEO_LM3EO
+	 */
+
+	/* Don't enable VME interrupts until we add a handler, else the board
+	 * will respond to it and we don't want that unless it knows how to
+	 * properly deal with it.
+	 * TSI148_LCSR_INTEO_IRQ7EO
+	 * TSI148_LCSR_INTEO_IRQ6EO
+	 * TSI148_LCSR_INTEO_IRQ5EO
+	 * TSI148_LCSR_INTEO_IRQ4EO
+	 * TSI148_LCSR_INTEO_IRQ3EO
+	 * TSI148_LCSR_INTEO_IRQ2EO
+	 * TSI148_LCSR_INTEO_IRQ1EO
+	 */
+
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEN);
+
+	return 0;
+}
+
+static void tsi148_irq_exit(struct vme_bridge *tsi148_bridge,
+	struct pci_dev *pdev)
+{
+	struct tsi148_driver *bridge = tsi148_bridge->driver_priv;
+
+	/* Turn off interrupts */
+	iowrite32be(0x0, bridge->base + TSI148_LCSR_INTEO);
+	iowrite32be(0x0, bridge->base + TSI148_LCSR_INTEN);
+
+	/* Clear all interrupts */
+	iowrite32be(0xFFFFFFFF, bridge->base + TSI148_LCSR_INTC);
+
+	/* Detach interrupt handler */
+	free_irq(pdev->irq, tsi148_bridge);
+}
+
+/*
+ * Check to see if an IACk has been received, return true (1) or false (0).
+ */
+static int tsi148_iack_received(struct tsi148_driver *bridge)
+{
+	u32 tmp;
+
+	tmp = ioread32be(bridge->base + TSI148_LCSR_VICR);
+
+	if (tmp & TSI148_LCSR_VICR_IRQS)
+		return 0;
+	else
+		return 1;
+}
+
+/*
+ * Configure VME interrupt
+ */
+static void tsi148_irq_set(struct vme_bridge *tsi148_bridge, int level,
+	int state, int sync)
+{
+	struct pci_dev *pdev;
+	u32 tmp;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	/* We need to do the ordering differently for enabling and disabling */
+	if (state == 0) {
+		tmp = ioread32be(bridge->base + TSI148_LCSR_INTEN);
+		tmp &= ~TSI148_LCSR_INTEN_IRQEN[level - 1];
+		iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEN);
+
+		tmp = ioread32be(bridge->base + TSI148_LCSR_INTEO);
+		tmp &= ~TSI148_LCSR_INTEO_IRQEO[level - 1];
+		iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
+
+		if (sync != 0) {
+			pdev = to_pci_dev(tsi148_bridge->parent);
+			synchronize_irq(pdev->irq);
+		}
+	} else {
+		tmp = ioread32be(bridge->base + TSI148_LCSR_INTEO);
+		tmp |= TSI148_LCSR_INTEO_IRQEO[level - 1];
+		iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
+
+		tmp = ioread32be(bridge->base + TSI148_LCSR_INTEN);
+		tmp |= TSI148_LCSR_INTEN_IRQEN[level - 1];
+		iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEN);
+	}
+}
+
+/*
+ * Generate a VME bus interrupt at the requested level & vector. Wait for
+ * interrupt to be acked.
+ */
+static int tsi148_irq_generate(struct vme_bridge *tsi148_bridge, int level,
+	int statid)
+{
+	u32 tmp;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	mutex_lock(&bridge->vme_int);
+
+	/* Read VICR register */
+	tmp = ioread32be(bridge->base + TSI148_LCSR_VICR);
+
+	/* Set Status/ID */
+	tmp = (tmp & ~TSI148_LCSR_VICR_STID_M) |
+		(statid & TSI148_LCSR_VICR_STID_M);
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_VICR);
+
+	/* Assert VMEbus IRQ */
+	tmp = tmp | TSI148_LCSR_VICR_IRQL[level];
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_VICR);
+
+	/* XXX Consider implementing a timeout? */
+	wait_event_interruptible(bridge->iack_queue,
+		tsi148_iack_received(bridge));
+
+	mutex_unlock(&bridge->vme_int);
+
+	return 0;
+}
+
+/*
+ * Initialize a slave window with the requested attributes.
+ */
+static int tsi148_slave_set(struct vme_slave_resource *image, int enabled,
+	unsigned long long vme_base, unsigned long long size,
+	dma_addr_t pci_base, u32 aspace, u32 cycle)
+{
+	unsigned int i, addr = 0, granularity = 0;
+	unsigned int temp_ctl = 0;
+	unsigned int vme_base_low, vme_base_high;
+	unsigned int vme_bound_low, vme_bound_high;
+	unsigned int pci_offset_low, pci_offset_high;
+	unsigned long long vme_bound, pci_offset;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	tsi148_bridge = image->parent;
+	bridge = tsi148_bridge->driver_priv;
+
+	i = image->number;
+
+	switch (aspace) {
+	case VME_A16:
+		granularity = 0x10;
+		addr |= TSI148_LCSR_ITAT_AS_A16;
+		break;
+	case VME_A24:
+		granularity = 0x1000;
+		addr |= TSI148_LCSR_ITAT_AS_A24;
+		break;
+	case VME_A32:
+		granularity = 0x10000;
+		addr |= TSI148_LCSR_ITAT_AS_A32;
+		break;
+	case VME_A64:
+		granularity = 0x10000;
+		addr |= TSI148_LCSR_ITAT_AS_A64;
+		break;
+	default:
+		dev_err(tsi148_bridge->parent, "Invalid address space\n");
+		return -EINVAL;
+	}
+
+	/* Convert 64-bit variables to 2x 32-bit variables */
+	reg_split(vme_base, &vme_base_high, &vme_base_low);
+
+	/*
+	 * Bound address is a valid address for the window, adjust
+	 * accordingly
+	 */
+	vme_bound = vme_base + size - granularity;
+	reg_split(vme_bound, &vme_bound_high, &vme_bound_low);
+	pci_offset = (unsigned long long)pci_base - vme_base;
+	reg_split(pci_offset, &pci_offset_high, &pci_offset_low);
+
+	if (vme_base_low & (granularity - 1)) {
+		dev_err(tsi148_bridge->parent, "Invalid VME base alignment\n");
+		return -EINVAL;
+	}
+	if (vme_bound_low & (granularity - 1)) {
+		dev_err(tsi148_bridge->parent, "Invalid VME bound alignment\n");
+		return -EINVAL;
+	}
+	if (pci_offset_low & (granularity - 1)) {
+		dev_err(tsi148_bridge->parent, "Invalid PCI Offset alignment\n");
+		return -EINVAL;
+	}
+
+	/*  Disable while we are mucking around */
+	temp_ctl = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITAT);
+	temp_ctl &= ~TSI148_LCSR_ITAT_EN;
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITAT);
+
+	/* Setup mapping */
+	iowrite32be(vme_base_high, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITSAU);
+	iowrite32be(vme_base_low, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITSAL);
+	iowrite32be(vme_bound_high, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITEAU);
+	iowrite32be(vme_bound_low, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITEAL);
+	iowrite32be(pci_offset_high, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITOFU);
+	iowrite32be(pci_offset_low, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITOFL);
+
+	/* Setup 2eSST speeds */
+	temp_ctl &= ~TSI148_LCSR_ITAT_2eSSTM_M;
+	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
+	case VME_2eSST160:
+		temp_ctl |= TSI148_LCSR_ITAT_2eSSTM_160;
+		break;
+	case VME_2eSST267:
+		temp_ctl |= TSI148_LCSR_ITAT_2eSSTM_267;
+		break;
+	case VME_2eSST320:
+		temp_ctl |= TSI148_LCSR_ITAT_2eSSTM_320;
+		break;
+	}
+
+	/* Setup cycle types */
+	temp_ctl &= ~(0x1F << 7);
+	if (cycle & VME_BLT)
+		temp_ctl |= TSI148_LCSR_ITAT_BLT;
+	if (cycle & VME_MBLT)
+		temp_ctl |= TSI148_LCSR_ITAT_MBLT;
+	if (cycle & VME_2eVME)
+		temp_ctl |= TSI148_LCSR_ITAT_2eVME;
+	if (cycle & VME_2eSST)
+		temp_ctl |= TSI148_LCSR_ITAT_2eSST;
+	if (cycle & VME_2eSSTB)
+		temp_ctl |= TSI148_LCSR_ITAT_2eSSTB;
+
+	/* Setup address space */
+	temp_ctl &= ~TSI148_LCSR_ITAT_AS_M;
+	temp_ctl |= addr;
+
+	temp_ctl &= ~0xF;
+	if (cycle & VME_SUPER)
+		temp_ctl |= TSI148_LCSR_ITAT_SUPR;
+	if (cycle & VME_USER)
+		temp_ctl |= TSI148_LCSR_ITAT_NPRIV;
+	if (cycle & VME_PROG)
+		temp_ctl |= TSI148_LCSR_ITAT_PGM;
+	if (cycle & VME_DATA)
+		temp_ctl |= TSI148_LCSR_ITAT_DATA;
+
+	/* Write ctl reg without enable */
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITAT);
+
+	if (enabled)
+		temp_ctl |= TSI148_LCSR_ITAT_EN;
+
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITAT);
+
+	return 0;
+}
+
+/*
+ * Get slave window configuration.
+ */
+static int tsi148_slave_get(struct vme_slave_resource *image, int *enabled,
+	unsigned long long *vme_base, unsigned long long *size,
+	dma_addr_t *pci_base, u32 *aspace, u32 *cycle)
+{
+	unsigned int i, granularity = 0, ctl = 0;
+	unsigned int vme_base_low, vme_base_high;
+	unsigned int vme_bound_low, vme_bound_high;
+	unsigned int pci_offset_low, pci_offset_high;
+	unsigned long long vme_bound, pci_offset;
+	struct tsi148_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	i = image->number;
+
+	/* Read registers */
+	ctl = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITAT);
+
+	vme_base_high = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITSAU);
+	vme_base_low = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITSAL);
+	vme_bound_high = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITEAU);
+	vme_bound_low = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITEAL);
+	pci_offset_high = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITOFU);
+	pci_offset_low = ioread32be(bridge->base + TSI148_LCSR_IT[i] +
+		TSI148_LCSR_OFFSET_ITOFL);
+
+	/* Convert 64-bit variables to 2x 32-bit variables */
+	reg_join(vme_base_high, vme_base_low, vme_base);
+	reg_join(vme_bound_high, vme_bound_low, &vme_bound);
+	reg_join(pci_offset_high, pci_offset_low, &pci_offset);
+
+	*pci_base = (dma_addr_t)(*vme_base + pci_offset);
+
+	*enabled = 0;
+	*aspace = 0;
+	*cycle = 0;
+
+	if (ctl & TSI148_LCSR_ITAT_EN)
+		*enabled = 1;
+
+	if ((ctl & TSI148_LCSR_ITAT_AS_M) == TSI148_LCSR_ITAT_AS_A16) {
+		granularity = 0x10;
+		*aspace |= VME_A16;
+	}
+	if ((ctl & TSI148_LCSR_ITAT_AS_M) == TSI148_LCSR_ITAT_AS_A24) {
+		granularity = 0x1000;
+		*aspace |= VME_A24;
+	}
+	if ((ctl & TSI148_LCSR_ITAT_AS_M) == TSI148_LCSR_ITAT_AS_A32) {
+		granularity = 0x10000;
+		*aspace |= VME_A32;
+	}
+	if ((ctl & TSI148_LCSR_ITAT_AS_M) == TSI148_LCSR_ITAT_AS_A64) {
+		granularity = 0x10000;
+		*aspace |= VME_A64;
+	}
+
+	/* Need granularity before we set the size */
+	*size = (unsigned long long)((vme_bound - *vme_base) + granularity);
+
+
+	if ((ctl & TSI148_LCSR_ITAT_2eSSTM_M) == TSI148_LCSR_ITAT_2eSSTM_160)
+		*cycle |= VME_2eSST160;
+	if ((ctl & TSI148_LCSR_ITAT_2eSSTM_M) == TSI148_LCSR_ITAT_2eSSTM_267)
+		*cycle |= VME_2eSST267;
+	if ((ctl & TSI148_LCSR_ITAT_2eSSTM_M) == TSI148_LCSR_ITAT_2eSSTM_320)
+		*cycle |= VME_2eSST320;
+
+	if (ctl & TSI148_LCSR_ITAT_BLT)
+		*cycle |= VME_BLT;
+	if (ctl & TSI148_LCSR_ITAT_MBLT)
+		*cycle |= VME_MBLT;
+	if (ctl & TSI148_LCSR_ITAT_2eVME)
+		*cycle |= VME_2eVME;
+	if (ctl & TSI148_LCSR_ITAT_2eSST)
+		*cycle |= VME_2eSST;
+	if (ctl & TSI148_LCSR_ITAT_2eSSTB)
+		*cycle |= VME_2eSSTB;
+
+	if (ctl & TSI148_LCSR_ITAT_SUPR)
+		*cycle |= VME_SUPER;
+	if (ctl & TSI148_LCSR_ITAT_NPRIV)
+		*cycle |= VME_USER;
+	if (ctl & TSI148_LCSR_ITAT_PGM)
+		*cycle |= VME_PROG;
+	if (ctl & TSI148_LCSR_ITAT_DATA)
+		*cycle |= VME_DATA;
+
+	return 0;
+}
+
+/*
+ * Allocate and map PCI Resource
+ */
+static int tsi148_alloc_resource(struct vme_master_resource *image,
+	unsigned long long size)
+{
+	unsigned long long existing_size;
+	int retval = 0;
+	struct pci_dev *pdev;
+	struct vme_bridge *tsi148_bridge;
+
+	tsi148_bridge = image->parent;
+
+	pdev = to_pci_dev(tsi148_bridge->parent);
+
+	existing_size = (unsigned long long)(image->bus_resource.end -
+		image->bus_resource.start);
+
+	/* If the existing size is OK, return */
+	if ((size != 0) && (existing_size == (size - 1)))
+		return 0;
+
+	if (existing_size != 0) {
+		iounmap(image->kern_base);
+		image->kern_base = NULL;
+		kfree(image->bus_resource.name);
+		release_resource(&image->bus_resource);
+		memset(&image->bus_resource, 0, sizeof(image->bus_resource));
+	}
+
+	/* Exit here if size is zero */
+	if (size == 0)
+		return 0;
+
+	if (!image->bus_resource.name) {
+		image->bus_resource.name = kmalloc(VMENAMSIZ+3, GFP_ATOMIC);
+		if (!image->bus_resource.name) {
+			retval = -ENOMEM;
+			goto err_name;
+		}
+	}
+
+	sprintf((char *)image->bus_resource.name, "%s.%d", tsi148_bridge->name,
+		image->number);
+
+	image->bus_resource.start = 0;
+	image->bus_resource.end = (unsigned long)size;
+	image->bus_resource.flags = IORESOURCE_MEM;
+
+	retval = pci_bus_alloc_resource(pdev->bus,
+		&image->bus_resource, size, 0x10000, PCIBIOS_MIN_MEM,
+		0, NULL, NULL);
+	if (retval) {
+		dev_err(tsi148_bridge->parent, "Failed to allocate mem resource for window %d size 0x%lx start 0x%lx\n",
+			image->number, (unsigned long)size,
+			(unsigned long)image->bus_resource.start);
+		goto err_resource;
+	}
+
+	image->kern_base = ioremap(
+		image->bus_resource.start, size);
+	if (!image->kern_base) {
+		dev_err(tsi148_bridge->parent, "Failed to remap resource\n");
+		retval = -ENOMEM;
+		goto err_remap;
+	}
+
+	return 0;
+
+err_remap:
+	release_resource(&image->bus_resource);
+err_resource:
+	kfree(image->bus_resource.name);
+	memset(&image->bus_resource, 0, sizeof(image->bus_resource));
+err_name:
+	return retval;
+}
+
+/*
+ * Free and unmap PCI Resource
+ */
+static void tsi148_free_resource(struct vme_master_resource *image)
+{
+	iounmap(image->kern_base);
+	image->kern_base = NULL;
+	release_resource(&image->bus_resource);
+	kfree(image->bus_resource.name);
+	memset(&image->bus_resource, 0, sizeof(image->bus_resource));
+}
+
+/*
+ * Set the attributes of an outbound window.
+ */
+static int tsi148_master_set(struct vme_master_resource *image, int enabled,
+	unsigned long long vme_base, unsigned long long size, u32 aspace,
+	u32 cycle, u32 dwidth)
+{
+	int retval = 0;
+	unsigned int i;
+	unsigned int temp_ctl = 0;
+	unsigned int pci_base_low, pci_base_high;
+	unsigned int pci_bound_low, pci_bound_high;
+	unsigned int vme_offset_low, vme_offset_high;
+	unsigned long long pci_bound, vme_offset, pci_base;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+	struct pci_bus_region region;
+	struct pci_dev *pdev;
+
+	tsi148_bridge = image->parent;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	pdev = to_pci_dev(tsi148_bridge->parent);
+
+	/* Verify input data */
+	if (vme_base & 0xFFFF) {
+		dev_err(tsi148_bridge->parent, "Invalid VME Window alignment\n");
+		retval = -EINVAL;
+		goto err_window;
+	}
+
+	if ((size == 0) && (enabled != 0)) {
+		dev_err(tsi148_bridge->parent, "Size must be non-zero for enabled windows\n");
+		retval = -EINVAL;
+		goto err_window;
+	}
+
+	spin_lock(&image->lock);
+
+	/* Let's allocate the resource here rather than further up the stack as
+	 * it avoids pushing loads of bus dependent stuff up the stack. If size
+	 * is zero, any existing resource will be freed.
+	 */
+	retval = tsi148_alloc_resource(image, size);
+	if (retval) {
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Unable to allocate memory for resource\n");
+		goto err_res;
+	}
+
+	if (size == 0) {
+		pci_base = 0;
+		pci_bound = 0;
+		vme_offset = 0;
+	} else {
+		pcibios_resource_to_bus(pdev->bus, &region,
+					&image->bus_resource);
+		pci_base = region.start;
+
+		/*
+		 * Bound address is a valid address for the window, adjust
+		 * according to window granularity.
+		 */
+		pci_bound = pci_base + (size - 0x10000);
+		vme_offset = vme_base - pci_base;
+	}
+
+	/* Convert 64-bit variables to 2x 32-bit variables */
+	reg_split(pci_base, &pci_base_high, &pci_base_low);
+	reg_split(pci_bound, &pci_bound_high, &pci_bound_low);
+	reg_split(vme_offset, &vme_offset_high, &vme_offset_low);
+
+	if (pci_base_low & 0xFFFF) {
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Invalid PCI base alignment\n");
+		retval = -EINVAL;
+		goto err_gran;
+	}
+	if (pci_bound_low & 0xFFFF) {
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Invalid PCI bound alignment\n");
+		retval = -EINVAL;
+		goto err_gran;
+	}
+	if (vme_offset_low & 0xFFFF) {
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Invalid VME Offset alignment\n");
+		retval = -EINVAL;
+		goto err_gran;
+	}
+
+	i = image->number;
+
+	/* Disable while we are mucking around */
+	temp_ctl = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTAT);
+	temp_ctl &= ~TSI148_LCSR_OTAT_EN;
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTAT);
+
+	/* Setup 2eSST speeds */
+	temp_ctl &= ~TSI148_LCSR_OTAT_2eSSTM_M;
+	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
+	case VME_2eSST160:
+		temp_ctl |= TSI148_LCSR_OTAT_2eSSTM_160;
+		break;
+	case VME_2eSST267:
+		temp_ctl |= TSI148_LCSR_OTAT_2eSSTM_267;
+		break;
+	case VME_2eSST320:
+		temp_ctl |= TSI148_LCSR_OTAT_2eSSTM_320;
+		break;
+	}
+
+	/* Setup cycle types */
+	if (cycle & VME_BLT) {
+		temp_ctl &= ~TSI148_LCSR_OTAT_TM_M;
+		temp_ctl |= TSI148_LCSR_OTAT_TM_BLT;
+	}
+	if (cycle & VME_MBLT) {
+		temp_ctl &= ~TSI148_LCSR_OTAT_TM_M;
+		temp_ctl |= TSI148_LCSR_OTAT_TM_MBLT;
+	}
+	if (cycle & VME_2eVME) {
+		temp_ctl &= ~TSI148_LCSR_OTAT_TM_M;
+		temp_ctl |= TSI148_LCSR_OTAT_TM_2eVME;
+	}
+	if (cycle & VME_2eSST) {
+		temp_ctl &= ~TSI148_LCSR_OTAT_TM_M;
+		temp_ctl |= TSI148_LCSR_OTAT_TM_2eSST;
+	}
+	if (cycle & VME_2eSSTB) {
+		dev_warn(tsi148_bridge->parent, "Currently not setting Broadcast Select Registers\n");
+		temp_ctl &= ~TSI148_LCSR_OTAT_TM_M;
+		temp_ctl |= TSI148_LCSR_OTAT_TM_2eSSTB;
+	}
+
+	/* Setup data width */
+	temp_ctl &= ~TSI148_LCSR_OTAT_DBW_M;
+	switch (dwidth) {
+	case VME_D16:
+		temp_ctl |= TSI148_LCSR_OTAT_DBW_16;
+		break;
+	case VME_D32:
+		temp_ctl |= TSI148_LCSR_OTAT_DBW_32;
+		break;
+	default:
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Invalid data width\n");
+		retval = -EINVAL;
+		goto err_dwidth;
+	}
+
+	/* Setup address space */
+	temp_ctl &= ~TSI148_LCSR_OTAT_AMODE_M;
+	switch (aspace) {
+	case VME_A16:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_A16;
+		break;
+	case VME_A24:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_A24;
+		break;
+	case VME_A32:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_A32;
+		break;
+	case VME_A64:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_A64;
+		break;
+	case VME_CRCSR:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_CRCSR;
+		break;
+	case VME_USER1:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_USER1;
+		break;
+	case VME_USER2:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_USER2;
+		break;
+	case VME_USER3:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_USER3;
+		break;
+	case VME_USER4:
+		temp_ctl |= TSI148_LCSR_OTAT_AMODE_USER4;
+		break;
+	default:
+		spin_unlock(&image->lock);
+		dev_err(tsi148_bridge->parent, "Invalid address space\n");
+		retval = -EINVAL;
+		goto err_aspace;
+	}
+
+	temp_ctl &= ~(3<<4);
+	if (cycle & VME_SUPER)
+		temp_ctl |= TSI148_LCSR_OTAT_SUP;
+	if (cycle & VME_PROG)
+		temp_ctl |= TSI148_LCSR_OTAT_PGM;
+
+	/* Setup mapping */
+	iowrite32be(pci_base_high, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAU);
+	iowrite32be(pci_base_low, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAL);
+	iowrite32be(pci_bound_high, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTEAU);
+	iowrite32be(pci_bound_low, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTEAL);
+	iowrite32be(vme_offset_high, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTOFU);
+	iowrite32be(vme_offset_low, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTOFL);
+
+	/* Write ctl reg without enable */
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTAT);
+
+	if (enabled)
+		temp_ctl |= TSI148_LCSR_OTAT_EN;
+
+	iowrite32be(temp_ctl, bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTAT);
+
+	spin_unlock(&image->lock);
+	return 0;
+
+err_aspace:
+err_dwidth:
+err_gran:
+	tsi148_free_resource(image);
+err_res:
+err_window:
+	return retval;
+
+}
+
+/*
+ * Set the attributes of an outbound window.
+ *
+ * XXX Not parsing prefetch information.
+ */
+static int __tsi148_master_get(struct vme_master_resource *image, int *enabled,
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
+{
+	unsigned int i, ctl;
+	unsigned int pci_base_low, pci_base_high;
+	unsigned int pci_bound_low, pci_bound_high;
+	unsigned int vme_offset_low, vme_offset_high;
+
+	unsigned long long pci_base, pci_bound, vme_offset;
+	struct tsi148_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	i = image->number;
+
+	ctl = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTAT);
+
+	pci_base_high = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAU);
+	pci_base_low = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAL);
+	pci_bound_high = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTEAU);
+	pci_bound_low = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTEAL);
+	vme_offset_high = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTOFU);
+	vme_offset_low = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTOFL);
+
+	/* Convert 64-bit variables to 2x 32-bit variables */
+	reg_join(pci_base_high, pci_base_low, &pci_base);
+	reg_join(pci_bound_high, pci_bound_low, &pci_bound);
+	reg_join(vme_offset_high, vme_offset_low, &vme_offset);
+
+	*vme_base = pci_base + vme_offset;
+	*size = (unsigned long long)(pci_bound - pci_base) + 0x10000;
+
+	*enabled = 0;
+	*aspace = 0;
+	*cycle = 0;
+	*dwidth = 0;
+
+	if (ctl & TSI148_LCSR_OTAT_EN)
+		*enabled = 1;
+
+	/* Setup address space */
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_A16)
+		*aspace |= VME_A16;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_A24)
+		*aspace |= VME_A24;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_A32)
+		*aspace |= VME_A32;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_A64)
+		*aspace |= VME_A64;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_CRCSR)
+		*aspace |= VME_CRCSR;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_USER1)
+		*aspace |= VME_USER1;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_USER2)
+		*aspace |= VME_USER2;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_USER3)
+		*aspace |= VME_USER3;
+	if ((ctl & TSI148_LCSR_OTAT_AMODE_M) == TSI148_LCSR_OTAT_AMODE_USER4)
+		*aspace |= VME_USER4;
+
+	/* Setup 2eSST speeds */
+	if ((ctl & TSI148_LCSR_OTAT_2eSSTM_M) == TSI148_LCSR_OTAT_2eSSTM_160)
+		*cycle |= VME_2eSST160;
+	if ((ctl & TSI148_LCSR_OTAT_2eSSTM_M) == TSI148_LCSR_OTAT_2eSSTM_267)
+		*cycle |= VME_2eSST267;
+	if ((ctl & TSI148_LCSR_OTAT_2eSSTM_M) == TSI148_LCSR_OTAT_2eSSTM_320)
+		*cycle |= VME_2eSST320;
+
+	/* Setup cycle types */
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_SCT)
+		*cycle |= VME_SCT;
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_BLT)
+		*cycle |= VME_BLT;
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_MBLT)
+		*cycle |= VME_MBLT;
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_2eVME)
+		*cycle |= VME_2eVME;
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_2eSST)
+		*cycle |= VME_2eSST;
+	if ((ctl & TSI148_LCSR_OTAT_TM_M) == TSI148_LCSR_OTAT_TM_2eSSTB)
+		*cycle |= VME_2eSSTB;
+
+	if (ctl & TSI148_LCSR_OTAT_SUP)
+		*cycle |= VME_SUPER;
+	else
+		*cycle |= VME_USER;
+
+	if (ctl & TSI148_LCSR_OTAT_PGM)
+		*cycle |= VME_PROG;
+	else
+		*cycle |= VME_DATA;
+
+	/* Setup data width */
+	if ((ctl & TSI148_LCSR_OTAT_DBW_M) == TSI148_LCSR_OTAT_DBW_16)
+		*dwidth = VME_D16;
+	if ((ctl & TSI148_LCSR_OTAT_DBW_M) == TSI148_LCSR_OTAT_DBW_32)
+		*dwidth = VME_D32;
+
+	return 0;
+}
+
+
+static int tsi148_master_get(struct vme_master_resource *image, int *enabled,
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
+{
+	int retval;
+
+	spin_lock(&image->lock);
+
+	retval = __tsi148_master_get(image, enabled, vme_base, size, aspace,
+		cycle, dwidth);
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+static ssize_t tsi148_master_read(struct vme_master_resource *image, void *buf,
+	size_t count, loff_t offset)
+{
+	int retval, enabled;
+	unsigned long long vme_base, size;
+	u32 aspace, cycle, dwidth;
+	struct vme_error_handler *handler = NULL;
+	struct vme_bridge *tsi148_bridge;
+	void __iomem *addr = image->kern_base + offset;
+	unsigned int done = 0;
+	unsigned int count32;
+
+	tsi148_bridge = image->parent;
+
+	spin_lock(&image->lock);
+
+	if (err_chk) {
+		__tsi148_master_get(image, &enabled, &vme_base, &size, &aspace,
+				    &cycle, &dwidth);
+		handler = vme_register_error_handler(tsi148_bridge, aspace,
+						     vme_base + offset, count);
+		if (!handler) {
+			spin_unlock(&image->lock);
+			return -ENOMEM;
+		}
+	}
+
+	/* The following code handles VME address alignment. We cannot use
+	 * memcpy_xxx here because it may cut data transfers in to 8-bit
+	 * cycles when D16 or D32 cycles are required on the VME bus.
+	 * On the other hand, the bridge itself assures that the maximum data
+	 * cycle configured for the transfer is used and splits it
+	 * automatically for non-aligned addresses, so we don't want the
+	 * overhead of needlessly forcing small transfers for the entire cycle.
+	 */
+	if ((uintptr_t)addr & 0x1) {
+		*(u8 *)buf = ioread8(addr);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+	if ((uintptr_t)(addr + done) & 0x2) {
+		if ((count - done) < 2) {
+			*(u8 *)(buf + done) = ioread8(addr + done);
+			done += 1;
+			goto out;
+		} else {
+			*(u16 *)(buf + done) = ioread16(addr + done);
+			done += 2;
+		}
+	}
+
+	count32 = (count - done) & ~0x3;
+	while (done < count32) {
+		*(u32 *)(buf + done) = ioread32(addr + done);
+		done += 4;
+	}
+
+	if ((count - done) & 0x2) {
+		*(u16 *)(buf + done) = ioread16(addr + done);
+		done += 2;
+	}
+	if ((count - done) & 0x1) {
+		*(u8 *)(buf + done) = ioread8(addr + done);
+		done += 1;
+	}
+
+out:
+	retval = count;
+
+	if (err_chk) {
+		if (handler->num_errors) {
+			dev_err(image->parent->parent,
+				"First VME read error detected an at address 0x%llx\n",
+				handler->first_error);
+			retval = handler->first_error - (vme_base + offset);
+		}
+		vme_unregister_error_handler(handler);
+	}
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+
+static ssize_t tsi148_master_write(struct vme_master_resource *image, void *buf,
+	size_t count, loff_t offset)
+{
+	int retval = 0, enabled;
+	unsigned long long vme_base, size;
+	u32 aspace, cycle, dwidth;
+	void __iomem *addr = image->kern_base + offset;
+	unsigned int done = 0;
+	unsigned int count32;
+
+	struct vme_error_handler *handler = NULL;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	tsi148_bridge = image->parent;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	spin_lock(&image->lock);
+
+	if (err_chk) {
+		__tsi148_master_get(image, &enabled, &vme_base, &size, &aspace,
+				    &cycle, &dwidth);
+		handler = vme_register_error_handler(tsi148_bridge, aspace,
+						     vme_base + offset, count);
+		if (!handler) {
+			spin_unlock(&image->lock);
+			return -ENOMEM;
+		}
+	}
+
+	/* Here we apply for the same strategy we do in master_read
+	 * function in order to assure the correct cycles.
+	 */
+	if ((uintptr_t)addr & 0x1) {
+		iowrite8(*(u8 *)buf, addr);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+	if ((uintptr_t)(addr + done) & 0x2) {
+		if ((count - done) < 2) {
+			iowrite8(*(u8 *)(buf + done), addr + done);
+			done += 1;
+			goto out;
+		} else {
+			iowrite16(*(u16 *)(buf + done), addr + done);
+			done += 2;
+		}
+	}
+
+	count32 = (count - done) & ~0x3;
+	while (done < count32) {
+		iowrite32(*(u32 *)(buf + done), addr + done);
+		done += 4;
+	}
+
+	if ((count - done) & 0x2) {
+		iowrite16(*(u16 *)(buf + done), addr + done);
+		done += 2;
+	}
+	if ((count - done) & 0x1) {
+		iowrite8(*(u8 *)(buf + done), addr + done);
+		done += 1;
+	}
+
+out:
+	retval = count;
+
+	/*
+	 * Writes are posted. We need to do a read on the VME bus to flush out
+	 * all of the writes before we check for errors. We can't guarantee
+	 * that reading the data we have just written is safe. It is believed
+	 * that there isn't any read, write re-ordering, so we can read any
+	 * location in VME space, so lets read the Device ID from the tsi148's
+	 * own registers as mapped into CR/CSR space.
+	 *
+	 * We check for saved errors in the written address range/space.
+	 */
+
+	if (err_chk) {
+		ioread16(bridge->flush_image->kern_base + 0x7F000);
+
+		if (handler->num_errors) {
+			dev_warn(tsi148_bridge->parent,
+				 "First VME write error detected an at address 0x%llx\n",
+				 handler->first_error);
+			retval = handler->first_error - (vme_base + offset);
+		}
+		vme_unregister_error_handler(handler);
+	}
+
+	spin_unlock(&image->lock);
+
+	return retval;
+}
+
+/*
+ * Perform an RMW cycle on the VME bus.
+ *
+ * Requires a previously configured master window, returns final value.
+ */
+static unsigned int tsi148_master_rmw(struct vme_master_resource *image,
+	unsigned int mask, unsigned int compare, unsigned int swap,
+	loff_t offset)
+{
+	unsigned long long pci_addr;
+	unsigned int pci_addr_high, pci_addr_low;
+	u32 tmp, result;
+	int i;
+	struct tsi148_driver *bridge;
+
+	bridge = image->parent->driver_priv;
+
+	/* Find the PCI address that maps to the desired VME address */
+	i = image->number;
+
+	/* Locking as we can only do one of these at a time */
+	mutex_lock(&bridge->vme_rmw);
+
+	/* Lock image */
+	spin_lock(&image->lock);
+
+	pci_addr_high = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAU);
+	pci_addr_low = ioread32be(bridge->base + TSI148_LCSR_OT[i] +
+		TSI148_LCSR_OFFSET_OTSAL);
+
+	reg_join(pci_addr_high, pci_addr_low, &pci_addr);
+	reg_split(pci_addr + offset, &pci_addr_high, &pci_addr_low);
+
+	/* Configure registers */
+	iowrite32be(mask, bridge->base + TSI148_LCSR_RMWEN);
+	iowrite32be(compare, bridge->base + TSI148_LCSR_RMWC);
+	iowrite32be(swap, bridge->base + TSI148_LCSR_RMWS);
+	iowrite32be(pci_addr_high, bridge->base + TSI148_LCSR_RMWAU);
+	iowrite32be(pci_addr_low, bridge->base + TSI148_LCSR_RMWAL);
+
+	/* Enable RMW */
+	tmp = ioread32be(bridge->base + TSI148_LCSR_VMCTRL);
+	tmp |= TSI148_LCSR_VMCTRL_RMWEN;
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_VMCTRL);
+
+	/* Kick process off with a read to the required address. */
+	result = ioread32be(image->kern_base + offset);
+
+	/* Disable RMW */
+	tmp = ioread32be(bridge->base + TSI148_LCSR_VMCTRL);
+	tmp &= ~TSI148_LCSR_VMCTRL_RMWEN;
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_VMCTRL);
+
+	spin_unlock(&image->lock);
+
+	mutex_unlock(&bridge->vme_rmw);
+
+	return result;
+}
+
+static int tsi148_dma_set_vme_src_attributes(struct device *dev, __be32 *attr,
+	u32 aspace, u32 cycle, u32 dwidth)
+{
+	u32 val;
+
+	val = be32_to_cpu(*attr);
+
+	/* Setup 2eSST speeds */
+	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
+	case VME_2eSST160:
+		val |= TSI148_LCSR_DSAT_2eSSTM_160;
+		break;
+	case VME_2eSST267:
+		val |= TSI148_LCSR_DSAT_2eSSTM_267;
+		break;
+	case VME_2eSST320:
+		val |= TSI148_LCSR_DSAT_2eSSTM_320;
+		break;
+	}
+
+	/* Setup cycle types */
+	if (cycle & VME_SCT)
+		val |= TSI148_LCSR_DSAT_TM_SCT;
+
+	if (cycle & VME_BLT)
+		val |= TSI148_LCSR_DSAT_TM_BLT;
+
+	if (cycle & VME_MBLT)
+		val |= TSI148_LCSR_DSAT_TM_MBLT;
+
+	if (cycle & VME_2eVME)
+		val |= TSI148_LCSR_DSAT_TM_2eVME;
+
+	if (cycle & VME_2eSST)
+		val |= TSI148_LCSR_DSAT_TM_2eSST;
+
+	if (cycle & VME_2eSSTB) {
+		dev_err(dev, "Currently not setting Broadcast Select Registers\n");
+		val |= TSI148_LCSR_DSAT_TM_2eSSTB;
+	}
+
+	/* Setup data width */
+	switch (dwidth) {
+	case VME_D16:
+		val |= TSI148_LCSR_DSAT_DBW_16;
+		break;
+	case VME_D32:
+		val |= TSI148_LCSR_DSAT_DBW_32;
+		break;
+	default:
+		dev_err(dev, "Invalid data width\n");
+		return -EINVAL;
+	}
+
+	/* Setup address space */
+	switch (aspace) {
+	case VME_A16:
+		val |= TSI148_LCSR_DSAT_AMODE_A16;
+		break;
+	case VME_A24:
+		val |= TSI148_LCSR_DSAT_AMODE_A24;
+		break;
+	case VME_A32:
+		val |= TSI148_LCSR_DSAT_AMODE_A32;
+		break;
+	case VME_A64:
+		val |= TSI148_LCSR_DSAT_AMODE_A64;
+		break;
+	case VME_CRCSR:
+		val |= TSI148_LCSR_DSAT_AMODE_CRCSR;
+		break;
+	case VME_USER1:
+		val |= TSI148_LCSR_DSAT_AMODE_USER1;
+		break;
+	case VME_USER2:
+		val |= TSI148_LCSR_DSAT_AMODE_USER2;
+		break;
+	case VME_USER3:
+		val |= TSI148_LCSR_DSAT_AMODE_USER3;
+		break;
+	case VME_USER4:
+		val |= TSI148_LCSR_DSAT_AMODE_USER4;
+		break;
+	default:
+		dev_err(dev, "Invalid address space\n");
+		return -EINVAL;
+	}
+
+	if (cycle & VME_SUPER)
+		val |= TSI148_LCSR_DSAT_SUP;
+	if (cycle & VME_PROG)
+		val |= TSI148_LCSR_DSAT_PGM;
+
+	*attr = cpu_to_be32(val);
+
+	return 0;
+}
+
+static int tsi148_dma_set_vme_dest_attributes(struct device *dev, __be32 *attr,
+	u32 aspace, u32 cycle, u32 dwidth)
+{
+	u32 val;
+
+	val = be32_to_cpu(*attr);
+
+	/* Setup 2eSST speeds */
+	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
+	case VME_2eSST160:
+		val |= TSI148_LCSR_DDAT_2eSSTM_160;
+		break;
+	case VME_2eSST267:
+		val |= TSI148_LCSR_DDAT_2eSSTM_267;
+		break;
+	case VME_2eSST320:
+		val |= TSI148_LCSR_DDAT_2eSSTM_320;
+		break;
+	}
+
+	/* Setup cycle types */
+	if (cycle & VME_SCT)
+		val |= TSI148_LCSR_DDAT_TM_SCT;
+
+	if (cycle & VME_BLT)
+		val |= TSI148_LCSR_DDAT_TM_BLT;
+
+	if (cycle & VME_MBLT)
+		val |= TSI148_LCSR_DDAT_TM_MBLT;
+
+	if (cycle & VME_2eVME)
+		val |= TSI148_LCSR_DDAT_TM_2eVME;
+
+	if (cycle & VME_2eSST)
+		val |= TSI148_LCSR_DDAT_TM_2eSST;
+
+	if (cycle & VME_2eSSTB) {
+		dev_err(dev, "Currently not setting Broadcast Select Registers\n");
+		val |= TSI148_LCSR_DDAT_TM_2eSSTB;
+	}
+
+	/* Setup data width */
+	switch (dwidth) {
+	case VME_D16:
+		val |= TSI148_LCSR_DDAT_DBW_16;
+		break;
+	case VME_D32:
+		val |= TSI148_LCSR_DDAT_DBW_32;
+		break;
+	default:
+		dev_err(dev, "Invalid data width\n");
+		return -EINVAL;
+	}
+
+	/* Setup address space */
+	switch (aspace) {
+	case VME_A16:
+		val |= TSI148_LCSR_DDAT_AMODE_A16;
+		break;
+	case VME_A24:
+		val |= TSI148_LCSR_DDAT_AMODE_A24;
+		break;
+	case VME_A32:
+		val |= TSI148_LCSR_DDAT_AMODE_A32;
+		break;
+	case VME_A64:
+		val |= TSI148_LCSR_DDAT_AMODE_A64;
+		break;
+	case VME_CRCSR:
+		val |= TSI148_LCSR_DDAT_AMODE_CRCSR;
+		break;
+	case VME_USER1:
+		val |= TSI148_LCSR_DDAT_AMODE_USER1;
+		break;
+	case VME_USER2:
+		val |= TSI148_LCSR_DDAT_AMODE_USER2;
+		break;
+	case VME_USER3:
+		val |= TSI148_LCSR_DDAT_AMODE_USER3;
+		break;
+	case VME_USER4:
+		val |= TSI148_LCSR_DDAT_AMODE_USER4;
+		break;
+	default:
+		dev_err(dev, "Invalid address space\n");
+		return -EINVAL;
+	}
+
+	if (cycle & VME_SUPER)
+		val |= TSI148_LCSR_DDAT_SUP;
+	if (cycle & VME_PROG)
+		val |= TSI148_LCSR_DDAT_PGM;
+
+	*attr = cpu_to_be32(val);
+
+	return 0;
+}
+
+/*
+ * Add a link list descriptor to the list
+ *
+ * Note: DMA engine expects the DMA descriptor to be big endian.
+ */
+static int tsi148_dma_list_add(struct vme_dma_list *list,
+	struct vme_dma_attr *src, struct vme_dma_attr *dest, size_t count)
+{
+	struct tsi148_dma_entry *entry, *prev;
+	u32 address_high, address_low, val;
+	struct vme_dma_pattern *pattern_attr;
+	struct vme_dma_pci *pci_attr;
+	struct vme_dma_vme *vme_attr;
+	int retval = 0;
+	struct vme_bridge *tsi148_bridge;
+
+	tsi148_bridge = list->parent->parent;
+
+	/* Descriptor must be aligned on 64-bit boundaries */
+	entry = kmalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry) {
+		retval = -ENOMEM;
+		goto err_mem;
+	}
+
+	/* Test descriptor alignment */
+	if ((unsigned long)&entry->descriptor & 0x7) {
+		dev_err(tsi148_bridge->parent, "Descriptor not aligned to 8 byte boundary as required: %p\n",
+			&entry->descriptor);
+		retval = -EINVAL;
+		goto err_align;
+	}
+
+	/* Given we are going to fill out the structure, we probably don't
+	 * need to zero it, but better safe than sorry for now.
+	 */
+	memset(&entry->descriptor, 0, sizeof(entry->descriptor));
+
+	/* Fill out source part */
+	switch (src->type) {
+	case VME_DMA_PATTERN:
+		pattern_attr = src->private;
+
+		entry->descriptor.dsal = cpu_to_be32(pattern_attr->pattern);
+
+		val = TSI148_LCSR_DSAT_TYP_PAT;
+
+		/* Default behaviour is 32 bit pattern */
+		if (pattern_attr->type & VME_DMA_PATTERN_BYTE)
+			val |= TSI148_LCSR_DSAT_PSZ;
+
+		/* It seems that the default behaviour is to increment */
+		if ((pattern_attr->type & VME_DMA_PATTERN_INCREMENT) == 0)
+			val |= TSI148_LCSR_DSAT_NIN;
+		entry->descriptor.dsat = cpu_to_be32(val);
+		break;
+	case VME_DMA_PCI:
+		pci_attr = src->private;
+
+		reg_split((unsigned long long)pci_attr->address, &address_high,
+			&address_low);
+		entry->descriptor.dsau = cpu_to_be32(address_high);
+		entry->descriptor.dsal = cpu_to_be32(address_low);
+		entry->descriptor.dsat = cpu_to_be32(TSI148_LCSR_DSAT_TYP_PCI);
+		break;
+	case VME_DMA_VME:
+		vme_attr = src->private;
+
+		reg_split((unsigned long long)vme_attr->address, &address_high,
+			&address_low);
+		entry->descriptor.dsau = cpu_to_be32(address_high);
+		entry->descriptor.dsal = cpu_to_be32(address_low);
+		entry->descriptor.dsat = cpu_to_be32(TSI148_LCSR_DSAT_TYP_VME);
+
+		retval = tsi148_dma_set_vme_src_attributes(
+			tsi148_bridge->parent, &entry->descriptor.dsat,
+			vme_attr->aspace, vme_attr->cycle, vme_attr->dwidth);
+		if (retval < 0)
+			goto err_source;
+		break;
+	default:
+		dev_err(tsi148_bridge->parent, "Invalid source type\n");
+		retval = -EINVAL;
+		goto err_source;
+	}
+
+	/* Assume last link - this will be over-written by adding another */
+	entry->descriptor.dnlau = cpu_to_be32(0);
+	entry->descriptor.dnlal = cpu_to_be32(TSI148_LCSR_DNLAL_LLA);
+
+	/* Fill out destination part */
+	switch (dest->type) {
+	case VME_DMA_PCI:
+		pci_attr = dest->private;
+
+		reg_split((unsigned long long)pci_attr->address, &address_high,
+			&address_low);
+		entry->descriptor.ddau = cpu_to_be32(address_high);
+		entry->descriptor.ddal = cpu_to_be32(address_low);
+		entry->descriptor.ddat = cpu_to_be32(TSI148_LCSR_DDAT_TYP_PCI);
+		break;
+	case VME_DMA_VME:
+		vme_attr = dest->private;
+
+		reg_split((unsigned long long)vme_attr->address, &address_high,
+			&address_low);
+		entry->descriptor.ddau = cpu_to_be32(address_high);
+		entry->descriptor.ddal = cpu_to_be32(address_low);
+		entry->descriptor.ddat = cpu_to_be32(TSI148_LCSR_DDAT_TYP_VME);
+
+		retval = tsi148_dma_set_vme_dest_attributes(
+			tsi148_bridge->parent, &entry->descriptor.ddat,
+			vme_attr->aspace, vme_attr->cycle, vme_attr->dwidth);
+		if (retval < 0)
+			goto err_dest;
+		break;
+	default:
+		dev_err(tsi148_bridge->parent, "Invalid destination type\n");
+		retval = -EINVAL;
+		goto err_dest;
+	}
+
+	/* Fill out count */
+	entry->descriptor.dcnt = cpu_to_be32((u32)count);
+
+	/* Add to list */
+	list_add_tail(&entry->list, &list->entries);
+
+	entry->dma_handle = dma_map_single(tsi148_bridge->parent,
+					   &entry->descriptor,
+					   sizeof(entry->descriptor),
+					   DMA_TO_DEVICE);
+	if (dma_mapping_error(tsi148_bridge->parent, entry->dma_handle)) {
+		dev_err(tsi148_bridge->parent, "DMA mapping error\n");
+		retval = -EINVAL;
+		goto err_dma;
+	}
+
+	/* Fill out previous descriptors "Next Address" */
+	if (entry->list.prev != &list->entries) {
+		reg_split((unsigned long long)entry->dma_handle, &address_high,
+			&address_low);
+		prev = list_entry(entry->list.prev, struct tsi148_dma_entry,
+				  list);
+		prev->descriptor.dnlau = cpu_to_be32(address_high);
+		prev->descriptor.dnlal = cpu_to_be32(address_low);
+
+	}
+
+	return 0;
+
+err_dma:
+	list_del(&entry->list);
+err_dest:
+err_source:
+err_align:
+		kfree(entry);
+err_mem:
+	return retval;
+}
+
+/*
+ * Check to see if the provided DMA channel is busy.
+ */
+static int tsi148_dma_busy(struct vme_bridge *tsi148_bridge, int channel)
+{
+	u32 tmp;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	tmp = ioread32be(bridge->base + TSI148_LCSR_DMA[channel] +
+		TSI148_LCSR_OFFSET_DSTA);
+
+	if (tmp & TSI148_LCSR_DSTA_BSY)
+		return 0;
+	else
+		return 1;
+
+}
+
+/*
+ * Execute a previously generated link list
+ *
+ * XXX Need to provide control register configuration.
+ */
+static int tsi148_dma_list_exec(struct vme_dma_list *list)
+{
+	struct vme_dma_resource *ctrlr;
+	int channel, retval;
+	struct tsi148_dma_entry *entry;
+	u32 bus_addr_high, bus_addr_low;
+	u32 val, dctlreg = 0;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	ctrlr = list->parent;
+
+	tsi148_bridge = ctrlr->parent;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	mutex_lock(&ctrlr->mtx);
+
+	channel = ctrlr->number;
+
+	if (!list_empty(&ctrlr->running)) {
+		/*
+		 * XXX We have an active DMA transfer and currently haven't
+		 *     sorted out the mechanism for "pending" DMA transfers.
+		 *     Return busy.
+		 */
+		/* Need to add to pending here */
+		mutex_unlock(&ctrlr->mtx);
+		return -EBUSY;
+	}
+
+	list_add(&list->list, &ctrlr->running);
+
+	/* Get first bus address and write into registers */
+	entry = list_first_entry(&list->entries, struct tsi148_dma_entry,
+		list);
+
+	mutex_unlock(&ctrlr->mtx);
+
+	reg_split(entry->dma_handle, &bus_addr_high, &bus_addr_low);
+
+	iowrite32be(bus_addr_high, bridge->base +
+		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DNLAU);
+	iowrite32be(bus_addr_low, bridge->base +
+		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DNLAL);
+
+	dctlreg = ioread32be(bridge->base + TSI148_LCSR_DMA[channel] +
+		TSI148_LCSR_OFFSET_DCTL);
+
+	/* Start the operation */
+	iowrite32be(dctlreg | TSI148_LCSR_DCTL_DGO, bridge->base +
+		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DCTL);
+
+	retval = wait_event_interruptible(bridge->dma_queue[channel],
+		tsi148_dma_busy(ctrlr->parent, channel));
+
+	if (retval) {
+		iowrite32be(dctlreg | TSI148_LCSR_DCTL_ABT, bridge->base +
+			TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DCTL);
+		/* Wait for the operation to abort */
+		wait_event(bridge->dma_queue[channel],
+			   tsi148_dma_busy(ctrlr->parent, channel));
+		retval = -EINTR;
+		goto exit;
+	}
+
+	/*
+	 * Read status register, this register is valid until we kick off a
+	 * new transfer.
+	 */
+	val = ioread32be(bridge->base + TSI148_LCSR_DMA[channel] +
+		TSI148_LCSR_OFFSET_DSTA);
+
+	if (val & TSI148_LCSR_DSTA_VBE) {
+		dev_err(tsi148_bridge->parent, "DMA Error. DSTA=%08X\n", val);
+		retval = -EIO;
+	}
+
+exit:
+	/* Remove list from running list */
+	mutex_lock(&ctrlr->mtx);
+	list_del(&list->list);
+	mutex_unlock(&ctrlr->mtx);
+
+	return retval;
+}
+
+/*
+ * Clean up a previously generated link list
+ *
+ * We have a separate function, don't assume that the chain can't be reused.
+ */
+static int tsi148_dma_list_empty(struct vme_dma_list *list)
+{
+	struct list_head *pos, *temp;
+	struct tsi148_dma_entry *entry;
+
+	struct vme_bridge *tsi148_bridge = list->parent->parent;
+
+	/* detach and free each entry */
+	list_for_each_safe(pos, temp, &list->entries) {
+		list_del(pos);
+		entry = list_entry(pos, struct tsi148_dma_entry, list);
+
+		dma_unmap_single(tsi148_bridge->parent, entry->dma_handle,
+			sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
+		kfree(entry);
+	}
+
+	return 0;
+}
+
+/*
+ * All 4 location monitors reside at the same base - this is therefore a
+ * system wide configuration.
+ *
+ * This does not enable the LM monitor - that should be done when the first
+ * callback is attached and disabled when the last callback is removed.
+ */
+static int tsi148_lm_set(struct vme_lm_resource *lm, unsigned long long lm_base,
+	u32 aspace, u32 cycle)
+{
+	u32 lm_base_high, lm_base_low, lm_ctl = 0;
+	int i;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	tsi148_bridge = lm->parent;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* If we already have a callback attached, we can't move it! */
+	for (i = 0; i < lm->monitors; i++) {
+		if (bridge->lm_callback[i]) {
+			mutex_unlock(&lm->mtx);
+			dev_err(tsi148_bridge->parent, "Location monitor callback attached, can't reset\n");
+			return -EBUSY;
+		}
+	}
+
+	switch (aspace) {
+	case VME_A16:
+		lm_ctl |= TSI148_LCSR_LMAT_AS_A16;
+		break;
+	case VME_A24:
+		lm_ctl |= TSI148_LCSR_LMAT_AS_A24;
+		break;
+	case VME_A32:
+		lm_ctl |= TSI148_LCSR_LMAT_AS_A32;
+		break;
+	case VME_A64:
+		lm_ctl |= TSI148_LCSR_LMAT_AS_A64;
+		break;
+	default:
+		mutex_unlock(&lm->mtx);
+		dev_err(tsi148_bridge->parent, "Invalid address space\n");
+		return -EINVAL;
+	}
+
+	if (cycle & VME_SUPER)
+		lm_ctl |= TSI148_LCSR_LMAT_SUPR;
+	if (cycle & VME_USER)
+		lm_ctl |= TSI148_LCSR_LMAT_NPRIV;
+	if (cycle & VME_PROG)
+		lm_ctl |= TSI148_LCSR_LMAT_PGM;
+	if (cycle & VME_DATA)
+		lm_ctl |= TSI148_LCSR_LMAT_DATA;
+
+	reg_split(lm_base, &lm_base_high, &lm_base_low);
+
+	iowrite32be(lm_base_high, bridge->base + TSI148_LCSR_LMBAU);
+	iowrite32be(lm_base_low, bridge->base + TSI148_LCSR_LMBAL);
+	iowrite32be(lm_ctl, bridge->base + TSI148_LCSR_LMAT);
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/* Get configuration of the callback monitor and return whether it is enabled
+ * or disabled.
+ */
+static int tsi148_lm_get(struct vme_lm_resource *lm,
+	unsigned long long *lm_base, u32 *aspace, u32 *cycle)
+{
+	u32 lm_base_high, lm_base_low, lm_ctl, enabled = 0;
+	struct tsi148_driver *bridge;
+
+	bridge = lm->parent->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	lm_base_high = ioread32be(bridge->base + TSI148_LCSR_LMBAU);
+	lm_base_low = ioread32be(bridge->base + TSI148_LCSR_LMBAL);
+	lm_ctl = ioread32be(bridge->base + TSI148_LCSR_LMAT);
+
+	reg_join(lm_base_high, lm_base_low, lm_base);
+
+	if (lm_ctl & TSI148_LCSR_LMAT_EN)
+		enabled = 1;
+
+	if ((lm_ctl & TSI148_LCSR_LMAT_AS_M) == TSI148_LCSR_LMAT_AS_A16)
+		*aspace |= VME_A16;
+
+	if ((lm_ctl & TSI148_LCSR_LMAT_AS_M) == TSI148_LCSR_LMAT_AS_A24)
+		*aspace |= VME_A24;
+
+	if ((lm_ctl & TSI148_LCSR_LMAT_AS_M) == TSI148_LCSR_LMAT_AS_A32)
+		*aspace |= VME_A32;
+
+	if ((lm_ctl & TSI148_LCSR_LMAT_AS_M) == TSI148_LCSR_LMAT_AS_A64)
+		*aspace |= VME_A64;
+
+
+	if (lm_ctl & TSI148_LCSR_LMAT_SUPR)
+		*cycle |= VME_SUPER;
+	if (lm_ctl & TSI148_LCSR_LMAT_NPRIV)
+		*cycle |= VME_USER;
+	if (lm_ctl & TSI148_LCSR_LMAT_PGM)
+		*cycle |= VME_PROG;
+	if (lm_ctl & TSI148_LCSR_LMAT_DATA)
+		*cycle |= VME_DATA;
+
+	mutex_unlock(&lm->mtx);
+
+	return enabled;
+}
+
+/*
+ * Attach a callback to a specific location monitor.
+ *
+ * Callback will be passed the monitor triggered.
+ */
+static int tsi148_lm_attach(struct vme_lm_resource *lm, int monitor,
+	void (*callback)(void *), void *data)
+{
+	u32 lm_ctl, tmp;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *bridge;
+
+	tsi148_bridge = lm->parent;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* Ensure that the location monitor is configured - need PGM or DATA */
+	lm_ctl = ioread32be(bridge->base + TSI148_LCSR_LMAT);
+	if ((lm_ctl & (TSI148_LCSR_LMAT_PGM | TSI148_LCSR_LMAT_DATA)) == 0) {
+		mutex_unlock(&lm->mtx);
+		dev_err(tsi148_bridge->parent, "Location monitor not properly configured\n");
+		return -EINVAL;
+	}
+
+	/* Check that a callback isn't already attached */
+	if (bridge->lm_callback[monitor]) {
+		mutex_unlock(&lm->mtx);
+		dev_err(tsi148_bridge->parent, "Existing callback attached\n");
+		return -EBUSY;
+	}
+
+	/* Attach callback */
+	bridge->lm_callback[monitor] = callback;
+	bridge->lm_data[monitor] = data;
+
+	/* Enable Location Monitor interrupt */
+	tmp = ioread32be(bridge->base + TSI148_LCSR_INTEN);
+	tmp |= TSI148_LCSR_INTEN_LMEN[monitor];
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEN);
+
+	tmp = ioread32be(bridge->base + TSI148_LCSR_INTEO);
+	tmp |= TSI148_LCSR_INTEO_LMEO[monitor];
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
+
+	/* Ensure that global Location Monitor Enable set */
+	if ((lm_ctl & TSI148_LCSR_LMAT_EN) == 0) {
+		lm_ctl |= TSI148_LCSR_LMAT_EN;
+		iowrite32be(lm_ctl, bridge->base + TSI148_LCSR_LMAT);
+	}
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/*
+ * Detach a callback function forn a specific location monitor.
+ */
+static int tsi148_lm_detach(struct vme_lm_resource *lm, int monitor)
+{
+	u32 lm_en, tmp;
+	struct tsi148_driver *bridge;
+
+	bridge = lm->parent->driver_priv;
+
+	mutex_lock(&lm->mtx);
+
+	/* Disable Location Monitor and ensure previous interrupts are clear */
+	lm_en = ioread32be(bridge->base + TSI148_LCSR_INTEN);
+	lm_en &= ~TSI148_LCSR_INTEN_LMEN[monitor];
+	iowrite32be(lm_en, bridge->base + TSI148_LCSR_INTEN);
+
+	tmp = ioread32be(bridge->base + TSI148_LCSR_INTEO);
+	tmp &= ~TSI148_LCSR_INTEO_LMEO[monitor];
+	iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
+
+	iowrite32be(TSI148_LCSR_INTC_LMC[monitor],
+		 bridge->base + TSI148_LCSR_INTC);
+
+	/* Detach callback */
+	bridge->lm_callback[monitor] = NULL;
+	bridge->lm_data[monitor] = NULL;
+
+	/* If all location monitors disabled, disable global Location Monitor */
+	if ((lm_en & (TSI148_LCSR_INTS_LM0S | TSI148_LCSR_INTS_LM1S |
+			TSI148_LCSR_INTS_LM2S | TSI148_LCSR_INTS_LM3S)) == 0) {
+		tmp = ioread32be(bridge->base + TSI148_LCSR_LMAT);
+		tmp &= ~TSI148_LCSR_LMAT_EN;
+		iowrite32be(tmp, bridge->base + TSI148_LCSR_LMAT);
+	}
+
+	mutex_unlock(&lm->mtx);
+
+	return 0;
+}
+
+/*
+ * Determine Geographical Addressing
+ */
+static int tsi148_slot_get(struct vme_bridge *tsi148_bridge)
+{
+	u32 slot = 0;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	if (!geoid) {
+		slot = ioread32be(bridge->base + TSI148_LCSR_VSTAT);
+		slot = slot & TSI148_LCSR_VSTAT_GA_M;
+	} else
+		slot = geoid;
+
+	return (int)slot;
+}
+
+static void *tsi148_alloc_consistent(struct device *parent, size_t size,
+	dma_addr_t *dma)
+{
+	struct pci_dev *pdev;
+
+	/* Find pci_dev container of dev */
+	pdev = to_pci_dev(parent);
+
+	return dma_alloc_coherent(&pdev->dev, size, dma, GFP_KERNEL);
+}
+
+static void tsi148_free_consistent(struct device *parent, size_t size,
+	void *vaddr, dma_addr_t dma)
+{
+	struct pci_dev *pdev;
+
+	/* Find pci_dev container of dev */
+	pdev = to_pci_dev(parent);
+
+	dma_free_coherent(&pdev->dev, size, vaddr, dma);
+}
+
+/*
+ * Configure CR/CSR space
+ *
+ * Access to the CR/CSR can be configured at power-up. The location of the
+ * CR/CSR registers in the CR/CSR address space is determined by the boards
+ * Auto-ID or Geographic address. This function ensures that the window is
+ * enabled at an offset consistent with the boards geopgraphic address.
+ *
+ * Each board has a 512kB window, with the highest 4kB being used for the
+ * boards registers, this means there is a fix length 508kB window which must
+ * be mapped onto PCI memory.
+ */
+static int tsi148_crcsr_init(struct vme_bridge *tsi148_bridge,
+	struct pci_dev *pdev)
+{
+	u32 cbar, crat, vstat;
+	u32 crcsr_bus_high, crcsr_bus_low;
+	int retval;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	/* Allocate mem for CR/CSR image */
+	bridge->crcsr_kernel = dma_alloc_coherent(&pdev->dev,
+						  VME_CRCSR_BUF_SIZE,
+						  &bridge->crcsr_bus, GFP_KERNEL);
+	if (!bridge->crcsr_kernel) {
+		dev_err(tsi148_bridge->parent, "Failed to allocate memory for CR/CSR image\n");
+		return -ENOMEM;
+	}
+
+	reg_split(bridge->crcsr_bus, &crcsr_bus_high, &crcsr_bus_low);
+
+	iowrite32be(crcsr_bus_high, bridge->base + TSI148_LCSR_CROU);
+	iowrite32be(crcsr_bus_low, bridge->base + TSI148_LCSR_CROL);
+
+	/* Ensure that the CR/CSR is configured at the correct offset */
+	cbar = ioread32be(bridge->base + TSI148_CBAR);
+	cbar = (cbar & TSI148_CRCSR_CBAR_M)>>3;
+
+	vstat = tsi148_slot_get(tsi148_bridge);
+
+	if (cbar != vstat) {
+		cbar = vstat;
+		dev_info(tsi148_bridge->parent, "Setting CR/CSR offset\n");
+		iowrite32be(cbar<<3, bridge->base + TSI148_CBAR);
+	}
+	dev_info(tsi148_bridge->parent, "CR/CSR Offset: %d\n", cbar);
+
+	crat = ioread32be(bridge->base + TSI148_LCSR_CRAT);
+	if (crat & TSI148_LCSR_CRAT_EN)
+		dev_info(tsi148_bridge->parent, "CR/CSR already enabled\n");
+	else {
+		dev_info(tsi148_bridge->parent, "Enabling CR/CSR space\n");
+		iowrite32be(crat | TSI148_LCSR_CRAT_EN,
+			bridge->base + TSI148_LCSR_CRAT);
+	}
+
+	/* If we want flushed, error-checked writes, set up a window
+	 * over the CR/CSR registers. We read from here to safely flush
+	 * through VME writes.
+	 */
+	if (err_chk) {
+		retval = tsi148_master_set(bridge->flush_image, 1,
+			(vstat * 0x80000), 0x80000, VME_CRCSR, VME_SCT,
+			VME_D16);
+		if (retval)
+			dev_err(tsi148_bridge->parent, "Configuring flush image failed\n");
+	}
+
+	return 0;
+
+}
+
+static void tsi148_crcsr_exit(struct vme_bridge *tsi148_bridge,
+	struct pci_dev *pdev)
+{
+	u32 crat;
+	struct tsi148_driver *bridge;
+
+	bridge = tsi148_bridge->driver_priv;
+
+	/* Turn off CR/CSR space */
+	crat = ioread32be(bridge->base + TSI148_LCSR_CRAT);
+	iowrite32be(crat & ~TSI148_LCSR_CRAT_EN,
+		bridge->base + TSI148_LCSR_CRAT);
+
+	/* Free image */
+	iowrite32be(0, bridge->base + TSI148_LCSR_CROU);
+	iowrite32be(0, bridge->base + TSI148_LCSR_CROL);
+
+	dma_free_coherent(&pdev->dev, VME_CRCSR_BUF_SIZE,
+			  bridge->crcsr_kernel, bridge->crcsr_bus);
+}
+
+static int tsi148_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	int retval, i, master_num;
+	u32 data;
+	struct list_head *pos = NULL, *n;
+	struct vme_bridge *tsi148_bridge;
+	struct tsi148_driver *tsi148_device;
+	struct vme_master_resource *master_image;
+	struct vme_slave_resource *slave_image;
+	struct vme_dma_resource *dma_ctrlr;
+	struct vme_lm_resource *lm;
+
+	/* If we want to support more than one of each bridge, we need to
+	 * dynamically generate this so we get one per device
+	 */
+	tsi148_bridge = kzalloc(sizeof(*tsi148_bridge), GFP_KERNEL);
+	if (!tsi148_bridge) {
+		retval = -ENOMEM;
+		goto err_struct;
+	}
+	vme_init_bridge(tsi148_bridge);
+
+	tsi148_device = kzalloc(sizeof(*tsi148_device), GFP_KERNEL);
+	if (!tsi148_device) {
+		retval = -ENOMEM;
+		goto err_driver;
+	}
+
+	tsi148_bridge->driver_priv = tsi148_device;
+
+	/* Enable the device */
+	retval = pci_enable_device(pdev);
+	if (retval) {
+		dev_err(&pdev->dev, "Unable to enable device\n");
+		goto err_enable;
+	}
+
+	/* Map Registers */
+	retval = pci_request_regions(pdev, driver_name);
+	if (retval) {
+		dev_err(&pdev->dev, "Unable to reserve resources\n");
+		goto err_resource;
+	}
+
+	/* map registers in BAR 0 */
+	tsi148_device->base = ioremap(pci_resource_start(pdev, 0),
+		4096);
+	if (!tsi148_device->base) {
+		dev_err(&pdev->dev, "Unable to remap CRG region\n");
+		retval = -EIO;
+		goto err_remap;
+	}
+
+	/* Check to see if the mapping worked out */
+	data = ioread32(tsi148_device->base + TSI148_PCFS_ID) & 0x0000FFFF;
+	if (data != PCI_VENDOR_ID_TUNDRA) {
+		dev_err(&pdev->dev, "CRG region check failed\n");
+		retval = -EIO;
+		goto err_test;
+	}
+
+	/* Initialize wait queues & mutual exclusion flags */
+	init_waitqueue_head(&tsi148_device->dma_queue[0]);
+	init_waitqueue_head(&tsi148_device->dma_queue[1]);
+	init_waitqueue_head(&tsi148_device->iack_queue);
+	mutex_init(&tsi148_device->vme_int);
+	mutex_init(&tsi148_device->vme_rmw);
+
+	tsi148_bridge->parent = &pdev->dev;
+	strcpy(tsi148_bridge->name, driver_name);
+
+	/* Setup IRQ */
+	retval = tsi148_irq_init(tsi148_bridge);
+	if (retval != 0) {
+		dev_err(&pdev->dev, "Chip Initialization failed.\n");
+		goto err_irq;
+	}
+
+	/* If we are going to flush writes, we need to read from the VME bus.
+	 * We need to do this safely, thus we read the devices own CR/CSR
+	 * register. To do this we must set up a window in CR/CSR space and
+	 * hence have one less master window resource available.
+	 */
+	master_num = TSI148_MAX_MASTER;
+	if (err_chk) {
+		master_num--;
+
+		tsi148_device->flush_image =
+			kmalloc(sizeof(*tsi148_device->flush_image),
+				GFP_KERNEL);
+		if (!tsi148_device->flush_image) {
+			retval = -ENOMEM;
+			goto err_master;
+		}
+		tsi148_device->flush_image->parent = tsi148_bridge;
+		spin_lock_init(&tsi148_device->flush_image->lock);
+		tsi148_device->flush_image->locked = 1;
+		tsi148_device->flush_image->number = master_num;
+		memset(&tsi148_device->flush_image->bus_resource, 0,
+		       sizeof(tsi148_device->flush_image->bus_resource));
+		tsi148_device->flush_image->kern_base  = NULL;
+	}
+
+	/* Add master windows to list */
+	for (i = 0; i < master_num; i++) {
+		master_image = kmalloc(sizeof(*master_image), GFP_KERNEL);
+		if (!master_image) {
+			retval = -ENOMEM;
+			goto err_master;
+		}
+		master_image->parent = tsi148_bridge;
+		spin_lock_init(&master_image->lock);
+		master_image->locked = 0;
+		master_image->number = i;
+		master_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
+			VME_A64 | VME_CRCSR | VME_USER1 | VME_USER2 |
+			VME_USER3 | VME_USER4;
+		master_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
+			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
+			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
+			VME_PROG | VME_DATA;
+		master_image->width_attr = VME_D16 | VME_D32;
+		memset(&master_image->bus_resource, 0,
+		       sizeof(master_image->bus_resource));
+		master_image->kern_base  = NULL;
+		list_add_tail(&master_image->list,
+			&tsi148_bridge->master_resources);
+	}
+
+	/* Add slave windows to list */
+	for (i = 0; i < TSI148_MAX_SLAVE; i++) {
+		slave_image = kmalloc(sizeof(*slave_image), GFP_KERNEL);
+		if (!slave_image) {
+			retval = -ENOMEM;
+			goto err_slave;
+		}
+		slave_image->parent = tsi148_bridge;
+		mutex_init(&slave_image->mtx);
+		slave_image->locked = 0;
+		slave_image->number = i;
+		slave_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
+			VME_A64;
+		slave_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
+			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
+			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
+			VME_PROG | VME_DATA;
+		list_add_tail(&slave_image->list,
+			&tsi148_bridge->slave_resources);
+	}
+
+	/* Add dma engines to list */
+	for (i = 0; i < TSI148_MAX_DMA; i++) {
+		dma_ctrlr = kmalloc(sizeof(*dma_ctrlr), GFP_KERNEL);
+		if (!dma_ctrlr) {
+			retval = -ENOMEM;
+			goto err_dma;
+		}
+		dma_ctrlr->parent = tsi148_bridge;
+		mutex_init(&dma_ctrlr->mtx);
+		dma_ctrlr->locked = 0;
+		dma_ctrlr->number = i;
+		dma_ctrlr->route_attr = VME_DMA_VME_TO_MEM |
+			VME_DMA_MEM_TO_VME | VME_DMA_VME_TO_VME |
+			VME_DMA_MEM_TO_MEM | VME_DMA_PATTERN_TO_VME |
+			VME_DMA_PATTERN_TO_MEM;
+		INIT_LIST_HEAD(&dma_ctrlr->pending);
+		INIT_LIST_HEAD(&dma_ctrlr->running);
+		list_add_tail(&dma_ctrlr->list,
+			&tsi148_bridge->dma_resources);
+	}
+
+	/* Add location monitor to list */
+	lm = kmalloc(sizeof(*lm), GFP_KERNEL);
+	if (!lm) {
+		retval = -ENOMEM;
+		goto err_lm;
+	}
+	lm->parent = tsi148_bridge;
+	mutex_init(&lm->mtx);
+	lm->locked = 0;
+	lm->number = 1;
+	lm->monitors = 4;
+	list_add_tail(&lm->list, &tsi148_bridge->lm_resources);
+
+	tsi148_bridge->slave_get = tsi148_slave_get;
+	tsi148_bridge->slave_set = tsi148_slave_set;
+	tsi148_bridge->master_get = tsi148_master_get;
+	tsi148_bridge->master_set = tsi148_master_set;
+	tsi148_bridge->master_read = tsi148_master_read;
+	tsi148_bridge->master_write = tsi148_master_write;
+	tsi148_bridge->master_rmw = tsi148_master_rmw;
+	tsi148_bridge->dma_list_add = tsi148_dma_list_add;
+	tsi148_bridge->dma_list_exec = tsi148_dma_list_exec;
+	tsi148_bridge->dma_list_empty = tsi148_dma_list_empty;
+	tsi148_bridge->irq_set = tsi148_irq_set;
+	tsi148_bridge->irq_generate = tsi148_irq_generate;
+	tsi148_bridge->lm_set = tsi148_lm_set;
+	tsi148_bridge->lm_get = tsi148_lm_get;
+	tsi148_bridge->lm_attach = tsi148_lm_attach;
+	tsi148_bridge->lm_detach = tsi148_lm_detach;
+	tsi148_bridge->slot_get = tsi148_slot_get;
+	tsi148_bridge->alloc_consistent = tsi148_alloc_consistent;
+	tsi148_bridge->free_consistent = tsi148_free_consistent;
+
+	data = ioread32be(tsi148_device->base + TSI148_LCSR_VSTAT);
+	dev_info(&pdev->dev, "Board is%s the VME system controller\n",
+		(data & TSI148_LCSR_VSTAT_SCONS) ? "" : " not");
+	if (!geoid)
+		dev_info(&pdev->dev, "VME geographical address is %d\n",
+			data & TSI148_LCSR_VSTAT_GA_M);
+	else
+		dev_info(&pdev->dev, "VME geographical address is set to %d\n",
+			geoid);
+
+	dev_info(&pdev->dev, "VME Write and flush and error check is %s\n",
+		err_chk ? "enabled" : "disabled");
+
+	retval = tsi148_crcsr_init(tsi148_bridge, pdev);
+	if (retval) {
+		dev_err(&pdev->dev, "CR/CSR configuration failed.\n");
+		goto err_crcsr;
+	}
+
+	retval = vme_register_bridge(tsi148_bridge);
+	if (retval != 0) {
+		dev_err(&pdev->dev, "Chip Registration failed.\n");
+		goto err_reg;
+	}
+
+	pci_set_drvdata(pdev, tsi148_bridge);
+
+	/* Clear VME bus "board fail", and "power-up reset" lines */
+	data = ioread32be(tsi148_device->base + TSI148_LCSR_VSTAT);
+	data &= ~TSI148_LCSR_VSTAT_BRDFL;
+	data |= TSI148_LCSR_VSTAT_CPURST;
+	iowrite32be(data, tsi148_device->base + TSI148_LCSR_VSTAT);
+
+	return 0;
+
+err_reg:
+	tsi148_crcsr_exit(tsi148_bridge, pdev);
+err_crcsr:
+err_lm:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &tsi148_bridge->lm_resources) {
+		lm = list_entry(pos, struct vme_lm_resource, list);
+		list_del(pos);
+		kfree(lm);
+	}
+err_dma:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &tsi148_bridge->dma_resources) {
+		dma_ctrlr = list_entry(pos, struct vme_dma_resource, list);
+		list_del(pos);
+		kfree(dma_ctrlr);
+	}
+err_slave:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &tsi148_bridge->slave_resources) {
+		slave_image = list_entry(pos, struct vme_slave_resource, list);
+		list_del(pos);
+		kfree(slave_image);
+	}
+err_master:
+	/* resources are stored in link list */
+	list_for_each_safe(pos, n, &tsi148_bridge->master_resources) {
+		master_image = list_entry(pos, struct vme_master_resource,
+			list);
+		list_del(pos);
+		kfree(master_image);
+	}
+
+	tsi148_irq_exit(tsi148_bridge, pdev);
+err_irq:
+err_test:
+	iounmap(tsi148_device->base);
+err_remap:
+	pci_release_regions(pdev);
+err_resource:
+	pci_disable_device(pdev);
+err_enable:
+	kfree(tsi148_device);
+err_driver:
+	kfree(tsi148_bridge);
+err_struct:
+	return retval;
+
+}
+
+static void tsi148_remove(struct pci_dev *pdev)
+{
+	struct list_head *pos = NULL;
+	struct list_head *tmplist;
+	struct vme_master_resource *master_image;
+	struct vme_slave_resource *slave_image;
+	struct vme_dma_resource *dma_ctrlr;
+	int i;
+	struct tsi148_driver *bridge;
+	struct vme_bridge *tsi148_bridge = pci_get_drvdata(pdev);
+
+	bridge = tsi148_bridge->driver_priv;
+
+
+	dev_dbg(&pdev->dev, "Driver is being unloaded.\n");
+
+	/*
+	 *  Shutdown all inbound and outbound windows.
+	 */
+	for (i = 0; i < 8; i++) {
+		iowrite32be(0, bridge->base + TSI148_LCSR_IT[i] +
+			TSI148_LCSR_OFFSET_ITAT);
+		iowrite32be(0, bridge->base + TSI148_LCSR_OT[i] +
+			TSI148_LCSR_OFFSET_OTAT);
+	}
+
+	/*
+	 *  Shutdown Location monitor.
+	 */
+	iowrite32be(0, bridge->base + TSI148_LCSR_LMAT);
+
+	/*
+	 *  Shutdown CRG map.
+	 */
+	iowrite32be(0, bridge->base + TSI148_LCSR_CSRAT);
+
+	/*
+	 *  Clear error status.
+	 */
+	iowrite32be(0xFFFFFFFF, bridge->base + TSI148_LCSR_EDPAT);
+	iowrite32be(0xFFFFFFFF, bridge->base + TSI148_LCSR_VEAT);
+	iowrite32be(0x07000700, bridge->base + TSI148_LCSR_PSTAT);
+
+	/*
+	 *  Remove VIRQ interrupt (if any)
+	 */
+	if (ioread32be(bridge->base + TSI148_LCSR_VICR) & 0x800)
+		iowrite32be(0x8000, bridge->base + TSI148_LCSR_VICR);
+
+	/*
+	 *  Map all Interrupts to PCI INTA
+	 */
+	iowrite32be(0x0, bridge->base + TSI148_LCSR_INTM1);
+	iowrite32be(0x0, bridge->base + TSI148_LCSR_INTM2);
+
+	tsi148_irq_exit(tsi148_bridge, pdev);
+
+	vme_unregister_bridge(tsi148_bridge);
+
+	tsi148_crcsr_exit(tsi148_bridge, pdev);
+
+	/* resources are stored in link list */
+	list_for_each_safe(pos, tmplist, &tsi148_bridge->dma_resources) {
+		dma_ctrlr = list_entry(pos, struct vme_dma_resource, list);
+		list_del(pos);
+		kfree(dma_ctrlr);
+	}
+
+	/* resources are stored in link list */
+	list_for_each_safe(pos, tmplist, &tsi148_bridge->slave_resources) {
+		slave_image = list_entry(pos, struct vme_slave_resource, list);
+		list_del(pos);
+		kfree(slave_image);
+	}
+
+	/* resources are stored in link list */
+	list_for_each_safe(pos, tmplist, &tsi148_bridge->master_resources) {
+		master_image = list_entry(pos, struct vme_master_resource,
+			list);
+		list_del(pos);
+		kfree(master_image);
+	}
+
+	iounmap(bridge->base);
+
+	pci_release_regions(pdev);
+
+	pci_disable_device(pdev);
+
+	kfree(tsi148_bridge->driver_priv);
+
+	kfree(tsi148_bridge);
+}
+
+module_pci_driver(tsi148_driver);
+
+MODULE_PARM_DESC(err_chk, "Check for VME errors on reads and writes");
+module_param(err_chk, bool, 0);
+
+MODULE_PARM_DESC(geoid, "Override geographical addressing");
+module_param(geoid, int, 0);
+
+MODULE_DESCRIPTION("VME driver for the Tundra Tempe VME bridge");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/vme_user/vme_tsi148.h b/drivers/staging/vme_user/vme_tsi148.h
new file mode 100644
index 000000000..226fedc6f
--- /dev/null
+++ b/drivers/staging/vme_user/vme_tsi148.h
@@ -0,0 +1,1407 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * tsi148.h
+ *
+ * Support for the Tundra TSI148 VME Bridge chip
+ *
+ * Author: Tom Armistead
+ * Updated and maintained by Ajit Prem
+ * Copyright 2004 Motorola Inc.
+ */
+
+#ifndef TSI148_H
+#define TSI148_H
+
+#ifndef	PCI_VENDOR_ID_TUNDRA
+#define	PCI_VENDOR_ID_TUNDRA 0x10e3
+#endif
+
+#ifndef	PCI_DEVICE_ID_TUNDRA_TSI148
+#define	PCI_DEVICE_ID_TUNDRA_TSI148 0x148
+#endif
+
+/*
+ *  Define the number of each that the Tsi148 supports.
+ */
+#define TSI148_MAX_MASTER		8	/* Max Master Windows */
+#define TSI148_MAX_SLAVE		8	/* Max Slave Windows */
+#define TSI148_MAX_DMA			2	/* Max DMA Controllers */
+#define TSI148_MAX_MAILBOX		4	/* Max Mail Box registers */
+#define TSI148_MAX_SEMAPHORE		8	/* Max Semaphores */
+
+/* Structure used to hold driver specific information */
+struct tsi148_driver {
+	void __iomem *base;	/* Base Address of device registers */
+	wait_queue_head_t dma_queue[2];
+	wait_queue_head_t iack_queue;
+	void (*lm_callback[4])(void *);	/* Called in interrupt handler */
+	void *lm_data[4];
+	void *crcsr_kernel;
+	dma_addr_t crcsr_bus;
+	struct vme_master_resource *flush_image;
+	struct mutex vme_rmw;		/* Only one RMW cycle at a time */
+	struct mutex vme_int;		/*
+					 * Only one VME interrupt can be
+					 * generated at a time, provide locking
+					 */
+};
+
+/*
+ * Layout of a DMAC Linked-List Descriptor
+ *
+ * Note: This structure is accessed via the chip and therefore must be
+ *       correctly laid out - It must also be aligned on 64-bit boundaries.
+ */
+struct tsi148_dma_descriptor {
+	__be32 dsau;      /* Source Address */
+	__be32 dsal;
+	__be32 ddau;      /* Destination Address */
+	__be32 ddal;
+	__be32 dsat;      /* Source attributes */
+	__be32 ddat;      /* Destination attributes */
+	__be32 dnlau;     /* Next link address */
+	__be32 dnlal;
+	__be32 dcnt;      /* Byte count */
+	__be32 ddbs;      /* 2eSST Broadcast select */
+};
+
+struct tsi148_dma_entry {
+	/*
+	 * The descriptor needs to be aligned on a 64-bit boundary, we increase
+	 * the chance of this by putting it first in the structure.
+	 */
+	struct tsi148_dma_descriptor descriptor;
+	struct list_head list;
+	dma_addr_t dma_handle;
+};
+
+/*
+ *  TSI148 ASIC register structure overlays and bit field definitions.
+ *
+ *      Note:   Tsi148 Register Group (CRG) consists of the following
+ *              combination of registers:
+ *                      PCFS    - PCI Configuration Space Registers
+ *                      LCSR    - Local Control and Status Registers
+ *                      GCSR    - Global Control and Status Registers
+ *                      CR/CSR  - Subset of Configuration ROM /
+ *                                Control and Status Registers
+ */
+
+
+/*
+ *  Command/Status Registers (CRG + $004)
+ */
+#define TSI148_PCFS_ID			0x0
+#define TSI148_PCFS_CSR			0x4
+#define TSI148_PCFS_CLASS		0x8
+#define TSI148_PCFS_MISC0		0xC
+#define TSI148_PCFS_MBARL		0x10
+#define TSI148_PCFS_MBARU		0x14
+
+#define TSI148_PCFS_SUBID		0x28
+
+#define TSI148_PCFS_CAPP		0x34
+
+#define TSI148_PCFS_MISC1		0x3C
+
+#define TSI148_PCFS_XCAPP		0x40
+#define TSI148_PCFS_XSTAT		0x44
+
+/*
+ * LCSR definitions
+ */
+
+/*
+ *    Outbound Translations
+ */
+#define TSI148_LCSR_OT0_OTSAU		0x100
+#define TSI148_LCSR_OT0_OTSAL		0x104
+#define TSI148_LCSR_OT0_OTEAU		0x108
+#define TSI148_LCSR_OT0_OTEAL		0x10C
+#define TSI148_LCSR_OT0_OTOFU		0x110
+#define TSI148_LCSR_OT0_OTOFL		0x114
+#define TSI148_LCSR_OT0_OTBS		0x118
+#define TSI148_LCSR_OT0_OTAT		0x11C
+
+#define TSI148_LCSR_OT1_OTSAU		0x120
+#define TSI148_LCSR_OT1_OTSAL		0x124
+#define TSI148_LCSR_OT1_OTEAU		0x128
+#define TSI148_LCSR_OT1_OTEAL		0x12C
+#define TSI148_LCSR_OT1_OTOFU		0x130
+#define TSI148_LCSR_OT1_OTOFL		0x134
+#define TSI148_LCSR_OT1_OTBS		0x138
+#define TSI148_LCSR_OT1_OTAT		0x13C
+
+#define TSI148_LCSR_OT2_OTSAU		0x140
+#define TSI148_LCSR_OT2_OTSAL		0x144
+#define TSI148_LCSR_OT2_OTEAU		0x148
+#define TSI148_LCSR_OT2_OTEAL		0x14C
+#define TSI148_LCSR_OT2_OTOFU		0x150
+#define TSI148_LCSR_OT2_OTOFL		0x154
+#define TSI148_LCSR_OT2_OTBS		0x158
+#define TSI148_LCSR_OT2_OTAT		0x15C
+
+#define TSI148_LCSR_OT3_OTSAU		0x160
+#define TSI148_LCSR_OT3_OTSAL		0x164
+#define TSI148_LCSR_OT3_OTEAU		0x168
+#define TSI148_LCSR_OT3_OTEAL		0x16C
+#define TSI148_LCSR_OT3_OTOFU		0x170
+#define TSI148_LCSR_OT3_OTOFL		0x174
+#define TSI148_LCSR_OT3_OTBS		0x178
+#define TSI148_LCSR_OT3_OTAT		0x17C
+
+#define TSI148_LCSR_OT4_OTSAU		0x180
+#define TSI148_LCSR_OT4_OTSAL		0x184
+#define TSI148_LCSR_OT4_OTEAU		0x188
+#define TSI148_LCSR_OT4_OTEAL		0x18C
+#define TSI148_LCSR_OT4_OTOFU		0x190
+#define TSI148_LCSR_OT4_OTOFL		0x194
+#define TSI148_LCSR_OT4_OTBS		0x198
+#define TSI148_LCSR_OT4_OTAT		0x19C
+
+#define TSI148_LCSR_OT5_OTSAU		0x1A0
+#define TSI148_LCSR_OT5_OTSAL		0x1A4
+#define TSI148_LCSR_OT5_OTEAU		0x1A8
+#define TSI148_LCSR_OT5_OTEAL		0x1AC
+#define TSI148_LCSR_OT5_OTOFU		0x1B0
+#define TSI148_LCSR_OT5_OTOFL		0x1B4
+#define TSI148_LCSR_OT5_OTBS		0x1B8
+#define TSI148_LCSR_OT5_OTAT		0x1BC
+
+#define TSI148_LCSR_OT6_OTSAU		0x1C0
+#define TSI148_LCSR_OT6_OTSAL		0x1C4
+#define TSI148_LCSR_OT6_OTEAU		0x1C8
+#define TSI148_LCSR_OT6_OTEAL		0x1CC
+#define TSI148_LCSR_OT6_OTOFU		0x1D0
+#define TSI148_LCSR_OT6_OTOFL		0x1D4
+#define TSI148_LCSR_OT6_OTBS		0x1D8
+#define TSI148_LCSR_OT6_OTAT		0x1DC
+
+#define TSI148_LCSR_OT7_OTSAU		0x1E0
+#define TSI148_LCSR_OT7_OTSAL		0x1E4
+#define TSI148_LCSR_OT7_OTEAU		0x1E8
+#define TSI148_LCSR_OT7_OTEAL		0x1EC
+#define TSI148_LCSR_OT7_OTOFU		0x1F0
+#define TSI148_LCSR_OT7_OTOFL		0x1F4
+#define TSI148_LCSR_OT7_OTBS		0x1F8
+#define TSI148_LCSR_OT7_OTAT		0x1FC
+
+#define TSI148_LCSR_OT0		0x100
+#define TSI148_LCSR_OT1		0x120
+#define TSI148_LCSR_OT2		0x140
+#define TSI148_LCSR_OT3		0x160
+#define TSI148_LCSR_OT4		0x180
+#define TSI148_LCSR_OT5		0x1A0
+#define TSI148_LCSR_OT6		0x1C0
+#define TSI148_LCSR_OT7		0x1E0
+
+static const int TSI148_LCSR_OT[8] = { TSI148_LCSR_OT0, TSI148_LCSR_OT1,
+					 TSI148_LCSR_OT2, TSI148_LCSR_OT3,
+					 TSI148_LCSR_OT4, TSI148_LCSR_OT5,
+					 TSI148_LCSR_OT6, TSI148_LCSR_OT7 };
+
+#define TSI148_LCSR_OFFSET_OTSAU	0x0
+#define TSI148_LCSR_OFFSET_OTSAL	0x4
+#define TSI148_LCSR_OFFSET_OTEAU	0x8
+#define TSI148_LCSR_OFFSET_OTEAL	0xC
+#define TSI148_LCSR_OFFSET_OTOFU	0x10
+#define TSI148_LCSR_OFFSET_OTOFL	0x14
+#define TSI148_LCSR_OFFSET_OTBS		0x18
+#define TSI148_LCSR_OFFSET_OTAT		0x1C
+
+/*
+ * VMEbus interrupt ack
+ * offset  200
+ */
+#define TSI148_LCSR_VIACK1	0x204
+#define TSI148_LCSR_VIACK2	0x208
+#define TSI148_LCSR_VIACK3	0x20C
+#define TSI148_LCSR_VIACK4	0x210
+#define TSI148_LCSR_VIACK5	0x214
+#define TSI148_LCSR_VIACK6	0x218
+#define TSI148_LCSR_VIACK7	0x21C
+
+static const int TSI148_LCSR_VIACK[8] = { 0, TSI148_LCSR_VIACK1,
+				TSI148_LCSR_VIACK2, TSI148_LCSR_VIACK3,
+				TSI148_LCSR_VIACK4, TSI148_LCSR_VIACK5,
+				TSI148_LCSR_VIACK6, TSI148_LCSR_VIACK7 };
+
+/*
+ * RMW
+ * offset    220
+ */
+#define TSI148_LCSR_RMWAU	0x220
+#define TSI148_LCSR_RMWAL	0x224
+#define TSI148_LCSR_RMWEN	0x228
+#define TSI148_LCSR_RMWC	0x22C
+#define TSI148_LCSR_RMWS	0x230
+
+/*
+ * VMEbus control
+ * offset    234
+ */
+#define TSI148_LCSR_VMCTRL	0x234
+#define TSI148_LCSR_VCTRL	0x238
+#define TSI148_LCSR_VSTAT	0x23C
+
+/*
+ * PCI status
+ * offset  240
+ */
+#define TSI148_LCSR_PSTAT	0x240
+
+/*
+ * VME filter.
+ * offset  250
+ */
+#define TSI148_LCSR_VMEFL	0x250
+
+	/*
+	 * VME exception.
+	 * offset  260
+ */
+#define TSI148_LCSR_VEAU	0x260
+#define TSI148_LCSR_VEAL	0x264
+#define TSI148_LCSR_VEAT	0x268
+
+	/*
+	 * PCI error
+	 * offset  270
+	 */
+#define TSI148_LCSR_EDPAU	0x270
+#define TSI148_LCSR_EDPAL	0x274
+#define TSI148_LCSR_EDPXA	0x278
+#define TSI148_LCSR_EDPXS	0x27C
+#define TSI148_LCSR_EDPAT	0x280
+
+	/*
+	 * Inbound Translations
+	 * offset  300
+	 */
+#define TSI148_LCSR_IT0_ITSAU		0x300
+#define TSI148_LCSR_IT0_ITSAL		0x304
+#define TSI148_LCSR_IT0_ITEAU		0x308
+#define TSI148_LCSR_IT0_ITEAL		0x30C
+#define TSI148_LCSR_IT0_ITOFU		0x310
+#define TSI148_LCSR_IT0_ITOFL		0x314
+#define TSI148_LCSR_IT0_ITAT		0x318
+
+#define TSI148_LCSR_IT1_ITSAU		0x320
+#define TSI148_LCSR_IT1_ITSAL		0x324
+#define TSI148_LCSR_IT1_ITEAU		0x328
+#define TSI148_LCSR_IT1_ITEAL		0x32C
+#define TSI148_LCSR_IT1_ITOFU		0x330
+#define TSI148_LCSR_IT1_ITOFL		0x334
+#define TSI148_LCSR_IT1_ITAT		0x338
+
+#define TSI148_LCSR_IT2_ITSAU		0x340
+#define TSI148_LCSR_IT2_ITSAL		0x344
+#define TSI148_LCSR_IT2_ITEAU		0x348
+#define TSI148_LCSR_IT2_ITEAL		0x34C
+#define TSI148_LCSR_IT2_ITOFU		0x350
+#define TSI148_LCSR_IT2_ITOFL		0x354
+#define TSI148_LCSR_IT2_ITAT		0x358
+
+#define TSI148_LCSR_IT3_ITSAU		0x360
+#define TSI148_LCSR_IT3_ITSAL		0x364
+#define TSI148_LCSR_IT3_ITEAU		0x368
+#define TSI148_LCSR_IT3_ITEAL		0x36C
+#define TSI148_LCSR_IT3_ITOFU		0x370
+#define TSI148_LCSR_IT3_ITOFL		0x374
+#define TSI148_LCSR_IT3_ITAT		0x378
+
+#define TSI148_LCSR_IT4_ITSAU		0x380
+#define TSI148_LCSR_IT4_ITSAL		0x384
+#define TSI148_LCSR_IT4_ITEAU		0x388
+#define TSI148_LCSR_IT4_ITEAL		0x38C
+#define TSI148_LCSR_IT4_ITOFU		0x390
+#define TSI148_LCSR_IT4_ITOFL		0x394
+#define TSI148_LCSR_IT4_ITAT		0x398
+
+#define TSI148_LCSR_IT5_ITSAU		0x3A0
+#define TSI148_LCSR_IT5_ITSAL		0x3A4
+#define TSI148_LCSR_IT5_ITEAU		0x3A8
+#define TSI148_LCSR_IT5_ITEAL		0x3AC
+#define TSI148_LCSR_IT5_ITOFU		0x3B0
+#define TSI148_LCSR_IT5_ITOFL		0x3B4
+#define TSI148_LCSR_IT5_ITAT		0x3B8
+
+#define TSI148_LCSR_IT6_ITSAU		0x3C0
+#define TSI148_LCSR_IT6_ITSAL		0x3C4
+#define TSI148_LCSR_IT6_ITEAU		0x3C8
+#define TSI148_LCSR_IT6_ITEAL		0x3CC
+#define TSI148_LCSR_IT6_ITOFU		0x3D0
+#define TSI148_LCSR_IT6_ITOFL		0x3D4
+#define TSI148_LCSR_IT6_ITAT		0x3D8
+
+#define TSI148_LCSR_IT7_ITSAU		0x3E0
+#define TSI148_LCSR_IT7_ITSAL		0x3E4
+#define TSI148_LCSR_IT7_ITEAU		0x3E8
+#define TSI148_LCSR_IT7_ITEAL		0x3EC
+#define TSI148_LCSR_IT7_ITOFU		0x3F0
+#define TSI148_LCSR_IT7_ITOFL		0x3F4
+#define TSI148_LCSR_IT7_ITAT		0x3F8
+
+
+#define TSI148_LCSR_IT0		0x300
+#define TSI148_LCSR_IT1		0x320
+#define TSI148_LCSR_IT2		0x340
+#define TSI148_LCSR_IT3		0x360
+#define TSI148_LCSR_IT4		0x380
+#define TSI148_LCSR_IT5		0x3A0
+#define TSI148_LCSR_IT6		0x3C0
+#define TSI148_LCSR_IT7		0x3E0
+
+static const int TSI148_LCSR_IT[8] = { TSI148_LCSR_IT0, TSI148_LCSR_IT1,
+					 TSI148_LCSR_IT2, TSI148_LCSR_IT3,
+					 TSI148_LCSR_IT4, TSI148_LCSR_IT5,
+					 TSI148_LCSR_IT6, TSI148_LCSR_IT7 };
+
+#define TSI148_LCSR_OFFSET_ITSAU	0x0
+#define TSI148_LCSR_OFFSET_ITSAL	0x4
+#define TSI148_LCSR_OFFSET_ITEAU	0x8
+#define TSI148_LCSR_OFFSET_ITEAL	0xC
+#define TSI148_LCSR_OFFSET_ITOFU	0x10
+#define TSI148_LCSR_OFFSET_ITOFL	0x14
+#define TSI148_LCSR_OFFSET_ITAT		0x18
+
+	/*
+	 * Inbound Translation GCSR
+	 * offset  400
+	 */
+#define TSI148_LCSR_GBAU	0x400
+#define TSI148_LCSR_GBAL	0x404
+#define TSI148_LCSR_GCSRAT	0x408
+
+	/*
+	 * Inbound Translation CRG
+	 * offset  40C
+	 */
+#define TSI148_LCSR_CBAU	0x40C
+#define TSI148_LCSR_CBAL	0x410
+#define TSI148_LCSR_CSRAT	0x414
+
+	/*
+	 * Inbound Translation CR/CSR
+	 *         CRG
+	 * offset  418
+	 */
+#define TSI148_LCSR_CROU	0x418
+#define TSI148_LCSR_CROL	0x41C
+#define TSI148_LCSR_CRAT	0x420
+
+	/*
+	 * Inbound Translation Location Monitor
+	 * offset  424
+	 */
+#define TSI148_LCSR_LMBAU	0x424
+#define TSI148_LCSR_LMBAL	0x428
+#define TSI148_LCSR_LMAT	0x42C
+
+	/*
+	 * VMEbus Interrupt Control.
+	 * offset  430
+	 */
+#define TSI148_LCSR_BCU		0x430
+#define TSI148_LCSR_BCL		0x434
+#define TSI148_LCSR_BPGTR	0x438
+#define TSI148_LCSR_BPCTR	0x43C
+#define TSI148_LCSR_VICR	0x440
+
+	/*
+	 * Local Bus Interrupt Control.
+	 * offset  448
+	 */
+#define TSI148_LCSR_INTEN	0x448
+#define TSI148_LCSR_INTEO	0x44C
+#define TSI148_LCSR_INTS	0x450
+#define TSI148_LCSR_INTC	0x454
+#define TSI148_LCSR_INTM1	0x458
+#define TSI148_LCSR_INTM2	0x45C
+
+	/*
+	 * DMA Controllers
+	 * offset 500
+	 */
+#define TSI148_LCSR_DCTL0	0x500
+#define TSI148_LCSR_DSTA0	0x504
+#define TSI148_LCSR_DCSAU0	0x508
+#define TSI148_LCSR_DCSAL0	0x50C
+#define TSI148_LCSR_DCDAU0	0x510
+#define TSI148_LCSR_DCDAL0	0x514
+#define TSI148_LCSR_DCLAU0	0x518
+#define TSI148_LCSR_DCLAL0	0x51C
+#define TSI148_LCSR_DSAU0	0x520
+#define TSI148_LCSR_DSAL0	0x524
+#define TSI148_LCSR_DDAU0	0x528
+#define TSI148_LCSR_DDAL0	0x52C
+#define TSI148_LCSR_DSAT0	0x530
+#define TSI148_LCSR_DDAT0	0x534
+#define TSI148_LCSR_DNLAU0	0x538
+#define TSI148_LCSR_DNLAL0	0x53C
+#define TSI148_LCSR_DCNT0	0x540
+#define TSI148_LCSR_DDBS0	0x544
+
+#define TSI148_LCSR_DCTL1	0x580
+#define TSI148_LCSR_DSTA1	0x584
+#define TSI148_LCSR_DCSAU1	0x588
+#define TSI148_LCSR_DCSAL1	0x58C
+#define TSI148_LCSR_DCDAU1	0x590
+#define TSI148_LCSR_DCDAL1	0x594
+#define TSI148_LCSR_DCLAU1	0x598
+#define TSI148_LCSR_DCLAL1	0x59C
+#define TSI148_LCSR_DSAU1	0x5A0
+#define TSI148_LCSR_DSAL1	0x5A4
+#define TSI148_LCSR_DDAU1	0x5A8
+#define TSI148_LCSR_DDAL1	0x5AC
+#define TSI148_LCSR_DSAT1	0x5B0
+#define TSI148_LCSR_DDAT1	0x5B4
+#define TSI148_LCSR_DNLAU1	0x5B8
+#define TSI148_LCSR_DNLAL1	0x5BC
+#define TSI148_LCSR_DCNT1	0x5C0
+#define TSI148_LCSR_DDBS1	0x5C4
+
+#define TSI148_LCSR_DMA0	0x500
+#define TSI148_LCSR_DMA1	0x580
+
+
+static const int TSI148_LCSR_DMA[TSI148_MAX_DMA] = { TSI148_LCSR_DMA0,
+						TSI148_LCSR_DMA1 };
+
+#define TSI148_LCSR_OFFSET_DCTL		0x0
+#define TSI148_LCSR_OFFSET_DSTA		0x4
+#define TSI148_LCSR_OFFSET_DCSAU	0x8
+#define TSI148_LCSR_OFFSET_DCSAL	0xC
+#define TSI148_LCSR_OFFSET_DCDAU	0x10
+#define TSI148_LCSR_OFFSET_DCDAL	0x14
+#define TSI148_LCSR_OFFSET_DCLAU	0x18
+#define TSI148_LCSR_OFFSET_DCLAL	0x1C
+#define TSI148_LCSR_OFFSET_DSAU		0x20
+#define TSI148_LCSR_OFFSET_DSAL		0x24
+#define TSI148_LCSR_OFFSET_DDAU		0x28
+#define TSI148_LCSR_OFFSET_DDAL		0x2C
+#define TSI148_LCSR_OFFSET_DSAT		0x30
+#define TSI148_LCSR_OFFSET_DDAT		0x34
+#define TSI148_LCSR_OFFSET_DNLAU	0x38
+#define TSI148_LCSR_OFFSET_DNLAL	0x3C
+#define TSI148_LCSR_OFFSET_DCNT		0x40
+#define TSI148_LCSR_OFFSET_DDBS		0x44
+
+	/*
+	 * GCSR Register Group
+	 */
+
+	/*
+	 *         GCSR    CRG
+	 * offset   00     600 - DEVI/VENI
+	 * offset   04     604 - CTRL/GA/REVID
+	 * offset   08     608 - Semaphore3/2/1/0
+	 * offset   0C     60C - Seamphore7/6/5/4
+	 */
+#define TSI148_GCSR_ID		0x600
+#define TSI148_GCSR_CSR		0x604
+#define TSI148_GCSR_SEMA0	0x608
+#define TSI148_GCSR_SEMA1	0x60C
+
+	/*
+	 * Mail Box
+	 *         GCSR    CRG
+	 * offset   10     610 - Mailbox0
+	 */
+#define TSI148_GCSR_MBOX0	0x610
+#define TSI148_GCSR_MBOX1	0x614
+#define TSI148_GCSR_MBOX2	0x618
+#define TSI148_GCSR_MBOX3	0x61C
+
+static const int TSI148_GCSR_MBOX[4] = { TSI148_GCSR_MBOX0,
+					TSI148_GCSR_MBOX1,
+					TSI148_GCSR_MBOX2,
+					TSI148_GCSR_MBOX3 };
+
+	/*
+	 * CR/CSR
+	 */
+
+	/*
+	 *        CR/CSR   CRG
+	 * offset  7FFF4   FF4 - CSRBCR
+	 * offset  7FFF8   FF8 - CSRBSR
+	 * offset  7FFFC   FFC - CBAR
+	 */
+#define TSI148_CSRBCR	0xFF4
+#define TSI148_CSRBSR	0xFF8
+#define TSI148_CBAR	0xFFC
+
+
+
+
+	/*
+	 *  TSI148 Register Bit Definitions
+	 */
+
+	/*
+	 *  PFCS Register Set
+	 */
+#define TSI148_PCFS_CMMD_SERR          (1<<8)	/* SERR_L out pin ssys err */
+#define TSI148_PCFS_CMMD_PERR          (1<<6)	/* PERR_L out pin  parity */
+#define TSI148_PCFS_CMMD_MSTR          (1<<2)	/* PCI bus master */
+#define TSI148_PCFS_CMMD_MEMSP         (1<<1)	/* PCI mem space access  */
+#define TSI148_PCFS_CMMD_IOSP          (1<<0)	/* PCI I/O space enable */
+
+#define TSI148_PCFS_STAT_RCPVE         (1<<15)	/* Detected Parity Error */
+#define TSI148_PCFS_STAT_SIGSE         (1<<14)	/* Signalled System Error */
+#define TSI148_PCFS_STAT_RCVMA         (1<<13)	/* Received Master Abort */
+#define TSI148_PCFS_STAT_RCVTA         (1<<12)	/* Received Target Abort */
+#define TSI148_PCFS_STAT_SIGTA         (1<<11)	/* Signalled Target Abort */
+#define TSI148_PCFS_STAT_SELTIM        (3<<9)	/* DELSEL Timing */
+#define TSI148_PCFS_STAT_DPAR          (1<<8)	/* Data Parity Err Reported */
+#define TSI148_PCFS_STAT_FAST          (1<<7)	/* Fast back-to-back Cap */
+#define TSI148_PCFS_STAT_P66M          (1<<5)	/* 66 MHz Capable */
+#define TSI148_PCFS_STAT_CAPL          (1<<4)	/* Capab List - address $34 */
+
+/*
+ *  Revision ID/Class Code Registers   (CRG +$008)
+ */
+#define TSI148_PCFS_CLAS_M             (0xFF<<24)	/* Class ID */
+#define TSI148_PCFS_SUBCLAS_M          (0xFF<<16)	/* Sub-Class ID */
+#define TSI148_PCFS_PROGIF_M           (0xFF<<8)	/* Sub-Class ID */
+#define TSI148_PCFS_REVID_M            (0xFF<<0)	/* Rev ID */
+
+/*
+ * Cache Line Size/ Master Latency Timer/ Header Type Registers (CRG + $00C)
+ */
+#define TSI148_PCFS_HEAD_M             (0xFF<<16)	/* Master Lat Timer */
+#define TSI148_PCFS_MLAT_M             (0xFF<<8)	/* Master Lat Timer */
+#define TSI148_PCFS_CLSZ_M             (0xFF<<0)	/* Cache Line Size */
+
+/*
+ *  Memory Base Address Lower Reg (CRG + $010)
+ */
+#define TSI148_PCFS_MBARL_BASEL_M      (0xFFFFF<<12) /* Base Addr Lower Mask */
+#define TSI148_PCFS_MBARL_PRE          (1<<3)	/* Prefetch */
+#define TSI148_PCFS_MBARL_MTYPE_M      (3<<1)	/* Memory Type Mask */
+#define TSI148_PCFS_MBARL_IOMEM        (1<<0)	/* I/O Space Indicator */
+
+/*
+ *  Message Signaled Interrupt Capabilities Register (CRG + $040)
+ */
+#define TSI148_PCFS_MSICAP_64BAC       (1<<7)	/* 64-bit Address Capable */
+#define TSI148_PCFS_MSICAP_MME_M       (7<<4)	/* Multiple Msg Enable Mask */
+#define TSI148_PCFS_MSICAP_MMC_M       (7<<1)	/* Multiple Msg Capable Mask */
+#define TSI148_PCFS_MSICAP_MSIEN       (1<<0)	/* Msg signaled INT Enable */
+
+/*
+ *  Message Address Lower Register (CRG +$044)
+ */
+#define TSI148_PCFS_MSIAL_M            (0x3FFFFFFF<<2)	/* Mask */
+
+/*
+ *  Message Data Register (CRG + 4C)
+ */
+#define TSI148_PCFS_MSIMD_M            (0xFFFF<<0)	/* Mask */
+
+/*
+ *  PCI-X Capabilities Register (CRG + $050)
+ */
+#define TSI148_PCFS_PCIXCAP_MOST_M     (7<<4)	/* Max outstanding Split Tran */
+#define TSI148_PCFS_PCIXCAP_MMRBC_M    (3<<2)	/* Max Mem Read byte cnt */
+#define TSI148_PCFS_PCIXCAP_ERO        (1<<1)	/* Enable Relaxed Ordering */
+#define TSI148_PCFS_PCIXCAP_DPERE      (1<<0)	/* Data Parity Recover Enable */
+
+/*
+ *  PCI-X Status Register (CRG +$054)
+ */
+#define TSI148_PCFS_PCIXSTAT_RSCEM     (1<<29)	/* Received Split Comp Error */
+#define TSI148_PCFS_PCIXSTAT_DMCRS_M   (7<<26)	/* max Cumulative Read Size */
+#define TSI148_PCFS_PCIXSTAT_DMOST_M   (7<<23)	/* max outstanding Split Trans
+						 */
+#define TSI148_PCFS_PCIXSTAT_DMMRC_M   (3<<21)	/* max mem read byte count */
+#define TSI148_PCFS_PCIXSTAT_DC        (1<<20)	/* Device Complexity */
+#define TSI148_PCFS_PCIXSTAT_USC       (1<<19)	/* Unexpected Split comp */
+#define TSI148_PCFS_PCIXSTAT_SCD       (1<<18)	/* Split completion discard */
+#define TSI148_PCFS_PCIXSTAT_133C      (1<<17)	/* 133MHz capable */
+#define TSI148_PCFS_PCIXSTAT_64D       (1<<16)	/* 64 bit device */
+#define TSI148_PCFS_PCIXSTAT_BN_M      (0xFF<<8)	/* Bus number */
+#define TSI148_PCFS_PCIXSTAT_DN_M      (0x1F<<3)	/* Device number */
+#define TSI148_PCFS_PCIXSTAT_FN_M      (7<<0)	/* Function Number */
+
+/*
+ *  LCSR Registers
+ */
+
+/*
+ *  Outbound Translation Starting Address Lower
+ */
+#define TSI148_LCSR_OTSAL_M            (0xFFFF<<16)	/* Mask */
+
+/*
+ *  Outbound Translation Ending Address Lower
+ */
+#define TSI148_LCSR_OTEAL_M            (0xFFFF<<16)	/* Mask */
+
+/*
+ *  Outbound Translation Offset Lower
+ */
+#define TSI148_LCSR_OTOFFL_M           (0xFFFF<<16)	/* Mask */
+
+/*
+ *  Outbound Translation 2eSST Broadcast Select
+ */
+#define TSI148_LCSR_OTBS_M             (0xFFFFF<<0)	/* Mask */
+
+/*
+ *  Outbound Translation Attribute
+ */
+#define TSI148_LCSR_OTAT_EN            (1<<31)	/* Window Enable */
+#define TSI148_LCSR_OTAT_MRPFD         (1<<18)	/* Prefetch Disable */
+
+#define TSI148_LCSR_OTAT_PFS_M         (3<<16)	/* Prefetch Size Mask */
+#define TSI148_LCSR_OTAT_PFS_2         (0<<16)	/* 2 Cache Lines P Size */
+#define TSI148_LCSR_OTAT_PFS_4         (1<<16)	/* 4 Cache Lines P Size */
+#define TSI148_LCSR_OTAT_PFS_8         (2<<16)	/* 8 Cache Lines P Size */
+#define TSI148_LCSR_OTAT_PFS_16        (3<<16)	/* 16 Cache Lines P Size */
+
+#define TSI148_LCSR_OTAT_2eSSTM_M      (7<<11)	/* 2eSST Xfer Rate Mask */
+#define TSI148_LCSR_OTAT_2eSSTM_160    (0<<11)	/* 160MB/s 2eSST Xfer Rate */
+#define TSI148_LCSR_OTAT_2eSSTM_267    (1<<11)	/* 267MB/s 2eSST Xfer Rate */
+#define TSI148_LCSR_OTAT_2eSSTM_320    (2<<11)	/* 320MB/s 2eSST Xfer Rate */
+
+#define TSI148_LCSR_OTAT_TM_M          (7<<8)	/* Xfer Protocol Mask */
+#define TSI148_LCSR_OTAT_TM_SCT        (0<<8)	/* SCT Xfer Protocol */
+#define TSI148_LCSR_OTAT_TM_BLT        (1<<8)	/* BLT Xfer Protocol */
+#define TSI148_LCSR_OTAT_TM_MBLT       (2<<8)	/* MBLT Xfer Protocol */
+#define TSI148_LCSR_OTAT_TM_2eVME      (3<<8)	/* 2eVME Xfer Protocol */
+#define TSI148_LCSR_OTAT_TM_2eSST      (4<<8)	/* 2eSST Xfer Protocol */
+#define TSI148_LCSR_OTAT_TM_2eSSTB     (5<<8)	/* 2eSST Bcast Xfer Protocol */
+
+#define TSI148_LCSR_OTAT_DBW_M         (3<<6)	/* Max Data Width */
+#define TSI148_LCSR_OTAT_DBW_16        (0<<6)	/* 16-bit Data Width */
+#define TSI148_LCSR_OTAT_DBW_32        (1<<6)	/* 32-bit Data Width */
+
+#define TSI148_LCSR_OTAT_SUP           (1<<5)	/* Supervisory Access */
+#define TSI148_LCSR_OTAT_PGM           (1<<4)	/* Program Access */
+
+#define TSI148_LCSR_OTAT_AMODE_M       (0xf<<0)	/* Address Mode Mask */
+#define TSI148_LCSR_OTAT_AMODE_A16     (0<<0)	/* A16 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_A24     (1<<0)	/* A24 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_A32     (2<<0)	/* A32 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_A64     (4<<0)	/* A32 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_CRCSR   (5<<0)	/* CR/CSR Address Space */
+#define TSI148_LCSR_OTAT_AMODE_USER1   (8<<0)	/* User1 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_USER2   (9<<0)	/* User2 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_USER3   (10<<0)	/* User3 Address Space */
+#define TSI148_LCSR_OTAT_AMODE_USER4   (11<<0)	/* User4 Address Space */
+
+/*
+ *  VME Master Control Register  CRG+$234
+ */
+#define TSI148_LCSR_VMCTRL_VSA         (1<<27)	/* VMEbus Stop Ack */
+#define TSI148_LCSR_VMCTRL_VS          (1<<26)	/* VMEbus Stop */
+#define TSI148_LCSR_VMCTRL_DHB         (1<<25)	/* Device Has Bus */
+#define TSI148_LCSR_VMCTRL_DWB         (1<<24)	/* Device Wants Bus */
+
+#define TSI148_LCSR_VMCTRL_RMWEN       (1<<20)	/* RMW Enable */
+
+#define TSI148_LCSR_VMCTRL_ATO_M       (7<<16)	/* Master Access Time-out Mask
+						 */
+#define TSI148_LCSR_VMCTRL_ATO_32      (0<<16)	/* 32 us */
+#define TSI148_LCSR_VMCTRL_ATO_128     (1<<16)	/* 128 us */
+#define TSI148_LCSR_VMCTRL_ATO_512     (2<<16)	/* 512 us */
+#define TSI148_LCSR_VMCTRL_ATO_2M      (3<<16)	/* 2 ms */
+#define TSI148_LCSR_VMCTRL_ATO_8M      (4<<16)	/* 8 ms */
+#define TSI148_LCSR_VMCTRL_ATO_32M     (5<<16)	/* 32 ms */
+#define TSI148_LCSR_VMCTRL_ATO_128M    (6<<16)	/* 128 ms */
+#define TSI148_LCSR_VMCTRL_ATO_DIS     (7<<16)	/* Disabled */
+
+#define TSI148_LCSR_VMCTRL_VTOFF_M     (7<<12)	/* VMEbus Master Time off */
+#define TSI148_LCSR_VMCTRL_VTOFF_0     (0<<12)	/* 0us */
+#define TSI148_LCSR_VMCTRL_VTOFF_1     (1<<12)	/* 1us */
+#define TSI148_LCSR_VMCTRL_VTOFF_2     (2<<12)	/* 2us */
+#define TSI148_LCSR_VMCTRL_VTOFF_4     (3<<12)	/* 4us */
+#define TSI148_LCSR_VMCTRL_VTOFF_8     (4<<12)	/* 8us */
+#define TSI148_LCSR_VMCTRL_VTOFF_16    (5<<12)	/* 16us */
+#define TSI148_LCSR_VMCTRL_VTOFF_32    (6<<12)	/* 32us */
+#define TSI148_LCSR_VMCTRL_VTOFF_64    (7<<12)	/* 64us */
+
+#define TSI148_LCSR_VMCTRL_VTON_M      (7<<8)	/* VMEbus Master Time On */
+#define TSI148_LCSR_VMCTRL_VTON_4      (0<<8)	/* 8us */
+#define TSI148_LCSR_VMCTRL_VTON_8      (1<<8)	/* 8us */
+#define TSI148_LCSR_VMCTRL_VTON_16     (2<<8)	/* 16us */
+#define TSI148_LCSR_VMCTRL_VTON_32     (3<<8)	/* 32us */
+#define TSI148_LCSR_VMCTRL_VTON_64     (4<<8)	/* 64us */
+#define TSI148_LCSR_VMCTRL_VTON_128    (5<<8)	/* 128us */
+#define TSI148_LCSR_VMCTRL_VTON_256    (6<<8)	/* 256us */
+#define TSI148_LCSR_VMCTRL_VTON_512    (7<<8)	/* 512us */
+
+#define TSI148_LCSR_VMCTRL_VREL_M      (3<<3)	/* VMEbus Master Rel Mode Mask
+						 */
+#define TSI148_LCSR_VMCTRL_VREL_T_D    (0<<3)	/* Time on or Done */
+#define TSI148_LCSR_VMCTRL_VREL_T_R_D  (1<<3)	/* Time on and REQ or Done */
+#define TSI148_LCSR_VMCTRL_VREL_T_B_D  (2<<3)	/* Time on and BCLR or Done */
+#define TSI148_LCSR_VMCTRL_VREL_T_D_R  (3<<3)	/* Time on or Done and REQ */
+
+#define TSI148_LCSR_VMCTRL_VFAIR       (1<<2)	/* VMEbus Master Fair Mode */
+#define TSI148_LCSR_VMCTRL_VREQL_M     (3<<0)	/* VMEbus Master Req Level Mask
+						 */
+
+/*
+ *  VMEbus Control Register CRG+$238
+ */
+#define TSI148_LCSR_VCTRL_LRE          (1<<31)	/* Late Retry Enable */
+
+#define TSI148_LCSR_VCTRL_DLT_M        (0xF<<24)	/* Deadlock Timer */
+#define TSI148_LCSR_VCTRL_DLT_OFF      (0<<24)	/* Deadlock Timer Off */
+#define TSI148_LCSR_VCTRL_DLT_16       (1<<24)	/* 16 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_32       (2<<24)	/* 32 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_64       (3<<24)	/* 64 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_128      (4<<24)	/* 128 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_256      (5<<24)	/* 256 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_512      (6<<24)	/* 512 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_1024     (7<<24)	/* 1024 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_2048     (8<<24)	/* 2048 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_4096     (9<<24)	/* 4096 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_8192     (0xA<<24)	/* 8192 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_16384    (0xB<<24)	/* 16384 VCLKS */
+#define TSI148_LCSR_VCTRL_DLT_32768    (0xC<<24)	/* 32768 VCLKS */
+
+#define TSI148_LCSR_VCTRL_NERBB        (1<<20)	/* No Early Release of Bus Busy
+						 */
+
+#define TSI148_LCSR_VCTRL_SRESET       (1<<17)	/* System Reset */
+#define TSI148_LCSR_VCTRL_LRESET       (1<<16)	/* Local Reset */
+
+#define TSI148_LCSR_VCTRL_SFAILAI      (1<<15)	/* SYSFAIL Auto Slot ID */
+#define TSI148_LCSR_VCTRL_BID_M        (0x1F<<8)	/* Broadcast ID Mask */
+
+#define TSI148_LCSR_VCTRL_ATOEN        (1<<7)	/* Arbiter Time-out Enable */
+#define TSI148_LCSR_VCTRL_ROBIN        (1<<6)	/* VMEbus Round Robin */
+
+#define TSI148_LCSR_VCTRL_GTO_M        (7<<0)	/* VMEbus Global Time-out Mask
+						 */
+#define TSI148_LCSR_VCTRL_GTO_8	      (0<<0)	/* 8 us */
+#define TSI148_LCSR_VCTRL_GTO_16	      (1<<0)	/* 16 us */
+#define TSI148_LCSR_VCTRL_GTO_32	      (2<<0)	/* 32 us */
+#define TSI148_LCSR_VCTRL_GTO_64	      (3<<0)	/* 64 us */
+#define TSI148_LCSR_VCTRL_GTO_128      (4<<0)	/* 128 us */
+#define TSI148_LCSR_VCTRL_GTO_256      (5<<0)	/* 256 us */
+#define TSI148_LCSR_VCTRL_GTO_512      (6<<0)	/* 512 us */
+#define TSI148_LCSR_VCTRL_GTO_DIS      (7<<0)	/* Disabled */
+
+/*
+ *  VMEbus Status Register  CRG + $23C
+ */
+#define TSI148_LCSR_VSTAT_CPURST       (1<<15)	/* Clear power up reset */
+#define TSI148_LCSR_VSTAT_BRDFL        (1<<14)	/* Board fail */
+#define TSI148_LCSR_VSTAT_PURSTS       (1<<12)	/* Power up reset status */
+#define TSI148_LCSR_VSTAT_BDFAILS      (1<<11)	/* Board Fail Status */
+#define TSI148_LCSR_VSTAT_SYSFAILS     (1<<10)	/* System Fail Status */
+#define TSI148_LCSR_VSTAT_ACFAILS      (1<<9)	/* AC fail status */
+#define TSI148_LCSR_VSTAT_SCONS        (1<<8)	/* System Cont Status */
+#define TSI148_LCSR_VSTAT_GAP          (1<<5)	/* Geographic Addr Parity */
+#define TSI148_LCSR_VSTAT_GA_M         (0x1F<<0)  /* Geographic Addr Mask */
+
+/*
+ *  PCI Configuration Status Register CRG+$240
+ */
+#define TSI148_LCSR_PSTAT_REQ64S       (1<<6)	/* Request 64 status set */
+#define TSI148_LCSR_PSTAT_M66ENS       (1<<5)	/* M66ENS 66Mhz enable */
+#define TSI148_LCSR_PSTAT_FRAMES       (1<<4)	/* Frame Status */
+#define TSI148_LCSR_PSTAT_IRDYS        (1<<3)	/* IRDY status */
+#define TSI148_LCSR_PSTAT_DEVSELS      (1<<2)	/* DEVL status */
+#define TSI148_LCSR_PSTAT_STOPS        (1<<1)	/* STOP status */
+#define TSI148_LCSR_PSTAT_TRDYS        (1<<0)	/* TRDY status */
+
+/*
+ *  VMEbus Exception Attributes Register  CRG + $268
+ */
+#define TSI148_LCSR_VEAT_VES           (1<<31)	/* Status */
+#define TSI148_LCSR_VEAT_VEOF          (1<<30)	/* Overflow */
+#define TSI148_LCSR_VEAT_VESCL         (1<<29)	/* Status Clear */
+#define TSI148_LCSR_VEAT_2EOT          (1<<21)	/* 2e Odd Termination */
+#define TSI148_LCSR_VEAT_2EST          (1<<20)	/* 2e Slave terminated */
+#define TSI148_LCSR_VEAT_BERR          (1<<19)	/* Bus Error */
+#define TSI148_LCSR_VEAT_LWORD         (1<<18)	/* LWORD_ signal state */
+#define TSI148_LCSR_VEAT_WRITE         (1<<17)	/* WRITE_ signal state */
+#define TSI148_LCSR_VEAT_IACK          (1<<16)	/* IACK_ signal state */
+#define TSI148_LCSR_VEAT_DS1           (1<<15)	/* DS1_ signal state */
+#define TSI148_LCSR_VEAT_DS0           (1<<14)	/* DS0_ signal state */
+#define TSI148_LCSR_VEAT_AM_M          (0x3F<<8)	/* Address Mode Mask */
+#define TSI148_LCSR_VEAT_XAM_M         (0xFF<<0)	/* Master AMode Mask */
+
+
+/*
+ * VMEbus PCI Error Diagnostics PCI/X Attributes Register  CRG + $280
+ */
+#define TSI148_LCSR_EDPAT_EDPCL        (1<<29)
+
+/*
+ *  Inbound Translation Starting Address Lower
+ */
+#define TSI148_LCSR_ITSAL6432_M        (0xFFFF<<16)	/* Mask */
+#define TSI148_LCSR_ITSAL24_M          (0x00FFF<<12)	/* Mask */
+#define TSI148_LCSR_ITSAL16_M          (0x0000FFF<<4)	/* Mask */
+
+/*
+ *  Inbound Translation Ending Address Lower
+ */
+#define TSI148_LCSR_ITEAL6432_M        (0xFFFF<<16)	/* Mask */
+#define TSI148_LCSR_ITEAL24_M          (0x00FFF<<12)	/* Mask */
+#define TSI148_LCSR_ITEAL16_M          (0x0000FFF<<4)	/* Mask */
+
+/*
+ *  Inbound Translation Offset Lower
+ */
+#define TSI148_LCSR_ITOFFL6432_M       (0xFFFF<<16)	/* Mask */
+#define TSI148_LCSR_ITOFFL24_M         (0xFFFFF<<12)	/* Mask */
+#define TSI148_LCSR_ITOFFL16_M         (0xFFFFFFF<<4)	/* Mask */
+
+/*
+ *  Inbound Translation Attribute
+ */
+#define TSI148_LCSR_ITAT_EN            (1<<31)	/* Window Enable */
+#define TSI148_LCSR_ITAT_TH            (1<<18)	/* Prefetch Threshold */
+
+#define TSI148_LCSR_ITAT_VFS_M         (3<<16)	/* Virtual FIFO Size Mask */
+#define TSI148_LCSR_ITAT_VFS_64        (0<<16)	/* 64 bytes Virtual FIFO Size */
+#define TSI148_LCSR_ITAT_VFS_128       (1<<16)	/* 128 bytes Virtual FIFO Sz */
+#define TSI148_LCSR_ITAT_VFS_256       (2<<16)	/* 256 bytes Virtual FIFO Sz */
+#define TSI148_LCSR_ITAT_VFS_512       (3<<16)	/* 512 bytes Virtual FIFO Sz */
+
+#define TSI148_LCSR_ITAT_2eSSTM_M      (7<<12)	/* 2eSST Xfer Rate Mask */
+#define TSI148_LCSR_ITAT_2eSSTM_160    (0<<12)	/* 160MB/s 2eSST Xfer Rate */
+#define TSI148_LCSR_ITAT_2eSSTM_267    (1<<12)	/* 267MB/s 2eSST Xfer Rate */
+#define TSI148_LCSR_ITAT_2eSSTM_320    (2<<12)	/* 320MB/s 2eSST Xfer Rate */
+
+#define TSI148_LCSR_ITAT_2eSSTB        (1<<11)	/* 2eSST Bcast Xfer Protocol */
+#define TSI148_LCSR_ITAT_2eSST         (1<<10)	/* 2eSST Xfer Protocol */
+#define TSI148_LCSR_ITAT_2eVME         (1<<9)	/* 2eVME Xfer Protocol */
+#define TSI148_LCSR_ITAT_MBLT          (1<<8)	/* MBLT Xfer Protocol */
+#define TSI148_LCSR_ITAT_BLT           (1<<7)	/* BLT Xfer Protocol */
+
+#define TSI148_LCSR_ITAT_AS_M          (7<<4)	/* Address Space Mask */
+#define TSI148_LCSR_ITAT_AS_A16        (0<<4)	/* A16 Address Space */
+#define TSI148_LCSR_ITAT_AS_A24        (1<<4)	/* A24 Address Space */
+#define TSI148_LCSR_ITAT_AS_A32        (2<<4)	/* A32 Address Space */
+#define TSI148_LCSR_ITAT_AS_A64        (4<<4)	/* A64 Address Space */
+
+#define TSI148_LCSR_ITAT_SUPR          (1<<3)	/* Supervisor Access */
+#define TSI148_LCSR_ITAT_NPRIV         (1<<2)	/* Non-Priv (User) Access */
+#define TSI148_LCSR_ITAT_PGM           (1<<1)	/* Program Access */
+#define TSI148_LCSR_ITAT_DATA          (1<<0)	/* Data Access */
+
+/*
+ *  GCSR Base Address Lower Address  CRG +$404
+ */
+#define TSI148_LCSR_GBAL_M             (0x7FFFFFF<<5)	/* Mask */
+
+/*
+ *  GCSR Attribute Register CRG + $408
+ */
+#define TSI148_LCSR_GCSRAT_EN          (1<<7)	/* Enable access to GCSR */
+
+#define TSI148_LCSR_GCSRAT_AS_M        (7<<4)	/* Address Space Mask */
+#define TSI148_LCSR_GCSRAT_AS_A16       (0<<4)	/* Address Space 16 */
+#define TSI148_LCSR_GCSRAT_AS_A24       (1<<4)	/* Address Space 24 */
+#define TSI148_LCSR_GCSRAT_AS_A32       (2<<4)	/* Address Space 32 */
+#define TSI148_LCSR_GCSRAT_AS_A64       (4<<4)	/* Address Space 64 */
+
+#define TSI148_LCSR_GCSRAT_SUPR        (1<<3)	/* Sup set -GCSR decoder */
+#define TSI148_LCSR_GCSRAT_NPRIV       (1<<2)	/* Non-Privliged set - CGSR */
+#define TSI148_LCSR_GCSRAT_PGM         (1<<1)	/* Program set - GCSR decoder */
+#define TSI148_LCSR_GCSRAT_DATA        (1<<0)	/* DATA set GCSR decoder */
+
+/*
+ *  CRG Base Address Lower Address  CRG + $410
+ */
+#define TSI148_LCSR_CBAL_M             (0xFFFFF<<12)
+
+/*
+ *  CRG Attribute Register  CRG + $414
+ */
+#define TSI148_LCSR_CRGAT_EN           (1<<7)	/* Enable PRG Access */
+
+#define TSI148_LCSR_CRGAT_AS_M         (7<<4)	/* Address Space */
+#define TSI148_LCSR_CRGAT_AS_A16       (0<<4)	/* Address Space 16 */
+#define TSI148_LCSR_CRGAT_AS_A24       (1<<4)	/* Address Space 24 */
+#define TSI148_LCSR_CRGAT_AS_A32       (2<<4)	/* Address Space 32 */
+#define TSI148_LCSR_CRGAT_AS_A64       (4<<4)	/* Address Space 64 */
+
+#define TSI148_LCSR_CRGAT_SUPR         (1<<3)	/* Supervisor Access */
+#define TSI148_LCSR_CRGAT_NPRIV        (1<<2)	/* Non-Privliged(User) Access */
+#define TSI148_LCSR_CRGAT_PGM          (1<<1)	/* Program Access */
+#define TSI148_LCSR_CRGAT_DATA         (1<<0)	/* Data Access */
+
+/*
+ *  CR/CSR Offset Lower Register  CRG + $41C
+ */
+#define TSI148_LCSR_CROL_M             (0x1FFF<<19)	/* Mask */
+
+/*
+ *  CR/CSR Attribute register  CRG + $420
+ */
+#define TSI148_LCSR_CRAT_EN            (1<<7)	/* Enable access to CR/CSR */
+
+/*
+ *  Location Monitor base address lower register  CRG + $428
+ */
+#define TSI148_LCSR_LMBAL_M            (0x7FFFFFF<<5)	/* Mask */
+
+/*
+ *  Location Monitor Attribute Register  CRG + $42C
+ */
+#define TSI148_LCSR_LMAT_EN            (1<<7)	/* Enable Location Monitor */
+
+#define TSI148_LCSR_LMAT_AS_M          (7<<4)	/* Address Space MASK  */
+#define TSI148_LCSR_LMAT_AS_A16        (0<<4)	/* A16 */
+#define TSI148_LCSR_LMAT_AS_A24        (1<<4)	/* A24 */
+#define TSI148_LCSR_LMAT_AS_A32        (2<<4)	/* A32 */
+#define TSI148_LCSR_LMAT_AS_A64        (4<<4)	/* A64 */
+
+#define TSI148_LCSR_LMAT_SUPR          (1<<3)	/* Supervisor Access */
+#define TSI148_LCSR_LMAT_NPRIV         (1<<2)	/* Non-Priv (User) Access */
+#define TSI148_LCSR_LMAT_PGM           (1<<1)	/* Program Access */
+#define TSI148_LCSR_LMAT_DATA          (1<<0)	/* Data Access  */
+
+/*
+ *  Broadcast Pulse Generator Timer Register  CRG + $438
+ */
+#define TSI148_LCSR_BPGTR_BPGT_M       (0xFFFF<<0)	/* Mask */
+
+/*
+ *  Broadcast Programmable Clock Timer Register  CRG + $43C
+ */
+#define TSI148_LCSR_BPCTR_BPCT_M       (0xFFFFFF<<0)	/* Mask */
+
+/*
+ *  VMEbus Interrupt Control Register           CRG + $43C
+ */
+#define TSI148_LCSR_VICR_CNTS_M        (3<<22)	/* Cntr Source MASK */
+#define TSI148_LCSR_VICR_CNTS_DIS      (1<<22)	/* Cntr Disable */
+#define TSI148_LCSR_VICR_CNTS_IRQ1     (2<<22)	/* IRQ1 to Cntr */
+#define TSI148_LCSR_VICR_CNTS_IRQ2     (3<<22)	/* IRQ2 to Cntr */
+
+#define TSI148_LCSR_VICR_EDGIS_M       (3<<20)	/* Edge interrupt MASK */
+#define TSI148_LCSR_VICR_EDGIS_DIS     (1<<20)	/* Edge interrupt Disable */
+#define TSI148_LCSR_VICR_EDGIS_IRQ1    (2<<20)	/* IRQ1 to Edge */
+#define TSI148_LCSR_VICR_EDGIS_IRQ2    (3<<20)	/* IRQ2 to Edge */
+
+#define TSI148_LCSR_VICR_IRQIF_M       (3<<18)	/* IRQ1* Function MASK */
+#define TSI148_LCSR_VICR_IRQIF_NORM    (1<<18)	/* Normal */
+#define TSI148_LCSR_VICR_IRQIF_PULSE   (2<<18)	/* Pulse Generator */
+#define TSI148_LCSR_VICR_IRQIF_PROG    (3<<18)	/* Programmable Clock */
+#define TSI148_LCSR_VICR_IRQIF_1U      (4<<18)	/* 1us Clock */
+
+#define TSI148_LCSR_VICR_IRQ2F_M       (3<<16)	/* IRQ2* Function MASK */
+#define TSI148_LCSR_VICR_IRQ2F_NORM    (1<<16)	/* Normal */
+#define TSI148_LCSR_VICR_IRQ2F_PULSE   (2<<16)	/* Pulse Generator */
+#define TSI148_LCSR_VICR_IRQ2F_PROG    (3<<16)	/* Programmable Clock */
+#define TSI148_LCSR_VICR_IRQ2F_1U      (4<<16)	/* 1us Clock */
+
+#define TSI148_LCSR_VICR_BIP           (1<<15)	/* Broadcast Interrupt Pulse */
+
+#define TSI148_LCSR_VICR_IRQC          (1<<12)	/* VMEbus IRQ Clear */
+#define TSI148_LCSR_VICR_IRQS          (1<<11)	/* VMEbus IRQ Status */
+
+#define TSI148_LCSR_VICR_IRQL_M        (7<<8)	/* VMEbus SW IRQ Level Mask */
+#define TSI148_LCSR_VICR_IRQL_1        (1<<8)	/* VMEbus SW IRQ Level 1 */
+#define TSI148_LCSR_VICR_IRQL_2        (2<<8)	/* VMEbus SW IRQ Level 2 */
+#define TSI148_LCSR_VICR_IRQL_3        (3<<8)	/* VMEbus SW IRQ Level 3 */
+#define TSI148_LCSR_VICR_IRQL_4        (4<<8)	/* VMEbus SW IRQ Level 4 */
+#define TSI148_LCSR_VICR_IRQL_5        (5<<8)	/* VMEbus SW IRQ Level 5 */
+#define TSI148_LCSR_VICR_IRQL_6        (6<<8)	/* VMEbus SW IRQ Level 6 */
+#define TSI148_LCSR_VICR_IRQL_7        (7<<8)	/* VMEbus SW IRQ Level 7 */
+
+static const int TSI148_LCSR_VICR_IRQL[8] = { 0, TSI148_LCSR_VICR_IRQL_1,
+			TSI148_LCSR_VICR_IRQL_2, TSI148_LCSR_VICR_IRQL_3,
+			TSI148_LCSR_VICR_IRQL_4, TSI148_LCSR_VICR_IRQL_5,
+			TSI148_LCSR_VICR_IRQL_6, TSI148_LCSR_VICR_IRQL_7 };
+
+#define TSI148_LCSR_VICR_STID_M        (0xFF<<0)	/* Status/ID Mask */
+
+/*
+ *  Interrupt Enable Register   CRG + $440
+ */
+#define TSI148_LCSR_INTEN_DMA1EN       (1<<25)	/* DMAC 1 */
+#define TSI148_LCSR_INTEN_DMA0EN       (1<<24)	/* DMAC 0 */
+#define TSI148_LCSR_INTEN_LM3EN        (1<<23)	/* Location Monitor 3 */
+#define TSI148_LCSR_INTEN_LM2EN        (1<<22)	/* Location Monitor 2 */
+#define TSI148_LCSR_INTEN_LM1EN        (1<<21)	/* Location Monitor 1 */
+#define TSI148_LCSR_INTEN_LM0EN        (1<<20)	/* Location Monitor 0 */
+#define TSI148_LCSR_INTEN_MB3EN        (1<<19)	/* Mail Box 3 */
+#define TSI148_LCSR_INTEN_MB2EN        (1<<18)	/* Mail Box 2 */
+#define TSI148_LCSR_INTEN_MB1EN        (1<<17)	/* Mail Box 1 */
+#define TSI148_LCSR_INTEN_MB0EN        (1<<16)	/* Mail Box 0 */
+#define TSI148_LCSR_INTEN_PERREN       (1<<13)	/* PCI/X Error */
+#define TSI148_LCSR_INTEN_VERREN       (1<<12)	/* VMEbus Error */
+#define TSI148_LCSR_INTEN_VIEEN        (1<<11)	/* VMEbus IRQ Edge */
+#define TSI148_LCSR_INTEN_IACKEN       (1<<10)	/* IACK */
+#define TSI148_LCSR_INTEN_SYSFLEN      (1<<9)	/* System Fail */
+#define TSI148_LCSR_INTEN_ACFLEN       (1<<8)	/* AC Fail */
+#define TSI148_LCSR_INTEN_IRQ7EN       (1<<7)	/* IRQ7 */
+#define TSI148_LCSR_INTEN_IRQ6EN       (1<<6)	/* IRQ6 */
+#define TSI148_LCSR_INTEN_IRQ5EN       (1<<5)	/* IRQ5 */
+#define TSI148_LCSR_INTEN_IRQ4EN       (1<<4)	/* IRQ4 */
+#define TSI148_LCSR_INTEN_IRQ3EN       (1<<3)	/* IRQ3 */
+#define TSI148_LCSR_INTEN_IRQ2EN       (1<<2)	/* IRQ2 */
+#define TSI148_LCSR_INTEN_IRQ1EN       (1<<1)	/* IRQ1 */
+
+static const int TSI148_LCSR_INTEN_LMEN[4] = { TSI148_LCSR_INTEN_LM0EN,
+					TSI148_LCSR_INTEN_LM1EN,
+					TSI148_LCSR_INTEN_LM2EN,
+					TSI148_LCSR_INTEN_LM3EN };
+
+static const int TSI148_LCSR_INTEN_IRQEN[7] = { TSI148_LCSR_INTEN_IRQ1EN,
+					TSI148_LCSR_INTEN_IRQ2EN,
+					TSI148_LCSR_INTEN_IRQ3EN,
+					TSI148_LCSR_INTEN_IRQ4EN,
+					TSI148_LCSR_INTEN_IRQ5EN,
+					TSI148_LCSR_INTEN_IRQ6EN,
+					TSI148_LCSR_INTEN_IRQ7EN };
+
+/*
+ *  Interrupt Enable Out Register CRG + $444
+ */
+#define TSI148_LCSR_INTEO_DMA1EO       (1<<25)	/* DMAC 1 */
+#define TSI148_LCSR_INTEO_DMA0EO       (1<<24)	/* DMAC 0 */
+#define TSI148_LCSR_INTEO_LM3EO        (1<<23)	/* Loc Monitor 3 */
+#define TSI148_LCSR_INTEO_LM2EO        (1<<22)	/* Loc Monitor 2 */
+#define TSI148_LCSR_INTEO_LM1EO        (1<<21)	/* Loc Monitor 1 */
+#define TSI148_LCSR_INTEO_LM0EO        (1<<20)	/* Location Monitor 0 */
+#define TSI148_LCSR_INTEO_MB3EO        (1<<19)	/* Mail Box 3 */
+#define TSI148_LCSR_INTEO_MB2EO        (1<<18)	/* Mail Box 2 */
+#define TSI148_LCSR_INTEO_MB1EO        (1<<17)	/* Mail Box 1 */
+#define TSI148_LCSR_INTEO_MB0EO        (1<<16)	/* Mail Box 0 */
+#define TSI148_LCSR_INTEO_PERREO       (1<<13)	/* PCI/X Error */
+#define TSI148_LCSR_INTEO_VERREO       (1<<12)	/* VMEbus Error */
+#define TSI148_LCSR_INTEO_VIEEO        (1<<11)	/* VMEbus IRQ Edge */
+#define TSI148_LCSR_INTEO_IACKEO       (1<<10)	/* IACK */
+#define TSI148_LCSR_INTEO_SYSFLEO      (1<<9)	/* System Fail */
+#define TSI148_LCSR_INTEO_ACFLEO       (1<<8)	/* AC Fail */
+#define TSI148_LCSR_INTEO_IRQ7EO       (1<<7)	/* IRQ7 */
+#define TSI148_LCSR_INTEO_IRQ6EO       (1<<6)	/* IRQ6 */
+#define TSI148_LCSR_INTEO_IRQ5EO       (1<<5)	/* IRQ5 */
+#define TSI148_LCSR_INTEO_IRQ4EO       (1<<4)	/* IRQ4 */
+#define TSI148_LCSR_INTEO_IRQ3EO       (1<<3)	/* IRQ3 */
+#define TSI148_LCSR_INTEO_IRQ2EO       (1<<2)	/* IRQ2 */
+#define TSI148_LCSR_INTEO_IRQ1EO       (1<<1)	/* IRQ1 */
+
+static const int TSI148_LCSR_INTEO_LMEO[4] = { TSI148_LCSR_INTEO_LM0EO,
+					TSI148_LCSR_INTEO_LM1EO,
+					TSI148_LCSR_INTEO_LM2EO,
+					TSI148_LCSR_INTEO_LM3EO };
+
+static const int TSI148_LCSR_INTEO_IRQEO[7] = { TSI148_LCSR_INTEO_IRQ1EO,
+					TSI148_LCSR_INTEO_IRQ2EO,
+					TSI148_LCSR_INTEO_IRQ3EO,
+					TSI148_LCSR_INTEO_IRQ4EO,
+					TSI148_LCSR_INTEO_IRQ5EO,
+					TSI148_LCSR_INTEO_IRQ6EO,
+					TSI148_LCSR_INTEO_IRQ7EO };
+
+/*
+ *  Interrupt Status Register CRG + $448
+ */
+#define TSI148_LCSR_INTS_DMA1S         (1<<25)	/* DMA 1 */
+#define TSI148_LCSR_INTS_DMA0S         (1<<24)	/* DMA 0 */
+#define TSI148_LCSR_INTS_LM3S          (1<<23)	/* Location Monitor 3 */
+#define TSI148_LCSR_INTS_LM2S          (1<<22)	/* Location Monitor 2 */
+#define TSI148_LCSR_INTS_LM1S          (1<<21)	/* Location Monitor 1 */
+#define TSI148_LCSR_INTS_LM0S          (1<<20)	/* Location Monitor 0 */
+#define TSI148_LCSR_INTS_MB3S          (1<<19)	/* Mail Box 3 */
+#define TSI148_LCSR_INTS_MB2S          (1<<18)	/* Mail Box 2 */
+#define TSI148_LCSR_INTS_MB1S          (1<<17)	/* Mail Box 1 */
+#define TSI148_LCSR_INTS_MB0S          (1<<16)	/* Mail Box 0 */
+#define TSI148_LCSR_INTS_PERRS         (1<<13)	/* PCI/X Error */
+#define TSI148_LCSR_INTS_VERRS         (1<<12)	/* VMEbus Error */
+#define TSI148_LCSR_INTS_VIES          (1<<11)	/* VMEbus IRQ Edge */
+#define TSI148_LCSR_INTS_IACKS         (1<<10)	/* IACK */
+#define TSI148_LCSR_INTS_SYSFLS        (1<<9)	/* System Fail */
+#define TSI148_LCSR_INTS_ACFLS         (1<<8)	/* AC Fail */
+#define TSI148_LCSR_INTS_IRQ7S         (1<<7)	/* IRQ7 */
+#define TSI148_LCSR_INTS_IRQ6S         (1<<6)	/* IRQ6 */
+#define TSI148_LCSR_INTS_IRQ5S         (1<<5)	/* IRQ5 */
+#define TSI148_LCSR_INTS_IRQ4S         (1<<4)	/* IRQ4 */
+#define TSI148_LCSR_INTS_IRQ3S         (1<<3)	/* IRQ3 */
+#define TSI148_LCSR_INTS_IRQ2S         (1<<2)	/* IRQ2 */
+#define TSI148_LCSR_INTS_IRQ1S         (1<<1)	/* IRQ1 */
+
+static const int TSI148_LCSR_INTS_LMS[4] = { TSI148_LCSR_INTS_LM0S,
+					TSI148_LCSR_INTS_LM1S,
+					TSI148_LCSR_INTS_LM2S,
+					TSI148_LCSR_INTS_LM3S };
+
+static const int TSI148_LCSR_INTS_MBS[4] = { TSI148_LCSR_INTS_MB0S,
+					TSI148_LCSR_INTS_MB1S,
+					TSI148_LCSR_INTS_MB2S,
+					TSI148_LCSR_INTS_MB3S };
+
+/*
+ *  Interrupt Clear Register CRG + $44C
+ */
+#define TSI148_LCSR_INTC_DMA1C         (1<<25)	/* DMA 1 */
+#define TSI148_LCSR_INTC_DMA0C         (1<<24)	/* DMA 0 */
+#define TSI148_LCSR_INTC_LM3C          (1<<23)	/* Location Monitor 3 */
+#define TSI148_LCSR_INTC_LM2C          (1<<22)	/* Location Monitor 2 */
+#define TSI148_LCSR_INTC_LM1C          (1<<21)	/* Location Monitor 1 */
+#define TSI148_LCSR_INTC_LM0C          (1<<20)	/* Location Monitor 0 */
+#define TSI148_LCSR_INTC_MB3C          (1<<19)	/* Mail Box 3 */
+#define TSI148_LCSR_INTC_MB2C          (1<<18)	/* Mail Box 2 */
+#define TSI148_LCSR_INTC_MB1C          (1<<17)	/* Mail Box 1 */
+#define TSI148_LCSR_INTC_MB0C          (1<<16)	/* Mail Box 0 */
+#define TSI148_LCSR_INTC_PERRC         (1<<13)	/* VMEbus Error */
+#define TSI148_LCSR_INTC_VERRC         (1<<12)	/* VMEbus Access Time-out */
+#define TSI148_LCSR_INTC_VIEC          (1<<11)	/* VMEbus IRQ Edge */
+#define TSI148_LCSR_INTC_IACKC         (1<<10)	/* IACK */
+#define TSI148_LCSR_INTC_SYSFLC        (1<<9)	/* System Fail */
+#define TSI148_LCSR_INTC_ACFLC         (1<<8)	/* AC Fail */
+
+static const int TSI148_LCSR_INTC_LMC[4] = { TSI148_LCSR_INTC_LM0C,
+					TSI148_LCSR_INTC_LM1C,
+					TSI148_LCSR_INTC_LM2C,
+					TSI148_LCSR_INTC_LM3C };
+
+static const int TSI148_LCSR_INTC_MBC[4] = { TSI148_LCSR_INTC_MB0C,
+					TSI148_LCSR_INTC_MB1C,
+					TSI148_LCSR_INTC_MB2C,
+					TSI148_LCSR_INTC_MB3C };
+
+/*
+ *  Interrupt Map Register 1 CRG + $458
+ */
+#define TSI148_LCSR_INTM1_DMA1M_M      (3<<18)	/* DMA 1 */
+#define TSI148_LCSR_INTM1_DMA0M_M      (3<<16)	/* DMA 0 */
+#define TSI148_LCSR_INTM1_LM3M_M       (3<<14)	/* Location Monitor 3 */
+#define TSI148_LCSR_INTM1_LM2M_M       (3<<12)	/* Location Monitor 2 */
+#define TSI148_LCSR_INTM1_LM1M_M       (3<<10)	/* Location Monitor 1 */
+#define TSI148_LCSR_INTM1_LM0M_M       (3<<8)	/* Location Monitor 0 */
+#define TSI148_LCSR_INTM1_MB3M_M       (3<<6)	/* Mail Box 3 */
+#define TSI148_LCSR_INTM1_MB2M_M       (3<<4)	/* Mail Box 2 */
+#define TSI148_LCSR_INTM1_MB1M_M       (3<<2)	/* Mail Box 1 */
+#define TSI148_LCSR_INTM1_MB0M_M       (3<<0)	/* Mail Box 0 */
+
+/*
+ *  Interrupt Map Register 2 CRG + $45C
+ */
+#define TSI148_LCSR_INTM2_PERRM_M      (3<<26)	/* PCI Bus Error */
+#define TSI148_LCSR_INTM2_VERRM_M      (3<<24)	/* VMEbus Error */
+#define TSI148_LCSR_INTM2_VIEM_M       (3<<22)	/* VMEbus IRQ Edge */
+#define TSI148_LCSR_INTM2_IACKM_M      (3<<20)	/* IACK */
+#define TSI148_LCSR_INTM2_SYSFLM_M     (3<<18)	/* System Fail */
+#define TSI148_LCSR_INTM2_ACFLM_M      (3<<16)	/* AC Fail */
+#define TSI148_LCSR_INTM2_IRQ7M_M      (3<<14)	/* IRQ7 */
+#define TSI148_LCSR_INTM2_IRQ6M_M      (3<<12)	/* IRQ6 */
+#define TSI148_LCSR_INTM2_IRQ5M_M      (3<<10)	/* IRQ5 */
+#define TSI148_LCSR_INTM2_IRQ4M_M      (3<<8)	/* IRQ4 */
+#define TSI148_LCSR_INTM2_IRQ3M_M      (3<<6)	/* IRQ3 */
+#define TSI148_LCSR_INTM2_IRQ2M_M      (3<<4)	/* IRQ2 */
+#define TSI148_LCSR_INTM2_IRQ1M_M      (3<<2)	/* IRQ1 */
+
+/*
+ *  DMA Control (0-1) Registers CRG + $500
+ */
+#define TSI148_LCSR_DCTL_ABT           (1<<27)	/* Abort */
+#define TSI148_LCSR_DCTL_PAU           (1<<26)	/* Pause */
+#define TSI148_LCSR_DCTL_DGO           (1<<25)	/* DMA Go */
+
+#define TSI148_LCSR_DCTL_MOD           (1<<23)	/* Mode */
+
+#define TSI148_LCSR_DCTL_VBKS_M        (7<<12)	/* VMEbus block Size MASK */
+#define TSI148_LCSR_DCTL_VBKS_32       (0<<12)	/* VMEbus block Size 32 */
+#define TSI148_LCSR_DCTL_VBKS_64       (1<<12)	/* VMEbus block Size 64 */
+#define TSI148_LCSR_DCTL_VBKS_128      (2<<12)	/* VMEbus block Size 128 */
+#define TSI148_LCSR_DCTL_VBKS_256      (3<<12)	/* VMEbus block Size 256 */
+#define TSI148_LCSR_DCTL_VBKS_512      (4<<12)	/* VMEbus block Size 512 */
+#define TSI148_LCSR_DCTL_VBKS_1024     (5<<12)	/* VMEbus block Size 1024 */
+#define TSI148_LCSR_DCTL_VBKS_2048     (6<<12)	/* VMEbus block Size 2048 */
+#define TSI148_LCSR_DCTL_VBKS_4096     (7<<12)	/* VMEbus block Size 4096 */
+
+#define TSI148_LCSR_DCTL_VBOT_M        (7<<8)	/* VMEbus back-off MASK */
+#define TSI148_LCSR_DCTL_VBOT_0        (0<<8)	/* VMEbus back-off  0us */
+#define TSI148_LCSR_DCTL_VBOT_1        (1<<8)	/* VMEbus back-off 1us */
+#define TSI148_LCSR_DCTL_VBOT_2        (2<<8)	/* VMEbus back-off 2us */
+#define TSI148_LCSR_DCTL_VBOT_4        (3<<8)	/* VMEbus back-off 4us */
+#define TSI148_LCSR_DCTL_VBOT_8        (4<<8)	/* VMEbus back-off 8us */
+#define TSI148_LCSR_DCTL_VBOT_16       (5<<8)	/* VMEbus back-off 16us */
+#define TSI148_LCSR_DCTL_VBOT_32       (6<<8)	/* VMEbus back-off 32us */
+#define TSI148_LCSR_DCTL_VBOT_64       (7<<8)	/* VMEbus back-off 64us */
+
+#define TSI148_LCSR_DCTL_PBKS_M        (7<<4)	/* PCI block size MASK */
+#define TSI148_LCSR_DCTL_PBKS_32       (0<<4)	/* PCI block size 32 bytes */
+#define TSI148_LCSR_DCTL_PBKS_64       (1<<4)	/* PCI block size 64 bytes */
+#define TSI148_LCSR_DCTL_PBKS_128      (2<<4)	/* PCI block size 128 bytes */
+#define TSI148_LCSR_DCTL_PBKS_256      (3<<4)	/* PCI block size 256 bytes */
+#define TSI148_LCSR_DCTL_PBKS_512      (4<<4)	/* PCI block size 512 bytes */
+#define TSI148_LCSR_DCTL_PBKS_1024     (5<<4)	/* PCI block size 1024 bytes */
+#define TSI148_LCSR_DCTL_PBKS_2048     (6<<4)	/* PCI block size 2048 bytes */
+#define TSI148_LCSR_DCTL_PBKS_4096     (7<<4)	/* PCI block size 4096 bytes */
+
+#define TSI148_LCSR_DCTL_PBOT_M        (7<<0)	/* PCI back off MASK */
+#define TSI148_LCSR_DCTL_PBOT_0        (0<<0)	/* PCI back off 0us */
+#define TSI148_LCSR_DCTL_PBOT_1        (1<<0)	/* PCI back off 1us */
+#define TSI148_LCSR_DCTL_PBOT_2        (2<<0)	/* PCI back off 2us */
+#define TSI148_LCSR_DCTL_PBOT_4        (3<<0)	/* PCI back off 3us */
+#define TSI148_LCSR_DCTL_PBOT_8        (4<<0)	/* PCI back off 4us */
+#define TSI148_LCSR_DCTL_PBOT_16       (5<<0)	/* PCI back off 8us */
+#define TSI148_LCSR_DCTL_PBOT_32       (6<<0)	/* PCI back off 16us */
+#define TSI148_LCSR_DCTL_PBOT_64       (7<<0)	/* PCI back off 32us */
+
+/*
+ *  DMA Status Registers (0-1)  CRG + $504
+ */
+#define TSI148_LCSR_DSTA_SMA           (1<<31)	/* PCI Signalled Master Abt */
+#define TSI148_LCSR_DSTA_RTA           (1<<30)	/* PCI Received Target Abt */
+#define TSI148_LCSR_DSTA_MRC           (1<<29)	/* PCI Max Retry Count */
+#define TSI148_LCSR_DSTA_VBE           (1<<28)	/* VMEbus error */
+#define TSI148_LCSR_DSTA_ABT           (1<<27)	/* Abort */
+#define TSI148_LCSR_DSTA_PAU           (1<<26)	/* Pause */
+#define TSI148_LCSR_DSTA_DON           (1<<25)	/* Done */
+#define TSI148_LCSR_DSTA_BSY           (1<<24)	/* Busy */
+
+/*
+ *  DMA Current Link Address Lower (0-1)
+ */
+#define TSI148_LCSR_DCLAL_M            (0x3FFFFFF<<6)	/* Mask */
+
+/*
+ *  DMA Source Attribute (0-1) Reg
+ */
+#define TSI148_LCSR_DSAT_TYP_M         (3<<28)	/* Source Bus Type */
+#define TSI148_LCSR_DSAT_TYP_PCI       (0<<28)	/* PCI Bus */
+#define TSI148_LCSR_DSAT_TYP_VME       (1<<28)	/* VMEbus */
+#define TSI148_LCSR_DSAT_TYP_PAT       (2<<28)	/* Data Pattern */
+
+#define TSI148_LCSR_DSAT_PSZ           (1<<25)	/* Pattern Size */
+#define TSI148_LCSR_DSAT_NIN           (1<<24)	/* No Increment */
+
+#define TSI148_LCSR_DSAT_2eSSTM_M      (3<<11)	/* 2eSST Trans Rate Mask */
+#define TSI148_LCSR_DSAT_2eSSTM_160    (0<<11)	/* 160 MB/s */
+#define TSI148_LCSR_DSAT_2eSSTM_267    (1<<11)	/* 267 MB/s */
+#define TSI148_LCSR_DSAT_2eSSTM_320    (2<<11)	/* 320 MB/s */
+
+#define TSI148_LCSR_DSAT_TM_M          (7<<8)	/* Bus Transfer Protocol Mask */
+#define TSI148_LCSR_DSAT_TM_SCT        (0<<8)	/* SCT */
+#define TSI148_LCSR_DSAT_TM_BLT        (1<<8)	/* BLT */
+#define TSI148_LCSR_DSAT_TM_MBLT       (2<<8)	/* MBLT */
+#define TSI148_LCSR_DSAT_TM_2eVME      (3<<8)	/* 2eVME */
+#define TSI148_LCSR_DSAT_TM_2eSST      (4<<8)	/* 2eSST */
+#define TSI148_LCSR_DSAT_TM_2eSSTB     (5<<8)	/* 2eSST Broadcast */
+
+#define TSI148_LCSR_DSAT_DBW_M         (3<<6)	/* Max Data Width MASK */
+#define TSI148_LCSR_DSAT_DBW_16        (0<<6)	/* 16 Bits */
+#define TSI148_LCSR_DSAT_DBW_32        (1<<6)	/* 32 Bits */
+
+#define TSI148_LCSR_DSAT_SUP           (1<<5)	/* Supervisory Mode */
+#define TSI148_LCSR_DSAT_PGM           (1<<4)	/* Program Mode */
+
+#define TSI148_LCSR_DSAT_AMODE_M       (0xf<<0)	/* Address Space Mask */
+#define TSI148_LCSR_DSAT_AMODE_A16     (0<<0)	/* A16 */
+#define TSI148_LCSR_DSAT_AMODE_A24     (1<<0)	/* A24 */
+#define TSI148_LCSR_DSAT_AMODE_A32     (2<<0)	/* A32 */
+#define TSI148_LCSR_DSAT_AMODE_A64     (4<<0)	/* A64 */
+#define TSI148_LCSR_DSAT_AMODE_CRCSR   (5<<0)	/* CR/CSR */
+#define TSI148_LCSR_DSAT_AMODE_USER1   (8<<0)	/* User1 */
+#define TSI148_LCSR_DSAT_AMODE_USER2   (9<<0)	/* User2 */
+#define TSI148_LCSR_DSAT_AMODE_USER3   (0xa<<0)	/* User3 */
+#define TSI148_LCSR_DSAT_AMODE_USER4   (0xb<<0)	/* User4 */
+
+/*
+ *  DMA Destination Attribute Registers (0-1)
+ */
+#define TSI148_LCSR_DDAT_TYP_PCI       (0<<28)	/* Destination PCI Bus  */
+#define TSI148_LCSR_DDAT_TYP_VME       (1<<28)	/* Destination VMEbus */
+
+#define TSI148_LCSR_DDAT_2eSSTM_M      (3<<11)	/* 2eSST Transfer Rate Mask */
+#define TSI148_LCSR_DDAT_2eSSTM_160    (0<<11)	/* 160 MB/s */
+#define TSI148_LCSR_DDAT_2eSSTM_267    (1<<11)	/* 267 MB/s */
+#define TSI148_LCSR_DDAT_2eSSTM_320    (2<<11)	/* 320 MB/s */
+
+#define TSI148_LCSR_DDAT_TM_M          (7<<8)	/* Bus Transfer Protocol Mask */
+#define TSI148_LCSR_DDAT_TM_SCT        (0<<8)	/* SCT */
+#define TSI148_LCSR_DDAT_TM_BLT        (1<<8)	/* BLT */
+#define TSI148_LCSR_DDAT_TM_MBLT       (2<<8)	/* MBLT */
+#define TSI148_LCSR_DDAT_TM_2eVME      (3<<8)	/* 2eVME */
+#define TSI148_LCSR_DDAT_TM_2eSST      (4<<8)	/* 2eSST */
+#define TSI148_LCSR_DDAT_TM_2eSSTB     (5<<8)	/* 2eSST Broadcast */
+
+#define TSI148_LCSR_DDAT_DBW_M         (3<<6)	/* Max Data Width MASK */
+#define TSI148_LCSR_DDAT_DBW_16        (0<<6)	/* 16 Bits */
+#define TSI148_LCSR_DDAT_DBW_32        (1<<6)	/* 32 Bits */
+
+#define TSI148_LCSR_DDAT_SUP           (1<<5)	/* Supervisory/User Access */
+#define TSI148_LCSR_DDAT_PGM           (1<<4)	/* Program/Data Access */
+
+#define TSI148_LCSR_DDAT_AMODE_M       (0xf<<0)	/* Address Space Mask */
+#define TSI148_LCSR_DDAT_AMODE_A16      (0<<0)	/* A16 */
+#define TSI148_LCSR_DDAT_AMODE_A24      (1<<0)	/* A24 */
+#define TSI148_LCSR_DDAT_AMODE_A32      (2<<0)	/* A32 */
+#define TSI148_LCSR_DDAT_AMODE_A64      (4<<0)	/* A64 */
+#define TSI148_LCSR_DDAT_AMODE_CRCSR   (5<<0)	/* CRC/SR */
+#define TSI148_LCSR_DDAT_AMODE_USER1   (8<<0)	/* User1 */
+#define TSI148_LCSR_DDAT_AMODE_USER2   (9<<0)	/* User2 */
+#define TSI148_LCSR_DDAT_AMODE_USER3   (0xa<<0)	/* User3 */
+#define TSI148_LCSR_DDAT_AMODE_USER4   (0xb<<0)	/* User4 */
+
+/*
+ *  DMA Next Link Address Lower
+ */
+#define TSI148_LCSR_DNLAL_DNLAL_M      (0x3FFFFFF<<6)	/* Address Mask */
+#define TSI148_LCSR_DNLAL_LLA          (1<<0)  /* Last Link Address Indicator */
+
+/*
+ *  DMA 2eSST Broadcast Select
+ */
+#define TSI148_LCSR_DBS_M              (0x1FFFFF<<0)	/* Mask */
+
+/*
+ *  GCSR Register Group
+ */
+
+/*
+ *  GCSR Control and Status Register  CRG + $604
+ */
+#define TSI148_GCSR_GCTRL_LRST         (1<<15)	/* Local Reset */
+#define TSI148_GCSR_GCTRL_SFAILEN      (1<<14)	/* System Fail enable */
+#define TSI148_GCSR_GCTRL_BDFAILS      (1<<13)	/* Board Fail Status */
+#define TSI148_GCSR_GCTRL_SCON         (1<<12)	/* System Copntroller */
+#define TSI148_GCSR_GCTRL_MEN          (1<<11)	/* Module Enable (READY) */
+
+#define TSI148_GCSR_GCTRL_LMI3S        (1<<7)	/* Loc Monitor 3 Int Status */
+#define TSI148_GCSR_GCTRL_LMI2S        (1<<6)	/* Loc Monitor 2 Int Status */
+#define TSI148_GCSR_GCTRL_LMI1S        (1<<5)	/* Loc Monitor 1 Int Status */
+#define TSI148_GCSR_GCTRL_LMI0S        (1<<4)	/* Loc Monitor 0 Int Status */
+#define TSI148_GCSR_GCTRL_MBI3S        (1<<3)	/* Mail box 3 Int Status */
+#define TSI148_GCSR_GCTRL_MBI2S        (1<<2)	/* Mail box 2 Int Status */
+#define TSI148_GCSR_GCTRL_MBI1S        (1<<1)	/* Mail box 1 Int Status */
+#define TSI148_GCSR_GCTRL_MBI0S        (1<<0)	/* Mail box 0 Int Status */
+
+#define TSI148_GCSR_GAP                (1<<5)	/* Geographic Addr Parity */
+#define TSI148_GCSR_GA_M               (0x1F<<0)  /* Geographic Address Mask */
+
+/*
+ *  CR/CSR Register Group
+ */
+
+/*
+ *  CR/CSR Bit Clear Register CRG + $FF4
+ */
+#define TSI148_CRCSR_CSRBCR_LRSTC      (1<<7)	/* Local Reset Clear */
+#define TSI148_CRCSR_CSRBCR_SFAILC     (1<<6)	/* System Fail Enable Clear */
+#define TSI148_CRCSR_CSRBCR_BDFAILS    (1<<5)	/* Board Fail Status */
+#define TSI148_CRCSR_CSRBCR_MENC       (1<<4)	/* Module Enable Clear */
+#define TSI148_CRCSR_CSRBCR_BERRSC     (1<<3)	/* Bus Error Status Clear */
+
+/*
+ *  CR/CSR Bit Set Register CRG+$FF8
+ */
+#define TSI148_CRCSR_CSRBSR_LISTS      (1<<7)	/* Local Reset Clear */
+#define TSI148_CRCSR_CSRBSR_SFAILS     (1<<6)	/* System Fail Enable Clear */
+#define TSI148_CRCSR_CSRBSR_BDFAILS    (1<<5)	/* Board Fail Status */
+#define TSI148_CRCSR_CSRBSR_MENS       (1<<4)	/* Module Enable Clear */
+#define TSI148_CRCSR_CSRBSR_BERRS      (1<<3)	/* Bus Error Status Clear */
+
+/*
+ *  CR/CSR Base Address Register CRG + FFC
+ */
+#define TSI148_CRCSR_CBAR_M            (0x1F<<3)	/* Mask */
+
+#endif				/* TSI148_H */
diff --git a/drivers/staging/vme_user/vme_user.c b/drivers/staging/vme_user/vme_user.c
new file mode 100644
index 000000000..4e533c0bf
--- /dev/null
+++ b/drivers/staging/vme_user/vme_user.c
@@ -0,0 +1,780 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * VMEbus User access driver
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2008 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * Based on work by:
+ *   Tom Armistead and Ajit Prem
+ *     Copyright 2004 Motorola Inc.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/refcount.h>
+#include <linux/cdev.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/ioctl.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/pagemap.h>
+#include <linux/pci.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/syscalls.h>
+#include <linux/types.h>
+
+#include <linux/io.h>
+#include <linux/uaccess.h>
+
+#include "vme.h"
+#include "vme_user.h"
+
+static const char driver_name[] = "vme_user";
+
+static int bus[VME_USER_BUS_MAX];
+static unsigned int bus_num;
+
+/* Currently Documentation/admin-guide/devices.rst defines the
+ * following for VME:
+ *
+ * 221 char	VME bus
+ *		  0 = /dev/bus/vme/m0		First master image
+ *		  1 = /dev/bus/vme/m1		Second master image
+ *		  2 = /dev/bus/vme/m2		Third master image
+ *		  3 = /dev/bus/vme/m3		Fourth master image
+ *		  4 = /dev/bus/vme/s0		First slave image
+ *		  5 = /dev/bus/vme/s1		Second slave image
+ *		  6 = /dev/bus/vme/s2		Third slave image
+ *		  7 = /dev/bus/vme/s3		Fourth slave image
+ *		  8 = /dev/bus/vme/ctl		Control
+ *
+ *		It is expected that all VME bus drivers will use the
+ *		same interface.  For interface documentation see
+ *		http://www.vmelinux.org/.
+ *
+ * However the VME driver at http://www.vmelinux.org/ is rather old and doesn't
+ * even support the tsi148 chipset (which has 8 master and 8 slave windows).
+ * We'll run with this for now as far as possible, however it probably makes
+ * sense to get rid of the old mappings and just do everything dynamically.
+ *
+ * So for now, we'll restrict the driver to providing 4 masters and 4 slaves as
+ * defined above and try to support at least some of the interface from
+ * http://www.vmelinux.org/ as an alternative the driver can be written
+ * providing a saner interface later.
+ *
+ * The vmelinux.org driver never supported slave images, the devices reserved
+ * for slaves were repurposed to support all 8 master images on the UniverseII!
+ * We shall support 4 masters and 4 slaves with this driver.
+ */
+#define VME_MAJOR	221	/* VME Major Device Number */
+#define VME_DEVS	9	/* Number of dev entries */
+
+#define MASTER_MINOR	0
+#define MASTER_MAX	3
+#define SLAVE_MINOR	4
+#define SLAVE_MAX	7
+#define CONTROL_MINOR	8
+
+#define PCI_BUF_SIZE  0x20000	/* Size of one slave image buffer */
+
+/*
+ * Structure to handle image related parameters.
+ */
+struct image_desc {
+	void *kern_buf;	/* Buffer address in kernel space */
+	dma_addr_t pci_buf;	/* Buffer address in PCI address space */
+	unsigned long long size_buf;	/* Buffer size */
+	struct mutex mutex;	/* Mutex for locking image */
+	struct device *device;	/* Sysfs device */
+	struct vme_resource *resource;	/* VME resource */
+	int mmap_count;		/* Number of current mmap's */
+};
+
+static struct image_desc image[VME_DEVS];
+
+static struct cdev *vme_user_cdev;		/* Character device */
+static struct class *vme_user_sysfs_class;	/* Sysfs class */
+static struct vme_dev *vme_user_bridge;		/* Pointer to user device */
+
+static const int type[VME_DEVS] = {	MASTER_MINOR,	MASTER_MINOR,
+					MASTER_MINOR,	MASTER_MINOR,
+					SLAVE_MINOR,	SLAVE_MINOR,
+					SLAVE_MINOR,	SLAVE_MINOR,
+					CONTROL_MINOR
+				};
+
+struct vme_user_vma_priv {
+	unsigned int minor;
+	refcount_t refcnt;
+};
+
+static ssize_t resource_to_user(int minor, char __user *buf, size_t count,
+				loff_t *ppos)
+{
+	ssize_t copied = 0;
+
+	if (count > image[minor].size_buf)
+		count = image[minor].size_buf;
+
+	copied = vme_master_read(image[minor].resource, image[minor].kern_buf,
+				 count, *ppos);
+	if (copied < 0)
+		return (int)copied;
+
+	if (copy_to_user(buf, image[minor].kern_buf, (unsigned long)copied))
+		return -EFAULT;
+
+	return copied;
+}
+
+static ssize_t resource_from_user(unsigned int minor, const char __user *buf,
+				  size_t count, loff_t *ppos)
+{
+	if (count > image[minor].size_buf)
+		count = image[minor].size_buf;
+
+	if (copy_from_user(image[minor].kern_buf, buf, (unsigned long)count))
+		return -EFAULT;
+
+	return vme_master_write(image[minor].resource, image[minor].kern_buf,
+				count, *ppos);
+}
+
+static ssize_t buffer_to_user(unsigned int minor, char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	void *image_ptr;
+
+	image_ptr = image[minor].kern_buf + *ppos;
+	if (copy_to_user(buf, image_ptr, (unsigned long)count))
+		return -EFAULT;
+
+	return count;
+}
+
+static ssize_t buffer_from_user(unsigned int minor, const char __user *buf,
+				size_t count, loff_t *ppos)
+{
+	void *image_ptr;
+
+	image_ptr = image[minor].kern_buf + *ppos;
+	if (copy_from_user(image_ptr, buf, (unsigned long)count))
+		return -EFAULT;
+
+	return count;
+}
+
+static ssize_t vme_user_read(struct file *file, char __user *buf, size_t count,
+			     loff_t *ppos)
+{
+	unsigned int minor = iminor(file_inode(file));
+	ssize_t retval;
+	size_t image_size;
+
+	if (minor == CONTROL_MINOR)
+		return 0;
+
+	mutex_lock(&image[minor].mutex);
+
+	/* XXX Do we *really* want this helper - we can use vme_*_get ? */
+	image_size = vme_get_size(image[minor].resource);
+
+	/* Ensure we are starting at a valid location */
+	if ((*ppos < 0) || (*ppos > (image_size - 1))) {
+		mutex_unlock(&image[minor].mutex);
+		return 0;
+	}
+
+	/* Ensure not reading past end of the image */
+	if (*ppos + count > image_size)
+		count = image_size - *ppos;
+
+	switch (type[minor]) {
+	case MASTER_MINOR:
+		retval = resource_to_user(minor, buf, count, ppos);
+		break;
+	case SLAVE_MINOR:
+		retval = buffer_to_user(minor, buf, count, ppos);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	mutex_unlock(&image[minor].mutex);
+	if (retval > 0)
+		*ppos += retval;
+
+	return retval;
+}
+
+static ssize_t vme_user_write(struct file *file, const char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	unsigned int minor = iminor(file_inode(file));
+	ssize_t retval;
+	size_t image_size;
+
+	if (minor == CONTROL_MINOR)
+		return 0;
+
+	mutex_lock(&image[minor].mutex);
+
+	image_size = vme_get_size(image[minor].resource);
+
+	/* Ensure we are starting at a valid location */
+	if ((*ppos < 0) || (*ppos > (image_size - 1))) {
+		mutex_unlock(&image[minor].mutex);
+		return 0;
+	}
+
+	/* Ensure not reading past end of the image */
+	if (*ppos + count > image_size)
+		count = image_size - *ppos;
+
+	switch (type[minor]) {
+	case MASTER_MINOR:
+		retval = resource_from_user(minor, buf, count, ppos);
+		break;
+	case SLAVE_MINOR:
+		retval = buffer_from_user(minor, buf, count, ppos);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	mutex_unlock(&image[minor].mutex);
+
+	if (retval > 0)
+		*ppos += retval;
+
+	return retval;
+}
+
+static loff_t vme_user_llseek(struct file *file, loff_t off, int whence)
+{
+	unsigned int minor = iminor(file_inode(file));
+	size_t image_size;
+	loff_t res;
+
+	switch (type[minor]) {
+	case MASTER_MINOR:
+	case SLAVE_MINOR:
+		mutex_lock(&image[minor].mutex);
+		image_size = vme_get_size(image[minor].resource);
+		res = fixed_size_llseek(file, off, whence, image_size);
+		mutex_unlock(&image[minor].mutex);
+		return res;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * The ioctls provided by the old VME access method (the one at vmelinux.org)
+ * are most certainly wrong as the effectively push the registers layout
+ * through to user space. Given that the VME core can handle multiple bridges,
+ * with different register layouts this is most certainly not the way to go.
+ *
+ * We aren't using the structures defined in the Motorola driver either - these
+ * are also quite low level, however we should use the definitions that have
+ * already been defined.
+ */
+static int vme_user_ioctl(struct inode *inode, struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	struct vme_master master;
+	struct vme_slave slave;
+	struct vme_irq_id irq_req;
+	unsigned long copied;
+	unsigned int minor = iminor(inode);
+	int retval;
+	dma_addr_t pci_addr;
+	void __user *argp = (void __user *)arg;
+
+	switch (type[minor]) {
+	case CONTROL_MINOR:
+		switch (cmd) {
+		case VME_IRQ_GEN:
+			copied = copy_from_user(&irq_req, argp,
+						sizeof(irq_req));
+			if (copied) {
+				pr_warn("Partial copy from userspace\n");
+				return -EFAULT;
+			}
+
+			return vme_irq_generate(vme_user_bridge,
+						  irq_req.level,
+						  irq_req.statid);
+		}
+		break;
+	case MASTER_MINOR:
+		switch (cmd) {
+		case VME_GET_MASTER:
+			memset(&master, 0, sizeof(master));
+
+			/* XXX	We do not want to push aspace, cycle and width
+			 *	to userspace as they are
+			 */
+			retval = vme_master_get(image[minor].resource,
+						&master.enable,
+						&master.vme_addr,
+						&master.size, &master.aspace,
+						&master.cycle, &master.dwidth);
+
+			copied = copy_to_user(argp, &master,
+					      sizeof(master));
+			if (copied) {
+				pr_warn("Partial copy to userspace\n");
+				return -EFAULT;
+			}
+
+			return retval;
+
+		case VME_SET_MASTER:
+
+			if (image[minor].mmap_count != 0) {
+				pr_warn("Can't adjust mapped window\n");
+				return -EPERM;
+			}
+
+			copied = copy_from_user(&master, argp, sizeof(master));
+			if (copied) {
+				pr_warn("Partial copy from userspace\n");
+				return -EFAULT;
+			}
+
+			/* XXX	We do not want to push aspace, cycle and width
+			 *	to userspace as they are
+			 */
+			return vme_master_set(image[minor].resource,
+				master.enable, master.vme_addr, master.size,
+				master.aspace, master.cycle, master.dwidth);
+
+			break;
+		}
+		break;
+	case SLAVE_MINOR:
+		switch (cmd) {
+		case VME_GET_SLAVE:
+			memset(&slave, 0, sizeof(slave));
+
+			/* XXX	We do not want to push aspace, cycle and width
+			 *	to userspace as they are
+			 */
+			retval = vme_slave_get(image[minor].resource,
+					       &slave.enable, &slave.vme_addr,
+					       &slave.size, &pci_addr,
+					       &slave.aspace, &slave.cycle);
+
+			copied = copy_to_user(argp, &slave,
+					      sizeof(slave));
+			if (copied) {
+				pr_warn("Partial copy to userspace\n");
+				return -EFAULT;
+			}
+
+			return retval;
+
+		case VME_SET_SLAVE:
+
+			copied = copy_from_user(&slave, argp, sizeof(slave));
+			if (copied) {
+				pr_warn("Partial copy from userspace\n");
+				return -EFAULT;
+			}
+
+			/* XXX	We do not want to push aspace, cycle and width
+			 *	to userspace as they are
+			 */
+			return vme_slave_set(image[minor].resource,
+				slave.enable, slave.vme_addr, slave.size,
+				image[minor].pci_buf, slave.aspace,
+				slave.cycle);
+
+			break;
+		}
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static long
+vme_user_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int ret;
+	struct inode *inode = file_inode(file);
+	unsigned int minor = iminor(inode);
+
+	mutex_lock(&image[minor].mutex);
+	ret = vme_user_ioctl(inode, file, cmd, arg);
+	mutex_unlock(&image[minor].mutex);
+
+	return ret;
+}
+
+static void vme_user_vm_open(struct vm_area_struct *vma)
+{
+	struct vme_user_vma_priv *vma_priv = vma->vm_private_data;
+
+	refcount_inc(&vma_priv->refcnt);
+}
+
+static void vme_user_vm_close(struct vm_area_struct *vma)
+{
+	struct vme_user_vma_priv *vma_priv = vma->vm_private_data;
+	unsigned int minor = vma_priv->minor;
+
+	if (!refcount_dec_and_test(&vma_priv->refcnt))
+		return;
+
+	mutex_lock(&image[minor].mutex);
+	image[minor].mmap_count--;
+	mutex_unlock(&image[minor].mutex);
+
+	kfree(vma_priv);
+}
+
+static const struct vm_operations_struct vme_user_vm_ops = {
+	.open = vme_user_vm_open,
+	.close = vme_user_vm_close,
+};
+
+static int vme_user_master_mmap(unsigned int minor, struct vm_area_struct *vma)
+{
+	int err;
+	struct vme_user_vma_priv *vma_priv;
+
+	mutex_lock(&image[minor].mutex);
+
+	err = vme_master_mmap(image[minor].resource, vma);
+	if (err) {
+		mutex_unlock(&image[minor].mutex);
+		return err;
+	}
+
+	vma_priv = kmalloc(sizeof(*vma_priv), GFP_KERNEL);
+	if (!vma_priv) {
+		mutex_unlock(&image[minor].mutex);
+		return -ENOMEM;
+	}
+
+	vma_priv->minor = minor;
+	refcount_set(&vma_priv->refcnt, 1);
+	vma->vm_ops = &vme_user_vm_ops;
+	vma->vm_private_data = vma_priv;
+
+	image[minor].mmap_count++;
+
+	mutex_unlock(&image[minor].mutex);
+
+	return 0;
+}
+
+static int vme_user_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned int minor = iminor(file_inode(file));
+
+	if (type[minor] == MASTER_MINOR)
+		return vme_user_master_mmap(minor, vma);
+
+	return -ENODEV;
+}
+
+static const struct file_operations vme_user_fops = {
+	.read = vme_user_read,
+	.write = vme_user_write,
+	.llseek = vme_user_llseek,
+	.unlocked_ioctl = vme_user_unlocked_ioctl,
+	.compat_ioctl = compat_ptr_ioctl,
+	.mmap = vme_user_mmap,
+};
+
+static int vme_user_match(struct vme_dev *vdev)
+{
+	int i;
+
+	int cur_bus = vme_bus_num(vdev);
+	int cur_slot = vme_slot_num(vdev);
+
+	for (i = 0; i < bus_num; i++)
+		if ((cur_bus == bus[i]) && (cur_slot == vdev->num))
+			return 1;
+
+	return 0;
+}
+
+/*
+ * In this simple access driver, the old behaviour is being preserved as much
+ * as practical. We will therefore reserve the buffers and request the images
+ * here so that we don't have to do it later.
+ */
+static int vme_user_probe(struct vme_dev *vdev)
+{
+	int i, err;
+	char *name;
+
+	/* Save pointer to the bridge device */
+	if (vme_user_bridge) {
+		dev_err(&vdev->dev, "Driver can only be loaded for 1 device\n");
+		err = -EINVAL;
+		goto err_dev;
+	}
+	vme_user_bridge = vdev;
+
+	/* Initialise descriptors */
+	for (i = 0; i < VME_DEVS; i++) {
+		image[i].kern_buf = NULL;
+		image[i].pci_buf = 0;
+		mutex_init(&image[i].mutex);
+		image[i].device = NULL;
+		image[i].resource = NULL;
+	}
+
+	/* Assign major and minor numbers for the driver */
+	err = register_chrdev_region(MKDEV(VME_MAJOR, 0), VME_DEVS,
+				     driver_name);
+	if (err) {
+		dev_warn(&vdev->dev, "Error getting Major Number %d for driver.\n",
+			 VME_MAJOR);
+		goto err_region;
+	}
+
+	/* Register the driver as a char device */
+	vme_user_cdev = cdev_alloc();
+	if (!vme_user_cdev) {
+		err = -ENOMEM;
+		goto err_char;
+	}
+	vme_user_cdev->ops = &vme_user_fops;
+	vme_user_cdev->owner = THIS_MODULE;
+	err = cdev_add(vme_user_cdev, MKDEV(VME_MAJOR, 0), VME_DEVS);
+	if (err)
+		goto err_class;
+
+	/* Request slave resources and allocate buffers (128kB wide) */
+	for (i = SLAVE_MINOR; i < (SLAVE_MAX + 1); i++) {
+		/* XXX Need to properly request attributes */
+		/* For ca91cx42 bridge there are only two slave windows
+		 * supporting A16 addressing, so we request A24 supported
+		 * by all windows.
+		 */
+		image[i].resource = vme_slave_request(vme_user_bridge,
+						      VME_A24, VME_SCT);
+		if (!image[i].resource) {
+			dev_warn(&vdev->dev,
+				 "Unable to allocate slave resource\n");
+			err = -ENOMEM;
+			goto err_slave;
+		}
+		image[i].size_buf = PCI_BUF_SIZE;
+		image[i].kern_buf = vme_alloc_consistent(image[i].resource,
+							 image[i].size_buf,
+							 &image[i].pci_buf);
+		if (!image[i].kern_buf) {
+			dev_warn(&vdev->dev,
+				 "Unable to allocate memory for buffer\n");
+			image[i].pci_buf = 0;
+			vme_slave_free(image[i].resource);
+			err = -ENOMEM;
+			goto err_slave;
+		}
+	}
+
+	/*
+	 * Request master resources allocate page sized buffers for small
+	 * reads and writes
+	 */
+	for (i = MASTER_MINOR; i < (MASTER_MAX + 1); i++) {
+		/* XXX Need to properly request attributes */
+		image[i].resource = vme_master_request(vme_user_bridge,
+						       VME_A32, VME_SCT,
+						       VME_D32);
+		if (!image[i].resource) {
+			dev_warn(&vdev->dev,
+				 "Unable to allocate master resource\n");
+			err = -ENOMEM;
+			goto err_master;
+		}
+		image[i].size_buf = PCI_BUF_SIZE;
+		image[i].kern_buf = kmalloc(image[i].size_buf, GFP_KERNEL);
+		if (!image[i].kern_buf) {
+			err = -ENOMEM;
+			vme_master_free(image[i].resource);
+			goto err_master;
+		}
+	}
+
+	/* Create sysfs entries - on udev systems this creates the dev files */
+	vme_user_sysfs_class = class_create(THIS_MODULE, driver_name);
+	if (IS_ERR(vme_user_sysfs_class)) {
+		dev_err(&vdev->dev, "Error creating vme_user class.\n");
+		err = PTR_ERR(vme_user_sysfs_class);
+		goto err_master;
+	}
+
+	/* Add sysfs Entries */
+	for (i = 0; i < VME_DEVS; i++) {
+		int num;
+
+		switch (type[i]) {
+		case MASTER_MINOR:
+			name = "bus/vme/m%d";
+			break;
+		case CONTROL_MINOR:
+			name = "bus/vme/ctl";
+			break;
+		case SLAVE_MINOR:
+			name = "bus/vme/s%d";
+			break;
+		default:
+			err = -EINVAL;
+			goto err_sysfs;
+		}
+
+		num = (type[i] == SLAVE_MINOR) ? i - (MASTER_MAX + 1) : i;
+		image[i].device = device_create(vme_user_sysfs_class, NULL,
+						MKDEV(VME_MAJOR, i), NULL,
+						name, num);
+		if (IS_ERR(image[i].device)) {
+			dev_info(&vdev->dev, "Error creating sysfs device\n");
+			err = PTR_ERR(image[i].device);
+			goto err_sysfs;
+		}
+	}
+
+	return 0;
+
+err_sysfs:
+	while (i > 0) {
+		i--;
+		device_destroy(vme_user_sysfs_class, MKDEV(VME_MAJOR, i));
+	}
+	class_destroy(vme_user_sysfs_class);
+
+	/* Ensure counter set correctly to unalloc all master windows */
+	i = MASTER_MAX + 1;
+err_master:
+	while (i > MASTER_MINOR) {
+		i--;
+		kfree(image[i].kern_buf);
+		vme_master_free(image[i].resource);
+	}
+
+	/*
+	 * Ensure counter set correctly to unalloc all slave windows and buffers
+	 */
+	i = SLAVE_MAX + 1;
+err_slave:
+	while (i > SLAVE_MINOR) {
+		i--;
+		vme_free_consistent(image[i].resource, image[i].size_buf,
+				    image[i].kern_buf, image[i].pci_buf);
+		vme_slave_free(image[i].resource);
+	}
+err_class:
+	cdev_del(vme_user_cdev);
+err_char:
+	unregister_chrdev_region(MKDEV(VME_MAJOR, 0), VME_DEVS);
+err_region:
+err_dev:
+	return err;
+}
+
+static void vme_user_remove(struct vme_dev *dev)
+{
+	int i;
+
+	/* Remove sysfs Entries */
+	for (i = 0; i < VME_DEVS; i++) {
+		mutex_destroy(&image[i].mutex);
+		device_destroy(vme_user_sysfs_class, MKDEV(VME_MAJOR, i));
+	}
+	class_destroy(vme_user_sysfs_class);
+
+	for (i = MASTER_MINOR; i < (MASTER_MAX + 1); i++) {
+		kfree(image[i].kern_buf);
+		vme_master_free(image[i].resource);
+	}
+
+	for (i = SLAVE_MINOR; i < (SLAVE_MAX + 1); i++) {
+		vme_slave_set(image[i].resource, 0, 0, 0, 0, VME_A32, 0);
+		vme_free_consistent(image[i].resource, image[i].size_buf,
+				    image[i].kern_buf, image[i].pci_buf);
+		vme_slave_free(image[i].resource);
+	}
+
+	/* Unregister device driver */
+	cdev_del(vme_user_cdev);
+
+	/* Unregister the major and minor device numbers */
+	unregister_chrdev_region(MKDEV(VME_MAJOR, 0), VME_DEVS);
+}
+
+static struct vme_driver vme_user_driver = {
+	.name = driver_name,
+	.match = vme_user_match,
+	.probe = vme_user_probe,
+	.remove = vme_user_remove,
+};
+
+static int __init vme_user_init(void)
+{
+	int retval = 0;
+
+	pr_info("VME User Space Access Driver\n");
+
+	if (bus_num == 0) {
+		pr_err("No cards, skipping registration\n");
+		retval = -ENODEV;
+		goto err_nocard;
+	}
+
+	/* Let's start by supporting one bus, we can support more than one
+	 * in future revisions if that ever becomes necessary.
+	 */
+	if (bus_num > VME_USER_BUS_MAX) {
+		pr_err("Driver only able to handle %d buses\n",
+		       VME_USER_BUS_MAX);
+		bus_num = VME_USER_BUS_MAX;
+	}
+
+	/*
+	 * Here we just register the maximum number of devices we can and
+	 * leave vme_user_match() to allow only 1 to go through to probe().
+	 * This way, if we later want to allow multiple user access devices,
+	 * we just change the code in vme_user_match().
+	 */
+	retval = vme_register_driver(&vme_user_driver, VME_MAX_SLOTS);
+	if (retval)
+		goto err_reg;
+
+	return retval;
+
+err_reg:
+err_nocard:
+	return retval;
+}
+
+static void __exit vme_user_exit(void)
+{
+	vme_unregister_driver(&vme_user_driver);
+}
+
+MODULE_PARM_DESC(bus, "Enumeration of VMEbus to which the driver is connected");
+module_param_array(bus, int, &bus_num, 0000);
+
+MODULE_DESCRIPTION("VME User Space Access Driver");
+MODULE_AUTHOR("Martyn Welch <martyn.welch@ge.com>");
+MODULE_LICENSE("GPL");
+
+module_init(vme_user_init);
+module_exit(vme_user_exit);
diff --git a/drivers/staging/vme_user/vme_user.h b/drivers/staging/vme_user/vme_user.h
new file mode 100644
index 000000000..19ecb0578
--- /dev/null
+++ b/drivers/staging/vme_user/vme_user.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _VME_USER_H_
+#define _VME_USER_H_
+
+#define VME_USER_BUS_MAX	1
+
+/*
+ * VMEbus Master Window Configuration Structure
+ */
+struct vme_master {
+	__u32 enable;		/* State of Window */
+	__u64 vme_addr;		/* Starting Address on the VMEbus */
+	__u64 size;		/* Window Size */
+	__u32 aspace;		/* Address Space */
+	__u32 cycle;		/* Cycle properties */
+	__u32 dwidth;		/* Maximum Data Width */
+#if 0
+	char prefetchenable;		/* Prefetch Read Enable State */
+	int prefetchsize;		/* Prefetch Read Size (Cache Lines) */
+	char wrpostenable;		/* Write Post State */
+#endif
+} __packed;
+
+/*
+ * IOCTL Commands and structures
+ */
+
+/* Magic number for use in ioctls */
+#define VME_IOC_MAGIC 0xAE
+
+/* VMEbus Slave Window Configuration Structure */
+struct vme_slave {
+	__u32 enable;		/* State of Window */
+	__u64 vme_addr;		/* Starting Address on the VMEbus */
+	__u64 size;		/* Window Size */
+	__u32 aspace;		/* Address Space */
+	__u32 cycle;		/* Cycle properties */
+#if 0
+	char wrpostenable;		/* Write Post State */
+	char rmwlock;			/* Lock PCI during RMW Cycles */
+	char data64bitcapable;		/* non-VMEbus capable of 64-bit Data */
+#endif
+} __packed;
+
+struct vme_irq_id {
+	__u8 level;
+	__u8 statid;
+};
+
+#define VME_GET_SLAVE _IOR(VME_IOC_MAGIC, 1, struct vme_slave)
+#define VME_SET_SLAVE _IOW(VME_IOC_MAGIC, 2, struct vme_slave)
+#define VME_GET_MASTER _IOR(VME_IOC_MAGIC, 3, struct vme_master)
+#define VME_SET_MASTER _IOW(VME_IOC_MAGIC, 4, struct vme_master)
+#define VME_IRQ_GEN _IOW(VME_IOC_MAGIC, 5, struct vme_irq_id)
+
+#endif /* _VME_USER_H_ */
+
diff --git a/drivers/staging/vt6655/Kconfig b/drivers/staging/vt6655/Kconfig
new file mode 100644
index 000000000..d1cd5de46
--- /dev/null
+++ b/drivers/staging/vt6655/Kconfig
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+config VT6655
+   tristate "VIA Technologies VT6655 support"
+   depends on PCI && MAC80211 && m
+   help
+     This is a vendor-written driver for VIA VT6655.
diff --git a/drivers/staging/vt6655/Makefile b/drivers/staging/vt6655/Makefile
new file mode 100644
index 000000000..e70357ec0
--- /dev/null
+++ b/drivers/staging/vt6655/Makefile
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0
+
+vt6655_stage-y +=	device_main.o \
+	card.o \
+	channel.o \
+	mac.o \
+	baseband.o \
+	rxtx.o \
+	dpc.o \
+	power.o \
+	srom.o \
+	key.o \
+	rf.o
+
+obj-$(CONFIG_VT6655) +=	vt6655_stage.o
diff --git a/drivers/staging/vt6655/TODO b/drivers/staging/vt6655/TODO
new file mode 100644
index 000000000..63607ef9c
--- /dev/null
+++ b/drivers/staging/vt6655/TODO
@@ -0,0 +1,21 @@
+TODO:
+- remove __cplusplus ifdefs -- done
+- prepare for merge with vt6656 driver:
+  - rename DEVICE_PRT() to DBG_PRT() -- done
+  - share 80211*.h includes
+  - split rf.c
+  - remove dead code
+  - abstract VT3253 chipset specific code
+- add common vt665x infrastructure
+- kill ttype.h
+- switch to use LIB80211
+- switch to use MAC80211
+- verify unsigned long usage for x86-64 arch
+- reduce .data footprint
+- use kernel coding style
+- checkpatch.pl fixes
+- sparse fixes
+- integrate with drivers/net/wireless
+
+Please send any patches to Greg Kroah-Hartman <greg@kroah.com>
+and Forest Bond <forest@alittletooquiet.net>.
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
new file mode 100644
index 000000000..6ce41983d
--- /dev/null
+++ b/drivers/staging/vt6655/baseband.c
@@ -0,0 +1,2301 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions to access baseband
+ *
+ * Author: Kyle Hsu
+ *
+ * Date: Aug.22, 2002
+ *
+ * Functions:
+ *      bb_get_frame_time	 - Calculate data frame transmitting time
+ *      bb_read_embedded	 - Embedded read baseband register via MAC
+ *      bb_write_embedded	 - Embedded write baseband register via MAC
+ *      bb_vt3253_init		 - VIA VT3253 baseband chip init code
+ *
+ * Revision History:
+ *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-07-2003 Bryan YC Fan:  Add MAXIM2827/2825 and RFMD2959 support.
+ *      08-26-2003 Kyle Hsu    :  Modify BBuGetFrameTime() and
+ *				  BBvCalculateParameter().
+ *                                cancel the setting of MAC_REG_SOFTPWRCTL on
+ *				  BBbVT3253Init().
+ *                                Add the comments.
+ *      09-01-2003 Bryan YC Fan:  RF & BB tables updated.
+ *                                Modified BBvLoopbackOn & BBvLoopbackOff().
+ *
+ *
+ */
+
+#include "mac.h"
+#include "baseband.h"
+#include "srom.h"
+#include "rf.h"
+
+/*---------------------  Static Classes  ----------------------------*/
+
+/*---------------------  Static Variables  --------------------------*/
+
+/*---------------------  Static Functions  --------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Static Definitions -------------------------*/
+
+/*---------------------  Static Classes  ----------------------------*/
+
+/*---------------------  Static Variables  --------------------------*/
+
+#define CB_VT3253_INIT_FOR_RFMD 446
+static const unsigned char by_vt3253_init_tab_rfmd[CB_VT3253_INIT_FOR_RFMD][2] = {
+	{0x00, 0x30},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x00},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x70},
+	{0x09, 0x45},
+	{0x0a, 0x2a},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x01},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x9d},
+	{0x1c, 0x05},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0xa8},
+	{0x2e, 0x1a},
+	{0x2f, 0x0c},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x00},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x0d},
+	{0x3e, 0x51},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x06},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x08},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x80},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x14},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0x44},
+	{0x61, 0x04},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x04},
+	{0x67, 0xb7},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00},
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x0b},
+	{0x81, 0x00},
+	{0x82, 0x3c},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x08},
+	{0x8b, 0xa6},
+	{0x8c, 0x84},
+	{0x8d, 0x47},
+	{0x8e, 0xbb},
+	{0x8f, 0x02},
+	{0x90, 0x21},
+	{0x91, 0x0c},
+	{0x92, 0x04},
+	{0x93, 0x22},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x04},
+	{0xa8, 0x10},
+	{0xa9, 0x00},
+	{0xaa, 0x8f},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x80},
+	{0xb0, 0x38},
+	{0xb1, 0x00},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0xee},
+	{0xb5, 0xff},
+	{0xb6, 0x10},
+	{0xb7, 0x00},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x00},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x10},
+	{0xc2, 0x18},
+	{0xc3, 0x20},
+	{0xc4, 0x10},
+	{0xc5, 0x00},
+	{0xc6, 0x22},
+	{0xc7, 0x14},
+	{0xc8, 0x0f},
+	{0xc9, 0x08},
+	{0xca, 0xa4},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x20},
+	{0xcf, 0x00},
+	{0xd0, 0x00},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x33},
+	{0xd6, 0x70},
+	{0xd7, 0x01},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0x00},
+	{0xe2, 0xcc},
+	{0xe3, 0x04},
+	{0xe4, 0x08},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x0e},
+	{0xe8, 0x88},
+	{0xe9, 0xd4},
+	{0xea, 0x05},
+	{0xeb, 0xf0},
+	{0xec, 0x79},
+	{0xed, 0x0f},
+	{0xee, 0x04},
+	{0xef, 0x04},
+	{0xf0, 0x00},
+	{0xf1, 0x00},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xF0, 0x00},
+	{0xF1, 0xF8},
+	{0xF0, 0x80},
+	{0xF0, 0x00},
+	{0xF1, 0xF4},
+	{0xF0, 0x81},
+	{0xF0, 0x01},
+	{0xF1, 0xF0},
+	{0xF0, 0x82},
+	{0xF0, 0x02},
+	{0xF1, 0xEC},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0xE8},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0xE4},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0xE0},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0xDC},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0xD8},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0xD4},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0xD0},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0xCC},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0xC8},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0xC4},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0xC0},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0xBC},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0xB8},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0xB4},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0xB0},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0xAC},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0xA8},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0xA4},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0xA0},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x9C},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x98},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x94},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x90},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x8C},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x88},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x84},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x80},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x7C},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x78},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x74},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x70},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x6C},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x68},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x64},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x60},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x5C},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x58},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x54},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x50},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x4C},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x48},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x44},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x40},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x3C},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x38},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x34},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x30},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x2C},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x28},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x24},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x20},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x1C},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x18},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x14},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x10},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x0C},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x08},
+	{0xF0, 0x00},
+	{0xF0, 0x3C},
+	{0xF1, 0x04},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x00},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x00},
+	{0xF0, 0xBF},
+	{0xF0, 0x3F},
+	{0xF1, 0x00},
+	{0xF0, 0xC0},
+	{0xF0, 0x00},
+};
+
+#define CB_VT3253B0_INIT_FOR_RFMD 256
+static const unsigned char byVT3253B0_RFMD[CB_VT3253B0_INIT_FOR_RFMD][2] = {
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x81},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x38},
+	{0x09, 0x45},
+	{0x0a, 0x2a},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8e},
+	{0x1c, 0x06},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x34},
+	{0x2e, 0x18},
+	{0x2f, 0x0c},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0xf8},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x09},
+	{0x3e, 0x0d},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x08},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x80},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x14},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0x39},
+	{0x61, 0x83},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0xc0},
+	{0x67, 0x49},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00},
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x89},
+	{0x81, 0x00},
+	{0x82, 0x0e},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0e},
+	{0x8b, 0xa7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x23},
+	{0x91, 0x0c},
+	{0x92, 0x06},
+	{0x93, 0x08},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0xcd},
+	{0xa4, 0x07},
+	{0xa5, 0x33},
+	{0xa6, 0x18},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x28},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x38},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0x00},
+	{0xb5, 0x00},
+	{0xb6, 0x84},
+	{0xb7, 0xfd},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x00},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x20},
+	{0xc2, 0x18},
+	{0xc3, 0x20},
+	{0xc4, 0x10},
+	{0xc5, 0x2c},
+	{0xc6, 0x1e},
+	{0xc7, 0x10},
+	{0xc8, 0x12},
+	{0xc9, 0x01},
+	{0xca, 0x6f},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x00},
+	{0xcf, 0x22},
+	{0xd0, 0x00},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x33},
+	{0xd6, 0x80},
+	{0xd7, 0x21},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xB3},
+	{0xe2, 0x00},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x18},
+	{0xe8, 0x08},
+	{0xe9, 0xd4},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x10},
+	{0xee, 0x30},
+	{0xef, 0x02},
+	{0xf0, 0x00},
+	{0xf1, 0x09},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
+};
+
+#define CB_VT3253B0_AGC_FOR_RFMD2959 195
+/* For RFMD2959 */
+static
+unsigned char byVT3253B0_AGC4_RFMD2959[CB_VT3253B0_AGC_FOR_RFMD2959][2] = {
+	{0xF0, 0x00},
+	{0xF1, 0x3E},
+	{0xF0, 0x80},
+	{0xF0, 0x00},
+	{0xF1, 0x3E},
+	{0xF0, 0x81},
+	{0xF0, 0x01},
+	{0xF1, 0x3E},
+	{0xF0, 0x82},
+	{0xF0, 0x02},
+	{0xF1, 0x3E},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0x3B},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0x39},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0x38},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0x37},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0x36},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0x35},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0x35},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0x34},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0x34},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0x33},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0x32},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0x31},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0x30},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0x2F},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0x2F},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0x2E},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0x2D},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0x2C},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0x2B},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x2B},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x2A},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x29},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x28},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x27},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x26},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x25},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x24},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x24},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x23},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x22},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x21},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x20},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x20},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x1F},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x1E},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x1D},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x1C},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x1B},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x1B},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x1A},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x1A},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x19},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x18},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x17},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x16},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x15},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x15},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x15},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x14},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x13},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x12},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x11},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x10},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x0F},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x0E},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x0D},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x0C},
+	{0xF0, 0xBC},
+	{0xF0, 0x3C},
+	{0xF1, 0x0B},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x0B},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x0A},
+	{0xF0, 0xBF},
+	{0xF0, 0x3F},
+	{0xF1, 0x09},
+	{0xF0, 0x00},
+};
+
+#define CB_VT3253B0_INIT_FOR_AIROHA2230 256
+/* For AIROHA */
+static
+unsigned char byVT3253B0_AIROHA2230[CB_VT3253B0_INIT_FOR_AIROHA2230][2] = {
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x80},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x70},
+	{0x09, 0x41},
+	{0x0a, 0x2A},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8f},
+	{0x1c, 0x09},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x4a},
+	{0x2e, 0x00},
+	{0x2f, 0x0a},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x79},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x0b},
+	{0x3e, 0x48},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x09},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x73},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x15},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0xe4},
+	{0x61, 0x80},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x98},
+	{0x67, 0x0a},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00}, /* RobertYu:20050125, request by JJSue */
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x8c},
+	{0x81, 0x01},
+	{0x82, 0x09},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0f},
+	{0x8b, 0xb7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x22},
+	{0x91, 0x00},
+	{0x92, 0x00},
+	{0x93, 0x00},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x01},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x00},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x38},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0xff},
+	{0xb5, 0x0f},
+	{0xb6, 0xe4},
+	{0xb7, 0xe2},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x01},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x18},
+	{0xc1, 0x20},
+	{0xc2, 0x07},
+	{0xc3, 0x18},
+	{0xc4, 0xff},
+	{0xc5, 0x2c},
+	{0xc6, 0x0c},
+	{0xc7, 0x0a},
+	{0xc8, 0x0e},
+	{0xc9, 0x01},
+	{0xca, 0x68},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x00},
+	{0xcf, 0x25},
+	{0xd0, 0x40},
+	{0xd1, 0x12},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x28},
+	{0xd6, 0x80},
+	{0xd7, 0x2A},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xB3},
+	{0xe2, 0x00},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x1C},
+	{0xe8, 0x00},
+	{0xe9, 0xf4},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x20},
+	{0xee, 0x30},
+	{0xef, 0x01},
+	{0xf0, 0x00},
+	{0xf1, 0x3e},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
+};
+
+#define CB_VT3253B0_INIT_FOR_UW2451 256
+/* For UW2451 */
+static unsigned char byVT3253B0_UW2451[CB_VT3253B0_INIT_FOR_UW2451][2] = {
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x81},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x38},
+	{0x09, 0x45},
+	{0x0a, 0x28},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8f},
+	{0x1c, 0x0f},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x18},
+	{0x2e, 0x00},
+	{0x2f, 0x0a},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x00},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x03},
+	{0x3e, 0x1d},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x09},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x90},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x15},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0xb3},
+	{0x61, 0x81},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x57},
+	{0x67, 0x6c},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00}, /* RobertYu:20050125, request by JJSue */
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x8c},
+	{0x81, 0x00},
+	{0x82, 0x0e},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0e},
+	{0x8b, 0xa7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x00},
+	{0x91, 0x00},
+	{0x92, 0x00},
+	{0x93, 0x00},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xe3},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x00},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x18},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0x00},
+	{0xb5, 0x00},
+	{0xb6, 0x00},
+	{0xb7, 0x00},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x01},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x20},
+	{0xc2, 0x00},
+	{0xc3, 0x20},
+	{0xc4, 0x00},
+	{0xc5, 0x2c},
+	{0xc6, 0x1c},
+	{0xc7, 0x10},
+	{0xc8, 0x10},
+	{0xc9, 0x01},
+	{0xca, 0x68},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x09},
+	{0xce, 0x00},
+	{0xcf, 0x20},
+	{0xd0, 0x40},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x20},
+	{0xd5, 0x28},
+	{0xd6, 0xa0},
+	{0xd7, 0x2a},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xd3},
+	{0xe2, 0xc0},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x12},
+	{0xe8, 0x12},
+	{0xe9, 0x34},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x20},
+	{0xee, 0x30},
+	{0xef, 0x01},
+	{0xf0, 0x00},
+	{0xf1, 0x3e},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
+};
+
+#define CB_VT3253B0_AGC 193
+/* For AIROHA */
+static unsigned char byVT3253B0_AGC[CB_VT3253B0_AGC][2] = {
+	{0xF0, 0x00},
+	{0xF1, 0x00},
+	{0xF0, 0x80},
+	{0xF0, 0x01},
+	{0xF1, 0x00},
+	{0xF0, 0x81},
+	{0xF0, 0x02},
+	{0xF1, 0x02},
+	{0xF0, 0x82},
+	{0xF0, 0x03},
+	{0xF1, 0x04},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0x04},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0x06},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0x06},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0x06},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0x08},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0x08},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0x0A},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0x0A},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0x0C},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0x0C},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0x0E},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0x0E},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0x10},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0x10},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0x12},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0x12},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0x14},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0x14},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0x16},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x16},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x18},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x18},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x1A},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x1A},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x1C},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x1C},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x1E},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x1E},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x20},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x20},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x22},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x22},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x24},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x24},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x26},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x26},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x28},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x28},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x2A},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x2A},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x2C},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x2C},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x2E},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x2E},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x30},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x30},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x32},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x32},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x34},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x34},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x36},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x36},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x38},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x38},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x3A},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x3A},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x3C},
+	{0xF0, 0xBC},
+	{0xF0, 0x3C},
+	{0xF1, 0x3C},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x3E},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x3E},
+	{0xF0, 0xBF},
+	{0xF0, 0x00},
+};
+
+static const unsigned short awc_frame_time[MAX_RATE] = {
+		10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216
+};
+
+/*---------------------  Export Variables  --------------------------*/
+/*
+ * Description: Calculate data frame transmitting time
+ *
+ * Parameters:
+ *  In:
+ *      preamble_type     - Preamble Type
+ *      by_pkt_type        - PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
+ *      cb_frame_length   - Baseband Type
+ *      tx_rate           - Tx Rate
+ *  Out:
+ *
+ * Return Value: FrameTime
+ *
+ */
+unsigned int bb_get_frame_time(unsigned char preamble_type,
+			       unsigned char by_pkt_type,
+			       unsigned int cb_frame_length,
+			       unsigned short tx_rate)
+{
+	unsigned int frame_time;
+	unsigned int preamble;
+	unsigned int tmp;
+	unsigned int rate_idx = (unsigned int)tx_rate;
+	unsigned int rate = 0;
+
+	if (rate_idx > RATE_54M)
+		return 0;
+
+	rate = (unsigned int)awc_frame_time[rate_idx];
+
+	if (rate_idx <= 3) {		    /* CCK mode */
+		if (preamble_type == PREAMBLE_SHORT)
+			preamble = 96;
+		else
+			preamble = 192;
+		frame_time = (cb_frame_length * 80) / rate;  /* ????? */
+		tmp = (frame_time * rate) / 80;
+		if (cb_frame_length != tmp)
+			frame_time++;
+
+		return preamble + frame_time;
+	}
+	frame_time = (cb_frame_length * 8 + 22) / rate; /* ???????? */
+	tmp = ((frame_time * rate) - 22) / 8;
+	if (cb_frame_length != tmp)
+		frame_time++;
+
+	frame_time = frame_time * 4;    /* ??????? */
+	if (by_pkt_type != PK_TYPE_11A)
+		frame_time += 6;     /* ?????? */
+
+	return 20 + frame_time; /* ?????? */
+}
+
+/*
+ * Description: Calculate Length, Service, and Signal fields of Phy for Tx
+ *
+ * Parameters:
+ *  In:
+ *      priv         - Device Structure
+ *      frame_length   - Tx Frame Length
+ *      tx_rate           - Tx Rate
+ *  Out:
+ *	struct vnt_phy_field *phy
+ *		- pointer to Phy Length field
+ *		- pointer to Phy Service field
+ *		- pointer to Phy Signal field
+ *
+ * Return Value: none
+ *
+ */
+void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy)
+{
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	u8 preamble_type = priv->preamble_type;
+
+	bit_count = frame_length * 8;
+	ext_bit = false;
+
+	switch (tx_rate) {
+	case RATE_1M:
+		count = bit_count;
+
+		phy->signal = 0x00;
+
+		break;
+	case RATE_2M:
+		count = bit_count / 2;
+
+		if (preamble_type == PREAMBLE_SHORT)
+			phy->signal = 0x09;
+		else
+			phy->signal = 0x01;
+
+		break;
+	case RATE_5M:
+		count = (bit_count * 10) / 55;
+		tmp = (count * 55) / 10;
+
+		if (tmp != bit_count)
+			count++;
+
+		if (preamble_type == PREAMBLE_SHORT)
+			phy->signal = 0x0a;
+		else
+			phy->signal = 0x02;
+
+		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
+
+		if (tmp != bit_count) {
+			count++;
+
+			if ((bit_count - tmp) <= 3)
+				ext_bit = true;
+		}
+
+		if (preamble_type == PREAMBLE_SHORT)
+			phy->signal = 0x0b;
+		else
+			phy->signal = 0x03;
+
+		break;
+	case RATE_6M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9b;
+		else
+			phy->signal = 0x8b;
+
+		break;
+	case RATE_9M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9f;
+		else
+			phy->signal = 0x8f;
+
+		break;
+	case RATE_12M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9a;
+		else
+			phy->signal = 0x8a;
+
+		break;
+	case RATE_18M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9e;
+		else
+			phy->signal = 0x8e;
+
+		break;
+	case RATE_24M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x99;
+		else
+			phy->signal = 0x89;
+
+		break;
+	case RATE_36M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9d;
+		else
+			phy->signal = 0x8d;
+
+		break;
+	case RATE_48M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x98;
+		else
+			phy->signal = 0x88;
+
+		break;
+	case RATE_54M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	default:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	}
+
+	if (pkt_type == PK_TYPE_11B) {
+		phy->service = 0x00;
+		if (ext_bit)
+			phy->service |= 0x80;
+		phy->len = cpu_to_le16((u16)count);
+	} else {
+		phy->service = 0x00;
+		phy->len = cpu_to_le16((u16)frame_length);
+	}
+}
+
+/*
+ * Description: Read a byte from BASEBAND, by embedded programming
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *      by_bb_addr  - address of register in Baseband
+ *  Out:
+ *      pby_data    - data read
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool bb_read_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		      unsigned char *pby_data)
+{
+	void __iomem *iobase = priv->port_offset;
+	unsigned short ww;
+	unsigned char by_value;
+
+	/* BB reg offset */
+	iowrite8(by_bb_addr, iobase + MAC_REG_BBREGADR);
+
+	/* turn on REGR */
+	vt6655_mac_reg_bits_on(iobase, MAC_REG_BBREGCTL, BBREGCTL_REGR);
+	/* W_MAX_TIMEOUT is the timeout period */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		by_value = ioread8(iobase + MAC_REG_BBREGCTL);
+		if (by_value & BBREGCTL_DONE)
+			break;
+	}
+
+	/* get BB data */
+	*pby_data = ioread8(iobase + MAC_REG_BBREGDATA);
+
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x30)\n");
+		return false;
+	}
+	return true;
+}
+
+/*
+ * Description: Write a Byte to BASEBAND, by embedded programming
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *      by_bb_addr  - address of register in Baseband
+ *      by_data     - data to write
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool bb_write_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		       unsigned char by_data)
+{
+	void __iomem *iobase = priv->port_offset;
+	unsigned short ww;
+	unsigned char by_value;
+
+	/* BB reg offset */
+	iowrite8(by_bb_addr, iobase + MAC_REG_BBREGADR);
+	/* set BB data */
+	iowrite8(by_data, iobase + MAC_REG_BBREGDATA);
+
+	/* turn on BBREGCTL_REGW */
+	vt6655_mac_reg_bits_on(iobase, MAC_REG_BBREGCTL, BBREGCTL_REGW);
+	/* W_MAX_TIMEOUT is the timeout period */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		by_value = ioread8(iobase + MAC_REG_BBREGCTL);
+		if (by_value & BBREGCTL_DONE)
+			break;
+	}
+
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x31)\n");
+		return false;
+	}
+	return true;
+}
+
+/*
+ * Description: VIA VT3253 Baseband chip init function
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *      byRevId     - Revision ID
+ *      byRFType    - RF type
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+
+bool bb_vt3253_init(struct vnt_private *priv)
+{
+	bool result = true;
+	int        ii;
+	void __iomem *iobase = priv->port_offset;
+	unsigned char by_rf_type = priv->byRFType;
+	unsigned char by_local_id = priv->local_id;
+
+	if (by_rf_type == RF_RFMD2959) {
+		if (by_local_id <= REV_ID_VT3253_A1) {
+			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++)
+				result &= bb_write_embedded(priv,
+					by_vt3253_init_tab_rfmd[ii][0],
+					by_vt3253_init_tab_rfmd[ii][1]);
+
+		} else {
+			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++)
+				result &= bb_write_embedded(priv,
+					byVT3253B0_RFMD[ii][0],
+					byVT3253B0_RFMD[ii][1]);
+
+			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++)
+				result &= bb_write_embedded(priv,
+					byVT3253B0_AGC4_RFMD2959[ii][0],
+					byVT3253B0_AGC4_RFMD2959[ii][1]);
+
+			iowrite32(0x23, iobase + MAC_REG_ITRTMSET);
+			vt6655_mac_reg_bits_on(iobase, MAC_REG_PAPEDELAY, BIT(0));
+		}
+		priv->abyBBVGA[0] = 0x18;
+		priv->abyBBVGA[1] = 0x0A;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->dbm_threshold[0] = -70;
+		priv->dbm_threshold[1] = -50;
+		priv->dbm_threshold[2] = 0;
+		priv->dbm_threshold[3] = 0;
+	} else if ((by_rf_type == RF_AIROHA) || (by_rf_type == RF_AL2230S)) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_AIROHA2230[ii][0],
+				byVT3253B0_AIROHA2230[ii][1]);
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x1C;
+		priv->abyBBVGA[1] = 0x10;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->dbm_threshold[0] = -70;
+		priv->dbm_threshold[1] = -48;
+		priv->dbm_threshold[2] = 0;
+		priv->dbm_threshold[3] = 0;
+	} else if (by_rf_type == RF_UW2451) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_UW2451[ii][0],
+				byVT3253B0_UW2451[ii][1]);
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_AGC[ii][0],
+				byVT3253B0_AGC[ii][1]);
+
+		iowrite8(0x23, iobase + MAC_REG_ITRTMSET);
+		vt6655_mac_reg_bits_on(iobase, MAC_REG_PAPEDELAY, BIT(0));
+
+		priv->abyBBVGA[0] = 0x14;
+		priv->abyBBVGA[1] = 0x0A;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->dbm_threshold[0] = -60;
+		priv->dbm_threshold[1] = -50;
+		priv->dbm_threshold[2] = 0;
+		priv->dbm_threshold[3] = 0;
+	} else if (by_rf_type == RF_VT3226) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_AIROHA2230[ii][0],
+				byVT3253B0_AIROHA2230[ii][1]);
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
+			result &= bb_write_embedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x1C;
+		priv->abyBBVGA[1] = 0x10;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->dbm_threshold[0] = -70;
+		priv->dbm_threshold[1] = -48;
+		priv->dbm_threshold[2] = 0;
+		priv->dbm_threshold[3] = 0;
+		/* Fix VT3226 DFC system timing issue */
+		vt6655_mac_word_reg_bits_on(iobase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_RFLEOPT);
+		/* {{ RobertYu: 20050104 */
+	} else {
+		/* No VGA Table now */
+		priv->bUpdateBBVGA = false;
+		priv->abyBBVGA[0] = 0x1C;
+	}
+
+	if (by_local_id > REV_ID_VT3253_A1) {
+		bb_write_embedded(priv, 0x04, 0x7F);
+		bb_write_embedded(priv, 0x0D, 0x01);
+	}
+
+	return result;
+}
+
+/*
+ * Description: Set ShortSlotTime mode
+ *
+ * Parameters:
+ *  In:
+ *      priv     - Device Structure
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void
+bb_set_short_slot_time(struct vnt_private *priv)
+{
+	unsigned char by_bb_rx_conf = 0;
+	unsigned char by_bb_vga = 0;
+
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
+
+	if (priv->short_slot_time)
+		by_bb_rx_conf &= 0xDF; /* 1101 1111 */
+	else
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
+
+	/* patch for 3253B0 Baseband with Cardbus module */
+	bb_read_embedded(priv, 0xE7, &by_bb_vga);
+	if (by_bb_vga == priv->abyBBVGA[0])
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
+
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
+}
+
+void bb_set_vga_gain_offset(struct vnt_private *priv, unsigned char by_data)
+{
+	unsigned char by_bb_rx_conf = 0;
+
+	bb_write_embedded(priv, 0xE7, by_data);
+
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
+	/* patch for 3253B0 Baseband with Cardbus module */
+	if (by_data == priv->abyBBVGA[0])
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
+	else if (priv->short_slot_time)
+		by_bb_rx_conf &= 0xDF; /* 1101 1111 */
+	else
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
+	priv->byBBVGACurrent = by_data;
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
+}
+
+/*
+ * Description: Baseband SoftwareReset
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void
+bb_software_reset(struct vnt_private *priv)
+{
+	bb_write_embedded(priv, 0x50, 0x40);
+	bb_write_embedded(priv, 0x50, 0);
+	bb_write_embedded(priv, 0x9C, 0x01);
+	bb_write_embedded(priv, 0x9C, 0);
+}
+
+/*
+ * Description: Baseband Power Save Mode ON
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void
+bb_power_save_mode_on(struct vnt_private *priv)
+{
+	unsigned char by_org_data;
+
+	bb_read_embedded(priv, 0x0D, &by_org_data);
+	by_org_data |= BIT(0);
+	bb_write_embedded(priv, 0x0D, by_org_data);
+}
+
+/*
+ * Description: Baseband Power Save Mode OFF
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void
+bb_power_save_mode_off(struct vnt_private *priv)
+{
+	unsigned char by_org_data;
+
+	bb_read_embedded(priv, 0x0D, &by_org_data);
+	by_org_data &= ~(BIT(0));
+	bb_write_embedded(priv, 0x0D, by_org_data);
+}
+
+/*
+ * Description: Set Tx Antenna mode
+ *
+ * Parameters:
+ *  In:
+ *      priv          - Device Structure
+ *      by_antenna_mode    - Antenna Mode
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+
+void
+bb_set_tx_antenna_mode(struct vnt_private *priv, unsigned char by_antenna_mode)
+{
+	unsigned char by_bb_tx_conf;
+
+	bb_read_embedded(priv, 0x09, &by_bb_tx_conf); /* CR09 */
+	if (by_antenna_mode == ANT_DIVERSITY) {
+		/* bit 1 is diversity */
+		by_bb_tx_conf |= 0x02;
+	} else if (by_antenna_mode == ANT_A) {
+		/* bit 2 is ANTSEL */
+		by_bb_tx_conf &= 0xF9; /* 1111 1001 */
+	} else if (by_antenna_mode == ANT_B) {
+		by_bb_tx_conf &= 0xFD; /* 1111 1101 */
+		by_bb_tx_conf |= 0x04;
+	}
+	bb_write_embedded(priv, 0x09, by_bb_tx_conf); /* CR09 */
+}
+
+/*
+ * Description: Set Rx Antenna mode
+ *
+ * Parameters:
+ *  In:
+ *      priv          - Device Structure
+ *      by_antenna_mode   - Antenna Mode
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+
+void
+bb_set_rx_antenna_mode(struct vnt_private *priv, unsigned char by_antenna_mode)
+{
+	unsigned char by_bb_rx_conf;
+
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
+	if (by_antenna_mode == ANT_DIVERSITY) {
+		by_bb_rx_conf |= 0x01;
+
+	} else if (by_antenna_mode == ANT_A) {
+		by_bb_rx_conf &= 0xFC; /* 1111 1100 */
+	} else if (by_antenna_mode == ANT_B) {
+		by_bb_rx_conf &= 0xFE; /* 1111 1110 */
+		by_bb_rx_conf |= 0x02;
+	}
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
+}
+
+/*
+ * Description: bb_set_deep_sleep
+ *
+ * Parameters:
+ *  In:
+ *      priv          - Device Structure
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void
+bb_set_deep_sleep(struct vnt_private *priv, unsigned char by_local_id)
+{
+	bb_write_embedded(priv, 0x0C, 0x17); /* CR12 */
+	bb_write_embedded(priv, 0x0D, 0xB9); /* CR13 */
+}
+
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
new file mode 100644
index 000000000..15b2802ed
--- /dev/null
+++ b/drivers/staging/vt6655/baseband.h
@@ -0,0 +1,74 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions to access baseband
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 5, 2002
+ *
+ */
+
+#ifndef __BASEBAND_H__
+#define __BASEBAND_H__
+
+#include "device.h"
+
+/*
+ * Registers in the BASEBAND
+ */
+#define BB_MAX_CONTEXT_SIZE 256
+
+/*
+ * Baseband RF pair definition in eeprom (Bits 6..0)
+ */
+
+#define PREAMBLE_LONG   0
+#define PREAMBLE_SHORT  1
+
+#define F5G             0
+#define F2_4G           1
+
+#define TOP_RATE_54M        0x80000000
+#define TOP_RATE_48M        0x40000000
+#define TOP_RATE_36M        0x20000000
+#define TOP_RATE_24M        0x10000000
+#define TOP_RATE_18M        0x08000000
+#define TOP_RATE_12M        0x04000000
+#define TOP_RATE_11M        0x02000000
+#define TOP_RATE_9M         0x01000000
+#define TOP_RATE_6M         0x00800000
+#define TOP_RATE_55M        0x00400000
+#define TOP_RATE_2M         0x00200000
+#define TOP_RATE_1M         0x00100000
+
+unsigned int bb_get_frame_time(unsigned char preamble_type,
+			       unsigned char by_pkt_type,
+			       unsigned int cb_frame_length,
+			       unsigned short w_rate);
+
+void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy);
+
+bool bb_read_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		      unsigned char *pby_data);
+bool bb_write_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		       unsigned char by_data);
+
+void bb_set_short_slot_time(struct vnt_private *priv);
+void bb_set_vga_gain_offset(struct vnt_private *priv, unsigned char by_data);
+
+/* VT3253 Baseband */
+bool bb_vt3253_init(struct vnt_private *priv);
+void bb_software_reset(struct vnt_private *priv);
+void bb_power_save_mode_on(struct vnt_private *priv);
+void bb_power_save_mode_off(struct vnt_private *priv);
+void bb_set_tx_antenna_mode(struct vnt_private *priv,
+			    unsigned char by_antenna_mode);
+void bb_set_rx_antenna_mode(struct vnt_private *priv,
+			    unsigned char by_antenna_mode);
+void bb_set_deep_sleep(struct vnt_private *priv, unsigned char by_local_id);
+
+#endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
new file mode 100644
index 000000000..c680925b9
--- /dev/null
+++ b/drivers/staging/vt6655/card.c
@@ -0,0 +1,836 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Provide functions to setup NIC operation mode
+ * Functions:
+ *      s_vSafeResetTx - Rest Tx
+ *      CARDvSetRSPINF - Set RSPINF
+ *      CARDvUpdateBasicTopRate - Update BasicTopRate
+ *      CARDbAddBasicRate - Add to BasicRateSet
+ *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
+ *      CARDqGetTSFOffset - Calculate TSFOffset
+ *      vt6655_get_current_tsf - Read Current NIC TSF counter
+ *      CARDqGetNextTBTT - Calculate Next Beacon TSF counter
+ *      CARDvSetFirstNextTBTT - Set NIC Beacon time
+ *      CARDvUpdateNextTBTT - Sync. NIC Beacon time
+ *      CARDbRadioPowerOff - Turn Off NIC Radio Power
+ *
+ * Revision History:
+ *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-26-2003 Kyle Hsu:      Modify the definition type of iobase.
+ *      09-01-2003 Bryan YC Fan:  Add vUpdateIFS().
+ *
+ */
+
+#include "card.h"
+#include "baseband.h"
+#include "mac.h"
+#include "desc.h"
+#include "rf.h"
+#include "power.h"
+
+/*---------------------  Static Definitions -------------------------*/
+
+#define C_SIFS_A        16      /* micro sec. */
+#define C_SIFS_BG       10
+
+#define C_EIFS          80      /* micro sec. */
+
+#define C_SLOT_SHORT    9       /* micro sec. */
+#define C_SLOT_LONG     20
+
+#define C_CWMIN_A       15      /* slot time */
+#define C_CWMIN_B       31
+
+#define C_CWMAX         1023    /* slot time */
+
+#define WAIT_BEACON_TX_DOWN_TMO         3    /* Times */
+
+/*---------------------  Static Variables  --------------------------*/
+
+static const unsigned short cwRXBCNTSFOff[MAX_RATE] = {
+	17, 17, 17, 17, 34, 23, 17, 11, 8, 5, 4, 3};
+
+/*---------------------  Static Functions  --------------------------*/
+
+static void vt6655_mac_set_bb_type(void __iomem *iobase, u32 mask)
+{
+	u32 reg_value;
+
+	reg_value = ioread32(iobase + MAC_REG_ENCFG);
+	reg_value = reg_value & ~ENCFG_BBTYPE_MASK;
+	reg_value = reg_value | mask;
+	iowrite32(reg_value, iobase + MAC_REG_ENCFG);
+}
+
+/*---------------------  Export Functions  --------------------------*/
+
+/*
+ * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
+ *
+ * Parameters:
+ *  In:
+ *      wRate           - Tx Rate
+ *      byPktType       - Tx Packet type
+ *  Out:
+ *      pbyTxRate       - pointer to RSPINF TxRate field
+ *      pbyRsvTime      - pointer to RSPINF RsvTime field
+ *
+ * Return Value: none
+ */
+static void s_vCalculateOFDMRParameter(unsigned char rate,
+				       u8 bb_type,
+				       unsigned char *pbyTxRate,
+				       unsigned char *pbyRsvTime)
+{
+	switch (rate) {
+	case RATE_6M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9B;
+			*pbyRsvTime = 44;
+		} else {
+			*pbyTxRate = 0x8B;
+			*pbyRsvTime = 50;
+		}
+		break;
+
+	case RATE_9M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9F;
+			*pbyRsvTime = 36;
+		} else {
+			*pbyTxRate = 0x8F;
+			*pbyRsvTime = 42;
+		}
+		break;
+
+	case RATE_12M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9A;
+			*pbyRsvTime = 32;
+		} else {
+			*pbyTxRate = 0x8A;
+			*pbyRsvTime = 38;
+		}
+		break;
+
+	case RATE_18M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9E;
+			*pbyRsvTime = 28;
+		} else {
+			*pbyTxRate = 0x8E;
+			*pbyRsvTime = 34;
+		}
+		break;
+
+	case RATE_36M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9D;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x8D;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_48M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x98;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x88;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_54M:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x9C;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x8C;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_24M:
+	default:
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
+			*pbyTxRate = 0x99;
+			*pbyRsvTime = 28;
+		} else {
+			*pbyTxRate = 0x89;
+			*pbyRsvTime = 34;
+		}
+		break;
+	}
+}
+
+/*---------------------  Export Functions  --------------------------*/
+
+/*
+ * Description: Update IFS
+ *
+ * Parameters:
+ *  In:
+ *      priv             - The adapter to be set
+ *  Out:
+ *      none
+ *
+ * Return Value: None.
+ */
+bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type)
+{
+	unsigned char byCWMaxMin = 0;
+	unsigned char bySlot = 0;
+	unsigned char bySIFS = 0;
+	unsigned char byDIFS = 0;
+	int i;
+
+	/* Set SIFS, DIFS, EIFS, SlotTime, CwMin */
+	if (bb_type == BB_TYPE_11A) {
+		vt6655_mac_set_bb_type(priv->port_offset, BB_TYPE_11A);
+		bb_write_embedded(priv, 0x88, 0x03);
+		bySlot = C_SLOT_SHORT;
+		bySIFS = C_SIFS_A;
+		byDIFS = C_SIFS_A + 2 * C_SLOT_SHORT;
+		byCWMaxMin = 0xA4;
+	} else if (bb_type == BB_TYPE_11B) {
+		vt6655_mac_set_bb_type(priv->port_offset, BB_TYPE_11B);
+		bb_write_embedded(priv, 0x88, 0x02);
+		bySlot = C_SLOT_LONG;
+		bySIFS = C_SIFS_BG;
+		byDIFS = C_SIFS_BG + 2 * C_SLOT_LONG;
+		byCWMaxMin = 0xA5;
+	} else { /* PK_TYPE_11GA & PK_TYPE_11GB */
+		vt6655_mac_set_bb_type(priv->port_offset, BB_TYPE_11G);
+		bb_write_embedded(priv, 0x88, 0x08);
+		bySIFS = C_SIFS_BG;
+
+		if (priv->short_slot_time) {
+			bySlot = C_SLOT_SHORT;
+			byDIFS = C_SIFS_BG + 2 * C_SLOT_SHORT;
+		} else {
+			bySlot = C_SLOT_LONG;
+			byDIFS = C_SIFS_BG + 2 * C_SLOT_LONG;
+		}
+
+		byCWMaxMin = 0xa4;
+
+		for (i = RATE_54M; i >= RATE_6M; i--) {
+			if (priv->basic_rates & ((u32)(0x1 << i))) {
+				byCWMaxMin |= 0x1;
+				break;
+			}
+		}
+	}
+
+	if (priv->byRFType == RF_RFMD2959) {
+		/*
+		 * bcs TX_PE will reserve 3 us hardware's processing
+		 * time here is 2 us.
+		 */
+		bySIFS -= 3;
+		byDIFS -= 3;
+		/*
+		 * TX_PE will reserve 3 us for MAX2829 A mode only, it is for
+		 * better TX throughput; MAC will need 2 us to process, so the
+		 * SIFS, DIFS can be shorter by 2 us.
+		 */
+	}
+
+	if (priv->bySIFS != bySIFS) {
+		priv->bySIFS = bySIFS;
+		iowrite8(priv->bySIFS, priv->port_offset + MAC_REG_SIFS);
+	}
+	if (priv->byDIFS != byDIFS) {
+		priv->byDIFS = byDIFS;
+		iowrite8(priv->byDIFS, priv->port_offset + MAC_REG_DIFS);
+	}
+	if (priv->byEIFS != C_EIFS) {
+		priv->byEIFS = C_EIFS;
+		iowrite8(priv->byEIFS, priv->port_offset + MAC_REG_EIFS);
+	}
+	if (priv->bySlot != bySlot) {
+		priv->bySlot = bySlot;
+		iowrite8(priv->bySlot, priv->port_offset + MAC_REG_SLOT);
+
+		bb_set_short_slot_time(priv);
+	}
+	if (priv->byCWMaxMin != byCWMaxMin) {
+		priv->byCWMaxMin = byCWMaxMin;
+		iowrite8(priv->byCWMaxMin, priv->port_offset + MAC_REG_CWMAXMIN0);
+	}
+
+	priv->byPacketType = CARDbyGetPktType(priv);
+
+	CARDvSetRSPINF(priv, bb_type);
+
+	return true;
+}
+
+/*
+ * Description: Sync. TSF counter to BSS
+ *              Get TSF offset and write to HW
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be sync.
+ *      byRxRate        - data rate of receive beacon
+ *      qwBSSTimestamp  - Rx BCN's TSF
+ *      qwLocalTSF      - Local TSF
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ */
+bool CARDbUpdateTSF(struct vnt_private *priv, unsigned char byRxRate,
+		    u64 qwBSSTimestamp)
+{
+	u64 local_tsf;
+	u64 qwTSFOffset = 0;
+
+	local_tsf = vt6655_get_current_tsf(priv);
+
+	if (qwBSSTimestamp != local_tsf) {
+		qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp,
+						local_tsf);
+		/* adjust TSF, HW's TSF add TSF Offset reg */
+		qwTSFOffset =  le64_to_cpu(qwTSFOffset);
+		iowrite32((u32)qwTSFOffset, priv->port_offset + MAC_REG_TSFOFST);
+		iowrite32((u32)(qwTSFOffset >> 32), priv->port_offset + MAC_REG_TSFOFST + 4);
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_TFTCTL, TFTCTL_TSFSYNCEN);
+	}
+	return true;
+}
+
+/*
+ * Description: Set NIC TSF counter for first Beacon time
+ *              Get NEXTTBTT from adjusted TSF and Beacon Interval
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be set.
+ *      wBeaconInterval - Beacon Interval
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeed; otherwise false
+ */
+bool CARDbSetBeaconPeriod(struct vnt_private *priv,
+			  unsigned short wBeaconInterval)
+{
+	u64 qwNextTBTT;
+
+	qwNextTBTT = vt6655_get_current_tsf(priv); /* Get Local TSF counter */
+
+	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
+
+	/* set HW beacon interval */
+	iowrite16(wBeaconInterval, priv->port_offset + MAC_REG_BI);
+	priv->wBeaconInterval = wBeaconInterval;
+	/* Set NextTBTT */
+	qwNextTBTT =  le64_to_cpu(qwNextTBTT);
+	iowrite32((u32)qwNextTBTT, priv->port_offset + MAC_REG_NEXTTBTT);
+	iowrite32((u32)(qwNextTBTT >> 32), priv->port_offset + MAC_REG_NEXTTBTT + 4);
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+
+	return true;
+}
+
+/*
+ * Description: Turn off Radio power
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be turned off
+ *  Out:
+ *      none
+ *
+ */
+void CARDbRadioPowerOff(struct vnt_private *priv)
+{
+	if (priv->radio_off)
+		return;
+
+	switch (priv->byRFType) {
+	case RF_RFMD2959:
+		vt6655_mac_word_reg_bits_off(priv->port_offset, MAC_REG_SOFTPWRCTL,
+					     SOFTPWRCTL_TXPEINV);
+		vt6655_mac_word_reg_bits_on(priv->port_offset, MAC_REG_SOFTPWRCTL,
+					    SOFTPWRCTL_SWPE1);
+		break;
+
+	case RF_AIROHA:
+	case RF_AL2230S:
+		vt6655_mac_word_reg_bits_off(priv->port_offset, MAC_REG_SOFTPWRCTL,
+					     SOFTPWRCTL_SWPE2);
+		vt6655_mac_word_reg_bits_off(priv->port_offset, MAC_REG_SOFTPWRCTL,
+					     SOFTPWRCTL_SWPE3);
+		break;
+	}
+
+	vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_HOSTCR, HOSTCR_RXON);
+
+	bb_set_deep_sleep(priv, priv->local_id);
+
+	priv->radio_off = true;
+	pr_debug("chester power off\n");
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_GPIOCTL0, LED_ACTSET);  /* LED issue */
+}
+
+void CARDvSafeResetTx(struct vnt_private *priv)
+{
+	unsigned int uu;
+	struct vnt_tx_desc *pCurrTD;
+
+	/* initialize TD index */
+	priv->apTailTD[0] = &priv->apTD0Rings[0];
+	priv->apCurrTD[0] = &priv->apTD0Rings[0];
+
+	priv->apTailTD[1] = &priv->apTD1Rings[0];
+	priv->apCurrTD[1] = &priv->apTD1Rings[0];
+
+	for (uu = 0; uu < TYPE_MAXTD; uu++)
+		priv->iTDUsed[uu] = 0;
+
+	for (uu = 0; uu < priv->opts.tx_descs[0]; uu++) {
+		pCurrTD = &priv->apTD0Rings[uu];
+		pCurrTD->td0.owner = OWNED_BY_HOST;
+		/* init all Tx Packet pointer to NULL */
+	}
+	for (uu = 0; uu < priv->opts.tx_descs[1]; uu++) {
+		pCurrTD = &priv->apTD1Rings[uu];
+		pCurrTD->td0.owner = OWNED_BY_HOST;
+		/* init all Tx Packet pointer to NULL */
+	}
+
+	/* set MAC TD pointer */
+	vt6655_mac_set_curr_tx_desc_addr(TYPE_TXDMA0, priv, priv->td0_pool_dma);
+
+	vt6655_mac_set_curr_tx_desc_addr(TYPE_AC0DMA, priv, priv->td1_pool_dma);
+
+	/* set MAC Beacon TX pointer */
+	iowrite32((u32)priv->tx_beacon_dma, priv->port_offset + MAC_REG_BCNDMAPTR);
+}
+
+/*
+ * Description:
+ *      Reset Rx
+ *
+ * Parameters:
+ *  In:
+ *      priv     - Pointer to the adapter
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ */
+void CARDvSafeResetRx(struct vnt_private *priv)
+{
+	unsigned int uu;
+	struct vnt_rx_desc *pDesc;
+
+	/* initialize RD index */
+	priv->pCurrRD[0] = &priv->aRD0Ring[0];
+	priv->pCurrRD[1] = &priv->aRD1Ring[0];
+
+	/* init state, all RD is chip's */
+	for (uu = 0; uu < priv->opts.rx_descs0; uu++) {
+		pDesc = &priv->aRD0Ring[uu];
+		pDesc->rd0.res_count = cpu_to_le16(priv->rx_buf_sz);
+		pDesc->rd0.owner = OWNED_BY_NIC;
+		pDesc->rd1.req_count = cpu_to_le16(priv->rx_buf_sz);
+	}
+
+	/* init state, all RD is chip's */
+	for (uu = 0; uu < priv->opts.rx_descs1; uu++) {
+		pDesc = &priv->aRD1Ring[uu];
+		pDesc->rd0.res_count = cpu_to_le16(priv->rx_buf_sz);
+		pDesc->rd0.owner = OWNED_BY_NIC;
+		pDesc->rd1.req_count = cpu_to_le16(priv->rx_buf_sz);
+	}
+
+	/* set perPkt mode */
+	iowrite32(RX_PERPKT, priv->port_offset + MAC_REG_RXDMACTL0);
+	iowrite32(RX_PERPKT, priv->port_offset + MAC_REG_RXDMACTL1);
+	/* set MAC RD pointer */
+	vt6655_mac_set_curr_rx_0_desc_addr(priv, priv->rd0_pool_dma);
+
+	vt6655_mac_set_curr_rx_1_desc_addr(priv, priv->rd1_pool_dma);
+}
+
+/*
+ * Description: Get response Control frame rate in CCK mode
+ *
+ * Parameters:
+ *  In:
+ *      priv             - The adapter to be set
+ *      wRateIdx            - Receiving data rate
+ *  Out:
+ *      none
+ *
+ * Return Value: response Control frame rate
+ */
+static unsigned short CARDwGetCCKControlRate(struct vnt_private *priv,
+					     unsigned short wRateIdx)
+{
+	unsigned int ui = (unsigned int)wRateIdx;
+
+	while (ui > RATE_1M) {
+		if (priv->basic_rates & ((u32)0x1 << ui))
+			return (unsigned short)ui;
+
+		ui--;
+	}
+	return (unsigned short)RATE_1M;
+}
+
+/*
+ * Description: Get response Control frame rate in OFDM mode
+ *
+ * Parameters:
+ *  In:
+ *      priv             - The adapter to be set
+ *      wRateIdx            - Receiving data rate
+ *  Out:
+ *      none
+ *
+ * Return Value: response Control frame rate
+ */
+static unsigned short CARDwGetOFDMControlRate(struct vnt_private *priv,
+					      unsigned short wRateIdx)
+{
+	unsigned int ui = (unsigned int)wRateIdx;
+
+	pr_debug("BASIC RATE: %X\n", priv->basic_rates);
+
+	if (!CARDbIsOFDMinBasicRate((void *)priv)) {
+		pr_debug("%s:(NO OFDM) %d\n", __func__, wRateIdx);
+		if (wRateIdx > RATE_24M)
+			wRateIdx = RATE_24M;
+		return wRateIdx;
+	}
+	while (ui > RATE_11M) {
+		if (priv->basic_rates & ((u32)0x1 << ui)) {
+			pr_debug("%s : %d\n", __func__, ui);
+			return (unsigned short)ui;
+		}
+		ui--;
+	}
+	pr_debug("%s: 6M\n", __func__);
+	return (unsigned short)RATE_24M;
+}
+
+/*
+ * Description: Set RSPINF
+ *
+ * Parameters:
+ *  In:
+ *      priv             - The adapter to be set
+ *  Out:
+ *      none
+ *
+ * Return Value: None.
+ */
+void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type)
+{
+	union vnt_phy_field_swap phy;
+	unsigned char byTxRate, byRsvTime;      /* For OFDM */
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Set to Page1 */
+	VT6655_MAC_SELECT_PAGE1(priv->port_offset);
+
+	/* RSPINF_b_1 */
+	vnt_get_phy_field(priv, 14,
+			  CARDwGetCCKControlRate(priv, RATE_1M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	 /* swap over to get correct write order */
+	swap(phy.swap[0], phy.swap[1]);
+
+	iowrite32(phy.field_write, priv->port_offset + MAC_REG_RSPINF_B_1);
+
+	/* RSPINF_b_2 */
+	vnt_get_phy_field(priv, 14,
+			  CARDwGetCCKControlRate(priv, RATE_2M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	iowrite32(phy.field_write, priv->port_offset + MAC_REG_RSPINF_B_2);
+
+	/* RSPINF_b_5 */
+	vnt_get_phy_field(priv, 14,
+			  CARDwGetCCKControlRate(priv, RATE_5M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	iowrite32(phy.field_write, priv->port_offset + MAC_REG_RSPINF_B_5);
+
+	/* RSPINF_b_11 */
+	vnt_get_phy_field(priv, 14,
+			  CARDwGetCCKControlRate(priv, RATE_11M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	iowrite32(phy.field_write, priv->port_offset + MAC_REG_RSPINF_B_11);
+
+	/* RSPINF_a_6 */
+	s_vCalculateOFDMRParameter(RATE_6M,
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_6);
+	/* RSPINF_a_9 */
+	s_vCalculateOFDMRParameter(RATE_9M,
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_9);
+	/* RSPINF_a_12 */
+	s_vCalculateOFDMRParameter(RATE_12M,
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_12);
+	/* RSPINF_a_18 */
+	s_vCalculateOFDMRParameter(RATE_18M,
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_18);
+	/* RSPINF_a_24 */
+	s_vCalculateOFDMRParameter(RATE_24M,
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_24);
+	/* RSPINF_a_36 */
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)priv,
+							   RATE_36M),
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_36);
+	/* RSPINF_a_48 */
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)priv,
+							   RATE_48M),
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_48);
+	/* RSPINF_a_54 */
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)priv,
+							   RATE_54M),
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_54);
+	/* RSPINF_a_72 */
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)priv,
+							   RATE_54M),
+				   bb_type,
+				   &byTxRate,
+				   &byRsvTime);
+	iowrite16(MAKEWORD(byTxRate, byRsvTime), priv->port_offset + MAC_REG_RSPINF_A_72);
+	/* Set to Page0 */
+	VT6655_MAC_SELECT_PAGE0(priv->port_offset);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+void CARDvUpdateBasicTopRate(struct vnt_private *priv)
+{
+	unsigned char byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
+	unsigned char ii;
+
+	/* Determines the highest basic rate. */
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((priv->basic_rates) & ((u32)(1 << ii))) {
+			byTopOFDM = ii;
+			break;
+		}
+	}
+	priv->byTopOFDMBasicRate = byTopOFDM;
+
+	for (ii = RATE_11M;; ii--) {
+		if ((priv->basic_rates) & ((u32)(1 << ii))) {
+			byTopCCK = ii;
+			break;
+		}
+		if (ii == RATE_1M)
+			break;
+	}
+	priv->byTopCCKBasicRate = byTopCCK;
+}
+
+bool CARDbIsOFDMinBasicRate(struct vnt_private *priv)
+{
+	int ii;
+
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((priv->basic_rates) & ((u32)BIT(ii)))
+			return true;
+	}
+	return false;
+}
+
+unsigned char CARDbyGetPktType(struct vnt_private *priv)
+{
+	if (priv->byBBType == BB_TYPE_11A || priv->byBBType == BB_TYPE_11B)
+		return (unsigned char)priv->byBBType;
+	else if (CARDbIsOFDMinBasicRate((void *)priv))
+		return PK_TYPE_11GA;
+	else
+		return PK_TYPE_11GB;
+}
+
+/*
+ * Description: Calculate TSF offset of two TSF input
+ *              Get TSF Offset from RxBCN's TSF and local TSF
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be sync.
+ *      qwTSF1          - Rx BCN's TSF
+ *      qwTSF2          - Local TSF
+ *  Out:
+ *      none
+ *
+ * Return Value: TSF Offset value
+ */
+u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
+{
+	unsigned short wRxBcnTSFOffst;
+
+	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate % MAX_RATE];
+
+	qwTSF2 += (u64)wRxBcnTSFOffst;
+
+	return qwTSF1 - qwTSF2;
+}
+
+/*
+ * Description: Read NIC TSF counter
+ *              Get local TSF counter
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be read
+ *  Out:
+ *      none
+ *
+ * Return Value: Current TSF counter
+ */
+u64 vt6655_get_current_tsf(struct vnt_private *priv)
+{
+	void __iomem *iobase = priv->port_offset;
+	unsigned short ww;
+	unsigned char data;
+	u32 low, high;
+
+	vt6655_mac_reg_bits_on(iobase, MAC_REG_TFTCTL, TFTCTL_TSFCNTRRD);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		data = ioread8(iobase + MAC_REG_TFTCTL);
+		if (!(data & TFTCTL_TSFCNTRRD))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT)
+		return 0;
+	low = ioread32(iobase + MAC_REG_TSFCNTR);
+	high = ioread32(iobase + MAC_REG_TSFCNTR + 4);
+	return le64_to_cpu(low + ((u64)high << 32));
+}
+
+/*
+ * Description: Read NIC TSF counter
+ *              Get NEXTTBTT from adjusted TSF and Beacon Interval
+ *
+ * Parameters:
+ *  In:
+ *      qwTSF           - Current TSF counter
+ *      wbeaconInterval - Beacon Interval
+ *  Out:
+ *      qwCurrTSF       - Current TSF counter
+ *
+ * Return Value: TSF value of next Beacon
+ */
+u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval)
+{
+	u32 beacon_int;
+
+	beacon_int = wBeaconInterval * 1024;
+	if (beacon_int) {
+		do_div(qwTSF, beacon_int);
+		qwTSF += 1;
+		qwTSF *= beacon_int;
+	}
+
+	return qwTSF;
+}
+
+/*
+ * Description: Set NIC TSF counter for first Beacon time
+ *              Get NEXTTBTT from adjusted TSF and Beacon Interval
+ *
+ * Parameters:
+ *  In:
+ *      iobase          - IO Base
+ *      wBeaconInterval - Beacon Interval
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ */
+void CARDvSetFirstNextTBTT(struct vnt_private *priv,
+			   unsigned short wBeaconInterval)
+{
+	void __iomem *iobase = priv->port_offset;
+	u64 qwNextTBTT;
+
+	qwNextTBTT = vt6655_get_current_tsf(priv); /* Get Local TSF counter */
+
+	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
+	/* Set NextTBTT */
+	qwNextTBTT =  le64_to_cpu(qwNextTBTT);
+	iowrite32((u32)qwNextTBTT, iobase + MAC_REG_NEXTTBTT);
+	iowrite32((u32)(qwNextTBTT >> 32), iobase + MAC_REG_NEXTTBTT + 4);
+	vt6655_mac_reg_bits_on(iobase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+}
+
+/*
+ * Description: Sync NIC TSF counter for Beacon time
+ *              Get NEXTTBTT and write to HW
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be set
+ *      qwTSF           - Current TSF counter
+ *      wBeaconInterval - Beacon Interval
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ */
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 qwTSF,
+			 unsigned short wBeaconInterval)
+{
+	void __iomem *iobase = priv->port_offset;
+
+	qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
+	/* Set NextTBTT */
+	qwTSF =  le64_to_cpu(qwTSF);
+	iowrite32((u32)qwTSF, iobase + MAC_REG_NEXTTBTT);
+	iowrite32((u32)(qwTSF >> 32), iobase + MAC_REG_NEXTTBTT + 4);
+	vt6655_mac_reg_bits_on(iobase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+	pr_debug("Card:Update Next TBTT[%8llx]\n", qwTSF);
+}
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
new file mode 100644
index 000000000..22dc359a6
--- /dev/null
+++ b/drivers/staging/vt6655/card.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Provide functions to setup NIC operation mode
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ */
+
+#ifndef __CARD_H__
+#define __CARD_H__
+
+#include <linux/types.h>
+#include <linux/nl80211.h>
+
+/*
+ * Loopback mode
+ *
+ * LOBYTE is MAC LB mode, HIBYTE is MII LB mode
+ */
+#define CARD_LB_NONE            MAKEWORD(MAC_LB_NONE, 0)
+/* PHY must ISO, avoid MAC loopback packet go out */
+#define CARD_LB_MAC             MAKEWORD(MAC_LB_INTERNAL, 0)
+#define CARD_LB_PHY             MAKEWORD(MAC_LB_EXT, 0)
+
+#define DEFAULT_MSDU_LIFETIME           512  /* ms */
+#define DEFAULT_MSDU_LIFETIME_RES_64us  8000 /* 64us */
+
+#define DEFAULT_MGN_LIFETIME            8    /* ms */
+#define DEFAULT_MGN_LIFETIME_RES_64us   125  /* 64us */
+
+#define CB_MAX_CHANNEL_24G      14
+#define CB_MAX_CHANNEL_5G       42
+#define CB_MAX_CHANNEL          (CB_MAX_CHANNEL_24G + CB_MAX_CHANNEL_5G)
+
+struct vnt_private;
+
+void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type);
+void CARDvUpdateBasicTopRate(struct vnt_private *priv);
+bool CARDbIsOFDMinBasicRate(struct vnt_private *priv);
+void CARDvSetFirstNextTBTT(struct vnt_private *priv,
+			   unsigned short wBeaconInterval);
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 qwTSF,
+			 unsigned short wBeaconInterval);
+u64 vt6655_get_current_tsf(struct vnt_private *priv);
+u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval);
+u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2);
+unsigned char CARDbyGetPktType(struct vnt_private *priv);
+void CARDvSafeResetTx(struct vnt_private *priv);
+void CARDvSafeResetRx(struct vnt_private *priv);
+void CARDbRadioPowerOff(struct vnt_private *priv);
+bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type);
+bool CARDbUpdateTSF(struct vnt_private *priv, unsigned char byRxRate,
+		    u64 qwBSSTimestamp);
+bool CARDbSetBeaconPeriod(struct vnt_private *priv,
+			  unsigned short wBeaconInterval);
+
+#endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
new file mode 100644
index 000000000..4122875eb
--- /dev/null
+++ b/drivers/staging/vt6655/channel.c
@@ -0,0 +1,135 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include "baseband.h"
+#include "channel.h"
+#include "device.h"
+#include "rf.h"
+
+static struct ieee80211_rate vnt_rates_bg[] = {
+	{ .bitrate = 10,  .hw_value = RATE_1M },
+	{ .bitrate = 20,  .hw_value = RATE_2M },
+	{ .bitrate = 55,  .hw_value = RATE_5M },
+	{ .bitrate = 110, .hw_value = RATE_11M },
+	{ .bitrate = 60,  .hw_value = RATE_6M },
+	{ .bitrate = 90,  .hw_value = RATE_9M },
+	{ .bitrate = 120, .hw_value = RATE_12M },
+	{ .bitrate = 180, .hw_value = RATE_18M },
+	{ .bitrate = 240, .hw_value = RATE_24M },
+	{ .bitrate = 360, .hw_value = RATE_36M },
+	{ .bitrate = 480, .hw_value = RATE_48M },
+	{ .bitrate = 540, .hw_value = RATE_54M },
+};
+
+static struct ieee80211_channel vnt_channels_2ghz[] = {
+	{ .center_freq = 2412, .hw_value = 1 },
+	{ .center_freq = 2417, .hw_value = 2 },
+	{ .center_freq = 2422, .hw_value = 3 },
+	{ .center_freq = 2427, .hw_value = 4 },
+	{ .center_freq = 2432, .hw_value = 5 },
+	{ .center_freq = 2437, .hw_value = 6 },
+	{ .center_freq = 2442, .hw_value = 7 },
+	{ .center_freq = 2447, .hw_value = 8 },
+	{ .center_freq = 2452, .hw_value = 9 },
+	{ .center_freq = 2457, .hw_value = 10 },
+	{ .center_freq = 2462, .hw_value = 11 },
+	{ .center_freq = 2467, .hw_value = 12 },
+	{ .center_freq = 2472, .hw_value = 13 },
+	{ .center_freq = 2484, .hw_value = 14 }
+};
+
+static struct ieee80211_supported_band vnt_supported_2ghz_band = {
+	.channels = vnt_channels_2ghz,
+	.n_channels = ARRAY_SIZE(vnt_channels_2ghz),
+	.bitrates = vnt_rates_bg,
+	.n_bitrates = ARRAY_SIZE(vnt_rates_bg),
+};
+
+static void vnt_init_band(struct vnt_private *priv,
+			  struct ieee80211_supported_band *supported_band,
+			  enum nl80211_band band)
+{
+	int i;
+
+	for (i = 0; i < supported_band->n_channels; i++) {
+		supported_band->channels[i].max_power = 0x3f;
+		supported_band->channels[i].flags =
+			IEEE80211_CHAN_NO_HT40;
+	}
+
+	priv->hw->wiphy->bands[band] = supported_band;
+}
+
+void vnt_init_bands(struct vnt_private *priv)
+{
+	vnt_init_band(priv, &vnt_supported_2ghz_band, NL80211_BAND_2GHZ);
+}
+
+/**
+ * set_channel() - Set NIC media channel
+ *
+ * @priv: The adapter to be set
+ * @ch: Channel to be set
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool set_channel(struct vnt_private *priv, struct ieee80211_channel *ch)
+{
+	bool ret = true;
+
+	if (priv->byCurrentCh == ch->hw_value)
+		return ret;
+
+	/* Set VGA to max sensitivity */
+	if (priv->bUpdateBBVGA &&
+	    priv->byBBVGACurrent != priv->abyBBVGA[0]) {
+		priv->byBBVGACurrent = priv->abyBBVGA[0];
+
+		bb_set_vga_gain_offset(priv, priv->byBBVGACurrent);
+	}
+
+	/* clear NAV */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_MACCR, MACCR_CLRNAV);
+
+	/* TX_PE will reserve 3 us for MAX2829 A mode only,
+	 * it is for better TX throughput
+	 */
+
+	priv->byCurrentCh = ch->hw_value;
+	ret &= RFbSelectChannel(priv, priv->byRFType,
+				ch->hw_value);
+
+	/* Init Synthesizer Table */
+	if (priv->bEnablePSMode)
+		rf_write_wake_prog_syn(priv, priv->byRFType, ch->hw_value);
+
+	bb_software_reset(priv);
+
+	if (priv->local_id > REV_ID_VT3253_B1) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&priv->lock, flags);
+
+		/* set HW default power register */
+		VT6655_MAC_SELECT_PAGE1(priv->port_offset);
+		RFbSetPower(priv, RATE_1M, priv->byCurrentCh);
+		iowrite8(priv->byCurPwr, priv->port_offset + MAC_REG_PWRCCK);
+		RFbSetPower(priv, RATE_6M, priv->byCurrentCh);
+		iowrite8(priv->byCurPwr, priv->port_offset + MAC_REG_PWROFDM);
+		VT6655_MAC_SELECT_PAGE0(priv->port_offset);
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	if (priv->byBBType == BB_TYPE_11B)
+		RFbSetPower(priv, RATE_1M, priv->byCurrentCh);
+	else
+		RFbSetPower(priv, RATE_6M, priv->byCurrentCh);
+
+	return ret;
+}
diff --git a/drivers/staging/vt6655/channel.h b/drivers/staging/vt6655/channel.h
new file mode 100644
index 000000000..78b2d8231
--- /dev/null
+++ b/drivers/staging/vt6655/channel.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ */
+
+#ifndef _CHANNEL_H_
+#define _CHANNEL_H_
+
+#include "card.h"
+
+void vnt_init_bands(struct vnt_private *priv);
+
+bool set_channel(struct vnt_private *priv, struct ieee80211_channel *ch);
+
+#endif /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
new file mode 100644
index 000000000..17a40c53b
--- /dev/null
+++ b/drivers/staging/vt6655/desc.h
@@ -0,0 +1,249 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:The header file of descriptor
+ *
+ * Revision History:
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ */
+
+#ifndef __DESC_H__
+#define __DESC_H__
+
+#include <linux/types.h>
+#include <linux/mm.h>
+#include "linux/ieee80211.h"
+
+#define B_OWNED_BY_CHIP     1
+#define B_OWNED_BY_HOST     0
+
+/* Bits in the RSR register */
+#define RSR_ADDRBROAD       0x80
+#define RSR_ADDRMULTI       0x40
+#define RSR_ADDRUNI         0x00
+#define RSR_IVLDTYP         0x20
+#define RSR_IVLDLEN         0x10        /* invalid len (> 2312 byte) */
+#define RSR_BSSIDOK         0x08
+#define RSR_CRCOK           0x04
+#define RSR_BCNSSIDOK       0x02
+#define RSR_ADDROK          0x01
+
+/* Bits in the new RSR register */
+#define NEWRSR_DECRYPTOK    0x10
+#define NEWRSR_CFPIND       0x08
+#define NEWRSR_HWUTSF       0x04
+#define NEWRSR_BCNHITAID    0x02
+#define NEWRSR_BCNHITAID0   0x01
+
+/* Bits in the TSR0 register */
+#define TSR0_PWRSTS1_2      0xC0
+#define TSR0_PWRSTS7        0x20
+#define TSR0_NCR            0x1F
+
+/* Bits in the TSR1 register */
+#define TSR1_TERR           0x80
+#define TSR1_PWRSTS4_6      0x70
+#define TSR1_RETRYTMO       0x08
+#define TSR1_TMO            0x04
+#define TSR1_PWRSTS3        0x02
+#define ACK_DATA            0x01
+
+/* Bits in the TCR register */
+#define EDMSDU              0x04        /* end of sdu */
+#define TCR_EDP             0x02        /* end of packet */
+#define TCR_STP             0x01        /* start of packet */
+
+/* max transmit or receive buffer size */
+#define CB_MAX_BUF_SIZE     2900U
+					/* NOTE: must be multiple of 4 */
+#define CB_MAX_TX_BUF_SIZE          CB_MAX_BUF_SIZE
+#define CB_MAX_RX_BUF_SIZE_NORMAL   CB_MAX_BUF_SIZE
+
+#define CB_BEACON_BUF_SIZE  512U
+
+#define CB_MAX_RX_DESC      128
+#define CB_MIN_RX_DESC      16
+#define CB_MAX_TX_DESC      64
+#define CB_MIN_TX_DESC      16
+
+#define CB_MAX_RECEIVED_PACKETS     16
+				/*
+				 * limit our receive routine to indicating
+				 * this many at a time for 2 reasons:
+				 * 1. driver flow control to protocol layer
+				 * 2. limit the time used in ISR routine
+				 */
+
+#define CB_EXTRA_RD_NUM     32
+#define CB_RD_NUM           32
+#define CB_TD_NUM           32
+
+/*
+ * max number of physical segments in a single NDIS packet. Above this
+ * threshold, the packet is copied into a single physically contiguous buffer
+ */
+#define CB_MAX_SEGMENT      4
+
+#define CB_MIN_MAP_REG_NUM  4
+#define CB_MAX_MAP_REG_NUM  CB_MAX_TX_DESC
+
+#define CB_PROTOCOL_RESERVED_SECTION    16
+
+/*
+ * if retrys excess 15 times , tx will abort, and if tx fifo underflow,
+ * tx will fail, we should try to resend it
+ */
+#define CB_MAX_TX_ABORT_RETRY   3
+
+/* WMAC definition FIFO Control */
+#define FIFOCTL_AUTO_FB_1   0x1000
+#define FIFOCTL_AUTO_FB_0   0x0800
+#define FIFOCTL_GRPACK      0x0400
+#define FIFOCTL_11GA        0x0300
+#define FIFOCTL_11GB        0x0200
+#define FIFOCTL_11B         0x0100
+#define FIFOCTL_11A         0x0000
+#define FIFOCTL_RTS         0x0080
+#define FIFOCTL_ISDMA0      0x0040
+#define FIFOCTL_GENINT      0x0020
+#define FIFOCTL_TMOEN       0x0010
+#define FIFOCTL_LRETRY      0x0008
+#define FIFOCTL_CRCDIS      0x0004
+#define FIFOCTL_NEEDACK     0x0002
+#define FIFOCTL_LHEAD       0x0001
+
+/* WMAC definition Frag Control */
+#define FRAGCTL_AES         0x0300
+#define FRAGCTL_TKIP        0x0200
+#define FRAGCTL_LEGACY      0x0100
+#define FRAGCTL_NONENCRYPT  0x0000
+#define FRAGCTL_ENDFRAG     0x0003
+#define FRAGCTL_MIDFRAG     0x0002
+#define FRAGCTL_STAFRAG     0x0001
+#define FRAGCTL_NONFRAG     0x0000
+
+#define TYPE_TXDMA0     0
+#define TYPE_AC0DMA     1
+#define TYPE_ATIMDMA    2
+#define TYPE_SYNCDMA    3
+#define TYPE_MAXTD      2
+
+#define TYPE_BEACONDMA  4
+
+#define TYPE_RXDMA0     0
+#define TYPE_RXDMA1     1
+#define TYPE_MAXRD      2
+
+/* TD_INFO flags control bit */
+#define TD_FLAGS_NETIF_SKB      0x01    /* check if need release skb */
+/* check if called from private skb (hostap) */
+#define TD_FLAGS_PRIV_SKB       0x02
+#define TD_FLAGS_PS_RETRY       0x04    /* check if PS STA frame re-transmit */
+
+/*
+ * ref_sk_buff is used for mapping the skb structure between pre-built
+ * driver-obj & running kernel. Since different kernel version (2.4x) may
+ * change skb structure, i.e. pre-built driver-obj may link to older skb that
+ * leads error.
+ */
+
+struct vnt_rd_info {
+	struct sk_buff *skb;
+	dma_addr_t  skb_dma;
+};
+
+struct vnt_rdes0 {
+	volatile __le16 res_count;
+#ifdef __BIG_ENDIAN
+	union {
+		volatile u16 f15_reserved;
+		struct {
+			volatile u8 f8_reserved1;
+			volatile u8 owner:1;
+			volatile u8 f7_reserved:7;
+		} __packed;
+	} __packed;
+#else
+	u16 f15_reserved:15;
+	u16 owner:1;
+#endif
+} __packed;
+
+struct vnt_rdes1 {
+	__le16 req_count;
+	u16 reserved;
+} __packed;
+
+/* Rx descriptor*/
+struct vnt_rx_desc {
+	volatile struct vnt_rdes0 rd0;
+	volatile struct vnt_rdes1 rd1;
+	volatile __le32 buff_addr;
+	volatile __le32 next_desc;
+	struct vnt_rx_desc *next __aligned(8);
+	struct vnt_rd_info *rd_info __aligned(8);
+} __packed;
+
+struct vnt_tdes0 {
+	volatile u8 tsr0;
+	volatile u8 tsr1;
+#ifdef __BIG_ENDIAN
+	union {
+		volatile u16 f15_txtime;
+		struct {
+			volatile u8 f8_reserved;
+			volatile u8 owner:1;
+			volatile u8 f7_reserved:7;
+		} __packed;
+	} __packed;
+#else
+	volatile u16 f15_txtime:15;
+	volatile u16 owner:1;
+#endif
+} __packed;
+
+struct vnt_tdes1 {
+	volatile __le16 req_count;
+	volatile u8 tcr;
+	volatile u8 reserved;
+} __packed;
+
+struct vnt_td_info {
+	void *mic_hdr;
+	struct sk_buff *skb;
+	unsigned char *buf;
+	dma_addr_t buf_dma;
+	u16 req_count;
+	u8 flags;
+};
+
+/* transmit descriptor */
+struct vnt_tx_desc {
+	volatile struct vnt_tdes0 td0;
+	volatile struct vnt_tdes1 td1;
+	volatile __le32 buff_addr;
+	volatile __le32 next_desc;
+	struct vnt_tx_desc *next __aligned(8);
+	struct vnt_td_info *td_info __aligned(8);
+} __packed;
+
+/* Length, Service, and Signal fields of Phy for Tx */
+struct vnt_phy_field {
+	u8 signal;
+	u8 service;
+	__le16 len;
+} __packed;
+
+union vnt_phy_field_swap {
+	struct vnt_phy_field field_read;
+	u16 swap[2];
+	u32 field_write;
+};
+
+#endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
new file mode 100644
index 000000000..c272a4ab2
--- /dev/null
+++ b/drivers/staging/vt6655/device.h
@@ -0,0 +1,292 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: MAC Data structure
+ *
+ * Author: Tevin Chen
+ *
+ * Date: Mar 17, 1997
+ *
+ */
+
+#ifndef __DEVICE_H__
+#define __DEVICE_H__
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/interrupt.h>
+#include <linux/crc32.h>
+#include <net/mac80211.h>
+
+/* device specific */
+
+#include "device_cfg.h"
+#include "card.h"
+#include "srom.h"
+#include "desc.h"
+#include "key.h"
+#include "mac.h"
+
+/*---------------------  Export Definitions -------------------------*/
+
+#define RATE_1M		0
+#define RATE_2M		1
+#define RATE_5M		2
+#define RATE_11M	3
+#define RATE_6M		4
+#define RATE_9M		5
+#define RATE_12M	6
+#define RATE_18M	7
+#define RATE_24M	8
+#define RATE_36M	9
+#define RATE_48M	10
+#define RATE_54M	11
+#define MAX_RATE	12
+
+#define AUTO_FB_NONE            0
+#define AUTO_FB_0               1
+#define AUTO_FB_1               2
+
+#define FB_RATE0                0
+#define FB_RATE1                1
+
+/* Antenna Mode */
+#define ANT_A                   0
+#define ANT_B                   1
+#define ANT_DIVERSITY           2
+#define ANT_RXD_TXA             3
+#define ANT_RXD_TXB             4
+#define ANT_UNKNOWN             0xFF
+
+#define BB_VGA_LEVEL            4
+#define BB_VGA_CHANGE_THRESHOLD 16
+
+#define MAKE_BEACON_RESERVED	10  /* (us) */
+
+/* BUILD OBJ mode */
+
+#define	AVAIL_TD(p, q)	((p)->opts.tx_descs[(q)] - ((p)->iTDUsed[(q)]))
+
+/* 0:11A 1:11B 2:11G */
+#define BB_TYPE_11A    0
+#define BB_TYPE_11B    1
+#define BB_TYPE_11G    2
+
+/* 0:11a, 1:11b, 2:11gb (only CCK in BasicRate), 3:11ga (OFDM in BasicRate) */
+#define PK_TYPE_11A     0
+#define PK_TYPE_11B     1
+#define PK_TYPE_11GB    2
+#define PK_TYPE_11GA    3
+
+#define OWNED_BY_HOST	0
+#define	OWNED_BY_NIC	1
+
+struct vnt_options {
+	int rx_descs0;		/* Number of RX descriptors0 */
+	int rx_descs1;		/* Number of RX descriptors1 */
+	int tx_descs[2];	/* Number of TX descriptors 0, 1 */
+	int int_works;		/* interrupt limits */
+	int short_retry;
+	int long_retry;
+	int bbp_type;
+	u32 flags;
+};
+
+struct vnt_private {
+	struct pci_dev *pcid;
+	/* mac80211 */
+	struct ieee80211_hw *hw;
+	struct ieee80211_vif *vif;
+	unsigned long key_entry_inuse;
+	u32 basic_rates;
+	u16 current_aid;
+	int mc_list_count;
+	u8 mac_hw;
+
+/* dma addr, rx/tx pool */
+	dma_addr_t                  pool_dma;
+	dma_addr_t                  rd0_pool_dma;
+	dma_addr_t                  rd1_pool_dma;
+
+	dma_addr_t                  td0_pool_dma;
+	dma_addr_t                  td1_pool_dma;
+
+	dma_addr_t                  tx_bufs_dma0;
+	dma_addr_t                  tx_bufs_dma1;
+	dma_addr_t                  tx_beacon_dma;
+
+	unsigned char *tx0_bufs;
+	unsigned char *tx1_bufs;
+	unsigned char *tx_beacon_bufs;
+
+	void __iomem                *port_offset;
+	u32                         memaddr;
+	u32                         ioaddr;
+
+	spinlock_t                  lock;
+
+	volatile int                iTDUsed[TYPE_MAXTD];
+
+	struct vnt_tx_desc *apCurrTD[TYPE_MAXTD];
+	struct vnt_tx_desc *apTailTD[TYPE_MAXTD];
+
+	struct vnt_tx_desc *apTD0Rings;
+	struct vnt_tx_desc *apTD1Rings;
+
+	struct vnt_rx_desc *aRD0Ring;
+	struct vnt_rx_desc *aRD1Ring;
+	struct vnt_rx_desc *pCurrRD[TYPE_MAXRD];
+
+	struct vnt_options opts;
+
+	u32                         flags;
+
+	u32                         rx_buf_sz;
+	u8 rx_rate;
+
+	u32                         rx_bytes;
+
+	/* Version control */
+	unsigned char local_id;
+	unsigned char byRFType;
+
+	unsigned char max_pwr_level;
+	unsigned char byZoneType;
+	bool bZoneRegExist;
+	unsigned char byOriginalZonetype;
+
+	unsigned char abyCurrentNetAddr[ETH_ALEN]; __aligned(2)
+	bool bLinkPass;          /* link status: OK or fail */
+
+	unsigned int current_rssi;
+	unsigned char byCurrSQ;
+
+	unsigned long dwTxAntennaSel;
+	unsigned long dwRxAntennaSel;
+	unsigned char byAntennaCount;
+	unsigned char byRxAntennaMode;
+	unsigned char byTxAntennaMode;
+	bool bTxRxAntInv;
+
+	unsigned char *pbyTmpBuff;
+	unsigned int	uSIFS;    /* Current SIFS */
+	unsigned int	uDIFS;    /* Current DIFS */
+	unsigned int	uEIFS;    /* Current EIFS */
+	unsigned int	uSlot;    /* Current SlotTime */
+	unsigned int	uCwMin;   /* Current CwMin */
+	unsigned int	uCwMax;   /* CwMax is fixed on 1023. */
+	/* PHY parameter */
+	unsigned char bySIFS;
+	unsigned char byDIFS;
+	unsigned char byEIFS;
+	unsigned char bySlot;
+	unsigned char byCWMaxMin;
+
+	u8		byBBType; /* 0:11A, 1:11B, 2:11G */
+	u8		byPacketType; /*
+				       * 0:11a,1:11b,2:11gb (only CCK
+				       * in BasicRate), 3:11ga (OFDM in
+				       * Basic Rate)
+				       */
+	unsigned short wBasicRate;
+	unsigned char byACKRate;
+	unsigned char byTopOFDMBasicRate;
+	unsigned char byTopCCKBasicRate;
+
+	unsigned char byMinChannel;
+	unsigned char byMaxChannel;
+
+	unsigned char preamble_type;
+	unsigned char byShortPreamble;
+
+	unsigned short wCurrentRate;
+	unsigned char byShortRetryLimit;
+	unsigned char byLongRetryLimit;
+	enum nl80211_iftype op_mode;
+	bool bBSSIDFilter;
+	unsigned short wMaxTransmitMSDULifetime;
+
+	bool bEncryptionEnable;
+	bool bLongHeader;
+	bool short_slot_time;
+	bool bProtectMode;
+	bool bNonERPPresent;
+	bool bBarkerPreambleMd;
+
+	bool bRadioControlOff;
+	bool radio_off;
+	bool bEnablePSMode;
+	unsigned short wListenInterval;
+	bool bPWBitOn;
+
+	/* GPIO Radio Control */
+	unsigned char byRadioCtl;
+	unsigned char byGPIO;
+	bool hw_radio_off;
+	bool bPrvActive4RadioOFF;
+	bool bGPIOBlockRead;
+
+	/* Beacon related */
+	unsigned short wSeqCounter;
+	unsigned short wBCNBufLen;
+	bool bBeaconBufReady;
+	bool bBeaconSent;
+	bool bIsBeaconBufReadySet;
+	unsigned int	cbBeaconBufReadySetCnt;
+	bool bFixRate;
+	u16 byCurrentCh;
+
+	bool bAES;
+
+	unsigned char byAutoFBCtrl;
+
+	/* For Update BaseBand VGA Gain Offset */
+	bool bUpdateBBVGA;
+	unsigned int	uBBVGADiffCount;
+	unsigned char byBBVGANew;
+	unsigned char byBBVGACurrent;
+	unsigned char abyBBVGA[BB_VGA_LEVEL];
+	long                    dbm_threshold[BB_VGA_LEVEL];
+
+	unsigned char byBBPreEDRSSI;
+	unsigned char byBBPreEDIndex;
+
+	unsigned long dwDiagRefCount;
+
+	/* For FOE Tuning */
+	unsigned char byFOETuning;
+
+	/* For RF Power table */
+	unsigned char byCCKPwr;
+	unsigned char byOFDMPwrG;
+	unsigned char byCurPwr;
+	char	 byCurPwrdBm;
+	unsigned char abyCCKPwrTbl[CB_MAX_CHANNEL_24G + 1];
+	unsigned char abyOFDMPwrTbl[CB_MAX_CHANNEL + 1];
+	char	abyCCKDefaultPwr[CB_MAX_CHANNEL_24G + 1];
+	char	abyOFDMDefaultPwr[CB_MAX_CHANNEL + 1];
+	char	abyRegPwr[CB_MAX_CHANNEL + 1];
+	char	abyLocalPwr[CB_MAX_CHANNEL + 1];
+
+	/* BaseBand Loopback Use */
+	unsigned char byBBCR4d;
+	unsigned char byBBCRc9;
+	unsigned char byBBCR88;
+	unsigned char byBBCR09;
+
+	unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE]; /* unsigned long alignment */
+
+	unsigned short wBeaconInterval;
+	u16 wake_up_count;
+
+	struct work_struct interrupt_work;
+
+	struct ieee80211_low_level_stats low_stats;
+};
+
+#endif
diff --git a/drivers/staging/vt6655/device_cfg.h b/drivers/staging/vt6655/device_cfg.h
new file mode 100644
index 000000000..2d647a361
--- /dev/null
+++ b/drivers/staging/vt6655/device_cfg.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Driver configuration header
+ * Author: Lyndon Chen
+ *
+ * Date: Dec 17, 2002
+ *
+ */
+
+#ifndef __DEVICE_CONFIG_H
+#define __DEVICE_CONFIG_H
+
+#include <linux/types.h>
+
+#define VID_TABLE_SIZE      64
+#define MCAST_TABLE_SIZE    64
+#define MCAM_SIZE           32
+#define VCAM_SIZE           32
+#define TX_QUEUE_NO         8
+
+#define DEVICE_NAME         "vt6655"
+#define DEVICE_FULL_DRV_NAM "VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver"
+
+#ifndef MAJOR_VERSION
+#define MAJOR_VERSION       1
+#endif
+
+#ifndef MINOR_VERSION
+#define MINOR_VERSION       17
+#endif
+
+#ifndef DEVICE_VERSION
+#define DEVICE_VERSION       "1.19.12"
+#endif
+
+#include <linux/fs.h>
+#include <linux/fcntl.h>
+
+#define PKT_BUF_SZ          2390
+
+#endif
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
new file mode 100644
index 000000000..56c3cf3ba
--- /dev/null
+++ b/drivers/staging/vt6655/device_main.c
@@ -0,0 +1,1863 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: driver entry for initial, open, close, tx and rx.
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: Jan 8, 2003
+ *
+ * Functions:
+ *
+ *   vt6655_probe - module initial (insmod) driver entry
+ *   vt6655_remove - module remove entry
+ *   device_free_info - device structure resource free function
+ *   device_print_info - print out resource
+ *   device_rx_srv - rx service function
+ *   device_alloc_rx_buf - rx buffer pre-allocated function
+ *   device_free_rx_buf - free rx buffer function
+ *   device_free_tx_buf - free tx buffer function
+ *   device_init_rd0_ring - initial rd dma0 ring
+ *   device_init_rd1_ring - initial rd dma1 ring
+ *   device_init_td0_ring - initial tx dma0 ring buffer
+ *   device_init_td1_ring - initial tx dma1 ring buffer
+ *   device_init_registers - initial MAC & BBP & RF internal registers.
+ *   device_init_rings - initial tx/rx ring buffer
+ *   device_free_rings - free all allocated ring buffer
+ *   device_tx_srv - tx interrupt service function
+ *
+ * Revision History:
+ */
+
+#include <linux/file.h>
+#include "device.h"
+#include "card.h"
+#include "channel.h"
+#include "baseband.h"
+#include "mac.h"
+#include "power.h"
+#include "rxtx.h"
+#include "dpc.h"
+#include "rf.h"
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+
+/*---------------------  Static Definitions -------------------------*/
+/*
+ * Define module options
+ */
+MODULE_AUTHOR("VIA Networking Technologies, Inc., <lyndonchen@vntek.com.tw>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver");
+
+#define DEVICE_PARAM(N, D)
+
+#define RX_DESC_MIN0     16
+#define RX_DESC_MAX0     128
+#define RX_DESC_DEF0     32
+DEVICE_PARAM(RxDescriptors0, "Number of receive descriptors0");
+
+#define RX_DESC_MIN1     16
+#define RX_DESC_MAX1     128
+#define RX_DESC_DEF1     32
+DEVICE_PARAM(RxDescriptors1, "Number of receive descriptors1");
+
+#define TX_DESC_MIN0     16
+#define TX_DESC_MAX0     128
+#define TX_DESC_DEF0     32
+DEVICE_PARAM(TxDescriptors0, "Number of transmit descriptors0");
+
+#define TX_DESC_MIN1     16
+#define TX_DESC_MAX1     128
+#define TX_DESC_DEF1     64
+DEVICE_PARAM(TxDescriptors1, "Number of transmit descriptors1");
+
+#define INT_WORKS_DEF   20
+#define INT_WORKS_MIN   10
+#define INT_WORKS_MAX   64
+
+DEVICE_PARAM(int_works, "Number of packets per interrupt services");
+
+#define RTS_THRESH_DEF     2347
+
+#define FRAG_THRESH_DEF     2346
+
+#define SHORT_RETRY_MIN     0
+#define SHORT_RETRY_MAX     31
+#define SHORT_RETRY_DEF     8
+
+DEVICE_PARAM(ShortRetryLimit, "Short frame retry limits");
+
+#define LONG_RETRY_MIN     0
+#define LONG_RETRY_MAX     15
+#define LONG_RETRY_DEF     4
+
+DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
+
+/* BasebandType[] baseband type selected
+ * 0: indicate 802.11a type
+ * 1: indicate 802.11b type
+ * 2: indicate 802.11g type
+ */
+#define BBP_TYPE_MIN     0
+#define BBP_TYPE_MAX     2
+#define BBP_TYPE_DEF     2
+
+DEVICE_PARAM(BasebandType, "baseband type");
+
+/*
+ * Static vars definitions
+ */
+static const struct pci_device_id vt6655_pci_id_table[] = {
+	{ PCI_VDEVICE(VIA, 0x3253) },
+	{ 0, }
+};
+
+/*---------------------  Static Functions  --------------------------*/
+
+static int  vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent);
+static void device_free_info(struct vnt_private *priv);
+static void device_print_info(struct vnt_private *priv);
+
+static void vt6655_mac_write_bssid_addr(void __iomem *iobase, const u8 *mac_addr);
+static void vt6655_mac_read_ether_addr(void __iomem *iobase, u8 *mac_addr);
+
+static int device_init_rd0_ring(struct vnt_private *priv);
+static int device_init_rd1_ring(struct vnt_private *priv);
+static int device_init_td0_ring(struct vnt_private *priv);
+static int device_init_td1_ring(struct vnt_private *priv);
+
+static int  device_rx_srv(struct vnt_private *priv, unsigned int idx);
+static int  device_tx_srv(struct vnt_private *priv, unsigned int idx);
+static bool device_alloc_rx_buf(struct vnt_private *, struct vnt_rx_desc *);
+static void device_free_rx_buf(struct vnt_private *priv,
+			       struct vnt_rx_desc *rd);
+static void device_init_registers(struct vnt_private *priv);
+static void device_free_tx_buf(struct vnt_private *, struct vnt_tx_desc *);
+static void device_free_td0_ring(struct vnt_private *priv);
+static void device_free_td1_ring(struct vnt_private *priv);
+static void device_free_rd0_ring(struct vnt_private *priv);
+static void device_free_rd1_ring(struct vnt_private *priv);
+static void device_free_rings(struct vnt_private *priv);
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+static void vt6655_remove(struct pci_dev *pcid)
+{
+	struct vnt_private *priv = pci_get_drvdata(pcid);
+
+	if (!priv)
+		return;
+	device_free_info(priv);
+}
+
+static void device_get_options(struct vnt_private *priv)
+{
+	struct vnt_options *opts = &priv->opts;
+
+	opts->rx_descs0 = RX_DESC_DEF0;
+	opts->rx_descs1 = RX_DESC_DEF1;
+	opts->tx_descs[0] = TX_DESC_DEF0;
+	opts->tx_descs[1] = TX_DESC_DEF1;
+	opts->int_works = INT_WORKS_DEF;
+
+	opts->short_retry = SHORT_RETRY_DEF;
+	opts->long_retry = LONG_RETRY_DEF;
+	opts->bbp_type = BBP_TYPE_DEF;
+}
+
+static void
+device_set_options(struct vnt_private *priv)
+{
+	priv->byShortRetryLimit = priv->opts.short_retry;
+	priv->byLongRetryLimit = priv->opts.long_retry;
+	priv->byBBType = priv->opts.bbp_type;
+	priv->byPacketType = priv->byBBType;
+	priv->byAutoFBCtrl = AUTO_FB_0;
+	priv->bUpdateBBVGA = true;
+	priv->preamble_type = 0;
+
+	pr_debug(" byShortRetryLimit= %d\n", (int)priv->byShortRetryLimit);
+	pr_debug(" byLongRetryLimit= %d\n", (int)priv->byLongRetryLimit);
+	pr_debug(" preamble_type= %d\n", (int)priv->preamble_type);
+	pr_debug(" byShortPreamble= %d\n", (int)priv->byShortPreamble);
+	pr_debug(" byBBType= %d\n", (int)priv->byBBType);
+}
+
+static void vt6655_mac_write_bssid_addr(void __iomem *iobase, const u8 *mac_addr)
+{
+	iowrite8(1, iobase + MAC_REG_PAGE1SEL);
+	for (int i = 0; i < 6; i++)
+		iowrite8(mac_addr[i], iobase + MAC_REG_BSSID0 + i);
+	iowrite8(0, iobase + MAC_REG_PAGE1SEL);
+}
+
+static void vt6655_mac_read_ether_addr(void __iomem *iobase, u8 *mac_addr)
+{
+	iowrite8(1, iobase + MAC_REG_PAGE1SEL);
+	for (int i = 0; i < 6; i++)
+		mac_addr[i] = ioread8(iobase + MAC_REG_PAR0 + i);
+	iowrite8(0, iobase + MAC_REG_PAGE1SEL);
+}
+
+static void vt6655_mac_dma_ctl(void __iomem *iobase, u8 reg_index)
+{
+	u32 reg_value;
+
+	reg_value = ioread32(iobase + reg_index);
+	if (reg_value & DMACTL_RUN)
+		iowrite32(DMACTL_WAKE, iobase + reg_index);
+	else
+		iowrite32(DMACTL_RUN, iobase + reg_index);
+}
+
+static void vt6655_mac_set_bits(void __iomem *iobase, u32 mask)
+{
+	u32 reg_value;
+
+	reg_value = ioread32(iobase + MAC_REG_ENCFG);
+	reg_value = reg_value | mask;
+	iowrite32(reg_value, iobase + MAC_REG_ENCFG);
+}
+
+static void vt6655_mac_clear_bits(void __iomem *iobase, u32 mask)
+{
+	u32 reg_value;
+
+	reg_value = ioread32(iobase + MAC_REG_ENCFG);
+	reg_value = reg_value & ~mask;
+	iowrite32(reg_value, iobase + MAC_REG_ENCFG);
+}
+
+static void vt6655_mac_en_protect_md(void __iomem *iobase)
+{
+	vt6655_mac_set_bits(iobase, ENCFG_PROTECTMD);
+}
+
+static void vt6655_mac_dis_protect_md(void __iomem *iobase)
+{
+	vt6655_mac_clear_bits(iobase, ENCFG_PROTECTMD);
+}
+
+static void vt6655_mac_en_barker_preamble_md(void __iomem *iobase)
+{
+	vt6655_mac_set_bits(iobase, ENCFG_BARKERPREAM);
+}
+
+static void vt6655_mac_dis_barker_preamble_md(void __iomem *iobase)
+{
+	vt6655_mac_clear_bits(iobase, ENCFG_BARKERPREAM);
+}
+
+/*
+ * Initialisation of MAC & BBP registers
+ */
+
+static void device_init_registers(struct vnt_private *priv)
+{
+	unsigned long flags;
+	unsigned int ii;
+	unsigned char byValue;
+	unsigned char byCCKPwrdBm = 0;
+	unsigned char byOFDMPwrdBm = 0;
+
+	MACbShutdown(priv);
+	bb_software_reset(priv);
+
+	/* Do MACbSoftwareReset in MACvInitialize */
+	MACbSoftwareReset(priv);
+
+	priv->bAES = false;
+
+	/* Only used in 11g type, sync with ERP IE */
+	priv->bProtectMode = false;
+
+	priv->bNonERPPresent = false;
+	priv->bBarkerPreambleMd = false;
+	priv->wCurrentRate = RATE_1M;
+	priv->byTopOFDMBasicRate = RATE_24M;
+	priv->byTopCCKBasicRate = RATE_1M;
+
+	/* init MAC */
+	MACvInitialize(priv);
+
+	/* Get Local ID */
+	priv->local_id = ioread8(priv->port_offset + MAC_REG_LOCALID);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	SROMvReadAllContents(priv->port_offset, priv->abyEEPROM);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Get Channel range */
+	priv->byMinChannel = 1;
+	priv->byMaxChannel = CB_MAX_CHANNEL;
+
+	/* Get Antena */
+	byValue = SROMbyReadEmbedded(priv->port_offset, EEP_OFS_ANTENNA);
+	if (byValue & EEP_ANTINV)
+		priv->bTxRxAntInv = true;
+	else
+		priv->bTxRxAntInv = false;
+
+	byValue &= (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+	/* if not set default is All */
+	if (byValue == 0)
+		byValue = (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+
+	if (byValue == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
+		priv->byAntennaCount = 2;
+		priv->byTxAntennaMode = ANT_B;
+		priv->dwTxAntennaSel = 1;
+		priv->dwRxAntennaSel = 1;
+
+		if (priv->bTxRxAntInv)
+			priv->byRxAntennaMode = ANT_A;
+		else
+			priv->byRxAntennaMode = ANT_B;
+	} else  {
+		priv->byAntennaCount = 1;
+		priv->dwTxAntennaSel = 0;
+		priv->dwRxAntennaSel = 0;
+
+		if (byValue & EEP_ANTENNA_AUX) {
+			priv->byTxAntennaMode = ANT_A;
+
+			if (priv->bTxRxAntInv)
+				priv->byRxAntennaMode = ANT_B;
+			else
+				priv->byRxAntennaMode = ANT_A;
+		} else {
+			priv->byTxAntennaMode = ANT_B;
+
+			if (priv->bTxRxAntInv)
+				priv->byRxAntennaMode = ANT_A;
+			else
+				priv->byRxAntennaMode = ANT_B;
+		}
+	}
+
+	/* Set initial antenna mode */
+	bb_set_tx_antenna_mode(priv, priv->byTxAntennaMode);
+	bb_set_rx_antenna_mode(priv, priv->byRxAntennaMode);
+
+	/* zonetype initial */
+	priv->byOriginalZonetype = priv->abyEEPROM[EEP_OFS_ZONETYPE];
+
+	if (!priv->bZoneRegExist)
+		priv->byZoneType = priv->abyEEPROM[EEP_OFS_ZONETYPE];
+
+	pr_debug("priv->byZoneType = %x\n", priv->byZoneType);
+
+	/* Init RF module */
+	RFbInit(priv);
+
+	/* Get Desire Power Value */
+	priv->byCurPwr = 0xFF;
+	priv->byCCKPwr = SROMbyReadEmbedded(priv->port_offset, EEP_OFS_PWR_CCK);
+	priv->byOFDMPwrG = SROMbyReadEmbedded(priv->port_offset,
+					      EEP_OFS_PWR_OFDMG);
+
+	/* Load power Table */
+	for (ii = 0; ii < CB_MAX_CHANNEL_24G; ii++) {
+		priv->abyCCKPwrTbl[ii + 1] =
+			SROMbyReadEmbedded(priv->port_offset,
+					   (unsigned char)(ii + EEP_OFS_CCK_PWR_TBL));
+		if (priv->abyCCKPwrTbl[ii + 1] == 0)
+			priv->abyCCKPwrTbl[ii + 1] = priv->byCCKPwr;
+
+		priv->abyOFDMPwrTbl[ii + 1] =
+			SROMbyReadEmbedded(priv->port_offset,
+					   (unsigned char)(ii + EEP_OFS_OFDM_PWR_TBL));
+		if (priv->abyOFDMPwrTbl[ii + 1] == 0)
+			priv->abyOFDMPwrTbl[ii + 1] = priv->byOFDMPwrG;
+
+		priv->abyCCKDefaultPwr[ii + 1] = byCCKPwrdBm;
+		priv->abyOFDMDefaultPwr[ii + 1] = byOFDMPwrdBm;
+	}
+
+	/* recover 12,13 ,14channel for EUROPE by 11 channel */
+	for (ii = 11; ii < 14; ii++) {
+		priv->abyCCKPwrTbl[ii] = priv->abyCCKPwrTbl[10];
+		priv->abyOFDMPwrTbl[ii] = priv->abyOFDMPwrTbl[10];
+	}
+
+	/* Load OFDM A Power Table */
+	for (ii = 0; ii < CB_MAX_CHANNEL_5G; ii++) {
+		priv->abyOFDMPwrTbl[ii + CB_MAX_CHANNEL_24G + 1] =
+			SROMbyReadEmbedded(priv->port_offset,
+					   (unsigned char)(ii + EEP_OFS_OFDMA_PWR_TBL));
+
+		priv->abyOFDMDefaultPwr[ii + CB_MAX_CHANNEL_24G + 1] =
+			SROMbyReadEmbedded(priv->port_offset,
+					   (unsigned char)(ii + EEP_OFS_OFDMA_PWR_dBm));
+	}
+
+	if (priv->local_id > REV_ID_VT3253_B1) {
+		VT6655_MAC_SELECT_PAGE1(priv->port_offset);
+
+		iowrite8(MSRCTL1_TXPWR | MSRCTL1_CSAPAREN, priv->port_offset + MAC_REG_MSRCTL + 1);
+
+		VT6655_MAC_SELECT_PAGE0(priv->port_offset);
+	}
+
+	/* use relative tx timeout and 802.11i D4 */
+	vt6655_mac_word_reg_bits_on(priv->port_offset, MAC_REG_CFG,
+				    (CFG_TKIPOPT | CFG_NOTXTIMEOUT));
+
+	/* set performance parameter by registry */
+	vt6655_mac_set_short_retry_limit(priv, priv->byShortRetryLimit);
+	MACvSetLongRetryLimit(priv, priv->byLongRetryLimit);
+
+	/* reset TSF counter */
+	iowrite8(TFTCTL_TSFCNTRST, priv->port_offset + MAC_REG_TFTCTL);
+	/* enable TSF counter */
+	iowrite8(TFTCTL_TSFCNTREN, priv->port_offset + MAC_REG_TFTCTL);
+
+	/* initialize BBP registers */
+	bb_vt3253_init(priv);
+
+	if (priv->bUpdateBBVGA) {
+		priv->byBBVGACurrent = priv->abyBBVGA[0];
+		priv->byBBVGANew = priv->byBBVGACurrent;
+		bb_set_vga_gain_offset(priv, priv->abyBBVGA[0]);
+	}
+
+	bb_set_rx_antenna_mode(priv, priv->byRxAntennaMode);
+	bb_set_tx_antenna_mode(priv, priv->byTxAntennaMode);
+
+	/* Set BB and packet type at the same time. */
+	/* Set Short Slot Time, xIFS, and RSPINF. */
+	priv->wCurrentRate = RATE_54M;
+
+	priv->radio_off = false;
+
+	priv->byRadioCtl = SROMbyReadEmbedded(priv->port_offset,
+					      EEP_OFS_RADIOCTL);
+	priv->hw_radio_off = false;
+
+	if (priv->byRadioCtl & EEP_RADIOCTL_ENABLE) {
+		/* Get GPIO */
+		priv->byGPIO = ioread8(priv->port_offset + MAC_REG_GPIOCTL1);
+
+		if (((priv->byGPIO & GPIO0_DATA) &&
+		     !(priv->byRadioCtl & EEP_RADIOCTL_INV)) ||
+		     (!(priv->byGPIO & GPIO0_DATA) &&
+		     (priv->byRadioCtl & EEP_RADIOCTL_INV)))
+			priv->hw_radio_off = true;
+	}
+
+	if (priv->hw_radio_off || priv->bRadioControlOff)
+		CARDbRadioPowerOff(priv);
+
+	/* get Permanent network address */
+	SROMvReadEtherAddress(priv->port_offset, priv->abyCurrentNetAddr);
+	pr_debug("Network address = %pM\n", priv->abyCurrentNetAddr);
+
+	/* reset Tx pointer */
+	CARDvSafeResetRx(priv);
+	/* reset Rx pointer */
+	CARDvSafeResetTx(priv);
+
+	if (priv->local_id <= REV_ID_VT3253_A1)
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_RCR, RCR_WPAERR);
+
+	/* Turn On Rx DMA */
+	vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_RXDMACTL0);
+	vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_RXDMACTL1);
+
+	/* start the adapter */
+	iowrite8(HOSTCR_MACEN | HOSTCR_RXON | HOSTCR_TXON, priv->port_offset + MAC_REG_HOSTCR);
+}
+
+static void device_print_info(struct vnt_private *priv)
+{
+	dev_info(&priv->pcid->dev, "MAC=%pM IO=0x%lx Mem=0x%lx IRQ=%d\n",
+		 priv->abyCurrentNetAddr, (unsigned long)priv->ioaddr,
+		 (unsigned long)priv->port_offset, priv->pcid->irq);
+}
+
+static void device_free_info(struct vnt_private *priv)
+{
+	if (!priv)
+		return;
+
+	if (priv->mac_hw)
+		ieee80211_unregister_hw(priv->hw);
+
+	if (priv->port_offset)
+		iounmap(priv->port_offset);
+
+	if (priv->pcid)
+		pci_release_regions(priv->pcid);
+
+	if (priv->hw)
+		ieee80211_free_hw(priv->hw);
+}
+
+static bool device_init_rings(struct vnt_private *priv)
+{
+	void *vir_pool;
+
+	/*allocate all RD/TD rings a single pool*/
+	vir_pool = dma_alloc_coherent(&priv->pcid->dev,
+				      priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc) +
+				      priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc) +
+				      priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc) +
+				      priv->opts.tx_descs[1] * sizeof(struct vnt_tx_desc),
+				      &priv->pool_dma, GFP_ATOMIC);
+	if (!vir_pool) {
+		dev_err(&priv->pcid->dev, "allocate desc dma memory failed\n");
+		return false;
+	}
+
+	priv->aRD0Ring = vir_pool;
+	priv->aRD1Ring = vir_pool +
+		priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc);
+
+	priv->rd0_pool_dma = priv->pool_dma;
+	priv->rd1_pool_dma = priv->rd0_pool_dma +
+		priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc);
+
+	priv->tx0_bufs = dma_alloc_coherent(&priv->pcid->dev,
+					    priv->opts.tx_descs[0] * PKT_BUF_SZ +
+					    priv->opts.tx_descs[1] * PKT_BUF_SZ +
+					    CB_BEACON_BUF_SIZE +
+					    CB_MAX_BUF_SIZE,
+					    &priv->tx_bufs_dma0, GFP_ATOMIC);
+	if (!priv->tx0_bufs) {
+		dev_err(&priv->pcid->dev, "allocate buf dma memory failed\n");
+
+		dma_free_coherent(&priv->pcid->dev,
+				  priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc) +
+				  priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc) +
+				  priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc) +
+				  priv->opts.tx_descs[1] * sizeof(struct vnt_tx_desc),
+				  vir_pool, priv->pool_dma);
+		return false;
+	}
+
+	priv->td0_pool_dma = priv->rd1_pool_dma +
+		priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc);
+
+	priv->td1_pool_dma = priv->td0_pool_dma +
+		priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc);
+
+	/* vir_pool: pvoid type */
+	priv->apTD0Rings = vir_pool
+		+ priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc)
+		+ priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc);
+
+	priv->apTD1Rings = vir_pool
+		+ priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc)
+		+ priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc)
+		+ priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc);
+
+	priv->tx1_bufs = priv->tx0_bufs +
+		priv->opts.tx_descs[0] * PKT_BUF_SZ;
+
+	priv->tx_beacon_bufs = priv->tx1_bufs +
+		priv->opts.tx_descs[1] * PKT_BUF_SZ;
+
+	priv->pbyTmpBuff = priv->tx_beacon_bufs +
+		CB_BEACON_BUF_SIZE;
+
+	priv->tx_bufs_dma1 = priv->tx_bufs_dma0 +
+		priv->opts.tx_descs[0] * PKT_BUF_SZ;
+
+	priv->tx_beacon_dma = priv->tx_bufs_dma1 +
+		priv->opts.tx_descs[1] * PKT_BUF_SZ;
+
+	return true;
+}
+
+static void device_free_rings(struct vnt_private *priv)
+{
+	dma_free_coherent(&priv->pcid->dev,
+			  priv->opts.rx_descs0 * sizeof(struct vnt_rx_desc) +
+			  priv->opts.rx_descs1 * sizeof(struct vnt_rx_desc) +
+			  priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc) +
+			  priv->opts.tx_descs[1] * sizeof(struct vnt_tx_desc),
+			  priv->aRD0Ring, priv->pool_dma);
+
+	dma_free_coherent(&priv->pcid->dev,
+			  priv->opts.tx_descs[0] * PKT_BUF_SZ +
+			  priv->opts.tx_descs[1] * PKT_BUF_SZ +
+			  CB_BEACON_BUF_SIZE +
+			  CB_MAX_BUF_SIZE,
+			  priv->tx0_bufs, priv->tx_bufs_dma0);
+}
+
+static int device_init_rd0_ring(struct vnt_private *priv)
+{
+	int i;
+	dma_addr_t      curr = priv->rd0_pool_dma;
+	struct vnt_rx_desc *desc;
+	int ret;
+
+	/* Init the RD0 ring entries */
+	for (i = 0; i < priv->opts.rx_descs0;
+	     i ++, curr += sizeof(struct vnt_rx_desc)) {
+		desc = &priv->aRD0Ring[i];
+		desc->rd_info = kzalloc(sizeof(*desc->rd_info), GFP_KERNEL);
+		if (!desc->rd_info) {
+			ret = -ENOMEM;
+			goto err_free_desc;
+		}
+
+		if (!device_alloc_rx_buf(priv, desc)) {
+			dev_err(&priv->pcid->dev, "can not alloc rx bufs\n");
+			ret = -ENOMEM;
+			goto err_free_rd;
+		}
+
+		desc->next = &priv->aRD0Ring[(i + 1) % priv->opts.rx_descs0];
+		desc->next_desc = cpu_to_le32(curr + sizeof(struct vnt_rx_desc));
+	}
+
+	if (i > 0)
+		priv->aRD0Ring[i - 1].next_desc = cpu_to_le32(priv->rd0_pool_dma);
+	priv->pCurrRD[0] = &priv->aRD0Ring[0];
+
+	return 0;
+
+err_free_rd:
+	kfree(desc->rd_info);
+
+err_free_desc:
+	while (i--) {
+		desc = &priv->aRD0Ring[i];
+		device_free_rx_buf(priv, desc);
+		kfree(desc->rd_info);
+	}
+
+	return ret;
+}
+
+static int device_init_rd1_ring(struct vnt_private *priv)
+{
+	int i;
+	dma_addr_t      curr = priv->rd1_pool_dma;
+	struct vnt_rx_desc *desc;
+	int ret;
+
+	/* Init the RD1 ring entries */
+	for (i = 0; i < priv->opts.rx_descs1;
+	     i ++, curr += sizeof(struct vnt_rx_desc)) {
+		desc = &priv->aRD1Ring[i];
+		desc->rd_info = kzalloc(sizeof(*desc->rd_info), GFP_KERNEL);
+		if (!desc->rd_info) {
+			ret = -ENOMEM;
+			goto err_free_desc;
+		}
+
+		if (!device_alloc_rx_buf(priv, desc)) {
+			dev_err(&priv->pcid->dev, "can not alloc rx bufs\n");
+			ret = -ENOMEM;
+			goto err_free_rd;
+		}
+
+		desc->next = &priv->aRD1Ring[(i + 1) % priv->opts.rx_descs1];
+		desc->next_desc = cpu_to_le32(curr + sizeof(struct vnt_rx_desc));
+	}
+
+	if (i > 0)
+		priv->aRD1Ring[i - 1].next_desc = cpu_to_le32(priv->rd1_pool_dma);
+	priv->pCurrRD[1] = &priv->aRD1Ring[0];
+
+	return 0;
+
+err_free_rd:
+	kfree(desc->rd_info);
+
+err_free_desc:
+	while (i--) {
+		desc = &priv->aRD1Ring[i];
+		device_free_rx_buf(priv, desc);
+		kfree(desc->rd_info);
+	}
+
+	return ret;
+}
+
+static void device_free_rd0_ring(struct vnt_private *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->opts.rx_descs0; i++) {
+		struct vnt_rx_desc *desc = &priv->aRD0Ring[i];
+
+		device_free_rx_buf(priv, desc);
+		kfree(desc->rd_info);
+	}
+}
+
+static void device_free_rd1_ring(struct vnt_private *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->opts.rx_descs1; i++) {
+		struct vnt_rx_desc *desc = &priv->aRD1Ring[i];
+
+		device_free_rx_buf(priv, desc);
+		kfree(desc->rd_info);
+	}
+}
+
+static int device_init_td0_ring(struct vnt_private *priv)
+{
+	int i;
+	dma_addr_t  curr;
+	struct vnt_tx_desc *desc;
+	int ret;
+
+	curr = priv->td0_pool_dma;
+	for (i = 0; i < priv->opts.tx_descs[0];
+	     i++, curr += sizeof(struct vnt_tx_desc)) {
+		desc = &priv->apTD0Rings[i];
+		desc->td_info = kzalloc(sizeof(*desc->td_info), GFP_KERNEL);
+		if (!desc->td_info) {
+			ret = -ENOMEM;
+			goto err_free_desc;
+		}
+
+		desc->td_info->buf = priv->tx0_bufs + i * PKT_BUF_SZ;
+		desc->td_info->buf_dma = priv->tx_bufs_dma0 + i * PKT_BUF_SZ;
+
+		desc->next = &(priv->apTD0Rings[(i + 1) % priv->opts.tx_descs[0]]);
+		desc->next_desc = cpu_to_le32(curr +
+					      sizeof(struct vnt_tx_desc));
+	}
+
+	if (i > 0)
+		priv->apTD0Rings[i - 1].next_desc = cpu_to_le32(priv->td0_pool_dma);
+	priv->apTailTD[0] = priv->apCurrTD[0] = &priv->apTD0Rings[0];
+
+	return 0;
+
+err_free_desc:
+	while (i--) {
+		desc = &priv->apTD0Rings[i];
+		kfree(desc->td_info);
+	}
+
+	return ret;
+}
+
+static int device_init_td1_ring(struct vnt_private *priv)
+{
+	int i;
+	dma_addr_t  curr;
+	struct vnt_tx_desc *desc;
+	int ret;
+
+	/* Init the TD ring entries */
+	curr = priv->td1_pool_dma;
+	for (i = 0; i < priv->opts.tx_descs[1];
+	     i++, curr += sizeof(struct vnt_tx_desc)) {
+		desc = &priv->apTD1Rings[i];
+		desc->td_info = kzalloc(sizeof(*desc->td_info), GFP_KERNEL);
+		if (!desc->td_info) {
+			ret = -ENOMEM;
+			goto err_free_desc;
+		}
+
+		desc->td_info->buf = priv->tx1_bufs + i * PKT_BUF_SZ;
+		desc->td_info->buf_dma = priv->tx_bufs_dma1 + i * PKT_BUF_SZ;
+
+		desc->next = &(priv->apTD1Rings[(i + 1) % priv->opts.tx_descs[1]]);
+		desc->next_desc = cpu_to_le32(curr + sizeof(struct vnt_tx_desc));
+	}
+
+	if (i > 0)
+		priv->apTD1Rings[i - 1].next_desc = cpu_to_le32(priv->td1_pool_dma);
+	priv->apTailTD[1] = priv->apCurrTD[1] = &priv->apTD1Rings[0];
+
+	return 0;
+
+err_free_desc:
+	while (i--) {
+		desc = &priv->apTD1Rings[i];
+		kfree(desc->td_info);
+	}
+
+	return ret;
+}
+
+static void device_free_td0_ring(struct vnt_private *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->opts.tx_descs[0]; i++) {
+		struct vnt_tx_desc *desc = &priv->apTD0Rings[i];
+		struct vnt_td_info *td_info = desc->td_info;
+
+		dev_kfree_skb(td_info->skb);
+		kfree(desc->td_info);
+	}
+}
+
+static void device_free_td1_ring(struct vnt_private *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->opts.tx_descs[1]; i++) {
+		struct vnt_tx_desc *desc = &priv->apTD1Rings[i];
+		struct vnt_td_info *td_info = desc->td_info;
+
+		dev_kfree_skb(td_info->skb);
+		kfree(desc->td_info);
+	}
+}
+
+/*-----------------------------------------------------------------*/
+
+static int device_rx_srv(struct vnt_private *priv, unsigned int idx)
+{
+	struct vnt_rx_desc *rd;
+	int works = 0;
+
+	for (rd = priv->pCurrRD[idx];
+	     rd->rd0.owner == OWNED_BY_HOST;
+	     rd = rd->next) {
+		if (works++ > 15)
+			break;
+
+		if (!rd->rd_info->skb)
+			break;
+
+		if (vnt_receive_frame(priv, rd)) {
+			if (!device_alloc_rx_buf(priv, rd)) {
+				dev_err(&priv->pcid->dev,
+					"can not allocate rx buf\n");
+				break;
+			}
+		}
+		rd->rd0.owner = OWNED_BY_NIC;
+	}
+
+	priv->pCurrRD[idx] = rd;
+
+	return works;
+}
+
+static bool device_alloc_rx_buf(struct vnt_private *priv,
+				struct vnt_rx_desc *rd)
+{
+	struct vnt_rd_info *rd_info = rd->rd_info;
+
+	rd_info->skb = dev_alloc_skb((int)priv->rx_buf_sz);
+	if (!rd_info->skb)
+		return false;
+
+	rd_info->skb_dma =
+		dma_map_single(&priv->pcid->dev,
+			       skb_put(rd_info->skb, skb_tailroom(rd_info->skb)),
+			       priv->rx_buf_sz, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&priv->pcid->dev, rd_info->skb_dma)) {
+		dev_kfree_skb(rd_info->skb);
+		rd_info->skb = NULL;
+		return false;
+	}
+
+	*((unsigned int *)&rd->rd0) = 0; /* FIX cast */
+
+	rd->rd0.res_count = cpu_to_le16(priv->rx_buf_sz);
+	rd->rd0.owner = OWNED_BY_NIC;
+	rd->rd1.req_count = cpu_to_le16(priv->rx_buf_sz);
+	rd->buff_addr = cpu_to_le32(rd_info->skb_dma);
+
+	return true;
+}
+
+static void device_free_rx_buf(struct vnt_private *priv,
+			       struct vnt_rx_desc *rd)
+{
+	struct vnt_rd_info *rd_info = rd->rd_info;
+
+	dma_unmap_single(&priv->pcid->dev, rd_info->skb_dma,
+			 priv->rx_buf_sz, DMA_FROM_DEVICE);
+	dev_kfree_skb(rd_info->skb);
+}
+
+static const u8 fallback_rate0[5][5] = {
+	{RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
+	{RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
+	{RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
+};
+
+static const u8 fallback_rate1[5][5] = {
+	{RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_48M, RATE_48M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
+};
+
+static int vnt_int_report_rate(struct vnt_private *priv,
+			       struct vnt_td_info *context, u8 tsr0, u8 tsr1)
+{
+	struct vnt_tx_fifo_head *fifo_head;
+	struct ieee80211_tx_info *info;
+	struct ieee80211_rate *rate;
+	u16 fb_option;
+	u8 tx_retry = (tsr0 & TSR0_NCR);
+	s8 idx;
+
+	if (!context)
+		return -ENOMEM;
+
+	if (!context->skb)
+		return -EINVAL;
+
+	fifo_head = (struct vnt_tx_fifo_head *)context->buf;
+	fb_option = (le16_to_cpu(fifo_head->fifo_ctl) &
+			(FIFOCTL_AUTO_FB_0 | FIFOCTL_AUTO_FB_1));
+
+	info = IEEE80211_SKB_CB(context->skb);
+	idx = info->control.rates[0].idx;
+
+	if (fb_option && !(tsr1 & TSR1_TERR)) {
+		u8 tx_rate;
+		u8 retry = tx_retry;
+
+		rate = ieee80211_get_tx_rate(priv->hw, info);
+		tx_rate = rate->hw_value - RATE_18M;
+
+		if (retry > 4)
+			retry = 4;
+
+		if (fb_option & FIFOCTL_AUTO_FB_0)
+			tx_rate = fallback_rate0[tx_rate][retry];
+		else if (fb_option & FIFOCTL_AUTO_FB_1)
+			tx_rate = fallback_rate1[tx_rate][retry];
+
+		if (info->band == NL80211_BAND_5GHZ)
+			idx = tx_rate - RATE_6M;
+		else
+			idx = tx_rate;
+	}
+
+	ieee80211_tx_info_clear_status(info);
+
+	info->status.rates[0].count = tx_retry;
+
+	if (!(tsr1 & TSR1_TERR)) {
+		info->status.rates[0].idx = idx;
+
+		if (info->flags & IEEE80211_TX_CTL_NO_ACK)
+			info->flags |= IEEE80211_TX_STAT_NOACK_TRANSMITTED;
+		else
+			info->flags |= IEEE80211_TX_STAT_ACK;
+	}
+
+	return 0;
+}
+
+static int device_tx_srv(struct vnt_private *priv, unsigned int idx)
+{
+	struct vnt_tx_desc *desc;
+	int                      works = 0;
+	unsigned char byTsr0;
+	unsigned char byTsr1;
+
+	for (desc = priv->apTailTD[idx]; priv->iTDUsed[idx] > 0; desc = desc->next) {
+		if (desc->td0.owner == OWNED_BY_NIC)
+			break;
+		if (works++ > 15)
+			break;
+
+		byTsr0 = desc->td0.tsr0;
+		byTsr1 = desc->td0.tsr1;
+
+		/* Only the status of first TD in the chain is correct */
+		if (desc->td1.tcr & TCR_STP) {
+			if ((desc->td_info->flags & TD_FLAGS_NETIF_SKB) != 0) {
+				if (!(byTsr1 & TSR1_TERR)) {
+					if (byTsr0 != 0) {
+						pr_debug(" Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X]\n",
+							 (int)idx, byTsr1,
+							 byTsr0);
+					}
+				} else {
+					pr_debug(" Tx[%d] dropped & tsr1[%02X] tsr0[%02X]\n",
+						 (int)idx, byTsr1, byTsr0);
+				}
+			}
+
+			if (byTsr1 & TSR1_TERR) {
+				if ((desc->td_info->flags & TD_FLAGS_PRIV_SKB) != 0) {
+					pr_debug(" Tx[%d] fail has error. tsr1[%02X] tsr0[%02X]\n",
+						 (int)idx, byTsr1, byTsr0);
+				}
+			}
+
+			vnt_int_report_rate(priv, desc->td_info, byTsr0, byTsr1);
+
+			device_free_tx_buf(priv, desc);
+			priv->iTDUsed[idx]--;
+		}
+	}
+
+	priv->apTailTD[idx] = desc;
+
+	return works;
+}
+
+static void device_error(struct vnt_private *priv, unsigned short status)
+{
+	if (status & ISR_FETALERR) {
+		dev_err(&priv->pcid->dev, "Hardware fatal error\n");
+
+		MACbShutdown(priv);
+		return;
+	}
+}
+
+static void device_free_tx_buf(struct vnt_private *priv,
+			       struct vnt_tx_desc *desc)
+{
+	struct vnt_td_info *td_info = desc->td_info;
+	struct sk_buff *skb = td_info->skb;
+
+	if (skb)
+		ieee80211_tx_status_irqsafe(priv->hw, skb);
+
+	td_info->skb = NULL;
+	td_info->flags = 0;
+}
+
+static void vnt_check_bb_vga(struct vnt_private *priv)
+{
+	long dbm;
+	int i;
+
+	if (!priv->bUpdateBBVGA)
+		return;
+
+	if (priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)
+		return;
+
+	if (!(priv->vif->cfg.assoc && priv->current_rssi))
+		return;
+
+	RFvRSSITodBm(priv, (u8)priv->current_rssi, &dbm);
+
+	for (i = 0; i < BB_VGA_LEVEL; i++) {
+		if (dbm < priv->dbm_threshold[i]) {
+			priv->byBBVGANew = priv->abyBBVGA[i];
+			break;
+		}
+	}
+
+	if (priv->byBBVGANew == priv->byBBVGACurrent) {
+		priv->uBBVGADiffCount = 1;
+		return;
+	}
+
+	priv->uBBVGADiffCount++;
+
+	if (priv->uBBVGADiffCount == 1) {
+		/* first VGA diff gain */
+		bb_set_vga_gain_offset(priv, priv->byBBVGANew);
+
+		dev_dbg(&priv->pcid->dev,
+			"First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+			(int)dbm, priv->byBBVGANew,
+			priv->byBBVGACurrent,
+			(int)priv->uBBVGADiffCount);
+	}
+
+	if (priv->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD) {
+		dev_dbg(&priv->pcid->dev,
+			"RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+			(int)dbm, priv->byBBVGANew,
+			priv->byBBVGACurrent,
+			(int)priv->uBBVGADiffCount);
+
+		bb_set_vga_gain_offset(priv, priv->byBBVGANew);
+	}
+}
+
+static void vnt_interrupt_process(struct vnt_private *priv)
+{
+	struct ieee80211_low_level_stats *low_stats = &priv->low_stats;
+	int             max_count = 0;
+	u32 mib_counter;
+	u32 isr;
+	unsigned long flags;
+
+	isr = ioread32(priv->port_offset + MAC_REG_ISR);
+
+	if (isr == 0)
+		return;
+
+	if (isr == 0xffffffff) {
+		pr_debug("isr = 0xffff\n");
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Read low level stats */
+	mib_counter = ioread32(priv->port_offset + MAC_REG_MIBCNTR);
+
+	low_stats->dot11RTSSuccessCount += mib_counter & 0xff;
+	low_stats->dot11RTSFailureCount += (mib_counter >> 8) & 0xff;
+	low_stats->dot11ACKFailureCount += (mib_counter >> 16) & 0xff;
+	low_stats->dot11FCSErrorCount += (mib_counter >> 24) & 0xff;
+
+	/*
+	 * TBD....
+	 * Must do this after doing rx/tx, cause ISR bit is slow
+	 * than RD/TD write back
+	 * update ISR counter
+	 */
+	while (isr && priv->vif) {
+		iowrite32(isr, priv->port_offset + MAC_REG_ISR);
+
+		if (isr & ISR_FETALERR) {
+			pr_debug(" ISR_FETALERR\n");
+			iowrite8(0, priv->port_offset + MAC_REG_SOFTPWRCTL);
+			iowrite16(SOFTPWRCTL_SWPECTI, priv->port_offset + MAC_REG_SOFTPWRCTL);
+			device_error(priv, isr);
+		}
+
+		if (isr & ISR_TBTT) {
+			if (priv->op_mode != NL80211_IFTYPE_ADHOC)
+				vnt_check_bb_vga(priv);
+
+			priv->bBeaconSent = false;
+			if (priv->bEnablePSMode)
+				PSbIsNextTBTTWakeUp((void *)priv);
+
+			if ((priv->op_mode == NL80211_IFTYPE_AP ||
+			    priv->op_mode == NL80211_IFTYPE_ADHOC) &&
+			    priv->vif->bss_conf.enable_beacon)
+				MACvOneShotTimer1MicroSec(priv,
+							  (priv->vif->bss_conf.beacon_int -
+							   MAKE_BEACON_RESERVED) << 10);
+
+			/* TODO: adhoc PS mode */
+		}
+
+		if (isr & ISR_BNTX) {
+			if (priv->op_mode == NL80211_IFTYPE_ADHOC) {
+				priv->bIsBeaconBufReadySet = false;
+				priv->cbBeaconBufReadySetCnt = 0;
+			}
+
+			priv->bBeaconSent = true;
+		}
+
+		if (isr & ISR_RXDMA0)
+			max_count += device_rx_srv(priv, TYPE_RXDMA0);
+
+		if (isr & ISR_RXDMA1)
+			max_count += device_rx_srv(priv, TYPE_RXDMA1);
+
+		if (isr & ISR_TXDMA0)
+			max_count += device_tx_srv(priv, TYPE_TXDMA0);
+
+		if (isr & ISR_AC0DMA)
+			max_count += device_tx_srv(priv, TYPE_AC0DMA);
+
+		if (isr & ISR_SOFTTIMER1) {
+			if (priv->vif->bss_conf.enable_beacon)
+				vnt_beacon_make(priv, priv->vif);
+		}
+
+		/* If both buffers available wake the queue */
+		if (AVAIL_TD(priv, TYPE_TXDMA0) &&
+		    AVAIL_TD(priv, TYPE_AC0DMA) &&
+		    ieee80211_queue_stopped(priv->hw, 0))
+			ieee80211_wake_queues(priv->hw);
+
+		isr = ioread32(priv->port_offset + MAC_REG_ISR);
+
+		vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_RXDMACTL0);
+		vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_RXDMACTL1);
+
+		if (max_count > priv->opts.int_works)
+			break;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void vnt_interrupt_work(struct work_struct *work)
+{
+	struct vnt_private *priv =
+		container_of(work, struct vnt_private, interrupt_work);
+
+	if (priv->vif)
+		vnt_interrupt_process(priv);
+
+	iowrite32(IMR_MASK_VALUE, priv->port_offset + MAC_REG_IMR);
+}
+
+static irqreturn_t vnt_interrupt(int irq,  void *arg)
+{
+	struct vnt_private *priv = arg;
+
+	schedule_work(&priv->interrupt_work);
+
+	iowrite32(0, priv->port_offset + MAC_REG_IMR);
+
+	return IRQ_HANDLED;
+}
+
+static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct vnt_tx_desc *head_td;
+	u32 dma_idx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (ieee80211_is_data(hdr->frame_control))
+		dma_idx = TYPE_AC0DMA;
+	else
+		dma_idx = TYPE_TXDMA0;
+
+	if (AVAIL_TD(priv, dma_idx) < 1) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		ieee80211_stop_queues(priv->hw);
+		return -ENOMEM;
+	}
+
+	head_td = priv->apCurrTD[dma_idx];
+
+	head_td->td1.tcr = 0;
+
+	head_td->td_info->skb = skb;
+
+	if (dma_idx == TYPE_AC0DMA)
+		head_td->td_info->flags = TD_FLAGS_NETIF_SKB;
+
+	priv->apCurrTD[dma_idx] = head_td->next;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	vnt_generate_fifo_header(priv, dma_idx, head_td, skb);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->bPWBitOn = false;
+
+	/* Set TSR1 & ReqCount in TxDescHead */
+	head_td->td1.tcr |= (TCR_STP | TCR_EDP | EDMSDU);
+	head_td->td1.req_count = cpu_to_le16(head_td->td_info->req_count);
+
+	head_td->buff_addr = cpu_to_le32(head_td->td_info->buf_dma);
+
+	/* Poll Transmit the adapter */
+	wmb();
+	head_td->td0.owner = OWNED_BY_NIC;
+	wmb(); /* second memory barrier */
+
+	if (head_td->td_info->flags & TD_FLAGS_NETIF_SKB)
+		vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_AC0DMACTL);
+	else
+		vt6655_mac_dma_ctl(priv->port_offset, MAC_REG_TXDMACTL0);
+
+	priv->iTDUsed[dma_idx]++;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static void vnt_tx_80211(struct ieee80211_hw *hw,
+			 struct ieee80211_tx_control *control,
+			 struct sk_buff *skb)
+{
+	struct vnt_private *priv = hw->priv;
+
+	if (vnt_tx_packet(priv, skb))
+		ieee80211_free_txskb(hw, skb);
+}
+
+static int vnt_start(struct ieee80211_hw *hw)
+{
+	struct vnt_private *priv = hw->priv;
+	int ret;
+
+	priv->rx_buf_sz = PKT_BUF_SZ;
+	if (!device_init_rings(priv))
+		return -ENOMEM;
+
+	ret = request_irq(priv->pcid->irq, vnt_interrupt,
+			  IRQF_SHARED, "vt6655", priv);
+	if (ret) {
+		dev_dbg(&priv->pcid->dev, "failed to start irq\n");
+		goto err_free_rings;
+	}
+
+	dev_dbg(&priv->pcid->dev, "call device init rd0 ring\n");
+	ret = device_init_rd0_ring(priv);
+	if (ret)
+		goto err_free_irq;
+	ret = device_init_rd1_ring(priv);
+	if (ret)
+		goto err_free_rd0_ring;
+	ret = device_init_td0_ring(priv);
+	if (ret)
+		goto err_free_rd1_ring;
+	ret = device_init_td1_ring(priv);
+	if (ret)
+		goto err_free_td0_ring;
+
+	device_init_registers(priv);
+
+	dev_dbg(&priv->pcid->dev, "enable MAC interrupt\n");
+	iowrite32(IMR_MASK_VALUE, priv->port_offset + MAC_REG_IMR);
+
+	ieee80211_wake_queues(hw);
+
+	return 0;
+
+err_free_td0_ring:
+	device_free_td0_ring(priv);
+err_free_rd1_ring:
+	device_free_rd1_ring(priv);
+err_free_rd0_ring:
+	device_free_rd0_ring(priv);
+err_free_irq:
+	free_irq(priv->pcid->irq, priv);
+err_free_rings:
+	device_free_rings(priv);
+	return ret;
+}
+
+static void vnt_stop(struct ieee80211_hw *hw)
+{
+	struct vnt_private *priv = hw->priv;
+
+	ieee80211_stop_queues(hw);
+
+	cancel_work_sync(&priv->interrupt_work);
+
+	MACbShutdown(priv);
+	MACbSoftwareReset(priv);
+	CARDbRadioPowerOff(priv);
+
+	device_free_td0_ring(priv);
+	device_free_td1_ring(priv);
+	device_free_rd0_ring(priv);
+	device_free_rd1_ring(priv);
+	device_free_rings(priv);
+
+	free_irq(priv->pcid->irq, priv);
+}
+
+static int vnt_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	priv->vif = vif;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_RCR, RCR_UNICAST);
+
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+
+		break;
+	case NL80211_IFTYPE_AP:
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_RCR, RCR_UNICAST);
+
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_HOSTCR, HOSTCR_AP);
+
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	priv->op_mode = vif->type;
+
+	return 0;
+}
+
+static void vnt_remove_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_TCR, TCR_AUTOBCNTX);
+		vt6655_mac_reg_bits_off(priv->port_offset,
+					MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+		break;
+	case NL80211_IFTYPE_AP:
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_TCR, TCR_AUTOBCNTX);
+		vt6655_mac_reg_bits_off(priv->port_offset,
+					MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_HOSTCR, HOSTCR_AP);
+		break;
+	default:
+		break;
+	}
+
+	priv->op_mode = NL80211_IFTYPE_UNSPECIFIED;
+}
+
+static int vnt_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct vnt_private *priv = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
+	u8 bb_type;
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		if (conf->flags & IEEE80211_CONF_PS)
+			PSvEnablePowerSaving(priv, conf->listen_interval);
+		else
+			PSvDisablePowerSaving(priv);
+	}
+
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) ||
+	    (conf->flags & IEEE80211_CONF_OFFCHANNEL)) {
+		set_channel(priv, conf->chandef.chan);
+
+		if (conf->chandef.chan->band == NL80211_BAND_5GHZ)
+			bb_type = BB_TYPE_11A;
+		else
+			bb_type = BB_TYPE_11G;
+
+		if (priv->byBBType != bb_type) {
+			priv->byBBType = bb_type;
+
+			CARDbSetPhyParameter(priv, priv->byBBType);
+		}
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		if (priv->byBBType == BB_TYPE_11B)
+			priv->wCurrentRate = RATE_1M;
+		else
+			priv->wCurrentRate = RATE_54M;
+
+		RFbSetPower(priv, priv->wCurrentRate,
+			    conf->chandef.chan->hw_value);
+	}
+
+	return 0;
+}
+
+static void vnt_bss_info_changed(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_bss_conf *conf, u64 changed)
+{
+	struct vnt_private *priv = hw->priv;
+
+	priv->current_aid = vif->cfg.aid;
+
+	if (changed & BSS_CHANGED_BSSID && conf->bssid) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&priv->lock, flags);
+
+		vt6655_mac_write_bssid_addr(priv->port_offset, conf->bssid);
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	if (changed & BSS_CHANGED_BASIC_RATES) {
+		priv->basic_rates = conf->basic_rates;
+
+		CARDvUpdateBasicTopRate(priv);
+
+		dev_dbg(&priv->pcid->dev,
+			"basic rates %x\n", conf->basic_rates);
+	}
+
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		if (conf->use_short_preamble) {
+			vt6655_mac_en_barker_preamble_md(priv->port_offset);
+			priv->preamble_type = true;
+		} else {
+			vt6655_mac_dis_barker_preamble_md(priv->port_offset);
+			priv->preamble_type = false;
+		}
+	}
+
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		if (conf->use_cts_prot)
+			vt6655_mac_en_protect_md(priv->port_offset);
+		else
+			vt6655_mac_dis_protect_md(priv->port_offset);
+	}
+
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		if (conf->use_short_slot)
+			priv->short_slot_time = true;
+		else
+			priv->short_slot_time = false;
+
+		CARDbSetPhyParameter(priv, priv->byBBType);
+		bb_set_vga_gain_offset(priv, priv->abyBBVGA[0]);
+	}
+
+	if (changed & BSS_CHANGED_TXPOWER)
+		RFbSetPower(priv, priv->wCurrentRate,
+			    conf->chandef.chan->hw_value);
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		dev_dbg(&priv->pcid->dev,
+			"Beacon enable %d\n", conf->enable_beacon);
+
+		if (conf->enable_beacon) {
+			vnt_beacon_enable(priv, vif, conf);
+
+			vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_TCR, TCR_AUTOBCNTX);
+		} else {
+			vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_TCR,
+						TCR_AUTOBCNTX);
+		}
+	}
+
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_BEACON_INFO) &&
+	    priv->op_mode != NL80211_IFTYPE_AP) {
+		if (vif->cfg.assoc && conf->beacon_rate) {
+			CARDbUpdateTSF(priv, conf->beacon_rate->hw_value,
+				       conf->sync_tsf);
+
+			CARDbSetBeaconPeriod(priv, conf->beacon_int);
+
+			CARDvSetFirstNextTBTT(priv, conf->beacon_int);
+		} else {
+			iowrite8(TFTCTL_TSFCNTRST, priv->port_offset + MAC_REG_TFTCTL);
+			iowrite8(TFTCTL_TSFCNTREN, priv->port_offset + MAC_REG_TFTCTL);
+		}
+	}
+}
+
+static u64 vnt_prepare_multicast(struct ieee80211_hw *hw,
+				 struct netdev_hw_addr_list *mc_list)
+{
+	struct vnt_private *priv = hw->priv;
+	struct netdev_hw_addr *ha;
+	u64 mc_filter = 0;
+	u32 bit_nr = 0;
+
+	netdev_hw_addr_list_for_each(ha, mc_list) {
+		bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+
+		mc_filter |= 1ULL << (bit_nr & 0x3f);
+	}
+
+	priv->mc_list_count = mc_list->count;
+
+	return mc_filter;
+}
+
+static void vnt_configure(struct ieee80211_hw *hw,
+			  unsigned int changed_flags,
+			  unsigned int *total_flags, u64 multicast)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 rx_mode = 0;
+
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC;
+
+	rx_mode = ioread8(priv->port_offset + MAC_REG_RCR);
+
+	dev_dbg(&priv->pcid->dev, "rx mode in = %x\n", rx_mode);
+
+	if (changed_flags & FIF_ALLMULTI) {
+		if (*total_flags & FIF_ALLMULTI) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&priv->lock, flags);
+
+			if (priv->mc_list_count > 2) {
+				VT6655_MAC_SELECT_PAGE1(priv->port_offset);
+
+				iowrite32(0xffffffff, priv->port_offset + MAC_REG_MAR0);
+				iowrite32(0xffffffff, priv->port_offset + MAC_REG_MAR0 + 4);
+
+				VT6655_MAC_SELECT_PAGE0(priv->port_offset);
+			} else {
+				VT6655_MAC_SELECT_PAGE1(priv->port_offset);
+
+				multicast =  le64_to_cpu(multicast);
+				iowrite32((u32)multicast, priv->port_offset +  MAC_REG_MAR0);
+				iowrite32((u32)(multicast >> 32),
+					  priv->port_offset + MAC_REG_MAR0 + 4);
+
+				VT6655_MAC_SELECT_PAGE0(priv->port_offset);
+			}
+
+			spin_unlock_irqrestore(&priv->lock, flags);
+
+			rx_mode |= RCR_MULTICAST | RCR_BROADCAST;
+		} else {
+			rx_mode &= ~(RCR_MULTICAST | RCR_BROADCAST);
+		}
+	}
+
+	if (changed_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC)) {
+		rx_mode |= RCR_MULTICAST | RCR_BROADCAST;
+
+		if (*total_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC))
+			rx_mode &= ~RCR_BSSID;
+		else
+			rx_mode |= RCR_BSSID;
+	}
+
+	iowrite8(rx_mode, priv->port_offset + MAC_REG_RCR);
+
+	dev_dbg(&priv->pcid->dev, "rx mode out= %x\n", rx_mode);
+}
+
+static int vnt_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key)
+{
+	struct vnt_private *priv = hw->priv;
+
+	switch (cmd) {
+	case SET_KEY:
+		if (vnt_set_keys(hw, sta, vif, key))
+			return -EOPNOTSUPP;
+		break;
+	case DISABLE_KEY:
+		if (test_bit(key->hw_key_idx, &priv->key_entry_inuse))
+			clear_bit(key->hw_key_idx, &priv->key_entry_inuse);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int vnt_get_stats(struct ieee80211_hw *hw,
+			 struct ieee80211_low_level_stats *stats)
+{
+	struct vnt_private *priv = hw->priv;
+
+	memcpy(stats, &priv->low_stats, sizeof(*stats));
+
+	return 0;
+}
+
+static u64 vnt_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+	u64 tsf;
+
+	tsf = vt6655_get_current_tsf(priv);
+
+	return tsf;
+}
+
+static void vnt_set_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			u64 tsf)
+{
+	struct vnt_private *priv = hw->priv;
+
+	CARDvUpdateNextTBTT(priv, tsf, vif->bss_conf.beacon_int);
+}
+
+static void vnt_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	/* reset TSF counter */
+	iowrite8(TFTCTL_TSFCNTRST, priv->port_offset + MAC_REG_TFTCTL);
+}
+
+static const struct ieee80211_ops vnt_mac_ops = {
+	.tx			= vnt_tx_80211,
+	.start			= vnt_start,
+	.stop			= vnt_stop,
+	.add_interface		= vnt_add_interface,
+	.remove_interface	= vnt_remove_interface,
+	.config			= vnt_config,
+	.bss_info_changed	= vnt_bss_info_changed,
+	.prepare_multicast	= vnt_prepare_multicast,
+	.configure_filter	= vnt_configure,
+	.set_key		= vnt_set_key,
+	.get_stats		= vnt_get_stats,
+	.get_tsf		= vnt_get_tsf,
+	.set_tsf		= vnt_set_tsf,
+	.reset_tsf		= vnt_reset_tsf,
+};
+
+static int vnt_init(struct vnt_private *priv)
+{
+	SET_IEEE80211_PERM_ADDR(priv->hw, priv->abyCurrentNetAddr);
+
+	vnt_init_bands(priv);
+
+	if (ieee80211_register_hw(priv->hw))
+		return -ENODEV;
+
+	priv->mac_hw = true;
+
+	CARDbRadioPowerOff(priv);
+
+	return 0;
+}
+
+static int
+vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
+{
+	struct vnt_private *priv;
+	struct ieee80211_hw *hw;
+	struct wiphy *wiphy;
+	int         rc;
+
+	dev_notice(&pcid->dev,
+		   "%s Ver. %s\n", DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
+
+	dev_notice(&pcid->dev,
+		   "Copyright (c) 2003 VIA Networking Technologies, Inc.\n");
+
+	hw = ieee80211_alloc_hw(sizeof(*priv), &vnt_mac_ops);
+	if (!hw) {
+		dev_err(&pcid->dev, "could not register ieee80211_hw\n");
+		return -ENOMEM;
+	}
+
+	priv = hw->priv;
+	priv->pcid = pcid;
+
+	spin_lock_init(&priv->lock);
+
+	priv->hw = hw;
+
+	SET_IEEE80211_DEV(priv->hw, &pcid->dev);
+
+	if (pci_enable_device(pcid)) {
+		device_free_info(priv);
+		return -ENODEV;
+	}
+
+	dev_dbg(&pcid->dev,
+		"Before get pci_info memaddr is %x\n", priv->memaddr);
+
+	pci_set_master(pcid);
+
+	priv->memaddr = pci_resource_start(pcid, 0);
+	priv->ioaddr = pci_resource_start(pcid, 1);
+	priv->port_offset = ioremap(priv->memaddr & PCI_BASE_ADDRESS_MEM_MASK,
+				   256);
+	if (!priv->port_offset) {
+		dev_err(&pcid->dev, ": Failed to IO remapping ..\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
+
+	rc = pci_request_regions(pcid, DEVICE_NAME);
+	if (rc) {
+		dev_err(&pcid->dev, ": Failed to find PCI device\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
+
+	if (dma_set_mask(&pcid->dev, DMA_BIT_MASK(32))) {
+		dev_err(&pcid->dev, ": Failed to set dma 32 bit mask\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
+
+	INIT_WORK(&priv->interrupt_work, vnt_interrupt_work);
+
+	/* do reset */
+	if (!MACbSoftwareReset(priv)) {
+		dev_err(&pcid->dev, ": Failed to access MAC hardware..\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
+	/* initial to reload eeprom */
+	MACvInitialize(priv);
+	vt6655_mac_read_ether_addr(priv->port_offset, priv->abyCurrentNetAddr);
+
+	/* Get RFType */
+	priv->byRFType = SROMbyReadEmbedded(priv->port_offset, EEP_OFS_RFTYPE);
+	priv->byRFType &= RF_MASK;
+
+	dev_dbg(&pcid->dev, "RF Type = %x\n", priv->byRFType);
+
+	device_get_options(priv);
+	device_set_options(priv);
+
+	wiphy = priv->hw->wiphy;
+
+	wiphy->frag_threshold = FRAG_THRESH_DEF;
+	wiphy->rts_threshold = RTS_THRESH_DEF;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
+
+	ieee80211_hw_set(priv->hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(priv->hw, SIGNAL_DBM);
+	ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(priv->hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(priv->hw, SUPPORTS_PS);
+
+	priv->hw->max_signal = 100;
+
+	if (vnt_init(priv)) {
+		device_free_info(priv);
+		return -ENODEV;
+	}
+
+	device_print_info(priv);
+	pci_set_drvdata(pcid, priv);
+
+	return 0;
+}
+
+/*------------------------------------------------------------------*/
+
+static int __maybe_unused vt6655_suspend(struct device *dev_d)
+{
+	struct vnt_private *priv = dev_get_drvdata(dev_d);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	MACbShutdown(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int __maybe_unused vt6655_resume(struct device *dev_d)
+{
+	device_wakeup_disable(dev_d);
+
+	return 0;
+}
+
+MODULE_DEVICE_TABLE(pci, vt6655_pci_id_table);
+
+static SIMPLE_DEV_PM_OPS(vt6655_pm_ops, vt6655_suspend, vt6655_resume);
+
+static struct pci_driver device_driver = {
+	.name = DEVICE_NAME,
+	.id_table = vt6655_pci_id_table,
+	.probe = vt6655_probe,
+	.remove = vt6655_remove,
+	.driver.pm = &vt6655_pm_ops,
+};
+
+module_pci_driver(device_driver);
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
new file mode 100644
index 000000000..c6ed3537f
--- /dev/null
+++ b/drivers/staging/vt6655/dpc.c
@@ -0,0 +1,145 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: handle dpc rx functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: May 20, 2003
+ *
+ * Functions:
+ *
+ * Revision History:
+ *
+ */
+
+#include "device.h"
+#include "baseband.h"
+#include "rf.h"
+#include "dpc.h"
+
+static bool vnt_rx_data(struct vnt_private *priv, struct sk_buff *skb,
+			u16 bytes_received)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_rx_status rx_status = { 0 };
+	struct ieee80211_hdr *hdr;
+	__le16 fc;
+	u8 *rsr, *new_rsr, *rssi;
+	__le64 *tsf_time;
+	u16 frame_size;
+	int ii, r;
+	u8 *rx_rate;
+	u8 *skb_data;
+	u8 rate_idx = 0;
+	u8 rate[MAX_RATE] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
+	long rx_dbm;
+
+	/* [31:16]RcvByteCount ( not include 4-byte Status ) */
+	frame_size = le16_to_cpu(*((__le16 *)(skb->data + 2)));
+	if (frame_size > 2346 || frame_size < 14) {
+		dev_dbg(&priv->pcid->dev, "------- WRONG Length 1\n");
+		return false;
+	}
+
+	skb_data = (u8 *)skb->data;
+
+	rx_rate = skb_data + 1;
+
+	sband = hw->wiphy->bands[hw->conf.chandef.chan->band];
+
+	for (r = RATE_1M; r < MAX_RATE; r++) {
+		if (*rx_rate == rate[r])
+			break;
+	}
+
+	priv->rx_rate = r;
+
+	for (ii = 0; ii < sband->n_bitrates; ii++) {
+		if (sband->bitrates[ii].hw_value == r) {
+			rate_idx = ii;
+				break;
+		}
+	}
+
+	if (ii == sband->n_bitrates) {
+		dev_dbg(&priv->pcid->dev, "Wrong RxRate %x\n", *rx_rate);
+		return false;
+	}
+
+	tsf_time = (__le64 *)(skb_data + bytes_received - 12);
+	new_rsr = skb_data + bytes_received - 3;
+	rssi = skb_data + bytes_received - 2;
+	rsr = skb_data + bytes_received - 1;
+	if (*rsr & (RSR_IVLDTYP | RSR_IVLDLEN))
+		return false;
+
+	RFvRSSITodBm(priv, *rssi, &rx_dbm);
+
+	priv->byBBPreEDRSSI = (u8)rx_dbm + 1;
+	priv->current_rssi = *rssi;
+
+	skb_pull(skb, 4);
+	skb_trim(skb, frame_size);
+
+	rx_status.mactime = le64_to_cpu(*tsf_time);
+	rx_status.band = hw->conf.chandef.chan->band;
+	rx_status.signal = rx_dbm;
+	rx_status.flag = 0;
+	rx_status.freq = hw->conf.chandef.chan->center_freq;
+
+	if (!(*rsr & RSR_CRCOK))
+		rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	hdr = (struct ieee80211_hdr *)(skb->data);
+	fc = hdr->frame_control;
+
+	rx_status.rate_idx = rate_idx;
+
+	if (ieee80211_has_protected(fc)) {
+		if (priv->local_id > REV_ID_VT3253_A1)
+			rx_status.flag |= RX_FLAG_DECRYPTED;
+
+		/* Drop packet */
+		if (!(*new_rsr & NEWRSR_DECRYPTOK))
+			return false;
+	}
+
+	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
+
+	ieee80211_rx_irqsafe(priv->hw, skb);
+
+	return true;
+}
+
+bool vnt_receive_frame(struct vnt_private *priv, struct vnt_rx_desc *curr_rd)
+{
+	struct vnt_rd_info *rd_info = curr_rd->rd_info;
+	struct sk_buff *skb;
+	u16 frame_size;
+
+	skb = rd_info->skb;
+
+	dma_unmap_single(&priv->pcid->dev, rd_info->skb_dma,
+			 priv->rx_buf_sz, DMA_FROM_DEVICE);
+
+	frame_size = le16_to_cpu(curr_rd->rd1.req_count)
+			- le16_to_cpu(curr_rd->rd0.res_count);
+
+	if ((frame_size > 2364) || (frame_size < 33)) {
+		/* Frame Size error drop this packet.*/
+		dev_dbg(&priv->pcid->dev, "Wrong frame size %d\n", frame_size);
+		dev_kfree_skb_irq(skb);
+		return true;
+	}
+
+	if (vnt_rx_data(priv, skb, frame_size))
+		return true;
+
+	dev_kfree_skb_irq(skb);
+
+	return true;
+}
diff --git a/drivers/staging/vt6655/dpc.h b/drivers/staging/vt6655/dpc.h
new file mode 100644
index 000000000..40364c0ab
--- /dev/null
+++ b/drivers/staging/vt6655/dpc.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 27, 2002
+ *
+ */
+
+#ifndef __DPC_H__
+#define __DPC_H__
+
+#include "device.h"
+
+bool vnt_receive_frame(struct vnt_private *priv, struct vnt_rx_desc *curr_rd);
+
+#endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
new file mode 100644
index 000000000..1469015eb
--- /dev/null
+++ b/drivers/staging/vt6655/key.c
@@ -0,0 +1,143 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions for 802.11i Key management
+ *
+ * Author: Jerry Chen
+ *
+ * Date: May 29, 2003
+ *
+ */
+
+#include "key.h"
+#include "mac.h"
+
+static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
+			   struct ieee80211_key_conf *key, u32 key_type,
+			   u32 mode, bool onfly_latch)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u16 key_mode = 0;
+	u32 entry = 0;
+	u8 *bssid;
+	u8 key_inx = key->keyidx;
+	u8 i;
+
+	if (mac_addr)
+		bssid = mac_addr;
+	else
+		bssid = &broadcast[0];
+
+	if (key_type != VNT_KEY_DEFAULTKEY) {
+		for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
+			if (!test_bit(i, &priv->key_entry_inuse)) {
+				set_bit(i, &priv->key_entry_inuse);
+
+				key->hw_key_idx = i;
+				entry = key->hw_key_idx;
+				break;
+			}
+		}
+	}
+
+	switch (key_type) {
+	case VNT_KEY_DEFAULTKEY:
+		/* default key last entry */
+		entry = MAX_KEY_TABLE - 1;
+		key->hw_key_idx = entry;
+		fallthrough;
+	case VNT_KEY_ALLGROUP:
+		key_mode |= VNT_KEY_ALLGROUP;
+		if (onfly_latch)
+			key_mode |= VNT_KEY_ONFLY_ALL;
+		fallthrough;
+	case VNT_KEY_GROUP_ADDRESS:
+		key_mode |= mode;
+		fallthrough;
+	case VNT_KEY_GROUP:
+		key_mode |= (mode << 4);
+		key_mode |= VNT_KEY_GROUP;
+		break;
+	case  VNT_KEY_PAIRWISE:
+		key_mode |= mode;
+		key_inx = 4;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (onfly_latch)
+		key_mode |= VNT_KEY_ONFLY;
+
+	if (mode == KEY_CTL_WEP) {
+		if (key->keylen == WLAN_KEY_LEN_WEP40)
+			key->key[15] &= 0x7f;
+		if (key->keylen == WLAN_KEY_LEN_WEP104)
+			key->key[15] |= 0x80;
+	}
+
+	MACvSetKeyEntry(priv, key_mode, entry, key_inx,
+			bssid, (u32 *)key->key, priv->local_id);
+
+	return 0;
+}
+
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key)
+{
+	struct ieee80211_bss_conf *conf = &vif->bss_conf;
+	struct vnt_private *priv = hw->priv;
+	u8 *mac_addr = NULL;
+	u8 key_dec_mode = 0;
+	int ret = 0;
+	u32 u;
+
+	if (sta)
+		mac_addr = &sta->addr[0];
+
+	switch (key->cipher) {
+	case 0:
+		for (u = 0 ; u < MAX_KEY_TABLE; u++)
+			MACvDisableKeyEntry(priv, u);
+		return ret;
+
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		for (u = 0; u < MAX_KEY_TABLE; u++)
+			MACvDisableKeyEntry(priv, u);
+
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_DEFAULTKEY, KEY_CTL_WEP, true);
+
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+
+		return ret;
+	case WLAN_CIPHER_SUITE_TKIP:
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+
+		key_dec_mode = KEY_CTL_TKIP;
+
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		key_dec_mode = KEY_CTL_CCMP;
+
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+	}
+
+	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_PAIRWISE, key_dec_mode, true);
+	} else {
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_DEFAULTKEY, key_dec_mode, true);
+
+		vnt_set_keymode(hw, (u8 *)conf->bssid,
+				key, VNT_KEY_GROUP_ADDRESS, key_dec_mode, true);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
new file mode 100644
index 000000000..d88da9dfb
--- /dev/null
+++ b/drivers/staging/vt6655/key.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions for 802.11i Key management
+ *
+ * Author: Jerry Chen
+ *
+ * Date: May 29, 2003
+ *
+ */
+
+#ifndef __KEY_H__
+#define __KEY_H__
+
+#include <net/mac80211.h>
+
+/*---------------------  Export Definitions -------------------------*/
+#define MAX_GROUP_KEY       4
+#define MAX_KEY_TABLE       11
+#define MAX_KEY_LEN         32
+#define AES_KEY_LEN         16
+
+#define AUTHENTICATOR_KEY   0x10000000
+#define USE_KEYRSC          0x20000000
+#define PAIRWISE_KEY        0x40000000
+#define TRANSMIT_KEY        0x80000000
+
+#define GROUP_KEY           0x00000000
+
+#define KEY_CTL_WEP         0x00
+#define KEY_CTL_NONE        0x01
+#define KEY_CTL_TKIP        0x02
+#define KEY_CTL_CCMP        0x03
+#define KEY_CTL_INVALID     0xFF
+
+#define VNT_KEY_DEFAULTKEY	0x1
+#define VNT_KEY_GROUP_ADDRESS	0x2
+#define VNT_KEY_ALLGROUP	0x4
+#define VNT_KEY_GROUP		0x40
+#define VNT_KEY_PAIRWISE	0x00
+#define VNT_KEY_ONFLY		0x8000
+#define VNT_KEY_ONFLY_ALL	0x4000
+
+struct vnt_private;
+
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key);
+
+#endif /* __KEY_H__ */
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
new file mode 100644
index 000000000..b4ebc7d31
--- /dev/null
+++ b/drivers/staging/vt6655/mac.c
@@ -0,0 +1,851 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:  MAC routines
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ * Functions:
+ *      vt6655_mac_is_reg_bits_off - Test if All test Bits Off
+ *      vt6655_mac_set_short_retry_limit - Set 802.11 Short Retry limit
+ *      MACvSetLongRetryLimit - Set 802.11 Long Retry limit
+ *      vt6655_mac_set_loopback_mode - Set MAC Loopback Mode
+ *      vt6655_mac_save_context - Save Context of MAC Registers
+ *      vt6655_mac_restore_context - Restore Context of MAC Registers
+ *      MACbSoftwareReset - Software Reset MAC
+ *      vt6655_mac_safe_rx_off - Turn Off MAC Rx
+ *      vt6655_mac_safe_tx_off - Turn Off MAC Tx
+ *      vt6655_mac_safe_stop - Stop MAC function
+ *      MACbShutdown - Shut down MAC
+ *      MACvInitialize - Initialize MAC
+ *      MACvSetCurrRxDescAddr - Set Rx Descriptors Address
+ *      MACvSetCurrTx0DescAddr - Set Tx0 Descriptors Address
+ *      MACvSetCurrTx1DescAddr - Set Tx1 Descriptors Address
+ *      MACvTimer0MicroSDelay - Micro Second Delay Loop by MAC
+ *
+ * Revision History:
+ *      08-22-2003 Kyle Hsu     :  Porting MAC functions from sim53
+ *      09-03-2003 Bryan YC Fan :  Add MACvClearBusSusInd()&
+ *				   MACvEnableBusSusEn()
+ *      09-18-2003 Jerry Chen   :  Add MACvSetKeyEntry & MACvDisableKeyEntry
+ *
+ */
+
+#include "mac.h"
+
+void vt6655_mac_reg_bits_on(void __iomem *iobase, const u8 reg_offset, const u8 bit_mask)
+{
+	unsigned char reg_value;
+
+	reg_value = ioread8(iobase + reg_offset);
+	iowrite8(reg_value | bit_mask, iobase + reg_offset);
+}
+
+void vt6655_mac_word_reg_bits_on(void __iomem *iobase, const u8 reg_offset, const u16 bit_mask)
+{
+	unsigned short reg_value;
+
+	reg_value = ioread16(iobase + reg_offset);
+	iowrite16(reg_value | (bit_mask), iobase + reg_offset);
+}
+
+void vt6655_mac_reg_bits_off(void __iomem *iobase, const u8 reg_offset, const u8 bit_mask)
+{
+	unsigned char reg_value;
+
+	reg_value = ioread8(iobase + reg_offset);
+	iowrite8(reg_value & ~(bit_mask), iobase + reg_offset);
+}
+
+void vt6655_mac_word_reg_bits_off(void __iomem *iobase, const u8 reg_offset, const u16 bit_mask)
+{
+	unsigned short reg_value;
+
+	reg_value = ioread16(iobase + reg_offset);
+	iowrite16(reg_value & ~(bit_mask), iobase + reg_offset);
+}
+
+static void vt6655_mac_clear_stck_ds(void __iomem *iobase)
+{
+	u8 reg_value;
+
+	reg_value = ioread8(iobase + MAC_REG_STICKHW);
+	reg_value = reg_value & 0xFC;
+	iowrite8(reg_value, iobase + MAC_REG_STICKHW);
+}
+
+/*
+ * Description:
+ *      Test if all test bits off
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      reg_offset - Offset of MAC Register
+ *      mask       - Test bits
+ *  Out:
+ *      none
+ *
+ * Return Value: true if all test bits Off; otherwise false
+ *
+ */
+static bool vt6655_mac_is_reg_bits_off(struct vnt_private *priv,
+				       unsigned char reg_offset,
+				       unsigned char mask)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	return !(ioread8(io_base + reg_offset) & mask);
+}
+
+/*
+ * Description:
+ *      Set 802.11 Short Retry Limit
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      retry_limit - Retry Limit
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void vt6655_mac_set_short_retry_limit(struct vnt_private *priv, unsigned char retry_limit)
+{
+	void __iomem *io_base = priv->port_offset;
+	/* set SRT */
+	iowrite8(retry_limit, io_base + MAC_REG_SRT);
+}
+
+/*
+ * Description:
+ *      Set 802.11 Long Retry Limit
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      byRetryLimit- Retry Limit
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void MACvSetLongRetryLimit(struct vnt_private *priv,
+			   unsigned char byRetryLimit)
+{
+	void __iomem *io_base = priv->port_offset;
+	/* set LRT */
+	iowrite8(byRetryLimit, io_base + MAC_REG_LRT);
+}
+
+/*
+ * Description:
+ *      Set MAC Loopback mode
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *      loopback_mode  - Loopback Mode
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+static void vt6655_mac_set_loopback_mode(struct vnt_private *priv, u8 loopback_mode)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	loopback_mode <<= 6;
+	/* set TCR */
+	iowrite8((ioread8(io_base + MAC_REG_TEST) & 0x3f) | loopback_mode, io_base + MAC_REG_TEST);
+}
+
+/*
+ * Description:
+ *      Save MAC registers to context buffer
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      cxt_buf   - Context buffer
+ *
+ * Return Value: none
+ *
+ */
+static void vt6655_mac_save_context(struct vnt_private *priv, u8 *cxt_buf)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	/* read page0 register */
+	memcpy_fromio(cxt_buf, io_base, MAC_MAX_CONTEXT_SIZE_PAGE0);
+
+	VT6655_MAC_SELECT_PAGE1(io_base);
+
+	/* read page1 register */
+	memcpy_fromio(cxt_buf + MAC_MAX_CONTEXT_SIZE_PAGE0, io_base,
+		      MAC_MAX_CONTEXT_SIZE_PAGE1);
+
+	VT6655_MAC_SELECT_PAGE0(io_base);
+}
+
+/*
+ * Description:
+ *      Restore MAC registers from context buffer
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      cxt_buf   - Context buffer
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+static void vt6655_mac_restore_context(struct vnt_private *priv, u8 *cxt_buf)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	VT6655_MAC_SELECT_PAGE1(io_base);
+	/* restore page1 */
+	memcpy_toio(io_base, cxt_buf + MAC_MAX_CONTEXT_SIZE_PAGE0,
+		    MAC_MAX_CONTEXT_SIZE_PAGE1);
+
+	VT6655_MAC_SELECT_PAGE0(io_base);
+
+	/* restore RCR,TCR,IMR... */
+	memcpy_toio(io_base + MAC_REG_RCR, cxt_buf + MAC_REG_RCR,
+		    MAC_REG_ISR - MAC_REG_RCR);
+
+	/* restore MAC Config. */
+	memcpy_toio(io_base + MAC_REG_LRT, cxt_buf + MAC_REG_LRT,
+		    MAC_REG_PAGE1SEL - MAC_REG_LRT);
+
+	iowrite8(*(cxt_buf + MAC_REG_CFG), io_base + MAC_REG_CFG);
+
+	/* restore PS Config. */
+	memcpy_toio(io_base + MAC_REG_PSCFG, cxt_buf + MAC_REG_PSCFG,
+		    MAC_REG_BBREGCTL - MAC_REG_PSCFG);
+
+	/* restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR */
+	iowrite32(*(u32 *)(cxt_buf + MAC_REG_TXDMAPTR0),
+		  io_base + MAC_REG_TXDMAPTR0);
+	iowrite32(*(u32 *)(cxt_buf + MAC_REG_AC0DMAPTR),
+		  io_base + MAC_REG_AC0DMAPTR);
+	iowrite32(*(u32 *)(cxt_buf + MAC_REG_BCNDMAPTR),
+		  io_base + MAC_REG_BCNDMAPTR);
+	iowrite32(*(u32 *)(cxt_buf + MAC_REG_RXDMAPTR0),
+		  io_base + MAC_REG_RXDMAPTR0);
+	iowrite32(*(u32 *)(cxt_buf + MAC_REG_RXDMAPTR1),
+		  io_base + MAC_REG_RXDMAPTR1);
+}
+
+/*
+ * Description:
+ *      Software Reset MAC
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if Reset Success; otherwise false
+ *
+ */
+bool MACbSoftwareReset(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+
+	/* turn on HOSTCR_SOFTRST, just write 0x01 to reset */
+	iowrite8(0x01, io_base + MAC_REG_HOSTCR);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_HOSTCR) & HOSTCR_SOFTRST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT)
+		return false;
+	return true;
+}
+
+/*
+ * Description:
+ *      save some important register's value, then do reset, then restore
+ *	register's value
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+static void vt6655_mac_save_soft_reset(struct vnt_private *priv)
+{
+	u8 tmp_reg_data[MAC_MAX_CONTEXT_SIZE_PAGE0 + MAC_MAX_CONTEXT_SIZE_PAGE1];
+
+	/* PATCH....
+	 * save some important register's value, then do
+	 * reset, then restore register's value
+	 */
+	/* save MAC context */
+	vt6655_mac_save_context(priv, tmp_reg_data);
+	/* do reset */
+	MACbSoftwareReset(priv);
+	/* restore MAC context, except CR0 */
+	vt6655_mac_restore_context(priv, tmp_reg_data);
+}
+
+/*
+ * Description:
+ *      Turn Off MAC Rx
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+static bool vt6655_mac_safe_rx_off(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+
+	/* turn off wow temp for turn off Rx safely */
+
+	/* Clear RX DMA0,1 */
+	iowrite32(DMACTL_CLRRUN, io_base + MAC_REG_RXDMACTL0);
+	iowrite32(DMACTL_CLRRUN, io_base + MAC_REG_RXDMACTL1);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread32(io_base + MAC_REG_RXDMACTL0) & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x10)\n");
+		return false;
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread32(io_base + MAC_REG_RXDMACTL1) & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x11)\n");
+		return false;
+	}
+
+	/* try to safe shutdown RX */
+	vt6655_mac_reg_bits_off(io_base, MAC_REG_HOSTCR, HOSTCR_RXON);
+	/* W_MAX_TIMEOUT is the timeout period */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_HOSTCR) & HOSTCR_RXONST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x12)\n");
+		return false;
+	}
+	return true;
+}
+
+/*
+ * Description:
+ *      Turn Off MAC Tx
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+static bool vt6655_mac_safe_tx_off(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+
+	/* Clear TX DMA */
+	/* Tx0 */
+	iowrite32(DMACTL_CLRRUN, io_base + MAC_REG_TXDMACTL0);
+	/* AC0 */
+	iowrite32(DMACTL_CLRRUN, io_base + MAC_REG_AC0DMACTL);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread32(io_base + MAC_REG_TXDMACTL0) & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x20)\n");
+		return false;
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread32(io_base + MAC_REG_AC0DMACTL) & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x21)\n");
+		return false;
+	}
+
+	/* try to safe shutdown TX */
+	vt6655_mac_reg_bits_off(io_base, MAC_REG_HOSTCR, HOSTCR_TXON);
+
+	/* W_MAX_TIMEOUT is the timeout period */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_HOSTCR) & HOSTCR_TXONST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x24)\n");
+		return false;
+	}
+	return true;
+}
+
+/*
+ * Description:
+ *      Stop MAC function
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+static bool vt6655_mac_safe_stop(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	vt6655_mac_reg_bits_off(io_base, MAC_REG_TCR, TCR_AUTOBCNTX);
+
+	if (!vt6655_mac_safe_rx_off(priv)) {
+		pr_debug(" vt6655_mac_safe_rx_off == false)\n");
+		vt6655_mac_save_soft_reset(priv);
+		return false;
+	}
+	if (!vt6655_mac_safe_tx_off(priv)) {
+		pr_debug(" vt6655_mac_safe_tx_off == false)\n");
+		vt6655_mac_save_soft_reset(priv);
+		return false;
+	}
+
+	vt6655_mac_reg_bits_off(io_base, MAC_REG_HOSTCR, HOSTCR_MACEN);
+
+	return true;
+}
+
+/*
+ * Description:
+ *      Shut Down MAC
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+bool MACbShutdown(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	/* disable MAC IMR */
+	iowrite32(0, io_base + MAC_REG_IMR);
+	vt6655_mac_set_loopback_mode(priv, MAC_LB_INTERNAL);
+	/* stop the adapter */
+	if (!vt6655_mac_safe_stop(priv)) {
+		vt6655_mac_set_loopback_mode(priv, MAC_LB_NONE);
+		return false;
+	}
+	vt6655_mac_set_loopback_mode(priv, MAC_LB_NONE);
+	return true;
+}
+
+/*
+ * Description:
+ *      Initialize MAC
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void MACvInitialize(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	/* clear sticky bits */
+	vt6655_mac_clear_stck_ds(io_base);
+	/* disable force PME-enable */
+	iowrite8(PME_OVR, io_base + MAC_REG_PMC1);
+	/* only 3253 A */
+
+	/* do reset */
+	MACbSoftwareReset(priv);
+
+	/* reset TSF counter */
+	iowrite8(TFTCTL_TSFCNTRST, io_base + MAC_REG_TFTCTL);
+	/* enable TSF counter */
+	iowrite8(TFTCTL_TSFCNTREN, io_base + MAC_REG_TFTCTL);
+}
+
+/*
+ * Description:
+ *      Set the chip with current rx descriptor address
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *      curr_desc_addr  - Descriptor Address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void vt6655_mac_set_curr_rx_0_desc_addr(struct vnt_private *priv, u32 curr_desc_addr)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+	unsigned char org_dma_ctl;
+
+	org_dma_ctl = ioread8(io_base + MAC_REG_RXDMACTL0);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_RXDMACTL0 + 2);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_RXDMACTL0) & DMACTL_RUN))
+			break;
+	}
+
+	iowrite32(curr_desc_addr, io_base + MAC_REG_RXDMAPTR0);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_RXDMACTL0);
+}
+
+/*
+ * Description:
+ *      Set the chip with current rx descriptor address
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *      curr_desc_addr  - Descriptor Address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void vt6655_mac_set_curr_rx_1_desc_addr(struct vnt_private *priv, u32 curr_desc_addr)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+	unsigned char org_dma_ctl;
+
+	org_dma_ctl = ioread8(io_base + MAC_REG_RXDMACTL1);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_RXDMACTL1 + 2);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_RXDMACTL1) & DMACTL_RUN))
+			break;
+	}
+
+	iowrite32(curr_desc_addr, io_base + MAC_REG_RXDMAPTR1);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_RXDMACTL1);
+}
+
+/*
+ * Description:
+ *      Set the chip with current tx0 descriptor address
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *      curr_desc_addr  - Descriptor Address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+static void vt6655_mac_set_curr_tx_0_desc_addr_ex(struct vnt_private *priv, u32 curr_desc_addr)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+	unsigned char org_dma_ctl;
+
+	org_dma_ctl = ioread8(io_base + MAC_REG_TXDMACTL0);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_TXDMACTL0 + 2);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_TXDMACTL0) & DMACTL_RUN))
+			break;
+	}
+
+	iowrite32(curr_desc_addr, io_base + MAC_REG_TXDMAPTR0);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_TXDMACTL0);
+}
+
+/*
+ * Description:
+ *      Set the chip with current AC0 descriptor address
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *      curr_desc_addr  - Descriptor Address
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+/* TxDMA1 = AC0DMA */
+static void vt6655_mac_set_curr_ac_0_desc_addr_ex(struct vnt_private *priv, u32 curr_desc_addr)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short ww;
+	unsigned char org_dma_ctl;
+
+	org_dma_ctl = ioread8(io_base + MAC_REG_AC0DMACTL);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_AC0DMACTL + 2);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (!(ioread8(io_base + MAC_REG_AC0DMACTL) & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT)
+		pr_debug(" DBG_PORT80(0x26)\n");
+	iowrite32(curr_desc_addr, io_base + MAC_REG_AC0DMAPTR);
+	if (org_dma_ctl & DMACTL_RUN)
+		iowrite8(DMACTL_RUN, io_base + MAC_REG_AC0DMACTL);
+}
+
+void vt6655_mac_set_curr_tx_desc_addr(int tx_type, struct vnt_private *priv, u32 curr_desc_addr)
+{
+	if (tx_type == TYPE_AC0DMA)
+		vt6655_mac_set_curr_ac_0_desc_addr_ex(priv, curr_desc_addr);
+	else if (tx_type == TYPE_TXDMA0)
+		vt6655_mac_set_curr_tx_0_desc_addr_ex(priv, curr_desc_addr);
+}
+
+/*
+ * Description:
+ *      Micro Second Delay via MAC
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      uDelay      - Delay time (timer resolution is 4 us)
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void MACvTimer0MicroSDelay(struct vnt_private *priv, unsigned int uDelay)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned char byValue;
+	unsigned int uu, ii;
+
+	iowrite8(0, io_base + MAC_REG_TMCTL0);
+	iowrite32(uDelay, io_base + MAC_REG_TMDATA0);
+	iowrite8((TMCTL_TMD | TMCTL_TE), io_base + MAC_REG_TMCTL0);
+	for (ii = 0; ii < 66; ii++) {  /* assume max PCI clock is 66Mhz */
+		for (uu = 0; uu < uDelay; uu++) {
+			byValue = ioread8(io_base + MAC_REG_TMCTL0);
+			if ((byValue == 0) ||
+			    (byValue & TMCTL_TSUSP)) {
+				iowrite8(0, io_base + MAC_REG_TMCTL0);
+				return;
+			}
+		}
+	}
+	iowrite8(0, io_base + MAC_REG_TMCTL0);
+}
+
+/*
+ * Description:
+ *      Micro Second One shot timer via MAC
+ *
+ * Parameters:
+ *  In:
+ *      io_base    - Base Address for MAC
+ *      uDelay      - Delay time
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void MACvOneShotTimer1MicroSec(struct vnt_private *priv,
+			       unsigned int uDelayTime)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	iowrite8(0, io_base + MAC_REG_TMCTL1);
+	iowrite32(uDelayTime, io_base + MAC_REG_TMDATA1);
+	iowrite8((TMCTL_TMD | TMCTL_TE), io_base + MAC_REG_TMCTL1);
+}
+
+void MACvSetMISCFifo(struct vnt_private *priv, unsigned short offset,
+		     u32 data)
+{
+	void __iomem *io_base = priv->port_offset;
+
+	if (offset > 273)
+		return;
+	iowrite16(offset, io_base + MAC_REG_MISCFFNDEX);
+	iowrite32(data, io_base + MAC_REG_MISCFFDATA);
+	iowrite16(MISCFFCTL_WRITE, io_base + MAC_REG_MISCFFCTL);
+}
+
+bool MACbPSWakeup(struct vnt_private *priv)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned int ww;
+	/* Read PSCTL */
+	if (vt6655_mac_is_reg_bits_off(priv, MAC_REG_PSCTL, PSCTL_PS))
+		return true;
+
+	/* Disable PS */
+	vt6655_mac_reg_bits_off(io_base, MAC_REG_PSCTL, PSCTL_PSEN);
+
+	/* Check if SyncFlushOK */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		if (ioread8(io_base + MAC_REG_PSCTL) & PSCTL_WAKEDONE)
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		pr_debug(" DBG_PORT80(0x33)\n");
+		return false;
+	}
+	return true;
+}
+
+/*
+ * Description:
+ *      Set the Key by MISCFIFO
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+
+void MACvSetKeyEntry(struct vnt_private *priv, unsigned short wKeyCtl,
+		     unsigned int uEntryIdx, unsigned int uKeyIdx,
+		     unsigned char *pbyAddr, u32 *pdwKey,
+		     unsigned char local_id)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short offset;
+	u32 data;
+	int     ii;
+
+	if (local_id <= 1)
+		return;
+
+	offset = MISCFIFO_KEYETRY0;
+	offset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
+
+	data = 0;
+	data |= wKeyCtl;
+	data <<= 16;
+	data |= MAKEWORD(*(pbyAddr + 4), *(pbyAddr + 5));
+	pr_debug("1. offset: %d, Data: %X, KeyCtl:%X\n",
+		 offset, data, wKeyCtl);
+
+	iowrite16(offset, io_base + MAC_REG_MISCFFNDEX);
+	iowrite32(data, io_base + MAC_REG_MISCFFDATA);
+	iowrite16(MISCFFCTL_WRITE, io_base + MAC_REG_MISCFFCTL);
+	offset++;
+
+	data = 0;
+	data |= *(pbyAddr + 3);
+	data <<= 8;
+	data |= *(pbyAddr + 2);
+	data <<= 8;
+	data |= *(pbyAddr + 1);
+	data <<= 8;
+	data |= *pbyAddr;
+	pr_debug("2. offset: %d, Data: %X\n", offset, data);
+
+	iowrite16(offset, io_base + MAC_REG_MISCFFNDEX);
+	iowrite32(data, io_base + MAC_REG_MISCFFDATA);
+	iowrite16(MISCFFCTL_WRITE, io_base + MAC_REG_MISCFFCTL);
+	offset++;
+
+	offset += (uKeyIdx * 4);
+	for (ii = 0; ii < 4; ii++) {
+		/* always push 128 bits */
+		pr_debug("3.(%d) offset: %d, Data: %X\n",
+			 ii, offset + ii, *pdwKey);
+		iowrite16(offset + ii, io_base + MAC_REG_MISCFFNDEX);
+		iowrite32(*pdwKey++, io_base + MAC_REG_MISCFFDATA);
+		iowrite16(MISCFFCTL_WRITE, io_base + MAC_REG_MISCFFCTL);
+	}
+}
+
+/*
+ * Description:
+ *      Disable the Key Entry by MISCFIFO
+ *
+ * Parameters:
+ *  In:
+ *      io_base        - Base Address for MAC
+ *
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+void MACvDisableKeyEntry(struct vnt_private *priv, unsigned int uEntryIdx)
+{
+	void __iomem *io_base = priv->port_offset;
+	unsigned short offset;
+
+	offset = MISCFIFO_KEYETRY0;
+	offset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
+
+	iowrite16(offset, io_base + MAC_REG_MISCFFNDEX);
+	iowrite32(0, io_base + MAC_REG_MISCFFDATA);
+	iowrite16(MISCFFCTL_WRITE, io_base + MAC_REG_MISCFFCTL);
+}
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
new file mode 100644
index 000000000..acf931c3f
--- /dev/null
+++ b/drivers/staging/vt6655/mac.h
@@ -0,0 +1,580 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: MAC routines
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ * Revision History:
+ *      07-01-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-25-2003 Kyle Hsu:      Porting MAC functions from sim53.
+ *      09-03-2003 Bryan YC Fan:  Add vt6655_mac_dis_protect_md & vt6655_mac_en_protect_md
+ */
+
+#ifndef __MAC_H__
+#define __MAC_H__
+
+#include "device.h"
+
+/*---------------------  Export Definitions -------------------------*/
+/* Registers in the MAC */
+#define MAC_MAX_CONTEXT_SIZE_PAGE0  256
+#define MAC_MAX_CONTEXT_SIZE_PAGE1  128
+
+/* Registers not related to 802.11b */
+#define MAC_REG_BCFG0       0x00
+#define MAC_REG_BCFG1       0x01
+#define MAC_REG_FCR0        0x02
+#define MAC_REG_FCR1        0x03
+#define MAC_REG_BISTCMD     0x04
+#define MAC_REG_BISTSR0     0x05
+#define MAC_REG_BISTSR1     0x06
+#define MAC_REG_BISTSR2     0x07
+#define MAC_REG_I2MCSR      0x08
+#define MAC_REG_I2MTGID     0x09
+#define MAC_REG_I2MTGAD     0x0A
+#define MAC_REG_I2MCFG      0x0B
+#define MAC_REG_I2MDIPT     0x0C
+#define MAC_REG_I2MDOPT     0x0E
+#define MAC_REG_PMC0        0x10
+#define MAC_REG_PMC1        0x11
+#define MAC_REG_STICKHW     0x12
+#define MAC_REG_LOCALID     0x14
+#define MAC_REG_TESTCFG     0x15
+#define MAC_REG_JUMPER0     0x16
+#define MAC_REG_JUMPER1     0x17
+#define MAC_REG_TMCTL0      0x18
+#define MAC_REG_TMCTL1      0x19
+#define MAC_REG_TMDATA0     0x1C
+
+/* MAC Parameter related */
+#define MAC_REG_LRT         0x20
+#define MAC_REG_SRT         0x21
+#define MAC_REG_SIFS        0x22
+#define MAC_REG_DIFS        0x23
+#define MAC_REG_EIFS        0x24
+#define MAC_REG_SLOT        0x25
+#define MAC_REG_BI          0x26
+#define MAC_REG_CWMAXMIN0   0x28
+#define MAC_REG_LINKOFFTOTM 0x2A
+#define MAC_REG_SWTMOT      0x2B
+#define MAC_REG_MIBCNTR     0x2C
+#define MAC_REG_RTSOKCNT    0x2C
+#define MAC_REG_RTSFAILCNT  0x2D
+#define MAC_REG_ACKFAILCNT  0x2E
+#define MAC_REG_FCSERRCNT   0x2F
+
+/* TSF Related */
+#define MAC_REG_TSFCNTR     0x30
+#define MAC_REG_NEXTTBTT    0x38
+#define MAC_REG_TSFOFST     0x40
+#define MAC_REG_TFTCTL      0x48
+
+/* WMAC Control/Status Related */
+#define MAC_REG_ENCFG       0x4C
+#define MAC_REG_PAGE1SEL    0x4F
+#define MAC_REG_CFG         0x50
+#define MAC_REG_TEST        0x52
+#define MAC_REG_HOSTCR      0x54
+#define MAC_REG_MACCR       0x55
+#define MAC_REG_RCR         0x56
+#define MAC_REG_TCR         0x57
+#define MAC_REG_IMR         0x58
+#define MAC_REG_ISR         0x5C
+
+/* Power Saving Related */
+#define MAC_REG_PSCFG       0x60
+#define MAC_REG_PSCTL       0x61
+#define MAC_REG_PSPWRSIG    0x62
+#define MAC_REG_BBCR13      0x63
+#define MAC_REG_AIDATIM     0x64
+#define MAC_REG_PWBT        0x66
+#define MAC_REG_WAKEOKTMR   0x68
+#define MAC_REG_CALTMR      0x69
+#define MAC_REG_SYNSPACCNT  0x6A
+#define MAC_REG_WAKSYNOPT   0x6B
+
+/* Baseband/IF Control Group */
+#define MAC_REG_BBREGCTL    0x6C
+#define MAC_REG_CHANNEL     0x6D
+#define MAC_REG_BBREGADR    0x6E
+#define MAC_REG_BBREGDATA   0x6F
+#define MAC_REG_IFREGCTL    0x70
+#define MAC_REG_IFDATA      0x71
+#define MAC_REG_ITRTMSET    0x74
+#define MAC_REG_PAPEDELAY   0x77
+#define MAC_REG_SOFTPWRCTL  0x78
+#define MAC_REG_GPIOCTL0    0x7A
+#define MAC_REG_GPIOCTL1    0x7B
+
+/* MAC DMA Related Group */
+#define MAC_REG_TXDMACTL0   0x7C
+#define MAC_REG_TXDMAPTR0   0x80
+#define MAC_REG_AC0DMACTL   0x84
+#define MAC_REG_AC0DMAPTR   0x88
+#define MAC_REG_BCNDMACTL   0x8C
+#define MAC_REG_BCNDMAPTR   0x90
+#define MAC_REG_RXDMACTL0   0x94
+#define MAC_REG_RXDMAPTR0   0x98
+#define MAC_REG_RXDMACTL1   0x9C
+#define MAC_REG_RXDMAPTR1   0xA0
+#define MAC_REG_SYNCDMACTL  0xA4
+#define MAC_REG_SYNCDMAPTR  0xA8
+#define MAC_REG_ATIMDMACTL  0xAC
+#define MAC_REG_ATIMDMAPTR  0xB0
+
+/* MiscFF PIO related */
+#define MAC_REG_MISCFFNDEX  0xB4
+#define MAC_REG_MISCFFCTL   0xB6
+#define MAC_REG_MISCFFDATA  0xB8
+
+/* Extend SW Timer */
+#define MAC_REG_TMDATA1     0xBC
+
+/* WOW Related Group */
+#define MAC_REG_WAKEUPEN0   0xC0
+#define MAC_REG_WAKEUPEN1   0xC1
+#define MAC_REG_WAKEUPSR0   0xC2
+#define MAC_REG_WAKEUPSR1   0xC3
+#define MAC_REG_WAKE128_0   0xC4
+#define MAC_REG_WAKE128_1   0xD4
+#define MAC_REG_WAKE128_2   0xE4
+#define MAC_REG_WAKE128_3   0xF4
+
+/************** Page 1 ******************/
+#define MAC_REG_CRC_128_0   0x04
+#define MAC_REG_CRC_128_1   0x06
+#define MAC_REG_CRC_128_2   0x08
+#define MAC_REG_CRC_128_3   0x0A
+
+/* MAC Configuration Group */
+#define MAC_REG_PAR0        0x0C
+#define MAC_REG_PAR4        0x10
+#define MAC_REG_BSSID0      0x14
+#define MAC_REG_BSSID4      0x18
+#define MAC_REG_MAR0        0x1C
+#define MAC_REG_MAR4        0x20
+
+/* MAC RSPPKT INFO Group */
+#define MAC_REG_RSPINF_B_1  0x24
+#define MAC_REG_RSPINF_B_2  0x28
+#define MAC_REG_RSPINF_B_5  0x2C
+#define MAC_REG_RSPINF_B_11 0x30
+#define MAC_REG_RSPINF_A_6  0x34
+#define MAC_REG_RSPINF_A_9  0x36
+#define MAC_REG_RSPINF_A_12 0x38
+#define MAC_REG_RSPINF_A_18 0x3A
+#define MAC_REG_RSPINF_A_24 0x3C
+#define MAC_REG_RSPINF_A_36 0x3E
+#define MAC_REG_RSPINF_A_48 0x40
+#define MAC_REG_RSPINF_A_54 0x42
+#define MAC_REG_RSPINF_A_72 0x44
+
+/* 802.11h relative */
+#define MAC_REG_QUIETINIT   0x60
+#define MAC_REG_QUIETGAP    0x62
+#define MAC_REG_QUIETDUR    0x64
+#define MAC_REG_MSRCTL      0x66
+#define MAC_REG_MSRBBSTS    0x67
+#define MAC_REG_MSRSTART    0x68
+#define MAC_REG_MSRDURATION 0x70
+#define MAC_REG_CCAFRACTION 0x72
+#define MAC_REG_PWRCCK      0x73
+#define MAC_REG_PWROFDM     0x7C
+
+/* Bits in the BCFG0 register */
+#define BCFG0_PERROFF       0x40
+#define BCFG0_MRDMDIS       0x20
+#define BCFG0_MRDLDIS       0x10
+#define BCFG0_MWMEN         0x08
+#define BCFG0_VSERREN       0x02
+#define BCFG0_LATMEN        0x01
+
+/* Bits in the BCFG1 register */
+#define BCFG1_CFUNOPT       0x80
+#define BCFG1_CREQOPT       0x40
+#define BCFG1_DMA8          0x10
+#define BCFG1_ARBITOPT      0x08
+#define BCFG1_PCIMEN        0x04
+#define BCFG1_MIOEN         0x02
+#define BCFG1_CISDLYEN      0x01
+
+/* Bits in RAMBIST registers */
+#define BISTCMD_TSTPAT5     0x00
+#define BISTCMD_TSTPATA     0x80
+#define BISTCMD_TSTERR      0x20
+#define BISTCMD_TSTPATF     0x18
+#define BISTCMD_TSTPAT0     0x10
+#define BISTCMD_TSTMODE     0x04
+#define BISTCMD_TSTITTX     0x03
+#define BISTCMD_TSTATRX     0x02
+#define BISTCMD_TSTATTX     0x01
+#define BISTCMD_TSTRX       0x00
+#define BISTSR0_BISTGO      0x01
+#define BISTSR1_TSTSR       0x01
+#define BISTSR2_CMDPRTEN    0x02
+#define BISTSR2_RAMTSTEN    0x01
+
+/* Bits in the I2MCFG EEPROM register */
+#define I2MCFG_BOUNDCTL     0x80
+#define I2MCFG_WAITCTL      0x20
+#define I2MCFG_SCLOECTL     0x10
+#define I2MCFG_WBUSYCTL     0x08
+#define I2MCFG_NORETRY      0x04
+#define I2MCFG_I2MLDSEQ     0x02
+#define I2MCFG_I2CMFAST     0x01
+
+/* Bits in the I2MCSR EEPROM register */
+#define I2MCSR_EEMW         0x80
+#define I2MCSR_EEMR         0x40
+#define I2MCSR_AUTOLD       0x08
+#define I2MCSR_NACK         0x02
+#define I2MCSR_DONE         0x01
+
+/* Bits in the PMC1 register */
+#define SPS_RST             0x80
+#define PCISTIKY            0x40
+#define PME_OVR             0x02
+
+/* Bits in the STICKYHW register */
+#define STICKHW_DS1_SHADOW  0x02
+#define STICKHW_DS0_SHADOW  0x01
+
+/* Bits in the TMCTL register */
+#define TMCTL_TSUSP         0x04
+#define TMCTL_TMD           0x02
+#define TMCTL_TE            0x01
+
+/* Bits in the TFTCTL register */
+#define TFTCTL_HWUTSF       0x80
+#define TFTCTL_TBTTSYNC     0x40
+#define TFTCTL_HWUTSFEN     0x20
+#define TFTCTL_TSFCNTRRD    0x10
+#define TFTCTL_TBTTSYNCEN   0x08
+#define TFTCTL_TSFSYNCEN    0x04
+#define TFTCTL_TSFCNTRST    0x02
+#define TFTCTL_TSFCNTREN    0x01
+
+/* Bits in the EnhanceCFG register */
+#define ENCFG_BARKERPREAM   0x00020000
+#define ENCFG_NXTBTTCFPSTR  0x00010000
+#define ENCFG_BCNSUSCLR     0x00000200
+#define ENCFG_BCNSUSIND     0x00000100
+#define ENCFG_CFP_PROTECTEN 0x00000040
+#define ENCFG_PROTECTMD     0x00000020
+#define ENCFG_HWPARCFP      0x00000010
+#define ENCFG_CFNULRSP      0x00000004
+#define ENCFG_BBTYPE_MASK   0x00000003
+#define ENCFG_BBTYPE_G      0x00000002
+#define ENCFG_BBTYPE_B      0x00000001
+#define ENCFG_BBTYPE_A      0x00000000
+
+/* Bits in the Page1Sel register */
+#define PAGE1_SEL           0x01
+
+/* Bits in the CFG register */
+#define CFG_TKIPOPT         0x80
+#define CFG_RXDMAOPT        0x40
+#define CFG_TMOT_SW         0x20
+#define CFG_TMOT_HWLONG     0x10
+#define CFG_TMOT_HW         0x00
+#define CFG_CFPENDOPT       0x08
+#define CFG_BCNSUSEN        0x04
+#define CFG_NOTXTIMEOUT     0x02
+#define CFG_NOBUFOPT        0x01
+
+/* Bits in the TEST register */
+#define TEST_LBEXT          0x80
+#define TEST_LBINT          0x40
+#define TEST_LBNONE         0x00
+#define TEST_SOFTINT        0x20
+#define TEST_CONTTX         0x10
+#define TEST_TXPE           0x08
+#define TEST_NAVDIS         0x04
+#define TEST_NOCTS          0x02
+#define TEST_NOACK          0x01
+
+/* Bits in the HOSTCR register */
+#define HOSTCR_TXONST       0x80
+#define HOSTCR_RXONST       0x40
+#define HOSTCR_ADHOC        0x20 /* Network Type 1 = Ad-hoc */
+#define HOSTCR_AP           0x10 /* Port Type 1 = AP */
+#define HOSTCR_TXON         0x08 /* 0000 1000 */
+#define HOSTCR_RXON         0x04 /* 0000 0100 */
+#define HOSTCR_MACEN        0x02 /* 0000 0010 */
+#define HOSTCR_SOFTRST      0x01 /* 0000 0001 */
+
+/* Bits in the MACCR register */
+#define MACCR_SYNCFLUSHOK   0x04
+#define MACCR_SYNCFLUSH     0x02
+#define MACCR_CLRNAV        0x01
+
+/* Bits in the MAC_REG_GPIOCTL0 register */
+#define LED_ACTSET           0x01
+#define LED_RFOFF            0x02
+#define LED_NOCONNECT        0x04
+
+/* Bits in the RCR register */
+#define RCR_SSID            0x80
+#define RCR_RXALLTYPE       0x40
+#define RCR_UNICAST         0x20
+#define RCR_BROADCAST       0x10
+#define RCR_MULTICAST       0x08
+#define RCR_WPAERR          0x04
+#define RCR_ERRCRC          0x02
+#define RCR_BSSID           0x01
+
+/* Bits in the TCR register */
+#define TCR_SYNCDCFOPT      0x02
+#define TCR_AUTOBCNTX       0x01 /* Beacon automatically transmit enable */
+
+/* Bits in the IMR register */
+#define IMR_MEASURESTART    0x80000000
+#define IMR_QUIETSTART      0x20000000
+#define IMR_RADARDETECT     0x10000000
+#define IMR_MEASUREEND      0x08000000
+#define IMR_SOFTTIMER1      0x00200000
+#define IMR_RXDMA1          0x00001000 /* 0000 0000 0001 0000 0000 0000 */
+#define IMR_RXNOBUF         0x00000800
+#define IMR_MIBNEARFULL     0x00000400
+#define IMR_SOFTINT         0x00000200
+#define IMR_FETALERR        0x00000100
+#define IMR_WATCHDOG        0x00000080
+#define IMR_SOFTTIMER       0x00000040
+#define IMR_GPIO            0x00000020
+#define IMR_TBTT            0x00000010
+#define IMR_RXDMA0          0x00000008
+#define IMR_BNTX            0x00000004
+#define IMR_AC0DMA          0x00000002
+#define IMR_TXDMA0          0x00000001
+
+/* Bits in the ISR register */
+#define ISR_MEASURESTART    0x80000000
+#define ISR_QUIETSTART      0x20000000
+#define ISR_RADARDETECT     0x10000000
+#define ISR_MEASUREEND      0x08000000
+#define ISR_SOFTTIMER1      0x00200000
+#define ISR_RXDMA1          0x00001000 /* 0000 0000 0001 0000 0000 0000 */
+#define ISR_RXNOBUF         0x00000800 /* 0000 0000 0000 1000 0000 0000 */
+#define ISR_MIBNEARFULL     0x00000400 /* 0000 0000 0000 0100 0000 0000 */
+#define ISR_SOFTINT         0x00000200
+#define ISR_FETALERR        0x00000100
+#define ISR_WATCHDOG        0x00000080
+#define ISR_SOFTTIMER       0x00000040
+#define ISR_GPIO            0x00000020
+#define ISR_TBTT            0x00000010
+#define ISR_RXDMA0          0x00000008
+#define ISR_BNTX            0x00000004
+#define ISR_AC0DMA          0x00000002
+#define ISR_TXDMA0          0x00000001
+
+/* Bits in the PSCFG register */
+#define PSCFG_PHILIPMD      0x40
+#define PSCFG_WAKECALEN     0x20
+#define PSCFG_WAKETMREN     0x10
+#define PSCFG_BBPSPROG      0x08
+#define PSCFG_WAKESYN       0x04
+#define PSCFG_SLEEPSYN      0x02
+#define PSCFG_AUTOSLEEP     0x01
+
+/* Bits in the PSCTL register */
+#define PSCTL_WAKEDONE      0x20
+#define PSCTL_PS            0x10
+#define PSCTL_GO2DOZE       0x08
+#define PSCTL_LNBCN         0x04
+#define PSCTL_ALBCN         0x02
+#define PSCTL_PSEN          0x01
+
+/* Bits in the PSPWSIG register */
+#define PSSIG_WPE3          0x80
+#define PSSIG_WPE2          0x40
+#define PSSIG_WPE1          0x20
+#define PSSIG_WRADIOPE      0x10
+#define PSSIG_SPE3          0x08
+#define PSSIG_SPE2          0x04
+#define PSSIG_SPE1          0x02
+#define PSSIG_SRADIOPE      0x01
+
+/* Bits in the BBREGCTL register */
+#define BBREGCTL_DONE       0x04
+#define BBREGCTL_REGR       0x02
+#define BBREGCTL_REGW       0x01
+
+/* Bits in the IFREGCTL register */
+#define IFREGCTL_DONE       0x04
+#define IFREGCTL_IFRF       0x02
+#define IFREGCTL_REGW       0x01
+
+/* Bits in the SOFTPWRCTL register */
+#define SOFTPWRCTL_RFLEOPT      0x0800
+#define SOFTPWRCTL_TXPEINV      0x0200
+#define SOFTPWRCTL_SWPECTI      0x0100
+#define SOFTPWRCTL_SWPAPE       0x0020
+#define SOFTPWRCTL_SWCALEN      0x0010
+#define SOFTPWRCTL_SWRADIO_PE   0x0008
+#define SOFTPWRCTL_SWPE2        0x0004
+#define SOFTPWRCTL_SWPE1        0x0002
+#define SOFTPWRCTL_SWPE3        0x0001
+
+/* Bits in the GPIOCTL1 register */
+#define GPIO1_DATA1             0x20
+#define GPIO1_MD1               0x10
+#define GPIO1_DATA0             0x02
+#define GPIO1_MD0               0x01
+
+/* Bits in the DMACTL register */
+#define DMACTL_CLRRUN       0x00080000
+#define DMACTL_RUN          0x00000008
+#define DMACTL_WAKE         0x00000004
+#define DMACTL_DEAD         0x00000002
+#define DMACTL_ACTIVE       0x00000001
+
+/* Bits in the RXDMACTL0 register */
+#define RX_PERPKT           0x00000100
+#define RX_PERPKTCLR        0x01000000
+
+/* Bits in the BCNDMACTL register */
+#define BEACON_READY        0x01
+
+/* Bits in the MISCFFCTL register */
+#define MISCFFCTL_WRITE     0x0001
+
+/* Bits in WAKEUPEN0 */
+#define WAKEUPEN0_DIRPKT    0x10
+#define WAKEUPEN0_LINKOFF   0x08
+#define WAKEUPEN0_ATIMEN    0x04
+#define WAKEUPEN0_TIMEN     0x02
+#define WAKEUPEN0_MAGICEN   0x01
+
+/* Bits in WAKEUPEN1 */
+#define WAKEUPEN1_128_3     0x08
+#define WAKEUPEN1_128_2     0x04
+#define WAKEUPEN1_128_1     0x02
+#define WAKEUPEN1_128_0     0x01
+
+/* Bits in WAKEUPSR0 */
+#define WAKEUPSR0_DIRPKT    0x10
+#define WAKEUPSR0_LINKOFF   0x08
+#define WAKEUPSR0_ATIMEN    0x04
+#define WAKEUPSR0_TIMEN     0x02
+#define WAKEUPSR0_MAGICEN   0x01
+
+/* Bits in WAKEUPSR1 */
+#define WAKEUPSR1_128_3     0x08
+#define WAKEUPSR1_128_2     0x04
+#define WAKEUPSR1_128_1     0x02
+#define WAKEUPSR1_128_0     0x01
+
+/* Bits in the MAC_REG_GPIOCTL register */
+#define GPIO0_MD            0x01
+#define GPIO0_DATA          0x02
+#define GPIO0_INTMD         0x04
+#define GPIO1_MD            0x10
+#define GPIO1_DATA          0x20
+
+/* Bits in the MSRCTL register */
+#define MSRCTL_FINISH       0x80
+#define MSRCTL_READY        0x40
+#define MSRCTL_RADARDETECT  0x20
+#define MSRCTL_EN           0x10
+#define MSRCTL_QUIETTXCHK   0x08
+#define MSRCTL_QUIETRPT     0x04
+#define MSRCTL_QUIETINT     0x02
+#define MSRCTL_QUIETEN      0x01
+
+/* Bits in the MSRCTL1 register */
+#define MSRCTL1_TXPWR       0x08
+#define MSRCTL1_CSAPAREN    0x04
+#define MSRCTL1_TXPAUSE     0x01
+
+/* Loopback mode */
+#define MAC_LB_EXT          0x02
+#define MAC_LB_INTERNAL     0x01
+#define MAC_LB_NONE         0x00
+
+#define DEFAULT_BI          0x200
+
+/* MiscFIFO Offset */
+#define MISCFIFO_KEYETRY0       32
+#define MISCFIFO_KEYENTRYSIZE   22
+#define MISCFIFO_SYNINFO_IDX    10
+#define MISCFIFO_SYNDATA_IDX    11
+#define MISCFIFO_SYNDATASIZE    21
+
+/* enabled mask value of irq */
+#define IMR_MASK_VALUE     (IMR_SOFTTIMER1 |	\
+			    IMR_RXDMA1 |	\
+			    IMR_RXNOBUF |	\
+			    IMR_MIBNEARFULL |	\
+			    IMR_SOFTINT |	\
+			    IMR_FETALERR |	\
+			    IMR_WATCHDOG |	\
+			    IMR_SOFTTIMER |	\
+			    IMR_GPIO |		\
+			    IMR_TBTT |		\
+			    IMR_RXDMA0 |	\
+			    IMR_BNTX |		\
+			    IMR_AC0DMA |	\
+			    IMR_TXDMA0)
+
+/* max time out delay time */
+#define W_MAX_TIMEOUT       0xFFF0U
+
+/* wait time within loop */
+#define CB_DELAY_LOOP_WAIT  10 /* 10ms */
+
+/* revision id */
+#define REV_ID_VT3253_A0    0x00
+#define REV_ID_VT3253_A1    0x01
+#define REV_ID_VT3253_B0    0x08
+#define REV_ID_VT3253_B1    0x09
+
+/*---------------------  Export Types  ------------------------------*/
+
+/*---------------------  Export Macros ------------------------------*/
+
+#define VT6655_MAC_SELECT_PAGE0(iobase) iowrite8(0, iobase + MAC_REG_PAGE1SEL)
+
+#define VT6655_MAC_SELECT_PAGE1(iobase) iowrite8(1, iobase + MAC_REG_PAGE1SEL)
+
+#define MAKEWORD(lb, hb) \
+	((unsigned short)(((unsigned char)(lb)) | (((unsigned short)((unsigned char)(hb))) << 8)))
+
+void vt6655_mac_reg_bits_on(void __iomem *iobase, const u8 reg_offset, const u8 bit_mask);
+void vt6655_mac_word_reg_bits_on(void __iomem *iobase, const u8 reg_offset, const u16 bit_mask);
+void vt6655_mac_reg_bits_off(void __iomem *iobase, const u8 reg_offset, const u8 bit_mask);
+void vt6655_mac_word_reg_bits_off(void __iomem *iobase, const u8 reg_offset, const u16 bit_mask);
+
+void vt6655_mac_set_short_retry_limit(struct vnt_private *priv, unsigned char retry_limit);
+
+void MACvSetLongRetryLimit(struct vnt_private *priv, unsigned char byRetryLimit);
+
+bool MACbSoftwareReset(struct vnt_private *priv);
+bool MACbShutdown(struct vnt_private *priv);
+void MACvInitialize(struct vnt_private *priv);
+void vt6655_mac_set_curr_rx_0_desc_addr(struct vnt_private *priv, u32 curr_desc_addr);
+void vt6655_mac_set_curr_rx_1_desc_addr(struct vnt_private *priv, u32 curr_desc_addr);
+void vt6655_mac_set_curr_tx_desc_addr(int tx_type, struct vnt_private *priv, u32 curr_desc_addr);
+void MACvSetCurrSyncDescAddrEx(struct vnt_private *priv,
+			       u32 curr_desc_addr);
+void MACvSetCurrATIMDescAddrEx(struct vnt_private *priv,
+			       u32 curr_desc_addr);
+void MACvTimer0MicroSDelay(struct vnt_private *priv, unsigned int uDelay);
+void MACvOneShotTimer1MicroSec(struct vnt_private *priv, unsigned int uDelayTime);
+
+void MACvSetMISCFifo(struct vnt_private *priv, unsigned short wOffset,
+		     u32 dwData);
+
+bool MACbPSWakeup(struct vnt_private *priv);
+
+void MACvSetKeyEntry(struct vnt_private *priv, unsigned short wKeyCtl,
+		     unsigned int uEntryIdx, unsigned int uKeyIdx,
+		     unsigned char *pbyAddr, u32 *pdwKey,
+		     unsigned char local_id);
+void MACvDisableKeyEntry(struct vnt_private *priv, unsigned int uEntryIdx);
+
+#endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
new file mode 100644
index 000000000..8527ad3ef
--- /dev/null
+++ b/drivers/staging/vt6655/power.c
@@ -0,0 +1,144 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles 802.11 power management  functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: July 17, 2002
+ *
+ * Functions:
+ *      PSvEnablePowerSaving - Enable Power Saving Mode
+ *      PSvDiasblePowerSaving - Disable Power Saving Mode
+ *      PSbConsiderPowerDown - Decide if we can Power Down
+ *      PSvSendPSPOLL - Send PS-POLL packet
+ *      PSbSendNullPacket - Send Null packet
+ *      PSbIsNextTBTTWakeUp - Decide if we need to wake up at next Beacon
+ *
+ * Revision History:
+ *
+ */
+
+#include "mac.h"
+#include "device.h"
+#include "power.h"
+#include "card.h"
+
+/*---------------------  Static Definitions -------------------------*/
+
+/*---------------------  Static Classes  ----------------------------*/
+
+/*---------------------  Static Functions  --------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+/*
+ *
+ * Routine Description:
+ * Enable hw power saving functions
+ *
+ * Return Value:
+ *    None.
+ *
+ */
+
+void PSvEnablePowerSaving(struct vnt_private *priv,
+			  unsigned short wListenInterval)
+{
+	u16 wAID = priv->current_aid | BIT(14) | BIT(15);
+
+	/* set period of power up before TBTT */
+	iowrite16(C_PWBT, priv->port_offset + MAC_REG_PWBT);
+	if (priv->op_mode != NL80211_IFTYPE_ADHOC) {
+		/* set AID */
+		iowrite16(wAID, priv->port_offset + MAC_REG_AIDATIM);
+	}
+
+	/* Set AutoSleep */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+
+	/* Set HWUTSF */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
+
+	if (wListenInterval >= 2) {
+		/* clear always listen beacon */
+		vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_PSCTL, PSCTL_ALBCN);
+		/* first time set listen next beacon */
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCTL, PSCTL_LNBCN);
+	} else {
+		/* always listen beacon */
+		vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCTL, PSCTL_ALBCN);
+	}
+
+	/* enable power saving hw function */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCTL, PSCTL_PSEN);
+	priv->bEnablePSMode = true;
+
+	priv->bPWBitOn = true;
+	pr_debug("PS:Power Saving Mode Enable...\n");
+}
+
+/*
+ *
+ * Routine Description:
+ * Disable hw power saving functions
+ *
+ * Return Value:
+ *    None.
+ *
+ */
+
+void PSvDisablePowerSaving(struct vnt_private *priv)
+{
+	/* disable power saving hw function */
+	MACbPSWakeup(priv);
+
+	/* clear AutoSleep */
+	vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+
+	/* clear HWUTSF */
+	vt6655_mac_reg_bits_off(priv->port_offset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
+
+	/* set always listen beacon */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCTL, PSCTL_ALBCN);
+
+	priv->bEnablePSMode = false;
+
+	priv->bPWBitOn = false;
+}
+
+/*
+ *
+ * Routine Description:
+ * Check if Next TBTT must wake up
+ *
+ * Return Value:
+ *    None.
+ *
+ */
+
+bool PSbIsNextTBTTWakeUp(struct vnt_private *priv)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_conf *conf = &hw->conf;
+	bool wake_up = false;
+
+	if (conf->listen_interval > 1) {
+		if (!priv->wake_up_count)
+			priv->wake_up_count = conf->listen_interval;
+
+		--priv->wake_up_count;
+
+		if (priv->wake_up_count == 1) {
+			/* Turn on wake up to listen next beacon */
+			vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_PSCTL, PSCTL_LNBCN);
+			wake_up = true;
+		}
+	}
+
+	return wake_up;
+}
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
new file mode 100644
index 000000000..060516f81
--- /dev/null
+++ b/drivers/staging/vt6655/power.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles 802.11 power management  functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: July 17, 2002
+ *
+ */
+
+#ifndef __POWER_H__
+#define __POWER_H__
+
+#include "device.h"
+
+#define C_PWBT                   1000    /* micro sec. power up before TBTT */
+#define PS_FAST_INTERVAL         1       /* Fast power saving listen interval */
+#define PS_MAX_INTERVAL          4       /* MAX power saving listen interval */
+
+void PSvDisablePowerSaving(struct vnt_private *priv);
+
+void PSvEnablePowerSaving(struct vnt_private *priv, unsigned short wListenInterval);
+
+bool PSbIsNextTBTTWakeUp(struct vnt_private *priv);
+
+#endif /* __POWER_H__ */
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
new file mode 100644
index 000000000..1fadc2fc4
--- /dev/null
+++ b/drivers/staging/vt6655/rf.c
@@ -0,0 +1,535 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: rf function code
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Feb. 19, 2004
+ *
+ * Functions:
+ *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
+ *
+ * Revision History:
+ *	RobertYu 2005
+ *	chester 2008
+ *
+ */
+
+#include "mac.h"
+#include "srom.h"
+#include "rf.h"
+#include "baseband.h"
+
+#define BY_AL2230_REG_LEN     23 /* 24bit */
+#define CB_AL2230_INIT_SEQ    15
+#define SWITCH_CHANNEL_DELAY_AL2230 200 /* us */
+#define AL2230_PWR_IDX_LEN    64
+
+#define BY_AL7230_REG_LEN     23 /* 24bit */
+#define CB_AL7230_INIT_SEQ    16
+#define SWITCH_CHANNEL_DELAY_AL7230 200 /* us */
+#define AL7230_PWR_IDX_LEN    64
+
+static const unsigned long al2230_init_table[CB_AL2230_INIT_SEQ] = {
+	0x03F79000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x01A00200 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00FFF300 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0005A400 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0F4DC500 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0805B600 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0146C700 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00068800 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0403B900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00DBBA00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00099B00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0BDFFC00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00000D00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x00580F00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW
+};
+
+static const unsigned long al2230_channel_table0[CB_MAX_CHANNEL] = {
+	0x03F79000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 1, Tf = 2412MHz */
+	0x03F79000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 2, Tf = 2417MHz */
+	0x03E79000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 3, Tf = 2422MHz */
+	0x03E79000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 4, Tf = 2427MHz */
+	0x03F7A000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 5, Tf = 2432MHz */
+	0x03F7A000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 6, Tf = 2437MHz */
+	0x03E7A000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 7, Tf = 2442MHz */
+	0x03E7A000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 8, Tf = 2447MHz */
+	0x03F7B000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 9, Tf = 2452MHz */
+	0x03F7B000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 10, Tf = 2457MHz */
+	0x03E7B000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 11, Tf = 2462MHz */
+	0x03E7B000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 12, Tf = 2467MHz */
+	0x03F7C000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 13, Tf = 2472MHz */
+	0x03E7C000 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW  /* channel = 14, Tf = 2412M */
+};
+
+static const unsigned long al2230_channel_table1[CB_MAX_CHANNEL] = {
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 1, Tf = 2412MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 2, Tf = 2417MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 3, Tf = 2422MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 4, Tf = 2427MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 5, Tf = 2432MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 6, Tf = 2437MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 7, Tf = 2442MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 8, Tf = 2447MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 9, Tf = 2452MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 10, Tf = 2457MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 11, Tf = 2462MHz */
+	0x0B333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 12, Tf = 2467MHz */
+	0x03333100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW, /* channel = 13, Tf = 2472MHz */
+	0x06666100 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW  /* channel = 14, Tf = 2412M */
+};
+
+static unsigned long al2230_power_table[AL2230_PWR_IDX_LEN] = {
+	0x04040900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04041900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04042900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04043900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04044900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04045900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04046900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04047900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04048900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04049900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404A900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404B900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404C900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404D900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404E900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0404F900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04050900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04051900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04052900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04053900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04054900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04055900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04056900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04057900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04058900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04059900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405A900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405B900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405C900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405D900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405E900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0405F900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04060900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04061900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04062900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04063900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04064900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04065900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04066900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04067900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04068900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04069900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406A900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406B900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406C900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406D900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406E900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0406F900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04070900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04071900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04072900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04073900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04074900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04075900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04076900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04077900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04078900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x04079900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407A900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407B900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407C900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407D900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407E900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW,
+	0x0407F900 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW
+};
+
+/*
+ * Description: Write to IF/RF, by embedded programming
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *      dwData      - data to write
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool IFRFbWriteEmbedded(struct vnt_private *priv, unsigned long dwData)
+{
+	void __iomem *iobase = priv->port_offset;
+	unsigned short ww;
+	unsigned long dwValue;
+
+	iowrite32((u32)dwData, iobase + MAC_REG_IFREGCTL);
+
+	/* W_MAX_TIMEOUT is the timeout period */
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		dwValue = ioread32(iobase + MAC_REG_IFREGCTL);
+		if (dwValue & IFREGCTL_DONE)
+			break;
+	}
+
+	if (ww == W_MAX_TIMEOUT)
+		return false;
+
+	return true;
+}
+
+/*
+ * Description: AIROHA IFRF chip init function
+ *
+ * Parameters:
+ *  In:
+ *      iobase      - I/O base address
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+static bool RFbAL2230Init(struct vnt_private *priv)
+{
+	void __iomem *iobase = priv->port_offset;
+	int     ii;
+	bool ret;
+
+	ret = true;
+
+	/* 3-wire control for normal mode */
+	iowrite8(0, iobase + MAC_REG_SOFTPWRCTL);
+
+	vt6655_mac_word_reg_bits_on(iobase, MAC_REG_SOFTPWRCTL,
+				    (SOFTPWRCTL_SWPECTI | SOFTPWRCTL_TXPEINV));
+	/* PLL  Off */
+	vt6655_mac_word_reg_bits_off(iobase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+
+	/* patch abnormal AL2230 frequency output */
+	IFRFbWriteEmbedded(priv, (0x07168700 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW));
+
+	for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
+		ret &= IFRFbWriteEmbedded(priv, al2230_init_table[ii]);
+	MACvTimer0MicroSDelay(priv, 30); /* delay 30 us */
+
+	/* PLL On */
+	vt6655_mac_word_reg_bits_on(iobase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+
+	MACvTimer0MicroSDelay(priv, 150);/* 150us */
+	ret &= IFRFbWriteEmbedded(priv, (0x00d80f00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW));
+	MACvTimer0MicroSDelay(priv, 30);/* 30us */
+	ret &= IFRFbWriteEmbedded(priv, (0x00780f00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW));
+	MACvTimer0MicroSDelay(priv, 30);/* 30us */
+	ret &= IFRFbWriteEmbedded(priv,
+				  al2230_init_table[CB_AL2230_INIT_SEQ - 1]);
+
+	vt6655_mac_word_reg_bits_on(iobase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
+								 SOFTPWRCTL_SWPE2    |
+								 SOFTPWRCTL_SWPECTI  |
+								 SOFTPWRCTL_TXPEINV));
+
+	/* 3-wire control for power saving mode */
+	iowrite8(PSSIG_WPE3 | PSSIG_WPE2, iobase + MAC_REG_PSPWRSIG);
+
+	return ret;
+}
+
+static bool RFbAL2230SelectChannel(struct vnt_private *priv, unsigned char byChannel)
+{
+	void __iomem *iobase = priv->port_offset;
+	bool ret;
+
+	ret = true;
+
+	ret &= IFRFbWriteEmbedded(priv, al2230_channel_table0[byChannel - 1]);
+	ret &= IFRFbWriteEmbedded(priv, al2230_channel_table1[byChannel - 1]);
+
+	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
+	iowrite8(byChannel & 0x7F, iobase + MAC_REG_CHANNEL);
+	MACvTimer0MicroSDelay(priv, SWITCH_CHANNEL_DELAY_AL2230);
+	/* Set Channel[7] = 1 to tell H/W channel change is done. */
+	iowrite8(byChannel | 0x80, iobase + MAC_REG_CHANNEL);
+
+	return ret;
+}
+
+/*
+ * Description: RF init function
+ *
+ * Parameters:
+ *  In:
+ *      byBBType
+ *      byRFType
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool RFbInit(struct vnt_private *priv)
+{
+	bool ret = true;
+
+	switch (priv->byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+		priv->max_pwr_level = AL2230_PWR_IDX_LEN;
+		ret = RFbAL2230Init(priv);
+		break;
+	case RF_NOTHING:
+		ret = true;
+		break;
+	default:
+		ret = false;
+		break;
+	}
+	return ret;
+}
+
+/*
+ * Description: Select channel
+ *
+ * Parameters:
+ *  In:
+ *      byRFType
+ *      byChannel    - Channel number
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool RFbSelectChannel(struct vnt_private *priv, unsigned char byRFType,
+		      u16 byChannel)
+{
+	bool ret = true;
+
+	switch (byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+		ret = RFbAL2230SelectChannel(priv, byChannel);
+		break;
+		/*{{ RobertYu: 20050104 */
+	case RF_NOTHING:
+		ret = true;
+		break;
+	default:
+		ret = false;
+		break;
+	}
+	return ret;
+}
+
+/*
+ * Description: Write WakeProgSyn
+ *
+ * Parameters:
+ *  In:
+ *      priv        - Device Structure
+ *      rf_type     - RF type
+ *      channel     - Channel number
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool rf_write_wake_prog_syn(struct vnt_private *priv, unsigned char rf_type,
+			    u16 channel)
+{
+	void __iomem *iobase = priv->port_offset;
+	int i;
+	unsigned char init_count = 0;
+	unsigned char sleep_count = 0;
+	unsigned short idx = MISCFIFO_SYNDATA_IDX;
+
+	iowrite16(0, iobase + MAC_REG_MISCFFNDEX);
+	switch (rf_type) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+
+		if (channel > CB_MAX_CHANNEL_24G)
+			return false;
+
+		 /* Init Reg + Channel Reg (2) */
+		init_count = CB_AL2230_INIT_SEQ + 2;
+		sleep_count = 0;
+
+		for (i = 0; i < CB_AL2230_INIT_SEQ; i++)
+			MACvSetMISCFifo(priv, idx++, al2230_init_table[i]);
+
+		MACvSetMISCFifo(priv, idx++, al2230_channel_table0[channel - 1]);
+		MACvSetMISCFifo(priv, idx++, al2230_channel_table1[channel - 1]);
+		break;
+
+		/* Need to check, PLLON need to be low for channel setting */
+
+	case RF_NOTHING:
+		return true;
+
+	default:
+		return false;
+	}
+
+	MACvSetMISCFifo(priv, MISCFIFO_SYNINFO_IDX, (unsigned long)MAKEWORD(sleep_count, init_count));
+
+	return true;
+}
+
+/*
+ * Description: Set Tx power
+ *
+ * Parameters:
+ *  In:
+ *      iobase         - I/O base address
+ *      dwRFPowerTable - RF Tx Power Setting
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+bool RFbSetPower(struct vnt_private *priv, unsigned int rate, u16 uCH)
+{
+	bool ret;
+	unsigned char byPwr = 0;
+	unsigned char byDec = 0;
+
+	if (priv->dwDiagRefCount != 0)
+		return true;
+
+	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL))
+		return false;
+
+	switch (rate) {
+	case RATE_1M:
+	case RATE_2M:
+	case RATE_5M:
+	case RATE_11M:
+		if (uCH > CB_MAX_CHANNEL_24G)
+			return false;
+
+		byPwr = priv->abyCCKPwrTbl[uCH];
+		break;
+	case RATE_6M:
+	case RATE_9M:
+	case RATE_12M:
+	case RATE_18M:
+		byPwr = priv->abyOFDMPwrTbl[uCH];
+		byDec = byPwr + 10;
+
+		if (byDec >= priv->max_pwr_level)
+			byDec = priv->max_pwr_level - 1;
+
+		byPwr = byDec;
+		break;
+	case RATE_24M:
+	case RATE_36M:
+	case RATE_48M:
+	case RATE_54M:
+		byPwr = priv->abyOFDMPwrTbl[uCH];
+		break;
+	}
+
+	if (priv->byCurPwr == byPwr)
+		return true;
+
+	ret = RFbRawSetPower(priv, byPwr, rate);
+	if (ret)
+		priv->byCurPwr = byPwr;
+
+	return ret;
+}
+
+/*
+ * Description: Set Tx power
+ *
+ * Parameters:
+ *  In:
+ *      iobase         - I/O base address
+ *      dwRFPowerTable - RF Tx Power Setting
+ *  Out:
+ *      none
+ *
+ * Return Value: true if succeeded; false if failed.
+ *
+ */
+
+bool RFbRawSetPower(struct vnt_private *priv, unsigned char byPwr,
+		    unsigned int rate)
+{
+	bool ret = true;
+
+	if (byPwr >= priv->max_pwr_level)
+		return false;
+
+	switch (priv->byRFType) {
+	case RF_AIROHA:
+		ret &= IFRFbWriteEmbedded(priv, al2230_power_table[byPwr]);
+		if (rate <= RATE_11M)
+			ret &= IFRFbWriteEmbedded(priv, 0x0001B400 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+		else
+			ret &= IFRFbWriteEmbedded(priv, 0x0005A400 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+
+		break;
+
+	case RF_AL2230S:
+		ret &= IFRFbWriteEmbedded(priv, al2230_power_table[byPwr]);
+		if (rate <= RATE_11M) {
+			ret &= IFRFbWriteEmbedded(priv, 0x040C1400 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+			ret &= IFRFbWriteEmbedded(priv, 0x00299B00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+		} else {
+			ret &= IFRFbWriteEmbedded(priv, 0x0005A400 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+			ret &= IFRFbWriteEmbedded(priv, 0x00099B00 + (BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
+		}
+
+		break;
+
+	default:
+		break;
+	}
+	return ret;
+}
+
+/*
+ *
+ * Routine Description:
+ *     Translate RSSI to dBm
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be translated
+ *      byCurrRSSI      - RSSI to be translated
+ *  Out:
+ *      pdwdbm          - Translated dbm number
+ *
+ * Return Value: none
+ *
+ */
+void
+RFvRSSITodBm(struct vnt_private *priv, unsigned char byCurrRSSI, long *pldBm)
+{
+	unsigned char byIdx = (((byCurrRSSI & 0xC0) >> 6) & 0x03);
+	long b = (byCurrRSSI & 0x3F);
+	long a = 0;
+	unsigned char abyAIROHARF[4] = {0, 18, 0, 40};
+
+	switch (priv->byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+		a = abyAIROHARF[byIdx];
+		break;
+	default:
+		break;
+	}
+
+	*pldBm = -1 * (a + b * 2);
+}
+
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
new file mode 100644
index 000000000..9fef81846
--- /dev/null
+++ b/drivers/staging/vt6655/rf.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Feb. 19, 2004
+ *
+ */
+
+#ifndef __RF_H__
+#define __RF_H__
+
+#include "device.h"
+
+/*---------------------  Export Definitions -------------------------*/
+/*
+ * Baseband RF pair definition in eeprom (Bits 6..0)
+ */
+#define RF_RFMD2959             0x01
+#define RF_MAXIMAG              0x02
+#define RF_AIROHA               0x03
+
+#define RF_UW2451               0x05
+#define RF_MAXIMG               0x06
+#define RF_MAXIM2829            0x07 /* RobertYu: 20041118 */
+#define RF_UW2452               0x08 /* RobertYu: 20041210 */
+#define RF_AIROHA7230           0x0a /* RobertYu: 20050104 */
+#define RF_UW2453               0x0b
+
+#define RF_VT3226               0x09
+#define RF_AL2230S              0x0e
+
+#define RF_NOTHING              0x7E
+#define RF_EMU                  0x80
+#define RF_MASK                 0x7F
+
+#define ZONE_FCC                0
+#define ZONE_MKK1               1
+#define ZONE_ETSI               2
+#define ZONE_IC                 3
+#define ZONE_SPAIN              4
+#define ZONE_FRANCE             5
+#define ZONE_MKK                6
+#define ZONE_ISRAEL             7
+
+/* [20050104] CB_MAXIM2829_CHANNEL_5G_HIGH, CB_UW2452_CHANNEL_5G_HIGH: 40==>41 */
+#define CB_MAXIM2829_CHANNEL_5G_HIGH    41 /* Index41: channel = 100, Tf = 5500MHz, set the (A3:A0=0101) D6=1 */
+#define CB_UW2452_CHANNEL_5G_HIGH       41 /* [20041210] Index41: channel = 100, Tf = 5500MHz, change VCO2->VCO3 */
+
+/*---------------------  Export Classes  ----------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+bool IFRFbWriteEmbedded(struct vnt_private *priv, unsigned long dwData);
+bool RFbSelectChannel(struct vnt_private *priv, unsigned char byRFType, u16 byChannel);
+bool RFbInit(struct vnt_private *priv);
+bool rf_write_wake_prog_syn(struct vnt_private *priv, unsigned char rf_type, u16 channel);
+bool RFbSetPower(struct vnt_private *priv, unsigned int rate, u16 uCH);
+bool RFbRawSetPower(struct vnt_private *priv, unsigned char byPwr,
+		    unsigned int rate);
+
+void RFvRSSITodBm(struct vnt_private *priv, unsigned char byCurrRSSI,
+		  long *pldBm);
+
+/* {{ RobertYu: 20050104 */
+bool RFbAL7230SelectChannelPostProcess(struct vnt_private *priv, u16 byOldChannel, u16 byNewChannel);
+/* }} RobertYu */
+
+#endif /* __RF_H__ */
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
new file mode 100644
index 000000000..5bdb51767
--- /dev/null
+++ b/drivers/staging/vt6655/rxtx.c
@@ -0,0 +1,1462 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: handle WMAC/802.3/802.11 rx & tx functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: May 20, 2003
+ *
+ * Functions:
+ *      s_vGenerateTxParameter - Generate tx dma required parameter.
+ *      vGenerateMACHeader - Translate 802.3 to 802.11 header
+ *      cbGetFragCount - Calculate fragment number count
+ *      csBeacon_xmit - beacon tx function
+ *      csMgmt_xmit - management tx function
+ *      s_cbFillTxBufHead - fulfill tx dma buffer header
+ *      s_uGetDataDuration - get tx data required duration
+ *      s_uFillDataHead- fulfill tx data duration header
+ *      s_uGetRTSCTSDuration- get rtx/cts required duration
+ *      get_rtscts_time- get rts/cts reserved time
+ *      s_uGetTxRsvTime- get frame reserved time
+ *      s_vFillCTSHead- fulfill CTS ctl header
+ *      s_vFillFragParameter- Set fragment ctl parameter.
+ *      s_vFillRTSHead- fulfill RTS ctl header
+ *      s_vFillTxKey- fulfill tx encrypt key
+ *      s_vSWencryption- Software encrypt header
+ *      vDMA0_tx_80211- tx 802.11 frame via dma0
+ *      vGenerateFIFOHeader- Generate tx FIFO ctl header
+ *
+ * Revision History:
+ *
+ */
+
+#include "device.h"
+#include "rxtx.h"
+#include "card.h"
+#include "mac.h"
+#include "baseband.h"
+#include "rf.h"
+
+/*---------------------  Static Definitions -------------------------*/
+
+/*---------------------  Static Classes  ----------------------------*/
+
+/*---------------------  Static Variables  --------------------------*/
+
+/*---------------------  Static Functions  --------------------------*/
+
+/*---------------------  Static Definitions -------------------------*/
+/* if packet size < 256 -> in-direct send
+ * vpacket size >= 256 -> direct send
+ */
+#define CRITICAL_PACKET_LEN      256
+
+static const unsigned short wTimeStampOff[2][MAX_RATE] = {
+	{384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, /* Long Preamble */
+	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, /* Short Preamble */
+};
+
+static const unsigned short wFB_Opt0[2][5] = {
+	{RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, /* fallback_rate0 */
+	{RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, /* fallback_rate1 */
+};
+
+static const unsigned short wFB_Opt1[2][5] = {
+	{RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, /* fallback_rate0 */
+	{RATE_6M,  RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, /* fallback_rate1 */
+};
+
+#define RTSDUR_BB       0
+#define RTSDUR_BA       1
+#define RTSDUR_AA       2
+#define CTSDUR_BA       3
+#define RTSDUR_BA_F0    4
+#define RTSDUR_AA_F0    5
+#define RTSDUR_BA_F1    6
+#define RTSDUR_AA_F1    7
+#define CTSDUR_BA_F0    8
+#define CTSDUR_BA_F1    9
+#define DATADUR_B       10
+#define DATADUR_A       11
+#define DATADUR_A_F0    12
+#define DATADUR_A_F1    13
+
+/*---------------------  Static Functions  --------------------------*/
+static
+void
+s_vFillRTSHead(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	void *pvRTS,
+	unsigned int	cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	struct ieee80211_hdr *hdr,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+);
+
+static
+void
+s_vGenerateTxParameter(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	struct vnt_tx_fifo_head *,
+	void *pvRrvTime,
+	void *pvRTS,
+	void *pvCTS,
+	unsigned int	cbFrameSize,
+	bool bNeedACK,
+	unsigned int	uDMAIdx,
+	void *psEthHeader,
+	unsigned short wCurrentRate
+);
+
+static unsigned int
+s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
+		  unsigned char *pbyTxBufferAddr,
+		  unsigned int uDMAIdx, struct vnt_tx_desc *pHeadTD,
+		  unsigned int uNodeIndex);
+
+static
+__le16
+s_uFillDataHead(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	void *pTxDataHead,
+	unsigned int cbFrameLength,
+	unsigned int uDMAIdx,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption,
+	unsigned short wCurrentRate,
+	bool is_pspoll
+);
+
+/*---------------------  Export Variables  --------------------------*/
+
+static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
+{
+	return cpu_to_le16(wTimeStampOff[priv->preamble_type % 2]
+							[rate % MAX_RATE]);
+}
+
+/* byPktType : PK_TYPE_11A     0
+ * PK_TYPE_11B     1
+ * PK_TYPE_11GB    2
+ * PK_TYPE_11GA    3
+ */
+static
+unsigned int
+s_uGetTxRsvTime(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	unsigned int cbFrameLength,
+	unsigned short wRate,
+	bool bNeedAck
+)
+{
+	unsigned int uDataTime, uAckTime;
+
+	uDataTime = bb_get_frame_time(pDevice->preamble_type, byPktType, cbFrameLength, wRate);
+
+	if (!bNeedAck)
+		return uDataTime;
+
+	/*
+	 * CCK mode  - 11b
+	 * OFDM mode - 11g 2.4G & 11a 5G
+	 */
+	uAckTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14,
+				     byPktType == PK_TYPE_11B ?
+				     pDevice->byTopCCKBasicRate :
+				     pDevice->byTopOFDMBasicRate);
+
+	return uDataTime + pDevice->uSIFS + uAckTime;
+}
+
+static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
+				    u32 frame_length, u16 rate, bool need_ack)
+{
+	return cpu_to_le16((u16)s_uGetTxRsvTime(priv, pkt_type,
+						frame_length, rate, need_ack));
+}
+
+/* byFreqType: 0=>5GHZ 1=>2.4GHZ */
+static __le16 get_rtscts_time(struct vnt_private *priv,
+			      unsigned char rts_rsvtype,
+			      unsigned char pkt_type,
+			      unsigned int frame_length,
+			      unsigned short current_rate)
+{
+	unsigned int rrv_time = 0;
+	unsigned int rts_time = 0;
+	unsigned int cts_time = 0;
+	unsigned int ack_time = 0;
+	unsigned int data_time = 0;
+
+	data_time = bb_get_frame_time(priv->preamble_type, pkt_type, frame_length, current_rate);
+	if (rts_rsvtype == 0) { /* RTSTxRrvTime_bb */
+		rts_time = bb_get_frame_time(priv->preamble_type, pkt_type, 20, priv->byTopCCKBasicRate);
+		ack_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopCCKBasicRate);
+		cts_time = ack_time;
+	} else if (rts_rsvtype == 1) { /* RTSTxRrvTime_ba, only in 2.4GHZ */
+		rts_time = bb_get_frame_time(priv->preamble_type, pkt_type, 20, priv->byTopCCKBasicRate);
+		cts_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopCCKBasicRate);
+		ack_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopOFDMBasicRate);
+	} else if (rts_rsvtype == 2) { /* RTSTxRrvTime_aa */
+		rts_time = bb_get_frame_time(priv->preamble_type, pkt_type, 20, priv->byTopOFDMBasicRate);
+		ack_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopOFDMBasicRate);
+		cts_time = ack_time;
+	} else if (rts_rsvtype == 3) { /* CTSTxRrvTime_ba, only in 2.4GHZ */
+		cts_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopCCKBasicRate);
+		ack_time = bb_get_frame_time(priv->preamble_type, pkt_type, 14, priv->byTopOFDMBasicRate);
+		rrv_time = cts_time + ack_time + data_time + 2 * priv->uSIFS;
+		return cpu_to_le16((u16)rrv_time);
+	}
+
+	/* RTSRrvTime */
+	rrv_time = rts_time + cts_time + ack_time + data_time + 3 * priv->uSIFS;
+	return cpu_to_le16((u16)rrv_time);
+}
+
+/* byFreqType 0: 5GHz, 1:2.4Ghz */
+static
+unsigned int
+s_uGetDataDuration(
+	struct vnt_private *pDevice,
+	unsigned char byDurType,
+	unsigned int cbFrameLength,
+	unsigned char byPktType,
+	unsigned short wRate,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption
+)
+{
+	bool bLastFrag = false;
+	unsigned int uAckTime = 0, uNextPktTime = 0, len;
+
+	if (uFragIdx == (uMACfragNum - 1))
+		bLastFrag = true;
+
+	if (uFragIdx == (uMACfragNum - 2))
+		len = cbLastFragmentSize;
+	else
+		len = cbFrameLength;
+
+	switch (byDurType) {
+	case DATADUR_B:    /* DATADUR_B */
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->preamble_type,
+						     byPktType, 14,
+						     pDevice->byTopCCKBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
+				return 0;
+		} else {
+			/* First Frag or Mid Frag */
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
+		}
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
+
+	case DATADUR_A:    /* DATADUR_A */
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->preamble_type,
+						     byPktType, 14,
+						     pDevice->byTopOFDMBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
+				return 0;
+		} else {
+			/* First Frag or Mid Frag */
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
+		}
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
+
+	case DATADUR_A_F0:    /* DATADUR_A_F0 */
+	case DATADUR_A_F1:    /* DATADUR_A_F1 */
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->preamble_type,
+						     byPktType, 14,
+						     pDevice->byTopOFDMBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
+				return 0;
+		} else {
+			/* First Frag or Mid Frag */
+			if (wRate < RATE_18M)
+				wRate = RATE_18M;
+			else if (wRate > RATE_54M)
+				wRate = RATE_54M;
+
+			wRate -= RATE_18M;
+
+			if (byFBOption == AUTO_FB_0)
+				wRate = wFB_Opt0[FB_RATE0][wRate];
+			else
+				wRate = wFB_Opt1[FB_RATE0][wRate];
+
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
+		}
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/* byFreqType: 0=>5GHZ 1=>2.4GHZ */
+static
+__le16
+s_uGetRTSCTSDuration(
+	struct vnt_private *pDevice,
+	unsigned char byDurType,
+	unsigned int cbFrameLength,
+	unsigned char byPktType,
+	unsigned short wRate,
+	bool bNeedAck,
+	unsigned char byFBOption
+)
+{
+	unsigned int uCTSTime = 0, uDurTime = 0;
+
+	switch (byDurType) {
+	case RTSDUR_BB:    /* RTSDuration_bb */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_BA:    /* RTSDuration_ba */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_AA:    /* RTSDuration_aa */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case CTSDUR_BA:    /* CTSDuration_ba */
+		uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_BA_F0: /* RTSDuration_ba_f0 */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopCCKBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	case RTSDUR_AA_F0: /* RTSDuration_aa_f0 */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	case RTSDUR_BA_F1: /* RTSDuration_ba_f1 */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopCCKBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	case RTSDUR_AA_F1: /* RTSDuration_aa_f1 */
+		uCTSTime = bb_get_frame_time(pDevice->preamble_type, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	case CTSDUR_BA_F0: /* CTSDuration_ba_f0 */
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	case CTSDUR_BA_F1: /* CTSDuration_ba_f1 */
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate - RATE_18M], bNeedAck);
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate - RATE_18M], bNeedAck);
+
+		break;
+
+	default:
+		break;
+	}
+
+	return cpu_to_le16((u16)uDurTime);
+}
+
+static
+__le16
+s_uFillDataHead(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	void *pTxDataHead,
+	unsigned int cbFrameLength,
+	unsigned int uDMAIdx,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption,
+	unsigned short wCurrentRate,
+	bool is_pspoll
+)
+{
+	struct vnt_tx_datahead_ab *buf = pTxDataHead;
+
+	if (!pTxDataHead)
+		return 0;
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		/* Auto Fallback */
+		struct vnt_tx_datahead_g_fb *buf = pTxDataHead;
+
+		if (byFBOption == AUTO_FB_NONE) {
+			struct vnt_tx_datahead_g *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->a);
+
+			vnt_get_phy_field(pDevice, cbFrameLength,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			if (is_pspoll) {
+				__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+				buf->duration_a = dur;
+				buf->duration_b = dur;
+			} else {
+				/* Get Duration and TimeStamp */
+				buf->duration_a =
+					cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
+									    byPktType, wCurrentRate, bNeedAck, uFragIdx,
+									    cbLastFragmentSize, uMACfragNum,
+									    byFBOption));
+				buf->duration_b =
+					cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
+									    PK_TYPE_11B, pDevice->byTopCCKBasicRate,
+									    bNeedAck, uFragIdx, cbLastFragmentSize,
+									    uMACfragNum, byFBOption));
+			}
+
+			buf->time_stamp_off_a = vnt_time_stamp_off(pDevice, wCurrentRate);
+			buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
+
+			return buf->duration_a;
+		}
+
+		/* Get SignalField, ServiceField & Length */
+		vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+				  byPktType, &buf->a);
+
+		vnt_get_phy_field(pDevice, cbFrameLength,
+				  pDevice->byTopCCKBasicRate,
+				  PK_TYPE_11B, &buf->b);
+		/* Get Duration and TimeStamp */
+		buf->duration_a = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+								      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+		buf->duration_b = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
+								       pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+		buf->duration_a_f0 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+									  wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+		buf->duration_a_f1 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+									 wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+
+		buf->time_stamp_off_a = vnt_time_stamp_off(pDevice, wCurrentRate);
+		buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
+
+		return buf->duration_a;
+		  /* if (byFBOption == AUTO_FB_NONE) */
+	} else if (byPktType == PK_TYPE_11A) {
+		struct vnt_tx_datahead_ab *buf = pTxDataHead;
+
+		if (byFBOption != AUTO_FB_NONE) {
+			/* Auto Fallback */
+			struct vnt_tx_datahead_a_fb *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->a);
+
+			/* Get Duration and TimeStampOff */
+			buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+									    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_f0 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+									       wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_f1 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+										wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+			return buf->duration;
+		}
+
+		/* Get SignalField, ServiceField & Length */
+		vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+				  byPktType, &buf->ab);
+
+		if (is_pspoll) {
+			__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+			buf->duration = dur;
+		} else {
+			/* Get Duration and TimeStampOff */
+			buf->duration =
+				cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+								    wCurrentRate, bNeedAck, uFragIdx,
+								    cbLastFragmentSize, uMACfragNum,
+								    byFBOption));
+		}
+
+		buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+		return buf->duration;
+	}
+
+	/* Get SignalField, ServiceField & Length */
+	vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+			  byPktType, &buf->ab);
+
+	if (is_pspoll) {
+		__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+		buf->duration = dur;
+	} else {
+		/* Get Duration and TimeStampOff */
+		buf->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
+							    wCurrentRate, bNeedAck, uFragIdx,
+							    cbLastFragmentSize, uMACfragNum,
+							    byFBOption));
+	}
+
+	buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+	return buf->duration;
+}
+
+static
+void
+s_vFillRTSHead(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	void *pvRTS,
+	unsigned int cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	struct ieee80211_hdr *hdr,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+)
+{
+	unsigned int uRTSFrameLen = 20;
+
+	if (!pvRTS)
+		return;
+
+	if (bDisCRC) {
+		/* When CRCDIS bit is on, H/W forgot to generate FCS for
+		 * RTS frame, in this case we need to decrease its length by 4.
+		 */
+		uRTSFrameLen -= 4;
+	}
+
+	/* Note: So far RTSHead doesn't appear in ATIM & Beacom DMA,
+	 * so we don't need to take them into account.
+	 * Otherwise, we need to modify codes for them.
+	 */
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (byFBOption == AUTO_FB_NONE) {
+			struct vnt_rts_g *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration_bb =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BB,
+						     cbFrameLength, PK_TYPE_11B,
+						     pDevice->byTopCCKBasicRate,
+						     bNeedAck, byFBOption);
+			buf->duration_aa =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			buf->data.duration = buf->duration_aa;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
+		} else {
+			struct vnt_rts_g_fb *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration_bb =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BB,
+						     cbFrameLength, PK_TYPE_11B,
+						     pDevice->byTopCCKBasicRate,
+						     bNeedAck, byFBOption);
+			buf->duration_aa =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_ba_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_aa_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_ba_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_aa_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration_aa;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
+		} /* if (byFBOption == AUTO_FB_NONE) */
+	} else if (byPktType == PK_TYPE_11A) {
+		if (byFBOption == AUTO_FB_NONE) {
+			struct vnt_rts_ab *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->ab);
+			/* Get Duration */
+			buf->duration =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
+		} else {
+			struct vnt_rts_a_fb *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
+		}
+	} else if (byPktType == PK_TYPE_11B) {
+		struct vnt_rts_ab *buf = pvRTS;
+		/* Get SignalField, ServiceField & Length */
+		vnt_get_phy_field(pDevice, uRTSFrameLen,
+				  pDevice->byTopCCKBasicRate,
+				  PK_TYPE_11B, &buf->ab);
+		/* Get Duration */
+		buf->duration =
+			s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength,
+					     byPktType, wCurrentRate, bNeedAck,
+					     byFBOption);
+
+		buf->data.duration = buf->duration;
+		/* Get RTS Frame body */
+		buf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
+
+		ether_addr_copy(buf->data.ra, hdr->addr1);
+		ether_addr_copy(buf->data.ta, hdr->addr2);
+	}
+}
+
+static
+void
+s_vFillCTSHead(
+	struct vnt_private *pDevice,
+	unsigned int uDMAIdx,
+	unsigned char byPktType,
+	void *pvCTS,
+	unsigned int cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+)
+{
+	unsigned int uCTSFrameLen = 14;
+
+	if (!pvCTS)
+		return;
+
+	if (bDisCRC) {
+		/* When CRCDIS bit is on, H/W forgot to generate FCS for
+		 * CTS frame, in this case we need to decrease its length by 4.
+		 */
+		uCTSFrameLen -= 4;
+	}
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) {
+			/* Auto Fall back */
+			struct vnt_cts_fb *buf = pvCTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uCTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTSDuration_ba_f0 */
+			buf->cts_duration_ba_f0 =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTSDuration_ba_f1 */
+			buf->cts_duration_ba_f1 =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTS Frame body */
+			buf->data.duration = buf->duration_ba;
+
+			buf->data.frame_control =
+				cpu_to_le16(IEEE80211_FTYPE_CTL |
+					    IEEE80211_STYPE_CTS);
+
+			buf->reserved2 = 0x0;
+
+			ether_addr_copy(buf->data.ra,
+					pDevice->abyCurrentNetAddr);
+		} else { /* if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) */
+			struct vnt_cts *buf = pvCTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uCTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			/* Get CTSDuration_ba */
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTS Frame body */
+			buf->data.duration = buf->duration_ba;
+
+			buf->data.frame_control =
+				cpu_to_le16(IEEE80211_FTYPE_CTL |
+					    IEEE80211_STYPE_CTS);
+
+			buf->reserved2 = 0x0;
+			ether_addr_copy(buf->data.ra,
+					pDevice->abyCurrentNetAddr);
+		}
+	}
+}
+
+/*
+ *
+ * Description:
+ *      Generate FIFO control for MAC & Baseband controller
+ *
+ * Parameters:
+ *  In:
+ *      pDevice         - Pointer to adapter
+ *      pTxDataHead     - Transmit Data Buffer
+ *      pTxBufHead      - pTxBufHead
+ *      pvRrvTime        - pvRrvTime
+ *      pvRTS            - RTS Buffer
+ *      pCTS            - CTS Buffer
+ *      cbFrameSize     - Transmit Data Length (Hdr+Payload+FCS)
+ *      bNeedACK        - If need ACK
+ *      uDescIdx        - Desc Index
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ -
+ * unsigned int cbFrameSize, Hdr+Payload+FCS
+ */
+static
+void
+s_vGenerateTxParameter(
+	struct vnt_private *pDevice,
+	unsigned char byPktType,
+	struct vnt_tx_fifo_head *tx_buffer_head,
+	void *pvRrvTime,
+	void *pvRTS,
+	void *pvCTS,
+	unsigned int cbFrameSize,
+	bool bNeedACK,
+	unsigned int uDMAIdx,
+	void *psEthHeader,
+	unsigned short wCurrentRate
+)
+{
+	u16 fifo_ctl = le16_to_cpu(tx_buffer_head->fifo_ctl);
+	bool bDisCRC = false;
+	unsigned char byFBOption = AUTO_FB_NONE;
+
+	tx_buffer_head->current_rate = cpu_to_le16(wCurrentRate);
+
+	if (fifo_ctl & FIFOCTL_CRCDIS)
+		bDisCRC = true;
+
+	if (fifo_ctl & FIFOCTL_AUTO_FB_0)
+		byFBOption = AUTO_FB_0;
+	else if (fifo_ctl & FIFOCTL_AUTO_FB_1)
+		byFBOption = AUTO_FB_1;
+
+	if (!pvRrvTime)
+		return;
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (pvRTS) { /* RTS_need */
+			/* Fill RsvTime */
+			struct vnt_rrv_time_rts *buf = pvRrvTime;
+
+			buf->rts_rrv_time_aa = get_rtscts_time(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
+			buf->rts_rrv_time_ba = get_rtscts_time(pDevice, 1, byPktType, cbFrameSize, wCurrentRate);
+			buf->rts_rrv_time_bb = get_rtscts_time(pDevice, 0, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time_a = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
+			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
+
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else {/* RTS_needless, PCF mode */
+			struct vnt_rrv_time_cts *buf = pvRrvTime;
+
+			buf->rrv_time_a = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
+			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
+			buf->cts_rrv_time_ba = get_rtscts_time(pDevice, 3, byPktType, cbFrameSize, wCurrentRate);
+
+			/* Fill CTS */
+			s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
+		}
+	} else if (byPktType == PK_TYPE_11A) {
+		if (pvRTS) {/* RTS_need, non PCF mode */
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rts_rrv_time = get_rtscts_time(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
+
+			/* Fill RTS */
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else if (!pvRTS) {/* RTS_needless, non PCF mode */
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK);
+		}
+	} else if (byPktType == PK_TYPE_11B) {
+		if (pvRTS) {/* RTS_need, non PCF mode */
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rts_rrv_time = get_rtscts_time(pDevice, 0, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
+
+			/* Fill RTS */
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else { /* RTS_needless, non PCF mode */
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
+		}
+	}
+}
+
+static unsigned int
+s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
+		  unsigned char *pbyTxBufferAddr,
+		  unsigned int uDMAIdx, struct vnt_tx_desc *pHeadTD,
+		  unsigned int is_pspoll)
+{
+	struct vnt_td_info *td_info = pHeadTD->td_info;
+	struct sk_buff *skb = td_info->skb;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct vnt_tx_fifo_head *tx_buffer_head =
+			(struct vnt_tx_fifo_head *)td_info->buf;
+	u16 fifo_ctl = le16_to_cpu(tx_buffer_head->fifo_ctl);
+	unsigned int cbFrameSize;
+	__le16 uDuration;
+	unsigned char *pbyBuffer;
+	unsigned int uLength = 0;
+	unsigned int cbMICHDR = 0;
+	unsigned int uMACfragNum = 1;
+	unsigned int uPadding = 0;
+	unsigned int cbReqCount = 0;
+	bool bNeedACK = (bool)(fifo_ctl & FIFOCTL_NEEDACK);
+	bool bRTS = (bool)(fifo_ctl & FIFOCTL_RTS);
+	struct vnt_tx_desc *ptdCurr;
+	unsigned int cbHeaderLength = 0;
+	void *pvRrvTime = NULL;
+	struct vnt_mic_hdr *pMICHDR = NULL;
+	void *pvRTS = NULL;
+	void *pvCTS = NULL;
+	void *pvTxDataHd = NULL;
+	unsigned short wTxBufSize;   /* FFinfo size */
+	unsigned char byFBOption = AUTO_FB_NONE;
+
+	cbFrameSize = skb->len + 4;
+
+	if (info->control.hw_key) {
+		switch (info->control.hw_key->cipher) {
+		case WLAN_CIPHER_SUITE_CCMP:
+			cbMICHDR = sizeof(struct vnt_mic_hdr);
+			break;
+		default:
+			break;
+		}
+
+		cbFrameSize += info->control.hw_key->icv_len;
+
+		if (pDevice->local_id > REV_ID_VT3253_A1) {
+			/* MAC Header should be padding 0 to DW alignment. */
+			uPadding = 4 - (ieee80211_get_hdrlen_from_skb(skb) % 4);
+			uPadding %= 4;
+		}
+	}
+
+	/*
+	 * Use for AUTO FALL BACK
+	 */
+	if (fifo_ctl & FIFOCTL_AUTO_FB_0)
+		byFBOption = AUTO_FB_0;
+	else if (fifo_ctl & FIFOCTL_AUTO_FB_1)
+		byFBOption = AUTO_FB_1;
+
+	/* Set RrvTime/RTS/CTS Buffer */
+	wTxBufSize = sizeof(struct vnt_tx_fifo_head);
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {/* 802.11g packet */
+
+		if (byFBOption == AUTO_FB_NONE) {
+			if (bRTS) {/* RTS_need */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+							cbMICHDR + sizeof(struct vnt_rts_g));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+							cbMICHDR + sizeof(struct vnt_rts_g) +
+							sizeof(struct vnt_tx_datahead_g);
+			} else { /* RTS_needless */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
+				pvRTS = NULL;
+				pvCTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+						sizeof(struct vnt_rrv_time_cts) + cbMICHDR + sizeof(struct vnt_cts));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+							cbMICHDR + sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
+			}
+		} else {
+			/* Auto Fall Back */
+			if (bRTS) {/* RTS_need */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
+				pvRTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+					cbMICHDR + sizeof(struct vnt_rts_g_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+					cbMICHDR + sizeof(struct vnt_rts_g_fb) + sizeof(struct vnt_tx_datahead_g_fb);
+			} else { /* RTS_needless */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
+				pvRTS = NULL;
+				pvCTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
+				pvTxDataHd = (void  *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+					cbMICHDR + sizeof(struct vnt_cts_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+					cbMICHDR + sizeof(struct vnt_cts_fb) + sizeof(struct vnt_tx_datahead_g_fb);
+			}
+		} /* Auto Fall Back */
+	} else {/* 802.11a/b packet */
+
+		if (byFBOption == AUTO_FB_NONE) {
+			if (bRTS) {
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_ab));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_rts_ab) + sizeof(struct vnt_tx_datahead_ab);
+			} else { /* RTS_needless, need MICHDR */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = NULL;
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_tx_datahead_ab);
+			}
+		} else {
+			/* Auto Fall Back */
+			if (bRTS) { /* RTS_need */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_a_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_rts_a_fb) + sizeof(struct vnt_tx_datahead_a_fb);
+			} else { /* RTS_needless */
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = NULL;
+				pvCTS = NULL;
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_tx_datahead_a_fb);
+			}
+		} /* Auto Fall Back */
+	}
+
+	td_info->mic_hdr = pMICHDR;
+
+	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderLength - wTxBufSize));
+
+	/* Fill FIFO,RrvTime,RTS,and CTS */
+	s_vGenerateTxParameter(pDevice, byPktType, tx_buffer_head, pvRrvTime, pvRTS, pvCTS,
+			       cbFrameSize, bNeedACK, uDMAIdx, hdr, pDevice->wCurrentRate);
+	/* Fill DataHead */
+	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
+				    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate, is_pspoll);
+
+	hdr->duration_id = uDuration;
+
+	cbReqCount = cbHeaderLength + uPadding + skb->len;
+	pbyBuffer = (unsigned char *)pHeadTD->td_info->buf;
+	uLength = cbHeaderLength + uPadding;
+
+	/* Copy the Packet into a tx Buffer */
+	memcpy((pbyBuffer + uLength), skb->data, skb->len);
+
+	ptdCurr = pHeadTD;
+
+	ptdCurr->td_info->req_count = (u16)cbReqCount;
+
+	return cbHeaderLength;
+}
+
+static void vnt_fill_txkey(struct ieee80211_hdr *hdr, u8 *key_buffer,
+			   struct ieee80211_key_conf *tx_key,
+			   struct sk_buff *skb,	u16 payload_len,
+			   struct vnt_mic_hdr *mic_hdr)
+{
+	u64 pn64;
+	u8 *iv = ((u8 *)hdr + ieee80211_get_hdrlen_from_skb(skb));
+
+	/* strip header and icv len from payload */
+	payload_len -= ieee80211_get_hdrlen_from_skb(skb);
+	payload_len -= tx_key->icv_len;
+
+	switch (tx_key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		memcpy(key_buffer, iv, 3);
+		memcpy(key_buffer + 3, tx_key->key, tx_key->keylen);
+
+		if (tx_key->keylen == WLAN_KEY_LEN_WEP40) {
+			memcpy(key_buffer + 8, iv, 3);
+			memcpy(key_buffer + 11,
+			       tx_key->key, WLAN_KEY_LEN_WEP40);
+		}
+
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		ieee80211_get_tkip_p2k(tx_key, skb, key_buffer);
+
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+
+		if (!mic_hdr)
+			return;
+
+		mic_hdr->id = 0x59;
+		mic_hdr->payload_len = cpu_to_be16(payload_len);
+		ether_addr_copy(mic_hdr->mic_addr2, hdr->addr2);
+
+		pn64 = atomic64_read(&tx_key->tx_pn);
+		mic_hdr->ccmp_pn[5] = pn64;
+		mic_hdr->ccmp_pn[4] = pn64 >> 8;
+		mic_hdr->ccmp_pn[3] = pn64 >> 16;
+		mic_hdr->ccmp_pn[2] = pn64 >> 24;
+		mic_hdr->ccmp_pn[1] = pn64 >> 32;
+		mic_hdr->ccmp_pn[0] = pn64 >> 40;
+
+		if (ieee80211_has_a4(hdr->frame_control))
+			mic_hdr->hlen = cpu_to_be16(28);
+		else
+			mic_hdr->hlen = cpu_to_be16(22);
+
+		ether_addr_copy(mic_hdr->addr1, hdr->addr1);
+		ether_addr_copy(mic_hdr->addr2, hdr->addr2);
+		ether_addr_copy(mic_hdr->addr3, hdr->addr3);
+
+		mic_hdr->frame_control = cpu_to_le16(
+			le16_to_cpu(hdr->frame_control) & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(
+				le16_to_cpu(hdr->seq_ctrl) & 0xf);
+
+		if (ieee80211_has_a4(hdr->frame_control))
+			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
+
+		memcpy(key_buffer, tx_key->key, WLAN_KEY_LEN_CCMP);
+
+		break;
+	default:
+		break;
+	}
+}
+
+int vnt_generate_fifo_header(struct vnt_private *priv, u32 dma_idx,
+			     struct vnt_tx_desc *head_td, struct sk_buff *skb)
+{
+	struct vnt_td_info *td_info = head_td->td_info;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *tx_rate = &info->control.rates[0];
+	struct ieee80211_rate *rate;
+	struct ieee80211_key_conf *tx_key;
+	struct ieee80211_hdr *hdr;
+	struct vnt_tx_fifo_head *tx_buffer_head =
+			(struct vnt_tx_fifo_head *)td_info->buf;
+	u16 tx_body_size = skb->len, current_rate;
+	u8 pkt_type;
+	bool is_pspoll = false;
+
+	memset(tx_buffer_head, 0, sizeof(*tx_buffer_head));
+
+	hdr = (struct ieee80211_hdr *)(skb->data);
+
+	rate = ieee80211_get_tx_rate(priv->hw, info);
+
+	current_rate = rate->hw_value;
+	if (priv->wCurrentRate != current_rate &&
+	    !(priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
+		priv->wCurrentRate = current_rate;
+
+		RFbSetPower(priv, priv->wCurrentRate,
+			    priv->hw->conf.chandef.chan->hw_value);
+	}
+
+	if (current_rate > RATE_11M) {
+		if (info->band == NL80211_BAND_5GHZ) {
+			pkt_type = PK_TYPE_11A;
+		} else {
+			if (tx_rate->flags & IEEE80211_TX_RC_USE_CTS_PROTECT)
+				pkt_type = PK_TYPE_11GB;
+			else
+				pkt_type = PK_TYPE_11GA;
+		}
+	} else {
+		pkt_type = PK_TYPE_11B;
+	}
+
+	/*Set fifo controls */
+	if (pkt_type == PK_TYPE_11A)
+		tx_buffer_head->fifo_ctl = 0;
+	else if (pkt_type == PK_TYPE_11B)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11B);
+	else if (pkt_type == PK_TYPE_11GB)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GB);
+	else if (pkt_type == PK_TYPE_11GA)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GA);
+
+	/* generate interrupt */
+	tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
+
+	if (!ieee80211_is_data(hdr->frame_control)) {
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_TMOEN);
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_ISDMA0);
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+	} else {
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
+	}
+
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_NEEDACK);
+
+	if (ieee80211_has_retry(hdr->frame_control))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LRETRY);
+
+	if (tx_rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
+		priv->preamble_type = PREAMBLE_SHORT;
+	else
+		priv->preamble_type = PREAMBLE_LONG;
+
+	if (tx_rate->flags & IEEE80211_TX_RC_USE_RTS_CTS)
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_RTS);
+
+	if (ieee80211_has_a4(hdr->frame_control)) {
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LHEAD);
+		priv->bLongHeader = true;
+	}
+
+	if (info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER)
+		is_pspoll = true;
+
+	tx_buffer_head->frag_ctl =
+			cpu_to_le16(ieee80211_get_hdrlen_from_skb(skb) << 10);
+
+	if (info->control.hw_key) {
+		switch (info->control.hw_key->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_LEGACY);
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_TKIP);
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_AES);
+			break;
+		default:
+			break;
+		}
+	}
+
+	tx_buffer_head->current_rate = cpu_to_le16(current_rate);
+
+	/* legacy rates TODO use ieee80211_tx_rate */
+	if (current_rate >= RATE_18M && ieee80211_is_data(hdr->frame_control)) {
+		if (priv->byAutoFBCtrl == AUTO_FB_0)
+			tx_buffer_head->fifo_ctl |=
+						cpu_to_le16(FIFOCTL_AUTO_FB_0);
+		else if (priv->byAutoFBCtrl == AUTO_FB_1)
+			tx_buffer_head->fifo_ctl |=
+						cpu_to_le16(FIFOCTL_AUTO_FB_1);
+	}
+
+	tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_NONFRAG);
+
+	s_cbFillTxBufHead(priv, pkt_type, (u8 *)tx_buffer_head,
+			  dma_idx, head_td, is_pspoll);
+
+	if (info->control.hw_key) {
+		tx_key = info->control.hw_key;
+		if (tx_key->keylen > 0)
+			vnt_fill_txkey(hdr, tx_buffer_head->tx_key,
+				       tx_key, skb, tx_body_size,
+				       td_info->mic_hdr);
+	}
+
+	return 0;
+}
+
+static int vnt_beacon_xmit(struct vnt_private *priv,
+			   struct sk_buff *skb)
+{
+	struct vnt_tx_short_buf_head *short_head =
+		(struct vnt_tx_short_buf_head *)priv->tx_beacon_bufs;
+	struct ieee80211_mgmt *mgmt_hdr = (struct ieee80211_mgmt *)
+				(priv->tx_beacon_bufs + sizeof(*short_head));
+	struct ieee80211_tx_info *info;
+	u32 frame_size = skb->len + 4;
+	u16 current_rate;
+
+	memset(priv->tx_beacon_bufs, 0, sizeof(*short_head));
+
+	if (priv->byBBType == BB_TYPE_11A) {
+		current_rate = RATE_6M;
+
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11A, &short_head->ab);
+
+		/* Get Duration and TimeStampOff */
+		short_head->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(priv, DATADUR_B,
+				    frame_size, PK_TYPE_11A, current_rate,
+				    false, 0, 0, 1, AUTO_FB_NONE));
+
+		short_head->time_stamp_off =
+				vnt_time_stamp_off(priv, current_rate);
+	} else {
+		current_rate = RATE_1M;
+		short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_11B);
+
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11B, &short_head->ab);
+
+		/* Get Duration and TimeStampOff */
+		short_head->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(priv, DATADUR_B,
+				    frame_size, PK_TYPE_11B, current_rate,
+				    false, 0, 0, 1, AUTO_FB_NONE));
+
+		short_head->time_stamp_off =
+			vnt_time_stamp_off(priv, current_rate);
+	}
+
+	short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
+
+	/* Copy Beacon */
+	memcpy(mgmt_hdr, skb->data, skb->len);
+
+	/* time stamp always 0 */
+	mgmt_hdr->u.beacon.timestamp = 0;
+
+	info = IEEE80211_SKB_CB(skb);
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mgmt_hdr;
+
+		hdr->duration_id = 0;
+		hdr->seq_ctrl = cpu_to_le16(priv->wSeqCounter << 4);
+	}
+
+	priv->wSeqCounter++;
+	if (priv->wSeqCounter > 0x0fff)
+		priv->wSeqCounter = 0;
+
+	priv->wBCNBufLen = sizeof(*short_head) + skb->len;
+
+	iowrite32((u32)priv->tx_beacon_dma, priv->port_offset + MAC_REG_BCNDMAPTR);
+
+	iowrite16(priv->wBCNBufLen, priv->port_offset + MAC_REG_BCNDMACTL + 2);
+	/* Set auto Transmit on */
+	vt6655_mac_reg_bits_on(priv->port_offset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	/* Poll Transmit the adapter */
+	iowrite8(BEACON_READY, priv->port_offset + MAC_REG_BCNDMACTL);
+
+	return 0;
+}
+
+int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
+{
+	struct sk_buff *beacon;
+
+	beacon = ieee80211_beacon_get(priv->hw, vif, 0);
+	if (!beacon)
+		return -ENOMEM;
+
+	if (vnt_beacon_xmit(priv, beacon)) {
+		ieee80211_free_txskb(priv->hw, beacon);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
+		      struct ieee80211_bss_conf *conf)
+{
+	iowrite8(TFTCTL_TSFCNTRST, priv->port_offset + MAC_REG_TFTCTL);
+
+	iowrite8(TFTCTL_TSFCNTREN, priv->port_offset + MAC_REG_TFTCTL);
+
+	CARDvSetFirstNextTBTT(priv, conf->beacon_int);
+
+	CARDbSetBeaconPeriod(priv, conf->beacon_int);
+
+	return vnt_beacon_make(priv, vif);
+}
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
new file mode 100644
index 000000000..a67757c9b
--- /dev/null
+++ b/drivers/staging/vt6655/rxtx.h
@@ -0,0 +1,185 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 27, 2002
+ *
+ */
+
+#ifndef __RXTX_H__
+#define __RXTX_H__
+
+#include "device.h"
+
+#define DEFAULT_MSDU_LIFETIME_RES_64us	8000 /* 64us */
+#define DEFAULT_MGN_LIFETIME_RES_64us	125  /* 64us */
+
+
+/*---------------------  Export Definitions -------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+/* MIC HDR data header */
+struct vnt_mic_hdr {
+	u8 id;
+	u8 tx_priority;
+	u8 mic_addr2[ETH_ALEN];
+	u8 ccmp_pn[IEEE80211_CCMP_PN_LEN];
+	__be16 payload_len;
+	__be16 hlen;
+	__le16 frame_control;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctrl;
+	u8 addr4[ETH_ALEN];
+	u16 packing; /* packing to 48 bytes */
+} __packed;
+
+/* RsvTime buffer header */
+struct vnt_rrv_time_rts {
+	__le16 rts_rrv_time_ba;
+	__le16 rts_rrv_time_aa;
+	__le16 rts_rrv_time_bb;
+	u16 reserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_cts {
+	__le16 cts_rrv_time_ba;
+	u16 reserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_ab {
+	__le16 rts_rrv_time;
+	__le16 rrv_time;
+} __packed;
+
+/* TX data header */
+struct vnt_tx_datahead_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_b;
+	__le16 duration_a;
+	__le16 time_stamp_off_b;
+	__le16 time_stamp_off_a;
+} __packed;
+
+struct vnt_tx_datahead_g_fb {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_b;
+	__le16 duration_a;
+	__le16 duration_a_f0;
+	__le16 duration_a_f1;
+	__le16 time_stamp_off_b;
+	__le16 time_stamp_off_a;
+} __packed;
+
+struct vnt_tx_datahead_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
+struct vnt_tx_datahead_a_fb {
+	struct vnt_phy_field a;
+	__le16 duration;
+	__le16 time_stamp_off;
+	__le16 duration_f0;
+	__le16 duration_f1;
+} __packed;
+
+/* RTS buffer header */
+struct vnt_rts_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_ba;
+	__le16 duration_aa;
+	__le16 duration_bb;
+	u16 reserved;
+	struct ieee80211_rts data;
+} __packed __aligned(2);
+
+struct vnt_rts_g_fb {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_ba;
+	__le16 duration_aa;
+	__le16 duration_bb;
+	u16 wReserved;
+	__le16 rts_duration_ba_f0;
+	__le16 rts_duration_aa_f0;
+	__le16 rts_duration_ba_f1;
+	__le16 rts_duration_aa_f1;
+	struct ieee80211_rts data;
+} __packed __aligned(2);
+
+struct vnt_rts_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	u16 reserved;
+	struct ieee80211_rts data;
+} __packed __aligned(2);
+
+struct vnt_rts_a_fb {
+	struct vnt_phy_field a;
+	__le16 duration;
+	u16 reserved;
+	__le16 rts_duration_f0;
+	__le16 rts_duration_f1;
+	struct ieee80211_rts data;
+} __packed __aligned(2);
+
+/* CTS buffer header */
+struct vnt_cts {
+	struct vnt_phy_field b;
+	__le16 duration_ba;
+	u16 reserved;
+	struct ieee80211_cts data;
+	u16 reserved2;
+} __packed __aligned(2);
+
+struct vnt_cts_fb {
+	struct vnt_phy_field b;
+	__le16 duration_ba;
+	u16 reserved;
+	__le16 cts_duration_ba_f0;
+	__le16 cts_duration_ba_f1;
+	struct ieee80211_cts data;
+	u16 reserved2;
+} __packed __aligned(2);
+
+struct vnt_tx_fifo_head {
+	u8 tx_key[WLAN_KEY_LEN_CCMP];
+	__le16 fifo_ctl;
+	__le16 time_stamp;
+	__le16 frag_ctl;
+	__le16 current_rate;
+} __packed;
+
+struct vnt_tx_short_buf_head {
+	__le16 fifo_ctl;
+	u16 time_stamp;
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
+int vnt_generate_fifo_header(struct vnt_private *priv, u32 dma_idx,
+			     struct vnt_tx_desc *head_td, struct sk_buff *skb);
+int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif);
+int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
+		      struct ieee80211_bss_conf *conf);
+
+#endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6655/srom.c b/drivers/staging/vt6655/srom.c
new file mode 100644
index 000000000..ee5ca4db7
--- /dev/null
+++ b/drivers/staging/vt6655/srom.c
@@ -0,0 +1,140 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:Implement functions to access eeprom
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jan 29, 2003
+ *
+ * Functions:
+ *      SROMbyReadEmbedded - Embedded read eeprom via MAC
+ *      SROMbWriteEmbedded - Embedded write eeprom via MAC
+ *      SROMvRegBitsOn - Set Bits On in eeprom
+ *      SROMvRegBitsOff - Clear Bits Off in eeprom
+ *      SROMbIsRegBitsOn - Test if Bits On in eeprom
+ *      SROMbIsRegBitsOff - Test if Bits Off in eeprom
+ *      SROMvReadAllContents - Read all contents in eeprom
+ *      SROMvWriteAllContents - Write all contents in eeprom
+ *      SROMvReadEtherAddress - Read Ethernet Address in eeprom
+ *      SROMvWriteEtherAddress - Write Ethernet Address in eeprom
+ *      SROMvReadSubSysVenId - Read Sub_VID and Sub_SysId in eeprom
+ *      SROMbAutoLoad - Auto Load eeprom to MAC register
+ *
+ * Revision History:
+ *
+ */
+
+#include "device.h"
+#include "mac.h"
+#include "srom.h"
+
+/*---------------------  Static Definitions -------------------------*/
+
+/*---------------------  Static Classes  ----------------------------*/
+
+/*---------------------  Static Variables  --------------------------*/
+
+/*---------------------  Static Functions  --------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+/*
+ * Description: Read a byte from EEPROM, by MAC I2C
+ *
+ * Parameters:
+ *  In:
+ *      iobase          - I/O base address
+ *      byContntOffset  - address of EEPROM
+ *  Out:
+ *      none
+ *
+ * Return Value: data read
+ *
+ */
+unsigned char SROMbyReadEmbedded(void __iomem *iobase,
+				 unsigned char byContntOffset)
+{
+	unsigned short wDelay, wNoACK;
+	unsigned char byWait;
+	unsigned char byData;
+	unsigned char byOrg;
+
+	byData = 0xFF;
+	byOrg = ioread8(iobase + MAC_REG_I2MCFG);
+	/* turn off hardware retry for getting NACK */
+	iowrite8(byOrg & (~I2MCFG_NORETRY), iobase + MAC_REG_I2MCFG);
+	for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
+		iowrite8(EEP_I2C_DEV_ID, iobase + MAC_REG_I2MTGID);
+		iowrite8(byContntOffset, iobase + MAC_REG_I2MTGAD);
+
+		/* issue read command */
+		iowrite8(I2MCSR_EEMR, iobase + MAC_REG_I2MCSR);
+		/* wait DONE be set */
+		for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
+			byWait = ioread8(iobase + MAC_REG_I2MCSR);
+			if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
+				break;
+			udelay(CB_DELAY_LOOP_WAIT);
+		}
+		if ((wDelay < W_MAX_TIMEOUT) &&
+		    (!(byWait & I2MCSR_NACK))) {
+			break;
+		}
+	}
+	byData = ioread8(iobase + MAC_REG_I2MDIPT);
+	iowrite8(byOrg, iobase + MAC_REG_I2MCFG);
+	return byData;
+}
+
+/*
+ * Description: Read all contents of eeprom to buffer
+ *
+ * Parameters:
+ *  In:
+ *      iobase          - I/O base address
+ *  Out:
+ *      pbyEepromRegs   - EEPROM content Buffer
+ *
+ * Return Value: none
+ *
+ */
+void SROMvReadAllContents(void __iomem *iobase, unsigned char *pbyEepromRegs)
+{
+	int     ii;
+
+	/* ii = Rom Address */
+	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
+		*pbyEepromRegs = SROMbyReadEmbedded(iobase,
+						    (unsigned char)ii);
+		pbyEepromRegs++;
+	}
+}
+
+/*
+ * Description: Read Ethernet Address from eeprom to buffer
+ *
+ * Parameters:
+ *  In:
+ *      iobase          - I/O base address
+ *  Out:
+ *      pbyEtherAddress - Ethernet Address buffer
+ *
+ * Return Value: none
+ *
+ */
+void SROMvReadEtherAddress(void __iomem *iobase,
+			   unsigned char *pbyEtherAddress)
+{
+	unsigned char ii;
+
+	/* ii = Rom Address */
+	for (ii = 0; ii < ETH_ALEN; ii++) {
+		*pbyEtherAddress = SROMbyReadEmbedded(iobase, ii);
+		pbyEtherAddress++;
+	}
+}
diff --git a/drivers/staging/vt6655/srom.h b/drivers/staging/vt6655/srom.h
new file mode 100644
index 000000000..b03073ffa
--- /dev/null
+++ b/drivers/staging/vt6655/srom.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions to access eeprom
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jan 29, 2003
+ */
+
+#ifndef __SROM_H__
+#define __SROM_H__
+
+/*---------------------  Export Definitions -------------------------*/
+
+#define EEP_MAX_CONTEXT_SIZE    256
+
+#define CB_EEPROM_READBYTE_WAIT 900     /* us */
+
+#define W_MAX_I2CRETRY          0x0fff
+
+/* Contents in the EEPROM */
+#define EEP_OFS_PAR         0x00        /* physical address */
+#define EEP_OFS_ANTENNA     0x16
+#define EEP_OFS_RADIOCTL    0x17
+#define EEP_OFS_RFTYPE      0x1B        /* for select RF */
+#define EEP_OFS_MINCHANNEL  0x1C        /* Min Channel # */
+#define EEP_OFS_MAXCHANNEL  0x1D        /* Max Channel # */
+#define EEP_OFS_SIGNATURE   0x1E
+#define EEP_OFS_ZONETYPE    0x1F
+#define EEP_OFS_RFTABLE     0x20        /* RF POWER TABLE */
+#define EEP_OFS_PWR_CCK     0x20
+#define EEP_OFS_SETPT_CCK   0x21
+#define EEP_OFS_PWR_OFDMG   0x23
+#define EEP_OFS_SETPT_OFDMG 0x24
+#define EEP_OFS_PWR_FORMULA_OST  0x26
+#define EEP_OFS_MAJOR_VER 0x2E
+#define EEP_OFS_MINOR_VER 0x2F
+#define EEP_OFS_CCK_PWR_TBL     0x30
+#define EEP_OFS_CCK_PWR_dBm     0x3F
+#define EEP_OFS_OFDM_PWR_TBL    0x40
+#define EEP_OFS_OFDM_PWR_dBm    0x4F
+/*{{ RobertYu: 20041124 */
+#define EEP_OFS_SETPT_OFDMA         0x4E
+#define EEP_OFS_OFDMA_PWR_TBL       0x50
+/*}}*/
+#define EEP_OFS_OFDMA_PWR_dBm       0xD2
+
+/*----------need to remove --------------------*/
+#define EEP_OFS_BBTAB_LEN   0x70        /* BB Table Length */
+#define EEP_OFS_BBTAB_ADR   0x71        /* BB Table Offset */
+#define EEP_OFS_CHECKSUM    0xFF        /* reserved area for baseband 28h~78h */
+
+#define EEP_I2C_DEV_ID      0x50        /* EEPROM device address on I2C bus */
+
+/* Bits in EEP_OFS_ANTENNA */
+#define EEP_ANTENNA_MAIN    0x01
+#define EEP_ANTENNA_AUX     0x02
+#define EEP_ANTINV          0x04
+
+/* Bits in EEP_OFS_RADIOCTL */
+#define EEP_RADIOCTL_ENABLE 0x80
+#define EEP_RADIOCTL_INV    0x01
+
+/*---------------------  Export Types  ------------------------------*/
+
+/*---------------------  Export Macros ------------------------------*/
+
+/*---------------------  Export Classes  ----------------------------*/
+
+/*---------------------  Export Variables  --------------------------*/
+
+/*---------------------  Export Functions  --------------------------*/
+
+unsigned char SROMbyReadEmbedded(void __iomem *iobase,
+				 unsigned char byContntOffset);
+
+void SROMvReadAllContents(void __iomem *iobase, unsigned char *pbyEepromRegs);
+
+void SROMvReadEtherAddress(void __iomem *iobase,
+			   unsigned char *pbyEtherAddress);
+
+#endif /* __EEPROM_H__*/
diff --git a/drivers/staging/vt6655/test b/drivers/staging/vt6655/test
new file mode 100644
index 000000000..ba6dec774
--- /dev/null
+++ b/drivers/staging/vt6655/test
@@ -0,0 +1,9 @@
+KSP :=  /lib/modules/$(shell uname -r)/build \
+	/usr/src/linux-$(shell uname -r) \
+	/usr/src/linux-$(shell uname -r | sed 's/-.*//') \
+#	/usr/src/kernel-headers-$(shell uname -r) \
+#	/usr/src/kernel-source-$(shell uname -r) \
+#	/usr/src/linux-$(shell uname -r | sed 's/\([0-9]*\.[0-9]*\)\..*/\1/') \
+#	/usr/src/linux   /home/plice
+test_dir = $(shell [ -e $(dir)/include/linux ] && echo $(dir))
+KSP := $(foreach dir, $(KSP), $(test_dir))
diff --git a/drivers/staging/vt6656/Kconfig b/drivers/staging/vt6656/Kconfig
new file mode 100644
index 000000000..f52a3f1d9
--- /dev/null
+++ b/drivers/staging/vt6656/Kconfig
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+config VT6656
+	tristate "VIA Technologies VT6656 support"
+	depends on MAC80211 && USB && WLAN && m
+	select FW_LOADER
+	help
+	  This is a vendor-written driver for VIA VT6656.
diff --git a/drivers/staging/vt6656/Makefile b/drivers/staging/vt6656/Makefile
new file mode 100644
index 000000000..f696a9d7a
--- /dev/null
+++ b/drivers/staging/vt6656/Makefile
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0
+
+vt6656_stage-y +=	main_usb.o \
+			card.o  \
+			mac.o   \
+			baseband.o \
+			wcmd.o\
+			rxtx.o \
+			power.o \
+			key.o \
+			rf.o \
+			usbpipe.o \
+			channel.o
+
+obj-$(CONFIG_VT6656) +=	vt6656_stage.o
diff --git a/drivers/staging/vt6656/TODO b/drivers/staging/vt6656/TODO
new file mode 100644
index 000000000..e154b2f3b
--- /dev/null
+++ b/drivers/staging/vt6656/TODO
@@ -0,0 +1,19 @@
+TODO:
+- remove __cplusplus ifdefs -- done
+- remove kernel version compatibility wrappers
+- remove support for older wireless extensions
+- prepare for merge with vt6655 driver:
+  - remove PRINT_K() macro
+  - split rf.c
+  - abstract VT3184 chipset specific code
+- add common vt665x infrastructure
+- kill ttype.h -- done
+- switch to use LIB80211
+- switch to use MAC80211
+- use kernel coding style
+- checkpatch.pl fixes
+- sparse fixes
+- integrate with drivers/net/wireless
+
+Please send any patches to Greg Kroah-Hartman <greg@kroah.com>
+and Forest Bond <forest@alittletooquiet.net>.
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
new file mode 100644
index 000000000..ad7b963f0
--- /dev/null
+++ b/drivers/staging/vt6656/baseband.c
@@ -0,0 +1,455 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions to access baseband
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 5, 2002
+ *
+ * Functions:
+ *	vnt_get_frame_time	- Calculate data frame transmitting time
+ *	vnt_get_phy_field	- Calculate PhyLength, PhyService and Phy
+ *				  Signal parameter for baseband Tx
+ *	vnt_vt3184_init		- VIA VT3184 baseband chip init code
+ *
+ * Revision History:
+ *
+ *
+ */
+
+#include <linux/bits.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include "device.h"
+#include "mac.h"
+#include "baseband.h"
+#include "rf.h"
+#include "usbpipe.h"
+
+static const u8 vnt_vt3184_agc[] = {
+	0x00, 0x00, 0x02, 0x02, 0x04, 0x04, 0x06, 0x06,
+	0x08, 0x08, 0x0a, 0x0a, 0x0c, 0x0c, 0x0e, 0x0e, /* 0x0f */
+	0x10, 0x10, 0x12, 0x12, 0x14, 0x14, 0x16, 0x16,
+	0x18, 0x18, 0x1a, 0x1a, 0x1c, 0x1c, 0x1e, 0x1e, /* 0x1f */
+	0x20, 0x20, 0x22, 0x22, 0x24, 0x24, 0x26, 0x26,
+	0x28, 0x28, 0x2a, 0x2a, 0x2c, 0x2c, 0x2e, 0x2e, /* 0x2f */
+	0x30, 0x30, 0x32, 0x32, 0x34, 0x34, 0x36, 0x36,
+	0x38, 0x38, 0x3a, 0x3a, 0x3c, 0x3c, 0x3e, 0x3e  /* 0x3f */
+};
+
+static u8 vnt_vt3184_al2230[] = {
+	0x31, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00,
+	0x70, 0x45, 0x2a, 0x76, 0x00, 0x00, 0x80, 0x00, /* 0x0f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x8e, 0x0a, 0x00, 0x00, 0x00, /* 0x1f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x4a, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x4a, 0x00, 0x0c, /* 0x2f */
+	0x26, 0x5b, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa,
+	0xff, 0xff, 0x79, 0x00, 0x00, 0x0b, 0x48, 0x04, /* 0x3f */
+	0x00, 0x08, 0x00, 0x08, 0x08, 0x14, 0x05, 0x09,
+	0x00, 0x00, 0x00, 0x00, 0x09, 0x73, 0x00, 0xc5, /* 0x4f */
+	0x00, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x5f */
+	0xe4, 0x80, 0x00, 0x00, 0x00, 0x00, 0x98, 0x0a,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x01, 0x00, /* 0x6f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x7f */
+	0x8c, 0x01, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x08, 0x00, 0x1f, 0xb7, 0x88, 0x47, 0xaa, 0x00, /* 0x8f */
+	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xeb,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, /* 0x9f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+	0x18, 0x00, 0x00, 0x00, 0x00, 0x15, 0x00, 0x18, /* 0xaf */
+	0x38, 0x30, 0x00, 0x00, 0xff, 0x0f, 0xe4, 0xe2,
+	0x00, 0x00, 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, /* 0xbf */
+	0x18, 0x20, 0x07, 0x18, 0xff, 0xff, 0x0e, 0x0a,
+	0x0e, 0x00, 0x82, 0xa7, 0x3c, 0x10, 0x30, 0x05, /* 0xcf */
+	0x40, 0x12, 0x00, 0x00, 0x10, 0x28, 0x80, 0x2a,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xdf */
+	0x00, 0xf3, 0x00, 0x00, 0x00, 0x10, 0x00, 0x12,
+	0x00, 0xf4, 0x00, 0xff, 0x79, 0x20, 0x30, 0x05, /* 0xef */
+	0x00, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00  /* 0xff */
+};
+
+/* {{RobertYu:20060515, new BB setting for VT3226D0 */
+static const u8 vnt_vt3184_vt3226d0[] = {
+	0x31, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00,
+	0x70, 0x45, 0x2a, 0x76, 0x00, 0x00, 0x80, 0x00, /* 0x0f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x8e, 0x0a, 0x00, 0x00, 0x00, /* 0x1f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x4a, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x4a, 0x00, 0x0c, /* 0x2f */
+	0x26, 0x5b, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa,
+	0xff, 0xff, 0x79, 0x00, 0x00, 0x0b, 0x48, 0x04, /* 0x3f */
+	0x00, 0x08, 0x00, 0x08, 0x08, 0x14, 0x05, 0x09,
+	0x00, 0x00, 0x00, 0x00, 0x09, 0x73, 0x00, 0xc5, /* 0x4f */
+	0x00, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x5f */
+	0xe4, 0x80, 0x00, 0x00, 0x00, 0x00, 0x98, 0x0a,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x01, 0x00, /* 0x6f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x7f */
+	0x8c, 0x01, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x08, 0x00, 0x1f, 0xb7, 0x88, 0x47, 0xaa, 0x00, /* 0x8f */
+	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xeb,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, /* 0x9f */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+	0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, /* 0xaf */
+	0x38, 0x30, 0x00, 0x00, 0xff, 0x0f, 0xe4, 0xe2,
+	0x00, 0x00, 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, /* 0xbf */
+	0x18, 0x20, 0x07, 0x18, 0xff, 0xff, 0x10, 0x0a,
+	0x0e, 0x00, 0x84, 0xa7, 0x3c, 0x10, 0x24, 0x05, /* 0xcf */
+	0x40, 0x12, 0x00, 0x00, 0x10, 0x28, 0x80, 0x2a,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xdf */
+	0x00, 0xf3, 0x00, 0x00, 0x00, 0x10, 0x00, 0x10,
+	0x00, 0xf4, 0x00, 0xff, 0x79, 0x20, 0x30, 0x08, /* 0xef */
+	0x00, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00  /* 0xff */
+};
+
+struct vnt_threshold {
+	u8 bb_pre_ed_rssi;
+	u8 cr_201;
+	u8 cr_206;
+};
+
+static const struct vnt_threshold al2230_vnt_threshold[] = {
+	{0, 0x00, 0x30},	/* Max sensitivity */
+	{68, 0x00, 0x36},
+	{67, 0x00, 0x43},
+	{66, 0x00, 0x51},
+	{65, 0x00, 0x62},
+	{64, 0x00, 0x79},
+	{63, 0x00, 0x93},
+	{62, 0x00, 0xb9},
+	{61, 0x00, 0xe3},
+	{60, 0x01, 0x18},
+	{59, 0x01, 0x54},
+	{58, 0x01, 0xa0},
+	{57, 0x02, 0x20},
+	{56, 0x02, 0xa0},
+	{55, 0x03, 0x00},
+	{53, 0x06, 0x00},
+	{51, 0x09, 0x00},
+	{49, 0x0e, 0x00},
+	{47, 0x15, 0x00},
+	{46, 0x1a, 0x00},
+	{45, 0xff, 0x00}
+};
+
+static const struct vnt_threshold vt3226_vnt_threshold[] = {
+	{0, 0x00, 0x24},	/* Max sensitivity */
+	{68, 0x00, 0x2d},
+	{67, 0x00, 0x36},
+	{66, 0x00, 0x43},
+	{65, 0x00, 0x52},
+	{64, 0x00, 0x68},
+	{63, 0x00, 0x80},
+	{62, 0x00, 0x9c},
+	{61, 0x00, 0xc0},
+	{60, 0x00, 0xea},
+	{59, 0x01, 0x30},
+	{58, 0x01, 0x70},
+	{57, 0x01, 0xb0},
+	{56, 0x02, 0x30},
+	{55, 0x02, 0xc0},
+	{53, 0x04, 0x00},
+	{51, 0x07, 0x00},
+	{49, 0x0a, 0x00},
+	{47, 0x11, 0x00},
+	{45, 0x18, 0x00},
+	{43, 0x26, 0x00},
+	{42, 0x36, 0x00},
+	{41, 0xff, 0x00}
+};
+
+/*
+ * Description: Set Antenna mode
+ *
+ * Parameters:
+ *  In:
+ *	priv		- Device Structure
+ *	antenna_mode	- Antenna Mode
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+int vnt_set_antenna_mode(struct vnt_private *priv, u8 antenna_mode)
+{
+	switch (antenna_mode) {
+	case ANT_TXA:
+	case ANT_TXB:
+		break;
+	case ANT_RXA:
+		priv->bb_rx_conf &= 0xFC;
+		break;
+	case ANT_RXB:
+		priv->bb_rx_conf &= 0xFE;
+		priv->bb_rx_conf |= 0x02;
+		break;
+	}
+
+	return vnt_control_out(priv, MESSAGE_TYPE_SET_ANTMD,
+			       (u16)antenna_mode, 0, 0, NULL);
+}
+
+/*
+ * Description: Set Antenna mode
+ *
+ * Parameters:
+ *  In:
+ *      pDevice          - Device Structure
+ *      byAntennaMode    - Antenna Mode
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+
+int vnt_vt3184_init(struct vnt_private *priv)
+{
+	int ret;
+	u16 length;
+	u8 *addr = NULL;
+	const u8 *c_addr;
+	u8 data;
+
+	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, 0, MESSAGE_REQUEST_EEPROM,
+			     EEP_MAX_CONTEXT_SIZE, priv->eeprom);
+	if (ret)
+		goto end;
+
+	priv->rf_type = priv->eeprom[EEP_OFS_RFTYPE];
+
+	dev_dbg(&priv->usb->dev, "RF Type %d\n", priv->rf_type);
+
+	if ((priv->rf_type == RF_AL2230) ||
+	    (priv->rf_type == RF_AL2230S)) {
+		priv->bb_rx_conf = vnt_vt3184_al2230[10];
+		length = sizeof(vnt_vt3184_al2230);
+		addr = vnt_vt3184_al2230;
+
+		priv->bb_vga[0] = 0x1c;
+		priv->bb_vga[1] = 0x10;
+		priv->bb_vga[2] = 0x0;
+		priv->bb_vga[3] = 0x0;
+
+	} else if ((priv->rf_type == RF_VT3226) ||
+		   (priv->rf_type == RF_VT3226D0)) {
+		priv->bb_rx_conf = vnt_vt3184_vt3226d0[10];
+		length = sizeof(vnt_vt3184_vt3226d0);
+		c_addr = vnt_vt3184_vt3226d0;
+
+		priv->bb_vga[0] = 0x20;
+		priv->bb_vga[1] = 0x10;
+		priv->bb_vga[2] = 0x0;
+		priv->bb_vga[3] = 0x0;
+
+		/* Fix VT3226 DFC system timing issue */
+		ret = vnt_mac_reg_bits_on(priv, MAC_REG_SOFTPWRCTL2,
+					  SOFTPWRCTL_RFLEOPT);
+		if (ret)
+			goto end;
+	} else {
+		goto end;
+	}
+
+	if (addr)
+		c_addr = addr;
+
+	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+				     MESSAGE_REQUEST_BBREG, length, c_addr);
+	if (ret)
+		goto end;
+
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+			      MESSAGE_REQUEST_BBAGC,
+			      sizeof(vnt_vt3184_agc), vnt_vt3184_agc);
+	if (ret)
+		goto end;
+
+	if ((priv->rf_type == RF_VT3226) ||
+	    (priv->rf_type == RF_VT3226D0)) {
+		data = (priv->rf_type == RF_VT3226D0) ? 0x11 : 0x23;
+
+		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
+					 MAC_REG_ITRTMSET, data);
+		if (ret)
+			goto end;
+
+		ret = vnt_mac_reg_bits_on(priv, MAC_REG_PAPEDELAY, BIT(0));
+		if (ret)
+			goto end;
+	}
+
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x04, 0x7f);
+	if (ret)
+		goto end;
+
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
+	if (ret)
+		goto end;
+
+	ret = vnt_rf_table_download(priv);
+	if (ret)
+		goto end;
+
+	/* Fix for TX USB resets from vendors driver */
+	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, USB_REG4,
+			     MESSAGE_REQUEST_MEM, sizeof(data), &data);
+	if (ret)
+		goto end;
+
+	data |= 0x2;
+
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, USB_REG4,
+			      MESSAGE_REQUEST_MEM, sizeof(data), &data);
+
+end:
+	return ret;
+}
+
+/*
+ * Description: Set ShortSlotTime mode
+ *
+ * Parameters:
+ *  In:
+ *	priv	- Device Structure
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+int vnt_set_short_slot_time(struct vnt_private *priv)
+{
+	int ret = 0;
+	u8 bb_vga = 0;
+
+	if (priv->short_slot_time)
+		priv->bb_rx_conf &= 0xdf;
+	else
+		priv->bb_rx_conf |= 0x20;
+
+	ret = vnt_control_in_u8(priv, MESSAGE_REQUEST_BBREG, 0xe7, &bb_vga);
+	if (ret)
+		return ret;
+
+	if (bb_vga == priv->bb_vga[0])
+		priv->bb_rx_conf |= 0x20;
+
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a,
+				  priv->bb_rx_conf);
+}
+
+int vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data)
+{
+	int ret;
+
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xE7, data);
+	if (ret)
+		return ret;
+
+	/* patch for 3253B0 Baseband with Cardbus module */
+	if (priv->short_slot_time)
+		priv->bb_rx_conf &= 0xdf; /* 1101 1111 */
+	else
+		priv->bb_rx_conf |= 0x20; /* 0010 0000 */
+
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a,
+				  priv->bb_rx_conf);
+}
+
+/*
+ * Description: vnt_set_deep_sleep
+ *
+ * Parameters:
+ *  In:
+ *	priv	- Device Structure
+ *  Out:
+ *      none
+ *
+ * Return Value: none
+ *
+ */
+int vnt_set_deep_sleep(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	/* CR12 */
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x17);
+	if (ret)
+		return ret;
+
+	/* CR13 */
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0xB9);
+}
+
+int vnt_exit_deep_sleep(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	/* CR12 */
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x00);
+	if (ret)
+		return ret;
+
+	/* CR13 */
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
+}
+
+int vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning)
+{
+	const struct vnt_threshold *threshold = NULL;
+	u8 length;
+	u8 cr_201, cr_206;
+	u8 ed_inx;
+	int ret;
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+	case RF_AL2230S:
+		threshold = al2230_vnt_threshold;
+		length = ARRAY_SIZE(al2230_vnt_threshold);
+		break;
+
+	case RF_VT3226:
+	case RF_VT3226D0:
+		threshold = vt3226_vnt_threshold;
+		length = ARRAY_SIZE(vt3226_vnt_threshold);
+		break;
+	}
+
+	if (!threshold)
+		return -EINVAL;
+
+	for (ed_inx = scanning ? 0 : length - 1; ed_inx > 0; ed_inx--) {
+		if (priv->bb_pre_ed_rssi <= threshold[ed_inx].bb_pre_ed_rssi)
+			break;
+	}
+
+	cr_201 = threshold[ed_inx].cr_201;
+	cr_206 = threshold[ed_inx].cr_206;
+
+	if (ed_inx == priv->bb_pre_ed_index && !scanning)
+		return 0;
+
+	priv->bb_pre_ed_index = ed_inx;
+
+	dev_dbg(&priv->usb->dev, "%s bb_pre_ed_rssi %d\n",
+		__func__, priv->bb_pre_ed_rssi);
+
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xc9, cr_201);
+	if (ret)
+		return ret;
+
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xce, cr_206);
+}
+
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
new file mode 100644
index 000000000..dce50a311
--- /dev/null
+++ b/drivers/staging/vt6656/baseband.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions to access baseband
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 5, 2002
+ *
+ * Revision History:
+ *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-26-2003 Kyle Hsu    :  Add defines of packet type and TX rate.
+ */
+
+#ifndef __BASEBAND_H__
+#define __BASEBAND_H__
+
+#include "device.h"
+
+#define PREAMBLE_LONG   0
+#define PREAMBLE_SHORT  1
+
+/*
+ * Registers in the BASEBAND
+ */
+#define BB_MAX_CONTEXT_SIZE 256
+
+#define C_SIFS_A      16      /* usec */
+#define C_SIFS_BG     10
+
+#define C_EIFS      80      /* usec */
+
+#define C_SLOT_SHORT   9      /* usec */
+#define C_SLOT_LONG   20
+
+#define C_CWMIN_A     15       /* slot time */
+#define C_CWMIN_B     31
+
+#define C_CWMAX      1023     /* slot time */
+
+/* 0:11A 1:11B 2:11G */
+#define BB_TYPE_11A    0
+#define BB_TYPE_11B    1
+#define BB_TYPE_11G    2
+
+/* 0:11a, 1:11b, 2:11gb (only CCK in BasicRate), 3:11ga (OFDM in BasicRate) */
+#define PK_TYPE_11A     0
+#define PK_TYPE_11B     1
+#define PK_TYPE_11GB    2
+#define PK_TYPE_11GA    3
+
+#define TOP_RATE_54M        0x80000000
+#define TOP_RATE_48M        0x40000000
+#define TOP_RATE_36M        0x20000000
+#define TOP_RATE_24M        0x10000000
+#define TOP_RATE_18M        0x08000000
+#define TOP_RATE_12M        0x04000000
+#define TOP_RATE_11M        0x02000000
+#define TOP_RATE_9M         0x01000000
+#define TOP_RATE_6M         0x00800000
+#define TOP_RATE_55M        0x00400000
+#define TOP_RATE_2M         0x00200000
+#define TOP_RATE_1M         0x00100000
+
+int vnt_set_short_slot_time(struct vnt_private *priv);
+int vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data);
+int vnt_set_antenna_mode(struct vnt_private *priv, u8 antenna_mode);
+int vnt_vt3184_init(struct vnt_private *priv);
+int vnt_set_deep_sleep(struct vnt_private *priv);
+int vnt_exit_deep_sleep(struct vnt_private *priv);
+int vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning);
+
+#endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
new file mode 100644
index 000000000..7827e579e
--- /dev/null
+++ b/drivers/staging/vt6656/card.c
@@ -0,0 +1,477 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Provide functions to setup NIC operation mode
+ * Functions:
+ *      vnt_set_rspinf - Set RSPINF
+ *      vnt_update_ifs - Update slotTime,SIFS,DIFS, and EIFS
+ *      vnt_update_top_rates - Update BasicTopRate
+ *      vnt_add_basic_rate - Add to BasicRateSet
+ *      vnt_ofdm_min_rate - Check if any OFDM rate is in BasicRateSet
+ *      vnt_get_tsf_offset - Calculate TSFOffset
+ *      vnt_get_current_tsf - Read Current NIC TSF counter
+ *      vnt_get_next_tbtt - Calculate Next Beacon TSF counter
+ *      vnt_reset_next_tbtt - Set NIC Beacon time
+ *      vnt_update_next_tbtt - Sync. NIC Beacon time
+ *      vnt_radio_power_off - Turn Off NIC Radio Power
+ *      vnt_radio_power_on - Turn On NIC Radio Power
+ *
+ * Revision History:
+ *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-26-2003 Kyle Hsu:      Modify the definition type of dwIoBase.
+ *      09-01-2003 Bryan YC Fan:  Add vnt_update_ifs().
+ *
+ */
+
+#include <linux/bitops.h>
+#include <linux/errno.h>
+#include "device.h"
+#include "card.h"
+#include "baseband.h"
+#include "mac.h"
+#include "desc.h"
+#include "rf.h"
+#include "power.h"
+#include "key.h"
+#include "usbpipe.h"
+
+/* const u16 cw_rxbcntsf_off[MAX_RATE] =
+ *   {17, 34, 96, 192, 34, 23, 17, 11, 8, 5, 4, 3};
+ */
+
+static const u16 cw_rxbcntsf_off[MAX_RATE] = {
+	192, 96, 34, 17, 34, 23, 17, 11, 8, 5, 4, 3
+};
+
+int vnt_set_channel(struct vnt_private *priv, u32 connection_channel)
+{
+	int ret;
+
+	if (connection_channel > CB_MAX_CHANNEL || !connection_channel)
+		return -EINVAL;
+
+	/* clear NAV */
+	vnt_mac_reg_bits_on(priv, MAC_REG_MACCR, MACCR_CLRNAV);
+
+	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
+	vnt_mac_reg_bits_off(priv, MAC_REG_CHANNEL,
+			     (BIT(7) | BIT(5) | BIT(4)));
+
+	ret = vnt_control_out(priv, MESSAGE_TYPE_SELECT_CHANNEL,
+			      connection_channel, 0, 0, NULL);
+	if (ret)
+		return ret;
+
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
+				  (u8)(connection_channel | 0x80));
+}
+
+static const u8 vnt_rspinf_b_short_table[] = {
+	0x70, 0x00, 0x00, 0x00, 0x38, 0x00, 0x09, 0x00,
+	0x15, 0x00, 0x0a, 0x00, 0x0b, 0x00, 0x0b, 0x80
+};
+
+static const u8 vnt_rspinf_b_long_table[] = {
+	0x70, 0x00, 0x00, 0x00, 0x38, 0x00, 0x01, 0x00,
+	0x15, 0x00, 0x02, 0x00, 0x0b, 0x00, 0x03, 0x80
+};
+
+static const u8 vnt_rspinf_a_table[] = {
+	0x9b, 0x18, 0x9f, 0x10, 0x9a, 0x0a, 0x9e, 0x08, 0x99,
+	0x08, 0x9d, 0x04, 0x98, 0x04, 0x9c, 0x04, 0x9c, 0x04
+};
+
+static const u8 vnt_rspinf_gb_table[] = {
+	0x8b, 0x1e, 0x8f, 0x16, 0x8a, 0x12, 0x8e, 0x0e, 0x89,
+	0x0e, 0x8d, 0x0a, 0x88, 0x0a, 0x8c, 0x0a, 0x8c, 0x0a
+};
+
+int vnt_set_rspinf(struct vnt_private *priv, u8 bb_type)
+{
+	const u8 *data;
+	u16 len;
+	int ret;
+
+	if (priv->preamble_type) {
+		data = vnt_rspinf_b_short_table;
+		len = ARRAY_SIZE(vnt_rspinf_b_short_table);
+	} else {
+		data = vnt_rspinf_b_long_table;
+		len = ARRAY_SIZE(vnt_rspinf_b_long_table);
+	}
+
+	 /* RSPINF_b_1 to RSPINF_b_11 */
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_RSPINF_B_1,
+			      MESSAGE_REQUEST_MACREG, len, data);
+	if (ret)
+		return ret;
+
+	if (bb_type == BB_TYPE_11A) {
+		data = vnt_rspinf_a_table;
+		len = ARRAY_SIZE(vnt_rspinf_a_table);
+	} else {
+		data = vnt_rspinf_gb_table;
+		len = ARRAY_SIZE(vnt_rspinf_gb_table);
+	}
+
+	/* RSPINF_a_6 to RSPINF_a_72 */
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_RSPINF_A_6,
+			       MESSAGE_REQUEST_MACREG, len, data);
+}
+
+int vnt_update_ifs(struct vnt_private *priv)
+{
+	u8 max_min = 0;
+	u8 data[4];
+	int ret;
+
+	if (priv->packet_type == PK_TYPE_11A) {
+		priv->slot = C_SLOT_SHORT;
+		priv->sifs = C_SIFS_A;
+		priv->difs = C_SIFS_A + 2 * C_SLOT_SHORT;
+		max_min = 4;
+	} else {
+		priv->sifs = C_SIFS_BG;
+
+		if (priv->short_slot_time) {
+			priv->slot = C_SLOT_SHORT;
+			max_min = 4;
+		} else {
+			priv->slot = C_SLOT_LONG;
+			max_min = 5;
+		}
+
+		priv->difs = C_SIFS_BG + 2 * priv->slot;
+	}
+
+	priv->eifs = C_EIFS;
+
+	data[0] = (u8)priv->sifs;
+	data[1] = (u8)priv->difs;
+	data[2] = (u8)priv->eifs;
+	data[3] = (u8)priv->slot;
+
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_SIFS,
+			      MESSAGE_REQUEST_MACREG, 4, &data[0]);
+	if (ret)
+		return ret;
+
+	max_min |= 0xa0;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_CWMAXMIN0,
+			       MESSAGE_REQUEST_MACREG, 1, &max_min);
+}
+
+void vnt_update_top_rates(struct vnt_private *priv)
+{
+	int pos;
+
+	pos = fls(priv->basic_rates & GENMASK(RATE_54M, RATE_6M));
+	priv->top_ofdm_basic_rate = pos ? (pos - 1) : RATE_24M;
+
+	pos = fls(priv->basic_rates & GENMASK(RATE_11M, RATE_1M));
+	priv->top_cck_basic_rate = pos ? (pos - 1) : RATE_1M;
+}
+
+bool vnt_ofdm_min_rate(struct vnt_private *priv)
+{
+	return priv->basic_rates & GENMASK(RATE_54M, RATE_6M) ? true : false;
+}
+
+u8 vnt_get_pkt_type(struct vnt_private *priv)
+{
+	if (priv->bb_type == BB_TYPE_11A || priv->bb_type == BB_TYPE_11B)
+		return (u8)priv->bb_type;
+	else if (vnt_ofdm_min_rate(priv))
+		return PK_TYPE_11GA;
+	return PK_TYPE_11GB;
+}
+
+/*
+ * Description: Calculate TSF offset of two TSF input
+ *              Get TSF Offset from RxBCN's TSF and local TSF
+ *
+ * Parameters:
+ *  In:
+ *      rx_rate	- rx rate.
+ *      tsf1	- Rx BCN's TSF
+ *      tsf2	- Local TSF
+ *  Out:
+ *      none
+ *
+ * Return Value: TSF Offset value
+ *
+ */
+u64 vnt_get_tsf_offset(u8 rx_rate, u64 tsf1, u64 tsf2)
+{
+	return tsf1 - tsf2 - (u64)cw_rxbcntsf_off[rx_rate % MAX_RATE];
+}
+
+int vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
+		   u64 time_stamp, u64 local_tsf)
+{
+	u64 tsf_offset = 0;
+	u8 data[8];
+
+	tsf_offset = vnt_get_tsf_offset(rx_rate, time_stamp, local_tsf);
+
+	data[0] = (u8)tsf_offset;
+	data[1] = (u8)(tsf_offset >> 8);
+	data[2] = (u8)(tsf_offset >> 16);
+	data[3] = (u8)(tsf_offset >> 24);
+	data[4] = (u8)(tsf_offset >> 32);
+	data[5] = (u8)(tsf_offset >> 40);
+	data[6] = (u8)(tsf_offset >> 48);
+	data[7] = (u8)(tsf_offset >> 56);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			       MESSAGE_REQUEST_TSF, 0, 8, data);
+}
+
+/*
+ * Description: Read NIC TSF counter
+ *              Get local TSF counter
+ *
+ * Parameters:
+ *  In:
+ *	priv		- The adapter to be read
+ *  Out:
+ *	current_tsf	- Current TSF counter
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+bool vnt_get_current_tsf(struct vnt_private *priv, u64 *current_tsf)
+{
+	*current_tsf = priv->current_tsf;
+
+	return true;
+}
+
+/*
+ * Description: Clear NIC TSF counter
+ *              Clear local TSF counter
+ *
+ * Parameters:
+ *  In:
+ *      priv	- The adapter to be read
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+bool vnt_clear_current_tsf(struct vnt_private *priv)
+{
+	vnt_mac_reg_bits_on(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+
+	priv->current_tsf = 0;
+
+	return true;
+}
+
+/*
+ * Description: Read NIC TSF counter
+ *              Get NEXTTBTT from adjusted TSF and Beacon Interval
+ *
+ * Parameters:
+ *  In:
+ *      tsf		- Current TSF counter
+ *      beacon_interval - Beacon Interval
+ *  Out:
+ *      tsf		- Current TSF counter
+ *
+ * Return Value: TSF value of next Beacon
+ *
+ */
+u64 vnt_get_next_tbtt(u64 tsf, u16 beacon_interval)
+{
+	u32 beacon_int;
+
+	beacon_int = beacon_interval * 1024;
+
+	/* Next TBTT =
+	 *	((local_current_TSF / beacon_interval) + 1) * beacon_interval
+	 */
+	if (beacon_int) {
+		do_div(tsf, beacon_int);
+		tsf += 1;
+		tsf *= beacon_int;
+	}
+
+	return tsf;
+}
+
+int vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval)
+{
+	u64 next_tbtt = 0;
+	u8 data[8];
+
+	vnt_clear_current_tsf(priv);
+
+	next_tbtt = vnt_get_next_tbtt(next_tbtt, beacon_interval);
+
+	data[0] = (u8)next_tbtt;
+	data[1] = (u8)(next_tbtt >> 8);
+	data[2] = (u8)(next_tbtt >> 16);
+	data[3] = (u8)(next_tbtt >> 24);
+	data[4] = (u8)(next_tbtt >> 32);
+	data[5] = (u8)(next_tbtt >> 40);
+	data[6] = (u8)(next_tbtt >> 48);
+	data[7] = (u8)(next_tbtt >> 56);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			       MESSAGE_REQUEST_TBTT, 0, 8, data);
+}
+
+int vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
+			 u16 beacon_interval)
+{
+	u8 data[8];
+	int ret;
+
+	tsf = vnt_get_next_tbtt(tsf, beacon_interval);
+
+	data[0] = (u8)tsf;
+	data[1] = (u8)(tsf >> 8);
+	data[2] = (u8)(tsf >> 16);
+	data[3] = (u8)(tsf >> 24);
+	data[4] = (u8)(tsf >> 32);
+	data[5] = (u8)(tsf >> 40);
+	data[6] = (u8)(tsf >> 48);
+	data[7] = (u8)(tsf >> 56);
+
+	ret = vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			      MESSAGE_REQUEST_TBTT, 0, 8, data);
+	if (ret)
+		return ret;
+
+	dev_dbg(&priv->usb->dev, "%s TBTT: %8llx\n", __func__, tsf);
+	return 0;
+}
+
+/*
+ * Description: Turn off Radio power
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be turned off
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+int vnt_radio_power_off(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_VT3226:
+	case RF_VT3226D0:
+		ret = vnt_mac_reg_bits_off(priv, MAC_REG_SOFTPWRCTL,
+					   (SOFTPWRCTL_SWPE2 |
+					    SOFTPWRCTL_SWPE3));
+		break;
+	}
+
+	if (ret)
+		goto end;
+
+	ret = vnt_mac_reg_bits_off(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
+	if (ret)
+		goto end;
+
+	ret = vnt_set_deep_sleep(priv);
+	if (ret)
+		goto end;
+
+	ret = vnt_mac_reg_bits_on(priv, MAC_REG_GPIOCTL1, GPIO3_INTMD);
+
+end:
+	return ret;
+}
+
+/*
+ * Description: Turn on Radio power
+ *
+ * Parameters:
+ *  In:
+ *      priv         - The adapter to be turned on
+ *  Out:
+ *      none
+ *
+ * Return Value: true if success; otherwise false
+ *
+ */
+int vnt_radio_power_on(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	ret = vnt_exit_deep_sleep(priv);
+	if (ret)
+		return ret;
+
+	ret = vnt_mac_reg_bits_on(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
+	if (ret)
+		return ret;
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_VT3226:
+	case RF_VT3226D0:
+		ret = vnt_mac_reg_bits_on(priv, MAC_REG_SOFTPWRCTL,
+					  (SOFTPWRCTL_SWPE2 |
+					   SOFTPWRCTL_SWPE3));
+		if (ret)
+			return ret;
+	}
+
+	return vnt_mac_reg_bits_off(priv, MAC_REG_GPIOCTL1, GPIO3_INTMD);
+}
+
+int vnt_set_bss_mode(struct vnt_private *priv)
+{
+	int ret;
+	unsigned char type = priv->bb_type;
+	unsigned char data = 0;
+	unsigned char bb_vga_2_3 = 0x00;
+
+	ret = vnt_mac_set_bb_type(priv, type);
+	if (ret)
+		return ret;
+
+	priv->packet_type = vnt_get_pkt_type(priv);
+
+	if (priv->bb_type == BB_TYPE_11A) {
+		data = 0x03;
+		bb_vga_2_3 = 0x10;
+	} else if (priv->bb_type == BB_TYPE_11B) {
+		data = 0x02;
+	} else if (priv->bb_type == BB_TYPE_11G) {
+		data = 0x08;
+	}
+
+	if (data) {
+		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+					 0x88, data);
+		if (ret)
+			return ret;
+	}
+
+	ret = vnt_update_ifs(priv);
+	if (ret)
+		return ret;
+
+	ret = vnt_set_rspinf(priv, priv->bb_type);
+	if (ret)
+		return ret;
+
+	priv->bb_vga[2] = bb_vga_2_3;
+	priv->bb_vga[3] = bb_vga_2_3;
+
+	return vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
+}
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
new file mode 100644
index 000000000..be32c25c9
--- /dev/null
+++ b/drivers/staging/vt6656/card.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Provide functions to setup NIC operation mode
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ */
+
+#ifndef __CARD_H__
+#define __CARD_H__
+#include "device.h"
+
+/* init card type */
+
+#define CB_MAX_CHANNEL_24G	14
+#define CB_MAX_CHANNEL_5G	42 /* add channel9(5045MHz), 41==>42 */
+#define CB_MAX_CHANNEL		(CB_MAX_CHANNEL_24G + CB_MAX_CHANNEL_5G)
+
+struct vnt_private;
+
+int vnt_set_channel(struct vnt_private *priv, u32 connection_channel);
+int vnt_set_rspinf(struct vnt_private *priv, u8 bb_type);
+int vnt_update_ifs(struct vnt_private *priv);
+void vnt_update_top_rates(struct vnt_private *priv);
+bool vnt_ofdm_min_rate(struct vnt_private *priv);
+int vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
+		   u64 time_stamp, u64 local_tsf);
+bool vnt_get_current_tsf(struct vnt_private *priv, u64 *current_tsf);
+bool vnt_clear_current_tsf(struct vnt_private *priv);
+int vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval);
+int vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
+			 u16 beacon_interval);
+u64 vnt_get_next_tbtt(u64 tsf, u16 beacon_interval);
+u64 vnt_get_tsf_offset(u8 rx_rate, u64 tsf1, u64 tsf2);
+int vnt_radio_power_off(struct vnt_private *priv);
+int vnt_radio_power_on(struct vnt_private *priv);
+u8 vnt_get_pkt_type(struct vnt_private *priv);
+int vnt_set_bss_mode(struct vnt_private *priv);
+
+#endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6656/channel.c b/drivers/staging/vt6656/channel.c
new file mode 100644
index 000000000..413e2fc4a
--- /dev/null
+++ b/drivers/staging/vt6656/channel.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Channel number mapping
+ *
+ * Author: Lucas Lin
+ *
+ * Date: Dec 24, 2004
+ *
+ *
+ *
+ * Revision History:
+ *	01-18-2005	RobertYu:	remove the for loop searching in
+ *					ChannelValid, change ChannelRuleTab
+ *					to lookup-type, reorder table items.
+ *
+ *
+ */
+
+#include "device.h"
+#include "channel.h"
+#include "rf.h"
+
+static struct ieee80211_rate vnt_rates_bg[] = {
+	{ .bitrate = 10,  .hw_value = RATE_1M },
+	{ .bitrate = 20,  .hw_value = RATE_2M },
+	{ .bitrate = 55,  .hw_value = RATE_5M },
+	{ .bitrate = 110, .hw_value = RATE_11M },
+	{ .bitrate = 60,  .hw_value = RATE_6M },
+	{ .bitrate = 90,  .hw_value = RATE_9M },
+	{ .bitrate = 120, .hw_value = RATE_12M },
+	{ .bitrate = 180, .hw_value = RATE_18M },
+	{ .bitrate = 240, .hw_value = RATE_24M },
+	{ .bitrate = 360, .hw_value = RATE_36M },
+	{ .bitrate = 480, .hw_value = RATE_48M },
+	{ .bitrate = 540, .hw_value = RATE_54M },
+};
+
+static struct ieee80211_channel vnt_channels_2ghz[] = {
+	{ .center_freq = 2412, .hw_value = 1 },
+	{ .center_freq = 2417, .hw_value = 2 },
+	{ .center_freq = 2422, .hw_value = 3 },
+	{ .center_freq = 2427, .hw_value = 4 },
+	{ .center_freq = 2432, .hw_value = 5 },
+	{ .center_freq = 2437, .hw_value = 6 },
+	{ .center_freq = 2442, .hw_value = 7 },
+	{ .center_freq = 2447, .hw_value = 8 },
+	{ .center_freq = 2452, .hw_value = 9 },
+	{ .center_freq = 2457, .hw_value = 10 },
+	{ .center_freq = 2462, .hw_value = 11 },
+	{ .center_freq = 2467, .hw_value = 12 },
+	{ .center_freq = 2472, .hw_value = 13 },
+	{ .center_freq = 2484, .hw_value = 14 }
+};
+
+static struct ieee80211_supported_band vnt_supported_2ghz_band = {
+	.channels = vnt_channels_2ghz,
+	.n_channels = ARRAY_SIZE(vnt_channels_2ghz),
+	.bitrates = vnt_rates_bg,
+	.n_bitrates = ARRAY_SIZE(vnt_rates_bg),
+};
+
+void vnt_init_bands(struct vnt_private *priv)
+{
+	struct ieee80211_channel *ch;
+	int i;
+
+	ch = vnt_channels_2ghz;
+	for (i = 0; i < ARRAY_SIZE(vnt_channels_2ghz); i++) {
+		ch[i].max_power = VNT_RF_MAX_POWER;
+		ch[i].flags = IEEE80211_CHAN_NO_HT40;
+	}
+	priv->hw->wiphy->bands[NL80211_BAND_2GHZ] =
+					&vnt_supported_2ghz_band;
+}
diff --git a/drivers/staging/vt6656/channel.h b/drivers/staging/vt6656/channel.h
new file mode 100644
index 000000000..723660e40
--- /dev/null
+++ b/drivers/staging/vt6656/channel.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Country Regulation Rules header file
+ *
+ * Author: Lucas Lin
+ *
+ * Date: Dec 23, 2004
+ *
+ */
+
+#ifndef _CHANNEL_H_
+#define _CHANNEL_H_
+
+#include "device.h"
+
+void vnt_init_bands(struct vnt_private *priv);
+
+#endif  /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
new file mode 100644
index 000000000..c13561e52
--- /dev/null
+++ b/drivers/staging/vt6656/desc.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:The header file of descriptor
+ *
+ * Revision History:
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ */
+
+#ifndef __DESC_H__
+#define __DESC_H__
+
+#include <linux/bits.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+
+/* max transmit or receive buffer size */
+#define CB_MAX_BUF_SIZE     2900U       /* NOTE: must be multiple of 4 */
+
+#define MAX_TOTAL_SIZE_WITH_ALL_HEADERS CB_MAX_BUF_SIZE
+
+#define MAX_INTERRUPT_SIZE              32
+
+#define CB_MAX_RX_DESC      128         /* max # of descriptors */
+#define CB_MIN_RX_DESC      16          /* min # of RX descriptors */
+#define CB_MAX_TX_DESC      128         /* max # of descriptors */
+#define CB_MIN_TX_DESC      16          /* min # of TX descriptors */
+
+/*
+ * bits in the RSR register
+ */
+#define RSR_ADDRBROAD       BIT(7)
+#define RSR_ADDRMULTI       BIT(6)
+#define RSR_ADDRUNI         0x00
+#define RSR_IVLDTYP         BIT(5)	/* invalid packet type */
+#define RSR_IVLDLEN         BIT(4)	/* invalid len (> 2312 byte) */
+#define RSR_BSSIDOK         BIT(3)
+#define RSR_CRCOK           BIT(2)
+#define RSR_BCNSSIDOK       BIT(1)
+#define RSR_ADDROK          BIT(0)
+
+/*
+ * bits in the new RSR register
+ */
+#define NEWRSR_DECRYPTOK    BIT(4)
+#define NEWRSR_CFPIND       BIT(3)
+#define NEWRSR_HWUTSF       BIT(2)
+#define NEWRSR_BCNHITAID    BIT(1)
+#define NEWRSR_BCNHITAID0   BIT(0)
+
+/*
+ * bits in the TSR register
+ */
+#define TSR_RETRYTMO        BIT(3)
+#define TSR_TMO             BIT(2)
+#define TSR_ACKDATA         BIT(1)
+#define TSR_VALID           BIT(0)
+
+#define FIFOCTL_AUTO_FB_1   0x1000
+#define FIFOCTL_AUTO_FB_0   0x0800
+#define FIFOCTL_GRPACK      0x0400
+#define FIFOCTL_11GA        0x0300
+#define FIFOCTL_11GB        0x0200
+#define FIFOCTL_11B         0x0100
+#define FIFOCTL_11A         0x0000
+#define FIFOCTL_RTS         0x0080
+#define FIFOCTL_ISDMA0      0x0040
+#define FIFOCTL_GENINT      0x0020
+#define FIFOCTL_TMOEN       0x0010
+#define FIFOCTL_LRETRY      0x0008
+#define FIFOCTL_CRCDIS      0x0004
+#define FIFOCTL_NEEDACK     0x0002
+#define FIFOCTL_LHEAD       0x0001
+
+/* WMAC definition Frag Control */
+#define FRAGCTL_AES         0x0300
+#define FRAGCTL_TKIP        0x0200
+#define FRAGCTL_LEGACY      0x0100
+#define FRAGCTL_NONENCRYPT  0x0000
+#define FRAGCTL_ENDFRAG     0x0003
+#define FRAGCTL_MIDFRAG     0x0002
+#define FRAGCTL_STAFRAG     0x0001
+#define FRAGCTL_NONFRAG     0x0000
+
+#endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
new file mode 100644
index 000000000..ca974d61d
--- /dev/null
+++ b/drivers/staging/vt6656/device.h
@@ -0,0 +1,386 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: MAC Data structure
+ *
+ * Author: Tevin Chen
+ *
+ * Date: Mar 17, 1997
+ *
+ */
+
+#ifndef __DEVICE_H__
+#define __DEVICE_H__
+
+#include <linux/bits.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/suspend.h>
+#include <linux/if_arp.h>
+#include <linux/wireless.h>
+#include <linux/timer.h>
+#include <linux/usb.h>
+#include <linux/crc32.h>
+#include <net/mac80211.h>
+
+#ifdef SIOCETHTOOL
+#define DEVICE_ETHTOOL_IOCTL_SUPPORT
+#include <linux/ethtool.h>
+#else
+#undef DEVICE_ETHTOOL_IOCTL_SUPPORT
+#endif
+
+#define RATE_1M		0
+#define RATE_2M		1
+#define RATE_5M		2
+#define RATE_11M	3
+#define RATE_6M		4
+#define RATE_9M		5
+#define RATE_12M	6
+#define RATE_18M	7
+#define RATE_24M	8
+#define RATE_36M	9
+#define RATE_48M	10
+#define RATE_54M	11
+#define RATE_AUTO	12
+
+#define MAX_RATE			12
+#define VNT_B_RATES	(BIT(RATE_1M) | BIT(RATE_2M) |\
+			BIT(RATE_5M) | BIT(RATE_11M))
+
+/*
+ * device specific
+ */
+
+#include "wcmd.h"
+#include "desc.h"
+#include "key.h"
+#include "card.h"
+
+#define VNT_USB_VENDOR_ID                     0x160a
+#define VNT_USB_PRODUCT_ID                    0x3184
+
+#define DEVICE_NAME			"vt6656"
+#define DEVICE_FULL_DRV_NAM		"VIA Networking Wireless LAN USB Driver"
+
+#define DEVICE_VERSION			"mac80211"
+
+#define FIRMWARE_VERSION		0x133		/* version 1.51 */
+#define FIRMWARE_NAME			"vntwusb.fw"
+#define FIRMWARE_CHUNK_SIZE		0x400
+
+#define MAX_UINTS			8
+#define OPTION_DEFAULT			{ [0 ... MAX_UINTS - 1] = -1}
+
+#define DUPLICATE_RX_CACHE_LENGTH       5
+
+#define AUTO_FB_NONE            0
+#define AUTO_FB_0               1
+#define AUTO_FB_1               2
+
+#define FB_RATE0                0
+#define FB_RATE1                1
+
+/* Antenna Mode */
+#define ANT_A                   0
+#define ANT_B                   1
+#define ANT_DIVERSITY           2
+#define ANT_RXD_TXA             3
+#define ANT_RXD_TXB             4
+#define ANT_UNKNOWN             0xFF
+#define ANT_TXA                 0
+#define ANT_TXB                 1
+#define ANT_RXA                 2
+#define ANT_RXB                 3
+
+#define BB_VGA_LEVEL            4
+#define BB_VGA_CHANGE_THRESHOLD 3
+
+#define EEP_MAX_CONTEXT_SIZE    256
+
+/* Contents in the EEPROM */
+#define EEP_OFS_PAR		0x0
+#define EEP_OFS_ANTENNA		0x17
+#define EEP_OFS_RADIOCTL	0x18
+#define EEP_OFS_RFTYPE		0x1b
+#define EEP_OFS_MINCHANNEL	0x1c
+#define EEP_OFS_MAXCHANNEL	0x1d
+#define EEP_OFS_SIGNATURE	0x1e
+#define EEP_OFS_ZONETYPE	0x1f
+#define EEP_OFS_RFTABLE		0x20
+#define EEP_OFS_PWR_CCK		0x20
+#define EEP_OFS_SETPT_CCK	0x21
+#define EEP_OFS_PWR_OFDMG	0x23
+
+#define EEP_OFS_CALIB_TX_IQ	0x24
+#define EEP_OFS_CALIB_TX_DC	0x25
+#define EEP_OFS_CALIB_RX_IQ	0x26
+
+#define EEP_OFS_MAJOR_VER	0x2e
+#define EEP_OFS_MINOR_VER	0x2f
+
+#define EEP_OFS_CCK_PWR_TBL	0x30
+#define EEP_OFS_OFDM_PWR_TBL	0x40
+#define EEP_OFS_OFDMA_PWR_TBL	0x50
+
+/* Bits in EEP_OFS_ANTENNA */
+#define EEP_ANTENNA_MAIN	BIT(0)
+#define EEP_ANTENNA_AUX		BIT(1)
+#define EEP_ANTINV		BIT(2)
+
+/* Bits in EEP_OFS_RADIOCTL */
+#define EEP_RADIOCTL_ENABLE	BIT(7)
+
+/* control commands */
+#define MESSAGE_TYPE_READ		0x1
+#define MESSAGE_TYPE_WRITE		0x0
+#define MESSAGE_TYPE_LOCK_OR		0x2
+#define MESSAGE_TYPE_LOCK_AND		0x3
+#define MESSAGE_TYPE_WRITE_MASK		0x4
+#define MESSAGE_TYPE_CARDINIT		0x5
+#define MESSAGE_TYPE_INIT_RSP		0x6
+#define MESSAGE_TYPE_MACSHUTDOWN	0x7
+#define MESSAGE_TYPE_SETKEY		0x8
+#define MESSAGE_TYPE_CLRKEYENTRY	0x9
+#define MESSAGE_TYPE_WRITE_MISCFF	0xa
+#define MESSAGE_TYPE_SET_ANTMD		0xb
+#define MESSAGE_TYPE_SELECT_CHANNEL	0xc
+#define MESSAGE_TYPE_SET_TSFTBTT	0xd
+#define MESSAGE_TYPE_SET_SSTIFS		0xe
+#define MESSAGE_TYPE_CHANGE_BBTYPE	0xf
+#define MESSAGE_TYPE_DISABLE_PS		0x10
+#define MESSAGE_TYPE_WRITE_IFRF		0x11
+
+/* command read/write(index) */
+#define MESSAGE_REQUEST_MEM		0x1
+#define MESSAGE_REQUEST_BBREG		0x2
+#define MESSAGE_REQUEST_MACREG		0x3
+#define MESSAGE_REQUEST_EEPROM		0x4
+#define MESSAGE_REQUEST_TSF		0x5
+#define MESSAGE_REQUEST_TBTT		0x6
+#define MESSAGE_REQUEST_BBAGC		0x7
+#define MESSAGE_REQUEST_VERSION		0x8
+#define MESSAGE_REQUEST_RF_INIT		0x9
+#define MESSAGE_REQUEST_RF_INIT2	0xa
+#define MESSAGE_REQUEST_RF_CH0		0xb
+#define MESSAGE_REQUEST_RF_CH1		0xc
+#define MESSAGE_REQUEST_RF_CH2		0xd
+
+/* USB registers */
+#define USB_REG4			0x604
+
+#define DEVICE_INIT_COLD	0x0 /* cold init */
+#define DEVICE_INIT_RESET	0x1 /* reset init or Dx to D0 power remain */
+#define DEVICE_INIT_DXPL	0x2 /* Dx to D0 power lost init */
+
+/* Device init */
+struct vnt_cmd_card_init {
+	u8 init_class;
+	u8 exist_sw_net_addr;
+	u8 sw_net_addr[6];
+	u8 short_retry_limit;
+	u8 long_retry_limit;
+};
+
+struct vnt_rsp_card_init {
+	u8 status;
+	u8 net_addr[6];
+	u8 rf_type;
+	u8 min_channel;
+	u8 max_channel;
+};
+
+/* USB */
+
+/*
+ * Enum of context types for SendPacket
+ */
+enum {
+	CONTEXT_DATA_PACKET = 0,
+	CONTEXT_BEACON_PACKET
+};
+
+struct vnt_rx_header {
+	u32 wbk_status;
+	u8 rx_sts;
+	u8 rx_rate;
+	u16 pay_load_len;
+} __packed;
+
+struct vnt_rx_tail {
+	__le64 tsf_time;
+	u8 sq;
+	u8 new_rsr;
+	u8 rssi;
+	u8 rsr;
+	u8 sq_3;
+} __packed;
+
+/* RCB (Receive Control Block) */
+struct vnt_rcb {
+	void *priv;
+	struct urb *urb;
+	struct sk_buff *skb;
+};
+
+/* used to track bulk out irps */
+struct vnt_usb_send_context {
+	void *priv;
+	struct sk_buff *skb;
+	void *tx_buffer;
+	u32 frame_len;
+	u16 tx_hdr_size;
+	u16 tx_rate;
+	u8 type;
+	u8 pkt_no;
+	u8 pkt_type;
+	bool in_use;
+};
+
+/*
+ * Structure to keep track of USB interrupt packets
+ */
+struct vnt_interrupt_buffer {
+	u8 *data_buf;
+};
+
+/* flags for options */
+#define DEVICE_FLAGS_UNPLUG		0
+#define DEVICE_FLAGS_DISCONNECTED	1
+
+struct vnt_private {
+	/* mac80211 */
+	struct ieee80211_hw *hw;
+	struct ieee80211_vif *vif;
+	u8 mac_hw;
+	/* netdev */
+	struct usb_device *usb;
+	struct usb_interface *intf;
+
+	u64 tsf_time;
+
+	u32 rx_buf_sz;
+	int mc_list_count;
+
+	spinlock_t lock;		/* prepare tx USB URB */
+	struct mutex usb_lock;		/* USB control messages */
+
+	unsigned long flags;
+
+	/* USB */
+	struct urb *interrupt_urb;
+	u32 int_interval;
+
+	/* Variables to track resources for the BULK In Pipe */
+	struct vnt_rcb *rcb[CB_MAX_RX_DESC];
+	u32 num_rcb;
+
+	/* Variables to track resources for the BULK Out Pipe */
+	struct vnt_usb_send_context *tx_context[CB_MAX_TX_DESC];
+	struct usb_anchor tx_submitted;
+	u32 num_tx_context;
+
+	/* Variables to track resources for the Interrupt In Pipe */
+	struct vnt_interrupt_buffer int_buf;
+
+	/* Version control */
+	u16 firmware_version;
+	u8 local_id;
+	u8 rf_type;
+	u8 bb_rx_conf;
+
+	struct vnt_cmd_card_init init_command;
+	struct vnt_rsp_card_init init_response;
+	u8 current_net_addr[ETH_ALEN] __aligned(2);
+	u8 permanent_net_addr[ETH_ALEN] __aligned(2);
+
+	u8 exist_sw_net_addr;
+
+	u64 current_tsf;
+
+	/* 802.11 MAC specific */
+	u32 current_rssi;
+
+	/* Antenna Diversity */
+	int tx_rx_ant_inv;
+	u32 rx_antenna_sel;
+	u8 rx_antenna_mode;
+	u8 tx_antenna_mode;
+	u8 radio_ctl;
+
+	/* IFS & Cw */
+	u32 sifs;  /* Current SIFS */
+	u32 difs;  /* Current DIFS */
+	u32 eifs;  /* Current EIFS */
+	u32 slot;  /* Current SlotTime */
+
+	/* Rate */
+	u8 bb_type; /* 0: 11A, 1:11B, 2:11G */
+	u8 packet_type; /* 0:11a 1:11b 2:11gb 3:11ga */
+	u32 basic_rates;
+	u8 top_ofdm_basic_rate;
+	u8 top_cck_basic_rate;
+
+	u8 eeprom[EEP_MAX_CONTEXT_SIZE];  /*u32 alignment */
+
+	u8 preamble_type;
+
+	/* For RF Power table */
+	u8 cck_pwr;
+	u8 ofdm_pwr_g;
+	u8 ofdm_pwr_a;
+	u8 power;
+	u8 cck_pwr_tbl[14];
+	u8 ofdm_pwr_tbl[14];
+	u8 ofdm_a_pwr_tbl[42];
+
+	u16 tx_rate_fb0;
+	u16 tx_rate_fb1;
+
+	enum nl80211_iftype op_mode;
+
+	int short_slot_time;
+
+	/* Power save */
+	u16 current_aid;
+
+	/* Beacon related */
+	u16 seq_counter;
+
+	enum vnt_cmd_state command_state;
+
+	enum vnt_cmd command;
+
+	/* 802.11 counter */
+
+	enum vnt_cmd cmd_queue[CMD_Q_SIZE];
+	u32 cmd_dequeue_idx;
+	u32 cmd_enqueue_idx;
+	u32 free_cmd_queue;
+	int cmd_running;
+
+	unsigned long key_entry_inuse;
+
+	u8 auto_fb_ctrl;
+
+	/* For Update BaseBand VGA Gain Offset */
+	u8 bb_vga[BB_VGA_LEVEL];
+
+	u8 bb_pre_ed_rssi;
+	u8 bb_pre_ed_index;
+
+	/* command timer */
+	struct delayed_work run_command_work;
+
+	struct ieee80211_low_level_stats low_stats;
+};
+
+int vnt_init(struct vnt_private *priv);
+
+#endif
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
new file mode 100644
index 000000000..bdc5f30c4
--- /dev/null
+++ b/drivers/staging/vt6656/key.c
@@ -0,0 +1,142 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions for 802.11i Key management
+ *
+ * Author: Jerry Chen
+ *
+ * Date: May 29, 2003
+ *
+ * Functions:
+ *
+ * Revision History:
+ *
+ */
+
+#include "mac.h"
+#include "key.h"
+#include "usbpipe.h"
+
+int vnt_key_init_table(struct vnt_private *priv)
+{
+	u8 i;
+	u8 data[MAX_KEY_TABLE];
+
+	for (i = 0; i < MAX_KEY_TABLE; i++)
+		data[i] = i;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY,
+			0, 0, ARRAY_SIZE(data), data);
+}
+
+static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
+			   struct ieee80211_key_conf *key, u32 key_type,
+			   u32 mode)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u16 key_mode = 0;
+	u32 entry = 0;
+	u8 *bssid;
+	u8 key_inx = key->keyidx;
+	u8 i;
+
+	if (mac_addr)
+		bssid = mac_addr;
+	else
+		bssid = &broadcast[0];
+
+	if (key_type != VNT_KEY_DEFAULTKEY) {
+		for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
+			if (!test_bit(i, &priv->key_entry_inuse)) {
+				set_bit(i, &priv->key_entry_inuse);
+
+				key->hw_key_idx = i;
+				entry = key->hw_key_idx;
+				break;
+			}
+		}
+	}
+
+	switch (key_type) {
+	case VNT_KEY_DEFAULTKEY:
+		/* default key last entry */
+		entry = MAX_KEY_TABLE - 1;
+		key->hw_key_idx = entry;
+		fallthrough;
+	case VNT_KEY_GROUP_ADDRESS:
+		key_mode = mode | (mode << 4);
+		break;
+	case VNT_KEY_GROUP:
+		key_mode = mode << 4;
+		break;
+	case  VNT_KEY_PAIRWISE:
+		key_mode |= mode;
+		key_inx = 4;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	key_mode |= key_type;
+
+	if (mode == KEY_CTL_WEP) {
+		if (key->keylen == WLAN_KEY_LEN_WEP40)
+			key->key[15] &= 0x7f;
+		if (key->keylen == WLAN_KEY_LEN_WEP104)
+			key->key[15] |= 0x80;
+	}
+
+	return vnt_mac_set_keyentry(priv, key_mode, entry,
+				    key_inx, bssid, key->key);
+}
+
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 *mac_addr = NULL;
+	u8 key_dec_mode = 0;
+
+	if (sta)
+		mac_addr = &sta->addr[0];
+
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		vnt_set_keymode(hw, mac_addr, key, VNT_KEY_DEFAULTKEY,
+				KEY_CTL_WEP);
+
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+
+		return vnt_set_keymode(hw, mac_addr, key, VNT_KEY_DEFAULTKEY,
+				       KEY_CTL_WEP);
+
+	case WLAN_CIPHER_SUITE_TKIP:
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+
+		key_dec_mode = KEY_CTL_TKIP;
+
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		if (priv->local_id <= MAC_REVISION_A1)
+			return -EOPNOTSUPP;
+
+		key_dec_mode = KEY_CTL_CCMP;
+
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+		return vnt_set_keymode(hw, mac_addr, key, VNT_KEY_PAIRWISE,
+				       key_dec_mode);
+
+	return vnt_set_keymode(hw, mac_addr, key,
+				VNT_KEY_GROUP_ADDRESS, key_dec_mode);
+}
diff --git a/drivers/staging/vt6656/key.h b/drivers/staging/vt6656/key.h
new file mode 100644
index 000000000..6f1d5b4f6
--- /dev/null
+++ b/drivers/staging/vt6656/key.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Implement functions for 802.11i Key management
+ *
+ * Author: Jerry Chen
+ *
+ * Date: May 29, 2003
+ *
+ */
+
+#ifndef __KEY_H__
+#define __KEY_H__
+
+#include "device.h"
+
+#define MAX_KEY_TABLE       11
+
+#define KEY_CTL_WEP         0x00
+#define KEY_CTL_NONE        0x01
+#define KEY_CTL_TKIP        0x02
+#define KEY_CTL_CCMP        0x03
+
+#define VNT_KEY_ONFLY_ALL	0x4000
+#define VNT_KEY_ONFLY		0x8000
+#define VNT_KEY_ALLGROUP	0x04
+#define VNT_KEY_GROUP		0x40
+#define VNT_KEY_PAIRWISE	VNT_KEY_ONFLY
+#define VNT_KEY_GROUP_ADDRESS	(VNT_KEY_ALLGROUP | VNT_KEY_GROUP)
+#define VNT_KEY_DEFAULTKEY	(VNT_KEY_GROUP_ADDRESS | VNT_KEY_ONFLY |\
+				VNT_KEY_ONFLY_ALL)
+
+int vnt_key_init_table(struct vnt_private *priv);
+
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key);
+
+#endif /* __KEY_H__ */
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
new file mode 100644
index 000000000..49430c0a9
--- /dev/null
+++ b/drivers/staging/vt6656/mac.c
@@ -0,0 +1,183 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:  MAC routines
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ * Functions:
+ *
+ * Revision History:
+ */
+
+#include <linux/etherdevice.h>
+
+#include "desc.h"
+#include "mac.h"
+#include "usbpipe.h"
+
+int vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter)
+{
+	__le64 le_mc = cpu_to_le64(mc_filter);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
+			       MESSAGE_REQUEST_MACREG, sizeof(le_mc),
+			       (u8 *)&le_mc);
+}
+
+int vnt_mac_shutdown(struct vnt_private *priv)
+{
+	return vnt_control_out(priv, MESSAGE_TYPE_MACSHUTDOWN, 0, 0, 0, NULL);
+}
+
+int vnt_mac_set_bb_type(struct vnt_private *priv, u8 type)
+{
+	u8 data[2];
+
+	data[0] = type;
+	data[1] = EN_CFG_BB_TYPE_MASK;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG,	ARRAY_SIZE(data),
+			       data);
+}
+
+int vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx)
+{
+	return vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY, 0, 0,
+			       sizeof(entry_idx), &entry_idx);
+}
+
+int vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+			 u32 key_idx, u8 *addr, u8 *key)
+{
+	struct vnt_mac_set_key set_key;
+	u16 offset;
+
+	offset = MISCFIFO_KEYETRY0;
+	offset += entry_idx * MISCFIFO_KEYENTRYSIZE;
+
+	set_key.u.write.key_ctl = cpu_to_le16(key_ctl);
+	ether_addr_copy(set_key.u.write.addr, addr);
+
+	/* swap over swap[0] and swap[1] to get correct write order */
+	swap(set_key.u.swap[0], set_key.u.swap[1]);
+
+	memcpy(set_key.key, key, WLAN_KEY_LEN_CCMP);
+
+	dev_dbg(&priv->usb->dev, "offset %d key ctl %d set key %24ph\n",
+		offset, key_ctl, (u8 *)&set_key);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_SETKEY, offset,
+			       (u16)key_idx, sizeof(struct vnt_mac_set_key),
+			       (u8 *)&set_key);
+}
+
+int vnt_mac_reg_bits_off(struct vnt_private *priv, u8 reg_ofs, u8 bits)
+{
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = bits;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, reg_ofs,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_reg_bits_on(struct vnt_private *priv, u8 reg_ofs, u8 bits)
+{
+	u8 data[2];
+
+	data[0] = bits;
+	data[1] = bits;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, reg_ofs,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word)
+{
+	u8 data[2];
+
+	data[0] = (u8)(word & 0xff);
+	data[1] = (u8)(word >> 8);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, reg_ofs,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr)
+{
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BSSID0,
+			       MESSAGE_REQUEST_MACREG, ETH_ALEN, addr);
+}
+
+int vnt_mac_enable_protect_mode(struct vnt_private *priv)
+{
+	u8 data[2];
+
+	data[0] = EN_CFG_PROTECT_MD;
+	data[1] = EN_CFG_PROTECT_MD;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_disable_protect_mode(struct vnt_private *priv)
+{
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EN_CFG_PROTECT_MD;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv)
+{
+	u8 data[2];
+
+	data[0] = EN_CFG_BARKER_PREAM;
+	data[1] = EN_CFG_BARKER_PREAM;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv)
+{
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EN_CFG_BARKER_PREAM;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval)
+{
+	u8 data[2];
+
+	data[0] = (u8)(interval & 0xff);
+	data[1] = (u8)(interval >> 8);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BI,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
+
+int vnt_mac_set_led(struct vnt_private *priv, u8 state, u8 led)
+{
+	u8 data[2];
+
+	data[0] = led;
+	data[1] = state;
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_PAPEDELAY,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
new file mode 100644
index 000000000..0ac845bd3
--- /dev/null
+++ b/drivers/staging/vt6656/mac.h
@@ -0,0 +1,373 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: MAC routines
+ *
+ * Author: Tevin Chen
+ *
+ * Date: May 21, 1996
+ *
+ * Revision History:
+ *      07-01-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
+ *      08-25-2003 Kyle Hsu:      Porting MAC functions from sim53.
+ *      09-03-2003 Bryan YC Fan:  Add MACvDisableProtectMD & MACvEnableProtectMD
+ */
+
+#ifndef __MAC_H__
+#define __MAC_H__
+
+#include <linux/bits.h>
+#include "device.h"
+
+#define REV_ID_VT3253_A0	0x00
+#define REV_ID_VT3253_A1	0x01
+#define REV_ID_VT3253_B0	0x08
+#define REV_ID_VT3253_B1	0x09
+
+/* Registers in the MAC */
+#define MAC_REG_BISTCMD		0x04
+#define MAC_REG_BISTSR0		0x05
+#define MAC_REG_BISTSR1		0x06
+#define MAC_REG_BISTSR2		0x07
+#define MAC_REG_I2MCSR		0x08
+#define MAC_REG_I2MTGID		0x09
+#define MAC_REG_I2MTGAD		0x0a
+#define MAC_REG_I2MCFG		0x0b
+#define MAC_REG_I2MDIPT		0x0c
+#define MAC_REG_I2MDOPT		0x0e
+#define MAC_REG_USBSUS		0x0f
+
+#define MAC_REG_LOCALID		0x14
+#define MAC_REG_TESTCFG		0x15
+#define MAC_REG_JUMPER0		0x16
+#define MAC_REG_JUMPER1		0x17
+#define MAC_REG_TMCTL		0x18
+#define MAC_REG_TMDATA0		0x1c
+#define MAC_REG_TMDATA1		0x1d
+#define MAC_REG_TMDATA2		0x1e
+#define MAC_REG_TMDATA3		0x1f
+
+/* MAC Parameter related */
+#define MAC_REG_LRT		0x20
+#define MAC_REG_SRT		0x21
+#define MAC_REG_SIFS		0x22
+#define MAC_REG_DIFS		0x23
+#define MAC_REG_EIFS		0x24
+#define MAC_REG_SLOT		0x25
+#define MAC_REG_BI		0x26
+#define MAC_REG_CWMAXMIN0	0x28
+#define MAC_REG_LINKOFFTOTM	0x2a
+#define MAC_REG_SWTMOT		0x2b
+#define MAC_REG_RTSOKCNT	0x2c
+#define MAC_REG_RTSFAILCNT	0x2d
+#define MAC_REG_ACKFAILCNT	0x2e
+#define MAC_REG_FCSERRCNT	0x2f
+
+/* TSF Related */
+#define MAC_REG_TSFCNTR		0x30
+#define MAC_REG_NEXTTBTT	0x38
+#define MAC_REG_TSFOFST		0x40
+#define MAC_REG_TFTCTL		0x48
+
+/* WMAC Control/Status Related */
+#define MAC_REG_ENCFG0		0x4c
+#define MAC_REG_ENCFG1		0x4d
+#define MAC_REG_ENCFG2		0x4e
+
+#define MAC_REG_CFG		0x50
+#define MAC_REG_TEST		0x52
+#define MAC_REG_HOSTCR		0x54
+#define MAC_REG_MACCR		0x55
+#define MAC_REG_RCR		0x56
+#define MAC_REG_TCR		0x57
+#define MAC_REG_IMR		0x58
+#define MAC_REG_ISR		0x5c
+#define MAC_REG_ISR1		0x5d
+
+/* Power Saving Related */
+#define MAC_REG_PSCFG		0x60
+#define MAC_REG_PSCTL		0x61
+#define MAC_REG_PSPWRSIG	0x62
+#define MAC_REG_BBCR13		0x63
+#define MAC_REG_AIDATIM		0x64
+#define MAC_REG_PWBT		0x66
+#define MAC_REG_WAKEOKTMR	0x68
+#define MAC_REG_CALTMR		0x69
+#define MAC_REG_SYNSPACCNT	0x6a
+#define MAC_REG_WAKSYNOPT	0x6b
+
+/* Baseband/IF Control Group */
+#define MAC_REG_BBREGCTL	0x6c
+#define MAC_REG_CHANNEL		0x6d
+#define MAC_REG_BBREGADR	0x6e
+#define MAC_REG_BBREGDATA	0x6f
+#define MAC_REG_IFREGCTL	0x70
+#define MAC_REG_IFDATA		0x71
+#define MAC_REG_ITRTMSET	0x74
+#define MAC_REG_PAPEDELAY	0x77
+#define MAC_REG_SOFTPWRCTL	0x78
+#define MAC_REG_SOFTPWRCTL2	0x79
+#define MAC_REG_GPIOCTL0	0x7a
+#define MAC_REG_GPIOCTL1	0x7b
+
+/* MiscFF PIO related */
+#define MAC_REG_MISCFFNDEX	0xbc
+#define MAC_REG_MISCFFCTL	0xbe
+#define MAC_REG_MISCFFDATA	0xc0
+
+/* MAC Configuration Group */
+#define MAC_REG_PAR0		0xc4
+#define MAC_REG_PAR4		0xc8
+#define MAC_REG_BSSID0		0xcc
+#define MAC_REG_BSSID4		0xd0
+#define MAC_REG_MAR0		0xd4
+#define MAC_REG_MAR4		0xd8
+
+/* MAC RSPPKT INFO Group */
+#define MAC_REG_RSPINF_B_1	0xdC
+#define MAC_REG_RSPINF_B_2	0xe0
+#define MAC_REG_RSPINF_B_5	0xe4
+#define MAC_REG_RSPINF_B_11	0xe8
+#define MAC_REG_RSPINF_A_6	0xec
+#define MAC_REG_RSPINF_A_9	0xee
+#define MAC_REG_RSPINF_A_12	0xf0
+#define MAC_REG_RSPINF_A_18	0xf2
+#define MAC_REG_RSPINF_A_24	0xf4
+#define MAC_REG_RSPINF_A_36	0xf6
+#define MAC_REG_RSPINF_A_48	0xf8
+#define MAC_REG_RSPINF_A_54	0xfa
+#define MAC_REG_RSPINF_A_72	0xfc
+
+/* Bits in the I2MCFG EEPROM register */
+#define I2MCFG_BOUNDCTL		BIT(7)
+#define I2MCFG_WAITCTL		BIT(5)
+#define I2MCFG_SCLOECTL		BIT(4)
+#define I2MCFG_WBUSYCTL		BIT(3)
+#define I2MCFG_NORETRY		BIT(2)
+#define I2MCFG_I2MLDSEQ		BIT(1)
+#define I2MCFG_I2CMFAST		BIT(0)
+
+/* Bits in the I2MCSR EEPROM register */
+#define I2MCSR_EEMW		BIT(7)
+#define I2MCSR_EEMR		BIT(6)
+#define I2MCSR_AUTOLD		BIT(3)
+#define I2MCSR_NACK		BIT(1)
+#define I2MCSR_DONE		BIT(0)
+
+/* Bits in the TMCTL register */
+#define TMCTL_TSUSP		BIT(2)
+#define TMCTL_TMD		BIT(1)
+#define TMCTL_TE		BIT(0)
+
+/* Bits in the TFTCTL register */
+#define TFTCTL_HWUTSF		BIT(7)
+#define TFTCTL_TBTTSYNC		BIT(6)
+#define TFTCTL_HWUTSFEN		BIT(5)
+#define TFTCTL_TSFCNTRRD	BIT(4)
+#define TFTCTL_TBTTSYNCEN	BIT(3)
+#define TFTCTL_TSFSYNCEN	BIT(2)
+#define TFTCTL_TSFCNTRST	BIT(1)
+#define TFTCTL_TSFCNTREN	BIT(0)
+
+/* Bits in the EnhanceCFG_0 register */
+#define EN_CFG_BB_TYPE_A	0x00
+#define EN_CFG_BB_TYPE_B	BIT(0)
+#define EN_CFG_BB_TYPE_G	BIT(1)
+#define EN_CFG_BB_TYPE_MASK	(EN_CFG_BB_TYPE_B | EN_CFG_BB_TYPE_G)
+#define EN_CFG_PROTECT_MD	BIT(5)
+
+/* Bits in the EnhanceCFG_1 register */
+#define EN_CFG_BCN_SUS_IND	BIT(0)
+#define EN_CFG_BCN_SUS_CLR	BIT(1)
+
+/* Bits in the EnhanceCFG_2 register */
+#define EN_CFG_NXTBTTCFPSTR	BIT(0)
+#define EN_CFG_BARKER_PREAM	BIT(1)
+#define EN_CFG_PKT_BURST_MD	BIT(2)
+
+/* Bits in the CFG register */
+#define CFG_TKIPOPT		BIT(7)
+#define CFG_RXDMAOPT		BIT(6)
+#define CFG_TMOT_SW		BIT(5)
+#define CFG_TMOT_HWLONG		BIT(4)
+#define CFG_TMOT_HW		0x00
+#define CFG_CFPENDOPT		BIT(3)
+#define CFG_BCNSUSEN		BIT(2)
+#define CFG_NOTXTIMEOUT		BIT(1)
+#define CFG_NOBUFOPT		BIT(0)
+
+/* Bits in the TEST register */
+#define TEST_LBEXT		BIT(7)
+#define TEST_LBINT		BIT(6)
+#define TEST_LBNONE		0x00
+#define TEST_SOFTINT		BIT(5)
+#define TEST_CONTTX		BIT(4)
+#define TEST_TXPE		BIT(3)
+#define TEST_NAVDIS		BIT(2)
+#define TEST_NOCTS		BIT(1)
+#define TEST_NOACK		BIT(0)
+
+/* Bits in the HOSTCR register */
+#define HOSTCR_TXONST		BIT(7)
+#define HOSTCR_RXONST		BIT(6)
+#define HOSTCR_ADHOC		BIT(5)
+#define HOSTCR_AP		BIT(4)
+#define HOSTCR_TXON		BIT(3)
+#define HOSTCR_RXON		BIT(2)
+#define HOSTCR_MACEN		BIT(1)
+#define HOSTCR_SOFTRST		BIT(0)
+
+/* Bits in the MACCR register */
+#define MACCR_SYNCFLUSHOK	BIT(2)
+#define MACCR_SYNCFLUSH		BIT(1)
+#define MACCR_CLRNAV		BIT(0)
+
+/* Bits in the RCR register */
+#define RCR_SSID		BIT(7)
+#define RCR_RXALLTYPE		BIT(6)
+#define RCR_UNICAST		BIT(5)
+#define RCR_BROADCAST		BIT(4)
+#define RCR_MULTICAST		BIT(3)
+#define RCR_WPAERR		BIT(2)
+#define RCR_ERRCRC		BIT(1)
+#define RCR_BSSID		BIT(0)
+
+/* Bits in the TCR register */
+#define TCR_SYNCDCFOPT		BIT(1)
+#define TCR_AUTOBCNTX		BIT(0)
+
+/* ISR1 */
+#define ISR_GPIO3		BIT(6)
+#define ISR_RXNOBUF		BIT(3)
+#define ISR_MIBNEARFULL		BIT(2)
+#define ISR_SOFTINT		BIT(1)
+#define ISR_FETALERR		BIT(0)
+
+#define LEDSTS_STS		0x06
+#define LEDSTS_TMLEN		0x78
+#define LEDSTS_OFF		0x00
+#define LEDSTS_ON		0x02
+#define LEDSTS_SLOW		0x04
+#define LEDSTS_INTER		0x06
+
+/* ISR0 */
+#define ISR_WATCHDOG		BIT(7)
+#define ISR_SOFTTIMER		BIT(6)
+#define ISR_GPIO0		BIT(5)
+#define ISR_TBTT		BIT(4)
+#define ISR_RXDMA0		BIT(3)
+#define ISR_BNTX		BIT(2)
+#define ISR_ACTX		BIT(0)
+
+/* Bits in the PSCFG register */
+#define PSCFG_PHILIPMD		BIT(6)
+#define PSCFG_WAKECALEN		BIT(5)
+#define PSCFG_WAKETMREN		BIT(4)
+#define PSCFG_BBPSPROG		BIT(3)
+#define PSCFG_WAKESYN		BIT(2)
+#define PSCFG_SLEEPSYN		BIT(1)
+#define PSCFG_AUTOSLEEP		BIT(0)
+
+/* Bits in the PSCTL register */
+#define PSCTL_WAKEDONE		BIT(5)
+#define PSCTL_PS		BIT(4)
+#define PSCTL_GO2DOZE		BIT(3)
+#define PSCTL_LNBCN		BIT(2)
+#define PSCTL_ALBCN		BIT(1)
+#define PSCTL_PSEN		BIT(0)
+
+/* Bits in the PSPWSIG register */
+#define PSSIG_WPE3		BIT(7)
+#define PSSIG_WPE2		BIT(6)
+#define PSSIG_WPE1		BIT(5)
+#define PSSIG_WRADIOPE		BIT(4)
+#define PSSIG_SPE3		BIT(3)
+#define PSSIG_SPE2		BIT(2)
+#define PSSIG_SPE1		BIT(1)
+#define PSSIG_SRADIOPE		BIT(0)
+
+/* Bits in the BBREGCTL register */
+#define BBREGCTL_DONE		BIT(2)
+#define BBREGCTL_REGR		BIT(1)
+#define BBREGCTL_REGW		BIT(0)
+
+/* Bits in the IFREGCTL register */
+#define IFREGCTL_DONE		BIT(2)
+#define IFREGCTL_IFRF		BIT(1)
+#define IFREGCTL_REGW		BIT(0)
+
+/* Bits in the SOFTPWRCTL register */
+#define SOFTPWRCTL_RFLEOPT	BIT(3)
+#define SOFTPWRCTL_TXPEINV	BIT(1)
+#define SOFTPWRCTL_SWPECTI	BIT(0)
+#define SOFTPWRCTL_SWPAPE	BIT(5)
+#define SOFTPWRCTL_SWCALEN	BIT(4)
+#define SOFTPWRCTL_SWRADIO_PE	BIT(3)
+#define SOFTPWRCTL_SWPE2	BIT(2)
+#define SOFTPWRCTL_SWPE1	BIT(1)
+#define SOFTPWRCTL_SWPE3	BIT(0)
+
+/* Bits in the GPIOCTL1 register */
+#define GPIO3_MD		BIT(5)
+#define GPIO3_DATA		BIT(6)
+#define GPIO3_INTMD		BIT(7)
+
+/* Bits in the MISCFFCTL register */
+#define MISCFFCTL_WRITE		BIT(0)
+
+/* Loopback mode */
+#define MAC_LB_EXT		BIT(1)
+#define MAC_LB_INTERNAL		BIT(0)
+#define MAC_LB_NONE		0x00
+
+/* Ethernet address filter type */
+#define PKT_TYPE_NONE		0x00 /* turn off receiver */
+#define PKT_TYPE_ALL_MULTICAST	BIT(7)
+#define PKT_TYPE_PROMISCUOUS	BIT(6)
+#define PKT_TYPE_DIRECTED	BIT(5)	/* obselete */
+#define PKT_TYPE_BROADCAST	BIT(4)
+#define PKT_TYPE_MULTICAST	BIT(3)
+#define PKT_TYPE_ERROR_WPA	BIT(2)
+#define PKT_TYPE_ERROR_CRC	BIT(1)
+#define PKT_TYPE_BSSID		BIT(0)
+
+#define DEFAULT_BI		0x200
+
+/* MiscFIFO Offset */
+#define MISCFIFO_KEYETRY0	32
+#define MISCFIFO_KEYENTRYSIZE	22
+
+#define MAC_REVISION_A0		0x00
+#define MAC_REVISION_A1		0x01
+
+struct vnt_mac_set_key {
+	union {
+		struct {
+			u8 addr[ETH_ALEN];
+			__le16 key_ctl;
+		} write __packed;
+		u32 swap[2];
+	} u;
+	u8 key[WLAN_KEY_LEN_CCMP];
+} __packed;
+
+int vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter);
+int vnt_mac_shutdown(struct vnt_private *priv);
+int vnt_mac_set_bb_type(struct vnt_private *priv, u8 type);
+int vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx);
+int vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+			 u32 key_idx, u8 *addr, u8 *key);
+int vnt_mac_reg_bits_off(struct vnt_private *priv, u8 reg_ofs, u8 bits);
+int vnt_mac_reg_bits_on(struct vnt_private *priv, u8 reg_ofs, u8 bits);
+int vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word);
+int vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr);
+int vnt_mac_enable_protect_mode(struct vnt_private *priv);
+int vnt_mac_disable_protect_mode(struct vnt_private *priv);
+int vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv);
+int vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv);
+int vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval);
+int vnt_mac_set_led(struct vnt_private *privpriv, u8 state, u8 led);
+
+#endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
new file mode 100644
index 000000000..897ee0f7f
--- /dev/null
+++ b/drivers/staging/vt6656/main_usb.c
@@ -0,0 +1,1116 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: driver entry for initial, open, close, tx and rx.
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: Dec 8, 2005
+ *
+ * Functions:
+ *
+ *   vt6656_probe - module initial (insmod) driver entry
+ *   vnt_free_tx_bufs - free tx buffer function
+ *   vnt_init_registers- initial MAC & BBP & RF internal registers.
+ *
+ * Revision History:
+ */
+#undef __NO_VERSION__
+
+#include <linux/bits.h>
+#include <linux/etherdevice.h>
+#include <linux/file.h>
+#include <linux/kernel.h>
+#include "device.h"
+#include "card.h"
+#include "baseband.h"
+#include "mac.h"
+#include "power.h"
+#include "wcmd.h"
+#include "rxtx.h"
+#include "rf.h"
+#include "usbpipe.h"
+#include "channel.h"
+
+/*
+ * define module options
+ */
+
+/* version information */
+#define DRIVER_AUTHOR \
+	"VIA Networking Technologies, Inc., <lyndonchen@vntek.com.tw>"
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DEVICE_FULL_DRV_NAM);
+
+#define RX_DESC_DEF0 64
+static int vnt_rx_buffers = RX_DESC_DEF0;
+module_param_named(rx_buffers, vnt_rx_buffers, int, 0644);
+MODULE_PARM_DESC(rx_buffers, "Number of receive usb rx buffers");
+
+#define TX_DESC_DEF0 64
+static int vnt_tx_buffers = TX_DESC_DEF0;
+module_param_named(tx_buffers, vnt_tx_buffers, int, 0644);
+MODULE_PARM_DESC(tx_buffers, "Number of receive usb tx buffers");
+
+#define RTS_THRESH_DEF     2347
+#define FRAG_THRESH_DEF     2346
+
+/* BasebandType[] baseband type selected
+ * 0: indicate 802.11a type
+ * 1: indicate 802.11b type
+ * 2: indicate 802.11g type
+ */
+
+#define BBP_TYPE_DEF     2
+
+/*
+ * Static vars definitions
+ */
+
+static const struct usb_device_id vt6656_table[] = {
+	{USB_DEVICE(VNT_USB_VENDOR_ID, VNT_USB_PRODUCT_ID)},
+	{}
+};
+
+static void vnt_set_options(struct vnt_private *priv)
+{
+	/* Set number of TX buffers */
+	if (vnt_tx_buffers < CB_MIN_TX_DESC || vnt_tx_buffers > CB_MAX_TX_DESC)
+		priv->num_tx_context = TX_DESC_DEF0;
+	else
+		priv->num_tx_context = vnt_tx_buffers;
+
+	/* Set number of RX buffers */
+	if (vnt_rx_buffers < CB_MIN_RX_DESC || vnt_rx_buffers > CB_MAX_RX_DESC)
+		priv->num_rcb = RX_DESC_DEF0;
+	else
+		priv->num_rcb = vnt_rx_buffers;
+
+	priv->op_mode = NL80211_IFTYPE_UNSPECIFIED;
+	priv->bb_type = BBP_TYPE_DEF;
+	priv->packet_type = priv->bb_type;
+	priv->preamble_type = PREAMBLE_LONG;
+	priv->exist_sw_net_addr = false;
+}
+
+static int vnt_download_firmware(struct vnt_private *priv)
+{
+	struct device *dev = &priv->usb->dev;
+	const struct firmware *fw;
+	u16 length;
+	int ii;
+	int ret = 0;
+
+	dev_dbg(dev, "---->Download firmware\n");
+
+	ret = request_firmware(&fw, FIRMWARE_NAME, dev);
+	if (ret) {
+		dev_err(dev, "firmware file %s request failed (%d)\n",
+			FIRMWARE_NAME, ret);
+		goto end;
+	}
+
+	for (ii = 0; ii < fw->size; ii += FIRMWARE_CHUNK_SIZE) {
+		length = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
+
+		ret = vnt_control_out(priv, 0, 0x1200 + ii, 0x0000, length,
+				      fw->data + ii);
+		if (ret)
+			goto free_fw;
+
+		dev_dbg(dev, "Download firmware...%d %zu\n", ii, fw->size);
+	}
+
+free_fw:
+	release_firmware(fw);
+end:
+	return ret;
+}
+
+static int vnt_firmware_branch_to_sram(struct vnt_private *priv)
+{
+	dev_dbg(&priv->usb->dev, "---->Branch to Sram\n");
+
+	return vnt_control_out(priv, 1, 0x1200, 0x0000, 0, NULL);
+}
+
+static int vnt_check_firmware_version(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, 0,
+			     MESSAGE_REQUEST_VERSION, 2,
+			     (u8 *)&priv->firmware_version);
+	if (ret) {
+		dev_dbg(&priv->usb->dev,
+			"Could not get firmware version: %d.\n", ret);
+		goto end;
+	}
+
+	dev_dbg(&priv->usb->dev, "Firmware Version [%04x]\n",
+		priv->firmware_version);
+
+	if (priv->firmware_version == 0xFFFF) {
+		dev_dbg(&priv->usb->dev, "In Loader.\n");
+		ret = -EINVAL;
+		goto end;
+	}
+
+	if (priv->firmware_version < FIRMWARE_VERSION) {
+		/* branch to loader for download new firmware */
+		ret = vnt_firmware_branch_to_sram(priv);
+		if (ret) {
+			dev_dbg(&priv->usb->dev,
+				"Could not branch to SRAM: %d.\n", ret);
+		} else {
+			ret = -EINVAL;
+		}
+	}
+
+end:
+	return ret;
+}
+
+/*
+ * initialization of MAC & BBP registers
+ */
+static int vnt_init_registers(struct vnt_private *priv)
+{
+	int ret;
+	struct vnt_cmd_card_init *init_cmd = &priv->init_command;
+	struct vnt_rsp_card_init *init_rsp = &priv->init_response;
+	u8 antenna;
+	int ii;
+	u8 tmp;
+	u8 calib_tx_iq = 0, calib_tx_dc = 0, calib_rx_iq = 0;
+
+	dev_dbg(&priv->usb->dev, "---->INIbInitAdapter. [%d][%d]\n",
+		DEVICE_INIT_COLD, priv->packet_type);
+
+	ret = vnt_check_firmware_version(priv);
+	if (ret) {
+		ret = vnt_download_firmware(priv);
+		if (ret) {
+			dev_dbg(&priv->usb->dev,
+				"Could not download firmware: %d.\n", ret);
+			goto end;
+		}
+
+		ret = vnt_firmware_branch_to_sram(priv);
+		if (ret) {
+			dev_dbg(&priv->usb->dev,
+				"Could not branch to SRAM: %d.\n", ret);
+			goto end;
+		}
+	}
+
+	ret = vnt_vt3184_init(priv);
+	if (ret) {
+		dev_dbg(&priv->usb->dev, "vnt_vt3184_init fail\n");
+		goto end;
+	}
+
+	init_cmd->init_class = DEVICE_INIT_COLD;
+	init_cmd->exist_sw_net_addr = priv->exist_sw_net_addr;
+	for (ii = 0; ii < ARRAY_SIZE(init_cmd->sw_net_addr); ii++)
+		init_cmd->sw_net_addr[ii] = priv->current_net_addr[ii];
+	init_cmd->short_retry_limit = priv->hw->wiphy->retry_short;
+	init_cmd->long_retry_limit = priv->hw->wiphy->retry_long;
+
+	/* issue card_init command to device */
+	ret = vnt_control_out(priv, MESSAGE_TYPE_CARDINIT, 0, 0,
+			      sizeof(struct vnt_cmd_card_init),
+			      (u8 *)init_cmd);
+	if (ret) {
+		dev_dbg(&priv->usb->dev, "Issue Card init fail\n");
+		goto end;
+	}
+
+	ret = vnt_control_in(priv, MESSAGE_TYPE_INIT_RSP, 0, 0,
+			     sizeof(struct vnt_rsp_card_init),
+			     (u8 *)init_rsp);
+	if (ret) {
+		dev_dbg(&priv->usb->dev, "Cardinit request in status fail!\n");
+		goto end;
+	}
+
+	/* local ID for AES functions */
+	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, MAC_REG_LOCALID,
+			     MESSAGE_REQUEST_MACREG, 1, &priv->local_id);
+	if (ret)
+		goto end;
+
+	/* do MACbSoftwareReset in MACvInitialize */
+
+	priv->top_ofdm_basic_rate = RATE_24M;
+	priv->top_cck_basic_rate = RATE_1M;
+
+	/* target to IF pin while programming to RF chip */
+	priv->power = 0xFF;
+
+	priv->cck_pwr = priv->eeprom[EEP_OFS_PWR_CCK];
+	priv->ofdm_pwr_g = priv->eeprom[EEP_OFS_PWR_OFDMG];
+	/* load power table */
+	for (ii = 0; ii < ARRAY_SIZE(priv->cck_pwr_tbl); ii++) {
+		priv->cck_pwr_tbl[ii] =
+			priv->eeprom[ii + EEP_OFS_CCK_PWR_TBL];
+		if (priv->cck_pwr_tbl[ii] == 0)
+			priv->cck_pwr_tbl[ii] = priv->cck_pwr;
+
+		priv->ofdm_pwr_tbl[ii] =
+				priv->eeprom[ii + EEP_OFS_OFDM_PWR_TBL];
+		if (priv->ofdm_pwr_tbl[ii] == 0)
+			priv->ofdm_pwr_tbl[ii] = priv->ofdm_pwr_g;
+	}
+
+	/*
+	 * original zonetype is USA, but custom zonetype is Europe,
+	 * then need to recover 12, 13, 14 channels with 11 channel
+	 */
+	for (ii = 11; ii < ARRAY_SIZE(priv->cck_pwr_tbl); ii++) {
+		priv->cck_pwr_tbl[ii] = priv->cck_pwr_tbl[10];
+		priv->ofdm_pwr_tbl[ii] = priv->ofdm_pwr_tbl[10];
+	}
+
+	priv->ofdm_pwr_a = 0x34; /* same as RFbMA2829SelectChannel */
+
+	/* load OFDM A power table */
+	for (ii = 0; ii < CB_MAX_CHANNEL_5G; ii++) {
+		priv->ofdm_a_pwr_tbl[ii] =
+			priv->eeprom[ii + EEP_OFS_OFDMA_PWR_TBL];
+
+		if (priv->ofdm_a_pwr_tbl[ii] == 0)
+			priv->ofdm_a_pwr_tbl[ii] = priv->ofdm_pwr_a;
+	}
+
+	antenna = priv->eeprom[EEP_OFS_ANTENNA];
+
+	if (antenna & EEP_ANTINV)
+		priv->tx_rx_ant_inv = true;
+	else
+		priv->tx_rx_ant_inv = false;
+
+	antenna &= (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+
+	if (antenna == 0) /* if not set default is both */
+		antenna = (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+
+	if (antenna == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
+		priv->tx_antenna_mode = ANT_B;
+		priv->rx_antenna_sel = 1;
+
+		if (priv->tx_rx_ant_inv)
+			priv->rx_antenna_mode = ANT_A;
+		else
+			priv->rx_antenna_mode = ANT_B;
+	} else  {
+		priv->rx_antenna_sel = 0;
+
+		if (antenna & EEP_ANTENNA_AUX) {
+			priv->tx_antenna_mode = ANT_A;
+
+			if (priv->tx_rx_ant_inv)
+				priv->rx_antenna_mode = ANT_B;
+			else
+				priv->rx_antenna_mode = ANT_A;
+		} else {
+			priv->tx_antenna_mode = ANT_B;
+
+			if (priv->tx_rx_ant_inv)
+				priv->rx_antenna_mode = ANT_A;
+			else
+				priv->rx_antenna_mode = ANT_B;
+		}
+	}
+
+	/* Set initial antenna mode */
+	ret = vnt_set_antenna_mode(priv, priv->rx_antenna_mode);
+	if (ret)
+		goto end;
+
+	/* default Auto Mode */
+	priv->bb_type = BB_TYPE_11G;
+
+	/* get RFType */
+	priv->rf_type = init_rsp->rf_type;
+
+	/* load vt3266 calibration parameters in EEPROM */
+	if (priv->rf_type == RF_VT3226D0) {
+		if ((priv->eeprom[EEP_OFS_MAJOR_VER] == 0x1) &&
+		    (priv->eeprom[EEP_OFS_MINOR_VER] >= 0x4)) {
+			calib_tx_iq = priv->eeprom[EEP_OFS_CALIB_TX_IQ];
+			calib_tx_dc = priv->eeprom[EEP_OFS_CALIB_TX_DC];
+			calib_rx_iq = priv->eeprom[EEP_OFS_CALIB_RX_IQ];
+			if (calib_tx_iq || calib_tx_dc || calib_rx_iq) {
+				/* CR255, enable TX/RX IQ and
+				 * DC compensation mode
+				 */
+				ret = vnt_control_out_u8(priv,
+							 MESSAGE_REQUEST_BBREG,
+							 0xff, 0x03);
+				if (ret)
+					goto end;
+
+				/* CR251, TX I/Q Imbalance Calibration */
+				ret = vnt_control_out_u8(priv,
+							 MESSAGE_REQUEST_BBREG,
+							 0xfb, calib_tx_iq);
+				if (ret)
+					goto end;
+
+				/* CR252, TX DC-Offset Calibration */
+				ret = vnt_control_out_u8(priv,
+							 MESSAGE_REQUEST_BBREG,
+							 0xfC, calib_tx_dc);
+				if (ret)
+					goto end;
+
+				/* CR253, RX I/Q Imbalance Calibration */
+				ret = vnt_control_out_u8(priv,
+							 MESSAGE_REQUEST_BBREG,
+							 0xfd, calib_rx_iq);
+				if (ret)
+					goto end;
+			} else {
+				/* CR255, turn off
+				 * BB Calibration compensation
+				 */
+				ret = vnt_control_out_u8(priv,
+							 MESSAGE_REQUEST_BBREG,
+							 0xff, 0x0);
+				if (ret)
+					goto end;
+			}
+		}
+	}
+
+	/* get permanent network address */
+	memcpy(priv->permanent_net_addr, init_rsp->net_addr, 6);
+	ether_addr_copy(priv->current_net_addr, priv->permanent_net_addr);
+
+	/* if exist SW network address, use it */
+	dev_dbg(&priv->usb->dev, "Network address = %pM\n",
+		priv->current_net_addr);
+
+	priv->radio_ctl = priv->eeprom[EEP_OFS_RADIOCTL];
+
+	if ((priv->radio_ctl & EEP_RADIOCTL_ENABLE) != 0) {
+		ret = vnt_control_in(priv, MESSAGE_TYPE_READ,
+				     MAC_REG_GPIOCTL1, MESSAGE_REQUEST_MACREG,
+				     1, &tmp);
+		if (ret)
+			goto end;
+
+		if ((tmp & GPIO3_DATA) == 0) {
+			ret = vnt_mac_reg_bits_on(priv, MAC_REG_GPIOCTL1,
+						  GPIO3_INTMD);
+		} else {
+			ret = vnt_mac_reg_bits_off(priv, MAC_REG_GPIOCTL1,
+						   GPIO3_INTMD);
+		}
+
+		if (ret)
+			goto end;
+	}
+
+	ret = vnt_mac_set_led(priv, LEDSTS_TMLEN, 0x38);
+	if (ret)
+		goto end;
+
+	ret = vnt_mac_set_led(priv, LEDSTS_STS, LEDSTS_SLOW);
+	if (ret)
+		goto end;
+
+	ret = vnt_mac_reg_bits_on(priv, MAC_REG_GPIOCTL0, BIT(0));
+	if (ret)
+		goto end;
+
+	ret = vnt_radio_power_on(priv);
+	if (ret)
+		goto end;
+
+	dev_dbg(&priv->usb->dev, "<----INIbInitAdapter Exit\n");
+
+end:
+	return ret;
+}
+
+static void vnt_free_tx_bufs(struct vnt_private *priv)
+{
+	struct vnt_usb_send_context *tx_context;
+	int ii;
+
+	usb_kill_anchored_urbs(&priv->tx_submitted);
+
+	for (ii = 0; ii < priv->num_tx_context; ii++) {
+		tx_context = priv->tx_context[ii];
+		if (!tx_context)
+			continue;
+
+		kfree(tx_context);
+	}
+}
+
+static void vnt_free_rx_bufs(struct vnt_private *priv)
+{
+	struct vnt_rcb *rcb;
+	int ii;
+
+	for (ii = 0; ii < priv->num_rcb; ii++) {
+		rcb = priv->rcb[ii];
+		if (!rcb)
+			continue;
+
+		/* deallocate URBs */
+		if (rcb->urb) {
+			usb_kill_urb(rcb->urb);
+			usb_free_urb(rcb->urb);
+		}
+
+		/* deallocate skb */
+		if (rcb->skb)
+			dev_kfree_skb(rcb->skb);
+
+		kfree(rcb);
+	}
+}
+
+static void vnt_free_int_bufs(struct vnt_private *priv)
+{
+	kfree(priv->int_buf.data_buf);
+}
+
+static int vnt_alloc_bufs(struct vnt_private *priv)
+{
+	int ret;
+	struct vnt_usb_send_context *tx_context;
+	struct vnt_rcb *rcb;
+	int ii;
+
+	init_usb_anchor(&priv->tx_submitted);
+
+	for (ii = 0; ii < priv->num_tx_context; ii++) {
+		tx_context = kmalloc(sizeof(*tx_context), GFP_KERNEL);
+		if (!tx_context) {
+			ret = -ENOMEM;
+			goto free_tx;
+		}
+
+		priv->tx_context[ii] = tx_context;
+		tx_context->priv = priv;
+		tx_context->pkt_no = ii;
+		tx_context->in_use = false;
+	}
+
+	for (ii = 0; ii < priv->num_rcb; ii++) {
+		priv->rcb[ii] = kzalloc(sizeof(*priv->rcb[ii]), GFP_KERNEL);
+		if (!priv->rcb[ii]) {
+			ret = -ENOMEM;
+			goto free_rx_tx;
+		}
+
+		rcb = priv->rcb[ii];
+
+		rcb->priv = priv;
+
+		/* allocate URBs */
+		rcb->urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!rcb->urb) {
+			ret = -ENOMEM;
+			goto free_rx_tx;
+		}
+
+		rcb->skb = dev_alloc_skb(priv->rx_buf_sz);
+		if (!rcb->skb) {
+			ret = -ENOMEM;
+			goto free_rx_tx;
+		}
+		/* submit rx urb */
+		ret = vnt_submit_rx_urb(priv, rcb);
+		if (ret)
+			goto free_rx_tx;
+	}
+
+	priv->interrupt_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!priv->interrupt_urb) {
+		ret = -ENOMEM;
+		goto free_rx_tx;
+	}
+
+	priv->int_buf.data_buf = kmalloc(MAX_INTERRUPT_SIZE, GFP_KERNEL);
+	if (!priv->int_buf.data_buf) {
+		ret = -ENOMEM;
+		goto free_rx_tx_urb;
+	}
+
+	return 0;
+
+free_rx_tx_urb:
+	usb_free_urb(priv->interrupt_urb);
+free_rx_tx:
+	vnt_free_rx_bufs(priv);
+free_tx:
+	vnt_free_tx_bufs(priv);
+	return ret;
+}
+
+static void vnt_tx_80211(struct ieee80211_hw *hw,
+			 struct ieee80211_tx_control *control,
+			 struct sk_buff *skb)
+{
+	struct vnt_private *priv = hw->priv;
+
+	if (vnt_tx_packet(priv, skb))
+		ieee80211_free_txskb(hw, skb);
+}
+
+static int vnt_start(struct ieee80211_hw *hw)
+{
+	int ret;
+	struct vnt_private *priv = hw->priv;
+
+	priv->rx_buf_sz = MAX_TOTAL_SIZE_WITH_ALL_HEADERS;
+
+	ret = vnt_alloc_bufs(priv);
+	if (ret) {
+		dev_dbg(&priv->usb->dev, "vnt_alloc_bufs fail...\n");
+		goto err;
+	}
+
+	clear_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags);
+
+	ret = vnt_init_registers(priv);
+	if (ret) {
+		dev_dbg(&priv->usb->dev, " init register fail\n");
+		goto free_all;
+	}
+
+	ret = vnt_key_init_table(priv);
+	if (ret)
+		goto free_all;
+
+	priv->int_interval = 1;  /* bInterval is set to 1 */
+
+	ret = vnt_start_interrupt_urb(priv);
+	if (ret)
+		goto free_all;
+
+	ieee80211_wake_queues(hw);
+
+	return 0;
+
+free_all:
+	vnt_free_rx_bufs(priv);
+	vnt_free_tx_bufs(priv);
+	vnt_free_int_bufs(priv);
+
+	usb_kill_urb(priv->interrupt_urb);
+	usb_free_urb(priv->interrupt_urb);
+err:
+	return ret;
+}
+
+static void vnt_stop(struct ieee80211_hw *hw)
+{
+	struct vnt_private *priv = hw->priv;
+	int i;
+
+	if (!priv)
+		return;
+
+	for (i = 0; i < MAX_KEY_TABLE; i++)
+		vnt_mac_disable_keyentry(priv, i);
+
+	/* clear all keys */
+	priv->key_entry_inuse = 0;
+
+	if (!test_bit(DEVICE_FLAGS_UNPLUG, &priv->flags))
+		vnt_mac_shutdown(priv);
+
+	ieee80211_stop_queues(hw);
+
+	set_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags);
+
+	cancel_delayed_work_sync(&priv->run_command_work);
+
+	priv->cmd_running = false;
+
+	vnt_free_tx_bufs(priv);
+	vnt_free_rx_bufs(priv);
+	vnt_free_int_bufs(priv);
+
+	usb_kill_urb(priv->interrupt_urb);
+	usb_free_urb(priv->interrupt_urb);
+}
+
+static int vnt_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	priv->vif = vif;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		vnt_mac_reg_bits_off(priv, MAC_REG_RCR, RCR_UNICAST);
+
+		vnt_mac_reg_bits_on(priv, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+
+		break;
+	case NL80211_IFTYPE_AP:
+		vnt_mac_reg_bits_off(priv, MAC_REG_RCR, RCR_UNICAST);
+
+		vnt_mac_reg_bits_on(priv, MAC_REG_HOSTCR, HOSTCR_AP);
+
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	priv->op_mode = vif->type;
+
+	/* LED blink on TX */
+	vnt_mac_set_led(priv, LEDSTS_STS, LEDSTS_INTER);
+
+	return 0;
+}
+
+static void vnt_remove_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+		vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		vnt_mac_reg_bits_off(priv, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+		break;
+	case NL80211_IFTYPE_AP:
+		vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+		vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		vnt_mac_reg_bits_off(priv, MAC_REG_HOSTCR, HOSTCR_AP);
+		break;
+	default:
+		break;
+	}
+
+	vnt_radio_power_off(priv);
+
+	priv->op_mode = NL80211_IFTYPE_UNSPECIFIED;
+
+	/* LED slow blink */
+	vnt_mac_set_led(priv, LEDSTS_STS, LEDSTS_SLOW);
+}
+
+static int vnt_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct vnt_private *priv = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		if (conf->flags & IEEE80211_CONF_PS)
+			vnt_enable_power_saving(priv, conf->listen_interval);
+		else
+			vnt_disable_power_saving(priv);
+	}
+
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) ||
+	    (conf->flags & IEEE80211_CONF_OFFCHANNEL)) {
+		vnt_set_channel(priv, conf->chandef.chan->hw_value);
+
+		if (conf->chandef.chan->band == NL80211_BAND_5GHZ)
+			priv->bb_type = BB_TYPE_11A;
+		else
+			priv->bb_type = BB_TYPE_11G;
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_POWER)
+		vnt_rf_setpower(priv, conf->chandef.chan);
+
+	if (conf->flags & (IEEE80211_CONF_OFFCHANNEL | IEEE80211_CONF_IDLE))
+		/* Set max sensitivity*/
+		vnt_update_pre_ed_threshold(priv, true);
+	else
+		vnt_update_pre_ed_threshold(priv, false);
+
+	return 0;
+}
+
+static void vnt_bss_info_changed(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_bss_conf *conf, u64 changed)
+{
+	struct vnt_private *priv = hw->priv;
+
+	priv->current_aid = vif->cfg.aid;
+
+	if (changed & BSS_CHANGED_BSSID && conf->bssid)
+		vnt_mac_set_bssid_addr(priv, (u8 *)conf->bssid);
+
+	if (changed & BSS_CHANGED_BASIC_RATES) {
+		priv->basic_rates = conf->basic_rates;
+
+		vnt_update_top_rates(priv);
+
+		dev_dbg(&priv->usb->dev, "basic rates %x\n", conf->basic_rates);
+	}
+
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		if (conf->use_short_preamble) {
+			vnt_mac_enable_barker_preamble_mode(priv);
+			priv->preamble_type = PREAMBLE_SHORT;
+		} else {
+			vnt_mac_disable_barker_preamble_mode(priv);
+			priv->preamble_type = PREAMBLE_LONG;
+		}
+	}
+
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		if (conf->use_cts_prot)
+			vnt_mac_enable_protect_mode(priv);
+		else
+			vnt_mac_disable_protect_mode(priv);
+	}
+
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		if (conf->use_short_slot)
+			priv->short_slot_time = true;
+		else
+			priv->short_slot_time = false;
+
+		vnt_set_short_slot_time(priv);
+		vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
+	}
+
+	if (changed & (BSS_CHANGED_BASIC_RATES | BSS_CHANGED_ERP_PREAMBLE |
+		       BSS_CHANGED_ERP_SLOT))
+		vnt_set_bss_mode(priv);
+
+	if (changed & (BSS_CHANGED_TXPOWER | BSS_CHANGED_BANDWIDTH))
+		vnt_rf_setpower(priv, conf->chandef.chan);
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		dev_dbg(&priv->usb->dev,
+			"Beacon enable %d\n", conf->enable_beacon);
+
+		if (conf->enable_beacon) {
+			vnt_beacon_enable(priv, vif, conf);
+
+			vnt_mac_reg_bits_on(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+		} else {
+			vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+		}
+	}
+
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_BEACON_INFO) &&
+	    priv->op_mode != NL80211_IFTYPE_AP) {
+		if (vif->cfg.assoc && conf->beacon_rate) {
+			u16 ps_beacon_int = conf->beacon_int;
+
+			if (conf->dtim_period)
+				ps_beacon_int *= conf->dtim_period;
+			else if (hw->conf.listen_interval)
+				ps_beacon_int *= hw->conf.listen_interval;
+
+			vnt_mac_reg_bits_on(priv, MAC_REG_TFTCTL,
+					    TFTCTL_TSFCNTREN);
+
+			vnt_mac_set_beacon_interval(priv, ps_beacon_int);
+
+			vnt_reset_next_tbtt(priv, conf->beacon_int);
+
+			vnt_adjust_tsf(priv, conf->beacon_rate->hw_value,
+				       conf->sync_tsf, priv->current_tsf);
+
+			vnt_update_next_tbtt(priv,
+					     conf->sync_tsf, ps_beacon_int);
+		} else {
+			vnt_clear_current_tsf(priv);
+
+			vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL,
+					     TFTCTL_TSFCNTREN);
+		}
+	}
+}
+
+static u64 vnt_prepare_multicast(struct ieee80211_hw *hw,
+				 struct netdev_hw_addr_list *mc_list)
+{
+	struct vnt_private *priv = hw->priv;
+	struct netdev_hw_addr *ha;
+	u64 mc_filter = 0;
+	u32 bit_nr;
+
+	netdev_hw_addr_list_for_each(ha, mc_list) {
+		bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+		mc_filter |= BIT_ULL(bit_nr);
+	}
+
+	priv->mc_list_count = mc_list->count;
+
+	return mc_filter;
+}
+
+static void vnt_configure(struct ieee80211_hw *hw,
+			  unsigned int changed_flags,
+			  unsigned int *total_flags, u64 multicast)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 rx_mode = 0;
+
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC;
+
+	vnt_control_in(priv, MESSAGE_TYPE_READ, MAC_REG_RCR,
+		       MESSAGE_REQUEST_MACREG, sizeof(u8), &rx_mode);
+
+	dev_dbg(&priv->usb->dev, "rx mode in = %x\n", rx_mode);
+
+	if (changed_flags & FIF_ALLMULTI) {
+		if (*total_flags & FIF_ALLMULTI) {
+			if (priv->mc_list_count > 2)
+				vnt_mac_set_filter(priv, ~0);
+			else
+				vnt_mac_set_filter(priv, multicast);
+
+			rx_mode |= RCR_MULTICAST | RCR_BROADCAST;
+		} else {
+			rx_mode &= ~(RCR_MULTICAST | RCR_BROADCAST);
+		}
+	}
+
+	if (changed_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC)) {
+		if (*total_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC))
+			rx_mode &= ~RCR_BSSID;
+		else
+			rx_mode |= RCR_BSSID;
+	}
+
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_RCR, rx_mode);
+
+	dev_dbg(&priv->usb->dev, "rx mode out= %x\n", rx_mode);
+}
+
+static int vnt_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key)
+{
+	struct vnt_private *priv = hw->priv;
+
+	switch (cmd) {
+	case SET_KEY:
+		return vnt_set_keys(hw, sta, vif, key);
+	case DISABLE_KEY:
+		if (test_bit(key->hw_key_idx, &priv->key_entry_inuse)) {
+			clear_bit(key->hw_key_idx, &priv->key_entry_inuse);
+
+			vnt_mac_disable_keyentry(priv, key->hw_key_idx);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int vnt_get_stats(struct ieee80211_hw *hw,
+			 struct ieee80211_low_level_stats *stats)
+{
+	struct vnt_private *priv = hw->priv;
+
+	memcpy(stats, &priv->low_stats, sizeof(*stats));
+
+	return 0;
+}
+
+static u64 vnt_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	return priv->current_tsf;
+}
+
+static void vnt_set_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			u64 tsf)
+{
+	struct vnt_private *priv = hw->priv;
+
+	vnt_update_next_tbtt(priv, tsf, vif->bss_conf.beacon_int);
+}
+
+static void vnt_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+
+	vnt_clear_current_tsf(priv);
+}
+
+static const struct ieee80211_ops vnt_mac_ops = {
+	.tx			= vnt_tx_80211,
+	.start			= vnt_start,
+	.stop			= vnt_stop,
+	.add_interface		= vnt_add_interface,
+	.remove_interface	= vnt_remove_interface,
+	.config			= vnt_config,
+	.bss_info_changed	= vnt_bss_info_changed,
+	.prepare_multicast	= vnt_prepare_multicast,
+	.configure_filter	= vnt_configure,
+	.set_key		= vnt_set_key,
+	.get_stats		= vnt_get_stats,
+	.get_tsf		= vnt_get_tsf,
+	.set_tsf		= vnt_set_tsf,
+	.reset_tsf		= vnt_reset_tsf,
+};
+
+int vnt_init(struct vnt_private *priv)
+{
+	if (vnt_init_registers(priv))
+		return -EAGAIN;
+
+	SET_IEEE80211_PERM_ADDR(priv->hw, priv->permanent_net_addr);
+
+	vnt_init_bands(priv);
+
+	if (ieee80211_register_hw(priv->hw))
+		return -ENODEV;
+
+	priv->mac_hw = true;
+
+	vnt_radio_power_off(priv);
+
+	return 0;
+}
+
+static int
+vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+	struct usb_device *udev;
+	struct vnt_private *priv;
+	struct ieee80211_hw *hw;
+	struct wiphy *wiphy;
+	int rc;
+
+	udev = usb_get_dev(interface_to_usbdev(intf));
+
+	dev_notice(&udev->dev, "%s Ver. %s\n",
+		   DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
+	dev_notice(&udev->dev,
+		   "Copyright (c) 2004 VIA Networking Technologies, Inc.\n");
+
+	hw = ieee80211_alloc_hw(sizeof(struct vnt_private), &vnt_mac_ops);
+	if (!hw) {
+		dev_err(&udev->dev, "could not register ieee80211_hw\n");
+		rc = -ENOMEM;
+		goto err_nomem;
+	}
+
+	priv = hw->priv;
+	priv->hw = hw;
+	priv->usb = udev;
+	priv->intf = intf;
+
+	vnt_set_options(priv);
+
+	spin_lock_init(&priv->lock);
+	mutex_init(&priv->usb_lock);
+
+	INIT_DELAYED_WORK(&priv->run_command_work, vnt_run_command);
+
+	usb_set_intfdata(intf, priv);
+
+	wiphy = priv->hw->wiphy;
+
+	wiphy->frag_threshold = FRAG_THRESH_DEF;
+	wiphy->rts_threshold = RTS_THRESH_DEF;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
+
+	ieee80211_hw_set(priv->hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(priv->hw, SIGNAL_DBM);
+	ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(priv->hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(priv->hw, SUPPORTS_PS);
+	ieee80211_hw_set(priv->hw, PS_NULLFUNC_STACK);
+
+	priv->hw->extra_tx_headroom =
+		sizeof(struct vnt_tx_buffer) + sizeof(struct vnt_tx_usb_header);
+	priv->hw->max_signal = 100;
+
+	SET_IEEE80211_DEV(priv->hw, &intf->dev);
+
+	rc = usb_reset_device(priv->usb);
+	if (rc)
+		dev_warn(&priv->usb->dev,
+			 "%s reset fail status=%d\n", __func__, rc);
+
+	clear_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags);
+	vnt_reset_command_timer(priv);
+
+	vnt_schedule_command(priv, WLAN_CMD_INIT_MAC80211);
+
+	return 0;
+
+err_nomem:
+	usb_put_dev(udev);
+
+	return rc;
+}
+
+static void vt6656_disconnect(struct usb_interface *intf)
+{
+	struct vnt_private *priv = usb_get_intfdata(intf);
+
+	if (!priv)
+		return;
+
+	if (priv->mac_hw)
+		ieee80211_unregister_hw(priv->hw);
+
+	usb_set_intfdata(intf, NULL);
+	usb_put_dev(interface_to_usbdev(intf));
+
+	set_bit(DEVICE_FLAGS_UNPLUG, &priv->flags);
+
+	ieee80211_free_hw(priv->hw);
+}
+
+#ifdef CONFIG_PM
+
+static int vt6656_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	return 0;
+}
+
+static int vt6656_resume(struct usb_interface *intf)
+{
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+
+MODULE_DEVICE_TABLE(usb, vt6656_table);
+
+static struct usb_driver vt6656_driver = {
+	.name =		DEVICE_NAME,
+	.probe =	vt6656_probe,
+	.disconnect =	vt6656_disconnect,
+	.id_table =	vt6656_table,
+#ifdef CONFIG_PM
+	.suspend = vt6656_suspend,
+	.resume = vt6656_resume,
+#endif /* CONFIG_PM */
+};
+
+module_usb_driver(vt6656_driver);
+
+MODULE_FIRMWARE(FIRMWARE_NAME);
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
new file mode 100644
index 000000000..e5411f628
--- /dev/null
+++ b/drivers/staging/vt6656/power.c
@@ -0,0 +1,112 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles 802.11 power management functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: July 17, 2002
+ *
+ * Functions:
+ *      vnt_enable_power_saving - Enable Power Saving Mode
+ *      PSvDiasblePowerSaving - Disable Power Saving Mode
+ *      vnt_next_tbtt_wakeup - Decide if we need to wake up at next Beacon
+ *
+ * Revision History:
+ *
+ */
+
+#include "mac.h"
+#include "device.h"
+#include "power.h"
+#include "wcmd.h"
+#include "rxtx.h"
+#include "card.h"
+#include "usbpipe.h"
+
+/*
+ *
+ * Routine Description:
+ * Enable hw power saving functions
+ *
+ * Return Value:
+ *    None.
+ *
+ */
+
+void vnt_enable_power_saving(struct vnt_private *priv, u16 listen_interval)
+{
+	u16 aid = priv->current_aid | BIT(14) | BIT(15);
+
+	/* set period of power up before TBTT */
+	vnt_mac_write_word(priv, MAC_REG_PWBT, C_PWBT);
+
+	if (priv->op_mode != NL80211_IFTYPE_ADHOC)
+		/* set AID */
+		vnt_mac_write_word(priv, MAC_REG_AIDATIM, aid);
+
+	/* Warren:06-18-2004,the sequence must follow
+	 * PSEN->AUTOSLEEP->GO2DOZE
+	 */
+	/* enable power saving hw function */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_PSEN);
+
+	/* Set AutoSleep */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+
+	/* Warren:MUST turn on this once before turn on AUTOSLEEP ,or the
+	 * AUTOSLEEP doesn't work
+	 */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_GO2DOZE);
+
+	/* always listen beacon */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
+
+	dev_dbg(&priv->usb->dev,  "PS:Power Saving Mode Enable...\n");
+}
+
+int vnt_disable_power_saving(struct vnt_private *priv)
+{
+	int ret;
+
+	/* disable power saving hw function */
+	ret = vnt_control_out(priv, MESSAGE_TYPE_DISABLE_PS, 0,
+			      0, 0, NULL);
+	if (ret)
+		return ret;
+
+	/* clear AutoSleep */
+	vnt_mac_reg_bits_off(priv, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+
+	/* set always listen beacon */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
+
+	return 0;
+}
+
+/*
+ *
+ * Routine Description:
+ * Check if Next TBTT must wake up
+ *
+ * Return Value:
+ *    None.
+ *
+ */
+
+int vnt_next_tbtt_wakeup(struct vnt_private *priv)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_conf *conf = &hw->conf;
+	int wake_up = false;
+
+	if (conf->listen_interval > 1) {
+		/* Turn on wake up to listen next beacon */
+		vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_LNBCN);
+		wake_up = true;
+	}
+
+	return wake_up;
+}
diff --git a/drivers/staging/vt6656/power.h b/drivers/staging/vt6656/power.h
new file mode 100644
index 000000000..9f9c70072
--- /dev/null
+++ b/drivers/staging/vt6656/power.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles 802.11 power management  functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: July 17, 2002
+ *
+ */
+
+#ifndef __POWER_H__
+#define __POWER_H__
+
+#define C_PWBT	1000 /* micro sec. power up before TBTT */
+
+int vnt_disable_power_saving(struct vnt_private *priv);
+void vnt_enable_power_saving(struct vnt_private *priv, u16 listen_interval);
+int vnt_next_tbtt_wakeup(struct vnt_private *priv);
+
+#endif /* __POWER_H__ */
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
new file mode 100644
index 000000000..464602c74
--- /dev/null
+++ b/drivers/staging/vt6656/rf.c
@@ -0,0 +1,443 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: rf function code
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Feb. 19, 2004
+ *
+ * Functions:
+ *	vnt_rf_write_embedded	- Embedded write RF register via MAC
+ *
+ * Revision History:
+ *	RF_VT3226: RobertYu:20051111, VT3226C0 and before
+ *	RF_VT3226D0: RobertYu:20051228
+ *	RF_VT3342A0: RobertYu:20060609
+ *
+ */
+
+#include <linux/errno.h>
+#include "mac.h"
+#include "rf.h"
+#include "baseband.h"
+#include "usbpipe.h"
+
+#define CB_AL2230_INIT_SEQ    15
+#define CB_AL7230_INIT_SEQ    16
+#define CB_VT3226_INIT_SEQ    11
+#define CB_VT3342_INIT_SEQ    13
+
+static u8 al2230_init_table[CB_AL2230_INIT_SEQ][3] = {
+	{0x03, 0xf7, 0x90},
+	{0x03, 0x33, 0x31},
+	{0x01, 0xb8, 0x02},
+	{0x00, 0xff, 0xf3},
+	{0x00, 0x05, 0xa4},
+	{0x0f, 0x4d, 0xc5},
+	{0x08, 0x05, 0xb6},
+	{0x01, 0x47, 0xc7},
+	{0x00, 0x06, 0x88},
+	{0x04, 0x03, 0xb9},
+	{0x00, 0xdb, 0xba},
+	{0x00, 0x09, 0x9b},
+	{0x0b, 0xdf, 0xfc},
+	{0x00, 0x00, 0x0d},
+	{0x00, 0x58, 0x0f}
+};
+
+static u8 al2230_channel_table0[CB_MAX_CHANNEL_24G][3] = {
+	{0x03, 0xf7, 0x90},
+	{0x03, 0xf7, 0x90},
+	{0x03, 0xe7, 0x90},
+	{0x03, 0xe7, 0x90},
+	{0x03, 0xf7, 0xa0},
+	{0x03, 0xf7, 0xa0},
+	{0x03, 0xe7, 0xa0},
+	{0x03, 0xe7, 0xa0},
+	{0x03, 0xf7, 0xb0},
+	{0x03, 0xf7, 0xb0},
+	{0x03, 0xe7, 0xb0},
+	{0x03, 0xe7, 0xb0},
+	{0x03, 0xf7, 0xc0},
+	{0x03, 0xe7, 0xc0}
+};
+
+static u8 al2230_channel_table1[CB_MAX_CHANNEL_24G][3] = {
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x0b, 0x33, 0x31},
+	{0x03, 0x33, 0x31},
+	{0x06, 0x66, 0x61}
+};
+
+static u8 vt3226_init_table[CB_VT3226_INIT_SEQ][3] = {
+	{0x03, 0xff, 0x80},
+	{0x02, 0x82, 0xa1},
+	{0x03, 0xc6, 0xa2},
+	{0x01, 0x97, 0x93},
+	{0x03, 0x66, 0x64},
+	{0x00, 0x61, 0xa5},
+	{0x01, 0x7b, 0xd6},
+	{0x00, 0x80, 0x17},
+	{0x03, 0xf8, 0x08},
+	{0x00, 0x02, 0x39},
+	{0x02, 0x00, 0x2a}
+};
+
+static u8 vt3226d0_init_table[CB_VT3226_INIT_SEQ][3] = {
+	{0x03, 0xff, 0x80},
+	{0x03, 0x02, 0x21},
+	{0x03, 0xc6, 0xa2},
+	{0x01, 0x97, 0x93},
+	{0x03, 0x66, 0x64},
+	{0x00, 0x71, 0xa5},
+	{0x01, 0x15, 0xc6},
+	{0x01, 0x2e, 0x07},
+	{0x00, 0x58, 0x08},
+	{0x00, 0x02, 0x79},
+	{0x02, 0x01, 0xaa}
+};
+
+static u8 vt3226_channel_table0[CB_MAX_CHANNEL_24G][3] = {
+	{0x01, 0x97, 0x83},
+	{0x01, 0x97, 0x83},
+	{0x01, 0x97, 0x93},
+	{0x01, 0x97, 0x93},
+	{0x01, 0x97, 0x93},
+	{0x01, 0x97, 0x93},
+	{0x01, 0x97, 0xa3},
+	{0x01, 0x97, 0xa3},
+	{0x01, 0x97, 0xa3},
+	{0x01, 0x97, 0xa3},
+	{0x01, 0x97, 0xb3},
+	{0x01, 0x97, 0xb3},
+	{0x01, 0x97, 0xb3},
+	{0x03, 0x37, 0xc3}
+};
+
+static u8 vt3226_channel_table1[CB_MAX_CHANNEL_24G][3] = {
+	{0x02, 0x66, 0x64},
+	{0x03, 0x66, 0x64},
+	{0x00, 0x66, 0x64},
+	{0x01, 0x66, 0x64},
+	{0x02, 0x66, 0x64},
+	{0x03, 0x66, 0x64},
+	{0x00, 0x66, 0x64},
+	{0x01, 0x66, 0x64},
+	{0x02, 0x66, 0x64},
+	{0x03, 0x66, 0x64},
+	{0x00, 0x66, 0x64},
+	{0x01, 0x66, 0x64},
+	{0x02, 0x66, 0x64},
+	{0x00, 0xcc, 0xc4}
+};
+
+static const u32 vt3226d0_lo_current_table[CB_MAX_CHANNEL_24G] = {
+	0x0135c600,
+	0x0135c600,
+	0x0235c600,
+	0x0235c600,
+	0x0235c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0335c600,
+	0x0135c600
+};
+
+enum {
+	VNT_TABLE_INIT = 0,
+	VNT_TABLE_INIT_2 = 0,
+	VNT_TABLE_0 = 1,
+	VNT_TABLE_1 = 2,
+	VNT_TABLE_2 = 1
+};
+
+struct vnt_table_info {
+	u8 *addr;
+	int length;
+};
+
+static const struct vnt_table_info vnt_table_seq[][3] = {
+	{	/* RF_AL2230, RF_AL2230S init table, channel table 0 and 1 */
+		{&al2230_init_table[0][0], CB_AL2230_INIT_SEQ * 3},
+		{&al2230_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&al2230_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}, {	/* RF_VT3226 init table, channel table 0 and 1 */
+		{&vt3226_init_table[0][0], CB_VT3226_INIT_SEQ * 3},
+		{&vt3226_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&vt3226_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}, {	/* RF_VT3226D0 init table, channel table 0 and 1 */
+		{&vt3226d0_init_table[0][0], CB_VT3226_INIT_SEQ * 3},
+		{&vt3226_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&vt3226_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}
+};
+
+/*
+ * Description: Write to IF/RF, by embedded programming
+ */
+int vnt_rf_write_embedded(struct vnt_private *priv, u32 data)
+{
+	u8 reg_data[4];
+
+	data |= (VNT_RF_REG_LEN << 3) | IFREGCTL_REGW;
+
+	reg_data[0] = (u8)data;
+	reg_data[1] = (u8)(data >> 8);
+	reg_data[2] = (u8)(data >> 16);
+	reg_data[3] = (u8)(data >> 24);
+
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_IFRF, 0, 0,
+			       ARRAY_SIZE(reg_data), reg_data);
+}
+
+static u8 vnt_rf_addpower(struct vnt_private *priv)
+{
+	int base;
+	s32 rssi = -priv->current_rssi;
+
+	if (!rssi)
+		return 7;
+
+	if (priv->rf_type == RF_VT3226D0)
+		base = -60;
+	else
+		base = -70;
+
+	if (rssi < base)
+		return ((rssi - base + 1) / -5) * 2 + 5;
+
+	return 0;
+}
+
+/* Set Tx power by power level and rate */
+static int vnt_rf_set_txpower(struct vnt_private *priv, u8 power,
+			      struct ieee80211_channel *ch)
+{
+	u32 power_setting = 0;
+	int ret = 0;
+
+	power += vnt_rf_addpower(priv);
+	if (power > VNT_RF_MAX_POWER)
+		power = VNT_RF_MAX_POWER;
+
+	if (priv->power == power)
+		return 0;
+
+	priv->power = power;
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+		power_setting = 0x0404090 | (power << 12);
+
+		ret = vnt_rf_write_embedded(priv, power_setting);
+		if (ret)
+			return ret;
+
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM)
+			ret = vnt_rf_write_embedded(priv, 0x0001b400);
+		else
+			ret = vnt_rf_write_embedded(priv, 0x0005a400);
+
+		break;
+	case RF_AL2230S:
+		power_setting = 0x0404090 | (power << 12);
+
+		ret = vnt_rf_write_embedded(priv, power_setting);
+		if (ret)
+			return ret;
+
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+			ret = vnt_rf_write_embedded(priv, 0x040c1400);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00299b00);
+		} else {
+			ret = vnt_rf_write_embedded(priv, 0x0005a400);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00099b00);
+		}
+
+		break;
+
+	case RF_VT3226:
+		power_setting = ((0x3f - power) << 20) | (0x17 << 8);
+
+		ret = vnt_rf_write_embedded(priv, power_setting);
+		break;
+	case RF_VT3226D0:
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+			u16 hw_value = ch->hw_value;
+
+			power_setting = ((0x3f - power) << 20) | (0xe07 << 8);
+
+			ret = vnt_rf_write_embedded(priv, power_setting);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x03c6a200);
+			if (ret)
+				return ret;
+
+			dev_dbg(&priv->usb->dev,
+				"%s 11b channel [%d]\n", __func__, hw_value);
+
+			hw_value--;
+
+			if (hw_value < ARRAY_SIZE(vt3226d0_lo_current_table)) {
+				ret = vnt_rf_write_embedded(priv,
+							    vt3226d0_lo_current_table[hw_value]);
+				if (ret)
+					return ret;
+			}
+
+			ret = vnt_rf_write_embedded(priv, 0x015C0800);
+		} else {
+			dev_dbg(&priv->usb->dev,
+				"@@@@ %s> 11G mode\n", __func__);
+
+			power_setting = ((0x3f - power) << 20) | (0x7 << 8);
+
+			ret = vnt_rf_write_embedded(priv, power_setting);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00C6A200);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x016BC600);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00900800);
+		}
+
+		break;
+
+	default:
+		break;
+	}
+	return ret;
+}
+
+/* Set Tx power by channel number type */
+int vnt_rf_setpower(struct vnt_private *priv,
+		    struct ieee80211_channel *ch)
+{
+	u16 channel;
+	u8 power = priv->cck_pwr;
+
+	if (!ch)
+		return -EINVAL;
+
+	/* set channel number to array number */
+	channel = ch->hw_value - 1;
+
+	if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+		if (channel < ARRAY_SIZE(priv->cck_pwr_tbl))
+			power = priv->cck_pwr_tbl[channel];
+	} else if (ch->band == NL80211_BAND_5GHZ) {
+		/* remove 14 channels to array size */
+		channel -= 14;
+
+		if (channel < ARRAY_SIZE(priv->ofdm_a_pwr_tbl))
+			power = priv->ofdm_a_pwr_tbl[channel];
+	} else {
+		if (channel < ARRAY_SIZE(priv->ofdm_pwr_tbl))
+			power = priv->ofdm_pwr_tbl[channel];
+	}
+
+	return vnt_rf_set_txpower(priv, power, ch);
+}
+
+/* Convert rssi to dbm */
+void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm)
+{
+	u8 idx = ((rssi & 0xc0) >> 6) & 0x03;
+	long b = rssi & 0x3f;
+	long a = 0;
+	u8 airoharf[4] = {0, 18, 0, 40};
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_VT3226:
+	case RF_VT3226D0:
+		a = airoharf[idx];
+		break;
+	default:
+		break;
+	}
+
+	*dbm = -1 * (a + b * 2);
+}
+
+int vnt_rf_table_download(struct vnt_private *priv)
+{
+	int ret;
+	int idx = -1;
+	const struct vnt_table_info *table_seq;
+
+	switch (priv->rf_type) {
+	case RF_AL2230:
+	case RF_AL2230S:
+		idx = 0;
+		break;
+	case RF_VT3226:
+		idx = 1;
+		break;
+	case RF_VT3226D0:
+		idx = 2;
+		break;
+	}
+
+	if (idx < 0)
+		return 0;
+
+	table_seq = &vnt_table_seq[idx][0];
+
+	/* Init Table */
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+			      MESSAGE_REQUEST_RF_INIT,
+			      table_seq[VNT_TABLE_INIT].length,
+			      table_seq[VNT_TABLE_INIT].addr);
+	if (ret)
+		return ret;
+
+	/* Channel Table 0 */
+	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+				     MESSAGE_REQUEST_RF_CH0,
+				     table_seq[VNT_TABLE_0].length,
+				     table_seq[VNT_TABLE_0].addr);
+	if (ret)
+		return ret;
+
+	/* Channel Table 1 */
+	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+				     MESSAGE_REQUEST_RF_CH1,
+				     table_seq[VNT_TABLE_1].length,
+				     table_seq[VNT_TABLE_1].addr);
+
+	return ret;
+}
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
new file mode 100644
index 000000000..b47e14987
--- /dev/null
+++ b/drivers/staging/vt6656/rf.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Feb. 19, 2004
+ *
+ */
+
+#ifndef __RF_H__
+#define __RF_H__
+
+#include "device.h"
+
+/* Baseband RF pair definition in eeprom (Bits 6..0) */
+#define RF_RFMD2959         0x01
+#define RF_MAXIMAG          0x02
+#define RF_AL2230           0x03
+#define RF_GCT5103          0x04
+#define RF_UW2451           0x05
+#define RF_MAXIMG           0x06
+#define RF_MAXIM2829        0x07
+#define RF_UW2452           0x08
+#define RF_VT3226           0x09
+#define RF_AIROHA7230       0x0a
+#define RF_UW2453           0x0b
+#define RF_VT3226D0         0x0c /* RobertYu:20051114 */
+#define RF_VT3342A0         0x0d /* RobertYu:20060609 */
+#define RF_AL2230S          0x0e
+
+#define RF_EMU              0x80
+#define RF_MASK             0x7F
+
+#define VNT_RF_MAX_POWER    0x3f
+#define	VNT_RF_REG_LEN      0x17 /* 24 bit length */
+
+int vnt_rf_write_embedded(struct vnt_private *priv, u32 data);
+int vnt_rf_setpower(struct vnt_private *priv, struct ieee80211_channel *ch);
+void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm);
+int vnt_rf_table_download(struct vnt_private *priv);
+
+#endif /* __RF_H__ */
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
new file mode 100644
index 000000000..cd99091c6
--- /dev/null
+++ b/drivers/staging/vt6656/rxtx.c
@@ -0,0 +1,730 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: handle WMAC/802.3/802.11 rx & tx functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: May 20, 2003
+ *
+ * Functions:
+ *      vnt_generate_tx_parameter - Generate tx dma required parameter.
+ *      vnt_get_rsvtime- get frame reserved time
+ *      vnt_fill_cts_head- fulfill CTS ctl header
+ *
+ * Revision History:
+ *
+ */
+
+#include <linux/etherdevice.h>
+#include "device.h"
+#include "rxtx.h"
+#include "card.h"
+#include "mac.h"
+#include "rf.h"
+#include "usbpipe.h"
+
+static const u16 vnt_time_stampoff[2][MAX_RATE] = {
+	/* Long Preamble */
+	{384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23},
+
+	/* Short Preamble */
+	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23},
+};
+
+#define DATADUR_B       10
+#define DATADUR_A       11
+
+static const u8 vnt_phy_signal[] = {
+	0x00,	/* RATE_1M  */
+	0x01,	/* RATE_2M  */
+	0x02,	/* RATE_5M  */
+	0x03,	/* RATE_11M */
+	0x8b,	/* RATE_6M  */
+	0x8f,	/* RATE_9M  */
+	0x8a,	/* RATE_12M */
+	0x8e,	/* RATE_18M */
+	0x89,	/* RATE_24M */
+	0x8d,	/* RATE_36M */
+	0x88,	/* RATE_48M */
+	0x8c	/* RATE_54M */
+};
+
+static struct vnt_usb_send_context
+	*vnt_get_free_context(struct vnt_private *priv)
+{
+	struct vnt_usb_send_context *context = NULL;
+	int ii;
+
+	for (ii = 0; ii < priv->num_tx_context; ii++) {
+		if (!priv->tx_context[ii])
+			return NULL;
+
+		context = priv->tx_context[ii];
+		if (!context->in_use) {
+			context->in_use = true;
+			return context;
+		}
+	}
+
+	if (ii == priv->num_tx_context) {
+		dev_dbg(&priv->usb->dev, "%s No Free Tx Context\n", __func__);
+
+		ieee80211_stop_queues(priv->hw);
+	}
+
+	return NULL;
+}
+
+/* Get Length, Service, and Signal fields of Phy for Tx */
+static void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+			      u16 tx_rate, u8 pkt_type,
+			      struct vnt_phy_field *phy)
+{
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	int i;
+	u8 mask = 0;
+	u8 preamble_type = priv->preamble_type;
+
+	bit_count = frame_length * 8;
+	ext_bit = false;
+
+	switch (tx_rate) {
+	case RATE_1M:
+		count = bit_count;
+		break;
+	case RATE_2M:
+		count = bit_count / 2;
+		break;
+	case RATE_5M:
+		count = DIV_ROUND_UP(bit_count * 10, 55);
+		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
+
+		if (tmp != bit_count) {
+			count++;
+
+			if ((bit_count - tmp) <= 3)
+				ext_bit = true;
+		}
+
+		break;
+	}
+
+	if (tx_rate > RATE_11M) {
+		if (pkt_type == PK_TYPE_11A)
+			mask = BIT(4);
+	} else if (tx_rate > RATE_1M) {
+		if (preamble_type == PREAMBLE_SHORT)
+			mask = BIT(3);
+	}
+
+	i = tx_rate > RATE_54M ? RATE_54M : tx_rate;
+	phy->signal = vnt_phy_signal[i] | mask;
+	phy->service = 0x00;
+
+	if (pkt_type == PK_TYPE_11B) {
+		if (ext_bit)
+			phy->service |= 0x80;
+		phy->len = cpu_to_le16((u16)count);
+	} else {
+		phy->len = cpu_to_le16((u16)frame_length);
+	}
+}
+
+static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
+{
+	return cpu_to_le16(vnt_time_stampoff[priv->preamble_type % 2]
+							[rate % MAX_RATE]);
+}
+
+static __le16 vnt_rxtx_rsvtime_le16(struct vnt_usb_send_context *context)
+{
+	struct vnt_private *priv = context->priv;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
+	struct ieee80211_rate *rate = ieee80211_get_tx_rate(priv->hw, info);
+
+	return ieee80211_generic_frame_duration(priv->hw,
+						 info->control.vif, info->band,
+						 context->frame_len,
+						 rate);
+}
+
+static __le16 vnt_get_rts_duration(struct vnt_usb_send_context *context)
+{
+	struct vnt_private *priv = context->priv;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
+
+	return ieee80211_rts_duration(priv->hw, priv->vif,
+				      context->frame_len, info);
+}
+
+static __le16 vnt_get_cts_duration(struct vnt_usb_send_context *context)
+{
+	struct vnt_private *priv = context->priv;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
+
+	return ieee80211_ctstoself_duration(priv->hw, priv->vif,
+					    context->frame_len, info);
+}
+
+static void vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
+				struct vnt_tx_datahead_g *buf)
+{
+	struct vnt_private *priv = tx_context->priv;
+	struct ieee80211_hdr *hdr =
+				(struct ieee80211_hdr *)tx_context->skb->data;
+	u32 frame_len = tx_context->frame_len;
+	u16 rate = tx_context->tx_rate;
+
+	/* Get SignalField,ServiceField,Length */
+	vnt_get_phy_field(priv, frame_len, rate, tx_context->pkt_type, &buf->a);
+	vnt_get_phy_field(priv, frame_len, priv->top_cck_basic_rate,
+			  PK_TYPE_11B, &buf->b);
+
+	/* Get Duration and TimeStamp */
+	buf->duration_a = hdr->duration_id;
+	buf->duration_b = hdr->duration_id;
+	buf->time_stamp_off_a = vnt_time_stamp_off(priv, rate);
+	buf->time_stamp_off_b = vnt_time_stamp_off(priv,
+						   priv->top_cck_basic_rate);
+}
+
+static void vnt_rxtx_datahead_ab(struct vnt_usb_send_context *tx_context,
+				 struct vnt_tx_datahead_ab *buf)
+{
+	struct vnt_private *priv = tx_context->priv;
+	struct ieee80211_hdr *hdr =
+				(struct ieee80211_hdr *)tx_context->skb->data;
+	u32 frame_len = tx_context->frame_len;
+	u16 rate = tx_context->tx_rate;
+
+	/* Get SignalField,ServiceField,Length */
+	vnt_get_phy_field(priv, frame_len, rate,
+			  tx_context->pkt_type, &buf->ab);
+
+	/* Get Duration and TimeStampOff */
+	buf->duration = hdr->duration_id;
+	buf->time_stamp_off = vnt_time_stamp_off(priv, rate);
+}
+
+static void vnt_fill_ieee80211_rts(struct vnt_usb_send_context *tx_context,
+				   struct ieee80211_rts *rts, __le16 duration)
+{
+	struct ieee80211_hdr *hdr =
+				(struct ieee80211_hdr *)tx_context->skb->data;
+
+	rts->duration = duration;
+	rts->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
+
+	ether_addr_copy(rts->ra, hdr->addr1);
+	ether_addr_copy(rts->ta, hdr->addr2);
+}
+
+static void vnt_rxtx_rts_g_head(struct vnt_usb_send_context *tx_context,
+				struct vnt_rts_g *buf)
+{
+	struct vnt_private *priv = tx_context->priv;
+	u16 rts_frame_len = 20;
+
+	vnt_get_phy_field(priv, rts_frame_len, priv->top_cck_basic_rate,
+			  PK_TYPE_11B, &buf->b);
+	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
+			  tx_context->pkt_type, &buf->a);
+
+	buf->duration_bb = vnt_get_rts_duration(tx_context);
+	buf->duration_aa = buf->duration_bb;
+	buf->duration_ba = buf->duration_bb;
+
+	vnt_fill_ieee80211_rts(tx_context, &buf->data, buf->duration_aa);
+
+	vnt_rxtx_datahead_g(tx_context, &buf->data_head);
+}
+
+static void vnt_rxtx_rts_ab_head(struct vnt_usb_send_context *tx_context,
+				 struct vnt_rts_ab *buf)
+{
+	struct vnt_private *priv = tx_context->priv;
+	u16 rts_frame_len = 20;
+
+	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
+			  tx_context->pkt_type, &buf->ab);
+
+	buf->duration = vnt_get_rts_duration(tx_context);
+
+	vnt_fill_ieee80211_rts(tx_context, &buf->data, buf->duration);
+
+	vnt_rxtx_datahead_ab(tx_context, &buf->data_head);
+}
+
+static void vnt_fill_cts_head(struct vnt_usb_send_context *tx_context,
+			      union vnt_tx_data_head *head)
+{
+	struct vnt_private *priv = tx_context->priv;
+	struct vnt_cts *buf = &head->cts_g;
+	u32 cts_frame_len = 14;
+
+	/* Get SignalField,ServiceField,Length */
+	vnt_get_phy_field(priv, cts_frame_len, priv->top_cck_basic_rate,
+			  PK_TYPE_11B, &buf->b);
+	/* Get CTSDuration_ba */
+	buf->duration_ba = vnt_get_cts_duration(tx_context);
+	/*Get CTS Frame body*/
+	buf->data.duration = buf->duration_ba;
+	buf->data.frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+
+	ether_addr_copy(buf->data.ra, priv->current_net_addr);
+
+	vnt_rxtx_datahead_g(tx_context, &buf->data_head);
+}
+
+/* returns true if mic_hdr is needed */
+static bool vnt_fill_txkey(struct vnt_tx_buffer *tx_buffer, struct sk_buff *skb)
+{
+	struct vnt_tx_fifo_head *fifo = &tx_buffer->fifo_head;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_key_conf *tx_key = info->control.hw_key;
+	struct vnt_mic_hdr *mic_hdr;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	u64 pn64;
+	u16 payload_len = skb->len;
+	u8 *iv = ((u8 *)hdr + ieee80211_get_hdrlen_from_skb(skb));
+
+	/* strip header and icv len from payload */
+	payload_len -= ieee80211_get_hdrlen_from_skb(skb);
+	payload_len -= tx_key->icv_len;
+
+	switch (tx_key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		memcpy(fifo->tx_key, iv, 3);
+		memcpy(fifo->tx_key + 3, tx_key->key, tx_key->keylen);
+
+		if (tx_key->keylen == WLAN_KEY_LEN_WEP40) {
+			memcpy(fifo->tx_key + 8, iv, 3);
+			memcpy(fifo->tx_key + 11,
+			       tx_key->key, WLAN_KEY_LEN_WEP40);
+		}
+
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_LEGACY);
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		ieee80211_get_tkip_p2k(tx_key, skb, fifo->tx_key);
+
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_TKIP);
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		if (info->control.use_cts_prot) {
+			if (info->control.use_rts)
+				mic_hdr = &tx_buffer->tx_head.tx_rts.tx.mic.hdr;
+			else
+				mic_hdr = &tx_buffer->tx_head.tx_cts.tx.mic.hdr;
+		} else {
+			mic_hdr = &tx_buffer->tx_head.tx_ab.tx.mic.hdr;
+		}
+
+		mic_hdr->id = 0x59;
+		mic_hdr->payload_len = cpu_to_be16(payload_len);
+		ether_addr_copy(mic_hdr->mic_addr2, hdr->addr2);
+
+		pn64 = atomic64_read(&tx_key->tx_pn);
+		mic_hdr->ccmp_pn[5] = pn64;
+		mic_hdr->ccmp_pn[4] = pn64 >> 8;
+		mic_hdr->ccmp_pn[3] = pn64 >> 16;
+		mic_hdr->ccmp_pn[2] = pn64 >> 24;
+		mic_hdr->ccmp_pn[1] = pn64 >> 32;
+		mic_hdr->ccmp_pn[0] = pn64 >> 40;
+
+		if (ieee80211_has_a4(hdr->frame_control))
+			mic_hdr->hlen = cpu_to_be16(28);
+		else
+			mic_hdr->hlen = cpu_to_be16(22);
+
+		ether_addr_copy(mic_hdr->addr1, hdr->addr1);
+		ether_addr_copy(mic_hdr->addr2, hdr->addr2);
+		ether_addr_copy(mic_hdr->addr3, hdr->addr3);
+
+		mic_hdr->frame_control = cpu_to_le16(le16_to_cpu(hdr->frame_control) & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(le16_to_cpu(hdr->seq_ctrl) & 0xf);
+
+		if (ieee80211_has_a4(hdr->frame_control))
+			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
+
+		memcpy(fifo->tx_key, tx_key->key, WLAN_KEY_LEN_CCMP);
+
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_AES);
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static void vnt_rxtx_rts(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_rts *buf = &tx_head->tx_rts.rts;
+	union vnt_tx_data_head *head = &tx_head->tx_rts.tx.head;
+
+	buf->rts_rrv_time_aa = vnt_get_rts_duration(tx_context);
+	buf->rts_rrv_time_ba = buf->rts_rrv_time_aa;
+	buf->rts_rrv_time_bb = buf->rts_rrv_time_aa;
+
+	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(tx_context);
+	buf->rrv_time_b = buf->rrv_time_a;
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_rts.tx.mic.head;
+	}
+
+	vnt_rxtx_rts_g_head(tx_context, &head->rts_g);
+}
+
+static void vnt_rxtx_cts(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_cts *buf = &tx_head->tx_cts.cts;
+	union vnt_tx_data_head *head = &tx_head->tx_cts.tx.head;
+
+	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(tx_context);
+	buf->rrv_time_b = buf->rrv_time_a;
+
+	buf->cts_rrv_time_ba = vnt_get_cts_duration(tx_context);
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_cts.tx.mic.head;
+	}
+
+	vnt_fill_cts_head(tx_context, head);
+}
+
+static void vnt_rxtx_ab(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_ab *buf = &tx_head->tx_ab.ab;
+	union vnt_tx_data_head *head = &tx_head->tx_ab.tx.head;
+
+	buf->rrv_time = vnt_rxtx_rsvtime_le16(tx_context);
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_ab.tx.mic.head;
+	}
+
+	if (info->control.use_rts) {
+		buf->rts_rrv_time = vnt_get_rts_duration(tx_context);
+
+		vnt_rxtx_rts_ab_head(tx_context, &head->rts_ab);
+
+		return;
+	}
+
+	vnt_rxtx_datahead_ab(tx_context, &head->data_head_ab);
+}
+
+static void vnt_generate_tx_parameter(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts) {
+			vnt_rxtx_rts(tx_context);
+
+			return;
+		}
+
+		vnt_rxtx_cts(tx_context);
+
+		return;
+	}
+
+	vnt_rxtx_ab(tx_context);
+}
+
+static u16 vnt_get_hdr_size(struct ieee80211_tx_info *info)
+{
+	u16 size = sizeof(struct vnt_tx_datahead_ab);
+
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts)
+			size = sizeof(struct vnt_rts_g);
+		else
+			size = sizeof(struct vnt_cts);
+	} else if (info->control.use_rts) {
+		size = sizeof(struct vnt_rts_ab);
+	}
+
+	if (info->control.hw_key) {
+		if (info->control.hw_key->cipher == WLAN_CIPHER_SUITE_CCMP)
+			size += sizeof(struct vnt_mic_hdr);
+	}
+
+	/* Get rrv_time header */
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts)
+			size += sizeof(struct vnt_rrv_time_rts);
+		else
+			size += sizeof(struct vnt_rrv_time_cts);
+	} else {
+		size += sizeof(struct vnt_rrv_time_ab);
+	}
+
+	size += sizeof(struct vnt_tx_fifo_head);
+
+	return size;
+}
+
+int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *tx_rate = &info->control.rates[0];
+	struct ieee80211_rate *rate;
+	struct ieee80211_hdr *hdr;
+	struct vnt_tx_buffer *tx_buffer;
+	struct vnt_tx_fifo_head *tx_buffer_head;
+	struct vnt_usb_send_context *tx_context;
+	unsigned long flags;
+	u8 pkt_type;
+
+	hdr = (struct ieee80211_hdr *)(skb->data);
+
+	rate = ieee80211_get_tx_rate(priv->hw, info);
+
+	if (rate->hw_value > RATE_11M) {
+		if (info->band == NL80211_BAND_5GHZ) {
+			pkt_type = PK_TYPE_11A;
+		} else {
+			if (tx_rate->flags & IEEE80211_TX_RC_USE_CTS_PROTECT) {
+				if (priv->basic_rates & VNT_B_RATES)
+					pkt_type = PK_TYPE_11GB;
+				else
+					pkt_type = PK_TYPE_11GA;
+			} else {
+				pkt_type = PK_TYPE_11A;
+			}
+		}
+	} else {
+		pkt_type = PK_TYPE_11B;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	tx_context = vnt_get_free_context(priv);
+	if (!tx_context) {
+		dev_dbg(&priv->usb->dev, "%s No free context\n", __func__);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return -ENOMEM;
+	}
+
+	tx_context->pkt_type = pkt_type;
+	tx_context->frame_len = skb->len + 4;
+	tx_context->tx_rate =  rate->hw_value;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	tx_context->skb = skb_clone(skb, GFP_ATOMIC);
+	if (!tx_context->skb) {
+		tx_context->in_use = false;
+		return -ENOMEM;
+	}
+
+	tx_buffer = skb_push(skb, vnt_get_hdr_size(info));
+	tx_context->tx_buffer = tx_buffer;
+	tx_buffer_head = &tx_buffer->fifo_head;
+
+	tx_context->type = CONTEXT_DATA_PACKET;
+
+	/*Set fifo controls */
+	if (pkt_type == PK_TYPE_11A)
+		tx_buffer_head->fifo_ctl = 0;
+	else if (pkt_type == PK_TYPE_11B)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11B);
+	else if (pkt_type == PK_TYPE_11GB)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GB);
+	else if (pkt_type == PK_TYPE_11GA)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GA);
+
+	if (!ieee80211_is_data(hdr->frame_control)) {
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT |
+							FIFOCTL_ISDMA0);
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_TMOEN);
+
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+	} else {
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
+	}
+
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_NEEDACK);
+
+	if (ieee80211_has_retry(hdr->frame_control))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LRETRY);
+
+	if (info->control.use_rts)
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_RTS);
+
+	if (ieee80211_has_a4(hdr->frame_control))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LHEAD);
+
+	tx_buffer_head->frag_ctl =
+			cpu_to_le16(ieee80211_hdrlen(hdr->frame_control) << 10);
+
+	if (info->control.hw_key)
+		tx_context->frame_len += info->control.hw_key->icv_len;
+
+	tx_buffer_head->current_rate = cpu_to_le16(rate->hw_value);
+
+	vnt_generate_tx_parameter(tx_context);
+
+	tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_NONFRAG);
+
+	priv->seq_counter = (le16_to_cpu(hdr->seq_ctrl) &
+						IEEE80211_SCTL_SEQ) >> 4;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (vnt_tx_context(priv, tx_context, skb)) {
+		dev_kfree_skb(tx_context->skb);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return -EIO;
+	}
+
+	dev_kfree_skb(skb);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
+{
+	struct vnt_tx_short_buf_head *short_head;
+	struct ieee80211_tx_info *info;
+	struct vnt_usb_send_context *context;
+	struct ieee80211_mgmt *mgmt_hdr;
+	unsigned long flags;
+	u32 frame_size = skb->len + 4;
+	u16 current_rate;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	context = vnt_get_free_context(priv);
+	if (!context) {
+		dev_dbg(&priv->usb->dev, "%s No free context!\n", __func__);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return -ENOMEM;
+	}
+
+	context->skb = skb;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	mgmt_hdr = (struct ieee80211_mgmt *)skb->data;
+	short_head = skb_push(skb, sizeof(*short_head));
+
+	if (priv->bb_type == BB_TYPE_11A) {
+		current_rate = RATE_6M;
+
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11A, &short_head->ab);
+
+		/* Get TimeStampOff */
+		short_head->time_stamp_off =
+				vnt_time_stamp_off(priv, current_rate);
+	} else {
+		current_rate = RATE_1M;
+		short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_11B);
+
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11B, &short_head->ab);
+
+		/* Get TimeStampOff */
+		short_head->time_stamp_off =
+			vnt_time_stamp_off(priv, current_rate);
+	}
+
+	/* Get Duration */
+	short_head->duration = mgmt_hdr->duration;
+
+	/* time stamp always 0 */
+	mgmt_hdr->u.beacon.timestamp = 0;
+
+	info = IEEE80211_SKB_CB(skb);
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mgmt_hdr;
+
+		hdr->duration_id = 0;
+		hdr->seq_ctrl = cpu_to_le16(priv->seq_counter << 4);
+	}
+
+	priv->seq_counter++;
+	if (priv->seq_counter > 0x0fff)
+		priv->seq_counter = 0;
+
+	context->type = CONTEXT_BEACON_PACKET;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (vnt_tx_context(priv, context, skb))
+		ieee80211_free_txskb(priv->hw, context->skb);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
+{
+	struct sk_buff *beacon;
+
+	beacon = ieee80211_beacon_get(priv->hw, vif, 0);
+	if (!beacon)
+		return -ENOMEM;
+
+	if (vnt_beacon_xmit(priv, beacon)) {
+		ieee80211_free_txskb(priv->hw, beacon);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
+		      struct ieee80211_bss_conf *conf)
+{
+	vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+
+	vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+
+	vnt_mac_set_beacon_interval(priv, conf->beacon_int);
+
+	vnt_clear_current_tsf(priv);
+
+	vnt_mac_reg_bits_on(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+
+	vnt_reset_next_tbtt(priv, conf->beacon_int);
+
+	return vnt_beacon_make(priv, vif);
+}
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
new file mode 100644
index 000000000..b9df0854b
--- /dev/null
+++ b/drivers/staging/vt6656/rxtx.h
@@ -0,0 +1,178 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Jerry Chen
+ *
+ * Date: Jun. 27, 2002
+ *
+ */
+
+#ifndef __RXTX_H__
+#define __RXTX_H__
+
+#include "device.h"
+#include "wcmd.h"
+#include "baseband.h"
+
+#define DEFAULT_MGN_LIFETIME_RES_64us	125  /* 64us */
+#define DEFAULT_MSDU_LIFETIME_RES_64us  8000
+
+/* Length, Service, and Signal fields of Phy for Tx */
+struct vnt_phy_field {
+	u8 signal;
+	u8 service;
+	__le16 len;
+} __packed;
+
+/* MIC HDR data header */
+struct vnt_mic_hdr {
+	u8 id;
+	u8 tx_priority;
+	u8 mic_addr2[6];
+	u8 ccmp_pn[IEEE80211_CCMP_PN_LEN];
+	__be16 payload_len;
+	__be16 hlen;
+	__le16 frame_control;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	__le16 seq_ctrl;
+	u8 addr4[6];
+	u16 packing; /* packing to 48 bytes */
+} __packed;
+
+/* RsvTime buffer header */
+struct vnt_rrv_time_rts {
+	__le16 rts_rrv_time_ba;
+	__le16 rts_rrv_time_aa;
+	__le16 rts_rrv_time_bb;
+	u16 wReserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_cts {
+	__le16 cts_rrv_time_ba;
+	u16 wReserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_ab {
+	__le16 rts_rrv_time;
+	__le16 rrv_time;
+} __packed;
+
+/* TX data header */
+struct vnt_tx_datahead_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_b;
+	__le16 duration_a;
+	__le16 time_stamp_off_b;
+	__le16 time_stamp_off_a;
+} __packed;
+
+struct vnt_tx_datahead_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
+/* RTS buffer header */
+struct vnt_rts_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_ba;
+	__le16 duration_aa;
+	__le16 duration_bb;
+	u16 wReserved;
+	struct ieee80211_rts data;
+	struct vnt_tx_datahead_g data_head;
+} __packed __aligned(2);
+
+struct vnt_rts_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	u16 wReserved;
+	struct ieee80211_rts data;
+	struct vnt_tx_datahead_ab data_head;
+} __packed __aligned(2);
+
+/* CTS buffer header */
+struct vnt_cts {
+	struct vnt_phy_field b;
+	__le16 duration_ba;
+	u16 wReserved;
+	struct ieee80211_cts data;
+	u16 reserved2;
+	struct vnt_tx_datahead_g data_head;
+} __packed __aligned(2);
+
+union vnt_tx_data_head {
+	/* rts g */
+	struct vnt_rts_g rts_g;
+	/* rts a/b */
+	struct vnt_rts_ab rts_ab;
+	/* cts g */
+	struct vnt_cts cts_g;
+	/* no rts/cts */
+	struct vnt_tx_datahead_ab data_head_ab;
+};
+
+struct vnt_tx_mic_hdr {
+	struct vnt_mic_hdr hdr;
+	union vnt_tx_data_head head;
+} __packed;
+
+union vnt_tx {
+	struct vnt_tx_mic_hdr mic;
+	union vnt_tx_data_head head;
+};
+
+union vnt_tx_head {
+	struct {
+		struct vnt_rrv_time_rts rts;
+		union vnt_tx tx;
+	} __packed tx_rts;
+	struct {
+		struct vnt_rrv_time_cts cts;
+		union vnt_tx tx;
+	} __packed tx_cts;
+	struct {
+		struct vnt_rrv_time_ab ab;
+		union vnt_tx tx;
+	} __packed tx_ab;
+};
+
+struct vnt_tx_fifo_head {
+	u8 tx_key[WLAN_KEY_LEN_CCMP];
+	__le16 fifo_ctl;
+	__le16 time_stamp;
+	__le16 frag_ctl;
+	__le16 current_rate;
+} __packed;
+
+struct vnt_tx_buffer {
+	struct vnt_tx_fifo_head fifo_head;
+	union vnt_tx_head tx_head;
+} __packed;
+
+struct vnt_tx_short_buf_head {
+	__le16 fifo_ctl;
+	u16 time_stamp;
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
+int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb);
+int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif);
+int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
+		      struct ieee80211_bss_conf *conf);
+
+#endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
new file mode 100644
index 000000000..d505b4b69
--- /dev/null
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -0,0 +1,506 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handle USB control endpoint
+ *
+ * Author: Warren Hsu
+ *
+ * Date: Mar. 29, 2005
+ *
+ * Functions:
+ *	vnt_control_out - Write variable length bytes to MEM/BB/MAC/EEPROM
+ *	vnt_control_in - Read variable length bytes from MEM/BB/MAC/EEPROM
+ *	vnt_control_out_u8 - Write one byte to MEM/BB/MAC/EEPROM
+ *	vnt_control_in_u8 - Read one byte from MEM/BB/MAC/EEPROM
+ *
+ * Revision History:
+ *      04-05-2004 Jerry Chen: Initial release
+ *      11-24-2004 Warren Hsu: Add ControlvWriteByte,ControlvReadByte,
+ *                             ControlvMaskByte
+ *
+ */
+
+#include "rxtx.h"
+#include "desc.h"
+#include "device.h"
+#include "usbpipe.h"
+#include "mac.h"
+#include "rf.h"
+
+#define USB_CTL_WAIT	500 /* ms */
+
+int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
+		    u16 index, u16 length, const u8 *buffer)
+{
+	int ret = 0;
+	u8 *usb_buffer;
+
+	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags)) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	mutex_lock(&priv->usb_lock);
+
+	usb_buffer = kmemdup(buffer, length, GFP_KERNEL);
+	if (!usb_buffer) {
+		ret = -ENOMEM;
+		goto end_unlock;
+	}
+
+	ret = usb_control_msg(priv->usb,
+			      usb_sndctrlpipe(priv->usb, 0),
+			      request, 0x40, value,
+			      index, usb_buffer, length, USB_CTL_WAIT);
+
+	kfree(usb_buffer);
+
+	if (ret == (int)length)
+		ret = 0;
+	else
+		ret = -EIO;
+
+end_unlock:
+	mutex_unlock(&priv->usb_lock);
+end:
+	return ret;
+}
+
+int vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 data)
+{
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+			       reg_off, reg, sizeof(u8), &data);
+}
+
+int vnt_control_out_blocks(struct vnt_private *priv,
+			   u16 block, u8 reg, u16 length, const u8 *data)
+{
+	int ret = 0, i;
+
+	for (i = 0; i < length; i += block) {
+		u16 len = min_t(int, length - i, block);
+
+		ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+				      i, reg, len, data + i);
+		if (ret)
+			goto end;
+	}
+end:
+	return ret;
+}
+
+int vnt_control_in(struct vnt_private *priv, u8 request, u16 value,
+		   u16 index, u16 length, u8 *buffer)
+{
+	int ret = 0;
+	u8 *usb_buffer;
+
+	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags)) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	mutex_lock(&priv->usb_lock);
+
+	usb_buffer = kmalloc(length, GFP_KERNEL);
+	if (!usb_buffer) {
+		ret = -ENOMEM;
+		goto end_unlock;
+	}
+
+	ret = usb_control_msg(priv->usb,
+			      usb_rcvctrlpipe(priv->usb, 0),
+			      request, 0xc0, value,
+			      index, usb_buffer, length, USB_CTL_WAIT);
+
+	if (ret == length)
+		memcpy(buffer, usb_buffer, length);
+
+	kfree(usb_buffer);
+
+	if (ret == (int)length)
+		ret = 0;
+	else
+		ret = -EIO;
+
+end_unlock:
+	mutex_unlock(&priv->usb_lock);
+end:
+	return ret;
+}
+
+int vnt_control_in_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 *data)
+{
+	return vnt_control_in(priv, MESSAGE_TYPE_READ,
+			      reg_off, reg, sizeof(u8), data);
+}
+
+static int vnt_int_report_rate(struct vnt_private *priv, u8 pkt_no, u8 tsr)
+{
+	struct vnt_usb_send_context *context;
+	struct ieee80211_tx_info *info;
+	u8 tx_retry = (tsr & 0xf0) >> 4;
+	s8 idx;
+
+	if (pkt_no >= priv->num_tx_context)
+		return -EINVAL;
+
+	context = priv->tx_context[pkt_no];
+
+	if (!context->skb)
+		return -EINVAL;
+
+	info = IEEE80211_SKB_CB(context->skb);
+	idx = info->control.rates[0].idx;
+
+	ieee80211_tx_info_clear_status(info);
+
+	info->status.rates[0].count = tx_retry;
+
+	if (!(tsr & TSR_TMO)) {
+		info->status.rates[0].idx = idx;
+
+		if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+			info->flags |= IEEE80211_TX_STAT_ACK;
+	}
+
+	ieee80211_tx_status_irqsafe(priv->hw, context->skb);
+
+	context->in_use = false;
+
+	return 0;
+}
+
+static void vnt_int_process_data(struct vnt_private *priv)
+{
+	struct vnt_interrupt_data *int_data;
+	struct ieee80211_low_level_stats *low_stats = &priv->low_stats;
+
+	dev_dbg(&priv->usb->dev, "---->s_nsInterruptProcessData\n");
+
+	int_data = (struct vnt_interrupt_data *)priv->int_buf.data_buf;
+
+	if (int_data->tsr0 & TSR_VALID)
+		vnt_int_report_rate(priv, int_data->pkt0, int_data->tsr0);
+
+	if (int_data->tsr1 & TSR_VALID)
+		vnt_int_report_rate(priv, int_data->pkt1, int_data->tsr1);
+
+	if (int_data->tsr2 & TSR_VALID)
+		vnt_int_report_rate(priv, int_data->pkt2, int_data->tsr2);
+
+	if (int_data->tsr3 & TSR_VALID)
+		vnt_int_report_rate(priv, int_data->pkt3, int_data->tsr3);
+
+	if (!int_data->isr0)
+		return;
+
+	if (int_data->isr0 & ISR_BNTX && priv->op_mode == NL80211_IFTYPE_AP)
+		vnt_schedule_command(priv, WLAN_CMD_BECON_SEND);
+
+	priv->current_tsf = le64_to_cpu(int_data->tsf);
+
+	low_stats->dot11RTSSuccessCount += int_data->rts_success;
+	low_stats->dot11RTSFailureCount += int_data->rts_fail;
+	low_stats->dot11ACKFailureCount += int_data->ack_fail;
+	low_stats->dot11FCSErrorCount += int_data->fcs_err;
+}
+
+static void vnt_start_interrupt_urb_complete(struct urb *urb)
+{
+	struct vnt_private *priv = urb->context;
+	int status = urb->status;
+
+	switch (status) {
+	case 0:
+	case -ETIMEDOUT:
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		return;
+	default:
+		break;
+	}
+
+	if (status)
+		dev_dbg(&priv->usb->dev, "%s status = %d\n", __func__, status);
+	else
+		vnt_int_process_data(priv);
+
+	if (!test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags))
+		status = usb_submit_urb(priv->interrupt_urb, GFP_ATOMIC);
+
+	if (status)
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
+}
+
+int vnt_start_interrupt_urb(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	dev_dbg(&priv->usb->dev, "---->Interrupt Polling Thread\n");
+
+	usb_fill_int_urb(priv->interrupt_urb,
+			 priv->usb,
+			 usb_rcvintpipe(priv->usb, 1),
+			 priv->int_buf.data_buf,
+			 MAX_INTERRUPT_SIZE,
+			 vnt_start_interrupt_urb_complete,
+			 priv,
+			 priv->int_interval);
+
+	ret = usb_submit_urb(priv->interrupt_urb, GFP_ATOMIC);
+	if (ret)
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", ret);
+
+	return ret;
+}
+
+static int vnt_rx_data(struct vnt_private *priv, struct vnt_rcb *ptr_rcb,
+		       unsigned long bytes_received)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_supported_band *sband;
+	struct sk_buff *skb;
+	struct ieee80211_rx_status *rx_status;
+	struct vnt_rx_header *head;
+	struct vnt_rx_tail *tail;
+	u32 frame_size;
+	int ii;
+	u16 rx_bitrate, pay_load_with_padding;
+	u8 rate_idx = 0;
+	long rx_dbm;
+
+	skb = ptr_rcb->skb;
+	rx_status = IEEE80211_SKB_RXCB(skb);
+
+	/* [31:16]RcvByteCount ( not include 4-byte Status ) */
+	head = (struct vnt_rx_header *)skb->data;
+	frame_size = head->wbk_status >> 16;
+	frame_size += 4;
+
+	if (bytes_received != frame_size) {
+		dev_dbg(&priv->usb->dev, "------- WRONG Length 1\n");
+		return false;
+	}
+
+	if ((bytes_received > 2372) || (bytes_received <= 40)) {
+		/* Frame Size error drop this packet.*/
+		dev_dbg(&priv->usb->dev, "------ WRONG Length 2\n");
+		return false;
+	}
+
+	/* real Frame Size = USBframe_size -4WbkStatus - 4RxStatus */
+	/* -8TSF - 4RSR - 4SQ3 - ?Padding */
+
+	/* if SQ3 the range is 24~27, if no SQ3 the range is 20~23 */
+
+	/*Fix hardware bug => PLCP_Length error */
+	if (((bytes_received - head->pay_load_len) > 27) ||
+	    ((bytes_received - head->pay_load_len) < 24) ||
+	    (bytes_received < head->pay_load_len)) {
+		dev_dbg(&priv->usb->dev, "Wrong PLCP Length %x\n",
+			head->pay_load_len);
+		return false;
+	}
+
+	sband = hw->wiphy->bands[hw->conf.chandef.chan->band];
+	rx_bitrate = head->rx_rate * 5; /* rx_rate * 5 */
+
+	for (ii = 0; ii < sband->n_bitrates; ii++) {
+		if (sband->bitrates[ii].bitrate == rx_bitrate) {
+			rate_idx = ii;
+				break;
+		}
+	}
+
+	if (ii == sband->n_bitrates) {
+		dev_dbg(&priv->usb->dev, "Wrong Rx Bit Rate %d\n", rx_bitrate);
+		return false;
+	}
+
+	pay_load_with_padding = ((head->pay_load_len / 4) +
+		((head->pay_load_len % 4) ? 1 : 0)) * 4;
+
+	tail = (struct vnt_rx_tail *)(skb->data +
+				      sizeof(*head) + pay_load_with_padding);
+	priv->tsf_time = le64_to_cpu(tail->tsf_time);
+
+	if (tail->rsr & (RSR_IVLDTYP | RSR_IVLDLEN))
+		return false;
+
+	vnt_rf_rssi_to_dbm(priv, tail->rssi, &rx_dbm);
+
+	priv->bb_pre_ed_rssi = (u8)-rx_dbm + 1;
+	priv->current_rssi = priv->bb_pre_ed_rssi;
+
+	skb_pull(skb, sizeof(*head));
+	skb_trim(skb, head->pay_load_len);
+
+	rx_status->mactime = priv->tsf_time;
+	rx_status->band = hw->conf.chandef.chan->band;
+	rx_status->signal = rx_dbm;
+	rx_status->flag = 0;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+
+	if (!(tail->rsr & RSR_CRCOK))
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	rx_status->rate_idx = rate_idx;
+
+	if (tail->new_rsr & NEWRSR_DECRYPTOK)
+		rx_status->flag |= RX_FLAG_DECRYPTED;
+
+	ieee80211_rx_irqsafe(priv->hw, skb);
+
+	return true;
+}
+
+static void vnt_submit_rx_urb_complete(struct urb *urb)
+{
+	struct vnt_rcb *rcb = urb->context;
+	struct vnt_private *priv = rcb->priv;
+
+	switch (urb->status) {
+	case 0:
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		return;
+	case -ETIMEDOUT:
+	default:
+		dev_dbg(&priv->usb->dev, "BULK In failed %d\n", urb->status);
+		break;
+	}
+
+	if (urb->actual_length) {
+		if (vnt_rx_data(priv, rcb, urb->actual_length)) {
+			rcb->skb = dev_alloc_skb(priv->rx_buf_sz);
+			if (!rcb->skb)
+				return;
+		} else {
+			skb_push(rcb->skb, skb_headroom(rcb->skb));
+			skb_trim(rcb->skb, 0);
+		}
+
+		urb->transfer_buffer = skb_put(rcb->skb,
+					       skb_tailroom(rcb->skb));
+	}
+
+	if (usb_submit_urb(urb, GFP_ATOMIC))
+		dev_dbg(&priv->usb->dev, "Failed to re submit rx skb\n");
+}
+
+int vnt_submit_rx_urb(struct vnt_private *priv, struct vnt_rcb *rcb)
+{
+	int ret = 0;
+	struct urb *urb = rcb->urb;
+
+	if (!rcb->skb) {
+		dev_dbg(&priv->usb->dev, "rcb->skb is null\n");
+		ret = -EINVAL;
+		goto end;
+	}
+
+	usb_fill_bulk_urb(urb,
+			  priv->usb,
+			  usb_rcvbulkpipe(priv->usb, 2),
+			  skb_put(rcb->skb, skb_tailroom(rcb->skb)),
+			  MAX_TOTAL_SIZE_WITH_ALL_HEADERS,
+			  vnt_submit_rx_urb_complete,
+			  rcb);
+
+	ret = usb_submit_urb(urb, GFP_ATOMIC);
+	if (ret)
+		dev_dbg(&priv->usb->dev, "Submit Rx URB failed %d\n", ret);
+end:
+	return ret;
+}
+
+static void vnt_tx_context_complete(struct urb *urb)
+{
+	struct vnt_usb_send_context *context = urb->context;
+	struct vnt_private *priv = context->priv;
+
+	switch (urb->status) {
+	case 0:
+		dev_dbg(&priv->usb->dev,
+			"Write %d bytes\n", urb->actual_length);
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		context->in_use = false;
+		return;
+	case -ETIMEDOUT:
+	default:
+		dev_dbg(&priv->usb->dev, "BULK Out failed %d\n", urb->status);
+		break;
+	}
+
+	if (context->type == CONTEXT_DATA_PACKET)
+		ieee80211_wake_queues(priv->hw);
+
+	if (urb->status || context->type == CONTEXT_BEACON_PACKET) {
+		if (context->skb)
+			ieee80211_free_txskb(priv->hw, context->skb);
+
+		context->in_use = false;
+	}
+}
+
+int vnt_tx_context(struct vnt_private *priv,
+		   struct vnt_usb_send_context *context,
+		   struct sk_buff *skb)
+{
+	struct vnt_tx_usb_header *usb;
+	struct urb *urb;
+	int status;
+	u16 count = skb->len;
+
+	usb = skb_push(skb, sizeof(*usb));
+	usb->tx_byte_count = cpu_to_le16(count);
+	usb->pkt_no = context->pkt_no;
+	usb->type = context->type;
+
+	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags)) {
+		context->in_use = false;
+		return -ENODEV;
+	}
+
+	if (skb->len > MAX_TOTAL_SIZE_WITH_ALL_HEADERS) {
+		context->in_use = false;
+		return -E2BIG;
+	}
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		context->in_use = false;
+		return -ENOMEM;
+	}
+
+	usb_fill_bulk_urb(urb,
+			  priv->usb,
+			  usb_sndbulkpipe(priv->usb, 3),
+			  skb->data,
+			  skb->len,
+			  vnt_tx_context_complete,
+			  context);
+
+	usb_anchor_urb(urb, &priv->tx_submitted);
+
+	status = usb_submit_urb(urb, GFP_ATOMIC);
+	if (status) {
+		dev_dbg(&priv->usb->dev, "Submit Tx URB failed %d\n", status);
+		usb_unanchor_urb(urb);
+		context->in_use = false;
+	}
+
+	usb_free_urb(urb);
+
+	return status;
+}
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
new file mode 100644
index 000000000..922312e29
--- /dev/null
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -0,0 +1,67 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose:
+ *
+ * Author: Warren Hsu
+ *
+ * Date: Mar. 30, 2005
+ *
+ */
+
+#ifndef __USBPIPE_H__
+#define __USBPIPE_H__
+
+#include "device.h"
+
+struct vnt_interrupt_data {
+	u8 tsr0;
+	u8 pkt0;
+	u16 time0;
+	u8 tsr1;
+	u8 pkt1;
+	u16 time1;
+	u8 tsr2;
+	u8 pkt2;
+	u16 time2;
+	u8 tsr3;
+	u8 pkt3;
+	u16 time3;
+	__le64 tsf;
+	u8 isr0;
+	u8 isr1;
+	u8 rts_success;
+	u8 rts_fail;
+	u8 ack_fail;
+	u8 fcs_err;
+	u8 sw[2];
+} __packed;
+
+struct vnt_tx_usb_header {
+	u8 type;
+	u8 pkt_no;
+	__le16 tx_byte_count;
+} __packed;
+
+#define VNT_REG_BLOCK_SIZE	64
+
+int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
+		    u16 index, u16 length, const u8 *buffer);
+int vnt_control_in(struct vnt_private *priv, u8 request, u16 value,
+		   u16 index, u16 length,  u8 *buffer);
+
+int vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 ref_off, u8 data);
+int vnt_control_in_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 *data);
+
+int vnt_control_out_blocks(struct vnt_private *priv,
+			   u16 block, u8 reg, u16 len, const u8 *data);
+
+int vnt_start_interrupt_urb(struct vnt_private *priv);
+int vnt_submit_rx_urb(struct vnt_private *priv, struct vnt_rcb *rcb);
+int vnt_tx_context(struct vnt_private *priv,
+		   struct vnt_usb_send_context *context,
+		   struct sk_buff *skb);
+
+#endif /* __USBPIPE_H__ */
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
new file mode 100644
index 000000000..14b8aa587
--- /dev/null
+++ b/drivers/staging/vt6656/wcmd.c
@@ -0,0 +1,185 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles the management command interface functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: May 8, 2003
+ *
+ * Functions:
+ *	vnt_cmd_complete - Command Complete function
+ *	vnt_schedule_command - Push Command and wait Command Scheduler to do
+ *	vnt_cmd_timer_wait- Call back timer
+ *
+ * Revision History:
+ *
+ */
+
+#include "device.h"
+#include "mac.h"
+#include "wcmd.h"
+#include "power.h"
+#include "usbpipe.h"
+#include "rxtx.h"
+#include "rf.h"
+
+static void vnt_cmd_timer_wait(struct vnt_private *priv, unsigned long msecs)
+{
+	schedule_delayed_work(&priv->run_command_work, msecs_to_jiffies(msecs));
+}
+
+static u32 add_one_with_wrap_around(u32 var, u8 modulo)
+{
+	if (var >= (modulo - 1))
+		var = 0;
+	else
+		var++;
+	return var;
+}
+
+static int vnt_cmd_complete(struct vnt_private *priv)
+{
+	priv->command_state = WLAN_CMD_IDLE;
+	if (priv->free_cmd_queue == CMD_Q_SIZE) {
+		/* Command Queue Empty */
+		priv->cmd_running = false;
+		return true;
+	}
+
+	priv->command = priv->cmd_queue[priv->cmd_dequeue_idx];
+
+	priv->cmd_dequeue_idx = add_one_with_wrap_around(priv->cmd_dequeue_idx, CMD_Q_SIZE);
+	priv->free_cmd_queue++;
+	priv->cmd_running = true;
+
+	switch (priv->command) {
+	case WLAN_CMD_INIT_MAC80211:
+		priv->command_state = WLAN_CMD_INIT_MAC80211_START;
+		break;
+
+	case WLAN_CMD_TBTT_WAKEUP:
+		priv->command_state = WLAN_CMD_TBTT_WAKEUP_START;
+		break;
+
+	case WLAN_CMD_BECON_SEND:
+		priv->command_state = WLAN_CMD_BECON_SEND_START;
+		break;
+
+	case WLAN_CMD_SETPOWER:
+		priv->command_state = WLAN_CMD_SETPOWER_START;
+		break;
+
+	case WLAN_CMD_CHANGE_ANTENNA:
+		priv->command_state = WLAN_CMD_CHANGE_ANTENNA_START;
+		break;
+
+	default:
+		break;
+	}
+
+	vnt_cmd_timer_wait(priv, 0);
+
+	return true;
+}
+
+void vnt_run_command(struct work_struct *work)
+{
+	struct vnt_private *priv =
+		container_of(work, struct vnt_private, run_command_work.work);
+
+	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags))
+		return;
+
+	if (!priv->cmd_running)
+		return;
+
+	switch (priv->command_state) {
+	case WLAN_CMD_INIT_MAC80211_START:
+		if (priv->mac_hw)
+			break;
+
+		dev_info(&priv->usb->dev, "Starting mac80211\n");
+
+		if (vnt_init(priv)) {
+			/* If fail all ends TODO retry */
+			dev_err(&priv->usb->dev, "failed to start\n");
+			usb_set_intfdata(priv->intf, NULL);
+			ieee80211_free_hw(priv->hw);
+			return;
+		}
+
+		break;
+
+	case WLAN_CMD_TBTT_WAKEUP_START:
+		vnt_next_tbtt_wakeup(priv);
+		break;
+
+	case WLAN_CMD_BECON_SEND_START:
+		if (!priv->vif)
+			break;
+
+		vnt_beacon_make(priv, priv->vif);
+
+		vnt_mac_reg_bits_on(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+
+		break;
+
+	case WLAN_CMD_SETPOWER_START:
+
+		vnt_rf_setpower(priv, priv->hw->conf.chandef.chan);
+
+		break;
+
+	case WLAN_CMD_CHANGE_ANTENNA_START:
+		dev_dbg(&priv->usb->dev, "Change from Antenna%d to",
+			priv->rx_antenna_sel);
+
+		if (priv->rx_antenna_sel == 0) {
+			priv->rx_antenna_sel = 1;
+			if (priv->tx_rx_ant_inv)
+				vnt_set_antenna_mode(priv, ANT_RXA);
+			else
+				vnt_set_antenna_mode(priv, ANT_RXB);
+		} else {
+			priv->rx_antenna_sel = 0;
+			if (priv->tx_rx_ant_inv)
+				vnt_set_antenna_mode(priv, ANT_RXB);
+			else
+				vnt_set_antenna_mode(priv, ANT_RXA);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	vnt_cmd_complete(priv);
+}
+
+int vnt_schedule_command(struct vnt_private *priv, enum vnt_cmd command)
+{
+	if (priv->free_cmd_queue == 0)
+		return false;
+
+	priv->cmd_queue[priv->cmd_enqueue_idx] = command;
+
+	priv->cmd_enqueue_idx = add_one_with_wrap_around(priv->cmd_enqueue_idx, CMD_Q_SIZE);
+	priv->free_cmd_queue--;
+
+	if (!priv->cmd_running)
+		vnt_cmd_complete(priv);
+
+	return true;
+}
+
+void vnt_reset_command_timer(struct vnt_private *priv)
+{
+	priv->free_cmd_queue = CMD_Q_SIZE;
+	priv->cmd_dequeue_idx = 0;
+	priv->cmd_enqueue_idx = 0;
+	priv->command_state = WLAN_CMD_IDLE;
+	priv->cmd_running = false;
+}
diff --git a/drivers/staging/vt6656/wcmd.h b/drivers/staging/vt6656/wcmd.h
new file mode 100644
index 000000000..a62924671
--- /dev/null
+++ b/drivers/staging/vt6656/wcmd.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
+ * All rights reserved.
+ *
+ * Purpose: Handles the management command interface functions
+ *
+ * Author: Lyndon Chen
+ *
+ * Date: May 8, 2002
+ *
+ */
+
+#ifndef __WCMD_H__
+#define __WCMD_H__
+
+#include "device.h"
+
+/* Command code */
+enum vnt_cmd {
+	WLAN_CMD_INIT_MAC80211,
+	WLAN_CMD_SETPOWER,
+	WLAN_CMD_TBTT_WAKEUP,
+	WLAN_CMD_BECON_SEND,
+	WLAN_CMD_CHANGE_ANTENNA
+};
+
+#define CMD_Q_SIZE              32
+
+/* Command state */
+enum vnt_cmd_state {
+	WLAN_CMD_INIT_MAC80211_START,
+	WLAN_CMD_SETPOWER_START,
+	WLAN_CMD_TBTT_WAKEUP_START,
+	WLAN_CMD_BECON_SEND_START,
+	WLAN_CMD_CHANGE_ANTENNA_START,
+	WLAN_CMD_IDLE
+};
+
+struct vnt_private;
+
+void vnt_reset_command_timer(struct vnt_private *priv);
+
+int vnt_schedule_command(struct vnt_private *priv, enum vnt_cmd);
+
+void vnt_run_command(struct work_struct *work);
+
+#endif /* __WCMD_H__ */
diff --git a/drivers/staging/wlan-ng/Kconfig b/drivers/staging/wlan-ng/Kconfig
new file mode 100644
index 000000000..082c16a31
--- /dev/null
+++ b/drivers/staging/wlan-ng/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+config PRISM2_USB
+	tristate "Prism2.5/3 USB driver"
+	depends on WLAN && USB && CFG80211
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select CRC32
+	help
+	  This is the wlan-ng prism 2.5/3 USB driver for a wide range of
+	  old USB wireless devices.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called prism2_usb.
diff --git a/drivers/staging/wlan-ng/Makefile b/drivers/staging/wlan-ng/Makefile
new file mode 100644
index 000000000..1d24b0f86
--- /dev/null
+++ b/drivers/staging/wlan-ng/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_PRISM2_USB) += prism2_usb.o
+
+prism2_usb-y := prism2usb.o \
+		p80211conv.o \
+		p80211req.o \
+		p80211wep.o \
+		p80211netdev.o
diff --git a/drivers/staging/wlan-ng/README b/drivers/staging/wlan-ng/README
new file mode 100644
index 000000000..d0621f827
--- /dev/null
+++ b/drivers/staging/wlan-ng/README
@@ -0,0 +1,8 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse warnings
+	- move to use the in-kernel wireless stack
+
+Please send any patches or complaints about this driver to Greg
+Kroah-Hartman <greg@kroah.com> and don't bother the upstream wireless
+kernel developers about it, they want nothing to do with it.
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
new file mode 100644
index 000000000..471bb3101
--- /dev/null
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -0,0 +1,718 @@
+// SPDX-License-Identifier: GPL-2.0
+/* cfg80211 Interface for prism2_usb module */
+#include "hfa384x.h"
+#include "prism2mgmt.h"
+
+/* Prism2 channel/frequency/bitrate declarations */
+static const struct ieee80211_channel prism2_channels[] = {
+	{ .center_freq = 2412 },
+	{ .center_freq = 2417 },
+	{ .center_freq = 2422 },
+	{ .center_freq = 2427 },
+	{ .center_freq = 2432 },
+	{ .center_freq = 2437 },
+	{ .center_freq = 2442 },
+	{ .center_freq = 2447 },
+	{ .center_freq = 2452 },
+	{ .center_freq = 2457 },
+	{ .center_freq = 2462 },
+	{ .center_freq = 2467 },
+	{ .center_freq = 2472 },
+	{ .center_freq = 2484 },
+};
+
+static const struct ieee80211_rate prism2_rates[] = {
+	{ .bitrate = 10 },
+	{ .bitrate = 20 },
+	{ .bitrate = 55 },
+	{ .bitrate = 110 }
+};
+
+#define PRISM2_NUM_CIPHER_SUITES 2
+static const u32 prism2_cipher_suites[PRISM2_NUM_CIPHER_SUITES] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104
+};
+
+/* prism2 device private data */
+struct prism2_wiphy_private {
+	struct wlandevice *wlandev;
+
+	struct ieee80211_supported_band band;
+	struct ieee80211_channel channels[ARRAY_SIZE(prism2_channels)];
+	struct ieee80211_rate rates[ARRAY_SIZE(prism2_rates)];
+
+	struct cfg80211_scan_request *scan_request;
+};
+
+static const void * const prism2_wiphy_privid = &prism2_wiphy_privid;
+
+/* Helper Functions */
+static int prism2_result2err(int prism2_result)
+{
+	int err = 0;
+
+	switch (prism2_result) {
+	case P80211ENUM_resultcode_invalid_parameters:
+		err = -EINVAL;
+		break;
+	case P80211ENUM_resultcode_implementation_failure:
+		err = -EIO;
+		break;
+	case P80211ENUM_resultcode_not_supported:
+		err = -EOPNOTSUPP;
+		break;
+	default:
+		err = 0;
+		break;
+	}
+
+	return err;
+}
+
+static int prism2_domibset_uint32(struct wlandevice *wlandev,
+				  u32 did, u32 data)
+{
+	struct p80211msg_dot11req_mibset msg;
+	struct p80211item_uint32 *mibitem =
+			(struct p80211item_uint32 *)&msg.mibattribute.data;
+
+	msg.msgcode = DIDMSG_DOT11REQ_MIBSET;
+	mibitem->did = did;
+	mibitem->data = data;
+
+	return p80211req_dorequest(wlandev, (u8 *)&msg);
+}
+
+static int prism2_domibset_pstr32(struct wlandevice *wlandev,
+				  u32 did, u8 len, const u8 *data)
+{
+	struct p80211msg_dot11req_mibset msg;
+	struct p80211item_pstr32 *mibitem =
+			(struct p80211item_pstr32 *)&msg.mibattribute.data;
+
+	msg.msgcode = DIDMSG_DOT11REQ_MIBSET;
+	mibitem->did = did;
+	mibitem->data.len = len;
+	memcpy(mibitem->data.data, data, len);
+
+	return p80211req_dorequest(wlandev, (u8 *)&msg);
+}
+
+/* The interface functions, called by the cfg80211 layer */
+static int prism2_change_virtual_intf(struct wiphy *wiphy,
+				      struct net_device *dev,
+				      enum nl80211_iftype type,
+				      struct vif_params *params)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	u32 data;
+	int result;
+	int err = 0;
+
+	switch (type) {
+	case NL80211_IFTYPE_ADHOC:
+		if (wlandev->macmode == WLAN_MACMODE_IBSS_STA)
+			goto exit;
+		wlandev->macmode = WLAN_MACMODE_IBSS_STA;
+		data = 0;
+		break;
+	case NL80211_IFTYPE_STATION:
+		if (wlandev->macmode == WLAN_MACMODE_ESS_STA)
+			goto exit;
+		wlandev->macmode = WLAN_MACMODE_ESS_STA;
+		data = 1;
+		break;
+	default:
+		netdev_warn(dev, "Operation mode: %d not support\n", type);
+		return -EOPNOTSUPP;
+	}
+
+	/* Set Operation mode to the PORT TYPE RID */
+	result = prism2_domibset_uint32(wlandev,
+					DIDMIB_P2_STATIC_CNFPORTTYPE,
+					data);
+
+	if (result)
+		err = -EFAULT;
+
+	dev->ieee80211_ptr->iftype = type;
+
+exit:
+	return err;
+}
+
+static int prism2_add_key(struct wiphy *wiphy, struct net_device *dev,
+			  int link_id, u8 key_index, bool pairwise,
+			  const u8 *mac_addr, struct key_params *params)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	u32 did;
+
+	if (key_index >= NUM_WEPKEYS)
+		return -EINVAL;
+
+	if (params->cipher != WLAN_CIPHER_SUITE_WEP40 &&
+	    params->cipher != WLAN_CIPHER_SUITE_WEP104) {
+		pr_debug("Unsupported cipher suite\n");
+		return -EFAULT;
+	}
+
+	if (prism2_domibset_uint32(wlandev,
+				   DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID,
+				   key_index))
+		return -EFAULT;
+
+	/* send key to driver */
+	did = didmib_dot11smt_wepdefaultkeystable_key(key_index + 1);
+
+	if (prism2_domibset_pstr32(wlandev, did, params->key_len, params->key))
+		return -EFAULT;
+	return 0;
+}
+
+static int prism2_get_key(struct wiphy *wiphy, struct net_device *dev,
+			  int link_id, u8 key_index, bool pairwise,
+			  const u8 *mac_addr, void *cookie,
+			  void (*callback)(void *cookie, struct key_params*))
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	struct key_params params;
+	int len;
+
+	if (key_index >= NUM_WEPKEYS)
+		return -EINVAL;
+
+	len = wlandev->wep_keylens[key_index];
+	memset(&params, 0, sizeof(params));
+
+	if (len == 13)
+		params.cipher = WLAN_CIPHER_SUITE_WEP104;
+	else if (len == 5)
+		params.cipher = WLAN_CIPHER_SUITE_WEP104;
+	else
+		return -ENOENT;
+	params.key_len = len;
+	params.key = wlandev->wep_keys[key_index];
+	params.seq_len = 0;
+
+	callback(cookie, &params);
+
+	return 0;
+}
+
+static int prism2_del_key(struct wiphy *wiphy, struct net_device *dev,
+			  int link_id, u8 key_index, bool pairwise,
+			  const u8 *mac_addr)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	u32 did;
+	int err = 0;
+	int result = 0;
+
+	/* There is no direct way in the hardware (AFAIK) of removing
+	 * a key, so we will cheat by setting the key to a bogus value
+	 */
+
+	if (key_index >= NUM_WEPKEYS)
+		return -EINVAL;
+
+	/* send key to driver */
+	did = didmib_dot11smt_wepdefaultkeystable_key(key_index + 1);
+	result = prism2_domibset_pstr32(wlandev, did, 13, "0000000000000");
+
+	if (result)
+		err = -EFAULT;
+
+	return err;
+}
+
+static int prism2_set_default_key(struct wiphy *wiphy, struct net_device *dev,
+				  int link_id, u8 key_index, bool unicast,
+				  bool multicast)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+
+	return  prism2_domibset_uint32(wlandev,
+				       DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID,
+				       key_index);
+}
+
+static int prism2_get_station(struct wiphy *wiphy, struct net_device *dev,
+			      const u8 *mac, struct station_info *sinfo)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	struct p80211msg_lnxreq_commsquality quality;
+	int result;
+
+	memset(sinfo, 0, sizeof(*sinfo));
+
+	if (!wlandev || (wlandev->msdstate != WLAN_MSD_RUNNING))
+		return -EOPNOTSUPP;
+
+	/* build request message */
+	quality.msgcode = DIDMSG_LNXREQ_COMMSQUALITY;
+	quality.dbm.data = P80211ENUM_truth_true;
+	quality.dbm.status = P80211ENUM_msgitem_status_data_ok;
+
+	/* send message to nsd */
+	if (!wlandev->mlmerequest)
+		return -EOPNOTSUPP;
+
+	result = wlandev->mlmerequest(wlandev, (struct p80211msg *)&quality);
+
+	if (result == 0) {
+		sinfo->txrate.legacy = quality.txrate.data;
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_BITRATE);
+		sinfo->signal = quality.level.data;
+		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_SIGNAL);
+	}
+
+	return result;
+}
+
+static int prism2_scan(struct wiphy *wiphy,
+		       struct cfg80211_scan_request *request)
+{
+	struct net_device *dev;
+	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
+	struct wlandevice *wlandev;
+	struct p80211msg_dot11req_scan msg1;
+	struct p80211msg_dot11req_scan_results *msg2;
+	struct cfg80211_bss *bss;
+	struct cfg80211_scan_info info = {};
+
+	int result;
+	int err = 0;
+	int numbss = 0;
+	int i = 0;
+	u8 ie_buf[46];
+	int ie_len;
+
+	if (!request)
+		return -EINVAL;
+
+	dev = request->wdev->netdev;
+	wlandev = dev->ml_priv;
+
+	if (priv->scan_request && priv->scan_request != request)
+		return -EBUSY;
+
+	if (wlandev->macmode == WLAN_MACMODE_ESS_AP) {
+		netdev_err(dev, "Can't scan in AP mode\n");
+		return -EOPNOTSUPP;
+	}
+
+	msg2 = kzalloc(sizeof(*msg2), GFP_KERNEL);
+	if (!msg2)
+		return -ENOMEM;
+
+	priv->scan_request = request;
+
+	memset(&msg1, 0x00, sizeof(msg1));
+	msg1.msgcode = DIDMSG_DOT11REQ_SCAN;
+	msg1.bsstype.data = P80211ENUM_bsstype_any;
+
+	memset(&msg1.bssid.data.data, 0xFF, sizeof(msg1.bssid.data.data));
+	msg1.bssid.data.len = 6;
+
+	if (request->n_ssids > 0) {
+		msg1.scantype.data = P80211ENUM_scantype_active;
+		msg1.ssid.data.len = request->ssids->ssid_len;
+		memcpy(msg1.ssid.data.data,
+		       request->ssids->ssid, request->ssids->ssid_len);
+	} else {
+		msg1.scantype.data = 0;
+	}
+	msg1.probedelay.data = 0;
+
+	for (i = 0;
+		(i < request->n_channels) && i < ARRAY_SIZE(prism2_channels);
+		i++)
+		msg1.channellist.data.data[i] =
+			ieee80211_frequency_to_channel(request->channels[i]->center_freq);
+	msg1.channellist.data.len = request->n_channels;
+
+	msg1.maxchanneltime.data = 250;
+	msg1.minchanneltime.data = 200;
+
+	result = p80211req_dorequest(wlandev, (u8 *)&msg1);
+	if (result) {
+		err = prism2_result2err(msg1.resultcode.data);
+		goto exit;
+	}
+	/* Now retrieve scan results */
+	numbss = msg1.numbss.data;
+
+	for (i = 0; i < numbss; i++) {
+		int freq;
+
+		msg2->msgcode = DIDMSG_DOT11REQ_SCAN_RESULTS;
+		msg2->bssindex.data = i;
+
+		result = p80211req_dorequest(wlandev, (u8 *)&msg2);
+		if ((result != 0) ||
+		    (msg2->resultcode.data != P80211ENUM_resultcode_success)) {
+			break;
+		}
+
+		ie_buf[0] = WLAN_EID_SSID;
+		ie_buf[1] = msg2->ssid.data.len;
+		ie_len = ie_buf[1] + 2;
+		memcpy(&ie_buf[2], &msg2->ssid.data.data, msg2->ssid.data.len);
+		freq = ieee80211_channel_to_frequency(msg2->dschannel.data,
+						      NL80211_BAND_2GHZ);
+		bss = cfg80211_inform_bss(wiphy,
+					  ieee80211_get_channel(wiphy, freq),
+					  CFG80211_BSS_FTYPE_UNKNOWN,
+					  (const u8 *)&msg2->bssid.data.data,
+					  msg2->timestamp.data, msg2->capinfo.data,
+					  msg2->beaconperiod.data,
+					  ie_buf,
+					  ie_len,
+					  (msg2->signal.data - 65536) * 100, /* Conversion to signed type */
+					  GFP_KERNEL);
+
+		if (!bss) {
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		cfg80211_put_bss(wiphy, bss);
+	}
+
+	if (result)
+		err = prism2_result2err(msg2->resultcode.data);
+
+exit:
+	info.aborted = !!(err);
+	cfg80211_scan_done(request, &info);
+	priv->scan_request = NULL;
+	kfree(msg2);
+	return err;
+}
+
+static int prism2_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
+	struct wlandevice *wlandev = priv->wlandev;
+	u32 data;
+	int result;
+	int err = 0;
+
+	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
+		if (wiphy->rts_threshold == -1)
+			data = 2347;
+		else
+			data = wiphy->rts_threshold;
+
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11MAC_OPERATIONTABLE_RTSTHRESHOLD,
+						data);
+		if (result) {
+			err = -EFAULT;
+			goto exit;
+		}
+	}
+
+	if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
+		if (wiphy->frag_threshold == -1)
+			data = 2346;
+		else
+			data = wiphy->frag_threshold;
+
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11MAC_OPERATIONTABLE_FRAGMENTATIONTHRESHOLD,
+						data);
+		if (result) {
+			err = -EFAULT;
+			goto exit;
+		}
+	}
+
+exit:
+	return err;
+}
+
+static int prism2_connect(struct wiphy *wiphy, struct net_device *dev,
+			  struct cfg80211_connect_params *sme)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	struct ieee80211_channel *channel = sme->channel;
+	struct p80211msg_lnxreq_autojoin msg_join;
+	u32 did;
+	int length = sme->ssid_len;
+	int chan = -1;
+	int is_wep = (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP40) ||
+	    (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP104);
+	int result;
+	int err = 0;
+
+	/* Set the channel */
+	if (channel) {
+		chan = ieee80211_frequency_to_channel(channel->center_freq);
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11PHY_DSSSTABLE_CURRENTCHANNEL,
+						chan);
+		if (result)
+			goto exit;
+	}
+
+	/* Set the authorization */
+	if ((sme->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM) ||
+	    ((sme->auth_type == NL80211_AUTHTYPE_AUTOMATIC) && !is_wep))
+		msg_join.authtype.data = P80211ENUM_authalg_opensystem;
+	else if ((sme->auth_type == NL80211_AUTHTYPE_SHARED_KEY) ||
+		 ((sme->auth_type == NL80211_AUTHTYPE_AUTOMATIC) && is_wep))
+		msg_join.authtype.data = P80211ENUM_authalg_sharedkey;
+	else
+		netdev_warn(dev,
+			    "Unhandled authorisation type for connect (%d)\n",
+			    sme->auth_type);
+
+	/* Set the encryption - we only support wep */
+	if (is_wep) {
+		if (sme->key) {
+			if (sme->key_idx >= NUM_WEPKEYS)
+				return -EINVAL;
+
+			result = prism2_domibset_uint32(wlandev,
+							DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID,
+				sme->key_idx);
+			if (result)
+				goto exit;
+
+			/* send key to driver */
+			did = didmib_dot11smt_wepdefaultkeystable_key(sme->key_idx + 1);
+			result = prism2_domibset_pstr32(wlandev,
+							did, sme->key_len,
+							(u8 *)sme->key);
+			if (result)
+				goto exit;
+		}
+
+		/* Assume we should set privacy invoked and exclude unencrypted
+		 * We could possible use sme->privacy here, but the assumption
+		 * seems reasonable anyways
+		 */
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11SMT_PRIVACYTABLE_PRIVACYINVOKED,
+						P80211ENUM_truth_true);
+		if (result)
+			goto exit;
+
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11SMT_PRIVACYTABLE_EXCLUDEUNENCRYPTED,
+						P80211ENUM_truth_true);
+		if (result)
+			goto exit;
+
+	} else {
+		/* Assume we should unset privacy invoked
+		 * and exclude unencrypted
+		 */
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11SMT_PRIVACYTABLE_PRIVACYINVOKED,
+						P80211ENUM_truth_false);
+		if (result)
+			goto exit;
+
+		result = prism2_domibset_uint32(wlandev,
+						DIDMIB_DOT11SMT_PRIVACYTABLE_EXCLUDEUNENCRYPTED,
+						P80211ENUM_truth_false);
+		if (result)
+			goto exit;
+	}
+
+	/* Now do the actual join. Note there is no way that I can
+	 * see to request a specific bssid
+	 */
+	msg_join.msgcode = DIDMSG_LNXREQ_AUTOJOIN;
+
+	memcpy(msg_join.ssid.data.data, sme->ssid, length);
+	msg_join.ssid.data.len = length;
+
+	result = p80211req_dorequest(wlandev, (u8 *)&msg_join);
+
+exit:
+	if (result)
+		err = -EFAULT;
+
+	return err;
+}
+
+static int prism2_disconnect(struct wiphy *wiphy, struct net_device *dev,
+			     u16 reason_code)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+	struct p80211msg_lnxreq_autojoin msg_join;
+	int result;
+	int err = 0;
+
+	/* Do a join, with a bogus ssid. Thats the only way I can think of */
+	msg_join.msgcode = DIDMSG_LNXREQ_AUTOJOIN;
+
+	memcpy(msg_join.ssid.data.data, "---", 3);
+	msg_join.ssid.data.len = 3;
+
+	result = p80211req_dorequest(wlandev, (u8 *)&msg_join);
+
+	if (result)
+		err = -EFAULT;
+
+	return err;
+}
+
+static int prism2_join_ibss(struct wiphy *wiphy, struct net_device *dev,
+			    struct cfg80211_ibss_params *params)
+{
+	return -EOPNOTSUPP;
+}
+
+static int prism2_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
+{
+	return -EOPNOTSUPP;
+}
+
+static int prism2_set_tx_power(struct wiphy *wiphy, struct wireless_dev *wdev,
+			       enum nl80211_tx_power_setting type, int mbm)
+{
+	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
+	struct wlandevice *wlandev = priv->wlandev;
+	u32 data;
+	int result;
+	int err = 0;
+
+	if (type == NL80211_TX_POWER_AUTOMATIC)
+		data = 30;
+	else
+		data = MBM_TO_DBM(mbm);
+
+	result = prism2_domibset_uint32(wlandev,
+					DIDMIB_DOT11PHY_TXPOWERTABLE_CURRENTTXPOWERLEVEL,
+		data);
+
+	if (result) {
+		err = -EFAULT;
+		goto exit;
+	}
+
+exit:
+	return err;
+}
+
+static int prism2_get_tx_power(struct wiphy *wiphy, struct wireless_dev *wdev,
+			       int *dbm)
+{
+	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
+	struct wlandevice *wlandev = priv->wlandev;
+	struct p80211msg_dot11req_mibget msg;
+	struct p80211item_uint32 *mibitem;
+	int result;
+	int err = 0;
+
+	mibitem = (struct p80211item_uint32 *)&msg.mibattribute.data;
+	msg.msgcode = DIDMSG_DOT11REQ_MIBGET;
+	mibitem->did = DIDMIB_DOT11PHY_TXPOWERTABLE_CURRENTTXPOWERLEVEL;
+
+	result = p80211req_dorequest(wlandev, (u8 *)&msg);
+
+	if (result) {
+		err = -EFAULT;
+		goto exit;
+	}
+
+	*dbm = mibitem->data;
+
+exit:
+	return err;
+}
+
+/* Interface callback functions, passing data back up to the cfg80211 layer */
+void prism2_connect_result(struct wlandevice *wlandev, u8 failed)
+{
+	u16 status = failed ?
+		     WLAN_STATUS_UNSPECIFIED_FAILURE : WLAN_STATUS_SUCCESS;
+
+	cfg80211_connect_result(wlandev->netdev, wlandev->bssid,
+				NULL, 0, NULL, 0, status, GFP_KERNEL);
+}
+
+void prism2_disconnected(struct wlandevice *wlandev)
+{
+	cfg80211_disconnected(wlandev->netdev, 0, NULL,
+			      0, false, GFP_KERNEL);
+}
+
+void prism2_roamed(struct wlandevice *wlandev)
+{
+	struct cfg80211_roam_info roam_info = {
+		.links[0].bssid = wlandev->bssid,
+	};
+
+	cfg80211_roamed(wlandev->netdev, &roam_info, GFP_KERNEL);
+}
+
+/* Structures for declaring wiphy interface */
+static const struct cfg80211_ops prism2_usb_cfg_ops = {
+	.change_virtual_intf = prism2_change_virtual_intf,
+	.add_key = prism2_add_key,
+	.get_key = prism2_get_key,
+	.del_key = prism2_del_key,
+	.set_default_key = prism2_set_default_key,
+	.get_station = prism2_get_station,
+	.scan = prism2_scan,
+	.set_wiphy_params = prism2_set_wiphy_params,
+	.connect = prism2_connect,
+	.disconnect = prism2_disconnect,
+	.join_ibss = prism2_join_ibss,
+	.leave_ibss = prism2_leave_ibss,
+	.set_tx_power = prism2_set_tx_power,
+	.get_tx_power = prism2_get_tx_power,
+};
+
+/* Functions to create/free wiphy interface */
+static struct wiphy *wlan_create_wiphy(struct device *dev,
+				       struct wlandevice *wlandev)
+{
+	struct wiphy *wiphy;
+	struct prism2_wiphy_private *priv;
+
+	wiphy = wiphy_new(&prism2_usb_cfg_ops, sizeof(*priv));
+	if (!wiphy)
+		return NULL;
+
+	priv = wiphy_priv(wiphy);
+	priv->wlandev = wlandev;
+	memcpy(priv->channels, prism2_channels, sizeof(prism2_channels));
+	memcpy(priv->rates, prism2_rates, sizeof(prism2_rates));
+	priv->band.channels = priv->channels;
+	priv->band.n_channels = ARRAY_SIZE(prism2_channels);
+	priv->band.bitrates = priv->rates;
+	priv->band.n_bitrates = ARRAY_SIZE(prism2_rates);
+	priv->band.band = NL80211_BAND_2GHZ;
+	priv->band.ht_cap.ht_supported = false;
+	wiphy->bands[NL80211_BAND_2GHZ] = &priv->band;
+
+	set_wiphy_dev(wiphy, dev);
+	wiphy->privid = prism2_wiphy_privid;
+	wiphy->max_scan_ssids = 1;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION)
+				 | BIT(NL80211_IFTYPE_ADHOC);
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	wiphy->n_cipher_suites = PRISM2_NUM_CIPHER_SUITES;
+	wiphy->cipher_suites = prism2_cipher_suites;
+
+	if (wiphy_register(wiphy) < 0) {
+		wiphy_free(wiphy);
+		return NULL;
+	}
+
+	return wiphy;
+}
+
+static void wlan_free_wiphy(struct wiphy *wiphy)
+{
+	wiphy_unregister(wiphy);
+	wiphy_free(wiphy);
+}
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
new file mode 100644
index 000000000..0611e37df
--- /dev/null
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -0,0 +1,1428 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Defines the constants and data structures for the hfa384x
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *    implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ *   [Implementation and usage notes]
+ *
+ *   [References]
+ *	CW10 Programmer's Manual v1.5
+ *	IEEE 802.11 D10.0
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _HFA384x_H
+#define _HFA384x_H
+
+#define HFA384x_FIRMWARE_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c))
+
+#include <linux/if_ether.h>
+#include <linux/usb.h>
+
+/*--- Mins & Maxs -----------------------------------*/
+#define	HFA384x_PORTID_MAX		((u16)7)
+#define	HFA384x_NUMPORTS_MAX		((u16)(HFA384x_PORTID_MAX + 1))
+#define	HFA384x_PDR_LEN_MAX		((u16)512) /* in bytes, from EK */
+#define	HFA384x_PDA_RECS_MAX		((u16)200) /* a guess */
+#define	HFA384x_PDA_LEN_MAX		((u16)1024) /* in bytes, from EK*/
+#define	HFA384x_SCANRESULT_MAX		((u16)31)
+#define	HFA384x_HSCANRESULT_MAX		((u16)31)
+#define	HFA384x_CHINFORESULT_MAX	((u16)16)
+#define	HFA384x_RID_GUESSING_MAXLEN	2048	/* I'm not really sure */
+#define	HFA384x_RIDDATA_MAXLEN		HFA384x_RID_GUESSING_MAXLEN
+#define	HFA384x_USB_RWMEM_MAXLEN	2048
+
+/*--- Support Constants -----------------------------*/
+#define		HFA384x_PORTTYPE_IBSS			((u16)0)
+#define		HFA384x_PORTTYPE_BSS			((u16)1)
+#define		HFA384x_PORTTYPE_PSUEDOIBSS		((u16)3)
+#define		HFA384x_WEPFLAGS_PRIVINVOKED		((u16)BIT(0))
+#define		HFA384x_WEPFLAGS_EXCLUDE		((u16)BIT(1))
+#define		HFA384x_WEPFLAGS_DISABLE_TXCRYPT	((u16)BIT(4))
+#define		HFA384x_WEPFLAGS_DISABLE_RXCRYPT	((u16)BIT(7))
+#define		HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM	((u16)3)
+#define		HFA384x_PORTSTATUS_DISABLED		((u16)1)
+#define		HFA384x_RATEBIT_1			((u16)1)
+#define		HFA384x_RATEBIT_2			((u16)2)
+#define		HFA384x_RATEBIT_5dot5			((u16)4)
+#define		HFA384x_RATEBIT_11			((u16)8)
+
+/*--- MAC Internal memory constants and macros ------*/
+/* masks and macros used to manipulate MAC internal memory addresses. */
+/* MAC internal memory addresses are 23 bit quantities.  The MAC uses
+ * a paged address space where the upper 16 bits are the page number
+ * and the lower 7 bits are the offset.  There are various Host API
+ * elements that require two 16-bit quantities to specify a MAC
+ * internal memory address.  Unfortunately, some of the API's use a
+ * page/offset format where the offset value is JUST the lower seven
+ * bits and the page is  the remaining 16 bits.  Some of the API's
+ * assume that the 23 bit address has been split at the 16th bit.  We
+ * refer to these two formats as AUX format and CMD format.  The
+ * macros below help handle some of this.
+ */
+
+/* Mask bits for discarding unwanted pieces in a flat address */
+#define		HFA384x_ADDR_FLAT_AUX_PAGE_MASK	(0x007fff80)
+#define		HFA384x_ADDR_FLAT_AUX_OFF_MASK	(0x0000007f)
+#define		HFA384x_ADDR_FLAT_CMD_PAGE_MASK	(0xffff0000)
+#define		HFA384x_ADDR_FLAT_CMD_OFF_MASK	(0x0000ffff)
+
+/* Mask bits for discarding unwanted pieces in AUX format
+ * 16-bit address parts
+ */
+#define		HFA384x_ADDR_AUX_PAGE_MASK	(0xffff)
+#define		HFA384x_ADDR_AUX_OFF_MASK	(0x007f)
+
+/* Make a 32-bit flat address from AUX format 16-bit page and offset */
+#define		HFA384x_ADDR_AUX_MKFLAT(p, o)	\
+		((((u32)(((u16)(p)) & HFA384x_ADDR_AUX_PAGE_MASK)) << 7) | \
+		((u32)(((u16)(o)) & HFA384x_ADDR_AUX_OFF_MASK)))
+
+/* Make CMD format offset and page from a 32-bit flat address */
+#define		HFA384x_ADDR_CMD_MKPAGE(f) \
+		((u16)((((u32)(f)) & HFA384x_ADDR_FLAT_CMD_PAGE_MASK) >> 16))
+#define		HFA384x_ADDR_CMD_MKOFF(f) \
+		((u16)(((u32)(f)) & HFA384x_ADDR_FLAT_CMD_OFF_MASK))
+
+/*--- Controller Memory addresses -------------------*/
+#define		HFA3842_PDA_BASE	(0x007f0000UL)
+#define		HFA3841_PDA_BASE	(0x003f0000UL)
+#define		HFA3841_PDA_BOGUS_BASE	(0x00390000UL)
+
+/*--- Driver Download states  -----------------------*/
+#define		HFA384x_DLSTATE_DISABLED		0
+#define		HFA384x_DLSTATE_RAMENABLED		1
+#define		HFA384x_DLSTATE_FLASHENABLED		2
+
+/*--- Register Field Masks --------------------------*/
+#define		HFA384x_CMD_AINFO		((u16)GENMASK(14, 8))
+#define		HFA384x_CMD_MACPORT		((u16)GENMASK(10, 8))
+#define		HFA384x_CMD_PROGMODE		((u16)GENMASK(9, 8))
+#define		HFA384x_CMD_CMDCODE		((u16)GENMASK(5, 0))
+#define		HFA384x_STATUS_RESULT		((u16)GENMASK(14, 8))
+
+/*--- Command Code Constants --------------------------*/
+/*--- Controller Commands --------------------------*/
+#define		HFA384x_CMDCODE_INIT		((u16)0x00)
+#define		HFA384x_CMDCODE_ENABLE		((u16)0x01)
+#define		HFA384x_CMDCODE_DISABLE		((u16)0x02)
+
+/*--- Regulate Commands --------------------------*/
+#define		HFA384x_CMDCODE_INQ		((u16)0x11)
+
+/*--- Configure Commands --------------------------*/
+#define		HFA384x_CMDCODE_DOWNLD		((u16)0x22)
+
+/*--- Debugging Commands -----------------------------*/
+#define		HFA384x_CMDCODE_MONITOR		((u16)(0x38))
+#define		HFA384x_MONITOR_ENABLE		((u16)(0x0b))
+#define		HFA384x_MONITOR_DISABLE		((u16)(0x0f))
+
+/*--- Result Codes --------------------------*/
+#define		HFA384x_CMD_ERR			((u16)(0x7F))
+
+/*--- Programming Modes --------------------------
+ *	MODE 0: Disable programming
+ *	MODE 1: Enable volatile memory programming
+ *	MODE 2: Enable non-volatile memory programming
+ *	MODE 3: Program non-volatile memory section
+ *-------------------------------------------------
+ */
+#define		HFA384x_PROGMODE_DISABLE	((u16)0x00)
+#define		HFA384x_PROGMODE_RAM		((u16)0x01)
+#define		HFA384x_PROGMODE_NV		((u16)0x02)
+#define		HFA384x_PROGMODE_NVWRITE	((u16)0x03)
+
+/*--- Record ID Constants --------------------------*/
+/*--------------------------------------------------------------------
+ * Configuration RIDs: Network Parameters, Static Configuration Entities
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_CNFPORTTYPE		((u16)0xFC00)
+#define		HFA384x_RID_CNFOWNMACADDR	((u16)0xFC01)
+#define		HFA384x_RID_CNFDESIREDSSID	((u16)0xFC02)
+#define		HFA384x_RID_CNFOWNCHANNEL	((u16)0xFC03)
+#define		HFA384x_RID_CNFOWNSSID		((u16)0xFC04)
+#define		HFA384x_RID_CNFMAXDATALEN	((u16)0xFC07)
+
+/*--------------------------------------------------------------------
+ * Configuration RID lengths: Network Params, Static Config Entities
+ * This is the length of JUST the DATA part of the RID (does not
+ * include the len or code fields)
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_CNFOWNMACADDR_LEN	((u16)6)
+#define		HFA384x_RID_CNFDESIREDSSID_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNSSID_LEN	((u16)34)
+
+/*--------------------------------------------------------------------
+ * Configuration RIDs: Network Parameters, Dynamic Configuration Entities
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_CREATEIBSS		((u16)0xFC81)
+#define		HFA384x_RID_FRAGTHRESH		((u16)0xFC82)
+#define		HFA384x_RID_RTSTHRESH		((u16)0xFC83)
+#define		HFA384x_RID_TXRATECNTL		((u16)0xFC84)
+#define		HFA384x_RID_PROMISCMODE		((u16)0xFC85)
+
+/*----------------------------------------------------------------------
+ * Information RIDs: NIC Information
+ *----------------------------------------------------------------------
+ */
+#define		HFA384x_RID_MAXLOADTIME		((u16)0xFD00)
+#define		HFA384x_RID_DOWNLOADBUFFER	((u16)0xFD01)
+#define		HFA384x_RID_PRIIDENTITY		((u16)0xFD02)
+#define		HFA384x_RID_PRISUPRANGE		((u16)0xFD03)
+#define		HFA384x_RID_PRI_CFIACTRANGES	((u16)0xFD04)
+#define		HFA384x_RID_NICSERIALNUMBER	((u16)0xFD0A)
+#define		HFA384x_RID_NICIDENTITY		((u16)0xFD0B)
+#define		HFA384x_RID_MFISUPRANGE		((u16)0xFD0C)
+#define		HFA384x_RID_CFISUPRANGE		((u16)0xFD0D)
+#define		HFA384x_RID_STAIDENTITY		((u16)0xFD20)
+#define		HFA384x_RID_STASUPRANGE		((u16)0xFD21)
+#define		HFA384x_RID_STA_MFIACTRANGES	((u16)0xFD22)
+#define		HFA384x_RID_STA_CFIACTRANGES	((u16)0xFD23)
+
+/*----------------------------------------------------------------------
+ * Information RID Lengths: NIC Information
+ * This is the length of JUST the DATA part of the RID (does not
+ * include the len or code fields)
+ *---------------------------------------------------------------------
+ */
+#define		HFA384x_RID_NICSERIALNUMBER_LEN		((u16)12)
+
+/*--------------------------------------------------------------------
+ * Information RIDs:  MAC Information
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_PORTSTATUS		((u16)0xFD40)
+#define		HFA384x_RID_CURRENTSSID		((u16)0xFD41)
+#define		HFA384x_RID_CURRENTBSSID	((u16)0xFD42)
+#define		HFA384x_RID_CURRENTTXRATE	((u16)0xFD44)
+#define		HFA384x_RID_SHORTRETRYLIMIT	((u16)0xFD48)
+#define		HFA384x_RID_LONGRETRYLIMIT	((u16)0xFD49)
+#define		HFA384x_RID_MAXTXLIFETIME	((u16)0xFD4A)
+#define		HFA384x_RID_PRIVACYOPTIMP	((u16)0xFD4F)
+#define		HFA384x_RID_DBMCOMMSQUALITY	((u16)0xFD51)
+
+/*--------------------------------------------------------------------
+ * Information RID Lengths:  MAC Information
+ * This is the length of JUST the DATA part of the RID (does not
+ * include the len or code fields)
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_DBMCOMMSQUALITY_LEN	 \
+	((u16)sizeof(struct hfa384x_dbmcommsquality))
+#define		HFA384x_RID_JOINREQUEST_LEN \
+	((u16)sizeof(struct hfa384x_join_request_data))
+
+/*--------------------------------------------------------------------
+ * Information RIDs:  Modem Information
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_CURRENTCHANNEL	((u16)0xFDC1)
+
+/*--------------------------------------------------------------------
+ * API ENHANCEMENTS (NOT ALREADY IMPLEMENTED)
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RID_CNFWEPDEFAULTKEYID	((u16)0xFC23)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY0	((u16)0xFC24)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY1	((u16)0xFC25)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY2	((u16)0xFC26)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY3	((u16)0xFC27)
+#define		HFA384x_RID_CNFWEPFLAGS		((u16)0xFC28)
+#define		HFA384x_RID_CNFAUTHENTICATION	((u16)0xFC2A)
+#define		HFA384x_RID_CNFROAMINGMODE	((u16)0xFC2D)
+#define		HFA384x_RID_CNFAPBCNINT		((u16)0xFC33)
+#define		HFA384x_RID_CNFDBMADJUST	((u16)0xFC46)
+#define		HFA384x_RID_CNFWPADATA		((u16)0xFC48)
+#define		HFA384x_RID_CNFBASICRATES	((u16)0xFCB3)
+#define		HFA384x_RID_CNFSUPPRATES	((u16)0xFCB4)
+#define		HFA384x_RID_CNFPASSIVESCANCTRL	((u16)0xFCBA)
+#define		HFA384x_RID_TXPOWERMAX		((u16)0xFCBE)
+#define		HFA384x_RID_JOINREQUEST		((u16)0xFCE2)
+#define		HFA384x_RID_AUTHENTICATESTA	((u16)0xFCE3)
+#define		HFA384x_RID_HOSTSCAN		((u16)0xFCE5)
+
+#define		HFA384x_RID_CNFWEPDEFAULTKEY_LEN	((u16)6)
+#define		HFA384x_RID_CNFWEP128DEFAULTKEY_LEN	((u16)14)
+
+/*--------------------------------------------------------------------
+ * PD Record codes
+ *--------------------------------------------------------------------
+ */
+#define HFA384x_PDR_PCB_PARTNUM		((u16)0x0001)
+#define HFA384x_PDR_PDAVER		((u16)0x0002)
+#define HFA384x_PDR_NIC_SERIAL		((u16)0x0003)
+#define HFA384x_PDR_MKK_MEASUREMENTS	((u16)0x0004)
+#define HFA384x_PDR_NIC_RAMSIZE		((u16)0x0005)
+#define HFA384x_PDR_MFISUPRANGE		((u16)0x0006)
+#define HFA384x_PDR_CFISUPRANGE		((u16)0x0007)
+#define HFA384x_PDR_NICID		((u16)0x0008)
+#define HFA384x_PDR_MAC_ADDRESS		((u16)0x0101)
+#define HFA384x_PDR_REGDOMAIN		((u16)0x0103)
+#define HFA384x_PDR_ALLOWED_CHANNEL	((u16)0x0104)
+#define HFA384x_PDR_DEFAULT_CHANNEL	((u16)0x0105)
+#define HFA384x_PDR_TEMPTYPE		((u16)0x0107)
+#define HFA384x_PDR_IFR_SETTING		((u16)0x0200)
+#define HFA384x_PDR_RFR_SETTING		((u16)0x0201)
+#define HFA384x_PDR_HFA3861_BASELINE	((u16)0x0202)
+#define HFA384x_PDR_HFA3861_SHADOW	((u16)0x0203)
+#define HFA384x_PDR_HFA3861_IFRF	((u16)0x0204)
+#define HFA384x_PDR_HFA3861_CHCALSP	((u16)0x0300)
+#define HFA384x_PDR_HFA3861_CHCALI	((u16)0x0301)
+#define HFA384x_PDR_MAX_TX_POWER	((u16)0x0302)
+#define HFA384x_PDR_MASTER_CHAN_LIST	((u16)0x0303)
+#define HFA384x_PDR_3842_NIC_CONFIG	((u16)0x0400)
+#define HFA384x_PDR_USB_ID		((u16)0x0401)
+#define HFA384x_PDR_PCI_ID		((u16)0x0402)
+#define HFA384x_PDR_PCI_IFCONF		((u16)0x0403)
+#define HFA384x_PDR_PCI_PMCONF		((u16)0x0404)
+#define HFA384x_PDR_RFENRGY		((u16)0x0406)
+#define HFA384x_PDR_USB_POWER_TYPE      ((u16)0x0407)
+#define HFA384x_PDR_USB_MAX_POWER	((u16)0x0409)
+#define HFA384x_PDR_USB_MANUFACTURER	((u16)0x0410)
+#define HFA384x_PDR_USB_PRODUCT		((u16)0x0411)
+#define HFA384x_PDR_ANT_DIVERSITY	((u16)0x0412)
+#define HFA384x_PDR_HFO_DELAY		((u16)0x0413)
+#define HFA384x_PDR_SCALE_THRESH	((u16)0x0414)
+
+#define HFA384x_PDR_HFA3861_MANF_TESTSP	((u16)0x0900)
+#define HFA384x_PDR_HFA3861_MANF_TESTI	((u16)0x0901)
+#define HFA384x_PDR_END_OF_PDA		((u16)0x0000)
+
+/*--- Register Test/Get/Set Field macros ------------------------*/
+
+#define		HFA384x_CMD_AINFO_SET(value)	((u16)((u16)(value) << 8))
+#define		HFA384x_CMD_MACPORT_SET(value)	\
+			((u16)HFA384x_CMD_AINFO_SET(value))
+#define		HFA384x_CMD_PROGMODE_SET(value)	\
+			((u16)HFA384x_CMD_AINFO_SET((u16)value))
+#define		HFA384x_CMD_CMDCODE_SET(value)		((u16)(value))
+
+#define		HFA384x_STATUS_RESULT_SET(value)	(((u16)(value)) << 8)
+
+/* Host Maintained State Info */
+#define HFA384x_STATE_PREINIT	0
+#define HFA384x_STATE_INIT	1
+#define HFA384x_STATE_RUNNING	2
+
+/*-------------------------------------------------------------*/
+/* Commonly used basic types */
+struct hfa384x_bytestr {
+	__le16 len;
+	u8 data[];
+} __packed;
+
+struct hfa384x_bytestr32 {
+	__le16 len;
+	u8 data[32];
+} __packed;
+
+/*--------------------------------------------------------------------
+ * Configuration Record Structures:
+ *	Network Parameters, Static Configuration Entities
+ *--------------------------------------------------------------------
+ */
+
+/*-- Hardware/Firmware Component Information ----------*/
+struct hfa384x_compident {
+	u16 id;
+	u16 variant;
+	u16 major;
+	u16 minor;
+} __packed;
+
+struct hfa384x_caplevel {
+	u16 role;
+	u16 id;
+	u16 variant;
+	u16 bottom;
+	u16 top;
+} __packed;
+
+/*-- Configuration Record: cnfAuthentication --*/
+#define HFA384x_CNFAUTHENTICATION_OPENSYSTEM	0x0001
+#define HFA384x_CNFAUTHENTICATION_SHAREDKEY	0x0002
+#define HFA384x_CNFAUTHENTICATION_LEAP		0x0004
+
+/*--------------------------------------------------------------------
+ * Configuration Record Structures:
+ *	Network Parameters, Dynamic Configuration Entities
+ *--------------------------------------------------------------------
+ */
+
+#define HFA384x_CREATEIBSS_JOINCREATEIBSS          0
+
+/*-- Configuration Record: HostScanRequest (data portion only) --*/
+struct hfa384x_host_scan_request_data {
+	__le16 channel_list;
+	__le16 tx_rate;
+	struct hfa384x_bytestr32 ssid;
+} __packed;
+
+/*-- Configuration Record: JoinRequest (data portion only) --*/
+struct hfa384x_join_request_data {
+	u8 bssid[WLAN_BSSID_LEN];
+	u16 channel;
+} __packed;
+
+/*-- Configuration Record: authenticateStation (data portion only) --*/
+struct hfa384x_authenticate_station_data {
+	u8 address[ETH_ALEN];
+	__le16 status;
+	__le16 algorithm;
+} __packed;
+
+/*-- Configuration Record: WPAData       (data portion only) --*/
+struct hfa384x_wpa_data {
+	__le16 datalen;
+	u8 data[];		/* max 80 */
+} __packed;
+
+/*--------------------------------------------------------------------
+ * Information Record Structures: NIC Information
+ *--------------------------------------------------------------------
+ */
+
+/*-- Information Record: DownLoadBuffer --*/
+/* NOTE: The page and offset are in AUX format */
+struct hfa384x_downloadbuffer {
+	u16 page;
+	u16 offset;
+	u16 len;
+} __packed;
+
+/*--------------------------------------------------------------------
+ * Information Record Structures: NIC Information
+ *--------------------------------------------------------------------
+ */
+
+#define HFA384x_PSTATUS_CONN_IBSS	((u16)3)
+
+/*-- Information Record: commsquality --*/
+struct hfa384x_commsquality {
+	__le16 cq_curr_bss;
+	__le16 asl_curr_bss;
+	__le16 anl_curr_fc;
+} __packed;
+
+/*-- Information Record: dmbcommsquality --*/
+struct hfa384x_dbmcommsquality {
+	u16 cq_dbm_curr_bss;
+	u16 asl_dbm_curr_bss;
+	u16 anl_dbm_curr_fc;
+} __packed;
+
+/*--------------------------------------------------------------------
+ * FRAME STRUCTURES: Communication Frames
+ *--------------------------------------------------------------------
+ * Communication Frames: Transmit Frames
+ *--------------------------------------------------------------------
+ */
+/*-- Communication Frame: Transmit Frame Structure --*/
+struct hfa384x_tx_frame {
+	u16 status;
+	u16 reserved1;
+	u16 reserved2;
+	u32 sw_support;
+	u8 tx_retrycount;
+	u8 tx_rate;
+	u16 tx_control;
+
+	/*-- 802.11 Header Information --*/
+	struct p80211_hdr hdr;
+	__le16 data_len;		/* little endian format */
+
+	/*-- 802.3 Header Information --*/
+
+	u8 dest_addr[6];
+	u8 src_addr[6];
+	u16 data_length;	/* big endian format */
+} __packed;
+/*--------------------------------------------------------------------
+ * Communication Frames: Field Masks for Transmit Frames
+ *--------------------------------------------------------------------
+ */
+/*-- Status Field --*/
+#define		HFA384x_TXSTATUS_ACKERR			((u16)BIT(5))
+#define		HFA384x_TXSTATUS_FORMERR		((u16)BIT(3))
+#define		HFA384x_TXSTATUS_DISCON			((u16)BIT(2))
+#define		HFA384x_TXSTATUS_AGEDERR		((u16)BIT(1))
+#define		HFA384x_TXSTATUS_RETRYERR		((u16)BIT(0))
+/*-- Transmit Control Field --*/
+#define		HFA384x_TX_MACPORT			((u16)GENMASK(10, 8))
+#define		HFA384x_TX_STRUCTYPE			((u16)GENMASK(4, 3))
+#define		HFA384x_TX_TXEX				((u16)BIT(2))
+#define		HFA384x_TX_TXOK				((u16)BIT(1))
+/*--------------------------------------------------------------------
+ * Communication Frames: Test/Get/Set Field Values for Transmit Frames
+ *--------------------------------------------------------------------
+ */
+/*-- Status Field --*/
+#define HFA384x_TXSTATUS_ISERROR(v)	\
+	(((u16)(v)) & \
+	(HFA384x_TXSTATUS_ACKERR | HFA384x_TXSTATUS_FORMERR | \
+	HFA384x_TXSTATUS_DISCON | HFA384x_TXSTATUS_AGEDERR | \
+	HFA384x_TXSTATUS_RETRYERR))
+
+#define	HFA384x_TX_SET(v, m, s)		((((u16)(v)) << ((u16)(s))) & ((u16)(m)))
+
+#define	HFA384x_TX_MACPORT_SET(v)	HFA384x_TX_SET(v, HFA384x_TX_MACPORT, 8)
+#define	HFA384x_TX_STRUCTYPE_SET(v)	HFA384x_TX_SET(v, \
+						HFA384x_TX_STRUCTYPE, 3)
+#define	HFA384x_TX_TXEX_SET(v)		HFA384x_TX_SET(v, HFA384x_TX_TXEX, 2)
+#define	HFA384x_TX_TXOK_SET(v)		HFA384x_TX_SET(v, HFA384x_TX_TXOK, 1)
+/*--------------------------------------------------------------------
+ * Communication Frames: Receive Frames
+ *--------------------------------------------------------------------
+ */
+/*-- Communication Frame: Receive Frame Structure --*/
+struct hfa384x_rx_frame {
+	/*-- MAC rx descriptor (hfa384x byte order) --*/
+	u16 status;
+	u32 time;
+	u8 silence;
+	u8 signal;
+	u8 rate;
+	u8 rx_flow;
+	u16 reserved1;
+	u16 reserved2;
+
+	/*-- 802.11 Header Information (802.11 byte order) --*/
+	struct p80211_hdr hdr;
+	__le16 data_len;		/* hfa384x (little endian) format */
+
+	/*-- 802.3 Header Information --*/
+	u8 dest_addr[6];
+	u8 src_addr[6];
+	u16 data_length;	/* IEEE? (big endian) format */
+} __packed;
+/*--------------------------------------------------------------------
+ * Communication Frames: Field Masks for Receive Frames
+ *--------------------------------------------------------------------
+ */
+
+/*-- Status Fields --*/
+#define		HFA384x_RXSTATUS_MACPORT		((u16)GENMASK(10, 8))
+#define		HFA384x_RXSTATUS_FCSERR			((u16)BIT(0))
+/*--------------------------------------------------------------------
+ * Communication Frames: Test/Get/Set Field Values for Receive Frames
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_RXSTATUS_MACPORT_GET(value)	((u16)((((u16)(value)) \
+					    & HFA384x_RXSTATUS_MACPORT) >> 8))
+#define		HFA384x_RXSTATUS_ISFCSERR(value)	((u16)(((u16)(value)) \
+						  & HFA384x_RXSTATUS_FCSERR))
+/*--------------------------------------------------------------------
+ * FRAME STRUCTURES: Information Types and Information Frame Structures
+ *--------------------------------------------------------------------
+ * Information Types
+ *--------------------------------------------------------------------
+ */
+#define		HFA384x_IT_HANDOVERADDR			((u16)0xF000UL)
+#define		HFA384x_IT_COMMTALLIES			((u16)0xF100UL)
+#define		HFA384x_IT_SCANRESULTS			((u16)0xF101UL)
+#define		HFA384x_IT_CHINFORESULTS		((u16)0xF102UL)
+#define		HFA384x_IT_HOSTSCANRESULTS		((u16)0xF103UL)
+#define		HFA384x_IT_LINKSTATUS			((u16)0xF200UL)
+#define		HFA384x_IT_ASSOCSTATUS			((u16)0xF201UL)
+#define		HFA384x_IT_AUTHREQ			((u16)0xF202UL)
+#define		HFA384x_IT_PSUSERCNT			((u16)0xF203UL)
+#define		HFA384x_IT_KEYIDCHANGED			((u16)0xF204UL)
+#define		HFA384x_IT_ASSOCREQ			((u16)0xF205UL)
+#define		HFA384x_IT_MICFAILURE			((u16)0xF206UL)
+
+/*--------------------------------------------------------------------
+ * Information Frames Structures
+ *--------------------------------------------------------------------
+ * Information Frames: Notification Frame Structures
+ *--------------------------------------------------------------------
+ */
+
+/*--  Inquiry Frame, Diagnose: Communication Tallies --*/
+struct hfa384x_comm_tallies_16 {
+	__le16 txunicastframes;
+	__le16 txmulticastframes;
+	__le16 txfragments;
+	__le16 txunicastoctets;
+	__le16 txmulticastoctets;
+	__le16 txdeferredtrans;
+	__le16 txsingleretryframes;
+	__le16 txmultipleretryframes;
+	__le16 txretrylimitexceeded;
+	__le16 txdiscards;
+	__le16 rxunicastframes;
+	__le16 rxmulticastframes;
+	__le16 rxfragments;
+	__le16 rxunicastoctets;
+	__le16 rxmulticastoctets;
+	__le16 rxfcserrors;
+	__le16 rxdiscardsnobuffer;
+	__le16 txdiscardswrongsa;
+	__le16 rxdiscardswepundecr;
+	__le16 rxmsginmsgfrag;
+	__le16 rxmsginbadmsgfrag;
+} __packed;
+
+struct hfa384x_comm_tallies_32 {
+	__le32 txunicastframes;
+	__le32 txmulticastframes;
+	__le32 txfragments;
+	__le32 txunicastoctets;
+	__le32 txmulticastoctets;
+	__le32 txdeferredtrans;
+	__le32 txsingleretryframes;
+	__le32 txmultipleretryframes;
+	__le32 txretrylimitexceeded;
+	__le32 txdiscards;
+	__le32 rxunicastframes;
+	__le32 rxmulticastframes;
+	__le32 rxfragments;
+	__le32 rxunicastoctets;
+	__le32 rxmulticastoctets;
+	__le32 rxfcserrors;
+	__le32 rxdiscardsnobuffer;
+	__le32 txdiscardswrongsa;
+	__le32 rxdiscardswepundecr;
+	__le32 rxmsginmsgfrag;
+	__le32 rxmsginbadmsgfrag;
+} __packed;
+
+/*--  Inquiry Frame, Diagnose: Scan Results & Subfields--*/
+struct hfa384x_scan_result_sub {
+	u16 chid;
+	u16 anl;
+	u16 sl;
+	u8 bssid[WLAN_BSSID_LEN];
+	u16 bcnint;
+	u16 capinfo;
+	struct hfa384x_bytestr32 ssid;
+	u8 supprates[10];	/* 802.11 info element */
+	u16 proberesp_rate;
+} __packed;
+
+struct hfa384x_scan_result {
+	u16 rsvd;
+	u16 scanreason;
+	struct hfa384x_scan_result_sub result[HFA384x_SCANRESULT_MAX];
+} __packed;
+
+/*--  Inquiry Frame, Diagnose: ChInfo Results & Subfields--*/
+struct hfa384x_ch_info_result_sub {
+	u16 chid;
+	u16 anl;
+	u16 pnl;
+	u16 active;
+} __packed;
+
+#define HFA384x_CHINFORESULT_BSSACTIVE	BIT(0)
+#define HFA384x_CHINFORESULT_PCFACTIVE	BIT(1)
+
+struct hfa384x_ch_info_result {
+	u16 scanchannels;
+	struct hfa384x_ch_info_result_sub result[HFA384x_CHINFORESULT_MAX];
+} __packed;
+
+/*--  Inquiry Frame, Diagnose: Host Scan Results & Subfields--*/
+struct hfa384x_hscan_result_sub {
+	__le16 chid;
+	__le16 anl;
+	__le16 sl;
+	u8 bssid[WLAN_BSSID_LEN];
+	__le16 bcnint;
+	__le16 capinfo;
+	struct hfa384x_bytestr32 ssid;
+	u8 supprates[10];	/* 802.11 info element */
+	u16 proberesp_rate;
+	__le16 atim;
+} __packed;
+
+struct hfa384x_hscan_result {
+	u16 nresult;
+	u16 rsvd;
+	struct hfa384x_hscan_result_sub result[HFA384x_HSCANRESULT_MAX];
+} __packed;
+
+/*--  Unsolicited Frame, MAC Mgmt: LinkStatus --*/
+
+#define HFA384x_LINK_NOTCONNECTED	((u16)0)
+#define HFA384x_LINK_CONNECTED		((u16)1)
+#define HFA384x_LINK_DISCONNECTED	((u16)2)
+#define HFA384x_LINK_AP_CHANGE		((u16)3)
+#define HFA384x_LINK_AP_OUTOFRANGE	((u16)4)
+#define HFA384x_LINK_AP_INRANGE		((u16)5)
+#define HFA384x_LINK_ASSOCFAIL		((u16)6)
+
+struct hfa384x_link_status {
+	__le16 linkstatus;
+} __packed;
+
+/*--  Unsolicited Frame, MAC Mgmt: AssociationStatus (--*/
+
+#define HFA384x_ASSOCSTATUS_STAASSOC	((u16)1)
+#define HFA384x_ASSOCSTATUS_REASSOC	((u16)2)
+#define HFA384x_ASSOCSTATUS_AUTHFAIL	((u16)5)
+
+struct hfa384x_assoc_status {
+	u16 assocstatus;
+	u8 sta_addr[ETH_ALEN];
+	/* old_ap_addr is only valid if assocstatus == 2 */
+	u8 old_ap_addr[ETH_ALEN];
+	u16 reason;
+	u16 reserved;
+} __packed;
+
+/*--  Unsolicited Frame, MAC Mgmt: AuthRequest (AP Only) --*/
+
+struct hfa384x_auth_request {
+	u8 sta_addr[ETH_ALEN];
+	__le16 algorithm;
+} __packed;
+
+/*--  Unsolicited Frame, MAC Mgmt: PSUserCount (AP Only) --*/
+
+struct hfa384x_ps_user_count {
+	__le16 usercnt;
+} __packed;
+
+struct hfa384x_key_id_changed {
+	u8 sta_addr[ETH_ALEN];
+	u16 keyid;
+} __packed;
+
+/*--  Collection of all Inf frames ---------------*/
+union hfa384x_infodata {
+	struct hfa384x_comm_tallies_16 commtallies16;
+	struct hfa384x_comm_tallies_32 commtallies32;
+	struct hfa384x_scan_result scanresult;
+	struct hfa384x_ch_info_result chinforesult;
+	struct hfa384x_hscan_result hscanresult;
+	struct hfa384x_link_status linkstatus;
+	struct hfa384x_assoc_status assocstatus;
+	struct hfa384x_auth_request authreq;
+	struct hfa384x_ps_user_count psusercnt;
+	struct hfa384x_key_id_changed keyidchanged;
+} __packed;
+
+struct hfa384x_inf_frame {
+	u16 framelen;
+	u16 infotype;
+	union hfa384x_infodata info;
+} __packed;
+
+/*--------------------------------------------------------------------
+ * USB Packet structures and constants.
+ *--------------------------------------------------------------------
+ */
+
+/* Should be sent to the bulkout endpoint */
+#define HFA384x_USB_TXFRM	0
+#define HFA384x_USB_CMDREQ	1
+#define HFA384x_USB_WRIDREQ	2
+#define HFA384x_USB_RRIDREQ	3
+#define HFA384x_USB_WMEMREQ	4
+#define HFA384x_USB_RMEMREQ	5
+
+/* Received from the bulkin endpoint */
+#define HFA384x_USB_ISTXFRM(a)	(((a) & 0x9000) == 0x1000)
+#define HFA384x_USB_ISRXFRM(a)	(!((a) & 0x9000))
+#define HFA384x_USB_INFOFRM	0x8000
+#define HFA384x_USB_CMDRESP	0x8001
+#define HFA384x_USB_WRIDRESP	0x8002
+#define HFA384x_USB_RRIDRESP	0x8003
+#define HFA384x_USB_WMEMRESP	0x8004
+#define HFA384x_USB_RMEMRESP	0x8005
+#define HFA384x_USB_BUFAVAIL	0x8006
+#define HFA384x_USB_ERROR	0x8007
+
+/*------------------------------------*/
+/* Request (bulk OUT) packet contents */
+
+struct hfa384x_usb_txfrm {
+	struct hfa384x_tx_frame desc;
+	u8 data[WLAN_DATA_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_cmdreq {
+	__le16 type;
+	__le16 cmd;
+	__le16 parm0;
+	__le16 parm1;
+	__le16 parm2;
+	u8 pad[54];
+} __packed;
+
+struct hfa384x_usb_wridreq {
+	__le16 type;
+	__le16 frmlen;
+	__le16 rid;
+	u8 data[HFA384x_RIDDATA_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_rridreq {
+	__le16 type;
+	__le16 frmlen;
+	__le16 rid;
+	u8 pad[58];
+} __packed;
+
+struct hfa384x_usb_wmemreq {
+	__le16 type;
+	__le16 frmlen;
+	__le16 offset;
+	__le16 page;
+	u8 data[HFA384x_USB_RWMEM_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_rmemreq {
+	__le16 type;
+	__le16 frmlen;
+	__le16 offset;
+	__le16 page;
+	u8 pad[56];
+} __packed;
+
+/*------------------------------------*/
+/* Response (bulk IN) packet contents */
+
+struct hfa384x_usb_rxfrm {
+	struct hfa384x_rx_frame desc;
+	u8 data[WLAN_DATA_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_infofrm {
+	u16 type;
+	struct hfa384x_inf_frame info;
+} __packed;
+
+struct hfa384x_usb_statusresp {
+	u16 type;
+	__le16 status;
+	__le16 resp0;
+	__le16 resp1;
+	__le16 resp2;
+} __packed;
+
+struct hfa384x_usb_rridresp {
+	u16 type;
+	__le16 frmlen;
+	__le16 rid;
+	u8 data[HFA384x_RIDDATA_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_rmemresp {
+	u16 type;
+	u16 frmlen;
+	u8 data[HFA384x_USB_RWMEM_MAXLEN];
+} __packed;
+
+struct hfa384x_usb_bufavail {
+	u16 type;
+	u16 frmlen;
+} __packed;
+
+struct hfa384x_usb_error {
+	u16 type;
+	u16 errortype;
+} __packed;
+
+/*----------------------------------------------------------*/
+/* Unions for packaging all the known packet types together */
+
+union hfa384x_usbout {
+	__le16 type;
+	struct hfa384x_usb_txfrm txfrm;
+	struct hfa384x_usb_cmdreq cmdreq;
+	struct hfa384x_usb_wridreq wridreq;
+	struct hfa384x_usb_rridreq rridreq;
+	struct hfa384x_usb_wmemreq wmemreq;
+	struct hfa384x_usb_rmemreq rmemreq;
+} __packed;
+
+union hfa384x_usbin {
+	__le16 type;
+	struct hfa384x_usb_rxfrm rxfrm;
+	struct hfa384x_usb_txfrm txfrm;
+	struct hfa384x_usb_infofrm infofrm;
+	struct hfa384x_usb_statusresp cmdresp;
+	struct hfa384x_usb_statusresp wridresp;
+	struct hfa384x_usb_rridresp rridresp;
+	struct hfa384x_usb_statusresp wmemresp;
+	struct hfa384x_usb_rmemresp rmemresp;
+	struct hfa384x_usb_bufavail bufavail;
+	struct hfa384x_usb_error usberror;
+	u8 boguspad[3000];
+} __packed;
+
+/*--------------------------------------------------------------------
+ * PD record structures.
+ *--------------------------------------------------------------------
+ */
+
+struct hfa384x_pdr_pcb_partnum {
+	u8 num[8];
+} __packed;
+
+struct hfa384x_pdr_pcb_tracenum {
+	u8 num[8];
+} __packed;
+
+struct hfa384x_pdr_nic_serial {
+	u8 num[12];
+} __packed;
+
+struct hfa384x_pdr_mkk_measurements {
+	double carrier_freq;
+	double occupied_band;
+	double power_density;
+	double tx_spur_f1;
+	double tx_spur_f2;
+	double tx_spur_f3;
+	double tx_spur_f4;
+	double tx_spur_l1;
+	double tx_spur_l2;
+	double tx_spur_l3;
+	double tx_spur_l4;
+	double rx_spur_f1;
+	double rx_spur_f2;
+	double rx_spur_l1;
+	double rx_spur_l2;
+} __packed;
+
+struct hfa384x_pdr_nic_ramsize {
+	u8 size[12];		/* units of KB */
+} __packed;
+
+struct hfa384x_pdr_mfisuprange {
+	u16 id;
+	u16 variant;
+	u16 bottom;
+	u16 top;
+} __packed;
+
+struct hfa384x_pdr_cfisuprange {
+	u16 id;
+	u16 variant;
+	u16 bottom;
+	u16 top;
+} __packed;
+
+struct hfa384x_pdr_nicid {
+	u16 id;
+	u16 variant;
+	u16 major;
+	u16 minor;
+} __packed;
+
+struct hfa384x_pdr_refdac_measurements {
+	u16 value[0];
+} __packed;
+
+struct hfa384x_pdr_vgdac_measurements {
+	u16 value[0];
+} __packed;
+
+struct hfa384x_pdr_level_comp_measurements {
+	u16 value[0];
+} __packed;
+
+struct hfa384x_pdr_mac_address {
+	u8 addr[6];
+} __packed;
+
+struct hfa384x_pdr_mkk_callname {
+	u8 callname[8];
+} __packed;
+
+struct hfa384x_pdr_regdomain {
+	u16 numdomains;
+	u16 domain[5];
+} __packed;
+
+struct hfa384x_pdr_allowed_channel {
+	u16 ch_bitmap;
+} __packed;
+
+struct hfa384x_pdr_default_channel {
+	u16 channel;
+} __packed;
+
+struct hfa384x_pdr_privacy_option {
+	u16 available;
+} __packed;
+
+struct hfa384x_pdr_temptype {
+	u16 type;
+} __packed;
+
+struct hfa384x_pdr_refdac_setup {
+	u16 ch_value[14];
+} __packed;
+
+struct hfa384x_pdr_vgdac_setup {
+	u16 ch_value[14];
+} __packed;
+
+struct hfa384x_pdr_level_comp_setup {
+	u16 ch_value[14];
+} __packed;
+
+struct hfa384x_pdr_trimdac_setup {
+	u16 trimidac;
+	u16 trimqdac;
+} __packed;
+
+struct hfa384x_pdr_ifr_setting {
+	u16 value[3];
+} __packed;
+
+struct hfa384x_pdr_rfr_setting {
+	u16 value[3];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_baseline {
+	u16 value[50];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_shadow {
+	u32 value[32];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_ifrf {
+	u32 value[20];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_chcalsp {
+	u16 value[14];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_chcali {
+	u16 value[17];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_nic_config {
+	u16 config_bitmap;
+} __packed;
+
+struct hfa384x_pdr_hfo_delay {
+	u8 hfo_delay;
+} __packed;
+
+struct hfa384x_pdr_hfa3861_manf_testsp {
+	u16 value[30];
+} __packed;
+
+struct hfa384x_pdr_hfa3861_manf_testi {
+	u16 value[30];
+} __packed;
+
+struct hfa384x_pdr_end_of_pda {
+	u16 crc;
+} __packed;
+
+struct hfa384x_pdrec {
+	__le16 len;		/* in words */
+	__le16 code;
+	union pdr {
+		struct hfa384x_pdr_pcb_partnum pcb_partnum;
+		struct hfa384x_pdr_pcb_tracenum pcb_tracenum;
+		struct hfa384x_pdr_nic_serial nic_serial;
+		struct hfa384x_pdr_mkk_measurements mkk_measurements;
+		struct hfa384x_pdr_nic_ramsize nic_ramsize;
+		struct hfa384x_pdr_mfisuprange mfisuprange;
+		struct hfa384x_pdr_cfisuprange cfisuprange;
+		struct hfa384x_pdr_nicid nicid;
+		struct hfa384x_pdr_refdac_measurements refdac_measurements;
+		struct hfa384x_pdr_vgdac_measurements vgdac_measurements;
+		struct hfa384x_pdr_level_comp_measurements level_compc_measurements;
+		struct hfa384x_pdr_mac_address mac_address;
+		struct hfa384x_pdr_mkk_callname mkk_callname;
+		struct hfa384x_pdr_regdomain regdomain;
+		struct hfa384x_pdr_allowed_channel allowed_channel;
+		struct hfa384x_pdr_default_channel default_channel;
+		struct hfa384x_pdr_privacy_option privacy_option;
+		struct hfa384x_pdr_temptype temptype;
+		struct hfa384x_pdr_refdac_setup refdac_setup;
+		struct hfa384x_pdr_vgdac_setup vgdac_setup;
+		struct hfa384x_pdr_level_comp_setup level_comp_setup;
+		struct hfa384x_pdr_trimdac_setup trimdac_setup;
+		struct hfa384x_pdr_ifr_setting ifr_setting;
+		struct hfa384x_pdr_rfr_setting rfr_setting;
+		struct hfa384x_pdr_hfa3861_baseline hfa3861_baseline;
+		struct hfa384x_pdr_hfa3861_shadow hfa3861_shadow;
+		struct hfa384x_pdr_hfa3861_ifrf hfa3861_ifrf;
+		struct hfa384x_pdr_hfa3861_chcalsp hfa3861_chcalsp;
+		struct hfa384x_pdr_hfa3861_chcali hfa3861_chcali;
+		struct hfa384x_pdr_hfa3861_nic_config nic_config;
+		struct hfa384x_pdr_hfo_delay hfo_delay;
+		struct hfa384x_pdr_hfa3861_manf_testsp hfa3861_manf_testsp;
+		struct hfa384x_pdr_hfa3861_manf_testi hfa3861_manf_testi;
+		struct hfa384x_pdr_end_of_pda end_of_pda;
+
+	} data;
+} __packed;
+
+#ifdef __KERNEL__
+/*--------------------------------------------------------------------
+ * ---  MAC state structure, argument to all functions --
+ * ---  Also, a collection of support types --
+ *--------------------------------------------------------------------
+ */
+struct hfa384x_cmdresult {
+	u16 status;
+	u16 resp0;
+	u16 resp1;
+	u16 resp2;
+};
+
+/* USB Control Exchange (CTLX):
+ *  A queue of the structure below is maintained for all of the
+ *  Request/Response type USB packets supported by Prism2.
+ */
+/* The following hfa384x_* structures are arguments to
+ * the usercb() for the different CTLX types.
+ */
+struct hfa384x_rridresult {
+	u16 rid;
+	const void *riddata;
+	unsigned int riddata_len;
+};
+
+enum ctlx_state {
+	CTLX_START = 0,		/* Start state, not queued */
+
+	CTLX_COMPLETE,		/* CTLX successfully completed */
+	CTLX_REQ_FAILED,	/* OUT URB completed w/ error */
+
+	CTLX_PENDING,		/* Queued, data valid */
+	CTLX_REQ_SUBMITTED,	/* OUT URB submitted */
+	CTLX_REQ_COMPLETE,	/* OUT URB complete */
+	CTLX_RESP_COMPLETE	/* IN URB received */
+};
+
+struct hfa384x_usbctlx;
+struct hfa384x;
+
+typedef void (*ctlx_cmdcb_t) (struct hfa384x *, const struct hfa384x_usbctlx *);
+
+typedef void (*ctlx_usercb_t) (struct hfa384x *hw,
+			       void *ctlxresult, void *usercb_data);
+
+struct hfa384x_usbctlx {
+	struct list_head list;
+
+	size_t outbufsize;
+	union hfa384x_usbout outbuf;	/* pkt buf for OUT */
+	union hfa384x_usbin inbuf;	/* pkt buf for IN(a copy) */
+
+	enum ctlx_state state;	/* Tracks running state */
+
+	struct completion done;
+	int reapable;		/* Food for the reaper task */
+
+	ctlx_cmdcb_t cmdcb;	/* Async command callback */
+	ctlx_usercb_t usercb;	/* Async user callback, */
+	void *usercb_data;	/*  at CTLX completion  */
+};
+
+struct hfa384x_usbctlxq {
+	spinlock_t lock;
+	struct list_head pending;
+	struct list_head active;
+	struct list_head completing;
+	struct list_head reapable;
+};
+
+struct hfa384x_metacmd {
+	u16 cmd;
+
+	u16 parm0;
+	u16 parm1;
+	u16 parm2;
+
+	struct hfa384x_cmdresult result;
+};
+
+#define	MAX_GRP_ADDR		32
+#define WLAN_COMMENT_MAX	80  /* Max. length of user comment string. */
+
+#define WLAN_AUTH_MAX           60  /* Max. # of authenticated stations. */
+#define WLAN_ACCESS_MAX		60  /* Max. # of stations in an access list. */
+#define WLAN_ACCESS_NONE	0   /* No stations may be authenticated. */
+#define WLAN_ACCESS_ALL		1   /* All stations may be authenticated. */
+#define WLAN_ACCESS_ALLOW	2   /* Authenticate only "allowed" stations. */
+#define WLAN_ACCESS_DENY	3   /* Do not authenticate "denied" stations. */
+
+/* XXX These are going away ASAP */
+struct prism2sta_authlist {
+	unsigned int cnt;
+	u8 addr[WLAN_AUTH_MAX][ETH_ALEN];
+	u8 assoc[WLAN_AUTH_MAX];
+};
+
+struct prism2sta_accesslist {
+	unsigned int modify;
+	unsigned int cnt;
+	u8 addr[WLAN_ACCESS_MAX][ETH_ALEN];
+	unsigned int cnt1;
+	u8 addr1[WLAN_ACCESS_MAX][ETH_ALEN];
+};
+
+struct hfa384x {
+	/* USB support data */
+	struct usb_device *usb;
+	struct urb rx_urb;
+	struct sk_buff *rx_urb_skb;
+	struct urb tx_urb;
+	struct urb ctlx_urb;
+	union hfa384x_usbout txbuff;
+	struct hfa384x_usbctlxq ctlxq;
+	struct timer_list reqtimer;
+	struct timer_list resptimer;
+
+	struct timer_list throttle;
+
+	struct work_struct reaper_bh;
+	struct work_struct completion_bh;
+
+	struct work_struct usb_work;
+
+	unsigned long usb_flags;
+#define THROTTLE_RX	0
+#define THROTTLE_TX	1
+#define WORK_RX_HALT	2
+#define WORK_TX_HALT	3
+#define WORK_RX_RESUME	4
+#define WORK_TX_RESUME	5
+
+	unsigned short req_timer_done:1;
+	unsigned short resp_timer_done:1;
+
+	int endp_in;
+	int endp_out;
+
+	int sniff_fcs;
+	int sniff_channel;
+	int sniff_truncate;
+	int sniffhdr;
+
+	wait_queue_head_t cmdq;	/* wait queue itself */
+
+	/* Controller state */
+	u32 state;
+	u32 isap;
+	u8 port_enabled[HFA384x_NUMPORTS_MAX];
+
+	/* Download support */
+	unsigned int dlstate;
+	struct hfa384x_downloadbuffer bufinfo;
+	u16 dltimeout;
+
+	int scanflag;		/* to signal scan complete */
+	int join_ap;		/* are we joined to a specific ap */
+	int join_retries;	/* number of join retries till we fail */
+	struct hfa384x_join_request_data joinreq;/* join request saved data */
+
+	struct wlandevice *wlandev;
+	/* Timer to allow for the deferred processing of linkstatus messages */
+	struct work_struct link_bh;
+
+	struct work_struct commsqual_bh;
+	struct hfa384x_commsquality qual;
+	struct timer_list commsqual_timer;
+
+	u16 link_status;
+	u16 link_status_new;
+	struct sk_buff_head authq;
+
+	u32 txrate;
+
+	/* And here we have stuff that used to be in priv */
+
+	/* State variables */
+	unsigned int presniff_port_type;
+	u16 presniff_wepflags;
+	u32 dot11_desired_bss_type;
+
+	int dbmadjust;
+
+	/* Group Addresses - right now, there are up to a total
+	 * of MAX_GRP_ADDR group addresses
+	 */
+	u8 dot11_grp_addr[MAX_GRP_ADDR][ETH_ALEN];
+	unsigned int dot11_grpcnt;
+
+	/* Component Identities */
+	struct hfa384x_compident ident_nic;
+	struct hfa384x_compident ident_pri_fw;
+	struct hfa384x_compident ident_sta_fw;
+	struct hfa384x_compident ident_ap_fw;
+	u16 mm_mods;
+
+	/* Supplier compatibility ranges */
+	struct hfa384x_caplevel cap_sup_mfi;
+	struct hfa384x_caplevel cap_sup_cfi;
+	struct hfa384x_caplevel cap_sup_pri;
+	struct hfa384x_caplevel cap_sup_sta;
+	struct hfa384x_caplevel cap_sup_ap;
+
+	/* Actor compatibility ranges */
+	struct hfa384x_caplevel cap_act_pri_cfi; /*
+						  * pri f/w to controller
+						  * interface
+						  */
+
+	struct hfa384x_caplevel cap_act_sta_cfi; /*
+						  * sta f/w to controller
+						  * interface
+						  */
+
+	struct hfa384x_caplevel cap_act_sta_mfi; /*
+						  * sta f/w to modem interface
+						  */
+
+	struct hfa384x_caplevel cap_act_ap_cfi;	/*
+						 * ap f/w to controller
+						 * interface
+						 */
+
+	struct hfa384x_caplevel cap_act_ap_mfi;	/* ap f/w to modem interface */
+
+	u32 psusercount;	/* Power save user count. */
+	struct hfa384x_comm_tallies_32 tallies;	/* Communication tallies. */
+	u8 comment[WLAN_COMMENT_MAX + 1];	/* User comment */
+
+	/* Channel Info request results (AP only) */
+	struct {
+		atomic_t done;
+		u8 count;
+		struct hfa384x_ch_info_result results;
+	} channel_info;
+
+	struct hfa384x_inf_frame *scanresults;
+
+	struct prism2sta_authlist authlist;	/*
+						 * Authenticated station list.
+						 */
+	unsigned int accessmode;		/* Access mode. */
+	struct prism2sta_accesslist allow;	/* Allowed station list. */
+	struct prism2sta_accesslist deny;	/* Denied station list. */
+
+};
+
+void hfa384x_create(struct hfa384x *hw, struct usb_device *usb);
+void hfa384x_destroy(struct hfa384x *hw);
+
+int hfa384x_corereset(struct hfa384x *hw, int holdtime, int settletime,
+		      int genesis);
+int hfa384x_drvr_disable(struct hfa384x *hw, u16 macport);
+int hfa384x_drvr_enable(struct hfa384x *hw, u16 macport);
+int hfa384x_drvr_flashdl_enable(struct hfa384x *hw);
+int hfa384x_drvr_flashdl_disable(struct hfa384x *hw);
+int hfa384x_drvr_flashdl_write(struct hfa384x *hw, u32 daddr, void *buf,
+			       u32 len);
+int hfa384x_drvr_getconfig(struct hfa384x *hw, u16 rid, void *buf, u16 len);
+int hfa384x_drvr_ramdl_enable(struct hfa384x *hw, u32 exeaddr);
+int hfa384x_drvr_ramdl_disable(struct hfa384x *hw);
+int hfa384x_drvr_ramdl_write(struct hfa384x *hw, u32 daddr, void *buf, u32 len);
+int hfa384x_drvr_readpda(struct hfa384x *hw, void *buf, unsigned int len);
+int hfa384x_drvr_setconfig(struct hfa384x *hw, u16 rid, void *buf, u16 len);
+
+static inline int
+hfa384x_drvr_getconfig16(struct hfa384x *hw, u16 rid, void *val)
+{
+	int result = 0;
+
+	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u16));
+	if (result == 0)
+		le16_to_cpus(val);
+	return result;
+}
+
+static inline int hfa384x_drvr_setconfig16(struct hfa384x *hw, u16 rid, u16 val)
+{
+	__le16 value = cpu_to_le16(val);
+
+	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
+}
+
+int
+hfa384x_drvr_setconfig_async(struct hfa384x *hw,
+			     u16 rid,
+			     void *buf,
+			     u16 len, ctlx_usercb_t usercb, void *usercb_data);
+
+static inline int
+hfa384x_drvr_setconfig16_async(struct hfa384x *hw, u16 rid, u16 val)
+{
+	__le16 value = cpu_to_le16(val);
+
+	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
+					    NULL, NULL);
+}
+
+int hfa384x_drvr_start(struct hfa384x *hw);
+int hfa384x_drvr_stop(struct hfa384x *hw);
+int
+hfa384x_drvr_txframe(struct hfa384x *hw, struct sk_buff *skb,
+		     struct p80211_hdr *p80211_hdr,
+		     struct p80211_metawep *p80211_wep);
+void hfa384x_tx_timeout(struct wlandevice *wlandev);
+
+int hfa384x_cmd_initialize(struct hfa384x *hw);
+int hfa384x_cmd_enable(struct hfa384x *hw, u16 macport);
+int hfa384x_cmd_disable(struct hfa384x *hw, u16 macport);
+int hfa384x_cmd_allocate(struct hfa384x *hw, u16 len);
+int hfa384x_cmd_monitor(struct hfa384x *hw, u16 enable);
+int
+hfa384x_cmd_download(struct hfa384x *hw,
+		     u16 mode, u16 lowaddr, u16 highaddr, u16 codelen);
+
+#endif /*__KERNEL__ */
+
+#endif /*_HFA384x_H */
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
new file mode 100644
index 000000000..02fdef7a1
--- /dev/null
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -0,0 +1,3901 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Functions that talk to the USB variant of the Intersil hfa384x MAC
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file implements functions that correspond to the prism2/hfa384x
+ * 802.11 MAC hardware and firmware host interface.
+ *
+ * The functions can be considered to represent several levels of
+ * abstraction.  The lowest level functions are simply C-callable wrappers
+ * around the register accesses.  The next higher level represents C-callable
+ * prism2 API functions that match the Intersil documentation as closely
+ * as is reasonable.  The next higher layer implements common sequences
+ * of invocations of the API layer (e.g. write to bap, followed by cmd).
+ *
+ * Common sequences:
+ * hfa384x_drvr_xxx	Highest level abstractions provided by the
+ *			hfa384x code.  They are driver defined wrappers
+ *			for common sequences.  These functions generally
+ *			use the services of the lower levels.
+ *
+ * hfa384x_drvr_xxxconfig  An example of the drvr level abstraction. These
+ *			functions are wrappers for the RID get/set
+ *			sequence. They call copy_[to|from]_bap() and
+ *			cmd_access(). These functions operate on the
+ *			RIDs and buffers without validation. The caller
+ *			is responsible for that.
+ *
+ * API wrapper functions:
+ * hfa384x_cmd_xxx	functions that provide access to the f/w commands.
+ *			The function arguments correspond to each command
+ *			argument, even command arguments that get packed
+ *			into single registers.  These functions _just_
+ *			issue the command by setting the cmd/parm regs
+ *			& reading the status/resp regs.  Additional
+ *			activities required to fully use a command
+ *			(read/write from/to bap, get/set int status etc.)
+ *			are implemented separately.  Think of these as
+ *			C-callable prism2 commands.
+ *
+ * Lowest Layer Functions:
+ * hfa384x_docmd_xxx	These functions implement the sequence required
+ *			to issue any prism2 command.  Primarily used by the
+ *			hfa384x_cmd_xxx functions.
+ *
+ * hfa384x_bap_xxx	BAP read/write access functions.
+ *			Note: we usually use BAP0 for non-interrupt context
+ *			 and BAP1 for interrupt context.
+ *
+ * hfa384x_dl_xxx	download related functions.
+ *
+ * Driver State Issues:
+ * Note that there are two pairs of functions that manage the
+ * 'initialized' and 'running' states of the hw/MAC combo.  The four
+ * functions are create(), destroy(), start(), and stop().  create()
+ * sets up the data structures required to support the hfa384x_*
+ * functions and destroy() cleans them up.  The start() function gets
+ * the actual hardware running and enables the interrupts.  The stop()
+ * function shuts the hardware down.  The sequence should be:
+ * create()
+ * start()
+ *  .
+ *  .  Do interesting things w/ the hardware
+ *  .
+ * stop()
+ * destroy()
+ *
+ * Note that destroy() can be called without calling stop() first.
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/timer.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <asm/byteorder.h>
+#include <linux/bitops.h>
+#include <linux/list.h>
+#include <linux/usb.h>
+#include <linux/byteorder/generic.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211mgmt.h"
+#include "p80211conv.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211req.h"
+#include "p80211metadef.h"
+#include "p80211metastruct.h"
+#include "hfa384x.h"
+#include "prism2mgmt.h"
+
+enum cmd_mode {
+	DOWAIT = 0,
+	DOASYNC
+};
+
+#define THROTTLE_JIFFIES	(HZ / 8)
+#define URB_ASYNC_UNLINK 0
+#define USB_QUEUE_BULK 0
+
+#define ROUNDUP64(a) (((a) + 63) & ~63)
+
+#ifdef DEBUG_USB
+static void dbprint_urb(struct urb *urb);
+#endif
+
+static void hfa384x_int_rxmonitor(struct wlandevice *wlandev,
+				  struct hfa384x_usb_rxfrm *rxfrm);
+
+static void hfa384x_usb_defer(struct work_struct *data);
+
+static int submit_rx_urb(struct hfa384x *hw, gfp_t flags);
+
+static int submit_tx_urb(struct hfa384x *hw, struct urb *tx_urb, gfp_t flags);
+
+/*---------------------------------------------------*/
+/* Callbacks */
+static void hfa384x_usbout_callback(struct urb *urb);
+static void hfa384x_ctlxout_callback(struct urb *urb);
+static void hfa384x_usbin_callback(struct urb *urb);
+
+static void
+hfa384x_usbin_txcompl(struct wlandevice *wlandev, union hfa384x_usbin *usbin);
+
+static void hfa384x_usbin_rx(struct wlandevice *wlandev, struct sk_buff *skb);
+
+static void hfa384x_usbin_info(struct wlandevice *wlandev,
+			       union hfa384x_usbin *usbin);
+
+static void hfa384x_usbin_ctlx(struct hfa384x *hw, union hfa384x_usbin *usbin,
+			       int urb_status);
+
+/*---------------------------------------------------*/
+/* Functions to support the prism2 usb command queue */
+
+static void hfa384x_usbctlxq_run(struct hfa384x *hw);
+
+static void hfa384x_usbctlx_reqtimerfn(struct timer_list *t);
+
+static void hfa384x_usbctlx_resptimerfn(struct timer_list *t);
+
+static void hfa384x_usb_throttlefn(struct timer_list *t);
+
+static void hfa384x_usbctlx_completion_task(struct work_struct *work);
+
+static void hfa384x_usbctlx_reaper_task(struct work_struct *work);
+
+static int hfa384x_usbctlx_submit(struct hfa384x *hw,
+				  struct hfa384x_usbctlx *ctlx);
+
+static void unlocked_usbctlx_complete(struct hfa384x *hw,
+				      struct hfa384x_usbctlx *ctlx);
+
+struct usbctlx_completor {
+	int (*complete)(struct usbctlx_completor *completor);
+};
+
+static int
+hfa384x_usbctlx_complete_sync(struct hfa384x *hw,
+			      struct hfa384x_usbctlx *ctlx,
+			      struct usbctlx_completor *completor);
+
+static int
+unlocked_usbctlx_cancel_async(struct hfa384x *hw, struct hfa384x_usbctlx *ctlx);
+
+static void hfa384x_cb_status(struct hfa384x *hw,
+			      const struct hfa384x_usbctlx *ctlx);
+
+static int
+usbctlx_get_status(const struct hfa384x_usb_statusresp *cmdresp,
+		   struct hfa384x_cmdresult *result);
+
+static void
+usbctlx_get_rridresult(const struct hfa384x_usb_rridresp *rridresp,
+		       struct hfa384x_rridresult *result);
+
+/*---------------------------------------------------*/
+/* Low level req/resp CTLX formatters and submitters */
+static inline int
+hfa384x_docmd(struct hfa384x *hw,
+	      struct hfa384x_metacmd *cmd);
+
+static int
+hfa384x_dorrid(struct hfa384x *hw,
+	       enum cmd_mode mode,
+	       u16 rid,
+	       void *riddata,
+	       unsigned int riddatalen,
+	       ctlx_cmdcb_t cmdcb, ctlx_usercb_t usercb, void *usercb_data);
+
+static int
+hfa384x_dowrid(struct hfa384x *hw,
+	       enum cmd_mode mode,
+	       u16 rid,
+	       void *riddata,
+	       unsigned int riddatalen,
+	       ctlx_cmdcb_t cmdcb, ctlx_usercb_t usercb, void *usercb_data);
+
+static int
+hfa384x_dormem(struct hfa384x *hw,
+	       u16 page,
+	       u16 offset,
+	       void *data,
+	       unsigned int len);
+
+static int
+hfa384x_dowmem(struct hfa384x *hw,
+	       u16 page,
+	       u16 offset,
+	       void *data,
+	       unsigned int len);
+
+static int hfa384x_isgood_pdrcode(u16 pdrcode);
+
+static inline const char *ctlxstr(enum ctlx_state s)
+{
+	static const char * const ctlx_str[] = {
+		"Initial state",
+		"Complete",
+		"Request failed",
+		"Request pending",
+		"Request packet submitted",
+		"Request packet completed",
+		"Response packet completed"
+	};
+
+	return ctlx_str[s];
+};
+
+static inline struct hfa384x_usbctlx *get_active_ctlx(struct hfa384x *hw)
+{
+	return list_entry(hw->ctlxq.active.next, struct hfa384x_usbctlx, list);
+}
+
+#ifdef DEBUG_USB
+void dbprint_urb(struct urb *urb)
+{
+	pr_debug("urb->pipe=0x%08x\n", urb->pipe);
+	pr_debug("urb->status=0x%08x\n", urb->status);
+	pr_debug("urb->transfer_flags=0x%08x\n", urb->transfer_flags);
+	pr_debug("urb->transfer_buffer=0x%08x\n",
+		 (unsigned int)urb->transfer_buffer);
+	pr_debug("urb->transfer_buffer_length=0x%08x\n",
+		 urb->transfer_buffer_length);
+	pr_debug("urb->actual_length=0x%08x\n", urb->actual_length);
+	pr_debug("urb->setup_packet(ctl)=0x%08x\n",
+		 (unsigned int)urb->setup_packet);
+	pr_debug("urb->start_frame(iso/irq)=0x%08x\n", urb->start_frame);
+	pr_debug("urb->interval(irq)=0x%08x\n", urb->interval);
+	pr_debug("urb->error_count(iso)=0x%08x\n", urb->error_count);
+	pr_debug("urb->context=0x%08x\n", (unsigned int)urb->context);
+	pr_debug("urb->complete=0x%08x\n", (unsigned int)urb->complete);
+}
+#endif
+
+/*----------------------------------------------------------------
+ * submit_rx_urb
+ *
+ * Listen for input data on the BULK-IN pipe. If the pipe has
+ * stalled then schedule it to be reset.
+ *
+ * Arguments:
+ *	hw		device struct
+ *	memflags	memory allocation flags
+ *
+ * Returns:
+ *	error code from submission
+ *
+ * Call context:
+ *	Any
+ *----------------------------------------------------------------
+ */
+static int submit_rx_urb(struct hfa384x *hw, gfp_t memflags)
+{
+	struct sk_buff *skb;
+	int result;
+
+	skb = dev_alloc_skb(sizeof(union hfa384x_usbin));
+	if (!skb) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Post the IN urb */
+	usb_fill_bulk_urb(&hw->rx_urb, hw->usb,
+			  hw->endp_in,
+			  skb->data, sizeof(union hfa384x_usbin),
+			  hfa384x_usbin_callback, hw->wlandev);
+
+	hw->rx_urb_skb = skb;
+
+	result = -ENOLINK;
+	if (!hw->wlandev->hwremoved &&
+	    !test_bit(WORK_RX_HALT, &hw->usb_flags)) {
+		result = usb_submit_urb(&hw->rx_urb, memflags);
+
+		/* Check whether we need to reset the RX pipe */
+		if (result == -EPIPE) {
+			netdev_warn(hw->wlandev->netdev,
+				    "%s rx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
+			if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
+				schedule_work(&hw->usb_work);
+		}
+	}
+
+	/* Don't leak memory if anything should go wrong */
+	if (result != 0) {
+		dev_kfree_skb(skb);
+		hw->rx_urb_skb = NULL;
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * submit_tx_urb
+ *
+ * Prepares and submits the URB of transmitted data. If the
+ * submission fails then it will schedule the output pipe to
+ * be reset.
+ *
+ * Arguments:
+ *	hw		device struct
+ *	tx_urb		URB of data for transmission
+ *	memflags	memory allocation flags
+ *
+ * Returns:
+ *	error code from submission
+ *
+ * Call context:
+ *	Any
+ *----------------------------------------------------------------
+ */
+static int submit_tx_urb(struct hfa384x *hw, struct urb *tx_urb, gfp_t memflags)
+{
+	struct net_device *netdev = hw->wlandev->netdev;
+	int result;
+
+	result = -ENOLINK;
+	if (netif_running(netdev)) {
+		if (!hw->wlandev->hwremoved &&
+		    !test_bit(WORK_TX_HALT, &hw->usb_flags)) {
+			result = usb_submit_urb(tx_urb, memflags);
+
+			/* Test whether we need to reset the TX pipe */
+			if (result == -EPIPE) {
+				netdev_warn(hw->wlandev->netdev,
+					    "%s tx pipe stalled: requesting reset\n",
+					    netdev->name);
+				set_bit(WORK_TX_HALT, &hw->usb_flags);
+				schedule_work(&hw->usb_work);
+			} else if (result == 0) {
+				netif_stop_queue(netdev);
+			}
+		}
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa394x_usb_defer
+ *
+ * There are some things that the USB stack cannot do while
+ * in interrupt context, so we arrange this function to run
+ * in process context.
+ *
+ * Arguments:
+ *	hw	device structure
+ *
+ * Returns:
+ *	nothing
+ *
+ * Call context:
+ *	process (by design)
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usb_defer(struct work_struct *data)
+{
+	struct hfa384x *hw = container_of(data, struct hfa384x, usb_work);
+	struct net_device *netdev = hw->wlandev->netdev;
+
+	/* Don't bother trying to reset anything if the plug
+	 * has been pulled ...
+	 */
+	if (hw->wlandev->hwremoved)
+		return;
+
+	/* Reception has stopped: try to reset the input pipe */
+	if (test_bit(WORK_RX_HALT, &hw->usb_flags)) {
+		int ret;
+
+		usb_kill_urb(&hw->rx_urb); /* Cannot be holding spinlock! */
+
+		ret = usb_clear_halt(hw->usb, hw->endp_in);
+		if (ret != 0) {
+			netdev_err(hw->wlandev->netdev,
+				   "Failed to clear rx pipe for %s: err=%d\n",
+				   netdev->name, ret);
+		} else {
+			netdev_info(hw->wlandev->netdev, "%s rx pipe reset complete.\n",
+				    netdev->name);
+			clear_bit(WORK_RX_HALT, &hw->usb_flags);
+			set_bit(WORK_RX_RESUME, &hw->usb_flags);
+		}
+	}
+
+	/* Resume receiving data back from the device. */
+	if (test_bit(WORK_RX_RESUME, &hw->usb_flags)) {
+		int ret;
+
+		ret = submit_rx_urb(hw, GFP_KERNEL);
+		if (ret != 0) {
+			netdev_err(hw->wlandev->netdev,
+				   "Failed to resume %s rx pipe.\n",
+				   netdev->name);
+		} else {
+			clear_bit(WORK_RX_RESUME, &hw->usb_flags);
+		}
+	}
+
+	/* Transmission has stopped: try to reset the output pipe */
+	if (test_bit(WORK_TX_HALT, &hw->usb_flags)) {
+		int ret;
+
+		usb_kill_urb(&hw->tx_urb);
+		ret = usb_clear_halt(hw->usb, hw->endp_out);
+		if (ret != 0) {
+			netdev_err(hw->wlandev->netdev,
+				   "Failed to clear tx pipe for %s: err=%d\n",
+				   netdev->name, ret);
+		} else {
+			netdev_info(hw->wlandev->netdev, "%s tx pipe reset complete.\n",
+				    netdev->name);
+			clear_bit(WORK_TX_HALT, &hw->usb_flags);
+			set_bit(WORK_TX_RESUME, &hw->usb_flags);
+
+			/* Stopping the BULK-OUT pipe also blocked
+			 * us from sending any more CTLX URBs, so
+			 * we need to re-run our queue ...
+			 */
+			hfa384x_usbctlxq_run(hw);
+		}
+	}
+
+	/* Resume transmitting. */
+	if (test_and_clear_bit(WORK_TX_RESUME, &hw->usb_flags))
+		netif_wake_queue(hw->wlandev->netdev);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_create
+ *
+ * Sets up the struct hfa384x data structure for use.  Note this
+ * does _not_ initialize the actual hardware, just the data structures
+ * we use to keep track of its state.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	irq		device irq number
+ *	iobase		i/o base address for register access
+ *	membase		memory base address for register access
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+void hfa384x_create(struct hfa384x *hw, struct usb_device *usb)
+{
+	hw->usb = usb;
+
+	/* Set up the waitq */
+	init_waitqueue_head(&hw->cmdq);
+
+	/* Initialize the command queue */
+	spin_lock_init(&hw->ctlxq.lock);
+	INIT_LIST_HEAD(&hw->ctlxq.pending);
+	INIT_LIST_HEAD(&hw->ctlxq.active);
+	INIT_LIST_HEAD(&hw->ctlxq.completing);
+	INIT_LIST_HEAD(&hw->ctlxq.reapable);
+
+	/* Initialize the authentication queue */
+	skb_queue_head_init(&hw->authq);
+
+	INIT_WORK(&hw->reaper_bh, hfa384x_usbctlx_reaper_task);
+	INIT_WORK(&hw->completion_bh, hfa384x_usbctlx_completion_task);
+	INIT_WORK(&hw->link_bh, prism2sta_processing_defer);
+	INIT_WORK(&hw->usb_work, hfa384x_usb_defer);
+
+	timer_setup(&hw->throttle, hfa384x_usb_throttlefn, 0);
+
+	timer_setup(&hw->resptimer, hfa384x_usbctlx_resptimerfn, 0);
+
+	timer_setup(&hw->reqtimer, hfa384x_usbctlx_reqtimerfn, 0);
+
+	usb_init_urb(&hw->rx_urb);
+	usb_init_urb(&hw->tx_urb);
+	usb_init_urb(&hw->ctlx_urb);
+
+	hw->link_status = HFA384x_LINK_NOTCONNECTED;
+	hw->state = HFA384x_STATE_INIT;
+
+	INIT_WORK(&hw->commsqual_bh, prism2sta_commsqual_defer);
+	timer_setup(&hw->commsqual_timer, prism2sta_commsqual_timer, 0);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_destroy
+ *
+ * Partner to hfa384x_create().  This function cleans up the hw
+ * structure so that it can be freed by the caller using a simple
+ * kfree.  Currently, this function is just a placeholder.  If, at some
+ * point in the future, an hw in the 'shutdown' state requires a 'deep'
+ * kfree, this is where it should be done.  Note that if this function
+ * is called on a _running_ hw structure, the drvr_stop() function is
+ * called.
+ *
+ * Arguments:
+ *	hw		device structure
+ *
+ * Returns:
+ *	nothing, this function is not allowed to fail.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+void hfa384x_destroy(struct hfa384x *hw)
+{
+	struct sk_buff *skb;
+
+	if (hw->state == HFA384x_STATE_RUNNING)
+		hfa384x_drvr_stop(hw);
+	hw->state = HFA384x_STATE_PREINIT;
+
+	kfree(hw->scanresults);
+	hw->scanresults = NULL;
+
+	/* Now to clean out the auth queue */
+	while ((skb = skb_dequeue(&hw->authq)))
+		dev_kfree_skb(skb);
+}
+
+static struct hfa384x_usbctlx *usbctlx_alloc(void)
+{
+	struct hfa384x_usbctlx *ctlx;
+
+	ctlx = kzalloc(sizeof(*ctlx),
+		       in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+	if (ctlx)
+		init_completion(&ctlx->done);
+
+	return ctlx;
+}
+
+static int
+usbctlx_get_status(const struct hfa384x_usb_statusresp *cmdresp,
+		   struct hfa384x_cmdresult *result)
+{
+	result->status = le16_to_cpu(cmdresp->status);
+	result->resp0 = le16_to_cpu(cmdresp->resp0);
+	result->resp1 = le16_to_cpu(cmdresp->resp1);
+	result->resp2 = le16_to_cpu(cmdresp->resp2);
+
+	pr_debug("cmdresult:status=0x%04x resp0=0x%04x resp1=0x%04x resp2=0x%04x\n",
+		 result->status, result->resp0, result->resp1, result->resp2);
+
+	return result->status & HFA384x_STATUS_RESULT;
+}
+
+static void
+usbctlx_get_rridresult(const struct hfa384x_usb_rridresp *rridresp,
+		       struct hfa384x_rridresult *result)
+{
+	result->rid = le16_to_cpu(rridresp->rid);
+	result->riddata = rridresp->data;
+	result->riddata_len = ((le16_to_cpu(rridresp->frmlen) - 1) * 2);
+}
+
+/*----------------------------------------------------------------
+ * Completor object:
+ * This completor must be passed to hfa384x_usbctlx_complete_sync()
+ * when processing a CTLX that returns a struct hfa384x_cmdresult structure.
+ *----------------------------------------------------------------
+ */
+struct usbctlx_cmd_completor {
+	struct usbctlx_completor head;
+
+	const struct hfa384x_usb_statusresp *cmdresp;
+	struct hfa384x_cmdresult *result;
+};
+
+static inline int usbctlx_cmd_completor_fn(struct usbctlx_completor *head)
+{
+	struct usbctlx_cmd_completor *complete;
+
+	complete = (struct usbctlx_cmd_completor *)head;
+	return usbctlx_get_status(complete->cmdresp, complete->result);
+}
+
+static inline struct usbctlx_completor *
+init_cmd_completor(struct usbctlx_cmd_completor *completor,
+		   const struct hfa384x_usb_statusresp *cmdresp,
+		   struct hfa384x_cmdresult *result)
+{
+	completor->head.complete = usbctlx_cmd_completor_fn;
+	completor->cmdresp = cmdresp;
+	completor->result = result;
+	return &completor->head;
+}
+
+/*----------------------------------------------------------------
+ * Completor object:
+ * This completor must be passed to hfa384x_usbctlx_complete_sync()
+ * when processing a CTLX that reads a RID.
+ *----------------------------------------------------------------
+ */
+struct usbctlx_rrid_completor {
+	struct usbctlx_completor head;
+
+	const struct hfa384x_usb_rridresp *rridresp;
+	void *riddata;
+	unsigned int riddatalen;
+};
+
+static int usbctlx_rrid_completor_fn(struct usbctlx_completor *head)
+{
+	struct usbctlx_rrid_completor *complete;
+	struct hfa384x_rridresult rridresult;
+
+	complete = (struct usbctlx_rrid_completor *)head;
+	usbctlx_get_rridresult(complete->rridresp, &rridresult);
+
+	/* Validate the length, note body len calculation in bytes */
+	if (rridresult.riddata_len != complete->riddatalen) {
+		pr_warn("RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
+			rridresult.rid,
+			complete->riddatalen, rridresult.riddata_len);
+		return -ENODATA;
+	}
+
+	memcpy(complete->riddata, rridresult.riddata, complete->riddatalen);
+	return 0;
+}
+
+static inline struct usbctlx_completor *
+init_rrid_completor(struct usbctlx_rrid_completor *completor,
+		    const struct hfa384x_usb_rridresp *rridresp,
+		    void *riddata,
+		    unsigned int riddatalen)
+{
+	completor->head.complete = usbctlx_rrid_completor_fn;
+	completor->rridresp = rridresp;
+	completor->riddata = riddata;
+	completor->riddatalen = riddatalen;
+	return &completor->head;
+}
+
+/*----------------------------------------------------------------
+ * Completor object:
+ * Interprets the results of a synchronous RID-write
+ *----------------------------------------------------------------
+ */
+#define init_wrid_completor  init_cmd_completor
+
+/*----------------------------------------------------------------
+ * Completor object:
+ * Interprets the results of a synchronous memory-write
+ *----------------------------------------------------------------
+ */
+#define init_wmem_completor  init_cmd_completor
+
+/*----------------------------------------------------------------
+ * Completor object:
+ * Interprets the results of a synchronous memory-read
+ *----------------------------------------------------------------
+ */
+struct usbctlx_rmem_completor {
+	struct usbctlx_completor head;
+
+	const struct hfa384x_usb_rmemresp *rmemresp;
+	void *data;
+	unsigned int len;
+};
+
+static int usbctlx_rmem_completor_fn(struct usbctlx_completor *head)
+{
+	struct usbctlx_rmem_completor *complete =
+		(struct usbctlx_rmem_completor *)head;
+
+	pr_debug("rmemresp:len=%d\n", complete->rmemresp->frmlen);
+	memcpy(complete->data, complete->rmemresp->data, complete->len);
+	return 0;
+}
+
+static inline struct usbctlx_completor *
+init_rmem_completor(struct usbctlx_rmem_completor *completor,
+		    struct hfa384x_usb_rmemresp *rmemresp,
+		    void *data,
+		    unsigned int len)
+{
+	completor->head.complete = usbctlx_rmem_completor_fn;
+	completor->rmemresp = rmemresp;
+	completor->data = data;
+	completor->len = len;
+	return &completor->head;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cb_status
+ *
+ * Ctlx_complete handler for async CMD type control exchanges.
+ * mark the hw struct as such.
+ *
+ * Note: If the handling is changed here, it should probably be
+ *       changed in docmd as well.
+ *
+ * Arguments:
+ *	hw		hw struct
+ *	ctlx		completed CTLX
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_cb_status(struct hfa384x *hw,
+			      const struct hfa384x_usbctlx *ctlx)
+{
+	if (ctlx->usercb) {
+		struct hfa384x_cmdresult cmdresult;
+
+		if (ctlx->state != CTLX_COMPLETE) {
+			memset(&cmdresult, 0, sizeof(cmdresult));
+			cmdresult.status =
+			    HFA384x_STATUS_RESULT_SET(HFA384x_CMD_ERR);
+		} else {
+			usbctlx_get_status(&ctlx->inbuf.cmdresp, &cmdresult);
+		}
+
+		ctlx->usercb(hw, &cmdresult, ctlx->usercb_data);
+	}
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cmd_initialize
+ *
+ * Issues the initialize command and sets the hw->state based
+ * on the result.
+ *
+ * Arguments:
+ *	hw		device structure
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_cmd_initialize(struct hfa384x *hw)
+{
+	int result = 0;
+	int i;
+	struct hfa384x_metacmd cmd;
+
+	cmd.cmd = HFA384x_CMDCODE_INIT;
+	cmd.parm0 = 0;
+	cmd.parm1 = 0;
+	cmd.parm2 = 0;
+
+	result = hfa384x_docmd(hw, &cmd);
+
+	pr_debug("cmdresp.init: status=0x%04x, resp0=0x%04x, resp1=0x%04x, resp2=0x%04x\n",
+		 cmd.result.status,
+		 cmd.result.resp0, cmd.result.resp1, cmd.result.resp2);
+	if (result == 0) {
+		for (i = 0; i < HFA384x_NUMPORTS_MAX; i++)
+			hw->port_enabled[i] = 0;
+	}
+
+	hw->link_status = HFA384x_LINK_NOTCONNECTED;
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cmd_disable
+ *
+ * Issues the disable command to stop communications on one of
+ * the MACs 'ports'.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	macport		MAC port number (host order)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_cmd_disable(struct hfa384x *hw, u16 macport)
+{
+	struct hfa384x_metacmd cmd;
+
+	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DISABLE) |
+	    HFA384x_CMD_MACPORT_SET(macport);
+	cmd.parm0 = 0;
+	cmd.parm1 = 0;
+	cmd.parm2 = 0;
+
+	return hfa384x_docmd(hw, &cmd);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cmd_enable
+ *
+ * Issues the enable command to enable communications on one of
+ * the MACs 'ports'.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	macport		MAC port number
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_cmd_enable(struct hfa384x *hw, u16 macport)
+{
+	struct hfa384x_metacmd cmd;
+
+	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ENABLE) |
+	    HFA384x_CMD_MACPORT_SET(macport);
+	cmd.parm0 = 0;
+	cmd.parm1 = 0;
+	cmd.parm2 = 0;
+
+	return hfa384x_docmd(hw, &cmd);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cmd_monitor
+ *
+ * Enables the 'monitor mode' of the MAC.  Here's the description of
+ * monitor mode that I've received thus far:
+ *
+ *  "The "monitor mode" of operation is that the MAC passes all
+ *  frames for which the PLCP checks are correct. All received
+ *  MPDUs are passed to the host with MAC Port = 7, with a
+ *  receive status of good, FCS error, or undecryptable. Passing
+ *  certain MPDUs is a violation of the 802.11 standard, but useful
+ *  for a debugging tool."  Normal communication is not possible
+ *  while monitor mode is enabled.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	enable		a code (0x0b|0x0f) that enables/disables
+ *			monitor mode. (host order)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_cmd_monitor(struct hfa384x *hw, u16 enable)
+{
+	struct hfa384x_metacmd cmd;
+
+	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_MONITOR) |
+	    HFA384x_CMD_AINFO_SET(enable);
+	cmd.parm0 = 0;
+	cmd.parm1 = 0;
+	cmd.parm2 = 0;
+
+	return hfa384x_docmd(hw, &cmd);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_cmd_download
+ *
+ * Sets the controls for the MAC controller code/data download
+ * process.  The arguments set the mode and address associated
+ * with a download.  Note that the aux registers should be enabled
+ * prior to setting one of the download enable modes.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	mode		0 - Disable programming and begin code exec
+ *			1 - Enable volatile mem programming
+ *			2 - Enable non-volatile mem programming
+ *			3 - Program non-volatile section from NV download
+ *			    buffer.
+ *			(host order)
+ *	lowaddr
+ *	highaddr	For mode 1, sets the high & low order bits of
+ *			the "destination address".  This address will be
+ *			the execution start address when download is
+ *			subsequently disabled.
+ *			For mode 2, sets the high & low order bits of
+ *			the destination in NV ram.
+ *			For modes 0 & 3, should be zero. (host order)
+ *			NOTE: these are CMD format.
+ *	codelen		Length of the data to write in mode 2,
+ *			zero otherwise. (host order)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_cmd_download(struct hfa384x *hw, u16 mode, u16 lowaddr,
+			 u16 highaddr, u16 codelen)
+{
+	struct hfa384x_metacmd cmd;
+
+	pr_debug("mode=%d, lowaddr=0x%04x, highaddr=0x%04x, codelen=%d\n",
+		 mode, lowaddr, highaddr, codelen);
+
+	cmd.cmd = (HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DOWNLD) |
+		   HFA384x_CMD_PROGMODE_SET(mode));
+
+	cmd.parm0 = lowaddr;
+	cmd.parm1 = highaddr;
+	cmd.parm2 = codelen;
+
+	return hfa384x_docmd(hw, &cmd);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_corereset
+ *
+ * Perform a reset of the hfa38xx MAC core.  We assume that the hw
+ * structure is in its "created" state.  That is, it is initialized
+ * with proper values.  Note that if a reset is done after the
+ * device has been active for awhile, the caller might have to clean
+ * up some leftover cruft in the hw structure.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	holdtime	how long (in ms) to hold the reset
+ *	settletime	how long (in ms) to wait after releasing
+ *			the reset
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_corereset(struct hfa384x *hw, int holdtime,
+		      int settletime, int genesis)
+{
+	int result;
+
+	result = usb_reset_device(hw->usb);
+	if (result < 0) {
+		netdev_err(hw->wlandev->netdev, "usb_reset_device() failed, result=%d.\n",
+			   result);
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_complete_sync
+ *
+ * Waits for a synchronous CTLX object to complete,
+ * and then handles the response.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	ctlx		CTLX ptr
+ *	completor	functor object to decide what to
+ *			do with the CTLX's result.
+ *
+ * Returns:
+ *	0		Success
+ *	-ERESTARTSYS	Interrupted by a signal
+ *	-EIO		CTLX failed
+ *	-ENODEV		Adapter was unplugged
+ *	???		Result from completor
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+static int hfa384x_usbctlx_complete_sync(struct hfa384x *hw,
+					 struct hfa384x_usbctlx *ctlx,
+					 struct usbctlx_completor *completor)
+{
+	unsigned long flags;
+	int result;
+
+	result = wait_for_completion_interruptible(&ctlx->done);
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/*
+	 * We can only handle the CTLX if the USB disconnect
+	 * function has not run yet ...
+	 */
+cleanup:
+	if (hw->wlandev->hwremoved) {
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+		result = -ENODEV;
+	} else if (result != 0) {
+		int runqueue = 0;
+
+		/*
+		 * We were probably interrupted, so delete
+		 * this CTLX asynchronously, kill the timers
+		 * and the URB, and then start the next
+		 * pending CTLX.
+		 *
+		 * NOTE: We can only delete the timers and
+		 *       the URB if this CTLX is active.
+		 */
+		if (ctlx == get_active_ctlx(hw)) {
+			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+			del_singleshot_timer_sync(&hw->reqtimer);
+			del_singleshot_timer_sync(&hw->resptimer);
+			hw->req_timer_done = 1;
+			hw->resp_timer_done = 1;
+			usb_kill_urb(&hw->ctlx_urb);
+
+			spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+			runqueue = 1;
+
+			/*
+			 * This scenario is so unlikely that I'm
+			 * happy with a grubby "goto" solution ...
+			 */
+			if (hw->wlandev->hwremoved)
+				goto cleanup;
+		}
+
+		/*
+		 * The completion task will send this CTLX
+		 * to the reaper the next time it runs. We
+		 * are no longer in a hurry.
+		 */
+		ctlx->reapable = 1;
+		ctlx->state = CTLX_REQ_FAILED;
+		list_move_tail(&ctlx->list, &hw->ctlxq.completing);
+
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+		if (runqueue)
+			hfa384x_usbctlxq_run(hw);
+	} else {
+		if (ctlx->state == CTLX_COMPLETE) {
+			result = completor->complete(completor);
+		} else {
+			netdev_warn(hw->wlandev->netdev, "CTLX[%d] error: state(%s)\n",
+				    le16_to_cpu(ctlx->outbuf.type),
+				    ctlxstr(ctlx->state));
+			result = -EIO;
+		}
+
+		list_del(&ctlx->list);
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+		kfree(ctlx);
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_docmd
+ *
+ * Constructs a command CTLX and submits it.
+ *
+ * NOTE: Any changes to the 'post-submit' code in this function
+ *       need to be carried over to hfa384x_cbcmd() since the handling
+ *       is virtually identical.
+ *
+ * Arguments:
+ *	hw		device structure
+ *       cmd             cmd structure.  Includes all arguments and result
+ *                       data points.  All in host order. in host order
+ *
+ * Returns:
+ *	0		success
+ *	-EIO		CTLX failure
+ *	-ERESTARTSYS	Awakened on signal
+ *	>0		command indicated error, Status and Resp0-2 are
+ *			in hw structure.
+ *
+ * Side effects:
+ *
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+static inline int
+hfa384x_docmd(struct hfa384x *hw,
+	      struct hfa384x_metacmd *cmd)
+{
+	int result;
+	struct hfa384x_usbctlx *ctlx;
+
+	ctlx = usbctlx_alloc();
+	if (!ctlx) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Initialize the command */
+	ctlx->outbuf.cmdreq.type = cpu_to_le16(HFA384x_USB_CMDREQ);
+	ctlx->outbuf.cmdreq.cmd = cpu_to_le16(cmd->cmd);
+	ctlx->outbuf.cmdreq.parm0 = cpu_to_le16(cmd->parm0);
+	ctlx->outbuf.cmdreq.parm1 = cpu_to_le16(cmd->parm1);
+	ctlx->outbuf.cmdreq.parm2 = cpu_to_le16(cmd->parm2);
+
+	ctlx->outbufsize = sizeof(ctlx->outbuf.cmdreq);
+
+	pr_debug("cmdreq: cmd=0x%04x parm0=0x%04x parm1=0x%04x parm2=0x%04x\n",
+		 cmd->cmd, cmd->parm0, cmd->parm1, cmd->parm2);
+
+	ctlx->reapable = DOWAIT;
+	ctlx->cmdcb = NULL;
+	ctlx->usercb = NULL;
+	ctlx->usercb_data = NULL;
+
+	result = hfa384x_usbctlx_submit(hw, ctlx);
+	if (result != 0) {
+		kfree(ctlx);
+	} else {
+		struct usbctlx_cmd_completor cmd_completor;
+		struct usbctlx_completor *completor;
+
+		completor = init_cmd_completor(&cmd_completor,
+					       &ctlx->inbuf.cmdresp,
+					       &cmd->result);
+
+		result = hfa384x_usbctlx_complete_sync(hw, ctlx, completor);
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_dorrid
+ *
+ * Constructs a read rid CTLX and issues it.
+ *
+ * NOTE: Any changes to the 'post-submit' code in this function
+ *       need to be carried over to hfa384x_cbrrid() since the handling
+ *       is virtually identical.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	mode		DOWAIT or DOASYNC
+ *	rid		Read RID number (host order)
+ *	riddata		Caller supplied buffer that MAC formatted RID.data
+ *			record will be written to for DOWAIT calls. Should
+ *			be NULL for DOASYNC calls.
+ *	riddatalen	Buffer length for DOWAIT calls. Zero for DOASYNC calls.
+ *	cmdcb		command callback for async calls, NULL for DOWAIT calls
+ *	usercb		user callback for async calls, NULL for DOWAIT calls
+ *	usercb_data	user supplied data pointer for async calls, NULL
+ *			for DOWAIT calls
+ *
+ * Returns:
+ *	0		success
+ *	-EIO		CTLX failure
+ *	-ERESTARTSYS	Awakened on signal
+ *	-ENODATA	riddatalen != macdatalen
+ *	>0		command indicated error, Status and Resp0-2 are
+ *			in hw structure.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt (DOASYNC)
+ *	process (DOWAIT or DOASYNC)
+ *----------------------------------------------------------------
+ */
+static int
+hfa384x_dorrid(struct hfa384x *hw,
+	       enum cmd_mode mode,
+	       u16 rid,
+	       void *riddata,
+	       unsigned int riddatalen,
+	       ctlx_cmdcb_t cmdcb, ctlx_usercb_t usercb, void *usercb_data)
+{
+	int result;
+	struct hfa384x_usbctlx *ctlx;
+
+	ctlx = usbctlx_alloc();
+	if (!ctlx) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Initialize the command */
+	ctlx->outbuf.rridreq.type = cpu_to_le16(HFA384x_USB_RRIDREQ);
+	ctlx->outbuf.rridreq.frmlen =
+	    cpu_to_le16(sizeof(ctlx->outbuf.rridreq.rid));
+	ctlx->outbuf.rridreq.rid = cpu_to_le16(rid);
+
+	ctlx->outbufsize = sizeof(ctlx->outbuf.rridreq);
+
+	ctlx->reapable = mode;
+	ctlx->cmdcb = cmdcb;
+	ctlx->usercb = usercb;
+	ctlx->usercb_data = usercb_data;
+
+	/* Submit the CTLX */
+	result = hfa384x_usbctlx_submit(hw, ctlx);
+	if (result != 0) {
+		kfree(ctlx);
+	} else if (mode == DOWAIT) {
+		struct usbctlx_rrid_completor completor;
+
+		result =
+		    hfa384x_usbctlx_complete_sync(hw, ctlx,
+						  init_rrid_completor
+						  (&completor,
+						   &ctlx->inbuf.rridresp,
+						   riddata, riddatalen));
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_dowrid
+ *
+ * Constructs a write rid CTLX and issues it.
+ *
+ * NOTE: Any changes to the 'post-submit' code in this function
+ *       need to be carried over to hfa384x_cbwrid() since the handling
+ *       is virtually identical.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	enum cmd_mode	DOWAIT or DOASYNC
+ *	rid		RID code
+ *	riddata		Data portion of RID formatted for MAC
+ *	riddatalen	Length of the data portion in bytes
+ *       cmdcb           command callback for async calls, NULL for DOWAIT calls
+ *	usercb		user callback for async calls, NULL for DOWAIT calls
+ *	usercb_data	user supplied data pointer for async calls
+ *
+ * Returns:
+ *	0		success
+ *	-ETIMEDOUT	timed out waiting for register ready or
+ *			command completion
+ *	>0		command indicated error, Status and Resp0-2 are
+ *			in hw structure.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt (DOASYNC)
+ *	process (DOWAIT or DOASYNC)
+ *----------------------------------------------------------------
+ */
+static int
+hfa384x_dowrid(struct hfa384x *hw,
+	       enum cmd_mode mode,
+	       u16 rid,
+	       void *riddata,
+	       unsigned int riddatalen,
+	       ctlx_cmdcb_t cmdcb, ctlx_usercb_t usercb, void *usercb_data)
+{
+	int result;
+	struct hfa384x_usbctlx *ctlx;
+
+	ctlx = usbctlx_alloc();
+	if (!ctlx) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Initialize the command */
+	ctlx->outbuf.wridreq.type = cpu_to_le16(HFA384x_USB_WRIDREQ);
+	ctlx->outbuf.wridreq.frmlen = cpu_to_le16((sizeof
+						   (ctlx->outbuf.wridreq.rid) +
+						   riddatalen + 1) / 2);
+	ctlx->outbuf.wridreq.rid = cpu_to_le16(rid);
+	memcpy(ctlx->outbuf.wridreq.data, riddata, riddatalen);
+
+	ctlx->outbufsize = sizeof(ctlx->outbuf.wridreq.type) +
+	    sizeof(ctlx->outbuf.wridreq.frmlen) +
+	    sizeof(ctlx->outbuf.wridreq.rid) + riddatalen;
+
+	ctlx->reapable = mode;
+	ctlx->cmdcb = cmdcb;
+	ctlx->usercb = usercb;
+	ctlx->usercb_data = usercb_data;
+
+	/* Submit the CTLX */
+	result = hfa384x_usbctlx_submit(hw, ctlx);
+	if (result != 0) {
+		kfree(ctlx);
+	} else if (mode == DOWAIT) {
+		struct usbctlx_cmd_completor completor;
+		struct hfa384x_cmdresult wridresult;
+
+		result = hfa384x_usbctlx_complete_sync(hw,
+						       ctlx,
+						       init_wrid_completor
+						       (&completor,
+							&ctlx->inbuf.wridresp,
+							&wridresult));
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_dormem
+ *
+ * Constructs a readmem CTLX and issues it.
+ *
+ * NOTE: Any changes to the 'post-submit' code in this function
+ *       need to be carried over to hfa384x_cbrmem() since the handling
+ *       is virtually identical.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	page		MAC address space page (CMD format)
+ *	offset		MAC address space offset
+ *	data		Ptr to data buffer to receive read
+ *	len		Length of the data to read (max == 2048)
+ *
+ * Returns:
+ *	0		success
+ *	-ETIMEDOUT	timed out waiting for register ready or
+ *			command completion
+ *	>0		command indicated error, Status and Resp0-2 are
+ *			in hw structure.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process (DOWAIT)
+ *----------------------------------------------------------------
+ */
+static int
+hfa384x_dormem(struct hfa384x *hw,
+	       u16 page,
+	       u16 offset,
+	       void *data,
+	       unsigned int len)
+{
+	int result;
+	struct hfa384x_usbctlx *ctlx;
+
+	ctlx = usbctlx_alloc();
+	if (!ctlx) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Initialize the command */
+	ctlx->outbuf.rmemreq.type = cpu_to_le16(HFA384x_USB_RMEMREQ);
+	ctlx->outbuf.rmemreq.frmlen =
+	    cpu_to_le16(sizeof(ctlx->outbuf.rmemreq.offset) +
+			sizeof(ctlx->outbuf.rmemreq.page) + len);
+	ctlx->outbuf.rmemreq.offset = cpu_to_le16(offset);
+	ctlx->outbuf.rmemreq.page = cpu_to_le16(page);
+
+	ctlx->outbufsize = sizeof(ctlx->outbuf.rmemreq);
+
+	pr_debug("type=0x%04x frmlen=%d offset=0x%04x page=0x%04x\n",
+		 ctlx->outbuf.rmemreq.type,
+		 ctlx->outbuf.rmemreq.frmlen,
+		 ctlx->outbuf.rmemreq.offset, ctlx->outbuf.rmemreq.page);
+
+	pr_debug("pktsize=%zd\n", ROUNDUP64(sizeof(ctlx->outbuf.rmemreq)));
+
+	ctlx->reapable = DOWAIT;
+	ctlx->cmdcb = NULL;
+	ctlx->usercb = NULL;
+	ctlx->usercb_data = NULL;
+
+	result = hfa384x_usbctlx_submit(hw, ctlx);
+	if (result != 0) {
+		kfree(ctlx);
+	} else {
+		struct usbctlx_rmem_completor completor;
+
+		result =
+		    hfa384x_usbctlx_complete_sync(hw, ctlx,
+						  init_rmem_completor
+						  (&completor,
+						   &ctlx->inbuf.rmemresp, data,
+						   len));
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_dowmem
+ *
+ * Constructs a writemem CTLX and issues it.
+ *
+ * NOTE: Any changes to the 'post-submit' code in this function
+ *       need to be carried over to hfa384x_cbwmem() since the handling
+ *       is virtually identical.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	page		MAC address space page (CMD format)
+ *	offset		MAC address space offset
+ *	data		Ptr to data buffer containing write data
+ *	len		Length of the data to read (max == 2048)
+ *
+ * Returns:
+ *	0		success
+ *	-ETIMEDOUT	timed out waiting for register ready or
+ *			command completion
+ *	>0		command indicated error, Status and Resp0-2 are
+ *			in hw structure.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt (DOWAIT)
+ *	process (DOWAIT)
+ *----------------------------------------------------------------
+ */
+static int
+hfa384x_dowmem(struct hfa384x *hw,
+	       u16 page,
+	       u16 offset,
+	       void *data,
+	       unsigned int len)
+{
+	int result;
+	struct hfa384x_usbctlx *ctlx;
+
+	pr_debug("page=0x%04x offset=0x%04x len=%d\n", page, offset, len);
+
+	ctlx = usbctlx_alloc();
+	if (!ctlx) {
+		result = -ENOMEM;
+		goto done;
+	}
+
+	/* Initialize the command */
+	ctlx->outbuf.wmemreq.type = cpu_to_le16(HFA384x_USB_WMEMREQ);
+	ctlx->outbuf.wmemreq.frmlen =
+	    cpu_to_le16(sizeof(ctlx->outbuf.wmemreq.offset) +
+			sizeof(ctlx->outbuf.wmemreq.page) + len);
+	ctlx->outbuf.wmemreq.offset = cpu_to_le16(offset);
+	ctlx->outbuf.wmemreq.page = cpu_to_le16(page);
+	memcpy(ctlx->outbuf.wmemreq.data, data, len);
+
+	ctlx->outbufsize = sizeof(ctlx->outbuf.wmemreq.type) +
+	    sizeof(ctlx->outbuf.wmemreq.frmlen) +
+	    sizeof(ctlx->outbuf.wmemreq.offset) +
+	    sizeof(ctlx->outbuf.wmemreq.page) + len;
+
+	ctlx->reapable = DOWAIT;
+	ctlx->cmdcb = NULL;
+	ctlx->usercb = NULL;
+	ctlx->usercb_data = NULL;
+
+	result = hfa384x_usbctlx_submit(hw, ctlx);
+	if (result != 0) {
+		kfree(ctlx);
+	} else {
+		struct usbctlx_cmd_completor completor;
+		struct hfa384x_cmdresult wmemresult;
+
+		result = hfa384x_usbctlx_complete_sync(hw,
+						       ctlx,
+						       init_wmem_completor
+						       (&completor,
+							&ctlx->inbuf.wmemresp,
+							&wmemresult));
+	}
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_disable
+ *
+ * Issues the disable command to stop communications on one of
+ * the MACs 'ports'.  Only macport 0 is valid  for stations.
+ * APs may also disable macports 1-6.  Only ports that have been
+ * previously enabled may be disabled.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	macport		MAC port number (host order)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_disable(struct hfa384x *hw, u16 macport)
+{
+	int result = 0;
+
+	if ((!hw->isap && macport != 0) ||
+	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
+	    !(hw->port_enabled[macport])) {
+		result = -EINVAL;
+	} else {
+		result = hfa384x_cmd_disable(hw, macport);
+		if (result == 0)
+			hw->port_enabled[macport] = 0;
+	}
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_enable
+ *
+ * Issues the enable command to enable communications on one of
+ * the MACs 'ports'.  Only macport 0 is valid  for stations.
+ * APs may also enable macports 1-6.  Only ports that are currently
+ * disabled may be enabled.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	macport		MAC port number
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported failure - f/w status code
+ *	<0		driver reported error (timeout|bad arg)
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_enable(struct hfa384x *hw, u16 macport)
+{
+	int result = 0;
+
+	if ((!hw->isap && macport != 0) ||
+	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
+	    (hw->port_enabled[macport])) {
+		result = -EINVAL;
+	} else {
+		result = hfa384x_cmd_enable(hw, macport);
+		if (result == 0)
+			hw->port_enabled[macport] = 1;
+	}
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_flashdl_enable
+ *
+ * Begins the flash download state.  Checks to see that we're not
+ * already in a download state and that a port isn't enabled.
+ * Sets the download state and retrieves the flash download
+ * buffer location, buffer size, and timeout length.
+ *
+ * Arguments:
+ *	hw		device structure
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_flashdl_enable(struct hfa384x *hw)
+{
+	int result = 0;
+	int i;
+
+	/* Check that a port isn't active */
+	for (i = 0; i < HFA384x_PORTID_MAX; i++) {
+		if (hw->port_enabled[i]) {
+			pr_debug("called when port enabled.\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Check that we're not already in a download state */
+	if (hw->dlstate != HFA384x_DLSTATE_DISABLED)
+		return -EINVAL;
+
+	/* Retrieve the buffer loc&size and timeout */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DOWNLOADBUFFER,
+					&hw->bufinfo, sizeof(hw->bufinfo));
+	if (result)
+		return result;
+
+	le16_to_cpus(&hw->bufinfo.page);
+	le16_to_cpus(&hw->bufinfo.offset);
+	le16_to_cpus(&hw->bufinfo.len);
+	result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
+					  &hw->dltimeout);
+	if (result)
+		return result;
+
+	le16_to_cpus(&hw->dltimeout);
+
+	pr_debug("flashdl_enable\n");
+
+	hw->dlstate = HFA384x_DLSTATE_FLASHENABLED;
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_flashdl_disable
+ *
+ * Ends the flash download state.  Note that this will cause the MAC
+ * firmware to restart.
+ *
+ * Arguments:
+ *	hw		device structure
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_flashdl_disable(struct hfa384x *hw)
+{
+	/* Check that we're already in the download state */
+	if (hw->dlstate != HFA384x_DLSTATE_FLASHENABLED)
+		return -EINVAL;
+
+	pr_debug("flashdl_enable\n");
+
+	/* There isn't much we can do at this point, so I don't */
+	/*  bother  w/ the return value */
+	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0, 0);
+	hw->dlstate = HFA384x_DLSTATE_DISABLED;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_flashdl_write
+ *
+ * Performs a FLASH download of a chunk of data. First checks to see
+ * that we're in the FLASH download state, then sets the download
+ * mode, uses the aux functions to 1) copy the data to the flash
+ * buffer, 2) sets the download 'write flash' mode, 3) readback and
+ * compare.  Lather rinse, repeat as many times an necessary to get
+ * all the given data into flash.
+ * When all data has been written using this function (possibly
+ * repeatedly), call drvr_flashdl_disable() to end the download state
+ * and restart the MAC.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	daddr		Card address to write to. (host order)
+ *	buf		Ptr to data to write.
+ *	len		Length of data (host order).
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_flashdl_write(struct hfa384x *hw, u32 daddr,
+			       void *buf, u32 len)
+{
+	int result = 0;
+	u32 dlbufaddr;
+	int nburns;
+	u32 burnlen;
+	u32 burndaddr;
+	u16 burnlo;
+	u16 burnhi;
+	int nwrites;
+	u8 *writebuf;
+	u16 writepage;
+	u16 writeoffset;
+	u32 writelen;
+	int i;
+	int j;
+
+	pr_debug("daddr=0x%08x len=%d\n", daddr, len);
+
+	/* Check that we're in the flash download state */
+	if (hw->dlstate != HFA384x_DLSTATE_FLASHENABLED)
+		return -EINVAL;
+
+	netdev_info(hw->wlandev->netdev,
+		    "Download %d bytes to flash @0x%06x\n", len, daddr);
+
+	/* Convert to flat address for arithmetic */
+	/* NOTE: dlbuffer RID stores the address in AUX format */
+	dlbufaddr =
+	    HFA384x_ADDR_AUX_MKFLAT(hw->bufinfo.page, hw->bufinfo.offset);
+	pr_debug("dlbuf.page=0x%04x dlbuf.offset=0x%04x dlbufaddr=0x%08x\n",
+		 hw->bufinfo.page, hw->bufinfo.offset, dlbufaddr);
+	/* Calculations to determine how many fills of the dlbuffer to do
+	 * and how many USB wmemreq's to do for each fill.  At this point
+	 * in time, the dlbuffer size and the wmemreq size are the same.
+	 * Therefore, nwrites should always be 1.  The extra complexity
+	 * here is a hedge against future changes.
+	 */
+
+	/* Figure out how many times to do the flash programming */
+	nburns = len / hw->bufinfo.len;
+	nburns += (len % hw->bufinfo.len) ? 1 : 0;
+
+	/* For each flash program cycle, how many USB wmemreq's are needed? */
+	nwrites = hw->bufinfo.len / HFA384x_USB_RWMEM_MAXLEN;
+	nwrites += (hw->bufinfo.len % HFA384x_USB_RWMEM_MAXLEN) ? 1 : 0;
+
+	/* For each burn */
+	for (i = 0; i < nburns; i++) {
+		/* Get the dest address and len */
+		burnlen = (len - (hw->bufinfo.len * i)) > hw->bufinfo.len ?
+		    hw->bufinfo.len : (len - (hw->bufinfo.len * i));
+		burndaddr = daddr + (hw->bufinfo.len * i);
+		burnlo = HFA384x_ADDR_CMD_MKOFF(burndaddr);
+		burnhi = HFA384x_ADDR_CMD_MKPAGE(burndaddr);
+
+		netdev_info(hw->wlandev->netdev, "Writing %d bytes to flash @0x%06x\n",
+			    burnlen, burndaddr);
+
+		/* Set the download mode */
+		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NV,
+					      burnlo, burnhi, burnlen);
+		if (result) {
+			netdev_err(hw->wlandev->netdev,
+				   "download(NV,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
+			goto exit_proc;
+		}
+
+		/* copy the data to the flash download buffer */
+		for (j = 0; j < nwrites; j++) {
+			writebuf = buf +
+			    (i * hw->bufinfo.len) +
+			    (j * HFA384x_USB_RWMEM_MAXLEN);
+
+			writepage = HFA384x_ADDR_CMD_MKPAGE(dlbufaddr +
+						(j * HFA384x_USB_RWMEM_MAXLEN));
+			writeoffset = HFA384x_ADDR_CMD_MKOFF(dlbufaddr +
+						(j * HFA384x_USB_RWMEM_MAXLEN));
+
+			writelen = burnlen - (j * HFA384x_USB_RWMEM_MAXLEN);
+			writelen = writelen > HFA384x_USB_RWMEM_MAXLEN ?
+			    HFA384x_USB_RWMEM_MAXLEN : writelen;
+
+			result = hfa384x_dowmem(hw,
+						writepage,
+						writeoffset,
+						writebuf, writelen);
+		}
+
+		/* set the download 'write flash' mode */
+		result = hfa384x_cmd_download(hw,
+					      HFA384x_PROGMODE_NVWRITE,
+					      0, 0, 0);
+		if (result) {
+			netdev_err(hw->wlandev->netdev,
+				   "download(NVWRITE,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
+			goto exit_proc;
+		}
+
+		/* TODO: We really should do a readback and compare. */
+	}
+
+exit_proc:
+
+	/* Leave the firmware in the 'post-prog' mode.  flashdl_disable will */
+	/*  actually disable programming mode.  Remember, that will cause the */
+	/*  the firmware to effectively reset itself. */
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_getconfig
+ *
+ * Performs the sequence necessary to read a config/info item.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	rid		config/info record id (host order)
+ *	buf		host side record buffer.  Upon return it will
+ *			contain the body portion of the record (minus the
+ *			RID and len).
+ *	len		buffer length (in bytes, should match record length)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *	-ENODATA	length mismatch between argument and retrieved
+ *			record.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_getconfig(struct hfa384x *hw, u16 rid, void *buf, u16 len)
+{
+	return hfa384x_dorrid(hw, DOWAIT, rid, buf, len, NULL, NULL, NULL);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_setconfig_async
+ *
+ * Performs the sequence necessary to write a config/info item.
+ *
+ * Arguments:
+ *       hw              device structure
+ *       rid             config/info record id (in host order)
+ *       buf             host side record buffer
+ *       len             buffer length (in bytes)
+ *       usercb          completion callback
+ *       usercb_data     completion callback argument
+ *
+ * Returns:
+ *       0               success
+ *       >0              f/w reported error - f/w status code
+ *       <0              driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *       process
+ *----------------------------------------------------------------
+ */
+int
+hfa384x_drvr_setconfig_async(struct hfa384x *hw,
+			     u16 rid,
+			     void *buf,
+			     u16 len, ctlx_usercb_t usercb, void *usercb_data)
+{
+	return hfa384x_dowrid(hw, DOASYNC, rid, buf, len, hfa384x_cb_status,
+			      usercb, usercb_data);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_ramdl_disable
+ *
+ * Ends the ram download state.
+ *
+ * Arguments:
+ *	hw		device structure
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_ramdl_disable(struct hfa384x *hw)
+{
+	/* Check that we're already in the download state */
+	if (hw->dlstate != HFA384x_DLSTATE_RAMENABLED)
+		return -EINVAL;
+
+	pr_debug("ramdl_disable()\n");
+
+	/* There isn't much we can do at this point, so I don't */
+	/*  bother  w/ the return value */
+	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0, 0);
+	hw->dlstate = HFA384x_DLSTATE_DISABLED;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_ramdl_enable
+ *
+ * Begins the ram download state.  Checks to see that we're not
+ * already in a download state and that a port isn't enabled.
+ * Sets the download state and calls cmd_download with the
+ * ENABLE_VOLATILE subcommand and the exeaddr argument.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	exeaddr		the card execution address that will be
+ *                       jumped to when ramdl_disable() is called
+ *			(host order).
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_ramdl_enable(struct hfa384x *hw, u32 exeaddr)
+{
+	int result = 0;
+	u16 lowaddr;
+	u16 hiaddr;
+	int i;
+
+	/* Check that a port isn't active */
+	for (i = 0; i < HFA384x_PORTID_MAX; i++) {
+		if (hw->port_enabled[i]) {
+			netdev_err(hw->wlandev->netdev,
+				   "Can't download with a macport enabled.\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Check that we're not already in a download state */
+	if (hw->dlstate != HFA384x_DLSTATE_DISABLED) {
+		netdev_err(hw->wlandev->netdev,
+			   "Download state not disabled.\n");
+		return -EINVAL;
+	}
+
+	pr_debug("ramdl_enable, exeaddr=0x%08x\n", exeaddr);
+
+	/* Call the download(1,addr) function */
+	lowaddr = HFA384x_ADDR_CMD_MKOFF(exeaddr);
+	hiaddr = HFA384x_ADDR_CMD_MKPAGE(exeaddr);
+
+	result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_RAM,
+				      lowaddr, hiaddr, 0);
+
+	if (result == 0) {
+		/* Set the download state */
+		hw->dlstate = HFA384x_DLSTATE_RAMENABLED;
+	} else {
+		pr_debug("cmd_download(0x%04x, 0x%04x) failed, result=%d.\n",
+			 lowaddr, hiaddr, result);
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_ramdl_write
+ *
+ * Performs a RAM download of a chunk of data. First checks to see
+ * that we're in the RAM download state, then uses the [read|write]mem USB
+ * commands to 1) copy the data, 2) readback and compare.  The download
+ * state is unaffected.  When all data has been written using
+ * this function, call drvr_ramdl_disable() to end the download state
+ * and restart the MAC.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	daddr		Card address to write to. (host order)
+ *	buf		Ptr to data to write.
+ *	len		Length of data (host order).
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_ramdl_write(struct hfa384x *hw, u32 daddr, void *buf, u32 len)
+{
+	int result = 0;
+	int nwrites;
+	u8 *data = buf;
+	int i;
+	u32 curraddr;
+	u16 currpage;
+	u16 curroffset;
+	u16 currlen;
+
+	/* Check that we're in the ram download state */
+	if (hw->dlstate != HFA384x_DLSTATE_RAMENABLED)
+		return -EINVAL;
+
+	netdev_info(hw->wlandev->netdev, "Writing %d bytes to ram @0x%06x\n",
+		    len, daddr);
+
+	/* How many dowmem calls?  */
+	nwrites = len / HFA384x_USB_RWMEM_MAXLEN;
+	nwrites += len % HFA384x_USB_RWMEM_MAXLEN ? 1 : 0;
+
+	/* Do blocking wmem's */
+	for (i = 0; i < nwrites; i++) {
+		/* make address args */
+		curraddr = daddr + (i * HFA384x_USB_RWMEM_MAXLEN);
+		currpage = HFA384x_ADDR_CMD_MKPAGE(curraddr);
+		curroffset = HFA384x_ADDR_CMD_MKOFF(curraddr);
+		currlen = len - (i * HFA384x_USB_RWMEM_MAXLEN);
+		if (currlen > HFA384x_USB_RWMEM_MAXLEN)
+			currlen = HFA384x_USB_RWMEM_MAXLEN;
+
+		/* Do blocking ctlx */
+		result = hfa384x_dowmem(hw,
+					currpage,
+					curroffset,
+					data + (i * HFA384x_USB_RWMEM_MAXLEN),
+					currlen);
+
+		if (result)
+			break;
+
+		/* TODO: We really should have a readback. */
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_readpda
+ *
+ * Performs the sequence to read the PDA space.  Note there is no
+ * drvr_writepda() function.  Writing a PDA is
+ * generally implemented by a calling component via calls to
+ * cmd_download and writing to the flash download buffer via the
+ * aux regs.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	buf		buffer to store PDA in
+ *	len		buffer length
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *	-ETIMEDOUT	timeout waiting for the cmd regs to become
+ *			available, or waiting for the control reg
+ *			to indicate the Aux port is enabled.
+ *	-ENODATA	the buffer does NOT contain a valid PDA.
+ *			Either the card PDA is bad, or the auxdata
+ *			reads are giving us garbage.
+ *
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process or non-card interrupt.
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_readpda(struct hfa384x *hw, void *buf, unsigned int len)
+{
+	int result = 0;
+	__le16 *pda = buf;
+	int pdaok = 0;
+	int morepdrs = 1;
+	int currpdr = 0;	/* word offset of the current pdr */
+	size_t i;
+	u16 pdrlen;		/* pdr length in bytes, host order */
+	u16 pdrcode;		/* pdr code, host order */
+	u16 currpage;
+	u16 curroffset;
+	struct pdaloc {
+		u32 cardaddr;
+		u16 auxctl;
+	} pdaloc[] = {
+		{
+		HFA3842_PDA_BASE, 0}, {
+		HFA3841_PDA_BASE, 0}, {
+		HFA3841_PDA_BOGUS_BASE, 0}
+	};
+
+	/* Read the pda from each known address.  */
+	for (i = 0; i < ARRAY_SIZE(pdaloc); i++) {
+		/* Make address */
+		currpage = HFA384x_ADDR_CMD_MKPAGE(pdaloc[i].cardaddr);
+		curroffset = HFA384x_ADDR_CMD_MKOFF(pdaloc[i].cardaddr);
+
+		/* units of bytes */
+		result = hfa384x_dormem(hw, currpage, curroffset, buf,
+					len);
+
+		if (result) {
+			netdev_warn(hw->wlandev->netdev,
+				    "Read from index %zd failed, continuing\n",
+				    i);
+			continue;
+		}
+
+		/* Test for garbage */
+		pdaok = 1;	/* initially assume good */
+		morepdrs = 1;
+		while (pdaok && morepdrs) {
+			pdrlen = le16_to_cpu(pda[currpdr]) * 2;
+			pdrcode = le16_to_cpu(pda[currpdr + 1]);
+			/* Test the record length */
+			if (pdrlen > HFA384x_PDR_LEN_MAX || pdrlen == 0) {
+				netdev_err(hw->wlandev->netdev,
+					   "pdrlen invalid=%d\n", pdrlen);
+				pdaok = 0;
+				break;
+			}
+			/* Test the code */
+			if (!hfa384x_isgood_pdrcode(pdrcode)) {
+				netdev_err(hw->wlandev->netdev, "pdrcode invalid=%d\n",
+					   pdrcode);
+				pdaok = 0;
+				break;
+			}
+			/* Test for completion */
+			if (pdrcode == HFA384x_PDR_END_OF_PDA)
+				morepdrs = 0;
+
+			/* Move to the next pdr (if necessary) */
+			if (morepdrs) {
+				/* note the access to pda[], need words here */
+				currpdr += le16_to_cpu(pda[currpdr]) + 1;
+			}
+		}
+		if (pdaok) {
+			netdev_info(hw->wlandev->netdev,
+				    "PDA Read from 0x%08x in %s space.\n",
+				    pdaloc[i].cardaddr,
+				    pdaloc[i].auxctl == 0 ? "EXTDS" :
+				    pdaloc[i].auxctl == 1 ? "NV" :
+				    pdaloc[i].auxctl == 2 ? "PHY" :
+				    pdaloc[i].auxctl == 3 ? "ICSRAM" :
+				    "<bogus auxctl>");
+			break;
+		}
+	}
+	result = pdaok ? 0 : -ENODATA;
+
+	if (result)
+		pr_debug("Failure: pda is not okay\n");
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_setconfig
+ *
+ * Performs the sequence necessary to write a config/info item.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	rid		config/info record id (in host order)
+ *	buf		host side record buffer
+ *	len		buffer length (in bytes)
+ *
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_setconfig(struct hfa384x *hw, u16 rid, void *buf, u16 len)
+{
+	return hfa384x_dowrid(hw, DOWAIT, rid, buf, len, NULL, NULL, NULL);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_start
+ *
+ * Issues the MAC initialize command, sets up some data structures,
+ * and enables the interrupts.  After this function completes, the
+ * low-level stuff should be ready for any/all commands.
+ *
+ * Arguments:
+ *	hw		device structure
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_start(struct hfa384x *hw)
+{
+	int result, result1, result2;
+	u16 status;
+
+	might_sleep();
+
+	/* Clear endpoint stalls - but only do this if the endpoint
+	 * is showing a stall status. Some prism2 cards seem to behave
+	 * badly if a clear_halt is called when the endpoint is already
+	 * ok
+	 */
+	result =
+	    usb_get_std_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_in,
+			       &status);
+	if (result < 0) {
+		netdev_err(hw->wlandev->netdev, "Cannot get bulk in endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_in))
+		netdev_err(hw->wlandev->netdev, "Failed to reset bulk in endpoint.\n");
+
+	result =
+	    usb_get_std_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_out,
+			       &status);
+	if (result < 0) {
+		netdev_err(hw->wlandev->netdev, "Cannot get bulk out endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_out))
+		netdev_err(hw->wlandev->netdev, "Failed to reset bulk out endpoint.\n");
+
+	/* Synchronous unlink, in case we're trying to restart the driver */
+	usb_kill_urb(&hw->rx_urb);
+
+	/* Post the IN urb */
+	result = submit_rx_urb(hw, GFP_KERNEL);
+	if (result != 0) {
+		netdev_err(hw->wlandev->netdev,
+			   "Fatal, failed to submit RX URB, result=%d\n",
+			   result);
+		goto done;
+	}
+
+	/* Call initialize twice, with a 1 second sleep in between.
+	 * This is a nasty work-around since many prism2 cards seem to
+	 * need time to settle after an init from cold. The second
+	 * call to initialize in theory is not necessary - but we call
+	 * it anyway as a double insurance policy:
+	 * 1) If the first init should fail, the second may well succeed
+	 *    and the card can still be used
+	 * 2) It helps ensures all is well with the card after the first
+	 *    init and settle time.
+	 */
+	result1 = hfa384x_cmd_initialize(hw);
+	msleep(1000);
+	result = hfa384x_cmd_initialize(hw);
+	result2 = result;
+	if (result1 != 0) {
+		if (result2 != 0) {
+			netdev_err(hw->wlandev->netdev,
+				   "cmd_initialize() failed on two attempts, results %d and %d\n",
+				   result1, result2);
+			usb_kill_urb(&hw->rx_urb);
+			goto done;
+		} else {
+			pr_debug("First cmd_initialize() failed (result %d),\n",
+				 result1);
+			pr_debug("but second attempt succeeded. All should be ok\n");
+		}
+	} else if (result2 != 0) {
+		netdev_warn(hw->wlandev->netdev, "First cmd_initialize() succeeded, but second attempt failed (result=%d)\n",
+			    result2);
+		netdev_warn(hw->wlandev->netdev,
+			    "Most likely the card will be functional\n");
+		goto done;
+	}
+
+	hw->state = HFA384x_STATE_RUNNING;
+
+done:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_stop
+ *
+ * Shuts down the MAC to the point where it is safe to unload the
+ * driver.  Any subsystem that may be holding a data or function
+ * ptr into the driver must be cleared/deinitialized.
+ *
+ * Arguments:
+ *	hw		device structure
+ * Returns:
+ *	0		success
+ *	>0		f/w reported error - f/w status code
+ *	<0		driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_stop(struct hfa384x *hw)
+{
+	int i;
+
+	might_sleep();
+
+	/* There's no need for spinlocks here. The USB "disconnect"
+	 * function sets this "removed" flag and then calls us.
+	 */
+	if (!hw->wlandev->hwremoved) {
+		/* Call initialize to leave the MAC in its 'reset' state */
+		hfa384x_cmd_initialize(hw);
+
+		/* Cancel the rxurb */
+		usb_kill_urb(&hw->rx_urb);
+	}
+
+	hw->link_status = HFA384x_LINK_NOTCONNECTED;
+	hw->state = HFA384x_STATE_INIT;
+
+	del_timer_sync(&hw->commsqual_timer);
+
+	/* Clear all the port status */
+	for (i = 0; i < HFA384x_NUMPORTS_MAX; i++)
+		hw->port_enabled[i] = 0;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_drvr_txframe
+ *
+ * Takes a frame from prism2sta and queues it for transmission.
+ *
+ * Arguments:
+ *	hw		device structure
+ *	skb		packet buffer struct.  Contains an 802.11
+ *			data frame.
+ *       p80211_hdr      points to the 802.11 header for the packet.
+ * Returns:
+ *	0		Success and more buffs available
+ *	1		Success but no more buffs
+ *	2		Allocation failure
+ *	4		Buffer full or queue busy
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int hfa384x_drvr_txframe(struct hfa384x *hw, struct sk_buff *skb,
+			 struct p80211_hdr *p80211_hdr,
+			 struct p80211_metawep *p80211_wep)
+{
+	int usbpktlen = sizeof(struct hfa384x_tx_frame);
+	int result;
+	int ret;
+	char *ptr;
+
+	if (hw->tx_urb.status == -EINPROGRESS) {
+		netdev_warn(hw->wlandev->netdev, "TX URB already in use\n");
+		result = 3;
+		goto exit;
+	}
+
+	/* Build Tx frame structure */
+	/* Set up the control field */
+	memset(&hw->txbuff.txfrm.desc, 0, sizeof(hw->txbuff.txfrm.desc));
+
+	/* Setup the usb type field */
+	hw->txbuff.type = cpu_to_le16(HFA384x_USB_TXFRM);
+
+	/* Set up the sw_support field to identify this frame */
+	hw->txbuff.txfrm.desc.sw_support = 0x0123;
+
+/* Tx complete and Tx exception disable per dleach.  Might be causing
+ * buf depletion
+ */
+/* #define DOEXC  SLP -- doboth breaks horribly under load, doexc less so. */
+#if defined(DOBOTH)
+	hw->txbuff.txfrm.desc.tx_control =
+	    HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
+	    HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(1);
+#elif defined(DOEXC)
+	hw->txbuff.txfrm.desc.tx_control =
+	    HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
+	    HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(0);
+#else
+	hw->txbuff.txfrm.desc.tx_control =
+	    HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
+	    HFA384x_TX_TXEX_SET(0) | HFA384x_TX_TXOK_SET(0);
+#endif
+	cpu_to_le16s(&hw->txbuff.txfrm.desc.tx_control);
+
+	/* copy the header over to the txdesc */
+	hw->txbuff.txfrm.desc.hdr = *p80211_hdr;
+
+	/* if we're using host WEP, increase size by IV+ICV */
+	if (p80211_wep->data) {
+		hw->txbuff.txfrm.desc.data_len = cpu_to_le16(skb->len + 8);
+		usbpktlen += 8;
+	} else {
+		hw->txbuff.txfrm.desc.data_len = cpu_to_le16(skb->len);
+	}
+
+	usbpktlen += skb->len;
+
+	/* copy over the WEP IV if we are using host WEP */
+	ptr = hw->txbuff.txfrm.data;
+	if (p80211_wep->data) {
+		memcpy(ptr, p80211_wep->iv, sizeof(p80211_wep->iv));
+		ptr += sizeof(p80211_wep->iv);
+		memcpy(ptr, p80211_wep->data, skb->len);
+	} else {
+		memcpy(ptr, skb->data, skb->len);
+	}
+	/* copy over the packet data */
+	ptr += skb->len;
+
+	/* copy over the WEP ICV if we are using host WEP */
+	if (p80211_wep->data)
+		memcpy(ptr, p80211_wep->icv, sizeof(p80211_wep->icv));
+
+	/* Send the USB packet */
+	usb_fill_bulk_urb(&hw->tx_urb, hw->usb,
+			  hw->endp_out,
+			  &hw->txbuff, ROUNDUP64(usbpktlen),
+			  hfa384x_usbout_callback, hw->wlandev);
+	hw->tx_urb.transfer_flags |= USB_QUEUE_BULK;
+
+	result = 1;
+	ret = submit_tx_urb(hw, &hw->tx_urb, GFP_ATOMIC);
+	if (ret != 0) {
+		netdev_err(hw->wlandev->netdev,
+			   "submit_tx_urb() failed, error=%d\n", ret);
+		result = 3;
+	}
+
+exit:
+	return result;
+}
+
+void hfa384x_tx_timeout(struct wlandevice *wlandev)
+{
+	struct hfa384x *hw = wlandev->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	if (!hw->wlandev->hwremoved) {
+		int sched;
+
+		sched = !test_and_set_bit(WORK_TX_HALT, &hw->usb_flags);
+		sched |= !test_and_set_bit(WORK_RX_HALT, &hw->usb_flags);
+		if (sched)
+			schedule_work(&hw->usb_work);
+	}
+
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_reaper_task
+ *
+ * Deferred work callback to delete dead CTLX objects
+ *
+ * Arguments:
+ *	work	contains ptr to a struct hfa384x
+ *
+ * Returns:
+ *
+ * Call context:
+ *      Task
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbctlx_reaper_task(struct work_struct *work)
+{
+	struct hfa384x *hw = container_of(work, struct hfa384x, reaper_bh);
+	struct hfa384x_usbctlx *ctlx, *temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/* This list is guaranteed to be empty if someone
+	 * has unplugged the adapter.
+	 */
+	list_for_each_entry_safe(ctlx, temp, &hw->ctlxq.reapable, list) {
+		list_del(&ctlx->list);
+		kfree(ctlx);
+	}
+
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_completion_task
+ *
+ * Deferred work callback to call completion handlers for returned CTLXs
+ *
+ * Arguments:
+ *	work	contains ptr to a struct hfa384x
+ *
+ * Returns:
+ *	Nothing
+ *
+ * Call context:
+ *      Task
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbctlx_completion_task(struct work_struct *work)
+{
+	struct hfa384x *hw = container_of(work, struct hfa384x, completion_bh);
+	struct hfa384x_usbctlx *ctlx, *temp;
+	unsigned long flags;
+
+	int reap = 0;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/* This list is guaranteed to be empty if someone
+	 * has unplugged the adapter ...
+	 */
+	list_for_each_entry_safe(ctlx, temp, &hw->ctlxq.completing, list) {
+		/* Call the completion function that this
+		 * command was assigned, assuming it has one.
+		 */
+		if (ctlx->cmdcb) {
+			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+			ctlx->cmdcb(hw, ctlx);
+			spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+			/* Make sure we don't try and complete
+			 * this CTLX more than once!
+			 */
+			ctlx->cmdcb = NULL;
+
+			/* Did someone yank the adapter out
+			 * while our list was (briefly) unlocked?
+			 */
+			if (hw->wlandev->hwremoved) {
+				reap = 0;
+				break;
+			}
+		}
+
+		/*
+		 * "Reapable" CTLXs are ones which don't have any
+		 * threads waiting for them to die. Hence they must
+		 * be delivered to The Reaper!
+		 */
+		if (ctlx->reapable) {
+			/* Move the CTLX off the "completing" list (hopefully)
+			 * on to the "reapable" list where the reaper task
+			 * can find it. And "reapable" means that this CTLX
+			 * isn't sitting on a wait-queue somewhere.
+			 */
+			list_move_tail(&ctlx->list, &hw->ctlxq.reapable);
+			reap = 1;
+		}
+
+		complete(&ctlx->done);
+	}
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+	if (reap)
+		schedule_work(&hw->reaper_bh);
+}
+
+/*----------------------------------------------------------------
+ * unlocked_usbctlx_cancel_async
+ *
+ * Mark the CTLX dead asynchronously, and ensure that the
+ * next command on the queue is run afterwards.
+ *
+ * Arguments:
+ *	hw	ptr to the struct hfa384x structure
+ *	ctlx	ptr to a CTLX structure
+ *
+ * Returns:
+ *	0	the CTLX's URB is inactive
+ * -EINPROGRESS	the URB is currently being unlinked
+ *
+ * Call context:
+ *	Either process or interrupt, but presumably interrupt
+ *----------------------------------------------------------------
+ */
+static int unlocked_usbctlx_cancel_async(struct hfa384x *hw,
+					 struct hfa384x_usbctlx *ctlx)
+{
+	int ret;
+
+	/*
+	 * Try to delete the URB containing our request packet.
+	 * If we succeed, then its completion handler will be
+	 * called with a status of -ECONNRESET.
+	 */
+	hw->ctlx_urb.transfer_flags |= URB_ASYNC_UNLINK;
+	ret = usb_unlink_urb(&hw->ctlx_urb);
+
+	if (ret != -EINPROGRESS) {
+		/*
+		 * The OUT URB had either already completed
+		 * or was still in the pending queue, so the
+		 * URB's completion function will not be called.
+		 * We will have to complete the CTLX ourselves.
+		 */
+		ctlx->state = CTLX_REQ_FAILED;
+		unlocked_usbctlx_complete(hw, ctlx);
+		ret = 0;
+	}
+
+	return ret;
+}
+
+/*----------------------------------------------------------------
+ * unlocked_usbctlx_complete
+ *
+ * A CTLX has completed.  It may have been successful, it may not
+ * have been. At this point, the CTLX should be quiescent.  The URBs
+ * aren't active and the timers should have been stopped.
+ *
+ * The CTLX is migrated to the "completing" queue, and the completing
+ * work is scheduled.
+ *
+ * Arguments:
+ *	hw		ptr to a struct hfa384x structure
+ *	ctlx		ptr to a ctlx structure
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	Either, assume interrupt
+ *----------------------------------------------------------------
+ */
+static void unlocked_usbctlx_complete(struct hfa384x *hw,
+				      struct hfa384x_usbctlx *ctlx)
+{
+	/* Timers have been stopped, and ctlx should be in
+	 * a terminal state. Retire it from the "active"
+	 * queue.
+	 */
+	list_move_tail(&ctlx->list, &hw->ctlxq.completing);
+	schedule_work(&hw->completion_bh);
+
+	switch (ctlx->state) {
+	case CTLX_COMPLETE:
+	case CTLX_REQ_FAILED:
+		/* This are the correct terminating states. */
+		break;
+
+	default:
+		netdev_err(hw->wlandev->netdev, "CTLX[%d] not in a terminating state(%s)\n",
+			   le16_to_cpu(ctlx->outbuf.type),
+			   ctlxstr(ctlx->state));
+		break;
+	}			/* switch */
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlxq_run
+ *
+ * Checks to see if the head item is running.  If not, starts it.
+ *
+ * Arguments:
+ *	hw	ptr to struct hfa384x
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	any
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbctlxq_run(struct hfa384x *hw)
+{
+	unsigned long flags;
+
+	/* acquire lock */
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/* Only one active CTLX at any one time, because there's no
+	 * other (reliable) way to match the response URB to the
+	 * correct CTLX.
+	 *
+	 * Don't touch any of these CTLXs if the hardware
+	 * has been removed or the USB subsystem is stalled.
+	 */
+	if (!list_empty(&hw->ctlxq.active) ||
+	    test_bit(WORK_TX_HALT, &hw->usb_flags) || hw->wlandev->hwremoved)
+		goto unlock;
+
+	while (!list_empty(&hw->ctlxq.pending)) {
+		struct hfa384x_usbctlx *head;
+		int result;
+
+		/* This is the first pending command */
+		head = list_entry(hw->ctlxq.pending.next,
+				  struct hfa384x_usbctlx, list);
+
+		/* We need to split this off to avoid a race condition */
+		list_move_tail(&head->list, &hw->ctlxq.active);
+
+		/* Fill the out packet */
+		usb_fill_bulk_urb(&hw->ctlx_urb, hw->usb,
+				  hw->endp_out,
+				  &head->outbuf, ROUNDUP64(head->outbufsize),
+				  hfa384x_ctlxout_callback, hw);
+		hw->ctlx_urb.transfer_flags |= USB_QUEUE_BULK;
+
+		/* Now submit the URB and update the CTLX's state */
+		result = usb_submit_urb(&hw->ctlx_urb, GFP_ATOMIC);
+		if (result == 0) {
+			/* This CTLX is now running on the active queue */
+			head->state = CTLX_REQ_SUBMITTED;
+
+			/* Start the OUT wait timer */
+			hw->req_timer_done = 0;
+			hw->reqtimer.expires = jiffies + HZ;
+			add_timer(&hw->reqtimer);
+
+			/* Start the IN wait timer */
+			hw->resp_timer_done = 0;
+			hw->resptimer.expires = jiffies + 2 * HZ;
+			add_timer(&hw->resptimer);
+
+			break;
+		}
+
+		if (result == -EPIPE) {
+			/* The OUT pipe needs resetting, so put
+			 * this CTLX back in the "pending" queue
+			 * and schedule a reset ...
+			 */
+			netdev_warn(hw->wlandev->netdev,
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
+			list_move(&head->list, &hw->ctlxq.pending);
+			set_bit(WORK_TX_HALT, &hw->usb_flags);
+			schedule_work(&hw->usb_work);
+			break;
+		}
+
+		if (result == -ESHUTDOWN) {
+			netdev_warn(hw->wlandev->netdev, "%s urb shutdown!\n",
+				    hw->wlandev->netdev->name);
+			break;
+		}
+
+		netdev_err(hw->wlandev->netdev, "Failed to submit CTLX[%d]: error=%d\n",
+			   le16_to_cpu(head->outbuf.type), result);
+		unlocked_usbctlx_complete(hw, head);
+	}			/* while */
+
+unlock:
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbin_callback
+ *
+ * Callback for URBs on the BULKIN endpoint.
+ *
+ * Arguments:
+ *	urb		ptr to the completed urb
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbin_callback(struct urb *urb)
+{
+	struct wlandevice *wlandev = urb->context;
+	struct hfa384x *hw;
+	union hfa384x_usbin *usbin;
+	struct sk_buff *skb = NULL;
+	int result;
+	int urb_status;
+	u16 type;
+
+	enum USBIN_ACTION {
+		HANDLE,
+		RESUBMIT,
+		ABORT
+	} action;
+
+	if (!wlandev || !wlandev->netdev || wlandev->hwremoved)
+		goto exit;
+
+	hw = wlandev->priv;
+	if (!hw)
+		goto exit;
+
+	skb = hw->rx_urb_skb;
+	if (!skb || (skb->data != urb->transfer_buffer)) {
+		WARN_ON(1);
+		return;
+	}
+
+	hw->rx_urb_skb = NULL;
+
+	/* Check for error conditions within the URB */
+	switch (urb->status) {
+	case 0:
+		action = HANDLE;
+
+		/* Check for short packet */
+		if (urb->actual_length == 0) {
+			wlandev->netdev->stats.rx_errors++;
+			wlandev->netdev->stats.rx_length_errors++;
+			action = RESUBMIT;
+		}
+		break;
+
+	case -EPIPE:
+		netdev_warn(hw->wlandev->netdev, "%s rx pipe stalled: requesting reset\n",
+			    wlandev->netdev->name);
+		if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
+			schedule_work(&hw->usb_work);
+		wlandev->netdev->stats.rx_errors++;
+		action = ABORT;
+		break;
+
+	case -EILSEQ:
+	case -ETIMEDOUT:
+	case -EPROTO:
+		if (!test_and_set_bit(THROTTLE_RX, &hw->usb_flags) &&
+		    !timer_pending(&hw->throttle)) {
+			mod_timer(&hw->throttle, jiffies + THROTTLE_JIFFIES);
+		}
+		wlandev->netdev->stats.rx_errors++;
+		action = ABORT;
+		break;
+
+	case -EOVERFLOW:
+		wlandev->netdev->stats.rx_over_errors++;
+		action = RESUBMIT;
+		break;
+
+	case -ENODEV:
+	case -ESHUTDOWN:
+		pr_debug("status=%d, device removed.\n", urb->status);
+		action = ABORT;
+		break;
+
+	case -ENOENT:
+	case -ECONNRESET:
+		pr_debug("status=%d, urb explicitly unlinked.\n", urb->status);
+		action = ABORT;
+		break;
+
+	default:
+		pr_debug("urb status=%d, transfer flags=0x%x\n",
+			 urb->status, urb->transfer_flags);
+		wlandev->netdev->stats.rx_errors++;
+		action = RESUBMIT;
+		break;
+	}
+
+	/* Save values from the RX URB before reposting overwrites it. */
+	urb_status = urb->status;
+	usbin = (union hfa384x_usbin *)urb->transfer_buffer;
+
+	if (action != ABORT) {
+		/* Repost the RX URB */
+		result = submit_rx_urb(hw, GFP_ATOMIC);
+
+		if (result != 0) {
+			netdev_err(hw->wlandev->netdev,
+				   "Fatal, failed to resubmit rx_urb. error=%d\n",
+				   result);
+		}
+	}
+
+	/* Handle any USB-IN packet */
+	/* Note: the check of the sw_support field, the type field doesn't
+	 *       have bit 12 set like the docs suggest.
+	 */
+	type = le16_to_cpu(usbin->type);
+	if (HFA384x_USB_ISRXFRM(type)) {
+		if (action == HANDLE) {
+			if (usbin->txfrm.desc.sw_support == 0x0123) {
+				hfa384x_usbin_txcompl(wlandev, usbin);
+			} else {
+				skb_put(skb, sizeof(*usbin));
+				hfa384x_usbin_rx(wlandev, skb);
+				skb = NULL;
+			}
+		}
+		goto exit;
+	}
+	if (HFA384x_USB_ISTXFRM(type)) {
+		if (action == HANDLE)
+			hfa384x_usbin_txcompl(wlandev, usbin);
+		goto exit;
+	}
+	switch (type) {
+	case HFA384x_USB_INFOFRM:
+		if (action == ABORT)
+			goto exit;
+		if (action == HANDLE)
+			hfa384x_usbin_info(wlandev, usbin);
+		break;
+
+	case HFA384x_USB_CMDRESP:
+	case HFA384x_USB_WRIDRESP:
+	case HFA384x_USB_RRIDRESP:
+	case HFA384x_USB_WMEMRESP:
+	case HFA384x_USB_RMEMRESP:
+		/* ALWAYS, ALWAYS, ALWAYS handle this CTLX!!!! */
+		hfa384x_usbin_ctlx(hw, usbin, urb_status);
+		break;
+
+	case HFA384x_USB_BUFAVAIL:
+		pr_debug("Received BUFAVAIL packet, frmlen=%d\n",
+			 usbin->bufavail.frmlen);
+		break;
+
+	case HFA384x_USB_ERROR:
+		pr_debug("Received USB_ERROR packet, errortype=%d\n",
+			 usbin->usberror.errortype);
+		break;
+
+	default:
+		pr_debug("Unrecognized USBIN packet, type=%x, status=%d\n",
+			 usbin->type, urb_status);
+		break;
+	}			/* switch */
+
+exit:
+
+	if (skb)
+		dev_kfree_skb(skb);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbin_ctlx
+ *
+ * We've received a URB containing a Prism2 "response" message.
+ * This message needs to be matched up with a CTLX on the active
+ * queue and our state updated accordingly.
+ *
+ * Arguments:
+ *	hw		ptr to struct hfa384x
+ *	usbin		ptr to USB IN packet
+ *	urb_status	status of this Bulk-In URB
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbin_ctlx(struct hfa384x *hw, union hfa384x_usbin *usbin,
+			       int urb_status)
+{
+	struct hfa384x_usbctlx *ctlx;
+	int run_queue = 0;
+	unsigned long flags;
+
+retry:
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/* There can be only one CTLX on the active queue
+	 * at any one time, and this is the CTLX that the
+	 * timers are waiting for.
+	 */
+	if (list_empty(&hw->ctlxq.active))
+		goto unlock;
+
+	/* Remove the "response timeout". It's possible that
+	 * we are already too late, and that the timeout is
+	 * already running. And that's just too bad for us,
+	 * because we could lose our CTLX from the active
+	 * queue here ...
+	 */
+	if (del_timer(&hw->resptimer) == 0) {
+		if (hw->resp_timer_done == 0) {
+			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+			goto retry;
+		}
+	} else {
+		hw->resp_timer_done = 1;
+	}
+
+	ctlx = get_active_ctlx(hw);
+
+	if (urb_status != 0) {
+		/*
+		 * Bad CTLX, so get rid of it. But we only
+		 * remove it from the active queue if we're no
+		 * longer expecting the OUT URB to complete.
+		 */
+		if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
+			run_queue = 1;
+	} else {
+		const __le16 intype = (usbin->type & ~cpu_to_le16(0x8000));
+
+		/*
+		 * Check that our message is what we're expecting ...
+		 */
+		if (ctlx->outbuf.type != intype) {
+			netdev_warn(hw->wlandev->netdev,
+				    "Expected IN[%d], received IN[%d] - ignored.\n",
+				    le16_to_cpu(ctlx->outbuf.type),
+				    le16_to_cpu(intype));
+			goto unlock;
+		}
+
+		/* This URB has succeeded, so grab the data ... */
+		memcpy(&ctlx->inbuf, usbin, sizeof(ctlx->inbuf));
+
+		switch (ctlx->state) {
+		case CTLX_REQ_SUBMITTED:
+			/*
+			 * We have received our response URB before
+			 * our request has been acknowledged. Odd,
+			 * but our OUT URB is still alive...
+			 */
+			pr_debug("Causality violation: please reboot Universe\n");
+			ctlx->state = CTLX_RESP_COMPLETE;
+			break;
+
+		case CTLX_REQ_COMPLETE:
+			/*
+			 * This is the usual path: our request
+			 * has already been acknowledged, and
+			 * now we have received the reply too.
+			 */
+			ctlx->state = CTLX_COMPLETE;
+			unlocked_usbctlx_complete(hw, ctlx);
+			run_queue = 1;
+			break;
+
+		default:
+			/*
+			 * Throw this CTLX away ...
+			 */
+			netdev_err(hw->wlandev->netdev,
+				   "Matched IN URB, CTLX[%d] in invalid state(%s). Discarded.\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state));
+			if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
+				run_queue = 1;
+			break;
+		}		/* switch */
+	}
+
+unlock:
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+	if (run_queue)
+		hfa384x_usbctlxq_run(hw);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbin_txcompl
+ *
+ * At this point we have the results of a previous transmit.
+ *
+ * Arguments:
+ *	wlandev		wlan device
+ *	usbin		ptr to the usb transfer buffer
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbin_txcompl(struct wlandevice *wlandev,
+				  union hfa384x_usbin *usbin)
+{
+	u16 status;
+
+	status = le16_to_cpu(usbin->type); /* yeah I know it says type... */
+
+	/* Was there an error? */
+	if (HFA384x_TXSTATUS_ISERROR(status))
+		prism2sta_ev_txexc(wlandev, status);
+	else
+		prism2sta_ev_tx(wlandev, status);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbin_rx
+ *
+ * At this point we have a successful received a rx frame packet.
+ *
+ * Arguments:
+ *	wlandev		wlan device
+ *	usbin		ptr to the usb transfer buffer
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbin_rx(struct wlandevice *wlandev, struct sk_buff *skb)
+{
+	union hfa384x_usbin *usbin = (union hfa384x_usbin *)skb->data;
+	struct hfa384x *hw = wlandev->priv;
+	int hdrlen;
+	struct p80211_rxmeta *rxmeta;
+	u16 data_len;
+	u16 fc;
+	u16 status;
+
+	/* Byte order convert once up front. */
+	le16_to_cpus(&usbin->rxfrm.desc.status);
+	le32_to_cpus(&usbin->rxfrm.desc.time);
+
+	/* Now handle frame based on port# */
+	status = HFA384x_RXSTATUS_MACPORT_GET(usbin->rxfrm.desc.status);
+
+	switch (status) {
+	case 0:
+		fc = le16_to_cpu(usbin->rxfrm.desc.hdr.frame_control);
+
+		/* If exclude and we receive an unencrypted, drop it */
+		if ((wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED) &&
+		    !WLAN_GET_FC_ISWEP(fc)) {
+			break;
+		}
+
+		data_len = le16_to_cpu(usbin->rxfrm.desc.data_len);
+
+		/* How much header data do we have? */
+		hdrlen = p80211_headerlen(fc);
+
+		/* Pull off the descriptor */
+		skb_pull(skb, sizeof(struct hfa384x_rx_frame));
+
+		/* Now shunt the header block up against the data block
+		 * with an "overlapping" copy
+		 */
+		memmove(skb_push(skb, hdrlen),
+			&usbin->rxfrm.desc.hdr, hdrlen);
+
+		skb->dev = wlandev->netdev;
+
+		/* And set the frame length properly */
+		skb_trim(skb, data_len + hdrlen);
+
+		/* The prism2 series does not return the CRC */
+		memset(skb_put(skb, WLAN_CRC_LEN), 0xff, WLAN_CRC_LEN);
+
+		skb_reset_mac_header(skb);
+
+		/* Attach the rxmeta, set some stuff */
+		p80211skb_rxmeta_attach(wlandev, skb);
+		rxmeta = p80211skb_rxmeta(skb);
+		rxmeta->mactime = usbin->rxfrm.desc.time;
+		rxmeta->rxrate = usbin->rxfrm.desc.rate;
+		rxmeta->signal = usbin->rxfrm.desc.signal - hw->dbmadjust;
+		rxmeta->noise = usbin->rxfrm.desc.silence - hw->dbmadjust;
+
+		p80211netdev_rx(wlandev, skb);
+
+		break;
+
+	case 7:
+		if (!HFA384x_RXSTATUS_ISFCSERR(usbin->rxfrm.desc.status)) {
+			/* Copy to wlansnif skb */
+			hfa384x_int_rxmonitor(wlandev, &usbin->rxfrm);
+			dev_kfree_skb(skb);
+		} else {
+			pr_debug("Received monitor frame: FCSerr set\n");
+		}
+		break;
+
+	default:
+		netdev_warn(hw->wlandev->netdev,
+			    "Received frame on unsupported port=%d\n",
+			    status);
+		break;
+	}
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_int_rxmonitor
+ *
+ * Helper function for int_rx.  Handles monitor frames.
+ * Note that this function allocates space for the FCS and sets it
+ * to 0xffffffff.  The hfa384x doesn't give us the FCS value but the
+ * higher layers expect it.  0xffffffff is used as a flag to indicate
+ * the FCS is bogus.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	rxfrm		rx descriptor read from card in int_rx
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *	Allocates an skb and passes it up via the PF_PACKET interface.
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_int_rxmonitor(struct wlandevice *wlandev,
+				  struct hfa384x_usb_rxfrm *rxfrm)
+{
+	struct hfa384x_rx_frame *rxdesc = &rxfrm->desc;
+	unsigned int hdrlen = 0;
+	unsigned int datalen = 0;
+	unsigned int skblen = 0;
+	u8 *datap;
+	u16 fc;
+	struct sk_buff *skb;
+	struct hfa384x *hw = wlandev->priv;
+
+	/* Remember the status, time, and data_len fields are in host order */
+	/* Figure out how big the frame is */
+	fc = le16_to_cpu(rxdesc->hdr.frame_control);
+	hdrlen = p80211_headerlen(fc);
+	datalen = le16_to_cpu(rxdesc->data_len);
+
+	/* Allocate an ind message+framesize skb */
+	skblen = sizeof(struct p80211_caphdr) + hdrlen + datalen + WLAN_CRC_LEN;
+
+	/* sanity check the length */
+	if (skblen >
+	    (sizeof(struct p80211_caphdr) +
+	     WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN)) {
+		pr_debug("overlen frm: len=%zd\n",
+			 skblen - sizeof(struct p80211_caphdr));
+
+		return;
+	}
+
+	skb = dev_alloc_skb(skblen);
+	if (!skb)
+		return;
+
+	/* only prepend the prism header if in the right mode */
+	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
+	    (hw->sniffhdr != 0)) {
+		struct p80211_caphdr *caphdr;
+		/* The NEW header format! */
+		datap = skb_put(skb, sizeof(struct p80211_caphdr));
+		caphdr = (struct p80211_caphdr *)datap;
+
+		caphdr->version = htonl(P80211CAPTURE_VERSION);
+		caphdr->length = htonl(sizeof(struct p80211_caphdr));
+		caphdr->mactime = __cpu_to_be64(rxdesc->time * 1000);
+		caphdr->hosttime = __cpu_to_be64(jiffies);
+		caphdr->phytype = htonl(4);	/* dss_dot11_b */
+		caphdr->channel = htonl(hw->sniff_channel);
+		caphdr->datarate = htonl(rxdesc->rate);
+		caphdr->antenna = htonl(0);	/* unknown */
+		caphdr->priority = htonl(0);	/* unknown */
+		caphdr->ssi_type = htonl(3);	/* rssi_raw */
+		caphdr->ssi_signal = htonl(rxdesc->signal);
+		caphdr->ssi_noise = htonl(rxdesc->silence);
+		caphdr->preamble = htonl(0);	/* unknown */
+		caphdr->encoding = htonl(1);	/* cck */
+	}
+
+	/* Copy the 802.11 header to the skb
+	 * (ctl frames may be less than a full header)
+	 */
+	skb_put_data(skb, &rxdesc->hdr.frame_control, hdrlen);
+
+	/* If any, copy the data from the card to the skb */
+	if (datalen > 0) {
+		datap = skb_put_data(skb, rxfrm->data, datalen);
+
+		/* check for unencrypted stuff if WEP bit set. */
+		if (*(datap - hdrlen + 1) & 0x40)	/* wep set */
+			if ((*(datap) == 0xaa) && (*(datap + 1) == 0xaa))
+				/* clear wep; it's the 802.2 header! */
+				*(datap - hdrlen + 1) &= 0xbf;
+	}
+
+	if (hw->sniff_fcs) {
+		/* Set the FCS */
+		datap = skb_put(skb, WLAN_CRC_LEN);
+		memset(datap, 0xff, WLAN_CRC_LEN);
+	}
+
+	/* pass it back up */
+	p80211netdev_rx(wlandev, skb);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbin_info
+ *
+ * At this point we have a successful received a Prism2 info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device
+ *	usbin		ptr to the usb transfer buffer
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbin_info(struct wlandevice *wlandev,
+			       union hfa384x_usbin *usbin)
+{
+	le16_to_cpus(&usbin->infofrm.info.framelen);
+	prism2sta_ev_info(wlandev, &usbin->infofrm.info);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbout_callback
+ *
+ * Callback for URBs on the BULKOUT endpoint.
+ *
+ * Arguments:
+ *	urb		ptr to the completed urb
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbout_callback(struct urb *urb)
+{
+	struct wlandevice *wlandev = urb->context;
+
+#ifdef DEBUG_USB
+	dbprint_urb(urb);
+#endif
+
+	if (wlandev && wlandev->netdev) {
+		switch (urb->status) {
+		case 0:
+			prism2sta_ev_alloc(wlandev);
+			break;
+
+		case -EPIPE: {
+			struct hfa384x *hw = wlandev->priv;
+
+			netdev_warn(hw->wlandev->netdev,
+				    "%s tx pipe stalled: requesting reset\n",
+				    wlandev->netdev->name);
+			if (!test_and_set_bit(WORK_TX_HALT, &hw->usb_flags))
+				schedule_work(&hw->usb_work);
+			wlandev->netdev->stats.tx_errors++;
+			break;
+		}
+
+		case -EPROTO:
+		case -ETIMEDOUT:
+		case -EILSEQ: {
+			struct hfa384x *hw = wlandev->priv;
+
+			if (!test_and_set_bit(THROTTLE_TX, &hw->usb_flags) &&
+			    !timer_pending(&hw->throttle)) {
+				mod_timer(&hw->throttle,
+					  jiffies + THROTTLE_JIFFIES);
+			}
+			wlandev->netdev->stats.tx_errors++;
+			netif_stop_queue(wlandev->netdev);
+			break;
+		}
+
+		case -ENOENT:
+		case -ESHUTDOWN:
+			/* Ignorable errors */
+			break;
+
+		default:
+			netdev_info(wlandev->netdev, "unknown urb->status=%d\n",
+				    urb->status);
+			wlandev->netdev->stats.tx_errors++;
+			break;
+		}		/* switch */
+	}
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_ctlxout_callback
+ *
+ * Callback for control data on the BULKOUT endpoint.
+ *
+ * Arguments:
+ *	urb		ptr to the completed urb
+ *
+ * Returns:
+ * nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ * interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_ctlxout_callback(struct urb *urb)
+{
+	struct hfa384x *hw = urb->context;
+	int delete_resptimer = 0;
+	int timer_ok = 1;
+	int run_queue = 0;
+	struct hfa384x_usbctlx *ctlx;
+	unsigned long flags;
+
+	pr_debug("urb->status=%d\n", urb->status);
+#ifdef DEBUG_USB
+	dbprint_urb(urb);
+#endif
+	if ((urb->status == -ESHUTDOWN) ||
+	    (urb->status == -ENODEV) || !hw)
+		return;
+
+retry:
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	/*
+	 * Only one CTLX at a time on the "active" list, and
+	 * none at all if we are unplugged. However, we can
+	 * rely on the disconnect function to clean everything
+	 * up if someone unplugged the adapter.
+	 */
+	if (list_empty(&hw->ctlxq.active)) {
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+		return;
+	}
+
+	/*
+	 * Having something on the "active" queue means
+	 * that we have timers to worry about ...
+	 */
+	if (del_timer(&hw->reqtimer) == 0) {
+		if (hw->req_timer_done == 0) {
+			/*
+			 * This timer was actually running while we
+			 * were trying to delete it. Let it terminate
+			 * gracefully instead.
+			 */
+			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+			goto retry;
+		}
+	} else {
+		hw->req_timer_done = 1;
+	}
+
+	ctlx = get_active_ctlx(hw);
+
+	if (urb->status == 0) {
+		/* Request portion of a CTLX is successful */
+		switch (ctlx->state) {
+		case CTLX_REQ_SUBMITTED:
+			/* This OUT-ACK received before IN */
+			ctlx->state = CTLX_REQ_COMPLETE;
+			break;
+
+		case CTLX_RESP_COMPLETE:
+			/* IN already received before this OUT-ACK,
+			 * so this command must now be complete.
+			 */
+			ctlx->state = CTLX_COMPLETE;
+			unlocked_usbctlx_complete(hw, ctlx);
+			run_queue = 1;
+			break;
+
+		default:
+			/* This is NOT a valid CTLX "success" state! */
+			netdev_err(hw->wlandev->netdev,
+				   "Illegal CTLX[%d] success state(%s, %d) in OUT URB\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state), urb->status);
+			break;
+		}		/* switch */
+	} else {
+		/* If the pipe has stalled then we need to reset it */
+		if ((urb->status == -EPIPE) &&
+		    !test_and_set_bit(WORK_TX_HALT, &hw->usb_flags)) {
+			netdev_warn(hw->wlandev->netdev,
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
+			schedule_work(&hw->usb_work);
+		}
+
+		/* If someone cancels the OUT URB then its status
+		 * should be either -ECONNRESET or -ENOENT.
+		 */
+		ctlx->state = CTLX_REQ_FAILED;
+		unlocked_usbctlx_complete(hw, ctlx);
+		delete_resptimer = 1;
+		run_queue = 1;
+	}
+
+delresp:
+	if (delete_resptimer) {
+		timer_ok = del_timer(&hw->resptimer);
+		if (timer_ok != 0)
+			hw->resp_timer_done = 1;
+	}
+
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+	if (!timer_ok && (hw->resp_timer_done == 0)) {
+		spin_lock_irqsave(&hw->ctlxq.lock, flags);
+		goto delresp;
+	}
+
+	if (run_queue)
+		hfa384x_usbctlxq_run(hw);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_reqtimerfn
+ *
+ * Timer response function for CTLX request timeouts.  If this
+ * function is called, it means that the callback for the OUT
+ * URB containing a Prism2.x XXX_Request was never called.
+ *
+ * Arguments:
+ *	data		a ptr to the struct hfa384x
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbctlx_reqtimerfn(struct timer_list *t)
+{
+	struct hfa384x *hw = from_timer(hw, t, reqtimer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	hw->req_timer_done = 1;
+
+	/* Removing the hardware automatically empties
+	 * the active list ...
+	 */
+	if (!list_empty(&hw->ctlxq.active)) {
+		/*
+		 * We must ensure that our URB is removed from
+		 * the system, if it hasn't already expired.
+		 */
+		hw->ctlx_urb.transfer_flags |= URB_ASYNC_UNLINK;
+		if (usb_unlink_urb(&hw->ctlx_urb) == -EINPROGRESS) {
+			struct hfa384x_usbctlx *ctlx = get_active_ctlx(hw);
+
+			ctlx->state = CTLX_REQ_FAILED;
+
+			/* This URB was active, but has now been
+			 * cancelled. It will now have a status of
+			 * -ECONNRESET in the callback function.
+			 *
+			 * We are cancelling this CTLX, so we're
+			 * not going to need to wait for a response.
+			 * The URB's callback function will check
+			 * that this timer is truly dead.
+			 */
+			if (del_timer(&hw->resptimer) != 0)
+				hw->resp_timer_done = 1;
+		}
+	}
+
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_resptimerfn
+ *
+ * Timer response function for CTLX response timeouts.  If this
+ * function is called, it means that the callback for the IN
+ * URB containing a Prism2.x XXX_Response was never called.
+ *
+ * Arguments:
+ *	data		a ptr to the struct hfa384x
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usbctlx_resptimerfn(struct timer_list *t)
+{
+	struct hfa384x *hw = from_timer(hw, t, resptimer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	hw->resp_timer_done = 1;
+
+	/* The active list will be empty if the
+	 * adapter has been unplugged ...
+	 */
+	if (!list_empty(&hw->ctlxq.active)) {
+		struct hfa384x_usbctlx *ctlx = get_active_ctlx(hw);
+
+		if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0) {
+			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+			hfa384x_usbctlxq_run(hw);
+			return;
+		}
+	}
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usb_throttlefn
+ *
+ *
+ * Arguments:
+ *	data	ptr to hw
+ *
+ * Returns:
+ *	Nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	Interrupt
+ *----------------------------------------------------------------
+ */
+static void hfa384x_usb_throttlefn(struct timer_list *t)
+{
+	struct hfa384x *hw = from_timer(hw, t, throttle);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	pr_debug("flags=0x%lx\n", hw->usb_flags);
+	if (!hw->wlandev->hwremoved) {
+		bool rx_throttle = test_and_clear_bit(THROTTLE_RX, &hw->usb_flags) &&
+				   !test_and_set_bit(WORK_RX_RESUME, &hw->usb_flags);
+		bool tx_throttle = test_and_clear_bit(THROTTLE_TX, &hw->usb_flags) &&
+				   !test_and_set_bit(WORK_TX_RESUME, &hw->usb_flags);
+		/*
+		 * We need to check BOTH the RX and the TX throttle controls,
+		 * so we use the bitwise OR instead of the logical OR.
+		 */
+		if (rx_throttle | tx_throttle)
+			schedule_work(&hw->usb_work);
+	}
+
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_usbctlx_submit
+ *
+ * Called from the doxxx functions to submit a CTLX to the queue
+ *
+ * Arguments:
+ *	hw		ptr to the hw struct
+ *	ctlx		ctlx structure to enqueue
+ *
+ * Returns:
+ *	-ENODEV if the adapter is unplugged
+ *	0
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process or interrupt
+ *----------------------------------------------------------------
+ */
+static int hfa384x_usbctlx_submit(struct hfa384x *hw,
+				  struct hfa384x_usbctlx *ctlx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+	if (hw->wlandev->hwremoved) {
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+		return -ENODEV;
+	}
+
+	ctlx->state = CTLX_PENDING;
+	list_add_tail(&ctlx->list, &hw->ctlxq.pending);
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+	hfa384x_usbctlxq_run(hw);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * hfa384x_isgood_pdrcore
+ *
+ * Quick check of PDR codes.
+ *
+ * Arguments:
+ *	pdrcode		PDR code number (host order)
+ *
+ * Returns:
+ *	zero		not good.
+ *	one		is good.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *----------------------------------------------------------------
+ */
+static int hfa384x_isgood_pdrcode(u16 pdrcode)
+{
+	switch (pdrcode) {
+	case HFA384x_PDR_END_OF_PDA:
+	case HFA384x_PDR_PCB_PARTNUM:
+	case HFA384x_PDR_PDAVER:
+	case HFA384x_PDR_NIC_SERIAL:
+	case HFA384x_PDR_MKK_MEASUREMENTS:
+	case HFA384x_PDR_NIC_RAMSIZE:
+	case HFA384x_PDR_MFISUPRANGE:
+	case HFA384x_PDR_CFISUPRANGE:
+	case HFA384x_PDR_NICID:
+	case HFA384x_PDR_MAC_ADDRESS:
+	case HFA384x_PDR_REGDOMAIN:
+	case HFA384x_PDR_ALLOWED_CHANNEL:
+	case HFA384x_PDR_DEFAULT_CHANNEL:
+	case HFA384x_PDR_TEMPTYPE:
+	case HFA384x_PDR_IFR_SETTING:
+	case HFA384x_PDR_RFR_SETTING:
+	case HFA384x_PDR_HFA3861_BASELINE:
+	case HFA384x_PDR_HFA3861_SHADOW:
+	case HFA384x_PDR_HFA3861_IFRF:
+	case HFA384x_PDR_HFA3861_CHCALSP:
+	case HFA384x_PDR_HFA3861_CHCALI:
+	case HFA384x_PDR_3842_NIC_CONFIG:
+	case HFA384x_PDR_USB_ID:
+	case HFA384x_PDR_PCI_ID:
+	case HFA384x_PDR_PCI_IFCONF:
+	case HFA384x_PDR_PCI_PMCONF:
+	case HFA384x_PDR_RFENRGY:
+	case HFA384x_PDR_HFA3861_MANF_TESTSP:
+	case HFA384x_PDR_HFA3861_MANF_TESTI:
+		/* code is OK */
+		return 1;
+	default:
+		if (pdrcode < 0x1000) {
+			/* code is OK, but we don't know exactly what it is */
+			pr_debug("Encountered unknown PDR#=0x%04x, assuming it's ok.\n",
+				 pdrcode);
+			return 1;
+		}
+		break;
+	}
+	/* bad code */
+	pr_debug("Encountered unknown PDR#=0x%04x, (>=0x1000), assuming it's bad.\n",
+		 pdrcode);
+	return 0;
+}
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
new file mode 100644
index 000000000..cd271b1da
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -0,0 +1,664 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Ether/802.11 conversions and packet buffer routines
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file defines the functions that perform Ethernet to/from
+ * 802.11 frame conversions.
+ *
+ * --------------------------------------------------------------------
+ *
+ *================================================================
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_ether.h>
+#include <linux/byteorder/generic.h>
+
+#include <asm/byteorder.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211conv.h"
+#include "p80211mgmt.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211ioctl.h"
+#include "p80211req.h"
+
+static const u8 oui_rfc1042[] = { 0x00, 0x00, 0x00 };
+static const u8 oui_8021h[] = { 0x00, 0x00, 0xf8 };
+
+/*----------------------------------------------------------------
+ * p80211pb_ether_to_80211
+ *
+ * Uses the contents of the ether frame and the etherconv setting
+ * to build the elements of the 802.11 frame.
+ *
+ * We don't actually set
+ * up the frame header here.  That's the MAC's job.  We're only handling
+ * conversion of DIXII or 802.3+LLC frames to something that works
+ * with 802.11.
+ *
+ * Note -- 802.11 header is NOT part of the skb.  Likewise, the 802.11
+ *         FCS is also not present and will need to be added elsewhere.
+ *
+ * Arguments:
+ *	ethconv		Conversion type to perform
+ *	skb		skbuff containing the ether frame
+ *       p80211_hdr      802.11 header
+ *
+ * Returns:
+ *	0 on success, non-zero otherwise
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+int skb_ether_to_p80211(struct wlandevice *wlandev, u32 ethconv,
+			struct sk_buff *skb, struct p80211_hdr *p80211_hdr,
+			struct p80211_metawep *p80211_wep)
+{
+	__le16 fc;
+	u16 proto;
+	struct wlan_ethhdr e_hdr;
+	struct wlan_llc *e_llc;
+	struct wlan_snap *e_snap;
+	int foo;
+
+	memcpy(&e_hdr, skb->data, sizeof(e_hdr));
+
+	if (skb->len <= 0) {
+		pr_debug("zero-length skb!\n");
+		return 1;
+	}
+
+	if (ethconv == WLAN_ETHCONV_ENCAP) {	/* simplest case */
+		pr_debug("ENCAP len: %d\n", skb->len);
+		/* here, we don't care what kind of ether frm. Just stick it */
+		/*  in the 80211 payload */
+		/* which is to say, leave the skb alone. */
+	} else {
+		/* step 1: classify ether frame, DIX or 802.3? */
+		proto = ntohs(e_hdr.type);
+		if (proto <= ETH_DATA_LEN) {
+			pr_debug("802.3 len: %d\n", skb->len);
+			/* codes <= 1500 reserved for 802.3 lengths */
+			/* it's 802.3, pass ether payload unchanged,  */
+
+			/* trim off ethernet header */
+			skb_pull(skb, ETH_HLEN);
+
+			/*   leave off any PAD octets.  */
+			skb_trim(skb, proto);
+		} else {
+			pr_debug("DIXII len: %d\n", skb->len);
+			/* it's DIXII, time for some conversion */
+
+			/* trim off ethernet header */
+			skb_pull(skb, ETH_HLEN);
+
+			/* tack on SNAP */
+			e_snap = skb_push(skb, sizeof(struct wlan_snap));
+			e_snap->type = htons(proto);
+			if (ethconv == WLAN_ETHCONV_8021h &&
+			    p80211_stt_findproto(proto)) {
+				memcpy(e_snap->oui, oui_8021h,
+				       WLAN_IEEE_OUI_LEN);
+			} else {
+				memcpy(e_snap->oui, oui_rfc1042,
+				       WLAN_IEEE_OUI_LEN);
+			}
+
+			/* tack on llc */
+			e_llc = skb_push(skb, sizeof(struct wlan_llc));
+			e_llc->dsap = 0xAA;	/* SNAP, see IEEE 802 */
+			e_llc->ssap = 0xAA;
+			e_llc->ctl = 0x03;
+		}
+	}
+
+	/* Set up the 802.11 header */
+	/* It's a data frame */
+	fc = cpu_to_le16(WLAN_SET_FC_FTYPE(WLAN_FTYPE_DATA) |
+			 WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DATAONLY));
+
+	switch (wlandev->macmode) {
+	case WLAN_MACMODE_IBSS_STA:
+		memcpy(p80211_hdr->address1, &e_hdr.daddr, ETH_ALEN);
+		memcpy(p80211_hdr->address2, wlandev->netdev->dev_addr, ETH_ALEN);
+		memcpy(p80211_hdr->address3, wlandev->bssid, ETH_ALEN);
+		break;
+	case WLAN_MACMODE_ESS_STA:
+		fc |= cpu_to_le16(WLAN_SET_FC_TODS(1));
+		memcpy(p80211_hdr->address1, wlandev->bssid, ETH_ALEN);
+		memcpy(p80211_hdr->address2, wlandev->netdev->dev_addr, ETH_ALEN);
+		memcpy(p80211_hdr->address3, &e_hdr.daddr, ETH_ALEN);
+		break;
+	case WLAN_MACMODE_ESS_AP:
+		fc |= cpu_to_le16(WLAN_SET_FC_FROMDS(1));
+		memcpy(p80211_hdr->address1, &e_hdr.daddr, ETH_ALEN);
+		memcpy(p80211_hdr->address2, wlandev->bssid, ETH_ALEN);
+		memcpy(p80211_hdr->address3, &e_hdr.saddr, ETH_ALEN);
+		break;
+	default:
+		netdev_err(wlandev->netdev,
+			   "Error: Converting eth to wlan in unknown mode.\n");
+		return 1;
+	}
+
+	p80211_wep->data = NULL;
+
+	if ((wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) &&
+	    (wlandev->hostwep & HOSTWEP_ENCRYPT)) {
+		/* XXXX need to pick keynum other than default? */
+
+		p80211_wep->data = kmalloc(skb->len, GFP_ATOMIC);
+		if (!p80211_wep->data)
+			return -ENOMEM;
+		foo = wep_encrypt(wlandev, skb->data, p80211_wep->data,
+				  skb->len,
+				  wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK,
+				  p80211_wep->iv, p80211_wep->icv);
+		if (foo) {
+			netdev_warn(wlandev->netdev,
+				    "Host en-WEP failed, dropping frame (%d).\n",
+				    foo);
+			kfree(p80211_wep->data);
+			return 2;
+		}
+		fc |= cpu_to_le16(WLAN_SET_FC_ISWEP(1));
+	}
+
+	/*      skb->nh.raw = skb->data; */
+
+	p80211_hdr->frame_control = fc;
+	p80211_hdr->duration_id = 0;
+	p80211_hdr->sequence_control = 0;
+
+	return 0;
+}
+
+/* jkriegl: from orinoco, modified */
+static void orinoco_spy_gather(struct wlandevice *wlandev, char *mac,
+			       struct p80211_rxmeta *rxmeta)
+{
+	int i;
+
+	/* Gather wireless spy statistics: for each packet, compare the
+	 * source address with out list, and if match, get the stats...
+	 */
+
+	for (i = 0; i < wlandev->spy_number; i++) {
+		if (!memcmp(wlandev->spy_address[i], mac, ETH_ALEN)) {
+			wlandev->spy_stat[i].level = rxmeta->signal;
+			wlandev->spy_stat[i].noise = rxmeta->noise;
+			wlandev->spy_stat[i].qual =
+			    (rxmeta->signal >
+			     rxmeta->noise) ? (rxmeta->signal -
+					       rxmeta->noise) : 0;
+			wlandev->spy_stat[i].updated = 0x7;
+		}
+	}
+}
+
+/*----------------------------------------------------------------
+ * p80211pb_80211_to_ether
+ *
+ * Uses the contents of a received 802.11 frame and the etherconv
+ * setting to build an ether frame.
+ *
+ * This function extracts the src and dest address from the 802.11
+ * frame to use in the construction of the eth frame.
+ *
+ * Arguments:
+ *	ethconv		Conversion type to perform
+ *	skb		Packet buffer containing the 802.11 frame
+ *
+ * Returns:
+ *	0 on success, non-zero otherwise
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+int skb_p80211_to_ether(struct wlandevice *wlandev, u32 ethconv,
+			struct sk_buff *skb)
+{
+	struct net_device *netdev = wlandev->netdev;
+	u16 fc;
+	unsigned int payload_length;
+	unsigned int payload_offset;
+	u8 daddr[ETH_ALEN];
+	u8 saddr[ETH_ALEN];
+	struct p80211_hdr *w_hdr;
+	struct wlan_ethhdr *e_hdr;
+	struct wlan_llc *e_llc;
+	struct wlan_snap *e_snap;
+
+	int foo;
+
+	payload_length = skb->len - WLAN_HDR_A3_LEN - WLAN_CRC_LEN;
+	payload_offset = WLAN_HDR_A3_LEN;
+
+	w_hdr = (struct p80211_hdr *)skb->data;
+
+	/* setup some vars for convenience */
+	fc = le16_to_cpu(w_hdr->frame_control);
+	if ((WLAN_GET_FC_TODS(fc) == 0) && (WLAN_GET_FC_FROMDS(fc) == 0)) {
+		ether_addr_copy(daddr, w_hdr->address1);
+		ether_addr_copy(saddr, w_hdr->address2);
+	} else if ((WLAN_GET_FC_TODS(fc) == 0) &&
+		   (WLAN_GET_FC_FROMDS(fc) == 1)) {
+		ether_addr_copy(daddr, w_hdr->address1);
+		ether_addr_copy(saddr, w_hdr->address3);
+	} else if ((WLAN_GET_FC_TODS(fc) == 1) &&
+		   (WLAN_GET_FC_FROMDS(fc) == 0)) {
+		ether_addr_copy(daddr, w_hdr->address3);
+		ether_addr_copy(saddr, w_hdr->address2);
+	} else {
+		payload_offset = WLAN_HDR_A4_LEN;
+		if (payload_length < WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN) {
+			netdev_err(netdev, "A4 frame too short!\n");
+			return 1;
+		}
+		payload_length -= (WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN);
+		ether_addr_copy(daddr, w_hdr->address3);
+		ether_addr_copy(saddr, w_hdr->address4);
+	}
+
+	/* perform de-wep if necessary.. */
+	if ((wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) &&
+	    WLAN_GET_FC_ISWEP(fc) &&
+	    (wlandev->hostwep & HOSTWEP_DECRYPT)) {
+		if (payload_length <= 8) {
+			netdev_err(netdev,
+				   "WEP frame too short (%u).\n", skb->len);
+			return 1;
+		}
+		foo = wep_decrypt(wlandev, skb->data + payload_offset + 4,
+				  payload_length - 8, -1,
+				  skb->data + payload_offset,
+				  skb->data + payload_offset +
+				  payload_length - 4);
+		if (foo) {
+			/* de-wep failed, drop skb. */
+			pr_debug("Host de-WEP failed, dropping frame (%d).\n",
+				 foo);
+			wlandev->rx.decrypt_err++;
+			return 2;
+		}
+
+		/* subtract the IV+ICV length off the payload */
+		payload_length -= 8;
+		/* chop off the IV */
+		skb_pull(skb, 4);
+		/* chop off the ICV. */
+		skb_trim(skb, skb->len - 4);
+
+		wlandev->rx.decrypt++;
+	}
+
+	e_hdr = (struct wlan_ethhdr *)(skb->data + payload_offset);
+
+	e_llc = (struct wlan_llc *)(skb->data + payload_offset);
+	e_snap =
+	    (struct wlan_snap *)(skb->data + payload_offset +
+		sizeof(struct wlan_llc));
+
+	/* Test for the various encodings */
+	if ((payload_length >= sizeof(struct wlan_ethhdr)) &&
+	    (e_llc->dsap != 0xaa || e_llc->ssap != 0xaa) &&
+	    ((!ether_addr_equal_unaligned(daddr, e_hdr->daddr)) ||
+	     (!ether_addr_equal_unaligned(saddr, e_hdr->saddr)))) {
+		pr_debug("802.3 ENCAP len: %d\n", payload_length);
+		/* 802.3 Encapsulated */
+		/* Test for an overlength frame */
+		if (payload_length > (netdev->mtu + ETH_HLEN)) {
+			/* A bogus length ethfrm has been encap'd. */
+			/* Is someone trying an oflow attack? */
+			netdev_err(netdev, "ENCAP frame too large (%d > %d)\n",
+				   payload_length, netdev->mtu + ETH_HLEN);
+			return 1;
+		}
+
+		/* Chop off the 802.11 header.  it's already sane. */
+		skb_pull(skb, payload_offset);
+		/* chop off the 802.11 CRC */
+		skb_trim(skb, skb->len - WLAN_CRC_LEN);
+
+	} else if ((payload_length >= sizeof(struct wlan_llc) +
+		sizeof(struct wlan_snap)) &&
+		(e_llc->dsap == 0xaa) &&
+		(e_llc->ssap == 0xaa) &&
+		(e_llc->ctl == 0x03) &&
+		   (((memcmp(e_snap->oui, oui_rfc1042,
+		   WLAN_IEEE_OUI_LEN) == 0) &&
+		   (ethconv == WLAN_ETHCONV_8021h) &&
+		   (p80211_stt_findproto(be16_to_cpu(e_snap->type)))) ||
+		   (memcmp(e_snap->oui, oui_rfc1042, WLAN_IEEE_OUI_LEN) !=
+			0))) {
+		pr_debug("SNAP+RFC1042 len: %d\n", payload_length);
+		/* it's a SNAP + RFC1042 frame && protocol is in STT */
+		/* build 802.3 + RFC1042 */
+
+		/* Test for an overlength frame */
+		if (payload_length > netdev->mtu) {
+			/* A bogus length ethfrm has been sent. */
+			/* Is someone trying an oflow attack? */
+			netdev_err(netdev, "SNAP frame too large (%d > %d)\n",
+				   payload_length, netdev->mtu);
+			return 1;
+		}
+
+		/* chop 802.11 header from skb. */
+		skb_pull(skb, payload_offset);
+
+		/* create 802.3 header at beginning of skb. */
+		e_hdr = skb_push(skb, ETH_HLEN);
+		ether_addr_copy(e_hdr->daddr, daddr);
+		ether_addr_copy(e_hdr->saddr, saddr);
+		e_hdr->type = htons(payload_length);
+
+		/* chop off the 802.11 CRC */
+		skb_trim(skb, skb->len - WLAN_CRC_LEN);
+
+	} else if ((payload_length >= sizeof(struct wlan_llc) +
+		sizeof(struct wlan_snap)) &&
+		(e_llc->dsap == 0xaa) &&
+		(e_llc->ssap == 0xaa) &&
+		(e_llc->ctl == 0x03)) {
+		pr_debug("802.1h/RFC1042 len: %d\n", payload_length);
+		/* it's an 802.1h frame || (an RFC1042 && protocol not in STT)
+		 * build a DIXII + RFC894
+		 */
+
+		/* Test for an overlength frame */
+		if ((payload_length - sizeof(struct wlan_llc) -
+			sizeof(struct wlan_snap))
+			> netdev->mtu) {
+			/* A bogus length ethfrm has been sent. */
+			/* Is someone trying an oflow attack? */
+			netdev_err(netdev, "DIXII frame too large (%ld > %d)\n",
+				   (long)(payload_length -
+				   sizeof(struct wlan_llc) -
+				   sizeof(struct wlan_snap)), netdev->mtu);
+			return 1;
+		}
+
+		/* chop 802.11 header from skb. */
+		skb_pull(skb, payload_offset);
+
+		/* chop llc header from skb. */
+		skb_pull(skb, sizeof(struct wlan_llc));
+
+		/* chop snap header from skb. */
+		skb_pull(skb, sizeof(struct wlan_snap));
+
+		/* create 802.3 header at beginning of skb. */
+		e_hdr = skb_push(skb, ETH_HLEN);
+		e_hdr->type = e_snap->type;
+		ether_addr_copy(e_hdr->daddr, daddr);
+		ether_addr_copy(e_hdr->saddr, saddr);
+
+		/* chop off the 802.11 CRC */
+		skb_trim(skb, skb->len - WLAN_CRC_LEN);
+	} else {
+		pr_debug("NON-ENCAP len: %d\n", payload_length);
+		/* any NON-ENCAP */
+		/* it's a generic 80211+LLC or IPX 'Raw 802.3' */
+		/*  build an 802.3 frame */
+		/* allocate space and setup hostbuf */
+
+		/* Test for an overlength frame */
+		if (payload_length > netdev->mtu) {
+			/* A bogus length ethfrm has been sent. */
+			/* Is someone trying an oflow attack? */
+			netdev_err(netdev, "OTHER frame too large (%d > %d)\n",
+				   payload_length, netdev->mtu);
+			return 1;
+		}
+
+		/* Chop off the 802.11 header. */
+		skb_pull(skb, payload_offset);
+
+		/* create 802.3 header at beginning of skb. */
+		e_hdr = skb_push(skb, ETH_HLEN);
+		ether_addr_copy(e_hdr->daddr, daddr);
+		ether_addr_copy(e_hdr->saddr, saddr);
+		e_hdr->type = htons(payload_length);
+
+		/* chop off the 802.11 CRC */
+		skb_trim(skb, skb->len - WLAN_CRC_LEN);
+	}
+
+	/*
+	 * Note that eth_type_trans() expects an skb w/ skb->data pointing
+	 * at the MAC header, it then sets the following skb members:
+	 * skb->mac_header,
+	 * skb->data, and
+	 * skb->pkt_type.
+	 * It then _returns_ the value that _we're_ supposed to stuff in
+	 * skb->protocol.  This is nuts.
+	 */
+	skb->protocol = eth_type_trans(skb, netdev);
+
+	/* jkriegl: process signal and noise as set in hfa384x_int_rx() */
+	/* jkriegl: only process signal/noise if requested by iwspy */
+	if (wlandev->spy_number)
+		orinoco_spy_gather(wlandev, eth_hdr(skb)->h_source,
+				   p80211skb_rxmeta(skb));
+
+	/* Free the metadata */
+	p80211skb_rxmeta_detach(skb);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * p80211_stt_findproto
+ *
+ * Searches the 802.1h Selective Translation Table for a given
+ * protocol.
+ *
+ * Arguments:
+ *	proto	protocol number (in host order) to search for.
+ *
+ * Returns:
+ *	1 - if the table is empty or a match is found.
+ *	0 - if the table is non-empty and a match is not found.
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+int p80211_stt_findproto(u16 proto)
+{
+	/* Always return found for now.  This is the behavior used by the */
+	/* Zoom Win95 driver when 802.1h mode is selected */
+	/* TODO: If necessary, add an actual search we'll probably
+	 * need this to match the CMAC's way of doing things.
+	 * Need to do some testing to confirm.
+	 */
+
+	if (proto == ETH_P_AARP)	/* APPLETALK */
+		return 1;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * p80211skb_rxmeta_detach
+ *
+ * Disconnects the frmmeta and rxmeta from an skb.
+ *
+ * Arguments:
+ *	wlandev		The wlandev this skb belongs to.
+ *	skb		The skb we're attaching to.
+ *
+ * Returns:
+ *	0 on success, non-zero otherwise
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+void p80211skb_rxmeta_detach(struct sk_buff *skb)
+{
+	struct p80211_rxmeta *rxmeta;
+	struct p80211_frmmeta *frmmeta;
+
+	/* Sanity checks */
+	if (!skb) {	/* bad skb */
+		pr_debug("Called w/ null skb.\n");
+		return;
+	}
+	frmmeta = p80211skb_frmmeta(skb);
+	if (!frmmeta) {	/* no magic */
+		pr_debug("Called w/ bad frmmeta magic.\n");
+		return;
+	}
+	rxmeta = frmmeta->rx;
+	if (!rxmeta) {	/* bad meta ptr */
+		pr_debug("Called w/ bad rxmeta ptr.\n");
+		return;
+	}
+
+	/* Free rxmeta */
+	kfree(rxmeta);
+
+	/* Clear skb->cb */
+	memset(skb->cb, 0, sizeof(skb->cb));
+}
+
+/*----------------------------------------------------------------
+ * p80211skb_rxmeta_attach
+ *
+ * Allocates a p80211rxmeta structure, initializes it, and attaches
+ * it to an skb.
+ *
+ * Arguments:
+ *	wlandev		The wlandev this skb belongs to.
+ *	skb		The skb we're attaching to.
+ *
+ * Returns:
+ *	0 on success, non-zero otherwise
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb)
+{
+	int result = 0;
+	struct p80211_rxmeta *rxmeta;
+	struct p80211_frmmeta *frmmeta;
+
+	/* If these already have metadata, we error out! */
+	if (p80211skb_rxmeta(skb)) {
+		netdev_err(wlandev->netdev,
+			   "%s: RXmeta already attached!\n", wlandev->name);
+		result = 0;
+		goto exit;
+	}
+
+	/* Allocate the rxmeta */
+	rxmeta = kzalloc(sizeof(*rxmeta), GFP_ATOMIC);
+
+	if (!rxmeta) {
+		result = 1;
+		goto exit;
+	}
+
+	/* Initialize the rxmeta */
+	rxmeta->wlandev = wlandev;
+	rxmeta->hosttime = jiffies;
+
+	/* Overlay a frmmeta_t onto skb->cb */
+	memset(skb->cb, 0, sizeof(struct p80211_frmmeta));
+	frmmeta = (struct p80211_frmmeta *)(skb->cb);
+	frmmeta->magic = P80211_FRMMETA_MAGIC;
+	frmmeta->rx = rxmeta;
+exit:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * p80211skb_free
+ *
+ * Frees an entire p80211skb by checking and freeing the meta struct
+ * and then freeing the skb.
+ *
+ * Arguments:
+ *	wlandev		The wlandev this skb belongs to.
+ *	skb		The skb we're attaching to.
+ *
+ * Returns:
+ *	0 on success, non-zero otherwise
+ *
+ * Call context:
+ *	May be called in interrupt or non-interrupt context
+ *----------------------------------------------------------------
+ */
+void p80211skb_free(struct wlandevice *wlandev, struct sk_buff *skb)
+{
+	struct p80211_frmmeta *meta;
+
+	meta = p80211skb_frmmeta(skb);
+	if (meta && meta->rx)
+		p80211skb_rxmeta_detach(skb);
+	else
+		netdev_err(wlandev->netdev,
+			   "Freeing an skb (%p) w/ no frmmeta.\n", skb);
+	dev_kfree_skb(skb);
+}
diff --git a/drivers/staging/wlan-ng/p80211conv.h b/drivers/staging/wlan-ng/p80211conv.h
new file mode 100644
index 000000000..dfb762bce
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211conv.h
@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Ether/802.11 conversions and packet buffer routines
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file declares the functions, types and macros that perform
+ * Ethernet to/from 802.11 frame conversions.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _LINUX_P80211CONV_H
+#define _LINUX_P80211CONV_H
+
+#define WLAN_IEEE_OUI_LEN	3
+
+#define WLAN_ETHCONV_ENCAP	1
+#define WLAN_ETHCONV_8021h	3
+
+#define P80211CAPTURE_VERSION	0x80211001
+
+#define	P80211_FRMMETA_MAGIC	0x802110
+
+struct p80211_rxmeta {
+	struct wlandevice *wlandev;
+
+	u64 mactime;		/* Hi-rez MAC-supplied time value */
+	u64 hosttime;		/* Best-rez host supplied time value */
+
+	unsigned int rxrate;	/* Receive data rate in 100kbps */
+	unsigned int priority;	/* 0-15, 0=contention, 6=CF */
+	int signal;		/* An SSI, see p80211netdev.h */
+	int noise;		/* An SSI, see p80211netdev.h */
+	unsigned int channel;	/* Receive channel (mostly for snifs) */
+	unsigned int preamble;	/* P80211ENUM_preambletype_* */
+	unsigned int encoding;	/* P80211ENUM_encoding_* */
+
+};
+
+struct p80211_frmmeta {
+	unsigned int magic;
+	struct p80211_rxmeta *rx;
+};
+
+void p80211skb_free(struct wlandevice *wlandev, struct sk_buff *skb);
+int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb);
+void p80211skb_rxmeta_detach(struct sk_buff *skb);
+
+static inline struct p80211_frmmeta *p80211skb_frmmeta(struct sk_buff *skb)
+{
+	struct p80211_frmmeta *frmmeta = (struct p80211_frmmeta *)skb->cb;
+
+	return frmmeta->magic == P80211_FRMMETA_MAGIC ? frmmeta : NULL;
+}
+
+static inline struct p80211_rxmeta *p80211skb_rxmeta(struct sk_buff *skb)
+{
+	struct p80211_frmmeta *frmmeta = p80211skb_frmmeta(skb);
+
+	return frmmeta ? frmmeta->rx : NULL;
+}
+
+/*
+ * Frame capture header.  (See doc/capturefrm.txt)
+ */
+struct p80211_caphdr {
+	__be32 version;
+	__be32 length;
+	__be64 mactime;
+	__be64 hosttime;
+	__be32 phytype;
+	__be32 channel;
+	__be32 datarate;
+	__be32 antenna;
+	__be32 priority;
+	__be32 ssi_type;
+	__be32 ssi_signal;
+	__be32 ssi_noise;
+	__be32 preamble;
+	__be32 encoding;
+};
+
+struct p80211_metawep {
+	void *data;
+	u8 iv[4];
+	u8 icv[4];
+};
+
+/* local ether header type */
+struct wlan_ethhdr {
+	u8 daddr[ETH_ALEN];
+	u8 saddr[ETH_ALEN];
+	__be16 type;
+} __packed;
+
+/* local llc header type */
+struct wlan_llc {
+	u8 dsap;
+	u8 ssap;
+	u8 ctl;
+} __packed;
+
+/* local snap header type */
+struct wlan_snap {
+	u8 oui[WLAN_IEEE_OUI_LEN];
+	__be16 type;
+} __packed;
+
+/* Circular include trick */
+struct wlandevice;
+
+int skb_p80211_to_ether(struct wlandevice *wlandev, u32 ethconv,
+			struct sk_buff *skb);
+int skb_ether_to_p80211(struct wlandevice *wlandev, u32 ethconv,
+			struct sk_buff *skb, struct p80211_hdr *p80211_hdr,
+			struct p80211_metawep *p80211_wep);
+
+int p80211_stt_findproto(u16 proto);
+
+#endif
diff --git a/drivers/staging/wlan-ng/p80211hdr.h b/drivers/staging/wlan-ng/p80211hdr.h
new file mode 100644
index 000000000..93195a4c5
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211hdr.h
@@ -0,0 +1,210 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Macros, types, and functions for handling 802.11 MAC headers
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file declares the constants and types used in the interface
+ * between a wlan driver and the user mode utilities.
+ *
+ * Note:
+ *  - Constant values are always in HOST byte order.  To assign
+ *    values to multi-byte fields they _must_ be converted to
+ *    ieee byte order.  To retrieve multi-byte values from incoming
+ *    frames, they must be converted to host order.
+ *
+ * All functions declared here are implemented in p80211.c
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211HDR_H
+#define _P80211HDR_H
+
+#include <linux/if_ether.h>
+
+/*--- Sizes -----------------------------------------------*/
+#define WLAN_CRC_LEN			4
+#define WLAN_BSSID_LEN			6
+#define WLAN_HDR_A3_LEN			24
+#define WLAN_HDR_A4_LEN			30
+#define WLAN_SSID_MAXLEN		32
+#define WLAN_DATA_MAXLEN		2312
+#define WLAN_WEP_IV_LEN			4
+#define WLAN_WEP_ICV_LEN		4
+
+/*--- Frame Control Field -------------------------------------*/
+/* Frame Types */
+#define WLAN_FTYPE_MGMT			0x00
+#define WLAN_FTYPE_CTL			0x01
+#define WLAN_FTYPE_DATA			0x02
+
+/* Frame subtypes */
+/* Management */
+#define WLAN_FSTYPE_ASSOCREQ		0x00
+#define WLAN_FSTYPE_ASSOCRESP		0x01
+#define WLAN_FSTYPE_REASSOCREQ		0x02
+#define WLAN_FSTYPE_REASSOCRESP		0x03
+#define WLAN_FSTYPE_PROBEREQ		0x04
+#define WLAN_FSTYPE_PROBERESP		0x05
+#define WLAN_FSTYPE_BEACON		0x08
+#define WLAN_FSTYPE_ATIM		0x09
+#define WLAN_FSTYPE_DISASSOC		0x0a
+#define WLAN_FSTYPE_AUTHEN		0x0b
+#define WLAN_FSTYPE_DEAUTHEN		0x0c
+
+/* Control */
+#define WLAN_FSTYPE_BLOCKACKREQ		0x8
+#define WLAN_FSTYPE_BLOCKACK		0x9
+#define WLAN_FSTYPE_PSPOLL		0x0a
+#define WLAN_FSTYPE_RTS			0x0b
+#define WLAN_FSTYPE_CTS			0x0c
+#define WLAN_FSTYPE_ACK			0x0d
+#define WLAN_FSTYPE_CFEND		0x0e
+#define WLAN_FSTYPE_CFENDCFACK		0x0f
+
+/* Data */
+#define WLAN_FSTYPE_DATAONLY		0x00
+#define WLAN_FSTYPE_DATA_CFACK		0x01
+#define WLAN_FSTYPE_DATA_CFPOLL		0x02
+#define WLAN_FSTYPE_DATA_CFACK_CFPOLL	0x03
+#define WLAN_FSTYPE_NULL		0x04
+#define WLAN_FSTYPE_CFACK		0x05
+#define WLAN_FSTYPE_CFPOLL		0x06
+#define WLAN_FSTYPE_CFACK_CFPOLL	0x07
+
+/*--- FC Macros ----------------------------------------------*/
+/* Macros to get/set the bitfields of the Frame Control Field */
+/*  GET_FC_??? - takes the host byte-order value of an FC     */
+/*               and retrieves the value of one of the        */
+/*               bitfields and moves that value so its lsb is */
+/*               in bit 0.                                    */
+/*  SET_FC_??? - takes a host order value for one of the FC   */
+/*               bitfields and moves it to the proper bit     */
+/*               location for ORing into a host order FC.     */
+/*               To send the FC produced from SET_FC_???,     */
+/*               one must put the bytes in IEEE order.        */
+/*  e.g.                                                      */
+/*     printf("the frame subtype is %x",                      */
+/*                 GET_FC_FTYPE( ieee2host( rx.fc )))         */
+/*                                                            */
+/*     tx.fc = host2ieee( SET_FC_FTYPE(WLAN_FTYP_CTL) |       */
+/*                        SET_FC_FSTYPE(WLAN_FSTYPE_RTS) );   */
+/*------------------------------------------------------------*/
+
+#define WLAN_GET_FC_FTYPE(n)	((((u16)(n)) & GENMASK(3, 2)) >> 2)
+#define WLAN_GET_FC_FSTYPE(n)	((((u16)(n)) & GENMASK(7, 4)) >> 4)
+#define WLAN_GET_FC_TODS(n)	((((u16)(n)) & (BIT(8))) >> 8)
+#define WLAN_GET_FC_FROMDS(n)	((((u16)(n)) & (BIT(9))) >> 9)
+#define WLAN_GET_FC_ISWEP(n)	((((u16)(n)) & (BIT(14))) >> 14)
+
+#define WLAN_SET_FC_FTYPE(n)	(((u16)(n)) << 2)
+#define WLAN_SET_FC_FSTYPE(n)	(((u16)(n)) << 4)
+#define WLAN_SET_FC_TODS(n)	(((u16)(n)) << 8)
+#define WLAN_SET_FC_FROMDS(n)	(((u16)(n)) << 9)
+#define WLAN_SET_FC_ISWEP(n)	(((u16)(n)) << 14)
+
+#define DOT11_RATE5_ISBASIC_GET(r)     (((u8)(r)) & BIT(7))
+
+/* Generic 802.11 Header types */
+
+struct p80211_hdr {
+	__le16	frame_control;
+	u16	duration_id;
+	u8	address1[ETH_ALEN];
+	u8	address2[ETH_ALEN];
+	u8	address3[ETH_ALEN];
+	u16	sequence_control;
+	u8	address4[ETH_ALEN];
+} __packed;
+
+/* Frame and header length macros */
+
+static inline u16 wlan_ctl_framelen(u16 fstype)
+{
+	switch (fstype)	{
+	case WLAN_FSTYPE_BLOCKACKREQ:
+		return 24;
+	case WLAN_FSTYPE_BLOCKACK:
+		return 152;
+	case WLAN_FSTYPE_PSPOLL:
+	case WLAN_FSTYPE_RTS:
+	case WLAN_FSTYPE_CFEND:
+	case WLAN_FSTYPE_CFENDCFACK:
+		return 20;
+	case WLAN_FSTYPE_CTS:
+	case WLAN_FSTYPE_ACK:
+		return 14;
+	default:
+		return 4;
+	}
+}
+
+#define WLAN_FCS_LEN			4
+
+/* ftcl in HOST order */
+static inline u16 p80211_headerlen(u16 fctl)
+{
+	u16 hdrlen = 0;
+
+	switch (WLAN_GET_FC_FTYPE(fctl)) {
+	case WLAN_FTYPE_MGMT:
+		hdrlen = WLAN_HDR_A3_LEN;
+		break;
+	case WLAN_FTYPE_DATA:
+		hdrlen = WLAN_HDR_A3_LEN;
+		if (WLAN_GET_FC_TODS(fctl) && WLAN_GET_FC_FROMDS(fctl))
+			hdrlen += ETH_ALEN;
+		break;
+	case WLAN_FTYPE_CTL:
+		hdrlen = wlan_ctl_framelen(WLAN_GET_FC_FSTYPE(fctl)) -
+		    WLAN_FCS_LEN;
+		break;
+	default:
+		hdrlen = WLAN_HDR_A3_LEN;
+	}
+
+	return hdrlen;
+}
+
+#endif /* _P80211HDR_H */
diff --git a/drivers/staging/wlan-ng/p80211ioctl.h b/drivers/staging/wlan-ng/p80211ioctl.h
new file mode 100644
index 000000000..b50ce1114
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211ioctl.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Declares constants and types for the p80211 ioctls
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ *  While this file is called 'ioctl' is purpose goes a little beyond
+ *  that.  This file defines the types and contants used to implement
+ *  the p80211 request/confirm/indicate interfaces on Linux.  The
+ *  request/confirm interface is, in fact, normally implemented as an
+ *  ioctl.  The indicate interface on the other hand, is implemented
+ *  using the Linux 'netlink' interface.
+ *
+ *  The reason I say that request/confirm is 'normally' implemented
+ *  via ioctl is that we're reserving the right to be able to send
+ *  request commands via the netlink interface.  This will be necessary
+ *  if we ever need to send request messages when there aren't any
+ *  wlan network devices present (i.e. sending a message that only p80211
+ *  cares about.
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211IOCTL_H
+#define _P80211IOCTL_H
+
+/* p80211 ioctl "request" codes.  See argument 2 of ioctl(2). */
+
+#define P80211_IFTEST		(SIOCDEVPRIVATE + 0)
+#define P80211_IFREQ		(SIOCDEVPRIVATE + 1)
+
+/*----------------------------------------------------------------*/
+/* Magic number, a quick test to see we're getting the desired struct */
+
+#define P80211_IOCTL_MAGIC	(0x4a2d464dUL)
+
+/*----------------------------------------------------------------*/
+/* A ptr to the following structure type is passed as the third */
+/*  argument to the ioctl system call when issuing a request to */
+/*  the p80211 module. */
+
+struct p80211ioctl_req {
+	char name[WLAN_DEVNAMELEN_MAX];
+	char __user *data;
+	u32 magic;
+	u16 len;
+	u32 result;
+} __packed;
+
+#endif /* _P80211IOCTL_H */
diff --git a/drivers/staging/wlan-ng/p80211metadef.h b/drivers/staging/wlan-ng/p80211metadef.h
new file mode 100644
index 000000000..1b91b64c1
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211metadef.h
@@ -0,0 +1,248 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/* --------------------------------------------------------------------
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211MKMETADEF_H
+#define _P80211MKMETADEF_H
+
+#define DIDMSG_DOT11REQ_MIBGET \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(1))
+#define DIDMSG_DOT11REQ_MIBGET_MIBATTRIBUTE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(1) | 0x00000000)
+#define DIDMSG_DOT11REQ_MIBGET_RESULTCODE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(2) | 0x00000000)
+#define DIDMSG_DOT11REQ_MIBSET \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(2))
+#define DIDMSG_DOT11REQ_MIBSET_MIBATTRIBUTE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(2) | \
+			P80211DID_MKITEM(1) | 0x00000000)
+#define DIDMSG_DOT11REQ_MIBSET_RESULTCODE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(2) | \
+			P80211DID_MKITEM(2) | 0x00000000)
+#define DIDMSG_DOT11REQ_SCAN \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(4))
+#define DIDMSG_DOT11REQ_SCAN_RESULTS \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(5))
+#define DIDMSG_DOT11REQ_START \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(13))
+#define DIDMSG_DOT11IND_AUTHENTICATE \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1))
+#define DIDMSG_DOT11IND_ASSOCIATE \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(3))
+#define DIDMSG_LNXREQ_IFSTATE \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(1))
+#define DIDMSG_LNXREQ_WLANSNIFF \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(2))
+#define DIDMSG_LNXREQ_HOSTWEP \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(3))
+#define DIDMSG_LNXREQ_COMMSQUALITY \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(4))
+#define DIDMSG_LNXREQ_AUTOJOIN \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(5))
+#define DIDMSG_P2REQ_READPDA \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(2))
+#define DIDMSG_P2REQ_READPDA_PDA \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(2) | \
+			P80211DID_MKITEM(1) | 0x00000000)
+#define DIDMSG_P2REQ_READPDA_RESULTCODE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(2) | \
+			P80211DID_MKITEM(2) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_STATE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(11))
+#define DIDMSG_P2REQ_RAMDL_STATE_ENABLE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(11) | \
+			P80211DID_MKITEM(1) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_STATE_EXEADDR \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(11) | \
+			P80211DID_MKITEM(2) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_STATE_RESULTCODE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(11) | \
+			P80211DID_MKITEM(3) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_WRITE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(12))
+#define DIDMSG_P2REQ_RAMDL_WRITE_ADDR \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(12) | \
+			P80211DID_MKITEM(1) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_WRITE_LEN \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(12) | \
+			P80211DID_MKITEM(2) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_WRITE_DATA \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(12) | \
+			P80211DID_MKITEM(3) | 0x00000000)
+#define DIDMSG_P2REQ_RAMDL_WRITE_RESULTCODE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(12) | \
+			P80211DID_MKITEM(4) | 0x00000000)
+#define DIDMSG_P2REQ_FLASHDL_STATE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(13))
+#define DIDMSG_P2REQ_FLASHDL_WRITE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(14))
+#define DIDMIB_CAT_DOT11SMT \
+			P80211DID_MKSECTION(1)
+#define DIDMIB_DOT11SMT_WEPDEFAULTKEYSTABLE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(4))
+#define didmib_dot11smt_wepdefaultkeystable_key(_i) \
+			(DIDMIB_DOT11SMT_WEPDEFAULTKEYSTABLE | \
+			P80211DID_MKITEM(_i) | 0x0c000000)
+#define DIDMIB_DOT11SMT_PRIVACYTABLE \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(6))
+#define DIDMIB_DOT11SMT_PRIVACYTABLE_PRIVACYINVOKED \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(6) | \
+			P80211DID_MKITEM(1) | 0x18000000)
+#define DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(6) | \
+			P80211DID_MKITEM(2) | 0x18000000)
+#define DIDMIB_DOT11SMT_PRIVACYTABLE_EXCLUDEUNENCRYPTED \
+			(P80211DID_MKSECTION(1) | \
+			P80211DID_MKGROUP(6) | \
+			P80211DID_MKITEM(4) | 0x18000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1))
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_MACADDRESS \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(1) | 0x18000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_RTSTHRESHOLD \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(2) | 0x18000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_SHORTRETRYLIMIT \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(3) | 0x10000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_LONGRETRYLIMIT \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(4) | 0x10000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_FRAGMENTATIONTHRESHOLD \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(5) | 0x18000000)
+#define DIDMIB_DOT11MAC_OPERATIONTABLE_MAXTRANSMITMSDULIFETIME \
+			(P80211DID_MKSECTION(2) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(6) | 0x10000000)
+#define DIDMIB_CAT_DOT11PHY \
+			P80211DID_MKSECTION(3)
+#define DIDMIB_DOT11PHY_OPERATIONTABLE \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(1))
+#define DIDMIB_DOT11PHY_TXPOWERTABLE_CURRENTTXPOWERLEVEL \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(3) | \
+			P80211DID_MKITEM(10) | 0x18000000)
+#define DIDMIB_DOT11PHY_DSSSTABLE \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(5))
+#define DIDMIB_DOT11PHY_DSSSTABLE_CURRENTCHANNEL \
+			(P80211DID_MKSECTION(3) | \
+			P80211DID_MKGROUP(5) | \
+			P80211DID_MKITEM(1) | 0x10000000)
+#define DIDMIB_CAT_LNX \
+			P80211DID_MKSECTION(4)
+#define DIDMIB_LNX_CONFIGTABLE \
+			(P80211DID_MKSECTION(4) | \
+			P80211DID_MKGROUP(1))
+#define DIDMIB_LNX_CONFIGTABLE_RSNAIE \
+			(P80211DID_MKSECTION(4) | \
+			P80211DID_MKGROUP(1) | \
+			P80211DID_MKITEM(1) | 0x18000000)
+#define DIDMIB_CAT_P2 \
+			P80211DID_MKSECTION(5)
+#define DIDMIB_P2_STATIC \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(2))
+#define DIDMIB_P2_STATIC_CNFPORTTYPE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(2) | \
+			P80211DID_MKITEM(1) | 0x18000000)
+#define DIDMIB_P2_NIC_PRISUPRANGE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(5) | \
+			P80211DID_MKITEM(6) | 0x10000000)
+#define DIDMIB_P2_MAC \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(6))
+#define DIDMIB_P2_MAC_CURRENTTXRATE \
+			(P80211DID_MKSECTION(5) | \
+			P80211DID_MKGROUP(6) | \
+			P80211DID_MKITEM(12) | 0x10000000)
+#endif
diff --git a/drivers/staging/wlan-ng/p80211metastruct.h b/drivers/staging/wlan-ng/p80211metastruct.h
new file mode 100644
index 000000000..4adc64580
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211metastruct.h
@@ -0,0 +1,271 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/* --------------------------------------------------------------------
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211MKMETASTRUCT_H
+#define _P80211MKMETASTRUCT_H
+
+struct p80211msg_dot11req_mibget {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_unk392 mibattribute;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_dot11req_mibset {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_unk392 mibattribute;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_dot11req_scan {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 bsstype;
+	struct p80211item_pstr6 bssid;
+	u8 pad_0C[1];
+	struct p80211item_pstr32 ssid;
+	u8 pad_1D[3];
+	struct p80211item_uint32 scantype;
+	struct p80211item_uint32 probedelay;
+	struct p80211item_pstr14 channellist;
+	u8 pad_2C[1];
+	struct p80211item_uint32 minchanneltime;
+	struct p80211item_uint32 maxchanneltime;
+	struct p80211item_uint32 resultcode;
+	struct p80211item_uint32 numbss;
+	struct p80211item_uint32 append;
+} __packed;
+
+struct p80211msg_dot11req_scan_results {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 bssindex;
+	struct p80211item_uint32 resultcode;
+	struct p80211item_uint32 signal;
+	struct p80211item_uint32 noise;
+	struct p80211item_pstr6 bssid;
+	u8 pad_3C[1];
+	struct p80211item_pstr32 ssid;
+	u8 pad_4D[3];
+	struct p80211item_uint32 bsstype;
+	struct p80211item_uint32 beaconperiod;
+	struct p80211item_uint32 dtimperiod;
+	struct p80211item_uint32 timestamp;
+	struct p80211item_uint32 localtime;
+	struct p80211item_uint32 fhdwelltime;
+	struct p80211item_uint32 fhhopset;
+	struct p80211item_uint32 fhhoppattern;
+	struct p80211item_uint32 fhhopindex;
+	struct p80211item_uint32 dschannel;
+	struct p80211item_uint32 cfpcount;
+	struct p80211item_uint32 cfpperiod;
+	struct p80211item_uint32 cfpmaxduration;
+	struct p80211item_uint32 cfpdurremaining;
+	struct p80211item_uint32 ibssatimwindow;
+	struct p80211item_uint32 cfpollable;
+	struct p80211item_uint32 cfpollreq;
+	struct p80211item_uint32 privacy;
+	struct p80211item_uint32 capinfo;
+	struct p80211item_uint32 basicrate1;
+	struct p80211item_uint32 basicrate2;
+	struct p80211item_uint32 basicrate3;
+	struct p80211item_uint32 basicrate4;
+	struct p80211item_uint32 basicrate5;
+	struct p80211item_uint32 basicrate6;
+	struct p80211item_uint32 basicrate7;
+	struct p80211item_uint32 basicrate8;
+	struct p80211item_uint32 supprate1;
+	struct p80211item_uint32 supprate2;
+	struct p80211item_uint32 supprate3;
+	struct p80211item_uint32 supprate4;
+	struct p80211item_uint32 supprate5;
+	struct p80211item_uint32 supprate6;
+	struct p80211item_uint32 supprate7;
+	struct p80211item_uint32 supprate8;
+} __packed;
+
+struct p80211msg_dot11req_start {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_pstr32 ssid;
+	u8 pad_12D[3];
+	struct p80211item_uint32 bsstype;
+	struct p80211item_uint32 beaconperiod;
+	struct p80211item_uint32 dtimperiod;
+	struct p80211item_uint32 cfpperiod;
+	struct p80211item_uint32 cfpmaxduration;
+	struct p80211item_uint32 fhdwelltime;
+	struct p80211item_uint32 fhhopset;
+	struct p80211item_uint32 fhhoppattern;
+	struct p80211item_uint32 dschannel;
+	struct p80211item_uint32 ibssatimwindow;
+	struct p80211item_uint32 probedelay;
+	struct p80211item_uint32 cfpollable;
+	struct p80211item_uint32 cfpollreq;
+	struct p80211item_uint32 basicrate1;
+	struct p80211item_uint32 basicrate2;
+	struct p80211item_uint32 basicrate3;
+	struct p80211item_uint32 basicrate4;
+	struct p80211item_uint32 basicrate5;
+	struct p80211item_uint32 basicrate6;
+	struct p80211item_uint32 basicrate7;
+	struct p80211item_uint32 basicrate8;
+	struct p80211item_uint32 operationalrate1;
+	struct p80211item_uint32 operationalrate2;
+	struct p80211item_uint32 operationalrate3;
+	struct p80211item_uint32 operationalrate4;
+	struct p80211item_uint32 operationalrate5;
+	struct p80211item_uint32 operationalrate6;
+	struct p80211item_uint32 operationalrate7;
+	struct p80211item_uint32 operationalrate8;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_lnxreq_ifstate {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 ifstate;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_lnxreq_wlansniff {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 enable;
+	struct p80211item_uint32 channel;
+	struct p80211item_uint32 prismheader;
+	struct p80211item_uint32 wlanheader;
+	struct p80211item_uint32 keepwepflags;
+	struct p80211item_uint32 stripfcs;
+	struct p80211item_uint32 packet_trunc;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_lnxreq_hostwep {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 resultcode;
+	struct p80211item_uint32 decrypt;
+	struct p80211item_uint32 encrypt;
+} __packed;
+
+struct p80211msg_lnxreq_commsquality {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 resultcode;
+	struct p80211item_uint32 dbm;
+	struct p80211item_uint32 link;
+	struct p80211item_uint32 level;
+	struct p80211item_uint32 noise;
+	struct p80211item_uint32 txrate;
+} __packed;
+
+struct p80211msg_lnxreq_autojoin {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_pstr32 ssid;
+	u8 pad_19D[3];
+	struct p80211item_uint32 authtype;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_p2req_readpda {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_unk1024 pda;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_p2req_ramdl_state {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 enable;
+	struct p80211item_uint32 exeaddr;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_p2req_ramdl_write {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 addr;
+	struct p80211item_uint32 len;
+	struct p80211item_unk4096 data;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_p2req_flashdl_state {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 enable;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+struct p80211msg_p2req_flashdl_write {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+	struct p80211item_uint32 addr;
+	struct p80211item_uint32 len;
+	struct p80211item_unk4096 data;
+	struct p80211item_uint32 resultcode;
+} __packed;
+
+#endif
diff --git a/drivers/staging/wlan-ng/p80211mgmt.h b/drivers/staging/wlan-ng/p80211mgmt.h
new file mode 100644
index 000000000..1ef30d3f3
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211mgmt.h
@@ -0,0 +1,521 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Macros, types, and functions to handle 802.11 mgmt frames
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file declares the constants and types used in the interface
+ * between a wlan driver and the user mode utilities.
+ *
+ * Notes:
+ *  - Constant values are always in HOST byte order.  To assign
+ *    values to multi-byte fields they _must_ be converted to
+ *    ieee byte order.  To retrieve multi-byte values from incoming
+ *    frames, they must be converted to host order.
+ *
+ *  - The len member of the frame structure does NOT!!! include
+ *    the MAC CRC.  Therefore, the len field on rx'd frames should
+ *    have 4 subtracted from it.
+ *
+ * All functions declared here are implemented in p80211.c
+ *
+ * The types, macros, and functions defined here are primarily
+ * used for encoding and decoding management frames.  They are
+ * designed to follow these patterns of use:
+ *
+ * DECODE:
+ * 1) a frame of length len is received into buffer b
+ * 2) using the hdr structure and macros, we determine the type
+ * 3) an appropriate mgmt frame structure, mf, is allocated and zeroed
+ * 4) mf.hdr = b
+ *    mf.buf = b
+ *    mf.len = len
+ * 5) call mgmt_decode( mf )
+ * 6) the frame field pointers in mf are now set.  Note that any
+ *    multi-byte frame field values accessed using the frame field
+ *    pointers are in ieee byte order and will have to be converted
+ *    to host order.
+ *
+ * ENCODE:
+ * 1) Library client allocates buffer space for maximum length
+ *    frame of the desired type
+ * 2) Library client allocates a mgmt frame structure, called mf,
+ *    of the desired type
+ * 3) Set the following:
+ *    mf.type = <desired type>
+ *    mf.buf = <allocated buffer address>
+ * 4) call mgmt_encode( mf )
+ * 5) all of the fixed field pointers and fixed length information element
+ *    pointers in mf are now set to their respective locations in the
+ *    allocated space (fortunately, all variable length information elements
+ *    fall at the end of their respective frames).
+ * 5a) The length field is set to include the last of the fixed and fixed
+ *     length fields.  It may have to be updated for optional or variable
+ *	length information elements.
+ * 6) Optional and variable length information elements are special cases
+ *    and must be handled individually by the client code.
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211MGMT_H
+#define _P80211MGMT_H
+
+#ifndef _P80211HDR_H
+#include "p80211hdr.h"
+#endif
+
+/*-- Information Element IDs --------------------*/
+#define WLAN_EID_SSID		0
+#define WLAN_EID_SUPP_RATES	1
+#define WLAN_EID_FH_PARMS	2
+#define WLAN_EID_DS_PARMS	3
+#define WLAN_EID_CF_PARMS	4
+#define WLAN_EID_TIM		5
+#define WLAN_EID_IBSS_PARMS	6
+/*-- values 7-15 reserved --*/
+#define WLAN_EID_CHALLENGE	16
+/*-- values 17-31 reserved for challenge text extension --*/
+/*-- values 32-255 reserved --*/
+
+/*-- Reason Codes -------------------------------*/
+#define WLAN_MGMT_REASON_RSVD			0
+#define WLAN_MGMT_REASON_UNSPEC			1
+#define WLAN_MGMT_REASON_PRIOR_AUTH_INVALID	2
+#define WLAN_MGMT_REASON_DEAUTH_LEAVING		3
+#define WLAN_MGMT_REASON_DISASSOC_INACTIVE	4
+#define WLAN_MGMT_REASON_DISASSOC_AP_BUSY	5
+#define WLAN_MGMT_REASON_CLASS2_NONAUTH		6
+#define WLAN_MGMT_REASON_CLASS3_NONASSOC	7
+#define WLAN_MGMT_REASON_DISASSOC_STA_HASLEFT	8
+#define WLAN_MGMT_REASON_CANT_ASSOC_NONAUTH	9
+
+/*-- Status Codes -------------------------------*/
+#define WLAN_MGMT_STATUS_SUCCESS		0
+#define WLAN_MGMT_STATUS_UNSPEC_FAILURE		1
+#define WLAN_MGMT_STATUS_CAPS_UNSUPPORTED	10
+#define WLAN_MGMT_STATUS_REASSOC_NO_ASSOC	11
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC	12
+#define WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG	13
+#define WLAN_MGMT_STATUS_RX_AUTH_NOSEQ		14
+#define WLAN_MGMT_STATUS_CHALLENGE_FAIL		15
+#define WLAN_MGMT_STATUS_AUTH_TIMEOUT		16
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY	17
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_RATES	18
+  /* p80211b additions */
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_NOSHORT	19
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_NOPBCC	20
+#define WLAN_MGMT_STATUS_ASSOC_DENIED_NOAGILITY	21
+
+/*-- Auth Algorithm Field ---------------------------*/
+#define WLAN_AUTH_ALG_OPENSYSTEM		0
+#define WLAN_AUTH_ALG_SHAREDKEY			1
+
+/*-- Management Frame Field Offsets -------------*/
+/* Note: Not all fields are listed because of variable lengths,   */
+/*       see the code in p80211.c to see how we search for fields */
+/* Note: These offsets are from the start of the frame data       */
+
+#define WLAN_BEACON_OFF_TS			0
+#define WLAN_BEACON_OFF_BCN_int			8
+#define WLAN_BEACON_OFF_CAPINFO			10
+#define WLAN_BEACON_OFF_SSID			12
+
+#define WLAN_DISASSOC_OFF_REASON		0
+
+#define WLAN_ASSOCREQ_OFF_CAP_INFO		0
+#define WLAN_ASSOCREQ_OFF_LISTEN_int		2
+#define WLAN_ASSOCREQ_OFF_SSID			4
+
+#define WLAN_ASSOCRESP_OFF_CAP_INFO		0
+#define WLAN_ASSOCRESP_OFF_STATUS		2
+#define WLAN_ASSOCRESP_OFF_AID			4
+#define WLAN_ASSOCRESP_OFF_SUPP_RATES		6
+
+#define WLAN_REASSOCREQ_OFF_CAP_INFO		0
+#define WLAN_REASSOCREQ_OFF_LISTEN_int		2
+#define WLAN_REASSOCREQ_OFF_CURR_AP		4
+#define WLAN_REASSOCREQ_OFF_SSID		10
+
+#define WLAN_REASSOCRESP_OFF_CAP_INFO		0
+#define WLAN_REASSOCRESP_OFF_STATUS		2
+#define WLAN_REASSOCRESP_OFF_AID		4
+#define WLAN_REASSOCRESP_OFF_SUPP_RATES		6
+
+#define WLAN_PROBEREQ_OFF_SSID			0
+
+#define WLAN_PROBERESP_OFF_TS			0
+#define WLAN_PROBERESP_OFF_BCN_int		8
+#define WLAN_PROBERESP_OFF_CAP_INFO		10
+#define WLAN_PROBERESP_OFF_SSID			12
+
+#define WLAN_AUTHEN_OFF_AUTH_ALG		0
+#define WLAN_AUTHEN_OFF_AUTH_SEQ		2
+#define WLAN_AUTHEN_OFF_STATUS			4
+#define WLAN_AUTHEN_OFF_CHALLENGE		6
+
+#define WLAN_DEAUTHEN_OFF_REASON		0
+
+/*-- Capability Field ---------------------------*/
+#define WLAN_GET_MGMT_CAP_INFO_ESS(n)		((n) & BIT(0))
+#define WLAN_GET_MGMT_CAP_INFO_IBSS(n)		(((n) & BIT(1)) >> 1)
+#define WLAN_GET_MGMT_CAP_INFO_CFPOLLABLE(n)	(((n) & BIT(2)) >> 2)
+#define WLAN_GET_MGMT_CAP_INFO_CFPOLLREQ(n)	(((n) & BIT(3)) >> 3)
+#define WLAN_GET_MGMT_CAP_INFO_PRIVACY(n)	(((n) & BIT(4)) >> 4)
+  /* p80211b additions */
+#define WLAN_GET_MGMT_CAP_INFO_SHORT(n)		(((n) & BIT(5)) >> 5)
+#define WLAN_GET_MGMT_CAP_INFO_PBCC(n)		(((n) & BIT(6)) >> 6)
+#define WLAN_GET_MGMT_CAP_INFO_AGILITY(n)	(((n) & BIT(7)) >> 7)
+
+#define WLAN_SET_MGMT_CAP_INFO_ESS(n)		(n)
+#define WLAN_SET_MGMT_CAP_INFO_IBSS(n)		((n) << 1)
+#define WLAN_SET_MGMT_CAP_INFO_CFPOLLABLE(n)	((n) << 2)
+#define WLAN_SET_MGMT_CAP_INFO_CFPOLLREQ(n)	((n) << 3)
+#define WLAN_SET_MGMT_CAP_INFO_PRIVACY(n)	((n) << 4)
+  /* p80211b additions */
+#define WLAN_SET_MGMT_CAP_INFO_SHORT(n)		((n) << 5)
+#define WLAN_SET_MGMT_CAP_INFO_PBCC(n)		((n) << 6)
+#define WLAN_SET_MGMT_CAP_INFO_AGILITY(n)	((n) << 7)
+
+/*-- Information Element Types --------------------*/
+/* prototype structure, all IEs start with these members */
+
+struct wlan_ie {
+	u8 eid;
+	u8 len;
+} __packed;
+
+/*-- Service Set Identity (SSID)  -----------------*/
+struct wlan_ie_ssid {
+	u8 eid;
+	u8 len;
+	u8 ssid[1];		/* may be zero, ptrs may overlap */
+} __packed;
+
+/*-- Supported Rates  -----------------------------*/
+struct wlan_ie_supp_rates {
+	u8 eid;
+	u8 len;
+	u8 rates[1];		/* had better be at LEAST one! */
+} __packed;
+
+/*-- FH Parameter Set  ----------------------------*/
+struct wlan_ie_fh_parms {
+	u8 eid;
+	u8 len;
+	u16 dwell;
+	u8 hopset;
+	u8 hoppattern;
+	u8 hopindex;
+} __packed;
+
+/*-- DS Parameter Set  ----------------------------*/
+struct wlan_ie_ds_parms {
+	u8 eid;
+	u8 len;
+	u8 curr_ch;
+} __packed;
+
+/*-- CF Parameter Set  ----------------------------*/
+
+struct wlan_ie_cf_parms {
+	u8 eid;
+	u8 len;
+	u8 cfp_cnt;
+	u8 cfp_period;
+	u16 cfp_maxdur;
+	u16 cfp_durremaining;
+} __packed;
+
+/*-- TIM ------------------------------------------*/
+struct wlan_ie_tim {
+	u8 eid;
+	u8 len;
+	u8 dtim_cnt;
+	u8 dtim_period;
+	u8 bitmap_ctl;
+	u8 virt_bm[1];
+} __packed;
+
+/*-- IBSS Parameter Set ---------------------------*/
+struct wlan_ie_ibss_parms {
+	u8 eid;
+	u8 len;
+	u16 atim_win;
+} __packed;
+
+/*-- Challenge Text  ------------------------------*/
+struct wlan_ie_challenge {
+	u8 eid;
+	u8 len;
+	u8 challenge[1];
+} __packed;
+
+/*-------------------------------------------------*/
+/*  Frame Types  */
+
+/* prototype structure, all mgmt frame types will start with these members */
+struct wlan_fr_mgmt {
+	u16 type;
+	u16 len;		/* DOES NOT include CRC !!!! */
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	/*-- info elements ----------*/
+};
+
+/*-- Beacon ---------------------------------------*/
+struct wlan_fr_beacon {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u64 *ts;
+	u16 *bcn_int;
+	u16 *cap_info;
+	/*-- info elements ----------*/
+	struct wlan_ie_ssid *ssid;
+	struct wlan_ie_supp_rates *supp_rates;
+	struct wlan_ie_fh_parms *fh_parms;
+	struct wlan_ie_ds_parms *ds_parms;
+	struct wlan_ie_cf_parms *cf_parms;
+	struct wlan_ie_ibss_parms *ibss_parms;
+	struct wlan_ie_tim *tim;
+
+};
+
+/*-- IBSS ATIM ------------------------------------*/
+struct wlan_fr_ibssatim {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+
+	/*-- fixed fields -----------*/
+	/*-- info elements ----------*/
+
+	/* this frame type has a null body */
+
+};
+
+/*-- Disassociation -------------------------------*/
+struct wlan_fr_disassoc {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *reason;
+
+	/*-- info elements ----------*/
+
+};
+
+/*-- Association Request --------------------------*/
+struct wlan_fr_assocreq {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *cap_info;
+	u16 *listen_int;
+	/*-- info elements ----------*/
+	struct wlan_ie_ssid *ssid;
+	struct wlan_ie_supp_rates *supp_rates;
+
+};
+
+/*-- Association Response -------------------------*/
+struct wlan_fr_assocresp {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *cap_info;
+	u16 *status;
+	u16 *aid;
+	/*-- info elements ----------*/
+	struct wlan_ie_supp_rates *supp_rates;
+
+};
+
+/*-- Reassociation Request ------------------------*/
+struct wlan_fr_reassocreq {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *cap_info;
+	u16 *listen_int;
+	u8 *curr_ap;
+	/*-- info elements ----------*/
+	struct wlan_ie_ssid *ssid;
+	struct wlan_ie_supp_rates *supp_rates;
+
+};
+
+/*-- Reassociation Response -----------------------*/
+struct wlan_fr_reassocresp {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *cap_info;
+	u16 *status;
+	u16 *aid;
+	/*-- info elements ----------*/
+	struct wlan_ie_supp_rates *supp_rates;
+
+};
+
+/*-- Probe Request --------------------------------*/
+struct wlan_fr_probereq {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	/*-- info elements ----------*/
+	struct wlan_ie_ssid *ssid;
+	struct wlan_ie_supp_rates *supp_rates;
+
+};
+
+/*-- Probe Response -------------------------------*/
+struct wlan_fr_proberesp {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u64 *ts;
+	u16 *bcn_int;
+	u16 *cap_info;
+	/*-- info elements ----------*/
+	struct wlan_ie_ssid *ssid;
+	struct wlan_ie_supp_rates *supp_rates;
+	struct wlan_ie_fh_parms *fh_parms;
+	struct wlan_ie_ds_parms *ds_parms;
+	struct wlan_ie_cf_parms *cf_parms;
+	struct wlan_ie_ibss_parms *ibss_parms;
+};
+
+/*-- Authentication -------------------------------*/
+struct wlan_fr_authen {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *auth_alg;
+	u16 *auth_seq;
+	u16 *status;
+	/*-- info elements ----------*/
+	struct wlan_ie_challenge *challenge;
+
+};
+
+/*-- Deauthenication -----------------------------*/
+struct wlan_fr_deauthen {
+	u16 type;
+	u16 len;
+	u8 *buf;
+	struct p80211_hdr *hdr;
+	/* used for target specific data, skb in Linux */
+	void *priv;
+	/*-- fixed fields -----------*/
+	u16 *reason;
+
+	/*-- info elements ----------*/
+
+};
+
+void wlan_mgmt_encode_beacon(struct wlan_fr_beacon *f);
+void wlan_mgmt_decode_beacon(struct wlan_fr_beacon *f);
+void wlan_mgmt_encode_disassoc(struct wlan_fr_disassoc *f);
+void wlan_mgmt_decode_disassoc(struct wlan_fr_disassoc *f);
+void wlan_mgmt_encode_assocreq(struct wlan_fr_assocreq *f);
+void wlan_mgmt_decode_assocreq(struct wlan_fr_assocreq *f);
+void wlan_mgmt_encode_assocresp(struct wlan_fr_assocresp *f);
+void wlan_mgmt_decode_assocresp(struct wlan_fr_assocresp *f);
+void wlan_mgmt_encode_reassocreq(struct wlan_fr_reassocreq *f);
+void wlan_mgmt_decode_reassocreq(struct wlan_fr_reassocreq *f);
+void wlan_mgmt_encode_reassocresp(struct wlan_fr_reassocresp *f);
+void wlan_mgmt_decode_reassocresp(struct wlan_fr_reassocresp *f);
+void wlan_mgmt_encode_probereq(struct wlan_fr_probereq *f);
+void wlan_mgmt_decode_probereq(struct wlan_fr_probereq *f);
+void wlan_mgmt_encode_proberesp(struct wlan_fr_proberesp *f);
+void wlan_mgmt_decode_proberesp(struct wlan_fr_proberesp *f);
+void wlan_mgmt_encode_authen(struct wlan_fr_authen *f);
+void wlan_mgmt_decode_authen(struct wlan_fr_authen *f);
+void wlan_mgmt_encode_deauthen(struct wlan_fr_deauthen *f);
+void wlan_mgmt_decode_deauthen(struct wlan_fr_deauthen *f);
+
+#endif /* _P80211MGMT_H */
diff --git a/drivers/staging/wlan-ng/p80211msg.h b/drivers/staging/wlan-ng/p80211msg.h
new file mode 100644
index 000000000..f68d8b7d5
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211msg.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Macros, constants, types, and funcs for req and ind messages
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _P80211MSG_H
+#define _P80211MSG_H
+
+#define WLAN_DEVNAMELEN_MAX	16
+
+struct p80211msg {
+	u32 msgcode;
+	u32 msglen;
+	u8 devname[WLAN_DEVNAMELEN_MAX];
+} __packed;
+
+#endif /* _P80211MSG_H */
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
new file mode 100644
index 000000000..e04fc666d
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -0,0 +1,1031 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Linux Kernel net device interface
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * The functions required for a Linux network device are defined here.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/proc_fs.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/kmod.h>
+#include <linux/if_arp.h>
+#include <linux/wireless.h>
+#include <linux/sockios.h>
+#include <linux/etherdevice.h>
+#include <linux/if_ether.h>
+#include <linux/byteorder/generic.h>
+#include <linux/bitops.h>
+#include <linux/uaccess.h>
+#include <asm/byteorder.h>
+
+#ifdef SIOCETHTOOL
+#include <linux/ethtool.h>
+#endif
+
+#include <net/iw_handler.h>
+#include <net/net_namespace.h>
+#include <net/cfg80211.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211conv.h"
+#include "p80211mgmt.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211ioctl.h"
+#include "p80211req.h"
+#include "p80211metastruct.h"
+#include "p80211metadef.h"
+
+#include "cfg80211.c"
+
+/* netdevice method functions */
+static int p80211knetdev_init(struct net_device *netdev);
+static int p80211knetdev_open(struct net_device *netdev);
+static int p80211knetdev_stop(struct net_device *netdev);
+static netdev_tx_t p80211knetdev_hard_start_xmit(struct sk_buff *skb,
+						 struct net_device *netdev);
+static void p80211knetdev_set_multicast_list(struct net_device *dev);
+static int p80211knetdev_siocdevprivate(struct net_device *dev, struct ifreq *ifr,
+					void __user *data, int cmd);
+static int p80211knetdev_set_mac_address(struct net_device *dev, void *addr);
+static void p80211knetdev_tx_timeout(struct net_device *netdev, unsigned int txqueue);
+static int p80211_rx_typedrop(struct wlandevice *wlandev, u16 fc);
+
+int wlan_watchdog = 5000;
+module_param(wlan_watchdog, int, 0644);
+MODULE_PARM_DESC(wlan_watchdog, "transmit timeout in milliseconds");
+
+int wlan_wext_write = 1;
+module_param(wlan_wext_write, int, 0644);
+MODULE_PARM_DESC(wlan_wext_write, "enable write wireless extensions");
+
+/*----------------------------------------------------------------
+ * p80211knetdev_init
+ *
+ * Init method for a Linux netdevice.  Called in response to
+ * register_netdev.
+ *
+ * Arguments:
+ *	none
+ *
+ * Returns:
+ *	nothing
+ *----------------------------------------------------------------
+ */
+static int p80211knetdev_init(struct net_device *netdev)
+{
+	/* Called in response to register_netdev */
+	/* This is usually the probe function, but the probe has */
+	/* already been done by the MSD and the create_kdev */
+	/* function.  All we do here is return success */
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_open
+ *
+ * Linux netdevice open method.  Following a successful call here,
+ * the device is supposed to be ready for tx and rx.  In our
+ * situation that may not be entirely true due to the state of the
+ * MAC below.
+ *
+ * Arguments:
+ *	netdev		Linux network device structure
+ *
+ * Returns:
+ *	zero on success, non-zero otherwise
+ *----------------------------------------------------------------
+ */
+static int p80211knetdev_open(struct net_device *netdev)
+{
+	int result = 0;		/* success */
+	struct wlandevice *wlandev = netdev->ml_priv;
+
+	/* Check to make sure the MSD is running */
+	if (wlandev->msdstate != WLAN_MSD_RUNNING)
+		return -ENODEV;
+
+	/* Tell the MSD to open */
+	if (wlandev->open) {
+		result = wlandev->open(wlandev);
+		if (result == 0) {
+			netif_start_queue(wlandev->netdev);
+			wlandev->state = WLAN_DEVICE_OPEN;
+		}
+	} else {
+		result = -EAGAIN;
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_stop
+ *
+ * Linux netdevice stop (close) method.  Following this call,
+ * no frames should go up or down through this interface.
+ *
+ * Arguments:
+ *	netdev		Linux network device structure
+ *
+ * Returns:
+ *	zero on success, non-zero otherwise
+ *----------------------------------------------------------------
+ */
+static int p80211knetdev_stop(struct net_device *netdev)
+{
+	int result = 0;
+	struct wlandevice *wlandev = netdev->ml_priv;
+
+	if (wlandev->close)
+		result = wlandev->close(wlandev);
+
+	netif_stop_queue(wlandev->netdev);
+	wlandev->state = WLAN_DEVICE_CLOSED;
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * p80211netdev_rx
+ *
+ * Frame receive function called by the mac specific driver.
+ *
+ * Arguments:
+ *	wlandev		WLAN network device structure
+ *	skb		skbuff containing a full 802.11 frame.
+ * Returns:
+ *	nothing
+ * Side effects:
+ *
+ *----------------------------------------------------------------
+ */
+void p80211netdev_rx(struct wlandevice *wlandev, struct sk_buff *skb)
+{
+	/* Enqueue for post-irq processing */
+	skb_queue_tail(&wlandev->nsd_rxq, skb);
+	tasklet_schedule(&wlandev->rx_bh);
+}
+
+#define CONV_TO_ETHER_SKIPPED	0x01
+#define CONV_TO_ETHER_FAILED	0x02
+
+/**
+ * p80211_convert_to_ether - conversion from 802.11 frame to ethernet frame
+ * @wlandev: pointer to WLAN device
+ * @skb: pointer to socket buffer
+ *
+ * Returns: 0 if conversion succeeded
+ *	    CONV_TO_ETHER_FAILED if conversion failed
+ *	    CONV_TO_ETHER_SKIPPED if frame is ignored
+ */
+static int p80211_convert_to_ether(struct wlandevice *wlandev,
+				   struct sk_buff *skb)
+{
+	struct p80211_hdr *hdr;
+
+	hdr = (struct p80211_hdr *)skb->data;
+	if (p80211_rx_typedrop(wlandev, le16_to_cpu(hdr->frame_control)))
+		return CONV_TO_ETHER_SKIPPED;
+
+	/* perform mcast filtering: allow my local address through but reject
+	 * anything else that isn't multicast
+	 */
+	if (wlandev->netdev->flags & IFF_ALLMULTI) {
+		if (!ether_addr_equal_unaligned(wlandev->netdev->dev_addr,
+						hdr->address1)) {
+			if (!is_multicast_ether_addr(hdr->address1))
+				return CONV_TO_ETHER_SKIPPED;
+		}
+	}
+
+	if (skb_p80211_to_ether(wlandev, wlandev->ethconv, skb) == 0) {
+		wlandev->netdev->stats.rx_packets++;
+		wlandev->netdev->stats.rx_bytes += skb->len;
+		netif_rx(skb);
+		return 0;
+	}
+
+	netdev_dbg(wlandev->netdev, "%s failed.\n", __func__);
+	return CONV_TO_ETHER_FAILED;
+}
+
+/**
+ * p80211netdev_rx_bh - deferred processing of all received frames
+ *
+ * @t: pointer to the tasklet associated with this handler
+ */
+static void p80211netdev_rx_bh(struct tasklet_struct *t)
+{
+	struct wlandevice *wlandev = from_tasklet(wlandev, t, rx_bh);
+	struct sk_buff *skb = NULL;
+	struct net_device *dev = wlandev->netdev;
+
+	/* Let's empty our queue */
+	while ((skb = skb_dequeue(&wlandev->nsd_rxq))) {
+		if (wlandev->state == WLAN_DEVICE_OPEN) {
+			if (dev->type != ARPHRD_ETHER) {
+				/* RAW frame; we shouldn't convert it */
+				/* XXX Append the Prism Header here instead. */
+
+				/* set up various data fields */
+				skb->dev = dev;
+				skb_reset_mac_header(skb);
+				skb->ip_summed = CHECKSUM_NONE;
+				skb->pkt_type = PACKET_OTHERHOST;
+				skb->protocol = htons(ETH_P_80211_RAW);
+
+				dev->stats.rx_packets++;
+				dev->stats.rx_bytes += skb->len;
+				netif_rx(skb);
+				continue;
+			} else {
+				if (!p80211_convert_to_ether(wlandev, skb))
+					continue;
+			}
+		}
+		dev_kfree_skb(skb);
+	}
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_hard_start_xmit
+ *
+ * Linux netdevice method for transmitting a frame.
+ *
+ * Arguments:
+ *	skb	Linux sk_buff containing the frame.
+ *	netdev	Linux netdevice.
+ *
+ * Side effects:
+ *	If the lower layers report that buffers are full. netdev->tbusy
+ *	will be set to prevent higher layers from sending more traffic.
+ *
+ *	Note: If this function returns non-zero, higher layers retain
+ *	      ownership of the skb.
+ *
+ * Returns:
+ *	zero on success, non-zero on failure.
+ *----------------------------------------------------------------
+ */
+static netdev_tx_t p80211knetdev_hard_start_xmit(struct sk_buff *skb,
+						 struct net_device *netdev)
+{
+	int result = 0;
+	int txresult;
+	struct wlandevice *wlandev = netdev->ml_priv;
+	struct p80211_hdr p80211_hdr;
+	struct p80211_metawep p80211_wep;
+
+	p80211_wep.data = NULL;
+
+	if (!skb)
+		return NETDEV_TX_OK;
+
+	if (wlandev->state != WLAN_DEVICE_OPEN) {
+		result = 1;
+		goto failed;
+	}
+
+	memset(&p80211_hdr, 0, sizeof(p80211_hdr));
+	memset(&p80211_wep, 0, sizeof(p80211_wep));
+
+	if (netif_queue_stopped(netdev)) {
+		netdev_dbg(netdev, "called when queue stopped.\n");
+		result = 1;
+		goto failed;
+	}
+
+	netif_stop_queue(netdev);
+
+	/* Check to see that a valid mode is set */
+	switch (wlandev->macmode) {
+	case WLAN_MACMODE_IBSS_STA:
+	case WLAN_MACMODE_ESS_STA:
+	case WLAN_MACMODE_ESS_AP:
+		break;
+	default:
+		/* Mode isn't set yet, just drop the frame
+		 * and return success .
+		 * TODO: we need a saner way to handle this
+		 */
+		if (be16_to_cpu(skb->protocol) != ETH_P_80211_RAW) {
+			netif_start_queue(wlandev->netdev);
+			netdev_notice(netdev, "Tx attempt prior to association, frame dropped.\n");
+			netdev->stats.tx_dropped++;
+			result = 0;
+			goto failed;
+		}
+		break;
+	}
+
+	/* Check for raw transmits */
+	if (be16_to_cpu(skb->protocol) == ETH_P_80211_RAW) {
+		if (!capable(CAP_NET_ADMIN)) {
+			result = 1;
+			goto failed;
+		}
+		/* move the header over */
+		memcpy(&p80211_hdr, skb->data, sizeof(p80211_hdr));
+		skb_pull(skb, sizeof(p80211_hdr));
+	} else {
+		if (skb_ether_to_p80211
+		    (wlandev, wlandev->ethconv, skb, &p80211_hdr,
+		     &p80211_wep) != 0) {
+			/* convert failed */
+			netdev_dbg(netdev, "ether_to_80211(%d) failed.\n",
+				   wlandev->ethconv);
+			result = 1;
+			goto failed;
+		}
+	}
+	if (!wlandev->txframe) {
+		result = 1;
+		goto failed;
+	}
+
+	netif_trans_update(netdev);
+
+	netdev->stats.tx_packets++;
+	/* count only the packet payload */
+	netdev->stats.tx_bytes += skb->len;
+
+	txresult = wlandev->txframe(wlandev, skb, &p80211_hdr, &p80211_wep);
+
+	if (txresult == 0) {
+		/* success and more buf */
+		/* avail, re: hw_txdata */
+		netif_wake_queue(wlandev->netdev);
+		result = NETDEV_TX_OK;
+	} else if (txresult == 1) {
+		/* success, no more avail */
+		netdev_dbg(netdev, "txframe success, no more bufs\n");
+		/* netdev->tbusy = 1;  don't set here, irqhdlr */
+		/*   may have already cleared it */
+		result = NETDEV_TX_OK;
+	} else if (txresult == 2) {
+		/* alloc failure, drop frame */
+		netdev_dbg(netdev, "txframe returned alloc_fail\n");
+		result = NETDEV_TX_BUSY;
+	} else {
+		/* buffer full or queue busy, drop frame. */
+		netdev_dbg(netdev, "txframe returned full or busy\n");
+		result = NETDEV_TX_BUSY;
+	}
+
+failed:
+	/* Free up the WEP buffer if it's not the same as the skb */
+	if ((p80211_wep.data) && (p80211_wep.data != skb->data))
+		kfree_sensitive(p80211_wep.data);
+
+	/* we always free the skb here, never in a lower level. */
+	if (!result)
+		dev_kfree_skb(skb);
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_set_multicast_list
+ *
+ * Called from higher layers whenever there's a need to set/clear
+ * promiscuous mode or rewrite the multicast list.
+ *
+ * Arguments:
+ *	none
+ *
+ * Returns:
+ *	nothing
+ *----------------------------------------------------------------
+ */
+static void p80211knetdev_set_multicast_list(struct net_device *dev)
+{
+	struct wlandevice *wlandev = dev->ml_priv;
+
+	/* TODO:  real multicast support as well */
+
+	if (wlandev->set_multicast_list)
+		wlandev->set_multicast_list(wlandev, dev);
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_siocdevprivate
+ *
+ * Handle an ioctl call on one of our devices.  Everything Linux
+ * ioctl specific is done here.  Then we pass the contents of the
+ * ifr->data to the request message handler.
+ *
+ * Arguments:
+ *	dev	Linux kernel netdevice
+ *	ifr	Our private ioctl request structure, typed for the
+ *		generic struct ifreq so we can use ptr to func
+ *		w/o cast.
+ *
+ * Returns:
+ *	zero on success, a negative errno on failure.  Possible values:
+ *		-ENETDOWN Device isn't up.
+ *		-EBUSY	cmd already in progress
+ *		-ETIME	p80211 cmd timed out (MSD may have its own timers)
+ *		-EFAULT memory fault copying msg from user buffer
+ *		-ENOMEM unable to allocate kernel msg buffer
+ *		-EINVAL	bad magic, it the cmd really for us?
+ *		-EintR	sleeping on cmd, awakened by signal, cmd cancelled.
+ *
+ * Call Context:
+ *	Process thread (ioctl caller).  TODO: SMP support may require
+ *	locks.
+ *----------------------------------------------------------------
+ */
+static int p80211knetdev_siocdevprivate(struct net_device *dev,
+					struct ifreq *ifr,
+					void __user *data, int cmd)
+{
+	int result = 0;
+	struct p80211ioctl_req *req = (struct p80211ioctl_req *)ifr;
+	struct wlandevice *wlandev = dev->ml_priv;
+	u8 *msgbuf;
+
+	netdev_dbg(dev, "rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
+
+	if (in_compat_syscall())
+		return -EOPNOTSUPP;
+
+	/* Test the magic, assume ifr is good if it's there */
+	if (req->magic != P80211_IOCTL_MAGIC) {
+		result = -EINVAL;
+		goto bail;
+	}
+
+	if (cmd == P80211_IFTEST) {
+		result = 0;
+		goto bail;
+	} else if (cmd != P80211_IFREQ) {
+		result = -EINVAL;
+		goto bail;
+	}
+
+	msgbuf = memdup_user(data, req->len);
+	if (IS_ERR(msgbuf)) {
+		result = PTR_ERR(msgbuf);
+		goto bail;
+	}
+
+	result = p80211req_dorequest(wlandev, msgbuf);
+
+	if (result == 0) {
+		if (copy_to_user(data, msgbuf, req->len))
+			result = -EFAULT;
+	}
+	kfree(msgbuf);
+
+bail:
+	/* If allocate,copyfrom or copyto fails, return errno */
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * p80211knetdev_set_mac_address
+ *
+ * Handles the ioctl for changing the MACAddress of a netdevice
+ *
+ * references: linux/netdevice.h and drivers/net/net_init.c
+ *
+ * NOTE: [MSM] We only prevent address changes when the netdev is
+ * up.  We don't control anything based on dot11 state.  If the
+ * address is changed on a STA that's currently associated, you
+ * will probably lose the ability to send and receive data frames.
+ * Just be aware.  Therefore, this should usually only be done
+ * prior to scan/join/auth/assoc.
+ *
+ * Arguments:
+ *	dev	netdevice struct
+ *	addr	the new MACAddress (a struct)
+ *
+ * Returns:
+ *	zero on success, a negative errno on failure.  Possible values:
+ *		-EBUSY	device is bussy (cmd not possible)
+ *		-and errors returned by: p80211req_dorequest(..)
+ *
+ * by: Collin R. Mulliner <collin@mulliner.org>
+ *----------------------------------------------------------------
+ */
+static int p80211knetdev_set_mac_address(struct net_device *dev, void *addr)
+{
+	struct sockaddr *new_addr = addr;
+	struct p80211msg_dot11req_mibset dot11req;
+	struct p80211item_unk392 *mibattr;
+	struct p80211item_pstr6 *macaddr;
+	struct p80211item_uint32 *resultcode;
+	int result;
+
+	/* If we're running, we don't allow MAC address changes */
+	if (netif_running(dev))
+		return -EBUSY;
+
+	/* Set up some convenience pointers. */
+	mibattr = &dot11req.mibattribute;
+	macaddr = (struct p80211item_pstr6 *)&mibattr->data;
+	resultcode = &dot11req.resultcode;
+
+	/* Set up a dot11req_mibset */
+	memset(&dot11req, 0, sizeof(dot11req));
+	dot11req.msgcode = DIDMSG_DOT11REQ_MIBSET;
+	dot11req.msglen = sizeof(dot11req);
+	memcpy(dot11req.devname,
+	       ((struct wlandevice *)dev->ml_priv)->name,
+	       WLAN_DEVNAMELEN_MAX - 1);
+
+	/* Set up the mibattribute argument */
+	mibattr->did = DIDMSG_DOT11REQ_MIBSET_MIBATTRIBUTE;
+	mibattr->status = P80211ENUM_msgitem_status_data_ok;
+	mibattr->len = sizeof(mibattr->data);
+
+	macaddr->did = DIDMIB_DOT11MAC_OPERATIONTABLE_MACADDRESS;
+	macaddr->status = P80211ENUM_msgitem_status_data_ok;
+	macaddr->len = sizeof(macaddr->data);
+	macaddr->data.len = ETH_ALEN;
+	memcpy(&macaddr->data.data, new_addr->sa_data, ETH_ALEN);
+
+	/* Set up the resultcode argument */
+	resultcode->did = DIDMSG_DOT11REQ_MIBSET_RESULTCODE;
+	resultcode->status = P80211ENUM_msgitem_status_no_value;
+	resultcode->len = sizeof(resultcode->data);
+	resultcode->data = 0;
+
+	/* now fire the request */
+	result = p80211req_dorequest(dev->ml_priv, (u8 *)&dot11req);
+
+	/* If the request wasn't successful, report an error and don't
+	 * change the netdev address
+	 */
+	if (result != 0 || resultcode->data != P80211ENUM_resultcode_success) {
+		netdev_err(dev, "Low-level driver failed dot11req_mibset(dot11MACAddress).\n");
+		result = -EADDRNOTAVAIL;
+	} else {
+		/* everything's ok, change the addr in netdev */
+		eth_hw_addr_set(dev, new_addr->sa_data);
+	}
+
+	return result;
+}
+
+static const struct net_device_ops p80211_netdev_ops = {
+	.ndo_init = p80211knetdev_init,
+	.ndo_open = p80211knetdev_open,
+	.ndo_stop = p80211knetdev_stop,
+	.ndo_start_xmit = p80211knetdev_hard_start_xmit,
+	.ndo_set_rx_mode = p80211knetdev_set_multicast_list,
+	.ndo_siocdevprivate = p80211knetdev_siocdevprivate,
+	.ndo_set_mac_address = p80211knetdev_set_mac_address,
+	.ndo_tx_timeout = p80211knetdev_tx_timeout,
+	.ndo_validate_addr = eth_validate_addr,
+};
+
+/*----------------------------------------------------------------
+ * wlan_setup
+ *
+ * Roughly matches the functionality of ether_setup.  Here
+ * we set up any members of the wlandevice structure that are common
+ * to all devices.  Additionally, we allocate a linux 'struct device'
+ * and perform the same setup as ether_setup.
+ *
+ * Note: It's important that the caller have setup the wlandev->name
+ *	ptr prior to calling this function.
+ *
+ * Arguments:
+ *	wlandev		ptr to the wlandev structure for the
+ *			interface.
+ *	physdev		ptr to usb device
+ * Returns:
+ *	zero on success, non-zero otherwise.
+ * Call Context:
+ *	Should be process thread.  We'll assume it might be
+ *	interrupt though.  When we add support for statically
+ *	compiled drivers, this function will be called in the
+ *	context of the kernel startup code.
+ *----------------------------------------------------------------
+ */
+int wlan_setup(struct wlandevice *wlandev, struct device *physdev)
+{
+	int result = 0;
+	struct net_device *netdev;
+	struct wiphy *wiphy;
+	struct wireless_dev *wdev;
+
+	/* Set up the wlandev */
+	wlandev->state = WLAN_DEVICE_CLOSED;
+	wlandev->ethconv = WLAN_ETHCONV_8021h;
+	wlandev->macmode = WLAN_MACMODE_NONE;
+
+	/* Set up the rx queue */
+	skb_queue_head_init(&wlandev->nsd_rxq);
+	tasklet_setup(&wlandev->rx_bh, p80211netdev_rx_bh);
+
+	/* Allocate and initialize the wiphy struct */
+	wiphy = wlan_create_wiphy(physdev, wlandev);
+	if (!wiphy) {
+		dev_err(physdev, "Failed to alloc wiphy.\n");
+		return 1;
+	}
+
+	/* Allocate and initialize the struct device */
+	netdev = alloc_netdev(sizeof(struct wireless_dev), "wlan%d",
+			      NET_NAME_UNKNOWN, ether_setup);
+	if (!netdev) {
+		dev_err(physdev, "Failed to alloc netdev.\n");
+		wlan_free_wiphy(wiphy);
+		result = 1;
+	} else {
+		wlandev->netdev = netdev;
+		netdev->ml_priv = wlandev;
+		netdev->netdev_ops = &p80211_netdev_ops;
+		wdev = netdev_priv(netdev);
+		wdev->wiphy = wiphy;
+		wdev->iftype = NL80211_IFTYPE_STATION;
+		netdev->ieee80211_ptr = wdev;
+		netdev->min_mtu = 68;
+		/* 2312 is max 802.11 payload, 20 is overhead,
+		 * (ether + llc + snap) and another 8 for wep.
+		 */
+		netdev->max_mtu = (2312 - 20 - 8);
+
+		netif_stop_queue(netdev);
+		netif_carrier_off(netdev);
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * wlan_unsetup
+ *
+ * This function is paired with the wlan_setup routine.  It should
+ * be called after unregister_wlandev.  Basically, all it does is
+ * free the 'struct device' that's associated with the wlandev.
+ * We do it here because the 'struct device' isn't allocated
+ * explicitly in the driver code, it's done in wlan_setup.  To
+ * do the free in the driver might seem like 'magic'.
+ *
+ * Arguments:
+ *	wlandev		ptr to the wlandev structure for the
+ *			interface.
+ * Call Context:
+ *	Should be process thread.  We'll assume it might be
+ *	interrupt though.  When we add support for statically
+ *	compiled drivers, this function will be called in the
+ *	context of the kernel startup code.
+ *----------------------------------------------------------------
+ */
+void wlan_unsetup(struct wlandevice *wlandev)
+{
+	struct wireless_dev *wdev;
+
+	tasklet_kill(&wlandev->rx_bh);
+
+	if (wlandev->netdev) {
+		wdev = netdev_priv(wlandev->netdev);
+		if (wdev->wiphy)
+			wlan_free_wiphy(wdev->wiphy);
+		free_netdev(wlandev->netdev);
+		wlandev->netdev = NULL;
+	}
+}
+
+/*----------------------------------------------------------------
+ * register_wlandev
+ *
+ * Roughly matches the functionality of register_netdev.  This function
+ * is called after the driver has successfully probed and set up the
+ * resources for the device.  It's now ready to become a named device
+ * in the Linux system.
+ *
+ * First we allocate a name for the device (if not already set), then
+ * we call the Linux function register_netdevice.
+ *
+ * Arguments:
+ *	wlandev		ptr to the wlandev structure for the
+ *			interface.
+ * Returns:
+ *	zero on success, non-zero otherwise.
+ * Call Context:
+ *	Can be either interrupt or not.
+ *----------------------------------------------------------------
+ */
+int register_wlandev(struct wlandevice *wlandev)
+{
+	return register_netdev(wlandev->netdev);
+}
+
+/*----------------------------------------------------------------
+ * unregister_wlandev
+ *
+ * Roughly matches the functionality of unregister_netdev.  This
+ * function is called to remove a named device from the system.
+ *
+ * First we tell linux that the device should no longer exist.
+ * Then we remove it from the list of known wlan devices.
+ *
+ * Arguments:
+ *	wlandev		ptr to the wlandev structure for the
+ *			interface.
+ * Returns:
+ *	zero on success, non-zero otherwise.
+ * Call Context:
+ *	Can be either interrupt or not.
+ *----------------------------------------------------------------
+ */
+int unregister_wlandev(struct wlandevice *wlandev)
+{
+	struct sk_buff *skb;
+
+	unregister_netdev(wlandev->netdev);
+
+	/* Now to clean out the rx queue */
+	while ((skb = skb_dequeue(&wlandev->nsd_rxq)))
+		dev_kfree_skb(skb);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * p80211netdev_hwremoved
+ *
+ * Hardware removed notification. This function should be called
+ * immediately after an MSD has detected that the underlying hardware
+ * has been yanked out from under us.  The primary things we need
+ * to do are:
+ *   - Mark the wlandev
+ *   - Prevent any further traffic from the knetdev i/f
+ *   - Prevent any further requests from mgmt i/f
+ *   - If there are any waitq'd mgmt requests or mgmt-frame exchanges,
+ *     shut them down.
+ *   - Call the MSD hwremoved function.
+ *
+ * The remainder of the cleanup will be handled by unregister().
+ * Our primary goal here is to prevent as much tickling of the MSD
+ * as possible since the MSD is already in a 'wounded' state.
+ *
+ * TODO: As new features are added, this function should be
+ *       updated.
+ *
+ * Arguments:
+ *	wlandev		WLAN network device structure
+ * Returns:
+ *	nothing
+ * Side effects:
+ *
+ * Call context:
+ *	Usually interrupt.
+ *----------------------------------------------------------------
+ */
+void p80211netdev_hwremoved(struct wlandevice *wlandev)
+{
+	wlandev->hwremoved = 1;
+	if (wlandev->state == WLAN_DEVICE_OPEN)
+		netif_stop_queue(wlandev->netdev);
+
+	netif_device_detach(wlandev->netdev);
+}
+
+/*----------------------------------------------------------------
+ * p80211_rx_typedrop
+ *
+ * Classifies the frame, increments the appropriate counter, and
+ * returns 0|1|2 indicating whether the driver should handle, ignore, or
+ * drop the frame
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	fc		frame control field
+ *
+ * Returns:
+ *	zero if the frame should be handled by the driver,
+ *       one if the frame should be ignored
+ *       anything else means we drop it.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+static int p80211_rx_typedrop(struct wlandevice *wlandev, u16 fc)
+{
+	u16 ftype;
+	u16 fstype;
+	int drop = 0;
+	/* Classify frame, increment counter */
+	ftype = WLAN_GET_FC_FTYPE(fc);
+	fstype = WLAN_GET_FC_FSTYPE(fc);
+	switch (ftype) {
+	case WLAN_FTYPE_MGMT:
+		if ((wlandev->netdev->flags & IFF_PROMISC) ||
+		    (wlandev->netdev->flags & IFF_ALLMULTI)) {
+			drop = 1;
+			break;
+		}
+		netdev_dbg(wlandev->netdev, "rx'd mgmt:\n");
+		wlandev->rx.mgmt++;
+		switch (fstype) {
+		case WLAN_FSTYPE_ASSOCREQ:
+			/* printk("assocreq"); */
+			wlandev->rx.assocreq++;
+			break;
+		case WLAN_FSTYPE_ASSOCRESP:
+			/* printk("assocresp"); */
+			wlandev->rx.assocresp++;
+			break;
+		case WLAN_FSTYPE_REASSOCREQ:
+			/* printk("reassocreq"); */
+			wlandev->rx.reassocreq++;
+			break;
+		case WLAN_FSTYPE_REASSOCRESP:
+			/* printk("reassocresp"); */
+			wlandev->rx.reassocresp++;
+			break;
+		case WLAN_FSTYPE_PROBEREQ:
+			/* printk("probereq"); */
+			wlandev->rx.probereq++;
+			break;
+		case WLAN_FSTYPE_PROBERESP:
+			/* printk("proberesp"); */
+			wlandev->rx.proberesp++;
+			break;
+		case WLAN_FSTYPE_BEACON:
+			/* printk("beacon"); */
+			wlandev->rx.beacon++;
+			break;
+		case WLAN_FSTYPE_ATIM:
+			/* printk("atim"); */
+			wlandev->rx.atim++;
+			break;
+		case WLAN_FSTYPE_DISASSOC:
+			/* printk("disassoc"); */
+			wlandev->rx.disassoc++;
+			break;
+		case WLAN_FSTYPE_AUTHEN:
+			/* printk("authen"); */
+			wlandev->rx.authen++;
+			break;
+		case WLAN_FSTYPE_DEAUTHEN:
+			/* printk("deauthen"); */
+			wlandev->rx.deauthen++;
+			break;
+		default:
+			/* printk("unknown"); */
+			wlandev->rx.mgmt_unknown++;
+			break;
+		}
+		/* printk("\n"); */
+		drop = 2;
+		break;
+
+	case WLAN_FTYPE_CTL:
+		if ((wlandev->netdev->flags & IFF_PROMISC) ||
+		    (wlandev->netdev->flags & IFF_ALLMULTI)) {
+			drop = 1;
+			break;
+		}
+		netdev_dbg(wlandev->netdev, "rx'd ctl:\n");
+		wlandev->rx.ctl++;
+		switch (fstype) {
+		case WLAN_FSTYPE_PSPOLL:
+			/* printk("pspoll"); */
+			wlandev->rx.pspoll++;
+			break;
+		case WLAN_FSTYPE_RTS:
+			/* printk("rts"); */
+			wlandev->rx.rts++;
+			break;
+		case WLAN_FSTYPE_CTS:
+			/* printk("cts"); */
+			wlandev->rx.cts++;
+			break;
+		case WLAN_FSTYPE_ACK:
+			/* printk("ack"); */
+			wlandev->rx.ack++;
+			break;
+		case WLAN_FSTYPE_CFEND:
+			/* printk("cfend"); */
+			wlandev->rx.cfend++;
+			break;
+		case WLAN_FSTYPE_CFENDCFACK:
+			/* printk("cfendcfack"); */
+			wlandev->rx.cfendcfack++;
+			break;
+		default:
+			/* printk("unknown"); */
+			wlandev->rx.ctl_unknown++;
+			break;
+		}
+		/* printk("\n"); */
+		drop = 2;
+		break;
+
+	case WLAN_FTYPE_DATA:
+		wlandev->rx.data++;
+		switch (fstype) {
+		case WLAN_FSTYPE_DATAONLY:
+			wlandev->rx.dataonly++;
+			break;
+		case WLAN_FSTYPE_DATA_CFACK:
+			wlandev->rx.data_cfack++;
+			break;
+		case WLAN_FSTYPE_DATA_CFPOLL:
+			wlandev->rx.data_cfpoll++;
+			break;
+		case WLAN_FSTYPE_DATA_CFACK_CFPOLL:
+			wlandev->rx.data__cfack_cfpoll++;
+			break;
+		case WLAN_FSTYPE_NULL:
+			netdev_dbg(wlandev->netdev, "rx'd data:null\n");
+			wlandev->rx.null++;
+			break;
+		case WLAN_FSTYPE_CFACK:
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack\n");
+			wlandev->rx.cfack++;
+			break;
+		case WLAN_FSTYPE_CFPOLL:
+			netdev_dbg(wlandev->netdev, "rx'd data:cfpoll\n");
+			wlandev->rx.cfpoll++;
+			break;
+		case WLAN_FSTYPE_CFACK_CFPOLL:
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack_cfpoll\n");
+			wlandev->rx.cfack_cfpoll++;
+			break;
+		default:
+			/* printk("unknown"); */
+			wlandev->rx.data_unknown++;
+			break;
+		}
+
+		break;
+	}
+	return drop;
+}
+
+static void p80211knetdev_tx_timeout(struct net_device *netdev, unsigned int txqueue)
+{
+	struct wlandevice *wlandev = netdev->ml_priv;
+
+	if (wlandev->tx_timeout) {
+		wlandev->tx_timeout(wlandev);
+	} else {
+		netdev_warn(netdev, "Implement tx_timeout for %s\n",
+			    wlandev->nsdname);
+		netif_wake_queue(wlandev->netdev);
+	}
+}
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
new file mode 100644
index 000000000..1cee51a10
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -0,0 +1,236 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * WLAN net device structure and functions
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file declares the structure type that represents each wlan
+ * interface.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _LINUX_P80211NETDEV_H
+#define _LINUX_P80211NETDEV_H
+
+#include <linux/interrupt.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+
+#define WLAN_RELEASE	"0.3.0-staging"
+
+#define WLAN_DEVICE_CLOSED	0
+#define WLAN_DEVICE_OPEN	1
+
+#define WLAN_MACMODE_NONE	0
+#define WLAN_MACMODE_IBSS_STA	1
+#define WLAN_MACMODE_ESS_STA	2
+#define WLAN_MACMODE_ESS_AP	3
+
+/* MSD States */
+#define WLAN_MSD_HWPRESENT_PENDING	1
+#define WLAN_MSD_HWFAIL			2
+#define WLAN_MSD_HWPRESENT		3
+#define WLAN_MSD_FWLOAD_PENDING		4
+#define WLAN_MSD_FWLOAD			5
+#define WLAN_MSD_RUNNING_PENDING	6
+#define WLAN_MSD_RUNNING		7
+
+#ifndef ETH_P_ECONET
+#define ETH_P_ECONET   0x0018	/* needed for 2.2.x kernels */
+#endif
+
+#define ETH_P_80211_RAW        (ETH_P_ECONET + 1)
+
+#ifndef ARPHRD_IEEE80211
+#define ARPHRD_IEEE80211 801	/* kernel 2.4.6 */
+#endif
+
+#ifndef ARPHRD_IEEE80211_PRISM	/* kernel 2.4.18 */
+#define ARPHRD_IEEE80211_PRISM 802
+#endif
+
+/*--- NSD Capabilities Flags ------------------------------*/
+#define P80211_NSDCAP_HARDWAREWEP           0x01  /* hardware wep engine */
+#define P80211_NSDCAP_SHORT_PREAMBLE        0x10  /* hardware supports */
+#define P80211_NSDCAP_HWFRAGMENT            0x80  /* nsd handles frag/defrag */
+#define P80211_NSDCAP_AUTOJOIN              0x100 /* nsd does autojoin */
+#define P80211_NSDCAP_NOSCAN                0x200 /* nsd can scan */
+
+/* Received frame statistics */
+struct p80211_frmrx {
+	u32 mgmt;
+	u32 assocreq;
+	u32 assocresp;
+	u32 reassocreq;
+	u32 reassocresp;
+	u32 probereq;
+	u32 proberesp;
+	u32 beacon;
+	u32 atim;
+	u32 disassoc;
+	u32 authen;
+	u32 deauthen;
+	u32 mgmt_unknown;
+	u32 ctl;
+	u32 pspoll;
+	u32 rts;
+	u32 cts;
+	u32 ack;
+	u32 cfend;
+	u32 cfendcfack;
+	u32 ctl_unknown;
+	u32 data;
+	u32 dataonly;
+	u32 data_cfack;
+	u32 data_cfpoll;
+	u32 data__cfack_cfpoll;
+	u32 null;
+	u32 cfack;
+	u32 cfpoll;
+	u32 cfack_cfpoll;
+	u32 data_unknown;
+	u32 decrypt;
+	u32 decrypt_err;
+};
+
+/* called by /proc/net/wireless */
+struct iw_statistics *p80211wext_get_wireless_stats(struct net_device *dev);
+
+/* WEP stuff */
+#define NUM_WEPKEYS 4
+#define MAX_KEYLEN 32
+
+#define HOSTWEP_DEFAULTKEY_MASK GENMASK(1, 0)
+#define HOSTWEP_SHAREDKEY BIT(3)
+#define HOSTWEP_DECRYPT  BIT(4)
+#define HOSTWEP_ENCRYPT  BIT(5)
+#define HOSTWEP_PRIVACYINVOKED BIT(6)
+#define HOSTWEP_EXCLUDEUNENCRYPTED BIT(7)
+
+extern int wlan_watchdog;
+extern int wlan_wext_write;
+
+/* WLAN device type */
+struct wlandevice {
+	void *priv;		/* private data for MSD */
+
+	/* Subsystem State */
+	char name[WLAN_DEVNAMELEN_MAX];	/* Dev name, from register_wlandev() */
+	char *nsdname;
+
+	u32 state;		/* Device I/F state (open/closed) */
+	u32 msdstate;		/* state of underlying driver */
+	u32 hwremoved;		/* Has the hw been yanked out? */
+
+	/* Hardware config */
+	unsigned int irq;
+	unsigned int iobase;
+	unsigned int membase;
+	u32 nsdcaps;		/* NSD Capabilities flags */
+
+	/* Config vars */
+	unsigned int ethconv;
+
+	/* device methods (init by MSD, used by p80211 */
+	int (*open)(struct wlandevice *wlandev);
+	int (*close)(struct wlandevice *wlandev);
+	void (*reset)(struct wlandevice *wlandev);
+	int (*txframe)(struct wlandevice *wlandev, struct sk_buff *skb,
+		       struct p80211_hdr *p80211_hdr,
+		       struct p80211_metawep *p80211_wep);
+	int (*mlmerequest)(struct wlandevice *wlandev, struct p80211msg *msg);
+	int (*set_multicast_list)(struct wlandevice *wlandev,
+				  struct net_device *dev);
+	void (*tx_timeout)(struct wlandevice *wlandev);
+
+	/* 802.11 State */
+	u8 bssid[WLAN_BSSID_LEN];
+	struct p80211pstr32 ssid;
+	u32 macmode;
+	int linkstatus;
+
+	/* WEP State */
+	u8 wep_keys[NUM_WEPKEYS][MAX_KEYLEN];
+	u8 wep_keylens[NUM_WEPKEYS];
+	int hostwep;
+
+	/* Request/Confirm i/f state (used by p80211) */
+	unsigned long request_pending;	/* flag, access atomically */
+
+	/* netlink socket */
+	/* queue for indications waiting for cmd completion */
+	/* Linux netdevice and support */
+	struct net_device *netdev;	/* ptr to linux netdevice */
+
+	/* Rx bottom half */
+	struct tasklet_struct rx_bh;
+
+	struct sk_buff_head nsd_rxq;
+
+	/* 802.11 device statistics */
+	struct p80211_frmrx rx;
+
+	struct iw_statistics wstats;
+
+	/* jkriegl: iwspy fields */
+	u8 spy_number;
+	char spy_address[IW_MAX_SPY][ETH_ALEN];
+	struct iw_quality spy_stat[IW_MAX_SPY];
+};
+
+/* WEP stuff */
+int wep_change_key(struct wlandevice *wlandev, int keynum, u8 *key, int keylen);
+int wep_decrypt(struct wlandevice *wlandev, u8 *buf, u32 len, int key_override,
+		u8 *iv, u8 *icv);
+int wep_encrypt(struct wlandevice *wlandev, u8 *buf, u8 *dst, u32 len,
+		int keynum, u8 *iv, u8 *icv);
+
+int wlan_setup(struct wlandevice *wlandev, struct device *physdev);
+void wlan_unsetup(struct wlandevice *wlandev);
+int register_wlandev(struct wlandevice *wlandev);
+int unregister_wlandev(struct wlandevice *wlandev);
+void p80211netdev_rx(struct wlandevice *wlandev, struct sk_buff *skb);
+void p80211netdev_hwremoved(struct wlandevice *wlandev);
+#endif
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
new file mode 100644
index 000000000..809cf3d48
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -0,0 +1,244 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Request/Indication/MacMgmt interface handling functions
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file contains the functions, types, and macros to support the
+ * MLME request interface that's implemented via the device ioctls.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <net/sock.h>
+#include <linux/netlink.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211mgmt.h"
+#include "p80211conv.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211ioctl.h"
+#include "p80211metadef.h"
+#include "p80211metastruct.h"
+#include "p80211req.h"
+
+static void p80211req_handlemsg(struct wlandevice *wlandev,
+				struct p80211msg *msg);
+static void p80211req_mibset_mibget(struct wlandevice *wlandev,
+				    struct p80211msg_dot11req_mibget *mib_msg,
+				    int isget);
+
+static void p80211req_handle_action(struct wlandevice *wlandev, u32 *data,
+				    int isget, u32 flag)
+{
+	if (isget) {
+		if (wlandev->hostwep & flag)
+			*data = P80211ENUM_truth_true;
+		else
+			*data = P80211ENUM_truth_false;
+	} else {
+		wlandev->hostwep &= ~flag;
+		if (*data == P80211ENUM_truth_true)
+			wlandev->hostwep |= flag;
+	}
+}
+
+/*----------------------------------------------------------------
+ * p80211req_dorequest
+ *
+ * Handles an MLME request/confirm message.
+ *
+ * Arguments:
+ *	wlandev		WLAN device struct
+ *	msgbuf		Buffer containing a request message
+ *
+ * Returns:
+ *	0 on success, an errno otherwise
+ *
+ * Call context:
+ *	Potentially blocks the caller, so it's a good idea to
+ *	not call this function from an interrupt context.
+ *----------------------------------------------------------------
+ */
+int p80211req_dorequest(struct wlandevice *wlandev, u8 *msgbuf)
+{
+	struct p80211msg *msg = (struct p80211msg *)msgbuf;
+
+	/* Check to make sure the MSD is running */
+	if (!((wlandev->msdstate == WLAN_MSD_HWPRESENT &&
+	       msg->msgcode == DIDMSG_LNXREQ_IFSTATE) ||
+	      wlandev->msdstate == WLAN_MSD_RUNNING ||
+	      wlandev->msdstate == WLAN_MSD_FWLOAD)) {
+		return -ENODEV;
+	}
+
+	/* Check Permissions */
+	if (!capable(CAP_NET_ADMIN) &&
+	    (msg->msgcode != DIDMSG_DOT11REQ_MIBGET)) {
+		netdev_err(wlandev->netdev,
+			   "%s: only dot11req_mibget allowed for non-root.\n",
+			   wlandev->name);
+		return -EPERM;
+	}
+
+	/* Check for busy status */
+	if (test_and_set_bit(1, &wlandev->request_pending))
+		return -EBUSY;
+
+	/* Allow p80211 to look at msg and handle if desired. */
+	/* So far, all p80211 msgs are immediate, no waitq/timer necessary */
+	/* This may change. */
+	p80211req_handlemsg(wlandev, msg);
+
+	/* Pass it down to wlandev via wlandev->mlmerequest */
+	if (wlandev->mlmerequest)
+		wlandev->mlmerequest(wlandev, msg);
+
+	clear_bit(1, &wlandev->request_pending);
+	return 0;	/* if result==0, msg->status still may contain an err */
+}
+
+/*----------------------------------------------------------------
+ * p80211req_handlemsg
+ *
+ * p80211 message handler.  Primarily looks for messages that
+ * belong to p80211 and then dispatches the appropriate response.
+ * TODO: we don't do anything yet.  Once the linuxMIB is better
+ *	defined we'll need a get/set handler.
+ *
+ * Arguments:
+ *	wlandev		WLAN device struct
+ *	msg		message structure
+ *
+ * Returns:
+ *	nothing (any results are set in the status field of the msg)
+ *
+ * Call context:
+ *	Process thread
+ *----------------------------------------------------------------
+ */
+static void p80211req_handlemsg(struct wlandevice *wlandev,
+				struct p80211msg *msg)
+{
+	switch (msg->msgcode) {
+	case DIDMSG_LNXREQ_HOSTWEP: {
+		struct p80211msg_lnxreq_hostwep *req =
+			(struct p80211msg_lnxreq_hostwep *)msg;
+		wlandev->hostwep &=
+				~(HOSTWEP_DECRYPT | HOSTWEP_ENCRYPT);
+		if (req->decrypt.data == P80211ENUM_truth_true)
+			wlandev->hostwep |= HOSTWEP_DECRYPT;
+		if (req->encrypt.data == P80211ENUM_truth_true)
+			wlandev->hostwep |= HOSTWEP_ENCRYPT;
+
+		break;
+	}
+	case DIDMSG_DOT11REQ_MIBGET:
+	case DIDMSG_DOT11REQ_MIBSET: {
+		int isget = (msg->msgcode == DIDMSG_DOT11REQ_MIBGET);
+		struct p80211msg_dot11req_mibget *mib_msg =
+			(struct p80211msg_dot11req_mibget *)msg;
+		p80211req_mibset_mibget(wlandev, mib_msg, isget);
+		break;
+	}
+	}			/* switch msg->msgcode */
+}
+
+static void p80211req_mibset_mibget(struct wlandevice *wlandev,
+				    struct p80211msg_dot11req_mibget *mib_msg,
+				    int isget)
+{
+	struct p80211itemd *mibitem =
+		(struct p80211itemd *)mib_msg->mibattribute.data;
+	struct p80211pstrd *pstr = (struct p80211pstrd *)mibitem->data;
+	u8 *key = mibitem->data + sizeof(struct p80211pstrd);
+
+	switch (mibitem->did) {
+	case didmib_dot11smt_wepdefaultkeystable_key(1):
+	case didmib_dot11smt_wepdefaultkeystable_key(2):
+	case didmib_dot11smt_wepdefaultkeystable_key(3):
+	case didmib_dot11smt_wepdefaultkeystable_key(4):
+		if (!isget)
+			wep_change_key(wlandev,
+				       P80211DID_ITEM(mibitem->did) - 1,
+				       key, pstr->len);
+		break;
+
+	case DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID: {
+		u32 *data = (u32 *)mibitem->data;
+
+		if (isget) {
+			*data = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
+		} else {
+			wlandev->hostwep &= ~(HOSTWEP_DEFAULTKEY_MASK);
+			wlandev->hostwep |= (*data & HOSTWEP_DEFAULTKEY_MASK);
+		}
+		break;
+	}
+	case DIDMIB_DOT11SMT_PRIVACYTABLE_PRIVACYINVOKED: {
+		u32 *data = (u32 *)mibitem->data;
+
+		p80211req_handle_action(wlandev, data, isget,
+					HOSTWEP_PRIVACYINVOKED);
+		break;
+	}
+	case DIDMIB_DOT11SMT_PRIVACYTABLE_EXCLUDEUNENCRYPTED: {
+		u32 *data = (u32 *)mibitem->data;
+
+		p80211req_handle_action(wlandev, data, isget,
+					HOSTWEP_EXCLUDEUNENCRYPTED);
+		break;
+	}
+	}
+}
diff --git a/drivers/staging/wlan-ng/p80211req.h b/drivers/staging/wlan-ng/p80211req.h
new file mode 100644
index 000000000..bc45cd5f9
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211req.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Request handling functions
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#ifndef _LINUX_P80211REQ_H
+#define _LINUX_P80211REQ_H
+
+int p80211req_dorequest(struct wlandevice *wlandev, u8 *msgbuf);
+
+#endif
diff --git a/drivers/staging/wlan-ng/p80211types.h b/drivers/staging/wlan-ng/p80211types.h
new file mode 100644
index 000000000..6486612a8
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211types.h
@@ -0,0 +1,319 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ *
+ * Macros, constants, types, and funcs for p80211 data types
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file declares some of the constants and types used in various
+ * parts of the linux-wlan system.
+ *
+ * Notes:
+ *   - Constant values are always in HOST byte order.
+ *
+ * All functions and statics declared here are implemented in p80211types.c
+ *   --------------------------------------------------------------------
+ */
+
+#ifndef _P80211TYPES_H
+#define _P80211TYPES_H
+
+/*----------------------------------------------------------------*/
+/* The following constants are indexes into the Mib Category List */
+/* and the Message Category List */
+
+/* Mib Category List */
+#define P80211_MIB_CAT_DOT11SMT		1
+#define P80211_MIB_CAT_DOT11MAC		2
+#define P80211_MIB_CAT_DOT11PHY		3
+
+#define P80211SEC_DOT11SMT		P80211_MIB_CAT_DOT11SMT
+#define P80211SEC_DOT11MAC		P80211_MIB_CAT_DOT11MAC
+#define P80211SEC_DOT11PHY		P80211_MIB_CAT_DOT11PHY
+
+/* Message Category List */
+#define P80211_MSG_CAT_DOT11REQ		1
+#define P80211_MSG_CAT_DOT11IND		2
+
+/*----------------------------------------------------------------*/
+/* p80211 enumeration constants.  The value to text mappings for */
+/*  these is in p80211types.c.  These defines were generated */
+/*  from the mappings. */
+
+/* error codes for lookups */
+
+#define P80211ENUM_truth_false			0
+#define P80211ENUM_truth_true			1
+#define P80211ENUM_ifstate_disable		0
+#define P80211ENUM_ifstate_fwload		1
+#define P80211ENUM_ifstate_enable		2
+#define P80211ENUM_bsstype_infrastructure	1
+#define P80211ENUM_bsstype_independent		2
+#define P80211ENUM_bsstype_any			3
+#define P80211ENUM_authalg_opensystem		1
+#define P80211ENUM_authalg_sharedkey		2
+#define P80211ENUM_scantype_active		1
+#define P80211ENUM_resultcode_success		1
+#define P80211ENUM_resultcode_invalid_parameters	2
+#define P80211ENUM_resultcode_not_supported	3
+#define P80211ENUM_resultcode_refused		6
+#define P80211ENUM_resultcode_cant_set_readonly_mib	10
+#define P80211ENUM_resultcode_implementation_failure	11
+#define P80211ENUM_resultcode_cant_get_writeonly_mib	12
+#define P80211ENUM_status_successful		0
+#define P80211ENUM_status_unspec_failure	1
+#define P80211ENUM_status_ap_full		17
+#define P80211ENUM_msgitem_status_data_ok		0
+#define P80211ENUM_msgitem_status_no_value		1
+
+/*----------------------------------------------------------------*/
+/* p80211 max length constants for the different pascal strings. */
+
+#define MAXLEN_PSTR6		(6)	/* pascal array of 6 bytes */
+#define MAXLEN_PSTR14		(14)	/* pascal array of 14 bytes */
+#define MAXLEN_PSTR32		(32)	/* pascal array of 32 bytes */
+#define MAXLEN_PSTR255		(255)	/* pascal array of 255 bytes */
+#define MAXLEN_MIBATTRIBUTE	(392)	/* maximum mibattribute */
+					/* where the size of the DATA itself */
+					/* is a DID-LEN-DATA triple */
+					/* with a max size of 4+4+384 */
+
+/*----------------------------------------------------------------
+ * The following constants and macros are used to construct and
+ * deconstruct the Data ID codes.  The coding is as follows:
+ *
+ *     ...rwtnnnnnnnniiiiiiggggggssssss      s - Section
+ *                                           g - Group
+ *                                           i - Item
+ *                                           n - Index
+ *                                           t - Table flag
+ *                                           w - Write flag
+ *                                           r - Read flag
+ *                                           . - Unused
+ */
+
+#define P80211DID_LSB_SECTION		(0)
+#define P80211DID_LSB_GROUP		(6)
+#define P80211DID_LSB_ITEM		(12)
+#define P80211DID_LSB_INDEX		(18)
+#define P80211DID_LSB_ISTABLE		(26)
+#define P80211DID_LSB_ACCESS		(27)
+
+#define P80211DID_MASK_SECTION		(0x0000003fUL)
+#define P80211DID_MASK_GROUP		(0x0000003fUL)
+#define P80211DID_MASK_ITEM		(0x0000003fUL)
+#define P80211DID_MASK_INDEX		(0x000000ffUL)
+#define P80211DID_MASK_ISTABLE		(0x00000001UL)
+#define P80211DID_MASK_ACCESS		(0x00000003UL)
+
+#define P80211DID_MK(a, m, l)	((((u32)(a)) & (m)) << (l))
+
+#define P80211DID_MKSECTION(a)	P80211DID_MK(a, \
+					P80211DID_MASK_SECTION, \
+					P80211DID_LSB_SECTION)
+#define P80211DID_MKGROUP(a)	P80211DID_MK(a, \
+					P80211DID_MASK_GROUP, \
+					P80211DID_LSB_GROUP)
+#define P80211DID_MKITEM(a)	P80211DID_MK(a, \
+					P80211DID_MASK_ITEM, \
+					P80211DID_LSB_ITEM)
+#define P80211DID_MKINDEX(a)	P80211DID_MK(a, \
+					P80211DID_MASK_INDEX, \
+					P80211DID_LSB_INDEX)
+#define P80211DID_MKISTABLE(a)	P80211DID_MK(a, \
+					P80211DID_MASK_ISTABLE, \
+					P80211DID_LSB_ISTABLE)
+
+#define P80211DID_MKID(s, g, i, n, t, a)	(P80211DID_MKSECTION(s) | \
+					P80211DID_MKGROUP(g) | \
+					P80211DID_MKITEM(i) | \
+					P80211DID_MKINDEX(n) | \
+					P80211DID_MKISTABLE(t) | \
+					(a))
+
+#define P80211DID_GET(a, m, l)	((((u32)(a)) >> (l)) & (m))
+
+#define P80211DID_SECTION(a)	P80211DID_GET(a, \
+					P80211DID_MASK_SECTION, \
+					P80211DID_LSB_SECTION)
+#define P80211DID_GROUP(a)	P80211DID_GET(a, \
+					P80211DID_MASK_GROUP, \
+					P80211DID_LSB_GROUP)
+#define P80211DID_ITEM(a)	P80211DID_GET(a, \
+					P80211DID_MASK_ITEM, \
+					P80211DID_LSB_ITEM)
+#define P80211DID_INDEX(a)	P80211DID_GET(a, \
+					P80211DID_MASK_INDEX, \
+					P80211DID_LSB_INDEX)
+#define P80211DID_ISTABLE(a)	P80211DID_GET(a, \
+					P80211DID_MASK_ISTABLE, \
+					P80211DID_LSB_ISTABLE)
+#define P80211DID_ACCESS(a)	P80211DID_GET(a, \
+					P80211DID_MASK_ACCESS, \
+					P80211DID_LSB_ACCESS)
+
+/*----------------------------------------------------------------*/
+/* The following structure types are used to store data items in */
+/*  messages. */
+
+/* Template pascal string */
+struct p80211pstr {
+	u8 len;
+} __packed;
+
+struct p80211pstrd {
+	u8 len;
+	u8 data[];
+} __packed;
+
+/* Maximum pascal string */
+struct p80211pstr255 {
+	u8 len;
+	u8 data[MAXLEN_PSTR255];
+} __packed;
+
+/* pascal string for macaddress and bssid */
+struct p80211pstr6 {
+	u8 len;
+	u8 data[MAXLEN_PSTR6];
+} __packed;
+
+/* pascal string for channel list */
+struct p80211pstr14 {
+	u8 len;
+	u8 data[MAXLEN_PSTR14];
+} __packed;
+
+/* pascal string for ssid */
+struct p80211pstr32 {
+	u8 len;
+	u8 data[MAXLEN_PSTR32];
+} __packed;
+
+/* MAC address array */
+struct p80211macarray {
+	u32 cnt;
+	u8 data[1][MAXLEN_PSTR6];
+} __packed;
+
+/* prototype template */
+struct p80211item {
+	u32 did;
+	u16 status;
+	u16 len;
+} __packed;
+
+/* prototype template w/ data item */
+struct p80211itemd {
+	u32 did;
+	u16 status;
+	u16 len;
+	u8 data[];
+} __packed;
+
+/* message data item for int, BOUNDEDINT, ENUMINT */
+struct p80211item_uint32 {
+	u32 did;
+	u16 status;
+	u16 len;
+	u32 data;
+} __packed;
+
+/* message data item for OCTETSTR, DISPLAYSTR */
+struct p80211item_pstr6 {
+	u32 did;
+	u16 status;
+	u16 len;
+	struct p80211pstr6 data;
+} __packed;
+
+/* message data item for OCTETSTR, DISPLAYSTR */
+struct p80211item_pstr14 {
+	u32 did;
+	u16 status;
+	u16 len;
+	struct p80211pstr14 data;
+} __packed;
+
+/* message data item for OCTETSTR, DISPLAYSTR */
+struct p80211item_pstr32 {
+	u32 did;
+	u16 status;
+	u16 len;
+	struct p80211pstr32 data;
+} __packed;
+
+/* message data item for OCTETSTR, DISPLAYSTR */
+struct p80211item_pstr255 {
+	u32 did;
+	u16 status;
+	u16 len;
+	struct p80211pstr255 data;
+} __packed;
+
+/* message data item for UNK 392, namely mib items */
+struct p80211item_unk392 {
+	u32 did;
+	u16 status;
+	u16 len;
+	u8 data[MAXLEN_MIBATTRIBUTE];
+} __packed;
+
+/* message data item for UNK 1025, namely p2 pdas */
+struct p80211item_unk1024 {
+	u32 did;
+	u16 status;
+	u16 len;
+	u8 data[1024];
+} __packed;
+
+/* message data item for UNK 4096, namely p2 download chunks */
+struct p80211item_unk4096 {
+	u32 did;
+	u16 status;
+	u16 len;
+	u8 data[4096];
+} __packed;
+
+#endif /* _P80211TYPES_H */
diff --git a/drivers/staging/wlan-ng/p80211wep.c b/drivers/staging/wlan-ng/p80211wep.c
new file mode 100644
index 000000000..3ff7ee701
--- /dev/null
+++ b/drivers/staging/wlan-ng/p80211wep.c
@@ -0,0 +1,228 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * WEP encode/decode for P80211.
+ *
+ * Copyright (C) 2002 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+/*================================================================*/
+/* System Includes */
+
+#include <linux/crc32.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/random.h>
+#include <linux/kernel.h>
+#include "p80211hdr.h"
+#include "p80211types.h"
+#include "p80211msg.h"
+#include "p80211conv.h"
+#include "p80211netdev.h"
+
+#define WEP_KEY(x)       (((x) & 0xC0) >> 6)
+
+/* keylen in bytes! */
+
+int wep_change_key(struct wlandevice *wlandev, int keynum, u8 *key, int keylen)
+{
+	if (keylen < 0)
+		return -1;
+	if (keylen >= MAX_KEYLEN)
+		return -1;
+	if (!key)
+		return -1;
+	if (keynum < 0)
+		return -1;
+	if (keynum >= NUM_WEPKEYS)
+		return -1;
+
+	wlandev->wep_keylens[keynum] = keylen;
+	memcpy(wlandev->wep_keys[keynum], key, keylen);
+
+	return 0;
+}
+
+/*
+ * 4-byte IV at start of buffer, 4-byte ICV at end of buffer.
+ * if successful, buf start is payload begin, length -= 8;
+ */
+int wep_decrypt(struct wlandevice *wlandev, u8 *buf, u32 len, int key_override,
+		u8 *iv, u8 *icv)
+{
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64], c_crc[4];
+	u8 keyidx;
+
+	/* Needs to be at least 8 bytes of payload */
+	if (len <= 0)
+		return -1;
+
+	/* initialize the first bytes of the key from the IV */
+	key[0] = iv[0];
+	key[1] = iv[1];
+	key[2] = iv[2];
+	keyidx = WEP_KEY(iv[3]);
+
+	if (key_override >= 0)
+		keyidx = key_override;
+
+	if (keyidx >= NUM_WEPKEYS)
+		return -2;
+
+	keylen = wlandev->wep_keylens[keyidx];
+
+	if (keylen == 0)
+		return -3;
+
+	/* copy the rest of the key over from the designated key */
+	memcpy(key + 3, wlandev->wep_keys[keyidx], keylen);
+
+	keylen += 3;		/* add in IV bytes */
+
+	/* set up the RC4 state */
+	for (i = 0; i < 256; i++)
+		s[i] = i;
+	j = 0;
+	for (i = 0; i < 256; i++) {
+		j = (j + s[i] + key[i % keylen]) & 0xff;
+		swap(i, j);
+	}
+
+	/* Apply the RC4 to the data, update the CRC32 */
+	i = 0;
+	j = 0;
+	for (k = 0; k < len; k++) {
+		i = (i + 1) & 0xff;
+		j = (j + s[i]) & 0xff;
+		swap(i, j);
+		buf[k] ^= s[(s[i] + s[j]) & 0xff];
+	}
+	crc = ~crc32_le(~0, buf, len);
+
+	/* now let's check the crc */
+	c_crc[0] = crc;
+	c_crc[1] = crc >> 8;
+	c_crc[2] = crc >> 16;
+	c_crc[3] = crc >> 24;
+
+	for (k = 0; k < 4; k++) {
+		i = (i + 1) & 0xff;
+		j = (j + s[i]) & 0xff;
+		swap(i, j);
+		if ((c_crc[k] ^ s[(s[i] + s[j]) & 0xff]) != icv[k])
+			return -(4 | (k << 4));	/* ICV mismatch */
+	}
+
+	return 0;
+}
+
+/* encrypts in-place. */
+int wep_encrypt(struct wlandevice *wlandev, u8 *buf,
+		u8 *dst, u32 len, int keynum, u8 *iv, u8 *icv)
+{
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64];
+
+	/* no point in WEPping an empty frame */
+	if (len <= 0)
+		return -1;
+
+	/* we need to have a real key.. */
+	if (keynum >= NUM_WEPKEYS)
+		return -2;
+	keylen = wlandev->wep_keylens[keynum];
+	if (keylen <= 0)
+		return -3;
+
+	/* use a random IV.  And skip known weak ones. */
+	get_random_bytes(iv, 3);
+	while ((iv[1] == 0xff) && (iv[0] >= 3) && (iv[0] < keylen))
+		get_random_bytes(iv, 3);
+
+	iv[3] = (keynum & 0x03) << 6;
+
+	key[0] = iv[0];
+	key[1] = iv[1];
+	key[2] = iv[2];
+
+	/* copy the rest of the key over from the designated key */
+	memcpy(key + 3, wlandev->wep_keys[keynum], keylen);
+
+	keylen += 3;		/* add in IV bytes */
+
+	/* set up the RC4 state */
+	for (i = 0; i < 256; i++)
+		s[i] = i;
+	j = 0;
+	for (i = 0; i < 256; i++) {
+		j = (j + s[i] + key[i % keylen]) & 0xff;
+		swap(i, j);
+	}
+
+	/* Update CRC32 then apply RC4 to the data */
+	i = 0;
+	j = 0;
+	for (k = 0; k < len; k++) {
+		i = (i + 1) & 0xff;
+		j = (j + s[i]) & 0xff;
+		swap(i, j);
+		dst[k] = buf[k] ^ s[(s[i] + s[j]) & 0xff];
+	}
+	crc = ~crc32_le(~0, buf, len);
+
+	/* now let's encrypt the crc */
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+
+	for (k = 0; k < 4; k++) {
+		i = (i + 1) & 0xff;
+		j = (j + s[i]) & 0xff;
+		swap(i, j);
+		icv[k] ^= s[(s[i] + s[j]) & 0xff];
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
new file mode 100644
index 000000000..02a2191d5
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -0,0 +1,1236 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/* from src/prism2/download/prism2dl.c
+ *
+ * utility for downloading prism2 images moved into kernelspace
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ */
+
+/*================================================================*/
+/* System Includes */
+#include <linux/ihex.h>
+#include <linux/slab.h>
+
+/*================================================================*/
+/* Local Constants */
+
+#define PRISM2_USB_FWFILE	"prism2_ru.fw"
+MODULE_FIRMWARE(PRISM2_USB_FWFILE);
+
+#define S3DATA_MAX		5000
+#define S3PLUG_MAX		200
+#define S3CRC_MAX		200
+#define S3INFO_MAX		50
+
+#define S3ADDR_PLUG		(0xff000000UL)
+#define S3ADDR_CRC		(0xff100000UL)
+#define S3ADDR_INFO		(0xff200000UL)
+#define S3ADDR_START		(0xff400000UL)
+
+#define CHUNKS_MAX		100
+
+#define WRITESIZE_MAX		4096
+
+/*================================================================*/
+/* Local Types */
+
+struct s3datarec {
+	u32 len;
+	u32 addr;
+	u8 checksum;
+	u8 *data;
+};
+
+struct s3plugrec {
+	u32 itemcode;
+	u32 addr;
+	u32 len;
+};
+
+struct s3crcrec {
+	u32 addr;
+	u32 len;
+	unsigned int dowrite;
+};
+
+struct s3inforec {
+	u16 len;
+	u16 type;
+	union {
+		struct hfa384x_compident version;
+		struct hfa384x_caplevel compat;
+		u16 buildseq;
+		struct hfa384x_compident platform;
+	} info;
+};
+
+struct pda {
+	u8 buf[HFA384x_PDA_LEN_MAX];
+	struct hfa384x_pdrec *rec[HFA384x_PDA_RECS_MAX];
+	unsigned int nrec;
+};
+
+struct imgchunk {
+	u32 addr;	/* start address */
+	u32 len;	/* in bytes */
+	u16 crc;	/* CRC value (if it falls at a chunk boundary) */
+	u8 *data;
+};
+
+/*================================================================*/
+/* Local Static Definitions */
+
+/*----------------------------------------------------------------*/
+/* s-record image processing */
+
+/* Data records */
+static unsigned int ns3data;
+static struct s3datarec *s3data;
+
+/* Plug records */
+static unsigned int ns3plug;
+static struct s3plugrec s3plug[S3PLUG_MAX];
+
+/* CRC records */
+static unsigned int ns3crc;
+static struct s3crcrec s3crc[S3CRC_MAX];
+
+/* Info records */
+static unsigned int ns3info;
+static struct s3inforec s3info[S3INFO_MAX];
+
+/* S7 record (there _better_ be only one) */
+static u32 startaddr;
+
+/* Load image chunks */
+static unsigned int nfchunks;
+static struct imgchunk fchunk[CHUNKS_MAX];
+
+/* Note that for the following pdrec_t arrays, the len and code */
+/*   fields are stored in HOST byte order. The mkpdrlist() function */
+/*   does the conversion.  */
+/*----------------------------------------------------------------*/
+/* PDA, built from [card|newfile]+[addfile1+addfile2...] */
+
+static struct pda pda;
+static struct hfa384x_compident nicid;
+static struct hfa384x_caplevel rfid;
+static struct hfa384x_caplevel macid;
+static struct hfa384x_caplevel priid;
+
+/*================================================================*/
+/* Local Function Declarations */
+
+static int prism2_fwapply(const struct ihex_binrec *rfptr,
+			  struct wlandevice *wlandev);
+
+static int read_fwfile(const struct ihex_binrec *rfptr);
+
+static int mkimage(struct imgchunk *clist, unsigned int *ccnt);
+
+static int read_cardpda(struct pda *pda, struct wlandevice *wlandev);
+
+static int mkpdrlist(struct pda *pda);
+
+static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
+		     struct s3plugrec *s3plug, unsigned int ns3plug,
+		     struct pda *pda);
+
+static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
+		    struct s3crcrec *s3crc, unsigned int ns3crc);
+
+static int writeimage(struct wlandevice *wlandev, struct imgchunk *fchunk,
+		      unsigned int nfchunks);
+
+static void free_chunks(struct imgchunk *fchunk, unsigned int *nfchunks);
+
+static void free_srecs(void);
+
+static int validate_identity(void);
+
+/*================================================================*/
+/* Function Definitions */
+
+/*----------------------------------------------------------------
+ * prism2_fwtry
+ *
+ * Try and get firmware into memory
+ *
+ * Arguments:
+ *	udev	usb device structure
+ *	wlandev wlan device structure
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure
+ *----------------------------------------------------------------
+ */
+static int prism2_fwtry(struct usb_device *udev, struct wlandevice *wlandev)
+{
+	const struct firmware *fw_entry = NULL;
+
+	netdev_info(wlandev->netdev, "prism2_usb: Checking for firmware %s\n",
+		    PRISM2_USB_FWFILE);
+	if (request_ihex_firmware(&fw_entry,
+				  PRISM2_USB_FWFILE, &udev->dev) != 0) {
+		netdev_info(wlandev->netdev,
+			    "prism2_usb: Firmware not available, but not essential\n");
+		netdev_info(wlandev->netdev,
+			    "prism2_usb: can continue to use card anyway.\n");
+		return 1;
+	}
+
+	netdev_info(wlandev->netdev,
+		    "prism2_usb: %s will be processed, size %zu\n",
+		    PRISM2_USB_FWFILE, fw_entry->size);
+	prism2_fwapply((const struct ihex_binrec *)fw_entry->data, wlandev);
+
+	release_firmware(fw_entry);
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2_fwapply
+ *
+ * Apply the firmware loaded into memory
+ *
+ * Arguments:
+ *	rfptr	firmware image in kernel memory
+ *	wlandev device
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure
+ *----------------------------------------------------------------
+ */
+static int prism2_fwapply(const struct ihex_binrec *rfptr,
+			  struct wlandevice *wlandev)
+{
+	signed int result = 0;
+	struct p80211msg_dot11req_mibget getmsg;
+	struct p80211itemd *item;
+	u32 *data;
+
+	/* Initialize the data structures */
+	ns3data = 0;
+	s3data = kcalloc(S3DATA_MAX, sizeof(*s3data), GFP_KERNEL);
+	if (!s3data) {
+		result = -ENOMEM;
+		goto out;
+	}
+
+	ns3plug = 0;
+	memset(s3plug, 0, sizeof(s3plug));
+	ns3crc = 0;
+	memset(s3crc, 0, sizeof(s3crc));
+	ns3info = 0;
+	memset(s3info, 0, sizeof(s3info));
+	startaddr = 0;
+
+	nfchunks = 0;
+	memset(fchunk, 0, sizeof(fchunk));
+	memset(&nicid, 0, sizeof(nicid));
+	memset(&rfid, 0, sizeof(rfid));
+	memset(&macid, 0, sizeof(macid));
+	memset(&priid, 0, sizeof(priid));
+
+	/* clear the pda and add an initial END record */
+	memset(&pda, 0, sizeof(pda));
+	pda.rec[0] = (struct hfa384x_pdrec *)pda.buf;
+	pda.rec[0]->len = cpu_to_le16(2);	/* len in words */
+	pda.rec[0]->code = cpu_to_le16(HFA384x_PDR_END_OF_PDA);
+	pda.nrec = 1;
+
+	/*-----------------------------------------------------*/
+	/* Put card into fwload state */
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_fwload);
+
+	/* Build the PDA we're going to use. */
+	if (read_cardpda(&pda, wlandev)) {
+		netdev_err(wlandev->netdev, "load_cardpda failed, exiting.\n");
+		result = 1;
+		goto out;
+	}
+
+	/* read the card's PRI-SUP */
+	memset(&getmsg, 0, sizeof(getmsg));
+	getmsg.msgcode = DIDMSG_DOT11REQ_MIBGET;
+	getmsg.msglen = sizeof(getmsg);
+	strscpy(getmsg.devname, wlandev->name, sizeof(getmsg.devname));
+
+	getmsg.mibattribute.did = DIDMSG_DOT11REQ_MIBGET_MIBATTRIBUTE;
+	getmsg.mibattribute.status = P80211ENUM_msgitem_status_data_ok;
+	getmsg.resultcode.did = DIDMSG_DOT11REQ_MIBGET_RESULTCODE;
+	getmsg.resultcode.status = P80211ENUM_msgitem_status_no_value;
+
+	item = (struct p80211itemd *)getmsg.mibattribute.data;
+	item->did = DIDMIB_P2_NIC_PRISUPRANGE;
+	item->status = P80211ENUM_msgitem_status_no_value;
+
+	data = (u32 *)item->data;
+
+	/* DIDmsg_dot11req_mibget */
+	prism2mgmt_mibset_mibget(wlandev, &getmsg);
+	if (getmsg.resultcode.data != P80211ENUM_resultcode_success)
+		netdev_err(wlandev->netdev, "Couldn't fetch PRI-SUP info\n");
+
+	/* Already in host order */
+	priid.role = *data++;
+	priid.id = *data++;
+	priid.variant = *data++;
+	priid.bottom = *data++;
+	priid.top = *data++;
+
+	/* Read the S3 file */
+	result = read_fwfile(rfptr);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Failed to read the data exiting.\n");
+		goto out;
+	}
+
+	result = validate_identity();
+	if (result) {
+		netdev_err(wlandev->netdev, "Incompatible firmware image.\n");
+		goto out;
+	}
+
+	if (startaddr == 0x00000000) {
+		netdev_err(wlandev->netdev,
+			   "Can't RAM download a Flash image!\n");
+		result = 1;
+		goto out;
+	}
+
+	/* Make the image chunks */
+	result = mkimage(fchunk, &nfchunks);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to make image chunk.\n");
+		goto free_chunks;
+	}
+
+	/* Do any plugging */
+	result = plugimage(fchunk, nfchunks, s3plug, ns3plug, &pda);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to plug data.\n");
+		goto free_chunks;
+	}
+
+	/* Insert any CRCs */
+	result = crcimage(fchunk, nfchunks, s3crc, ns3crc);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to insert all CRCs\n");
+		goto free_chunks;
+	}
+
+	/* Write the image */
+	result = writeimage(wlandev, fchunk, nfchunks);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to ramwrite image data.\n");
+		goto free_chunks;
+	}
+
+	netdev_info(wlandev->netdev, "prism2_usb: firmware loading finished.\n");
+
+free_chunks:
+	/* clear any allocated memory */
+	free_chunks(fchunk, &nfchunks);
+	free_srecs();
+
+out:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * crcimage
+ *
+ * Adds a CRC16 in the two bytes prior to each block identified by
+ * an S3 CRC record.  Currently, we don't actually do a CRC we just
+ * insert the value 0xC0DE in hfa384x order.
+ *
+ * Arguments:
+ *	fchunk		Array of image chunks
+ *	nfchunks	Number of image chunks
+ *	s3crc		Array of crc records
+ *	ns3crc		Number of crc records
+ *
+ * Returns:
+ *	0	success
+ *	~0	failure
+ *----------------------------------------------------------------
+ */
+static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
+		    struct s3crcrec *s3crc, unsigned int ns3crc)
+{
+	int result = 0;
+	int i;
+	int c;
+	u32 crcstart;
+	u32 cstart = 0;
+	u32 cend;
+	u8 *dest;
+	u32 chunkoff;
+
+	for (i = 0; i < ns3crc; i++) {
+		if (!s3crc[i].dowrite)
+			continue;
+		crcstart = s3crc[i].addr;
+		/* Find chunk */
+		for (c = 0; c < nfchunks; c++) {
+			cstart = fchunk[c].addr;
+			cend = fchunk[c].addr + fchunk[c].len;
+			/* the line below does an address & len match search */
+			/* unfortunately, I've found that the len fields of */
+			/* some crc records don't match with the length of */
+			/* the actual data, so we're not checking right now */
+			/* if (crcstart-2 >= cstart && crcend <= cend) break; */
+
+			/* note the -2 below, it's to make sure the chunk has */
+			/* space for the CRC value */
+			if (crcstart - 2 >= cstart && crcstart < cend)
+				break;
+		}
+		if (c >= nfchunks) {
+			pr_err("Failed to find chunk for crcrec[%d], addr=0x%06x len=%d , aborting crc.\n",
+			       i, s3crc[i].addr, s3crc[i].len);
+			return 1;
+		}
+
+		/* Insert crc */
+		pr_debug("Adding crc @ 0x%06x\n", s3crc[i].addr - 2);
+		chunkoff = crcstart - cstart - 2;
+		dest = fchunk[c].data + chunkoff;
+		*dest = 0xde;
+		*(dest + 1) = 0xc0;
+	}
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * free_chunks
+ *
+ * Clears the chunklist data structures in preparation for a new file.
+ *
+ * Arguments:
+ *	none
+ *
+ * Returns:
+ *	nothing
+ *----------------------------------------------------------------
+ */
+static void free_chunks(struct imgchunk *fchunk, unsigned int *nfchunks)
+{
+	int i;
+
+	for (i = 0; i < *nfchunks; i++)
+		kfree(fchunk[i].data);
+
+	*nfchunks = 0;
+	memset(fchunk, 0, sizeof(*fchunk));
+}
+
+/*----------------------------------------------------------------
+ * free_srecs
+ *
+ * Clears the srec data structures in preparation for a new file.
+ *
+ * Arguments:
+ *	none
+ *
+ * Returns:
+ *	nothing
+ *----------------------------------------------------------------
+ */
+static void free_srecs(void)
+{
+	ns3data = 0;
+	kfree(s3data);
+	ns3plug = 0;
+	memset(s3plug, 0, sizeof(s3plug));
+	ns3crc = 0;
+	memset(s3crc, 0, sizeof(s3crc));
+	ns3info = 0;
+	memset(s3info, 0, sizeof(s3info));
+	startaddr = 0;
+}
+
+/*----------------------------------------------------------------
+ * mkimage
+ *
+ * Scans the currently loaded set of S records for data residing
+ * in contiguous memory regions.  Each contiguous region is then
+ * made into a 'chunk'.  This function assumes that we're building
+ * a new chunk list.  Assumes the s3data items are in sorted order.
+ *
+ * Arguments:	none
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure (probably an errno)
+ *----------------------------------------------------------------
+ */
+static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
+{
+	int result = 0;
+	int i;
+	int j;
+	int currchunk = 0;
+	u32 nextaddr = 0;
+	u32 s3start;
+	u32 s3end;
+	u32 cstart = 0;
+	u32 cend;
+	u32 coffset;
+
+	/* There may already be data in the chunklist */
+	*ccnt = 0;
+
+	/* Establish the location and size of each chunk */
+	for (i = 0; i < ns3data; i++) {
+		if (s3data[i].addr == nextaddr) {
+			/* existing chunk, grow it */
+			clist[currchunk].len += s3data[i].len;
+			nextaddr += s3data[i].len;
+		} else {
+			/* New chunk */
+			(*ccnt)++;
+			currchunk = *ccnt - 1;
+			clist[currchunk].addr = s3data[i].addr;
+			clist[currchunk].len = s3data[i].len;
+			nextaddr = s3data[i].addr + s3data[i].len;
+			/* Expand the chunk if there is a CRC record at */
+			/* their beginning bound */
+			for (j = 0; j < ns3crc; j++) {
+				if (s3crc[j].dowrite &&
+				    s3crc[j].addr == clist[currchunk].addr) {
+					clist[currchunk].addr -= 2;
+					clist[currchunk].len += 2;
+				}
+			}
+		}
+	}
+
+	/* We're currently assuming there aren't any overlapping chunks */
+	/*  if this proves false, we'll need to add code to coalesce. */
+
+	/* Allocate buffer space for chunks */
+	for (i = 0; i < *ccnt; i++) {
+		clist[i].data = kzalloc(clist[i].len, GFP_KERNEL);
+		if (!clist[i].data)
+			return 1;
+
+		pr_debug("chunk[%d]: addr=0x%06x len=%d\n",
+			 i, clist[i].addr, clist[i].len);
+	}
+
+	/* Copy srec data to chunks */
+	for (i = 0; i < ns3data; i++) {
+		s3start = s3data[i].addr;
+		s3end = s3start + s3data[i].len - 1;
+		for (j = 0; j < *ccnt; j++) {
+			cstart = clist[j].addr;
+			cend = cstart + clist[j].len - 1;
+			if (s3start >= cstart && s3end <= cend)
+				break;
+		}
+		if (((unsigned int)j) >= (*ccnt)) {
+			pr_err("s3rec(a=0x%06x,l=%d), no chunk match, exiting.\n",
+			       s3start, s3data[i].len);
+			return 1;
+		}
+		coffset = s3start - cstart;
+		memcpy(clist[j].data + coffset, s3data[i].data, s3data[i].len);
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * mkpdrlist
+ *
+ * Reads a raw PDA and builds an array of pdrec_t structures.
+ *
+ * Arguments:
+ *	pda	buffer containing raw PDA bytes
+ *	pdrec	ptr to an array of pdrec_t's.  Will be filled on exit.
+ *	nrec	ptr to a variable that will contain the count of PDRs
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure (probably an errno)
+ *----------------------------------------------------------------
+ */
+static int mkpdrlist(struct pda *pda)
+{
+	__le16 *pda16 = (__le16 *)pda->buf;
+	int curroff;		/* in 'words' */
+
+	pda->nrec = 0;
+	curroff = 0;
+	while (curroff < (HFA384x_PDA_LEN_MAX / 2 - 1) &&
+	       le16_to_cpu(pda16[curroff + 1]) != HFA384x_PDR_END_OF_PDA) {
+		pda->rec[pda->nrec] = (struct hfa384x_pdrec *)&pda16[curroff];
+
+		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
+		    HFA384x_PDR_NICID) {
+			memcpy(&nicid, &pda->rec[pda->nrec]->data.nicid,
+			       sizeof(nicid));
+			le16_to_cpus(&nicid.id);
+			le16_to_cpus(&nicid.variant);
+			le16_to_cpus(&nicid.major);
+			le16_to_cpus(&nicid.minor);
+		}
+		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
+		    HFA384x_PDR_MFISUPRANGE) {
+			memcpy(&rfid, &pda->rec[pda->nrec]->data.mfisuprange,
+			       sizeof(rfid));
+			le16_to_cpus(&rfid.id);
+			le16_to_cpus(&rfid.variant);
+			le16_to_cpus(&rfid.bottom);
+			le16_to_cpus(&rfid.top);
+		}
+		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
+		    HFA384x_PDR_CFISUPRANGE) {
+			memcpy(&macid, &pda->rec[pda->nrec]->data.cfisuprange,
+			       sizeof(macid));
+			le16_to_cpus(&macid.id);
+			le16_to_cpus(&macid.variant);
+			le16_to_cpus(&macid.bottom);
+			le16_to_cpus(&macid.top);
+		}
+
+		(pda->nrec)++;
+		curroff += le16_to_cpu(pda16[curroff]) + 1;
+	}
+	if (curroff >= (HFA384x_PDA_LEN_MAX / 2 - 1)) {
+		pr_err("no end record found or invalid lengths in PDR data, exiting. %x %d\n",
+		       curroff, pda->nrec);
+		return 1;
+	}
+	pda->rec[pda->nrec] = (struct hfa384x_pdrec *)&pda16[curroff];
+	(pda->nrec)++;
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * plugimage
+ *
+ * Plugs the given image using the given plug records from the given
+ * PDA and filename.
+ *
+ * Arguments:
+ *	fchunk		Array of image chunks
+ *	nfchunks	Number of image chunks
+ *	s3plug		Array of plug records
+ *	ns3plug		Number of plug records
+ *	pda		Current pda data
+ *
+ * Returns:
+ *	0	success
+ *	~0	failure
+ *----------------------------------------------------------------
+ */
+static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
+		     struct s3plugrec *s3plug, unsigned int ns3plug,
+		     struct pda *pda)
+{
+	int result = 0;
+	int i;			/* plug index */
+	int j;			/* index of PDR or -1 if fname plug */
+	int c;			/* chunk index */
+	u32 pstart;
+	u32 pend;
+	u32 cstart = 0;
+	u32 cend;
+	u32 chunkoff;
+	u8 *dest;
+
+	/* for each plug record */
+	for (i = 0; i < ns3plug; i++) {
+		pstart = s3plug[i].addr;
+		pend = s3plug[i].addr + s3plug[i].len;
+		/* find the matching PDR (or filename) */
+		if (s3plug[i].itemcode != 0xffffffffUL) { /* not filename */
+			for (j = 0; j < pda->nrec; j++) {
+				if (s3plug[i].itemcode ==
+				    le16_to_cpu(pda->rec[j]->code))
+					break;
+			}
+		} else {
+			j = -1;
+		}
+		if (j >= pda->nrec && j != -1) { /*  if no matching PDR, fail */
+			pr_warn("warning: Failed to find PDR for plugrec 0x%04x.\n",
+				s3plug[i].itemcode);
+			continue;	/* and move on to the next PDR */
+
+			/* MSM: They swear that unless it's the MAC address,
+			 * the serial number, or the TX calibration records,
+			 * then there's reasonable defaults in the f/w
+			 * image.  Therefore, missing PDRs in the card
+			 * should only be a warning, not fatal.
+			 * TODO: add fatals for the PDRs mentioned above.
+			 */
+		}
+
+		/* Validate plug len against PDR len */
+		if (j != -1 && s3plug[i].len < le16_to_cpu(pda->rec[j]->len)) {
+			pr_err("error: Plug vs. PDR len mismatch for plugrec 0x%04x, abort plugging.\n",
+			       s3plug[i].itemcode);
+			result = 1;
+			continue;
+		}
+
+		/*
+		 * Validate plug address against
+		 * chunk data and identify chunk
+		 */
+		for (c = 0; c < nfchunks; c++) {
+			cstart = fchunk[c].addr;
+			cend = fchunk[c].addr + fchunk[c].len;
+			if (pstart >= cstart && pend <= cend)
+				break;
+		}
+		if (c >= nfchunks) {
+			pr_err("error: Failed to find image chunk for plugrec 0x%04x.\n",
+			       s3plug[i].itemcode);
+			result = 1;
+			continue;
+		}
+
+		/* Plug data */
+		chunkoff = pstart - cstart;
+		dest = fchunk[c].data + chunkoff;
+		pr_debug("Plugging item 0x%04x @ 0x%06x, len=%d, cnum=%d coff=0x%06x\n",
+			 s3plug[i].itemcode, pstart, s3plug[i].len,
+			 c, chunkoff);
+
+		if (j == -1) {	/* plug the filename */
+			memset(dest, 0, s3plug[i].len);
+			strncpy(dest, PRISM2_USB_FWFILE, s3plug[i].len - 1);
+		} else {	/* plug a PDR */
+			memcpy(dest, &pda->rec[j]->data, s3plug[i].len);
+		}
+	}
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * read_cardpda
+ *
+ * Sends the command for the driver to read the pda from the card
+ * named in the device variable.  Upon success, the card pda is
+ * stored in the "cardpda" variables.  Note that the pda structure
+ * is considered 'well formed' after this function.  That means
+ * that the nrecs is valid, the rec array has been set up, and there's
+ * a valid PDAEND record in the raw PDA data.
+ *
+ * Arguments:
+ *	pda		pda structure
+ *	wlandev		device
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure (probably an errno)
+ *----------------------------------------------------------------
+ */
+static int read_cardpda(struct pda *pda, struct wlandevice *wlandev)
+{
+	int result = 0;
+	struct p80211msg_p2req_readpda *msg;
+
+	msg = kzalloc(sizeof(*msg), GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	/* set up the msg */
+	msg->msgcode = DIDMSG_P2REQ_READPDA;
+	msg->msglen = sizeof(msg);
+	strscpy(msg->devname, wlandev->name, sizeof(msg->devname));
+	msg->pda.did = DIDMSG_P2REQ_READPDA_PDA;
+	msg->pda.len = HFA384x_PDA_LEN_MAX;
+	msg->pda.status = P80211ENUM_msgitem_status_no_value;
+	msg->resultcode.did = DIDMSG_P2REQ_READPDA_RESULTCODE;
+	msg->resultcode.len = sizeof(u32);
+	msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+
+	if (prism2mgmt_readpda(wlandev, msg) != 0) {
+		/* prism2mgmt_readpda prints an errno if appropriate */
+		result = -1;
+	} else if (msg->resultcode.data == P80211ENUM_resultcode_success) {
+		memcpy(pda->buf, msg->pda.data, HFA384x_PDA_LEN_MAX);
+		result = mkpdrlist(pda);
+	} else {
+		/* resultcode must've been something other than success */
+		result = -1;
+	}
+
+	kfree(msg);
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * read_fwfile
+ *
+ * Reads the given fw file which should have been compiled from an srec
+ * file. Each record in the fw file will either be a plain data record,
+ * a start address record, or other records used for plugging.
+ *
+ * Note that data records are expected to be sorted into
+ * ascending address order in the fw file.
+ *
+ * Note also that the start address record, originally an S7 record in
+ * the srec file, is expected in the fw file to be like a data record but
+ * with a certain address to make it identifiable.
+ *
+ * Here's the SREC format that the fw should have come from:
+ * S[37]nnaaaaaaaaddd...dddcc
+ *
+ *       nn - number of bytes starting with the address field
+ * aaaaaaaa - address in readable (or big endian) format
+ * dd....dd - 0-245 data bytes (two chars per byte)
+ *       cc - checksum
+ *
+ * The S7 record's (there should be only one) address value gets
+ * converted to an S3 record with address of 0xff400000, with the
+ * start address being stored as a 4 byte data word. That address is
+ * the start execution address used for RAM downloads.
+ *
+ * The S3 records have a collection of subformats indicated by the
+ * value of aaaaaaaa:
+ *   0xff000000 - Plug record, data field format:
+ *                xxxxxxxxaaaaaaaassssssss
+ *                x - PDR code number (little endian)
+ *                a - Address in load image to plug (little endian)
+ *                s - Length of plug data area (little endian)
+ *
+ *   0xff100000 - CRC16 generation record, data field format:
+ *                aaaaaaaassssssssbbbbbbbb
+ *                a - Start address for CRC calculation (little endian)
+ *                s - Length of data to  calculate over (little endian)
+ *                b - Boolean, true=write crc, false=don't write
+ *
+ *   0xff200000 - Info record, data field format:
+ *                ssssttttdd..dd
+ *                s - Size in words (little endian)
+ *                t - Info type (little endian), see #defines and
+ *                    struct s3inforec for details about types.
+ *                d - (s - 1) little endian words giving the contents of
+ *                    the given info type.
+ *
+ *   0xff400000 - Start address record, data field format:
+ *                aaaaaaaa
+ *                a - Address in load image to plug (little endian)
+ *
+ * Arguments:
+ *	record	firmware image (ihex record structure) in kernel memory
+ *
+ * Returns:
+ *	0	- success
+ *	~0	- failure (probably an errno)
+ *----------------------------------------------------------------
+ */
+static int read_fwfile(const struct ihex_binrec *record)
+{
+	int		i;
+	int		rcnt = 0;
+	u16		*tmpinfo;
+	u16		*ptr16;
+	u32		*ptr32, len, addr;
+
+	pr_debug("Reading fw file ...\n");
+
+	while (record) {
+		rcnt++;
+
+		len = be16_to_cpu(record->len);
+		addr = be32_to_cpu(record->addr);
+
+		/* Point into data for different word lengths */
+		ptr32 = (u32 *)record->data;
+		ptr16 = (u16 *)record->data;
+
+		/* parse what was an S3 srec and put it in the right array */
+		switch (addr) {
+		case S3ADDR_START:
+			startaddr = *ptr32;
+			pr_debug("  S7 start addr, record=%d addr=0x%08x\n",
+				 rcnt,
+				 startaddr);
+			break;
+		case S3ADDR_PLUG:
+			s3plug[ns3plug].itemcode = *ptr32;
+			s3plug[ns3plug].addr = *(ptr32 + 1);
+			s3plug[ns3plug].len = *(ptr32 + 2);
+
+			pr_debug("  S3 plugrec, record=%d itemcode=0x%08x addr=0x%08x len=%d\n",
+				 rcnt,
+				 s3plug[ns3plug].itemcode,
+				 s3plug[ns3plug].addr,
+				 s3plug[ns3plug].len);
+
+			ns3plug++;
+			if (ns3plug == S3PLUG_MAX) {
+				pr_err("S3 plugrec limit reached - aborting\n");
+				return 1;
+			}
+			break;
+		case S3ADDR_CRC:
+			s3crc[ns3crc].addr = *ptr32;
+			s3crc[ns3crc].len = *(ptr32 + 1);
+			s3crc[ns3crc].dowrite = *(ptr32 + 2);
+
+			pr_debug("  S3 crcrec, record=%d addr=0x%08x len=%d write=0x%08x\n",
+				 rcnt,
+				 s3crc[ns3crc].addr,
+				 s3crc[ns3crc].len,
+				 s3crc[ns3crc].dowrite);
+			ns3crc++;
+			if (ns3crc == S3CRC_MAX) {
+				pr_err("S3 crcrec limit reached - aborting\n");
+				return 1;
+			}
+			break;
+		case S3ADDR_INFO:
+			s3info[ns3info].len = *ptr16;
+			s3info[ns3info].type = *(ptr16 + 1);
+
+			pr_debug("  S3 inforec, record=%d len=0x%04x type=0x%04x\n",
+				 rcnt,
+				 s3info[ns3info].len,
+				 s3info[ns3info].type);
+			if (((s3info[ns3info].len - 1) * sizeof(u16)) >
+			   sizeof(s3info[ns3info].info)) {
+				pr_err("S3 inforec length too long - aborting\n");
+				return 1;
+			}
+
+			tmpinfo = (u16 *)&s3info[ns3info].info.version;
+			pr_debug("            info=");
+			for (i = 0; i < s3info[ns3info].len - 1; i++) {
+				tmpinfo[i] = *(ptr16 + 2 + i);
+				pr_debug("%04x ", tmpinfo[i]);
+			}
+			pr_debug("\n");
+
+			ns3info++;
+			if (ns3info == S3INFO_MAX) {
+				pr_err("S3 inforec limit reached - aborting\n");
+				return 1;
+			}
+			break;
+		default:	/* Data record */
+			s3data[ns3data].addr = addr;
+			s3data[ns3data].len = len;
+			s3data[ns3data].data = (uint8_t *)record->data;
+			ns3data++;
+			if (ns3data == S3DATA_MAX) {
+				pr_err("S3 datarec limit reached - aborting\n");
+				return 1;
+			}
+			break;
+		}
+		record = ihex_next_binrec(record);
+	}
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * writeimage
+ *
+ * Takes the chunks, builds p80211 messages and sends them down
+ * to the driver for writing to the card.
+ *
+ * Arguments:
+ *	wlandev		device
+ *	fchunk		Array of image chunks
+ *	nfchunks	Number of image chunks
+ *
+ * Returns:
+ *	0	success
+ *	~0	failure
+ *----------------------------------------------------------------
+ */
+static int writeimage(struct wlandevice *wlandev, struct imgchunk *fchunk,
+		      unsigned int nfchunks)
+{
+	int result = 0;
+	struct p80211msg_p2req_ramdl_state *rstmsg;
+	struct p80211msg_p2req_ramdl_write *rwrmsg;
+	u32 resultcode;
+	int i;
+	int j;
+	unsigned int nwrites;
+	u32 curroff;
+	u32 currlen;
+	u32 currdaddr;
+
+	rstmsg = kzalloc(sizeof(*rstmsg), GFP_KERNEL);
+	rwrmsg = kzalloc(sizeof(*rwrmsg), GFP_KERNEL);
+	if (!rstmsg || !rwrmsg) {
+		kfree(rstmsg);
+		kfree(rwrmsg);
+		netdev_err(wlandev->netdev,
+			   "%s: no memory for firmware download, aborting download\n",
+			   __func__);
+		return -ENOMEM;
+	}
+
+	/* Initialize the messages */
+	strscpy(rstmsg->devname, wlandev->name, sizeof(rstmsg->devname));
+	rstmsg->msgcode = DIDMSG_P2REQ_RAMDL_STATE;
+	rstmsg->msglen = sizeof(*rstmsg);
+	rstmsg->enable.did = DIDMSG_P2REQ_RAMDL_STATE_ENABLE;
+	rstmsg->exeaddr.did = DIDMSG_P2REQ_RAMDL_STATE_EXEADDR;
+	rstmsg->resultcode.did = DIDMSG_P2REQ_RAMDL_STATE_RESULTCODE;
+	rstmsg->enable.status = P80211ENUM_msgitem_status_data_ok;
+	rstmsg->exeaddr.status = P80211ENUM_msgitem_status_data_ok;
+	rstmsg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+	rstmsg->enable.len = sizeof(u32);
+	rstmsg->exeaddr.len = sizeof(u32);
+	rstmsg->resultcode.len = sizeof(u32);
+
+	strscpy(rwrmsg->devname, wlandev->name, sizeof(rwrmsg->devname));
+	rwrmsg->msgcode = DIDMSG_P2REQ_RAMDL_WRITE;
+	rwrmsg->msglen = sizeof(*rwrmsg);
+	rwrmsg->addr.did = DIDMSG_P2REQ_RAMDL_WRITE_ADDR;
+	rwrmsg->len.did = DIDMSG_P2REQ_RAMDL_WRITE_LEN;
+	rwrmsg->data.did = DIDMSG_P2REQ_RAMDL_WRITE_DATA;
+	rwrmsg->resultcode.did = DIDMSG_P2REQ_RAMDL_WRITE_RESULTCODE;
+	rwrmsg->addr.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->len.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->data.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+	rwrmsg->addr.len = sizeof(u32);
+	rwrmsg->len.len = sizeof(u32);
+	rwrmsg->data.len = WRITESIZE_MAX;
+	rwrmsg->resultcode.len = sizeof(u32);
+
+	/* Send xxx_state(enable) */
+	pr_debug("Sending dl_state(enable) message.\n");
+	rstmsg->enable.data = P80211ENUM_truth_true;
+	rstmsg->exeaddr.data = startaddr;
+
+	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "%s state enable failed w/ result=%d, aborting download\n",
+			   __func__, result);
+		goto free_result;
+	}
+	resultcode = rstmsg->resultcode.data;
+	if (resultcode != P80211ENUM_resultcode_success) {
+		netdev_err(wlandev->netdev,
+			   "%s()->xxxdl_state msg indicates failure, w/ resultcode=%d, aborting download.\n",
+			   __func__, resultcode);
+		result = 1;
+		goto free_result;
+	}
+
+	/* Now, loop through the data chunks and send WRITESIZE_MAX data */
+	for (i = 0; i < nfchunks; i++) {
+		nwrites = fchunk[i].len / WRITESIZE_MAX;
+		nwrites += (fchunk[i].len % WRITESIZE_MAX) ? 1 : 0;
+		curroff = 0;
+		for (j = 0; j < nwrites; j++) {
+			/* TODO Move this to a separate function */
+			int lenleft = fchunk[i].len - (WRITESIZE_MAX * j);
+
+			if (fchunk[i].len > WRITESIZE_MAX)
+				currlen = WRITESIZE_MAX;
+			else
+				currlen = lenleft;
+			curroff = j * WRITESIZE_MAX;
+			currdaddr = fchunk[i].addr + curroff;
+			/* Setup the message */
+			rwrmsg->addr.data = currdaddr;
+			rwrmsg->len.data = currlen;
+			memcpy(rwrmsg->data.data,
+			       fchunk[i].data + curroff, currlen);
+
+			/* Send flashdl_write(pda) */
+			pr_debug
+			    ("Sending xxxdl_write message addr=%06x len=%d.\n",
+			     currdaddr, currlen);
+
+			result = prism2mgmt_ramdl_write(wlandev, rwrmsg);
+
+			/* Check the results */
+			if (result) {
+				netdev_err(wlandev->netdev,
+					   "%s chunk write failed w/ result=%d, aborting download\n",
+					   __func__, result);
+				goto free_result;
+			}
+			resultcode = rstmsg->resultcode.data;
+			if (resultcode != P80211ENUM_resultcode_success) {
+				pr_err("%s()->xxxdl_write msg indicates failure, w/ resultcode=%d, aborting download.\n",
+				       __func__, resultcode);
+				result = 1;
+				goto free_result;
+			}
+		}
+	}
+
+	/* Send xxx_state(disable) */
+	pr_debug("Sending dl_state(disable) message.\n");
+	rstmsg->enable.data = P80211ENUM_truth_false;
+	rstmsg->exeaddr.data = 0;
+
+	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "%s state disable failed w/ result=%d, aborting download\n",
+			   __func__, result);
+		goto free_result;
+	}
+	resultcode = rstmsg->resultcode.data;
+	if (resultcode != P80211ENUM_resultcode_success) {
+		netdev_err(wlandev->netdev,
+			   "%s()->xxxdl_state msg indicates failure, w/ resultcode=%d, aborting download.\n",
+			   __func__, resultcode);
+		result = 1;
+		goto free_result;
+	}
+
+free_result:
+	kfree(rstmsg);
+	kfree(rwrmsg);
+	return result;
+}
+
+static int validate_identity(void)
+{
+	int i;
+	int result = 1;
+	int trump = 0;
+
+	pr_debug("NIC ID: %#x v%d.%d.%d\n",
+		 nicid.id, nicid.major, nicid.minor, nicid.variant);
+	pr_debug("MFI ID: %#x v%d %d->%d\n",
+		 rfid.id, rfid.variant, rfid.bottom, rfid.top);
+	pr_debug("CFI ID: %#x v%d %d->%d\n",
+		 macid.id, macid.variant, macid.bottom, macid.top);
+	pr_debug("PRI ID: %#x v%d %d->%d\n",
+		 priid.id, priid.variant, priid.bottom, priid.top);
+
+	for (i = 0; i < ns3info; i++) {
+		switch (s3info[i].type) {
+		case 1:
+			pr_debug("Version:  ID %#x %d.%d.%d\n",
+				 s3info[i].info.version.id,
+				 s3info[i].info.version.major,
+				 s3info[i].info.version.minor,
+				 s3info[i].info.version.variant);
+			break;
+		case 2:
+			pr_debug("Compat: Role %#x Id %#x v%d %d->%d\n",
+				 s3info[i].info.compat.role,
+				 s3info[i].info.compat.id,
+				 s3info[i].info.compat.variant,
+				 s3info[i].info.compat.bottom,
+				 s3info[i].info.compat.top);
+
+			/* MAC compat range */
+			if ((s3info[i].info.compat.role == 1) &&
+			    (s3info[i].info.compat.id == 2)) {
+				if (s3info[i].info.compat.variant !=
+				    macid.variant) {
+					result = 2;
+				}
+			}
+
+			/* PRI compat range */
+			if ((s3info[i].info.compat.role == 1) &&
+			    (s3info[i].info.compat.id == 3)) {
+				if ((s3info[i].info.compat.bottom >
+				     priid.top) ||
+				    (s3info[i].info.compat.top <
+				     priid.bottom)) {
+					result = 3;
+				}
+			}
+			/* SEC compat range */
+			if ((s3info[i].info.compat.role == 1) &&
+			    (s3info[i].info.compat.id == 4)) {
+				/* FIXME: isn't something missing here? */
+			}
+
+			break;
+		case 3:
+			pr_debug("Seq: %#x\n", s3info[i].info.buildseq);
+
+			break;
+		case 4:
+			pr_debug("Platform:  ID %#x %d.%d.%d\n",
+				 s3info[i].info.version.id,
+				 s3info[i].info.version.major,
+				 s3info[i].info.version.minor,
+				 s3info[i].info.version.variant);
+
+			if (nicid.id != s3info[i].info.version.id)
+				continue;
+			if (nicid.major != s3info[i].info.version.major)
+				continue;
+			if (nicid.minor != s3info[i].info.version.minor)
+				continue;
+			if ((nicid.variant != s3info[i].info.version.variant) &&
+			    (nicid.id != 0x8008))
+				continue;
+
+			trump = 1;
+			break;
+		case 0x8001:
+			pr_debug("name inforec len %d\n", s3info[i].len);
+
+			break;
+		default:
+			pr_debug("Unknown inforec type %d\n", s3info[i].type);
+		}
+	}
+	/* walk through */
+
+	if (trump && (result != 2))
+		result = 0;
+	return result;
+}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
new file mode 100644
index 000000000..9030a8939
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -0,0 +1,1356 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Management request handler functions.
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * The functions in this file handle management requests sent from
+ * user mode.
+ *
+ * Most of these functions have two separate blocks of code that are
+ * conditional on whether this is a station or an AP.  This is used
+ * to separate out the STA and AP responses to these management primitives.
+ * It's a choice (good, bad, indifferent?) to have the code in the same
+ * place so it's clear that the same primitive is implemented in both
+ * cases but has different behavior.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/if_arp.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <asm/byteorder.h>
+#include <linux/random.h>
+#include <linux/usb.h>
+#include <linux/bitops.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211mgmt.h"
+#include "p80211conv.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211metadef.h"
+#include "p80211metastruct.h"
+#include "hfa384x.h"
+#include "prism2mgmt.h"
+
+/* Converts 802.11 format rate specifications to prism2 */
+static inline u16 p80211rate_to_p2bit(u32 rate)
+{
+	switch (rate & ~BIT(7)) {
+	case 2:
+		return BIT(0);
+	case 4:
+		return BIT(1);
+	case 11:
+		return BIT(2);
+	case 22:
+		return BIT(3);
+	default:
+		return 0;
+	}
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_scan
+ *
+ * Initiate a scan for BSSs.
+ *
+ * This function corresponds to MLME-scan.request and part of
+ * MLME-scan.confirm.  As far as I can tell in the standard, there
+ * are no restrictions on when a scan.request may be issued.  We have
+ * to handle in whatever state the driver/MAC happen to be.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_scan(struct wlandevice *wlandev, void *msgp)
+{
+	int result = 0;
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_dot11req_scan *msg = msgp;
+	u16 roamingmode, word;
+	int i, timeout;
+	int istmpenable = 0;
+
+	struct hfa384x_host_scan_request_data scanreq;
+
+	/* gatekeeper check */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) <
+	    HFA384x_FIRMWARE_VERSION(1, 3, 2)) {
+		netdev_err(wlandev->netdev,
+			   "HostScan not supported with current firmware (<1.3.2).\n");
+		result = 1;
+		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
+		goto exit;
+	}
+
+	memset(&scanreq, 0, sizeof(scanreq));
+
+	/* save current roaming mode */
+	result = hfa384x_drvr_getconfig16(hw,
+					  HFA384x_RID_CNFROAMINGMODE,
+					  &roamingmode);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "getconfig(ROAMMODE) failed. result=%d\n", result);
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		goto exit;
+	}
+
+	/* drop into mode 3 for the scan */
+	result = hfa384x_drvr_setconfig16(hw,
+					  HFA384x_RID_CNFROAMINGMODE,
+					  HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMINGMODE) failed. result=%d\n",
+			   result);
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		goto exit;
+	}
+
+	/* active or passive? */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) >
+	    HFA384x_FIRMWARE_VERSION(1, 5, 0)) {
+		if (msg->scantype.data != P80211ENUM_scantype_active)
+			word = msg->maxchanneltime.data;
+		else
+			word = 0;
+
+		result =
+		    hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPASSIVESCANCTRL,
+					     word);
+		if (result) {
+			netdev_warn(wlandev->netdev,
+				    "Passive scan not supported with current firmware.  (<1.5.1)\n");
+		}
+	}
+
+	/* set up the txrate to be 2MBPS. Should be fastest basicrate... */
+	word = HFA384x_RATEBIT_2;
+	scanreq.tx_rate = cpu_to_le16(word);
+
+	/* set up the channel list */
+	word = 0;
+	for (i = 0; i < msg->channellist.data.len; i++) {
+		u8 channel = msg->channellist.data.data[i];
+
+		if (channel > 14)
+			continue;
+		/* channel 1 is BIT 0 ... channel 14 is BIT 13 */
+		word |= (1 << (channel - 1));
+	}
+	scanreq.channel_list = cpu_to_le16(word);
+
+	/* set up the ssid, if present. */
+	scanreq.ssid.len = cpu_to_le16(msg->ssid.data.len);
+	memcpy(scanreq.ssid.data, msg->ssid.data.data, msg->ssid.data.len);
+
+	/* Enable the MAC port if it's not already enabled  */
+	result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_PORTSTATUS, &word);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "getconfig(PORTSTATUS) failed. result=%d\n", result);
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		goto exit;
+	}
+	if (word == HFA384x_PORTSTATUS_DISABLED) {
+		__le16 wordbuf[17];
+
+		result = hfa384x_drvr_setconfig16(hw,
+						  HFA384x_RID_CNFROAMINGMODE,
+						  HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "setconfig(ROAMINGMODE) failed. result=%d\n",
+				   result);
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		/* Construct a bogus SSID and assign it to OwnSSID and
+		 * DesiredSSID
+		 */
+		wordbuf[0] = cpu_to_le16(WLAN_SSID_MAXLEN);
+		get_random_bytes(&wordbuf[1], WLAN_SSID_MAXLEN);
+		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFOWNSSID,
+						wordbuf,
+						HFA384x_RID_CNFOWNSSID_LEN);
+		if (result) {
+			netdev_err(wlandev->netdev, "Failed to set OwnSSID.\n");
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFDESIREDSSID,
+						wordbuf,
+						HFA384x_RID_CNFDESIREDSSID_LEN);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "Failed to set DesiredSSID.\n");
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		/* bsstype */
+		result = hfa384x_drvr_setconfig16(hw,
+						  HFA384x_RID_CNFPORTTYPE,
+						  HFA384x_PORTTYPE_IBSS);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "Failed to set CNFPORTTYPE.\n");
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		/* ibss options */
+		result = hfa384x_drvr_setconfig16(hw,
+						  HFA384x_RID_CREATEIBSS,
+						  HFA384x_CREATEIBSS_JOINCREATEIBSS);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "Failed to set CREATEIBSS.\n");
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		result = hfa384x_drvr_enable(hw, 0);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "drvr_enable(0) failed. result=%d\n",
+				   result);
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+		istmpenable = 1;
+	}
+
+	/* Figure out our timeout first Kus, then HZ */
+	timeout = msg->channellist.data.len * msg->maxchanneltime.data;
+	timeout = (timeout * HZ) / 1000;
+
+	/* Issue the scan request */
+	hw->scanflag = 0;
+
+	result = hfa384x_drvr_setconfig(hw,
+					HFA384x_RID_HOSTSCAN, &scanreq,
+					sizeof(scanreq));
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "setconfig(SCANREQUEST) failed. result=%d\n",
+			   result);
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		goto exit;
+	}
+
+	/* sleep until info frame arrives */
+	wait_event_interruptible_timeout(hw->cmdq, hw->scanflag, timeout);
+
+	msg->numbss.status = P80211ENUM_msgitem_status_data_ok;
+	if (hw->scanflag == -1)
+		hw->scanflag = 0;
+
+	msg->numbss.data = hw->scanflag;
+
+	hw->scanflag = 0;
+
+	/* Disable port if we temporarily enabled it. */
+	if (istmpenable) {
+		result = hfa384x_drvr_disable(hw, 0);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "drvr_disable(0) failed. result=%d\n",
+				   result);
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			goto exit;
+		}
+	}
+
+	/* restore original roaming mode */
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFROAMINGMODE,
+					  roamingmode);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMMODE) failed. result=%d\n", result);
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		goto exit;
+	}
+
+	result = 0;
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+exit:
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_scan_results
+ *
+ * Retrieve the BSS description for one of the BSSs identified in
+ * a scan.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_scan_results(struct wlandevice *wlandev, void *msgp)
+{
+	int result = 0;
+	struct p80211msg_dot11req_scan_results *req;
+	struct hfa384x *hw = wlandev->priv;
+	struct hfa384x_hscan_result_sub *item = NULL;
+
+	int count;
+
+	req = msgp;
+
+	req->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+
+	if (!hw->scanresults) {
+		netdev_err(wlandev->netdev,
+			   "dot11req_scan_results can only be used after a successful dot11req_scan.\n");
+		result = 2;
+		req->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
+		goto exit;
+	}
+
+	count = (hw->scanresults->framelen - 3) / 32;
+	if (count > HFA384x_SCANRESULT_MAX)
+		count = HFA384x_SCANRESULT_MAX;
+
+	if (req->bssindex.data >= count) {
+		netdev_dbg(wlandev->netdev,
+			   "requested index (%d) out of range (%d)\n",
+			   req->bssindex.data, count);
+		result = 2;
+		req->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
+		goto exit;
+	}
+
+	item = &hw->scanresults->info.hscanresult.result[req->bssindex.data];
+	/* signal and noise */
+	req->signal.status = P80211ENUM_msgitem_status_data_ok;
+	req->noise.status = P80211ENUM_msgitem_status_data_ok;
+	req->signal.data = le16_to_cpu(item->sl);
+	req->noise.data = le16_to_cpu(item->anl);
+
+	/* BSSID */
+	req->bssid.status = P80211ENUM_msgitem_status_data_ok;
+	req->bssid.data.len = WLAN_BSSID_LEN;
+	memcpy(req->bssid.data.data, item->bssid, WLAN_BSSID_LEN);
+
+	/* SSID */
+	req->ssid.status = P80211ENUM_msgitem_status_data_ok;
+	req->ssid.data.len = le16_to_cpu(item->ssid.len);
+	req->ssid.data.len = min_t(u16, req->ssid.data.len, WLAN_SSID_MAXLEN);
+	memcpy(req->ssid.data.data, item->ssid.data, req->ssid.data.len);
+
+	/* supported rates */
+	for (count = 0; count < 10; count++)
+		if (item->supprates[count] == 0)
+			break;
+
+#define REQBASICRATE(N) \
+	do { \
+		if ((count >= (N)) && DOT11_RATE5_ISBASIC_GET(	\
+			item->supprates[(N) - 1])) { \
+			req->basicrate ## N .data = item->supprates[(N) - 1]; \
+			req->basicrate ## N .status = \
+				P80211ENUM_msgitem_status_data_ok; \
+		} \
+	} while (0)
+
+	REQBASICRATE(1);
+	REQBASICRATE(2);
+	REQBASICRATE(3);
+	REQBASICRATE(4);
+	REQBASICRATE(5);
+	REQBASICRATE(6);
+	REQBASICRATE(7);
+	REQBASICRATE(8);
+
+#define REQSUPPRATE(N) \
+	do { \
+		if (count >= (N)) {					\
+			req->supprate ## N .data = item->supprates[(N) - 1]; \
+			req->supprate ## N .status = \
+				P80211ENUM_msgitem_status_data_ok; \
+		} \
+	} while (0)
+
+	REQSUPPRATE(1);
+	REQSUPPRATE(2);
+	REQSUPPRATE(3);
+	REQSUPPRATE(4);
+	REQSUPPRATE(5);
+	REQSUPPRATE(6);
+	REQSUPPRATE(7);
+	REQSUPPRATE(8);
+
+	/* beacon period */
+	req->beaconperiod.status = P80211ENUM_msgitem_status_data_ok;
+	req->beaconperiod.data = le16_to_cpu(item->bcnint);
+
+	/* timestamps */
+	req->timestamp.status = P80211ENUM_msgitem_status_data_ok;
+	req->timestamp.data = jiffies;
+	req->localtime.status = P80211ENUM_msgitem_status_data_ok;
+	req->localtime.data = jiffies;
+
+	/* atim window */
+	req->ibssatimwindow.status = P80211ENUM_msgitem_status_data_ok;
+	req->ibssatimwindow.data = le16_to_cpu(item->atim);
+
+	/* Channel */
+	req->dschannel.status = P80211ENUM_msgitem_status_data_ok;
+	req->dschannel.data = le16_to_cpu(item->chid);
+
+	/* capinfo bits */
+	count = le16_to_cpu(item->capinfo);
+	req->capinfo.status = P80211ENUM_msgitem_status_data_ok;
+	req->capinfo.data = count;
+
+	/* privacy flag */
+	req->privacy.status = P80211ENUM_msgitem_status_data_ok;
+	req->privacy.data = WLAN_GET_MGMT_CAP_INFO_PRIVACY(count);
+
+	/* cfpollable */
+	req->cfpollable.status = P80211ENUM_msgitem_status_data_ok;
+	req->cfpollable.data = WLAN_GET_MGMT_CAP_INFO_CFPOLLABLE(count);
+
+	/* cfpollreq */
+	req->cfpollreq.status = P80211ENUM_msgitem_status_data_ok;
+	req->cfpollreq.data = WLAN_GET_MGMT_CAP_INFO_CFPOLLREQ(count);
+
+	/* bsstype */
+	req->bsstype.status = P80211ENUM_msgitem_status_data_ok;
+	req->bsstype.data = (WLAN_GET_MGMT_CAP_INFO_ESS(count)) ?
+	    P80211ENUM_bsstype_infrastructure : P80211ENUM_bsstype_independent;
+
+	result = 0;
+	req->resultcode.data = P80211ENUM_resultcode_success;
+
+exit:
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_start
+ *
+ * Start a BSS.  Any station can do this for IBSS, only AP for ESS.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_start(struct wlandevice *wlandev, void *msgp)
+{
+	int result = 0;
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_dot11req_start *msg = msgp;
+
+	struct p80211pstrd *pstr;
+	u8 bytebuf[80];
+	struct hfa384x_bytestr *p2bytestr = (struct hfa384x_bytestr *)bytebuf;
+	u16 word;
+
+	wlandev->macmode = WLAN_MACMODE_NONE;
+
+	/* Set the SSID */
+	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
+
+	/*** ADHOC IBSS ***/
+	/* see if current f/w is less than 8c3 */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) <
+	    HFA384x_FIRMWARE_VERSION(0, 8, 3)) {
+		/* Ad-Hoc not quite supported on Prism2 */
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
+		goto done;
+	}
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+
+	/*** STATION ***/
+	/* Set the REQUIRED config items */
+	/* SSID */
+	pstr = (struct p80211pstrd *)&msg->ssid.data;
+	prism2mgmt_pstr2bytestr(p2bytestr, pstr);
+	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFOWNSSID,
+					bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to set CnfOwnSSID\n");
+		goto failed;
+	}
+	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFDESIREDSSID,
+					bytebuf,
+					HFA384x_RID_CNFDESIREDSSID_LEN);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to set CnfDesiredSSID\n");
+		goto failed;
+	}
+
+	/* bsstype - we use the default in the ap firmware */
+	/* IBSS port */
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 0);
+
+	/* beacon period */
+	word = msg->beaconperiod.data;
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNINT, word);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Failed to set beacon period=%d.\n", word);
+		goto failed;
+	}
+
+	/* dschannel */
+	word = msg->dschannel.data;
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNCHANNEL, word);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Failed to set channel=%d.\n", word);
+		goto failed;
+	}
+	/* Basic rates */
+	word = p80211rate_to_p2bit(msg->basicrate1.data);
+	if (msg->basicrate2.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate2.data);
+
+	if (msg->basicrate3.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate3.data);
+
+	if (msg->basicrate4.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate4.data);
+
+	if (msg->basicrate5.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate5.data);
+
+	if (msg->basicrate6.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate6.data);
+
+	if (msg->basicrate7.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate7.data);
+
+	if (msg->basicrate8.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->basicrate8.data);
+
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, word);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Failed to set basicrates=%d.\n", word);
+		goto failed;
+	}
+
+	/* Operational rates (supprates and txratecontrol) */
+	word = p80211rate_to_p2bit(msg->operationalrate1.data);
+	if (msg->operationalrate2.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate2.data);
+
+	if (msg->operationalrate3.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate3.data);
+
+	if (msg->operationalrate4.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate4.data);
+
+	if (msg->operationalrate5.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate5.data);
+
+	if (msg->operationalrate6.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate6.data);
+
+	if (msg->operationalrate7.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate7.data);
+
+	if (msg->operationalrate8.status == P80211ENUM_msgitem_status_data_ok)
+		word |= p80211rate_to_p2bit(msg->operationalrate8.data);
+
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, word);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Failed to set supprates=%d.\n", word);
+		goto failed;
+	}
+
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to set txrates=%d.\n",
+			   word);
+		goto failed;
+	}
+
+	/* Set the macmode so the frame setup code knows what to do */
+	if (msg->bsstype.data == P80211ENUM_bsstype_independent) {
+		wlandev->macmode = WLAN_MACMODE_IBSS_STA;
+		/* lets extend the data length a bit */
+		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN, 2304);
+	}
+
+	/* Enable the Port */
+	result = hfa384x_drvr_enable(hw, 0);
+	if (result) {
+		netdev_err(wlandev->netdev,
+			   "Enable macport failed, result=%d.\n", result);
+		goto failed;
+	}
+
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+	goto done;
+failed:
+	netdev_dbg(wlandev->netdev,
+		   "Failed to set a config option, result=%d\n", result);
+	msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
+
+done:
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_readpda
+ *
+ * Collect the PDA data and put it in the message.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_readpda(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_p2req_readpda *msg = msgp;
+	int result;
+
+	/* We only support collecting the PDA when in the FWLOAD
+	 * state.
+	 */
+	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
+		netdev_err(wlandev->netdev,
+			   "PDA may only be read in the fwload state.\n");
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	} else {
+		/*  Call drvr_readpda(), it handles the auxport enable
+		 *  and validating the returned PDA.
+		 */
+		result = hfa384x_drvr_readpda(hw,
+					      msg->pda.data,
+					      HFA384x_PDA_LEN_MAX);
+		if (result) {
+			netdev_err(wlandev->netdev,
+				   "hfa384x_drvr_readpda() failed, result=%d\n",
+				   result);
+
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			msg->resultcode.status =
+			    P80211ENUM_msgitem_status_data_ok;
+			return 0;
+		}
+		msg->pda.status = P80211ENUM_msgitem_status_data_ok;
+		msg->resultcode.data = P80211ENUM_resultcode_success;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	}
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_ramdl_state
+ *
+ * Establishes the beginning/end of a card RAM download session.
+ *
+ * It is expected that the ramdl_write() function will be called
+ * one or more times between the 'enable' and 'disable' calls to
+ * this function.
+ *
+ * Note: This function should not be called when a mac comm port
+ *       is active.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_ramdl_state(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_p2req_ramdl_state *msg = msgp;
+
+	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
+		netdev_err(wlandev->netdev,
+			   "ramdl_state(): may only be called in the fwload state.\n");
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		return 0;
+	}
+
+	/*
+	 ** Note: Interrupts are locked out if this is an AP and are NOT
+	 ** locked out if this is a station.
+	 */
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	if (msg->enable.data == P80211ENUM_truth_true) {
+		if (hfa384x_drvr_ramdl_enable(hw, msg->exeaddr.data)) {
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+		} else {
+			msg->resultcode.data = P80211ENUM_resultcode_success;
+		}
+	} else {
+		hfa384x_drvr_ramdl_disable(hw);
+		msg->resultcode.data = P80211ENUM_resultcode_success;
+	}
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_ramdl_write
+ *
+ * Writes a buffer to the card RAM using the download state.  This
+ * is for writing code to card RAM.  To just read or write raw data
+ * use the aux functions.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_ramdl_write(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_p2req_ramdl_write *msg = msgp;
+	u32 addr;
+	u32 len;
+	u8 *buf;
+
+	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
+		netdev_err(wlandev->netdev,
+			   "ramdl_write(): may only be called in the fwload state.\n");
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		return 0;
+	}
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	/* first validate the length */
+	if (msg->len.data > sizeof(msg->data.data)) {
+		msg->resultcode.status =
+		    P80211ENUM_resultcode_invalid_parameters;
+		return 0;
+	}
+	/* call the hfa384x function to do the write */
+	addr = msg->addr.data;
+	len = msg->len.data;
+	buf = msg->data.data;
+	if (hfa384x_drvr_ramdl_write(hw, addr, buf, len))
+		msg->resultcode.data = P80211ENUM_resultcode_refused;
+
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_flashdl_state
+ *
+ * Establishes the beginning/end of a card Flash download session.
+ *
+ * It is expected that the flashdl_write() function will be called
+ * one or more times between the 'enable' and 'disable' calls to
+ * this function.
+ *
+ * Note: This function should not be called when a mac comm port
+ *       is active.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_flashdl_state(struct wlandevice *wlandev, void *msgp)
+{
+	int result = 0;
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_p2req_flashdl_state *msg = msgp;
+
+	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
+		netdev_err(wlandev->netdev,
+			   "flashdl_state(): may only be called in the fwload state.\n");
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		return 0;
+	}
+
+	/*
+	 ** Note: Interrupts are locked out if this is an AP and are NOT
+	 ** locked out if this is a station.
+	 */
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	if (msg->enable.data == P80211ENUM_truth_true) {
+		if (hfa384x_drvr_flashdl_enable(hw)) {
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+		} else {
+			msg->resultcode.data = P80211ENUM_resultcode_success;
+		}
+	} else {
+		hfa384x_drvr_flashdl_disable(hw);
+		msg->resultcode.data = P80211ENUM_resultcode_success;
+		/* NOTE: At this point, the MAC is in the post-reset
+		 * state and the driver is in the fwload state.
+		 * We need to get the MAC back into the fwload
+		 * state.  To do this, we set the nsdstate to HWPRESENT
+		 * and then call the ifstate function to redo everything
+		 * that got us into the fwload state.
+		 */
+		wlandev->msdstate = WLAN_MSD_HWPRESENT;
+		result = prism2sta_ifstate(wlandev, P80211ENUM_ifstate_fwload);
+		if (result != P80211ENUM_resultcode_success) {
+			netdev_err(wlandev->netdev,
+				   "prism2sta_ifstate(fwload) failed, P80211ENUM_resultcode=%d\n",
+				   result);
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+			result = -1;
+		}
+	}
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_flashdl_write
+ *
+ *
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_flashdl_write(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct p80211msg_p2req_flashdl_write *msg = msgp;
+	u32 addr;
+	u32 len;
+	u8 *buf;
+
+	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
+		netdev_err(wlandev->netdev,
+			   "flashdl_write(): may only be called in the fwload state.\n");
+		msg->resultcode.data =
+		    P80211ENUM_resultcode_implementation_failure;
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		return 0;
+	}
+
+	/*
+	 ** Note: Interrupts are locked out if this is an AP and are NOT
+	 ** locked out if this is a station.
+	 */
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	/* first validate the length */
+	if (msg->len.data > sizeof(msg->data.data)) {
+		msg->resultcode.status =
+		    P80211ENUM_resultcode_invalid_parameters;
+		return 0;
+	}
+	/* call the hfa384x function to do the write */
+	addr = msg->addr.data;
+	len = msg->len.data;
+	buf = msg->data.data;
+	if (hfa384x_drvr_flashdl_write(hw, addr, buf, len))
+		msg->resultcode.data = P80211ENUM_resultcode_refused;
+
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_autojoin
+ *
+ * Associate with an ESS.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_autojoin(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	int result = 0;
+	u16 reg;
+	u16 port_type;
+	struct p80211msg_lnxreq_autojoin *msg = msgp;
+	struct p80211pstrd *pstr;
+	u8 bytebuf[256];
+	struct hfa384x_bytestr *p2bytestr = (struct hfa384x_bytestr *)bytebuf;
+
+	wlandev->macmode = WLAN_MACMODE_NONE;
+
+	/* Set the SSID */
+	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
+
+	/* Disable the Port */
+	hfa384x_drvr_disable(hw, 0);
+
+	/*** STATION ***/
+	/* Set the TxRates */
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, 0x000f);
+
+	/* Set the auth type */
+	if (msg->authtype.data == P80211ENUM_authalg_sharedkey)
+		reg = HFA384x_CNFAUTHENTICATION_SHAREDKEY;
+	else
+		reg = HFA384x_CNFAUTHENTICATION_OPENSYSTEM;
+
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAUTHENTICATION, reg);
+
+	/* Set the ssid */
+	memset(bytebuf, 0, 256);
+	pstr = (struct p80211pstrd *)&msg->ssid.data;
+	prism2mgmt_pstr2bytestr(p2bytestr, pstr);
+	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFDESIREDSSID,
+					bytebuf,
+					HFA384x_RID_CNFDESIREDSSID_LEN);
+	port_type = HFA384x_PORTTYPE_BSS;
+	/* Set the PortType */
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, port_type);
+
+	/* Enable the Port */
+	hfa384x_drvr_enable(hw, 0);
+
+	/* Set the resultcode */
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+	return result;
+}
+
+/*----------------------------------------------------------------
+ * prism2mgmt_wlansniff
+ *
+ * Start or stop sniffing.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ *----------------------------------------------------------------
+ */
+int prism2mgmt_wlansniff(struct wlandevice *wlandev, void *msgp)
+{
+	int result = 0;
+	struct p80211msg_lnxreq_wlansniff *msg = msgp;
+
+	struct hfa384x *hw = wlandev->priv;
+	u16 word;
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	switch (msg->enable.data) {
+	case P80211ENUM_truth_false:
+		/* Confirm that we're in monitor mode */
+		if (wlandev->netdev->type == ARPHRD_ETHER) {
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_invalid_parameters;
+			return 0;
+		}
+		/* Disable monitor mode */
+		result = hfa384x_cmd_monitor(hw, HFA384x_MONITOR_DISABLE);
+		if (result) {
+			netdev_dbg(wlandev->netdev,
+				   "failed to disable monitor mode, result=%d\n",
+				   result);
+			goto failed;
+		}
+		/* Disable port 0 */
+		result = hfa384x_drvr_disable(hw, 0);
+		if (result) {
+			netdev_dbg
+			(wlandev->netdev,
+			     "failed to disable port 0 after sniffing, result=%d\n",
+			     result);
+			goto failed;
+		}
+		/* Clear the driver state */
+		wlandev->netdev->type = ARPHRD_ETHER;
+
+		/* Restore the wepflags */
+		result = hfa384x_drvr_setconfig16(hw,
+						  HFA384x_RID_CNFWEPFLAGS,
+						  hw->presniff_wepflags);
+		if (result) {
+			netdev_dbg
+			    (wlandev->netdev,
+			     "failed to restore wepflags=0x%04x, result=%d\n",
+			     hw->presniff_wepflags, result);
+			goto failed;
+		}
+
+		/* Set the port to its prior type and enable (if necessary) */
+		if (hw->presniff_port_type != 0) {
+			word = hw->presniff_port_type;
+			result = hfa384x_drvr_setconfig16(hw,
+							  HFA384x_RID_CNFPORTTYPE,
+							  word);
+			if (result) {
+				netdev_dbg
+				    (wlandev->netdev,
+				     "failed to restore porttype, result=%d\n",
+				     result);
+				goto failed;
+			}
+
+			/* Enable the port */
+			result = hfa384x_drvr_enable(hw, 0);
+			if (result) {
+				netdev_dbg(wlandev->netdev,
+					   "failed to enable port to presniff setting, result=%d\n",
+					   result);
+				goto failed;
+			}
+		} else {
+			result = hfa384x_drvr_disable(hw, 0);
+		}
+
+		netdev_info(wlandev->netdev, "monitor mode disabled\n");
+		msg->resultcode.data = P80211ENUM_resultcode_success;
+		return 0;
+	case P80211ENUM_truth_true:
+		/* Disable the port (if enabled), only check Port 0 */
+		if (hw->port_enabled[0]) {
+			if (wlandev->netdev->type == ARPHRD_ETHER) {
+				/* Save macport 0 state */
+				result = hfa384x_drvr_getconfig16(hw,
+								  HFA384x_RID_CNFPORTTYPE,
+								  &hw->presniff_port_type);
+				if (result) {
+					netdev_dbg
+					(wlandev->netdev,
+					     "failed to read porttype, result=%d\n",
+					     result);
+					goto failed;
+				}
+				/* Save the wepflags state */
+				result = hfa384x_drvr_getconfig16(hw,
+								  HFA384x_RID_CNFWEPFLAGS,
+								  &hw->presniff_wepflags);
+				if (result) {
+					netdev_dbg
+					(wlandev->netdev,
+					     "failed to read wepflags, result=%d\n",
+					     result);
+					goto failed;
+				}
+				hfa384x_drvr_stop(hw);
+				result = hfa384x_drvr_start(hw);
+				if (result) {
+					netdev_dbg(wlandev->netdev,
+						   "failed to restart the card for sniffing, result=%d\n",
+						   result);
+					goto failed;
+				}
+			} else {
+				/* Disable the port */
+				result = hfa384x_drvr_disable(hw, 0);
+				if (result) {
+					netdev_dbg(wlandev->netdev,
+						   "failed to enable port for sniffing, result=%d\n",
+						   result);
+					goto failed;
+				}
+			}
+		} else {
+			hw->presniff_port_type = 0;
+		}
+
+		/* Set the channel we wish to sniff  */
+		word = msg->channel.data;
+		result = hfa384x_drvr_setconfig16(hw,
+						  HFA384x_RID_CNFOWNCHANNEL,
+						  word);
+		hw->sniff_channel = word;
+
+		if (result) {
+			netdev_dbg(wlandev->netdev,
+				   "failed to set channel %d, result=%d\n",
+				   word, result);
+			goto failed;
+		}
+
+		/* Now if we're already sniffing, we can skip the rest */
+		if (wlandev->netdev->type != ARPHRD_ETHER) {
+			/* Set the port type to pIbss */
+			word = HFA384x_PORTTYPE_PSUEDOIBSS;
+			result = hfa384x_drvr_setconfig16(hw,
+							  HFA384x_RID_CNFPORTTYPE,
+							  word);
+			if (result) {
+				netdev_dbg
+				    (wlandev->netdev,
+				     "failed to set porttype %d, result=%d\n",
+				     word, result);
+				goto failed;
+			}
+			if ((msg->keepwepflags.status ==
+			     P80211ENUM_msgitem_status_data_ok) &&
+			    (msg->keepwepflags.data != P80211ENUM_truth_true)) {
+				/* Set the wepflags for no decryption */
+				word = HFA384x_WEPFLAGS_DISABLE_TXCRYPT |
+				    HFA384x_WEPFLAGS_DISABLE_RXCRYPT;
+				result =
+				    hfa384x_drvr_setconfig16(hw,
+							     HFA384x_RID_CNFWEPFLAGS,
+							     word);
+			}
+
+			if (result) {
+				netdev_dbg
+				  (wlandev->netdev,
+				   "failed to set wepflags=0x%04x, result=%d\n",
+				   word, result);
+				goto failed;
+			}
+		}
+
+		/* Do we want to strip the FCS in monitor mode? */
+		if ((msg->stripfcs.status ==
+		     P80211ENUM_msgitem_status_data_ok) &&
+		    (msg->stripfcs.data == P80211ENUM_truth_true)) {
+			hw->sniff_fcs = 0;
+		} else {
+			hw->sniff_fcs = 1;
+		}
+
+		/* Do we want to truncate the packets? */
+		if (msg->packet_trunc.status ==
+		    P80211ENUM_msgitem_status_data_ok) {
+			hw->sniff_truncate = msg->packet_trunc.data;
+		} else {
+			hw->sniff_truncate = 0;
+		}
+
+		/* Enable the port */
+		result = hfa384x_drvr_enable(hw, 0);
+		if (result) {
+			netdev_dbg
+			    (wlandev->netdev,
+			     "failed to enable port for sniffing, result=%d\n",
+			     result);
+			goto failed;
+		}
+		/* Enable monitor mode */
+		result = hfa384x_cmd_monitor(hw, HFA384x_MONITOR_ENABLE);
+		if (result) {
+			netdev_dbg(wlandev->netdev,
+				   "failed to enable monitor mode, result=%d\n",
+				   result);
+			goto failed;
+		}
+
+		if (wlandev->netdev->type == ARPHRD_ETHER)
+			netdev_info(wlandev->netdev, "monitor mode enabled\n");
+
+		/* Set the driver state */
+		/* Do we want the prism2 header? */
+		if ((msg->prismheader.status ==
+		     P80211ENUM_msgitem_status_data_ok) &&
+		    (msg->prismheader.data == P80211ENUM_truth_true)) {
+			hw->sniffhdr = 0;
+			wlandev->netdev->type = ARPHRD_IEEE80211_PRISM;
+		} else if ((msg->wlanheader.status ==
+			    P80211ENUM_msgitem_status_data_ok) &&
+			   (msg->wlanheader.data == P80211ENUM_truth_true)) {
+			hw->sniffhdr = 1;
+			wlandev->netdev->type = ARPHRD_IEEE80211_PRISM;
+		} else {
+			wlandev->netdev->type = ARPHRD_IEEE80211;
+		}
+
+		msg->resultcode.data = P80211ENUM_resultcode_success;
+		return 0;
+	default:
+		msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
+		return 0;
+	}
+
+failed:
+	msg->resultcode.data = P80211ENUM_resultcode_refused;
+	return 0;
+}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
new file mode 100644
index 000000000..7132cec2d
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -0,0 +1,120 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1) */
+/*
+ *
+ * Declares the mgmt command handler functions
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file contains the constants and data structures for interaction
+ * with the hfa384x Wireless LAN (WLAN) Media Access Controller (MAC).
+ * The hfa384x is a portion of the Harris PRISM(tm) WLAN chipset.
+ *
+ * [Implementation and usage notes]
+ *
+ * [References]
+ *   CW10 Programmer's Manual v1.5
+ *   IEEE 802.11 D10.0
+ *
+ *    --------------------------------------------------------------------
+ */
+
+#ifndef _PRISM2MGMT_H
+#define _PRISM2MGMT_H
+
+extern int prism2_reset_holdtime;
+extern int prism2_reset_settletime;
+
+u32 prism2sta_ifstate(struct wlandevice *wlandev, u32 ifstate);
+
+void prism2sta_ev_info(struct wlandevice *wlandev,
+		       struct hfa384x_inf_frame *inf);
+void prism2sta_ev_txexc(struct wlandevice *wlandev, u16 status);
+void prism2sta_ev_tx(struct wlandevice *wlandev, u16 status);
+void prism2sta_ev_alloc(struct wlandevice *wlandev);
+
+int prism2mgmt_mibset_mibget(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_scan(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_scan_results(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_start(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_wlansniff(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_readpda(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_ramdl_state(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_ramdl_write(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_flashdl_state(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_flashdl_write(struct wlandevice *wlandev, void *msgp);
+int prism2mgmt_autojoin(struct wlandevice *wlandev, void *msgp);
+
+/*---------------------------------------------------------------
+ * conversion functions going between wlan message data types and
+ * Prism2 data types
+ *---------------------------------------------------------------
+ */
+
+/* byte area conversion functions*/
+void prism2mgmt_bytearea2pstr(u8 *bytearea, struct p80211pstrd *pstr, int len);
+
+/* byte string conversion functions*/
+void prism2mgmt_pstr2bytestr(struct hfa384x_bytestr *bytestr,
+			     struct p80211pstrd *pstr);
+void prism2mgmt_bytestr2pstr(struct hfa384x_bytestr *bytestr,
+			     struct p80211pstrd *pstr);
+
+/* functions to convert Group Addresses */
+void prism2mgmt_get_grpaddr(u32 did, struct p80211pstrd *pstr,
+			    struct hfa384x *priv);
+int prism2mgmt_set_grpaddr(u32 did,
+			   u8 *prism2buf, struct p80211pstrd *pstr,
+			   struct hfa384x *priv);
+int prism2mgmt_get_grpaddr_index(u32 did);
+
+void prism2sta_processing_defer(struct work_struct *data);
+
+void prism2sta_commsqual_defer(struct work_struct *data);
+void prism2sta_commsqual_timer(struct timer_list *t);
+
+/* Interface callback functions, passing data back up to the cfg80211 layer */
+void prism2_connect_result(struct wlandevice *wlandev, u8 failed);
+void prism2_disconnected(struct wlandevice *wlandev);
+void prism2_roamed(struct wlandevice *wlandev);
+
+#endif
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
new file mode 100644
index 000000000..fcf831387
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -0,0 +1,763 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Management request for mibset/mibget
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * The functions in this file handle the mibset/mibget management
+ * functions.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <asm/byteorder.h>
+#include <linux/usb.h>
+#include <linux/bitops.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211mgmt.h"
+#include "p80211conv.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211metadef.h"
+#include "p80211metastruct.h"
+#include "hfa384x.h"
+#include "prism2mgmt.h"
+
+#define MIB_TMP_MAXLEN    200	/* Max length of RID record (in bytes). */
+
+#define  F_STA        0x1	/* MIB is supported on stations. */
+#define  F_READ       0x2	/* MIB may be read. */
+#define  F_WRITE      0x4	/* MIB may be written. */
+
+struct mibrec {
+	u32 did;
+	u16 flag;
+	u16 parm1;
+	u16 parm2;
+	u16 parm3;
+	int (*func)(struct mibrec *mib,
+		    int isget,
+		    struct wlandevice *wlandev,
+		    struct hfa384x *hw,
+		    struct p80211msg_dot11req_mibset *msg, void *data);
+};
+
+static int prism2mib_bytearea2pstr(struct mibrec *mib,
+				   int isget,
+				   struct wlandevice *wlandev,
+				   struct hfa384x *hw,
+				   struct p80211msg_dot11req_mibset *msg,
+				   void *data);
+
+static int prism2mib_uint32(struct mibrec *mib,
+			    int isget,
+			    struct wlandevice *wlandev,
+			    struct hfa384x *hw,
+			    struct p80211msg_dot11req_mibset *msg, void *data);
+
+static int prism2mib_flag(struct mibrec *mib,
+			  int isget,
+			  struct wlandevice *wlandev,
+			  struct hfa384x *hw,
+			  struct p80211msg_dot11req_mibset *msg, void *data);
+
+static int prism2mib_wepdefaultkey(struct mibrec *mib,
+				   int isget,
+				   struct wlandevice *wlandev,
+				   struct hfa384x *hw,
+				   struct p80211msg_dot11req_mibset *msg,
+				   void *data);
+
+static int prism2mib_privacyinvoked(struct mibrec *mib,
+				    int isget,
+				    struct wlandevice *wlandev,
+				    struct hfa384x *hw,
+				    struct p80211msg_dot11req_mibset *msg,
+				    void *data);
+
+static int
+prism2mib_fragmentationthreshold(struct mibrec *mib,
+				 int isget,
+				 struct wlandevice *wlandev,
+				 struct hfa384x *hw,
+				 struct p80211msg_dot11req_mibset *msg,
+				 void *data);
+
+static int prism2mib_priv(struct mibrec *mib,
+			  int isget,
+			  struct wlandevice *wlandev,
+			  struct hfa384x *hw,
+			  struct p80211msg_dot11req_mibset *msg, void *data);
+
+static struct mibrec mibtab[] = {
+	/* dot11smt MIB's */
+	{didmib_dot11smt_wepdefaultkeystable_key(1),
+	 F_STA | F_WRITE,
+	 HFA384x_RID_CNFWEPDEFAULTKEY0, 0, 0,
+	 prism2mib_wepdefaultkey},
+	{didmib_dot11smt_wepdefaultkeystable_key(2),
+	 F_STA | F_WRITE,
+	 HFA384x_RID_CNFWEPDEFAULTKEY1, 0, 0,
+	 prism2mib_wepdefaultkey},
+	{didmib_dot11smt_wepdefaultkeystable_key(3),
+	 F_STA | F_WRITE,
+	 HFA384x_RID_CNFWEPDEFAULTKEY2, 0, 0,
+	 prism2mib_wepdefaultkey},
+	{didmib_dot11smt_wepdefaultkeystable_key(4),
+	 F_STA | F_WRITE,
+	 HFA384x_RID_CNFWEPDEFAULTKEY3, 0, 0,
+	 prism2mib_wepdefaultkey},
+	{DIDMIB_DOT11SMT_PRIVACYTABLE_PRIVACYINVOKED,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_PRIVINVOKED, 0,
+	 prism2mib_privacyinvoked},
+	{DIDMIB_DOT11SMT_PRIVACYTABLE_WEPDEFAULTKEYID,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFWEPDEFAULTKEYID, 0, 0,
+	 prism2mib_uint32},
+	{DIDMIB_DOT11SMT_PRIVACYTABLE_EXCLUDEUNENCRYPTED,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_EXCLUDE, 0,
+	 prism2mib_flag},
+
+	/* dot11mac MIB's */
+
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_MACADDRESS,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
+	 prism2mib_bytearea2pstr},
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_RTSTHRESHOLD,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_RTSTHRESH, 0, 0,
+	 prism2mib_uint32},
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_SHORTRETRYLIMIT,
+	 F_STA | F_READ,
+	 HFA384x_RID_SHORTRETRYLIMIT, 0, 0,
+	 prism2mib_uint32},
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_LONGRETRYLIMIT,
+	 F_STA | F_READ,
+	 HFA384x_RID_LONGRETRYLIMIT, 0, 0,
+	 prism2mib_uint32},
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_FRAGMENTATIONTHRESHOLD,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_FRAGTHRESH, 0, 0,
+	 prism2mib_fragmentationthreshold},
+	{DIDMIB_DOT11MAC_OPERATIONTABLE_MAXTRANSMITMSDULIFETIME,
+	 F_STA | F_READ,
+	 HFA384x_RID_MAXTXLIFETIME, 0, 0,
+	 prism2mib_uint32},
+
+	/* dot11phy MIB's */
+
+	{DIDMIB_DOT11PHY_DSSSTABLE_CURRENTCHANNEL,
+	 F_STA | F_READ,
+	 HFA384x_RID_CURRENTCHANNEL, 0, 0,
+	 prism2mib_uint32},
+	{DIDMIB_DOT11PHY_TXPOWERTABLE_CURRENTTXPOWERLEVEL,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_TXPOWERMAX, 0, 0,
+	 prism2mib_uint32},
+
+	/* p2Static MIB's */
+
+	{DIDMIB_P2_STATIC_CNFPORTTYPE,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFPORTTYPE, 0, 0,
+	 prism2mib_uint32},
+
+	/* p2MAC MIB's */
+
+	{DIDMIB_P2_MAC_CURRENTTXRATE,
+	 F_STA | F_READ,
+	 HFA384x_RID_CURRENTTXRATE, 0, 0,
+	 prism2mib_uint32},
+
+	/* And finally, lnx mibs */
+	{DIDMIB_LNX_CONFIGTABLE_RSNAIE,
+	 F_STA | F_READ | F_WRITE,
+	 HFA384x_RID_CNFWPADATA, 0, 0,
+	 prism2mib_priv},
+	{0, 0, 0, 0, 0, NULL}
+};
+
+/*
+ * prism2mgmt_mibset_mibget
+ *
+ * Set the value of a mib item.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	0	success and done
+ *	<0	success, but we're waiting for something to finish.
+ *	>0	an error occurred while handling the message.
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ */
+
+int prism2mgmt_mibset_mibget(struct wlandevice *wlandev, void *msgp)
+{
+	struct hfa384x *hw = wlandev->priv;
+	int result, isget;
+	struct mibrec *mib;
+
+	u16 which;
+
+	struct p80211msg_dot11req_mibset *msg = msgp;
+	struct p80211itemd *mibitem;
+
+	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+	msg->resultcode.data = P80211ENUM_resultcode_success;
+
+	/*
+	 ** Determine if this is an Access Point or a station.
+	 */
+
+	which = F_STA;
+
+	/*
+	 ** Find the MIB in the MIB table.  Note that a MIB may be in the
+	 ** table twice...once for an AP and once for a station.  Make sure
+	 ** to get the correct one.  Note that DID=0 marks the end of the
+	 ** MIB table.
+	 */
+
+	mibitem = (struct p80211itemd *)msg->mibattribute.data;
+
+	for (mib = mibtab; mib->did != 0; mib++)
+		if (mib->did == mibitem->did && (mib->flag & which))
+			break;
+
+	if (mib->did == 0) {
+		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
+		goto done;
+	}
+
+	/*
+	 ** Determine if this is a "mibget" or a "mibset".  If this is a
+	 ** "mibget", then make sure that the MIB may be read.  Otherwise,
+	 ** this is a "mibset" so make sure that the MIB may be written.
+	 */
+
+	isget = (msg->msgcode == DIDMSG_DOT11REQ_MIBGET);
+
+	if (isget) {
+		if (!(mib->flag & F_READ)) {
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_cant_get_writeonly_mib;
+			goto done;
+		}
+	} else {
+		if (!(mib->flag & F_WRITE)) {
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_cant_set_readonly_mib;
+			goto done;
+		}
+	}
+
+	/*
+	 ** Execute the MIB function.  If things worked okay, then make
+	 ** sure that the MIB function also worked okay.  If so, and this
+	 ** is a "mibget", then the status value must be set for both the
+	 ** "mibattribute" parameter and the mib item within the data
+	 ** portion of the "mibattribute".
+	 */
+
+	result = mib->func(mib, isget, wlandev, hw, msg, (void *)mibitem->data);
+
+	if (msg->resultcode.data == P80211ENUM_resultcode_success) {
+		if (result != 0) {
+			pr_debug("get/set failure, result=%d\n", result);
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_implementation_failure;
+		} else {
+			if (isget) {
+				msg->mibattribute.status =
+				    P80211ENUM_msgitem_status_data_ok;
+				mibitem->status =
+				    P80211ENUM_msgitem_status_data_ok;
+			}
+		}
+	}
+
+done:
+	return 0;
+}
+
+/*
+ * prism2mib_bytearea2pstr
+ *
+ * Get/set pstr data to/from a byte area.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Number of bytes of RID data.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_bytearea2pstr(struct mibrec *mib,
+				   int isget,
+				   struct wlandevice *wlandev,
+				   struct hfa384x *hw,
+				   struct p80211msg_dot11req_mibset *msg,
+				   void *data)
+{
+	int result;
+	struct p80211pstrd *pstr = data;
+	u8 bytebuf[MIB_TMP_MAXLEN];
+
+	if (isget) {
+		result =
+		    hfa384x_drvr_getconfig(hw, mib->parm1, bytebuf, mib->parm2);
+		prism2mgmt_bytearea2pstr(bytebuf, pstr, mib->parm2);
+	} else {
+		memset(bytebuf, 0, mib->parm2);
+		memcpy(bytebuf, pstr->data, pstr->len);
+		result =
+		    hfa384x_drvr_setconfig(hw, mib->parm1, bytebuf, mib->parm2);
+	}
+
+	return result;
+}
+
+/*
+ * prism2mib_uint32
+ *
+ * Get/set uint32 data.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Not used.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_uint32(struct mibrec *mib,
+			    int isget,
+			    struct wlandevice *wlandev,
+			    struct hfa384x *hw,
+			    struct p80211msg_dot11req_mibset *msg, void *data)
+{
+	int result;
+	u32 *uint32 = data;
+	u8 bytebuf[MIB_TMP_MAXLEN];
+	u16 *wordbuf = (u16 *)bytebuf;
+
+	if (isget) {
+		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
+		*uint32 = *wordbuf;
+	} else {
+		*wordbuf = *uint32;
+		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
+	}
+
+	return result;
+}
+
+/*
+ * prism2mib_flag
+ *
+ * Get/set a flag.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Bit to get/set.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_flag(struct mibrec *mib,
+			  int isget,
+			  struct wlandevice *wlandev,
+			  struct hfa384x *hw,
+			  struct p80211msg_dot11req_mibset *msg, void *data)
+{
+	int result;
+	u32 *uint32 = data;
+	u8 bytebuf[MIB_TMP_MAXLEN];
+	u16 *wordbuf = (u16 *)bytebuf;
+	u32 flags;
+
+	result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
+	if (result == 0) {
+		flags = *wordbuf;
+		if (isget) {
+			*uint32 = (flags & mib->parm2) ?
+			    P80211ENUM_truth_true : P80211ENUM_truth_false;
+		} else {
+			if ((*uint32) == P80211ENUM_truth_true)
+				flags |= mib->parm2;
+			else
+				flags &= ~mib->parm2;
+			*wordbuf = flags;
+			result =
+			    hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
+		}
+	}
+
+	return result;
+}
+
+/*
+ * prism2mib_wepdefaultkey
+ *
+ * Get/set WEP default keys.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Number of bytes of RID data.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_wepdefaultkey(struct mibrec *mib,
+				   int isget,
+				   struct wlandevice *wlandev,
+				   struct hfa384x *hw,
+				   struct p80211msg_dot11req_mibset *msg,
+				   void *data)
+{
+	int result;
+	struct p80211pstrd *pstr = data;
+	u8 bytebuf[MIB_TMP_MAXLEN];
+	u16 len;
+
+	if (isget) {
+		result = 0;	/* Should never happen. */
+	} else {
+		len = (pstr->len > 5) ? HFA384x_RID_CNFWEP128DEFAULTKEY_LEN :
+		    HFA384x_RID_CNFWEPDEFAULTKEY_LEN;
+		memset(bytebuf, 0, len);
+		memcpy(bytebuf, pstr->data, pstr->len);
+		result = hfa384x_drvr_setconfig(hw, mib->parm1, bytebuf, len);
+	}
+
+	return result;
+}
+
+/*
+ * prism2mib_privacyinvoked
+ *
+ * Get/set the dot11PrivacyInvoked value.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Bit value for PrivacyInvoked flag.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_privacyinvoked(struct mibrec *mib,
+				    int isget,
+				    struct wlandevice *wlandev,
+				    struct hfa384x *hw,
+				    struct p80211msg_dot11req_mibset *msg,
+				    void *data)
+{
+	if (wlandev->hostwep & HOSTWEP_DECRYPT) {
+		if (wlandev->hostwep & HOSTWEP_DECRYPT)
+			mib->parm2 |= HFA384x_WEPFLAGS_DISABLE_RXCRYPT;
+		if (wlandev->hostwep & HOSTWEP_ENCRYPT)
+			mib->parm2 |= HFA384x_WEPFLAGS_DISABLE_TXCRYPT;
+	}
+
+	return prism2mib_flag(mib, isget, wlandev, hw, msg, data);
+}
+
+/*
+ * prism2mib_fragmentationthreshold
+ *
+ * Get/set the fragmentation threshold.
+ *
+ * MIB record parameters:
+ *       parm1    Prism2 RID value.
+ *       parm2    Not used.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int
+prism2mib_fragmentationthreshold(struct mibrec *mib,
+				 int isget,
+				 struct wlandevice *wlandev,
+				 struct hfa384x *hw,
+				 struct p80211msg_dot11req_mibset *msg,
+				 void *data)
+{
+	u32 *uint32 = data;
+
+	if (!isget)
+		if ((*uint32) % 2) {
+			netdev_warn(wlandev->netdev,
+				    "Attempt to set odd number FragmentationThreshold\n");
+			msg->resultcode.data =
+			    P80211ENUM_resultcode_not_supported;
+			return 0;
+		}
+
+	return prism2mib_uint32(mib, isget, wlandev, hw, msg, data);
+}
+
+/*
+ * prism2mib_priv
+ *
+ * Get/set values in the "priv" data structure.
+ *
+ * MIB record parameters:
+ *       parm1    Not used.
+ *       parm2    Not used.
+ *       parm3    Not used.
+ *
+ * Arguments:
+ *       mib      MIB record.
+ *       isget    MIBGET/MIBSET flag.
+ *       wlandev  wlan device structure.
+ *       priv     "priv" structure.
+ *       hw       "hw" structure.
+ *       msg      Message structure.
+ *       data     Data buffer.
+ *
+ * Returns:
+ *       0   - Success.
+ *       ~0  - Error.
+ *
+ */
+
+static int prism2mib_priv(struct mibrec *mib,
+			  int isget,
+			  struct wlandevice *wlandev,
+			  struct hfa384x *hw,
+			  struct p80211msg_dot11req_mibset *msg, void *data)
+{
+	struct p80211pstrd *pstr = data;
+
+	switch (mib->did) {
+	case DIDMIB_LNX_CONFIGTABLE_RSNAIE: {
+		/*
+		 * This can never work: wpa is on the stack
+		 * and has no bytes allocated in wpa.data.
+		 */
+		struct hfa384x_wpa_data wpa;
+
+		if (isget) {
+			hfa384x_drvr_getconfig(hw,
+					       HFA384x_RID_CNFWPADATA,
+					       (u8 *)&wpa,
+					       sizeof(wpa));
+			pstr->len = 0;
+		} else {
+			wpa.datalen = 0;
+
+			hfa384x_drvr_setconfig(hw,
+					       HFA384x_RID_CNFWPADATA,
+					       (u8 *)&wpa,
+					       sizeof(wpa));
+		}
+		break;
+	}
+	default:
+		netdev_err(wlandev->netdev, "Unhandled DID 0x%08x\n", mib->did);
+	}
+
+	return 0;
+}
+
+/*
+ * prism2mgmt_pstr2bytestr
+ *
+ * Convert the pstr data in the WLAN message structure into an hfa384x
+ * byte string format.
+ *
+ * Arguments:
+ *	bytestr		hfa384x byte string data type
+ *	pstr		wlan message data
+ *
+ * Returns:
+ *	Nothing
+ *
+ */
+
+void prism2mgmt_pstr2bytestr(struct hfa384x_bytestr *bytestr,
+			     struct p80211pstrd *pstr)
+{
+	bytestr->len = cpu_to_le16((u16)(pstr->len));
+	memcpy(bytestr->data, pstr->data, pstr->len);
+}
+
+/*
+ * prism2mgmt_bytestr2pstr
+ *
+ * Convert the data in an hfa384x byte string format into a
+ * pstr in the WLAN message.
+ *
+ * Arguments:
+ *	bytestr		hfa384x byte string data type
+ *	msg		wlan message
+ *
+ * Returns:
+ *	Nothing
+ *
+ */
+
+void prism2mgmt_bytestr2pstr(struct hfa384x_bytestr *bytestr,
+			     struct p80211pstrd *pstr)
+{
+	pstr->len = (u8)(le16_to_cpu(bytestr->len));
+	memcpy(pstr->data, bytestr->data, pstr->len);
+}
+
+/*
+ * prism2mgmt_bytearea2pstr
+ *
+ * Convert the data in an hfa384x byte area format into a pstr
+ * in the WLAN message.
+ *
+ * Arguments:
+ *	bytearea	hfa384x byte area data type
+ *	msg		wlan message
+ *
+ * Returns:
+ *	Nothing
+ *
+ */
+
+void prism2mgmt_bytearea2pstr(u8 *bytearea, struct p80211pstrd *pstr, int len)
+{
+	pstr->len = (u8)len;
+	memcpy(pstr->data, bytearea, len);
+}
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
new file mode 100644
index 000000000..daa7cc4e8
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -0,0 +1,2005 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MPL-1.1)
+/*
+ *
+ * Implements the station functionality for prism2
+ *
+ * Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
+ * --------------------------------------------------------------------
+ *
+ * linux-wlan
+ *
+ *   The contents of this file are subject to the Mozilla Public
+ *   License Version 1.1 (the "License"); you may not use this file
+ *   except in compliance with the License. You may obtain a copy of
+ *   the License at http://www.mozilla.org/MPL/
+ *
+ *   Software distributed under the License is distributed on an "AS
+ *   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+ *   implied. See the License for the specific language governing
+ *   rights and limitations under the License.
+ *
+ *   Alternatively, the contents of this file may be used under the
+ *   terms of the GNU Public License version 2 (the "GPL"), in which
+ *   case the provisions of the GPL are applicable instead of the
+ *   above.  If you wish to allow the use of your version of this file
+ *   only under the terms of the GPL and not to allow others to use
+ *   your version of this file under the MPL, indicate your decision
+ *   by deleting the provisions above and replace them with the notice
+ *   and other provisions required by the GPL.  If you do not delete
+ *   the provisions above, a recipient may use your version of this
+ *   file under either the MPL or the GPL.
+ *
+ * --------------------------------------------------------------------
+ *
+ * Inquiries regarding the linux-wlan Open Source project can be
+ * made directly to:
+ *
+ * AbsoluteValue Systems Inc.
+ * info@linux-wlan.com
+ * http://www.linux-wlan.com
+ *
+ * --------------------------------------------------------------------
+ *
+ * Portions of the development of this software were funded by
+ * Intersil Corporation as part of PRISM(R) chipset product development.
+ *
+ * --------------------------------------------------------------------
+ *
+ * This file implements the module and linux pcmcia routines for the
+ * prism2 driver.
+ *
+ * --------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/wireless.h>
+#include <linux/netdevice.h>
+#include <linux/workqueue.h>
+#include <linux/byteorder/generic.h>
+#include <linux/etherdevice.h>
+
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <asm/byteorder.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/bitops.h>
+
+#include "p80211types.h"
+#include "p80211hdr.h"
+#include "p80211mgmt.h"
+#include "p80211conv.h"
+#include "p80211msg.h"
+#include "p80211netdev.h"
+#include "p80211req.h"
+#include "p80211metadef.h"
+#include "p80211metastruct.h"
+#include "hfa384x.h"
+#include "prism2mgmt.h"
+
+static char *dev_info = "prism2_usb";
+static struct wlandevice *create_wlan(void);
+
+int prism2_reset_holdtime = 30;	/* Reset hold time in ms */
+int prism2_reset_settletime = 100;	/* Reset settle time in ms */
+
+static int prism2_doreset;	/* Do a reset at init? */
+
+module_param(prism2_doreset, int, 0644);
+MODULE_PARM_DESC(prism2_doreset, "Issue a reset on initialization");
+
+module_param(prism2_reset_holdtime, int, 0644);
+MODULE_PARM_DESC(prism2_reset_holdtime, "reset hold time in ms");
+module_param(prism2_reset_settletime, int, 0644);
+MODULE_PARM_DESC(prism2_reset_settletime, "reset settle time in ms");
+
+MODULE_LICENSE("Dual MPL/GPL");
+
+static int prism2sta_open(struct wlandevice *wlandev);
+static int prism2sta_close(struct wlandevice *wlandev);
+static void prism2sta_reset(struct wlandevice *wlandev);
+static int prism2sta_txframe(struct wlandevice *wlandev, struct sk_buff *skb,
+			     struct p80211_hdr *p80211_hdr,
+			     struct p80211_metawep *p80211_wep);
+static int prism2sta_mlmerequest(struct wlandevice *wlandev,
+				 struct p80211msg *msg);
+static int prism2sta_getcardinfo(struct wlandevice *wlandev);
+static int prism2sta_globalsetup(struct wlandevice *wlandev);
+static int prism2sta_setmulticast(struct wlandevice *wlandev,
+				  struct net_device *dev);
+
+static void prism2sta_inf_handover(struct wlandevice *wlandev,
+				   struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_tallies(struct wlandevice *wlandev,
+				  struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_hostscanresults(struct wlandevice *wlandev,
+					  struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_scanresults(struct wlandevice *wlandev,
+				      struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_chinforesults(struct wlandevice *wlandev,
+					struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_linkstatus(struct wlandevice *wlandev,
+				     struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_assocstatus(struct wlandevice *wlandev,
+				      struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_authreq(struct wlandevice *wlandev,
+				  struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_authreq_defer(struct wlandevice *wlandev,
+					struct hfa384x_inf_frame *inf);
+static void prism2sta_inf_psusercnt(struct wlandevice *wlandev,
+				    struct hfa384x_inf_frame *inf);
+
+/*
+ * prism2sta_open
+ *
+ * WLAN device open method.  Called from p80211netdev when kernel
+ * device open (start) method is called in response to the
+ * SIOCSIIFFLAGS ioctl changing the flags bit IFF_UP
+ * from clear to set.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *
+ * Returns:
+ *	0	success
+ *	>0	f/w reported error
+ *	<0	driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static int prism2sta_open(struct wlandevice *wlandev)
+{
+	/* We don't currently have to do anything else.
+	 * The setup of the MAC should be subsequently completed via
+	 * the mlme commands.
+	 * Higher layers know we're ready from dev->start==1 and
+	 * dev->tbusy==0.  Our rx path knows to pass up received/
+	 * frames because of dev->flags&IFF_UP is true.
+	 */
+
+	return 0;
+}
+
+/*
+ * prism2sta_close
+ *
+ * WLAN device close method.  Called from p80211netdev when kernel
+ * device close method is called in response to the
+ * SIOCSIIFFLAGS ioctl changing the flags bit IFF_UP
+ * from set to clear.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *
+ * Returns:
+ *	0	success
+ *	>0	f/w reported error
+ *	<0	driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static int prism2sta_close(struct wlandevice *wlandev)
+{
+	/* We don't currently have to do anything else.
+	 * Higher layers know we're not ready from dev->start==0 and
+	 * dev->tbusy==1.  Our rx path knows to not pass up received
+	 * frames because of dev->flags&IFF_UP is false.
+	 */
+
+	return 0;
+}
+
+/*
+ * prism2sta_reset
+ *
+ * Currently not implemented.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	none
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static void prism2sta_reset(struct wlandevice *wlandev)
+{
+}
+
+/*
+ * prism2sta_txframe
+ *
+ * Takes a frame from p80211 and queues it for transmission.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	pb		packet buffer struct.  Contains an 802.11
+ *			data frame.
+ *       p80211_hdr      points to the 802.11 header for the packet.
+ * Returns:
+ *	0		Success and more buffs available
+ *	1		Success but no more buffs
+ *	2		Allocation failure
+ *	4		Buffer full or queue busy
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static int prism2sta_txframe(struct wlandevice *wlandev, struct sk_buff *skb,
+			     struct p80211_hdr *p80211_hdr,
+			     struct p80211_metawep *p80211_wep)
+{
+	struct hfa384x *hw = wlandev->priv;
+
+	/* If necessary, set the 802.11 WEP bit */
+	if ((wlandev->hostwep & (HOSTWEP_PRIVACYINVOKED | HOSTWEP_ENCRYPT)) ==
+	    HOSTWEP_PRIVACYINVOKED) {
+		p80211_hdr->frame_control |= cpu_to_le16(WLAN_SET_FC_ISWEP(1));
+	}
+
+	return hfa384x_drvr_txframe(hw, skb, p80211_hdr, p80211_wep);
+}
+
+/*
+ * prism2sta_mlmerequest
+ *
+ * wlan command message handler.  All we do here is pass the message
+ * over to the prism2sta_mgmt_handler.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msg		wlan command message
+ * Returns:
+ *	0		success
+ *	<0		successful acceptance of message, but we're
+ *			waiting for an async process to finish before
+ *			we're done with the msg.  When the asynch
+ *			process is done, we'll call the p80211
+ *			function p80211req_confirm() .
+ *	>0		An error occurred while we were handling
+ *			the message.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static int prism2sta_mlmerequest(struct wlandevice *wlandev,
+				 struct p80211msg *msg)
+{
+	struct hfa384x *hw = wlandev->priv;
+
+	int result = 0;
+
+	switch (msg->msgcode) {
+	case DIDMSG_DOT11REQ_MIBGET:
+		pr_debug("Received mibget request\n");
+		result = prism2mgmt_mibset_mibget(wlandev, msg);
+		break;
+	case DIDMSG_DOT11REQ_MIBSET:
+		pr_debug("Received mibset request\n");
+		result = prism2mgmt_mibset_mibget(wlandev, msg);
+		break;
+	case DIDMSG_DOT11REQ_SCAN:
+		pr_debug("Received scan request\n");
+		result = prism2mgmt_scan(wlandev, msg);
+		break;
+	case DIDMSG_DOT11REQ_SCAN_RESULTS:
+		pr_debug("Received scan_results request\n");
+		result = prism2mgmt_scan_results(wlandev, msg);
+		break;
+	case DIDMSG_DOT11REQ_START:
+		pr_debug("Received mlme start request\n");
+		result = prism2mgmt_start(wlandev, msg);
+		break;
+		/*
+		 * Prism2 specific messages
+		 */
+	case DIDMSG_P2REQ_READPDA:
+		pr_debug("Received mlme readpda request\n");
+		result = prism2mgmt_readpda(wlandev, msg);
+		break;
+	case DIDMSG_P2REQ_RAMDL_STATE:
+		pr_debug("Received mlme ramdl_state request\n");
+		result = prism2mgmt_ramdl_state(wlandev, msg);
+		break;
+	case DIDMSG_P2REQ_RAMDL_WRITE:
+		pr_debug("Received mlme ramdl_write request\n");
+		result = prism2mgmt_ramdl_write(wlandev, msg);
+		break;
+	case DIDMSG_P2REQ_FLASHDL_STATE:
+		pr_debug("Received mlme flashdl_state request\n");
+		result = prism2mgmt_flashdl_state(wlandev, msg);
+		break;
+	case DIDMSG_P2REQ_FLASHDL_WRITE:
+		pr_debug("Received mlme flashdl_write request\n");
+		result = prism2mgmt_flashdl_write(wlandev, msg);
+		break;
+		/*
+		 * Linux specific messages
+		 */
+	case DIDMSG_LNXREQ_HOSTWEP:
+		break;		/* ignore me. */
+	case DIDMSG_LNXREQ_IFSTATE: {
+		struct p80211msg_lnxreq_ifstate *ifstatemsg;
+
+		pr_debug("Received mlme ifstate request\n");
+		ifstatemsg = (struct p80211msg_lnxreq_ifstate *)msg;
+		result = prism2sta_ifstate(wlandev,
+					   ifstatemsg->ifstate.data);
+		ifstatemsg->resultcode.status =
+			P80211ENUM_msgitem_status_data_ok;
+		ifstatemsg->resultcode.data = result;
+		result = 0;
+		break;
+	}
+	case DIDMSG_LNXREQ_WLANSNIFF:
+		pr_debug("Received mlme wlansniff request\n");
+		result = prism2mgmt_wlansniff(wlandev, msg);
+		break;
+	case DIDMSG_LNXREQ_AUTOJOIN:
+		pr_debug("Received mlme autojoin request\n");
+		result = prism2mgmt_autojoin(wlandev, msg);
+		break;
+	case DIDMSG_LNXREQ_COMMSQUALITY: {
+		struct p80211msg_lnxreq_commsquality *qualmsg;
+
+		pr_debug("Received commsquality request\n");
+
+		qualmsg = (struct p80211msg_lnxreq_commsquality *)msg;
+
+		qualmsg->link.status = P80211ENUM_msgitem_status_data_ok;
+		qualmsg->level.status = P80211ENUM_msgitem_status_data_ok;
+		qualmsg->noise.status = P80211ENUM_msgitem_status_data_ok;
+
+		qualmsg->link.data = le16_to_cpu(hw->qual.cq_curr_bss);
+		qualmsg->level.data = le16_to_cpu(hw->qual.asl_curr_bss);
+		qualmsg->noise.data = le16_to_cpu(hw->qual.anl_curr_fc);
+		qualmsg->txrate.data = hw->txrate;
+
+		break;
+	}
+	default:
+		netdev_warn(wlandev->netdev,
+			    "Unknown mgmt request message 0x%08x",
+			    msg->msgcode);
+		break;
+	}
+
+	return result;
+}
+
+/*
+ * prism2sta_ifstate
+ *
+ * Interface state.  This is the primary WLAN interface enable/disable
+ * handler.  Following the driver/load/deviceprobe sequence, this
+ * function must be called with a state of "enable" before any other
+ * commands will be accepted.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	msgp		ptr to msg buffer
+ *
+ * Returns:
+ *	A p80211 message resultcode value.
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread  (usually)
+ *	interrupt
+ */
+u32 prism2sta_ifstate(struct wlandevice *wlandev, u32 ifstate)
+{
+	struct hfa384x *hw = wlandev->priv;
+	u32 result;
+
+	result = P80211ENUM_resultcode_implementation_failure;
+
+	pr_debug("Current MSD state(%d), requesting(%d)\n",
+		 wlandev->msdstate, ifstate);
+	switch (ifstate) {
+	case P80211ENUM_ifstate_fwload:
+		switch (wlandev->msdstate) {
+		case WLAN_MSD_HWPRESENT:
+			wlandev->msdstate = WLAN_MSD_FWLOAD_PENDING;
+			/*
+			 * Initialize the device+driver sufficiently
+			 * for firmware loading.
+			 */
+			result = hfa384x_drvr_start(hw);
+			if (result) {
+				netdev_err(wlandev->netdev,
+					   "hfa384x_drvr_start() failed,result=%d\n",
+					   (int)result);
+				result =
+				 P80211ENUM_resultcode_implementation_failure;
+				wlandev->msdstate = WLAN_MSD_HWPRESENT;
+				break;
+			}
+			wlandev->msdstate = WLAN_MSD_FWLOAD;
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_FWLOAD:
+			hfa384x_cmd_initialize(hw);
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_RUNNING:
+			netdev_warn(wlandev->netdev,
+				    "Cannot enter fwload state from enable state, you must disable first.\n");
+			result = P80211ENUM_resultcode_invalid_parameters;
+			break;
+		case WLAN_MSD_HWFAIL:
+		default:
+			/* probe() had a problem or the msdstate contains
+			 * an unrecognized value, there's nothing we can do.
+			 */
+			result = P80211ENUM_resultcode_implementation_failure;
+			break;
+		}
+		break;
+	case P80211ENUM_ifstate_enable:
+		switch (wlandev->msdstate) {
+		case WLAN_MSD_HWPRESENT:
+		case WLAN_MSD_FWLOAD:
+			wlandev->msdstate = WLAN_MSD_RUNNING_PENDING;
+			/* Initialize the device+driver for full
+			 * operation. Note that this might me an FWLOAD
+			 * to RUNNING transition so we must not do a chip
+			 * or board level reset.  Note that on failure,
+			 * the MSD state is set to HWPRESENT because we
+			 * can't make any assumptions about the state
+			 * of the hardware or a previous firmware load.
+			 */
+			result = hfa384x_drvr_start(hw);
+			if (result) {
+				netdev_err(wlandev->netdev,
+					   "hfa384x_drvr_start() failed,result=%d\n",
+					   (int)result);
+				result =
+				  P80211ENUM_resultcode_implementation_failure;
+				wlandev->msdstate = WLAN_MSD_HWPRESENT;
+				break;
+			}
+
+			result = prism2sta_getcardinfo(wlandev);
+			if (result) {
+				netdev_err(wlandev->netdev,
+					   "prism2sta_getcardinfo() failed,result=%d\n",
+					   (int)result);
+				result =
+				  P80211ENUM_resultcode_implementation_failure;
+				hfa384x_drvr_stop(hw);
+				wlandev->msdstate = WLAN_MSD_HWPRESENT;
+				break;
+			}
+			result = prism2sta_globalsetup(wlandev);
+			if (result) {
+				netdev_err(wlandev->netdev,
+					   "prism2sta_globalsetup() failed,result=%d\n",
+					   (int)result);
+				result =
+				  P80211ENUM_resultcode_implementation_failure;
+				hfa384x_drvr_stop(hw);
+				wlandev->msdstate = WLAN_MSD_HWPRESENT;
+				break;
+			}
+			wlandev->msdstate = WLAN_MSD_RUNNING;
+			hw->join_ap = 0;
+			hw->join_retries = 60;
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_RUNNING:
+			/* Do nothing, we're already in this state. */
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_HWFAIL:
+		default:
+			/* probe() had a problem or the msdstate contains
+			 * an unrecognized value, there's nothing we can do.
+			 */
+			result = P80211ENUM_resultcode_implementation_failure;
+			break;
+		}
+		break;
+	case P80211ENUM_ifstate_disable:
+		switch (wlandev->msdstate) {
+		case WLAN_MSD_HWPRESENT:
+			/* Do nothing, we're already in this state. */
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_FWLOAD:
+		case WLAN_MSD_RUNNING:
+			wlandev->msdstate = WLAN_MSD_HWPRESENT_PENDING;
+			/*
+			 * TODO: Shut down the MAC completely. Here a chip
+			 * or board level reset is probably called for.
+			 * After a "disable" _all_ results are lost, even
+			 * those from a fwload.
+			 */
+			if (!wlandev->hwremoved)
+				netif_carrier_off(wlandev->netdev);
+
+			hfa384x_drvr_stop(hw);
+
+			wlandev->macmode = WLAN_MACMODE_NONE;
+			wlandev->msdstate = WLAN_MSD_HWPRESENT;
+			result = P80211ENUM_resultcode_success;
+			break;
+		case WLAN_MSD_HWFAIL:
+		default:
+			/* probe() had a problem or the msdstate contains
+			 * an unrecognized value, there's nothing we can do.
+			 */
+			result = P80211ENUM_resultcode_implementation_failure;
+			break;
+		}
+		break;
+	default:
+		result = P80211ENUM_resultcode_invalid_parameters;
+		break;
+	}
+
+	return result;
+}
+
+/*
+ * prism2sta_getcardinfo
+ *
+ * Collect the NICID, firmware version and any other identifiers
+ * we'd like to have in host-side data structures.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *
+ * Returns:
+ *	0	success
+ *	>0	f/w reported error
+ *	<0	driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	Either.
+ */
+static int prism2sta_getcardinfo(struct wlandevice *wlandev)
+{
+	int result = 0;
+	struct hfa384x *hw = wlandev->priv;
+	u16 temp;
+	u8 snum[HFA384x_RID_NICSERIALNUMBER_LEN];
+	u8 addr[ETH_ALEN];
+
+	/* Collect version and compatibility info */
+	/*  Some are critical, some are not */
+	/* NIC identity */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_NICIDENTITY,
+					&hw->ident_nic,
+					sizeof(struct hfa384x_compident));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve NICIDENTITY\n");
+		goto failed;
+	}
+
+	/* get all the nic id fields in host byte order */
+	le16_to_cpus(&hw->ident_nic.id);
+	le16_to_cpus(&hw->ident_nic.variant);
+	le16_to_cpus(&hw->ident_nic.major);
+	le16_to_cpus(&hw->ident_nic.minor);
+
+	netdev_info(wlandev->netdev, "ident: nic h/w: id=0x%02x %d.%d.%d\n",
+		    hw->ident_nic.id, hw->ident_nic.major,
+		    hw->ident_nic.minor, hw->ident_nic.variant);
+
+	/* Primary f/w identity */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_PRIIDENTITY,
+					&hw->ident_pri_fw,
+					sizeof(struct hfa384x_compident));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve PRIIDENTITY\n");
+		goto failed;
+	}
+
+	/* get all the private fw id fields in host byte order */
+	le16_to_cpus(&hw->ident_pri_fw.id);
+	le16_to_cpus(&hw->ident_pri_fw.variant);
+	le16_to_cpus(&hw->ident_pri_fw.major);
+	le16_to_cpus(&hw->ident_pri_fw.minor);
+
+	netdev_info(wlandev->netdev, "ident: pri f/w: id=0x%02x %d.%d.%d\n",
+		    hw->ident_pri_fw.id, hw->ident_pri_fw.major,
+		    hw->ident_pri_fw.minor, hw->ident_pri_fw.variant);
+
+	/* Station (Secondary?) f/w identity */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_STAIDENTITY,
+					&hw->ident_sta_fw,
+					sizeof(struct hfa384x_compident));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve STAIDENTITY\n");
+		goto failed;
+	}
+
+	if (hw->ident_nic.id < 0x8000) {
+		netdev_err(wlandev->netdev,
+			   "FATAL: Card is not an Intersil Prism2/2.5/3\n");
+		result = -1;
+		goto failed;
+	}
+
+	/* get all the station fw id fields in host byte order */
+	le16_to_cpus(&hw->ident_sta_fw.id);
+	le16_to_cpus(&hw->ident_sta_fw.variant);
+	le16_to_cpus(&hw->ident_sta_fw.major);
+	le16_to_cpus(&hw->ident_sta_fw.minor);
+
+	/* strip out the 'special' variant bits */
+	hw->mm_mods = hw->ident_sta_fw.variant & GENMASK(15, 14);
+	hw->ident_sta_fw.variant &= ~((u16)GENMASK(15, 14));
+
+	if (hw->ident_sta_fw.id == 0x1f) {
+		netdev_info(wlandev->netdev,
+			    "ident: sta f/w: id=0x%02x %d.%d.%d\n",
+			    hw->ident_sta_fw.id, hw->ident_sta_fw.major,
+			    hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
+	} else {
+		netdev_info(wlandev->netdev,
+			    "ident:  ap f/w: id=0x%02x %d.%d.%d\n",
+			    hw->ident_sta_fw.id, hw->ident_sta_fw.major,
+			    hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
+		netdev_err(wlandev->netdev, "Unsupported Tertiary AP firmware loaded!\n");
+		goto failed;
+	}
+
+	/* Compatibility range, Modem supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_MFISUPRANGE,
+					&hw->cap_sup_mfi,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve MFISUPRANGE\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, modem interface supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_sup_mfi.role);
+	le16_to_cpus(&hw->cap_sup_mfi.id);
+	le16_to_cpus(&hw->cap_sup_mfi.variant);
+	le16_to_cpus(&hw->cap_sup_mfi.bottom);
+	le16_to_cpus(&hw->cap_sup_mfi.top);
+
+	netdev_info(wlandev->netdev,
+		    "MFI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_sup_mfi.role, hw->cap_sup_mfi.id,
+		    hw->cap_sup_mfi.variant, hw->cap_sup_mfi.bottom,
+		    hw->cap_sup_mfi.top);
+
+	/* Compatibility range, Controller supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CFISUPRANGE,
+					&hw->cap_sup_cfi,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve CFISUPRANGE\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, controller interface supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_sup_cfi.role);
+	le16_to_cpus(&hw->cap_sup_cfi.id);
+	le16_to_cpus(&hw->cap_sup_cfi.variant);
+	le16_to_cpus(&hw->cap_sup_cfi.bottom);
+	le16_to_cpus(&hw->cap_sup_cfi.top);
+
+	netdev_info(wlandev->netdev,
+		    "CFI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_sup_cfi.role, hw->cap_sup_cfi.id,
+		    hw->cap_sup_cfi.variant, hw->cap_sup_cfi.bottom,
+		    hw->cap_sup_cfi.top);
+
+	/* Compatibility range, Primary f/w supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_PRISUPRANGE,
+					&hw->cap_sup_pri,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve PRISUPRANGE\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, primary firmware supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_sup_pri.role);
+	le16_to_cpus(&hw->cap_sup_pri.id);
+	le16_to_cpus(&hw->cap_sup_pri.variant);
+	le16_to_cpus(&hw->cap_sup_pri.bottom);
+	le16_to_cpus(&hw->cap_sup_pri.top);
+
+	netdev_info(wlandev->netdev,
+		    "PRI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_sup_pri.role, hw->cap_sup_pri.id,
+		    hw->cap_sup_pri.variant, hw->cap_sup_pri.bottom,
+		    hw->cap_sup_pri.top);
+
+	/* Compatibility range, Station f/w supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_STASUPRANGE,
+					&hw->cap_sup_sta,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve STASUPRANGE\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, station firmware supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_sup_sta.role);
+	le16_to_cpus(&hw->cap_sup_sta.id);
+	le16_to_cpus(&hw->cap_sup_sta.variant);
+	le16_to_cpus(&hw->cap_sup_sta.bottom);
+	le16_to_cpus(&hw->cap_sup_sta.top);
+
+	if (hw->cap_sup_sta.id == 0x04) {
+		netdev_info(wlandev->netdev,
+			    "STA:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+			    hw->cap_sup_sta.role, hw->cap_sup_sta.id,
+			    hw->cap_sup_sta.variant, hw->cap_sup_sta.bottom,
+			    hw->cap_sup_sta.top);
+	} else {
+		netdev_info(wlandev->netdev,
+			    "AP:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+			    hw->cap_sup_sta.role, hw->cap_sup_sta.id,
+			    hw->cap_sup_sta.variant, hw->cap_sup_sta.bottom,
+			    hw->cap_sup_sta.top);
+	}
+
+	/* Compatibility range, primary f/w actor, CFI supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_PRI_CFIACTRANGES,
+					&hw->cap_act_pri_cfi,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve PRI_CFIACTRANGES\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, primary f/w actor, CFI supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_act_pri_cfi.role);
+	le16_to_cpus(&hw->cap_act_pri_cfi.id);
+	le16_to_cpus(&hw->cap_act_pri_cfi.variant);
+	le16_to_cpus(&hw->cap_act_pri_cfi.bottom);
+	le16_to_cpus(&hw->cap_act_pri_cfi.top);
+
+	netdev_info(wlandev->netdev,
+		    "PRI-CFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_act_pri_cfi.role, hw->cap_act_pri_cfi.id,
+		    hw->cap_act_pri_cfi.variant, hw->cap_act_pri_cfi.bottom,
+		    hw->cap_act_pri_cfi.top);
+
+	/* Compatibility range, sta f/w actor, CFI supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_STA_CFIACTRANGES,
+					&hw->cap_act_sta_cfi,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve STA_CFIACTRANGES\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, station f/w actor, CFI supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_act_sta_cfi.role);
+	le16_to_cpus(&hw->cap_act_sta_cfi.id);
+	le16_to_cpus(&hw->cap_act_sta_cfi.variant);
+	le16_to_cpus(&hw->cap_act_sta_cfi.bottom);
+	le16_to_cpus(&hw->cap_act_sta_cfi.top);
+
+	netdev_info(wlandev->netdev,
+		    "STA-CFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_act_sta_cfi.role, hw->cap_act_sta_cfi.id,
+		    hw->cap_act_sta_cfi.variant, hw->cap_act_sta_cfi.bottom,
+		    hw->cap_act_sta_cfi.top);
+
+	/* Compatibility range, sta f/w actor, MFI supplier */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_STA_MFIACTRANGES,
+					&hw->cap_act_sta_mfi,
+					sizeof(struct hfa384x_caplevel));
+	if (result) {
+		netdev_err(wlandev->netdev, "Failed to retrieve STA_MFIACTRANGES\n");
+		goto failed;
+	}
+
+	/* get all the Compatibility range, station f/w actor, MFI supplier
+	 * fields in byte order
+	 */
+	le16_to_cpus(&hw->cap_act_sta_mfi.role);
+	le16_to_cpus(&hw->cap_act_sta_mfi.id);
+	le16_to_cpus(&hw->cap_act_sta_mfi.variant);
+	le16_to_cpus(&hw->cap_act_sta_mfi.bottom);
+	le16_to_cpus(&hw->cap_act_sta_mfi.top);
+
+	netdev_info(wlandev->netdev,
+		    "STA-MFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
+		    hw->cap_act_sta_mfi.role, hw->cap_act_sta_mfi.id,
+		    hw->cap_act_sta_mfi.variant, hw->cap_act_sta_mfi.bottom,
+		    hw->cap_act_sta_mfi.top);
+
+	/* Serial Number */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_NICSERIALNUMBER,
+					snum, HFA384x_RID_NICSERIALNUMBER_LEN);
+	if (!result) {
+		netdev_info(wlandev->netdev, "Prism2 card SN: %*pE\n",
+			    HFA384x_RID_NICSERIALNUMBER_LEN, snum);
+	} else {
+		netdev_err(wlandev->netdev, "Failed to retrieve Prism2 Card SN\n");
+		goto failed;
+	}
+
+	/* Collect the MAC address */
+	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CNFOWNMACADDR,
+					addr, ETH_ALEN);
+	if (result != 0) {
+		netdev_err(wlandev->netdev, "Failed to retrieve mac address\n");
+		goto failed;
+	}
+	eth_hw_addr_set(wlandev->netdev, addr);
+
+	/* short preamble is always implemented */
+	wlandev->nsdcaps |= P80211_NSDCAP_SHORT_PREAMBLE;
+
+	/* find out if hardware wep is implemented */
+	hfa384x_drvr_getconfig16(hw, HFA384x_RID_PRIVACYOPTIMP, &temp);
+	if (temp)
+		wlandev->nsdcaps |= P80211_NSDCAP_HARDWAREWEP;
+
+	/* get the dBm Scaling constant */
+	hfa384x_drvr_getconfig16(hw, HFA384x_RID_CNFDBMADJUST, &temp);
+	hw->dbmadjust = temp;
+
+	/* Only enable scan by default on newer firmware */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) <
+	    HFA384x_FIRMWARE_VERSION(1, 5, 5)) {
+		wlandev->nsdcaps |= P80211_NSDCAP_NOSCAN;
+	}
+
+	/* TODO: Set any internally managed config items */
+
+	goto done;
+failed:
+	netdev_err(wlandev->netdev, "Failed, result=%d\n", result);
+done:
+	return result;
+}
+
+/*
+ * prism2sta_globalsetup
+ *
+ * Set any global RIDs that we want to set at device activation.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *
+ * Returns:
+ *	0	success
+ *	>0	f/w reported error
+ *	<0	driver reported error
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	process thread
+ */
+static int prism2sta_globalsetup(struct wlandevice *wlandev)
+{
+	struct hfa384x *hw = wlandev->priv;
+
+	/* Set the maximum frame size */
+	return hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN,
+					WLAN_DATA_MAXLEN);
+}
+
+static int prism2sta_setmulticast(struct wlandevice *wlandev,
+				  struct net_device *dev)
+{
+	int result = 0;
+	struct hfa384x *hw = wlandev->priv;
+
+	u16 promisc;
+
+	/* If we're not ready, what's the point? */
+	if (hw->state != HFA384x_STATE_RUNNING)
+		goto exit;
+
+	if ((dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) != 0)
+		promisc = P80211ENUM_truth_true;
+	else
+		promisc = P80211ENUM_truth_false;
+
+	result =
+	    hfa384x_drvr_setconfig16_async(hw, HFA384x_RID_PROMISCMODE,
+					   promisc);
+exit:
+	return result;
+}
+
+/*
+ * prism2sta_inf_handover
+ *
+ * Handles the receipt of a Handover info frame. Should only be present
+ * in APs only.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_handover(struct wlandevice *wlandev,
+				   struct hfa384x_inf_frame *inf)
+{
+	pr_debug("received infoframe:HANDOVER (unhandled)\n");
+}
+
+/*
+ * prism2sta_inf_tallies
+ *
+ * Handles the receipt of a CommTallies info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_tallies(struct wlandevice *wlandev,
+				  struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	__le16 *src16;
+	u32 *dst;
+	__le32 *src32;
+	int i;
+	int cnt;
+
+	/*
+	 * Determine if these are 16-bit or 32-bit tallies, based on the
+	 * record length of the info record.
+	 */
+
+	cnt = sizeof(struct hfa384x_comm_tallies_32) / sizeof(u32);
+	if (inf->framelen > 22) {
+		dst = (u32 *)&hw->tallies;
+		src32 = (__le32 *)&inf->info.commtallies32;
+		for (i = 0; i < cnt; i++, dst++, src32++)
+			*dst += le32_to_cpu(*src32);
+	} else {
+		dst = (u32 *)&hw->tallies;
+		src16 = (__le16 *)&inf->info.commtallies16;
+		for (i = 0; i < cnt; i++, dst++, src16++)
+			*dst += le16_to_cpu(*src16);
+	}
+}
+
+/*
+ * prism2sta_inf_scanresults
+ *
+ * Handles the receipt of a Scan Results info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_scanresults(struct wlandevice *wlandev,
+				      struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	int nbss;
+	struct hfa384x_scan_result *sr = &inf->info.scanresult;
+	int i;
+	struct hfa384x_join_request_data joinreq;
+	int result;
+
+	/* Get the number of results, first in bytes, then in results */
+	nbss = (inf->framelen * sizeof(u16)) -
+	    sizeof(inf->infotype) - sizeof(inf->info.scanresult.scanreason);
+	nbss /= sizeof(struct hfa384x_scan_result_sub);
+
+	/* Print em */
+	pr_debug("rx scanresults, reason=%d, nbss=%d:\n",
+		 inf->info.scanresult.scanreason, nbss);
+	for (i = 0; i < nbss; i++) {
+		pr_debug("chid=%d anl=%d sl=%d bcnint=%d\n",
+			 sr->result[i].chid,
+			 sr->result[i].anl,
+			 sr->result[i].sl, sr->result[i].bcnint);
+		pr_debug("  capinfo=0x%04x proberesp_rate=%d\n",
+			 sr->result[i].capinfo, sr->result[i].proberesp_rate);
+	}
+	/* issue a join request */
+	joinreq.channel = sr->result[0].chid;
+	memcpy(joinreq.bssid, sr->result[0].bssid, WLAN_BSSID_LEN);
+	result = hfa384x_drvr_setconfig(hw,
+					HFA384x_RID_JOINREQUEST,
+					&joinreq, HFA384x_RID_JOINREQUEST_LEN);
+	if (result) {
+		netdev_err(wlandev->netdev, "setconfig(joinreq) failed, result=%d\n",
+			   result);
+	}
+}
+
+/*
+ * prism2sta_inf_hostscanresults
+ *
+ * Handles the receipt of a Scan Results info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_hostscanresults(struct wlandevice *wlandev,
+					  struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	int nbss;
+
+	nbss = (inf->framelen - 3) / 32;
+	pr_debug("Received %d hostscan results\n", nbss);
+
+	if (nbss > 32)
+		nbss = 32;
+
+	kfree(hw->scanresults);
+
+	hw->scanresults = kmemdup(inf, sizeof(*inf), GFP_ATOMIC);
+
+	if (nbss == 0)
+		nbss = -1;
+
+	/* Notify/wake the sleeping caller. */
+	hw->scanflag = nbss;
+	wake_up_interruptible(&hw->cmdq);
+};
+
+/*
+ * prism2sta_inf_chinforesults
+ *
+ * Handles the receipt of a Channel Info Results info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_chinforesults(struct wlandevice *wlandev,
+					struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	unsigned int i, n;
+
+	hw->channel_info.results.scanchannels =
+	    inf->info.chinforesult.scanchannels;
+
+	for (i = 0, n = 0; i < HFA384x_CHINFORESULT_MAX; i++) {
+		struct hfa384x_ch_info_result_sub *result;
+		struct hfa384x_ch_info_result_sub *chinforesult;
+		int chan;
+
+		if (!(hw->channel_info.results.scanchannels & (1 << i)))
+			continue;
+
+		result = &inf->info.chinforesult.result[n];
+		chan = result->chid - 1;
+
+		if (chan < 0 || chan >= HFA384x_CHINFORESULT_MAX)
+			continue;
+
+		chinforesult = &hw->channel_info.results.result[chan];
+		chinforesult->chid = chan;
+		chinforesult->anl = result->anl;
+		chinforesult->pnl = result->pnl;
+		chinforesult->active = result->active;
+
+		pr_debug("chinfo: channel %d, %s level (avg/peak)=%d/%d dB, pcf %d\n",
+			 chan + 1,
+			 (chinforesult->active & HFA384x_CHINFORESULT_BSSACTIVE)
+				? "signal" : "noise",
+			 chinforesult->anl, chinforesult->pnl,
+			 (chinforesult->active & HFA384x_CHINFORESULT_PCFACTIVE)
+				? 1 : 0);
+		n++;
+	}
+	atomic_set(&hw->channel_info.done, 2);
+
+	hw->channel_info.count = n;
+}
+
+void prism2sta_processing_defer(struct work_struct *data)
+{
+	struct hfa384x *hw = container_of(data, struct hfa384x, link_bh);
+	struct wlandevice *wlandev = hw->wlandev;
+	struct hfa384x_bytestr32 ssid;
+	int result;
+
+	/* First let's process the auth frames */
+	{
+		struct sk_buff *skb;
+		struct hfa384x_inf_frame *inf;
+
+		while ((skb = skb_dequeue(&hw->authq))) {
+			inf = (struct hfa384x_inf_frame *)skb->data;
+			prism2sta_inf_authreq_defer(wlandev, inf);
+		}
+	}
+
+	/* Now let's handle the linkstatus stuff */
+	if (hw->link_status == hw->link_status_new)
+		return;
+
+	hw->link_status = hw->link_status_new;
+
+	switch (hw->link_status) {
+	case HFA384x_LINK_NOTCONNECTED:
+		/* I'm currently assuming that this is the initial link
+		 * state.  It should only be possible immediately
+		 * following an Enable command.
+		 * Response:
+		 * Block Transmits, Ignore receives of data frames
+		 */
+		netif_carrier_off(wlandev->netdev);
+
+		netdev_info(wlandev->netdev, "linkstatus=NOTCONNECTED (unhandled)\n");
+		break;
+
+	case HFA384x_LINK_CONNECTED:
+		/* This one indicates a successful scan/join/auth/assoc.
+		 * When we have the full MLME complement, this event will
+		 * signify successful completion of both mlme_authenticate
+		 * and mlme_associate.  State management will get a little
+		 * ugly here.
+		 * Response:
+		 * Indicate authentication and/or association
+		 * Enable Transmits, Receives and pass up data frames
+		 */
+
+		netif_carrier_on(wlandev->netdev);
+
+		/* If we are joining a specific AP, set our
+		 * state and reset retries
+		 */
+		if (hw->join_ap == 1)
+			hw->join_ap = 2;
+		hw->join_retries = 60;
+
+		/* Don't call this in monitor mode */
+		if (wlandev->netdev->type == ARPHRD_ETHER) {
+			u16 portstatus;
+
+			netdev_info(wlandev->netdev, "linkstatus=CONNECTED\n");
+
+			/* For non-usb devices, we can use the sync versions */
+			/* Collect the BSSID, and set state to allow tx */
+
+			result = hfa384x_drvr_getconfig(hw,
+							HFA384x_RID_CURRENTBSSID,
+							wlandev->bssid,
+							WLAN_BSSID_LEN);
+			if (result) {
+				pr_debug
+				    ("getconfig(0x%02x) failed, result = %d\n",
+				     HFA384x_RID_CURRENTBSSID, result);
+				return;
+			}
+
+			result = hfa384x_drvr_getconfig(hw,
+							HFA384x_RID_CURRENTSSID,
+							&ssid, sizeof(ssid));
+			if (result) {
+				pr_debug
+				    ("getconfig(0x%02x) failed, result = %d\n",
+				     HFA384x_RID_CURRENTSSID, result);
+				return;
+			}
+			prism2mgmt_bytestr2pstr((struct hfa384x_bytestr *)&ssid,
+						(struct p80211pstrd *)&wlandev->ssid);
+
+			/* Collect the port status */
+			result = hfa384x_drvr_getconfig16(hw,
+							  HFA384x_RID_PORTSTATUS,
+							  &portstatus);
+			if (result) {
+				pr_debug
+				    ("getconfig(0x%02x) failed, result = %d\n",
+				     HFA384x_RID_PORTSTATUS, result);
+				return;
+			}
+			wlandev->macmode =
+			    (portstatus == HFA384x_PSTATUS_CONN_IBSS) ?
+			    WLAN_MACMODE_IBSS_STA : WLAN_MACMODE_ESS_STA;
+
+			/* signal back up to cfg80211 layer */
+			prism2_connect_result(wlandev, P80211ENUM_truth_false);
+
+			/* Get the ball rolling on the comms quality stuff */
+			prism2sta_commsqual_defer(&hw->commsqual_bh);
+		}
+		break;
+
+	case HFA384x_LINK_DISCONNECTED:
+		/* This one indicates that our association is gone.  We've
+		 * lost connection with the AP and/or been disassociated.
+		 * This indicates that the MAC has completely cleared it's
+		 * associated state.  We * should send a deauth indication
+		 * (implying disassoc) up * to the MLME.
+		 * Response:
+		 * Indicate Deauthentication
+		 * Block Transmits, Ignore receives of data frames
+		 */
+		if (wlandev->netdev->type == ARPHRD_ETHER)
+			netdev_info(wlandev->netdev,
+				    "linkstatus=DISCONNECTED (unhandled)\n");
+		wlandev->macmode = WLAN_MACMODE_NONE;
+
+		netif_carrier_off(wlandev->netdev);
+
+		/* signal back up to cfg80211 layer */
+		prism2_disconnected(wlandev);
+
+		break;
+
+	case HFA384x_LINK_AP_CHANGE:
+		/* This one indicates that the MAC has decided to and
+		 * successfully completed a change to another AP.  We
+		 * should probably implement a reassociation indication
+		 * in response to this one.  I'm thinking that the
+		 * p80211 layer needs to be notified in case of
+		 * buffering/queueing issues.  User mode also needs to be
+		 * notified so that any BSS dependent elements can be
+		 * updated.
+		 * associated state.  We * should send a deauth indication
+		 * (implying disassoc) up * to the MLME.
+		 * Response:
+		 * Indicate Reassociation
+		 * Enable Transmits, Receives and pass up data frames
+		 */
+		netdev_info(wlandev->netdev, "linkstatus=AP_CHANGE\n");
+
+		result = hfa384x_drvr_getconfig(hw,
+						HFA384x_RID_CURRENTBSSID,
+						wlandev->bssid, WLAN_BSSID_LEN);
+		if (result) {
+			pr_debug("getconfig(0x%02x) failed, result = %d\n",
+				 HFA384x_RID_CURRENTBSSID, result);
+			return;
+		}
+
+		result = hfa384x_drvr_getconfig(hw,
+						HFA384x_RID_CURRENTSSID,
+						&ssid, sizeof(ssid));
+		if (result) {
+			pr_debug("getconfig(0x%02x) failed, result = %d\n",
+				 HFA384x_RID_CURRENTSSID, result);
+			return;
+		}
+		prism2mgmt_bytestr2pstr((struct hfa384x_bytestr *)&ssid,
+					(struct p80211pstrd *)&wlandev->ssid);
+
+		hw->link_status = HFA384x_LINK_CONNECTED;
+		netif_carrier_on(wlandev->netdev);
+
+		/* signal back up to cfg80211 layer */
+		prism2_roamed(wlandev);
+
+		break;
+
+	case HFA384x_LINK_AP_OUTOFRANGE:
+		/* This one indicates that the MAC has decided that the
+		 * AP is out of range, but hasn't found a better candidate
+		 * so the MAC maintains its "associated" state in case
+		 * we get back in range.  We should block transmits and
+		 * receives in this state.  Do we need an indication here?
+		 * Probably not since a polling user-mode element would
+		 * get this status from p2PortStatus(FD40). What about
+		 * p80211?
+		 * Response:
+		 * Block Transmits, Ignore receives of data frames
+		 */
+		netdev_info(wlandev->netdev, "linkstatus=AP_OUTOFRANGE (unhandled)\n");
+
+		netif_carrier_off(wlandev->netdev);
+
+		break;
+
+	case HFA384x_LINK_AP_INRANGE:
+		/* This one indicates that the MAC has decided that the
+		 * AP is back in range.  We continue working with our
+		 * existing association.
+		 * Response:
+		 * Enable Transmits, Receives and pass up data frames
+		 */
+		netdev_info(wlandev->netdev, "linkstatus=AP_INRANGE\n");
+
+		hw->link_status = HFA384x_LINK_CONNECTED;
+		netif_carrier_on(wlandev->netdev);
+
+		break;
+
+	case HFA384x_LINK_ASSOCFAIL:
+		/* This one is actually a peer to CONNECTED.  We've
+		 * requested a join for a given SSID and optionally BSSID.
+		 * We can use this one to indicate authentication and
+		 * association failures.  The trick is going to be
+		 * 1) identifying the failure, and 2) state management.
+		 * Response:
+		 * Disable Transmits, Ignore receives of data frames
+		 */
+		if (hw->join_ap && --hw->join_retries > 0) {
+			struct hfa384x_join_request_data joinreq;
+
+			joinreq = hw->joinreq;
+			/* Send the join request */
+			hfa384x_drvr_setconfig(hw,
+					       HFA384x_RID_JOINREQUEST,
+					       &joinreq,
+					       HFA384x_RID_JOINREQUEST_LEN);
+			netdev_info(wlandev->netdev,
+				    "linkstatus=ASSOCFAIL (re-submitting join)\n");
+		} else {
+			netdev_info(wlandev->netdev, "linkstatus=ASSOCFAIL (unhandled)\n");
+		}
+
+		netif_carrier_off(wlandev->netdev);
+
+		/* signal back up to cfg80211 layer */
+		prism2_connect_result(wlandev, P80211ENUM_truth_true);
+
+		break;
+
+	default:
+		/* This is bad, IO port problems? */
+		netdev_warn(wlandev->netdev,
+			    "unknown linkstatus=0x%02x\n", hw->link_status);
+		return;
+	}
+
+	wlandev->linkstatus = (hw->link_status == HFA384x_LINK_CONNECTED);
+}
+
+/*
+ * prism2sta_inf_linkstatus
+ *
+ * Handles the receipt of a Link Status info frame.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_linkstatus(struct wlandevice *wlandev,
+				     struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+
+	hw->link_status_new = le16_to_cpu(inf->info.linkstatus.linkstatus);
+
+	schedule_work(&hw->link_bh);
+}
+
+/*
+ * prism2sta_inf_assocstatus
+ *
+ * Handles the receipt of an Association Status info frame. Should
+ * be present in APs only.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_assocstatus(struct wlandevice *wlandev,
+				      struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct hfa384x_assoc_status rec;
+	int i;
+
+	memcpy(&rec, &inf->info.assocstatus, sizeof(rec));
+	le16_to_cpus(&rec.assocstatus);
+	le16_to_cpus(&rec.reason);
+
+	/*
+	 * Find the address in the list of authenticated stations.
+	 * If it wasn't found, then this address has not been previously
+	 * authenticated and something weird has happened if this is
+	 * anything other than an "authentication failed" message.
+	 * If the address was found, then set the "associated" flag for
+	 * that station, based on whether the station is associating or
+	 * losing its association.  Something weird has also happened
+	 * if we find the address in the list of authenticated stations
+	 * but we are getting an "authentication failed" message.
+	 */
+
+	for (i = 0; i < hw->authlist.cnt; i++)
+		if (ether_addr_equal(rec.sta_addr, hw->authlist.addr[i]))
+			break;
+
+	if (i >= hw->authlist.cnt) {
+		if (rec.assocstatus != HFA384x_ASSOCSTATUS_AUTHFAIL)
+			netdev_warn(wlandev->netdev,
+				    "assocstatus info frame received for non-authenticated station.\n");
+	} else {
+		hw->authlist.assoc[i] =
+		    (rec.assocstatus == HFA384x_ASSOCSTATUS_STAASSOC ||
+		     rec.assocstatus == HFA384x_ASSOCSTATUS_REASSOC);
+
+		if (rec.assocstatus == HFA384x_ASSOCSTATUS_AUTHFAIL)
+			netdev_warn(wlandev->netdev,
+				    "authfail assocstatus info frame received for authenticated station.\n");
+	}
+}
+
+/*
+ * prism2sta_inf_authreq
+ *
+ * Handles the receipt of an Authentication Request info frame. Should
+ * be present in APs only.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ *
+ */
+static void prism2sta_inf_authreq(struct wlandevice *wlandev,
+				  struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct sk_buff *skb;
+
+	skb = dev_alloc_skb(sizeof(*inf));
+	if (skb) {
+		skb_put(skb, sizeof(*inf));
+		memcpy(skb->data, inf, sizeof(*inf));
+		skb_queue_tail(&hw->authq, skb);
+		schedule_work(&hw->link_bh);
+	}
+}
+
+static void prism2sta_inf_authreq_defer(struct wlandevice *wlandev,
+					struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+	struct hfa384x_authenticate_station_data rec;
+
+	int i, added, result, cnt;
+	u8 *addr;
+
+	/*
+	 * Build the AuthenticateStation record.  Initialize it for denying
+	 * authentication.
+	 */
+
+	ether_addr_copy(rec.address, inf->info.authreq.sta_addr);
+	rec.status = cpu_to_le16(P80211ENUM_status_unspec_failure);
+
+	/*
+	 * Authenticate based on the access mode.
+	 */
+
+	switch (hw->accessmode) {
+	case WLAN_ACCESS_NONE:
+
+		/*
+		 * Deny all new authentications.  However, if a station
+		 * is ALREADY authenticated, then accept it.
+		 */
+
+		for (i = 0; i < hw->authlist.cnt; i++)
+			if (ether_addr_equal(rec.address,
+					     hw->authlist.addr[i])) {
+				rec.status = cpu_to_le16(P80211ENUM_status_successful);
+				break;
+			}
+
+		break;
+
+	case WLAN_ACCESS_ALL:
+
+		/*
+		 * Allow all authentications.
+		 */
+
+		rec.status = cpu_to_le16(P80211ENUM_status_successful);
+		break;
+
+	case WLAN_ACCESS_ALLOW:
+
+		/*
+		 * Only allow the authentication if the MAC address
+		 * is in the list of allowed addresses.
+		 *
+		 * Since this is the interrupt handler, we may be here
+		 * while the access list is in the middle of being
+		 * updated.  Choose the list which is currently okay.
+		 * See "prism2mib_priv_accessallow()" for details.
+		 */
+
+		if (hw->allow.modify == 0) {
+			cnt = hw->allow.cnt;
+			addr = hw->allow.addr[0];
+		} else {
+			cnt = hw->allow.cnt1;
+			addr = hw->allow.addr1[0];
+		}
+
+		for (i = 0; i < cnt; i++, addr += ETH_ALEN)
+			if (ether_addr_equal(rec.address, addr)) {
+				rec.status = cpu_to_le16(P80211ENUM_status_successful);
+				break;
+			}
+
+		break;
+
+	case WLAN_ACCESS_DENY:
+
+		/*
+		 * Allow the authentication UNLESS the MAC address is
+		 * in the list of denied addresses.
+		 *
+		 * Since this is the interrupt handler, we may be here
+		 * while the access list is in the middle of being
+		 * updated.  Choose the list which is currently okay.
+		 * See "prism2mib_priv_accessdeny()" for details.
+		 */
+
+		if (hw->deny.modify == 0) {
+			cnt = hw->deny.cnt;
+			addr = hw->deny.addr[0];
+		} else {
+			cnt = hw->deny.cnt1;
+			addr = hw->deny.addr1[0];
+		}
+
+		rec.status = cpu_to_le16(P80211ENUM_status_successful);
+
+		for (i = 0; i < cnt; i++, addr += ETH_ALEN)
+			if (ether_addr_equal(rec.address, addr)) {
+				rec.status = cpu_to_le16(P80211ENUM_status_unspec_failure);
+				break;
+			}
+
+		break;
+	}
+
+	/*
+	 * If the authentication is okay, then add the MAC address to the
+	 * list of authenticated stations.  Don't add the address if it
+	 * is already in the list. (802.11b does not seem to disallow
+	 * a station from issuing an authentication request when the
+	 * station is already authenticated. Does this sort of thing
+	 * ever happen?  We might as well do the check just in case.)
+	 */
+
+	added = 0;
+
+	if (rec.status == cpu_to_le16(P80211ENUM_status_successful)) {
+		for (i = 0; i < hw->authlist.cnt; i++)
+			if (ether_addr_equal(rec.address,
+					     hw->authlist.addr[i]))
+				break;
+
+		if (i >= hw->authlist.cnt) {
+			if (hw->authlist.cnt >= WLAN_AUTH_MAX) {
+				rec.status = cpu_to_le16(P80211ENUM_status_ap_full);
+			} else {
+				ether_addr_copy(hw->authlist.addr[hw->authlist.cnt],
+						rec.address);
+				hw->authlist.cnt++;
+				added = 1;
+			}
+		}
+	}
+
+	/*
+	 * Send back the results of the authentication.  If this doesn't work,
+	 * then make sure to remove the address from the authenticated list if
+	 * it was added.
+	 */
+
+	rec.algorithm = inf->info.authreq.algorithm;
+
+	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_AUTHENTICATESTA,
+					&rec, sizeof(rec));
+	if (result) {
+		if (added)
+			hw->authlist.cnt--;
+		netdev_err(wlandev->netdev,
+			   "setconfig(authenticatestation) failed, result=%d\n",
+			   result);
+	}
+}
+
+/*
+ * prism2sta_inf_psusercnt
+ *
+ * Handles the receipt of a PowerSaveUserCount info frame. Should
+ * be present in APs only.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to info frame (contents in hfa384x order)
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+static void prism2sta_inf_psusercnt(struct wlandevice *wlandev,
+				    struct hfa384x_inf_frame *inf)
+{
+	struct hfa384x *hw = wlandev->priv;
+
+	hw->psusercount = le16_to_cpu(inf->info.psusercnt.usercnt);
+}
+
+/*
+ * prism2sta_ev_info
+ *
+ * Handles the Info event.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	inf		ptr to a generic info frame
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+void prism2sta_ev_info(struct wlandevice *wlandev,
+		       struct hfa384x_inf_frame *inf)
+{
+	le16_to_cpus(&inf->infotype);
+	/* Dispatch */
+	switch (inf->infotype) {
+	case HFA384x_IT_HANDOVERADDR:
+		prism2sta_inf_handover(wlandev, inf);
+		break;
+	case HFA384x_IT_COMMTALLIES:
+		prism2sta_inf_tallies(wlandev, inf);
+		break;
+	case HFA384x_IT_HOSTSCANRESULTS:
+		prism2sta_inf_hostscanresults(wlandev, inf);
+		break;
+	case HFA384x_IT_SCANRESULTS:
+		prism2sta_inf_scanresults(wlandev, inf);
+		break;
+	case HFA384x_IT_CHINFORESULTS:
+		prism2sta_inf_chinforesults(wlandev, inf);
+		break;
+	case HFA384x_IT_LINKSTATUS:
+		prism2sta_inf_linkstatus(wlandev, inf);
+		break;
+	case HFA384x_IT_ASSOCSTATUS:
+		prism2sta_inf_assocstatus(wlandev, inf);
+		break;
+	case HFA384x_IT_AUTHREQ:
+		prism2sta_inf_authreq(wlandev, inf);
+		break;
+	case HFA384x_IT_PSUSERCNT:
+		prism2sta_inf_psusercnt(wlandev, inf);
+		break;
+	case HFA384x_IT_KEYIDCHANGED:
+		netdev_warn(wlandev->netdev, "Unhandled IT_KEYIDCHANGED\n");
+		break;
+	case HFA384x_IT_ASSOCREQ:
+		netdev_warn(wlandev->netdev, "Unhandled IT_ASSOCREQ\n");
+		break;
+	case HFA384x_IT_MICFAILURE:
+		netdev_warn(wlandev->netdev, "Unhandled IT_MICFAILURE\n");
+		break;
+	default:
+		netdev_warn(wlandev->netdev,
+			    "Unknown info type=0x%02x\n", inf->infotype);
+		break;
+	}
+}
+
+/*
+ * prism2sta_ev_txexc
+ *
+ * Handles the TxExc event.  A Transmit Exception event indicates
+ * that the MAC's TX process was unsuccessful - so the packet did
+ * not get transmitted.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	status		tx frame status word
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+void prism2sta_ev_txexc(struct wlandevice *wlandev, u16 status)
+{
+	pr_debug("TxExc status=0x%x.\n", status);
+}
+
+/*
+ * prism2sta_ev_tx
+ *
+ * Handles the Tx event.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *	status		tx frame status word
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+void prism2sta_ev_tx(struct wlandevice *wlandev, u16 status)
+{
+	pr_debug("Tx Complete, status=0x%04x\n", status);
+	/* update linux network stats */
+	wlandev->netdev->stats.tx_packets++;
+}
+
+/*
+ * prism2sta_ev_alloc
+ *
+ * Handles the Alloc event.
+ *
+ * Arguments:
+ *	wlandev		wlan device structure
+ *
+ * Returns:
+ *	nothing
+ *
+ * Side effects:
+ *
+ * Call context:
+ *	interrupt
+ */
+void prism2sta_ev_alloc(struct wlandevice *wlandev)
+{
+	netif_wake_queue(wlandev->netdev);
+}
+
+/*
+ * create_wlan
+ *
+ * Called at module init time.  This creates the struct wlandevice structure
+ * and initializes it with relevant bits.
+ *
+ * Arguments:
+ *	none
+ *
+ * Returns:
+ *	the created struct wlandevice structure.
+ *
+ * Side effects:
+ *	also allocates the priv/hw structures.
+ *
+ * Call context:
+ *	process thread
+ *
+ */
+static struct wlandevice *create_wlan(void)
+{
+	struct wlandevice *wlandev = NULL;
+	struct hfa384x *hw = NULL;
+
+	/* Alloc our structures */
+	wlandev = kzalloc(sizeof(*wlandev), GFP_KERNEL);
+	hw = kzalloc(sizeof(*hw), GFP_KERNEL);
+
+	if (!wlandev || !hw) {
+		kfree(wlandev);
+		kfree(hw);
+		return NULL;
+	}
+
+	/* Initialize the network device object. */
+	wlandev->nsdname = dev_info;
+	wlandev->msdstate = WLAN_MSD_HWPRESENT_PENDING;
+	wlandev->priv = hw;
+	wlandev->open = prism2sta_open;
+	wlandev->close = prism2sta_close;
+	wlandev->reset = prism2sta_reset;
+	wlandev->txframe = prism2sta_txframe;
+	wlandev->mlmerequest = prism2sta_mlmerequest;
+	wlandev->set_multicast_list = prism2sta_setmulticast;
+	wlandev->tx_timeout = hfa384x_tx_timeout;
+
+	wlandev->nsdcaps = P80211_NSDCAP_HWFRAGMENT | P80211_NSDCAP_AUTOJOIN;
+
+	/* Initialize the device private data structure. */
+	hw->dot11_desired_bss_type = 1;
+
+	return wlandev;
+}
+
+void prism2sta_commsqual_defer(struct work_struct *data)
+{
+	struct hfa384x *hw = container_of(data, struct hfa384x, commsqual_bh);
+	struct wlandevice *wlandev = hw->wlandev;
+	struct hfa384x_bytestr32 ssid;
+	struct p80211msg_dot11req_mibget msg;
+	struct p80211item_uint32 *mibitem = (struct p80211item_uint32 *)
+						&msg.mibattribute.data;
+	int result = 0;
+
+	if (hw->wlandev->hwremoved)
+		return;
+
+	/* we don't care if we're in AP mode */
+	if ((wlandev->macmode == WLAN_MACMODE_NONE) ||
+	    (wlandev->macmode == WLAN_MACMODE_ESS_AP)) {
+		return;
+	}
+
+	/* It only makes sense to poll these in non-IBSS */
+	if (wlandev->macmode != WLAN_MACMODE_IBSS_STA) {
+		result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DBMCOMMSQUALITY,
+						&hw->qual, HFA384x_RID_DBMCOMMSQUALITY_LEN);
+
+		if (result) {
+			netdev_err(wlandev->netdev, "error fetching commsqual\n");
+			return;
+		}
+
+		pr_debug("commsqual %d %d %d\n",
+			 le16_to_cpu(hw->qual.cq_curr_bss),
+			 le16_to_cpu(hw->qual.asl_curr_bss),
+			 le16_to_cpu(hw->qual.anl_curr_fc));
+	}
+
+	/* Get the signal rate */
+	msg.msgcode = DIDMSG_DOT11REQ_MIBGET;
+	mibitem->did = DIDMIB_P2_MAC_CURRENTTXRATE;
+	result = p80211req_dorequest(wlandev, (u8 *)&msg);
+
+	if (result) {
+		pr_debug("get signal rate failed, result = %d\n",
+			 result);
+		return;
+	}
+
+	switch (mibitem->data) {
+	case HFA384x_RATEBIT_1:
+		hw->txrate = 10;
+		break;
+	case HFA384x_RATEBIT_2:
+		hw->txrate = 20;
+		break;
+	case HFA384x_RATEBIT_5dot5:
+		hw->txrate = 55;
+		break;
+	case HFA384x_RATEBIT_11:
+		hw->txrate = 110;
+		break;
+	default:
+		pr_debug("Bad ratebit (%d)\n", mibitem->data);
+	}
+
+	/* Lastly, we need to make sure the BSSID didn't change on us */
+	result = hfa384x_drvr_getconfig(hw,
+					HFA384x_RID_CURRENTBSSID,
+					wlandev->bssid, WLAN_BSSID_LEN);
+	if (result) {
+		pr_debug("getconfig(0x%02x) failed, result = %d\n",
+			 HFA384x_RID_CURRENTBSSID, result);
+		return;
+	}
+
+	result = hfa384x_drvr_getconfig(hw,
+					HFA384x_RID_CURRENTSSID,
+					&ssid, sizeof(ssid));
+	if (result) {
+		pr_debug("getconfig(0x%02x) failed, result = %d\n",
+			 HFA384x_RID_CURRENTSSID, result);
+		return;
+	}
+	prism2mgmt_bytestr2pstr((struct hfa384x_bytestr *)&ssid,
+				(struct p80211pstrd *)&wlandev->ssid);
+
+	/* Reschedule timer */
+	mod_timer(&hw->commsqual_timer, jiffies + HZ);
+}
+
+void prism2sta_commsqual_timer(struct timer_list *t)
+{
+	struct hfa384x *hw = from_timer(hw, t, commsqual_timer);
+
+	schedule_work(&hw->commsqual_bh);
+}
diff --git a/drivers/staging/wlan-ng/prism2usb.c b/drivers/staging/wlan-ng/prism2usb.c
new file mode 100644
index 000000000..e13da7fad
--- /dev/null
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -0,0 +1,299 @@
+// SPDX-License-Identifier: GPL-2.0
+#include "hfa384x_usb.c"
+#include "prism2mgmt.c"
+#include "prism2mib.c"
+#include "prism2sta.c"
+#include "prism2fw.c"
+
+#define PRISM_DEV(vid, pid, name)		\
+	{ USB_DEVICE(vid, pid),			\
+	.driver_info = (unsigned long)name }
+
+static const struct usb_device_id usb_prism_tbl[] = {
+	PRISM_DEV(0x04bb, 0x0922, "IOData AirPort WN-B11/USBS"),
+	PRISM_DEV(0x07aa, 0x0012, "Corega Wireless LAN USB Stick-11"),
+	PRISM_DEV(0x09aa, 0x3642, "Prism2.x 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0408, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0421, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1915, 0x2236, "Linksys WUSB11v3.0 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2212, "Linksys WUSB11v2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2213, "Linksys WUSB12v1.1 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0411, 0x0016, "Melco WLI-USB-S11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x08de, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x8086, 0x1111, "Intel PRO/Wireless 2011B LAN USB Adapter"),
+	PRISM_DEV(0x0d8e, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x045e, 0x006e, "Microsoft MN510 Wireless USB Adapter"),
+	PRISM_DEV(0x0967, 0x0204, "Acer Warplink USB Adapter"),
+	PRISM_DEV(0x0cde, 0x0002, "Z-Com 725/726 Prism2.5 USB/USB Integrated"),
+	PRISM_DEV(0x0cde, 0x0005, "Z-Com Xl735 Wireless 802.11b USB Adapter"),
+	PRISM_DEV(0x413c, 0x8100, "Dell TrueMobile 1180 Wireless USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1601, "ALLNET 0193 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1602, "ZyXEL ZyAIR B200 Wireless USB Adapter"),
+	PRISM_DEV(0x0baf, 0x00eb, "USRobotics USR1120 Wireless USB Adapter"),
+	PRISM_DEV(0x0411, 0x0027, "Melco WLI-USB-KS11G 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x04f1, 0x3009, "JVC MP-XP7250 Builtin USB WLAN Adapter"),
+	PRISM_DEV(0x0846, 0x4110, "NetGear MA111"),
+	PRISM_DEV(0x03f3, 0x0020, "Adaptec AWN-8020 USB WLAN Adapter"),
+	PRISM_DEV(0x2821, 0x3300, "ASUS-WL140 / Hawking HighDB Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3700, "DWL-122 Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3702, "DWL-120 Rev F Wireless USB Adapter"),
+	PRISM_DEV(0x50c2, 0x4013, "Averatec USB WLAN Adapter"),
+	PRISM_DEV(0x2c02, 0x14ea, "Planex GW-US11H WLAN USB Adapter"),
+	PRISM_DEV(0x124a, 0x168b, "Airvast PRISM3 WLAN USB Adapter"),
+	PRISM_DEV(0x083a, 0x3503, "T-Sinus 111 USB WLAN Adapter"),
+	PRISM_DEV(0x0411, 0x0044, "Melco WLI-USB-KB11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x1668, 0x6106, "ROPEX FreeLan 802.11b USB Adapter"),
+	PRISM_DEV(0x124a, 0x4017, "Pheenet WL-503IA 802.11b USB Adapter"),
+	PRISM_DEV(0x0bb2, 0x0302, "Ambit Microsystems Corp."),
+	PRISM_DEV(0x9016, 0x182d, "Sitecom WL-022 802.11b USB Adapter"),
+	PRISM_DEV(0x0543, 0x0f01,
+		  "ViewSonic Airsync USB Adapter 11Mbps (Prism2.5)"),
+	PRISM_DEV(0x067c, 0x1022,
+		  "Siemens SpeedStream 1022 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x049f, 0x0033,
+		  "Compaq/Intel W100 PRO/Wireless 11Mbps multiport WLAN Adapter"),
+	{ } /* terminator */
+};
+MODULE_DEVICE_TABLE(usb, usb_prism_tbl);
+
+static int prism2sta_probe_usb(struct usb_interface *interface,
+			       const struct usb_device_id *id)
+{
+	struct usb_device *dev;
+	struct usb_endpoint_descriptor *bulk_in, *bulk_out;
+	struct usb_host_interface *iface_desc = interface->cur_altsetting;
+	struct wlandevice *wlandev = NULL;
+	struct hfa384x *hw = NULL;
+	int result = 0;
+
+	result = usb_find_common_endpoints(iface_desc, &bulk_in, &bulk_out, NULL, NULL);
+	if (result)
+		goto failed;
+
+	dev = interface_to_usbdev(interface);
+	wlandev = create_wlan();
+	if (!wlandev) {
+		dev_err(&interface->dev, "Memory allocation failure.\n");
+		result = -EIO;
+		goto failed;
+	}
+	hw = wlandev->priv;
+
+	if (wlan_setup(wlandev, &interface->dev) != 0) {
+		dev_err(&interface->dev, "wlan_setup() failed.\n");
+		result = -EIO;
+		goto failed;
+	}
+
+	/* Initialize the hw data */
+	hw->endp_in = usb_rcvbulkpipe(dev, bulk_in->bEndpointAddress);
+	hw->endp_out = usb_sndbulkpipe(dev, bulk_out->bEndpointAddress);
+	hfa384x_create(hw, dev);
+	hw->wlandev = wlandev;
+
+	/* Register the wlandev, this gets us a name and registers the
+	 * linux netdevice.
+	 */
+	SET_NETDEV_DEV(wlandev->netdev, &interface->dev);
+
+	/* Do a chip-level reset on the MAC */
+	if (prism2_doreset) {
+		result = hfa384x_corereset(hw,
+					   prism2_reset_holdtime,
+					   prism2_reset_settletime, 0);
+		if (result != 0) {
+			result = -EIO;
+			dev_err(&interface->dev,
+				"hfa384x_corereset() failed.\n");
+			goto failed_reset;
+		}
+	}
+
+	usb_get_dev(dev);
+
+	wlandev->msdstate = WLAN_MSD_HWPRESENT;
+
+	/* Try and load firmware, then enable card before we register */
+	prism2_fwtry(dev, wlandev);
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_enable);
+
+	if (register_wlandev(wlandev) != 0) {
+		dev_err(&interface->dev, "register_wlandev() failed.\n");
+		result = -EIO;
+		goto failed_register;
+	}
+
+	goto done;
+
+failed_register:
+	usb_put_dev(dev);
+failed_reset:
+	wlan_unsetup(wlandev);
+failed:
+	kfree(wlandev);
+	kfree(hw);
+	wlandev = NULL;
+
+done:
+	usb_set_intfdata(interface, wlandev);
+	return result;
+}
+
+static void prism2sta_disconnect_usb(struct usb_interface *interface)
+{
+	struct wlandevice *wlandev;
+
+	wlandev = usb_get_intfdata(interface);
+	if (wlandev) {
+		LIST_HEAD(cleanlist);
+		struct hfa384x_usbctlx *ctlx, *temp;
+		unsigned long flags;
+
+		struct hfa384x *hw = wlandev->priv;
+
+		if (!hw)
+			goto exit;
+
+		spin_lock_irqsave(&hw->ctlxq.lock, flags);
+
+		p80211netdev_hwremoved(wlandev);
+		list_splice_init(&hw->ctlxq.reapable, &cleanlist);
+		list_splice_init(&hw->ctlxq.completing, &cleanlist);
+		list_splice_init(&hw->ctlxq.pending, &cleanlist);
+		list_splice_init(&hw->ctlxq.active, &cleanlist);
+
+		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+
+		/* There's no hardware to shutdown, but the driver
+		 * might have some tasks that must be stopped before
+		 * we can tear everything down.
+		 */
+		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
+
+		del_singleshot_timer_sync(&hw->throttle);
+		del_singleshot_timer_sync(&hw->reqtimer);
+		del_singleshot_timer_sync(&hw->resptimer);
+
+		/* Unlink all the URBs. This "removes the wheels"
+		 * from the entire CTLX handling mechanism.
+		 */
+		usb_kill_urb(&hw->rx_urb);
+		usb_kill_urb(&hw->tx_urb);
+		usb_kill_urb(&hw->ctlx_urb);
+
+		cancel_work_sync(&hw->completion_bh);
+		cancel_work_sync(&hw->reaper_bh);
+
+		cancel_work_sync(&hw->link_bh);
+		cancel_work_sync(&hw->commsqual_bh);
+		cancel_work_sync(&hw->usb_work);
+
+		/* Now we complete any outstanding commands
+		 * and tell everyone who is waiting for their
+		 * responses that we have shut down.
+		 */
+		list_for_each_entry(ctlx, &cleanlist, list)
+			complete(&ctlx->done);
+
+		/* Give any outstanding synchronous commands
+		 * a chance to complete. All they need to do
+		 * is "wake up", so that's easy.
+		 * (I'd like a better way to do this, really.)
+		 */
+		msleep(100);
+
+		/* Now delete the CTLXs, because no-one else can now. */
+		list_for_each_entry_safe(ctlx, temp, &cleanlist, list)
+			kfree(ctlx);
+
+		/* Unhook the wlandev */
+		unregister_wlandev(wlandev);
+		wlan_unsetup(wlandev);
+
+		usb_put_dev(hw->usb);
+
+		hfa384x_destroy(hw);
+		kfree(hw);
+
+		kfree(wlandev);
+	}
+
+exit:
+	usb_set_intfdata(interface, NULL);
+}
+
+#ifdef CONFIG_PM
+static int prism2sta_suspend(struct usb_interface *interface,
+			     pm_message_t message)
+{
+	struct hfa384x *hw = NULL;
+	struct wlandevice *wlandev;
+
+	wlandev = usb_get_intfdata(interface);
+	if (!wlandev)
+		return -ENODEV;
+
+	hw = wlandev->priv;
+	if (!hw)
+		return -ENODEV;
+
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
+
+	usb_kill_urb(&hw->rx_urb);
+	usb_kill_urb(&hw->tx_urb);
+	usb_kill_urb(&hw->ctlx_urb);
+
+	return 0;
+}
+
+static int prism2sta_resume(struct usb_interface *interface)
+{
+	int result = 0;
+	struct hfa384x *hw = NULL;
+	struct wlandevice *wlandev;
+
+	wlandev = usb_get_intfdata(interface);
+	if (!wlandev)
+		return -ENODEV;
+
+	hw = wlandev->priv;
+	if (!hw)
+		return -ENODEV;
+
+	/* Do a chip-level reset on the MAC */
+	if (prism2_doreset) {
+		result = hfa384x_corereset(hw,
+					   prism2_reset_holdtime,
+					   prism2_reset_settletime, 0);
+		if (result != 0) {
+			unregister_wlandev(wlandev);
+			hfa384x_destroy(hw);
+			dev_err(&interface->dev, "hfa384x_corereset() failed.\n");
+			kfree(wlandev);
+			kfree(hw);
+			wlandev = NULL;
+			return -ENODEV;
+		}
+	}
+
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_enable);
+
+	return 0;
+}
+#else
+#define prism2sta_suspend NULL
+#define prism2sta_resume NULL
+#endif /* CONFIG_PM */
+
+static struct usb_driver prism2_usb_driver = {
+	.name = "prism2_usb",
+	.probe = prism2sta_probe_usb,
+	.disconnect = prism2sta_disconnect_usb,
+	.id_table = usb_prism_tbl,
+	.suspend = prism2sta_suspend,
+	.resume = prism2sta_resume,
+	.reset_resume = prism2sta_resume,
+	/* fops, minor? */
+};
+
+module_usb_driver(prism2_usb_driver);